Merge branch 'master' of git://git.kernel.org/pub/scm/linux/kernel/git/torvalds/linux-2.6 into for-upstream

1069 files changed, 481613 insertions, 29328 deletions

diff --git a/drivers/acpi/processor_idle.c b/drivers/acpi/processor_idle.c
index 38aca048e95..66a9d814556 100644
--- a/drivers/acpi/processor_idle.c
+++ b/drivers/acpi/processor_idle.c
@@ -41,6 +41,7 @@
 #include <linux/pm_qos_params.h>
 #include <linux/clockchips.h>
 #include <linux/cpuidle.h>
+#include <linux/irqflags.h>
 
 /*
  * Include the apic definitions for x86 to have the APIC timer related defines
diff --git a/drivers/ata/libata-scsi.c b/drivers/ata/libata-scsi.c
index 4040d8b5321..9e92107691f 100644
--- a/drivers/ata/libata-scsi.c
+++ b/drivers/ata/libata-scsi.c
@@ -3369,7 +3369,7 @@ static void ata_scsi_remove_dev(struct ata_device *dev)
 
 	if (sdev) {
 		ata_dev_printk(dev, KERN_INFO, "detaching (SCSI %s)\n",
-			       sdev->sdev_gendev.bus_id);
+			       dev_name(&sdev->sdev_gendev));
 
 		scsi_remove_device(sdev);
 		scsi_device_put(sdev);
diff --git a/drivers/base/attribute_container.c b/drivers/base/attribute_container.c
index f57652db0a2..b9cda053d3c 100644
--- a/drivers/base/attribute_container.c
+++ b/drivers/base/attribute_container.c
@@ -167,7 +167,7 @@ attribute_container_add_device(struct device *dev,
 		ic->classdev.parent = get_device(dev);
 		ic->classdev.class = cont->class;
 		cont->class->dev_release = attribute_container_release;
-		strcpy(ic->classdev.bus_id, dev->bus_id);
+		dev_set_name(&ic->classdev, dev_name(dev));
 		if (fn)
 			fn(cont, dev, &ic->classdev);
 		else
diff --git a/drivers/base/base.h b/drivers/base/base.h
index 0a5f055dffb..b676f8f801f 100644
--- a/drivers/base/base.h
+++ b/drivers/base/base.h
@@ -63,6 +63,32 @@ struct class_private {
 #define to_class(obj)	\
 	container_of(obj, struct class_private, class_subsys.kobj)
 
+/**
+ * struct device_private - structure to hold the private to the driver core portions of the device structure.
+ *
+ * @klist_children - klist containing all children of this device
+ * @knode_parent - node in sibling list
+ * @knode_driver - node in driver list
+ * @knode_bus - node in bus list
+ * @device - pointer back to the struct class that this structure is
+ * associated with.
+ *
+ * Nothing outside of the driver core should ever touch these fields.
+ */
+struct device_private {
+	struct klist klist_children;
+	struct klist_node knode_parent;
+	struct klist_node knode_driver;
+	struct klist_node knode_bus;
+	struct device *device;
+};
+#define to_device_private_parent(obj)	\
+	container_of(obj, struct device_private, knode_parent)
+#define to_device_private_driver(obj)	\
+	container_of(obj, struct device_private, knode_driver)
+#define to_device_private_bus(obj)	\
+	container_of(obj, struct device_private, knode_bus)
+
 /* initialisation functions */
 extern int devices_init(void);
 extern int buses_init(void);
diff --git a/drivers/base/bus.c b/drivers/base/bus.c
index 5aee1c0169e..0f0a5044467 100644
--- a/drivers/base/bus.c
+++ b/drivers/base/bus.c
@@ -253,7 +253,14 @@ static ssize_t store_drivers_probe(struct bus_type *bus,
 static struct device *next_device(struct klist_iter *i)
 {
 	struct klist_node *n = klist_next(i);
-	return n ? container_of(n, struct device, knode_bus) : NULL;
+	struct device *dev = NULL;
+	struct device_private *dev_prv;
+
+	if (n) {
+		dev_prv = to_device_private_bus(n);
+		dev = dev_prv->device;
+	}
+	return dev;
 }
 
 /**
@@ -286,7 +293,7 @@ int bus_for_each_dev(struct bus_type *bus, struct device *start,
 		return -EINVAL;
 
 	klist_iter_init_node(&bus->p->klist_devices, &i,
-			     (start ? &start->knode_bus : NULL));
+			     (start ? &start->p->knode_bus : NULL));
 	while ((dev = next_device(&i)) && !error)
 		error = fn(dev, data);
 	klist_iter_exit(&i);
@@ -320,7 +327,7 @@ struct device *bus_find_device(struct bus_type *bus,
 		return NULL;
 
 	klist_iter_init_node(&bus->p->klist_devices, &i,
-			     (start ? &start->knode_bus : NULL));
+			     (start ? &start->p->knode_bus : NULL));
 	while ((dev = next_device(&i)))
 		if (match(dev, data) && get_device(dev))
 			break;
@@ -333,7 +340,7 @@ static int match_name(struct device *dev, void *data)
 {
 	const char *name = data;
 
-	return sysfs_streq(name, dev->bus_id);
+	return sysfs_streq(name, dev_name(dev));
 }
 
 /**
@@ -461,12 +468,12 @@ int bus_add_device(struct device *dev)
 	int error = 0;
 
 	if (bus) {
-		pr_debug("bus: '%s': add device %s\n", bus->name, dev->bus_id);
+		pr_debug("bus: '%s': add device %s\n", bus->name, dev_name(dev));
 		error = device_add_attrs(bus, dev);
 		if (error)
 			goto out_put;
 		error = sysfs_create_link(&bus->p->devices_kset->kobj,
-						&dev->kobj, dev->bus_id);
+						&dev->kobj, dev_name(dev));
 		if (error)
 			goto out_id;
 		error = sysfs_create_link(&dev->kobj,
@@ -482,7 +489,7 @@ int bus_add_device(struct device *dev)
 out_deprecated:
 	sysfs_remove_link(&dev->kobj, "subsystem");
 out_subsys:
-	sysfs_remove_link(&bus->p->devices_kset->kobj, dev->bus_id);
+	sysfs_remove_link(&bus->p->devices_kset->kobj, dev_name(dev));
 out_id:
 	device_remove_attrs(bus, dev);
 out_put:
@@ -507,7 +514,8 @@ void bus_attach_device(struct device *dev)
 			ret = device_attach(dev);
 		WARN_ON(ret < 0);
 		if (ret >= 0)
-			klist_add_tail(&dev->knode_bus, &bus->p->klist_devices);
+			klist_add_tail(&dev->p->knode_bus,
+				       &bus->p->klist_devices);
 	}
 }
 
@@ -526,13 +534,13 @@ void bus_remove_device(struct device *dev)
 		sysfs_remove_link(&dev->kobj, "subsystem");
 		remove_deprecated_bus_links(dev);
 		sysfs_remove_link(&dev->bus->p->devices_kset->kobj,
-				  dev->bus_id);
+				  dev_name(dev));
 		device_remove_attrs(dev->bus, dev);
-		if (klist_node_attached(&dev->knode_bus))
-			klist_del(&dev->knode_bus);
+		if (klist_node_attached(&dev->p->knode_bus))
+			klist_del(&dev->p->knode_bus);
 
 		pr_debug("bus: '%s': remove device %s\n",
-			 dev->bus->name, dev->bus_id);
+			 dev->bus->name, dev_name(dev));
 		device_release_driver(dev);
 		bus_put(dev->bus);
 	}
@@ -831,14 +839,16 @@ static void bus_remove_attrs(struct bus_type *bus)
 
 static void klist_devices_get(struct klist_node *n)
 {
-	struct device *dev = container_of(n, struct device, knode_bus);
+	struct device_private *dev_prv = to_device_private_bus(n);
+	struct device *dev = dev_prv->device;
 
 	get_device(dev);
 }
 
 static void klist_devices_put(struct klist_node *n)
 {
-	struct device *dev = container_of(n, struct device, knode_bus);
+	struct device_private *dev_prv = to_device_private_bus(n);
+	struct device *dev = dev_prv->device;
 
 	put_device(dev);
 }
@@ -993,18 +1003,20 @@ static void device_insertion_sort_klist(struct device *a, struct list_head *list
 {
 	struct list_head *pos;
 	struct klist_node *n;
+	struct device_private *dev_prv;
 	struct device *b;
 
 	list_for_each(pos, list) {
 		n = container_of(pos, struct klist_node, n_node);
-		b = container_of(n, struct device, knode_bus);
+		dev_prv = to_device_private_bus(n);
+		b = dev_prv->device;
 		if (compare(a, b) <= 0) {
-			list_move_tail(&a->knode_bus.n_node,
-				       &b->knode_bus.n_node);
+			list_move_tail(&a->p->knode_bus.n_node,
+				       &b->p->knode_bus.n_node);
 			return;
 		}
 	}
-	list_move_tail(&a->knode_bus.n_node, list);
+	list_move_tail(&a->p->knode_bus.n_node, list);
 }
 
 void bus_sort_breadthfirst(struct bus_type *bus,
@@ -1014,6 +1026,7 @@ void bus_sort_breadthfirst(struct bus_type *bus,
 	LIST_HEAD(sorted_devices);
 	struct list_head *pos, *tmp;
 	struct klist_node *n;
+	struct device_private *dev_prv;
 	struct device *dev;
 	struct klist *device_klist;
 
@@ -1022,7 +1035,8 @@ void bus_sort_breadthfirst(struct bus_type *bus,
 	spin_lock(&device_klist->k_lock);
 	list_for_each_safe(pos, tmp, &device_klist->k_list) {
 		n = container_of(pos, struct klist_node, n_node);
-		dev = container_of(n, struct device, knode_bus);
+		dev_prv = to_device_private_bus(n);
+		dev = dev_prv->device;
 		device_insertion_sort_klist(dev, &sorted_devices, compare);
 	}
 	list_splice(&sorted_devices, &device_klist->k_list);
diff --git a/drivers/base/core.c b/drivers/base/core.c
index 8c2cc2648f5..61df508fa62 100644
--- a/drivers/base/core.c
+++ b/drivers/base/core.c
@@ -109,6 +109,7 @@ static struct sysfs_ops dev_sysfs_ops = {
 static void device_release(struct kobject *kobj)
 {
 	struct device *dev = to_dev(kobj);
+	struct device_private *p = dev->p;
 
 	if (dev->release)
 		dev->release(dev);
@@ -119,7 +120,8 @@ static void device_release(struct kobject *kobj)
 	else
 		WARN(1, KERN_ERR "Device '%s' does not have a release() "
 			"function, it is broken and must be fixed.\n",
-			dev->bus_id);
+			dev_name(dev));
+	kfree(p);
 }
 
 static struct kobj_type device_ktype = {
@@ -209,7 +211,7 @@ static int dev_uevent(struct kset *kset, struct kobject *kobj,
 		retval = dev->bus->uevent(dev, env);
 		if (retval)
 			pr_debug("device: '%s': %s: bus uevent() returned %d\n",
-				 dev->bus_id, __func__, retval);
+				 dev_name(dev), __func__, retval);
 	}
 
 	/* have the class specific function add its stuff */
@@ -217,7 +219,7 @@ static int dev_uevent(struct kset *kset, struct kobject *kobj,
 		retval = dev->class->dev_uevent(dev, env);
 		if (retval)
 			pr_debug("device: '%s': %s: class uevent() "
-				 "returned %d\n", dev->bus_id,
+				 "returned %d\n", dev_name(dev),
 				 __func__, retval);
 	}
 
@@ -226,7 +228,7 @@ static int dev_uevent(struct kset *kset, struct kobject *kobj,
 		retval = dev->type->uevent(dev, env);
 		if (retval)
 			pr_debug("device: '%s': %s: dev_type uevent() "
-				 "returned %d\n", dev->bus_id,
+				 "returned %d\n", dev_name(dev),
 				 __func__, retval);
 	}
 
@@ -507,14 +509,16 @@ EXPORT_SYMBOL_GPL(device_schedule_callback_owner);
 
 static void klist_children_get(struct klist_node *n)
 {
-	struct device *dev = container_of(n, struct device, knode_parent);
+	struct device_private *p = to_device_private_parent(n);
+	struct device *dev = p->device;
 
 	get_device(dev);
 }
 
 static void klist_children_put(struct klist_node *n)
 {
-	struct device *dev = container_of(n, struct device, knode_parent);
+	struct device_private *p = to_device_private_parent(n);
+	struct device *dev = p->device;
 
 	put_device(dev);
 }
@@ -536,9 +540,15 @@ static void klist_children_put(struct klist_node *n)
  */
 void device_initialize(struct device *dev)
 {
+	dev->p = kzalloc(sizeof(*dev->p), GFP_KERNEL);
+	if (!dev->p) {
+		WARN_ON(1);
+		return;
+	}
+	dev->p->device = dev;
 	dev->kobj.kset = devices_kset;
 	kobject_init(&dev->kobj, &device_ktype);
-	klist_init(&dev->klist_children, klist_children_get,
+	klist_init(&dev->p->klist_children, klist_children_get,
 		   klist_children_put);
 	INIT_LIST_HEAD(&dev->dma_pools);
 	init_MUTEX(&dev->sem);
@@ -672,7 +682,7 @@ static int device_add_class_symlinks(struct device *dev)
 	if (dev->kobj.parent != &dev->class->p->class_subsys.kobj &&
 	    device_is_not_partition(dev)) {
 		error = sysfs_create_link(&dev->class->p->class_subsys.kobj,
-					  &dev->kobj, dev->bus_id);
+					  &dev->kobj, dev_name(dev));
 		if (error)
 			goto out_subsys;
 	}
@@ -712,11 +722,11 @@ out_busid:
 	if (dev->kobj.parent != &dev->class->p->class_subsys.kobj &&
 	    device_is_not_partition(dev))
 		sysfs_remove_link(&dev->class->p->class_subsys.kobj,
-				  dev->bus_id);
+				  dev_name(dev));
 #else
 	/* link in the class directory pointing to the device */
 	error = sysfs_create_link(&dev->class->p->class_subsys.kobj,
-				  &dev->kobj, dev->bus_id);
+				  &dev->kobj, dev_name(dev));
 	if (error)
 		goto out_subsys;
 
@@ -729,7 +739,7 @@ out_busid:
 	return 0;
 
 out_busid:
-	sysfs_remove_link(&dev->class->p->class_subsys.kobj, dev->bus_id);
+	sysfs_remove_link(&dev->class->p->class_subsys.kobj, dev_name(dev));
 #endif
 
 out_subsys:
@@ -758,12 +768,12 @@ static void device_remove_class_symlinks(struct device *dev)
 	if (dev->kobj.parent != &dev->class->p->class_subsys.kobj &&
 	    device_is_not_partition(dev))
 		sysfs_remove_link(&dev->class->p->class_subsys.kobj,
-				  dev->bus_id);
+				  dev_name(dev));
 #else
 	if (dev->parent && device_is_not_partition(dev))
 		sysfs_remove_link(&dev->kobj, "device");
 
-	sysfs_remove_link(&dev->class->p->class_subsys.kobj, dev->bus_id);
+	sysfs_remove_link(&dev->class->p->class_subsys.kobj, dev_name(dev));
 #endif
 
 	sysfs_remove_link(&dev->kobj, "subsystem");
@@ -866,7 +876,7 @@ int device_add(struct device *dev)
 	if (!strlen(dev->bus_id))
 		goto done;
 
-	pr_debug("device: '%s': %s\n", dev->bus_id, __func__);
+	pr_debug("device: '%s': %s\n", dev_name(dev), __func__);
 
 	parent = get_device(dev->parent);
 	setup_parent(dev, parent);
@@ -876,7 +886,7 @@ int device_add(struct device *dev)
 		set_dev_node(dev, dev_to_node(parent));
 
 	/* first, register with generic layer. */
-	error = kobject_add(&dev->kobj, dev->kobj.parent, "%s", dev->bus_id);
+	error = kobject_add(&dev->kobj, dev->kobj.parent, "%s", dev_name(dev));
 	if (error)
 		goto Error;
 
@@ -884,11 +894,6 @@ int device_add(struct device *dev)
 	if (platform_notify)
 		platform_notify(dev);
 
-	/* notify clients of device entry (new way) */
-	if (dev->bus)
-		blocking_notifier_call_chain(&dev->bus->p->bus_notifier,
-					     BUS_NOTIFY_ADD_DEVICE, dev);
-
 	error = device_create_file(dev, &uevent_attr);
 	if (error)
 		goto attrError;
@@ -916,10 +921,19 @@ int device_add(struct device *dev)
 	if (error)
 		goto DPMError;
 	device_pm_add(dev);
+
+	/* Notify clients of device addition.  This call must come
+	 * after dpm_sysf_add() and before kobject_uevent().
+	 */
+	if (dev->bus)
+		blocking_notifier_call_chain(&dev->bus->p->bus_notifier,
+					     BUS_NOTIFY_ADD_DEVICE, dev);
+
 	kobject_uevent(&dev->kobj, KOBJ_ADD);
 	bus_attach_device(dev);
 	if (parent)
-		klist_add_tail(&dev->knode_parent, &parent->klist_children);
+		klist_add_tail(&dev->p->knode_parent,
+			       &parent->p->klist_children);
 
 	if (dev->class) {
 		mutex_lock(&dev->class->p->class_mutex);
@@ -940,9 +954,6 @@ done:
  DPMError:
 	bus_remove_device(dev);
  BusError:
-	if (dev->bus)
-		blocking_notifier_call_chain(&dev->bus->p->bus_notifier,
-					     BUS_NOTIFY_DEL_DEVICE, dev);
 	device_remove_attrs(dev);
  AttrsError:
 	device_remove_class_symlinks(dev);
@@ -1027,10 +1038,16 @@ void device_del(struct device *dev)
 	struct device *parent = dev->parent;
 	struct class_interface *class_intf;
 
+	/* Notify clients of device removal.  This call must come
+	 * before dpm_sysfs_remove().
+	 */
+	if (dev->bus)
+		blocking_notifier_call_chain(&dev->bus->p->bus_notifier,
+					     BUS_NOTIFY_DEL_DEVICE, dev);
 	device_pm_remove(dev);
 	dpm_sysfs_remove(dev);
 	if (parent)
-		klist_del(&dev->knode_parent);
+		klist_del(&dev->p->knode_parent);
 	if (MAJOR(dev->devt)) {
 		device_remove_sys_dev_entry(dev);
 		device_remove_file(dev, &devt_attr);
@@ -1064,9 +1081,6 @@ void device_del(struct device *dev)
 	 */
 	if (platform_notify_remove)
 		platform_notify_remove(dev);
-	if (dev->bus)
-		blocking_notifier_call_chain(&dev->bus->p->bus_notifier,
-					     BUS_NOTIFY_DEL_DEVICE, dev);
 	kobject_uevent(&dev->kobj, KOBJ_REMOVE);
 	cleanup_device_parent(dev);
 	kobject_del(&dev->kobj);
@@ -1086,7 +1100,7 @@ void device_del(struct device *dev)
  */
 void device_unregister(struct device *dev)
 {
-	pr_debug("device: '%s': %s\n", dev->bus_id, __func__);
+	pr_debug("device: '%s': %s\n", dev_name(dev), __func__);
 	device_del(dev);
 	put_device(dev);
 }
@@ -1094,7 +1108,14 @@ void device_unregister(struct device *dev)
 static struct device *next_device(struct klist_iter *i)
 {
 	struct klist_node *n = klist_next(i);
-	return n ? container_of(n, struct device, knode_parent) : NULL;
+	struct device *dev = NULL;
+	struct device_private *p;
+
+	if (n) {
+		p = to_device_private_parent(n);
+		dev = p->device;
+	}
+	return dev;
 }
 
 /**
@@ -1116,7 +1137,7 @@ int device_for_each_child(struct device *parent, void *data,
 	struct device *child;
 	int error = 0;
 
-	klist_iter_init(&parent->klist_children, &i);
+	klist_iter_init(&parent->p->klist_children, &i);
 	while ((child = next_device(&i)) && !error)
 		error = fn(child, data);
 	klist_iter_exit(&i);
@@ -1147,7 +1168,7 @@ struct device *device_find_child(struct device *parent, void *data,
 	if (!parent)
 		return NULL;
 
-	klist_iter_init(&parent->klist_children, &i);
+	klist_iter_init(&parent->p->klist_children, &i);
 	while ((child = next_device(&i)))
 		if (match(child, data) && get_device(child))
 			break;
@@ -1196,10 +1217,101 @@ EXPORT_SYMBOL_GPL(put_device);
 EXPORT_SYMBOL_GPL(device_create_file);
 EXPORT_SYMBOL_GPL(device_remove_file);
 
+struct root_device
+{
+	struct device dev;
+	struct module *owner;
+};
+
+#define to_root_device(dev) container_of(dev, struct root_device, dev)
+
+static void root_device_release(struct device *dev)
+{
+	kfree(to_root_device(dev));
+}
+
+/**
+ * __root_device_register - allocate and register a root device
+ * @name: root device name
+ * @owner: owner module of the root device, usually THIS_MODULE
+ *
+ * This function allocates a root device and registers it
+ * using device_register(). In order to free the returned
+ * device, use root_device_unregister().
+ *
+ * Root devices are dummy devices which allow other devices
+ * to be grouped under /sys/devices. Use this function to
+ * allocate a root device and then use it as the parent of
+ * any device which should appear under /sys/devices/{name}
+ *
+ * The /sys/devices/{name} directory will also contain a
+ * 'module' symlink which points to the @owner directory
+ * in sysfs.
+ *
+ * Note: You probably want to use root_device_register().
+ */
+struct device *__root_device_register(const char *name, struct module *owner)
+{
+	struct root_device *root;
+	int err = -ENOMEM;
+
+	root = kzalloc(sizeof(struct root_device), GFP_KERNEL);
+	if (!root)
+		return ERR_PTR(err);
+
+	err = dev_set_name(&root->dev, name);
+	if (err) {
+		kfree(root);
+		return ERR_PTR(err);
+	}
+
+	root->dev.release = root_device_release;
+
+	err = device_register(&root->dev);
+	if (err) {
+		put_device(&root->dev);
+		return ERR_PTR(err);
+	}
+
+#ifdef CONFIG_MODULE	/* gotta find a "cleaner" way to do this */
+	if (owner) {
+		struct module_kobject *mk = &owner->mkobj;
+
+		err = sysfs_create_link(&root->dev.kobj, &mk->kobj, "module");
+		if (err) {
+			device_unregister(&root->dev);
+			return ERR_PTR(err);
+		}
+		root->owner = owner;
+	}
+#endif
+
+	return &root->dev;
+}
+EXPORT_SYMBOL_GPL(__root_device_register);
+
+/**
+ * root_device_unregister - unregister and free a root device
+ * @root: device going away.
+ *
+ * This function unregisters and cleans up a device that was created by
+ * root_device_register().
+ */
+void root_device_unregister(struct device *dev)
+{
+	struct root_device *root = to_root_device(dev);
+
+	if (root->owner)
+		sysfs_remove_link(&root->dev.kobj, "module");
+
+	device_unregister(dev);
+}
+EXPORT_SYMBOL_GPL(root_device_unregister);
+
 
 static void device_create_release(struct device *dev)
 {
-	pr_debug("device: '%s': %s\n", dev->bus_id, __func__);
+	pr_debug("device: '%s': %s\n", dev_name(dev), __func__);
 	kfree(dev);
 }
 
@@ -1344,7 +1456,7 @@ int device_rename(struct device *dev, char *new_name)
 	if (!dev)
 		return -EINVAL;
 
-	pr_debug("device: '%s': %s: renaming to '%s'\n", dev->bus_id,
+	pr_debug("device: '%s': %s: renaming to '%s'\n", dev_name(dev),
 		 __func__, new_name);
 
 #ifdef CONFIG_SYSFS_DEPRECATED
@@ -1381,7 +1493,7 @@ int device_rename(struct device *dev, char *new_name)
 #else
 	if (dev->class) {
 		error = sysfs_create_link_nowarn(&dev->class->p->class_subsys.kobj,
-						 &dev->kobj, dev->bus_id);
+						 &dev->kobj, dev_name(dev));
 		if (error)
 			goto out;
 		sysfs_remove_link(&dev->class->p->class_subsys.kobj,
@@ -1459,8 +1571,8 @@ int device_move(struct device *dev, struct device *new_parent)
 	new_parent = get_device(new_parent);
 	new_parent_kobj = get_device_parent(dev, new_parent);
 
-	pr_debug("device: '%s': %s: moving to '%s'\n", dev->bus_id,
-		 __func__, new_parent ? new_parent->bus_id : "<NULL>");
+	pr_debug("device: '%s': %s: moving to '%s'\n", dev_name(dev),
+		 __func__, new_parent ? dev_name(new_parent) : "<NULL>");
 	error = kobject_move(&dev->kobj, new_parent_kobj);
 	if (error) {
 		cleanup_glue_dir(dev, new_parent_kobj);
@@ -1470,9 +1582,10 @@ int device_move(struct device *dev, struct device *new_parent)
 	old_parent = dev->parent;
 	dev->parent = new_parent;
 	if (old_parent)
-		klist_remove(&dev->knode_parent);
+		klist_remove(&dev->p->knode_parent);
 	if (new_parent) {
-		klist_add_tail(&dev->knode_parent, &new_parent->klist_children);
+		klist_add_tail(&dev->p->knode_parent,
+			       &new_parent->p->klist_children);
 		set_dev_node(dev, dev_to_node(new_parent));
 	}
 
@@ -1484,11 +1597,11 @@ int device_move(struct device *dev, struct device *new_parent)
 		device_move_class_links(dev, new_parent, old_parent);
 		if (!kobject_move(&dev->kobj, &old_parent->kobj)) {
 			if (new_parent)
-				klist_remove(&dev->knode_parent);
+				klist_remove(&dev->p->knode_parent);
 			dev->parent = old_parent;
 			if (old_parent) {
-				klist_add_tail(&dev->knode_parent,
-					       &old_parent->klist_children);
+				klist_add_tail(&dev->p->knode_parent,
+					       &old_parent->p->klist_children);
 				set_dev_node(dev, dev_to_node(old_parent));
 			}
 		}
diff --git a/drivers/base/dd.c b/drivers/base/dd.c
index 20febc00a52..6fdaf76f033 100644
--- a/drivers/base/dd.c
+++ b/drivers/base/dd.c
@@ -28,20 +28,20 @@
 
 static void driver_bound(struct device *dev)
 {
-	if (klist_node_attached(&dev->knode_driver)) {
+	if (klist_node_attached(&dev->p->knode_driver)) {
 		printk(KERN_WARNING "%s: device %s already bound\n",
 			__func__, kobject_name(&dev->kobj));
 		return;
 	}
 
-	pr_debug("driver: '%s': %s: bound to device '%s'\n", dev->bus_id,
+	pr_debug("driver: '%s': %s: bound to device '%s'\n", dev_name(dev),
 		 __func__, dev->driver->name);
 
 	if (dev->bus)
 		blocking_notifier_call_chain(&dev->bus->p->bus_notifier,
 					     BUS_NOTIFY_BOUND_DRIVER, dev);
 
-	klist_add_tail(&dev->knode_driver, &dev->driver->p->klist_devices);
+	klist_add_tail(&dev->p->knode_driver, &dev->driver->p->klist_devices);
 }
 
 static int driver_sysfs_add(struct device *dev)
@@ -104,13 +104,13 @@ static int really_probe(struct device *dev, struct device_driver *drv)
 
 	atomic_inc(&probe_count);
 	pr_debug("bus: '%s': %s: probing driver %s with device %s\n",
-		 drv->bus->name, __func__, drv->name, dev->bus_id);
+		 drv->bus->name, __func__, drv->name, dev_name(dev));
 	WARN_ON(!list_empty(&dev->devres_head));
 
 	dev->driver = drv;
 	if (driver_sysfs_add(dev)) {
 		printk(KERN_ERR "%s: driver_sysfs_add(%s) failed\n",
-			__func__, dev->bus_id);
+			__func__, dev_name(dev));
 		goto probe_failed;
 	}
 
@@ -127,7 +127,7 @@ static int really_probe(struct device *dev, struct device_driver *drv)
 	driver_bound(dev);
 	ret = 1;
 	pr_debug("bus: '%s': %s: bound device %s to driver %s\n",
-		 drv->bus->name, __func__, dev->bus_id, drv->name);
+		 drv->bus->name, __func__, dev_name(dev), drv->name);
 	goto done;
 
 probe_failed:
@@ -139,7 +139,7 @@ probe_failed:
 		/* driver matched but the probe failed */
 		printk(KERN_WARNING
 		       "%s: probe of %s failed with error %d\n",
-		       drv->name, dev->bus_id, ret);
+		       drv->name, dev_name(dev), ret);
 	}
 	/*
 	 * Ignore errors returned by ->probe so that the next driver can try
@@ -194,7 +194,7 @@ int driver_probe_device(struct device_driver *drv, struct device *dev)
 		goto done;
 
 	pr_debug("bus: '%s': %s: matched device %s with driver %s\n",
-		 drv->bus->name, __func__, dev->bus_id, drv->name);
+		 drv->bus->name, __func__, dev_name(dev), drv->name);
 
 	ret = really_probe(dev, drv);
 
@@ -298,7 +298,6 @@ static void __device_release_driver(struct device *dev)
 	drv = dev->driver;
 	if (drv) {
 		driver_sysfs_remove(dev);
-		sysfs_remove_link(&dev->kobj, "driver");
 
 		if (dev->bus)
 			blocking_notifier_call_chain(&dev->bus->p->bus_notifier,
@@ -311,7 +310,7 @@ static void __device_release_driver(struct device *dev)
 			drv->remove(dev);
 		devres_release_all(dev);
 		dev->driver = NULL;
-		klist_remove(&dev->knode_driver);
+		klist_remove(&dev->p->knode_driver);
 	}
 }
 
@@ -341,6 +340,7 @@ EXPORT_SYMBOL_GPL(device_release_driver);
  */
 void driver_detach(struct device_driver *drv)
 {
+	struct device_private *dev_prv;
 	struct device *dev;
 
 	for (;;) {
@@ -349,8 +349,10 @@ void driver_detach(struct device_driver *drv)
 			spin_unlock(&drv->p->klist_devices.k_lock);
 			break;
 		}
-		dev = list_entry(drv->p->klist_devices.k_list.prev,
-				struct device, knode_driver.n_node);
+		dev_prv = list_entry(drv->p->klist_devices.k_list.prev,
+				     struct device_private,
+				     knode_driver.n_node);
+		dev = dev_prv->device;
 		get_device(dev);
 		spin_unlock(&drv->p->klist_devices.k_lock);
 
diff --git a/drivers/base/driver.c b/drivers/base/driver.c
index 1e2bda780e4..b76cc69f110 100644
--- a/drivers/base/driver.c
+++ b/drivers/base/driver.c
@@ -19,7 +19,14 @@
 static struct device *next_device(struct klist_iter *i)
 {
 	struct klist_node *n = klist_next(i);
-	return n ? container_of(n, struct device, knode_driver) : NULL;
+	struct device *dev = NULL;
+	struct device_private *dev_prv;
+
+	if (n) {
+		dev_prv = to_device_private_driver(n);
+		dev = dev_prv->device;
+	}
+	return dev;
 }
 
 /**
@@ -42,7 +49,7 @@ int driver_for_each_device(struct device_driver *drv, struct device *start,
 		return -EINVAL;
 
 	klist_iter_init_node(&drv->p->klist_devices, &i,
-			     start ? &start->knode_driver : NULL);
+			     start ? &start->p->knode_driver : NULL);
 	while ((dev = next_device(&i)) && !error)
 		error = fn(dev, data);
 	klist_iter_exit(&i);
@@ -76,7 +83,7 @@ struct device *driver_find_device(struct device_driver *drv,
 		return NULL;
 
 	klist_iter_init_node(&drv->p->klist_devices, &i,
-			     (start ? &start->knode_driver : NULL));
+			     (start ? &start->p->knode_driver : NULL));
 	while ((dev = next_device(&i)))
 		if (match(dev, data) && get_device(dev))
 			break;
diff --git a/drivers/base/firmware_class.c b/drivers/base/firmware_class.c
index b7e571031ec..44699d9dd85 100644
--- a/drivers/base/firmware_class.c
+++ b/drivers/base/firmware_class.c
@@ -291,12 +291,6 @@ firmware_class_timeout(u_long data)
 	fw_load_abort(fw_priv);
 }
 
-static inline void fw_setup_device_id(struct device *f_dev, struct device *dev)
-{
-	/* XXX warning we should watch out for name collisions */
-	strlcpy(f_dev->bus_id, dev->bus_id, BUS_ID_SIZE);
-}
-
 static int fw_register_device(struct device **dev_p, const char *fw_name,
 			      struct device *device)
 {
@@ -321,7 +315,7 @@ static int fw_register_device(struct device **dev_p, const char *fw_name,
 	fw_priv->timeout.data = (u_long) fw_priv;
 	init_timer(&fw_priv->timeout);
 
-	fw_setup_device_id(f_dev, device);
+	dev_set_name(f_dev, dev_name(device));
 	f_dev->parent = device;
 	f_dev->class = &firmware_class;
 	dev_set_drvdata(f_dev, fw_priv);
diff --git a/drivers/base/isa.c b/drivers/base/isa.c
index efd57757494..479694b6cbe 100644
--- a/drivers/base/isa.c
+++ b/drivers/base/isa.c
@@ -11,7 +11,7 @@
 #include <linux/isa.h>
 
 static struct device isa_bus = {
-	.bus_id		= "isa"
+	.init_name	= "isa"
 };
 
 struct isa_dev {
@@ -135,9 +135,8 @@ int isa_register_driver(struct isa_driver *isa_driver, unsigned int ndev)
 		isa_dev->dev.parent	= &isa_bus;
 		isa_dev->dev.bus	= &isa_bus_type;
 
-		snprintf(isa_dev->dev.bus_id, BUS_ID_SIZE, "%s.%u",
-				isa_driver->driver.name, id);
-
+		dev_set_name(&isa_dev->dev, "%s.%u",
+			     isa_driver->driver.name, id);
 		isa_dev->dev.platform_data	= isa_driver;
 		isa_dev->dev.release		= isa_dev_release;
 		isa_dev->id			= id;
diff --git a/drivers/base/memory.c b/drivers/base/memory.c
index 5260e9e0df4..989429cfed8 100644
--- a/drivers/base/memory.c
+++ b/drivers/base/memory.c
@@ -347,8 +347,9 @@ static inline int memory_probe_init(void)
  * section belongs to...
  */
 
-static int add_memory_block(unsigned long node_id, struct mem_section *section,
-		     unsigned long state, int phys_device)
+static int add_memory_block(int nid, struct mem_section *section,
+			unsigned long state, int phys_device,
+			enum mem_add_context context)
 {
 	struct memory_block *mem = kzalloc(sizeof(*mem), GFP_KERNEL);
 	int ret = 0;
@@ -370,6 +371,10 @@ static int add_memory_block(unsigned long node_id, struct mem_section *section,
 		ret = mem_create_simple_file(mem, phys_device);
 	if (!ret)
 		ret = mem_create_simple_file(mem, removable);
+	if (!ret) {
+		if (context == HOTPLUG)
+			ret = register_mem_sect_under_node(mem, nid);
+	}
 
 	return ret;
 }
@@ -382,7 +387,7 @@ static int add_memory_block(unsigned long node_id, struct mem_section *section,
  *
  * This could be made generic for all sysdev classes.
  */
-static struct memory_block *find_memory_block(struct mem_section *section)
+struct memory_block *find_memory_block(struct mem_section *section)
 {
 	struct kobject *kobj;
 	struct sys_device *sysdev;
@@ -411,6 +416,7 @@ int remove_memory_block(unsigned long node_id, struct mem_section *section,
 	struct memory_block *mem;
 
 	mem = find_memory_block(section);
+	unregister_mem_sect_under_nodes(mem);
 	mem_remove_simple_file(mem, phys_index);
 	mem_remove_simple_file(mem, state);
 	mem_remove_simple_file(mem, phys_device);
@@ -424,9 +430,9 @@ int remove_memory_block(unsigned long node_id, struct mem_section *section,
  * need an interface for the VM to add new memory regions,
  * but without onlining it.
  */
-int register_new_memory(struct mem_section *section)
+int register_new_memory(int nid, struct mem_section *section)
 {
-	return add_memory_block(0, section, MEM_OFFLINE, 0);
+	return add_memory_block(nid, section, MEM_OFFLINE, 0, HOTPLUG);
 }
 
 int unregister_memory_section(struct mem_section *section)
@@ -458,7 +464,8 @@ int __init memory_dev_init(void)
 	for (i = 0; i < NR_MEM_SECTIONS; i++) {
 		if (!present_section_nr(i))
 			continue;
-		err = add_memory_block(0, __nr_to_section(i), MEM_ONLINE, 0);
+		err = add_memory_block(0, __nr_to_section(i), MEM_ONLINE,
+					0, BOOT);
 		if (!ret)
 			ret = err;
 	}
diff --git a/drivers/base/node.c b/drivers/base/node.c
index 91636cd8b6c..43fa90b837e 100644
--- a/drivers/base/node.c
+++ b/drivers/base/node.c
@@ -6,6 +6,7 @@
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/mm.h>
+#include <linux/memory.h>
 #include <linux/node.h>
 #include <linux/hugetlb.h>
 #include <linux/cpumask.h>
@@ -248,6 +249,105 @@ int unregister_cpu_under_node(unsigned int cpu, unsigned int nid)
 	return 0;
 }
 
+#ifdef CONFIG_MEMORY_HOTPLUG_SPARSE
+#define page_initialized(page)  (page->lru.next)
+
+static int get_nid_for_pfn(unsigned long pfn)
+{
+	struct page *page;
+
+	if (!pfn_valid_within(pfn))
+		return -1;
+	page = pfn_to_page(pfn);
+	if (!page_initialized(page))
+		return -1;
+	return pfn_to_nid(pfn);
+}
+
+/* register memory section under specified node if it spans that node */
+int register_mem_sect_under_node(struct memory_block *mem_blk, int nid)
+{
+	unsigned long pfn, sect_start_pfn, sect_end_pfn;
+
+	if (!mem_blk)
+		return -EFAULT;
+	if (!node_online(nid))
+		return 0;
+	sect_start_pfn = section_nr_to_pfn(mem_blk->phys_index);
+	sect_end_pfn = sect_start_pfn + PAGES_PER_SECTION - 1;
+	for (pfn = sect_start_pfn; pfn <= sect_end_pfn; pfn++) {
+		int page_nid;
+
+		page_nid = get_nid_for_pfn(pfn);
+		if (page_nid < 0)
+			continue;
+		if (page_nid != nid)
+			continue;
+		return sysfs_create_link_nowarn(&node_devices[nid].sysdev.kobj,
+					&mem_blk->sysdev.kobj,
+					kobject_name(&mem_blk->sysdev.kobj));
+	}
+	/* mem section does not span the specified node */
+	return 0;
+}
+
+/* unregister memory section under all nodes that it spans */
+int unregister_mem_sect_under_nodes(struct memory_block *mem_blk)
+{
+	nodemask_t unlinked_nodes;
+	unsigned long pfn, sect_start_pfn, sect_end_pfn;
+
+	if (!mem_blk)
+		return -EFAULT;
+	nodes_clear(unlinked_nodes);
+	sect_start_pfn = section_nr_to_pfn(mem_blk->phys_index);
+	sect_end_pfn = sect_start_pfn + PAGES_PER_SECTION - 1;
+	for (pfn = sect_start_pfn; pfn <= sect_end_pfn; pfn++) {
+		unsigned int nid;
+
+		nid = get_nid_for_pfn(pfn);
+		if (nid < 0)
+			continue;
+		if (!node_online(nid))
+			continue;
+		if (node_test_and_set(nid, unlinked_nodes))
+			continue;
+		sysfs_remove_link(&node_devices[nid].sysdev.kobj,
+			 kobject_name(&mem_blk->sysdev.kobj));
+	}
+	return 0;
+}
+
+static int link_mem_sections(int nid)
+{
+	unsigned long start_pfn = NODE_DATA(nid)->node_start_pfn;
+	unsigned long end_pfn = start_pfn + NODE_DATA(nid)->node_spanned_pages;
+	unsigned long pfn;
+	int err = 0;
+
+	for (pfn = start_pfn; pfn < end_pfn; pfn += PAGES_PER_SECTION) {
+		unsigned long section_nr = pfn_to_section_nr(pfn);
+		struct mem_section *mem_sect;
+		struct memory_block *mem_blk;
+		int ret;
+
+		if (!present_section_nr(section_nr))
+			continue;
+		mem_sect = __nr_to_section(section_nr);
+		mem_blk = find_memory_block(mem_sect);
+		ret = register_mem_sect_under_node(mem_blk, nid);
+		if (!err)
+			err = ret;
+
+		/* discard ref obtained in find_memory_block() */
+		kobject_put(&mem_blk->sysdev.kobj);
+	}
+	return err;
+}
+#else
+static int link_mem_sections(int nid) { return 0; }
+#endif /* CONFIG_MEMORY_HOTPLUG_SPARSE */
+
 int register_one_node(int nid)
 {
 	int error = 0;
@@ -267,6 +367,9 @@ int register_one_node(int nid)
 			if (cpu_to_node(cpu) == nid)
 				register_cpu_under_node(cpu, nid);
 		}
+
+		/* link memory sections under this node */
+		error = link_mem_sections(nid);
 	}
 
 	return error;
diff --git a/drivers/base/platform.c b/drivers/base/platform.c
index dfcbfe50486..349a1013603 100644
--- a/drivers/base/platform.c
+++ b/drivers/base/platform.c
@@ -24,7 +24,7 @@
 				 driver))
 
 struct device platform_bus = {
-	.bus_id		= "platform",
+	.init_name	= "platform",
 };
 EXPORT_SYMBOL_GPL(platform_bus);
 
@@ -242,16 +242,15 @@ int platform_device_add(struct platform_device *pdev)
 	pdev->dev.bus = &platform_bus_type;
 
 	if (pdev->id != -1)
-		snprintf(pdev->dev.bus_id, BUS_ID_SIZE, "%s.%d", pdev->name,
-			 pdev->id);
+		dev_set_name(&pdev->dev, "%s.%d", pdev->name,  pdev->id);
 	else
-		strlcpy(pdev->dev.bus_id, pdev->name, BUS_ID_SIZE);
+		dev_set_name(&pdev->dev, pdev->name);
 
 	for (i = 0; i < pdev->num_resources; i++) {
 		struct resource *p, *r = &pdev->resource[i];
 
 		if (r->name == NULL)
-			r->name = pdev->dev.bus_id;
+			r->name = dev_name(&pdev->dev);
 
 		p = r->parent;
 		if (!p) {
@@ -264,14 +263,14 @@ int platform_device_add(struct platform_device *pdev)
 		if (p && insert_resource(p, r)) {
 			printk(KERN_ERR
 			       "%s: failed to claim resource %d\n",
-			       pdev->dev.bus_id, i);
+			       dev_name(&pdev->dev), i);
 			ret = -EBUSY;
 			goto failed;
 		}
 	}
 
 	pr_debug("Registering platform device '%s'. Parent at %s\n",
-		 pdev->dev.bus_id, pdev->dev.parent->bus_id);
+		 dev_name(&pdev->dev), dev_name(pdev->dev.parent));
 
 	ret = device_add(&pdev->dev);
 	if (ret == 0)
@@ -503,8 +502,6 @@ int platform_driver_register(struct platform_driver *drv)
 		drv->driver.suspend = platform_drv_suspend;
 	if (drv->resume)
 		drv->driver.resume = platform_drv_resume;
-	if (drv->pm)
-		drv->driver.pm = &drv->pm->base;
 	return driver_register(&drv->driver);
 }
 EXPORT_SYMBOL_GPL(platform_driver_register);
@@ -609,7 +606,7 @@ static int platform_match(struct device *dev, struct device_driver *drv)
 	struct platform_device *pdev;
 
 	pdev = container_of(dev, struct platform_device, dev);
-	return (strncmp(pdev->name, drv->name, BUS_ID_SIZE) == 0);
+	return (strcmp(pdev->name, drv->name) == 0);
 }
 
 #ifdef CONFIG_PM_SLEEP
@@ -686,7 +683,10 @@ static int platform_pm_suspend(struct device *dev)
 	struct device_driver *drv = dev->driver;
 	int ret = 0;
 
-	if (drv && drv->pm) {
+	if (!drv)
+		return 0;
+
+	if (drv->pm) {
 		if (drv->pm->suspend)
 			ret = drv->pm->suspend(dev);
 	} else {
@@ -698,16 +698,15 @@ static int platform_pm_suspend(struct device *dev)
 
 static int platform_pm_suspend_noirq(struct device *dev)
 {
-	struct platform_driver *pdrv;
+	struct device_driver *drv = dev->driver;
 	int ret = 0;
 
-	if (!dev->driver)
+	if (!drv)
 		return 0;
 
-	pdrv = to_platform_driver(dev->driver);
-	if (pdrv->pm) {
-		if (pdrv->pm->suspend_noirq)
-			ret = pdrv->pm->suspend_noirq(dev);
+	if (drv->pm) {
+		if (drv->pm->suspend_noirq)
+			ret = drv->pm->suspend_noirq(dev);
 	} else {
 		ret = platform_legacy_suspend_late(dev, PMSG_SUSPEND);
 	}
@@ -720,7 +719,10 @@ static int platform_pm_resume(struct device *dev)
 	struct device_driver *drv = dev->driver;
 	int ret = 0;
 
-	if (drv && drv->pm) {
+	if (!drv)
+		return 0;
+
+	if (drv->pm) {
 		if (drv->pm->resume)
 			ret = drv->pm->resume(dev);
 	} else {
@@ -732,16 +734,15 @@ static int platform_pm_resume(struct device *dev)
 
 static int platform_pm_resume_noirq(struct device *dev)
 {
-	struct platform_driver *pdrv;
+	struct device_driver *drv = dev->driver;
 	int ret = 0;
 
-	if (!dev->driver)
+	if (!drv)
 		return 0;
 
-	pdrv = to_platform_driver(dev->driver);
-	if (pdrv->pm) {
-		if (pdrv->pm->resume_noirq)
-			ret = pdrv->pm->resume_noirq(dev);
+	if (drv->pm) {
+		if (drv->pm->resume_noirq)
+			ret = drv->pm->resume_noirq(dev);
 	} else {
 		ret = platform_legacy_resume_early(dev);
 	}
@@ -780,16 +781,15 @@ static int platform_pm_freeze(struct device *dev)
 
 static int platform_pm_freeze_noirq(struct device *dev)
 {
-	struct platform_driver *pdrv;
+	struct device_driver *drv = dev->driver;
 	int ret = 0;
 
-	if (!dev->driver)
+	if (!drv)
 		return 0;
 
-	pdrv = to_platform_driver(dev->driver);
-	if (pdrv->pm) {
-		if (pdrv->pm->freeze_noirq)
-			ret = pdrv->pm->freeze_noirq(dev);
+	if (drv->pm) {
+		if (drv->pm->freeze_noirq)
+			ret = drv->pm->freeze_noirq(dev);
 	} else {
 		ret = platform_legacy_suspend_late(dev, PMSG_FREEZE);
 	}
@@ -802,7 +802,10 @@ static int platform_pm_thaw(struct device *dev)
 	struct device_driver *drv = dev->driver;
 	int ret = 0;
 
-	if (drv && drv->pm) {
+	if (!drv)
+		return 0;
+
+	if (drv->pm) {
 		if (drv->pm->thaw)
 			ret = drv->pm->thaw(dev);
 	} else {
@@ -814,16 +817,15 @@ static int platform_pm_thaw(struct device *dev)
 
 static int platform_pm_thaw_noirq(struct device *dev)
 {
-	struct platform_driver *pdrv;
+	struct device_driver *drv = dev->driver;
 	int ret = 0;
 
-	if (!dev->driver)
+	if (!drv)
 		return 0;
 
-	pdrv = to_platform_driver(dev->driver);
-	if (pdrv->pm) {
-		if (pdrv->pm->thaw_noirq)
-			ret = pdrv->pm->thaw_noirq(dev);
+	if (drv->pm) {
+		if (drv->pm->thaw_noirq)
+			ret = drv->pm->thaw_noirq(dev);
 	} else {
 		ret = platform_legacy_resume_early(dev);
 	}
@@ -836,7 +838,10 @@ static int platform_pm_poweroff(struct device *dev)
 	struct device_driver *drv = dev->driver;
 	int ret = 0;
 
-	if (drv && drv->pm) {
+	if (!drv)
+		return 0;
+
+	if (drv->pm) {
 		if (drv->pm->poweroff)
 			ret = drv->pm->poweroff(dev);
 	} else {
@@ -848,16 +853,15 @@ static int platform_pm_poweroff(struct device *dev)
 
 static int platform_pm_poweroff_noirq(struct device *dev)
 {
-	struct platform_driver *pdrv;
+	struct device_driver *drv = dev->driver;
 	int ret = 0;
 
-	if (!dev->driver)
+	if (!drv)
 		return 0;
 
-	pdrv = to_platform_driver(dev->driver);
-	if (pdrv->pm) {
-		if (pdrv->pm->poweroff_noirq)
-			ret = pdrv->pm->poweroff_noirq(dev);
+	if (drv->pm) {
+		if (drv->pm->poweroff_noirq)
+			ret = drv->pm->poweroff_noirq(dev);
 	} else {
 		ret = platform_legacy_suspend_late(dev, PMSG_HIBERNATE);
 	}
@@ -870,7 +874,10 @@ static int platform_pm_restore(struct device *dev)
 	struct device_driver *drv = dev->driver;
 	int ret = 0;
 
-	if (drv && drv->pm) {
+	if (!drv)
+		return 0;
+
+	if (drv->pm) {
 		if (drv->pm->restore)
 			ret = drv->pm->restore(dev);
 	} else {
@@ -882,16 +889,15 @@ static int platform_pm_restore(struct device *dev)
 
 static int platform_pm_restore_noirq(struct device *dev)
 {
-	struct platform_driver *pdrv;
+	struct device_driver *drv = dev->driver;
 	int ret = 0;
 
-	if (!dev->driver)
+	if (!drv)
 		return 0;
 
-	pdrv = to_platform_driver(dev->driver);
-	if (pdrv->pm) {
-		if (pdrv->pm->restore_noirq)
-			ret = pdrv->pm->restore_noirq(dev);
+	if (drv->pm) {
+		if (drv->pm->restore_noirq)
+			ret = drv->pm->restore_noirq(dev);
 	} else {
 		ret = platform_legacy_resume_early(dev);
 	}
@@ -912,17 +918,15 @@ static int platform_pm_restore_noirq(struct device *dev)
 
 #endif /* !CONFIG_HIBERNATION */
 
-static struct pm_ext_ops platform_pm_ops = {
-	.base = {
-		.prepare = platform_pm_prepare,
-		.complete = platform_pm_complete,
-		.suspend = platform_pm_suspend,
-		.resume = platform_pm_resume,
-		.freeze = platform_pm_freeze,
-		.thaw = platform_pm_thaw,
-		.poweroff = platform_pm_poweroff,
-		.restore = platform_pm_restore,
-	},
+static struct dev_pm_ops platform_dev_pm_ops = {
+	.prepare = platform_pm_prepare,
+	.complete = platform_pm_complete,
+	.suspend = platform_pm_suspend,
+	.resume = platform_pm_resume,
+	.freeze = platform_pm_freeze,
+	.thaw = platform_pm_thaw,
+	.poweroff = platform_pm_poweroff,
+	.restore = platform_pm_restore,
 	.suspend_noirq = platform_pm_suspend_noirq,
 	.resume_noirq = platform_pm_resume_noirq,
 	.freeze_noirq = platform_pm_freeze_noirq,
@@ -931,7 +935,7 @@ static struct pm_ext_ops platform_pm_ops = {
 	.restore_noirq = platform_pm_restore_noirq,
 };
 
-#define PLATFORM_PM_OPS_PTR	&platform_pm_ops
+#define PLATFORM_PM_OPS_PTR	(&platform_dev_pm_ops)
 
 #else /* !CONFIG_PM_SLEEP */
 
diff --git a/drivers/base/power/main.c b/drivers/base/power/main.c
index 692c20ba514..670c9d6c140 100644
--- a/drivers/base/power/main.c
+++ b/drivers/base/power/main.c
@@ -76,7 +76,7 @@ void device_pm_add(struct device *dev)
 	if (dev->parent) {
 		if (dev->parent->power.status >= DPM_SUSPENDING)
 			dev_warn(dev, "parent %s should not be sleeping\n",
-				dev->parent->bus_id);
+				 dev_name(dev->parent));
 	} else if (transition_started) {
 		/*
 		 * We refuse to register parentless devices while a PM
@@ -112,7 +112,8 @@ void device_pm_remove(struct device *dev)
  *	@ops:	PM operations to choose from.
  *	@state:	PM transition of the system being carried out.
  */
-static int pm_op(struct device *dev, struct pm_ops *ops, pm_message_t state)
+static int pm_op(struct device *dev, struct dev_pm_ops *ops,
+			pm_message_t state)
 {
 	int error = 0;
 
@@ -174,7 +175,7 @@ static int pm_op(struct device *dev, struct pm_ops *ops, pm_message_t state)
  *	The operation is executed with interrupts disabled by the only remaining
  *	functional CPU in the system.
  */
-static int pm_noirq_op(struct device *dev, struct pm_ext_ops *ops,
+static int pm_noirq_op(struct device *dev, struct dev_pm_ops *ops,
 			pm_message_t state)
 {
 	int error = 0;
@@ -354,7 +355,7 @@ static int resume_device(struct device *dev, pm_message_t state)
 	if (dev->bus) {
 		if (dev->bus->pm) {
 			pm_dev_dbg(dev, state, "");
-			error = pm_op(dev, &dev->bus->pm->base, state);
+			error = pm_op(dev, dev->bus->pm, state);
 		} else if (dev->bus->resume) {
 			pm_dev_dbg(dev, state, "legacy ");
 			error = dev->bus->resume(dev);
@@ -451,9 +452,9 @@ static void complete_device(struct device *dev, pm_message_t state)
 		dev->type->pm->complete(dev);
 	}
 
-	if (dev->bus && dev->bus->pm && dev->bus->pm->base.complete) {
+	if (dev->bus && dev->bus->pm && dev->bus->pm->complete) {
 		pm_dev_dbg(dev, state, "completing ");
-		dev->bus->pm->base.complete(dev);
+		dev->bus->pm->complete(dev);
 	}
 
 	up(&dev->sem);
@@ -624,7 +625,7 @@ static int suspend_device(struct device *dev, pm_message_t state)
 	if (dev->bus) {
 		if (dev->bus->pm) {
 			pm_dev_dbg(dev, state, "");
-			error = pm_op(dev, &dev->bus->pm->base, state);
+			error = pm_op(dev, dev->bus->pm, state);
 		} else if (dev->bus->suspend) {
 			pm_dev_dbg(dev, state, "legacy ");
 			error = dev->bus->suspend(dev, state);
@@ -685,10 +686,10 @@ static int prepare_device(struct device *dev, pm_message_t state)
 
 	down(&dev->sem);
 
-	if (dev->bus && dev->bus->pm && dev->bus->pm->base.prepare) {
+	if (dev->bus && dev->bus->pm && dev->bus->pm->prepare) {
 		pm_dev_dbg(dev, state, "preparing ");
-		error = dev->bus->pm->base.prepare(dev);
-		suspend_report_result(dev->bus->pm->base.prepare, error);
+		error = dev->bus->pm->prepare(dev);
+		suspend_report_result(dev->bus->pm->prepare, error);
 		if (error)
 			goto End;
 	}
diff --git a/drivers/base/power/trace.c b/drivers/base/power/trace.c
index 2aa6e8fc4de..0a1a2c4dbc6 100644
--- a/drivers/base/power/trace.c
+++ b/drivers/base/power/trace.c
@@ -140,7 +140,7 @@ static unsigned int hash_string(unsigned int seed, const char *data, unsigned in
 
 void set_trace_device(struct device *dev)
 {
-	dev_hash_value = hash_string(DEVSEED, dev->bus_id, DEVHASH);
+	dev_hash_value = hash_string(DEVSEED, dev_name(dev), DEVHASH);
 }
 EXPORT_SYMBOL(set_trace_device);
 
@@ -192,7 +192,7 @@ static int show_dev_hash(unsigned int value)
 
 	while (entry != &dpm_list) {
 		struct device * dev = to_device(entry);
-		unsigned int hash = hash_string(DEVSEED, dev->bus_id, DEVHASH);
+		unsigned int hash = hash_string(DEVSEED, dev_name(dev), DEVHASH);
 		if (hash == value) {
 			dev_info(dev, "hash matches\n");
 			match++;
diff --git a/drivers/char/Kconfig b/drivers/char/Kconfig
index 1697043119b..35914b6e1d2 100644
--- a/drivers/char/Kconfig
+++ b/drivers/char/Kconfig
@@ -841,7 +841,7 @@ config JS_RTC
 
 config GEN_RTC
 	tristate "Generic /dev/rtc emulation"
-	depends on RTC!=y && !IA64 && !ARM && !M32R && !MIPS && !SPARC && !FRV && !S390 && !SUPERH && !AVR32
+	depends on RTC!=y && !IA64 && !ARM && !M32R && !MIPS && !SPARC && !FRV && !S390 && !SUPERH && !AVR32 && !BLACKFIN
 	---help---
 	  If you say Y here and create a character special file /dev/rtc with
 	  major number 10 and minor number 135 using mknod ("man mknod"), you
diff --git a/drivers/char/consolemap.c b/drivers/char/consolemap.c
index 4246b8e36cb..45d3e80156d 100644
--- a/drivers/char/consolemap.c
+++ b/drivers/char/consolemap.c
@@ -554,7 +554,7 @@ int con_set_unimap(struct vc_data *vc, ushort ct, struct unipair __user *list)
 		__get_user(fontpos, &list->fontpos);
 		if ((err1 = con_insert_unipair(p, unicode,fontpos)) != 0)
 			err = err1;
-			list++;
+		list++;
 	}
 	
 	if (con_unify_unimap(vc, p))
diff --git a/drivers/char/mem.c b/drivers/char/mem.c
index 6431f6921a6..3586b3b3df3 100644
--- a/drivers/char/mem.c
+++ b/drivers/char/mem.c
@@ -425,9 +425,6 @@ static ssize_t read_oldmem(struct file *file, char __user *buf,
 }
 #endif
 
-extern long vread(char *buf, char *addr, unsigned long count);
-extern long vwrite(char *buf, char *addr, unsigned long count);
-
 #ifdef CONFIG_DEVKMEM
 /*
  * This function reads the *virtual* memory as seen by the kernel.
diff --git a/drivers/char/mwave/mwavedd.c b/drivers/char/mwave/mwavedd.c
index 4f8d67fed29..94ad2c3bfc4 100644
--- a/drivers/char/mwave/mwavedd.c
+++ b/drivers/char/mwave/mwavedd.c
@@ -663,7 +663,7 @@ static int __init mwave_init(void)
 #if 0
 	/* sysfs */
 	memset(&mwave_device, 0, sizeof (struct device));
-	snprintf(mwave_device.bus_id, BUS_ID_SIZE, "mwave");
+	dev_set_name(&mwave_device, "mwave");
 
 	if (device_register(&mwave_device))
 		goto cleanup_error;
diff --git a/drivers/char/random.c b/drivers/char/random.c
index c7afc068c28..7c13581ca9c 100644
--- a/drivers/char/random.c
+++ b/drivers/char/random.c
@@ -407,7 +407,7 @@ struct entropy_store {
 	/* read-write data: */
 	spinlock_t lock;
 	unsigned add_ptr;
-	int entropy_count;	/* Must at no time exceed ->POOLBITS! */
+	int entropy_count;
 	int input_rotate;
 };
 
@@ -767,11 +767,10 @@ static size_t account(struct entropy_store *r, size_t nbytes, int min,
 {
 	unsigned long flags;
 
-	BUG_ON(r->entropy_count > r->poolinfo->POOLBITS);
-
 	/* Hold lock while accounting */
 	spin_lock_irqsave(&r->lock, flags);
 
+	BUG_ON(r->entropy_count > r->poolinfo->POOLBITS);
 	DEBUG_ENT("trying to extract %d bits from %s\n",
 		  nbytes * 8, r->name);
 
diff --git a/drivers/char/sysrq.c b/drivers/char/sysrq.c
index 94966edfb44..d41b9f6f790 100644
--- a/drivers/char/sysrq.c
+++ b/drivers/char/sysrq.c
@@ -82,7 +82,7 @@ static void sysrq_handle_loglevel(int key, struct tty_struct *tty)
 }
 static struct sysrq_key_op sysrq_loglevel_op = {
 	.handler	= sysrq_handle_loglevel,
-	.help_msg	= "loglevel0-8",
+	.help_msg	= "loglevel(0-9)",
 	.action_msg	= "Changing Loglevel",
 	.enable_mask	= SYSRQ_ENABLE_LOG,
 };
@@ -233,7 +233,7 @@ static void sysrq_handle_showallcpus(int key, struct tty_struct *tty)
 
 static struct sysrq_key_op sysrq_showallcpus_op = {
 	.handler	= sysrq_handle_showallcpus,
-	.help_msg	= "aLlcpus",
+	.help_msg	= "show-backtrace-all-active-cpus(L)",
 	.action_msg	= "Show backtrace of all active CPUs",
 	.enable_mask	= SYSRQ_ENABLE_DUMP,
 };
@@ -247,7 +247,7 @@ static void sysrq_handle_showregs(int key, struct tty_struct *tty)
 }
 static struct sysrq_key_op sysrq_showregs_op = {
 	.handler	= sysrq_handle_showregs,
-	.help_msg	= "showPc",
+	.help_msg	= "show-registers(P)",
 	.action_msg	= "Show Regs",
 	.enable_mask	= SYSRQ_ENABLE_DUMP,
 };
@@ -258,7 +258,7 @@ static void sysrq_handle_showstate(int key, struct tty_struct *tty)
 }
 static struct sysrq_key_op sysrq_showstate_op = {
 	.handler	= sysrq_handle_showstate,
-	.help_msg	= "showTasks",
+	.help_msg	= "show-task-states(T)",
 	.action_msg	= "Show State",
 	.enable_mask	= SYSRQ_ENABLE_DUMP,
 };
@@ -269,7 +269,7 @@ static void sysrq_handle_showstate_blocked(int key, struct tty_struct *tty)
 }
 static struct sysrq_key_op sysrq_showstate_blocked_op = {
 	.handler	= sysrq_handle_showstate_blocked,
-	.help_msg	= "shoW-blocked-tasks",
+	.help_msg	= "show-blocked-tasks(W)",
 	.action_msg	= "Show Blocked State",
 	.enable_mask	= SYSRQ_ENABLE_DUMP,
 };
@@ -297,7 +297,7 @@ static void sysrq_handle_showmem(int key, struct tty_struct *tty)
 }
 static struct sysrq_key_op sysrq_showmem_op = {
 	.handler	= sysrq_handle_showmem,
-	.help_msg	= "showMem",
+	.help_msg	= "show-memory-usage(M)",
 	.action_msg	= "Show Memory",
 	.enable_mask	= SYSRQ_ENABLE_DUMP,
 };
@@ -323,7 +323,7 @@ static void sysrq_handle_term(int key, struct tty_struct *tty)
 }
 static struct sysrq_key_op sysrq_term_op = {
 	.handler	= sysrq_handle_term,
-	.help_msg	= "tErm",
+	.help_msg	= "terminate-all-tasks(E)",
 	.action_msg	= "Terminate All Tasks",
 	.enable_mask	= SYSRQ_ENABLE_SIGNAL,
 };
@@ -341,7 +341,7 @@ static void sysrq_handle_moom(int key, struct tty_struct *tty)
 }
 static struct sysrq_key_op sysrq_moom_op = {
 	.handler	= sysrq_handle_moom,
-	.help_msg	= "Full",
+	.help_msg	= "memory-full-oom-kill(F)",
 	.action_msg	= "Manual OOM execution",
 	.enable_mask	= SYSRQ_ENABLE_SIGNAL,
 };
@@ -353,7 +353,7 @@ static void sysrq_handle_kill(int key, struct tty_struct *tty)
 }
 static struct sysrq_key_op sysrq_kill_op = {
 	.handler	= sysrq_handle_kill,
-	.help_msg	= "kIll",
+	.help_msg	= "kill-all-tasks(I)",
 	.action_msg	= "Kill All Tasks",
 	.enable_mask	= SYSRQ_ENABLE_SIGNAL,
 };
@@ -364,7 +364,7 @@ static void sysrq_handle_unrt(int key, struct tty_struct *tty)
 }
 static struct sysrq_key_op sysrq_unrt_op = {
 	.handler	= sysrq_handle_unrt,
-	.help_msg	= "Nice",
+	.help_msg	= "nice-all-RT-tasks(N)",
 	.action_msg	= "Nice All RT Tasks",
 	.enable_mask	= SYSRQ_ENABLE_RTNICE,
 };
diff --git a/drivers/edac/Kconfig b/drivers/edac/Kconfig
index e2667a8c299..eee47fd16d7 100644
--- a/drivers/edac/Kconfig
+++ b/drivers/edac/Kconfig
@@ -109,6 +109,13 @@ config EDAC_X38
 	  Support for error detection and correction on the Intel
 	  X38 server chipsets.
 
+config EDAC_I5400
+	tristate "Intel 5400 (Seaburg) chipsets"
+	depends on EDAC_MM_EDAC && PCI && X86
+	help
+	  Support for error detection and correction the Intel
+	  i5400 MCH chipset (Seaburg).
+
 config EDAC_I82860
 	tristate "Intel 82860"
 	depends on EDAC_MM_EDAC && PCI && X86_32
diff --git a/drivers/edac/Makefile b/drivers/edac/Makefile
index 62c2d9bad8d..b75196927de 100644
--- a/drivers/edac/Makefile
+++ b/drivers/edac/Makefile
@@ -20,6 +20,7 @@ endif
 obj-$(CONFIG_EDAC_AMD76X)		+= amd76x_edac.o
 obj-$(CONFIG_EDAC_I5000)		+= i5000_edac.o
 obj-$(CONFIG_EDAC_I5100)		+= i5100_edac.o
+obj-$(CONFIG_EDAC_I5400)		+= i5400_edac.o
 obj-$(CONFIG_EDAC_E7XXX)		+= e7xxx_edac.o
 obj-$(CONFIG_EDAC_E752X)		+= e752x_edac.o
 obj-$(CONFIG_EDAC_I82443BXGX)		+= i82443bxgx_edac.o
diff --git a/drivers/edac/edac_device.c b/drivers/edac/edac_device.c
index 4041e914328..ca9113e1c10 100644
--- a/drivers/edac/edac_device.c
+++ b/drivers/edac/edac_device.c
@@ -333,7 +333,7 @@ static int add_edac_dev_to_global_list(struct edac_device_ctl_info *edac_dev)
 fail0:
 	edac_printk(KERN_WARNING, EDAC_MC,
 			"%s (%s) %s %s already assigned %d\n",
-			rover->dev->bus_id, edac_dev_name(rover),
+			dev_name(rover->dev), edac_dev_name(rover),
 			rover->mod_name, rover->ctl_name, rover->dev_idx);
 	return 1;
 
diff --git a/drivers/edac/edac_mc.c b/drivers/edac/edac_mc.c
index d110392d48f..25d66940b4f 100644
--- a/drivers/edac/edac_mc.c
+++ b/drivers/edac/edac_mc.c
@@ -401,7 +401,7 @@ static int add_mc_to_global_list(struct mem_ctl_info *mci)
 
 fail0:
 	edac_printk(KERN_WARNING, EDAC_MC,
-		"%s (%s) %s %s already assigned %d\n", p->dev->bus_id,
+		"%s (%s) %s %s already assigned %d\n", dev_name(p->dev),
 		edac_dev_name(mci), p->mod_name, p->ctl_name, p->mc_idx);
 	return 1;
 
diff --git a/drivers/edac/edac_pci.c b/drivers/edac/edac_pci.c
index 22ec9d5d431..5d3c8083a40 100644
--- a/drivers/edac/edac_pci.c
+++ b/drivers/edac/edac_pci.c
@@ -150,7 +150,7 @@ static int add_edac_pci_to_global_list(struct edac_pci_ctl_info *pci)
 fail0:
 	edac_printk(KERN_WARNING, EDAC_PCI,
 		"%s (%s) %s %s already assigned %d\n",
-		rover->dev->bus_id, edac_dev_name(rover),
+		dev_name(rover->dev), edac_dev_name(rover),
 		rover->mod_name, rover->ctl_name, rover->pci_idx);
 	return 1;
 
diff --git a/drivers/edac/edac_pci_sysfs.c b/drivers/edac/edac_pci_sysfs.c
index 5c153dccc95..422728cfe99 100644
--- a/drivers/edac/edac_pci_sysfs.c
+++ b/drivers/edac/edac_pci_sysfs.c
@@ -569,7 +569,7 @@ static void edac_pci_dev_parity_test(struct pci_dev *dev)
 
 	local_irq_restore(flags);
 
-	debugf4("PCI STATUS= 0x%04x %s\n", status, dev->dev.bus_id);
+	debugf4("PCI STATUS= 0x%04x %s\n", status, dev_name(&dev->dev));
 
 	/* check the status reg for errors on boards NOT marked as broken
 	 * if broken, we cannot trust any of the status bits
@@ -600,13 +600,13 @@ static void edac_pci_dev_parity_test(struct pci_dev *dev)
 	}
 
 
-	debugf4("PCI HEADER TYPE= 0x%02x %s\n", header_type, dev->dev.bus_id);
+	debugf4("PCI HEADER TYPE= 0x%02x %s\n", header_type, dev_name(&dev->dev));
 
 	if ((header_type & 0x7F) == PCI_HEADER_TYPE_BRIDGE) {
 		/* On bridges, need to examine secondary status register  */
 		status = get_pci_parity_status(dev, 1);
 
-		debugf4("PCI SEC_STATUS= 0x%04x %s\n", status, dev->dev.bus_id);
+		debugf4("PCI SEC_STATUS= 0x%04x %s\n", status, dev_name(&dev->dev));
 
 		/* check the secondary status reg for errors,
 		 * on NOT broken boards
diff --git a/drivers/edac/i5400_edac.c b/drivers/edac/i5400_edac.c
new file mode 100644
index 00000000000..b08b6d8e2dc
--- /dev/null
+++ b/drivers/edac/i5400_edac.c
@@ -0,0 +1,1476 @@
+/*
+ * Intel 5400 class Memory Controllers kernel module (Seaburg)
+ *
+ * This file may be distributed under the terms of the
+ * GNU General Public License.
+ *
+ * Copyright (c) 2008 by:
+ *	 Ben Woodard <woodard@redhat.com>
+ *	 Mauro Carvalho Chehab <mchehab@redhat.com>
+ *
+ * Red Hat Inc. http://www.redhat.com
+ *
+ * Forked and adapted from the i5000_edac driver which was
+ * written by Douglas Thompson Linux Networx <norsk5@xmission.com>
+ *
+ * This module is based on the following document:
+ *
+ * Intel 5400 Chipset Memory Controller Hub (MCH) - Datasheet
+ * 	http://developer.intel.com/design/chipsets/datashts/313070.htm
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/pci.h>
+#include <linux/pci_ids.h>
+#include <linux/slab.h>
+#include <linux/edac.h>
+#include <linux/mmzone.h>
+
+#include "edac_core.h"
+
+/*
+ * Alter this version for the I5400 module when modifications are made
+ */
+#define I5400_REVISION    " Ver: 1.0.0 " __DATE__
+
+#define EDAC_MOD_STR      "i5400_edac"
+
+#define i5400_printk(level, fmt, arg...) \
+	edac_printk(level, "i5400", fmt, ##arg)
+
+#define i5400_mc_printk(mci, level, fmt, arg...) \
+	edac_mc_chipset_printk(mci, level, "i5400", fmt, ##arg)
+
+/* Limits for i5400 */
+#define NUM_MTRS_PER_BRANCH	4
+#define CHANNELS_PER_BRANCH	2
+#define	MAX_CHANNELS		4
+#define MAX_DIMMS		(MAX_CHANNELS * 4)	/* Up to 4 DIMM's per channel */
+#define MAX_CSROWS		(MAX_DIMMS * 2)		/* max possible csrows per channel */
+
+/* Device 16,
+ * Function 0: System Address
+ * Function 1: Memory Branch Map, Control, Errors Register
+ * Function 2: FSB Error Registers
+ *
+ * All 3 functions of Device 16 (0,1,2) share the SAME DID and
+ * uses PCI_DEVICE_ID_INTEL_5400_ERR for device 16 (0,1,2),
+ * PCI_DEVICE_ID_INTEL_5400_FBD0 and PCI_DEVICE_ID_INTEL_5400_FBD1
+ * for device 21 (0,1).
+ */
+
+	/* OFFSETS for Function 0 */
+#define		AMBASE			0x48 /* AMB Mem Mapped Reg Region Base */
+#define		MAXCH			0x56 /* Max Channel Number */
+#define		MAXDIMMPERCH		0x57 /* Max DIMM PER Channel Number */
+
+	/* OFFSETS for Function 1 */
+#define		TOLM			0x6C
+#define		REDMEMB			0x7C
+#define			REC_ECC_LOCATOR_ODD(x)	((x) & 0x3fe00) /* bits [17:9] indicate ODD, [8:0]  indicate EVEN */
+#define		MIR0			0x80
+#define		MIR1			0x84
+#define		AMIR0			0x8c
+#define		AMIR1			0x90
+
+	/* Fatal error registers */
+#define		FERR_FAT_FBD		0x98	/* also called as FERR_FAT_FB_DIMM at datasheet */
+#define			FERR_FAT_FBDCHAN (3<<28)	/* channel index where the highest-order error occurred */
+
+#define		NERR_FAT_FBD		0x9c
+#define		FERR_NF_FBD		0xa0	/* also called as FERR_NFAT_FB_DIMM at datasheet */
+
+	/* Non-fatal error register */
+#define		NERR_NF_FBD		0xa4
+
+	/* Enable error mask */
+#define		EMASK_FBD		0xa8
+
+#define		ERR0_FBD		0xac
+#define		ERR1_FBD		0xb0
+#define		ERR2_FBD		0xb4
+#define		MCERR_FBD		0xb8
+
+	/* No OFFSETS for Device 16 Function 2 */
+
+/*
+ * Device 21,
+ * Function 0: Memory Map Branch 0
+ *
+ * Device 22,
+ * Function 0: Memory Map Branch 1
+ */
+
+	/* OFFSETS for Function 0 */
+#define AMBPRESENT_0	0x64
+#define AMBPRESENT_1	0x66
+#define MTR0		0x80
+#define MTR1		0x82
+#define MTR2		0x84
+#define MTR3		0x86
+
+	/* OFFSETS for Function 1 */
+#define NRECFGLOG		0x74
+#define RECFGLOG		0x78
+#define NRECMEMA		0xbe
+#define NRECMEMB		0xc0
+#define NRECFB_DIMMA		0xc4
+#define NRECFB_DIMMB		0xc8
+#define NRECFB_DIMMC		0xcc
+#define NRECFB_DIMMD		0xd0
+#define NRECFB_DIMME		0xd4
+#define NRECFB_DIMMF		0xd8
+#define REDMEMA			0xdC
+#define RECMEMA			0xf0
+#define RECMEMB			0xf4
+#define RECFB_DIMMA		0xf8
+#define RECFB_DIMMB		0xec
+#define RECFB_DIMMC		0xf0
+#define RECFB_DIMMD		0xf4
+#define RECFB_DIMME		0xf8
+#define RECFB_DIMMF		0xfC
+
+/*
+ * Error indicator bits and masks
+ * Error masks are according with Table 5-17 of i5400 datasheet
+ */
+
+enum error_mask {
+	EMASK_M1  = 1<<0,  /* Memory Write error on non-redundant retry */
+	EMASK_M2  = 1<<1,  /* Memory or FB-DIMM configuration CRC read error */
+	EMASK_M3  = 1<<2,  /* Reserved */
+	EMASK_M4  = 1<<3,  /* Uncorrectable Data ECC on Replay */
+	EMASK_M5  = 1<<4,  /* Aliased Uncorrectable Non-Mirrored Demand Data ECC */
+	EMASK_M6  = 1<<5,  /* Unsupported on i5400 */
+	EMASK_M7  = 1<<6,  /* Aliased Uncorrectable Resilver- or Spare-Copy Data ECC */
+	EMASK_M8  = 1<<7,  /* Aliased Uncorrectable Patrol Data ECC */
+	EMASK_M9  = 1<<8,  /* Non-Aliased Uncorrectable Non-Mirrored Demand Data ECC */
+	EMASK_M10 = 1<<9,  /* Unsupported on i5400 */
+	EMASK_M11 = 1<<10, /* Non-Aliased Uncorrectable Resilver- or Spare-Copy Data ECC  */
+	EMASK_M12 = 1<<11, /* Non-Aliased Uncorrectable Patrol Data ECC */
+	EMASK_M13 = 1<<12, /* Memory Write error on first attempt */
+	EMASK_M14 = 1<<13, /* FB-DIMM Configuration Write error on first attempt */
+	EMASK_M15 = 1<<14, /* Memory or FB-DIMM configuration CRC read error */
+	EMASK_M16 = 1<<15, /* Channel Failed-Over Occurred */
+	EMASK_M17 = 1<<16, /* Correctable Non-Mirrored Demand Data ECC */
+	EMASK_M18 = 1<<17, /* Unsupported on i5400 */
+	EMASK_M19 = 1<<18, /* Correctable Resilver- or Spare-Copy Data ECC */
+	EMASK_M20 = 1<<19, /* Correctable Patrol Data ECC */
+	EMASK_M21 = 1<<20, /* FB-DIMM Northbound parity error on FB-DIMM Sync Status */
+	EMASK_M22 = 1<<21, /* SPD protocol Error */
+	EMASK_M23 = 1<<22, /* Non-Redundant Fast Reset Timeout */
+	EMASK_M24 = 1<<23, /* Refresh error */
+	EMASK_M25 = 1<<24, /* Memory Write error on redundant retry */
+	EMASK_M26 = 1<<25, /* Redundant Fast Reset Timeout */
+	EMASK_M27 = 1<<26, /* Correctable Counter Threshold Exceeded */
+	EMASK_M28 = 1<<27, /* DIMM-Spare Copy Completed */
+	EMASK_M29 = 1<<28, /* DIMM-Isolation Completed */
+};
+
+/*
+ * Names to translate bit error into something useful
+ */
+static const char *error_name[] = {
+	[0]  = "Memory Write error on non-redundant retry",
+	[1]  = "Memory or FB-DIMM configuration CRC read error",
+	/* Reserved */
+	[3]  = "Uncorrectable Data ECC on Replay",
+	[4]  = "Aliased Uncorrectable Non-Mirrored Demand Data ECC",
+	/* M6 Unsupported on i5400 */
+	[6]  = "Aliased Uncorrectable Resilver- or Spare-Copy Data ECC",
+	[7]  = "Aliased Uncorrectable Patrol Data ECC",
+	[8]  = "Non-Aliased Uncorrectable Non-Mirrored Demand Data ECC",
+	/* M10 Unsupported on i5400 */
+	[10] = "Non-Aliased Uncorrectable Resilver- or Spare-Copy Data ECC",
+	[11] = "Non-Aliased Uncorrectable Patrol Data ECC",
+	[12] = "Memory Write error on first attempt",
+	[13] = "FB-DIMM Configuration Write error on first attempt",
+	[14] = "Memory or FB-DIMM configuration CRC read error",
+	[15] = "Channel Failed-Over Occurred",
+	[16] = "Correctable Non-Mirrored Demand Data ECC",
+	/* M18 Unsupported on i5400 */
+	[18] = "Correctable Resilver- or Spare-Copy Data ECC",
+	[19] = "Correctable Patrol Data ECC",
+	[20] = "FB-DIMM Northbound parity error on FB-DIMM Sync Status",
+	[21] = "SPD protocol Error",
+	[22] = "Non-Redundant Fast Reset Timeout",
+	[23] = "Refresh error",
+	[24] = "Memory Write error on redundant retry",
+	[25] = "Redundant Fast Reset Timeout",
+	[26] = "Correctable Counter Threshold Exceeded",
+	[27] = "DIMM-Spare Copy Completed",
+	[28] = "DIMM-Isolation Completed",
+};
+
+/* Fatal errors */
+#define ERROR_FAT_MASK		(EMASK_M1 | \
+				 EMASK_M2 | \
+				 EMASK_M23)
+
+/* Correctable errors */
+#define ERROR_NF_CORRECTABLE	(EMASK_M27 | \
+				 EMASK_M20 | \
+				 EMASK_M19 | \
+				 EMASK_M18 | \
+				 EMASK_M17 | \
+				 EMASK_M16)
+#define ERROR_NF_DIMM_SPARE	(EMASK_M29 | \
+				 EMASK_M28)
+#define ERROR_NF_SPD_PROTOCOL	(EMASK_M22)
+#define ERROR_NF_NORTH_CRC	(EMASK_M21)
+
+/* Recoverable errors */
+#define ERROR_NF_RECOVERABLE	(EMASK_M26 | \
+				 EMASK_M25 | \
+				 EMASK_M24 | \
+				 EMASK_M15 | \
+				 EMASK_M14 | \
+				 EMASK_M13 | \
+				 EMASK_M12 | \
+				 EMASK_M11 | \
+				 EMASK_M9  | \
+				 EMASK_M8  | \
+				 EMASK_M7  | \
+				 EMASK_M5)
+
+/* uncorrectable errors */
+#define ERROR_NF_UNCORRECTABLE	(EMASK_M4)
+
+/* mask to all non-fatal errors */
+#define ERROR_NF_MASK		(ERROR_NF_CORRECTABLE   | \
+				 ERROR_NF_UNCORRECTABLE | \
+				 ERROR_NF_RECOVERABLE   | \
+				 ERROR_NF_DIMM_SPARE    | \
+				 ERROR_NF_SPD_PROTOCOL  | \
+				 ERROR_NF_NORTH_CRC)
+
+/*
+ * Define error masks for the several registers
+ */
+
+/* Enable all fatal and non fatal errors */
+#define ENABLE_EMASK_ALL	(ERROR_FAT_MASK | ERROR_NF_MASK)
+
+/* mask for fatal error registers */
+#define FERR_FAT_MASK ERROR_FAT_MASK
+
+/* masks for non-fatal error register */
+static inline int to_nf_mask(unsigned int mask)
+{
+	return (mask & EMASK_M29) | (mask >> 3);
+};
+
+static inline int from_nf_ferr(unsigned int mask)
+{
+	return (mask & EMASK_M29) |		/* Bit 28 */
+	       (mask & ((1 << 28) - 1) << 3);	/* Bits 0 to 27 */
+};
+
+#define FERR_NF_MASK		to_nf_mask(ERROR_NF_MASK)
+#define FERR_NF_CORRECTABLE	to_nf_mask(ERROR_NF_CORRECTABLE)
+#define FERR_NF_DIMM_SPARE	to_nf_mask(ERROR_NF_DIMM_SPARE)
+#define FERR_NF_SPD_PROTOCOL	to_nf_mask(ERROR_NF_SPD_PROTOCOL)
+#define FERR_NF_NORTH_CRC	to_nf_mask(ERROR_NF_NORTH_CRC)
+#define FERR_NF_RECOVERABLE	to_nf_mask(ERROR_NF_RECOVERABLE)
+#define FERR_NF_UNCORRECTABLE	to_nf_mask(ERROR_NF_UNCORRECTABLE)
+
+/* Defines to extract the vaious fields from the
+ *	MTRx - Memory Technology Registers
+ */
+#define MTR_DIMMS_PRESENT(mtr)		((mtr) & (1 << 10))
+#define MTR_DIMMS_ETHROTTLE(mtr)	((mtr) & (1 << 9))
+#define MTR_DRAM_WIDTH(mtr)		(((mtr) & (1 << 8)) ? 8 : 4)
+#define MTR_DRAM_BANKS(mtr)		(((mtr) & (1 << 6)) ? 8 : 4)
+#define MTR_DRAM_BANKS_ADDR_BITS(mtr)	((MTR_DRAM_BANKS(mtr) == 8) ? 3 : 2)
+#define MTR_DIMM_RANK(mtr)		(((mtr) >> 5) & 0x1)
+#define MTR_DIMM_RANK_ADDR_BITS(mtr)	(MTR_DIMM_RANK(mtr) ? 2 : 1)
+#define MTR_DIMM_ROWS(mtr)		(((mtr) >> 2) & 0x3)
+#define MTR_DIMM_ROWS_ADDR_BITS(mtr)	(MTR_DIMM_ROWS(mtr) + 13)
+#define MTR_DIMM_COLS(mtr)		((mtr) & 0x3)
+#define MTR_DIMM_COLS_ADDR_BITS(mtr)	(MTR_DIMM_COLS(mtr) + 10)
+
+/* This applies to FERR_NF_FB-DIMM as well as FERR_FAT_FB-DIMM */
+static inline int extract_fbdchan_indx(u32 x)
+{
+	return (x>>28) & 0x3;
+}
+
+#ifdef CONFIG_EDAC_DEBUG
+/* MTR NUMROW */
+static const char *numrow_toString[] = {
+	"8,192 - 13 rows",
+	"16,384 - 14 rows",
+	"32,768 - 15 rows",
+	"65,536 - 16 rows"
+};
+
+/* MTR NUMCOL */
+static const char *numcol_toString[] = {
+	"1,024 - 10 columns",
+	"2,048 - 11 columns",
+	"4,096 - 12 columns",
+	"reserved"
+};
+#endif
+
+/* Device name and register DID (Device ID) */
+struct i5400_dev_info {
+	const char *ctl_name;	/* name for this device */
+	u16 fsb_mapping_errors;	/* DID for the branchmap,control */
+};
+
+/* Table of devices attributes supported by this driver */
+static const struct i5400_dev_info i5400_devs[] = {
+	{
+		.ctl_name = "I5400",
+		.fsb_mapping_errors = PCI_DEVICE_ID_INTEL_5400_ERR,
+	},
+};
+
+struct i5400_dimm_info {
+	int megabytes;		/* size, 0 means not present  */
+	int dual_rank;
+};
+
+/* driver private data structure */
+struct i5400_pvt {
+	struct pci_dev *system_address;		/* 16.0 */
+	struct pci_dev *branchmap_werrors;	/* 16.1 */
+	struct pci_dev *fsb_error_regs;		/* 16.2 */
+	struct pci_dev *branch_0;		/* 21.0 */
+	struct pci_dev *branch_1;		/* 22.0 */
+
+	u16 tolm;				/* top of low memory */
+	u64 ambase;				/* AMB BAR */
+
+	u16 mir0, mir1;
+
+	u16 b0_mtr[NUM_MTRS_PER_BRANCH];	/* Memory Technlogy Reg */
+	u16 b0_ambpresent0;			/* Branch 0, Channel 0 */
+	u16 b0_ambpresent1;			/* Brnach 0, Channel 1 */
+
+	u16 b1_mtr[NUM_MTRS_PER_BRANCH];	/* Memory Technlogy Reg */
+	u16 b1_ambpresent0;			/* Branch 1, Channel 8 */
+	u16 b1_ambpresent1;			/* Branch 1, Channel 1 */
+
+	/* DIMM information matrix, allocating architecture maximums */
+	struct i5400_dimm_info dimm_info[MAX_CSROWS][MAX_CHANNELS];
+
+	/* Actual values for this controller */
+	int maxch;				/* Max channels */
+	int maxdimmperch;			/* Max DIMMs per channel */
+};
+
+/* I5400 MCH error information retrieved from Hardware */
+struct i5400_error_info {
+	/* These registers are always read from the MC */
+	u32 ferr_fat_fbd;	/* First Errors Fatal */
+	u32 nerr_fat_fbd;	/* Next Errors Fatal */
+	u32 ferr_nf_fbd;	/* First Errors Non-Fatal */
+	u32 nerr_nf_fbd;	/* Next Errors Non-Fatal */
+
+	/* These registers are input ONLY if there was a Recoverable Error */
+	u32 redmemb;		/* Recoverable Mem Data Error log B */
+	u16 recmema;		/* Recoverable Mem Error log A */
+	u32 recmemb;		/* Recoverable Mem Error log B */
+
+	/* These registers are input ONLY if there was a Non-Rec Error */
+	u16 nrecmema;		/* Non-Recoverable Mem log A */
+	u16 nrecmemb;		/* Non-Recoverable Mem log B */
+
+};
+
+/* note that nrec_rdwr changed from NRECMEMA to NRECMEMB between the 5000 and
+   5400 better to use an inline function than a macro in this case */
+static inline int nrec_bank(struct i5400_error_info *info)
+{
+	return ((info->nrecmema) >> 12) & 0x7;
+}
+static inline int nrec_rank(struct i5400_error_info *info)
+{
+	return ((info->nrecmema) >> 8) & 0xf;
+}
+static inline int nrec_buf_id(struct i5400_error_info *info)
+{
+	return ((info->nrecmema)) & 0xff;
+}
+static inline int nrec_rdwr(struct i5400_error_info *info)
+{
+	return (info->nrecmemb) >> 31;
+}
+/* This applies to both NREC and REC string so it can be used with nrec_rdwr
+   and rec_rdwr */
+static inline const char *rdwr_str(int rdwr)
+{
+	return rdwr ? "Write" : "Read";
+}
+static inline int nrec_cas(struct i5400_error_info *info)
+{
+	return ((info->nrecmemb) >> 16) & 0x1fff;
+}
+static inline int nrec_ras(struct i5400_error_info *info)
+{
+	return (info->nrecmemb) & 0xffff;
+}
+static inline int rec_bank(struct i5400_error_info *info)
+{
+	return ((info->recmema) >> 12) & 0x7;
+}
+static inline int rec_rank(struct i5400_error_info *info)
+{
+	return ((info->recmema) >> 8) & 0xf;
+}
+static inline int rec_rdwr(struct i5400_error_info *info)
+{
+	return (info->recmemb) >> 31;
+}
+static inline int rec_cas(struct i5400_error_info *info)
+{
+	return ((info->recmemb) >> 16) & 0x1fff;
+}
+static inline int rec_ras(struct i5400_error_info *info)
+{
+	return (info->recmemb) & 0xffff;
+}
+
+static struct edac_pci_ctl_info *i5400_pci;
+
+/*
+ *	i5400_get_error_info	Retrieve the hardware error information from
+ *				the hardware and cache it in the 'info'
+ *				structure
+ */
+static void i5400_get_error_info(struct mem_ctl_info *mci,
+				 struct i5400_error_info *info)
+{
+	struct i5400_pvt *pvt;
+	u32 value;
+
+	pvt = mci->pvt_info;
+
+	/* read in the 1st FATAL error register */
+	pci_read_config_dword(pvt->branchmap_werrors, FERR_FAT_FBD, &value);
+
+	/* Mask only the bits that the doc says are valid
+	 */
+	value &= (FERR_FAT_FBDCHAN | FERR_FAT_MASK);
+
+	/* If there is an error, then read in the
+	   NEXT FATAL error register and the Memory Error Log Register A
+	 */
+	if (value & FERR_FAT_MASK) {
+		info->ferr_fat_fbd = value;
+
+		/* harvest the various error data we need */
+		pci_read_config_dword(pvt->branchmap_werrors,
+				NERR_FAT_FBD, &info->nerr_fat_fbd);
+		pci_read_config_word(pvt->branchmap_werrors,
+				NRECMEMA, &info->nrecmema);
+		pci_read_config_word(pvt->branchmap_werrors,
+				NRECMEMB, &info->nrecmemb);
+
+		/* Clear the error bits, by writing them back */
+		pci_write_config_dword(pvt->branchmap_werrors,
+				FERR_FAT_FBD, value);
+	} else {
+		info->ferr_fat_fbd = 0;
+		info->nerr_fat_fbd = 0;
+		info->nrecmema = 0;
+		info->nrecmemb = 0;
+	}
+
+	/* read in the 1st NON-FATAL error register */
+	pci_read_config_dword(pvt->branchmap_werrors, FERR_NF_FBD, &value);
+
+	/* If there is an error, then read in the 1st NON-FATAL error
+	 * register as well */
+	if (value & FERR_NF_MASK) {
+		info->ferr_nf_fbd = value;
+
+		/* harvest the various error data we need */
+		pci_read_config_dword(pvt->branchmap_werrors,
+				NERR_NF_FBD, &info->nerr_nf_fbd);
+		pci_read_config_word(pvt->branchmap_werrors,
+				RECMEMA, &info->recmema);
+		pci_read_config_dword(pvt->branchmap_werrors,
+				RECMEMB, &info->recmemb);
+		pci_read_config_dword(pvt->branchmap_werrors,
+				REDMEMB, &info->redmemb);
+
+		/* Clear the error bits, by writing them back */
+		pci_write_config_dword(pvt->branchmap_werrors,
+				FERR_NF_FBD, value);
+	} else {
+		info->ferr_nf_fbd = 0;
+		info->nerr_nf_fbd = 0;
+		info->recmema = 0;
+		info->recmemb = 0;
+		info->redmemb = 0;
+	}
+}
+
+/*
+ * i5400_proccess_non_recoverable_info(struct mem_ctl_info *mci,
+ * 					struct i5400_error_info *info,
+ * 					int handle_errors);
+ *
+ *	handle the Intel FATAL and unrecoverable errors, if any
+ */
+static void i5400_proccess_non_recoverable_info(struct mem_ctl_info *mci,
+				    struct i5400_error_info *info,
+				    unsigned long allErrors)
+{
+	char msg[EDAC_MC_LABEL_LEN + 1 + 90 + 80];
+	int branch;
+	int channel;
+	int bank;
+	int buf_id;
+	int rank;
+	int rdwr;
+	int ras, cas;
+	int errnum;
+	char *type = NULL;
+
+	if (!allErrors)
+		return;		/* if no error, return now */
+
+	if (allErrors &  ERROR_FAT_MASK)
+		type = "FATAL";
+	else if (allErrors & FERR_NF_UNCORRECTABLE)
+		type = "NON-FATAL uncorrected";
+	else
+		type = "NON-FATAL recoverable";
+
+	/* ONLY ONE of the possible error bits will be set, as per the docs */
+
+	branch = extract_fbdchan_indx(info->ferr_fat_fbd);
+	channel = branch;
+
+	/* Use the NON-Recoverable macros to extract data */
+	bank = nrec_bank(info);
+	rank = nrec_rank(info);
+	buf_id = nrec_buf_id(info);
+	rdwr = nrec_rdwr(info);
+	ras = nrec_ras(info);
+	cas = nrec_cas(info);
+
+	debugf0("\t\tCSROW= %d  Channels= %d,%d  (Branch= %d "
+		"DRAM Bank= %d Buffer ID = %d rdwr= %s ras= %d cas= %d)\n",
+		rank, channel, channel + 1, branch >> 1, bank,
+		buf_id, rdwr_str(rdwr), ras, cas);
+
+	/* Only 1 bit will be on */
+	errnum = find_first_bit(&allErrors, ARRAY_SIZE(error_name));
+
+	/* Form out message */
+	snprintf(msg, sizeof(msg),
+		 "%s (Branch=%d DRAM-Bank=%d Buffer ID = %d RDWR=%s "
+		 "RAS=%d CAS=%d %s Err=0x%lx (%s))",
+		 type, branch >> 1, bank, buf_id, rdwr_str(rdwr), ras, cas,
+		 type, allErrors, error_name[errnum]);
+
+	/* Call the helper to output message */
+	edac_mc_handle_fbd_ue(mci, rank, channel, channel + 1, msg);
+}
+
+/*
+ * i5400_process_fatal_error_info(struct mem_ctl_info *mci,
+ * 				struct i5400_error_info *info,
+ * 				int handle_errors);
+ *
+ *	handle the Intel NON-FATAL errors, if any
+ */
+static void i5400_process_nonfatal_error_info(struct mem_ctl_info *mci,
+					struct i5400_error_info *info)
+{
+	char msg[EDAC_MC_LABEL_LEN + 1 + 90 + 80];
+	unsigned long allErrors;
+	int branch;
+	int channel;
+	int bank;
+	int rank;
+	int rdwr;
+	int ras, cas;
+	int errnum;
+
+	/* mask off the Error bits that are possible */
+	allErrors = from_nf_ferr(info->ferr_nf_fbd & FERR_NF_MASK);
+	if (!allErrors)
+		return;		/* if no error, return now */
+
+	/* ONLY ONE of the possible error bits will be set, as per the docs */
+
+	if (allErrors & (ERROR_NF_UNCORRECTABLE | ERROR_NF_RECOVERABLE)) {
+		i5400_proccess_non_recoverable_info(mci, info, allErrors);
+		return;
+	}
+
+	/* Correctable errors */
+	if (allErrors & ERROR_NF_CORRECTABLE) {
+		debugf0("\tCorrected bits= 0x%lx\n", allErrors);
+
+		branch = extract_fbdchan_indx(info->ferr_nf_fbd);
+
+		channel = 0;
+		if (REC_ECC_LOCATOR_ODD(info->redmemb))
+			channel = 1;
+
+		/* Convert channel to be based from zero, instead of
+		 * from branch base of 0 */
+		channel += branch;
+
+		bank = rec_bank(info);
+		rank = rec_rank(info);
+		rdwr = rec_rdwr(info);
+		ras = rec_ras(info);
+		cas = rec_cas(info);
+
+		/* Only 1 bit will be on */
+		errnum = find_first_bit(&allErrors, ARRAY_SIZE(error_name));
+
+		debugf0("\t\tCSROW= %d Channel= %d  (Branch %d "
+			"DRAM Bank= %d rdwr= %s ras= %d cas= %d)\n",
+			rank, channel, branch >> 1, bank,
+			rdwr_str(rdwr), ras, cas);
+
+		/* Form out message */
+		snprintf(msg, sizeof(msg),
+			 "Corrected error (Branch=%d DRAM-Bank=%d RDWR=%s "
+			 "RAS=%d CAS=%d, CE Err=0x%lx (%s))",
+			 branch >> 1, bank, rdwr_str(rdwr), ras, cas,
+			 allErrors, error_name[errnum]);
+
+		/* Call the helper to output message */
+		edac_mc_handle_fbd_ce(mci, rank, channel, msg);
+
+		return;
+	}
+
+	/* Miscelaneous errors */
+	errnum = find_first_bit(&allErrors, ARRAY_SIZE(error_name));
+
+	branch = extract_fbdchan_indx(info->ferr_nf_fbd);
+
+	i5400_mc_printk(mci, KERN_EMERG,
+			"Non-Fatal misc error (Branch=%d Err=%#lx (%s))",
+			branch >> 1, allErrors, error_name[errnum]);
+}
+
+/*
+ *	i5400_process_error_info	Process the error info that is
+ *	in the 'info' structure, previously retrieved from hardware
+ */
+static void i5400_process_error_info(struct mem_ctl_info *mci,
+				struct i5400_error_info *info)
+{	u32 allErrors;
+
+	/* First handle any fatal errors that occurred */
+	allErrors = (info->ferr_fat_fbd & FERR_FAT_MASK);
+	i5400_proccess_non_recoverable_info(mci, info, allErrors);
+
+	/* now handle any non-fatal errors that occurred */
+	i5400_process_nonfatal_error_info(mci, info);
+}
+
+/*
+ *	i5400_clear_error	Retrieve any error from the hardware
+ *				but do NOT process that error.
+ *				Used for 'clearing' out of previous errors
+ *				Called by the Core module.
+ */
+static void i5400_clear_error(struct mem_ctl_info *mci)
+{
+	struct i5400_error_info info;
+
+	i5400_get_error_info(mci, &info);
+}
+
+/*
+ *	i5400_check_error	Retrieve and process errors reported by the
+ *				hardware. Called by the Core module.
+ */
+static void i5400_check_error(struct mem_ctl_info *mci)
+{
+	struct i5400_error_info info;
+	debugf4("MC%d: " __FILE__ ": %s()\n", mci->mc_idx, __func__);
+	i5400_get_error_info(mci, &info);
+	i5400_process_error_info(mci, &info);
+}
+
+/*
+ *	i5400_put_devices	'put' all the devices that we have
+ *				reserved via 'get'
+ */
+static void i5400_put_devices(struct mem_ctl_info *mci)
+{
+	struct i5400_pvt *pvt;
+
+	pvt = mci->pvt_info;
+
+	/* Decrement usage count for devices */
+	pci_dev_put(pvt->branch_1);
+	pci_dev_put(pvt->branch_0);
+	pci_dev_put(pvt->fsb_error_regs);
+	pci_dev_put(pvt->branchmap_werrors);
+}
+
+/*
+ *	i5400_get_devices	Find and perform 'get' operation on the MCH's
+ *			device/functions we want to reference for this driver
+ *
+ *			Need to 'get' device 16 func 1 and func 2
+ */
+static int i5400_get_devices(struct mem_ctl_info *mci, int dev_idx)
+{
+	struct i5400_pvt *pvt;
+	struct pci_dev *pdev;
+
+	pvt = mci->pvt_info;
+	pvt->branchmap_werrors = NULL;
+	pvt->fsb_error_regs = NULL;
+	pvt->branch_0 = NULL;
+	pvt->branch_1 = NULL;
+
+	/* Attempt to 'get' the MCH register we want */
+	pdev = NULL;
+	while (!pvt->branchmap_werrors || !pvt->fsb_error_regs) {
+		pdev = pci_get_device(PCI_VENDOR_ID_INTEL,
+				      PCI_DEVICE_ID_INTEL_5400_ERR, pdev);
+		if (!pdev) {
+			/* End of list, leave */
+			i5400_printk(KERN_ERR,
+				"'system address,Process Bus' "
+				"device not found:"
+				"vendor 0x%x device 0x%x ERR funcs "
+				"(broken BIOS?)\n",
+				PCI_VENDOR_ID_INTEL,
+				PCI_DEVICE_ID_INTEL_5400_ERR);
+			goto error;
+		}
+
+		/* Store device 16 funcs 1 and 2 */
+		switch (PCI_FUNC(pdev->devfn)) {
+		case 1:
+			pvt->branchmap_werrors = pdev;
+			break;
+		case 2:
+			pvt->fsb_error_regs = pdev;
+			break;
+		}
+	}
+
+	debugf1("System Address, processor bus- PCI Bus ID: %s  %x:%x\n",
+		pci_name(pvt->system_address),
+		pvt->system_address->vendor, pvt->system_address->device);
+	debugf1("Branchmap, control and errors - PCI Bus ID: %s  %x:%x\n",
+		pci_name(pvt->branchmap_werrors),
+		pvt->branchmap_werrors->vendor, pvt->branchmap_werrors->device);
+	debugf1("FSB Error Regs - PCI Bus ID: %s  %x:%x\n",
+		pci_name(pvt->fsb_error_regs),
+		pvt->fsb_error_regs->vendor, pvt->fsb_error_regs->device);
+
+	pvt->branch_0 = pci_get_device(PCI_VENDOR_ID_INTEL,
+				       PCI_DEVICE_ID_INTEL_5400_FBD0, NULL);
+	if (!pvt->branch_0) {
+		i5400_printk(KERN_ERR,
+			"MC: 'BRANCH 0' device not found:"
+			"vendor 0x%x device 0x%x Func 0 (broken BIOS?)\n",
+			PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_5400_FBD0);
+		goto error;
+	}
+
+	/* If this device claims to have more than 2 channels then
+	 * fetch Branch 1's information
+	 */
+	if (pvt->maxch < CHANNELS_PER_BRANCH)
+		return 0;
+
+	pvt->branch_1 = pci_get_device(PCI_VENDOR_ID_INTEL,
+				       PCI_DEVICE_ID_INTEL_5400_FBD1, NULL);
+	if (!pvt->branch_1) {
+		i5400_printk(KERN_ERR,
+			"MC: 'BRANCH 1' device not found:"
+			"vendor 0x%x device 0x%x Func 0 "
+			"(broken BIOS?)\n",
+			PCI_VENDOR_ID_INTEL,
+			PCI_DEVICE_ID_INTEL_5400_FBD1);
+		goto error;
+	}
+
+	return 0;
+
+error:
+	i5400_put_devices(mci);
+	return -ENODEV;
+}
+
+/*
+ *	determine_amb_present
+ *
+ *		the information is contained in NUM_MTRS_PER_BRANCH different
+ *		registers determining which of the NUM_MTRS_PER_BRANCH requires
+ *              knowing which channel is in question
+ *
+ *	2 branches, each with 2 channels
+ *		b0_ambpresent0 for channel '0'
+ *		b0_ambpresent1 for channel '1'
+ *		b1_ambpresent0 for channel '2'
+ *		b1_ambpresent1 for channel '3'
+ */
+static int determine_amb_present_reg(struct i5400_pvt *pvt, int channel)
+{
+	int amb_present;
+
+	if (channel < CHANNELS_PER_BRANCH) {
+		if (channel & 0x1)
+			amb_present = pvt->b0_ambpresent1;
+		else
+			amb_present = pvt->b0_ambpresent0;
+	} else {
+		if (channel & 0x1)
+			amb_present = pvt->b1_ambpresent1;
+		else
+			amb_present = pvt->b1_ambpresent0;
+	}
+
+	return amb_present;
+}
+
+/*
+ * determine_mtr(pvt, csrow, channel)
+ *
+ * return the proper MTR register as determine by the csrow and desired channel
+ */
+static int determine_mtr(struct i5400_pvt *pvt, int csrow, int channel)
+{
+	int mtr;
+	int n;
+
+	/* There is one MTR for each slot pair of FB-DIMMs,
+	   Each slot may have one or two ranks (2 csrows),
+	   Each slot pair may be at branch 0 or branch 1.
+	   So, csrow should be divided by eight
+	 */
+	n = csrow >> 3;
+
+	if (n >= NUM_MTRS_PER_BRANCH) {
+		debugf0("ERROR: trying to access an invalid csrow: %d\n",
+			csrow);
+		return 0;
+	}
+
+	if (channel < CHANNELS_PER_BRANCH)
+		mtr = pvt->b0_mtr[n];
+	else
+		mtr = pvt->b1_mtr[n];
+
+	return mtr;
+}
+
+/*
+ */
+static void decode_mtr(int slot_row, u16 mtr)
+{
+	int ans;
+
+	ans = MTR_DIMMS_PRESENT(mtr);
+
+	debugf2("\tMTR%d=0x%x:  DIMMs are %s\n", slot_row, mtr,
+		ans ? "Present" : "NOT Present");
+	if (!ans)
+		return;
+
+	debugf2("\t\tWIDTH: x%d\n", MTR_DRAM_WIDTH(mtr));
+
+	debugf2("\t\tELECTRICAL THROTTLING is %s\n",
+		MTR_DIMMS_ETHROTTLE(mtr) ? "enabled" : "disabled");
+
+	debugf2("\t\tNUMBANK: %d bank(s)\n", MTR_DRAM_BANKS(mtr));
+	debugf2("\t\tNUMRANK: %s\n", MTR_DIMM_RANK(mtr) ? "double" : "single");
+	debugf2("\t\tNUMROW: %s\n", numrow_toString[MTR_DIMM_ROWS(mtr)]);
+	debugf2("\t\tNUMCOL: %s\n", numcol_toString[MTR_DIMM_COLS(mtr)]);
+}
+
+static void handle_channel(struct i5400_pvt *pvt, int csrow, int channel,
+			struct i5400_dimm_info *dinfo)
+{
+	int mtr;
+	int amb_present_reg;
+	int addrBits;
+
+	mtr = determine_mtr(pvt, csrow, channel);
+	if (MTR_DIMMS_PRESENT(mtr)) {
+		amb_present_reg = determine_amb_present_reg(pvt, channel);
+
+		/* Determine if there is a DIMM present in this DIMM slot */
+		if (amb_present_reg & (1 << (csrow >> 1))) {
+			dinfo->dual_rank = MTR_DIMM_RANK(mtr);
+
+			if (!((dinfo->dual_rank == 0) &&
+				((csrow & 0x1) == 0x1))) {
+				/* Start with the number of bits for a Bank
+				 * on the DRAM */
+				addrBits = MTR_DRAM_BANKS_ADDR_BITS(mtr);
+				/* Add thenumber of ROW bits */
+				addrBits += MTR_DIMM_ROWS_ADDR_BITS(mtr);
+				/* add the number of COLUMN bits */
+				addrBits += MTR_DIMM_COLS_ADDR_BITS(mtr);
+
+				addrBits += 6;	/* add 64 bits per DIMM */
+				addrBits -= 20;	/* divide by 2^^20 */
+				addrBits -= 3;	/* 8 bits per bytes */
+
+				dinfo->megabytes = 1 << addrBits;
+			}
+		}
+	}
+}
+
+/*
+ *	calculate_dimm_size
+ *
+ *	also will output a DIMM matrix map, if debug is enabled, for viewing
+ *	how the DIMMs are populated
+ */
+static void calculate_dimm_size(struct i5400_pvt *pvt)
+{
+	struct i5400_dimm_info *dinfo;
+	int csrow, max_csrows;
+	char *p, *mem_buffer;
+	int space, n;
+	int channel;
+
+	/* ================= Generate some debug output ================= */
+	space = PAGE_SIZE;
+	mem_buffer = p = kmalloc(space, GFP_KERNEL);
+	if (p == NULL) {
+		i5400_printk(KERN_ERR, "MC: %s:%s() kmalloc() failed\n",
+			__FILE__, __func__);
+		return;
+	}
+
+	/* Scan all the actual CSROWS (which is # of DIMMS * 2)
+	 * and calculate the information for each DIMM
+	 * Start with the highest csrow first, to display it first
+	 * and work toward the 0th csrow
+	 */
+	max_csrows = pvt->maxdimmperch * 2;
+	for (csrow = max_csrows - 1; csrow >= 0; csrow--) {
+
+		/* on an odd csrow, first output a 'boundary' marker,
+		 * then reset the message buffer  */
+		if (csrow & 0x1) {
+			n = snprintf(p, space, "---------------------------"
+					"--------------------------------");
+			p += n;
+			space -= n;
+			debugf2("%s\n", mem_buffer);
+			p = mem_buffer;
+			space = PAGE_SIZE;
+		}
+		n = snprintf(p, space, "csrow %2d    ", csrow);
+		p += n;
+		space -= n;
+
+		for (channel = 0; channel < pvt->maxch; channel++) {
+			dinfo = &pvt->dimm_info[csrow][channel];
+			handle_channel(pvt, csrow, channel, dinfo);
+			n = snprintf(p, space, "%4d MB   | ", dinfo->megabytes);
+			p += n;
+			space -= n;
+		}
+		debugf2("%s\n", mem_buffer);
+		p = mem_buffer;
+		space = PAGE_SIZE;
+	}
+
+	/* Output the last bottom 'boundary' marker */
+	n = snprintf(p, space, "---------------------------"
+			"--------------------------------");
+	p += n;
+	space -= n;
+	debugf2("%s\n", mem_buffer);
+	p = mem_buffer;
+	space = PAGE_SIZE;
+
+	/* now output the 'channel' labels */
+	n = snprintf(p, space, "            ");
+	p += n;
+	space -= n;
+	for (channel = 0; channel < pvt->maxch; channel++) {
+		n = snprintf(p, space, "channel %d | ", channel);
+		p += n;
+		space -= n;
+	}
+
+	/* output the last message and free buffer */
+	debugf2("%s\n", mem_buffer);
+	kfree(mem_buffer);
+}
+
+/*
+ *	i5400_get_mc_regs	read in the necessary registers and
+ *				cache locally
+ *
+ *			Fills in the private data members
+ */
+static void i5400_get_mc_regs(struct mem_ctl_info *mci)
+{
+	struct i5400_pvt *pvt;
+	u32 actual_tolm;
+	u16 limit;
+	int slot_row;
+	int maxch;
+	int maxdimmperch;
+	int way0, way1;
+
+	pvt = mci->pvt_info;
+
+	pci_read_config_dword(pvt->system_address, AMBASE,
+			(u32 *) &pvt->ambase);
+	pci_read_config_dword(pvt->system_address, AMBASE + sizeof(u32),
+			((u32 *) &pvt->ambase) + sizeof(u32));
+
+	maxdimmperch = pvt->maxdimmperch;
+	maxch = pvt->maxch;
+
+	debugf2("AMBASE= 0x%lx  MAXCH= %d  MAX-DIMM-Per-CH= %d\n",
+		(long unsigned int)pvt->ambase, pvt->maxch, pvt->maxdimmperch);
+
+	/* Get the Branch Map regs */
+	pci_read_config_word(pvt->branchmap_werrors, TOLM, &pvt->tolm);
+	pvt->tolm >>= 12;
+	debugf2("\nTOLM (number of 256M regions) =%u (0x%x)\n", pvt->tolm,
+		pvt->tolm);
+
+	actual_tolm = (u32) ((1000l * pvt->tolm) >> (30 - 28));
+	debugf2("Actual TOLM byte addr=%u.%03u GB (0x%x)\n",
+		actual_tolm/1000, actual_tolm % 1000, pvt->tolm << 28);
+
+	pci_read_config_word(pvt->branchmap_werrors, MIR0, &pvt->mir0);
+	pci_read_config_word(pvt->branchmap_werrors, MIR1, &pvt->mir1);
+
+	/* Get the MIR[0-1] regs */
+	limit = (pvt->mir0 >> 4) & 0x0fff;
+	way0 = pvt->mir0 & 0x1;
+	way1 = pvt->mir0 & 0x2;
+	debugf2("MIR0: limit= 0x%x  WAY1= %u  WAY0= %x\n", limit, way1, way0);
+	limit = (pvt->mir1 >> 4) & 0xfff;
+	way0 = pvt->mir1 & 0x1;
+	way1 = pvt->mir1 & 0x2;
+	debugf2("MIR1: limit= 0x%x  WAY1= %u  WAY0= %x\n", limit, way1, way0);
+
+	/* Get the set of MTR[0-3] regs by each branch */
+	for (slot_row = 0; slot_row < NUM_MTRS_PER_BRANCH; slot_row++) {
+		int where = MTR0 + (slot_row * sizeof(u32));
+
+		/* Branch 0 set of MTR registers */
+		pci_read_config_word(pvt->branch_0, where,
+				&pvt->b0_mtr[slot_row]);
+
+		debugf2("MTR%d where=0x%x B0 value=0x%x\n", slot_row, where,
+			pvt->b0_mtr[slot_row]);
+
+		if (pvt->maxch < CHANNELS_PER_BRANCH) {
+			pvt->b1_mtr[slot_row] = 0;
+			continue;
+		}
+
+		/* Branch 1 set of MTR registers */
+		pci_read_config_word(pvt->branch_1, where,
+				&pvt->b1_mtr[slot_row]);
+		debugf2("MTR%d where=0x%x B1 value=0x%x\n", slot_row, where,
+			pvt->b1_mtr[slot_row]);
+	}
+
+	/* Read and dump branch 0's MTRs */
+	debugf2("\nMemory Technology Registers:\n");
+	debugf2("   Branch 0:\n");
+	for (slot_row = 0; slot_row < NUM_MTRS_PER_BRANCH; slot_row++)
+		decode_mtr(slot_row, pvt->b0_mtr[slot_row]);
+
+	pci_read_config_word(pvt->branch_0, AMBPRESENT_0,
+			&pvt->b0_ambpresent0);
+	debugf2("\t\tAMB-Branch 0-present0 0x%x:\n", pvt->b0_ambpresent0);
+	pci_read_config_word(pvt->branch_0, AMBPRESENT_1,
+			&pvt->b0_ambpresent1);
+	debugf2("\t\tAMB-Branch 0-present1 0x%x:\n", pvt->b0_ambpresent1);
+
+	/* Only if we have 2 branchs (4 channels) */
+	if (pvt->maxch < CHANNELS_PER_BRANCH) {
+		pvt->b1_ambpresent0 = 0;
+		pvt->b1_ambpresent1 = 0;
+	} else {
+		/* Read and dump  branch 1's MTRs */
+		debugf2("   Branch 1:\n");
+		for (slot_row = 0; slot_row < NUM_MTRS_PER_BRANCH; slot_row++)
+			decode_mtr(slot_row, pvt->b1_mtr[slot_row]);
+
+		pci_read_config_word(pvt->branch_1, AMBPRESENT_0,
+				&pvt->b1_ambpresent0);
+		debugf2("\t\tAMB-Branch 1-present0 0x%x:\n",
+			pvt->b1_ambpresent0);
+		pci_read_config_word(pvt->branch_1, AMBPRESENT_1,
+				&pvt->b1_ambpresent1);
+		debugf2("\t\tAMB-Branch 1-present1 0x%x:\n",
+			pvt->b1_ambpresent1);
+	}
+
+	/* Go and determine the size of each DIMM and place in an
+	 * orderly matrix */
+	calculate_dimm_size(pvt);
+}
+
+/*
+ *	i5400_init_csrows	Initialize the 'csrows' table within
+ *				the mci control	structure with the
+ *				addressing of memory.
+ *
+ *	return:
+ *		0	success
+ *		1	no actual memory found on this MC
+ */
+static int i5400_init_csrows(struct mem_ctl_info *mci)
+{
+	struct i5400_pvt *pvt;
+	struct csrow_info *p_csrow;
+	int empty, channel_count;
+	int max_csrows;
+	int mtr;
+	int csrow_megs;
+	int channel;
+	int csrow;
+
+	pvt = mci->pvt_info;
+
+	channel_count = pvt->maxch;
+	max_csrows = pvt->maxdimmperch * 2;
+
+	empty = 1;		/* Assume NO memory */
+
+	for (csrow = 0; csrow < max_csrows; csrow++) {
+		p_csrow = &mci->csrows[csrow];
+
+		p_csrow->csrow_idx = csrow;
+
+		/* use branch 0 for the basis */
+		mtr = determine_mtr(pvt, csrow, 0);
+
+		/* if no DIMMS on this row, continue */
+		if (!MTR_DIMMS_PRESENT(mtr))
+			continue;
+
+		/* FAKE OUT VALUES, FIXME */
+		p_csrow->first_page = 0 + csrow * 20;
+		p_csrow->last_page = 9 + csrow * 20;
+		p_csrow->page_mask = 0xFFF;
+
+		p_csrow->grain = 8;
+
+		csrow_megs = 0;
+		for (channel = 0; channel < pvt->maxch; channel++)
+			csrow_megs += pvt->dimm_info[csrow][channel].megabytes;
+
+		p_csrow->nr_pages = csrow_megs << 8;
+
+		/* Assume DDR2 for now */
+		p_csrow->mtype = MEM_FB_DDR2;
+
+		/* ask what device type on this row */
+		if (MTR_DRAM_WIDTH(mtr))
+			p_csrow->dtype = DEV_X8;
+		else
+			p_csrow->dtype = DEV_X4;
+
+		p_csrow->edac_mode = EDAC_S8ECD8ED;
+
+		empty = 0;
+	}
+
+	return empty;
+}
+
+/*
+ *	i5400_enable_error_reporting
+ *			Turn on the memory reporting features of the hardware
+ */
+static void i5400_enable_error_reporting(struct mem_ctl_info *mci)
+{
+	struct i5400_pvt *pvt;
+	u32 fbd_error_mask;
+
+	pvt = mci->pvt_info;
+
+	/* Read the FBD Error Mask Register */
+	pci_read_config_dword(pvt->branchmap_werrors, EMASK_FBD,
+			&fbd_error_mask);
+
+	/* Enable with a '0' */
+	fbd_error_mask &= ~(ENABLE_EMASK_ALL);
+
+	pci_write_config_dword(pvt->branchmap_werrors, EMASK_FBD,
+			fbd_error_mask);
+}
+
+/*
+ * i5400_get_dimm_and_channel_counts(pdev, &num_csrows, &num_channels)
+ *
+ *	ask the device how many channels are present and how many CSROWS
+ *	 as well
+ */
+static void i5400_get_dimm_and_channel_counts(struct pci_dev *pdev,
+					int *num_dimms_per_channel,
+					int *num_channels)
+{
+	u8 value;
+
+	/* Need to retrieve just how many channels and dimms per channel are
+	 * supported on this memory controller
+	 */
+	pci_read_config_byte(pdev, MAXDIMMPERCH, &value);
+	*num_dimms_per_channel = (int)value * 2;
+
+	pci_read_config_byte(pdev, MAXCH, &value);
+	*num_channels = (int)value;
+}
+
+/*
+ *	i5400_probe1	Probe for ONE instance of device to see if it is
+ *			present.
+ *	return:
+ *		0 for FOUND a device
+ *		< 0 for error code
+ */
+static int i5400_probe1(struct pci_dev *pdev, int dev_idx)
+{
+	struct mem_ctl_info *mci;
+	struct i5400_pvt *pvt;
+	int num_channels;
+	int num_dimms_per_channel;
+	int num_csrows;
+
+	if (dev_idx >= ARRAY_SIZE(i5400_devs))
+		return -EINVAL;
+
+	debugf0("MC: " __FILE__ ": %s(), pdev bus %u dev=0x%x fn=0x%x\n",
+		__func__,
+		pdev->bus->number,
+		PCI_SLOT(pdev->devfn), PCI_FUNC(pdev->devfn));
+
+	/* We only are looking for func 0 of the set */
+	if (PCI_FUNC(pdev->devfn) != 0)
+		return -ENODEV;
+
+	/* Ask the devices for the number of CSROWS and CHANNELS so
+	 * that we can calculate the memory resources, etc
+	 *
+	 * The Chipset will report what it can handle which will be greater
+	 * or equal to what the motherboard manufacturer will implement.
+	 *
+	 * As we don't have a motherboard identification routine to determine
+	 * actual number of slots/dimms per channel, we thus utilize the
+	 * resource as specified by the chipset. Thus, we might have
+	 * have more DIMMs per channel than actually on the mobo, but this
+	 * allows the driver to support upto the chipset max, without
+	 * some fancy mobo determination.
+	 */
+	i5400_get_dimm_and_channel_counts(pdev, &num_dimms_per_channel,
+					&num_channels);
+	num_csrows = num_dimms_per_channel * 2;
+
+	debugf0("MC: %s(): Number of - Channels= %d  DIMMS= %d  CSROWS= %d\n",
+		__func__, num_channels, num_dimms_per_channel, num_csrows);
+
+	/* allocate a new MC control structure */
+	mci = edac_mc_alloc(sizeof(*pvt), num_csrows, num_channels, 0);
+
+	if (mci == NULL)
+		return -ENOMEM;
+
+	debugf0("MC: " __FILE__ ": %s(): mci = %p\n", __func__, mci);
+
+	mci->dev = &pdev->dev;	/* record ptr  to the generic device */
+
+	pvt = mci->pvt_info;
+	pvt->system_address = pdev;	/* Record this device in our private */
+	pvt->maxch = num_channels;
+	pvt->maxdimmperch = num_dimms_per_channel;
+
+	/* 'get' the pci devices we want to reserve for our use */
+	if (i5400_get_devices(mci, dev_idx))
+		goto fail0;
+
+	/* Time to get serious */
+	i5400_get_mc_regs(mci);	/* retrieve the hardware registers */
+
+	mci->mc_idx = 0;
+	mci->mtype_cap = MEM_FLAG_FB_DDR2;
+	mci->edac_ctl_cap = EDAC_FLAG_NONE;
+	mci->edac_cap = EDAC_FLAG_NONE;
+	mci->mod_name = "i5400_edac.c";
+	mci->mod_ver = I5400_REVISION;
+	mci->ctl_name = i5400_devs[dev_idx].ctl_name;
+	mci->dev_name = pci_name(pdev);
+	mci->ctl_page_to_phys = NULL;
+
+	/* Set the function pointer to an actual operation function */
+	mci->edac_check = i5400_check_error;
+
+	/* initialize the MC control structure 'csrows' table
+	 * with the mapping and control information */
+	if (i5400_init_csrows(mci)) {
+		debugf0("MC: Setting mci->edac_cap to EDAC_FLAG_NONE\n"
+			"    because i5400_init_csrows() returned nonzero "
+			"value\n");
+		mci->edac_cap = EDAC_FLAG_NONE;	/* no csrows found */
+	} else {
+		debugf1("MC: Enable error reporting now\n");
+		i5400_enable_error_reporting(mci);
+	}
+
+	/* add this new MC control structure to EDAC's list of MCs */
+	if (edac_mc_add_mc(mci)) {
+		debugf0("MC: " __FILE__
+			": %s(): failed edac_mc_add_mc()\n", __func__);
+		/* FIXME: perhaps some code should go here that disables error
+		 * reporting if we just enabled it
+		 */
+		goto fail1;
+	}
+
+	i5400_clear_error(mci);
+
+	/* allocating generic PCI control info */
+	i5400_pci = edac_pci_create_generic_ctl(&pdev->dev, EDAC_MOD_STR);
+	if (!i5400_pci) {
+		printk(KERN_WARNING
+			"%s(): Unable to create PCI control\n",
+			__func__);
+		printk(KERN_WARNING
+			"%s(): PCI error report via EDAC not setup\n",
+			__func__);
+	}
+
+	return 0;
+
+	/* Error exit unwinding stack */
+fail1:
+
+	i5400_put_devices(mci);
+
+fail0:
+	edac_mc_free(mci);
+	return -ENODEV;
+}
+
+/*
+ *	i5400_init_one	constructor for one instance of device
+ *
+ * 	returns:
+ *		negative on error
+ *		count (>= 0)
+ */
+static int __devinit i5400_init_one(struct pci_dev *pdev,
+				const struct pci_device_id *id)
+{
+	int rc;
+
+	debugf0("MC: " __FILE__ ": %s()\n", __func__);
+
+	/* wake up device */
+	rc = pci_enable_device(pdev);
+	if (rc == -EIO)
+		return rc;
+
+	/* now probe and enable the device */
+	return i5400_probe1(pdev, id->driver_data);
+}
+
+/*
+ *	i5400_remove_one	destructor for one instance of device
+ *
+ */
+static void __devexit i5400_remove_one(struct pci_dev *pdev)
+{
+	struct mem_ctl_info *mci;
+
+	debugf0(__FILE__ ": %s()\n", __func__);
+
+	if (i5400_pci)
+		edac_pci_release_generic_ctl(i5400_pci);
+
+	mci = edac_mc_del_mc(&pdev->dev);
+	if (!mci)
+		return;
+
+	/* retrieve references to resources, and free those resources */
+	i5400_put_devices(mci);
+
+	edac_mc_free(mci);
+}
+
+/*
+ *	pci_device_id	table for which devices we are looking for
+ *
+ *	The "E500P" device is the first device supported.
+ */
+static const struct pci_device_id i5400_pci_tbl[] __devinitdata = {
+	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_5400_ERR)},
+	{0,}			/* 0 terminated list. */
+};
+
+MODULE_DEVICE_TABLE(pci, i5400_pci_tbl);
+
+/*
+ *	i5400_driver	pci_driver structure for this module
+ *
+ */
+static struct pci_driver i5400_driver = {
+	.name = "i5400_edac",
+	.probe = i5400_init_one,
+	.remove = __devexit_p(i5400_remove_one),
+	.id_table = i5400_pci_tbl,
+};
+
+/*
+ *	i5400_init		Module entry function
+ *			Try to initialize this module for its devices
+ */
+static int __init i5400_init(void)
+{
+	int pci_rc;
+
+	debugf2("MC: " __FILE__ ": %s()\n", __func__);
+
+	/* Ensure that the OPSTATE is set correctly for POLL or NMI */
+	opstate_init();
+
+	pci_rc = pci_register_driver(&i5400_driver);
+
+	return (pci_rc < 0) ? pci_rc : 0;
+}
+
+/*
+ *	i5400_exit()	Module exit function
+ *			Unregister the driver
+ */
+static void __exit i5400_exit(void)
+{
+	debugf2("MC: " __FILE__ ": %s()\n", __func__);
+	pci_unregister_driver(&i5400_driver);
+}
+
+module_init(i5400_init);
+module_exit(i5400_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Ben Woodard <woodard@redhat.com>");
+MODULE_AUTHOR("Mauro Carvalho Chehab <mchehab@redhat.com>");
+MODULE_AUTHOR("Red Hat Inc. (http://www.redhat.com)");
+MODULE_DESCRIPTION("MC Driver for Intel I5400 memory controllers - "
+		   I5400_REVISION);
+
+module_param(edac_op_state, int, 0444);
+MODULE_PARM_DESC(edac_op_state, "EDAC Error Reporting state: 0=Poll,1=NMI");
diff --git a/drivers/edac/i82875p_edac.c b/drivers/edac/i82875p_edac.c
index ebb037b7875..b2d83b95033 100644
--- a/drivers/edac/i82875p_edac.c
+++ b/drivers/edac/i82875p_edac.c
@@ -311,9 +311,7 @@ static int i82875p_setup_overfl_dev(struct pci_dev *pdev,
 	}
 
 	/* cache is irrelevant for PCI bus reads/writes */
-	window = ioremap_nocache(pci_resource_start(dev, 0),
-				 pci_resource_len(dev, 0));
-
+	window = pci_ioremap_bar(dev, 0);
 	if (window == NULL) {
 		i82875p_printk(KERN_ERR, "%s(): Failed to ioremap bar6\n",
 			__func__);
diff --git a/drivers/edac/mpc85xx_edac.c b/drivers/edac/mpc85xx_edac.c
index 0cfcb2d075a..853ef37ec00 100644
--- a/drivers/edac/mpc85xx_edac.c
+++ b/drivers/edac/mpc85xx_edac.c
@@ -630,27 +630,22 @@ static int mpc85xx_l2_err_remove(struct of_device *op)
 }
 
 static struct of_device_id mpc85xx_l2_err_of_match[] = {
-	{
-	 .compatible = "fsl,8540-l2-cache-controller",
-	 },
-	{
-	 .compatible = "fsl,8541-l2-cache-controller",
-	 },
-	{
-	 .compatible = "fsl,8544-l2-cache-controller",
-	 },
-	{
-	 .compatible = "fsl,8548-l2-cache-controller",
-	 },
-	{
-	 .compatible = "fsl,8555-l2-cache-controller",
-	 },
-	{
-	 .compatible = "fsl,8568-l2-cache-controller",
-	 },
-	{
-	 .compatible = "fsl,mpc8572-l2-cache-controller",
-	 },
+/* deprecate the fsl,85.. forms in the future, 2.6.30? */
+	{ .compatible = "fsl,8540-l2-cache-controller", },
+	{ .compatible = "fsl,8541-l2-cache-controller", },
+	{ .compatible = "fsl,8544-l2-cache-controller", },
+	{ .compatible = "fsl,8548-l2-cache-controller", },
+	{ .compatible = "fsl,8555-l2-cache-controller", },
+	{ .compatible = "fsl,8568-l2-cache-controller", },
+	{ .compatible = "fsl,mpc8536-l2-cache-controller", },
+	{ .compatible = "fsl,mpc8540-l2-cache-controller", },
+	{ .compatible = "fsl,mpc8541-l2-cache-controller", },
+	{ .compatible = "fsl,mpc8544-l2-cache-controller", },
+	{ .compatible = "fsl,mpc8548-l2-cache-controller", },
+	{ .compatible = "fsl,mpc8555-l2-cache-controller", },
+	{ .compatible = "fsl,mpc8560-l2-cache-controller", },
+	{ .compatible = "fsl,mpc8568-l2-cache-controller", },
+	{ .compatible = "fsl,mpc8572-l2-cache-controller", },
 	{},
 };
 
@@ -967,27 +962,22 @@ static int mpc85xx_mc_err_remove(struct of_device *op)
 }
 
 static struct of_device_id mpc85xx_mc_err_of_match[] = {
-	{
-	 .compatible = "fsl,8540-memory-controller",
-	 },
-	{
-	 .compatible = "fsl,8541-memory-controller",
-	 },
-	{
-	 .compatible = "fsl,8544-memory-controller",
-	 },
-	{
-	 .compatible = "fsl,8548-memory-controller",
-	 },
-	{
-	 .compatible = "fsl,8555-memory-controller",
-	 },
-	{
-	 .compatible = "fsl,8568-memory-controller",
-	 },
-	{
-	 .compatible = "fsl,mpc8572-memory-controller",
-	 },
+/* deprecate the fsl,85.. forms in the future, 2.6.30? */
+	{ .compatible = "fsl,8540-memory-controller", },
+	{ .compatible = "fsl,8541-memory-controller", },
+	{ .compatible = "fsl,8544-memory-controller", },
+	{ .compatible = "fsl,8548-memory-controller", },
+	{ .compatible = "fsl,8555-memory-controller", },
+	{ .compatible = "fsl,8568-memory-controller", },
+	{ .compatible = "fsl,mpc8536-memory-controller", },
+	{ .compatible = "fsl,mpc8540-memory-controller", },
+	{ .compatible = "fsl,mpc8541-memory-controller", },
+	{ .compatible = "fsl,mpc8544-memory-controller", },
+	{ .compatible = "fsl,mpc8548-memory-controller", },
+	{ .compatible = "fsl,mpc8555-memory-controller", },
+	{ .compatible = "fsl,mpc8560-memory-controller", },
+	{ .compatible = "fsl,mpc8568-memory-controller", },
+	{ .compatible = "fsl,mpc8572-memory-controller", },
 	{},
 };
 
diff --git a/drivers/firmware/dmi-id.c b/drivers/firmware/dmi-id.c
index e880d6c8d89..5a76d056b9d 100644
--- a/drivers/firmware/dmi-id.c
+++ b/drivers/firmware/dmi-id.c
@@ -223,7 +223,7 @@ static int __init dmi_id_init(void)
 	}
 
 	dmi_dev->class = &dmi_class;
-	strcpy(dmi_dev->bus_id, "id");
+	dev_set_name(dmi_dev, "id");
 	dmi_dev->groups = sys_dmi_attribute_groups;
 
 	ret = device_register(dmi_dev);
diff --git a/drivers/firmware/dmi_scan.c b/drivers/firmware/dmi_scan.c
index 78b989d202a..d76adfea5df 100644
--- a/drivers/firmware/dmi_scan.c
+++ b/drivers/firmware/dmi_scan.c
@@ -468,8 +468,8 @@ const char *dmi_get_system_info(int field)
 EXPORT_SYMBOL(dmi_get_system_info);
 
 /**
- *	dmi_name_in_serial - 	Check if string is in the DMI product serial
- *				information.
+ * dmi_name_in_serial - Check if string is in the DMI product serial information
+ * @str: string to check for
  */
 int dmi_name_in_serial(const char *str)
 {
@@ -585,6 +585,8 @@ EXPORT_SYMBOL_GPL(dmi_walk);
 
 /**
  * dmi_match - compare a string to the dmi field (if exists)
+ * @f: DMI field identifier
+ * @str: string to compare the DMI field to
  *
  * Returns true if the requested field equals to the str (including NULL).
  */
diff --git a/drivers/gpio/Kconfig b/drivers/gpio/Kconfig
index 48f49d93d24..3d2565441b3 100644
--- a/drivers/gpio/Kconfig
+++ b/drivers/gpio/Kconfig
@@ -95,7 +95,7 @@ config GPIO_MAX732X
 	  number for these GPIOs.
 
 config GPIO_PCA953X
-	tristate "PCA953x, PCA955x, and MAX7310 I/O ports"
+	tristate "PCA953x, PCA955x, TCA64xx, and MAX7310 I/O ports"
 	depends on I2C
 	help
 	  Say yes here to provide access to several register-oriented
@@ -104,9 +104,10 @@ config GPIO_PCA953X
 
 	  4 bits:	pca9536, pca9537
 
-	  8 bits:	max7310, pca9534, pca9538, pca9554, pca9557
+	  8 bits:	max7310, pca9534, pca9538, pca9554, pca9557,
+	  		tca6408
 
-	  16 bits:	pca9535, pca9539, pca9555
+	  16 bits:	pca9535, pca9539, pca9555, tca6416
 
 	  This driver can also be built as a module.  If so, the module
 	  will be called pca953x.
diff --git a/drivers/gpio/gpiolib.c b/drivers/gpio/gpiolib.c
index 82020abc329..35e7aea4222 100644
--- a/drivers/gpio/gpiolib.c
+++ b/drivers/gpio/gpiolib.c
@@ -1213,7 +1213,7 @@ static int gpiolib_show(struct seq_file *s, void *unused)
 		if (dev)
 			seq_printf(s, ", %s/%s",
 				dev->bus ? dev->bus->name : "no-bus",
-				dev->bus_id);
+				dev_name(dev));
 		if (chip->label)
 			seq_printf(s, ", %s", chip->label);
 		if (chip->can_sleep)
diff --git a/drivers/gpio/pca953x.c b/drivers/gpio/pca953x.c
index 9ceeb89f132..37f35388a2a 100644
--- a/drivers/gpio/pca953x.c
+++ b/drivers/gpio/pca953x.c
@@ -33,7 +33,12 @@ static const struct i2c_device_id pca953x_id[] = {
 	{ "pca9554", 8, },
 	{ "pca9555", 16, },
 	{ "pca9557", 8, },
+
 	{ "max7310", 8, },
+	{ "pca6107", 8, },
+	{ "tca6408", 8, },
+	{ "tca6416", 16, },
+	/* NYET:  { "tca6424", 24, }, */
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, pca953x_id);
@@ -47,9 +52,6 @@ struct pca953x_chip {
 	struct gpio_chip gpio_chip;
 };
 
-/* NOTE:  we can't currently rely on fault codes to come from SMBus
- * calls, so we map all errors to EIO here and return zero otherwise.
- */
 static int pca953x_write_reg(struct pca953x_chip *chip, int reg, uint16_t val)
 {
 	int ret;
@@ -61,7 +63,7 @@ static int pca953x_write_reg(struct pca953x_chip *chip, int reg, uint16_t val)
 
 	if (ret < 0) {
 		dev_err(&chip->client->dev, "failed writing register\n");
-		return -EIO;
+		return ret;
 	}
 
 	return 0;
@@ -78,7 +80,7 @@ static int pca953x_read_reg(struct pca953x_chip *chip, int reg, uint16_t *val)
 
 	if (ret < 0) {
 		dev_err(&chip->client->dev, "failed reading register\n");
-		return -EIO;
+		return ret;
 	}
 
 	*val = (uint16_t)ret;
diff --git a/drivers/gpio/twl4030-gpio.c b/drivers/gpio/twl4030-gpio.c
index 37d3eec8730..afad1479214 100644
--- a/drivers/gpio/twl4030-gpio.c
+++ b/drivers/gpio/twl4030-gpio.c
@@ -202,37 +202,6 @@ static int twl4030_get_gpio_datain(int gpio)
 	return ret;
 }
 
-/*
- * Configure debounce timing value for a GPIO pin on TWL4030
- */
-int twl4030_set_gpio_debounce(int gpio, int enable)
-{
-	u8 d_bnk = gpio >> 3;
-	u8 d_msk = BIT(gpio & 0x7);
-	u8 reg = 0;
-	u8 base = 0;
-	int ret = 0;
-
-	if (unlikely((gpio >= TWL4030_GPIO_MAX)
-		|| !(gpio_usage_count & BIT(gpio))))
-		return -EPERM;
-
-	base = REG_GPIO_DEBEN1 + d_bnk;
-	mutex_lock(&gpio_lock);
-	ret = gpio_twl4030_read(base);
-	if (ret >= 0) {
-		if (enable)
-			reg = ret | d_msk;
-		else
-			reg = ret & ~d_msk;
-
-		ret = gpio_twl4030_write(base, reg);
-	}
-	mutex_unlock(&gpio_lock);
-	return ret;
-}
-EXPORT_SYMBOL(twl4030_set_gpio_debounce);
-
 /*----------------------------------------------------------------------*/
 
 static int twl_request(struct gpio_chip *chip, unsigned offset)
@@ -405,6 +374,23 @@ static int __devinit gpio_twl4030_pulls(u32 ups, u32 downs)
 				REG_GPIOPUPDCTR1, 5);
 }
 
+static int __devinit gpio_twl4030_debounce(u32 debounce, u8 mmc_cd)
+{
+	u8		message[4];
+
+	/* 30 msec of debouncing is always used for MMC card detect,
+	 * and is optional for everything else.
+	 */
+	message[1] = (debounce & 0xff) | (mmc_cd & 0x03);
+	debounce >>= 8;
+	message[2] = (debounce & 0xff);
+	debounce >>= 8;
+	message[3] = (debounce & 0x03);
+
+	return twl4030_i2c_write(TWL4030_MODULE_GPIO, message,
+				REG_GPIO_DEBEN1, 3);
+}
+
 static int gpio_twl4030_remove(struct platform_device *pdev);
 
 static int __devinit gpio_twl4030_probe(struct platform_device *pdev)
@@ -439,6 +425,12 @@ no_irqs:
 				pdata->pullups, pdata->pulldowns,
 				ret);
 
+	ret = gpio_twl4030_debounce(pdata->debounce, pdata->mmc_cd);
+	if (ret)
+		dev_dbg(&pdev->dev, "debounce %.03x %.01x --> %d\n",
+				pdata->debounce, pdata->mmc_cd,
+				ret);
+
 	twl_gpiochip.base = pdata->gpio_base;
 	twl_gpiochip.ngpio = TWL4030_GPIO_MAX;
 	twl_gpiochip.dev = &pdev->dev;
diff --git a/drivers/gpu/drm/drm_drv.c b/drivers/gpu/drm/drm_drv.c
index febb517ee67..5ff88d95222 100644
--- a/drivers/gpu/drm/drm_drv.c
+++ b/drivers/gpu/drm/drm_drv.c
@@ -314,14 +314,14 @@ static void drm_cleanup(struct drm_device * dev)
 		DRM_DEBUG("mtrr_del=%d\n", retval);
 	}
 
+	if (dev->driver->unload)
+		dev->driver->unload(dev);
+
 	if (drm_core_has_AGP(dev) && dev->agp) {
 		drm_free(dev->agp, sizeof(*dev->agp), DRM_MEM_AGPLISTS);
 		dev->agp = NULL;
 	}
 
-	if (dev->driver->unload)
-		dev->driver->unload(dev);
-
 	drm_ht_remove(&dev->map_hash);
 	drm_ctxbitmap_cleanup(dev);
 
diff --git a/drivers/gpu/drm/drm_fops.c b/drivers/gpu/drm/drm_fops.c
index 3733e36d135..b06a5371585 100644
--- a/drivers/gpu/drm/drm_fops.c
+++ b/drivers/gpu/drm/drm_fops.c
@@ -183,6 +183,10 @@ int drm_stub_open(struct inode *inode, struct file *filp)
 
 	old_fops = filp->f_op;
 	filp->f_op = fops_get(&dev->driver->fops);
+	if (filp->f_op == NULL) {
+		filp->f_op = old_fops;
+		goto out;
+	}
 	if (filp->f_op->open && (err = filp->f_op->open(inode, filp))) {
 		fops_put(filp->f_op);
 		filp->f_op = fops_get(old_fops);
diff --git a/drivers/gpu/drm/drm_sysfs.c b/drivers/gpu/drm/drm_sysfs.c
index 65d72d094c8..5aa6780652a 100644
--- a/drivers/gpu/drm/drm_sysfs.c
+++ b/drivers/gpu/drm/drm_sysfs.c
@@ -488,7 +488,7 @@ int drm_sysfs_device_add(struct drm_minor *minor)
         else
                 minor_str = "card%d";
 
-	snprintf(minor->kdev.bus_id, BUS_ID_SIZE, minor_str, minor->index);
+	dev_set_name(&minor->kdev, minor_str, minor->index);
 
 	err = device_register(&minor->kdev);
 	if (err) {
diff --git a/drivers/gpu/drm/i915/Makefile b/drivers/gpu/drm/i915/Makefile
index dd57a5bd457..793cba39d83 100644
--- a/drivers/gpu/drm/i915/Makefile
+++ b/drivers/gpu/drm/i915/Makefile
@@ -13,6 +13,7 @@ i915-y := i915_drv.o i915_dma.o i915_irq.o i915_mem.o \
 	  intel_crt.o \
 	  intel_lvds.o \
 	  intel_bios.o \
+	  intel_hdmi.o \
 	  intel_sdvo.o \
 	  intel_modes.o \
 	  intel_i2c.o \
diff --git a/drivers/gpu/drm/i915/i915_dma.c b/drivers/gpu/drm/i915/i915_dma.c
index 3d7082af5b7..62a4bf7b49d 100644
--- a/drivers/gpu/drm/i915/i915_dma.c
+++ b/drivers/gpu/drm/i915/i915_dma.c
@@ -827,6 +827,7 @@ static int i915_probe_agp(struct drm_device *dev, unsigned long *aperture_size,
 	struct pci_dev *bridge_dev;
 	u16 tmp = 0;
 	unsigned long overhead;
+	unsigned long stolen;
 
 	bridge_dev = pci_get_bus_and_slot(0, PCI_DEVFN(0,0));
 	if (!bridge_dev) {
@@ -866,36 +867,55 @@ static int i915_probe_agp(struct drm_device *dev, unsigned long *aperture_size,
 	else
 		overhead = (*aperture_size / 1024) + 4096;
 
-	switch (tmp & INTEL_855_GMCH_GMS_MASK) {
+	switch (tmp & INTEL_GMCH_GMS_MASK) {
+	case INTEL_855_GMCH_GMS_DISABLED:
+		DRM_ERROR("video memory is disabled\n");
+		return -1;
 	case INTEL_855_GMCH_GMS_STOLEN_1M:
-		break; /* 1M already */
+		stolen = 1 * 1024 * 1024;
+		break;
 	case INTEL_855_GMCH_GMS_STOLEN_4M:
-		*preallocated_size *= 4;
+		stolen = 4 * 1024 * 1024;
 		break;
 	case INTEL_855_GMCH_GMS_STOLEN_8M:
-		*preallocated_size *= 8;
+		stolen = 8 * 1024 * 1024;
 		break;
 	case INTEL_855_GMCH_GMS_STOLEN_16M:
-		*preallocated_size *= 16;
+		stolen = 16 * 1024 * 1024;
 		break;
 	case INTEL_855_GMCH_GMS_STOLEN_32M:
-		*preallocated_size *= 32;
+		stolen = 32 * 1024 * 1024;
 		break;
 	case INTEL_915G_GMCH_GMS_STOLEN_48M:
-		*preallocated_size *= 48;
+		stolen = 48 * 1024 * 1024;
 		break;
 	case INTEL_915G_GMCH_GMS_STOLEN_64M:
-		*preallocated_size *= 64;
+		stolen = 64 * 1024 * 1024;
+		break;
+	case INTEL_GMCH_GMS_STOLEN_128M:
+		stolen = 128 * 1024 * 1024;
+		break;
+	case INTEL_GMCH_GMS_STOLEN_256M:
+		stolen = 256 * 1024 * 1024;
+		break;
+	case INTEL_GMCH_GMS_STOLEN_96M:
+		stolen = 96 * 1024 * 1024;
+		break;
+	case INTEL_GMCH_GMS_STOLEN_160M:
+		stolen = 160 * 1024 * 1024;
+		break;
+	case INTEL_GMCH_GMS_STOLEN_224M:
+		stolen = 224 * 1024 * 1024;
+		break;
+	case INTEL_GMCH_GMS_STOLEN_352M:
+		stolen = 352 * 1024 * 1024;
 		break;
-	case INTEL_855_GMCH_GMS_DISABLED:
-		DRM_ERROR("video memory is disabled\n");
-		return -1;
 	default:
 		DRM_ERROR("unexpected GMCH_GMS value: 0x%02x\n",
-			tmp & INTEL_855_GMCH_GMS_MASK);
+			tmp & INTEL_GMCH_GMS_MASK);
 		return -1;
 	}
-	*preallocated_size -= overhead;
+	*preallocated_size = stolen - overhead;
 
 	return 0;
 }
diff --git a/drivers/gpu/drm/i915/i915_drv.h b/drivers/gpu/drm/i915/i915_drv.h
index 4756e5cd6b5..563de18063f 100644
--- a/drivers/gpu/drm/i915/i915_drv.h
+++ b/drivers/gpu/drm/i915/i915_drv.h
@@ -664,6 +664,7 @@ extern void intel_modeset_cleanup(struct drm_device *dev);
 				 writel(upper_32_bits(val), dev_priv->regs + \
 					(reg) + 4))
 #endif
+#define POSTING_READ(reg)	(void)I915_READ(reg)
 
 #define I915_VERBOSE 0
 
@@ -760,6 +761,7 @@ extern int i915_wait_ring(struct drm_device * dev, int n, const char *caller);
 			IS_I945GM(dev) || IS_I965GM(dev) || IS_GM45(dev))
 
 #define I915_NEED_GFX_HWS(dev) (IS_G33(dev) || IS_GM45(dev) || IS_G4X(dev))
+#define SUPPORTS_INTEGRATED_HDMI(dev)	(IS_G4X(dev))
 
 #define PRIMARY_RINGBUFFER_SIZE         (128*1024)
 
diff --git a/drivers/gpu/drm/i915/i915_gem.c b/drivers/gpu/drm/i915/i915_gem.c
index cc2ca5561fe..14afc23a0e2 100644
--- a/drivers/gpu/drm/i915/i915_gem.c
+++ b/drivers/gpu/drm/i915/i915_gem.c
@@ -1623,6 +1623,8 @@ i915_gem_object_bind_to_gtt(struct drm_gem_object *obj, unsigned alignment)
 	struct drm_mm_node *free_space;
 	int page_count, ret;
 
+	if (dev_priv->mm.suspended)
+		return -EBUSY;
 	if (alignment == 0)
 		alignment = PAGE_SIZE;
 	if (alignment & (PAGE_SIZE - 1)) {
@@ -2641,7 +2643,7 @@ i915_gem_object_pin(struct drm_gem_object *obj, uint32_t alignment)
 	if (obj_priv->gtt_space == NULL) {
 		ret = i915_gem_object_bind_to_gtt(obj, alignment);
 		if (ret != 0) {
-			if (ret != -ERESTARTSYS)
+			if (ret != -EBUSY && ret != -ERESTARTSYS)
 				DRM_ERROR("Failure to bind: %d", ret);
 			return ret;
 		}
@@ -3219,20 +3221,21 @@ i915_gem_entervt_ioctl(struct drm_device *dev, void *data,
 		dev_priv->mm.wedged = 0;
 	}
 
-	ret = i915_gem_init_ringbuffer(dev);
-	if (ret != 0)
-		return ret;
-
 	dev_priv->mm.gtt_mapping = io_mapping_create_wc(dev->agp->base,
 							dev->agp->agp_info.aper_size
 							* 1024 * 1024);
 
 	mutex_lock(&dev->struct_mutex);
+	dev_priv->mm.suspended = 0;
+
+	ret = i915_gem_init_ringbuffer(dev);
+	if (ret != 0)
+		return ret;
+
 	BUG_ON(!list_empty(&dev_priv->mm.active_list));
 	BUG_ON(!list_empty(&dev_priv->mm.flushing_list));
 	BUG_ON(!list_empty(&dev_priv->mm.inactive_list));
 	BUG_ON(!list_empty(&dev_priv->mm.request_list));
-	dev_priv->mm.suspended = 0;
 	mutex_unlock(&dev->struct_mutex);
 
 	drm_irq_install(dev);
diff --git a/drivers/gpu/drm/i915/i915_reg.h b/drivers/gpu/drm/i915/i915_reg.h
index 47e6bafeb74..273162579e1 100644
--- a/drivers/gpu/drm/i915/i915_reg.h
+++ b/drivers/gpu/drm/i915/i915_reg.h
@@ -35,7 +35,7 @@
 #define INTEL_GMCH_MEM_64M	0x1
 #define INTEL_GMCH_MEM_128M	0
 
-#define INTEL_855_GMCH_GMS_MASK		(0x7 << 4)
+#define INTEL_GMCH_GMS_MASK		(0xf << 4)
 #define INTEL_855_GMCH_GMS_DISABLED	(0x0 << 4)
 #define INTEL_855_GMCH_GMS_STOLEN_1M	(0x1 << 4)
 #define INTEL_855_GMCH_GMS_STOLEN_4M	(0x2 << 4)
@@ -45,6 +45,12 @@
 
 #define INTEL_915G_GMCH_GMS_STOLEN_48M	(0x6 << 4)
 #define INTEL_915G_GMCH_GMS_STOLEN_64M	(0x7 << 4)
+#define INTEL_GMCH_GMS_STOLEN_128M	(0x8 << 4)
+#define INTEL_GMCH_GMS_STOLEN_256M	(0x9 << 4)
+#define INTEL_GMCH_GMS_STOLEN_96M	(0xa << 4)
+#define INTEL_GMCH_GMS_STOLEN_160M	(0xb << 4)
+#define INTEL_GMCH_GMS_STOLEN_224M	(0xc << 4)
+#define INTEL_GMCH_GMS_STOLEN_352M	(0xd << 4)
 
 /* PCI config space */
 
@@ -549,6 +555,8 @@
 /** GM965 GM45 render standby register */
 #define MCHBAR_RENDER_STANDBY	0x111B8
 
+#define PEG_BAND_GAP_DATA	0x14d68
+
 /*
  * Overlay regs
  */
@@ -612,6 +620,9 @@
 
 /* Hotplug control (945+ only) */
 #define PORT_HOTPLUG_EN		0x61110
+#define   HDMIB_HOTPLUG_INT_EN			(1 << 29)
+#define   HDMIC_HOTPLUG_INT_EN			(1 << 28)
+#define   HDMID_HOTPLUG_INT_EN			(1 << 27)
 #define   SDVOB_HOTPLUG_INT_EN			(1 << 26)
 #define   SDVOC_HOTPLUG_INT_EN			(1 << 25)
 #define   TV_HOTPLUG_INT_EN			(1 << 18)
@@ -619,6 +630,9 @@
 #define   CRT_HOTPLUG_FORCE_DETECT		(1 << 3)
 
 #define PORT_HOTPLUG_STAT	0x61114
+#define   HDMIB_HOTPLUG_INT_STATUS		(1 << 29)
+#define   HDMIC_HOTPLUG_INT_STATUS		(1 << 28)
+#define   HDMID_HOTPLUG_INT_STATUS		(1 << 27)
 #define   CRT_HOTPLUG_INT_STATUS		(1 << 11)
 #define   TV_HOTPLUG_INT_STATUS			(1 << 10)
 #define   CRT_HOTPLUG_MONITOR_MASK		(3 << 8)
@@ -648,7 +662,16 @@
 #define   SDVO_PHASE_SELECT_DEFAULT	(6 << 19)
 #define   SDVO_CLOCK_OUTPUT_INVERT	(1 << 18)
 #define   SDVOC_GANG_MODE		(1 << 16)
+#define   SDVO_ENCODING_SDVO		(0x0 << 10)
+#define   SDVO_ENCODING_HDMI		(0x2 << 10)
+/** Requird for HDMI operation */
+#define   SDVO_NULL_PACKETS_DURING_VSYNC (1 << 9)
 #define   SDVO_BORDER_ENABLE		(1 << 7)
+#define   SDVO_AUDIO_ENABLE		(1 << 6)
+/** New with 965, default is to be set */
+#define   SDVO_VSYNC_ACTIVE_HIGH	(1 << 4)
+/** New with 965, default is to be set */
+#define   SDVO_HSYNC_ACTIVE_HIGH	(1 << 3)
 #define   SDVOB_PCIE_CONCURRENCY	(1 << 3)
 #define   SDVO_DETECTED			(1 << 2)
 /* Bits to be preserved when writing */
diff --git a/drivers/gpu/drm/i915/intel_display.c b/drivers/gpu/drm/i915/intel_display.c
index e5c1c80d1f9..8ccb9c3ab86 100644
--- a/drivers/gpu/drm/i915/intel_display.c
+++ b/drivers/gpu/drm/i915/intel_display.c
@@ -751,6 +751,7 @@ static void intel_crtc_mode_set(struct drm_crtc *crtc,
 			is_lvds = true;
 			break;
 		case INTEL_OUTPUT_SDVO:
+		case INTEL_OUTPUT_HDMI:
 			is_sdvo = true;
 			break;
 		case INTEL_OUTPUT_DVO:
@@ -986,19 +987,17 @@ static int intel_crtc_cursor_set(struct drm_crtc *crtc,
 	uint32_t base = (pipe == 0) ? CURABASE : CURBBASE;
 	uint32_t temp;
 	size_t addr;
+	int ret;
 
 	DRM_DEBUG("\n");
 
 	/* if we want to turn off the cursor ignore width and height */
 	if (!handle) {
 		DRM_DEBUG("cursor off\n");
-		/* turn of the cursor */
-		temp = 0;
-		temp |= CURSOR_MODE_DISABLE;
-
-		I915_WRITE(control, temp);
-		I915_WRITE(base, 0);
-		return 0;
+		temp = CURSOR_MODE_DISABLE;
+		addr = 0;
+		bo = NULL;
+		goto finish;
 	}
 
 	/* Currently we only support 64x64 cursors */
@@ -1025,15 +1024,30 @@ static int intel_crtc_cursor_set(struct drm_crtc *crtc,
 		addr = obj_priv->gtt_offset;
 	}
 
-	intel_crtc->cursor_addr = addr;
+	ret = i915_gem_object_pin(bo, PAGE_SIZE);
+	if (ret) {
+		DRM_ERROR("failed to pin cursor bo\n");
+		drm_gem_object_unreference(bo);
+		return ret;
+	}
+
 	temp = 0;
 	/* set the pipe for the cursor */
 	temp |= (pipe << 28);
 	temp |= CURSOR_MODE_64_ARGB_AX | MCURSOR_GAMMA_ENABLE;
 
+ finish:
 	I915_WRITE(control, temp);
 	I915_WRITE(base, addr);
 
+	if (intel_crtc->cursor_bo) {
+		i915_gem_object_unpin(intel_crtc->cursor_bo);
+		drm_gem_object_unreference(intel_crtc->cursor_bo);
+	}
+
+	intel_crtc->cursor_addr = addr;
+	intel_crtc->cursor_bo = bo;
+
 	return 0;
 }
 
@@ -1430,12 +1444,19 @@ static void intel_setup_outputs(struct drm_device *dev)
 		intel_lvds_init(dev);
 
 	if (IS_I9XX(dev)) {
-		intel_sdvo_init(dev, SDVOB);
-		intel_sdvo_init(dev, SDVOC);
+		int found;
+
+		found = intel_sdvo_init(dev, SDVOB);
+		if (!found && SUPPORTS_INTEGRATED_HDMI(dev))
+			intel_hdmi_init(dev, SDVOB);
+
+		found = intel_sdvo_init(dev, SDVOC);
+		if (!found && SUPPORTS_INTEGRATED_HDMI(dev))
+			intel_hdmi_init(dev, SDVOC);
 	} else
 		intel_dvo_init(dev);
 
-	if (IS_I9XX(dev) && !IS_I915G(dev))
+	if (IS_I9XX(dev) && IS_MOBILE(dev))
 		intel_tv_init(dev);
 
 	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
@@ -1445,6 +1466,11 @@ static void intel_setup_outputs(struct drm_device *dev)
 
 		/* valid crtcs */
 		switch(intel_output->type) {
+		case INTEL_OUTPUT_HDMI:
+			crtc_mask = ((1 << 0)|
+				     (1 << 1));
+			clone_mask = ((1 << INTEL_OUTPUT_HDMI));
+			break;
 		case INTEL_OUTPUT_DVO:
 		case INTEL_OUTPUT_SDVO:
 			crtc_mask = ((1 << 0)|
diff --git a/drivers/gpu/drm/i915/intel_drv.h b/drivers/gpu/drm/i915/intel_drv.h
index 407edd5bf58..8a4cc50c5b4 100644
--- a/drivers/gpu/drm/i915/intel_drv.h
+++ b/drivers/gpu/drm/i915/intel_drv.h
@@ -53,6 +53,7 @@
 #define INTEL_OUTPUT_SDVO 3
 #define INTEL_OUTPUT_LVDS 4
 #define INTEL_OUTPUT_TVOUT 5
+#define INTEL_OUTPUT_HDMI 6
 
 #define INTEL_DVO_CHIP_NONE 0
 #define INTEL_DVO_CHIP_LVDS 1
@@ -88,6 +89,7 @@ struct intel_crtc {
 	struct drm_crtc base;
 	int pipe;
 	int plane;
+	struct drm_gem_object *cursor_bo;
 	uint32_t cursor_addr;
 	u8 lut_r[256], lut_g[256], lut_b[256];
 	int dpms_mode;
@@ -108,7 +110,8 @@ int intel_ddc_get_modes(struct intel_output *intel_output);
 extern bool intel_ddc_probe(struct intel_output *intel_output);
 
 extern void intel_crt_init(struct drm_device *dev);
-extern void intel_sdvo_init(struct drm_device *dev, int output_device);
+extern void intel_hdmi_init(struct drm_device *dev, int sdvox_reg);
+extern bool intel_sdvo_init(struct drm_device *dev, int output_device);
 extern void intel_dvo_init(struct drm_device *dev);
 extern void intel_tv_init(struct drm_device *dev);
 extern void intel_lvds_init(struct drm_device *dev);
diff --git a/drivers/gpu/drm/i915/intel_hdmi.c b/drivers/gpu/drm/i915/intel_hdmi.c
new file mode 100644
index 00000000000..b06a4a3ff08
--- /dev/null
+++ b/drivers/gpu/drm/i915/intel_hdmi.c
@@ -0,0 +1,280 @@
+/*
+ * Copyright 2006 Dave Airlie <airlied@linux.ie>
+ * Copyright © 2006-2009 Intel Corporation
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ *
+ * Authors:
+ *	Eric Anholt <eric@anholt.net>
+ *	Jesse Barnes <jesse.barnes@intel.com>
+ */
+
+#include <linux/i2c.h>
+#include <linux/delay.h>
+#include "drmP.h"
+#include "drm.h"
+#include "drm_crtc.h"
+#include "intel_drv.h"
+#include "i915_drm.h"
+#include "i915_drv.h"
+
+struct intel_hdmi_priv {
+	u32 sdvox_reg;
+	u32 save_SDVOX;
+	int has_hdmi_sink;
+};
+
+static void intel_hdmi_mode_set(struct drm_encoder *encoder,
+				struct drm_display_mode *mode,
+				struct drm_display_mode *adjusted_mode)
+{
+	struct drm_device *dev = encoder->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_crtc *crtc = encoder->crtc;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	struct intel_output *intel_output = enc_to_intel_output(encoder);
+	struct intel_hdmi_priv *hdmi_priv = intel_output->dev_priv;
+	u32 sdvox;
+
+	sdvox = SDVO_ENCODING_HDMI |
+		SDVO_BORDER_ENABLE |
+		SDVO_VSYNC_ACTIVE_HIGH |
+		SDVO_HSYNC_ACTIVE_HIGH;
+
+	if (hdmi_priv->has_hdmi_sink)
+		sdvox |= SDVO_AUDIO_ENABLE;
+
+	if (intel_crtc->pipe == 1)
+		sdvox |= SDVO_PIPE_B_SELECT;
+
+	I915_WRITE(hdmi_priv->sdvox_reg, sdvox);
+	POSTING_READ(hdmi_priv->sdvox_reg);
+}
+
+static void intel_hdmi_dpms(struct drm_encoder *encoder, int mode)
+{
+	struct drm_device *dev = encoder->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_output *intel_output = enc_to_intel_output(encoder);
+	struct intel_hdmi_priv *hdmi_priv = intel_output->dev_priv;
+	u32 temp;
+
+	if (mode != DRM_MODE_DPMS_ON) {
+		temp = I915_READ(hdmi_priv->sdvox_reg);
+		I915_WRITE(hdmi_priv->sdvox_reg, temp & ~SDVO_ENABLE);
+	} else {
+		temp = I915_READ(hdmi_priv->sdvox_reg);
+		I915_WRITE(hdmi_priv->sdvox_reg, temp | SDVO_ENABLE);
+	}
+	POSTING_READ(hdmi_priv->sdvox_reg);
+}
+
+static void intel_hdmi_save(struct drm_connector *connector)
+{
+	struct drm_device *dev = connector->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_output *intel_output = to_intel_output(connector);
+	struct intel_hdmi_priv *hdmi_priv = intel_output->dev_priv;
+
+	hdmi_priv->save_SDVOX = I915_READ(hdmi_priv->sdvox_reg);
+}
+
+static void intel_hdmi_restore(struct drm_connector *connector)
+{
+	struct drm_device *dev = connector->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_output *intel_output = to_intel_output(connector);
+	struct intel_hdmi_priv *hdmi_priv = intel_output->dev_priv;
+
+	I915_WRITE(hdmi_priv->sdvox_reg, hdmi_priv->save_SDVOX);
+	POSTING_READ(hdmi_priv->sdvox_reg);
+}
+
+static int intel_hdmi_mode_valid(struct drm_connector *connector,
+				 struct drm_display_mode *mode)
+{
+	if (mode->clock > 165000)
+		return MODE_CLOCK_HIGH;
+	if (mode->clock < 20000)
+		return MODE_CLOCK_HIGH;
+
+	if (mode->flags & DRM_MODE_FLAG_DBLSCAN)
+		return MODE_NO_DBLESCAN;
+
+	return MODE_OK;
+}
+
+static bool intel_hdmi_mode_fixup(struct drm_encoder *encoder,
+				  struct drm_display_mode *mode,
+				  struct drm_display_mode *adjusted_mode)
+{
+	return true;
+}
+
+static enum drm_connector_status
+intel_hdmi_detect(struct drm_connector *connector)
+{
+	struct drm_device *dev = connector->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_output *intel_output = to_intel_output(connector);
+	struct intel_hdmi_priv *hdmi_priv = intel_output->dev_priv;
+	u32 temp, bit;
+
+	temp = I915_READ(PORT_HOTPLUG_EN);
+
+	I915_WRITE(PORT_HOTPLUG_EN,
+		   temp |
+		   HDMIB_HOTPLUG_INT_EN |
+		   HDMIC_HOTPLUG_INT_EN |
+		   HDMID_HOTPLUG_INT_EN);
+
+	POSTING_READ(PORT_HOTPLUG_EN);
+
+	switch (hdmi_priv->sdvox_reg) {
+	case SDVOB:
+		bit = HDMIB_HOTPLUG_INT_STATUS;
+		break;
+	case SDVOC:
+		bit = HDMIC_HOTPLUG_INT_STATUS;
+		break;
+	default:
+		return connector_status_unknown;
+	}
+
+	if ((I915_READ(PORT_HOTPLUG_STAT) & bit) != 0)
+		return connector_status_connected;
+	else
+		return connector_status_disconnected;
+}
+
+static int intel_hdmi_get_modes(struct drm_connector *connector)
+{
+	struct intel_output *intel_output = to_intel_output(connector);
+
+	/* We should parse the EDID data and find out if it's an HDMI sink so
+	 * we can send audio to it.
+	 */
+
+	return intel_ddc_get_modes(intel_output);
+}
+
+static void intel_hdmi_destroy(struct drm_connector *connector)
+{
+	struct intel_output *intel_output = to_intel_output(connector);
+
+	if (intel_output->i2c_bus)
+		intel_i2c_destroy(intel_output->i2c_bus);
+	drm_sysfs_connector_remove(connector);
+	drm_connector_cleanup(connector);
+	kfree(intel_output);
+}
+
+static const struct drm_encoder_helper_funcs intel_hdmi_helper_funcs = {
+	.dpms = intel_hdmi_dpms,
+	.mode_fixup = intel_hdmi_mode_fixup,
+	.prepare = intel_encoder_prepare,
+	.mode_set = intel_hdmi_mode_set,
+	.commit = intel_encoder_commit,
+};
+
+static const struct drm_connector_funcs intel_hdmi_connector_funcs = {
+	.save = intel_hdmi_save,
+	.restore = intel_hdmi_restore,
+	.detect = intel_hdmi_detect,
+	.fill_modes = drm_helper_probe_single_connector_modes,
+	.destroy = intel_hdmi_destroy,
+};
+
+static const struct drm_connector_helper_funcs intel_hdmi_connector_helper_funcs = {
+	.get_modes = intel_hdmi_get_modes,
+	.mode_valid = intel_hdmi_mode_valid,
+	.best_encoder = intel_best_encoder,
+};
+
+static void intel_hdmi_enc_destroy(struct drm_encoder *encoder)
+{
+	drm_encoder_cleanup(encoder);
+}
+
+static const struct drm_encoder_funcs intel_hdmi_enc_funcs = {
+	.destroy = intel_hdmi_enc_destroy,
+};
+
+
+void intel_hdmi_init(struct drm_device *dev, int sdvox_reg)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_connector *connector;
+	struct intel_output *intel_output;
+	struct intel_hdmi_priv *hdmi_priv;
+
+	intel_output = kcalloc(sizeof(struct intel_output) +
+			       sizeof(struct intel_hdmi_priv), 1, GFP_KERNEL);
+	if (!intel_output)
+		return;
+	hdmi_priv = (struct intel_hdmi_priv *)(intel_output + 1);
+
+	connector = &intel_output->base;
+	drm_connector_init(dev, connector, &intel_hdmi_connector_funcs,
+			   DRM_MODE_CONNECTOR_DVID);
+	drm_connector_helper_add(connector, &intel_hdmi_connector_helper_funcs);
+
+	intel_output->type = INTEL_OUTPUT_HDMI;
+
+	connector->interlace_allowed = 0;
+	connector->doublescan_allowed = 0;
+
+	/* Set up the DDC bus. */
+	if (sdvox_reg == SDVOB)
+		intel_output->ddc_bus = intel_i2c_create(dev, GPIOE, "HDMIB");
+	else
+		intel_output->ddc_bus = intel_i2c_create(dev, GPIOD, "HDMIC");
+
+	if (!intel_output->ddc_bus)
+		goto err_connector;
+
+	hdmi_priv->sdvox_reg = sdvox_reg;
+	intel_output->dev_priv = hdmi_priv;
+
+	drm_encoder_init(dev, &intel_output->enc, &intel_hdmi_enc_funcs,
+			 DRM_MODE_ENCODER_TMDS);
+	drm_encoder_helper_add(&intel_output->enc, &intel_hdmi_helper_funcs);
+
+	drm_mode_connector_attach_encoder(&intel_output->base,
+					  &intel_output->enc);
+	drm_sysfs_connector_add(connector);
+
+	/* For G4X desktop chip, PEG_BAND_GAP_DATA 3:0 must first be written
+	 * 0xd.  Failure to do so will result in spurious interrupts being
+	 * generated on the port when a cable is not attached.
+	 */
+	if (IS_G4X(dev) && !IS_GM45(dev)) {
+		u32 temp = I915_READ(PEG_BAND_GAP_DATA);
+		I915_WRITE(PEG_BAND_GAP_DATA, (temp & ~0xf) | 0xd);
+	}
+
+	return;
+
+err_connector:
+	drm_connector_cleanup(connector);
+	kfree(intel_output);
+
+	return;
+}
diff --git a/drivers/gpu/drm/i915/intel_sdvo.c b/drivers/gpu/drm/i915/intel_sdvo.c
index fbbaa4f414a..40721546910 100644
--- a/drivers/gpu/drm/i915/intel_sdvo.c
+++ b/drivers/gpu/drm/i915/intel_sdvo.c
@@ -978,7 +978,7 @@ static const struct drm_encoder_funcs intel_sdvo_enc_funcs = {
 };
 
 
-void intel_sdvo_init(struct drm_device *dev, int output_device)
+bool intel_sdvo_init(struct drm_device *dev, int output_device)
 {
 	struct drm_connector *connector;
 	struct intel_output *intel_output;
@@ -991,7 +991,7 @@ void intel_sdvo_init(struct drm_device *dev, int output_device)
 
 	intel_output = kcalloc(sizeof(struct intel_output)+sizeof(struct intel_sdvo_priv), 1, GFP_KERNEL);
 	if (!intel_output) {
-		return;
+		return false;
 	}
 
 	connector = &intel_output->base;
@@ -1116,7 +1116,7 @@ void intel_sdvo_init(struct drm_device *dev, int output_device)
 
 	intel_output->ddc_bus = i2cbus;
 
-	return;
+	return true;
 
 err_i2c:
 	intel_i2c_destroy(intel_output->i2c_bus);
@@ -1124,5 +1124,5 @@ err_connector:
 	drm_connector_cleanup(connector);
 	kfree(intel_output);
 
-	return;
+	return false;
 }
diff --git a/drivers/hwmon/adt7462.c b/drivers/hwmon/adt7462.c
index 66107b4dc12..1852f27bac5 100644
--- a/drivers/hwmon/adt7462.c
+++ b/drivers/hwmon/adt7462.c
@@ -204,8 +204,6 @@ I2C_CLIENT_INSMOD_1(adt7462);
 #define MASK_AND_SHIFT(value, prefix)	\
 	(((value) & prefix##_MASK) >> prefix##_SHIFT)
 
-#define ROUND_DIV(x, divisor)  (((x) + ((divisor) / 2)) / (divisor))
-
 struct adt7462_data {
 	struct device		*hwmon_dev;
 	struct attribute_group	attrs;
@@ -840,7 +838,7 @@ static ssize_t set_temp_min(struct device *dev,
 	if (strict_strtol(buf, 10, &temp) || !temp_enabled(data, attr->index))
 		return -EINVAL;
 
-	temp = ROUND_DIV(temp, 1000) + 64;
+	temp = DIV_ROUND_CLOSEST(temp, 1000) + 64;
 	temp = SENSORS_LIMIT(temp, 0, 255);
 
 	mutex_lock(&data->lock);
@@ -878,7 +876,7 @@ static ssize_t set_temp_max(struct device *dev,
 	if (strict_strtol(buf, 10, &temp) || !temp_enabled(data, attr->index))
 		return -EINVAL;
 
-	temp = ROUND_DIV(temp, 1000) + 64;
+	temp = DIV_ROUND_CLOSEST(temp, 1000) + 64;
 	temp = SENSORS_LIMIT(temp, 0, 255);
 
 	mutex_lock(&data->lock);
@@ -943,7 +941,7 @@ static ssize_t set_volt_max(struct device *dev,
 		return -EINVAL;
 
 	temp *= 1000; /* convert mV to uV */
-	temp = ROUND_DIV(temp, x);
+	temp = DIV_ROUND_CLOSEST(temp, x);
 	temp = SENSORS_LIMIT(temp, 0, 255);
 
 	mutex_lock(&data->lock);
@@ -985,7 +983,7 @@ static ssize_t set_volt_min(struct device *dev,
 		return -EINVAL;
 
 	temp *= 1000; /* convert mV to uV */
-	temp = ROUND_DIV(temp, x);
+	temp = DIV_ROUND_CLOSEST(temp, x);
 	temp = SENSORS_LIMIT(temp, 0, 255);
 
 	mutex_lock(&data->lock);
@@ -1250,7 +1248,7 @@ static ssize_t set_pwm_hyst(struct device *dev,
 	if (strict_strtol(buf, 10, &temp))
 		return -EINVAL;
 
-	temp = ROUND_DIV(temp, 1000);
+	temp = DIV_ROUND_CLOSEST(temp, 1000);
 	temp = SENSORS_LIMIT(temp, 0, 15);
 
 	/* package things up */
@@ -1337,7 +1335,7 @@ static ssize_t set_pwm_tmin(struct device *dev,
 	if (strict_strtol(buf, 10, &temp))
 		return -EINVAL;
 
-	temp = ROUND_DIV(temp, 1000) + 64;
+	temp = DIV_ROUND_CLOSEST(temp, 1000) + 64;
 	temp = SENSORS_LIMIT(temp, 0, 255);
 
 	mutex_lock(&data->lock);
diff --git a/drivers/hwmon/adt7470.c b/drivers/hwmon/adt7470.c
index 1311a595147..633e1a1e9d7 100644
--- a/drivers/hwmon/adt7470.c
+++ b/drivers/hwmon/adt7470.c
@@ -28,6 +28,7 @@
 #include <linux/mutex.h>
 #include <linux/delay.h>
 #include <linux/log2.h>
+#include <linux/kthread.h>
 
 /* Addresses to scan */
 static const unsigned short normal_i2c[] = { 0x2C, 0x2E, 0x2F, I2C_CLIENT_END };
@@ -74,6 +75,7 @@ I2C_CLIENT_INSMOD_1(adt7470);
 #define ADT7470_REG_PWM12_CFG			0x68
 #define		ADT7470_PWM2_AUTO_MASK		0x40
 #define		ADT7470_PWM1_AUTO_MASK		0x80
+#define		ADT7470_PWM_AUTO_MASK		0xC0
 #define ADT7470_REG_PWM34_CFG			0x69
 #define		ADT7470_PWM3_AUTO_MASK		0x40
 #define		ADT7470_PWM4_AUTO_MASK		0x80
@@ -128,8 +130,11 @@ I2C_CLIENT_INSMOD_1(adt7470);
 /* How often do we reread sensor limit values? (In jiffies) */
 #define LIMIT_REFRESH_INTERVAL	(60 * HZ)
 
-/* sleep 1s while gathering temperature data */
-#define TEMP_COLLECTION_TIME	1000
+/* Wait at least 200ms per sensor for 10 sensors */
+#define TEMP_COLLECTION_TIME	2000
+
+/* auto update thing won't fire more than every 2s */
+#define AUTO_UPDATE_INTERVAL	2000
 
 /* datasheet says to divide this number by the fan reading to get fan rpm */
 #define FAN_PERIOD_TO_RPM(x)	((90000 * 60) / (x))
@@ -137,8 +142,6 @@ I2C_CLIENT_INSMOD_1(adt7470);
 #define FAN_PERIOD_INVALID	65535
 #define FAN_DATA_VALID(x)	((x) && (x) != FAN_PERIOD_INVALID)
 
-#define ROUND_DIV(x, divisor)	(((x) + ((divisor) / 2)) / (divisor))
-
 struct adt7470_data {
 	struct device		*hwmon_dev;
 	struct attribute_group	attrs;
@@ -148,6 +151,9 @@ struct adt7470_data {
 	unsigned long		sensors_last_updated;	/* In jiffies */
 	unsigned long		limits_last_updated;	/* In jiffies */
 
+	int			num_temp_sensors;	/* -1 = probe */
+	int			temperatures_probed;
+
 	s8			temp[ADT7470_TEMP_COUNT];
 	s8			temp_min[ADT7470_TEMP_COUNT];
 	s8			temp_max[ADT7470_TEMP_COUNT];
@@ -163,6 +169,10 @@ struct adt7470_data {
 	u8			pwm_min[ADT7470_PWM_COUNT];
 	s8			pwm_tmin[ADT7470_PWM_COUNT];
 	u8			pwm_auto_temp[ADT7470_PWM_COUNT];
+
+	struct task_struct	*auto_update;
+	struct completion	auto_update_stop;
+	unsigned int		auto_update_interval;
 };
 
 static int adt7470_probe(struct i2c_client *client,
@@ -220,40 +230,126 @@ static void adt7470_init_client(struct i2c_client *client)
 	}
 }
 
-static struct adt7470_data *adt7470_update_device(struct device *dev)
+/* Probe for temperature sensors.  Assumes lock is held */
+static int adt7470_read_temperatures(struct i2c_client *client,
+				     struct adt7470_data *data)
 {
-	struct i2c_client *client = to_i2c_client(dev);
-	struct adt7470_data *data = i2c_get_clientdata(client);
-	unsigned long local_jiffies = jiffies;
-	u8 cfg;
+	unsigned long res;
 	int i;
+	u8 cfg, pwm[4], pwm_cfg[2];
 
-	mutex_lock(&data->lock);
-	if (time_before(local_jiffies, data->sensors_last_updated +
-		SENSOR_REFRESH_INTERVAL)
-		&& data->sensors_valid)
-		goto no_sensor_update;
+	/* save pwm[1-4] config register */
+	pwm_cfg[0] = i2c_smbus_read_byte_data(client, ADT7470_REG_PWM_CFG(0));
+	pwm_cfg[1] = i2c_smbus_read_byte_data(client, ADT7470_REG_PWM_CFG(2));
+
+	/* set manual pwm to whatever it is set to now */
+	for (i = 0; i < ADT7470_FAN_COUNT; i++)
+		pwm[i] = i2c_smbus_read_byte_data(client, ADT7470_REG_PWM(i));
+
+	/* put pwm in manual mode */
+	i2c_smbus_write_byte_data(client, ADT7470_REG_PWM_CFG(0),
+		pwm_cfg[0] & ~(ADT7470_PWM_AUTO_MASK));
+	i2c_smbus_write_byte_data(client, ADT7470_REG_PWM_CFG(2),
+		pwm_cfg[1] & ~(ADT7470_PWM_AUTO_MASK));
+
+	/* write pwm control to whatever it was */
+	for (i = 0; i < ADT7470_FAN_COUNT; i++)
+		i2c_smbus_write_byte_data(client, ADT7470_REG_PWM(i), pwm[i]);
 
 	/* start reading temperature sensors */
 	cfg = i2c_smbus_read_byte_data(client, ADT7470_REG_CFG);
 	cfg |= 0x80;
 	i2c_smbus_write_byte_data(client, ADT7470_REG_CFG, cfg);
 
-	/*
-	 * Delay is 200ms * number of tmp05 sensors.  Too bad
-	 * there's no way to figure out how many are connected.
-	 * For now, assume 1s will work.
-	 */
-	msleep(TEMP_COLLECTION_TIME);
+	/* Delay is 200ms * number of temp sensors. */
+	res = msleep_interruptible((data->num_temp_sensors >= 0 ?
+				    data->num_temp_sensors * 200 :
+				    TEMP_COLLECTION_TIME));
 
 	/* done reading temperature sensors */
 	cfg = i2c_smbus_read_byte_data(client, ADT7470_REG_CFG);
 	cfg &= ~0x80;
 	i2c_smbus_write_byte_data(client, ADT7470_REG_CFG, cfg);
 
-	for (i = 0; i < ADT7470_TEMP_COUNT; i++)
+	/* restore pwm[1-4] config registers */
+	i2c_smbus_write_byte_data(client, ADT7470_REG_PWM_CFG(0), pwm_cfg[0]);
+	i2c_smbus_write_byte_data(client, ADT7470_REG_PWM_CFG(2), pwm_cfg[1]);
+
+	if (res) {
+		printk(KERN_ERR "ha ha, interrupted");
+		return -EAGAIN;
+	}
+
+	/* Only count fans if we have to */
+	if (data->num_temp_sensors >= 0)
+		return 0;
+
+	for (i = 0; i < ADT7470_TEMP_COUNT; i++) {
 		data->temp[i] = i2c_smbus_read_byte_data(client,
 						ADT7470_TEMP_REG(i));
+		if (data->temp[i])
+			data->num_temp_sensors = i + 1;
+	}
+	data->temperatures_probed = 1;
+	return 0;
+}
+
+static int adt7470_update_thread(void *p)
+{
+	struct i2c_client *client = p;
+	struct adt7470_data *data = i2c_get_clientdata(client);
+
+	while (!kthread_should_stop()) {
+		mutex_lock(&data->lock);
+		adt7470_read_temperatures(client, data);
+		mutex_unlock(&data->lock);
+		if (kthread_should_stop())
+			break;
+		msleep_interruptible(data->auto_update_interval);
+	}
+
+	complete_all(&data->auto_update_stop);
+	return 0;
+}
+
+static struct adt7470_data *adt7470_update_device(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct adt7470_data *data = i2c_get_clientdata(client);
+	unsigned long local_jiffies = jiffies;
+	u8 cfg;
+	int i;
+	int need_sensors = 1;
+	int need_limits = 1;
+
+	/*
+	 * Figure out if we need to update the shadow registers.
+	 * Lockless means that we may occasionally report out of
+	 * date data.
+	 */
+	if (time_before(local_jiffies, data->sensors_last_updated +
+			SENSOR_REFRESH_INTERVAL) &&
+	    data->sensors_valid)
+		need_sensors = 0;
+
+	if (time_before(local_jiffies, data->limits_last_updated +
+			LIMIT_REFRESH_INTERVAL) &&
+	    data->limits_valid)
+		need_limits = 0;
+
+	if (!need_sensors && !need_limits)
+		return data;
+
+	mutex_lock(&data->lock);
+	if (!need_sensors)
+		goto no_sensor_update;
+
+	if (!data->temperatures_probed)
+		adt7470_read_temperatures(client, data);
+	else
+		for (i = 0; i < ADT7470_TEMP_COUNT; i++)
+			data->temp[i] = i2c_smbus_read_byte_data(client,
+						ADT7470_TEMP_REG(i));
 
 	for (i = 0; i < ADT7470_FAN_COUNT; i++)
 		data->fan[i] = adt7470_read_word_data(client,
@@ -302,9 +398,7 @@ static struct adt7470_data *adt7470_update_device(struct device *dev)
 	data->sensors_valid = 1;
 
 no_sensor_update:
-	if (time_before(local_jiffies, data->limits_last_updated +
-		LIMIT_REFRESH_INTERVAL)
-		&& data->limits_valid)
+	if (!need_limits)
 		goto out;
 
 	for (i = 0; i < ADT7470_TEMP_COUNT; i++) {
@@ -338,6 +432,66 @@ out:
 	return data;
 }
 
+static ssize_t show_auto_update_interval(struct device *dev,
+					 struct device_attribute *devattr,
+					 char *buf)
+{
+	struct adt7470_data *data = adt7470_update_device(dev);
+	return sprintf(buf, "%d\n", data->auto_update_interval);
+}
+
+static ssize_t set_auto_update_interval(struct device *dev,
+					struct device_attribute *devattr,
+					const char *buf,
+					size_t count)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct adt7470_data *data = i2c_get_clientdata(client);
+	long temp;
+
+	if (strict_strtol(buf, 10, &temp))
+		return -EINVAL;
+
+	temp = SENSORS_LIMIT(temp, 0, 60000);
+
+	mutex_lock(&data->lock);
+	data->auto_update_interval = temp;
+	mutex_unlock(&data->lock);
+
+	return count;
+}
+
+static ssize_t show_num_temp_sensors(struct device *dev,
+				     struct device_attribute *devattr,
+				     char *buf)
+{
+	struct adt7470_data *data = adt7470_update_device(dev);
+	return sprintf(buf, "%d\n", data->num_temp_sensors);
+}
+
+static ssize_t set_num_temp_sensors(struct device *dev,
+				    struct device_attribute *devattr,
+				    const char *buf,
+				    size_t count)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct adt7470_data *data = i2c_get_clientdata(client);
+	long temp;
+
+	if (strict_strtol(buf, 10, &temp))
+		return -EINVAL;
+
+	temp = SENSORS_LIMIT(temp, -1, 10);
+
+	mutex_lock(&data->lock);
+	data->num_temp_sensors = temp;
+	if (temp < 0)
+		data->temperatures_probed = 0;
+	mutex_unlock(&data->lock);
+
+	return count;
+}
+
 static ssize_t show_temp_min(struct device *dev,
 			     struct device_attribute *devattr,
 			     char *buf)
@@ -360,7 +514,7 @@ static ssize_t set_temp_min(struct device *dev,
 	if (strict_strtol(buf, 10, &temp))
 		return -EINVAL;
 
-	temp = ROUND_DIV(temp, 1000);
+	temp = DIV_ROUND_CLOSEST(temp, 1000);
 	temp = SENSORS_LIMIT(temp, 0, 255);
 
 	mutex_lock(&data->lock);
@@ -394,7 +548,7 @@ static ssize_t set_temp_max(struct device *dev,
 	if (strict_strtol(buf, 10, &temp))
 		return -EINVAL;
 
-	temp = ROUND_DIV(temp, 1000);
+	temp = DIV_ROUND_CLOSEST(temp, 1000);
 	temp = SENSORS_LIMIT(temp, 0, 255);
 
 	mutex_lock(&data->lock);
@@ -671,7 +825,7 @@ static ssize_t set_pwm_tmin(struct device *dev,
 	if (strict_strtol(buf, 10, &temp))
 		return -EINVAL;
 
-	temp = ROUND_DIV(temp, 1000);
+	temp = DIV_ROUND_CLOSEST(temp, 1000);
 	temp = SENSORS_LIMIT(temp, 0, 255);
 
 	mutex_lock(&data->lock);
@@ -804,6 +958,10 @@ static ssize_t show_alarm(struct device *dev,
 }
 
 static DEVICE_ATTR(alarm_mask, S_IRUGO, show_alarm_mask, NULL);
+static DEVICE_ATTR(num_temp_sensors, S_IWUSR | S_IRUGO, show_num_temp_sensors,
+		   set_num_temp_sensors);
+static DEVICE_ATTR(auto_update_interval, S_IWUSR | S_IRUGO,
+		   show_auto_update_interval, set_auto_update_interval);
 
 static SENSOR_DEVICE_ATTR(temp1_max, S_IWUSR | S_IRUGO, show_temp_max,
 		    set_temp_max, 0);
@@ -976,6 +1134,8 @@ static SENSOR_DEVICE_ATTR(pwm4_auto_channels_temp, S_IWUSR | S_IRUGO,
 static struct attribute *adt7470_attr[] =
 {
 	&dev_attr_alarm_mask.attr,
+	&dev_attr_num_temp_sensors.attr,
+	&dev_attr_auto_update_interval.attr,
 	&sensor_dev_attr_temp1_max.dev_attr.attr,
 	&sensor_dev_attr_temp2_max.dev_attr.attr,
 	&sensor_dev_attr_temp3_max.dev_attr.attr,
@@ -1108,6 +1268,9 @@ static int adt7470_probe(struct i2c_client *client,
 		goto exit;
 	}
 
+	data->num_temp_sensors = -1;
+	data->auto_update_interval = AUTO_UPDATE_INTERVAL;
+
 	i2c_set_clientdata(client, data);
 	mutex_init(&data->lock);
 
@@ -1127,8 +1290,16 @@ static int adt7470_probe(struct i2c_client *client,
 		goto exit_remove;
 	}
 
+	init_completion(&data->auto_update_stop);
+	data->auto_update = kthread_run(adt7470_update_thread, client,
+					dev_name(data->hwmon_dev));
+	if (IS_ERR(data->auto_update))
+		goto exit_unregister;
+
 	return 0;
 
+exit_unregister:
+	hwmon_device_unregister(data->hwmon_dev);
 exit_remove:
 	sysfs_remove_group(&client->dev.kobj, &data->attrs);
 exit_free:
@@ -1141,6 +1312,8 @@ static int adt7470_remove(struct i2c_client *client)
 {
 	struct adt7470_data *data = i2c_get_clientdata(client);
 
+	kthread_stop(data->auto_update);
+	wait_for_completion(&data->auto_update_stop);
 	hwmon_device_unregister(data->hwmon_dev);
 	sysfs_remove_group(&client->dev.kobj, &data->attrs);
 	kfree(data);
diff --git a/drivers/hwmon/adt7473.c b/drivers/hwmon/adt7473.c
index 18aa30866a6..0a6ce2367b4 100644
--- a/drivers/hwmon/adt7473.c
+++ b/drivers/hwmon/adt7473.c
@@ -129,8 +129,6 @@ I2C_CLIENT_INSMOD_1(adt7473);
 #define FAN_PERIOD_INVALID	65535
 #define FAN_DATA_VALID(x)	((x) && (x) != FAN_PERIOD_INVALID)
 
-#define ROUND_DIV(x, divisor)	(((x) + ((divisor) / 2)) / (divisor))
-
 struct adt7473_data {
 	struct device		*hwmon_dev;
 	struct attribute_group	attrs;
@@ -459,7 +457,7 @@ static ssize_t set_temp_min(struct device *dev,
 	if (strict_strtol(buf, 10, &temp))
 		return -EINVAL;
 
-	temp = ROUND_DIV(temp, 1000);
+	temp = DIV_ROUND_CLOSEST(temp, 1000);
 	temp = encode_temp(data->temp_twos_complement, temp);
 
 	mutex_lock(&data->lock);
@@ -495,7 +493,7 @@ static ssize_t set_temp_max(struct device *dev,
 	if (strict_strtol(buf, 10, &temp))
 		return -EINVAL;
 
-	temp = ROUND_DIV(temp, 1000);
+	temp = DIV_ROUND_CLOSEST(temp, 1000);
 	temp = encode_temp(data->temp_twos_complement, temp);
 
 	mutex_lock(&data->lock);
@@ -720,7 +718,7 @@ static ssize_t set_temp_tmax(struct device *dev,
 	if (strict_strtol(buf, 10, &temp))
 		return -EINVAL;
 
-	temp = ROUND_DIV(temp, 1000);
+	temp = DIV_ROUND_CLOSEST(temp, 1000);
 	temp = encode_temp(data->temp_twos_complement, temp);
 
 	mutex_lock(&data->lock);
@@ -756,7 +754,7 @@ static ssize_t set_temp_tmin(struct device *dev,
 	if (strict_strtol(buf, 10, &temp))
 		return -EINVAL;
 
-	temp = ROUND_DIV(temp, 1000);
+	temp = DIV_ROUND_CLOSEST(temp, 1000);
 	temp = encode_temp(data->temp_twos_complement, temp);
 
 	mutex_lock(&data->lock);
diff --git a/drivers/hwmon/applesmc.c b/drivers/hwmon/applesmc.c
index 086c2a5cef0..dca47a591ba 100644
--- a/drivers/hwmon/applesmc.c
+++ b/drivers/hwmon/applesmc.c
@@ -131,6 +131,10 @@ static const char* temperature_sensors_sets[][36] = {
 /* Set 14: iMac 6,1 */
 	{ "TA0P", "TC0D", "TC0H", "TC0P", "TG0D", "TG0H", "TG0P", "TH0P",
 	  "TO0P", "Tp0P", NULL },
+/* Set 15: MacBook Air 2,1 */
+	{ "TB0T", "TB1S", "TB1T", "TB2S", "TB2T", "TC0D", "TN0D", "TTF0",
+	  "TV0P", "TVFP", "TW0P", "Th0P", "Tp0P", "Tp1P", "TpFP", "Ts0P",
+	  "Ts0S", NULL },
 };
 
 /* List of keys used to read/write fan speeds */
@@ -1301,11 +1305,17 @@ static __initdata struct dmi_match_data applesmc_dmi_data[] = {
 	{ .accelerometer = 0, .light = 0, .temperature_set = 13 },
 /* iMac 6: light sensor only, temperature set 14 */
 	{ .accelerometer = 0, .light = 0, .temperature_set = 14 },
+/* MacBook Air 2,1: accelerometer, backlight and temperature set 15 */
+	{ .accelerometer = 1, .light = 1, .temperature_set = 15 },
 };
 
 /* Note that DMI_MATCH(...,"MacBook") will match "MacBookPro1,1".
  * So we need to put "Apple MacBook Pro" before "Apple MacBook". */
 static __initdata struct dmi_system_id applesmc_whitelist[] = {
+	{ applesmc_dmi_match, "Apple MacBook Air 2", {
+	  DMI_MATCH(DMI_BOARD_VENDOR, "Apple"),
+	  DMI_MATCH(DMI_PRODUCT_NAME, "MacBookAir2") },
+		&applesmc_dmi_data[15]},
 	{ applesmc_dmi_match, "Apple MacBook Air", {
 	  DMI_MATCH(DMI_BOARD_VENDOR, "Apple"),
 	  DMI_MATCH(DMI_PRODUCT_NAME, "MacBookAir") },
diff --git a/drivers/hwmon/hwmon.c b/drivers/hwmon/hwmon.c
index 076a59cdabe..e15c3e7b07e 100644
--- a/drivers/hwmon/hwmon.c
+++ b/drivers/hwmon/hwmon.c
@@ -76,7 +76,7 @@ void hwmon_device_unregister(struct device *dev)
 {
 	int id;
 
-	if (likely(sscanf(dev->bus_id, HWMON_ID_FORMAT, &id) == 1)) {
+	if (likely(sscanf(dev_name(dev), HWMON_ID_FORMAT, &id) == 1)) {
 		device_unregister(dev);
 		spin_lock(&idr_lock);
 		idr_remove(&hwmon_idr, id);
diff --git a/drivers/hwmon/ibmpex.c b/drivers/hwmon/ibmpex.c
index 537d9fb2ff8..a36363312f2 100644
--- a/drivers/hwmon/ibmpex.c
+++ b/drivers/hwmon/ibmpex.c
@@ -40,7 +40,7 @@
 
 static inline u16 extract_value(const char *data, int offset)
 {
-	return be16_to_cpup((u16 *)&data[offset]);
+	return be16_to_cpup((__be16 *)&data[offset]);
 }
 
 #define TEMP_SENSOR		1
diff --git a/drivers/hwmon/lm75.c b/drivers/hwmon/lm75.c
index 8f9595f2fb5..55bd87c15c9 100644
--- a/drivers/hwmon/lm75.c
+++ b/drivers/hwmon/lm75.c
@@ -190,7 +190,7 @@ lm75_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	}
 
 	dev_info(&client->dev, "%s: sensor '%s'\n",
-		data->hwmon_dev->bus_id, client->name);
+		 dev_name(data->hwmon_dev), client->name);
 
 	return 0;
 
diff --git a/drivers/ide/Kconfig b/drivers/ide/Kconfig
index 4ee85fcf9aa..3f9503867e6 100644
--- a/drivers/ide/Kconfig
+++ b/drivers/ide/Kconfig
@@ -511,6 +511,13 @@ config BLK_DEV_PIIX
 	  This allows the kernel to change PIO, DMA and UDMA speeds and to
 	  configure the chip to optimum performance.
 
+config BLK_DEV_IT8172
+	tristate "IT8172 IDE support"
+	select BLK_DEV_IDEDMA_PCI
+	help
+	  This driver adds support for the IDE controller on the
+	  IT8172 System Controller.
+
 config BLK_DEV_IT8213
 	tristate "IT8213 IDE support"
 	select BLK_DEV_IDEDMA_PCI
diff --git a/drivers/ide/Makefile b/drivers/ide/Makefile
index 410728992e6..c2b9c93f009 100644
--- a/drivers/ide/Makefile
+++ b/drivers/ide/Makefile
@@ -47,6 +47,7 @@ obj-$(CONFIG_BLK_DEV_SC1200)		+= sc1200.o
 obj-$(CONFIG_BLK_DEV_CY82C693)		+= cy82c693.o
 obj-$(CONFIG_BLK_DEV_DELKIN)		+= delkin_cb.o
 obj-$(CONFIG_BLK_DEV_HPT366)		+= hpt366.o
+obj-$(CONFIG_BLK_DEV_IT8172)		+= it8172.o
 obj-$(CONFIG_BLK_DEV_IT8213)		+= it8213.o
 obj-$(CONFIG_BLK_DEV_IT821X)		+= it821x.o
 obj-$(CONFIG_BLK_DEV_JMICRON)		+= jmicron.o
diff --git a/drivers/ide/aec62xx.c b/drivers/ide/aec62xx.c
index 4142c698e0d..4485b9c6f0e 100644
--- a/drivers/ide/aec62xx.c
+++ b/drivers/ide/aec62xx.c
@@ -83,7 +83,7 @@ static u8 pci_bus_clock_list_ultra (u8 speed, struct chipset_bus_clock_list_entr
 
 static void aec6210_set_mode(ide_drive_t *drive, const u8 speed)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	struct ide_host *host	= pci_get_drvdata(dev);
 	struct chipset_bus_clock_list_entry *bus_clock = host->host_priv;
@@ -111,7 +111,7 @@ static void aec6210_set_mode(ide_drive_t *drive, const u8 speed)
 
 static void aec6260_set_mode(ide_drive_t *drive, const u8 speed)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	struct ide_host *host	= pci_get_drvdata(dev);
 	struct chipset_bus_clock_list_entry *bus_clock = host->host_priv;
diff --git a/drivers/ide/alim15x3.c b/drivers/ide/alim15x3.c
index 45d2356bb72..66f43083408 100644
--- a/drivers/ide/alim15x3.c
+++ b/drivers/ide/alim15x3.c
@@ -68,7 +68,7 @@ static struct pci_dev *isa_dev;
 
 static void ali_set_pio_mode(ide_drive_t *drive, const u8 pio)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct pci_dev *dev = to_pci_dev(hwif->dev);
 	struct ide_timing *t = ide_timing_find_mode(XFER_PIO_0 + pio);
 	int s_time = t->setup, a_time = t->active, c_time = t->cycle;
@@ -150,7 +150,7 @@ static u8 ali_udma_filter(ide_drive_t *drive)
 
 static void ali_set_dma_mode(ide_drive_t *drive, const u8 speed)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	u8 speed1		= speed;
 	u8 unit			= drive->dn & 1;
@@ -198,7 +198,7 @@ static void ali_set_dma_mode(ide_drive_t *drive, const u8 speed)
 static int ali15x3_dma_setup(ide_drive_t *drive)
 {
 	if (m5229_revision < 0xC2 && drive->media != ide_disk) {
-		if (rq_data_dir(drive->hwif->hwgroup->rq))
+		if (rq_data_dir(drive->hwif->rq))
 			return 1;	/* try PIO instead of DMA */
 	}
 	return ide_dma_setup(drive);
@@ -490,8 +490,6 @@ static int __devinit init_dma_ali15x3(ide_hwif_t *hwif,
 	if (ide_allocate_dma_engine(hwif))
 		return -1;
 
-	hwif->dma_ops = &sff_dma_ops;
-
 	return 0;
 }
 
@@ -511,6 +509,7 @@ static const struct ide_dma_ops ali_dma_ops = {
 	.dma_test_irq		= ide_dma_test_irq,
 	.dma_lost_irq		= ide_dma_lost_irq,
 	.dma_timeout		= ide_dma_timeout,
+	.dma_sff_read_status	= ide_dma_sff_read_status,
 };
 
 static const struct ide_port_info ali15x3_chipset __devinitdata = {
@@ -519,6 +518,7 @@ static const struct ide_port_info ali15x3_chipset __devinitdata = {
 	.init_hwif	= init_hwif_ali15x3,
 	.init_dma	= init_dma_ali15x3,
 	.port_ops	= &ali_port_ops,
+	.dma_ops	= &sff_dma_ops,
 	.pio_mask	= ATA_PIO5,
 	.swdma_mask	= ATA_SWDMA2,
 	.mwdma_mask	= ATA_MWDMA2,
diff --git a/drivers/ide/amd74xx.c b/drivers/ide/amd74xx.c
index c6bcd3014a2..69660a431cd 100644
--- a/drivers/ide/amd74xx.c
+++ b/drivers/ide/amd74xx.c
@@ -82,7 +82,7 @@ static void amd_set_drive(ide_drive_t *drive, const u8 speed)
 {
 	ide_hwif_t *hwif = drive->hwif;
 	struct pci_dev *dev = to_pci_dev(hwif->dev);
-	ide_drive_t *peer = hwif->drives + (~drive->dn & 1);
+	ide_drive_t *peer = ide_get_pair_dev(drive);
 	struct ide_timing t, p;
 	int T, UT;
 	u8 udma_mask = hwif->ultra_mask;
@@ -92,7 +92,7 @@ static void amd_set_drive(ide_drive_t *drive, const u8 speed)
 
 	ide_timing_compute(drive, speed, &t, T, UT);
 
-	if (peer->dev_flags & IDE_DFLAG_PRESENT) {
+	if (peer) {
 		ide_timing_compute(peer, peer->current_speed, &p, T, UT);
 		ide_timing_merge(&p, &t, &t, IDE_TIMING_8BIT);
 	}
diff --git a/drivers/ide/au1xxx-ide.c b/drivers/ide/au1xxx-ide.c
index 0ec8fd1e4dc..79a2dfed8eb 100644
--- a/drivers/ide/au1xxx-ide.c
+++ b/drivers/ide/au1xxx-ide.c
@@ -212,8 +212,8 @@ static void auide_set_dma_mode(ide_drive_t *drive, const u8 speed)
 static int auide_build_dmatable(ide_drive_t *drive)
 {
 	int i, iswrite, count = 0;
-	ide_hwif_t *hwif = HWIF(drive);
-	struct request *rq = HWGROUP(drive)->rq;
+	ide_hwif_t *hwif = drive->hwif;
+	struct request *rq = hwif->rq;
 	_auide_hwif *ahwif = &auide_hwif;
 	struct scatterlist *sg;
 
@@ -286,7 +286,7 @@ static int auide_build_dmatable(ide_drive_t *drive)
 
 static int auide_dma_end(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 
 	if (hwif->sg_nents) {
 		ide_destroy_dmatable(drive);
@@ -309,8 +309,8 @@ static void auide_dma_exec_cmd(ide_drive_t *drive, u8 command)
 }
 
 static int auide_dma_setup(ide_drive_t *drive)
-{       	
-	struct request *rq = HWGROUP(drive)->rq;
+{
+	struct request *rq = drive->hwif->rq;
 
 	if (!auide_build_dmatable(drive)) {
 		ide_map_sg(drive, rq);
@@ -502,7 +502,6 @@ static const struct ide_tp_ops au1xxx_tp_ops = {
 	.exec_command		= ide_exec_command,
 	.read_status		= ide_read_status,
 	.read_altstatus		= ide_read_altstatus,
-	.read_sff_dma_status	= ide_read_sff_dma_status,
 
 	.set_irq		= ide_set_irq,
 
diff --git a/drivers/ide/cmd640.c b/drivers/ide/cmd640.c
index e4306647d00..8890276fef7 100644
--- a/drivers/ide/cmd640.c
+++ b/drivers/ide/cmd640.c
@@ -467,11 +467,10 @@ static void program_drive_counts(ide_drive_t *drive, unsigned int index)
 	 * so we merge the timings, using the slowest value for each timing.
 	 */
 	if (index > 1) {
-		ide_hwif_t *hwif = drive->hwif;
-		ide_drive_t *peer = &hwif->drives[!(drive->dn & 1)];
+		ide_drive_t *peer = ide_get_pair_dev(drive);
 		unsigned int mate = index ^ 1;
 
-		if (peer->dev_flags & IDE_DFLAG_PRESENT) {
+		if (peer) {
 			if (setup_count < setup_counts[mate])
 				setup_count = setup_counts[mate];
 			if (active_count < active_counts[mate])
diff --git a/drivers/ide/cmd64x.c b/drivers/ide/cmd64x.c
index 3623bf013bc..2f9688d87ec 100644
--- a/drivers/ide/cmd64x.c
+++ b/drivers/ide/cmd64x.c
@@ -115,7 +115,7 @@ static void program_cycle_times (ide_drive_t *drive, int cycle_time, int active_
  */
 static void cmd64x_tune_pio(ide_drive_t *drive, const u8 pio)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	struct ide_timing *t	= ide_timing_find_mode(XFER_PIO_0 + pio);
 	unsigned int cycle_time;
@@ -138,10 +138,12 @@ static void cmd64x_tune_pio(ide_drive_t *drive, const u8 pio)
 	 * the slowest address setup timing ourselves.
 	 */
 	if (hwif->channel) {
-		ide_drive_t *drives = hwif->drives;
+		ide_drive_t *pair = ide_get_pair_dev(drive);
 
 		drive->drive_data = setup_count;
-		setup_count = max(drives[0].drive_data, drives[1].drive_data);
+
+		if (pair)
+			setup_count = max_t(u8, setup_count, pair->drive_data);
 	}
 
 	if (setup_count > 5)		/* shouldn't actually happen... */
@@ -180,7 +182,7 @@ static void cmd64x_set_pio_mode(ide_drive_t *drive, const u8 pio)
 
 static void cmd64x_set_dma_mode(ide_drive_t *drive, const u8 speed)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	u8 unit			= drive->dn & 0x01;
 	u8 regU = 0, pciU	= hwif->channel ? UDIDETCR1 : UDIDETCR0;
@@ -226,7 +228,7 @@ static void cmd64x_set_dma_mode(ide_drive_t *drive, const u8 speed)
 
 static int cmd648_dma_end(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	unsigned long base	= hwif->dma_base - (hwif->channel * 8);
 	int err			= ide_dma_end(drive);
 	u8  irq_mask		= hwif->channel ? MRDMODE_INTR_CH1 :
@@ -242,7 +244,7 @@ static int cmd648_dma_end(ide_drive_t *drive)
 
 static int cmd64x_dma_end(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	int irq_reg		= hwif->channel ? ARTTIM23 : CFR;
 	u8  irq_mask		= hwif->channel ? ARTTIM23_INTR_CH1 :
@@ -259,7 +261,7 @@ static int cmd64x_dma_end(ide_drive_t *drive)
 
 static int cmd648_dma_test_irq(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	unsigned long base	= hwif->dma_base - (hwif->channel * 8);
 	u8 irq_mask		= hwif->channel ? MRDMODE_INTR_CH1 :
 						  MRDMODE_INTR_CH0;
@@ -282,7 +284,7 @@ static int cmd648_dma_test_irq(ide_drive_t *drive)
 
 static int cmd64x_dma_test_irq(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	int irq_reg		= hwif->channel ? ARTTIM23 : CFR;
 	u8  irq_mask		= hwif->channel ? ARTTIM23_INTR_CH1 :
@@ -313,7 +315,7 @@ static int cmd64x_dma_test_irq(ide_drive_t *drive)
 
 static int cmd646_1_dma_end(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	u8 dma_stat = 0, dma_cmd = 0;
 
 	drive->waiting_for_dma = 0;
@@ -383,6 +385,7 @@ static const struct ide_dma_ops cmd64x_dma_ops = {
 	.dma_test_irq		= cmd64x_dma_test_irq,
 	.dma_lost_irq		= ide_dma_lost_irq,
 	.dma_timeout		= ide_dma_timeout,
+	.dma_sff_read_status	= ide_dma_sff_read_status,
 };
 
 static const struct ide_dma_ops cmd646_rev1_dma_ops = {
@@ -394,6 +397,7 @@ static const struct ide_dma_ops cmd646_rev1_dma_ops = {
 	.dma_test_irq		= ide_dma_test_irq,
 	.dma_lost_irq		= ide_dma_lost_irq,
 	.dma_timeout		= ide_dma_timeout,
+	.dma_sff_read_status	= ide_dma_sff_read_status,
 };
 
 static const struct ide_dma_ops cmd648_dma_ops = {
@@ -405,6 +409,7 @@ static const struct ide_dma_ops cmd648_dma_ops = {
 	.dma_test_irq		= cmd648_dma_test_irq,
 	.dma_lost_irq		= ide_dma_lost_irq,
 	.dma_timeout		= ide_dma_timeout,
+	.dma_sff_read_status	= ide_dma_sff_read_status,
 };
 
 static const struct ide_port_info cmd64x_chipsets[] __devinitdata = {
diff --git a/drivers/ide/cs5520.c b/drivers/ide/cs5520.c
index 5efb467f8fa..d003bec56ff 100644
--- a/drivers/ide/cs5520.c
+++ b/drivers/ide/cs5520.c
@@ -59,7 +59,7 @@ static struct pio_clocks cs5520_pio_clocks[]={
 
 static void cs5520_set_pio_mode(ide_drive_t *drive, const u8 pio)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct pci_dev *pdev = to_pci_dev(hwif->dev);
 	int controller = drive->dn > 1 ? 1 : 0;
 
diff --git a/drivers/ide/cy82c693.c b/drivers/ide/cy82c693.c
index d37baf8ecc5..74fc5401f40 100644
--- a/drivers/ide/cy82c693.c
+++ b/drivers/ide/cy82c693.c
@@ -203,7 +203,7 @@ static void cy82c693_set_dma_mode(ide_drive_t *drive, const u8 mode)
 
 static void cy82c693_set_pio_mode(ide_drive_t *drive, const u8 pio)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct pci_dev *dev = to_pci_dev(hwif->dev);
 	pio_clocks_t pclk;
 	unsigned int addrCtrl;
diff --git a/drivers/ide/falconide.c b/drivers/ide/falconide.c
index 39d500d84b0..a5ba820d69b 100644
--- a/drivers/ide/falconide.c
+++ b/drivers/ide/falconide.c
@@ -70,7 +70,6 @@ static const struct ide_tp_ops falconide_tp_ops = {
 	.exec_command		= ide_exec_command,
 	.read_status		= ide_read_status,
 	.read_altstatus		= ide_read_altstatus,
-	.read_sff_dma_status	= ide_read_sff_dma_status,
 
 	.set_irq		= ide_set_irq,
 
diff --git a/drivers/ide/hpt366.c b/drivers/ide/hpt366.c
index b18e10d99d2..3eb9b5c63a0 100644
--- a/drivers/ide/hpt366.c
+++ b/drivers/ide/hpt366.c
@@ -626,7 +626,7 @@ static struct hpt_info *hpt3xx_get_info(struct device *dev)
 
 static u8 hpt3xx_udma_filter(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct hpt_info *info	= hpt3xx_get_info(hwif->dev);
 	u8 mask 		= hwif->ultra_mask;
 
@@ -665,7 +665,7 @@ static u8 hpt3xx_udma_filter(ide_drive_t *drive)
 
 static u8 hpt3xx_mdma_filter(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct hpt_info *info	= hpt3xx_get_info(hwif->dev);
 
 	switch (info->chip_type) {
@@ -743,7 +743,7 @@ static void hpt3xx_quirkproc(ide_drive_t *drive)
 
 static void hpt3xx_maskproc(ide_drive_t *drive, int mask)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev	*dev	= to_pci_dev(hwif->dev);
 	struct hpt_info *info	= hpt3xx_get_info(hwif->dev);
 
@@ -788,7 +788,7 @@ static void hpt366_dma_lost_irq(ide_drive_t *drive)
 
 static void hpt370_clear_engine(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct pci_dev *dev = to_pci_dev(hwif->dev);
 
 	pci_write_config_byte(dev, hwif->select_data, 0x37);
@@ -797,7 +797,7 @@ static void hpt370_clear_engine(ide_drive_t *drive)
 
 static void hpt370_irq_timeout(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	u16 bfifo		= 0;
 	u8  dma_cmd;
@@ -822,7 +822,7 @@ static void hpt370_dma_start(ide_drive_t *drive)
 
 static int hpt370_dma_end(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	u8  dma_stat		= inb(hwif->dma_base + ATA_DMA_STATUS);
 
 	if (dma_stat & 0x01) {
@@ -844,7 +844,7 @@ static void hpt370_dma_timeout(ide_drive_t *drive)
 /* returns 1 if DMA IRQ issued, 0 otherwise */
 static int hpt374_dma_test_irq(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	u16 bfifo		= 0;
 	u8  dma_stat;
@@ -865,7 +865,7 @@ static int hpt374_dma_test_irq(ide_drive_t *drive)
 
 static int hpt374_dma_end(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	u8 mcr	= 0, mcr_addr	= hwif->select_data;
 	u8 bwsr = 0, mask	= hwif->channel ? 0x02 : 0x01;
@@ -927,7 +927,7 @@ static void hpt3xxn_set_clock(ide_hwif_t *hwif, u8 mode)
 
 static void hpt3xxn_rw_disk(ide_drive_t *drive, struct request *rq)
 {
-	hpt3xxn_set_clock(HWIF(drive), rq_data_dir(rq) ? 0x23 : 0x21);
+	hpt3xxn_set_clock(drive->hwif, rq_data_dir(rq) ? 0x23 : 0x21);
 }
 
 /**
@@ -1349,8 +1349,6 @@ static int __devinit init_dma_hpt366(ide_hwif_t *hwif,
 	if (ide_allocate_dma_engine(hwif))
 		return -1;
 
-	hwif->dma_ops = &sff_dma_ops;
-
 	return 0;
 }
 
@@ -1426,6 +1424,7 @@ static const struct ide_dma_ops hpt37x_dma_ops = {
 	.dma_test_irq		= hpt374_dma_test_irq,
 	.dma_lost_irq		= ide_dma_lost_irq,
 	.dma_timeout		= ide_dma_timeout,
+	.dma_sff_read_status	= ide_dma_sff_read_status,
 };
 
 static const struct ide_dma_ops hpt370_dma_ops = {
@@ -1437,6 +1436,7 @@ static const struct ide_dma_ops hpt370_dma_ops = {
 	.dma_test_irq		= ide_dma_test_irq,
 	.dma_lost_irq		= ide_dma_lost_irq,
 	.dma_timeout		= hpt370_dma_timeout,
+	.dma_sff_read_status	= ide_dma_sff_read_status,
 };
 
 static const struct ide_dma_ops hpt36x_dma_ops = {
@@ -1448,6 +1448,7 @@ static const struct ide_dma_ops hpt36x_dma_ops = {
 	.dma_test_irq		= ide_dma_test_irq,
 	.dma_lost_irq		= hpt366_dma_lost_irq,
 	.dma_timeout		= ide_dma_timeout,
+	.dma_sff_read_status	= ide_dma_sff_read_status,
 };
 
 static const struct ide_port_info hpt366_chipsets[] __devinitdata = {
diff --git a/drivers/ide/icside.c b/drivers/ide/icside.c
index 81f70caeb40..97a35c667ae 100644
--- a/drivers/ide/icside.c
+++ b/drivers/ide/icside.c
@@ -166,7 +166,7 @@ static const expansioncard_ops_t icside_ops_arcin_v6 = {
  */
 static void icside_maskproc(ide_drive_t *drive, int mask)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct expansion_card *ec = ECARD_DEV(hwif->dev);
 	struct icside_state *state = ecard_get_drvdata(ec);
 	unsigned long flags;
@@ -284,7 +284,7 @@ static void icside_dma_host_set(ide_drive_t *drive, int on)
 
 static int icside_dma_end(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct expansion_card *ec = ECARD_DEV(hwif->dev);
 
 	drive->waiting_for_dma = 0;
@@ -299,7 +299,7 @@ static int icside_dma_end(ide_drive_t *drive)
 
 static void icside_dma_start(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct expansion_card *ec = ECARD_DEV(hwif->dev);
 
 	/* We can not enable DMA on both channels simultaneously. */
@@ -309,10 +309,10 @@ static void icside_dma_start(ide_drive_t *drive)
 
 static int icside_dma_setup(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct expansion_card *ec = ECARD_DEV(hwif->dev);
 	struct icside_state *state = ecard_get_drvdata(ec);
-	struct request *rq = hwif->hwgroup->rq;
+	struct request *rq = hwif->rq;
 	unsigned int dma_mode;
 
 	if (rq_data_dir(rq))
@@ -362,7 +362,7 @@ static void icside_dma_exec_cmd(ide_drive_t *drive, u8 cmd)
 
 static int icside_dma_test_irq(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct expansion_card *ec = ECARD_DEV(hwif->dev);
 	struct icside_state *state = ecard_get_drvdata(ec);
 
diff --git a/drivers/ide/ide-acpi.c b/drivers/ide/ide-acpi.c
index fd4a3643305..2f9e941968d 100644
--- a/drivers/ide/ide-acpi.c
+++ b/drivers/ide/ide-acpi.c
@@ -218,7 +218,7 @@ static acpi_handle ide_acpi_hwif_get_handle(ide_hwif_t *hwif)
  */
 static acpi_handle ide_acpi_drive_get_handle(ide_drive_t *drive)
 {
-	ide_hwif_t	*hwif = HWIF(drive);
+	ide_hwif_t	*hwif = drive->hwif;
 	int		 port;
 	acpi_handle	 drive_handle;
 
@@ -263,7 +263,7 @@ static int do_drive_get_GTF(ide_drive_t *drive,
 	acpi_status			status;
 	struct acpi_buffer		output;
 	union acpi_object 		*out_obj;
-	ide_hwif_t			*hwif = HWIF(drive);
+	ide_hwif_t			*hwif = drive->hwif;
 	struct device			*dev = hwif->gendev.parent;
 	int				err = -ENODEV;
 	int				port;
@@ -641,7 +641,8 @@ void ide_acpi_push_timing(ide_hwif_t *hwif)
  */
 void ide_acpi_set_state(ide_hwif_t *hwif, int on)
 {
-	int unit;
+	ide_drive_t *drive;
+	int i;
 
 	if (ide_noacpi || ide_noacpi_psx)
 		return;
@@ -655,9 +656,8 @@ void ide_acpi_set_state(ide_hwif_t *hwif, int on)
 	/* channel first and then drives for power on and verse versa for power off */
 	if (on)
 		acpi_bus_set_power(hwif->acpidata->obj_handle, ACPI_STATE_D0);
-	for (unit = 0; unit < MAX_DRIVES; ++unit) {
-		ide_drive_t *drive = &hwif->drives[unit];
 
+	ide_port_for_each_dev(i, drive, hwif) {
 		if (!drive->acpidata->obj_handle)
 			drive->acpidata->obj_handle = ide_acpi_drive_get_handle(drive);
 
@@ -711,15 +711,13 @@ void ide_acpi_port_init_devices(ide_hwif_t *hwif)
 	 * for both drives, regardless whether they are connected
 	 * or not.
 	 */
-	hwif->drives[0].acpidata = &hwif->acpidata->master;
-	hwif->drives[1].acpidata = &hwif->acpidata->slave;
+	hwif->devices[0]->acpidata = &hwif->acpidata->master;
+	hwif->devices[1]->acpidata = &hwif->acpidata->slave;
 
 	/*
 	 * Send IDENTIFY for each drive
 	 */
-	for (i = 0; i < MAX_DRIVES; i++) {
-		drive = &hwif->drives[i];
-
+	ide_port_for_each_dev(i, drive, hwif) {
 		memset(drive->acpidata, 0, sizeof(*drive->acpidata));
 
 		if ((drive->dev_flags & IDE_DFLAG_PRESENT) == 0)
@@ -744,9 +742,7 @@ void ide_acpi_port_init_devices(ide_hwif_t *hwif)
 	ide_acpi_get_timing(hwif);
 	ide_acpi_push_timing(hwif);
 
-	for (i = 0; i < MAX_DRIVES; i++) {
-		drive = &hwif->drives[i];
-
+	ide_port_for_each_dev(i, drive, hwif) {
 		if (drive->dev_flags & IDE_DFLAG_PRESENT)
 			/* Execute ACPI startup code */
 			ide_acpi_exec_tfs(drive);
diff --git a/drivers/ide/ide-atapi.c b/drivers/ide/ide-atapi.c
index e8688c0f864..e96c0126059 100644
--- a/drivers/ide/ide-atapi.c
+++ b/drivers/ide/ide-atapi.c
@@ -243,7 +243,7 @@ EXPORT_SYMBOL_GPL(ide_retry_pc);
 
 int ide_cd_expiry(ide_drive_t *drive)
 {
-	struct request *rq = HWGROUP(drive)->rq;
+	struct request *rq = drive->hwif->rq;
 	unsigned long wait = 0;
 
 	debug_log("%s: rq->cmd[0]: 0x%x\n", __func__, rq->cmd[0]);
@@ -294,7 +294,7 @@ static ide_startstop_t ide_pc_intr(ide_drive_t *drive)
 {
 	struct ide_atapi_pc *pc = drive->pc;
 	ide_hwif_t *hwif = drive->hwif;
-	struct request *rq = hwif->hwgroup->rq;
+	struct request *rq = hwif->rq;
 	const struct ide_tp_ops *tp_ops = hwif->tp_ops;
 	xfer_func_t *xferfunc;
 	unsigned int timeout, temp;
@@ -491,7 +491,7 @@ static ide_startstop_t ide_transfer_pc(ide_drive_t *drive)
 {
 	struct ide_atapi_pc *uninitialized_var(pc);
 	ide_hwif_t *hwif = drive->hwif;
-	struct request *rq = hwif->hwgroup->rq;
+	struct request *rq = hwif->rq;
 	ide_expiry_t *expiry;
 	unsigned int timeout;
 	int cmd_len;
@@ -549,7 +549,10 @@ static ide_startstop_t ide_transfer_pc(ide_drive_t *drive)
 	}
 
 	/* Set the interrupt routine */
-	ide_set_handler(drive, ide_pc_intr, timeout, expiry);
+	ide_set_handler(drive,
+			(dev_is_idecd(drive) ? drive->irq_handler
+					     : ide_pc_intr),
+			timeout, expiry);
 
 	/* Begin DMA, if necessary */
 	if (dev_is_idecd(drive)) {
@@ -580,7 +583,7 @@ ide_startstop_t ide_issue_pc(ide_drive_t *drive)
 
 	if (dev_is_idecd(drive)) {
 		tf_flags = IDE_TFLAG_OUT_NSECT | IDE_TFLAG_OUT_LBAL;
-		bcount = ide_cd_get_xferlen(hwif->hwgroup->rq);
+		bcount = ide_cd_get_xferlen(hwif->rq);
 		expiry = ide_cd_expiry;
 		timeout = ATAPI_WAIT_PC;
 
diff --git a/drivers/ide/ide-cd.c b/drivers/ide/ide-cd.c
index 1a7410f8824..cae69372cf4 100644
--- a/drivers/ide/ide-cd.c
+++ b/drivers/ide/ide-cd.c
@@ -239,7 +239,7 @@ static void cdrom_queue_request_sense(ide_drive_t *drive, void *sense,
 
 static void cdrom_end_request(ide_drive_t *drive, int uptodate)
 {
-	struct request *rq = HWGROUP(drive)->rq;
+	struct request *rq = drive->hwif->rq;
 	int nsectors = rq->hard_cur_sectors;
 
 	ide_debug_log(IDE_DBG_FUNC, "Call %s, cmd: 0x%x, uptodate: 0x%x, "
@@ -306,8 +306,7 @@ static void ide_dump_status_no_sense(ide_drive_t *drive, const char *msg, u8 st)
 static int cdrom_decode_status(ide_drive_t *drive, int good_stat, int *stat_ret)
 {
 	ide_hwif_t *hwif = drive->hwif;
-	ide_hwgroup_t *hwgroup = hwif->hwgroup;
-	struct request *rq = hwgroup->rq;
+	struct request *rq = hwif->rq;
 	int stat, err, sense_key;
 
 	/* check for errors */
@@ -502,7 +501,7 @@ end_request:
 		blkdev_dequeue_request(rq);
 		spin_unlock_irqrestore(q->queue_lock, flags);
 
-		hwgroup->rq = NULL;
+		hwif->rq = NULL;
 
 		cdrom_queue_request_sense(drive, rq->sense, rq);
 	} else
@@ -511,106 +510,6 @@ end_request:
 	return 1;
 }
 
-static ide_startstop_t cdrom_transfer_packet_command(ide_drive_t *);
-static ide_startstop_t cdrom_newpc_intr(ide_drive_t *);
-
-/*
- * Set up the device registers for transferring a packet command on DEV,
- * expecting to later transfer XFERLEN bytes.  HANDLER is the routine
- * which actually transfers the command to the drive.  If this is a
- * drq_interrupt device, this routine will arrange for HANDLER to be
- * called when the interrupt from the drive arrives.  Otherwise, HANDLER
- * will be called immediately after the drive is prepared for the transfer.
- */
-static ide_startstop_t cdrom_start_packet_command(ide_drive_t *drive)
-{
-	ide_hwif_t *hwif = drive->hwif;
-	struct request *rq = hwif->hwgroup->rq;
-	int xferlen;
-
-	xferlen = ide_cd_get_xferlen(rq);
-
-	ide_debug_log(IDE_DBG_PC, "Call %s, xferlen: %d\n", __func__, xferlen);
-
-	/* FIXME: for Virtual DMA we must check harder */
-	if (drive->dma)
-		drive->dma = !hwif->dma_ops->dma_setup(drive);
-
-	/* set up the controller registers */
-	ide_pktcmd_tf_load(drive, IDE_TFLAG_OUT_NSECT | IDE_TFLAG_OUT_LBAL,
-			   xferlen, drive->dma);
-
-	if (drive->atapi_flags & IDE_AFLAG_DRQ_INTERRUPT) {
-		/* waiting for CDB interrupt, not DMA yet. */
-		if (drive->dma)
-			drive->waiting_for_dma = 0;
-
-		/* packet command */
-		ide_execute_command(drive, ATA_CMD_PACKET,
-				    cdrom_transfer_packet_command,
-				    ATAPI_WAIT_PC, ide_cd_expiry);
-		return ide_started;
-	} else {
-		ide_execute_pkt_cmd(drive);
-
-		return cdrom_transfer_packet_command(drive);
-	}
-}
-
-/*
- * Send a packet command to DRIVE described by CMD_BUF and CMD_LEN. The device
- * registers must have already been prepared by cdrom_start_packet_command.
- * HANDLER is the interrupt handler to call when the command completes or
- * there's data ready.
- */
-#define ATAPI_MIN_CDB_BYTES 12
-static ide_startstop_t cdrom_transfer_packet_command(ide_drive_t *drive)
-{
-	ide_hwif_t *hwif = drive->hwif;
-	struct request *rq = hwif->hwgroup->rq;
-	int cmd_len;
-	ide_startstop_t startstop;
-
-	ide_debug_log(IDE_DBG_PC, "Call %s\n", __func__);
-
-	if (drive->atapi_flags & IDE_AFLAG_DRQ_INTERRUPT) {
-		/*
-		 * Here we should have been called after receiving an interrupt
-		 * from the device.  DRQ should how be set.
-		 */
-
-		/* check for errors */
-		if (cdrom_decode_status(drive, ATA_DRQ, NULL))
-			return ide_stopped;
-
-		/* ok, next interrupt will be DMA interrupt */
-		if (drive->dma)
-			drive->waiting_for_dma = 1;
-	} else {
-		/* otherwise, we must wait for DRQ to get set */
-		if (ide_wait_stat(&startstop, drive, ATA_DRQ,
-				  ATA_BUSY, WAIT_READY))
-			return startstop;
-	}
-
-	/* arm the interrupt handler */
-	ide_set_handler(drive, cdrom_newpc_intr, rq->timeout, ide_cd_expiry);
-
-	/* ATAPI commands get padded out to 12 bytes minimum */
-	cmd_len = COMMAND_SIZE(rq->cmd[0]);
-	if (cmd_len < ATAPI_MIN_CDB_BYTES)
-		cmd_len = ATAPI_MIN_CDB_BYTES;
-
-	/* send the command to the device */
-	hwif->tp_ops->output_data(drive, NULL, rq->cmd, cmd_len);
-
-	/* start the DMA if need be */
-	if (drive->dma)
-		hwif->dma_ops->dma_start(drive);
-
-	return ide_started;
-}
-
 /*
  * Check the contents of the interrupt reason register from the cdrom
  * and attempt to recover if there are problems.  Returns  0 if everything's
@@ -854,8 +753,7 @@ static int cdrom_newpc_intr_dummy_cb(struct request *rq)
 static ide_startstop_t cdrom_newpc_intr(ide_drive_t *drive)
 {
 	ide_hwif_t *hwif = drive->hwif;
-	ide_hwgroup_t *hwgroup = hwif->hwgroup;
-	struct request *rq = hwgroup->rq;
+	struct request *rq = hwif->rq;
 	xfer_func_t *xferfunc;
 	ide_expiry_t *expiry = NULL;
 	int dma_error = 0, dma, stat, thislen, uptodate = 0;
@@ -1061,7 +959,7 @@ end_request:
 		if (blk_end_request(rq, 0, dlen))
 			BUG();
 
-		hwgroup->rq = NULL;
+		hwif->rq = NULL;
 	} else {
 		if (!uptodate)
 			rq->cmd_flags |= REQ_FAILED;
@@ -1183,7 +1081,7 @@ static ide_startstop_t ide_cd_do_request(ide_drive_t *drive, struct request *rq,
 		return ide_stopped;
 	}
 
-	return cdrom_start_packet_command(drive);
+	return ide_issue_pc(drive);
 }
 
 /*
@@ -1916,7 +1814,7 @@ static void ide_cd_release(struct kref *kref)
 
 static int ide_cd_probe(ide_drive_t *);
 
-static ide_driver_t ide_cdrom_driver = {
+static struct ide_driver ide_cdrom_driver = {
 	.gen_driver = {
 		.owner		= THIS_MODULE,
 		.name		= "ide-cdrom",
@@ -1927,7 +1825,6 @@ static ide_driver_t ide_cdrom_driver = {
 	.version		= IDECD_VERSION,
 	.do_request		= ide_cd_do_request,
 	.end_request		= ide_end_request,
-	.error			= __ide_error,
 #ifdef CONFIG_IDE_PROC_FS
 	.proc_entries		= ide_cd_proc_entries,
 	.proc_devsets		= ide_cd_proc_devsets,
@@ -2082,6 +1979,7 @@ static int ide_cd_probe(ide_drive_t *drive)
 	}
 
 	drive->debug_mask = debug_mask;
+	drive->irq_handler = cdrom_newpc_intr;
 
 	info = kzalloc(sizeof(struct cdrom_info), GFP_KERNEL);
 	if (info == NULL) {
diff --git a/drivers/ide/ide-cd.h b/drivers/ide/ide-cd.h
index bf676b26218..ac40d6cb90a 100644
--- a/drivers/ide/ide-cd.h
+++ b/drivers/ide/ide-cd.h
@@ -33,33 +33,33 @@
 
 /* Structure of a MSF cdrom address. */
 struct atapi_msf {
-	byte reserved;
-	byte minute;
-	byte second;
-	byte frame;
+	u8 reserved;
+	u8 minute;
+	u8 second;
+	u8 frame;
 };
 
 /* Space to hold the disk TOC. */
 #define MAX_TRACKS 99
 struct atapi_toc_header {
 	unsigned short toc_length;
-	byte first_track;
-	byte last_track;
+	u8 first_track;
+	u8 last_track;
 };
 
 struct atapi_toc_entry {
-	byte reserved1;
+	u8 reserved1;
 #if defined(__BIG_ENDIAN_BITFIELD)
-	__u8 adr     : 4;
-	__u8 control : 4;
+	u8 adr     : 4;
+	u8 control : 4;
 #elif defined(__LITTLE_ENDIAN_BITFIELD)
-	__u8 control : 4;
-	__u8 adr     : 4;
+	u8 control : 4;
+	u8 adr     : 4;
 #else
 #error "Please fix <asm/byteorder.h>"
 #endif
-	byte track;
-	byte reserved2;
+	u8 track;
+	u8 reserved2;
 	union {
 		unsigned lba;
 		struct atapi_msf msf;
@@ -77,10 +77,10 @@ struct atapi_toc {
 
 /* Extra per-device info for cdrom drives. */
 struct cdrom_info {
-	ide_drive_t	*drive;
-	ide_driver_t	*driver;
-	struct gendisk	*disk;
-	struct kref	kref;
+	ide_drive_t		*drive;
+	struct ide_driver	*driver;
+	struct gendisk		*disk;
+	struct kref		kref;
 
 	/* Buffer for table of contents.  NULL if we haven't allocated
 	   a TOC buffer for this device yet. */
diff --git a/drivers/ide/ide-disk.c b/drivers/ide/ide-disk.c
index eb9fac4d0f0..4088a622873 100644
--- a/drivers/ide/ide-disk.c
+++ b/drivers/ide/ide-disk.c
@@ -89,7 +89,7 @@ static void ide_tf_set_cmd(ide_drive_t *drive, ide_task_t *task, u8 dma)
 static ide_startstop_t __ide_do_rw_disk(ide_drive_t *drive, struct request *rq,
 					sector_t block)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	u16 nsectors		= (u16)rq->nr_sectors;
 	u8 lba48		= !!(drive->dev_flags & IDE_DFLAG_LBA48);
 	u8 dma			= !!(drive->dev_flags & IDE_DFLAG_USING_DMA);
@@ -187,7 +187,7 @@ static ide_startstop_t __ide_do_rw_disk(ide_drive_t *drive, struct request *rq,
 static ide_startstop_t ide_do_rw_disk(ide_drive_t *drive, struct request *rq,
 				      sector_t block)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 
 	BUG_ON(drive->dev_flags & IDE_DFLAG_BLOCKED);
 
diff --git a/drivers/ide/ide-dma-sff.c b/drivers/ide/ide-dma-sff.c
index f6d2d44d8a9..123d393658a 100644
--- a/drivers/ide/ide-dma-sff.c
+++ b/drivers/ide/ide-dma-sff.c
@@ -50,6 +50,27 @@ int config_drive_for_dma(ide_drive_t *drive)
 	return 0;
 }
 
+u8 ide_dma_sff_read_status(ide_hwif_t *hwif)
+{
+	unsigned long addr = hwif->dma_base + ATA_DMA_STATUS;
+
+	if (hwif->host_flags & IDE_HFLAG_MMIO)
+		return readb((void __iomem *)addr);
+	else
+		return inb(addr);
+}
+EXPORT_SYMBOL_GPL(ide_dma_sff_read_status);
+
+static void ide_dma_sff_write_status(ide_hwif_t *hwif, u8 val)
+{
+	unsigned long addr = hwif->dma_base + ATA_DMA_STATUS;
+
+	if (hwif->host_flags & IDE_HFLAG_MMIO)
+		writeb(val, (void __iomem *)addr);
+	else
+		outb(val, addr);
+}
+
 /**
  *	ide_dma_host_set	-	Enable/disable DMA on a host
  *	@drive: drive to control
@@ -62,18 +83,14 @@ void ide_dma_host_set(ide_drive_t *drive, int on)
 {
 	ide_hwif_t *hwif = drive->hwif;
 	u8 unit = drive->dn & 1;
-	u8 dma_stat = hwif->tp_ops->read_sff_dma_status(hwif);
+	u8 dma_stat = hwif->dma_ops->dma_sff_read_status(hwif);
 
 	if (on)
 		dma_stat |= (1 << (5 + unit));
 	else
 		dma_stat &= ~(1 << (5 + unit));
 
-	if (hwif->host_flags & IDE_HFLAG_MMIO)
-		writeb(dma_stat,
-		       (void __iomem *)(hwif->dma_base + ATA_DMA_STATUS));
-	else
-		outb(dma_stat, hwif->dma_base + ATA_DMA_STATUS);
+	ide_dma_sff_write_status(hwif, dma_stat);
 }
 EXPORT_SYMBOL_GPL(ide_dma_host_set);
 
@@ -175,7 +192,7 @@ EXPORT_SYMBOL_GPL(ide_build_dmatable);
 int ide_dma_setup(ide_drive_t *drive)
 {
 	ide_hwif_t *hwif = drive->hwif;
-	struct request *rq = hwif->hwgroup->rq;
+	struct request *rq = hwif->rq;
 	unsigned int reading = rq_data_dir(rq) ? 0 : ATA_DMA_WR;
 	u8 mmio = (hwif->host_flags & IDE_HFLAG_MMIO) ? 1 : 0;
 	u8 dma_stat;
@@ -187,7 +204,7 @@ int ide_dma_setup(ide_drive_t *drive)
 	}
 
 	/* PRD table */
-	if (hwif->host_flags & IDE_HFLAG_MMIO)
+	if (mmio)
 		writel(hwif->dmatable_dma,
 		       (void __iomem *)(hwif->dma_base + ATA_DMA_TABLE_OFS));
 	else
@@ -200,15 +217,10 @@ int ide_dma_setup(ide_drive_t *drive)
 		outb(reading, hwif->dma_base + ATA_DMA_CMD);
 
 	/* read DMA status for INTR & ERROR flags */
-	dma_stat = hwif->tp_ops->read_sff_dma_status(hwif);
+	dma_stat = hwif->dma_ops->dma_sff_read_status(hwif);
 
 	/* clear INTR & ERROR flags */
-	if (mmio)
-		writeb(dma_stat | ATA_DMA_ERR | ATA_DMA_INTR,
-		       (void __iomem *)(hwif->dma_base + ATA_DMA_STATUS));
-	else
-		outb(dma_stat | ATA_DMA_ERR | ATA_DMA_INTR,
-		     hwif->dma_base + ATA_DMA_STATUS);
+	ide_dma_sff_write_status(hwif, dma_stat | ATA_DMA_ERR | ATA_DMA_INTR);
 
 	drive->waiting_for_dma = 1;
 	return 0;
@@ -232,7 +244,7 @@ EXPORT_SYMBOL_GPL(ide_dma_setup);
 static int dma_timer_expiry(ide_drive_t *drive)
 {
 	ide_hwif_t *hwif = drive->hwif;
-	u8 dma_stat = hwif->tp_ops->read_sff_dma_status(hwif);
+	u8 dma_stat = hwif->dma_ops->dma_sff_read_status(hwif);
 
 	printk(KERN_WARNING "%s: %s: DMA status (0x%02x)\n",
 		drive->name, __func__, dma_stat);
@@ -240,7 +252,7 @@ static int dma_timer_expiry(ide_drive_t *drive)
 	if ((dma_stat & 0x18) == 0x18)	/* BUSY Stupid Early Timer !! */
 		return WAIT_CMD;
 
-	hwif->hwgroup->expiry = NULL;	/* one free ride for now */
+	hwif->expiry = NULL;	/* one free ride for now */
 
 	if (dma_stat & ATA_DMA_ERR)	/* ERROR */
 		return -1;
@@ -289,13 +301,12 @@ EXPORT_SYMBOL_GPL(ide_dma_start);
 int ide_dma_end(ide_drive_t *drive)
 {
 	ide_hwif_t *hwif = drive->hwif;
-	u8 mmio = (hwif->host_flags & IDE_HFLAG_MMIO) ? 1 : 0;
 	u8 dma_stat = 0, dma_cmd = 0, mask;
 
 	drive->waiting_for_dma = 0;
 
 	/* stop DMA */
-	if (mmio) {
+	if (hwif->host_flags & IDE_HFLAG_MMIO) {
 		dma_cmd = readb((void __iomem *)(hwif->dma_base + ATA_DMA_CMD));
 		writeb(dma_cmd & ~ATA_DMA_START,
 		       (void __iomem *)(hwif->dma_base + ATA_DMA_CMD));
@@ -305,15 +316,10 @@ int ide_dma_end(ide_drive_t *drive)
 	}
 
 	/* get DMA status */
-	dma_stat = hwif->tp_ops->read_sff_dma_status(hwif);
+	dma_stat = hwif->dma_ops->dma_sff_read_status(hwif);
 
-	if (mmio)
-		/* clear the INTR & ERROR bits */
-		writeb(dma_stat | ATA_DMA_ERR | ATA_DMA_INTR,
-		       (void __iomem *)(hwif->dma_base + ATA_DMA_STATUS));
-	else
-		outb(dma_stat | ATA_DMA_ERR | ATA_DMA_INTR,
-		     hwif->dma_base + ATA_DMA_STATUS);
+	/* clear INTR & ERROR bits */
+	ide_dma_sff_write_status(hwif, dma_stat | ATA_DMA_ERR | ATA_DMA_INTR);
 
 	/* purge DMA mappings */
 	ide_destroy_dmatable(drive);
@@ -331,7 +337,7 @@ EXPORT_SYMBOL_GPL(ide_dma_end);
 int ide_dma_test_irq(ide_drive_t *drive)
 {
 	ide_hwif_t *hwif = drive->hwif;
-	u8 dma_stat = hwif->tp_ops->read_sff_dma_status(hwif);
+	u8 dma_stat = hwif->dma_ops->dma_sff_read_status(hwif);
 
 	return (dma_stat & ATA_DMA_INTR) ? 1 : 0;
 }
@@ -346,5 +352,6 @@ const struct ide_dma_ops sff_dma_ops = {
 	.dma_test_irq		= ide_dma_test_irq,
 	.dma_timeout		= ide_dma_timeout,
 	.dma_lost_irq		= ide_dma_lost_irq,
+	.dma_sff_read_status	= ide_dma_sff_read_status,
 };
 EXPORT_SYMBOL_GPL(sff_dma_ops);
diff --git a/drivers/ide/ide-dma.c b/drivers/ide/ide-dma.c
index fffd11717b2..72ebab0bc75 100644
--- a/drivers/ide/ide-dma.c
+++ b/drivers/ide/ide-dma.c
@@ -96,7 +96,7 @@ ide_startstop_t ide_dma_intr(ide_drive_t *drive)
 
 	if (OK_STAT(stat, DRIVE_READY, drive->bad_wstat | ATA_DRQ)) {
 		if (!dma_stat) {
-			struct request *rq = hwif->hwgroup->rq;
+			struct request *rq = hwif->rq;
 
 			task_end_request(drive, rq, stat);
 			return ide_stopped;
diff --git a/drivers/ide/ide-floppy.c b/drivers/ide/ide-floppy.c
index 0a48e2dc53a..3eab1c6c9b3 100644
--- a/drivers/ide/ide-floppy.c
+++ b/drivers/ide/ide-floppy.c
@@ -71,7 +71,7 @@
 static int ide_floppy_end_request(ide_drive_t *drive, int uptodate, int nsecs)
 {
 	struct ide_disk_obj *floppy = drive->driver_data;
-	struct request *rq = HWGROUP(drive)->rq;
+	struct request *rq = drive->hwif->rq;
 	int error;
 
 	ide_debug_log(IDE_DBG_FUNC, "Call %s\n", __func__);
diff --git a/drivers/ide/ide-gd.c b/drivers/ide/ide-gd.c
index b8078b3231f..7857b209c6d 100644
--- a/drivers/ide/ide-gd.c
+++ b/drivers/ide/ide-gd.c
@@ -149,7 +149,7 @@ static int ide_gd_end_request(ide_drive_t *drive, int uptodate, int nrsecs)
 	return drive->disk_ops->end_request(drive, uptodate, nrsecs);
 }
 
-static ide_driver_t ide_gd_driver = {
+static struct ide_driver ide_gd_driver = {
 	.gen_driver = {
 		.owner		= THIS_MODULE,
 		.name		= "ide-gd",
@@ -162,7 +162,6 @@ static ide_driver_t ide_gd_driver = {
 	.version		= IDE_GD_VERSION,
 	.do_request		= ide_gd_do_request,
 	.end_request		= ide_gd_end_request,
-	.error			= __ide_error,
 #ifdef CONFIG_IDE_PROC_FS
 	.proc_entries		= ide_disk_proc_entries,
 	.proc_devsets		= ide_disk_proc_devsets,
diff --git a/drivers/ide/ide-gd.h b/drivers/ide/ide-gd.h
index 7d3d101713e..a86779f0756 100644
--- a/drivers/ide/ide-gd.h
+++ b/drivers/ide/ide-gd.h
@@ -14,11 +14,11 @@
 #endif
 
 struct ide_disk_obj {
-	ide_drive_t	*drive;
-	ide_driver_t	*driver;
-	struct gendisk	*disk;
-	struct kref	kref;
-	unsigned int	openers;	/* protected by BKL for now */
+	ide_drive_t		*drive;
+	struct ide_driver	*driver;
+	struct gendisk		*disk;
+	struct kref		kref;
+	unsigned int		openers;	/* protected by BKL for now */
 
 	/* Last failed packet command */
 	struct ide_atapi_pc *failed_pc;
diff --git a/drivers/ide/ide-h8300.c b/drivers/ide/ide-h8300.c
index e2cdd2e9cde..9270d3255ee 100644
--- a/drivers/ide/ide-h8300.c
+++ b/drivers/ide/ide-h8300.c
@@ -159,7 +159,6 @@ static const struct ide_tp_ops h8300_tp_ops = {
 	.exec_command		= ide_exec_command,
 	.read_status		= ide_read_status,
 	.read_altstatus		= ide_read_altstatus,
-	.read_sff_dma_status	= ide_read_sff_dma_status,
 
 	.set_irq		= ide_set_irq,
 
diff --git a/drivers/ide/ide-io.c b/drivers/ide/ide-io.c
index 1c36a8e83d3..cc163319dfb 100644
--- a/drivers/ide/ide-io.c
+++ b/drivers/ide/ide-io.c
@@ -88,7 +88,7 @@ static int __ide_end_request(ide_drive_t *drive, struct request *rq,
 		ret = 0;
 
 	if (ret == 0 && dequeue)
-		drive->hwif->hwgroup->rq = NULL;
+		drive->hwif->rq = NULL;
 
 	return ret;
 }
@@ -107,7 +107,7 @@ static int __ide_end_request(ide_drive_t *drive, struct request *rq,
 int ide_end_request (ide_drive_t *drive, int uptodate, int nr_sectors)
 {
 	unsigned int nr_bytes = nr_sectors << 9;
-	struct request *rq = drive->hwif->hwgroup->rq;
+	struct request *rq = drive->hwif->rq;
 
 	if (!nr_bytes) {
 		if (blk_pc_request(rq))
@@ -160,8 +160,8 @@ EXPORT_SYMBOL_GPL(ide_end_dequeued_request);
  
 void ide_end_drive_cmd (ide_drive_t *drive, u8 stat, u8 err)
 {
-	ide_hwgroup_t *hwgroup = drive->hwif->hwgroup;
-	struct request *rq = hwgroup->rq;
+	ide_hwif_t *hwif = drive->hwif;
+	struct request *rq = hwif->rq;
 
 	if (rq->cmd_type == REQ_TYPE_ATA_TASKFILE) {
 		ide_task_t *task = (ide_task_t *)rq->special;
@@ -186,7 +186,7 @@ void ide_end_drive_cmd (ide_drive_t *drive, u8 stat, u8 err)
 		return;
 	}
 
-	hwgroup->rq = NULL;
+	hwif->rq = NULL;
 
 	rq->errors = err;
 
@@ -199,9 +199,9 @@ EXPORT_SYMBOL(ide_end_drive_cmd);
 static void ide_kill_rq(ide_drive_t *drive, struct request *rq)
 {
 	if (rq->rq_disk) {
-		ide_driver_t *drv;
+		struct ide_driver *drv;
 
-		drv = *(ide_driver_t **)rq->rq_disk->private_data;
+		drv = *(struct ide_driver **)rq->rq_disk->private_data;
 		drv->end_request(drive, 0, 0);
 	} else
 		ide_end_request(drive, 0, 0);
@@ -291,7 +291,7 @@ static ide_startstop_t ide_atapi_error(ide_drive_t *drive, struct request *rq, u
 	return ide_stopped;
 }
 
-ide_startstop_t
+static ide_startstop_t
 __ide_error(ide_drive_t *drive, struct request *rq, u8 stat, u8 err)
 {
 	if (drive->media == ide_disk)
@@ -299,8 +299,6 @@ __ide_error(ide_drive_t *drive, struct request *rq, u8 stat, u8 err)
 	return ide_atapi_error(drive, rq, stat, err);
 }
 
-EXPORT_SYMBOL_GPL(__ide_error);
-
 /**
  *	ide_error	-	handle an error on the IDE
  *	@drive: drive the error occurred on
@@ -321,7 +319,8 @@ ide_startstop_t ide_error (ide_drive_t *drive, const char *msg, u8 stat)
 
 	err = ide_dump_status(drive, msg, stat);
 
-	if ((rq = HWGROUP(drive)->rq) == NULL)
+	rq = drive->hwif->rq;
+	if (rq == NULL)
 		return ide_stopped;
 
 	/* retry only "normal" I/O: */
@@ -331,15 +330,8 @@ ide_startstop_t ide_error (ide_drive_t *drive, const char *msg, u8 stat)
 		return ide_stopped;
 	}
 
-	if (rq->rq_disk) {
-		ide_driver_t *drv;
-
-		drv = *(ide_driver_t **)rq->rq_disk->private_data;
-		return drv->error(drive, rq, stat, err);
-	} else
-		return __ide_error(drive, rq, stat, err);
+	return __ide_error(drive, rq, stat, err);
 }
-
 EXPORT_SYMBOL_GPL(ide_error);
 
 static void ide_tf_set_specify_cmd(ide_drive_t *drive, struct ide_taskfile *tf)
@@ -462,7 +454,7 @@ EXPORT_SYMBOL_GPL(ide_init_sg_cmd);
 static ide_startstop_t execute_drive_cmd (ide_drive_t *drive,
 		struct request *rq)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	ide_task_t *task = rq->special;
 
 	if (task) {
@@ -586,7 +578,7 @@ static ide_startstop_t start_request (ide_drive_t *drive, struct request *rq)
 
 #ifdef DEBUG
 	printk("%s: start_request: current=0x%08lx\n",
-		HWIF(drive)->name, (unsigned long) rq);
+		drive->hwif->name, (unsigned long) rq);
 #endif
 
 	/* bail early if we've exceeded max_failures */
@@ -605,7 +597,7 @@ static ide_startstop_t start_request (ide_drive_t *drive, struct request *rq)
 		return startstop;
 	}
 	if (!drive->special.all) {
-		ide_driver_t *drv;
+		struct ide_driver *drv;
 
 		/*
 		 * We reset the drive so we need to issue a SETFEATURES.
@@ -638,7 +630,7 @@ static ide_startstop_t start_request (ide_drive_t *drive, struct request *rq)
 			 */
 			return ide_special_rq(drive, rq);
 
-		drv = *(ide_driver_t **)rq->rq_disk->private_data;
+		drv = *(struct ide_driver **)rq->rq_disk->private_data;
 
 		return drv->do_request(drive, rq, rq->sector);
 	}
@@ -654,7 +646,7 @@ kill_rq:
  *	@timeout: time to stall for (jiffies)
  *
  *	ide_stall_queue() can be used by a drive to give excess bandwidth back
- *	to the hwgroup by sleeping for timeout jiffies.
+ *	to the port by sleeping for timeout jiffies.
  */
  
 void ide_stall_queue (ide_drive_t *drive, unsigned long timeout)
@@ -666,45 +658,53 @@ void ide_stall_queue (ide_drive_t *drive, unsigned long timeout)
 }
 EXPORT_SYMBOL(ide_stall_queue);
 
+static inline int ide_lock_port(ide_hwif_t *hwif)
+{
+	if (hwif->busy)
+		return 1;
+
+	hwif->busy = 1;
+
+	return 0;
+}
+
+static inline void ide_unlock_port(ide_hwif_t *hwif)
+{
+	hwif->busy = 0;
+}
+
+static inline int ide_lock_host(struct ide_host *host, ide_hwif_t *hwif)
+{
+	int rc = 0;
+
+	if (host->host_flags & IDE_HFLAG_SERIALIZE) {
+		rc = test_and_set_bit_lock(IDE_HOST_BUSY, &host->host_busy);
+		if (rc == 0) {
+			/* for atari only */
+			ide_get_lock(ide_intr, hwif);
+		}
+	}
+	return rc;
+}
+
+static inline void ide_unlock_host(struct ide_host *host)
+{
+	if (host->host_flags & IDE_HFLAG_SERIALIZE) {
+		/* for atari only */
+		ide_release_lock();
+		clear_bit_unlock(IDE_HOST_BUSY, &host->host_busy);
+	}
+}
+
 /*
- * Issue a new request to a drive from hwgroup
- *
- * A hwgroup is a serialized group of IDE interfaces.  Usually there is
- * exactly one hwif (interface) per hwgroup, but buggy controllers (eg. CMD640)
- * may have both interfaces in a single hwgroup to "serialize" access.
- * Or possibly multiple ISA interfaces can share a common IRQ by being grouped
- * together into one hwgroup for serialized access.
- *
- * Note also that several hwgroups can end up sharing a single IRQ,
- * possibly along with many other devices.  This is especially common in
- * PCI-based systems with off-board IDE controller cards.
- *
- * The IDE driver uses a per-hwgroup lock to protect the hwgroup->busy flag.
- *
- * The first thread into the driver for a particular hwgroup sets the
- * hwgroup->busy flag to indicate that this hwgroup is now active,
- * and then initiates processing of the top request from the request queue.
- *
- * Other threads attempting entry notice the busy setting, and will simply
- * queue their new requests and exit immediately.  Note that hwgroup->busy
- * remains set even when the driver is merely awaiting the next interrupt.
- * Thus, the meaning is "this hwgroup is busy processing a request".
- *
- * When processing of a request completes, the completing thread or IRQ-handler
- * will start the next request from the queue.  If no more work remains,
- * the driver will clear the hwgroup->busy flag and exit.
- *
- * The per-hwgroup spinlock is used to protect all access to the
- * hwgroup->busy flag, but is otherwise not needed for most processing in
- * the driver.  This makes the driver much more friendlier to shared IRQs
- * than previous designs, while remaining 100% (?) SMP safe and capable.
+ * Issue a new request to a device.
  */
 void do_ide_request(struct request_queue *q)
 {
 	ide_drive_t	*drive = q->queuedata;
 	ide_hwif_t	*hwif = drive->hwif;
-	ide_hwgroup_t	*hwgroup = hwif->hwgroup;
-	struct request	*rq;
+	struct ide_host *host = hwif->host;
+	struct request	*rq = NULL;
 	ide_startstop_t	startstop;
 
 	/*
@@ -721,32 +721,40 @@ void do_ide_request(struct request_queue *q)
 		blk_remove_plug(q);
 
 	spin_unlock_irq(q->queue_lock);
-	spin_lock_irq(&hwgroup->lock);
 
-	if (!ide_lock_hwgroup(hwgroup)) {
+	if (ide_lock_host(host, hwif))
+		goto plug_device_2;
+
+	spin_lock_irq(&hwif->lock);
+
+	if (!ide_lock_port(hwif)) {
+		ide_hwif_t *prev_port;
 repeat:
-		hwgroup->rq = NULL;
+		prev_port = hwif->host->cur_port;
+		hwif->rq = NULL;
 
 		if (drive->dev_flags & IDE_DFLAG_SLEEPING) {
 			if (time_before(drive->sleep, jiffies)) {
-				ide_unlock_hwgroup(hwgroup);
+				ide_unlock_port(hwif);
 				goto plug_device;
 			}
 		}
 
-		if (hwif != hwgroup->hwif) {
+		if ((hwif->host->host_flags & IDE_HFLAG_SERIALIZE) &&
+		    hwif != prev_port) {
 			/*
-			 * set nIEN for previous hwif, drives in the
+			 * set nIEN for previous port, drives in the
 			 * quirk_list may not like intr setups/cleanups
 			 */
-			if (drive->quirk_list == 0)
-				hwif->tp_ops->set_irq(hwif, 0);
+			if (prev_port && prev_port->cur_dev->quirk_list == 0)
+				prev_port->tp_ops->set_irq(prev_port, 0);
+
+			hwif->host->cur_port = hwif;
 		}
-		hwgroup->hwif = hwif;
-		hwgroup->drive = drive;
+		hwif->cur_dev = drive;
 		drive->dev_flags &= ~(IDE_DFLAG_SLEEPING | IDE_DFLAG_PARKED);
 
-		spin_unlock_irq(&hwgroup->lock);
+		spin_unlock_irq(&hwif->lock);
 		spin_lock_irq(q->queue_lock);
 		/*
 		 * we know that the queue isn't empty, but this can happen
@@ -754,10 +762,10 @@ repeat:
 		 */
 		rq = elv_next_request(drive->queue);
 		spin_unlock_irq(q->queue_lock);
-		spin_lock_irq(&hwgroup->lock);
+		spin_lock_irq(&hwif->lock);
 
 		if (!rq) {
-			ide_unlock_hwgroup(hwgroup);
+			ide_unlock_port(hwif);
 			goto out;
 		}
 
@@ -778,27 +786,31 @@ repeat:
 		    blk_pm_request(rq) == 0 &&
 		    (rq->cmd_flags & REQ_PREEMPT) == 0) {
 			/* there should be no pending command at this point */
-			ide_unlock_hwgroup(hwgroup);
+			ide_unlock_port(hwif);
 			goto plug_device;
 		}
 
-		hwgroup->rq = rq;
+		hwif->rq = rq;
 
-		spin_unlock_irq(&hwgroup->lock);
+		spin_unlock_irq(&hwif->lock);
 		startstop = start_request(drive, rq);
-		spin_lock_irq(&hwgroup->lock);
+		spin_lock_irq(&hwif->lock);
 
 		if (startstop == ide_stopped)
 			goto repeat;
 	} else
 		goto plug_device;
 out:
-	spin_unlock_irq(&hwgroup->lock);
+	spin_unlock_irq(&hwif->lock);
+	if (rq == NULL)
+		ide_unlock_host(host);
 	spin_lock_irq(q->queue_lock);
 	return;
 
 plug_device:
-	spin_unlock_irq(&hwgroup->lock);
+	spin_unlock_irq(&hwif->lock);
+	ide_unlock_host(host);
+plug_device_2:
 	spin_lock_irq(q->queue_lock);
 
 	if (!elv_queue_empty(q))
@@ -806,13 +818,13 @@ plug_device:
 }
 
 /*
- * un-busy the hwgroup etc, and clear any pending DMA status. we want to
+ * un-busy the port etc, and clear any pending DMA status. we want to
  * retry the current request in pio mode instead of risking tossing it
  * all away
  */
 static ide_startstop_t ide_dma_timeout_retry(ide_drive_t *drive, int error)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct request *rq;
 	ide_startstop_t ret = ide_stopped;
 
@@ -840,15 +852,14 @@ static ide_startstop_t ide_dma_timeout_retry(ide_drive_t *drive, int error)
 	ide_dma_off_quietly(drive);
 
 	/*
-	 * un-busy drive etc (hwgroup->busy is cleared on return) and
-	 * make sure request is sane
+	 * un-busy drive etc and make sure request is sane
 	 */
-	rq = HWGROUP(drive)->rq;
 
+	rq = hwif->rq;
 	if (!rq)
 		goto out;
 
-	HWGROUP(drive)->rq = NULL;
+	hwif->rq = NULL;
 
 	rq->errors = 0;
 
@@ -876,7 +887,7 @@ static void ide_plug_device(ide_drive_t *drive)
 
 /**
  *	ide_timer_expiry	-	handle lack of an IDE interrupt
- *	@data: timer callback magic (hwgroup)
+ *	@data: timer callback magic (hwif)
  *
  *	An IDE command has timed out before the expected drive return
  *	occurred. At this point we attempt to clean up the current
@@ -890,18 +901,18 @@ static void ide_plug_device(ide_drive_t *drive)
  
 void ide_timer_expiry (unsigned long data)
 {
-	ide_hwgroup_t	*hwgroup = (ide_hwgroup_t *) data;
+	ide_hwif_t	*hwif = (ide_hwif_t *)data;
 	ide_drive_t	*uninitialized_var(drive);
 	ide_handler_t	*handler;
-	ide_expiry_t	*expiry;
 	unsigned long	flags;
 	unsigned long	wait = -1;
 	int		plug_device = 0;
 
-	spin_lock_irqsave(&hwgroup->lock, flags);
+	spin_lock_irqsave(&hwif->lock, flags);
 
-	if (((handler = hwgroup->handler) == NULL) ||
-	    (hwgroup->req_gen != hwgroup->req_gen_timer)) {
+	handler = hwif->handler;
+
+	if (handler == NULL || hwif->req_gen != hwif->req_gen_timer) {
 		/*
 		 * Either a marginal timeout occurred
 		 * (got the interrupt just as timer expired),
@@ -909,72 +920,68 @@ void ide_timer_expiry (unsigned long data)
 		 * Either way, we don't really want to complain about anything.
 		 */
 	} else {
-		drive = hwgroup->drive;
-		if (!drive) {
-			printk(KERN_ERR "ide_timer_expiry: hwgroup->drive was NULL\n");
-			hwgroup->handler = NULL;
-		} else {
-			ide_hwif_t *hwif;
-			ide_startstop_t startstop = ide_stopped;
-
-			if ((expiry = hwgroup->expiry) != NULL) {
-				/* continue */
-				if ((wait = expiry(drive)) > 0) {
-					/* reset timer */
-					hwgroup->timer.expires  = jiffies + wait;
-					hwgroup->req_gen_timer = hwgroup->req_gen;
-					add_timer(&hwgroup->timer);
-					spin_unlock_irqrestore(&hwgroup->lock, flags);
-					return;
-				}
-			}
-			hwgroup->handler = NULL;
-			/*
-			 * We need to simulate a real interrupt when invoking
-			 * the handler() function, which means we need to
-			 * globally mask the specific IRQ:
-			 */
-			spin_unlock(&hwgroup->lock);
-			hwif  = HWIF(drive);
-			/* disable_irq_nosync ?? */
-			disable_irq(hwif->irq);
-			/* local CPU only,
-			 * as if we were handling an interrupt */
-			local_irq_disable();
-			if (hwgroup->polling) {
-				startstop = handler(drive);
-			} else if (drive_is_ready(drive)) {
-				if (drive->waiting_for_dma)
-					hwif->dma_ops->dma_lost_irq(drive);
-				(void)ide_ack_intr(hwif);
-				printk(KERN_WARNING "%s: lost interrupt\n", drive->name);
-				startstop = handler(drive);
-			} else {
-				if (drive->waiting_for_dma) {
-					startstop = ide_dma_timeout_retry(drive, wait);
-				} else
-					startstop =
-					ide_error(drive, "irq timeout",
-						  hwif->tp_ops->read_status(hwif));
-			}
-			spin_lock_irq(&hwgroup->lock);
-			enable_irq(hwif->irq);
-			if (startstop == ide_stopped) {
-				ide_unlock_hwgroup(hwgroup);
-				plug_device = 1;
+		ide_expiry_t *expiry = hwif->expiry;
+		ide_startstop_t startstop = ide_stopped;
+
+		drive = hwif->cur_dev;
+
+		if (expiry) {
+			wait = expiry(drive);
+			if (wait > 0) { /* continue */
+				/* reset timer */
+				hwif->timer.expires = jiffies + wait;
+				hwif->req_gen_timer = hwif->req_gen;
+				add_timer(&hwif->timer);
+				spin_unlock_irqrestore(&hwif->lock, flags);
+				return;
 			}
 		}
+		hwif->handler = NULL;
+		/*
+		 * We need to simulate a real interrupt when invoking
+		 * the handler() function, which means we need to
+		 * globally mask the specific IRQ:
+		 */
+		spin_unlock(&hwif->lock);
+		/* disable_irq_nosync ?? */
+		disable_irq(hwif->irq);
+		/* local CPU only, as if we were handling an interrupt */
+		local_irq_disable();
+		if (hwif->polling) {
+			startstop = handler(drive);
+		} else if (drive_is_ready(drive)) {
+			if (drive->waiting_for_dma)
+				hwif->dma_ops->dma_lost_irq(drive);
+			(void)ide_ack_intr(hwif);
+			printk(KERN_WARNING "%s: lost interrupt\n",
+				drive->name);
+			startstop = handler(drive);
+		} else {
+			if (drive->waiting_for_dma)
+				startstop = ide_dma_timeout_retry(drive, wait);
+			else
+				startstop = ide_error(drive, "irq timeout",
+					hwif->tp_ops->read_status(hwif));
+		}
+		spin_lock_irq(&hwif->lock);
+		enable_irq(hwif->irq);
+		if (startstop == ide_stopped) {
+			ide_unlock_port(hwif);
+			plug_device = 1;
+		}
 	}
-	spin_unlock_irqrestore(&hwgroup->lock, flags);
+	spin_unlock_irqrestore(&hwif->lock, flags);
 
-	if (plug_device)
+	if (plug_device) {
+		ide_unlock_host(hwif->host);
 		ide_plug_device(drive);
+	}
 }
 
 /**
  *	unexpected_intr		-	handle an unexpected IDE interrupt
  *	@irq: interrupt line
- *	@hwgroup: hwgroup being processed
+ *	@hwif: port being processed
  *
  *	There's nothing really useful we can do with an unexpected interrupt,
  *	other than reading the status register (to clear it), and logging it.
@@ -998,52 +1005,38 @@ void ide_timer_expiry (unsigned long data)
  *	before completing the issuance of any new drive command, so we will not
  *	be accidentally invoked as a result of any valid command completion
  *	interrupt.
- *
- *	Note that we must walk the entire hwgroup here. We know which hwif
- *	is doing the current command, but we don't know which hwif burped
- *	mysteriously.
  */
- 
-static void unexpected_intr (int irq, ide_hwgroup_t *hwgroup)
-{
-	u8 stat;
-	ide_hwif_t *hwif = hwgroup->hwif;
 
-	/*
-	 * handle the unexpected interrupt
-	 */
-	do {
-		if (hwif->irq == irq) {
-			stat = hwif->tp_ops->read_status(hwif);
-
-			if (!OK_STAT(stat, ATA_DRDY, BAD_STAT)) {
-				/* Try to not flood the console with msgs */
-				static unsigned long last_msgtime, count;
-				++count;
-				if (time_after(jiffies, last_msgtime + HZ)) {
-					last_msgtime = jiffies;
-					printk(KERN_ERR "%s%s: unexpected interrupt, "
-						"status=0x%02x, count=%ld\n",
-						hwif->name,
-						(hwif->next==hwgroup->hwif) ? "" : "(?)", stat, count);
-				}
-			}
+static void unexpected_intr(int irq, ide_hwif_t *hwif)
+{
+	u8 stat = hwif->tp_ops->read_status(hwif);
+
+	if (!OK_STAT(stat, ATA_DRDY, BAD_STAT)) {
+		/* Try to not flood the console with msgs */
+		static unsigned long last_msgtime, count;
+		++count;
+
+		if (time_after(jiffies, last_msgtime + HZ)) {
+			last_msgtime = jiffies;
+			printk(KERN_ERR "%s: unexpected interrupt, "
+				"status=0x%02x, count=%ld\n",
+				hwif->name, stat, count);
 		}
-	} while ((hwif = hwif->next) != hwgroup->hwif);
+	}
 }
 
 /**
  *	ide_intr	-	default IDE interrupt handler
  *	@irq: interrupt number
- *	@dev_id: hwif group
+ *	@dev_id: hwif
  *	@regs: unused weirdness from the kernel irq layer
  *
  *	This is the default IRQ handler for the IDE layer. You should
  *	not need to override it. If you do be aware it is subtle in
  *	places
  *
- *	hwgroup->hwif is the interface in the group currently performing
- *	a command. hwgroup->drive is the drive and hwgroup->handler is
+ *	hwif is the interface in the group currently performing
+ *	a command. hwif->cur_dev is the drive and hwif->handler is
  *	the IRQ handler to call. As we issue a command the handlers
  *	step through multiple states, reassigning the handler to the
  *	next step in the process. Unlike a smart SCSI controller IDE
@@ -1054,26 +1047,32 @@ static void unexpected_intr (int irq, ide_hwgroup_t *hwgroup)
  *
  *	The handler eventually returns ide_stopped to indicate the
  *	request completed. At this point we issue the next request
- *	on the hwgroup and the process begins again.
+ *	on the port and the process begins again.
  */
- 
+
 irqreturn_t ide_intr (int irq, void *dev_id)
 {
-	unsigned long flags;
-	ide_hwgroup_t *hwgroup = (ide_hwgroup_t *)dev_id;
-	ide_hwif_t *hwif = hwgroup->hwif;
+	ide_hwif_t *hwif = (ide_hwif_t *)dev_id;
 	ide_drive_t *uninitialized_var(drive);
 	ide_handler_t *handler;
+	unsigned long flags;
 	ide_startstop_t startstop;
 	irqreturn_t irq_ret = IRQ_NONE;
 	int plug_device = 0;
 
-	spin_lock_irqsave(&hwgroup->lock, flags);
+	if (hwif->host->host_flags & IDE_HFLAG_SERIALIZE) {
+		if (hwif != hwif->host->cur_port)
+			goto out_early;
+	}
+
+	spin_lock_irqsave(&hwif->lock, flags);
 
 	if (!ide_ack_intr(hwif))
 		goto out;
 
-	if ((handler = hwgroup->handler) == NULL || hwgroup->polling) {
+	handler = hwif->handler;
+
+	if (handler == NULL || hwif->polling) {
 		/*
 		 * Not expecting an interrupt from this drive.
 		 * That means this could be:
@@ -1097,7 +1096,7 @@ irqreturn_t ide_intr (int irq, void *dev_id)
 			 * Probably not a shared PCI interrupt,
 			 * so we can safely try to do something about it:
 			 */
-			unexpected_intr(irq, hwgroup);
+			unexpected_intr(irq, hwif);
 #ifdef CONFIG_BLK_DEV_IDEPCI
 		} else {
 			/*
@@ -1110,16 +1109,7 @@ irqreturn_t ide_intr (int irq, void *dev_id)
 		goto out;
 	}
 
-	drive = hwgroup->drive;
-	if (!drive) {
-		/*
-		 * This should NEVER happen, and there isn't much
-		 * we could do about it here.
-		 *
-		 * [Note - this can occur if the drive is hot unplugged]
-		 */
-		goto out_handled;
-	}
+	drive = hwif->cur_dev;
 
 	if (!drive_is_ready(drive))
 		/*
@@ -1131,10 +1121,10 @@ irqreturn_t ide_intr (int irq, void *dev_id)
 		 */
 		goto out;
 
-	hwgroup->handler = NULL;
-	hwgroup->req_gen++;
-	del_timer(&hwgroup->timer);
-	spin_unlock(&hwgroup->lock);
+	hwif->handler = NULL;
+	hwif->req_gen++;
+	del_timer(&hwif->timer);
+	spin_unlock(&hwif->lock);
 
 	if (hwif->port_ops && hwif->port_ops->clear_irq)
 		hwif->port_ops->clear_irq(drive);
@@ -1145,7 +1135,7 @@ irqreturn_t ide_intr (int irq, void *dev_id)
 	/* service this interrupt, may set handler for next interrupt */
 	startstop = handler(drive);
 
-	spin_lock_irq(&hwgroup->lock);
+	spin_lock_irq(&hwif->lock);
 	/*
 	 * Note that handler() may have set things up for another
 	 * interrupt to occur soon, but it cannot happen until
@@ -1154,20 +1144,18 @@ irqreturn_t ide_intr (int irq, void *dev_id)
 	 * won't allow another of the same (on any CPU) until we return.
 	 */
 	if (startstop == ide_stopped) {
-		if (hwgroup->handler == NULL) {	/* paranoia */
-			ide_unlock_hwgroup(hwgroup);
-			plug_device = 1;
-		} else
-			printk(KERN_ERR "%s: %s: huh? expected NULL handler "
-					"on exit\n", __func__, drive->name);
+		BUG_ON(hwif->handler);
+		ide_unlock_port(hwif);
+		plug_device = 1;
 	}
-out_handled:
 	irq_ret = IRQ_HANDLED;
 out:
-	spin_unlock_irqrestore(&hwgroup->lock, flags);
-
-	if (plug_device)
+	spin_unlock_irqrestore(&hwif->lock, flags);
+out_early:
+	if (plug_device) {
+		ide_unlock_host(hwif->host);
 		ide_plug_device(drive);
+	}
 
 	return irq_ret;
 }
@@ -1189,15 +1177,13 @@ out:
 
 void ide_do_drive_cmd(ide_drive_t *drive, struct request *rq)
 {
-	ide_hwgroup_t *hwgroup = drive->hwif->hwgroup;
 	struct request_queue *q = drive->queue;
 	unsigned long flags;
 
-	hwgroup->rq = NULL;
+	drive->hwif->rq = NULL;
 
 	spin_lock_irqsave(q->queue_lock, flags);
 	__elv_add_request(q, rq, ELEVATOR_INSERT_FRONT, 0);
-	blk_start_queueing(q);
 	spin_unlock_irqrestore(q->queue_lock, flags);
 }
 EXPORT_SYMBOL(ide_do_drive_cmd);
diff --git a/drivers/ide/ide-iops.c b/drivers/ide/ide-iops.c
index ad8bd653928..e728cfe7273 100644
--- a/drivers/ide/ide-iops.c
+++ b/drivers/ide/ide-iops.c
@@ -105,15 +105,6 @@ u8 ide_read_altstatus(ide_hwif_t *hwif)
 }
 EXPORT_SYMBOL_GPL(ide_read_altstatus);
 
-u8 ide_read_sff_dma_status(ide_hwif_t *hwif)
-{
-	if (hwif->host_flags & IDE_HFLAG_MMIO)
-		return readb((void __iomem *)(hwif->dma_base + ATA_DMA_STATUS));
-	else
-		return inb(hwif->dma_base + ATA_DMA_STATUS);
-}
-EXPORT_SYMBOL_GPL(ide_read_sff_dma_status);
-
 void ide_set_irq(ide_hwif_t *hwif, int on)
 {
 	u8 ctl = ATA_DEVCTL_OBS;
@@ -388,7 +379,6 @@ const struct ide_tp_ops default_tp_ops = {
 	.exec_command		= ide_exec_command,
 	.read_status		= ide_read_status,
 	.read_altstatus		= ide_read_altstatus,
-	.read_sff_dma_status	= ide_read_sff_dma_status,
 
 	.set_irq		= ide_set_irq,
 
@@ -451,7 +441,7 @@ EXPORT_SYMBOL(ide_fixstring);
  */
 int drive_is_ready (ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	u8 stat			= 0;
 
 	if (drive->waiting_for_dma)
@@ -503,7 +493,8 @@ static int __ide_wait_stat(ide_drive_t *drive, u8 good, u8 bad, unsigned long ti
 	stat = tp_ops->read_status(hwif);
 
 	if (stat & ATA_BUSY) {
-		local_irq_set(flags);
+		local_irq_save(flags);
+		local_irq_enable_in_hardirq();
 		timeout += jiffies;
 		while ((stat = tp_ops->read_status(hwif)) & ATA_BUSY) {
 			if (time_after(jiffies, timeout)) {
@@ -822,25 +813,25 @@ int ide_config_drive_speed(ide_drive_t *drive, u8 speed)
 static void __ide_set_handler (ide_drive_t *drive, ide_handler_t *handler,
 		      unsigned int timeout, ide_expiry_t *expiry)
 {
-	ide_hwgroup_t *hwgroup = HWGROUP(drive);
-
-	BUG_ON(hwgroup->handler);
-	hwgroup->handler	= handler;
-	hwgroup->expiry		= expiry;
-	hwgroup->timer.expires	= jiffies + timeout;
-	hwgroup->req_gen_timer	= hwgroup->req_gen;
-	add_timer(&hwgroup->timer);
+	ide_hwif_t *hwif = drive->hwif;
+
+	BUG_ON(hwif->handler);
+	hwif->handler		= handler;
+	hwif->expiry		= expiry;
+	hwif->timer.expires	= jiffies + timeout;
+	hwif->req_gen_timer	= hwif->req_gen;
+	add_timer(&hwif->timer);
 }
 
 void ide_set_handler (ide_drive_t *drive, ide_handler_t *handler,
 		      unsigned int timeout, ide_expiry_t *expiry)
 {
-	ide_hwgroup_t *hwgroup = drive->hwif->hwgroup;
+	ide_hwif_t *hwif = drive->hwif;
 	unsigned long flags;
 
-	spin_lock_irqsave(&hwgroup->lock, flags);
+	spin_lock_irqsave(&hwif->lock, flags);
 	__ide_set_handler(drive, handler, timeout, expiry);
-	spin_unlock_irqrestore(&hwgroup->lock, flags);
+	spin_unlock_irqrestore(&hwif->lock, flags);
 }
 
 EXPORT_SYMBOL(ide_set_handler);
@@ -863,10 +854,9 @@ void ide_execute_command(ide_drive_t *drive, u8 cmd, ide_handler_t *handler,
 			 unsigned timeout, ide_expiry_t *expiry)
 {
 	ide_hwif_t *hwif = drive->hwif;
-	ide_hwgroup_t *hwgroup = hwif->hwgroup;
 	unsigned long flags;
 
-	spin_lock_irqsave(&hwgroup->lock, flags);
+	spin_lock_irqsave(&hwif->lock, flags);
 	__ide_set_handler(drive, handler, timeout, expiry);
 	hwif->tp_ops->exec_command(hwif, cmd);
 	/*
@@ -876,26 +866,25 @@ void ide_execute_command(ide_drive_t *drive, u8 cmd, ide_handler_t *handler,
 	 * FIXME: we could skip this delay with care on non shared devices
 	 */
 	ndelay(400);
-	spin_unlock_irqrestore(&hwgroup->lock, flags);
+	spin_unlock_irqrestore(&hwif->lock, flags);
 }
 EXPORT_SYMBOL(ide_execute_command);
 
 void ide_execute_pkt_cmd(ide_drive_t *drive)
 {
 	ide_hwif_t *hwif = drive->hwif;
-	ide_hwgroup_t *hwgroup = hwif->hwgroup;
 	unsigned long flags;
 
-	spin_lock_irqsave(&hwgroup->lock, flags);
+	spin_lock_irqsave(&hwif->lock, flags);
 	hwif->tp_ops->exec_command(hwif, ATA_CMD_PACKET);
 	ndelay(400);
-	spin_unlock_irqrestore(&hwgroup->lock, flags);
+	spin_unlock_irqrestore(&hwif->lock, flags);
 }
 EXPORT_SYMBOL_GPL(ide_execute_pkt_cmd);
 
 static inline void ide_complete_drive_reset(ide_drive_t *drive, int err)
 {
-	struct request *rq = drive->hwif->hwgroup->rq;
+	struct request *rq = drive->hwif->rq;
 
 	if (rq && blk_special_request(rq) && rq->cmd[0] == REQ_DRIVE_RESET)
 		ide_end_request(drive, err ? err : 1, 0);
@@ -913,7 +902,6 @@ static ide_startstop_t do_reset1 (ide_drive_t *, int);
 static ide_startstop_t atapi_reset_pollfunc (ide_drive_t *drive)
 {
 	ide_hwif_t *hwif = drive->hwif;
-	ide_hwgroup_t *hwgroup = hwif->hwgroup;
 	u8 stat;
 
 	SELECT_DRIVE(drive);
@@ -923,20 +911,20 @@ static ide_startstop_t atapi_reset_pollfunc (ide_drive_t *drive)
 	if (OK_STAT(stat, 0, ATA_BUSY))
 		printk("%s: ATAPI reset complete\n", drive->name);
 	else {
-		if (time_before(jiffies, hwgroup->poll_timeout)) {
+		if (time_before(jiffies, hwif->poll_timeout)) {
 			ide_set_handler(drive, &atapi_reset_pollfunc, HZ/20, NULL);
 			/* continue polling */
 			return ide_started;
 		}
 		/* end of polling */
-		hwgroup->polling = 0;
+		hwif->polling = 0;
 		printk("%s: ATAPI reset timed-out, status=0x%02x\n",
 				drive->name, stat);
 		/* do it the old fashioned way */
 		return do_reset1(drive, 1);
 	}
 	/* done polling */
-	hwgroup->polling = 0;
+	hwif->polling = 0;
 	ide_complete_drive_reset(drive, 0);
 	return ide_stopped;
 }
@@ -968,8 +956,7 @@ static void ide_reset_report_error(ide_hwif_t *hwif, u8 err)
  */
 static ide_startstop_t reset_pollfunc (ide_drive_t *drive)
 {
-	ide_hwgroup_t *hwgroup	= HWGROUP(drive);
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	const struct ide_port_ops *port_ops = hwif->port_ops;
 	u8 tmp;
 	int err = 0;
@@ -986,7 +973,7 @@ static ide_startstop_t reset_pollfunc (ide_drive_t *drive)
 	tmp = hwif->tp_ops->read_status(hwif);
 
 	if (!OK_STAT(tmp, 0, ATA_BUSY)) {
-		if (time_before(jiffies, hwgroup->poll_timeout)) {
+		if (time_before(jiffies, hwif->poll_timeout)) {
 			ide_set_handler(drive, &reset_pollfunc, HZ/20, NULL);
 			/* continue polling */
 			return ide_started;
@@ -1007,7 +994,7 @@ static ide_startstop_t reset_pollfunc (ide_drive_t *drive)
 		}
 	}
 out:
-	hwgroup->polling = 0;	/* done polling */
+	hwif->polling = 0;	/* done polling */
 	ide_complete_drive_reset(drive, err);
 	return ide_stopped;
 }
@@ -1081,18 +1068,18 @@ static void pre_reset(ide_drive_t *drive)
 static ide_startstop_t do_reset1 (ide_drive_t *drive, int do_not_try_atapi)
 {
 	ide_hwif_t *hwif = drive->hwif;
-	ide_hwgroup_t *hwgroup = hwif->hwgroup;
 	struct ide_io_ports *io_ports = &hwif->io_ports;
 	const struct ide_tp_ops *tp_ops = hwif->tp_ops;
 	const struct ide_port_ops *port_ops;
+	ide_drive_t *tdrive;
 	unsigned long flags, timeout;
-	unsigned int unit;
+	int i;
 	DEFINE_WAIT(wait);
 
-	spin_lock_irqsave(&hwgroup->lock, flags);
+	spin_lock_irqsave(&hwif->lock, flags);
 
 	/* We must not reset with running handlers */
-	BUG_ON(hwgroup->handler != NULL);
+	BUG_ON(hwif->handler != NULL);
 
 	/* For an ATAPI device, first try an ATAPI SRST. */
 	if (drive->media != ide_disk && !do_not_try_atapi) {
@@ -1101,10 +1088,10 @@ static ide_startstop_t do_reset1 (ide_drive_t *drive, int do_not_try_atapi)
 		udelay (20);
 		tp_ops->exec_command(hwif, ATA_CMD_DEV_RESET);
 		ndelay(400);
-		hwgroup->poll_timeout = jiffies + WAIT_WORSTCASE;
-		hwgroup->polling = 1;
+		hwif->poll_timeout = jiffies + WAIT_WORSTCASE;
+		hwif->polling = 1;
 		__ide_set_handler(drive, &atapi_reset_pollfunc, HZ/20, NULL);
-		spin_unlock_irqrestore(&hwgroup->lock, flags);
+		spin_unlock_irqrestore(&hwif->lock, flags);
 		return ide_started;
 	}
 
@@ -1114,9 +1101,7 @@ static ide_startstop_t do_reset1 (ide_drive_t *drive, int do_not_try_atapi)
 
 		prepare_to_wait(&ide_park_wq, &wait, TASK_UNINTERRUPTIBLE);
 		timeout = jiffies;
-		for (unit = 0; unit < MAX_DRIVES; unit++) {
-			ide_drive_t *tdrive = &hwif->drives[unit];
-
+		ide_port_for_each_dev(i, tdrive, hwif) {
 			if (tdrive->dev_flags & IDE_DFLAG_PRESENT &&
 			    tdrive->dev_flags & IDE_DFLAG_PARKED &&
 			    time_after(tdrive->sleep, timeout))
@@ -1127,9 +1112,9 @@ static ide_startstop_t do_reset1 (ide_drive_t *drive, int do_not_try_atapi)
 		if (time_before_eq(timeout, now))
 			break;
 
-		spin_unlock_irqrestore(&hwgroup->lock, flags);
+		spin_unlock_irqrestore(&hwif->lock, flags);
 		timeout = schedule_timeout_uninterruptible(timeout - now);
-		spin_lock_irqsave(&hwgroup->lock, flags);
+		spin_lock_irqsave(&hwif->lock, flags);
 	} while (timeout);
 	finish_wait(&ide_park_wq, &wait);
 
@@ -1137,11 +1122,11 @@ static ide_startstop_t do_reset1 (ide_drive_t *drive, int do_not_try_atapi)
 	 * First, reset any device state data we were maintaining
 	 * for any of the drives on this interface.
 	 */
-	for (unit = 0; unit < MAX_DRIVES; ++unit)
-		pre_reset(&hwif->drives[unit]);
+	ide_port_for_each_dev(i, tdrive, hwif)
+		pre_reset(tdrive);
 
 	if (io_ports->ctl_addr == 0) {
-		spin_unlock_irqrestore(&hwgroup->lock, flags);
+		spin_unlock_irqrestore(&hwif->lock, flags);
 		ide_complete_drive_reset(drive, -ENXIO);
 		return ide_stopped;
 	}
@@ -1164,8 +1149,8 @@ static ide_startstop_t do_reset1 (ide_drive_t *drive, int do_not_try_atapi)
 	tp_ops->set_irq(hwif, drive->quirk_list == 2);
 	/* more than enough time */
 	udelay(10);
-	hwgroup->poll_timeout = jiffies + WAIT_WORSTCASE;
-	hwgroup->polling = 1;
+	hwif->poll_timeout = jiffies + WAIT_WORSTCASE;
+	hwif->polling = 1;
 	__ide_set_handler(drive, &reset_pollfunc, HZ/20, NULL);
 
 	/*
@@ -1177,7 +1162,7 @@ static ide_startstop_t do_reset1 (ide_drive_t *drive, int do_not_try_atapi)
 	if (port_ops && port_ops->resetproc)
 		port_ops->resetproc(drive);
 
-	spin_unlock_irqrestore(&hwgroup->lock, flags);
+	spin_unlock_irqrestore(&hwif->lock, flags);
 	return ide_started;
 }
 
@@ -1221,6 +1206,3 @@ int ide_wait_not_busy(ide_hwif_t *hwif, unsigned long timeout)
 	}
 	return -EBUSY;
 }
-
-EXPORT_SYMBOL_GPL(ide_wait_not_busy);
-
diff --git a/drivers/ide/ide-lib.c b/drivers/ide/ide-lib.c
index 9f6e33d8a8b..09526a0de73 100644
--- a/drivers/ide/ide-lib.c
+++ b/drivers/ide/ide-lib.c
@@ -273,7 +273,7 @@ int ide_set_xfer_rate(ide_drive_t *drive, u8 rate)
 
 static void ide_dump_opcode(ide_drive_t *drive)
 {
-	struct request *rq = drive->hwif->hwgroup->rq;
+	struct request *rq = drive->hwif->rq;
 	ide_task_t *task = NULL;
 
 	if (!rq)
@@ -346,10 +346,13 @@ static void ide_dump_ata_error(ide_drive_t *drive, u8 err)
 	printk(KERN_CONT "}");
 	if ((err & (ATA_BBK | ATA_ABORTED)) == ATA_BBK ||
 	    (err & (ATA_UNC | ATA_IDNF | ATA_AMNF))) {
+		struct request *rq = drive->hwif->rq;
+
 		ide_dump_sector(drive);
-		if (HWGROUP(drive) && HWGROUP(drive)->rq)
+
+		if (rq)
 			printk(KERN_CONT ", sector=%llu",
-			       (unsigned long long)HWGROUP(drive)->rq->sector);
+			       (unsigned long long)rq->sector);
 	}
 	printk(KERN_CONT "\n");
 }
diff --git a/drivers/ide/ide-park.c b/drivers/ide/ide-park.c
index 678454ac248..c875a957596 100644
--- a/drivers/ide/ide-park.c
+++ b/drivers/ide/ide-park.c
@@ -7,22 +7,22 @@ DECLARE_WAIT_QUEUE_HEAD(ide_park_wq);
 
 static void issue_park_cmd(ide_drive_t *drive, unsigned long timeout)
 {
-	ide_hwgroup_t *hwgroup = drive->hwif->hwgroup;
+	ide_hwif_t *hwif = drive->hwif;
 	struct request_queue *q = drive->queue;
 	struct request *rq;
 	int rc;
 
 	timeout += jiffies;
-	spin_lock_irq(&hwgroup->lock);
+	spin_lock_irq(&hwif->lock);
 	if (drive->dev_flags & IDE_DFLAG_PARKED) {
 		int reset_timer = time_before(timeout, drive->sleep);
 		int start_queue = 0;
 
 		drive->sleep = timeout;
 		wake_up_all(&ide_park_wq);
-		if (reset_timer && del_timer(&hwgroup->timer))
+		if (reset_timer && del_timer(&hwif->timer))
 			start_queue = 1;
-		spin_unlock_irq(&hwgroup->lock);
+		spin_unlock_irq(&hwif->lock);
 
 		if (start_queue) {
 			spin_lock_irq(q->queue_lock);
@@ -31,7 +31,7 @@ static void issue_park_cmd(ide_drive_t *drive, unsigned long timeout)
 		}
 		return;
 	}
-	spin_unlock_irq(&hwgroup->lock);
+	spin_unlock_irq(&hwif->lock);
 
 	rq = blk_get_request(q, READ, __GFP_WAIT);
 	rq->cmd[0] = REQ_PARK_HEADS;
@@ -64,21 +64,21 @@ ssize_t ide_park_show(struct device *dev, struct device_attribute *attr,
 		      char *buf)
 {
 	ide_drive_t *drive = to_ide_device(dev);
-	ide_hwgroup_t *hwgroup = drive->hwif->hwgroup;
+	ide_hwif_t *hwif = drive->hwif;
 	unsigned long now;
 	unsigned int msecs;
 
 	if (drive->dev_flags & IDE_DFLAG_NO_UNLOAD)
 		return -EOPNOTSUPP;
 
-	spin_lock_irq(&hwgroup->lock);
+	spin_lock_irq(&hwif->lock);
 	now = jiffies;
 	if (drive->dev_flags & IDE_DFLAG_PARKED &&
 	    time_after(drive->sleep, now))
 		msecs = jiffies_to_msecs(drive->sleep - now);
 	else
 		msecs = 0;
-	spin_unlock_irq(&hwgroup->lock);
+	spin_unlock_irq(&hwif->lock);
 
 	return snprintf(buf, 20, "%u\n", msecs);
 }
diff --git a/drivers/ide/ide-pm.c b/drivers/ide/ide-pm.c
index 8282c6086e6..4b3bf6a06b7 100644
--- a/drivers/ide/ide-pm.c
+++ b/drivers/ide/ide-pm.c
@@ -5,7 +5,7 @@
 int generic_ide_suspend(struct device *dev, pm_message_t mesg)
 {
 	ide_drive_t *drive = dev->driver_data, *pair = ide_get_pair_dev(drive);
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct request *rq;
 	struct request_pm_state rqpm;
 	ide_task_t args;
@@ -39,7 +39,7 @@ int generic_ide_suspend(struct device *dev, pm_message_t mesg)
 int generic_ide_resume(struct device *dev)
 {
 	ide_drive_t *drive = dev->driver_data, *pair = ide_get_pair_dev(drive);
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct request *rq;
 	struct request_pm_state rqpm;
 	ide_task_t args;
@@ -67,7 +67,7 @@ int generic_ide_resume(struct device *dev)
 	blk_put_request(rq);
 
 	if (err == 0 && dev->driver) {
-		ide_driver_t *drv = to_ide_driver(dev->driver);
+		struct ide_driver *drv = to_ide_driver(dev->driver);
 
 		if (drv->resume)
 			drv->resume(drive);
@@ -194,7 +194,7 @@ void ide_complete_pm_request(ide_drive_t *drive, struct request *rq)
 	}
 	spin_unlock_irqrestore(q->queue_lock, flags);
 
-	drive->hwif->hwgroup->rq = NULL;
+	drive->hwif->rq = NULL;
 
 	if (blk_end_request(rq, 0, 0))
 		BUG();
diff --git a/drivers/ide/ide-probe.c b/drivers/ide/ide-probe.c
index c5adb7b9c5b..0ccbb4459fb 100644
--- a/drivers/ide/ide-probe.c
+++ b/drivers/ide/ide-probe.c
@@ -189,7 +189,7 @@ static void ide_classify_atapi_dev(ide_drive_t *drive)
 
 static void do_identify(ide_drive_t *drive, u8 cmd)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	u16 *id = drive->id;
 	char *m = (char *)&id[ATA_ID_PROD];
 	unsigned long flags;
@@ -266,7 +266,7 @@ err_misc:
 
 static int actual_try_to_identify (ide_drive_t *drive, u8 cmd)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct ide_io_ports *io_ports = &hwif->io_ports;
 	const struct ide_tp_ops *tp_ops = hwif->tp_ops;
 	int use_altstatus = 0, rc;
@@ -341,7 +341,7 @@ static int actual_try_to_identify (ide_drive_t *drive, u8 cmd)
  
 static int try_to_identify (ide_drive_t *drive, u8 cmd)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	const struct ide_tp_ops *tp_ops = hwif->tp_ops;
 	int retval;
 	int autoprobe = 0;
@@ -438,7 +438,7 @@ static u8 ide_read_device(ide_drive_t *drive)
 
 static int do_probe (ide_drive_t *drive, u8 cmd)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	const struct ide_tp_ops *tp_ops = hwif->tp_ops;
 	int rc;
 	u8 present = !!(drive->dev_flags & IDE_DFLAG_PRESENT), stat;
@@ -463,7 +463,7 @@ static int do_probe (ide_drive_t *drive, u8 cmd)
 	if (ide_read_device(drive) != drive->select && present == 0) {
 		if (drive->dn & 1) {
 			/* exit with drive0 selected */
-			SELECT_DRIVE(&hwif->drives[0]);
+			SELECT_DRIVE(hwif->devices[0]);
 			/* allow ATA_BUSY to assert & clear */
 			msleep(50);
 		}
@@ -509,7 +509,7 @@ static int do_probe (ide_drive_t *drive, u8 cmd)
 	}
 	if (drive->dn & 1) {
 		/* exit with drive0 selected */
-		SELECT_DRIVE(&hwif->drives[0]);
+		SELECT_DRIVE(hwif->devices[0]);
 		msleep(50);
 		/* ensure drive irq is clear */
 		(void)tp_ops->read_status(hwif);
@@ -522,7 +522,7 @@ static int do_probe (ide_drive_t *drive, u8 cmd)
  */
 static void enable_nest (ide_drive_t *drive)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	const struct ide_tp_ops *tp_ops = hwif->tp_ops;
 	u8 stat;
 
@@ -697,7 +697,8 @@ out:
 
 static int ide_port_wait_ready(ide_hwif_t *hwif)
 {
-	int unit, rc;
+	ide_drive_t *drive;
+	int i, rc;
 
 	printk(KERN_DEBUG "Probing IDE interface %s...\n", hwif->name);
 
@@ -714,9 +715,7 @@ static int ide_port_wait_ready(ide_hwif_t *hwif)
 		return rc;
 
 	/* Now make sure both master & slave are ready */
-	for (unit = 0; unit < MAX_DRIVES; unit++) {
-		ide_drive_t *drive = &hwif->drives[unit];
-
+	ide_port_for_each_dev(i, drive, hwif) {
 		/* Ignore disks that we will not probe for later. */
 		if ((drive->dev_flags & IDE_DFLAG_NOPROBE) == 0 ||
 		    (drive->dev_flags & IDE_DFLAG_PRESENT)) {
@@ -732,8 +731,8 @@ static int ide_port_wait_ready(ide_hwif_t *hwif)
 	}
 out:
 	/* Exit function with master reselected (let's be sane) */
-	if (unit)
-		SELECT_DRIVE(&hwif->drives[0]);
+	if (i)
+		SELECT_DRIVE(hwif->devices[0]);
 
 	return rc;
 }
@@ -749,7 +748,7 @@ out:
 
 void ide_undecoded_slave(ide_drive_t *dev1)
 {
-	ide_drive_t *dev0 = &dev1->hwif->drives[0];
+	ide_drive_t *dev0 = dev1->hwif->devices[0];
 
 	if ((dev1->dn & 1) == 0 || (dev0->dev_flags & IDE_DFLAG_PRESENT) == 0)
 		return;
@@ -778,14 +777,15 @@ EXPORT_SYMBOL_GPL(ide_undecoded_slave);
 
 static int ide_probe_port(ide_hwif_t *hwif)
 {
+	ide_drive_t *drive;
 	unsigned long flags;
 	unsigned int irqd;
-	int unit, rc = -ENODEV;
+	int i, rc = -ENODEV;
 
 	BUG_ON(hwif->present);
 
-	if ((hwif->drives[0].dev_flags & IDE_DFLAG_NOPROBE) &&
-	    (hwif->drives[1].dev_flags & IDE_DFLAG_NOPROBE))
+	if ((hwif->devices[0]->dev_flags & IDE_DFLAG_NOPROBE) &&
+	    (hwif->devices[1]->dev_flags & IDE_DFLAG_NOPROBE))
 		return -EACCES;
 
 	/*
@@ -796,7 +796,8 @@ static int ide_probe_port(ide_hwif_t *hwif)
 	if (irqd)
 		disable_irq(hwif->irq);
 
-	local_irq_set(flags);
+	local_irq_save(flags);
+	local_irq_enable_in_hardirq();
 
 	if (ide_port_wait_ready(hwif) == -EBUSY)
 		printk(KERN_DEBUG "%s: Wait for ready failed before probe !\n", hwif->name);
@@ -805,9 +806,7 @@ static int ide_probe_port(ide_hwif_t *hwif)
 	 * Second drive should only exist if first drive was found,
 	 * but a lot of cdrom drives are configured as single slaves.
 	 */
-	for (unit = 0; unit < MAX_DRIVES; ++unit) {
-		ide_drive_t *drive = &hwif->drives[unit];
-
+	ide_port_for_each_dev(i, drive, hwif) {
 		(void) probe_for_drive(drive);
 		if (drive->dev_flags & IDE_DFLAG_PRESENT)
 			rc = 0;
@@ -828,20 +827,17 @@ static int ide_probe_port(ide_hwif_t *hwif)
 static void ide_port_tune_devices(ide_hwif_t *hwif)
 {
 	const struct ide_port_ops *port_ops = hwif->port_ops;
-	int unit;
-
-	for (unit = 0; unit < MAX_DRIVES; unit++) {
-		ide_drive_t *drive = &hwif->drives[unit];
+	ide_drive_t *drive;
+	int i;
 
+	ide_port_for_each_dev(i, drive, hwif) {
 		if (drive->dev_flags & IDE_DFLAG_PRESENT) {
 			if (port_ops && port_ops->quirkproc)
 				port_ops->quirkproc(drive);
 		}
 	}
 
-	for (unit = 0; unit < MAX_DRIVES; ++unit) {
-		ide_drive_t *drive = &hwif->drives[unit];
-
+	ide_port_for_each_dev(i, drive, hwif) {
 		if (drive->dev_flags & IDE_DFLAG_PRESENT) {
 			ide_set_max_pio(drive);
 
@@ -852,11 +848,8 @@ static void ide_port_tune_devices(ide_hwif_t *hwif)
 		}
 	}
 
-	for (unit = 0; unit < MAX_DRIVES; ++unit) {
-		ide_drive_t *drive = &hwif->drives[unit];
-
-		if ((hwif->host_flags & IDE_HFLAG_NO_IO_32BIT) ||
-		    drive->id[ATA_ID_DWORD_IO])
+	ide_port_for_each_dev(i, drive, hwif) {
+		if (hwif->host_flags & IDE_HFLAG_NO_IO_32BIT)
 			drive->dev_flags |= IDE_DFLAG_NO_IO_32BIT;
 		else
 			drive->dev_flags &= ~IDE_DFLAG_NO_IO_32BIT;
@@ -869,7 +862,7 @@ static void ide_port_tune_devices(ide_hwif_t *hwif)
 static int ide_init_queue(ide_drive_t *drive)
 {
 	struct request_queue *q;
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	int max_sectors = 256;
 	int max_sg_entries = PRD_ENTRIES;
 
@@ -918,36 +911,19 @@ static int ide_init_queue(ide_drive_t *drive)
 	return 0;
 }
 
-static void ide_add_drive_to_hwgroup(ide_drive_t *drive)
-{
-	ide_hwgroup_t *hwgroup = drive->hwif->hwgroup;
-
-	spin_lock_irq(&hwgroup->lock);
-	if (!hwgroup->drive) {
-		/* first drive for hwgroup. */
-		drive->next = drive;
-		hwgroup->drive = drive;
-		hwgroup->hwif = HWIF(hwgroup->drive);
-	} else {
-		drive->next = hwgroup->drive->next;
-		hwgroup->drive->next = drive;
-	}
-	spin_unlock_irq(&hwgroup->lock);
-}
+static DEFINE_MUTEX(ide_cfg_mtx);
 
 /*
  * For any present drive:
  * - allocate the block device queue
- * - link drive into the hwgroup
  */
 static int ide_port_setup_devices(ide_hwif_t *hwif)
 {
+	ide_drive_t *drive;
 	int i, j = 0;
 
 	mutex_lock(&ide_cfg_mtx);
-	for (i = 0; i < MAX_DRIVES; i++) {
-		ide_drive_t *drive = &hwif->drives[i];
-
+	ide_port_for_each_dev(i, drive, hwif) {
 		if ((drive->dev_flags & IDE_DFLAG_PRESENT) == 0)
 			continue;
 
@@ -961,139 +937,39 @@ static int ide_port_setup_devices(ide_hwif_t *hwif)
 		}
 
 		j++;
-
-		ide_add_drive_to_hwgroup(drive);
 	}
 	mutex_unlock(&ide_cfg_mtx);
 
 	return j;
 }
 
-static ide_hwif_t *ide_ports[MAX_HWIFS];
-
-void ide_remove_port_from_hwgroup(ide_hwif_t *hwif)
-{
-	ide_hwgroup_t *hwgroup = hwif->hwgroup;
-
-	ide_ports[hwif->index] = NULL;
-
-	spin_lock_irq(&hwgroup->lock);
-	/*
-	 * Remove us from the hwgroup, and free
-	 * the hwgroup if we were the only member
-	 */
-	if (hwif->next == hwif) {
-		BUG_ON(hwgroup->hwif != hwif);
-		kfree(hwgroup);
-	} else {
-		/* There is another interface in hwgroup.
-		 * Unlink us, and set hwgroup->drive and ->hwif to
-		 * something sane.
-		 */
-		ide_hwif_t *g = hwgroup->hwif;
-
-		while (g->next != hwif)
-			g = g->next;
-		g->next = hwif->next;
-		if (hwgroup->hwif == hwif) {
-			/* Chose a random hwif for hwgroup->hwif.
-			 * It's guaranteed that there are no drives
-			 * left in the hwgroup.
-			 */
-			BUG_ON(hwgroup->drive != NULL);
-			hwgroup->hwif = g;
-		}
-		BUG_ON(hwgroup->hwif == hwif);
-	}
-	spin_unlock_irq(&hwgroup->lock);
-}
-
 /*
- * This routine sets up the irq for an ide interface, and creates a new
- * hwgroup for the irq/hwif if none was previously assigned.
- *
- * Much of the code is for correctly detecting/handling irq sharing
- * and irq serialization situations.  This is somewhat complex because
- * it handles static as well as dynamic (PCMCIA) IDE interfaces.
+ * This routine sets up the IRQ for an IDE interface.
  */
 static int init_irq (ide_hwif_t *hwif)
 {
 	struct ide_io_ports *io_ports = &hwif->io_ports;
-	unsigned int index;
-	ide_hwgroup_t *hwgroup;
-	ide_hwif_t *match = NULL;
+	int sa = 0;
 
 	mutex_lock(&ide_cfg_mtx);
-	hwif->hwgroup = NULL;
+	spin_lock_init(&hwif->lock);
 
-	for (index = 0; index < MAX_HWIFS; index++) {
-		ide_hwif_t *h = ide_ports[index];
+	init_timer(&hwif->timer);
+	hwif->timer.function = &ide_timer_expiry;
+	hwif->timer.data = (unsigned long)hwif;
 
-		if (h && h->hwgroup) {  /* scan only initialized ports */
-			if (hwif->host->host_flags & IDE_HFLAG_SERIALIZE) {
-				if (hwif->host == h->host)
-					match = h;
-			}
-		}
-	}
-
-	/*
-	 * If we are still without a hwgroup, then form a new one
-	 */
-	if (match) {
-		hwgroup = match->hwgroup;
-		hwif->hwgroup = hwgroup;
-		/*
-		 * Link us into the hwgroup.
-		 * This must be done early, do ensure that unexpected_intr
-		 * can find the hwif and prevent irq storms.
-		 * No drives are attached to the new hwif, choose_drive
-		 * can't do anything stupid (yet).
-		 * Add ourself as the 2nd entry to the hwgroup->hwif
-		 * linked list, the first entry is the hwif that owns
-		 * hwgroup->handler - do not change that.
-		 */
-		spin_lock_irq(&hwgroup->lock);
-		hwif->next = hwgroup->hwif->next;
-		hwgroup->hwif->next = hwif;
-		BUG_ON(hwif->next == hwif);
-		spin_unlock_irq(&hwgroup->lock);
-	} else {
-		hwgroup = kmalloc_node(sizeof(*hwgroup), GFP_KERNEL|__GFP_ZERO,
-				       hwif_to_node(hwif));
-		if (hwgroup == NULL)
-			goto out_up;
-
-		spin_lock_init(&hwgroup->lock);
-
-		hwif->hwgroup = hwgroup;
-		hwgroup->hwif = hwif->next = hwif;
-
-		init_timer(&hwgroup->timer);
-		hwgroup->timer.function = &ide_timer_expiry;
-		hwgroup->timer.data = (unsigned long) hwgroup;
-	}
-
-	ide_ports[hwif->index] = hwif;
-
-	/*
-	 * Allocate the irq, if not already obtained for another hwif
-	 */
-	if (!match || match->irq != hwif->irq) {
-		int sa = 0;
 #if defined(__mc68000__)
-		sa = IRQF_SHARED;
+	sa = IRQF_SHARED;
 #endif /* __mc68000__ */
 
-		if (hwif->chipset == ide_pci)
-			sa = IRQF_SHARED;
+	if (hwif->chipset == ide_pci)
+		sa = IRQF_SHARED;
 
-		if (io_ports->ctl_addr)
-			hwif->tp_ops->set_irq(hwif, 1);
+	if (io_ports->ctl_addr)
+		hwif->tp_ops->set_irq(hwif, 1);
 
-		if (request_irq(hwif->irq,&ide_intr,sa,hwif->name,hwgroup))
-	       		goto out_unlink;
-	}
+	if (request_irq(hwif->irq, &ide_intr, sa, hwif->name, hwif))
+		goto out_up;
 
 	if (!hwif->rqsize) {
 		if ((hwif->host_flags & IDE_HFLAG_NO_LBA48) ||
@@ -1111,14 +987,12 @@ static int init_irq (ide_hwif_t *hwif)
 	printk(KERN_INFO "%s at 0x%08lx on irq %d", hwif->name,
 		io_ports->data_addr, hwif->irq);
 #endif /* __mc68000__ */
-	if (match)
-		printk(KERN_CONT " (serialized with %s)", match->name);
+	if (hwif->host->host_flags & IDE_HFLAG_SERIALIZE)
+		printk(KERN_CONT " (serialized)");
 	printk(KERN_CONT "\n");
 
 	mutex_unlock(&ide_cfg_mtx);
 	return 0;
-out_unlink:
-	ide_remove_port_from_hwgroup(hwif);
 out_up:
 	mutex_unlock(&ide_cfg_mtx);
 	return 1;
@@ -1134,7 +1008,7 @@ static struct kobject *ata_probe(dev_t dev, int *part, void *data)
 {
 	ide_hwif_t *hwif = data;
 	int unit = *part >> PARTN_BITS;
-	ide_drive_t *drive = &hwif->drives[unit];
+	ide_drive_t *drive = hwif->devices[unit];
 
 	if ((drive->dev_flags & IDE_DFLAG_PRESENT) == 0)
 		return NULL;
@@ -1196,47 +1070,23 @@ void ide_init_disk(struct gendisk *disk, ide_drive_t *drive)
 
 EXPORT_SYMBOL_GPL(ide_init_disk);
 
-static void ide_remove_drive_from_hwgroup(ide_drive_t *drive)
-{
-	ide_hwgroup_t *hwgroup = drive->hwif->hwgroup;
-
-	if (drive == drive->next) {
-		/* special case: last drive from hwgroup. */
-		BUG_ON(hwgroup->drive != drive);
-		hwgroup->drive = NULL;
-	} else {
-		ide_drive_t *walk;
-
-		walk = hwgroup->drive;
-		while (walk->next != drive)
-			walk = walk->next;
-		walk->next = drive->next;
-		if (hwgroup->drive == drive) {
-			hwgroup->drive = drive->next;
-			hwgroup->hwif = hwgroup->drive->hwif;
-		}
-	}
-	BUG_ON(hwgroup->drive == drive);
-}
-
 static void drive_release_dev (struct device *dev)
 {
 	ide_drive_t *drive = container_of(dev, ide_drive_t, gendev);
-	ide_hwgroup_t *hwgroup = drive->hwif->hwgroup;
+	ide_hwif_t *hwif = drive->hwif;
 
 	ide_proc_unregister_device(drive);
 
-	spin_lock_irq(&hwgroup->lock);
-	ide_remove_drive_from_hwgroup(drive);
+	spin_lock_irq(&hwif->lock);
 	kfree(drive->id);
 	drive->id = NULL;
 	drive->dev_flags &= ~IDE_DFLAG_PRESENT;
 	/* Messed up locking ... */
-	spin_unlock_irq(&hwgroup->lock);
+	spin_unlock_irq(&hwif->lock);
 	blk_cleanup_queue(drive->queue);
-	spin_lock_irq(&hwgroup->lock);
+	spin_lock_irq(&hwif->lock);
 	drive->queue = NULL;
-	spin_unlock_irq(&hwgroup->lock);
+	spin_unlock_irq(&hwif->lock);
 
 	complete(&drive->gendev_rel_comp);
 }
@@ -1302,10 +1152,10 @@ out:
 
 static void hwif_register_devices(ide_hwif_t *hwif)
 {
+	ide_drive_t *drive;
 	unsigned int i;
 
-	for (i = 0; i < MAX_DRIVES; i++) {
-		ide_drive_t *drive = &hwif->drives[i];
+	ide_port_for_each_dev(i, drive, hwif) {
 		struct device *dev = &drive->gendev;
 		int ret;
 
@@ -1328,11 +1178,10 @@ static void hwif_register_devices(ide_hwif_t *hwif)
 static void ide_port_init_devices(ide_hwif_t *hwif)
 {
 	const struct ide_port_ops *port_ops = hwif->port_ops;
+	ide_drive_t *drive;
 	int i;
 
-	for (i = 0; i < MAX_DRIVES; i++) {
-		ide_drive_t *drive = &hwif->drives[i];
-
+	ide_port_for_each_dev(i, drive, hwif) {
 		drive->dn = i + hwif->channel * 2;
 
 		if (hwif->host_flags & IDE_HFLAG_IO_32BIT)
@@ -1380,6 +1229,8 @@ static void ide_init_port(ide_hwif_t *hwif, unsigned int port,
 	if ((d->host_flags & IDE_HFLAG_NO_DMA) == 0) {
 		int rc;
 
+		hwif->dma_ops = d->dma_ops;
+
 		if (d->init_dma)
 			rc = d->init_dma(hwif, d);
 		else
@@ -1387,12 +1238,13 @@ static void ide_init_port(ide_hwif_t *hwif, unsigned int port,
 
 		if (rc < 0) {
 			printk(KERN_INFO "%s: DMA disabled\n", hwif->name);
+
+			hwif->dma_ops = NULL;
 			hwif->dma_base = 0;
 			hwif->swdma_mask = 0;
 			hwif->mwdma_mask = 0;
 			hwif->ultra_mask = 0;
-		} else if (d->dma_ops)
-			hwif->dma_ops = d->dma_ops;
+		}
 	}
 
 	if ((d->host_flags & IDE_HFLAG_SERIALIZE) ||
@@ -1417,6 +1269,66 @@ static void ide_port_cable_detect(ide_hwif_t *hwif)
 	}
 }
 
+static const u8 ide_hwif_to_major[] =
+	{ IDE0_MAJOR, IDE1_MAJOR, IDE2_MAJOR, IDE3_MAJOR, IDE4_MAJOR,
+	  IDE5_MAJOR, IDE6_MAJOR, IDE7_MAJOR, IDE8_MAJOR, IDE9_MAJOR };
+
+static void ide_port_init_devices_data(ide_hwif_t *hwif)
+{
+	ide_drive_t *drive;
+	int i;
+
+	ide_port_for_each_dev(i, drive, hwif) {
+		u8 j = (hwif->index * MAX_DRIVES) + i;
+
+		memset(drive, 0, sizeof(*drive));
+
+		drive->media			= ide_disk;
+		drive->select			= (i << 4) | ATA_DEVICE_OBS;
+		drive->hwif			= hwif;
+		drive->ready_stat		= ATA_DRDY;
+		drive->bad_wstat		= BAD_W_STAT;
+		drive->special.b.recalibrate	= 1;
+		drive->special.b.set_geometry	= 1;
+		drive->name[0]			= 'h';
+		drive->name[1]			= 'd';
+		drive->name[2]			= 'a' + j;
+		drive->max_failures		= IDE_DEFAULT_MAX_FAILURES;
+
+		INIT_LIST_HEAD(&drive->list);
+		init_completion(&drive->gendev_rel_comp);
+	}
+}
+
+static void ide_init_port_data(ide_hwif_t *hwif, unsigned int index)
+{
+	/* fill in any non-zero initial values */
+	hwif->index	= index;
+	hwif->major	= ide_hwif_to_major[index];
+
+	hwif->name[0]	= 'i';
+	hwif->name[1]	= 'd';
+	hwif->name[2]	= 'e';
+	hwif->name[3]	= '0' + index;
+
+	init_completion(&hwif->gendev_rel_comp);
+
+	hwif->tp_ops = &default_tp_ops;
+
+	ide_port_init_devices_data(hwif);
+}
+
+static void ide_init_port_hw(ide_hwif_t *hwif, hw_regs_t *hw)
+{
+	memcpy(&hwif->io_ports, &hw->io_ports, sizeof(hwif->io_ports));
+	hwif->irq = hw->irq;
+	hwif->chipset = hw->chipset;
+	hwif->dev = hw->dev;
+	hwif->gendev.parent = hw->parent ? hw->parent : hw->dev;
+	hwif->ack_intr = hw->ack_intr;
+	hwif->config_data = hw->config;
+}
+
 static unsigned int ide_indexes;
 
 /**
@@ -1466,12 +1378,43 @@ static void ide_free_port_slot(int idx)
 	mutex_unlock(&ide_cfg_mtx);
 }
 
+static void ide_port_free_devices(ide_hwif_t *hwif)
+{
+	ide_drive_t *drive;
+	int i;
+
+	ide_port_for_each_dev(i, drive, hwif)
+		kfree(drive);
+}
+
+static int ide_port_alloc_devices(ide_hwif_t *hwif, int node)
+{
+	int i;
+
+	for (i = 0; i < MAX_DRIVES; i++) {
+		ide_drive_t *drive;
+
+		drive = kzalloc_node(sizeof(*drive), GFP_KERNEL, node);
+		if (drive == NULL)
+			goto out_nomem;
+
+		hwif->devices[i] = drive;
+	}
+	return 0;
+
+out_nomem:
+	ide_port_free_devices(hwif);
+	return -ENOMEM;
+}
+
 struct ide_host *ide_host_alloc(const struct ide_port_info *d, hw_regs_t **hws)
 {
 	struct ide_host *host;
+	struct device *dev = hws[0] ? hws[0]->dev : NULL;
+	int node = dev ? dev_to_node(dev) : -1;
 	int i;
 
-	host = kzalloc(sizeof(*host), GFP_KERNEL);
+	host = kzalloc_node(sizeof(*host), GFP_KERNEL, node);
 	if (host == NULL)
 		return NULL;
 
@@ -1482,10 +1425,15 @@ struct ide_host *ide_host_alloc(const struct ide_port_info *d, hw_regs_t **hws)
 		if (hws[i] == NULL)
 			continue;
 
-		hwif = kzalloc(sizeof(*hwif), GFP_KERNEL);
+		hwif = kzalloc_node(sizeof(*hwif), GFP_KERNEL, node);
 		if (hwif == NULL)
 			continue;
 
+		if (ide_port_alloc_devices(hwif, node) < 0) {
+			kfree(hwif);
+			continue;
+		}
+
 		idx = ide_find_port_slot(d);
 		if (idx < 0) {
 			printk(KERN_ERR "%s: no free slot for interface\n",
@@ -1507,8 +1455,7 @@ struct ide_host *ide_host_alloc(const struct ide_port_info *d, hw_regs_t **hws)
 		return NULL;
 	}
 
-	if (hws[0])
-		host->dev[0] = hws[0]->dev;
+	host->dev[0] = dev;
 
 	if (d) {
 		host->init_chipset = d->init_chipset;
@@ -1525,9 +1472,7 @@ int ide_host_register(struct ide_host *host, const struct ide_port_info *d,
 	ide_hwif_t *hwif, *mate = NULL;
 	int i, j = 0;
 
-	for (i = 0; i < MAX_HOST_PORTS; i++) {
-		hwif = host->ports[i];
-
+	ide_host_for_each_port(i, hwif, host) {
 		if (hwif == NULL) {
 			mate = NULL;
 			continue;
@@ -1553,9 +1498,7 @@ int ide_host_register(struct ide_host *host, const struct ide_port_info *d,
 		ide_port_init_devices(hwif);
 	}
 
-	for (i = 0; i < MAX_HOST_PORTS; i++) {
-		hwif = host->ports[i];
-
+	ide_host_for_each_port(i, hwif, host) {
 		if (hwif == NULL)
 			continue;
 
@@ -1570,9 +1513,7 @@ int ide_host_register(struct ide_host *host, const struct ide_port_info *d,
 			ide_port_tune_devices(hwif);
 	}
 
-	for (i = 0; i < MAX_HOST_PORTS; i++) {
-		hwif = host->ports[i];
-
+	ide_host_for_each_port(i, hwif, host) {
 		if (hwif == NULL)
 			continue;
 
@@ -1597,9 +1538,7 @@ int ide_host_register(struct ide_host *host, const struct ide_port_info *d,
 			ide_acpi_port_init_devices(hwif);
 	}
 
-	for (i = 0; i < MAX_HOST_PORTS; i++) {
-		hwif = host->ports[i];
-
+	ide_host_for_each_port(i, hwif, host) {
 		if (hwif == NULL)
 			continue;
 
@@ -1607,9 +1546,7 @@ int ide_host_register(struct ide_host *host, const struct ide_port_info *d,
 			hwif_register_devices(hwif);
 	}
 
-	for (i = 0; i < MAX_HOST_PORTS; i++) {
-		hwif = host->ports[i];
-
+	ide_host_for_each_port(i, hwif, host) {
 		if (hwif == NULL)
 			continue;
 
@@ -1647,17 +1584,85 @@ int ide_host_add(const struct ide_port_info *d, hw_regs_t **hws,
 }
 EXPORT_SYMBOL_GPL(ide_host_add);
 
+static void __ide_port_unregister_devices(ide_hwif_t *hwif)
+{
+	ide_drive_t *drive;
+	int i;
+
+	ide_port_for_each_dev(i, drive, hwif) {
+		if (drive->dev_flags & IDE_DFLAG_PRESENT) {
+			device_unregister(&drive->gendev);
+			wait_for_completion(&drive->gendev_rel_comp);
+		}
+	}
+}
+
+void ide_port_unregister_devices(ide_hwif_t *hwif)
+{
+	mutex_lock(&ide_cfg_mtx);
+	__ide_port_unregister_devices(hwif);
+	hwif->present = 0;
+	ide_port_init_devices_data(hwif);
+	mutex_unlock(&ide_cfg_mtx);
+}
+EXPORT_SYMBOL_GPL(ide_port_unregister_devices);
+
+/**
+ *	ide_unregister		-	free an IDE interface
+ *	@hwif: IDE interface
+ *
+ *	Perform the final unregister of an IDE interface.
+ *
+ *	Locking:
+ *	The caller must not hold the IDE locks.
+ *
+ *	It is up to the caller to be sure there is no pending I/O here,
+ *	and that the interface will not be reopened (present/vanishing
+ *	locking isn't yet done BTW).
+ */
+
+static void ide_unregister(ide_hwif_t *hwif)
+{
+	BUG_ON(in_interrupt());
+	BUG_ON(irqs_disabled());
+
+	mutex_lock(&ide_cfg_mtx);
+
+	if (hwif->present) {
+		__ide_port_unregister_devices(hwif);
+		hwif->present = 0;
+	}
+
+	ide_proc_unregister_port(hwif);
+
+	free_irq(hwif->irq, hwif);
+
+	device_unregister(hwif->portdev);
+	device_unregister(&hwif->gendev);
+	wait_for_completion(&hwif->gendev_rel_comp);
+
+	/*
+	 * Remove us from the kernel's knowledge
+	 */
+	blk_unregister_region(MKDEV(hwif->major, 0), MAX_DRIVES<<PARTN_BITS);
+	kfree(hwif->sg_table);
+	unregister_blkdev(hwif->major, hwif->name);
+
+	ide_release_dma_engine(hwif);
+
+	mutex_unlock(&ide_cfg_mtx);
+}
+
 void ide_host_free(struct ide_host *host)
 {
 	ide_hwif_t *hwif;
 	int i;
 
-	for (i = 0; i < MAX_HOST_PORTS; i++) {
-		hwif = host->ports[i];
-
+	ide_host_for_each_port(i, hwif, host) {
 		if (hwif == NULL)
 			continue;
 
+		ide_port_free_devices(hwif);
 		ide_free_port_slot(hwif->index);
 		kfree(hwif);
 	}
@@ -1668,11 +1673,12 @@ EXPORT_SYMBOL_GPL(ide_host_free);
 
 void ide_host_remove(struct ide_host *host)
 {
+	ide_hwif_t *hwif;
 	int i;
 
-	for (i = 0; i < MAX_HOST_PORTS; i++) {
-		if (host->ports[i])
-			ide_unregister(host->ports[i]);
+	ide_host_for_each_port(i, hwif, host) {
+		if (hwif)
+			ide_unregister(hwif);
 	}
 
 	ide_host_free(host);
@@ -1691,8 +1697,8 @@ void ide_port_scan(ide_hwif_t *hwif)
 	hwif->present = 1;
 
 	ide_port_tune_devices(hwif);
-	ide_acpi_port_init_devices(hwif);
 	ide_port_setup_devices(hwif);
+	ide_acpi_port_init_devices(hwif);
 	hwif_register_devices(hwif);
 	ide_proc_port_register_devices(hwif);
 }
diff --git a/drivers/ide/ide-proc.c b/drivers/ide/ide-proc.c
index a14e2938e4f..1d8978b3314 100644
--- a/drivers/ide/ide-proc.c
+++ b/drivers/ide/ide-proc.c
@@ -439,13 +439,13 @@ static int proc_ide_read_dmodel
 static int proc_ide_read_driver
 	(char *page, char **start, off_t off, int count, int *eof, void *data)
 {
-	ide_drive_t	*drive = (ide_drive_t *) data;
-	struct device	*dev = &drive->gendev;
-	ide_driver_t	*ide_drv;
-	int		len;
+	ide_drive_t		*drive = (ide_drive_t *)data;
+	struct device		*dev = &drive->gendev;
+	struct ide_driver	*ide_drv;
+	int			len;
 
 	if (dev->driver) {
-		ide_drv = container_of(dev->driver, ide_driver_t, gen_driver);
+		ide_drv = to_ide_driver(dev->driver);
 		len = sprintf(page, "%s version %s\n",
 				dev->driver->name, ide_drv->version);
 	} else
@@ -555,7 +555,7 @@ static void ide_remove_proc_entries(struct proc_dir_entry *dir, ide_proc_entry_t
 	}
 }
 
-void ide_proc_register_driver(ide_drive_t *drive, ide_driver_t *driver)
+void ide_proc_register_driver(ide_drive_t *drive, struct ide_driver *driver)
 {
 	mutex_lock(&ide_setting_mtx);
 	drive->settings = driver->proc_devsets(drive);
@@ -577,7 +577,7 @@ EXPORT_SYMBOL(ide_proc_register_driver);
  *	Takes ide_setting_mtx.
  */
 
-void ide_proc_unregister_driver(ide_drive_t *drive, ide_driver_t *driver)
+void ide_proc_unregister_driver(ide_drive_t *drive, struct ide_driver *driver)
 {
 	ide_remove_proc_entries(drive->proc, driver->proc_entries(drive));
 
@@ -593,14 +593,13 @@ EXPORT_SYMBOL(ide_proc_unregister_driver);
 
 void ide_proc_port_register_devices(ide_hwif_t *hwif)
 {
-	int	d;
 	struct proc_dir_entry *ent;
 	struct proc_dir_entry *parent = hwif->proc;
+	ide_drive_t *drive;
 	char name[64];
+	int i;
 
-	for (d = 0; d < MAX_DRIVES; d++) {
-		ide_drive_t *drive = &hwif->drives[d];
-
+	ide_port_for_each_dev(i, drive, hwif) {
 		if ((drive->dev_flags & IDE_DFLAG_PRESENT) == 0 || drive->proc)
 			continue;
 
@@ -653,7 +652,7 @@ void ide_proc_unregister_port(ide_hwif_t *hwif)
 
 static int proc_print_driver(struct device_driver *drv, void *data)
 {
-	ide_driver_t *ide_drv = container_of(drv, ide_driver_t, gen_driver);
+	struct ide_driver *ide_drv = to_ide_driver(drv);
 	struct seq_file *s = data;
 
 	seq_printf(s, "%s version %s\n", drv->name, ide_drv->version);
diff --git a/drivers/ide/ide-tape.c b/drivers/ide/ide-tape.c
index 5d2aa22cd6e..d7ecd3c7975 100644
--- a/drivers/ide/ide-tape.c
+++ b/drivers/ide/ide-tape.c
@@ -166,10 +166,10 @@ struct idetape_bh {
  * to an interrupt or a timer event is stored in the struct defined below.
  */
 typedef struct ide_tape_obj {
-	ide_drive_t	*drive;
-	ide_driver_t	*driver;
-	struct gendisk	*disk;
-	struct kref	kref;
+	ide_drive_t		*drive;
+	struct ide_driver	*driver;
+	struct gendisk		*disk;
+	struct kref		kref;
 
 	/*
 	 *	failed_pc points to the last failed packet command, or contains
@@ -479,7 +479,7 @@ static void ide_tape_kfree_buffer(idetape_tape_t *tape)
 
 static int idetape_end_request(ide_drive_t *drive, int uptodate, int nr_sects)
 {
-	struct request *rq = HWGROUP(drive)->rq;
+	struct request *rq = drive->hwif->rq;
 	idetape_tape_t *tape = drive->driver_data;
 	unsigned long flags;
 	int error;
@@ -531,7 +531,7 @@ static void ide_tape_callback(ide_drive_t *drive, int dsc)
 			printk(KERN_ERR "ide-tape: Error in REQUEST SENSE "
 					"itself - Aborting request!\n");
 	} else if (pc->c[0] == READ_6 || pc->c[0] == WRITE_6) {
-		struct request *rq = drive->hwif->hwgroup->rq;
+		struct request *rq = drive->hwif->rq;
 		int blocks = pc->xferred / tape->blk_size;
 
 		tape->avg_size += blocks * tape->blk_size;
@@ -576,7 +576,7 @@ static void ide_tape_callback(ide_drive_t *drive, int dsc)
 
 /*
  * Postpone the current request so that ide.c will be able to service requests
- * from another device on the same hwgroup while we are polling for DSC.
+ * from another device on the same port while we are polling for DSC.
  */
 static void idetape_postpone_request(ide_drive_t *drive)
 {
@@ -584,7 +584,8 @@ static void idetape_postpone_request(ide_drive_t *drive)
 
 	debug_log(DBG_PROCS, "Enter %s\n", __func__);
 
-	tape->postponed_rq = HWGROUP(drive)->rq;
+	tape->postponed_rq = drive->hwif->rq;
+
 	ide_stall_queue(drive, tape->dsc_poll_freq);
 }
 
@@ -2312,7 +2313,7 @@ static const struct ide_proc_devset *ide_tape_proc_devsets(ide_drive_t *drive)
 
 static int ide_tape_probe(ide_drive_t *);
 
-static ide_driver_t idetape_driver = {
+static struct ide_driver idetape_driver = {
 	.gen_driver = {
 		.owner		= THIS_MODULE,
 		.name		= "ide-tape",
@@ -2323,7 +2324,6 @@ static ide_driver_t idetape_driver = {
 	.version		= IDETAPE_VERSION,
 	.do_request		= idetape_do_request,
 	.end_request		= idetape_end_request,
-	.error			= __ide_error,
 #ifdef CONFIG_IDE_PROC_FS
 	.proc_entries		= ide_tape_proc_entries,
 	.proc_devsets		= ide_tape_proc_devsets,
diff --git a/drivers/ide/ide-taskfile.c b/drivers/ide/ide-taskfile.c
index bf4fb9d8d17..16138bce84a 100644
--- a/drivers/ide/ide-taskfile.c
+++ b/drivers/ide/ide-taskfile.c
@@ -58,7 +58,7 @@ static ide_startstop_t task_in_intr(ide_drive_t *);
 
 ide_startstop_t do_rw_taskfile (ide_drive_t *drive, ide_task_t *task)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct ide_taskfile *tf = &task->tf;
 	ide_handler_t *handler = NULL;
 	const struct ide_tp_ops *tp_ops = hwif->tp_ops;
@@ -309,9 +309,9 @@ static ide_startstop_t task_error(ide_drive_t *drive, struct request *rq,
 		}
 
 		if (sectors > 0) {
-			ide_driver_t *drv;
+			struct ide_driver *drv;
 
-			drv = *(ide_driver_t **)rq->rq_disk->private_data;
+			drv = *(struct ide_driver **)rq->rq_disk->private_data;
 			drv->end_request(drive, 1, sectors);
 		}
 	}
@@ -328,9 +328,9 @@ void task_end_request(ide_drive_t *drive, struct request *rq, u8 stat)
 	}
 
 	if (rq->rq_disk) {
-		ide_driver_t *drv;
+		struct ide_driver *drv;
 
-		drv = *(ide_driver_t **)rq->rq_disk->private_data;;
+		drv = *(struct ide_driver **)rq->rq_disk->private_data;;
 		drv->end_request(drive, 1, rq->nr_sectors);
 	} else
 		ide_end_request(drive, 1, rq->nr_sectors);
@@ -361,7 +361,7 @@ static ide_startstop_t task_in_unexpected(ide_drive_t *drive, struct request *rq
 static ide_startstop_t task_in_intr(ide_drive_t *drive)
 {
 	ide_hwif_t *hwif = drive->hwif;
-	struct request *rq = hwif->hwgroup->rq;
+	struct request *rq = hwif->rq;
 	u8 stat = hwif->tp_ops->read_status(hwif);
 
 	/* Error? */
@@ -395,7 +395,7 @@ static ide_startstop_t task_in_intr(ide_drive_t *drive)
 static ide_startstop_t task_out_intr (ide_drive_t *drive)
 {
 	ide_hwif_t *hwif = drive->hwif;
-	struct request *rq = HWGROUP(drive)->rq;
+	struct request *rq = hwif->rq;
 	u8 stat = hwif->tp_ops->read_status(hwif);
 
 	if (!OK_STAT(stat, DRIVE_READY, drive->bad_wstat))
diff --git a/drivers/ide/ide.c b/drivers/ide/ide.c
index 46a2d4ca812..258805da15c 100644
--- a/drivers/ide/ide.c
+++ b/drivers/ide/ide.c
@@ -60,179 +60,8 @@
 #include <linux/completion.h>
 #include <linux/device.h>
 
-
-/* default maximum number of failures */
-#define IDE_DEFAULT_MAX_FAILURES 	1
-
 struct class *ide_port_class;
 
-static const u8 ide_hwif_to_major[] = { IDE0_MAJOR, IDE1_MAJOR,
-					IDE2_MAJOR, IDE3_MAJOR,
-					IDE4_MAJOR, IDE5_MAJOR,
-					IDE6_MAJOR, IDE7_MAJOR,
-					IDE8_MAJOR, IDE9_MAJOR };
-
-DEFINE_MUTEX(ide_cfg_mtx);
-
-static void ide_port_init_devices_data(ide_hwif_t *);
-
-/*
- * Do not even *think* about calling this!
- */
-void ide_init_port_data(ide_hwif_t *hwif, unsigned int index)
-{
-	/* bulk initialize hwif & drive info with zeros */
-	memset(hwif, 0, sizeof(ide_hwif_t));
-
-	/* fill in any non-zero initial values */
-	hwif->index	= index;
-	hwif->major	= ide_hwif_to_major[index];
-
-	hwif->name[0]	= 'i';
-	hwif->name[1]	= 'd';
-	hwif->name[2]	= 'e';
-	hwif->name[3]	= '0' + index;
-
-	init_completion(&hwif->gendev_rel_comp);
-
-	hwif->tp_ops = &default_tp_ops;
-
-	ide_port_init_devices_data(hwif);
-}
-
-static void ide_port_init_devices_data(ide_hwif_t *hwif)
-{
-	int unit;
-
-	for (unit = 0; unit < MAX_DRIVES; ++unit) {
-		ide_drive_t *drive = &hwif->drives[unit];
-		u8 j = (hwif->index * MAX_DRIVES) + unit;
-
-		memset(drive, 0, sizeof(*drive));
-
-		drive->media			= ide_disk;
-		drive->select			= (unit << 4) | ATA_DEVICE_OBS;
-		drive->hwif			= hwif;
-		drive->ready_stat		= ATA_DRDY;
-		drive->bad_wstat		= BAD_W_STAT;
-		drive->special.b.recalibrate	= 1;
-		drive->special.b.set_geometry	= 1;
-		drive->name[0]			= 'h';
-		drive->name[1]			= 'd';
-		drive->name[2]			= 'a' + j;
-		drive->max_failures		= IDE_DEFAULT_MAX_FAILURES;
-
-		INIT_LIST_HEAD(&drive->list);
-		init_completion(&drive->gendev_rel_comp);
-	}
-}
-
-static void __ide_port_unregister_devices(ide_hwif_t *hwif)
-{
-	int i;
-
-	for (i = 0; i < MAX_DRIVES; i++) {
-		ide_drive_t *drive = &hwif->drives[i];
-
-		if (drive->dev_flags & IDE_DFLAG_PRESENT) {
-			device_unregister(&drive->gendev);
-			wait_for_completion(&drive->gendev_rel_comp);
-		}
-	}
-}
-
-void ide_port_unregister_devices(ide_hwif_t *hwif)
-{
-	mutex_lock(&ide_cfg_mtx);
-	__ide_port_unregister_devices(hwif);
-	hwif->present = 0;
-	ide_port_init_devices_data(hwif);
-	mutex_unlock(&ide_cfg_mtx);
-}
-EXPORT_SYMBOL_GPL(ide_port_unregister_devices);
-
-/**
- *	ide_unregister		-	free an IDE interface
- *	@hwif: IDE interface
- *
- *	Perform the final unregister of an IDE interface. At the moment
- *	we don't refcount interfaces so this will also get split up.
- *
- *	Locking:
- *	The caller must not hold the IDE locks
- *	The drive present/vanishing is not yet properly locked
- *	Take care with the callbacks. These have been split to avoid
- *	deadlocking the IDE layer. The shutdown callback is called
- *	before we take the lock and free resources. It is up to the
- *	caller to be sure there is no pending I/O here, and that
- *	the interface will not be reopened (present/vanishing locking
- *	isn't yet done BTW). After we commit to the final kill we
- *	call the cleanup callback with the ide locks held.
- *
- *	Unregister restores the hwif structures to the default state.
- *	This is raving bonkers.
- */
-
-void ide_unregister(ide_hwif_t *hwif)
-{
-	ide_hwif_t *g;
-	ide_hwgroup_t *hwgroup;
-	int irq_count = 0;
-
-	BUG_ON(in_interrupt());
-	BUG_ON(irqs_disabled());
-
-	mutex_lock(&ide_cfg_mtx);
-
-	if (hwif->present) {
-		__ide_port_unregister_devices(hwif);
-		hwif->present = 0;
-	}
-
-	ide_proc_unregister_port(hwif);
-
-	hwgroup = hwif->hwgroup;
-	/*
-	 * free the irq if we were the only hwif using it
-	 */
-	g = hwgroup->hwif;
-	do {
-		if (g->irq == hwif->irq)
-			++irq_count;
-		g = g->next;
-	} while (g != hwgroup->hwif);
-	if (irq_count == 1)
-		free_irq(hwif->irq, hwgroup);
-
-	ide_remove_port_from_hwgroup(hwif);
-
-	device_unregister(hwif->portdev);
-	device_unregister(&hwif->gendev);
-	wait_for_completion(&hwif->gendev_rel_comp);
-
-	/*
-	 * Remove us from the kernel's knowledge
-	 */
-	blk_unregister_region(MKDEV(hwif->major, 0), MAX_DRIVES<<PARTN_BITS);
-	kfree(hwif->sg_table);
-	unregister_blkdev(hwif->major, hwif->name);
-
-	ide_release_dma_engine(hwif);
-
-	mutex_unlock(&ide_cfg_mtx);
-}
-
-void ide_init_port_hw(ide_hwif_t *hwif, hw_regs_t *hw)
-{
-	memcpy(&hwif->io_ports, &hw->io_ports, sizeof(hwif->io_ports));
-	hwif->irq = hw->irq;
-	hwif->chipset = hw->chipset;
-	hwif->dev = hw->dev;
-	hwif->gendev.parent = hw->parent ? hw->parent : hw->dev;
-	hwif->ack_intr = hw->ack_intr;
-	hwif->config_data = hw->config;
-}
-
 /*
  *	Locks for IDE setting functionality
  */
@@ -330,7 +159,6 @@ static int set_pio_mode_abuse(ide_hwif_t *hwif, u8 req_pio)
 static int set_pio_mode(ide_drive_t *drive, int arg)
 {
 	ide_hwif_t *hwif = drive->hwif;
-	ide_hwgroup_t *hwgroup = hwif->hwgroup;
 	const struct ide_port_ops *port_ops = hwif->port_ops;
 
 	if (arg < 0 || arg > 255)
@@ -345,9 +173,9 @@ static int set_pio_mode(ide_drive_t *drive, int arg)
 			unsigned long flags;
 
 			/* take lock for IDE_DFLAG_[NO_]UNMASK/[NO_]IO_32BIT */
-			spin_lock_irqsave(&hwgroup->lock, flags);
+			spin_lock_irqsave(&hwif->lock, flags);
 			port_ops->set_pio_mode(drive, arg);
-			spin_unlock_irqrestore(&hwgroup->lock, flags);
+			spin_unlock_irqrestore(&hwif->lock, flags);
 		} else
 			port_ops->set_pio_mode(drive, arg);
 	} else {
@@ -453,7 +281,7 @@ static int ide_uevent(struct device *dev, struct kobj_uevent_env *env)
 static int generic_ide_probe(struct device *dev)
 {
 	ide_drive_t *drive = to_ide_device(dev);
-	ide_driver_t *drv = to_ide_driver(dev->driver);
+	struct ide_driver *drv = to_ide_driver(dev->driver);
 
 	return drv->probe ? drv->probe(drive) : -ENODEV;
 }
@@ -461,7 +289,7 @@ static int generic_ide_probe(struct device *dev)
 static int generic_ide_remove(struct device *dev)
 {
 	ide_drive_t *drive = to_ide_device(dev);
-	ide_driver_t *drv = to_ide_driver(dev->driver);
+	struct ide_driver *drv = to_ide_driver(dev->driver);
 
 	if (drv->remove)
 		drv->remove(drive);
@@ -472,7 +300,7 @@ static int generic_ide_remove(struct device *dev)
 static void generic_ide_shutdown(struct device *dev)
 {
 	ide_drive_t *drive = to_ide_device(dev);
-	ide_driver_t *drv = to_ide_driver(dev->driver);
+	struct ide_driver *drv = to_ide_driver(dev->driver);
 
 	if (dev->driver && drv->shutdown)
 		drv->shutdown(drive);
@@ -660,6 +488,7 @@ MODULE_PARM_DESC(ignore_cable, "ignore cable detection");
 
 void ide_port_apply_params(ide_hwif_t *hwif)
 {
+	ide_drive_t *drive;
 	int i;
 
 	if (ide_ignore_cable & (1 << hwif->index)) {
@@ -668,8 +497,8 @@ void ide_port_apply_params(ide_hwif_t *hwif)
 		hwif->cbl = ATA_CBL_PATA40_SHORT;
 	}
 
-	for (i = 0; i < MAX_DRIVES; i++)
-		ide_dev_apply_params(&hwif->drives[i], i);
+	ide_port_for_each_dev(i, drive, hwif)
+		ide_dev_apply_params(drive, i);
 }
 
 /*
diff --git a/drivers/ide/it8172.c b/drivers/ide/it8172.c
new file mode 100644
index 00000000000..e021078cd06
--- /dev/null
+++ b/drivers/ide/it8172.c
@@ -0,0 +1,166 @@
+/*
+ *
+ * BRIEF MODULE DESCRIPTION
+ *      IT8172 IDE controller support
+ *
+ * Copyright (C) 2000 MontaVista Software Inc.
+ * Copyright (C) 2008 Shane McDonald
+ *
+ *  This program is free software; you can redistribute  it and/or modify it
+ *  under  the terms of  the GNU General  Public License as published by the
+ *  Free Software Foundation;  either version 2 of the  License, or (at your
+ *  option) any later version.
+ *
+ *  THIS  SOFTWARE  IS PROVIDED   ``AS  IS'' AND   ANY  EXPRESS OR IMPLIED
+ *  WARRANTIES,   INCLUDING, BUT NOT  LIMITED  TO, THE IMPLIED WARRANTIES OF
+ *  MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.  IN
+ *  NO  EVENT  SHALL   THE AUTHOR  BE    LIABLE FOR ANY   DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ *  NOT LIMITED   TO, PROCUREMENT OF  SUBSTITUTE GOODS  OR SERVICES; LOSS OF
+ *  USE, DATA,  OR PROFITS; OR  BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ *  ANY THEORY OF LIABILITY, WHETHER IN  CONTRACT, STRICT LIABILITY, OR TORT
+ *  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ *  THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *  You should have received a copy of the  GNU General Public License along
+ *  with this program; if not, write  to the Free Software Foundation, Inc.,
+ *  675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/ioport.h>
+#include <linux/pci.h>
+#include <linux/ide.h>
+#include <linux/init.h>
+
+#define DRV_NAME "IT8172"
+
+static void it8172_set_pio_mode(ide_drive_t *drive, const u8 pio)
+{
+	ide_hwif_t *hwif	= drive->hwif;
+	struct pci_dev *dev	= to_pci_dev(hwif->dev);
+	u16 drive_enables;
+	u32 drive_timing;
+
+	/*
+	 * The highest value of DIOR/DIOW pulse width and recovery time
+	 * that can be set in the IT8172 is 8 PCI clock cycles.  As a result,
+	 * it cannot be configured for PIO mode 0.  This table sets these
+	 * parameters to the maximum supported by the IT8172.
+	 */
+	static const u8 timings[] = { 0x3f, 0x3c, 0x1b, 0x12, 0x0a };
+
+	pci_read_config_word(dev, 0x40, &drive_enables);
+	pci_read_config_dword(dev, 0x44, &drive_timing);
+
+	/*
+	 * Enable port 0x44. The IT8172 spec is confused; it calls
+	 * this register the "Slave IDE Timing Register", but in fact,
+	 * it controls timing for both master and slave drives.
+	 */
+	drive_enables |= 0x4000;
+
+	drive_enables &= drive->dn ? 0xc006 : 0xc060;
+	if (drive->media == ide_disk)
+		/* enable prefetch */
+		drive_enables |= 0x0004 << (drive->dn * 4);
+	if (ata_id_has_iordy(drive->id))
+		/* enable IORDY sample-point */
+		drive_enables |= 0x0002 << (drive->dn * 4);
+
+	drive_timing &= drive->dn ? 0x00003f00 : 0x000fc000;
+	drive_timing |= timings[pio] << (drive->dn * 6 + 8);
+
+	pci_write_config_word(dev, 0x40, drive_enables);
+	pci_write_config_dword(dev, 0x44, drive_timing);
+}
+
+static void it8172_set_dma_mode(ide_drive_t *drive, const u8 speed)
+{
+	ide_hwif_t *hwif	= drive->hwif;
+	struct pci_dev *dev	= to_pci_dev(hwif->dev);
+	int a_speed		= 3 << (drive->dn * 4);
+	int u_flag		= 1 << drive->dn;
+	int u_speed		= 0;
+	u8 reg48, reg4a;
+
+	pci_read_config_byte(dev, 0x48, &reg48);
+	pci_read_config_byte(dev, 0x4a, &reg4a);
+
+	if (speed >= XFER_UDMA_0) {
+		u8 udma = speed - XFER_UDMA_0;
+		u_speed = udma << (drive->dn * 4);
+
+		pci_write_config_byte(dev, 0x48, reg48 | u_flag);
+		reg4a &= ~a_speed;
+		pci_write_config_byte(dev, 0x4a, reg4a | u_speed);
+	} else {
+		const u8 mwdma_to_pio[] = { 0, 3, 4 };
+		u8 pio;
+
+		pci_write_config_byte(dev, 0x48, reg48 & ~u_flag);
+		pci_write_config_byte(dev, 0x4a, reg4a & ~a_speed);
+
+		pio = mwdma_to_pio[speed - XFER_MW_DMA_0];
+
+		it8172_set_pio_mode(drive, pio);
+	}
+}
+
+
+static const struct ide_port_ops it8172_port_ops = {
+	.set_pio_mode	= it8172_set_pio_mode,
+	.set_dma_mode	= it8172_set_dma_mode,
+};
+
+static const struct ide_port_info it8172_port_info __devinitdata = {
+	.name		= DRV_NAME,
+	.port_ops	= &it8172_port_ops,
+	.enablebits	= { {0x41, 0x80, 0x80}, {0x00, 0x00, 0x00} },
+	.host_flags	= IDE_HFLAG_SINGLE,
+	.pio_mask	= ATA_PIO4 & ~ATA_PIO0,
+	.mwdma_mask	= ATA_MWDMA2,
+	.udma_mask	= ATA_UDMA2,
+};
+
+static int __devinit it8172_init_one(struct pci_dev *dev,
+					const struct pci_device_id *id)
+{
+	if ((dev->class >> 8) != PCI_CLASS_STORAGE_IDE)
+		return -ENODEV; /* IT8172 is more than an IDE controller */
+	return ide_pci_init_one(dev, &it8172_port_info, NULL);
+}
+
+static struct pci_device_id it8172_pci_tbl[] = {
+	{ PCI_VDEVICE(ITE, PCI_DEVICE_ID_ITE_8172), 0 },
+	{ 0, },
+};
+MODULE_DEVICE_TABLE(pci, it8172_pci_tbl);
+
+static struct pci_driver it8172_pci_driver = {
+	.name		= "IT8172_IDE",
+	.id_table	= it8172_pci_tbl,
+	.probe		= it8172_init_one,
+	.remove		= ide_pci_remove,
+	.suspend	= ide_pci_suspend,
+	.resume		= ide_pci_resume,
+};
+
+static int __init it8172_ide_init(void)
+{
+	return ide_pci_register_driver(&it8172_pci_driver);
+}
+
+static void __exit it8172_ide_exit(void)
+{
+	pci_unregister_driver(&it8172_pci_driver);
+}
+
+module_init(it8172_ide_init);
+module_exit(it8172_ide_exit);
+
+MODULE_AUTHOR("Steve Longerbeam");
+MODULE_DESCRIPTION("PCI driver module for ITE 8172 IDE");
+MODULE_LICENSE("GPL");
diff --git a/drivers/ide/it8213.c b/drivers/ide/it8213.c
index 7c2feeb3c5e..d7969b6d139 100644
--- a/drivers/ide/it8213.c
+++ b/drivers/ide/it8213.c
@@ -25,7 +25,7 @@
 
 static void it8213_set_pio_mode(ide_drive_t *drive, const u8 pio)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	int is_slave		= drive->dn & 1;
 	int master_port		= 0x40;
@@ -82,7 +82,7 @@ static void it8213_set_pio_mode(ide_drive_t *drive, const u8 pio)
 
 static void it8213_set_dma_mode(ide_drive_t *drive, const u8 speed)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	u8 maslave		= 0x40;
 	int a_speed		= 3 << (drive->dn * 4);
diff --git a/drivers/ide/it821x.c b/drivers/ide/it821x.c
index ef004089761..0be27ac1f07 100644
--- a/drivers/ide/it821x.c
+++ b/drivers/ide/it821x.c
@@ -167,12 +167,10 @@ static void it821x_clock_strategy(ide_drive_t *drive)
 	ide_hwif_t *hwif = drive->hwif;
 	struct pci_dev *dev = to_pci_dev(hwif->dev);
 	struct it821x_dev *itdev = ide_get_hwifdata(hwif);
-	ide_drive_t *pair;
+	ide_drive_t *pair = ide_get_pair_dev(drive);
 	int clock, altclock, sel = 0;
 	u8 unit = drive->dn & 1, v;
 
-	pair = &hwif->drives[1 - unit];
-
 	if(itdev->want[0][0] > itdev->want[1][0]) {
 		clock = itdev->want[0][1];
 		altclock = itdev->want[1][1];
@@ -239,15 +237,13 @@ static void it821x_set_pio_mode(ide_drive_t *drive, const u8 pio)
 {
 	ide_hwif_t *hwif = drive->hwif;
 	struct it821x_dev *itdev = ide_get_hwifdata(hwif);
-	ide_drive_t *pair;
+	ide_drive_t *pair = ide_get_pair_dev(drive);
 	u8 unit = drive->dn & 1, set_pio = pio;
 
 	/* Spec says 89 ref driver uses 88 */
 	static u16 pio_timings[]= { 0xAA88, 0xA382, 0xA181, 0x3332, 0x3121 };
 	static u8 pio_want[]    = { ATA_66, ATA_66, ATA_66, ATA_66, ATA_ANY };
 
-	pair = &hwif->drives[1 - unit];
-
 	/*
 	 * Compute the best PIO mode we can for a given device. We must
 	 * pick a speed that does not cause problems with the other device
@@ -279,7 +275,7 @@ static void it821x_set_pio_mode(ide_drive_t *drive, const u8 pio)
  *	the shared MWDMA/PIO timing register.
  */
 
-static void it821x_tune_mwdma (ide_drive_t *drive, byte mode_wanted)
+static void it821x_tune_mwdma(ide_drive_t *drive, u8 mode_wanted)
 {
 	ide_hwif_t *hwif = drive->hwif;
 	struct pci_dev *dev = to_pci_dev(hwif->dev);
@@ -316,7 +312,7 @@ static void it821x_tune_mwdma (ide_drive_t *drive, byte mode_wanted)
  *	controller when doing UDMA modes in pass through.
  */
 
-static void it821x_tune_udma (ide_drive_t *drive, byte mode_wanted)
+static void it821x_tune_udma(ide_drive_t *drive, u8 mode_wanted)
 {
 	ide_hwif_t *hwif = drive->hwif;
 	struct pci_dev *dev = to_pci_dev(hwif->dev);
@@ -516,6 +512,7 @@ static struct ide_dma_ops it821x_pass_through_dma_ops = {
 	.dma_test_irq		= ide_dma_test_irq,
 	.dma_timeout		= ide_dma_timeout,
 	.dma_lost_irq		= ide_dma_lost_irq,
+	.dma_sff_read_status	= ide_dma_sff_read_status,
 };
 
 /**
diff --git a/drivers/ide/ns87415.c b/drivers/ide/ns87415.c
index 13789060f40..83643ed9a42 100644
--- a/drivers/ide/ns87415.c
+++ b/drivers/ide/ns87415.c
@@ -56,7 +56,7 @@ static u8 superio_read_status(ide_hwif_t *hwif)
 	return superio_ide_inb(hwif->io_ports.status_addr);
 }
 
-static u8 superio_read_sff_dma_status(ide_hwif_t *hwif)
+static u8 superio_dma_sff_read_status(ide_hwif_t *hwif)
 {
 	return superio_ide_inb(hwif->dma_base + ATA_DMA_STATUS);
 }
@@ -109,7 +109,6 @@ static const struct ide_tp_ops superio_tp_ops = {
 	.exec_command		= ide_exec_command,
 	.read_status		= superio_read_status,
 	.read_altstatus		= ide_read_altstatus,
-	.read_sff_dma_status	= superio_read_sff_dma_status,
 
 	.set_irq		= ide_set_irq,
 
@@ -132,18 +131,20 @@ static void __devinit superio_init_iops(struct hwif_s *hwif)
 	tmp = superio_ide_inb(dma_stat);
 	outb(tmp | 0x66, dma_stat);
 }
+#else
+#define superio_dma_sff_read_status ide_dma_sff_read_status
 #endif
 
 static unsigned int ns87415_count = 0, ns87415_control[MAX_HWIFS] = { 0 };
 
 /*
  * This routine either enables/disables (according to IDE_DFLAG_PRESENT)
- * the IRQ associated with the port (HWIF(drive)),
+ * the IRQ associated with the port,
  * and selects either PIO or DMA handshaking for the next I/O operation.
  */
 static void ns87415_prepare_drive (ide_drive_t *drive, unsigned int use_dma)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct pci_dev *dev = to_pci_dev(hwif->dev);
 	unsigned int bit, other, new, *old = (unsigned int *) hwif->select_data;
 	unsigned long flags;
@@ -197,11 +198,11 @@ static void ns87415_selectproc (ide_drive_t *drive)
 
 static int ns87415_dma_end(ide_drive_t *drive)
 {
-	ide_hwif_t      *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	u8 dma_stat = 0, dma_cmd = 0;
 
 	drive->waiting_for_dma = 0;
-	dma_stat = hwif->tp_ops->read_sff_dma_status(hwif);
+	dma_stat = hwif->dma_ops->dma_sff_read_status(hwif);
 	/* get DMA command mode */
 	dma_cmd = inb(hwif->dma_base + ATA_DMA_CMD);
 	/* stop DMA */
@@ -308,6 +309,7 @@ static const struct ide_dma_ops ns87415_dma_ops = {
 	.dma_test_irq		= ide_dma_test_irq,
 	.dma_lost_irq		= ide_dma_lost_irq,
 	.dma_timeout		= ide_dma_timeout,
+	.dma_sff_read_status	= superio_dma_sff_read_status,
 };
 
 static const struct ide_port_info ns87415_chipset __devinitdata = {
diff --git a/drivers/ide/palm_bk3710.c b/drivers/ide/palm_bk3710.c
index 122ed3c072f..a7ac490c9ae 100644
--- a/drivers/ide/palm_bk3710.c
+++ b/drivers/ide/palm_bk3710.c
@@ -324,8 +324,6 @@ static int __devinit palm_bk3710_init_dma(ide_hwif_t *hwif,
 
 	hwif->dma_base = hwif->io_ports.data_addr - IDE_PALM_ATA_PRI_REG_OFFSET;
 
-	hwif->dma_ops = &sff_dma_ops;
-
 	return 0;
 }
 
@@ -338,6 +336,7 @@ static const struct ide_port_ops palm_bk3710_ports_ops = {
 static struct ide_port_info __devinitdata palm_bk3710_port_info = {
 	.init_dma		= palm_bk3710_init_dma,
 	.port_ops		= &palm_bk3710_ports_ops,
+	.dma_ops		= &sff_dma_ops,
 	.host_flags		= IDE_HFLAG_MMIO,
 	.pio_mask		= ATA_PIO4,
 	.mwdma_mask		= ATA_MWDMA2,
diff --git a/drivers/ide/pdc202xx_new.c b/drivers/ide/pdc202xx_new.c
index 211ae46e3e0..f21290c4b44 100644
--- a/drivers/ide/pdc202xx_new.c
+++ b/drivers/ide/pdc202xx_new.c
@@ -143,7 +143,7 @@ static struct udma_timing {
 
 static void pdcnew_set_dma_mode(ide_drive_t *drive, const u8 speed)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	u8 adj			= (drive->dn & 1) ? 0x08 : 0x00;
 
@@ -219,7 +219,7 @@ static void pdcnew_reset(ide_drive_t *drive)
 	 * Deleted this because it is redundant from the caller.
 	 */
 	printk(KERN_WARNING "pdc202xx_new: %s channel reset.\n",
-		HWIF(drive)->channel ? "Secondary" : "Primary");
+		drive->hwif->channel ? "Secondary" : "Primary");
 }
 
 /**
diff --git a/drivers/ide/pdc202xx_old.c b/drivers/ide/pdc202xx_old.c
index 624e62e5cc9..97193323aeb 100644
--- a/drivers/ide/pdc202xx_old.c
+++ b/drivers/ide/pdc202xx_old.c
@@ -39,7 +39,7 @@ static void pdc_old_disable_66MHz_clock(ide_hwif_t *);
 
 static void pdc202xx_set_mode(ide_drive_t *drive, const u8 speed)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	u8 drive_pci		= 0x60 + (drive->dn << 2);
 
@@ -169,8 +169,8 @@ static void pdc202xx_dma_start(ide_drive_t *drive)
 	if (drive->current_speed > XFER_UDMA_2)
 		pdc_old_enable_66MHz_clock(drive->hwif);
 	if (drive->media != ide_disk || (drive->dev_flags & IDE_DFLAG_LBA48)) {
-		struct request *rq	= HWGROUP(drive)->rq;
-		ide_hwif_t *hwif	= HWIF(drive);
+		ide_hwif_t *hwif	= drive->hwif;
+		struct request *rq	= hwif->rq;
 		unsigned long high_16	= hwif->extra_base - 16;
 		unsigned long atapi_reg	= high_16 + (hwif->channel ? 0x24 : 0x20);
 		u32 word_count	= 0;
@@ -189,7 +189,7 @@ static void pdc202xx_dma_start(ide_drive_t *drive)
 static int pdc202xx_dma_end(ide_drive_t *drive)
 {
 	if (drive->media != ide_disk || (drive->dev_flags & IDE_DFLAG_LBA48)) {
-		ide_hwif_t *hwif	= HWIF(drive);
+		ide_hwif_t *hwif	= drive->hwif;
 		unsigned long high_16	= hwif->extra_base - 16;
 		unsigned long atapi_reg	= high_16 + (hwif->channel ? 0x24 : 0x20);
 		u8 clock		= 0;
@@ -205,7 +205,7 @@ static int pdc202xx_dma_end(ide_drive_t *drive)
 
 static int pdc202xx_dma_test_irq(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	unsigned long high_16	= hwif->extra_base - 16;
 	u8 dma_stat		= inb(hwif->dma_base + ATA_DMA_STATUS);
 	u8 sc1d			= inb(high_16 + 0x001d);
@@ -243,7 +243,7 @@ static void pdc202xx_reset_host (ide_hwif_t *hwif)
 
 static void pdc202xx_reset (ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	ide_hwif_t *mate	= hwif->mate;
 
 	pdc202xx_reset_host(hwif);
@@ -337,6 +337,7 @@ static const struct ide_dma_ops pdc20246_dma_ops = {
 	.dma_test_irq		= pdc202xx_dma_test_irq,
 	.dma_lost_irq		= pdc202xx_dma_lost_irq,
 	.dma_timeout		= pdc202xx_dma_timeout,
+	.dma_sff_read_status	= ide_dma_sff_read_status,
 };
 
 static const struct ide_dma_ops pdc2026x_dma_ops = {
@@ -348,6 +349,7 @@ static const struct ide_dma_ops pdc2026x_dma_ops = {
 	.dma_test_irq		= pdc202xx_dma_test_irq,
 	.dma_lost_irq		= pdc202xx_dma_lost_irq,
 	.dma_timeout		= pdc202xx_dma_timeout,
+	.dma_sff_read_status	= ide_dma_sff_read_status,
 };
 
 #define DECLARE_PDC2026X_DEV(udma, sectors) \
diff --git a/drivers/ide/piix.c b/drivers/ide/piix.c
index 61d2d920a5c..f1e2e4ef0d7 100644
--- a/drivers/ide/piix.c
+++ b/drivers/ide/piix.c
@@ -67,7 +67,7 @@ static int no_piix_dma;
 
 static void piix_set_pio_mode(ide_drive_t *drive, const u8 pio)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	int is_slave		= drive->dn & 1;
 	int master_port		= hwif->channel ? 0x42 : 0x40;
@@ -136,7 +136,7 @@ static void piix_set_pio_mode(ide_drive_t *drive, const u8 pio)
 
 static void piix_set_dma_mode(ide_drive_t *drive, const u8 speed)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	u8 maslave		= hwif->channel ? 0x42 : 0x40;
 	int a_speed		= 3 << (drive->dn * 4);
@@ -224,7 +224,7 @@ static unsigned int init_chipset_ich(struct pci_dev *dev)
  */
 static void ich_clear_irq(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	u8 dma_stat;
 
 	/*
@@ -260,6 +260,8 @@ static const struct ich_laptop ich_laptop[] = {
 	{ 0x27DF, 0x103C, 0x30A1 },	/* ICH7 on HP Compaq nc2400 */
 	{ 0x27DF, 0x1071, 0xD221 },	/* ICH7 on Hercules EC-900 */
 	{ 0x24CA, 0x1025, 0x0061 },	/* ICH4 on Acer Aspire 2023WLMi */
+	{ 0x24CA, 0x1025, 0x003d },	/* ICH4 on ACER TM290 */
+	{ 0x266F, 0x1025, 0x0066 },	/* ICH6 on ACER Aspire 1694WLMi */
 	{ 0x2653, 0x1043, 0x82D8 },	/* ICH6M on Asus Eee 701 */
 	/* end marker */
 	{ 0, }
diff --git a/drivers/ide/pmac.c b/drivers/ide/pmac.c
index 7c481bb56fa..74625e821a4 100644
--- a/drivers/ide/pmac.c
+++ b/drivers/ide/pmac.c
@@ -955,7 +955,6 @@ static const struct ide_tp_ops pmac_tp_ops = {
 	.exec_command		= pmac_exec_command,
 	.read_status		= ide_read_status,
 	.read_altstatus		= ide_read_altstatus,
-	.read_sff_dma_status	= ide_read_sff_dma_status,
 
 	.set_irq		= pmac_set_irq,
 
@@ -1513,10 +1512,10 @@ use_pio_instead:
 static int
 pmac_ide_dma_setup(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	pmac_ide_hwif_t *pmif =
 		(pmac_ide_hwif_t *)dev_get_drvdata(hwif->gendev.parent);
-	struct request *rq = HWGROUP(drive)->rq;
+	struct request *rq = hwif->rq;
 	u8 unit = drive->dn & 1, ata4 = (pmif->kind == controller_kl_ata4);
 
 	if (!pmac_ide_build_dmatable(drive, rq)) {
@@ -1637,7 +1636,7 @@ pmac_ide_dma_test_irq (ide_drive_t *drive)
 			break;
 		if (++timeout > 100) {
 			printk(KERN_WARNING "ide%d, ide_dma_test_irq \
-			timeout flushing channel\n", HWIF(drive)->index);
+			timeout flushing channel\n", hwif->index);
 			break;
 		}
 	}	
diff --git a/drivers/ide/q40ide.c b/drivers/ide/q40ide.c
index 4af4a8ce4cd..9f9c0b3cc3a 100644
--- a/drivers/ide/q40ide.c
+++ b/drivers/ide/q40ide.c
@@ -99,7 +99,6 @@ static const struct ide_tp_ops q40ide_tp_ops = {
 	.exec_command		= ide_exec_command,
 	.read_status		= ide_read_status,
 	.read_altstatus		= ide_read_altstatus,
-	.read_sff_dma_status	= ide_read_sff_dma_status,
 
 	.set_irq		= ide_set_irq,
 
diff --git a/drivers/ide/qd65xx.c b/drivers/ide/qd65xx.c
index bc27c7aba93..5b2e3af43c4 100644
--- a/drivers/ide/qd65xx.c
+++ b/drivers/ide/qd65xx.c
@@ -202,7 +202,8 @@ static void qd6500_set_pio_mode(ide_drive_t *drive, const u8 pio)
 		recovery_time = drive->id[ATA_ID_EIDE_PIO] - 120;
 	}
 
-	qd_set_timing(drive, qd6500_compute_timing(HWIF(drive), active_time, recovery_time));
+	qd_set_timing(drive, qd6500_compute_timing(drive->hwif,
+				active_time, recovery_time));
 }
 
 static void qd6580_set_pio_mode(ide_drive_t *drive, const u8 pio)
@@ -245,11 +246,11 @@ static void qd6580_set_pio_mode(ide_drive_t *drive, const u8 pio)
 		printk(KERN_INFO "%s: PIO mode%d\n", drive->name,pio);
 	}
 
-	if (!HWIF(drive)->channel && drive->media != ide_disk) {
+	if (!hwif->channel && drive->media != ide_disk) {
 		outb(0x5f, QD_CONTROL_PORT);
 		printk(KERN_WARNING "%s: ATAPI: disabled read-ahead FIFO "
 			"and post-write buffer on %s.\n",
-			drive->name, HWIF(drive)->name);
+			drive->name, hwif->name);
 	}
 
 	qd_set_timing(drive, qd6580_compute_timing(active_time, recovery_time));
diff --git a/drivers/ide/qd65xx.h b/drivers/ide/qd65xx.h
index c83dea85e62..6636f9665d1 100644
--- a/drivers/ide/qd65xx.h
+++ b/drivers/ide/qd65xx.h
@@ -31,8 +31,8 @@
 
 #define QD_CONFIG(hwif)		((hwif)->config_data & 0x00ff)
 
-#define QD_TIMING(drive)	(byte)(((drive)->drive_data) & 0x00ff)
-#define QD_TIMREG(drive)	(byte)((((drive)->drive_data) & 0xff00) >> 8)
+#define QD_TIMING(drive)	(u8)(((drive)->drive_data) & 0x00ff)
+#define QD_TIMREG(drive)	(u8)((((drive)->drive_data) & 0xff00) >> 8)
 
 #define QD6500_DEF_DATA		((QD_TIM1_PORT<<8) | (QD_ID3 ? 0x0c : 0x08))
 #define QD6580_DEF_DATA		((QD_TIM1_PORT<<8) | (QD_ID3 ? 0x0a : 0x00))
diff --git a/drivers/ide/sc1200.c b/drivers/ide/sc1200.c
index ec7f766ef5e..dbdd2985a0d 100644
--- a/drivers/ide/sc1200.c
+++ b/drivers/ide/sc1200.c
@@ -125,7 +125,7 @@ out:
 
 static void sc1200_set_dma_mode(ide_drive_t *drive, const u8 mode)
 {
-	ide_hwif_t		*hwif = HWIF(drive);
+	ide_hwif_t		*hwif = drive->hwif;
 	struct pci_dev		*dev = to_pci_dev(hwif->dev);
 	unsigned int		reg, timings;
 	unsigned short		pci_clock;
@@ -170,9 +170,9 @@ static void sc1200_set_dma_mode(ide_drive_t *drive, const u8 mode)
  */
 static int sc1200_dma_end(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	unsigned long dma_base = hwif->dma_base;
-	byte dma_stat;
+	u8 dma_stat;
 
 	dma_stat = inb(dma_base+2);		/* get DMA status */
 
@@ -199,7 +199,7 @@ static int sc1200_dma_end(ide_drive_t *drive)
 
 static void sc1200_set_pio_mode(ide_drive_t *drive, const u8 pio)
 {
-	ide_hwif_t	*hwif = HWIF(drive);
+	ide_hwif_t	*hwif = drive->hwif;
 	int		mode = -1;
 
 	/*
@@ -292,6 +292,7 @@ static const struct ide_dma_ops sc1200_dma_ops = {
 	.dma_test_irq		= ide_dma_test_irq,
 	.dma_lost_irq		= ide_dma_lost_irq,
 	.dma_timeout		= ide_dma_timeout,
+	.dma_sff_read_status	= ide_dma_sff_read_status,
 };
 
 static const struct ide_port_info sc1200_chipset __devinitdata = {
diff --git a/drivers/ide/scc_pata.c b/drivers/ide/scc_pata.c
index 0f48f9dacfa..8d2314b6327 100644
--- a/drivers/ide/scc_pata.c
+++ b/drivers/ide/scc_pata.c
@@ -143,7 +143,7 @@ static u8 scc_read_altstatus(ide_hwif_t *hwif)
 	return (u8)in_be32((void *)hwif->io_ports.ctl_addr);
 }
 
-static u8 scc_read_sff_dma_status(ide_hwif_t *hwif)
+static u8 scc_dma_sff_read_status(ide_hwif_t *hwif)
 {
 	return (u8)in_be32((void *)(hwif->dma_base + 4));
 }
@@ -217,7 +217,7 @@ scc_ide_outsl(unsigned long port, void *addr, u32 count)
 
 static void scc_set_pio_mode(ide_drive_t *drive, const u8 pio)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct scc_ports *ports = ide_get_hwifdata(hwif);
 	unsigned long ctl_base = ports->ctl;
 	unsigned long cckctrl_port = ctl_base + 0xff0;
@@ -249,7 +249,7 @@ static void scc_set_pio_mode(ide_drive_t *drive, const u8 pio)
 
 static void scc_set_dma_mode(ide_drive_t *drive, const u8 speed)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct scc_ports *ports = ide_get_hwifdata(hwif);
 	unsigned long ctl_base = ports->ctl;
 	unsigned long cckctrl_port = ctl_base + 0xff0;
@@ -259,7 +259,7 @@ static void scc_set_dma_mode(ide_drive_t *drive, const u8 speed)
 	unsigned long scrcst_port = ctl_base + 0x014;
 	unsigned long udenvt_port = ctl_base + 0x018;
 	unsigned long tdvhsel_port   = ctl_base + 0x020;
-	int is_slave = (&hwif->drives[1] == drive);
+	int is_slave = drive->dn & 1;
 	int offset, idx;
 	unsigned long reg;
 	unsigned long jcactsel;
@@ -292,7 +292,7 @@ static void scc_dma_host_set(ide_drive_t *drive, int on)
 {
 	ide_hwif_t *hwif = drive->hwif;
 	u8 unit = drive->dn & 1;
-	u8 dma_stat = scc_ide_inb(hwif->dma_base + 4);
+	u8 dma_stat = scc_dma_sff_read_status(hwif);
 
 	if (on)
 		dma_stat |= (1 << (5 + unit));
@@ -316,7 +316,7 @@ static void scc_dma_host_set(ide_drive_t *drive, int on)
 static int scc_dma_setup(ide_drive_t *drive)
 {
 	ide_hwif_t *hwif = drive->hwif;
-	struct request *rq = HWGROUP(drive)->rq;
+	struct request *rq = hwif->rq;
 	unsigned int reading;
 	u8 dma_stat;
 
@@ -338,7 +338,7 @@ static int scc_dma_setup(ide_drive_t *drive)
 	out_be32((void __iomem *)hwif->dma_base, reading);
 
 	/* read DMA status for INTR & ERROR flags */
-	dma_stat = in_be32((void __iomem *)(hwif->dma_base + 4));
+	dma_stat = scc_dma_sff_read_status(hwif);
 
 	/* clear INTR & ERROR flags */
 	out_be32((void __iomem *)(hwif->dma_base + 4), dma_stat | 6);
@@ -367,7 +367,7 @@ static int __scc_dma_end(ide_drive_t *drive)
 	/* stop DMA */
 	scc_ide_outb(dma_cmd & ~1, hwif->dma_base);
 	/* get DMA status */
-	dma_stat = scc_ide_inb(hwif->dma_base + 4);
+	dma_stat = scc_dma_sff_read_status(hwif);
 	/* clear the INTR & ERROR bits */
 	scc_ide_outb(dma_stat | 6, hwif->dma_base + 4);
 	/* purge DMA mappings */
@@ -387,7 +387,7 @@ static int __scc_dma_end(ide_drive_t *drive)
 
 static int scc_dma_end(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	void __iomem *dma_base = (void __iomem *)hwif->dma_base;
 	unsigned long intsts_port = hwif->dma_base + 0x014;
 	u32 reg;
@@ -405,17 +405,18 @@ static int scc_dma_end(ide_drive_t *drive)
 			       drive->name);
 			data_loss = 1;
 			if (retry++) {
-				struct request *rq = HWGROUP(drive)->rq;
-				int unit;
+				struct request *rq = hwif->rq;
+				ide_drive_t *drive;
+				int i;
+
 				/* ERROR_RESET and drive->crc_count are needed
 				 * to reduce DMA transfer mode in retry process.
 				 */
 				if (rq)
 					rq->errors |= ERROR_RESET;
-				for (unit = 0; unit < MAX_DRIVES; unit++) {
-					ide_drive_t *drive = &hwif->drives[unit];
+
+				ide_port_for_each_dev(i, drive, hwif)
 					drive->crc_count++;
-				}
 			}
 		}
 	}
@@ -496,7 +497,7 @@ static int scc_dma_end(ide_drive_t *drive)
 /* returns 1 if dma irq issued, 0 otherwise */
 static int scc_dma_test_irq(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	u32 int_stat = in_be32((void __iomem *)hwif->dma_base + 0x014);
 
 	/* SCC errata A252,A308 workaround: Step4 */
@@ -852,7 +853,6 @@ static const struct ide_tp_ops scc_tp_ops = {
 	.exec_command		= scc_exec_command,
 	.read_status		= scc_read_status,
 	.read_altstatus		= scc_read_altstatus,
-	.read_sff_dma_status	= scc_read_sff_dma_status,
 
 	.set_irq		= scc_set_irq,
 
@@ -879,6 +879,7 @@ static const struct ide_dma_ops scc_dma_ops = {
 	.dma_test_irq		= scc_dma_test_irq,
 	.dma_lost_irq		= ide_dma_lost_irq,
 	.dma_timeout		= ide_dma_timeout,
+	.dma_sff_read_status	= scc_dma_sff_read_status,
 };
 
 #define DECLARE_SCC_DEV(name_str)			\
diff --git a/drivers/ide/serverworks.c b/drivers/ide/serverworks.c
index 437bc919daf..382102ba467 100644
--- a/drivers/ide/serverworks.c
+++ b/drivers/ide/serverworks.c
@@ -151,7 +151,7 @@ static void svwks_set_dma_mode(ide_drive_t *drive, const u8 speed)
 	static const u8 dma_modes[]		= { 0x77, 0x21, 0x20 };
 	static const u8 drive_pci2[]		= { 0x45, 0x44, 0x47, 0x46 };
 
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	u8 unit			= drive->dn & 1;
 
diff --git a/drivers/ide/setup-pci.c b/drivers/ide/setup-pci.c
index 9f1f9163a13..e85d1ed29c2 100644
--- a/drivers/ide/setup-pci.c
+++ b/drivers/ide/setup-pci.c
@@ -130,7 +130,7 @@ int ide_pci_check_simplex(ide_hwif_t *hwif, const struct ide_port_info *d)
 	 * we tune the drive then try to grab DMA ownership if we want to be
 	 * the DMA end.  This has to be become dynamic to handle hot-plug.
 	 */
-	dma_stat = hwif->tp_ops->read_sff_dma_status(hwif);
+	dma_stat = hwif->dma_ops->dma_sff_read_status(hwif);
 	if ((dma_stat & 0x80) && hwif->mate && hwif->mate->dma_base) {
 		printk(KERN_INFO "%s %s: simplex device: DMA disabled\n",
 			d->name, pci_name(dev));
@@ -377,6 +377,9 @@ int ide_hwif_setup_dma(ide_hwif_t *hwif, const struct ide_port_info *d)
 
 		hwif->dma_base = base;
 
+		if (hwif->dma_ops == NULL)
+			hwif->dma_ops = &sff_dma_ops;
+
 		if (ide_pci_check_simplex(hwif, d) < 0)
 			return -1;
 
@@ -393,8 +396,6 @@ int ide_hwif_setup_dma(ide_hwif_t *hwif, const struct ide_port_info *d)
 
 		if (ide_allocate_dma_engine(hwif))
 			return -1;
-
-		hwif->dma_ops = &sff_dma_ops;
 	}
 
 	return 0;
@@ -471,7 +472,7 @@ void ide_pci_setup_ports(struct pci_dev *dev, const struct ide_port_info *d,
 	 */
 
 	for (port = 0; port < channels; ++port) {
-		const ide_pci_enablebit_t *e = &(d->enablebits[port]);
+		const struct ide_pci_enablebit *e = &d->enablebits[port];
 
 		if (e->reg && (pci_read_config_byte(dev, e->reg, &tmp) ||
 		    (tmp & e->mask) != e->val)) {
@@ -519,8 +520,7 @@ static int do_ide_setup_pci_device(struct pci_dev *dev,
 	if (ret < 0)
 		goto out;
 
-	/* Is it an "IDE storage" device in non-PCI mode? */
-	if ((dev->class >> 8) == PCI_CLASS_STORAGE_IDE && (dev->class & 5) != 5) {
+	if (ide_pci_is_in_compatibility_mode(dev)) {
 		if (noisy)
 			printk(KERN_INFO "%s %s: not 100%% native mode: will "
 				"probe irqs later\n", d->name, pci_name(dev));
diff --git a/drivers/ide/sgiioc4.c b/drivers/ide/sgiioc4.c
index a687a7dfea6..fdb9d703769 100644
--- a/drivers/ide/sgiioc4.c
+++ b/drivers/ide/sgiioc4.c
@@ -123,7 +123,7 @@ static int
 sgiioc4_clearirq(ide_drive_t * drive)
 {
 	u32 intr_reg;
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct ide_io_ports *io_ports = &hwif->io_ports;
 	unsigned long other_ir = io_ports->irq_addr + (IOC4_INTR_REG << 2);
 
@@ -181,7 +181,7 @@ sgiioc4_clearirq(ide_drive_t * drive)
 
 static void sgiioc4_dma_start(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	unsigned long ioc4_dma_addr = hwif->dma_base + IOC4_DMA_CTRL * 4;
 	unsigned int reg = readl((void __iomem *)ioc4_dma_addr);
 	unsigned int temp_reg = reg | IOC4_S_DMA_START;
@@ -209,7 +209,7 @@ sgiioc4_ide_dma_stop(ide_hwif_t *hwif, u64 dma_base)
 static int sgiioc4_dma_end(ide_drive_t *drive)
 {
 	u32 ioc4_dma, bc_dev, bc_mem, num, valid = 0, cnt = 0;
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	unsigned long dma_base = hwif->dma_base;
 	int dma_stat = 0;
 	unsigned long *ending_dma = ide_get_hwifdata(hwif);
@@ -271,7 +271,7 @@ static void sgiioc4_set_dma_mode(ide_drive_t *drive, const u8 speed)
 /* returns 1 if dma irq issued, 0 otherwise */
 static int sgiioc4_dma_test_irq(ide_drive_t *drive)
 {
-	return sgiioc4_checkirq(HWIF(drive));
+	return sgiioc4_checkirq(drive->hwif);
 }
 
 static void sgiioc4_dma_host_set(ide_drive_t *drive, int on)
@@ -367,7 +367,7 @@ static void
 sgiioc4_configure_for_dma(int dma_direction, ide_drive_t * drive)
 {
 	u32 ioc4_dma;
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	unsigned long dma_base = hwif->dma_base;
 	unsigned long ioc4_dma_addr = dma_base + IOC4_DMA_CTRL * 4;
 	u32 dma_addr, ending_dma_addr;
@@ -427,7 +427,7 @@ sgiioc4_configure_for_dma(int dma_direction, ide_drive_t * drive)
 static unsigned int
 sgiioc4_build_dma_table(ide_drive_t * drive, struct request *rq, int ddir)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	unsigned int *table = hwif->dmatable_cpu;
 	unsigned int count = 0, i = 1;
 	struct scatterlist *sg;
@@ -492,7 +492,7 @@ use_pio_instead:
 
 static int sgiioc4_dma_setup(ide_drive_t *drive)
 {
-	struct request *rq = HWGROUP(drive)->rq;
+	struct request *rq = drive->hwif->rq;
 	unsigned int count = 0;
 	int ddir;
 
@@ -523,7 +523,6 @@ static const struct ide_tp_ops sgiioc4_tp_ops = {
 	.exec_command		= ide_exec_command,
 	.read_status		= sgiioc4_read_status,
 	.read_altstatus		= ide_read_altstatus,
-	.read_sff_dma_status	= ide_read_sff_dma_status,
 
 	.set_irq		= ide_set_irq,
 
diff --git a/drivers/ide/siimage.c b/drivers/ide/siimage.c
index 7d622d20bc4..cb2b352b876 100644
--- a/drivers/ide/siimage.c
+++ b/drivers/ide/siimage.c
@@ -114,7 +114,7 @@ static unsigned long siimage_selreg(ide_hwif_t *hwif, int r)
 
 static inline unsigned long siimage_seldev(ide_drive_t *drive, int r)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	unsigned long base	= (unsigned long)hwif->hwif_data;
 	u8 unit			= drive->dn & 1;
 
@@ -243,7 +243,7 @@ static void sil_set_pio_mode(ide_drive_t *drive, u8 pio)
 	static const u16 tf_speed[]   = { 0x328a, 0x2283, 0x1281, 0x10c3, 0x10c1 };
 	static const u16 data_speed[] = { 0x328a, 0x2283, 0x1104, 0x10c3, 0x10c1 };
 
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	ide_drive_t *pair	= ide_get_pair_dev(drive);
 	u32 speedt		= 0;
@@ -300,7 +300,7 @@ static void sil_set_dma_mode(ide_drive_t *drive, const u8 speed)
 	static const u8 ultra5[] = { 0x0C, 0x07, 0x05, 0x04, 0x02, 0x01 };
 	static const u16 dma[]	 = { 0x2208, 0x10C2, 0x10C1 };
 
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	unsigned long base	= (unsigned long)hwif->hwif_data;
 	u16 ultra = 0, multi	= 0;
@@ -340,7 +340,7 @@ static void sil_set_dma_mode(ide_drive_t *drive, const u8 speed)
 /* returns 1 if dma irq issued, 0 otherwise */
 static int siimage_io_dma_test_irq(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	u8 dma_altstat		= 0;
 	unsigned long addr	= siimage_selreg(hwif, 1);
@@ -367,7 +367,7 @@ static int siimage_io_dma_test_irq(ide_drive_t *drive)
 
 static int siimage_mmio_dma_test_irq(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	unsigned long addr	= siimage_selreg(hwif, 0x1);
 	void __iomem *sata_error_addr
 		= (void __iomem *)hwif->sata_scr[SATA_ERROR_OFFSET];
@@ -717,6 +717,7 @@ static const struct ide_dma_ops sil_dma_ops = {
 	.dma_test_irq		= siimage_dma_test_irq,
 	.dma_timeout		= ide_dma_timeout,
 	.dma_lost_irq		= ide_dma_lost_irq,
+	.dma_sff_read_status	= ide_dma_sff_read_status,
 };
 
 #define DECLARE_SII_DEV(p_ops)				\
diff --git a/drivers/ide/sis5513.c b/drivers/ide/sis5513.c
index ad32e18c5ba..9ec1a4a4432 100644
--- a/drivers/ide/sis5513.c
+++ b/drivers/ide/sis5513.c
@@ -274,7 +274,7 @@ static void sis_program_timings(ide_drive_t *drive, const u8 mode)
 
 static void config_drive_art_rwp(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	u8 reg4bh		= 0;
 	u8 rw_prefetch		= 0;
diff --git a/drivers/ide/sl82c105.c b/drivers/ide/sl82c105.c
index 84dc33602ff..48cc748c504 100644
--- a/drivers/ide/sl82c105.c
+++ b/drivers/ide/sl82c105.c
@@ -140,7 +140,7 @@ static inline void sl82c105_reset_host(struct pci_dev *dev)
  */
 static void sl82c105_dma_lost_irq(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	u32 val, mask		= hwif->channel ? CTRL_IDE_IRQB : CTRL_IDE_IRQA;
 	u8 dma_cmd;
@@ -177,7 +177,7 @@ static void sl82c105_dma_lost_irq(ide_drive_t *drive)
  */
 static void sl82c105_dma_start(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	int reg 		= 0x44 + drive->dn * 4;
 
@@ -299,6 +299,7 @@ static const struct ide_dma_ops sl82c105_dma_ops = {
 	.dma_test_irq		= ide_dma_test_irq,
 	.dma_lost_irq		= sl82c105_dma_lost_irq,
 	.dma_timeout		= sl82c105_dma_timeout,
+	.dma_sff_read_status	= ide_dma_sff_read_status,
 };
 
 static const struct ide_port_info sl82c105_chipset __devinitdata = {
diff --git a/drivers/ide/slc90e66.c b/drivers/ide/slc90e66.c
index 0f759e4ed77..40b4b94a428 100644
--- a/drivers/ide/slc90e66.c
+++ b/drivers/ide/slc90e66.c
@@ -20,7 +20,7 @@ static DEFINE_SPINLOCK(slc90e66_lock);
 
 static void slc90e66_set_pio_mode(ide_drive_t *drive, const u8 pio)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	int is_slave		= drive->dn & 1;
 	int master_port		= hwif->channel ? 0x42 : 0x40;
@@ -73,7 +73,7 @@ static void slc90e66_set_pio_mode(ide_drive_t *drive, const u8 pio)
 
 static void slc90e66_set_dma_mode(ide_drive_t *drive, const u8 speed)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	struct pci_dev *dev	= to_pci_dev(hwif->dev);
 	u8 maslave		= hwif->channel ? 0x42 : 0x40;
 	int sitre = 0, a_speed	= 7 << (drive->dn * 4);
diff --git a/drivers/ide/tc86c001.c b/drivers/ide/tc86c001.c
index 93e2cce4b29..84109f5a163 100644
--- a/drivers/ide/tc86c001.c
+++ b/drivers/ide/tc86c001.c
@@ -15,7 +15,7 @@
 
 static void tc86c001_set_mode(ide_drive_t *drive, const u8 speed)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	unsigned long scr_port	= hwif->config_data + (drive->dn ? 0x02 : 0x00);
 	u16 mode, scr		= inw(scr_port);
 
@@ -62,13 +62,12 @@ static void tc86c001_set_pio_mode(ide_drive_t *drive, const u8 pio)
  */
 static int tc86c001_timer_expiry(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	ide_expiry_t *expiry	= ide_get_hwifdata(hwif);
-	ide_hwgroup_t *hwgroup	= HWGROUP(drive);
 	u8 dma_stat		= inb(hwif->dma_base + ATA_DMA_STATUS);
 
 	/* Restore a higher level driver's expiry handler first. */
-	hwgroup->expiry	= expiry;
+	hwif->expiry = expiry;
 
 	if ((dma_stat & 5) == 1) {	/* DMA active and no interrupt */
 		unsigned long sc_base	= hwif->config_data;
@@ -110,11 +109,10 @@ static int tc86c001_timer_expiry(ide_drive_t *drive)
 
 static void tc86c001_dma_start(ide_drive_t *drive)
 {
-	ide_hwif_t *hwif	= HWIF(drive);
-	ide_hwgroup_t *hwgroup	= HWGROUP(drive);
+	ide_hwif_t *hwif	= drive->hwif;
 	unsigned long sc_base	= hwif->config_data;
 	unsigned long twcr_port	= sc_base + (drive->dn ? 0x06 : 0x04);
-	unsigned long nsectors	= hwgroup->rq->nr_sectors;
+	unsigned long nsectors	= hwif->rq->nr_sectors;
 
 	/*
 	 * We have to manually load the sector count and size into
@@ -125,8 +123,8 @@ static void tc86c001_dma_start(ide_drive_t *drive)
 	outw(SECTOR_SIZE / 2, twcr_port); /* Transfer Word Count 1/2 */
 
 	/* Install our timeout expiry hook, saving the current handler... */
-	ide_set_hwifdata(hwif, hwgroup->expiry);
-	hwgroup->expiry = &tc86c001_timer_expiry;
+	ide_set_hwifdata(hwif, hwif->expiry);
+	hwif->expiry = &tc86c001_timer_expiry;
 
 	ide_dma_start(drive);
 }
@@ -190,6 +188,7 @@ static const struct ide_dma_ops tc86c001_dma_ops = {
 	.dma_test_irq		= ide_dma_test_irq,
 	.dma_lost_irq		= ide_dma_lost_irq,
 	.dma_timeout		= ide_dma_timeout,
+	.dma_sff_read_status	= ide_dma_sff_read_status,
 };
 
 static const struct ide_port_info tc86c001_chipset __devinitdata = {
diff --git a/drivers/ide/triflex.c b/drivers/ide/triflex.c
index b6ff40336aa..8773c3ba746 100644
--- a/drivers/ide/triflex.c
+++ b/drivers/ide/triflex.c
@@ -36,7 +36,7 @@
 
 static void triflex_set_mode(ide_drive_t *drive, const u8 speed)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	struct pci_dev *dev = to_pci_dev(hwif->dev);
 	u32 triflex_timings = 0;
 	u16 timing = 0;
diff --git a/drivers/ide/trm290.c b/drivers/ide/trm290.c
index 2a5ea90cf8b..b6a1285a402 100644
--- a/drivers/ide/trm290.c
+++ b/drivers/ide/trm290.c
@@ -144,7 +144,7 @@
 
 static void trm290_prepare_drive (ide_drive_t *drive, unsigned int use_dma)
 {
-	ide_hwif_t *hwif = HWIF(drive);
+	ide_hwif_t *hwif = drive->hwif;
 	u16 reg = 0;
 	unsigned long flags;
 
@@ -184,7 +184,7 @@ static void trm290_dma_exec_cmd(ide_drive_t *drive, u8 command)
 static int trm290_dma_setup(ide_drive_t *drive)
 {
 	ide_hwif_t *hwif = drive->hwif;
-	struct request *rq = hwif->hwgroup->rq;
+	struct request *rq = hwif->rq;
 	unsigned int count, rw;
 
 	if (rq_data_dir(rq)) {
@@ -222,15 +222,15 @@ static int trm290_dma_end(ide_drive_t *drive)
 	drive->waiting_for_dma = 0;
 	/* purge DMA mappings */
 	ide_destroy_dmatable(drive);
-	status = inw(HWIF(drive)->dma_base + 2);
+	status = inw(drive->hwif->dma_base + 2);
+
 	return status != 0x00ff;
 }
 
 static int trm290_dma_test_irq(ide_drive_t *drive)
 {
-	u16 status;
+	u16 status = inw(drive->hwif->dma_base + 2);
 
-	status = inw(HWIF(drive)->dma_base + 2);
 	return status == 0x00ff;
 }
 
diff --git a/drivers/ide/tx4939ide.c b/drivers/ide/tx4939ide.c
index 4a8c5a21bd4..882f6f07c47 100644
--- a/drivers/ide/tx4939ide.c
+++ b/drivers/ide/tx4939ide.c
@@ -293,7 +293,7 @@ static int tx4939ide_dma_setup(ide_drive_t *drive)
 {
 	ide_hwif_t *hwif = drive->hwif;
 	void __iomem *base = TX4939IDE_BASE(hwif);
-	struct request *rq = hwif->hwgroup->rq;
+	struct request *rq = hwif->rq;
 	u8 reading;
 	int nent;
 
@@ -397,6 +397,17 @@ static int tx4939ide_dma_test_irq(ide_drive_t *drive)
 	return found;
 }
 
+#ifdef __BIG_ENDIAN
+static u8 tx4939ide_dma_sff_read_status(ide_hwif_t *hwif)
+{
+	void __iomem *base = TX4939IDE_BASE(hwif);
+
+	return tx4939ide_readb(base, TX4939IDE_DMA_Stat);
+}
+#else
+#define tx4939ide_dma_sff_read_status ide_dma_sff_read_status
+#endif
+
 static void tx4939ide_init_hwif(ide_hwif_t *hwif)
 {
 	void __iomem *base = TX4939IDE_BASE(hwif);
@@ -443,13 +454,6 @@ static void tx4939ide_tf_load_fixup(ide_drive_t *drive, ide_task_t *task)
 
 #ifdef __BIG_ENDIAN
 
-static u8 tx4939ide_read_sff_dma_status(ide_hwif_t *hwif)
-{
-	void __iomem *base = TX4939IDE_BASE(hwif);
-
-	return tx4939ide_readb(base, TX4939IDE_DMA_Stat);
-}
-
 /* custom iops (independent from SWAP_IO_SPACE) */
 static u8 tx4939ide_inb(unsigned long port)
 {
@@ -585,7 +589,6 @@ static const struct ide_tp_ops tx4939ide_tp_ops = {
 	.exec_command		= ide_exec_command,
 	.read_status		= ide_read_status,
 	.read_altstatus		= ide_read_altstatus,
-	.read_sff_dma_status	= tx4939ide_read_sff_dma_status,
 
 	.set_irq		= ide_set_irq,
 
@@ -609,7 +612,6 @@ static const struct ide_tp_ops tx4939ide_tp_ops = {
 	.exec_command		= ide_exec_command,
 	.read_status		= ide_read_status,
 	.read_altstatus		= ide_read_altstatus,
-	.read_sff_dma_status	= ide_read_sff_dma_status,
 
 	.set_irq		= ide_set_irq,
 
@@ -638,6 +640,7 @@ static const struct ide_dma_ops tx4939ide_dma_ops = {
 	.dma_test_irq		= tx4939ide_dma_test_irq,
 	.dma_lost_irq		= ide_dma_lost_irq,
 	.dma_timeout		= ide_dma_timeout,
+	.dma_sff_read_status	= tx4939ide_dma_sff_read_status,
 };
 
 static const struct ide_port_info tx4939ide_port_info __initdata = {
diff --git a/drivers/ide/umc8672.c b/drivers/ide/umc8672.c
index e29978cf619..0608d41fb6d 100644
--- a/drivers/ide/umc8672.c
+++ b/drivers/ide/umc8672.c
@@ -106,22 +106,21 @@ static void umc_set_speeds(u8 speeds[])
 
 static void umc_set_pio_mode(ide_drive_t *drive, const u8 pio)
 {
-	ide_hwif_t *hwif = drive->hwif;
-	ide_hwgroup_t *mate_hwgroup = hwif->mate ? hwif->mate->hwgroup : NULL;
+	ide_hwif_t *hwif = drive->hwif, *mate = hwif->mate;
 	unsigned long uninitialized_var(flags);
 
 	printk("%s: setting umc8672 to PIO mode%d (speed %d)\n",
 		drive->name, pio, pio_to_umc[pio]);
-	if (mate_hwgroup)
-		spin_lock_irqsave(&mate_hwgroup->lock, flags);
-	if (mate_hwgroup && mate_hwgroup->handler) {
+	if (mate)
+		spin_lock_irqsave(&mate->lock, flags);
+	if (mate && mate->handler) {
 		printk(KERN_ERR "umc8672: other interface is busy: exiting tune_umc()\n");
 	} else {
 		current_speeds[drive->name[2] - 'a'] = pio_to_umc[pio];
 		umc_set_speeds(current_speeds);
 	}
-	if (mate_hwgroup)
-		spin_unlock_irqrestore(&mate_hwgroup->lock, flags);
+	if (mate)
+		spin_unlock_irqrestore(&mate->lock, flags);
 }
 
 static const struct ide_port_ops umc8672_port_ops = {
diff --git a/drivers/ide/via82cxxx.c b/drivers/ide/via82cxxx.c
index 2a812d3207e..fecc0e03c3f 100644
--- a/drivers/ide/via82cxxx.c
+++ b/drivers/ide/via82cxxx.c
@@ -178,7 +178,7 @@ static void via_set_drive(ide_drive_t *drive, const u8 speed)
 		ide_timing_merge(&p, &t, &t, IDE_TIMING_8BIT);
 	}
 
-	via_set_speed(HWIF(drive), drive->dn, &t);
+	via_set_speed(hwif, drive->dn, &t);
 }
 
 /**
diff --git a/drivers/idle/i7300_idle.c b/drivers/idle/i7300_idle.c
index fb176f6ef9f..17e8ddd0133 100644
--- a/drivers/idle/i7300_idle.c
+++ b/drivers/idle/i7300_idle.c
@@ -177,7 +177,7 @@ static int __init i7300_idle_ioat_selftest(u8 *ctl,
 }
 
 static struct device dummy_dma_dev = {
-	.bus_id = "fallback device",
+	.init_name = "fallback device",
 	.coherent_dma_mask = DMA_64BIT_MASK,
 	.dma_mask = &dummy_dma_dev.coherent_dma_mask,
 };
diff --git a/drivers/infiniband/core/sysfs.c b/drivers/infiniband/core/sysfs.c
index 4f4d1bb9f06..b43f7d3682d 100644
--- a/drivers/infiniband/core/sysfs.c
+++ b/drivers/infiniband/core/sysfs.c
@@ -778,7 +778,7 @@ int ib_device_register_sysfs(struct ib_device *device)
 	class_dev->class      = &ib_class;
 	class_dev->driver_data = device;
 	class_dev->parent     = device->dma_device;
-	strlcpy(class_dev->bus_id, device->name, BUS_ID_SIZE);
+	dev_set_name(class_dev, device->name);
 
 	INIT_LIST_HEAD(&device->port_list);
 
diff --git a/drivers/infiniband/core/ucm.c b/drivers/infiniband/core/ucm.c
index e603736682b..51bd9669cb1 100644
--- a/drivers/infiniband/core/ucm.c
+++ b/drivers/infiniband/core/ucm.c
@@ -1266,8 +1266,7 @@ static void ib_ucm_add_one(struct ib_device *device)
 	ucm_dev->dev.parent = device->dma_device;
 	ucm_dev->dev.devt = ucm_dev->cdev.dev;
 	ucm_dev->dev.release = ib_ucm_release_dev;
-	snprintf(ucm_dev->dev.bus_id, BUS_ID_SIZE, "ucm%d",
-		 ucm_dev->devnum);
+	dev_set_name(&ucm_dev->dev, "ucm%d", ucm_dev->devnum);
 	if (device_register(&ucm_dev->dev))
 		goto err_cdev;
 
diff --git a/drivers/infiniband/ulp/srp/ib_srp.c b/drivers/infiniband/ulp/srp/ib_srp.c
index 7c13db885bf..54c8fe25c42 100644
--- a/drivers/infiniband/ulp/srp/ib_srp.c
+++ b/drivers/infiniband/ulp/srp/ib_srp.c
@@ -1949,8 +1949,7 @@ static struct srp_host *srp_add_port(struct srp_device *device, u8 port)
 
 	host->dev.class = &srp_class;
 	host->dev.parent = device->dev->dma_device;
-	snprintf(host->dev.bus_id, BUS_ID_SIZE, "srp-%s-%d",
-		 device->dev->name, port);
+	dev_set_name(&host->dev, "srp-%s-%d", device->dev->name, port);
 
 	if (device_register(&host->dev))
 		goto free_host;
diff --git a/drivers/input/Makefile b/drivers/input/Makefile
index 98c4f9a7787..4c9c745a702 100644
--- a/drivers/input/Makefile
+++ b/drivers/input/Makefile
@@ -5,7 +5,7 @@
 # Each configuration option enables a list of files.
 
 obj-$(CONFIG_INPUT)		+= input-core.o
-input-core-objs := input.o ff-core.o
+input-core-objs := input.o input-compat.o ff-core.o
 
 obj-$(CONFIG_INPUT_FF_MEMLESS)	+= ff-memless.o
 obj-$(CONFIG_INPUT_POLLDEV)	+= input-polldev.o
diff --git a/drivers/input/evbug.c b/drivers/input/evbug.c
index 0353601ac3b..f7c5c14ec12 100644
--- a/drivers/input/evbug.c
+++ b/drivers/input/evbug.c
@@ -39,7 +39,7 @@ MODULE_LICENSE("GPL");
 static void evbug_event(struct input_handle *handle, unsigned int type, unsigned int code, int value)
 {
 	printk(KERN_DEBUG "evbug.c: Event. Dev: %s, Type: %d, Code: %d, Value: %d\n",
-		handle->dev->dev.bus_id, type, code, value);
+		dev_name(&handle->dev->dev), type, code, value);
 }
 
 static int evbug_connect(struct input_handler *handler, struct input_dev *dev,
@@ -65,7 +65,7 @@ static int evbug_connect(struct input_handler *handler, struct input_dev *dev,
 		goto err_unregister_handle;
 
 	printk(KERN_DEBUG "evbug.c: Connected device: %s (%s at %s)\n",
-		dev->dev.bus_id,
+		dev_name(&dev->dev),
 		dev->name ?: "unknown",
 		dev->phys ?: "unknown");
 
@@ -81,7 +81,7 @@ static int evbug_connect(struct input_handler *handler, struct input_dev *dev,
 static void evbug_disconnect(struct input_handle *handle)
 {
 	printk(KERN_DEBUG "evbug.c: Disconnected device: %s\n",
-		handle->dev->dev.bus_id);
+		dev_name(&handle->dev->dev));
 
 	input_close_device(handle);
 	input_unregister_handle(handle);
diff --git a/drivers/input/evdev.c b/drivers/input/evdev.c
index 1070db330d3..ed8baa0aec3 100644
--- a/drivers/input/evdev.c
+++ b/drivers/input/evdev.c
@@ -19,7 +19,7 @@
 #include <linux/input.h>
 #include <linux/major.h>
 #include <linux/device.h>
-#include <linux/compat.h>
+#include "input-compat.h"
 
 struct evdev {
 	int exist;
@@ -290,187 +290,6 @@ static int evdev_open(struct inode *inode, struct file *file)
 	return error;
 }
 
-#ifdef CONFIG_COMPAT
-
-struct input_event_compat {
-	struct compat_timeval time;
-	__u16 type;
-	__u16 code;
-	__s32 value;
-};
-
-struct ff_periodic_effect_compat {
-	__u16 waveform;
-	__u16 period;
-	__s16 magnitude;
-	__s16 offset;
-	__u16 phase;
-
-	struct ff_envelope envelope;
-
-	__u32 custom_len;
-	compat_uptr_t custom_data;
-};
-
-struct ff_effect_compat {
-	__u16 type;
-	__s16 id;
-	__u16 direction;
-	struct ff_trigger trigger;
-	struct ff_replay replay;
-
-	union {
-		struct ff_constant_effect constant;
-		struct ff_ramp_effect ramp;
-		struct ff_periodic_effect_compat periodic;
-		struct ff_condition_effect condition[2]; /* One for each axis */
-		struct ff_rumble_effect rumble;
-	} u;
-};
-
-/* Note to the author of this code: did it ever occur to
-   you why the ifdefs are needed? Think about it again. -AK */
-#ifdef CONFIG_X86_64
-#  define COMPAT_TEST is_compat_task()
-#elif defined(CONFIG_IA64)
-#  define COMPAT_TEST IS_IA32_PROCESS(task_pt_regs(current))
-#elif defined(CONFIG_S390)
-#  define COMPAT_TEST test_thread_flag(TIF_31BIT)
-#elif defined(CONFIG_MIPS)
-#  define COMPAT_TEST test_thread_flag(TIF_32BIT_ADDR)
-#else
-#  define COMPAT_TEST test_thread_flag(TIF_32BIT)
-#endif
-
-static inline size_t evdev_event_size(void)
-{
-	return COMPAT_TEST ?
-		sizeof(struct input_event_compat) : sizeof(struct input_event);
-}
-
-static int evdev_event_from_user(const char __user *buffer,
-				 struct input_event *event)
-{
-	if (COMPAT_TEST) {
-		struct input_event_compat compat_event;
-
-		if (copy_from_user(&compat_event, buffer,
-				   sizeof(struct input_event_compat)))
-			return -EFAULT;
-
-		event->time.tv_sec = compat_event.time.tv_sec;
-		event->time.tv_usec = compat_event.time.tv_usec;
-		event->type = compat_event.type;
-		event->code = compat_event.code;
-		event->value = compat_event.value;
-
-	} else {
-		if (copy_from_user(event, buffer, sizeof(struct input_event)))
-			return -EFAULT;
-	}
-
-	return 0;
-}
-
-static int evdev_event_to_user(char __user *buffer,
-				const struct input_event *event)
-{
-	if (COMPAT_TEST) {
-		struct input_event_compat compat_event;
-
-		compat_event.time.tv_sec = event->time.tv_sec;
-		compat_event.time.tv_usec = event->time.tv_usec;
-		compat_event.type = event->type;
-		compat_event.code = event->code;
-		compat_event.value = event->value;
-
-		if (copy_to_user(buffer, &compat_event,
-				 sizeof(struct input_event_compat)))
-			return -EFAULT;
-
-	} else {
-		if (copy_to_user(buffer, event, sizeof(struct input_event)))
-			return -EFAULT;
-	}
-
-	return 0;
-}
-
-static int evdev_ff_effect_from_user(const char __user *buffer, size_t size,
-				     struct ff_effect *effect)
-{
-	if (COMPAT_TEST) {
-		struct ff_effect_compat *compat_effect;
-
-		if (size != sizeof(struct ff_effect_compat))
-			return -EINVAL;
-
-		/*
-		 * It so happens that the pointer which needs to be changed
-		 * is the last field in the structure, so we can copy the
-		 * whole thing and replace just the pointer.
-		 */
-
-		compat_effect = (struct ff_effect_compat *)effect;
-
-		if (copy_from_user(compat_effect, buffer,
-				   sizeof(struct ff_effect_compat)))
-			return -EFAULT;
-
-		if (compat_effect->type == FF_PERIODIC &&
-		    compat_effect->u.periodic.waveform == FF_CUSTOM)
-			effect->u.periodic.custom_data =
-				compat_ptr(compat_effect->u.periodic.custom_data);
-	} else {
-		if (size != sizeof(struct ff_effect))
-			return -EINVAL;
-
-		if (copy_from_user(effect, buffer, sizeof(struct ff_effect)))
-			return -EFAULT;
-	}
-
-	return 0;
-}
-
-#else
-
-static inline size_t evdev_event_size(void)
-{
-	return sizeof(struct input_event);
-}
-
-static int evdev_event_from_user(const char __user *buffer,
-				 struct input_event *event)
-{
-	if (copy_from_user(event, buffer, sizeof(struct input_event)))
-		return -EFAULT;
-
-	return 0;
-}
-
-static int evdev_event_to_user(char __user *buffer,
-				const struct input_event *event)
-{
-	if (copy_to_user(buffer, event, sizeof(struct input_event)))
-		return -EFAULT;
-
-	return 0;
-}
-
-static int evdev_ff_effect_from_user(const char __user *buffer, size_t size,
-				     struct ff_effect *effect)
-{
-	if (size != sizeof(struct ff_effect))
-		return -EINVAL;
-
-	if (copy_from_user(effect, buffer, sizeof(struct ff_effect)))
-		return -EFAULT;
-
-	return 0;
-}
-
-#endif /* CONFIG_COMPAT */
-
 static ssize_t evdev_write(struct file *file, const char __user *buffer,
 			   size_t count, loff_t *ppos)
 {
@@ -490,14 +309,14 @@ static ssize_t evdev_write(struct file *file, const char __user *buffer,
 
 	while (retval < count) {
 
-		if (evdev_event_from_user(buffer + retval, &event)) {
+		if (input_event_from_user(buffer + retval, &event)) {
 			retval = -EFAULT;
 			goto out;
 		}
 
 		input_inject_event(&evdev->handle,
 				   event.type, event.code, event.value);
-		retval += evdev_event_size();
+		retval += input_event_size();
 	}
 
  out:
@@ -531,7 +350,7 @@ static ssize_t evdev_read(struct file *file, char __user *buffer,
 	struct input_event event;
 	int retval;
 
-	if (count < evdev_event_size())
+	if (count < input_event_size())
 		return -EINVAL;
 
 	if (client->head == client->tail && evdev->exist &&
@@ -546,13 +365,13 @@ static ssize_t evdev_read(struct file *file, char __user *buffer,
 	if (!evdev->exist)
 		return -ENODEV;
 
-	while (retval + evdev_event_size() <= count &&
+	while (retval + input_event_size() <= count &&
 	       evdev_fetch_next_event(client, &event)) {
 
-		if (evdev_event_to_user(buffer + retval, &event))
+		if (input_event_to_user(buffer + retval, &event))
 			return -EFAULT;
 
-		retval += evdev_event_size();
+		retval += input_event_size();
 	}
 
 	return retval;
@@ -823,7 +642,7 @@ static long evdev_do_ioctl(struct file *file, unsigned int cmd,
 
 			if (_IOC_NR(cmd) == _IOC_NR(EVIOCSFF)) {
 
-				if (evdev_ff_effect_from_user(p, _IOC_SIZE(cmd), &effect))
+				if (input_ff_effect_from_user(p, _IOC_SIZE(cmd), &effect))
 					return -EFAULT;
 
 				error = input_ff_upload(dev, &effect, file);
@@ -1000,7 +819,7 @@ static int evdev_connect(struct input_handler *handler, struct input_dev *dev,
 	evdev->handle.handler = handler;
 	evdev->handle.private = evdev;
 
-	strlcpy(evdev->dev.bus_id, evdev->name, sizeof(evdev->dev.bus_id));
+	dev_set_name(&evdev->dev, evdev->name);
 	evdev->dev.devt = MKDEV(INPUT_MAJOR, EVDEV_MINOR_BASE + minor);
 	evdev->dev.class = &input_class;
 	evdev->dev.parent = &dev->dev;
diff --git a/drivers/input/gameport/gameport.c b/drivers/input/gameport/gameport.c
index 2880eaae157..ebf4be5b7c4 100644
--- a/drivers/input/gameport/gameport.c
+++ b/drivers/input/gameport/gameport.c
@@ -530,8 +530,7 @@ static void gameport_init_port(struct gameport *gameport)
 
 	mutex_init(&gameport->drv_mutex);
 	device_initialize(&gameport->dev);
-	snprintf(gameport->dev.bus_id, sizeof(gameport->dev.bus_id),
-		 "gameport%lu", (unsigned long)atomic_inc_return(&gameport_no) - 1);
+	dev_set_name(&gameport->dev, "gameport%lu", (unsigned long)atomic_inc_return(&gameport_no) - 1);
 	gameport->dev.bus = &gameport_bus;
 	gameport->dev.release = gameport_release_port;
 	if (gameport->parent)
diff --git a/drivers/input/gameport/ns558.c b/drivers/input/gameport/ns558.c
index 2b282cde4b8..db556b71ddd 100644
--- a/drivers/input/gameport/ns558.c
+++ b/drivers/input/gameport/ns558.c
@@ -226,7 +226,7 @@ static int ns558_pnp_probe(struct pnp_dev *dev, const struct pnp_device_id *did)
 	ns558->gameport = port;
 
 	gameport_set_name(port, "NS558 PnP Gameport");
-	gameport_set_phys(port, "pnp%s/gameport0", dev->dev.bus_id);
+	gameport_set_phys(port, "pnp%s/gameport0", dev_name(&dev->dev));
 	port->dev.parent = &dev->dev;
 	port->io = ioport;
 
diff --git a/drivers/input/input-compat.c b/drivers/input/input-compat.c
new file mode 100644
index 00000000000..1accb89ae66
--- /dev/null
+++ b/drivers/input/input-compat.c
@@ -0,0 +1,135 @@
+/*
+ * 32bit compatibility wrappers for the input subsystem.
+ *
+ * Very heavily based on evdev.c - Copyright (c) 1999-2002 Vojtech Pavlik
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ */
+
+#include <asm/uaccess.h>
+#include "input-compat.h"
+
+#ifdef CONFIG_COMPAT
+
+int input_event_from_user(const char __user *buffer,
+			  struct input_event *event)
+{
+	if (INPUT_COMPAT_TEST) {
+		struct input_event_compat compat_event;
+
+		if (copy_from_user(&compat_event, buffer,
+				   sizeof(struct input_event_compat)))
+			return -EFAULT;
+
+		event->time.tv_sec = compat_event.time.tv_sec;
+		event->time.tv_usec = compat_event.time.tv_usec;
+		event->type = compat_event.type;
+		event->code = compat_event.code;
+		event->value = compat_event.value;
+
+	} else {
+		if (copy_from_user(event, buffer, sizeof(struct input_event)))
+			return -EFAULT;
+	}
+
+	return 0;
+}
+
+int input_event_to_user(char __user *buffer,
+			const struct input_event *event)
+{
+	if (INPUT_COMPAT_TEST) {
+		struct input_event_compat compat_event;
+
+		compat_event.time.tv_sec = event->time.tv_sec;
+		compat_event.time.tv_usec = event->time.tv_usec;
+		compat_event.type = event->type;
+		compat_event.code = event->code;
+		compat_event.value = event->value;
+
+		if (copy_to_user(buffer, &compat_event,
+				 sizeof(struct input_event_compat)))
+			return -EFAULT;
+
+	} else {
+		if (copy_to_user(buffer, event, sizeof(struct input_event)))
+			return -EFAULT;
+	}
+
+	return 0;
+}
+
+int input_ff_effect_from_user(const char __user *buffer, size_t size,
+			      struct ff_effect *effect)
+{
+	if (INPUT_COMPAT_TEST) {
+		struct ff_effect_compat *compat_effect;
+
+		if (size != sizeof(struct ff_effect_compat))
+			return -EINVAL;
+
+		/*
+		 * It so happens that the pointer which needs to be changed
+		 * is the last field in the structure, so we can retrieve the
+		 * whole thing and replace just the pointer.
+		 */
+		compat_effect = (struct ff_effect_compat *)effect;
+
+		if (copy_from_user(compat_effect, buffer,
+				   sizeof(struct ff_effect_compat)))
+			return -EFAULT;
+
+		if (compat_effect->type == FF_PERIODIC &&
+		    compat_effect->u.periodic.waveform == FF_CUSTOM)
+			effect->u.periodic.custom_data =
+				compat_ptr(compat_effect->u.periodic.custom_data);
+	} else {
+		if (size != sizeof(struct ff_effect))
+			return -EINVAL;
+
+		if (copy_from_user(effect, buffer, sizeof(struct ff_effect)))
+			return -EFAULT;
+	}
+
+	return 0;
+}
+
+#else
+
+int input_event_from_user(const char __user *buffer,
+			 struct input_event *event)
+{
+	if (copy_from_user(event, buffer, sizeof(struct input_event)))
+		return -EFAULT;
+
+	return 0;
+}
+
+int input_event_to_user(char __user *buffer,
+			const struct input_event *event)
+{
+	if (copy_to_user(buffer, event, sizeof(struct input_event)))
+		return -EFAULT;
+
+	return 0;
+}
+
+int input_ff_effect_from_user(const char __user *buffer, size_t size,
+			      struct ff_effect *effect)
+{
+	if (size != sizeof(struct ff_effect))
+		return -EINVAL;
+
+	if (copy_from_user(effect, buffer, sizeof(struct ff_effect)))
+		return -EFAULT;
+
+	return 0;
+}
+
+#endif /* CONFIG_COMPAT */
+
+EXPORT_SYMBOL_GPL(input_event_from_user);
+EXPORT_SYMBOL_GPL(input_event_to_user);
+EXPORT_SYMBOL_GPL(input_ff_effect_from_user);
diff --git a/drivers/input/input-compat.h b/drivers/input/input-compat.h
new file mode 100644
index 00000000000..47cd9eaee66
--- /dev/null
+++ b/drivers/input/input-compat.h
@@ -0,0 +1,94 @@
+#ifndef _INPUT_COMPAT_H
+#define _INPUT_COMPAT_H
+
+/*
+ * 32bit compatibility wrappers for the input subsystem.
+ *
+ * Very heavily based on evdev.c - Copyright (c) 1999-2002 Vojtech Pavlik
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ */
+
+#include <linux/compiler.h>
+#include <linux/compat.h>
+#include <linux/input.h>
+
+#ifdef CONFIG_COMPAT
+
+/* Note to the author of this code: did it ever occur to
+   you why the ifdefs are needed? Think about it again. -AK */
+#ifdef CONFIG_X86_64
+#  define INPUT_COMPAT_TEST is_compat_task()
+#elif defined(CONFIG_IA64)
+#  define INPUT_COMPAT_TEST IS_IA32_PROCESS(task_pt_regs(current))
+#elif defined(CONFIG_S390)
+#  define INPUT_COMPAT_TEST test_thread_flag(TIF_31BIT)
+#elif defined(CONFIG_MIPS)
+#  define INPUT_COMPAT_TEST test_thread_flag(TIF_32BIT_ADDR)
+#else
+#  define INPUT_COMPAT_TEST test_thread_flag(TIF_32BIT)
+#endif
+
+struct input_event_compat {
+	struct compat_timeval time;
+	__u16 type;
+	__u16 code;
+	__s32 value;
+};
+
+struct ff_periodic_effect_compat {
+	__u16 waveform;
+	__u16 period;
+	__s16 magnitude;
+	__s16 offset;
+	__u16 phase;
+
+	struct ff_envelope envelope;
+
+	__u32 custom_len;
+	compat_uptr_t custom_data;
+};
+
+struct ff_effect_compat {
+	__u16 type;
+	__s16 id;
+	__u16 direction;
+	struct ff_trigger trigger;
+	struct ff_replay replay;
+
+	union {
+		struct ff_constant_effect constant;
+		struct ff_ramp_effect ramp;
+		struct ff_periodic_effect_compat periodic;
+		struct ff_condition_effect condition[2]; /* One for each axis */
+		struct ff_rumble_effect rumble;
+	} u;
+};
+
+static inline size_t input_event_size(void)
+{
+	return INPUT_COMPAT_TEST ?
+		sizeof(struct input_event_compat) : sizeof(struct input_event);
+}
+
+#else
+
+static inline size_t input_event_size(void)
+{
+	return sizeof(struct input_event);
+}
+
+#endif /* CONFIG_COMPAT */
+
+int input_event_from_user(const char __user *buffer,
+			 struct input_event *event);
+
+int input_event_to_user(char __user *buffer,
+			const struct input_event *event);
+
+int input_ff_effect_from_user(const char __user *buffer, size_t size,
+			      struct ff_effect *effect);
+
+#endif /* _INPUT_COMPAT_H */
diff --git a/drivers/input/input.c b/drivers/input/input.c
index c13ced3e0d3..1730d7331a5 100644
--- a/drivers/input/input.c
+++ b/drivers/input/input.c
@@ -1389,8 +1389,8 @@ int input_register_device(struct input_dev *dev)
 	if (!dev->setkeycode)
 		dev->setkeycode = input_default_setkeycode;
 
-	snprintf(dev->dev.bus_id, sizeof(dev->dev.bus_id),
-		 "input%ld", (unsigned long) atomic_inc_return(&input_no) - 1);
+	dev_set_name(&dev->dev, "input%ld",
+		     (unsigned long) atomic_inc_return(&input_no) - 1);
 
 	error = device_add(&dev->dev);
 	if (error)
diff --git a/drivers/input/joydev.c b/drivers/input/joydev.c
index a85b1485e77..6f2366220a5 100644
--- a/drivers/input/joydev.c
+++ b/drivers/input/joydev.c
@@ -800,7 +800,7 @@ static int joydev_connect(struct input_handler *handler, struct input_dev *dev,
 		}
 	}
 
-	strlcpy(joydev->dev.bus_id, joydev->name, sizeof(joydev->dev.bus_id));
+	dev_set_name(&joydev->dev, joydev->name);
 	joydev->dev.devt = MKDEV(INPUT_MAJOR, JOYDEV_MINOR_BASE + minor);
 	joydev->dev.class = &input_class;
 	joydev->dev.parent = &dev->dev;
diff --git a/drivers/input/joystick/Kconfig b/drivers/input/joystick/Kconfig
index be5c14a5a0a..b11419590cf 100644
--- a/drivers/input/joystick/Kconfig
+++ b/drivers/input/joystick/Kconfig
@@ -294,4 +294,28 @@ config JOYSTICK_XPAD_LEDS
 	  This option enables support for the LED which surrounds the Big X on
 	  XBox 360 controller.
 
+config JOYSTICK_WALKERA0701
+	tristate "Walkera WK-0701 RC transmitter"
+	depends on HIGH_RES_TIMERS && PARPORT
+	help
+	  Say Y or M here if you have a Walkera WK-0701 transmitter which is
+	  supplied with a ready to fly Walkera helicopters such as HM36,
+	  HM37, HM60 and want to use it via parport as a joystick. More
+	  information is available: <file:Documentation/input/walkera0701.txt>
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called walkera0701.
+
+config JOYSTICK_MAPLE
+	tristate "Dreamcast control pad"
+	depends on MAPLE
+	help
+	  Say Y here if you have a SEGA Dreamcast and want to use your
+	  controller as a joystick.
+
+	  Most Dreamcast users will say Y.
+
+	  To compile this as a module choose M here: the module will be called
+	  maplecontrol.
+
 endif
diff --git a/drivers/input/joystick/Makefile b/drivers/input/joystick/Makefile
index fdbf8c4c287..f3a8cbe2abb 100644
--- a/drivers/input/joystick/Makefile
+++ b/drivers/input/joystick/Makefile
@@ -19,6 +19,7 @@ obj-$(CONFIG_JOYSTICK_IFORCE)		+= iforce/
 obj-$(CONFIG_JOYSTICK_INTERACT)		+= interact.o
 obj-$(CONFIG_JOYSTICK_JOYDUMP)		+= joydump.o
 obj-$(CONFIG_JOYSTICK_MAGELLAN)		+= magellan.o
+obj-$(CONFIG_JOYSTICK_MAPLE)		+= maplecontrol.o
 obj-$(CONFIG_JOYSTICK_SIDEWINDER)	+= sidewinder.o
 obj-$(CONFIG_JOYSTICK_SPACEBALL)	+= spaceball.o
 obj-$(CONFIG_JOYSTICK_SPACEORB)		+= spaceorb.o
@@ -29,4 +30,5 @@ obj-$(CONFIG_JOYSTICK_TWIDJOY)		+= twidjoy.o
 obj-$(CONFIG_JOYSTICK_WARRIOR)		+= warrior.o
 obj-$(CONFIG_JOYSTICK_XPAD)		+= xpad.o
 obj-$(CONFIG_JOYSTICK_ZHENHUA)		+= zhenhua.o
+obj-$(CONFIG_JOYSTICK_WALKERA0701)	+= walkera0701.o
 
diff --git a/drivers/input/joystick/maplecontrol.c b/drivers/input/joystick/maplecontrol.c
new file mode 100644
index 00000000000..e50047bfe93
--- /dev/null
+++ b/drivers/input/joystick/maplecontrol.c
@@ -0,0 +1,193 @@
+/*
+ *	SEGA Dreamcast controller driver
+ *	Based on drivers/usb/iforce.c
+ *
+ *	Copyright Yaegashi Takeshi, 2001
+ *	Adrian McMenamin, 2008
+ */
+
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/input.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/timer.h>
+#include <linux/maple.h>
+
+MODULE_AUTHOR("Adrian McMenamin <adrian@mcmen.demon.co.uk>");
+MODULE_DESCRIPTION("SEGA Dreamcast controller driver");
+MODULE_LICENSE("GPL");
+
+struct dc_pad {
+	struct input_dev *dev;
+	struct maple_device *mdev;
+};
+
+static void dc_pad_callback(struct mapleq *mq)
+{
+	unsigned short buttons;
+	struct maple_device *mapledev = mq->dev;
+	struct dc_pad *pad = maple_get_drvdata(mapledev);
+	struct input_dev *dev = pad->dev;
+	unsigned char *res = mq->recvbuf;
+
+	buttons = ~le16_to_cpup((__le16 *)(res + 8));
+
+	input_report_abs(dev, ABS_HAT0Y,
+		(buttons & 0x0010 ? -1 : 0) + (buttons & 0x0020 ? 1 : 0));
+	input_report_abs(dev, ABS_HAT0X,
+		(buttons & 0x0040 ? -1 : 0) + (buttons & 0x0080 ? 1 : 0));
+	input_report_abs(dev, ABS_HAT1Y,
+		(buttons & 0x1000 ? -1 : 0) + (buttons & 0x2000 ? 1 : 0));
+	input_report_abs(dev, ABS_HAT1X,
+		(buttons & 0x4000 ? -1 : 0) + (buttons & 0x8000 ? 1 : 0));
+
+	input_report_key(dev, BTN_C,      buttons & 0x0001);
+	input_report_key(dev, BTN_B,      buttons & 0x0002);
+	input_report_key(dev, BTN_A,      buttons & 0x0004);
+	input_report_key(dev, BTN_START,  buttons & 0x0008);
+	input_report_key(dev, BTN_Z,      buttons & 0x0100);
+	input_report_key(dev, BTN_Y,      buttons & 0x0200);
+	input_report_key(dev, BTN_X,      buttons & 0x0400);
+	input_report_key(dev, BTN_SELECT, buttons & 0x0800);
+
+	input_report_abs(dev, ABS_GAS,    res[10]);
+	input_report_abs(dev, ABS_BRAKE,  res[11]);
+	input_report_abs(dev, ABS_X,      res[12]);
+	input_report_abs(dev, ABS_Y,      res[13]);
+	input_report_abs(dev, ABS_RX,     res[14]);
+	input_report_abs(dev, ABS_RY,     res[15]);
+}
+
+static int dc_pad_open(struct input_dev *dev)
+{
+	struct dc_pad *pad = dev->dev.platform_data;
+
+	maple_getcond_callback(pad->mdev, dc_pad_callback, HZ/20,
+		MAPLE_FUNC_CONTROLLER);
+
+	return 0;
+}
+
+static void dc_pad_close(struct input_dev *dev)
+{
+	struct dc_pad *pad = dev->dev.platform_data;
+
+	maple_getcond_callback(pad->mdev, dc_pad_callback, 0,
+		MAPLE_FUNC_CONTROLLER);
+}
+
+/* allow the controller to be used */
+static int __devinit probe_maple_controller(struct device *dev)
+{
+	static const short btn_bit[32] = {
+		BTN_C, BTN_B, BTN_A, BTN_START, -1, -1, -1, -1,
+		BTN_Z, BTN_Y, BTN_X, BTN_SELECT, -1, -1, -1, -1,
+		-1, -1, -1, -1, -1, -1, -1, -1,
+		-1, -1, -1, -1, -1, -1, -1, -1,
+	};
+
+	static const short abs_bit[32] = {
+		-1, -1, -1, -1, ABS_HAT0Y, ABS_HAT0Y, ABS_HAT0X, ABS_HAT0X,
+		-1, -1, -1, -1, ABS_HAT1Y, ABS_HAT1Y, ABS_HAT1X, ABS_HAT1X,
+		ABS_GAS, ABS_BRAKE, ABS_X, ABS_Y, ABS_RX, ABS_RY, -1, -1,
+		-1, -1, -1, -1, -1, -1, -1, -1,
+	};
+
+	struct maple_device *mdev = to_maple_dev(dev);
+	struct maple_driver *mdrv = to_maple_driver(dev->driver);
+	int i, error;
+	struct dc_pad *pad;
+	struct input_dev *idev;
+	unsigned long data = be32_to_cpu(mdev->devinfo.function_data[0]);
+
+	pad = kzalloc(sizeof(struct dc_pad), GFP_KERNEL);
+	idev = input_allocate_device();
+	if (!pad || !idev) {
+		error = -ENOMEM;
+		goto fail;
+	}
+
+	pad->dev = idev;
+	pad->mdev = mdev;
+
+	idev->open = dc_pad_open;
+	idev->close = dc_pad_close;
+
+	for (i = 0; i < 32; i++) {
+		if (data & (1 << i)) {
+			if (btn_bit[i] >= 0)
+				__set_bit(btn_bit[i], idev->keybit);
+			else if (abs_bit[i] >= 0)
+				__set_bit(abs_bit[i], idev->absbit);
+		}
+	}
+
+	if (idev->keybit[BIT_WORD(BTN_JOYSTICK)])
+		idev->evbit[0] |= BIT_MASK(EV_KEY);
+
+	if (idev->absbit[0])
+		idev->evbit[0] |= BIT_MASK(EV_ABS);
+
+	for (i = ABS_X; i <= ABS_BRAKE; i++)
+		input_set_abs_params(idev, i, 0, 255, 0, 0);
+
+	for (i = ABS_HAT0X; i <= ABS_HAT3Y; i++)
+		input_set_abs_params(idev, i, 1, -1, 0, 0);
+
+	idev->dev.platform_data = pad;
+	idev->dev.parent = &mdev->dev;
+	idev->name = mdev->product_name;
+	idev->id.bustype = BUS_HOST;
+	input_set_drvdata(idev, pad);
+
+	error = input_register_device(idev);
+	if (error)
+		goto fail;
+
+	mdev->driver = mdrv;
+	maple_set_drvdata(mdev, pad);
+
+	return 0;
+
+fail:
+	input_free_device(idev);
+	kfree(pad);
+	maple_set_drvdata(mdev, NULL);
+	return error;
+}
+
+static int __devexit remove_maple_controller(struct device *dev)
+{
+	struct maple_device *mdev = to_maple_dev(dev);
+	struct dc_pad *pad = maple_get_drvdata(mdev);
+
+	mdev->callback = NULL;
+	input_unregister_device(pad->dev);
+	maple_set_drvdata(mdev, NULL);
+	kfree(pad);
+
+	return 0;
+}
+
+static struct maple_driver dc_pad_driver = {
+	.function =	MAPLE_FUNC_CONTROLLER,
+	.drv = {
+		.name	= "Dreamcast_controller",
+		.probe	= probe_maple_controller,
+		.remove	= __devexit_p(remove_maple_controller),
+	},
+};
+
+static int __init dc_pad_init(void)
+{
+	return maple_driver_register(&dc_pad_driver);
+}
+
+static void __exit dc_pad_exit(void)
+{
+	maple_driver_unregister(&dc_pad_driver);
+}
+
+module_init(dc_pad_init);
+module_exit(dc_pad_exit);
diff --git a/drivers/input/joystick/walkera0701.c b/drivers/input/joystick/walkera0701.c
new file mode 100644
index 00000000000..4dfa1eed4b7
--- /dev/null
+++ b/drivers/input/joystick/walkera0701.c
@@ -0,0 +1,292 @@
+/*
+ *  Parallel port to Walkera WK-0701 TX joystick
+ *
+ *  Copyright (c) 2008 Peter Popovec
+ *
+ *  More about driver:  <file:Documentation/input/walkera0701.txt>
+ */
+
+/*
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+*/
+
+/* #define WK0701_DEBUG */
+
+#define RESERVE 20000
+#define SYNC_PULSE 1306000
+#define BIN0_PULSE 288000
+#define BIN1_PULSE 438000
+
+#define ANALOG_MIN_PULSE 318000
+#define ANALOG_MAX_PULSE 878000
+#define ANALOG_DELTA 80000
+
+#define BIN_SAMPLE ((BIN0_PULSE + BIN1_PULSE) / 2)
+
+#define NO_SYNC 25
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/parport.h>
+#include <linux/input.h>
+#include <linux/hrtimer.h>
+
+MODULE_AUTHOR("Peter Popovec <popovec@fei.tuke.sk>");
+MODULE_DESCRIPTION("Walkera WK-0701 TX as joystick");
+MODULE_LICENSE("GPL");
+
+static unsigned int walkera0701_pp_no;
+module_param_named(port, walkera0701_pp_no, int, 0);
+MODULE_PARM_DESC(port,
+		 "Parallel port adapter for Walkera WK-0701 TX (default is 0)");
+
+/*
+ * For now, only one device is supported, if somebody need more devices, code
+ * can be expanded, one struct walkera_dev per device must be allocated and
+ * set up by walkera0701_connect (release of device by walkera0701_disconnect)
+ */
+
+struct walkera_dev {
+	unsigned char buf[25];
+	u64 irq_time, irq_lasttime;
+	int counter;
+	int ack;
+
+	struct input_dev *input_dev;
+	struct hrtimer timer;
+
+	struct parport *parport;
+	struct pardevice *pardevice;
+};
+
+static struct walkera_dev w_dev;
+
+static inline void walkera0701_parse_frame(struct walkera_dev *w)
+{
+	int i;
+	int val1, val2, val3, val4, val5, val6, val7, val8;
+	int crc1, crc2;
+
+	for (crc1 = crc2 = i = 0; i < 10; i++) {
+		crc1 += w->buf[i] & 7;
+		crc2 += (w->buf[i] & 8) >> 3;
+	}
+	if ((w->buf[10] & 7) != (crc1 & 7))
+		return;
+	if (((w->buf[10] & 8) >> 3) != (((crc1 >> 3) + crc2) & 1))
+		return;
+	for (crc1 = crc2 = 0, i = 11; i < 23; i++) {
+		crc1 += w->buf[i] & 7;
+		crc2 += (w->buf[i] & 8) >> 3;
+	}
+	if ((w->buf[23] & 7) != (crc1 & 7))
+		return;
+	if (((w->buf[23] & 8) >> 3) != (((crc1 >> 3) + crc2) & 1))
+		return;
+	val1 = ((w->buf[0] & 7) * 256 + w->buf[1] * 16 + w->buf[2]) >> 2;
+	val1 *= ((w->buf[0] >> 2) & 2) - 1;	/* sign */
+	val2 = (w->buf[2] & 1) << 8 | (w->buf[3] << 4) | w->buf[4];
+	val2 *= (w->buf[2] & 2) - 1;	/* sign */
+	val3 = ((w->buf[5] & 7) * 256 + w->buf[6] * 16 + w->buf[7]) >> 2;
+	val3 *= ((w->buf[5] >> 2) & 2) - 1;	/* sign */
+	val4 = (w->buf[7] & 1) << 8 | (w->buf[8] << 4) | w->buf[9];
+	val4 *= (w->buf[7] & 2) - 1;	/* sign */
+	val5 = ((w->buf[11] & 7) * 256 + w->buf[12] * 16 + w->buf[13]) >> 2;
+	val5 *= ((w->buf[11] >> 2) & 2) - 1;	/* sign */
+	val6 = (w->buf[13] & 1) << 8 | (w->buf[14] << 4) | w->buf[15];
+	val6 *= (w->buf[13] & 2) - 1;	/* sign */
+	val7 = ((w->buf[16] & 7) * 256 + w->buf[17] * 16 + w->buf[18]) >> 2;
+	val7 *= ((w->buf[16] >> 2) & 2) - 1;	/*sign */
+	val8 = (w->buf[18] & 1) << 8 | (w->buf[19] << 4) | w->buf[20];
+	val8 *= (w->buf[18] & 2) - 1;	/*sign */
+
+#ifdef WK0701_DEBUG
+	{
+		int magic, magic_bit;
+		magic = (w->buf[21] << 4) | w->buf[22];
+		magic_bit = (w->buf[24] & 8) >> 3;
+		printk(KERN_DEBUG
+		       "walkera0701: %4d %4d %4d %4d  %4d %4d %4d %4d (magic %2x %d)\n",
+		       val1, val2, val3, val4, val5, val6, val7, val8, magic,
+		       magic_bit);
+	}
+#endif
+	input_report_abs(w->input_dev, ABS_X, val2);
+	input_report_abs(w->input_dev, ABS_Y, val1);
+	input_report_abs(w->input_dev, ABS_Z, val6);
+	input_report_abs(w->input_dev, ABS_THROTTLE, val3);
+	input_report_abs(w->input_dev, ABS_RUDDER, val4);
+	input_report_abs(w->input_dev, ABS_MISC, val7);
+	input_report_key(w->input_dev, BTN_GEAR_DOWN, val5 > 0);
+}
+
+static inline int read_ack(struct pardevice *p)
+{
+	return parport_read_status(p->port) & 0x40;
+}
+
+/* falling edge, prepare to BIN value calculation */
+static void walkera0701_irq_handler(void *handler_data)
+{
+	u64 pulse_time;
+	struct walkera_dev *w = handler_data;
+
+	w->irq_time = ktime_to_ns(ktime_get());
+	pulse_time = w->irq_time - w->irq_lasttime;
+	w->irq_lasttime = w->irq_time;
+
+	/* cancel timer, if in handler or active do resync */
+	if (unlikely(0 != hrtimer_try_to_cancel(&w->timer))) {
+		w->counter = NO_SYNC;
+		return;
+	}
+
+	if (w->counter < NO_SYNC) {
+		if (w->ack) {
+			pulse_time -= BIN1_PULSE;
+			w->buf[w->counter] = 8;
+		} else {
+			pulse_time -= BIN0_PULSE;
+			w->buf[w->counter] = 0;
+		}
+		if (w->counter == 24) {	/* full frame */
+			walkera0701_parse_frame(w);
+			w->counter = NO_SYNC;
+			if (abs(pulse_time - SYNC_PULSE) < RESERVE)	/* new frame sync */
+				w->counter = 0;
+		} else {
+			if ((pulse_time > (ANALOG_MIN_PULSE - RESERVE)
+			     && (pulse_time < (ANALOG_MAX_PULSE + RESERVE)))) {
+				pulse_time -= (ANALOG_MIN_PULSE - RESERVE);
+				pulse_time = (u32) pulse_time / ANALOG_DELTA;	/* overtiping is safe, pulsetime < s32.. */
+				w->buf[w->counter++] |= (pulse_time & 7);
+			} else
+				w->counter = NO_SYNC;
+		}
+	} else if (abs(pulse_time - SYNC_PULSE - BIN0_PULSE) <
+				RESERVE + BIN1_PULSE - BIN0_PULSE)	/* frame sync .. */
+		w->counter = 0;
+
+	hrtimer_start(&w->timer, ktime_set(0, BIN_SAMPLE), HRTIMER_MODE_REL);
+}
+
+static enum hrtimer_restart timer_handler(struct hrtimer
+					  *handle)
+{
+	struct walkera_dev *w;
+
+	w = container_of(handle, struct walkera_dev, timer);
+	w->ack = read_ack(w->pardevice);
+
+	return HRTIMER_NORESTART;
+}
+
+static int walkera0701_open(struct input_dev *dev)
+{
+	struct walkera_dev *w = input_get_drvdata(dev);
+
+	parport_enable_irq(w->parport);
+	return 0;
+}
+
+static void walkera0701_close(struct input_dev *dev)
+{
+	struct walkera_dev *w = input_get_drvdata(dev);
+
+	parport_disable_irq(w->parport);
+}
+
+static int walkera0701_connect(struct walkera_dev *w, int parport)
+{
+	int err = -ENODEV;
+
+	w->parport = parport_find_number(parport);
+	if (w->parport == NULL)
+		return -ENODEV;
+
+	if (w->parport->irq == -1) {
+		printk(KERN_ERR "walkera0701: parport without interrupt\n");
+		goto init_err;
+	}
+
+	err = -EBUSY;
+	w->pardevice = parport_register_device(w->parport, "walkera0701",
+				    NULL, NULL, walkera0701_irq_handler,
+				    PARPORT_DEV_EXCL, w);
+	if (!w->pardevice)
+		goto init_err;
+
+	if (parport_negotiate(w->pardevice->port, IEEE1284_MODE_COMPAT))
+		goto init_err1;
+
+	if (parport_claim(w->pardevice))
+		goto init_err1;
+
+	w->input_dev = input_allocate_device();
+	if (!w->input_dev)
+		goto init_err2;
+
+	input_set_drvdata(w->input_dev, w);
+	w->input_dev->name = "Walkera WK-0701 TX";
+	w->input_dev->phys = w->parport->name;
+	w->input_dev->id.bustype = BUS_PARPORT;
+
+	/* TODO what id vendor/product/version ? */
+	w->input_dev->id.vendor = 0x0001;
+	w->input_dev->id.product = 0x0001;
+	w->input_dev->id.version = 0x0100;
+	w->input_dev->open = walkera0701_open;
+	w->input_dev->close = walkera0701_close;
+
+	w->input_dev->evbit[0] = BIT(EV_ABS) | BIT_MASK(EV_KEY);
+	w->input_dev->keybit[BIT_WORD(BTN_GEAR_DOWN)] = BIT_MASK(BTN_GEAR_DOWN);
+
+	input_set_abs_params(w->input_dev, ABS_X, -512, 512, 0, 0);
+	input_set_abs_params(w->input_dev, ABS_Y, -512, 512, 0, 0);
+	input_set_abs_params(w->input_dev, ABS_Z, -512, 512, 0, 0);
+	input_set_abs_params(w->input_dev, ABS_THROTTLE, -512, 512, 0, 0);
+	input_set_abs_params(w->input_dev, ABS_RUDDER, -512, 512, 0, 0);
+	input_set_abs_params(w->input_dev, ABS_MISC, -512, 512, 0, 0);
+
+	err = input_register_device(w->input_dev);
+	if (err)
+		goto init_err3;
+
+	hrtimer_init(&w->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	w->timer.function = timer_handler;
+	return 0;
+
+ init_err3:
+	input_free_device(w->input_dev);
+ init_err2:
+	parport_release(w->pardevice);
+ init_err1:
+	parport_unregister_device(w->pardevice);
+ init_err:
+	parport_put_port(w->parport);
+	return err;
+}
+
+static void walkera0701_disconnect(struct walkera_dev *w)
+{
+	hrtimer_cancel(&w->timer);
+	input_unregister_device(w->input_dev);
+	parport_release(w->pardevice);
+	parport_unregister_device(w->pardevice);
+	parport_put_port(w->parport);
+}
+
+static int __init walkera0701_init(void)
+{
+	return walkera0701_connect(&w_dev, walkera0701_pp_no);
+}
+
+static void __exit walkera0701_exit(void)
+{
+	walkera0701_disconnect(&w_dev);
+}
+
+module_init(walkera0701_init);
+module_exit(walkera0701_exit);
diff --git a/drivers/input/keyboard/Kconfig b/drivers/input/keyboard/Kconfig
index efd70a97459..35561689ff3 100644
--- a/drivers/input/keyboard/Kconfig
+++ b/drivers/input/keyboard/Kconfig
@@ -268,6 +268,15 @@ config KEYBOARD_PXA27x
 	  To compile this driver as a module, choose M here: the
 	  module will be called pxa27x_keypad.
 
+config KEYBOARD_PXA930_ROTARY
+	tristate "PXA930/PXA935 Enhanced Rotary Controller Support"
+	depends on CPU_PXA930 || CPU_PXA935
+	help
+	  Enable support for PXA930/PXA935 Enhanced Rotary Controller.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called pxa930_rotary.
+
 config KEYBOARD_AAED2000
 	tristate "AAED-2000 keyboard"
 	depends on MACH_AAED2000
diff --git a/drivers/input/keyboard/Makefile b/drivers/input/keyboard/Makefile
index 0edc8f285d1..36351e1190f 100644
--- a/drivers/input/keyboard/Makefile
+++ b/drivers/input/keyboard/Makefile
@@ -20,6 +20,7 @@ obj-$(CONFIG_KEYBOARD_HIL)		+= hil_kbd.o
 obj-$(CONFIG_KEYBOARD_HIL_OLD)		+= hilkbd.o
 obj-$(CONFIG_KEYBOARD_OMAP)		+= omap-keypad.o
 obj-$(CONFIG_KEYBOARD_PXA27x)		+= pxa27x_keypad.o
+obj-$(CONFIG_KEYBOARD_PXA930_ROTARY)	+= pxa930_rotary.o
 obj-$(CONFIG_KEYBOARD_AAED2000)		+= aaed2000_kbd.o
 obj-$(CONFIG_KEYBOARD_GPIO)		+= gpio_keys.o
 obj-$(CONFIG_KEYBOARD_HP6XX)		+= jornada680_kbd.o
diff --git a/drivers/input/keyboard/atkbd.c b/drivers/input/keyboard/atkbd.c
index 379b7ff354e..f6e9f39a527 100644
--- a/drivers/input/keyboard/atkbd.c
+++ b/drivers/input/keyboard/atkbd.c
@@ -884,6 +884,39 @@ static void atkbd_inventec_keymap_fixup(struct atkbd *atkbd)
 }
 
 /*
+ * Perform fixup for HP Pavilion ZV6100 laptop that doesn't generate release
+ * for its volume buttons
+ */
+static void atkbd_hp_zv6100_keymap_fixup(struct atkbd *atkbd)
+{
+	const unsigned int forced_release_keys[] = {
+		0xae, 0xb0,
+	};
+	int i;
+
+	if (atkbd->set == 2)
+		for (i = 0; i < ARRAY_SIZE(forced_release_keys); i++)
+			__set_bit(forced_release_keys[i],
+					atkbd->force_release_mask);
+}
+
+/*
+ * Samsung NC10 with Fn+F? key release not working
+ */
+static void atkbd_samsung_keymap_fixup(struct atkbd *atkbd)
+{
+	const unsigned int forced_release_keys[] = {
+		0x82, 0x83, 0x84, 0x86, 0x88, 0x89, 0xb3, 0xf7, 0xf9,
+	};
+	int i;
+
+	if (atkbd->set == 2)
+		for (i = 0; i < ARRAY_SIZE(forced_release_keys); i++)
+			__set_bit(forced_release_keys[i],
+				  atkbd->force_release_mask);
+}
+
+/*
  * atkbd_set_keycode_table() initializes keyboard's keycode table
  * according to the selected scancode set
  */
@@ -1476,6 +1509,15 @@ static struct dmi_system_id atkbd_dmi_quirk_table[] __initdata = {
 		.driver_data = atkbd_dell_laptop_keymap_fixup,
 	},
 	{
+		.ident = "Dell Laptop",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "Dell Computer Corporation"),
+			DMI_MATCH(DMI_CHASSIS_TYPE, "8"), /* Portable */
+		},
+		.callback = atkbd_setup_fixup,
+		.driver_data = atkbd_dell_laptop_keymap_fixup,
+	},
+	{
 		.ident = "HP 2133",
 		.matches = {
 			DMI_MATCH(DMI_SYS_VENDOR, "Hewlett-Packard"),
@@ -1485,6 +1527,15 @@ static struct dmi_system_id atkbd_dmi_quirk_table[] __initdata = {
 		.driver_data = atkbd_hp_keymap_fixup,
 	},
 	{
+		.ident = "HP Pavilion ZV6100",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "Hewlett-Packard"),
+			DMI_MATCH(DMI_PRODUCT_NAME, "Pavilion ZV6100"),
+		},
+		.callback = atkbd_setup_fixup,
+		.driver_data = atkbd_hp_zv6100_keymap_fixup,
+	},
+	{
 		.ident = "Inventec Symphony",
 		.matches = {
 			DMI_MATCH(DMI_SYS_VENDOR, "INVENTEC"),
@@ -1493,6 +1544,15 @@ static struct dmi_system_id atkbd_dmi_quirk_table[] __initdata = {
 		.callback = atkbd_setup_fixup,
 		.driver_data = atkbd_inventec_keymap_fixup,
 	},
+	{
+		.ident = "Samsung NC10",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "SAMSUNG ELECTRONICS CO., LTD."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "NC10"),
+		},
+		.callback = atkbd_setup_fixup,
+		.driver_data = atkbd_samsung_keymap_fixup,
+	},
 	{ }
 };
 
diff --git a/drivers/input/keyboard/gpio_keys.c b/drivers/input/keyboard/gpio_keys.c
index 05f3f43582c..ad67d763fdb 100644
--- a/drivers/input/keyboard/gpio_keys.c
+++ b/drivers/input/keyboard/gpio_keys.c
@@ -98,6 +98,10 @@ static int __devinit gpio_keys_probe(struct platform_device *pdev)
 	input->id.product = 0x0001;
 	input->id.version = 0x0100;
 
+	/* Enable auto repeat feature of Linux input subsystem */
+	if (pdata->rep)
+		__set_bit(EV_REP, input->evbit);
+
 	ddata->input = input;
 
 	for (i = 0; i < pdata->nbuttons; i++) {
diff --git a/drivers/input/keyboard/hil_kbd.c b/drivers/input/keyboard/hil_kbd.c
index 71c1971abf8..6f356705ee3 100644
--- a/drivers/input/keyboard/hil_kbd.c
+++ b/drivers/input/keyboard/hil_kbd.c
@@ -47,6 +47,7 @@
 MODULE_AUTHOR("Brian S. Julin <bri@calyx.com>");
 MODULE_DESCRIPTION(HIL_GENERIC_NAME " driver");
 MODULE_LICENSE("Dual BSD/GPL");
+MODULE_ALIAS("serio:ty03pr25id00ex*");
 
 #define HIL_KBD_MAX_LENGTH 16
 
diff --git a/drivers/input/keyboard/omap-keypad.c b/drivers/input/keyboard/omap-keypad.c
index db22fd9b4cf..3f3d1198cdb 100644
--- a/drivers/input/keyboard/omap-keypad.c
+++ b/drivers/input/keyboard/omap-keypad.c
@@ -122,14 +122,10 @@ static void omap_kp_scan_keypad(struct omap_kp *omap_kp, unsigned char *state)
 
 	/* read the keypad status */
 	if (cpu_is_omap24xx()) {
-		int i;
-		for (i = 0; i < omap_kp->rows; i++)
-			disable_irq(OMAP_GPIO_IRQ(row_gpios[i]));
-
 		/* read the keypad status */
 		for (col = 0; col < omap_kp->cols; col++) {
 			set_col_gpio_val(omap_kp, ~(1 << col));
-			state[col] = ~(get_row_gpio_val(omap_kp)) & 0x3f;
+			state[col] = ~(get_row_gpio_val(omap_kp)) & 0xff;
 		}
 		set_col_gpio_val(omap_kp, 0);
 
diff --git a/drivers/input/keyboard/pxa930_rotary.c b/drivers/input/keyboard/pxa930_rotary.c
new file mode 100644
index 00000000000..95fbba470e6
--- /dev/null
+++ b/drivers/input/keyboard/pxa930_rotary.c
@@ -0,0 +1,212 @@
+/*
+ * Driver for the enhanced rotary controller on pxa930 and pxa935
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/input.h>
+#include <linux/platform_device.h>
+#include <linux/io.h>
+
+#include <mach/pxa930_rotary.h>
+
+#define SBCR	(0x04)
+#define ERCR	(0x0c)
+
+#define SBCR_ERSB	(1 << 5)
+
+struct pxa930_rotary {
+	struct input_dev	*input_dev;
+	void __iomem		*mmio_base;
+	int			last_ercr;
+
+	struct pxa930_rotary_platform_data *pdata;
+};
+
+static void clear_sbcr(struct pxa930_rotary *r)
+{
+	uint32_t sbcr = __raw_readl(r->mmio_base + SBCR);
+
+	__raw_writel(sbcr | SBCR_ERSB, r->mmio_base + SBCR);
+	__raw_writel(sbcr & ~SBCR_ERSB, r->mmio_base + SBCR);
+}
+
+static irqreturn_t rotary_irq(int irq, void *dev_id)
+{
+	struct pxa930_rotary *r = dev_id;
+	struct pxa930_rotary_platform_data *pdata = r->pdata;
+	int ercr, delta, key;
+
+	ercr = __raw_readl(r->mmio_base + ERCR) & 0xf;
+	clear_sbcr(r);
+
+	delta = ercr - r->last_ercr;
+	if (delta == 0)
+		return IRQ_HANDLED;
+
+	r->last_ercr = ercr;
+
+	if (pdata->up_key && pdata->down_key) {
+		key = (delta > 0) ? pdata->up_key : pdata->down_key;
+		input_report_key(r->input_dev, key, 1);
+		input_sync(r->input_dev);
+		input_report_key(r->input_dev, key, 0);
+	} else
+		input_report_rel(r->input_dev, pdata->rel_code, delta);
+
+	input_sync(r->input_dev);
+
+	return IRQ_HANDLED;
+}
+
+static int pxa930_rotary_open(struct input_dev *dev)
+{
+	struct pxa930_rotary *r = input_get_drvdata(dev);
+
+	clear_sbcr(r);
+
+	return 0;
+}
+
+static void pxa930_rotary_close(struct input_dev *dev)
+{
+	struct pxa930_rotary *r = input_get_drvdata(dev);
+
+	clear_sbcr(r);
+}
+
+static int __devinit pxa930_rotary_probe(struct platform_device *pdev)
+{
+	struct pxa930_rotary_platform_data *pdata = pdev->dev.platform_data;
+	struct pxa930_rotary *r;
+	struct input_dev *input_dev;
+	struct resource *res;
+	int irq;
+	int err;
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		dev_err(&pdev->dev, "no irq for rotary controller\n");
+		return -ENXIO;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "no I/O memory defined\n");
+		return -ENXIO;
+	}
+
+	if (!pdata) {
+		dev_err(&pdev->dev, "no platform data defined\n");
+		return -EINVAL;
+	}
+
+	r = kzalloc(sizeof(struct pxa930_rotary), GFP_KERNEL);
+	if (!r)
+		return -ENOMEM;
+
+	r->mmio_base = ioremap_nocache(res->start, resource_size(res));
+	if (r->mmio_base == NULL) {
+		dev_err(&pdev->dev, "failed to remap IO memory\n");
+		err = -ENXIO;
+		goto failed_free;
+	}
+
+	r->pdata = pdata;
+	platform_set_drvdata(pdev, r);
+
+	/* allocate and register the input device */
+	input_dev = input_allocate_device();
+	if (!input_dev) {
+		dev_err(&pdev->dev, "failed to allocate input device\n");
+		err = -ENOMEM;
+		goto failed_free_io;
+	}
+
+	input_dev->name = pdev->name;
+	input_dev->id.bustype = BUS_HOST;
+	input_dev->open = pxa930_rotary_open;
+	input_dev->close = pxa930_rotary_close;
+	input_dev->dev.parent = &pdev->dev;
+
+	if (pdata->up_key && pdata->down_key) {
+		__set_bit(pdata->up_key, input_dev->keybit);
+		__set_bit(pdata->down_key, input_dev->keybit);
+		__set_bit(EV_KEY, input_dev->evbit);
+	} else {
+		__set_bit(pdata->rel_code, input_dev->relbit);
+		__set_bit(EV_REL, input_dev->evbit);
+	}
+
+	r->input_dev = input_dev;
+	input_set_drvdata(input_dev, r);
+
+	err = request_irq(irq, rotary_irq, IRQF_DISABLED,
+			"enhanced rotary", r);
+	if (err) {
+		dev_err(&pdev->dev, "failed to request IRQ\n");
+		goto failed_free_input;
+	}
+
+	err = input_register_device(input_dev);
+	if (err) {
+		dev_err(&pdev->dev, "failed to register input device\n");
+		goto failed_free_irq;
+	}
+
+	return 0;
+
+failed_free_irq:
+	free_irq(irq, r);
+failed_free_input:
+	input_free_device(input_dev);
+failed_free_io:
+	iounmap(r->mmio_base);
+failed_free:
+	kfree(r);
+	return err;
+}
+
+static int __devexit pxa930_rotary_remove(struct platform_device *pdev)
+{
+	struct pxa930_rotary *r = platform_get_drvdata(pdev);
+
+	free_irq(platform_get_irq(pdev, 0), r);
+	input_unregister_device(r->input_dev);
+	iounmap(r->mmio_base);
+	platform_set_drvdata(pdev, NULL);
+	kfree(r);
+
+	return 0;
+}
+
+static struct platform_driver pxa930_rotary_driver = {
+	.driver		= {
+		.name	= "pxa930-rotary",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= pxa930_rotary_probe,
+	.remove		= __devexit_p(pxa930_rotary_remove),
+};
+
+static int __init pxa930_rotary_init(void)
+{
+	return platform_driver_register(&pxa930_rotary_driver);
+}
+module_init(pxa930_rotary_init);
+
+static void __exit pxa930_rotary_exit(void)
+{
+	platform_driver_unregister(&pxa930_rotary_driver);
+}
+module_exit(pxa930_rotary_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Driver for PXA93x Enhanced Rotary Controller");
+MODULE_AUTHOR("Yao Yong <yaoyong@marvell.com>");
diff --git a/drivers/input/misc/pcspkr.c b/drivers/input/misc/pcspkr.c
index 43aaa5cebd1..d6a30cee7bc 100644
--- a/drivers/input/misc/pcspkr.c
+++ b/drivers/input/misc/pcspkr.c
@@ -52,13 +52,13 @@ static int pcspkr_event(struct input_dev *dev, unsigned int type, unsigned int c
 	spin_lock_irqsave(&i8253_lock, flags);
 
 	if (count) {
-		/* enable counter 2 */
-		outb_p(inb_p(0x61) | 3, 0x61);
 		/* set command for counter 2, 2 byte write */
 		outb_p(0xB6, 0x43);
 		/* select desired HZ */
 		outb_p(count & 0xff, 0x42);
 		outb((count >> 8) & 0xff, 0x42);
+		/* enable counter 2 */
+		outb_p(inb_p(0x61) | 3, 0x61);
 	} else {
 		/* disable counter 2 */
 		outb(inb_p(0x61) & 0xFC, 0x61);
diff --git a/drivers/input/misc/uinput.c b/drivers/input/misc/uinput.c
index 223d56d5555..46b7caeb281 100644
--- a/drivers/input/misc/uinput.c
+++ b/drivers/input/misc/uinput.c
@@ -37,6 +37,7 @@
 #include <linux/fs.h>
 #include <linux/miscdevice.h>
 #include <linux/uinput.h>
+#include "../input-compat.h"
 
 static int uinput_dev_event(struct input_dev *dev, unsigned int type, unsigned int code, int value)
 {
@@ -78,6 +79,7 @@ static struct uinput_request* uinput_request_find(struct uinput_device *udev, in
 	/* Find an input request, by ID. Returns NULL if the ID isn't valid. */
 	if (id >= UINPUT_NUM_REQUESTS || id < 0)
 		return NULL;
+
 	return udev->requests[id];
 }
 
@@ -127,6 +129,17 @@ static int uinput_dev_upload_effect(struct input_dev *dev, struct ff_effect *eff
 	struct uinput_request request;
 	int retval;
 
+	/*
+	 * uinput driver does not currently support periodic effects with
+	 * custom waveform since it does not have a way to pass buffer of
+	 * samples (custom_data) to userspace. If ever there is a device
+	 * supporting custom waveforms we would need to define an additional
+	 * ioctl (UI_UPLOAD_SAMPLES) but for now we just bail out.
+	 */
+	if (effect->type == FF_PERIODIC &&
+			effect->u.periodic.waveform == FF_CUSTOM)
+		return -EINVAL;
+
 	request.id = -1;
 	init_completion(&request.done);
 	request.code = UI_FF_UPLOAD;
@@ -353,15 +366,15 @@ static inline ssize_t uinput_inject_event(struct uinput_device *udev, const char
 {
 	struct input_event ev;
 
-	if (count != sizeof(struct input_event))
+	if (count < input_event_size())
 		return -EINVAL;
 
-	if (copy_from_user(&ev, buffer, sizeof(struct input_event)))
+	if (input_event_from_user(buffer, &ev))
 		return -EFAULT;
 
 	input_event(udev->dev, ev.type, ev.code, ev.value);
 
-	return sizeof(struct input_event);
+	return input_event_size();
 }
 
 static ssize_t uinput_write(struct file *file, const char __user *buffer, size_t count, loff_t *ppos)
@@ -407,13 +420,13 @@ static ssize_t uinput_read(struct file *file, char __user *buffer, size_t count,
 		goto out;
 	}
 
-	while (udev->head != udev->tail && retval + sizeof(struct input_event) <= count) {
-		if (copy_to_user(buffer + retval, &udev->buff[udev->tail], sizeof(struct input_event))) {
+	while (udev->head != udev->tail && retval + input_event_size() <= count) {
+		if (input_event_to_user(buffer + retval, &udev->buff[udev->tail])) {
 			retval = -EFAULT;
 			goto out;
 		}
 		udev->tail = (udev->tail + 1) % UINPUT_BUFFER_SIZE;
-		retval += sizeof(struct input_event);
+		retval += input_event_size();
 	}
 
  out:
@@ -444,6 +457,93 @@ static int uinput_release(struct inode *inode, struct file *file)
 	return 0;
 }
 
+#ifdef CONFIG_COMPAT
+struct uinput_ff_upload_compat {
+	int			request_id;
+	int			retval;
+	struct ff_effect_compat	effect;
+	struct ff_effect_compat	old;
+};
+
+static int uinput_ff_upload_to_user(char __user *buffer,
+				    const struct uinput_ff_upload *ff_up)
+{
+	if (INPUT_COMPAT_TEST) {
+		struct uinput_ff_upload_compat ff_up_compat;
+
+		ff_up_compat.request_id = ff_up->request_id;
+		ff_up_compat.retval = ff_up->retval;
+		/*
+		 * It so happens that the pointer that gives us the trouble
+		 * is the last field in the structure. Since we don't support
+		 * custom waveforms in uinput anyway we can just copy the whole
+		 * thing (to the compat size) and ignore the pointer.
+		 */
+		memcpy(&ff_up_compat.effect, &ff_up->effect,
+			sizeof(struct ff_effect_compat));
+		memcpy(&ff_up_compat.old, &ff_up->old,
+			sizeof(struct ff_effect_compat));
+
+		if (copy_to_user(buffer, &ff_up_compat,
+				 sizeof(struct uinput_ff_upload_compat)))
+			return -EFAULT;
+	} else {
+		if (copy_to_user(buffer, ff_up,
+				 sizeof(struct uinput_ff_upload)))
+			return -EFAULT;
+	}
+
+	return 0;
+}
+
+static int uinput_ff_upload_from_user(const char __user *buffer,
+				      struct uinput_ff_upload *ff_up)
+{
+	if (INPUT_COMPAT_TEST) {
+		struct uinput_ff_upload_compat ff_up_compat;
+
+		if (copy_from_user(&ff_up_compat, buffer,
+				   sizeof(struct uinput_ff_upload_compat)))
+			return -EFAULT;
+
+		ff_up->request_id = ff_up_compat.request_id;
+		ff_up->retval = ff_up_compat.retval;
+		memcpy(&ff_up->effect, &ff_up_compat.effect,
+			sizeof(struct ff_effect_compat));
+		memcpy(&ff_up->old, &ff_up_compat.old,
+			sizeof(struct ff_effect_compat));
+
+	} else {
+		if (copy_from_user(ff_up, buffer,
+				   sizeof(struct uinput_ff_upload)))
+			return -EFAULT;
+	}
+
+	return 0;
+}
+
+#else
+
+static int uinput_ff_upload_to_user(char __user *buffer,
+				    const struct uinput_ff_upload *ff_up)
+{
+	if (copy_to_user(buffer, ff_up, sizeof(struct uinput_ff_upload)))
+		return -EFAULT;
+
+	return 0;
+}
+
+static int uinput_ff_upload_from_user(const char __user *buffer,
+				      struct uinput_ff_upload *ff_up)
+{
+	if (copy_from_user(ff_up, buffer, sizeof(struct uinput_ff_upload)))
+		return -EFAULT;
+
+	return 0;
+}
+
+#endif
+
 #define uinput_set_bit(_arg, _bit, _max)		\
 ({							\
 	int __ret = 0;					\
@@ -455,19 +555,17 @@ static int uinput_release(struct inode *inode, struct file *file)
 	__ret;						\
 })
 
-static long uinput_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+static long uinput_ioctl_handler(struct file *file, unsigned int cmd,
+				 unsigned long arg, void __user *p)
 {
 	int			retval;
-	struct uinput_device	*udev;
-	void __user             *p = (void __user *)arg;
+	struct uinput_device	*udev = file->private_data;
 	struct uinput_ff_upload ff_up;
 	struct uinput_ff_erase  ff_erase;
 	struct uinput_request   *req;
 	int                     length;
 	char			*phys;
 
-	udev = file->private_data;
-
 	retval = mutex_lock_interruptible(&udev->mutex);
 	if (retval)
 		return retval;
@@ -549,26 +647,24 @@ static long uinput_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 			break;
 
 		case UI_BEGIN_FF_UPLOAD:
-			if (copy_from_user(&ff_up, p, sizeof(ff_up))) {
-				retval = -EFAULT;
+			retval = uinput_ff_upload_from_user(p, &ff_up);
+			if (retval)
 				break;
-			}
+
 			req = uinput_request_find(udev, ff_up.request_id);
-			if (!(req && req->code == UI_FF_UPLOAD && req->u.upload.effect)) {
+			if (!req || req->code != UI_FF_UPLOAD || !req->u.upload.effect) {
 				retval = -EINVAL;
 				break;
 			}
+
 			ff_up.retval = 0;
-			memcpy(&ff_up.effect, req->u.upload.effect, sizeof(struct ff_effect));
+			ff_up.effect = *req->u.upload.effect;
 			if (req->u.upload.old)
-				memcpy(&ff_up.old, req->u.upload.old, sizeof(struct ff_effect));
+				ff_up.old = *req->u.upload.old;
 			else
 				memset(&ff_up.old, 0, sizeof(struct ff_effect));
 
-			if (copy_to_user(p, &ff_up, sizeof(ff_up))) {
-				retval = -EFAULT;
-				break;
-			}
+			retval = uinput_ff_upload_to_user(p, &ff_up);
 			break;
 
 		case UI_BEGIN_FF_ERASE:
@@ -576,29 +672,34 @@ static long uinput_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 				retval = -EFAULT;
 				break;
 			}
+
 			req = uinput_request_find(udev, ff_erase.request_id);
-			if (!(req && req->code == UI_FF_ERASE)) {
+			if (!req || req->code != UI_FF_ERASE) {
 				retval = -EINVAL;
 				break;
 			}
+
 			ff_erase.retval = 0;
 			ff_erase.effect_id = req->u.effect_id;
 			if (copy_to_user(p, &ff_erase, sizeof(ff_erase))) {
 				retval = -EFAULT;
 				break;
 			}
+
 			break;
 
 		case UI_END_FF_UPLOAD:
-			if (copy_from_user(&ff_up, p, sizeof(ff_up))) {
-				retval = -EFAULT;
+			retval = uinput_ff_upload_from_user(p, &ff_up);
+			if (retval)
 				break;
-			}
+
 			req = uinput_request_find(udev, ff_up.request_id);
-			if (!(req && req->code == UI_FF_UPLOAD && req->u.upload.effect)) {
+			if (!req || req->code != UI_FF_UPLOAD ||
+			    !req->u.upload.effect) {
 				retval = -EINVAL;
 				break;
 			}
+
 			req->retval = ff_up.retval;
 			uinput_request_done(udev, req);
 			break;
@@ -608,11 +709,13 @@ static long uinput_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 				retval = -EFAULT;
 				break;
 			}
+
 			req = uinput_request_find(udev, ff_erase.request_id);
-			if (!(req && req->code == UI_FF_ERASE)) {
+			if (!req || req->code != UI_FF_ERASE) {
 				retval = -EINVAL;
 				break;
 			}
+
 			req->retval = ff_erase.retval;
 			uinput_request_done(udev, req);
 			break;
@@ -626,6 +729,18 @@ static long uinput_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 	return retval;
 }
 
+static long uinput_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	return uinput_ioctl_handler(file, cmd, arg, (void __user *)arg);
+}
+
+#ifdef CONFIG_COMPAT
+static long uinput_compat_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	return uinput_ioctl_handler(file, cmd, arg, compat_ptr(arg));
+}
+#endif
+
 static const struct file_operations uinput_fops = {
 	.owner		= THIS_MODULE,
 	.open		= uinput_open,
@@ -634,6 +749,9 @@ static const struct file_operations uinput_fops = {
 	.write		= uinput_write,
 	.poll		= uinput_poll,
 	.unlocked_ioctl	= uinput_ioctl,
+#ifdef CONFIG_COMPAT
+	.compat_ioctl	= uinput_compat_ioctl,
+#endif
 };
 
 static struct miscdevice uinput_misc = {
diff --git a/drivers/input/mouse/Kconfig b/drivers/input/mouse/Kconfig
index 4e993425977..093c8c1bca7 100644
--- a/drivers/input/mouse/Kconfig
+++ b/drivers/input/mouse/Kconfig
@@ -286,4 +286,10 @@ config MOUSE_GPIO
 	  To compile this driver as a module, choose M here: the
 	  module will be called gpio_mouse.
 
+config MOUSE_PXA930_TRKBALL
+	tristate "PXA930 Trackball mouse"
+	depends on CPU_PXA930 || CPU_PXA935
+	help
+	  Say Y here to support PXA930 Trackball mouse.
+
 endif
diff --git a/drivers/input/mouse/Makefile b/drivers/input/mouse/Makefile
index 96f1dd8037f..8c8a1f236e2 100644
--- a/drivers/input/mouse/Makefile
+++ b/drivers/input/mouse/Makefile
@@ -4,19 +4,20 @@
 
 # Each configuration option enables a list of files.
 
-obj-$(CONFIG_MOUSE_AMIGA)	+= amimouse.o
-obj-$(CONFIG_MOUSE_APPLETOUCH)	+= appletouch.o
-obj-$(CONFIG_MOUSE_BCM5974)	+= bcm5974.o
-obj-$(CONFIG_MOUSE_ATARI)	+= atarimouse.o
-obj-$(CONFIG_MOUSE_RISCPC)	+= rpcmouse.o
-obj-$(CONFIG_MOUSE_INPORT)	+= inport.o
-obj-$(CONFIG_MOUSE_LOGIBM)	+= logibm.o
-obj-$(CONFIG_MOUSE_PC110PAD)	+= pc110pad.o
-obj-$(CONFIG_MOUSE_PS2)		+= psmouse.o
-obj-$(CONFIG_MOUSE_SERIAL)	+= sermouse.o
-obj-$(CONFIG_MOUSE_HIL)		+= hil_ptr.o
-obj-$(CONFIG_MOUSE_VSXXXAA)	+= vsxxxaa.o
-obj-$(CONFIG_MOUSE_GPIO)	+= gpio_mouse.o
+obj-$(CONFIG_MOUSE_AMIGA)		+= amimouse.o
+obj-$(CONFIG_MOUSE_APPLETOUCH)		+= appletouch.o
+obj-$(CONFIG_MOUSE_BCM5974)		+= bcm5974.o
+obj-$(CONFIG_MOUSE_ATARI)		+= atarimouse.o
+obj-$(CONFIG_MOUSE_RISCPC)		+= rpcmouse.o
+obj-$(CONFIG_MOUSE_INPORT)		+= inport.o
+obj-$(CONFIG_MOUSE_LOGIBM)		+= logibm.o
+obj-$(CONFIG_MOUSE_PC110PAD)		+= pc110pad.o
+obj-$(CONFIG_MOUSE_PS2)			+= psmouse.o
+obj-$(CONFIG_MOUSE_PXA930_TRKBALL)	+= pxa930_trkball.o
+obj-$(CONFIG_MOUSE_SERIAL)		+= sermouse.o
+obj-$(CONFIG_MOUSE_HIL)			+= hil_ptr.o
+obj-$(CONFIG_MOUSE_VSXXXAA)		+= vsxxxaa.o
+obj-$(CONFIG_MOUSE_GPIO)		+= gpio_mouse.o
 
 psmouse-objs := psmouse-base.o synaptics.o
 
diff --git a/drivers/input/mouse/appletouch.c b/drivers/input/mouse/appletouch.c
index 079816e6b23..454b96112f0 100644
--- a/drivers/input/mouse/appletouch.c
+++ b/drivers/input/mouse/appletouch.c
@@ -3,7 +3,7 @@
  *
  * Copyright (C) 2001-2004 Greg Kroah-Hartman (greg@kroah.com)
  * Copyright (C) 2005-2008 Johannes Berg (johannes@sipsolutions.net)
- * Copyright (C) 2005      Stelian Pop (stelian@popies.net)
+ * Copyright (C) 2005-2008 Stelian Pop (stelian@popies.net)
  * Copyright (C) 2005      Frank Arnold (frank@scirocco-5v-turbo.de)
  * Copyright (C) 2005      Peter Osterlund (petero2@telia.com)
  * Copyright (C) 2005      Michael Hanselmann (linux-kernel@hansmi.ch)
@@ -35,16 +35,74 @@
 #include <linux/module.h>
 #include <linux/usb/input.h>
 
-/* Type of touchpad */
-enum atp_touchpad_type {
-	ATP_FOUNTAIN,
-	ATP_GEYSER1,
-	ATP_GEYSER2,
-	ATP_GEYSER3,
-	ATP_GEYSER4
+/*
+ * Note: We try to keep the touchpad aspect ratio while still doing only
+ * simple arithmetics:
+ *	0 <= x <= (xsensors - 1) * xfact
+ *	0 <= y <= (ysensors - 1) * yfact
+ */
+struct atp_info {
+	int xsensors;				/* number of X sensors */
+	int xsensors_17;			/* 17" models have more sensors */
+	int ysensors;				/* number of Y sensors */
+	int xfact;				/* X multiplication factor */
+	int yfact;				/* Y multiplication factor */
+	int datalen;				/* size of USB transfers */
+	void (*callback)(struct urb *);		/* callback function */
+};
+
+static void atp_complete_geyser_1_2(struct urb *urb);
+static void atp_complete_geyser_3_4(struct urb *urb);
+
+static const struct atp_info fountain_info = {
+	.xsensors	= 16,
+	.xsensors_17	= 26,
+	.ysensors	= 16,
+	.xfact		= 64,
+	.yfact		= 43,
+	.datalen	= 81,
+	.callback	= atp_complete_geyser_1_2,
+};
+
+static const struct atp_info geyser1_info = {
+	.xsensors	= 16,
+	.xsensors_17	= 26,
+	.ysensors	= 16,
+	.xfact		= 64,
+	.yfact		= 43,
+	.datalen	= 81,
+	.callback	= atp_complete_geyser_1_2,
+};
+
+static const struct atp_info geyser2_info = {
+	.xsensors	= 15,
+	.xsensors_17	= 20,
+	.ysensors	= 9,
+	.xfact		= 64,
+	.yfact		= 43,
+	.datalen	= 64,
+	.callback	= atp_complete_geyser_1_2,
+};
+
+static const struct atp_info geyser3_info = {
+	.xsensors	= 20,
+	.ysensors	= 10,
+	.xfact		= 64,
+	.yfact		= 64,
+	.datalen	= 64,
+	.callback	= atp_complete_geyser_3_4,
 };
 
-#define ATP_DEVICE(prod, type)					\
+static const struct atp_info geyser4_info = {
+	.xsensors	= 20,
+	.ysensors	= 10,
+	.xfact		= 64,
+	.yfact		= 64,
+	.datalen	= 64,
+	.callback	= atp_complete_geyser_3_4,
+};
+
+#define ATP_DEVICE(prod, info)					\
 {								\
 	.match_flags = USB_DEVICE_ID_MATCH_DEVICE |		\
 		       USB_DEVICE_ID_MATCH_INT_CLASS |		\
@@ -53,7 +111,7 @@ enum atp_touchpad_type {
 	.idProduct = (prod),					\
 	.bInterfaceClass = 0x03,				\
 	.bInterfaceProtocol = 0x02,				\
-	.driver_info = ATP_ ## type,				\
+	.driver_info = (unsigned long) &info,			\
 }
 
 /*
@@ -62,43 +120,39 @@ enum atp_touchpad_type {
  *  According to Info.plist Geyser IV is the same as Geyser III.)
  */
 
-static struct usb_device_id atp_table [] = {
+static struct usb_device_id atp_table[] = {
 	/* PowerBooks Feb 2005, iBooks G4 */
-	ATP_DEVICE(0x020e, FOUNTAIN),	/* FOUNTAIN ANSI */
-	ATP_DEVICE(0x020f, FOUNTAIN),	/* FOUNTAIN ISO */
-	ATP_DEVICE(0x030a, FOUNTAIN),	/* FOUNTAIN TP ONLY */
-	ATP_DEVICE(0x030b, GEYSER1),	/* GEYSER 1 TP ONLY */
+	ATP_DEVICE(0x020e, fountain_info),	/* FOUNTAIN ANSI */
+	ATP_DEVICE(0x020f, fountain_info),	/* FOUNTAIN ISO */
+	ATP_DEVICE(0x030a, fountain_info),	/* FOUNTAIN TP ONLY */
+	ATP_DEVICE(0x030b, geyser1_info),	/* GEYSER 1 TP ONLY */
 
 	/* PowerBooks Oct 2005 */
-	ATP_DEVICE(0x0214, GEYSER2),	/* GEYSER 2 ANSI */
-	ATP_DEVICE(0x0215, GEYSER2),	/* GEYSER 2 ISO */
-	ATP_DEVICE(0x0216, GEYSER2),	/* GEYSER 2 JIS */
+	ATP_DEVICE(0x0214, geyser2_info),	/* GEYSER 2 ANSI */
+	ATP_DEVICE(0x0215, geyser2_info),	/* GEYSER 2 ISO */
+	ATP_DEVICE(0x0216, geyser2_info),	/* GEYSER 2 JIS */
 
 	/* Core Duo MacBook & MacBook Pro */
-	ATP_DEVICE(0x0217, GEYSER3),	/* GEYSER 3 ANSI */
-	ATP_DEVICE(0x0218, GEYSER3),	/* GEYSER 3 ISO */
-	ATP_DEVICE(0x0219, GEYSER3),	/* GEYSER 3 JIS */
+	ATP_DEVICE(0x0217, geyser3_info),	/* GEYSER 3 ANSI */
+	ATP_DEVICE(0x0218, geyser3_info),	/* GEYSER 3 ISO */
+	ATP_DEVICE(0x0219, geyser3_info),	/* GEYSER 3 JIS */
 
 	/* Core2 Duo MacBook & MacBook Pro */
-	ATP_DEVICE(0x021a, GEYSER4),	/* GEYSER 4 ANSI */
-	ATP_DEVICE(0x021b, GEYSER4),	/* GEYSER 4 ISO */
-	ATP_DEVICE(0x021c, GEYSER4),	/* GEYSER 4 JIS */
+	ATP_DEVICE(0x021a, geyser4_info),	/* GEYSER 4 ANSI */
+	ATP_DEVICE(0x021b, geyser4_info),	/* GEYSER 4 ISO */
+	ATP_DEVICE(0x021c, geyser4_info),	/* GEYSER 4 JIS */
 
 	/* Core2 Duo MacBook3,1 */
-	ATP_DEVICE(0x0229, GEYSER4),	/* GEYSER 4 HF ANSI */
-	ATP_DEVICE(0x022a, GEYSER4),	/* GEYSER 4 HF ISO */
-	ATP_DEVICE(0x022b, GEYSER4),	/* GEYSER 4 HF JIS */
+	ATP_DEVICE(0x0229, geyser4_info),	/* GEYSER 4 HF ANSI */
+	ATP_DEVICE(0x022a, geyser4_info),	/* GEYSER 4 HF ISO */
+	ATP_DEVICE(0x022b, geyser4_info),	/* GEYSER 4 HF JIS */
 
 	/* Terminating entry */
 	{ }
 };
 MODULE_DEVICE_TABLE(usb, atp_table);
 
-/*
- * number of sensors. Note that only 16 instead of 26 X (horizontal)
- * sensors exist on 12" and 15" PowerBooks. All models have 16 Y
- * (vertical) sensors.
- */
+/* maximum number of sensors */
 #define ATP_XSENSORS	26
 #define ATP_YSENSORS	16
 
@@ -107,21 +161,6 @@ MODULE_DEVICE_TABLE(usb, atp_table);
 
 /* maximum pressure this driver will report */
 #define ATP_PRESSURE	300
-/*
- * multiplication factor for the X and Y coordinates.
- * We try to keep the touchpad aspect ratio while still doing only simple
- * arithmetics.
- * The factors below give coordinates like:
- *
- *      0 <= x <  960 on 12" and 15" Powerbooks
- *      0 <= x < 1600 on 17" Powerbooks and 17" MacBook Pro
- *      0 <= x < 1216 on MacBooks and 15" MacBook Pro
- *
- *      0 <= y <  646 on all Powerbooks
- *      0 <= y <  774 on all MacBooks
- */
-#define ATP_XFACT	64
-#define ATP_YFACT	43
 
 /*
  * Threshold for the touchpad sensors. Any change less than ATP_THRESHOLD is
@@ -159,7 +198,7 @@ struct atp {
 	struct urb		*urb;		/* usb request block */
 	u8			*data;		/* transferred data */
 	struct input_dev	*input;		/* input dev */
-	enum atp_touchpad_type	type;		/* type of touchpad */
+	const struct atp_info	*info;		/* touchpad model */
 	bool			open;
 	bool			valid;		/* are the samples valid? */
 	bool			size_detect_done;
@@ -169,7 +208,6 @@ struct atp {
 	signed char		xy_cur[ATP_XSENSORS + ATP_YSENSORS];
 	signed char		xy_old[ATP_XSENSORS + ATP_YSENSORS];
 	int			xy_acc[ATP_XSENSORS + ATP_YSENSORS];
-	int			datalen;	/* size of USB transfer */
 	int			idlecount;	/* number of empty packets */
 	struct work_struct	work;
 };
@@ -359,7 +397,7 @@ static int atp_status_check(struct urb *urb)
 		if (!dev->overflow_warned) {
 			printk(KERN_WARNING "appletouch: OVERFLOW with data "
 				"length %d, actual length is %d\n",
-				dev->datalen, dev->urb->actual_length);
+				dev->info->datalen, dev->urb->actual_length);
 			dev->overflow_warned = true;
 		}
 	case -ECONNRESET:
@@ -377,7 +415,7 @@ static int atp_status_check(struct urb *urb)
 	}
 
 	/* drop incomplete datasets */
-	if (dev->urb->actual_length != dev->datalen) {
+	if (dev->urb->actual_length != dev->info->datalen) {
 		dprintk("appletouch: incomplete data package"
 			" (first byte: %d, length: %d).\n",
 			dev->data[0], dev->urb->actual_length);
@@ -387,6 +425,25 @@ static int atp_status_check(struct urb *urb)
 	return ATP_URB_STATUS_SUCCESS;
 }
 
+static void atp_detect_size(struct atp *dev)
+{
+	int i;
+
+	/* 17" Powerbooks have extra X sensors */
+	for (i = dev->info->xsensors; i < ATP_XSENSORS; i++) {
+		if (dev->xy_cur[i]) {
+
+			printk(KERN_INFO "appletouch: 17\" model detected.\n");
+
+			input_set_abs_params(dev->input, ABS_X, 0,
+					     (dev->info->xsensors_17 - 1) *
+							dev->info->xfact - 1,
+					     ATP_FUZZ, 0);
+			break;
+		}
+	}
+}
+
 /*
  * USB interrupt callback functions
  */
@@ -407,7 +464,7 @@ static void atp_complete_geyser_1_2(struct urb *urb)
 		goto exit;
 
 	/* reorder the sensors values */
-	if (dev->type == ATP_GEYSER2) {
+	if (dev->info == &geyser2_info) {
 		memset(dev->xy_cur, 0, sizeof(dev->xy_cur));
 
 		/*
@@ -437,8 +494,8 @@ static void atp_complete_geyser_1_2(struct urb *urb)
 				dev->xy_cur[i + 24] = dev->data[5 * i + 44];
 
 			/* Y values */
-			dev->xy_cur[i + 26] = dev->data[5 * i +  1];
-			dev->xy_cur[i + 34] = dev->data[5 * i +  3];
+			dev->xy_cur[ATP_XSENSORS + i] = dev->data[5 * i +  1];
+			dev->xy_cur[ATP_XSENSORS + i + 8] = dev->data[5 * i + 3];
 		}
 	}
 
@@ -453,32 +510,8 @@ static void atp_complete_geyser_1_2(struct urb *urb)
 		memcpy(dev->xy_old, dev->xy_cur, sizeof(dev->xy_old));
 
 		/* Perform size detection, if not done already */
-		if (!dev->size_detect_done) {
-
-			/* 17" Powerbooks have extra X sensors */
-			for (i = (dev->type == ATP_GEYSER2 ? 15 : 16);
-			     i < ATP_XSENSORS; i++) {
-				if (!dev->xy_cur[i])
-					continue;
-
-				printk(KERN_INFO
-					"appletouch: 17\" model detected.\n");
-
-				if (dev->type == ATP_GEYSER2)
-					input_set_abs_params(dev->input, ABS_X,
-							     0,
-							     (20 - 1) *
-							     ATP_XFACT - 1,
-							     ATP_FUZZ, 0);
-				else
-					input_set_abs_params(dev->input, ABS_X,
-							     0,
-							     (26 - 1) *
-							     ATP_XFACT - 1,
-							     ATP_FUZZ, 0);
-				break;
-			}
-
+		if (unlikely(!dev->size_detect_done)) {
+			atp_detect_size(dev);
 			dev->size_detect_done = 1;
 			goto exit;
 		}
@@ -499,10 +532,10 @@ static void atp_complete_geyser_1_2(struct urb *urb)
 	dbg_dump("accumulator", dev->xy_acc);
 
 	x = atp_calculate_abs(dev->xy_acc, ATP_XSENSORS,
-			      ATP_XFACT, &x_z, &x_f);
+			      dev->info->xfact, &x_z, &x_f);
 	y = atp_calculate_abs(dev->xy_acc + ATP_XSENSORS, ATP_YSENSORS,
-			      ATP_YFACT, &y_z, &y_f);
-	key = dev->data[dev->datalen - 1] & ATP_STATUS_BUTTON;
+			      dev->info->yfact, &y_z, &y_f);
+	key = dev->data[dev->info->datalen - 1] & ATP_STATUS_BUTTON;
 
 	if (x && y) {
 		if (dev->x_old != -1) {
@@ -583,7 +616,7 @@ static void atp_complete_geyser_3_4(struct urb *urb)
 	dbg_dump("sample", dev->xy_cur);
 
 	/* Just update the base values (i.e. touchpad in untouched state) */
-	if (dev->data[dev->datalen - 1] & ATP_STATUS_BASE_UPDATE) {
+	if (dev->data[dev->info->datalen - 1] & ATP_STATUS_BASE_UPDATE) {
 
 		dprintk(KERN_DEBUG "appletouch: updated base values\n");
 
@@ -610,10 +643,10 @@ static void atp_complete_geyser_3_4(struct urb *urb)
 	dbg_dump("accumulator", dev->xy_acc);
 
 	x = atp_calculate_abs(dev->xy_acc, ATP_XSENSORS,
-			      ATP_XFACT, &x_z, &x_f);
+			      dev->info->xfact, &x_z, &x_f);
 	y = atp_calculate_abs(dev->xy_acc + ATP_XSENSORS, ATP_YSENSORS,
-			      ATP_YFACT, &y_z, &y_f);
-	key = dev->data[dev->datalen - 1] & ATP_STATUS_BUTTON;
+			      dev->info->yfact, &y_z, &y_f);
+	key = dev->data[dev->info->datalen - 1] & ATP_STATUS_BUTTON;
 
 	if (x && y) {
 		if (dev->x_old != -1) {
@@ -705,7 +738,7 @@ static int atp_handle_geyser(struct atp *dev)
 {
 	struct usb_device *udev = dev->udev;
 
-	if (dev->type != ATP_FOUNTAIN) {
+	if (dev->info != &fountain_info) {
 		/* switch to raw sensor mode */
 		if (atp_geyser_init(udev))
 			return -EIO;
@@ -726,6 +759,7 @@ static int atp_probe(struct usb_interface *iface,
 	struct usb_endpoint_descriptor *endpoint;
 	int int_in_endpointAddr = 0;
 	int i, error = -ENOMEM;
+	const struct atp_info *info = (const struct atp_info *)id->driver_info;
 
 	/* set up the endpoint information */
 	/* use only the first interrupt-in endpoint */
@@ -753,35 +787,22 @@ static int atp_probe(struct usb_interface *iface,
 
 	dev->udev = udev;
 	dev->input = input_dev;
-	dev->type = id->driver_info;
+	dev->info = info;
 	dev->overflow_warned = false;
-	if (dev->type == ATP_FOUNTAIN || dev->type == ATP_GEYSER1)
-		dev->datalen = 81;
-	else
-		dev->datalen = 64;
 
 	dev->urb = usb_alloc_urb(0, GFP_KERNEL);
 	if (!dev->urb)
 		goto err_free_devs;
 
-	dev->data = usb_buffer_alloc(dev->udev, dev->datalen, GFP_KERNEL,
+	dev->data = usb_buffer_alloc(dev->udev, dev->info->datalen, GFP_KERNEL,
 				     &dev->urb->transfer_dma);
 	if (!dev->data)
 		goto err_free_urb;
 
-	/* Select the USB complete (callback) function */
-	if (dev->type == ATP_FOUNTAIN ||
-	    dev->type == ATP_GEYSER1 ||
-	    dev->type == ATP_GEYSER2)
-		usb_fill_int_urb(dev->urb, udev,
-				 usb_rcvintpipe(udev, int_in_endpointAddr),
-				 dev->data, dev->datalen,
-				 atp_complete_geyser_1_2, dev, 1);
-	else
-		usb_fill_int_urb(dev->urb, udev,
-				 usb_rcvintpipe(udev, int_in_endpointAddr),
-				 dev->data, dev->datalen,
-				 atp_complete_geyser_3_4, dev, 1);
+	usb_fill_int_urb(dev->urb, udev,
+			 usb_rcvintpipe(udev, int_in_endpointAddr),
+			 dev->data, dev->info->datalen,
+			 dev->info->callback, dev, 1);
 
 	error = atp_handle_geyser(dev);
 	if (error)
@@ -802,35 +823,12 @@ static int atp_probe(struct usb_interface *iface,
 
 	set_bit(EV_ABS, input_dev->evbit);
 
-	if (dev->type == ATP_GEYSER3 || dev->type == ATP_GEYSER4) {
-		/*
-		 * MacBook have 20 X sensors, 10 Y sensors
-		 */
-		input_set_abs_params(input_dev, ABS_X, 0,
-				     ((20 - 1) * ATP_XFACT) - 1, ATP_FUZZ, 0);
-		input_set_abs_params(input_dev, ABS_Y, 0,
-				     ((10 - 1) * ATP_YFACT) - 1, ATP_FUZZ, 0);
-	} else if (dev->type == ATP_GEYSER2) {
-		/*
-		 * Oct 2005 15" PowerBooks have 15 X sensors, 17" are detected
-		 * later.
-		 */
-		input_set_abs_params(input_dev, ABS_X, 0,
-				     ((15 - 1) * ATP_XFACT) - 1, ATP_FUZZ, 0);
-		input_set_abs_params(input_dev, ABS_Y, 0,
-				     ((9 - 1) * ATP_YFACT) - 1, ATP_FUZZ, 0);
-	} else {
-		/*
-		 * 12" and 15" Powerbooks only have 16 x sensors,
-		 * 17" models are detected later.
-		 */
-		input_set_abs_params(input_dev, ABS_X, 0,
-				     (16 - 1) * ATP_XFACT - 1,
-				     ATP_FUZZ, 0);
-		input_set_abs_params(input_dev, ABS_Y, 0,
-				     (ATP_YSENSORS - 1) * ATP_YFACT - 1,
-				     ATP_FUZZ, 0);
-	}
+	input_set_abs_params(input_dev, ABS_X, 0,
+			     (dev->info->xsensors - 1) * dev->info->xfact - 1,
+			     ATP_FUZZ, 0);
+	input_set_abs_params(input_dev, ABS_Y, 0,
+			     (dev->info->ysensors - 1) * dev->info->yfact - 1,
+			     ATP_FUZZ, 0);
 	input_set_abs_params(input_dev, ABS_PRESSURE, 0, ATP_PRESSURE, 0, 0);
 
 	set_bit(EV_KEY, input_dev->evbit);
@@ -852,7 +850,7 @@ static int atp_probe(struct usb_interface *iface,
 	return 0;
 
  err_free_buffer:
-	usb_buffer_free(dev->udev, dev->datalen,
+	usb_buffer_free(dev->udev, dev->info->datalen,
 			dev->data, dev->urb->transfer_dma);
  err_free_urb:
 	usb_free_urb(dev->urb);
@@ -871,7 +869,7 @@ static void atp_disconnect(struct usb_interface *iface)
 	if (dev) {
 		usb_kill_urb(dev->urb);
 		input_unregister_device(dev->input);
-		usb_buffer_free(dev->udev, dev->datalen,
+		usb_buffer_free(dev->udev, dev->info->datalen,
 				dev->data, dev->urb->transfer_dma);
 		usb_free_urb(dev->urb);
 		kfree(dev);
diff --git a/drivers/input/mouse/gpio_mouse.c b/drivers/input/mouse/gpio_mouse.c
index 72cf5e33790..0db8d16c5ed 100644
--- a/drivers/input/mouse/gpio_mouse.c
+++ b/drivers/input/mouse/gpio_mouse.c
@@ -173,7 +173,7 @@ static int __devexit gpio_mouse_remove(struct platform_device *pdev)
 /* work with hotplug and coldplug */
 MODULE_ALIAS("platform:gpio_mouse");
 
-struct platform_driver gpio_mouse_device_driver = {
+static struct platform_driver gpio_mouse_device_driver = {
 	.remove		= __devexit_p(gpio_mouse_remove),
 	.driver		= {
 		.name	= "gpio_mouse",
diff --git a/drivers/input/mouse/hgpk.c b/drivers/input/mouse/hgpk.c
index 88f04bf2ad6..81e6ebf323e 100644
--- a/drivers/input/mouse/hgpk.c
+++ b/drivers/input/mouse/hgpk.c
@@ -48,6 +48,30 @@ module_param(recalib_delta, int, 0644);
 MODULE_PARM_DESC(recalib_delta,
 	"packets containing a delta this large will cause a recalibration.");
 
+static int jumpy_delay = 1000;
+module_param(jumpy_delay, int, 0644);
+MODULE_PARM_DESC(jumpy_delay,
+	"delay (ms) before recal after jumpiness detected");
+
+static int spew_delay = 1000;
+module_param(spew_delay, int, 0644);
+MODULE_PARM_DESC(spew_delay,
+	"delay (ms) before recal after packet spew detected");
+
+static int recal_guard_time = 2000;
+module_param(recal_guard_time, int, 0644);
+MODULE_PARM_DESC(recal_guard_time,
+	"interval (ms) during which recal will be restarted if packet received");
+
+static int post_interrupt_delay = 1000;
+module_param(post_interrupt_delay, int, 0644);
+MODULE_PARM_DESC(post_interrupt_delay,
+	"delay (ms) before recal after recal interrupt detected");
+
+static int autorecal = 1;
+module_param(autorecal, int, 0644);
+MODULE_PARM_DESC(autorecal, "enable recalibration in the driver");
+
 /*
  * When the touchpad gets ultra-sensitive, one can keep their finger 1/2"
  * above the pad and still have it send packets.  This causes a jump cursor
@@ -66,7 +90,7 @@ static void hgpk_jumpy_hack(struct psmouse *psmouse, int x, int y)
 		/* My car gets forty rods to the hogshead and that's the
 		 * way I likes it! */
 		psmouse_queue_work(psmouse, &priv->recalib_wq,
-				msecs_to_jiffies(1000));
+				msecs_to_jiffies(jumpy_delay));
 	}
 }
 
@@ -103,7 +127,7 @@ static void hgpk_spewing_hack(struct psmouse *psmouse,
 			hgpk_dbg(psmouse, "packet spew detected (%d,%d)\n",
 				 priv->x_tally, priv->y_tally);
 			psmouse_queue_work(psmouse, &priv->recalib_wq,
-					   msecs_to_jiffies(1000));
+					   msecs_to_jiffies(spew_delay));
 		}
 		/* reset every 100 packets */
 		priv->count = 0;
@@ -181,7 +205,7 @@ static psmouse_ret_t hgpk_process_byte(struct psmouse *psmouse)
 				 "packet inside calibration window, "
 				 "queueing another recalibration\n");
 			psmouse_queue_work(psmouse, &priv->recalib_wq,
-					msecs_to_jiffies(1000));
+					msecs_to_jiffies(post_interrupt_delay));
 		}
 		priv->recalib_window = 0;
 	}
@@ -231,7 +255,7 @@ static int hgpk_force_recalibrate(struct psmouse *psmouse)
 	 * If someone's finger *was* on the touchpad, it's probably
 	 * miscalibrated.  So, we should schedule another recalibration
 	 */
-	priv->recalib_window = jiffies +  msecs_to_jiffies(2000);
+	priv->recalib_window = jiffies +  msecs_to_jiffies(recal_guard_time);
 
 	return 0;
 }
diff --git a/drivers/input/mouse/hil_ptr.c b/drivers/input/mouse/hil_ptr.c
index e532c48410e..3263ce083bf 100644
--- a/drivers/input/mouse/hil_ptr.c
+++ b/drivers/input/mouse/hil_ptr.c
@@ -46,7 +46,7 @@
 MODULE_AUTHOR("Brian S. Julin <bri@calyx.com>");
 MODULE_DESCRIPTION(HIL_GENERIC_NAME " driver");
 MODULE_LICENSE("Dual BSD/GPL");
-
+MODULE_ALIAS("serio:ty03pr25id0Fex*");
 
 #define TABLET_SIMULATES_MOUSE	/* allow tablet to be used as mouse */
 #undef  TABLET_AUTOADJUST	/* auto-adjust valid tablet ranges */
diff --git a/drivers/input/mouse/pxa930_trkball.c b/drivers/input/mouse/pxa930_trkball.c
new file mode 100644
index 00000000000..a0f45c4fc19
--- /dev/null
+++ b/drivers/input/mouse/pxa930_trkball.c
@@ -0,0 +1,269 @@
+/*
+ * PXA930 track ball mouse driver
+ *
+ * Copyright (C) 2007 Marvell International Ltd.
+ * 2008-02-28: Yong Yao <yaoyong@marvell.com>
+ *             initial version
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2 as
+ *  published by the Free Software Foundation.
+ */
+
+#include <linux/init.h>
+#include <linux/input.h>
+#include <linux/version.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+
+#include <mach/hardware.h>
+#include <mach/pxa930_trkball.h>
+
+/* Trackball Controller Register Definitions */
+#define TBCR		(0x000C)
+#define TBCNTR		(0x0010)
+#define TBSBC		(0x0014)
+
+#define TBCR_TBRST	(1 << 1)
+#define TBCR_TBSB	(1 << 10)
+
+#define TBCR_Y_FLT(n)	(((n) & 0xf) << 6)
+#define TBCR_X_FLT(n)	(((n) & 0xf) << 2)
+
+#define TBCNTR_YM(n)	(((n) >> 24) & 0xff)
+#define TBCNTR_YP(n)	(((n) >> 16) & 0xff)
+#define TBCNTR_XM(n)	(((n) >> 8) & 0xff)
+#define TBCNTR_XP(n)	((n) & 0xff)
+
+#define TBSBC_TBSBC	(0x1)
+
+struct pxa930_trkball {
+	struct pxa930_trkball_platform_data *pdata;
+
+	/* Memory Mapped Register */
+	struct resource *mem;
+	void __iomem *mmio_base;
+
+	struct input_dev *input;
+};
+
+static irqreturn_t pxa930_trkball_interrupt(int irq, void *dev_id)
+{
+	struct pxa930_trkball *trkball = dev_id;
+	struct input_dev *input = trkball->input;
+	int tbcntr, x, y;
+
+	/* According to the spec software must read TBCNTR twice:
+	 * if the read value is the same, the reading is valid
+	 */
+	tbcntr = __raw_readl(trkball->mmio_base + TBCNTR);
+
+	if (tbcntr == __raw_readl(trkball->mmio_base + TBCNTR)) {
+		x = (TBCNTR_XP(tbcntr) - TBCNTR_XM(tbcntr)) / 2;
+		y = (TBCNTR_YP(tbcntr) - TBCNTR_YM(tbcntr)) / 2;
+
+		input_report_rel(input, REL_X, x);
+		input_report_rel(input, REL_Y, y);
+		input_sync(input);
+	}
+
+	__raw_writel(TBSBC_TBSBC, trkball->mmio_base + TBSBC);
+	__raw_writel(0, trkball->mmio_base + TBSBC);
+
+	return IRQ_HANDLED;
+}
+
+/* For TBCR, we need to wait for a while to make sure it has been modified. */
+static int write_tbcr(struct pxa930_trkball *trkball, int v)
+{
+	int i = 100;
+
+	__raw_writel(v, trkball->mmio_base + TBCR);
+
+	while (i--) {
+		if (__raw_readl(trkball->mmio_base + TBCR) == v)
+			break;
+		msleep(1);
+	}
+
+	if (i == 0) {
+		pr_err("%s: timed out writing TBCR(%x)!\n", __func__, v);
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+static void pxa930_trkball_config(struct pxa930_trkball *trkball)
+{
+	uint32_t tbcr;
+
+	/* According to spec, need to write the filters of x,y to 0xf first! */
+	tbcr = __raw_readl(trkball->mmio_base + TBCR);
+	write_tbcr(trkball, tbcr | TBCR_X_FLT(0xf) | TBCR_Y_FLT(0xf));
+	write_tbcr(trkball, TBCR_X_FLT(trkball->pdata->x_filter) |
+			    TBCR_Y_FLT(trkball->pdata->y_filter));
+
+	/* According to spec, set TBCR_TBRST first, before clearing it! */
+	tbcr = __raw_readl(trkball->mmio_base + TBCR);
+	write_tbcr(trkball, tbcr | TBCR_TBRST);
+	write_tbcr(trkball, tbcr & ~TBCR_TBRST);
+
+	__raw_writel(TBSBC_TBSBC, trkball->mmio_base + TBSBC);
+	__raw_writel(0, trkball->mmio_base + TBSBC);
+
+	pr_debug("%s: final TBCR=%x!\n", __func__,
+		 __raw_readl(trkball->mmio_base + TBCR));
+}
+
+static int pxa930_trkball_open(struct input_dev *dev)
+{
+	struct pxa930_trkball *trkball = input_get_drvdata(dev);
+
+	pxa930_trkball_config(trkball);
+
+	return 0;
+}
+
+static void pxa930_trkball_disable(struct pxa930_trkball *trkball)
+{
+	uint32_t tbcr = __raw_readl(trkball->mmio_base + TBCR);
+
+	/* Held in reset, gate the 32-KHz input clock off */
+	write_tbcr(trkball, tbcr | TBCR_TBRST);
+}
+
+static void pxa930_trkball_close(struct input_dev *dev)
+{
+	struct pxa930_trkball *trkball = input_get_drvdata(dev);
+
+	pxa930_trkball_disable(trkball);
+}
+
+static int __devinit pxa930_trkball_probe(struct platform_device *pdev)
+{
+	struct pxa930_trkball *trkball;
+	struct input_dev *input;
+	struct resource *res;
+	int irq, error;
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		dev_err(&pdev->dev, "failed to get trkball irq\n");
+		return -ENXIO;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "failed to get register memory\n");
+		return -ENXIO;
+	}
+
+	trkball = kzalloc(sizeof(struct pxa930_trkball), GFP_KERNEL);
+	if (!trkball)
+		return -ENOMEM;
+
+	trkball->pdata = pdev->dev.platform_data;
+	if (!trkball->pdata) {
+		dev_err(&pdev->dev, "no platform data defined\n");
+		error = -EINVAL;
+		goto failed;
+	}
+
+	trkball->mmio_base = ioremap_nocache(res->start, resource_size(res));
+	if (!trkball->mmio_base) {
+		dev_err(&pdev->dev, "failed to ioremap registers\n");
+		error = -ENXIO;
+		goto failed;
+	}
+
+	/* held the module in reset, will be enabled in open() */
+	pxa930_trkball_disable(trkball);
+
+	error = request_irq(irq, pxa930_trkball_interrupt, IRQF_DISABLED,
+			    pdev->name, trkball);
+	if (error) {
+		dev_err(&pdev->dev, "failed to request irq: %d\n", ret);
+		goto failed_free_io;
+	}
+
+	platform_set_drvdata(pdev, trkball);
+
+	input = input_allocate_device();
+	if (!input) {
+		dev_err(&pdev->dev, "failed to allocate input device\n");
+		error = -ENOMEM;
+		goto failed_free_irq;
+	}
+
+	input->name = pdev->name;
+	input->id.bustype = BUS_HOST;
+	input->open = pxa930_trkball_open;
+	input->close = pxa930_trkball_close;
+	input->dev.parent = &pdev->dev;
+	input_set_drvdata(input, trkball);
+
+	trkball->input = input;
+
+	input_set_capability(input, EV_REL, REL_X);
+	input_set_capability(input, EV_REL, REL_Y);
+
+	error = input_register_device(input);
+	if (error) {
+		dev_err(&pdev->dev, "unable to register input device\n");
+		goto failed_free_input;
+	}
+
+	return 0;
+
+failed_free_input:
+	input_free_device(input);
+failed_free_irq:
+	free_irq(irq, trkball);
+failed_free_io:
+	iounmap(trkball->mmio_base);
+failed:
+	kfree(trkball);
+	return ret;
+}
+
+static int __devexit pxa930_trkball_remove(struct platform_device *pdev)
+{
+	struct pxa930_trkball *trkball = platform_get_drvdata(pdev);
+	int irq = platform_get_irq(pdev, 0);
+
+	input_unregister_device(trkball->input);
+	free_irq(irq, trkball);
+	iounmap(trkball->mmio_base);
+	kfree(trkball);
+
+	return 0;
+}
+
+static struct platform_driver pxa930_trkball_driver = {
+	.driver		= {
+		.name	= "pxa930-trkball",
+	},
+	.probe		= pxa930_trkball_probe,
+	.remove		= __devexit_p(pxa930_trkball_remove),
+};
+
+static int __init pxa930_trkball_init(void)
+{
+	return platform_driver_register(&pxa930_trkball_driver);
+}
+
+static void __exit pxa930_trkball_exit(void)
+{
+	platform_driver_unregister(&pxa930_trkball_driver);
+}
+
+module_init(pxa930_trkball_init);
+module_exit(pxa930_trkball_exit);
+
+MODULE_AUTHOR("Yong Yao <yaoyong@marvell.com>");
+MODULE_DESCRIPTION("PXA930 Trackball Mouse Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/mouse/synaptics.c b/drivers/input/mouse/synaptics.c
index d349c4a5e3e..865fc69e9bc 100644
--- a/drivers/input/mouse/synaptics.c
+++ b/drivers/input/mouse/synaptics.c
@@ -445,12 +445,14 @@ static void synaptics_process_packet(struct psmouse *psmouse)
 
 	input_report_abs(dev, ABS_TOOL_WIDTH, finger_width);
 	input_report_key(dev, BTN_TOOL_FINGER, num_fingers == 1);
-	input_report_key(dev, BTN_TOOL_DOUBLETAP, num_fingers == 2);
-	input_report_key(dev, BTN_TOOL_TRIPLETAP, num_fingers == 3);
-
 	input_report_key(dev, BTN_LEFT, hw.left);
 	input_report_key(dev, BTN_RIGHT, hw.right);
 
+	if (SYN_CAP_MULTIFINGER(priv->capabilities)) {
+		input_report_key(dev, BTN_TOOL_DOUBLETAP, num_fingers == 2);
+		input_report_key(dev, BTN_TOOL_TRIPLETAP, num_fingers == 3);
+	}
+
 	if (SYN_CAP_MIDDLE_BUTTON(priv->capabilities))
 		input_report_key(dev, BTN_MIDDLE, hw.middle);
 
@@ -543,12 +545,14 @@ static void set_input_params(struct input_dev *dev, struct synaptics_data *priv)
 	set_bit(EV_KEY, dev->evbit);
 	set_bit(BTN_TOUCH, dev->keybit);
 	set_bit(BTN_TOOL_FINGER, dev->keybit);
-	set_bit(BTN_TOOL_DOUBLETAP, dev->keybit);
-	set_bit(BTN_TOOL_TRIPLETAP, dev->keybit);
-
 	set_bit(BTN_LEFT, dev->keybit);
 	set_bit(BTN_RIGHT, dev->keybit);
 
+	if (SYN_CAP_MULTIFINGER(priv->capabilities)) {
+		set_bit(BTN_TOOL_DOUBLETAP, dev->keybit);
+		set_bit(BTN_TOOL_TRIPLETAP, dev->keybit);
+	}
+
 	if (SYN_CAP_MIDDLE_BUTTON(priv->capabilities))
 		set_bit(BTN_MIDDLE, dev->keybit);
 
diff --git a/drivers/input/mousedev.c b/drivers/input/mousedev.c
index d8c056fe7e9..ef99a7e6d40 100644
--- a/drivers/input/mousedev.c
+++ b/drivers/input/mousedev.c
@@ -878,8 +878,7 @@ static struct mousedev *mousedev_create(struct input_dev *dev,
 	mousedev->handle.handler = handler;
 	mousedev->handle.private = mousedev;
 
-	strlcpy(mousedev->dev.bus_id, mousedev->name,
-		sizeof(mousedev->dev.bus_id));
+	dev_set_name(&mousedev->dev, mousedev->name);
 	mousedev->dev.class = &input_class;
 	if (dev)
 		mousedev->dev.parent = &dev->dev;
diff --git a/drivers/input/serio/hil_mlc.c b/drivers/input/serio/hil_mlc.c
index 37586a68d34..7ba9f2b2c04 100644
--- a/drivers/input/serio/hil_mlc.c
+++ b/drivers/input/serio/hil_mlc.c
@@ -934,6 +934,7 @@ int hil_mlc_register(hil_mlc *mlc)
 		snprintf(mlc_serio->name, sizeof(mlc_serio->name)-1, "HIL_SERIO%d", i);
 		snprintf(mlc_serio->phys, sizeof(mlc_serio->phys)-1, "HIL%d", i);
 		mlc_serio->id			= hil_mlc_serio_id;
+		mlc_serio->id.id		= i; /* HIL port no. */
 		mlc_serio->write		= hil_mlc_serio_write;
 		mlc_serio->open			= hil_mlc_serio_open;
 		mlc_serio->close		= hil_mlc_serio_close;
diff --git a/drivers/input/serio/i8042-x86ia64io.h b/drivers/input/serio/i8042-x86ia64io.h
index 29e686388a2..6fa2deff744 100644
--- a/drivers/input/serio/i8042-x86ia64io.h
+++ b/drivers/input/serio/i8042-x86ia64io.h
@@ -143,6 +143,14 @@ static struct dmi_system_id __initdata i8042_dmi_noloop_table[] = {
 			DMI_MATCH(DMI_PRODUCT_VERSION, "M606"),
 		},
 	},
+	{
+		.ident = "Gigabyte M912",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "GIGABYTE"),
+			DMI_MATCH(DMI_PRODUCT_NAME, "M912"),
+			DMI_MATCH(DMI_PRODUCT_VERSION, "01"),
+		},
+	},
 	{ }
 };
 
@@ -351,6 +359,13 @@ static struct dmi_system_id __initdata i8042_dmi_nomux_table[] = {
 			DMI_MATCH(DMI_PRODUCT_NAME, "HEL80I"),
 		},
 	},
+	{
+		.ident = "Dell Vostro 1510",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "Dell Inc."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "Vostro1510"),
+		},
+	},
 	{ }
 };
 
diff --git a/drivers/input/serio/libps2.c b/drivers/input/serio/libps2.c
index 2b304c22c20..67248c31e19 100644
--- a/drivers/input/serio/libps2.c
+++ b/drivers/input/serio/libps2.c
@@ -262,9 +262,17 @@ int ps2_handle_ack(struct ps2dev *ps2dev, unsigned char data)
 			break;
 
 		case PS2_RET_NAK:
-			ps2dev->nak = 1;
+			ps2dev->flags |= PS2_FLAG_NAK;
+			ps2dev->nak = PS2_RET_NAK;
 			break;
 
+		case PS2_RET_ERR:
+			if (ps2dev->flags & PS2_FLAG_NAK) {
+				ps2dev->flags &= ~PS2_FLAG_NAK;
+				ps2dev->nak = PS2_RET_ERR;
+				break;
+			}
+
 		/*
 		 * Workaround for mice which don't ACK the Get ID command.
 		 * These are valid mouse IDs that we recognize.
@@ -282,8 +290,11 @@ int ps2_handle_ack(struct ps2dev *ps2dev, unsigned char data)
 	}
 
 
-	if (!ps2dev->nak && ps2dev->cmdcnt)
-		ps2dev->flags |= PS2_FLAG_CMD | PS2_FLAG_CMD1;
+	if (!ps2dev->nak) {
+		ps2dev->flags &= ~PS2_FLAG_NAK;
+		if (ps2dev->cmdcnt)
+			ps2dev->flags |= PS2_FLAG_CMD | PS2_FLAG_CMD1;
+	}
 
 	ps2dev->flags &= ~PS2_FLAG_ACK;
 	wake_up(&ps2dev->wait);
@@ -329,6 +340,7 @@ void ps2_cmd_aborted(struct ps2dev *ps2dev)
 	if (ps2dev->flags & (PS2_FLAG_ACK | PS2_FLAG_CMD))
 		wake_up(&ps2dev->wait);
 
-	ps2dev->flags = 0;
+	/* reset all flags except last nack */
+	ps2dev->flags &= PS2_FLAG_NAK;
 }
 EXPORT_SYMBOL(ps2_cmd_aborted);
diff --git a/drivers/input/serio/pcips2.c b/drivers/input/serio/pcips2.c
index 1b404f9e3bf..1dacbe0d934 100644
--- a/drivers/input/serio/pcips2.c
+++ b/drivers/input/serio/pcips2.c
@@ -153,7 +153,7 @@ static int __devinit pcips2_probe(struct pci_dev *dev, const struct pci_device_i
 	serio->open		= pcips2_open;
 	serio->close		= pcips2_close;
 	strlcpy(serio->name, pci_name(dev), sizeof(serio->name));
-	strlcpy(serio->phys, dev->dev.bus_id, sizeof(serio->phys));
+	strlcpy(serio->phys, dev_name(&dev->dev), sizeof(serio->phys));
 	serio->port_data	= ps2if;
 	serio->dev.parent	= &dev->dev;
 	ps2if->io		= serio;
diff --git a/drivers/input/serio/serio.c b/drivers/input/serio/serio.c
index 2f12d60eee3..bc033250dfc 100644
--- a/drivers/input/serio/serio.c
+++ b/drivers/input/serio/serio.c
@@ -546,8 +546,8 @@ static void serio_init_port(struct serio *serio)
 	spin_lock_init(&serio->lock);
 	mutex_init(&serio->drv_mutex);
 	device_initialize(&serio->dev);
-	snprintf(serio->dev.bus_id, sizeof(serio->dev.bus_id),
-		 "serio%ld", (long)atomic_inc_return(&serio_no) - 1);
+	dev_set_name(&serio->dev, "serio%ld",
+			(long)atomic_inc_return(&serio_no) - 1);
 	serio->dev.bus = &serio_bus;
 	serio->dev.release = serio_release_port;
 	if (serio->parent) {
diff --git a/drivers/input/serio/xilinx_ps2.c b/drivers/input/serio/xilinx_ps2.c
index 765007899d9..ebb22f88c84 100644
--- a/drivers/input/serio/xilinx_ps2.c
+++ b/drivers/input/serio/xilinx_ps2.c
@@ -58,23 +58,20 @@
 
 /* Mask for all the Receive Interrupts */
 #define XPS2_IPIXR_RX_ALL	(XPS2_IPIXR_RX_OVF | XPS2_IPIXR_RX_ERR |  \
-					XPS2_IPIXR_RX_FULL)
+				 XPS2_IPIXR_RX_FULL)
 
 /* Mask for all the Interrupts */
 #define XPS2_IPIXR_ALL		(XPS2_IPIXR_TX_ALL | XPS2_IPIXR_RX_ALL |  \
-					XPS2_IPIXR_WDT_TOUT)
+				 XPS2_IPIXR_WDT_TOUT)
 
 /* Global Interrupt Enable mask */
 #define XPS2_GIER_GIE_MASK	0x80000000
 
 struct xps2data {
 	int irq;
-	u32 phys_addr;
-	u32 remap_size;
 	spinlock_t lock;
-	u8 rxb;				/* Rx buffer */
 	void __iomem *base_address;	/* virt. address of control registers */
-	unsigned int dfl;
+	unsigned int flags;
 	struct serio serio;		/* serio */
 };
 
@@ -82,8 +79,13 @@ struct xps2data {
 /* XPS PS/2 data transmission calls */
 /************************************/
 
-/*
- * xps2_recv() will attempt to receive a byte of data from the PS/2 port.
+/**
+ * xps2_recv() - attempts to receive a byte from the PS/2 port.
+ * @drvdata:	pointer to ps2 device private data structure
+ * @byte:	address where the read data will be copied
+ *
+ * If there is any data available in the PS/2 receiver, this functions reads
+ * the data, otherwise it returns error.
  */
 static int xps2_recv(struct xps2data *drvdata, u8 *byte)
 {
@@ -116,33 +118,27 @@ static irqreturn_t xps2_interrupt(int irq, void *dev_id)
 
 	/* Check which interrupt is active */
 	if (intr_sr & XPS2_IPIXR_RX_OVF)
-		printk(KERN_WARNING "%s: receive overrun error\n",
-			drvdata->serio.name);
+		dev_warn(drvdata->serio.dev.parent, "receive overrun error\n");
 
 	if (intr_sr & XPS2_IPIXR_RX_ERR)
-		drvdata->dfl |= SERIO_PARITY;
+		drvdata->flags |= SERIO_PARITY;
 
 	if (intr_sr & (XPS2_IPIXR_TX_NOACK | XPS2_IPIXR_WDT_TOUT))
-		drvdata->dfl |= SERIO_TIMEOUT;
+		drvdata->flags |= SERIO_TIMEOUT;
 
 	if (intr_sr & XPS2_IPIXR_RX_FULL) {
-		status = xps2_recv(drvdata, &drvdata->rxb);
+		status = xps2_recv(drvdata, &c);
 
 		/* Error, if a byte is not received */
 		if (status) {
-			printk(KERN_ERR
-				"%s: wrong rcvd byte count (%d)\n",
-				drvdata->serio.name, status);
+			dev_err(drvdata->serio.dev.parent,
+				"wrong rcvd byte count (%d)\n", status);
 		} else {
-			c = drvdata->rxb;
-			serio_interrupt(&drvdata->serio, c, drvdata->dfl);
-			drvdata->dfl = 0;
+			serio_interrupt(&drvdata->serio, c, drvdata->flags);
+			drvdata->flags = 0;
 		}
 	}
 
-	if (intr_sr & XPS2_IPIXR_TX_ACK)
-		drvdata->dfl = 0;
-
 	return IRQ_HANDLED;
 }
 
@@ -150,8 +146,15 @@ static irqreturn_t xps2_interrupt(int irq, void *dev_id)
 /* serio callbacks */
 /*******************/
 
-/*
- * sxps2_write() sends a byte out through the PS/2 interface.
+/**
+ * sxps2_write() - sends a byte out through the PS/2 port.
+ * @pserio:	pointer to the serio structure of the PS/2 port
+ * @c:		data that needs to be written to the PS/2 port
+ *
+ * This function checks if the PS/2 transmitter is empty and sends a byte.
+ * Otherwise it returns error. Transmission fails only when nothing is connected
+ * to the PS/2 port. Thats why, we do not try to resend the data in case of a
+ * failure.
  */
 static int sxps2_write(struct serio *pserio, unsigned char c)
 {
@@ -174,33 +177,39 @@ static int sxps2_write(struct serio *pserio, unsigned char c)
 	return status;
 }
 
-/*
- * sxps2_open() is called when a port is open by the higher layer.
+/**
+ * sxps2_open() - called when a port is opened by the higher layer.
+ * @pserio:	pointer to the serio structure of the PS/2 device
+ *
+ * This function requests irq and enables interrupts for the PS/2 device.
  */
 static int sxps2_open(struct serio *pserio)
 {
 	struct xps2data *drvdata = pserio->port_data;
-	int retval;
+	int error;
+	u8 c;
 
-	retval = request_irq(drvdata->irq, &xps2_interrupt, 0,
+	error = request_irq(drvdata->irq, &xps2_interrupt, 0,
 				DRIVER_NAME, drvdata);
-	if (retval) {
-		printk(KERN_ERR
-			"%s: Couldn't allocate interrupt %d\n",
-			drvdata->serio.name, drvdata->irq);
-		return retval;
+	if (error) {
+		dev_err(drvdata->serio.dev.parent,
+			"Couldn't allocate interrupt %d\n", drvdata->irq);
+		return error;
 	}
 
 	/* start reception by enabling the interrupts */
 	out_be32(drvdata->base_address + XPS2_GIER_OFFSET, XPS2_GIER_GIE_MASK);
 	out_be32(drvdata->base_address + XPS2_IPIER_OFFSET, XPS2_IPIXR_RX_ALL);
-	(void)xps2_recv(drvdata, &drvdata->rxb);
+	(void)xps2_recv(drvdata, &c);
 
 	return 0;		/* success */
 }
 
-/*
- * sxps2_close() frees the interrupt.
+/**
+ * sxps2_close() - frees the interrupt.
+ * @pserio:	pointer to the serio structure of the PS/2 device
+ *
+ * This function frees the irq and disables interrupts for the PS/2 device.
  */
 static void sxps2_close(struct serio *pserio)
 {
@@ -212,24 +221,41 @@ static void sxps2_close(struct serio *pserio)
 	free_irq(drvdata->irq, drvdata);
 }
 
-/*********************/
-/* Device setup code */
-/*********************/
-
-static int xps2_setup(struct device *dev, struct resource *regs_res,
-		      struct resource *irq_res)
+/**
+ * xps2_of_probe - probe method for the PS/2 device.
+ * @of_dev:	pointer to OF device structure
+ * @match:	pointer to the stucture used for matching a device
+ *
+ * This function probes the PS/2 device in the device tree.
+ * It initializes the driver data structure and the hardware.
+ * It returns 0, if the driver is bound to the PS/2 device, or a negative
+ * value if there is an error.
+ */
+static int __devinit xps2_of_probe(struct of_device *ofdev,
+				   const struct of_device_id *match)
 {
+	struct resource r_irq; /* Interrupt resources */
+	struct resource r_mem; /* IO mem resources */
 	struct xps2data *drvdata;
 	struct serio *serio;
-	unsigned long remap_size;
-	int retval;
+	struct device *dev = &ofdev->dev;
+	resource_size_t remap_size, phys_addr;
+	int error;
+
+	dev_info(dev, "Device Tree Probing \'%s\'\n",
+			ofdev->node->name);
 
-	if (!dev)
-		return -EINVAL;
+	/* Get iospace for the device */
+	error = of_address_to_resource(ofdev->node, 0, &r_mem);
+	if (error) {
+		dev_err(dev, "invalid address\n");
+		return error;
+	}
 
-	if (!regs_res || !irq_res) {
-		dev_err(dev, "IO resource(s) not found\n");
-		return -EINVAL;
+	/* Get IRQ for the device */
+	if (of_irq_to_resource(ofdev->node, 0, &r_irq) == NO_IRQ) {
+		dev_err(dev, "no IRQ found\n");
+		return -ENODEV;
 	}
 
 	drvdata = kzalloc(sizeof(struct xps2data), GFP_KERNEL);
@@ -241,24 +267,23 @@ static int xps2_setup(struct device *dev, struct resource *regs_res,
 	dev_set_drvdata(dev, drvdata);
 
 	spin_lock_init(&drvdata->lock);
-	drvdata->irq = irq_res->start;
-
-	remap_size = regs_res->end - regs_res->start + 1;
-	if (!request_mem_region(regs_res->start, remap_size, DRIVER_NAME)) {
-		dev_err(dev, "Couldn't lock memory region at 0x%08X\n",
-			(unsigned int)regs_res->start);
-		retval = -EBUSY;
+	drvdata->irq = r_irq.start;
+
+	phys_addr = r_mem.start;
+	remap_size = r_mem.end - r_mem.start + 1;
+	if (!request_mem_region(phys_addr, remap_size, DRIVER_NAME)) {
+		dev_err(dev, "Couldn't lock memory region at 0x%08llX\n",
+			(unsigned long long)phys_addr);
+		error = -EBUSY;
 		goto failed1;
 	}
 
 	/* Fill in configuration data and add them to the list */
-	drvdata->phys_addr = regs_res->start;
-	drvdata->remap_size = remap_size;
-	drvdata->base_address = ioremap(regs_res->start, remap_size);
+	drvdata->base_address = ioremap(phys_addr, remap_size);
 	if (drvdata->base_address == NULL) {
-		dev_err(dev, "Couldn't ioremap memory at 0x%08X\n",
-			(unsigned int)regs_res->start);
-		retval = -EFAULT;
+		dev_err(dev, "Couldn't ioremap memory at 0x%08llX\n",
+			(unsigned long long)phys_addr);
+		error = -EFAULT;
 		goto failed2;
 	}
 
@@ -269,8 +294,9 @@ static int xps2_setup(struct device *dev, struct resource *regs_res,
 	 * we have the PS2 in a good state */
 	out_be32(drvdata->base_address + XPS2_SRST_OFFSET, XPS2_SRST_RESET);
 
-	dev_info(dev, "Xilinx PS2 at 0x%08X mapped to 0x%p, irq=%d\n",
-		drvdata->phys_addr, drvdata->base_address, drvdata->irq);
+	dev_info(dev, "Xilinx PS2 at 0x%08llX mapped to 0x%p, irq=%d\n",
+		 (unsigned long long)phys_addr, drvdata->base_address,
+		 drvdata->irq);
 
 	serio = &drvdata->serio;
 	serio->id.type = SERIO_8042;
@@ -280,71 +306,51 @@ static int xps2_setup(struct device *dev, struct resource *regs_res,
 	serio->port_data = drvdata;
 	serio->dev.parent = dev;
 	snprintf(serio->name, sizeof(serio->name),
-		 "Xilinx XPS PS/2 at %08X", drvdata->phys_addr);
+		 "Xilinx XPS PS/2 at %08llX", (unsigned long long)phys_addr);
 	snprintf(serio->phys, sizeof(serio->phys),
-		 "xilinxps2/serio at %08X", drvdata->phys_addr);
+		 "xilinxps2/serio at %08llX", (unsigned long long)phys_addr);
+
 	serio_register_port(serio);
 
 	return 0;		/* success */
 
 failed2:
-	release_mem_region(regs_res->start, remap_size);
+	release_mem_region(phys_addr, remap_size);
 failed1:
 	kfree(drvdata);
 	dev_set_drvdata(dev, NULL);
 
-	return retval;
-}
-
-/***************************/
-/* OF Platform Bus Support */
-/***************************/
-
-static int __devinit xps2_of_probe(struct of_device *ofdev, const struct
-				   of_device_id * match)
-{
-	struct resource r_irq; /* Interrupt resources */
-	struct resource r_mem; /* IO mem resources */
-	int rc = 0;
-
-	printk(KERN_INFO "Device Tree Probing \'%s\'\n",
-			ofdev->node->name);
-
-	/* Get iospace for the device */
-	rc = of_address_to_resource(ofdev->node, 0, &r_mem);
-	if (rc) {
-		dev_err(&ofdev->dev, "invalid address\n");
-		return rc;
-	}
-
-	/* Get IRQ for the device */
-	rc = of_irq_to_resource(ofdev->node, 0, &r_irq);
-	if (rc == NO_IRQ) {
-		dev_err(&ofdev->dev, "no IRQ found\n");
-		return rc;
-	}
-
-	return xps2_setup(&ofdev->dev, &r_mem, &r_irq);
+	return error;
 }
 
+/**
+ * xps2_of_remove - unbinds the driver from the PS/2 device.
+ * @of_dev:	pointer to OF device structure
+ *
+ * This function is called if a device is physically removed from the system or
+ * if the driver module is being unloaded. It frees any resources allocated to
+ * the device.
+ */
 static int __devexit xps2_of_remove(struct of_device *of_dev)
 {
 	struct device *dev = &of_dev->dev;
-	struct xps2data *drvdata;
-
-	if (!dev)
-		return -EINVAL;
-
-	drvdata = dev_get_drvdata(dev);
+	struct xps2data *drvdata = dev_get_drvdata(dev);
+	struct resource r_mem; /* IO mem resources */
 
 	serio_unregister_port(&drvdata->serio);
 	iounmap(drvdata->base_address);
-	release_mem_region(drvdata->phys_addr, drvdata->remap_size);
+
+	/* Get iospace of the device */
+	if (of_address_to_resource(of_dev->node, 0, &r_mem))
+		dev_err(dev, "invalid address\n");
+	else
+		release_mem_region(r_mem.start, r_mem.end - r_mem.start + 1);
+
 	kfree(drvdata);
 
 	dev_set_drvdata(dev, NULL);
 
-	return 0;		/* success */
+	return 0;
 }
 
 /* Match table for of_platform binding */
diff --git a/drivers/input/tablet/gtco.c b/drivers/input/tablet/gtco.c
index 5524e01dbb1..2e18a1c0c35 100644
--- a/drivers/input/tablet/gtco.c
+++ b/drivers/input/tablet/gtco.c
@@ -877,7 +877,7 @@ static int gtco_probe(struct usb_interface *usbinterface,
 	dbg("num endpoints:     %d", usbinterface->cur_altsetting->desc.bNumEndpoints);
 	dbg("interface class:   %d", usbinterface->cur_altsetting->desc.bInterfaceClass);
 	dbg("endpoint: attribute:0x%x type:0x%x", endpoint->bmAttributes, endpoint->bDescriptorType);
-	if ((endpoint->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK) == USB_ENDPOINT_XFER_INT)
+	if (usb_endpoint_xfer_int(endpoint))
 		dbg("endpoint: we have interrupt endpoint\n");
 
 	dbg("endpoint extra len:%d ", usbinterface->altsetting[0].extralen);
diff --git a/drivers/input/tablet/wacom_wac.c b/drivers/input/tablet/wacom_wac.c
index 8dc8d1e59be..2638811c61a 100644
--- a/drivers/input/tablet/wacom_wac.c
+++ b/drivers/input/tablet/wacom_wac.c
@@ -535,7 +535,7 @@ static int wacom_intuos_irq(struct wacom_wac *wacom, void *wcombo)
 	return 1;
 }
 
-int wacom_tpc_irq(struct wacom_wac *wacom, void *wcombo)
+static int wacom_tpc_irq(struct wacom_wac *wacom, void *wcombo)
 {
 	char *data = wacom->data;
 	int prox = 0, pressure;
diff --git a/drivers/input/touchscreen/Kconfig b/drivers/input/touchscreen/Kconfig
index 3d1ab8fa9ac..bb6486a8c07 100644
--- a/drivers/input/touchscreen/Kconfig
+++ b/drivers/input/touchscreen/Kconfig
@@ -58,6 +58,14 @@ config TOUCHSCREEN_CORGI
 	  NOTE: this driver is deprecated, try enable SPI and generic
 	  ADS7846-based touchscreen driver.
 
+config TOUCHSCREEN_DA9034
+	tristate "Touchscreen support for Dialog Semiconductor DA9034"
+	depends on PMIC_DA903X
+	default y
+	help
+	  Say Y here to enable the support for the touchscreen found
+	  on Dialog Semiconductor DA9034 PMIC.
+
 config TOUCHSCREEN_FUJITSU
 	tristate "Fujitsu serial touchscreen"
 	select SERIO
@@ -95,6 +103,19 @@ config TOUCHSCREEN_ELO
 	  To compile this driver as a module, choose M here: the
 	  module will be called elo.
 
+config TOUCHSCREEN_WACOM_W8001
+	tristate "Wacom W8001 penabled serial touchscreen"
+	select SERIO
+	help
+	  Say Y here if you have an Wacom W8001 penabled serial touchscreen
+	  connected to your system.
+
+	  If unsure, say N.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called wacom_w8001.
+
+
 config TOUCHSCREEN_MTOUCH
 	tristate "MicroTouch serial touchscreens"
 	select SERIO
@@ -376,4 +397,15 @@ config TOUCHSCREEN_TOUCHIT213
 	  To compile this driver as a module, choose M here: the
 	  module will be called touchit213.
 
+config TOUCHSCREEN_TSC2007
+	tristate "TSC2007 based touchscreens"
+	depends on I2C
+	help
+	  Say Y here if you have a TSC2007 based touchscreen.
+
+	  If unsure, say N.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called tsc2007.
+
 endif
diff --git a/drivers/input/touchscreen/Makefile b/drivers/input/touchscreen/Makefile
index 15cf2907948..d3375aff46f 100644
--- a/drivers/input/touchscreen/Makefile
+++ b/drivers/input/touchscreen/Makefile
@@ -25,8 +25,11 @@ obj-$(CONFIG_TOUCHSCREEN_PENMOUNT)	+= penmount.o
 obj-$(CONFIG_TOUCHSCREEN_TOUCHIT213)	+= touchit213.o
 obj-$(CONFIG_TOUCHSCREEN_TOUCHRIGHT)	+= touchright.o
 obj-$(CONFIG_TOUCHSCREEN_TOUCHWIN)	+= touchwin.o
+obj-$(CONFIG_TOUCHSCREEN_TSC2007)	+= tsc2007.o
 obj-$(CONFIG_TOUCHSCREEN_UCB1400)	+= ucb1400_ts.o
+obj-$(CONFIG_TOUCHSCREEN_WACOM_W8001)	+= wacom_w8001.o
 obj-$(CONFIG_TOUCHSCREEN_WM97XX)	+= wm97xx-ts.o
+obj-$(CONFIG_TOUCHSCREEN_DA9034)	+= da9034-ts.o
 wm97xx-ts-$(CONFIG_TOUCHSCREEN_WM9705)	+= wm9705.o
 wm97xx-ts-$(CONFIG_TOUCHSCREEN_WM9712)	+= wm9712.o
 wm97xx-ts-$(CONFIG_TOUCHSCREEN_WM9713)	+= wm9713.o
diff --git a/drivers/input/touchscreen/ads7846.c b/drivers/input/touchscreen/ads7846.c
index e1ece89fe92..7c27c8b9b6d 100644
--- a/drivers/input/touchscreen/ads7846.c
+++ b/drivers/input/touchscreen/ads7846.c
@@ -472,7 +472,7 @@ static ssize_t ads7846_disable_store(struct device *dev,
 				     const char *buf, size_t count)
 {
 	struct ads7846 *ts = dev_get_drvdata(dev);
-	long i;
+	unsigned long i;
 
 	if (strict_strtoul(buf, 10, &i))
 		return -EINVAL;
@@ -559,7 +559,7 @@ static void ads7846_rx(void *ads)
 	if (packet->tc.ignore || Rt > ts->pressure_max) {
 #ifdef VERBOSE
 		pr_debug("%s: ignored %d pressure %d\n",
-			ts->spi->dev.bus_id, packet->tc.ignore, Rt);
+			dev_name(&ts->spi->dev), packet->tc.ignore, Rt);
 #endif
 		hrtimer_start(&ts->timer, ktime_set(0, TS_POLL_PERIOD),
 			      HRTIMER_MODE_REL);
@@ -947,7 +947,7 @@ static int __devinit ads7846_probe(struct spi_device *spi)
 		ts->penirq_recheck_delay_usecs =
 				pdata->penirq_recheck_delay_usecs;
 
-	snprintf(ts->phys, sizeof(ts->phys), "%s/input0", spi->dev.bus_id);
+	snprintf(ts->phys, sizeof(ts->phys), "%s/input0", dev_name(&spi->dev));
 
 	input_dev->name = "ADS784x Touchscreen";
 	input_dev->phys = ts->phys;
diff --git a/drivers/input/touchscreen/da9034-ts.c b/drivers/input/touchscreen/da9034-ts.c
new file mode 100644
index 00000000000..4342e77814b
--- /dev/null
+++ b/drivers/input/touchscreen/da9034-ts.c
@@ -0,0 +1,389 @@
+/*
+ * Touchscreen driver for Dialog Semiconductor DA9034
+ *
+ * Copyright (C) 2006-2008 Marvell International Ltd.
+ *	Fengwei Yin <fengwei.yin@marvell.com>
+ *	Eric Miao <eric.miao@marvell.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/input.h>
+#include <linux/mfd/da903x.h>
+
+#define DA9034_MANUAL_CTRL	0x50
+#define DA9034_LDO_ADC_EN	(1 << 4)
+
+#define DA9034_AUTO_CTRL1	0x51
+
+#define DA9034_AUTO_CTRL2	0x52
+#define DA9034_AUTO_TSI_EN	(1 << 3)
+#define DA9034_PEN_DETECT	(1 << 4)
+
+#define DA9034_TSI_CTRL1	0x53
+#define DA9034_TSI_CTRL2	0x54
+#define DA9034_TSI_X_MSB	0x6c
+#define DA9034_TSI_Y_MSB	0x6d
+#define DA9034_TSI_XY_LSB	0x6e
+
+enum {
+	STATE_IDLE,	/* wait for pendown */
+	STATE_BUSY,	/* TSI busy sampling */
+	STATE_STOP,	/* sample available */
+	STATE_WAIT,	/* Wait to start next sample */
+};
+
+enum {
+	EVENT_PEN_DOWN,
+	EVENT_PEN_UP,
+	EVENT_TSI_READY,
+	EVENT_TIMEDOUT,
+};
+
+struct da9034_touch {
+	struct device		*da9034_dev;
+	struct input_dev	*input_dev;
+
+	struct delayed_work	tsi_work;
+	struct notifier_block	notifier;
+
+	int	state;
+
+	int	interval_ms;
+	int	x_inverted;
+	int	y_inverted;
+
+	int	last_x;
+	int	last_y;
+};
+
+static inline int is_pen_down(struct da9034_touch *touch)
+{
+	return da903x_query_status(touch->da9034_dev, DA9034_STATUS_PEN_DOWN);
+}
+
+static inline int detect_pen_down(struct da9034_touch *touch, int on)
+{
+	if (on)
+		return da903x_set_bits(touch->da9034_dev,
+				DA9034_AUTO_CTRL2, DA9034_PEN_DETECT);
+	else
+		return da903x_clr_bits(touch->da9034_dev,
+				DA9034_AUTO_CTRL2, DA9034_PEN_DETECT);
+}
+
+static int read_tsi(struct da9034_touch *touch)
+{
+	uint8_t _x, _y, _v;
+	int ret;
+
+	ret = da903x_read(touch->da9034_dev, DA9034_TSI_X_MSB, &_x);
+	if (ret)
+		return ret;
+
+	ret = da903x_read(touch->da9034_dev, DA9034_TSI_Y_MSB, &_y);
+	if (ret)
+		return ret;
+
+	ret = da903x_read(touch->da9034_dev, DA9034_TSI_XY_LSB, &_v);
+	if (ret)
+		return ret;
+
+	touch->last_x = ((_x << 2) & 0x3fc) | (_v & 0x3);
+	touch->last_y = ((_y << 2) & 0x3fc) | ((_v & 0xc) >> 2);
+
+	return 0;
+}
+
+static inline int start_tsi(struct da9034_touch *touch)
+{
+	return da903x_set_bits(touch->da9034_dev,
+			DA9034_AUTO_CTRL2, DA9034_AUTO_TSI_EN);
+}
+
+static inline int stop_tsi(struct da9034_touch *touch)
+{
+	return da903x_clr_bits(touch->da9034_dev,
+			DA9034_AUTO_CTRL2, DA9034_AUTO_TSI_EN);
+}
+
+static inline void report_pen_down(struct da9034_touch *touch)
+{
+	int x = touch->last_x;
+	int y = touch->last_y;
+
+	x &= 0xfff;
+	if (touch->x_inverted)
+		x = 1024 - x;
+	y &= 0xfff;
+	if (touch->y_inverted)
+		y = 1024 - y;
+
+	input_report_abs(touch->input_dev, ABS_X, x);
+	input_report_abs(touch->input_dev, ABS_Y, y);
+	input_report_key(touch->input_dev, BTN_TOUCH, 1);
+
+	input_sync(touch->input_dev);
+}
+
+static inline void report_pen_up(struct da9034_touch *touch)
+{
+	input_report_key(touch->input_dev, BTN_TOUCH, 0);
+	input_sync(touch->input_dev);
+}
+
+static void da9034_event_handler(struct da9034_touch *touch, int event)
+{
+	int err;
+
+	switch (touch->state) {
+	case STATE_IDLE:
+		if (event != EVENT_PEN_DOWN)
+			break;
+
+		/* Enable auto measurement of the TSI, this will
+		 * automatically disable pen down detection
+		 */
+		err = start_tsi(touch);
+		if (err)
+			goto err_reset;
+
+		touch->state = STATE_BUSY;
+		break;
+
+	case STATE_BUSY:
+		if (event != EVENT_TSI_READY)
+			break;
+
+		err = read_tsi(touch);
+		if (err)
+			goto err_reset;
+
+		/* Disable auto measurement of the TSI, so that
+		 * pen down status will be available
+		 */
+		err = stop_tsi(touch);
+		if (err)
+			goto err_reset;
+
+		touch->state = STATE_STOP;
+		break;
+
+	case STATE_STOP:
+		if (event == EVENT_PEN_DOWN) {
+			report_pen_down(touch);
+			schedule_delayed_work(&touch->tsi_work,
+				msecs_to_jiffies(touch->interval_ms));
+			touch->state = STATE_WAIT;
+		}
+
+		if (event == EVENT_PEN_UP) {
+			report_pen_up(touch);
+			touch->state = STATE_IDLE;
+		}
+
+		input_sync(touch->input_dev);
+		break;
+
+	case STATE_WAIT:
+		if (event != EVENT_TIMEDOUT)
+			break;
+
+		if (is_pen_down(touch)) {
+			start_tsi(touch);
+			touch->state = STATE_BUSY;
+		} else
+			touch->state = STATE_IDLE;
+		break;
+	}
+	return;
+
+err_reset:
+	touch->state = STATE_IDLE;
+	stop_tsi(touch);
+	detect_pen_down(touch, 1);
+}
+
+static void da9034_tsi_work(struct work_struct *work)
+{
+	struct da9034_touch *touch =
+		container_of(work, struct da9034_touch, tsi_work.work);
+
+	da9034_event_handler(touch, EVENT_TIMEDOUT);
+}
+
+static int da9034_touch_notifier(struct notifier_block *nb,
+				 unsigned long event, void *data)
+{
+	struct da9034_touch *touch =
+		container_of(nb, struct da9034_touch, notifier);
+
+	if (event & DA9034_EVENT_PEN_DOWN) {
+		if (is_pen_down(touch))
+			da9034_event_handler(touch, EVENT_PEN_DOWN);
+		else
+			da9034_event_handler(touch, EVENT_PEN_UP);
+	}
+
+	if (event & DA9034_EVENT_TSI_READY)
+		da9034_event_handler(touch, EVENT_TSI_READY);
+
+	return 0;
+}
+
+static int da9034_touch_open(struct input_dev *dev)
+{
+	struct da9034_touch *touch = input_get_drvdata(dev);
+	int ret;
+
+	ret = da903x_register_notifier(touch->da9034_dev, &touch->notifier,
+			DA9034_EVENT_PEN_DOWN | DA9034_EVENT_TSI_READY);
+	if (ret)
+		return -EBUSY;
+
+	/* Enable ADC LDO */
+	ret = da903x_set_bits(touch->da9034_dev,
+			DA9034_MANUAL_CTRL, DA9034_LDO_ADC_EN);
+	if (ret)
+		return ret;
+
+	/* TSI_DELAY: 3 slots, TSI_SKIP: 3 slots */
+	ret = da903x_write(touch->da9034_dev, DA9034_TSI_CTRL1, 0x1b);
+	if (ret)
+		return ret;
+
+	ret = da903x_write(touch->da9034_dev, DA9034_TSI_CTRL2, 0x00);
+	if (ret)
+		return ret;
+
+	touch->state = STATE_IDLE;
+	detect_pen_down(touch, 1);
+
+	return 0;
+}
+
+static void da9034_touch_close(struct input_dev *dev)
+{
+	struct da9034_touch *touch = input_get_drvdata(dev);
+
+	da903x_unregister_notifier(touch->da9034_dev, &touch->notifier,
+			DA9034_EVENT_PEN_DOWN | DA9034_EVENT_TSI_READY);
+
+	cancel_delayed_work_sync(&touch->tsi_work);
+
+	touch->state = STATE_IDLE;
+	stop_tsi(touch);
+	detect_pen_down(touch, 0);
+
+	/* Disable ADC LDO */
+	da903x_clr_bits(touch->da9034_dev,
+			DA9034_MANUAL_CTRL, DA9034_LDO_ADC_EN);
+}
+
+
+static int __devinit da9034_touch_probe(struct platform_device *pdev)
+{
+	struct da9034_touch_pdata *pdata = pdev->dev.platform_data;
+	struct da9034_touch *touch;
+	struct input_dev *input_dev;
+	int ret;
+
+	touch = kzalloc(sizeof(struct da9034_touch), GFP_KERNEL);
+	if (touch == NULL) {
+		dev_err(&pdev->dev, "failed to allocate driver data\n");
+		return -ENOMEM;
+	}
+
+	touch->da9034_dev = pdev->dev.parent;
+
+	if (pdata) {
+		touch->interval_ms	= pdata->interval_ms;
+		touch->x_inverted	= pdata->x_inverted;
+		touch->y_inverted	= pdata->y_inverted;
+	} else
+		/* fallback into default */
+		touch->interval_ms	= 10;
+
+	INIT_DELAYED_WORK(&touch->tsi_work, da9034_tsi_work);
+	touch->notifier.notifier_call = da9034_touch_notifier;
+
+	input_dev = input_allocate_device();
+	if (!input_dev) {
+		dev_err(&pdev->dev, "failed to allocate input device\n");
+		ret = -ENOMEM;
+		goto err_free_touch;
+	}
+
+	input_dev->name		= pdev->name;
+	input_dev->open		= da9034_touch_open;
+	input_dev->close	= da9034_touch_close;
+	input_dev->dev.parent	= &pdev->dev;
+
+	__set_bit(EV_ABS, input_dev->evbit);
+	__set_bit(ABS_X, input_dev->absbit);
+	__set_bit(ABS_Y, input_dev->absbit);
+	input_set_abs_params(input_dev, ABS_X, 0, 1023, 0, 0);
+	input_set_abs_params(input_dev, ABS_Y, 0, 1023, 0, 0);
+
+	__set_bit(EV_KEY, input_dev->evbit);
+	__set_bit(BTN_TOUCH, input_dev->keybit);
+
+	touch->input_dev = input_dev;
+	input_set_drvdata(input_dev, touch);
+
+	ret = input_register_device(input_dev);
+	if (ret)
+		goto err_free_input;
+
+	platform_set_drvdata(pdev, touch);
+	return 0;
+
+err_free_input:
+	input_free_device(input_dev);
+err_free_touch:
+	kfree(touch);
+	return ret;
+}
+
+static int __devexit da9034_touch_remove(struct platform_device *pdev)
+{
+	struct da9034_touch *touch = platform_get_drvdata(pdev);
+
+	input_unregister_device(touch->input_dev);
+	kfree(touch);
+
+	return 0;
+}
+
+static struct platform_driver da9034_touch_driver = {
+	.driver	= {
+		.name	= "da9034-touch",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= da9034_touch_probe,
+	.remove		= __devexit_p(da9034_touch_remove),
+};
+
+static int __init da9034_touch_init(void)
+{
+	return platform_driver_register(&da9034_touch_driver);
+}
+module_init(da9034_touch_init);
+
+static void __exit da9034_touch_exit(void)
+{
+	platform_driver_unregister(&da9034_touch_driver);
+}
+module_exit(da9034_touch_exit);
+
+MODULE_DESCRIPTION("Touchscreen driver for Dialog Semiconductor DA9034");
+MODULE_AUTHOR("Eric Miao <eric.miao@marvell.com>");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:da9034-touch");
diff --git a/drivers/input/touchscreen/tsc2007.c b/drivers/input/touchscreen/tsc2007.c
new file mode 100644
index 00000000000..b75dc299057
--- /dev/null
+++ b/drivers/input/touchscreen/tsc2007.c
@@ -0,0 +1,381 @@
+/*
+ * drivers/input/touchscreen/tsc2007.c
+ *
+ * Copyright (c) 2008 MtekVision Co., Ltd.
+ *	Kwangwoo Lee <kwlee@mtekvision.com>
+ *
+ * Using code from:
+ *  - ads7846.c
+ *	Copyright (c) 2005 David Brownell
+ *	Copyright (c) 2006 Nokia Corporation
+ *  - corgi_ts.c
+ *	Copyright (C) 2004-2005 Richard Purdie
+ *  - omap_ts.[hc], ads7846.h, ts_osk.c
+ *	Copyright (C) 2002 MontaVista Software
+ *	Copyright (C) 2004 Texas Instruments
+ *	Copyright (C) 2005 Dirk Behme
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License version 2 as
+ *  published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/hrtimer.h>
+#include <linux/slab.h>
+#include <linux/input.h>
+#include <linux/interrupt.h>
+#include <linux/i2c.h>
+#include <linux/i2c/tsc2007.h>
+
+#define TS_POLL_DELAY	(10 * 1000)	/* ns delay before the first sample */
+#define TS_POLL_PERIOD	(5 * 1000)	/* ns delay between samples */
+
+#define TSC2007_MEASURE_TEMP0		(0x0 << 4)
+#define TSC2007_MEASURE_AUX		(0x2 << 4)
+#define TSC2007_MEASURE_TEMP1		(0x4 << 4)
+#define TSC2007_ACTIVATE_XN		(0x8 << 4)
+#define TSC2007_ACTIVATE_YN		(0x9 << 4)
+#define TSC2007_ACTIVATE_YP_XN		(0xa << 4)
+#define TSC2007_SETUP			(0xb << 4)
+#define TSC2007_MEASURE_X		(0xc << 4)
+#define TSC2007_MEASURE_Y		(0xd << 4)
+#define TSC2007_MEASURE_Z1		(0xe << 4)
+#define TSC2007_MEASURE_Z2		(0xf << 4)
+
+#define TSC2007_POWER_OFF_IRQ_EN	(0x0 << 2)
+#define TSC2007_ADC_ON_IRQ_DIS0		(0x1 << 2)
+#define TSC2007_ADC_OFF_IRQ_EN		(0x2 << 2)
+#define TSC2007_ADC_ON_IRQ_DIS1		(0x3 << 2)
+
+#define TSC2007_12BIT			(0x0 << 1)
+#define TSC2007_8BIT			(0x1 << 1)
+
+#define	MAX_12BIT			((1 << 12) - 1)
+
+#define ADC_ON_12BIT	(TSC2007_12BIT | TSC2007_ADC_ON_IRQ_DIS0)
+
+#define READ_Y		(ADC_ON_12BIT | TSC2007_MEASURE_Y)
+#define READ_Z1		(ADC_ON_12BIT | TSC2007_MEASURE_Z1)
+#define READ_Z2		(ADC_ON_12BIT | TSC2007_MEASURE_Z2)
+#define READ_X		(ADC_ON_12BIT | TSC2007_MEASURE_X)
+#define PWRDOWN		(TSC2007_12BIT | TSC2007_POWER_OFF_IRQ_EN)
+
+struct ts_event {
+	u16	x;
+	u16	y;
+	u16	z1, z2;
+};
+
+struct tsc2007 {
+	struct input_dev	*input;
+	char			phys[32];
+	struct hrtimer		timer;
+	struct ts_event		tc;
+
+	struct i2c_client	*client;
+
+	spinlock_t		lock;
+
+	u16			model;
+	u16			x_plate_ohms;
+
+	unsigned		pendown;
+	int			irq;
+
+	int			(*get_pendown_state)(void);
+	void			(*clear_penirq)(void);
+};
+
+static inline int tsc2007_xfer(struct tsc2007 *tsc, u8 cmd)
+{
+	s32 data;
+	u16 val;
+
+	data = i2c_smbus_read_word_data(tsc->client, cmd);
+	if (data < 0) {
+		dev_err(&tsc->client->dev, "i2c io error: %d\n", data);
+		return data;
+	}
+
+	/* The protocol and raw data format from i2c interface:
+	 * S Addr Wr [A] Comm [A] S Addr Rd [A] [DataLow] A [DataHigh] NA P
+	 * Where DataLow has [D11-D4], DataHigh has [D3-D0 << 4 | Dummy 4bit].
+	 */
+	val = swab16(data) >> 4;
+
+	dev_dbg(&tsc->client->dev, "data: 0x%x, val: 0x%x\n", data, val);
+
+	return val;
+}
+
+static void tsc2007_send_event(void *tsc)
+{
+	struct tsc2007	*ts = tsc;
+	u32		rt;
+	u16		x, y, z1, z2;
+
+	x = ts->tc.x;
+	y = ts->tc.y;
+	z1 = ts->tc.z1;
+	z2 = ts->tc.z2;
+
+	/* range filtering */
+	if (x == MAX_12BIT)
+		x = 0;
+
+	if (likely(x && z1)) {
+		/* compute touch pressure resistance using equation #1 */
+		rt = z2;
+		rt -= z1;
+		rt *= x;
+		rt *= ts->x_plate_ohms;
+		rt /= z1;
+		rt = (rt + 2047) >> 12;
+	} else
+		rt = 0;
+
+	/* Sample found inconsistent by debouncing or pressure is beyond
+	 * the maximum. Don't report it to user space, repeat at least
+	 * once more the measurement
+	 */
+	if (rt > MAX_12BIT) {
+		dev_dbg(&ts->client->dev, "ignored pressure %d\n", rt);
+
+		hrtimer_start(&ts->timer, ktime_set(0, TS_POLL_PERIOD),
+			      HRTIMER_MODE_REL);
+		return;
+	}
+
+	/* NOTE: We can't rely on the pressure to determine the pen down
+	 * state, even this controller has a pressure sensor.  The pressure
+	 * value can fluctuate for quite a while after lifting the pen and
+	 * in some cases may not even settle at the expected value.
+	 *
+	 * The only safe way to check for the pen up condition is in the
+	 * timer by reading the pen signal state (it's a GPIO _and_ IRQ).
+	 */
+	if (rt) {
+		struct input_dev *input = ts->input;
+
+		if (!ts->pendown) {
+			dev_dbg(&ts->client->dev, "DOWN\n");
+
+			input_report_key(input, BTN_TOUCH, 1);
+			ts->pendown = 1;
+		}
+
+		input_report_abs(input, ABS_X, x);
+		input_report_abs(input, ABS_Y, y);
+		input_report_abs(input, ABS_PRESSURE, rt);
+
+		input_sync(input);
+
+		dev_dbg(&ts->client->dev, "point(%4d,%4d), pressure (%4u)\n",
+			x, y, rt);
+	}
+
+	hrtimer_start(&ts->timer, ktime_set(0, TS_POLL_PERIOD),
+			HRTIMER_MODE_REL);
+}
+
+static int tsc2007_read_values(struct tsc2007 *tsc)
+{
+	/* y- still on; turn on only y+ (and ADC) */
+	tsc->tc.y = tsc2007_xfer(tsc, READ_Y);
+
+	/* turn y- off, x+ on, then leave in lowpower */
+	tsc->tc.x = tsc2007_xfer(tsc, READ_X);
+
+	/* turn y+ off, x- on; we'll use formula #1 */
+	tsc->tc.z1 = tsc2007_xfer(tsc, READ_Z1);
+	tsc->tc.z2 = tsc2007_xfer(tsc, READ_Z2);
+
+	/* power down */
+	tsc2007_xfer(tsc, PWRDOWN);
+
+	return 0;
+}
+
+static enum hrtimer_restart tsc2007_timer(struct hrtimer *handle)
+{
+	struct tsc2007 *ts = container_of(handle, struct tsc2007, timer);
+
+	spin_lock_irq(&ts->lock);
+
+	if (unlikely(!ts->get_pendown_state() && ts->pendown)) {
+		struct input_dev *input = ts->input;
+
+		dev_dbg(&ts->client->dev, "UP\n");
+
+		input_report_key(input, BTN_TOUCH, 0);
+		input_report_abs(input, ABS_PRESSURE, 0);
+		input_sync(input);
+
+		ts->pendown = 0;
+		enable_irq(ts->irq);
+	} else {
+		/* pen is still down, continue with the measurement */
+		dev_dbg(&ts->client->dev, "pen is still down\n");
+
+		tsc2007_read_values(ts);
+		tsc2007_send_event(ts);
+	}
+
+	spin_unlock_irq(&ts->lock);
+
+	return HRTIMER_NORESTART;
+}
+
+static irqreturn_t tsc2007_irq(int irq, void *handle)
+{
+	struct tsc2007 *ts = handle;
+	unsigned long flags;
+
+	spin_lock_irqsave(&ts->lock, flags);
+
+	if (likely(ts->get_pendown_state())) {
+		disable_irq(ts->irq);
+		hrtimer_start(&ts->timer, ktime_set(0, TS_POLL_DELAY),
+					HRTIMER_MODE_REL);
+	}
+
+	if (ts->clear_penirq)
+		ts->clear_penirq();
+
+	spin_unlock_irqrestore(&ts->lock, flags);
+
+	return IRQ_HANDLED;
+}
+
+static int tsc2007_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct tsc2007 *ts;
+	struct tsc2007_platform_data *pdata = pdata = client->dev.platform_data;
+	struct input_dev *input_dev;
+	int err;
+
+	if (!pdata) {
+		dev_err(&client->dev, "platform data is required!\n");
+		return -EINVAL;
+	}
+
+	if (!i2c_check_functionality(client->adapter,
+				     I2C_FUNC_SMBUS_READ_WORD_DATA))
+		return -EIO;
+
+	ts = kzalloc(sizeof(struct tsc2007), GFP_KERNEL);
+	input_dev = input_allocate_device();
+	if (!ts || !input_dev) {
+		err = -ENOMEM;
+		goto err_free_mem;
+	}
+
+	ts->client = client;
+	i2c_set_clientdata(client, ts);
+
+	ts->input = input_dev;
+
+	hrtimer_init(&ts->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	ts->timer.function = tsc2007_timer;
+
+	spin_lock_init(&ts->lock);
+
+	ts->model             = pdata->model;
+	ts->x_plate_ohms      = pdata->x_plate_ohms;
+	ts->get_pendown_state = pdata->get_pendown_state;
+	ts->clear_penirq      = pdata->clear_penirq;
+
+	pdata->init_platform_hw();
+
+	snprintf(ts->phys, sizeof(ts->phys), "%s/input0", client->dev.bus_id);
+
+	input_dev->name = "TSC2007 Touchscreen";
+	input_dev->phys = ts->phys;
+	input_dev->id.bustype = BUS_I2C;
+
+	input_dev->evbit[0] = BIT_MASK(EV_KEY) | BIT_MASK(EV_ABS);
+	input_dev->keybit[BIT_WORD(BTN_TOUCH)] = BIT_MASK(BTN_TOUCH);
+
+	input_set_abs_params(input_dev, ABS_X, 0, MAX_12BIT, 0, 0);
+	input_set_abs_params(input_dev, ABS_Y, 0, MAX_12BIT, 0, 0);
+	input_set_abs_params(input_dev, ABS_PRESSURE, 0, MAX_12BIT, 0, 0);
+
+	tsc2007_read_values(ts);
+
+	ts->irq = client->irq;
+
+	err = request_irq(ts->irq, tsc2007_irq, 0,
+			client->dev.driver->name, ts);
+	if (err < 0) {
+		dev_err(&client->dev, "irq %d busy?\n", ts->irq);
+		goto err_free_mem;
+	}
+
+	err = input_register_device(input_dev);
+	if (err)
+		goto err_free_irq;
+
+	dev_info(&client->dev, "registered with irq (%d)\n", ts->irq);
+
+	return 0;
+
+ err_free_irq:
+	free_irq(ts->irq, ts);
+	hrtimer_cancel(&ts->timer);
+ err_free_mem:
+	input_free_device(input_dev);
+	kfree(ts);
+	return err;
+}
+
+static int tsc2007_remove(struct i2c_client *client)
+{
+	struct tsc2007	*ts = i2c_get_clientdata(client);
+	struct tsc2007_platform_data *pdata;
+
+	pdata = client->dev.platform_data;
+	pdata->exit_platform_hw();
+
+	free_irq(ts->irq, ts);
+	hrtimer_cancel(&ts->timer);
+	input_unregister_device(ts->input);
+	kfree(ts);
+
+	return 0;
+}
+
+static struct i2c_device_id tsc2007_idtable[] = {
+	{ "tsc2007", 0 },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(i2c, tsc2007_idtable);
+
+static struct i2c_driver tsc2007_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "tsc2007"
+	},
+	.id_table	= tsc2007_idtable,
+	.probe		= tsc2007_probe,
+	.remove		= tsc2007_remove,
+};
+
+static int __init tsc2007_init(void)
+{
+	return i2c_add_driver(&tsc2007_driver);
+}
+
+static void __exit tsc2007_exit(void)
+{
+	i2c_del_driver(&tsc2007_driver);
+}
+
+module_init(tsc2007_init);
+module_exit(tsc2007_exit);
+
+MODULE_AUTHOR("Kwangwoo Lee <kwlee@mtekvision.com>");
+MODULE_DESCRIPTION("TSC2007 TouchScreen Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/touchscreen/usbtouchscreen.c b/drivers/input/touchscreen/usbtouchscreen.c
index fdd645c214a..5080b26ba16 100644
--- a/drivers/input/touchscreen/usbtouchscreen.c
+++ b/drivers/input/touchscreen/usbtouchscreen.c
@@ -424,7 +424,7 @@ static int dmc_tsc10_init(struct usbtouch_usb *usbtouch)
 	                      0, 0, buf, 2, USB_CTRL_SET_TIMEOUT);
 	if (ret < 0)
 		goto err_out;
-	if (buf[0] != 0x06 || buf[1] != 0x00) {
+	if (buf[0] != 0x06) {
 		ret = -ENODEV;
 		goto err_out;
 	}
@@ -437,8 +437,7 @@ static int dmc_tsc10_init(struct usbtouch_usb *usbtouch)
 	                      TSC10_RATE_150, 0, buf, 2, USB_CTRL_SET_TIMEOUT);
 	if (ret < 0)
 		goto err_out;
-	if ((buf[0] != 0x06 || buf[1] != 0x00) &&
-	    (buf[0] != 0x15 || buf[1] != 0x01)) {
+	if ((buf[0] != 0x06) && (buf[0] != 0x15 || buf[1] != 0x01)) {
 		ret = -ENODEV;
 		goto err_out;
 	}
diff --git a/drivers/input/touchscreen/wacom_w8001.c b/drivers/input/touchscreen/wacom_w8001.c
new file mode 100644
index 00000000000..2f33a016764
--- /dev/null
+++ b/drivers/input/touchscreen/wacom_w8001.c
@@ -0,0 +1,325 @@
+/*
+ * Wacom W8001 penabled serial touchscreen driver
+ *
+ * Copyright (c) 2008 Jaya Kumar
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License. See the file COPYING in the main directory of this archive for
+ * more details.
+ *
+ * Layout based on Elo serial touchscreen driver by Vojtech Pavlik
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/input.h>
+#include <linux/serio.h>
+#include <linux/init.h>
+#include <linux/ctype.h>
+
+#define DRIVER_DESC	"Wacom W8001 serial touchscreen driver"
+
+MODULE_AUTHOR("Jaya Kumar <jayakumar.lkml@gmail.com>");
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_LICENSE("GPL");
+
+/*
+ * Definitions & global arrays.
+ */
+
+#define W8001_MAX_LENGTH	11
+#define W8001_PACKET_LEN	11
+#define W8001_LEAD_MASK 0x80
+#define W8001_LEAD_BYTE 0x80
+#define W8001_TAB_MASK 0x40
+#define W8001_TAB_BYTE 0x40
+
+#define W8001_QUERY_PACKET 0x20
+
+struct w8001_coord {
+	u8 rdy;
+	u8 tsw;
+	u8 f1;
+	u8 f2;
+	u16 x;
+	u16 y;
+	u16 pen_pressure;
+	u8 tilt_x;
+	u8 tilt_y;
+};
+
+/*
+ * Per-touchscreen data.
+ */
+
+struct w8001 {
+	struct input_dev *dev;
+	struct serio *serio;
+	struct mutex cmd_mutex;
+	struct completion cmd_done;
+	int id;
+	int idx;
+	unsigned char expected_packet;
+	unsigned char data[W8001_MAX_LENGTH];
+	unsigned char response[W8001_PACKET_LEN];
+	char phys[32];
+};
+
+static int parse_data(u8 *data, struct w8001_coord *coord)
+{
+	coord->rdy = data[0] & 0x20;
+	coord->tsw = data[0] & 0x01;
+	coord->f1 = data[0] & 0x02;
+	coord->f2 = data[0] & 0x04;
+
+	coord->x = (data[1] & 0x7F) << 9;
+	coord->x |= (data[2] & 0x7F) << 2;
+	coord->x |= (data[6] & 0x60) >> 5;
+
+	coord->y = (data[3] & 0x7F) << 9;
+	coord->y |= (data[4] & 0x7F) << 2;
+	coord->y |= (data[6] & 0x18) >> 3;
+
+	coord->pen_pressure = data[5] & 0x7F;
+	coord->pen_pressure |= (data[6] & 0x07) << 7 ;
+
+	coord->tilt_x = data[7] & 0x7F;
+	coord->tilt_y = data[8] & 0x7F;
+
+	return 0;
+}
+
+static void w8001_process_data(struct w8001 *w8001, unsigned char data)
+{
+	struct input_dev *dev = w8001->dev;
+	u8 tmp;
+	struct w8001_coord coord;
+
+	w8001->data[w8001->idx] = data;
+	switch (w8001->idx++) {
+	case 0:
+		if ((data & W8001_LEAD_MASK) != W8001_LEAD_BYTE) {
+			pr_debug("w8001: unsynchronized data: 0x%02x\n", data);
+			w8001->idx = 0;
+		}
+		break;
+	case 8:
+		tmp = w8001->data[0] & W8001_TAB_MASK;
+		if (unlikely(tmp == W8001_TAB_BYTE))
+			break;
+		w8001->idx = 0;
+		memset(&coord, 0, sizeof(coord));
+		parse_data(w8001->data, &coord);
+		input_report_abs(dev, ABS_X, coord.x);
+		input_report_abs(dev, ABS_Y, coord.y);
+		input_report_abs(dev, ABS_PRESSURE, coord.pen_pressure);
+		input_report_key(dev, BTN_TOUCH, coord.tsw);
+		input_sync(dev);
+		break;
+	case 10:
+		w8001->idx = 0;
+		memcpy(w8001->response, &w8001->data, W8001_PACKET_LEN);
+		w8001->expected_packet = W8001_QUERY_PACKET;
+		complete(&w8001->cmd_done);
+		break;
+	}
+}
+
+
+static irqreturn_t w8001_interrupt(struct serio *serio,
+		unsigned char data, unsigned int flags)
+{
+	struct w8001 *w8001 = serio_get_drvdata(serio);
+
+	w8001_process_data(w8001, data);
+
+	return IRQ_HANDLED;
+}
+
+static int w8001_async_command(struct w8001 *w8001, unsigned char *packet,
+					int len)
+{
+	int rc = -1;
+	int i;
+
+	mutex_lock(&w8001->cmd_mutex);
+
+	for (i = 0; i < len; i++) {
+		if (serio_write(w8001->serio, packet[i]))
+			goto out;
+	}
+	rc = 0;
+
+out:
+	mutex_unlock(&w8001->cmd_mutex);
+	return rc;
+}
+
+static int w8001_command(struct w8001 *w8001, unsigned char *packet, int len)
+{
+	int rc = -1;
+	int i;
+
+	mutex_lock(&w8001->cmd_mutex);
+
+	serio_pause_rx(w8001->serio);
+	init_completion(&w8001->cmd_done);
+	serio_continue_rx(w8001->serio);
+
+	for (i = 0; i < len; i++) {
+		if (serio_write(w8001->serio, packet[i]))
+			goto out;
+	}
+
+	wait_for_completion_timeout(&w8001->cmd_done, HZ);
+
+	if (w8001->expected_packet == W8001_QUERY_PACKET) {
+		/* We are back in reporting mode, the query was ACKed */
+		memcpy(packet, w8001->response, W8001_PACKET_LEN);
+		rc = 0;
+	}
+
+out:
+	mutex_unlock(&w8001->cmd_mutex);
+	return rc;
+}
+
+static int w8001_setup(struct w8001 *w8001)
+{
+	struct w8001_coord coord;
+	struct input_dev *dev = w8001->dev;
+	unsigned char start[1] = { '1' };
+	unsigned char query[11] = { '*' };
+
+	if (w8001_command(w8001, query, 1))
+		return -1;
+
+	memset(&coord, 0, sizeof(coord));
+	parse_data(query, &coord);
+
+	input_set_abs_params(dev, ABS_X, 0, coord.x, 0, 0);
+	input_set_abs_params(dev, ABS_Y, 0, coord.y, 0, 0);
+	input_set_abs_params(dev, ABS_PRESSURE, 0, coord.pen_pressure, 0, 0);
+	input_set_abs_params(dev, ABS_TILT_X, 0, coord.tilt_x, 0, 0);
+	input_set_abs_params(dev, ABS_TILT_Y, 0, coord.tilt_y, 0, 0);
+
+	if (w8001_async_command(w8001, start, 1))
+		return -1;
+
+	return 0;
+}
+
+/*
+ * w8001_disconnect() is the opposite of w8001_connect()
+ */
+
+static void w8001_disconnect(struct serio *serio)
+{
+	struct w8001 *w8001 = serio_get_drvdata(serio);
+
+	input_get_device(w8001->dev);
+	input_unregister_device(w8001->dev);
+	serio_close(serio);
+	serio_set_drvdata(serio, NULL);
+	input_put_device(w8001->dev);
+	kfree(w8001);
+}
+
+/*
+ * w8001_connect() is the routine that is called when someone adds a
+ * new serio device that supports the w8001 protocol and registers it as
+ * an input device.
+ */
+
+static int w8001_connect(struct serio *serio, struct serio_driver *drv)
+{
+	struct w8001 *w8001;
+	struct input_dev *input_dev;
+	int err;
+
+	w8001 = kzalloc(sizeof(struct w8001), GFP_KERNEL);
+	input_dev = input_allocate_device();
+	if (!w8001 || !input_dev) {
+		err = -ENOMEM;
+		goto fail1;
+	}
+
+	w8001->serio = serio;
+	w8001->id = serio->id.id;
+	w8001->dev = input_dev;
+	mutex_init(&w8001->cmd_mutex);
+	init_completion(&w8001->cmd_done);
+	snprintf(w8001->phys, sizeof(w8001->phys), "%s/input0", serio->phys);
+
+	input_dev->name = "Wacom W8001 Penabled Serial TouchScreen";
+	input_dev->phys = w8001->phys;
+	input_dev->id.bustype = BUS_RS232;
+	input_dev->id.vendor = SERIO_W8001;
+	input_dev->id.product = w8001->id;
+	input_dev->id.version = 0x0100;
+	input_dev->dev.parent = &serio->dev;
+
+	input_dev->evbit[0] = BIT_MASK(EV_KEY) | BIT_MASK(EV_ABS);
+	input_dev->keybit[BIT_WORD(BTN_TOUCH)] = BIT_MASK(BTN_TOUCH);
+
+	serio_set_drvdata(serio, w8001);
+	err = serio_open(serio, drv);
+	if (err)
+		goto fail2;
+
+	if (w8001_setup(w8001))
+		goto fail3;
+
+	err = input_register_device(w8001->dev);
+	if (err)
+		goto fail3;
+
+	return 0;
+
+fail3:
+	serio_close(serio);
+fail2:
+	serio_set_drvdata(serio, NULL);
+fail1:
+	input_free_device(input_dev);
+	kfree(w8001);
+	return err;
+}
+
+static struct serio_device_id w8001_serio_ids[] = {
+	{
+		.type	= SERIO_RS232,
+		.proto	= SERIO_W8001,
+		.id	= SERIO_ANY,
+		.extra	= SERIO_ANY,
+	},
+	{ 0 }
+};
+
+MODULE_DEVICE_TABLE(serio, w8001_serio_ids);
+
+static struct serio_driver w8001_drv = {
+	.driver		= {
+		.name	= "w8001",
+	},
+	.description	= DRIVER_DESC,
+	.id_table	= w8001_serio_ids,
+	.interrupt	= w8001_interrupt,
+	.connect	= w8001_connect,
+	.disconnect	= w8001_disconnect,
+};
+
+static int __init w8001_init(void)
+{
+	return serio_register_driver(&w8001_drv);
+}
+
+static void __exit w8001_exit(void)
+{
+	serio_unregister_driver(&w8001_drv);
+}
+
+module_init(w8001_init);
+module_exit(w8001_exit);
diff --git a/drivers/isdn/mISDN/dsp_pipeline.c b/drivers/isdn/mISDN/dsp_pipeline.c
index 5ee6651b45b..83639be7f7a 100644
--- a/drivers/isdn/mISDN/dsp_pipeline.c
+++ b/drivers/isdn/mISDN/dsp_pipeline.c
@@ -91,7 +91,7 @@ int mISDN_dsp_element_register(struct mISDN_dsp_element *elem)
 
 	entry->dev.class = elements_class;
 	dev_set_drvdata(&entry->dev, elem);
-	snprintf(entry->dev.bus_id, BUS_ID_SIZE, elem->name);
+	dev_set_name(&entry->dev, elem->name);
 	ret = device_register(&entry->dev);
 	if (ret) {
 		printk(KERN_ERR "%s: failed to register %s\n",
diff --git a/drivers/lguest/lguest_device.c b/drivers/lguest/lguest_device.c
index 915da6b8c92..b4d44e571d7 100644
--- a/drivers/lguest/lguest_device.c
+++ b/drivers/lguest/lguest_device.c
@@ -321,10 +321,7 @@ static struct virtio_config_ops lguest_config_ops = {
 
 /* The root device for the lguest virtio devices.  This makes them appear as
  * /sys/devices/lguest/0,1,2 not /sys/devices/0,1,2. */
-static struct device lguest_root = {
-	.parent = NULL,
-	.bus_id = "lguest",
-};
+static struct device *lguest_root;
 
 /*D:120 This is the core of the lguest bus: actually adding a new device.
  * It's a separate function because it's neater that way, and because an
@@ -351,7 +348,7 @@ static void add_lguest_device(struct lguest_device_desc *d,
 	}
 
 	/* This devices' parent is the lguest/ dir. */
-	ldev->vdev.dev.parent = &lguest_root;
+	ldev->vdev.dev.parent = lguest_root;
 	/* We have a unique device index thanks to the dev_index counter. */
 	ldev->vdev.id.device = d->type;
 	/* We have a simple set of routines for querying the device's
@@ -407,7 +404,8 @@ static int __init lguest_devices_init(void)
 	if (strcmp(pv_info.name, "lguest") != 0)
 		return 0;
 
-	if (device_register(&lguest_root) != 0)
+	lguest_root = root_device_register("lguest");
+	if (IS_ERR(lguest_root))
 		panic("Could not register lguest root");
 
 	/* Devices are in a single page above top of "normal" mem */
diff --git a/drivers/macintosh/macio_asic.c b/drivers/macintosh/macio_asic.c
index ec9e5f32f0a..6e149f4a1ff 100644
--- a/drivers/macintosh/macio_asic.c
+++ b/drivers/macintosh/macio_asic.c
@@ -33,7 +33,7 @@
 
 #undef DEBUG
 
-#define MAX_NODE_NAME_SIZE (BUS_ID_SIZE - 12)
+#define MAX_NODE_NAME_SIZE (20 - 12)
 
 static struct macio_chip      *macio_on_hold;
 
@@ -240,7 +240,7 @@ static void macio_create_fixup_irq(struct macio_dev *dev, int index,
 	if (irq != NO_IRQ) {
 		dev->interrupt[index].start = irq;
 		dev->interrupt[index].flags = IORESOURCE_IRQ;
-		dev->interrupt[index].name = dev->ofdev.dev.bus_id;
+		dev->interrupt[index].name = dev_name(&dev->ofdev.dev);
 	}
 	if (dev->n_interrupts <= index)
 		dev->n_interrupts = index + 1;
@@ -303,7 +303,7 @@ static void macio_setup_interrupts(struct macio_dev *dev)
 			break;
 		res->start = irq;
 		res->flags = IORESOURCE_IRQ;
-		res->name = dev->ofdev.dev.bus_id;
+		res->name = dev_name(&dev->ofdev.dev);
 		if (macio_resource_quirks(np, res, i - 1)) {
 			memset(res, 0, sizeof(struct resource));
 			continue;
@@ -325,7 +325,7 @@ static void macio_setup_resources(struct macio_dev *dev,
 		if (index >= MACIO_DEV_COUNT_RESOURCES)
 			break;
 		*res = r;
-		res->name = dev->ofdev.dev.bus_id;
+		res->name = dev_name(&dev->ofdev.dev);
 
 		if (macio_resource_quirks(np, res, index)) {
 			memset(res, 0, sizeof(struct resource));
@@ -338,7 +338,7 @@ static void macio_setup_resources(struct macio_dev *dev,
 		if (insert_resource(parent_res, res)) {
 			printk(KERN_WARNING "Can't request resource "
 			       "%d for MacIO device %s\n",
-			       index, dev->ofdev.dev.bus_id);
+			       index, dev_name(&dev->ofdev.dev));
 		}
 	}
 	dev->n_resources = index;
@@ -385,8 +385,8 @@ static struct macio_dev * macio_add_one_device(struct macio_chip *chip,
 
 	/* MacIO itself has a different reg, we use it's PCI base */
 	if (np == chip->of_node) {
-		sprintf(dev->ofdev.dev.bus_id, "%1d.%08x:%.*s",
-			chip->lbus.index,
+		dev_set_name(&dev->ofdev.dev, "%1d.%08x:%.*s",
+			     chip->lbus.index,
 #ifdef CONFIG_PCI
 			(unsigned int)pci_resource_start(chip->lbus.pdev, 0),
 #else
@@ -395,9 +395,9 @@ static struct macio_dev * macio_add_one_device(struct macio_chip *chip,
 			MAX_NODE_NAME_SIZE, np->name);
 	} else {
 		reg = of_get_property(np, "reg", NULL);
-		sprintf(dev->ofdev.dev.bus_id, "%1d.%08x:%.*s",
-			chip->lbus.index,
-			reg ? *reg : 0, MAX_NODE_NAME_SIZE, np->name);
+		dev_set_name(&dev->ofdev.dev, "%1d.%08x:%.*s",
+			     chip->lbus.index,
+			     reg ? *reg : 0, MAX_NODE_NAME_SIZE, np->name);
 	}
 
 	/* Setup interrupts & resources */
@@ -408,7 +408,7 @@ static struct macio_dev * macio_add_one_device(struct macio_chip *chip,
 	/* Register with core */
 	if (of_device_register(&dev->ofdev) != 0) {
 		printk(KERN_DEBUG"macio: device registration error for %s!\n",
-		       dev->ofdev.dev.bus_id);
+		       dev_name(&dev->ofdev.dev));
 		kfree(dev);
 		return NULL;
 	}
@@ -558,7 +558,7 @@ err_out:
 		resource_no,
 		macio_resource_len(dev, resource_no),
 		macio_resource_start(dev, resource_no),
-		dev->ofdev.dev.bus_id);
+		dev_name(&dev->ofdev.dev));
 	return -EBUSY;
 }
 
diff --git a/drivers/macintosh/therm_adt746x.c b/drivers/macintosh/therm_adt746x.c
index 22bf981d393..82607add69a 100644
--- a/drivers/macintosh/therm_adt746x.c
+++ b/drivers/macintosh/therm_adt746x.c
@@ -554,7 +554,7 @@ thermostat_init(void)
 	const u32 *prop;
 	int i = 0, offset = 0;
 	int err;
-	
+
 	np = of_find_node_by_name(NULL, "fan");
 	if (!np)
 		return -ENODEV;
@@ -613,13 +613,13 @@ thermostat_init(void)
 	}
 
 	of_dev = of_platform_device_create(np, "temperatures", NULL);
-	
+	of_node_put(np);
+
 	if (of_dev == NULL) {
 		printk(KERN_ERR "Can't register temperatures device !\n");
-		of_node_put(np);
 		return -ENODEV;
 	}
-	
+
 	err = device_create_file(&of_dev->dev, &dev_attr_sensor1_temperature);
 	err |= device_create_file(&of_dev->dev, &dev_attr_sensor2_temperature);
 	err |= device_create_file(&of_dev->dev, &dev_attr_sensor1_limit);
diff --git a/drivers/media/dvb/dvb-core/dvbdev.c b/drivers/media/dvb/dvb-core/dvbdev.c
index 65d69665f1f..6a32680dbb1 100644
--- a/drivers/media/dvb/dvb-core/dvbdev.c
+++ b/drivers/media/dvb/dvb-core/dvbdev.c
@@ -79,6 +79,10 @@ static int dvb_device_open(struct inode *inode, struct file *file)
 		file->private_data = dvbdev;
 		old_fops = file->f_op;
 		file->f_op = fops_get(dvbdev->fops);
+		if (file->f_op == NULL) {
+			file->f_op = old_fops;
+			goto fail;
+		}
 		if(file->f_op->open)
 			err = file->f_op->open(inode,file);
 		if (err) {
@@ -90,6 +94,7 @@ static int dvb_device_open(struct inode *inode, struct file *file)
 		unlock_kernel();
 		return err;
 	}
+fail:
 	up_read(&minor_rwsem);
 	unlock_kernel();
 	return -ENODEV;
diff --git a/drivers/media/video/v4l1-compat.c b/drivers/media/video/v4l1-compat.c
index d450cab20be..b617bf05e2d 100644
--- a/drivers/media/video/v4l1-compat.c
+++ b/drivers/media/video/v4l1-compat.c
@@ -203,7 +203,6 @@ static int poll_one(struct file *file, struct poll_wqueues *pwq)
 	table = &pwq->pt;
 	for (;;) {
 		int mask;
-		set_current_state(TASK_INTERRUPTIBLE);
 		mask = file->f_op->poll(file, table);
 		if (mask & POLLIN)
 			break;
@@ -212,9 +211,8 @@ static int poll_one(struct file *file, struct poll_wqueues *pwq)
 			retval = -ERESTARTSYS;
 			break;
 		}
-		schedule();
+		poll_schedule(pwq, TASK_INTERRUPTIBLE);
 	}
-	set_current_state(TASK_RUNNING);
 	poll_freewait(pwq);
 	return retval;
 }
diff --git a/drivers/memstick/core/memstick.c b/drivers/memstick/core/memstick.c
index cea46906408..a5b448ea4ea 100644
--- a/drivers/memstick/core/memstick.c
+++ b/drivers/memstick/core/memstick.c
@@ -385,8 +385,7 @@ static struct memstick_dev *memstick_alloc_card(struct memstick_host *host)
 
 	if (card) {
 		card->host = host;
-		snprintf(card->dev.bus_id, sizeof(card->dev.bus_id),
-			 "%s", host->dev.bus_id);
+		dev_set_name(&card->dev, "%s", dev_name(&host->dev));
 		card->dev.parent = &host->dev;
 		card->dev.bus = &memstick_bus_type;
 		card->dev.release = memstick_free_card;
@@ -519,7 +518,7 @@ int memstick_add_host(struct memstick_host *host)
 	if (rc)
 		return rc;
 
-	snprintf(host->dev.bus_id, BUS_ID_SIZE, "memstick%u", host->id);
+	dev_set_name(&host->dev, "memstick%u", host->id);
 
 	rc = device_add(&host->dev);
 	if (rc) {
diff --git a/drivers/memstick/core/mspro_block.c b/drivers/memstick/core/mspro_block.c
index 7911151e56a..1f1e3982b1a 100644
--- a/drivers/memstick/core/mspro_block.c
+++ b/drivers/memstick/core/mspro_block.c
@@ -887,14 +887,14 @@ try_again:
 	if (rc) {
 		printk(KERN_WARNING
 		       "%s: could not switch to 4-bit mode, error %d\n",
-		       card->dev.bus_id, rc);
+		       dev_name(&card->dev), rc);
 		return 0;
 	}
 
 	msb->system = MEMSTICK_SYS_PAR4;
 	host->set_param(host, MEMSTICK_INTERFACE, MEMSTICK_PAR4);
 	printk(KERN_INFO "%s: switching to 4-bit parallel mode\n",
-	       card->dev.bus_id);
+	       dev_name(&card->dev));
 
 	if (msb->caps & MEMSTICK_CAP_PAR8) {
 		rc = mspro_block_set_interface(card, MEMSTICK_SYS_PAR8);
@@ -905,11 +905,11 @@ try_again:
 					MEMSTICK_PAR8);
 			printk(KERN_INFO
 			       "%s: switching to 8-bit parallel mode\n",
-			       card->dev.bus_id);
+			       dev_name(&card->dev));
 		} else
 			printk(KERN_WARNING
 			       "%s: could not switch to 8-bit mode, error %d\n",
-			       card->dev.bus_id, rc);
+			       dev_name(&card->dev), rc);
 	}
 
 	card->next_request = h_mspro_block_req_init;
@@ -922,7 +922,7 @@ try_again:
 	if (rc) {
 		printk(KERN_WARNING
 		       "%s: interface error, trying to fall back to serial\n",
-		       card->dev.bus_id);
+		       dev_name(&card->dev));
 		msb->system = MEMSTICK_SYS_SERIAL;
 		host->set_param(host, MEMSTICK_POWER, MEMSTICK_POWER_OFF);
 		msleep(10);
@@ -992,14 +992,14 @@ static int mspro_block_read_attributes(struct memstick_dev *card)
 
 	if (be16_to_cpu(attr->signature) != MSPRO_BLOCK_SIGNATURE) {
 		printk(KERN_ERR "%s: unrecognized device signature %x\n",
-		       card->dev.bus_id, be16_to_cpu(attr->signature));
+		       dev_name(&card->dev), be16_to_cpu(attr->signature));
 		rc = -ENODEV;
 		goto out_free_attr;
 	}
 
 	if (attr->count > MSPRO_BLOCK_MAX_ATTRIBUTES) {
 		printk(KERN_WARNING "%s: way too many attribute entries\n",
-		       card->dev.bus_id);
+		       dev_name(&card->dev));
 		attr_count = MSPRO_BLOCK_MAX_ATTRIBUTES;
 	} else
 		attr_count = attr->count;
diff --git a/drivers/memstick/host/tifm_ms.c b/drivers/memstick/host/tifm_ms.c
index d32d6ad8f3f..03f71a431c8 100644
--- a/drivers/memstick/host/tifm_ms.c
+++ b/drivers/memstick/host/tifm_ms.c
@@ -546,7 +546,7 @@ static void tifm_ms_abort(unsigned long data)
 	printk(KERN_ERR
 	       "%s : card failed to respond for a long period of time "
 	       "(%x, %x)\n",
-	       host->dev->dev.bus_id, host->req ? host->req->tpc : 0,
+	       dev_name(&host->dev->dev), host->req ? host->req->tpc : 0,
 	       host->cmd_flags);
 
 	tifm_eject(host->dev);
@@ -561,7 +561,7 @@ static int tifm_ms_probe(struct tifm_dev *sock)
 	if (!(TIFM_SOCK_STATE_OCCUPIED
 	      & readl(sock->addr + SOCK_PRESENT_STATE))) {
 		printk(KERN_WARNING "%s : card gone, unexpectedly\n",
-		       sock->dev.bus_id);
+		       dev_name(&sock->dev));
 		return rc;
 	}
 
diff --git a/drivers/message/i2o/device.c b/drivers/message/i2o/device.c
index 54c2e9ae23e..0ee4264f5db 100644
--- a/drivers/message/i2o/device.c
+++ b/drivers/message/i2o/device.c
@@ -52,7 +52,6 @@ static inline int i2o_device_issue_claim(struct i2o_device *dev, u32 cmd,
 /**
  *	i2o_device_claim - claim a device for use by an OSM
  *	@dev: I2O device to claim
- *	@drv: I2O driver which wants to claim the device
  *
  *	Do the leg work to assign a device to a given OSM. If the claim succeeds,
  *	the owner is the primary. If the attempt fails a negative errno code
@@ -80,7 +79,6 @@ int i2o_device_claim(struct i2o_device *dev)
 /**
  *	i2o_device_claim_release - release a device that the OSM is using
  *	@dev: device to release
- *	@drv: driver which claimed the device
  *
  *	Drop a claim by an OSM on a given I2O device.
  *
@@ -134,7 +132,7 @@ static void i2o_device_release(struct device *dev)
 {
 	struct i2o_device *i2o_dev = to_i2o_device(dev);
 
-	pr_debug("i2o: device %s released\n", dev->bus_id);
+	pr_debug("i2o: device %s released\n", dev_name(dev));
 
 	kfree(i2o_dev);
 }
@@ -229,8 +227,8 @@ static int i2o_device_add(struct i2o_controller *c, i2o_lct_entry *entry)
 
 	i2o_dev->lct_data = *entry;
 
-	snprintf(i2o_dev->device.bus_id, BUS_ID_SIZE, "%d:%03x", c->unit,
-		 i2o_dev->lct_data.tid);
+	dev_set_name(&i2o_dev->device, "%d:%03x", c->unit,
+		     i2o_dev->lct_data.tid);
 
 	i2o_dev->iop = c;
 	i2o_dev->device.parent = &c->device;
@@ -281,7 +279,7 @@ static int i2o_device_add(struct i2o_controller *c, i2o_lct_entry *entry)
 
 	i2o_driver_notify_device_add_all(i2o_dev);
 
-	pr_debug("i2o: device %s added\n", i2o_dev->device.bus_id);
+	pr_debug("i2o: device %s added\n", dev_name(&i2o_dev->device));
 
 	return 0;
 
diff --git a/drivers/message/i2o/driver.c b/drivers/message/i2o/driver.c
index e0d474b1743..a0421efe04c 100644
--- a/drivers/message/i2o/driver.c
+++ b/drivers/message/i2o/driver.c
@@ -173,7 +173,6 @@ void i2o_driver_unregister(struct i2o_driver *drv)
  *	i2o_driver_dispatch - dispatch an I2O reply message
  *	@c: I2O controller of the message
  *	@m: I2O message number
- *	@msg: I2O message to be delivered
  *
  *	The reply is delivered to the driver from which the original message
  *	was. This function is only called from interrupt context.
diff --git a/drivers/message/i2o/i2o_proc.c b/drivers/message/i2o/i2o_proc.c
index 54a3016ff45..9a36b5a7de5 100644
--- a/drivers/message/i2o/i2o_proc.c
+++ b/drivers/message/i2o/i2o_proc.c
@@ -1300,7 +1300,7 @@ static int i2o_seq_show_dev_name(struct seq_file *seq, void *v)
 {
 	struct i2o_device *d = (struct i2o_device *)seq->private;
 
-	seq_printf(seq, "%s\n", d->device.bus_id);
+	seq_printf(seq, "%s\n", dev_name(&d->device));
 
 	return 0;
 }
diff --git a/drivers/message/i2o/iop.c b/drivers/message/i2o/iop.c
index 35c67d1f255..27cf4af0e13 100644
--- a/drivers/message/i2o/iop.c
+++ b/drivers/message/i2o/iop.c
@@ -1072,7 +1072,7 @@ struct i2o_controller *i2o_iop_alloc(void)
 
 	c->device.release = &i2o_iop_release;
 
-	snprintf(c->device.bus_id, BUS_ID_SIZE, "iop%d", c->unit);
+	dev_set_name(&c->device, "iop%d", c->unit);
 
 #if BITS_PER_LONG == 64
 	spin_lock_init(&c->context_list_lock);
diff --git a/drivers/misc/ibmasm/module.c b/drivers/misc/ibmasm/module.c
index b5f6add34b0..dc14b0b9cbf 100644
--- a/drivers/misc/ibmasm/module.c
+++ b/drivers/misc/ibmasm/module.c
@@ -104,8 +104,7 @@ static int __devinit ibmasm_init_one(struct pci_dev *pdev, const struct pci_devi
 	}
 
 	sp->irq = pdev->irq;
-	sp->base_address = ioremap(pci_resource_start(pdev, 0),
-					pci_resource_len(pdev, 0));
+	sp->base_address = pci_ioremap_bar(pdev, 0);
 	if (!sp->base_address) {
 		dev_err(sp->dev, "Failed to ioremap pci memory\n");
 		result =  -ENODEV;
diff --git a/drivers/misc/ioc4.c b/drivers/misc/ioc4.c
index 6f76573e7c8..60b0b1a4fb3 100644
--- a/drivers/misc/ioc4.c
+++ b/drivers/misc/ioc4.c
@@ -269,6 +269,16 @@ ioc4_variant(struct ioc4_driver_data *idd)
 	return IOC4_VARIANT_PCI_RT;
 }
 
+static void
+ioc4_load_modules(struct work_struct *work)
+{
+	/* arg just has to be freed */
+
+	request_module("sgiioc4");
+
+	kfree(work);
+}
+
 /* Adds a new instance of an IOC4 card */
 static int
 ioc4_probe(struct pci_dev *pdev, const struct pci_device_id *pci_id)
@@ -378,6 +388,30 @@ ioc4_probe(struct pci_dev *pdev, const struct pci_device_id *pci_id)
 	}
 	mutex_unlock(&ioc4_mutex);
 
+	/* Request sgiioc4 IDE driver on boards that bring that functionality
+	 * off of IOC4.  The root filesystem may be hosted on a drive connected
+	 * to IOC4, so we need to make sure the sgiioc4 driver is loaded as it
+	 * won't be picked up by modprobes due to the ioc4 module owning the
+	 * PCI device.
+	 */
+	if (idd->idd_variant != IOC4_VARIANT_PCI_RT) {
+		struct work_struct *work;
+		work = kzalloc(sizeof(struct work_struct), GFP_KERNEL);
+		if (!work) {
+			printk(KERN_WARNING
+			       "%s: IOC4 unable to allocate memory for "
+			       "load of sub-modules.\n", __func__);
+		} else {
+			/* Request the module from a work procedure as the
+			 * modprobe goes out to a userland helper and that
+			 * will hang if done directly from ioc4_probe().
+			 */
+			printk(KERN_INFO "IOC4 loading sgiioc4 submodule\n");
+			INIT_WORK(work, ioc4_load_modules);
+			schedule_work(work);
+		}
+	}
+
 	return 0;
 
 out_misc_region:
@@ -462,6 +496,8 @@ ioc4_init(void)
 static void __devexit
 ioc4_exit(void)
 {
+	/* Ensure ioc4_load_modules() has completed before exiting */
+	flush_scheduled_work();
 	pci_unregister_driver(&ioc4_driver);
 }
 
diff --git a/drivers/misc/sgi-gru/grumain.c b/drivers/misc/sgi-gru/grumain.c
index e11e1ac5090..3d2fc216bae 100644
--- a/drivers/misc/sgi-gru/grumain.c
+++ b/drivers/misc/sgi-gru/grumain.c
@@ -29,7 +29,7 @@ static struct device_driver gru_driver = {
 };
 
 static struct device gru_device = {
-	.bus_id = {0},
+	.init_name = "",
 	.driver = &gru_driver,
 };
 
diff --git a/drivers/misc/sgi-xp/xp_main.c b/drivers/misc/sgi-xp/xp_main.c
index 9a2e77172d9..16f8dcab2da 100644
--- a/drivers/misc/sgi-xp/xp_main.c
+++ b/drivers/misc/sgi-xp/xp_main.c
@@ -25,7 +25,7 @@ struct device_driver xp_dbg_name = {
 };
 
 struct device xp_dbg_subname = {
-	.bus_id = {0},		/* set to "" */
+	.init_name = "",		/* set to "" */
 	.driver = &xp_dbg_name
 };
 
diff --git a/drivers/misc/sgi-xp/xpc_main.c b/drivers/misc/sgi-xp/xpc_main.c
index e8d5cfbd32c..89218f7cfaa 100644
--- a/drivers/misc/sgi-xp/xpc_main.c
+++ b/drivers/misc/sgi-xp/xpc_main.c
@@ -59,12 +59,12 @@ struct device_driver xpc_dbg_name = {
 };
 
 struct device xpc_part_dbg_subname = {
-	.bus_id = {0},		/* set to "part" at xpc_init() time */
+	.init_name = "",	/* set to "part" at xpc_init() time */
 	.driver = &xpc_dbg_name
 };
 
 struct device xpc_chan_dbg_subname = {
-	.bus_id = {0},		/* set to "chan" at xpc_init() time */
+	.init_name = "",	/* set to "chan" at xpc_init() time */
 	.driver = &xpc_dbg_name
 };
 
@@ -1258,8 +1258,8 @@ xpc_init(void)
 	int ret;
 	struct task_struct *kthread;
 
-	snprintf(xpc_part->bus_id, BUS_ID_SIZE, "part");
-	snprintf(xpc_chan->bus_id, BUS_ID_SIZE, "chan");
+	dev_set_name(xpc_part, "part");
+	dev_set_name(xpc_chan, "chan");
 
 	if (is_shub()) {
 		/*
diff --git a/drivers/misc/sgi-xp/xpnet.c b/drivers/misc/sgi-xp/xpnet.c
index 8e6aa9508f4..81152b3e360 100644
--- a/drivers/misc/sgi-xp/xpnet.c
+++ b/drivers/misc/sgi-xp/xpnet.c
@@ -138,7 +138,7 @@ struct device_driver xpnet_dbg_name = {
 };
 
 struct device xpnet_dbg_subname = {
-	.bus_id = {0},		/* set to "" */
+	.init_name = "",	/* set to "" */
 	.driver = &xpnet_dbg_name
 };
 
diff --git a/drivers/misc/tifm_7xx1.c b/drivers/misc/tifm_7xx1.c
index 67503ea71d2..be5672a9870 100644
--- a/drivers/misc/tifm_7xx1.c
+++ b/drivers/misc/tifm_7xx1.c
@@ -164,7 +164,7 @@ static void tifm_7xx1_switch_media(struct work_struct *work)
 		if (sock) {
 			printk(KERN_INFO
 			       "%s : demand removing card from socket %u:%u\n",
-			       fm->dev.bus_id, fm->id, cnt);
+			       dev_name(&fm->dev), fm->id, cnt);
 			fm->sockets[cnt] = NULL;
 			sock_addr = sock->addr;
 			spin_unlock_irqrestore(&fm->lock, flags);
@@ -354,8 +354,7 @@ static int tifm_7xx1_probe(struct pci_dev *dev,
 	fm->has_ms_pif = tifm_7xx1_has_ms_pif;
 	pci_set_drvdata(dev, fm);
 
-	fm->addr = ioremap(pci_resource_start(dev, 0),
-			   pci_resource_len(dev, 0));
+	fm->addr = pci_ioremap_bar(dev, 0);
 	if (!fm->addr)
 		goto err_out_free;
 
diff --git a/drivers/misc/tifm_core.c b/drivers/misc/tifm_core.c
index 82dc72a1484..98bcba521da 100644
--- a/drivers/misc/tifm_core.c
+++ b/drivers/misc/tifm_core.c
@@ -203,7 +203,7 @@ int tifm_add_adapter(struct tifm_adapter *fm)
 	if (rc)
 		return rc;
 
-	snprintf(fm->dev.bus_id, BUS_ID_SIZE, "tifm%u", fm->id);
+	dev_set_name(&fm->dev, "tifm%u", fm->id);
 	rc = device_add(&fm->dev);
 	if (rc) {
 		spin_lock(&tifm_adapter_lock);
@@ -266,9 +266,8 @@ struct tifm_dev *tifm_alloc_device(struct tifm_adapter *fm, unsigned int id,
 		sock->dev.dma_mask = fm->dev.parent->dma_mask;
 		sock->dev.release = tifm_free_device;
 
-		snprintf(sock->dev.bus_id, BUS_ID_SIZE,
-			 "tifm_%s%u:%u", tifm_media_type_name(type, 2),
-			 fm->id, id);
+		dev_set_name(&sock->dev, "tifm_%s%u:%u",
+			     tifm_media_type_name(type, 2), fm->id, id);
 		printk(KERN_INFO DRIVER_NAME
 		       ": %s card detected in socket %u:%u\n",
 		       tifm_media_type_name(type, 0), fm->id, id);
diff --git a/drivers/mtd/devices/m25p80.c b/drivers/mtd/devices/m25p80.c
index 6659b2275c0..5733f064384 100644
--- a/drivers/mtd/devices/m25p80.c
+++ b/drivers/mtd/devices/m25p80.c
@@ -170,7 +170,7 @@ static int wait_till_ready(struct m25p *flash)
 static int erase_chip(struct m25p *flash)
 {
 	DEBUG(MTD_DEBUG_LEVEL3, "%s: %s %dKiB\n",
-			flash->spi->dev.bus_id, __func__,
+			dev_name(&flash->spi->dev), __func__,
 			flash->mtd.size / 1024);
 
 	/* Wait until finished previous write command. */
@@ -197,7 +197,7 @@ static int erase_chip(struct m25p *flash)
 static int erase_sector(struct m25p *flash, u32 offset)
 {
 	DEBUG(MTD_DEBUG_LEVEL3, "%s: %s %dKiB at 0x%08x\n",
-			flash->spi->dev.bus_id, __func__,
+			dev_name(&flash->spi->dev), __func__,
 			flash->mtd.erasesize / 1024, offset);
 
 	/* Wait until finished previous write command. */
@@ -234,7 +234,7 @@ static int m25p80_erase(struct mtd_info *mtd, struct erase_info *instr)
 	u32 addr,len;
 
 	DEBUG(MTD_DEBUG_LEVEL2, "%s: %s %s 0x%08x, len %d\n",
-			flash->spi->dev.bus_id, __func__, "at",
+			dev_name(&flash->spi->dev), __func__, "at",
 			(u32)instr->addr, instr->len);
 
 	/* sanity checks */
@@ -295,7 +295,7 @@ static int m25p80_read(struct mtd_info *mtd, loff_t from, size_t len,
 	struct spi_message m;
 
 	DEBUG(MTD_DEBUG_LEVEL2, "%s: %s %s 0x%08x, len %zd\n",
-			flash->spi->dev.bus_id, __func__, "from",
+			dev_name(&flash->spi->dev), __func__, "from",
 			(u32)from, len);
 
 	/* sanity checks */
@@ -367,7 +367,7 @@ static int m25p80_write(struct mtd_info *mtd, loff_t to, size_t len,
 	struct spi_message m;
 
 	DEBUG(MTD_DEBUG_LEVEL2, "%s: %s %s 0x%08x, len %zd\n",
-			flash->spi->dev.bus_id, __func__, "to",
+			dev_name(&flash->spi->dev), __func__, "to",
 			(u32)to, len);
 
 	if (retlen)
@@ -563,7 +563,7 @@ static struct flash_info *__devinit jedec_probe(struct spi_device *spi)
 	tmp = spi_write_then_read(spi, &code, 1, id, 5);
 	if (tmp < 0) {
 		DEBUG(MTD_DEBUG_LEVEL0, "%s: error %d reading JEDEC ID\n",
-			spi->dev.bus_id, tmp);
+			dev_name(&spi->dev), tmp);
 		return NULL;
 	}
 	jedec = id[0];
@@ -617,7 +617,7 @@ static int __devinit m25p_probe(struct spi_device *spi)
 		/* unrecognized chip? */
 		if (i == ARRAY_SIZE(m25p_data)) {
 			DEBUG(MTD_DEBUG_LEVEL0, "%s: unrecognized id %s\n",
-					spi->dev.bus_id, data->type);
+					dev_name(&spi->dev), data->type);
 			info = NULL;
 
 		/* recognized; is that chip really what's there? */
@@ -658,7 +658,7 @@ static int __devinit m25p_probe(struct spi_device *spi)
 	if (data && data->name)
 		flash->mtd.name = data->name;
 	else
-		flash->mtd.name = spi->dev.bus_id;
+		flash->mtd.name = dev_name(&spi->dev);
 
 	flash->mtd.type = MTD_NORFLASH;
 	flash->mtd.writesize = 1;
diff --git a/drivers/mtd/devices/mtd_dataflash.c b/drivers/mtd/devices/mtd_dataflash.c
index 6dd9aff8bb2..65126cd668f 100644
--- a/drivers/mtd/devices/mtd_dataflash.c
+++ b/drivers/mtd/devices/mtd_dataflash.c
@@ -128,7 +128,7 @@ static int dataflash_waitready(struct spi_device *spi)
 		status = dataflash_status(spi);
 		if (status < 0) {
 			DEBUG(MTD_DEBUG_LEVEL1, "%s: status %d?\n",
-					spi->dev.bus_id, status);
+					dev_name(&spi->dev), status);
 			status = 0;
 		}
 
@@ -154,7 +154,7 @@ static int dataflash_erase(struct mtd_info *mtd, struct erase_info *instr)
 	uint8_t			*command;
 
 	DEBUG(MTD_DEBUG_LEVEL2, "%s: erase addr=0x%x len 0x%x\n",
-			spi->dev.bus_id,
+			dev_name(&spi->dev),
 			instr->addr, instr->len);
 
 	/* Sanity checks */
@@ -197,7 +197,7 @@ static int dataflash_erase(struct mtd_info *mtd, struct erase_info *instr)
 
 		if (status < 0) {
 			printk(KERN_ERR "%s: erase %x, err %d\n",
-				spi->dev.bus_id, pageaddr, status);
+				dev_name(&spi->dev), pageaddr, status);
 			/* REVISIT:  can retry instr->retries times; or
 			 * giveup and instr->fail_addr = instr->addr;
 			 */
@@ -239,7 +239,7 @@ static int dataflash_read(struct mtd_info *mtd, loff_t from, size_t len,
 	int			status;
 
 	DEBUG(MTD_DEBUG_LEVEL2, "%s: read 0x%x..0x%x\n",
-		priv->spi->dev.bus_id, (unsigned)from, (unsigned)(from + len));
+		dev_name(&priv->spi->dev), (unsigned)from, (unsigned)(from + len));
 
 	*retlen = 0;
 
@@ -288,7 +288,7 @@ static int dataflash_read(struct mtd_info *mtd, loff_t from, size_t len,
 		status = 0;
 	} else
 		DEBUG(MTD_DEBUG_LEVEL1, "%s: read %x..%x --> %d\n",
-			priv->spi->dev.bus_id,
+			dev_name(&priv->spi->dev),
 			(unsigned)from, (unsigned)(from + len),
 			status);
 	return status;
@@ -315,7 +315,7 @@ static int dataflash_write(struct mtd_info *mtd, loff_t to, size_t len,
 	uint8_t			*command;
 
 	DEBUG(MTD_DEBUG_LEVEL2, "%s: write 0x%x..0x%x\n",
-		spi->dev.bus_id, (unsigned)to, (unsigned)(to + len));
+		dev_name(&spi->dev), (unsigned)to, (unsigned)(to + len));
 
 	*retlen = 0;
 
@@ -374,7 +374,7 @@ static int dataflash_write(struct mtd_info *mtd, loff_t to, size_t len,
 			status = spi_sync(spi, &msg);
 			if (status < 0)
 				DEBUG(MTD_DEBUG_LEVEL1, "%s: xfer %u -> %d \n",
-					spi->dev.bus_id, addr, status);
+					dev_name(&spi->dev), addr, status);
 
 			(void) dataflash_waitready(priv->spi);
 		}
@@ -396,7 +396,7 @@ static int dataflash_write(struct mtd_info *mtd, loff_t to, size_t len,
 		spi_transfer_del(x + 1);
 		if (status < 0)
 			DEBUG(MTD_DEBUG_LEVEL1, "%s: pgm %u/%u -> %d \n",
-				spi->dev.bus_id, addr, writelen, status);
+				dev_name(&spi->dev), addr, writelen, status);
 
 		(void) dataflash_waitready(priv->spi);
 
@@ -416,14 +416,14 @@ static int dataflash_write(struct mtd_info *mtd, loff_t to, size_t len,
 		status = spi_sync(spi, &msg);
 		if (status < 0)
 			DEBUG(MTD_DEBUG_LEVEL1, "%s: compare %u -> %d \n",
-				spi->dev.bus_id, addr, status);
+				dev_name(&spi->dev), addr, status);
 
 		status = dataflash_waitready(priv->spi);
 
 		/* Check result of the compare operation */
 		if (status & (1 << 6)) {
 			printk(KERN_ERR "%s: compare page %u, err %d\n",
-				spi->dev.bus_id, pageaddr, status);
+				dev_name(&spi->dev), pageaddr, status);
 			remaining = 0;
 			status = -EIO;
 			break;
@@ -779,7 +779,7 @@ static struct flash_info *__devinit jedec_probe(struct spi_device *spi)
 	tmp = spi_write_then_read(spi, &code, 1, id, 3);
 	if (tmp < 0) {
 		DEBUG(MTD_DEBUG_LEVEL0, "%s: error %d reading JEDEC ID\n",
-			spi->dev.bus_id, tmp);
+			dev_name(&spi->dev), tmp);
 		return ERR_PTR(tmp);
 	}
 	if (id[0] != 0x1f)
@@ -869,7 +869,7 @@ static int __devinit dataflash_probe(struct spi_device *spi)
 	status = dataflash_status(spi);
 	if (status <= 0 || status == 0xff) {
 		DEBUG(MTD_DEBUG_LEVEL1, "%s: status error %d\n",
-				spi->dev.bus_id, status);
+				dev_name(&spi->dev), status);
 		if (status == 0 || status == 0xff)
 			status = -ENODEV;
 		return status;
@@ -905,13 +905,13 @@ static int __devinit dataflash_probe(struct spi_device *spi)
 	/* obsolete AT45DB1282 not (yet?) supported */
 	default:
 		DEBUG(MTD_DEBUG_LEVEL1, "%s: unsupported device (%x)\n",
-				spi->dev.bus_id, status & 0x3c);
+				dev_name(&spi->dev), status & 0x3c);
 		status = -ENODEV;
 	}
 
 	if (status < 0)
 		DEBUG(MTD_DEBUG_LEVEL1, "%s: add_dataflash --> %d\n",
-				spi->dev.bus_id, status);
+				dev_name(&spi->dev), status);
 
 	return status;
 }
@@ -921,7 +921,7 @@ static int __devexit dataflash_remove(struct spi_device *spi)
 	struct dataflash	*flash = dev_get_drvdata(&spi->dev);
 	int			status;
 
-	DEBUG(MTD_DEBUG_LEVEL1, "%s: remove\n", spi->dev.bus_id);
+	DEBUG(MTD_DEBUG_LEVEL1, "%s: remove\n", dev_name(&spi->dev));
 
 	if (mtd_has_partitions() && flash->partitioned)
 		status = del_mtd_partitions(&flash->mtd);
diff --git a/drivers/mtd/maps/integrator-flash.c b/drivers/mtd/maps/integrator-flash.c
index 7100ee3c7b0..d2ec262666c 100644
--- a/drivers/mtd/maps/integrator-flash.c
+++ b/drivers/mtd/maps/integrator-flash.c
@@ -105,7 +105,7 @@ static int armflash_probe(struct platform_device *dev)
 	info->map.bankwidth	= plat->width;
 	info->map.phys		= res->start;
 	info->map.virt		= base;
-	info->map.name		= dev->dev.bus_id;
+	info->map.name		= dev_name(&dev->dev);
 	info->map.set_vpp	= armflash_set_vpp;
 
 	simple_map_init(&info->map);
diff --git a/drivers/mtd/maps/ixp2000.c b/drivers/mtd/maps/ixp2000.c
index 3ea1de9be72..d4fb9a3ab4d 100644
--- a/drivers/mtd/maps/ixp2000.c
+++ b/drivers/mtd/maps/ixp2000.c
@@ -188,7 +188,7 @@ static int ixp2000_flash_probe(struct platform_device *dev)
  	 */
 	info->map.map_priv_2 = (unsigned long) ixp_data->bank_setup;
 
-	info->map.name = dev->dev.bus_id;
+	info->map.name = dev_name(&dev->dev);
 	info->map.read = ixp2000_flash_read8;
 	info->map.write = ixp2000_flash_write8;
 	info->map.copy_from = ixp2000_flash_copy_from;
@@ -196,7 +196,7 @@ static int ixp2000_flash_probe(struct platform_device *dev)
 
 	info->res = request_mem_region(dev->resource->start,
 			dev->resource->end - dev->resource->start + 1,
-			dev->dev.bus_id);
+			dev_name(&dev->dev));
 	if (!info->res) {
 		dev_err(&dev->dev, "Could not reserve memory region\n");
 		err = -ENOMEM;
diff --git a/drivers/mtd/maps/ixp4xx.c b/drivers/mtd/maps/ixp4xx.c
index 16555cbeaea..7214b876feb 100644
--- a/drivers/mtd/maps/ixp4xx.c
+++ b/drivers/mtd/maps/ixp4xx.c
@@ -218,7 +218,7 @@ static int ixp4xx_flash_probe(struct platform_device *dev)
 	 * handle that.
 	 */
 	info->map.bankwidth = 2;
-	info->map.name = dev->dev.bus_id;
+	info->map.name = dev_name(&dev->dev);
 	info->map.read = ixp4xx_read16,
 	info->map.write = ixp4xx_probe_write16,
 	info->map.copy_from = ixp4xx_copy_from,
diff --git a/drivers/mtd/maps/omap_nor.c b/drivers/mtd/maps/omap_nor.c
index 05f276af15d..7e50e9b1b78 100644
--- a/drivers/mtd/maps/omap_nor.c
+++ b/drivers/mtd/maps/omap_nor.c
@@ -101,7 +101,7 @@ static int __init omapflash_probe(struct platform_device *pdev)
 		err = -ENOMEM;
 		goto out_release_mem_region;
 	}
-	info->map.name		= pdev->dev.bus_id;
+	info->map.name		= dev_name(&pdev->dev);
 	info->map.phys		= res->start;
 	info->map.size		= size;
 	info->map.bankwidth	= pdata->width;
diff --git a/drivers/mtd/maps/physmap.c b/drivers/mtd/maps/physmap.c
index dfbf3f270ce..1db16e549e3 100644
--- a/drivers/mtd/maps/physmap.c
+++ b/drivers/mtd/maps/physmap.c
@@ -108,13 +108,13 @@ static int physmap_flash_probe(struct platform_device *dev)
 		if (!devm_request_mem_region(&dev->dev,
 			dev->resource[i].start,
 			dev->resource[i].end - dev->resource[i].start + 1,
-			dev->dev.bus_id)) {
+			dev_name(&dev->dev))) {
 			dev_err(&dev->dev, "Could not reserve memory region\n");
 			err = -ENOMEM;
 			goto err_out;
 		}
 
-		info->map[i].name = dev->dev.bus_id;
+		info->map[i].name = dev_name(&dev->dev);
 		info->map[i].phys = dev->resource[i].start;
 		info->map[i].size = dev->resource[i].end - dev->resource[i].start + 1;
 		info->map[i].bankwidth = physmap_data->width;
@@ -150,7 +150,7 @@ static int physmap_flash_probe(struct platform_device *dev)
 		 * We detected multiple devices. Concatenate them together.
 		 */
 #ifdef CONFIG_MTD_CONCAT
-		info->cmtd = mtd_concat_create(info->mtd, devices_found, dev->dev.bus_id);
+		info->cmtd = mtd_concat_create(info->mtd, devices_found, dev_name(&dev->dev));
 		if (info->cmtd == NULL)
 			err = -ENXIO;
 #else
diff --git a/drivers/mtd/maps/physmap_of.c b/drivers/mtd/maps/physmap_of.c
index 5fcfec034a9..fbf0ca939d7 100644
--- a/drivers/mtd/maps/physmap_of.c
+++ b/drivers/mtd/maps/physmap_of.c
@@ -183,7 +183,7 @@ static int __devinit of_flash_probe(struct of_device *dev,
 
 	err = -EBUSY;
 	info->res = request_mem_region(res.start, res.end - res.start + 1,
-				       dev->dev.bus_id);
+				       dev_name(&dev->dev));
 	if (!info->res)
 		goto err_out;
 
@@ -194,7 +194,7 @@ static int __devinit of_flash_probe(struct of_device *dev,
 		goto err_out;
 	}
 
-	info->map.name = dev->dev.bus_id;
+	info->map.name = dev_name(&dev->dev);
 	info->map.phys = res.start;
 	info->map.size = res.end - res.start + 1;
 	info->map.bankwidth = *width;
diff --git a/drivers/mtd/mtdconcat.c b/drivers/mtd/mtdconcat.c
index 789842d0e6f..1a05cf37851 100644
--- a/drivers/mtd/mtdconcat.c
+++ b/drivers/mtd/mtdconcat.c
@@ -691,7 +691,7 @@ static int concat_block_markbad(struct mtd_info *mtd, loff_t ofs)
  */
 struct mtd_info *mtd_concat_create(struct mtd_info *subdev[],	/* subdevices to concatenate */
 				   int num_devs,	/* number of subdevices      */
-				   char *name)
+				   const char *name)
 {				/* name for the new device   */
 	int i;
 	size_t size;
diff --git a/drivers/mtd/nand/fsl_upm.c b/drivers/mtd/nand/fsl_upm.c
index a83192f80eb..7815a404a63 100644
--- a/drivers/mtd/nand/fsl_upm.c
+++ b/drivers/mtd/nand/fsl_upm.c
@@ -222,7 +222,7 @@ static int __devinit fun_probe(struct of_device *ofdev,
 
 	fun->rnb_gpio = of_get_gpio(ofdev->node, 0);
 	if (fun->rnb_gpio >= 0) {
-		ret = gpio_request(fun->rnb_gpio, ofdev->dev.bus_id);
+		ret = gpio_request(fun->rnb_gpio, dev_name(&ofdev->dev));
 		if (ret) {
 			dev_err(&ofdev->dev, "can't request RNB gpio\n");
 			goto err2;
diff --git a/drivers/mtd/nand/plat_nand.c b/drivers/mtd/nand/plat_nand.c
index f674c5427b1..75f9f4874ec 100644
--- a/drivers/mtd/nand/plat_nand.c
+++ b/drivers/mtd/nand/plat_nand.c
@@ -54,7 +54,7 @@ static int __init plat_nand_probe(struct platform_device *pdev)
 	data->chip.priv = &data;
 	data->mtd.priv = &data->chip;
 	data->mtd.owner = THIS_MODULE;
-	data->mtd.name = pdev->dev.bus_id;
+	data->mtd.name = dev_name(&pdev->dev);
 
 	data->chip.IO_ADDR_R = data->io_base;
 	data->chip.IO_ADDR_W = data->io_base;
diff --git a/drivers/mtd/nand/tmio_nand.c b/drivers/mtd/nand/tmio_nand.c
index edb1e322113..daa6a4c3b8c 100644
--- a/drivers/mtd/nand/tmio_nand.c
+++ b/drivers/mtd/nand/tmio_nand.c
@@ -433,7 +433,7 @@ static int tmio_probe(struct platform_device *dev)
 	nand_chip->chip_delay = 15;
 
 	retval = request_irq(irq, &tmio_irq,
-				IRQF_DISABLED, dev->dev.bus_id, tmio);
+				IRQF_DISABLED, dev_name(&dev->dev), tmio);
 	if (retval) {
 		dev_err(&dev->dev, "request_irq error %d\n", retval);
 		goto err_irq;
diff --git a/drivers/mtd/onenand/generic.c b/drivers/mtd/onenand/generic.c
index ad81ab8e95e..5b69e7773c6 100644
--- a/drivers/mtd/onenand/generic.c
+++ b/drivers/mtd/onenand/generic.c
@@ -63,7 +63,7 @@ static int __devinit generic_onenand_probe(struct device *dev)
 	info->onenand.mmcontrol = pdata->mmcontrol;
 	info->onenand.irq = platform_get_irq(pdev, 0);
 
-	info->mtd.name = pdev->dev.bus_id;
+	info->mtd.name = dev_name(&pdev->dev);
 	info->mtd.priv = &info->onenand;
 	info->mtd.owner = THIS_MODULE;
 
diff --git a/drivers/mtd/onenand/omap2.c b/drivers/mtd/onenand/omap2.c
index d1e0b8e7224..96ecc1766fa 100644
--- a/drivers/mtd/onenand/omap2.c
+++ b/drivers/mtd/onenand/omap2.c
@@ -668,7 +668,7 @@ static int __devinit omap2_onenand_probe(struct platform_device *pdev)
 		 c->onenand.base);
 
 	c->pdev = pdev;
-	c->mtd.name = pdev->dev.bus_id;
+	c->mtd.name = dev_name(&pdev->dev);
 	c->mtd.priv = &c->onenand;
 	c->mtd.owner = THIS_MODULE;
 
diff --git a/drivers/mtd/ubi/build.c b/drivers/mtd/ubi/build.c
index ba0bd3d5775..7caf22cd5ad 100644
--- a/drivers/mtd/ubi/build.c
+++ b/drivers/mtd/ubi/build.c
@@ -280,7 +280,7 @@ static int ubi_sysfs_init(struct ubi_device *ubi)
 	ubi->dev.release = dev_release;
 	ubi->dev.devt = ubi->cdev.dev;
 	ubi->dev.class = ubi_class;
-	sprintf(&ubi->dev.bus_id[0], UBI_NAME_STR"%d", ubi->ubi_num);
+	dev_set_name(&ubi->dev, UBI_NAME_STR"%d", ubi->ubi_num);
 	err = device_register(&ubi->dev);
 	if (err)
 		return err;
diff --git a/drivers/mtd/ubi/vmt.c b/drivers/mtd/ubi/vmt.c
index 3531ca9a1e2..22e1d7398fc 100644
--- a/drivers/mtd/ubi/vmt.c
+++ b/drivers/mtd/ubi/vmt.c
@@ -329,7 +329,7 @@ int ubi_create_volume(struct ubi_device *ubi, struct ubi_mkvol_req *req)
 	vol->dev.devt = dev;
 	vol->dev.class = ubi_class;
 
-	sprintf(&vol->dev.bus_id[0], "%s_%d", ubi->ubi_name, vol->vol_id);
+	dev_set_name(&vol->dev, "%s_%d", ubi->ubi_name, vol->vol_id);
 	err = device_register(&vol->dev);
 	if (err) {
 		ubi_err("cannot register device");
@@ -678,7 +678,7 @@ int ubi_add_volume(struct ubi_device *ubi, struct ubi_volume *vol)
 	vol->dev.parent = &ubi->dev;
 	vol->dev.devt = dev;
 	vol->dev.class = ubi_class;
-	sprintf(&vol->dev.bus_id[0], "%s_%d", ubi->ubi_name, vol->vol_id);
+	dev_set_name(&vol->dev, "%s_%d", ubi->ubi_name, vol->vol_id);
 	err = device_register(&vol->dev);
 	if (err)
 		goto out_gluebi;
diff --git a/drivers/parport/ieee1284.c b/drivers/parport/ieee1284.c
index ac2a805ac7e..8901ecf6e03 100644
--- a/drivers/parport/ieee1284.c
+++ b/drivers/parport/ieee1284.c
@@ -84,7 +84,7 @@ int parport_wait_event (struct parport *port, signed long timeout)
 
 	add_timer (&timer);
 	ret = down_interruptible (&port->physport->ieee1284.irq);
-	if (!del_timer (&timer) && !ret)
+	if (!del_timer_sync(&timer) && !ret)
 		/* Timed out. */
 		ret = 1;
 
diff --git a/drivers/pci/pci-driver.c b/drivers/pci/pci-driver.c
index b4cdd690ae7..99d867bcf22 100644
--- a/drivers/pci/pci-driver.c
+++ b/drivers/pci/pci-driver.c
@@ -300,6 +300,14 @@ static void pci_device_shutdown(struct device *dev)
 
 #ifdef CONFIG_PM_SLEEP
 
+static bool pci_has_legacy_pm_support(struct pci_dev *pci_dev)
+{
+	struct pci_driver *drv = pci_dev->driver;
+
+	return drv && (drv->suspend || drv->suspend_late || drv->resume
+		|| drv->resume_early);
+}
+
 /*
  * Default "suspend" method for devices that have no driver provided suspend,
  * or not even a driver at all.
@@ -317,14 +325,22 @@ static void pci_default_pm_suspend(struct pci_dev *pci_dev)
 
 /*
  * Default "resume" method for devices that have no driver provided resume,
- * or not even a driver at all.
+ * or not even a driver at all (first part).
  */
-static int pci_default_pm_resume(struct pci_dev *pci_dev)
+static void pci_default_pm_resume_early(struct pci_dev *pci_dev)
 {
-	int retval = 0;
-
 	/* restore the PCI config space */
 	pci_restore_state(pci_dev);
+}
+
+/*
+ * Default "resume" method for devices that have no driver provided resume,
+ * or not even a driver at all (second part).
+ */
+static int pci_default_pm_resume_late(struct pci_dev *pci_dev)
+{
+	int retval;
+
 	/* if the device was enabled before suspend, reenable */
 	retval = pci_reenable_device(pci_dev);
 	/*
@@ -371,10 +387,12 @@ static int pci_legacy_resume(struct device *dev)
 	struct pci_dev * pci_dev = to_pci_dev(dev);
 	struct pci_driver * drv = pci_dev->driver;
 
-	if (drv && drv->resume)
+	if (drv && drv->resume) {
 		error = drv->resume(pci_dev);
-	else
-		error = pci_default_pm_resume(pci_dev);
+	} else {
+		pci_default_pm_resume_early(pci_dev);
+		error = pci_default_pm_resume_late(pci_dev);
+	}
 	return error;
 }
 
@@ -420,10 +438,8 @@ static int pci_pm_suspend(struct device *dev)
 		if (drv->pm->suspend) {
 			error = drv->pm->suspend(dev);
 			suspend_report_result(drv->pm->suspend, error);
-		} else {
-			pci_default_pm_suspend(pci_dev);
 		}
-	} else {
+	} else if (pci_has_legacy_pm_support(pci_dev)) {
 		error = pci_legacy_suspend(dev, PMSG_SUSPEND);
 	}
 	pci_fixup_device(pci_fixup_suspend, pci_dev);
@@ -434,7 +450,7 @@ static int pci_pm_suspend(struct device *dev)
 static int pci_pm_suspend_noirq(struct device *dev)
 {
 	struct pci_dev *pci_dev = to_pci_dev(dev);
-	struct pci_driver *drv = pci_dev->driver;
+	struct device_driver *drv = dev->driver;
 	int error = 0;
 
 	if (drv && drv->pm) {
@@ -442,8 +458,10 @@ static int pci_pm_suspend_noirq(struct device *dev)
 			error = drv->pm->suspend_noirq(dev);
 			suspend_report_result(drv->pm->suspend_noirq, error);
 		}
-	} else {
+	} else if (pci_has_legacy_pm_support(pci_dev)) {
 		error = pci_legacy_suspend_late(dev, PMSG_SUSPEND);
+	} else {
+		pci_default_pm_suspend(pci_dev);
 	}
 
 	return error;
@@ -453,15 +471,17 @@ static int pci_pm_resume(struct device *dev)
 {
 	struct pci_dev *pci_dev = to_pci_dev(dev);
 	struct device_driver *drv = dev->driver;
-	int error;
+	int error = 0;
 
 	pci_fixup_device(pci_fixup_resume, pci_dev);
 
 	if (drv && drv->pm) {
-		error = drv->pm->resume ? drv->pm->resume(dev) :
-			pci_default_pm_resume(pci_dev);
-	} else {
+		if (drv->pm->resume)
+			error = drv->pm->resume(dev);
+	} else if (pci_has_legacy_pm_support(pci_dev)) {
 		error = pci_legacy_resume(dev);
+	} else {
+		error = pci_default_pm_resume_late(pci_dev);
 	}
 
 	return error;
@@ -470,16 +490,18 @@ static int pci_pm_resume(struct device *dev)
 static int pci_pm_resume_noirq(struct device *dev)
 {
 	struct pci_dev *pci_dev = to_pci_dev(dev);
-	struct pci_driver *drv = pci_dev->driver;
+	struct device_driver *drv = dev->driver;
 	int error = 0;
 
-	pci_fixup_device(pci_fixup_resume_early, pci_dev);
+	pci_fixup_device(pci_fixup_resume_early, to_pci_dev(dev));
 
 	if (drv && drv->pm) {
 		if (drv->pm->resume_noirq)
 			error = drv->pm->resume_noirq(dev);
-	} else {
+	} else if (pci_has_legacy_pm_support(pci_dev)) {
 		error = pci_legacy_resume_early(dev);
+	} else {
+		pci_default_pm_resume_early(pci_dev);
 	}
 
 	return error;
@@ -506,10 +528,8 @@ static int pci_pm_freeze(struct device *dev)
 		if (drv->pm->freeze) {
 			error = drv->pm->freeze(dev);
 			suspend_report_result(drv->pm->freeze, error);
-		} else {
-			pci_default_pm_suspend(pci_dev);
 		}
-	} else {
+	} else if (pci_has_legacy_pm_support(pci_dev)) {
 		error = pci_legacy_suspend(dev, PMSG_FREEZE);
 		pci_fixup_device(pci_fixup_suspend, pci_dev);
 	}
@@ -520,7 +540,7 @@ static int pci_pm_freeze(struct device *dev)
 static int pci_pm_freeze_noirq(struct device *dev)
 {
 	struct pci_dev *pci_dev = to_pci_dev(dev);
-	struct pci_driver *drv = pci_dev->driver;
+	struct device_driver *drv = dev->driver;
 	int error = 0;
 
 	if (drv && drv->pm) {
@@ -528,8 +548,10 @@ static int pci_pm_freeze_noirq(struct device *dev)
 			error = drv->pm->freeze_noirq(dev);
 			suspend_report_result(drv->pm->freeze_noirq, error);
 		}
-	} else {
+	} else if (pci_has_legacy_pm_support(pci_dev)) {
 		error = pci_legacy_suspend_late(dev, PMSG_FREEZE);
+	} else {
+		pci_default_pm_suspend(pci_dev);
 	}
 
 	return error;
@@ -537,14 +559,15 @@ static int pci_pm_freeze_noirq(struct device *dev)
 
 static int pci_pm_thaw(struct device *dev)
 {
+	struct pci_dev *pci_dev = to_pci_dev(dev);
 	struct device_driver *drv = dev->driver;
 	int error = 0;
 
 	if (drv && drv->pm) {
 		if (drv->pm->thaw)
 			error =  drv->pm->thaw(dev);
-	} else {
-		pci_fixup_device(pci_fixup_resume, to_pci_dev(dev));
+	} else if (pci_has_legacy_pm_support(pci_dev)) {
+		pci_fixup_device(pci_fixup_resume, pci_dev);
 		error = pci_legacy_resume(dev);
 	}
 
@@ -554,14 +577,14 @@ static int pci_pm_thaw(struct device *dev)
 static int pci_pm_thaw_noirq(struct device *dev)
 {
 	struct pci_dev *pci_dev = to_pci_dev(dev);
-	struct pci_driver *drv = pci_dev->driver;
+	struct device_driver *drv = dev->driver;
 	int error = 0;
 
 	if (drv && drv->pm) {
 		if (drv->pm->thaw_noirq)
 			error = drv->pm->thaw_noirq(dev);
-	} else {
-		pci_fixup_device(pci_fixup_resume_early, pci_dev);
+	} else if (pci_has_legacy_pm_support(pci_dev)) {
+		pci_fixup_device(pci_fixup_resume_early, to_pci_dev(dev));
 		error = pci_legacy_resume_early(dev);
 	}
 
@@ -570,17 +593,18 @@ static int pci_pm_thaw_noirq(struct device *dev)
 
 static int pci_pm_poweroff(struct device *dev)
 {
+	struct pci_dev *pci_dev = to_pci_dev(dev);
 	struct device_driver *drv = dev->driver;
 	int error = 0;
 
-	pci_fixup_device(pci_fixup_suspend, to_pci_dev(dev));
+	pci_fixup_device(pci_fixup_suspend, pci_dev);
 
 	if (drv && drv->pm) {
 		if (drv->pm->poweroff) {
 			error = drv->pm->poweroff(dev);
 			suspend_report_result(drv->pm->poweroff, error);
 		}
-	} else {
+	} else if (pci_has_legacy_pm_support(pci_dev)) {
 		error = pci_legacy_suspend(dev, PMSG_HIBERNATE);
 	}
 
@@ -589,8 +613,7 @@ static int pci_pm_poweroff(struct device *dev)
 
 static int pci_pm_poweroff_noirq(struct device *dev)
 {
-	struct pci_dev *pci_dev = to_pci_dev(dev);
-	struct pci_driver *drv = pci_dev->driver;
+	struct device_driver *drv = dev->driver;
 	int error = 0;
 
 	if (drv && drv->pm) {
@@ -598,7 +621,7 @@ static int pci_pm_poweroff_noirq(struct device *dev)
 			error = drv->pm->poweroff_noirq(dev);
 			suspend_report_result(drv->pm->poweroff_noirq, error);
 		}
-	} else {
+	} else if (pci_has_legacy_pm_support(to_pci_dev(dev))) {
 		error = pci_legacy_suspend_late(dev, PMSG_HIBERNATE);
 	}
 
@@ -609,13 +632,15 @@ static int pci_pm_restore(struct device *dev)
 {
 	struct pci_dev *pci_dev = to_pci_dev(dev);
 	struct device_driver *drv = dev->driver;
-	int error;
+	int error = 0;
 
 	if (drv && drv->pm) {
-		error = drv->pm->restore ? drv->pm->restore(dev) :
-			pci_default_pm_resume(pci_dev);
-	} else {
+		if (drv->pm->restore)
+			error = drv->pm->restore(dev);
+	} else if (pci_has_legacy_pm_support(pci_dev)) {
 		error = pci_legacy_resume(dev);
+	} else {
+		error = pci_default_pm_resume_late(pci_dev);
 	}
 	pci_fixup_device(pci_fixup_resume, pci_dev);
 
@@ -625,7 +650,7 @@ static int pci_pm_restore(struct device *dev)
 static int pci_pm_restore_noirq(struct device *dev)
 {
 	struct pci_dev *pci_dev = to_pci_dev(dev);
-	struct pci_driver *drv = pci_dev->driver;
+	struct device_driver *drv = dev->driver;
 	int error = 0;
 
 	pci_fixup_device(pci_fixup_resume, pci_dev);
@@ -633,8 +658,10 @@ static int pci_pm_restore_noirq(struct device *dev)
 	if (drv && drv->pm) {
 		if (drv->pm->restore_noirq)
 			error = drv->pm->restore_noirq(dev);
-	} else {
+	} else if (pci_has_legacy_pm_support(pci_dev)) {
 		error = pci_legacy_resume_early(dev);
+	} else {
+		pci_default_pm_resume_early(pci_dev);
 	}
 	pci_fixup_device(pci_fixup_resume_early, pci_dev);
 
@@ -654,17 +681,15 @@ static int pci_pm_restore_noirq(struct device *dev)
 
 #endif /* !CONFIG_HIBERNATION */
 
-struct pm_ext_ops pci_pm_ops = {
-	.base = {
-		.prepare = pci_pm_prepare,
-		.complete = pci_pm_complete,
-		.suspend = pci_pm_suspend,
-		.resume = pci_pm_resume,
-		.freeze = pci_pm_freeze,
-		.thaw = pci_pm_thaw,
-		.poweroff = pci_pm_poweroff,
-		.restore = pci_pm_restore,
-	},
+struct dev_pm_ops pci_dev_pm_ops = {
+	.prepare = pci_pm_prepare,
+	.complete = pci_pm_complete,
+	.suspend = pci_pm_suspend,
+	.resume = pci_pm_resume,
+	.freeze = pci_pm_freeze,
+	.thaw = pci_pm_thaw,
+	.poweroff = pci_pm_poweroff,
+	.restore = pci_pm_restore,
 	.suspend_noirq = pci_pm_suspend_noirq,
 	.resume_noirq = pci_pm_resume_noirq,
 	.freeze_noirq = pci_pm_freeze_noirq,
@@ -673,7 +698,7 @@ struct pm_ext_ops pci_pm_ops = {
 	.restore_noirq = pci_pm_restore_noirq,
 };
 
-#define PCI_PM_OPS_PTR	&pci_pm_ops
+#define PCI_PM_OPS_PTR	(&pci_dev_pm_ops)
 
 #else /* !CONFIG_PM_SLEEP */
 
@@ -703,9 +728,6 @@ int __pci_register_driver(struct pci_driver *drv, struct module *owner,
 	drv->driver.owner = owner;
 	drv->driver.mod_name = mod_name;
 
-	if (drv->pm)
-		drv->driver.pm = &drv->pm->base;
-
 	spin_lock_init(&drv->dynids.lock);
 	INIT_LIST_HEAD(&drv->dynids.list);
 
diff --git a/drivers/pnp/card.c b/drivers/pnp/card.c
index e75b060daa9..efea128f02d 100644
--- a/drivers/pnp/card.c
+++ b/drivers/pnp/card.c
@@ -165,8 +165,7 @@ struct pnp_card *pnp_alloc_card(struct pnp_protocol *protocol, int id, char *pnp
 	card->number = id;
 
 	card->dev.parent = &card->protocol->dev;
-	sprintf(card->dev.bus_id, "%02x:%02x", card->protocol->number,
-		card->number);
+	dev_set_name(&card->dev, "%02x:%02x", card->protocol->number, card->number);
 
 	card->dev.coherent_dma_mask = DMA_24BIT_MASK;
 	card->dev.dma_mask = &card->dev.coherent_dma_mask;
@@ -295,8 +294,8 @@ int pnp_add_card_device(struct pnp_card *card, struct pnp_dev *dev)
 {
 	dev->dev.parent = &card->dev;
 	dev->card_link = NULL;
-	snprintf(dev->dev.bus_id, BUS_ID_SIZE, "%02x:%02x.%02x",
-		 dev->protocol->number, card->number, dev->number);
+	dev_set_name(&dev->dev, "%02x:%02x.%02x",
+		     dev->protocol->number, card->number, dev->number);
 	spin_lock(&pnp_lock);
 	dev->card = card;
 	list_add_tail(&dev->card_list, &card->devices);
diff --git a/drivers/pnp/core.c b/drivers/pnp/core.c
index 16c01c6fa7c..14814f23173 100644
--- a/drivers/pnp/core.c
+++ b/drivers/pnp/core.c
@@ -70,7 +70,7 @@ int pnp_register_protocol(struct pnp_protocol *protocol)
 	spin_unlock(&pnp_lock);
 
 	protocol->number = nodenum;
-	sprintf(protocol->dev.bus_id, "pnp%d", nodenum);
+	dev_set_name(&protocol->dev, "pnp%d", nodenum);
 	return device_register(&protocol->dev);
 }
 
@@ -145,8 +145,7 @@ struct pnp_dev *pnp_alloc_dev(struct pnp_protocol *protocol, int id, char *pnpid
 	dev->dev.coherent_dma_mask = dev->dma_mask;
 	dev->dev.release = &pnp_release_device;
 
-	sprintf(dev->dev.bus_id, "%02x:%02x", dev->protocol->number,
-		dev->number);
+	dev_set_name(&dev->dev, "%02x:%02x", dev->protocol->number, dev->number);
 
 	dev_id = pnp_add_id(dev, pnpid);
 	if (!dev_id) {
diff --git a/drivers/pnp/system.c b/drivers/pnp/system.c
index 764f3a31068..59b90922da8 100644
--- a/drivers/pnp/system.c
+++ b/drivers/pnp/system.c
@@ -26,7 +26,7 @@ static void reserve_range(struct pnp_dev *dev, resource_size_t start,
 			  resource_size_t end, int port)
 {
 	char *regionid;
-	const char *pnpid = dev->dev.bus_id;
+	const char *pnpid = dev_name(&dev->dev);
 	struct resource *res;
 
 	regionid = kmalloc(16, GFP_KERNEL);
diff --git a/drivers/power/ds2760_battery.c b/drivers/power/ds2760_battery.c
index 308ddb201b6..1d768928e0b 100644
--- a/drivers/power/ds2760_battery.c
+++ b/drivers/power/ds2760_battery.c
@@ -354,7 +354,7 @@ static int ds2760_battery_probe(struct platform_device *pdev)
 	pdata = pdev->dev.platform_data;
 	di->dev		= &pdev->dev;
 	di->w1_dev	     = pdev->dev.parent;
-	di->bat.name	   = pdev->dev.bus_id;
+	di->bat.name	   = dev_name(&pdev->dev);
 	di->bat.type	   = POWER_SUPPLY_TYPE_BATTERY;
 	di->bat.properties     = ds2760_battery_props;
 	di->bat.num_properties = ARRAY_SIZE(ds2760_battery_props);
@@ -371,7 +371,7 @@ static int ds2760_battery_probe(struct platform_device *pdev)
 	}
 
 	INIT_DELAYED_WORK(&di->monitor_work, ds2760_battery_work);
-	di->monitor_wqueue = create_singlethread_workqueue(pdev->dev.bus_id);
+	di->monitor_wqueue = create_singlethread_workqueue(dev_name(&pdev->dev));
 	if (!di->monitor_wqueue) {
 		retval = -ESRCH;
 		goto workqueue_failed;
diff --git a/drivers/rapidio/rio-driver.c b/drivers/rapidio/rio-driver.c
index 956d3e79f6a..addb87cf44d 100644
--- a/drivers/rapidio/rio-driver.c
+++ b/drivers/rapidio/rio-driver.c
@@ -79,7 +79,6 @@ void rio_dev_put(struct rio_dev *rdev)
 
 /**
  *  rio_device_probe - Tell if a RIO device structure has a matching RIO device id structure
- *  @id: the RIO device id structure to match against
  *  @dev: the RIO device structure to match against
  *
  * return 0 and set rio_dev->driver when drv claims rio_dev, else error
diff --git a/drivers/rtc/Kconfig b/drivers/rtc/Kconfig
index 165a8184335..4ad831de41a 100644
--- a/drivers/rtc/Kconfig
+++ b/drivers/rtc/Kconfig
@@ -35,8 +35,8 @@ config RTC_HCTOSYS_DEVICE
 	default "rtc0"
 	help
 	  The RTC device that will be used to (re)initialize the system
-	  clock, usually rtc0.  Initialization is done when the system
-	  starts up, and when it resumes from a low power state.  This
+	  clock, usually rtc0. Initialization is done when the system
+	  starts up, and when it resumes from a low power state. This
 	  device should record time in UTC, since the kernel won't do
 	  timezone correction.
 
@@ -44,7 +44,7 @@ config RTC_HCTOSYS_DEVICE
 	  functions run, so it must usually be statically linked.
 
 	  This clock should be battery-backed, so that it reads the correct
-	  time when the system boots from a power-off state.  Otherwise, your
+	  time when the system boots from a power-off state. Otherwise, your
 	  system will need an external clock source (like an NTP server).
 
 	  If the clock you specify here is not battery backed, it may still
@@ -69,8 +69,7 @@ config RTC_INTF_SYSFS
 	  Say yes here if you want to use your RTCs using sysfs interfaces,
 	  /sys/class/rtc/rtc0 through /sys/.../rtcN.
 
-	  This driver can also be built as a module. If so, the module
-	  will be called rtc-sysfs.
+	  If unsure, say Y.
 
 config RTC_INTF_PROC
 	boolean "/proc/driver/rtc (procfs for rtc0)"
@@ -78,11 +77,10 @@ config RTC_INTF_PROC
 	default RTC_CLASS
 	help
 	  Say yes here if you want to use your first RTC through the proc
-	  interface, /proc/driver/rtc.  Other RTCs will not be available
+	  interface, /proc/driver/rtc. Other RTCs will not be available
 	  through that API.
 
-	  This driver can also be built as a module. If so, the module
-	  will be called rtc-proc.
+	  If unsure, say Y.
 
 config RTC_INTF_DEV
 	boolean "/dev/rtcN (character devices)"
@@ -90,12 +88,14 @@ config RTC_INTF_DEV
 	help
 	  Say yes here if you want to use your RTCs using the /dev
 	  interfaces, which "udev" sets up as /dev/rtc0 through
-	  /dev/rtcN.  You may want to set up a symbolic link so one
-	  of these can be accessed as /dev/rtc, which is a name
-	  expected by "hwclock" and some other programs.
+	  /dev/rtcN.
 
-	  This driver can also be built as a module. If so, the module
-	  will be called rtc-dev.
+	  You may want to set up a symbolic link so one of these
+	  can be accessed as /dev/rtc, which is a name
+	  expected by "hwclock" and some other programs. Recent
+	  versions of "udev" are known to set up the symlink for you.
+
+	  If unsure, say Y.
 
 config RTC_INTF_DEV_UIE_EMUL
 	bool "RTC UIE emulation on dev interface"
@@ -132,14 +132,14 @@ config RTC_DRV_DS1307
 	tristate "Dallas/Maxim DS1307/37/38/39/40, ST M41T00"
 	help
 	  If you say yes here you get support for various compatible RTC
-	  chips (often with battery backup) connected with I2C.  This driver
+	  chips (often with battery backup) connected with I2C. This driver
 	  should handle DS1307, DS1337, DS1338, DS1339, DS1340, ST M41T00,
-	  and probably other chips.  In some cases the RTC must already
+	  and probably other chips. In some cases the RTC must already
 	  have been initialized (by manufacturing or a bootloader).
 
 	  The first seven registers on these chips hold an RTC, and other
 	  registers may add features such as NVRAM, a trickle charger for
-	  the RTC/NVRAM backup power, and alarms.  NVRAM is visible in
+	  the RTC/NVRAM backup power, and alarms. NVRAM is visible in
 	  sysfs, but other chip features may not be available.
 
 	  This driver can also be built as a module. If so, the module
@@ -150,10 +150,10 @@ config RTC_DRV_DS1374
 	depends on RTC_CLASS && I2C
 	help
 	  If you say yes here you get support for Dallas Semiconductor
-	  DS1374 real-time clock chips.  If an interrupt is associated
+	  DS1374 real-time clock chips. If an interrupt is associated
 	  with the device, the alarm functionality is supported.
 
-	  This driver can also be built as a module.  If so, the module
+	  This driver can also be built as a module. If so, the module
 	  will be called rtc-ds1374.
 
 config RTC_DRV_DS1672
@@ -247,7 +247,7 @@ config RTC_DRV_TWL92330
 	help
 	  If you say yes here you get support for the RTC on the
 	  TWL92330 "Menelaus" power management chip, used with OMAP2
-	  platforms.  The support is integrated with the rest of
+	  platforms. The support is integrated with the rest of
 	  the Menelaus driver; it's not separate module.
 
 config RTC_DRV_TWL4030
@@ -308,7 +308,7 @@ config RTC_DRV_DS1305
 	tristate "Dallas/Maxim DS1305/DS1306"
 	help
 	  Select this driver to get support for the Dallas/Maxim DS1305
-	  and DS1306 real time clock chips.  These support a trickle
+	  and DS1306 real time clock chips. These support a trickle
 	  charger, alarms, and NVRAM in addition to the clock.
 
 	  This driver can also be built as a module. If so, the module
@@ -317,7 +317,8 @@ config RTC_DRV_DS1305
 config RTC_DRV_DS1390
 	tristate "Dallas/Maxim DS1390/93/94"
 	help
-	  If you say yes here you get support for the DS1390/93/94 chips.
+	  If you say yes here you get support for the
+	  Dallas/Maxim DS1390/93/94 chips.
 
 	  This driver only supports the RTC feature, and not other chip
 	  features such as alarms and trickle charging.
@@ -381,7 +382,7 @@ config RTC_DRV_CMOS
 	  or LPC bus chips, and so on.
 
 	  Your system will need to define the platform device used by
-	  this driver, otherwise it won't be accessible.  This means
+	  this driver, otherwise it won't be accessible. This means
 	  you can safely enable this driver if you don't know whether
 	  or not your board has this kind of hardware.
 
@@ -598,7 +599,7 @@ config RTC_DRV_AT91RM9200
 	depends on ARCH_AT91RM9200 || ARCH_AT91SAM9RL
 	help
 	  Driver for the internal RTC (Realtime Clock) module found on
-	  Atmel AT91RM9200's and AT91SAM9RL chips.  On SAM9RL chips
+	  Atmel AT91RM9200's and AT91SAM9RL chips. On SAM9RL chips
 	  this is powered by the backup power supply.
 
 config RTC_DRV_AT91SAM9
@@ -620,8 +621,8 @@ config RTC_DRV_AT91SAM9_RTT
 	prompt "RTT module Number" if ARCH_AT91SAM9263
 	depends on RTC_DRV_AT91SAM9
 	help
-	  More than one RTT module is available.  You can choose which
-	  one will be used as an RTC.  The default of zero is normally
+	  More than one RTT module is available. You can choose which
+	  one will be used as an RTC. The default of zero is normally
 	  OK to use, though some systems use that for non-RTC purposes.
 
 config RTC_DRV_AT91SAM9_GPBR
@@ -633,10 +634,20 @@ config RTC_DRV_AT91SAM9_GPBR
 	depends on RTC_DRV_AT91SAM9
 	help
 	  The RTC driver needs to use one of the General Purpose Backup
-	  Registers (GPBRs) as well as the RTT.  You can choose which one
-	  will be used.  The default of zero is normally OK to use, but
+	  Registers (GPBRs) as well as the RTT. You can choose which one
+	  will be used. The default of zero is normally OK to use, but
 	  on some systems other software needs to use that register.
 
+config RTC_DRV_AU1XXX
+	tristate "Au1xxx Counter0 RTC support"
+	depends on SOC_AU1X00
+	help
+	  This is a driver for the Au1xxx on-chip Counter0 (Time-Of-Year
+	  counter) to be used as a RTC.
+
+	  This driver can also be built as a module. If so, the module
+	  will be called rtc-au1xxx.
+
 config RTC_DRV_BFIN
 	tristate "Blackfin On-Chip RTC"
 	depends on BLACKFIN && !BF561
@@ -669,6 +680,17 @@ config RTC_DRV_PPC
 	 the RTC. This exposes that functionality through the generic RTC
 	 class.
 
+config RTC_DRV_PXA
+       tristate "PXA27x/PXA3xx"
+       depends on ARCH_PXA
+       help
+         If you say Y here you will get access to the real time clock
+         built into your PXA27x or PXA3xx CPU.
+
+         This RTC driver uses PXA RTC registers available since pxa27x
+         series (RDxR, RYxR) instead of legacy RCNR, RTAR.
+
+
 config RTC_DRV_SUN4V
 	bool "SUN4V Hypervisor RTC"
 	depends on SPARC64
@@ -683,4 +705,22 @@ config RTC_DRV_STARFIRE
 	  If you say Y here you will get support for the RTC found on
 	  Starfire systems.
 
+config RTC_DRV_TX4939
+	tristate "TX4939 SoC"
+	depends on SOC_TX4939
+	help
+	  Driver for the internal RTC (Realtime Clock) module found on
+	  Toshiba TX4939 SoC.
+
+config RTC_DRV_MV
+	tristate "Marvell SoC RTC"
+	depends on ARCH_KIRKWOOD
+	help
+	  If you say yes here you will get support for the in-chip RTC
+	  that can be found in some of Marvell's SoC devices, such as
+	  the Kirkwood 88F6281 and 88F6192.
+
+	  This driver can also be built as a module. If so, the module
+	  will be called rtc-mv.
+
 endif # RTC_CLASS
diff --git a/drivers/rtc/Makefile b/drivers/rtc/Makefile
index 6e79c912bf9..9a4340d48f2 100644
--- a/drivers/rtc/Makefile
+++ b/drivers/rtc/Makefile
@@ -20,6 +20,7 @@ rtc-core-$(CONFIG_RTC_INTF_SYSFS) += rtc-sysfs.o
 obj-$(CONFIG_RTC_DRV_AT32AP700X)+= rtc-at32ap700x.o
 obj-$(CONFIG_RTC_DRV_AT91RM9200)+= rtc-at91rm9200.o
 obj-$(CONFIG_RTC_DRV_AT91SAM9)	+= rtc-at91sam9.o
+obj-$(CONFIG_RTC_DRV_AU1XXX)	+= rtc-au1xxx.o
 obj-$(CONFIG_RTC_DRV_BFIN)	+= rtc-bfin.o
 obj-$(CONFIG_RTC_DRV_CMOS)	+= rtc-cmos.o
 obj-$(CONFIG_RTC_DRV_DS1216)	+= rtc-ds1216.o
@@ -47,6 +48,7 @@ obj-$(CONFIG_RTC_DRV_SUN4V)	+= rtc-sun4v.o
 obj-$(CONFIG_RTC_DRV_STARFIRE)	+= rtc-starfire.o
 obj-$(CONFIG_RTC_DRV_MAX6900)	+= rtc-max6900.o
 obj-$(CONFIG_RTC_DRV_MAX6902)	+= rtc-max6902.o
+obj-$(CONFIG_RTC_DRV_MV)	+= rtc-mv.o
 obj-$(CONFIG_RTC_DRV_OMAP)	+= rtc-omap.o
 obj-$(CONFIG_RTC_DRV_PCF8563)	+= rtc-pcf8563.o
 obj-$(CONFIG_RTC_DRV_PCF8583)	+= rtc-pcf8583.o
@@ -54,6 +56,7 @@ obj-$(CONFIG_RTC_DRV_PL030)	+= rtc-pl030.o
 obj-$(CONFIG_RTC_DRV_PL031)	+= rtc-pl031.o
 obj-$(CONFIG_RTC_DRV_PARISC)	+= rtc-parisc.o
 obj-$(CONFIG_RTC_DRV_PPC)	+= rtc-ppc.o
+obj-$(CONFIG_RTC_DRV_PXA)	+= rtc-pxa.o
 obj-$(CONFIG_RTC_DRV_R9701)	+= rtc-r9701.o
 obj-$(CONFIG_RTC_DRV_RS5C313)	+= rtc-rs5c313.o
 obj-$(CONFIG_RTC_DRV_RS5C348)	+= rtc-rs5c348.o
@@ -66,6 +69,7 @@ obj-$(CONFIG_RTC_DRV_SH)	+= rtc-sh.o
 obj-$(CONFIG_RTC_DRV_STK17TA8)	+= rtc-stk17ta8.o
 obj-$(CONFIG_RTC_DRV_TEST)	+= rtc-test.o
 obj-$(CONFIG_RTC_DRV_TWL4030)	+= rtc-twl4030.o
+obj-$(CONFIG_RTC_DRV_TX4939)	+= rtc-tx4939.o
 obj-$(CONFIG_RTC_DRV_V3020)	+= rtc-v3020.o
 obj-$(CONFIG_RTC_DRV_VR41XX)	+= rtc-vr41xx.o
 obj-$(CONFIG_RTC_DRV_WM8350)	+= rtc-wm8350.o
diff --git a/drivers/rtc/class.c b/drivers/rtc/class.c
index 4dfdf019fcc..be5a6b73e60 100644
--- a/drivers/rtc/class.c
+++ b/drivers/rtc/class.c
@@ -48,9 +48,7 @@ static int rtc_suspend(struct device *dev, pm_message_t mesg)
 	struct rtc_time		tm;
 	struct timespec		ts = current_kernel_time();
 
-	if (strncmp(rtc->dev.bus_id,
-				CONFIG_RTC_HCTOSYS_DEVICE,
-				BUS_ID_SIZE) != 0)
+	if (strcmp(dev_name(&rtc->dev), CONFIG_RTC_HCTOSYS_DEVICE) != 0)
 		return 0;
 
 	rtc_read_time(rtc, &tm);
@@ -71,20 +69,18 @@ static int rtc_resume(struct device *dev)
 	time_t			newtime;
 	struct timespec		time;
 
-	if (strncmp(rtc->dev.bus_id,
-				CONFIG_RTC_HCTOSYS_DEVICE,
-				BUS_ID_SIZE) != 0)
+	if (strcmp(dev_name(&rtc->dev), CONFIG_RTC_HCTOSYS_DEVICE) != 0)
 		return 0;
 
 	rtc_read_time(rtc, &tm);
 	if (rtc_valid_tm(&tm) != 0) {
-		pr_debug("%s:  bogus resume time\n", rtc->dev.bus_id);
+		pr_debug("%s:  bogus resume time\n", dev_name(&rtc->dev));
 		return 0;
 	}
 	rtc_tm_to_time(&tm, &newtime);
 	if (newtime <= oldtime) {
 		if (newtime < oldtime)
-			pr_debug("%s:  time travel!\n", rtc->dev.bus_id);
+			pr_debug("%s:  time travel!\n", dev_name(&rtc->dev));
 		return 0;
 	}
 
@@ -156,7 +152,7 @@ struct rtc_device *rtc_device_register(const char *name, struct device *dev,
 	init_waitqueue_head(&rtc->irq_queue);
 
 	strlcpy(rtc->name, name, RTC_DEVICE_NAME_SIZE);
-	snprintf(rtc->dev.bus_id, BUS_ID_SIZE, "rtc%d", id);
+	dev_set_name(&rtc->dev, "rtc%d", id);
 
 	rtc_dev_prepare(rtc);
 
@@ -169,7 +165,7 @@ struct rtc_device *rtc_device_register(const char *name, struct device *dev,
 	rtc_proc_add_device(rtc);
 
 	dev_info(dev, "rtc core: registered %s as %s\n",
-			rtc->name, rtc->dev.bus_id);
+			rtc->name, dev_name(&rtc->dev));
 
 	return rtc;
 
diff --git a/drivers/rtc/interface.c b/drivers/rtc/interface.c
index fd2c652504f..4348c4b0d45 100644
--- a/drivers/rtc/interface.c
+++ b/drivers/rtc/interface.c
@@ -50,10 +50,15 @@ int rtc_set_time(struct rtc_device *rtc, struct rtc_time *tm)
 
 	if (!rtc->ops)
 		err = -ENODEV;
-	else if (!rtc->ops->set_time)
-		err = -EINVAL;
-	else
+	else if (rtc->ops->set_time)
 		err = rtc->ops->set_time(rtc->dev.parent, tm);
+	else if (rtc->ops->set_mmss) {
+		unsigned long secs;
+		err = rtc_tm_to_time(tm, &secs);
+		if (err == 0)
+			err = rtc->ops->set_mmss(rtc->dev.parent, secs);
+	} else
+		err = -EINVAL;
 
 	mutex_unlock(&rtc->ops_lock);
 	return err;
@@ -389,7 +394,7 @@ static int __rtc_match(struct device *dev, void *data)
 {
 	char *name = (char *)data;
 
-	if (strncmp(dev->bus_id, name, BUS_ID_SIZE) == 0)
+	if (strcmp(dev_name(dev), name) == 0)
 		return 1;
 	return 0;
 }
@@ -504,9 +509,6 @@ int rtc_irq_set_freq(struct rtc_device *rtc, struct rtc_task *task, int freq)
 	if (rtc->ops->irq_set_freq == NULL)
 		return -ENXIO;
 
-	if (!is_power_of_2(freq))
-		return -EINVAL;
-
 	spin_lock_irqsave(&rtc->irq_task_lock, flags);
 	if (rtc->irq_task != NULL && task == NULL)
 		err = -EBUSY;
diff --git a/drivers/rtc/rtc-at32ap700x.c b/drivers/rtc/rtc-at32ap700x.c
index 90b9a6503e1..e1ec33e40e3 100644
--- a/drivers/rtc/rtc-at32ap700x.c
+++ b/drivers/rtc/rtc-at32ap700x.c
@@ -205,7 +205,7 @@ static int __init at32_rtc_probe(struct platform_device *pdev)
 {
 	struct resource	*regs;
 	struct rtc_at32ap700x *rtc;
-	int irq = -1;
+	int irq;
 	int ret;
 
 	rtc = kzalloc(sizeof(struct rtc_at32ap700x), GFP_KERNEL);
@@ -222,7 +222,7 @@ static int __init at32_rtc_probe(struct platform_device *pdev)
 	}
 
 	irq = platform_get_irq(pdev, 0);
-	if (irq < 0) {
+	if (irq <= 0) {
 		dev_dbg(&pdev->dev, "could not get irq\n");
 		ret = -ENXIO;
 		goto out;
diff --git a/drivers/rtc/rtc-au1xxx.c b/drivers/rtc/rtc-au1xxx.c
new file mode 100644
index 00000000000..8906a688e6a
--- /dev/null
+++ b/drivers/rtc/rtc-au1xxx.c
@@ -0,0 +1,153 @@
+/*
+ * Au1xxx counter0 (aka Time-Of-Year counter) RTC interface driver.
+ *
+ * Copyright (C) 2008 Manuel Lauss <mano@roarinelk.homelinux.net>
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.  See the file "COPYING" in the main directory of this archive
+ * for more details.
+ */
+
+/* All current Au1xxx SoCs have 2 counters fed by an external 32.768 kHz
+ * crystal. Counter 0, which keeps counting during sleep/powerdown, is
+ * used to count seconds since the beginning of the unix epoch.
+ *
+ * The counters must be configured and enabled by bootloader/board code;
+ * no checks as to whether they really get a proper 32.768kHz clock are
+ * made as this would take far too long.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/rtc.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/io.h>
+#include <asm/mach-au1x00/au1000.h>
+
+/* 32kHz clock enabled and detected */
+#define CNTR_OK (SYS_CNTRL_E0 | SYS_CNTRL_32S)
+
+static int au1xtoy_rtc_read_time(struct device *dev, struct rtc_time *tm)
+{
+	unsigned long t;
+
+	t = au_readl(SYS_TOYREAD);
+
+	rtc_time_to_tm(t, tm);
+
+	return rtc_valid_tm(tm);
+}
+
+static int au1xtoy_rtc_set_time(struct device *dev, struct rtc_time *tm)
+{
+	unsigned long t;
+
+	rtc_tm_to_time(tm, &t);
+
+	au_writel(t, SYS_TOYWRITE);
+	au_sync();
+
+	/* wait for the pending register write to succeed.  This can
+	 * take up to 6 seconds...
+	 */
+	while (au_readl(SYS_COUNTER_CNTRL) & SYS_CNTRL_C0S)
+		msleep(1);
+
+	return 0;
+}
+
+static struct rtc_class_ops au1xtoy_rtc_ops = {
+	.read_time	= au1xtoy_rtc_read_time,
+	.set_time	= au1xtoy_rtc_set_time,
+};
+
+static int __devinit au1xtoy_rtc_probe(struct platform_device *pdev)
+{
+	struct rtc_device *rtcdev;
+	unsigned long t;
+	int ret;
+
+	t = au_readl(SYS_COUNTER_CNTRL);
+	if (!(t & CNTR_OK)) {
+		dev_err(&pdev->dev, "counters not working; aborting.\n");
+		ret = -ENODEV;
+		goto out_err;
+	}
+
+	ret = -ETIMEDOUT;
+
+	/* set counter0 tickrate to 1Hz if necessary */
+	if (au_readl(SYS_TOYTRIM) != 32767) {
+		/* wait until hardware gives access to TRIM register */
+		t = 0x00100000;
+		while ((au_readl(SYS_COUNTER_CNTRL) & SYS_CNTRL_T0S) && t--)
+			msleep(1);
+
+		if (!t) {
+			/* timed out waiting for register access; assume
+			 * counters are unusable.
+			 */
+			dev_err(&pdev->dev, "timeout waiting for access\n");
+			goto out_err;
+		}
+
+		/* set 1Hz TOY tick rate */
+		au_writel(32767, SYS_TOYTRIM);
+		au_sync();
+	}
+
+	/* wait until the hardware allows writes to the counter reg */
+	while (au_readl(SYS_COUNTER_CNTRL) & SYS_CNTRL_C0S)
+		msleep(1);
+
+	rtcdev = rtc_device_register("rtc-au1xxx", &pdev->dev,
+				     &au1xtoy_rtc_ops, THIS_MODULE);
+	if (IS_ERR(rtcdev)) {
+		ret = PTR_ERR(rtcdev);
+		goto out_err;
+	}
+
+	platform_set_drvdata(pdev, rtcdev);
+
+	return 0;
+
+out_err:
+	return ret;
+}
+
+static int __devexit au1xtoy_rtc_remove(struct platform_device *pdev)
+{
+	struct rtc_device *rtcdev = platform_get_drvdata(pdev);
+
+	rtc_device_unregister(rtcdev);
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+static struct platform_driver au1xrtc_driver = {
+	.driver		= {
+		.name	= "rtc-au1xxx",
+		.owner	= THIS_MODULE,
+	},
+	.remove		= __devexit_p(au1xtoy_rtc_remove),
+};
+
+static int __init au1xtoy_rtc_init(void)
+{
+	return platform_driver_probe(&au1xrtc_driver, au1xtoy_rtc_probe);
+}
+
+static void __exit au1xtoy_rtc_exit(void)
+{
+	platform_driver_unregister(&au1xrtc_driver);
+}
+
+module_init(au1xtoy_rtc_init);
+module_exit(au1xtoy_rtc_exit);
+
+MODULE_DESCRIPTION("Au1xxx TOY-counter-based RTC driver");
+MODULE_AUTHOR("Manuel Lauss <manuel.lauss@gmail.com>");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:rtc-au1xxx");
diff --git a/drivers/rtc/rtc-bfin.c b/drivers/rtc/rtc-bfin.c
index 34439ce3967..aafd3e6ebb0 100644
--- a/drivers/rtc/rtc-bfin.c
+++ b/drivers/rtc/rtc-bfin.c
@@ -390,7 +390,7 @@ static int __devinit bfin_rtc_probe(struct platform_device *pdev)
 
 	/* Register our RTC with the RTC framework */
 	rtc->rtc_dev = rtc_device_register(pdev->name, dev, &bfin_rtc_ops, THIS_MODULE);
-	if (unlikely(IS_ERR(rtc))) {
+	if (unlikely(IS_ERR(rtc->rtc_dev))) {
 		ret = PTR_ERR(rtc->rtc_dev);
 		goto err_irq;
 	}
diff --git a/drivers/rtc/rtc-cmos.c b/drivers/rtc/rtc-cmos.c
index 6cf8e282338..b6d35f50e40 100644
--- a/drivers/rtc/rtc-cmos.c
+++ b/drivers/rtc/rtc-cmos.c
@@ -35,6 +35,7 @@
 #include <linux/spinlock.h>
 #include <linux/platform_device.h>
 #include <linux/mod_devicetable.h>
+#include <linux/log2.h>
 
 /* this is for "generic access to PC-style RTC" using CMOS_READ/CMOS_WRITE */
 #include <asm-generic/rtc.h>
@@ -58,7 +59,7 @@ struct cmos_rtc {
 };
 
 /* both platform and pnp busses use negative numbers for invalid irqs */
-#define is_valid_irq(n)		((n) >= 0)
+#define is_valid_irq(n)		((n) > 0)
 
 static const char driver_name[] = "rtc_cmos";
 
@@ -384,6 +385,8 @@ static int cmos_irq_set_freq(struct device *dev, int freq)
 	if (!is_valid_irq(cmos->irq))
 		return -ENXIO;
 
+	if (!is_power_of_2(freq))
+		return -EINVAL;
 	/* 0 = no irqs; 1 = 2^15 Hz ... 15 = 2^0 Hz */
 	f = ffs(freq);
 	if (f-- > 16)
@@ -729,7 +732,7 @@ cmos_do_probe(struct device *dev, struct resource *ports, int rtc_irq)
 
 	cmos_rtc.dev = dev;
 	dev_set_drvdata(dev, &cmos_rtc);
-	rename_region(ports, cmos_rtc.rtc->dev.bus_id);
+	rename_region(ports, dev_name(&cmos_rtc.rtc->dev));
 
 	spin_lock_irq(&rtc_lock);
 
@@ -777,7 +780,7 @@ cmos_do_probe(struct device *dev, struct resource *ports, int rtc_irq)
 			rtc_cmos_int_handler = cmos_interrupt;
 
 		retval = request_irq(rtc_irq, rtc_cmos_int_handler,
-				IRQF_DISABLED, cmos_rtc.rtc->dev.bus_id,
+				IRQF_DISABLED, dev_name(&cmos_rtc.rtc->dev),
 				cmos_rtc.rtc);
 		if (retval < 0) {
 			dev_dbg(dev, "IRQ %d is already in use\n", rtc_irq);
@@ -795,7 +798,7 @@ cmos_do_probe(struct device *dev, struct resource *ports, int rtc_irq)
 	}
 
 	pr_info("%s: alarms up to one %s%s, %zd bytes nvram%s\n",
-			cmos_rtc.rtc->dev.bus_id,
+			dev_name(&cmos_rtc.rtc->dev),
 			is_valid_irq(rtc_irq)
 				?  (cmos_rtc.mon_alrm
 					? "year"
@@ -885,7 +888,7 @@ static int cmos_suspend(struct device *dev, pm_message_t mesg)
 	}
 
 	pr_debug("%s: suspend%s, ctrl %02x\n",
-			cmos_rtc.rtc->dev.bus_id,
+			dev_name(&cmos_rtc.rtc->dev),
 			(tmp & RTC_AIE) ? ", alarm may wake" : "",
 			tmp);
 
@@ -941,7 +944,7 @@ static int cmos_resume(struct device *dev)
 	}
 
 	pr_debug("%s: resume, ctrl %02x\n",
-			cmos_rtc.rtc->dev.bus_id,
+			dev_name(&cmos_rtc.rtc->dev),
 			tmp);
 
 	return 0;
diff --git a/drivers/rtc/rtc-ds1216.c b/drivers/rtc/rtc-ds1216.c
index 9a234a4ec06..4aedc705518 100644
--- a/drivers/rtc/rtc-ds1216.c
+++ b/drivers/rtc/rtc-ds1216.c
@@ -10,7 +10,7 @@
 #include <linux/platform_device.h>
 #include <linux/bcd.h>
 
-#define DRV_VERSION "0.1"
+#define DRV_VERSION "0.2"
 
 struct ds1216_regs {
 	u8 tsec;
@@ -101,7 +101,8 @@ static int ds1216_rtc_read_time(struct device *dev, struct rtc_time *tm)
 	tm->tm_year = bcd2bin(regs.year);
 	if (tm->tm_year < 70)
 		tm->tm_year += 100;
-	return 0;
+
+	return rtc_valid_tm(tm);
 }
 
 static int ds1216_rtc_set_time(struct device *dev, struct rtc_time *tm)
@@ -138,9 +139,8 @@ static const struct rtc_class_ops ds1216_rtc_ops = {
 	.set_time	= ds1216_rtc_set_time,
 };
 
-static int __devinit ds1216_rtc_probe(struct platform_device *pdev)
+static int __init ds1216_rtc_probe(struct platform_device *pdev)
 {
-	struct rtc_device *rtc;
 	struct resource *res;
 	struct ds1216_priv *priv;
 	int ret = 0;
@@ -152,7 +152,10 @@ static int __devinit ds1216_rtc_probe(struct platform_device *pdev)
 	priv = kzalloc(sizeof *priv, GFP_KERNEL);
 	if (!priv)
 		return -ENOMEM;
-	priv->size = res->end - res->start + 1;
+
+	platform_set_drvdata(pdev, priv);
+
+	priv->size = resource_size(res);
 	if (!request_mem_region(res->start, priv->size, pdev->name)) {
 		ret = -EBUSY;
 		goto out;
@@ -163,22 +166,18 @@ static int __devinit ds1216_rtc_probe(struct platform_device *pdev)
 		ret = -ENOMEM;
 		goto out;
 	}
-	rtc = rtc_device_register("ds1216", &pdev->dev,
+	priv->rtc = rtc_device_register("ds1216", &pdev->dev,
 				  &ds1216_rtc_ops, THIS_MODULE);
-	if (IS_ERR(rtc)) {
-		ret = PTR_ERR(rtc);
+	if (IS_ERR(priv->rtc)) {
+		ret = PTR_ERR(priv->rtc);
 		goto out;
 	}
-	priv->rtc = rtc;
-	platform_set_drvdata(pdev, priv);
 
 	/* dummy read to get clock into a known state */
 	ds1216_read(priv->ioaddr, dummy);
 	return 0;
 
 out:
-	if (priv->rtc)
-		rtc_device_unregister(priv->rtc);
 	if (priv->ioaddr)
 		iounmap(priv->ioaddr);
 	if (priv->baseaddr)
@@ -187,7 +186,7 @@ out:
 	return ret;
 }
 
-static int __devexit ds1216_rtc_remove(struct platform_device *pdev)
+static int __exit ds1216_rtc_remove(struct platform_device *pdev)
 {
 	struct ds1216_priv *priv = platform_get_drvdata(pdev);
 
@@ -203,13 +202,12 @@ static struct platform_driver ds1216_rtc_platform_driver = {
 		.name	= "rtc-ds1216",
 		.owner	= THIS_MODULE,
 	},
-	.probe		= ds1216_rtc_probe,
-	.remove		= __devexit_p(ds1216_rtc_remove),
+	.remove		= __exit_p(ds1216_rtc_remove),
 };
 
 static int __init ds1216_rtc_init(void)
 {
-	return platform_driver_register(&ds1216_rtc_platform_driver);
+	return platform_driver_probe(&ds1216_rtc_platform_driver, ds1216_rtc_probe);
 }
 
 static void __exit ds1216_rtc_exit(void)
diff --git a/drivers/rtc/rtc-ds1390.c b/drivers/rtc/rtc-ds1390.c
index 599e976bf01..e54b5c619bd 100644
--- a/drivers/rtc/rtc-ds1390.c
+++ b/drivers/rtc/rtc-ds1390.c
@@ -1,5 +1,5 @@
 /*
- * rtc-ds1390.c -- driver for DS1390/93/94
+ * rtc-ds1390.c -- driver for the Dallas/Maxim DS1390/93/94 SPI RTC
  *
  * Copyright (C) 2008 Mercury IMC Ltd
  * Written by Mark Jackson <mpfj@mimc.co.uk>
@@ -8,11 +8,13 @@
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
  *
- * NOTE : Currently this driver only supports the bare minimum for read
- * and write the RTC.  The extra features provided by the chip family
+ * NOTE: Currently this driver only supports the bare minimum for read
+ * and write the RTC. The extra features provided by the chip family
  * (alarms, trickle charger, different control registers) are unavailable.
  */
 
+#include <linux/init.h>
+#include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/rtc.h>
 #include <linux/spi/spi.h>
@@ -42,20 +44,6 @@ struct ds1390 {
 	u8 txrx_buf[9];	/* cmd + 8 registers */
 };
 
-static void ds1390_set_reg(struct device *dev, unsigned char address,
-				unsigned char data)
-{
-	struct spi_device *spi = to_spi_device(dev);
-	struct ds1390 *chip = dev_get_drvdata(dev);
-
-	/* Set MSB to indicate write */
-	chip->txrx_buf[0] = address | 0x80;
-	chip->txrx_buf[1] = data;
-
-	/* do the i/o */
-	spi_write_then_read(spi, chip->txrx_buf, 2, NULL, 0);
-}
-
 static int ds1390_get_reg(struct device *dev, unsigned char address,
 				unsigned char *data)
 {
@@ -78,7 +66,7 @@ static int ds1390_get_reg(struct device *dev, unsigned char address,
 	return 0;
 }
 
-static int ds1390_get_datetime(struct device *dev, struct rtc_time *dt)
+static int ds1390_read_time(struct device *dev, struct rtc_time *dt)
 {
 	struct spi_device *spi = to_spi_device(dev);
 	struct ds1390 *chip = dev_get_drvdata(dev);
@@ -107,7 +95,7 @@ static int ds1390_get_datetime(struct device *dev, struct rtc_time *dt)
 	return rtc_valid_tm(dt);
 }
 
-static int ds1390_set_datetime(struct device *dev, struct rtc_time *dt)
+static int ds1390_set_time(struct device *dev, struct rtc_time *dt)
 {
 	struct spi_device *spi = to_spi_device(dev);
 	struct ds1390 *chip = dev_get_drvdata(dev);
@@ -127,16 +115,6 @@ static int ds1390_set_datetime(struct device *dev, struct rtc_time *dt)
 	return spi_write_then_read(spi, chip->txrx_buf, 8, NULL, 0);
 }
 
-static int ds1390_read_time(struct device *dev, struct rtc_time *tm)
-{
-	return ds1390_get_datetime(dev, tm);
-}
-
-static int ds1390_set_time(struct device *dev, struct rtc_time *tm)
-{
-	return ds1390_set_datetime(dev, tm);
-}
-
 static const struct rtc_class_ops ds1390_rtc_ops = {
 	.read_time	= ds1390_read_time,
 	.set_time	= ds1390_set_time,
@@ -149,46 +127,40 @@ static int __devinit ds1390_probe(struct spi_device *spi)
 	struct ds1390 *chip;
 	int res;
 
-	printk(KERN_DEBUG "DS1390 SPI RTC driver\n");
-
-	rtc = rtc_device_register("ds1390",
-				&spi->dev, &ds1390_rtc_ops, THIS_MODULE);
-	if (IS_ERR(rtc)) {
-		printk(KERN_ALERT "RTC : unable to register device\n");
-		return PTR_ERR(rtc);
-	}
-
 	spi->mode = SPI_MODE_3;
 	spi->bits_per_word = 8;
 	spi_setup(spi);
 
 	chip = kzalloc(sizeof *chip, GFP_KERNEL);
 	if (!chip) {
-		printk(KERN_ALERT "RTC : unable to allocate device memory\n");
-		rtc_device_unregister(rtc);
+		dev_err(&spi->dev, "unable to allocate device memory\n");
 		return -ENOMEM;
 	}
-	chip->rtc = rtc;
 	dev_set_drvdata(&spi->dev, chip);
 
 	res = ds1390_get_reg(&spi->dev, DS1390_REG_SECONDS, &tmp);
-	if (res) {
-		printk(KERN_ALERT "RTC : unable to read device\n");
-		rtc_device_unregister(rtc);
+	if (res != 0) {
+		dev_err(&spi->dev, "unable to read device\n");
+		kfree(chip);
 		return res;
 	}
 
-	return 0;
+	chip->rtc = rtc_device_register("ds1390",
+				&spi->dev, &ds1390_rtc_ops, THIS_MODULE);
+	if (IS_ERR(chip->rtc)) {
+		dev_err(&spi->dev, "unable to register device\n");
+		res = PTR_ERR(chip->rtc);
+		kfree(chip);
+	}
+
+	return res;
 }
 
 static int __devexit ds1390_remove(struct spi_device *spi)
 {
 	struct ds1390 *chip = platform_get_drvdata(spi);
-	struct rtc_device *rtc = chip->rtc;
-
-	if (rtc)
-		rtc_device_unregister(rtc);
 
+	rtc_device_unregister(chip->rtc);
 	kfree(chip);
 
 	return 0;
@@ -215,6 +187,6 @@ static __exit void ds1390_exit(void)
 }
 module_exit(ds1390_exit);
 
-MODULE_DESCRIPTION("DS1390/93/94 SPI RTC driver");
+MODULE_DESCRIPTION("Dallas/Maxim DS1390/93/94 SPI RTC driver");
 MODULE_AUTHOR("Mark Jackson <mpfj@mimc.co.uk>");
 MODULE_LICENSE("GPL");
diff --git a/drivers/rtc/rtc-ds1511.c b/drivers/rtc/rtc-ds1511.c
index 25caada7839..23a07fe15a2 100644
--- a/drivers/rtc/rtc-ds1511.c
+++ b/drivers/rtc/rtc-ds1511.c
@@ -326,9 +326,9 @@ ds1511_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alrm)
 	struct platform_device *pdev = to_platform_device(dev);
 	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
 
-	if (pdata->irq < 0) {
+	if (pdata->irq <= 0)
 		return -EINVAL;
-	}
+
 	pdata->alrm_mday = alrm->time.tm_mday;
 	pdata->alrm_hour = alrm->time.tm_hour;
 	pdata->alrm_min = alrm->time.tm_min;
@@ -346,9 +346,9 @@ ds1511_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alrm)
 	struct platform_device *pdev = to_platform_device(dev);
 	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
 
-	if (pdata->irq < 0) {
+	if (pdata->irq <= 0)
 		return -EINVAL;
-	}
+
 	alrm->time.tm_mday = pdata->alrm_mday < 0 ? 0 : pdata->alrm_mday;
 	alrm->time.tm_hour = pdata->alrm_hour < 0 ? 0 : pdata->alrm_hour;
 	alrm->time.tm_min = pdata->alrm_min < 0 ? 0 : pdata->alrm_min;
@@ -385,7 +385,7 @@ ds1511_rtc_ioctl(struct device *dev, unsigned int cmd, unsigned long arg)
 	struct platform_device *pdev = to_platform_device(dev);
 	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
 
-	if (pdata->irq < 0) {
+	if (pdata->irq <= 0) {
 		return -ENOIOCTLCMD; /* fall back into rtc-dev's emulation */
 	}
 	switch (cmd) {
@@ -503,7 +503,6 @@ ds1511_rtc_probe(struct platform_device *pdev)
 	if (!pdata) {
 		return -ENOMEM;
 	}
-	pdata->irq = -1;
 	pdata->size = res->end - res->start + 1;
 	if (!request_mem_region(res->start, pdata->size, pdev->name)) {
 		ret = -EBUSY;
@@ -545,13 +544,13 @@ ds1511_rtc_probe(struct platform_device *pdev)
 	 * if the platform has an interrupt in mind for this device,
 	 * then by all means, set it
 	 */
-	if (pdata->irq >= 0) {
+	if (pdata->irq > 0) {
 		rtc_read(RTC_CMD1);
 		if (request_irq(pdata->irq, ds1511_interrupt,
 			IRQF_DISABLED | IRQF_SHARED, pdev->name, pdev) < 0) {
 
 			dev_warn(&pdev->dev, "interrupt not available.\n");
-			pdata->irq = -1;
+			pdata->irq = 0;
 		}
 	}
 
@@ -572,7 +571,7 @@ ds1511_rtc_probe(struct platform_device *pdev)
 	if (pdata->rtc) {
 		rtc_device_unregister(pdata->rtc);
 	}
-	if (pdata->irq >= 0) {
+	if (pdata->irq > 0) {
 		free_irq(pdata->irq, pdev);
 	}
 	if (ds1511_base) {
@@ -595,7 +594,7 @@ ds1511_rtc_remove(struct platform_device *pdev)
 	sysfs_remove_bin_file(&pdev->dev.kobj, &ds1511_nvram_attr);
 	rtc_device_unregister(pdata->rtc);
 	pdata->rtc = NULL;
-	if (pdata->irq >= 0) {
+	if (pdata->irq > 0) {
 		/*
 		 * disable the alarm interrupt
 		 */
diff --git a/drivers/rtc/rtc-ds1553.c b/drivers/rtc/rtc-ds1553.c
index b9475cd2021..38d472b6340 100644
--- a/drivers/rtc/rtc-ds1553.c
+++ b/drivers/rtc/rtc-ds1553.c
@@ -162,7 +162,7 @@ static int ds1553_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alrm)
 	struct platform_device *pdev = to_platform_device(dev);
 	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
 
-	if (pdata->irq < 0)
+	if (pdata->irq <= 0)
 		return -EINVAL;
 	pdata->alrm_mday = alrm->time.tm_mday;
 	pdata->alrm_hour = alrm->time.tm_hour;
@@ -179,7 +179,7 @@ static int ds1553_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alrm)
 	struct platform_device *pdev = to_platform_device(dev);
 	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
 
-	if (pdata->irq < 0)
+	if (pdata->irq <= 0)
 		return -EINVAL;
 	alrm->time.tm_mday = pdata->alrm_mday < 0 ? 0 : pdata->alrm_mday;
 	alrm->time.tm_hour = pdata->alrm_hour < 0 ? 0 : pdata->alrm_hour;
@@ -213,7 +213,7 @@ static int ds1553_rtc_ioctl(struct device *dev, unsigned int cmd,
 	struct platform_device *pdev = to_platform_device(dev);
 	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
 
-	if (pdata->irq < 0)
+	if (pdata->irq <= 0)
 		return -ENOIOCTLCMD; /* fall back into rtc-dev's emulation */
 	switch (cmd) {
 	case RTC_AIE_OFF:
@@ -301,7 +301,6 @@ static int __devinit ds1553_rtc_probe(struct platform_device *pdev)
 	pdata = kzalloc(sizeof(*pdata), GFP_KERNEL);
 	if (!pdata)
 		return -ENOMEM;
-	pdata->irq = -1;
 	if (!request_mem_region(res->start, RTC_REG_SIZE, pdev->name)) {
 		ret = -EBUSY;
 		goto out;
@@ -327,13 +326,13 @@ static int __devinit ds1553_rtc_probe(struct platform_device *pdev)
 	if (readb(ioaddr + RTC_FLAGS) & RTC_FLAGS_BLF)
 		dev_warn(&pdev->dev, "voltage-low detected.\n");
 
-	if (pdata->irq >= 0) {
+	if (pdata->irq > 0) {
 		writeb(0, ioaddr + RTC_INTERRUPTS);
 		if (request_irq(pdata->irq, ds1553_rtc_interrupt,
 				IRQF_DISABLED | IRQF_SHARED,
 				pdev->name, pdev) < 0) {
 			dev_warn(&pdev->dev, "interrupt not available.\n");
-			pdata->irq = -1;
+			pdata->irq = 0;
 		}
 	}
 
@@ -353,7 +352,7 @@ static int __devinit ds1553_rtc_probe(struct platform_device *pdev)
  out:
 	if (pdata->rtc)
 		rtc_device_unregister(pdata->rtc);
-	if (pdata->irq >= 0)
+	if (pdata->irq > 0)
 		free_irq(pdata->irq, pdev);
 	if (ioaddr)
 		iounmap(ioaddr);
@@ -369,7 +368,7 @@ static int __devexit ds1553_rtc_remove(struct platform_device *pdev)
 
 	sysfs_remove_bin_file(&pdev->dev.kobj, &ds1553_nvram_attr);
 	rtc_device_unregister(pdata->rtc);
-	if (pdata->irq >= 0) {
+	if (pdata->irq > 0) {
 		writeb(0, pdata->ioaddr + RTC_INTERRUPTS);
 		free_irq(pdata->irq, pdev);
 	}
diff --git a/drivers/rtc/rtc-ds1672.c b/drivers/rtc/rtc-ds1672.c
index 4e91419e891..06dfb54f99b 100644
--- a/drivers/rtc/rtc-ds1672.c
+++ b/drivers/rtc/rtc-ds1672.c
@@ -83,32 +83,11 @@ static int ds1672_set_mmss(struct i2c_client *client, unsigned long secs)
 	return 0;
 }
 
-static int ds1672_set_datetime(struct i2c_client *client, struct rtc_time *tm)
-{
-	unsigned long secs;
-
-	dev_dbg(&client->dev,
-		"%s: secs=%d, mins=%d, hours=%d, "
-		"mday=%d, mon=%d, year=%d, wday=%d\n",
-		__func__,
-		tm->tm_sec, tm->tm_min, tm->tm_hour,
-		tm->tm_mday, tm->tm_mon, tm->tm_year, tm->tm_wday);
-
-	rtc_tm_to_time(tm, &secs);
-
-	return ds1672_set_mmss(client, secs);
-}
-
 static int ds1672_rtc_read_time(struct device *dev, struct rtc_time *tm)
 {
 	return ds1672_get_datetime(to_i2c_client(dev), tm);
 }
 
-static int ds1672_rtc_set_time(struct device *dev, struct rtc_time *tm)
-{
-	return ds1672_set_datetime(to_i2c_client(dev), tm);
-}
-
 static int ds1672_rtc_set_mmss(struct device *dev, unsigned long secs)
 {
 	return ds1672_set_mmss(to_i2c_client(dev), secs);
@@ -152,7 +131,6 @@ static DEVICE_ATTR(control, S_IRUGO, show_control, NULL);
 
 static const struct rtc_class_ops ds1672_rtc_ops = {
 	.read_time = ds1672_rtc_read_time,
-	.set_time = ds1672_rtc_set_time,
 	.set_mmss = ds1672_rtc_set_mmss,
 };
 
diff --git a/drivers/rtc/rtc-ds3234.c b/drivers/rtc/rtc-ds3234.c
index 45e5b106af7..c51589ede5b 100644
--- a/drivers/rtc/rtc-ds3234.c
+++ b/drivers/rtc/rtc-ds3234.c
@@ -1,4 +1,4 @@
-/* drivers/rtc/rtc-ds3234.c
+/* rtc-ds3234.c
  *
  * Driver for Dallas Semiconductor (DS3234) SPI RTC with Integrated Crystal
  * and SRAM.
@@ -9,13 +9,10 @@
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
  *
- * Changelog:
- *
- * 07-May-2008: Dennis Aberilla <denzzzhome@yahoo.com>
- *		- Created based on the max6902 code. Only implements the
- *		  date/time keeping functions; no SRAM yet.
  */
 
+#include <linux/init.h>
+#include <linux/module.h>
 #include <linux/device.h>
 #include <linux/platform_device.h>
 #include <linux/rtc.h>
@@ -34,16 +31,7 @@
 #define DS3234_REG_CONTROL	0x0E
 #define DS3234_REG_CONT_STAT	0x0F
 
-#undef DS3234_DEBUG
-
-struct ds3234 {
-	struct rtc_device *rtc;
-	u8 buf[8]; /* Burst read: addr + 7 regs */
-	u8 tx_buf[2];
-	u8 rx_buf[2];
-};
-
-static void ds3234_set_reg(struct device *dev, unsigned char address,
+static int ds3234_set_reg(struct device *dev, unsigned char address,
 				unsigned char data)
 {
 	struct spi_device *spi = to_spi_device(dev);
@@ -53,107 +41,45 @@ static void ds3234_set_reg(struct device *dev, unsigned char address,
 	buf[0] = address | 0x80;
 	buf[1] = data;
 
-	spi_write(spi, buf, 2);
+	return spi_write_then_read(spi, buf, 2, NULL, 0);
 }
 
 static int ds3234_get_reg(struct device *dev, unsigned char address,
 				unsigned char *data)
 {
 	struct spi_device *spi = to_spi_device(dev);
-	struct ds3234 *chip = dev_get_drvdata(dev);
-	struct spi_message message;
-	struct spi_transfer xfer;
-	int status;
-
-	if (!data)
-		return -EINVAL;
-
-	/* Build our spi message */
-	spi_message_init(&message);
-	memset(&xfer, 0, sizeof(xfer));
-
-	/* Address + dummy tx byte */
-	xfer.len = 2;
-	xfer.tx_buf = chip->tx_buf;
-	xfer.rx_buf = chip->rx_buf;
-
-	chip->tx_buf[0] = address;
-	chip->tx_buf[1] = 0xff;
 
-	spi_message_add_tail(&xfer, &message);
+	*data = address & 0x7f;
 
-	/* do the i/o */
-	status = spi_sync(spi, &message);
-	if (status == 0)
-		status = message.status;
-	else
-		return status;
-
-	*data = chip->rx_buf[1];
-
-	return status;
+	return spi_write_then_read(spi, data, 1, data, 1);
 }
 
-static int ds3234_get_datetime(struct device *dev, struct rtc_time *dt)
+static int ds3234_read_time(struct device *dev, struct rtc_time *dt)
 {
+	int err;
+	unsigned char buf[8];
 	struct spi_device *spi = to_spi_device(dev);
-	struct ds3234 *chip = dev_get_drvdata(dev);
-	struct spi_message message;
-	struct spi_transfer xfer;
-	int status;
-
-	/* build the message */
-	spi_message_init(&message);
-	memset(&xfer, 0, sizeof(xfer));
-	xfer.len = 1 + 7;	/* Addr + 7 registers */
-	xfer.tx_buf = chip->buf;
-	xfer.rx_buf = chip->buf;
-	chip->buf[0] = 0x00;	/* Start address */
-	spi_message_add_tail(&xfer, &message);
-
-	/* do the i/o */
-	status = spi_sync(spi, &message);
-	if (status == 0)
-		status = message.status;
-	else
-		return status;
 
-	/* Seconds, Minutes, Hours, Day, Date, Month, Year */
-	dt->tm_sec	= bcd2bin(chip->buf[1]);
-	dt->tm_min	= bcd2bin(chip->buf[2]);
-	dt->tm_hour	= bcd2bin(chip->buf[3] & 0x3f);
-	dt->tm_wday	= bcd2bin(chip->buf[4]) - 1; /* 0 = Sun */
-	dt->tm_mday	= bcd2bin(chip->buf[5]);
-	dt->tm_mon	= bcd2bin(chip->buf[6] & 0x1f) - 1; /* 0 = Jan */
-	dt->tm_year 	= bcd2bin(chip->buf[7] & 0xff) + 100; /* Assume 20YY */
-
-#ifdef DS3234_DEBUG
-	dev_dbg(dev, "\n%s : Read RTC values\n", __func__);
-	dev_dbg(dev, "tm_hour: %i\n", dt->tm_hour);
-	dev_dbg(dev, "tm_min : %i\n", dt->tm_min);
-	dev_dbg(dev, "tm_sec : %i\n", dt->tm_sec);
-	dev_dbg(dev, "tm_wday: %i\n", dt->tm_wday);
-	dev_dbg(dev, "tm_mday: %i\n", dt->tm_mday);
-	dev_dbg(dev, "tm_mon : %i\n", dt->tm_mon);
-	dev_dbg(dev, "tm_year: %i\n", dt->tm_year);
-#endif
+	buf[0] = 0x00; /* Start address */
 
-	return 0;
+	err = spi_write_then_read(spi, buf, 1, buf, 8);
+	if (err != 0)
+		return err;
+
+	/* Seconds, Minutes, Hours, Day, Date, Month, Year */
+	dt->tm_sec	= bcd2bin(buf[0]);
+	dt->tm_min	= bcd2bin(buf[1]);
+	dt->tm_hour	= bcd2bin(buf[2] & 0x3f);
+	dt->tm_wday	= bcd2bin(buf[3]) - 1; /* 0 = Sun */
+	dt->tm_mday	= bcd2bin(buf[4]);
+	dt->tm_mon	= bcd2bin(buf[5] & 0x1f) - 1; /* 0 = Jan */
+	dt->tm_year 	= bcd2bin(buf[6] & 0xff) + 100; /* Assume 20YY */
+
+	return rtc_valid_tm(dt);
 }
 
-static int ds3234_set_datetime(struct device *dev, struct rtc_time *dt)
+static int ds3234_set_time(struct device *dev, struct rtc_time *dt)
 {
-#ifdef DS3234_DEBUG
-	dev_dbg(dev, "\n%s : Setting RTC values\n", __func__);
-	dev_dbg(dev, "tm_sec : %i\n", dt->tm_sec);
-	dev_dbg(dev, "tm_min : %i\n", dt->tm_min);
-	dev_dbg(dev, "tm_hour: %i\n", dt->tm_hour);
-	dev_dbg(dev, "tm_wday: %i\n", dt->tm_wday);
-	dev_dbg(dev, "tm_mday: %i\n", dt->tm_mday);
-	dev_dbg(dev, "tm_mon : %i\n", dt->tm_mon);
-	dev_dbg(dev, "tm_year: %i\n", dt->tm_year);
-#endif
-
 	ds3234_set_reg(dev, DS3234_REG_SECONDS, bin2bcd(dt->tm_sec));
 	ds3234_set_reg(dev, DS3234_REG_MINUTES, bin2bcd(dt->tm_min));
 	ds3234_set_reg(dev, DS3234_REG_HOURS, bin2bcd(dt->tm_hour) & 0x3f);
@@ -174,16 +100,6 @@ static int ds3234_set_datetime(struct device *dev, struct rtc_time *dt)
 	return 0;
 }
 
-static int ds3234_read_time(struct device *dev, struct rtc_time *tm)
-{
-	return ds3234_get_datetime(dev, tm);
-}
-
-static int ds3234_set_time(struct device *dev, struct rtc_time *tm)
-{
-	return ds3234_set_datetime(dev, tm);
-}
-
 static const struct rtc_class_ops ds3234_rtc_ops = {
 	.read_time	= ds3234_read_time,
 	.set_time	= ds3234_set_time,
@@ -193,31 +109,15 @@ static int __devinit ds3234_probe(struct spi_device *spi)
 {
 	struct rtc_device *rtc;
 	unsigned char tmp;
-	struct ds3234 *chip;
 	int res;
 
-	rtc = rtc_device_register("ds3234",
-				&spi->dev, &ds3234_rtc_ops, THIS_MODULE);
-	if (IS_ERR(rtc))
-		return PTR_ERR(rtc);
-
 	spi->mode = SPI_MODE_3;
 	spi->bits_per_word = 8;
 	spi_setup(spi);
 
-	chip = kzalloc(sizeof(struct ds3234), GFP_KERNEL);
-	if (!chip) {
-		rtc_device_unregister(rtc);
-		return -ENOMEM;
-	}
-	chip->rtc = rtc;
-	dev_set_drvdata(&spi->dev, chip);
-
 	res = ds3234_get_reg(&spi->dev, DS3234_REG_SECONDS, &tmp);
-	if (res) {
-		rtc_device_unregister(rtc);
+	if (res != 0)
 		return res;
-	}
 
 	/* Control settings
 	 *
@@ -246,26 +146,27 @@ static int __devinit ds3234_probe(struct spi_device *spi)
 	ds3234_get_reg(&spi->dev, DS3234_REG_CONT_STAT, &tmp);
 	dev_info(&spi->dev, "Ctrl/Stat Reg: 0x%02x\n", tmp);
 
+	rtc = rtc_device_register("ds3234",
+				&spi->dev, &ds3234_rtc_ops, THIS_MODULE);
+	if (IS_ERR(rtc))
+		return PTR_ERR(rtc);
+
+	dev_set_drvdata(&spi->dev, rtc);
+
 	return 0;
 }
 
 static int __devexit ds3234_remove(struct spi_device *spi)
 {
-	struct ds3234 *chip = platform_get_drvdata(spi);
-	struct rtc_device *rtc = chip->rtc;
-
-	if (rtc)
-		rtc_device_unregister(rtc);
-
-	kfree(chip);
+	struct rtc_device *rtc = platform_get_drvdata(spi);
 
+	rtc_device_unregister(rtc);
 	return 0;
 }
 
 static struct spi_driver ds3234_driver = {
 	.driver = {
 		.name	 = "ds3234",
-		.bus	= &spi_bus_type,
 		.owner	= THIS_MODULE,
 	},
 	.probe	 = ds3234_probe,
@@ -274,7 +175,6 @@ static struct spi_driver ds3234_driver = {
 
 static __init int ds3234_init(void)
 {
-	printk(KERN_INFO "DS3234 SPI RTC Driver\n");
 	return spi_register_driver(&ds3234_driver);
 }
 module_init(ds3234_init);
diff --git a/drivers/rtc/rtc-ep93xx.c b/drivers/rtc/rtc-ep93xx.c
index 36e4ac0bd69..f7a3283dd02 100644
--- a/drivers/rtc/rtc-ep93xx.c
+++ b/drivers/rtc/rtc-ep93xx.c
@@ -49,18 +49,6 @@ static int ep93xx_rtc_set_mmss(struct device *dev, unsigned long secs)
 	return 0;
 }
 
-static int ep93xx_rtc_set_time(struct device *dev, struct rtc_time *tm)
-{
-	int err;
-	unsigned long secs;
-
-	err = rtc_tm_to_time(tm, &secs);
-	if (err != 0)
-		return err;
-
-	return ep93xx_rtc_set_mmss(dev, secs);
-}
-
 static int ep93xx_rtc_proc(struct device *dev, struct seq_file *seq)
 {
 	unsigned short preload, delete;
@@ -75,7 +63,6 @@ static int ep93xx_rtc_proc(struct device *dev, struct seq_file *seq)
 
 static const struct rtc_class_ops ep93xx_rtc_ops = {
 	.read_time	= ep93xx_rtc_read_time,
-	.set_time	= ep93xx_rtc_set_time,
 	.set_mmss	= ep93xx_rtc_set_mmss,
 	.proc		= ep93xx_rtc_proc,
 };
diff --git a/drivers/rtc/rtc-m48t59.c b/drivers/rtc/rtc-m48t59.c
index 43afb7ab528..33921a6b170 100644
--- a/drivers/rtc/rtc-m48t59.c
+++ b/drivers/rtc/rtc-m48t59.c
@@ -450,7 +450,7 @@ static int __devinit m48t59_rtc_probe(struct platform_device *pdev)
 	 * the mode without IRQ.
 	 */
 	m48t59->irq = platform_get_irq(pdev, 0);
-	if (m48t59->irq < 0)
+	if (m48t59->irq <= 0)
 		m48t59->irq = NO_IRQ;
 
 	if (m48t59->irq != NO_IRQ) {
diff --git a/drivers/rtc/rtc-max6902.c b/drivers/rtc/rtc-max6902.c
index 2f6507df7b4..36a8ea9ed8b 100644
--- a/drivers/rtc/rtc-max6902.c
+++ b/drivers/rtc/rtc-max6902.c
@@ -9,14 +9,6 @@
  *
  * Driver for MAX6902 spi RTC
  *
- * Changelog:
- *
- * 24-May-2006: Raphael Assenat <raph@8d.com>
- *                - Major rework
- *				Converted to rtc_device and uses the SPI layer.
- *
- * ??-???-2005: Someone at Compulab
- *                - Initial driver creation.
  */
 
 #include <linux/module.h>
@@ -26,7 +18,6 @@
 #include <linux/rtc.h>
 #include <linux/spi/spi.h>
 #include <linux/bcd.h>
-#include <linux/delay.h>
 
 #define MAX6902_REG_SECONDS		0x01
 #define MAX6902_REG_MINUTES		0x03
@@ -38,16 +29,7 @@
 #define MAX6902_REG_CONTROL		0x0F
 #define MAX6902_REG_CENTURY		0x13
 
-#undef MAX6902_DEBUG
-
-struct max6902 {
-	struct rtc_device *rtc;
-	u8 buf[9]; /* Burst read cmd + 8 registers */
-	u8 tx_buf[2];
-	u8 rx_buf[2];
-};
-
-static void max6902_set_reg(struct device *dev, unsigned char address,
+static int max6902_set_reg(struct device *dev, unsigned char address,
 				unsigned char data)
 {
 	struct spi_device *spi = to_spi_device(dev);
@@ -57,113 +39,58 @@ static void max6902_set_reg(struct device *dev, unsigned char address,
 	buf[0] = address & 0x7f;
 	buf[1] = data;
 
-	spi_write(spi, buf, 2);
+	return spi_write_then_read(spi, buf, 2, NULL, 0);
 }
 
 static int max6902_get_reg(struct device *dev, unsigned char address,
 				unsigned char *data)
 {
 	struct spi_device *spi = to_spi_device(dev);
-	struct max6902 *chip = dev_get_drvdata(dev);
-	struct spi_message message;
-	struct spi_transfer xfer;
-	int status;
-
-	if (!data)
-		return -EINVAL;
-
-	/* Build our spi message */
-	spi_message_init(&message);
-	memset(&xfer, 0, sizeof(xfer));
-	xfer.len = 2;
-	/* Can tx_buf and rx_buf be equal? The doc in spi.h is not sure... */
-	xfer.tx_buf = chip->tx_buf;
-	xfer.rx_buf = chip->rx_buf;
 
 	/* Set MSB to indicate read */
-	chip->tx_buf[0] = address | 0x80;
-
-	spi_message_add_tail(&xfer, &message);
+	*data = address | 0x80;
 
-	/* do the i/o */
-	status = spi_sync(spi, &message);
-
-	if (status == 0)
-		*data = chip->rx_buf[1];
-	return status;
+	return spi_write_then_read(spi, data, 1, data, 1);
 }
 
-static int max6902_get_datetime(struct device *dev, struct rtc_time *dt)
+static int max6902_read_time(struct device *dev, struct rtc_time *dt)
 {
-	unsigned char tmp;
-	int century;
-	int err;
+	int err, century;
 	struct spi_device *spi = to_spi_device(dev);
-	struct max6902 *chip = dev_get_drvdata(dev);
-	struct spi_message message;
-	struct spi_transfer xfer;
-	int status;
+	unsigned char buf[8];
 
-	err = max6902_get_reg(dev, MAX6902_REG_CENTURY, &tmp);
-	if (err)
-		return err;
-
-	/* build the message */
-	spi_message_init(&message);
-	memset(&xfer, 0, sizeof(xfer));
-	xfer.len = 1 + 7;	/* Burst read command + 7 registers */
-	xfer.tx_buf = chip->buf;
-	xfer.rx_buf = chip->buf;
-	chip->buf[0] = 0xbf;	/* Burst read */
-	spi_message_add_tail(&xfer, &message);
+	buf[0] = 0xbf;	/* Burst read */
 
-	/* do the i/o */
-	status = spi_sync(spi, &message);
-	if (status)
-		return status;
+	err = spi_write_then_read(spi, buf, 1, buf, 8);
+	if (err != 0)
+		return err;
 
 	/* The chip sends data in this order:
 	 * Seconds, Minutes, Hours, Date, Month, Day, Year */
-	dt->tm_sec	= bcd2bin(chip->buf[1]);
-	dt->tm_min	= bcd2bin(chip->buf[2]);
-	dt->tm_hour	= bcd2bin(chip->buf[3]);
-	dt->tm_mday	= bcd2bin(chip->buf[4]);
-	dt->tm_mon	= bcd2bin(chip->buf[5]) - 1;
-	dt->tm_wday	= bcd2bin(chip->buf[6]);
-	dt->tm_year = bcd2bin(chip->buf[7]);
+	dt->tm_sec	= bcd2bin(buf[0]);
+	dt->tm_min	= bcd2bin(buf[1]);
+	dt->tm_hour	= bcd2bin(buf[2]);
+	dt->tm_mday	= bcd2bin(buf[3]);
+	dt->tm_mon	= bcd2bin(buf[4]) - 1;
+	dt->tm_wday	= bcd2bin(buf[5]);
+	dt->tm_year	= bcd2bin(buf[6]);
+
+	/* Read century */
+	err = max6902_get_reg(dev, MAX6902_REG_CENTURY, &buf[0]);
+	if (err != 0)
+		return err;
 
-	century = bcd2bin(tmp) * 100;
+	century = bcd2bin(buf[0]) * 100;
 
 	dt->tm_year += century;
 	dt->tm_year -= 1900;
 
-#ifdef MAX6902_DEBUG
-	printk("\n%s : Read RTC values\n",__func__);
-	printk("tm_hour: %i\n",dt->tm_hour);
-	printk("tm_min : %i\n",dt->tm_min);
-	printk("tm_sec : %i\n",dt->tm_sec);
-	printk("tm_year: %i\n",dt->tm_year);
-	printk("tm_mon : %i\n",dt->tm_mon);
-	printk("tm_mday: %i\n",dt->tm_mday);
-	printk("tm_wday: %i\n",dt->tm_wday);
-#endif
-
-	return 0;
+	return rtc_valid_tm(dt);
 }
 
-static int max6902_set_datetime(struct device *dev, struct rtc_time *dt)
+static int max6902_set_time(struct device *dev, struct rtc_time *dt)
 {
-	dt->tm_year = dt->tm_year+1900;
-
-#ifdef MAX6902_DEBUG
-	printk("\n%s : Setting RTC values\n",__func__);
-	printk("tm_sec : %i\n",dt->tm_sec);
-	printk("tm_min : %i\n",dt->tm_min);
-	printk("tm_hour: %i\n",dt->tm_hour);
-	printk("tm_mday: %i\n",dt->tm_mday);
-	printk("tm_wday: %i\n",dt->tm_wday);
-	printk("tm_year: %i\n",dt->tm_year);
-#endif
+	dt->tm_year = dt->tm_year + 1900;
 
 	/* Remove write protection */
 	max6902_set_reg(dev, 0xF, 0);
@@ -173,10 +100,10 @@ static int max6902_set_datetime(struct device *dev, struct rtc_time *dt)
 	max6902_set_reg(dev, 0x05, bin2bcd(dt->tm_hour));
 
 	max6902_set_reg(dev, 0x07, bin2bcd(dt->tm_mday));
-	max6902_set_reg(dev, 0x09, bin2bcd(dt->tm_mon+1));
+	max6902_set_reg(dev, 0x09, bin2bcd(dt->tm_mon + 1));
 	max6902_set_reg(dev, 0x0B, bin2bcd(dt->tm_wday));
-	max6902_set_reg(dev, 0x0D, bin2bcd(dt->tm_year%100));
-	max6902_set_reg(dev, 0x13, bin2bcd(dt->tm_year/100));
+	max6902_set_reg(dev, 0x0D, bin2bcd(dt->tm_year % 100));
+	max6902_set_reg(dev, 0x13, bin2bcd(dt->tm_year / 100));
 
 	/* Compulab used a delay here. However, the datasheet
 	 * does not mention a delay being required anywhere... */
@@ -188,16 +115,6 @@ static int max6902_set_datetime(struct device *dev, struct rtc_time *dt)
 	return 0;
 }
 
-static int max6902_read_time(struct device *dev, struct rtc_time *tm)
-{
-	return max6902_get_datetime(dev, tm);
-}
-
-static int max6902_set_time(struct device *dev, struct rtc_time *tm)
-{
-	return max6902_set_datetime(dev, tm);
-}
-
 static const struct rtc_class_ops max6902_rtc_ops = {
 	.read_time	= max6902_read_time,
 	.set_time	= max6902_set_time,
@@ -207,45 +124,29 @@ static int __devinit max6902_probe(struct spi_device *spi)
 {
 	struct rtc_device *rtc;
 	unsigned char tmp;
-	struct max6902 *chip;
 	int res;
 
-	rtc = rtc_device_register("max6902",
-				&spi->dev, &max6902_rtc_ops, THIS_MODULE);
-	if (IS_ERR(rtc))
-		return PTR_ERR(rtc);
-
 	spi->mode = SPI_MODE_3;
 	spi->bits_per_word = 8;
 	spi_setup(spi);
 
-	chip = kzalloc(sizeof *chip, GFP_KERNEL);
-	if (!chip) {
-		rtc_device_unregister(rtc);
-		return -ENOMEM;
-	}
-	chip->rtc = rtc;
-	dev_set_drvdata(&spi->dev, chip);
-
 	res = max6902_get_reg(&spi->dev, MAX6902_REG_SECONDS, &tmp);
-	if (res) {
-		rtc_device_unregister(rtc);
+	if (res != 0)
 		return res;
-	}
+
+	rtc = rtc_device_register("max6902",
+				&spi->dev, &max6902_rtc_ops, THIS_MODULE);
+	if (IS_ERR(rtc))
+		return PTR_ERR(rtc);
 
 	return 0;
 }
 
 static int __devexit max6902_remove(struct spi_device *spi)
 {
-	struct max6902 *chip = platform_get_drvdata(spi);
-	struct rtc_device *rtc = chip->rtc;
-
-	if (rtc)
-		rtc_device_unregister(rtc);
-
-	kfree(chip);
+	struct rtc_device *rtc = platform_get_drvdata(spi);
 
+	rtc_device_unregister(rtc);
 	return 0;
 }
 
@@ -261,7 +162,6 @@ static struct spi_driver max6902_driver = {
 
 static __init int max6902_init(void)
 {
-	printk("max6902 spi driver\n");
 	return spi_register_driver(&max6902_driver);
 }
 module_init(max6902_init);
diff --git a/drivers/rtc/rtc-mv.c b/drivers/rtc/rtc-mv.c
new file mode 100644
index 00000000000..45f12dcd371
--- /dev/null
+++ b/drivers/rtc/rtc-mv.c
@@ -0,0 +1,163 @@
+/*
+ * Driver for the RTC in Marvell SoCs.
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2.  This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/rtc.h>
+#include <linux/bcd.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+
+
+#define RTC_TIME_REG_OFFS	0
+#define RTC_SECONDS_OFFS	0
+#define RTC_MINUTES_OFFS	8
+#define RTC_HOURS_OFFS		16
+#define RTC_WDAY_OFFS		24
+#define RTC_HOURS_12H_MODE		(1 << 22) /* 12 hours mode */
+
+#define RTC_DATE_REG_OFFS	4
+#define RTC_MDAY_OFFS		0
+#define RTC_MONTH_OFFS		8
+#define RTC_YEAR_OFFS		16
+
+
+struct rtc_plat_data {
+	struct rtc_device *rtc;
+	void __iomem *ioaddr;
+};
+
+static int mv_rtc_set_time(struct device *dev, struct rtc_time *tm)
+{
+	struct rtc_plat_data *pdata = dev_get_drvdata(dev);
+	void __iomem *ioaddr = pdata->ioaddr;
+	u32	rtc_reg;
+
+	rtc_reg = (bin2bcd(tm->tm_sec) << RTC_SECONDS_OFFS) |
+		(bin2bcd(tm->tm_min) << RTC_MINUTES_OFFS) |
+		(bin2bcd(tm->tm_hour) << RTC_HOURS_OFFS) |
+		(bin2bcd(tm->tm_wday) << RTC_WDAY_OFFS);
+	writel(rtc_reg, ioaddr + RTC_TIME_REG_OFFS);
+
+	rtc_reg = (bin2bcd(tm->tm_mday) << RTC_MDAY_OFFS) |
+		(bin2bcd(tm->tm_mon + 1) << RTC_MONTH_OFFS) |
+		(bin2bcd(tm->tm_year % 100) << RTC_YEAR_OFFS);
+	writel(rtc_reg, ioaddr + RTC_DATE_REG_OFFS);
+
+	return 0;
+}
+
+static int mv_rtc_read_time(struct device *dev, struct rtc_time *tm)
+{
+	struct rtc_plat_data *pdata = dev_get_drvdata(dev);
+	void __iomem *ioaddr = pdata->ioaddr;
+	u32	rtc_time, rtc_date;
+	unsigned int year, month, day, hour, minute, second, wday;
+
+	rtc_time = readl(ioaddr + RTC_TIME_REG_OFFS);
+	rtc_date = readl(ioaddr + RTC_DATE_REG_OFFS);
+
+	second = rtc_time & 0x7f;
+	minute = (rtc_time >> RTC_MINUTES_OFFS) & 0x7f;
+	hour = (rtc_time >> RTC_HOURS_OFFS) & 0x3f; /* assume 24 hours mode */
+	wday = (rtc_time >> RTC_WDAY_OFFS) & 0x7;
+
+	day = rtc_date & 0x3f;
+	month = (rtc_date >> RTC_MONTH_OFFS) & 0x3f;
+	year = (rtc_date >> RTC_YEAR_OFFS) & 0xff;
+
+	tm->tm_sec = bcd2bin(second);
+	tm->tm_min = bcd2bin(minute);
+	tm->tm_hour = bcd2bin(hour);
+	tm->tm_mday = bcd2bin(day);
+	tm->tm_wday = bcd2bin(wday);
+	tm->tm_mon = bcd2bin(month) - 1;
+	/* hw counts from year 2000, but tm_year is relative to 1900 */
+	tm->tm_year = bcd2bin(year) + 100;
+
+	return rtc_valid_tm(tm);
+}
+
+static const struct rtc_class_ops mv_rtc_ops = {
+	.read_time	= mv_rtc_read_time,
+	.set_time	= mv_rtc_set_time,
+};
+
+static int __init mv_rtc_probe(struct platform_device *pdev)
+{
+	struct resource *res;
+	struct rtc_plat_data *pdata;
+	resource_size_t size;
+	u32 rtc_time;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -ENODEV;
+
+	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return -ENOMEM;
+
+	size = resource_size(res);
+	if (!devm_request_mem_region(&pdev->dev, res->start, size,
+				     pdev->name))
+		return -EBUSY;
+
+	pdata->ioaddr = devm_ioremap(&pdev->dev, res->start, size);
+	if (!pdata->ioaddr)
+		return -ENOMEM;
+
+	/* make sure the 24 hours mode is enabled */
+	rtc_time = readl(pdata->ioaddr + RTC_TIME_REG_OFFS);
+	if (rtc_time & RTC_HOURS_12H_MODE) {
+		dev_err(&pdev->dev, "24 Hours mode not supported.\n");
+		return -EINVAL;
+	}
+
+	platform_set_drvdata(pdev, pdata);
+	pdata->rtc = rtc_device_register(pdev->name, &pdev->dev,
+					 &mv_rtc_ops, THIS_MODULE);
+	if (IS_ERR(pdata->rtc))
+		return PTR_ERR(pdata->rtc);
+
+	return 0;
+}
+
+static int __exit mv_rtc_remove(struct platform_device *pdev)
+{
+	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
+
+	rtc_device_unregister(pdata->rtc);
+	return 0;
+}
+
+static struct platform_driver mv_rtc_driver = {
+	.remove		= __exit_p(mv_rtc_remove),
+	.driver		= {
+		.name	= "rtc-mv",
+		.owner	= THIS_MODULE,
+	},
+};
+
+static __init int mv_init(void)
+{
+	return platform_driver_probe(&mv_rtc_driver, mv_rtc_probe);
+}
+
+static __exit void mv_exit(void)
+{
+	platform_driver_unregister(&mv_rtc_driver);
+}
+
+module_init(mv_init);
+module_exit(mv_exit);
+
+MODULE_AUTHOR("Saeed Bishara <saeed@marvell.com>");
+MODULE_DESCRIPTION("Marvell RTC driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:rtc-mv");
diff --git a/drivers/rtc/rtc-pxa.c b/drivers/rtc/rtc-pxa.c
new file mode 100644
index 00000000000..cc7eb8767b8
--- /dev/null
+++ b/drivers/rtc/rtc-pxa.c
@@ -0,0 +1,489 @@
+/*
+ * Real Time Clock interface for XScale PXA27x and PXA3xx
+ *
+ * Copyright (C) 2008 Robert Jarzmik
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/module.h>
+#include <linux/rtc.h>
+#include <linux/seq_file.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+
+#define TIMER_FREQ		CLOCK_TICK_RATE
+#define RTC_DEF_DIVIDER		(32768 - 1)
+#define RTC_DEF_TRIM		0
+#define MAXFREQ_PERIODIC	1000
+
+/*
+ * PXA Registers and bits definitions
+ */
+#define RTSR_PICE	(1 << 15)	/* Periodic interrupt count enable */
+#define RTSR_PIALE	(1 << 14)	/* Periodic interrupt Alarm enable */
+#define RTSR_PIAL	(1 << 13)	/* Periodic interrupt detected */
+#define RTSR_SWALE2	(1 << 11)	/* RTC stopwatch alarm2 enable */
+#define RTSR_SWAL2	(1 << 10)	/* RTC stopwatch alarm2 detected */
+#define RTSR_SWALE1	(1 << 9)	/* RTC stopwatch alarm1 enable */
+#define RTSR_SWAL1	(1 << 8)	/* RTC stopwatch alarm1 detected */
+#define RTSR_RDALE2	(1 << 7)	/* RTC alarm2 enable */
+#define RTSR_RDAL2	(1 << 6)	/* RTC alarm2 detected */
+#define RTSR_RDALE1	(1 << 5)	/* RTC alarm1 enable */
+#define RTSR_RDAL1	(1 << 4)	/* RTC alarm1 detected */
+#define RTSR_HZE	(1 << 3)	/* HZ interrupt enable */
+#define RTSR_ALE	(1 << 2)	/* RTC alarm interrupt enable */
+#define RTSR_HZ		(1 << 1)	/* HZ rising-edge detected */
+#define RTSR_AL		(1 << 0)	/* RTC alarm detected */
+#define RTSR_TRIG_MASK	(RTSR_AL | RTSR_HZ | RTSR_RDAL1 | RTSR_RDAL2\
+			 | RTSR_SWAL1 | RTSR_SWAL2)
+#define RYxR_YEAR_S	9
+#define RYxR_YEAR_MASK	(0xfff << RYxR_YEAR_S)
+#define RYxR_MONTH_S	5
+#define RYxR_MONTH_MASK	(0xf << RYxR_MONTH_S)
+#define RYxR_DAY_MASK	0x1f
+#define RDxR_HOUR_S	12
+#define RDxR_HOUR_MASK	(0x1f << RDxR_HOUR_S)
+#define RDxR_MIN_S	6
+#define RDxR_MIN_MASK	(0x3f << RDxR_MIN_S)
+#define RDxR_SEC_MASK	0x3f
+
+#define RTSR		0x08
+#define RTTR		0x0c
+#define RDCR		0x10
+#define RYCR		0x14
+#define RDAR1		0x18
+#define RYAR1		0x1c
+#define RTCPICR		0x34
+#define PIAR		0x38
+
+#define rtc_readl(pxa_rtc, reg)	\
+	__raw_readl((pxa_rtc)->base + (reg))
+#define rtc_writel(pxa_rtc, reg, value)	\
+	__raw_writel((value), (pxa_rtc)->base + (reg))
+
+struct pxa_rtc {
+	struct resource	*ress;
+	void __iomem		*base;
+	int			irq_1Hz;
+	int			irq_Alrm;
+	struct rtc_device	*rtc;
+	spinlock_t		lock;		/* Protects this structure */
+	struct rtc_time		rtc_alarm;
+};
+
+static u32 ryxr_calc(struct rtc_time *tm)
+{
+	return ((tm->tm_year + 1900) << RYxR_YEAR_S)
+		| ((tm->tm_mon + 1) << RYxR_MONTH_S)
+		| tm->tm_mday;
+}
+
+static u32 rdxr_calc(struct rtc_time *tm)
+{
+	return (tm->tm_hour << RDxR_HOUR_S) | (tm->tm_min << RDxR_MIN_S)
+		| tm->tm_sec;
+}
+
+static void tm_calc(u32 rycr, u32 rdcr, struct rtc_time *tm)
+{
+	tm->tm_year = ((rycr & RYxR_YEAR_MASK) >> RYxR_YEAR_S) - 1900;
+	tm->tm_mon = (((rycr & RYxR_MONTH_MASK) >> RYxR_MONTH_S)) - 1;
+	tm->tm_mday = (rycr & RYxR_DAY_MASK);
+	tm->tm_hour = (rdcr & RDxR_HOUR_MASK) >> RDxR_HOUR_S;
+	tm->tm_min = (rdcr & RDxR_MIN_MASK) >> RDxR_MIN_S;
+	tm->tm_sec = rdcr & RDxR_SEC_MASK;
+}
+
+static void rtsr_clear_bits(struct pxa_rtc *pxa_rtc, u32 mask)
+{
+	u32 rtsr;
+
+	rtsr = rtc_readl(pxa_rtc, RTSR);
+	rtsr &= ~RTSR_TRIG_MASK;
+	rtsr &= ~mask;
+	rtc_writel(pxa_rtc, RTSR, rtsr);
+}
+
+static void rtsr_set_bits(struct pxa_rtc *pxa_rtc, u32 mask)
+{
+	u32 rtsr;
+
+	rtsr = rtc_readl(pxa_rtc, RTSR);
+	rtsr &= ~RTSR_TRIG_MASK;
+	rtsr |= mask;
+	rtc_writel(pxa_rtc, RTSR, rtsr);
+}
+
+static irqreturn_t pxa_rtc_irq(int irq, void *dev_id)
+{
+	struct platform_device *pdev = to_platform_device(dev_id);
+	struct pxa_rtc *pxa_rtc = platform_get_drvdata(pdev);
+	u32 rtsr;
+	unsigned long events = 0;
+
+	spin_lock(&pxa_rtc->lock);
+
+	/* clear interrupt sources */
+	rtsr = rtc_readl(pxa_rtc, RTSR);
+	rtc_writel(pxa_rtc, RTSR, rtsr);
+
+	/* temporary disable rtc interrupts */
+	rtsr_clear_bits(pxa_rtc, RTSR_RDALE1 | RTSR_PIALE | RTSR_HZE);
+
+	/* clear alarm interrupt if it has occurred */
+	if (rtsr & RTSR_RDAL1)
+		rtsr &= ~RTSR_RDALE1;
+
+	/* update irq data & counter */
+	if (rtsr & RTSR_RDAL1)
+		events |= RTC_AF | RTC_IRQF;
+	if (rtsr & RTSR_HZ)
+		events |= RTC_UF | RTC_IRQF;
+	if (rtsr & RTSR_PIAL)
+		events |= RTC_PF | RTC_IRQF;
+
+	rtc_update_irq(pxa_rtc->rtc, 1, events);
+
+	/* enable back rtc interrupts */
+	rtc_writel(pxa_rtc, RTSR, rtsr & ~RTSR_TRIG_MASK);
+
+	spin_unlock(&pxa_rtc->lock);
+	return IRQ_HANDLED;
+}
+
+static int pxa_rtc_open(struct device *dev)
+{
+	struct pxa_rtc *pxa_rtc = dev_get_drvdata(dev);
+	int ret;
+
+	ret = request_irq(pxa_rtc->irq_1Hz, pxa_rtc_irq, IRQF_DISABLED,
+			  "rtc 1Hz", dev);
+	if (ret < 0) {
+		dev_err(dev, "can't get irq %i, err %d\n", pxa_rtc->irq_1Hz,
+			ret);
+		goto err_irq_1Hz;
+	}
+	ret = request_irq(pxa_rtc->irq_Alrm, pxa_rtc_irq, IRQF_DISABLED,
+			  "rtc Alrm", dev);
+	if (ret < 0) {
+		dev_err(dev, "can't get irq %i, err %d\n", pxa_rtc->irq_Alrm,
+			ret);
+		goto err_irq_Alrm;
+	}
+
+	return 0;
+
+err_irq_Alrm:
+	free_irq(pxa_rtc->irq_1Hz, dev);
+err_irq_1Hz:
+	return ret;
+}
+
+static void pxa_rtc_release(struct device *dev)
+{
+	struct pxa_rtc *pxa_rtc = dev_get_drvdata(dev);
+
+	spin_lock_irq(&pxa_rtc->lock);
+	rtsr_clear_bits(pxa_rtc, RTSR_PIALE | RTSR_RDALE1 | RTSR_HZE);
+	spin_unlock_irq(&pxa_rtc->lock);
+
+	free_irq(pxa_rtc->irq_Alrm, dev);
+	free_irq(pxa_rtc->irq_1Hz, dev);
+}
+
+static int pxa_periodic_irq_set_freq(struct device *dev, int freq)
+{
+	struct pxa_rtc *pxa_rtc = dev_get_drvdata(dev);
+	int period_ms;
+
+	if (freq < 1 || freq > MAXFREQ_PERIODIC)
+		return -EINVAL;
+
+	period_ms = 1000 / freq;
+	rtc_writel(pxa_rtc, PIAR, period_ms);
+
+	return 0;
+}
+
+static int pxa_periodic_irq_set_state(struct device *dev, int enabled)
+{
+	struct pxa_rtc *pxa_rtc = dev_get_drvdata(dev);
+
+	if (enabled)
+		rtsr_set_bits(pxa_rtc, RTSR_PIALE | RTSR_PICE);
+	else
+		rtsr_clear_bits(pxa_rtc, RTSR_PIALE | RTSR_PICE);
+
+	return 0;
+}
+
+static int pxa_rtc_ioctl(struct device *dev, unsigned int cmd,
+		unsigned long arg)
+{
+	struct pxa_rtc *pxa_rtc = dev_get_drvdata(dev);
+	int ret = 0;
+
+	spin_lock_irq(&pxa_rtc->lock);
+	switch (cmd) {
+	case RTC_AIE_OFF:
+		rtsr_clear_bits(pxa_rtc, RTSR_RDALE1);
+		break;
+	case RTC_AIE_ON:
+		rtsr_set_bits(pxa_rtc, RTSR_RDALE1);
+		break;
+	case RTC_UIE_OFF:
+		rtsr_clear_bits(pxa_rtc, RTSR_HZE);
+		break;
+	case RTC_UIE_ON:
+		rtsr_set_bits(pxa_rtc, RTSR_HZE);
+		break;
+	default:
+		ret = -ENOIOCTLCMD;
+	}
+
+	spin_unlock_irq(&pxa_rtc->lock);
+	return ret;
+}
+
+static int pxa_rtc_read_time(struct device *dev, struct rtc_time *tm)
+{
+	struct pxa_rtc *pxa_rtc = dev_get_drvdata(dev);
+	u32 rycr, rdcr;
+
+	rycr = rtc_readl(pxa_rtc, RYCR);
+	rdcr = rtc_readl(pxa_rtc, RDCR);
+
+	tm_calc(rycr, rdcr, tm);
+	return 0;
+}
+
+static int pxa_rtc_set_time(struct device *dev, struct rtc_time *tm)
+{
+	struct pxa_rtc *pxa_rtc = dev_get_drvdata(dev);
+
+	rtc_writel(pxa_rtc, RYCR, ryxr_calc(tm));
+	rtc_writel(pxa_rtc, RDCR, rdxr_calc(tm));
+
+	return 0;
+}
+
+static int pxa_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alrm)
+{
+	struct pxa_rtc *pxa_rtc = dev_get_drvdata(dev);
+	u32 rtsr, ryar, rdar;
+
+	ryar = rtc_readl(pxa_rtc, RYAR1);
+	rdar = rtc_readl(pxa_rtc, RDAR1);
+	tm_calc(ryar, rdar, &alrm->time);
+
+	rtsr = rtc_readl(pxa_rtc, RTSR);
+	alrm->enabled = (rtsr & RTSR_RDALE1) ? 1 : 0;
+	alrm->pending = (rtsr & RTSR_RDAL1) ? 1 : 0;
+	return 0;
+}
+
+static int pxa_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alrm)
+{
+	struct pxa_rtc *pxa_rtc = dev_get_drvdata(dev);
+	u32 rtsr;
+
+	spin_lock_irq(&pxa_rtc->lock);
+
+	rtc_writel(pxa_rtc, RYAR1, ryxr_calc(&alrm->time));
+	rtc_writel(pxa_rtc, RDAR1, rdxr_calc(&alrm->time));
+
+	rtsr = rtc_readl(pxa_rtc, RTSR);
+	if (alrm->enabled)
+		rtsr |= RTSR_RDALE1;
+	else
+		rtsr &= ~RTSR_RDALE1;
+	rtc_writel(pxa_rtc, RTSR, rtsr);
+
+	spin_unlock_irq(&pxa_rtc->lock);
+
+	return 0;
+}
+
+static int pxa_rtc_proc(struct device *dev, struct seq_file *seq)
+{
+	struct pxa_rtc *pxa_rtc = dev_get_drvdata(dev);
+
+	seq_printf(seq, "trim/divider\t: 0x%08x\n", rtc_readl(pxa_rtc, RTTR));
+	seq_printf(seq, "update_IRQ\t: %s\n",
+		   (rtc_readl(pxa_rtc, RTSR) & RTSR_HZE) ? "yes" : "no");
+	seq_printf(seq, "periodic_IRQ\t: %s\n",
+		   (rtc_readl(pxa_rtc, RTSR) & RTSR_PIALE) ? "yes" : "no");
+	seq_printf(seq, "periodic_freq\t: %u\n", rtc_readl(pxa_rtc, PIAR));
+
+	return 0;
+}
+
+static const struct rtc_class_ops pxa_rtc_ops = {
+	.open = pxa_rtc_open,
+	.release = pxa_rtc_release,
+	.ioctl = pxa_rtc_ioctl,
+	.read_time = pxa_rtc_read_time,
+	.set_time = pxa_rtc_set_time,
+	.read_alarm = pxa_rtc_read_alarm,
+	.set_alarm = pxa_rtc_set_alarm,
+	.proc = pxa_rtc_proc,
+	.irq_set_state = pxa_periodic_irq_set_state,
+	.irq_set_freq = pxa_periodic_irq_set_freq,
+};
+
+static int __init pxa_rtc_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct pxa_rtc *pxa_rtc;
+	int ret;
+	u32 rttr;
+
+	pxa_rtc = kzalloc(sizeof(struct pxa_rtc), GFP_KERNEL);
+	if (!pxa_rtc)
+		return -ENOMEM;
+
+	spin_lock_init(&pxa_rtc->lock);
+	platform_set_drvdata(pdev, pxa_rtc);
+
+	ret = -ENXIO;
+	pxa_rtc->ress = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!pxa_rtc->ress) {
+		dev_err(dev, "No I/O memory resource defined\n");
+		goto err_ress;
+	}
+
+	pxa_rtc->irq_1Hz = platform_get_irq(pdev, 0);
+	if (pxa_rtc->irq_1Hz < 0) {
+		dev_err(dev, "No 1Hz IRQ resource defined\n");
+		goto err_ress;
+	}
+	pxa_rtc->irq_Alrm = platform_get_irq(pdev, 1);
+	if (pxa_rtc->irq_Alrm < 0) {
+		dev_err(dev, "No alarm IRQ resource defined\n");
+		goto err_ress;
+	}
+
+	ret = -ENOMEM;
+	pxa_rtc->base = ioremap(pxa_rtc->ress->start,
+				resource_size(pxa_rtc->ress));
+	if (!pxa_rtc->base) {
+		dev_err(&pdev->dev, "Unable to map pxa RTC I/O memory\n");
+		goto err_map;
+	}
+
+	/*
+	 * If the clock divider is uninitialized then reset it to the
+	 * default value to get the 1Hz clock.
+	 */
+	if (rtc_readl(pxa_rtc, RTTR) == 0) {
+		rttr = RTC_DEF_DIVIDER + (RTC_DEF_TRIM << 16);
+		rtc_writel(pxa_rtc, RTTR, rttr);
+		dev_warn(dev, "warning: initializing default clock"
+			 " divider/trim value\n");
+	}
+
+	rtsr_clear_bits(pxa_rtc, RTSR_PIALE | RTSR_RDALE1 | RTSR_HZE);
+
+	pxa_rtc->rtc = rtc_device_register("pxa-rtc", &pdev->dev, &pxa_rtc_ops,
+					   THIS_MODULE);
+	ret = PTR_ERR(pxa_rtc->rtc);
+	if (IS_ERR(pxa_rtc->rtc)) {
+		dev_err(dev, "Failed to register RTC device -> %d\n", ret);
+		goto err_rtc_reg;
+	}
+
+	device_init_wakeup(dev, 1);
+
+	return 0;
+
+err_rtc_reg:
+	 iounmap(pxa_rtc->base);
+err_ress:
+err_map:
+	kfree(pxa_rtc);
+	return ret;
+}
+
+static int __exit pxa_rtc_remove(struct platform_device *pdev)
+{
+	struct pxa_rtc *pxa_rtc = platform_get_drvdata(pdev);
+
+	rtc_device_unregister(pxa_rtc->rtc);
+
+	spin_lock_irq(&pxa_rtc->lock);
+	iounmap(pxa_rtc->base);
+	spin_unlock_irq(&pxa_rtc->lock);
+
+	kfree(pxa_rtc);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int pxa_rtc_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct pxa_rtc *pxa_rtc = platform_get_drvdata(pdev);
+
+	if (device_may_wakeup(&pdev->dev))
+		enable_irq_wake(pxa_rtc->irq_Alrm);
+	return 0;
+}
+
+static int pxa_rtc_resume(struct platform_device *pdev)
+{
+	struct pxa_rtc *pxa_rtc = platform_get_drvdata(pdev);
+
+	if (device_may_wakeup(&pdev->dev))
+		disable_irq_wake(pxa_rtc->irq_Alrm);
+	return 0;
+}
+#else
+#define pxa_rtc_suspend	NULL
+#define pxa_rtc_resume	NULL
+#endif
+
+static struct platform_driver pxa_rtc_driver = {
+	.remove		= __exit_p(pxa_rtc_remove),
+	.suspend	= pxa_rtc_suspend,
+	.resume		= pxa_rtc_resume,
+	.driver		= {
+		.name		= "pxa-rtc",
+	},
+};
+
+static int __init pxa_rtc_init(void)
+{
+	if (cpu_is_pxa27x() || cpu_is_pxa3xx())
+		return platform_driver_probe(&pxa_rtc_driver, pxa_rtc_probe);
+
+	return -ENODEV;
+}
+
+static void __exit pxa_rtc_exit(void)
+{
+	platform_driver_unregister(&pxa_rtc_driver);
+}
+
+module_init(pxa_rtc_init);
+module_exit(pxa_rtc_exit);
+
+MODULE_AUTHOR("Robert Jarzmik");
+MODULE_DESCRIPTION("PXA27x/PXA3xx Realtime Clock Driver (RTC)");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:pxa-rtc");
diff --git a/drivers/rtc/rtc-s3c.c b/drivers/rtc/rtc-s3c.c
index 7a568beba3f..e0d7b999150 100644
--- a/drivers/rtc/rtc-s3c.c
+++ b/drivers/rtc/rtc-s3c.c
@@ -94,6 +94,9 @@ static int s3c_rtc_setfreq(struct device *dev, int freq)
 {
 	unsigned int tmp;
 
+	if (!is_power_of_2(freq))
+		return -EINVAL;
+
 	spin_lock_irq(&s3c_rtc_pie_lock);
 
 	tmp = readb(s3c_rtc_base + S3C2410_TICNT) & S3C2410_TICNT_ENABLE;
diff --git a/drivers/rtc/rtc-sh.c b/drivers/rtc/rtc-sh.c
index aaf9d6a337c..1c3fc6b428e 100644
--- a/drivers/rtc/rtc-sh.c
+++ b/drivers/rtc/rtc-sh.c
@@ -24,6 +24,7 @@
 #include <linux/interrupt.h>
 #include <linux/spinlock.h>
 #include <linux/io.h>
+#include <linux/log2.h>
 #include <asm/rtc.h>
 
 #define DRV_NAME	"sh-rtc"
@@ -89,7 +90,9 @@ struct sh_rtc {
 	void __iomem *regbase;
 	unsigned long regsize;
 	struct resource *res;
-	unsigned int alarm_irq, periodic_irq, carry_irq;
+	int alarm_irq;
+	int periodic_irq;
+	int carry_irq;
 	struct rtc_device *rtc_dev;
 	spinlock_t lock;
 	unsigned long capabilities;	/* See asm-sh/rtc.h for cap bits */
@@ -549,6 +552,8 @@ static int sh_rtc_irq_set_state(struct device *dev, int enabled)
 
 static int sh_rtc_irq_set_freq(struct device *dev, int freq)
 {
+	if (!is_power_of_2(freq))
+		return -EINVAL;
 	return sh_rtc_ioctl(dev, RTC_IRQP_SET, freq);
 }
 
@@ -578,7 +583,7 @@ static int __devinit sh_rtc_probe(struct platform_device *pdev)
 
 	/* get periodic/carry/alarm irqs */
 	ret = platform_get_irq(pdev, 0);
-	if (unlikely(ret < 0)) {
+	if (unlikely(ret <= 0)) {
 		ret = -ENOENT;
 		dev_err(&pdev->dev, "No IRQ for period\n");
 		goto err_badres;
@@ -586,7 +591,7 @@ static int __devinit sh_rtc_probe(struct platform_device *pdev)
 	rtc->periodic_irq = ret;
 
 	ret = platform_get_irq(pdev, 1);
-	if (unlikely(ret < 0)) {
+	if (unlikely(ret <= 0)) {
 		ret = -ENOENT;
 		dev_err(&pdev->dev, "No IRQ for carry\n");
 		goto err_badres;
@@ -594,7 +599,7 @@ static int __devinit sh_rtc_probe(struct platform_device *pdev)
 	rtc->carry_irq = ret;
 
 	ret = platform_get_irq(pdev, 2);
-	if (unlikely(ret < 0)) {
+	if (unlikely(ret <= 0)) {
 		ret = -ENOENT;
 		dev_err(&pdev->dev, "No IRQ for alarm\n");
 		goto err_badres;
diff --git a/drivers/rtc/rtc-stk17ta8.c b/drivers/rtc/rtc-stk17ta8.c
index f4cd46e15af..dc0b6224ad9 100644
--- a/drivers/rtc/rtc-stk17ta8.c
+++ b/drivers/rtc/rtc-stk17ta8.c
@@ -170,7 +170,7 @@ static int stk17ta8_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alrm)
 	struct platform_device *pdev = to_platform_device(dev);
 	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
 
-	if (pdata->irq < 0)
+	if (pdata->irq <= 0)
 		return -EINVAL;
 	pdata->alrm_mday = alrm->time.tm_mday;
 	pdata->alrm_hour = alrm->time.tm_hour;
@@ -187,7 +187,7 @@ static int stk17ta8_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alrm)
 	struct platform_device *pdev = to_platform_device(dev);
 	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
 
-	if (pdata->irq < 0)
+	if (pdata->irq <= 0)
 		return -EINVAL;
 	alrm->time.tm_mday = pdata->alrm_mday < 0 ? 0 : pdata->alrm_mday;
 	alrm->time.tm_hour = pdata->alrm_hour < 0 ? 0 : pdata->alrm_hour;
@@ -221,7 +221,7 @@ static int stk17ta8_rtc_ioctl(struct device *dev, unsigned int cmd,
 	struct platform_device *pdev = to_platform_device(dev);
 	struct rtc_plat_data *pdata = platform_get_drvdata(pdev);
 
-	if (pdata->irq < 0)
+	if (pdata->irq <= 0)
 		return -ENOIOCTLCMD; /* fall back into rtc-dev's emulation */
 	switch (cmd) {
 	case RTC_AIE_OFF:
@@ -303,7 +303,6 @@ static int __init stk17ta8_rtc_probe(struct platform_device *pdev)
 	pdata = kzalloc(sizeof(*pdata), GFP_KERNEL);
 	if (!pdata)
 		return -ENOMEM;
-	pdata->irq = -1;
 	if (!request_mem_region(res->start, RTC_REG_SIZE, pdev->name)) {
 		ret = -EBUSY;
 		goto out;
@@ -329,13 +328,13 @@ static int __init stk17ta8_rtc_probe(struct platform_device *pdev)
 	if (readb(ioaddr + RTC_FLAGS) & RTC_FLAGS_PF)
 		dev_warn(&pdev->dev, "voltage-low detected.\n");
 
-	if (pdata->irq >= 0) {
+	if (pdata->irq > 0) {
 		writeb(0, ioaddr + RTC_INTERRUPTS);
 		if (request_irq(pdata->irq, stk17ta8_rtc_interrupt,
 				IRQF_DISABLED | IRQF_SHARED,
 				pdev->name, pdev) < 0) {
 			dev_warn(&pdev->dev, "interrupt not available.\n");
-			pdata->irq = -1;
+			pdata->irq = 0;
 		}
 	}
 
@@ -355,7 +354,7 @@ static int __init stk17ta8_rtc_probe(struct platform_device *pdev)
  out:
 	if (pdata->rtc)
 		rtc_device_unregister(pdata->rtc);
-	if (pdata->irq >= 0)
+	if (pdata->irq > 0)
 		free_irq(pdata->irq, pdev);
 	if (ioaddr)
 		iounmap(ioaddr);
@@ -371,7 +370,7 @@ static int __devexit stk17ta8_rtc_remove(struct platform_device *pdev)
 
 	sysfs_remove_bin_file(&pdev->dev.kobj, &stk17ta8_nvram_attr);
 	rtc_device_unregister(pdata->rtc);
-	if (pdata->irq >= 0) {
+	if (pdata->irq > 0) {
 		writeb(0, pdata->ioaddr + RTC_INTERRUPTS);
 		free_irq(pdata->irq, pdev);
 	}
diff --git a/drivers/rtc/rtc-test.c b/drivers/rtc/rtc-test.c
index bc930022004..e478280ff62 100644
--- a/drivers/rtc/rtc-test.c
+++ b/drivers/rtc/rtc-test.c
@@ -34,14 +34,9 @@ static int test_rtc_read_time(struct device *dev,
 	return 0;
 }
 
-static int test_rtc_set_time(struct device *dev,
-	struct rtc_time *tm)
-{
-	return 0;
-}
-
 static int test_rtc_set_mmss(struct device *dev, unsigned long secs)
 {
+	dev_info(dev, "%s, secs = %lu\n", __func__, secs);
 	return 0;
 }
 
@@ -78,7 +73,6 @@ static int test_rtc_ioctl(struct device *dev, unsigned int cmd,
 static const struct rtc_class_ops test_rtc_ops = {
 	.proc = test_rtc_proc,
 	.read_time = test_rtc_read_time,
-	.set_time = test_rtc_set_time,
 	.read_alarm = test_rtc_read_alarm,
 	.set_alarm = test_rtc_set_alarm,
 	.set_mmss = test_rtc_set_mmss,
diff --git a/drivers/rtc/rtc-twl4030.c b/drivers/rtc/rtc-twl4030.c
index 01d8da9afdc..8ce5f74ee45 100644
--- a/drivers/rtc/rtc-twl4030.c
+++ b/drivers/rtc/rtc-twl4030.c
@@ -19,6 +19,7 @@
  */
 
 #include <linux/kernel.h>
+#include <linux/errno.h>
 #include <linux/init.h>
 #include <linux/module.h>
 #include <linux/types.h>
@@ -415,8 +416,8 @@ static int __devinit twl4030_rtc_probe(struct platform_device *pdev)
 	int irq = platform_get_irq(pdev, 0);
 	u8 rd_reg;
 
-	if (irq < 0)
-		return irq;
+	if (irq <= 0)
+		return -EINVAL;
 
 	rtc = rtc_device_register(pdev->name,
 				  &pdev->dev, &twl4030_rtc_ops, THIS_MODULE);
diff --git a/drivers/rtc/rtc-tx4939.c b/drivers/rtc/rtc-tx4939.c
new file mode 100644
index 00000000000..4ee4857ff20
--- /dev/null
+++ b/drivers/rtc/rtc-tx4939.c
@@ -0,0 +1,317 @@
+/*
+ * TX4939 internal RTC driver
+ * Based on RBTX49xx patch from CELF patch archive.
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.  See the file "COPYING" in the main directory of this archive
+ * for more details.
+ *
+ * (C) Copyright TOSHIBA CORPORATION 2005-2007
+ */
+#include <linux/rtc.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <asm/txx9/tx4939.h>
+
+struct tx4939rtc_plat_data {
+	struct rtc_device *rtc;
+	struct tx4939_rtc_reg __iomem *rtcreg;
+};
+
+static struct tx4939rtc_plat_data *get_tx4939rtc_plat_data(struct device *dev)
+{
+	return platform_get_drvdata(to_platform_device(dev));
+}
+
+static int tx4939_rtc_cmd(struct tx4939_rtc_reg __iomem *rtcreg, int cmd)
+{
+	int i = 0;
+
+	__raw_writel(cmd, &rtcreg->ctl);
+	/* This might take 30us (next 32.768KHz clock) */
+	while (__raw_readl(&rtcreg->ctl) & TX4939_RTCCTL_BUSY) {
+		/* timeout on approx. 100us (@ GBUS200MHz) */
+		if (i++ > 200 * 100)
+			return -EBUSY;
+		cpu_relax();
+	}
+	return 0;
+}
+
+static int tx4939_rtc_set_mmss(struct device *dev, unsigned long secs)
+{
+	struct tx4939rtc_plat_data *pdata = get_tx4939rtc_plat_data(dev);
+	struct tx4939_rtc_reg __iomem *rtcreg = pdata->rtcreg;
+	int i, ret;
+	unsigned char buf[6];
+
+	buf[0] = 0;
+	buf[1] = 0;
+	buf[2] = secs;
+	buf[3] = secs >> 8;
+	buf[4] = secs >> 16;
+	buf[5] = secs >> 24;
+	spin_lock_irq(&pdata->rtc->irq_lock);
+	__raw_writel(0, &rtcreg->adr);
+	for (i = 0; i < 6; i++)
+		__raw_writel(buf[i], &rtcreg->dat);
+	ret = tx4939_rtc_cmd(rtcreg,
+			     TX4939_RTCCTL_COMMAND_SETTIME |
+			     (__raw_readl(&rtcreg->ctl) & TX4939_RTCCTL_ALME));
+	spin_unlock_irq(&pdata->rtc->irq_lock);
+	return ret;
+}
+
+static int tx4939_rtc_read_time(struct device *dev, struct rtc_time *tm)
+{
+	struct tx4939rtc_plat_data *pdata = get_tx4939rtc_plat_data(dev);
+	struct tx4939_rtc_reg __iomem *rtcreg = pdata->rtcreg;
+	int i, ret;
+	unsigned long sec;
+	unsigned char buf[6];
+
+	spin_lock_irq(&pdata->rtc->irq_lock);
+	ret = tx4939_rtc_cmd(rtcreg,
+			     TX4939_RTCCTL_COMMAND_GETTIME |
+			     (__raw_readl(&rtcreg->ctl) & TX4939_RTCCTL_ALME));
+	if (ret) {
+		spin_unlock_irq(&pdata->rtc->irq_lock);
+		return ret;
+	}
+	__raw_writel(2, &rtcreg->adr);
+	for (i = 2; i < 6; i++)
+		buf[i] = __raw_readl(&rtcreg->dat);
+	spin_unlock_irq(&pdata->rtc->irq_lock);
+	sec = (buf[5] << 24) | (buf[4] << 16) | (buf[3] << 8) | buf[2];
+	rtc_time_to_tm(sec, tm);
+	return rtc_valid_tm(tm);
+}
+
+static int tx4939_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alrm)
+{
+	struct tx4939rtc_plat_data *pdata = get_tx4939rtc_plat_data(dev);
+	struct tx4939_rtc_reg __iomem *rtcreg = pdata->rtcreg;
+	int i, ret;
+	unsigned long sec;
+	unsigned char buf[6];
+
+	if (alrm->time.tm_sec < 0 ||
+	    alrm->time.tm_min < 0 ||
+	    alrm->time.tm_hour < 0 ||
+	    alrm->time.tm_mday < 0 ||
+	    alrm->time.tm_mon < 0 ||
+	    alrm->time.tm_year < 0)
+		return -EINVAL;
+	rtc_tm_to_time(&alrm->time, &sec);
+	buf[0] = 0;
+	buf[1] = 0;
+	buf[2] = sec;
+	buf[3] = sec >> 8;
+	buf[4] = sec >> 16;
+	buf[5] = sec >> 24;
+	spin_lock_irq(&pdata->rtc->irq_lock);
+	__raw_writel(0, &rtcreg->adr);
+	for (i = 0; i < 6; i++)
+		__raw_writel(buf[i], &rtcreg->dat);
+	ret = tx4939_rtc_cmd(rtcreg, TX4939_RTCCTL_COMMAND_SETALARM |
+			     (alrm->enabled ? TX4939_RTCCTL_ALME : 0));
+	spin_unlock_irq(&pdata->rtc->irq_lock);
+	return ret;
+}
+
+static int tx4939_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alrm)
+{
+	struct tx4939rtc_plat_data *pdata = get_tx4939rtc_plat_data(dev);
+	struct tx4939_rtc_reg __iomem *rtcreg = pdata->rtcreg;
+	int i, ret;
+	unsigned long sec;
+	unsigned char buf[6];
+	u32 ctl;
+
+	spin_lock_irq(&pdata->rtc->irq_lock);
+	ret = tx4939_rtc_cmd(rtcreg,
+			     TX4939_RTCCTL_COMMAND_GETALARM |
+			     (__raw_readl(&rtcreg->ctl) & TX4939_RTCCTL_ALME));
+	if (ret) {
+		spin_unlock_irq(&pdata->rtc->irq_lock);
+		return ret;
+	}
+	__raw_writel(2, &rtcreg->adr);
+	for (i = 2; i < 6; i++)
+		buf[i] = __raw_readl(&rtcreg->dat);
+	ctl = __raw_readl(&rtcreg->ctl);
+	alrm->enabled = (ctl & TX4939_RTCCTL_ALME) ? 1 : 0;
+	alrm->pending = (ctl & TX4939_RTCCTL_ALMD) ? 1 : 0;
+	spin_unlock_irq(&pdata->rtc->irq_lock);
+	sec = (buf[5] << 24) | (buf[4] << 16) | (buf[3] << 8) | buf[2];
+	rtc_time_to_tm(sec, &alrm->time);
+	return rtc_valid_tm(&alrm->time);
+}
+
+static int tx4939_rtc_alarm_irq_enable(struct device *dev, unsigned int enabled)
+{
+	struct tx4939rtc_plat_data *pdata = get_tx4939rtc_plat_data(dev);
+
+	spin_lock_irq(&pdata->rtc->irq_lock);
+	tx4939_rtc_cmd(pdata->rtcreg,
+		       TX4939_RTCCTL_COMMAND_NOP |
+		       (enabled ? TX4939_RTCCTL_ALME : 0));
+	spin_unlock_irq(&pdata->rtc->irq_lock);
+	return 0;
+}
+
+static irqreturn_t tx4939_rtc_interrupt(int irq, void *dev_id)
+{
+	struct tx4939rtc_plat_data *pdata = get_tx4939rtc_plat_data(dev_id);
+	struct tx4939_rtc_reg __iomem *rtcreg = pdata->rtcreg;
+	unsigned long events = RTC_IRQF;
+
+	spin_lock(&pdata->rtc->irq_lock);
+	if (__raw_readl(&rtcreg->ctl) & TX4939_RTCCTL_ALMD) {
+		events |= RTC_AF;
+		tx4939_rtc_cmd(rtcreg, TX4939_RTCCTL_COMMAND_NOP);
+	}
+	spin_unlock(&pdata->rtc->irq_lock);
+	rtc_update_irq(pdata->rtc, 1, events);
+	return IRQ_HANDLED;
+}
+
+static const struct rtc_class_ops tx4939_rtc_ops = {
+	.read_time		= tx4939_rtc_read_time,
+	.read_alarm		= tx4939_rtc_read_alarm,
+	.set_alarm		= tx4939_rtc_set_alarm,
+	.set_mmss		= tx4939_rtc_set_mmss,
+	.alarm_irq_enable	= tx4939_rtc_alarm_irq_enable,
+};
+
+static ssize_t tx4939_rtc_nvram_read(struct kobject *kobj,
+				     struct bin_attribute *bin_attr,
+				     char *buf, loff_t pos, size_t size)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct tx4939rtc_plat_data *pdata = get_tx4939rtc_plat_data(dev);
+	struct tx4939_rtc_reg __iomem *rtcreg = pdata->rtcreg;
+	ssize_t count;
+
+	spin_lock_irq(&pdata->rtc->irq_lock);
+	for (count = 0; size > 0 && pos < TX4939_RTC_REG_RAMSIZE;
+	     count++, size--) {
+		__raw_writel(pos++, &rtcreg->adr);
+		*buf++ = __raw_readl(&rtcreg->dat);
+	}
+	spin_unlock_irq(&pdata->rtc->irq_lock);
+	return count;
+}
+
+static ssize_t tx4939_rtc_nvram_write(struct kobject *kobj,
+				      struct bin_attribute *bin_attr,
+				      char *buf, loff_t pos, size_t size)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct tx4939rtc_plat_data *pdata = get_tx4939rtc_plat_data(dev);
+	struct tx4939_rtc_reg __iomem *rtcreg = pdata->rtcreg;
+	ssize_t count;
+
+	spin_lock_irq(&pdata->rtc->irq_lock);
+	for (count = 0; size > 0 && pos < TX4939_RTC_REG_RAMSIZE;
+	     count++, size--) {
+		__raw_writel(pos++, &rtcreg->adr);
+		__raw_writel(*buf++, &rtcreg->dat);
+	}
+	spin_unlock_irq(&pdata->rtc->irq_lock);
+	return count;
+}
+
+static struct bin_attribute tx4939_rtc_nvram_attr = {
+	.attr = {
+		.name = "nvram",
+		.mode = S_IRUGO | S_IWUSR,
+	},
+	.size = TX4939_RTC_REG_RAMSIZE,
+	.read = tx4939_rtc_nvram_read,
+	.write = tx4939_rtc_nvram_write,
+};
+
+static int __init tx4939_rtc_probe(struct platform_device *pdev)
+{
+	struct rtc_device *rtc;
+	struct tx4939rtc_plat_data *pdata;
+	struct resource *res;
+	int irq, ret;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -ENODEV;
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0)
+		return -ENODEV;
+	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return -ENOMEM;
+	platform_set_drvdata(pdev, pdata);
+
+	if (!devm_request_mem_region(&pdev->dev, res->start,
+				     resource_size(res), pdev->name))
+		return -EBUSY;
+	pdata->rtcreg = devm_ioremap(&pdev->dev, res->start,
+				     resource_size(res));
+	if (!pdata->rtcreg)
+		return -EBUSY;
+
+	tx4939_rtc_cmd(pdata->rtcreg, TX4939_RTCCTL_COMMAND_NOP);
+	if (devm_request_irq(&pdev->dev, irq, tx4939_rtc_interrupt,
+			     IRQF_DISABLED | IRQF_SHARED,
+			     pdev->name, &pdev->dev) < 0) {
+		return -EBUSY;
+	}
+	rtc = rtc_device_register(pdev->name, &pdev->dev,
+				  &tx4939_rtc_ops, THIS_MODULE);
+	if (IS_ERR(rtc))
+		return PTR_ERR(rtc);
+	pdata->rtc = rtc;
+	ret = sysfs_create_bin_file(&pdev->dev.kobj, &tx4939_rtc_nvram_attr);
+	if (ret)
+		rtc_device_unregister(rtc);
+	return ret;
+}
+
+static int __exit tx4939_rtc_remove(struct platform_device *pdev)
+{
+	struct tx4939rtc_plat_data *pdata = platform_get_drvdata(pdev);
+	struct rtc_device *rtc = pdata->rtc;
+
+	spin_lock_irq(&rtc->irq_lock);
+	tx4939_rtc_cmd(pdata->rtcreg, TX4939_RTCCTL_COMMAND_NOP);
+	spin_unlock_irq(&rtc->irq_lock);
+	sysfs_remove_bin_file(&pdev->dev.kobj, &tx4939_rtc_nvram_attr);
+	rtc_device_unregister(rtc);
+	platform_set_drvdata(pdev, NULL);
+	return 0;
+}
+
+static struct platform_driver tx4939_rtc_driver = {
+	.remove		= __exit_p(tx4939_rtc_remove),
+	.driver		= {
+		.name	= "tx4939rtc",
+		.owner	= THIS_MODULE,
+	},
+};
+
+static int __init tx4939rtc_init(void)
+{
+	return platform_driver_probe(&tx4939_rtc_driver, tx4939_rtc_probe);
+}
+
+static void __exit tx4939rtc_exit(void)
+{
+	platform_driver_unregister(&tx4939_rtc_driver);
+}
+
+module_init(tx4939rtc_init);
+module_exit(tx4939rtc_exit);
+
+MODULE_AUTHOR("Atsushi Nemoto <anemo@mba.ocn.ne.jp>");
+MODULE_DESCRIPTION("TX4939 internal RTC driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:tx4939rtc");
diff --git a/drivers/rtc/rtc-vr41xx.c b/drivers/rtc/rtc-vr41xx.c
index 834dcc6d785..f11297aff85 100644
--- a/drivers/rtc/rtc-vr41xx.c
+++ b/drivers/rtc/rtc-vr41xx.c
@@ -27,6 +27,7 @@
 #include <linux/rtc.h>
 #include <linux/spinlock.h>
 #include <linux/types.h>
+#include <linux/log2.h>
 
 #include <asm/div64.h>
 #include <asm/io.h>
@@ -84,8 +85,8 @@ static DEFINE_SPINLOCK(rtc_lock);
 static char rtc_name[] = "RTC";
 static unsigned long periodic_count;
 static unsigned int alarm_enabled;
-static int aie_irq = -1;
-static int pie_irq = -1;
+static int aie_irq;
+static int pie_irq;
 
 static inline unsigned long read_elapsed_second(void)
 {
@@ -210,6 +211,8 @@ static int vr41xx_rtc_irq_set_freq(struct device *dev, int freq)
 {
 	unsigned long count;
 
+	if (!is_power_of_2(freq))
+		return -EINVAL;
 	count = RTC_FREQUENCY;
 	do_div(count, freq);
 
@@ -360,7 +363,7 @@ static int __devinit rtc_probe(struct platform_device *pdev)
 	spin_unlock_irq(&rtc_lock);
 
 	aie_irq = platform_get_irq(pdev, 0);
-	if (aie_irq < 0 || aie_irq >= nr_irqs) {
+	if (aie_irq <= 0) {
 		retval = -EBUSY;
 		goto err_device_unregister;
 	}
@@ -371,7 +374,7 @@ static int __devinit rtc_probe(struct platform_device *pdev)
 		goto err_device_unregister;
 
 	pie_irq = platform_get_irq(pdev, 1);
-	if (pie_irq < 0 || pie_irq >= nr_irqs)
+	if (pie_irq <= 0)
 		goto err_free_irq;
 
 	retval = request_irq(pie_irq, rtclong1_interrupt, IRQF_DISABLED,
diff --git a/drivers/s390/Makefile b/drivers/s390/Makefile
index 4f4e7cf105d..d0eae59bc36 100644
--- a/drivers/s390/Makefile
+++ b/drivers/s390/Makefile
@@ -4,7 +4,7 @@
 
 CFLAGS_sysinfo.o += -Iinclude/math-emu -Iarch/s390/math-emu -w
 
-obj-y += s390mach.o sysinfo.o s390_rdev.o
+obj-y += s390mach.o sysinfo.o
 obj-y += cio/ block/ char/ crypto/ net/ scsi/ kvm/
 
 drivers-y += drivers/s390/built-in.o
diff --git a/drivers/s390/block/dcssblk.c b/drivers/s390/block/dcssblk.c
index 26ffc6ab441..cfdcf1aed33 100644
--- a/drivers/s390/block/dcssblk.c
+++ b/drivers/s390/block/dcssblk.c
@@ -18,7 +18,6 @@
 #include <asm/io.h>
 #include <linux/completion.h>
 #include <linux/interrupt.h>
-#include <asm/s390_rdev.h>
 
 #define DCSSBLK_NAME "dcssblk"
 #define DCSSBLK_MINORS_PER_DISK 1
@@ -946,7 +945,7 @@ dcssblk_check_params(void)
 static void __exit
 dcssblk_exit(void)
 {
-	s390_root_dev_unregister(dcssblk_root_dev);
+	root_device_unregister(dcssblk_root_dev);
 	unregister_blkdev(dcssblk_major, DCSSBLK_NAME);
 }
 
@@ -955,22 +954,22 @@ dcssblk_init(void)
 {
 	int rc;
 
-	dcssblk_root_dev = s390_root_dev_register("dcssblk");
+	dcssblk_root_dev = root_device_register("dcssblk");
 	if (IS_ERR(dcssblk_root_dev))
 		return PTR_ERR(dcssblk_root_dev);
 	rc = device_create_file(dcssblk_root_dev, &dev_attr_add);
 	if (rc) {
-		s390_root_dev_unregister(dcssblk_root_dev);
+		root_device_unregister(dcssblk_root_dev);
 		return rc;
 	}
 	rc = device_create_file(dcssblk_root_dev, &dev_attr_remove);
 	if (rc) {
-		s390_root_dev_unregister(dcssblk_root_dev);
+		root_device_unregister(dcssblk_root_dev);
 		return rc;
 	}
 	rc = register_blkdev(0, DCSSBLK_NAME);
 	if (rc < 0) {
-		s390_root_dev_unregister(dcssblk_root_dev);
+		root_device_unregister(dcssblk_root_dev);
 		return rc;
 	}
 	dcssblk_major = rc;
diff --git a/drivers/s390/crypto/ap_bus.c b/drivers/s390/crypto/ap_bus.c
index 1f5f5d2d87d..9c148406b98 100644
--- a/drivers/s390/crypto/ap_bus.c
+++ b/drivers/s390/crypto/ap_bus.c
@@ -36,7 +36,6 @@
 #include <linux/notifier.h>
 #include <linux/kthread.h>
 #include <linux/mutex.h>
-#include <asm/s390_rdev.h>
 #include <asm/reset.h>
 #include <asm/airq.h>
 #include <asm/atomic.h>
@@ -1522,7 +1521,7 @@ int __init ap_module_init(void)
 	}
 
 	/* Create /sys/devices/ap. */
-	ap_root_device = s390_root_dev_register("ap");
+	ap_root_device = root_device_register("ap");
 	rc = IS_ERR(ap_root_device) ? PTR_ERR(ap_root_device) : 0;
 	if (rc)
 		goto out_bus;
@@ -1565,7 +1564,7 @@ out_work:
 	hrtimer_cancel(&ap_poll_timer);
 	destroy_workqueue(ap_work_queue);
 out_root:
-	s390_root_dev_unregister(ap_root_device);
+	root_device_unregister(ap_root_device);
 out_bus:
 	while (i--)
 		bus_remove_file(&ap_bus_type, ap_bus_attrs[i]);
@@ -1600,7 +1599,7 @@ void ap_module_exit(void)
 	hrtimer_cancel(&ap_poll_timer);
 	destroy_workqueue(ap_work_queue);
 	tasklet_kill(&ap_tasklet);
-	s390_root_dev_unregister(ap_root_device);
+	root_device_unregister(ap_root_device);
 	while ((dev = bus_find_device(&ap_bus_type, NULL, NULL,
 		    __ap_match_all)))
 	{
diff --git a/drivers/s390/kvm/kvm_virtio.c b/drivers/s390/kvm/kvm_virtio.c
index 28c90b89f2b..cbc8566fab7 100644
--- a/drivers/s390/kvm/kvm_virtio.c
+++ b/drivers/s390/kvm/kvm_virtio.c
@@ -24,7 +24,6 @@
 #include <asm/kvm_virtio.h>
 #include <asm/setup.h>
 #include <asm/s390_ext.h>
-#include <asm/s390_rdev.h>
 
 #define VIRTIO_SUBCODE_64 0x0D00
 
@@ -335,7 +334,7 @@ static int __init kvm_devices_init(void)
 	if (!MACHINE_IS_KVM)
 		return -ENODEV;
 
-	kvm_root = s390_root_dev_register("kvm_s390");
+	kvm_root = root_device_register("kvm_s390");
 	if (IS_ERR(kvm_root)) {
 		rc = PTR_ERR(kvm_root);
 		printk(KERN_ERR "Could not register kvm_s390 root device");
@@ -344,7 +343,7 @@ static int __init kvm_devices_init(void)
 
 	rc = vmem_add_mapping(real_memory_size, PAGE_SIZE);
 	if (rc) {
-		s390_root_dev_unregister(kvm_root);
+		root_device_unregister(kvm_root);
 		return rc;
 	}
 
diff --git a/drivers/s390/net/cu3088.c b/drivers/s390/net/cu3088.c
index f4a32375c03..48383459e99 100644
--- a/drivers/s390/net/cu3088.c
+++ b/drivers/s390/net/cu3088.c
@@ -25,7 +25,6 @@
 #include <linux/module.h>
 #include <linux/err.h>
 
-#include <asm/s390_rdev.h>
 #include <asm/ccwdev.h>
 #include <asm/ccwgroup.h>
 
@@ -120,12 +119,12 @@ cu3088_init (void)
 {
 	int rc;
 
-	cu3088_root_dev = s390_root_dev_register("cu3088");
+	cu3088_root_dev = root_device_register("cu3088");
 	if (IS_ERR(cu3088_root_dev))
 		return PTR_ERR(cu3088_root_dev);
 	rc = ccw_driver_register(&cu3088_driver);
 	if (rc)
-		s390_root_dev_unregister(cu3088_root_dev);
+		root_device_unregister(cu3088_root_dev);
 
 	return rc;
 }
@@ -134,7 +133,7 @@ static void __exit
 cu3088_exit (void)
 {
 	ccw_driver_unregister(&cu3088_driver);
-	s390_root_dev_unregister(cu3088_root_dev);
+	root_device_unregister(cu3088_root_dev);
 }
 
 MODULE_DEVICE_TABLE(ccw,cu3088_ids);
diff --git a/drivers/s390/net/qeth_core_main.c b/drivers/s390/net/qeth_core_main.c
index 6811dd529f4..d1b5bebea7f 100644
--- a/drivers/s390/net/qeth_core_main.c
+++ b/drivers/s390/net/qeth_core_main.c
@@ -24,7 +24,6 @@
 
 #include <asm/ebcdic.h>
 #include <asm/io.h>
-#include <asm/s390_rdev.h>
 
 #include "qeth_core.h"
 #include "qeth_core_offl.h"
@@ -4525,7 +4524,7 @@ static int __init qeth_core_init(void)
 				&driver_attr_group);
 	if (rc)
 		goto driver_err;
-	qeth_core_root_dev = s390_root_dev_register("qeth");
+	qeth_core_root_dev = root_device_register("qeth");
 	rc = IS_ERR(qeth_core_root_dev) ? PTR_ERR(qeth_core_root_dev) : 0;
 	if (rc)
 		goto register_err;
@@ -4539,7 +4538,7 @@ static int __init qeth_core_init(void)
 
 	return 0;
 slab_err:
-	s390_root_dev_unregister(qeth_core_root_dev);
+	root_device_unregister(qeth_core_root_dev);
 register_err:
 	driver_remove_file(&qeth_core_ccwgroup_driver.driver,
 			   &driver_attr_group);
@@ -4557,7 +4556,7 @@ out_err:
 
 static void __exit qeth_core_exit(void)
 {
-	s390_root_dev_unregister(qeth_core_root_dev);
+	root_device_unregister(qeth_core_root_dev);
 	driver_remove_file(&qeth_core_ccwgroup_driver.driver,
 			   &driver_attr_group);
 	ccwgroup_driver_unregister(&qeth_core_ccwgroup_driver);
diff --git a/drivers/s390/net/qeth_l2_main.c b/drivers/s390/net/qeth_l2_main.c
index 21627ba3093..591a2b3ae4c 100644
--- a/drivers/s390/net/qeth_l2_main.c
+++ b/drivers/s390/net/qeth_l2_main.c
@@ -20,8 +20,6 @@
 #include <linux/mii.h>
 #include <linux/ip.h>
 
-#include <asm/s390_rdev.h>
-
 #include "qeth_core.h"
 #include "qeth_core_offl.h"
 
diff --git a/drivers/s390/net/qeth_l3_main.c b/drivers/s390/net/qeth_l3_main.c
index cfda1ecffdf..4693ee4e7b9 100644
--- a/drivers/s390/net/qeth_l3_main.c
+++ b/drivers/s390/net/qeth_l3_main.c
@@ -26,8 +26,6 @@
 #include <net/ip.h>
 #include <net/arp.h>
 
-#include <asm/s390_rdev.h>
-
 #include "qeth_l3.h"
 #include "qeth_core_offl.h"
 
diff --git a/drivers/s390/s390_rdev.c b/drivers/s390/s390_rdev.c
deleted file mode 100644
index 64371c05a3b..00000000000
--- a/drivers/s390/s390_rdev.c
+++ /dev/null
@@ -1,51 +0,0 @@
-/*
- *  drivers/s390/s390_rdev.c
- *  s390 root device
- *
- *    Copyright (C) 2002, 2005 IBM Deutschland Entwicklung GmbH,
- *			 IBM Corporation
- *    Author(s): Cornelia Huck (cornelia.huck@de.ibm.com)
- *		  Carsten Otte  (cotte@de.ibm.com)
- */
-
-#include <linux/slab.h>
-#include <linux/err.h>
-#include <linux/device.h>
-#include <asm/s390_rdev.h>
-
-static void
-s390_root_dev_release(struct device *dev)
-{
-	kfree(dev);
-}
-
-struct device *
-s390_root_dev_register(const char *name)
-{
-	struct device *dev;
-	int ret;
-
-	if (!strlen(name))
-		return ERR_PTR(-EINVAL);
-	dev = kzalloc(sizeof(struct device), GFP_KERNEL);
-	if (!dev)
-		return ERR_PTR(-ENOMEM);
-	dev_set_name(dev, name);
-	dev->release = s390_root_dev_release;
-	ret = device_register(dev);
-	if (ret) {
-		kfree(dev);
-		return ERR_PTR(ret);
-	}
-	return dev;
-}
-
-void
-s390_root_dev_unregister(struct device *dev)
-{
-	if (dev)
-		device_unregister(dev);
-}
-
-EXPORT_SYMBOL(s390_root_dev_register);
-EXPORT_SYMBOL(s390_root_dev_unregister);
diff --git a/drivers/serial/serial_core.c b/drivers/serial/serial_core.c
index dc68b7e0c93..42f4e66fcca 100644
--- a/drivers/serial/serial_core.c
+++ b/drivers/serial/serial_core.c
@@ -2030,7 +2030,7 @@ int uart_suspend_port(struct uart_driver *drv, struct uart_port *port)
 		if (!tries)
 			printk(KERN_ERR "%s%s%s%d: Unable to drain "
 					"transmitter\n",
-			       port->dev ? port->dev->bus_id : "",
+			       port->dev ? dev_name(port->dev) : "",
 			       port->dev ? ": " : "",
 			       drv->dev_name,
 			       drv->tty_driver->name_base + port->line);
@@ -2156,7 +2156,7 @@ uart_report_port(struct uart_driver *drv, struct uart_port *port)
 	}
 
 	printk(KERN_INFO "%s%s%s%d at %s (irq = %d) is a %s\n",
-	       port->dev ? port->dev->bus_id : "",
+	       port->dev ? dev_name(port->dev) : "",
 	       port->dev ? ": " : "",
 	       drv->dev_name,
 	       drv->tty_driver->name_base + port->line,
diff --git a/drivers/spi/Kconfig b/drivers/spi/Kconfig
index b9d0efb6803..4a6fe01831a 100644
--- a/drivers/spi/Kconfig
+++ b/drivers/spi/Kconfig
@@ -78,7 +78,7 @@ config SPI_AU1550
 	  will be called au1550_spi.
 
 config SPI_BITBANG
-	tristate "Bitbanging SPI master"
+	tristate "Utilities for Bitbanging SPI masters"
 	help
 	  With a few GPIO pins, your system can bitbang the SPI protocol.
 	  Select this to get SPI support through I/O pins (GPIO, parallel
@@ -100,6 +100,22 @@ config SPI_BUTTERFLY
 	  inexpensive battery powered microcontroller evaluation board.
 	  This same cable can be used to flash new firmware.
 
+config SPI_GPIO
+	tristate "GPIO-based bitbanging SPI Master"
+	depends on GENERIC_GPIO
+	select SPI_BITBANG
+	help
+	  This simple GPIO bitbanging SPI master uses the arch-neutral GPIO
+	  interface to manage MOSI, MISO, SCK, and chipselect signals.  SPI
+	  slaves connected to a bus using this driver are configured as usual,
+	  except that the spi_board_info.controller_data holds the GPIO number
+	  for the chipselect used by this controller driver.
+
+	  Note that this driver often won't achieve even 1 Mbit/sec speeds,
+	  making it unusually slow for SPI.  If your platform can inline
+	  GPIO operations, you should be able to leverage that for better
+	  speed with a custom version of this driver; see the source code.
+
 config SPI_IMX
 	tristate "Freescale iMX SPI controller"
 	depends on ARCH_IMX && EXPERIMENTAL
diff --git a/drivers/spi/Makefile b/drivers/spi/Makefile
index ccf18de34e1..5e9f521b884 100644
--- a/drivers/spi/Makefile
+++ b/drivers/spi/Makefile
@@ -16,6 +16,7 @@ obj-$(CONFIG_SPI_BFIN)			+= spi_bfin5xx.o
 obj-$(CONFIG_SPI_BITBANG)		+= spi_bitbang.o
 obj-$(CONFIG_SPI_AU1550)		+= au1550_spi.o
 obj-$(CONFIG_SPI_BUTTERFLY)		+= spi_butterfly.o
+obj-$(CONFIG_SPI_GPIO)			+= spi_gpio.o
 obj-$(CONFIG_SPI_IMX)			+= spi_imx.o
 obj-$(CONFIG_SPI_LM70_LLP)		+= spi_lm70llp.o
 obj-$(CONFIG_SPI_PXA2XX)		+= pxa2xx_spi.o
diff --git a/drivers/spi/atmel_spi.c b/drivers/spi/atmel_spi.c
index 8abae4ad0fa..5e39bac9c51 100644
--- a/drivers/spi/atmel_spi.c
+++ b/drivers/spi/atmel_spi.c
@@ -30,13 +30,6 @@
  * The core SPI transfer engine just talks to a register bank to set up
  * DMA transfers; transfer queue progress is driven by IRQs.  The clock
  * framework provides the base clock, subdivided for each spi_device.
- *
- * Newer controllers, marked with "new_1" flag, have:
- *  - CR.LASTXFER
- *  - SPI_MR.DIV32 may become FDIV or must-be-zero (here: always zero)
- *  - SPI_SR.TXEMPTY, SPI_SR.NSSR (and corresponding irqs)
- *  - SPI_CSRx.CSAAT
- *  - SPI_CSRx.SBCR allows faster clocking
  */
 struct atmel_spi {
 	spinlock_t		lock;
@@ -45,7 +38,6 @@ struct atmel_spi {
 	int			irq;
 	struct clk		*clk;
 	struct platform_device	*pdev;
-	unsigned		new_1:1;
 	struct spi_device	*stay;
 
 	u8			stopping;
@@ -59,10 +51,33 @@ struct atmel_spi {
 	dma_addr_t		buffer_dma;
 };
 
+/* Controller-specific per-slave state */
+struct atmel_spi_device {
+	unsigned int		npcs_pin;
+	u32			csr;
+};
+
 #define BUFFER_SIZE		PAGE_SIZE
 #define INVALID_DMA_ADDRESS	0xffffffff
 
 /*
+ * Version 2 of the SPI controller has
+ *  - CR.LASTXFER
+ *  - SPI_MR.DIV32 may become FDIV or must-be-zero (here: always zero)
+ *  - SPI_SR.TXEMPTY, SPI_SR.NSSR (and corresponding irqs)
+ *  - SPI_CSRx.CSAAT
+ *  - SPI_CSRx.SBCR allows faster clocking
+ *
+ * We can determine the controller version by reading the VERSION
+ * register, but I haven't checked that it exists on all chips, and
+ * this is cheaper anyway.
+ */
+static bool atmel_spi_is_v2(void)
+{
+	return !cpu_is_at91rm9200();
+}
+
+/*
  * Earlier SPI controllers (e.g. on at91rm9200) have a design bug whereby
  * they assume that spi slave device state will not change on deselect, so
  * that automagic deselection is OK.  ("NPCSx rises if no data is to be
@@ -80,39 +95,58 @@ struct atmel_spi {
  * Master on Chip Select 0.")  No workaround exists for that ... so for
  * nCS0 on that chip, we (a) don't use the GPIO, (b) can't support CS_HIGH,
  * and (c) will trigger that first erratum in some cases.
+ *
+ * TODO: Test if the atmel_spi_is_v2() branch below works on
+ * AT91RM9200 if we use some other register than CSR0. However, don't
+ * do this unconditionally since AP7000 has an errata where the BITS
+ * field in CSR0 overrides all other CSRs.
  */
 
 static void cs_activate(struct atmel_spi *as, struct spi_device *spi)
 {
-	unsigned gpio = (unsigned) spi->controller_data;
+	struct atmel_spi_device *asd = spi->controller_state;
 	unsigned active = spi->mode & SPI_CS_HIGH;
 	u32 mr;
-	int i;
-	u32 csr;
-	u32 cpol = (spi->mode & SPI_CPOL) ? SPI_BIT(CPOL) : 0;
-
-	/* Make sure clock polarity is correct */
-	for (i = 0; i < spi->master->num_chipselect; i++) {
-		csr = spi_readl(as, CSR0 + 4 * i);
-		if ((csr ^ cpol) & SPI_BIT(CPOL))
-			spi_writel(as, CSR0 + 4 * i, csr ^ SPI_BIT(CPOL));
-	}
 
-	mr = spi_readl(as, MR);
-	mr = SPI_BFINS(PCS, ~(1 << spi->chip_select), mr);
+	if (atmel_spi_is_v2()) {
+		/*
+		 * Always use CSR0. This ensures that the clock
+		 * switches to the correct idle polarity before we
+		 * toggle the CS.
+		 */
+		spi_writel(as, CSR0, asd->csr);
+		spi_writel(as, MR, SPI_BF(PCS, 0x0e) | SPI_BIT(MODFDIS)
+				| SPI_BIT(MSTR));
+		mr = spi_readl(as, MR);
+		gpio_set_value(asd->npcs_pin, active);
+	} else {
+		u32 cpol = (spi->mode & SPI_CPOL) ? SPI_BIT(CPOL) : 0;
+		int i;
+		u32 csr;
+
+		/* Make sure clock polarity is correct */
+		for (i = 0; i < spi->master->num_chipselect; i++) {
+			csr = spi_readl(as, CSR0 + 4 * i);
+			if ((csr ^ cpol) & SPI_BIT(CPOL))
+				spi_writel(as, CSR0 + 4 * i,
+						csr ^ SPI_BIT(CPOL));
+		}
+
+		mr = spi_readl(as, MR);
+		mr = SPI_BFINS(PCS, ~(1 << spi->chip_select), mr);
+		if (spi->chip_select != 0)
+			gpio_set_value(asd->npcs_pin, active);
+		spi_writel(as, MR, mr);
+	}
 
 	dev_dbg(&spi->dev, "activate %u%s, mr %08x\n",
-			gpio, active ? " (high)" : "",
+			asd->npcs_pin, active ? " (high)" : "",
 			mr);
-
-	if (!(cpu_is_at91rm9200() && spi->chip_select == 0))
-		gpio_set_value(gpio, active);
-	spi_writel(as, MR, mr);
 }
 
 static void cs_deactivate(struct atmel_spi *as, struct spi_device *spi)
 {
-	unsigned gpio = (unsigned) spi->controller_data;
+	struct atmel_spi_device *asd = spi->controller_state;
 	unsigned active = spi->mode & SPI_CS_HIGH;
 	u32 mr;
 
@@ -126,11 +160,11 @@ static void cs_deactivate(struct atmel_spi *as, struct spi_device *spi)
 	}
 
 	dev_dbg(&spi->dev, "DEactivate %u%s, mr %08x\n",
-			gpio, active ? " (low)" : "",
+			asd->npcs_pin, active ? " (low)" : "",
 			mr);
 
-	if (!(cpu_is_at91rm9200() && spi->chip_select == 0))
-		gpio_set_value(gpio, !active);
+	if (atmel_spi_is_v2() || spi->chip_select != 0)
+		gpio_set_value(asd->npcs_pin, !active);
 }
 
 static inline int atmel_spi_xfer_is_last(struct spi_message *msg,
@@ -502,6 +536,7 @@ atmel_spi_interrupt(int irq, void *dev_id)
 static int atmel_spi_setup(struct spi_device *spi)
 {
 	struct atmel_spi	*as;
+	struct atmel_spi_device	*asd;
 	u32			scbr, csr;
 	unsigned int		bits = spi->bits_per_word;
 	unsigned long		bus_hz;
@@ -536,19 +571,16 @@ static int atmel_spi_setup(struct spi_device *spi)
 	}
 
 	/* see notes above re chipselect */
-	if (cpu_is_at91rm9200()
+	if (!atmel_spi_is_v2()
 			&& spi->chip_select == 0
 			&& (spi->mode & SPI_CS_HIGH)) {
 		dev_dbg(&spi->dev, "setup: can't be active-high\n");
 		return -EINVAL;
 	}
 
-	/*
-	 * Pre-new_1 chips start out at half the peripheral
-	 * bus speed.
-	 */
+	/* v1 chips start out at half the peripheral bus speed. */
 	bus_hz = clk_get_rate(as->clk);
-	if (!as->new_1)
+	if (!atmel_spi_is_v2())
 		bus_hz /= 2;
 
 	if (spi->max_speed_hz) {
@@ -589,11 +621,20 @@ static int atmel_spi_setup(struct spi_device *spi)
 
 	/* chipselect must have been muxed as GPIO (e.g. in board setup) */
 	npcs_pin = (unsigned int)spi->controller_data;
-	if (!spi->controller_state) {
+	asd = spi->controller_state;
+	if (!asd) {
+		asd = kzalloc(sizeof(struct atmel_spi_device), GFP_KERNEL);
+		if (!asd)
+			return -ENOMEM;
+
 		ret = gpio_request(npcs_pin, spi->dev.bus_id);
-		if (ret)
+		if (ret) {
+			kfree(asd);
 			return ret;
-		spi->controller_state = (void *)npcs_pin;
+		}
+
+		asd->npcs_pin = npcs_pin;
+		spi->controller_state = asd;
 		gpio_direction_output(npcs_pin, !(spi->mode & SPI_CS_HIGH));
 	} else {
 		unsigned long		flags;
@@ -605,11 +646,14 @@ static int atmel_spi_setup(struct spi_device *spi)
 		spin_unlock_irqrestore(&as->lock, flags);
 	}
 
+	asd->csr = csr;
+
 	dev_dbg(&spi->dev,
 		"setup: %lu Hz bpw %u mode 0x%x -> csr%d %08x\n",
 		bus_hz / scbr, bits, spi->mode, spi->chip_select, csr);
 
-	spi_writel(as, CSR0 + 4 * spi->chip_select, csr);
+	if (!atmel_spi_is_v2())
+		spi_writel(as, CSR0 + 4 * spi->chip_select, csr);
 
 	return 0;
 }
@@ -684,10 +728,11 @@ static int atmel_spi_transfer(struct spi_device *spi, struct spi_message *msg)
 static void atmel_spi_cleanup(struct spi_device *spi)
 {
 	struct atmel_spi	*as = spi_master_get_devdata(spi->master);
+	struct atmel_spi_device	*asd = spi->controller_state;
 	unsigned		gpio = (unsigned) spi->controller_data;
 	unsigned long		flags;
 
-	if (!spi->controller_state)
+	if (!asd)
 		return;
 
 	spin_lock_irqsave(&as->lock, flags);
@@ -697,7 +742,9 @@ static void atmel_spi_cleanup(struct spi_device *spi)
 	}
 	spin_unlock_irqrestore(&as->lock, flags);
 
+	spi->controller_state = NULL;
 	gpio_free(gpio);
+	kfree(asd);
 }
 
 /*-------------------------------------------------------------------------*/
@@ -755,8 +802,6 @@ static int __init atmel_spi_probe(struct platform_device *pdev)
 		goto out_free_buffer;
 	as->irq = irq;
 	as->clk = clk;
-	if (!cpu_is_at91rm9200())
-		as->new_1 = 1;
 
 	ret = request_irq(irq, atmel_spi_interrupt, 0,
 			pdev->dev.bus_id, master);
diff --git a/drivers/spi/pxa2xx_spi.c b/drivers/spi/pxa2xx_spi.c
index 6104f461a3c..d0fc4ca2f65 100644
--- a/drivers/spi/pxa2xx_spi.c
+++ b/drivers/spi/pxa2xx_spi.c
@@ -1561,11 +1561,12 @@ out_error_master_alloc:
 static int pxa2xx_spi_remove(struct platform_device *pdev)
 {
 	struct driver_data *drv_data = platform_get_drvdata(pdev);
-	struct ssp_device *ssp = drv_data->ssp;
+	struct ssp_device *ssp;
 	int status = 0;
 
 	if (!drv_data)
 		return 0;
+	ssp = drv_data->ssp;
 
 	/* Remove the queue */
 	status = destroy_queue(drv_data);
diff --git a/drivers/spi/spi.c b/drivers/spi/spi.c
index 3734dc9708e..643908b74bc 100644
--- a/drivers/spi/spi.c
+++ b/drivers/spi/spi.c
@@ -47,7 +47,7 @@ modalias_show(struct device *dev, struct device_attribute *a, char *buf)
 {
 	const struct spi_device	*spi = to_spi_device(dev);
 
-	return snprintf(buf, BUS_ID_SIZE + 1, "%s\n", spi->modalias);
+	return sprintf(buf, "%s\n", spi->modalias);
 }
 
 static struct device_attribute spi_dev_attrs[] = {
@@ -63,7 +63,7 @@ static int spi_match_device(struct device *dev, struct device_driver *drv)
 {
 	const struct spi_device	*spi = to_spi_device(dev);
 
-	return strncmp(spi->modalias, drv->name, BUS_ID_SIZE) == 0;
+	return strcmp(spi->modalias, drv->name) == 0;
 }
 
 static int spi_uevent(struct device *dev, struct kobj_uevent_env *env)
@@ -243,8 +243,7 @@ int spi_add_device(struct spi_device *spi)
 	}
 
 	/* Set the bus ID string */
-	snprintf(spi->dev.bus_id, sizeof spi->dev.bus_id,
-			"%s.%u", spi->master->dev.bus_id,
+	dev_set_name(&spi->dev, "%s.%u", dev_name(&spi->master->dev),
 			spi->chip_select);
 
 
@@ -254,7 +253,7 @@ int spi_add_device(struct spi_device *spi)
 	 */
 	mutex_lock(&spi_add_lock);
 
-	if (bus_find_device_by_name(&spi_bus_type, NULL, spi->dev.bus_id)
+	if (bus_find_device_by_name(&spi_bus_type, NULL, dev_name(&spi->dev))
 			!= NULL) {
 		dev_err(dev, "chipselect %d already in use\n",
 				spi->chip_select);
@@ -269,7 +268,7 @@ int spi_add_device(struct spi_device *spi)
 	status = spi->master->setup(spi);
 	if (status < 0) {
 		dev_err(dev, "can't %s %s, status %d\n",
-				"setup", spi->dev.bus_id, status);
+				"setup", dev_name(&spi->dev), status);
 		goto done;
 	}
 
@@ -277,9 +276,9 @@ int spi_add_device(struct spi_device *spi)
 	status = device_add(&spi->dev);
 	if (status < 0)
 		dev_err(dev, "can't %s %s, status %d\n",
-				"add", spi->dev.bus_id, status);
+				"add", dev_name(&spi->dev), status);
 	else
-		dev_dbg(dev, "registered child %s\n", spi->dev.bus_id);
+		dev_dbg(dev, "registered child %s\n", dev_name(&spi->dev));
 
 done:
 	mutex_unlock(&spi_add_lock);
@@ -504,12 +503,11 @@ int spi_register_master(struct spi_master *master)
 	/* register the device, then userspace will see it.
 	 * registration fails if the bus ID is in use.
 	 */
-	snprintf(master->dev.bus_id, sizeof master->dev.bus_id,
-		"spi%u", master->bus_num);
+	dev_set_name(&master->dev, "spi%u", master->bus_num);
 	status = device_add(&master->dev);
 	if (status < 0)
 		goto done;
-	dev_dbg(dev, "registered master %s%s\n", master->dev.bus_id,
+	dev_dbg(dev, "registered master %s%s\n", dev_name(&master->dev),
 			dynamic ? " (dynamic)" : "");
 
 	/* populate children from any spi device tables */
diff --git a/drivers/spi/spi_bitbang.c b/drivers/spi/spi_bitbang.c
index 96cc39ecb6e..85e61f45121 100644
--- a/drivers/spi/spi_bitbang.c
+++ b/drivers/spi/spi_bitbang.c
@@ -475,7 +475,7 @@ int spi_bitbang_start(struct spi_bitbang *bitbang)
 	/* this task is the only thing to touch the SPI bits */
 	bitbang->busy = 0;
 	bitbang->workqueue = create_singlethread_workqueue(
-			bitbang->master->dev.parent->bus_id);
+			dev_name(bitbang->master->dev.parent));
 	if (bitbang->workqueue == NULL) {
 		status = -EBUSY;
 		goto err1;
diff --git a/drivers/spi/spi_butterfly.c b/drivers/spi/spi_butterfly.c
index 0ee2b209025..c2184866fa9 100644
--- a/drivers/spi/spi_butterfly.c
+++ b/drivers/spi/spi_butterfly.c
@@ -287,7 +287,7 @@ static void butterfly_attach(struct parport *p)
 	pp->dataflash = spi_new_device(pp->bitbang.master, &pp->info[0]);
 	if (pp->dataflash)
 		pr_debug("%s: dataflash at %s\n", p->name,
-				pp->dataflash->dev.bus_id);
+				dev_name(&pp->dataflash->dev));
 
 	// dev_info(_what?_, ...)
 	pr_info("%s: AVR Butterfly\n", p->name);
diff --git a/drivers/spi/spi_gpio.c b/drivers/spi/spi_gpio.c
new file mode 100644
index 00000000000..49698cabc30
--- /dev/null
+++ b/drivers/spi/spi_gpio.c
@@ -0,0 +1,360 @@
+/*
+ * spi_gpio.c - SPI master driver using generic bitbanged GPIO
+ *
+ * Copyright (C) 2006,2008 David Brownell
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/platform_device.h>
+#include <linux/gpio.h>
+
+#include <linux/spi/spi.h>
+#include <linux/spi/spi_bitbang.h>
+#include <linux/spi/spi_gpio.h>
+
+
+/*
+ * This bitbanging SPI master driver should help make systems usable
+ * when a native hardware SPI engine is not available, perhaps because
+ * its driver isn't yet working or because the I/O pins it requires
+ * are used for other purposes.
+ *
+ * platform_device->driver_data ... points to spi_gpio
+ *
+ * spi->controller_state ... reserved for bitbang framework code
+ * spi->controller_data ... holds chipselect GPIO
+ *
+ * spi->master->dev.driver_data ... points to spi_gpio->bitbang
+ */
+
+struct spi_gpio {
+	struct spi_bitbang		bitbang;
+	struct spi_gpio_platform_data	pdata;
+	struct platform_device		*pdev;
+};
+
+/*----------------------------------------------------------------------*/
+
+/*
+ * Because the overhead of going through four GPIO procedure calls
+ * per transferred bit can make performance a problem, this code
+ * is set up so that you can use it in either of two ways:
+ *
+ *   - The slow generic way:  set up platform_data to hold the GPIO
+ *     numbers used for MISO/MOSI/SCK, and issue procedure calls for
+ *     each of them.  This driver can handle several such busses.
+ *
+ *   - The quicker inlined way:  only helps with platform GPIO code
+ *     that inlines operations for constant GPIOs.  This can give
+ *     you tight (fast!) inner loops, but each such bus needs a
+ *     new driver.  You'll define a new C file, with Makefile and
+ *     Kconfig support; the C code can be a total of six lines:
+ *
+ *		#define DRIVER_NAME	"myboard_spi2"
+ *		#define	SPI_MISO_GPIO	119
+ *		#define	SPI_MOSI_GPIO	120
+ *		#define	SPI_SCK_GPIO	121
+ *		#define	SPI_N_CHIPSEL	4
+ *		#include "spi_gpio.c"
+ */
+
+#ifndef DRIVER_NAME
+#define DRIVER_NAME	"spi_gpio"
+
+#define GENERIC_BITBANG	/* vs tight inlines */
+
+/* all functions referencing these symbols must define pdata */
+#define SPI_MISO_GPIO	((pdata)->miso)
+#define SPI_MOSI_GPIO	((pdata)->mosi)
+#define SPI_SCK_GPIO	((pdata)->sck)
+
+#define SPI_N_CHIPSEL	((pdata)->num_chipselect)
+
+#endif
+
+/*----------------------------------------------------------------------*/
+
+static inline const struct spi_gpio_platform_data * __pure
+spi_to_pdata(const struct spi_device *spi)
+{
+	const struct spi_bitbang	*bang;
+	const struct spi_gpio		*spi_gpio;
+
+	bang = spi_master_get_devdata(spi->master);
+	spi_gpio = container_of(bang, struct spi_gpio, bitbang);
+	return &spi_gpio->pdata;
+}
+
+/* this is #defined to avoid unused-variable warnings when inlining */
+#define pdata		spi_to_pdata(spi)
+
+static inline void setsck(const struct spi_device *spi, int is_on)
+{
+	gpio_set_value(SPI_SCK_GPIO, is_on);
+}
+
+static inline void setmosi(const struct spi_device *spi, int is_on)
+{
+	gpio_set_value(SPI_MOSI_GPIO, is_on);
+}
+
+static inline int getmiso(const struct spi_device *spi)
+{
+	return gpio_get_value(SPI_MISO_GPIO);
+}
+
+#undef pdata
+
+/*
+ * NOTE:  this clocks "as fast as we can".  It "should" be a function of the
+ * requested device clock.  Software overhead means we usually have trouble
+ * reaching even one Mbit/sec (except when we can inline bitops), so for now
+ * we'll just assume we never need additional per-bit slowdowns.
+ */
+#define spidelay(nsecs)	do {} while (0)
+
+#define	EXPAND_BITBANG_TXRX
+#include <linux/spi/spi_bitbang.h>
+
+/*
+ * These functions can leverage inline expansion of GPIO calls to shrink
+ * costs for a txrx bit, often by factors of around ten (by instruction
+ * count).  That is particularly visible for larger word sizes, but helps
+ * even with default 8-bit words.
+ *
+ * REVISIT overheads calling these functions for each word also have
+ * significant performance costs.  Having txrx_bufs() calls that inline
+ * the txrx_word() logic would help performance, e.g. on larger blocks
+ * used with flash storage or MMC/SD.  There should also be ways to make
+ * GCC be less stupid about reloading registers inside the I/O loops,
+ * even without inlined GPIO calls; __attribute__((hot)) on GCC 4.3?
+ */
+
+static u32 spi_gpio_txrx_word_mode0(struct spi_device *spi,
+		unsigned nsecs, u32 word, u8 bits)
+{
+	return bitbang_txrx_be_cpha0(spi, nsecs, 0, word, bits);
+}
+
+static u32 spi_gpio_txrx_word_mode1(struct spi_device *spi,
+		unsigned nsecs, u32 word, u8 bits)
+{
+	return bitbang_txrx_be_cpha1(spi, nsecs, 0, word, bits);
+}
+
+static u32 spi_gpio_txrx_word_mode2(struct spi_device *spi,
+		unsigned nsecs, u32 word, u8 bits)
+{
+	return bitbang_txrx_be_cpha0(spi, nsecs, 1, word, bits);
+}
+
+static u32 spi_gpio_txrx_word_mode3(struct spi_device *spi,
+		unsigned nsecs, u32 word, u8 bits)
+{
+	return bitbang_txrx_be_cpha1(spi, nsecs, 1, word, bits);
+}
+
+/*----------------------------------------------------------------------*/
+
+static void spi_gpio_chipselect(struct spi_device *spi, int is_active)
+{
+	unsigned long cs = (unsigned long) spi->controller_data;
+
+	/* set initial clock polarity */
+	if (is_active)
+		setsck(spi, spi->mode & SPI_CPOL);
+
+	/* SPI is normally active-low */
+	gpio_set_value(cs, (spi->mode & SPI_CS_HIGH) ? is_active : !is_active);
+}
+
+static int spi_gpio_setup(struct spi_device *spi)
+{
+	unsigned long	cs = (unsigned long) spi->controller_data;
+	int		status = 0;
+
+	if (spi->bits_per_word > 32)
+		return -EINVAL;
+
+	if (!spi->controller_state) {
+		status = gpio_request(cs, spi->dev.bus_id);
+		if (status)
+			return status;
+		status = gpio_direction_output(cs, spi->mode & SPI_CS_HIGH);
+	}
+	if (!status)
+		status = spi_bitbang_setup(spi);
+	if (status) {
+		if (!spi->controller_state)
+			gpio_free(cs);
+	}
+	return status;
+}
+
+static void spi_gpio_cleanup(struct spi_device *spi)
+{
+	unsigned long	cs = (unsigned long) spi->controller_data;
+
+	gpio_free(cs);
+	spi_bitbang_cleanup(spi);
+}
+
+static int __init spi_gpio_alloc(unsigned pin, const char *label, bool is_in)
+{
+	int value;
+
+	value = gpio_request(pin, label);
+	if (value == 0) {
+		if (is_in)
+			value = gpio_direction_input(pin);
+		else
+			value = gpio_direction_output(pin, 0);
+	}
+	return value;
+}
+
+static int __init
+spi_gpio_request(struct spi_gpio_platform_data *pdata, const char *label)
+{
+	int value;
+
+	/* NOTE:  SPI_*_GPIO symbols may reference "pdata" */
+
+	value = spi_gpio_alloc(SPI_MOSI_GPIO, label, false);
+	if (value)
+		goto done;
+
+	value = spi_gpio_alloc(SPI_MISO_GPIO, label, true);
+	if (value)
+		goto free_mosi;
+
+	value = spi_gpio_alloc(SPI_SCK_GPIO, label, false);
+	if (value)
+		goto free_miso;
+
+	goto done;
+
+free_miso:
+	gpio_free(SPI_MISO_GPIO);
+free_mosi:
+	gpio_free(SPI_MOSI_GPIO);
+done:
+	return value;
+}
+
+static int __init spi_gpio_probe(struct platform_device *pdev)
+{
+	int				status;
+	struct spi_master		*master;
+	struct spi_gpio			*spi_gpio;
+	struct spi_gpio_platform_data	*pdata;
+
+	pdata = pdev->dev.platform_data;
+#ifdef GENERIC_BITBANG
+	if (!pdata || !pdata->num_chipselect)
+		return -ENODEV;
+#endif
+
+	status = spi_gpio_request(pdata, dev_name(&pdev->dev));
+	if (status < 0)
+		return status;
+
+	master = spi_alloc_master(&pdev->dev, sizeof *spi_gpio);
+	if (!master) {
+		status = -ENOMEM;
+		goto gpio_free;
+	}
+	spi_gpio = spi_master_get_devdata(master);
+	platform_set_drvdata(pdev, spi_gpio);
+
+	spi_gpio->pdev = pdev;
+	if (pdata)
+		spi_gpio->pdata = *pdata;
+
+	master->bus_num = pdev->id;
+	master->num_chipselect = SPI_N_CHIPSEL;
+	master->setup = spi_gpio_setup;
+	master->cleanup = spi_gpio_cleanup;
+
+	spi_gpio->bitbang.master = spi_master_get(master);
+	spi_gpio->bitbang.chipselect = spi_gpio_chipselect;
+	spi_gpio->bitbang.txrx_word[SPI_MODE_0] = spi_gpio_txrx_word_mode0;
+	spi_gpio->bitbang.txrx_word[SPI_MODE_1] = spi_gpio_txrx_word_mode1;
+	spi_gpio->bitbang.txrx_word[SPI_MODE_2] = spi_gpio_txrx_word_mode2;
+	spi_gpio->bitbang.txrx_word[SPI_MODE_3] = spi_gpio_txrx_word_mode3;
+	spi_gpio->bitbang.setup_transfer = spi_bitbang_setup_transfer;
+	spi_gpio->bitbang.flags = SPI_CS_HIGH;
+
+	status = spi_bitbang_start(&spi_gpio->bitbang);
+	if (status < 0) {
+		spi_master_put(spi_gpio->bitbang.master);
+gpio_free:
+		gpio_free(SPI_MISO_GPIO);
+		gpio_free(SPI_MOSI_GPIO);
+		gpio_free(SPI_SCK_GPIO);
+		spi_master_put(master);
+	}
+
+	return status;
+}
+
+static int __exit spi_gpio_remove(struct platform_device *pdev)
+{
+	struct spi_gpio			*spi_gpio;
+	struct spi_gpio_platform_data	*pdata;
+	int				status;
+
+	spi_gpio = platform_get_drvdata(pdev);
+	pdata = pdev->dev.platform_data;
+
+	/* stop() unregisters child devices too */
+	status = spi_bitbang_stop(&spi_gpio->bitbang);
+	spi_master_put(spi_gpio->bitbang.master);
+
+	platform_set_drvdata(pdev, NULL);
+
+	gpio_free(SPI_MISO_GPIO);
+	gpio_free(SPI_MOSI_GPIO);
+	gpio_free(SPI_SCK_GPIO);
+
+	return status;
+}
+
+MODULE_ALIAS("platform:" DRIVER_NAME);
+
+static struct platform_driver spi_gpio_driver = {
+	.driver.name	= DRIVER_NAME,
+	.driver.owner	= THIS_MODULE,
+	.remove		= __exit_p(spi_gpio_remove),
+};
+
+static int __init spi_gpio_init(void)
+{
+	return platform_driver_probe(&spi_gpio_driver, spi_gpio_probe);
+}
+module_init(spi_gpio_init);
+
+static void __exit spi_gpio_exit(void)
+{
+	platform_driver_unregister(&spi_gpio_driver);
+}
+module_exit(spi_gpio_exit);
+
+
+MODULE_DESCRIPTION("SPI master driver using generic bitbanged GPIO ");
+MODULE_AUTHOR("David Brownell");
+MODULE_LICENSE("GPL");
diff --git a/drivers/spi/spi_lm70llp.c b/drivers/spi/spi_lm70llp.c
index 39d8d8ad65c..af6526767e2 100644
--- a/drivers/spi/spi_lm70llp.c
+++ b/drivers/spi/spi_lm70llp.c
@@ -287,7 +287,7 @@ static void spi_lm70llp_attach(struct parport *p)
 	pp->spidev_lm70 = spi_new_device(pp->bitbang.master, &pp->info);
 	if (pp->spidev_lm70)
 		dev_dbg(&pp->spidev_lm70->dev, "spidev_lm70 at %s\n",
-				pp->spidev_lm70->dev.bus_id);
+				dev_name(&pp->spidev_lm70->dev));
 	else {
 		printk(KERN_WARNING "%s: spi_new_device failed\n", DRVNAME);
 		status = -ENODEV;
diff --git a/drivers/spi/spi_s3c24xx.c b/drivers/spi/spi_s3c24xx.c
index 256d18395a2..b3ebc1d0f85 100644
--- a/drivers/spi/spi_s3c24xx.c
+++ b/drivers/spi/spi_s3c24xx.c
@@ -19,6 +19,7 @@
 #include <linux/err.h>
 #include <linux/clk.h>
 #include <linux/platform_device.h>
+#include <linux/gpio.h>
 
 #include <linux/spi/spi.h>
 #include <linux/spi/spi_bitbang.h>
@@ -27,7 +28,6 @@
 #include <asm/dma.h>
 #include <mach/hardware.h>
 
-#include <mach/regs-gpio.h>
 #include <plat/regs-spi.h>
 #include <mach/spi.h>
 
@@ -66,7 +66,7 @@ static inline struct s3c24xx_spi *to_hw(struct spi_device *sdev)
 
 static void s3c24xx_spi_gpiocs(struct s3c2410_spi_info *spi, int cs, int pol)
 {
-	s3c2410_gpio_setpin(spi->pin_cs, pol);
+	gpio_set_value(spi->pin_cs, pol);
 }
 
 static void s3c24xx_spi_chipsel(struct spi_device *spi, int value)
@@ -248,8 +248,13 @@ static void s3c24xx_spi_initialsetup(struct s3c24xx_spi *hw)
 	writeb(SPPIN_DEFAULT, hw->regs + S3C2410_SPPIN);
 	writeb(SPCON_DEFAULT, hw->regs + S3C2410_SPCON);
 
-	if (hw->pdata && hw->pdata->gpio_setup)
-		hw->pdata->gpio_setup(hw->pdata, 1);
+	if (hw->pdata) {
+		if (hw->set_cs == s3c24xx_spi_gpiocs)
+			gpio_direction_output(hw->pdata->pin_cs, 1);
+
+		if (hw->pdata->gpio_setup)
+			hw->pdata->gpio_setup(hw->pdata, 1);
+	}
 }
 
 static int __init s3c24xx_spi_probe(struct platform_device *pdev)
@@ -343,18 +348,27 @@ static int __init s3c24xx_spi_probe(struct platform_device *pdev)
 		goto err_no_clk;
 	}
 
-	s3c24xx_spi_initialsetup(hw);
-
 	/* setup any gpio we can */
 
 	if (!pdata->set_cs) {
-		hw->set_cs = s3c24xx_spi_gpiocs;
+		if (pdata->pin_cs < 0) {
+			dev_err(&pdev->dev, "No chipselect pin\n");
+			goto err_register;
+		}
 
-		s3c2410_gpio_setpin(pdata->pin_cs, 1);
-		s3c2410_gpio_cfgpin(pdata->pin_cs, S3C2410_GPIO_OUTPUT);
+		err = gpio_request(pdata->pin_cs, dev_name(&pdev->dev));
+		if (err) {
+			dev_err(&pdev->dev, "Failed to get gpio for cs\n");
+			goto err_register;
+		}
+
+		hw->set_cs = s3c24xx_spi_gpiocs;
+		gpio_direction_output(pdata->pin_cs, 1);
 	} else
 		hw->set_cs = pdata->set_cs;
 
+	s3c24xx_spi_initialsetup(hw);
+
 	/* register our spi controller */
 
 	err = spi_bitbang_start(&hw->bitbang);
@@ -366,6 +380,9 @@ static int __init s3c24xx_spi_probe(struct platform_device *pdev)
 	return 0;
 
  err_register:
+	if (hw->set_cs == s3c24xx_spi_gpiocs)
+		gpio_free(pdata->pin_cs);
+
 	clk_disable(hw->clk);
 	clk_put(hw->clk);
 
@@ -401,6 +418,9 @@ static int __exit s3c24xx_spi_remove(struct platform_device *dev)
 	free_irq(hw->irq, hw);
 	iounmap(hw->regs);
 
+	if (hw->set_cs == s3c24xx_spi_gpiocs)
+		gpio_free(hw->pdata->pin_cs);
+
 	release_resource(hw->ioarea);
 	kfree(hw->ioarea);
 
diff --git a/drivers/staging/Kconfig b/drivers/staging/Kconfig
index 5d457c96bd7..ce6badded47 100644
--- a/drivers/staging/Kconfig
+++ b/drivers/staging/Kconfig
@@ -49,6 +49,8 @@ source "drivers/staging/sxg/Kconfig"
 
 source "drivers/staging/me4000/Kconfig"
 
+source "drivers/staging/meilhaus/Kconfig"
+
 source "drivers/staging/go7007/Kconfig"
 
 source "drivers/staging/usbip/Kconfig"
@@ -63,5 +65,35 @@ source "drivers/staging/at76_usb/Kconfig"
 
 source "drivers/staging/poch/Kconfig"
 
+source "drivers/staging/agnx/Kconfig"
+
+source "drivers/staging/otus/Kconfig"
+
+source "drivers/staging/rt2860/Kconfig"
+
+source "drivers/staging/rt2870/Kconfig"
+
+source "drivers/staging/benet/Kconfig"
+
+source "drivers/staging/comedi/Kconfig"
+
+source "drivers/staging/asus_oled/Kconfig"
+
+source "drivers/staging/panel/Kconfig"
+
+source "drivers/staging/altpciechdma/Kconfig"
+
+source "drivers/staging/rtl8187se/Kconfig"
+
+source "drivers/staging/rspiusb/Kconfig"
+
+source "drivers/staging/mimio/Kconfig"
+
+source "drivers/staging/frontier/Kconfig"
+
+source "drivers/staging/epl/Kconfig"
+
+source "drivers/staging/android/Kconfig"
+
 endif # !STAGING_EXCLUDE_BUILD
 endif # STAGING
diff --git a/drivers/staging/Makefile b/drivers/staging/Makefile
index 71c4d53760b..9ddcc2bb336 100644
--- a/drivers/staging/Makefile
+++ b/drivers/staging/Makefile
@@ -7,6 +7,7 @@ obj-$(CONFIG_ET131X)		+= et131x/
 obj-$(CONFIG_SLICOSS)		+= slicoss/
 obj-$(CONFIG_SXG)		+= sxg/
 obj-$(CONFIG_ME4000)		+= me4000/
+obj-$(CONFIG_MEILHAUS)		+= meilhaus/
 obj-$(CONFIG_VIDEO_GO7007)	+= go7007/
 obj-$(CONFIG_USB_IP_COMMON)	+= usbip/
 obj-$(CONFIG_W35UND)		+= winbond/
@@ -14,3 +15,18 @@ obj-$(CONFIG_PRISM2_USB)	+= wlan-ng/
 obj-$(CONFIG_ECHO)		+= echo/
 obj-$(CONFIG_USB_ATMEL)		+= at76_usb/
 obj-$(CONFIG_POCH)		+= poch/
+obj-$(CONFIG_AGNX)		+= agnx/
+obj-$(CONFIG_OTUS)		+= otus/
+obj-$(CONFIG_RT2860)		+= rt2860/
+obj-$(CONFIG_RT2870)		+= rt2870/
+obj-$(CONFIG_BENET)		+= benet/
+obj-$(CONFIG_COMEDI)		+= comedi/
+obj-$(CONFIG_ASUS_OLED)		+= asus_oled/
+obj-$(CONFIG_PANEL)		+= panel/
+obj-$(CONFIG_ALTERA_PCIE_CHDMA)	+= altpciechdma/
+obj-$(CONFIG_RTL8187SE)		+= rtl8187se/
+obj-$(CONFIG_USB_RSPI)		+= rspiusb/
+obj-$(CONFIG_INPUT_MIMIO)	+= mimio/
+obj-$(CONFIG_TRANZPORT)		+= frontier/
+obj-$(CONFIG_EPL)		+= epl/
+obj-$(CONFIG_ANDROID)		+= android/
diff --git a/drivers/staging/agnx/Kconfig b/drivers/staging/agnx/Kconfig
new file mode 100644
index 00000000000..7f43549e36d
--- /dev/null
+++ b/drivers/staging/agnx/Kconfig
@@ -0,0 +1,5 @@
+config AGNX
+	tristate "Wireless Airgo AGNX support"
+	depends on WLAN_80211 && MAC80211
+	---help---
+	  This is an experimental driver for Airgo AGNX00 wireless chip.
diff --git a/drivers/staging/agnx/Makefile b/drivers/staging/agnx/Makefile
new file mode 100644
index 00000000000..1216564a312
--- /dev/null
+++ b/drivers/staging/agnx/Makefile
@@ -0,0 +1,8 @@
+obj-$(CONFIG_AGNX)	+= agnx.o
+
+agnx-objs :=	rf.o	\
+		pci.o	\
+		xmit.o	\
+		table.o	\
+		sta.o	\
+		phy.o
diff --git a/drivers/staging/agnx/TODO b/drivers/staging/agnx/TODO
new file mode 100644
index 00000000000..89bec74318a
--- /dev/null
+++ b/drivers/staging/agnx/TODO
@@ -0,0 +1,22 @@
+2008 7/18
+
+The RX has can't receive OFDM packet correctly,
+Guess it need be do RX calibrate.
+
+
+before 2008 3/1
+
+1: The RX get too much "CRC failed" pakets, it make the card work very unstable,
+2: After running a while, the card will get infinity "RX Frame" and "Error"
+interrupt, not know the root reason so far, try to fix it
+3: Using two tx queue txd and txm but not only txm.
+4: Set the hdr correctly.
+5: Try to do recalibrate correvtly
+6: To support G mode in future
+7: Fix the mac address can't be readed and set correctly in BE machine.
+8: Fix include and exclude FCS in promisous mode and manage mode
+9: Using sta_notify to notice sta change
+10: Turn on frame reception at the end of start
+11: Guess the card support HW_MULTICAST_FILTER
+12: The tx process should be implment atomic?
+13: Using mac80211 function to control the TX&RX LED.
diff --git a/drivers/staging/agnx/agnx.h b/drivers/staging/agnx/agnx.h
new file mode 100644
index 00000000000..a75b0db3726
--- /dev/null
+++ b/drivers/staging/agnx/agnx.h
@@ -0,0 +1,154 @@
+#ifndef AGNX_H_
+#define AGNX_H_
+
+#include "xmit.h"
+
+#define PFX				KBUILD_MODNAME ": "
+
+static inline u32 agnx_read32(void __iomem *mem_region, u32 offset)
+{
+	return ioread32(mem_region + offset);
+}
+
+static inline void agnx_write32(void __iomem *mem_region, u32 offset, u32 val)
+{
+	iowrite32(val, mem_region + offset);
+}
+
+/* static const struct ieee80211_rate agnx_rates_80211b[] = { */
+/* 	{ .rate = 10, */
+/* 	  .val = 0xa, */
+/* 	  .flags = IEEE80211_RATE_CCK }, */
+/* 	{ .rate = 20, */
+/* 	  .val = 0x14, */
+/* 	  .hw_value = -0x14, */
+/* 	  .flags = IEEE80211_RATE_CCK_2 }, */
+/* 	{ .rate = 55, */
+/* 	  .val = 0x37, */
+/* 	  .val2 = -0x37, */
+/* 	  .flags = IEEE80211_RATE_CCK_2 }, */
+/* 	{ .rate = 110, */
+/* 	  .val = 0x6e, */
+/* 	  .val2 = -0x6e, */
+/* 	  .flags = IEEE80211_RATE_CCK_2 } */
+/* }; */
+
+
+static const struct ieee80211_rate agnx_rates_80211g[] = {
+/* 	{ .bitrate = 10, .hw_value = 1, .flags = IEEE80211_RATE_SHORT_PREAMBLE }, */
+/* 	{ .bitrate = 20, .hw_value = 2, .flags = IEEE80211_RATE_SHORT_PREAMBLE }, */
+/* 	{ .bitrate = 55, .hw_value = 3, .flags = IEEE80211_RATE_SHORT_PREAMBLE }, */
+/* 	{ .bitrate = 110, .hw_value = 4, .flags = IEEE80211_RATE_SHORT_PREAMBLE }, */
+ 	{ .bitrate = 10, .hw_value = 1, },
+ 	{ .bitrate = 20, .hw_value = 2, },
+ 	{ .bitrate = 55, .hw_value = 3, },
+ 	{ .bitrate = 110, .hw_value = 4,},
+
+	{ .bitrate = 60, .hw_value = 0xB, },
+	{ .bitrate = 90, .hw_value = 0xF, },
+	{ .bitrate = 120, .hw_value = 0xA },
+	{ .bitrate = 180, .hw_value = 0xE, },
+//	{ .bitrate = 240, .hw_value = 0xd, },
+	{ .bitrate = 360, .hw_value = 0xD, },
+	{ .bitrate = 480, .hw_value = 0x8, },
+	{ .bitrate = 540, .hw_value = 0xC, },
+};
+
+static const struct ieee80211_channel agnx_channels[] = {
+	{ .center_freq = 2412, .hw_value = 1, },
+	{ .center_freq = 2417, .hw_value = 2, },
+	{ .center_freq = 2422, .hw_value = 3, },
+	{ .center_freq = 2427, .hw_value = 4, },
+	{ .center_freq = 2432, .hw_value = 5, },
+	{ .center_freq = 2437, .hw_value = 6, },
+	{ .center_freq = 2442, .hw_value = 7, },
+	{ .center_freq = 2447, .hw_value = 8, },
+	{ .center_freq = 2452, .hw_value = 9, },
+	{ .center_freq = 2457, .hw_value = 10, },
+	{ .center_freq = 2462, .hw_value = 11, },
+	{ .center_freq = 2467, .hw_value = 12, },
+	{ .center_freq = 2472, .hw_value = 13, },
+	{ .center_freq = 2484, .hw_value = 14, },
+};
+
+#define NUM_DRIVE_MODES	2
+/* Agnx operate mode */
+enum {
+	AGNX_MODE_80211A,
+	AGNX_MODE_80211A_OOB,
+	AGNX_MODE_80211A_MIMO,
+	AGNX_MODE_80211B_SHORT,
+	AGNX_MODE_80211B_LONG,
+	AGNX_MODE_80211G,
+	AGNX_MODE_80211G_OOB,
+	AGNX_MODE_80211G_MIMO,
+};
+
+enum {
+	AGNX_UNINIT,
+	AGNX_START,
+	AGNX_STOP,
+};
+
+struct agnx_priv {
+	struct pci_dev *pdev;
+	struct ieee80211_hw *hw;
+
+	spinlock_t lock;
+	struct mutex mutex;
+	unsigned int init_status;
+
+	void __iomem *ctl;	/* pointer to base ram address */
+	void __iomem *data;	/* pointer to mem region #2 */
+
+	struct agnx_ring rx;
+	struct agnx_ring txm;
+	struct agnx_ring txd;
+
+	/* Need volatile? */
+	u32 irq_status;
+
+        struct delayed_work periodic_work; /* Periodic tasks like recalibrate*/
+	struct ieee80211_low_level_stats stats;
+
+//        unsigned int phymode;
+	int mode;
+	int channel;
+	u8 bssid[ETH_ALEN];
+
+	u8 mac_addr[ETH_ALEN];
+	u8 revid;
+
+	struct ieee80211_supported_band band;
+};
+
+
+#define AGNX_CHAINS_MAX	6
+#define AGNX_PERIODIC_DELAY 60000 /* unit: ms */
+#define LOCAL_STAID	0	/* the station entry for the card itself */
+#define BSSID_STAID	1	/* the station entry for the bsssid AP */
+#define	spi_delay()	udelay(40)
+#define eeprom_delay()	udelay(40)
+#define	routing_table_delay()	udelay(50)
+
+/* PDU pool MEM region #2 */
+#define AGNX_PDUPOOL		0x40000	/* PDU pool */
+#define AGNX_PDUPOOL_SIZE	0x8000	/* PDU pool size*/
+#define AGNX_PDU_TX_WQ		0x41000	/* PDU list TX workqueue */
+#define AGNX_PDU_FREE		0x41800	/* Free Pool */
+#define PDU_SIZE		0x80	/* Free Pool node size */
+#define PDU_FREE_CNT		0xd0 /* Free pool node count */
+
+
+/* RF stuffs */
+extern void rf_chips_init(struct agnx_priv *priv);
+extern void spi_rc_write(void __iomem *mem_region, u32 chip_ids, u32 sw);
+extern void calibrate_oscillator(struct agnx_priv *priv);
+extern void do_calibration(struct agnx_priv *priv);
+extern void antenna_calibrate(struct agnx_priv *priv);
+extern void __antenna_calibrate(struct agnx_priv *priv);
+extern void print_offsets(struct agnx_priv *priv);
+extern int agnx_set_channel(struct agnx_priv *priv, unsigned int channel);
+
+
+#endif /* AGNX_H_ */
diff --git a/drivers/staging/agnx/debug.h b/drivers/staging/agnx/debug.h
new file mode 100644
index 00000000000..e3e25dda0ba
--- /dev/null
+++ b/drivers/staging/agnx/debug.h
@@ -0,0 +1,418 @@
+#ifndef AGNX_DEBUG_H_
+#define AGNX_DEBUG_H_
+
+#include "agnx.h"
+#include "phy.h"
+#include "sta.h"
+#include "xmit.h"
+
+#define AGNX_TRACE              printk(KERN_ERR PFX "function:%s line:%d\n", __func__, __LINE__)
+
+#define PRINTK_LE16(prefix, var)	printk(KERN_DEBUG PFX #prefix ": " #var " 0x%.4x\n", le16_to_cpu(var))
+#define PRINTK_LE32(prefix, var)	printk(KERN_DEBUG PFX #prefix ": " #var " 0x%.8x\n", le32_to_cpu(var))
+#define PRINTK_U8(prefix, var) 		printk(KERN_DEBUG PFX #prefix ": " #var " 0x%.2x\n", var)
+#define PRINTK_BE16(prefix, var)	printk(KERN_DEBUG PFX #prefix ": " #var " 0x%.4x\n", be16_to_cpu(var))
+#define PRINTK_BE32(prefix, var)	printk(KERN_DEBUG PFX #prefix ": " #var " 0x%.8x\n", be32_to_cpu(var))
+#define PRINTK_BITS(prefix, field)    	printk(KERN_DEBUG PFX #prefix ": " #field ": 0x%x\n", (reg & field) >> field##_SHIFT)
+
+static inline void agnx_bug(char *reason)
+{
+	printk(KERN_ERR PFX "%s\n", reason);
+	BUG();
+}
+
+static inline void agnx_print_desc(struct agnx_desc *desc)
+{
+        u32 reg = be32_to_cpu(desc->frag);
+
+	PRINTK_BITS(DESC, PACKET_LEN);
+
+	if (reg & FIRST_FRAG) {
+		PRINTK_BITS(DESC, FIRST_PACKET_MASK);
+		PRINTK_BITS(DESC, FIRST_RESERV2);
+		PRINTK_BITS(DESC, FIRST_TKIP_ERROR);
+		PRINTK_BITS(DESC, FIRST_TKIP_PACKET);
+		PRINTK_BITS(DESC, FIRST_RESERV1);
+		PRINTK_BITS(DESC, FIRST_FRAG_LEN);
+	} else {
+		PRINTK_BITS(DESC, SUB_RESERV2);
+		PRINTK_BITS(DESC, SUB_TKIP_ERROR);
+		PRINTK_BITS(DESC, SUB_TKIP_PACKET);
+		PRINTK_BITS(DESC, SUB_RESERV1);
+		PRINTK_BITS(DESC, SUB_FRAG_LEN);
+	}
+
+	PRINTK_BITS(DESC, FIRST_FRAG);
+	PRINTK_BITS(DESC, LAST_FRAG);
+	PRINTK_BITS(DESC, OWNER);
+}
+
+
+static inline void dump_ieee80211b_phy_hdr(__be32 _11b0, __be32 _11b1)
+{
+
+}
+
+static inline void agnx_print_hdr(struct agnx_hdr *hdr)
+{
+	u32 reg;
+	int i;
+
+	reg = be32_to_cpu(hdr->reg0);
+	PRINTK_BITS(HDR, RTS);
+	PRINTK_BITS(HDR, MULTICAST);
+	PRINTK_BITS(HDR, ACK);
+	PRINTK_BITS(HDR, TM);
+	PRINTK_BITS(HDR, RELAY);
+	PRINTK_BITS(HDR, REVISED_FCS);
+	PRINTK_BITS(HDR, NEXT_BUFFER_ADDR);
+
+	reg = be32_to_cpu(hdr->reg1);
+	PRINTK_BITS(HDR, MAC_HDR_LEN);
+	PRINTK_BITS(HDR, DURATION_OVERIDE);
+	PRINTK_BITS(HDR, PHY_HDR_OVERIDE);
+	PRINTK_BITS(HDR, CRC_FAIL);
+	PRINTK_BITS(HDR, SEQUENCE_NUMBER);
+	PRINTK_BITS(HDR, BUFF_HEAD_ADDR);
+
+	reg = be32_to_cpu(hdr->reg2);
+	PRINTK_BITS(HDR, PDU_COUNT);
+	PRINTK_BITS(HDR, WEP_KEY);
+	PRINTK_BITS(HDR, USES_WEP_KEY);
+	PRINTK_BITS(HDR, KEEP_ALIVE);
+	PRINTK_BITS(HDR, BUFF_TAIL_ADDR);
+
+	reg = be32_to_cpu(hdr->reg3);
+	PRINTK_BITS(HDR, CTS_11G);
+	PRINTK_BITS(HDR, RTS_11G);
+	PRINTK_BITS(HDR, FRAG_SIZE);
+	PRINTK_BITS(HDR, PAYLOAD_LEN);
+	PRINTK_BITS(HDR, FRAG_NUM);
+
+	reg = be32_to_cpu(hdr->reg4);
+	PRINTK_BITS(HDR, RELAY_STAID);
+	PRINTK_BITS(HDR, STATION_ID);
+	PRINTK_BITS(HDR, WORKQUEUE_ID);
+
+	reg = be32_to_cpu(hdr->reg5);
+	/* printf the route flag */
+	PRINTK_BITS(HDR, ROUTE_HOST);
+	PRINTK_BITS(HDR, ROUTE_CARD_CPU);
+	PRINTK_BITS(HDR, ROUTE_ENCRYPTION);
+	PRINTK_BITS(HDR, ROUTE_TX);
+	PRINTK_BITS(HDR, ROUTE_RX1);
+	PRINTK_BITS(HDR, ROUTE_RX2);
+	PRINTK_BITS(HDR, ROUTE_COMPRESSION);
+
+	PRINTK_BE32(HDR, hdr->_11g0);
+	PRINTK_BE32(HDR, hdr->_11g1);
+	PRINTK_BE32(HDR, hdr->_11b0);
+	PRINTK_BE32(HDR, hdr->_11b1);
+
+	dump_ieee80211b_phy_hdr(hdr->_11b0, hdr->_11b1);
+
+	/* Fixme */
+	for (i = 0; i < ARRAY_SIZE(hdr->mac_hdr); i++) {
+		if (i == 0)
+			printk(KERN_DEBUG PFX "IEEE80211 HDR: ");
+		printk("%.2x ", hdr->mac_hdr[i]);
+		if (i + 1 == ARRAY_SIZE(hdr->mac_hdr))
+			printk("\n");
+	}
+
+	PRINTK_BE16(HDR, hdr->rts_duration);
+	PRINTK_BE16(HDR, hdr->last_duration);
+	PRINTK_BE16(HDR, hdr->sec_last_duration);
+	PRINTK_BE16(HDR, hdr->other_duration);
+	PRINTK_BE16(HDR, hdr->tx_other_duration);
+	PRINTK_BE16(HDR, hdr->last_11g_len);
+	PRINTK_BE16(HDR, hdr->other_11g_len);
+	PRINTK_BE16(HDR, hdr->last_11b_len);
+	PRINTK_BE16(HDR, hdr->other_11b_len);
+
+	/* FIXME */
+	reg = be16_to_cpu(hdr->reg6);
+	PRINTK_BITS(HDR, MBF);
+	PRINTK_BITS(HDR, RSVD4);
+
+	PRINTK_BE16(HDR, hdr->rx_frag_stat);
+
+	PRINTK_BE32(HDR, hdr->time_stamp);
+	PRINTK_BE32(HDR, hdr->phy_stats_hi);
+	PRINTK_BE32(HDR, hdr->phy_stats_lo);
+	PRINTK_BE32(HDR, hdr->mic_key0);
+	PRINTK_BE32(HDR, hdr->mic_key1);
+} /* agnx_print_hdr */
+
+
+static inline void agnx_print_rx_hdr(struct agnx_hdr *hdr)
+{
+	agnx_print_hdr(hdr);
+
+	PRINTK_BE16(HDR, hdr->rx.rx_packet_duration);
+	PRINTK_BE16(HDR, hdr->rx.replay_cnt);
+
+	PRINTK_U8(HDR, hdr->rx_channel);
+}
+
+static inline void agnx_print_tx_hdr(struct agnx_hdr *hdr)
+{
+	agnx_print_hdr(hdr);
+
+	PRINTK_U8(HDR, hdr->tx.long_retry_limit);
+	PRINTK_U8(HDR, hdr->tx.short_retry_limit);
+	PRINTK_U8(HDR, hdr->tx.long_retry_cnt);
+	PRINTK_U8(HDR, hdr->tx.short_retry_cnt);
+
+	PRINTK_U8(HDR, hdr->rx_channel);
+}
+
+static inline void
+agnx_print_sta_power(struct agnx_priv *priv, unsigned int sta_idx)
+{
+	struct agnx_sta_power power;
+	u32 reg;
+
+	get_sta_power(priv, &power, sta_idx);
+
+	reg = le32_to_cpu(power.reg);
+	PRINTK_BITS(STA_POWER, SIGNAL);
+	PRINTK_BITS(STA_POWER, RATE);
+	PRINTK_BITS(STA_POWER, TIFS);
+	PRINTK_BITS(STA_POWER, EDCF);
+	PRINTK_BITS(STA_POWER, CHANNEL_BOND);
+	PRINTK_BITS(STA_POWER, PHY_MODE);
+	PRINTK_BITS(STA_POWER, POWER_LEVEL);
+	PRINTK_BITS(STA_POWER, NUM_TRANSMITTERS);
+}
+
+static inline void
+agnx_print_sta_tx_wq(struct agnx_priv *priv, unsigned int sta_idx, unsigned int wq_idx)
+{
+	struct agnx_sta_tx_wq tx_wq;
+	u32 reg;
+
+	get_sta_tx_wq(priv, &tx_wq, sta_idx, wq_idx);
+
+	reg = le32_to_cpu(tx_wq.reg0);
+	PRINTK_BITS(STA_TX_WQ, TAIL_POINTER);
+	PRINTK_BITS(STA_TX_WQ, HEAD_POINTER_LOW);
+
+	reg = le32_to_cpu(tx_wq.reg3);
+	PRINTK_BITS(STA_TX_WQ, HEAD_POINTER_HIGH);
+	PRINTK_BITS(STA_TX_WQ, ACK_POINTER_LOW);
+
+	reg = le32_to_cpu(tx_wq.reg1);
+	PRINTK_BITS(STA_TX_WQ, ACK_POINTER_HIGH);
+	PRINTK_BITS(STA_TX_WQ, HEAD_TIMOUT_TAIL_PACK_CNT);
+	PRINTK_BITS(STA_TX_WQ, ACK_TIMOUT_TAIL_PACK_CNT);
+
+	reg = le32_to_cpu(tx_wq.reg2);
+	PRINTK_BITS(STA_TX_WQ, HEAD_TIMOUT_WIN_LIM_BYTE_CNT);
+	PRINTK_BITS(STA_TX_WQ, HEAD_TIMOUT_WIN_LIM_FRAG_CNT);
+	PRINTK_BITS(STA_TX_WQ, WORK_QUEUE_ACK_TYPE);
+	PRINTK_BITS(STA_TX_WQ, WORK_QUEUE_VALID);
+}
+
+static inline void agnx_print_sta_traffic(struct agnx_sta_traffic *traffic)
+{
+	u32 reg;
+
+	reg = le32_to_cpu(traffic->reg0);
+	PRINTK_BITS(STA_TRAFFIC, ACK_TIMOUT_CNT);
+	PRINTK_BITS(STA_TRAFFIC, TRAFFIC_ACK_TYPE);
+	PRINTK_BITS(STA_TRAFFIC, NEW_PACKET);
+	PRINTK_BITS(STA_TRAFFIC, TRAFFIC_VALID);
+	PRINTK_BITS(STA_TRAFFIC, RX_HDR_DESC_POINTER);
+
+	reg = le32_to_cpu(traffic->reg1);
+	PRINTK_BITS(STA_TRAFFIC, RX_PACKET_TIMESTAMP);
+	PRINTK_BITS(STA_TRAFFIC, TRAFFIC_RESERVED);
+	PRINTK_BITS(STA_TRAFFIC, SV);
+	PRINTK_BITS(STA_TRAFFIC, RX_SEQUENCE_NUM);
+
+	PRINTK_LE32(STA_TRAFFIC, traffic->tx_replay_cnt_low);
+
+	PRINTK_LE16(STA_TRAFFIC, traffic->tx_replay_cnt_high);
+	PRINTK_LE16(STA_TRAFFIC, traffic->rx_replay_cnt_high);
+
+	PRINTK_LE32(STA_TRAFFIC, traffic->rx_replay_cnt_low);
+}
+
+static inline void agnx_print_sta(struct agnx_priv *priv, unsigned int sta_idx)
+{
+	struct agnx_sta station;
+	struct agnx_sta *sta = &station;
+	u32 reg;
+	unsigned int i;
+
+	get_sta(priv, sta, sta_idx);
+
+	for (i = 0; i < 4; i++)
+		PRINTK_LE32(STA, sta->tx_session_keys[i]);
+	for (i = 0; i < 4; i++)
+		PRINTK_LE32(STA, sta->rx_session_keys[i]);
+
+	reg = le32_to_cpu(sta->reg);
+	PRINTK_BITS(STA, ID_1);
+	PRINTK_BITS(STA, ID_0);
+	PRINTK_BITS(STA, ENABLE_CONCATENATION);
+	PRINTK_BITS(STA, ENABLE_DECOMPRESSION);
+	PRINTK_BITS(STA, STA_RESERVED);
+	PRINTK_BITS(STA, EAP);
+	PRINTK_BITS(STA, ED_NULL);
+	PRINTK_BITS(STA, ENCRYPTION_POLICY);
+	PRINTK_BITS(STA, DEFINED_KEY_ID);
+	PRINTK_BITS(STA, FIXED_KEY);
+	PRINTK_BITS(STA, KEY_VALID);
+	PRINTK_BITS(STA, STATION_VALID);
+
+	PRINTK_LE32(STA, sta->tx_aes_blks_unicast);
+	PRINTK_LE32(STA, sta->rx_aes_blks_unicast);
+
+	PRINTK_LE16(STA, sta->aes_format_err_unicast_cnt);
+	PRINTK_LE16(STA, sta->aes_replay_unicast);
+
+	PRINTK_LE16(STA, sta->aes_decrypt_err_unicast);
+	PRINTK_LE16(STA, sta->aes_decrypt_err_default);
+
+	PRINTK_LE16(STA, sta->single_retry_packets);
+	PRINTK_LE16(STA, sta->failed_tx_packets);
+
+	PRINTK_LE16(STA, sta->muti_retry_packets);
+	PRINTK_LE16(STA, sta->ack_timeouts);
+
+	PRINTK_LE16(STA, sta->frag_tx_cnt);
+	PRINTK_LE16(STA, sta->rts_brq_sent);
+
+	PRINTK_LE16(STA, sta->tx_packets);
+	PRINTK_LE16(STA, sta->cts_back_timeout);
+
+	PRINTK_LE32(STA, sta->phy_stats_high);
+	PRINTK_LE32(STA, sta->phy_stats_low);
+
+//	for (i = 0; i < 8; i++)
+	agnx_print_sta_traffic(sta->traffic + 0);
+
+	PRINTK_LE16(STA, sta->traffic_class0_frag_success);
+	PRINTK_LE16(STA, sta->traffic_class1_frag_success);
+	PRINTK_LE16(STA, sta->traffic_class2_frag_success);
+	PRINTK_LE16(STA, sta->traffic_class3_frag_success);
+	PRINTK_LE16(STA, sta->traffic_class4_frag_success);
+	PRINTK_LE16(STA, sta->traffic_class5_frag_success);
+	PRINTK_LE16(STA, sta->traffic_class6_frag_success);
+	PRINTK_LE16(STA, sta->traffic_class7_frag_success);
+
+	PRINTK_LE16(STA, sta->num_frag_non_prime_rates);
+	PRINTK_LE16(STA, sta->ack_timeout_non_prime_rates);
+}
+
+
+static inline void dump_ieee80211_hdr(struct ieee80211_hdr *hdr, char *tag)
+{
+	u16 fctl;
+        int hdrlen;
+	DECLARE_MAC_BUF(mac);
+
+        fctl = le16_to_cpu(hdr->frame_control);
+	switch (fctl & IEEE80211_FCTL_FTYPE) {
+	case IEEE80211_FTYPE_DATA:
+		printk(PFX "%s DATA ", tag);
+		break;
+	case IEEE80211_FTYPE_CTL:
+		printk(PFX "%s CTL ", tag);
+		break;
+	case IEEE80211_FTYPE_MGMT:
+		printk(PFX "%s MGMT ", tag);
+		switch(fctl & IEEE80211_FCTL_STYPE) {
+		case IEEE80211_STYPE_ASSOC_REQ:
+			printk("SubType: ASSOC_REQ ");
+			break;
+		case IEEE80211_STYPE_ASSOC_RESP:
+			printk("SubType: ASSOC_RESP ");
+			break;
+		case IEEE80211_STYPE_REASSOC_REQ:
+			printk("SubType: REASSOC_REQ ");
+			break;
+		case IEEE80211_STYPE_REASSOC_RESP:
+			printk("SubType: REASSOC_RESP ");
+			break;
+		case IEEE80211_STYPE_PROBE_REQ:
+			printk("SubType: PROBE_REQ ");
+			break;
+		case IEEE80211_STYPE_PROBE_RESP:
+			printk("SubType: PROBE_RESP ");
+			break;
+		case IEEE80211_STYPE_BEACON:
+			printk("SubType: BEACON ");
+			break;
+		case IEEE80211_STYPE_ATIM:
+			printk("SubType: ATIM ");
+			break;
+		case IEEE80211_STYPE_DISASSOC:
+			printk("SubType: DISASSOC ");
+			break;
+		case IEEE80211_STYPE_AUTH:
+			printk("SubType: AUTH ");
+			break;
+		case IEEE80211_STYPE_DEAUTH:
+			printk("SubType: DEAUTH ");
+			break;
+		case IEEE80211_STYPE_ACTION:
+			printk("SubType: ACTION ");
+			break;
+		default:
+			printk("SubType: Unknow\n");
+		}
+		break;
+	default:
+		printk(PFX "%s Packet type: Unknow\n", tag);
+	}
+
+        hdrlen = ieee80211_hdrlen(fctl);
+
+	if (hdrlen >= 4)
+		printk("FC=0x%04x DUR=0x%04x",
+		       fctl, le16_to_cpu(hdr->duration_id));
+	if (hdrlen >= 10)
+		printk(" A1=%s", print_mac(mac, hdr->addr1));
+	if (hdrlen >= 16)
+		printk(" A2=%s", print_mac(mac, hdr->addr2));
+	if (hdrlen >= 24)
+		printk(" A3=%s", print_mac(mac, hdr->addr3));
+	if (hdrlen >= 30)
+		printk(" A4=%s", print_mac(mac, hdr->addr4));
+	printk("\n");
+}
+
+static inline void dump_txm_registers(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	int i;
+	for (i = 0; i <=0x1e8; i += 4) {
+		printk(KERN_DEBUG PFX "TXM: %x---> 0x%.8x\n", i, ioread32(ctl + i));
+	}
+}
+static inline void dump_rxm_registers(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	int i;
+	for (i = 0; i <=0x108; i += 4)
+		printk(KERN_DEBUG PFX "RXM: %x---> 0x%.8x\n", i, ioread32(ctl + 0x2000 + i));
+}
+static inline void dump_bm_registers(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	int i;
+	for (i = 0; i <=0x90; i += 4)
+		printk(KERN_DEBUG PFX "BM: %x---> 0x%.8x\n", i, ioread32(ctl + 0x2c00 + i));
+}
+static inline void dump_cir_registers(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	int i;
+	for (i = 0; i <=0xb8; i += 4)
+		printk(KERN_DEBUG PFX "CIR: %x---> 0x%.8x\n", i, ioread32(ctl + 0x3000 + i));
+}
+
+#endif /* AGNX_DEBUG_H_ */
diff --git a/drivers/staging/agnx/pci.c b/drivers/staging/agnx/pci.c
new file mode 100644
index 00000000000..854630cb527
--- /dev/null
+++ b/drivers/staging/agnx/pci.c
@@ -0,0 +1,644 @@
+/**
+ * Airgo MIMO wireless driver
+ *
+ * Copyright (c) 2007 Li YanBo <dreamfly281@gmail.com>
+
+ * Thanks for Jeff Williams <angelbane@gmail.com> do reverse engineer
+ * works and published the SPECS at http://airgo.wdwconsulting.net/mymoin
+
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/init.h>
+#include <linux/etherdevice.h>
+#include <linux/pci.h>
+#include <linux/delay.h>
+
+#include "agnx.h"
+#include "debug.h"
+#include "xmit.h"
+#include "phy.h"
+
+MODULE_AUTHOR("Li YanBo <dreamfly281@gmail.com>");
+MODULE_DESCRIPTION("Airgo MIMO PCI wireless driver");
+MODULE_LICENSE("GPL");
+
+static struct pci_device_id agnx_pci_id_tbl[] __devinitdata = {
+	{ PCI_DEVICE(0x17cb, 0x0001) },	/* Beklin F5d8010, Netgear WGM511 etc */
+	{ PCI_DEVICE(0x17cb, 0x0002) },	/* Netgear Wpnt511 */
+	{ 0 }
+};
+
+MODULE_DEVICE_TABLE(pci, agnx_pci_id_tbl);
+
+
+static inline void agnx_interrupt_ack(struct agnx_priv *priv, u32 *reason)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+
+	if ( *reason & AGNX_STAT_RX ) {
+		/* Mark complete RX */
+		reg = ioread32(ctl + AGNX_CIR_RXCTL);
+		reg |= 0x4;
+		iowrite32(reg, ctl + AGNX_CIR_RXCTL);
+		/* disable Rx interrupt */
+	}
+	if ( *reason & AGNX_STAT_TX ) {
+		reg = ioread32(ctl + AGNX_CIR_TXDCTL);
+		if (reg & 0x4) {
+			iowrite32(reg, ctl + AGNX_CIR_TXDCTL);
+			*reason |= AGNX_STAT_TXD;
+		}
+ 		reg = ioread32(ctl + AGNX_CIR_TXMCTL);
+		if (reg & 0x4) {
+			iowrite32(reg, ctl + AGNX_CIR_TXMCTL);
+			*reason |= AGNX_STAT_TXM;
+		}
+	}
+	if ( *reason & AGNX_STAT_X ) {
+/* 		reg = ioread32(ctl + AGNX_INT_STAT); */
+/* 		iowrite32(reg, ctl + AGNX_INT_STAT); */
+/* 		/\* FIXME reinit interrupt mask *\/ */
+/* 		reg = 0xc390bf9 & ~IRQ_TX_BEACON; */
+/* 		reg &= ~IRQ_TX_DISABLE; */
+/* 		iowrite32(reg, ctl + AGNX_INT_MASK); */
+/* 		iowrite32(0x800, ctl + AGNX_CIR_BLKCTL); */
+	}
+} /* agnx_interrupt_ack */
+
+static irqreturn_t agnx_interrupt_handler(int irq, void *dev_id)
+{
+	struct ieee80211_hw *dev = dev_id;
+	struct agnx_priv *priv = dev->priv;
+	void __iomem *ctl = priv->ctl;
+	irqreturn_t ret = IRQ_NONE;
+	u32 irq_reason;
+
+	spin_lock(&priv->lock);
+
+//	printk(KERN_ERR PFX "Get a interrupt %s\n", __func__);
+
+	if (priv->init_status != AGNX_START)
+		goto out;
+
+	/* FiXME  Here has no lock, Is this will lead to race? */
+	irq_reason = ioread32(ctl + AGNX_CIR_BLKCTL);
+	if (!(irq_reason & 0x7))
+		goto out;
+
+	ret = IRQ_HANDLED;
+	priv->irq_status = ioread32(ctl + AGNX_INT_STAT);
+
+//	printk(PFX "Interrupt reason is 0x%x\n", irq_reason);
+	/* Make sure the txm and txd flags don't conflict with other unknown
+	   interrupt flag, maybe is not necessary */
+	irq_reason &= 0xF;
+
+	disable_rx_interrupt(priv);
+	/* TODO Make sure the card finished initialized */
+	agnx_interrupt_ack(priv, &irq_reason);
+
+	if ( irq_reason & AGNX_STAT_RX )
+		handle_rx_irq(priv);
+	if ( irq_reason & AGNX_STAT_TXD )
+		handle_txd_irq(priv);
+	if ( irq_reason & AGNX_STAT_TXM )
+		handle_txm_irq(priv);
+	if ( irq_reason & AGNX_STAT_X )
+		handle_other_irq(priv);
+
+	enable_rx_interrupt(priv);
+out:
+	spin_unlock(&priv->lock);
+	return ret;
+} /* agnx_interrupt_handler */
+
+
+/* FIXME */
+static int agnx_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
+{
+	AGNX_TRACE;
+	return _agnx_tx(dev->priv, skb);
+} /* agnx_tx */
+
+
+static int agnx_get_mac_address(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+	AGNX_TRACE;
+
+	/* Attention! directly read the MAC or other date from EEPROM will
+	 lead to cardbus(WGM511) lock up when write to PM PLL register */
+	reg = agnx_read32(ctl, 0x3544);
+	udelay(40);
+	reg = agnx_read32(ctl, 0x354c);
+	udelay(50);
+	/* Get the mac address */
+	reg = agnx_read32(ctl, 0x3544);
+	udelay(40);
+
+	/* HACK */
+	reg = cpu_to_le32(reg);
+	priv->mac_addr[0] = ((u8 *)&reg)[2];
+	priv->mac_addr[1] = ((u8 *)&reg)[3];
+	reg = agnx_read32(ctl, 0x3548);
+	udelay(50);
+	*((u32 *)(priv->mac_addr + 2)) = cpu_to_le32(reg);
+
+	if (!is_valid_ether_addr(priv->mac_addr)) {
+		DECLARE_MAC_BUF(mbuf);
+		printk(KERN_WARNING PFX "read mac %s\n", print_mac(mbuf, priv->mac_addr));
+		printk(KERN_WARNING PFX "Invalid hwaddr! Using random hwaddr\n");
+		random_ether_addr(priv->mac_addr);
+	}
+
+	return 0;
+} /* agnx_get_mac_address */
+
+static int agnx_alloc_rings(struct agnx_priv *priv)
+{
+	unsigned int len;
+	AGNX_TRACE;
+
+	/* Allocate RX/TXM/TXD rings info */
+	priv->rx.size = AGNX_RX_RING_SIZE;
+	priv->txm.size = AGNX_TXM_RING_SIZE;
+	priv->txd.size = AGNX_TXD_RING_SIZE;
+
+	len = priv->rx.size + priv->txm.size + priv->txd.size;
+
+//	priv->rx.info = kzalloc(sizeof(struct agnx_info) * len, GFP_KERNEL);
+	priv->rx.info = kzalloc(sizeof(struct agnx_info) * len, GFP_ATOMIC);
+	if (!priv->rx.info)
+		return -ENOMEM;
+	priv->txm.info = priv->rx.info + priv->rx.size;
+	priv->txd.info = priv->txm.info + priv->txm.size;
+
+	/* Allocate RX/TXM/TXD descriptors */
+	priv->rx.desc = pci_alloc_consistent(priv->pdev, sizeof(struct agnx_desc) * len,
+					     &priv->rx.dma);
+	if (!priv->rx.desc) {
+		kfree(priv->rx.info);
+		return -ENOMEM;
+	}
+
+	priv->txm.desc = priv->rx.desc + priv->rx.size;
+	priv->txm.dma = priv->rx.dma + sizeof(struct agnx_desc) * priv->rx.size;
+	priv->txd.desc = priv->txm.desc + priv->txm.size;
+	priv->txd.dma = priv->txm.dma + sizeof(struct agnx_desc) * priv->txm.size;
+
+	return 0;
+} /* agnx_alloc_rings */
+
+static void rings_free(struct agnx_priv *priv)
+{
+	unsigned int len = priv->rx.size + priv->txm.size + priv->txd.size;
+	unsigned long flags;
+	AGNX_TRACE;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	kfree(priv->rx.info);
+	pci_free_consistent(priv->pdev, sizeof(struct agnx_desc) * len,
+			    priv->rx.desc, priv->rx.dma);
+	spin_unlock_irqrestore(&priv->lock, flags);
+}
+
+#if 0
+static void agnx_periodic_work_handler(struct work_struct *work)
+{
+	struct agnx_priv *priv = container_of(work, struct agnx_priv,
+                                             periodic_work.work);
+//	unsigned long flags;
+	unsigned long delay;
+
+	/* fixme: using mutex?? */
+//	spin_lock_irqsave(&priv->lock, flags);
+
+	/* TODO Recalibrate*/
+//	calibrate_oscillator(priv);
+//	antenna_calibrate(priv);
+//	agnx_send_packet(priv, 997);
+	/* FIXME */
+/* 	if (debug == 3) */
+/*                 delay = msecs_to_jiffies(AGNX_PERIODIC_DELAY); */
+/* 	else */
+	delay = msecs_to_jiffies(AGNX_PERIODIC_DELAY);
+//		delay = round_jiffies(HZ * 15);
+
+	queue_delayed_work(priv->hw->workqueue, &priv->periodic_work, delay);
+
+//	spin_unlock_irqrestore(&priv->lock, flags);
+}
+#endif
+
+static int agnx_start(struct ieee80211_hw *dev)
+{
+	struct agnx_priv *priv = dev->priv;
+	/* unsigned long delay; */
+	int err = 0;
+	AGNX_TRACE;
+
+	err = agnx_alloc_rings(priv);
+	if (err) {
+		printk(KERN_ERR PFX "Can't alloc RX/TXM/TXD rings\n");
+		goto out;
+	}
+	err = request_irq(priv->pdev->irq, &agnx_interrupt_handler,
+			  IRQF_SHARED, "agnx_pci", dev);
+	if (err) {
+		printk(KERN_ERR PFX "Failed to register IRQ handler\n");
+		rings_free(priv);
+		goto out;
+	}
+
+//	mdelay(500);
+
+	might_sleep();
+	agnx_hw_init(priv);
+
+//	mdelay(500);
+	might_sleep();
+
+	priv->init_status = AGNX_START;
+/*         INIT_DELAYED_WORK(&priv->periodic_work, agnx_periodic_work_handler); */
+/* 	delay = msecs_to_jiffies(AGNX_PERIODIC_DELAY); */
+/*         queue_delayed_work(priv->hw->workqueue, &priv->periodic_work, delay); */
+out:
+	return err;
+} /* agnx_start */
+
+static void agnx_stop(struct ieee80211_hw *dev)
+{
+	struct agnx_priv *priv = dev->priv;
+	AGNX_TRACE;
+
+	priv->init_status = AGNX_STOP;
+	/* make sure hardware will not generate irq */
+	agnx_hw_reset(priv);
+	free_irq(priv->pdev->irq, dev);
+        flush_workqueue(priv->hw->workqueue);
+//	cancel_delayed_work_sync(&priv->periodic_work);
+	unfill_rings(priv);
+	rings_free(priv);
+}
+
+static int agnx_config(struct ieee80211_hw *dev,
+		       struct ieee80211_conf *conf)
+{
+	struct agnx_priv *priv = dev->priv;
+	int channel = ieee80211_frequency_to_channel(conf->channel->center_freq);
+	AGNX_TRACE;
+
+	spin_lock(&priv->lock);
+	/* FIXME need priv lock? */
+	if (channel != priv->channel) {
+		priv->channel = channel;
+		agnx_set_channel(priv, priv->channel);
+	}
+
+	spin_unlock(&priv->lock);
+	return 0;
+}
+
+static int agnx_config_interface(struct ieee80211_hw *dev,
+				 struct ieee80211_vif *vif,
+				 struct ieee80211_if_conf *conf)
+{
+	struct agnx_priv *priv = dev->priv;
+	void __iomem *ctl = priv->ctl;
+	AGNX_TRACE;
+
+	spin_lock(&priv->lock);
+
+	if (memcmp(conf->bssid, priv->bssid, ETH_ALEN)) {
+//		u32 reghi, reglo;
+		agnx_set_bssid(priv, conf->bssid);
+		memcpy(priv->bssid, conf->bssid, ETH_ALEN);
+		hash_write(priv, conf->bssid, BSSID_STAID);
+		sta_init(priv, BSSID_STAID);
+		/* FIXME needed? */
+		sta_power_init(priv, BSSID_STAID);
+		agnx_write32(ctl, AGNX_BM_MTSM, 0xff & ~0x1);
+	}
+	spin_unlock(&priv->lock);
+	return 0;
+} /* agnx_config_interface */
+
+
+static void agnx_configure_filter(struct ieee80211_hw *dev,
+				  unsigned int changed_flags,
+				  unsigned int *total_flags,
+				  int mc_count, struct dev_mc_list *mclist)
+{
+	unsigned int new_flags = 0;
+
+	*total_flags = new_flags;
+	/* TODO */
+}
+
+static int agnx_add_interface(struct ieee80211_hw *dev,
+			      struct ieee80211_if_init_conf *conf)
+{
+	struct agnx_priv *priv = dev->priv;
+	AGNX_TRACE;
+
+	spin_lock(&priv->lock);
+	/* FIXME */
+	if (priv->mode != NL80211_IFTYPE_MONITOR)
+		return -EOPNOTSUPP;
+
+	switch (conf->type) {
+	case NL80211_IFTYPE_STATION:
+		priv->mode = conf->type;
+		break;
+	default:
+		return -EOPNOTSUPP;
+	}
+
+	spin_unlock(&priv->lock);
+
+	return 0;
+}
+
+static void agnx_remove_interface(struct ieee80211_hw *dev,
+				  struct ieee80211_if_init_conf *conf)
+{
+	struct agnx_priv *priv = dev->priv;
+	AGNX_TRACE;
+
+	/* TODO */
+	priv->mode = NL80211_IFTYPE_MONITOR;
+}
+
+static int agnx_get_stats(struct ieee80211_hw *dev,
+			  struct ieee80211_low_level_stats *stats)
+{
+	struct agnx_priv *priv = dev->priv;
+	AGNX_TRACE;
+	spin_lock(&priv->lock);
+	/* TODO !! */
+	memcpy(stats, &priv->stats, sizeof(*stats));
+	spin_unlock(&priv->lock);
+
+	return 0;
+}
+
+static u64 agnx_get_tsft(struct ieee80211_hw *dev)
+{
+	void __iomem *ctl = ((struct agnx_priv *)dev->priv)->ctl;
+	u32 tsftl;
+	u64 tsft;
+	AGNX_TRACE;
+
+	/* FIXME */
+	tsftl = ioread32(ctl + AGNX_TXM_TIMESTAMPLO);
+	tsft = ioread32(ctl + AGNX_TXM_TIMESTAMPHI);
+	tsft <<= 32;
+	tsft |= tsftl;
+
+	return tsft;
+}
+
+static int agnx_get_tx_stats(struct ieee80211_hw *dev,
+			     struct ieee80211_tx_queue_stats *stats)
+{
+	struct agnx_priv *priv = dev->priv;
+	AGNX_TRACE;
+
+	/* FIXME now we just using txd queue, but should using txm queue too */
+	stats[0].len = (priv->txd.idx - priv->txd.idx_sent) / 2;
+	stats[0].limit = priv->txd.size - 2;
+	stats[0].count = priv->txd.idx / 2;
+
+	return 0;
+}
+
+static struct ieee80211_ops agnx_ops = {
+	.tx			= agnx_tx,
+	.start			= agnx_start,
+	.stop			= agnx_stop,
+	.add_interface		= agnx_add_interface,
+	.remove_interface	= agnx_remove_interface,
+	.config			= agnx_config,
+	.config_interface	= agnx_config_interface,
+ 	.configure_filter	= agnx_configure_filter,
+	.get_stats		= agnx_get_stats,
+	.get_tx_stats		= agnx_get_tx_stats,
+	.get_tsf		= agnx_get_tsft
+};
+
+static void __devexit agnx_pci_remove(struct pci_dev *pdev)
+{
+	struct ieee80211_hw *dev = pci_get_drvdata(pdev);
+	struct agnx_priv *priv = dev->priv;
+	AGNX_TRACE;
+
+	if (!dev)
+		return;
+	ieee80211_unregister_hw(dev);
+	pci_iounmap(pdev, priv->ctl);
+	pci_iounmap(pdev, priv->data);
+	pci_release_regions(pdev);
+	pci_disable_device(pdev);
+
+	ieee80211_free_hw(dev);
+}
+
+static int __devinit agnx_pci_probe(struct pci_dev *pdev,
+				    const struct pci_device_id *id)
+{
+	struct ieee80211_hw *dev;
+	struct agnx_priv *priv;
+	u32 mem_addr0, mem_len0;
+	u32 mem_addr1, mem_len1;
+	int err;
+	DECLARE_MAC_BUF(mac);
+
+	err = pci_enable_device(pdev);
+	if (err) {
+		printk(KERN_ERR PFX "Can't enable new PCI device\n");
+		return err;
+	}
+
+	/* get pci resource */
+	mem_addr0 = pci_resource_start(pdev, 0);
+	mem_len0 = pci_resource_len(pdev, 0);
+	mem_addr1 = pci_resource_start(pdev, 1);
+	mem_len1 = pci_resource_len(pdev, 1);
+	printk(KERN_DEBUG PFX "Memaddr0 is %x, length is %x\n", mem_addr0, mem_len0);
+	printk(KERN_DEBUG PFX "Memaddr1 is %x, length is %x\n", mem_addr1, mem_len1);
+
+	err = pci_request_regions(pdev, "agnx-pci");
+	if (err) {
+		printk(KERN_ERR PFX "Can't obtain PCI resource\n");
+		return err;
+	}
+
+	if (pci_set_dma_mask(pdev, DMA_32BIT_MASK) ||
+	    pci_set_consistent_dma_mask(pdev, DMA_32BIT_MASK)) {
+		printk(KERN_ERR PFX "No suitable DMA available\n");
+		goto err_free_reg;
+	}
+
+	pci_set_master(pdev);
+	printk(KERN_DEBUG PFX "pdev->irq is %d\n", pdev->irq);
+
+	dev = ieee80211_alloc_hw(sizeof(*priv), &agnx_ops);
+	if (!dev) {
+		printk(KERN_ERR PFX "ieee80211 alloc failed\n");
+		err = -ENOMEM;
+		goto err_free_reg;
+	}
+	/* init priv  */
+	priv = dev->priv;
+	memset(priv, 0, sizeof(*priv));
+	priv->mode = NL80211_IFTYPE_MONITOR;
+	priv->pdev = pdev;
+	priv->hw = dev;
+	spin_lock_init(&priv->lock);
+	priv->init_status = AGNX_UNINIT;
+
+	/* Map mem #1 and #2 */
+	priv->ctl = pci_iomap(pdev, 0, mem_len0);
+//	printk(KERN_DEBUG PFX"MEM1 mapped address is 0x%p\n", priv->ctl);
+	if (!priv->ctl) {
+		printk(KERN_ERR PFX "Can't map device memory\n");
+		goto err_free_dev;
+	}
+	priv->data = pci_iomap(pdev, 1, mem_len1);
+	printk(KERN_DEBUG PFX "MEM2 mapped address is 0x%p\n", priv->data);
+	if (!priv->data) {
+		printk(KERN_ERR PFX "Can't map device memory\n");
+		goto err_iounmap2;
+	}
+
+	pci_read_config_byte(pdev, PCI_REVISION_ID, &priv->revid);
+
+	priv->band.channels   = (struct ieee80211_channel *)agnx_channels;
+	priv->band.n_channels = ARRAY_SIZE(agnx_channels);
+	priv->band.bitrates   = (struct ieee80211_rate *)agnx_rates_80211g;
+	priv->band.n_bitrates = ARRAY_SIZE(agnx_rates_80211g);
+
+	/* Init ieee802.11 dev  */
+	SET_IEEE80211_DEV(dev, &pdev->dev);
+	pci_set_drvdata(pdev, dev);
+	dev->extra_tx_headroom = sizeof(struct agnx_hdr);
+
+	/* FIXME It only include FCS in promious mode but not manage mode */
+/*      dev->flags =  IEEE80211_HW_RX_INCLUDES_FCS; */
+	dev->channel_change_time = 5000;
+	dev->max_signal = 100;
+	/* FIXME */
+	dev->queues = 1;
+
+	agnx_get_mac_address(priv);
+
+	SET_IEEE80211_PERM_ADDR(dev, priv->mac_addr);
+
+/* 	/\* FIXME *\/ */
+/* 	for (i = 1; i < NUM_DRIVE_MODES; i++) { */
+/* 		err = ieee80211_register_hwmode(dev, &priv->modes[i]); */
+/* 		if (err) { */
+/* 			printk(KERN_ERR PFX "Can't register hwmode\n"); */
+/* 			goto  err_iounmap; */
+/* 		} */
+/* 	} */
+
+	priv->channel = 1;
+	dev->wiphy->bands[IEEE80211_BAND_2GHZ] = &priv->band;
+
+	err = ieee80211_register_hw(dev);
+	if (err) {
+		printk(KERN_ERR PFX "Can't register hardware\n");
+		goto err_iounmap;
+	}
+
+	agnx_hw_reset(priv);
+
+
+	printk(PFX "%s: hwaddr %s, Rev 0x%02x\n", wiphy_name(dev->wiphy),
+	       print_mac(mac, dev->wiphy->perm_addr), priv->revid);
+	return 0;
+
+ err_iounmap:
+	pci_iounmap(pdev, priv->data);
+
+ err_iounmap2:
+	pci_iounmap(pdev, priv->ctl);
+
+ err_free_dev:
+	pci_set_drvdata(pdev, NULL);
+	ieee80211_free_hw(dev);
+
+ err_free_reg:
+	pci_release_regions(pdev);
+
+	pci_disable_device(pdev);
+	return err;
+} /* agnx_pci_probe*/
+
+#ifdef CONFIG_PM
+
+static int agnx_pci_suspend(struct pci_dev *pdev, pm_message_t state)
+{
+	struct ieee80211_hw *dev = pci_get_drvdata(pdev);
+	AGNX_TRACE;
+
+	ieee80211_stop_queues(dev);
+	agnx_stop(dev);
+
+	pci_save_state(pdev);
+	pci_set_power_state(pdev, pci_choose_state(pdev, state));
+	return 0;
+}
+
+static int agnx_pci_resume(struct pci_dev *pdev)
+{
+	struct ieee80211_hw *dev = pci_get_drvdata(pdev);
+	AGNX_TRACE;
+
+	pci_set_power_state(pdev, PCI_D0);
+	pci_restore_state(pdev);
+
+	agnx_start(dev);
+	ieee80211_wake_queues(dev);
+
+	return 0;
+}
+
+#else
+
+#define agnx_pci_suspend NULL
+#define agnx_pci_resume NULL
+
+#endif /* CONFIG_PM */
+
+
+static struct pci_driver agnx_pci_driver = {
+	.name		= "agnx-pci",
+	.id_table	= agnx_pci_id_tbl,
+	.probe		= agnx_pci_probe,
+	.remove		= __devexit_p(agnx_pci_remove),
+	.suspend	= agnx_pci_suspend,
+	.resume		= agnx_pci_resume,
+};
+
+static int __init agnx_pci_init(void)
+{
+	AGNX_TRACE;
+	return pci_register_driver(&agnx_pci_driver);
+}
+
+static void __exit agnx_pci_exit(void)
+{
+	AGNX_TRACE;
+	pci_unregister_driver(&agnx_pci_driver);
+}
+
+
+module_init(agnx_pci_init);
+module_exit(agnx_pci_exit);
diff --git a/drivers/staging/agnx/phy.c b/drivers/staging/agnx/phy.c
new file mode 100644
index 00000000000..da8f10c0838
--- /dev/null
+++ b/drivers/staging/agnx/phy.c
@@ -0,0 +1,960 @@
+/**
+ * Airgo MIMO wireless driver
+ *
+ * Copyright (c) 2007 Li YanBo <dreamfly281@gmail.com>
+
+ * Thanks for Jeff Williams <angelbane@gmail.com> do reverse engineer
+ * works and published the SPECS at http://airgo.wdwconsulting.net/mymoin
+
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/init.h>
+#include <linux/etherdevice.h>
+#include <linux/pci.h>
+#include <linux/delay.h>
+#include "agnx.h"
+#include "debug.h"
+#include "phy.h"
+#include "table.h"
+#include "sta.h"
+#include "xmit.h"
+
+u8 read_from_eeprom(struct agnx_priv *priv, u16 address)
+{
+	void __iomem *ctl = priv->ctl;
+	struct agnx_eeprom cmd;
+	u32 reg;
+
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.cmd = EEPROM_CMD_READ << AGNX_EEPROM_COMMAND_SHIFT;
+	cmd.address = address;
+	/* Verify that the Status bit is clear */
+	/* Read Command and Address are written to the Serial Interface */
+	iowrite32(*(__le32 *)&cmd, ctl + AGNX_CIR_SERIALITF);
+	/* Wait for the Status bit to clear again */
+	eeprom_delay();
+	/* Read from Data */
+	reg = ioread32(ctl + AGNX_CIR_SERIALITF);
+
+	cmd = *(struct agnx_eeprom *)&reg;
+
+	return cmd.data;
+}
+
+static int card_full_reset(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+	AGNX_TRACE;
+
+	reg = agnx_read32(ctl, AGNX_CIR_BLKCTL);
+	agnx_write32(ctl, AGNX_CIR_BLKCTL, 0x80);
+	reg = agnx_read32(ctl, AGNX_CIR_BLKCTL);
+	return 0;
+}
+
+inline void enable_power_saving(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+
+	reg = agnx_read32(ctl, AGNX_PM_PMCTL);
+	reg &= ~0x8;
+	agnx_write32(ctl, AGNX_PM_PMCTL, reg);
+}
+
+inline void disable_power_saving(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+
+	reg = agnx_read32(ctl, AGNX_PM_PMCTL);
+	reg |= 0x8;
+	agnx_write32(ctl, AGNX_PM_PMCTL, reg);
+}
+
+
+void disable_receiver(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	AGNX_TRACE;
+
+	/* FIXME Disable the receiver */
+	agnx_write32(ctl, AGNX_GCR_DISCOVMOD, 0x0);
+	/* Set gain control reset */
+	agnx_write32(ctl, AGNX_GCR_RSTGCTL, 0x1);
+	/* Reset gain control reset */
+	agnx_write32(ctl, AGNX_GCR_RSTGCTL, 0x0);
+}
+
+
+/* Fixme this shoule be disable RX, above is enable RX */
+void enable_receiver(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	AGNX_TRACE;
+
+	/* Set adaptive gain control discovery mode */
+	agnx_write32(ctl, AGNX_GCR_DISCOVMOD, 0x3);
+	/* Set gain control reset */
+	agnx_write32(ctl, AGNX_GCR_RSTGCTL, 0x1);
+	/* Clear gain control reset */
+	agnx_write32(ctl, AGNX_GCR_RSTGCTL, 0x0);
+}
+
+static void mac_address_set(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	u8 *mac_addr = priv->mac_addr;
+	u32 reg;
+
+	/* FIXME */
+	reg = (mac_addr[0] << 24) | (mac_addr[1] << 16) | mac_addr[2] << 8 | mac_addr[3];
+	iowrite32(reg, ctl + AGNX_RXM_MACHI);
+ 	reg = (mac_addr[4] << 8) | mac_addr[5];
+	iowrite32(reg, ctl + AGNX_RXM_MACLO);
+}
+
+static void receiver_bssid_set(struct agnx_priv *priv, u8 *bssid)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+
+	disable_receiver(priv);
+	/* FIXME */
+	reg = bssid[0] << 24 | (bssid[1] << 16) | (bssid[2] << 8) | bssid[3];
+	iowrite32(reg, ctl + AGNX_RXM_BSSIDHI);
+ 	reg = (bssid[4] << 8) | bssid[5];
+	iowrite32(reg, ctl + AGNX_RXM_BSSIDLO);
+
+	/* Enable the receiver */
+	enable_receiver(priv);
+
+	/* Clear the TSF */
+/* 	agnx_write32(ctl, AGNX_TXM_TSFLO, 0x0); */
+/* 	agnx_write32(ctl, AGNX_TXM_TSFHI, 0x0); */
+	/* Clear the TBTT */
+	agnx_write32(ctl, AGNX_TXM_TBTTLO, 0x0);
+	agnx_write32(ctl, AGNX_TXM_TBTTHI, 0x0);
+	disable_receiver(priv);
+} /* receiver_bssid_set */
+
+static void band_management_init(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	void __iomem *data = priv->data;
+	u32 reg;
+	int i;
+	AGNX_TRACE;
+
+	agnx_write32(ctl, AGNX_BM_TXWADDR, AGNX_PDU_TX_WQ);
+	agnx_write32(ctl, AGNX_CIR_ADDRWIN, 0x0);
+	memset_io(data + AGNX_PDUPOOL, 0x0, AGNX_PDUPOOL_SIZE);
+	agnx_write32(ctl, AGNX_BM_BMCTL, 0x200);
+
+	agnx_write32(ctl, AGNX_BM_CIPDUWCNT, 0x40);
+	agnx_write32(ctl, AGNX_BM_SPPDUWCNT, 0x2);
+	agnx_write32(ctl, AGNX_BM_RFPPDUWCNT, 0x0);
+	agnx_write32(ctl, AGNX_BM_RHPPDUWCNT, 0x22);
+
+	/* FIXME Initialize the Free Pool Linked List */
+	/*    1. Write the Address of the Next Node ((0x41800 + node*size)/size)
+	      to the first word of each node.  */
+	for (i = 0; i < PDU_FREE_CNT; i++) {
+		iowrite32((AGNX_PDU_FREE + (i+1)*PDU_SIZE)/PDU_SIZE,
+			  data + AGNX_PDU_FREE + (PDU_SIZE * i));
+		/* The last node should be set to 0x0 */
+		if ((i + 1) == PDU_FREE_CNT)
+			memset_io(data + AGNX_PDU_FREE + (PDU_SIZE * i),
+				  0x0, PDU_SIZE);
+	}
+
+	/* Head is First Pool address (0x41800) / size (0x80) */
+	agnx_write32(ctl, AGNX_BM_FPLHP, AGNX_PDU_FREE/PDU_SIZE);
+	/* Tail is Last Pool Address (0x47f80) / size (0x80) */
+	agnx_write32(ctl, AGNX_BM_FPLTP, 0x47f80/PDU_SIZE);
+	/* Count is Number of Nodes in the Pool (0xd0) */
+	agnx_write32(ctl, AGNX_BM_FPCNT, PDU_FREE_CNT);
+
+	/* Start all workqueue */
+	agnx_write32(ctl, AGNX_BM_CIWQCTL, 0x80000);
+	agnx_write32(ctl, AGNX_BM_CPULWCTL, 0x80000);
+	agnx_write32(ctl, AGNX_BM_CPUHWCTL, 0x80000);
+	agnx_write32(ctl, AGNX_BM_CPUTXWCTL, 0x80000);
+	agnx_write32(ctl, AGNX_BM_CPURXWCTL, 0x80000);
+	agnx_write32(ctl, AGNX_BM_SPRXWCTL, 0x80000);
+	agnx_write32(ctl, AGNX_BM_SPTXWCTL, 0x80000);
+	agnx_write32(ctl, AGNX_BM_RFPWCTL, 0x80000);
+
+	/* Enable the Band Management */
+	reg = agnx_read32(ctl, AGNX_BM_BMCTL);
+	reg |= 0x1;
+	agnx_write32(ctl, AGNX_BM_BMCTL, reg);
+} /* band_managment_init */
+
+
+static void system_itf_init(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+	AGNX_TRACE;
+
+	agnx_write32(ctl, AGNX_SYSITF_GPIOUT, 0x0);
+	agnx_write32(ctl, AGNX_PM_TESTPHY, 0x11e143a);
+
+	if (priv->revid == 0) {
+		reg = agnx_read32(ctl, AGNX_SYSITF_SYSMODE);
+		reg |= 0x11;
+		agnx_write32(ctl, AGNX_SYSITF_SYSMODE, reg);
+	}
+	/* ??? What is that means? it should difference for differice type
+	 of cards */
+	agnx_write32(ctl, AGNX_CIR_SERIALITF, 0xfff81006);
+
+	agnx_write32(ctl, AGNX_SYSITF_GPIOIN, 0x1f0000);
+	agnx_write32(ctl, AGNX_SYSITF_GPIOUT, 0x5);
+	reg = agnx_read32(ctl, AGNX_SYSITF_GPIOIN);
+}
+
+static void encryption_init(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	AGNX_TRACE;
+
+	agnx_write32(ctl, AGNX_ENCRY_WEPKEY0, 0x0);
+	agnx_write32(ctl, AGNX_ENCRY_WEPKEY1, 0x0);
+	agnx_write32(ctl, AGNX_ENCRY_WEPKEY2, 0x0);
+	agnx_write32(ctl, AGNX_ENCRY_WEPKEY3, 0x0);
+	agnx_write32(ctl, AGNX_ENCRY_CCMRECTL, 0x8);
+}
+
+static void tx_management_init(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	void __iomem *data = priv->data;
+	u32 reg;
+	AGNX_TRACE;
+
+	/* Fill out the ComputationalEngineLookupTable
+	 * starting at memory #2 offset 0x800
+	 */
+	tx_engine_lookup_tbl_init(priv);
+	memset_io(data + 0x1000, 0, 0xfe0);
+	/* Enable Transmission Management Functions */
+	agnx_write32(ctl, AGNX_TXM_ETMF, 0x3ff);
+	/* Write 0x3f to Transmission Template */
+	agnx_write32(ctl, AGNX_TXM_TXTEMP, 0x3f);
+
+	if (priv->revid >= 2)
+		agnx_write32(ctl, AGNX_TXM_SIFSPIFS, 0x1e140a0b);
+	else
+		agnx_write32(ctl, AGNX_TXM_SIFSPIFS, 0x1e190a0b);
+
+	reg = agnx_read32(ctl, AGNX_TXM_TIFSEIFS);
+	reg &= 0xff00;
+	reg |= 0xb;
+	agnx_write32(ctl, AGNX_TXM_TIFSEIFS, reg);
+	reg = agnx_read32(ctl, AGNX_TXM_TIFSEIFS);
+	reg &= 0xffff00ff;
+	reg |= 0xa00;
+	agnx_write32(ctl, AGNX_TXM_TIFSEIFS, reg);
+	/* Enable TIFS */
+	agnx_write32(ctl, AGNX_TXM_CTL, 0x40000);
+
+	reg = agnx_read32(ctl, AGNX_TXM_TIFSEIFS);
+	reg &= 0xff00ffff;
+	reg |= 0x510000;
+	agnx_write32(ctl, AGNX_TXM_TIFSEIFS, reg);
+	reg = agnx_read32(ctl, AGNX_TXM_PROBDELAY);
+	reg &= 0xff00ffff;
+	agnx_write32(ctl, AGNX_TXM_PROBDELAY, reg);
+	reg = agnx_read32(ctl, AGNX_TXM_TIFSEIFS);
+	reg &= 0x00ffffff;
+	reg |= 0x1c000000;
+	agnx_write32(ctl, AGNX_TXM_TIFSEIFS, reg);
+	reg = agnx_read32(ctl, AGNX_TXM_PROBDELAY);
+	reg &= 0x00ffffff;
+	reg |= 0x01000000;
+	agnx_write32(ctl, AGNX_TXM_PROBDELAY, reg);
+
+	/* # Set DIF 0-1,2-3,4-5,6-7 to defaults */
+	agnx_write32(ctl, AGNX_TXM_DIF01, 0x321d321d);
+	agnx_write32(ctl, AGNX_TXM_DIF23, 0x321d321d);
+	agnx_write32(ctl, AGNX_TXM_DIF45, 0x321d321d);
+	agnx_write32(ctl, AGNX_TXM_DIF67, 0x321d321d);
+
+	/* Max Ack timeout limit */
+	agnx_write32(ctl, AGNX_TXM_MAXACKTIM, 0x1e19);
+	/* Max RX Data Timeout count, */
+	reg = agnx_read32(ctl, AGNX_TXM_MAXRXTIME);
+	reg &= 0xffff0000;
+	reg |= 0xff;
+	agnx_write32(ctl, AGNX_TXM_MAXRXTIME, reg);
+
+	/* CF poll RX Timeout count */
+	reg = agnx_read32(ctl, AGNX_TXM_CFPOLLRXTIM);
+	reg &= 0xffff;
+	reg |= 0xff0000;
+	agnx_write32(ctl, AGNX_TXM_CFPOLLRXTIM, reg);
+
+	/* Max Timeout Exceeded count, */
+	reg = agnx_read32(ctl, AGNX_TXM_MAXTIMOUT);
+	reg &= 0xff00ffff;
+	reg |= 0x190000;
+	agnx_write32(ctl, AGNX_TXM_MAXTIMOUT, reg);
+
+	/* CF ack timeout limit for 11b */
+	reg = agnx_read32(ctl, AGNX_TXM_CFACKT11B);
+	reg &= 0xff00;
+	reg |= 0x1e;
+	agnx_write32(ctl, AGNX_TXM_CFACKT11B, reg);
+
+	/* Max CF Poll Timeout Count */
+	reg = agnx_read32(ctl, AGNX_TXM_CFPOLLRXTIM);
+	reg &= 0xffff0000;
+	reg |= 0x19;
+	agnx_write32(ctl, AGNX_TXM_CFPOLLRXTIM, reg);
+	/* CF Poll RX Timeout Count */
+	reg = agnx_read32(ctl, AGNX_TXM_CFPOLLRXTIM);
+	reg &= 0xffff0000;
+	reg |= 0x1e;
+	agnx_write32(ctl, AGNX_TXM_CFPOLLRXTIM, reg);
+
+	/* # write default to */
+	/*    1. Schedule Empty Count */
+	agnx_write32(ctl, AGNX_TXM_SCHEMPCNT, 0x5);
+	/*    2. CFP Period Count */
+	agnx_write32(ctl, AGNX_TXM_CFPERCNT, 0x1);
+	/*    3. CFP MDV  */
+	agnx_write32(ctl, AGNX_TXM_CFPMDV, 0x10000);
+
+	/* Probe Delay */
+	reg = agnx_read32(ctl, AGNX_TXM_PROBDELAY);
+	reg &= 0xffff0000;
+	reg |= 0x400;
+	agnx_write32(ctl, AGNX_TXM_PROBDELAY, reg);
+
+	/* Max CCA count Slot */
+	reg = agnx_read32(ctl, AGNX_TXM_MAXCCACNTSLOT);
+	reg &= 0xffff00ff;
+	reg |= 0x900;
+	agnx_write32(ctl, AGNX_TXM_MAXCCACNTSLOT, reg);
+
+	/* Slot limit/1 msec Limit */
+	reg = agnx_read32(ctl, AGNX_TXM_SLOTLIMIT);
+	reg &= 0xff00ffff;
+	reg |= 0x140077;
+	agnx_write32(ctl, AGNX_TXM_SLOTLIMIT, reg);
+
+	/* # Set CW #(0-7) to default */
+	agnx_write32(ctl, AGNX_TXM_CW0, 0xff0007);
+	agnx_write32(ctl, AGNX_TXM_CW1, 0xff0007);
+	agnx_write32(ctl, AGNX_TXM_CW2, 0xff0007);
+	agnx_write32(ctl, AGNX_TXM_CW3, 0xff0007);
+	agnx_write32(ctl, AGNX_TXM_CW4, 0xff0007);
+	agnx_write32(ctl, AGNX_TXM_CW5, 0xff0007);
+	agnx_write32(ctl, AGNX_TXM_CW6, 0xff0007);
+	agnx_write32(ctl, AGNX_TXM_CW7, 0xff0007);
+
+	/* # Set Short/Long limit #(0-7) to default */
+	agnx_write32(ctl, AGNX_TXM_SLBEALIM0,  0xa000a);
+	agnx_write32(ctl, AGNX_TXM_SLBEALIM1,  0xa000a);
+	agnx_write32(ctl, AGNX_TXM_SLBEALIM2,  0xa000a);
+	agnx_write32(ctl, AGNX_TXM_SLBEALIM3,  0xa000a);
+	agnx_write32(ctl, AGNX_TXM_SLBEALIM4,  0xa000a);
+	agnx_write32(ctl, AGNX_TXM_SLBEALIM5,  0xa000a);
+	agnx_write32(ctl, AGNX_TXM_SLBEALIM6,  0xa000a);
+	agnx_write32(ctl, AGNX_TXM_SLBEALIM7,  0xa000a);
+
+	reg = agnx_read32(ctl, AGNX_TXM_CTL);
+	reg |= 0x1400;
+	agnx_write32(ctl, AGNX_TXM_CTL, reg);
+	/* Wait for bit 0 in Control Reg to clear  */
+	udelay(80);
+	reg = agnx_read32(ctl, AGNX_TXM_CTL);
+	/* Or 0x18000 to Control reg */
+	reg = agnx_read32(ctl, AGNX_TXM_CTL);
+	reg |= 0x18000;
+	agnx_write32(ctl, AGNX_TXM_CTL, reg);
+	/* Wait for bit 0 in Control Reg to clear */
+	udelay(80);
+	reg = agnx_read32(ctl, AGNX_TXM_CTL);
+
+	/* Set Listen Interval Count to default */
+	agnx_write32(ctl, AGNX_TXM_LISINTERCNT, 0x1);
+	/* Set DTIM period count to default */
+	agnx_write32(ctl, AGNX_TXM_DTIMPERICNT, 0x2000);
+} /* tx_management_init */
+
+static void rx_management_init(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	AGNX_TRACE;
+
+	/* Initialize the Routing Table */
+	routing_table_init(priv);
+
+	if (priv->revid >= 3) {
+		agnx_write32(ctl, 0x2074, 0x1f171710);
+		agnx_write32(ctl, 0x2078, 0x10100d0d);
+		agnx_write32(ctl, 0x207c, 0x11111010);
+	}
+	else
+		agnx_write32(ctl, AGNX_RXM_DELAY11, 0x0);
+	agnx_write32(ctl, AGNX_RXM_REQRATE, 0x8195e00);
+}
+
+
+static void agnx_timer_init(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	AGNX_TRACE;
+
+/* 	/\* Write 0x249f00 (tick duration?) to Timer 1 *\/ */
+/* 	agnx_write32(ctl, AGNX_TIMCTL_TIMER1, 0x249f00); */
+/* 	/\* Write 0xe2 to Timer 1 Control *\/ */
+/* 	agnx_write32(ctl, AGNX_TIMCTL_TIM1CTL, 0xe2); */
+
+	/* Write 0x249f00 (tick duration?) to Timer 1 */
+	agnx_write32(ctl, AGNX_TIMCTL_TIMER1, 0x0);
+	/* Write 0xe2 to Timer 1 Control */
+	agnx_write32(ctl, AGNX_TIMCTL_TIM1CTL, 0x0);
+
+	iowrite32(0xFFFFFFFF, priv->ctl + AGNX_TXM_BEACON_CTL);
+}
+
+static void power_manage_init(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+	AGNX_TRACE;
+
+	agnx_write32(ctl, AGNX_PM_MACMSW, 0x1f);
+	agnx_write32(ctl, AGNX_PM_RFCTL, 0x1f);
+
+	reg = agnx_read32(ctl, AGNX_PM_PMCTL);
+	reg &= 0xf00f;
+	reg |= 0xa0;
+	agnx_write32(ctl, AGNX_PM_PMCTL, reg);
+
+	if (priv->revid >= 3) {
+		reg = agnx_read32(ctl, AGNX_PM_SOFTRST);
+		reg |= 0x18;
+		agnx_write32(ctl, AGNX_PM_SOFTRST, reg);
+	}
+} /* power_manage_init */
+
+
+static void gain_ctlcnt_init(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+	AGNX_TRACE;
+
+	agnx_write32(ctl, AGNX_GCR_TRACNT5, 0x119);
+	agnx_write32(ctl, AGNX_GCR_TRACNT6, 0x118);
+	agnx_write32(ctl, AGNX_GCR_TRACNT7, 0x117);
+
+	reg = agnx_read32(ctl, AGNX_PM_PMCTL);
+	reg |= 0x8;
+	agnx_write32(ctl, AGNX_PM_PMCTL, reg);
+
+	reg = agnx_read32(ctl, AGNX_PM_PMCTL);
+	reg &= ~0x8;
+	agnx_write32(ctl, AGNX_PM_PMCTL, reg);
+
+	agnx_write32(ctl, AGNX_CIR_ADDRWIN, 0x0);
+
+	/* FIXME Write the initial Station Descriptor for the card */
+	sta_init(priv, LOCAL_STAID);
+	sta_init(priv, BSSID_STAID);
+
+	/* Enable staion 0 and 1 can do TX */
+	/* It seemed if we set other bit to 1 the bit 0 will
+	   be auto change to 0 */
+	agnx_write32(ctl, AGNX_BM_TXTOPEER, 0x2 | 0x1);
+//	agnx_write32(ctl, AGNX_BM_TXTOPEER, 0x1);
+} /* gain_ctlcnt_init */
+
+
+static void phy_init(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	void __iomem *data = priv->data;
+	u32 reg;
+	AGNX_TRACE;
+
+	/* Load InitialGainTable */
+	gain_table_init(priv);
+
+  	agnx_write32(ctl, AGNX_CIR_ADDRWIN, 0x2000000);
+
+	/* Clear the following offsets in Memory Range #2: */
+	memset_io(data + 0x5040, 0, 0xa * 4);
+	memset_io(data + 0x5080, 0, 0xa * 4);
+	memset_io(data + 0x50c0, 0, 0xa * 4);
+	memset_io(data + 0x5400, 0, 0x80 * 4);
+	memset_io(data + 0x6000, 0, 0x280 * 4);
+	memset_io(data + 0x7000, 0, 0x280 * 4);
+	memset_io(data + 0x8000, 0, 0x280 * 4);
+
+	/* Initialize the Following Registers According to PCI Revision ID */
+	if (priv->revid == 0) {
+		/* fixme the part hasn't been update but below has been update
+		   based on WGM511 */
+		agnx_write32(ctl, AGNX_ACI_LEN, 0xf);
+		agnx_write32(ctl, AGNX_ACI_TIMER1, 0x1d);
+		agnx_write32(ctl, AGNX_ACI_TIMER2, 0x3);
+		agnx_write32(ctl, AGNX_ACI_AICCHA0OVE, 0x11);
+		agnx_write32(ctl, AGNX_ACI_AICCHA1OVE, 0x0);
+		agnx_write32(ctl, AGNX_GCR_THD0A, 0x64);
+		agnx_write32(ctl, AGNX_GCR_THD0AL, 0x4b);
+		agnx_write32(ctl, AGNX_GCR_THD0B, 0x4b);
+		agnx_write32(ctl, AGNX_GCR_DUNSAT, 0x14);
+		agnx_write32(ctl, AGNX_GCR_DSAT, 0x24);
+		agnx_write32(ctl, AGNX_GCR_DFIRCAL, 0x8);
+		agnx_write32(ctl, AGNX_GCR_DGCTL11A, 0x1a);
+		agnx_write32(ctl, AGNX_GCR_DGCTL11B, 0x3);
+		agnx_write32(ctl, AGNX_GCR_GAININIT, 0xd);
+		agnx_write32(ctl, AGNX_GCR_THNOSIG, 0x1);
+		agnx_write32(ctl, AGNX_GCR_COARSTEP, 0x7);
+		agnx_write32(ctl, AGNX_GCR_SIFST11A, 0x28);
+		agnx_write32(ctl, AGNX_GCR_SIFST11B, 0x28);
+		reg = agnx_read32(ctl, AGNX_GCR_CWDETEC);
+		reg |= 0x1;
+		agnx_write32(ctl, AGNX_GCR_CWDETEC, reg);
+		agnx_write32(ctl, AGNX_GCR_0X38, 0x1e);
+		agnx_write32(ctl, AGNX_GCR_BOACT, 0x26);
+		agnx_write32(ctl, AGNX_GCR_DISCOVMOD, 0x3);
+		agnx_write32(ctl, AGNX_GCR_NLISTANT, 0x3);
+		agnx_write32(ctl, AGNX_GCR_NACTIANT, 0x3);
+		agnx_write32(ctl, AGNX_GCR_NMEASANT, 0x3);
+		agnx_write32(ctl, AGNX_GCR_NCAPTANT, 0x3);
+		agnx_write32(ctl, AGNX_GCR_THCAP11A, 0x0);
+		agnx_write32(ctl, AGNX_GCR_THCAP11B, 0x0);
+		agnx_write32(ctl, AGNX_GCR_THCAPRX11A, 0x0);
+		agnx_write32(ctl, AGNX_GCR_THCAPRX11B, 0x0);
+		agnx_write32(ctl, AGNX_GCR_THLEVDRO, 0x10);
+		agnx_write32(ctl, AGNX_GCR_MAXRXTIME11A, 0x1);
+		agnx_write32(ctl, AGNX_GCR_MAXRXTIME11B, 0x1);
+		agnx_write32(ctl, AGNX_GCR_CORRTIME, 0x190);
+		agnx_write32(ctl, AGNX_GCR_SIGHTH, 0x78);
+		agnx_write32(ctl, AGNX_GCR_SIGLTH, 0x1c);
+		agnx_write32(ctl, AGNX_GCR_CORRDROP, 0x0);
+		agnx_write32(ctl, AGNX_GCR_THCD, 0x0);
+		agnx_write32(ctl, AGNX_GCR_MAXPOWDIFF, 0x1);
+		agnx_write32(ctl, AGNX_GCR_TESTBUS, 0x0);
+		agnx_write32(ctl, AGNX_GCR_ANTCFG, 0x1f);
+		agnx_write32(ctl, AGNX_GCR_THJUMP, 0x14);
+		agnx_write32(ctl, AGNX_GCR_THPOWER, 0x0);
+		agnx_write32(ctl, AGNX_GCR_THPOWCLIP, 0x30);
+		agnx_write32(ctl, AGNX_GCR_THD0BTFEST, 0x32);
+		agnx_write32(ctl, AGNX_GCR_THRX11BPOWMIN, 0x19);
+		agnx_write32(ctl, AGNX_GCR_0X14c, 0x0);
+		agnx_write32(ctl, AGNX_GCR_0X150, 0x0);
+		agnx_write32(ctl, 0x9400, 0x0);
+		agnx_write32(ctl, 0x940c, 0x6ff);
+		agnx_write32(ctl, 0x9428, 0xa0);
+		agnx_write32(ctl, 0x9434, 0x0);
+		agnx_write32(ctl, 0x9c04, 0x15);
+		agnx_write32(ctl, 0x9c0c, 0x7f);
+		agnx_write32(ctl, 0x9c34, 0x0);
+		agnx_write32(ctl, 0xc000, 0x38d);
+		agnx_write32(ctl, 0x14018, 0x0);
+		agnx_write32(ctl, 0x16000, 0x1);
+		agnx_write32(ctl, 0x11004, 0x0);
+		agnx_write32(ctl, 0xec54, 0xa);
+		agnx_write32(ctl, 0xec1c, 0x5);
+	} else if (priv->revid > 0) {
+		agnx_write32(ctl, AGNX_ACI_LEN, 0xf);
+		agnx_write32(ctl, AGNX_ACI_TIMER1, 0x21);
+		agnx_write32(ctl, AGNX_ACI_TIMER2, 0x27);
+		agnx_write32(ctl, AGNX_ACI_AICCHA0OVE, 0x11);
+		agnx_write32(ctl, AGNX_ACI_AICCHA1OVE, 0x0);
+		agnx_write32(ctl, AGNX_GCR_DUNSAT, 0x14);
+		agnx_write32(ctl, AGNX_GCR_DSAT, 0x24);
+		agnx_write32(ctl, AGNX_GCR_DFIRCAL, 0x8);
+		agnx_write32(ctl, AGNX_GCR_DGCTL11A, 0x1a);
+		agnx_write32(ctl, AGNX_GCR_DGCTL11B, 0x3);
+		agnx_write32(ctl, AGNX_GCR_GAININIT, 0xd);
+		agnx_write32(ctl, AGNX_GCR_THNOSIG, 0x1);
+		agnx_write32(ctl, AGNX_GCR_COARSTEP, 0x7);
+		agnx_write32(ctl, AGNX_GCR_SIFST11A, 0x28);
+		agnx_write32(ctl, AGNX_GCR_SIFST11B, 0x28);
+		agnx_write32(ctl, AGNX_GCR_CWDETEC, 0x0);
+		agnx_write32(ctl, AGNX_GCR_0X38, 0x1e);
+//		agnx_write32(ctl, AGNX_GCR_BOACT, 0x26);
+		agnx_write32(ctl, AGNX_GCR_DISCOVMOD, 0x3);
+
+		agnx_write32(ctl, AGNX_GCR_THCAP11A, 0x32);
+		agnx_write32(ctl, AGNX_GCR_THCAP11B, 0x32);
+		agnx_write32(ctl, AGNX_GCR_THCAPRX11A, 0x32);
+		agnx_write32(ctl, AGNX_GCR_THCAPRX11B, 0x32);
+		agnx_write32(ctl, AGNX_GCR_THLEVDRO, 0x10);
+		agnx_write32(ctl, AGNX_GCR_MAXRXTIME11A, 0x1ad);
+		agnx_write32(ctl, AGNX_GCR_MAXRXTIME11B, 0xa10);
+		agnx_write32(ctl, AGNX_GCR_CORRTIME, 0x190);
+		agnx_write32(ctl, AGNX_GCR_CORRDROP, 0x0);
+		agnx_write32(ctl, AGNX_GCR_THCD, 0x0);
+		agnx_write32(ctl, AGNX_GCR_THCS, 0x0);
+		agnx_write32(ctl, AGNX_GCR_MAXPOWDIFF, 0x4);
+		agnx_write32(ctl, AGNX_GCR_TESTBUS, 0x0);
+		agnx_write32(ctl, AGNX_GCR_THJUMP, 0x1e);
+		agnx_write32(ctl, AGNX_GCR_THPOWER, 0x0);
+		agnx_write32(ctl, AGNX_GCR_THPOWCLIP, 0x2a);
+		agnx_write32(ctl, AGNX_GCR_THD0BTFEST, 0x3c);
+		agnx_write32(ctl, AGNX_GCR_THRX11BPOWMIN, 0x19);
+		agnx_write32(ctl, AGNX_GCR_0X14c, 0x0);
+		agnx_write32(ctl, AGNX_GCR_0X150, 0x0);
+		agnx_write32(ctl, AGNX_GCR_RXOVERIDE, 0x0);
+		agnx_write32(ctl, AGNX_GCR_WATCHDOG, 0x37);
+		agnx_write32(ctl, 0x9400, 0x0);
+		agnx_write32(ctl, 0x940c, 0x6ff);
+		agnx_write32(ctl, 0x9428, 0xa0);
+		agnx_write32(ctl, 0x9434, 0x0);
+		agnx_write32(ctl, 0x9c04, 0x15);
+		agnx_write32(ctl, 0x9c0c, 0x7f);
+		agnx_write32(ctl, 0x9c34, 0x0);
+		agnx_write32(ctl, 0xc000, 0x38d);
+		agnx_write32(ctl, 0x14014, 0x1000);
+		agnx_write32(ctl, 0x14018, 0x0);
+		agnx_write32(ctl, 0x16000, 0x1);
+		agnx_write32(ctl, 0x11004, 0x0);
+		agnx_write32(ctl, 0xec54, 0xa);
+		agnx_write32(ctl, 0xec1c, 0x50);
+	} else if (priv->revid > 1) {
+		reg = agnx_read32(ctl, 0xec18);
+		reg |= 0x8;
+		agnx_write32(ctl, 0xec18, reg);
+	}
+
+	/* Write the TX Fir Coefficient Table */
+	tx_fir_table_init(priv);
+
+	reg = agnx_read32(ctl, AGNX_PM_PMCTL);
+	reg &= ~0x8;
+	agnx_write32(ctl, AGNX_PM_PMCTL, reg);
+	reg = agnx_read32(ctl, AGNX_PM_PLLCTL);
+	reg |= 0x1;
+	agnx_write32(ctl, AGNX_PM_PLLCTL, reg);
+
+/* 	reg = agnx_read32(ctl, 0x1a030); */
+/* 	reg &= ~0x4; */
+/* 	agnx_write32(ctl, 0x1a030, reg); */
+
+	agnx_write32(ctl, AGNX_GCR_TRACNT4, 0x113);
+} /* phy_init */
+
+static void chip_init(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+	AGNX_TRACE;
+
+	band_management_init(priv);
+
+	rf_chips_init(priv);
+
+	reg = agnx_read32(ctl, AGNX_PM_PMCTL);
+	reg |= 0x8;
+	agnx_write32(ctl, AGNX_PM_PMCTL, reg);
+
+	/* Initialize the PHY */
+	phy_init(priv);
+
+	encryption_init(priv);
+
+	tx_management_init(priv);
+
+	rx_management_init(priv);
+
+	power_manage_init(priv);
+
+	/* Initialize the Timers */
+	agnx_timer_init(priv);
+
+	/* Write 0xc390bf9 to Interrupt Mask (Disable TX) */
+	reg = 0xc390bf9 & ~IRQ_TX_BEACON;
+	reg &= ~IRQ_TX_DISABLE;
+	agnx_write32(ctl, AGNX_INT_MASK, reg);
+
+	reg = agnx_read32(ctl, AGNX_CIR_BLKCTL);
+	reg |= 0x800;
+	agnx_write32(ctl, AGNX_CIR_BLKCTL, reg);
+
+	/* set it when need get multicast enable? */
+	agnx_write32(ctl, AGNX_BM_MTSM, 0xff);
+} /* chip_init */
+
+
+static inline void set_promis_and_managed(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	agnx_write32(ctl, AGNX_SYSITF_SYSMODE, 0x10 | 0x2);
+	agnx_write32(ctl, AGNX_SYSITF_SYSMODE, 0x10 | 0x2);
+}
+static inline void set_learn_mode(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	agnx_write32(ctl, AGNX_SYSITF_SYSMODE, 0x8);
+}
+static inline void set_scan_mode(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	agnx_write32(ctl, AGNX_SYSITF_SYSMODE, 0x20);
+}
+static inline void set_promiscuous_mode(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	/* agnx_write32(ctl, AGNX_SYSITF_SYSMODE, 0x210);*/
+	agnx_write32(ctl, AGNX_SYSITF_SYSMODE, 0x10);
+}
+static inline void set_managed_mode(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	agnx_write32(ctl, AGNX_SYSITF_SYSMODE, 0x2);
+}
+static inline void set_adhoc_mode(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	agnx_write32(ctl, AGNX_SYSITF_SYSMODE, 0x0);
+}
+
+#if 0
+static void unknow_register_write(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x0, 0x3e);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x4, 0xb2);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x8, 0x140);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0xc, 0x1C0);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x10, 0x1FF);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x14, 0x1DD);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x18, 0x15F);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x1c, 0xA1);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x20, 0x3E7);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x24, 0x36B);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x28, 0x348);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x2c, 0x37D);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x30, 0x3DE);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x34, 0x36);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x38, 0x64);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x3c, 0x57);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x40, 0x23);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x44, 0x3ED);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x48, 0x3C9);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x4c, 0x3CA);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x50, 0x3E7);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x54, 0x8);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x58, 0x1F);
+	agnx_write32(ctl, AGNX_UNKNOWN_BASE + 0x5c, 0x1a);
+}
+#endif
+
+static void card_interface_init(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	u8 bssid[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	u32 reg;
+	unsigned int i;
+	AGNX_TRACE;
+
+	might_sleep();
+	/* Clear RX Control and Enable RX queues */
+	agnx_write32(ctl, AGNX_CIR_RXCTL, 0x8);
+
+	might_sleep();
+	/* Do a full reset of the card */
+	card_full_reset(priv);
+	might_sleep();
+
+	/* Check and set Card Endianness */
+	reg = ioread32(priv->ctl + AGNX_CIR_ENDIAN);
+	/* TODO If not 0xB3B2B1B0 set to 0xB3B2B1B0 */
+	printk(KERN_INFO PFX "CIR_ENDIAN is %x\n", reg);
+
+
+	/* Config the eeprom */
+	agnx_write32(ctl, AGNX_CIR_SERIALITF, 0x7000086);
+	udelay(10);
+	reg = agnx_read32(ctl, AGNX_CIR_SERIALITF);
+
+
+	agnx_write32(ctl, AGNX_PM_SOFTRST, 0x80000033);
+	reg = agnx_read32(ctl, 0xec50);
+	reg |= 0xf;
+	agnx_write32(ctl, 0xec50, reg);
+	agnx_write32(ctl, AGNX_PM_SOFTRST, 0x0);
+
+
+	reg = agnx_read32(ctl, AGNX_SYSITF_GPIOIN);
+	udelay(10);
+	reg = agnx_read32(ctl, AGNX_CIR_SERIALITF);
+
+	/* Dump the eeprom */
+	do {
+		char eeprom[0x100000/0x100];
+
+		for (i = 0; i < 0x100000; i += 0x100) {
+			agnx_write32(ctl, AGNX_CIR_SERIALITF, 0x3000000 + i);
+			udelay(13);
+			reg = agnx_read32(ctl, AGNX_CIR_SERIALITF);
+			udelay(70);
+			reg = agnx_read32(ctl, AGNX_CIR_SERIALITF);
+			eeprom[i/0x100] = reg & 0xFF;
+			udelay(10);
+		}
+		print_hex_dump_bytes(PFX "EEPROM: ", DUMP_PREFIX_NONE, eeprom,
+				     ARRAY_SIZE(eeprom));
+	} while(0);
+
+	spi_rc_write(ctl, RF_CHIP0, 0x26);
+        reg = agnx_read32(ctl, AGNX_SPI_RLSW);
+
+	/* Initialize the system interface */
+	system_itf_init(priv);
+
+	might_sleep();
+	/* Chip Initialization (Polaris) */
+	chip_init(priv);
+	might_sleep();
+
+	/* Calibrate the antennae */
+	antenna_calibrate(priv);
+
+	reg = agnx_read32(ctl, 0xec50);
+	reg &= ~0x40;
+	agnx_write32(ctl, 0xec50, reg);
+	agnx_write32(ctl, AGNX_PM_SOFTRST, 0x0);
+	agnx_write32(ctl, AGNX_PM_PLLCTL, 0x1);
+
+	reg = agnx_read32(ctl, AGNX_BM_BMCTL);
+	reg |= 0x8000;
+	agnx_write32(ctl, AGNX_BM_BMCTL, reg);
+	enable_receiver(priv);
+	reg = agnx_read32(ctl, AGNX_SYSITF_SYSMODE);
+	reg |= 0x200;
+	agnx_write32(ctl, AGNX_SYSITF_SYSMODE, reg);
+	enable_receiver(priv);
+
+	might_sleep();
+	/* Initialize Gain Control Counts */
+	gain_ctlcnt_init(priv);
+
+	/* Write Initial Station Power Template for this station(#0) */
+	sta_power_init(priv, LOCAL_STAID);
+
+	might_sleep();
+	/* Initialize the rx,td,tm rings, for each node in the ring */
+	fill_rings(priv);
+
+	might_sleep();
+
+
+	agnx_write32(ctl, AGNX_PM_SOFTRST, 0x80000033);
+	agnx_write32(ctl, 0xec50, 0xc);
+	agnx_write32(ctl, AGNX_PM_SOFTRST, 0x0);
+
+	/* FIXME Initialize the transmit control register */
+	agnx_write32(ctl, AGNX_TXM_CTL, 0x194c1);
+
+	enable_receiver(priv);
+
+	might_sleep();
+	/* FIXME Set the Receive Control Mac Address to card address */
+	mac_address_set(priv);
+	enable_receiver(priv);
+	might_sleep();
+
+	/* Set the recieve request rate */
+	/* FIXME Enable the request */
+	/* Check packet length */
+	/* Set maximum packet length */
+/* 	agnx_write32(ctl, AGNX_RXM_REQRATE, 0x88195e00); */
+/* 	enable_receiver(priv); */
+
+	/* Set the Receiver BSSID */
+	receiver_bssid_set(priv, bssid);
+
+	/* FIXME Set to managed mode */
+	set_managed_mode(priv);
+//	set_promiscuous_mode(priv);
+/* 	set_scan_mode(priv); */
+/* 	set_learn_mode(priv); */
+// 	set_promis_and_managed(priv);
+// 	set_adhoc_mode(priv);
+
+	/* Set the recieve request rate */
+	/* Check packet length */
+	agnx_write32(ctl, AGNX_RXM_REQRATE, 0x08000000);
+	reg = agnx_read32(ctl, AGNX_RXM_REQRATE);
+	/* Set maximum packet length */
+	reg |= 0x00195e00;
+	agnx_write32(ctl, AGNX_RXM_REQRATE, reg);
+
+	/* Configure the RX and TX interrupt */
+	reg = ENABLE_RX_INTERRUPT | RX_CACHE_LINE | FRAG_LEN_2048 | FRAG_BE;
+	agnx_write32(ctl, AGNX_CIR_RXCFG, reg);
+	/* FIXME */
+	reg = ENABLE_TX_INTERRUPT | TX_CACHE_LINE | FRAG_LEN_2048 | FRAG_BE;
+	agnx_write32(ctl, AGNX_CIR_TXCFG, reg);
+
+	/* Enable RX TX Interrupts */
+	agnx_write32(ctl, AGNX_CIR_RXCTL, 0x80);
+	agnx_write32(ctl, AGNX_CIR_TXMCTL, 0x80);
+	agnx_write32(ctl, AGNX_CIR_TXDCTL, 0x80);
+
+	/* FIXME Set the master control interrupt in block control */
+	agnx_write32(ctl, AGNX_CIR_BLKCTL, 0x800);
+
+	/* Enable RX and TX queues */
+	reg = agnx_read32(ctl, AGNX_CIR_RXCTL);
+	reg |= 0x8;
+	agnx_write32(ctl, AGNX_CIR_RXCTL, reg);
+	reg = agnx_read32(ctl, AGNX_CIR_TXMCTL);
+	reg |= 0x8;
+	agnx_write32(ctl, AGNX_CIR_TXMCTL, reg);
+	reg = agnx_read32(ctl, AGNX_CIR_TXDCTL);
+	reg |= 0x8;
+	agnx_write32(ctl, AGNX_CIR_TXDCTL, reg);
+
+	agnx_write32(ctl, AGNX_SYSITF_GPIOUT, 0x5);
+	/* FIXME */
+	/*  unknow_register_write(priv); */
+	/* Update local card hash entry */
+	hash_write(priv, priv->mac_addr, LOCAL_STAID);
+
+	might_sleep();
+
+	/* FIXME */
+	agnx_set_channel(priv, 1);
+	might_sleep();
+} /* agnx_card_interface_init */
+
+
+void agnx_hw_init(struct agnx_priv *priv)
+{
+	AGNX_TRACE;
+	might_sleep();
+	card_interface_init(priv);
+}
+
+int agnx_hw_reset(struct agnx_priv *priv)
+{
+	return card_full_reset(priv);
+}
+
+int agnx_set_ssid(struct agnx_priv *priv, u8 *ssid, size_t ssid_len)
+{
+	AGNX_TRACE;
+	return 0;
+}
+
+void agnx_set_bssid(struct agnx_priv *priv, u8 *bssid)
+{
+	receiver_bssid_set(priv, bssid);
+}
diff --git a/drivers/staging/agnx/phy.h b/drivers/staging/agnx/phy.h
new file mode 100644
index 00000000000..55e1e222179
--- /dev/null
+++ b/drivers/staging/agnx/phy.h
@@ -0,0 +1,409 @@
+#ifndef AGNX_PHY_H_
+#define AGNX_PHY_H_
+
+#include "agnx.h"
+
+/* Transmission Managment Registers */
+#define AGNX_TXM_BASE		0x0000
+#define AGNX_TXM_CTL		0x0000	/* control register */
+#define AGNX_TXM_ETMF		0x0004 /* enable transmission management functions */
+#define AGNX_TXM_TXTEMP		0x0008 /* transmission template */
+#define AGNX_TXM_RETRYSTAID	0x000c /* Retry Station ID */
+#define AGNX_TXM_TIMESTAMPLO		0x0010	/* Timestamp Lo */
+#define AGNX_TXM_TIMESTAMPHI		0x0014	/* Timestamp Hi */
+#define AGNX_TXM_TXDELAY	0x0018  /* tx delay */
+#define AGNX_TXM_TBTTLO		0x0020	/* tbtt Lo */
+#define AGNX_TXM_TBTTHI		0x0024	/* tbtt Hi */
+#define AGNX_TXM_BEAINTER	0x0028 /* Beacon Interval */
+#define AGNX_TXM_NAV		0x0030 /* NAV */
+#define AGNX_TXM_CFPMDV		0x0034 /* CFP MDV */
+#define AGNX_TXM_CFPERCNT	0x0038 /* CFP period count */
+#define AGNX_TXM_PROBDELAY	0x003c /* probe delay */
+#define AGNX_TXM_LISINTERCNT	0x0040 /* listen interval count */
+#define AGNX_TXM_DTIMPERICNT	0x004c /* DTIM period count */
+
+#define AGNX_TXM_BEACON_CTL	0x005c /* beacon control */
+
+#define AGNX_TXM_SCHEMPCNT	0x007c /* schedule empty count */
+#define AGNX_TXM_MAXTIMOUT	0x0084 /* max timeout exceed count */
+#define AGNX_TXM_MAXCFPTIM	0x0088 /* max CF poll timeout count */
+#define AGNX_TXM_MAXRXTIME	0x008c /* max RX timeout count */
+#define AGNX_TXM_MAXACKTIM	0x0090	/* max ACK timeout count */
+#define AGNX_TXM_DIF01		0x00a0 /* DIF 0-1 */
+#define AGNX_TXM_DIF23		0x00a4 /* DIF 2-3 */
+#define AGNX_TXM_DIF45		0x00a8 /* DIF 4-5 */
+#define AGNX_TXM_DIF67		0x00ac /* DIF 6-7 */
+#define AGNX_TXM_SIFSPIFS	0x00b0 /* SIFS/PIFS */
+#define AGNX_TXM_TIFSEIFS	0x00b4 /* TIFS/EIFS */
+#define AGNX_TXM_MAXCCACNTSLOT	0x00b8 /* max CCA count slot */
+#define AGNX_TXM_SLOTLIMIT	0x00bc /* slot limit/1 msec limit */
+#define AGNX_TXM_CFPOLLRXTIM	0x00f0 /* CF poll RX timeout count */
+#define AGNX_TXM_CFACKT11B	0x00f4 /* CF ack timeout limit for 11b */
+#define AGNX_TXM_CW0		0x0100 /* CW 0 */
+#define AGNX_TXM_SLBEALIM0	0x0108 /* short/long beacon limit 0 */
+#define AGNX_TXM_CW1		0x0120 /* CW 1 */
+#define AGNX_TXM_SLBEALIM1	0x0128 /* short/long beacon limit 1 */
+#define AGNX_TXM_CW2		0x0140 /* CW 2 */
+#define AGNX_TXM_SLBEALIM2	0x0148 /* short/long beacon limit 2 */
+#define AGNX_TXM_CW3		0x0160 /* CW 3 */
+#define AGNX_TXM_SLBEALIM3	0x0168 /* short/long beacon limit 3 */
+#define AGNX_TXM_CW4		0x0180 /* CW 4 */
+#define AGNX_TXM_SLBEALIM4	0x0188 /* short/long beacon limit 4 */
+#define AGNX_TXM_CW5		0x01a0 /* CW 5 */
+#define AGNX_TXM_SLBEALIM5	0x01a8 /* short/long beacon limit 5 */
+#define AGNX_TXM_CW6		0x01c0 /* CW 6 */
+#define AGNX_TXM_SLBEALIM6	0x01c8 /* short/long beacon limit 6 */
+#define AGNX_TXM_CW7		0x01e0 /* CW 7 */
+#define AGNX_TXM_SLBEALIM7	0x01e8 /* short/long beacon limit 7 */
+#define AGNX_TXM_BEACONTEMP     0x1000	/* beacon template */
+#define AGNX_TXM_STAPOWTEMP	0x1a00 /*  Station Power Template */
+
+/* Receive Management Control Registers */
+#define AGNX_RXM_BASE		0x2000
+#define AGNX_RXM_REQRATE	0x2000	/* requested rate */
+#define AGNX_RXM_MACHI		0x2004	/* first 4 bytes of mac address */
+#define AGNX_RXM_MACLO		0x2008	/* last 2 bytes of mac address */
+#define AGNX_RXM_BSSIDHI	0x200c	/* bssid hi */
+#define AGNX_RXM_BSSIDLO	0x2010	/* bssid lo */
+#define AGNX_RXM_HASH_CMD_FLAG	0x2014	/* Flags for the RX Hash Command Default:0 */
+#define AGNX_RXM_HASH_CMD_HIGH	0x2018	/* The High half of the Hash Command */
+#define AGNX_RXM_HASH_CMD_LOW	0x201c	/* The Low half of the Hash Command */
+#define AGNX_RXM_ROUTAB		0x2020	/* routing table */
+#define		ROUTAB_SUBTYPE_SHIFT	24
+#define		ROUTAB_TYPE_SHIFT	28
+#define		ROUTAB_STATUS_SHIFT	30
+#define		ROUTAB_RW_SHIFT		31
+#define		ROUTAB_ROUTE_DROP	0xf00000 /* Drop */
+#define		ROUTAB_ROUTE_CPU	0x400000 /* CPU */
+#define		ROUTAB_ROUTE_ENCRY	0x500800 /* Encryption */
+#define		ROUTAB_ROUTE_RFP	0x800000 /* RFP */
+
+#define		ROUTAB_TYPE_MANAG	0x0 /* Management */
+#define		ROUTAB_TYPE_CTL		0x1 /* Control */
+#define		ROUTAB_TYPE_DATA	0x2 /* Data */
+
+#define		ROUTAB_SUBTYPE_DATA		0x0
+#define		ROUTAB_SUBTYPE_DATAACK		0x1
+#define		ROUTAB_SUBTYPE_DATAPOLL		0x2
+#define		ROUTAB_SUBTYPE_DATAPOLLACK	0x3
+#define		ROUTAB_SUBTYPE_NULL		0x4 /* NULL */
+#define		ROUTAB_SUBTYPE_NULLACK		0x5
+#define		ROUTAB_SUBTYPE_NULLPOLL		0x6
+#define		ROUTAB_SUBTYPE_NULLPOLLACK	0x7
+#define		ROUTAB_SUBTYPE_QOSDATA		0x8 /* QOS DATA */
+#define		ROUTAB_SUBTYPE_QOSDATAACK	0x9
+#define		ROUTAB_SUBTYPE_QOSDATAPOLL	0xa
+#define		ROUTAB_SUBTYPE_QOSDATAACKPOLL	0xb
+#define		ROUTAB_SUBTYPE_QOSNULL		0xc
+#define		ROUTAB_SUBTYPE_QOSNULLACK	0xd
+#define		ROUTAB_SUBTYPE_QOSNULLPOLL	0xe
+#define		ROUTAB_SUBTYPE_QOSNULLPOLLACK	0xf
+#define AGNX_RXM_DELAY11	   0x2024	/* delay 11(AB) */
+#define AGNX_RXM_SOF_CNT	   0x2028	/* SOF Count */
+#define AGNX_RXM_FRAG_CNT	   0x202c	/* Fragment Count*/
+#define AGNX_RXM_FCS_CNT	   0x2030	/* FCS Count */
+#define AGNX_RXM_BSSID_MISS_CNT	   0x2034	/* BSSID Miss Count */
+#define AGNX_RXM_PDU_ERR_CNT	   0x2038	/* PDU Error Count */
+#define AGNX_RXM_DEST_MISS_CNT	   0x203C	/* Destination Miss Count */
+#define AGNX_RXM_DROP_CNT	   0x2040	/* Drop Count */
+#define AGNX_RXM_ABORT_CNT	   0x2044	/* Abort Count */
+#define AGNX_RXM_RELAY_CNT	   0x2048	/* Relay Count */
+#define AGNX_RXM_HASH_MISS_CNT	   0x204c	/* Hash Miss Count */
+#define AGNX_RXM_SA_HI		   0x2050	/* Address of received packet Hi */
+#define AGNX_RXM_SA_LO		   0x2054	/* Address of received packet Lo */
+#define AGNX_RXM_HASH_DUMP_LST	   0x2100	/* Contains Hash Data */
+#define AGNX_RXM_HASH_DUMP_MST	   0x2104	/* Contains Hash Data */
+#define AGNX_RXM_HASH_DUMP_DATA    0x2108	/* The Station ID to dump */
+
+
+/* Encryption Managment */
+#define AGNX_ENCRY_BASE		0x2400
+#define AGNX_ENCRY_WEPKEY0	0x2440 /* wep key #0 */
+#define AGNX_ENCRY_WEPKEY1	0x2444 /* wep key #1 */
+#define AGNX_ENCRY_WEPKEY2	0x2448 /* wep key #2 */
+#define AGNX_ENCRY_WEPKEY3	0x244c /* wep key #3 */
+#define AGNX_ENCRY_CCMRECTL	0x2460 /* ccm replay control */
+
+
+/* Band Management Registers */
+#define AGNX_BM_BASE		0x2c00
+#define AGNX_BM_BMCTL		0x2c00  /* band management control */
+#define AGNX_BM_TXWADDR		0x2c18  /* tx workqueue address start */
+#define AGNX_BM_TXTOPEER	0x2c24	/* transmit to peers */
+#define AGNX_BM_FPLHP		0x2c2c  /* free pool list head pointer */
+#define AGNX_BM_FPLTP		0x2c30  /* free pool list tail pointer */
+#define AGNX_BM_FPCNT		0x2c34  /* free pool count */
+#define AGNX_BM_CIPDUWCNT	0x2c38  /* card interface pdu workqueue count */
+#define AGNX_BM_SPPDUWCNT	0x2c3c  /* sp pdu workqueue count */
+#define AGNX_BM_RFPPDUWCNT	0x2c40  /* rfp pdu workqueue count */
+#define AGNX_BM_RHPPDUWCNT	0x2c44  /* rhp pdu workqueue count */
+#define AGNX_BM_CIWQCTL		0x2c48 /* Card Interface WorkQueue Control */
+#define AGNX_BM_CPUTXWCTL	0x2c50  /* cpu tx workqueue control */
+#define AGNX_BM_CPURXWCTL	0x2c58  /* cpu rx workqueue control */
+#define AGNX_BM_CPULWCTL	0x2c60 /* cpu low workqueue control */
+#define AGNX_BM_CPUHWCTL	0x2c68 /* cpu high workqueue control */
+#define AGNX_BM_SPTXWCTL	0x2c70 /* sp tx workqueue control */
+#define AGNX_BM_SPRXWCTL	0x2c78 /* sp rx workqueue control */
+#define AGNX_BM_RFPWCTL		0x2c80 /* RFP workqueue control */
+#define AGNX_BM_MTSM		0x2c90 /* Multicast Transmit Station Mask */
+
+/* Card Interface Registers (32bits) */
+#define AGNX_CIR_BASE		0x3000
+#define AGNX_CIR_BLKCTL		0x3000	/* block control*/
+#define		AGNX_STAT_TX	0x1
+#define		AGNX_STAT_RX	0x2
+#define		AGNX_STAT_X	0x4
+/* Below two interrupt flags will be set by our but not CPU or the card */
+#define		AGNX_STAT_TXD	0x10
+#define		AGNX_STAT_TXM	0x20
+
+#define AGNX_CIR_ADDRWIN	0x3004	/* Addressable Windows*/
+#define AGNX_CIR_ENDIAN		0x3008  /* card endianness */
+#define AGNX_CIR_SERIALITF	0x3020	/* serial interface */
+#define AGNX_CIR_RXCFG		0x3040	/* receive config */
+#define		ENABLE_RX_INTERRUPT 0x20
+#define		RX_CACHE_LINE	    0x8
+/* the RX fragment length */
+#define		FRAG_LEN_256	0x0 /* 256B */
+#define		FRAG_LEN_512	0x1
+#define		FRAG_LEN_1024	0x2
+#define		FRAG_LEN_2048	0x3
+#define		FRAG_BE		0x10
+#define AGNX_CIR_RXCTL		0x3050	/* receive control */
+/* memory address, chipside */
+#define AGNX_CIR_RXCMSTART	0x3054	/* receive client memory start */
+#define AGNX_CIR_RXCMEND	0x3058	/* receive client memory end */
+/* memory address, pci */
+#define AGNX_CIR_RXHOSTADDR	0x3060	/* receive hostside address */
+/* memory address, chipside */
+#define AGNX_CIR_RXCLIADDR	0x3064	/* receive clientside address */
+#define AGNX_CIR_RXDMACTL	0x3068	/* receive dma control */
+#define AGNX_CIR_TXCFG		0x3080	/* transmit config */
+#define AGNX_CIR_TXMCTL		0x3090 /* Transmit Management Control */
+#define		ENABLE_TX_INTERRUPT 0x20
+#define		TX_CACHE_LINE	    0x8
+#define AGNX_CIR_TXMSTART	0x3094 /* Transmit Management Start */
+#define AGNX_CIR_TXMEND		0x3098 /* Transmit Management End */
+#define AGNX_CIR_TXDCTL		0x30a0	/* transmit data control */
+/* memeory address, chipset */
+#define AGNX_CIR_TXDSTART	0x30a4	/* transmit data start */
+#define AGNX_CIR_TXDEND		0x30a8	/* transmit data end */
+#define AGNX_CIR_TXMHADDR	0x30b0 /* Transmit Management Hostside Address */
+#define AGNX_CIR_TXMCADDR	0x30b4 /* Transmit Management Clientside Address */
+#define AGNX_CIR_TXDMACTL	0x30b8	/* transmit dma control */
+
+
+/* Power Managment Unit */
+#define AGNX_PM_BASE		0x3c00
+#define AGNX_PM_PMCTL		0x3c00	/* PM Control*/
+#define AGNX_PM_MACMSW		0x3c08 /* MAC Manual Slow Work Enable */
+#define AGNX_PM_RFCTL		0x3c0c /* RF Control */
+#define AGNX_PM_PHYMW		0x3c14	/* Phy Mannal Work */
+#define AGNX_PM_SOFTRST		0x3c18	/* PMU Soft Reset */
+#define AGNX_PM_PLLCTL		0x3c1c	/* PMU PLL control*/
+#define AGNX_PM_TESTPHY		0x3c24 /* PMU Test Phy */
+
+
+/* Interrupt Control interface */
+#define AGNX_INT_BASE		0x4000
+#define AGNX_INT_STAT		0x4000	/* interrupt status */
+#define AGNX_INT_MASK		0x400c	/* interrupt mask */
+/* FIXME */
+#define		IRQ_TX_BEACON	0x1	/* TX Beacon */
+#define		IRQ_TX_RETRY	0x8	/* TX Retry Interrupt */
+#define		IRQ_TX_ACTIVITY	0x10	/* TX Activity */
+#define		IRQ_RX_ACTIVITY	0x20	/* RX Activity */
+/* FIXME I guess that instead RX a none exist staion's packet or
+   the station hasn't been init */
+#define		IRQ_RX_X	0x40
+#define		IRQ_RX_Y	0x80	/* RX ? */
+#define		IRQ_RX_HASHHIT	0x100	/* RX Hash Hit */
+#define		IRQ_RX_FRAME	0x200	/* RX Frame */
+#define		IRQ_ERR_INT	0x400	/* Error Interrupt */
+#define		IRQ_TX_QUE_FULL	0x800	/* TX Workqueue Full */
+#define		IRQ_BANDMAN_ERR	0x10000	/* Bandwidth Management Error */
+#define		IRQ_TX_DISABLE	0x20000	/* TX Disable */
+#define		IRQ_RX_IVASESKEY 0x80000 /* RX Invalid Session Key */
+#define		IRQ_RX_KEYIDMIS	0x100000 /* RX key ID Mismatch */
+#define		IRQ_REP_THHIT	0x200000 /* Replay Threshold Hit */
+#define		IRQ_TIMER1	0x4000000 /* Timer1 */
+#define		IRQ_TIMER_CNT	0x10000000 /* Timer Count */
+#define		IRQ_PHY_FASTINT 0x20000000 /* Phy Fast Interrupt */
+#define		IRQ_PHY_SLOWINT	0x40000000 /* Phy Slow Interrupt */
+#define		IRQ_OTHER	0x80000000 /* Unknow interrupt */
+#define		AGNX_IRQ_ALL   	0xffffffff
+
+/* System Interface */
+#define AGNX_SYSITF_BASE	0x4400
+#define AGNX_SYSITF_SYSMODE	0x4400	/* system mode */
+#define AGNX_SYSITF_GPIOIN	0x4410 /* GPIO In */
+/* PIN lines for leds? */
+#define AGNX_SYSITF_GPIOUT	0x4414	/* GPIO Out */
+
+/* Timer Control */
+#define AGNX_TIMCTL_TIMER1	0x4800 /* Timer 1 */
+#define AGNX_TIMCTL_TIM1CTL	0x4808 /* Timer 1 Control */
+
+
+/* Antenna Calibration Interface */
+#define AGNX_ACI_BASE		0x5000
+#define AGNX_ACI_MODE		0x5000 /* Mode */
+#define AGNX_ACI_MEASURE	0x5004 /* Measure */
+#define AGNX_ACI_SELCHAIN	0x5008 /* Select Chain */
+#define AGNX_ACI_LEN		0x500c /* Length */
+#define AGNX_ACI_TIMER1		0x5018 /* Timer 1 */
+#define AGNX_ACI_TIMER2		0x501c /* Timer 2 */
+#define AGNX_ACI_OFFSET		0x5020 /* Offset */
+#define AGNX_ACI_STATUS		0x5030 /* Status */
+#define		CALI_IDLE	0x0
+#define		CALI_DONE	0x1
+#define		CALI_BUSY	0x2
+#define		CALI_ERR	0x3
+#define AGNX_ACI_AICCHA0OVE	0x5034 /* AIC Channel 0 Override */
+#define AGNX_ACI_AICCHA1OVE	0x5038 /* AIC Channel 1 Override */
+
+/* Gain Control Registers */
+#define AGNX_GCR_BASE		0x9000
+/* threshold of primary antenna */
+#define AGNX_GCR_THD0A		0x9000	/* threshold? D0 A */
+/* low threshold of primary antenna */
+#define AGNX_GCR_THD0AL		0x9004	/* threshold? D0 A low */
+/* threshold of secondary antenna */
+#define AGNX_GCR_THD0B		0x9008	/* threshold? D0_B */
+#define AGNX_GCR_DUNSAT		0x900c /* d unsaturated */
+#define AGNX_GCR_DSAT		0x9010 /* d saturated */
+#define AGNX_GCR_DFIRCAL	0x9014 /* D Fir/Cal */
+#define AGNX_GCR_DGCTL11A	0x9018 /* d gain control 11a */
+#define AGNX_GCR_DGCTL11B	0x901c /* d gain control 11b */
+/* strength of gain */
+#define AGNX_GCR_GAININIT	0x9020	/* gain initialization */
+#define AGNX_GCR_THNOSIG	0x9024 /* threhold no signal */
+#define AGNX_GCR_COARSTEP	0x9028 /* coarse stepping */
+#define AGNX_GCR_SIFST11A	0x902c /* sifx time 11a */
+#define AGNX_GCR_SIFST11B	0x9030 /* sifx time 11b */
+#define AGNX_GCR_CWDETEC	0x9034 /* cw detection */
+#define AGNX_GCR_0X38		0x9038 /* ???? */
+#define AGNX_GCR_BOACT		0x903c	/* BO Active */
+#define AGNX_GCR_BOINACT	0x9040	/* BO Inactive */
+#define AGNX_GCR_BODYNA		0x9044	/* BO dynamic */
+/* 802.11 mode(a,b,g) */
+#define AGNX_GCR_DISCOVMOD	0x9048	/* discovery mode */
+#define AGNX_GCR_NLISTANT	0x904c	/* number of listening antenna */
+#define AGNX_GCR_NACTIANT	0x9050	/* number of active antenna */
+#define AGNX_GCR_NMEASANT	0x9054	/* number of measuring antenna */
+#define AGNX_GCR_NCAPTANT	0x9058	/* number of capture antenna */
+#define AGNX_GCR_THCAP11A	0x905c /* threshold capture 11a */
+#define AGNX_GCR_THCAP11B	0x9060 /* threshold capture 11b */
+#define AGNX_GCR_THCAPRX11A	0x9064 /* threshold capture rx 11a */
+#define AGNX_GCR_THCAPRX11B	0x9068 /* threshold capture rx 11b */
+#define AGNX_GCR_THLEVDRO	0x906c /* threshold level drop */
+#define AGNX_GCR_GAINSET0	0x9070 /* Gainset 0 */
+#define AGNX_GCR_GAINSET1	0x9074 /* Gainset 1 */
+#define AGNX_GCR_GAINSET2	0x9078 /* Gainset 2 */
+#define AGNX_GCR_MAXRXTIME11A	0x907c /* maximum rx time 11a */
+#define AGNX_GCR_MAXRXTIME11B	0x9080 /* maximum rx time 11b */
+#define AGNX_GCR_CORRTIME	0x9084 /* correction time */
+/* reset the subsystem, 0 = disable, 1 = enable */
+#define AGNX_GCR_RSTGCTL	0x9088	/* reset gain control */
+/* channel receiving */
+#define AGNX_GCR_RXCHANEL	0x908c	/* receive channel */
+#define AGNX_GCR_NOISE0		0x9090 /* Noise 0 */
+#define AGNX_GCR_NOISE1		0x9094 /* Noise 1 */
+#define AGNX_GCR_NOISE2		0x9098 /* Noise 2 */
+#define AGNX_GCR_SIGHTH		0x909c	/* Signal High Threshold */
+#define AGNX_GCR_SIGLTH		0x90a0	/* Signal Low Threshold */
+#define AGNX_GCR_CORRDROP	0x90a4 /* correction drop */
+/* threshold of tertiay antenna */
+#define AGNX_GCR_THCD		0x90a8	/* threshold? CD */
+#define AGNX_GCR_THCS		0x90ac	/* threshold? CS */
+#define AGNX_GCR_MAXPOWDIFF	0x90b8 /* maximum power difference */
+#define AGNX_GCR_TRACNT4	0x90ec /* Transition Count 4 */
+#define AGNX_GCR_TRACNT5      	0x90f0	/* transition count 5 */
+#define AGNX_GCR_TRACNT6       	0x90f4	/* transition count 6 */
+#define AGNX_GCR_TRACNT7       	0x90f8	/* transition coutn 7 */
+#define AGNX_GCR_TESTBUS	0x911c /* test bus */
+#define AGNX_GCR_CHAINNUM	0x9120 /* Number of Chains */
+#define AGNX_GCR_ANTCFG		0x9124	/* Antenna Config */
+#define AGNX_GCR_THJUMP		0x912c /* threhold jump */
+#define AGNX_GCR_THPOWER	0x9130 /* threshold power */
+#define AGNX_GCR_THPOWCLIP	0x9134 /* threshold power clip*/
+#define AGNX_GCR_FORCECTLCLK	0x9138 /* Force Gain Control Clock */
+#define AGNX_GCR_GAINSETWRITE	0x913c /* Gainset Write */
+#define AGNX_GCR_THD0BTFEST	0x9140	/* threshold d0 b tf estimate */
+#define AGNX_GCR_THRX11BPOWMIN	0x9144	/* threshold rx 11b power minimum */
+#define AGNX_GCR_0X14c		0x914c /* ?? */
+#define AGNX_GCR_0X150		0x9150 /* ?? */
+#define AGNX_GCR_RXOVERIDE	0x9194	/* recieve override */
+#define AGNX_GCR_WATCHDOG	0x91b0	/* watchdog timeout */
+
+
+/* Spi Interface */
+#define AGNX_SPI_BASE		0xdc00
+#define AGNX_SPI_CFG		0xdc00 /* spi configuration */
+/* Only accept 16 bits */
+#define AGNX_SPI_WMSW		0xdc04	/* write most significant word */
+/* Only accept 16 bits */
+#define AGNX_SPI_WLSW		0xdc08	/* write least significant word */
+#define AGNX_SPI_CTL		0xdc0c	/* spi control */
+#define AGNX_SPI_RMSW		0xdc10 /* read most significant word */
+#define AGNX_SPI_RLSW		0xdc14 /* read least significant word */
+/* SPI Control Mask */
+#define		SPI_READ_CTL		0x4000 /* read control */
+#define		SPI_BUSY_CTL		0x8000 /* busy control */
+/* RF and synth chips in spi */
+#define		RF_CHIP0	0x400
+#define		RF_CHIP1	0x800
+#define		RF_CHIP2	0x1000
+#define		SYNTH_CHIP	0x2000
+
+/* Unknown register */
+#define AGNX_UNKNOWN_BASE	0x7800
+
+/* FIXME MonitorGain */
+#define AGNX_MONGCR_BASE	0x12000
+
+/* Gain Table */
+#define AGNX_GAIN_TABLE		0x12400
+
+/* The initial FIR coefficient table */
+#define AGNX_FIR_BASE		0x19804
+
+#define AGNX_ENGINE_LOOKUP_TBL	0x800
+
+/* eeprom commands */
+#define EEPROM_CMD_NULL		0x0 /* NULL */
+#define EEPROM_CMD_WRITE	0x2 /* write */
+#define EEPROM_CMD_READ		0x3 /* read */
+#define EEPROM_CMD_STATUSREAD	0x5 /* status register read */
+#define EEPROM_CMD_WRITEENABLE	0x6 /* write enable */
+#define EEPROM_CMD_CONFIGURE	0x7 /* configure */
+
+#define EEPROM_DATAFORCOFIGURE	0x6 /* ??? */
+
+/* eeprom address */
+#define EEPROM_ADDR_SUBVID	0x0 /* Sub Vendor ID */
+#define EEPROM_ADDR_SUBSID	0x2 /* Sub System ID */
+#define EEPROM_ADDR_MACADDR	0x146 /* MAC Address */
+#define EEPROM_ADDR_LOTYPE	0x14f /* LO type */
+
+struct agnx_eeprom {
+	u8 data;	/* date */
+	u16 address;	/* address in EEPROM */
+	u8 cmd;		/* command, unknown, status */
+}  __attribute__((__packed__));
+
+#define AGNX_EEPROM_COMMAND_SHIFT	5
+#define AGNX_EEPROM_COMMAND_STAT	0x01
+
+void disable_receiver(struct agnx_priv *priv);
+void enable_receiver(struct agnx_priv *priv);
+u8 read_from_eeprom(struct agnx_priv *priv, u16 address);
+void agnx_hw_init(struct agnx_priv *priv);
+int agnx_hw_reset(struct agnx_priv *priv);
+int agnx_set_ssid(struct agnx_priv *priv, u8 *ssid, size_t ssid_len);
+void agnx_set_bssid(struct agnx_priv *priv, u8 *bssid);
+void enable_power_saving(struct agnx_priv *priv);
+void disable_power_saving(struct agnx_priv *priv);
+void calibrate_antenna_period(unsigned long data);
+
+#endif /* AGNX_PHY_H_ */
diff --git a/drivers/staging/agnx/rf.c b/drivers/staging/agnx/rf.c
new file mode 100644
index 00000000000..8294b6e2eb9
--- /dev/null
+++ b/drivers/staging/agnx/rf.c
@@ -0,0 +1,894 @@
+/**
+ * Airgo MIMO wireless driver
+ *
+ * Copyright (c) 2007 Li YanBo <dreamfly281@gmail.com>
+
+ * Thanks for Jeff Williams <angelbane@gmail.com> do reverse engineer
+ * works and published the SPECS at http://airgo.wdwconsulting.net/mymoin
+
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation;
+ */
+
+#include <linux/pci.h>
+#include <linux/delay.h>
+#include "agnx.h"
+#include "debug.h"
+#include "phy.h"
+#include "table.h"
+
+/* FIXME! */
+static inline void spi_write(void __iomem *region, u32 chip_ids, u32 sw,
+		      u16 size, u32 control)
+{
+	u32 reg;
+	u32 lsw = sw & 0xffff;		/* lower 16 bits of sw*/
+	u32 msw = sw >> 16;		/* high 16 bits of sw */
+
+	/* FIXME Write Most Significant Word of the 32bit data to MSW */
+	/* FIXME And Least Significant Word to LSW */
+	iowrite32((lsw), region + AGNX_SPI_WLSW);
+	iowrite32((msw), region + AGNX_SPI_WMSW);
+	reg = chip_ids | size | control;
+	/* Write chip id(s), write size and busy control to Control Register */
+	iowrite32((reg), region + AGNX_SPI_CTL);
+	/* Wait for Busy control to clear */
+	spi_delay();
+}
+
+/*
+ * Write to SPI Synth register
+ */
+static inline void spi_sy_write(void __iomem *region, u32 chip_ids, u32 sw)
+{
+	/* FIXME the size 0x15 is a magic value*/
+	spi_write(region, chip_ids, sw, 0x15, SPI_BUSY_CTL);
+}
+
+/*
+ * Write to SPI RF register
+ */
+static inline void spi_rf_write(void __iomem *region, u32 chip_ids, u32 sw)
+{
+	/* FIXME the size 0xd is a magic value*/
+	spi_write(region, chip_ids, sw, 0xd, SPI_BUSY_CTL);
+} /* spi_rf_write */
+
+/*
+ * Write to SPI with Read Control bit set
+ */
+inline void spi_rc_write(void __iomem *region, u32 chip_ids, u32 sw)
+{
+	/* FIXME the size 0xe5 is a magic value */
+	spi_write(region, chip_ids, sw, 0xe5, SPI_BUSY_CTL|SPI_READ_CTL);
+}
+
+/* Get the active chains's count */
+static int get_active_chains(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	int num = 0;
+	u32 reg;
+	AGNX_TRACE;
+
+	spi_rc_write(ctl, RF_CHIP0, 0x21);
+	reg = agnx_read32(ctl, AGNX_SPI_RLSW);
+	if (reg == 1)
+		num++;
+
+	spi_rc_write(ctl, RF_CHIP1, 0x21);
+	reg = agnx_read32(ctl, AGNX_SPI_RLSW);
+	if (reg == 1)
+		num++;
+
+	spi_rc_write(ctl, RF_CHIP2, 0x21);
+	reg = agnx_read32(ctl, AGNX_SPI_RLSW);
+	if (reg == 1)
+		num++;
+
+	spi_rc_write(ctl, RF_CHIP0, 0x26);
+	reg = agnx_read32(ctl, AGNX_SPI_RLSW);
+	if (0x33 != reg)
+		printk(KERN_WARNING PFX "Unmatched rf chips result\n");
+
+	return num;
+} /* get_active_chains */
+
+void rf_chips_init(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+	int num;
+	AGNX_TRACE;
+
+	if (priv->revid == 1) {
+		reg = agnx_read32(ctl, AGNX_SYSITF_GPIOUT);
+		reg |= 0x8;
+		agnx_write32(ctl, AGNX_SYSITF_GPIOUT, reg);
+	}
+
+	/* Set SPI clock speed to 200NS */
+        reg = agnx_read32(ctl, AGNX_SPI_CFG);
+        reg &= ~0xF;
+        reg |= 0x3;
+        agnx_write32(ctl, AGNX_SPI_CFG, reg);
+
+        /* Set SPI clock speed to 50NS */
+	reg = agnx_read32(ctl, AGNX_SPI_CFG);
+	reg &= ~0xF;
+	reg |= 0x1;
+	agnx_write32(ctl, AGNX_SPI_CFG, reg);
+
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1|RF_CHIP2, 0x1101);
+
+	num = get_active_chains(priv);
+	printk(KERN_INFO PFX "Active chains are %d\n", num);
+
+	reg = agnx_read32(ctl, AGNX_SPI_CFG);
+	reg &= ~0xF;
+	agnx_write32(ctl, AGNX_SPI_CFG, reg);
+
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1|RF_CHIP2, 0x1908);
+} /* rf_chips_init */
+
+
+static u32 channel_tbl[15][9] = {
+	{0,  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
+	{1,  0x00, 0x00, 0x624, 0x00, 0x1a4, 0x28, 0x00, 0x1e},
+	{2,  0x00, 0x00, 0x615, 0x00, 0x1ae, 0x28, 0x00, 0x1e},
+	{3,  0x00, 0x00, 0x61a, 0x00, 0x1ae, 0x28, 0x00, 0x1e},
+	{4,  0x00, 0x00, 0x61f, 0x00, 0x1ae, 0x28, 0x00, 0x1e},
+	{5,  0x00, 0x00, 0x624, 0x00, 0x1ae, 0x28, 0x00, 0x1e},
+	{6,  0x00, 0x00, 0x61f, 0x00, 0x1b3, 0x28, 0x00, 0x1e},
+	{7,  0x00, 0x00, 0x624, 0x00, 0x1b3, 0x28, 0x00, 0x1e},
+	{8,  0x00, 0x00, 0x629, 0x00, 0x1b3, 0x28, 0x00, 0x1e},
+	{9,  0x00, 0x00, 0x624, 0x00, 0x1b8, 0x28, 0x00, 0x1e},
+	{10, 0x00, 0x00, 0x629, 0x00, 0x1b8, 0x28, 0x00, 0x1e},
+	{11, 0x00, 0x00, 0x62e, 0x00, 0x1b8, 0x28, 0x00, 0x1e},
+	{12, 0x00, 0x00, 0x633, 0x00, 0x1b8, 0x28, 0x00, 0x1e},
+	{13, 0x00, 0x00, 0x628, 0x00, 0x1b8, 0x28, 0x00, 0x1e},
+	{14, 0x00, 0x00, 0x644, 0x00, 0x1b8, 0x28, 0x00, 0x1e},
+};
+
+
+static inline void
+channel_tbl_write(struct agnx_priv *priv, unsigned int channel, unsigned int reg_num)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+
+	reg = channel_tbl[channel][reg_num];
+	reg <<= 4;
+	reg |= reg_num;
+	spi_sy_write(ctl, SYNTH_CHIP, reg);
+}
+
+static void synth_freq_set(struct agnx_priv *priv, unsigned int channel)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+	AGNX_TRACE;
+
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1, 0x1201);
+
+	/* Set the Clock bits to 50NS */
+	reg = agnx_read32(ctl, AGNX_SPI_CFG);
+	reg &= ~0xF;
+	reg |= 0x1;
+	agnx_write32(ctl, AGNX_SPI_CFG, reg);
+
+	/* Write 0x00c0 to LSW and 0x3 to MSW of Synth Chip */
+	spi_sy_write(ctl, SYNTH_CHIP, 0x300c0);
+
+	spi_sy_write(ctl, SYNTH_CHIP, 0x32);
+
+	/* # Write to Register 1 on the Synth Chip */
+	channel_tbl_write(priv, channel, 1);
+	/* # Write to Register 3 on the Synth Chip */
+	channel_tbl_write(priv, channel, 3);
+	/* # Write to Register 6 on the Synth Chip */
+	channel_tbl_write(priv, channel, 6);
+	/* # Write to Register 5 on the Synth Chip */
+	channel_tbl_write(priv, channel, 5);
+	/* # Write to register 8 on the Synth Chip */
+	channel_tbl_write(priv, channel, 8);
+
+	/* FIXME Clear the clock bits */
+	reg = agnx_read32(ctl, AGNX_SPI_CFG);
+	reg &= ~0xf;
+	agnx_write32(ctl, AGNX_SPI_CFG, reg);
+} /* synth_chip_init */
+
+
+static void antenna_init(struct agnx_priv *priv, int num_antenna)
+{
+	void __iomem *ctl = priv->ctl;
+
+	switch (num_antenna) {
+	case 1:
+		agnx_write32(ctl, AGNX_GCR_NLISTANT, 1);
+		agnx_write32(ctl, AGNX_GCR_NMEASANT, 1);
+		agnx_write32(ctl, AGNX_GCR_NACTIANT, 1);
+		agnx_write32(ctl, AGNX_GCR_NCAPTANT, 1);
+
+		agnx_write32(ctl, AGNX_GCR_ANTCFG, 7);
+		agnx_write32(ctl, AGNX_GCR_BOACT, 34);
+		agnx_write32(ctl, AGNX_GCR_BOINACT, 34);
+		agnx_write32(ctl, AGNX_GCR_BODYNA, 30);
+
+		agnx_write32(ctl, AGNX_GCR_THD0A, 125);
+		agnx_write32(ctl, AGNX_GCR_THD0AL, 100);
+		agnx_write32(ctl, AGNX_GCR_THD0B, 90);
+
+		agnx_write32(ctl, AGNX_GCR_THD0BTFEST, 80);
+		agnx_write32(ctl, AGNX_GCR_SIGHTH, 100);
+		agnx_write32(ctl, AGNX_GCR_SIGLTH, 16);
+		break;
+	case 2:
+		agnx_write32(ctl, AGNX_GCR_NLISTANT, 2);
+		agnx_write32(ctl, AGNX_GCR_NMEASANT, 2);
+		agnx_write32(ctl, AGNX_GCR_NACTIANT, 2);
+		agnx_write32(ctl, AGNX_GCR_NCAPTANT, 2);
+		agnx_write32(ctl, AGNX_GCR_ANTCFG, 15);
+		agnx_write32(ctl, AGNX_GCR_BOACT, 36);
+		agnx_write32(ctl, AGNX_GCR_BOINACT, 36);
+		agnx_write32(ctl, AGNX_GCR_BODYNA, 32);
+		agnx_write32(ctl, AGNX_GCR_THD0A, 120);
+		agnx_write32(ctl, AGNX_GCR_THD0AL, 100);
+		agnx_write32(ctl, AGNX_GCR_THD0B, 80);
+		agnx_write32(ctl, AGNX_GCR_THD0BTFEST, 70);
+		agnx_write32(ctl, AGNX_GCR_SIGHTH, 100);
+		agnx_write32(ctl, AGNX_GCR_SIGLTH, 32);
+		break;
+	case 3:
+		agnx_write32(ctl, AGNX_GCR_NLISTANT, 3);
+		agnx_write32(ctl, AGNX_GCR_NMEASANT, 3);
+		agnx_write32(ctl, AGNX_GCR_NACTIANT, 3);
+		agnx_write32(ctl, AGNX_GCR_NCAPTANT, 3);
+		agnx_write32(ctl, AGNX_GCR_ANTCFG, 31);
+		agnx_write32(ctl, AGNX_GCR_BOACT, 36);
+		agnx_write32(ctl, AGNX_GCR_BOINACT, 36);
+		agnx_write32(ctl, AGNX_GCR_BODYNA, 32);
+		agnx_write32(ctl, AGNX_GCR_THD0A, 100);
+		agnx_write32(ctl, AGNX_GCR_THD0AL, 100);
+		agnx_write32(ctl, AGNX_GCR_THD0B, 70);
+		agnx_write32(ctl, AGNX_GCR_THD0BTFEST, 70);
+		agnx_write32(ctl, AGNX_GCR_SIGHTH, 100);
+		agnx_write32(ctl, AGNX_GCR_SIGLTH, 48);
+//		agnx_write32(ctl, AGNX_GCR_SIGLTH, 16);
+		break;
+	default:
+		printk(KERN_WARNING PFX "Unknow antenna number\n");
+	}
+} /* antenna_init */
+
+static void chain_update(struct agnx_priv *priv, u32 chain)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+	AGNX_TRACE;
+
+	spi_rc_write(ctl, RF_CHIP0, 0x20);
+	reg = agnx_read32(ctl, AGNX_SPI_RLSW);
+
+	if (reg == 0x4)
+		spi_rf_write(ctl, RF_CHIP0|RF_CHIP1, reg|0x1000);
+	else if (reg != 0x0)
+     		spi_rf_write(ctl, RF_CHIP0|RF_CHIP1|RF_CHIP2, reg|0x1000);
+        else {
+		if (chain == 3 || chain == 6) {
+			spi_rf_write(ctl, RF_CHIP0|RF_CHIP1|RF_CHIP2, reg|0x1000);
+			agnx_write32(ctl, AGNX_GCR_RXOVERIDE, 0x0);
+		} else if (chain == 2 || chain == 4) {
+			spi_rf_write(ctl, RF_CHIP0|RF_CHIP1, reg|0x1000);
+			spi_rf_write(ctl, RF_CHIP2, 0x1005);
+			agnx_write32(ctl, AGNX_GCR_RXOVERIDE, 0x824);
+		} else if (chain == 1) {
+			spi_rf_write(ctl, RF_CHIP0, reg|0x1000);
+			spi_rf_write(ctl, RF_CHIP1|RF_CHIP2, 0x1004);
+			agnx_write32(ctl, AGNX_GCR_RXOVERIDE, 0xc36);
+		}
+	}
+
+	spi_rc_write(ctl, RF_CHIP0, 0x22);
+	reg = agnx_read32(ctl, AGNX_SPI_RLSW);
+
+	switch (reg) {
+	case 0:
+		spi_rf_write(ctl, RF_CHIP0|RF_CHIP1|RF_CHIP2, 0x1005);
+		break;
+	case 1:
+		spi_rf_write(ctl, RF_CHIP0|RF_CHIP1, 0x1201);
+		break;
+	case 2:
+		if (chain == 6 || chain == 4) {
+			spi_rf_write(ctl, RF_CHIP0|RF_CHIP1, 0x1202);
+			spi_rf_write(ctl, RF_CHIP2, 0x1005);
+		} else if (chain < 3) {
+			spi_rf_write(ctl, RF_CHIP0, 0x1202);
+			spi_rf_write(ctl, RF_CHIP1|RF_CHIP2, 0x1005);
+		}
+		break;
+	default:
+		if (chain == 3) {
+			spi_rf_write(ctl, RF_CHIP0|RF_CHIP1, 0x1203);
+			spi_rf_write(ctl, RF_CHIP2, 0x1201);
+		} else if (chain == 2) {
+			spi_rf_write(ctl, RF_CHIP0, 0x1203);
+			spi_rf_write(ctl, RF_CHIP2, 0x1200);
+			spi_rf_write(ctl, RF_CHIP1, 0x1201);
+		} else if (chain == 1) {
+			spi_rf_write(ctl, RF_CHIP0, 0x1203);
+			spi_rf_write(ctl, RF_CHIP1|RF_CHIP2, 0x1200);
+		} else if (chain == 4) {
+			spi_rf_write(ctl, RF_CHIP0|RF_CHIP1, 0x1203);
+			spi_rf_write(ctl, RF_CHIP2, 0x1201);
+		} else {
+			spi_rf_write(ctl, RF_CHIP0, 0x1203);
+			spi_rf_write(ctl, RF_CHIP1|RF_CHIP2, 0x1201);
+		}
+	}
+} /* chain_update */
+
+static void antenna_config(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+	AGNX_TRACE;
+
+	/* Write 0x0 to the TX Management Control Register Enable bit */
+	reg = agnx_read32(ctl, AGNX_TXM_CTL);
+	reg &= ~0x1;
+	agnx_write32(ctl, AGNX_TXM_CTL, reg);
+
+	/* FIXME */
+	/* Set initial value based on number of Antennae */
+	antenna_init(priv, 3);
+
+	/* FIXME Update Power Templates for current valid Stations */
+	/* sta_power_init(priv, 0);*/
+
+	/* FIXME the number of chains should get from eeprom*/
+	chain_update(priv, AGNX_CHAINS_MAX);
+} /* antenna_config */
+
+void calibrate_oscillator(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+	AGNX_TRACE;
+
+	spi_rc_write(ctl, RF_CHIP0|RF_CHIP1, 0x1201);
+	reg = agnx_read32(ctl, AGNX_GCR_GAINSET1);
+	reg |= 0x10;
+	agnx_write32(ctl, AGNX_GCR_GAINSET1, reg);
+
+	agnx_write32(ctl, AGNX_GCR_GAINSETWRITE, 1);
+	agnx_write32(ctl, AGNX_GCR_RSTGCTL, 1);
+
+	agnx_write32(ctl, AGNX_ACI_LEN, 0x3ff);
+
+	agnx_write32(ctl, AGNX_ACI_TIMER1, 0x27);
+	agnx_write32(ctl, AGNX_ACI_TIMER2, 0x27);
+	/* (Residual DC Calibration) to Calibration Mode */
+	agnx_write32(ctl, AGNX_ACI_MODE, 0x2);
+
+	spi_rc_write(ctl, RF_CHIP0|RF_CHIP1, 0x1004);
+	agnx_write32(ctl, AGNX_ACI_LEN, 0x3ff);
+	/* (TX LO Calibration) to Calibration Mode */
+	agnx_write32(ctl, AGNX_ACI_MODE, 0x4);
+
+	do {
+		u32  reg1, reg2, reg3;
+		/* Enable Power Saving Control */
+		enable_power_saving(priv);
+		/* Save the following registers to restore */
+		reg1 = ioread32(ctl + 0x11000);
+		reg2 = ioread32(ctl + 0xec50);
+		reg3 = ioread32(ctl + 0xec54);
+		wmb();
+
+		agnx_write32(ctl, 0x11000, 0xcfdf);
+		agnx_write32(ctl, 0xec50, 0x70);
+		/* Restore the registers */
+		agnx_write32(ctl, 0x11000, reg1);
+		agnx_write32(ctl, 0xec50, reg2);
+		agnx_write32(ctl, 0xec54, reg3);
+		/* Disable Power Saving Control */
+		disable_power_saving(priv);
+	} while (0);
+
+	agnx_write32(ctl, AGNX_GCR_RSTGCTL, 0);
+} /* calibrate_oscillator */
+
+
+static void radio_channel_set(struct agnx_priv *priv, unsigned int channel)
+{
+	void __iomem *ctl = priv->ctl;
+	unsigned int freq = priv->band.channels[channel - 1].center_freq;
+	u32 reg;
+	AGNX_TRACE;
+
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1, 0x1201);
+	/* Set SPI Clock to 50 Ns */
+	reg = agnx_read32(ctl, AGNX_SPI_CFG);
+	reg &= ~0xF;
+	reg |= 0x1;
+	agnx_write32(ctl, AGNX_SPI_CFG, reg);
+
+	/* Clear the Disable Tx interrupt bit in Interrupt Mask */
+/* 	reg = agnx_read32(ctl, AGNX_INT_MASK); */
+/* 	reg &= ~IRQ_TX_DISABLE; */
+/* 	agnx_write32(ctl, AGNX_INT_MASK, reg); */
+
+	/* Band Selection */
+	reg = agnx_read32(ctl, AGNX_SYSITF_GPIOUT);
+	reg |= 0x8;
+	agnx_write32(ctl, AGNX_SYSITF_GPIOUT, reg);
+
+	/* FIXME Set the SiLabs Chip Frequency */
+	synth_freq_set(priv, channel);
+
+	reg = agnx_read32(ctl, AGNX_PM_SOFTRST);
+	reg |= 0x80100030;
+	agnx_write32(ctl, AGNX_PM_SOFTRST, reg);
+	reg = agnx_read32(ctl, AGNX_PM_PLLCTL);
+	reg |= 0x20009;
+	agnx_write32(ctl, AGNX_PM_PLLCTL, reg);
+
+	agnx_write32(ctl, AGNX_SYSITF_GPIOUT, 0x5);
+
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1|RF_CHIP2, 0x1100);
+
+	/* Load the MonitorGain Table */
+	monitor_gain_table_init(priv);
+
+	/* Load the TX Fir table */
+	tx_fir_table_init(priv);
+
+	reg = agnx_read32(ctl, AGNX_PM_PMCTL);
+	reg |= 0x8;
+	agnx_write32(ctl, AGNX_PM_PMCTL, reg);
+
+	spi_rc_write(ctl, RF_CHIP0|RF_CHIP1, 0x22);
+	udelay(80);
+	reg = agnx_read32(ctl, AGNX_SPI_RLSW);
+
+
+	agnx_write32(ctl, AGNX_GCR_RXOVERIDE, 0xff);
+	agnx_write32(ctl, AGNX_GCR_DISCOVMOD, 0x3);
+
+	reg = agnx_read32(ctl, 0xec50);
+	reg |= 0x4f;
+	agnx_write32(ctl, 0xec50, reg);
+
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1, 0x1201);
+	agnx_write32(ctl, 0x11008, 0x1);
+	agnx_write32(ctl, 0x1100c, 0x0);
+	agnx_write32(ctl, 0x11008, 0x0);
+	agnx_write32(ctl, 0xec50, 0xc);
+
+	agnx_write32(ctl, AGNX_GCR_DISCOVMOD, 0x3);
+	agnx_write32(ctl, AGNX_GCR_RXOVERIDE, 0x0);
+	agnx_write32(ctl, 0x11010, 0x6e);
+	agnx_write32(ctl, 0x11014, 0x6c);
+
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1, 0x1201);
+
+	/* Calibrate the Antenna */
+	/* antenna_calibrate(priv); */
+	/* Calibrate the TxLocalOscillator */
+	calibrate_oscillator(priv);
+
+	reg = agnx_read32(ctl, AGNX_PM_PMCTL);
+	reg &= ~0x8;
+	agnx_write32(ctl, AGNX_PM_PMCTL, reg);
+	agnx_write32(ctl, AGNX_GCR_GAININIT, 0xa);
+	agnx_write32(ctl, AGNX_GCR_THCD, 0x0);
+
+	agnx_write32(ctl, 0x11018, 0xb);
+	agnx_write32(ctl, AGNX_GCR_DISCOVMOD, 0x0);
+
+	/* Write Frequency to Gain Control Channel */
+	agnx_write32(ctl, AGNX_GCR_RXCHANEL, freq);
+	/* Write 0x140000/Freq to 0x9c08 */
+	reg = 0x140000/freq;
+	agnx_write32(ctl, 0x9c08, reg);
+
+	reg = agnx_read32(ctl, AGNX_PM_SOFTRST);
+	reg &= ~0x80100030;
+	agnx_write32(ctl, AGNX_PM_SOFTRST, reg);
+
+	reg = agnx_read32(ctl, AGNX_PM_PLLCTL);
+	reg &= ~0x20009;
+	reg |= 0x1;
+	agnx_write32(ctl, AGNX_PM_PLLCTL, reg);
+
+	agnx_write32(ctl, AGNX_ACI_MODE, 0x0);
+
+/* FIXME According to Number of Chains: */
+
+/* 			   1. 1: */
+/*          1. Write 0x1203 to RF Chip 0 */
+/*          2. Write 0x1200 to RF Chips 1 +2  */
+/* 			   2. 2: */
+/*          1. Write 0x1203 to RF Chip 0 */
+/*          2. Write 0x1200 to RF Chip 2 */
+/*          3. Write 0x1201 to RF Chip 1  */
+/* 			   3. 3: */
+/*          1. Write 0x1203 to RF Chip 0 */
+/*          2. Write 0x1201 to RF Chip 1 + 2  */
+/* 			   4. 4: */
+/*          1. Write 0x1203 to RF Chip 0 + 1 */
+/*          2. Write 0x1200 to RF Chip 2  */
+
+/* 			   5. 6: */
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1, 0x1203);
+	spi_rf_write(ctl, RF_CHIP2, 0x1201);
+
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1|RF_CHIP2, 0x1000);
+	agnx_write32(ctl, AGNX_GCR_RXOVERIDE, 0x0);
+
+	/* FIXME Set the Disable Tx interrupt bit in Interrupt Mask
+	   (Or 0x20000 to Interrupt Mask) */
+/* 	reg = agnx_read32(ctl, AGNX_INT_MASK); */
+/* 	reg |= IRQ_TX_DISABLE; */
+/* 	agnx_write32(ctl, AGNX_INT_MASK, reg); */
+
+	agnx_write32(ctl, AGNX_GCR_RSTGCTL, 0x1);
+	agnx_write32(ctl, AGNX_GCR_RSTGCTL, 0x0);
+
+	/* Configure the Antenna */
+	antenna_config(priv);
+
+	/* Write 0x0 to Discovery Mode Enable detect G, B, A packet? */
+	agnx_write32(ctl, AGNX_GCR_DISCOVMOD, 0);
+
+	reg = agnx_read32(ctl, AGNX_RXM_REQRATE);
+	reg |= 0x80000000;
+	agnx_write32(ctl, AGNX_RXM_REQRATE, reg);
+	agnx_write32(ctl, AGNX_GCR_RSTGCTL, 0x1);
+	agnx_write32(ctl, AGNX_GCR_RSTGCTL, 0x0);
+
+	/* enable radio on and the power LED */
+	reg = agnx_read32(ctl, AGNX_SYSITF_GPIOUT);
+	reg &= ~0x1;
+	reg |= 0x2;
+	agnx_write32(ctl, AGNX_SYSITF_GPIOUT, reg);
+
+	reg = agnx_read32(ctl, AGNX_TXM_CTL);
+	reg |= 0x1;
+	agnx_write32(ctl, AGNX_TXM_CTL, reg);
+} /* radio_channel_set */
+
+static void base_band_filter_calibrate(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1|RF_CHIP2, 0x1700);
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1|RF_CHIP2, 0x1001);
+	agnx_write32(ctl, AGNX_GCR_FORCECTLCLK, 0x0);
+	spi_rc_write(ctl, RF_CHIP0, 0x27);
+	spi_rc_write(ctl, RF_CHIP1, 0x27);
+	spi_rc_write(ctl, RF_CHIP2, 0x27);
+	agnx_write32(ctl, AGNX_GCR_FORCECTLCLK, 0x1);
+}
+
+static void print_offset(struct agnx_priv *priv, u32 chain)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 offset;
+
+	iowrite32((chain), ctl + AGNX_ACI_SELCHAIN);
+	udelay(10);
+	offset = (ioread32(ctl + AGNX_ACI_OFFSET));
+	printk(PFX "Chain is 0x%x, Offset is 0x%x\n", chain, offset);
+}
+
+void print_offsets(struct agnx_priv *priv)
+{
+	print_offset(priv, 0);
+	print_offset(priv, 4);
+	print_offset(priv, 1);
+	print_offset(priv, 5);
+	print_offset(priv, 2);
+	print_offset(priv, 6);
+}
+
+
+struct chains {
+	u32 cali;		/* calibrate  value*/
+
+#define  NEED_CALIBRATE		0
+#define  SUCCESS_CALIBRATE	1
+	int status;
+};
+
+static void chain_calibrate(struct agnx_priv *priv, struct chains *chains,
+			    unsigned int num)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 calibra = chains[num].cali;
+
+	if (num < 3)
+		calibra |= 0x1400;
+	else
+		calibra |= 0x1500;
+
+	switch (num) {
+	case 0:
+	case 4:
+		spi_rf_write(ctl, RF_CHIP0, calibra);
+		break;
+	case 1:
+	case 5:
+		spi_rf_write(ctl, RF_CHIP1, calibra);
+		break;
+	case 2:
+	case 6:
+		spi_rf_write(ctl, RF_CHIP2, calibra);
+		break;
+	default:
+		BUG();
+	}
+} /* chain_calibrate */
+
+
+static void inline get_calibrete_value(struct agnx_priv *priv, struct chains *chains,
+				       unsigned int num)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 offset;
+
+	iowrite32((num), ctl + AGNX_ACI_SELCHAIN);
+	/* FIXME */
+	udelay(10);
+	offset = (ioread32(ctl + AGNX_ACI_OFFSET));
+
+	if (offset < 0xf) {
+		chains[num].status = SUCCESS_CALIBRATE;
+		return;
+	}
+
+	if (num == 0 || num == 1 || num == 2) {
+		if ( 0 == chains[num].cali)
+			chains[num].cali = 0xff;
+		else
+			chains[num].cali--;
+	} else
+		chains[num].cali++;
+
+	chains[num].status = NEED_CALIBRATE;
+}
+
+static inline void calibra_delay(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+	unsigned int i = 100;
+
+	wmb();
+	while (i--) {
+		reg = (ioread32(ctl + AGNX_ACI_STATUS));
+		if (reg == 0x4000)
+			break;
+		udelay(10);
+	}
+	if (!i)
+		printk(PFX "calibration failed\n");
+}
+
+void do_calibration(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	struct chains chains[7];
+	unsigned int i, j;
+	AGNX_TRACE;
+
+	for (i = 0; i < 7; i++) {
+		if (i == 3)
+			continue;
+
+		chains[i].cali = 0x7f;
+		chains[i].status = NEED_CALIBRATE;
+	}
+
+	/* FIXME 0x300 is a magic number */
+	for (j = 0; j < 0x300; j++) {
+		if (chains[0].status == SUCCESS_CALIBRATE &&
+		    chains[1].status == SUCCESS_CALIBRATE &&
+		    chains[2].status == SUCCESS_CALIBRATE &&
+		    chains[4].status == SUCCESS_CALIBRATE &&
+		    chains[5].status == SUCCESS_CALIBRATE &&
+		    chains[6].status == SUCCESS_CALIBRATE)
+			break;
+
+		/* Attention, there is no chain 3 */
+		for (i = 0; i < 7; i++) {
+			if (i == 3)
+				continue;
+			if (chains[i].status == NEED_CALIBRATE)
+				chain_calibrate(priv, chains, i);
+		}
+		/* Write 0x1 to Calibration Measure */
+		iowrite32((0x1), ctl + AGNX_ACI_MEASURE);
+		calibra_delay(priv);
+
+		for (i = 0; i < 7; i++) {
+			if (i == 3)
+				continue;
+
+			get_calibrete_value(priv, chains, i);
+		}
+	}
+	printk(PFX "Clibrate times is %d\n", j);
+	print_offsets(priv);
+} /* do_calibration */
+
+void antenna_calibrate(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+	AGNX_TRACE;
+
+	agnx_write32(ctl, AGNX_GCR_NLISTANT, 0x3);
+	agnx_write32(ctl, AGNX_GCR_NMEASANT, 0x3);
+	agnx_write32(ctl, AGNX_GCR_NACTIANT, 0x3);
+	agnx_write32(ctl, AGNX_GCR_NCAPTANT, 0x3);
+
+	agnx_write32(ctl, AGNX_GCR_ANTCFG, 0x1f);
+	agnx_write32(ctl, AGNX_GCR_BOACT, 0x24);
+	agnx_write32(ctl, AGNX_GCR_BOINACT, 0x24);
+	agnx_write32(ctl, AGNX_GCR_BODYNA, 0x20);
+	agnx_write32(ctl, AGNX_GCR_THD0A, 0x64);
+	agnx_write32(ctl, AGNX_GCR_THD0AL, 0x64);
+	agnx_write32(ctl, AGNX_GCR_THD0B, 0x46);
+	agnx_write32(ctl, AGNX_GCR_THD0BTFEST, 0x3c);
+	agnx_write32(ctl, AGNX_GCR_SIGHTH, 0x64);
+	agnx_write32(ctl, AGNX_GCR_SIGLTH, 0x30);
+
+	spi_rc_write(ctl, RF_CHIP0, 0x20);
+	/* Fixme */
+	udelay(80);
+	/*    1. Should read 0x0  */
+	reg = agnx_read32(ctl, AGNX_SPI_RLSW);
+	if (0x0 != reg)
+		printk(KERN_WARNING PFX "Unmatched rf chips result\n");
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1|RF_CHIP2, 0x1000);
+
+	agnx_write32(ctl, AGNX_GCR_RXOVERIDE, 0x0);
+
+	spi_rc_write(ctl, RF_CHIP0, 0x22);
+	udelay(80);
+	reg = agnx_read32(ctl, AGNX_SPI_RLSW);
+	if (0x0 != reg)
+		printk(KERN_WARNING PFX "Unmatched rf chips result\n");
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1|RF_CHIP2, 0x1005);
+
+	agnx_write32(ctl, AGNX_GCR_RSTGCTL, 0x1);
+	agnx_write32(ctl, AGNX_GCR_RSTGCTL, 0x0);
+
+	reg = agnx_read32(ctl, AGNX_PM_SOFTRST);
+	reg |= 0x1c000032;
+	agnx_write32(ctl, AGNX_PM_SOFTRST, reg);
+	reg = agnx_read32(ctl, AGNX_PM_PLLCTL);
+	reg |= 0x0003f07;
+	agnx_write32(ctl, AGNX_PM_PLLCTL, reg);
+
+	reg = agnx_read32(ctl, 0xec50);
+	reg |= 0x40;
+	agnx_write32(ctl, 0xec50, reg);
+
+	agnx_write32(ctl, AGNX_GCR_RXOVERIDE, 0xff8);
+	agnx_write32(ctl, AGNX_GCR_DISCOVMOD, 0x3);
+
+	agnx_write32(ctl, AGNX_GCR_CHAINNUM, 0x6);
+	agnx_write32(ctl, 0x19874, 0x0);
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1|RF_CHIP2, 0x1700);
+
+	/* Calibrate the BaseBandFilter */
+	base_band_filter_calibrate(priv);
+
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1|RF_CHIP2, 0x1002);
+
+	agnx_write32(ctl, AGNX_GCR_GAINSET0, 0x1d);
+	agnx_write32(ctl, AGNX_GCR_GAINSET1, 0x1d);
+	agnx_write32(ctl, AGNX_GCR_GAINSET2, 0x1d);
+	agnx_write32(ctl, AGNX_GCR_GAINSETWRITE, 0x1);
+
+	agnx_write32(ctl, AGNX_ACI_MODE, 0x1);
+	agnx_write32(ctl, AGNX_ACI_LEN, 0x3ff);
+
+	agnx_write32(ctl, AGNX_ACI_TIMER1, 0x27);
+	agnx_write32(ctl, AGNX_ACI_TIMER2, 0x27);
+
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1|RF_CHIP2, 0x1400);
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1|RF_CHIP2, 0x1500);
+
+	/* Measure Calibration */
+	agnx_write32(ctl, AGNX_ACI_MEASURE, 0x1);
+	calibra_delay(priv);
+
+	/* do calibration */
+	do_calibration(priv);
+
+	agnx_write32(ctl, AGNX_GCR_RXOVERIDE, 0x0);
+	agnx_write32(ctl, AGNX_ACI_TIMER1, 0x21);
+	agnx_write32(ctl, AGNX_ACI_TIMER2, 0x27);
+	agnx_write32(ctl, AGNX_ACI_LEN, 0xf);
+
+	reg = agnx_read32(ctl, AGNX_GCR_GAINSET0);
+	reg &= 0xf;
+	agnx_write32(ctl, AGNX_GCR_GAINSET0, reg);
+	reg = agnx_read32(ctl, AGNX_GCR_GAINSET1);
+	reg &= 0xf;
+	agnx_write32(ctl, AGNX_GCR_GAINSET1, reg);
+	reg = agnx_read32(ctl, AGNX_GCR_GAINSET2);
+	reg &= 0xf;
+	agnx_write32(ctl, AGNX_GCR_GAINSET2, reg);
+
+	agnx_write32(ctl, AGNX_GCR_GAINSETWRITE, 0x0);
+	disable_receiver(priv);
+} /* antenna_calibrate */
+
+void __antenna_calibrate(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+
+	/* Calibrate the BaseBandFilter */
+	/* base_band_filter_calibrate(priv); */
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1|RF_CHIP2, 0x1002);
+
+
+	agnx_write32(ctl, AGNX_GCR_GAINSET0, 0x1d);
+	agnx_write32(ctl, AGNX_GCR_GAINSET1, 0x1d);
+	agnx_write32(ctl, AGNX_GCR_GAINSET2, 0x1d);
+
+	agnx_write32(ctl, AGNX_GCR_GAINSETWRITE, 0x1);
+
+	agnx_write32(ctl, AGNX_ACI_MODE, 0x1);
+	agnx_write32(ctl, AGNX_ACI_LEN, 0x3ff);
+
+
+	agnx_write32(ctl, AGNX_ACI_TIMER1, 0x27);
+	agnx_write32(ctl, AGNX_ACI_TIMER2, 0x27);
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1|RF_CHIP2, 0x1400);
+	spi_rf_write(ctl, RF_CHIP0|RF_CHIP1|RF_CHIP2, 0x1500);
+	/* Measure Calibration */
+	agnx_write32(ctl, AGNX_ACI_MEASURE, 0x1);
+	calibra_delay(priv);
+	do_calibration(priv);
+	agnx_write32(ctl, AGNX_GCR_RXOVERIDE, 0x0);
+
+	agnx_write32(ctl, AGNX_ACI_TIMER1, 0x21);
+	agnx_write32(ctl, AGNX_ACI_TIMER2, 0x27);
+
+	agnx_write32(ctl, AGNX_ACI_LEN, 0xf);
+
+	reg = agnx_read32(ctl, AGNX_GCR_GAINSET0);
+	reg &= 0xf;
+	agnx_write32(ctl, AGNX_GCR_GAINSET0, reg);
+	reg = agnx_read32(ctl, AGNX_GCR_GAINSET1);
+	reg &= 0xf;
+	agnx_write32(ctl, AGNX_GCR_GAINSET1, reg);
+	reg = agnx_read32(ctl, AGNX_GCR_GAINSET2);
+	reg &= 0xf;
+	agnx_write32(ctl, AGNX_GCR_GAINSET2, reg);
+
+
+	agnx_write32(ctl, AGNX_GCR_GAINSETWRITE, 0x0);
+
+	/* Write 0x3 Gain Control Discovery Mode */
+	enable_receiver(priv);
+}
+
+int agnx_set_channel(struct agnx_priv *priv, unsigned int channel)
+{
+	AGNX_TRACE;
+
+	printk(KERN_ERR PFX "Channel is %d %s\n", channel, __func__);
+	radio_channel_set(priv, channel);
+	return 0;
+}
diff --git a/drivers/staging/agnx/sta.c b/drivers/staging/agnx/sta.c
new file mode 100644
index 00000000000..d3ac675e45b
--- /dev/null
+++ b/drivers/staging/agnx/sta.c
@@ -0,0 +1,219 @@
+#include <linux/delay.h>
+#include <linux/etherdevice.h>
+#include "phy.h"
+#include "sta.h"
+#include "debug.h"
+
+void hash_read(struct agnx_priv *priv, u32 reghi, u32 reglo, u8 sta_id)
+{
+	void __iomem *ctl = priv->ctl;
+
+	reglo &= 0xFFFF;
+	reglo |= 0x30000000;
+	reglo |= 0x40000000;	/* Set status busy */
+	reglo |= sta_id << 16;
+
+	iowrite32(0, ctl + AGNX_RXM_HASH_CMD_FLAG);
+	iowrite32(reghi, ctl + AGNX_RXM_HASH_CMD_HIGH);
+	iowrite32(reglo, ctl + AGNX_RXM_HASH_CMD_LOW);
+
+	reghi = ioread32(ctl + AGNX_RXM_HASH_CMD_HIGH);
+        reglo = ioread32(ctl + AGNX_RXM_HASH_CMD_LOW);
+	printk(PFX "RX hash cmd are : %.8x%.8x\n", reghi, reglo);
+}
+
+void hash_write(struct agnx_priv *priv, u8 *mac_addr, u8 sta_id)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reghi, reglo;
+
+	if (!is_valid_ether_addr(mac_addr))
+		printk(KERN_WARNING PFX "Update hash table: Invalid hwaddr!\n");
+
+	reghi = mac_addr[0] << 24 | mac_addr[1] << 16 | mac_addr[2] << 8 | mac_addr[3];
+	reglo = mac_addr[4] << 8 | mac_addr[5];
+	reglo |= 0x10000000;	/* Set hash commmand */
+	reglo |= 0x40000000;	/* Set status busy */
+	reglo |= sta_id << 16;
+
+	iowrite32(0, ctl + AGNX_RXM_HASH_CMD_FLAG);
+	iowrite32(reghi, ctl + AGNX_RXM_HASH_CMD_HIGH);
+	iowrite32(reglo, ctl + AGNX_RXM_HASH_CMD_LOW);
+
+        reglo = ioread32(ctl + AGNX_RXM_HASH_CMD_LOW);
+	if (!(reglo & 0x80000000))
+		printk(KERN_WARNING PFX "Update hash table failed\n");
+}
+
+void hash_delete(struct agnx_priv *priv, u32 reghi, u32 reglo, u8 sta_id)
+{
+	void __iomem *ctl = priv->ctl;
+
+	reglo &= 0xFFFF;
+	reglo |= 0x20000000;
+	reglo |= 0x40000000;	/* Set status busy */
+	reglo |= sta_id << 16;
+
+	iowrite32(0, ctl + AGNX_RXM_HASH_CMD_FLAG);
+	iowrite32(reghi, ctl + AGNX_RXM_HASH_CMD_HIGH);
+	iowrite32(reglo, ctl + AGNX_RXM_HASH_CMD_LOW);
+	reghi = ioread32(ctl + AGNX_RXM_HASH_CMD_HIGH);
+
+        reglo = ioread32(ctl + AGNX_RXM_HASH_CMD_LOW);
+	printk(PFX "RX hash cmd are : %.8x%.8x\n", reghi, reglo);
+
+}
+
+void hash_dump(struct agnx_priv *priv, u8 sta_id)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reghi, reglo;
+
+	reglo = 0x0;		/* dump command */
+	reglo|= 0x40000000;  	/* status bit */
+	iowrite32(reglo, ctl + AGNX_RXM_HASH_CMD_LOW);
+	iowrite32(sta_id << 16, ctl + AGNX_RXM_HASH_DUMP_DATA);
+
+	udelay(80);
+
+	reghi = ioread32(ctl + AGNX_RXM_HASH_CMD_HIGH);
+        reglo = ioread32(ctl + AGNX_RXM_HASH_CMD_LOW);
+	printk(PFX "hash cmd are : %.8x%.8x\n", reghi, reglo);
+	reghi = ioread32(ctl + AGNX_RXM_HASH_CMD_FLAG);
+	printk(PFX "hash flag is : %.8x\n", reghi);
+	reghi = ioread32(ctl + AGNX_RXM_HASH_DUMP_MST);
+	reglo = ioread32(ctl + AGNX_RXM_HASH_DUMP_LST);
+	printk(PFX "hash dump mst lst: %.8x%.8x\n", reghi, reglo);
+	reghi = ioread32(ctl + AGNX_RXM_HASH_DUMP_DATA);
+	printk(PFX "hash dump data: %.8x\n", reghi);
+}
+
+void get_sta_power(struct agnx_priv *priv, struct agnx_sta_power *power, unsigned int sta_idx)
+{
+	void __iomem *ctl = priv->ctl;
+        memcpy_fromio(power, ctl + AGNX_TXM_STAPOWTEMP + sizeof(*power) * sta_idx,
+		      sizeof(*power));
+}
+
+inline void
+set_sta_power(struct agnx_priv *priv, struct agnx_sta_power *power, unsigned int sta_idx)
+{
+	void __iomem *ctl = priv->ctl;
+	/* FIXME   2. Write Template to offset + station number  */
+        memcpy_toio(ctl + AGNX_TXM_STAPOWTEMP + sizeof(*power) * sta_idx,
+		    power, sizeof(*power));
+}
+
+
+void get_sta_tx_wq(struct agnx_priv *priv, struct agnx_sta_tx_wq *tx_wq,
+		   unsigned int sta_idx, unsigned int wq_idx)
+{
+	void __iomem *data = priv->data;
+	memcpy_fromio(tx_wq, data + AGNX_PDU_TX_WQ + sizeof(*tx_wq) * STA_TX_WQ_NUM * sta_idx +
+		      sizeof(*tx_wq) * wq_idx,  sizeof(*tx_wq));
+
+}
+
+inline void set_sta_tx_wq(struct agnx_priv *priv, struct agnx_sta_tx_wq *tx_wq,
+		   unsigned int sta_idx, unsigned int wq_idx)
+{
+	void __iomem *data = priv->data;
+	memcpy_toio(data + AGNX_PDU_TX_WQ + sizeof(*tx_wq) * STA_TX_WQ_NUM * sta_idx +
+		    sizeof(*tx_wq) * wq_idx, tx_wq, sizeof(*tx_wq));
+}
+
+
+void get_sta(struct agnx_priv *priv, struct agnx_sta *sta, unsigned int sta_idx)
+{
+	void __iomem *data = priv->data;
+
+	memcpy_fromio(sta, data + AGNX_PDUPOOL + sizeof(*sta) * sta_idx,
+		      sizeof(*sta));
+}
+
+inline void set_sta(struct agnx_priv *priv, struct agnx_sta *sta, unsigned int sta_idx)
+{
+	void __iomem *data = priv->data;
+
+        memcpy_toio(data + AGNX_PDUPOOL + sizeof(*sta) * sta_idx,
+		    sta, sizeof(*sta));
+}
+
+/* FIXME */
+void sta_power_init(struct agnx_priv *priv, unsigned int sta_idx)
+{
+	struct agnx_sta_power power;
+	u32 reg;
+	AGNX_TRACE;
+
+	memset(&power, 0, sizeof(power));
+	reg = agnx_set_bits(EDCF, EDCF_SHIFT, 0x1);
+	power.reg = cpu_to_le32(reg);
+	set_sta_power(priv, &power, sta_idx);
+	udelay(40);
+} /* add_power_template */
+
+
+/* @num: The #number of station that is visible to the card */
+static void sta_tx_workqueue_init(struct agnx_priv *priv, unsigned int sta_idx)
+{
+	struct agnx_sta_tx_wq tx_wq;
+	u32 reg;
+	unsigned int i;
+
+	memset(&tx_wq, 0, sizeof(tx_wq));
+
+	reg = agnx_set_bits(WORK_QUEUE_VALID, WORK_QUEUE_VALID_SHIFT, 1);
+	reg |= agnx_set_bits(WORK_QUEUE_ACK_TYPE, WORK_QUEUE_ACK_TYPE_SHIFT, 1);
+//	reg |= agnx_set_bits(WORK_QUEUE_ACK_TYPE, WORK_QUEUE_ACK_TYPE_SHIFT, 0);
+	tx_wq.reg2 |= cpu_to_le32(reg);
+
+	/* Suppose all 8 traffic class are used */
+	for (i = 0; i < STA_TX_WQ_NUM; i++)
+		set_sta_tx_wq(priv, &tx_wq, sta_idx, i);
+} /* sta_tx_workqueue_init */
+
+
+static void sta_traffic_init(struct agnx_sta_traffic *traffic)
+{
+	u32 reg;
+	memset(traffic, 0, sizeof(*traffic));
+
+	reg = agnx_set_bits(NEW_PACKET, NEW_PACKET_SHIFT, 1);
+	reg |= agnx_set_bits(TRAFFIC_VALID, TRAFFIC_VALID_SHIFT, 1);
+//	reg |= agnx_set_bits(TRAFFIC_ACK_TYPE, TRAFFIC_ACK_TYPE_SHIFT, 1);
+	traffic->reg0 = cpu_to_le32(reg);
+
+	/* 	3. setting RX Sequence Number to 4095 */
+	reg = agnx_set_bits(RX_SEQUENCE_NUM, RX_SEQUENCE_NUM_SHIFT, 4095);
+	traffic->reg1 = cpu_to_le32(reg);
+}
+
+
+/* @num: The #number of station that is visible to the card */
+void sta_init(struct agnx_priv *priv, unsigned int sta_idx)
+{
+	/* FIXME the length of sta is 256 bytes Is that
+	 * dangerous to stack overflow? */
+	struct agnx_sta sta;
+	u32 reg;
+	int i;
+
+	memset(&sta, 0, sizeof(sta));
+	/* Set valid to 1 */
+	reg = agnx_set_bits(STATION_VALID, STATION_VALID_SHIFT, 1);
+	/* Set Enable Concatenation to 0 (?) */
+	reg |= agnx_set_bits(ENABLE_CONCATENATION, ENABLE_CONCATENATION_SHIFT, 0);
+	/* Set Enable Decompression to 0 (?) */
+	reg |= agnx_set_bits(ENABLE_DECOMPRESSION, ENABLE_DECOMPRESSION_SHIFT, 0);
+	sta.reg = cpu_to_le32(reg);
+
+	/* Initialize each of the Traffic Class Structures by: */
+	for (i = 0; i < 8; i++)
+		sta_traffic_init(sta.traffic + i);
+
+	set_sta(priv, &sta, sta_idx);
+	sta_tx_workqueue_init(priv, sta_idx);
+} /* sta_descriptor_init */
+
+
diff --git a/drivers/staging/agnx/sta.h b/drivers/staging/agnx/sta.h
new file mode 100644
index 00000000000..58d0b12900d
--- /dev/null
+++ b/drivers/staging/agnx/sta.h
@@ -0,0 +1,222 @@
+#ifndef AGNX_STA_H_
+#define AGNX_STA_H_
+
+#define STA_TX_WQ_NUM	8	/* The number of TX workqueue one STA has */
+
+struct agnx_hash_cmd {
+	__be32 cmdhi;
+#define MACLO		0xFFFF0000
+#define MACLO_SHIFT	16
+#define STA_ID		0x0000FFF0
+#define STA_ID_SHIFT	4
+#define CMD		0x0000000C
+#define CMD_SHIFT	2
+#define STATUS		0x00000002
+#define STATUS_SHIFT	1
+#define PASS		0x00000001
+#define PASS_SHIFT	1
+	__be32 cmdlo;
+}__attribute__((__packed__));
+
+
+/*
+ * Station Power Template
+ * FIXME Just for agn100 yet
+ */
+struct agnx_sta_power {
+	__le32 reg;
+#define SIGNAL			0x000000FF /* signal */
+#define SIGNAL_SHIFT		0
+#define RATE			0x00000F00
+#define RATE_SHIFT		8
+#define TIFS			0x00001000
+#define TIFS_SHIFT		12
+#define EDCF			0x00002000
+#define EDCF_SHIFT		13
+#define CHANNEL_BOND		0x00004000
+#define CHANNEL_BOND_SHIFT	14
+#define PHY_MODE		0x00038000
+#define PHY_MODE_SHIFT		15
+#define POWER_LEVEL		0x007C0000
+#define POWER_LEVEL_SHIFT	18
+#define NUM_TRANSMITTERS	0x00800000
+#define NUM_TRANSMITTERS_SHIFT	23
+} __attribute__((__packed__));
+
+/*
+ * TX Workqueue Descriptor
+ */
+struct agnx_sta_tx_wq {
+	__le32 reg0;
+#define HEAD_POINTER_LOW	0xFF000000 /* Head pointer low */
+#define HEAD_POINTER_LOW_SHIFT	24
+#define TAIL_POINTER		0x00FFFFFF /* Tail pointer */
+#define TAIL_POINTER_SHIFT	0
+
+	__le32 reg3;
+#define ACK_POINTER_LOW	        0xFFFF0000	/* ACK pointer low */
+#define ACK_POINTER_LOW_SHIFT	16
+#define HEAD_POINTER_HIGH	0x0000FFFF	/* Head pointer high */
+#define HEAD_POINTER_HIGH_SHIFT	0
+
+	__le32 reg1;
+/* ACK timeout tail packet count */
+#define ACK_TIMOUT_TAIL_PACK_CNT	0xFFF00000
+#define ACK_TIMOUT_TAIL_PACK_CNT_SHIFT	20
+/* Head timeout tail packet count */
+#define HEAD_TIMOUT_TAIL_PACK_CNT	0x000FFF00
+#define HEAD_TIMOUT_TAIL_PACK_CNT_SHIFT	8
+#define ACK_POINTER_HIGH	        0x000000FF /* ACK pointer high */
+#define ACK_POINTER_HIGH_SHIFT		0
+
+	__le32 reg2;
+#define WORK_QUEUE_VALID		0x80000000 /* valid */
+#define WORK_QUEUE_VALID_SHIFT		31
+#define WORK_QUEUE_ACK_TYPE		0x40000000 /* ACK type */
+#define WORK_QUEUE_ACK_TYPE_SHIFT	30
+/* Head timeout window limit fragmentation count */
+#define HEAD_TIMOUT_WIN_LIM_FRAG_CNT	0x3FFF0000
+#define HEAD_TIMOUT_WIN_LIM_FRAG_CNT_SHIFT	16
+/* Head timeout window limit byte count */
+#define HEAD_TIMOUT_WIN_LIM_BYTE_CNT	0x0000FFFF
+#define HEAD_TIMOUT_WIN_LIM_BYTE_CNT_SHIFT	 0
+} __attribute__((__packed__));
+
+
+/*
+ * Traffic Class Structure
+ */
+struct agnx_sta_traffic {
+	__le32 reg0;
+#define ACK_TIMOUT_CNT		0xFF800000 /* ACK Timeout Counts */
+#define ACK_TIMOUT_CNT_SHIFT	23
+#define TRAFFIC_ACK_TYPE	0x00600000 /* ACK Type */
+#define TRAFFIC_ACK_TYPE_SHIFT	21
+#define NEW_PACKET		0x00100000 /* New Packet  */
+#define NEW_PACKET_SHIFT	20
+#define TRAFFIC_VALID		0x00080000 /* Valid */
+#define TRAFFIC_VALID_SHIFT	19
+#define RX_HDR_DESC_POINTER	0x0007FFFF /* RX Header Descripter pointer */
+#define RX_HDR_DESC_POINTER_SHIFT	 0
+
+	__le32 reg1;
+#define RX_PACKET_TIMESTAMP	0xFFFF0000 /* RX Packet Timestamp */
+#define RX_PACKET_TIMESTAMP_SHIFT	16
+#define TRAFFIC_RESERVED	0x0000E000 /* Reserved */
+#define TRAFFIC_RESERVED_SHIFT  13
+#define SV			0x00001000 /* sv */
+#define SV_SHIFT		12
+#define RX_SEQUENCE_NUM		0x00000FFF /* RX Sequence Number */
+#define RX_SEQUENCE_NUM_SHIFT	0
+
+	__le32 tx_replay_cnt_low; /* TX Replay Counter Low */
+
+	__le16 tx_replay_cnt_high; /* TX Replay Counter High */
+	__le16 rx_replay_cnt_high; /* RX Replay Counter High */
+
+	__be32 rx_replay_cnt_low; /* RX Replay Counter Low */
+} __attribute__((__packed__));
+
+/*
+ * Station Descriptors
+ */
+struct agnx_sta {
+	__le32 tx_session_keys[4]; /* Transmit Session Key (0-3) */
+	__le32 rx_session_keys[4]; /* Receive Session Key (0-3) */
+
+	__le32 reg;
+#define ID_1			0xC0000000 /* id 1 */
+#define ID_1_SHIFT		30
+#define ID_0			0x30000000 /* id 0 */
+#define ID_0_SHIFT		28
+#define ENABLE_CONCATENATION	0x0FF00000 /* Enable concatenation */
+#define ENABLE_CONCATENATION_SHIFT	20
+#define ENABLE_DECOMPRESSION	0x000FF000 /* Enable decompression */
+#define ENABLE_DECOMPRESSION_SHIFT	12
+#define STA_RESERVED		0x00000C00 /* Reserved */
+#define STA_RESERVED_SHIFT	10
+#define EAP			0x00000200 /* EAP */
+#define EAP_SHIFT		9
+#define ED_NULL			0x00000100 /* ED NULL */
+#define ED_NULL_SHIFT		8
+#define ENCRYPTION_POLICY	0x000000E0 /* Encryption Policy */
+#define ENCRYPTION_POLICY_SHIFT 5
+#define DEFINED_KEY_ID		0x00000018 /* Defined Key ID */
+#define DEFINED_KEY_ID_SHIFT	3
+#define FIXED_KEY		0x00000004 /* Fixed Key */
+#define FIXED_KEY_SHIFT		2
+#define KEY_VALID		0x00000002 /* Key Valid */
+#define KEY_VALID_SHIFT		1
+#define STATION_VALID		0x00000001 /* Station Valid */
+#define STATION_VALID_SHIFT	0
+
+	__le32 tx_aes_blks_unicast; /* TX AES Blks Unicast */
+	__le32 rx_aes_blks_unicast; /* RX AES Blks Unicast */
+
+	__le16 aes_format_err_unicast_cnt; /* AES Format Error Unicast Counts */
+	__le16 aes_replay_unicast; /* AES Replay Unicast */
+
+	__le16 aes_decrypt_err_unicast;	/* AES Decrypt Error Unicast */
+	__le16 aes_decrypt_err_default;	/* AES Decrypt Error default */
+
+	__le16 single_retry_packets; /* Single Retry Packets */
+	__le16 failed_tx_packets; /* Failed Tx Packets */
+
+	__le16 muti_retry_packets; /* Multiple Retry Packets */
+	__le16 ack_timeouts;	/* ACK Timeouts */
+
+	__le16 frag_tx_cnt;	/* Fragment TX Counts */
+	__le16 rts_brq_sent;	/* RTS Brq Sent */
+
+	__le16 tx_packets;	/* TX Packets */
+	__le16 cts_back_timeout; /* CTS Back Timeout */
+
+	__le32 phy_stats_high;	/* PHY Stats High */
+	__le32 phy_stats_low;	/* PHY Stats Low */
+
+	struct agnx_sta_traffic traffic[8];	/* Traffic Class Structure (8) */
+
+	__le16 traffic_class0_frag_success; /* Traffic Class 0 Fragment Success */
+	__le16 traffic_class1_frag_success; /* Traffic Class 1 Fragment Success */
+	__le16 traffic_class2_frag_success; /* Traffic Class 2 Fragment Success */
+	__le16 traffic_class3_frag_success; /* Traffic Class 3 Fragment Success */
+	__le16 traffic_class4_frag_success; /* Traffic Class 4 Fragment Success */
+	__le16 traffic_class5_frag_success; /* Traffic Class 5 Fragment Success */
+	__le16 traffic_class6_frag_success; /* Traffic Class 6 Fragment Success */
+	__le16 traffic_class7_frag_success; /* Traffic Class 7 Fragment Success */
+
+	__le16 num_frag_non_prime_rates; /* number of Fragments for non-prime rates */
+	__le16 ack_timeout_non_prime_rates; /* ACK Timeout for non-prime rates */
+
+} __attribute__((__packed__));
+
+
+struct agnx_beacon_hdr {
+	struct agnx_sta_power power; /* Tx Station Power Template  */
+	u8 phy_hdr[6];		/* PHY Hdr */
+	u8 frame_len_lo;	/* Frame Length Lo */
+	u8 frame_len_hi;	/* Frame Length Hi */
+	u8 mac_hdr[24];		/* MAC Header */
+	/* FIXME */
+	/* 802.11(abg) beacon */
+} __attribute__((__packed__));
+
+void hash_write(struct agnx_priv *priv, u8 *mac_addr, u8 sta_id);
+void hash_dump(struct agnx_priv *priv, u8 sta_id);
+void hash_read(struct agnx_priv *priv, u32 reghi, u32 reglo, u8 sta_id);
+void hash_delete(struct agnx_priv *priv, u32 reghi, u32 reglo, u8 sta_id);
+
+void get_sta_power(struct agnx_priv *priv, struct agnx_sta_power *power, unsigned int sta_idx);
+void set_sta_power(struct agnx_priv *priv, struct agnx_sta_power *power,
+		   unsigned int sta_idx);
+void get_sta_tx_wq(struct agnx_priv *priv, struct agnx_sta_tx_wq *tx_wq,
+		   unsigned int sta_idx, unsigned int wq_idx);
+void set_sta_tx_wq(struct agnx_priv *priv, struct agnx_sta_tx_wq *tx_wq,
+		   unsigned int sta_idx, unsigned int wq_idx);
+void get_sta(struct agnx_priv *priv, struct agnx_sta *sta, unsigned int sta_idx);
+void set_sta(struct agnx_priv *priv, struct agnx_sta *sta, unsigned int sta_idx);
+
+void sta_power_init(struct agnx_priv *priv, unsigned int num);
+void sta_init(struct agnx_priv *priv, unsigned int num);
+
+#endif /* AGNX_STA_H_ */
diff --git a/drivers/staging/agnx/table.c b/drivers/staging/agnx/table.c
new file mode 100644
index 00000000000..c60048487b5
--- /dev/null
+++ b/drivers/staging/agnx/table.c
@@ -0,0 +1,168 @@
+#include <linux/pci.h>
+#include <linux/delay.h>
+#include "agnx.h"
+#include "debug.h"
+#include "phy.h"
+
+static const u32
+tx_fir_table[] = { 0x19, 0x5d, 0xce, 0x151, 0x1c3, 0x1ff, 0x1ea, 0x17c, 0xcf,
+		   0x19, 0x38e, 0x350, 0x362, 0x3ad, 0x5, 0x44, 0x59, 0x49,
+		   0x21, 0x3f7, 0x3e0, 0x3e3, 0x3f3, 0x0 };
+
+void tx_fir_table_init(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(tx_fir_table); i++)
+		iowrite32(tx_fir_table[i], ctl + AGNX_FIR_BASE + i*4);
+} /* fir_table_setup */
+
+
+static const u32
+gain_table[] = { 0x8, 0x8, 0xf, 0x13, 0x17, 0x1b, 0x1f, 0x23, 0x27, 0x2b,
+		 0x2f, 0x33, 0x37, 0x3b, 0x3f, 0x43, 0x47, 0x4b, 0x4f,
+		 0x53, 0x57, 0x5b, 0x5f, 0x5f, 0x5f, 0x5f, 0x5f, 0x5f,
+		 0x5f, 0x5f, 0x5f, 0x5f };
+
+void gain_table_init(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(gain_table); i++) {
+		iowrite32(gain_table[i], ctl + AGNX_GAIN_TABLE + i*4);
+		iowrite32(gain_table[i], ctl + AGNX_GAIN_TABLE + i*4 + 0x80);
+	}
+} /* gain_table_init */
+
+void monitor_gain_table_init(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	unsigned int i;
+
+	for (i = 0; i < 0x44; i += 4) {
+		iowrite32(0x61, ctl + AGNX_MONGCR_BASE + i);
+		iowrite32(0x61, ctl + AGNX_MONGCR_BASE + 0x200 + i);
+	}
+	for (i = 0x44; i < 0x64; i += 4) {
+		iowrite32(0x6e, ctl + AGNX_MONGCR_BASE + i);
+		iowrite32(0x6e, ctl + AGNX_MONGCR_BASE + 0x200 + i);
+	}
+	for (i = 0x64; i < 0x94; i += 4) {
+		iowrite32(0x7a, ctl + AGNX_MONGCR_BASE + i);
+		iowrite32(0x7a, ctl + AGNX_MONGCR_BASE + 0x200 + i);
+	}
+	for (i = 0x94; i < 0xdc; i += 4) {
+		iowrite32(0x87, ctl + AGNX_MONGCR_BASE + i);
+		iowrite32(0x87, ctl + AGNX_MONGCR_BASE + 0x200 + i);
+	}
+	for (i = 0xdc; i < 0x148; i += 4) {
+		iowrite32(0x95, ctl + AGNX_MONGCR_BASE + i);
+		iowrite32(0x95, ctl + AGNX_MONGCR_BASE + 0x200 + i);
+	}
+	for (i = 0x148; i < 0x1e8; i += 4) {
+		iowrite32(0xa2, ctl + AGNX_MONGCR_BASE + i);
+		iowrite32(0xa2, ctl + AGNX_MONGCR_BASE + 0x200 + i);
+	}
+	for (i = 0x1e8; i <= 0x1fc; i += 4) {
+		iowrite32(0xb0, ctl + AGNX_MONGCR_BASE + i);
+		iowrite32(0xb0, ctl + AGNX_MONGCR_BASE + 0x200 + i);
+	}
+} /* monitor_gain_table_init */
+
+
+void routing_table_init(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	unsigned int type, subtype;
+	u32 reg;
+
+	disable_receiver(priv);
+
+	for ( type = 0; type < 0x3; type++ ) {
+		for (subtype = 0; subtype < 0x10; subtype++) {
+			/* 1. Set Routing table to R/W and to Return status on Read */
+			reg = (type << ROUTAB_TYPE_SHIFT) |
+				(subtype << ROUTAB_SUBTYPE_SHIFT);
+			reg |= (1 << ROUTAB_RW_SHIFT) | (1 << ROUTAB_STATUS_SHIFT);
+			if (type == ROUTAB_TYPE_DATA) {
+				/* NULL goes to RFP */
+				if (subtype == ROUTAB_SUBTYPE_NULL)
+//					reg |= ROUTAB_ROUTE_RFP;
+					reg |= ROUTAB_ROUTE_CPU;
+				/* QOS NULL goes to CPU */
+				else if (subtype == ROUTAB_SUBTYPE_QOSNULL)
+					reg |= ROUTAB_ROUTE_CPU;
+				/* All Data and QOS data subtypes go to Encryption */
+				else if ((subtype == ROUTAB_SUBTYPE_DATA) ||
+					 (subtype == ROUTAB_SUBTYPE_DATAACK) ||
+					 (subtype == ROUTAB_SUBTYPE_DATAPOLL) ||
+					 (subtype == ROUTAB_SUBTYPE_DATAPOLLACK) ||
+					 (subtype == ROUTAB_SUBTYPE_QOSDATA) ||
+					 (subtype == ROUTAB_SUBTYPE_QOSDATAACK) ||
+					 (subtype == ROUTAB_SUBTYPE_QOSDATAPOLL) ||
+					 (subtype == ROUTAB_SUBTYPE_QOSDATAACKPOLL))
+					reg |= ROUTAB_ROUTE_ENCRY;
+//					reg |= ROUTAB_ROUTE_CPU;
+				/*Drop NULL and QOS NULL ack, poll and poll ack*/
+				else if ((subtype == ROUTAB_SUBTYPE_NULLACK) ||
+					 (subtype == ROUTAB_SUBTYPE_QOSNULLACK) ||
+					 (subtype == ROUTAB_SUBTYPE_NULLPOLL) ||
+					 (subtype == ROUTAB_SUBTYPE_QOSNULLPOLL) ||
+					 (subtype == ROUTAB_SUBTYPE_NULLPOLLACK) ||
+					 (subtype == ROUTAB_SUBTYPE_QOSNULLPOLLACK))
+//					reg |= ROUTAB_ROUTE_DROP;
+					reg |= ROUTAB_ROUTE_CPU;
+			}
+			else
+				reg |= (ROUTAB_ROUTE_CPU);
+			iowrite32(reg, ctl + AGNX_RXM_ROUTAB);
+			/* Check to verify that the status bit cleared */
+			routing_table_delay();
+		}
+	}
+	enable_receiver(priv);
+} /* routing_table_init */
+
+void tx_engine_lookup_tbl_init(struct agnx_priv *priv)
+{
+	void __iomem *data = priv->data;
+	unsigned int i;
+
+	for (i = 0; i <= 28; i += 4)
+		iowrite32(0xb00c, data + AGNX_ENGINE_LOOKUP_TBL + i);
+	for (i = 32; i <= 120; i += 8) {
+		iowrite32(0x1e58, data + AGNX_ENGINE_LOOKUP_TBL + i);
+		iowrite32(0xb00c, data + AGNX_ENGINE_LOOKUP_TBL + i + 4);
+	}
+
+	for (i = 128; i <= 156; i += 4)
+		iowrite32(0x980c, data + AGNX_ENGINE_LOOKUP_TBL + i);
+	for (i = 160; i <= 248; i += 8) {
+		iowrite32(0x1858, data + AGNX_ENGINE_LOOKUP_TBL + i);
+		iowrite32(0x980c, data + AGNX_ENGINE_LOOKUP_TBL + i + 4);
+	}
+
+	for (i = 256; i <= 284; i += 4)
+		iowrite32(0x980c, data + AGNX_ENGINE_LOOKUP_TBL + i);
+	for (i = 288; i <= 376; i += 8) {
+		iowrite32(0x1a58, data + AGNX_ENGINE_LOOKUP_TBL + i);
+		iowrite32(0x1858, data + AGNX_ENGINE_LOOKUP_TBL + i + 4);
+	}
+
+	for (i = 512; i <= 540; i += 4)
+		iowrite32(0xc00c, data + AGNX_ENGINE_LOOKUP_TBL + i);
+	for (i = 544; i <= 632; i += 8) {
+		iowrite32(0x2058, data + AGNX_ENGINE_LOOKUP_TBL + i);
+		iowrite32(0xc00c, data + AGNX_ENGINE_LOOKUP_TBL + i + 4);
+	}
+
+	for (i = 640; i <= 668; i += 4)
+		iowrite32(0xc80c, data + AGNX_ENGINE_LOOKUP_TBL + i);
+	for (i = 672; i <= 764; i += 8) {
+		iowrite32(0x2258, data + AGNX_ENGINE_LOOKUP_TBL + i);
+		iowrite32(0xc80c, data + AGNX_ENGINE_LOOKUP_TBL + i + 4);
+	}
+}
+
diff --git a/drivers/staging/agnx/table.h b/drivers/staging/agnx/table.h
new file mode 100644
index 00000000000..f0626b5ee86
--- /dev/null
+++ b/drivers/staging/agnx/table.h
@@ -0,0 +1,10 @@
+#ifndef AGNX_TABLE_H_
+#define AGNX_TABLE_H_
+
+void tx_fir_table_init(struct agnx_priv *priv);
+void gain_table_init(struct agnx_priv *priv);
+void monitor_gain_table_init(struct agnx_priv *priv);
+void routing_table_init(struct agnx_priv *priv);
+void tx_engine_lookup_tbl_init(struct agnx_priv *priv);
+
+#endif /* AGNX_TABLE_H_ */
diff --git a/drivers/staging/agnx/xmit.c b/drivers/staging/agnx/xmit.c
new file mode 100644
index 00000000000..7f01528b8a4
--- /dev/null
+++ b/drivers/staging/agnx/xmit.c
@@ -0,0 +1,819 @@
+/**
+ * Airgo MIMO wireless driver
+ *
+ * Copyright (c) 2007 Li YanBo <dreamfly281@gmail.com>
+
+ * Thanks for Jeff Williams <angelbane@gmail.com> do reverse engineer
+ * works and published the SPECS at http://airgo.wdwconsulting.net/mymoin
+
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/pci.h>
+#include <linux/delay.h>
+#include "agnx.h"
+#include "debug.h"
+#include "phy.h"
+
+unsigned int rx_frame_cnt = 0;
+//unsigned int local_tx_sent_cnt = 0;
+
+static inline void disable_rx_engine(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	iowrite32(0x100, ctl + AGNX_CIR_RXCTL);
+	/* Wait for RX Control to have the Disable Rx Interrupt (0x100) set */
+	ioread32(ctl + AGNX_CIR_RXCTL);
+}
+
+static inline void enable_rx_engine(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	iowrite32(0x80, ctl + AGNX_CIR_RXCTL);
+	ioread32(ctl + AGNX_CIR_RXCTL);
+}
+
+inline void disable_rx_interrupt(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+
+	disable_rx_engine(priv);
+	reg = ioread32(ctl + AGNX_CIR_RXCFG);
+	reg &= ~0x20;
+	iowrite32(reg, ctl + AGNX_CIR_RXCFG);
+	ioread32(ctl + AGNX_CIR_RXCFG);
+}
+
+inline void enable_rx_interrupt(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+
+	reg = ioread32(ctl + AGNX_CIR_RXCFG);
+	reg |= 0x20;
+	iowrite32(reg, ctl + AGNX_CIR_RXCFG);
+	ioread32(ctl + AGNX_CIR_RXCFG);
+	enable_rx_engine(priv);
+}
+
+static inline void rx_desc_init(struct agnx_priv *priv, unsigned int idx)
+{
+	struct agnx_desc *desc = priv->rx.desc + idx;
+	struct agnx_info *info = priv->rx.info + idx;
+
+	memset(info, 0, sizeof(*info));
+
+	info->dma_len = IEEE80211_MAX_RTS_THRESHOLD + sizeof(struct agnx_hdr);
+	info->skb = dev_alloc_skb(info->dma_len);
+	if (info->skb == NULL)
+		agnx_bug("refill err");
+
+	info->mapping = pci_map_single(priv->pdev, skb_tail_pointer(info->skb),
+				       info->dma_len, PCI_DMA_FROMDEVICE);
+	memset(desc, 0, sizeof(*desc));
+	desc->dma_addr = cpu_to_be32(info->mapping);
+	/* Set the owner to the card */
+	desc->frag = cpu_to_be32(be32_to_cpu(desc->frag) | OWNER);
+}
+
+static inline void rx_desc_reinit(struct agnx_priv *priv, unsigned int idx)
+{
+	struct agnx_info *info = priv->rx.info + idx;
+
+	/* Cause ieee80211 will free the skb buffer, so we needn't to free it again?! */
+	pci_unmap_single(priv->pdev, info->mapping, info->dma_len, PCI_DMA_FROMDEVICE);
+	rx_desc_init(priv, idx);
+}
+
+static inline void rx_desc_reusing(struct agnx_priv *priv, unsigned int idx)
+{
+	struct agnx_desc *desc = priv->rx.desc + idx;
+	struct agnx_info *info = priv->rx.info + idx;
+
+	memset(desc, 0, sizeof(*desc));
+	desc->dma_addr = cpu_to_be32(info->mapping);
+	/* Set the owner to the card */
+	desc->frag = cpu_to_be32(be32_to_cpu(desc->frag) | OWNER);
+}
+
+static void rx_desc_free(struct agnx_priv *priv, unsigned int idx)
+{
+	struct agnx_desc *desc = priv->rx.desc + idx;
+	struct agnx_info *info = priv->rx.info + idx;
+
+	BUG_ON(!desc || !info);
+	if (info->mapping)
+		pci_unmap_single(priv->pdev, info->mapping, info->dma_len, PCI_DMA_FROMDEVICE);
+	if (info->skb)
+		dev_kfree_skb(info->skb);
+	memset(info, 0, sizeof(*info));
+	memset(desc, 0, sizeof(*desc));
+}
+
+static inline void __tx_desc_free(struct agnx_priv *priv,
+				  struct agnx_desc *desc, struct agnx_info *info)
+{
+	BUG_ON(!desc || !info);
+	/* TODO make sure mapping, skb and len are consistency */
+	if (info->mapping)
+		pci_unmap_single(priv->pdev, info->mapping,
+				 info->dma_len, PCI_DMA_TODEVICE);
+	if (info->type == PACKET)
+		dev_kfree_skb(info->skb);
+
+	memset(info, 0, sizeof(*info));
+	memset(desc, 0, sizeof(*desc));
+}
+
+static void txm_desc_free(struct agnx_priv *priv, unsigned int idx)
+{
+	struct agnx_desc *desc = priv->txm.desc + idx;
+	struct agnx_info *info = priv->txm.info + idx;
+
+	__tx_desc_free(priv, desc, info);
+}
+
+static void txd_desc_free(struct agnx_priv *priv, unsigned int idx)
+{
+	struct agnx_desc *desc = priv->txd.desc + idx;
+	struct agnx_info *info = priv->txd.info + idx;
+
+	__tx_desc_free(priv, desc, info);
+}
+
+int fill_rings(struct agnx_priv *priv)
+{
+	void __iomem *ctl = priv->ctl;
+	unsigned int i;
+	u32 reg;
+	AGNX_TRACE;
+
+	priv->txd.idx_sent = priv->txm.idx_sent = 0;
+	priv->rx.idx = priv->txm.idx = priv->txd.idx = 0;
+
+	for (i = 0; i < priv->rx.size; i++)
+		rx_desc_init(priv, i);
+	for (i = 0; i < priv->txm.size; i++) {
+		memset(priv->txm.desc + i, 0, sizeof(struct agnx_desc));
+		memset(priv->txm.info + i, 0, sizeof(struct agnx_info));
+	}
+	for (i = 0; i < priv->txd.size; i++) {
+		memset(priv->txd.desc + i, 0, sizeof(struct agnx_desc));
+		memset(priv->txd.info + i, 0, sizeof(struct agnx_info));
+	}
+
+	/* FIXME Set the card RX TXM and TXD address */
+	agnx_write32(ctl, AGNX_CIR_RXCMSTART, priv->rx.dma);
+	agnx_write32(ctl, AGNX_CIR_RXCMEND, priv->txm.dma);
+
+	agnx_write32(ctl, AGNX_CIR_TXMSTART, priv->txm.dma);
+	agnx_write32(ctl, AGNX_CIR_TXMEND, priv->txd.dma);
+
+	agnx_write32(ctl, AGNX_CIR_TXDSTART, priv->txd.dma);
+	agnx_write32(ctl, AGNX_CIR_TXDEND, priv->txd.dma +
+		     sizeof(struct agnx_desc) * priv->txd.size);
+
+	/* FIXME Relinquish control of rings to card */
+	reg = agnx_read32(ctl, AGNX_CIR_BLKCTL);
+	reg &= ~0x800;
+	agnx_write32(ctl, AGNX_CIR_BLKCTL, reg);
+	return 0;
+} /* fill_rings */
+
+void unfill_rings(struct agnx_priv *priv)
+{
+	unsigned long flags;
+	unsigned int i;
+	AGNX_TRACE;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	for (i = 0; i < priv->rx.size; i++)
+		rx_desc_free(priv, i);
+	for (i = 0; i < priv->txm.size; i++)
+		txm_desc_free(priv, i);
+	for (i = 0; i < priv->txd.size; i++)
+		txd_desc_free(priv, i);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+}
+
+/* Extract the bitrate out of a CCK PLCP header.
+   copy from bcm43xx driver */
+static inline u8 agnx_plcp_get_bitrate_cck(__be32 *phyhdr_11b)
+{
+	/* FIXME */
+	switch (*(u8 *)phyhdr_11b) {
+	case 0x0A:
+		return 0;
+	case 0x14:
+		return 1;
+	case 0x37:
+		return 2;
+	case 0x6E:
+		return 3;
+	}
+	agnx_bug("Wrong plcp rate");
+	return 0;
+}
+
+/* FIXME */
+static inline u8 agnx_plcp_get_bitrate_ofdm(__be32 *phyhdr_11g)
+{
+	u8 rate = *(u8 *)phyhdr_11g & 0xF;
+
+	printk(PFX "G mode rate is 0x%x\n", rate);
+	return rate;
+}
+
+/* FIXME */
+static void get_rx_stats(struct agnx_priv *priv, struct agnx_hdr *hdr,
+			 struct ieee80211_rx_status *stat)
+{
+	void __iomem *ctl = priv->ctl;
+	u8 *rssi;
+	u32 noise;
+	/* FIXME just for test */
+	int snr = 40;		/* signal-to-noise ratio */
+
+	memset(stat, 0, sizeof(*stat));
+	/* RSSI */
+	rssi = (u8 *)&hdr->phy_stats_lo;
+//	stat->ssi = (rssi[0] + rssi[1] + rssi[2]) / 3;
+	/* Noise */
+	noise = ioread32(ctl + AGNX_GCR_NOISE0);
+	noise += ioread32(ctl + AGNX_GCR_NOISE1);
+	noise += ioread32(ctl + AGNX_GCR_NOISE2);
+	stat->noise = noise / 3;
+	/* Signal quality */
+	//snr = stat->ssi - stat->noise;
+	if (snr >=0 && snr < 40)
+		stat->signal = 5 * snr / 2;
+	else if (snr >= 40)
+		stat->signal = 100;
+	else
+		stat->signal = 0;
+
+
+	if (hdr->_11b0 && !hdr->_11g0) {
+		stat->rate_idx = agnx_plcp_get_bitrate_cck(&hdr->_11b0);
+	} else if (!hdr->_11b0 && hdr->_11g0) {
+		printk(PFX "RX: Found G mode packet\n");
+		stat->rate_idx = agnx_plcp_get_bitrate_ofdm(&hdr->_11g0);
+	} else
+		agnx_bug("Unknown packets type");
+
+
+	stat->band = IEEE80211_BAND_2GHZ;
+	stat->freq = agnx_channels[priv->channel - 1].center_freq;
+//	stat->antenna = 3;
+//	stat->mactime = be32_to_cpu(hdr->time_stamp);
+//	stat->channel = priv->channel;
+
+}
+
+static inline void combine_hdr_frag(struct ieee80211_hdr *ieeehdr,
+				    struct sk_buff *skb)
+{
+	u16 fctl;
+	unsigned int hdrlen;
+
+	fctl = le16_to_cpu(ieeehdr->frame_control);
+	hdrlen = ieee80211_hdrlen(fctl);
+	/* FIXME */
+	if (hdrlen < (2+2+6)/*minimum hdr*/ ||
+	    hdrlen > sizeof(struct ieee80211_mgmt)) {
+		printk(KERN_ERR PFX "hdr len is %d\n", hdrlen);
+		agnx_bug("Wrong ieee80211 hdr detected");
+	}
+	skb_push(skb, hdrlen);
+	memcpy(skb->data, ieeehdr, hdrlen);
+} /* combine_hdr_frag */
+
+static inline int agnx_packet_check(struct agnx_priv *priv, struct agnx_hdr *agnxhdr,
+				    unsigned packet_len)
+{
+	if (agnx_get_bits(CRC_FAIL, CRC_FAIL_SHIFT, be32_to_cpu(agnxhdr->reg1)) == 1){
+		printk(PFX "RX: CRC check fail\n");
+		goto drop;
+	}
+	if (packet_len > 2048) {
+		printk(PFX "RX: Too long packet detected\n");
+		goto drop;
+	}
+
+	/* FIXME Just usable for Promious Mode, for Manage mode exclude FCS */
+/* 	if (packet_len - sizeof(*agnxhdr) < FCS_LEN) { */
+/* 		printk(PFX "RX: Too short packet detected\n"); */
+/* 		goto drop; */
+/* 	} */
+	return 0;
+drop:
+	priv->stats.dot11FCSErrorCount++;
+	return -1;
+}
+
+void handle_rx_irq(struct agnx_priv *priv)
+{
+	struct ieee80211_rx_status status;
+	unsigned int len;
+//	AGNX_TRACE;
+
+	do {
+		struct agnx_desc *desc;
+		u32 frag;
+		struct agnx_info *info;
+		struct agnx_hdr *hdr;
+		struct sk_buff *skb;
+		unsigned int i = priv->rx.idx % priv->rx.size;
+
+		desc = priv->rx.desc + i;
+		frag = be32_to_cpu(desc->frag);
+		if (frag & OWNER)
+			break;
+
+		info = priv->rx.info + i;
+		skb = info->skb;
+		hdr = (struct agnx_hdr *)(skb->data);
+
+		len = (frag & PACKET_LEN) >> PACKET_LEN_SHIFT;
+		if (agnx_packet_check(priv, hdr, len) == -1) {
+ 			rx_desc_reusing(priv, i);
+			continue;
+		}
+		skb_put(skb, len);
+
+		do {
+			u16 fctl;
+			fctl = le16_to_cpu(((struct ieee80211_hdr *)hdr->mac_hdr)->frame_control);
+			if ((fctl & IEEE80211_FCTL_STYPE) != IEEE80211_STYPE_BEACON)// && !(fctl & IEEE80211_STYPE_BEACON))
+				dump_ieee80211_hdr((struct ieee80211_hdr *)hdr->mac_hdr, "RX");
+		} while (0);
+
+		if (hdr->_11b0 && !hdr->_11g0) {
+/* 			int j; */
+/* 			u16 fctl = le16_to_cpu(((struct ieee80211_hdr *)hdr->mac_hdr) */
+/* 					       ->frame_control); */
+/* 			if ( (fctl & IEEE80211_FCTL_FTYPE) ==  IEEE80211_FTYPE_DATA) { */
+/* 				agnx_print_rx_hdr(hdr); */
+// 				agnx_print_sta(priv, BSSID_STAID);
+/* 				for (j = 0; j < 8; j++) */
+/* 					agnx_print_sta_tx_wq(priv, BSSID_STAID, j);		 */
+/* 			} */
+
+			get_rx_stats(priv, hdr, &status);
+			skb_pull(skb, sizeof(*hdr));
+			combine_hdr_frag((struct ieee80211_hdr *)hdr->mac_hdr, skb);
+		} else if (!hdr->_11b0 && hdr->_11g0) {
+//			int j;
+			agnx_print_rx_hdr(hdr);
+			agnx_print_sta(priv, BSSID_STAID);
+//			for (j = 0; j < 8; j++)
+			agnx_print_sta_tx_wq(priv, BSSID_STAID, 0);
+
+			print_hex_dump_bytes("agnx: RX_PACKET: ", DUMP_PREFIX_NONE,
+					     skb->data, skb->len + 8);
+
+//			if (agnx_plcp_get_bitrate_ofdm(&hdr->_11g0) == 0)
+			get_rx_stats(priv, hdr, &status);
+			skb_pull(skb, sizeof(*hdr));
+			combine_hdr_frag((struct ieee80211_hdr *)
+					 ((void *)&hdr->mac_hdr), skb);
+//			dump_ieee80211_hdr((struct ieee80211_hdr *)skb->data, "RX G");
+		} else
+			agnx_bug("Unknown packets type");
+		ieee80211_rx_irqsafe(priv->hw, skb, &status);
+		rx_desc_reinit(priv, i);
+
+	} while ( priv->rx.idx++ );
+} /* handle_rx_irq */
+
+static inline void handle_tx_irq(struct agnx_priv *priv, struct agnx_ring *ring)
+{
+	struct agnx_desc *desc;
+	struct agnx_info *info;
+	unsigned int idx;
+
+	for (idx = ring->idx_sent; idx < ring->idx; idx++) {
+		unsigned int i = idx % ring->size;
+		u32  frag;
+
+		desc = ring->desc + i;
+		info = ring->info + i;
+
+		frag = be32_to_cpu(desc->frag);
+		if (frag & OWNER) {
+			if (info->type == HEADER)
+				break;
+			else
+				agnx_bug("TX error");
+		}
+
+		pci_unmap_single(priv->pdev, info->mapping, info->dma_len, PCI_DMA_TODEVICE);
+
+		do {
+//			int j;
+			size_t len;
+			len = info->skb->len - sizeof(struct agnx_hdr) + info->hdr_len;
+			//	if (len == 614) {
+//				agnx_print_desc(desc);
+				if (info->type == PACKET) {
+//					agnx_print_tx_hdr((struct agnx_hdr *)info->skb->data);
+/* 					agnx_print_sta_power(priv, LOCAL_STAID); */
+/* 					agnx_print_sta(priv, LOCAL_STAID); */
+/* //					for (j = 0; j < 8; j++) */
+/* 					agnx_print_sta_tx_wq(priv, LOCAL_STAID, 0); */
+//					agnx_print_sta_power(priv, BSSID_STAID);
+//					agnx_print_sta(priv, BSSID_STAID);
+//					for (j = 0; j < 8; j++)
+//					agnx_print_sta_tx_wq(priv, BSSID_STAID, 0);
+				}
+//			}
+		} while (0);
+
+		if (info->type == PACKET) {
+//			dump_txm_registers(priv);
+//			dump_rxm_registers(priv);
+//			dump_bm_registers(priv);
+//			dump_cir_registers(priv);
+		}
+
+		if (info->type == PACKET) {
+//			struct ieee80211_hdr *hdr;
+			struct ieee80211_tx_info *txi = IEEE80211_SKB_CB(info->skb);
+
+			skb_pull(info->skb, sizeof(struct agnx_hdr));
+			memcpy(skb_push(info->skb, info->hdr_len), &info->hdr, info->hdr_len);
+
+//			dump_ieee80211_hdr((struct ieee80211_hdr *)info->skb->data, "TX_HANDLE");
+/* 			print_hex_dump_bytes("agnx: TX_HANDLE: ", DUMP_PREFIX_NONE, */
+/* 					     info->skb->data, info->skb->len); */
+
+			if (!(txi->flags & IEEE80211_TX_CTL_NO_ACK))
+				txi->flags |= IEEE80211_TX_STAT_ACK;
+
+			ieee80211_tx_status_irqsafe(priv->hw, info->skb);
+
+
+/* 				info->tx_status.queue_number = (ring->size - i) / 2; */
+/* 				ieee80211_tx_status_irqsafe(priv->hw, info->skb, &(info->tx_status)); */
+/* 			} else */
+/* 				dev_kfree_skb_irq(info->skb); */
+ 		}
+		memset(desc, 0, sizeof(*desc));
+		memset(info, 0, sizeof(*info));
+	}
+
+	ring->idx_sent = idx;
+	/* TODO fill the priv->low_level_stats */
+
+	/* ieee80211_wake_queue(priv->hw, 0); */
+}
+
+void handle_txm_irq(struct agnx_priv *priv)
+{
+	handle_tx_irq(priv, &priv->txm);
+}
+
+void handle_txd_irq(struct agnx_priv *priv)
+{
+	handle_tx_irq(priv, &priv->txd);
+}
+
+void handle_other_irq(struct agnx_priv *priv)
+{
+//	void __iomem *ctl = priv->ctl;
+	u32 status = priv->irq_status;
+	void __iomem *ctl = priv->ctl;
+	u32 reg;
+
+	if (status & IRQ_TX_BEACON) {
+		iowrite32(IRQ_TX_BEACON, ctl + AGNX_INT_STAT);
+		printk(PFX "IRQ: TX Beacon control is 0X%.8X\n", ioread32(ctl + AGNX_TXM_BEACON_CTL));
+		printk(PFX "IRQ: TX Beacon rx frame num: %d\n", rx_frame_cnt);
+	}
+	if (status & IRQ_TX_RETRY) {
+		reg = ioread32(ctl + AGNX_TXM_RETRYSTAID);
+		printk(PFX "IRQ: TX Retry, RETRY STA ID is %x\n", reg);
+	}
+	if (status & IRQ_TX_ACTIVITY)
+		printk(PFX "IRQ: TX Activity\n");
+	if (status & IRQ_RX_ACTIVITY)
+		printk(PFX "IRQ: RX Activity\n");
+	if (status & IRQ_RX_X)
+		printk(PFX "IRQ: RX X\n");
+	if (status & IRQ_RX_Y) {
+		reg = ioread32(ctl + AGNX_INT_MASK);
+		reg &= ~IRQ_RX_Y;
+		iowrite32(reg, ctl + AGNX_INT_MASK);
+		iowrite32(IRQ_RX_Y, ctl + AGNX_INT_STAT);
+		printk(PFX "IRQ: RX Y\n");
+	}
+	if (status & IRQ_RX_HASHHIT)  {
+		reg = ioread32(ctl + AGNX_INT_MASK);
+		reg &= ~IRQ_RX_HASHHIT;
+		iowrite32(reg, ctl + AGNX_INT_MASK);
+		iowrite32(IRQ_RX_HASHHIT, ctl + AGNX_INT_STAT);
+		printk(PFX "IRQ: RX Hash Hit\n");
+
+	}
+	if (status & IRQ_RX_FRAME) {
+		reg = ioread32(ctl + AGNX_INT_MASK);
+		reg &= ~IRQ_RX_FRAME;
+		iowrite32(reg, ctl + AGNX_INT_MASK);
+		iowrite32(IRQ_RX_FRAME, ctl + AGNX_INT_STAT);
+		printk(PFX "IRQ: RX Frame\n");
+ 		rx_frame_cnt++;
+	}
+	if (status & IRQ_ERR_INT) {
+		iowrite32(IRQ_ERR_INT, ctl + AGNX_INT_STAT);
+//		agnx_hw_reset(priv);
+		printk(PFX "IRQ: Error Interrupt\n");
+	}
+	if (status & IRQ_TX_QUE_FULL)
+		printk(PFX "IRQ: TX Workqueue Full\n");
+	if (status & IRQ_BANDMAN_ERR)
+		printk(PFX "IRQ: Bandwidth Management Error\n");
+	if (status & IRQ_TX_DISABLE)
+		printk(PFX "IRQ: TX Disable\n");
+	if (status & IRQ_RX_IVASESKEY)
+		printk(PFX "IRQ: RX Invalid Session Key\n");
+	if (status & IRQ_REP_THHIT)
+		printk(PFX "IRQ: Replay Threshold Hit\n");
+	if (status & IRQ_TIMER1)
+		printk(PFX "IRQ: Timer1\n");
+	if (status & IRQ_TIMER_CNT)
+		printk(PFX "IRQ: Timer Count\n");
+	if (status & IRQ_PHY_FASTINT)
+		printk(PFX "IRQ: Phy Fast Interrupt\n");
+	if (status & IRQ_PHY_SLOWINT)
+		printk(PFX "IRQ: Phy Slow Interrupt\n");
+	if (status & IRQ_OTHER)
+		printk(PFX "IRQ: 0x80000000\n");
+} /* handle_other_irq */
+
+
+static inline void route_flag_set(struct agnx_hdr *txhdr)
+{
+//	u32 reg = 0;
+
+	/* FIXME */
+/*  	reg = (0x7 << ROUTE_COMPRESSION_SHIFT) & ROUTE_COMPRESSION; */
+/* 	txhdr->reg5 = cpu_to_be32(reg); */
+ 	txhdr->reg5 = (0xa << 0x0) | (0x7 << 0x18);
+// 	txhdr->reg5 = cpu_to_be32((0xa << 0x0) | (0x7 << 0x18));
+// 	txhdr->reg5 = cpu_to_be32(0x7 << 0x0);
+}
+
+/* Return 0 if no match */
+static inline unsigned int get_power_level(unsigned int rate, unsigned int antennas_num)
+{
+	unsigned int power_level;
+
+	switch (rate) {
+	case 10:
+	case 20:
+	case 55:
+	case 60:
+	case 90:
+	case 120: power_level = 22; break;
+	case 180: power_level = 19; break;
+	case 240: power_level = 18; break;
+	case 360: power_level = 16; break;
+	case 480: power_level = 15; break;
+	case 540: power_level = 14; break;
+	default:
+		agnx_bug("Error rate setting\n");
+	}
+
+	if (power_level && (antennas_num == 2))
+		power_level -= 3;
+
+	return power_level;
+}
+
+static inline void fill_agnx_hdr(struct agnx_priv *priv, struct agnx_info *tx_info)
+{
+	struct agnx_hdr *txhdr = (struct agnx_hdr *)tx_info->skb->data;
+	size_t len;
+	u16 fc = le16_to_cpu(*(__le16 *)&tx_info->hdr);
+	u32 reg;
+
+	memset(txhdr, 0, sizeof(*txhdr));
+
+//	reg = agnx_set_bits(STATION_ID, STATION_ID_SHIFT, LOCAL_STAID);
+	reg = agnx_set_bits(STATION_ID, STATION_ID_SHIFT, BSSID_STAID);
+	reg |= agnx_set_bits(WORKQUEUE_ID, WORKQUEUE_ID_SHIFT, 0);
+	txhdr->reg4 = cpu_to_be32(reg);
+
+	/* Set the Hardware Sequence Number to 1? */
+	reg = agnx_set_bits(SEQUENCE_NUMBER, SEQUENCE_NUMBER_SHIFT, 0);
+//	reg = agnx_set_bits(SEQUENCE_NUMBER, SEQUENCE_NUMBER_SHIFT, 1);
+	reg |= agnx_set_bits(MAC_HDR_LEN, MAC_HDR_LEN_SHIFT, tx_info->hdr_len);
+	txhdr->reg1 = cpu_to_be32(reg);
+	/* Set the agnx_hdr's MAC header */
+	memcpy(txhdr->mac_hdr, &tx_info->hdr, tx_info->hdr_len);
+
+	reg = agnx_set_bits(ACK, ACK_SHIFT, 1);
+//	reg = agnx_set_bits(ACK, ACK_SHIFT, 0);
+	reg |= agnx_set_bits(MULTICAST, MULTICAST_SHIFT, 0);
+//	reg |= agnx_set_bits(MULTICAST, MULTICAST_SHIFT, 1);
+	reg |= agnx_set_bits(RELAY, RELAY_SHIFT, 0);
+	reg |= agnx_set_bits(TM, TM_SHIFT, 0);
+	txhdr->reg0 = cpu_to_be32(reg);
+
+	/* Set the long and short retry limits */
+ 	txhdr->tx.short_retry_limit = tx_info->txi->control.rates[0].count;
+ 	txhdr->tx.long_retry_limit = tx_info->txi->control.rates[0].count;
+
+	/* FIXME */
+	len = tx_info->skb->len - sizeof(*txhdr) + tx_info->hdr_len + FCS_LEN;
+	if (fc & IEEE80211_FCTL_PROTECTED)
+		len += 8;
+	len = 2398;
+	reg = agnx_set_bits(FRAG_SIZE, FRAG_SIZE_SHIFT, len);
+	len = tx_info->skb->len - sizeof(*txhdr);
+	reg |= agnx_set_bits(PAYLOAD_LEN, PAYLOAD_LEN_SHIFT, len);
+	txhdr->reg3 = cpu_to_be32(reg);
+
+	route_flag_set(txhdr);
+} /* fill_hdr */
+
+static void txm_power_set(struct agnx_priv *priv,
+			  struct ieee80211_tx_info *txi)
+{
+	struct agnx_sta_power power;
+	u32 reg;
+
+	/* FIXME */
+	if (txi->control.rates[0].idx < 0) {
+		/* For B mode Short Preamble */
+		reg = agnx_set_bits(PHY_MODE, PHY_MODE_SHIFT, AGNX_MODE_80211B_SHORT);
+//		control->tx_rate = -control->tx_rate;
+	} else
+		reg = agnx_set_bits(PHY_MODE, PHY_MODE_SHIFT, AGNX_MODE_80211G);
+//		reg = agnx_set_bits(PHY_MODE, PHY_MODE_SHIFT, AGNX_MODE_80211B_LONG);
+	reg |= agnx_set_bits(SIGNAL, SIGNAL_SHIFT, 0xB);
+	reg |= agnx_set_bits(RATE, RATE_SHIFT, 0xB);
+//	reg |= agnx_set_bits(POWER_LEVEL, POWER_LEVEL_SHIFT, 15);
+	reg |= agnx_set_bits(POWER_LEVEL, POWER_LEVEL_SHIFT, 20);
+	/* if rate < 11M set it to 0 */
+	reg |= agnx_set_bits(NUM_TRANSMITTERS, NUM_TRANSMITTERS_SHIFT, 1);
+//	reg |= agnx_set_bits(EDCF, EDCF_SHIFT, 1);
+//	reg |= agnx_set_bits(TIFS, TIFS_SHIFT, 1);
+
+	power.reg = reg;
+//	power.reg = cpu_to_le32(reg);
+
+//	set_sta_power(priv, &power, LOCAL_STAID);
+	set_sta_power(priv, &power, BSSID_STAID);
+}
+
+static inline int tx_packet_check(struct sk_buff *skb)
+{
+	unsigned int ieee_len = ieee80211_get_hdrlen_from_skb(skb);
+	if (skb->len > 2048) {
+		printk(KERN_ERR PFX "length is %d\n", skb->len);
+		agnx_bug("Too long TX skb");
+		return -1;
+	}
+	/* FIXME */
+	if (skb->len == ieee_len) {
+		printk(PFX "A strange TX packet\n");
+		return -1;
+		/* tx_faile_irqsafe(); */
+	}
+	return 0;
+}
+
+static int __agnx_tx(struct agnx_priv *priv, struct sk_buff *skb,
+		     struct agnx_ring *ring)
+{
+	struct agnx_desc *hdr_desc, *frag_desc;
+	struct agnx_info *hdr_info, *frag_info;
+	struct ieee80211_tx_info *txi = IEEE80211_SKB_CB(skb);
+	unsigned long flags;
+	unsigned int i;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	/* The RX interrupt need be Disable until this TX packet
+	   is handled in the next tx interrupt */
+	disable_rx_interrupt(priv);
+
+	i = ring->idx;
+	ring->idx += 2;
+/*   	if (priv->txm_idx - priv->txm_idx_sent == AGNX_TXM_RING_SIZE - 2) */
+/* 		ieee80211_stop_queue(priv->hw, 0); */
+
+	/* Set agnx header's info and desc */
+	i %= ring->size;
+	hdr_desc = ring->desc + i;
+	hdr_info = ring->info + i;
+	hdr_info->hdr_len = ieee80211_get_hdrlen_from_skb(skb);
+	memcpy(&hdr_info->hdr, skb->data, hdr_info->hdr_len);
+
+	/* Add the agnx header to the front of the SKB */
+	skb_push(skb, sizeof(struct agnx_hdr) - hdr_info->hdr_len);
+
+	hdr_info->txi = txi;
+	hdr_info->dma_len = sizeof(struct agnx_hdr);
+	hdr_info->skb = skb;
+	hdr_info->type = HEADER;
+	fill_agnx_hdr(priv, hdr_info);
+	hdr_info->mapping = pci_map_single(priv->pdev, skb->data,
+					   hdr_info->dma_len, PCI_DMA_TODEVICE);
+	do {
+		u32 frag = 0;
+		frag |= agnx_set_bits(FIRST_FRAG, FIRST_FRAG_SHIFT, 1);
+		frag |= agnx_set_bits(LAST_FRAG, LAST_FRAG_SHIFT, 0);
+		frag |= agnx_set_bits(PACKET_LEN, PACKET_LEN_SHIFT, skb->len);
+		frag |= agnx_set_bits(FIRST_FRAG_LEN, FIRST_FRAG_LEN_SHIFT, 1);
+		frag |= agnx_set_bits(OWNER, OWNER_SHIFT, 1);
+		hdr_desc->frag = cpu_to_be32(frag);
+	} while (0);
+	hdr_desc->dma_addr = cpu_to_be32(hdr_info->mapping);
+
+
+	/* Set Frag's info and desc */
+	i = (i + 1) % ring->size;
+	frag_desc = ring->desc + i;
+	frag_info = ring->info + i;
+	memcpy(frag_info, hdr_info, sizeof(struct agnx_info));
+	frag_info->type = PACKET;
+	frag_info->dma_len = skb->len - hdr_info->dma_len;
+	frag_info->mapping = pci_map_single(priv->pdev, skb->data + hdr_info->dma_len,
+					    frag_info->dma_len, PCI_DMA_TODEVICE);
+	do {
+		u32 frag = 0;
+		frag |= agnx_set_bits(FIRST_FRAG, FIRST_FRAG_SHIFT, 0);
+		frag |= agnx_set_bits(LAST_FRAG, LAST_FRAG_SHIFT, 1);
+		frag |= agnx_set_bits(PACKET_LEN, PACKET_LEN_SHIFT, skb->len);
+		frag |= agnx_set_bits(SUB_FRAG_LEN, SUB_FRAG_LEN_SHIFT, frag_info->dma_len);
+		frag_desc->frag = cpu_to_be32(frag);
+	} while (0);
+	frag_desc->dma_addr = cpu_to_be32(frag_info->mapping);
+
+	txm_power_set(priv, txi);
+
+/* 	do { */
+/* 		int j; */
+/* 		size_t len; */
+/* 		len = skb->len - hdr_info->dma_len + hdr_info->hdr_len;  */
+/* //		if (len == 614) { */
+/* 			agnx_print_desc(hdr_desc); */
+/* 			agnx_print_desc(frag_desc); */
+/* 			agnx_print_tx_hdr((struct agnx_hdr *)skb->data); */
+/* 			agnx_print_sta_power(priv, LOCAL_STAID); */
+/* 			agnx_print_sta(priv, LOCAL_STAID); */
+/* 			for (j = 0; j < 8; j++) */
+/* 				agnx_print_sta_tx_wq(priv, LOCAL_STAID, j); */
+/* 			agnx_print_sta_power(priv, BSSID_STAID); */
+/* 			agnx_print_sta(priv, BSSID_STAID); */
+/* 			for (j = 0; j < 8; j++) */
+/* 				agnx_print_sta_tx_wq(priv, BSSID_STAID, j); */
+/* 			//	} */
+/* 	} while (0); */
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	/* FIXME ugly code */
+	/* Trigger TXM */
+	do {
+		u32 reg;
+		reg = (ioread32(priv->ctl + AGNX_CIR_TXMCTL));
+		reg |= 0x8;
+		iowrite32((reg), priv->ctl + AGNX_CIR_TXMCTL);
+	}while (0);
+
+	/* Trigger TXD */
+	do {
+		u32 reg;
+		reg = (ioread32(priv->ctl + AGNX_CIR_TXDCTL));
+		reg |= 0x8;
+		iowrite32((reg), priv->ctl + AGNX_CIR_TXDCTL);
+	}while (0);
+
+	return 0;
+}
+
+int _agnx_tx(struct agnx_priv *priv, struct sk_buff *skb)
+{
+	u16 fctl;
+
+	if (tx_packet_check(skb))
+		return 0;
+
+/* 	print_hex_dump_bytes("agnx: TX_PACKET: ", DUMP_PREFIX_NONE, */
+/* 			     skb->data, skb->len); */
+
+        fctl = le16_to_cpu(*((__le16 *)skb->data));
+
+	if ( (fctl & IEEE80211_FCTL_FTYPE)  == IEEE80211_FTYPE_DATA )
+		return __agnx_tx(priv, skb, &priv->txd);
+	else
+		return __agnx_tx(priv, skb, &priv->txm);
+}
diff --git a/drivers/staging/agnx/xmit.h b/drivers/staging/agnx/xmit.h
new file mode 100644
index 00000000000..93ac4157e69
--- /dev/null
+++ b/drivers/staging/agnx/xmit.h
@@ -0,0 +1,250 @@
+#ifndef AGNX_XMIT_H_
+#define AGNX_XMIT_H_
+
+#include <net/mac80211.h>
+
+struct agnx_priv;
+
+static inline u32 agnx_set_bits(u32 mask, u8 shift, u32 value)
+{
+	return (value << shift) & mask;
+}
+
+static inline u32 agnx_get_bits(u32 mask, u8 shift, u32 value)
+{
+	return (value & mask) >> shift;
+}
+
+
+struct agnx_rx {
+	__be16 rx_packet_duration; /*  RX Packet Duration */
+	__be16 replay_cnt;	/* Replay Count */
+} __attribute__((__packed__));
+
+
+struct agnx_tx {
+	u8 long_retry_limit; /* Long Retry Limit */
+	u8 short_retry_limit; /* Short Retry Limit */
+	u8 long_retry_cnt;	/* Long Retry Count */
+	u8 short_retry_cnt; /* Short Retry Count */
+} __attribute__((__packed__));
+
+
+/* Copy from bcm43xx */
+#define P4D_BYT3S(magic, nr_bytes)      u8 __p4dding##magic[nr_bytes]
+#define P4D_BYTES(line, nr_bytes)       P4D_BYT3S(line, nr_bytes)
+#define PAD_BYTES(nr_bytes)             P4D_BYTES(__LINE__, nr_bytes)
+
+#define P4D_BIT3S(magic, nr_bits)       __be32 __padding##magic:nr_bits
+#define P4D_BITS(line, nr_bits)         P4D_BIT3S(line, nr_bits)
+#define PAD_BITS(nr_bits)	        P4D_BITS(__LINE__, nr_bits)
+
+
+struct agnx_hdr {
+	__be32 reg0;
+#define RTS		        0x80000000 /* RTS */
+#define RTS_SHIFT		31
+#define MULTICAST	        0x40000000 /* multicast */
+#define MULTICAST_SHIFT		30
+#define ACK			0x30000000 /* ACK */
+#define ACK_SHIFT		28
+#define TM			0x08000000 /* TM */
+#define TM_SHIFT		27
+#define RELAY			0x04000000 /* Relay */
+#define RELAY_SHIFT		26
+/* 	PAD_BITS(4); */
+#define REVISED_FCS		0x00380000 /* revised FCS */
+#define REVISED_FCS_SHIFT	19
+#define NEXT_BUFFER_ADDR	0x0007FFFF /* Next Buffer Address */
+#define NEXT_BUFFER_ADDR_SHIFT	0
+
+	__be32 reg1;
+#define MAC_HDR_LEN		0xFC000000 /* MAC Header Length  */
+#define MAC_HDR_LEN_SHIFT	26
+#define DURATION_OVERIDE	0x02000000 /* Duration Override */
+#define DURATION_OVERIDE_SHIFT	25
+#define PHY_HDR_OVERIDE		0x01000000 /* PHY Header Override */
+#define PHY_HDR_OVERIDE_SHIFT	24
+#define CRC_FAIL		0x00800000 /* CRC fail */
+#define CRC_FAIL_SHIFT		23
+/*	PAD_BITS(1); */
+#define SEQUENCE_NUMBER		0x00200000 /* Sequence Number */
+#define SEQUENCE_NUMBER_SHIFT	21
+/*	PAD_BITS(2); */
+#define BUFF_HEAD_ADDR		0x0007FFFF /* Buffer Head Address */
+#define BUFF_HEAD_ADDR_SHIFT	0
+
+	__be32 reg2;
+#define PDU_COUNT		0xFC000000 /* PDU Count */
+#define PDU_COUNT_SHIFT		26
+/* 	PAD_BITS(3); */
+#define WEP_KEY			0x00600000 /* WEP Key # */
+#define WEP_KEY_SHIFT		21
+#define USES_WEP_KEY		0x00100000 /* Uses WEP Key */
+#define USES_WEP_KEY_SHIFT	20
+#define KEEP_ALIVE		0x00080000 /* Keep alive */
+#define KEEP_ALIVE_SHIFT	19
+#define BUFF_TAIL_ADDR		0x0007FFFF /* Buffer Tail Address */
+#define BUFF_TAIL_ADDR_SHIFT	0
+
+	__be32 reg3;
+#define CTS_11G			0x80000000	/* CTS in 11g */
+#define CTS_11G_SHIFT		31
+#define RTS_11G			0x40000000	/* RTS in 11g */
+#define RTS_11G_SHIFT		30
+/* PAD_BITS(2); */
+#define FRAG_SIZE		0x0FFF0000	/* fragment size */
+#define FRAG_SIZE_SHIFT		16
+#define PAYLOAD_LEN		0x0000FFF0	/* payload length */
+#define PAYLOAD_LEN_SHIFT	4
+#define FRAG_NUM		0x0000000F	/* number of frags */
+#define FRAG_NUM_SHIFT		0
+
+	__be32 reg4;
+/* 	PAD_BITS(4); */
+#define RELAY_STAID		0x0FFF0000 /* relayStald */
+#define RELAY_STAID_SHIFT	16
+#define STATION_ID		0x0000FFF0 /* Station ID */
+#define STATION_ID_SHIFT	4
+#define WORKQUEUE_ID		0x0000000F /* Workqueue ID */
+#define WORKQUEUE_ID_SHIFT	0
+
+	/* FIXME this register maybe is LE? */
+	__be32 reg5;
+/* 	PAD_BITS(4); */
+#define ROUTE_HOST		0x0F000000
+#define ROUTE_HOST_SHIFT	24
+#define ROUTE_CARD_CPU		0x00F00000
+#define ROUTE_CARD_CPU_SHIFT	20
+#define ROUTE_ENCRYPTION	0x000F0000
+#define ROUTE_ENCRYPTION_SHIFT	16
+#define ROUTE_TX		0x0000F000
+#define ROUTE_TX_SHIFT		12
+#define ROUTE_RX1		0x00000F00
+#define ROUTE_RX1_SHIFT		8
+#define ROUTE_RX2		0x000000F0
+#define ROUTE_RX2_SHIFT		4
+#define ROUTE_COMPRESSION	0x0000000F
+#define ROUTE_COMPRESSION_SHIFT 0
+
+	__be32 _11g0;			/* 11g */
+	__be32 _11g1;			/* 11g */
+	__be32 _11b0;			/* 11b */
+	__be32 _11b1;			/* 11b */
+	u8 mac_hdr[32];			/* MAC header */
+
+	__be16 rts_duration;		/* RTS duration */
+	__be16 last_duration;		/* Last duration */
+	__be16 sec_last_duration;	/* Second to Last duration */
+	__be16 other_duration;		/* Other duration */
+	__be16 tx_last_duration;	/* TX Last duration */
+	__be16 tx_other_duration;	/* TX Other Duration */
+	__be16 last_11g_len;		/* Length of last 11g */
+	__be16 other_11g_len;		/* Lenght of other 11g */
+
+	__be16 last_11b_len;		/* Length of last 11b */
+	__be16 other_11b_len;		/* Lenght of other 11b */
+
+
+	__be16 reg6;
+#define MBF			0xF000 /* mbf */
+#define MBF_SHIFT		12
+#define RSVD4			0x0FFF /* rsvd4 */
+#define RSVD4_SHIFT		0
+
+	__be16 rx_frag_stat;	/* RX fragmentation status */
+
+	__be32 time_stamp;	/* TimeStamp */
+	__be32 phy_stats_hi;	/* PHY stats hi */
+	__be32 phy_stats_lo;	/* PHY stats lo */
+	__be32 mic_key0;	/* MIC key 0 */
+	__be32 mic_key1;	/* MIC key 1 */
+
+	union {			/* RX/TX Union */
+		struct agnx_rx rx;
+		struct agnx_tx tx;
+	};
+
+	u8 rx_channel;		/* Recieve Channel */
+	PAD_BYTES(3);
+
+	u8 reserved[4];
+} __attribute__((__packed__));
+
+
+struct agnx_desc {
+#define PACKET_LEN		0xFFF00000
+#define PACKET_LEN_SHIFT	20
+/* ------------------------------------------------ */
+#define FIRST_PACKET_MASK	0x00080000
+#define FIRST_PACKET_MASK_SHIFT	19
+#define FIRST_RESERV2		0x00040000
+#define FIRST_RESERV2_SHIFT	18
+#define FIRST_TKIP_ERROR	0x00020000
+#define FIRST_TKIP_ERROR_SHIFT	17
+#define FIRST_TKIP_PACKET	0x00010000
+#define FIRST_TKIP_PACKET_SHIFT	16
+#define FIRST_RESERV1		0x0000F000
+#define FIRST_RESERV1_SHIFT	12
+#define FIRST_FRAG_LEN		0x00000FF8
+#define FIRST_FRAG_LEN_SHIFT	3
+/* ------------------------------------------------ */
+#define SUB_RESERV2		0x000c0000
+#define SUB_RESERV2_SHIFT	18
+#define SUB_TKIP_ERROR		0x00020000
+#define SUB_TKIP_ERROR_SHIFT	17
+#define SUB_TKIP_PACKET		0x00010000
+#define SUB_TKIP_PACKET_SHIFT	16
+#define SUB_RESERV1		0x00008000
+#define SUB_RESERV1_SHIFT	15
+#define SUB_FRAG_LEN		0x00007FF8
+#define SUB_FRAG_LEN_SHIFT	3
+/* ------------------------------------------------ */
+#define FIRST_FRAG		0x00000004
+#define FIRST_FRAG_SHIFT	2
+#define LAST_FRAG		0x00000002
+#define LAST_FRAG_SHIFT		1
+#define OWNER			0x00000001
+#define OWNER_SHIFT		0
+	__be32 frag;
+	__be32 dma_addr;
+} __attribute__((__packed__));
+
+enum {HEADER, PACKET};
+
+struct agnx_info {
+        struct sk_buff *skb;
+        dma_addr_t mapping;
+	u32 dma_len;		/* dma buffer len  */
+	/* Below fields only usful for tx */
+	u32 hdr_len;		/* ieee80211 header length */
+	unsigned int type;
+        struct ieee80211_tx_info *txi;
+        struct ieee80211_hdr hdr;
+};
+
+
+struct agnx_ring {
+	struct agnx_desc *desc;
+	dma_addr_t dma;
+	struct agnx_info *info;
+	/* Will lead to overflow when sent packet number enough? */
+	unsigned int idx;
+	unsigned int idx_sent;		/* only usful for txd and txm */
+	unsigned int size;
+};
+
+#define AGNX_RX_RING_SIZE	128
+#define AGNX_TXD_RING_SIZE	256
+#define AGNX_TXM_RING_SIZE	128
+
+void disable_rx_interrupt(struct agnx_priv *priv);
+void enable_rx_interrupt(struct agnx_priv *priv);
+int fill_rings(struct agnx_priv *priv);
+void unfill_rings(struct agnx_priv *priv);
+void handle_rx_irq(struct agnx_priv *priv);
+void handle_txd_irq(struct agnx_priv *priv);
+void handle_txm_irq(struct agnx_priv *priv);
+void handle_other_irq(struct agnx_priv *priv);
+int _agnx_tx(struct agnx_priv *priv, struct sk_buff *skb);
+#endif /* AGNX_XMIT_H_ */
diff --git a/drivers/staging/altpciechdma/Kconfig b/drivers/staging/altpciechdma/Kconfig
new file mode 100644
index 00000000000..0f4bf92cbbf
--- /dev/null
+++ b/drivers/staging/altpciechdma/Kconfig
@@ -0,0 +1,10 @@
+config ALTERA_PCIE_CHDMA
+	tristate "Altera PCI Express Chaining DMA driver"
+	depends on PCI
+	default N
+	---help---
+	  A reference driver that exercises the Chaining DMA logic reference
+	  design generated along the Altera FPGA PCI Express soft or hard core,
+	  only if instantiated using the MegaWizard, not the SOPC builder, of
+	  Quartus 8.1.
+
diff --git a/drivers/staging/altpciechdma/Makefile b/drivers/staging/altpciechdma/Makefile
new file mode 100644
index 00000000000..c08c8437f4d
--- /dev/null
+++ b/drivers/staging/altpciechdma/Makefile
@@ -0,0 +1,2 @@
+obj-$(CONFIG_ALTERA_PCIE_CHDMA)	+= altpciechdma.o
+
diff --git a/drivers/staging/altpciechdma/TODO b/drivers/staging/altpciechdma/TODO
new file mode 100644
index 00000000000..12c945fd61e
--- /dev/null
+++ b/drivers/staging/altpciechdma/TODO
@@ -0,0 +1,15 @@
+DONE:
+    - functionality similar to logic testbench
+
+TODO:
+	- checkpatch.pl cleanups.
+	- keep state of DMA engines.
+	- keep data structure that keeps state of each transfer.
+	- interrupt handler should iterate over outstanding descriptor tables.
+	- complete userspace cdev to read/write using the DMA engines.
+	- split off the DMA support functions in a module, re-usable by custom
+	  drivers.
+
+Please coordinate work with, and send patches to
+Leon Woestenberg <leon@sidebranch.com>
+
diff --git a/drivers/staging/altpciechdma/altpciechdma.c b/drivers/staging/altpciechdma/altpciechdma.c
new file mode 100644
index 00000000000..8e2b4ca0651
--- /dev/null
+++ b/drivers/staging/altpciechdma/altpciechdma.c
@@ -0,0 +1,1184 @@
+/**
+ * Driver for Altera PCIe core chaining DMA reference design.
+ *
+ * Copyright (C) 2008 Leon Woestenberg  <leon.woestenberg@axon.tv>
+ * Copyright (C) 2008 Nickolas Heppermann  <heppermannwdt@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ *
+ * Rationale: This driver exercises the chaining DMA read and write engine
+ * in the reference design. It is meant as a complementary reference
+ * driver that can be used for testing early designs as well as a basis to
+ * write your custom driver.
+ *
+ * Status: Test results from Leon Woestenberg  <leon.woestenberg@axon.tv>:
+ *
+ * Sendero Board w/ Cyclone II EP2C35F672C6N, PX1011A PCIe x1 PHY on a
+ * Dell Precision 370 PC, x86, kernel 2.6.20 from Ubuntu 7.04.
+ *
+ * Sendero Board w/ Cyclone II EP2C35F672C6N, PX1011A PCIe x1 PHY on a
+ * Freescale MPC8313E-RDB board, PowerPC, 2.6.24 w/ Freescale patches.
+ *
+ * Driver tests passed with PCIe Compiler 8.1. With PCIe 8.0 the DMA
+ * loopback test had reproducable compare errors. I assume a change
+ * in the compiler or reference design, but could not find evidence nor
+ * documentation on a change or fix in that direction.
+ *
+ * The reference design does not have readable locations and thus a
+ * dummy read, used to flush PCI posted writes, cannot be performed.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/cdev.h>
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/jiffies.h>
+#include <linux/module.h>
+#include <linux/pci.h>
+
+
+/* by default do not build the character device interface */
+/* XXX It is non-functional yet */
+#ifndef ALTPCIECHDMA_CDEV
+#  define ALTPCIECHDMA_CDEV 0
+#endif
+
+/* build the character device interface? */
+#if ALTPCIECHDMA_CDEV
+#  define MAX_CHDMA_SIZE (8 * 1024 * 1024)
+#  include "mapper_user_to_sg.h"
+#endif
+
+/** driver name, mimicks Altera naming of the reference design */
+#define DRV_NAME "altpciechdma"
+/** number of BARs on the device */
+#define APE_BAR_NUM (6)
+/** BAR number where the RCSLAVE memory sits */
+#define APE_BAR_RCSLAVE (0)
+/** BAR number where the Descriptor Header sits */
+#define APE_BAR_HEADER (2)
+
+/** maximum size in bytes of the descriptor table, chdma logic limit */
+#define APE_CHDMA_TABLE_SIZE (4096)
+/* single transfer must not exceed 255 table entries. worst case this can be
+ * achieved by 255 scattered pages, with only a single byte in the head and
+ * tail pages. 253 * PAGE_SIZE is a safe upper bound for the transfer size.
+ */
+#define APE_CHDMA_MAX_TRANSFER_LEN (253 * PAGE_SIZE)
+
+/**
+ * Specifies those BARs to be mapped and the length of each mapping.
+ *
+ * Zero (0) means do not map, otherwise specifies the BAR lengths to be mapped.
+ * If the actual BAR length is less, this is considered an error; then
+ * reconfigure your PCIe core.
+ *
+ * @see ug_pci_express 8.0, table 7-2 at page 7-13.
+ */
+static const unsigned long bar_min_len[APE_BAR_NUM] =
+	{ 32768, 0, 256, 0, 32768, 0 };
+
+/**
+ * Descriptor Header, controls the DMA read engine or write engine.
+ *
+ * The descriptor header is the main data structure for starting DMA transfers.
+ *
+ * It sits in End Point (FPGA) memory BAR[2] for 32-bit or BAR[3:2] for 64-bit.
+ * It references a descriptor table which exists in Root Complex (PC) memory.
+ * Writing the rclast field starts the DMA operation, thus all other structures
+ * and fields must be setup before doing so.
+ *
+ * @see ug_pci_express 8.0, tables 7-3, 7-4 and 7-5 at page 7-14.
+ * @note This header must be written in four 32-bit (PCI DWORD) writes.
+ */
+struct ape_chdma_header {
+	/**
+	 * w0 consists of two 16-bit fields:
+	 * lsb u16 number; number of descriptors in ape_chdma_table
+	 * msb u16 control; global control flags
+	 */
+	u32 w0;
+	/* bus address to ape_chdma_table in Root Complex memory */
+	u32 bdt_addr_h;
+	u32 bdt_addr_l;
+	/**
+	 * w3 consists of two 16-bit fields:
+	 * - lsb u16 rclast; last descriptor number available in Root Complex
+	 *    - zero (0) means the first descriptor is ready,
+	 *    - one (1) means two descriptors are ready, etc.
+	 * - msb u16 reserved;
+	 *
+	 * @note writing to this memory location starts the DMA operation!
+	 */
+	u32 w3;
+} __attribute__ ((packed));
+
+/**
+ * Descriptor Entry, describing a (non-scattered) single memory block transfer.
+ *
+ * There is one descriptor for each memory block involved in the transfer, a
+ * block being a contiguous address range on the bus.
+ *
+ * Multiple descriptors are chained by means of the ape_chdma_table data
+ * structure.
+ *
+ * @see ug_pci_express 8.0, tables 7-6, 7-7 and 7-8 at page 7-14 and page 7-15.
+ */
+struct ape_chdma_desc {
+	/**
+	 * w0 consists of two 16-bit fields:
+	 * number of DWORDS to transfer
+	 * - lsb u16 length;
+	 * global control
+	 * - msb u16 control;
+	 */
+	u32 w0;
+	/* address of memory in the End Point */
+	u32 ep_addr;
+	/* bus address of source or destination memory in the Root Complex */
+	u32 rc_addr_h;
+	u32 rc_addr_l;
+} __attribute__ ((packed));
+
+/**
+ * Descriptor Table, an array of descriptors describing a chained transfer.
+ *
+ * An array of descriptors, preceded by workspace for the End Point.
+ * It exists in Root Complex memory.
+ *
+ * The End Point can update its last completed descriptor number in the
+ * eplast field if requested by setting the EPLAST_ENA bit either
+ * globally in the header's or locally in any descriptor's control field.
+ *
+ * @note this structure may not exceed 4096 bytes. This results in a
+ * maximum of 4096 / (4 * 4) - 1 = 255 descriptors per chained transfer.
+ *
+ * @see ug_pci_express 8.0, tables 7-9, 7-10 and 7-11 at page 7-17 and page 7-18.
+ */
+struct ape_chdma_table {
+	/* workspace 0x00-0x0b, reserved */
+	u32 reserved1[3];
+	/* workspace 0x0c-0x0f, last descriptor handled by End Point */
+	u32 w3;
+	/* the actual array of descriptors
+    * 0x10-0x1f, 0x20-0x2f, ... 0xff0-0xfff (255 entries)
+    */
+	struct ape_chdma_desc desc[255];
+} __attribute__ ((packed));
+
+/**
+ * Altera PCI Express ('ape') board specific book keeping data
+ *
+ * Keeps state of the PCIe core and the Chaining DMA controller
+ * application.
+ */
+struct ape_dev {
+	/** the kernel pci device data structure provided by probe() */
+	struct pci_dev *pci_dev;
+	/**
+	 * kernel virtual address of the mapped BAR memory and IO regions of
+	 * the End Point. Used by map_bars()/unmap_bars().
+	 */
+	void * __iomem bar[APE_BAR_NUM];
+	/** kernel virtual address for Descriptor Table in Root Complex memory */
+	struct ape_chdma_table *table_virt;
+	/**
+	 * bus address for the Descriptor Table in Root Complex memory, in
+	 * CPU-native endianess
+	 */
+	dma_addr_t table_bus;
+	/* if the device regions could not be allocated, assume and remember it
+	 * is in use by another driver; this driver must not disable the device.
+	 */
+	int in_use;
+	/* whether this driver enabled msi for the device */
+	int msi_enabled;
+	/* whether this driver could obtain the regions */
+	int got_regions;
+	/* irq line succesfully requested by this driver, -1 otherwise */
+	int irq_line;
+	/* board revision */
+	u8 revision;
+	/* interrupt count, incremented by the interrupt handler */
+	int irq_count;
+#if ALTPCIECHDMA_CDEV
+	/* character device */
+	dev_t cdevno;
+	struct cdev cdev;
+	/* user space scatter gather mapper */
+	struct sg_mapping_t *sgm;
+#endif
+};
+
+/**
+ * Using the subsystem vendor id and subsystem id, it is possible to
+ * distinguish between different cards bases around the same
+ * (third-party) logic core.
+ *
+ * Default Altera vendor and device ID's, and some (non-reserved)
+ * ID's are now used here that are used amongst the testers/developers.
+ */
+static const struct pci_device_id ids[] = {
+	{ PCI_DEVICE(0x1172, 0xE001), },
+	{ PCI_DEVICE(0x2071, 0x2071), },
+	{ 0, }
+};
+MODULE_DEVICE_TABLE(pci, ids);
+
+#if ALTPCIECHDMA_CDEV
+/* prototypes for character device */
+static int sg_init(struct ape_dev *ape);
+static void sg_exit(struct ape_dev *ape);
+#endif
+
+/**
+ * altpciechdma_isr() - Interrupt handler
+ *
+ */
+static irqreturn_t altpciechdma_isr(int irq, void *dev_id)
+{
+	struct ape_dev *ape = (struct ape_dev *)dev_id;
+	if (!ape)
+		return IRQ_NONE;
+	ape->irq_count++;
+	return IRQ_HANDLED;
+}
+
+static int __devinit scan_bars(struct ape_dev *ape, struct pci_dev *dev)
+{
+	int i;
+	for (i = 0; i < APE_BAR_NUM; i++) {
+		unsigned long bar_start = pci_resource_start(dev, i);
+		if (bar_start) {
+			unsigned long bar_end = pci_resource_end(dev, i);
+			unsigned long bar_flags = pci_resource_flags(dev, i);
+			printk(KERN_DEBUG "BAR%d 0x%08lx-0x%08lx flags 0x%08lx\n",
+			  i, bar_start, bar_end, bar_flags);
+		}
+	}
+	return 0;
+}
+
+/**
+ * Unmap the BAR regions that had been mapped earlier using map_bars()
+ */
+static void unmap_bars(struct ape_dev *ape, struct pci_dev *dev)
+{
+	int i;
+	for (i = 0; i < APE_BAR_NUM; i++) {
+	  /* is this BAR mapped? */
+		if (ape->bar[i]) {
+			/* unmap BAR */
+			pci_iounmap(dev, ape->bar[i]);
+			ape->bar[i] = NULL;
+		}
+	}
+}
+
+/**
+ * Map the device memory regions into kernel virtual address space after
+ * verifying their sizes respect the minimum sizes needed, given by the
+ * bar_min_len[] array.
+ */
+static int __devinit map_bars(struct ape_dev *ape, struct pci_dev *dev)
+{
+	int rc;
+	int i;
+	/* iterate through all the BARs */
+	for (i = 0; i < APE_BAR_NUM; i++) {
+		unsigned long bar_start = pci_resource_start(dev, i);
+		unsigned long bar_end = pci_resource_end(dev, i);
+		unsigned long bar_length = bar_end - bar_start + 1;
+		ape->bar[i] = NULL;
+		/* do not map, and skip, BARs with length 0 */
+		if (!bar_min_len[i])
+			continue;
+		/* do not map BARs with address 0 */
+		if (!bar_start || !bar_end) {
+            printk(KERN_DEBUG "BAR #%d is not present?!\n", i);
+			rc = -1;
+			goto fail;
+		}
+		bar_length = bar_end - bar_start + 1;
+		/* BAR length is less than driver requires? */
+		if (bar_length < bar_min_len[i]) {
+            printk(KERN_DEBUG "BAR #%d length = %lu bytes but driver "
+            "requires at least %lu bytes\n", i, bar_length, bar_min_len[i]);
+			rc = -1;
+			goto fail;
+		}
+		/* map the device memory or IO region into kernel virtual
+		 * address space */
+		ape->bar[i] = pci_iomap(dev, i, bar_min_len[i]);
+		if (!ape->bar[i]) {
+			printk(KERN_DEBUG "Could not map BAR #%d.\n", i);
+			rc = -1;
+			goto fail;
+		}
+        printk(KERN_DEBUG "BAR[%d] mapped at 0x%p with length %lu(/%lu).\n", i,
+			ape->bar[i], bar_min_len[i], bar_length);
+	}
+	/* succesfully mapped all required BAR regions */
+	rc = 0;
+	goto success;
+fail:
+	/* unmap any BARs that we did map */
+	unmap_bars(ape, dev);
+success:
+	return rc;
+}
+
+#if 0 /* not yet implemented fully FIXME add opcode */
+static void __devinit rcslave_test(struct ape_dev *ape, struct pci_dev *dev)
+{
+	u32 *rcslave_mem = (u32 *)ape->bar[APE_BAR_RCSLAVE];
+	u32 result = 0;
+	/** this number is assumed to be different each time this test runs */
+	u32 seed = (u32)jiffies;
+	u32 value = seed;
+	int i;
+
+	/* write loop */
+	value = seed;
+	for (i = 1024; i < 32768 / 4 ; i++) {
+		printk(KERN_DEBUG "Writing 0x%08x to 0x%p.\n",
+			(u32)value, (void *)rcslave_mem + i);
+		iowrite32(value, rcslave_mem + i);
+		value++;
+	}
+	/* read-back loop */
+	value = seed;
+	for (i = 1024; i < 32768 / 4; i++) {
+		result = ioread32(rcslave_mem + i);
+		if (result != value) {
+			printk(KERN_DEBUG "Wrote 0x%08x to 0x%p, but read back 0x%08x.\n",
+				(u32)value, (void *)rcslave_mem + i, (u32)result);
+			break;
+		}
+		value++;
+	}
+}
+#endif
+
+/* obtain the 32 most significant (high) bits of a 32-bit or 64-bit address */
+#define pci_dma_h(addr) ((addr >> 16) >> 16)
+/* obtain the 32 least significant (low) bits of a 32-bit or 64-bit address */
+#define pci_dma_l(addr) (addr & 0xffffffffUL)
+
+/* ape_fill_chdma_desc() - Fill a Altera PCI Express Chaining DMA descriptor
+ *
+ * @desc pointer to descriptor to be filled
+ * @addr root complex address
+ * @ep_addr end point address
+ * @len number of bytes, must be a multiple of 4.
+ */
+static inline void ape_chdma_desc_set(struct ape_chdma_desc *desc, dma_addr_t addr, u32 ep_addr, int len)
+{
+  BUG_ON(len & 3);
+	desc->w0 = cpu_to_le32(len / 4);
+	desc->ep_addr = cpu_to_le32(ep_addr);
+	desc->rc_addr_h = cpu_to_le32(pci_dma_h(addr));
+	desc->rc_addr_l = cpu_to_le32(pci_dma_l(addr));
+}
+
+/*
+ * ape_sg_to_chdma_table() - Create a device descriptor table from a scatterlist.
+ *
+ * The scatterlist must have been mapped by pci_map_sg(sgm->sgl).
+ *
+ * @sgl scatterlist.
+ * @nents Number of entries in the scatterlist.
+ * @first Start index in the scatterlist sgm->sgl.
+ * @ep_addr End Point address for the scatter/gather transfer.
+ * @desc pointer to first descriptor
+ *
+ * Returns Number of entries in the table on success, -1 on error.
+ */
+static int ape_sg_to_chdma_table(struct scatterlist *sgl, int nents, int first, struct ape_chdma_desc *desc, u32 ep_addr)
+{
+	int i = first, j = 0;
+	/* inspect first entry */
+	dma_addr_t addr = sg_dma_address(&sgl[i]);
+	unsigned int len = sg_dma_len(&sgl[i]);
+	/* contiguous block */
+	dma_addr_t cont_addr = addr;
+	unsigned int cont_len = len;
+	/* iterate over remaining entries */
+	for (; j < 25 && i < nents - 1; i++) {
+		/* bus address of next entry i + 1 */
+		dma_addr_t next = sg_dma_address(&sgl[i + 1]);
+		/* length of this entry i */
+		len = sg_dma_len(&sgl[i]);
+		printk(KERN_DEBUG "%04d: addr=0x%08x length=0x%08x\n", i, addr, len);
+		/* entry i + 1 is non-contiguous with entry i? */
+		if (next != addr + len) {
+			/* TODO create entry here (we could overwrite i) */
+			printk(KERN_DEBUG "%4d: cont_addr=0x%08x cont_len=0x%08x\n", j, cont_addr, cont_len);
+			/* set descriptor for contiguous transfer */
+			ape_chdma_desc_set(&desc[j], cont_addr, ep_addr, cont_len);
+			/* next end point memory address */
+			ep_addr += cont_len;
+			/* start new contiguous block */
+			cont_addr = next;
+			cont_len = 0;
+			j++;
+		}
+		/* add entry i + 1 to current contiguous block */
+		cont_len += len;
+		/* goto entry i + 1 */
+		addr = next;
+	}
+	/* TODO create entry here  (we could overwrite i) */
+	printk(KERN_DEBUG "%04d: addr=0x%08x length=0x%08x\n", i, addr, len);
+	printk(KERN_DEBUG "%4d: cont_addr=0x%08x length=0x%08x\n", j, cont_addr, cont_len);
+	j++;
+	return j;
+}
+
+/* compare buffers */
+static inline int compare(u32 *p, u32 *q, int len)
+{
+	int result = -1;
+	int fail = 0;
+	int i;
+	for (i = 0; i < len / 4; i++) {
+		if (*p == *q) {
+			/* every so many u32 words, show equals */
+			if ((i & 255) == 0)
+				printk(KERN_DEBUG "[%p] = 0x%08x    [%p] = 0x%08x\n", p, *p, q, *q);
+		} else {
+			fail++;
+			/* show the first few miscompares */
+			if (fail < 10) {
+                printk(KERN_DEBUG "[%p] = 0x%08x != [%p] = 0x%08x ?!\n", p, *p, q, *q);
+            /* but stop after a while */
+            } else if (fail == 10) {
+                printk(KERN_DEBUG "---more errors follow! not printed---\n");
+		  	} else {
+				/* stop compare after this many errors */
+                break;
+            }
+		}
+		p++;
+		q++;
+	}
+	if (!fail)
+		result = 0;
+	return result;
+}
+
+/* dma_test() - Perform DMA loop back test to end point and back to root complex.
+ *
+ * Allocate a cache-coherent buffer in host memory, consisting of four pages.
+ *
+ * Fill the four memory pages such that each 32-bit word contains its own address.
+ *
+ * Now perform a loop back test, have the end point device copy the first buffer
+ * half to end point memory, then have it copy back into the second half.
+ *
+ *   Create a descriptor table to copy the first buffer half into End Point
+ *   memory. Instruct the End Point to do a DMA read using that table.
+ *
+ *   Create a descriptor table to copy End Point memory to the second buffer
+ *   half. Instruct the End Point to do a DMA write using that table.
+ *
+ * Compare results, fail or pass.
+ *
+ */
+static int __devinit dma_test(struct ape_dev *ape, struct pci_dev *dev)
+{
+	/* test result; guilty until proven innocent */
+	int result = -1;
+	/* the DMA read header sits at address 0x00 of the DMA engine BAR */
+	struct ape_chdma_header *write_header = (struct ape_chdma_header *)ape->bar[APE_BAR_HEADER];
+	/* the write DMA header sits after the read header at address 0x10 */
+	struct ape_chdma_header *read_header = write_header + 1;
+	/* virtual address of the allocated buffer */
+	u8 *buffer_virt = 0;
+	/* bus address of the allocated buffer */
+	dma_addr_t buffer_bus = 0;
+	int i, n = 0, irq_count;
+
+	/* temporary value used to construct 32-bit data words */
+	u32 w;
+
+	printk(KERN_DEBUG "bar_tests(), PAGE_SIZE = 0x%0x\n", (int)PAGE_SIZE);
+	printk(KERN_DEBUG "write_header = 0x%p.\n", write_header);
+	printk(KERN_DEBUG "read_header = 0x%p.\n", read_header);
+	printk(KERN_DEBUG "&write_header->w3 = 0x%p\n", &write_header->w3);
+	printk(KERN_DEBUG "&read_header->w3 = 0x%p\n", &read_header->w3);
+	printk(KERN_DEBUG "ape->table_virt = 0x%p.\n", ape->table_virt);
+
+	if (!write_header || !read_header || !ape->table_virt)
+        goto fail;
+
+	/* allocate and map coherently-cached memory for a DMA-able buffer */
+	/* @see 2.6.26.2/Documentation/DMA-mapping.txt line 318 */
+	buffer_virt = (u8 *)pci_alloc_consistent(dev, PAGE_SIZE * 4, &buffer_bus);
+	if (!buffer_virt) {
+		printk(KERN_DEBUG "Could not allocate coherent DMA buffer.\n");
+		goto fail;
+	}
+	printk(KERN_DEBUG "Allocated cache-coherent DMA buffer (virtual address = 0x%016llx, bus address = 0x%016llx).\n",
+		(u64)buffer_virt, (u64)buffer_bus);
+
+	/* fill first half of buffer with its virtual address as data */
+	for (i = 0; i < 4 * PAGE_SIZE; i += 4)
+#if 0
+		*(u32 *)(buffer_virt + i) = i / PAGE_SIZE + 1;
+#else
+		*(u32 *)(buffer_virt + i) = (buffer_virt + i);
+#endif
+#if 0
+  compare((u32 *)buffer_virt, (u32 *)(buffer_virt + 2 * PAGE_SIZE), 8192);
+#endif
+
+#if 0
+	/* fill second half of buffer with zeroes */
+	for (i = 2 * PAGE_SIZE; i < 4 * PAGE_SIZE; i += 4)
+		*(u32 *)(buffer_virt + i) = 0;
+#endif
+
+	/* invalidate EPLAST, outside 0-255, 0xFADE is from the testbench */
+	ape->table_virt->w3 = cpu_to_le32(0x0000FADE);
+
+	/* fill in first descriptor */
+	n = 0;
+	/* read 8192 bytes from RC buffer to EP address 4096 */
+	ape_chdma_desc_set(&ape->table_virt->desc[n], buffer_bus, 4096, 2 * PAGE_SIZE);
+#if 1
+	for (i = 0; i < 255; i++) {
+		ape_chdma_desc_set(&ape->table_virt->desc[i], buffer_bus, 4096, 2 * PAGE_SIZE);
+	}
+	/* index of last descriptor */
+	n = i - 1;
+#endif
+#if 0
+	/* fill in next descriptor */
+	n++;
+	/* read 1024 bytes from RC buffer to EP address 4096 + 1024 */
+	ape_chdma_desc_set(&ape->table_virt->desc[n], buffer_bus + 1024, 4096 + 1024, 1024);
+#endif
+
+#if 1
+	/* enable MSI after the last descriptor is completed */
+	if (ape->msi_enabled)
+		ape->table_virt->desc[n].w0 |= cpu_to_le32(1UL << 16)/*local MSI*/;
+#endif
+#if 0
+	/* dump descriptor table for debugging */
+	printk(KERN_DEBUG "Descriptor Table (Read, in Root Complex Memory, # = %d)\n", n + 1);
+	for (i = 0; i < 4 + (n + 1) * 4; i += 4) {
+		u32 *p = (u32 *)ape->table_virt;
+		p += i;
+		printk(KERN_DEBUG "0x%08x/0x%02x: 0x%08x (LEN=0x%x)\n", (u32)p, (u32)p & 15, *p, 4 * le32_to_cpu(*p));
+		p++;
+		printk(KERN_DEBUG "0x%08x/0x%02x: 0x%08x (EPA=0x%x)\n", (u32)p, (u32)p & 15, *p, le32_to_cpu(*p));
+		p++;
+		printk(KERN_DEBUG "0x%08x/0x%02x: 0x%08x (RCH=0x%x)\n", (u32)p, (u32)p & 15, *p, le32_to_cpu(*p));
+		p++;
+		printk(KERN_DEBUG "0x%08x/0x%02x: 0x%08x (RCL=0x%x)\n", (u32)p, (u32)p & 15, *p, le32_to_cpu(*p));
+	}
+#endif
+	/* set available number of descriptors in table */
+	w = (u32)(n + 1);
+	w |= (1UL << 18)/*global EPLAST_EN*/;
+#if 0
+	if (ape->msi_enabled)
+		w |= (1UL << 17)/*global MSI*/;
+#endif
+	printk(KERN_DEBUG "writing 0x%08x to 0x%p\n", w, (void *)&read_header->w0);
+	iowrite32(w, &read_header->w0);
+
+	/* write table address (higher 32-bits) */
+	printk(KERN_DEBUG "writing 0x%08x to 0x%p\n", (u32)((ape->table_bus >> 16) >> 16), (void *)&read_header->bdt_addr_h);
+	iowrite32(pci_dma_h(ape->table_bus), &read_header->bdt_addr_h);
+
+	/* write table address (lower 32-bits) */
+	printk(KERN_DEBUG "writing 0x%08x to 0x%p\n", (u32)(ape->table_bus & 0xffffffffUL), (void *)&read_header->bdt_addr_l);
+	iowrite32(pci_dma_l(ape->table_bus), &read_header->bdt_addr_l);
+
+	/* memory write barrier */
+	wmb();
+	printk(KERN_DEBUG "Flush posted writes\n");
+	/** FIXME Add dummy read to flush posted writes but need a readable location! */
+#if 0
+	(void)ioread32();
+#endif
+
+	/* remember IRQ count before the transfer */
+	irq_count = ape->irq_count;
+	/* write number of descriptors - this starts the DMA */
+	printk(KERN_DEBUG "\nStart DMA read\n");
+	printk(KERN_DEBUG "writing 0x%08x to 0x%p\n", (u32)n, (void *)&read_header->w3);
+	iowrite32(n, &read_header->w3);
+	printk(KERN_DEBUG "EPLAST = %lu\n", le32_to_cpu(*(u32 *)&ape->table_virt->w3) & 0xffffUL);
+
+	/** memory write barrier */
+	wmb();
+	/* dummy read to flush posted writes */
+	/* FIXME Need a readable location! */
+#if 0
+	(void)ioread32();
+#endif
+	printk(KERN_DEBUG "POLL FOR READ:\n");
+	/* poll for chain completion, 1000 times 1 millisecond */
+	for (i = 0; i < 100; i++) {
+		volatile u32 *p = &ape->table_virt->w3;
+		u32 eplast = le32_to_cpu(*p) & 0xffffUL;
+		printk(KERN_DEBUG "EPLAST = %u, n = %d\n", eplast, n);
+		if (eplast == n) {
+			printk(KERN_DEBUG "DONE\n");
+            /* print IRQ count before the transfer */
+			printk(KERN_DEBUG "#IRQs during transfer: %d\n", ape->irq_count - irq_count);
+			break;
+		}
+		udelay(100);
+	}
+
+	/* invalidate EPLAST, outside 0-255, 0xFADE is from the testbench */
+	ape->table_virt->w3 = cpu_to_le32(0x0000FADE);
+
+	/* setup first descriptor */
+	n = 0;
+	ape_chdma_desc_set(&ape->table_virt->desc[n], buffer_bus + 8192, 4096, 2 * PAGE_SIZE);
+#if 1
+	for (i = 0; i < 255; i++) {
+		ape_chdma_desc_set(&ape->table_virt->desc[i], buffer_bus + 8192, 4096, 2 * PAGE_SIZE);
+	}
+	/* index of last descriptor */
+	n = i - 1;
+#endif
+#if 1 /* test variable, make a module option later */
+	if (ape->msi_enabled)
+		ape->table_virt->desc[n].w0 |= cpu_to_le32(1UL << 16)/*local MSI*/;
+#endif
+#if 0
+	/* dump descriptor table for debugging */
+	printk(KERN_DEBUG "Descriptor Table (Write, in Root Complex Memory, # = %d)\n", n + 1);
+	for (i = 0; i < 4 + (n + 1) * 4; i += 4) {
+		u32 *p = (u32 *)ape->table_virt;
+		p += i;
+		printk(KERN_DEBUG "0x%08x/0x%02x: 0x%08x (LEN=0x%x)\n", (u32)p, (u32)p & 15, *p, 4 * le32_to_cpu(*p));
+		p++;
+		printk(KERN_DEBUG "0x%08x/0x%02x: 0x%08x (EPA=0x%x)\n", (u32)p, (u32)p & 15, *p, le32_to_cpu(*p));
+		p++;
+		printk(KERN_DEBUG "0x%08x/0x%02x: 0x%08x (RCH=0x%x)\n", (u32)p, (u32)p & 15, *p, le32_to_cpu(*p));
+		p++;
+		printk(KERN_DEBUG "0x%08x/0x%02x: 0x%08x (RCL=0x%x)\n", (u32)p, (u32)p & 15, *p, le32_to_cpu(*p));
+	}
+#endif
+
+	/* set number of available descriptors in the table */
+	w = (u32)(n + 1);
+	/* enable updates of eplast for each descriptor completion */
+	w |= (u32)(1UL << 18)/*global EPLAST_EN*/;
+#if 0 // test variable, make a module option later
+	/* enable MSI for each descriptor completion */
+	if (ape->msi_enabled)
+		w |= (1UL << 17)/*global MSI*/;
+#endif
+	iowrite32(w, &write_header->w0);
+	iowrite32(pci_dma_h(ape->table_bus), &write_header->bdt_addr_h);
+	iowrite32(pci_dma_l(ape->table_bus), &write_header->bdt_addr_l);
+
+	/** memory write barrier and flush posted writes */
+	wmb();
+	/* dummy read to flush posted writes */
+	/* FIXME Need a readable location! */
+#if 0
+	(void)ioread32();
+#endif
+	irq_count = ape->irq_count;
+
+	printk(KERN_DEBUG "\nStart DMA write\n");
+	iowrite32(n, &write_header->w3);
+
+	/** memory write barrier */
+	wmb();
+	/** dummy read to flush posted writes */
+	//(void)ioread32();
+
+	printk(KERN_DEBUG "POLL FOR WRITE:\n");
+	/* poll for completion, 1000 times 1 millisecond */
+	for (i = 0; i < 100; i++) {
+		volatile u32 *p = &ape->table_virt->w3;
+		u32 eplast = le32_to_cpu(*p) & 0xffffUL;
+		printk(KERN_DEBUG "EPLAST = %u, n = %d\n", eplast, n);
+		if (eplast == n) {
+			printk(KERN_DEBUG "DONE\n");
+			/* print IRQ count before the transfer */
+			printk(KERN_DEBUG "#IRQs during transfer: %d\n", ape->irq_count - irq_count);
+			break;
+		}
+		udelay(100);
+	}
+	/* soft-reset DMA write engine */
+	iowrite32(0x0000ffffUL, &write_header->w0);
+	/* soft-reset DMA read engine */
+	iowrite32(0x0000ffffUL, &read_header->w0);
+
+	/** memory write barrier */
+	wmb();
+	/* dummy read to flush posted writes */
+	/* FIXME Need a readable location! */
+#if 0
+	(void)ioread32();
+#endif
+	/* compare first half of buffer with second half, should be identical */
+	result = compare((u32 *)buffer_virt, (u32 *)(buffer_virt + 2 * PAGE_SIZE), 8192);
+	printk(KERN_DEBUG "DMA loop back test %s.\n", result ? "FAILED" : "PASSED");
+
+	pci_free_consistent(dev, 4 * PAGE_SIZE, buffer_virt, buffer_bus);
+fail:
+	printk(KERN_DEBUG "bar_tests() end, result %d\n", result);
+	return result;
+}
+
+/* Called when the PCI sub system thinks we can control the given device.
+ * Inspect if we can support the device and if so take control of it.
+ *
+ * Return 0 when we have taken control of the given device.
+ *
+ * - allocate board specific bookkeeping
+ * - allocate coherently-mapped memory for the descriptor table
+ * - enable the board
+ * - verify board revision
+ * - request regions
+ * - query DMA mask
+ * - obtain and request irq
+ * - map regions into kernel address space
+ */
+static int __devinit probe(struct pci_dev *dev, const struct pci_device_id *id)
+{
+	int rc = 0;
+	struct ape_dev *ape = NULL;
+	u8 irq_pin, irq_line;
+	printk(KERN_DEBUG "probe(dev = 0x%p, pciid = 0x%p)\n", dev, id);
+
+	/* allocate memory for per-board book keeping */
+	ape = kzalloc(sizeof(struct ape_dev), GFP_KERNEL);
+	if (!ape) {
+		printk(KERN_DEBUG "Could not kzalloc()ate memory.\n");
+		goto err_ape;
+	}
+	ape->pci_dev = dev;
+	dev->dev.driver_data = (void *)ape;
+	printk(KERN_DEBUG "probe() ape = 0x%p\n", ape);
+
+	printk(KERN_DEBUG "sizeof(struct ape_chdma_table) = %d.\n",
+		(int)sizeof(struct ape_chdma_table));
+	/* the reference design has a size restriction on the table size */
+	BUG_ON(sizeof(struct ape_chdma_table) > APE_CHDMA_TABLE_SIZE);
+
+	/* allocate and map coherently-cached memory for a descriptor table */
+	/* @see LDD3 page 446 */
+	ape->table_virt = (struct ape_chdma_table *)pci_alloc_consistent(dev,
+		APE_CHDMA_TABLE_SIZE, &ape->table_bus);
+	/* could not allocate table? */
+	if (!ape->table_virt) {
+		printk(KERN_DEBUG "Could not dma_alloc()ate_coherent memory.\n");
+		goto err_table;
+	}
+
+	printk(KERN_DEBUG "table_virt = 0x%16llx, table_bus = 0x%16llx.\n",
+		(u64)ape->table_virt, (u64)ape->table_bus);
+
+	/* enable device */
+	rc = pci_enable_device(dev);
+	if (rc) {
+		printk(KERN_DEBUG "pci_enable_device() failed\n");
+		goto err_enable;
+	}
+
+	/* enable bus master capability on device */
+	pci_set_master(dev);
+	/* enable message signaled interrupts */
+	rc = pci_enable_msi(dev);
+	/* could not use MSI? */
+	if (rc) {
+		/* resort to legacy interrupts */
+		printk(KERN_DEBUG "Could not enable MSI interrupting.\n");
+		ape->msi_enabled = 0;
+	/* MSI enabled, remember for cleanup */
+	} else {
+		printk(KERN_DEBUG "Enabled MSI interrupting.\n");
+		ape->msi_enabled = 1;
+	}
+
+	pci_read_config_byte(dev, PCI_REVISION_ID, &ape->revision);
+#if 0 /* example */
+	/* (for example) this driver does not support revision 0x42 */
+    if (ape->revision == 0x42) {
+		printk(KERN_DEBUG "Revision 0x42 is not supported by this driver.\n");
+		rc = -ENODEV;
+		goto err_rev;
+	}
+#endif
+	/** XXX check for native or legacy PCIe endpoint? */
+
+	rc = pci_request_regions(dev, DRV_NAME);
+	/* could not request all regions? */
+	if (rc) {
+		/* assume device is in use (and do not disable it later!) */
+		ape->in_use = 1;
+		goto err_regions;
+	}
+	ape->got_regions = 1;
+
+#if 1 // @todo For now, disable 64-bit, because I do not understand the implications (DAC!)
+	/* query for DMA transfer */
+	/* @see Documentation/DMA-mapping.txt */
+	if (!pci_set_dma_mask(dev, DMA_64BIT_MASK)) {
+		pci_set_consistent_dma_mask(dev, DMA_64BIT_MASK);
+		/* use 64-bit DMA */
+		printk(KERN_DEBUG "Using a 64-bit DMA mask.\n");
+	} else
+#endif
+	if (!pci_set_dma_mask(dev, DMA_32BIT_MASK)) {
+		printk(KERN_DEBUG "Could not set 64-bit DMA mask.\n");
+		pci_set_consistent_dma_mask(dev, DMA_32BIT_MASK);
+		/* use 32-bit DMA */
+		printk(KERN_DEBUG "Using a 32-bit DMA mask.\n");
+	} else {
+		printk(KERN_DEBUG "No suitable DMA possible.\n");
+		/** @todo Choose proper error return code */
+		rc = -1;
+		goto err_mask;
+	}
+
+	rc = pci_read_config_byte(dev, PCI_INTERRUPT_PIN, &irq_pin);
+	/* could not read? */
+	if (rc)
+		goto err_irq;
+	printk(KERN_DEBUG "IRQ pin #%d (0=none, 1=INTA#...4=INTD#).\n", irq_pin);
+
+	/* @see LDD3, page 318 */
+	rc = pci_read_config_byte(dev, PCI_INTERRUPT_LINE, &irq_line);
+	/* could not read? */
+	if (rc) {
+		printk(KERN_DEBUG "Could not query PCI_INTERRUPT_LINE, error %d\n", rc);
+		goto err_irq;
+	}
+	printk(KERN_DEBUG "IRQ line #%d.\n", irq_line);
+#if 1
+	irq_line = dev->irq;
+	/* @see LDD3, page 259 */
+	rc = request_irq(irq_line, altpciechdma_isr, IRQF_SHARED, DRV_NAME, (void *)ape);
+	if (rc) {
+		printk(KERN_DEBUG "Could not request IRQ #%d, error %d\n", irq_line, rc);
+		ape->irq_line = -1;
+		goto err_irq;
+	}
+	/* remember which irq we allocated */
+	ape->irq_line = (int)irq_line;
+	printk(KERN_DEBUG "Succesfully requested IRQ #%d with dev_id 0x%p\n", irq_line, ape);
+#endif
+	/* show BARs */
+	scan_bars(ape, dev);
+	/* map BARs */
+	rc = map_bars(ape, dev);
+	if (rc)
+		goto err_map;
+#if ALTPCIECHDMA_CDEV
+	/* initialize character device */
+	rc = sg_init(ape);
+	if (rc)
+		goto err_cdev;
+#endif
+	/* perform DMA engines loop back test */
+	rc = dma_test(ape, dev);
+	(void)rc;
+	/* succesfully took the device */
+	rc = 0;
+	printk(KERN_DEBUG "probe() successful.\n");
+	goto end;
+err_cdev:
+	/* unmap the BARs */
+	unmap_bars(ape, dev);
+err_map:
+	/* free allocated irq */
+	if (ape->irq_line >= 0)
+		free_irq(ape->irq_line, (void *)ape);
+err_irq:
+	if (ape->msi_enabled)
+		pci_disable_msi(dev);
+	/* disable the device iff it is not in use */
+	if (!ape->in_use)
+		pci_disable_device(dev);
+	if (ape->got_regions)
+		pci_release_regions(dev);
+err_mask:
+err_regions:
+err_rev:
+/* clean up everything before device enable() */
+err_enable:
+	if (ape->table_virt)
+		pci_free_consistent(dev, APE_CHDMA_TABLE_SIZE, ape->table_virt, ape->table_bus);
+/* clean up everything before allocating descriptor table */
+err_table:
+	if (ape)
+		kfree(ape);
+err_ape:
+end:
+	return rc;
+}
+
+static void __devexit remove(struct pci_dev *dev)
+{
+	struct ape_dev *ape;
+	printk(KERN_DEBUG "remove(0x%p)\n", dev);
+	if ((dev == 0) || (dev->dev.driver_data == 0)) {
+		printk(KERN_DEBUG "remove(dev = 0x%p) dev->dev.driver_data = 0x%p\n", dev, dev->dev.driver_data);
+		return;
+	}
+	ape = (struct ape_dev *)dev->dev.driver_data;
+	printk(KERN_DEBUG "remove(dev = 0x%p) where dev->dev.driver_data = 0x%p\n", dev, ape);
+	if (ape->pci_dev != dev) {
+		printk(KERN_DEBUG "dev->dev.driver_data->pci_dev (0x%08lx) != dev (0x%08lx)\n",
+		(unsigned long)ape->pci_dev, (unsigned long)dev);
+	}
+	/* remove character device */
+#if ALTPCIECHDMA_CDEV
+	sg_exit(ape);
+#endif
+
+	if (ape->table_virt)
+		pci_free_consistent(dev, APE_CHDMA_TABLE_SIZE, ape->table_virt, ape->table_bus);
+
+	/* free IRQ
+	 * @see LDD3 page 279
+	 */
+	if (ape->irq_line >= 0) {
+		printk(KERN_DEBUG "Freeing IRQ #%d for dev_id 0x%08lx.\n",
+		ape->irq_line, (unsigned long)ape);
+		free_irq(ape->irq_line, (void *)ape);
+	}
+	/* MSI was enabled? */
+	if (ape->msi_enabled) {
+		/* Disable MSI @see Documentation/MSI-HOWTO.txt */
+		pci_disable_msi(dev);
+		ape->msi_enabled = 0;
+	}
+	/* unmap the BARs */
+	unmap_bars(ape, dev);
+	if (!ape->in_use)
+		pci_disable_device(dev);
+	if (ape->got_regions)
+		/* to be called after device disable */
+		pci_release_regions(dev);
+}
+
+#if ALTPCIECHDMA_CDEV
+
+/*
+ * Called when the device goes from unused to used.
+ */
+static int sg_open(struct inode *inode, struct file *file)
+{
+	struct ape_dev *ape;
+	printk(KERN_DEBUG DRV_NAME "_open()\n");
+	/* pointer to containing data structure of the character device inode */
+	ape = container_of(inode->i_cdev, struct ape_dev, cdev);
+	/* create a reference to our device state in the opened file */
+	file->private_data = ape;
+	/* create virtual memory mapper */
+	ape->sgm = sg_create_mapper(MAX_CHDMA_SIZE);
+	return 0;
+}
+
+/*
+ * Called when the device goes from used to unused.
+ */
+static int sg_close(struct inode *inode, struct file *file)
+{
+	/* fetch device specific data stored earlier during open */
+	struct ape_dev *ape = (struct ape_dev *)file->private_data;
+	printk(KERN_DEBUG DRV_NAME "_close()\n");
+	/* destroy virtual memory mapper */
+	sg_destroy_mapper(ape->sgm);
+	return 0;
+}
+
+static ssize_t sg_read(struct file *file, char __user *buf, size_t count, loff_t *pos)
+{
+	/* fetch device specific data stored earlier during open */
+	struct ape_dev *ape = (struct ape_dev *)file->private_data;
+	(void)ape;
+	printk(KERN_DEBUG DRV_NAME "_read(buf=0x%p, count=%lld, pos=%llu)\n", buf, (s64)count, (u64)*pos);
+	return count;
+}
+
+/* sg_write() - Write to the device
+ *
+ * @buf userspace buffer
+ * @count number of bytes in the userspace buffer
+ *
+ * Iterate over the userspace buffer, taking at most 255 * PAGE_SIZE bytes for
+ * each DMA transfer.
+ *   For each transfer, get the user pages, build a sglist, map, build a
+ *   descriptor table. submit the transfer. wait for the interrupt handler
+ *   to wake us on completion.
+ */
+static ssize_t sg_write(struct file *file, const char __user *buf, size_t count, loff_t *pos)
+{
+	int hwnents, tents;
+	size_t transfer_len, remaining = count, done = 0;
+	u64 transfer_addr = (u64)buf;
+	/* fetch device specific data stored earlier during open */
+	struct ape_dev *ape = (struct ape_dev *)file->private_data;
+	printk(KERN_DEBUG DRV_NAME "_write(buf=0x%p, count=%lld, pos=%llu)\n",
+		buf, (s64)count, (u64)*pos);
+	/* TODO transfer boundaries at PAGE_SIZE granularity */
+	while (remaining > 0)
+	{
+		/* limit DMA transfer size */
+		transfer_len = (remaining < APE_CHDMA_MAX_TRANSFER_LEN)? remaining:
+			APE_CHDMA_MAX_TRANSFER_LEN;
+		/* get all user space buffer pages and create a scattergather list */
+		sgm_map_user_pages(ape->sgm, transfer_addr, transfer_len, 0/*read from userspace*/);
+		printk(KERN_DEBUG DRV_NAME "mapped_pages=%d\n", ape->sgm->mapped_pages);
+		/* map all entries in the scattergather list */
+		hwnents = pci_map_sg(ape->pci_dev, ape->sgm->sgl, ape->sgm->mapped_pages, DMA_TO_DEVICE);
+		printk(KERN_DEBUG DRV_NAME "hwnents=%d\n", hwnents);
+		/* build device descriptor tables and submit them to the DMA engine */
+		tents = ape_sg_to_chdma_table(ape->sgm->sgl, hwnents, 0, &ape->table_virt->desc[0], 4096);
+		printk(KERN_DEBUG DRV_NAME "tents=%d\n", hwnents);
+#if 0
+		while (tables) {
+			/* TODO build table */
+			/* TODO submit table to the device */
+			/* if engine stopped and unfinished work then start engine */
+		}
+		put ourselves on wait queue
+#endif
+
+		dma_unmap_sg(NULL, ape->sgm->sgl, ape->sgm->mapped_pages, DMA_TO_DEVICE);
+		/* dirty and free the pages */
+		sgm_unmap_user_pages(ape->sgm, 1/*dirtied*/);
+		/* book keeping */
+		transfer_addr += transfer_len;
+		remaining -= transfer_len;
+		done += transfer_len;
+	}
+	return done;
+}
+
+/*
+ * character device file operations
+ */
+static struct file_operations sg_fops = {
+  .owner = THIS_MODULE,
+  .open = sg_open,
+  .release = sg_close,
+  .read = sg_read,
+  .write = sg_write,
+};
+
+/* sg_init() - Initialize character device
+ *
+ * XXX Should ideally be tied to the device, on device probe, not module init.
+ */
+static int sg_init(struct ape_dev *ape)
+{
+	int rc;
+	printk(KERN_DEBUG DRV_NAME " sg_init()\n");
+	/* allocate a dynamically allocated character device node */
+	rc = alloc_chrdev_region(&ape->cdevno, 0/*requested minor*/, 1/*count*/, DRV_NAME);
+	/* allocation failed? */
+	if (rc < 0) {
+		printk("alloc_chrdev_region() = %d\n", rc);
+		goto fail_alloc;
+	}
+	/* couple the device file operations to the character device */
+	cdev_init(&ape->cdev, &sg_fops);
+	ape->cdev.owner = THIS_MODULE;
+	/* bring character device live */
+	rc = cdev_add(&ape->cdev, ape->cdevno, 1/*count*/);
+	if (rc < 0) {
+		printk("cdev_add() = %d\n", rc);
+		goto fail_add;
+	}
+	printk(KERN_DEBUG "altpciechdma = %d:%d\n", MAJOR(ape->cdevno), MINOR(ape->cdevno));
+	return 0;
+fail_add:
+	/* free the dynamically allocated character device node */
+    unregister_chrdev_region(ape->cdevno, 1/*count*/);
+fail_alloc:
+	return -1;
+}
+
+/* sg_exit() - Cleanup character device
+ *
+ * XXX Should ideally be tied to the device, on device remove, not module exit.
+ */
+
+static void sg_exit(struct ape_dev *ape)
+{
+	printk(KERN_DEBUG DRV_NAME " sg_exit()\n");
+	/* remove the character device */
+	cdev_del(&ape->cdev);
+	/* free the dynamically allocated character device node */
+	unregister_chrdev_region(ape->cdevno, 1/*count*/);
+}
+
+#endif /* ALTPCIECHDMA_CDEV */
+
+/* used to register the driver with the PCI kernel sub system
+ * @see LDD3 page 311
+ */
+static struct pci_driver pci_driver = {
+	.name = DRV_NAME,
+	.id_table = ids,
+	.probe = probe,
+	.remove = remove,
+	/* resume, suspend are optional */
+};
+
+/**
+ * alterapciechdma_init() - Module initialization, registers devices.
+ */
+static int __init alterapciechdma_init(void)
+{
+  int rc = 0;
+	printk(KERN_DEBUG DRV_NAME " init(), built at " __DATE__ " " __TIME__ "\n");
+	/* register this driver with the PCI bus driver */
+	rc = pci_register_driver(&pci_driver);
+	if (rc < 0)
+	  return rc;
+	return 0;
+}
+
+/**
+ * alterapciechdma_init() - Module cleanup, unregisters devices.
+ */
+static void __exit alterapciechdma_exit(void)
+{
+	printk(KERN_DEBUG DRV_NAME " exit(), built at " __DATE__ " " __TIME__ "\n");
+	/* unregister this driver from the PCI bus driver */
+	pci_unregister_driver(&pci_driver);
+}
+
+MODULE_LICENSE("GPL");
+
+module_init(alterapciechdma_init);
+module_exit(alterapciechdma_exit);
+
diff --git a/drivers/staging/android/Kconfig b/drivers/staging/android/Kconfig
new file mode 100644
index 00000000000..6b996db0dd6
--- /dev/null
+++ b/drivers/staging/android/Kconfig
@@ -0,0 +1,86 @@
+menu "Android"
+
+config ANDROID
+	bool "Android Drivers"
+	default N
+	---help---
+	  Enable support for various drivers needed on the Android platform
+
+config ANDROID_BINDER_IPC
+	bool "Android Binder IPC Driver"
+	default n
+
+config ANDROID_LOGGER
+	tristate "Android log driver"
+	default n
+
+config ANDROID_RAM_CONSOLE
+	bool "Android RAM buffer console"
+	default n
+
+config ANDROID_RAM_CONSOLE_ENABLE_VERBOSE
+	bool "Enable verbose console messages on Android RAM console"
+	default y
+	depends on ANDROID_RAM_CONSOLE
+
+menuconfig ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+	bool "Android RAM Console Enable error correction"
+	default n
+	depends on ANDROID_RAM_CONSOLE
+	select REED_SOLOMON
+	select REED_SOLOMON_ENC8
+	select REED_SOLOMON_DEC8
+
+if ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+
+config ANDROID_RAM_CONSOLE_ERROR_CORRECTION_DATA_SIZE
+	int "Android RAM Console Data data size"
+	default 128
+	help
+	  Must be a power of 2.
+
+config ANDROID_RAM_CONSOLE_ERROR_CORRECTION_ECC_SIZE
+	int "Android RAM Console ECC size"
+	default 16
+
+config ANDROID_RAM_CONSOLE_ERROR_CORRECTION_SYMBOL_SIZE
+	int "Android RAM Console Symbol size"
+	default 8
+
+config ANDROID_RAM_CONSOLE_ERROR_CORRECTION_POLYNOMIAL
+	hex "Android RAM Console Polynomial"
+	default 0x19 if (ANDROID_RAM_CONSOLE_ERROR_CORRECTION_SYMBOL_SIZE = 4)
+	default 0x29 if (ANDROID_RAM_CONSOLE_ERROR_CORRECTION_SYMBOL_SIZE = 5)
+	default 0x61 if (ANDROID_RAM_CONSOLE_ERROR_CORRECTION_SYMBOL_SIZE = 6)
+	default 0x89 if (ANDROID_RAM_CONSOLE_ERROR_CORRECTION_SYMBOL_SIZE = 7)
+	default 0x11d if (ANDROID_RAM_CONSOLE_ERROR_CORRECTION_SYMBOL_SIZE = 8)
+
+endif # ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+
+config ANDROID_RAM_CONSOLE_EARLY_INIT
+	bool "Start Android RAM console early"
+	default n
+	depends on ANDROID_RAM_CONSOLE
+
+config ANDROID_RAM_CONSOLE_EARLY_ADDR
+	hex "Android RAM console virtual address"
+	default 0
+	depends on ANDROID_RAM_CONSOLE_EARLY_INIT
+
+config ANDROID_RAM_CONSOLE_EARLY_SIZE
+	hex "Android RAM console buffer size"
+	default 0
+	depends on ANDROID_RAM_CONSOLE_EARLY_INIT
+
+config ANDROID_TIMED_GPIO
+	tristate "Android timed gpio driver"
+	depends on GENERIC_GPIO
+	default n
+
+config ANDROID_LOW_MEMORY_KILLER
+	bool "Android Low Memory Killer"
+	default N
+	---help---
+	  Register processes to be killed when memory is low
+
+endmenu
diff --git a/drivers/staging/android/Makefile b/drivers/staging/android/Makefile
new file mode 100644
index 00000000000..95209d6273a
--- /dev/null
+++ b/drivers/staging/android/Makefile
@@ -0,0 +1,5 @@
+obj-$(CONFIG_ANDROID_BINDER_IPC)	+= binder.o
+obj-$(CONFIG_ANDROID_LOGGER)		+= logger.o
+obj-$(CONFIG_ANDROID_RAM_CONSOLE)	+= ram_console.o
+obj-$(CONFIG_ANDROID_TIMED_GPIO)	+= timed_gpio.o
+obj-$(CONFIG_ANDROID_LOW_MEMORY_KILLER)	+= lowmemorykiller.o
diff --git a/drivers/staging/android/TODO b/drivers/staging/android/TODO
new file mode 100644
index 00000000000..e59c5be4be2
--- /dev/null
+++ b/drivers/staging/android/TODO
@@ -0,0 +1,10 @@
+TODO:
+	- checkpatch.pl cleanups
+	- sparse fixes
+	- rename files to be not so "generic"
+	- make sure things build as modules properly
+	- add proper arch dependancies as needed
+	- audit userspace interfaces to make sure they are sane
+
+Please send patches to Greg Kroah-Hartman <greg@kroah.com> and Cc:
+Brian Swetland <swetland@google.com>
diff --git a/drivers/staging/android/binder.c b/drivers/staging/android/binder.c
new file mode 100644
index 00000000000..6a4ceacb33f
--- /dev/null
+++ b/drivers/staging/android/binder.c
@@ -0,0 +1,3503 @@
+/* binder.c
+ *
+ * Android IPC Subsystem
+ *
+ * Copyright (C) 2007-2008 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <asm/cacheflush.h>
+#include <linux/fdtable.h>
+#include <linux/file.h>
+#include <linux/fs.h>
+#include <linux/list.h>
+#include <linux/miscdevice.h>
+#include <linux/mm.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/nsproxy.h>
+#include <linux/poll.h>
+#include <linux/proc_fs.h>
+#include <linux/rbtree.h>
+#include <linux/sched.h>
+#include <linux/uaccess.h>
+#include <linux/vmalloc.h>
+#include "binder.h"
+
+static DEFINE_MUTEX(binder_lock);
+static HLIST_HEAD(binder_procs);
+static struct binder_node *binder_context_mgr_node;
+static uid_t binder_context_mgr_uid = -1;
+static int binder_last_id;
+static struct proc_dir_entry *binder_proc_dir_entry_root;
+static struct proc_dir_entry *binder_proc_dir_entry_proc;
+static struct hlist_head binder_dead_nodes;
+
+static int binder_read_proc_proc(
+	char *page, char **start, off_t off, int count, int *eof, void *data);
+
+/* This is only defined in include/asm-arm/sizes.h */
+#ifndef SZ_1K
+#define SZ_1K                               0x400
+#endif
+
+#ifndef SZ_4M
+#define SZ_4M                               0x400000
+#endif
+
+#ifndef __i386__
+#define FORBIDDEN_MMAP_FLAGS                (VM_WRITE | VM_EXEC)
+#else
+#define FORBIDDEN_MMAP_FLAGS                (VM_WRITE)
+#endif
+
+#define BINDER_SMALL_BUF_SIZE (PAGE_SIZE * 64)
+
+enum {
+	BINDER_DEBUG_USER_ERROR             = 1U << 0,
+	BINDER_DEBUG_FAILED_TRANSACTION     = 1U << 1,
+	BINDER_DEBUG_DEAD_TRANSACTION       = 1U << 2,
+	BINDER_DEBUG_OPEN_CLOSE             = 1U << 3,
+	BINDER_DEBUG_DEAD_BINDER            = 1U << 4,
+	BINDER_DEBUG_DEATH_NOTIFICATION     = 1U << 5,
+	BINDER_DEBUG_READ_WRITE             = 1U << 6,
+	BINDER_DEBUG_USER_REFS              = 1U << 7,
+	BINDER_DEBUG_THREADS                = 1U << 8,
+	BINDER_DEBUG_TRANSACTION            = 1U << 9,
+	BINDER_DEBUG_TRANSACTION_COMPLETE   = 1U << 10,
+	BINDER_DEBUG_FREE_BUFFER            = 1U << 11,
+	BINDER_DEBUG_INTERNAL_REFS          = 1U << 12,
+	BINDER_DEBUG_BUFFER_ALLOC           = 1U << 13,
+	BINDER_DEBUG_PRIORITY_CAP           = 1U << 14,
+	BINDER_DEBUG_BUFFER_ALLOC_ASYNC     = 1U << 15,
+};
+static uint32_t binder_debug_mask = BINDER_DEBUG_USER_ERROR |
+	BINDER_DEBUG_FAILED_TRANSACTION | BINDER_DEBUG_DEAD_TRANSACTION;
+module_param_named(debug_mask, binder_debug_mask, uint, S_IWUSR | S_IRUGO);
+static int binder_debug_no_lock;
+module_param_named(proc_no_lock, binder_debug_no_lock, bool, S_IWUSR | S_IRUGO);
+static DECLARE_WAIT_QUEUE_HEAD(binder_user_error_wait);
+static int binder_stop_on_user_error;
+static int binder_set_stop_on_user_error(
+	const char *val, struct kernel_param *kp)
+{
+	int ret;
+	ret = param_set_int(val, kp);
+	if (binder_stop_on_user_error < 2)
+		wake_up(&binder_user_error_wait);
+	return ret;
+}
+module_param_call(stop_on_user_error, binder_set_stop_on_user_error,
+	param_get_int, &binder_stop_on_user_error, S_IWUSR | S_IRUGO);
+
+#define binder_user_error(x...) \
+	do { \
+		if (binder_debug_mask & BINDER_DEBUG_USER_ERROR) \
+			printk(KERN_INFO x); \
+		if (binder_stop_on_user_error) \
+			binder_stop_on_user_error = 2; \
+	} while (0)
+
+enum {
+	BINDER_STAT_PROC,
+	BINDER_STAT_THREAD,
+	BINDER_STAT_NODE,
+	BINDER_STAT_REF,
+	BINDER_STAT_DEATH,
+	BINDER_STAT_TRANSACTION,
+	BINDER_STAT_TRANSACTION_COMPLETE,
+	BINDER_STAT_COUNT
+};
+
+struct binder_stats {
+	int br[_IOC_NR(BR_FAILED_REPLY) + 1];
+	int bc[_IOC_NR(BC_DEAD_BINDER_DONE) + 1];
+	int obj_created[BINDER_STAT_COUNT];
+	int obj_deleted[BINDER_STAT_COUNT];
+};
+
+static struct binder_stats binder_stats;
+
+struct binder_transaction_log_entry {
+	int debug_id;
+	int call_type;
+	int from_proc;
+	int from_thread;
+	int target_handle;
+	int to_proc;
+	int to_thread;
+	int to_node;
+	int data_size;
+	int offsets_size;
+};
+struct binder_transaction_log {
+	int next;
+	int full;
+	struct binder_transaction_log_entry entry[32];
+};
+struct binder_transaction_log binder_transaction_log;
+struct binder_transaction_log binder_transaction_log_failed;
+
+static struct binder_transaction_log_entry *binder_transaction_log_add(
+	struct binder_transaction_log *log)
+{
+	struct binder_transaction_log_entry *e;
+	e = &log->entry[log->next];
+	memset(e, 0, sizeof(*e));
+	log->next++;
+	if (log->next == ARRAY_SIZE(log->entry)) {
+		log->next = 0;
+		log->full = 1;
+	}
+	return e;
+}
+
+struct binder_work {
+	struct list_head entry;
+	enum {
+		BINDER_WORK_TRANSACTION = 1,
+		BINDER_WORK_TRANSACTION_COMPLETE,
+		BINDER_WORK_NODE,
+		BINDER_WORK_DEAD_BINDER,
+		BINDER_WORK_DEAD_BINDER_AND_CLEAR,
+		BINDER_WORK_CLEAR_DEATH_NOTIFICATION,
+	} type;
+};
+
+struct binder_node {
+	int debug_id;
+	struct binder_work work;
+	union {
+		struct rb_node rb_node;
+		struct hlist_node dead_node;
+	};
+	struct binder_proc *proc;
+	struct hlist_head refs;
+	int internal_strong_refs;
+	int local_weak_refs;
+	int local_strong_refs;
+	void __user *ptr;
+	void __user *cookie;
+	unsigned has_strong_ref : 1;
+	unsigned pending_strong_ref : 1;
+	unsigned has_weak_ref : 1;
+	unsigned pending_weak_ref : 1;
+	unsigned has_async_transaction : 1;
+	unsigned accept_fds : 1;
+	int min_priority : 8;
+	struct list_head async_todo;
+};
+
+struct binder_ref_death {
+	struct binder_work work;
+	void __user *cookie;
+};
+
+struct binder_ref {
+	/* Lookups needed: */
+	/*   node + proc => ref (transaction) */
+	/*   desc + proc => ref (transaction, inc/dec ref) */
+	/*   node => refs + procs (proc exit) */
+	int debug_id;
+	struct rb_node rb_node_desc;
+	struct rb_node rb_node_node;
+	struct hlist_node node_entry;
+	struct binder_proc *proc;
+	struct binder_node *node;
+	uint32_t desc;
+	int strong;
+	int weak;
+	struct binder_ref_death *death;
+};
+
+struct binder_buffer {
+	struct list_head entry; /* free and allocated entries by addesss */
+	struct rb_node rb_node; /* free entry by size or allocated entry */
+				/* by address */
+	unsigned free : 1;
+	unsigned allow_user_free : 1;
+	unsigned async_transaction : 1;
+	unsigned debug_id : 29;
+
+	struct binder_transaction *transaction;
+
+	struct binder_node *target_node;
+	size_t data_size;
+	size_t offsets_size;
+	uint8_t data[0];
+};
+
+struct binder_proc {
+	struct hlist_node proc_node;
+	struct rb_root threads;
+	struct rb_root nodes;
+	struct rb_root refs_by_desc;
+	struct rb_root refs_by_node;
+	int pid;
+	struct vm_area_struct *vma;
+	struct task_struct *tsk;
+	void *buffer;
+	size_t user_buffer_offset;
+
+	struct list_head buffers;
+	struct rb_root free_buffers;
+	struct rb_root allocated_buffers;
+	size_t free_async_space;
+
+	struct page **pages;
+	size_t buffer_size;
+	uint32_t buffer_free;
+	struct list_head todo;
+	wait_queue_head_t wait;
+	struct binder_stats stats;
+	struct list_head delivered_death;
+	int max_threads;
+	int requested_threads;
+	int requested_threads_started;
+	int ready_threads;
+	long default_priority;
+};
+
+enum {
+	BINDER_LOOPER_STATE_REGISTERED  = 0x01,
+	BINDER_LOOPER_STATE_ENTERED     = 0x02,
+	BINDER_LOOPER_STATE_EXITED      = 0x04,
+	BINDER_LOOPER_STATE_INVALID     = 0x08,
+	BINDER_LOOPER_STATE_WAITING     = 0x10,
+	BINDER_LOOPER_STATE_NEED_RETURN = 0x20
+};
+
+struct binder_thread {
+	struct binder_proc *proc;
+	struct rb_node rb_node;
+	int pid;
+	int looper;
+	struct binder_transaction *transaction_stack;
+	struct list_head todo;
+	uint32_t return_error; /* Write failed, return error code in read buf */
+	uint32_t return_error2; /* Write failed, return error code in read */
+		/* buffer. Used when sending a reply to a dead process that */
+		/* we are also waiting on */
+	wait_queue_head_t wait;
+	struct binder_stats stats;
+};
+
+struct binder_transaction {
+	int debug_id;
+	struct binder_work work;
+	struct binder_thread *from;
+	struct binder_transaction *from_parent;
+	struct binder_proc *to_proc;
+	struct binder_thread *to_thread;
+	struct binder_transaction *to_parent;
+	unsigned need_reply : 1;
+	/*unsigned is_dead : 1;*/ /* not used at the moment */
+
+	struct binder_buffer *buffer;
+	unsigned int	code;
+	unsigned int	flags;
+	long	priority;
+	long	saved_priority;
+	uid_t	sender_euid;
+};
+
+/*
+ * copied from get_unused_fd_flags
+ */
+int task_get_unused_fd_flags(struct task_struct *tsk, int flags)
+{
+	struct files_struct *files = get_files_struct(tsk);
+	int fd, error;
+	struct fdtable *fdt;
+	unsigned long rlim_cur;
+
+	if (files == NULL)
+		return -ESRCH;
+
+	error = -EMFILE;
+	spin_lock(&files->file_lock);
+
+repeat:
+	fdt = files_fdtable(files);
+	fd = find_next_zero_bit(fdt->open_fds->fds_bits, fdt->max_fds,
+				files->next_fd);
+
+	/*
+	 * N.B. For clone tasks sharing a files structure, this test
+	 * will limit the total number of files that can be opened.
+	 */
+	rcu_read_lock();
+	if (tsk->signal)
+		rlim_cur = tsk->signal->rlim[RLIMIT_NOFILE].rlim_cur;
+	else
+		rlim_cur = 0;
+	rcu_read_unlock();
+	if (fd >= rlim_cur)
+		goto out;
+
+	/* Do we need to expand the fd array or fd set?  */
+	error = expand_files(files, fd);
+	if (error < 0)
+		goto out;
+
+	if (error) {
+		/*
+		 * If we needed to expand the fs array we
+		 * might have blocked - try again.
+		 */
+		error = -EMFILE;
+		goto repeat;
+	}
+
+	FD_SET(fd, fdt->open_fds);
+	if (flags & O_CLOEXEC)
+		FD_SET(fd, fdt->close_on_exec);
+	else
+		FD_CLR(fd, fdt->close_on_exec);
+	files->next_fd = fd + 1;
+#if 1
+	/* Sanity check */
+	if (fdt->fd[fd] != NULL) {
+		printk(KERN_WARNING "get_unused_fd: slot %d not NULL!\n", fd);
+		fdt->fd[fd] = NULL;
+	}
+#endif
+	error = fd;
+
+out:
+	spin_unlock(&files->file_lock);
+	put_files_struct(files);
+	return error;
+}
+
+/*
+ * copied from fd_install
+ */
+static void task_fd_install(
+	struct task_struct *tsk, unsigned int fd, struct file *file)
+{
+	struct files_struct *files = get_files_struct(tsk);
+	struct fdtable *fdt;
+
+	if (files == NULL)
+		return;
+
+	spin_lock(&files->file_lock);
+	fdt = files_fdtable(files);
+	BUG_ON(fdt->fd[fd] != NULL);
+	rcu_assign_pointer(fdt->fd[fd], file);
+	spin_unlock(&files->file_lock);
+	put_files_struct(files);
+}
+
+/*
+ * copied from __put_unused_fd in open.c
+ */
+static void __put_unused_fd(struct files_struct *files, unsigned int fd)
+{
+	struct fdtable *fdt = files_fdtable(files);
+	__FD_CLR(fd, fdt->open_fds);
+	if (fd < files->next_fd)
+		files->next_fd = fd;
+}
+
+/*
+ * copied from sys_close
+ */
+static long task_close_fd(struct task_struct *tsk, unsigned int fd)
+{
+	struct file *filp;
+	struct files_struct *files = get_files_struct(tsk);
+	struct fdtable *fdt;
+	int retval;
+
+	if (files == NULL)
+		return -ESRCH;
+
+	spin_lock(&files->file_lock);
+	fdt = files_fdtable(files);
+	if (fd >= fdt->max_fds)
+		goto out_unlock;
+	filp = fdt->fd[fd];
+	if (!filp)
+		goto out_unlock;
+	rcu_assign_pointer(fdt->fd[fd], NULL);
+	FD_CLR(fd, fdt->close_on_exec);
+	__put_unused_fd(files, fd);
+	spin_unlock(&files->file_lock);
+	retval = filp_close(filp, files);
+
+	/* can't restart close syscall because file table entry was cleared */
+	if (unlikely(retval == -ERESTARTSYS ||
+		     retval == -ERESTARTNOINTR ||
+		     retval == -ERESTARTNOHAND ||
+		     retval == -ERESTART_RESTARTBLOCK))
+		retval = -EINTR;
+
+	put_files_struct(files);
+	return retval;
+
+out_unlock:
+	spin_unlock(&files->file_lock);
+	put_files_struct(files);
+	return -EBADF;
+}
+
+static void binder_set_nice(long nice)
+{
+	long min_nice;
+	if (can_nice(current, nice)) {
+		set_user_nice(current, nice);
+		return;
+	}
+	min_nice = 20 - current->signal->rlim[RLIMIT_NICE].rlim_cur;
+	if (binder_debug_mask & BINDER_DEBUG_PRIORITY_CAP)
+		printk(KERN_INFO "binder: %d: nice value %ld not allowed use "
+		       "%ld instead\n", current->pid, nice, min_nice);
+	set_user_nice(current, min_nice);
+	if (min_nice < 20)
+		return;
+	binder_user_error("binder: %d RLIMIT_NICE not set\n", current->pid);
+}
+
+static size_t binder_buffer_size(
+	struct binder_proc *proc, struct binder_buffer *buffer)
+{
+	if (list_is_last(&buffer->entry, &proc->buffers))
+		return proc->buffer + proc->buffer_size - (void *)buffer->data;
+	else
+		return (size_t)list_entry(buffer->entry.next,
+			struct binder_buffer, entry) - (size_t)buffer->data;
+}
+
+static void binder_insert_free_buffer(
+	struct binder_proc *proc, struct binder_buffer *new_buffer)
+{
+	struct rb_node **p = &proc->free_buffers.rb_node;
+	struct rb_node *parent = NULL;
+	struct binder_buffer *buffer;
+	size_t buffer_size;
+	size_t new_buffer_size;
+
+	BUG_ON(!new_buffer->free);
+
+	new_buffer_size = binder_buffer_size(proc, new_buffer);
+
+	if (binder_debug_mask & BINDER_DEBUG_BUFFER_ALLOC)
+		printk(KERN_INFO "binder: %d: add free buffer, size %zd, "
+		       "at %p\n", proc->pid, new_buffer_size, new_buffer);
+
+	while (*p) {
+		parent = *p;
+		buffer = rb_entry(parent, struct binder_buffer, rb_node);
+		BUG_ON(!buffer->free);
+
+		buffer_size = binder_buffer_size(proc, buffer);
+
+		if (new_buffer_size < buffer_size)
+			p = &parent->rb_left;
+		else
+			p = &parent->rb_right;
+	}
+	rb_link_node(&new_buffer->rb_node, parent, p);
+	rb_insert_color(&new_buffer->rb_node, &proc->free_buffers);
+}
+
+static void binder_insert_allocated_buffer(
+	struct binder_proc *proc, struct binder_buffer *new_buffer)
+{
+	struct rb_node **p = &proc->allocated_buffers.rb_node;
+	struct rb_node *parent = NULL;
+	struct binder_buffer *buffer;
+
+	BUG_ON(new_buffer->free);
+
+	while (*p) {
+		parent = *p;
+		buffer = rb_entry(parent, struct binder_buffer, rb_node);
+		BUG_ON(buffer->free);
+
+		if (new_buffer < buffer)
+			p = &parent->rb_left;
+		else if (new_buffer > buffer)
+			p = &parent->rb_right;
+		else
+			BUG();
+	}
+	rb_link_node(&new_buffer->rb_node, parent, p);
+	rb_insert_color(&new_buffer->rb_node, &proc->allocated_buffers);
+}
+
+static struct binder_buffer *binder_buffer_lookup(
+	struct binder_proc *proc, void __user *user_ptr)
+{
+	struct rb_node *n = proc->allocated_buffers.rb_node;
+	struct binder_buffer *buffer;
+	struct binder_buffer *kern_ptr;
+
+	kern_ptr = user_ptr - proc->user_buffer_offset
+		- offsetof(struct binder_buffer, data);
+
+	while (n) {
+		buffer = rb_entry(n, struct binder_buffer, rb_node);
+		BUG_ON(buffer->free);
+
+		if (kern_ptr < buffer)
+			n = n->rb_left;
+		else if (kern_ptr > buffer)
+			n = n->rb_right;
+		else
+			return buffer;
+	}
+	return NULL;
+}
+
+static int binder_update_page_range(struct binder_proc *proc, int allocate,
+	void *start, void *end, struct vm_area_struct *vma)
+{
+	void *page_addr;
+	unsigned long user_page_addr;
+	struct vm_struct tmp_area;
+	struct page **page;
+	struct mm_struct *mm;
+
+	if (binder_debug_mask & BINDER_DEBUG_BUFFER_ALLOC)
+		printk(KERN_INFO "binder: %d: %s pages %p-%p\n",
+		       proc->pid, allocate ? "allocate" : "free", start, end);
+
+	if (end <= start)
+		return 0;
+
+	if (vma)
+		mm = NULL;
+	else
+		mm = get_task_mm(proc->tsk);
+
+	if (mm) {
+		down_write(&mm->mmap_sem);
+		vma = proc->vma;
+	}
+
+	if (allocate == 0)
+		goto free_range;
+
+	if (vma == NULL) {
+		printk(KERN_ERR "binder: %d: binder_alloc_buf failed to "
+		       "map pages in userspace, no vma\n", proc->pid);
+		goto err_no_vma;
+	}
+
+	for (page_addr = start; page_addr < end; page_addr += PAGE_SIZE) {
+		int ret;
+		struct page **page_array_ptr;
+		page = &proc->pages[(page_addr - proc->buffer) / PAGE_SIZE];
+
+		BUG_ON(*page);
+		*page = alloc_page(GFP_KERNEL | __GFP_ZERO);
+		if (*page == NULL) {
+			printk(KERN_ERR "binder: %d: binder_alloc_buf failed "
+			       "for page at %p\n", proc->pid, page_addr);
+			goto err_alloc_page_failed;
+		}
+		tmp_area.addr = page_addr;
+		tmp_area.size = PAGE_SIZE + PAGE_SIZE /* guard page? */;
+		page_array_ptr = page;
+		ret = map_vm_area(&tmp_area, PAGE_KERNEL, &page_array_ptr);
+		if (ret) {
+			printk(KERN_ERR "binder: %d: binder_alloc_buf failed "
+			       "to map page at %p in kernel\n",
+			       proc->pid, page_addr);
+			goto err_map_kernel_failed;
+		}
+		user_page_addr = (size_t)page_addr + proc->user_buffer_offset;
+		ret = vm_insert_page(vma, user_page_addr, page[0]);
+		if (ret) {
+			printk(KERN_ERR "binder: %d: binder_alloc_buf failed "
+			       "to map page at %lx in userspace\n",
+			       proc->pid, user_page_addr);
+			goto err_vm_insert_page_failed;
+		}
+		/* vm_insert_page does not seem to increment the refcount */
+	}
+	if (mm) {
+		up_write(&mm->mmap_sem);
+		mmput(mm);
+	}
+	return 0;
+
+free_range:
+	for (page_addr = end - PAGE_SIZE; page_addr >= start;
+	     page_addr -= PAGE_SIZE) {
+		page = &proc->pages[(page_addr - proc->buffer) / PAGE_SIZE];
+		if (vma)
+			zap_page_range(vma, (size_t)page_addr +
+				proc->user_buffer_offset, PAGE_SIZE, NULL);
+err_vm_insert_page_failed:
+		unmap_kernel_range((unsigned long)page_addr, PAGE_SIZE);
+err_map_kernel_failed:
+		__free_page(*page);
+		*page = NULL;
+err_alloc_page_failed:
+		;
+	}
+err_no_vma:
+	if (mm) {
+		up_write(&mm->mmap_sem);
+		mmput(mm);
+	}
+	return -ENOMEM;
+}
+
+static struct binder_buffer *binder_alloc_buf(struct binder_proc *proc,
+	size_t data_size, size_t offsets_size, int is_async)
+{
+	struct rb_node *n = proc->free_buffers.rb_node;
+	struct binder_buffer *buffer;
+	size_t buffer_size;
+	struct rb_node *best_fit = NULL;
+	void *has_page_addr;
+	void *end_page_addr;
+	size_t size;
+
+	if (proc->vma == NULL) {
+		printk(KERN_ERR "binder: %d: binder_alloc_buf, no vma\n",
+		       proc->pid);
+		return NULL;
+	}
+
+	size = ALIGN(data_size, sizeof(void *)) +
+		ALIGN(offsets_size, sizeof(void *));
+
+	if (size < data_size || size < offsets_size) {
+		binder_user_error("binder: %d: got transaction with invalid "
+			"size %zd-%zd\n", proc->pid, data_size, offsets_size);
+		return NULL;
+	}
+
+	if (is_async &&
+	    proc->free_async_space < size + sizeof(struct binder_buffer)) {
+		if (binder_debug_mask & BINDER_DEBUG_BUFFER_ALLOC)
+			printk(KERN_ERR "binder: %d: binder_alloc_buf size %zd f"
+			       "ailed, no async space left\n", proc->pid, size);
+		return NULL;
+	}
+
+	while (n) {
+		buffer = rb_entry(n, struct binder_buffer, rb_node);
+		BUG_ON(!buffer->free);
+		buffer_size = binder_buffer_size(proc, buffer);
+
+		if (size < buffer_size) {
+			best_fit = n;
+			n = n->rb_left;
+		} else if (size > buffer_size)
+			n = n->rb_right;
+		else {
+			best_fit = n;
+			break;
+		}
+	}
+	if (best_fit == NULL) {
+		printk(KERN_ERR "binder: %d: binder_alloc_buf size %zd failed, "
+		       "no address space\n", proc->pid, size);
+		return NULL;
+	}
+	if (n == NULL) {
+		buffer = rb_entry(best_fit, struct binder_buffer, rb_node);
+		buffer_size = binder_buffer_size(proc, buffer);
+	}
+	if (binder_debug_mask & BINDER_DEBUG_BUFFER_ALLOC)
+		printk(KERN_INFO "binder: %d: binder_alloc_buf size %zd got buff"
+		       "er %p size %zd\n", proc->pid, size, buffer, buffer_size);
+
+	has_page_addr =
+		(void *)(((size_t)buffer->data + buffer_size) & PAGE_MASK);
+	if (n == NULL) {
+		if (size + sizeof(struct binder_buffer) + 4 >= buffer_size)
+			buffer_size = size; /* no room for other buffers */
+		else
+			buffer_size = size + sizeof(struct binder_buffer);
+	}
+	end_page_addr = (void *)PAGE_ALIGN((size_t)buffer->data + buffer_size);
+	if (end_page_addr > has_page_addr)
+		end_page_addr = has_page_addr;
+	if (binder_update_page_range(proc, 1,
+	    (void *)PAGE_ALIGN((size_t)buffer->data), end_page_addr, NULL))
+		return NULL;
+
+	rb_erase(best_fit, &proc->free_buffers);
+	buffer->free = 0;
+	binder_insert_allocated_buffer(proc, buffer);
+	if (buffer_size != size) {
+		struct binder_buffer *new_buffer = (void *)buffer->data + size;
+		list_add(&new_buffer->entry, &buffer->entry);
+		new_buffer->free = 1;
+		binder_insert_free_buffer(proc, new_buffer);
+	}
+	if (binder_debug_mask & BINDER_DEBUG_BUFFER_ALLOC)
+		printk(KERN_INFO "binder: %d: binder_alloc_buf size %zd got "
+		       "%p\n", proc->pid, size, buffer);
+	buffer->data_size = data_size;
+	buffer->offsets_size = offsets_size;
+	buffer->async_transaction = is_async;
+	if (is_async) {
+		proc->free_async_space -= size + sizeof(struct binder_buffer);
+		if (binder_debug_mask & BINDER_DEBUG_BUFFER_ALLOC_ASYNC)
+			printk(KERN_INFO "binder: %d: binder_alloc_buf size %zd "
+			       "async free %zd\n", proc->pid, size,
+			       proc->free_async_space);
+	}
+
+	return buffer;
+}
+
+static void *buffer_start_page(struct binder_buffer *buffer)
+{
+	return (void *)((size_t)buffer & PAGE_MASK);
+}
+
+static void *buffer_end_page(struct binder_buffer *buffer)
+{
+	return (void *)(((size_t)(buffer + 1) - 1) & PAGE_MASK);
+}
+
+static void binder_delete_free_buffer(
+	struct binder_proc *proc, struct binder_buffer *buffer)
+{
+	struct binder_buffer *prev, *next = NULL;
+	int free_page_end = 1;
+	int free_page_start = 1;
+
+	BUG_ON(proc->buffers.next == &buffer->entry);
+	prev = list_entry(buffer->entry.prev, struct binder_buffer, entry);
+	BUG_ON(!prev->free);
+	if (buffer_end_page(prev) == buffer_start_page(buffer)) {
+		free_page_start = 0;
+		if (buffer_end_page(prev) == buffer_end_page(buffer))
+			free_page_end = 0;
+		if (binder_debug_mask & BINDER_DEBUG_BUFFER_ALLOC)
+			printk(KERN_INFO "binder: %d: merge free, buffer %p "
+			       "share page with %p\n", proc->pid, buffer, prev);
+	}
+
+	if (!list_is_last(&buffer->entry, &proc->buffers)) {
+		next = list_entry(buffer->entry.next,
+				  struct binder_buffer, entry);
+		if (buffer_start_page(next) == buffer_end_page(buffer)) {
+			free_page_end = 0;
+			if (buffer_start_page(next) ==
+			    buffer_start_page(buffer))
+				free_page_start = 0;
+			if (binder_debug_mask & BINDER_DEBUG_BUFFER_ALLOC)
+				printk(KERN_INFO "binder: %d: merge free, "
+				       "buffer %p share page with %p\n",
+				       proc->pid, buffer, prev);
+		}
+	}
+	list_del(&buffer->entry);
+	if (free_page_start || free_page_end) {
+		if (binder_debug_mask & BINDER_DEBUG_BUFFER_ALLOC)
+			printk(KERN_INFO "binder: %d: merge free, buffer %p do "
+			       "not share page%s%s with with %p or %p\n",
+			       proc->pid, buffer, free_page_start ? "" : " end",
+			       free_page_end ? "" : " start", prev, next);
+		binder_update_page_range(proc, 0, free_page_start ?
+			buffer_start_page(buffer) : buffer_end_page(buffer),
+			(free_page_end ? buffer_end_page(buffer) :
+			buffer_start_page(buffer)) + PAGE_SIZE, NULL);
+	}
+}
+
+static void binder_free_buf(
+	struct binder_proc *proc, struct binder_buffer *buffer)
+{
+	size_t size, buffer_size;
+
+	buffer_size = binder_buffer_size(proc, buffer);
+
+	size = ALIGN(buffer->data_size, sizeof(void *)) +
+		ALIGN(buffer->offsets_size, sizeof(void *));
+	if (binder_debug_mask & BINDER_DEBUG_BUFFER_ALLOC)
+		printk(KERN_INFO "binder: %d: binder_free_buf %p size %zd buffer"
+		       "_size %zd\n", proc->pid, buffer, size, buffer_size);
+
+	BUG_ON(buffer->free);
+	BUG_ON(size > buffer_size);
+	BUG_ON(buffer->transaction != NULL);
+	BUG_ON((void *)buffer < proc->buffer);
+	BUG_ON((void *)buffer > proc->buffer + proc->buffer_size);
+
+	if (buffer->async_transaction) {
+		proc->free_async_space += size + sizeof(struct binder_buffer);
+		if (binder_debug_mask & BINDER_DEBUG_BUFFER_ALLOC_ASYNC)
+			printk(KERN_INFO "binder: %d: binder_free_buf size %zd "
+			       "async free %zd\n", proc->pid, size,
+			       proc->free_async_space);
+	}
+
+	binder_update_page_range(proc, 0,
+		(void *)PAGE_ALIGN((size_t)buffer->data),
+		(void *)(((size_t)buffer->data + buffer_size) & PAGE_MASK),
+		NULL);
+	rb_erase(&buffer->rb_node, &proc->allocated_buffers);
+	buffer->free = 1;
+	if (!list_is_last(&buffer->entry, &proc->buffers)) {
+		struct binder_buffer *next = list_entry(buffer->entry.next,
+						struct binder_buffer, entry);
+		if (next->free) {
+			rb_erase(&next->rb_node, &proc->free_buffers);
+			binder_delete_free_buffer(proc, next);
+		}
+	}
+	if (proc->buffers.next != &buffer->entry) {
+		struct binder_buffer *prev = list_entry(buffer->entry.prev,
+						struct binder_buffer, entry);
+		if (prev->free) {
+			binder_delete_free_buffer(proc, buffer);
+			rb_erase(&prev->rb_node, &proc->free_buffers);
+			buffer = prev;
+		}
+	}
+	binder_insert_free_buffer(proc, buffer);
+}
+
+static struct binder_node *
+binder_get_node(struct binder_proc *proc, void __user *ptr)
+{
+	struct rb_node *n = proc->nodes.rb_node;
+	struct binder_node *node;
+
+	while (n) {
+		node = rb_entry(n, struct binder_node, rb_node);
+
+		if (ptr < node->ptr)
+			n = n->rb_left;
+		else if (ptr > node->ptr)
+			n = n->rb_right;
+		else
+			return node;
+	}
+	return NULL;
+}
+
+static struct binder_node *
+binder_new_node(struct binder_proc *proc, void __user *ptr, void __user *cookie)
+{
+	struct rb_node **p = &proc->nodes.rb_node;
+	struct rb_node *parent = NULL;
+	struct binder_node *node;
+
+	while (*p) {
+		parent = *p;
+		node = rb_entry(parent, struct binder_node, rb_node);
+
+		if (ptr < node->ptr)
+			p = &(*p)->rb_left;
+		else if (ptr > node->ptr)
+			p = &(*p)->rb_right;
+		else
+			return NULL;
+	}
+
+	node = kzalloc(sizeof(*node), GFP_KERNEL);
+	if (node == NULL)
+		return NULL;
+	binder_stats.obj_created[BINDER_STAT_NODE]++;
+	rb_link_node(&node->rb_node, parent, p);
+	rb_insert_color(&node->rb_node, &proc->nodes);
+	node->debug_id = ++binder_last_id;
+	node->proc = proc;
+	node->ptr = ptr;
+	node->cookie = cookie;
+	node->work.type = BINDER_WORK_NODE;
+	INIT_LIST_HEAD(&node->work.entry);
+	INIT_LIST_HEAD(&node->async_todo);
+	if (binder_debug_mask & BINDER_DEBUG_INTERNAL_REFS)
+		printk(KERN_INFO "binder: %d:%d node %d u%p c%p created\n",
+		       proc->pid, current->pid, node->debug_id,
+		       node->ptr, node->cookie);
+	return node;
+}
+
+static int
+binder_inc_node(struct binder_node *node, int strong, int internal,
+		struct list_head *target_list)
+{
+	if (strong) {
+		if (internal) {
+			if (target_list == NULL &&
+			    node->internal_strong_refs == 0 &&
+			    !(node == binder_context_mgr_node &&
+			    node->has_strong_ref)) {
+				printk(KERN_ERR "binder: invalid inc strong "
+					"node for %d\n", node->debug_id);
+				return -EINVAL;
+			}
+			node->internal_strong_refs++;
+		} else
+			node->local_strong_refs++;
+		if (!node->has_strong_ref && target_list) {
+			list_del_init(&node->work.entry);
+			list_add_tail(&node->work.entry, target_list);
+		}
+	} else {
+		if (!internal)
+			node->local_weak_refs++;
+		if (!node->has_weak_ref && list_empty(&node->work.entry)) {
+			if (target_list == NULL) {
+				printk(KERN_ERR "binder: invalid inc weak node "
+					"for %d\n", node->debug_id);
+				return -EINVAL;
+			}
+			list_add_tail(&node->work.entry, target_list);
+		}
+	}
+	return 0;
+}
+
+static int
+binder_dec_node(struct binder_node *node, int strong, int internal)
+{
+	if (strong) {
+		if (internal)
+			node->internal_strong_refs--;
+		else
+			node->local_strong_refs--;
+		if (node->local_strong_refs || node->internal_strong_refs)
+			return 0;
+	} else {
+		if (!internal)
+			node->local_weak_refs--;
+		if (node->local_weak_refs || !hlist_empty(&node->refs))
+			return 0;
+	}
+	if (node->proc && (node->has_strong_ref || node->has_weak_ref)) {
+		if (list_empty(&node->work.entry)) {
+			list_add_tail(&node->work.entry, &node->proc->todo);
+			wake_up_interruptible(&node->proc->wait);
+		}
+	} else {
+		if (hlist_empty(&node->refs) && !node->local_strong_refs &&
+		    !node->local_weak_refs) {
+			list_del_init(&node->work.entry);
+			if (node->proc) {
+				rb_erase(&node->rb_node, &node->proc->nodes);
+				if (binder_debug_mask & BINDER_DEBUG_INTERNAL_REFS)
+					printk(KERN_INFO "binder: refless node %d deleted\n", node->debug_id);
+			} else {
+				hlist_del(&node->dead_node);
+				if (binder_debug_mask & BINDER_DEBUG_INTERNAL_REFS)
+					printk(KERN_INFO "binder: dead node %d deleted\n", node->debug_id);
+			}
+			kfree(node);
+			binder_stats.obj_deleted[BINDER_STAT_NODE]++;
+		}
+	}
+
+	return 0;
+}
+
+
+static struct binder_ref *
+binder_get_ref(struct binder_proc *proc, uint32_t desc)
+{
+	struct rb_node *n = proc->refs_by_desc.rb_node;
+	struct binder_ref *ref;
+
+	while (n) {
+		ref = rb_entry(n, struct binder_ref, rb_node_desc);
+
+		if (desc < ref->desc)
+			n = n->rb_left;
+		else if (desc > ref->desc)
+			n = n->rb_right;
+		else
+			return ref;
+	}
+	return NULL;
+}
+
+static struct binder_ref *
+binder_get_ref_for_node(struct binder_proc *proc, struct binder_node *node)
+{
+	struct rb_node *n;
+	struct rb_node **p = &proc->refs_by_node.rb_node;
+	struct rb_node *parent = NULL;
+	struct binder_ref *ref, *new_ref;
+
+	while (*p) {
+		parent = *p;
+		ref = rb_entry(parent, struct binder_ref, rb_node_node);
+
+		if (node < ref->node)
+			p = &(*p)->rb_left;
+		else if (node > ref->node)
+			p = &(*p)->rb_right;
+		else
+			return ref;
+	}
+	new_ref = kzalloc(sizeof(*ref), GFP_KERNEL);
+	if (new_ref == NULL)
+		return NULL;
+	binder_stats.obj_created[BINDER_STAT_REF]++;
+	new_ref->debug_id = ++binder_last_id;
+	new_ref->proc = proc;
+	new_ref->node = node;
+	rb_link_node(&new_ref->rb_node_node, parent, p);
+	rb_insert_color(&new_ref->rb_node_node, &proc->refs_by_node);
+
+	new_ref->desc = (node == binder_context_mgr_node) ? 0 : 1;
+	for (n = rb_first(&proc->refs_by_desc); n != NULL; n = rb_next(n)) {
+		ref = rb_entry(n, struct binder_ref, rb_node_desc);
+		if (ref->desc > new_ref->desc)
+			break;
+		new_ref->desc = ref->desc + 1;
+	}
+
+	p = &proc->refs_by_desc.rb_node;
+	while (*p) {
+		parent = *p;
+		ref = rb_entry(parent, struct binder_ref, rb_node_desc);
+
+		if (new_ref->desc < ref->desc)
+			p = &(*p)->rb_left;
+		else if (new_ref->desc > ref->desc)
+			p = &(*p)->rb_right;
+		else
+			BUG();
+	}
+	rb_link_node(&new_ref->rb_node_desc, parent, p);
+	rb_insert_color(&new_ref->rb_node_desc, &proc->refs_by_desc);
+	if (node) {
+		hlist_add_head(&new_ref->node_entry, &node->refs);
+		if (binder_debug_mask & BINDER_DEBUG_INTERNAL_REFS)
+			printk(KERN_INFO "binder: %d new ref %d desc %d for "
+				"node %d\n", proc->pid, new_ref->debug_id,
+				new_ref->desc, node->debug_id);
+	} else {
+		if (binder_debug_mask & BINDER_DEBUG_INTERNAL_REFS)
+			printk(KERN_INFO "binder: %d new ref %d desc %d for "
+				"dead node\n", proc->pid, new_ref->debug_id,
+				new_ref->desc);
+	}
+	return new_ref;
+}
+
+static void
+binder_delete_ref(struct binder_ref *ref)
+{
+	if (binder_debug_mask & BINDER_DEBUG_INTERNAL_REFS)
+		printk(KERN_INFO "binder: %d delete ref %d desc %d for "
+			"node %d\n", ref->proc->pid, ref->debug_id,
+			ref->desc, ref->node->debug_id);
+	rb_erase(&ref->rb_node_desc, &ref->proc->refs_by_desc);
+	rb_erase(&ref->rb_node_node, &ref->proc->refs_by_node);
+	if (ref->strong)
+		binder_dec_node(ref->node, 1, 1);
+	hlist_del(&ref->node_entry);
+	binder_dec_node(ref->node, 0, 1);
+	if (ref->death) {
+		if (binder_debug_mask & BINDER_DEBUG_DEAD_BINDER)
+			printk(KERN_INFO "binder: %d delete ref %d desc %d "
+				"has death notification\n", ref->proc->pid,
+				ref->debug_id, ref->desc);
+		list_del(&ref->death->work.entry);
+		kfree(ref->death);
+		binder_stats.obj_deleted[BINDER_STAT_DEATH]++;
+	}
+	kfree(ref);
+	binder_stats.obj_deleted[BINDER_STAT_REF]++;
+}
+
+static int
+binder_inc_ref(
+	struct binder_ref *ref, int strong, struct list_head *target_list)
+{
+	int ret;
+	if (strong) {
+		if (ref->strong == 0) {
+			ret = binder_inc_node(ref->node, 1, 1, target_list);
+			if (ret)
+				return ret;
+		}
+		ref->strong++;
+	} else {
+		if (ref->weak == 0) {
+			ret = binder_inc_node(ref->node, 0, 1, target_list);
+			if (ret)
+				return ret;
+		}
+		ref->weak++;
+	}
+	return 0;
+}
+
+
+static int
+binder_dec_ref(struct binder_ref *ref, int strong)
+{
+	if (strong) {
+		if (ref->strong == 0) {
+			binder_user_error("binder: %d invalid dec strong, "
+					  "ref %d desc %d s %d w %d\n",
+					  ref->proc->pid, ref->debug_id,
+					  ref->desc, ref->strong, ref->weak);
+			return -EINVAL;
+		}
+		ref->strong--;
+		if (ref->strong == 0) {
+			int ret;
+			ret = binder_dec_node(ref->node, strong, 1);
+			if (ret)
+				return ret;
+		}
+	} else {
+		if (ref->weak == 0) {
+			binder_user_error("binder: %d invalid dec weak, "
+					  "ref %d desc %d s %d w %d\n",
+					  ref->proc->pid, ref->debug_id,
+					  ref->desc, ref->strong, ref->weak);
+			return -EINVAL;
+		}
+		ref->weak--;
+	}
+	if (ref->strong == 0 && ref->weak == 0)
+		binder_delete_ref(ref);
+	return 0;
+}
+
+static void
+binder_pop_transaction(
+	struct binder_thread *target_thread, struct binder_transaction *t)
+{
+	if (target_thread) {
+		BUG_ON(target_thread->transaction_stack != t);
+		BUG_ON(target_thread->transaction_stack->from != target_thread);
+		target_thread->transaction_stack =
+			target_thread->transaction_stack->from_parent;
+		t->from = NULL;
+	}
+	t->need_reply = 0;
+	if (t->buffer)
+		t->buffer->transaction = NULL;
+	kfree(t);
+	binder_stats.obj_deleted[BINDER_STAT_TRANSACTION]++;
+}
+
+static void
+binder_send_failed_reply(struct binder_transaction *t, uint32_t error_code)
+{
+	struct binder_thread *target_thread;
+	BUG_ON(t->flags & TF_ONE_WAY);
+	while (1) {
+		target_thread = t->from;
+		if (target_thread) {
+			if (target_thread->return_error != BR_OK &&
+			   target_thread->return_error2 == BR_OK) {
+				target_thread->return_error2 =
+					target_thread->return_error;
+				target_thread->return_error = BR_OK;
+			}
+			if (target_thread->return_error == BR_OK) {
+				if (binder_debug_mask & BINDER_DEBUG_FAILED_TRANSACTION)
+					printk(KERN_INFO "binder: send failed reply for transaction %d to %d:%d\n",
+					       t->debug_id, target_thread->proc->pid, target_thread->pid);
+
+				binder_pop_transaction(target_thread, t);
+				target_thread->return_error = error_code;
+				wake_up_interruptible(&target_thread->wait);
+			} else {
+				printk(KERN_ERR "binder: reply failed, target "
+					"thread, %d:%d, has error code %d "
+					"already\n", target_thread->proc->pid,
+					target_thread->pid,
+					target_thread->return_error);
+			}
+			return;
+		} else {
+			struct binder_transaction *next = t->from_parent;
+
+			if (binder_debug_mask & BINDER_DEBUG_FAILED_TRANSACTION)
+				printk(KERN_INFO "binder: send failed reply "
+					"for transaction %d, target dead\n",
+					t->debug_id);
+
+			binder_pop_transaction(target_thread, t);
+			if (next == NULL) {
+				if (binder_debug_mask & BINDER_DEBUG_DEAD_BINDER)
+					printk(KERN_INFO "binder: reply failed,"
+						" no target thread at root\n");
+				return;
+			}
+			t = next;
+			if (binder_debug_mask & BINDER_DEBUG_DEAD_BINDER)
+				printk(KERN_INFO "binder: reply failed, no targ"
+					"et thread -- retry %d\n", t->debug_id);
+		}
+	}
+}
+
+static void
+binder_transaction_buffer_release(struct binder_proc *proc,
+			struct binder_buffer *buffer, size_t *failed_at);
+
+static void
+binder_transaction(struct binder_proc *proc, struct binder_thread *thread,
+	struct binder_transaction_data *tr, int reply)
+{
+	struct binder_transaction *t;
+	struct binder_work *tcomplete;
+	size_t *offp, *off_end;
+	struct binder_proc *target_proc;
+	struct binder_thread *target_thread = NULL;
+	struct binder_node *target_node = NULL;
+	struct list_head *target_list;
+	wait_queue_head_t *target_wait;
+	struct binder_transaction *in_reply_to = NULL;
+	struct binder_transaction_log_entry *e;
+	uint32_t return_error;
+
+	e = binder_transaction_log_add(&binder_transaction_log);
+	e->call_type = reply ? 2 : !!(tr->flags & TF_ONE_WAY);
+	e->from_proc = proc->pid;
+	e->from_thread = thread->pid;
+	e->target_handle = tr->target.handle;
+	e->data_size = tr->data_size;
+	e->offsets_size = tr->offsets_size;
+
+	if (reply) {
+		in_reply_to = thread->transaction_stack;
+		if (in_reply_to == NULL) {
+			binder_user_error("binder: %d:%d got reply transaction "
+					  "with no transaction stack\n",
+					  proc->pid, thread->pid);
+			return_error = BR_FAILED_REPLY;
+			goto err_empty_call_stack;
+		}
+		binder_set_nice(in_reply_to->saved_priority);
+		if (in_reply_to->to_thread != thread) {
+			binder_user_error("binder: %d:%d got reply transaction "
+				"with bad transaction stack,"
+				" transaction %d has target %d:%d\n",
+				proc->pid, thread->pid, in_reply_to->debug_id,
+				in_reply_to->to_proc ?
+				in_reply_to->to_proc->pid : 0,
+				in_reply_to->to_thread ?
+				in_reply_to->to_thread->pid : 0);
+			return_error = BR_FAILED_REPLY;
+			in_reply_to = NULL;
+			goto err_bad_call_stack;
+		}
+		thread->transaction_stack = in_reply_to->to_parent;
+		target_thread = in_reply_to->from;
+		if (target_thread == NULL) {
+			return_error = BR_DEAD_REPLY;
+			goto err_dead_binder;
+		}
+		if (target_thread->transaction_stack != in_reply_to) {
+			binder_user_error("binder: %d:%d got reply transaction "
+				"with bad target transaction stack %d, "
+				"expected %d\n",
+				proc->pid, thread->pid,
+				target_thread->transaction_stack ?
+				target_thread->transaction_stack->debug_id : 0,
+				in_reply_to->debug_id);
+			return_error = BR_FAILED_REPLY;
+			in_reply_to = NULL;
+			target_thread = NULL;
+			goto err_dead_binder;
+		}
+		target_proc = target_thread->proc;
+	} else {
+		if (tr->target.handle) {
+			struct binder_ref *ref;
+			ref = binder_get_ref(proc, tr->target.handle);
+			if (ref == NULL) {
+				binder_user_error("binder: %d:%d got "
+					"transaction to invalid handle\n",
+					proc->pid, thread->pid);
+				return_error = BR_FAILED_REPLY;
+				goto err_invalid_target_handle;
+			}
+			target_node = ref->node;
+		} else {
+			target_node = binder_context_mgr_node;
+			if (target_node == NULL) {
+				return_error = BR_DEAD_REPLY;
+				goto err_no_context_mgr_node;
+			}
+		}
+		e->to_node = target_node->debug_id;
+		target_proc = target_node->proc;
+		if (target_proc == NULL) {
+			return_error = BR_DEAD_REPLY;
+			goto err_dead_binder;
+		}
+		if (!(tr->flags & TF_ONE_WAY) && thread->transaction_stack) {
+			struct binder_transaction *tmp;
+			tmp = thread->transaction_stack;
+			while (tmp) {
+				if (tmp->from && tmp->from->proc == target_proc)
+					target_thread = tmp->from;
+				tmp = tmp->from_parent;
+			}
+		}
+	}
+	if (target_thread) {
+		e->to_thread = target_thread->pid;
+		target_list = &target_thread->todo;
+		target_wait = &target_thread->wait;
+	} else {
+		target_list = &target_proc->todo;
+		target_wait = &target_proc->wait;
+	}
+	e->to_proc = target_proc->pid;
+
+	/* TODO: reuse incoming transaction for reply */
+	t = kzalloc(sizeof(*t), GFP_KERNEL);
+	if (t == NULL) {
+		return_error = BR_FAILED_REPLY;
+		goto err_alloc_t_failed;
+	}
+	binder_stats.obj_created[BINDER_STAT_TRANSACTION]++;
+
+	tcomplete = kzalloc(sizeof(*tcomplete), GFP_KERNEL);
+	if (tcomplete == NULL) {
+		return_error = BR_FAILED_REPLY;
+		goto err_alloc_tcomplete_failed;
+	}
+	binder_stats.obj_created[BINDER_STAT_TRANSACTION_COMPLETE]++;
+
+	t->debug_id = ++binder_last_id;
+	e->debug_id = t->debug_id;
+
+	if (binder_debug_mask & BINDER_DEBUG_TRANSACTION) {
+		if (reply)
+			printk(KERN_INFO "binder: %d:%d BC_REPLY %d -> %d:%d, "
+			       "data %p-%p size %zd-%zd\n",
+			       proc->pid, thread->pid, t->debug_id,
+			       target_proc->pid, target_thread->pid,
+			       tr->data.ptr.buffer, tr->data.ptr.offsets,
+			       tr->data_size, tr->offsets_size);
+		else
+			printk(KERN_INFO "binder: %d:%d BC_TRANSACTION %d -> "
+			       "%d - node %d, data %p-%p size %zd-%zd\n",
+			       proc->pid, thread->pid, t->debug_id,
+			       target_proc->pid, target_node->debug_id,
+			       tr->data.ptr.buffer, tr->data.ptr.offsets,
+			       tr->data_size, tr->offsets_size);
+	}
+
+	if (!reply && !(tr->flags & TF_ONE_WAY))
+		t->from = thread;
+	else
+		t->from = NULL;
+	t->sender_euid = proc->tsk->cred->euid;
+	t->to_proc = target_proc;
+	t->to_thread = target_thread;
+	t->code = tr->code;
+	t->flags = tr->flags;
+	t->priority = task_nice(current);
+	t->buffer = binder_alloc_buf(target_proc, tr->data_size,
+		tr->offsets_size, !reply && (t->flags & TF_ONE_WAY));
+	if (t->buffer == NULL) {
+		return_error = BR_FAILED_REPLY;
+		goto err_binder_alloc_buf_failed;
+	}
+	t->buffer->allow_user_free = 0;
+	t->buffer->debug_id = t->debug_id;
+	t->buffer->transaction = t;
+	t->buffer->target_node = target_node;
+	if (target_node)
+		binder_inc_node(target_node, 1, 0, NULL);
+
+	offp = (size_t *)(t->buffer->data + ALIGN(tr->data_size, sizeof(void *)));
+
+	if (copy_from_user(t->buffer->data, tr->data.ptr.buffer, tr->data_size)) {
+		binder_user_error("binder: %d:%d got transaction with invalid "
+			"data ptr\n", proc->pid, thread->pid);
+		return_error = BR_FAILED_REPLY;
+		goto err_copy_data_failed;
+	}
+	if (copy_from_user(offp, tr->data.ptr.offsets, tr->offsets_size)) {
+		binder_user_error("binder: %d:%d got transaction with invalid "
+			"offsets ptr\n", proc->pid, thread->pid);
+		return_error = BR_FAILED_REPLY;
+		goto err_copy_data_failed;
+	}
+	off_end = (void *)offp + tr->offsets_size;
+	for (; offp < off_end; offp++) {
+		struct flat_binder_object *fp;
+		if (*offp > t->buffer->data_size - sizeof(*fp)) {
+			binder_user_error("binder: %d:%d got transaction with "
+				"invalid offset, %zd\n",
+				proc->pid, thread->pid, *offp);
+			return_error = BR_FAILED_REPLY;
+			goto err_bad_offset;
+		}
+		fp = (struct flat_binder_object *)(t->buffer->data + *offp);
+		switch (fp->type) {
+		case BINDER_TYPE_BINDER:
+		case BINDER_TYPE_WEAK_BINDER: {
+			struct binder_ref *ref;
+			struct binder_node *node = binder_get_node(proc, fp->binder);
+			if (node == NULL) {
+				node = binder_new_node(proc, fp->binder, fp->cookie);
+				if (node == NULL) {
+					return_error = BR_FAILED_REPLY;
+					goto err_binder_new_node_failed;
+				}
+				node->min_priority = fp->flags & FLAT_BINDER_FLAG_PRIORITY_MASK;
+				node->accept_fds = !!(fp->flags & FLAT_BINDER_FLAG_ACCEPTS_FDS);
+			}
+			if (fp->cookie != node->cookie) {
+				binder_user_error("binder: %d:%d sending u%p "
+					"node %d, cookie mismatch %p != %p\n",
+					proc->pid, thread->pid,
+					fp->binder, node->debug_id,
+					fp->cookie, node->cookie);
+				goto err_binder_get_ref_for_node_failed;
+			}
+			ref = binder_get_ref_for_node(target_proc, node);
+			if (ref == NULL) {
+				return_error = BR_FAILED_REPLY;
+				goto err_binder_get_ref_for_node_failed;
+			}
+			if (fp->type == BINDER_TYPE_BINDER)
+				fp->type = BINDER_TYPE_HANDLE;
+			else
+				fp->type = BINDER_TYPE_WEAK_HANDLE;
+			fp->handle = ref->desc;
+			binder_inc_ref(ref, fp->type == BINDER_TYPE_HANDLE, &thread->todo);
+			if (binder_debug_mask & BINDER_DEBUG_TRANSACTION)
+				printk(KERN_INFO "        node %d u%p -> ref %d desc %d\n",
+				       node->debug_id, node->ptr, ref->debug_id, ref->desc);
+		} break;
+		case BINDER_TYPE_HANDLE:
+		case BINDER_TYPE_WEAK_HANDLE: {
+			struct binder_ref *ref = binder_get_ref(proc, fp->handle);
+			if (ref == NULL) {
+				binder_user_error("binder: %d:%d got "
+					"transaction with invalid "
+					"handle, %ld\n", proc->pid,
+					thread->pid, fp->handle);
+				return_error = BR_FAILED_REPLY;
+				goto err_binder_get_ref_failed;
+			}
+			if (ref->node->proc == target_proc) {
+				if (fp->type == BINDER_TYPE_HANDLE)
+					fp->type = BINDER_TYPE_BINDER;
+				else
+					fp->type = BINDER_TYPE_WEAK_BINDER;
+				fp->binder = ref->node->ptr;
+				fp->cookie = ref->node->cookie;
+				binder_inc_node(ref->node, fp->type == BINDER_TYPE_BINDER, 0, NULL);
+				if (binder_debug_mask & BINDER_DEBUG_TRANSACTION)
+					printk(KERN_INFO "        ref %d desc %d -> node %d u%p\n",
+					       ref->debug_id, ref->desc, ref->node->debug_id, ref->node->ptr);
+			} else {
+				struct binder_ref *new_ref;
+				new_ref = binder_get_ref_for_node(target_proc, ref->node);
+				if (new_ref == NULL) {
+					return_error = BR_FAILED_REPLY;
+					goto err_binder_get_ref_for_node_failed;
+				}
+				fp->handle = new_ref->desc;
+				binder_inc_ref(new_ref, fp->type == BINDER_TYPE_HANDLE, NULL);
+				if (binder_debug_mask & BINDER_DEBUG_TRANSACTION)
+					printk(KERN_INFO "        ref %d desc %d -> ref %d desc %d (node %d)\n",
+					       ref->debug_id, ref->desc, new_ref->debug_id, new_ref->desc, ref->node->debug_id);
+			}
+		} break;
+
+		case BINDER_TYPE_FD: {
+			int target_fd;
+			struct file *file;
+
+			if (reply) {
+				if (!(in_reply_to->flags & TF_ACCEPT_FDS)) {
+					binder_user_error("binder: %d:%d got reply with fd, %ld, but target does not allow fds\n",
+						proc->pid, thread->pid, fp->handle);
+					return_error = BR_FAILED_REPLY;
+					goto err_fd_not_allowed;
+				}
+			} else if (!target_node->accept_fds) {
+				binder_user_error("binder: %d:%d got transaction with fd, %ld, but target does not allow fds\n",
+					proc->pid, thread->pid, fp->handle);
+				return_error = BR_FAILED_REPLY;
+				goto err_fd_not_allowed;
+			}
+
+			file = fget(fp->handle);
+			if (file == NULL) {
+				binder_user_error("binder: %d:%d got transaction with invalid fd, %ld\n",
+					proc->pid, thread->pid, fp->handle);
+				return_error = BR_FAILED_REPLY;
+				goto err_fget_failed;
+			}
+			target_fd = task_get_unused_fd_flags(target_proc->tsk, O_CLOEXEC);
+			if (target_fd < 0) {
+				fput(file);
+				return_error = BR_FAILED_REPLY;
+				goto err_get_unused_fd_failed;
+			}
+			task_fd_install(target_proc->tsk, target_fd, file);
+			if (binder_debug_mask & BINDER_DEBUG_TRANSACTION)
+				printk(KERN_INFO "        fd %ld -> %d\n", fp->handle, target_fd);
+			/* TODO: fput? */
+			fp->handle = target_fd;
+		} break;
+
+		default:
+			binder_user_error("binder: %d:%d got transactio"
+				"n with invalid object type, %lx\n",
+				proc->pid, thread->pid, fp->type);
+			return_error = BR_FAILED_REPLY;
+			goto err_bad_object_type;
+		}
+	}
+	if (reply) {
+		BUG_ON(t->buffer->async_transaction != 0);
+		binder_pop_transaction(target_thread, in_reply_to);
+	} else if (!(t->flags & TF_ONE_WAY)) {
+		BUG_ON(t->buffer->async_transaction != 0);
+		t->need_reply = 1;
+		t->from_parent = thread->transaction_stack;
+		thread->transaction_stack = t;
+	} else {
+		BUG_ON(target_node == NULL);
+		BUG_ON(t->buffer->async_transaction != 1);
+		if (target_node->has_async_transaction) {
+			target_list = &target_node->async_todo;
+			target_wait = NULL;
+		} else
+			target_node->has_async_transaction = 1;
+	}
+	t->work.type = BINDER_WORK_TRANSACTION;
+	list_add_tail(&t->work.entry, target_list);
+	tcomplete->type = BINDER_WORK_TRANSACTION_COMPLETE;
+	list_add_tail(&tcomplete->entry, &thread->todo);
+	if (target_wait)
+		wake_up_interruptible(target_wait);
+	return;
+
+err_get_unused_fd_failed:
+err_fget_failed:
+err_fd_not_allowed:
+err_binder_get_ref_for_node_failed:
+err_binder_get_ref_failed:
+err_binder_new_node_failed:
+err_bad_object_type:
+err_bad_offset:
+err_copy_data_failed:
+	binder_transaction_buffer_release(target_proc, t->buffer, offp);
+	t->buffer->transaction = NULL;
+	binder_free_buf(target_proc, t->buffer);
+err_binder_alloc_buf_failed:
+	kfree(tcomplete);
+	binder_stats.obj_deleted[BINDER_STAT_TRANSACTION_COMPLETE]++;
+err_alloc_tcomplete_failed:
+	kfree(t);
+	binder_stats.obj_deleted[BINDER_STAT_TRANSACTION]++;
+err_alloc_t_failed:
+err_bad_call_stack:
+err_empty_call_stack:
+err_dead_binder:
+err_invalid_target_handle:
+err_no_context_mgr_node:
+	if (binder_debug_mask & BINDER_DEBUG_FAILED_TRANSACTION)
+		printk(KERN_INFO "binder: %d:%d transaction failed %d, size"
+				"%zd-%zd\n",
+			   proc->pid, thread->pid, return_error,
+			   tr->data_size, tr->offsets_size);
+
+	{
+		struct binder_transaction_log_entry *fe;
+		fe = binder_transaction_log_add(&binder_transaction_log_failed);
+		*fe = *e;
+	}
+
+	BUG_ON(thread->return_error != BR_OK);
+	if (in_reply_to) {
+		thread->return_error = BR_TRANSACTION_COMPLETE;
+		binder_send_failed_reply(in_reply_to, return_error);
+	} else
+		thread->return_error = return_error;
+}
+
+static void
+binder_transaction_buffer_release(struct binder_proc *proc, struct binder_buffer *buffer, size_t *failed_at)
+{
+	size_t *offp, *off_end;
+	int debug_id = buffer->debug_id;
+
+	if (binder_debug_mask & BINDER_DEBUG_TRANSACTION)
+		printk(KERN_INFO "binder: %d buffer release %d, size %zd-%zd, failed at %p\n",
+			   proc->pid, buffer->debug_id,
+			   buffer->data_size, buffer->offsets_size, failed_at);
+
+	if (buffer->target_node)
+		binder_dec_node(buffer->target_node, 1, 0);
+
+	offp = (size_t *)(buffer->data + ALIGN(buffer->data_size, sizeof(void *)));
+	if (failed_at)
+		off_end = failed_at;
+	else
+		off_end = (void *)offp + buffer->offsets_size;
+	for (; offp < off_end; offp++) {
+		struct flat_binder_object *fp;
+		if (*offp > buffer->data_size - sizeof(*fp)) {
+			printk(KERN_ERR "binder: transaction release %d bad"
+					"offset %zd, size %zd\n", debug_id, *offp, buffer->data_size);
+			continue;
+		}
+		fp = (struct flat_binder_object *)(buffer->data + *offp);
+		switch (fp->type) {
+		case BINDER_TYPE_BINDER:
+		case BINDER_TYPE_WEAK_BINDER: {
+			struct binder_node *node = binder_get_node(proc, fp->binder);
+			if (node == NULL) {
+				printk(KERN_ERR "binder: transaction release %d bad node %p\n", debug_id, fp->binder);
+				break;
+			}
+			if (binder_debug_mask & BINDER_DEBUG_TRANSACTION)
+				printk(KERN_INFO "        node %d u%p\n",
+				       node->debug_id, node->ptr);
+			binder_dec_node(node, fp->type == BINDER_TYPE_BINDER, 0);
+		} break;
+		case BINDER_TYPE_HANDLE:
+		case BINDER_TYPE_WEAK_HANDLE: {
+			struct binder_ref *ref = binder_get_ref(proc, fp->handle);
+			if (ref == NULL) {
+				printk(KERN_ERR "binder: transaction release %d bad handle %ld\n", debug_id, fp->handle);
+				break;
+			}
+			if (binder_debug_mask & BINDER_DEBUG_TRANSACTION)
+				printk(KERN_INFO "        ref %d desc %d (node %d)\n",
+				       ref->debug_id, ref->desc, ref->node->debug_id);
+			binder_dec_ref(ref, fp->type == BINDER_TYPE_HANDLE);
+		} break;
+
+		case BINDER_TYPE_FD:
+			if (binder_debug_mask & BINDER_DEBUG_TRANSACTION)
+				printk(KERN_INFO "        fd %ld\n", fp->handle);
+			if (failed_at)
+				task_close_fd(proc->tsk, fp->handle);
+			break;
+
+		default:
+			printk(KERN_ERR "binder: transaction release %d bad object type %lx\n", debug_id, fp->type);
+			break;
+		}
+	}
+}
+
+int
+binder_thread_write(struct binder_proc *proc, struct binder_thread *thread,
+		    void __user *buffer, int size, signed long *consumed)
+{
+	uint32_t cmd;
+	void __user *ptr = buffer + *consumed;
+	void __user *end = buffer + size;
+
+	while (ptr < end && thread->return_error == BR_OK) {
+		if (get_user(cmd, (uint32_t __user *)ptr))
+			return -EFAULT;
+		ptr += sizeof(uint32_t);
+		if (_IOC_NR(cmd) < ARRAY_SIZE(binder_stats.bc)) {
+			binder_stats.bc[_IOC_NR(cmd)]++;
+			proc->stats.bc[_IOC_NR(cmd)]++;
+			thread->stats.bc[_IOC_NR(cmd)]++;
+		}
+		switch (cmd) {
+		case BC_INCREFS:
+		case BC_ACQUIRE:
+		case BC_RELEASE:
+		case BC_DECREFS: {
+			uint32_t target;
+			struct binder_ref *ref;
+			const char *debug_string;
+
+			if (get_user(target, (uint32_t __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(uint32_t);
+			if (target == 0 && binder_context_mgr_node &&
+			    (cmd == BC_INCREFS || cmd == BC_ACQUIRE)) {
+				ref = binder_get_ref_for_node(proc,
+					       binder_context_mgr_node);
+				if (ref->desc != target) {
+					binder_user_error("binder: %d:"
+						"%d tried to acquire "
+						"reference to desc 0, "
+						"got %d instead\n",
+						proc->pid, thread->pid,
+						ref->desc);
+				}
+			} else
+				ref = binder_get_ref(proc, target);
+			if (ref == NULL) {
+				binder_user_error("binder: %d:%d refcou"
+					"nt change on invalid ref %d\n",
+					proc->pid, thread->pid, target);
+				break;
+			}
+			switch (cmd) {
+			case BC_INCREFS:
+				debug_string = "IncRefs";
+				binder_inc_ref(ref, 0, NULL);
+				break;
+			case BC_ACQUIRE:
+				debug_string = "Acquire";
+				binder_inc_ref(ref, 1, NULL);
+				break;
+			case BC_RELEASE:
+				debug_string = "Release";
+				binder_dec_ref(ref, 1);
+				break;
+			case BC_DECREFS:
+			default:
+				debug_string = "DecRefs";
+				binder_dec_ref(ref, 0);
+				break;
+			}
+			if (binder_debug_mask & BINDER_DEBUG_USER_REFS)
+				printk(KERN_INFO "binder: %d:%d %s ref %d desc %d s %d w %d for node %d\n",
+				       proc->pid, thread->pid, debug_string, ref->debug_id, ref->desc, ref->strong, ref->weak, ref->node->debug_id);
+			break;
+		}
+		case BC_INCREFS_DONE:
+		case BC_ACQUIRE_DONE: {
+			void __user *node_ptr;
+			void *cookie;
+			struct binder_node *node;
+
+			if (get_user(node_ptr, (void * __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(void *);
+			if (get_user(cookie, (void * __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(void *);
+			node = binder_get_node(proc, node_ptr);
+			if (node == NULL) {
+				binder_user_error("binder: %d:%d "
+					"%s u%p no match\n",
+					proc->pid, thread->pid,
+					cmd == BC_INCREFS_DONE ?
+					"BC_INCREFS_DONE" :
+					"BC_ACQUIRE_DONE",
+					node_ptr);
+				break;
+			}
+			if (cookie != node->cookie) {
+				binder_user_error("binder: %d:%d %s u%p node %d"
+					" cookie mismatch %p != %p\n",
+					proc->pid, thread->pid,
+					cmd == BC_INCREFS_DONE ?
+					"BC_INCREFS_DONE" : "BC_ACQUIRE_DONE",
+					node_ptr, node->debug_id,
+					cookie, node->cookie);
+				break;
+			}
+			if (cmd == BC_ACQUIRE_DONE) {
+				if (node->pending_strong_ref == 0) {
+					binder_user_error("binder: %d:%d "
+						"BC_ACQUIRE_DONE node %d has "
+						"no pending acquire request\n",
+						proc->pid, thread->pid,
+						node->debug_id);
+					break;
+				}
+				node->pending_strong_ref = 0;
+			} else {
+				if (node->pending_weak_ref == 0) {
+					binder_user_error("binder: %d:%d "
+						"BC_INCREFS_DONE node %d has "
+						"no pending increfs request\n",
+						proc->pid, thread->pid,
+						node->debug_id);
+					break;
+				}
+				node->pending_weak_ref = 0;
+			}
+			binder_dec_node(node, cmd == BC_ACQUIRE_DONE, 0);
+			if (binder_debug_mask & BINDER_DEBUG_USER_REFS)
+				printk(KERN_INFO "binder: %d:%d %s node %d ls %d lw %d\n",
+				       proc->pid, thread->pid, cmd == BC_INCREFS_DONE ? "BC_INCREFS_DONE" : "BC_ACQUIRE_DONE", node->debug_id, node->local_strong_refs, node->local_weak_refs);
+			break;
+		}
+		case BC_ATTEMPT_ACQUIRE:
+			printk(KERN_ERR "binder: BC_ATTEMPT_ACQUIRE not supported\n");
+			return -EINVAL;
+		case BC_ACQUIRE_RESULT:
+			printk(KERN_ERR "binder: BC_ACQUIRE_RESULT not supported\n");
+			return -EINVAL;
+
+		case BC_FREE_BUFFER: {
+			void __user *data_ptr;
+			struct binder_buffer *buffer;
+
+			if (get_user(data_ptr, (void * __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(void *);
+
+			buffer = binder_buffer_lookup(proc, data_ptr);
+			if (buffer == NULL) {
+				binder_user_error("binder: %d:%d "
+					"BC_FREE_BUFFER u%p no match\n",
+					proc->pid, thread->pid, data_ptr);
+				break;
+			}
+			if (!buffer->allow_user_free) {
+				binder_user_error("binder: %d:%d "
+					"BC_FREE_BUFFER u%p matched "
+					"unreturned buffer\n",
+					proc->pid, thread->pid, data_ptr);
+				break;
+			}
+			if (binder_debug_mask & BINDER_DEBUG_FREE_BUFFER)
+				printk(KERN_INFO "binder: %d:%d BC_FREE_BUFFER u%p found buffer %d for %s transaction\n",
+				       proc->pid, thread->pid, data_ptr, buffer->debug_id,
+				       buffer->transaction ? "active" : "finished");
+
+			if (buffer->transaction) {
+				buffer->transaction->buffer = NULL;
+				buffer->transaction = NULL;
+			}
+			if (buffer->async_transaction && buffer->target_node) {
+				BUG_ON(!buffer->target_node->has_async_transaction);
+				if (list_empty(&buffer->target_node->async_todo))
+					buffer->target_node->has_async_transaction = 0;
+				else
+					list_move_tail(buffer->target_node->async_todo.next, &thread->todo);
+			}
+			binder_transaction_buffer_release(proc, buffer, NULL);
+			binder_free_buf(proc, buffer);
+			break;
+		}
+
+		case BC_TRANSACTION:
+		case BC_REPLY: {
+			struct binder_transaction_data tr;
+
+			if (copy_from_user(&tr, ptr, sizeof(tr)))
+				return -EFAULT;
+			ptr += sizeof(tr);
+			binder_transaction(proc, thread, &tr, cmd == BC_REPLY);
+			break;
+		}
+
+		case BC_REGISTER_LOOPER:
+			if (binder_debug_mask & BINDER_DEBUG_THREADS)
+				printk(KERN_INFO "binder: %d:%d BC_REGISTER_LOOPER\n",
+				       proc->pid, thread->pid);
+			if (thread->looper & BINDER_LOOPER_STATE_ENTERED) {
+				thread->looper |= BINDER_LOOPER_STATE_INVALID;
+				binder_user_error("binder: %d:%d ERROR:"
+					" BC_REGISTER_LOOPER called "
+					"after BC_ENTER_LOOPER\n",
+					proc->pid, thread->pid);
+			} else if (proc->requested_threads == 0) {
+				thread->looper |= BINDER_LOOPER_STATE_INVALID;
+				binder_user_error("binder: %d:%d ERROR:"
+					" BC_REGISTER_LOOPER called "
+					"without request\n",
+					proc->pid, thread->pid);
+			} else {
+				proc->requested_threads--;
+				proc->requested_threads_started++;
+			}
+			thread->looper |= BINDER_LOOPER_STATE_REGISTERED;
+			break;
+		case BC_ENTER_LOOPER:
+			if (binder_debug_mask & BINDER_DEBUG_THREADS)
+				printk(KERN_INFO "binder: %d:%d BC_ENTER_LOOPER\n",
+				       proc->pid, thread->pid);
+			if (thread->looper & BINDER_LOOPER_STATE_REGISTERED) {
+				thread->looper |= BINDER_LOOPER_STATE_INVALID;
+				binder_user_error("binder: %d:%d ERROR:"
+					" BC_ENTER_LOOPER called after "
+					"BC_REGISTER_LOOPER\n",
+					proc->pid, thread->pid);
+			}
+			thread->looper |= BINDER_LOOPER_STATE_ENTERED;
+			break;
+		case BC_EXIT_LOOPER:
+			if (binder_debug_mask & BINDER_DEBUG_THREADS)
+				printk(KERN_INFO "binder: %d:%d BC_EXIT_LOOPER\n",
+				       proc->pid, thread->pid);
+			thread->looper |= BINDER_LOOPER_STATE_EXITED;
+			break;
+
+		case BC_REQUEST_DEATH_NOTIFICATION:
+		case BC_CLEAR_DEATH_NOTIFICATION: {
+			uint32_t target;
+			void __user *cookie;
+			struct binder_ref *ref;
+			struct binder_ref_death *death;
+
+			if (get_user(target, (uint32_t __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(uint32_t);
+			if (get_user(cookie, (void __user * __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(void *);
+			ref = binder_get_ref(proc, target);
+			if (ref == NULL) {
+				binder_user_error("binder: %d:%d %s "
+					"invalid ref %d\n",
+					proc->pid, thread->pid,
+					cmd == BC_REQUEST_DEATH_NOTIFICATION ?
+					"BC_REQUEST_DEATH_NOTIFICATION" :
+					"BC_CLEAR_DEATH_NOTIFICATION",
+					target);
+				break;
+			}
+
+			if (binder_debug_mask & BINDER_DEBUG_DEATH_NOTIFICATION)
+				printk(KERN_INFO "binder: %d:%d %s %p ref %d desc %d s %d w %d for node %d\n",
+				       proc->pid, thread->pid,
+				       cmd == BC_REQUEST_DEATH_NOTIFICATION ?
+				       "BC_REQUEST_DEATH_NOTIFICATION" :
+				       "BC_CLEAR_DEATH_NOTIFICATION",
+				       cookie, ref->debug_id, ref->desc,
+				       ref->strong, ref->weak, ref->node->debug_id);
+
+			if (cmd == BC_REQUEST_DEATH_NOTIFICATION) {
+				if (ref->death) {
+					binder_user_error("binder: %d:%"
+						"d BC_REQUEST_DEATH_NOTI"
+						"FICATION death notific"
+						"ation already set\n",
+						proc->pid, thread->pid);
+					break;
+				}
+				death = kzalloc(sizeof(*death), GFP_KERNEL);
+				if (death == NULL) {
+					thread->return_error = BR_ERROR;
+					if (binder_debug_mask & BINDER_DEBUG_FAILED_TRANSACTION)
+						printk(KERN_INFO "binder: %d:%d "
+							"BC_REQUEST_DEATH_NOTIFICATION failed\n",
+							proc->pid, thread->pid);
+					break;
+				}
+				binder_stats.obj_created[BINDER_STAT_DEATH]++;
+				INIT_LIST_HEAD(&death->work.entry);
+				death->cookie = cookie;
+				ref->death = death;
+				if (ref->node->proc == NULL) {
+					ref->death->work.type = BINDER_WORK_DEAD_BINDER;
+					if (thread->looper & (BINDER_LOOPER_STATE_REGISTERED | BINDER_LOOPER_STATE_ENTERED)) {
+						list_add_tail(&ref->death->work.entry, &thread->todo);
+					} else {
+						list_add_tail(&ref->death->work.entry, &proc->todo);
+						wake_up_interruptible(&proc->wait);
+					}
+				}
+			} else {
+				if (ref->death == NULL) {
+					binder_user_error("binder: %d:%"
+						"d BC_CLEAR_DEATH_NOTIFI"
+						"CATION death notificat"
+						"ion not active\n",
+						proc->pid, thread->pid);
+					break;
+				}
+				death = ref->death;
+				if (death->cookie != cookie) {
+					binder_user_error("binder: %d:%"
+						"d BC_CLEAR_DEATH_NOTIFI"
+						"CATION death notificat"
+						"ion cookie mismatch "
+						"%p != %p\n",
+						proc->pid, thread->pid,
+						death->cookie, cookie);
+					break;
+				}
+				ref->death = NULL;
+				if (list_empty(&death->work.entry)) {
+					death->work.type = BINDER_WORK_CLEAR_DEATH_NOTIFICATION;
+					if (thread->looper & (BINDER_LOOPER_STATE_REGISTERED | BINDER_LOOPER_STATE_ENTERED)) {
+						list_add_tail(&death->work.entry, &thread->todo);
+					} else {
+						list_add_tail(&death->work.entry, &proc->todo);
+						wake_up_interruptible(&proc->wait);
+					}
+				} else {
+					BUG_ON(death->work.type != BINDER_WORK_DEAD_BINDER);
+					death->work.type = BINDER_WORK_DEAD_BINDER_AND_CLEAR;
+				}
+			}
+		} break;
+		case BC_DEAD_BINDER_DONE: {
+			struct binder_work *w;
+			void __user *cookie;
+			struct binder_ref_death *death = NULL;
+			if (get_user(cookie, (void __user * __user *)ptr))
+				return -EFAULT;
+
+			ptr += sizeof(void *);
+			list_for_each_entry(w, &proc->delivered_death, entry) {
+				struct binder_ref_death *tmp_death = container_of(w, struct binder_ref_death, work);
+				if (tmp_death->cookie == cookie) {
+					death = tmp_death;
+					break;
+				}
+			}
+			if (binder_debug_mask & BINDER_DEBUG_DEAD_BINDER)
+				printk(KERN_INFO "binder: %d:%d BC_DEAD_BINDER_DONE %p found %p\n",
+				       proc->pid, thread->pid, cookie, death);
+			if (death == NULL) {
+				binder_user_error("binder: %d:%d BC_DEAD"
+					"_BINDER_DONE %p not found\n",
+					proc->pid, thread->pid, cookie);
+				break;
+			}
+
+			list_del_init(&death->work.entry);
+			if (death->work.type == BINDER_WORK_DEAD_BINDER_AND_CLEAR) {
+				death->work.type = BINDER_WORK_CLEAR_DEATH_NOTIFICATION;
+				if (thread->looper & (BINDER_LOOPER_STATE_REGISTERED | BINDER_LOOPER_STATE_ENTERED)) {
+					list_add_tail(&death->work.entry, &thread->todo);
+				} else {
+					list_add_tail(&death->work.entry, &proc->todo);
+					wake_up_interruptible(&proc->wait);
+				}
+			}
+		} break;
+
+		default:
+			printk(KERN_ERR "binder: %d:%d unknown command %d\n", proc->pid, thread->pid, cmd);
+			return -EINVAL;
+		}
+		*consumed = ptr - buffer;
+	}
+	return 0;
+}
+
+void
+binder_stat_br(struct binder_proc *proc, struct binder_thread *thread, uint32_t cmd)
+{
+	if (_IOC_NR(cmd) < ARRAY_SIZE(binder_stats.br)) {
+		binder_stats.br[_IOC_NR(cmd)]++;
+		proc->stats.br[_IOC_NR(cmd)]++;
+		thread->stats.br[_IOC_NR(cmd)]++;
+	}
+}
+
+static int
+binder_has_proc_work(struct binder_proc *proc, struct binder_thread *thread)
+{
+	return !list_empty(&proc->todo) || (thread->looper & BINDER_LOOPER_STATE_NEED_RETURN);
+}
+
+static int
+binder_has_thread_work(struct binder_thread *thread)
+{
+	return !list_empty(&thread->todo) || thread->return_error != BR_OK ||
+		(thread->looper & BINDER_LOOPER_STATE_NEED_RETURN);
+}
+
+static int
+binder_thread_read(struct binder_proc *proc, struct binder_thread *thread,
+	void  __user *buffer, int size, signed long *consumed, int non_block)
+{
+	void __user *ptr = buffer + *consumed;
+	void __user *end = buffer + size;
+
+	int ret = 0;
+	int wait_for_proc_work;
+
+	if (*consumed == 0) {
+		if (put_user(BR_NOOP, (uint32_t __user *)ptr))
+			return -EFAULT;
+		ptr += sizeof(uint32_t);
+	}
+
+retry:
+	wait_for_proc_work = thread->transaction_stack == NULL && list_empty(&thread->todo);
+
+	if (thread->return_error != BR_OK && ptr < end) {
+		if (thread->return_error2 != BR_OK) {
+			if (put_user(thread->return_error2, (uint32_t __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(uint32_t);
+			if (ptr == end)
+				goto done;
+			thread->return_error2 = BR_OK;
+		}
+		if (put_user(thread->return_error, (uint32_t __user *)ptr))
+			return -EFAULT;
+		ptr += sizeof(uint32_t);
+		thread->return_error = BR_OK;
+		goto done;
+	}
+
+
+	thread->looper |= BINDER_LOOPER_STATE_WAITING;
+	if (wait_for_proc_work)
+		proc->ready_threads++;
+	mutex_unlock(&binder_lock);
+	if (wait_for_proc_work) {
+		if (!(thread->looper & (BINDER_LOOPER_STATE_REGISTERED |
+					BINDER_LOOPER_STATE_ENTERED))) {
+			binder_user_error("binder: %d:%d ERROR: Thread waiting "
+				"for process work before calling BC_REGISTER_"
+				"LOOPER or BC_ENTER_LOOPER (state %x)\n",
+				proc->pid, thread->pid, thread->looper);
+			wait_event_interruptible(binder_user_error_wait, binder_stop_on_user_error < 2);
+		}
+		binder_set_nice(proc->default_priority);
+		if (non_block) {
+			if (!binder_has_proc_work(proc, thread))
+				ret = -EAGAIN;
+		} else
+			ret = wait_event_interruptible_exclusive(proc->wait, binder_has_proc_work(proc, thread));
+	} else {
+		if (non_block) {
+			if (!binder_has_thread_work(thread))
+				ret = -EAGAIN;
+		} else
+			ret = wait_event_interruptible(thread->wait, binder_has_thread_work(thread));
+	}
+	mutex_lock(&binder_lock);
+	if (wait_for_proc_work)
+		proc->ready_threads--;
+	thread->looper &= ~BINDER_LOOPER_STATE_WAITING;
+
+	if (ret)
+		return ret;
+
+	while (1) {
+		uint32_t cmd;
+		struct binder_transaction_data tr;
+		struct binder_work *w;
+		struct binder_transaction *t = NULL;
+
+		if (!list_empty(&thread->todo))
+			w = list_first_entry(&thread->todo, struct binder_work, entry);
+		else if (!list_empty(&proc->todo) && wait_for_proc_work)
+			w = list_first_entry(&proc->todo, struct binder_work, entry);
+		else {
+			if (ptr - buffer == 4 && !(thread->looper & BINDER_LOOPER_STATE_NEED_RETURN)) /* no data added */
+				goto retry;
+			break;
+		}
+
+		if (end - ptr < sizeof(tr) + 4)
+			break;
+
+		switch (w->type) {
+		case BINDER_WORK_TRANSACTION: {
+			t = container_of(w, struct binder_transaction, work);
+		} break;
+		case BINDER_WORK_TRANSACTION_COMPLETE: {
+			cmd = BR_TRANSACTION_COMPLETE;
+			if (put_user(cmd, (uint32_t __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(uint32_t);
+
+			binder_stat_br(proc, thread, cmd);
+			if (binder_debug_mask & BINDER_DEBUG_TRANSACTION_COMPLETE)
+				printk(KERN_INFO "binder: %d:%d BR_TRANSACTION_COMPLETE\n",
+				       proc->pid, thread->pid);
+
+			list_del(&w->entry);
+			kfree(w);
+			binder_stats.obj_deleted[BINDER_STAT_TRANSACTION_COMPLETE]++;
+		} break;
+		case BINDER_WORK_NODE: {
+			struct binder_node *node = container_of(w, struct binder_node, work);
+			uint32_t cmd = BR_NOOP;
+			const char *cmd_name;
+			int strong = node->internal_strong_refs || node->local_strong_refs;
+			int weak = !hlist_empty(&node->refs) || node->local_weak_refs || strong;
+			if (weak && !node->has_weak_ref) {
+				cmd = BR_INCREFS;
+				cmd_name = "BR_INCREFS";
+				node->has_weak_ref = 1;
+				node->pending_weak_ref = 1;
+				node->local_weak_refs++;
+			} else if (strong && !node->has_strong_ref) {
+				cmd = BR_ACQUIRE;
+				cmd_name = "BR_ACQUIRE";
+				node->has_strong_ref = 1;
+				node->pending_strong_ref = 1;
+				node->local_strong_refs++;
+			} else if (!strong && node->has_strong_ref) {
+				cmd = BR_RELEASE;
+				cmd_name = "BR_RELEASE";
+				node->has_strong_ref = 0;
+			} else if (!weak && node->has_weak_ref) {
+				cmd = BR_DECREFS;
+				cmd_name = "BR_DECREFS";
+				node->has_weak_ref = 0;
+			}
+			if (cmd != BR_NOOP) {
+				if (put_user(cmd, (uint32_t __user *)ptr))
+					return -EFAULT;
+				ptr += sizeof(uint32_t);
+				if (put_user(node->ptr, (void * __user *)ptr))
+					return -EFAULT;
+				ptr += sizeof(void *);
+				if (put_user(node->cookie, (void * __user *)ptr))
+					return -EFAULT;
+				ptr += sizeof(void *);
+
+				binder_stat_br(proc, thread, cmd);
+				if (binder_debug_mask & BINDER_DEBUG_USER_REFS)
+					printk(KERN_INFO "binder: %d:%d %s %d u%p c%p\n",
+					       proc->pid, thread->pid, cmd_name, node->debug_id, node->ptr, node->cookie);
+			} else {
+				list_del_init(&w->entry);
+				if (!weak && !strong) {
+					if (binder_debug_mask & BINDER_DEBUG_INTERNAL_REFS)
+						printk(KERN_INFO "binder: %d:%d node %d u%p c%p deleted\n",
+						       proc->pid, thread->pid, node->debug_id, node->ptr, node->cookie);
+					rb_erase(&node->rb_node, &proc->nodes);
+					kfree(node);
+					binder_stats.obj_deleted[BINDER_STAT_NODE]++;
+				} else {
+					if (binder_debug_mask & BINDER_DEBUG_INTERNAL_REFS)
+						printk(KERN_INFO "binder: %d:%d node %d u%p c%p state unchanged\n",
+						       proc->pid, thread->pid, node->debug_id, node->ptr, node->cookie);
+				}
+			}
+		} break;
+		case BINDER_WORK_DEAD_BINDER:
+		case BINDER_WORK_DEAD_BINDER_AND_CLEAR:
+		case BINDER_WORK_CLEAR_DEATH_NOTIFICATION: {
+			struct binder_ref_death *death = container_of(w, struct binder_ref_death, work);
+			uint32_t cmd;
+			if (w->type == BINDER_WORK_CLEAR_DEATH_NOTIFICATION)
+				cmd = BR_CLEAR_DEATH_NOTIFICATION_DONE;
+			else
+				cmd = BR_DEAD_BINDER;
+			if (put_user(cmd, (uint32_t __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(uint32_t);
+			if (put_user(death->cookie, (void * __user *)ptr))
+				return -EFAULT;
+			ptr += sizeof(void *);
+			if (binder_debug_mask & BINDER_DEBUG_DEATH_NOTIFICATION)
+				printk(KERN_INFO "binder: %d:%d %s %p\n",
+				       proc->pid, thread->pid,
+				       cmd == BR_DEAD_BINDER ?
+				       "BR_DEAD_BINDER" :
+				       "BR_CLEAR_DEATH_NOTIFICATION_DONE",
+				       death->cookie);
+
+			if (w->type == BINDER_WORK_CLEAR_DEATH_NOTIFICATION) {
+				list_del(&w->entry);
+				kfree(death);
+				binder_stats.obj_deleted[BINDER_STAT_DEATH]++;
+			} else
+				list_move(&w->entry, &proc->delivered_death);
+			if (cmd == BR_DEAD_BINDER)
+				goto done; /* DEAD_BINDER notifications can cause transactions */
+		} break;
+		}
+
+		if (!t)
+			continue;
+
+		BUG_ON(t->buffer == NULL);
+		if (t->buffer->target_node) {
+			struct binder_node *target_node = t->buffer->target_node;
+			tr.target.ptr = target_node->ptr;
+			tr.cookie =  target_node->cookie;
+			t->saved_priority = task_nice(current);
+			if (t->priority < target_node->min_priority &&
+			    !(t->flags & TF_ONE_WAY))
+				binder_set_nice(t->priority);
+			else if (!(t->flags & TF_ONE_WAY) ||
+				 t->saved_priority > target_node->min_priority)
+				binder_set_nice(target_node->min_priority);
+			cmd = BR_TRANSACTION;
+		} else {
+			tr.target.ptr = NULL;
+			tr.cookie = NULL;
+			cmd = BR_REPLY;
+		}
+		tr.code = t->code;
+		tr.flags = t->flags;
+		tr.sender_euid = t->sender_euid;
+
+		if (t->from) {
+			struct task_struct *sender = t->from->proc->tsk;
+			tr.sender_pid = task_tgid_nr_ns(sender, current->nsproxy->pid_ns);
+		} else {
+			tr.sender_pid = 0;
+		}
+
+		tr.data_size = t->buffer->data_size;
+		tr.offsets_size = t->buffer->offsets_size;
+		tr.data.ptr.buffer = (void *)((void *)t->buffer->data + proc->user_buffer_offset);
+		tr.data.ptr.offsets = tr.data.ptr.buffer + ALIGN(t->buffer->data_size, sizeof(void *));
+
+		if (put_user(cmd, (uint32_t __user *)ptr))
+			return -EFAULT;
+		ptr += sizeof(uint32_t);
+		if (copy_to_user(ptr, &tr, sizeof(tr)))
+			return -EFAULT;
+		ptr += sizeof(tr);
+
+		binder_stat_br(proc, thread, cmd);
+		if (binder_debug_mask & BINDER_DEBUG_TRANSACTION)
+			printk(KERN_INFO "binder: %d:%d %s %d %d:%d, cmd %d"
+				"size %zd-%zd ptr %p-%p\n",
+			       proc->pid, thread->pid,
+			       (cmd == BR_TRANSACTION) ? "BR_TRANSACTION" : "BR_REPLY",
+			       t->debug_id, t->from ? t->from->proc->pid : 0,
+			       t->from ? t->from->pid : 0, cmd,
+			       t->buffer->data_size, t->buffer->offsets_size,
+			       tr.data.ptr.buffer, tr.data.ptr.offsets);
+
+		list_del(&t->work.entry);
+		t->buffer->allow_user_free = 1;
+		if (cmd == BR_TRANSACTION && !(t->flags & TF_ONE_WAY)) {
+			t->to_parent = thread->transaction_stack;
+			t->to_thread = thread;
+			thread->transaction_stack = t;
+		} else {
+			t->buffer->transaction = NULL;
+			kfree(t);
+			binder_stats.obj_deleted[BINDER_STAT_TRANSACTION]++;
+		}
+		break;
+	}
+
+done:
+
+	*consumed = ptr - buffer;
+	if (proc->requested_threads + proc->ready_threads == 0 &&
+	    proc->requested_threads_started < proc->max_threads &&
+	    (thread->looper & (BINDER_LOOPER_STATE_REGISTERED |
+	     BINDER_LOOPER_STATE_ENTERED)) /* the user-space code fails to */
+	     /*spawn a new thread if we leave this out */) {
+		proc->requested_threads++;
+		if (binder_debug_mask & BINDER_DEBUG_THREADS)
+			printk(KERN_INFO "binder: %d:%d BR_SPAWN_LOOPER\n",
+			       proc->pid, thread->pid);
+		if (put_user(BR_SPAWN_LOOPER, (uint32_t __user *)buffer))
+			return -EFAULT;
+	}
+	return 0;
+}
+
+static void binder_release_work(struct list_head *list)
+{
+	struct binder_work *w;
+	while (!list_empty(list)) {
+		w = list_first_entry(list, struct binder_work, entry);
+		list_del_init(&w->entry);
+		switch (w->type) {
+		case BINDER_WORK_TRANSACTION: {
+			struct binder_transaction *t = container_of(w, struct binder_transaction, work);
+			if (t->buffer->target_node && !(t->flags & TF_ONE_WAY))
+				binder_send_failed_reply(t, BR_DEAD_REPLY);
+		} break;
+		case BINDER_WORK_TRANSACTION_COMPLETE: {
+			kfree(w);
+			binder_stats.obj_deleted[BINDER_STAT_TRANSACTION_COMPLETE]++;
+		} break;
+		default:
+			break;
+		}
+	}
+
+}
+
+static struct binder_thread *binder_get_thread(struct binder_proc *proc)
+{
+	struct binder_thread *thread = NULL;
+	struct rb_node *parent = NULL;
+	struct rb_node **p = &proc->threads.rb_node;
+
+	while (*p) {
+		parent = *p;
+		thread = rb_entry(parent, struct binder_thread, rb_node);
+
+		if (current->pid < thread->pid)
+			p = &(*p)->rb_left;
+		else if (current->pid > thread->pid)
+			p = &(*p)->rb_right;
+		else
+			break;
+	}
+	if (*p == NULL) {
+		thread = kzalloc(sizeof(*thread), GFP_KERNEL);
+		if (thread == NULL)
+			return NULL;
+		binder_stats.obj_created[BINDER_STAT_THREAD]++;
+		thread->proc = proc;
+		thread->pid = current->pid;
+		init_waitqueue_head(&thread->wait);
+		INIT_LIST_HEAD(&thread->todo);
+		rb_link_node(&thread->rb_node, parent, p);
+		rb_insert_color(&thread->rb_node, &proc->threads);
+		thread->looper |= BINDER_LOOPER_STATE_NEED_RETURN;
+		thread->return_error = BR_OK;
+		thread->return_error2 = BR_OK;
+	}
+	return thread;
+}
+
+static int binder_free_thread(struct binder_proc *proc, struct binder_thread *thread)
+{
+	struct binder_transaction *t;
+	struct binder_transaction *send_reply = NULL;
+	int active_transactions = 0;
+
+	rb_erase(&thread->rb_node, &proc->threads);
+	t = thread->transaction_stack;
+	if (t && t->to_thread == thread)
+		send_reply = t;
+	while (t) {
+		active_transactions++;
+		if (binder_debug_mask & BINDER_DEBUG_DEAD_TRANSACTION)
+			printk(KERN_INFO "binder: release %d:%d transaction %d %s, still active\n",
+			       proc->pid, thread->pid, t->debug_id, (t->to_thread == thread) ? "in" : "out");
+		if (t->to_thread == thread) {
+			t->to_proc = NULL;
+			t->to_thread = NULL;
+			if (t->buffer) {
+				t->buffer->transaction = NULL;
+				t->buffer = NULL;
+			}
+			t = t->to_parent;
+		} else if (t->from == thread) {
+			t->from = NULL;
+			t = t->from_parent;
+		} else
+			BUG();
+	}
+	if (send_reply)
+		binder_send_failed_reply(send_reply, BR_DEAD_REPLY);
+	binder_release_work(&thread->todo);
+	kfree(thread);
+	binder_stats.obj_deleted[BINDER_STAT_THREAD]++;
+	return active_transactions;
+}
+
+static unsigned int binder_poll(struct file *filp, struct poll_table_struct *wait)
+{
+	struct binder_proc *proc = filp->private_data;
+	struct binder_thread *thread = NULL;
+	int wait_for_proc_work;
+
+	mutex_lock(&binder_lock);
+	thread = binder_get_thread(proc);
+
+	wait_for_proc_work = thread->transaction_stack == NULL &&
+		list_empty(&thread->todo) && thread->return_error == BR_OK;
+	mutex_unlock(&binder_lock);
+
+	if (wait_for_proc_work) {
+		if (binder_has_proc_work(proc, thread))
+			return POLLIN;
+		poll_wait(filp, &proc->wait, wait);
+		if (binder_has_proc_work(proc, thread))
+			return POLLIN;
+	} else {
+		if (binder_has_thread_work(thread))
+			return POLLIN;
+		poll_wait(filp, &thread->wait, wait);
+		if (binder_has_thread_work(thread))
+			return POLLIN;
+	}
+	return 0;
+}
+
+static long binder_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
+{
+	int ret;
+	struct binder_proc *proc = filp->private_data;
+	struct binder_thread *thread;
+	unsigned int size = _IOC_SIZE(cmd);
+	void __user *ubuf = (void __user *)arg;
+
+	/*printk(KERN_INFO "binder_ioctl: %d:%d %x %lx\n", proc->pid, current->pid, cmd, arg);*/
+
+	ret = wait_event_interruptible(binder_user_error_wait, binder_stop_on_user_error < 2);
+	if (ret)
+		return ret;
+
+	mutex_lock(&binder_lock);
+	thread = binder_get_thread(proc);
+	if (thread == NULL) {
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	switch (cmd) {
+	case BINDER_WRITE_READ: {
+		struct binder_write_read bwr;
+		if (size != sizeof(struct binder_write_read)) {
+			ret = -EINVAL;
+			goto err;
+		}
+		if (copy_from_user(&bwr, ubuf, sizeof(bwr))) {
+			ret = -EFAULT;
+			goto err;
+		}
+		if (binder_debug_mask & BINDER_DEBUG_READ_WRITE)
+			printk(KERN_INFO "binder: %d:%d write %ld at %08lx, read %ld at %08lx\n",
+			       proc->pid, thread->pid, bwr.write_size, bwr.write_buffer, bwr.read_size, bwr.read_buffer);
+		if (bwr.write_size > 0) {
+			ret = binder_thread_write(proc, thread, (void __user *)bwr.write_buffer, bwr.write_size, &bwr.write_consumed);
+			if (ret < 0) {
+				bwr.read_consumed = 0;
+				if (copy_to_user(ubuf, &bwr, sizeof(bwr)))
+					ret = -EFAULT;
+				goto err;
+			}
+		}
+		if (bwr.read_size > 0) {
+			ret = binder_thread_read(proc, thread, (void __user *)bwr.read_buffer, bwr.read_size, &bwr.read_consumed, filp->f_flags & O_NONBLOCK);
+			if (!list_empty(&proc->todo))
+				wake_up_interruptible(&proc->wait);
+			if (ret < 0) {
+				if (copy_to_user(ubuf, &bwr, sizeof(bwr)))
+					ret = -EFAULT;
+				goto err;
+			}
+		}
+		if (binder_debug_mask & BINDER_DEBUG_READ_WRITE)
+			printk(KERN_INFO "binder: %d:%d wrote %ld of %ld, read return %ld of %ld\n",
+			       proc->pid, thread->pid, bwr.write_consumed, bwr.write_size, bwr.read_consumed, bwr.read_size);
+		if (copy_to_user(ubuf, &bwr, sizeof(bwr))) {
+			ret = -EFAULT;
+			goto err;
+		}
+		break;
+	}
+	case BINDER_SET_MAX_THREADS:
+		if (copy_from_user(&proc->max_threads, ubuf, sizeof(proc->max_threads))) {
+			ret = -EINVAL;
+			goto err;
+		}
+		break;
+	case BINDER_SET_CONTEXT_MGR:
+		if (binder_context_mgr_node != NULL) {
+			printk(KERN_ERR "binder: BINDER_SET_CONTEXT_MGR already set\n");
+			ret = -EBUSY;
+			goto err;
+		}
+		if (binder_context_mgr_uid != -1) {
+			if (binder_context_mgr_uid != current->cred->euid) {
+				printk(KERN_ERR "binder: BINDER_SET_"
+				       "CONTEXT_MGR bad uid %d != %d\n",
+				       current->cred->euid,
+				       binder_context_mgr_uid);
+				ret = -EPERM;
+				goto err;
+			}
+		} else
+			binder_context_mgr_uid = current->cred->euid;
+		binder_context_mgr_node = binder_new_node(proc, NULL, NULL);
+		if (binder_context_mgr_node == NULL) {
+			ret = -ENOMEM;
+			goto err;
+		}
+		binder_context_mgr_node->local_weak_refs++;
+		binder_context_mgr_node->local_strong_refs++;
+		binder_context_mgr_node->has_strong_ref = 1;
+		binder_context_mgr_node->has_weak_ref = 1;
+		break;
+	case BINDER_THREAD_EXIT:
+		if (binder_debug_mask & BINDER_DEBUG_THREADS)
+			printk(KERN_INFO "binder: %d:%d exit\n",
+			       proc->pid, thread->pid);
+		binder_free_thread(proc, thread);
+		thread = NULL;
+		break;
+	case BINDER_VERSION:
+		if (size != sizeof(struct binder_version)) {
+			ret = -EINVAL;
+			goto err;
+		}
+		if (put_user(BINDER_CURRENT_PROTOCOL_VERSION, &((struct binder_version *)ubuf)->protocol_version)) {
+			ret = -EINVAL;
+			goto err;
+		}
+		break;
+	default:
+		ret = -EINVAL;
+		goto err;
+	}
+	ret = 0;
+err:
+	if (thread)
+		thread->looper &= ~BINDER_LOOPER_STATE_NEED_RETURN;
+	mutex_unlock(&binder_lock);
+	wait_event_interruptible(binder_user_error_wait, binder_stop_on_user_error < 2);
+	if (ret && ret != -ERESTARTSYS)
+		printk(KERN_INFO "binder: %d:%d ioctl %x %lx returned %d\n", proc->pid, current->pid, cmd, arg, ret);
+	return ret;
+}
+
+static void binder_vma_open(struct vm_area_struct *vma)
+{
+	struct binder_proc *proc = vma->vm_private_data;
+	if (binder_debug_mask & BINDER_DEBUG_OPEN_CLOSE)
+		printk(KERN_INFO "binder: %d open vm area %lx-%lx (%ld K) vma %lx pagep %lx\n", proc->pid, vma->vm_start, vma->vm_end, (vma->vm_end - vma->vm_start) / SZ_1K, vma->vm_flags, vma->vm_page_prot.pgprot);
+	dump_stack();
+}
+static void binder_vma_close(struct vm_area_struct *vma)
+{
+	struct binder_proc *proc = vma->vm_private_data;
+	if (binder_debug_mask & BINDER_DEBUG_OPEN_CLOSE)
+		printk(KERN_INFO "binder: %d close vm area %lx-%lx (%ld K) vma %lx pagep %lx\n", proc->pid, vma->vm_start, vma->vm_end, (vma->vm_end - vma->vm_start) / SZ_1K, vma->vm_flags, vma->vm_page_prot.pgprot);
+	proc->vma = NULL;
+}
+
+static struct vm_operations_struct binder_vm_ops = {
+	.open = binder_vma_open,
+	.close = binder_vma_close,
+};
+
+static int binder_mmap(struct file *filp, struct vm_area_struct *vma)
+{
+	int ret;
+	struct vm_struct *area;
+	struct binder_proc *proc = filp->private_data;
+	const char *failure_string;
+	struct binder_buffer *buffer;
+
+	if ((vma->vm_end - vma->vm_start) > SZ_4M)
+		vma->vm_end = vma->vm_start + SZ_4M;
+
+	if (binder_debug_mask & BINDER_DEBUG_OPEN_CLOSE)
+		printk(KERN_INFO "binder_mmap: %d %lx-%lx (%ld K) vma %lx pagep %lx\n", proc->pid, vma->vm_start, vma->vm_end, (vma->vm_end - vma->vm_start) / SZ_1K, vma->vm_flags, vma->vm_page_prot.pgprot);
+
+	if (vma->vm_flags & FORBIDDEN_MMAP_FLAGS) {
+		ret = -EPERM;
+		failure_string = "bad vm_flags";
+		goto err_bad_arg;
+	}
+	vma->vm_flags = (vma->vm_flags | VM_DONTCOPY) & ~VM_MAYWRITE;
+
+	area = get_vm_area(vma->vm_end - vma->vm_start, VM_IOREMAP);
+	if (area == NULL) {
+		ret = -ENOMEM;
+		failure_string = "get_vm_area";
+		goto err_get_vm_area_failed;
+	}
+	proc->buffer = area->addr;
+	proc->user_buffer_offset = vma->vm_start - (size_t)proc->buffer;
+
+#ifdef CONFIG_CPU_CACHE_VIPT
+	if (cache_is_vipt_aliasing()) {
+		while (CACHE_COLOUR((vma->vm_start ^ (uint32_t)proc->buffer))) {
+			printk(KERN_INFO "binder_mmap: %d %lx-%lx maps %p bad alignment\n", proc->pid, vma->vm_start, vma->vm_end, proc->buffer);
+			vma->vm_start += PAGE_SIZE;
+		}
+	}
+#endif
+	proc->pages = kzalloc(sizeof(proc->pages[0]) * ((vma->vm_end - vma->vm_start) / PAGE_SIZE), GFP_KERNEL);
+	if (proc->pages == NULL) {
+		ret = -ENOMEM;
+		failure_string = "alloc page array";
+		goto err_alloc_pages_failed;
+	}
+	proc->buffer_size = vma->vm_end - vma->vm_start;
+
+	vma->vm_ops = &binder_vm_ops;
+	vma->vm_private_data = proc;
+
+	if (binder_update_page_range(proc, 1, proc->buffer, proc->buffer + PAGE_SIZE, vma)) {
+		ret = -ENOMEM;
+		failure_string = "alloc small buf";
+		goto err_alloc_small_buf_failed;
+	}
+	buffer = proc->buffer;
+	INIT_LIST_HEAD(&proc->buffers);
+	list_add(&buffer->entry, &proc->buffers);
+	buffer->free = 1;
+	binder_insert_free_buffer(proc, buffer);
+	proc->free_async_space = proc->buffer_size / 2;
+	barrier();
+	proc->vma = vma;
+
+	/*printk(KERN_INFO "binder_mmap: %d %lx-%lx maps %p\n", proc->pid, vma->vm_start, vma->vm_end, proc->buffer);*/
+	return 0;
+
+err_alloc_small_buf_failed:
+	kfree(proc->pages);
+err_alloc_pages_failed:
+	vfree(proc->buffer);
+err_get_vm_area_failed:
+	mutex_unlock(&binder_lock);
+err_bad_arg:
+	printk(KERN_ERR "binder_mmap: %d %lx-%lx %s failed %d\n", proc->pid, vma->vm_start, vma->vm_end, failure_string, ret);
+	return ret;
+}
+
+static int binder_open(struct inode *nodp, struct file *filp)
+{
+	struct binder_proc *proc;
+
+	if (binder_debug_mask & BINDER_DEBUG_OPEN_CLOSE)
+		printk(KERN_INFO "binder_open: %d:%d\n", current->group_leader->pid, current->pid);
+
+	proc = kzalloc(sizeof(*proc), GFP_KERNEL);
+	if (proc == NULL)
+		return -ENOMEM;
+	get_task_struct(current);
+	proc->tsk = current;
+	INIT_LIST_HEAD(&proc->todo);
+	init_waitqueue_head(&proc->wait);
+	proc->default_priority = task_nice(current);
+	mutex_lock(&binder_lock);
+	binder_stats.obj_created[BINDER_STAT_PROC]++;
+	hlist_add_head(&proc->proc_node, &binder_procs);
+	proc->pid = current->group_leader->pid;
+	INIT_LIST_HEAD(&proc->delivered_death);
+	filp->private_data = proc;
+	mutex_unlock(&binder_lock);
+
+	if (binder_proc_dir_entry_proc) {
+		char strbuf[11];
+		snprintf(strbuf, sizeof(strbuf), "%u", proc->pid);
+		create_proc_read_entry(strbuf, S_IRUGO, binder_proc_dir_entry_proc, binder_read_proc_proc, proc);
+	}
+
+	return 0;
+}
+
+static int binder_flush(struct file *filp, fl_owner_t id)
+{
+	struct rb_node *n;
+	struct binder_proc *proc = filp->private_data;
+	int wake_count = 0;
+
+	mutex_lock(&binder_lock);
+	for (n = rb_first(&proc->threads); n != NULL; n = rb_next(n)) {
+		struct binder_thread *thread = rb_entry(n, struct binder_thread, rb_node);
+		thread->looper |= BINDER_LOOPER_STATE_NEED_RETURN;
+		if (thread->looper & BINDER_LOOPER_STATE_WAITING) {
+			wake_up_interruptible(&thread->wait);
+			wake_count++;
+		}
+	}
+	wake_up_interruptible_all(&proc->wait);
+	mutex_unlock(&binder_lock);
+
+	if (binder_debug_mask & BINDER_DEBUG_OPEN_CLOSE)
+		printk(KERN_INFO "binder_flush: %d woke %d threads\n", proc->pid, wake_count);
+
+	return 0;
+}
+
+static int binder_release(struct inode *nodp, struct file *filp)
+{
+	struct hlist_node *pos;
+	struct binder_transaction *t;
+	struct rb_node *n;
+	struct binder_proc *proc = filp->private_data;
+	int threads, nodes, incoming_refs, outgoing_refs, buffers, active_transactions, page_count;
+
+	if (binder_proc_dir_entry_proc) {
+		char strbuf[11];
+		snprintf(strbuf, sizeof(strbuf), "%u", proc->pid);
+		remove_proc_entry(strbuf, binder_proc_dir_entry_proc);
+	}
+	mutex_lock(&binder_lock);
+	hlist_del(&proc->proc_node);
+	if (binder_context_mgr_node && binder_context_mgr_node->proc == proc) {
+		if (binder_debug_mask & BINDER_DEBUG_DEAD_BINDER)
+			printk(KERN_INFO "binder_release: %d context_mgr_node gone\n", proc->pid);
+		binder_context_mgr_node = NULL;
+	}
+
+	threads = 0;
+	active_transactions = 0;
+	while ((n = rb_first(&proc->threads))) {
+		struct binder_thread *thread = rb_entry(n, struct binder_thread, rb_node);
+		threads++;
+		active_transactions += binder_free_thread(proc, thread);
+	}
+	nodes = 0;
+	incoming_refs = 0;
+	while ((n = rb_first(&proc->nodes))) {
+		struct binder_node *node = rb_entry(n, struct binder_node, rb_node);
+
+		nodes++;
+		rb_erase(&node->rb_node, &proc->nodes);
+		list_del_init(&node->work.entry);
+		if (hlist_empty(&node->refs)) {
+			kfree(node);
+			binder_stats.obj_deleted[BINDER_STAT_NODE]++;
+		} else {
+			struct binder_ref *ref;
+			int death = 0;
+
+			node->proc = NULL;
+			node->local_strong_refs = 0;
+			node->local_weak_refs = 0;
+			hlist_add_head(&node->dead_node, &binder_dead_nodes);
+
+			hlist_for_each_entry(ref, pos, &node->refs, node_entry) {
+				incoming_refs++;
+				if (ref->death) {
+					death++;
+					if (list_empty(&ref->death->work.entry)) {
+						ref->death->work.type = BINDER_WORK_DEAD_BINDER;
+						list_add_tail(&ref->death->work.entry, &ref->proc->todo);
+						wake_up_interruptible(&ref->proc->wait);
+					} else
+						BUG();
+				}
+			}
+			if (binder_debug_mask & BINDER_DEBUG_DEAD_BINDER)
+				printk(KERN_INFO "binder: node %d now dead, refs %d, death %d\n", node->debug_id, incoming_refs, death);
+		}
+	}
+	outgoing_refs = 0;
+	while ((n = rb_first(&proc->refs_by_desc))) {
+		struct binder_ref *ref = rb_entry(n, struct binder_ref, rb_node_desc);
+		outgoing_refs++;
+		binder_delete_ref(ref);
+	}
+	binder_release_work(&proc->todo);
+	buffers = 0;
+
+	while ((n = rb_first(&proc->allocated_buffers))) {
+		struct binder_buffer *buffer = rb_entry(n, struct binder_buffer, rb_node);
+		t = buffer->transaction;
+		if (t) {
+			t->buffer = NULL;
+			buffer->transaction = NULL;
+			printk(KERN_ERR "binder: release proc %d, transaction %d, not freed\n", proc->pid, t->debug_id);
+			/*BUG();*/
+		}
+		binder_free_buf(proc, buffer);
+		buffers++;
+	}
+
+	binder_stats.obj_deleted[BINDER_STAT_PROC]++;
+	mutex_unlock(&binder_lock);
+
+	page_count = 0;
+	if (proc->pages) {
+		int i;
+		for (i = 0; i < proc->buffer_size / PAGE_SIZE; i++) {
+			if (proc->pages[i]) {
+				if (binder_debug_mask & BINDER_DEBUG_BUFFER_ALLOC)
+					printk(KERN_INFO "binder_release: %d: page %d at %p not freed\n", proc->pid, i, proc->buffer + i * PAGE_SIZE);
+				__free_page(proc->pages[i]);
+				page_count++;
+			}
+		}
+		kfree(proc->pages);
+		vfree(proc->buffer);
+	}
+
+	put_task_struct(proc->tsk);
+
+	if (binder_debug_mask & BINDER_DEBUG_OPEN_CLOSE)
+		printk(KERN_INFO "binder_release: %d threads %d, nodes %d (ref %d), refs %d, active transactions %d, buffers %d, pages %d\n",
+		       proc->pid, threads, nodes, incoming_refs, outgoing_refs, active_transactions, buffers, page_count);
+
+	kfree(proc);
+	return 0;
+}
+
+static char *print_binder_transaction(char *buf, char *end, const char *prefix, struct binder_transaction *t)
+{
+	buf += snprintf(buf, end - buf, "%s %d: %p from %d:%d to %d:%d code %x flags %x pri %ld r%d",
+			prefix, t->debug_id, t, t->from ? t->from->proc->pid : 0,
+			t->from ? t->from->pid : 0,
+			t->to_proc ? t->to_proc->pid : 0,
+			t->to_thread ? t->to_thread->pid : 0,
+			t->code, t->flags, t->priority, t->need_reply);
+	if (buf >= end)
+		return buf;
+	if (t->buffer == NULL) {
+		buf += snprintf(buf, end - buf, " buffer free\n");
+		return buf;
+	}
+	if (t->buffer->target_node) {
+		buf += snprintf(buf, end - buf, " node %d",
+				t->buffer->target_node->debug_id);
+		if (buf >= end)
+			return buf;
+	}
+	buf += snprintf(buf, end - buf, " size %zd:%zd data %p\n",
+			t->buffer->data_size, t->buffer->offsets_size,
+			t->buffer->data);
+	return buf;
+}
+
+static char *print_binder_buffer(char *buf, char *end, const char *prefix, struct binder_buffer *buffer)
+{
+	buf += snprintf(buf, end - buf, "%s %d: %p size %zd:%zd %s\n",
+			prefix, buffer->debug_id, buffer->data,
+			buffer->data_size, buffer->offsets_size,
+			buffer->transaction ? "active" : "delivered");
+	return buf;
+}
+
+static char *print_binder_work(char *buf, char *end, const char *prefix,
+	const char *transaction_prefix, struct binder_work *w)
+{
+	struct binder_node *node;
+	struct binder_transaction *t;
+
+	switch (w->type) {
+	case BINDER_WORK_TRANSACTION:
+		t = container_of(w, struct binder_transaction, work);
+		buf = print_binder_transaction(buf, end, transaction_prefix, t);
+		break;
+	case BINDER_WORK_TRANSACTION_COMPLETE:
+		buf += snprintf(buf, end - buf,
+				"%stransaction complete\n", prefix);
+		break;
+	case BINDER_WORK_NODE:
+		node = container_of(w, struct binder_node, work);
+		buf += snprintf(buf, end - buf, "%snode work %d: u%p c%p\n",
+				prefix, node->debug_id, node->ptr, node->cookie);
+		break;
+	case BINDER_WORK_DEAD_BINDER:
+		buf += snprintf(buf, end - buf, "%shas dead binder\n", prefix);
+		break;
+	case BINDER_WORK_DEAD_BINDER_AND_CLEAR:
+		buf += snprintf(buf, end - buf,
+				"%shas cleared dead binder\n", prefix);
+		break;
+	case BINDER_WORK_CLEAR_DEATH_NOTIFICATION:
+		buf += snprintf(buf, end - buf,
+				"%shas cleared death notification\n", prefix);
+		break;
+	default:
+		buf += snprintf(buf, end - buf, "%sunknown work: type %d\n",
+				prefix, w->type);
+		break;
+	}
+	return buf;
+}
+
+static char *print_binder_thread(char *buf, char *end, struct binder_thread *thread, int print_always)
+{
+	struct binder_transaction *t;
+	struct binder_work *w;
+	char *start_buf = buf;
+	char *header_buf;
+
+	buf += snprintf(buf, end - buf, "  thread %d: l %02x\n", thread->pid, thread->looper);
+	header_buf = buf;
+	t = thread->transaction_stack;
+	while (t) {
+		if (buf >= end)
+			break;
+		if (t->from == thread) {
+			buf = print_binder_transaction(buf, end, "    outgoing transaction", t);
+			t = t->from_parent;
+		} else if (t->to_thread == thread) {
+			buf = print_binder_transaction(buf, end, "    incoming transaction", t);
+			t = t->to_parent;
+		} else {
+			buf = print_binder_transaction(buf, end, "    bad transaction", t);
+			t = NULL;
+		}
+	}
+	list_for_each_entry(w, &thread->todo, entry) {
+		if (buf >= end)
+			break;
+		buf = print_binder_work(buf, end, "    ",
+					"    pending transaction", w);
+	}
+	if (!print_always && buf == header_buf)
+		buf = start_buf;
+	return buf;
+}
+
+static char *print_binder_node(char *buf, char *end, struct binder_node *node)
+{
+	struct binder_ref *ref;
+	struct hlist_node *pos;
+	struct binder_work *w;
+	int count;
+	count = 0;
+	hlist_for_each_entry(ref, pos, &node->refs, node_entry)
+		count++;
+
+	buf += snprintf(buf, end - buf, "  node %d: u%p c%p hs %d hw %d ls %d lw %d is %d iw %d",
+			node->debug_id, node->ptr, node->cookie,
+			node->has_strong_ref, node->has_weak_ref,
+			node->local_strong_refs, node->local_weak_refs,
+			node->internal_strong_refs, count);
+	if (buf >= end)
+		return buf;
+	if (count) {
+		buf += snprintf(buf, end - buf, " proc");
+		if (buf >= end)
+			return buf;
+		hlist_for_each_entry(ref, pos, &node->refs, node_entry) {
+			buf += snprintf(buf, end - buf, " %d", ref->proc->pid);
+			if (buf >= end)
+				return buf;
+		}
+	}
+	buf += snprintf(buf, end - buf, "\n");
+	list_for_each_entry(w, &node->async_todo, entry) {
+		if (buf >= end)
+			break;
+		buf = print_binder_work(buf, end, "    ",
+					"    pending async transaction", w);
+	}
+	return buf;
+}
+
+static char *print_binder_ref(char *buf, char *end, struct binder_ref *ref)
+{
+	buf += snprintf(buf, end - buf, "  ref %d: desc %d %snode %d s %d w %d d %p\n",
+			ref->debug_id, ref->desc, ref->node->proc ? "" : "dead ",
+			ref->node->debug_id, ref->strong, ref->weak, ref->death);
+	return buf;
+}
+
+static char *print_binder_proc(char *buf, char *end, struct binder_proc *proc, int print_all)
+{
+	struct binder_work *w;
+	struct rb_node *n;
+	char *start_buf = buf;
+	char *header_buf;
+
+	buf += snprintf(buf, end - buf, "proc %d\n", proc->pid);
+	header_buf = buf;
+
+	for (n = rb_first(&proc->threads); n != NULL && buf < end; n = rb_next(n))
+		buf = print_binder_thread(buf, end, rb_entry(n, struct binder_thread, rb_node), print_all);
+	for (n = rb_first(&proc->nodes); n != NULL && buf < end; n = rb_next(n)) {
+		struct binder_node *node = rb_entry(n, struct binder_node, rb_node);
+		if (print_all || node->has_async_transaction)
+			buf = print_binder_node(buf, end, node);
+	}
+	if (print_all) {
+		for (n = rb_first(&proc->refs_by_desc); n != NULL && buf < end; n = rb_next(n))
+			buf = print_binder_ref(buf, end, rb_entry(n, struct binder_ref, rb_node_desc));
+	}
+	for (n = rb_first(&proc->allocated_buffers); n != NULL && buf < end; n = rb_next(n))
+		buf = print_binder_buffer(buf, end, "  buffer", rb_entry(n, struct binder_buffer, rb_node));
+	list_for_each_entry(w, &proc->todo, entry) {
+		if (buf >= end)
+			break;
+		buf = print_binder_work(buf, end, "  ",
+					"  pending transaction", w);
+	}
+	list_for_each_entry(w, &proc->delivered_death, entry) {
+		if (buf >= end)
+			break;
+		buf += snprintf(buf, end - buf, "  has delivered dead binder\n");
+		break;
+	}
+	if (!print_all && buf == header_buf)
+		buf = start_buf;
+	return buf;
+}
+
+static const char *binder_return_strings[] = {
+	"BR_ERROR",
+	"BR_OK",
+	"BR_TRANSACTION",
+	"BR_REPLY",
+	"BR_ACQUIRE_RESULT",
+	"BR_DEAD_REPLY",
+	"BR_TRANSACTION_COMPLETE",
+	"BR_INCREFS",
+	"BR_ACQUIRE",
+	"BR_RELEASE",
+	"BR_DECREFS",
+	"BR_ATTEMPT_ACQUIRE",
+	"BR_NOOP",
+	"BR_SPAWN_LOOPER",
+	"BR_FINISHED",
+	"BR_DEAD_BINDER",
+	"BR_CLEAR_DEATH_NOTIFICATION_DONE",
+	"BR_FAILED_REPLY"
+};
+
+static const char *binder_command_strings[] = {
+	"BC_TRANSACTION",
+	"BC_REPLY",
+	"BC_ACQUIRE_RESULT",
+	"BC_FREE_BUFFER",
+	"BC_INCREFS",
+	"BC_ACQUIRE",
+	"BC_RELEASE",
+	"BC_DECREFS",
+	"BC_INCREFS_DONE",
+	"BC_ACQUIRE_DONE",
+	"BC_ATTEMPT_ACQUIRE",
+	"BC_REGISTER_LOOPER",
+	"BC_ENTER_LOOPER",
+	"BC_EXIT_LOOPER",
+	"BC_REQUEST_DEATH_NOTIFICATION",
+	"BC_CLEAR_DEATH_NOTIFICATION",
+	"BC_DEAD_BINDER_DONE"
+};
+
+static const char *binder_objstat_strings[] = {
+	"proc",
+	"thread",
+	"node",
+	"ref",
+	"death",
+	"transaction",
+	"transaction_complete"
+};
+
+static char *print_binder_stats(char *buf, char *end, const char *prefix, struct binder_stats *stats)
+{
+	int i;
+
+	BUILD_BUG_ON(ARRAY_SIZE(stats->bc) != ARRAY_SIZE(binder_command_strings));
+	for (i = 0; i < ARRAY_SIZE(stats->bc); i++) {
+		if (stats->bc[i])
+			buf += snprintf(buf, end - buf, "%s%s: %d\n", prefix,
+					binder_command_strings[i], stats->bc[i]);
+		if (buf >= end)
+			return buf;
+	}
+
+	BUILD_BUG_ON(ARRAY_SIZE(stats->br) != ARRAY_SIZE(binder_return_strings));
+	for (i = 0; i < ARRAY_SIZE(stats->br); i++) {
+		if (stats->br[i])
+			buf += snprintf(buf, end - buf, "%s%s: %d\n", prefix,
+					binder_return_strings[i], stats->br[i]);
+		if (buf >= end)
+			return buf;
+	}
+
+	BUILD_BUG_ON(ARRAY_SIZE(stats->obj_created) != ARRAY_SIZE(binder_objstat_strings));
+	BUILD_BUG_ON(ARRAY_SIZE(stats->obj_created) != ARRAY_SIZE(stats->obj_deleted));
+	for (i = 0; i < ARRAY_SIZE(stats->obj_created); i++) {
+		if (stats->obj_created[i] || stats->obj_deleted[i])
+			buf += snprintf(buf, end - buf, "%s%s: active %d total %d\n", prefix,
+					binder_objstat_strings[i],
+					stats->obj_created[i] - stats->obj_deleted[i],
+					stats->obj_created[i]);
+		if (buf >= end)
+			return buf;
+	}
+	return buf;
+}
+
+static char *print_binder_proc_stats(char *buf, char *end, struct binder_proc *proc)
+{
+	struct binder_work *w;
+	struct rb_node *n;
+	int count, strong, weak;
+
+	buf += snprintf(buf, end - buf, "proc %d\n", proc->pid);
+	if (buf >= end)
+		return buf;
+	count = 0;
+	for (n = rb_first(&proc->threads); n != NULL; n = rb_next(n))
+		count++;
+	buf += snprintf(buf, end - buf, "  threads: %d\n", count);
+	if (buf >= end)
+		return buf;
+	buf += snprintf(buf, end - buf, "  requested threads: %d+%d/%d\n"
+			"  ready threads %d\n"
+			"  free async space %zd\n", proc->requested_threads,
+			proc->requested_threads_started, proc->max_threads,
+			proc->ready_threads, proc->free_async_space);
+	if (buf >= end)
+		return buf;
+	count = 0;
+	for (n = rb_first(&proc->nodes); n != NULL; n = rb_next(n))
+		count++;
+	buf += snprintf(buf, end - buf, "  nodes: %d\n", count);
+	if (buf >= end)
+		return buf;
+	count = 0;
+	strong = 0;
+	weak = 0;
+	for (n = rb_first(&proc->refs_by_desc); n != NULL; n = rb_next(n)) {
+		struct binder_ref *ref = rb_entry(n, struct binder_ref, rb_node_desc);
+		count++;
+		strong += ref->strong;
+		weak += ref->weak;
+	}
+	buf += snprintf(buf, end - buf, "  refs: %d s %d w %d\n", count, strong, weak);
+	if (buf >= end)
+		return buf;
+
+	count = 0;
+	for (n = rb_first(&proc->allocated_buffers); n != NULL; n = rb_next(n))
+		count++;
+	buf += snprintf(buf, end - buf, "  buffers: %d\n", count);
+	if (buf >= end)
+		return buf;
+
+	count = 0;
+	list_for_each_entry(w, &proc->todo, entry) {
+		switch (w->type) {
+		case BINDER_WORK_TRANSACTION:
+			count++;
+			break;
+		default:
+			break;
+		}
+	}
+	buf += snprintf(buf, end - buf, "  pending transactions: %d\n", count);
+	if (buf >= end)
+		return buf;
+
+	buf = print_binder_stats(buf, end, "  ", &proc->stats);
+
+	return buf;
+}
+
+
+static int binder_read_proc_state(
+	char *page, char **start, off_t off, int count, int *eof, void *data)
+{
+	struct binder_proc *proc;
+	struct hlist_node *pos;
+	struct binder_node *node;
+	int len = 0;
+	char *buf = page;
+	char *end = page + PAGE_SIZE;
+	int do_lock = !binder_debug_no_lock;
+
+	if (off)
+		return 0;
+
+	if (do_lock)
+		mutex_lock(&binder_lock);
+
+	buf += snprintf(buf, end - buf, "binder state:\n");
+
+	if (!hlist_empty(&binder_dead_nodes))
+		buf += snprintf(buf, end - buf, "dead nodes:\n");
+	hlist_for_each_entry(node, pos, &binder_dead_nodes, dead_node) {
+		if (buf >= end)
+			break;
+		buf = print_binder_node(buf, end, node);
+	}
+
+	hlist_for_each_entry(proc, pos, &binder_procs, proc_node) {
+		if (buf >= end)
+			break;
+		buf = print_binder_proc(buf, end, proc, 1);
+	}
+	if (do_lock)
+		mutex_unlock(&binder_lock);
+	if (buf > page + PAGE_SIZE)
+		buf = page + PAGE_SIZE;
+
+	*start = page + off;
+
+	len = buf - page;
+	if (len > off)
+		len -= off;
+	else
+		len = 0;
+
+	return len < count ? len  : count;
+}
+
+static int binder_read_proc_stats(
+	char *page, char **start, off_t off, int count, int *eof, void *data)
+{
+	struct binder_proc *proc;
+	struct hlist_node *pos;
+	int len = 0;
+	char *p = page;
+	int do_lock = !binder_debug_no_lock;
+
+	if (off)
+		return 0;
+
+	if (do_lock)
+		mutex_lock(&binder_lock);
+
+	p += snprintf(p, PAGE_SIZE, "binder stats:\n");
+
+	p = print_binder_stats(p, page + PAGE_SIZE, "", &binder_stats);
+
+	hlist_for_each_entry(proc, pos, &binder_procs, proc_node) {
+		if (p >= page + PAGE_SIZE)
+			break;
+		p = print_binder_proc_stats(p, page + PAGE_SIZE, proc);
+	}
+	if (do_lock)
+		mutex_unlock(&binder_lock);
+	if (p > page + PAGE_SIZE)
+		p = page + PAGE_SIZE;
+
+	*start = page + off;
+
+	len = p - page;
+	if (len > off)
+		len -= off;
+	else
+		len = 0;
+
+	return len < count ? len  : count;
+}
+
+static int binder_read_proc_transactions(
+	char *page, char **start, off_t off, int count, int *eof, void *data)
+{
+	struct binder_proc *proc;
+	struct hlist_node *pos;
+	int len = 0;
+	char *buf = page;
+	char *end = page + PAGE_SIZE;
+	int do_lock = !binder_debug_no_lock;
+
+	if (off)
+		return 0;
+
+	if (do_lock)
+		mutex_lock(&binder_lock);
+
+	buf += snprintf(buf, end - buf, "binder transactions:\n");
+	hlist_for_each_entry(proc, pos, &binder_procs, proc_node) {
+		if (buf >= end)
+			break;
+		buf = print_binder_proc(buf, end, proc, 0);
+	}
+	if (do_lock)
+		mutex_unlock(&binder_lock);
+	if (buf > page + PAGE_SIZE)
+		buf = page + PAGE_SIZE;
+
+	*start = page + off;
+
+	len = buf - page;
+	if (len > off)
+		len -= off;
+	else
+		len = 0;
+
+	return len < count ? len  : count;
+}
+
+static int binder_read_proc_proc(
+	char *page, char **start, off_t off, int count, int *eof, void *data)
+{
+	struct binder_proc *proc = data;
+	int len = 0;
+	char *p = page;
+	int do_lock = !binder_debug_no_lock;
+
+	if (off)
+		return 0;
+
+	if (do_lock)
+		mutex_lock(&binder_lock);
+	p += snprintf(p, PAGE_SIZE, "binder proc state:\n");
+	p = print_binder_proc(p, page + PAGE_SIZE, proc, 1);
+	if (do_lock)
+		mutex_unlock(&binder_lock);
+
+	if (p > page + PAGE_SIZE)
+		p = page + PAGE_SIZE;
+	*start = page + off;
+
+	len = p - page;
+	if (len > off)
+		len -= off;
+	else
+		len = 0;
+
+	return len < count ? len  : count;
+}
+
+static char *print_binder_transaction_log_entry(char *buf, char *end, struct binder_transaction_log_entry *e)
+{
+	buf += snprintf(buf, end - buf, "%d: %s from %d:%d to %d:%d node %d handle %d size %d:%d\n",
+			e->debug_id, (e->call_type == 2) ? "reply" :
+			((e->call_type == 1) ? "async" : "call "), e->from_proc,
+			e->from_thread, e->to_proc, e->to_thread, e->to_node,
+			e->target_handle, e->data_size, e->offsets_size);
+	return buf;
+}
+
+static int binder_read_proc_transaction_log(
+	char *page, char **start, off_t off, int count, int *eof, void *data)
+{
+	struct binder_transaction_log *log = data;
+	int len = 0;
+	int i;
+	char *buf = page;
+	char *end = page + PAGE_SIZE;
+
+	if (off)
+		return 0;
+
+	if (log->full) {
+		for (i = log->next; i < ARRAY_SIZE(log->entry); i++) {
+			if (buf >= end)
+				break;
+			buf = print_binder_transaction_log_entry(buf, end, &log->entry[i]);
+		}
+	}
+	for (i = 0; i < log->next; i++) {
+		if (buf >= end)
+			break;
+		buf = print_binder_transaction_log_entry(buf, end, &log->entry[i]);
+	}
+
+	*start = page + off;
+
+	len = buf - page;
+	if (len > off)
+		len -= off;
+	else
+		len = 0;
+
+	return len < count ? len  : count;
+}
+
+static struct file_operations binder_fops = {
+	.owner = THIS_MODULE,
+	.poll = binder_poll,
+	.unlocked_ioctl = binder_ioctl,
+	.mmap = binder_mmap,
+	.open = binder_open,
+	.flush = binder_flush,
+	.release = binder_release,
+};
+
+static struct miscdevice binder_miscdev = {
+	.minor = MISC_DYNAMIC_MINOR,
+	.name = "binder",
+	.fops = &binder_fops
+};
+
+static int __init binder_init(void)
+{
+	int ret;
+
+	binder_proc_dir_entry_root = proc_mkdir("binder", NULL);
+	if (binder_proc_dir_entry_root)
+		binder_proc_dir_entry_proc = proc_mkdir("proc", binder_proc_dir_entry_root);
+	ret = misc_register(&binder_miscdev);
+	if (binder_proc_dir_entry_root) {
+		create_proc_read_entry("state", S_IRUGO, binder_proc_dir_entry_root, binder_read_proc_state, NULL);
+		create_proc_read_entry("stats", S_IRUGO, binder_proc_dir_entry_root, binder_read_proc_stats, NULL);
+		create_proc_read_entry("transactions", S_IRUGO, binder_proc_dir_entry_root, binder_read_proc_transactions, NULL);
+		create_proc_read_entry("transaction_log", S_IRUGO, binder_proc_dir_entry_root, binder_read_proc_transaction_log, &binder_transaction_log);
+		create_proc_read_entry("failed_transaction_log", S_IRUGO, binder_proc_dir_entry_root, binder_read_proc_transaction_log, &binder_transaction_log_failed);
+	}
+	return ret;
+}
+
+device_initcall(binder_init);
+
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/android/binder.h b/drivers/staging/android/binder.h
new file mode 100644
index 00000000000..863ae1ad5d5
--- /dev/null
+++ b/drivers/staging/android/binder.h
@@ -0,0 +1,330 @@
+/*
+ * Copyright (C) 2008 Google, Inc.
+ *
+ * Based on, but no longer compatible with, the original
+ * OpenBinder.org binder driver interface, which is:
+ *
+ * Copyright (c) 2005 Palmsource, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef _LINUX_BINDER_H
+#define _LINUX_BINDER_H
+
+#include <linux/ioctl.h>
+
+#define B_PACK_CHARS(c1, c2, c3, c4) \
+	((((c1)<<24)) | (((c2)<<16)) | (((c3)<<8)) | (c4))
+#define B_TYPE_LARGE 0x85
+
+enum {
+	BINDER_TYPE_BINDER	= B_PACK_CHARS('s', 'b', '*', B_TYPE_LARGE),
+	BINDER_TYPE_WEAK_BINDER	= B_PACK_CHARS('w', 'b', '*', B_TYPE_LARGE),
+	BINDER_TYPE_HANDLE	= B_PACK_CHARS('s', 'h', '*', B_TYPE_LARGE),
+	BINDER_TYPE_WEAK_HANDLE	= B_PACK_CHARS('w', 'h', '*', B_TYPE_LARGE),
+	BINDER_TYPE_FD		= B_PACK_CHARS('f', 'd', '*', B_TYPE_LARGE),
+};
+
+enum {
+	FLAT_BINDER_FLAG_PRIORITY_MASK = 0xff,
+	FLAT_BINDER_FLAG_ACCEPTS_FDS = 0x100,
+};
+
+/*
+ * This is the flattened representation of a Binder object for transfer
+ * between processes.  The 'offsets' supplied as part of a binder transaction
+ * contains offsets into the data where these structures occur.  The Binder
+ * driver takes care of re-writing the structure type and data as it moves
+ * between processes.
+ */
+struct flat_binder_object {
+	/* 8 bytes for large_flat_header. */
+	unsigned long		type;
+	unsigned long		flags;
+
+	/* 8 bytes of data. */
+	union {
+		void		*binder;	/* local object */
+		signed long	handle;		/* remote object */
+	};
+
+	/* extra data associated with local object */
+	void			*cookie;
+};
+
+/*
+ * On 64-bit platforms where user code may run in 32-bits the driver must
+ * translate the buffer (and local binder) addresses apropriately.
+ */
+
+struct binder_write_read {
+	signed long	write_size;	/* bytes to write */
+	signed long	write_consumed;	/* bytes consumed by driver */
+	unsigned long	write_buffer;
+	signed long	read_size;	/* bytes to read */
+	signed long	read_consumed;	/* bytes consumed by driver */
+	unsigned long	read_buffer;
+};
+
+/* Use with BINDER_VERSION, driver fills in fields. */
+struct binder_version {
+	/* driver protocol version -- increment with incompatible change */
+	signed long	protocol_version;
+};
+
+/* This is the current protocol version. */
+#define BINDER_CURRENT_PROTOCOL_VERSION 7
+
+#define BINDER_WRITE_READ   		_IOWR('b', 1, struct binder_write_read)
+#define	BINDER_SET_IDLE_TIMEOUT		_IOW('b', 3, int64_t)
+#define	BINDER_SET_MAX_THREADS		_IOW('b', 5, size_t)
+#define	BINDER_SET_IDLE_PRIORITY	_IOW('b', 6, int)
+#define	BINDER_SET_CONTEXT_MGR		_IOW('b', 7, int)
+#define	BINDER_THREAD_EXIT		_IOW('b', 8, int)
+#define BINDER_VERSION			_IOWR('b', 9, struct binder_version)
+
+/*
+ * NOTE: Two special error codes you should check for when calling
+ * in to the driver are:
+ *
+ * EINTR -- The operation has been interupted.  This should be
+ * handled by retrying the ioctl() until a different error code
+ * is returned.
+ *
+ * ECONNREFUSED -- The driver is no longer accepting operations
+ * from your process.  That is, the process is being destroyed.
+ * You should handle this by exiting from your process.  Note
+ * that once this error code is returned, all further calls to
+ * the driver from any thread will return this same code.
+ */
+
+enum transaction_flags {
+	TF_ONE_WAY	= 0x01,	/* this is a one-way call: async, no return */
+	TF_ROOT_OBJECT	= 0x04,	/* contents are the component's root object */
+	TF_STATUS_CODE	= 0x08,	/* contents are a 32-bit status code */
+	TF_ACCEPT_FDS	= 0x10,	/* allow replies with file descriptors */
+};
+
+struct binder_transaction_data {
+	/* The first two are only used for bcTRANSACTION and brTRANSACTION,
+	 * identifying the target and contents of the transaction.
+	 */
+	union {
+		size_t	handle;	/* target descriptor of command transaction */
+		void	*ptr;	/* target descriptor of return transaction */
+	} target;
+	void		*cookie;	/* target object cookie */
+	unsigned int	code;		/* transaction command */
+
+	/* General information about the transaction. */
+	unsigned int	flags;
+	pid_t		sender_pid;
+	uid_t		sender_euid;
+	size_t		data_size;	/* number of bytes of data */
+	size_t		offsets_size;	/* number of bytes of offsets */
+
+	/* If this transaction is inline, the data immediately
+	 * follows here; otherwise, it ends with a pointer to
+	 * the data buffer.
+	 */
+	union {
+		struct {
+			/* transaction data */
+			const void	*buffer;
+			/* offsets from buffer to flat_binder_object structs */
+			const void	*offsets;
+		} ptr;
+		uint8_t	buf[8];
+	} data;
+};
+
+struct binder_ptr_cookie {
+	void *ptr;
+	void *cookie;
+};
+
+struct binder_pri_desc {
+	int priority;
+	int desc;
+};
+
+struct binder_pri_ptr_cookie {
+	int priority;
+	void *ptr;
+	void *cookie;
+};
+
+enum BinderDriverReturnProtocol {
+	BR_ERROR = _IOR('r', 0, int),
+	/*
+	 * int: error code
+	 */
+
+	BR_OK = _IO('r', 1),
+	/* No parameters! */
+
+	BR_TRANSACTION = _IOR('r', 2, struct binder_transaction_data),
+	BR_REPLY = _IOR('r', 3, struct binder_transaction_data),
+	/*
+	 * binder_transaction_data: the received command.
+	 */
+
+	BR_ACQUIRE_RESULT = _IOR('r', 4, int),
+	/*
+	 * not currently supported
+	 * int: 0 if the last bcATTEMPT_ACQUIRE was not successful.
+	 * Else the remote object has acquired a primary reference.
+	 */
+
+	BR_DEAD_REPLY = _IO('r', 5),
+	/*
+	 * The target of the last transaction (either a bcTRANSACTION or
+	 * a bcATTEMPT_ACQUIRE) is no longer with us.  No parameters.
+	 */
+
+	BR_TRANSACTION_COMPLETE = _IO('r', 6),
+	/*
+	 * No parameters... always refers to the last transaction requested
+	 * (including replies).  Note that this will be sent even for
+	 * asynchronous transactions.
+	 */
+
+	BR_INCREFS = _IOR('r', 7, struct binder_ptr_cookie),
+	BR_ACQUIRE = _IOR('r', 8, struct binder_ptr_cookie),
+	BR_RELEASE = _IOR('r', 9, struct binder_ptr_cookie),
+	BR_DECREFS = _IOR('r', 10, struct binder_ptr_cookie),
+	/*
+	 * void *:	ptr to binder
+	 * void *: cookie for binder
+	 */
+
+	BR_ATTEMPT_ACQUIRE = _IOR('r', 11, struct binder_pri_ptr_cookie),
+	/*
+	 * not currently supported
+	 * int:	priority
+	 * void *: ptr to binder
+	 * void *: cookie for binder
+	 */
+
+	BR_NOOP = _IO('r', 12),
+	/*
+	 * No parameters.  Do nothing and examine the next command.  It exists
+	 * primarily so that we can replace it with a BR_SPAWN_LOOPER command.
+	 */
+
+	BR_SPAWN_LOOPER = _IO('r', 13),
+	/*
+	 * No parameters.  The driver has determined that a process has no
+	 * threads waiting to service incomming transactions.  When a process
+	 * receives this command, it must spawn a new service thread and
+	 * register it via bcENTER_LOOPER.
+	 */
+
+	BR_FINISHED = _IO('r', 14),
+	/*
+	 * not currently supported
+	 * stop threadpool thread
+	 */
+
+	BR_DEAD_BINDER = _IOR('r', 15, void *),
+	/*
+	 * void *: cookie
+	 */
+	BR_CLEAR_DEATH_NOTIFICATION_DONE = _IOR('r', 16, void *),
+	/*
+	 * void *: cookie
+	 */
+
+	BR_FAILED_REPLY = _IO('r', 17),
+	/*
+	 * The the last transaction (either a bcTRANSACTION or
+	 * a bcATTEMPT_ACQUIRE) failed (e.g. out of memory).  No parameters.
+	 */
+};
+
+enum BinderDriverCommandProtocol {
+	BC_TRANSACTION = _IOW('c', 0, struct binder_transaction_data),
+	BC_REPLY = _IOW('c', 1, struct binder_transaction_data),
+	/*
+	 * binder_transaction_data: the sent command.
+	 */
+
+	BC_ACQUIRE_RESULT = _IOW('c', 2, int),
+	/*
+	 * not currently supported
+	 * int:  0 if the last BR_ATTEMPT_ACQUIRE was not successful.
+	 * Else you have acquired a primary reference on the object.
+	 */
+
+	BC_FREE_BUFFER = _IOW('c', 3, int),
+	/*
+	 * void *: ptr to transaction data received on a read
+	 */
+
+	BC_INCREFS = _IOW('c', 4, int),
+	BC_ACQUIRE = _IOW('c', 5, int),
+	BC_RELEASE = _IOW('c', 6, int),
+	BC_DECREFS = _IOW('c', 7, int),
+	/*
+	 * int:	descriptor
+	 */
+
+	BC_INCREFS_DONE = _IOW('c', 8, struct binder_ptr_cookie),
+	BC_ACQUIRE_DONE = _IOW('c', 9, struct binder_ptr_cookie),
+	/*
+	 * void *: ptr to binder
+	 * void *: cookie for binder
+	 */
+
+	BC_ATTEMPT_ACQUIRE = _IOW('c', 10, struct binder_pri_desc),
+	/*
+	 * not currently supported
+	 * int: priority
+	 * int: descriptor
+	 */
+
+	BC_REGISTER_LOOPER = _IO('c', 11),
+	/*
+	 * No parameters.
+	 * Register a spawned looper thread with the device.
+	 */
+
+	BC_ENTER_LOOPER = _IO('c', 12),
+	BC_EXIT_LOOPER = _IO('c', 13),
+	/*
+	 * No parameters.
+	 * These two commands are sent as an application-level thread
+	 * enters and exits the binder loop, respectively.  They are
+	 * used so the binder can have an accurate count of the number
+	 * of looping threads it has available.
+	 */
+
+	BC_REQUEST_DEATH_NOTIFICATION = _IOW('c', 14, struct binder_ptr_cookie),
+	/*
+	 * void *: ptr to binder
+	 * void *: cookie
+	 */
+
+	BC_CLEAR_DEATH_NOTIFICATION = _IOW('c', 15, struct binder_ptr_cookie),
+	/*
+	 * void *: ptr to binder
+	 * void *: cookie
+	 */
+
+	BC_DEAD_BINDER_DONE = _IOW('c', 16, void *),
+	/*
+	 * void *: cookie
+	 */
+};
+
+#endif /* _LINUX_BINDER_H */
+
diff --git a/drivers/staging/android/logger.c b/drivers/staging/android/logger.c
new file mode 100644
index 00000000000..ab32003ecd0
--- /dev/null
+++ b/drivers/staging/android/logger.c
@@ -0,0 +1,607 @@
+/*
+ * drivers/misc/logger.c
+ *
+ * A Logging Subsystem
+ *
+ * Copyright (C) 2007-2008 Google, Inc.
+ *
+ * Robert Love <rlove@google.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/module.h>
+#include <linux/fs.h>
+#include <linux/miscdevice.h>
+#include <linux/uaccess.h>
+#include <linux/poll.h>
+#include <linux/time.h>
+#include "logger.h"
+
+#include <asm/ioctls.h>
+
+/*
+ * struct logger_log - represents a specific log, such as 'main' or 'radio'
+ *
+ * This structure lives from module insertion until module removal, so it does
+ * not need additional reference counting. The structure is protected by the
+ * mutex 'mutex'.
+ */
+struct logger_log {
+	unsigned char *		buffer;	/* the ring buffer itself */
+	struct miscdevice	misc;	/* misc device representing the log */
+	wait_queue_head_t	wq;	/* wait queue for readers */
+	struct list_head	readers; /* this log's readers */
+	struct mutex		mutex;	/* mutex protecting buffer */
+	size_t			w_off;	/* current write head offset */
+	size_t			head;	/* new readers start here */
+	size_t			size;	/* size of the log */
+};
+
+/*
+ * struct logger_reader - a logging device open for reading
+ *
+ * This object lives from open to release, so we don't need additional
+ * reference counting. The structure is protected by log->mutex.
+ */
+struct logger_reader {
+	struct logger_log *	log;	/* associated log */
+	struct list_head	list;	/* entry in logger_log's list */
+	size_t			r_off;	/* current read head offset */
+};
+
+/* logger_offset - returns index 'n' into the log via (optimized) modulus */
+#define logger_offset(n)	((n) & (log->size - 1))
+
+/*
+ * file_get_log - Given a file structure, return the associated log
+ *
+ * This isn't aesthetic. We have several goals:
+ *
+ * 	1) Need to quickly obtain the associated log during an I/O operation
+ * 	2) Readers need to maintain state (logger_reader)
+ * 	3) Writers need to be very fast (open() should be a near no-op)
+ *
+ * In the reader case, we can trivially go file->logger_reader->logger_log.
+ * For a writer, we don't want to maintain a logger_reader, so we just go
+ * file->logger_log. Thus what file->private_data points at depends on whether
+ * or not the file was opened for reading. This function hides that dirtiness.
+ */
+static inline struct logger_log * file_get_log(struct file *file)
+{
+	if (file->f_mode & FMODE_READ) {
+		struct logger_reader *reader = file->private_data;
+		return reader->log;
+	} else
+		return file->private_data;
+}
+
+/*
+ * get_entry_len - Grabs the length of the payload of the next entry starting
+ * from 'off'.
+ *
+ * Caller needs to hold log->mutex.
+ */
+static __u32 get_entry_len(struct logger_log *log, size_t off)
+{
+	__u16 val;
+
+	switch (log->size - off) {
+	case 1:
+		memcpy(&val, log->buffer + off, 1);
+		memcpy(((char *) &val) + 1, log->buffer, 1);
+		break;
+	default:
+		memcpy(&val, log->buffer + off, 2);
+	}
+
+	return sizeof(struct logger_entry) + val;
+}
+
+/*
+ * do_read_log_to_user - reads exactly 'count' bytes from 'log' into the
+ * user-space buffer 'buf'. Returns 'count' on success.
+ *
+ * Caller must hold log->mutex.
+ */
+static ssize_t do_read_log_to_user(struct logger_log *log,
+				   struct logger_reader *reader,
+				   char __user *buf,
+				   size_t count)
+{
+	size_t len;
+
+	/*
+	 * We read from the log in two disjoint operations. First, we read from
+	 * the current read head offset up to 'count' bytes or to the end of
+	 * the log, whichever comes first.
+	 */
+	len = min(count, log->size - reader->r_off);
+	if (copy_to_user(buf, log->buffer + reader->r_off, len))
+		return -EFAULT;
+
+	/*
+	 * Second, we read any remaining bytes, starting back at the head of
+	 * the log.
+	 */
+	if (count != len)
+		if (copy_to_user(buf + len, log->buffer, count - len))
+			return -EFAULT;
+
+	reader->r_off = logger_offset(reader->r_off + count);
+
+	return count;
+}
+
+/*
+ * logger_read - our log's read() method
+ *
+ * Behavior:
+ *
+ * 	- O_NONBLOCK works
+ * 	- If there are no log entries to read, blocks until log is written to
+ * 	- Atomically reads exactly one log entry
+ *
+ * Optimal read size is LOGGER_ENTRY_MAX_LEN. Will set errno to EINVAL if read
+ * buffer is insufficient to hold next entry.
+ */
+static ssize_t logger_read(struct file *file, char __user *buf,
+			   size_t count, loff_t *pos)
+{
+	struct logger_reader *reader = file->private_data;
+	struct logger_log *log = reader->log;
+	ssize_t ret;
+	DEFINE_WAIT(wait);
+
+start:
+	while (1) {
+		prepare_to_wait(&log->wq, &wait, TASK_INTERRUPTIBLE);
+
+		mutex_lock(&log->mutex);
+		ret = (log->w_off == reader->r_off);
+		mutex_unlock(&log->mutex);
+		if (!ret)
+			break;
+
+		if (file->f_flags & O_NONBLOCK) {
+			ret = -EAGAIN;
+			break;
+		}
+
+		if (signal_pending(current)) {
+			ret = -EINTR;
+			break;
+		}
+
+		schedule();
+	}
+
+	finish_wait(&log->wq, &wait);
+	if (ret)
+		return ret;
+
+	mutex_lock(&log->mutex);
+
+	/* is there still something to read or did we race? */
+	if (unlikely(log->w_off == reader->r_off)) {
+		mutex_unlock(&log->mutex);
+		goto start;
+	}
+
+	/* get the size of the next entry */
+	ret = get_entry_len(log, reader->r_off);
+	if (count < ret) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	/* get exactly one entry from the log */
+	ret = do_read_log_to_user(log, reader, buf, ret);
+
+out:
+	mutex_unlock(&log->mutex);
+
+	return ret;
+}
+
+/*
+ * get_next_entry - return the offset of the first valid entry at least 'len'
+ * bytes after 'off'.
+ *
+ * Caller must hold log->mutex.
+ */
+static size_t get_next_entry(struct logger_log *log, size_t off, size_t len)
+{
+	size_t count = 0;
+
+	do {
+		size_t nr = get_entry_len(log, off);
+		off = logger_offset(off + nr);
+		count += nr;
+	} while (count < len);
+
+	return off;
+}
+
+/*
+ * clock_interval - is a < c < b in mod-space? Put another way, does the line
+ * from a to b cross c?
+ */
+static inline int clock_interval(size_t a, size_t b, size_t c)
+{
+	if (b < a) {
+		if (a < c || b >= c)
+			return 1;
+	} else {
+		if (a < c && b >= c)
+			return 1;
+	}
+
+	return 0;
+}
+
+/*
+ * fix_up_readers - walk the list of all readers and "fix up" any who were
+ * lapped by the writer; also do the same for the default "start head".
+ * We do this by "pulling forward" the readers and start head to the first
+ * entry after the new write head.
+ *
+ * The caller needs to hold log->mutex.
+ */
+static void fix_up_readers(struct logger_log *log, size_t len)
+{
+	size_t old = log->w_off;
+	size_t new = logger_offset(old + len);
+	struct logger_reader *reader;
+
+	if (clock_interval(old, new, log->head))
+		log->head = get_next_entry(log, log->head, len);
+
+	list_for_each_entry(reader, &log->readers, list)
+		if (clock_interval(old, new, reader->r_off))
+			reader->r_off = get_next_entry(log, reader->r_off, len);
+}
+
+/*
+ * do_write_log - writes 'len' bytes from 'buf' to 'log'
+ *
+ * The caller needs to hold log->mutex.
+ */
+static void do_write_log(struct logger_log *log, const void *buf, size_t count)
+{
+	size_t len;
+
+	len = min(count, log->size - log->w_off);
+	memcpy(log->buffer + log->w_off, buf, len);
+
+	if (count != len)
+		memcpy(log->buffer, buf + len, count - len);
+
+	log->w_off = logger_offset(log->w_off + count);
+
+}
+
+/*
+ * do_write_log_user - writes 'len' bytes from the user-space buffer 'buf' to
+ * the log 'log'
+ *
+ * The caller needs to hold log->mutex.
+ *
+ * Returns 'count' on success, negative error code on failure.
+ */
+static ssize_t do_write_log_from_user(struct logger_log *log,
+				      const void __user *buf, size_t count)
+{
+	size_t len;
+
+	len = min(count, log->size - log->w_off);
+	if (len && copy_from_user(log->buffer + log->w_off, buf, len))
+		return -EFAULT;
+
+	if (count != len)
+		if (copy_from_user(log->buffer, buf + len, count - len))
+			return -EFAULT;
+
+	log->w_off = logger_offset(log->w_off + count);
+
+	return count;
+}
+
+/*
+ * logger_aio_write - our write method, implementing support for write(),
+ * writev(), and aio_write(). Writes are our fast path, and we try to optimize
+ * them above all else.
+ */
+ssize_t logger_aio_write(struct kiocb *iocb, const struct iovec *iov,
+			 unsigned long nr_segs, loff_t ppos)
+{
+	struct logger_log *log = file_get_log(iocb->ki_filp);
+	size_t orig = log->w_off;
+	struct logger_entry header;
+	struct timespec now;
+	ssize_t ret = 0;
+
+	now = current_kernel_time();
+
+	header.pid = current->tgid;
+	header.tid = current->pid;
+	header.sec = now.tv_sec;
+	header.nsec = now.tv_nsec;
+	header.len = min_t(size_t, iocb->ki_left, LOGGER_ENTRY_MAX_PAYLOAD);
+
+	/* null writes succeed, return zero */
+	if (unlikely(!header.len))
+		return 0;
+
+	mutex_lock(&log->mutex);
+
+	/*
+	 * Fix up any readers, pulling them forward to the first readable
+	 * entry after (what will be) the new write offset. We do this now
+	 * because if we partially fail, we can end up with clobbered log
+	 * entries that encroach on readable buffer.
+	 */
+	fix_up_readers(log, sizeof(struct logger_entry) + header.len);
+
+	do_write_log(log, &header, sizeof(struct logger_entry));
+
+	while (nr_segs-- > 0) {
+		size_t len;
+		ssize_t nr;
+
+		/* figure out how much of this vector we can keep */
+		len = min_t(size_t, iov->iov_len, header.len - ret);
+
+		/* write out this segment's payload */
+		nr = do_write_log_from_user(log, iov->iov_base, len);
+		if (unlikely(nr < 0)) {
+			log->w_off = orig;
+			mutex_unlock(&log->mutex);
+			return nr;
+		}
+
+		iov++;
+		ret += nr;
+	}
+
+	mutex_unlock(&log->mutex);
+
+	/* wake up any blocked readers */
+	wake_up_interruptible(&log->wq);
+
+	return ret;
+}
+
+static struct logger_log * get_log_from_minor(int);
+
+/*
+ * logger_open - the log's open() file operation
+ *
+ * Note how near a no-op this is in the write-only case. Keep it that way!
+ */
+static int logger_open(struct inode *inode, struct file *file)
+{
+	struct logger_log *log;
+	int ret;
+
+	ret = nonseekable_open(inode, file);
+	if (ret)
+		return ret;
+
+	log = get_log_from_minor(MINOR(inode->i_rdev));
+	if (!log)
+		return -ENODEV;
+
+	if (file->f_mode & FMODE_READ) {
+		struct logger_reader *reader;
+
+		reader = kmalloc(sizeof(struct logger_reader), GFP_KERNEL);
+		if (!reader)
+			return -ENOMEM;
+
+		reader->log = log;
+		INIT_LIST_HEAD(&reader->list);
+
+		mutex_lock(&log->mutex);
+		reader->r_off = log->head;
+		list_add_tail(&reader->list, &log->readers);
+		mutex_unlock(&log->mutex);
+
+		file->private_data = reader;
+	} else
+		file->private_data = log;
+
+	return 0;
+}
+
+/*
+ * logger_release - the log's release file operation
+ *
+ * Note this is a total no-op in the write-only case. Keep it that way!
+ */
+static int logger_release(struct inode *ignored, struct file *file)
+{
+	if (file->f_mode & FMODE_READ) {
+		struct logger_reader *reader = file->private_data;
+		list_del(&reader->list);
+		kfree(reader);
+	}
+
+	return 0;
+}
+
+/*
+ * logger_poll - the log's poll file operation, for poll/select/epoll
+ *
+ * Note we always return POLLOUT, because you can always write() to the log.
+ * Note also that, strictly speaking, a return value of POLLIN does not
+ * guarantee that the log is readable without blocking, as there is a small
+ * chance that the writer can lap the reader in the interim between poll()
+ * returning and the read() request.
+ */
+static unsigned int logger_poll(struct file *file, poll_table *wait)
+{
+	struct logger_reader *reader;
+	struct logger_log *log;
+	unsigned int ret = POLLOUT | POLLWRNORM;
+
+	if (!(file->f_mode & FMODE_READ))
+		return ret;
+
+	reader = file->private_data;
+	log = reader->log;
+
+	poll_wait(file, &log->wq, wait);
+
+	mutex_lock(&log->mutex);
+	if (log->w_off != reader->r_off)
+		ret |= POLLIN | POLLRDNORM;
+	mutex_unlock(&log->mutex);
+
+	return ret;
+}
+
+static long logger_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	struct logger_log *log = file_get_log(file);
+	struct logger_reader *reader;
+	long ret = -ENOTTY;
+
+	mutex_lock(&log->mutex);
+
+	switch (cmd) {
+	case LOGGER_GET_LOG_BUF_SIZE:
+		ret = log->size;
+		break;
+	case LOGGER_GET_LOG_LEN:
+		if (!(file->f_mode & FMODE_READ)) {
+			ret = -EBADF;
+			break;
+		}
+		reader = file->private_data;
+		if (log->w_off >= reader->r_off)
+			ret = log->w_off - reader->r_off;
+		else
+			ret = (log->size - reader->r_off) + log->w_off;
+		break;
+	case LOGGER_GET_NEXT_ENTRY_LEN:
+		if (!(file->f_mode & FMODE_READ)) {
+			ret = -EBADF;
+			break;
+		}
+		reader = file->private_data;
+		if (log->w_off != reader->r_off)
+			ret = get_entry_len(log, reader->r_off);
+		else
+			ret = 0;
+		break;
+	case LOGGER_FLUSH_LOG:
+		if (!(file->f_mode & FMODE_WRITE)) {
+			ret = -EBADF;
+			break;
+		}
+		list_for_each_entry(reader, &log->readers, list)
+			reader->r_off = log->w_off;
+		log->head = log->w_off;
+		ret = 0;
+		break;
+	}
+
+	mutex_unlock(&log->mutex);
+
+	return ret;
+}
+
+static struct file_operations logger_fops = {
+	.owner = THIS_MODULE,
+	.read = logger_read,
+	.aio_write = logger_aio_write,
+	.poll = logger_poll,
+	.unlocked_ioctl = logger_ioctl,
+	.compat_ioctl = logger_ioctl,
+	.open = logger_open,
+	.release = logger_release,
+};
+
+/*
+ * Defines a log structure with name 'NAME' and a size of 'SIZE' bytes, which
+ * must be a power of two, greater than LOGGER_ENTRY_MAX_LEN, and less than
+ * LONG_MAX minus LOGGER_ENTRY_MAX_LEN.
+ */
+#define DEFINE_LOGGER_DEVICE(VAR, NAME, SIZE) \
+static unsigned char _buf_ ## VAR[SIZE]; \
+static struct logger_log VAR = { \
+	.buffer = _buf_ ## VAR, \
+	.misc = { \
+		.minor = MISC_DYNAMIC_MINOR, \
+		.name = NAME, \
+		.fops = &logger_fops, \
+		.parent = NULL, \
+	}, \
+	.wq = __WAIT_QUEUE_HEAD_INITIALIZER(VAR .wq), \
+	.readers = LIST_HEAD_INIT(VAR .readers), \
+	.mutex = __MUTEX_INITIALIZER(VAR .mutex), \
+	.w_off = 0, \
+	.head = 0, \
+	.size = SIZE, \
+};
+
+DEFINE_LOGGER_DEVICE(log_main, LOGGER_LOG_MAIN, 64*1024)
+DEFINE_LOGGER_DEVICE(log_events, LOGGER_LOG_EVENTS, 256*1024)
+DEFINE_LOGGER_DEVICE(log_radio, LOGGER_LOG_RADIO, 64*1024)
+
+static struct logger_log * get_log_from_minor(int minor)
+{
+	if (log_main.misc.minor == minor)
+		return &log_main;
+	if (log_events.misc.minor == minor)
+		return &log_events;
+	if (log_radio.misc.minor == minor)
+		return &log_radio;
+	return NULL;
+}
+
+static int __init init_log(struct logger_log *log)
+{
+	int ret;
+
+	ret = misc_register(&log->misc);
+	if (unlikely(ret)) {
+		printk(KERN_ERR "logger: failed to register misc "
+		       "device for log '%s'!\n", log->misc.name);
+		return ret;
+	}
+
+	printk(KERN_INFO "logger: created %luK log '%s'\n",
+	       (unsigned long) log->size >> 10, log->misc.name);
+
+	return 0;
+}
+
+static int __init logger_init(void)
+{
+	int ret;
+
+	ret = init_log(&log_main);
+	if (unlikely(ret))
+		goto out;
+
+	ret = init_log(&log_events);
+	if (unlikely(ret))
+		goto out;
+
+	ret = init_log(&log_radio);
+	if (unlikely(ret))
+		goto out;
+
+out:
+	return ret;
+}
+device_initcall(logger_init);
diff --git a/drivers/staging/android/logger.h b/drivers/staging/android/logger.h
new file mode 100644
index 00000000000..a562434d741
--- /dev/null
+++ b/drivers/staging/android/logger.h
@@ -0,0 +1,48 @@
+/* include/linux/logger.h
+ *
+ * Copyright (C) 2007-2008 Google, Inc.
+ * Author: Robert Love <rlove@android.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef _LINUX_LOGGER_H
+#define _LINUX_LOGGER_H
+
+#include <linux/types.h>
+#include <linux/ioctl.h>
+
+struct logger_entry {
+	__u16		len;	/* length of the payload */
+	__u16		__pad;	/* no matter what, we get 2 bytes of padding */
+	__s32		pid;	/* generating process's pid */
+	__s32		tid;	/* generating process's tid */
+	__s32		sec;	/* seconds since Epoch */
+	__s32		nsec;	/* nanoseconds */
+	char		msg[0];	/* the entry's payload */
+};
+
+#define LOGGER_LOG_RADIO	"log_radio"	/* radio-related messages */
+#define LOGGER_LOG_EVENTS	"log_events"	/* system/hardware events */
+#define LOGGER_LOG_MAIN		"log_main"	/* everything else */
+
+#define LOGGER_ENTRY_MAX_LEN		(4*1024)
+#define LOGGER_ENTRY_MAX_PAYLOAD	\
+	(LOGGER_ENTRY_MAX_LEN - sizeof(struct logger_entry))
+
+#define __LOGGERIO	0xAE
+
+#define LOGGER_GET_LOG_BUF_SIZE		_IO(__LOGGERIO, 1) /* size of log */
+#define LOGGER_GET_LOG_LEN		_IO(__LOGGERIO, 2) /* used log len */
+#define LOGGER_GET_NEXT_ENTRY_LEN	_IO(__LOGGERIO, 3) /* next entry len */
+#define LOGGER_FLUSH_LOG		_IO(__LOGGERIO, 4) /* flush log */
+
+#endif /* _LINUX_LOGGER_H */
diff --git a/drivers/staging/android/lowmemorykiller.c b/drivers/staging/android/lowmemorykiller.c
new file mode 100644
index 00000000000..3715d56ca96
--- /dev/null
+++ b/drivers/staging/android/lowmemorykiller.c
@@ -0,0 +1,119 @@
+/* drivers/misc/lowmemorykiller.c
+ *
+ * Copyright (C) 2007-2008 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/oom.h>
+#include <linux/sched.h>
+
+static int lowmem_shrink(int nr_to_scan, gfp_t gfp_mask);
+
+static struct shrinker lowmem_shrinker = {
+	.shrink = lowmem_shrink,
+	.seeks = DEFAULT_SEEKS * 16
+};
+static uint32_t lowmem_debug_level = 2;
+static int lowmem_adj[6] = {
+	0,
+	1,
+	6,
+	12,
+};
+static int lowmem_adj_size = 4;
+static size_t lowmem_minfree[6] = {
+	3*512, // 6MB
+	2*1024, // 8MB
+	4*1024, // 16MB
+	16*1024, // 64MB
+};
+static int lowmem_minfree_size = 4;
+
+#define lowmem_print(level, x...) do { if(lowmem_debug_level >= (level)) printk(x); } while(0)
+
+module_param_named(cost, lowmem_shrinker.seeks, int, S_IRUGO | S_IWUSR);
+module_param_array_named(adj, lowmem_adj, int, &lowmem_adj_size, S_IRUGO | S_IWUSR);
+module_param_array_named(minfree, lowmem_minfree, uint, &lowmem_minfree_size, S_IRUGO | S_IWUSR);
+module_param_named(debug_level, lowmem_debug_level, uint, S_IRUGO | S_IWUSR);
+
+static int lowmem_shrink(int nr_to_scan, gfp_t gfp_mask)
+{
+	struct task_struct *p;
+	struct task_struct *selected = NULL;
+	int rem = 0;
+	int tasksize;
+	int i;
+	int min_adj = OOM_ADJUST_MAX + 1;
+	int selected_tasksize = 0;
+	int array_size = ARRAY_SIZE(lowmem_adj);
+	int other_free = global_page_state(NR_FREE_PAGES) + global_page_state(NR_FILE_PAGES);
+	if(lowmem_adj_size < array_size)
+		array_size = lowmem_adj_size;
+	if(lowmem_minfree_size < array_size)
+		array_size = lowmem_minfree_size;
+	for(i = 0; i < array_size; i++) {
+		if(other_free < lowmem_minfree[i]) {
+			min_adj = lowmem_adj[i];
+			break;
+		}
+	}
+	if(nr_to_scan > 0)
+		lowmem_print(3, "lowmem_shrink %d, %x, ofree %d, ma %d\n", nr_to_scan, gfp_mask, other_free, min_adj);
+	read_lock(&tasklist_lock);
+	for_each_process(p) {
+		if(p->oomkilladj >= 0 && p->mm) {
+			tasksize = get_mm_rss(p->mm);
+			if(nr_to_scan > 0 && tasksize > 0 && p->oomkilladj >= min_adj) {
+				if(selected == NULL ||
+				   p->oomkilladj > selected->oomkilladj ||
+				   (p->oomkilladj == selected->oomkilladj &&
+				    tasksize > selected_tasksize)) {
+					selected = p;
+					selected_tasksize = tasksize;
+					lowmem_print(2, "select %d (%s), adj %d, size %d, to kill\n",
+					             p->pid, p->comm, p->oomkilladj, tasksize);
+				}
+			}
+			rem += tasksize;
+		}
+	}
+	if(selected != NULL) {
+		lowmem_print(1, "send sigkill to %d (%s), adj %d, size %d\n",
+		             selected->pid, selected->comm,
+		             selected->oomkilladj, selected_tasksize);
+		force_sig(SIGKILL, selected);
+		rem -= selected_tasksize;
+	}
+	lowmem_print(4, "lowmem_shrink %d, %x, return %d\n", nr_to_scan, gfp_mask, rem);
+	read_unlock(&tasklist_lock);
+	return rem;
+}
+
+static int __init lowmem_init(void)
+{
+	register_shrinker(&lowmem_shrinker);
+	return 0;
+}
+
+static void __exit lowmem_exit(void)
+{
+	unregister_shrinker(&lowmem_shrinker);
+}
+
+module_init(lowmem_init);
+module_exit(lowmem_exit);
+
+MODULE_LICENSE("GPL");
+
diff --git a/drivers/staging/android/ram_console.c b/drivers/staging/android/ram_console.c
new file mode 100644
index 00000000000..bf006857a87
--- /dev/null
+++ b/drivers/staging/android/ram_console.c
@@ -0,0 +1,395 @@
+/* drivers/android/ram_console.c
+ *
+ * Copyright (C) 2007-2008 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/console.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/proc_fs.h>
+#include <linux/string.h>
+#include <linux/uaccess.h>
+#include <asm/io.h>
+
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+#include <linux/rslib.h>
+#endif
+
+struct ram_console_buffer {
+	uint32_t    sig;
+	uint32_t    start;
+	uint32_t    size;
+	uint8_t     data[0];
+};
+
+#define RAM_CONSOLE_SIG (0x43474244) /* DBGC */
+
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_EARLY_INIT
+static char __initdata
+	ram_console_old_log_init_buffer[CONFIG_ANDROID_RAM_CONSOLE_EARLY_SIZE];
+#endif
+static char *ram_console_old_log;
+static size_t ram_console_old_log_size;
+
+static struct ram_console_buffer *ram_console_buffer;
+static size_t ram_console_buffer_size;
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+static char *ram_console_par_buffer;
+static struct rs_control *ram_console_rs_decoder;
+static int ram_console_corrected_bytes;
+static int ram_console_bad_blocks;
+#define ECC_BLOCK_SIZE CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION_DATA_SIZE
+#define ECC_SIZE CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION_ECC_SIZE
+#define ECC_SYMSIZE CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION_SYMBOL_SIZE
+#define ECC_POLY CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION_POLYNOMIAL
+#endif
+
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+static void ram_console_encode_rs8(uint8_t *data, size_t len, uint8_t *ecc)
+{
+	int i;
+	uint16_t par[ECC_SIZE];
+	/* Initialize the parity buffer */
+	memset(par, 0, sizeof(par));
+	encode_rs8(ram_console_rs_decoder, data, len, par, 0);
+	for (i = 0; i < ECC_SIZE; i++)
+		ecc[i] = par[i];
+}
+
+static int ram_console_decode_rs8(void *data, size_t len, uint8_t *ecc)
+{
+	int i;
+	uint16_t par[ECC_SIZE];
+	for (i = 0; i < ECC_SIZE; i++)
+		par[i] = ecc[i];
+	return decode_rs8(ram_console_rs_decoder, data, par, len,
+				NULL, 0, NULL, 0, NULL);
+}
+#endif
+
+static void ram_console_update(const char *s, unsigned int count)
+{
+	struct ram_console_buffer *buffer = ram_console_buffer;
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+	uint8_t *buffer_end = buffer->data + ram_console_buffer_size;
+	uint8_t *block;
+	uint8_t *par;
+	int size = ECC_BLOCK_SIZE;
+#endif
+	memcpy(buffer->data + buffer->start, s, count);
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+	block = buffer->data + (buffer->start & ~(ECC_BLOCK_SIZE - 1));
+	par = ram_console_par_buffer +
+	      (buffer->start / ECC_BLOCK_SIZE) * ECC_SIZE;
+	do {
+		if (block + ECC_BLOCK_SIZE > buffer_end)
+			size = buffer_end - block;
+		ram_console_encode_rs8(block, size, par);
+		block += ECC_BLOCK_SIZE;
+		par += ECC_SIZE;
+	} while (block < buffer->data + buffer->start + count);
+#endif
+}
+
+static void ram_console_update_header(void)
+{
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+	struct ram_console_buffer *buffer = ram_console_buffer;
+	uint8_t *par;
+	par = ram_console_par_buffer +
+	      DIV_ROUND_UP(ram_console_buffer_size, ECC_BLOCK_SIZE) * ECC_SIZE;
+	ram_console_encode_rs8((uint8_t *)buffer, sizeof(*buffer), par);
+#endif
+}
+
+static void
+ram_console_write(struct console *console, const char *s, unsigned int count)
+{
+	int rem;
+	struct ram_console_buffer *buffer = ram_console_buffer;
+
+	if (count > ram_console_buffer_size) {
+		s += count - ram_console_buffer_size;
+		count = ram_console_buffer_size;
+	}
+	rem = ram_console_buffer_size - buffer->start;
+	if (rem < count) {
+		ram_console_update(s, rem);
+		s += rem;
+		count -= rem;
+		buffer->start = 0;
+		buffer->size = ram_console_buffer_size;
+	}
+	ram_console_update(s, count);
+
+	buffer->start += count;
+	if (buffer->size < ram_console_buffer_size)
+		buffer->size += count;
+	ram_console_update_header();
+}
+
+static struct console ram_console = {
+	.name	= "ram",
+	.write	= ram_console_write,
+	.flags	= CON_PRINTBUFFER | CON_ENABLED,
+	.index	= -1,
+};
+
+static void __init
+ram_console_save_old(struct ram_console_buffer *buffer, char *dest)
+{
+	size_t old_log_size = buffer->size;
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+	uint8_t *block;
+	uint8_t *par;
+	char strbuf[80];
+	int strbuf_len;
+
+	block = buffer->data;
+	par = ram_console_par_buffer;
+	while (block < buffer->data + buffer->size) {
+		int numerr;
+		int size = ECC_BLOCK_SIZE;
+		if (block + size > buffer->data + ram_console_buffer_size)
+			size = buffer->data + ram_console_buffer_size - block;
+		numerr = ram_console_decode_rs8(block, size, par);
+		if (numerr > 0) {
+#if 0
+			printk(KERN_INFO "ram_console: error in block %p, %d\n",
+			       block, numerr);
+#endif
+			ram_console_corrected_bytes += numerr;
+		} else if (numerr < 0) {
+#if 0
+			printk(KERN_INFO "ram_console: uncorrectable error in "
+			       "block %p\n", block);
+#endif
+			ram_console_bad_blocks++;
+		}
+		block += ECC_BLOCK_SIZE;
+		par += ECC_SIZE;
+	}
+	if (ram_console_corrected_bytes || ram_console_bad_blocks)
+		strbuf_len = snprintf(strbuf, sizeof(strbuf),
+			"\n%d Corrected bytes, %d unrecoverable blocks\n",
+			ram_console_corrected_bytes, ram_console_bad_blocks);
+	else
+		strbuf_len = snprintf(strbuf, sizeof(strbuf),
+				      "\nNo errors detected\n");
+	if (strbuf_len >= sizeof(strbuf))
+		strbuf_len = sizeof(strbuf) - 1;
+	old_log_size += strbuf_len;
+#endif
+
+	if (dest == NULL) {
+		dest = kmalloc(old_log_size, GFP_KERNEL);
+		if (dest == NULL) {
+			printk(KERN_ERR
+			       "ram_console: failed to allocate buffer\n");
+			return;
+		}
+	}
+
+	ram_console_old_log = dest;
+	ram_console_old_log_size = old_log_size;
+	memcpy(ram_console_old_log,
+	       &buffer->data[buffer->start], buffer->size - buffer->start);
+	memcpy(ram_console_old_log + buffer->size - buffer->start,
+	       &buffer->data[0], buffer->start);
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+	memcpy(ram_console_old_log + old_log_size - strbuf_len,
+	       strbuf, strbuf_len);
+#endif
+}
+
+static int __init ram_console_init(struct ram_console_buffer *buffer,
+				   size_t buffer_size, char *old_buf)
+{
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+	int numerr;
+	uint8_t *par;
+#endif
+	ram_console_buffer = buffer;
+	ram_console_buffer_size =
+		buffer_size - sizeof(struct ram_console_buffer);
+
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ERROR_CORRECTION
+	ram_console_buffer_size -= (DIV_ROUND_UP(ram_console_buffer_size,
+						ECC_BLOCK_SIZE) + 1) * ECC_SIZE;
+	ram_console_par_buffer = buffer->data + ram_console_buffer_size;
+
+
+	/* first consecutive root is 0
+	 * primitive element to generate roots = 1
+	 */
+	ram_console_rs_decoder = init_rs(ECC_SYMSIZE, ECC_POLY, 0, 1, ECC_SIZE);
+	if (ram_console_rs_decoder == NULL) {
+		printk(KERN_INFO "ram_console: init_rs failed\n");
+		return 0;
+	}
+
+	ram_console_corrected_bytes = 0;
+	ram_console_bad_blocks = 0;
+
+	par = ram_console_par_buffer +
+	      DIV_ROUND_UP(ram_console_buffer_size, ECC_BLOCK_SIZE) * ECC_SIZE;
+
+	numerr = ram_console_decode_rs8(buffer, sizeof(*buffer), par);
+	if (numerr > 0) {
+		printk(KERN_INFO "ram_console: error in header, %d\n", numerr);
+		ram_console_corrected_bytes += numerr;
+	} else if (numerr < 0) {
+		printk(KERN_INFO
+		       "ram_console: uncorrectable error in header\n");
+		ram_console_bad_blocks++;
+	}
+#endif
+
+	if (buffer->sig == RAM_CONSOLE_SIG) {
+		if (buffer->size > ram_console_buffer_size
+		    || buffer->start > buffer->size)
+			printk(KERN_INFO "ram_console: found existing invalid "
+			       "buffer, size %d, start %d\n",
+			       buffer->size, buffer->start);
+		else {
+			printk(KERN_INFO "ram_console: found existing buffer, "
+			       "size %d, start %d\n",
+			       buffer->size, buffer->start);
+			ram_console_save_old(buffer, old_buf);
+		}
+	} else {
+		printk(KERN_INFO "ram_console: no valid data in buffer "
+		       "(sig = 0x%08x)\n", buffer->sig);
+	}
+
+	buffer->sig = RAM_CONSOLE_SIG;
+	buffer->start = 0;
+	buffer->size = 0;
+
+	register_console(&ram_console);
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_ENABLE_VERBOSE
+	console_verbose();
+#endif
+	return 0;
+}
+
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_EARLY_INIT
+static int __init ram_console_early_init(void)
+{
+	return ram_console_init((struct ram_console_buffer *)
+		CONFIG_ANDROID_RAM_CONSOLE_EARLY_ADDR,
+		CONFIG_ANDROID_RAM_CONSOLE_EARLY_SIZE,
+		ram_console_old_log_init_buffer);
+}
+#else
+static int ram_console_driver_probe(struct platform_device *pdev)
+{
+	struct resource *res = pdev->resource;
+	size_t start;
+	size_t buffer_size;
+	void *buffer;
+
+	if (res == NULL || pdev->num_resources != 1 ||
+	    !(res->flags & IORESOURCE_MEM)) {
+		printk(KERN_ERR "ram_console: invalid resource, %p %d flags "
+		       "%lx\n", res, pdev->num_resources, res ? res->flags : 0);
+		return -ENXIO;
+	}
+	buffer_size = res->end - res->start + 1;
+	start = res->start;
+	printk(KERN_INFO "ram_console: got buffer at %x, size %x\n",
+	       start, buffer_size);
+	buffer = ioremap(res->start, buffer_size);
+	if (buffer == NULL) {
+		printk(KERN_ERR "ram_console: failed to map memory\n");
+		return -ENOMEM;
+	}
+
+	return ram_console_init(buffer, buffer_size, NULL/* allocate */);
+}
+
+static struct platform_driver ram_console_driver = {
+	.probe = ram_console_driver_probe,
+	.driver		= {
+		.name	= "ram_console",
+	},
+};
+
+static int __init ram_console_module_init(void)
+{
+	int err;
+	err = platform_driver_register(&ram_console_driver);
+	return err;
+}
+#endif
+
+static ssize_t ram_console_read_old(struct file *file, char __user *buf,
+				    size_t len, loff_t *offset)
+{
+	loff_t pos = *offset;
+	ssize_t count;
+
+	if (pos >= ram_console_old_log_size)
+		return 0;
+
+	count = min(len, (size_t)(ram_console_old_log_size - pos));
+	if (copy_to_user(buf, ram_console_old_log + pos, count))
+		return -EFAULT;
+
+	*offset += count;
+	return count;
+}
+
+static struct file_operations ram_console_file_ops = {
+	.owner = THIS_MODULE,
+	.read = ram_console_read_old,
+};
+
+static int __init ram_console_late_init(void)
+{
+	struct proc_dir_entry *entry;
+
+	if (ram_console_old_log == NULL)
+		return 0;
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_EARLY_INIT
+	ram_console_old_log = kmalloc(ram_console_old_log_size, GFP_KERNEL);
+	if (ram_console_old_log == NULL) {
+		printk(KERN_ERR
+		       "ram_console: failed to allocate buffer for old log\n");
+		ram_console_old_log_size = 0;
+		return 0;
+	}
+	memcpy(ram_console_old_log,
+	       ram_console_old_log_init_buffer, ram_console_old_log_size);
+#endif
+	entry = create_proc_entry("last_kmsg", S_IFREG | S_IRUGO, NULL);
+	if (!entry) {
+		printk(KERN_ERR "ram_console: failed to create proc entry\n");
+		kfree(ram_console_old_log);
+		ram_console_old_log = NULL;
+		return 0;
+	}
+
+	entry->proc_fops = &ram_console_file_ops;
+	entry->size = ram_console_old_log_size;
+	return 0;
+}
+
+#ifdef CONFIG_ANDROID_RAM_CONSOLE_EARLY_INIT
+console_initcall(ram_console_early_init);
+#else
+module_init(ram_console_module_init);
+#endif
+late_initcall(ram_console_late_init);
+
diff --git a/drivers/staging/android/timed_gpio.c b/drivers/staging/android/timed_gpio.c
new file mode 100644
index 00000000000..bea68c9fc94
--- /dev/null
+++ b/drivers/staging/android/timed_gpio.c
@@ -0,0 +1,177 @@
+/* drivers/misc/timed_gpio.c
+ *
+ * Copyright (C) 2008 Google, Inc.
+ * Author: Mike Lockwood <lockwood@android.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/hrtimer.h>
+#include <linux/err.h>
+#include <asm/arch/gpio.h>
+
+#include "timed_gpio.h"
+
+
+static struct class *timed_gpio_class;
+
+struct timed_gpio_data {
+	struct device *dev;
+	struct hrtimer timer;
+	spinlock_t lock;
+	unsigned 	gpio;
+	int 		max_timeout;
+	u8 		active_low;
+};
+
+static enum hrtimer_restart gpio_timer_func(struct hrtimer *timer)
+{
+	struct timed_gpio_data *gpio_data = container_of(timer, struct timed_gpio_data, timer);
+
+	gpio_direction_output(gpio_data->gpio, gpio_data->active_low ? 1 : 0);
+	return HRTIMER_NORESTART;
+}
+
+static ssize_t gpio_enable_show(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct timed_gpio_data *gpio_data = dev_get_drvdata(dev);
+	int remaining;
+
+	if (hrtimer_active(&gpio_data->timer)) {
+		ktime_t r = hrtimer_get_remaining(&gpio_data->timer);
+		remaining = r.tv.sec * 1000 + r.tv.nsec / 1000000;
+	} else
+		remaining = 0;
+
+	return sprintf(buf, "%d\n", remaining);
+}
+
+static ssize_t gpio_enable_store(
+		struct device *dev, struct device_attribute *attr,
+		const char *buf, size_t size)
+{
+	struct timed_gpio_data *gpio_data = dev_get_drvdata(dev);
+	int value;
+	unsigned long	flags;
+
+	sscanf(buf, "%d", &value);
+
+	spin_lock_irqsave(&gpio_data->lock, flags);
+
+	/* cancel previous timer and set GPIO according to value */
+	hrtimer_cancel(&gpio_data->timer);
+	gpio_direction_output(gpio_data->gpio, gpio_data->active_low ? !value : !!value);
+
+	if (value > 0) {
+		if (value > gpio_data->max_timeout)
+			value = gpio_data->max_timeout;
+
+		hrtimer_start(&gpio_data->timer,
+						ktime_set(value / 1000, (value % 1000) * 1000000),
+						HRTIMER_MODE_REL);
+	}
+
+	spin_unlock_irqrestore(&gpio_data->lock, flags);
+
+	return size;
+}
+
+static DEVICE_ATTR(enable, S_IRUGO | S_IWUSR, gpio_enable_show, gpio_enable_store);
+
+static int timed_gpio_probe(struct platform_device *pdev)
+{
+	struct timed_gpio_platform_data *pdata = pdev->dev.platform_data;
+	struct timed_gpio *cur_gpio;
+	struct timed_gpio_data *gpio_data, *gpio_dat;
+	int i, ret = 0;
+
+	if (!pdata)
+		return -EBUSY;
+
+	gpio_data = kzalloc(sizeof(struct timed_gpio_data) * pdata->num_gpios, GFP_KERNEL);
+	if (!gpio_data)
+		return -ENOMEM;
+
+	for (i = 0; i < pdata->num_gpios; i++) {
+		cur_gpio = &pdata->gpios[i];
+		gpio_dat = &gpio_data[i];
+
+		hrtimer_init(&gpio_dat->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+		gpio_dat->timer.function = gpio_timer_func;
+		spin_lock_init(&gpio_dat->lock);
+
+		gpio_dat->gpio = cur_gpio->gpio;
+		gpio_dat->max_timeout = cur_gpio->max_timeout;
+		gpio_dat->active_low = cur_gpio->active_low;
+		gpio_direction_output(gpio_dat->gpio, gpio_dat->active_low);
+
+		gpio_dat->dev = device_create(timed_gpio_class, &pdev->dev, 0, "%s", cur_gpio->name);
+		if (unlikely(IS_ERR(gpio_dat->dev)))
+			return PTR_ERR(gpio_dat->dev);
+
+		dev_set_drvdata(gpio_dat->dev, gpio_dat);
+		ret = device_create_file(gpio_dat->dev, &dev_attr_enable);
+		if (ret)
+			return ret;
+	}
+
+	platform_set_drvdata(pdev, gpio_data);
+
+	return 0;
+}
+
+static int timed_gpio_remove(struct platform_device *pdev)
+{
+	struct timed_gpio_platform_data *pdata = pdev->dev.platform_data;
+	struct timed_gpio_data *gpio_data = platform_get_drvdata(pdev);
+	int i;
+
+	for (i = 0; i < pdata->num_gpios; i++) {
+		device_remove_file(gpio_data[i].dev, &dev_attr_enable);
+		device_unregister(gpio_data[i].dev);
+	}
+
+	kfree(gpio_data);
+
+	return 0;
+}
+
+static struct platform_driver timed_gpio_driver = {
+	.probe		= timed_gpio_probe,
+	.remove		= timed_gpio_remove,
+	.driver		= {
+		.name		= "timed-gpio",
+		.owner		= THIS_MODULE,
+	},
+};
+
+static int __init timed_gpio_init(void)
+{
+	timed_gpio_class = class_create(THIS_MODULE, "timed_output");
+	if (IS_ERR(timed_gpio_class))
+		return PTR_ERR(timed_gpio_class);
+	return platform_driver_register(&timed_gpio_driver);
+}
+
+static void __exit timed_gpio_exit(void)
+{
+	class_destroy(timed_gpio_class);
+	platform_driver_unregister(&timed_gpio_driver);
+}
+
+module_init(timed_gpio_init);
+module_exit(timed_gpio_exit);
+
+MODULE_AUTHOR("Mike Lockwood <lockwood@android.com>");
+MODULE_DESCRIPTION("timed gpio driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/android/timed_gpio.h b/drivers/staging/android/timed_gpio.h
new file mode 100644
index 00000000000..78449b2161c
--- /dev/null
+++ b/drivers/staging/android/timed_gpio.h
@@ -0,0 +1,31 @@
+/* include/linux/timed_gpio.h
+ *
+ * Copyright (C) 2008 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+*/
+
+#ifndef _LINUX_TIMED_GPIO_H
+#define _LINUX_TIMED_GPIO_H
+
+struct timed_gpio {
+	const char *name;
+	unsigned 	gpio;
+	int     max_timeout;
+	u8 		active_low;
+};
+
+struct timed_gpio_platform_data {
+	int 		num_gpios;
+	struct timed_gpio *gpios;
+};
+
+#endif
diff --git a/drivers/staging/asus_oled/Kconfig b/drivers/staging/asus_oled/Kconfig
new file mode 100644
index 00000000000..e56dbb25ac5
--- /dev/null
+++ b/drivers/staging/asus_oled/Kconfig
@@ -0,0 +1,6 @@
+config ASUS_OLED
+	tristate "Asus OLED driver"
+	depends on USB
+	default N
+	---help---
+	  Enable support for the OLED display present in some Asus laptops.
diff --git a/drivers/staging/asus_oled/Makefile b/drivers/staging/asus_oled/Makefile
new file mode 100644
index 00000000000..e71f9aa9e03
--- /dev/null
+++ b/drivers/staging/asus_oled/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_ASUS_OLED)		+= asus_oled.o
diff --git a/drivers/staging/asus_oled/README b/drivers/staging/asus_oled/README
new file mode 100644
index 00000000000..96b9717f168
--- /dev/null
+++ b/drivers/staging/asus_oled/README
@@ -0,0 +1,156 @@
+
+    Driver for Asus OLED display present in some Asus laptops.
+
+    The code of this driver is based on 'asusoled' program taken from
+    https://launchpad.net/asusoled/. I just wanted to have a simple
+    kernel driver for controlling this device, but I didn't know how
+    to do that. Now I know ;) Also, that program can not be used
+    with usbhid loaded, which means no USB mouse/keyboard while
+    controlling OLED display :(
+
+    It has been tested on Asus G1 and didn't cause any problems,
+    but I don't guarantee that it won't do anything wrong :)
+
+    It can (and probably does) have errors. It is usable
+    in my case, and I hope others will find it useful too!
+
+*******
+
+Building the module
+
+   To build the module you need kernel 2.6 include files and some C compiler.
+
+   Just run:
+   make
+   make install (as a root)
+
+   It will build (hopefully) the module and install it in
+   /lib/modules/'uname -r'/extra/asus_oled.ko.
+
+   To load it just use:
+   modprobe asus_oled
+
+   You can check if it has detected your OLED display by looking into dmesg output.
+   There should be something like this:
+   asus-oled 2-7:1.0: Attached Asus OLED device
+
+   If it doesn't find your display, you can try removing usbhid module.
+   If you add asus_oled into the list of modules loaded during system boot
+   before usbhid, it will work even when usbhid is present.
+
+   If it still doesn't detect your hardware, check lsusb output.
+   There should be similar line:
+   Bus 002 Device 005: ID 0b05:1726 ASUSTek Computer, Inc.
+
+   If you don't see any lines with '0b05:1726' it means that you have different
+   type of hardware that is not detected (it may or may not work, but the driver
+   knows only '0b05:1726' device).
+
+*******
+
+Configuration
+
+   There is only one option: start_off.
+   You can use it by: 'modprobe asus_oled start_off=1', or by adding this
+   line to /etc/modprobe.conf:
+   options asus_oled start_off=1
+
+   With this option provided, asus_oled driver will switch off the display
+   when it is detected and attached. It is nice feature to just switch off the 'ASUS'
+   logo. If you don't use the display, it is probably the good idea to switch it off,
+   to protect OLEDs from "wearing off".
+
+*******
+
+Usage
+
+   This module can be controlled with two special files:
+   /sys/class/asus_oled/oled_N/enabled
+   /sys/class/asus_oled/oled_N/picture
+
+   (N is the device number, the first, and probably the only, has number 1,
+    so it is /sys/class/asus_oled/oled_1/enabled
+    and /sys/class/asus_oled/oled_1/picture)
+
+   'enabled' files is for reading and writing, 'picture' is writeable only.
+
+   You can write 0 or 1 to 'enabled' file, which will switch
+   on and off the display. Reading from this file will tell you the last
+   status set, either 0 or 1. By default it is 1, so if the device was set to 'off',
+   and the computer was rebooted without power-off, this file will contain wrong
+   value - because the device is off, but hasn't been disabled this time and is
+   assumed to be on...
+
+   To 'picture' file you write pictures to be displayed by the OLED device.
+   The format of the file:
+   <M:WxH>
+   00001110010111000
+   00010101010101010
+   ....
+
+   First line is a configuration parameter. Meaning of fields in <M:WxH>:
+   M - picture mode. It can be either 's' for static pictures,
+       'r' for rolling pictures, and 'f' for flashing pictures.
+   W - width of the picture. May be between 1 and 1792
+   H - height of the picture. May be between 1 and 32
+
+   For example <s:128x32> means static picture, 128 pixels long and 32 pixels high.
+
+   The physical size of the display is 128x32 pixels. Static and flashing pictures
+   can't be larger than that (actually they can, but only part of them will be displayed ;) )
+
+   If the picture is smaller than 128x32 it will be centered. Rolling pictures wider than
+   128 pixels will be centered too, unless their width = n*128. Vertically they will be
+   centered just like static pictures, if their height is smaller than 32.
+
+   Flashing pictures will be centered horizontally if their width < 128, but they were
+   centered vertically in a different way. If their height < 16, they will be centered
+   in the upper half of the display (rows 0-15). This is because only the first half
+   of flashing pictures is used for flashing. When the picture with heigh = 32 is
+   displayed in flashing mode, its upper 16 rows will be flashing in the upper half
+   of the display, and the lower half will be empty. After few seconds upper part will
+   stop flashing (but that part of the picture will remain there), and the lower
+   half of the display will start displayin the lower half of the picture
+   in rolling mode, unless it is empty, or the picture was small enough to fit in
+   upper part. It is not mine idea, this is just the way Asus' display work ;)
+   So if you need just flashing, use at most 128x16 picture. If you need flashing and
+   rolling, use whole size of the display.
+
+   Lines following the first, configuration, line are picture data. Each '1' means
+   that the pixel is lit, and '0' means that it is not. You can also use '#' as ON,
+   and ' ' (space) as OFF. Empty lines and all other characters are ignored.
+
+   It is possible to write everything in one line <M:WxH>01010101010101010...,
+   and W*H characters will be used. If there is not enough characters, nothing will be
+   displayed. However, the 'line mode' is easier to read (and write), and it also
+   lets to omit parts of data. Whenever End-Of-Line character is found, but
+   the line is not W characters long, it is assumed that all missing characters
+   are equal to the last character in the line.
+
+   Following line represents '0', '1' and a lots of '0's, dependng on the width of the picture
+   provided in configuration data:
+   010
+
+   So if you need empty line, it is sufficient to write line with only one '0' in it.
+   The same works with '1' (or ' ' and '#').
+
+   If there are too many data in the file, they will be ignored. If you are not sure
+   how many characters you are missing, you can add few lines with one zero in each of them.
+
+   There are some example pictures in .txt format, that can be used as follows:
+   cat foo.txt > /sys/class/asus_oled/oled_1/picture
+
+   If the display is switched off you also need to run:
+   echo 1 > /sys/class/asus_oled/oled_1/enabled
+   To switch it off, just use:
+   echo 0 > /sys/class/asus_oled/oled_1/enabled
+
+
+*******
+
+   For any additional info please have a look at http://lapsus.berlios.de/asus_oled.html
+
+
+
+   Jakub Schmidtke (sjakub@gmail.com)
+
diff --git a/drivers/staging/asus_oled/TODO b/drivers/staging/asus_oled/TODO
new file mode 100644
index 00000000000..2514131670a
--- /dev/null
+++ b/drivers/staging/asus_oled/TODO
@@ -0,0 +1,10 @@
+TODO:
+	- checkpatch.pl cleanups
+	- sparse fixes
+	- audit the userspace interface
+		- sysfs vs. char?
+	- Documentation/ABI/ needs to be added
+	- put the sample .txt files and README file somewhere.
+
+Please send patches to Greg Kroah-Hartman <greg@kroah.com> and
+Cc: Jakub Schmidtke <sjakub@gmail.com>
diff --git a/drivers/staging/asus_oled/asus_oled.c b/drivers/staging/asus_oled/asus_oled.c
new file mode 100644
index 00000000000..666a186e212
--- /dev/null
+++ b/drivers/staging/asus_oled/asus_oled.c
@@ -0,0 +1,745 @@
+/*
+ *  Asus OLED USB driver
+ *
+ *  Copyright (C) 2007,2008 Jakub Schmidtke (sjakub@gmail.com)
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc.,
+ *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ *
+ *
+ *  This module is based on usbled and asus-laptop modules.
+ *
+ *
+ *  Asus OLED support is based on asusoled program taken from
+ *  https://launchpad.net/asusoled/.
+ *
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/usb.h>
+#include <linux/platform_device.h>
+#include <linux/ctype.h>
+
+#define ASUS_OLED_VERSION		"0.04-dev"
+#define ASUS_OLED_NAME			"asus-oled"
+#define ASUS_OLED_UNDERSCORE_NAME	"asus_oled"
+
+#define ASUS_OLED_ERROR			"Asus OLED Display Error: "
+
+#define ASUS_OLED_STATIC		's'
+#define ASUS_OLED_ROLL			'r'
+#define ASUS_OLED_FLASH			'f'
+
+#define ASUS_OLED_MAX_WIDTH		1792
+#define ASUS_OLED_DISP_HEIGHT		32
+#define ASUS_OLED_PACKET_BUF_SIZE	256
+
+MODULE_AUTHOR("Jakub Schmidtke, sjakub@gmail.com");
+MODULE_DESCRIPTION("Asus OLED Driver v" ASUS_OLED_VERSION);
+MODULE_LICENSE("GPL");
+
+static struct class *oled_class = 0;
+static int oled_num = 0;
+
+static uint start_off = 0;
+
+module_param(start_off, uint, 0644);
+
+MODULE_PARM_DESC(start_off, "Set to 1 to switch off OLED display after it is attached");
+
+typedef enum {
+	PACK_MODE_G1,
+	PACK_MODE_G50,
+	PACK_MODE_LAST
+} oled_pack_mode_t;
+
+struct oled_dev_desc_str {
+	uint16_t		idVendor;
+	uint16_t		idProduct;
+	uint16_t		devWidth; // width of display
+	oled_pack_mode_t	packMode; // formula to be used while packing the picture
+	const char		*devDesc;
+};
+
+/* table of devices that work with this driver */
+static struct usb_device_id id_table [] = {
+	{ USB_DEVICE(0x0b05, 0x1726) }, // Asus G1/G2 (and variants)
+	{ USB_DEVICE(0x0b05, 0x175b) }, // Asus G50V (and possibly others - G70? G71?)
+	{ },
+};
+
+/* parameters of specific devices */
+static struct oled_dev_desc_str oled_dev_desc_table [] = {
+	{ 0x0b05, 0x1726, 128, PACK_MODE_G1, "G1/G2" },
+	{ 0x0b05, 0x175b, 256, PACK_MODE_G50, "G50" },
+	{ },
+};
+
+MODULE_DEVICE_TABLE (usb, id_table);
+
+#define SETUP_PACKET_HEADER(packet, val1, val2, val3, val4, val5, val6, val7) \
+	do {					\
+		memset(packet, 0, sizeof(struct asus_oled_header));		\
+		packet->header.magic1 = 0x55;		\
+		packet->header.magic2 = 0xaa;		\
+		packet->header.flags = val1;		\
+		packet->header.value3 = val2;		\
+		packet->header.buffer1 = val3;		\
+		packet->header.buffer2 = val4;		\
+		packet->header.value6 = val5;		\
+		packet->header.value7 = val6;		\
+		packet->header.value8 = val7;		\
+	} while(0);
+
+struct asus_oled_header {
+	uint8_t		magic1;
+	uint8_t		magic2;
+	uint8_t		flags;
+	uint8_t		value3;
+	uint8_t		buffer1;
+	uint8_t		buffer2;
+	uint8_t		value6;
+	uint8_t		value7;
+	uint8_t		value8;
+	uint8_t		padding2[7];
+} __attribute((packed));
+
+struct asus_oled_packet {
+	struct asus_oled_header		header;
+	uint8_t				bitmap[ASUS_OLED_PACKET_BUF_SIZE];
+} __attribute((packed));
+
+struct asus_oled_dev {
+	struct usb_device *	udev;
+	uint8_t			pic_mode;
+	uint16_t		dev_width;
+	oled_pack_mode_t	pack_mode;
+	size_t			height;
+	size_t			width;
+	size_t			x_shift;
+	size_t			y_shift;
+	size_t			buf_offs;
+	uint8_t			last_val;
+	size_t			buf_size;
+	char			*buf;
+	uint8_t			enabled;
+	struct device		*dev;
+};
+
+static void enable_oled(struct asus_oled_dev *odev, uint8_t enabl)
+{
+	int a;
+	int retval;
+	int act_len;
+	struct asus_oled_packet * packet;
+
+	packet = kzalloc(sizeof(struct asus_oled_packet), GFP_KERNEL);
+
+	if (!packet) {
+		dev_err(&odev->udev->dev, "out of memory\n");
+		return;
+	}
+
+	SETUP_PACKET_HEADER(packet, 0x20, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00);
+
+	if (enabl) packet->bitmap[0] = 0xaf;
+	else packet->bitmap[0] = 0xae;
+
+	for (a=0; a<1; a++) {
+		retval = usb_bulk_msg(odev->udev,
+			usb_sndbulkpipe(odev->udev, 2),
+			packet,
+			sizeof(struct asus_oled_header) + 1,
+			&act_len,
+			-1);
+
+		if (retval)
+			dev_dbg(&odev->udev->dev, "retval = %d\n", retval);
+	}
+
+	odev->enabled = enabl;
+
+	kfree(packet);
+}
+
+static ssize_t set_enabled(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct usb_interface *intf = to_usb_interface(dev);
+	struct asus_oled_dev *odev = usb_get_intfdata(intf);
+	int temp = simple_strtoul(buf, NULL, 10);
+
+	enable_oled(odev, temp);
+
+	return count;
+}
+
+static ssize_t class_set_enabled(struct device *device, struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct asus_oled_dev *odev = (struct asus_oled_dev *) dev_get_drvdata(device);
+
+	int temp = simple_strtoul(buf, NULL, 10);
+
+	enable_oled(odev, temp);
+
+	return count;
+}
+
+static ssize_t get_enabled(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct usb_interface *intf = to_usb_interface(dev);
+	struct asus_oled_dev *odev = usb_get_intfdata(intf);
+
+	return sprintf(buf, "%d\n", odev->enabled);
+}
+
+static ssize_t class_get_enabled(struct device *device, struct device_attribute *attr, char *buf)
+{
+	struct asus_oled_dev *odev = (struct asus_oled_dev *) dev_get_drvdata(device);
+
+	return sprintf(buf, "%d\n", odev->enabled);
+}
+
+static void send_packets(struct usb_device *udev, struct asus_oled_packet *packet,
+	char *buf, uint8_t p_type, size_t p_num)
+{
+	size_t i;
+	int act_len;
+
+	for (i = 0; i < p_num; i++) {
+		int retval;
+
+		switch (p_type) {
+			case ASUS_OLED_ROLL:
+				SETUP_PACKET_HEADER(packet, 0x40, 0x80, p_num, i + 1, 0x00, 0x01, 0xff);
+			break;
+			case ASUS_OLED_STATIC:
+				SETUP_PACKET_HEADER(packet, 0x10 + i, 0x80, 0x01, 0x01, 0x00, 0x01, 0x00);
+			break;
+			case ASUS_OLED_FLASH:
+				SETUP_PACKET_HEADER(packet, 0x10 + i, 0x80, 0x01, 0x01, 0x00, 0x00, 0xff);
+			break;
+		}
+
+		memcpy(packet->bitmap, buf + (ASUS_OLED_PACKET_BUF_SIZE*i), ASUS_OLED_PACKET_BUF_SIZE);
+
+		retval = usb_bulk_msg(udev,
+			usb_sndctrlpipe(udev, 2),
+			packet,
+			sizeof(struct asus_oled_packet),
+			&act_len,
+			-1);
+
+		if (retval)
+			dev_dbg(&udev->dev, "retval = %d\n", retval);
+	}
+}
+
+static void send_packet(struct usb_device *udev, struct asus_oled_packet *packet, size_t offset, size_t len, char *buf, uint8_t b1, uint8_t b2, uint8_t b3, uint8_t b4, uint8_t b5, uint8_t b6){
+	int retval;
+	int act_len;
+
+	SETUP_PACKET_HEADER(packet, b1, b2, b3, b4, b5, b6, 0x00);
+	memcpy(packet->bitmap, buf + offset, len);
+
+	retval = usb_bulk_msg(udev,
+			usb_sndctrlpipe(udev, 2),
+			packet,
+			sizeof(struct asus_oled_packet),
+			&act_len,
+			-1);
+
+	if (retval)
+		dev_dbg(&udev->dev, "retval = %d\n", retval);
+}
+
+
+static void send_packets_g50(struct usb_device *udev, struct asus_oled_packet *packet, char *buf)
+{
+	send_packet(udev, packet,     0, 0x100, buf, 0x10, 0x00, 0x02, 0x01, 0x00, 0x01);
+	send_packet(udev, packet, 0x100, 0x080, buf, 0x10, 0x00, 0x02, 0x02, 0x80, 0x00);
+
+	send_packet(udev, packet, 0x180, 0x100, buf, 0x11, 0x00, 0x03, 0x01, 0x00, 0x01);
+	send_packet(udev, packet, 0x280, 0x100, buf, 0x11, 0x00, 0x03, 0x02, 0x00, 0x01);
+	send_packet(udev, packet, 0x380, 0x080, buf, 0x11, 0x00, 0x03, 0x03, 0x80, 0x00);
+}
+
+
+static void send_data(struct asus_oled_dev *odev)
+{
+	size_t packet_num = odev->buf_size / ASUS_OLED_PACKET_BUF_SIZE;
+	struct asus_oled_packet * packet;
+
+	packet = kzalloc(sizeof(struct asus_oled_packet), GFP_KERNEL);
+
+	if (!packet) {
+		dev_err(&odev->udev->dev, "out of memory\n");
+		return;
+	}
+
+	if (odev->pack_mode==PACK_MODE_G1){
+		// When sending roll-mode data the display updated only first packet.
+		// I have no idea why, but when static picture is send just before
+		// rolling picture - everything works fine.
+		if (odev->pic_mode == ASUS_OLED_ROLL)
+			send_packets(odev->udev, packet, odev->buf, ASUS_OLED_STATIC, 2);
+
+		// Only ROLL mode can use more than 2 packets.
+		if (odev->pic_mode != ASUS_OLED_ROLL && packet_num > 2)
+			packet_num = 2;
+
+		send_packets(odev->udev, packet, odev->buf, odev->pic_mode, packet_num);
+	}
+	else
+	if (odev->pack_mode==PACK_MODE_G50){
+		send_packets_g50(odev->udev, packet, odev->buf);
+	}
+
+	kfree(packet);
+}
+
+static int append_values(struct asus_oled_dev *odev, uint8_t val, size_t count)
+{
+	while (count-- > 0) {
+		if (val) {
+			size_t x = odev->buf_offs % odev->width;
+			size_t y = odev->buf_offs / odev->width;
+			size_t i;
+
+			x += odev->x_shift;
+			y += odev->y_shift;
+
+			switch(odev->pack_mode)
+			{
+				case PACK_MODE_G1:
+					// i = (x/128)*640 + 127 - x + (y/8)*128;
+					// This one for 128 is the same, but might be better for different widths?
+					i = (x/odev->dev_width)*640 + odev->dev_width - 1 - x + (y/8)*odev->dev_width;
+				break;
+
+				case PACK_MODE_G50:
+					i =  (odev->dev_width - 1 - x)/8 + y*odev->dev_width/8;
+				break;
+
+				default:
+					i = 0;
+					printk(ASUS_OLED_ERROR "Unknown OLED Pack Mode: %d!\n", odev->pack_mode);
+				break;
+			}
+
+			if (i >= odev->buf_size) {
+				printk(ASUS_OLED_ERROR "Buffer overflow! Report a bug in the driver: offs: %d >= %d i: %d (x: %d y: %d)\n",
+					(int) odev->buf_offs, (int) odev->buf_size, (int) i, (int) x, (int) y);
+				return -EIO;
+			}
+
+			switch (odev->pack_mode)
+			{
+				case PACK_MODE_G1:
+					odev->buf[i] &= ~(1<<(y%8));
+				break;
+
+				case PACK_MODE_G50:
+					odev->buf[i] &= ~(1<<(x%8));
+				break;
+
+				default:
+					// cannot get here; stops gcc complaining
+				;
+			}
+		}
+
+		odev->last_val = val;
+		odev->buf_offs++;
+	}
+
+	return 0;
+}
+
+static ssize_t odev_set_picture(struct asus_oled_dev *odev, const char *buf, size_t count)
+{
+	size_t offs = 0, max_offs;
+
+	if (count < 1) return 0;
+
+	if (tolower(buf[0]) == 'b'){
+	    // binary mode, set the entire memory
+
+	    size_t i;
+
+	    odev->buf_size = (odev->dev_width * ASUS_OLED_DISP_HEIGHT) / 8;
+
+	    if (odev->buf) kfree(odev->buf);
+	    odev->buf = kmalloc(odev->buf_size, GFP_KERNEL);
+
+	    memset(odev->buf, 0xff, odev->buf_size);
+
+	    for (i=1; i < count && i<=32*32; i++){
+		odev->buf[i-1] = buf[i];
+		odev->buf_offs = i-1;
+	    }
+
+	    odev->width=odev->dev_width / 8;
+	    odev->height=ASUS_OLED_DISP_HEIGHT;
+	    odev->x_shift=0;
+	    odev->y_shift=0;
+	    odev->last_val=0;
+
+	    send_data(odev);
+
+	    return count;
+	}
+
+	if (buf[0] == '<') {
+		size_t i;
+		size_t w = 0, h = 0;
+		size_t w_mem, h_mem;
+
+		if (count < 10 || buf[2] != ':') {
+			goto error_header;
+		}
+
+		switch(tolower(buf[1])) {
+			case ASUS_OLED_STATIC:
+			case ASUS_OLED_ROLL:
+			case ASUS_OLED_FLASH:
+				odev->pic_mode = buf[1];
+				break;
+			default:
+				printk(ASUS_OLED_ERROR "Wrong picture mode: '%c'.\n", buf[1]);
+				return -EIO;
+				break;
+		}
+
+		for (i = 3; i < count; ++i) {
+			if (buf[i] >= '0' && buf[i] <= '9') {
+				w = 10*w + (buf[i] - '0');
+
+				if (w > ASUS_OLED_MAX_WIDTH) goto error_width;
+			}
+			else if (tolower(buf[i]) == 'x') break;
+			else goto error_width;
+		}
+
+		for (++i; i < count; ++i) {
+			if (buf[i] >= '0' && buf[i] <= '9') {
+				h = 10*h + (buf[i] - '0');
+
+				if (h > ASUS_OLED_DISP_HEIGHT) goto error_height;
+			}
+			else if (tolower(buf[i]) == '>') break;
+			else goto error_height;
+		}
+
+		if (w < 1 || w > ASUS_OLED_MAX_WIDTH) goto error_width;
+
+		if (h < 1 || h > ASUS_OLED_DISP_HEIGHT) goto error_height;
+
+		if (i >= count || buf[i] != '>') goto error_header;
+
+		offs = i+1;
+
+		if (w % (odev->dev_width) != 0)
+			w_mem = (w/(odev->dev_width) + 1)*(odev->dev_width);
+		else
+			w_mem = w;
+
+		if (h < ASUS_OLED_DISP_HEIGHT)
+			h_mem = ASUS_OLED_DISP_HEIGHT;
+		else
+			h_mem = h;
+
+		odev->buf_size = w_mem * h_mem / 8;
+
+		if (odev->buf) kfree(odev->buf);
+		odev->buf = kmalloc(odev->buf_size, GFP_KERNEL);
+
+		if (odev->buf == NULL) {
+			odev->buf_size = 0;
+			printk(ASUS_OLED_ERROR "Out of memory!\n");
+			return -ENOMEM;
+		}
+
+		memset(odev->buf, 0xff, odev->buf_size);
+
+		odev->buf_offs = 0;
+		odev->width = w;
+		odev->height = h;
+		odev->x_shift = 0;
+		odev->y_shift = 0;
+		odev->last_val = 0;
+
+		if (odev->pic_mode == ASUS_OLED_FLASH) {
+			if (h < ASUS_OLED_DISP_HEIGHT/2)
+				odev->y_shift = (ASUS_OLED_DISP_HEIGHT/2 - h)/2;
+		}
+		else {
+			if (h < ASUS_OLED_DISP_HEIGHT)
+				odev->y_shift = (ASUS_OLED_DISP_HEIGHT - h)/2;
+		}
+
+		if (w < (odev->dev_width))
+			odev->x_shift = ((odev->dev_width) - w)/2;
+	}
+
+	max_offs = odev->width * odev->height;
+
+	while (offs < count && odev->buf_offs < max_offs) {
+		int ret;
+
+		if (buf[offs] == '1' || buf[offs] == '#') {
+			if ( (ret = append_values(odev, 1, 1)) < 0) return ret;
+		}
+		else if (buf[offs] == '0' || buf[offs] == ' ') {
+			if ( (ret = append_values(odev, 0, 1)) < 0) return ret;
+		}
+		else if (buf[offs] == '\n') {
+			// New line detected. Lets assume, that all characters till the end of the
+			// line were equal to the last character in this line.
+			if (odev->buf_offs % odev->width != 0)
+				if ( (ret = append_values(odev, odev->last_val,
+				      odev->width - (odev->buf_offs % odev->width))) < 0) return ret;
+		}
+
+		offs++;
+	}
+
+	if (odev->buf_offs >= max_offs) send_data(odev);
+
+	return count;
+
+error_width:
+	printk(ASUS_OLED_ERROR "Wrong picture width specified.\n");
+	return -EIO;
+
+error_height:
+	printk(ASUS_OLED_ERROR "Wrong picture height specified.\n");
+	return -EIO;
+
+error_header:
+	printk(ASUS_OLED_ERROR "Wrong picture header.\n");
+	return -EIO;
+}
+
+static ssize_t set_picture(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct usb_interface *intf = to_usb_interface(dev);
+
+	return odev_set_picture(usb_get_intfdata(intf), buf, count);
+}
+
+static ssize_t class_set_picture(struct device *device, struct device_attribute *attr, const char *buf, size_t count)
+{
+	return odev_set_picture((struct asus_oled_dev *) dev_get_drvdata(device), buf, count);
+}
+
+#define ASUS_OLED_DEVICE_ATTR(_file)		dev_attr_asus_oled_##_file
+
+static DEVICE_ATTR(asus_oled_enabled, S_IWUGO | S_IRUGO, get_enabled, set_enabled);
+static DEVICE_ATTR(asus_oled_picture, S_IWUGO , NULL, set_picture);
+
+static DEVICE_ATTR(enabled, S_IWUGO | S_IRUGO, class_get_enabled, class_set_enabled);
+static DEVICE_ATTR(picture, S_IWUGO, NULL, class_set_picture);
+
+static int asus_oled_probe(struct usb_interface *interface, const struct usb_device_id *id)
+{
+	struct usb_device *udev = interface_to_usbdev(interface);
+	struct asus_oled_dev *odev = NULL;
+	int retval = -ENOMEM;
+	uint16_t dev_width = 0;
+	oled_pack_mode_t pack_mode = PACK_MODE_LAST;
+	const struct oled_dev_desc_str * dev_desc = oled_dev_desc_table;
+	const char *desc = 0;
+
+	if (id == 0) {
+		// Even possible? Just to make sure...
+		dev_err(&interface->dev, "No usb_device_id provided!\n");
+		return -ENODEV;
+	}
+
+	for (; dev_desc->idVendor; dev_desc++)
+	{
+		if (dev_desc->idVendor == id->idVendor
+			&& dev_desc->idProduct == id->idProduct)
+		{
+			dev_width = dev_desc->devWidth;
+			desc = dev_desc->devDesc;
+			pack_mode = dev_desc->packMode;
+			break;
+		}
+	}
+
+	if ( !desc || dev_width < 1 || pack_mode == PACK_MODE_LAST) {
+		dev_err(&interface->dev, "Missing or incomplete device description!\n");
+		return -ENODEV;
+	}
+
+	odev = kzalloc(sizeof(struct asus_oled_dev), GFP_KERNEL);
+
+	if (odev == NULL) {
+		dev_err(&interface->dev, "Out of memory\n");
+		return -ENOMEM;
+	}
+
+	odev->udev = usb_get_dev(udev);
+	odev->pic_mode = ASUS_OLED_STATIC;
+	odev->dev_width = dev_width;
+	odev->pack_mode = pack_mode;
+	odev->height = 0;
+	odev->width = 0;
+	odev->x_shift = 0;
+	odev->y_shift = 0;
+	odev->buf_offs = 0;
+	odev->buf_size = 0;
+	odev->last_val = 0;
+	odev->buf = NULL;
+	odev->enabled = 1;
+	odev->dev = 0;
+
+	usb_set_intfdata (interface, odev);
+
+	if ((retval = device_create_file(&interface->dev, &ASUS_OLED_DEVICE_ATTR(enabled)))) {
+		goto err_files;
+	}
+
+	if ((retval = device_create_file(&interface->dev, &ASUS_OLED_DEVICE_ATTR(picture)))) {
+		goto err_files;
+	}
+
+	odev->dev = device_create(oled_class, &interface->dev, MKDEV(0,0),
+				NULL,"oled_%d", ++oled_num);
+
+	if (IS_ERR(odev->dev)) {
+		retval = PTR_ERR(odev->dev);
+		goto err_files;
+	}
+
+	dev_set_drvdata(odev->dev, odev);
+
+	if ( (retval = device_create_file(odev->dev, &dev_attr_enabled))) {
+		goto err_class_enabled;
+	}
+
+	if ( (retval = device_create_file(odev->dev, &dev_attr_picture))) {
+		goto err_class_picture;
+	}
+
+	dev_info(&interface->dev, "Attached Asus OLED device: %s [width %u, pack_mode %d]\n", desc, odev->dev_width, odev->pack_mode);
+
+	if (start_off)
+		enable_oled(odev, 0);
+
+	return 0;
+
+err_class_picture:
+	device_remove_file(odev->dev, &dev_attr_picture);
+
+err_class_enabled:
+	device_remove_file(odev->dev, &dev_attr_enabled);
+	device_unregister(odev->dev);
+
+err_files:
+	device_remove_file(&interface->dev, &ASUS_OLED_DEVICE_ATTR(enabled));
+	device_remove_file(&interface->dev, &ASUS_OLED_DEVICE_ATTR(picture));
+
+	usb_set_intfdata (interface, NULL);
+	usb_put_dev(odev->udev);
+	kfree(odev);
+
+	return retval;
+}
+
+static void asus_oled_disconnect(struct usb_interface *interface)
+{
+	struct asus_oled_dev *odev;
+
+	odev = usb_get_intfdata (interface);
+	usb_set_intfdata (interface, NULL);
+
+	device_remove_file(odev->dev, &dev_attr_picture);
+	device_remove_file(odev->dev, &dev_attr_enabled);
+	device_unregister(odev->dev);
+
+	device_remove_file(&interface->dev, & ASUS_OLED_DEVICE_ATTR(picture));
+	device_remove_file(&interface->dev, & ASUS_OLED_DEVICE_ATTR(enabled));
+
+	usb_put_dev(odev->udev);
+
+	if (odev->buf) kfree(odev->buf);
+
+	kfree(odev);
+
+	dev_info(&interface->dev, "Disconnected Asus OLED device\n");
+}
+
+static struct usb_driver oled_driver = {
+	.name =		ASUS_OLED_NAME,
+	.probe =	asus_oled_probe,
+	.disconnect =	asus_oled_disconnect,
+	.id_table =	id_table,
+};
+
+static ssize_t version_show(struct class *dev, char *buf)
+{
+	return sprintf(buf, ASUS_OLED_UNDERSCORE_NAME " %s\n", ASUS_OLED_VERSION);
+}
+
+static CLASS_ATTR(version, S_IRUGO, version_show, NULL);
+
+static int __init asus_oled_init(void)
+{
+	int retval = 0;
+	oled_class = class_create(THIS_MODULE, ASUS_OLED_UNDERSCORE_NAME);
+
+	if (IS_ERR(oled_class)) {
+		err("Error creating " ASUS_OLED_UNDERSCORE_NAME " class");
+		return PTR_ERR(oled_class);
+	}
+
+	if ((retval = class_create_file(oled_class, &class_attr_version))) {
+		err("Error creating class version file");
+		goto error;
+	}
+
+	retval = usb_register(&oled_driver);
+
+	if (retval) {
+		err("usb_register failed. Error number %d", retval);
+		goto error;
+	}
+
+	return retval;
+
+error:
+	class_destroy(oled_class);
+	return retval;
+}
+
+static void __exit asus_oled_exit(void)
+{
+	class_remove_file(oled_class, &class_attr_version);
+	class_destroy(oled_class);
+
+	usb_deregister(&oled_driver);
+}
+
+module_init (asus_oled_init);
+module_exit (asus_oled_exit);
+
diff --git a/drivers/staging/asus_oled/linux.txt b/drivers/staging/asus_oled/linux.txt
new file mode 100644
index 00000000000..dc758b0eb37
--- /dev/null
+++ b/drivers/staging/asus_oled/linux.txt
@@ -0,0 +1,33 @@
+<s:74x32>
+0
+0
+00000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000000000000000000000000
+01111111111000000000000000000000000000000000000000000000000000000000000000
+00011111100000000000000111000000000000000000000000000000000000000000000000
+00001111000000000000000111000000000000000000000000000000000000000000000000
+00001111000000000000000111000000000000000000000000000000000000000000000000
+00001111000000000000000000000000000000000000000000000000000000000000000000
+00001111000000000000000000000000000000000000000000000000000000000000000000
+00001111000000000000011100001111111111100000111110011111100011111101111000
+00001111000000000000111110000011111000111000111110000111100001111000110000
+00001111000000000001101110000011111000111000001111000111100000111100100000
+00001111000000000001001110000011110000111100001111000111100000111101100000
+00001111000000000100001110000011110000111100001111000111100000011111000000
+00001111000000000100011110000011110000111100001111000111100000001111000000
+00001111000000000100011110000011110000111100001111000111100000001111000000
+00001111000000000100011100100011110000111100001111000111100000001111100000
+00001111000000001100111100100011110000111100001111000111100000001111110000
+00001111000000001100111101100011110000111100001111000111100000011011110000
+00001111000000011100111101000011110000111100001111000111100000010001111000
+00011111000001111100111011000011110000111100001111001111100000110000111100
+11111111111111111100011110001111111011111110000111110111111011111011111110
+00000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000000000000000000000000
+0
+0
+0
+0
diff --git a/drivers/staging/asus_oled/linux_f.txt b/drivers/staging/asus_oled/linux_f.txt
new file mode 100644
index 00000000000..b4bb85cc6eb
--- /dev/null
+++ b/drivers/staging/asus_oled/linux_f.txt
@@ -0,0 +1,18 @@
+<f:128x16>
+00000000000000000000000000000000001111111100000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000000001110000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000000001110000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000000011000111111111100001111001111100111110111000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000000111100001111000110001111000111100011100010000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000001011100001111000111000111100111100001110110000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000000011100001110000111000111100111100001111100000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000100011100001110000111000111100111100000111100000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000100011100001110000111000111100111100000111100000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000100111001001110000111000111100111100000111110000000000000000000000000000000000000
+00000000000000000000000000000000000011110000001100111011001110000111000111100111100000111110000000000000000000000000000000000000
+00000000000000000000000000000000000011110000001100111010001110000111000111100111100000100111000000000000000000000000000000000000
+00000000000000000000000000000000000011110000111100110110001110000111000111100111100001000011100000000000000000000000000000000000
+00000000000000000000000000000000001111111111111100111100111111011111100011110111110111101111110000000000000000000000000000000000
+
diff --git a/drivers/staging/asus_oled/linux_fr.txt b/drivers/staging/asus_oled/linux_fr.txt
new file mode 100644
index 00000000000..f88e2b3bdd1
--- /dev/null
+++ b/drivers/staging/asus_oled/linux_fr.txt
@@ -0,0 +1,33 @@
+<f:128x32>
+00000000000000000000000000000000001111111100000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000000001110000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000000001110000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000000011000111111111100001111001111100111110111000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000000111100001111000110001111000111100011100010000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000001011100001111000111000111100111100001110110000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000000011100001110000111000111100111100001111100000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000100011100001110000111000111100111100000111100000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000100011100001110000111000111100111100000111100000000000000000000000000000000000000
+00000000000000000000000000000000000011110000000100111001001110000111000111100111100000111110000000000000000000000000000000000000
+00000000000000000000000000000000000011110000001100111011001110000111000111100111100000111110000000000000000000000000000000000000
+00000000000000000000000000000000000011110000001100111010001110000111000111100111100000100111000000000000000000000000000000000000
+00000000000000000000000000000000000011110000111100110110001110000111000111100111100001000011100000000000000000000000000000000000
+00000000000000000000000000000000001111111111111100111100111111011111100011110111110111101111110000000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000110000000000000000000000000000000000000000000000000000000000000000110000000000000000000000000000000
+00000000000000000000000000001111000000000000000000000000000000000000000000000000000000000000001111000000000000000000000000000000
+00000000000000000000000000011111100000000000000000000000000000000000000000000000000000000000011111100000000000000000000000000000
+00000000000000000000000000011111100000000000000000000000000000000000000000000000000000000000011111100000000000000000000000000000
+00000000000000000000000000001111000000000000000000000000000000000000000000000000000000000000001111000000000000000000000000000000
+00000000000000000000000000000110000000000000000000000000000000000000000000000000000000000000000110000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
diff --git a/drivers/staging/asus_oled/tux.txt b/drivers/staging/asus_oled/tux.txt
new file mode 100644
index 00000000000..9d2052854b6
--- /dev/null
+++ b/drivers/staging/asus_oled/tux.txt
@@ -0,0 +1,33 @@
+<s:32x32>
+00000000000001111111000000000000
+0000000000001       100000000000
+000000000001         10000000000
+000000000001         10000000000
+000000000001         10000000000
+000000000001 1  111  10000000000
+000000000001    1 1   1000000000
+000000000001  111     1000000000
+000000000001 111111   1000000000
+000000000001 111111   1000000000
+000000000001    1 1    100000000
+00000000001      11    100000000
+00000000001 11111111    10000000
+0000000001  11111111     1000000
+000000001   111111111    1000000
+000000001  1111111111     100000
+00000001   11111111111    100000
+00000001  111111111111     10000
+0000001   111111111111     10000
+0000001   111111111111     10000
+0000001   111111111111     10000
+0000001   111111111111     10000
+000000011 11111111111      10000
+000011 11  11111111111    100000
+0001  1111  111111111111111 1000
+001 1111111  11111111111111 1000
+001 1111111  1111111  111111 100
+001 11111111 111111   1111111 10
+001 11111111          11111  100
+001  1111111          111  11100
+000111   111   11111  11  100000
+000000111   111111111    1000000
diff --git a/drivers/staging/asus_oled/tux_r.txt b/drivers/staging/asus_oled/tux_r.txt
new file mode 100644
index 00000000000..fd81a3e8494
--- /dev/null
+++ b/drivers/staging/asus_oled/tux_r.txt
@@ -0,0 +1,33 @@
+<r:32x32>
+00000000000001111111000000000000
+0000000000001       100000000000
+000000000001         10000000000
+000000000001         10000000000
+000000000001         10000000000
+000000000001 1  111  10000000000
+000000000001    1 1   1000000000
+000000000001  111     1000000000
+000000000001 111111   1000000000
+000000000001 111111   1000000000
+000000000001    1 1    100000000
+00000000001      11    100000000
+00000000001 11111111    10000000
+0000000001  11111111     1000000
+000000001   111111111    1000000
+000000001  1111111111     100000
+00000001   11111111111    100000
+00000001  111111111111     10000
+0000001   111111111111     10000
+0000001   111111111111     10000
+0000001   111111111111     10000
+0000001   111111111111     10000
+000000011 11111111111      10000
+000011 11  11111111111    100000
+0001  1111  111111111111111 1000
+001 1111111  11111111111111 1000
+001 1111111  1111111  111111 100
+001 11111111 111111   1111111 10
+001 11111111          11111  100
+001  1111111          111  11100
+000111   111   11111  11  100000
+000000111   111111111    1000000
diff --git a/drivers/staging/asus_oled/tux_r2.txt b/drivers/staging/asus_oled/tux_r2.txt
new file mode 100644
index 00000000000..e94d84eaab0
--- /dev/null
+++ b/drivers/staging/asus_oled/tux_r2.txt
@@ -0,0 +1,33 @@
+<r:256x32>
+000000000000000000000000000000000000000000000000000000000000011111110000000000000000000000000000000000000000000000000000000000000
+0000000000000000000000000000000000000000000000000000000000001       1000000000000000000000000000000000000000000000000000000000000
+000000000000000000000000000000000000000000000000000000000001         1000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+000000000000000000000000000000000000000000000000000000000001         1000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+000000000000000000000000000000000000000000000000000000000001         1000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+000000000000000000000000000000000000000000000000000000000001 1  111  1000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+000000000000000000000000000000000000000000000000000000000001    1 1   100000000000000000000000000000000000000000000000000000000000000000000000000000000000001111111111000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+000000000000000000000000000000000000000000000000000000000001  111     100000000000000000000000000000000000000000000000000000000000000000000000000000000000000011111100000000000000111000000000000000000000000000000000000000000000000000000000000000000000000000
+000000000000000000000000000000000000000000000000000000000001 111111   100000000000000000000000000000000000000000000000000000000000000000000000000000000000000001111000000000000000111000000000000000000000000000000000000000000000000000000000000000000000000000
+000000000000000000000000000000000000000000000000000000000001 111111   100000000000000000000000000000000000000000000000000000000000000000000000000000000000000001111000000000000000111000000000000000000000000000000000000000000000000000000000000000000000000000
+000000000000000000000000000000000000000000000000000000000001    1 1    10000000000000000000000000000000000000000000000000000000000000000000000000000000000000001111000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000000001      11    10000000000000000000000000000000000000000000000000000000000000000000000000000000000000001111000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000000001 11111111    1000000000000000000000000000000000000000000000000000000000000000000000000000000000000001111000000000000011100001111111111100000111110011111100011111101111000000000000000000000000000000
+0000000000000000000000000000000000000000000000000000000001  11111111     100000000000000000000000000000000000000000000000000000000000000000000000000000000000001111000000000000111110000011111000111000111110000111100001111000110000000000000000000000000000000
+000000000000000000000000000000000000000000000000000000001   111111111    100000000000000000000000000000000000000000000000000000000000000000000000000000000000001111000000000001101110000011111000111000001111000111100000111100100000000000000000000000000000000
+000000000000000000000000000000000000000000000000000000001  1111111111     10000000000000000000000000000000000000000000000000000000000000000000000000000000000001111000000000001001110000011110000111100001111000111100000111101100000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000001   11111111111    10000000000000000000000000000000000000000000000000000000000000000000000000000000000001111000000000100001110000011110000111100001111000111100000011111000000000000000000000000000000000
+00000000000000000000000000000000000000000000000000000001  111111111111     10000000000000000000000000000000000000000000000000000000000000000000000000000000000011110000000001000111100000111100001111000011110001111000000011110000000
+0000000000000000000000000000000000000000000000000000001   111111111111     1000000000000000000000000000000000000000000000000000000000000000000000000000000000001111000000000100011110000011110000111100001111000111100000001111000000
+0000000000000000000000000000000000000000000000000000001   111111111111     1000000000000000000000000000000000000000000000000000000000000000000000000000000000001111000000000100011100100011110000111100001111000111100000001111100000000000000000000000000000000
+0000000000000000000000000000000000000000000000000000001   111111111111     1000000000000000000000000000000000000000000000000000000000000000000000000000000000001111000000001100111100100011110000111100001111000111100000001111110000000000000000000000000000000
+0000000000000000000000000000000000000000000000000000001   111111111111     1000000000000000000000000000000000000000000000000000000000000000000000000000000000001111000000001100111101100011110000111100001111000111100000011011110000000000000000000000000000000
+000000000000000000000000000000000000000000000000000000011 11111111111      1000000000000000000000000000000000000000000000000000000000000000000000000000000000001111000000011100111101000011110000111100001111000111100000010001111000000000000000000000000000000
+000000000000000000000000000000000000000000000000000011 11  11111111111    10000000000000000000000000000000000000000000000000000000000000000000000000000000000011111000001111100111011000011110000111100001111001111100000110000111100000000000000000000000000000
+0000000000000000000000000000000000000000000000000001  1111  111111111111111 100000000000000000000000000000000000000000000000000000000000000000000000000000011111111111111111100011110001111111011111110000111110111111011111011111110000000000000000000000000000
+000000000000000000000000000000000000000000000000001 1111111  11111111111111 100000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+000000000000000000000000000000000000000000000000001 1111111  1111111  111111 10000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+000000000000000000000000000000000000000000000000001 11111111 111111   1111111 1000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
+000000000000000000000000000000000000000000000000001 11111111          11111  1000000000000000000000000000000000000000000000000000
+000000000000000000000000000000000000000000000000001  1111111          111  111000000000000000000000000000000000000000000000000000
+000000000000000000000000000000000000000000000000000111   111   11111  11  10
+000000000000000000000000000000000000000000000000000000111   111111111    10
diff --git a/drivers/staging/asus_oled/zig.txt b/drivers/staging/asus_oled/zig.txt
new file mode 100644
index 00000000000..31573d8f799
--- /dev/null
+++ b/drivers/staging/asus_oled/zig.txt
@@ -0,0 +1,33 @@
+<r:128x32>
+10000000000000000000000000000000000000000000000000000000000000011000000000000000000000000000000000000000000000000000000000000001
+01000000000000000000000000000000000000000000000000000000000000100100000000000000000000000000000000000000000000000000000000000010
+00100000000000000000000000000000000000000000000000000000000001000010000000000000000000000000000000000000000000000000000000000100
+00010000000000000000000000000000000000000000000000000000000010000001000000000000000000000000000000000000000000000000000000001000
+00001000000000000000000000000000000000000000000000000000000100000000100000000000000000000000000000000000000000000000000000010000
+00000100000000000000000000000000000000000000000000000000001000000000010000000000000000000000000000000000000000000000000000100000
+00000010000000000000000000000000000000000000000000000000010000000000001000000000000000000000000000000000000000000000000001000000
+00000001000000000000000000000000000000000000000000000000100000000000000100000000000000000000000000000000000000000000000010000000
+00000000100000000000000000000000000000000000000000000001000000000000000010000000000000000000000000000000000000000000000100000000
+00000000010000000000000000000000000000000000000000000010000000000000000001000000000000000000000000000000000000000000001000000000
+00000000001000000000000000000000000000000000000000000100000000000000000000100000000000000000000000000000000000000000010000000000
+00000000000100000000000000000000000000000000000000001000000000000000000000010000000000000000000000000000000000000000100000000000
+00000000000010000000000000000000000000000000000000010000000000000000000000001000000000000000000000000000000000000001000000000000
+00000000000001000000000000000000000000000000000000100000000000000000000000000100000000000000000000000000000000000010000000000000
+00000000000000100000000000000000000000000000000001000000000000000000000000000010000000000000000000000000000000000100000000000000
+00000000000000010000000000000000000000000000000010000000000000000000000000000001000000000000000000000000000000001000000000000000
+00000000000000001000000000000000000000000000000100000000000000000000000000000000100000000000000000000000000000010000000000000000
+00000000000000000100000000000000000000000000001000000000000000000000000000000000010000000000000000000000000000100000000000000000
+00000000000000000010000000000000000000000000010000000000000000000000000000000000001000000000000000000000000001000000000000000000
+00000000000000000001000000000000000000000000100000000000000000000000000000000000000100000000000000000000000010000000000000000000
+00000000000000000000100000000000000000000001000000000000000000000000000000000000000010000000000000000000000100000000000000000000
+00000000000000000000010000000000000000000010000000000000000000000000000000000000000001000000000000000000001000000000000000000000
+00000000000000000000001000000000000000000100000000000000000000000000000000000000000000100000000000000000010000000000000000000000
+00000000000000000000000100000000000000001000000000000000000000000000000000000000000000010000000000000000100000000000000000000000
+00000000000000000000000010000000000000010000000000000000000000000000000000000000000000001000000000000001000000000000000000000000
+00000000000000000000000001000000000000100000000000000000000000000000000000000000000000000100000000000010000000000000000000000000
+00000000000000000000000000100000000001000000000000000000000000000000000000000000000000000010000000000100000000000000000000000000
+00000000000000000000000000010000000010000000000000000000000000000000000000000000000000000001000000001000000000000000000000000000
+00000000000000000000000000001000000100000000000000000000000000000000000000000000000000000000100000010000000000000000000000000000
+00000000000000000000000000000100001000000000000000000000000000000000000000000000000000000000010000100000000000000000000000000000
+00000000000000000000000000000010010000000000000000000000000000000000000000000000000000000000001001000000000000000000000000000000
+00000000000000000000000000000001100000000000000000000000000000000000000000000000000000000000000110000000000000000000000000000000
diff --git a/drivers/staging/at76_usb/Kconfig b/drivers/staging/at76_usb/Kconfig
index 8606f962162..4c0e55e1544 100644
--- a/drivers/staging/at76_usb/Kconfig
+++ b/drivers/staging/at76_usb/Kconfig
@@ -1,6 +1,6 @@
 config USB_ATMEL
 	tristate "Atmel at76c503/at76c505/at76c505a USB cards"
-	depends on WLAN_80211 && USB
+	depends on MAC80211 && WLAN_80211 && USB
 	default N
 	select FW_LOADER
 	---help---
diff --git a/drivers/staging/at76_usb/at76_usb.c b/drivers/staging/at76_usb/at76_usb.c
index 174e2bec922..185533e5476 100644
--- a/drivers/staging/at76_usb/at76_usb.c
+++ b/drivers/staging/at76_usb/at76_usb.c
@@ -6,6 +6,7 @@
  * Copyright (c) 2004 Nick Jones
  * Copyright (c) 2004 Balint Seeber <n0_5p4m_p13453@hotmail.com>
  * Copyright (c) 2007 Guido Guenther <agx@sigxcpu.org>
+ * Copyright (c) 2007 Kalle Valo <kalle.valo@iki.fi>
  *
  * This program is free software; you can redistribute it and/or
  * modify it under the terms of the GNU General Public License as
@@ -16,6 +17,13 @@
  * Atmel AT76C503A/505/505A.
  *
  * Some iw_handler code was taken from airo.c, (C) 1999 Benjamin Reed
+ *
+ * TODO for the mac80211 port:
+ * o adhoc support
+ * o RTS/CTS support
+ * o Power Save Mode support
+ * o support for short/long preambles
+ * o export variables through debugfs/sysfs
  */
 
 #include <linux/init.h>
@@ -36,7 +44,7 @@
 #include <net/ieee80211_radiotap.h>
 #include <linux/firmware.h>
 #include <linux/leds.h>
-#include <net/ieee80211.h>
+#include <net/mac80211.h>
 
 #include "at76_usb.h"
 
@@ -76,31 +84,43 @@
 #define DBG_WE_EVENTS		0x08000000	/* dump wireless events */
 #define DBG_FW			0x10000000	/* firmware download */
 #define DBG_DFU			0x20000000	/* device firmware upgrade */
+#define DBG_CMD			0x40000000
+#define DBG_MAC80211		0x80000000
 
 #define DBG_DEFAULTS		0
 
 /* Use our own dbg macro */
 #define at76_dbg(bits, format, arg...) \
-	do { \
-		if (at76_debug & (bits)) \
+do {	\
+	if (at76_debug & (bits))	\
+		printk(KERN_DEBUG DRIVER_NAME ": " format "\n" , ## arg); \
+} while (0)
+
+#define at76_dbg_dump(bits, buf, len, format, arg...)	\
+do {	\
+	if (at76_debug & (bits)) {	\
 		printk(KERN_DEBUG DRIVER_NAME ": " format "\n" , ## arg); \
-	} while (0)
+		print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, buf, len);	\
+		}							\
+} while (0)
 
 static int at76_debug = DBG_DEFAULTS;
 
+#define FIRMWARE_IS_WPA(ver) ((ver.major == 1) && (ver.minor == 103))
+
 /* Protect against concurrent firmware loading and parsing */
 static struct mutex fw_mutex;
 
 static struct fwentry firmwares[] = {
-	[0] = {""},
-	[BOARD_503_ISL3861] = {"atmel_at76c503-i3861.bin"},
-	[BOARD_503_ISL3863] = {"atmel_at76c503-i3863.bin"},
-	[BOARD_503] = {"atmel_at76c503-rfmd.bin"},
-	[BOARD_503_ACC] = {"atmel_at76c503-rfmd-acc.bin"},
-	[BOARD_505] = {"atmel_at76c505-rfmd.bin"},
-	[BOARD_505_2958] = {"atmel_at76c505-rfmd2958.bin"},
-	[BOARD_505A] = {"atmel_at76c505a-rfmd2958.bin"},
-	[BOARD_505AMX] = {"atmel_at76c505amx-rfmd.bin"},
+	[0] = { "" },
+	[BOARD_503_ISL3861] = { "atmel_at76c503-i3861.bin" },
+	[BOARD_503_ISL3863] = { "atmel_at76c503-i3863.bin" },
+	[BOARD_503] = { "atmel_at76c503-rfmd.bin" },
+	[BOARD_503_ACC] = { "atmel_at76c503-rfmd-acc.bin" },
+	[BOARD_505] = { "atmel_at76c505-rfmd.bin" },
+	[BOARD_505_2958] = { "atmel_at76c505-rfmd2958.bin" },
+	[BOARD_505A] = { "atmel_at76c505a-rfmd2958.bin" },
+	[BOARD_505AMX] = { "atmel_at76c505amx-rfmd.bin" },
 };
 
 #define USB_DEVICE_DATA(__ops)	.driver_info = (kernel_ulong_t)(__ops)
@@ -110,133 +130,133 @@ static struct usb_device_id dev_table[] = {
 	 * at76c503-i3861
 	 */
 	/* Generic AT76C503/3861 device */
-	{USB_DEVICE(0x03eb, 0x7603), USB_DEVICE_DATA(BOARD_503_ISL3861)},
+	{ USB_DEVICE(0x03eb, 0x7603), USB_DEVICE_DATA(BOARD_503_ISL3861) },
 	/* Linksys WUSB11 v2.1/v2.6 */
-	{USB_DEVICE(0x066b, 0x2211), USB_DEVICE_DATA(BOARD_503_ISL3861)},
+	{ USB_DEVICE(0x066b, 0x2211), USB_DEVICE_DATA(BOARD_503_ISL3861) },
 	/* Netgear MA101 rev. A */
-	{USB_DEVICE(0x0864, 0x4100), USB_DEVICE_DATA(BOARD_503_ISL3861)},
+	{ USB_DEVICE(0x0864, 0x4100), USB_DEVICE_DATA(BOARD_503_ISL3861) },
 	/* Tekram U300C / Allnet ALL0193 */
-	{USB_DEVICE(0x0b3b, 0x1612), USB_DEVICE_DATA(BOARD_503_ISL3861)},
+	{ USB_DEVICE(0x0b3b, 0x1612), USB_DEVICE_DATA(BOARD_503_ISL3861) },
 	/* HP HN210W J7801A */
-	{USB_DEVICE(0x03f0, 0x011c), USB_DEVICE_DATA(BOARD_503_ISL3861)},
+	{ USB_DEVICE(0x03f0, 0x011c), USB_DEVICE_DATA(BOARD_503_ISL3861) },
 	/* Sitecom/Z-Com/Zyxel M4Y-750 */
-	{USB_DEVICE(0x0cde, 0x0001), USB_DEVICE_DATA(BOARD_503_ISL3861)},
+	{ USB_DEVICE(0x0cde, 0x0001), USB_DEVICE_DATA(BOARD_503_ISL3861) },
 	/* Dynalink/Askey WLL013 (intersil) */
-	{USB_DEVICE(0x069a, 0x0320), USB_DEVICE_DATA(BOARD_503_ISL3861)},
+	{ USB_DEVICE(0x069a, 0x0320), USB_DEVICE_DATA(BOARD_503_ISL3861) },
 	/* EZ connect 11Mpbs Wireless USB Adapter SMC2662W v1 */
-	{USB_DEVICE(0x0d5c, 0xa001), USB_DEVICE_DATA(BOARD_503_ISL3861)},
+	{ USB_DEVICE(0x0d5c, 0xa001), USB_DEVICE_DATA(BOARD_503_ISL3861) },
 	/* BenQ AWL300 */
-	{USB_DEVICE(0x04a5, 0x9000), USB_DEVICE_DATA(BOARD_503_ISL3861)},
+	{ USB_DEVICE(0x04a5, 0x9000), USB_DEVICE_DATA(BOARD_503_ISL3861) },
 	/* Addtron AWU-120, Compex WLU11 */
-	{USB_DEVICE(0x05dd, 0xff31), USB_DEVICE_DATA(BOARD_503_ISL3861)},
+	{ USB_DEVICE(0x05dd, 0xff31), USB_DEVICE_DATA(BOARD_503_ISL3861) },
 	/* Intel AP310 AnyPoint II USB */
-	{USB_DEVICE(0x8086, 0x0200), USB_DEVICE_DATA(BOARD_503_ISL3861)},
+	{ USB_DEVICE(0x8086, 0x0200), USB_DEVICE_DATA(BOARD_503_ISL3861) },
 	/* Dynalink L11U */
-	{USB_DEVICE(0x0d8e, 0x7100), USB_DEVICE_DATA(BOARD_503_ISL3861)},
+	{ USB_DEVICE(0x0d8e, 0x7100), USB_DEVICE_DATA(BOARD_503_ISL3861) },
 	/* Arescom WL-210, FCC id 07J-GL2411USB */
-	{USB_DEVICE(0x0d8e, 0x7110), USB_DEVICE_DATA(BOARD_503_ISL3861)},
+	{ USB_DEVICE(0x0d8e, 0x7110), USB_DEVICE_DATA(BOARD_503_ISL3861) },
 	/* I-O DATA WN-B11/USB */
-	{USB_DEVICE(0x04bb, 0x0919), USB_DEVICE_DATA(BOARD_503_ISL3861)},
+	{ USB_DEVICE(0x04bb, 0x0919), USB_DEVICE_DATA(BOARD_503_ISL3861) },
 	/* BT Voyager 1010 */
-	{USB_DEVICE(0x069a, 0x0821), USB_DEVICE_DATA(BOARD_503_ISL3861)},
+	{ USB_DEVICE(0x069a, 0x0821), USB_DEVICE_DATA(BOARD_503_ISL3861) },
 	/*
 	 * at76c503-i3863
 	 */
 	/* Generic AT76C503/3863 device */
-	{USB_DEVICE(0x03eb, 0x7604), USB_DEVICE_DATA(BOARD_503_ISL3863)},
+	{ USB_DEVICE(0x03eb, 0x7604), USB_DEVICE_DATA(BOARD_503_ISL3863) },
 	/* Samsung SWL-2100U */
-	{USB_DEVICE(0x055d, 0xa000), USB_DEVICE_DATA(BOARD_503_ISL3863)},
+	{ USB_DEVICE(0x055d, 0xa000), USB_DEVICE_DATA(BOARD_503_ISL3863) },
 	/*
 	 * at76c503-rfmd
 	 */
 	/* Generic AT76C503/RFMD device */
-	{USB_DEVICE(0x03eb, 0x7605), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x03eb, 0x7605), USB_DEVICE_DATA(BOARD_503) },
 	/* Dynalink/Askey WLL013 (rfmd) */
-	{USB_DEVICE(0x069a, 0x0321), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x069a, 0x0321), USB_DEVICE_DATA(BOARD_503) },
 	/* Linksys WUSB11 v2.6 */
-	{USB_DEVICE(0x077b, 0x2219), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x077b, 0x2219), USB_DEVICE_DATA(BOARD_503) },
 	/* Network Everywhere NWU11B */
-	{USB_DEVICE(0x077b, 0x2227), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x077b, 0x2227), USB_DEVICE_DATA(BOARD_503) },
 	/* Netgear MA101 rev. B */
-	{USB_DEVICE(0x0864, 0x4102), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x0864, 0x4102), USB_DEVICE_DATA(BOARD_503) },
 	/* D-Link DWL-120 rev. E */
-	{USB_DEVICE(0x2001, 0x3200), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x2001, 0x3200), USB_DEVICE_DATA(BOARD_503) },
 	/* Actiontec 802UAT1, HWU01150-01UK */
-	{USB_DEVICE(0x1668, 0x7605), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x1668, 0x7605), USB_DEVICE_DATA(BOARD_503) },
 	/* AirVast W-Buddie WN210 */
-	{USB_DEVICE(0x03eb, 0x4102), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x03eb, 0x4102), USB_DEVICE_DATA(BOARD_503) },
 	/* Dick Smith Electronics XH1153 802.11b USB adapter */
-	{USB_DEVICE(0x1371, 0x5743), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x1371, 0x5743), USB_DEVICE_DATA(BOARD_503) },
 	/* CNet CNUSB611 */
-	{USB_DEVICE(0x1371, 0x0001), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x1371, 0x0001), USB_DEVICE_DATA(BOARD_503) },
 	/* FiberLine FL-WL200U */
-	{USB_DEVICE(0x1371, 0x0002), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x1371, 0x0002), USB_DEVICE_DATA(BOARD_503) },
 	/* BenQ AWL400 USB stick */
-	{USB_DEVICE(0x04a5, 0x9001), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x04a5, 0x9001), USB_DEVICE_DATA(BOARD_503) },
 	/* 3Com 3CRSHEW696 */
-	{USB_DEVICE(0x0506, 0x0a01), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x0506, 0x0a01), USB_DEVICE_DATA(BOARD_503) },
 	/* Siemens Santis ADSL WLAN USB adapter WLL 013 */
-	{USB_DEVICE(0x0681, 0x001b), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x0681, 0x001b), USB_DEVICE_DATA(BOARD_503) },
 	/* Belkin F5D6050, version 2 */
-	{USB_DEVICE(0x050d, 0x0050), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x050d, 0x0050), USB_DEVICE_DATA(BOARD_503) },
 	/* iBlitzz, BWU613 (not *B or *SB) */
-	{USB_DEVICE(0x07b8, 0xb000), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x07b8, 0xb000), USB_DEVICE_DATA(BOARD_503) },
 	/* Gigabyte GN-WLBM101 */
-	{USB_DEVICE(0x1044, 0x8003), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x1044, 0x8003), USB_DEVICE_DATA(BOARD_503) },
 	/* Planex GW-US11S */
-	{USB_DEVICE(0x2019, 0x3220), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x2019, 0x3220), USB_DEVICE_DATA(BOARD_503) },
 	/* Internal WLAN adapter in h5[4,5]xx series iPAQs */
-	{USB_DEVICE(0x049f, 0x0032), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x049f, 0x0032), USB_DEVICE_DATA(BOARD_503) },
 	/* Corega Wireless LAN USB-11 mini */
-	{USB_DEVICE(0x07aa, 0x0011), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x07aa, 0x0011), USB_DEVICE_DATA(BOARD_503) },
 	/* Corega Wireless LAN USB-11 mini2 */
-	{USB_DEVICE(0x07aa, 0x0018), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x07aa, 0x0018), USB_DEVICE_DATA(BOARD_503) },
 	/* Uniden PCW100 */
-	{USB_DEVICE(0x05dd, 0xff35), USB_DEVICE_DATA(BOARD_503)},
+	{ USB_DEVICE(0x05dd, 0xff35), USB_DEVICE_DATA(BOARD_503) },
 	/*
 	 * at76c503-rfmd-acc
 	 */
 	/* SMC2664W */
-	{USB_DEVICE(0x083a, 0x3501), USB_DEVICE_DATA(BOARD_503_ACC)},
+	{ USB_DEVICE(0x083a, 0x3501), USB_DEVICE_DATA(BOARD_503_ACC) },
 	/* Belkin F5D6050, SMC2662W v2, SMC2662W-AR */
-	{USB_DEVICE(0x0d5c, 0xa002), USB_DEVICE_DATA(BOARD_503_ACC)},
+	{ USB_DEVICE(0x0d5c, 0xa002), USB_DEVICE_DATA(BOARD_503_ACC) },
 	/*
 	 * at76c505-rfmd
 	 */
 	/* Generic AT76C505/RFMD */
-	{USB_DEVICE(0x03eb, 0x7606), USB_DEVICE_DATA(BOARD_505)},
+	{ USB_DEVICE(0x03eb, 0x7606), USB_DEVICE_DATA(BOARD_505) },
 	/*
 	 * at76c505-rfmd2958
 	 */
 	/* Generic AT76C505/RFMD, OvisLink WL-1130USB */
-	{USB_DEVICE(0x03eb, 0x7613), USB_DEVICE_DATA(BOARD_505_2958)},
+	{ USB_DEVICE(0x03eb, 0x7613), USB_DEVICE_DATA(BOARD_505_2958) },
 	/* Fiberline FL-WL240U */
-	{USB_DEVICE(0x1371, 0x0014), USB_DEVICE_DATA(BOARD_505_2958)},
+	{ USB_DEVICE(0x1371, 0x0014), USB_DEVICE_DATA(BOARD_505_2958) },
 	/* CNet CNUSB-611G */
-	{USB_DEVICE(0x1371, 0x0013), USB_DEVICE_DATA(BOARD_505_2958)},
+	{ USB_DEVICE(0x1371, 0x0013), USB_DEVICE_DATA(BOARD_505_2958) },
 	/* Linksys WUSB11 v2.8 */
-	{USB_DEVICE(0x1915, 0x2233), USB_DEVICE_DATA(BOARD_505_2958)},
+	{ USB_DEVICE(0x1915, 0x2233), USB_DEVICE_DATA(BOARD_505_2958) },
 	/* Xterasys XN-2122B, IBlitzz BWU613B/BWU613SB */
-	{USB_DEVICE(0x12fd, 0x1001), USB_DEVICE_DATA(BOARD_505_2958)},
+	{ USB_DEVICE(0x12fd, 0x1001), USB_DEVICE_DATA(BOARD_505_2958) },
 	/* Corega WLAN USB Stick 11 */
-	{USB_DEVICE(0x07aa, 0x7613), USB_DEVICE_DATA(BOARD_505_2958)},
+	{ USB_DEVICE(0x07aa, 0x7613), USB_DEVICE_DATA(BOARD_505_2958) },
 	/* Microstar MSI Box MS6978 */
-	{USB_DEVICE(0x0db0, 0x1020), USB_DEVICE_DATA(BOARD_505_2958)},
+	{ USB_DEVICE(0x0db0, 0x1020), USB_DEVICE_DATA(BOARD_505_2958) },
 	/*
 	 * at76c505a-rfmd2958
 	 */
 	/* Generic AT76C505A device */
-	{USB_DEVICE(0x03eb, 0x7614), USB_DEVICE_DATA(BOARD_505A)},
+	{ USB_DEVICE(0x03eb, 0x7614), USB_DEVICE_DATA(BOARD_505A) },
 	/* Generic AT76C505AS device */
-	{USB_DEVICE(0x03eb, 0x7617), USB_DEVICE_DATA(BOARD_505A)},
+	{ USB_DEVICE(0x03eb, 0x7617), USB_DEVICE_DATA(BOARD_505A) },
 	/* Siemens Gigaset USB WLAN Adapter 11 */
-	{USB_DEVICE(0x1690, 0x0701), USB_DEVICE_DATA(BOARD_505A)},
+	{ USB_DEVICE(0x1690, 0x0701), USB_DEVICE_DATA(BOARD_505A) },
 	/*
 	 * at76c505amx-rfmd
 	 */
 	/* Generic AT76C505AMX device */
-	{USB_DEVICE(0x03eb, 0x7615), USB_DEVICE_DATA(BOARD_505AMX)},
-	{}
+	{ USB_DEVICE(0x03eb, 0x7615), USB_DEVICE_DATA(BOARD_505AMX) },
+	{ }
 };
 
 MODULE_DEVICE_TABLE(usb, dev_table);
@@ -244,26 +264,8 @@ MODULE_DEVICE_TABLE(usb, dev_table);
 /* Supported rates of this hardware, bit 7 marks basic rates */
 static const u8 hw_rates[] = { 0x82, 0x84, 0x0b, 0x16 };
 
-/* Frequency of each channel in MHz */
-static const long channel_frequency[] = {
-	2412, 2417, 2422, 2427, 2432, 2437, 2442,
-	2447, 2452, 2457, 2462, 2467, 2472, 2484
-};
-
-#define NUM_CHANNELS ARRAY_SIZE(channel_frequency)
-
 static const char *const preambles[] = { "long", "short", "auto" };
 
-static const char *const mac_states[] = {
-	[MAC_INIT] = "INIT",
-	[MAC_SCANNING] = "SCANNING",
-	[MAC_AUTH] = "AUTH",
-	[MAC_ASSOC] = "ASSOC",
-	[MAC_JOINING] = "JOINING",
-	[MAC_CONNECTED] = "CONNECTED",
-	[MAC_OWN_IBSS] = "OWN_IBSS"
-};
-
 /* Firmware download */
 /* DFU states */
 #define STATE_IDLE			0x00
@@ -298,17 +300,30 @@ struct dfu_status {
 
 static inline int at76_is_intersil(enum board_type board)
 {
-	return (board == BOARD_503_ISL3861 || board == BOARD_503_ISL3863);
+	if (board == BOARD_503_ISL3861 || board == BOARD_503_ISL3863)
+		return 1;
+	return 0;
 }
 
 static inline int at76_is_503rfmd(enum board_type board)
 {
-	return (board == BOARD_503 || board == BOARD_503_ACC);
+	if (board == BOARD_503 || board == BOARD_503_ACC)
+		return 1;
+	return 0;
+}
+
+static inline int at76_is_505(enum board_type board)
+{
+	if (board == BOARD_505 || board == BOARD_505_2958)
+		return 1;
+	return 0;
 }
 
 static inline int at76_is_505a(enum board_type board)
 {
-	return (board == BOARD_505A || board == BOARD_505AMX);
+	if (board == BOARD_505A || board == BOARD_505AMX)
+		return 1;
+	return 0;
 }
 
 /* Load a block of the first (internal) part of the firmware */
@@ -489,41 +504,6 @@ exit:
 	return ret;
 }
 
-/* Report that the scan results are ready */
-static inline void at76_iwevent_scan_complete(struct net_device *netdev)
-{
-	union iwreq_data wrqu;
-	wrqu.data.length = 0;
-	wrqu.data.flags = 0;
-	wireless_send_event(netdev, SIOCGIWSCAN, &wrqu, NULL);
-	at76_dbg(DBG_WE_EVENTS, "%s: SIOCGIWSCAN sent", netdev->name);
-}
-
-static inline void at76_iwevent_bss_connect(struct net_device *netdev,
-					    u8 *bssid)
-{
-	union iwreq_data wrqu;
-	wrqu.data.length = 0;
-	wrqu.data.flags = 0;
-	memcpy(wrqu.ap_addr.sa_data, bssid, ETH_ALEN);
-	wrqu.ap_addr.sa_family = ARPHRD_ETHER;
-	wireless_send_event(netdev, SIOCGIWAP, &wrqu, NULL);
-	at76_dbg(DBG_WE_EVENTS, "%s: %s: SIOCGIWAP sent", netdev->name,
-		 __func__);
-}
-
-static inline void at76_iwevent_bss_disconnect(struct net_device *netdev)
-{
-	union iwreq_data wrqu;
-	wrqu.data.length = 0;
-	wrqu.data.flags = 0;
-	memset(wrqu.ap_addr.sa_data, 0, ETH_ALEN);
-	wrqu.ap_addr.sa_family = ARPHRD_ETHER;
-	wireless_send_event(netdev, SIOCGIWAP, &wrqu, NULL);
-	at76_dbg(DBG_WE_EVENTS, "%s: %s: SIOCGIWAP sent", netdev->name,
-		 __func__);
-}
-
 #define HEX2STR_BUFFERS 4
 #define HEX2STR_MAX_LEN 64
 #define BIN2HEX(x) ((x) < 10 ? '0' + (x) : (x) + 'A' - 10)
@@ -595,37 +575,6 @@ static void at76_ledtrig_tx_activity(void)
 		mod_timer(&ledtrig_tx_timer, jiffies + HZ / 4);
 }
 
-/* Check if the given ssid is hidden */
-static inline int at76_is_hidden_ssid(u8 *ssid, int length)
-{
-	static const u8 zeros[32];
-
-	if (length == 0)
-		return 1;
-
-	if (length == 1 && ssid[0] == ' ')
-		return 1;
-
-	return (memcmp(ssid, zeros, length) == 0);
-}
-
-static inline void at76_free_bss_list(struct at76_priv *priv)
-{
-	struct list_head *next, *ptr;
-	unsigned long flags;
-
-	spin_lock_irqsave(&priv->bss_list_spinlock, flags);
-
-	priv->curr_bss = NULL;
-
-	list_for_each_safe(ptr, next, &priv->bss_list) {
-		list_del(ptr);
-		kfree(list_entry(ptr, struct bss_info, list));
-	}
-
-	spin_unlock_irqrestore(&priv->bss_list_spinlock, flags);
-}
-
 static int at76_remap(struct usb_device *udev)
 {
 	int ret;
@@ -641,18 +590,25 @@ static int at76_remap(struct usb_device *udev)
 static int at76_get_op_mode(struct usb_device *udev)
 {
 	int ret;
-	u8 op_mode;
+	u8 saved;
+	u8 *op_mode;
 
+	op_mode = kmalloc(1, GFP_NOIO);
+	if (!op_mode)
+		return -ENOMEM;
 	ret = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0), 0x33,
 			      USB_TYPE_VENDOR | USB_DIR_IN |
 			      USB_RECIP_INTERFACE, 0x01, 0, &op_mode, 1,
 			      USB_CTRL_GET_TIMEOUT);
+	saved = *op_mode;
+	kfree(op_mode);
+
 	if (ret < 0)
 		return ret;
 	else if (ret < 1)
 		return -EIO;
 	else
-		return op_mode;
+		return saved;
 }
 
 /* Load a block of the second ("external") part of the firmware */
@@ -720,7 +676,7 @@ exit:
 	kfree(hwcfg);
 	if (ret < 0)
 		printk(KERN_ERR "%s: cannot get HW Config (error %d)\n",
-		       priv->netdev->name, ret);
+		       wiphy_name(priv->hw->wiphy), ret);
 
 	return ret;
 }
@@ -729,15 +685,15 @@ static struct reg_domain const *at76_get_reg_domain(u16 code)
 {
 	int i;
 	static struct reg_domain const fd_tab[] = {
-		{0x10, "FCC (USA)", 0x7ff},	/* ch 1-11 */
-		{0x20, "IC (Canada)", 0x7ff},	/* ch 1-11 */
-		{0x30, "ETSI (most of Europe)", 0x1fff},	/* ch 1-13 */
-		{0x31, "Spain", 0x600},	/* ch 10-11 */
-		{0x32, "France", 0x1e00},	/* ch 10-13 */
-		{0x40, "MKK (Japan)", 0x2000},	/* ch 14 */
-		{0x41, "MKK1 (Japan)", 0x3fff},	/* ch 1-14 */
-		{0x50, "Israel", 0x3fc},	/* ch 3-9 */
-		{0x00, "<unknown>", 0xffffffff}	/* ch 1-32 */
+		{ 0x10, "FCC (USA)", 0x7ff },	/* ch 1-11 */
+		{ 0x20, "IC (Canada)", 0x7ff },	/* ch 1-11 */
+		{ 0x30, "ETSI (most of Europe)", 0x1fff },	/* ch 1-13 */
+		{ 0x31, "Spain", 0x600 },	/* ch 10-11 */
+		{ 0x32, "France", 0x1e00 },	/* ch 10-13 */
+		{ 0x40, "MKK (Japan)", 0x2000 },	/* ch 14 */
+		{ 0x41, "MKK1 (Japan)", 0x3fff },	/* ch 1-14 */
+		{ 0x50, "Israel", 0x3fc },	/* ch 3-9 */
+		{ 0x00, "<unknown>", 0xffffffff }	/* ch 1-32 */
 	};
 
 	/* Last entry is fallback for unknown domain code */
@@ -765,20 +721,43 @@ static inline int at76_get_mib(struct usb_device *udev, u16 mib, void *buf,
 /* Return positive number for status, negative for an error */
 static inline int at76_get_cmd_status(struct usb_device *udev, u8 cmd)
 {
-	u8 stat_buf[40];
+	u8 *stat_buf;
 	int ret;
 
+	stat_buf = kmalloc(40, GFP_NOIO);
+	if (!stat_buf)
+		return -ENOMEM;
+
 	ret = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0), 0x22,
 			      USB_TYPE_VENDOR | USB_DIR_IN |
 			      USB_RECIP_INTERFACE, cmd, 0, stat_buf,
 			      sizeof(stat_buf), USB_CTRL_GET_TIMEOUT);
-	if (ret < 0)
-		return ret;
+	if (ret >= 0)
+		ret = stat_buf[5];
+	kfree(stat_buf);
 
-	return stat_buf[5];
+	return ret;
 }
 
-static int at76_set_card_command(struct usb_device *udev, int cmd, void *buf,
+#define MAKE_CMD_CASE(c) case (c): return #c
+
+static const char *at76_get_cmd_string(u8 cmd_status)
+{
+	switch (cmd_status) {
+		MAKE_CMD_CASE(CMD_SET_MIB);
+		MAKE_CMD_CASE(CMD_GET_MIB);
+		MAKE_CMD_CASE(CMD_SCAN);
+		MAKE_CMD_CASE(CMD_JOIN);
+		MAKE_CMD_CASE(CMD_START_IBSS);
+		MAKE_CMD_CASE(CMD_RADIO_ON);
+		MAKE_CMD_CASE(CMD_RADIO_OFF);
+		MAKE_CMD_CASE(CMD_STARTUP);
+	}
+
+	return "UNKNOWN";
+}
+
+static int at76_set_card_command(struct usb_device *udev, u8 cmd, void *buf,
 				 int buf_size)
 {
 	int ret;
@@ -793,6 +772,10 @@ static int at76_set_card_command(struct usb_device *udev, int cmd, void *buf,
 	cmd_buf->size = cpu_to_le16(buf_size);
 	memcpy(cmd_buf->data, buf, buf_size);
 
+	at76_dbg_dump(DBG_CMD, cmd_buf, sizeof(struct at76_command) + buf_size,
+		      "issuing command %s (0x%02x)",
+		      at76_get_cmd_string(cmd), cmd);
+
 	ret = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x0e,
 			      USB_TYPE_VENDOR | USB_DIR_OUT | USB_RECIP_DEVICE,
 			      0, 0, cmd_buf,
@@ -830,13 +813,13 @@ static int at76_wait_completion(struct at76_priv *priv, int cmd)
 		status = at76_get_cmd_status(priv->udev, cmd);
 		if (status < 0) {
 			printk(KERN_ERR "%s: at76_get_cmd_status failed: %d\n",
-			       priv->netdev->name, status);
+			       wiphy_name(priv->hw->wiphy), status);
 			break;
 		}
 
 		at76_dbg(DBG_WAIT_COMPLETE,
 			 "%s: Waiting on cmd %d, status = %d (%s)",
-			 priv->netdev->name, cmd, status,
+			 wiphy_name(priv->hw->wiphy), cmd, status,
 			 at76_get_cmd_status_string(status));
 
 		if (status != CMD_STATUS_IN_PROGRESS
@@ -847,7 +830,7 @@ static int at76_wait_completion(struct at76_priv *priv, int cmd)
 		if (time_after(jiffies, timeout)) {
 			printk(KERN_ERR
 			       "%s: completion timeout for command %d\n",
-			       priv->netdev->name, cmd);
+			       wiphy_name(priv->hw->wiphy), cmd);
 			status = -ETIMEDOUT;
 			break;
 		}
@@ -870,7 +853,7 @@ static int at76_set_mib(struct at76_priv *priv, struct set_mib_buffer *buf)
 	if (ret != CMD_STATUS_COMPLETE) {
 		printk(KERN_INFO
 		       "%s: set_mib: at76_wait_completion failed "
-		       "with %d\n", priv->netdev->name, ret);
+		       "with %d\n", wiphy_name(priv->hw->wiphy), ret);
 		ret = -EIO;
 	}
 
@@ -891,7 +874,7 @@ static int at76_set_radio(struct at76_priv *priv, int enable)
 	ret = at76_set_card_command(priv->udev, cmd, NULL, 0);
 	if (ret < 0)
 		printk(KERN_ERR "%s: at76_set_card_command(%d) failed: %d\n",
-		       priv->netdev->name, cmd, ret);
+		       wiphy_name(priv->hw->wiphy), cmd, ret);
 	else
 		ret = 1;
 
@@ -912,44 +895,7 @@ static int at76_set_pm_mode(struct at76_priv *priv)
 	ret = at76_set_mib(priv, &priv->mib_buf);
 	if (ret < 0)
 		printk(KERN_ERR "%s: set_mib (pm_mode) failed: %d\n",
-		       priv->netdev->name, ret);
-
-	return ret;
-}
-
-/* Set the association id for power save mode */
-static int at76_set_associd(struct at76_priv *priv, u16 id)
-{
-	int ret = 0;
-
-	priv->mib_buf.type = MIB_MAC_MGMT;
-	priv->mib_buf.size = 2;
-	priv->mib_buf.index = offsetof(struct mib_mac_mgmt, station_id);
-	priv->mib_buf.data.word = cpu_to_le16(id);
-
-	ret = at76_set_mib(priv, &priv->mib_buf);
-	if (ret < 0)
-		printk(KERN_ERR "%s: set_mib (associd) failed: %d\n",
-		       priv->netdev->name, ret);
-
-	return ret;
-}
-
-/* Set the listen interval for power save mode */
-static int at76_set_listen_interval(struct at76_priv *priv, u16 interval)
-{
-	int ret = 0;
-
-	priv->mib_buf.type = MIB_MAC;
-	priv->mib_buf.size = 2;
-	priv->mib_buf.index = offsetof(struct mib_mac, listen_interval);
-	priv->mib_buf.data.word = cpu_to_le16(interval);
-
-	ret = at76_set_mib(priv, &priv->mib_buf);
-	if (ret < 0)
-		printk(KERN_ERR
-		       "%s: set_mib (listen_interval) failed: %d\n",
-		       priv->netdev->name, ret);
+		       wiphy_name(priv->hw->wiphy), ret);
 
 	return ret;
 }
@@ -966,7 +912,7 @@ static int at76_set_preamble(struct at76_priv *priv, u8 type)
 	ret = at76_set_mib(priv, &priv->mib_buf);
 	if (ret < 0)
 		printk(KERN_ERR "%s: set_mib (preamble) failed: %d\n",
-		       priv->netdev->name, ret);
+		       wiphy_name(priv->hw->wiphy), ret);
 
 	return ret;
 }
@@ -983,7 +929,7 @@ static int at76_set_frag(struct at76_priv *priv, u16 size)
 	ret = at76_set_mib(priv, &priv->mib_buf);
 	if (ret < 0)
 		printk(KERN_ERR "%s: set_mib (frag threshold) failed: %d\n",
-		       priv->netdev->name, ret);
+		       wiphy_name(priv->hw->wiphy), ret);
 
 	return ret;
 }
@@ -1000,7 +946,7 @@ static int at76_set_rts(struct at76_priv *priv, u16 size)
 	ret = at76_set_mib(priv, &priv->mib_buf);
 	if (ret < 0)
 		printk(KERN_ERR "%s: set_mib (rts) failed: %d\n",
-		       priv->netdev->name, ret);
+		       wiphy_name(priv->hw->wiphy), ret);
 
 	return ret;
 }
@@ -1017,24 +963,41 @@ static int at76_set_autorate_fallback(struct at76_priv *priv, int onoff)
 	ret = at76_set_mib(priv, &priv->mib_buf);
 	if (ret < 0)
 		printk(KERN_ERR "%s: set_mib (autorate fallback) failed: %d\n",
-		       priv->netdev->name, ret);
+		       wiphy_name(priv->hw->wiphy), ret);
 
 	return ret;
 }
 
-static int at76_add_mac_address(struct at76_priv *priv, void *addr)
+static int at76_set_tkip_bssid(struct at76_priv *priv, const void *addr)
 {
 	int ret = 0;
 
-	priv->mib_buf.type = MIB_MAC_ADDR;
+	priv->mib_buf.type = MIB_MAC_ENCRYPTION;
 	priv->mib_buf.size = ETH_ALEN;
-	priv->mib_buf.index = offsetof(struct mib_mac_addr, mac_addr);
+	priv->mib_buf.index = offsetof(struct mib_mac_encryption, tkip_bssid);
 	memcpy(priv->mib_buf.data.addr, addr, ETH_ALEN);
 
 	ret = at76_set_mib(priv, &priv->mib_buf);
 	if (ret < 0)
-		printk(KERN_ERR "%s: set_mib (MAC_ADDR, mac_addr) failed: %d\n",
-		       priv->netdev->name, ret);
+		printk(KERN_ERR "%s: set_mib (MAC_ENCRYPTION, tkip_bssid) failed: %d\n",
+		       wiphy_name(priv->hw->wiphy), ret);
+
+	return ret;
+}
+
+static int at76_reset_rsc(struct at76_priv *priv)
+{
+	int ret = 0;
+
+	priv->mib_buf.type = MIB_MAC_ENCRYPTION;
+	priv->mib_buf.size = 4 * 8;
+	priv->mib_buf.index = offsetof(struct mib_mac_encryption, key_rsc);
+	memset(priv->mib_buf.data.data, 0 , priv->mib_buf.size);
+
+	ret = at76_set_mib(priv, &priv->mib_buf);
+	if (ret < 0)
+		printk(KERN_ERR "%s: set_mib (MAC_ENCRYPTION, key_rsc) failed: %d\n",
+		       wiphy_name(priv->hw->wiphy), ret);
 
 	return ret;
 }
@@ -1053,16 +1016,16 @@ static void at76_dump_mib_mac_addr(struct at76_priv *priv)
 			   sizeof(struct mib_mac_addr));
 	if (ret < 0) {
 		printk(KERN_ERR "%s: at76_get_mib (MAC_ADDR) failed: %d\n",
-		       priv->netdev->name, ret);
+		       wiphy_name(priv->hw->wiphy), ret);
 		goto exit;
 	}
 
 	at76_dbg(DBG_MIB, "%s: MIB MAC_ADDR: mac_addr %s res 0x%x 0x%x",
-		 priv->netdev->name,
+		 wiphy_name(priv->hw->wiphy),
 		 mac2str(m->mac_addr), m->res[0], m->res[1]);
 	for (i = 0; i < ARRAY_SIZE(m->group_addr); i++)
 		at76_dbg(DBG_MIB, "%s: MIB MAC_ADDR: group addr %d: %s, "
-			 "status %d", priv->netdev->name, i,
+			 "status %d", wiphy_name(priv->hw->wiphy), i,
 			 mac2str(m->group_addr[i]), m->group_addr_status[i]);
 exit:
 	kfree(m);
@@ -1082,13 +1045,13 @@ static void at76_dump_mib_mac_wep(struct at76_priv *priv)
 			   sizeof(struct mib_mac_wep));
 	if (ret < 0) {
 		printk(KERN_ERR "%s: at76_get_mib (MAC_WEP) failed: %d\n",
-		       priv->netdev->name, ret);
+		       wiphy_name(priv->hw->wiphy), ret);
 		goto exit;
 	}
 
 	at76_dbg(DBG_MIB, "%s: MIB MAC_WEP: priv_invoked %u def_key_id %u "
 		 "key_len %u excl_unencr %u wep_icv_err %u wep_excluded %u "
-		 "encr_level %u key %d", priv->netdev->name,
+		 "encr_level %u key %d", wiphy_name(priv->hw->wiphy),
 		 m->privacy_invoked, m->wep_default_key_id,
 		 m->wep_key_mapping_len, m->exclude_unencrypted,
 		 le32_to_cpu(m->wep_icv_error_count),
@@ -1100,12 +1063,55 @@ static void at76_dump_mib_mac_wep(struct at76_priv *priv)
 
 	for (i = 0; i < WEP_KEYS; i++)
 		at76_dbg(DBG_MIB, "%s: MIB MAC_WEP: key %d: %s",
-			 priv->netdev->name, i,
+			 wiphy_name(priv->hw->wiphy), i,
 			 hex2str(m->wep_default_keyvalue[i], key_len));
 exit:
 	kfree(m);
 }
 
+static void at76_dump_mib_mac_encryption(struct at76_priv *priv)
+{
+	int i;
+	int ret;
+	/*int key_len;*/
+	struct mib_mac_encryption *m;
+
+	m = kmalloc(sizeof(struct mib_mac_encryption), GFP_KERNEL);
+	if (!m)
+		return;
+
+	ret = at76_get_mib(priv->udev, MIB_MAC_ENCRYPTION, m,
+			   sizeof(struct mib_mac_encryption));
+	if (ret < 0) {
+		dev_err(&priv->udev->dev,
+			"%s: at76_get_mib (MAC_ENCRYPTION) failed: %d\n",
+			wiphy_name(priv->hw->wiphy), ret);
+		goto exit;
+	}
+
+	at76_dbg(DBG_MIB,
+		 "%s: MIB MAC_ENCRYPTION: tkip_bssid %s priv_invoked %u "
+		 "ciph_key_id %u grp_key_id %u excl_unencr %u "
+		 "ckip_key_perm %u wep_icv_err %u wep_excluded %u",
+		 wiphy_name(priv->hw->wiphy), mac2str(m->tkip_bssid),
+		 m->privacy_invoked, m->cipher_default_key_id,
+		 m->cipher_default_group_key_id, m->exclude_unencrypted,
+		 m->ckip_key_permutation,
+		 le32_to_cpu(m->wep_icv_error_count),
+		 le32_to_cpu(m->wep_excluded_count));
+
+	/*key_len = (m->encryption_level == 1) ?
+	    WEP_SMALL_KEY_LEN : WEP_LARGE_KEY_LEN;*/
+
+	for (i = 0; i < CIPHER_KEYS; i++)
+		at76_dbg(DBG_MIB, "%s: MIB MAC_ENCRYPTION: key %d: %s",
+			 wiphy_name(priv->hw->wiphy), i,
+			 hex2str(m->cipher_default_keyvalue[i],
+				 CIPHER_KEY_LEN));
+exit:
+	kfree(m);
+}
+
 static void at76_dump_mib_mac_mgmt(struct at76_priv *priv)
 {
 	int ret;
@@ -1119,7 +1125,7 @@ static void at76_dump_mib_mac_mgmt(struct at76_priv *priv)
 			   sizeof(struct mib_mac_mgmt));
 	if (ret < 0) {
 		printk(KERN_ERR "%s: at76_get_mib (MAC_MGMT) failed: %d\n",
-		       priv->netdev->name, ret);
+		       wiphy_name(priv->hw->wiphy), ret);
 		goto exit;
 	}
 
@@ -1130,7 +1136,7 @@ static void at76_dump_mib_mac_mgmt(struct at76_priv *priv)
 		 "pm_mode %d ibss_change %d res %d "
 		 "multi_domain_capability_implemented %d "
 		 "international_roaming %d country_string %.3s",
-		 priv->netdev->name, le16_to_cpu(m->beacon_period),
+		 wiphy_name(priv->hw->wiphy), le16_to_cpu(m->beacon_period),
 		 le16_to_cpu(m->CFP_max_duration),
 		 le16_to_cpu(m->medium_occupancy_limit),
 		 le16_to_cpu(m->station_id), le16_to_cpu(m->ATIM_window),
@@ -1155,7 +1161,7 @@ static void at76_dump_mib_mac(struct at76_priv *priv)
 	ret = at76_get_mib(priv->udev, MIB_MAC, m, sizeof(struct mib_mac));
 	if (ret < 0) {
 		printk(KERN_ERR "%s: at76_get_mib (MAC) failed: %d\n",
-		       priv->netdev->name, ret);
+		       wiphy_name(priv->hw->wiphy), ret);
 		goto exit;
 	}
 
@@ -1165,7 +1171,8 @@ static void at76_dump_mib_mac(struct at76_priv *priv)
 		 "scan_type %d scan_channel %d probe_delay %u "
 		 "min_channel_time %d max_channel_time %d listen_int %d "
 		 "desired_ssid %s desired_bssid %s desired_bsstype %d",
-		 priv->netdev->name, le32_to_cpu(m->max_tx_msdu_lifetime),
+		 wiphy_name(priv->hw->wiphy),
+		 le32_to_cpu(m->max_tx_msdu_lifetime),
 		 le32_to_cpu(m->max_rx_lifetime),
 		 le16_to_cpu(m->frag_threshold), le16_to_cpu(m->rts_threshold),
 		 le16_to_cpu(m->cwmin), le16_to_cpu(m->cwmax),
@@ -1191,7 +1198,7 @@ static void at76_dump_mib_phy(struct at76_priv *priv)
 	ret = at76_get_mib(priv->udev, MIB_PHY, m, sizeof(struct mib_phy));
 	if (ret < 0) {
 		printk(KERN_ERR "%s: at76_get_mib (PHY) failed: %d\n",
-		       priv->netdev->name, ret);
+		       wiphy_name(priv->hw->wiphy), ret);
 		goto exit;
 	}
 
@@ -1200,7 +1207,7 @@ static void at76_dump_mib_phy(struct at76_priv *priv)
 		 "mpdu_max_length %d cca_mode_supported %d operation_rate_set "
 		 "0x%x 0x%x 0x%x 0x%x channel_id %d current_cca_mode %d "
 		 "phy_type %d current_reg_domain %d",
-		 priv->netdev->name, le32_to_cpu(m->ed_threshold),
+		 wiphy_name(priv->hw->wiphy), le32_to_cpu(m->ed_threshold),
 		 le16_to_cpu(m->slot_time), le16_to_cpu(m->sifs_time),
 		 le16_to_cpu(m->preamble_length),
 		 le16_to_cpu(m->plcp_header_length),
@@ -1224,13 +1231,14 @@ static void at76_dump_mib_local(struct at76_priv *priv)
 	ret = at76_get_mib(priv->udev, MIB_LOCAL, m, sizeof(struct mib_local));
 	if (ret < 0) {
 		printk(KERN_ERR "%s: at76_get_mib (LOCAL) failed: %d\n",
-		       priv->netdev->name, ret);
+		       wiphy_name(priv->hw->wiphy), ret);
 		goto exit;
 	}
 
 	at76_dbg(DBG_MIB, "%s: MIB LOCAL: beacon_enable %d "
 		 "txautorate_fallback %d ssid_size %d promiscuous_mode %d "
-		 "preamble_type %d", priv->netdev->name, m->beacon_enable,
+		 "preamble_type %d", wiphy_name(priv->hw->wiphy),
+		 m->beacon_enable,
 		 m->txautorate_fallback, m->ssid_size, m->promiscuous_mode,
 		 m->preamble_type);
 exit:
@@ -1249,118 +1257,21 @@ static void at76_dump_mib_mdomain(struct at76_priv *priv)
 			   sizeof(struct mib_mdomain));
 	if (ret < 0) {
 		printk(KERN_ERR "%s: at76_get_mib (MDOMAIN) failed: %d\n",
-		       priv->netdev->name, ret);
+		       wiphy_name(priv->hw->wiphy), ret);
 		goto exit;
 	}
 
 	at76_dbg(DBG_MIB, "%s: MIB MDOMAIN: channel_list %s",
-		 priv->netdev->name,
+		 wiphy_name(priv->hw->wiphy),
 		 hex2str(m->channel_list, sizeof(m->channel_list)));
 
 	at76_dbg(DBG_MIB, "%s: MIB MDOMAIN: tx_powerlevel %s",
-		 priv->netdev->name,
+		 wiphy_name(priv->hw->wiphy),
 		 hex2str(m->tx_powerlevel, sizeof(m->tx_powerlevel)));
 exit:
 	kfree(m);
 }
 
-static int at76_get_current_bssid(struct at76_priv *priv)
-{
-	int ret = 0;
-	struct mib_mac_mgmt *mac_mgmt =
-	    kmalloc(sizeof(struct mib_mac_mgmt), GFP_KERNEL);
-
-	if (!mac_mgmt) {
-		ret = -ENOMEM;
-		goto exit;
-	}
-
-	ret = at76_get_mib(priv->udev, MIB_MAC_MGMT, mac_mgmt,
-			   sizeof(struct mib_mac_mgmt));
-	if (ret < 0) {
-		printk(KERN_ERR "%s: at76_get_mib failed: %d\n",
-		       priv->netdev->name, ret);
-		goto error;
-	}
-	memcpy(priv->bssid, mac_mgmt->current_bssid, ETH_ALEN);
-	printk(KERN_INFO "%s: using BSSID %s\n", priv->netdev->name,
-	       mac2str(priv->bssid));
-error:
-	kfree(mac_mgmt);
-exit:
-	return ret;
-}
-
-static int at76_get_current_channel(struct at76_priv *priv)
-{
-	int ret = 0;
-	struct mib_phy *phy = kmalloc(sizeof(struct mib_phy), GFP_KERNEL);
-
-	if (!phy) {
-		ret = -ENOMEM;
-		goto exit;
-	}
-	ret = at76_get_mib(priv->udev, MIB_PHY, phy, sizeof(struct mib_phy));
-	if (ret < 0) {
-		printk(KERN_ERR "%s: at76_get_mib(MIB_PHY) failed: %d\n",
-		       priv->netdev->name, ret);
-		goto error;
-	}
-	priv->channel = phy->channel_id;
-error:
-	kfree(phy);
-exit:
-	return ret;
-}
-
-/**
- * at76_start_scan - start a scan
- *
- * @use_essid - use the configured ESSID in non passive mode
- */
-static int at76_start_scan(struct at76_priv *priv, int use_essid)
-{
-	struct at76_req_scan scan;
-
-	memset(&scan, 0, sizeof(struct at76_req_scan));
-	memset(scan.bssid, 0xff, ETH_ALEN);
-
-	if (use_essid) {
-		memcpy(scan.essid, priv->essid, IW_ESSID_MAX_SIZE);
-		scan.essid_size = priv->essid_size;
-	} else
-		scan.essid_size = 0;
-
-	/* jal: why should we start at a certain channel? we do scan the whole
-	   range allowed by reg domain. */
-	scan.channel = priv->channel;
-
-	/* atmelwlandriver differs between scan type 0 and 1 (active/passive)
-	   For ad-hoc mode, it uses type 0 only. */
-	scan.scan_type = priv->scan_mode;
-
-	/* INFO: For probe_delay, not multiplying by 1024 as this will be
-	   slightly less than min_channel_time
-	   (per spec: probe delay < min. channel time) */
-	scan.min_channel_time = cpu_to_le16(priv->scan_min_time);
-	scan.max_channel_time = cpu_to_le16(priv->scan_max_time);
-	scan.probe_delay = cpu_to_le16(priv->scan_min_time * 1000);
-	scan.international_scan = 0;
-
-	/* other values are set to 0 for type 0 */
-
-	at76_dbg(DBG_PROGRESS, "%s: start_scan (use_essid = %d, intl = %d, "
-		 "channel = %d, probe_delay = %d, scan_min_time = %d, "
-		 "scan_max_time = %d)",
-		 priv->netdev->name, use_essid,
-		 scan.international_scan, scan.channel,
-		 le16_to_cpu(scan.probe_delay),
-		 le16_to_cpu(scan.min_channel_time),
-		 le16_to_cpu(scan.max_channel_time));
-
-	return at76_set_card_command(priv->udev, CMD_SCAN, &scan, sizeof(scan));
-}
-
 /* Enable monitor mode */
 static int at76_start_monitor(struct at76_priv *priv)
 {
@@ -1381,86 +1292,6 @@ static int at76_start_monitor(struct at76_priv *priv)
 	return ret;
 }
 
-static int at76_start_ibss(struct at76_priv *priv)
-{
-	struct at76_req_ibss bss;
-	int ret;
-
-	WARN_ON(priv->mac_state != MAC_OWN_IBSS);
-	if (priv->mac_state != MAC_OWN_IBSS)
-		return -EBUSY;
-
-	memset(&bss, 0, sizeof(struct at76_req_ibss));
-	memset(bss.bssid, 0xff, ETH_ALEN);
-	memcpy(bss.essid, priv->essid, IW_ESSID_MAX_SIZE);
-	bss.essid_size = priv->essid_size;
-	bss.bss_type = ADHOC_MODE;
-	bss.channel = priv->channel;
-
-	ret = at76_set_card_command(priv->udev, CMD_START_IBSS, &bss,
-				    sizeof(struct at76_req_ibss));
-	if (ret < 0) {
-		printk(KERN_ERR "%s: start_ibss failed: %d\n",
-		       priv->netdev->name, ret);
-		return ret;
-	}
-
-	ret = at76_wait_completion(priv, CMD_START_IBSS);
-	if (ret != CMD_STATUS_COMPLETE) {
-		printk(KERN_ERR "%s: start_ibss failed to complete, %d\n",
-		       priv->netdev->name, ret);
-		return ret;
-	}
-
-	ret = at76_get_current_bssid(priv);
-	if (ret < 0)
-		return ret;
-
-	ret = at76_get_current_channel(priv);
-	if (ret < 0)
-		return ret;
-
-	/* not sure what this is good for ??? */
-	priv->mib_buf.type = MIB_MAC_MGMT;
-	priv->mib_buf.size = 1;
-	priv->mib_buf.index = offsetof(struct mib_mac_mgmt, ibss_change);
-	priv->mib_buf.data.byte = 0;
-
-	ret = at76_set_mib(priv, &priv->mib_buf);
-	if (ret < 0) {
-		printk(KERN_ERR "%s: set_mib (ibss change ok) failed: %d\n",
-		       priv->netdev->name, ret);
-		return ret;
-	}
-
-	netif_carrier_on(priv->netdev);
-	netif_start_queue(priv->netdev);
-	return 0;
-}
-
-/* Request card to join BSS in managed or ad-hoc mode */
-static int at76_join_bss(struct at76_priv *priv, struct bss_info *ptr)
-{
-	struct at76_req_join join;
-
-	BUG_ON(!ptr);
-
-	memset(&join, 0, sizeof(struct at76_req_join));
-	memcpy(join.bssid, ptr->bssid, ETH_ALEN);
-	memcpy(join.essid, ptr->ssid, ptr->ssid_len);
-	join.essid_size = ptr->ssid_len;
-	join.bss_type = (priv->iw_mode == IW_MODE_ADHOC ? 1 : 2);
-	join.channel = ptr->channel;
-	join.timeout = cpu_to_le16(2000);
-
-	at76_dbg(DBG_PROGRESS,
-		 "%s join addr %s ssid %s type %d ch %d timeout %d",
-		 priv->netdev->name, mac2str(join.bssid), join.essid,
-		 join.bss_type, join.channel, le16_to_cpu(join.timeout));
-	return at76_set_card_command(priv->udev, CMD_JOIN, &join,
-				     sizeof(struct at76_req_join));
-}
-
 /* Calculate padding from txbuf->wlength (which excludes the USB TX header),
    likely to compensate a flaw in the AT76C503A USB part ... */
 static inline int at76_calc_padding(int wlen)
@@ -1479,14 +1310,6 @@ static inline int at76_calc_padding(int wlen)
 	return 0;
 }
 
-/* We are doing a lot of things here in an interrupt. Need
-   a bh handler (Watching TV with a TV card is probably
-   a good test: if you see flickers, we are doing too much.
-   Currently I do see flickers... even with our tasklet :-( )
-   Maybe because the bttv driver and usb-uhci use the same interrupt
-*/
-/* Or maybe because our BH handler is preempting bttv's BH handler.. BHs don't
- * solve everything.. (alex) */
 static void at76_rx_callback(struct urb *urb)
 {
 	struct at76_priv *priv = urb->context;
@@ -1496,1758 +1319,6 @@ static void at76_rx_callback(struct urb *urb)
 	return;
 }
 
-static void at76_tx_callback(struct urb *urb)
-{
-	struct at76_priv *priv = urb->context;
-	struct net_device_stats *stats = &priv->stats;
-	unsigned long flags;
-	struct at76_tx_buffer *mgmt_buf;
-	int ret;
-
-	switch (urb->status) {
-	case 0:
-		stats->tx_packets++;
-		break;
-	case -ENOENT:
-	case -ECONNRESET:
-		/* urb has been unlinked */
-		return;
-	default:
-		at76_dbg(DBG_URB, "%s - nonzero tx status received: %d",
-			 __func__, urb->status);
-		stats->tx_errors++;
-		break;
-	}
-
-	spin_lock_irqsave(&priv->mgmt_spinlock, flags);
-	mgmt_buf = priv->next_mgmt_bulk;
-	priv->next_mgmt_bulk = NULL;
-	spin_unlock_irqrestore(&priv->mgmt_spinlock, flags);
-
-	if (!mgmt_buf) {
-		netif_wake_queue(priv->netdev);
-		return;
-	}
-
-	/* we don't copy the padding bytes, but add them
-	   to the length */
-	memcpy(priv->bulk_out_buffer, mgmt_buf,
-	       le16_to_cpu(mgmt_buf->wlength) + AT76_TX_HDRLEN);
-	usb_fill_bulk_urb(priv->tx_urb, priv->udev, priv->tx_pipe,
-			  priv->bulk_out_buffer,
-			  le16_to_cpu(mgmt_buf->wlength) + mgmt_buf->padding +
-			  AT76_TX_HDRLEN, at76_tx_callback, priv);
-	ret = usb_submit_urb(priv->tx_urb, GFP_ATOMIC);
-	if (ret)
-		printk(KERN_ERR "%s: error in tx submit urb: %d\n",
-		       priv->netdev->name, ret);
-
-	kfree(mgmt_buf);
-}
-
-/* Send a management frame on bulk-out.  txbuf->wlength must be set */
-static int at76_tx_mgmt(struct at76_priv *priv, struct at76_tx_buffer *txbuf)
-{
-	unsigned long flags;
-	int ret;
-	int urb_status;
-	void *oldbuf = NULL;
-
-	netif_carrier_off(priv->netdev);	/* stop netdev watchdog */
-	netif_stop_queue(priv->netdev);	/* stop tx data packets */
-
-	spin_lock_irqsave(&priv->mgmt_spinlock, flags);
-
-	urb_status = priv->tx_urb->status;
-	if (urb_status == -EINPROGRESS) {
-		/* cannot transmit now, put in the queue */
-		oldbuf = priv->next_mgmt_bulk;
-		priv->next_mgmt_bulk = txbuf;
-	}
-	spin_unlock_irqrestore(&priv->mgmt_spinlock, flags);
-
-	if (oldbuf) {
-		/* a data/mgmt tx is already pending in the URB -
-		   if this is no error in some situations we must
-		   implement a queue or silently modify the old msg */
-		printk(KERN_ERR "%s: removed pending mgmt buffer %s\n",
-		       priv->netdev->name, hex2str(oldbuf, 64));
-		kfree(oldbuf);
-		return 0;
-	}
-
-	txbuf->tx_rate = TX_RATE_1MBIT;
-	txbuf->padding = at76_calc_padding(le16_to_cpu(txbuf->wlength));
-	memset(txbuf->reserved, 0, sizeof(txbuf->reserved));
-
-	if (priv->next_mgmt_bulk)
-		printk(KERN_ERR "%s: URB status %d, but mgmt is pending\n",
-		       priv->netdev->name, urb_status);
-
-	at76_dbg(DBG_TX_MGMT,
-		 "%s: tx mgmt: wlen %d tx_rate %d pad %d %s",
-		 priv->netdev->name, le16_to_cpu(txbuf->wlength),
-		 txbuf->tx_rate, txbuf->padding,
-		 hex2str(txbuf->packet, le16_to_cpu(txbuf->wlength)));
-
-	/* txbuf was not consumed above -> send mgmt msg immediately */
-	memcpy(priv->bulk_out_buffer, txbuf,
-	       le16_to_cpu(txbuf->wlength) + AT76_TX_HDRLEN);
-	usb_fill_bulk_urb(priv->tx_urb, priv->udev, priv->tx_pipe,
-			  priv->bulk_out_buffer,
-			  le16_to_cpu(txbuf->wlength) + txbuf->padding +
-			  AT76_TX_HDRLEN, at76_tx_callback, priv);
-	ret = usb_submit_urb(priv->tx_urb, GFP_ATOMIC);
-	if (ret)
-		printk(KERN_ERR "%s: error in tx submit urb: %d\n",
-		       priv->netdev->name, ret);
-
-	kfree(txbuf);
-
-	return ret;
-}
-
-/* Go to the next information element */
-static inline void next_ie(struct ieee80211_info_element **ie)
-{
-	*ie = (struct ieee80211_info_element *)(&(*ie)->data[(*ie)->len]);
-}
-
-/* Challenge is the challenge string (in TLV format)
-   we got with seq_nr 2 for shared secret authentication only and
-   send in seq_nr 3 WEP encrypted to prove we have the correct WEP key;
-   otherwise it is NULL */
-static int at76_auth_req(struct at76_priv *priv, struct bss_info *bss,
-			 int seq_nr, struct ieee80211_info_element *challenge)
-{
-	struct at76_tx_buffer *tx_buffer;
-	struct ieee80211_hdr_3addr *mgmt;
-	struct ieee80211_auth *req;
-	int buf_len = (seq_nr != 3 ? AUTH_FRAME_SIZE :
-		       AUTH_FRAME_SIZE + 1 + 1 + challenge->len);
-
-	BUG_ON(!bss);
-	BUG_ON(seq_nr == 3 && !challenge);
-	tx_buffer = kmalloc(buf_len + MAX_PADDING_SIZE, GFP_ATOMIC);
-	if (!tx_buffer)
-		return -ENOMEM;
-
-	req = (struct ieee80211_auth *)tx_buffer->packet;
-	mgmt = &req->header;
-
-	/* make wireless header */
-	/* first auth msg is not encrypted, only the second (seq_nr == 3) */
-	mgmt->frame_ctl =
-	    cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_AUTH |
-			(seq_nr == 3 ? IEEE80211_FCTL_PROTECTED : 0));
-
-	mgmt->duration_id = cpu_to_le16(0x8000);
-	memcpy(mgmt->addr1, bss->bssid, ETH_ALEN);
-	memcpy(mgmt->addr2, priv->netdev->dev_addr, ETH_ALEN);
-	memcpy(mgmt->addr3, bss->bssid, ETH_ALEN);
-	mgmt->seq_ctl = cpu_to_le16(0);
-
-	req->algorithm = cpu_to_le16(priv->auth_mode);
-	req->transaction = cpu_to_le16(seq_nr);
-	req->status = cpu_to_le16(0);
-
-	if (seq_nr == 3)
-		memcpy(req->info_element, challenge, 1 + 1 + challenge->len);
-
-	/* init. at76_priv tx header */
-	tx_buffer->wlength = cpu_to_le16(buf_len - AT76_TX_HDRLEN);
-	at76_dbg(DBG_TX_MGMT, "%s: AuthReq bssid %s alg %d seq_nr %d",
-		 priv->netdev->name, mac2str(mgmt->addr3),
-		 le16_to_cpu(req->algorithm), le16_to_cpu(req->transaction));
-	if (seq_nr == 3)
-		at76_dbg(DBG_TX_MGMT, "%s: AuthReq challenge: %s ...",
-			 priv->netdev->name, hex2str(req->info_element, 18));
-
-	/* either send immediately (if no data tx is pending
-	   or put it in pending list */
-	return at76_tx_mgmt(priv, tx_buffer);
-}
-
-static int at76_assoc_req(struct at76_priv *priv, struct bss_info *bss)
-{
-	struct at76_tx_buffer *tx_buffer;
-	struct ieee80211_hdr_3addr *mgmt;
-	struct ieee80211_assoc_request *req;
-	struct ieee80211_info_element *ie;
-	char *essid;
-	int essid_len;
-	u16 capa;
-
-	BUG_ON(!bss);
-
-	tx_buffer = kmalloc(ASSOCREQ_MAX_SIZE + MAX_PADDING_SIZE, GFP_ATOMIC);
-	if (!tx_buffer)
-		return -ENOMEM;
-
-	req = (struct ieee80211_assoc_request *)tx_buffer->packet;
-	mgmt = &req->header;
-	ie = req->info_element;
-
-	/* make wireless header */
-	mgmt->frame_ctl = cpu_to_le16(IEEE80211_FTYPE_MGMT |
-				      IEEE80211_STYPE_ASSOC_REQ);
-
-	mgmt->duration_id = cpu_to_le16(0x8000);
-	memcpy(mgmt->addr1, bss->bssid, ETH_ALEN);
-	memcpy(mgmt->addr2, priv->netdev->dev_addr, ETH_ALEN);
-	memcpy(mgmt->addr3, bss->bssid, ETH_ALEN);
-	mgmt->seq_ctl = cpu_to_le16(0);
-
-	/* we must set the Privacy bit in the capabilities to assure an
-	   Agere-based AP with optional WEP transmits encrypted frames
-	   to us.  AP only set the Privacy bit in their capabilities
-	   if WEP is mandatory in the BSS! */
-	capa = bss->capa;
-	if (priv->wep_enabled)
-		capa |= WLAN_CAPABILITY_PRIVACY;
-	if (priv->preamble_type != PREAMBLE_TYPE_LONG)
-		capa |= WLAN_CAPABILITY_SHORT_PREAMBLE;
-	req->capability = cpu_to_le16(capa);
-
-	req->listen_interval = cpu_to_le16(2 * bss->beacon_interval);
-
-	/* write TLV data elements */
-
-	ie->id = MFIE_TYPE_SSID;
-	ie->len = bss->ssid_len;
-	memcpy(ie->data, bss->ssid, bss->ssid_len);
-	next_ie(&ie);
-
-	ie->id = MFIE_TYPE_RATES;
-	ie->len = sizeof(hw_rates);
-	memcpy(ie->data, hw_rates, sizeof(hw_rates));
-	next_ie(&ie);		/* ie points behind the supp_rates field */
-
-	/* init. at76_priv tx header */
-	tx_buffer->wlength = cpu_to_le16((u8 *)ie - (u8 *)mgmt);
-
-	ie = req->info_element;
-	essid = ie->data;
-	essid_len = min_t(int, IW_ESSID_MAX_SIZE, ie->len);
-
-	next_ie(&ie);		/* points to IE of rates now */
-	at76_dbg(DBG_TX_MGMT,
-		 "%s: AssocReq bssid %s capa 0x%04x ssid %.*s rates %s",
-		 priv->netdev->name, mac2str(mgmt->addr3),
-		 le16_to_cpu(req->capability), essid_len, essid,
-		 hex2str(ie->data, ie->len));
-
-	/* either send immediately (if no data tx is pending
-	   or put it in pending list */
-	return at76_tx_mgmt(priv, tx_buffer);
-}
-
-/* We got to check the bss_list for old entries */
-static void at76_bss_list_timeout(unsigned long par)
-{
-	struct at76_priv *priv = (struct at76_priv *)par;
-	unsigned long flags;
-	struct list_head *lptr, *nptr;
-	struct bss_info *ptr;
-
-	spin_lock_irqsave(&priv->bss_list_spinlock, flags);
-
-	list_for_each_safe(lptr, nptr, &priv->bss_list) {
-
-		ptr = list_entry(lptr, struct bss_info, list);
-
-		if (ptr != priv->curr_bss
-		    && time_after(jiffies, ptr->last_rx + BSS_LIST_TIMEOUT)) {
-			at76_dbg(DBG_BSS_TABLE_RM,
-				 "%s: bss_list: removing old BSS %s ch %d",
-				 priv->netdev->name, mac2str(ptr->bssid),
-				 ptr->channel);
-			list_del(&ptr->list);
-			kfree(ptr);
-		}
-	}
-	spin_unlock_irqrestore(&priv->bss_list_spinlock, flags);
-	/* restart the timer */
-	mod_timer(&priv->bss_list_timer, jiffies + BSS_LIST_TIMEOUT);
-}
-
-static inline void at76_set_mac_state(struct at76_priv *priv,
-				      enum mac_state mac_state)
-{
-	at76_dbg(DBG_MAC_STATE, "%s state: %s", priv->netdev->name,
-		 mac_states[mac_state]);
-	priv->mac_state = mac_state;
-}
-
-static void at76_dump_bss_table(struct at76_priv *priv)
-{
-	struct bss_info *ptr;
-	unsigned long flags;
-	struct list_head *lptr;
-
-	spin_lock_irqsave(&priv->bss_list_spinlock, flags);
-
-	at76_dbg(DBG_BSS_TABLE, "%s BSS table (curr=%p):", priv->netdev->name,
-		 priv->curr_bss);
-
-	list_for_each(lptr, &priv->bss_list) {
-		ptr = list_entry(lptr, struct bss_info, list);
-		at76_dbg(DBG_BSS_TABLE, "0x%p: bssid %s channel %d ssid %.*s "
-			 "(%s) capa 0x%04x rates %s rssi %d link %d noise %d",
-			 ptr, mac2str(ptr->bssid), ptr->channel, ptr->ssid_len,
-			 ptr->ssid, hex2str(ptr->ssid, ptr->ssid_len),
-			 ptr->capa, hex2str(ptr->rates, ptr->rates_len),
-			 ptr->rssi, ptr->link_qual, ptr->noise_level);
-	}
-	spin_unlock_irqrestore(&priv->bss_list_spinlock, flags);
-}
-
-/* Called upon successful association to mark interface as connected */
-static void at76_work_assoc_done(struct work_struct *work)
-{
-	struct at76_priv *priv = container_of(work, struct at76_priv,
-					      work_assoc_done);
-
-	mutex_lock(&priv->mtx);
-
-	WARN_ON(priv->mac_state != MAC_ASSOC);
-	WARN_ON(!priv->curr_bss);
-	if (priv->mac_state != MAC_ASSOC || !priv->curr_bss)
-		goto exit;
-
-	if (priv->iw_mode == IW_MODE_INFRA) {
-		if (priv->pm_mode != AT76_PM_OFF) {
-			/* calculate the listen interval in units of
-			   beacon intervals of the curr_bss */
-			u32 pm_period_beacon = (priv->pm_period >> 10) /
-			    priv->curr_bss->beacon_interval;
-
-			pm_period_beacon = max(pm_period_beacon, 2u);
-			pm_period_beacon = min(pm_period_beacon, 0xffffu);
-
-			at76_dbg(DBG_PM,
-				 "%s: pm_mode %d assoc id 0x%x listen int %d",
-				 priv->netdev->name, priv->pm_mode,
-				 priv->assoc_id, pm_period_beacon);
-
-			at76_set_associd(priv, priv->assoc_id);
-			at76_set_listen_interval(priv, (u16)pm_period_beacon);
-		}
-		schedule_delayed_work(&priv->dwork_beacon, BEACON_TIMEOUT);
-	}
-	at76_set_pm_mode(priv);
-
-	netif_carrier_on(priv->netdev);
-	netif_wake_queue(priv->netdev);
-	at76_set_mac_state(priv, MAC_CONNECTED);
-	at76_iwevent_bss_connect(priv->netdev, priv->curr_bss->bssid);
-	at76_dbg(DBG_PROGRESS, "%s: connected to BSSID %s",
-		 priv->netdev->name, mac2str(priv->curr_bss->bssid));
-
-exit:
-	mutex_unlock(&priv->mtx);
-}
-
-/* We only store the new mac address in netdev struct,
-   it gets set when the netdev is opened. */
-static int at76_set_mac_address(struct net_device *netdev, void *addr)
-{
-	struct sockaddr *mac = addr;
-	memcpy(netdev->dev_addr, mac->sa_data, ETH_ALEN);
-	return 1;
-}
-
-static struct net_device_stats *at76_get_stats(struct net_device *netdev)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	return &priv->stats;
-}
-
-static struct iw_statistics *at76_get_wireless_stats(struct net_device *netdev)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-
-	at76_dbg(DBG_IOCTL, "RETURN qual %d level %d noise %d updated %d",
-		 priv->wstats.qual.qual, priv->wstats.qual.level,
-		 priv->wstats.qual.noise, priv->wstats.qual.updated);
-
-	return &priv->wstats;
-}
-
-static void at76_set_multicast(struct net_device *netdev)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int promisc;
-
-	promisc = ((netdev->flags & IFF_PROMISC) != 0);
-	if (promisc != priv->promisc) {
-		/* This gets called in interrupt, must reschedule */
-		priv->promisc = promisc;
-		schedule_work(&priv->work_set_promisc);
-	}
-}
-
-/* Stop all network activity, flush all pending tasks */
-static void at76_quiesce(struct at76_priv *priv)
-{
-	unsigned long flags;
-
-	netif_stop_queue(priv->netdev);
-	netif_carrier_off(priv->netdev);
-
-	at76_set_mac_state(priv, MAC_INIT);
-
-	cancel_delayed_work(&priv->dwork_get_scan);
-	cancel_delayed_work(&priv->dwork_beacon);
-	cancel_delayed_work(&priv->dwork_auth);
-	cancel_delayed_work(&priv->dwork_assoc);
-	cancel_delayed_work(&priv->dwork_restart);
-
-	spin_lock_irqsave(&priv->mgmt_spinlock, flags);
-	kfree(priv->next_mgmt_bulk);
-	priv->next_mgmt_bulk = NULL;
-	spin_unlock_irqrestore(&priv->mgmt_spinlock, flags);
-}
-
-/*******************************************************************************
- * at76_priv implementations of iw_handler functions:
- */
-static int at76_iw_handler_commit(struct net_device *netdev,
-				  struct iw_request_info *info,
-				  void *null, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-
-	at76_dbg(DBG_IOCTL, "%s %s: restarting the device", netdev->name,
-		 __func__);
-
-	if (priv->mac_state != MAC_INIT)
-		at76_quiesce(priv);
-
-	/* Wait half second before the restart to process subsequent
-	 * requests from the same iwconfig in a single restart */
-	schedule_delayed_work(&priv->dwork_restart, HZ / 2);
-
-	return 0;
-}
-
-static int at76_iw_handler_get_name(struct net_device *netdev,
-				    struct iw_request_info *info,
-				    char *name, char *extra)
-{
-	strcpy(name, "IEEE 802.11b");
-	at76_dbg(DBG_IOCTL, "%s: SIOCGIWNAME - name %s", netdev->name, name);
-	return 0;
-}
-
-static int at76_iw_handler_set_freq(struct net_device *netdev,
-				    struct iw_request_info *info,
-				    struct iw_freq *freq, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int chan = -1;
-	int ret = -EIWCOMMIT;
-	at76_dbg(DBG_IOCTL, "%s: SIOCSIWFREQ - freq.m %d freq.e %d",
-		 netdev->name, freq->m, freq->e);
-
-	if ((freq->e == 0) && (freq->m <= 1000))
-		/* Setting by channel number */
-		chan = freq->m;
-	else {
-		/* Setting by frequency - search the table */
-		int mult = 1;
-		int i;
-
-		for (i = 0; i < (6 - freq->e); i++)
-			mult *= 10;
-
-		for (i = 0; i < NUM_CHANNELS; i++) {
-			if (freq->m == (channel_frequency[i] * mult))
-				chan = i + 1;
-		}
-	}
-
-	if (chan < 1 || !priv->domain)
-		/* non-positive channels are invalid
-		 * we need a domain info to set the channel
-		 * either that or an invalid frequency was
-		 * provided by the user */
-		ret = -EINVAL;
-	else if (!(priv->domain->channel_map & (1 << (chan - 1)))) {
-		printk(KERN_INFO "%s: channel %d not allowed for domain %s\n",
-		       priv->netdev->name, chan, priv->domain->name);
-		ret = -EINVAL;
-	}
-
-	if (ret == -EIWCOMMIT) {
-		priv->channel = chan;
-		at76_dbg(DBG_IOCTL, "%s: SIOCSIWFREQ - ch %d", netdev->name,
-			 chan);
-	}
-
-	return ret;
-}
-
-static int at76_iw_handler_get_freq(struct net_device *netdev,
-				    struct iw_request_info *info,
-				    struct iw_freq *freq, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-
-	freq->m = priv->channel;
-	freq->e = 0;
-
-	if (priv->channel)
-		at76_dbg(DBG_IOCTL, "%s: SIOCGIWFREQ - freq %ld x 10e%d",
-			 netdev->name, channel_frequency[priv->channel - 1], 6);
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCGIWFREQ - ch %d", netdev->name,
-		 priv->channel);
-
-	return 0;
-}
-
-static int at76_iw_handler_set_mode(struct net_device *netdev,
-				    struct iw_request_info *info,
-				    __u32 *mode, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCSIWMODE - %d", netdev->name, *mode);
-
-	if ((*mode != IW_MODE_ADHOC) && (*mode != IW_MODE_INFRA) &&
-	    (*mode != IW_MODE_MONITOR))
-		return -EINVAL;
-
-	priv->iw_mode = *mode;
-	if (priv->iw_mode != IW_MODE_INFRA)
-		priv->pm_mode = AT76_PM_OFF;
-
-	return -EIWCOMMIT;
-}
-
-static int at76_iw_handler_get_mode(struct net_device *netdev,
-				    struct iw_request_info *info,
-				    __u32 *mode, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-
-	*mode = priv->iw_mode;
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCGIWMODE - %d", netdev->name, *mode);
-
-	return 0;
-}
-
-static int at76_iw_handler_get_range(struct net_device *netdev,
-				     struct iw_request_info *info,
-				     struct iw_point *data, char *extra)
-{
-	/* inspired by atmel.c */
-	struct at76_priv *priv = netdev_priv(netdev);
-	struct iw_range *range = (struct iw_range *)extra;
-	int i;
-
-	data->length = sizeof(struct iw_range);
-	memset(range, 0, sizeof(struct iw_range));
-
-	/* TODO: range->throughput = xxxxxx; */
-
-	range->min_nwid = 0x0000;
-	range->max_nwid = 0x0000;
-
-	/* this driver doesn't maintain sensitivity information */
-	range->sensitivity = 0;
-
-	range->max_qual.qual = 100;
-	range->max_qual.level = 100;
-	range->max_qual.noise = 0;
-	range->max_qual.updated = IW_QUAL_NOISE_INVALID;
-
-	range->avg_qual.qual = 50;
-	range->avg_qual.level = 50;
-	range->avg_qual.noise = 0;
-	range->avg_qual.updated = IW_QUAL_NOISE_INVALID;
-
-	range->bitrate[0] = 1000000;
-	range->bitrate[1] = 2000000;
-	range->bitrate[2] = 5500000;
-	range->bitrate[3] = 11000000;
-	range->num_bitrates = 4;
-
-	range->min_rts = 0;
-	range->max_rts = MAX_RTS_THRESHOLD;
-
-	range->min_frag = MIN_FRAG_THRESHOLD;
-	range->max_frag = MAX_FRAG_THRESHOLD;
-
-	range->pmp_flags = IW_POWER_PERIOD;
-	range->pmt_flags = IW_POWER_ON;
-	range->pm_capa = IW_POWER_PERIOD | IW_POWER_ALL_R;
-
-	range->encoding_size[0] = WEP_SMALL_KEY_LEN;
-	range->encoding_size[1] = WEP_LARGE_KEY_LEN;
-	range->num_encoding_sizes = 2;
-	range->max_encoding_tokens = WEP_KEYS;
-
-	/* both WL-240U and Linksys WUSB11 v2.6 specify 15 dBm as output power
-	   - take this for all (ignore antenna gains) */
-	range->txpower[0] = 15;
-	range->num_txpower = 1;
-	range->txpower_capa = IW_TXPOW_DBM;
-
-	range->we_version_source = WIRELESS_EXT;
-	range->we_version_compiled = WIRELESS_EXT;
-
-	/* same as the values used in atmel.c */
-	range->retry_capa = IW_RETRY_LIMIT;
-	range->retry_flags = IW_RETRY_LIMIT;
-	range->r_time_flags = 0;
-	range->min_retry = 1;
-	range->max_retry = 255;
-
-	range->num_channels = NUM_CHANNELS;
-	range->num_frequency = 0;
-
-	for (i = 0; i < NUM_CHANNELS; i++) {
-		/* test if channel map bit is raised */
-		if (priv->domain->channel_map & (0x1 << i)) {
-			range->num_frequency += 1;
-
-			range->freq[i].i = i + 1;
-			range->freq[i].m = channel_frequency[i] * 100000;
-			range->freq[i].e = 1;	/* freq * 10^1 */
-		}
-	}
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCGIWRANGE", netdev->name);
-
-	return 0;
-}
-
-static int at76_iw_handler_set_spy(struct net_device *netdev,
-				   struct iw_request_info *info,
-				   struct iw_point *data, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int ret = 0;
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCSIWSPY - number of addresses %d",
-		 netdev->name, data->length);
-
-	spin_lock_bh(&priv->spy_spinlock);
-	ret = iw_handler_set_spy(priv->netdev, info, (union iwreq_data *)data,
-				 extra);
-	spin_unlock_bh(&priv->spy_spinlock);
-
-	return ret;
-}
-
-static int at76_iw_handler_get_spy(struct net_device *netdev,
-				   struct iw_request_info *info,
-				   struct iw_point *data, char *extra)
-{
-
-	struct at76_priv *priv = netdev_priv(netdev);
-	int ret = 0;
-
-	spin_lock_bh(&priv->spy_spinlock);
-	ret = iw_handler_get_spy(priv->netdev, info,
-				 (union iwreq_data *)data, extra);
-	spin_unlock_bh(&priv->spy_spinlock);
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCGIWSPY - number of addresses %d",
-		 netdev->name, data->length);
-
-	return ret;
-}
-
-static int at76_iw_handler_set_thrspy(struct net_device *netdev,
-				      struct iw_request_info *info,
-				      struct iw_point *data, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int ret;
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCSIWTHRSPY - number of addresses %d)",
-		 netdev->name, data->length);
-
-	spin_lock_bh(&priv->spy_spinlock);
-	ret = iw_handler_set_thrspy(netdev, info, (union iwreq_data *)data,
-				    extra);
-	spin_unlock_bh(&priv->spy_spinlock);
-
-	return ret;
-}
-
-static int at76_iw_handler_get_thrspy(struct net_device *netdev,
-				      struct iw_request_info *info,
-				      struct iw_point *data, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int ret;
-
-	spin_lock_bh(&priv->spy_spinlock);
-	ret = iw_handler_get_thrspy(netdev, info, (union iwreq_data *)data,
-				    extra);
-	spin_unlock_bh(&priv->spy_spinlock);
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCGIWTHRSPY - number of addresses %d)",
-		 netdev->name, data->length);
-
-	return ret;
-}
-
-static int at76_iw_handler_set_wap(struct net_device *netdev,
-				   struct iw_request_info *info,
-				   struct sockaddr *ap_addr, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCSIWAP - wap/bssid %s", netdev->name,
-		 mac2str(ap_addr->sa_data));
-
-	/* if the incoming address == ff:ff:ff:ff:ff:ff, the user has
-	   chosen any or auto AP preference */
-	if (is_broadcast_ether_addr(ap_addr->sa_data)
-	    || is_zero_ether_addr(ap_addr->sa_data))
-		priv->wanted_bssid_valid = 0;
-	else {
-		/* user wants to set a preferred AP address */
-		priv->wanted_bssid_valid = 1;
-		memcpy(priv->wanted_bssid, ap_addr->sa_data, ETH_ALEN);
-	}
-
-	return -EIWCOMMIT;
-}
-
-static int at76_iw_handler_get_wap(struct net_device *netdev,
-				   struct iw_request_info *info,
-				   struct sockaddr *ap_addr, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-
-	ap_addr->sa_family = ARPHRD_ETHER;
-	memcpy(ap_addr->sa_data, priv->bssid, ETH_ALEN);
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCGIWAP - wap/bssid %s", netdev->name,
-		 mac2str(ap_addr->sa_data));
-
-	return 0;
-}
-
-static int at76_iw_handler_set_scan(struct net_device *netdev,
-				    struct iw_request_info *info,
-				    union iwreq_data *wrqu, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int ret = 0;
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCSIWSCAN", netdev->name);
-
-	if (mutex_lock_interruptible(&priv->mtx))
-		return -EINTR;
-
-	if (!netif_running(netdev)) {
-		ret = -ENETDOWN;
-		goto exit;
-	}
-
-	/* jal: we don't allow "iwlist ethX scan" while we are
-	   in monitor mode */
-	if (priv->iw_mode == IW_MODE_MONITOR) {
-		ret = -EBUSY;
-		goto exit;
-	}
-
-	/* Discard old scan results */
-	if ((jiffies - priv->last_scan) > (20 * HZ))
-		priv->scan_state = SCAN_IDLE;
-	priv->last_scan = jiffies;
-
-	/* Initiate a scan command */
-	if (priv->scan_state == SCAN_IN_PROGRESS) {
-		ret = -EBUSY;
-		goto exit;
-	}
-
-	priv->scan_state = SCAN_IN_PROGRESS;
-
-	at76_quiesce(priv);
-
-	/* Try to do passive or active scan if WE asks as. */
-	if (wrqu->data.length
-	    && wrqu->data.length == sizeof(struct iw_scan_req)) {
-		struct iw_scan_req *req = (struct iw_scan_req *)extra;
-
-		if (req->scan_type == IW_SCAN_TYPE_PASSIVE)
-			priv->scan_mode = SCAN_TYPE_PASSIVE;
-		else if (req->scan_type == IW_SCAN_TYPE_ACTIVE)
-			priv->scan_mode = SCAN_TYPE_ACTIVE;
-
-		/* Sanity check values? */
-		if (req->min_channel_time > 0)
-			priv->scan_min_time = req->min_channel_time;
-
-		if (req->max_channel_time > 0)
-			priv->scan_max_time = req->max_channel_time;
-	}
-
-	/* change to scanning state */
-	at76_set_mac_state(priv, MAC_SCANNING);
-	schedule_work(&priv->work_start_scan);
-
-exit:
-	mutex_unlock(&priv->mtx);
-	return ret;
-}
-
-static int at76_iw_handler_get_scan(struct net_device *netdev,
-				    struct iw_request_info *info,
-				    struct iw_point *data, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	unsigned long flags;
-	struct list_head *lptr, *nptr;
-	struct bss_info *curr_bss;
-	struct iw_event *iwe = kmalloc(sizeof(struct iw_event), GFP_KERNEL);
-	char *curr_val, *curr_pos = extra;
-	int i;
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCGIWSCAN", netdev->name);
-
-	if (!iwe)
-		return -ENOMEM;
-
-	if (priv->scan_state != SCAN_COMPLETED) {
-		/* scan not yet finished */
-		kfree(iwe);
-		return -EAGAIN;
-	}
-
-	spin_lock_irqsave(&priv->bss_list_spinlock, flags);
-
-	list_for_each_safe(lptr, nptr, &priv->bss_list) {
-		curr_bss = list_entry(lptr, struct bss_info, list);
-
-		iwe->cmd = SIOCGIWAP;
-		iwe->u.ap_addr.sa_family = ARPHRD_ETHER;
-		memcpy(iwe->u.ap_addr.sa_data, curr_bss->bssid, 6);
-		curr_pos = iwe_stream_add_event(info, curr_pos,
-						extra + IW_SCAN_MAX_DATA, iwe,
-						IW_EV_ADDR_LEN);
-
-		iwe->u.data.length = curr_bss->ssid_len;
-		iwe->cmd = SIOCGIWESSID;
-		iwe->u.data.flags = 1;
-
-		curr_pos = iwe_stream_add_point(info, curr_pos,
-						extra + IW_SCAN_MAX_DATA, iwe,
-						curr_bss->ssid);
-
-		iwe->cmd = SIOCGIWMODE;
-		iwe->u.mode = (curr_bss->capa & WLAN_CAPABILITY_IBSS) ?
-		    IW_MODE_ADHOC :
-		    (curr_bss->capa & WLAN_CAPABILITY_ESS) ?
-		    IW_MODE_MASTER : IW_MODE_AUTO;
-		/* IW_MODE_AUTO = 0 which I thought is
-		 * the most logical value to return in this case */
-		curr_pos = iwe_stream_add_event(info, curr_pos,
-						extra + IW_SCAN_MAX_DATA, iwe,
-						IW_EV_UINT_LEN);
-
-		iwe->cmd = SIOCGIWFREQ;
-		iwe->u.freq.m = curr_bss->channel;
-		iwe->u.freq.e = 0;
-		curr_pos = iwe_stream_add_event(info, curr_pos,
-						extra + IW_SCAN_MAX_DATA, iwe,
-						IW_EV_FREQ_LEN);
-
-		iwe->cmd = SIOCGIWENCODE;
-		if (curr_bss->capa & WLAN_CAPABILITY_PRIVACY)
-			iwe->u.data.flags = IW_ENCODE_ENABLED | IW_ENCODE_NOKEY;
-		else
-			iwe->u.data.flags = IW_ENCODE_DISABLED;
-
-		iwe->u.data.length = 0;
-		curr_pos = iwe_stream_add_point(info, curr_pos,
-						extra + IW_SCAN_MAX_DATA, iwe,
-						NULL);
-
-		/* Add quality statistics */
-		iwe->cmd = IWEVQUAL;
-		iwe->u.qual.noise = 0;
-		iwe->u.qual.updated =
-		    IW_QUAL_NOISE_INVALID | IW_QUAL_LEVEL_UPDATED;
-		iwe->u.qual.level = (curr_bss->rssi * 100 / 42);
-		if (iwe->u.qual.level > 100)
-			iwe->u.qual.level = 100;
-		if (at76_is_intersil(priv->board_type))
-			iwe->u.qual.qual = curr_bss->link_qual;
-		else {
-			iwe->u.qual.qual = 0;
-			iwe->u.qual.updated |= IW_QUAL_QUAL_INVALID;
-		}
-		/* Add new value to event */
-		curr_pos = iwe_stream_add_event(info, curr_pos,
-						extra + IW_SCAN_MAX_DATA, iwe,
-						IW_EV_QUAL_LEN);
-
-		/* Rate: stuffing multiple values in a single event requires
-		 * a bit more of magic - Jean II */
-		curr_val = curr_pos + IW_EV_LCP_LEN;
-
-		iwe->cmd = SIOCGIWRATE;
-		/* Those two flags are ignored... */
-		iwe->u.bitrate.fixed = 0;
-		iwe->u.bitrate.disabled = 0;
-		/* Max 8 values */
-		for (i = 0; i < curr_bss->rates_len; i++) {
-			/* Bit rate given in 500 kb/s units (+ 0x80) */
-			iwe->u.bitrate.value =
-			    ((curr_bss->rates[i] & 0x7f) * 500000);
-			/* Add new value to event */
-			curr_val = iwe_stream_add_value(info, curr_pos,
-							curr_val,
-							extra +
-							IW_SCAN_MAX_DATA, iwe,
-							IW_EV_PARAM_LEN);
-		}
-
-		/* Check if we added any event */
-		if ((curr_val - curr_pos) > IW_EV_LCP_LEN)
-			curr_pos = curr_val;
-
-		/* more information may be sent back using IWECUSTOM */
-
-	}
-
-	spin_unlock_irqrestore(&priv->bss_list_spinlock, flags);
-
-	data->length = (curr_pos - extra);
-	data->flags = 0;
-
-	kfree(iwe);
-	return 0;
-}
-
-static int at76_iw_handler_set_essid(struct net_device *netdev,
-				     struct iw_request_info *info,
-				     struct iw_point *data, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCSIWESSID - %s", netdev->name, extra);
-
-	if (data->flags) {
-		memcpy(priv->essid, extra, data->length);
-		priv->essid_size = data->length;
-	} else
-		priv->essid_size = 0;	/* Use any SSID */
-
-	return -EIWCOMMIT;
-}
-
-static int at76_iw_handler_get_essid(struct net_device *netdev,
-				     struct iw_request_info *info,
-				     struct iw_point *data, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-
-	if (priv->essid_size) {
-		/* not the ANY ssid in priv->essid */
-		data->flags = 1;
-		data->length = priv->essid_size;
-		memcpy(extra, priv->essid, data->length);
-	} else {
-		/* the ANY ssid was specified */
-		if (priv->mac_state == MAC_CONNECTED && priv->curr_bss) {
-			/* report the SSID we have found */
-			data->flags = 1;
-			data->length = priv->curr_bss->ssid_len;
-			memcpy(extra, priv->curr_bss->ssid, data->length);
-		} else {
-			/* report ANY back */
-			data->flags = 0;
-			data->length = 0;
-		}
-	}
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCGIWESSID - %.*s", netdev->name,
-		 data->length, extra);
-
-	return 0;
-}
-
-static int at76_iw_handler_set_rate(struct net_device *netdev,
-				    struct iw_request_info *info,
-				    struct iw_param *bitrate, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int ret = -EIWCOMMIT;
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCSIWRATE - %d", netdev->name,
-		 bitrate->value);
-
-	switch (bitrate->value) {
-	case -1:
-		priv->txrate = TX_RATE_AUTO;
-		break;		/* auto rate */
-	case 1000000:
-		priv->txrate = TX_RATE_1MBIT;
-		break;
-	case 2000000:
-		priv->txrate = TX_RATE_2MBIT;
-		break;
-	case 5500000:
-		priv->txrate = TX_RATE_5_5MBIT;
-		break;
-	case 11000000:
-		priv->txrate = TX_RATE_11MBIT;
-		break;
-	default:
-		ret = -EINVAL;
-	}
-
-	return ret;
-}
-
-static int at76_iw_handler_get_rate(struct net_device *netdev,
-				    struct iw_request_info *info,
-				    struct iw_param *bitrate, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int ret = 0;
-
-	switch (priv->txrate) {
-		/* return max rate if RATE_AUTO */
-	case TX_RATE_AUTO:
-		bitrate->value = 11000000;
-		break;
-	case TX_RATE_1MBIT:
-		bitrate->value = 1000000;
-		break;
-	case TX_RATE_2MBIT:
-		bitrate->value = 2000000;
-		break;
-	case TX_RATE_5_5MBIT:
-		bitrate->value = 5500000;
-		break;
-	case TX_RATE_11MBIT:
-		bitrate->value = 11000000;
-		break;
-	default:
-		ret = -EINVAL;
-	}
-
-	bitrate->fixed = (priv->txrate != TX_RATE_AUTO);
-	bitrate->disabled = 0;
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCGIWRATE - %d", netdev->name,
-		 bitrate->value);
-
-	return ret;
-}
-
-static int at76_iw_handler_set_rts(struct net_device *netdev,
-				   struct iw_request_info *info,
-				   struct iw_param *rts, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int ret = -EIWCOMMIT;
-	int rthr = rts->value;
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCSIWRTS - value %d disabled %s",
-		 netdev->name, rts->value, (rts->disabled) ? "true" : "false");
-
-	if (rts->disabled)
-		rthr = MAX_RTS_THRESHOLD;
-
-	if ((rthr < 0) || (rthr > MAX_RTS_THRESHOLD))
-		ret = -EINVAL;
-	else
-		priv->rts_threshold = rthr;
-
-	return ret;
-}
-
-static int at76_iw_handler_get_rts(struct net_device *netdev,
-				   struct iw_request_info *info,
-				   struct iw_param *rts, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-
-	rts->value = priv->rts_threshold;
-	rts->disabled = (rts->value >= MAX_RTS_THRESHOLD);
-	rts->fixed = 1;
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCGIWRTS - value %d disabled %s",
-		 netdev->name, rts->value, (rts->disabled) ? "true" : "false");
-
-	return 0;
-}
-
-static int at76_iw_handler_set_frag(struct net_device *netdev,
-				    struct iw_request_info *info,
-				    struct iw_param *frag, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int ret = -EIWCOMMIT;
-	int fthr = frag->value;
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCSIWFRAG - value %d, disabled %s",
-		 netdev->name, frag->value,
-		 (frag->disabled) ? "true" : "false");
-
-	if (frag->disabled)
-		fthr = MAX_FRAG_THRESHOLD;
-
-	if ((fthr < MIN_FRAG_THRESHOLD) || (fthr > MAX_FRAG_THRESHOLD))
-		ret = -EINVAL;
-	else
-		priv->frag_threshold = fthr & ~0x1;	/* get an even value */
-
-	return ret;
-}
-
-static int at76_iw_handler_get_frag(struct net_device *netdev,
-				    struct iw_request_info *info,
-				    struct iw_param *frag, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-
-	frag->value = priv->frag_threshold;
-	frag->disabled = (frag->value >= MAX_FRAG_THRESHOLD);
-	frag->fixed = 1;
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCGIWFRAG - value %d, disabled %s",
-		 netdev->name, frag->value,
-		 (frag->disabled) ? "true" : "false");
-
-	return 0;
-}
-
-static int at76_iw_handler_get_txpow(struct net_device *netdev,
-				     struct iw_request_info *info,
-				     struct iw_param *power, char *extra)
-{
-	power->value = 15;
-	power->fixed = 1;	/* No power control */
-	power->disabled = 0;
-	power->flags = IW_TXPOW_DBM;
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCGIWTXPOW - txpow %d dBm", netdev->name,
-		 power->value);
-
-	return 0;
-}
-
-/* jal: short retry is handled by the firmware (at least 0.90.x),
-   while long retry is not (?) */
-static int at76_iw_handler_set_retry(struct net_device *netdev,
-				     struct iw_request_info *info,
-				     struct iw_param *retry, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int ret = -EIWCOMMIT;
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCSIWRETRY disabled %d flags 0x%x val %d",
-		 netdev->name, retry->disabled, retry->flags, retry->value);
-
-	if (!retry->disabled && (retry->flags & IW_RETRY_LIMIT)) {
-		if ((retry->flags & IW_RETRY_MIN) ||
-		    !(retry->flags & IW_RETRY_MAX))
-			priv->short_retry_limit = retry->value;
-		else
-			ret = -EINVAL;
-	} else
-		ret = -EINVAL;
-
-	return ret;
-}
-
-/* Adapted (ripped) from atmel.c */
-static int at76_iw_handler_get_retry(struct net_device *netdev,
-				     struct iw_request_info *info,
-				     struct iw_param *retry, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCGIWRETRY", netdev->name);
-
-	retry->disabled = 0;	/* Can't be disabled */
-	retry->flags = IW_RETRY_LIMIT;
-	retry->value = priv->short_retry_limit;
-
-	return 0;
-}
-
-static int at76_iw_handler_set_encode(struct net_device *netdev,
-				      struct iw_request_info *info,
-				      struct iw_point *encoding, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int index = (encoding->flags & IW_ENCODE_INDEX) - 1;
-	int len = encoding->length;
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCSIWENCODE - enc.flags %08x "
-		 "pointer %p len %d", netdev->name, encoding->flags,
-		 encoding->pointer, encoding->length);
-	at76_dbg(DBG_IOCTL,
-		 "%s: SIOCSIWENCODE - old wepstate: enabled %s key_id %d "
-		 "auth_mode %s", netdev->name,
-		 (priv->wep_enabled) ? "true" : "false", priv->wep_key_id,
-		 (priv->auth_mode ==
-		  WLAN_AUTH_SHARED_KEY) ? "restricted" : "open");
-
-	/* take the old default key if index is invalid */
-	if ((index < 0) || (index >= WEP_KEYS))
-		index = priv->wep_key_id;
-
-	if (len > 0) {
-		if (len > WEP_LARGE_KEY_LEN)
-			len = WEP_LARGE_KEY_LEN;
-
-		memset(priv->wep_keys[index], 0, WEP_KEY_LEN);
-		memcpy(priv->wep_keys[index], extra, len);
-		priv->wep_keys_len[index] = (len <= WEP_SMALL_KEY_LEN) ?
-		    WEP_SMALL_KEY_LEN : WEP_LARGE_KEY_LEN;
-		priv->wep_enabled = 1;
-	}
-
-	priv->wep_key_id = index;
-	priv->wep_enabled = ((encoding->flags & IW_ENCODE_DISABLED) == 0);
-
-	if (encoding->flags & IW_ENCODE_RESTRICTED)
-		priv->auth_mode = WLAN_AUTH_SHARED_KEY;
-	if (encoding->flags & IW_ENCODE_OPEN)
-		priv->auth_mode = WLAN_AUTH_OPEN;
-
-	at76_dbg(DBG_IOCTL,
-		 "%s: SIOCSIWENCODE - new wepstate: enabled %s key_id %d "
-		 "key_len %d auth_mode %s", netdev->name,
-		 (priv->wep_enabled) ? "true" : "false", priv->wep_key_id + 1,
-		 priv->wep_keys_len[priv->wep_key_id],
-		 (priv->auth_mode ==
-		  WLAN_AUTH_SHARED_KEY) ? "restricted" : "open");
-
-	return -EIWCOMMIT;
-}
-
-static int at76_iw_handler_get_encode(struct net_device *netdev,
-				      struct iw_request_info *info,
-				      struct iw_point *encoding, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int index = (encoding->flags & IW_ENCODE_INDEX) - 1;
-
-	if ((index < 0) || (index >= WEP_KEYS))
-		index = priv->wep_key_id;
-
-	encoding->flags =
-	    (priv->auth_mode == WLAN_AUTH_SHARED_KEY) ?
-	    IW_ENCODE_RESTRICTED : IW_ENCODE_OPEN;
-
-	if (!priv->wep_enabled)
-		encoding->flags |= IW_ENCODE_DISABLED;
-
-	if (encoding->pointer) {
-		encoding->length = priv->wep_keys_len[index];
-
-		memcpy(extra, priv->wep_keys[index], priv->wep_keys_len[index]);
-
-		encoding->flags |= (index + 1);
-	}
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCGIWENCODE - enc.flags %08x "
-		 "pointer %p len %d", netdev->name, encoding->flags,
-		 encoding->pointer, encoding->length);
-	at76_dbg(DBG_IOCTL,
-		 "%s: SIOCGIWENCODE - wepstate: enabled %s key_id %d "
-		 "key_len %d auth_mode %s", netdev->name,
-		 (priv->wep_enabled) ? "true" : "false", priv->wep_key_id + 1,
-		 priv->wep_keys_len[priv->wep_key_id],
-		 (priv->auth_mode ==
-		  WLAN_AUTH_SHARED_KEY) ? "restricted" : "open");
-
-	return 0;
-}
-
-static int at76_iw_handler_set_power(struct net_device *netdev,
-				     struct iw_request_info *info,
-				     struct iw_param *prq, char *extra)
-{
-	int err = -EIWCOMMIT;
-	struct at76_priv *priv = netdev_priv(netdev);
-
-	at76_dbg(DBG_IOCTL,
-		 "%s: SIOCSIWPOWER - disabled %s flags 0x%x value 0x%x",
-		 netdev->name, (prq->disabled) ? "true" : "false", prq->flags,
-		 prq->value);
-
-	if (prq->disabled)
-		priv->pm_mode = AT76_PM_OFF;
-	else {
-		switch (prq->flags & IW_POWER_MODE) {
-		case IW_POWER_ALL_R:
-		case IW_POWER_ON:
-			break;
-		default:
-			err = -EINVAL;
-			goto exit;
-		}
-		if (prq->flags & IW_POWER_PERIOD)
-			priv->pm_period = prq->value;
-
-		if (prq->flags & IW_POWER_TIMEOUT) {
-			err = -EINVAL;
-			goto exit;
-		}
-		priv->pm_mode = AT76_PM_ON;
-	}
-exit:
-	return err;
-}
-
-static int at76_iw_handler_get_power(struct net_device *netdev,
-				     struct iw_request_info *info,
-				     struct iw_param *power, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-
-	power->disabled = (priv->pm_mode == AT76_PM_OFF);
-	if (!power->disabled) {
-		power->flags = IW_POWER_PERIOD | IW_POWER_ALL_R;
-		power->value = priv->pm_period;
-	}
-
-	at76_dbg(DBG_IOCTL, "%s: SIOCGIWPOWER - %s flags 0x%x value 0x%x",
-		 netdev->name, power->disabled ? "disabled" : "enabled",
-		 power->flags, power->value);
-
-	return 0;
-}
-
-/*******************************************************************************
- * Private IOCTLS
- */
-static int at76_iw_set_short_preamble(struct net_device *netdev,
-				      struct iw_request_info *info, char *name,
-				      char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int val = *((int *)name);
-	int ret = -EIWCOMMIT;
-
-	at76_dbg(DBG_IOCTL, "%s: AT76_SET_SHORT_PREAMBLE, %d",
-		 netdev->name, val);
-
-	if (val < PREAMBLE_TYPE_LONG || val > PREAMBLE_TYPE_AUTO)
-		ret = -EINVAL;
-	else
-		priv->preamble_type = val;
-
-	return ret;
-}
-
-static int at76_iw_get_short_preamble(struct net_device *netdev,
-				      struct iw_request_info *info,
-				      union iwreq_data *wrqu, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-
-	snprintf(wrqu->name, sizeof(wrqu->name), "%s (%d)",
-		 preambles[priv->preamble_type], priv->preamble_type);
-	return 0;
-}
-
-static int at76_iw_set_debug(struct net_device *netdev,
-			     struct iw_request_info *info,
-			     struct iw_point *data, char *extra)
-{
-	char *ptr;
-	u32 val;
-
-	if (data->length > 0) {
-		val = simple_strtol(extra, &ptr, 0);
-
-		if (ptr == extra)
-			val = DBG_DEFAULTS;
-
-		at76_dbg(DBG_IOCTL, "%s: AT76_SET_DEBUG input %d: %s -> 0x%x",
-			 netdev->name, data->length, extra, val);
-	} else
-		val = DBG_DEFAULTS;
-
-	at76_dbg(DBG_IOCTL, "%s: AT76_SET_DEBUG, old 0x%x, new 0x%x",
-		 netdev->name, at76_debug, val);
-
-	/* jal: some more output to pin down lockups */
-	at76_dbg(DBG_IOCTL, "%s: netif running %d queue_stopped %d "
-		 "carrier_ok %d", netdev->name, netif_running(netdev),
-		 netif_queue_stopped(netdev), netif_carrier_ok(netdev));
-
-	at76_debug = val;
-
-	return 0;
-}
-
-static int at76_iw_get_debug(struct net_device *netdev,
-			     struct iw_request_info *info,
-			     union iwreq_data *wrqu, char *extra)
-{
-	snprintf(wrqu->name, sizeof(wrqu->name), "0x%08x", at76_debug);
-	return 0;
-}
-
-static int at76_iw_set_powersave_mode(struct net_device *netdev,
-				      struct iw_request_info *info, char *name,
-				      char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int val = *((int *)name);
-	int ret = -EIWCOMMIT;
-
-	at76_dbg(DBG_IOCTL, "%s: AT76_SET_POWERSAVE_MODE, %d (%s)",
-		 netdev->name, val,
-		 val == AT76_PM_OFF ? "active" : val == AT76_PM_ON ? "save" :
-		 val == AT76_PM_SMART ? "smart save" : "<invalid>");
-	if (val < AT76_PM_OFF || val > AT76_PM_SMART)
-		ret = -EINVAL;
-	else
-		priv->pm_mode = val;
-
-	return ret;
-}
-
-static int at76_iw_get_powersave_mode(struct net_device *netdev,
-				      struct iw_request_info *info,
-				      union iwreq_data *wrqu, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int *param = (int *)extra;
-
-	param[0] = priv->pm_mode;
-	return 0;
-}
-
-static int at76_iw_set_scan_times(struct net_device *netdev,
-				  struct iw_request_info *info, char *name,
-				  char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int mint = *((int *)name);
-	int maxt = *((int *)name + 1);
-	int ret = -EIWCOMMIT;
-
-	at76_dbg(DBG_IOCTL, "%s: AT76_SET_SCAN_TIMES - min %d max %d",
-		 netdev->name, mint, maxt);
-	if (mint <= 0 || maxt <= 0 || mint > maxt)
-		ret = -EINVAL;
-	else {
-		priv->scan_min_time = mint;
-		priv->scan_max_time = maxt;
-	}
-
-	return ret;
-}
-
-static int at76_iw_get_scan_times(struct net_device *netdev,
-				  struct iw_request_info *info,
-				  union iwreq_data *wrqu, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int *param = (int *)extra;
-
-	param[0] = priv->scan_min_time;
-	param[1] = priv->scan_max_time;
-	return 0;
-}
-
-static int at76_iw_set_scan_mode(struct net_device *netdev,
-				 struct iw_request_info *info, char *name,
-				 char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int val = *((int *)name);
-	int ret = -EIWCOMMIT;
-
-	at76_dbg(DBG_IOCTL, "%s: AT76_SET_SCAN_MODE - mode %s",
-		 netdev->name, (val = SCAN_TYPE_ACTIVE) ? "active" :
-		 (val = SCAN_TYPE_PASSIVE) ? "passive" : "<invalid>");
-
-	if (val != SCAN_TYPE_ACTIVE && val != SCAN_TYPE_PASSIVE)
-		ret = -EINVAL;
-	else
-		priv->scan_mode = val;
-
-	return ret;
-}
-
-static int at76_iw_get_scan_mode(struct net_device *netdev,
-				 struct iw_request_info *info,
-				 union iwreq_data *wrqu, char *extra)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int *param = (int *)extra;
-
-	param[0] = priv->scan_mode;
-	return 0;
-}
-
-#define AT76_SET_HANDLER(h, f) [h - SIOCIWFIRST] = (iw_handler) f
-
-/* Standard wireless handlers */
-static const iw_handler at76_handlers[] = {
-	AT76_SET_HANDLER(SIOCSIWCOMMIT, at76_iw_handler_commit),
-	AT76_SET_HANDLER(SIOCGIWNAME, at76_iw_handler_get_name),
-	AT76_SET_HANDLER(SIOCSIWFREQ, at76_iw_handler_set_freq),
-	AT76_SET_HANDLER(SIOCGIWFREQ, at76_iw_handler_get_freq),
-	AT76_SET_HANDLER(SIOCSIWMODE, at76_iw_handler_set_mode),
-	AT76_SET_HANDLER(SIOCGIWMODE, at76_iw_handler_get_mode),
-	AT76_SET_HANDLER(SIOCGIWRANGE, at76_iw_handler_get_range),
-	AT76_SET_HANDLER(SIOCSIWSPY, at76_iw_handler_set_spy),
-	AT76_SET_HANDLER(SIOCGIWSPY, at76_iw_handler_get_spy),
-	AT76_SET_HANDLER(SIOCSIWTHRSPY, at76_iw_handler_set_thrspy),
-	AT76_SET_HANDLER(SIOCGIWTHRSPY, at76_iw_handler_get_thrspy),
-	AT76_SET_HANDLER(SIOCSIWAP, at76_iw_handler_set_wap),
-	AT76_SET_HANDLER(SIOCGIWAP, at76_iw_handler_get_wap),
-	AT76_SET_HANDLER(SIOCSIWSCAN, at76_iw_handler_set_scan),
-	AT76_SET_HANDLER(SIOCGIWSCAN, at76_iw_handler_get_scan),
-	AT76_SET_HANDLER(SIOCSIWESSID, at76_iw_handler_set_essid),
-	AT76_SET_HANDLER(SIOCGIWESSID, at76_iw_handler_get_essid),
-	AT76_SET_HANDLER(SIOCSIWRATE, at76_iw_handler_set_rate),
-	AT76_SET_HANDLER(SIOCGIWRATE, at76_iw_handler_get_rate),
-	AT76_SET_HANDLER(SIOCSIWRTS, at76_iw_handler_set_rts),
-	AT76_SET_HANDLER(SIOCGIWRTS, at76_iw_handler_get_rts),
-	AT76_SET_HANDLER(SIOCSIWFRAG, at76_iw_handler_set_frag),
-	AT76_SET_HANDLER(SIOCGIWFRAG, at76_iw_handler_get_frag),
-	AT76_SET_HANDLER(SIOCGIWTXPOW, at76_iw_handler_get_txpow),
-	AT76_SET_HANDLER(SIOCSIWRETRY, at76_iw_handler_set_retry),
-	AT76_SET_HANDLER(SIOCGIWRETRY, at76_iw_handler_get_retry),
-	AT76_SET_HANDLER(SIOCSIWENCODE, at76_iw_handler_set_encode),
-	AT76_SET_HANDLER(SIOCGIWENCODE, at76_iw_handler_get_encode),
-	AT76_SET_HANDLER(SIOCSIWPOWER, at76_iw_handler_set_power),
-	AT76_SET_HANDLER(SIOCGIWPOWER, at76_iw_handler_get_power)
-};
-
-#define AT76_SET_PRIV(h, f) [h - SIOCIWFIRSTPRIV] = (iw_handler) f
-
-/* Private wireless handlers */
-static const iw_handler at76_priv_handlers[] = {
-	AT76_SET_PRIV(AT76_SET_SHORT_PREAMBLE, at76_iw_set_short_preamble),
-	AT76_SET_PRIV(AT76_GET_SHORT_PREAMBLE, at76_iw_get_short_preamble),
-	AT76_SET_PRIV(AT76_SET_DEBUG, at76_iw_set_debug),
-	AT76_SET_PRIV(AT76_GET_DEBUG, at76_iw_get_debug),
-	AT76_SET_PRIV(AT76_SET_POWERSAVE_MODE, at76_iw_set_powersave_mode),
-	AT76_SET_PRIV(AT76_GET_POWERSAVE_MODE, at76_iw_get_powersave_mode),
-	AT76_SET_PRIV(AT76_SET_SCAN_TIMES, at76_iw_set_scan_times),
-	AT76_SET_PRIV(AT76_GET_SCAN_TIMES, at76_iw_get_scan_times),
-	AT76_SET_PRIV(AT76_SET_SCAN_MODE, at76_iw_set_scan_mode),
-	AT76_SET_PRIV(AT76_GET_SCAN_MODE, at76_iw_get_scan_mode),
-};
-
-/* Names and arguments of private wireless handlers */
-static const struct iw_priv_args at76_priv_args[] = {
-	/* 0 - long, 1 - short */
-	{AT76_SET_SHORT_PREAMBLE,
-	 IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "set_preamble"},
-
-	{AT76_GET_SHORT_PREAMBLE,
-	 0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | 10, "get_preamble"},
-
-	/* we must pass the new debug mask as a string, because iwpriv cannot
-	 * parse hex numbers starting with 0x :-(  */
-	{AT76_SET_DEBUG,
-	 IW_PRIV_TYPE_CHAR | 10, 0, "set_debug"},
-
-	{AT76_GET_DEBUG,
-	 0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | 10, "get_debug"},
-
-	/* 1 - active, 2 - power save, 3 - smart power save */
-	{AT76_SET_POWERSAVE_MODE,
-	 IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "set_powersave"},
-
-	{AT76_GET_POWERSAVE_MODE,
-	 0, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, "get_powersave"},
-
-	/* min_channel_time, max_channel_time */
-	{AT76_SET_SCAN_TIMES,
-	 IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 2, 0, "set_scan_times"},
-
-	{AT76_GET_SCAN_TIMES,
-	 0, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 2, "get_scan_times"},
-
-	/* 0 - active, 1 - passive scan */
-	{AT76_SET_SCAN_MODE,
-	 IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "set_scan_mode"},
-
-	{AT76_GET_SCAN_MODE,
-	 0, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, "get_scan_mode"},
-};
-
-static const struct iw_handler_def at76_handler_def = {
-	.num_standard = ARRAY_SIZE(at76_handlers),
-	.num_private = ARRAY_SIZE(at76_priv_handlers),
-	.num_private_args = ARRAY_SIZE(at76_priv_args),
-	.standard = at76_handlers,
-	.private = at76_priv_handlers,
-	.private_args = at76_priv_args,
-	.get_wireless_stats = at76_get_wireless_stats,
-};
-
-static const u8 snapsig[] = { 0xaa, 0xaa, 0x03 };
-
-/* RFC 1042 encapsulates Ethernet frames in 802.2 SNAP (0xaa, 0xaa, 0x03) with
- * a SNAP OID of 0 (0x00, 0x00, 0x00) */
-static const u8 rfc1042sig[] = { 0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00 };
-
-static int at76_tx(struct sk_buff *skb, struct net_device *netdev)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	struct net_device_stats *stats = &priv->stats;
-	int ret = 0;
-	int wlen;
-	int submit_len;
-	struct at76_tx_buffer *tx_buffer = priv->bulk_out_buffer;
-	struct ieee80211_hdr_3addr *i802_11_hdr =
-	    (struct ieee80211_hdr_3addr *)tx_buffer->packet;
-	u8 *payload = i802_11_hdr->payload;
-	struct ethhdr *eh = (struct ethhdr *)skb->data;
-
-	if (netif_queue_stopped(netdev)) {
-		printk(KERN_ERR "%s: %s called while netdev is stopped\n",
-		       netdev->name, __func__);
-		/* skip this packet */
-		dev_kfree_skb(skb);
-		return 0;
-	}
-
-	if (priv->tx_urb->status == -EINPROGRESS) {
-		printk(KERN_ERR "%s: %s called while tx urb is pending\n",
-		       netdev->name, __func__);
-		/* skip this packet */
-		dev_kfree_skb(skb);
-		return 0;
-	}
-
-	if (skb->len < ETH_HLEN) {
-		printk(KERN_ERR "%s: %s: skb too short (%d)\n",
-		       netdev->name, __func__, skb->len);
-		dev_kfree_skb(skb);
-		return 0;
-	}
-
-	at76_ledtrig_tx_activity();	/* tell ledtrigger we send a packet */
-
-	/* we can get rid of memcpy if we set netdev->hard_header_len to
-	   reserve enough space, but we would need to keep the skb around */
-
-	if (ntohs(eh->h_proto) <= ETH_DATA_LEN) {
-		/* this is a 802.3 packet */
-		if (skb->len >= ETH_HLEN + sizeof(rfc1042sig)
-		    && skb->data[ETH_HLEN] == rfc1042sig[0]
-		    && skb->data[ETH_HLEN + 1] == rfc1042sig[1]) {
-			/* higher layer delivered SNAP header - keep it */
-			memcpy(payload, skb->data + ETH_HLEN,
-			       skb->len - ETH_HLEN);
-			wlen = IEEE80211_3ADDR_LEN + skb->len - ETH_HLEN;
-		} else {
-			printk(KERN_ERR "%s: dropping non-SNAP 802.2 packet "
-			       "(DSAP 0x%02x SSAP 0x%02x cntrl 0x%02x)\n",
-			       priv->netdev->name, skb->data[ETH_HLEN],
-			       skb->data[ETH_HLEN + 1],
-			       skb->data[ETH_HLEN + 2]);
-			dev_kfree_skb(skb);
-			return 0;
-		}
-	} else {
-		/* add RFC 1042 header in front */
-		memcpy(payload, rfc1042sig, sizeof(rfc1042sig));
-		memcpy(payload + sizeof(rfc1042sig), &eh->h_proto,
-		       skb->len - offsetof(struct ethhdr, h_proto));
-		wlen = IEEE80211_3ADDR_LEN + sizeof(rfc1042sig) + skb->len -
-		    offsetof(struct ethhdr, h_proto);
-	}
-
-	/* make wireless header */
-	i802_11_hdr->frame_ctl =
-	    cpu_to_le16(IEEE80211_FTYPE_DATA |
-			(priv->wep_enabled ? IEEE80211_FCTL_PROTECTED : 0) |
-			(priv->iw_mode ==
-			 IW_MODE_INFRA ? IEEE80211_FCTL_TODS : 0));
-
-	if (priv->iw_mode == IW_MODE_ADHOC) {
-		memcpy(i802_11_hdr->addr1, eh->h_dest, ETH_ALEN);
-		memcpy(i802_11_hdr->addr2, eh->h_source, ETH_ALEN);
-		memcpy(i802_11_hdr->addr3, priv->bssid, ETH_ALEN);
-	} else if (priv->iw_mode == IW_MODE_INFRA) {
-		memcpy(i802_11_hdr->addr1, priv->bssid, ETH_ALEN);
-		memcpy(i802_11_hdr->addr2, eh->h_source, ETH_ALEN);
-		memcpy(i802_11_hdr->addr3, eh->h_dest, ETH_ALEN);
-	}
-
-	i802_11_hdr->duration_id = cpu_to_le16(0);
-	i802_11_hdr->seq_ctl = cpu_to_le16(0);
-
-	/* setup 'Atmel' header */
-	tx_buffer->wlength = cpu_to_le16(wlen);
-	tx_buffer->tx_rate = priv->txrate;
-	/* for broadcast destination addresses, the firmware 0.100.x
-	   seems to choose the highest rate set with CMD_STARTUP in
-	   basic_rate_set replacing this value */
-
-	memset(tx_buffer->reserved, 0, sizeof(tx_buffer->reserved));
-
-	tx_buffer->padding = at76_calc_padding(wlen);
-	submit_len = wlen + AT76_TX_HDRLEN + tx_buffer->padding;
-
-	at76_dbg(DBG_TX_DATA_CONTENT, "%s skb->data %s", priv->netdev->name,
-		 hex2str(skb->data, 32));
-	at76_dbg(DBG_TX_DATA, "%s tx: wlen 0x%x pad 0x%x rate %d hdr %s",
-		 priv->netdev->name,
-		 le16_to_cpu(tx_buffer->wlength),
-		 tx_buffer->padding, tx_buffer->tx_rate,
-		 hex2str(i802_11_hdr, sizeof(*i802_11_hdr)));
-	at76_dbg(DBG_TX_DATA_CONTENT, "%s payload %s", priv->netdev->name,
-		 hex2str(payload, 48));
-
-	/* send stuff */
-	netif_stop_queue(netdev);
-	netdev->trans_start = jiffies;
-
-	usb_fill_bulk_urb(priv->tx_urb, priv->udev, priv->tx_pipe, tx_buffer,
-			  submit_len, at76_tx_callback, priv);
-	ret = usb_submit_urb(priv->tx_urb, GFP_ATOMIC);
-	if (ret) {
-		stats->tx_errors++;
-		printk(KERN_ERR "%s: error in tx submit urb: %d\n",
-		       netdev->name, ret);
-		if (ret == -EINVAL)
-			printk(KERN_ERR
-			       "%s: -EINVAL: tx urb %p hcpriv %p complete %p\n",
-			       priv->netdev->name, priv->tx_urb,
-			       priv->tx_urb->hcpriv, priv->tx_urb->complete);
-	} else {
-		stats->tx_bytes += skb->len;
-		dev_kfree_skb(skb);
-	}
-
-	return ret;
-}
-
-static void at76_tx_timeout(struct net_device *netdev)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-
-	if (!priv)
-		return;
-	dev_warn(&netdev->dev, "tx timeout.");
-
-	usb_unlink_urb(priv->tx_urb);
-	priv->stats.tx_errors++;
-}
-
 static int at76_submit_rx_urb(struct at76_priv *priv)
 {
 	int ret;
@@ -3256,7 +1327,7 @@ static int at76_submit_rx_urb(struct at76_priv *priv)
 
 	if (!priv->rx_urb) {
 		printk(KERN_ERR "%s: %s: priv->rx_urb is NULL\n",
-		       priv->netdev->name, __func__);
+		       wiphy_name(priv->hw->wiphy), __func__);
 		return -EFAULT;
 	}
 
@@ -3264,7 +1335,7 @@ static int at76_submit_rx_urb(struct at76_priv *priv)
 		skb = dev_alloc_skb(sizeof(struct at76_rx_buffer));
 		if (!skb) {
 			printk(KERN_ERR "%s: cannot allocate rx skbuff\n",
-			       priv->netdev->name);
+			       wiphy_name(priv->hw->wiphy));
 			ret = -ENOMEM;
 			goto exit;
 		}
@@ -3284,110 +1355,18 @@ static int at76_submit_rx_urb(struct at76_priv *priv)
 				 "usb_submit_urb returned -ENODEV");
 		else
 			printk(KERN_ERR "%s: rx, usb_submit_urb failed: %d\n",
-			       priv->netdev->name, ret);
+			       wiphy_name(priv->hw->wiphy), ret);
 	}
 
 exit:
 	if (ret < 0 && ret != -ENODEV)
 		printk(KERN_ERR "%s: cannot submit rx urb - please unload the "
 		       "driver and/or power cycle the device\n",
-		       priv->netdev->name);
+		       wiphy_name(priv->hw->wiphy));
 
 	return ret;
 }
 
-static int at76_open(struct net_device *netdev)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	int ret = 0;
-
-	at76_dbg(DBG_PROC_ENTRY, "%s(): entry", __func__);
-
-	if (mutex_lock_interruptible(&priv->mtx))
-		return -EINTR;
-
-	/* if netdev->dev_addr != priv->mac_addr we must
-	   set the mac address in the device ! */
-	if (compare_ether_addr(netdev->dev_addr, priv->mac_addr)) {
-		if (at76_add_mac_address(priv, netdev->dev_addr) >= 0)
-			at76_dbg(DBG_PROGRESS, "%s: set new MAC addr %s",
-				 netdev->name, mac2str(netdev->dev_addr));
-	}
-
-	priv->scan_state = SCAN_IDLE;
-	priv->last_scan = jiffies;
-
-	ret = at76_submit_rx_urb(priv);
-	if (ret < 0) {
-		printk(KERN_ERR "%s: open: submit_rx_urb failed: %d\n",
-		       netdev->name, ret);
-		goto error;
-	}
-
-	schedule_delayed_work(&priv->dwork_restart, 0);
-
-	at76_dbg(DBG_PROC_ENTRY, "%s(): end", __func__);
-error:
-	mutex_unlock(&priv->mtx);
-	return ret < 0 ? ret : 0;
-}
-
-static int at76_stop(struct net_device *netdev)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-
-	at76_dbg(DBG_DEVSTART, "%s: ENTER", __func__);
-
-	if (mutex_lock_interruptible(&priv->mtx))
-		return -EINTR;
-
-	at76_quiesce(priv);
-
-	if (!priv->device_unplugged) {
-		/* We are called by "ifconfig ethX down", not because the
-		 * device is not available anymore. */
-		at76_set_radio(priv, 0);
-
-		/* We unlink rx_urb because at76_open() re-submits it.
-		 * If unplugged, at76_delete_device() takes care of it. */
-		usb_kill_urb(priv->rx_urb);
-	}
-
-	/* free the bss_list */
-	at76_free_bss_list(priv);
-
-	mutex_unlock(&priv->mtx);
-	at76_dbg(DBG_DEVSTART, "%s: EXIT", __func__);
-
-	return 0;
-}
-
-static void at76_ethtool_get_drvinfo(struct net_device *netdev,
-				     struct ethtool_drvinfo *info)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-
-	strncpy(info->driver, DRIVER_NAME, sizeof(info->driver));
-	strncpy(info->version, DRIVER_VERSION, sizeof(info->version));
-
-	usb_make_path(priv->udev, info->bus_info, sizeof(info->bus_info));
-
-	snprintf(info->fw_version, sizeof(info->fw_version), "%d.%d.%d-%d",
-		 priv->fw_version.major, priv->fw_version.minor,
-		 priv->fw_version.patch, priv->fw_version.build);
-}
-
-static u32 at76_ethtool_get_link(struct net_device *netdev)
-{
-	struct at76_priv *priv = netdev_priv(netdev);
-	return priv->mac_state == MAC_CONNECTED;
-}
-
-static struct ethtool_ops at76_ethtool_ops = {
-	.get_drvinfo = at76_ethtool_get_drvinfo,
-	.get_link = at76_ethtool_get_link,
-};
-
 /* Download external firmware */
 static int at76_load_external_fw(struct usb_device *udev, struct fwentry *fwe)
 {
@@ -3484,406 +1463,6 @@ exit:
 	return ret;
 }
 
-static int at76_match_essid(struct at76_priv *priv, struct bss_info *ptr)
-{
-	/* common criteria for both modi */
-
-	int ret = (priv->essid_size == 0 /* ANY ssid */  ||
-		   (priv->essid_size == ptr->ssid_len &&
-		    !memcmp(priv->essid, ptr->ssid, ptr->ssid_len)));
-	if (!ret)
-		at76_dbg(DBG_BSS_MATCH,
-			 "%s bss table entry %p: essid didn't match",
-			 priv->netdev->name, ptr);
-	return ret;
-}
-
-static inline int at76_match_mode(struct at76_priv *priv, struct bss_info *ptr)
-{
-	int ret;
-
-	if (priv->iw_mode == IW_MODE_ADHOC)
-		ret = ptr->capa & WLAN_CAPABILITY_IBSS;
-	else
-		ret = ptr->capa & WLAN_CAPABILITY_ESS;
-	if (!ret)
-		at76_dbg(DBG_BSS_MATCH,
-			 "%s bss table entry %p: mode didn't match",
-			 priv->netdev->name, ptr);
-	return ret;
-}
-
-static int at76_match_rates(struct at76_priv *priv, struct bss_info *ptr)
-{
-	int i;
-
-	for (i = 0; i < ptr->rates_len; i++) {
-		u8 rate = ptr->rates[i];
-
-		if (!(rate & 0x80))
-			continue;
-
-		/* this is a basic rate we have to support
-		   (see IEEE802.11, ch. 7.3.2.2) */
-		if (rate != (0x80 | hw_rates[0])
-		    && rate != (0x80 | hw_rates[1])
-		    && rate != (0x80 | hw_rates[2])
-		    && rate != (0x80 | hw_rates[3])) {
-			at76_dbg(DBG_BSS_MATCH,
-				 "%s: bss table entry %p: basic rate %02x not "
-				 "supported", priv->netdev->name, ptr, rate);
-			return 0;
-		}
-	}
-
-	/* if we use short preamble, the bss must support it */
-	if (priv->preamble_type == PREAMBLE_TYPE_SHORT &&
-	    !(ptr->capa & WLAN_CAPABILITY_SHORT_PREAMBLE)) {
-		at76_dbg(DBG_BSS_MATCH,
-			 "%s: %p does not support short preamble",
-			 priv->netdev->name, ptr);
-		return 0;
-	} else
-		return 1;
-}
-
-static inline int at76_match_wep(struct at76_priv *priv, struct bss_info *ptr)
-{
-	if (!priv->wep_enabled && ptr->capa & WLAN_CAPABILITY_PRIVACY) {
-		/* we have disabled WEP, but the BSS signals privacy */
-		at76_dbg(DBG_BSS_MATCH,
-			 "%s: bss table entry %p: requires encryption",
-			 priv->netdev->name, ptr);
-		return 0;
-	}
-	/* otherwise if the BSS does not signal privacy it may well
-	   accept encrypted packets from us ... */
-	return 1;
-}
-
-static inline int at76_match_bssid(struct at76_priv *priv, struct bss_info *ptr)
-{
-	if (!priv->wanted_bssid_valid ||
-	    !compare_ether_addr(ptr->bssid, priv->wanted_bssid))
-		return 1;
-
-	at76_dbg(DBG_BSS_MATCH,
-		 "%s: requested bssid - %s does not match",
-		 priv->netdev->name, mac2str(priv->wanted_bssid));
-	at76_dbg(DBG_BSS_MATCH,
-		 "      AP bssid - %s of bss table entry %p",
-		 mac2str(ptr->bssid), ptr);
-	return 0;
-}
-
-/**
- * at76_match_bss - try to find a matching bss in priv->bss
- *
- * last - last bss tried
- *
- * last == NULL signals a new round starting with priv->bss_list.next
- * this function must be called inside an acquired priv->bss_list_spinlock
- * otherwise the timeout on bss may remove the newly chosen entry
- */
-static struct bss_info *at76_match_bss(struct at76_priv *priv,
-				       struct bss_info *last)
-{
-	struct bss_info *ptr = NULL;
-	struct list_head *curr;
-
-	curr = last ? last->list.next : priv->bss_list.next;
-	while (curr != &priv->bss_list) {
-		ptr = list_entry(curr, struct bss_info, list);
-		if (at76_match_essid(priv, ptr) && at76_match_mode(priv, ptr)
-		    && at76_match_wep(priv, ptr) && at76_match_rates(priv, ptr)
-		    && at76_match_bssid(priv, ptr))
-			break;
-		curr = curr->next;
-	}
-
-	if (curr == &priv->bss_list)
-		ptr = NULL;
-	/* otherwise ptr points to the struct bss_info we have chosen */
-
-	at76_dbg(DBG_BSS_TABLE, "%s %s: returned %p", priv->netdev->name,
-		 __func__, ptr);
-	return ptr;
-}
-
-/* Start joining a matching BSS, or create own IBSS */
-static void at76_work_join(struct work_struct *work)
-{
-	struct at76_priv *priv = container_of(work, struct at76_priv,
-					      work_join);
-	int ret;
-	unsigned long flags;
-
-	mutex_lock(&priv->mtx);
-
-	WARN_ON(priv->mac_state != MAC_JOINING);
-	if (priv->mac_state != MAC_JOINING)
-		goto exit;
-
-	/* secure the access to priv->curr_bss ! */
-	spin_lock_irqsave(&priv->bss_list_spinlock, flags);
-	priv->curr_bss = at76_match_bss(priv, priv->curr_bss);
-	spin_unlock_irqrestore(&priv->bss_list_spinlock, flags);
-
-	if (!priv->curr_bss) {
-		/* here we haven't found a matching (i)bss ... */
-		if (priv->iw_mode == IW_MODE_ADHOC) {
-			at76_set_mac_state(priv, MAC_OWN_IBSS);
-			at76_start_ibss(priv);
-			goto exit;
-		}
-		/* haven't found a matching BSS in infra mode - try again */
-		at76_set_mac_state(priv, MAC_SCANNING);
-		schedule_work(&priv->work_start_scan);
-		goto exit;
-	}
-
-	ret = at76_join_bss(priv, priv->curr_bss);
-	if (ret < 0) {
-		printk(KERN_ERR "%s: join_bss failed with %d\n",
-		       priv->netdev->name, ret);
-		goto exit;
-	}
-
-	ret = at76_wait_completion(priv, CMD_JOIN);
-	if (ret != CMD_STATUS_COMPLETE) {
-		if (ret != CMD_STATUS_TIME_OUT)
-			printk(KERN_ERR "%s: join_bss completed with %d\n",
-			       priv->netdev->name, ret);
-		else
-			printk(KERN_INFO "%s: join_bss ssid %s timed out\n",
-			       priv->netdev->name,
-			       mac2str(priv->curr_bss->bssid));
-
-		/* retry next BSS immediately */
-		schedule_work(&priv->work_join);
-		goto exit;
-	}
-
-	/* here we have joined the (I)BSS */
-	if (priv->iw_mode == IW_MODE_ADHOC) {
-		struct bss_info *bptr = priv->curr_bss;
-		at76_set_mac_state(priv, MAC_CONNECTED);
-		/* get ESSID, BSSID and channel for priv->curr_bss */
-		priv->essid_size = bptr->ssid_len;
-		memcpy(priv->essid, bptr->ssid, bptr->ssid_len);
-		memcpy(priv->bssid, bptr->bssid, ETH_ALEN);
-		priv->channel = bptr->channel;
-		at76_iwevent_bss_connect(priv->netdev, bptr->bssid);
-		netif_carrier_on(priv->netdev);
-		netif_start_queue(priv->netdev);
-		/* just to be sure */
-		cancel_delayed_work(&priv->dwork_get_scan);
-		cancel_delayed_work(&priv->dwork_auth);
-		cancel_delayed_work(&priv->dwork_assoc);
-	} else {
-		/* send auth req */
-		priv->retries = AUTH_RETRIES;
-		at76_set_mac_state(priv, MAC_AUTH);
-		at76_auth_req(priv, priv->curr_bss, 1, NULL);
-		at76_dbg(DBG_MGMT_TIMER,
-			 "%s:%d: starting mgmt_timer + HZ", __func__, __LINE__);
-		schedule_delayed_work(&priv->dwork_auth, AUTH_TIMEOUT);
-	}
-
-exit:
-	mutex_unlock(&priv->mtx);
-}
-
-/* Reap scan results */
-static void at76_dwork_get_scan(struct work_struct *work)
-{
-	int status;
-	int ret;
-	struct at76_priv *priv = container_of(work, struct at76_priv,
-					      dwork_get_scan.work);
-
-	mutex_lock(&priv->mtx);
-	WARN_ON(priv->mac_state != MAC_SCANNING);
-	if (priv->mac_state != MAC_SCANNING)
-		goto exit;
-
-	status = at76_get_cmd_status(priv->udev, CMD_SCAN);
-	if (status < 0) {
-		printk(KERN_ERR "%s: %s: at76_get_cmd_status failed with %d\n",
-		       priv->netdev->name, __func__, status);
-		status = CMD_STATUS_IN_PROGRESS;
-		/* INFO: Hope it was a one off error - if not, scanning
-		   further down the line and stop this cycle */
-	}
-	at76_dbg(DBG_PROGRESS,
-		 "%s %s: got cmd_status %d (state %s, need_any %d)",
-		 priv->netdev->name, __func__, status,
-		 mac_states[priv->mac_state], priv->scan_need_any);
-
-	if (status != CMD_STATUS_COMPLETE) {
-		if ((status != CMD_STATUS_IN_PROGRESS) &&
-		    (status != CMD_STATUS_IDLE))
-			printk(KERN_ERR "%s: %s: Bad scan status: %s\n",
-			       priv->netdev->name, __func__,
-			       at76_get_cmd_status_string(status));
-
-		/* the first cmd status after scan start is always a IDLE ->
-		   start the timer to poll again until COMPLETED */
-		at76_dbg(DBG_MGMT_TIMER,
-			 "%s:%d: starting mgmt_timer for %d ticks",
-			 __func__, __LINE__, SCAN_POLL_INTERVAL);
-		schedule_delayed_work(&priv->dwork_get_scan,
-				      SCAN_POLL_INTERVAL);
-		goto exit;
-	}
-
-	if (at76_debug & DBG_BSS_TABLE)
-		at76_dump_bss_table(priv);
-
-	if (priv->scan_need_any) {
-		ret = at76_start_scan(priv, 0);
-		if (ret < 0)
-			printk(KERN_ERR
-			       "%s: %s: start_scan (ANY) failed with %d\n",
-			       priv->netdev->name, __func__, ret);
-		at76_dbg(DBG_MGMT_TIMER,
-			 "%s:%d: starting mgmt_timer for %d ticks", __func__,
-			 __LINE__, SCAN_POLL_INTERVAL);
-		schedule_delayed_work(&priv->dwork_get_scan,
-				      SCAN_POLL_INTERVAL);
-		priv->scan_need_any = 0;
-	} else {
-		priv->scan_state = SCAN_COMPLETED;
-		/* report the end of scan to user space */
-		at76_iwevent_scan_complete(priv->netdev);
-		at76_set_mac_state(priv, MAC_JOINING);
-		schedule_work(&priv->work_join);
-	}
-
-exit:
-	mutex_unlock(&priv->mtx);
-}
-
-/* Handle loss of beacons from the AP */
-static void at76_dwork_beacon(struct work_struct *work)
-{
-	struct at76_priv *priv = container_of(work, struct at76_priv,
-					      dwork_beacon.work);
-
-	mutex_lock(&priv->mtx);
-	if (priv->mac_state != MAC_CONNECTED || priv->iw_mode != IW_MODE_INFRA)
-		goto exit;
-
-	/* We haven't received any beacons from out AP for BEACON_TIMEOUT */
-	printk(KERN_INFO "%s: lost beacon bssid %s\n",
-	       priv->netdev->name, mac2str(priv->curr_bss->bssid));
-
-	netif_carrier_off(priv->netdev);
-	netif_stop_queue(priv->netdev);
-	at76_iwevent_bss_disconnect(priv->netdev);
-	at76_set_mac_state(priv, MAC_SCANNING);
-	schedule_work(&priv->work_start_scan);
-
-exit:
-	mutex_unlock(&priv->mtx);
-}
-
-/* Handle authentication response timeout */
-static void at76_dwork_auth(struct work_struct *work)
-{
-	struct at76_priv *priv = container_of(work, struct at76_priv,
-					      dwork_auth.work);
-
-	mutex_lock(&priv->mtx);
-	WARN_ON(priv->mac_state != MAC_AUTH);
-	if (priv->mac_state != MAC_AUTH)
-		goto exit;
-
-	at76_dbg(DBG_PROGRESS, "%s: authentication response timeout",
-		 priv->netdev->name);
-
-	if (priv->retries-- >= 0) {
-		at76_auth_req(priv, priv->curr_bss, 1, NULL);
-		at76_dbg(DBG_MGMT_TIMER, "%s:%d: starting mgmt_timer + HZ",
-			 __func__, __LINE__);
-		schedule_delayed_work(&priv->dwork_auth, AUTH_TIMEOUT);
-	} else {
-		/* try to get next matching BSS */
-		at76_set_mac_state(priv, MAC_JOINING);
-		schedule_work(&priv->work_join);
-	}
-
-exit:
-	mutex_unlock(&priv->mtx);
-}
-
-/* Handle association response timeout */
-static void at76_dwork_assoc(struct work_struct *work)
-{
-	struct at76_priv *priv = container_of(work, struct at76_priv,
-					      dwork_assoc.work);
-
-	mutex_lock(&priv->mtx);
-	WARN_ON(priv->mac_state != MAC_ASSOC);
-	if (priv->mac_state != MAC_ASSOC)
-		goto exit;
-
-	at76_dbg(DBG_PROGRESS, "%s: association response timeout",
-		 priv->netdev->name);
-
-	if (priv->retries-- >= 0) {
-		at76_assoc_req(priv, priv->curr_bss);
-		at76_dbg(DBG_MGMT_TIMER, "%s:%d: starting mgmt_timer + HZ",
-			 __func__, __LINE__);
-		schedule_delayed_work(&priv->dwork_assoc, ASSOC_TIMEOUT);
-	} else {
-		/* try to get next matching BSS */
-		at76_set_mac_state(priv, MAC_JOINING);
-		schedule_work(&priv->work_join);
-	}
-
-exit:
-	mutex_unlock(&priv->mtx);
-}
-
-/* Read new bssid in ad-hoc mode */
-static void at76_work_new_bss(struct work_struct *work)
-{
-	struct at76_priv *priv = container_of(work, struct at76_priv,
-					      work_new_bss);
-	int ret;
-	struct mib_mac_mgmt mac_mgmt;
-
-	mutex_lock(&priv->mtx);
-
-	ret = at76_get_mib(priv->udev, MIB_MAC_MGMT, &mac_mgmt,
-			   sizeof(struct mib_mac_mgmt));
-	if (ret < 0) {
-		printk(KERN_ERR "%s: at76_get_mib failed: %d\n",
-		       priv->netdev->name, ret);
-		goto exit;
-	}
-
-	at76_dbg(DBG_PROGRESS, "ibss_change = 0x%2x", mac_mgmt.ibss_change);
-	memcpy(priv->bssid, mac_mgmt.current_bssid, ETH_ALEN);
-	at76_dbg(DBG_PROGRESS, "using BSSID %s", mac2str(priv->bssid));
-
-	at76_iwevent_bss_connect(priv->netdev, priv->bssid);
-
-	priv->mib_buf.type = MIB_MAC_MGMT;
-	priv->mib_buf.size = 1;
-	priv->mib_buf.index = offsetof(struct mib_mac_mgmt, ibss_change);
-	priv->mib_buf.data.byte = 0;
-
-	ret = at76_set_mib(priv, &priv->mib_buf);
-	if (ret < 0)
-		printk(KERN_ERR "%s: set_mib (ibss change ok) failed: %d\n",
-		       priv->netdev->name, ret);
-
-exit:
-	mutex_unlock(&priv->mtx);
-}
-
 static int at76_startup_device(struct at76_priv *priv)
 {
 	struct at76_card_config *ccfg = &priv->card_config;
@@ -3891,14 +1470,14 @@ static int at76_startup_device(struct at76_priv *priv)
 
 	at76_dbg(DBG_PARAMS,
 		 "%s param: ssid %.*s (%s) mode %s ch %d wep %s key %d "
-		 "keylen %d", priv->netdev->name, priv->essid_size, priv->essid,
-		 hex2str(priv->essid, IW_ESSID_MAX_SIZE),
+		 "keylen %d", wiphy_name(priv->hw->wiphy), priv->essid_size,
+		 priv->essid, hex2str(priv->essid, IW_ESSID_MAX_SIZE),
 		 priv->iw_mode == IW_MODE_ADHOC ? "adhoc" : "infra",
 		 priv->channel, priv->wep_enabled ? "enabled" : "disabled",
 		 priv->wep_key_id, priv->wep_keys_len[priv->wep_key_id]);
 	at76_dbg(DBG_PARAMS,
 		 "%s param: preamble %s rts %d retry %d frag %d "
-		 "txrate %s auth_mode %d", priv->netdev->name,
+		 "txrate %s auth_mode %d", wiphy_name(priv->hw->wiphy),
 		 preambles[priv->preamble_type], priv->rts_threshold,
 		 priv->short_retry_limit, priv->frag_threshold,
 		 priv->txrate == TX_RATE_1MBIT ? "1MBit" : priv->txrate ==
@@ -3909,7 +1488,7 @@ static int at76_startup_device(struct at76_priv *priv)
 	at76_dbg(DBG_PARAMS,
 		 "%s param: pm_mode %d pm_period %d auth_mode %s "
 		 "scan_times %d %d scan_mode %s",
-		 priv->netdev->name, priv->pm_mode, priv->pm_period,
+		 wiphy_name(priv->hw->wiphy), priv->pm_mode, priv->pm_period,
 		 priv->auth_mode == WLAN_AUTH_OPEN ? "open" : "shared_secret",
 		 priv->scan_min_time, priv->scan_max_time,
 		 priv->scan_mode == SCAN_TYPE_ACTIVE ? "active" : "passive");
@@ -3943,7 +1522,8 @@ static int at76_startup_device(struct at76_priv *priv)
 	ccfg->ssid_len = priv->essid_size;
 
 	ccfg->wep_default_key_id = priv->wep_key_id;
-	memcpy(ccfg->wep_default_key_value, priv->wep_keys, 4 * WEP_KEY_LEN);
+	memcpy(ccfg->wep_default_key_value, priv->wep_keys,
+	       sizeof(priv->wep_keys));
 
 	ccfg->short_preamble = priv->preamble_type;
 	ccfg->beacon_period = cpu_to_le16(priv->beacon_period);
@@ -3952,7 +1532,7 @@ static int at76_startup_device(struct at76_priv *priv)
 				    sizeof(struct at76_card_config));
 	if (ret < 0) {
 		printk(KERN_ERR "%s: at76_set_card_command failed: %d\n",
-		       priv->netdev->name, ret);
+		       wiphy_name(priv->hw->wiphy), ret);
 		return ret;
 	}
 
@@ -3998,69 +1578,6 @@ static int at76_startup_device(struct at76_priv *priv)
 	return 0;
 }
 
-/* Restart the interface */
-static void at76_dwork_restart(struct work_struct *work)
-{
-	struct at76_priv *priv = container_of(work, struct at76_priv,
-					      dwork_restart.work);
-
-	mutex_lock(&priv->mtx);
-
-	netif_carrier_off(priv->netdev);	/* stop netdev watchdog */
-	netif_stop_queue(priv->netdev);	/* stop tx data packets */
-
-	at76_startup_device(priv);
-
-	if (priv->iw_mode != IW_MODE_MONITOR) {
-		priv->netdev->type = ARPHRD_ETHER;
-		at76_set_mac_state(priv, MAC_SCANNING);
-		schedule_work(&priv->work_start_scan);
-	} else {
-		priv->netdev->type = ARPHRD_IEEE80211_RADIOTAP;
-		at76_start_monitor(priv);
-	}
-
-	mutex_unlock(&priv->mtx);
-}
-
-/* Initiate scanning */
-static void at76_work_start_scan(struct work_struct *work)
-{
-	struct at76_priv *priv = container_of(work, struct at76_priv,
-					      work_start_scan);
-	int ret;
-
-	mutex_lock(&priv->mtx);
-
-	WARN_ON(priv->mac_state != MAC_SCANNING);
-	if (priv->mac_state != MAC_SCANNING)
-		goto exit;
-
-	/* only clear the bss list when a scan is actively initiated,
-	 * otherwise simply rely on at76_bss_list_timeout */
-	if (priv->scan_state == SCAN_IN_PROGRESS) {
-		at76_free_bss_list(priv);
-		priv->scan_need_any = 1;
-	} else
-		priv->scan_need_any = 0;
-
-	ret = at76_start_scan(priv, 1);
-
-	if (ret < 0)
-		printk(KERN_ERR "%s: %s: start_scan failed with %d\n",
-		       priv->netdev->name, __func__, ret);
-	else {
-		at76_dbg(DBG_MGMT_TIMER,
-			 "%s:%d: starting mgmt_timer for %d ticks",
-			 __func__, __LINE__, SCAN_POLL_INTERVAL);
-		schedule_delayed_work(&priv->dwork_get_scan,
-				      SCAN_POLL_INTERVAL);
-	}
-
-exit:
-	mutex_unlock(&priv->mtx);
-}
-
 /* Enable or disable promiscuous mode */
 static void at76_work_set_promisc(struct work_struct *work)
 {
@@ -4078,7 +1595,7 @@ static void at76_work_set_promisc(struct work_struct *work)
 	ret = at76_set_mib(priv, &priv->mib_buf);
 	if (ret < 0)
 		printk(KERN_ERR "%s: set_mib (promiscuous_mode) failed: %d\n",
-		       priv->netdev->name, ret);
+		       wiphy_name(priv->hw->wiphy), ret);
 
 	mutex_unlock(&priv->mtx);
 }
@@ -4094,1088 +1611,759 @@ static void at76_work_submit_rx(struct work_struct *work)
 	mutex_unlock(&priv->mtx);
 }
 
-/* We got an association response */
-static void at76_rx_mgmt_assoc(struct at76_priv *priv,
-			       struct at76_rx_buffer *buf)
+static void at76_rx_tasklet(unsigned long param)
 {
-	struct ieee80211_assoc_response *resp =
-	    (struct ieee80211_assoc_response *)buf->packet;
-	u16 assoc_id = le16_to_cpu(resp->aid);
-	u16 status = le16_to_cpu(resp->status);
-
-	at76_dbg(DBG_RX_MGMT, "%s: rx AssocResp bssid %s capa 0x%04x status "
-		 "0x%04x assoc_id 0x%04x rates %s", priv->netdev->name,
-		 mac2str(resp->header.addr3), le16_to_cpu(resp->capability),
-		 status, assoc_id, hex2str(resp->info_element->data,
-					   resp->info_element->len));
-
-	if (priv->mac_state != MAC_ASSOC) {
-		printk(KERN_INFO "%s: AssocResp in state %s ignored\n",
-		       priv->netdev->name, mac_states[priv->mac_state]);
-		return;
-	}
-
-	BUG_ON(!priv->curr_bss);
-
-	cancel_delayed_work(&priv->dwork_assoc);
-	if (status == WLAN_STATUS_SUCCESS) {
-		struct bss_info *ptr = priv->curr_bss;
-		priv->assoc_id = assoc_id & 0x3fff;
-		/* update iwconfig params */
-		memcpy(priv->bssid, ptr->bssid, ETH_ALEN);
-		memcpy(priv->essid, ptr->ssid, ptr->ssid_len);
-		priv->essid_size = ptr->ssid_len;
-		priv->channel = ptr->channel;
-		schedule_work(&priv->work_assoc_done);
-	} else {
-		at76_set_mac_state(priv, MAC_JOINING);
-		schedule_work(&priv->work_join);
-	}
-}
+	struct urb *urb = (struct urb *)param;
+	struct at76_priv *priv = urb->context;
+	struct at76_rx_buffer *buf;
+	struct ieee80211_rx_status rx_status = { 0 };
 
-/* Process disassociation request from the AP */
-static void at76_rx_mgmt_disassoc(struct at76_priv *priv,
-				  struct at76_rx_buffer *buf)
-{
-	struct ieee80211_disassoc *resp =
-	    (struct ieee80211_disassoc *)buf->packet;
-	struct ieee80211_hdr_3addr *mgmt = &resp->header;
-
-	at76_dbg(DBG_RX_MGMT,
-		 "%s: rx DisAssoc bssid %s reason 0x%04x destination %s",
-		 priv->netdev->name, mac2str(mgmt->addr3),
-		 le16_to_cpu(resp->reason), mac2str(mgmt->addr1));
-
-	/* We are not connected, ignore */
-	if (priv->mac_state == MAC_SCANNING || priv->mac_state == MAC_INIT
-	    || !priv->curr_bss)
+	if (priv->device_unplugged) {
+		at76_dbg(DBG_DEVSTART, "device unplugged");
+		if (urb)
+			at76_dbg(DBG_DEVSTART, "urb status %d", urb->status);
 		return;
+	}
 
-	/* Not our BSSID, ignore */
-	if (compare_ether_addr(mgmt->addr3, priv->curr_bss->bssid))
+	if (!priv->rx_skb || !priv->rx_skb->data)
 		return;
 
-	/* Not for our STA and not broadcast, ignore */
-	if (compare_ether_addr(priv->netdev->dev_addr, mgmt->addr1)
-	    && !is_broadcast_ether_addr(mgmt->addr1))
-		return;
+	buf = (struct at76_rx_buffer *)priv->rx_skb->data;
 
-	if (priv->mac_state != MAC_ASSOC && priv->mac_state != MAC_CONNECTED
-	    && priv->mac_state != MAC_JOINING) {
-		printk(KERN_INFO "%s: DisAssoc in state %s ignored\n",
-		       priv->netdev->name, mac_states[priv->mac_state]);
+	if (urb->status != 0) {
+		if (urb->status != -ENOENT && urb->status != -ECONNRESET)
+			at76_dbg(DBG_URB,
+				 "%s %s: - nonzero Rx bulk status received: %d",
+				 __func__, wiphy_name(priv->hw->wiphy),
+				 urb->status);
 		return;
 	}
 
-	if (priv->mac_state == MAC_CONNECTED) {
-		netif_carrier_off(priv->netdev);
-		netif_stop_queue(priv->netdev);
-		at76_iwevent_bss_disconnect(priv->netdev);
-	}
-	cancel_delayed_work(&priv->dwork_get_scan);
-	cancel_delayed_work(&priv->dwork_beacon);
-	cancel_delayed_work(&priv->dwork_auth);
-	cancel_delayed_work(&priv->dwork_assoc);
-	at76_set_mac_state(priv, MAC_JOINING);
-	schedule_work(&priv->work_join);
+	at76_dbg(DBG_RX_ATMEL_HDR,
+		 "%s: rx frame: rate %d rssi %d noise %d link %d",
+		 wiphy_name(priv->hw->wiphy), buf->rx_rate, buf->rssi,
+		 buf->noise_level, buf->link_quality);
+
+	skb_trim(priv->rx_skb, le16_to_cpu(buf->wlength) + AT76_RX_HDRLEN);
+	at76_dbg_dump(DBG_RX_DATA, &priv->rx_skb->data[AT76_RX_HDRLEN],
+		      priv->rx_skb->len, "RX: len=%d",
+		      (int)(priv->rx_skb->len - AT76_RX_HDRLEN));
+
+	rx_status.signal = buf->rssi;
+	/* FIXME: is rate_idx still present in structure? */
+	rx_status.rate_idx = buf->rx_rate;
+	rx_status.flag |= RX_FLAG_DECRYPTED;
+	rx_status.flag |= RX_FLAG_IV_STRIPPED;
+
+	skb_pull(priv->rx_skb, AT76_RX_HDRLEN);
+	at76_dbg(DBG_MAC80211, "calling ieee80211_rx_irqsafe(): %d/%d",
+		 priv->rx_skb->len, priv->rx_skb->data_len);
+	ieee80211_rx_irqsafe(priv->hw, priv->rx_skb, &rx_status);
+
+	/* Use a new skb for the next receive */
+	priv->rx_skb = NULL;
+
+	at76_submit_rx_urb(priv);
 }
 
-static void at76_rx_mgmt_auth(struct at76_priv *priv,
-			      struct at76_rx_buffer *buf)
+/* Load firmware into kernel memory and parse it */
+static struct fwentry *at76_load_firmware(struct usb_device *udev,
+					  enum board_type board_type)
 {
-	struct ieee80211_auth *resp = (struct ieee80211_auth *)buf->packet;
-	struct ieee80211_hdr_3addr *mgmt = &resp->header;
-	int seq_nr = le16_to_cpu(resp->transaction);
-	int alg = le16_to_cpu(resp->algorithm);
-	int status = le16_to_cpu(resp->status);
-
-	at76_dbg(DBG_RX_MGMT,
-		 "%s: rx AuthFrame bssid %s alg %d seq_nr %d status %d "
-		 "destination %s", priv->netdev->name, mac2str(mgmt->addr3),
-		 alg, seq_nr, status, mac2str(mgmt->addr1));
-
-	if (alg == WLAN_AUTH_SHARED_KEY && seq_nr == 2)
-		at76_dbg(DBG_RX_MGMT, "%s: AuthFrame challenge %s ...",
-			 priv->netdev->name, hex2str(resp->info_element, 18));
-
-	if (priv->mac_state != MAC_AUTH) {
-		printk(KERN_INFO "%s: ignored AuthFrame in state %s\n",
-		       priv->netdev->name, mac_states[priv->mac_state]);
-		return;
-	}
-	if (priv->auth_mode != alg) {
-		printk(KERN_INFO "%s: ignored AuthFrame for alg %d\n",
-		       priv->netdev->name, alg);
-		return;
+	int ret;
+	char *str;
+	struct at76_fw_header *fwh;
+	struct fwentry *fwe = &firmwares[board_type];
+
+	mutex_lock(&fw_mutex);
+
+	if (fwe->loaded) {
+		at76_dbg(DBG_FW, "re-using previously loaded fw");
+		goto exit;
 	}
 
-	BUG_ON(!priv->curr_bss);
+	at76_dbg(DBG_FW, "downloading firmware %s", fwe->fwname);
+	ret = request_firmware(&fwe->fw, fwe->fwname, &udev->dev);
+	if (ret < 0) {
+		dev_printk(KERN_ERR, &udev->dev, "firmware %s not found!\n",
+			   fwe->fwname);
+		dev_printk(KERN_ERR, &udev->dev,
+			   "you may need to download the firmware from "
+			   "http://developer.berlios.de/projects/at76c503a/\n");
+		goto exit;
+	}
 
-	/* Not our BSSID or not for our STA, ignore */
-	if (compare_ether_addr(mgmt->addr3, priv->curr_bss->bssid)
-	    || compare_ether_addr(priv->netdev->dev_addr, mgmt->addr1))
-		return;
+	at76_dbg(DBG_FW, "got it.");
+	fwh = (struct at76_fw_header *)(fwe->fw->data);
 
-	cancel_delayed_work(&priv->dwork_auth);
-	if (status != WLAN_STATUS_SUCCESS) {
-		/* try to join next bss */
-		at76_set_mac_state(priv, MAC_JOINING);
-		schedule_work(&priv->work_join);
-		return;
+	if (fwe->fw->size <= sizeof(*fwh)) {
+		dev_printk(KERN_ERR, &udev->dev,
+			   "firmware is too short (0x%zx)\n", fwe->fw->size);
+		goto exit;
 	}
 
-	if (priv->auth_mode == WLAN_AUTH_OPEN || seq_nr == 4) {
-		priv->retries = ASSOC_RETRIES;
-		at76_set_mac_state(priv, MAC_ASSOC);
-		at76_assoc_req(priv, priv->curr_bss);
-		at76_dbg(DBG_MGMT_TIMER,
-			 "%s:%d: starting mgmt_timer + HZ", __func__, __LINE__);
-		schedule_delayed_work(&priv->dwork_assoc, ASSOC_TIMEOUT);
-		return;
+	/* CRC currently not checked */
+	fwe->board_type = le32_to_cpu(fwh->board_type);
+	if (fwe->board_type != board_type) {
+		dev_printk(KERN_ERR, &udev->dev,
+			   "board type mismatch, requested %u, got %u\n",
+			   board_type, fwe->board_type);
+		goto exit;
 	}
 
-	WARN_ON(seq_nr != 2);
-	at76_auth_req(priv, priv->curr_bss, seq_nr + 1, resp->info_element);
-	at76_dbg(DBG_MGMT_TIMER, "%s:%d: starting mgmt_timer + HZ", __func__,
-		 __LINE__);
-	schedule_delayed_work(&priv->dwork_auth, AUTH_TIMEOUT);
-}
+	fwe->fw_version.major = fwh->major;
+	fwe->fw_version.minor = fwh->minor;
+	fwe->fw_version.patch = fwh->patch;
+	fwe->fw_version.build = fwh->build;
 
-static void at76_rx_mgmt_deauth(struct at76_priv *priv,
-				struct at76_rx_buffer *buf)
-{
-	struct ieee80211_disassoc *resp =
-	    (struct ieee80211_disassoc *)buf->packet;
-	struct ieee80211_hdr_3addr *mgmt = &resp->header;
-
-	at76_dbg(DBG_RX_MGMT | DBG_PROGRESS,
-		 "%s: rx DeAuth bssid %s reason 0x%04x destination %s",
-		 priv->netdev->name, mac2str(mgmt->addr3),
-		 le16_to_cpu(resp->reason), mac2str(mgmt->addr1));
-
-	if (priv->mac_state != MAC_AUTH && priv->mac_state != MAC_ASSOC
-	    && priv->mac_state != MAC_CONNECTED) {
-		printk(KERN_INFO "%s: DeAuth in state %s ignored\n",
-		       priv->netdev->name, mac_states[priv->mac_state]);
-		return;
-	}
+	str = (char *)fwh + le32_to_cpu(fwh->str_offset);
+	fwe->intfw = (u8 *)fwh + le32_to_cpu(fwh->int_fw_offset);
+	fwe->intfw_size = le32_to_cpu(fwh->int_fw_len);
+	fwe->extfw = (u8 *)fwh + le32_to_cpu(fwh->ext_fw_offset);
+	fwe->extfw_size = le32_to_cpu(fwh->ext_fw_len);
 
-	BUG_ON(!priv->curr_bss);
+	fwe->loaded = 1;
 
-	/* Not our BSSID, ignore */
-	if (compare_ether_addr(mgmt->addr3, priv->curr_bss->bssid))
-		return;
+	dev_printk(KERN_DEBUG, &udev->dev,
+		   "using firmware %s (version %d.%d.%d-%d)\n",
+		   fwe->fwname, fwh->major, fwh->minor, fwh->patch, fwh->build);
 
-	/* Not for our STA and not broadcast, ignore */
-	if (compare_ether_addr(priv->netdev->dev_addr, mgmt->addr1)
-	    && !is_broadcast_ether_addr(mgmt->addr1))
-		return;
+	at76_dbg(DBG_DEVSTART, "board %u, int %d:%d, ext %d:%d", board_type,
+		 le32_to_cpu(fwh->int_fw_offset), le32_to_cpu(fwh->int_fw_len),
+		 le32_to_cpu(fwh->ext_fw_offset), le32_to_cpu(fwh->ext_fw_len));
+	at76_dbg(DBG_DEVSTART, "firmware id %s", str);
 
-	if (priv->mac_state == MAC_CONNECTED)
-		at76_iwevent_bss_disconnect(priv->netdev);
+exit:
+	mutex_unlock(&fw_mutex);
 
-	at76_set_mac_state(priv, MAC_JOINING);
-	schedule_work(&priv->work_join);
-	cancel_delayed_work(&priv->dwork_get_scan);
-	cancel_delayed_work(&priv->dwork_beacon);
-	cancel_delayed_work(&priv->dwork_auth);
-	cancel_delayed_work(&priv->dwork_assoc);
+	if (fwe->loaded)
+		return fwe;
+	else
+		return NULL;
 }
 
-static void at76_rx_mgmt_beacon(struct at76_priv *priv,
-				struct at76_rx_buffer *buf)
+static void at76_mac80211_tx_callback(struct urb *urb)
 {
-	int varpar_len;
-	/* beacon content */
-	struct ieee80211_beacon *bdata = (struct ieee80211_beacon *)buf->packet;
-	struct ieee80211_hdr_3addr *mgmt = &bdata->header;
-
-	struct list_head *lptr;
-	struct bss_info *match;	/* entry matching addr3 with its bssid */
-	int new_entry = 0;
-	int len;
-	struct ieee80211_info_element *ie;
-	int have_ssid = 0;
-	int have_rates = 0;
-	int have_channel = 0;
-	int keep_going = 1;
-	unsigned long flags;
-
-	spin_lock_irqsave(&priv->bss_list_spinlock, flags);
-	if (priv->mac_state == MAC_CONNECTED) {
-		/* in state MAC_CONNECTED we use the mgmt_timer to control
-		   the beacon of the BSS */
-		BUG_ON(!priv->curr_bss);
-
-		if (!compare_ether_addr(priv->curr_bss->bssid, mgmt->addr3)) {
-			/* We got our AP's beacon, defer the timeout handler.
-			   Kill pending work first, as schedule_delayed_work()
-			   won't do it. */
-			cancel_delayed_work(&priv->dwork_beacon);
-			schedule_delayed_work(&priv->dwork_beacon,
-					      BEACON_TIMEOUT);
-			priv->curr_bss->rssi = buf->rssi;
-			priv->beacons_received++;
-			goto exit;
-		}
-	}
+	struct at76_priv *priv = urb->context;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(priv->tx_skb);
 
-	/* look if we have this BSS already in the list */
-	match = NULL;
+	at76_dbg(DBG_MAC80211, "%s()", __func__);
 
-	if (!list_empty(&priv->bss_list)) {
-		list_for_each(lptr, &priv->bss_list) {
-			struct bss_info *bss_ptr =
-			    list_entry(lptr, struct bss_info, list);
-			if (!compare_ether_addr(bss_ptr->bssid, mgmt->addr3)) {
-				match = bss_ptr;
-				break;
-			}
-		}
+	switch (urb->status) {
+	case 0:
+		/* success */
+		/* FIXME:
+		 * is the frame really ACKed when tx_callback is called ? */
+		info->flags |= IEEE80211_TX_STAT_ACK;
+		break;
+	case -ENOENT:
+	case -ECONNRESET:
+		/* fail, urb has been unlinked */
+		/* FIXME: add error message */
+		break;
+	default:
+		at76_dbg(DBG_URB, "%s - nonzero tx status received: %d",
+			 __func__, urb->status);
+		break;
 	}
 
-	if (!match) {
-		/* BSS not in the list - append it */
-		match = kzalloc(sizeof(struct bss_info), GFP_ATOMIC);
-		if (!match) {
-			at76_dbg(DBG_BSS_TABLE,
-				 "%s: cannot kmalloc new bss info (%zd byte)",
-				 priv->netdev->name, sizeof(struct bss_info));
-			goto exit;
-		}
-		new_entry = 1;
-		list_add_tail(&match->list, &priv->bss_list);
-	}
+	memset(&info->status, 0, sizeof(info->status));
 
-	match->capa = le16_to_cpu(bdata->capability);
-	match->beacon_interval = le16_to_cpu(bdata->beacon_interval);
-	match->rssi = buf->rssi;
-	match->link_qual = buf->link_quality;
-	match->noise_level = buf->noise_level;
-	memcpy(match->bssid, mgmt->addr3, ETH_ALEN);
-	at76_dbg(DBG_RX_BEACON, "%s: bssid %s", priv->netdev->name,
-		 mac2str(match->bssid));
-
-	ie = bdata->info_element;
-
-	/* length of var length beacon parameters */
-	varpar_len = min_t(int, le16_to_cpu(buf->wlength) -
-			   sizeof(struct ieee80211_beacon),
-			   BEACON_MAX_DATA_LENGTH);
-
-	/* This routine steps through the bdata->data array to get
-	 * some useful information about the access point.
-	 * Currently, this implementation supports receipt of: SSID,
-	 * supported transfer rates and channel, in any order, with some
-	 * tolerance for intermittent unknown codes (although this
-	 * functionality may not be necessary as the useful information will
-	 * usually arrive in consecutively, but there have been some
-	 * reports of some of the useful information fields arriving in a
-	 * different order).
-	 * It does not support any more IE types although MFIE_TYPE_TIM may
-	 * be supported (on my AP at least).
-	 * The bdata->data array is about 1500 bytes long but only ~36 of those
-	 * bytes are useful, hence the have_ssid etc optimizations. */
-
-	while (keep_going &&
-	       ((&ie->data[ie->len] - (u8 *)bdata->info_element) <=
-		varpar_len)) {
-
-		switch (ie->id) {
-
-		case MFIE_TYPE_SSID:
-			if (have_ssid)
-				break;
+	ieee80211_tx_status_irqsafe(priv->hw, priv->tx_skb);
 
-			len = min_t(int, IW_ESSID_MAX_SIZE, ie->len);
+	priv->tx_skb = NULL;
 
-			/* we copy only if this is a new entry,
-			   or the incoming SSID is not a hidden SSID. This
-			   will protect us from overwriting a real SSID read
-			   in a ProbeResponse with a hidden one from a
-			   following beacon. */
-			if (!new_entry && at76_is_hidden_ssid(ie->data, len)) {
-				have_ssid = 1;
-				break;
-			}
+	ieee80211_wake_queues(priv->hw);
+}
 
-			match->ssid_len = len;
-			memcpy(match->ssid, ie->data, len);
-			at76_dbg(DBG_RX_BEACON, "%s: SSID - %.*s",
-				 priv->netdev->name, len, match->ssid);
-			have_ssid = 1;
-			break;
+static int at76_mac80211_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
+{
+	struct at76_priv *priv = hw->priv;
+	struct at76_tx_buffer *tx_buffer = priv->bulk_out_buffer;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+	int padding, submit_len, ret;
 
-		case MFIE_TYPE_RATES:
-			if (have_rates)
-				break;
+	at76_dbg(DBG_MAC80211, "%s()", __func__);
 
-			match->rates_len =
-			    min_t(int, sizeof(match->rates), ie->len);
-			memcpy(match->rates, ie->data, match->rates_len);
-			have_rates = 1;
-			at76_dbg(DBG_RX_BEACON, "%s: SUPPORTED RATES %s",
-				 priv->netdev->name,
-				 hex2str(ie->data, ie->len));
-			break;
+	if (priv->tx_urb->status == -EINPROGRESS) {
+		printk(KERN_ERR "%s: %s called while tx urb is pending\n",
+		       wiphy_name(priv->hw->wiphy), __func__);
+		return NETDEV_TX_BUSY;
+	}
 
-		case MFIE_TYPE_DS_SET:
-			if (have_channel)
-				break;
+	ieee80211_stop_queues(hw);
 
-			match->channel = ie->data[0];
-			have_channel = 1;
-			at76_dbg(DBG_RX_BEACON, "%s: CHANNEL - %d",
-				 priv->netdev->name, match->channel);
-			break;
+	at76_ledtrig_tx_activity();	/* tell ledtrigger we send a packet */
 
-		case MFIE_TYPE_CF_SET:
-		case MFIE_TYPE_TIM:
-		case MFIE_TYPE_IBSS_SET:
-		default:
-			at76_dbg(DBG_RX_BEACON, "%s: beacon IE id %d len %d %s",
-				 priv->netdev->name, ie->id, ie->len,
-				 hex2str(ie->data, ie->len));
-			break;
-		}
+	WARN_ON(priv->tx_skb != NULL);
 
-		/* advance to the next informational element */
-		next_ie(&ie);
+	priv->tx_skb = skb;
+	padding = at76_calc_padding(skb->len);
+	submit_len = AT76_TX_HDRLEN + skb->len + padding;
 
-		/* Optimization: after all, the bdata->data array is
-		 * varpar_len bytes long, whereas we get all of the useful
-		 * information after only ~36 bytes, this saves us a lot of
-		 * time (and trouble as the remaining portion of the array
-		 * could be full of junk)
-		 * Comment this out if you want to see what other information
-		 * comes from the AP - although little of it may be useful */
-	}
+	/* setup 'Atmel' header */
+	memset(tx_buffer, 0, sizeof(*tx_buffer));
+	tx_buffer->padding = padding;
+	tx_buffer->wlength = cpu_to_le16(skb->len);
+	tx_buffer->tx_rate = ieee80211_get_tx_rate(hw, info)->hw_value;
+	if (FIRMWARE_IS_WPA(priv->fw_version) && info->control.hw_key) {
+		tx_buffer->key_id = (info->control.hw_key->keyidx);
+		tx_buffer->cipher_type =
+			priv->keys[info->control.hw_key->keyidx].cipher;
+		tx_buffer->cipher_length =
+			priv->keys[info->control.hw_key->keyidx].keylen;
+		tx_buffer->reserved = 0;
+	} else {
+		tx_buffer->key_id = 0;
+		tx_buffer->cipher_type = 0;
+		tx_buffer->cipher_length = 0;
+		tx_buffer->reserved = 0;
+	};
+	/* memset(tx_buffer->reserved, 0, sizeof(tx_buffer->reserved)); */
+	memcpy(tx_buffer->packet, skb->data, skb->len);
 
-	at76_dbg(DBG_RX_BEACON, "%s: Finished processing beacon data",
-		 priv->netdev->name);
+	at76_dbg(DBG_TX_DATA, "%s tx: wlen 0x%x pad 0x%x rate %d hdr",
+		 wiphy_name(priv->hw->wiphy), le16_to_cpu(tx_buffer->wlength),
+		 tx_buffer->padding, tx_buffer->tx_rate);
 
-	match->last_rx = jiffies;	/* record last rx of beacon */
+	/* send stuff */
+	at76_dbg_dump(DBG_TX_DATA_CONTENT, tx_buffer, submit_len,
+		      "%s(): tx_buffer %d bytes:", __func__, submit_len);
+	usb_fill_bulk_urb(priv->tx_urb, priv->udev, priv->tx_pipe, tx_buffer,
+			  submit_len, at76_mac80211_tx_callback, priv);
+	ret = usb_submit_urb(priv->tx_urb, GFP_ATOMIC);
+	if (ret) {
+		printk(KERN_ERR "%s: error in tx submit urb: %d\n",
+		       wiphy_name(priv->hw->wiphy), ret);
+		if (ret == -EINVAL)
+			printk(KERN_ERR
+			       "%s: -EINVAL: tx urb %p hcpriv %p complete %p\n",
+			       wiphy_name(priv->hw->wiphy), priv->tx_urb,
+			       priv->tx_urb->hcpriv, priv->tx_urb->complete);
+	}
 
-exit:
-	spin_unlock_irqrestore(&priv->bss_list_spinlock, flags);
+	return 0;
 }
 
-/* Calculate the link level from a given rx_buffer */
-static void at76_calc_level(struct at76_priv *priv, struct at76_rx_buffer *buf,
-			    struct iw_quality *qual)
+static int at76_mac80211_start(struct ieee80211_hw *hw)
 {
-	/* just a guess for now, might be different for other chips */
-	int max_rssi = 42;
+	struct at76_priv *priv = hw->priv;
+	int ret;
 
-	qual->level = (buf->rssi * 100 / max_rssi);
-	if (qual->level > 100)
-		qual->level = 100;
-	qual->updated |= IW_QUAL_LEVEL_UPDATED;
-}
+	at76_dbg(DBG_MAC80211, "%s()", __func__);
 
-/* Calculate the link quality from a given rx_buffer */
-static void at76_calc_qual(struct at76_priv *priv, struct at76_rx_buffer *buf,
-			   struct iw_quality *qual)
-{
-	if (at76_is_intersil(priv->board_type))
-		qual->qual = buf->link_quality;
-	else {
-		unsigned long elapsed;
+	mutex_lock(&priv->mtx);
 
-		/* Update qual at most once a second */
-		elapsed = jiffies - priv->beacons_last_qual;
-		if (elapsed < 1 * HZ)
-			return;
+	ret = at76_submit_rx_urb(priv);
+	if (ret < 0) {
+		printk(KERN_ERR "%s: open: submit_rx_urb failed: %d\n",
+		       wiphy_name(priv->hw->wiphy), ret);
+		goto error;
+	}
 
-		qual->qual = qual->level * priv->beacons_received *
-		    msecs_to_jiffies(priv->beacon_period) / elapsed;
+	at76_startup_device(priv);
 
-		priv->beacons_last_qual = jiffies;
-		priv->beacons_received = 0;
-	}
-	qual->qual = (qual->qual > 100) ? 100 : qual->qual;
-	qual->updated |= IW_QUAL_QUAL_UPDATED;
-}
+	at76_start_monitor(priv);
 
-/* Calculate the noise quality from a given rx_buffer */
-static void at76_calc_noise(struct at76_priv *priv, struct at76_rx_buffer *buf,
-			    struct iw_quality *qual)
-{
-	qual->noise = 0;
-	qual->updated |= IW_QUAL_NOISE_INVALID;
+error:
+	mutex_unlock(&priv->mtx);
+
+	return 0;
 }
 
-static void at76_update_wstats(struct at76_priv *priv,
-			       struct at76_rx_buffer *buf)
+static void at76_mac80211_stop(struct ieee80211_hw *hw)
 {
-	struct iw_quality *qual = &priv->wstats.qual;
+	struct at76_priv *priv = hw->priv;
 
-	if (buf->rssi && priv->mac_state == MAC_CONNECTED) {
-		qual->updated = 0;
-		at76_calc_level(priv, buf, qual);
-		at76_calc_qual(priv, buf, qual);
-		at76_calc_noise(priv, buf, qual);
-	} else {
-		qual->qual = 0;
-		qual->level = 0;
-		qual->noise = 0;
-		qual->updated = IW_QUAL_ALL_INVALID;
-	}
-}
+	at76_dbg(DBG_MAC80211, "%s()", __func__);
 
-static void at76_rx_mgmt(struct at76_priv *priv, struct at76_rx_buffer *buf)
-{
-	struct ieee80211_hdr_3addr *mgmt =
-	    (struct ieee80211_hdr_3addr *)buf->packet;
-	u16 framectl = le16_to_cpu(mgmt->frame_ctl);
-
-	/* update wstats */
-	if (priv->mac_state != MAC_INIT && priv->mac_state != MAC_SCANNING) {
-		/* jal: this is a dirty hack needed by Tim in ad-hoc mode */
-		/* Data packets always seem to have a 0 link level, so we
-		   only read link quality info from management packets.
-		   Atmel driver actually averages the present, and previous
-		   values, we just present the raw value at the moment - TJS */
-		if (priv->iw_mode == IW_MODE_ADHOC
-		    || (priv->curr_bss
-			&& !compare_ether_addr(mgmt->addr3,
-					       priv->curr_bss->bssid)))
-			at76_update_wstats(priv, buf);
-	}
+	mutex_lock(&priv->mtx);
 
-	at76_dbg(DBG_RX_MGMT_CONTENT, "%s rx mgmt framectl 0x%x %s",
-		 priv->netdev->name, framectl,
-		 hex2str(mgmt, le16_to_cpu(buf->wlength)));
+	if (!priv->device_unplugged) {
+		/* We are called by "ifconfig ethX down", not because the
+		 * device is not available anymore. */
+		if (at76_set_radio(priv, 0) == 1)
+			at76_wait_completion(priv, CMD_RADIO_ON);
 
-	switch (framectl & IEEE80211_FCTL_STYPE) {
-	case IEEE80211_STYPE_BEACON:
-	case IEEE80211_STYPE_PROBE_RESP:
-		at76_rx_mgmt_beacon(priv, buf);
-		break;
+		/* We unlink rx_urb because at76_open() re-submits it.
+		 * If unplugged, at76_delete_device() takes care of it. */
+		usb_kill_urb(priv->rx_urb);
+	}
 
-	case IEEE80211_STYPE_ASSOC_RESP:
-		at76_rx_mgmt_assoc(priv, buf);
-		break;
+	mutex_unlock(&priv->mtx);
+}
 
-	case IEEE80211_STYPE_DISASSOC:
-		at76_rx_mgmt_disassoc(priv, buf);
-		break;
+static int at76_add_interface(struct ieee80211_hw *hw,
+			      struct ieee80211_if_init_conf *conf)
+{
+	struct at76_priv *priv = hw->priv;
+	int ret = 0;
 
-	case IEEE80211_STYPE_AUTH:
-		at76_rx_mgmt_auth(priv, buf);
-		break;
+	at76_dbg(DBG_MAC80211, "%s()", __func__);
 
-	case IEEE80211_STYPE_DEAUTH:
-		at76_rx_mgmt_deauth(priv, buf);
-		break;
+	mutex_lock(&priv->mtx);
 
+	switch (conf->type) {
+	case NL80211_IFTYPE_STATION:
+		priv->iw_mode = IW_MODE_INFRA;
+		break;
 	default:
-		printk(KERN_DEBUG "%s: ignoring frame with framectl 0x%04x\n",
-		       priv->netdev->name, framectl);
+		ret = -EOPNOTSUPP;
+		goto exit;
 	}
 
-	return;
+exit:
+	mutex_unlock(&priv->mtx);
+
+	return ret;
 }
 
-/* Convert the 802.11 header into an ethernet-style header, make skb
- * ready for consumption by netif_rx() */
-static void at76_ieee80211_to_eth(struct sk_buff *skb, int iw_mode)
+static void at76_remove_interface(struct ieee80211_hw *hw,
+				  struct ieee80211_if_init_conf *conf)
 {
-	struct ieee80211_hdr_3addr *i802_11_hdr;
-	struct ethhdr *eth_hdr_p;
-	u8 *src_addr;
-	u8 *dest_addr;
+	at76_dbg(DBG_MAC80211, "%s()", __func__);
+}
 
-	i802_11_hdr = (struct ieee80211_hdr_3addr *)skb->data;
+static int at76_join(struct at76_priv *priv)
+{
+	struct at76_req_join join;
+	int ret;
 
-	/* That would be the ethernet header if the hardware converted
-	 * the frame for us.  Make sure the source and the destination
-	 * match the 802.11 header.  Which hardware does it? */
-	eth_hdr_p = (struct ethhdr *)skb_pull(skb, IEEE80211_3ADDR_LEN);
+	memset(&join, 0, sizeof(struct at76_req_join));
+	memcpy(join.essid, priv->essid, priv->essid_size);
+	join.essid_size = priv->essid_size;
+	memcpy(join.bssid, priv->bssid, ETH_ALEN);
+	join.bss_type = INFRASTRUCTURE_MODE;
+	join.channel = priv->channel;
+	join.timeout = cpu_to_le16(2000);
 
-	dest_addr = i802_11_hdr->addr1;
-	if (iw_mode == IW_MODE_ADHOC)
-		src_addr = i802_11_hdr->addr2;
-	else
-		src_addr = i802_11_hdr->addr3;
+	at76_dbg(DBG_MAC80211, "%s: sending CMD_JOIN", __func__);
+	ret = at76_set_card_command(priv->udev, CMD_JOIN, &join,
+				    sizeof(struct at76_req_join));
 
-	if (!compare_ether_addr(eth_hdr_p->h_source, src_addr) &&
-	    !compare_ether_addr(eth_hdr_p->h_dest, dest_addr))
-		/* Yes, we already have an ethernet header */
-		skb_reset_mac_header(skb);
-	else {
-		u16 len;
-
-		/* Need to build an ethernet header */
-		if (!memcmp(skb->data, snapsig, sizeof(snapsig))) {
-			/* SNAP frame - decapsulate, keep proto */
-			skb_push(skb, offsetof(struct ethhdr, h_proto) -
-				 sizeof(rfc1042sig));
-			len = 0;
-		} else {
-			/* 802.3 frame, proto is length */
-			len = skb->len;
-			skb_push(skb, ETH_HLEN);
-		}
+	if (ret < 0) {
+		printk(KERN_ERR "%s: at76_set_card_command failed: %d\n",
+		       wiphy_name(priv->hw->wiphy), ret);
+		return 0;
+	}
 
-		skb_reset_mac_header(skb);
-		eth_hdr_p = eth_hdr(skb);
-		/* This needs to be done in this order (eth_hdr_p->h_dest may
-		 * overlap src_addr) */
-		memcpy(eth_hdr_p->h_source, src_addr, ETH_ALEN);
-		memcpy(eth_hdr_p->h_dest, dest_addr, ETH_ALEN);
-		if (len)
-			eth_hdr_p->h_proto = htons(len);
+	ret = at76_wait_completion(priv, CMD_JOIN);
+	at76_dbg(DBG_MAC80211, "%s: CMD_JOIN returned: 0x%02x", __func__, ret);
+	if (ret != CMD_STATUS_COMPLETE) {
+		printk(KERN_ERR "%s: at76_wait_completion failed: %d\n",
+		       wiphy_name(priv->hw->wiphy), ret);
+		return 0;
 	}
 
-	skb->protocol = eth_type_trans(skb, skb->dev);
+	at76_set_tkip_bssid(priv, priv->bssid);
+	at76_set_pm_mode(priv);
+
+	return 0;
 }
 
-/* Check for fragmented data in priv->rx_skb. If the packet was no fragment
-   or it was the last of a fragment set a skb containing the whole packet
-   is returned for further processing. Otherwise we get NULL and are
-   done and the packet is either stored inside the fragment buffer
-   or thrown away.  Every returned skb starts with the ieee802_11 header
-   and contains _no_ FCS at the end */
-static struct sk_buff *at76_check_for_rx_frags(struct at76_priv *priv)
+static void at76_dwork_hw_scan(struct work_struct *work)
 {
-	struct sk_buff *skb = priv->rx_skb;
-	struct at76_rx_buffer *buf = (struct at76_rx_buffer *)skb->data;
-	struct ieee80211_hdr_3addr *i802_11_hdr =
-	    (struct ieee80211_hdr_3addr *)buf->packet;
-	/* seq_ctrl, fragment_number, sequence number of new packet */
-	u16 sctl = le16_to_cpu(i802_11_hdr->seq_ctl);
-	u16 fragnr = sctl & 0xf;
-	u16 seqnr = sctl >> 4;
-	u16 frame_ctl = le16_to_cpu(i802_11_hdr->frame_ctl);
-
-	/* Length including the IEEE802.11 header, but without the trailing
-	 * FCS and without the Atmel Rx header */
-	int length = le16_to_cpu(buf->wlength) - IEEE80211_FCS_LEN;
+	struct at76_priv *priv = container_of(work, struct at76_priv,
+					      dwork_hw_scan.work);
+	int ret;
 
-	/* where does the data payload start in skb->data ? */
-	u8 *data = i802_11_hdr->payload;
+	ret = at76_get_cmd_status(priv->udev, CMD_SCAN);
+	at76_dbg(DBG_MAC80211, "%s: CMD_SCAN status 0x%02x", __func__, ret);
 
-	/* length of payload, excl. the trailing FCS */
-	int data_len = length - IEEE80211_3ADDR_LEN;
+	/* FIXME: add maximum time for scan to complete */
 
-	int i;
-	struct rx_data_buf *bptr, *optr;
-	unsigned long oldest = ~0UL;
-
-	at76_dbg(DBG_RX_FRAGS,
-		 "%s: rx data frame_ctl %04x addr2 %s seq/frag %d/%d "
-		 "length %d data %d: %s ...", priv->netdev->name, frame_ctl,
-		 mac2str(i802_11_hdr->addr2), seqnr, fragnr, length, data_len,
-		 hex2str(data, 32));
-
-	at76_dbg(DBG_RX_FRAGS_SKB, "%s: incoming skb: head %p data %p "
-		 "tail %p end %p len %d", priv->netdev->name, skb->head,
-		 skb->data, skb_tail_pointer(skb), skb_end_pointer(skb),
-		 skb->len);
-
-	if (data_len < 0) {
-		/* make sure data starts in the buffer */
-		printk(KERN_INFO "%s: data frame too short\n",
-		       priv->netdev->name);
-		return NULL;
+	if (ret != CMD_STATUS_COMPLETE) {
+		queue_delayed_work(priv->hw->workqueue, &priv->dwork_hw_scan,
+				   SCAN_POLL_INTERVAL);
+		goto exit;
 	}
 
-	WARN_ON(length <= AT76_RX_HDRLEN);
-	if (length <= AT76_RX_HDRLEN)
-		return NULL;
+	ieee80211_scan_completed(priv->hw);
 
-	/* remove the at76_rx_buffer header - we don't need it anymore */
-	/* we need the IEEE802.11 header (for the addresses) if this packet
-	   is the first of a chain */
-	skb_pull(skb, AT76_RX_HDRLEN);
-
-	/* remove FCS at end */
-	skb_trim(skb, length);
-
-	at76_dbg(DBG_RX_FRAGS_SKB, "%s: trimmed skb: head %p data %p tail %p "
-		 "end %p len %d data %p data_len %d", priv->netdev->name,
-		 skb->head, skb->data, skb_tail_pointer(skb),
-		 skb_end_pointer(skb), skb->len, data, data_len);
-
-	if (fragnr == 0 && !(frame_ctl & IEEE80211_FCTL_MOREFRAGS)) {
-		/* unfragmented packet received */
-		/* Use a new skb for the next receive */
-		priv->rx_skb = NULL;
-		at76_dbg(DBG_RX_FRAGS, "%s: unfragmented", priv->netdev->name);
-		return skb;
+	if (is_valid_ether_addr(priv->bssid)) {
+		ieee80211_wake_queues(priv->hw);
+		at76_join(priv);
 	}
 
-	/* look if we've got a chain for the sender address.
-	   afterwards optr points to first free or the oldest entry,
-	   or, if i < NR_RX_DATA_BUF, bptr points to the entry for the
-	   sender address */
-	/* determining the oldest entry doesn't cope with jiffies wrapping
-	   but I don't care to delete a young entry at these rare moments ... */
-
-	bptr = priv->rx_data;
-	optr = NULL;
-	for (i = 0; i < NR_RX_DATA_BUF; i++, bptr++) {
-		if (!bptr->skb) {
-			optr = bptr;
-			oldest = 0UL;
-			continue;
-		}
+	ieee80211_wake_queues(priv->hw);
 
-		if (!compare_ether_addr(i802_11_hdr->addr2, bptr->sender))
-			break;
+exit:
+	return;
+}
 
-		if (!optr) {
-			optr = bptr;
-			oldest = bptr->last_rx;
-		} else if (bptr->last_rx < oldest)
-			optr = bptr;
-	}
+static int at76_hw_scan(struct ieee80211_hw *hw, u8 *ssid, size_t len)
+{
+	struct at76_priv *priv = hw->priv;
+	struct at76_req_scan scan;
+	int ret;
 
-	if (i < NR_RX_DATA_BUF) {
-
-		at76_dbg(DBG_RX_FRAGS, "%s: %d. cacheentry (seq/frag = %d/%d) "
-			 "matched sender addr",
-			 priv->netdev->name, i, bptr->seqnr, bptr->fragnr);
-
-		/* bptr points to an entry for the sender address */
-		if (bptr->seqnr == seqnr) {
-			int left;
-			/* the fragment has the current sequence number */
-			if (((bptr->fragnr + 1) & 0xf) != fragnr) {
-				/* wrong fragment number -> ignore it */
-				/* is & 0xf necessary above ??? */
-				at76_dbg(DBG_RX_FRAGS,
-					 "%s: frag nr mismatch: %d + 1 != %d",
-					 priv->netdev->name, bptr->fragnr,
-					 fragnr);
-				return NULL;
-			}
-			bptr->last_rx = jiffies;
-			/* the next following fragment number ->
-			   add the data at the end */
-
-			/* for test only ??? */
-			left = skb_tailroom(bptr->skb);
-			if (left < data_len)
-				printk(KERN_INFO
-				       "%s: only %d byte free (need %d)\n",
-				       priv->netdev->name, left, data_len);
-			else
-				memcpy(skb_put(bptr->skb, data_len), data,
-				       data_len);
-
-			bptr->fragnr = fragnr;
-			if (frame_ctl & IEEE80211_FCTL_MOREFRAGS)
-				return NULL;
-
-			/* this was the last fragment - send it */
-			skb = bptr->skb;
-			bptr->skb = NULL;	/* free the entry */
-			at76_dbg(DBG_RX_FRAGS, "%s: last frag of seq %d",
-				 priv->netdev->name, seqnr);
-			return skb;
-		}
+	at76_dbg(DBG_MAC80211, "%s():", __func__);
+	at76_dbg_dump(DBG_MAC80211, ssid, len, "ssid %zd bytes:", len);
 
-		/* got another sequence number */
-		if (fragnr == 0) {
-			/* it's the start of a new chain - replace the
-			   old one by this */
-			/* bptr->sender has the correct value already */
-			at76_dbg(DBG_RX_FRAGS,
-				 "%s: start of new seq %d, removing old seq %d",
-				 priv->netdev->name, seqnr, bptr->seqnr);
-			bptr->seqnr = seqnr;
-			bptr->fragnr = 0;
-			bptr->last_rx = jiffies;
-			/* swap bptr->skb and priv->rx_skb */
-			skb = bptr->skb;
-			bptr->skb = priv->rx_skb;
-			priv->rx_skb = skb;
-		} else {
-			/* it from the middle of a new chain ->
-			   delete the old entry and skip the new one */
-			at76_dbg(DBG_RX_FRAGS,
-				 "%s: middle of new seq %d (%d) "
-				 "removing old seq %d",
-				 priv->netdev->name, seqnr, fragnr,
-				 bptr->seqnr);
-			dev_kfree_skb(bptr->skb);
-			bptr->skb = NULL;
-		}
-		return NULL;
-	}
+	mutex_lock(&priv->mtx);
 
-	/* if we didn't find a chain for the sender address, optr
-	   points either to the first free or the oldest entry */
+	ieee80211_stop_queues(hw);
 
-	if (fragnr != 0) {
-		/* this is not the begin of a fragment chain ... */
-		at76_dbg(DBG_RX_FRAGS,
-			 "%s: no chain for non-first fragment (%d)",
-			 priv->netdev->name, fragnr);
-		return NULL;
+	memset(&scan, 0, sizeof(struct at76_req_scan));
+	memset(scan.bssid, 0xFF, ETH_ALEN);
+	scan.scan_type = SCAN_TYPE_ACTIVE;
+	if (priv->essid_size > 0) {
+		memcpy(scan.essid, ssid, len);
+		scan.essid_size = len;
 	}
+	scan.min_channel_time = cpu_to_le16(priv->scan_min_time);
+	scan.max_channel_time = cpu_to_le16(priv->scan_max_time);
+	scan.probe_delay = cpu_to_le16(priv->scan_min_time * 1000);
+	scan.international_scan = 0;
 
-	BUG_ON(!optr);
-	if (optr->skb) {
-		/* swap the skb's */
-		skb = optr->skb;
-		optr->skb = priv->rx_skb;
-		priv->rx_skb = skb;
+	at76_dbg(DBG_MAC80211, "%s: sending CMD_SCAN", __func__);
+	ret = at76_set_card_command(priv->udev, CMD_SCAN, &scan, sizeof(scan));
 
-		at76_dbg(DBG_RX_FRAGS,
-			 "%s: free old contents: sender %s seq/frag %d/%d",
-			 priv->netdev->name, mac2str(optr->sender),
-			 optr->seqnr, optr->fragnr);
+	if (ret < 0) {
+		err("CMD_SCAN failed: %d", ret);
+		goto exit;
+	}
 
-	} else {
-		/* take the skb from priv->rx_skb */
-		optr->skb = priv->rx_skb;
-		/* let at76_submit_rx_urb() allocate a new skb */
-		priv->rx_skb = NULL;
+	queue_delayed_work(priv->hw->workqueue, &priv->dwork_hw_scan,
+			   SCAN_POLL_INTERVAL);
 
-		at76_dbg(DBG_RX_FRAGS, "%s: use a free entry",
-			 priv->netdev->name);
-	}
-	memcpy(optr->sender, i802_11_hdr->addr2, ETH_ALEN);
-	optr->seqnr = seqnr;
-	optr->fragnr = 0;
-	optr->last_rx = jiffies;
+exit:
+	mutex_unlock(&priv->mtx);
 
-	return NULL;
+	return 0;
 }
 
-/* Rx interrupt: we expect the complete data buffer in priv->rx_skb */
-static void at76_rx_data(struct at76_priv *priv)
+static int at76_config(struct ieee80211_hw *hw, u32 changed)
 {
-	struct net_device *netdev = priv->netdev;
-	struct net_device_stats *stats = &priv->stats;
-	struct sk_buff *skb = priv->rx_skb;
-	struct at76_rx_buffer *buf = (struct at76_rx_buffer *)skb->data;
-	struct ieee80211_hdr_3addr *i802_11_hdr;
-	int length = le16_to_cpu(buf->wlength);
-
-	at76_dbg(DBG_RX_DATA, "%s received data packet: %s", netdev->name,
-		 hex2str(skb->data, AT76_RX_HDRLEN));
+	struct at76_priv *priv = hw->priv;
+	struct ieee80211_conf *conf = &hw->conf;
 
-	at76_dbg(DBG_RX_DATA_CONTENT, "rx packet: %s",
-		 hex2str(skb->data + AT76_RX_HDRLEN, length));
+	at76_dbg(DBG_MAC80211, "%s(): channel %d radio %d",
+		 __func__, conf->channel->hw_value, conf->radio_enabled);
+	at76_dbg_dump(DBG_MAC80211, priv->essid, priv->essid_size, "ssid:");
+	at76_dbg_dump(DBG_MAC80211, priv->bssid, ETH_ALEN, "bssid:");
 
-	skb = at76_check_for_rx_frags(priv);
-	if (!skb)
-		return;
-
-	/* Atmel header and the FCS are already removed */
-	i802_11_hdr = (struct ieee80211_hdr_3addr *)skb->data;
-
-	skb->dev = netdev;
-	skb->ip_summed = CHECKSUM_NONE;	/* TODO: should check CRC */
+	mutex_lock(&priv->mtx);
 
-	if (is_broadcast_ether_addr(i802_11_hdr->addr1)) {
-		if (!compare_ether_addr(i802_11_hdr->addr1, netdev->broadcast))
-			skb->pkt_type = PACKET_BROADCAST;
-		else
-			skb->pkt_type = PACKET_MULTICAST;
-	} else if (compare_ether_addr(i802_11_hdr->addr1, netdev->dev_addr))
-		skb->pkt_type = PACKET_OTHERHOST;
+	priv->channel = conf->channel->hw_value;
 
-	at76_ieee80211_to_eth(skb, priv->iw_mode);
+	if (is_valid_ether_addr(priv->bssid)) {
+		at76_join(priv);
+		ieee80211_wake_queues(priv->hw);
+	} else {
+		ieee80211_stop_queues(priv->hw);
+		at76_start_monitor(priv);
+	};
 
-	netdev->last_rx = jiffies;
-	netif_rx(skb);
-	stats->rx_packets++;
-	stats->rx_bytes += length;
+	mutex_unlock(&priv->mtx);
 
-	return;
+	return 0;
 }
 
-static void at76_rx_monitor_mode(struct at76_priv *priv)
+static int at76_config_interface(struct ieee80211_hw *hw,
+				 struct ieee80211_vif *vif,
+				 struct ieee80211_if_conf *conf)
 {
-	struct at76_rx_radiotap *rt;
-	u8 *payload;
-	int skblen;
-	struct net_device *netdev = priv->netdev;
-	struct at76_rx_buffer *buf =
-	    (struct at76_rx_buffer *)priv->rx_skb->data;
-	/* length including the IEEE802.11 header and the trailing FCS,
-	   but not at76_rx_buffer */
-	int length = le16_to_cpu(buf->wlength);
-	struct sk_buff *skb = priv->rx_skb;
-	struct net_device_stats *stats = &priv->stats;
+	struct at76_priv *priv = hw->priv;
 
-	if (length < IEEE80211_FCS_LEN) {
-		/* buffer contains no data */
-		at76_dbg(DBG_MONITOR_MODE,
-			 "%s: MONITOR MODE: rx skb without data",
-			 priv->netdev->name);
-		return;
-	}
+	at76_dbg_dump(DBG_MAC80211, conf->bssid, ETH_ALEN, "bssid:");
 
-	skblen = sizeof(struct at76_rx_radiotap) + length;
+	mutex_lock(&priv->mtx);
 
-	skb = dev_alloc_skb(skblen);
-	if (!skb) {
-		printk(KERN_ERR "%s: MONITOR MODE: dev_alloc_skb for radiotap "
-		       "header returned NULL\n", priv->netdev->name);
-		return;
-	}
+	memcpy(priv->bssid, conf->bssid, ETH_ALEN);
+//	memcpy(priv->essid, conf->ssid, conf->ssid_len);
+//	priv->essid_size = conf->ssid_len;
 
-	skb_put(skb, skblen);
-
-	rt = (struct at76_rx_radiotap *)skb->data;
-	payload = skb->data + sizeof(struct at76_rx_radiotap);
-
-	rt->rt_hdr.it_version = 0;
-	rt->rt_hdr.it_pad = 0;
-	rt->rt_hdr.it_len = cpu_to_le16(sizeof(struct at76_rx_radiotap));
-	rt->rt_hdr.it_present = cpu_to_le32(AT76_RX_RADIOTAP_PRESENT);
-
-	rt->rt_tsft = cpu_to_le64(le32_to_cpu(buf->rx_time));
-	rt->rt_rate = hw_rates[buf->rx_rate] & (~0x80);
-	rt->rt_signal = buf->rssi;
-	rt->rt_noise = buf->noise_level;
-	rt->rt_flags = IEEE80211_RADIOTAP_F_FCS;
-	if (buf->fragmentation)
-		rt->rt_flags |= IEEE80211_RADIOTAP_F_FRAG;
-
-	memcpy(payload, buf->packet, length);
-	skb->dev = netdev;
-	skb->ip_summed = CHECKSUM_NONE;
-	skb_reset_mac_header(skb);
-	skb->pkt_type = PACKET_OTHERHOST;
-	skb->protocol = htons(ETH_P_802_2);
-
-	netdev->last_rx = jiffies;
-	netif_rx(skb);
-	stats->rx_packets++;
-	stats->rx_bytes += length;
+	if (is_valid_ether_addr(priv->bssid)) {
+		/* mac80211 is joining a bss */
+		ieee80211_wake_queues(priv->hw);
+		at76_join(priv);
+	} else
+		ieee80211_stop_queues(priv->hw);
+
+	mutex_unlock(&priv->mtx);
+
+	return 0;
 }
 
-/* Check if we spy on the sender address in buf and update stats */
-static void at76_iwspy_update(struct at76_priv *priv,
-			      struct at76_rx_buffer *buf)
+/* must be atomic */
+static void at76_configure_filter(struct ieee80211_hw *hw,
+				  unsigned int changed_flags,
+				  unsigned int *total_flags, int mc_count,
+				  struct dev_addr_list *mc_list)
 {
-	struct ieee80211_hdr_3addr *hdr =
-	    (struct ieee80211_hdr_3addr *)buf->packet;
-	struct iw_quality qual;
+	struct at76_priv *priv = hw->priv;
+	int flags;
 
-	/* We can only set the level here */
-	qual.updated = IW_QUAL_QUAL_INVALID | IW_QUAL_NOISE_INVALID;
-	qual.level = 0;
-	qual.noise = 0;
-	at76_calc_level(priv, buf, &qual);
+	at76_dbg(DBG_MAC80211, "%s(): changed_flags=0x%08x "
+		 "total_flags=0x%08x mc_count=%d",
+		 __func__, changed_flags, *total_flags, mc_count);
 
-	spin_lock_bh(&priv->spy_spinlock);
+	flags = changed_flags & AT76_SUPPORTED_FILTERS;
+	*total_flags = AT76_SUPPORTED_FILTERS;
 
-	if (priv->spy_data.spy_number > 0)
-		wireless_spy_update(priv->netdev, hdr->addr2, &qual);
+	/* FIXME: access to priv->promisc should be protected with
+	 * priv->mtx, but it's impossible because this function needs to be
+	 * atomic */
 
-	spin_unlock_bh(&priv->spy_spinlock);
+	if (flags && !priv->promisc) {
+		/* mac80211 wants us to enable promiscuous mode */
+		priv->promisc = 1;
+	} else if (!flags && priv->promisc) {
+		/* we need to disable promiscuous mode */
+		priv->promisc = 0;
+	} else
+		return;
+
+	queue_work(hw->workqueue, &priv->work_set_promisc);
 }
 
-static void at76_rx_tasklet(unsigned long param)
+static int at76_set_key_oldfw(struct ieee80211_hw *hw, enum set_key_cmd cmd,
+			const u8 *local_address, const u8 *address,
+			struct ieee80211_key_conf *key)
 {
-	struct urb *urb = (struct urb *)param;
-	struct at76_priv *priv = urb->context;
-	struct net_device *netdev = priv->netdev;
-	struct at76_rx_buffer *buf;
-	struct ieee80211_hdr_3addr *i802_11_hdr;
-	u16 frame_ctl;
+	struct at76_priv *priv = hw->priv;
 
-	if (priv->device_unplugged) {
-		at76_dbg(DBG_DEVSTART, "device unplugged");
-		if (urb)
-			at76_dbg(DBG_DEVSTART, "urb status %d", urb->status);
-		return;
-	}
-
-	if (!priv->rx_skb || !netdev || !priv->rx_skb->data)
-		return;
+	int i;
 
-	buf = (struct at76_rx_buffer *)priv->rx_skb->data;
+	at76_dbg(DBG_MAC80211, "%s(): cmd %d key->alg %d key->keyidx %d "
+		 "key->keylen %d",
+		 __func__, cmd, key->alg, key->keyidx, key->keylen);
 
-	i802_11_hdr = (struct ieee80211_hdr_3addr *)buf->packet;
+	if (key->alg != ALG_WEP)
+		return -EOPNOTSUPP;
 
-	frame_ctl = le16_to_cpu(i802_11_hdr->frame_ctl);
+	key->hw_key_idx = key->keyidx;
 
-	if (urb->status != 0) {
-		if (urb->status != -ENOENT && urb->status != -ECONNRESET)
-			at76_dbg(DBG_URB,
-				 "%s %s: - nonzero Rx bulk status received: %d",
-				 __func__, netdev->name, urb->status);
-		return;
-	}
+	mutex_lock(&priv->mtx);
 
-	at76_dbg(DBG_RX_ATMEL_HDR,
-		 "%s: rx frame: rate %d rssi %d noise %d link %d %s",
-		 priv->netdev->name, buf->rx_rate, buf->rssi, buf->noise_level,
-		 buf->link_quality, hex2str(i802_11_hdr, 48));
-	if (priv->iw_mode == IW_MODE_MONITOR) {
-		at76_rx_monitor_mode(priv);
-		goto exit;
-	}
+	switch (cmd) {
+	case SET_KEY:
+		memcpy(priv->wep_keys[key->keyidx], key->key, key->keylen);
+		priv->wep_keys_len[key->keyidx] = key->keylen;
 
-	/* there is a new bssid around, accept it: */
-	if (buf->newbss && priv->iw_mode == IW_MODE_ADHOC) {
-		at76_dbg(DBG_PROGRESS, "%s: rx newbss", netdev->name);
-		schedule_work(&priv->work_new_bss);
-	}
+		/* FIXME: find out how to do this properly */
+		priv->wep_key_id = key->keyidx;
 
-	switch (frame_ctl & IEEE80211_FCTL_FTYPE) {
-	case IEEE80211_FTYPE_DATA:
-		at76_rx_data(priv);
 		break;
+	case DISABLE_KEY:
+	default:
+		priv->wep_keys_len[key->keyidx] = 0;
+		break;
+	}
 
-	case IEEE80211_FTYPE_MGMT:
-		/* jal: TODO: find out if we can update iwspy also on
-		   other frames than management (might depend on the
-		   radio chip / firmware version !) */
+	priv->wep_enabled = 0;
 
-		at76_iwspy_update(priv, buf);
+	for (i = 0; i < WEP_KEYS; i++) {
+		if (priv->wep_keys_len[i] != 0)
+			priv->wep_enabled = 1;
+	}
 
-		at76_rx_mgmt(priv, buf);
-		break;
+	at76_startup_device(priv);
 
-	case IEEE80211_FTYPE_CTL:
-		at76_dbg(DBG_RX_CTRL, "%s: ignored ctrl frame: %04x",
-			 priv->netdev->name, frame_ctl);
-		break;
+	mutex_unlock(&priv->mtx);
 
-	default:
-		printk(KERN_DEBUG "%s: ignoring frame with framectl 0x%04x\n",
-		       priv->netdev->name, frame_ctl);
-	}
-exit:
-	at76_submit_rx_urb(priv);
+	return 0;
 }
 
-/* Load firmware into kernel memory and parse it */
-static struct fwentry *at76_load_firmware(struct usb_device *udev,
-					  enum board_type board_type)
+static int at76_set_key_newfw(struct ieee80211_hw *hw, enum set_key_cmd cmd,
+			const u8 *local_address, const u8 *address,
+			struct ieee80211_key_conf *key)
 {
-	int ret;
-	char *str;
-	struct at76_fw_header *fwh;
-	struct fwentry *fwe = &firmwares[board_type];
+	struct at76_priv *priv = hw->priv;
+	int ret = -EOPNOTSUPP;
 
-	mutex_lock(&fw_mutex);
+	at76_dbg(DBG_MAC80211, "%s(): cmd %d key->alg %d key->keyidx %d "
+		 "key->keylen %d",
+		 __func__, cmd, key->alg, key->keyidx, key->keylen);
 
-	if (fwe->loaded) {
-		at76_dbg(DBG_FW, "re-using previously loaded fw");
-		goto exit;
-	}
+	mutex_lock(&priv->mtx);
 
-	at76_dbg(DBG_FW, "downloading firmware %s", fwe->fwname);
-	ret = request_firmware(&fwe->fw, fwe->fwname, &udev->dev);
-	if (ret < 0) {
-		dev_printk(KERN_ERR, &udev->dev, "firmware %s not found!\n",
-			   fwe->fwname);
-		dev_printk(KERN_ERR, &udev->dev,
-			   "you may need to download the firmware from "
-			   "http://developer.berlios.de/projects/at76c503a/");
-		goto exit;
-	}
+	priv->mib_buf.type = MIB_MAC_ENCRYPTION;
 
-	at76_dbg(DBG_FW, "got it.");
-	fwh = (struct at76_fw_header *)(fwe->fw->data);
+	if (cmd == DISABLE_KEY) {
+		priv->mib_buf.size = CIPHER_KEY_LEN;
+		priv->mib_buf.index = offsetof(struct mib_mac_encryption,
+				cipher_default_keyvalue[key->keyidx]);
+		memset(priv->mib_buf.data.data, 0, CIPHER_KEY_LEN);
+		if (at76_set_mib(priv, &priv->mib_buf) != CMD_STATUS_COMPLETE)
+			ret = -EOPNOTSUPP; /* -EIO would be probably better */
+		else {
 
-	if (fwe->fw->size <= sizeof(*fwh)) {
-		dev_printk(KERN_ERR, &udev->dev,
-			   "firmware is too short (0x%zx)\n", fwe->fw->size);
-		goto exit;
-	}
+			priv->keys[key->keyidx].cipher = CIPHER_NONE;
+			priv->keys[key->keyidx].keylen = 0;
+		};
+		if (priv->default_group_key == key->keyidx)
+			priv->default_group_key = 0xff;
 
-	/* CRC currently not checked */
-	fwe->board_type = le32_to_cpu(fwh->board_type);
-	if (fwe->board_type != board_type) {
-		dev_printk(KERN_ERR, &udev->dev,
-			   "board type mismatch, requested %u, got %u\n",
-			   board_type, fwe->board_type);
+		if (priv->default_pairwise_key == key->keyidx)
+			priv->default_pairwise_key = 0xff;
+		/* If default pairwise key is removed, fall back to
+		 * group key? */
+		ret = 0;
 		goto exit;
-	}
+	};
 
-	fwe->fw_version.major = fwh->major;
-	fwe->fw_version.minor = fwh->minor;
-	fwe->fw_version.patch = fwh->patch;
-	fwe->fw_version.build = fwh->build;
+	if (cmd == SET_KEY) {
+		/* store key into MIB */
+		priv->mib_buf.size = CIPHER_KEY_LEN;
+		priv->mib_buf.index = offsetof(struct mib_mac_encryption,
+				cipher_default_keyvalue[key->keyidx]);
+		memset(priv->mib_buf.data.data, 0, CIPHER_KEY_LEN);
+		memcpy(priv->mib_buf.data.data, key->key, key->keylen);
+
+		switch (key->alg) {
+		case ALG_WEP:
+			if (key->keylen == 5) {
+				priv->keys[key->keyidx].cipher =
+					CIPHER_WEP64;
+				priv->keys[key->keyidx].keylen = 8;
+			} else if (key->keylen == 13) {
+				priv->keys[key->keyidx].cipher =
+					CIPHER_WEP128;
+				/* Firmware needs this */
+				priv->keys[key->keyidx].keylen = 8;
+			} else {
+				ret = -EOPNOTSUPP;
+				goto exit;
+			};
+			break;
+		case ALG_TKIP:
+			key->flags |= IEEE80211_KEY_FLAG_GENERATE_MMIC;
+			priv->keys[key->keyidx].cipher = CIPHER_TKIP;
+			priv->keys[key->keyidx].keylen = 12;
+			break;
 
-	str = (char *)fwh + le32_to_cpu(fwh->str_offset);
-	fwe->intfw = (u8 *)fwh + le32_to_cpu(fwh->int_fw_offset);
-	fwe->intfw_size = le32_to_cpu(fwh->int_fw_len);
-	fwe->extfw = (u8 *)fwh + le32_to_cpu(fwh->ext_fw_offset);
-	fwe->extfw_size = le32_to_cpu(fwh->ext_fw_len);
+		case ALG_CCMP:
+			if (!at76_is_505a(priv->board_type)) {
+				ret = -EOPNOTSUPP;
+				goto exit;
+			};
+			key->flags |= IEEE80211_KEY_FLAG_GENERATE_MMIC;
+			priv->keys[key->keyidx].cipher = CIPHER_CCMP;
+			priv->keys[key->keyidx].keylen = 16;
+			break;
 
-	fwe->loaded = 1;
+		default:
+			ret = -EOPNOTSUPP;
+			goto exit;
+		};
+
+		priv->mib_buf.data.data[38] = priv->keys[key->keyidx].cipher;
+		priv->mib_buf.data.data[39] = 1; /* Taken from atmelwlandriver,
+						    not documented */
+
+		if (is_valid_ether_addr(address))
+			/* Pairwise key */
+			priv->mib_buf.data.data[39] |= (KEY_PAIRWISE | KEY_TX);
+		else if (is_broadcast_ether_addr(address))
+			/* Group key */
+			priv->mib_buf.data.data[39] |= (KEY_TX);
+		else	/* Key used only for transmission ??? */
+			priv->mib_buf.data.data[39] |= (KEY_TX);
+
+		if (at76_set_mib(priv, &priv->mib_buf) !=
+				CMD_STATUS_COMPLETE) {
+			ret = -EOPNOTSUPP; /* -EIO would be probably better */
+			goto exit;
+		};
 
-	dev_printk(KERN_DEBUG, &udev->dev,
-		   "using firmware %s (version %d.%d.%d-%d)\n",
-		   fwe->fwname, fwh->major, fwh->minor, fwh->patch, fwh->build);
+		if ((key->alg == ALG_TKIP) || (key->alg == ALG_CCMP))
+			at76_reset_rsc(priv);
 
-	at76_dbg(DBG_DEVSTART, "board %u, int %d:%d, ext %d:%d", board_type,
-		 le32_to_cpu(fwh->int_fw_offset), le32_to_cpu(fwh->int_fw_len),
-		 le32_to_cpu(fwh->ext_fw_offset), le32_to_cpu(fwh->ext_fw_len));
-	at76_dbg(DBG_DEVSTART, "firmware id %s", str);
+		key->hw_key_idx = key->keyidx;
+
+		/* Set up default keys */
+		if (is_broadcast_ether_addr(address))
+			priv->default_group_key = key->keyidx;
+		if (is_valid_ether_addr(address))
+			priv->default_pairwise_key = key->keyidx;
 
+		/* Set up encryption MIBs */
+
+		/* first block of settings */
+		priv->mib_buf.size = 3;
+		priv->mib_buf.index = offsetof(struct mib_mac_encryption,
+				privacy_invoked);
+		priv->mib_buf.data.data[0] = 1;	/* privacy_invoked */
+		priv->mib_buf.data.data[1] = priv->default_pairwise_key;
+		priv->mib_buf.data.data[2] = priv->default_group_key;
+
+		ret = at76_set_mib(priv, &priv->mib_buf);
+		if (ret != CMD_STATUS_COMPLETE)
+			goto exit;
+
+		/* second block of settings */
+		priv->mib_buf.size = 3;
+		priv->mib_buf.index = offsetof(struct mib_mac_encryption,
+				exclude_unencrypted);
+		priv->mib_buf.data.data[0] = 1;	/* exclude_unencrypted */
+		priv->mib_buf.data.data[1] = 0;	/* wep_encryption_type */
+		priv->mib_buf.data.data[2] = 0;	/* ckip_key_permutation */
+
+		ret = at76_set_mib(priv, &priv->mib_buf);
+		if (ret != CMD_STATUS_COMPLETE)
+			goto exit;
+		ret = 0;
+	};
 exit:
-	mutex_unlock(&fw_mutex);
+	at76_dump_mib_mac_encryption(priv);
+	mutex_unlock(&priv->mtx);
+	return ret;
+}
 
-	if (fwe->loaded)
-		return fwe;
+static int at76_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
+			const u8 *local_address, const u8 *address,
+			struct ieee80211_key_conf *key)
+{
+	struct at76_priv *priv = hw->priv;
+
+	at76_dbg(DBG_MAC80211, "%s(): cmd %d key->alg %d key->keyidx %d "
+		 "key->keylen %d",
+		 __func__, cmd, key->alg, key->keyidx, key->keylen);
+
+	if (FIRMWARE_IS_WPA(priv->fw_version))
+		return at76_set_key_newfw(hw, cmd, local_address, address, key);
 	else
-		return NULL;
+		return at76_set_key_oldfw(hw, cmd, local_address, address, key);
+
 }
 
+static const struct ieee80211_ops at76_ops = {
+	.tx = at76_mac80211_tx,
+	.add_interface = at76_add_interface,
+	.remove_interface = at76_remove_interface,
+	.config = at76_config,
+	.config_interface = at76_config_interface,
+	.configure_filter = at76_configure_filter,
+	.start = at76_mac80211_start,
+	.stop = at76_mac80211_stop,
+	.hw_scan = at76_hw_scan,
+	.set_key = at76_set_key,
+};
+
 /* Allocate network device and initialize private data */
 static struct at76_priv *at76_alloc_new_device(struct usb_device *udev)
 {
-	struct net_device *netdev;
+	struct ieee80211_hw *hw;
 	struct at76_priv *priv;
-	int i;
 
-	/* allocate memory for our device state and initialize it */
-	netdev = alloc_etherdev(sizeof(struct at76_priv));
-	if (!netdev) {
-		dev_printk(KERN_ERR, &udev->dev, "out of memory\n");
+	hw = ieee80211_alloc_hw(sizeof(struct at76_priv), &at76_ops);
+	if (!hw) {
+		printk(KERN_ERR DRIVER_NAME ": could not register"
+		       " ieee80211_hw\n");
 		return NULL;
 	}
 
-	priv = netdev_priv(netdev);
+	priv = hw->priv;
+	priv->hw = hw;
 
 	priv->udev = udev;
-	priv->netdev = netdev;
 
 	mutex_init(&priv->mtx);
-	INIT_WORK(&priv->work_assoc_done, at76_work_assoc_done);
-	INIT_WORK(&priv->work_join, at76_work_join);
-	INIT_WORK(&priv->work_new_bss, at76_work_new_bss);
-	INIT_WORK(&priv->work_start_scan, at76_work_start_scan);
 	INIT_WORK(&priv->work_set_promisc, at76_work_set_promisc);
 	INIT_WORK(&priv->work_submit_rx, at76_work_submit_rx);
-	INIT_DELAYED_WORK(&priv->dwork_restart, at76_dwork_restart);
-	INIT_DELAYED_WORK(&priv->dwork_get_scan, at76_dwork_get_scan);
-	INIT_DELAYED_WORK(&priv->dwork_beacon, at76_dwork_beacon);
-	INIT_DELAYED_WORK(&priv->dwork_auth, at76_dwork_auth);
-	INIT_DELAYED_WORK(&priv->dwork_assoc, at76_dwork_assoc);
-
-	spin_lock_init(&priv->mgmt_spinlock);
-	priv->next_mgmt_bulk = NULL;
-	priv->mac_state = MAC_INIT;
-
-	/* initialize empty BSS list */
-	priv->curr_bss = NULL;
-	INIT_LIST_HEAD(&priv->bss_list);
-	spin_lock_init(&priv->bss_list_spinlock);
-
-	init_timer(&priv->bss_list_timer);
-	priv->bss_list_timer.data = (unsigned long)priv;
-	priv->bss_list_timer.function = at76_bss_list_timeout;
-
-	spin_lock_init(&priv->spy_spinlock);
-
-	/* mark all rx data entries as unused */
-	for (i = 0; i < NR_RX_DATA_BUF; i++)
-		priv->rx_data[i].skb = NULL;
+	INIT_DELAYED_WORK(&priv->dwork_hw_scan, at76_dwork_hw_scan);
 
 	priv->rx_tasklet.func = at76_rx_tasklet;
 	priv->rx_tasklet.data = 0;
@@ -5183,6 +2371,9 @@ static struct at76_priv *at76_alloc_new_device(struct usb_device *udev)
 	priv->pm_mode = AT76_PM_OFF;
 	priv->pm_period = 0;
 
+	/* unit us */
+	priv->hw->channel_change_time = 100000;
+
 	return priv;
 }
 
@@ -5245,11 +2436,42 @@ static int at76_alloc_urbs(struct at76_priv *priv,
 	return 0;
 }
 
+static struct ieee80211_rate at76_rates[] = {
+	{ .bitrate = 10, .hw_value = TX_RATE_1MBIT, },
+	{ .bitrate = 20, .hw_value = TX_RATE_2MBIT, },
+	{ .bitrate = 55, .hw_value = TX_RATE_5_5MBIT, },
+	{ .bitrate = 110, .hw_value = TX_RATE_11MBIT, },
+};
+
+static struct ieee80211_channel at76_channels[] = {
+	{ .center_freq = 2412, .hw_value = 1 },
+	{ .center_freq = 2417, .hw_value = 2 },
+	{ .center_freq = 2422, .hw_value = 3 },
+	{ .center_freq = 2427, .hw_value = 4 },
+	{ .center_freq = 2432, .hw_value = 5 },
+	{ .center_freq = 2437, .hw_value = 6 },
+	{ .center_freq = 2442, .hw_value = 7 },
+	{ .center_freq = 2447, .hw_value = 8 },
+	{ .center_freq = 2452, .hw_value = 9 },
+	{ .center_freq = 2457, .hw_value = 10 },
+	{ .center_freq = 2462, .hw_value = 11 },
+	{ .center_freq = 2467, .hw_value = 12 },
+	{ .center_freq = 2472, .hw_value = 13 },
+	{ .center_freq = 2484, .hw_value = 14 }
+};
+
+static struct ieee80211_supported_band at76_supported_band = {
+	.channels = at76_channels,
+	.n_channels = ARRAY_SIZE(at76_channels),
+	.bitrates = at76_rates,
+	.n_bitrates = ARRAY_SIZE(at76_rates),
+};
+
 /* Register network device and initialize the hardware */
 static int at76_init_new_device(struct at76_priv *priv,
 				struct usb_interface *interface)
 {
-	struct net_device *netdev = priv->netdev;
+	struct device *dev = &interface->dev;
 	int ret;
 
 	/* set up the endpoint information */
@@ -5265,14 +2487,11 @@ static int at76_init_new_device(struct at76_priv *priv,
 	/* MAC address */
 	ret = at76_get_hw_config(priv);
 	if (ret < 0) {
-		dev_printk(KERN_ERR, &interface->dev,
-			   "cannot get MAC address\n");
+		dev_err(dev, "cannot get MAC address\n");
 		goto exit;
 	}
 
 	priv->domain = at76_get_reg_domain(priv->regulatory_domain);
-	/* init. netdev->dev_addr */
-	memcpy(netdev->dev_addr, priv->mac_addr, ETH_ALEN);
 
 	priv->channel = DEF_CHANNEL;
 	priv->iw_mode = IW_MODE_INFRA;
@@ -5282,47 +2501,54 @@ static int at76_init_new_device(struct at76_priv *priv,
 	priv->txrate = TX_RATE_AUTO;
 	priv->preamble_type = PREAMBLE_TYPE_LONG;
 	priv->beacon_period = 100;
-	priv->beacons_last_qual = jiffies;
 	priv->auth_mode = WLAN_AUTH_OPEN;
 	priv->scan_min_time = DEF_SCAN_MIN_TIME;
 	priv->scan_max_time = DEF_SCAN_MAX_TIME;
 	priv->scan_mode = SCAN_TYPE_ACTIVE;
+	priv->default_pairwise_key = 0xff;
+	priv->default_group_key = 0xff;
+
+	/* mac80211 initialisation */
+	priv->hw->wiphy->bands[IEEE80211_BAND_2GHZ] = &at76_supported_band;
 
-	netdev->flags &= ~IFF_MULTICAST;	/* not yet or never */
-	netdev->open = at76_open;
-	netdev->stop = at76_stop;
-	netdev->get_stats = at76_get_stats;
-	netdev->ethtool_ops = &at76_ethtool_ops;
-
-	/* Add pointers to enable iwspy support. */
-	priv->wireless_data.spy_data = &priv->spy_data;
-	netdev->wireless_data = &priv->wireless_data;
-
-	netdev->hard_start_xmit = at76_tx;
-	netdev->tx_timeout = at76_tx_timeout;
-	netdev->watchdog_timeo = 2 * HZ;
-	netdev->wireless_handlers = &at76_handler_def;
-	netdev->set_multicast_list = at76_set_multicast;
-	netdev->set_mac_address = at76_set_mac_address;
-	dev_alloc_name(netdev, "wlan%d");
-
-	ret = register_netdev(priv->netdev);
+	if (FIRMWARE_IS_WPA(priv->fw_version) &&
+		(at76_is_503rfmd(priv->board_type) ||
+		 at76_is_505(priv->board_type)))
+		priv->hw->flags = IEEE80211_HW_SIGNAL_UNSPEC;
+	else
+		priv->hw->flags = IEEE80211_HW_RX_INCLUDES_FCS |
+				  IEEE80211_HW_SIGNAL_UNSPEC;
+
+	priv->hw->wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION);
+
+	SET_IEEE80211_DEV(priv->hw, &interface->dev);
+	SET_IEEE80211_PERM_ADDR(priv->hw, priv->mac_addr);
+
+	ret = ieee80211_register_hw(priv->hw);
 	if (ret) {
-		dev_printk(KERN_ERR, &interface->dev,
-			   "cannot register netdevice (status %d)!\n", ret);
+		dev_err(dev, "cannot register mac80211 hw (status %d)!\n", ret);
 		goto exit;
 	}
-	priv->netdev_registered = 1;
 
-	printk(KERN_INFO "%s: USB %s, MAC %s, firmware %d.%d.%d-%d\n",
-	       netdev->name, interface->dev.bus_id, mac2str(priv->mac_addr),
-	       priv->fw_version.major, priv->fw_version.minor,
-	       priv->fw_version.patch, priv->fw_version.build);
-	printk(KERN_INFO "%s: regulatory domain 0x%02x: %s\n", netdev->name,
-	       priv->regulatory_domain, priv->domain->name);
+	priv->mac80211_registered = 1;
 
-	/* we let this timer run the whole time this driver instance lives */
-	mod_timer(&priv->bss_list_timer, jiffies + BSS_LIST_TIMEOUT);
+	dev_info(dev, "%s: USB %s, MAC %s, firmware %d.%d.%d-%d\n",
+		 wiphy_name(priv->hw->wiphy),
+		 dev_name(&interface->dev), mac2str(priv->mac_addr),
+		 priv->fw_version.major, priv->fw_version.minor,
+		 priv->fw_version.patch, priv->fw_version.build);
+	dev_info(dev, "%s: regulatory domain 0x%02x: %s\n",
+		 wiphy_name(priv->hw->wiphy),
+		 priv->regulatory_domain, priv->domain->name);
+	dev_info(dev, "%s: WPA support: ", wiphy_name(priv->hw->wiphy));
+	if (!FIRMWARE_IS_WPA(priv->fw_version))
+		printk("none\n");
+	else {
+		if (!at76_is_505a(priv->board_type))
+			printk("TKIP\n");
+		else
+			printk("TKIP, AES/CCMP\n");
+	};
 
 exit:
 	return ret;
@@ -5330,15 +2556,13 @@ exit:
 
 static void at76_delete_device(struct at76_priv *priv)
 {
-	int i;
-
 	at76_dbg(DBG_PROC_ENTRY, "%s: ENTER", __func__);
 
 	/* The device is gone, don't bother turning it off */
 	priv->device_unplugged = 1;
 
-	if (priv->netdev_registered)
-		unregister_netdev(priv->netdev);
+	if (priv->mac80211_registered)
+		ieee80211_unregister_hw(priv->hw);
 
 	/* assuming we used keventd, it must quiesce too */
 	flush_scheduled_work();
@@ -5359,25 +2583,11 @@ static void at76_delete_device(struct at76_priv *priv)
 	if (priv->rx_skb)
 		kfree_skb(priv->rx_skb);
 
-	at76_free_bss_list(priv);
-	del_timer_sync(&priv->bss_list_timer);
-	cancel_delayed_work(&priv->dwork_get_scan);
-	cancel_delayed_work(&priv->dwork_beacon);
-	cancel_delayed_work(&priv->dwork_auth);
-	cancel_delayed_work(&priv->dwork_assoc);
-
-	if (priv->mac_state == MAC_CONNECTED)
-		at76_iwevent_bss_disconnect(priv->netdev);
-
-	for (i = 0; i < NR_RX_DATA_BUF; i++)
-		if (priv->rx_data[i].skb) {
-			dev_kfree_skb(priv->rx_data[i].skb);
-			priv->rx_data[i].skb = NULL;
-		}
 	usb_put_dev(priv->udev);
 
-	at76_dbg(DBG_PROC_ENTRY, "%s: before freeing priv/netdev", __func__);
-	free_netdev(priv->netdev);	/* priv is in netdev */
+	at76_dbg(DBG_PROC_ENTRY, "%s: before freeing priv/ieee80211_hw",
+		 __func__);
+	ieee80211_free_hw(priv->hw);
 
 	at76_dbg(DBG_PROC_ENTRY, "%s: EXIT", __func__);
 }
@@ -5411,8 +2621,8 @@ static int at76_probe(struct usb_interface *interface,
 	   we get 204 with 2.4.23, Fiberline FL-WL240u (505A+RFMD2958) ??? */
 
 	if (op_mode == OPMODE_HW_CONFIG_MODE) {
-		dev_printk(KERN_ERR, &interface->dev,
-			   "cannot handle a device in HW_CONFIG_MODE\n");
+		dev_err(&interface->dev,
+			"cannot handle a device in HW_CONFIG_MODE\n");
 		ret = -EBUSY;
 		goto error;
 	}
@@ -5420,13 +2630,12 @@ static int at76_probe(struct usb_interface *interface,
 	if (op_mode != OPMODE_NORMAL_NIC_WITH_FLASH
 	    && op_mode != OPMODE_NORMAL_NIC_WITHOUT_FLASH) {
 		/* download internal firmware part */
-		dev_printk(KERN_DEBUG, &interface->dev,
-			   "downloading internal firmware\n");
+		dev_dbg(&interface->dev, "downloading internal firmware\n");
 		ret = at76_load_internal_fw(udev, fwe);
 		if (ret < 0) {
-			dev_printk(KERN_ERR, &interface->dev,
-				   "error %d downloading internal firmware\n",
-				   ret);
+			dev_err(&interface->dev,
+				"error %d downloading internal firmware\n",
+				ret);
 			goto error;
 		}
 		usb_put_dev(udev);
@@ -5451,8 +2660,7 @@ static int at76_probe(struct usb_interface *interface,
 		need_ext_fw = 1;
 
 	if (need_ext_fw) {
-		dev_printk(KERN_DEBUG, &interface->dev,
-			   "downloading external firmware\n");
+		dev_dbg(&interface->dev, "downloading external firmware\n");
 
 		ret = at76_load_external_fw(udev, fwe);
 		if (ret)
@@ -5461,8 +2669,8 @@ static int at76_probe(struct usb_interface *interface,
 		/* Re-check firmware version */
 		ret = at76_get_mib(udev, MIB_FW_VERSION, &fwv, sizeof(fwv));
 		if (ret < 0) {
-			dev_printk(KERN_ERR, &interface->dev,
-				   "error %d getting firmware version\n", ret);
+			dev_err(&interface->dev,
+				"error %d getting firmware version\n", ret);
 			goto error;
 		}
 	}
@@ -5473,7 +2681,6 @@ static int at76_probe(struct usb_interface *interface,
 		goto error;
 	}
 
-	SET_NETDEV_DEV(priv->netdev, &interface->dev);
 	usb_set_intfdata(interface, priv);
 
 	memcpy(&priv->fw_version, &fwv, sizeof(struct mib_fw_version));
@@ -5501,7 +2708,7 @@ static void at76_disconnect(struct usb_interface *interface)
 	if (!priv)
 		return;
 
-	printk(KERN_INFO "%s: disconnecting\n", priv->netdev->name);
+	printk(KERN_INFO "%s: disconnecting\n", wiphy_name(priv->hw->wiphy));
 	at76_delete_device(priv);
 	dev_printk(KERN_INFO, &interface->dev, "disconnected\n");
 }
@@ -5557,5 +2764,8 @@ MODULE_AUTHOR("Alex <alex@foogod.com>");
 MODULE_AUTHOR("Nick Jones");
 MODULE_AUTHOR("Balint Seeber <n0_5p4m_p13453@hotmail.com>");
 MODULE_AUTHOR("Pavel Roskin <proski@gnu.org>");
+MODULE_AUTHOR("Guido Guenther <agx@sigxcpu.org>");
+MODULE_AUTHOR("Kalle Valo <kalle.valo@iki.fi>");
+MODULE_AUTHOR("Milan Plzik <milan.plzik@gmail.com>");
 MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/at76_usb/at76_usb.h b/drivers/staging/at76_usb/at76_usb.h
index b20be9da1fa..8bb352f16d4 100644
--- a/drivers/staging/at76_usb/at76_usb.h
+++ b/drivers/staging/at76_usb/at76_usb.h
@@ -34,23 +34,6 @@ enum board_type {
 	BOARD_505AMX = 8
 };
 
-/* our private ioctl's */
-/* preamble length (0 - long, 1 - short, 2 - auto) */
-#define AT76_SET_SHORT_PREAMBLE		(SIOCIWFIRSTPRIV + 0)
-#define AT76_GET_SHORT_PREAMBLE		(SIOCIWFIRSTPRIV + 1)
-/* which debug channels are enabled */
-#define AT76_SET_DEBUG			(SIOCIWFIRSTPRIV + 2)
-#define AT76_GET_DEBUG			(SIOCIWFIRSTPRIV + 3)
-/* power save mode (incl. the Atmel proprietary smart save mode) */
-#define AT76_SET_POWERSAVE_MODE		(SIOCIWFIRSTPRIV + 4)
-#define AT76_GET_POWERSAVE_MODE		(SIOCIWFIRSTPRIV + 5)
-/* min and max channel times for scan */
-#define AT76_SET_SCAN_TIMES		(SIOCIWFIRSTPRIV + 6)
-#define AT76_GET_SCAN_TIMES		(SIOCIWFIRSTPRIV + 7)
-/* scan mode (0 - active, 1 - passive) */
-#define AT76_SET_SCAN_MODE		(SIOCIWFIRSTPRIV + 8)
-#define AT76_GET_SCAN_MODE		(SIOCIWFIRSTPRIV + 9)
-
 #define CMD_STATUS_IDLE				0x00
 #define CMD_STATUS_COMPLETE			0x01
 #define CMD_STATUS_UNKNOWN			0x02
@@ -82,6 +65,7 @@ enum board_type {
 #define MIB_MAC			0x03
 #define MIB_MAC_MGMT		0x05
 #define MIB_MAC_WEP		0x06
+#define MIB_MAC_ENCRYPTION	0x06
 #define MIB_PHY			0x07
 #define MIB_FW_VERSION		0x08
 #define MIB_MDOMAIN		0x09
@@ -106,6 +90,26 @@ enum board_type {
 #define AT76_PM_ON		2
 #define AT76_PM_SMART		3
 
+/* cipher values for encryption keys */
+#define CIPHER_NONE		0	/* this value is only guessed */
+#define CIPHER_WEP64		1
+#define CIPHER_TKIP		2
+#define CIPHER_CCMP		3
+#define CIPHER_CCX		4	/* for consistency sake only */
+#define CIPHER_WEP128		5
+
+/* bit flags key types for encryption keys */
+#define KEY_PAIRWISE		2
+#define KEY_TX			4
+
+#define CIPHER_KEYS		(4)
+#define CIPHER_KEY_LEN		(40)
+
+struct key_config {
+	u8 cipher;
+	u8 keylen;
+};
+
 struct hwcfg_r505 {
 	u8 cr39_values[14];
 	u8 reserved1[14];
@@ -147,6 +151,9 @@ union at76_hwcfg {
 
 #define WEP_SMALL_KEY_LEN	(40 / 8)
 #define WEP_LARGE_KEY_LEN	(104 / 8)
+#define WEP_KEYS		(4)
+
+
 
 struct at76_card_config {
 	u8 exclude_unencrypted;
@@ -161,7 +168,7 @@ struct at76_card_config {
 	u8 privacy_invoked;
 	u8 wep_default_key_id;	/* 0..3 */
 	u8 current_ssid[32];
-	u8 wep_default_key_value[4][WEP_KEY_LEN];
+	u8 wep_default_key_value[4][WEP_LARGE_KEY_LEN];
 	u8 ssid_len;
 	u8 short_preamble;
 	__le16 beacon_period;
@@ -186,7 +193,7 @@ struct at76_rx_buffer {
 	u8 link_quality;
 	u8 noise_level;
 	__le32 rx_time;
-	u8 packet[IEEE80211_FRAME_LEN + IEEE80211_FCS_LEN];
+	u8 packet[IEEE80211_MAX_FRAG_THRESHOLD];
 } __attribute__((packed));
 
 /* Length of Atmel-specific Tx header before 802.11 frame */
@@ -196,8 +203,11 @@ struct at76_tx_buffer {
 	__le16 wlength;
 	u8 tx_rate;
 	u8 padding;
-	u8 reserved[4];
-	u8 packet[IEEE80211_FRAME_LEN + IEEE80211_FCS_LEN];
+	u8 key_id;
+	u8 cipher_type;
+	u8 cipher_length;
+	u8 reserved;
+	u8 packet[IEEE80211_MAX_FRAG_THRESHOLD];
 } __attribute__((packed));
 
 /* defines for scan_type below */
@@ -244,6 +254,7 @@ struct set_mib_buffer {
 		u8 byte;
 		__le16 word;
 		u8 addr[ETH_ALEN];
+		u8 data[256];	/* we need more space for mib_mac_encryption */
 	} data;
 } __attribute__((packed));
 
@@ -317,10 +328,24 @@ struct mib_mac_wep {
 	u8 exclude_unencrypted;
 	__le32 wep_icv_error_count;
 	__le32 wep_excluded_count;
-	u8 wep_default_keyvalue[WEP_KEYS][WEP_KEY_LEN];
+	u8 wep_default_keyvalue[WEP_KEYS][WEP_LARGE_KEY_LEN];
 	u8 encryption_level;	/* 1 for 40bit, 2 for 104bit encryption */
 } __attribute__((packed));
 
+struct mib_mac_encryption {
+	u8 cipher_default_keyvalue[CIPHER_KEYS][CIPHER_KEY_LEN];
+	u8 tkip_bssid[6];
+	u8 privacy_invoked;
+	u8 cipher_default_key_id;
+	u8 cipher_default_group_key_id;
+	u8 exclude_unencrypted;
+	u8 wep_encryption_type;
+	u8 ckip_key_permutation;	/* bool */
+	__le32 wep_icv_error_count;
+	__le32 wep_excluded_count;
+	u8 key_rsc[CIPHER_KEYS][8];
+} __attribute__((packed));
+
 struct mib_phy {
 	__le32 ed_threshold;
 
@@ -364,16 +389,6 @@ struct at76_fw_header {
 	__le32 ext_fw_len;	/* external firmware image length */
 } __attribute__((packed));
 
-enum mac_state {
-	MAC_INIT,
-	MAC_SCANNING,
-	MAC_AUTH,
-	MAC_ASSOC,
-	MAC_JOINING,
-	MAC_CONNECTED,
-	MAC_OWN_IBSS
-};
-
 /* a description of a regulatory domain and the allowed channels */
 struct reg_domain {
 	u16 code;
@@ -381,47 +396,6 @@ struct reg_domain {
 	u32 channel_map;	/* if bit N is set, channel (N+1) is allowed */
 };
 
-/* how long do we keep a (I)BSS in the bss_list in jiffies
-   this should be long enough for the user to retrieve the table
-   (by iwlist ?) after the device started, because all entries from
-   other channels than the one the device locks on get removed, too */
-#define BSS_LIST_TIMEOUT	(120 * HZ)
-/* struct to store BSS info found during scan */
-#define BSS_LIST_MAX_RATE_LEN	32	/* 32 rates should be enough ... */
-
-struct bss_info {
-	struct list_head list;
-
-	u8 bssid[ETH_ALEN];	/* bssid */
-	u8 ssid[IW_ESSID_MAX_SIZE];	/* essid */
-	u8 ssid_len;		/* length of ssid above */
-	u8 channel;
-	u16 capa;		/* BSS capabilities */
-	u16 beacon_interval;	/* beacon interval, Kus (1024 microseconds) */
-	u8 rates[BSS_LIST_MAX_RATE_LEN];	/* supported rates in units of
-						   500 kbps, ORed with 0x80 for
-						   basic rates */
-	u8 rates_len;
-
-	/* quality of received beacon */
-	u8 rssi;
-	u8 link_qual;
-	u8 noise_level;
-
-	unsigned long last_rx;	/* time (jiffies) of last beacon received */
-};
-
-/* a rx data buffer to collect rx fragments */
-struct rx_data_buf {
-	u8 sender[ETH_ALEN];	/* sender address */
-	u16 seqnr;		/* sequence number */
-	u16 fragnr;		/* last fragment received */
-	unsigned long last_rx;	/* jiffies of last rx */
-	struct sk_buff *skb;	/* == NULL if entry is free */
-};
-
-#define NR_RX_DATA_BUF		8
-
 /* Data for one loaded firmware file */
 struct fwentry {
 	const char *const fwname;
@@ -438,11 +412,9 @@ struct fwentry {
 
 struct at76_priv {
 	struct usb_device *udev;	/* USB device pointer */
-	struct net_device *netdev;	/* net device pointer */
-	struct net_device_stats stats;	/* net device stats */
-	struct iw_statistics wstats;	/* wireless stats */
 
 	struct sk_buff *rx_skb;	/* skbuff for receiving data */
+	struct sk_buff *tx_skb;	/* skbuff for transmitting data */
 	void *bulk_out_buffer;	/* buffer for sending data */
 
 	struct urb *tx_urb;	/* URB for sending data */
@@ -454,26 +426,17 @@ struct at76_priv {
 	struct mutex mtx;	/* locks this structure */
 
 	/* work queues */
-	struct work_struct work_assoc_done;
-	struct work_struct work_join;
-	struct work_struct work_new_bss;
-	struct work_struct work_start_scan;
 	struct work_struct work_set_promisc;
 	struct work_struct work_submit_rx;
-	struct delayed_work dwork_restart;
-	struct delayed_work dwork_get_scan;
-	struct delayed_work dwork_beacon;
-	struct delayed_work dwork_auth;
-	struct delayed_work dwork_assoc;
+	struct delayed_work dwork_hw_scan;
 
 	struct tasklet_struct rx_tasklet;
 
 	/* the WEP stuff */
 	int wep_enabled;	/* 1 if WEP is enabled */
 	int wep_key_id;		/* key id to be used */
-	u8 wep_keys[WEP_KEYS][WEP_KEY_LEN];	/* the four WEP keys,
-						   5 or 13 bytes are used */
-	u8 wep_keys_len[WEP_KEYS];	/* the length of the above keys */
+	u8 wep_keys[WEP_KEYS][WEP_LARGE_KEY_LEN];	/* WEP keys */
+	u8 wep_keys_len[WEP_KEYS];	/* length of WEP keys */
 
 	int channel;
 	int iw_mode;
@@ -495,44 +458,13 @@ struct at76_priv {
 	int scan_mode;		/* SCAN_TYPE_ACTIVE, SCAN_TYPE_PASSIVE */
 	int scan_need_any;	/* if set, need to scan for any ESSID */
 
-	/* the list we got from scanning */
-	spinlock_t bss_list_spinlock;	/* protects bss_list operations */
-	struct list_head bss_list;	/* list of BSS we got beacons from */
-	struct timer_list bss_list_timer;	/* timer to purge old entries
-						   from bss_list */
-	struct bss_info *curr_bss;	/* current BSS */
 	u16 assoc_id;		/* current association ID, if associated */
 
-	u8 wanted_bssid[ETH_ALEN];
-	int wanted_bssid_valid;	/* != 0 if wanted_bssid is to be used */
-
-	/* some data for infrastructure mode only */
-	spinlock_t mgmt_spinlock;	/* this spinlock protects access to
-					   next_mgmt_bulk */
-
-	struct at76_tx_buffer *next_mgmt_bulk;	/* pending management msg to
-						   send via bulk out */
-	enum mac_state mac_state;
-	enum {
-		SCAN_IDLE,
-		SCAN_IN_PROGRESS,
-		SCAN_COMPLETED
-	} scan_state;
-	time_t last_scan;
-
-	int retries;		/* remaining retries in case of timeout when
-				 * sending AuthReq or AssocReq */
 	u8 pm_mode;		/* power management mode */
 	u32 pm_period;		/* power management period in microseconds */
 
 	struct reg_domain const *domain;	/* reg domain description */
 
-	/* iwspy support */
-	spinlock_t spy_spinlock;
-	struct iw_spy_data spy_data;
-
-	struct iw_public_data wireless_data;
-
 	/* These fields contain HW config provided by the device (not all of
 	 * these fields are used by all board types) */
 	u8 mac_addr[ETH_ALEN];
@@ -540,9 +472,6 @@ struct at76_priv {
 
 	struct at76_card_config card_config;
 
-	/* store rx fragments until complete */
-	struct rx_data_buf rx_data[NR_RX_DATA_BUF];
-
 	enum board_type board_type;
 	struct mib_fw_version fw_version;
 
@@ -550,58 +479,20 @@ struct at76_priv {
 	unsigned int netdev_registered:1;
 	struct set_mib_buffer mib_buf;	/* global buffer for set_mib calls */
 
-	/* beacon counting */
 	int beacon_period;	/* period of mgmt beacons, Kus */
-	int beacons_received;
-	unsigned long beacons_last_qual;	/* time we restarted counting
-						   beacons */
-};
 
-struct at76_rx_radiotap {
-	struct ieee80211_radiotap_header rt_hdr;
-	__le64 rt_tsft;
-	u8 rt_flags;
-	u8 rt_rate;
-	s8 rt_signal;
-	s8 rt_noise;
-};
-
-#define AT76_RX_RADIOTAP_PRESENT		  \
-	((1 << IEEE80211_RADIOTAP_TSFT)		| \
-	(1 << IEEE80211_RADIOTAP_FLAGS)		| \
-	(1 << IEEE80211_RADIOTAP_RATE)		| \
-	(1 << IEEE80211_RADIOTAP_DB_ANTSIGNAL)	| \
-	(1 << IEEE80211_RADIOTAP_DB_ANTNOISE))
-
-#define BEACON_MAX_DATA_LENGTH	1500
-
-/* the maximum size of an AssocReq packet */
-#define ASSOCREQ_MAX_SIZE \
-  (AT76_TX_HDRLEN + sizeof(struct ieee80211_assoc_request) + \
-   1 + 1 + IW_ESSID_MAX_SIZE + 1 + 1 + 4)
-
-/* for shared secret auth, add the challenge text size */
-#define AUTH_FRAME_SIZE (AT76_TX_HDRLEN + sizeof(struct ieee80211_auth))
+	struct ieee80211_hw *hw;
+	int mac80211_registered;
 
-/* Maximal number of AuthReq retries */
-#define AUTH_RETRIES		3
-
-/* Maximal number of AssocReq retries */
-#define ASSOC_RETRIES		3
-
-/* Beacon timeout in managed mode when we are connected */
-#define BEACON_TIMEOUT		(10 * HZ)
-
-/* Timeout for authentication response */
-#define AUTH_TIMEOUT		(1 * HZ)
+	struct key_config keys[4];	/* installed key types */
+	u8 default_pairwise_key;
+	u8 default_group_key;
+};
 
-/* Timeout for association response */
-#define ASSOC_TIMEOUT		(1 * HZ)
+#define AT76_SUPPORTED_FILTERS FIF_PROMISC_IN_BSS
 
-/* Polling interval when scan is running */
 #define SCAN_POLL_INTERVAL	(HZ / 4)
 
-/* Command completion timeout */
 #define CMD_COMPLETION_TIMEOUT	(5 * HZ)
 
 #define DEF_RTS_THRESHOLD	1536
@@ -611,8 +502,6 @@ struct at76_rx_radiotap {
 #define DEF_SCAN_MIN_TIME	10
 #define DEF_SCAN_MAX_TIME	120
 
-#define MAX_RTS_THRESHOLD	(MAX_FRAG_THRESHOLD + 1)
-
 /* the max padding size for tx in bytes (see calc_padding) */
 #define MAX_PADDING_SIZE	53
 
diff --git a/drivers/staging/benet/Kconfig b/drivers/staging/benet/Kconfig
new file mode 100644
index 00000000000..f6806074f99
--- /dev/null
+++ b/drivers/staging/benet/Kconfig
@@ -0,0 +1,7 @@
+config BENET
+	tristate "ServerEngines 10Gb NIC - BladeEngine"
+	depends on PCI && INET
+	select INET_LRO
+	help
+	  This driver implements the NIC functionality for ServerEngines
+	  10Gb network adapter BladeEngine (EC 3210).
diff --git a/drivers/staging/benet/MAINTAINERS b/drivers/staging/benet/MAINTAINERS
new file mode 100644
index 00000000000..d5ce340218b
--- /dev/null
+++ b/drivers/staging/benet/MAINTAINERS
@@ -0,0 +1,6 @@
+SERVER ENGINES 10Gbe NIC - BLADE-ENGINE
+P:	Subbu Seetharaman
+M:	subbus@serverengines.com
+L:	netdev@vger.kernel.org
+W:	http://www.serverengines.com
+S:	Supported
diff --git a/drivers/staging/benet/Makefile b/drivers/staging/benet/Makefile
new file mode 100644
index 00000000000..460b923b99b
--- /dev/null
+++ b/drivers/staging/benet/Makefile
@@ -0,0 +1,14 @@
+#
+# Makefile to build the network driver for ServerEngine's BladeEngine
+#
+obj-$(CONFIG_BENET) += benet.o
+
+benet-y :=  be_init.o \
+			be_int.o \
+			be_netif.o \
+			be_ethtool.o \
+			funcobj.o \
+			cq.o \
+			eq.o \
+			mpu.o \
+			eth.o
diff --git a/drivers/staging/benet/TODO b/drivers/staging/benet/TODO
new file mode 100644
index 00000000000..a51dfb59a62
--- /dev/null
+++ b/drivers/staging/benet/TODO
@@ -0,0 +1,6 @@
+TODO:
+	- remove wrappers around common iowrite functions
+	- full netdev audit of common problems/issues
+
+Please send all patches and questions to Subbu Seetharaman
+<subbus@serverengines.com> and Greg Kroah-Hartman <greg@kroah.com>
diff --git a/drivers/staging/benet/asyncmesg.h b/drivers/staging/benet/asyncmesg.h
new file mode 100644
index 00000000000..d1e779adb84
--- /dev/null
+++ b/drivers/staging/benet/asyncmesg.h
@@ -0,0 +1,82 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __asyncmesg_amap_h__
+#define __asyncmesg_amap_h__
+#include "fwcmd_common.h"
+
+/* --- ASYNC_EVENT_CODES --- */
+#define ASYNC_EVENT_CODE_LINK_STATE     (1)
+#define ASYNC_EVENT_CODE_ISCSI          (2)
+
+/* --- ASYNC_LINK_STATES --- */
+#define ASYNC_EVENT_LINK_DOWN           (0)	/* Link Down on a port */
+#define ASYNC_EVENT_LINK_UP             (1)	/* Link Up on a port */
+
+/*
+ * The last 4 bytes of the async events have this common format.  It allows
+ * the driver to distinguish [link]MCC_CQ_ENTRY[/link] structs from
+ * asynchronous events.  Both arrive on the same completion queue.  This
+ * structure also contains the common fields used to decode the async event.
+ */
+struct BE_ASYNC_EVENT_TRAILER_AMAP {
+	u8 rsvd0[8];	/* DWORD 0 */
+	u8 event_code[8];	/* DWORD 0 */
+	u8 event_type[8];	/* DWORD 0 */
+	u8 rsvd1[6];	/* DWORD 0 */
+	u8 async_event;	/* DWORD 0 */
+	u8 valid;		/* DWORD 0 */
+} __packed;
+struct ASYNC_EVENT_TRAILER_AMAP {
+	u32 dw[1];
+};
+
+/*
+ * Applicable in Initiator, Target and NIC modes.
+ * A link state async event is seen by all device drivers as soon they
+ * create an MCC ring. Thereafter, anytime the link status changes the
+ * drivers will receive a link state async event. Notifications continue to
+ * be sent until a driver destroys its MCC ring. A link down event is
+ * reported when either port loses link. A link up event is reported
+ * when either port regains link. When BE's failover mechanism is enabled, a
+ * link down on the active port causes traffic to be diverted to the standby
+ * port by the BE's ARM firmware (assuming the standby port has link). In
+ * this case, the standy port assumes the active status. Note: when link is
+ * restored on the failed port, traffic continues on the currently active
+ * port. The ARM firmware does not attempt to 'fail back' traffic to
+ * the restored port.
+ */
+struct BE_ASYNC_EVENT_LINK_STATE_AMAP {
+	u8 port0_link_status[8];
+	u8 port1_link_status[8];
+	u8 active_port[8];
+	u8 rsvd0[8];	/* DWORD 0 */
+	u8 port0_duplex[8];
+	u8 port0_speed[8];
+	u8 port1_duplex[8];
+	u8 port1_speed[8];
+	u8 port0_fault[8];
+	u8 port1_fault[8];
+	u8 rsvd1[2][8];	/* DWORD 2 */
+	struct BE_ASYNC_EVENT_TRAILER_AMAP trailer;
+} __packed;
+struct ASYNC_EVENT_LINK_STATE_AMAP {
+	u32 dw[4];
+};
+#endif /* __asyncmesg_amap_h__ */
diff --git a/drivers/staging/benet/be_cm.h b/drivers/staging/benet/be_cm.h
new file mode 100644
index 00000000000..b7a1dfd20c3
--- /dev/null
+++ b/drivers/staging/benet/be_cm.h
@@ -0,0 +1,134 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __be_cm_amap_h__
+#define __be_cm_amap_h__
+#include "be_common.h"
+#include "etx_context.h"
+#include "mpu_context.h"
+
+/*
+ * --- CEV_WATERMARK_ENUM ---
+ * CQ/EQ Watermark Encodings. Encoded as number of free entries in
+ * Queue when Watermark is reached.
+ */
+#define CEV_WMARK_0        (0)	/* Watermark when Queue full */
+#define CEV_WMARK_16       (1)	/* Watermark at 16 free entries */
+#define CEV_WMARK_32       (2)	/* Watermark at 32 free entries */
+#define CEV_WMARK_48       (3)	/* Watermark at 48 free entries */
+#define CEV_WMARK_64       (4)	/* Watermark at 64 free entries */
+#define CEV_WMARK_80       (5)	/* Watermark at 80 free entries */
+#define CEV_WMARK_96       (6)	/* Watermark at 96 free entries */
+#define CEV_WMARK_112      (7)	/* Watermark at 112 free entries */
+#define CEV_WMARK_128      (8)	/* Watermark at 128 free entries */
+#define CEV_WMARK_144      (9)	/* Watermark at 144 free entries */
+#define CEV_WMARK_160      (10)	/* Watermark at 160 free entries */
+#define CEV_WMARK_176      (11)	/* Watermark at 176 free entries */
+#define CEV_WMARK_192      (12)	/* Watermark at 192 free entries */
+#define CEV_WMARK_208      (13)	/* Watermark at 208 free entries */
+#define CEV_WMARK_224      (14)	/* Watermark at 224 free entries */
+#define CEV_WMARK_240      (15)	/* Watermark at 240 free entries */
+
+/*
+ * --- CQ_CNT_ENUM ---
+ * Completion Queue Count Encodings.
+ */
+#define CEV_CQ_CNT_256                  (0)	/* CQ has 256 entries */
+#define CEV_CQ_CNT_512                  (1)	/* CQ has 512 entries */
+#define CEV_CQ_CNT_1024                 (2)	/* CQ has 1024 entries */
+
+/*
+ * --- EQ_CNT_ENUM ---
+ * Event Queue Count Encodings.
+ */
+#define CEV_EQ_CNT_256     (0)	/* EQ has 256 entries (16-byte EQEs only) */
+#define CEV_EQ_CNT_512     (1)	/* EQ has 512 entries (16-byte EQEs only) */
+#define CEV_EQ_CNT_1024    (2)	/* EQ has 1024 entries (4-byte or */
+				/* 16-byte EQEs only) */
+#define CEV_EQ_CNT_2048    (3)	/* EQ has 2048 entries (4-byte or */
+				/* 16-byte EQEs only) */
+#define CEV_EQ_CNT_4096    (4)	/* EQ has 4096 entries (4-byte EQEs only) */
+
+/*
+ * --- EQ_SIZE_ENUM ---
+ * Event Queue Entry Size Encoding.
+ */
+#define CEV_EQ_SIZE_4                   (0)	/* EQE is 4 bytes */
+#define CEV_EQ_SIZE_16                  (1)	/* EQE is 16 bytes */
+
+/*
+ * Completion Queue Context Table Entry. Contains the state of a CQ.
+ * Located in RAM within the CEV block.
+ */
+struct BE_CQ_CONTEXT_AMAP {
+	u8 Cidx[11];	/* DWORD 0 */
+	u8 Watermark[4];	/* DWORD 0 */
+	u8 NoDelay;		/* DWORD 0 */
+	u8 EPIdx[11];	/* DWORD 0 */
+	u8 Count[2];	/* DWORD 0 */
+	u8 valid;		/* DWORD 0 */
+	u8 SolEvent;	/* DWORD 0 */
+	u8 Eventable;	/* DWORD 0 */
+	u8 Pidx[11];	/* DWORD 1 */
+	u8 PD[10];		/* DWORD 1 */
+	u8 EQID[7];		/* DWORD 1 */
+	u8 Func;		/* DWORD 1 */
+	u8 WME;		/* DWORD 1 */
+	u8 Stalled;		/* DWORD 1 */
+	u8 Armed;		/* DWORD 1 */
+} __packed;
+struct CQ_CONTEXT_AMAP {
+	u32 dw[2];
+};
+
+/*
+ * Event Queue Context Table Entry. Contains the state of an EQ.
+ * Located in RAM in the CEV block.
+ */
+struct BE_EQ_CONTEXT_AMAP {
+	u8 Cidx[13];	/* DWORD 0 */
+	u8 rsvd0[2];	/* DWORD 0 */
+	u8 Func;		/* DWORD 0 */
+	u8 EPIdx[13];	/* DWORD 0 */
+	u8 valid;		/* DWORD 0 */
+	u8 rsvd1;		/* DWORD 0 */
+	u8 Size;		/* DWORD 0 */
+	u8 Pidx[13];	/* DWORD 1 */
+	u8 rsvd2[3];	/* DWORD 1 */
+	u8 PD[10];		/* DWORD 1 */
+	u8 Count[3];	/* DWORD 1 */
+	u8 SolEvent;	/* DWORD 1 */
+	u8 Stalled;		/* DWORD 1 */
+	u8 Armed;		/* DWORD 1 */
+	u8 Watermark[4];	/* DWORD 2 */
+	u8 WME;		/* DWORD 2 */
+	u8 rsvd3[3];	/* DWORD 2 */
+	u8 EventVect[6];	/* DWORD 2 */
+	u8 rsvd4[2];	/* DWORD 2 */
+	u8 Delay[8];	/* DWORD 2 */
+	u8 rsvd5[6];	/* DWORD 2 */
+	u8 TMR;		/* DWORD 2 */
+	u8 rsvd6;		/* DWORD 2 */
+	u8 rsvd7[32];	/* DWORD 3 */
+} __packed;
+struct EQ_CONTEXT_AMAP {
+	u32 dw[4];
+};
+
+#endif /* __be_cm_amap_h__ */
diff --git a/drivers/staging/benet/be_common.h b/drivers/staging/benet/be_common.h
new file mode 100644
index 00000000000..7e63dc5e334
--- /dev/null
+++ b/drivers/staging/benet/be_common.h
@@ -0,0 +1,53 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __be_common_amap_h__
+#define __be_common_amap_h__
+
+/* Physical Address. */
+struct BE_PHYS_ADDR_AMAP {
+	u8 lo[32];		/* DWORD 0 */
+	u8 hi[32];		/* DWORD 1 */
+} __packed;
+struct PHYS_ADDR_AMAP {
+	u32 dw[2];
+};
+
+/* Virtual Address. */
+struct BE_VIRT_ADDR_AMAP {
+	u8 lo[32];		/* DWORD 0 */
+	u8 hi[32];		/* DWORD 1 */
+} __packed;
+struct VIRT_ADDR_AMAP {
+	u32 dw[2];
+};
+
+/* Scatter gather element. */
+struct BE_SGE_AMAP {
+	u8 addr_hi[32];	/* DWORD 0 */
+	u8 addr_lo[32];	/* DWORD 1 */
+	u8 rsvd0[32];	/* DWORD 2 */
+	u8 len[16];		/* DWORD 3 */
+	u8 rsvd1[16];	/* DWORD 3 */
+} __packed;
+struct SGE_AMAP {
+	u32 dw[4];
+};
+
+#endif /* __be_common_amap_h__ */
diff --git a/drivers/staging/benet/be_ethtool.c b/drivers/staging/benet/be_ethtool.c
new file mode 100644
index 00000000000..027af85707a
--- /dev/null
+++ b/drivers/staging/benet/be_ethtool.c
@@ -0,0 +1,348 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * be_ethtool.c
+ *
+ * 	This file contains various functions that ethtool can use
+ * 	to talk to the driver and the BE H/W.
+ */
+
+#include "benet.h"
+
+#include <linux/ethtool.h>
+
+static const char benet_gstrings_stats[][ETH_GSTRING_LEN] = {
+/* net_device_stats */
+	"rx_packets",
+	"tx_packets",
+	"rx_bytes",
+	"tx_bytes",
+	"rx_errors",
+	"tx_errors",
+	"rx_dropped",
+	"tx_dropped",
+	"multicast",
+	"collisions",
+	"rx_length_errors",
+	"rx_over_errors",
+	"rx_crc_errors",
+	"rx_frame_errors",
+	"rx_fifo_errors",
+	"rx_missed_errors",
+	"tx_aborted_errors",
+	"tx_carrier_errors",
+	"tx_fifo_errors",
+	"tx_heartbeat_errors",
+	"tx_window_errors",
+	"rx_compressed",
+	"tc_compressed",
+/* BE driver Stats */
+	"bes_tx_reqs",
+	"bes_tx_fails",
+	"bes_fwd_reqs",
+	"bes_tx_wrbs",
+	"bes_interrupts",
+	"bes_events",
+	"bes_tx_events",
+	"bes_rx_events",
+	"bes_tx_compl",
+	"bes_rx_compl",
+	"bes_ethrx_post_fail",
+	"bes_802_3_dropped_frames",
+	"bes_802_3_malformed_frames",
+	"bes_rx_misc_pkts",
+	"bes_eth_tx_rate",
+	"bes_eth_rx_rate",
+	"Num Packets collected",
+	"Num Times Flushed",
+};
+
+#define NET_DEV_STATS_LEN \
+	(sizeof(struct net_device_stats)/sizeof(unsigned long))
+
+#define BENET_STATS_LEN  ARRAY_SIZE(benet_gstrings_stats)
+
+static void
+be_get_drvinfo(struct net_device *netdev, struct ethtool_drvinfo *drvinfo)
+{
+	struct be_net_object *pnob = netdev_priv(netdev);
+	struct be_adapter *adapter = pnob->adapter;
+
+	strncpy(drvinfo->driver, be_driver_name, 32);
+	strncpy(drvinfo->version, be_drvr_ver, 32);
+	strncpy(drvinfo->fw_version, be_fw_ver, 32);
+	strcpy(drvinfo->bus_info, pci_name(adapter->pdev));
+	drvinfo->testinfo_len = 0;
+	drvinfo->regdump_len = 0;
+	drvinfo->eedump_len = 0;
+}
+
+static int
+be_get_coalesce(struct net_device *netdev, struct ethtool_coalesce *coalesce)
+{
+	struct be_net_object *pnob = netdev_priv(netdev);
+	struct be_adapter *adapter = pnob->adapter;
+
+	coalesce->rx_max_coalesced_frames = adapter->max_rx_coal;
+
+	coalesce->rx_coalesce_usecs = adapter->cur_eqd;
+	coalesce->rx_coalesce_usecs_high = adapter->max_eqd;
+	coalesce->rx_coalesce_usecs_low = adapter->min_eqd;
+
+	coalesce->tx_coalesce_usecs = adapter->cur_eqd;
+	coalesce->tx_coalesce_usecs_high = adapter->max_eqd;
+	coalesce->tx_coalesce_usecs_low = adapter->min_eqd;
+
+	coalesce->use_adaptive_rx_coalesce = adapter->enable_aic;
+	coalesce->use_adaptive_tx_coalesce = adapter->enable_aic;
+
+	return 0;
+}
+
+/*
+ * This routine is used to set interrup coalescing delay *as well as*
+ * the number of pkts to coalesce for LRO.
+ */
+static int
+be_set_coalesce(struct net_device *netdev, struct ethtool_coalesce *coalesce)
+{
+	struct be_net_object *pnob = netdev_priv(netdev);
+	struct be_adapter *adapter = pnob->adapter;
+	struct be_eq_object *eq_objectp;
+	u32 max, min, cur;
+	int status;
+
+	adapter->max_rx_coal = coalesce->rx_max_coalesced_frames;
+	if (adapter->max_rx_coal >= BE_LRO_MAX_PKTS)
+		adapter->max_rx_coal = BE_LRO_MAX_PKTS;
+
+	if (adapter->enable_aic == 0 &&
+		coalesce->use_adaptive_rx_coalesce == 1) {
+		/* if AIC is being turned on now, start with an EQD of 0 */
+		adapter->cur_eqd = 0;
+	}
+	adapter->enable_aic = coalesce->use_adaptive_rx_coalesce;
+
+	/* round off to nearest multiple of 8 */
+	max = (((coalesce->rx_coalesce_usecs_high + 4) >> 3) << 3);
+	min = (((coalesce->rx_coalesce_usecs_low + 4) >> 3) << 3);
+	cur = (((coalesce->rx_coalesce_usecs + 4) >> 3) << 3);
+
+	if (adapter->enable_aic) {
+		/* accept low and high if AIC is enabled */
+		if (max > MAX_EQD)
+			max = MAX_EQD;
+		if (min > max)
+			min = max;
+		adapter->max_eqd = max;
+		adapter->min_eqd = min;
+		if (adapter->cur_eqd > max)
+			adapter->cur_eqd = max;
+		if (adapter->cur_eqd < min)
+			adapter->cur_eqd = min;
+	} else {
+		/* accept specified coalesce_usecs only if AIC is disabled */
+		if (cur > MAX_EQD)
+			cur = MAX_EQD;
+		eq_objectp = &pnob->event_q_obj;
+		status =
+		    be_eq_modify_delay(&pnob->fn_obj, 1, &eq_objectp, &cur,
+				       NULL, NULL, NULL);
+		if (status == BE_SUCCESS)
+			adapter->cur_eqd = cur;
+	}
+	return 0;
+}
+
+static u32 be_get_rx_csum(struct net_device *netdev)
+{
+	struct be_net_object *pnob = netdev_priv(netdev);
+	struct be_adapter *adapter = pnob->adapter;
+	return adapter->rx_csum;
+}
+
+static int be_set_rx_csum(struct net_device *netdev, uint32_t data)
+{
+	struct be_net_object *pnob = netdev_priv(netdev);
+	struct be_adapter *adapter = pnob->adapter;
+
+	if (data)
+		adapter->rx_csum = 1;
+	else
+		adapter->rx_csum = 0;
+
+	return 0;
+}
+
+static void
+be_get_strings(struct net_device *netdev, uint32_t stringset, uint8_t *data)
+{
+	switch (stringset) {
+	case ETH_SS_STATS:
+		memcpy(data, *benet_gstrings_stats,
+		       sizeof(benet_gstrings_stats));
+		break;
+	}
+}
+
+static int be_get_stats_count(struct net_device *netdev)
+{
+	return BENET_STATS_LEN;
+}
+
+static void
+be_get_ethtool_stats(struct net_device *netdev,
+		     struct ethtool_stats *stats, uint64_t *data)
+{
+	struct be_net_object *pnob = netdev_priv(netdev);
+	struct be_adapter *adapter = pnob->adapter;
+	int i;
+
+	benet_get_stats(netdev);
+
+	for (i = 0; i <= NET_DEV_STATS_LEN; i++)
+		data[i] = ((unsigned long *)&adapter->benet_stats)[i];
+
+	data[i] = adapter->be_stat.bes_tx_reqs;
+	data[i++] = adapter->be_stat.bes_tx_fails;
+	data[i++] = adapter->be_stat.bes_fwd_reqs;
+	data[i++] = adapter->be_stat.bes_tx_wrbs;
+
+	data[i++] = adapter->be_stat.bes_ints;
+	data[i++] = adapter->be_stat.bes_events;
+	data[i++] = adapter->be_stat.bes_tx_events;
+	data[i++] = adapter->be_stat.bes_rx_events;
+	data[i++] = adapter->be_stat.bes_tx_compl;
+	data[i++] = adapter->be_stat.bes_rx_compl;
+	data[i++] = adapter->be_stat.bes_ethrx_post_fail;
+	data[i++] = adapter->be_stat.bes_802_3_dropped_frames;
+	data[i++] = adapter->be_stat.bes_802_3_malformed_frames;
+	data[i++] = adapter->be_stat.bes_rx_misc_pkts;
+	data[i++] = adapter->be_stat.bes_eth_tx_rate;
+	data[i++] = adapter->be_stat.bes_eth_rx_rate;
+	data[i++] = adapter->be_stat.bes_rx_coal;
+	data[i++] = adapter->be_stat.bes_rx_flush;
+
+}
+
+static int be_get_settings(struct net_device *netdev, struct ethtool_cmd *ecmd)
+{
+	ecmd->speed = SPEED_10000;
+	ecmd->duplex = DUPLEX_FULL;
+	ecmd->autoneg = AUTONEG_DISABLE;
+	return 0;
+}
+
+/* Get the Ring parameters from the pnob */
+static void
+be_get_ringparam(struct net_device *netdev, struct ethtool_ringparam *ring)
+{
+	struct be_net_object *pnob = netdev_priv(netdev);
+
+	/* Pre Set Maxims */
+	ring->rx_max_pending = pnob->rx_q_len;
+	ring->rx_mini_max_pending = ring->rx_mini_max_pending;
+	ring->rx_jumbo_max_pending = ring->rx_jumbo_max_pending;
+	ring->tx_max_pending = pnob->tx_q_len;
+
+	/* Current hardware Settings                */
+	ring->rx_pending = atomic_read(&pnob->rx_q_posted);
+	ring->rx_mini_pending = ring->rx_mini_pending;
+	ring->rx_jumbo_pending = ring->rx_jumbo_pending;
+	ring->tx_pending = atomic_read(&pnob->tx_q_used);
+
+}
+
+static void
+be_get_pauseparam(struct net_device *netdev, struct ethtool_pauseparam *ecmd)
+{
+	struct be_net_object *pnob = netdev_priv(netdev);
+	bool rxfc, txfc;
+	int status;
+
+	status = be_eth_get_flow_control(&pnob->fn_obj, &txfc, &rxfc);
+	if (status != BE_SUCCESS) {
+		dev_info(&netdev->dev, "Unable to get pause frame settings\n");
+		/* return defaults */
+		ecmd->rx_pause = 1;
+		ecmd->tx_pause = 0;
+		ecmd->autoneg = AUTONEG_ENABLE;
+		return;
+	}
+
+	if (txfc == true)
+		ecmd->tx_pause = 1;
+	else
+		ecmd->tx_pause = 0;
+
+	if (rxfc == true)
+		ecmd->rx_pause = 1;
+	else
+		ecmd->rx_pause = 0;
+
+	ecmd->autoneg = AUTONEG_ENABLE;
+}
+
+static int
+be_set_pauseparam(struct net_device *netdev, struct ethtool_pauseparam *ecmd)
+{
+	struct be_net_object *pnob = netdev_priv(netdev);
+	bool txfc, rxfc;
+	int status;
+
+	if (ecmd->autoneg != AUTONEG_ENABLE)
+		return -EINVAL;
+
+	if (ecmd->tx_pause)
+		txfc = true;
+	else
+		txfc = false;
+
+	if (ecmd->rx_pause)
+		rxfc = true;
+	else
+		rxfc = false;
+
+	status = be_eth_set_flow_control(&pnob->fn_obj, txfc, rxfc);
+	if (status != BE_SUCCESS) {
+		dev_info(&netdev->dev, "Unable to set pause frame settings\n");
+		return -1;
+	}
+	return 0;
+}
+
+struct ethtool_ops be_ethtool_ops = {
+	.get_settings = be_get_settings,
+	.get_drvinfo = be_get_drvinfo,
+	.get_link = ethtool_op_get_link,
+	.get_coalesce = be_get_coalesce,
+	.set_coalesce = be_set_coalesce,
+	.get_ringparam = be_get_ringparam,
+	.get_pauseparam = be_get_pauseparam,
+	.set_pauseparam = be_set_pauseparam,
+	.get_rx_csum = be_get_rx_csum,
+	.set_rx_csum = be_set_rx_csum,
+	.get_tx_csum = ethtool_op_get_tx_csum,
+	.set_tx_csum = ethtool_op_set_tx_csum,
+	.get_sg = ethtool_op_get_sg,
+	.set_sg = ethtool_op_set_sg,
+	.get_tso = ethtool_op_get_tso,
+	.set_tso = ethtool_op_set_tso,
+	.get_strings = be_get_strings,
+	.get_stats_count = be_get_stats_count,
+	.get_ethtool_stats = be_get_ethtool_stats,
+};
diff --git a/drivers/staging/benet/be_init.c b/drivers/staging/benet/be_init.c
new file mode 100644
index 00000000000..12a026c3f9e
--- /dev/null
+++ b/drivers/staging/benet/be_init.c
@@ -0,0 +1,1382 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+#include <linux/etherdevice.h>
+#include "benet.h"
+
+#define  DRVR_VERSION  "1.0.728"
+
+static const struct pci_device_id be_device_id_table[] = {
+	{PCI_DEVICE(0x19a2, 0x0201)},
+	{0}
+};
+
+MODULE_DEVICE_TABLE(pci, be_device_id_table);
+
+MODULE_VERSION(DRVR_VERSION);
+
+#define DRV_DESCRIPTION "ServerEngines BladeEngine Network Driver Version "
+
+MODULE_DESCRIPTION(DRV_DESCRIPTION DRVR_VERSION);
+MODULE_AUTHOR("ServerEngines");
+MODULE_LICENSE("GPL");
+
+static unsigned int msix = 1;
+module_param(msix, uint, S_IRUGO);
+MODULE_PARM_DESC(msix, "Use MSI-x interrupts");
+
+static unsigned int rxbuf_size = 2048;	/* Default RX frag size */
+module_param(rxbuf_size, uint, S_IRUGO);
+MODULE_PARM_DESC(rxbuf_size, "Size of buffers to hold Rx data");
+
+const char be_drvr_ver[] = DRVR_VERSION;
+char be_fw_ver[32];		/* F/W version filled in by be_probe */
+char be_driver_name[] = "benet";
+
+/*
+ * Number of entries in each queue.
+ */
+#define EVENT_Q_LEN		1024
+#define ETH_TXQ_LEN		2048
+#define ETH_TXCQ_LEN		1024
+#define ETH_RXQ_LEN		1024	/* Does not support any other value */
+#define ETH_UC_RXCQ_LEN		1024
+#define ETH_BC_RXCQ_LEN		256
+#define MCC_Q_LEN               64	/* total size not to exceed 8 pages */
+#define MCC_CQ_LEN              256
+
+/* Bit mask describing events of interest to be traced */
+unsigned int trace_level;
+
+static int
+init_pci_be_function(struct be_adapter *adapter, struct pci_dev *pdev)
+{
+	u64 pa;
+
+	/* CSR */
+	pa = pci_resource_start(pdev, 2);
+	adapter->csr_va = ioremap_nocache(pa, pci_resource_len(pdev, 2));
+	if (adapter->csr_va == NULL)
+		return -ENOMEM;
+
+	/* Door Bell */
+	pa = pci_resource_start(pdev, 4);
+	adapter->db_va = ioremap_nocache(pa, (128 * 1024));
+	if (adapter->db_va == NULL) {
+		iounmap(adapter->csr_va);
+		return -ENOMEM;
+	}
+
+	/* PCI */
+	pa = pci_resource_start(pdev, 1);
+	adapter->pci_va = ioremap_nocache(pa, pci_resource_len(pdev, 1));
+	if (adapter->pci_va == NULL) {
+		iounmap(adapter->csr_va);
+		iounmap(adapter->db_va);
+		return -ENOMEM;
+	}
+	return 0;
+}
+
+/*
+   This function enables the interrupt corresponding to the Event
+   queue ID for the given NetObject
+*/
+void be_enable_eq_intr(struct be_net_object *pnob)
+{
+	struct CQ_DB_AMAP cqdb;
+	cqdb.dw[0] = 0;
+	AMAP_SET_BITS_PTR(CQ_DB, event, &cqdb, 1);
+	AMAP_SET_BITS_PTR(CQ_DB, rearm, &cqdb, 1);
+	AMAP_SET_BITS_PTR(CQ_DB, num_popped, &cqdb, 0);
+	AMAP_SET_BITS_PTR(CQ_DB, qid, &cqdb, pnob->event_q_id);
+	PD_WRITE(&pnob->fn_obj, cq_db, cqdb.dw[0]);
+}
+
+/*
+   This function disables the interrupt corresponding to the Event
+   queue ID for the given NetObject
+*/
+void be_disable_eq_intr(struct be_net_object *pnob)
+{
+	struct CQ_DB_AMAP cqdb;
+	cqdb.dw[0] = 0;
+	AMAP_SET_BITS_PTR(CQ_DB, event, &cqdb, 1);
+	AMAP_SET_BITS_PTR(CQ_DB, rearm, &cqdb, 0);
+	AMAP_SET_BITS_PTR(CQ_DB, num_popped, &cqdb, 0);
+	AMAP_SET_BITS_PTR(CQ_DB, qid, &cqdb, pnob->event_q_id);
+	PD_WRITE(&pnob->fn_obj, cq_db, cqdb.dw[0]);
+}
+
+/*
+    This function enables the interrupt from the  network function
+    of the BladeEngine. Use the function be_disable_eq_intr()
+    to enable the interrupt from the event queue of only one specific
+    NetObject
+*/
+void be_enable_intr(struct be_net_object *pnob)
+{
+	struct PCICFG_HOST_TIMER_INT_CTRL_CSR_AMAP ctrl;
+	u32 host_intr;
+
+	ctrl.dw[0] = PCICFG1_READ(&pnob->fn_obj, host_timer_int_ctrl);
+	host_intr = AMAP_GET_BITS_PTR(PCICFG_HOST_TIMER_INT_CTRL_CSR,
+							hostintr, ctrl.dw);
+	if (!host_intr) {
+		AMAP_SET_BITS_PTR(PCICFG_HOST_TIMER_INT_CTRL_CSR,
+			hostintr, ctrl.dw, 1);
+		PCICFG1_WRITE(&pnob->fn_obj, host_timer_int_ctrl,
+			ctrl.dw[0]);
+	}
+}
+
+/*
+   This function disables the interrupt from the network function of
+   the BladeEngine.  Use the function be_disable_eq_intr() to
+   disable the interrupt from the event queue of only one specific NetObject
+*/
+void be_disable_intr(struct be_net_object *pnob)
+{
+
+	struct PCICFG_HOST_TIMER_INT_CTRL_CSR_AMAP ctrl;
+	u32 host_intr;
+	ctrl.dw[0] = PCICFG1_READ(&pnob->fn_obj, host_timer_int_ctrl);
+	host_intr = AMAP_GET_BITS_PTR(PCICFG_HOST_TIMER_INT_CTRL_CSR,
+							hostintr, ctrl.dw);
+	if (host_intr) {
+		AMAP_SET_BITS_PTR(PCICFG_HOST_TIMER_INT_CTRL_CSR, hostintr,
+			ctrl.dw, 0);
+		PCICFG1_WRITE(&pnob->fn_obj, host_timer_int_ctrl,
+			ctrl.dw[0]);
+	}
+}
+
+static int be_enable_msix(struct be_adapter *adapter)
+{
+	int i, ret;
+
+	if (!msix)
+		return -1;
+
+	for (i = 0; i < BE_MAX_REQ_MSIX_VECTORS; i++)
+		adapter->msix_entries[i].entry = i;
+
+	ret = pci_enable_msix(adapter->pdev, adapter->msix_entries,
+		BE_MAX_REQ_MSIX_VECTORS);
+
+	if (ret == 0)
+		adapter->msix_enabled = 1;
+	return ret;
+}
+
+static int be_register_isr(struct be_adapter *adapter,
+		struct be_net_object *pnob)
+{
+	struct net_device *netdev = pnob->netdev;
+	int intx = 0, r;
+
+	netdev->irq = adapter->pdev->irq;
+	r = be_enable_msix(adapter);
+
+	if (r == 0) {
+		r = request_irq(adapter->msix_entries[0].vector,
+				be_int, IRQF_SHARED, netdev->name, netdev);
+		if (r) {
+			printk(KERN_WARNING
+				"MSIX Request IRQ failed - Errno %d\n", r);
+			intx = 1;
+			pci_disable_msix(adapter->pdev);
+			adapter->msix_enabled = 0;
+		}
+	} else {
+		intx = 1;
+	}
+
+	if (intx) {
+		r = request_irq(netdev->irq, be_int, IRQF_SHARED,
+				netdev->name, netdev);
+		if (r) {
+			printk(KERN_WARNING
+				"INTx Request IRQ failed - Errno %d\n", r);
+			return -1;
+		}
+	}
+	adapter->isr_registered = 1;
+	return 0;
+}
+
+static void be_unregister_isr(struct be_adapter *adapter)
+{
+	struct net_device *netdev = adapter->netdevp;
+	if (adapter->isr_registered) {
+		if (adapter->msix_enabled) {
+			free_irq(adapter->msix_entries[0].vector, netdev);
+			pci_disable_msix(adapter->pdev);
+			adapter->msix_enabled = 0;
+		} else {
+			free_irq(netdev->irq, netdev);
+		}
+		adapter->isr_registered = 0;
+	}
+}
+
+/*
+    This function processes the Flush Completions that are issued by the
+    ARM F/W, when a Recv Ring is destroyed.  A flush completion is
+    identified when a Rx COmpl descriptor has the tcpcksum and udpcksum
+    set and the pktsize is 32.  These completions are received on the
+    Rx Completion Queue.
+*/
+static u32 be_process_rx_flush_cmpl(struct be_net_object *pnob)
+{
+	struct ETH_RX_COMPL_AMAP *rxcp;
+	unsigned int i = 0;
+	while ((rxcp = be_get_rx_cmpl(pnob)) != NULL) {
+		be_notify_cmpl(pnob, 1, pnob->rx_cq_id, 1);
+		i++;
+	}
+	return i;
+}
+
+static void be_tx_q_clean(struct be_net_object *pnob)
+{
+	while (atomic_read(&pnob->tx_q_used))
+		process_one_tx_compl(pnob, tx_compl_lastwrb_idx_get(pnob));
+}
+
+static void be_rx_q_clean(struct be_net_object *pnob)
+{
+	if (pnob->rx_ctxt) {
+		int i;
+		struct be_rx_page_info *rx_page_info;
+		for (i = 0; i < pnob->rx_q_len; i++) {
+			rx_page_info = &(pnob->rx_page_info[i]);
+			if (!pnob->rx_pg_shared || rx_page_info->page_offset) {
+				pci_unmap_page(pnob->adapter->pdev,
+				       pci_unmap_addr(rx_page_info, bus),
+					       pnob->rx_buf_size,
+					       PCI_DMA_FROMDEVICE);
+			}
+			if (rx_page_info->page)
+				put_page(rx_page_info->page);
+			memset(rx_page_info, 0, sizeof(struct be_rx_page_info));
+		}
+		pnob->rx_pg_info_hd = 0;
+	}
+}
+
+static void be_destroy_netobj(struct be_net_object *pnob)
+{
+	int status;
+
+	if (pnob->tx_q_created) {
+		status = be_eth_sq_destroy(&pnob->tx_q_obj);
+		pnob->tx_q_created = 0;
+	}
+
+	if (pnob->rx_q_created) {
+		status = be_eth_rq_destroy(&pnob->rx_q_obj);
+		if (status != 0) {
+			status = be_eth_rq_destroy_options(&pnob->rx_q_obj, 0,
+						      NULL, NULL);
+			BUG_ON(status);
+		}
+		pnob->rx_q_created = 0;
+	}
+
+	be_process_rx_flush_cmpl(pnob);
+
+	if (pnob->tx_cq_created) {
+		status = be_cq_destroy(&pnob->tx_cq_obj);
+		pnob->tx_cq_created = 0;
+	}
+
+	if (pnob->rx_cq_created) {
+		status = be_cq_destroy(&pnob->rx_cq_obj);
+		pnob->rx_cq_created = 0;
+	}
+
+	if (pnob->mcc_q_created) {
+		status = be_mcc_ring_destroy(&pnob->mcc_q_obj);
+		pnob->mcc_q_created = 0;
+	}
+	if (pnob->mcc_cq_created) {
+		status = be_cq_destroy(&pnob->mcc_cq_obj);
+		pnob->mcc_cq_created = 0;
+	}
+
+	if (pnob->event_q_created) {
+		status = be_eq_destroy(&pnob->event_q_obj);
+		pnob->event_q_created = 0;
+	}
+	be_function_cleanup(&pnob->fn_obj);
+}
+
+/*
+ * free all resources associated with a pnob
+ * Called at the time of module cleanup as well a any error during
+ * module init.  Some resources may be partially allocated in a NetObj.
+ */
+static void netobject_cleanup(struct be_adapter *adapter,
+			struct be_net_object *pnob)
+{
+	struct net_device *netdev = adapter->netdevp;
+
+	if (netif_running(netdev)) {
+		netif_stop_queue(netdev);
+		be_wait_nic_tx_cmplx_cmpl(pnob);
+		be_disable_eq_intr(pnob);
+	}
+
+	be_unregister_isr(adapter);
+
+	if (adapter->tasklet_started) {
+		tasklet_kill(&(adapter->sts_handler));
+		adapter->tasklet_started = 0;
+	}
+	if (pnob->fn_obj_created)
+		be_disable_intr(pnob);
+
+	if (adapter->dev_state != BE_DEV_STATE_NONE)
+		unregister_netdev(netdev);
+
+	if (pnob->fn_obj_created)
+		be_destroy_netobj(pnob);
+
+	adapter->net_obj = NULL;
+	adapter->netdevp = NULL;
+
+	be_rx_q_clean(pnob);
+	if (pnob->rx_ctxt) {
+		kfree(pnob->rx_page_info);
+		kfree(pnob->rx_ctxt);
+	}
+
+	be_tx_q_clean(pnob);
+	kfree(pnob->tx_ctxt);
+
+	if (pnob->mcc_q)
+		pci_free_consistent(adapter->pdev, pnob->mcc_q_size,
+			pnob->mcc_q, pnob->mcc_q_bus);
+
+	if (pnob->mcc_wrb_ctxt)
+		free_pages((unsigned long)pnob->mcc_wrb_ctxt,
+			   get_order(pnob->mcc_wrb_ctxt_size));
+
+	if (pnob->mcc_cq)
+		pci_free_consistent(adapter->pdev, pnob->mcc_cq_size,
+			pnob->mcc_cq, pnob->mcc_cq_bus);
+
+	if (pnob->event_q)
+		pci_free_consistent(adapter->pdev, pnob->event_q_size,
+			pnob->event_q, pnob->event_q_bus);
+
+	if (pnob->tx_cq)
+		pci_free_consistent(adapter->pdev, pnob->tx_cq_size,
+			pnob->tx_cq, pnob->tx_cq_bus);
+
+	if (pnob->tx_q)
+		pci_free_consistent(adapter->pdev, pnob->tx_q_size,
+			pnob->tx_q, pnob->tx_q_bus);
+
+	if (pnob->rx_q)
+		pci_free_consistent(adapter->pdev, pnob->rx_q_size,
+			pnob->rx_q, pnob->rx_q_bus);
+
+	if (pnob->rx_cq)
+		pci_free_consistent(adapter->pdev, pnob->rx_cq_size,
+			pnob->rx_cq, pnob->rx_cq_bus);
+
+
+	if (pnob->mb_ptr)
+		pci_free_consistent(adapter->pdev, pnob->mb_size, pnob->mb_ptr,
+			pnob->mb_bus);
+
+	free_netdev(netdev);
+}
+
+
+static int be_nob_ring_alloc(struct be_adapter *adapter,
+	struct be_net_object *pnob)
+{
+	u32 size;
+
+	/* Mail box rd; mailbox pointer needs to be 16 byte aligned */
+	pnob->mb_size = sizeof(struct MCC_MAILBOX_AMAP) + 16;
+	pnob->mb_ptr = pci_alloc_consistent(adapter->pdev, pnob->mb_size,
+				&pnob->mb_bus);
+	if (!pnob->mb_bus)
+		return -1;
+	memset(pnob->mb_ptr, 0, pnob->mb_size);
+	pnob->mb_rd.va = PTR_ALIGN(pnob->mb_ptr, 16);
+	pnob->mb_rd.pa = PTR_ALIGN(pnob->mb_bus, 16);
+	pnob->mb_rd.length = sizeof(struct MCC_MAILBOX_AMAP);
+	/*
+	 * Event queue
+	 */
+	pnob->event_q_len = EVENT_Q_LEN;
+	pnob->event_q_size = pnob->event_q_len * sizeof(struct EQ_ENTRY_AMAP);
+	pnob->event_q = pci_alloc_consistent(adapter->pdev, pnob->event_q_size,
+				&pnob->event_q_bus);
+	if (!pnob->event_q_bus)
+		return -1;
+	memset(pnob->event_q, 0, pnob->event_q_size);
+	/*
+	 * Eth TX queue
+	 */
+	pnob->tx_q_len = ETH_TXQ_LEN;
+	pnob->tx_q_port = 0;
+	pnob->tx_q_size =  pnob->tx_q_len * sizeof(struct ETH_WRB_AMAP);
+	pnob->tx_q = pci_alloc_consistent(adapter->pdev, pnob->tx_q_size,
+				&pnob->tx_q_bus);
+	if (!pnob->tx_q_bus)
+		return -1;
+	memset(pnob->tx_q, 0, pnob->tx_q_size);
+	/*
+	 * Eth TX Compl queue
+	 */
+	pnob->txcq_len = ETH_TXCQ_LEN;
+	pnob->tx_cq_size = pnob->txcq_len * sizeof(struct ETH_TX_COMPL_AMAP);
+	pnob->tx_cq = pci_alloc_consistent(adapter->pdev, pnob->tx_cq_size,
+				&pnob->tx_cq_bus);
+	if (!pnob->tx_cq_bus)
+		return -1;
+	memset(pnob->tx_cq, 0, pnob->tx_cq_size);
+	/*
+	 * Eth RX queue
+	 */
+	pnob->rx_q_len = ETH_RXQ_LEN;
+	pnob->rx_q_size =  pnob->rx_q_len * sizeof(struct ETH_RX_D_AMAP);
+	pnob->rx_q = pci_alloc_consistent(adapter->pdev, pnob->rx_q_size,
+				&pnob->rx_q_bus);
+	if (!pnob->rx_q_bus)
+		return -1;
+	memset(pnob->rx_q, 0, pnob->rx_q_size);
+	/*
+	 * Eth Unicast RX Compl queue
+	 */
+	pnob->rx_cq_len = ETH_UC_RXCQ_LEN;
+	pnob->rx_cq_size =  pnob->rx_cq_len *
+			sizeof(struct ETH_RX_COMPL_AMAP);
+	pnob->rx_cq = pci_alloc_consistent(adapter->pdev, pnob->rx_cq_size,
+				&pnob->rx_cq_bus);
+	if (!pnob->rx_cq_bus)
+		return -1;
+	memset(pnob->rx_cq, 0, pnob->rx_cq_size);
+
+	/* TX resources */
+	size = pnob->tx_q_len * sizeof(void **);
+	pnob->tx_ctxt = kzalloc(size, GFP_KERNEL);
+	if (pnob->tx_ctxt == NULL)
+		return -1;
+
+	/* RX resources */
+	size = pnob->rx_q_len * sizeof(void *);
+	pnob->rx_ctxt = kzalloc(size, GFP_KERNEL);
+	if (pnob->rx_ctxt == NULL)
+		return -1;
+
+	size = (pnob->rx_q_len * sizeof(struct be_rx_page_info));
+	pnob->rx_page_info = kzalloc(size, GFP_KERNEL);
+	if (pnob->rx_page_info == NULL)
+		return -1;
+
+	adapter->eth_statsp = kzalloc(sizeof(struct FWCMD_ETH_GET_STATISTICS),
+				GFP_KERNEL);
+	if (adapter->eth_statsp == NULL)
+		return -1;
+	pnob->rx_buf_size = rxbuf_size;
+	return 0;
+}
+
+/*
+    This function initializes the be_net_object for subsequent
+    network operations.
+
+    Before calling this function, the driver  must have allocated
+    space for the NetObject structure, initialized the structure,
+    allocated DMAable memory for all the network queues that form
+    part of the NetObject and populated the start address (virtual)
+    and number of entries allocated for each queue in the NetObject structure.
+
+    The driver must also have allocated memory to hold the
+    mailbox structure (MCC_MAILBOX) and post the physical address,
+    virtual addresses and the size of the mailbox memory in the
+    NetObj.mb_rd.  This structure is used by BECLIB for
+    initial communication with the embedded MCC processor. BECLIB
+    uses the mailbox until MCC rings are created for  more  efficient
+    communication with the MCC processor.
+
+    If the driver wants to create multiple network interface for more
+    than one protection domain, it can call be_create_netobj()
+    multiple times  once for each protection domain.  A Maximum of
+    32 protection domains are supported.
+
+*/
+static int
+be_create_netobj(struct be_net_object *pnob, u8 __iomem *csr_va,
+	u8 __iomem *db_va, u8 __iomem *pci_va)
+{
+	int status = 0;
+	bool  eventable = false, tx_no_delay = false, rx_no_delay = false;
+	struct be_eq_object *eq_objectp = NULL;
+	struct be_function_object *pfob = &pnob->fn_obj;
+	struct ring_desc rd;
+	u32 set_rxbuf_size;
+	u32 tx_cmpl_wm = CEV_WMARK_96;	/* 0xffffffff to disable */
+	u32 rx_cmpl_wm = CEV_WMARK_160;	/* 0xffffffff to disable */
+	u32 eq_delay = 0; /* delay in 8usec units. 0xffffffff to disable */
+
+	memset(&rd, 0, sizeof(struct ring_desc));
+
+	status = be_function_object_create(csr_va, db_va, pci_va,
+			BE_FUNCTION_TYPE_NETWORK, &pnob->mb_rd, pfob);
+	if (status != BE_SUCCESS)
+		return status;
+	pnob->fn_obj_created = true;
+
+	if (tx_cmpl_wm == 0xffffffff)
+		tx_no_delay = true;
+	if (rx_cmpl_wm == 0xffffffff)
+		rx_no_delay = true;
+	/*
+	 * now create the necessary rings
+	 * Event Queue first.
+	 */
+	if (pnob->event_q_len) {
+		rd.va = pnob->event_q;
+		rd.pa = pnob->event_q_bus;
+		rd.length = pnob->event_q_size;
+
+		status = be_eq_create(pfob, &rd, 4, pnob->event_q_len,
+				(u32) -1,	/* CEV_WMARK_* or -1 */
+				eq_delay,	/* in 8us units, or -1 */
+				&pnob->event_q_obj);
+		if (status != BE_SUCCESS)
+			goto error_ret;
+		pnob->event_q_id = pnob->event_q_obj.eq_id;
+		pnob->event_q_created = 1;
+		eventable = true;
+		eq_objectp = &pnob->event_q_obj;
+	}
+	/*
+	 * Now Eth Tx Compl. queue.
+	 */
+	if (pnob->txcq_len) {
+		rd.va = pnob->tx_cq;
+		rd.pa = pnob->tx_cq_bus;
+		rd.length = pnob->tx_cq_size;
+
+		status = be_cq_create(pfob, &rd,
+			pnob->txcq_len * sizeof(struct ETH_TX_COMPL_AMAP),
+			false,	/* solicted events,  */
+			tx_no_delay,	/* nodelay  */
+			tx_cmpl_wm,	/* Watermark encodings */
+			eq_objectp, &pnob->tx_cq_obj);
+		if (status != BE_SUCCESS)
+			goto error_ret;
+
+		pnob->tx_cq_id = pnob->tx_cq_obj.cq_id;
+		pnob->tx_cq_created = 1;
+	}
+	/*
+	 * Eth Tx queue
+	 */
+	if (pnob->tx_q_len) {
+		struct be_eth_sq_parameters ex_params = { 0 };
+		u32 type;
+
+		if (pnob->tx_q_port) {
+			/* TXQ to be bound to a specific port */
+			type = BE_ETH_TX_RING_TYPE_BOUND;
+			ex_params.port = pnob->tx_q_port - 1;
+		} else
+			type = BE_ETH_TX_RING_TYPE_STANDARD;
+
+		rd.va = pnob->tx_q;
+		rd.pa = pnob->tx_q_bus;
+		rd.length = pnob->tx_q_size;
+
+		status = be_eth_sq_create_ex(pfob, &rd,
+				pnob->tx_q_len * sizeof(struct ETH_WRB_AMAP),
+				type, 2, &pnob->tx_cq_obj,
+				&ex_params, &pnob->tx_q_obj);
+
+		if (status != BE_SUCCESS)
+			goto error_ret;
+
+		pnob->tx_q_id = pnob->tx_q_obj.bid;
+		pnob->tx_q_created = 1;
+	}
+	/*
+	 * Now Eth Rx compl. queue.  Always needed.
+	 */
+	rd.va = pnob->rx_cq;
+	rd.pa = pnob->rx_cq_bus;
+	rd.length = pnob->rx_cq_size;
+
+	status = be_cq_create(pfob, &rd,
+			pnob->rx_cq_len * sizeof(struct ETH_RX_COMPL_AMAP),
+			false,	/* solicted events,  */
+			rx_no_delay,	/* nodelay  */
+			rx_cmpl_wm,	/* Watermark encodings */
+			eq_objectp, &pnob->rx_cq_obj);
+	if (status != BE_SUCCESS)
+		goto error_ret;
+
+	pnob->rx_cq_id = pnob->rx_cq_obj.cq_id;
+	pnob->rx_cq_created = 1;
+
+	status = be_eth_rq_set_frag_size(pfob, pnob->rx_buf_size,
+			(u32 *) &set_rxbuf_size);
+	if (status != BE_SUCCESS) {
+		be_eth_rq_get_frag_size(pfob, (u32 *) &pnob->rx_buf_size);
+		if ((pnob->rx_buf_size != 2048) && (pnob->rx_buf_size != 4096)
+		    && (pnob->rx_buf_size != 8192))
+			goto error_ret;
+	} else {
+		if (pnob->rx_buf_size != set_rxbuf_size)
+			pnob->rx_buf_size = set_rxbuf_size;
+	}
+	/*
+	 * Eth RX queue. be_eth_rq_create() always assumes 2 pages size
+	 */
+	rd.va = pnob->rx_q;
+	rd.pa = pnob->rx_q_bus;
+	rd.length = pnob->rx_q_size;
+
+	status = be_eth_rq_create(pfob, &rd, &pnob->rx_cq_obj,
+			     &pnob->rx_cq_obj, &pnob->rx_q_obj);
+
+	if (status != BE_SUCCESS)
+		goto error_ret;
+
+	pnob->rx_q_id = pnob->rx_q_obj.rid;
+	pnob->rx_q_created = 1;
+
+	return BE_SUCCESS;	/* All required queues created. */
+
+error_ret:
+	be_destroy_netobj(pnob);
+	return status;
+}
+
+static int be_nob_ring_init(struct be_adapter *adapter,
+				struct be_net_object *pnob)
+{
+	int status;
+
+	pnob->event_q_tl = 0;
+
+	pnob->tx_q_hd = 0;
+	pnob->tx_q_tl = 0;
+
+	pnob->tx_cq_tl = 0;
+
+	pnob->rx_cq_tl = 0;
+
+	memset(pnob->event_q, 0, pnob->event_q_size);
+	memset(pnob->tx_cq, 0, pnob->tx_cq_size);
+	memset(pnob->tx_ctxt, 0, pnob->tx_q_len * sizeof(void **));
+	memset(pnob->rx_ctxt, 0, pnob->rx_q_len * sizeof(void *));
+	pnob->rx_pg_info_hd = 0;
+	pnob->rx_q_hd = 0;
+	atomic_set(&pnob->rx_q_posted, 0);
+
+	status = be_create_netobj(pnob, adapter->csr_va, adapter->db_va,
+				adapter->pci_va);
+	if (status != BE_SUCCESS)
+		return -1;
+
+	be_post_eth_rx_buffs(pnob);
+	return 0;
+}
+
+/* This function handles async callback for link status */
+static void
+be_link_status_async_callback(void *context, u32 event_code, void *event)
+{
+	struct ASYNC_EVENT_LINK_STATE_AMAP *link_status = event;
+	struct be_adapter *adapter = context;
+	bool link_enable = false;
+	struct be_net_object *pnob;
+	struct ASYNC_EVENT_TRAILER_AMAP *async_trailer;
+	struct net_device *netdev;
+	u32 async_event_code, async_event_type, active_port;
+	u32 port0_link_status, port1_link_status, port0_duplex, port1_duplex;
+	u32 port0_speed, port1_speed;
+
+	if (event_code != ASYNC_EVENT_CODE_LINK_STATE) {
+		/* Not our event to handle */
+		return;
+	}
+	async_trailer = (struct ASYNC_EVENT_TRAILER_AMAP *)
+	    ((u8 *) event + sizeof(struct MCC_CQ_ENTRY_AMAP) -
+	     sizeof(struct ASYNC_EVENT_TRAILER_AMAP));
+
+	async_event_code = AMAP_GET_BITS_PTR(ASYNC_EVENT_TRAILER, event_code,
+					     async_trailer);
+	BUG_ON(async_event_code != ASYNC_EVENT_CODE_LINK_STATE);
+
+	pnob = adapter->net_obj;
+	netdev = pnob->netdev;
+
+	/* Determine if this event is a switch VLD or a physical link event */
+	async_event_type = AMAP_GET_BITS_PTR(ASYNC_EVENT_TRAILER, event_type,
+					     async_trailer);
+	active_port = AMAP_GET_BITS_PTR(ASYNC_EVENT_LINK_STATE,
+					active_port, link_status);
+	port0_link_status = AMAP_GET_BITS_PTR(ASYNC_EVENT_LINK_STATE,
+					      port0_link_status, link_status);
+	port1_link_status = AMAP_GET_BITS_PTR(ASYNC_EVENT_LINK_STATE,
+					      port1_link_status, link_status);
+	port0_duplex = AMAP_GET_BITS_PTR(ASYNC_EVENT_LINK_STATE,
+					 port0_duplex, link_status);
+	port1_duplex = AMAP_GET_BITS_PTR(ASYNC_EVENT_LINK_STATE,
+					 port1_duplex, link_status);
+	port0_speed = AMAP_GET_BITS_PTR(ASYNC_EVENT_LINK_STATE,
+					port0_speed, link_status);
+	port1_speed = AMAP_GET_BITS_PTR(ASYNC_EVENT_LINK_STATE,
+					port1_speed, link_status);
+	if (async_event_type == NTWK_LINK_TYPE_VIRTUAL) {
+		adapter->be_stat.bes_link_change_virtual++;
+		if (adapter->be_link_sts->active_port != active_port) {
+			dev_notice(&netdev->dev,
+			       "Active port changed due to VLD on switch\n");
+		} else {
+			dev_notice(&netdev->dev, "Link status update\n");
+		}
+
+	} else {
+		adapter->be_stat.bes_link_change_physical++;
+		if (adapter->be_link_sts->active_port != active_port) {
+			dev_notice(&netdev->dev,
+			       "Active port changed due to port link"
+			       " status change\n");
+		} else {
+			dev_notice(&netdev->dev, "Link status update\n");
+		}
+	}
+
+	memset(adapter->be_link_sts, 0, sizeof(adapter->be_link_sts));
+
+	if ((port0_link_status == ASYNC_EVENT_LINK_UP) ||
+	    (port1_link_status == ASYNC_EVENT_LINK_UP)) {
+		if ((adapter->port0_link_sts == BE_PORT_LINK_DOWN) &&
+		    (adapter->port1_link_sts == BE_PORT_LINK_DOWN)) {
+			/* Earlier both the ports are down So link is up */
+			link_enable = true;
+		}
+
+		if (port0_link_status == ASYNC_EVENT_LINK_UP) {
+			adapter->port0_link_sts = BE_PORT_LINK_UP;
+			adapter->be_link_sts->mac0_duplex = port0_duplex;
+			adapter->be_link_sts->mac0_speed = port0_speed;
+			if (active_port == NTWK_PORT_A)
+				adapter->be_link_sts->active_port = 0;
+		} else
+			adapter->port0_link_sts = BE_PORT_LINK_DOWN;
+
+		if (port1_link_status == ASYNC_EVENT_LINK_UP) {
+			adapter->port1_link_sts = BE_PORT_LINK_UP;
+			adapter->be_link_sts->mac1_duplex = port1_duplex;
+			adapter->be_link_sts->mac1_speed = port1_speed;
+			if (active_port == NTWK_PORT_B)
+				adapter->be_link_sts->active_port = 1;
+		} else
+			adapter->port1_link_sts = BE_PORT_LINK_DOWN;
+
+		printk(KERN_INFO "Link Properties for %s:\n", netdev->name);
+		dev_info(&netdev->dev, "Link Properties:\n");
+		be_print_link_info(adapter->be_link_sts);
+
+		if (!link_enable)
+			return;
+		/*
+		 * Both ports were down previously, but atleast one of
+		 * them has come up if this netdevice's carrier is not up,
+		 * then indicate to stack
+		 */
+		if (!netif_carrier_ok(netdev)) {
+			netif_start_queue(netdev);
+			netif_carrier_on(netdev);
+		}
+		return;
+	}
+
+	/* Now both the ports are down. Tell the stack about it */
+	dev_info(&netdev->dev, "Both ports are down\n");
+	adapter->port0_link_sts = BE_PORT_LINK_DOWN;
+	adapter->port1_link_sts = BE_PORT_LINK_DOWN;
+	if (netif_carrier_ok(netdev)) {
+		netif_carrier_off(netdev);
+		netif_stop_queue(netdev);
+	}
+	return;
+}
+
+static int be_mcc_create(struct be_adapter *adapter)
+{
+	struct be_net_object *pnob;
+
+	pnob = adapter->net_obj;
+	/*
+	 * Create the MCC ring so that all further communication with
+	 * MCC can go thru the ring. we do this at the end since
+	 * we do not want to be dealing with interrupts until the
+	 * initialization is complete.
+	 */
+	pnob->mcc_q_len = MCC_Q_LEN;
+	pnob->mcc_q_size = pnob->mcc_q_len * sizeof(struct MCC_WRB_AMAP);
+	pnob->mcc_q =  pci_alloc_consistent(adapter->pdev, pnob->mcc_q_size,
+				&pnob->mcc_q_bus);
+	if (!pnob->mcc_q_bus)
+		return -1;
+	/*
+	 * space for MCC WRB context
+	 */
+	pnob->mcc_wrb_ctxtLen = MCC_Q_LEN;
+	pnob->mcc_wrb_ctxt_size =  pnob->mcc_wrb_ctxtLen *
+		sizeof(struct be_mcc_wrb_context);
+	pnob->mcc_wrb_ctxt = (void *)__get_free_pages(GFP_KERNEL,
+		get_order(pnob->mcc_wrb_ctxt_size));
+	if (pnob->mcc_wrb_ctxt == NULL)
+		return -1;
+	/*
+	 * Space for MCC compl. ring
+	 */
+	pnob->mcc_cq_len = MCC_CQ_LEN;
+	pnob->mcc_cq_size = pnob->mcc_cq_len * sizeof(struct MCC_CQ_ENTRY_AMAP);
+	pnob->mcc_cq = pci_alloc_consistent(adapter->pdev, pnob->mcc_cq_size,
+				&pnob->mcc_cq_bus);
+	if (!pnob->mcc_cq_bus)
+		return -1;
+	return 0;
+}
+
+/*
+    This function creates the MCC request and completion ring required
+    for communicating with the ARM processor.  The caller must have
+    allocated required amount of memory for the MCC ring and MCC
+    completion ring and posted the virtual address and number of
+    entries in the corresponding members (mcc_q and mcc_cq) in the
+    NetObject struture.
+
+    When this call is completed, all further communication with
+    ARM will switch from mailbox to this ring.
+
+    pnob	- Pointer to the NetObject structure. This NetObject should
+		  have been created using a previous call to be_create_netobj()
+*/
+int be_create_mcc_rings(struct be_net_object *pnob)
+{
+	int status = 0;
+	struct ring_desc rd;
+	struct be_function_object *pfob = &pnob->fn_obj;
+
+	memset(&rd, 0, sizeof(struct ring_desc));
+	if (pnob->mcc_cq_len) {
+		rd.va = pnob->mcc_cq;
+		rd.pa = pnob->mcc_cq_bus;
+		rd.length = pnob->mcc_cq_size;
+
+		status = be_cq_create(pfob, &rd,
+			pnob->mcc_cq_len * sizeof(struct MCC_CQ_ENTRY_AMAP),
+			false,	/* solicted events,  */
+			true,	/* nodelay  */
+			0,	/* 0 Watermark since Nodelay is true */
+			&pnob->event_q_obj,
+			&pnob->mcc_cq_obj);
+
+		if (status != BE_SUCCESS)
+			return status;
+
+		pnob->mcc_cq_id = pnob->mcc_cq_obj.cq_id;
+		pnob->mcc_cq_created = 1;
+	}
+	if (pnob->mcc_q_len) {
+		rd.va = pnob->mcc_q;
+		rd.pa = pnob->mcc_q_bus;
+		rd.length = pnob->mcc_q_size;
+
+		status = be_mcc_ring_create(pfob, &rd,
+				pnob->mcc_q_len * sizeof(struct MCC_WRB_AMAP),
+				pnob->mcc_wrb_ctxt, pnob->mcc_wrb_ctxtLen,
+				&pnob->mcc_cq_obj, &pnob->mcc_q_obj);
+
+		if (status != BE_SUCCESS)
+			return status;
+
+		pnob->mcc_q_created = 1;
+	}
+	return BE_SUCCESS;
+}
+
+static int be_mcc_init(struct be_adapter *adapter)
+{
+	u32 r;
+	struct be_net_object *pnob;
+
+	pnob = adapter->net_obj;
+	memset(pnob->mcc_q, 0, pnob->mcc_q_size);
+	pnob->mcc_q_hd = 0;
+
+	memset(pnob->mcc_wrb_ctxt, 0, pnob->mcc_wrb_ctxt_size);
+
+	memset(pnob->mcc_cq, 0, pnob->mcc_cq_size);
+	pnob->mcc_cq_tl = 0;
+
+	r = be_create_mcc_rings(adapter->net_obj);
+	if (r != BE_SUCCESS)
+		return -1;
+
+	return 0;
+}
+
+static void be_remove(struct pci_dev *pdev)
+{
+	struct be_net_object *pnob;
+	struct be_adapter *adapter;
+
+	adapter = pci_get_drvdata(pdev);
+	if (!adapter)
+		return;
+
+	pci_set_drvdata(pdev, NULL);
+	pnob = (struct be_net_object *)adapter->net_obj;
+
+	flush_scheduled_work();
+
+	if (pnob) {
+		/* Unregister async callback function for link status updates */
+		if (pnob->mcc_q_created)
+			be_mcc_add_async_event_callback(&pnob->mcc_q_obj,
+								NULL, NULL);
+		netobject_cleanup(adapter, pnob);
+	}
+
+	if (adapter->csr_va)
+		iounmap(adapter->csr_va);
+	if (adapter->db_va)
+		iounmap(adapter->db_va);
+	if (adapter->pci_va)
+		iounmap(adapter->pci_va);
+
+	pci_release_regions(adapter->pdev);
+	pci_disable_device(adapter->pdev);
+
+	kfree(adapter->be_link_sts);
+	kfree(adapter->eth_statsp);
+
+	if (adapter->timer_ctxt.get_stats_timer.function)
+		del_timer_sync(&adapter->timer_ctxt.get_stats_timer);
+	kfree(adapter);
+}
+
+/*
+ * This function is called by the PCI sub-system when it finds a PCI
+ * device with dev/vendor IDs that match with one of our devices.
+ * All of the driver initialization is done in this function.
+ */
+static int be_probe(struct pci_dev *pdev, const struct pci_device_id *pdev_id)
+{
+	int status = 0;
+	struct be_adapter *adapter;
+	struct FWCMD_COMMON_GET_FW_VERSION_RESPONSE_PAYLOAD get_fwv;
+	struct be_net_object *pnob;
+	struct net_device *netdev;
+
+	status = pci_enable_device(pdev);
+	if (status)
+		goto error;
+
+	status = pci_request_regions(pdev, be_driver_name);
+	if (status)
+		goto error_pci_req;
+
+	pci_set_master(pdev);
+	adapter = kzalloc(sizeof(struct be_adapter), GFP_KERNEL);
+	if (adapter == NULL) {
+		status = -ENOMEM;
+		goto error_adapter;
+	}
+	adapter->dev_state = BE_DEV_STATE_NONE;
+	adapter->pdev = pdev;
+	pci_set_drvdata(pdev, adapter);
+
+	adapter->enable_aic = 1;
+	adapter->max_eqd = MAX_EQD;
+	adapter->min_eqd = 0;
+	adapter->cur_eqd = 0;
+
+	status = pci_set_dma_mask(pdev, DMA_64BIT_MASK);
+	if (!status) {
+		adapter->dma_64bit_cap = true;
+	} else {
+		adapter->dma_64bit_cap = false;
+		status = pci_set_dma_mask(pdev, DMA_32BIT_MASK);
+		if (status != 0) {
+			printk(KERN_ERR "Could not set PCI DMA Mask\n");
+			goto cleanup;
+		}
+	}
+
+	status = init_pci_be_function(adapter, pdev);
+	if (status != 0) {
+		printk(KERN_ERR "Failed to map PCI BARS\n");
+		status = -ENOMEM;
+		goto cleanup;
+	}
+
+	be_trace_set_level(DL_ALWAYS | DL_ERR);
+
+	adapter->be_link_sts = kmalloc(sizeof(struct BE_LINK_STATUS),
+					GFP_KERNEL);
+	if (adapter->be_link_sts == NULL) {
+		printk(KERN_ERR "Memory allocation for link status "
+		       "buffer failed\n");
+		goto cleanup;
+	}
+	spin_lock_init(&adapter->txq_lock);
+
+	netdev = alloc_etherdev(sizeof(struct be_net_object));
+	if (netdev == NULL) {
+		status = -ENOMEM;
+		goto cleanup;
+	}
+	pnob = netdev_priv(netdev);
+	adapter->net_obj = pnob;
+	adapter->netdevp = netdev;
+	pnob->adapter = adapter;
+	pnob->netdev = netdev;
+
+	status = be_nob_ring_alloc(adapter, pnob);
+	if (status != 0)
+		goto cleanup;
+
+	status = be_nob_ring_init(adapter, pnob);
+	if (status != 0)
+		goto cleanup;
+
+	be_rxf_mac_address_read_write(&pnob->fn_obj, false, false, false,
+		false, false, netdev->dev_addr, NULL, NULL);
+
+	netdev->init = &benet_init;
+	netif_carrier_off(netdev);
+	netif_stop_queue(netdev);
+
+	SET_NETDEV_DEV(netdev, &(adapter->pdev->dev));
+
+	netif_napi_add(netdev, &pnob->napi, be_poll, 64);
+
+	/* if the rx_frag size if 2K, one page is shared as two RX frags */
+	pnob->rx_pg_shared =
+		(pnob->rx_buf_size <= PAGE_SIZE / 2) ? true : false;
+	if (pnob->rx_buf_size != rxbuf_size) {
+		printk(KERN_WARNING
+		       "Could not set Rx buffer size to %d. Using %d\n",
+				       rxbuf_size, pnob->rx_buf_size);
+		rxbuf_size = pnob->rx_buf_size;
+	}
+
+	tasklet_init(&(adapter->sts_handler), be_process_intr,
+		     (unsigned long)adapter);
+	adapter->tasklet_started = 1;
+	spin_lock_init(&(adapter->int_lock));
+
+	status = be_register_isr(adapter, pnob);
+	if (status != 0)
+		goto cleanup;
+
+	adapter->rx_csum = 1;
+	adapter->max_rx_coal = BE_LRO_MAX_PKTS;
+
+	memset(&get_fwv, 0,
+	       sizeof(struct FWCMD_COMMON_GET_FW_VERSION_RESPONSE_PAYLOAD));
+	printk(KERN_INFO "BladeEngine Driver version:%s. "
+	       "Copyright ServerEngines, Corporation 2005 - 2008\n",
+			       be_drvr_ver);
+	status = be_function_get_fw_version(&pnob->fn_obj, &get_fwv, NULL,
+					    NULL);
+	if (status == BE_SUCCESS) {
+		strncpy(be_fw_ver, get_fwv.firmware_version_string, 32);
+		printk(KERN_INFO "BladeEngine Firmware Version:%s\n",
+		       get_fwv.firmware_version_string);
+	} else {
+		printk(KERN_WARNING "Unable to get BE Firmware Version\n");
+	}
+
+	sema_init(&adapter->get_eth_stat_sem, 0);
+	init_timer(&adapter->timer_ctxt.get_stats_timer);
+	atomic_set(&adapter->timer_ctxt.get_stat_flag, 0);
+	adapter->timer_ctxt.get_stats_timer.function =
+	    &be_get_stats_timer_handler;
+
+	status = be_mcc_create(adapter);
+	if (status < 0)
+		goto cleanup;
+	status = be_mcc_init(adapter);
+	if (status < 0)
+		goto cleanup;
+
+
+	status = be_mcc_add_async_event_callback(&adapter->net_obj->mcc_q_obj,
+			 be_link_status_async_callback, (void *)adapter);
+	if (status != BE_SUCCESS) {
+		printk(KERN_WARNING "add_async_event_callback failed");
+		printk(KERN_WARNING
+		       "Link status changes may not be reflected\n");
+	}
+
+	status = register_netdev(netdev);
+	if (status != 0)
+		goto cleanup;
+	be_update_link_status(adapter);
+	adapter->dev_state = BE_DEV_STATE_INIT;
+	return 0;
+
+cleanup:
+	be_remove(pdev);
+	return status;
+error_adapter:
+	pci_release_regions(pdev);
+error_pci_req:
+	pci_disable_device(pdev);
+error:
+	printk(KERN_ERR "BladeEngine initalization failed\n");
+	return status;
+}
+
+/*
+ * Get the current link status and print the status on console
+ */
+void be_update_link_status(struct be_adapter *adapter)
+{
+	int status;
+	struct be_net_object *pnob = adapter->net_obj;
+
+	status = be_rxf_link_status(&pnob->fn_obj, adapter->be_link_sts, NULL,
+			NULL, NULL);
+	if (status == BE_SUCCESS) {
+		if (adapter->be_link_sts->mac0_speed &&
+		    adapter->be_link_sts->mac0_duplex)
+			adapter->port0_link_sts = BE_PORT_LINK_UP;
+		else
+			adapter->port0_link_sts = BE_PORT_LINK_DOWN;
+
+		if (adapter->be_link_sts->mac1_speed &&
+		    adapter->be_link_sts->mac1_duplex)
+			adapter->port1_link_sts = BE_PORT_LINK_UP;
+		else
+			adapter->port1_link_sts = BE_PORT_LINK_DOWN;
+
+		dev_info(&pnob->netdev->dev, "Link Properties:\n");
+		be_print_link_info(adapter->be_link_sts);
+		return;
+	}
+	dev_info(&pnob->netdev->dev, "Could not get link status\n");
+	return;
+}
+
+
+#ifdef CONFIG_PM
+static void
+be_pm_cleanup(struct be_adapter *adapter,
+	      struct be_net_object *pnob, struct net_device *netdev)
+{
+	netif_carrier_off(netdev);
+	netif_stop_queue(netdev);
+
+	be_wait_nic_tx_cmplx_cmpl(pnob);
+	be_disable_eq_intr(pnob);
+
+	if (adapter->tasklet_started) {
+		tasklet_kill(&adapter->sts_handler);
+		adapter->tasklet_started = 0;
+	}
+
+	be_unregister_isr(adapter);
+	be_disable_intr(pnob);
+
+	be_tx_q_clean(pnob);
+	be_rx_q_clean(pnob);
+
+	be_destroy_netobj(pnob);
+}
+
+static int be_suspend(struct pci_dev *pdev, pm_message_t state)
+{
+	struct be_adapter *adapter = pci_get_drvdata(pdev);
+	struct net_device *netdev =  adapter->netdevp;
+	struct be_net_object *pnob = netdev_priv(netdev);
+
+	adapter->dev_pm_state = adapter->dev_state;
+	adapter->dev_state = BE_DEV_STATE_SUSPEND;
+
+	netif_device_detach(netdev);
+	if (netif_running(netdev))
+		be_pm_cleanup(adapter, pnob, netdev);
+
+	pci_enable_wake(pdev, 3, 1);
+	pci_enable_wake(pdev, 4, 1);	/* D3 Cold = 4 */
+	pci_save_state(pdev);
+	pci_disable_device(pdev);
+	pci_set_power_state(pdev, pci_choose_state(pdev, state));
+	return 0;
+}
+
+static void be_up(struct be_adapter *adapter)
+{
+	struct be_net_object *pnob = adapter->net_obj;
+
+	if (pnob->num_vlans != 0)
+		be_rxf_vlan_config(&pnob->fn_obj, false, pnob->num_vlans,
+			pnob->vlan_tag, NULL, NULL, NULL);
+
+}
+
+static int be_resume(struct pci_dev *pdev)
+{
+	int status = 0;
+	struct be_adapter *adapter = pci_get_drvdata(pdev);
+	struct net_device *netdev =  adapter->netdevp;
+	struct be_net_object *pnob = netdev_priv(netdev);
+
+	netif_device_detach(netdev);
+
+	status = pci_enable_device(pdev);
+	if (status)
+		return status;
+
+	pci_set_power_state(pdev, 0);
+	pci_restore_state(pdev);
+	pci_enable_wake(pdev, 3, 0);
+	pci_enable_wake(pdev, 4, 0);	/* 4 is D3 cold */
+
+	netif_carrier_on(netdev);
+	netif_start_queue(netdev);
+
+	if (netif_running(netdev)) {
+		be_rxf_mac_address_read_write(&pnob->fn_obj, false, false,
+			false, true, false, netdev->dev_addr, NULL, NULL);
+
+		status = be_nob_ring_init(adapter, pnob);
+		if (status < 0)
+			return status;
+
+		tasklet_init(&(adapter->sts_handler), be_process_intr,
+			     (unsigned long)adapter);
+		adapter->tasklet_started = 1;
+
+		if (be_register_isr(adapter, pnob) != 0) {
+			printk(KERN_ERR "be_register_isr failed\n");
+			return status;
+		}
+
+
+		status = be_mcc_init(adapter);
+		if (status < 0) {
+			printk(KERN_ERR "be_mcc_init failed\n");
+			return status;
+		}
+		be_update_link_status(adapter);
+		/*
+		 * Register async call back function to handle link
+		 * status updates
+		 */
+		status = be_mcc_add_async_event_callback(
+				&adapter->net_obj->mcc_q_obj,
+				be_link_status_async_callback, (void *)adapter);
+		if (status != BE_SUCCESS) {
+			printk(KERN_WARNING "add_async_event_callback failed");
+			printk(KERN_WARNING
+			       "Link status changes may not be reflected\n");
+		}
+		be_enable_intr(pnob);
+		be_enable_eq_intr(pnob);
+		be_up(adapter);
+	}
+	netif_device_attach(netdev);
+	adapter->dev_state = adapter->dev_pm_state;
+	return 0;
+
+}
+
+#endif
+
+/* Wait until no more pending transmits  */
+void be_wait_nic_tx_cmplx_cmpl(struct be_net_object *pnob)
+{
+	int i;
+
+	/* Wait for 20us * 50000 (= 1s) and no more */
+	i = 0;
+	while ((pnob->tx_q_tl != pnob->tx_q_hd) && (i < 50000)) {
+		++i;
+		udelay(20);
+	}
+
+	/* Check for no more pending transmits */
+	if (i >= 50000) {
+		printk(KERN_WARNING
+		       "Did not receive completions for all TX requests\n");
+	}
+}
+
+static struct pci_driver be_driver = {
+	.name = be_driver_name,
+	.id_table = be_device_id_table,
+	.probe = be_probe,
+#ifdef CONFIG_PM
+	.suspend = be_suspend,
+	.resume = be_resume,
+#endif
+	.remove = be_remove
+};
+
+/*
+ * Module init entry point. Registers our our device and return.
+ * Our probe will be called if the device is found.
+ */
+static int __init be_init_module(void)
+{
+	int ret;
+
+	if (rxbuf_size != 8192 && rxbuf_size != 4096 && rxbuf_size != 2048) {
+		printk(KERN_WARNING
+		       "Unsupported receive buffer size (%d) requested\n",
+		       rxbuf_size);
+		printk(KERN_WARNING
+		       "Must be 2048, 4096 or 8192. Defaulting to 2048\n");
+		rxbuf_size = 2048;
+	}
+
+	ret = pci_register_driver(&be_driver);
+
+	return ret;
+}
+
+module_init(be_init_module);
+
+/*
+ * be_exit_module - Driver Exit Cleanup Routine
+ */
+static void __exit be_exit_module(void)
+{
+	pci_unregister_driver(&be_driver);
+}
+
+module_exit(be_exit_module);
diff --git a/drivers/staging/benet/be_int.c b/drivers/staging/benet/be_int.c
new file mode 100644
index 00000000000..cba95d09a8b
--- /dev/null
+++ b/drivers/staging/benet/be_int.c
@@ -0,0 +1,863 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+#include <linux/if_vlan.h>
+#include <linux/inet_lro.h>
+
+#include "benet.h"
+
+/* number of bytes of RX frame that are copied to skb->data */
+#define BE_HDR_LEN 64
+
+#define NETIF_RX(skb) netif_receive_skb(skb)
+#define VLAN_ACCEL_RX(skb, pnob, vt) \
+		vlan_hwaccel_rx(skb, pnob->vlan_grp, vt)
+
+/*
+    This function notifies BladeEngine of the number of completion
+    entries processed from the specified completion queue by writing
+    the number of popped entries to the door bell.
+
+    pnob	- Pointer to the NetObject structure
+    n		- Number of completion entries processed
+    cq_id	- Queue ID of the completion queue for which notification
+			is being done.
+    re_arm	- 1  - rearm the completion ring to generate an event.
+		- 0  - dont rearm the completion ring to generate an event
+*/
+void be_notify_cmpl(struct be_net_object *pnob, int n, int cq_id, int re_arm)
+{
+	struct CQ_DB_AMAP cqdb;
+
+	cqdb.dw[0] = 0;
+	AMAP_SET_BITS_PTR(CQ_DB, qid, &cqdb, cq_id);
+	AMAP_SET_BITS_PTR(CQ_DB, rearm, &cqdb, re_arm);
+	AMAP_SET_BITS_PTR(CQ_DB, num_popped, &cqdb, n);
+	PD_WRITE(&pnob->fn_obj, cq_db, cqdb.dw[0]);
+}
+
+/*
+ * adds additional receive frags indicated by BE starting from given
+ * frag index (fi) to specified skb's frag list
+ */
+static void
+add_skb_frags(struct be_net_object *pnob, struct sk_buff *skb,
+	      u32 nresid, u32 fi)
+{
+	struct be_adapter *adapter = pnob->adapter;
+	u32 sk_frag_idx, n;
+	struct be_rx_page_info *rx_page_info;
+	u32 frag_sz = pnob->rx_buf_size;
+
+	sk_frag_idx = skb_shinfo(skb)->nr_frags;
+	while (nresid) {
+		index_inc(&fi, pnob->rx_q_len);
+
+		rx_page_info = (struct be_rx_page_info *)pnob->rx_ctxt[fi];
+		pnob->rx_ctxt[fi] = NULL;
+		if ((rx_page_info->page_offset) ||
+		    (pnob->rx_pg_shared == false)) {
+			pci_unmap_page(adapter->pdev,
+				       pci_unmap_addr(rx_page_info, bus),
+				       frag_sz, PCI_DMA_FROMDEVICE);
+		}
+
+		n = min(nresid, frag_sz);
+		skb_shinfo(skb)->frags[sk_frag_idx].page = rx_page_info->page;
+		skb_shinfo(skb)->frags[sk_frag_idx].page_offset
+		    = rx_page_info->page_offset;
+		skb_shinfo(skb)->frags[sk_frag_idx].size = n;
+
+		sk_frag_idx++;
+		skb->len += n;
+		skb->data_len += n;
+		skb_shinfo(skb)->nr_frags++;
+		nresid -= n;
+
+		memset(rx_page_info, 0, sizeof(struct be_rx_page_info));
+		atomic_dec(&pnob->rx_q_posted);
+	}
+}
+
+/*
+ * This function processes incoming nic packets over various Rx queues.
+ * This function takes the adapter, the current Rx status descriptor
+ * entry and the Rx completion queue ID as argument.
+ */
+static inline int process_nic_rx_completion(struct be_net_object *pnob,
+					    struct ETH_RX_COMPL_AMAP *rxcp)
+{
+	struct be_adapter *adapter = pnob->adapter;
+	struct sk_buff *skb;
+	int udpcksm, tcpcksm;
+	int n;
+	u32 nresid, fi;
+	u32 frag_sz = pnob->rx_buf_size;
+	u8 *va;
+	struct be_rx_page_info *rx_page_info;
+	u32 numfrags, vtp, vtm, vlan_tag, pktsize;
+
+	fi = AMAP_GET_BITS_PTR(ETH_RX_COMPL, fragndx, rxcp);
+	BUG_ON(fi >= (int)pnob->rx_q_len);
+	BUG_ON(fi < 0);
+
+	rx_page_info = (struct be_rx_page_info *)pnob->rx_ctxt[fi];
+	BUG_ON(!rx_page_info->page);
+	pnob->rx_ctxt[fi] = NULL;
+
+	/*
+	 * If one page is used per fragment or if this is the second half of
+	 *  of the page, unmap the page here
+	 */
+	if ((rx_page_info->page_offset) || (pnob->rx_pg_shared == false)) {
+		pci_unmap_page(adapter->pdev,
+			       pci_unmap_addr(rx_page_info, bus), frag_sz,
+			       PCI_DMA_FROMDEVICE);
+	}
+
+	atomic_dec(&pnob->rx_q_posted);
+	udpcksm = AMAP_GET_BITS_PTR(ETH_RX_COMPL, udpcksm, rxcp);
+	tcpcksm = AMAP_GET_BITS_PTR(ETH_RX_COMPL, tcpcksm, rxcp);
+	pktsize = AMAP_GET_BITS_PTR(ETH_RX_COMPL, pktsize, rxcp);
+	/*
+	 * get rid of RX flush completions first.
+	 */
+	if ((tcpcksm) && (udpcksm) && (pktsize == 32)) {
+		put_page(rx_page_info->page);
+		memset(rx_page_info, 0, sizeof(struct be_rx_page_info));
+		return 0;
+	}
+	skb = netdev_alloc_skb(pnob->netdev, BE_HDR_LEN + NET_IP_ALIGN);
+	if (skb == NULL) {
+		dev_info(&pnob->netdev->dev, "alloc_skb() failed\n");
+		put_page(rx_page_info->page);
+		memset(rx_page_info, 0, sizeof(struct be_rx_page_info));
+		goto free_frags;
+	}
+	skb_reserve(skb, NET_IP_ALIGN);
+
+	skb->dev = pnob->netdev;
+
+	n = min(pktsize, frag_sz);
+
+	va = page_address(rx_page_info->page) + rx_page_info->page_offset;
+	prefetch(va);
+
+	skb->len = n;
+	skb->data_len = n;
+	if (n <= BE_HDR_LEN) {
+		memcpy(skb->data, va, n);
+		put_page(rx_page_info->page);
+		skb->data_len -= n;
+		skb->tail += n;
+	} else {
+
+		/* Setup the SKB with page buffer information */
+		skb_shinfo(skb)->frags[0].page = rx_page_info->page;
+		skb_shinfo(skb)->nr_frags++;
+
+		/* Copy the header into the skb_data */
+		memcpy(skb->data, va, BE_HDR_LEN);
+		skb_shinfo(skb)->frags[0].page_offset =
+		    rx_page_info->page_offset + BE_HDR_LEN;
+		skb_shinfo(skb)->frags[0].size = n - BE_HDR_LEN;
+		skb->data_len -= BE_HDR_LEN;
+		skb->tail += BE_HDR_LEN;
+	}
+	memset(rx_page_info, 0, sizeof(struct be_rx_page_info));
+	nresid = pktsize - n;
+
+	skb->protocol = eth_type_trans(skb, pnob->netdev);
+
+	if ((tcpcksm || udpcksm) && adapter->rx_csum)
+		skb->ip_summed = CHECKSUM_UNNECESSARY;
+	else
+		skb->ip_summed = CHECKSUM_NONE;
+	/*
+	 * if we have more bytes left, the frame has been
+	 * given to us in multiple fragments.  This happens
+	 * with Jumbo frames. Add the remaining fragments to
+	 * skb->frags[] array.
+	 */
+	if (nresid)
+		add_skb_frags(pnob, skb, nresid, fi);
+
+	/* update the the true size of the skb. */
+	skb->truesize = skb->len + sizeof(struct sk_buff);
+
+	/*
+	 * If a 802.3 frame or 802.2 LLC frame
+	 * (i.e) contains length field in MAC Hdr
+	 * and frame len is greater than 64 bytes
+	 */
+	if (((skb->protocol == ntohs(ETH_P_802_2)) ||
+	     (skb->protocol == ntohs(ETH_P_802_3)))
+	    && (pktsize > BE_HDR_LEN)) {
+		/*
+		 * If the length given in Mac Hdr is less than frame size
+		 * Erraneous frame, Drop it
+		 */
+		if ((ntohs(*(u16 *) (va + 12)) + ETH_HLEN) < pktsize) {
+			/* Increment Non Ether type II frames dropped */
+			adapter->be_stat.bes_802_3_dropped_frames++;
+
+			kfree_skb(skb);
+			return 0;
+		}
+		/*
+		 * else if the length given in Mac Hdr is greater than
+		 * frame size, should not be seeing this sort of frames
+		 * dump the pkt and pass to stack
+		 */
+		else if ((ntohs(*(u16 *) (va + 12)) + ETH_HLEN) > pktsize) {
+			/* Increment Non Ether type II frames malformed */
+			adapter->be_stat.bes_802_3_malformed_frames++;
+		}
+	}
+
+	vtp = AMAP_GET_BITS_PTR(ETH_RX_COMPL, vtp, rxcp);
+	vtm = AMAP_GET_BITS_PTR(ETH_RX_COMPL, vtm, rxcp);
+	if (vtp && vtm) {
+		/* Vlan tag present in pkt and BE found
+		 * that the tag matched an entry in VLAN table
+		 */
+		if (!pnob->vlan_grp || pnob->num_vlans == 0) {
+			/* But we have no VLANs configured.
+			 * This should never happen.  Drop the packet.
+			 */
+			dev_info(&pnob->netdev->dev,
+			       "BladeEngine: Unexpected vlan tagged packet\n");
+			kfree_skb(skb);
+			return 0;
+		}
+		/* pass the VLAN packet to stack */
+		vlan_tag = AMAP_GET_BITS_PTR(ETH_RX_COMPL, vlan_tag, rxcp);
+		VLAN_ACCEL_RX(skb, pnob, be16_to_cpu(vlan_tag));
+
+	} else {
+		NETIF_RX(skb);
+	}
+	return 0;
+
+free_frags:
+	/* free all frags associated with the current rxcp */
+	numfrags = AMAP_GET_BITS_PTR(ETH_RX_COMPL, numfrags, rxcp);
+	while (numfrags-- > 1) {
+		index_inc(&fi, pnob->rx_q_len);
+
+		rx_page_info = (struct be_rx_page_info *)
+		    pnob->rx_ctxt[fi];
+		pnob->rx_ctxt[fi] = (void *)NULL;
+		if (rx_page_info->page_offset || !pnob->rx_pg_shared) {
+			pci_unmap_page(adapter->pdev,
+				       pci_unmap_addr(rx_page_info, bus),
+				       frag_sz, PCI_DMA_FROMDEVICE);
+		}
+
+		put_page(rx_page_info->page);
+		memset(rx_page_info, 0, sizeof(struct be_rx_page_info));
+		atomic_dec(&pnob->rx_q_posted);
+	}
+	return -ENOMEM;
+}
+
+static void process_nic_rx_completion_lro(struct be_net_object *pnob,
+					  struct ETH_RX_COMPL_AMAP *rxcp)
+{
+	struct be_adapter *adapter = pnob->adapter;
+	struct skb_frag_struct rx_frags[BE_MAX_FRAGS_PER_FRAME];
+	unsigned int udpcksm, tcpcksm;
+	u32 numfrags, vlanf, vtm, vlan_tag, nresid;
+	u16 vlant;
+	unsigned int fi, idx, n;
+	struct be_rx_page_info *rx_page_info;
+	u32 frag_sz = pnob->rx_buf_size, pktsize;
+	bool rx_coal = (adapter->max_rx_coal <= 1) ? 0 : 1;
+	u8 err, *va;
+	__wsum csum = 0;
+
+	if (AMAP_GET_BITS_PTR(ETH_RX_COMPL, ipsec, rxcp)) {
+		/*  Drop the pkt and move to the next completion.  */
+		adapter->be_stat.bes_rx_misc_pkts++;
+		return;
+	}
+	err = AMAP_GET_BITS_PTR(ETH_RX_COMPL, err, rxcp);
+	if (err || !rx_coal) {
+		/* We won't coalesce Rx pkts if the err bit set.
+		 * take the path of normal completion processing */
+		process_nic_rx_completion(pnob, rxcp);
+		return;
+	}
+
+	fi = AMAP_GET_BITS_PTR(ETH_RX_COMPL, fragndx, rxcp);
+	BUG_ON(fi >= (int)pnob->rx_q_len);
+	BUG_ON(fi < 0);
+	rx_page_info = (struct be_rx_page_info *)pnob->rx_ctxt[fi];
+	BUG_ON(!rx_page_info->page);
+	pnob->rx_ctxt[fi] = (void *)NULL;
+	/*  If one page is used per fragment or if this is the
+	 * second half of the page, unmap the page here
+	 */
+	if (rx_page_info->page_offset || !pnob->rx_pg_shared) {
+		pci_unmap_page(adapter->pdev,
+			       pci_unmap_addr(rx_page_info, bus),
+			       frag_sz, PCI_DMA_FROMDEVICE);
+	}
+
+	numfrags = AMAP_GET_BITS_PTR(ETH_RX_COMPL, numfrags, rxcp);
+	udpcksm = AMAP_GET_BITS_PTR(ETH_RX_COMPL, udpcksm, rxcp);
+	tcpcksm = AMAP_GET_BITS_PTR(ETH_RX_COMPL, tcpcksm, rxcp);
+	vlan_tag = AMAP_GET_BITS_PTR(ETH_RX_COMPL, vlan_tag, rxcp);
+	vlant = be16_to_cpu(vlan_tag);
+	vlanf = AMAP_GET_BITS_PTR(ETH_RX_COMPL, vtp, rxcp);
+	vtm = AMAP_GET_BITS_PTR(ETH_RX_COMPL, vtm, rxcp);
+	pktsize = AMAP_GET_BITS_PTR(ETH_RX_COMPL, pktsize, rxcp);
+
+	atomic_dec(&pnob->rx_q_posted);
+
+	if (tcpcksm && udpcksm && pktsize == 32) {
+		/* flush completion entries */
+		put_page(rx_page_info->page);
+		memset(rx_page_info, 0, sizeof(struct be_rx_page_info));
+		return;
+	}
+	/* Only one of udpcksum and tcpcksum can be set */
+	BUG_ON(udpcksm && tcpcksm);
+
+	/* jumbo frames could come in multiple fragments */
+	BUG_ON(numfrags != ((pktsize + (frag_sz - 1)) / frag_sz));
+	n = min(pktsize, frag_sz);
+	nresid = pktsize - n;	/* will be useful for jumbo pkts */
+	idx = 0;
+
+	va = page_address(rx_page_info->page) + rx_page_info->page_offset;
+	prefetch(va);
+	rx_frags[idx].page = rx_page_info->page;
+	rx_frags[idx].page_offset = (rx_page_info->page_offset);
+	rx_frags[idx].size = n;
+	memset(rx_page_info, 0, sizeof(struct be_rx_page_info));
+
+	/* If we got multiple fragments, we have more data. */
+	while (nresid) {
+		idx++;
+		index_inc(&fi, pnob->rx_q_len);
+
+		rx_page_info = (struct be_rx_page_info *)pnob->rx_ctxt[fi];
+		pnob->rx_ctxt[fi] = (void *)NULL;
+		if (rx_page_info->page_offset || !pnob->rx_pg_shared) {
+			pci_unmap_page(adapter->pdev,
+				       pci_unmap_addr(rx_page_info, bus),
+				       frag_sz, PCI_DMA_FROMDEVICE);
+		}
+
+		n = min(nresid, frag_sz);
+		rx_frags[idx].page = rx_page_info->page;
+		rx_frags[idx].page_offset = (rx_page_info->page_offset);
+		rx_frags[idx].size = n;
+
+		nresid -= n;
+		memset(rx_page_info, 0, sizeof(struct be_rx_page_info));
+		atomic_dec(&pnob->rx_q_posted);
+	}
+
+	if (likely(!(vlanf && vtm))) {
+		lro_receive_frags(&pnob->lro_mgr, rx_frags,
+				  pktsize, pktsize,
+				  (void *)(unsigned long)csum, csum);
+	} else {
+		/* Vlan tag present in pkt and BE found
+		 * that the tag matched an entry in VLAN table
+		 */
+		if (unlikely(!pnob->vlan_grp || pnob->num_vlans == 0)) {
+			/* But we have no VLANs configured.
+			 * This should never happen.  Drop the packet.
+			 */
+			dev_info(&pnob->netdev->dev,
+			       "BladeEngine: Unexpected vlan tagged packet\n");
+			return;
+		}
+		/* pass the VLAN packet to stack */
+		lro_vlan_hwaccel_receive_frags(&pnob->lro_mgr,
+					       rx_frags, pktsize, pktsize,
+					       pnob->vlan_grp, vlant,
+					       (void *)(unsigned long)csum,
+					       csum);
+	}
+
+	adapter->be_stat.bes_rx_coal++;
+}
+
+struct ETH_RX_COMPL_AMAP *be_get_rx_cmpl(struct be_net_object *pnob)
+{
+	struct ETH_RX_COMPL_AMAP *rxcp = &pnob->rx_cq[pnob->rx_cq_tl];
+	u32 valid, ct;
+
+	valid = AMAP_GET_BITS_PTR(ETH_RX_COMPL, valid, rxcp);
+	if (valid == 0)
+		return NULL;
+
+	ct = AMAP_GET_BITS_PTR(ETH_RX_COMPL, ct, rxcp);
+	if (ct != 0) {
+		/* Invalid chute #. treat as error */
+		AMAP_SET_BITS_PTR(ETH_RX_COMPL, err, rxcp, 1);
+	}
+
+	be_adv_rxcq_tl(pnob);
+	AMAP_SET_BITS_PTR(ETH_RX_COMPL, valid, rxcp, 0);
+	return rxcp;
+}
+
+static void update_rx_rate(struct be_adapter *adapter)
+{
+	/* update the rate once in two seconds */
+	if ((jiffies - adapter->eth_rx_jiffies) > 2 * (HZ)) {
+		u32 r;
+		r = adapter->eth_rx_bytes /
+		    ((jiffies - adapter->eth_rx_jiffies) / (HZ));
+		r = (r / 1000000);	/* MB/Sec */
+
+		/* Mega Bits/Sec */
+		adapter->be_stat.bes_eth_rx_rate = (r * 8);
+		adapter->eth_rx_jiffies = jiffies;
+		adapter->eth_rx_bytes = 0;
+	}
+}
+
+static int process_rx_completions(struct be_net_object *pnob, int max_work)
+{
+	struct be_adapter *adapter = pnob->adapter;
+	struct ETH_RX_COMPL_AMAP *rxcp;
+	u32 nc = 0;
+	unsigned int pktsize;
+
+	while (max_work && (rxcp = be_get_rx_cmpl(pnob))) {
+		prefetch(rxcp);
+		pktsize = AMAP_GET_BITS_PTR(ETH_RX_COMPL, pktsize, rxcp);
+		process_nic_rx_completion_lro(pnob, rxcp);
+		adapter->eth_rx_bytes += pktsize;
+		update_rx_rate(adapter);
+		nc++;
+		max_work--;
+		adapter->be_stat.bes_rx_compl++;
+	}
+	if (likely(adapter->max_rx_coal > 1)) {
+		adapter->be_stat.bes_rx_flush++;
+		lro_flush_all(&pnob->lro_mgr);
+	}
+
+	/* Refill the queue */
+	if (atomic_read(&pnob->rx_q_posted) < 900)
+		be_post_eth_rx_buffs(pnob);
+
+	return nc;
+}
+
+static struct ETH_TX_COMPL_AMAP *be_get_tx_cmpl(struct be_net_object *pnob)
+{
+	struct ETH_TX_COMPL_AMAP *txcp = &pnob->tx_cq[pnob->tx_cq_tl];
+	u32 valid;
+
+	valid = AMAP_GET_BITS_PTR(ETH_TX_COMPL, valid, txcp);
+	if (valid == 0)
+		return NULL;
+
+	AMAP_SET_BITS_PTR(ETH_TX_COMPL, valid, txcp, 0);
+	be_adv_txcq_tl(pnob);
+	return txcp;
+
+}
+
+void process_one_tx_compl(struct be_net_object *pnob, u32 end_idx)
+{
+	struct be_adapter *adapter = pnob->adapter;
+	int cur_index, tx_wrbs_completed = 0;
+	struct sk_buff *skb;
+	u64 busaddr, pa, pa_lo, pa_hi;
+	struct ETH_WRB_AMAP *wrb;
+	u32 frag_len, last_index, j;
+
+	last_index = tx_compl_lastwrb_idx_get(pnob);
+	BUG_ON(last_index != end_idx);
+	pnob->tx_ctxt[pnob->tx_q_tl] = NULL;
+	do {
+		cur_index = pnob->tx_q_tl;
+		wrb = &pnob->tx_q[cur_index];
+		pa_hi = AMAP_GET_BITS_PTR(ETH_WRB, frag_pa_hi, wrb);
+		pa_lo = AMAP_GET_BITS_PTR(ETH_WRB, frag_pa_lo, wrb);
+		frag_len = AMAP_GET_BITS_PTR(ETH_WRB, frag_len, wrb);
+		busaddr = (pa_hi << 32) | pa_lo;
+		if (busaddr != 0) {
+			pa = le64_to_cpu(busaddr);
+			pci_unmap_single(adapter->pdev, pa,
+					 frag_len, PCI_DMA_TODEVICE);
+		}
+		if (cur_index == last_index) {
+			skb = (struct sk_buff *)pnob->tx_ctxt[cur_index];
+			BUG_ON(!skb);
+			for (j = 0; j < skb_shinfo(skb)->nr_frags; j++) {
+				struct skb_frag_struct *frag;
+				frag = &skb_shinfo(skb)->frags[j];
+				pci_unmap_page(adapter->pdev,
+					       (ulong) frag->page, frag->size,
+					       PCI_DMA_TODEVICE);
+			}
+			kfree_skb(skb);
+			pnob->tx_ctxt[cur_index] = NULL;
+		} else {
+			BUG_ON(pnob->tx_ctxt[cur_index]);
+		}
+		tx_wrbs_completed++;
+		be_adv_txq_tl(pnob);
+	} while (cur_index != last_index);
+	atomic_sub(tx_wrbs_completed, &pnob->tx_q_used);
+}
+
+/* there is no need to take an SMP lock here since currently
+ * we have only one instance of the tasklet that does completion
+ * processing.
+ */
+static void process_nic_tx_completions(struct be_net_object *pnob)
+{
+	struct be_adapter *adapter = pnob->adapter;
+	struct ETH_TX_COMPL_AMAP *txcp;
+	struct net_device *netdev = pnob->netdev;
+	u32 end_idx, num_processed = 0;
+
+	adapter->be_stat.bes_tx_events++;
+
+	while ((txcp = be_get_tx_cmpl(pnob))) {
+		end_idx = AMAP_GET_BITS_PTR(ETH_TX_COMPL, wrb_index, txcp);
+		process_one_tx_compl(pnob, end_idx);
+		num_processed++;
+		adapter->be_stat.bes_tx_compl++;
+	}
+	be_notify_cmpl(pnob, num_processed, pnob->tx_cq_id, 1);
+	/*
+	 * We got Tx completions and have usable WRBs.
+	 * If the netdev's queue has been stopped
+	 * because we had run out of WRBs, wake it now.
+	 */
+	spin_lock(&adapter->txq_lock);
+	if (netif_queue_stopped(netdev)
+	    && atomic_read(&pnob->tx_q_used) < pnob->tx_q_len / 2) {
+		netif_wake_queue(netdev);
+	}
+	spin_unlock(&adapter->txq_lock);
+}
+
+static u32 post_rx_buffs(struct be_net_object *pnob, struct list_head *rxbl)
+{
+	u32 nposted = 0;
+	struct ETH_RX_D_AMAP *rxd = NULL;
+	struct be_recv_buffer *rxbp;
+	void **rx_ctxp;
+	struct RQ_DB_AMAP rqdb;
+
+	rx_ctxp = pnob->rx_ctxt;
+
+	while (!list_empty(rxbl) &&
+	       (rx_ctxp[pnob->rx_q_hd] == NULL) && nposted < 255) {
+
+		rxbp = list_first_entry(rxbl, struct be_recv_buffer, rxb_list);
+		list_del(&rxbp->rxb_list);
+		rxd = pnob->rx_q + pnob->rx_q_hd;
+		AMAP_SET_BITS_PTR(ETH_RX_D, fragpa_lo, rxd, rxbp->rxb_pa_lo);
+		AMAP_SET_BITS_PTR(ETH_RX_D, fragpa_hi, rxd, rxbp->rxb_pa_hi);
+
+		rx_ctxp[pnob->rx_q_hd] = rxbp->rxb_ctxt;
+		be_adv_rxq_hd(pnob);
+		nposted++;
+	}
+
+	if (nposted) {
+		/* Now press the door bell to notify BladeEngine. */
+		rqdb.dw[0] = 0;
+		AMAP_SET_BITS_PTR(RQ_DB, numPosted, &rqdb, nposted);
+		AMAP_SET_BITS_PTR(RQ_DB, rq, &rqdb, pnob->rx_q_id);
+		PD_WRITE(&pnob->fn_obj, erx_rq_db, rqdb.dw[0]);
+	}
+	atomic_add(nposted, &pnob->rx_q_posted);
+	return nposted;
+}
+
+void be_post_eth_rx_buffs(struct be_net_object *pnob)
+{
+	struct be_adapter *adapter = pnob->adapter;
+	u32 num_bufs, r;
+	u64 busaddr = 0, tmp_pa;
+	u32 max_bufs, pg_hd;
+	u32 frag_size;
+	struct be_recv_buffer *rxbp;
+	struct list_head rxbl;
+	struct be_rx_page_info *rx_page_info;
+	struct page *page = NULL;
+	u32 page_order = 0;
+	gfp_t alloc_flags = GFP_ATOMIC;
+
+	BUG_ON(!adapter);
+
+	max_bufs = 64;		/* should be even # <= 255. */
+
+	frag_size = pnob->rx_buf_size;
+	page_order = get_order(frag_size);
+
+	if (frag_size == 8192)
+		alloc_flags |= (gfp_t) __GFP_COMP;
+	/*
+	 * Form a linked list of RECV_BUFFFER structure to be be posted.
+	 * We will post even number of buffer so that pages can be
+	 * shared.
+	 */
+	INIT_LIST_HEAD(&rxbl);
+
+	for (num_bufs = 0; num_bufs < max_bufs &&
+		!pnob->rx_page_info[pnob->rx_pg_info_hd].page; ++num_bufs) {
+
+		rxbp = &pnob->eth_rx_bufs[num_bufs];
+		pg_hd = pnob->rx_pg_info_hd;
+		rx_page_info = &pnob->rx_page_info[pg_hd];
+
+		if (!page) {
+			page = alloc_pages(alloc_flags, page_order);
+			if (unlikely(page == NULL)) {
+				adapter->be_stat.bes_ethrx_post_fail++;
+				pnob->rxbuf_post_fail++;
+				break;
+			}
+			pnob->rxbuf_post_fail = 0;
+			busaddr = pci_map_page(adapter->pdev, page, 0,
+					       frag_size, PCI_DMA_FROMDEVICE);
+			rx_page_info->page_offset = 0;
+			rx_page_info->page = page;
+			/*
+			 * If we are sharing a page among two skbs,
+			 * alloc a new one on the next iteration
+			 */
+			if (pnob->rx_pg_shared == false)
+				page = NULL;
+		} else {
+			get_page(page);
+			rx_page_info->page_offset += frag_size;
+			rx_page_info->page = page;
+			/*
+			 * We are finished with the alloced page,
+			 * Alloc a new one on the next iteration
+			 */
+			page = NULL;
+		}
+		rxbp->rxb_ctxt = (void *)rx_page_info;
+		index_inc(&pnob->rx_pg_info_hd, pnob->rx_q_len);
+
+		pci_unmap_addr_set(rx_page_info, bus, busaddr);
+		tmp_pa = busaddr + rx_page_info->page_offset;
+		rxbp->rxb_pa_lo = (tmp_pa & 0xFFFFFFFF);
+		rxbp->rxb_pa_hi = (tmp_pa >> 32);
+		rxbp->rxb_len = frag_size;
+		list_add_tail(&rxbp->rxb_list, &rxbl);
+	}			/* End of for */
+
+	r = post_rx_buffs(pnob, &rxbl);
+	BUG_ON(r != num_bufs);
+	return;
+}
+
+/*
+ * Interrupt service for network function.  We just schedule the
+ * tasklet which does all completion processing.
+ */
+irqreturn_t be_int(int irq, void *dev)
+{
+	struct net_device *netdev = dev;
+	struct be_net_object *pnob = netdev_priv(netdev);
+	struct be_adapter *adapter = pnob->adapter;
+	u32 isr;
+
+	isr = CSR_READ(&pnob->fn_obj, cev.isr1);
+	if (unlikely(!isr))
+		return IRQ_NONE;
+
+	spin_lock(&adapter->int_lock);
+	adapter->isr |= isr;
+	spin_unlock(&adapter->int_lock);
+
+	adapter->be_stat.bes_ints++;
+
+	tasklet_schedule(&adapter->sts_handler);
+	return IRQ_HANDLED;
+}
+
+/*
+ * Poll function called by NAPI with a work budget.
+ * We process as many UC. BC and MC receive completions
+ * as the budget allows and return the actual number of
+ * RX ststutses processed.
+ */
+int be_poll(struct napi_struct *napi, int budget)
+{
+	struct be_net_object *pnob =
+			container_of(napi, struct be_net_object, napi);
+	u32 work_done;
+
+	pnob->adapter->be_stat.bes_polls++;
+	work_done = process_rx_completions(pnob, budget);
+	BUG_ON(work_done > budget);
+
+	/* All consumed */
+	if (work_done < budget) {
+		netif_rx_complete(napi);
+		/* enable intr */
+		be_notify_cmpl(pnob, work_done, pnob->rx_cq_id, 1);
+	} else {
+		/* More to be consumed; continue with interrupts disabled */
+		be_notify_cmpl(pnob, work_done, pnob->rx_cq_id, 0);
+	}
+	return work_done;
+}
+
+static struct EQ_ENTRY_AMAP *get_event(struct be_net_object *pnob)
+{
+	struct EQ_ENTRY_AMAP *eqp = &(pnob->event_q[pnob->event_q_tl]);
+	if (!AMAP_GET_BITS_PTR(EQ_ENTRY, Valid, eqp))
+		return NULL;
+	be_adv_eq_tl(pnob);
+	return eqp;
+}
+
+/*
+ * Processes all valid events in the event ring associated with given
+ * NetObject.  Also, notifies BE the number of events processed.
+ */
+static inline u32 process_events(struct be_net_object *pnob)
+{
+	struct be_adapter *adapter = pnob->adapter;
+	struct EQ_ENTRY_AMAP *eqp;
+	u32 rid, num_events = 0;
+	struct net_device *netdev = pnob->netdev;
+
+	while ((eqp = get_event(pnob)) != NULL) {
+		adapter->be_stat.bes_events++;
+		rid = AMAP_GET_BITS_PTR(EQ_ENTRY, ResourceID, eqp);
+		if (rid == pnob->rx_cq_id) {
+			adapter->be_stat.bes_rx_events++;
+			netif_rx_schedule(&pnob->napi);
+		} else if (rid == pnob->tx_cq_id) {
+			process_nic_tx_completions(pnob);
+		} else if (rid == pnob->mcc_cq_id) {
+			be_mcc_process_cq(&pnob->mcc_q_obj, 1);
+		} else {
+			dev_info(&netdev->dev,
+					"Invalid EQ ResourceID %d\n", rid);
+		}
+		AMAP_SET_BITS_PTR(EQ_ENTRY, Valid, eqp, 0);
+		AMAP_SET_BITS_PTR(EQ_ENTRY, ResourceID, eqp, 0);
+		num_events++;
+	}
+	return num_events;
+}
+
+static void update_eqd(struct be_adapter *adapter, struct be_net_object *pnob)
+{
+	int status;
+	struct be_eq_object *eq_objectp;
+
+	/* update once a second */
+	if ((jiffies - adapter->ips_jiffies) > 1 * (HZ)) {
+		/* One second elapsed since last update  */
+		u32 r, new_eqd = -1;
+		r = adapter->be_stat.bes_ints - adapter->be_stat.bes_prev_ints;
+		r = r / ((jiffies - adapter->ips_jiffies) / (HZ));
+		adapter->be_stat.bes_ips = r;
+		adapter->ips_jiffies = jiffies;
+		adapter->be_stat.bes_prev_ints = adapter->be_stat.bes_ints;
+		if (r > IPS_HI_WM && adapter->cur_eqd < adapter->max_eqd)
+			new_eqd = (adapter->cur_eqd + 8);
+		if (r < IPS_LO_WM && adapter->cur_eqd > adapter->min_eqd)
+			new_eqd = (adapter->cur_eqd - 8);
+		if (adapter->enable_aic && new_eqd != -1) {
+			eq_objectp = &pnob->event_q_obj;
+			status = be_eq_modify_delay(&pnob->fn_obj, 1,
+						    &eq_objectp, &new_eqd, NULL,
+						    NULL, NULL);
+			if (status == BE_SUCCESS)
+				adapter->cur_eqd = new_eqd;
+		}
+	}
+}
+
+/*
+    This function notifies BladeEngine of how many events were processed
+    from the event queue by ringing the corresponding door bell and
+    optionally re-arms the event queue.
+    n		- number of events processed
+    re_arm	- 1 - re-arm the EQ, 0 - do not re-arm the EQ
+
+*/
+static void be_notify_event(struct be_net_object *pnob, int n, int re_arm)
+{
+	struct CQ_DB_AMAP eqdb;
+	eqdb.dw[0] = 0;
+
+	AMAP_SET_BITS_PTR(CQ_DB, qid, &eqdb, pnob->event_q_id);
+	AMAP_SET_BITS_PTR(CQ_DB, rearm, &eqdb, re_arm);
+	AMAP_SET_BITS_PTR(CQ_DB, event, &eqdb, 1);
+	AMAP_SET_BITS_PTR(CQ_DB, num_popped, &eqdb, n);
+	/*
+	 * Under some situations we see an interrupt and no valid
+	 * EQ entry.  To keep going, we need to ring the DB even if
+	 * numPOsted is 0.
+	 */
+	PD_WRITE(&pnob->fn_obj, cq_db, eqdb.dw[0]);
+	return;
+}
+
+/*
+ * Called from the tasklet scheduled by ISR.  All real interrupt processing
+ * is done here.
+ */
+void be_process_intr(unsigned long context)
+{
+	struct be_adapter *adapter = (struct be_adapter *)context;
+	struct be_net_object *pnob = adapter->net_obj;
+	u32 isr, n;
+	ulong flags = 0;
+
+	isr = adapter->isr;
+
+	/*
+	 * we create only one NIC event queue in Linux. Event is
+	 * expected only in the first event queue
+	 */
+	BUG_ON(isr & 0xfffffffe);
+	if ((isr & 1) == 0)
+		return;		/* not our interrupt */
+	n = process_events(pnob);
+	/*
+	 * Clear the event bit. adapter->isr is  set by
+	 * hard interrupt.  Prevent race with lock.
+	 */
+	spin_lock_irqsave(&adapter->int_lock, flags);
+	adapter->isr &= ~1;
+	spin_unlock_irqrestore(&adapter->int_lock, flags);
+	be_notify_event(pnob, n, 1);
+	/*
+	 * If previous allocation attempts had failed and
+	 * BE has used up all posted buffers, post RX buffers here
+	 */
+	if (pnob->rxbuf_post_fail && atomic_read(&pnob->rx_q_posted) == 0)
+		be_post_eth_rx_buffs(pnob);
+	update_eqd(adapter, pnob);
+	return;
+}
diff --git a/drivers/staging/benet/be_netif.c b/drivers/staging/benet/be_netif.c
new file mode 100644
index 00000000000..2b8daf63dc7
--- /dev/null
+++ b/drivers/staging/benet/be_netif.c
@@ -0,0 +1,705 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * be_netif.c
+ *
+ * This file contains various entry points of drivers seen by tcp/ip stack.
+ */
+
+#include <linux/if_vlan.h>
+#include <linux/in.h>
+#include "benet.h"
+#include <linux/ip.h>
+#include <linux/inet_lro.h>
+
+/* Strings to print Link properties */
+static const char *link_speed[] = {
+	"Invalid link Speed Value",
+	"10 Mbps",
+	"100 Mbps",
+	"1 Gbps",
+	"10 Gbps"
+};
+
+static const char *link_duplex[] = {
+	"Invalid Duplex Value",
+	"Half Duplex",
+	"Full Duplex"
+};
+
+static const char *link_state[] = {
+	"",
+	"(active)"
+};
+
+void be_print_link_info(struct BE_LINK_STATUS *lnk_status)
+{
+	u16 si, di, ai;
+
+	/* Port 0 */
+	if (lnk_status->mac0_speed && lnk_status->mac0_duplex) {
+		/* Port is up and running */
+		si = (lnk_status->mac0_speed < 5) ? lnk_status->mac0_speed : 0;
+		di = (lnk_status->mac0_duplex < 3) ?
+		    lnk_status->mac0_duplex : 0;
+		ai = (lnk_status->active_port == 0) ? 1 : 0;
+		printk(KERN_INFO "PortNo. 0: Speed - %s %s %s\n",
+		       link_speed[si], link_duplex[di], link_state[ai]);
+	} else
+		printk(KERN_INFO "PortNo. 0: Down\n");
+
+	/* Port 1 */
+	if (lnk_status->mac1_speed && lnk_status->mac1_duplex) {
+		/* Port is up and running */
+		si = (lnk_status->mac1_speed < 5) ? lnk_status->mac1_speed : 0;
+		di = (lnk_status->mac1_duplex < 3) ?
+		    lnk_status->mac1_duplex : 0;
+		ai = (lnk_status->active_port == 0) ? 1 : 0;
+		printk(KERN_INFO "PortNo. 1: Speed - %s %s %s\n",
+		       link_speed[si], link_duplex[di], link_state[ai]);
+	} else
+		printk(KERN_INFO "PortNo. 1: Down\n");
+
+	return;
+}
+
+static int
+be_get_frag_header(struct skb_frag_struct *frag, void **mac_hdr,
+		   void **ip_hdr, void **tcpudp_hdr,
+		   u64 *hdr_flags, void *priv)
+{
+	struct ethhdr *eh;
+	struct vlan_ethhdr *veh;
+	struct iphdr *iph;
+	u8 *va = page_address(frag->page) + frag->page_offset;
+	unsigned long ll_hlen;
+
+	/* find the mac header, abort if not IPv4 */
+
+	prefetch(va);
+	eh = (struct ethhdr *)va;
+	*mac_hdr = eh;
+	ll_hlen = ETH_HLEN;
+	if (eh->h_proto != htons(ETH_P_IP)) {
+		if (eh->h_proto == htons(ETH_P_8021Q)) {
+			veh = (struct vlan_ethhdr *)va;
+			if (veh->h_vlan_encapsulated_proto != htons(ETH_P_IP))
+				return -1;
+
+			ll_hlen += VLAN_HLEN;
+
+		} else {
+			return -1;
+		}
+	}
+	*hdr_flags = LRO_IPV4;
+
+	iph = (struct iphdr *)(va + ll_hlen);
+	*ip_hdr = iph;
+	if (iph->protocol != IPPROTO_TCP)
+		return -1;
+	*hdr_flags |= LRO_TCP;
+	*tcpudp_hdr = (u8 *) (*ip_hdr) + (iph->ihl << 2);
+
+	return 0;
+}
+
+static int benet_open(struct net_device *netdev)
+{
+	struct be_net_object *pnob = netdev_priv(netdev);
+	struct be_adapter *adapter = pnob->adapter;
+	struct net_lro_mgr *lro_mgr;
+
+	if (adapter->dev_state < BE_DEV_STATE_INIT)
+		return -EAGAIN;
+
+	lro_mgr = &pnob->lro_mgr;
+	lro_mgr->dev = netdev;
+
+	lro_mgr->features = LRO_F_NAPI;
+	lro_mgr->ip_summed = CHECKSUM_UNNECESSARY;
+	lro_mgr->ip_summed_aggr = CHECKSUM_UNNECESSARY;
+	lro_mgr->max_desc = BE_MAX_LRO_DESCRIPTORS;
+	lro_mgr->lro_arr = pnob->lro_desc;
+	lro_mgr->get_frag_header = be_get_frag_header;
+	lro_mgr->max_aggr = adapter->max_rx_coal;
+	lro_mgr->frag_align_pad = 2;
+	if (lro_mgr->max_aggr > MAX_SKB_FRAGS)
+		lro_mgr->max_aggr = MAX_SKB_FRAGS;
+
+	adapter->max_rx_coal = BE_LRO_MAX_PKTS;
+
+	be_update_link_status(adapter);
+
+	/*
+	 * Set carrier on only if Physical Link up
+	 * Either of the port link status up signifies this
+	 */
+	if ((adapter->port0_link_sts == BE_PORT_LINK_UP) ||
+	    (adapter->port1_link_sts == BE_PORT_LINK_UP)) {
+		netif_start_queue(netdev);
+		netif_carrier_on(netdev);
+	}
+
+	adapter->dev_state = BE_DEV_STATE_OPEN;
+	napi_enable(&pnob->napi);
+	be_enable_intr(pnob);
+	be_enable_eq_intr(pnob);
+	/*
+	 * RX completion queue may be in dis-armed state. Arm it.
+	 */
+	be_notify_cmpl(pnob, 0, pnob->rx_cq_id, 1);
+
+	return 0;
+}
+
+static int benet_close(struct net_device *netdev)
+{
+	struct be_net_object *pnob = netdev_priv(netdev);
+	struct be_adapter *adapter = pnob->adapter;
+
+	netif_stop_queue(netdev);
+	synchronize_irq(netdev->irq);
+
+	be_wait_nic_tx_cmplx_cmpl(pnob);
+	adapter->dev_state = BE_DEV_STATE_INIT;
+	netif_carrier_off(netdev);
+
+	adapter->port0_link_sts = BE_PORT_LINK_DOWN;
+	adapter->port1_link_sts = BE_PORT_LINK_DOWN;
+	be_disable_intr(pnob);
+	be_disable_eq_intr(pnob);
+	napi_disable(&pnob->napi);
+
+	return 0;
+}
+
+/*
+ * Setting a Mac Address for BE
+ * Takes netdev and a void pointer as arguments.
+ * The pointer holds the new addres to be used.
+ */
+static int benet_set_mac_addr(struct net_device *netdev, void *p)
+{
+	struct sockaddr *addr = p;
+	struct be_net_object *pnob = netdev_priv(netdev);
+
+	memcpy(netdev->dev_addr, addr->sa_data, netdev->addr_len);
+	be_rxf_mac_address_read_write(&pnob->fn_obj, 0, 0, false, true, false,
+				netdev->dev_addr, NULL, NULL);
+	/*
+	 * Since we are doing Active-Passive failover, both
+	 * ports should have matching MAC addresses everytime.
+	 */
+	be_rxf_mac_address_read_write(&pnob->fn_obj, 1, 0, false, true, false,
+				      netdev->dev_addr, NULL, NULL);
+
+	return 0;
+}
+
+void be_get_stats_timer_handler(unsigned long context)
+{
+	struct be_timer_ctxt *ctxt = (struct be_timer_ctxt *)context;
+
+	if (atomic_read(&ctxt->get_stat_flag)) {
+		atomic_dec(&ctxt->get_stat_flag);
+		up((void *)ctxt->get_stat_sem_addr);
+	}
+	del_timer(&ctxt->get_stats_timer);
+	return;
+}
+
+void be_get_stat_cb(void *context, int status,
+		    struct MCC_WRB_AMAP *optional_wrb)
+{
+	struct be_timer_ctxt *ctxt = (struct be_timer_ctxt *)context;
+	/*
+	 * just up the semaphore if the get_stat_flag
+	 * reads 1. so that the waiter can continue.
+	 * If it is 0, then it was handled by the timer handler.
+	 */
+	del_timer(&ctxt->get_stats_timer);
+	if (atomic_read(&ctxt->get_stat_flag)) {
+		atomic_dec(&ctxt->get_stat_flag);
+		up((void *)ctxt->get_stat_sem_addr);
+	}
+}
+
+struct net_device_stats *benet_get_stats(struct net_device *dev)
+{
+	struct be_net_object *pnob = netdev_priv(dev);
+	struct be_adapter *adapter = pnob->adapter;
+	u64 pa;
+	struct be_timer_ctxt *ctxt = &adapter->timer_ctxt;
+
+	if (adapter->dev_state != BE_DEV_STATE_OPEN) {
+		/* Return previously read stats */
+		return &(adapter->benet_stats);
+	}
+	/* Get Physical Addr */
+	pa = pci_map_single(adapter->pdev, adapter->eth_statsp,
+			    sizeof(struct FWCMD_ETH_GET_STATISTICS),
+			    PCI_DMA_FROMDEVICE);
+	ctxt->get_stat_sem_addr = (unsigned long)&adapter->get_eth_stat_sem;
+	atomic_inc(&ctxt->get_stat_flag);
+
+	be_rxf_query_eth_statistics(&pnob->fn_obj, adapter->eth_statsp,
+				    cpu_to_le64(pa), be_get_stat_cb, ctxt,
+				    NULL);
+
+	ctxt->get_stats_timer.data = (unsigned long)ctxt;
+	mod_timer(&ctxt->get_stats_timer, (jiffies + (HZ * 2)));
+	down((void *)ctxt->get_stat_sem_addr);	/* callback will unblock us */
+
+	/* Adding port0 and port1 stats. */
+	adapter->benet_stats.rx_packets =
+	    adapter->eth_statsp->params.response.p0recvdtotalframes +
+	    adapter->eth_statsp->params.response.p1recvdtotalframes;
+	adapter->benet_stats.tx_packets =
+	    adapter->eth_statsp->params.response.p0xmitunicastframes +
+	    adapter->eth_statsp->params.response.p1xmitunicastframes;
+	adapter->benet_stats.tx_bytes =
+	    adapter->eth_statsp->params.response.p0xmitbyteslsd +
+	    adapter->eth_statsp->params.response.p1xmitbyteslsd;
+	adapter->benet_stats.rx_errors =
+	    adapter->eth_statsp->params.response.p0crcerrors +
+	    adapter->eth_statsp->params.response.p1crcerrors;
+	adapter->benet_stats.rx_errors +=
+	    adapter->eth_statsp->params.response.p0alignmentsymerrs +
+	    adapter->eth_statsp->params.response.p1alignmentsymerrs;
+	adapter->benet_stats.rx_errors +=
+	    adapter->eth_statsp->params.response.p0inrangelenerrors +
+	    adapter->eth_statsp->params.response.p1inrangelenerrors;
+	adapter->benet_stats.rx_bytes =
+	    adapter->eth_statsp->params.response.p0recvdtotalbytesLSD +
+	    adapter->eth_statsp->params.response.p1recvdtotalbytesLSD;
+	adapter->benet_stats.rx_crc_errors =
+	    adapter->eth_statsp->params.response.p0crcerrors +
+	    adapter->eth_statsp->params.response.p1crcerrors;
+
+	adapter->benet_stats.tx_packets +=
+	    adapter->eth_statsp->params.response.p0xmitmulticastframes +
+	    adapter->eth_statsp->params.response.p1xmitmulticastframes;
+	adapter->benet_stats.tx_packets +=
+	    adapter->eth_statsp->params.response.p0xmitbroadcastframes +
+	    adapter->eth_statsp->params.response.p1xmitbroadcastframes;
+	adapter->benet_stats.tx_errors = 0;
+
+	adapter->benet_stats.multicast =
+	    adapter->eth_statsp->params.response.p0xmitmulticastframes +
+	    adapter->eth_statsp->params.response.p1xmitmulticastframes;
+
+	adapter->benet_stats.rx_fifo_errors =
+	    adapter->eth_statsp->params.response.p0rxfifooverflowdropped +
+	    adapter->eth_statsp->params.response.p1rxfifooverflowdropped;
+	adapter->benet_stats.rx_frame_errors =
+	    adapter->eth_statsp->params.response.p0alignmentsymerrs +
+	    adapter->eth_statsp->params.response.p1alignmentsymerrs;
+	adapter->benet_stats.rx_length_errors =
+	    adapter->eth_statsp->params.response.p0inrangelenerrors +
+	    adapter->eth_statsp->params.response.p1inrangelenerrors;
+	adapter->benet_stats.rx_length_errors +=
+	    adapter->eth_statsp->params.response.p0outrangeerrors +
+	    adapter->eth_statsp->params.response.p1outrangeerrors;
+	adapter->benet_stats.rx_length_errors +=
+	    adapter->eth_statsp->params.response.p0frametoolongerrors +
+	    adapter->eth_statsp->params.response.p1frametoolongerrors;
+
+	pci_unmap_single(adapter->pdev, (ulong) adapter->eth_statsp,
+			 sizeof(struct FWCMD_ETH_GET_STATISTICS),
+			 PCI_DMA_FROMDEVICE);
+	return &(adapter->benet_stats);
+
+}
+
+static void be_start_tx(struct be_net_object *pnob, u32 nposted)
+{
+#define CSR_ETH_MAX_SQPOSTS 255
+	struct SQ_DB_AMAP sqdb;
+
+	sqdb.dw[0] = 0;
+
+	AMAP_SET_BITS_PTR(SQ_DB, cid, &sqdb, pnob->tx_q_id);
+	while (nposted) {
+		if (nposted > CSR_ETH_MAX_SQPOSTS) {
+			AMAP_SET_BITS_PTR(SQ_DB, numPosted, &sqdb,
+					  CSR_ETH_MAX_SQPOSTS);
+			nposted -= CSR_ETH_MAX_SQPOSTS;
+		} else {
+			AMAP_SET_BITS_PTR(SQ_DB, numPosted, &sqdb, nposted);
+			nposted = 0;
+		}
+		PD_WRITE(&pnob->fn_obj, etx_sq_db, sqdb.dw[0]);
+	}
+
+	return;
+}
+
+static void update_tx_rate(struct be_adapter *adapter)
+{
+	/* update the rate once in two seconds */
+	if ((jiffies - adapter->eth_tx_jiffies) > 2 * (HZ)) {
+		u32 r;
+		r = adapter->eth_tx_bytes /
+		    ((jiffies - adapter->eth_tx_jiffies) / (HZ));
+		r = (r / 1000000);	/* M bytes/s */
+		adapter->be_stat.bes_eth_tx_rate = (r * 8); /* M bits/s */
+		adapter->eth_tx_jiffies = jiffies;
+		adapter->eth_tx_bytes = 0;
+	}
+}
+
+static int wrb_cnt_in_skb(struct sk_buff *skb)
+{
+	int cnt = 0;
+	while (skb) {
+		if (skb->len > skb->data_len)
+			cnt++;
+		cnt += skb_shinfo(skb)->nr_frags;
+		skb = skb_shinfo(skb)->frag_list;
+	}
+	BUG_ON(cnt > BE_MAX_TX_FRAG_COUNT);
+	return cnt;
+}
+
+static void wrb_fill(struct ETH_WRB_AMAP *wrb, u64 addr, int len)
+{
+	AMAP_SET_BITS_PTR(ETH_WRB, frag_pa_hi, wrb, addr >> 32);
+	AMAP_SET_BITS_PTR(ETH_WRB, frag_pa_lo, wrb, addr & 0xFFFFFFFF);
+	AMAP_SET_BITS_PTR(ETH_WRB, frag_len, wrb, len);
+}
+
+static void wrb_fill_extra(struct ETH_WRB_AMAP *wrb, struct sk_buff *skb,
+			   struct be_net_object *pnob)
+{
+	wrb->dw[2] = 0;
+	wrb->dw[3] = 0;
+	AMAP_SET_BITS_PTR(ETH_WRB, crc, wrb, 1);
+	if (skb_shinfo(skb)->gso_segs > 1 && skb_shinfo(skb)->gso_size) {
+		AMAP_SET_BITS_PTR(ETH_WRB, lso, wrb, 1);
+		AMAP_SET_BITS_PTR(ETH_WRB, lso_mss, wrb,
+				  skb_shinfo(skb)->gso_size);
+	} else if (skb->ip_summed == CHECKSUM_PARTIAL) {
+		u8 proto = ((struct iphdr *)ip_hdr(skb))->protocol;
+		if (proto == IPPROTO_TCP)
+			AMAP_SET_BITS_PTR(ETH_WRB, tcpcs, wrb, 1);
+		else if (proto == IPPROTO_UDP)
+			AMAP_SET_BITS_PTR(ETH_WRB, udpcs, wrb, 1);
+	}
+	if (pnob->vlan_grp && vlan_tx_tag_present(skb)) {
+		AMAP_SET_BITS_PTR(ETH_WRB, vlan, wrb, 1);
+		AMAP_SET_BITS_PTR(ETH_WRB, vlan_tag, wrb, vlan_tx_tag_get(skb));
+	}
+}
+
+static inline void wrb_copy_extra(struct ETH_WRB_AMAP *to,
+				  struct ETH_WRB_AMAP *from)
+{
+
+	to->dw[2] = from->dw[2];
+	to->dw[3] = from->dw[3];
+}
+
+/* Returns the actual count of wrbs used including a possible dummy */
+static int copy_skb_to_txq(struct be_net_object *pnob, struct sk_buff *skb,
+			   u32 wrb_cnt, u32 *copied)
+{
+	u64 busaddr;
+	struct ETH_WRB_AMAP *wrb = NULL, *first = NULL;
+	u32 i;
+	bool dummy = true;
+	struct pci_dev *pdev = pnob->adapter->pdev;
+
+	if (wrb_cnt & 1)
+		wrb_cnt++;
+	else
+		dummy = false;
+
+	atomic_add(wrb_cnt, &pnob->tx_q_used);
+
+	while (skb) {
+		if (skb->len > skb->data_len) {
+			int len = skb->len - skb->data_len;
+			busaddr = pci_map_single(pdev, skb->data, len,
+						 PCI_DMA_TODEVICE);
+			busaddr = cpu_to_le64(busaddr);
+			wrb = &pnob->tx_q[pnob->tx_q_hd];
+			if (first == NULL) {
+				wrb_fill_extra(wrb, skb, pnob);
+				first = wrb;
+			} else {
+				wrb_copy_extra(wrb, first);
+			}
+			wrb_fill(wrb, busaddr, len);
+			be_adv_txq_hd(pnob);
+			*copied += len;
+		}
+
+		for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) {
+			struct skb_frag_struct *frag =
+			    &skb_shinfo(skb)->frags[i];
+			busaddr = pci_map_page(pdev, frag->page,
+					       frag->page_offset, frag->size,
+					       PCI_DMA_TODEVICE);
+			busaddr = cpu_to_le64(busaddr);
+			wrb = &pnob->tx_q[pnob->tx_q_hd];
+			if (first == NULL) {
+				wrb_fill_extra(wrb, skb, pnob);
+				first = wrb;
+			} else {
+				wrb_copy_extra(wrb, first);
+			}
+			wrb_fill(wrb, busaddr, frag->size);
+			be_adv_txq_hd(pnob);
+			*copied += frag->size;
+		}
+		skb = skb_shinfo(skb)->frag_list;
+	}
+
+	if (dummy) {
+		wrb = &pnob->tx_q[pnob->tx_q_hd];
+		BUG_ON(first == NULL);
+		wrb_copy_extra(wrb, first);
+		wrb_fill(wrb, 0, 0);
+		be_adv_txq_hd(pnob);
+	}
+	AMAP_SET_BITS_PTR(ETH_WRB, complete, wrb, 1);
+	AMAP_SET_BITS_PTR(ETH_WRB, last, wrb, 1);
+	return wrb_cnt;
+}
+
+/* For each skb transmitted, tx_ctxt stores the num of wrbs in the
+ * start index and skb pointer in the end index
+ */
+static inline void be_tx_wrb_info_remember(struct be_net_object *pnob,
+					   struct sk_buff *skb, int wrb_cnt,
+					   u32 start)
+{
+	*(u32 *) (&pnob->tx_ctxt[start]) = wrb_cnt;
+	index_adv(&start, wrb_cnt - 1, pnob->tx_q_len);
+	pnob->tx_ctxt[start] = skb;
+}
+
+static int benet_xmit(struct sk_buff *skb, struct net_device *netdev)
+{
+	struct be_net_object *pnob = netdev_priv(netdev);
+	struct be_adapter *adapter = pnob->adapter;
+	u32 wrb_cnt, copied = 0;
+	u32 start = pnob->tx_q_hd;
+
+	adapter->be_stat.bes_tx_reqs++;
+
+	wrb_cnt = wrb_cnt_in_skb(skb);
+	spin_lock_bh(&adapter->txq_lock);
+	if ((pnob->tx_q_len - 2 - atomic_read(&pnob->tx_q_used)) <= wrb_cnt) {
+		netif_stop_queue(pnob->netdev);
+		spin_unlock_bh(&adapter->txq_lock);
+		adapter->be_stat.bes_tx_fails++;
+		return NETDEV_TX_BUSY;
+	}
+	spin_unlock_bh(&adapter->txq_lock);
+
+	wrb_cnt = copy_skb_to_txq(pnob, skb, wrb_cnt, &copied);
+	be_tx_wrb_info_remember(pnob, skb, wrb_cnt, start);
+
+	be_start_tx(pnob, wrb_cnt);
+
+	adapter->eth_tx_bytes += copied;
+	adapter->be_stat.bes_tx_wrbs += wrb_cnt;
+	update_tx_rate(adapter);
+	netdev->trans_start = jiffies;
+
+	return NETDEV_TX_OK;
+}
+
+/*
+ * This is the driver entry point to change the mtu of the device
+ * Returns 0 for success and errno for failure.
+ */
+static int benet_change_mtu(struct net_device *netdev, int new_mtu)
+{
+	/*
+	 * BE supports jumbo frame size upto 9000 bytes including the link layer
+	 * header. Considering the different variants of frame formats possible
+	 * like VLAN, SNAP/LLC, the maximum possible value for MTU is 8974 bytes
+	 */
+
+	if (new_mtu < (ETH_ZLEN + ETH_FCS_LEN) || (new_mtu > BE_MAX_MTU)) {
+		dev_info(&netdev->dev, "Invalid MTU requested. "
+			       "Must be between %d and %d bytes\n",
+				       (ETH_ZLEN + ETH_FCS_LEN), BE_MAX_MTU);
+		return -EINVAL;
+	}
+	dev_info(&netdev->dev, "MTU changed from %d to %d\n",
+						netdev->mtu, new_mtu);
+	netdev->mtu = new_mtu;
+	return 0;
+}
+
+/*
+ * This is the driver entry point to register a vlan with the device
+ */
+static void benet_vlan_register(struct net_device *netdev,
+				struct vlan_group *grp)
+{
+	struct be_net_object *pnob = netdev_priv(netdev);
+
+	be_disable_eq_intr(pnob);
+	pnob->vlan_grp = grp;
+	pnob->num_vlans = 0;
+	be_enable_eq_intr(pnob);
+}
+
+/*
+ * This is the driver entry point to add a vlan vlan_id
+ * with the device netdev
+ */
+static void benet_vlan_add_vid(struct net_device *netdev, u16 vlan_id)
+{
+	struct be_net_object *pnob = netdev_priv(netdev);
+
+	if (pnob->num_vlans == (BE_NUM_VLAN_SUPPORTED - 1)) {
+		/* no  way to return an error */
+		dev_info(&netdev->dev,
+		       "BladeEngine: Cannot configure more than %d Vlans\n",
+			       BE_NUM_VLAN_SUPPORTED);
+		return;
+	}
+	/* The new vlan tag will be in the slot indicated by num_vlans. */
+	pnob->vlan_tag[pnob->num_vlans++] = vlan_id;
+	be_rxf_vlan_config(&pnob->fn_obj, false, pnob->num_vlans,
+			   pnob->vlan_tag, NULL, NULL, NULL);
+}
+
+/*
+ * This is the driver entry point to remove a vlan vlan_id
+ * with the device netdev
+ */
+static void benet_vlan_rem_vid(struct net_device *netdev, u16 vlan_id)
+{
+	struct be_net_object *pnob = netdev_priv(netdev);
+
+	u32 i, value;
+
+	/*
+	 * In Blade Engine, we support 32 vlan tag filters across both ports.
+	 * To program a vlan tag, the RXF_RTPR_CSR register is used.
+	 * Each 32-bit value of RXF_RTDR_CSR can address 2 vlan tag entries.
+	 * The Vlan table is of depth 16. thus we support 32 tags.
+	 */
+
+	value = vlan_id | VLAN_VALID_BIT;
+	for (i = 0; i < BE_NUM_VLAN_SUPPORTED; i++) {
+		if (pnob->vlan_tag[i] == vlan_id)
+			break;
+	}
+
+	if (i == BE_NUM_VLAN_SUPPORTED)
+		return;
+	/* Now compact the vlan tag array by removing hole created. */
+	while ((i + 1) < BE_NUM_VLAN_SUPPORTED) {
+		pnob->vlan_tag[i] = pnob->vlan_tag[i + 1];
+		i++;
+	}
+	if ((i + 1) == BE_NUM_VLAN_SUPPORTED)
+		pnob->vlan_tag[i] = (u16) 0x0;
+	pnob->num_vlans--;
+	be_rxf_vlan_config(&pnob->fn_obj, false, pnob->num_vlans,
+			   pnob->vlan_tag, NULL, NULL, NULL);
+}
+
+/*
+ * This function is called to program multicast
+ * address in the multicast filter of the ASIC.
+ */
+static void be_set_multicast_filter(struct net_device *netdev)
+{
+	struct be_net_object *pnob = netdev_priv(netdev);
+	struct dev_mc_list *mc_ptr;
+	u8 mac_addr[32][ETH_ALEN];
+	int i;
+
+	if (netdev->flags & IFF_ALLMULTI) {
+		/* set BE in Multicast promiscuous */
+		be_rxf_multicast_config(&pnob->fn_obj, true, 0, NULL, NULL,
+					NULL, NULL);
+		return;
+	}
+
+	for (mc_ptr = netdev->mc_list, i = 0; mc_ptr;
+	     mc_ptr = mc_ptr->next, i++) {
+		memcpy(&mac_addr[i][0], mc_ptr->dmi_addr, ETH_ALEN);
+	}
+
+	/* reset the promiscuous mode also. */
+	be_rxf_multicast_config(&pnob->fn_obj, false, i,
+				&mac_addr[0][0], NULL, NULL, NULL);
+}
+
+/*
+ * This is the driver entry point to set multicast list
+ * with the device netdev. This function will be used to
+ * set promiscuous mode or multicast promiscuous mode
+ * or multicast mode....
+ */
+static void benet_set_multicast_list(struct net_device *netdev)
+{
+	struct be_net_object *pnob = netdev_priv(netdev);
+
+	if (netdev->flags & IFF_PROMISC) {
+		be_rxf_promiscuous(&pnob->fn_obj, 1, 1, NULL, NULL, NULL);
+	} else {
+		be_rxf_promiscuous(&pnob->fn_obj, 0, 0, NULL, NULL, NULL);
+		be_set_multicast_filter(netdev);
+	}
+}
+
+int benet_init(struct net_device *netdev)
+{
+	struct be_net_object *pnob = netdev_priv(netdev);
+	struct be_adapter *adapter = pnob->adapter;
+
+	ether_setup(netdev);
+
+	netdev->open = &benet_open;
+	netdev->stop = &benet_close;
+	netdev->hard_start_xmit = &benet_xmit;
+
+	netdev->get_stats = &benet_get_stats;
+
+	netdev->set_multicast_list = &benet_set_multicast_list;
+
+	netdev->change_mtu = &benet_change_mtu;
+	netdev->set_mac_address = &benet_set_mac_addr;
+
+	netdev->vlan_rx_register = benet_vlan_register;
+	netdev->vlan_rx_add_vid = benet_vlan_add_vid;
+	netdev->vlan_rx_kill_vid = benet_vlan_rem_vid;
+
+	netdev->features =
+	    NETIF_F_SG | NETIF_F_HIGHDMA | NETIF_F_HW_VLAN_RX | NETIF_F_TSO |
+	    NETIF_F_HW_VLAN_TX | NETIF_F_HW_VLAN_FILTER | NETIF_F_IP_CSUM;
+
+	netdev->flags |= IFF_MULTICAST;
+
+	/* If device is DAC Capable, set the HIGHDMA flag for netdevice. */
+	if (adapter->dma_64bit_cap)
+		netdev->features |= NETIF_F_HIGHDMA;
+
+	SET_ETHTOOL_OPS(netdev, &be_ethtool_ops);
+	return 0;
+}
diff --git a/drivers/staging/benet/benet.h b/drivers/staging/benet/benet.h
new file mode 100644
index 00000000000..09a1f081772
--- /dev/null
+++ b/drivers/staging/benet/benet.h
@@ -0,0 +1,429 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+#ifndef _BENET_H_
+#define _BENET_H_
+
+#include <linux/pci.h>
+#include <linux/netdevice.h>
+#include <linux/inet_lro.h>
+#include "hwlib.h"
+
+#define _SA_MODULE_NAME "net-driver"
+
+#define VLAN_VALID_BIT		0x8000
+#define BE_NUM_VLAN_SUPPORTED	32
+#define BE_PORT_LINK_DOWN       0000
+#define BE_PORT_LINK_UP         0001
+#define	BE_MAX_TX_FRAG_COUNT		(30)
+
+/* Flag bits for send operation */
+#define IPCS            (1 << 0)	/* Enable IP checksum offload */
+#define UDPCS           (1 << 1)	/* Enable UDP checksum offload */
+#define TCPCS           (1 << 2)	/* Enable TCP checksum offload */
+#define LSO             (1 << 3)	/* Enable Large Segment  offload */
+#define ETHVLAN         (1 << 4)	/* Enable VLAN insert */
+#define ETHEVENT        (1 << 5)	/* Generate  event on completion */
+#define ETHCOMPLETE     (1 << 6)	/* Generate completion when done */
+#define IPSEC           (1 << 7)	/* Enable IPSEC */
+#define FORWARD         (1 << 8)	/* Send the packet in forwarding path */
+#define FIN             (1 << 9)	/* Issue FIN segment */
+
+#define BE_MAX_MTU	8974
+
+#define BE_MAX_LRO_DESCRIPTORS			8
+#define BE_LRO_MAX_PKTS				64
+#define BE_MAX_FRAGS_PER_FRAME			6
+
+extern const char be_drvr_ver[];
+extern char be_fw_ver[];
+extern char be_driver_name[];
+
+extern struct ethtool_ops be_ethtool_ops;
+
+#define BE_DEV_STATE_NONE 0
+#define BE_DEV_STATE_INIT 1
+#define BE_DEV_STATE_OPEN 2
+#define BE_DEV_STATE_SUSPEND 3
+
+/* This structure is used to describe physical fragments to use
+ * for DMAing data from NIC.
+ */
+struct be_recv_buffer {
+	struct list_head rxb_list;	/* for maintaining a linked list */
+	void *rxb_va;		/* buffer virtual address */
+	u32 rxb_pa_lo;		/* low part of physical address */
+	u32 rxb_pa_hi;		/* high part of physical address */
+	u32 rxb_len;		/* length of recv buffer */
+	void *rxb_ctxt;		/* context for OSM driver to use */
+};
+
+/*
+ * fragment list to describe scattered data.
+ */
+struct be_tx_frag_list {
+	u32 txb_len;		/* Size of this fragment */
+	u32 txb_pa_lo;		/* Lower 32 bits of 64 bit physical addr */
+	u32 txb_pa_hi;		/* Higher 32 bits of 64 bit physical addr */
+};
+
+struct be_rx_page_info {
+	struct page *page;
+	dma_addr_t bus;
+	u16 page_offset;
+};
+
+/*
+ *  This structure is the main tracking structure for a NIC interface.
+ */
+struct be_net_object {
+	/* MCC Ring - used to send fwcmds to embedded ARM processor */
+	struct MCC_WRB_AMAP *mcc_q;	/* VA of the start of the ring */
+	u32 mcc_q_len;			/* # of WRB entries in this ring */
+	u32 mcc_q_size;
+	u32 mcc_q_hd;			/* MCC ring head */
+	u8 mcc_q_created;		/* flag to help cleanup */
+	struct be_mcc_object mcc_q_obj;	/* BECLIB's MCC ring Object */
+	dma_addr_t mcc_q_bus;		/* DMA'ble bus address */
+
+	/* MCC Completion Ring - FW responses to fwcmds sent from MCC ring */
+	struct MCC_CQ_ENTRY_AMAP *mcc_cq; /* VA of the start of the ring */
+	u32 mcc_cq_len;			/* # of compl. entries in this ring */
+	u32 mcc_cq_size;
+	u32 mcc_cq_tl;			/* compl. ring tail */
+	u8 mcc_cq_created;		/* flag to help cleanup */
+	struct be_cq_object mcc_cq_obj;	/* BECLIB's MCC compl. ring object */
+	u32 mcc_cq_id;			/* MCC ring ID */
+	dma_addr_t mcc_cq_bus;		/* DMA'ble bus address */
+
+	struct ring_desc mb_rd;		/* RD for MCC_MAIL_BOX */
+	void *mb_ptr;			/* mailbox ptr to be freed  */
+	dma_addr_t mb_bus;		/* DMA'ble bus address */
+	u32 mb_size;
+
+	/* BEClib uses an array of context objects to track outstanding
+	 * requests to the MCC.  We need allocate the same number of
+	 * conext entries as the number of entries in the MCC WRB ring
+	 */
+	u32 mcc_wrb_ctxt_size;
+	void *mcc_wrb_ctxt;		/* pointer to the context area */
+	u32 mcc_wrb_ctxtLen;		/* Number of entries in the context */
+	/*
+	 * NIC send request ring - used for xmitting raw ether frames.
+	 */
+	struct ETH_WRB_AMAP *tx_q;	/* VA of the start of the ring */
+	u32 tx_q_len;			/* # if entries in the send ring */
+	u32 tx_q_size;
+	u32 tx_q_hd;			/* Head index. Next req. goes here */
+	u32 tx_q_tl;			/* Tail indx. oldest outstanding req. */
+	u8 tx_q_created;		/* flag to help cleanup */
+	struct be_ethsq_object tx_q_obj;/* BECLIB's send Q handle */
+	dma_addr_t tx_q_bus;		/* DMA'ble bus address */
+	u32 tx_q_id;			/* send queue ring ID */
+	u32 tx_q_port;			/* 0 no binding, 1 port A,  2 port B */
+	atomic_t tx_q_used;		/* # of WRBs used */
+	/* ptr to an array in which we store context info for each send req. */
+	void **tx_ctxt;
+	/*
+	 * NIC Send compl. ring - completion status for all NIC frames xmitted.
+	 */
+	struct ETH_TX_COMPL_AMAP *tx_cq;/* VA of start of the ring */
+	u32 txcq_len;			/* # of entries in the ring */
+	u32 tx_cq_size;
+	/*
+	 * index into compl ring where the host expects next completion entry
+	 */
+	u32 tx_cq_tl;
+	u32 tx_cq_id;			/* completion queue id */
+	u8 tx_cq_created;		/* flag to help cleanup */
+	struct be_cq_object tx_cq_obj;
+	dma_addr_t tx_cq_bus;		/* DMA'ble bus address */
+	/*
+	 * Event Queue - all completion entries post events here.
+	 */
+	struct EQ_ENTRY_AMAP *event_q;	/* VA of start of event queue */
+	u32 event_q_len;		/* # of entries */
+	u32 event_q_size;
+	u32 event_q_tl;			/* Tail of the event queue */
+	u32 event_q_id;			/* Event queue ID */
+	u8 event_q_created;		/* flag to help cleanup */
+	struct be_eq_object event_q_obj; /* Queue handle */
+	dma_addr_t event_q_bus;		/* DMA'ble bus address */
+	/*
+	 * NIC receive queue - Data buffers to be used for receiving unicast,
+	 * broadcast and multi-cast frames  are posted here.
+	 */
+	struct ETH_RX_D_AMAP *rx_q;	/* VA of start of the queue */
+	u32 rx_q_len;			/* # of entries */
+	u32 rx_q_size;
+	u32 rx_q_hd;			/* Head of the queue */
+	atomic_t rx_q_posted;		/* number of posted buffers */
+	u32 rx_q_id;			/* queue ID */
+	u8 rx_q_created;		/* flag to help cleanup */
+	struct be_ethrq_object rx_q_obj;	/* NIC RX queue handle */
+	dma_addr_t rx_q_bus;		/* DMA'ble bus address */
+	/*
+	 * Pointer to an array of opaque context object for use by OSM driver
+	 */
+	void **rx_ctxt;
+	/*
+	 * NIC unicast RX completion queue - all unicast ether frame completion
+	 * statuses from BE come here.
+	 */
+	struct ETH_RX_COMPL_AMAP *rx_cq;	/* VA of start of the queue */
+	u32 rx_cq_len;		/* # of entries */
+	u32 rx_cq_size;
+	u32 rx_cq_tl;			/* Tail of the queue */
+	u32 rx_cq_id;			/* queue ID */
+	u8 rx_cq_created;		/* flag to help cleanup */
+	struct be_cq_object rx_cq_obj;	/* queue handle */
+	dma_addr_t rx_cq_bus;		/* DMA'ble bus address */
+	struct be_function_object fn_obj;	/* function object   */
+	bool	fn_obj_created;
+	u32 rx_buf_size;		/* Size of the RX buffers */
+
+	struct net_device *netdev;
+	struct be_recv_buffer eth_rx_bufs[256];	/* to pass Rx buffer
+							   addresses */
+	struct be_adapter *adapter;	/* Pointer to OSM adapter */
+	u32 devno;		/* OSM, network dev no. */
+	u32 use_port;		/* Current active port */
+	struct be_rx_page_info *rx_page_info;	/* Array of Rx buf pages */
+	u32 rx_pg_info_hd;	/* Head of queue */
+	int rxbuf_post_fail;	/* RxBuff posting fail count */
+	bool rx_pg_shared;	/* Is an allocsted page shared as two frags ? */
+	struct vlan_group *vlan_grp;
+	u32 num_vlans;		/* Number of vlans in BE's filter */
+	u16 vlan_tag[BE_NUM_VLAN_SUPPORTED]; /* vlans currently configured */
+	struct napi_struct napi;
+	struct net_lro_mgr lro_mgr;
+	struct net_lro_desc lro_desc[BE_MAX_LRO_DESCRIPTORS];
+};
+
+#define NET_FH(np)       (&(np)->fn_obj)
+
+/*
+ * BE driver statistics.
+ */
+struct be_drvr_stat {
+	u32 bes_tx_reqs;	/* number of TX requests initiated */
+	u32 bes_tx_fails;	/* number of TX requests that failed */
+	u32 bes_fwd_reqs;	/* number of send reqs through forwarding i/f */
+	u32 bes_tx_wrbs;	/* number of tx WRBs used */
+
+	u32 bes_ints;		/* number of interrupts */
+	u32 bes_polls;		/* number of times NAPI called poll function */
+	u32 bes_events;		/* total evet entries processed */
+	u32 bes_tx_events;	/* number of tx completion events  */
+	u32 bes_rx_events;	/* number of ucast rx completion events  */
+	u32 bes_tx_compl;	/* number of tx completion entries processed */
+	u32 bes_rx_compl;	/* number of rx completion entries
+				   processed */
+	u32 bes_ethrx_post_fail;	/* number of ethrx buffer alloc
+					   failures */
+	/*
+	 * number of non ether type II frames dropped where
+	 * frame len > length field of Mac Hdr
+	 */
+	u32 bes_802_3_dropped_frames;
+	/*
+	 * number of non ether type II frames malformed where
+	 * in frame len < length field of Mac Hdr
+	 */
+	u32 bes_802_3_malformed_frames;
+	u32 bes_ips;		/*  interrupts / sec */
+	u32 bes_prev_ints;	/* bes_ints at last IPS calculation  */
+	u16 bes_eth_tx_rate;	/*  ETH TX rate - Mb/sec */
+	u16 bes_eth_rx_rate;	/*  ETH RX rate - Mb/sec */
+	u32 bes_rx_coal;	/* Num pkts coalasced */
+	u32 bes_rx_flush;	/* Num times coalasced */
+	u32 bes_link_change_physical;	/*Num of times physical link changed */
+	u32 bes_link_change_virtual;	/*Num of times virtual link changed */
+	u32 bes_rx_misc_pkts;	/* Misc pkts received */
+};
+
+/* Maximum interrupt delay (in microseconds) allowed */
+#define MAX_EQD				120
+
+/*
+ * timer to prevent system shutdown hang for ever if h/w stops responding
+ */
+struct be_timer_ctxt {
+	atomic_t get_stat_flag;
+	struct timer_list get_stats_timer;
+	unsigned long get_stat_sem_addr;
+} ;
+
+/* This structure is the main BladeEngine driver context.  */
+struct be_adapter {
+	struct net_device *netdevp;
+	struct be_drvr_stat be_stat;
+	struct net_device_stats benet_stats;
+
+	/* PCI BAR mapped addresses */
+	u8 __iomem *csr_va;	/* CSR */
+	u8 __iomem *db_va;	/* Door  Bell  */
+	u8 __iomem *pci_va;	/* PCI Config */
+
+	struct tasklet_struct sts_handler;
+	struct timer_list cq_timer;
+	spinlock_t int_lock;	/* to protect the isr field in adapter */
+
+	struct FWCMD_ETH_GET_STATISTICS *eth_statsp;
+	/*
+	 * This will enable the use of ethtool to enable or disable
+	 * Checksum on Rx pkts to be obeyed or disobeyed.
+	 * If this is true = 1, then whatever is the checksum on the
+	 * Received pkt as per BE, it will be given to the stack.
+	 * Else the stack will re calculate it.
+	 */
+	bool rx_csum;
+	/*
+	 * This will enable the use of ethtool to enable or disable
+	 * Coalese on Rx pkts to be obeyed or disobeyed.
+	 * If this is grater than 0 and less than 16 then coalascing
+	 * is enabled else it is disabled
+	 */
+	u32 max_rx_coal;
+	struct pci_dev *pdev;	/* Pointer to OS's PCI dvice */
+
+	spinlock_t txq_lock;	/* to stop/wake queue based on tx_q_used */
+
+	u32 isr;		/* copy of Intr status reg. */
+
+	u32 port0_link_sts;	/* Port 0 link status */
+	u32 port1_link_sts;	/* port 1 list status */
+	struct BE_LINK_STATUS *be_link_sts;
+
+	/* pointer to the first netobject of this adapter */
+	struct be_net_object *net_obj;
+
+	/*  Flags to indicate what to clean up */
+	bool tasklet_started;
+	bool isr_registered;
+	/*
+	 * adaptive interrupt coalescing (AIC) related
+	 */
+	bool enable_aic;	/* 1 if AIC is enabled */
+	u16 min_eqd;		/* minimum EQ delay in usec */
+	u16 max_eqd;		/* minimum EQ delay in usec */
+	u16 cur_eqd;		/* current EQ delay in usec */
+	/*
+	 * book keeping for interrupt / sec and TX/RX rate calculation
+	 */
+	ulong ips_jiffies;	/* jiffies at last IPS calc */
+	u32 eth_tx_bytes;
+	ulong eth_tx_jiffies;
+	u32 eth_rx_bytes;
+	ulong eth_rx_jiffies;
+
+	struct semaphore get_eth_stat_sem;
+
+	/* timer ctxt to prevent shutdown hanging due to un-responsive BE */
+	struct be_timer_ctxt timer_ctxt;
+
+#define BE_MAX_MSIX_VECTORS             32
+#define BE_MAX_REQ_MSIX_VECTORS         1 /* only one EQ in Linux driver */
+	struct msix_entry msix_entries[BE_MAX_MSIX_VECTORS];
+	bool msix_enabled;
+	bool dma_64bit_cap;	/* the Device DAC capable  or not */
+	u8 dev_state;	/* The current state of the device */
+	u8 dev_pm_state; /* The State of device before going to suspend */
+};
+
+/*
+ * Every second we look at the ints/sec and adjust eq_delay
+ * between adapter->min_eqd and adapter->max_eqd to keep the ints/sec between
+ * IPS_HI_WM and IPS_LO_WM.
+ */
+#define IPS_HI_WM	18000
+#define IPS_LO_WM	8000
+
+
+static inline void index_adv(u32 *index, u32 val,  u32 limit)
+{
+	BUG_ON(limit & (limit-1));
+	*index = (*index + val) & (limit - 1);
+}
+
+static inline void index_inc(u32 *index, u32 limit)
+{
+	BUG_ON(limit & (limit-1));
+	*index = (*index + 1) & (limit - 1);
+}
+
+static inline void be_adv_eq_tl(struct be_net_object *pnob)
+{
+	index_inc(&pnob->event_q_tl, pnob->event_q_len);
+}
+
+static inline void be_adv_txq_hd(struct be_net_object *pnob)
+{
+	index_inc(&pnob->tx_q_hd, pnob->tx_q_len);
+}
+
+static inline void be_adv_txq_tl(struct be_net_object *pnob)
+{
+	index_inc(&pnob->tx_q_tl, pnob->tx_q_len);
+}
+
+static inline void be_adv_txcq_tl(struct be_net_object *pnob)
+{
+	index_inc(&pnob->tx_cq_tl, pnob->txcq_len);
+}
+
+static inline void be_adv_rxq_hd(struct be_net_object *pnob)
+{
+	index_inc(&pnob->rx_q_hd, pnob->rx_q_len);
+}
+
+static inline void be_adv_rxcq_tl(struct be_net_object *pnob)
+{
+	index_inc(&pnob->rx_cq_tl, pnob->rx_cq_len);
+}
+
+static inline u32 tx_compl_lastwrb_idx_get(struct be_net_object *pnob)
+{
+	return (pnob->tx_q_tl + *(u32 *)&pnob->tx_ctxt[pnob->tx_q_tl] - 1)
+		    & (pnob->tx_q_len - 1);
+}
+
+int benet_init(struct net_device *);
+int be_ethtool_ioctl(struct net_device *, struct ifreq *);
+struct net_device_stats *benet_get_stats(struct net_device *);
+void be_process_intr(unsigned long context);
+irqreturn_t be_int(int irq, void *dev);
+void be_post_eth_rx_buffs(struct be_net_object *);
+void be_get_stat_cb(void *, int, struct MCC_WRB_AMAP *);
+void be_get_stats_timer_handler(unsigned long);
+void be_wait_nic_tx_cmplx_cmpl(struct be_net_object *);
+void be_print_link_info(struct BE_LINK_STATUS *);
+void be_update_link_status(struct be_adapter *);
+void be_init_procfs(struct be_adapter *);
+void be_cleanup_procfs(struct be_adapter *);
+int be_poll(struct napi_struct *, int);
+struct ETH_RX_COMPL_AMAP *be_get_rx_cmpl(struct be_net_object *);
+void be_notify_cmpl(struct be_net_object *, int, int, int);
+void be_enable_intr(struct be_net_object *);
+void be_enable_eq_intr(struct be_net_object *);
+void be_disable_intr(struct be_net_object *);
+void be_disable_eq_intr(struct be_net_object *);
+int be_set_uc_mac_adr(struct be_net_object *, u8, u8, u8,
+		    u8 *, mcc_wrb_cqe_callback, void *);
+int be_get_flow_ctl(struct be_function_object *pFnObj, bool *, bool *);
+void process_one_tx_compl(struct be_net_object *pnob, u32 end_idx);
+
+#endif /* _BENET_H_ */
diff --git a/drivers/staging/benet/bestatus.h b/drivers/staging/benet/bestatus.h
new file mode 100644
index 00000000000..59c7a4b6222
--- /dev/null
+++ b/drivers/staging/benet/bestatus.h
@@ -0,0 +1,103 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+#ifndef _BESTATUS_H_
+#define _BESTATUS_H_
+
+#define BE_SUCCESS                      (0x00000000L)
+/*
+ * MessageId: BE_PENDING
+ *  The BladeEngine Driver call succeeded, and pended operation.
+ */
+#define BE_PENDING                       (0x20070001L)
+#define BE_STATUS_PENDING                (BE_PENDING)
+/*
+ * MessageId: BE_NOT_OK
+ *  An error occurred.
+ */
+#define BE_NOT_OK                        (0xE0070002L)
+/*
+ * MessageId: BE_STATUS_SYSTEM_RESOURCES
+ *  Insufficient host system resources exist to complete the API.
+ */
+#define BE_STATUS_SYSTEM_RESOURCES       (0xE0070003L)
+/*
+ * MessageId: BE_STATUS_CHIP_RESOURCES
+ *  Insufficient chip resources exist to complete the API.
+ */
+#define BE_STATUS_CHIP_RESOURCES         (0xE0070004L)
+/*
+ * MessageId: BE_STATUS_NO_RESOURCE
+ *  Insufficient resources to complete request.
+ */
+#define BE_STATUS_NO_RESOURCE            (0xE0070005L)
+/*
+ * MessageId: BE_STATUS_BUSY
+ *  Resource is currently busy.
+ */
+#define BE_STATUS_BUSY                   (0xE0070006L)
+/*
+ * MessageId: BE_STATUS_INVALID_PARAMETER
+ *  Invalid Parameter in request.
+ */
+#define BE_STATUS_INVALID_PARAMETER      (0xE0000007L)
+/*
+ * MessageId: BE_STATUS_NOT_SUPPORTED
+ *  Requested operation is not supported.
+ */
+#define BE_STATUS_NOT_SUPPORTED          (0xE000000DL)
+
+/*
+ * ***************************************************************************
+ *                     E T H E R N E T   S T A T U S
+ * ***************************************************************************
+ */
+
+/*
+ * MessageId: BE_ETH_TX_ERROR
+ *  The Ethernet device driver failed to transmit a packet.
+ */
+#define BE_ETH_TX_ERROR                  (0xE0070101L)
+
+/*
+ * ***************************************************************************
+ *                     S H A R E D   S T A T U S
+ * ***************************************************************************
+ */
+
+/*
+ * MessageId: BE_STATUS_VBD_INVALID_VERSION
+ *  The device driver is not compatible with this version of the VBD.
+ */
+#define BE_STATUS_INVALID_VERSION    (0xE0070402L)
+/*
+ * MessageId: BE_STATUS_DOMAIN_DENIED
+ *  The operation failed to complete due to insufficient access
+ *  rights for the requesting domain.
+ */
+#define BE_STATUS_DOMAIN_DENIED          (0xE0070403L)
+/*
+ * MessageId: BE_STATUS_TCP_NOT_STARTED
+ *  The embedded TCP/IP stack has not been started.
+ */
+#define BE_STATUS_TCP_NOT_STARTED        (0xE0070409L)
+/*
+ * MessageId: BE_STATUS_NO_MCC_WRB
+ *  No free MCC WRB are available for posting the request.
+ */
+#define BE_STATUS_NO_MCC_WRB                 (0xE0070414L)
+
+#endif /* _BESTATUS_ */
diff --git a/drivers/staging/benet/cev.h b/drivers/staging/benet/cev.h
new file mode 100644
index 00000000000..30996920a54
--- /dev/null
+++ b/drivers/staging/benet/cev.h
@@ -0,0 +1,243 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __cev_amap_h__
+#define __cev_amap_h__
+#include "ep.h"
+
+/*
+ * Host Interrupt Status Register 0. The first of four application
+ * interrupt status registers. This register contains the interrupts
+ * for Event Queues EQ0 through EQ31.
+ */
+struct BE_CEV_ISR0_CSR_AMAP {
+	u8 interrupt0;	/* DWORD 0 */
+	u8 interrupt1;	/* DWORD 0 */
+	u8 interrupt2;	/* DWORD 0 */
+	u8 interrupt3;	/* DWORD 0 */
+	u8 interrupt4;	/* DWORD 0 */
+	u8 interrupt5;	/* DWORD 0 */
+	u8 interrupt6;	/* DWORD 0 */
+	u8 interrupt7;	/* DWORD 0 */
+	u8 interrupt8;	/* DWORD 0 */
+	u8 interrupt9;	/* DWORD 0 */
+	u8 interrupt10;	/* DWORD 0 */
+	u8 interrupt11;	/* DWORD 0 */
+	u8 interrupt12;	/* DWORD 0 */
+	u8 interrupt13;	/* DWORD 0 */
+	u8 interrupt14;	/* DWORD 0 */
+	u8 interrupt15;	/* DWORD 0 */
+	u8 interrupt16;	/* DWORD 0 */
+	u8 interrupt17;	/* DWORD 0 */
+	u8 interrupt18;	/* DWORD 0 */
+	u8 interrupt19;	/* DWORD 0 */
+	u8 interrupt20;	/* DWORD 0 */
+	u8 interrupt21;	/* DWORD 0 */
+	u8 interrupt22;	/* DWORD 0 */
+	u8 interrupt23;	/* DWORD 0 */
+	u8 interrupt24;	/* DWORD 0 */
+	u8 interrupt25;	/* DWORD 0 */
+	u8 interrupt26;	/* DWORD 0 */
+	u8 interrupt27;	/* DWORD 0 */
+	u8 interrupt28;	/* DWORD 0 */
+	u8 interrupt29;	/* DWORD 0 */
+	u8 interrupt30;	/* DWORD 0 */
+	u8 interrupt31;	/* DWORD 0 */
+} __packed;
+struct CEV_ISR0_CSR_AMAP {
+	u32 dw[1];
+};
+
+/*
+ * Host Interrupt Status Register 1. The second of four application
+ * interrupt status registers. This register contains the interrupts
+ * for Event Queues EQ32 through EQ63.
+ */
+struct BE_CEV_ISR1_CSR_AMAP {
+	u8 interrupt32;	/* DWORD 0 */
+	u8 interrupt33;	/* DWORD 0 */
+	u8 interrupt34;	/* DWORD 0 */
+	u8 interrupt35;	/* DWORD 0 */
+	u8 interrupt36;	/* DWORD 0 */
+	u8 interrupt37;	/* DWORD 0 */
+	u8 interrupt38;	/* DWORD 0 */
+	u8 interrupt39;	/* DWORD 0 */
+	u8 interrupt40;	/* DWORD 0 */
+	u8 interrupt41;	/* DWORD 0 */
+	u8 interrupt42;	/* DWORD 0 */
+	u8 interrupt43;	/* DWORD 0 */
+	u8 interrupt44;	/* DWORD 0 */
+	u8 interrupt45;	/* DWORD 0 */
+	u8 interrupt46;	/* DWORD 0 */
+	u8 interrupt47;	/* DWORD 0 */
+	u8 interrupt48;	/* DWORD 0 */
+	u8 interrupt49;	/* DWORD 0 */
+	u8 interrupt50;	/* DWORD 0 */
+	u8 interrupt51;	/* DWORD 0 */
+	u8 interrupt52;	/* DWORD 0 */
+	u8 interrupt53;	/* DWORD 0 */
+	u8 interrupt54;	/* DWORD 0 */
+	u8 interrupt55;	/* DWORD 0 */
+	u8 interrupt56;	/* DWORD 0 */
+	u8 interrupt57;	/* DWORD 0 */
+	u8 interrupt58;	/* DWORD 0 */
+	u8 interrupt59;	/* DWORD 0 */
+	u8 interrupt60;	/* DWORD 0 */
+	u8 interrupt61;	/* DWORD 0 */
+	u8 interrupt62;	/* DWORD 0 */
+	u8 interrupt63;	/* DWORD 0 */
+} __packed;
+struct CEV_ISR1_CSR_AMAP {
+	u32 dw[1];
+};
+/*
+ * Host Interrupt Status Register 2. The third of four application
+ * interrupt status registers. This register contains the interrupts
+ * for Event Queues EQ64 through EQ95.
+ */
+struct BE_CEV_ISR2_CSR_AMAP {
+	u8 interrupt64;	/* DWORD 0 */
+	u8 interrupt65;	/* DWORD 0 */
+	u8 interrupt66;	/* DWORD 0 */
+	u8 interrupt67;	/* DWORD 0 */
+	u8 interrupt68;	/* DWORD 0 */
+	u8 interrupt69;	/* DWORD 0 */
+	u8 interrupt70;	/* DWORD 0 */
+	u8 interrupt71;	/* DWORD 0 */
+	u8 interrupt72;	/* DWORD 0 */
+	u8 interrupt73;	/* DWORD 0 */
+	u8 interrupt74;	/* DWORD 0 */
+	u8 interrupt75;	/* DWORD 0 */
+	u8 interrupt76;	/* DWORD 0 */
+	u8 interrupt77;	/* DWORD 0 */
+	u8 interrupt78;	/* DWORD 0 */
+	u8 interrupt79;	/* DWORD 0 */
+	u8 interrupt80;	/* DWORD 0 */
+	u8 interrupt81;	/* DWORD 0 */
+	u8 interrupt82;	/* DWORD 0 */
+	u8 interrupt83;	/* DWORD 0 */
+	u8 interrupt84;	/* DWORD 0 */
+	u8 interrupt85;	/* DWORD 0 */
+	u8 interrupt86;	/* DWORD 0 */
+	u8 interrupt87;	/* DWORD 0 */
+	u8 interrupt88;	/* DWORD 0 */
+	u8 interrupt89;	/* DWORD 0 */
+	u8 interrupt90;	/* DWORD 0 */
+	u8 interrupt91;	/* DWORD 0 */
+	u8 interrupt92;	/* DWORD 0 */
+	u8 interrupt93;	/* DWORD 0 */
+	u8 interrupt94;	/* DWORD 0 */
+	u8 interrupt95;	/* DWORD 0 */
+} __packed;
+struct CEV_ISR2_CSR_AMAP {
+	u32 dw[1];
+};
+
+/*
+ * Host Interrupt Status Register 3. The fourth of four application
+ * interrupt status registers. This register contains the interrupts
+ * for Event Queues EQ96 through EQ127.
+ */
+struct BE_CEV_ISR3_CSR_AMAP {
+	u8 interrupt96;	/* DWORD 0 */
+	u8 interrupt97;	/* DWORD 0 */
+	u8 interrupt98;	/* DWORD 0 */
+	u8 interrupt99;	/* DWORD 0 */
+	u8 interrupt100;	/* DWORD 0 */
+	u8 interrupt101;	/* DWORD 0 */
+	u8 interrupt102;	/* DWORD 0 */
+	u8 interrupt103;	/* DWORD 0 */
+	u8 interrupt104;	/* DWORD 0 */
+	u8 interrupt105;	/* DWORD 0 */
+	u8 interrupt106;	/* DWORD 0 */
+	u8 interrupt107;	/* DWORD 0 */
+	u8 interrupt108;	/* DWORD 0 */
+	u8 interrupt109;	/* DWORD 0 */
+	u8 interrupt110;	/* DWORD 0 */
+	u8 interrupt111;	/* DWORD 0 */
+	u8 interrupt112;	/* DWORD 0 */
+	u8 interrupt113;	/* DWORD 0 */
+	u8 interrupt114;	/* DWORD 0 */
+	u8 interrupt115;	/* DWORD 0 */
+	u8 interrupt116;	/* DWORD 0 */
+	u8 interrupt117;	/* DWORD 0 */
+	u8 interrupt118;	/* DWORD 0 */
+	u8 interrupt119;	/* DWORD 0 */
+	u8 interrupt120;	/* DWORD 0 */
+	u8 interrupt121;	/* DWORD 0 */
+	u8 interrupt122;	/* DWORD 0 */
+	u8 interrupt123;	/* DWORD 0 */
+	u8 interrupt124;	/* DWORD 0 */
+	u8 interrupt125;	/* DWORD 0 */
+	u8 interrupt126;	/* DWORD 0 */
+	u8 interrupt127;	/* DWORD 0 */
+} __packed;
+struct CEV_ISR3_CSR_AMAP {
+	u32 dw[1];
+};
+
+/*  Completions and Events block Registers.  */
+struct BE_CEV_CSRMAP_AMAP {
+	u8 rsvd0[32];	/* DWORD 0 */
+	u8 rsvd1[32];	/* DWORD 1 */
+	u8 rsvd2[32];	/* DWORD 2 */
+	u8 rsvd3[32];	/* DWORD 3 */
+	struct BE_CEV_ISR0_CSR_AMAP isr0;
+	struct BE_CEV_ISR1_CSR_AMAP isr1;
+	struct BE_CEV_ISR2_CSR_AMAP isr2;
+	struct BE_CEV_ISR3_CSR_AMAP isr3;
+	u8 rsvd4[32];	/* DWORD 8 */
+	u8 rsvd5[32];	/* DWORD 9 */
+	u8 rsvd6[32];	/* DWORD 10 */
+	u8 rsvd7[32];	/* DWORD 11 */
+	u8 rsvd8[32];	/* DWORD 12 */
+	u8 rsvd9[32];	/* DWORD 13 */
+	u8 rsvd10[32];	/* DWORD 14 */
+	u8 rsvd11[32];	/* DWORD 15 */
+	u8 rsvd12[32];	/* DWORD 16 */
+	u8 rsvd13[32];	/* DWORD 17 */
+	u8 rsvd14[32];	/* DWORD 18 */
+	u8 rsvd15[32];	/* DWORD 19 */
+	u8 rsvd16[32];	/* DWORD 20 */
+	u8 rsvd17[32];	/* DWORD 21 */
+	u8 rsvd18[32];	/* DWORD 22 */
+	u8 rsvd19[32];	/* DWORD 23 */
+	u8 rsvd20[32];	/* DWORD 24 */
+	u8 rsvd21[32];	/* DWORD 25 */
+	u8 rsvd22[32];	/* DWORD 26 */
+	u8 rsvd23[32];	/* DWORD 27 */
+	u8 rsvd24[32];	/* DWORD 28 */
+	u8 rsvd25[32];	/* DWORD 29 */
+	u8 rsvd26[32];	/* DWORD 30 */
+	u8 rsvd27[32];	/* DWORD 31 */
+	u8 rsvd28[32];	/* DWORD 32 */
+	u8 rsvd29[32];	/* DWORD 33 */
+	u8 rsvd30[192];	/* DWORD 34 */
+	u8 rsvd31[192];	/* DWORD 40 */
+	u8 rsvd32[160];	/* DWORD 46 */
+	u8 rsvd33[160];	/* DWORD 51 */
+	u8 rsvd34[160];	/* DWORD 56 */
+	u8 rsvd35[96];	/* DWORD 61 */
+	u8 rsvd36[192][32];	/* DWORD 64 */
+} __packed;
+struct CEV_CSRMAP_AMAP {
+	u32 dw[256];
+};
+
+#endif /* __cev_amap_h__ */
diff --git a/drivers/staging/benet/cq.c b/drivers/staging/benet/cq.c
new file mode 100644
index 00000000000..65045864543
--- /dev/null
+++ b/drivers/staging/benet/cq.c
@@ -0,0 +1,211 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+#include "hwlib.h"
+#include "bestatus.h"
+
+/*
+ * Completion Queue Objects
+ */
+/*
+ *============================================================================
+ *                  P U B L I C  R O U T I N E S
+ *============================================================================
+ */
+
+/*
+    This routine creates a completion queue based on the client completion
+    queue configuration information.
+
+
+    FunctionObject      - Handle to a function object
+    CqBaseVa            - Base VA for a the CQ ring
+    NumEntries          - CEV_CQ_CNT_* values
+    solEventEnable      - 0 = All CQEs can generate Events if CQ is eventable
+			1 = only CQEs with solicited bit set are eventable
+    eventable           - Eventable CQ, generates interrupts.
+    nodelay             - 1 = Force interrupt, relevent if CQ eventable.
+			Interrupt is asserted immediately after EQE
+			write is confirmed, regardless of EQ Timer
+			or watermark settings.
+    wme                 - Enable watermark based coalescing
+    wmThresh            - High watermark(CQ fullness at which event
+			or interrupt should be asserted).  These are the
+			CEV_WATERMARK encoded values.
+    EqObject            - EQ Handle to assign to this CQ
+    ppCqObject          - Internal CQ Handle returned.
+
+    Returns BE_SUCCESS if successfull, otherwise a useful error code is
+	returned.
+
+    IRQL < DISPATCH_LEVEL
+
+*/
+int be_cq_create(struct be_function_object *pfob,
+	struct ring_desc *rd, u32 length, bool solicited_eventable,
+	bool no_delay, u32 wm_thresh,
+	struct be_eq_object *eq_object, struct be_cq_object *cq_object)
+{
+	int status = BE_SUCCESS;
+	u32 num_entries_encoding;
+	u32 num_entries = length / sizeof(struct MCC_CQ_ENTRY_AMAP);
+	struct FWCMD_COMMON_CQ_CREATE *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	u32 n;
+	unsigned long irql;
+
+	ASSERT(rd);
+	ASSERT(cq_object);
+	ASSERT(length % sizeof(struct MCC_CQ_ENTRY_AMAP) == 0);
+
+	switch (num_entries) {
+	case 256:
+		num_entries_encoding = CEV_CQ_CNT_256;
+		break;
+	case 512:
+		num_entries_encoding = CEV_CQ_CNT_512;
+		break;
+	case 1024:
+		num_entries_encoding = CEV_CQ_CNT_1024;
+		break;
+	default:
+		ASSERT(0);
+		return BE_STATUS_INVALID_PARAMETER;
+	}
+
+	/*
+	 * All cq entries all the same size.  Use iSCSI version
+	 * as a test for the proper rd length.
+	 */
+	memset(cq_object, 0, sizeof(*cq_object));
+
+	atomic_set(&cq_object->ref_count, 0);
+	cq_object->parent_function = pfob;
+	cq_object->eq_object = eq_object;
+	cq_object->num_entries = num_entries;
+	/* save for MCC cq processing */
+	cq_object->va = rd->va;
+
+	/* map into UT. */
+	length = num_entries * sizeof(struct MCC_CQ_ENTRY_AMAP);
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		ASSERT(wrb);
+		TRACE(DL_ERR, "No free MCC WRBs in create EQ.");
+		status = BE_STATUS_NO_MCC_WRB;
+		goto Error;
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, COMMON_CQ_CREATE);
+
+	fwcmd->params.request.num_pages = PAGES_SPANNED(OFFSET_IN_PAGE(rd->va),
+									length);
+
+	AMAP_SET_BITS_PTR(CQ_CONTEXT, valid, &fwcmd->params.request.context, 1);
+	n = pfob->pci_function_number;
+	AMAP_SET_BITS_PTR(CQ_CONTEXT, Func, &fwcmd->params.request.context, n);
+
+	n = (eq_object != NULL);
+	AMAP_SET_BITS_PTR(CQ_CONTEXT, Eventable,
+				&fwcmd->params.request.context, n);
+	AMAP_SET_BITS_PTR(CQ_CONTEXT, Armed, &fwcmd->params.request.context, 1);
+
+	n = eq_object ? eq_object->eq_id : 0;
+	AMAP_SET_BITS_PTR(CQ_CONTEXT, EQID, &fwcmd->params.request.context, n);
+	AMAP_SET_BITS_PTR(CQ_CONTEXT, Count,
+			&fwcmd->params.request.context, num_entries_encoding);
+
+	n = 0; /* Protection Domain is always 0 in  Linux  driver */
+	AMAP_SET_BITS_PTR(CQ_CONTEXT, PD, &fwcmd->params.request.context, n);
+	AMAP_SET_BITS_PTR(CQ_CONTEXT, NoDelay,
+				&fwcmd->params.request.context, no_delay);
+	AMAP_SET_BITS_PTR(CQ_CONTEXT, SolEvent,
+			&fwcmd->params.request.context, solicited_eventable);
+
+	n = (wm_thresh != 0xFFFFFFFF);
+	AMAP_SET_BITS_PTR(CQ_CONTEXT, WME, &fwcmd->params.request.context, n);
+
+	n = (n ? wm_thresh : 0);
+	AMAP_SET_BITS_PTR(CQ_CONTEXT, Watermark,
+				&fwcmd->params.request.context, n);
+	/* Create a page list for the FWCMD. */
+	be_rd_to_pa_list(rd, fwcmd->params.request.pages,
+			  ARRAY_SIZE(fwcmd->params.request.pages));
+
+	/* Post the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, NULL, NULL, NULL,
+			NULL, NULL, fwcmd, NULL);
+	if (status != BE_SUCCESS) {
+		TRACE(DL_ERR, "MCC to create CQ failed.");
+		goto Error;
+	}
+	/* Remember the CQ id. */
+	cq_object->cq_id = fwcmd->params.response.cq_id;
+
+	/* insert this cq into eq_object reference */
+	if (eq_object) {
+		atomic_inc(&eq_object->ref_count);
+		list_add_tail(&cq_object->cqlist_for_eq,
+					&eq_object->cq_list_head);
+	}
+
+Error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+/*
+
+    Deferences the given object. Once the object's reference count drops to
+    zero, the object is destroyed and all resources that are held by this object
+    are released.  The on-chip context is also destroyed along with the queue
+    ID, and any mappings made into the UT.
+
+    cq_object            - CQ handle returned from cq_object_create.
+
+    returns the current reference count on the object
+
+    IRQL: IRQL < DISPATCH_LEVEL
+*/
+int be_cq_destroy(struct be_cq_object *cq_object)
+{
+	int status = 0;
+
+	/* Nothing should reference this CQ at this point. */
+	ASSERT(atomic_read(&cq_object->ref_count) == 0);
+
+	/* Send fwcmd to destroy the CQ. */
+	status = be_function_ring_destroy(cq_object->parent_function,
+		     cq_object->cq_id, FWCMD_RING_TYPE_CQ,
+					NULL, NULL, NULL, NULL);
+	ASSERT(status == 0);
+
+	/* Remove reference if this is an eventable CQ. */
+	if (cq_object->eq_object) {
+		atomic_dec(&cq_object->eq_object->ref_count);
+		list_del(&cq_object->cqlist_for_eq);
+	}
+	return BE_SUCCESS;
+}
+
diff --git a/drivers/staging/benet/descriptors.h b/drivers/staging/benet/descriptors.h
new file mode 100644
index 00000000000..8da438c407d
--- /dev/null
+++ b/drivers/staging/benet/descriptors.h
@@ -0,0 +1,71 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __descriptors_amap_h__
+#define __descriptors_amap_h__
+
+/*
+ * --- IPC_NODE_ID_ENUM ---
+ * IPC processor id values
+ */
+#define TPOST_NODE_ID                   (0)	/* TPOST ID */
+#define TPRE_NODE_ID                    (1)	/* TPRE ID */
+#define TXULP0_NODE_ID                  (2)	/* TXULP0 ID */
+#define TXULP1_NODE_ID                  (3)	/* TXULP1 ID */
+#define TXULP2_NODE_ID                  (4)	/* TXULP2 ID */
+#define RXULP0_NODE_ID                  (5)	/* RXULP0 ID */
+#define RXULP1_NODE_ID                  (6)	/* RXULP1 ID */
+#define RXULP2_NODE_ID                  (7)	/* RXULP2 ID */
+#define MPU_NODE_ID                     (15)	/* MPU ID */
+
+/*
+ * --- MAC_ID_ENUM ---
+ * Meaning of the mac_id field in rxpp_eth_d
+ */
+#define PORT0_HOST_MAC0    (0)  /* PD 0, Port 0, host networking, MAC 0. */
+#define PORT0_HOST_MAC1    (1)	/* PD 0, Port 0, host networking, MAC 1. */
+#define PORT0_STORAGE_MAC0 (2)	/* PD 0, Port 0, host storage, MAC 0. */
+#define PORT0_STORAGE_MAC1 (3)	/* PD 0, Port 0, host storage, MAC 1. */
+#define PORT1_HOST_MAC0    (4)	/* PD 0, Port 1 host networking, MAC 0. */
+#define PORT1_HOST_MAC1    (5)	/* PD 0, Port 1 host networking, MAC 1. */
+#define PORT1_STORAGE_MAC0 (6)	/* PD 0, Port 1 host storage, MAC 0. */
+#define PORT1_STORAGE_MAC1 (7)	/* PD 0, Port 1 host storage, MAC 1. */
+#define FIRST_VM_MAC       (8)	/* PD 1 MAC. Protection domains have IDs */
+				/* from 0x8-0x26, one per PD. */
+#define LAST_VM_MAC        (38)	/* PD 31 MAC. */
+#define MGMT_MAC           (39)	/* Management port MAC. */
+#define MARBLE_MAC0        (59)	/* Used for flushing function 0 receive */
+				  /*
+				   * queues before re-using a torn-down
+				   * receive ring. the DA =
+				   * 00-00-00-00-00-00, and the MSB of the
+				   * SA = 00
+				   */
+#define MARBLE_MAC1        (60)	/* Used for flushing function 1 receive */
+				  /*
+				   * queues before re-using a torn-down
+				   * receive ring. the DA =
+				   * 00-00-00-00-00-00, and the MSB of the
+				   * SA != 00
+				   */
+#define NULL_MAC           (61)	/* Promiscuous mode, indicates no match */
+#define MCAST_MAC          (62)	/* Multicast match. */
+#define BCAST_MATCH        (63)	/* Broadcast match. */
+
+#endif /* __descriptors_amap_h__ */
diff --git a/drivers/staging/benet/doorbells.h b/drivers/staging/benet/doorbells.h
new file mode 100644
index 00000000000..550cc4d5d6f
--- /dev/null
+++ b/drivers/staging/benet/doorbells.h
@@ -0,0 +1,179 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __doorbells_amap_h__
+#define __doorbells_amap_h__
+
+/* The TX/RDMA send queue doorbell. */
+struct BE_SQ_DB_AMAP {
+	u8 cid[11];		/* DWORD 0 */
+	u8 rsvd0[5];	/* DWORD 0 */
+	u8 numPosted[14];	/* DWORD 0 */
+	u8 rsvd1[2];	/* DWORD 0 */
+} __packed;
+struct SQ_DB_AMAP {
+	u32 dw[1];
+};
+
+/* The receive queue doorbell. */
+struct BE_RQ_DB_AMAP {
+	u8 rq[10];		/* DWORD 0 */
+	u8 rsvd0[13];	/* DWORD 0 */
+	u8 Invalidate;	/* DWORD 0 */
+	u8 numPosted[8];	/* DWORD 0 */
+} __packed;
+struct RQ_DB_AMAP {
+	u32 dw[1];
+};
+
+/*
+ * The CQ/EQ doorbell. Software MUST set reserved fields in this
+ * descriptor to zero, otherwise (CEV) hardware will not execute the
+ * doorbell (flagging a bad_db_qid error instead).
+ */
+struct BE_CQ_DB_AMAP {
+	u8 qid[10];		/* DWORD 0 */
+	u8 rsvd0[4];	/* DWORD 0 */
+	u8 rearm;		/* DWORD 0 */
+	u8 event;		/* DWORD 0 */
+	u8 num_popped[13];	/* DWORD 0 */
+	u8 rsvd1[3];	/* DWORD 0 */
+} __packed;
+struct CQ_DB_AMAP {
+	u32 dw[1];
+};
+
+struct BE_TPM_RQ_DB_AMAP {
+	u8 qid[10];		/* DWORD 0 */
+	u8 rsvd0[6];	/* DWORD 0 */
+	u8 numPosted[11];	/* DWORD 0 */
+	u8 mss_cnt[5];	/* DWORD 0 */
+} __packed;
+struct TPM_RQ_DB_AMAP {
+	u32 dw[1];
+};
+
+/*
+ * Post WRB Queue Doorbell Register used by the host Storage stack
+ * to notify the controller of a posted Work Request Block
+ */
+struct BE_WRB_POST_DB_AMAP {
+	u8 wrb_cid[10];	/* DWORD 0 */
+	u8 rsvd0[6];	/* DWORD 0 */
+	u8 wrb_index[8];	/* DWORD 0 */
+	u8 numberPosted[8];	/* DWORD 0 */
+} __packed;
+struct WRB_POST_DB_AMAP {
+	u32 dw[1];
+};
+
+/*
+ * Update Default PDU Queue Doorbell Register used to communicate
+ * to the controller that the driver has stopped processing the queue
+ * and where in the queue it stopped, this is
+ * a CQ Entry Type. Used by storage driver.
+ */
+struct BE_DEFAULT_PDU_DB_AMAP {
+	u8 qid[10];		/* DWORD 0 */
+	u8 rsvd0[4];	/* DWORD 0 */
+	u8 rearm;		/* DWORD 0 */
+	u8 event;		/* DWORD 0 */
+	u8 cqproc[14];	/* DWORD 0 */
+	u8 rsvd1[2];	/* DWORD 0 */
+} __packed;
+struct DEFAULT_PDU_DB_AMAP {
+	u32 dw[1];
+};
+
+/* Management Command and Controller default fragment ring */
+struct BE_MCC_DB_AMAP {
+	u8 rid[11];		/* DWORD 0 */
+	u8 rsvd0[5];	/* DWORD 0 */
+	u8 numPosted[14];	/* DWORD 0 */
+	u8 rsvd1[2];	/* DWORD 0 */
+} __packed;
+struct MCC_DB_AMAP {
+	u32 dw[1];
+};
+
+/*
+ * Used for bootstrapping the Host interface. This register is
+ * used for driver communication with the MPU when no MCC Rings exist.
+ * The software must write this register twice to post any MCC
+ * command. First, it writes the register with hi=1 and the upper bits of
+ * the  physical address for the MCC_MAILBOX structure.  Software must poll
+ * the ready bit until this is acknowledged.  Then, sotware writes the
+ * register with hi=0 with the lower bits in the address.  It must
+ * poll the ready bit until the MCC command is complete.  Upon completion,
+ * the MCC_MAILBOX will contain a valid completion queue  entry.
+ */
+struct BE_MPU_MAILBOX_DB_AMAP {
+	u8 ready;		/* DWORD 0 */
+	u8 hi;		/* DWORD 0 */
+	u8 address[30];	/* DWORD 0 */
+} __packed;
+struct MPU_MAILBOX_DB_AMAP {
+	u32 dw[1];
+};
+
+/*
+ *  This is the protection domain doorbell register map. Note that
+ *  while this map shows doorbells for all Blade Engine supported
+ *  protocols, not all of these may be valid in a given function or
+ *  protection domain. It is the responsibility of the application
+ *  accessing the doorbells to know which are valid. Each doorbell
+ *  occupies 32 bytes of space, but unless otherwise specified,
+ *  only the first 4 bytes should be written.  There are 32 instances
+ *  of these doorbells for the host and 31 virtual machines respectively.
+ *  The host and VMs will only map the doorbell pages belonging to its
+ *  protection domain. It will not be able to touch the doorbells for
+ *  another VM. The doorbells are the only registers directly accessible
+ *  by a virtual machine. Similarly, there are 511 additional
+ *  doorbells for RDMA protection domains. PD 0 for RDMA shares
+ *  the same physical protection domain doorbell page as ETH/iSCSI.
+ *
+ */
+struct BE_PROTECTION_DOMAIN_DBMAP_AMAP {
+	u8 rsvd0[512];	/* DWORD 0 */
+	struct BE_SQ_DB_AMAP rdma_sq_db;
+	u8 rsvd1[7][32];	/* DWORD 17 */
+	struct BE_WRB_POST_DB_AMAP iscsi_wrb_post_db;
+	u8 rsvd2[7][32];	/* DWORD 25 */
+	struct BE_SQ_DB_AMAP etx_sq_db;
+	u8 rsvd3[7][32];	/* DWORD 33 */
+	struct BE_RQ_DB_AMAP rdma_rq_db;
+	u8 rsvd4[7][32];	/* DWORD 41 */
+	struct BE_DEFAULT_PDU_DB_AMAP iscsi_default_pdu_db;
+	u8 rsvd5[7][32];	/* DWORD 49 */
+	struct BE_TPM_RQ_DB_AMAP tpm_rq_db;
+	u8 rsvd6[7][32];	/* DWORD 57 */
+	struct BE_RQ_DB_AMAP erx_rq_db;
+	u8 rsvd7[7][32];	/* DWORD 65 */
+	struct BE_CQ_DB_AMAP cq_db;
+	u8 rsvd8[7][32];	/* DWORD 73 */
+	struct BE_MCC_DB_AMAP mpu_mcc_db;
+	u8 rsvd9[7][32];	/* DWORD 81 */
+	struct BE_MPU_MAILBOX_DB_AMAP mcc_bootstrap_db;
+	u8 rsvd10[935][32];	/* DWORD 89 */
+} __packed;
+struct PROTECTION_DOMAIN_DBMAP_AMAP {
+	u32 dw[1024];
+};
+
+#endif /* __doorbells_amap_h__ */
diff --git a/drivers/staging/benet/ep.h b/drivers/staging/benet/ep.h
new file mode 100644
index 00000000000..72fcf64a9ff
--- /dev/null
+++ b/drivers/staging/benet/ep.h
@@ -0,0 +1,66 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __ep_amap_h__
+#define __ep_amap_h__
+
+/* General Control and Status Register. */
+struct BE_EP_CONTROL_CSR_AMAP {
+	u8 m0_RxPbuf;	/* DWORD 0 */
+	u8 m1_RxPbuf;	/* DWORD 0 */
+	u8 m2_RxPbuf;	/* DWORD 0 */
+	u8 ff_en;		/* DWORD 0 */
+	u8 rsvd0[27];	/* DWORD 0 */
+	u8 CPU_reset;	/* DWORD 0 */
+} __packed;
+struct EP_CONTROL_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Semaphore Register. */
+struct BE_EP_SEMAPHORE_CSR_AMAP {
+	u8 value[32];	/* DWORD 0 */
+} __packed;
+struct EP_SEMAPHORE_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Embedded Processor Specific Registers. */
+struct BE_EP_CSRMAP_AMAP {
+	struct BE_EP_CONTROL_CSR_AMAP ep_control;
+	u8 rsvd0[32];	/* DWORD 1 */
+	u8 rsvd1[32];	/* DWORD 2 */
+	u8 rsvd2[32];	/* DWORD 3 */
+	u8 rsvd3[32];	/* DWORD 4 */
+	u8 rsvd4[32];	/* DWORD 5 */
+	u8 rsvd5[8][128];	/* DWORD 6 */
+	u8 rsvd6[32];	/* DWORD 38 */
+	u8 rsvd7[32];	/* DWORD 39 */
+	u8 rsvd8[32];	/* DWORD 40 */
+	u8 rsvd9[32];	/* DWORD 41 */
+	u8 rsvd10[32];	/* DWORD 42 */
+	struct BE_EP_SEMAPHORE_CSR_AMAP ep_semaphore;
+	u8 rsvd11[32];	/* DWORD 44 */
+	u8 rsvd12[19][32];	/* DWORD 45 */
+} __packed;
+struct EP_CSRMAP_AMAP {
+	u32 dw[64];
+};
+
+#endif /* __ep_amap_h__ */
diff --git a/drivers/staging/benet/eq.c b/drivers/staging/benet/eq.c
new file mode 100644
index 00000000000..db92ccd8fed
--- /dev/null
+++ b/drivers/staging/benet/eq.c
@@ -0,0 +1,299 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+#include "hwlib.h"
+#include "bestatus.h"
+/*
+    This routine creates an event queue based on the client completion
+    queue configuration information.
+
+    FunctionObject      - Handle to a function object
+    EqBaseVa            - Base VA for a the EQ ring
+    SizeEncoding        - The encoded size for the EQ entries. This value is
+			either CEV_EQ_SIZE_4 or CEV_EQ_SIZE_16
+    NumEntries          - CEV_CQ_CNT_* values.
+    Watermark           - Enables watermark based coalescing.  This parameter
+			must be of the type CEV_WMARK_* if watermarks
+			are enabled.  If watermarks to to be disabled
+			this value should be-1.
+    TimerDelay          - If a timer delay is enabled this value should be the
+			time of the delay in 8 microsecond units.  If
+			delays are not used this parameter should be
+			set to -1.
+    ppEqObject          - Internal EQ Handle returned.
+
+    Returns BE_SUCCESS if successfull,, otherwise a useful error code
+	is returned.
+
+    IRQL < DISPATCH_LEVEL
+*/
+int
+be_eq_create(struct be_function_object *pfob,
+		struct ring_desc *rd, u32 eqe_size, u32 num_entries,
+		u32 watermark,	/* CEV_WMARK_* or -1 */
+		u32 timer_delay,	/* in 8us units, or -1 */
+		struct be_eq_object *eq_object)
+{
+	int status = BE_SUCCESS;
+	u32 num_entries_encoding, eqe_size_encoding, length;
+	struct FWCMD_COMMON_EQ_CREATE *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	u32 n;
+	unsigned long irql;
+
+	ASSERT(rd);
+	ASSERT(eq_object);
+
+	switch (num_entries) {
+	case 256:
+		num_entries_encoding = CEV_EQ_CNT_256;
+		break;
+	case 512:
+		num_entries_encoding = CEV_EQ_CNT_512;
+		break;
+	case 1024:
+		num_entries_encoding = CEV_EQ_CNT_1024;
+		break;
+	case 2048:
+		num_entries_encoding = CEV_EQ_CNT_2048;
+		break;
+	case 4096:
+		num_entries_encoding = CEV_EQ_CNT_4096;
+		break;
+	default:
+		ASSERT(0);
+		return BE_STATUS_INVALID_PARAMETER;
+	}
+
+	switch (eqe_size) {
+	case 4:
+		eqe_size_encoding = CEV_EQ_SIZE_4;
+		break;
+	case 16:
+		eqe_size_encoding = CEV_EQ_SIZE_16;
+		break;
+	default:
+		ASSERT(0);
+		return BE_STATUS_INVALID_PARAMETER;
+	}
+
+	if ((eqe_size == 4 && num_entries < 1024) ||
+	    (eqe_size == 16 && num_entries == 4096)) {
+		TRACE(DL_ERR, "Bad EQ size. eqe_size:%d num_entries:%d",
+		      eqe_size, num_entries);
+		ASSERT(0);
+		return BE_STATUS_INVALID_PARAMETER;
+	}
+
+	memset(eq_object, 0, sizeof(*eq_object));
+
+	atomic_set(&eq_object->ref_count, 0);
+	eq_object->parent_function = pfob;
+	eq_object->eq_id = 0xFFFFFFFF;
+
+	INIT_LIST_HEAD(&eq_object->cq_list_head);
+
+	length = num_entries * eqe_size;
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		ASSERT(wrb);
+		TRACE(DL_ERR, "No free MCC WRBs in create EQ.");
+		status = BE_STATUS_NO_MCC_WRB;
+		goto Error;
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, COMMON_EQ_CREATE);
+
+	fwcmd->params.request.num_pages = PAGES_SPANNED(OFFSET_IN_PAGE(rd->va),
+									length);
+	n = pfob->pci_function_number;
+	AMAP_SET_BITS_PTR(EQ_CONTEXT, Func, &fwcmd->params.request.context, n);
+
+	AMAP_SET_BITS_PTR(EQ_CONTEXT, valid, &fwcmd->params.request.context, 1);
+
+	AMAP_SET_BITS_PTR(EQ_CONTEXT, Size,
+			&fwcmd->params.request.context, eqe_size_encoding);
+
+	n = 0; /* Protection Domain is always 0 in  Linux  driver */
+	AMAP_SET_BITS_PTR(EQ_CONTEXT, PD, &fwcmd->params.request.context, n);
+
+	/* Let the caller ARM the EQ with the doorbell. */
+	AMAP_SET_BITS_PTR(EQ_CONTEXT, Armed, &fwcmd->params.request.context, 0);
+
+	AMAP_SET_BITS_PTR(EQ_CONTEXT, Count, &fwcmd->params.request.context,
+					num_entries_encoding);
+
+	n = pfob->pci_function_number * 32;
+	AMAP_SET_BITS_PTR(EQ_CONTEXT, EventVect,
+				&fwcmd->params.request.context, n);
+	if (watermark != -1) {
+		AMAP_SET_BITS_PTR(EQ_CONTEXT, WME,
+				&fwcmd->params.request.context, 1);
+		AMAP_SET_BITS_PTR(EQ_CONTEXT, Watermark,
+				&fwcmd->params.request.context, watermark);
+		ASSERT(watermark <= CEV_WMARK_240);
+	} else
+		AMAP_SET_BITS_PTR(EQ_CONTEXT, WME,
+					&fwcmd->params.request.context, 0);
+	if (timer_delay != -1) {
+		AMAP_SET_BITS_PTR(EQ_CONTEXT, TMR,
+					&fwcmd->params.request.context, 1);
+
+		ASSERT(timer_delay <= 250);	/* max value according to EAS */
+		timer_delay = min(timer_delay, (u32)250);
+
+		AMAP_SET_BITS_PTR(EQ_CONTEXT, Delay,
+				&fwcmd->params.request.context, timer_delay);
+	} else {
+		AMAP_SET_BITS_PTR(EQ_CONTEXT, TMR,
+				&fwcmd->params.request.context, 0);
+	}
+	/* Create a page list for the FWCMD. */
+	be_rd_to_pa_list(rd, fwcmd->params.request.pages,
+			  ARRAY_SIZE(fwcmd->params.request.pages));
+
+	status = be_function_post_mcc_wrb(pfob, wrb, NULL, NULL, NULL,
+					NULL, NULL, fwcmd, NULL);
+	if (status != BE_SUCCESS) {
+		TRACE(DL_ERR, "MCC to create EQ failed.");
+		goto Error;
+	}
+	/* Get the EQ id.  The MPU allocates the IDs. */
+	eq_object->eq_id = fwcmd->params.response.eq_id;
+
+Error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+/*
+    Deferences the given object. Once the object's reference count drops to
+    zero, the object is destroyed and all resources that are held by this
+    object are released.  The on-chip context is also destroyed along with
+    the queue ID, and any mappings made into the UT.
+
+    eq_object            - EQ handle returned from eq_object_create.
+
+    Returns BE_SUCCESS if successfull, otherwise a useful error code
+	is returned.
+
+    IRQL: IRQL < DISPATCH_LEVEL
+*/
+int be_eq_destroy(struct be_eq_object *eq_object)
+{
+	int status = 0;
+
+	ASSERT(atomic_read(&eq_object->ref_count) == 0);
+	/* no CQs should reference this EQ now */
+	ASSERT(list_empty(&eq_object->cq_list_head));
+
+	/* Send fwcmd to destroy the EQ. */
+	status = be_function_ring_destroy(eq_object->parent_function,
+			     eq_object->eq_id, FWCMD_RING_TYPE_EQ,
+					NULL, NULL, NULL, NULL);
+	ASSERT(status == 0);
+
+	return BE_SUCCESS;
+}
+/*
+ *---------------------------------------------------------------------------
+ * Function: be_eq_modify_delay
+ *   Changes the EQ delay for a group of EQs.
+ * num_eq             - The number of EQs in the eq_array to adjust.
+ * 			This also is the number of delay values in
+ * 			the eq_delay_array.
+ * eq_array           - Array of struct be_eq_object pointers to adjust.
+ * eq_delay_array     - Array of "num_eq" timer delays in units
+ * 			of microseconds. The be_eq_query_delay_range
+ * 			fwcmd returns the resolution and range of
+ *                      legal EQ delays.
+ * cb           -
+ * cb_context   -
+ * q_ctxt             - Optional. Pointer to a previously allocated
+ * 			struct. If the MCC WRB ring is full, this
+ * 			structure is used to queue the operation. It
+ *                      will be posted to the MCC ring when space
+ *                      becomes available. All queued commands will
+ *                      be posted to the ring in the order they are
+ *                      received. It is always valid to pass a pointer to
+ *                      a generic be_generic_q_cntxt. However,
+ *                      the specific context structs
+ *                      are generally smaller than the generic struct.
+ * return pend_status - BE_SUCCESS (0) on success.
+ * 			BE_PENDING (postive value) if the FWCMD
+ *                      completion is pending. Negative error code on failure.
+ *-------------------------------------------------------------------------
+ */
+int
+be_eq_modify_delay(struct be_function_object *pfob,
+		   u32 num_eq, struct be_eq_object **eq_array,
+		   u32 *eq_delay_array, mcc_wrb_cqe_callback cb,
+		   void *cb_context, struct be_eq_modify_delay_q_ctxt *q_ctxt)
+{
+	struct FWCMD_COMMON_MODIFY_EQ_DELAY *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	int status = 0;
+	struct be_generic_q_ctxt *gen_ctxt = NULL;
+	u32 i;
+	unsigned long irql;
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		if (q_ctxt && cb) {
+			wrb = (struct MCC_WRB_AMAP *) &q_ctxt->wrb_header;
+			gen_ctxt = (struct be_generic_q_ctxt *) q_ctxt;
+			gen_ctxt->context.bytes = sizeof(*q_ctxt);
+		} else {
+			status = BE_STATUS_NO_MCC_WRB;
+			goto Error;
+		}
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, COMMON_MODIFY_EQ_DELAY);
+
+	ASSERT(num_eq > 0);
+	ASSERT(num_eq <= ARRAY_SIZE(fwcmd->params.request.delay));
+	fwcmd->params.request.num_eq = num_eq;
+	for (i = 0; i < num_eq; i++) {
+		fwcmd->params.request.delay[i].eq_id = eq_array[i]->eq_id;
+		fwcmd->params.request.delay[i].delay_in_microseconds =
+		    eq_delay_array[i];
+	}
+
+	/* Post the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, gen_ctxt,
+			cb, cb_context, NULL, NULL, fwcmd, NULL);
+
+Error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
diff --git a/drivers/staging/benet/eth.c b/drivers/staging/benet/eth.c
new file mode 100644
index 00000000000..f641b6260d0
--- /dev/null
+++ b/drivers/staging/benet/eth.c
@@ -0,0 +1,1273 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+#include <linux/if_ether.h>
+#include "hwlib.h"
+#include "bestatus.h"
+
+/*
+ *---------------------------------------------------------
+ * Function: be_eth_sq_create_ex
+ *   Creates an ethernet send ring - extended version with
+ *   additional parameters.
+ * pfob -
+ * rd             - ring address
+ * length_in_bytes -
+ * type            - The type of ring to create.
+ * ulp             - The requested ULP number for the ring.
+ * 		     This should be zero based, i.e. 0,1,2. This must
+ * 		     be valid NIC ULP based on the firmware config.
+ *                   All doorbells for this ring must be sent to
+ *                   this ULP. The first network ring allocated for
+ *                   each ULP are higher performance than subsequent rings.
+ * cq_object       - cq object for completions
+ * ex_parameters   - Additional parameters (that may increase in
+ * 		     future revisions). These parameters are only used
+ * 		     for certain ring types -- see
+ *                   struct be_eth_sq_parameters for details.
+ * eth_sq          -
+ * return status   - BE_SUCCESS (0) on success. Negative error code on failure.
+ *---------------------------------------------------------
+ */
+int
+be_eth_sq_create_ex(struct be_function_object *pfob, struct ring_desc *rd,
+		u32 length, u32 type, u32 ulp, struct be_cq_object *cq_object,
+		struct be_eth_sq_parameters *ex_parameters,
+		struct be_ethsq_object *eth_sq)
+{
+	struct FWCMD_COMMON_ETH_TX_CREATE *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	int status = 0;
+	u32 n;
+	unsigned long irql;
+
+	ASSERT(rd);
+	ASSERT(eth_sq);
+	ASSERT(ex_parameters);
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	memset(eth_sq, 0, sizeof(*eth_sq));
+
+	eth_sq->parent_function = pfob;
+	eth_sq->bid = 0xFFFFFFFF;
+	eth_sq->cq_object = cq_object;
+
+	/* Translate hwlib interface to arm interface. */
+	switch (type) {
+	case BE_ETH_TX_RING_TYPE_FORWARDING:
+		type = ETH_TX_RING_TYPE_FORWARDING;
+		break;
+	case BE_ETH_TX_RING_TYPE_STANDARD:
+		type = ETH_TX_RING_TYPE_STANDARD;
+		break;
+	case BE_ETH_TX_RING_TYPE_BOUND:
+		ASSERT(ex_parameters->port < 2);
+		type = ETH_TX_RING_TYPE_BOUND;
+		break;
+	default:
+		TRACE(DL_ERR, "Invalid eth tx ring type:%d", type);
+		return BE_NOT_OK;
+		break;
+	}
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		ASSERT(wrb);
+		TRACE(DL_ERR, "No free MCC WRBs in create EQ.");
+		status = BE_STATUS_NO_MCC_WRB;
+		goto Error;
+	}
+	/* NIC must be supported by the current config. */
+	ASSERT(pfob->fw_config.nic_ulp_mask);
+
+	/*
+	 * The ulp parameter must select a valid NIC ULP
+	 * for the current config.
+	 */
+	ASSERT((1 << ulp) & pfob->fw_config.nic_ulp_mask);
+
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, COMMON_ETH_TX_CREATE);
+	fwcmd->header.request.port_number = ex_parameters->port;
+
+	AMAP_SET_BITS_PTR(ETX_CONTEXT, pd_id,
+				&fwcmd->params.request.context, 0);
+
+	n = be_ring_length_to_encoding(length, sizeof(struct ETH_WRB_AMAP));
+	AMAP_SET_BITS_PTR(ETX_CONTEXT, tx_ring_size,
+					&fwcmd->params.request.context, n);
+
+	AMAP_SET_BITS_PTR(ETX_CONTEXT, cq_id_send,
+			&fwcmd->params.request.context, cq_object->cq_id);
+
+	n = pfob->pci_function_number;
+	AMAP_SET_BITS_PTR(ETX_CONTEXT, func, &fwcmd->params.request.context, n);
+
+	fwcmd->params.request.type = type;
+	fwcmd->params.request.ulp_num  = (1 << ulp);
+	fwcmd->params.request.num_pages = DIV_ROUND_UP(length, PAGE_SIZE);
+	ASSERT(PAGES_SPANNED(rd->va, rd->length) >=
+				fwcmd->params.request.num_pages);
+
+	/* Create a page list for the FWCMD. */
+	be_rd_to_pa_list(rd, fwcmd->params.request.pages,
+			  ARRAY_SIZE(fwcmd->params.request.pages));
+
+	status = be_function_post_mcc_wrb(pfob, wrb, NULL, NULL, NULL,
+					NULL, NULL, fwcmd, NULL);
+	if (status != BE_SUCCESS) {
+		TRACE(DL_ERR, "MCC to create etx queue failed.");
+		goto Error;
+	}
+	/* save the butler ID */
+	eth_sq->bid = fwcmd->params.response.cid;
+
+	/* add a reference to the corresponding CQ */
+	atomic_inc(&cq_object->ref_count);
+
+Error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+
+/*
+    This routine destroys an ethernet send queue
+
+    EthSq - EthSq Handle returned from EthSqCreate
+
+    This function always return BE_SUCCESS.
+
+    This function frees memory allocated by EthSqCreate for the EthSq Object.
+
+*/
+int be_eth_sq_destroy(struct be_ethsq_object *eth_sq)
+{
+	int status = 0;
+
+	/* Send fwcmd to destroy the queue. */
+	status = be_function_ring_destroy(eth_sq->parent_function, eth_sq->bid,
+		     FWCMD_RING_TYPE_ETH_TX, NULL, NULL, NULL, NULL);
+	ASSERT(status == 0);
+
+	/* Derefence any associated CQs. */
+	atomic_dec(&eth_sq->cq_object->ref_count);
+	return status;
+}
+/*
+    This routine attempts to set the transmit flow control parameters.
+
+    FunctionObject      - Handle to a function object
+
+    txfc_enable         - transmit flow control enable - true for
+			  enable, false for disable
+
+    rxfc_enable         - receive flow control enable - true for
+				enable, false for disable
+
+    Returns BE_SUCCESS if successfull, otherwise a useful int error
+    code is returned.
+
+    IRQL: < DISPATCH_LEVEL
+
+    This function always fails in non-privileged machine context.
+*/
+int
+be_eth_set_flow_control(struct be_function_object *pfob,
+			bool txfc_enable, bool rxfc_enable)
+{
+	struct FWCMD_COMMON_SET_FLOW_CONTROL *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	int status = 0;
+	unsigned long irql;
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		TRACE(DL_ERR, "MCC wrb peek failed.");
+		status = BE_STATUS_NO_MCC_WRB;
+		goto error;
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, COMMON_SET_FLOW_CONTROL);
+
+	fwcmd->params.request.rx_flow_control = rxfc_enable;
+	fwcmd->params.request.tx_flow_control = txfc_enable;
+
+	/* Post the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, NULL, NULL, NULL,
+					NULL, NULL, fwcmd, NULL);
+
+	if (status != 0) {
+		TRACE(DL_ERR, "set flow control fwcmd failed.");
+		goto error;
+	}
+
+error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+/*
+    This routine attempts to get the transmit flow control parameters.
+
+    pfob      - Handle to a function object
+
+    txfc_enable         - transmit flow control enable - true for
+			enable, false for disable
+
+    rxfc_enable         - receive flow control enable - true for enable,
+			false for disable
+
+    Returns BE_SUCCESS if successfull, otherwise a useful int error code
+			is returned.
+
+    IRQL: < DISPATCH_LEVEL
+
+    This function always fails in non-privileged machine context.
+*/
+int
+be_eth_get_flow_control(struct be_function_object *pfob,
+			bool *txfc_enable, bool *rxfc_enable)
+{
+	struct FWCMD_COMMON_GET_FLOW_CONTROL *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	int status = 0;
+	unsigned long irql;
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		TRACE(DL_ERR, "MCC wrb peek failed.");
+		status = BE_STATUS_NO_MCC_WRB;
+		goto error;
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, COMMON_GET_FLOW_CONTROL);
+
+	/* Post the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, NULL, NULL, NULL,
+						NULL, NULL, fwcmd, NULL);
+
+	if (status != 0) {
+		TRACE(DL_ERR, "get flow control fwcmd failed.");
+		goto error;
+	}
+
+	*txfc_enable = fwcmd->params.response.tx_flow_control;
+	*rxfc_enable = fwcmd->params.response.rx_flow_control;
+
+error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+/*
+ *---------------------------------------------------------
+ * Function: be_eth_set_qos
+ *   This function sets the ethernet transmit Quality of Service (QoS)
+ *   characteristics of BladeEngine for the domain. All ethernet
+ *   transmit rings of the domain will evenly share the bandwidth.
+ *   The exeception to sharing is the host primary (super) ethernet
+ *   transmit ring as well as the host ethernet forwarding ring
+ *   for missed offload data.
+ * pfob -
+ * max_bps         - the maximum bits per second in units of
+ * 			10 Mbps (valid 0-100)
+ * max_pps         - the maximum packets per second in units
+ * 			of 1 Kpps (0 indicates no limit)
+ * return status   - BE_SUCCESS (0) on success. Negative error code on failure.
+ *---------------------------------------------------------
+ */
+int
+be_eth_set_qos(struct be_function_object *pfob, u32 max_bps, u32 max_pps)
+{
+	struct FWCMD_COMMON_SET_QOS *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	int status = 0;
+	unsigned long irql;
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		TRACE(DL_ERR, "MCC wrb peek failed.");
+		status = BE_STATUS_NO_MCC_WRB;
+		goto error;
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, COMMON_SET_QOS);
+
+	/* Set fields in fwcmd */
+	fwcmd->params.request.max_bits_per_second_NIC = max_bps;
+	fwcmd->params.request.max_packets_per_second_NIC = max_pps;
+	fwcmd->params.request.valid_flags = QOS_BITS_NIC | QOS_PKTS_NIC;
+
+	/* Post the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, NULL, NULL, NULL,
+					NULL, NULL, fwcmd, NULL);
+
+	if (status != 0)
+		TRACE(DL_ERR, "network set qos fwcmd failed.");
+
+error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+/*
+ *---------------------------------------------------------
+ * Function: be_eth_get_qos
+ *   This function retrieves the ethernet transmit Quality of Service (QoS)
+ *   characteristics for the domain.
+ * max_bps         - the maximum bits per second in units of
+ * 			10 Mbps (valid 0-100)
+ * max_pps         - the maximum packets per second in units of
+ * 			1 Kpps (0 indicates no limit)
+ * return status   - BE_SUCCESS (0) on success. Negative error code on failure.
+ *---------------------------------------------------------
+ */
+int
+be_eth_get_qos(struct be_function_object *pfob, u32 *max_bps, u32 *max_pps)
+{
+	struct FWCMD_COMMON_GET_QOS *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	int status = 0;
+	unsigned long irql;
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		TRACE(DL_ERR, "MCC wrb peek failed.");
+		status = BE_STATUS_NO_MCC_WRB;
+		goto error;
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, COMMON_GET_QOS);
+
+	/* Post the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, NULL, NULL, NULL,
+					NULL, NULL, fwcmd, NULL);
+
+	if (status != 0) {
+		TRACE(DL_ERR, "network get qos fwcmd failed.");
+		goto error;
+	}
+
+	*max_bps = fwcmd->params.response.max_bits_per_second_NIC;
+	*max_pps = fwcmd->params.response.max_packets_per_second_NIC;
+
+error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+/*
+ *---------------------------------------------------------
+ * Function: be_eth_set_frame_size
+ *   This function sets the ethernet maximum frame size. The previous
+ *   values are returned.
+ * pfob -
+ * tx_frame_size   - maximum transmit frame size in bytes
+ * rx_frame_size   - maximum receive frame size in bytes
+ * return status   - BE_SUCCESS (0) on success. Negative error code on failure.
+ *---------------------------------------------------------
+ */
+int
+be_eth_set_frame_size(struct be_function_object *pfob,
+		      u32 *tx_frame_size, u32 *rx_frame_size)
+{
+	struct FWCMD_COMMON_SET_FRAME_SIZE *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	int status = 0;
+	unsigned long irql;
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		TRACE(DL_ERR, "MCC wrb peek failed.");
+		status = BE_STATUS_NO_MCC_WRB;
+		goto error;
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, COMMON_SET_FRAME_SIZE);
+	fwcmd->params.request.max_tx_frame_size = *tx_frame_size;
+	fwcmd->params.request.max_rx_frame_size = *rx_frame_size;
+
+	/* Post the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, NULL, NULL, NULL,
+						NULL, NULL, fwcmd, NULL);
+
+	if (status != 0) {
+		TRACE(DL_ERR, "network set frame size fwcmd failed.");
+		goto error;
+	}
+
+	*tx_frame_size = fwcmd->params.response.chip_max_tx_frame_size;
+	*rx_frame_size = fwcmd->params.response.chip_max_rx_frame_size;
+
+error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+
+/*
+    This routine creates a Ethernet receive ring.
+
+    pfob      - handle to a function object
+    rq_base_va            - base VA for the default receive ring. this must be
+			exactly 8K in length and continguous physical memory.
+    cq_object            - handle to a previously created CQ to be associated
+			with the RQ.
+    pp_eth_rq             - pointer to an opqaue handle where an eth
+			receive object is returned.
+    Returns BE_SUCCESS if successfull, , otherwise a useful
+    int error code is returned.
+
+    IRQL: < DISPATCH_LEVEL
+    this function allocates a struct be_ethrq_object *object.
+    there must be no more than 1 of these per function object, unless the
+    function object supports RSS (is networking and on the host).
+    the rq_base_va must point to a buffer of exactly 8K.
+    the erx::host_cqid (or host_stor_cqid) register and erx::ring_page registers
+    will be updated as appropriate on return
+*/
+int
+be_eth_rq_create(struct be_function_object *pfob,
+			struct ring_desc *rd, struct be_cq_object *cq_object,
+			struct be_cq_object *bcmc_cq_object,
+			struct be_ethrq_object *eth_rq)
+{
+	int status = 0;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	struct FWCMD_COMMON_ETH_RX_CREATE *fwcmd = NULL;
+	unsigned long irql;
+
+	/* MPU will set the  */
+	ASSERT(rd);
+	ASSERT(eth_rq);
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	eth_rq->parent_function = pfob;
+	eth_rq->cq_object = cq_object;
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		TRACE(DL_ERR, "MCC wrb peek failed.");
+		status = BE_STATUS_NO_MCC_WRB;
+		goto Error;
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, COMMON_ETH_RX_CREATE);
+
+	fwcmd->params.request.num_pages = 2;	/* required length */
+	fwcmd->params.request.cq_id = cq_object->cq_id;
+
+	if (bcmc_cq_object)
+		fwcmd->params.request.bcmc_cq_id = bcmc_cq_object->cq_id;
+	else
+		fwcmd->params.request.bcmc_cq_id = 0xFFFF;
+
+	/* Create a page list for the FWCMD. */
+	be_rd_to_pa_list(rd, fwcmd->params.request.pages,
+			  ARRAY_SIZE(fwcmd->params.request.pages));
+
+	/* Post the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, NULL, NULL, NULL,
+						NULL, NULL, fwcmd, NULL);
+	if (status != BE_SUCCESS) {
+		TRACE(DL_ERR, "fwcmd to map eth rxq frags failed.");
+		goto Error;
+	}
+	/* Save the ring ID for cleanup. */
+	eth_rq->rid = fwcmd->params.response.id;
+
+	atomic_inc(&cq_object->ref_count);
+
+Error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+/*
+    This routine destroys an Ethernet receive queue
+
+    eth_rq - ethernet receive queue handle returned from eth_rq_create
+
+    Returns BE_SUCCESS on success and an appropriate int on failure.
+
+    This function frees resourcs allocated by EthRqCreate.
+    The erx::host_cqid (or host_stor_cqid) register and erx::ring_page
+    registers will be updated as appropriate on return
+    IRQL: < DISPATCH_LEVEL
+*/
+
+static void be_eth_rq_destroy_internal_cb(void *context, int status,
+					 struct MCC_WRB_AMAP *wrb)
+{
+	struct be_ethrq_object *eth_rq = (struct be_ethrq_object *) context;
+
+	if (status != BE_SUCCESS) {
+		TRACE(DL_ERR, "Destroy eth rq failed in internal callback.\n");
+	} else {
+		/* Dereference any CQs associated with this queue. */
+		atomic_dec(&eth_rq->cq_object->ref_count);
+	}
+
+	return;
+}
+
+int be_eth_rq_destroy(struct be_ethrq_object *eth_rq)
+{
+	int status = BE_SUCCESS;
+
+	/* Send fwcmd to destroy the RQ. */
+	status = be_function_ring_destroy(eth_rq->parent_function,
+			eth_rq->rid, FWCMD_RING_TYPE_ETH_RX, NULL, NULL,
+			be_eth_rq_destroy_internal_cb, eth_rq);
+
+	return status;
+}
+
+/*
+ *---------------------------------------------------------------------------
+ * Function: be_eth_rq_destroy_options
+ *   Destroys an ethernet receive ring with finer granularity options
+ *   than the standard be_eth_rq_destroy() API function.
+ * eth_rq           -
+ * flush            - Set to 1 to flush the ring, set to 0 to bypass the flush
+ * cb               - Callback function on completion
+ * cb_context       - Callback context
+ * return status    - BE_SUCCESS (0) on success. Negative error code on failure.
+ *----------------------------------------------------------------------------
+ */
+int
+be_eth_rq_destroy_options(struct be_ethrq_object *eth_rq, bool flush,
+		mcc_wrb_cqe_callback cb, void *cb_context)
+{
+	struct FWCMD_COMMON_RING_DESTROY *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	int status = BE_SUCCESS;
+	struct be_function_object *pfob = NULL;
+	unsigned long irql;
+
+	pfob = eth_rq->parent_function;
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	TRACE(DL_INFO, "Destroy eth_rq ring id:%d, flush:%d", eth_rq->rid,
+	      flush);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		ASSERT(wrb);
+		TRACE(DL_ERR, "No free MCC WRBs in destroy eth_rq ring.");
+		status = BE_STATUS_NO_MCC_WRB;
+		goto Error;
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, COMMON_RING_DESTROY);
+
+	fwcmd->params.request.id = eth_rq->rid;
+	fwcmd->params.request.ring_type = FWCMD_RING_TYPE_ETH_RX;
+	fwcmd->params.request.bypass_flush = ((0 == flush) ? 1 : 0);
+
+	/* Post the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, NULL, cb, cb_context,
+			be_eth_rq_destroy_internal_cb, eth_rq, fwcmd, NULL);
+
+	if (status != BE_SUCCESS && status != BE_PENDING) {
+		TRACE(DL_ERR, "eth_rq ring destroy failed. id:%d, flush:%d",
+		      eth_rq->rid, flush);
+		goto Error;
+	}
+
+Error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+/*
+    This routine queries the frag size for erx.
+
+    pfob      - handle to a function object
+
+    frag_size_bytes       - erx frag size in bytes that is/was set.
+
+    Returns BE_SUCCESS if successfull, otherwise a useful int error
+    code is returned.
+
+    IRQL: < DISPATCH_LEVEL
+
+*/
+int
+be_eth_rq_get_frag_size(struct be_function_object *pfob, u32 *frag_size_bytes)
+{
+	struct FWCMD_ETH_GET_RX_FRAG_SIZE *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	int status = 0;
+	unsigned long irql;
+
+	ASSERT(frag_size_bytes);
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		TRACE(DL_ERR, "MCC wrb peek failed.");
+		return BE_STATUS_NO_MCC_WRB;
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, ETH_GET_RX_FRAG_SIZE);
+
+	/* Post the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, NULL, NULL, NULL,
+				NULL, NULL, fwcmd, NULL);
+
+	if (status != 0) {
+		TRACE(DL_ERR, "get frag size fwcmd failed.");
+		goto error;
+	}
+
+	*frag_size_bytes = 1 << fwcmd->params.response.actual_fragsize_log2;
+
+error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+/*
+    This routine attempts to set the frag size for erx.  If the frag size is
+    already set, the attempt fails and the current frag size is returned.
+
+    pfob      - Handle to a function object
+
+    frag_size       - Erx frag size in bytes that is/was set.
+
+    current_frag_size_bytes    - Pointer to location where currrent frag
+				 is to be rturned
+
+    Returns BE_SUCCESS if successfull, otherwise a useful int error
+    code is returned.
+
+    IRQL: < DISPATCH_LEVEL
+
+    This function always fails in non-privileged machine context.
+*/
+int
+be_eth_rq_set_frag_size(struct be_function_object *pfob,
+			u32 frag_size, u32 *frag_size_bytes)
+{
+	struct FWCMD_ETH_SET_RX_FRAG_SIZE *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	int status = 0;
+	unsigned long irql;
+
+	ASSERT(frag_size_bytes);
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		TRACE(DL_ERR, "MCC wrb peek failed.");
+		status = BE_STATUS_NO_MCC_WRB;
+		goto error;
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, ETH_SET_RX_FRAG_SIZE);
+
+	ASSERT(frag_size >= 128 && frag_size <= 16 * 1024);
+
+	/* This is the log2 of the fragsize.  This is not the exact
+	 * ERX encoding. */
+	fwcmd->params.request.new_fragsize_log2 = __ilog2_u32(frag_size);
+
+	/* Post the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, NULL, NULL, NULL,
+				NULL, NULL, fwcmd, NULL);
+
+	if (status != 0) {
+		TRACE(DL_ERR, "set frag size fwcmd failed.");
+		goto error;
+	}
+
+	*frag_size_bytes = 1 << fwcmd->params.response.actual_fragsize_log2;
+error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+
+/*
+    This routine gets or sets a mac address for a domain
+    given the port and mac.
+
+    FunctionObject  - Function object handle.
+    port1           - Set to TRUE if this function will set/get the Port 1
+			address.  Only the host may set this to TRUE.
+    mac1            - Set to TRUE if this function will set/get the
+			MAC 1 address.  Only the host may set this to TRUE.
+    write           - Set to TRUE if this function should write the mac address.
+    mac_address      - Buffer of the mac address to read or write.
+
+    Returns BE_SUCCESS if successfull, otherwise a useful int is returned.
+
+    IRQL: < DISPATCH_LEVEL
+*/
+int be_rxf_mac_address_read_write(struct be_function_object *pfob,
+		bool port1,	/* VM must always set to false */
+		bool mac1,	/* VM must always set to false */
+		bool mgmt, bool write,
+		bool permanent, u8 *mac_address,
+		mcc_wrb_cqe_callback cb,	/* optional */
+		void *cb_context)	/* optional */
+{
+	int status = BE_SUCCESS;
+	union {
+		struct FWCMD_COMMON_NTWK_MAC_QUERY *query;
+		struct FWCMD_COMMON_NTWK_MAC_SET *set;
+	} fwcmd = {NULL};
+	struct MCC_WRB_AMAP *wrb = NULL;
+	u32 type = 0;
+	unsigned long irql;
+	struct be_mcc_wrb_response_copy rc;
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	ASSERT(mac_address);
+
+	ASSERT(port1 == false);
+	ASSERT(mac1 == false);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		TRACE(DL_ERR, "MCC wrb peek failed.");
+		status = BE_STATUS_NO_MCC_WRB;
+		goto Error;
+	}
+
+	if (mgmt) {
+		type = MAC_ADDRESS_TYPE_MANAGEMENT;
+	} else {
+		if (pfob->type == BE_FUNCTION_TYPE_NETWORK)
+			type = MAC_ADDRESS_TYPE_NETWORK;
+		else
+			type = MAC_ADDRESS_TYPE_STORAGE;
+	}
+
+	if (write) {
+		/* Prepares an embedded fwcmd, including
+		 * request/response sizes.
+		 */
+		fwcmd.set = BE_PREPARE_EMBEDDED_FWCMD(pfob,
+					       wrb, COMMON_NTWK_MAC_SET);
+
+		fwcmd.set->params.request.invalidate = 0;
+		fwcmd.set->params.request.mac1 = (mac1 ? 1 : 0);
+		fwcmd.set->params.request.port = (port1 ? 1 : 0);
+		fwcmd.set->params.request.type = type;
+
+		/* Copy the mac address to set. */
+		fwcmd.set->params.request.mac.SizeOfStructure =
+			    sizeof(fwcmd.set->params.request.mac);
+		memcpy(fwcmd.set->params.request.mac.MACAddress,
+			mac_address, ETH_ALEN);
+
+		/* Post the f/w command */
+		status = be_function_post_mcc_wrb(pfob, wrb, NULL,
+				cb, cb_context, NULL, NULL, fwcmd.set, NULL);
+
+	} else {
+
+		/*
+		 * Prepares an embedded fwcmd, including
+		 * request/response sizes.
+		 */
+		fwcmd.query = BE_PREPARE_EMBEDDED_FWCMD(pfob,
+					       wrb, COMMON_NTWK_MAC_QUERY);
+
+		fwcmd.query->params.request.mac1 = (mac1 ? 1 : 0);
+		fwcmd.query->params.request.port = (port1 ? 1 : 0);
+		fwcmd.query->params.request.type = type;
+		fwcmd.query->params.request.permanent = permanent;
+
+		rc.length = FIELD_SIZEOF(struct FWCMD_COMMON_NTWK_MAC_QUERY,
+						params.response.mac.MACAddress);
+		rc.fwcmd_offset = offsetof(struct FWCMD_COMMON_NTWK_MAC_QUERY,
+						params.response.mac.MACAddress);
+		rc.va = mac_address;
+		/* Post the f/w command (with a copy for the response) */
+		status = be_function_post_mcc_wrb(pfob, wrb, NULL, cb,
+				cb_context, NULL, NULL, fwcmd.query, &rc);
+	}
+
+	if (status < 0) {
+		TRACE(DL_ERR, "mac set/query failed.");
+		goto Error;
+	}
+
+Error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+/*
+    This routine writes data to context memory.
+
+    pfob  - Function object handle.
+    mac_table     - Set to the 128-bit multicast address hash table.
+
+    Returns BE_SUCCESS if successfull, otherwise a useful int is returned.
+
+    IRQL: < DISPATCH_LEVEL
+*/
+
+int be_rxf_multicast_config(struct be_function_object *pfob,
+		bool promiscuous, u32 num, u8 *mac_table,
+		mcc_wrb_cqe_callback cb,	/* optional */
+		void *cb_context,
+		struct be_multicast_q_ctxt *q_ctxt)
+{
+	int status = BE_SUCCESS;
+	struct FWCMD_COMMON_NTWK_MULTICAST_SET *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	struct be_generic_q_ctxt *generic_ctxt = NULL;
+	unsigned long irql;
+
+	ASSERT(num <= ARRAY_SIZE(fwcmd->params.request.mac));
+
+	if (num > ARRAY_SIZE(fwcmd->params.request.mac)) {
+		TRACE(DL_ERR, "Too many multicast addresses. BE supports %d.",
+		      (int) ARRAY_SIZE(fwcmd->params.request.mac));
+		return BE_NOT_OK;
+	}
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		if (q_ctxt && cb) {
+			wrb = (struct MCC_WRB_AMAP *) &q_ctxt->wrb_header;
+			generic_ctxt = (struct be_generic_q_ctxt *) q_ctxt;
+			generic_ctxt->context.bytes = sizeof(*q_ctxt);
+		} else {
+			status = BE_STATUS_NO_MCC_WRB;
+			goto Error;
+		}
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, COMMON_NTWK_MULTICAST_SET);
+
+	fwcmd->params.request.promiscuous = promiscuous;
+	if (!promiscuous) {
+		fwcmd->params.request.num_mac = num;
+		if (num > 0) {
+			ASSERT(mac_table);
+			memcpy(fwcmd->params.request.mac,
+						mac_table, ETH_ALEN * num);
+		}
+	}
+
+	/* Post the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, generic_ctxt,
+			cb, cb_context, NULL, NULL, fwcmd, NULL);
+	if (status < 0) {
+		TRACE(DL_ERR, "multicast fwcmd failed.");
+		goto Error;
+	}
+
+Error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+/*
+    This routine adds or removes a vlan tag from the rxf table.
+
+    FunctionObject  - Function object handle.
+    VLanTag         - VLan tag to add or remove.
+    Add             - Set to TRUE if this will add a vlan tag
+
+    Returns BE_SUCCESS if successfull, otherwise a useful int is returned.
+
+    IRQL: < DISPATCH_LEVEL
+*/
+int be_rxf_vlan_config(struct be_function_object *pfob,
+		bool promiscuous, u32 num, u16 *vlan_tag_array,
+		mcc_wrb_cqe_callback cb,	/* optional */
+		void *cb_context,
+		struct be_vlan_q_ctxt *q_ctxt)	/* optional */
+{
+	int status = BE_SUCCESS;
+	struct FWCMD_COMMON_NTWK_VLAN_CONFIG *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	struct be_generic_q_ctxt *generic_ctxt = NULL;
+	unsigned long irql;
+
+	if (num > ARRAY_SIZE(fwcmd->params.request.vlan_tag)) {
+		TRACE(DL_ERR, "Too many VLAN tags.");
+		return BE_NOT_OK;
+	}
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		if (q_ctxt && cb) {
+			wrb = (struct MCC_WRB_AMAP *) &q_ctxt->wrb_header;
+			generic_ctxt = (struct be_generic_q_ctxt *) q_ctxt;
+			generic_ctxt->context.bytes = sizeof(*q_ctxt);
+		} else {
+			status = BE_STATUS_NO_MCC_WRB;
+			goto Error;
+		}
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, COMMON_NTWK_VLAN_CONFIG);
+
+	fwcmd->params.request.promiscuous = promiscuous;
+	if (!promiscuous) {
+		fwcmd->params.request.num_vlan = num;
+
+		if (num > 0) {
+			ASSERT(vlan_tag_array);
+			memcpy(fwcmd->params.request.vlan_tag, vlan_tag_array,
+				  num * sizeof(vlan_tag_array[0]));
+		}
+	}
+
+	/* Post the commadn */
+	status = be_function_post_mcc_wrb(pfob, wrb, generic_ctxt,
+			cb, cb_context, NULL, NULL, fwcmd, NULL);
+	if (status < 0) {
+		TRACE(DL_ERR, "vlan fwcmd failed.");
+		goto Error;
+	}
+
+Error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+
+int be_rxf_link_status(struct be_function_object *pfob,
+		struct BE_LINK_STATUS *link_status,
+		mcc_wrb_cqe_callback cb,
+		void *cb_context,
+		struct be_link_status_q_ctxt *q_ctxt)
+{
+	struct FWCMD_COMMON_NTWK_LINK_STATUS_QUERY *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	int status = 0;
+	struct be_generic_q_ctxt *generic_ctxt = NULL;
+	unsigned long irql;
+	struct be_mcc_wrb_response_copy rc;
+
+	ASSERT(link_status);
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+
+	if (!wrb) {
+		if (q_ctxt && cb) {
+			wrb = (struct MCC_WRB_AMAP *) &q_ctxt->wrb_header;
+			generic_ctxt = (struct be_generic_q_ctxt *) q_ctxt;
+			generic_ctxt->context.bytes = sizeof(*q_ctxt);
+		} else {
+			status = BE_STATUS_NO_MCC_WRB;
+			goto Error;
+		}
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb,
+					       COMMON_NTWK_LINK_STATUS_QUERY);
+
+	rc.length = FIELD_SIZEOF(struct FWCMD_COMMON_NTWK_LINK_STATUS_QUERY,
+					params.response);
+	rc.fwcmd_offset = offsetof(struct FWCMD_COMMON_NTWK_LINK_STATUS_QUERY,
+					params.response);
+	rc.va = link_status;
+	/* Post or queue the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, generic_ctxt,
+			cb, cb_context, NULL, NULL, fwcmd, &rc);
+
+	if (status < 0) {
+		TRACE(DL_ERR, "link status fwcmd failed.");
+		goto Error;
+	}
+
+Error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+int
+be_rxf_query_eth_statistics(struct be_function_object *pfob,
+		    struct FWCMD_ETH_GET_STATISTICS *va_for_fwcmd,
+		    u64 pa_for_fwcmd, mcc_wrb_cqe_callback cb,
+		    void *cb_context,
+		    struct be_nonembedded_q_ctxt *q_ctxt)
+{
+	struct MCC_WRB_AMAP *wrb = NULL;
+	int status = 0;
+	struct be_generic_q_ctxt *generic_ctxt = NULL;
+	unsigned long irql;
+
+	ASSERT(va_for_fwcmd);
+	ASSERT(pa_for_fwcmd);
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+
+	if (!wrb) {
+		if (q_ctxt && cb) {
+			wrb = (struct MCC_WRB_AMAP *) &q_ctxt->wrb_header;
+			generic_ctxt = (struct be_generic_q_ctxt *) q_ctxt;
+			generic_ctxt->context.bytes = sizeof(*q_ctxt);
+		} else {
+			status = BE_STATUS_NO_MCC_WRB;
+			goto Error;
+		}
+	}
+
+	TRACE(DL_INFO, "Query eth stats. fwcmd va:%p pa:0x%08x_%08x",
+	      va_for_fwcmd, upper_32_bits(pa_for_fwcmd), (u32)pa_for_fwcmd);
+
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	va_for_fwcmd = BE_PREPARE_NONEMBEDDED_FWCMD(pfob, wrb,
+			  va_for_fwcmd, pa_for_fwcmd, ETH_GET_STATISTICS);
+
+	/* Post the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, generic_ctxt,
+		cb, cb_context, NULL, NULL, va_for_fwcmd, NULL);
+	if (status < 0) {
+		TRACE(DL_ERR, "eth stats fwcmd failed.");
+		goto Error;
+	}
+
+Error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+int
+be_rxf_promiscuous(struct be_function_object *pfob,
+		   bool enable_port0, bool enable_port1,
+		   mcc_wrb_cqe_callback cb, void *cb_context,
+		   struct be_promiscuous_q_ctxt *q_ctxt)
+{
+	struct FWCMD_ETH_PROMISCUOUS *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	int status = 0;
+	struct be_generic_q_ctxt *generic_ctxt = NULL;
+	unsigned long irql;
+
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+
+	if (!wrb) {
+		if (q_ctxt && cb) {
+			wrb = (struct MCC_WRB_AMAP *) &q_ctxt->wrb_header;
+			generic_ctxt = (struct be_generic_q_ctxt *) q_ctxt;
+			generic_ctxt->context.bytes = sizeof(*q_ctxt);
+		} else {
+			status = BE_STATUS_NO_MCC_WRB;
+			goto Error;
+		}
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, ETH_PROMISCUOUS);
+
+	fwcmd->params.request.port0_promiscuous = enable_port0;
+	fwcmd->params.request.port1_promiscuous = enable_port1;
+
+	/* Post the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, generic_ctxt,
+			cb, cb_context, NULL, NULL, fwcmd, NULL);
+
+	if (status < 0) {
+		TRACE(DL_ERR, "promiscuous fwcmd failed.");
+		goto Error;
+	}
+
+Error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+
+/*
+ *-------------------------------------------------------------------------
+ * Function: be_rxf_filter_config
+ *   Configures BladeEngine ethernet receive filter settings.
+ * pfob    -
+ * settings           - Pointer to the requested filter settings.
+ * 			The response from BladeEngine will be placed back
+ * 			in this structure.
+ * cb                 - optional
+ * cb_context         - optional
+ * q_ctxt             - Optional. Pointer to a previously allocated struct.
+ * 			If the MCC WRB ring is full, this structure is
+ * 			used to queue the operation. It will be posted
+ * 			to the MCC ring when space becomes available. All
+ *                      queued commands will be posted to the ring in
+ *                      the order they are received. It is always valid
+ *                      to pass a pointer to a generic
+ *                      be_generic_q_ctxt. However, the specific
+ *                      context structs are generally smaller than
+ *                      the generic struct.
+ * return pend_status - BE_SUCCESS (0) on success.
+ * 			BE_PENDING (postive value) if the FWCMD
+ *                      completion is pending. Negative error code on failure.
+ *---------------------------------------------------------------------------
+ */
+int
+be_rxf_filter_config(struct be_function_object *pfob,
+		     struct NTWK_RX_FILTER_SETTINGS *settings,
+		     mcc_wrb_cqe_callback cb, void *cb_context,
+		     struct be_rxf_filter_q_ctxt *q_ctxt)
+{
+	struct FWCMD_COMMON_NTWK_RX_FILTER *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	int status = 0;
+	struct be_generic_q_ctxt *generic_ctxt = NULL;
+	unsigned long irql;
+	struct be_mcc_wrb_response_copy rc;
+
+	ASSERT(settings);
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+
+	if (!wrb) {
+		if (q_ctxt && cb) {
+			wrb = (struct MCC_WRB_AMAP *) &q_ctxt->wrb_header;
+			generic_ctxt = (struct be_generic_q_ctxt *) q_ctxt;
+			generic_ctxt->context.bytes = sizeof(*q_ctxt);
+		} else {
+			status = BE_STATUS_NO_MCC_WRB;
+			goto Error;
+		}
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, COMMON_NTWK_RX_FILTER);
+	memcpy(&fwcmd->params.request, settings, sizeof(*settings));
+
+	rc.length = FIELD_SIZEOF(struct FWCMD_COMMON_NTWK_RX_FILTER,
+					params.response);
+	rc.fwcmd_offset = offsetof(struct FWCMD_COMMON_NTWK_RX_FILTER,
+					params.response);
+	rc.va = settings;
+	/* Post or queue the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, generic_ctxt,
+			cb, cb_context, NULL, NULL, fwcmd, &rc);
+
+	if (status < 0) {
+		TRACE(DL_ERR, "RXF/ERX filter config fwcmd failed.");
+		goto Error;
+	}
+
+Error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
diff --git a/drivers/staging/benet/etx_context.h b/drivers/staging/benet/etx_context.h
new file mode 100644
index 00000000000..554fbe5d127
--- /dev/null
+++ b/drivers/staging/benet/etx_context.h
@@ -0,0 +1,55 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __etx_context_amap_h__
+#define __etx_context_amap_h__
+
+/* ETX ring  context structure. */
+struct BE_ETX_CONTEXT_AMAP {
+	u8 tx_cidx[11];	/* DWORD 0 */
+	u8 rsvd0[5];	/* DWORD 0 */
+	u8 rsvd1[16];	/* DWORD 0 */
+	u8 tx_pidx[11];	/* DWORD 1 */
+	u8 rsvd2;		/* DWORD 1 */
+	u8 tx_ring_size[4];	/* DWORD 1 */
+	u8 pd_id[5];	/* DWORD 1 */
+	u8 pd_id_not_valid;	/* DWORD 1 */
+	u8 cq_id_send[10];	/* DWORD 1 */
+	u8 rsvd3[32];	/* DWORD 2 */
+	u8 rsvd4[32];	/* DWORD 3 */
+	u8 cur_bytes[32];	/* DWORD 4 */
+	u8 max_bytes[32];	/* DWORD 5 */
+	u8 time_stamp[32];	/* DWORD 6 */
+	u8 rsvd5[11];	/* DWORD 7 */
+	u8 func;		/* DWORD 7 */
+	u8 rsvd6[20];	/* DWORD 7 */
+	u8 cur_txd_count[32];	/* DWORD 8 */
+	u8 max_txd_count[32];	/* DWORD 9 */
+	u8 rsvd7[32];	/* DWORD 10 */
+	u8 rsvd8[32];	/* DWORD 11 */
+	u8 rsvd9[32];	/* DWORD 12 */
+	u8 rsvd10[32];	/* DWORD 13 */
+	u8 rsvd11[32];	/* DWORD 14 */
+	u8 rsvd12[32];	/* DWORD 15 */
+} __packed;
+struct ETX_CONTEXT_AMAP {
+	u32 dw[16];
+};
+
+#endif /* __etx_context_amap_h__ */
diff --git a/drivers/staging/benet/funcobj.c b/drivers/staging/benet/funcobj.c
new file mode 100644
index 00000000000..0f57eb58dae
--- /dev/null
+++ b/drivers/staging/benet/funcobj.c
@@ -0,0 +1,565 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+#include "hwlib.h"
+#include "bestatus.h"
+
+
+int
+be_function_internal_query_firmware_config(struct be_function_object *pfob,
+				   struct BE_FIRMWARE_CONFIG *config)
+{
+	struct FWCMD_COMMON_FIRMWARE_CONFIG *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	int status = 0;
+	unsigned long irql;
+	struct be_mcc_wrb_response_copy rc;
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		TRACE(DL_ERR, "MCC wrb peek failed.");
+		status = BE_STATUS_NO_MCC_WRB;
+		goto error;
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, COMMON_FIRMWARE_CONFIG);
+
+	rc.length = FIELD_SIZEOF(struct FWCMD_COMMON_FIRMWARE_CONFIG,
+					params.response);
+	rc.fwcmd_offset = offsetof(struct FWCMD_COMMON_FIRMWARE_CONFIG,
+					params.response);
+	rc.va = config;
+
+	/* Post the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, NULL, NULL,
+					NULL, NULL, NULL, fwcmd, &rc);
+error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+/*
+    This allocates and initializes a function object based on the information
+    provided by upper layer drivers.
+
+    Returns BE_SUCCESS on success and an appropriate int on failure.
+
+    A function object represents a single BladeEngine (logical) PCI function.
+    That is a function object either represents
+    the networking side of BladeEngine or the iSCSI side of BladeEngine.
+
+    This routine will also detect and create an appropriate PD object for the
+    PCI function as needed.
+*/
+int
+be_function_object_create(u8 __iomem *csr_va, u8 __iomem *db_va,
+		u8 __iomem *pci_va, u32 function_type,
+		struct ring_desc *mailbox, struct be_function_object *pfob)
+{
+	int status;
+
+	ASSERT(pfob);	/* not a magic assert */
+	ASSERT(function_type <= 2);
+
+	TRACE(DL_INFO, "Create function object. type:%s object:0x%p",
+	      (function_type == BE_FUNCTION_TYPE_ISCSI ? "iSCSI" :
+	       (function_type == BE_FUNCTION_TYPE_NETWORK ? "Network" :
+		"Arm")), pfob);
+
+	memset(pfob, 0, sizeof(*pfob));
+
+	pfob->type = function_type;
+	pfob->csr_va = csr_va;
+	pfob->db_va = db_va;
+	pfob->pci_va = pci_va;
+
+	spin_lock_init(&pfob->cq_lock);
+	spin_lock_init(&pfob->post_lock);
+	spin_lock_init(&pfob->mcc_context_lock);
+
+
+	pfob->pci_function_number = 1;
+
+
+	pfob->emulate = false;
+	TRACE(DL_NOTE, "Non-emulation mode");
+	status = be_drive_POST(pfob);
+	if (status != BE_SUCCESS) {
+		TRACE(DL_ERR, "BladeEngine POST failed.");
+		goto error;
+	}
+
+	/* Initialize the mailbox */
+	status = be_mpu_init_mailbox(pfob, mailbox);
+	if (status != BE_SUCCESS) {
+		TRACE(DL_ERR, "Failed to initialize mailbox.");
+		goto error;
+	}
+	/*
+	 * Cache the firmware config for ASSERTs in hwclib and later
+	 * driver queries.
+	 */
+	status = be_function_internal_query_firmware_config(pfob,
+					       &pfob->fw_config);
+	if (status != BE_SUCCESS) {
+		TRACE(DL_ERR, "Failed to query firmware config.");
+		goto error;
+	}
+
+error:
+	if (status != BE_SUCCESS) {
+		/* No cleanup necessary */
+		TRACE(DL_ERR, "Failed to create function.");
+		memset(pfob, 0, sizeof(*pfob));
+	}
+	return status;
+}
+
+/*
+    This routine drops the reference count on a given function object. Once
+    the reference count falls to zero, the function object is destroyed and all
+    resources held are freed.
+
+    FunctionObject      - The function object to drop the reference to.
+*/
+int be_function_object_destroy(struct be_function_object *pfob)
+{
+	TRACE(DL_INFO, "Destroy pfob. Object:0x%p",
+	      pfob);
+
+
+	ASSERT(pfob->mcc == NULL);
+
+	return BE_SUCCESS;
+}
+
+int be_function_cleanup(struct be_function_object *pfob)
+{
+	int status = 0;
+	u32 isr;
+	u32 host_intr;
+	struct PCICFG_HOST_TIMER_INT_CTRL_CSR_AMAP ctrl;
+
+
+	if (pfob->type == BE_FUNCTION_TYPE_NETWORK) {
+		status = be_rxf_multicast_config(pfob, false, 0,
+						NULL, NULL, NULL, NULL);
+		ASSERT(status == BE_SUCCESS);
+	}
+	/* VLAN */
+	status = be_rxf_vlan_config(pfob, false, 0, NULL, NULL, NULL, NULL);
+	ASSERT(status == BE_SUCCESS);
+	/*
+	 * MCC Queue -- Switches to mailbox mode.  May want to destroy
+	 * all but the MCC CQ before this call if polling CQ is much better
+	 * performance than polling mailbox register.
+	 */
+	if (pfob->mcc)
+		status = be_mcc_ring_destroy(pfob->mcc);
+	/*
+	 * If interrupts are disabled, clear any CEV interrupt assertions that
+	 * fired after we stopped processing EQs.
+	 */
+	ctrl.dw[0] = PCICFG1_READ(pfob, host_timer_int_ctrl);
+	host_intr = AMAP_GET_BITS_PTR(PCICFG_HOST_TIMER_INT_CTRL_CSR,
+							hostintr, ctrl.dw);
+	if (!host_intr)
+		if (pfob->type == BE_FUNCTION_TYPE_NETWORK)
+			isr = CSR_READ(pfob, cev.isr1);
+		else
+			isr = CSR_READ(pfob, cev.isr0);
+	else
+		/* This should never happen... */
+		TRACE(DL_ERR, "function_cleanup called with interrupt enabled");
+	/* Function object destroy */
+	status = be_function_object_destroy(pfob);
+	ASSERT(status == BE_SUCCESS);
+
+	return status;
+}
+
+
+void *
+be_function_prepare_embedded_fwcmd(struct be_function_object *pfob,
+	struct MCC_WRB_AMAP *wrb, u32 payld_len, u32 request_length,
+	u32 response_length, u32 opcode, u32 subsystem)
+{
+	struct FWCMD_REQUEST_HEADER *header = NULL;
+	u32 n;
+
+	ASSERT(wrb);
+
+	n = offsetof(struct BE_MCC_WRB_AMAP, payload)/8;
+	AMAP_SET_BITS_PTR(MCC_WRB, embedded, wrb, 1);
+	AMAP_SET_BITS_PTR(MCC_WRB, payload_length, wrb, min(payld_len, n));
+	header = (struct FWCMD_REQUEST_HEADER *)((u8 *)wrb + n);
+
+	header->timeout = 0;
+	header->domain = 0;
+	header->request_length = max(request_length, response_length);
+	header->opcode = opcode;
+	header->subsystem = subsystem;
+
+	return header;
+}
+
+void *
+be_function_prepare_nonembedded_fwcmd(struct be_function_object *pfob,
+	struct MCC_WRB_AMAP *wrb,
+	void *fwcmd_va, u64 fwcmd_pa,
+	u32 payld_len,
+	u32 request_length,
+	u32 response_length,
+	u32 opcode, u32 subsystem)
+{
+	struct FWCMD_REQUEST_HEADER *header = NULL;
+	u32 n;
+	struct MCC_WRB_PAYLOAD_AMAP *plp;
+
+	ASSERT(wrb);
+	ASSERT(fwcmd_va);
+
+	header = (struct FWCMD_REQUEST_HEADER *) fwcmd_va;
+
+	AMAP_SET_BITS_PTR(MCC_WRB, embedded, wrb, 0);
+	AMAP_SET_BITS_PTR(MCC_WRB, payload_length, wrb, payld_len);
+
+	/*
+	 * Assume one fragment. The caller may override the SGL by
+	 * rewriting the 0th length and adding more entries.  They
+	 * will also need to update the sge_count.
+	 */
+	AMAP_SET_BITS_PTR(MCC_WRB, sge_count, wrb, 1);
+
+	n = offsetof(struct BE_MCC_WRB_AMAP, payload)/8;
+	plp = (struct MCC_WRB_PAYLOAD_AMAP *)((u8 *)wrb + n);
+	AMAP_SET_BITS_PTR(MCC_WRB_PAYLOAD, sgl[0].length, plp, payld_len);
+	AMAP_SET_BITS_PTR(MCC_WRB_PAYLOAD, sgl[0].pa_lo, plp, (u32)fwcmd_pa);
+	AMAP_SET_BITS_PTR(MCC_WRB_PAYLOAD, sgl[0].pa_hi, plp,
+					upper_32_bits(fwcmd_pa));
+
+	header->timeout = 0;
+	header->domain = 0;
+	header->request_length = max(request_length, response_length);
+	header->opcode = opcode;
+	header->subsystem = subsystem;
+
+	return header;
+}
+
+struct MCC_WRB_AMAP *
+be_function_peek_mcc_wrb(struct be_function_object *pfob)
+{
+	struct MCC_WRB_AMAP *wrb = NULL;
+	u32 offset;
+
+	if (pfob->mcc)
+		wrb = _be_mpu_peek_ring_wrb(pfob->mcc, false);
+	else {
+		offset = offsetof(struct BE_MCC_MAILBOX_AMAP, wrb)/8;
+		wrb = (struct MCC_WRB_AMAP *) ((u8 *) pfob->mailbox.va +
+				offset);
+	}
+
+	if (wrb)
+		memset(wrb, 0, sizeof(struct MCC_WRB_AMAP));
+
+	return wrb;
+}
+
+#if defined(BE_DEBUG)
+void be_function_debug_print_wrb(struct be_function_object *pfob,
+		struct MCC_WRB_AMAP *wrb, void *optional_fwcmd_va,
+		struct be_mcc_wrb_context *wrb_context)
+{
+
+	struct FWCMD_REQUEST_HEADER *header = NULL;
+	u8 embedded;
+	u32 n;
+
+	embedded = AMAP_GET_BITS_PTR(MCC_WRB, embedded, wrb);
+
+	if (embedded) {
+		n = offsetof(struct BE_MCC_WRB_AMAP, payload)/8;
+		header = (struct FWCMD_REQUEST_HEADER *)((u8 *)wrb + n);
+	} else {
+		header = (struct FWCMD_REQUEST_HEADER *) optional_fwcmd_va;
+	}
+
+	/* Save the completed count before posting for a debug assert. */
+
+	if (header) {
+		wrb_context->opcode = header->opcode;
+		wrb_context->subsystem = header->subsystem;
+
+	} else {
+		wrb_context->opcode = 0;
+		wrb_context->subsystem = 0;
+	}
+}
+#else
+#define be_function_debug_print_wrb(a_, b_, c_, d_)
+#endif
+
+int
+be_function_post_mcc_wrb(struct be_function_object *pfob,
+		struct MCC_WRB_AMAP *wrb,
+		struct be_generic_q_ctxt *q_ctxt,
+		mcc_wrb_cqe_callback cb, void *cb_context,
+		mcc_wrb_cqe_callback internal_cb,
+		void *internal_cb_context, void *optional_fwcmd_va,
+		struct be_mcc_wrb_response_copy *rc)
+{
+	int status;
+	struct be_mcc_wrb_context *wrb_context = NULL;
+	u64 *p;
+
+	if (q_ctxt) {
+		/* Initialize context.         */
+		q_ctxt->context.internal_cb = internal_cb;
+		q_ctxt->context.internal_cb_context = internal_cb_context;
+		q_ctxt->context.cb = cb;
+		q_ctxt->context.cb_context = cb_context;
+		if (rc) {
+			q_ctxt->context.copy.length = rc->length;
+			q_ctxt->context.copy.fwcmd_offset = rc->fwcmd_offset;
+			q_ctxt->context.copy.va = rc->va;
+		} else
+			q_ctxt->context.copy.length = 0;
+
+		q_ctxt->context.optional_fwcmd_va = optional_fwcmd_va;
+
+		/* Queue this request */
+		status = be_function_queue_mcc_wrb(pfob, q_ctxt);
+
+		goto Error;
+	}
+	/*
+	 * Allocate a WRB context struct to hold the callback pointers,
+	 * status, etc.  This is required if commands complete out of order.
+	 */
+	wrb_context = _be_mcc_allocate_wrb_context(pfob);
+	if (!wrb_context) {
+		TRACE(DL_WARN, "Failed to allocate MCC WRB context.");
+		status = BE_STATUS_SYSTEM_RESOURCES;
+		goto Error;
+	}
+	/* Initialize context. */
+	memset(wrb_context, 0, sizeof(*wrb_context));
+	wrb_context->internal_cb = internal_cb;
+	wrb_context->internal_cb_context = internal_cb_context;
+	wrb_context->cb = cb;
+	wrb_context->cb_context = cb_context;
+	if (rc) {
+		wrb_context->copy.length = rc->length;
+		wrb_context->copy.fwcmd_offset = rc->fwcmd_offset;
+		wrb_context->copy.va = rc->va;
+	} else
+		wrb_context->copy.length = 0;
+	wrb_context->wrb = wrb;
+
+	/*
+	 * Copy the context pointer into the WRB opaque tag field.
+	 * Verify assumption of 64-bit tag with a compile time assert.
+	 */
+	p = (u64 *) ((u8 *)wrb + offsetof(struct BE_MCC_WRB_AMAP, tag)/8);
+	*p = (u64)(size_t)wrb_context;
+
+	/* Print info about this FWCMD for debug builds. */
+	be_function_debug_print_wrb(pfob, wrb, optional_fwcmd_va, wrb_context);
+
+	/*
+	 * issue the WRB to the MPU as appropriate
+	 */
+	if (pfob->mcc) {
+		/*
+		 * we're in WRB mode, pass to the mcc layer
+		 */
+		status = _be_mpu_post_wrb_ring(pfob->mcc, wrb, wrb_context);
+	} else {
+		/*
+		 * we're in mailbox mode
+		 */
+		status = _be_mpu_post_wrb_mailbox(pfob, wrb, wrb_context);
+
+		/* mailbox mode always completes synchronously */
+		ASSERT(status != BE_STATUS_PENDING);
+	}
+
+Error:
+
+	return status;
+}
+
+int
+be_function_ring_destroy(struct be_function_object *pfob,
+		u32 id, u32 ring_type, mcc_wrb_cqe_callback cb,
+		void *cb_context, mcc_wrb_cqe_callback internal_cb,
+		void *internal_cb_context)
+{
+
+	struct FWCMD_COMMON_RING_DESTROY *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	int status = 0;
+	unsigned long irql;
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	TRACE(DL_INFO, "Destroy ring id:%d type:%d", id, ring_type);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		ASSERT(wrb);
+		TRACE(DL_ERR, "No free MCC WRBs in destroy ring.");
+		status = BE_STATUS_NO_MCC_WRB;
+		goto Error;
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, COMMON_RING_DESTROY);
+
+	fwcmd->params.request.id = id;
+	fwcmd->params.request.ring_type = ring_type;
+
+	/* Post the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, NULL, cb, cb_context,
+				internal_cb, internal_cb_context, fwcmd, NULL);
+	if (status != BE_SUCCESS && status != BE_PENDING) {
+		TRACE(DL_ERR, "Ring destroy fwcmd failed. id:%d ring_type:%d",
+			id, ring_type);
+		goto Error;
+	}
+
+Error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+void
+be_rd_to_pa_list(struct ring_desc *rd, struct PHYS_ADDR *pa_list, u32 max_num)
+{
+	u32 num_pages = PAGES_SPANNED(rd->va, rd->length);
+	u32 i = 0;
+	u64 pa = rd->pa;
+	__le64 lepa;
+
+	ASSERT(pa_list);
+	ASSERT(pa);
+
+	for (i = 0; i < min(num_pages, max_num); i++) {
+		lepa = cpu_to_le64(pa);
+		pa_list[i].lo = (u32)lepa;
+		pa_list[i].hi = upper_32_bits(lepa);
+		pa += PAGE_SIZE;
+	}
+}
+
+
+
+/*-----------------------------------------------------------------------------
+ * Function: be_function_get_fw_version
+ *   Retrieves the firmware version on the adpater. If the callback is
+ *   NULL this call executes synchronously. If the callback is not NULL,
+ *   the returned status will be BE_PENDING if the command was issued
+ *   successfully.
+ * pfob    -
+ * fwv         - Pointer to response buffer if callback is NULL.
+ * cb           - Callback function invoked when the FWCMD completes.
+ * cb_context   - Passed to the callback function.
+ * return pend_status - BE_SUCCESS (0) on success.
+ * 			BE_PENDING (postive value) if the FWCMD
+ *                      completion is pending. Negative error code on failure.
+ *---------------------------------------------------------------------------
+ */
+int
+be_function_get_fw_version(struct be_function_object *pfob,
+		struct FWCMD_COMMON_GET_FW_VERSION_RESPONSE_PAYLOAD *fwv,
+		mcc_wrb_cqe_callback cb, void *cb_context)
+{
+	int status = BE_SUCCESS;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	struct FWCMD_COMMON_GET_FW_VERSION *fwcmd = NULL;
+	unsigned long irql;
+	struct be_mcc_wrb_response_copy rc;
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		TRACE(DL_ERR, "MCC wrb peek failed.");
+		status = BE_STATUS_NO_MCC_WRB;
+		goto Error;
+	}
+
+	if (!cb && !fwv) {
+		TRACE(DL_ERR, "callback and response buffer NULL!");
+		status = BE_NOT_OK;
+		goto Error;
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, COMMON_GET_FW_VERSION);
+
+	rc.length = FIELD_SIZEOF(struct FWCMD_COMMON_GET_FW_VERSION,
+					params.response);
+	rc.fwcmd_offset = offsetof(struct FWCMD_COMMON_GET_FW_VERSION,
+					params.response);
+	rc.va = fwv;
+
+	/* Post the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, NULL, cb,
+				cb_context, NULL, NULL, fwcmd, &rc);
+
+Error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+int
+be_function_queue_mcc_wrb(struct be_function_object *pfob,
+			  struct be_generic_q_ctxt *q_ctxt)
+{
+	int status;
+
+	ASSERT(q_ctxt);
+
+	/*
+	 * issue the WRB to the MPU as appropriate
+	 */
+	if (pfob->mcc) {
+
+		/* We're in ring mode.  Queue this item. */
+		pfob->mcc->backlog_length++;
+		list_add_tail(&q_ctxt->context.list, &pfob->mcc->backlog);
+		status = BE_PENDING;
+	} else {
+		status = BE_NOT_OK;
+	}
+	return status;
+}
+
diff --git a/drivers/staging/benet/fwcmd_common.h b/drivers/staging/benet/fwcmd_common.h
new file mode 100644
index 00000000000..406e0d6fa98
--- /dev/null
+++ b/drivers/staging/benet/fwcmd_common.h
@@ -0,0 +1,222 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __fwcmd_common_amap_h__
+#define __fwcmd_common_amap_h__
+#include "host_struct.h"
+
+/* --- PHY_LINK_DUPLEX_ENUM --- */
+#define PHY_LINK_DUPLEX_NONE            (0)
+#define PHY_LINK_DUPLEX_HALF            (1)
+#define PHY_LINK_DUPLEX_FULL            (2)
+
+/* --- PHY_LINK_SPEED_ENUM --- */
+#define PHY_LINK_SPEED_ZERO             (0)	/* No link. */
+#define PHY_LINK_SPEED_10MBPS           (1)	/* 10 Mbps */
+#define PHY_LINK_SPEED_100MBPS          (2)	/* 100 Mbps */
+#define PHY_LINK_SPEED_1GBPS            (3)	/* 1 Gbps */
+#define PHY_LINK_SPEED_10GBPS           (4)	/* 10 Gbps */
+
+/* --- PHY_LINK_FAULT_ENUM --- */
+#define PHY_LINK_FAULT_NONE             (0)	/* No fault status
+							available or detected */
+#define PHY_LINK_FAULT_LOCAL            (1)	/* Local fault detected */
+#define PHY_LINK_FAULT_REMOTE           (2)	/* Remote fault detected */
+
+/* --- BE_ULP_MASK --- */
+#define BE_ULP0_MASK                    (1)
+#define BE_ULP1_MASK                    (2)
+#define BE_ULP2_MASK                    (4)
+
+/* --- NTWK_ACTIVE_PORT --- */
+#define NTWK_PORT_A                     (0)	/* Port A is currently active */
+#define NTWK_PORT_B                     (1)	/* Port B is currently active */
+#define NTWK_NO_ACTIVE_PORT             (15)	/* Both ports have lost link */
+
+/* --- NTWK_LINK_TYPE --- */
+#define NTWK_LINK_TYPE_PHYSICAL         (0)	/* link up/down event
+						   applies to BladeEngine's
+						   Physical Ports
+						   */
+#define NTWK_LINK_TYPE_VIRTUAL          (1)	/* Virtual link up/down event
+						   reported by BladeExchange.
+						   This applies only when the
+						   VLD feature is enabled
+						   */
+
+/*
+ * --- FWCMD_MAC_TYPE_ENUM ---
+ * This enum defines the types of MAC addresses in the RXF MAC Address Table.
+ */
+#define MAC_ADDRESS_TYPE_STORAGE        (0)	/* Storage MAC Address */
+#define MAC_ADDRESS_TYPE_NETWORK        (1)	/* Network MAC Address */
+#define MAC_ADDRESS_TYPE_PD             (2)	/* Protection Domain MAC Addr */
+#define MAC_ADDRESS_TYPE_MANAGEMENT     (3)	/* Managment MAC Address */
+
+
+/* --- FWCMD_RING_TYPE_ENUM --- */
+#define FWCMD_RING_TYPE_ETH_RX          (1)	/* Ring created with */
+					/* FWCMD_COMMON_ETH_RX_CREATE. */
+#define FWCMD_RING_TYPE_ETH_TX          (2)	/* Ring created with */
+					/* FWCMD_COMMON_ETH_TX_CREATE. */
+#define FWCMD_RING_TYPE_ISCSI_WRBQ      (3)	/* Ring created with */
+					/* FWCMD_COMMON_ISCSI_WRBQ_CREATE. */
+#define FWCMD_RING_TYPE_ISCSI_DEFQ      (4)	/* Ring created with */
+					/* FWCMD_COMMON_ISCSI_DEFQ_CREATE. */
+#define FWCMD_RING_TYPE_TPM_WRBQ        (5)	/* Ring created with */
+					/* FWCMD_COMMON_TPM_WRBQ_CREATE. */
+#define FWCMD_RING_TYPE_TPM_DEFQ        (6)	/* Ring created with */
+					/* FWCMD_COMMONTPM_TDEFQ_CREATE. */
+#define FWCMD_RING_TYPE_TPM_RQ          (7)	/* Ring created with */
+					/* FWCMD_COMMON_TPM_RQ_CREATE. */
+#define FWCMD_RING_TYPE_MCC             (8)	/* Ring created with */
+					/* FWCMD_COMMON_MCC_CREATE. */
+#define FWCMD_RING_TYPE_CQ              (9)	/* Ring created with */
+					/* FWCMD_COMMON_CQ_CREATE. */
+#define FWCMD_RING_TYPE_EQ              (10)	/* Ring created with */
+					/* FWCMD_COMMON_EQ_CREATE. */
+#define FWCMD_RING_TYPE_QP              (11)	/* Ring created with */
+					/* FWCMD_RDMA_QP_CREATE. */
+
+
+/* --- ETH_TX_RING_TYPE_ENUM --- */
+#define ETH_TX_RING_TYPE_FORWARDING     (1)	/* Ethernet ring for
+						   forwarding packets */
+#define ETH_TX_RING_TYPE_STANDARD       (2)	/* Ethernet ring for sending
+						   network packets. */
+#define ETH_TX_RING_TYPE_BOUND          (3)	/* Ethernet ring bound to the
+						   port specified in the command
+						   header.port_number field.
+						   Rings of this type are
+						   NOT subject to the
+						   failover logic implemented
+						   in the BladeEngine.
+						   */
+
+/* --- FWCMD_COMMON_QOS_TYPE_ENUM --- */
+#define QOS_BITS_NIC                    (1)	/* max_bits_per_second_NIC */
+						  /* field is valid.  */
+#define QOS_PKTS_NIC                    (2)	/* max_packets_per_second_NIC */
+						  /* field is valid.  */
+#define QOS_IOPS_ISCSI                  (4)	/* max_ios_per_second_iSCSI */
+						  /*field is valid.  */
+#define QOS_VLAN_TAG                    (8)	/* domain_VLAN_tag field
+						   is valid. */
+#define QOS_FABRIC_ID                   (16)	/* fabric_domain_ID field
+						   is valid. */
+#define QOS_OEM_PARAMS                  (32)	/* qos_params_oem field
+						   is valid. */
+#define QOS_TPUT_ISCSI                  (64)	/* max_bytes_per_second_iSCSI
+						   field  is valid.  */
+
+
+/*
+ * --- FAILOVER_CONFIG_ENUM ---
+ * Failover configuration setting used in FWCMD_COMMON_FORCE_FAILOVER
+ */
+#define FAILOVER_CONFIG_NO_CHANGE       (0)	/* No change to automatic */
+						  /* port failover setting. */
+#define FAILOVER_CONFIG_ON              (1)	/* Automatic port failover
+						   on link down  is enabled. */
+#define FAILOVER_CONFIG_OFF             (2)	/* Automatic port failover
+						   on link down is disabled. */
+
+/*
+ * --- FAILOVER_PORT_ENUM ---
+ * Failover port setting used in FWCMD_COMMON_FORCE_FAILOVER
+ */
+#define FAILOVER_PORT_A                 (0)	/* Selects port A. */
+#define FAILOVER_PORT_B                 (1)	/* Selects port B. */
+#define FAILOVER_PORT_NONE              (15)	/* No port change requested. */
+
+
+/*
+ * --- MGMT_FLASHROM_OPCODE ---
+ * Flash ROM operation code
+ */
+#define MGMT_FLASHROM_OPCODE_FLASH      (1)	/* Commit downloaded data
+						   to Flash ROM */
+#define MGMT_FLASHROM_OPCODE_SAVE       (2)	/* Save downloaded data to
+						   ARM's DDR - do not flash */
+#define MGMT_FLASHROM_OPCODE_CLEAR      (3)	/* Erase specified component
+						   from FlashROM */
+#define MGMT_FLASHROM_OPCODE_REPORT     (4)	/* Read specified component
+						   from Flash ROM */
+#define MGMT_FLASHROM_OPCODE_IMAGE_INFO (5)	/* Returns size of a
+						   component */
+
+/*
+ * --- MGMT_FLASHROM_OPTYPE ---
+ * Flash ROM operation type
+ */
+#define MGMT_FLASHROM_OPTYPE_CODE_FIRMWARE (0)	/* Includes ARM firmware,
+						   IPSec (optional) and EP
+						   firmware  */
+#define MGMT_FLASHROM_OPTYPE_CODE_REDBOOT (1)
+#define MGMT_FLASHROM_OPTYPE_CODE_BIOS  (2)
+#define MGMT_FLASHROM_OPTYPE_CODE_PXE_BIOS (3)
+#define MGMT_FLASHROM_OPTYPE_CODE_CTRLS (4)
+#define MGMT_FLASHROM_OPTYPE_CFG_IPSEC  (5)
+#define MGMT_FLASHROM_OPTYPE_CFG_INI    (6)
+#define MGMT_FLASHROM_OPTYPE_ROM_OFFSET_SPECIFIED (7)
+
+/*
+ * --- FLASHROM_TYPE ---
+ * Flash ROM manufacturers supported in the f/w
+ */
+#define INTEL                           (0)
+#define SPANSION                        (1)
+#define MICRON                          (2)
+
+/* --- DDR_CAS_TYPE --- */
+#define CAS_3                           (0)
+#define CAS_4                           (1)
+#define CAS_5                           (2)
+
+/* --- DDR_SIZE_TYPE --- */
+#define SIZE_256MB                      (0)
+#define SIZE_512MB                      (1)
+
+/* --- DDR_MODE_TYPE --- */
+#define DDR_NO_ECC                      (0)
+#define DDR_ECC                         (1)
+
+/* --- INTERFACE_10GB_TYPE --- */
+#define CX4_TYPE                        (0)
+#define XFP_TYPE                        (1)
+
+/* --- BE_CHIP_MAX_MTU --- */
+#define CHIP_MAX_MTU                    (9000)
+
+/* --- XAUI_STATE_ENUM --- */
+#define XAUI_STATE_ENABLE               (0)	/* This MUST be the default
+						   value for all requests
+						   which set/change
+						   equalization parameter.  */
+#define XAUI_STATE_DISABLE              (255)	/* The XAUI for both ports
+						   may be disabled for EMI
+						   tests. There is no
+						   provision for turning off
+						   individual ports.
+						   */
+/* --- BE_ASIC_REVISION --- */
+#define BE_ASIC_REV_A0                  (1)
+#define BE_ASIC_REV_A1                  (2)
+
+#endif /* __fwcmd_common_amap_h__ */
diff --git a/drivers/staging/benet/fwcmd_common_bmap.h b/drivers/staging/benet/fwcmd_common_bmap.h
new file mode 100644
index 00000000000..a007cf27650
--- /dev/null
+++ b/drivers/staging/benet/fwcmd_common_bmap.h
@@ -0,0 +1,717 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __fwcmd_common_bmap_h__
+#define __fwcmd_common_bmap_h__
+#include "fwcmd_types_bmap.h"
+#include "fwcmd_hdr_bmap.h"
+
+#if defined(__BIG_ENDIAN)
+   /* Physical Address. */
+struct PHYS_ADDR {
+	union {
+		struct {
+			u32 lo;	/* DWORD 0 */
+			u32 hi;	/* DWORD 1 */
+		} __packed;	/* unnamed struct */
+		u32 dw[2];	/* dword union */
+	};			/* unnamed union */
+} __packed ;
+
+
+#else
+   /* Physical Address. */
+struct PHYS_ADDR {
+	union {
+		struct {
+			u32 lo;	/* DWORD 0 */
+			u32 hi;	/* DWORD 1 */
+		} __packed;	/* unnamed struct */
+		u32 dw[2];	/* dword union */
+	};			/* unnamed union */
+} __packed ;
+
+struct BE_LINK_STATUS {
+	u8 mac0_duplex;
+	u8 mac0_speed;
+	u8 mac1_duplex;
+	u8 mac1_speed;
+	u8 mgmt_mac_duplex;
+	u8 mgmt_mac_speed;
+	u8 active_port;
+	u8 rsvd0;
+	u8 mac0_fault;
+	u8 mac1_fault;
+	u16 rsvd1;
+} __packed;
+#endif
+
+struct FWCMD_COMMON_ANON_170_REQUEST {
+	u32 rsvd0;
+} __packed;
+
+union LINK_STATUS_QUERY_PARAMS {
+	struct BE_LINK_STATUS response;
+	struct FWCMD_COMMON_ANON_170_REQUEST request;
+} __packed;
+
+/*
+ *  Queries the the link status for all ports.  The valid values below
+ *  DO NOT indicate that  a particular duplex or speed is supported by
+ *  BladeEngine. These enumerations simply  list all possible duplexes
+ *  and speeds for any port. Consult BladeEngine product  documentation
+ *  for the supported parameters.
+ */
+struct FWCMD_COMMON_NTWK_LINK_STATUS_QUERY {
+	union FWCMD_HEADER header;
+	union LINK_STATUS_QUERY_PARAMS params;
+} __packed;
+
+struct FWCMD_COMMON_ANON_171_REQUEST {
+	u8 type;
+	u8 port;
+	u8 mac1;
+	u8 permanent;
+} __packed;
+
+struct FWCMD_COMMON_ANON_172_RESPONSE {
+	struct MAC_ADDRESS_FORMAT mac;
+} __packed;
+
+union NTWK_MAC_QUERY_PARAMS {
+	struct FWCMD_COMMON_ANON_171_REQUEST request;
+	struct FWCMD_COMMON_ANON_172_RESPONSE response;
+} __packed;
+
+/* Queries one MAC address.  */
+struct FWCMD_COMMON_NTWK_MAC_QUERY {
+	union FWCMD_HEADER header;
+	union NTWK_MAC_QUERY_PARAMS params;
+} __packed;
+
+struct MAC_SET_PARAMS_IN {
+	u8 type;
+	u8 port;
+	u8 mac1;
+	u8 invalidate;
+	struct MAC_ADDRESS_FORMAT mac;
+} __packed;
+
+struct MAC_SET_PARAMS_OUT {
+	u32 rsvd0;
+} __packed;
+
+union MAC_SET_PARAMS {
+	struct MAC_SET_PARAMS_IN request;
+	struct MAC_SET_PARAMS_OUT response;
+} __packed;
+
+/* Sets a MAC address.  */
+struct FWCMD_COMMON_NTWK_MAC_SET {
+	union FWCMD_HEADER header;
+	union MAC_SET_PARAMS params;
+} __packed;
+
+/* MAC address list. */
+struct NTWK_MULTICAST_MAC_LIST {
+	u8 byte[6];
+} __packed;
+
+struct FWCMD_COMMON_NTWK_MULTICAST_SET_REQUEST_PAYLOAD {
+	u16 num_mac;
+	u8 promiscuous;
+	u8 rsvd0;
+	struct NTWK_MULTICAST_MAC_LIST mac[32];
+} __packed;
+
+struct FWCMD_COMMON_ANON_174_RESPONSE {
+	u32 rsvd0;
+} __packed;
+
+union FWCMD_COMMON_ANON_173_PARAMS {
+	struct FWCMD_COMMON_NTWK_MULTICAST_SET_REQUEST_PAYLOAD request;
+	struct FWCMD_COMMON_ANON_174_RESPONSE response;
+} __packed;
+
+/*
+ *  Sets multicast address hash. The MPU will merge the MAC address lists
+ *  from all clients,  including the networking and storage functions.
+ *  This command may fail if the final merged  list of MAC addresses exceeds
+ *  32 entries.
+ */
+struct FWCMD_COMMON_NTWK_MULTICAST_SET {
+	union FWCMD_HEADER header;
+	union FWCMD_COMMON_ANON_173_PARAMS params;
+} __packed;
+
+struct FWCMD_COMMON_NTWK_VLAN_CONFIG_REQUEST_PAYLOAD {
+	u16 num_vlan;
+	u8 promiscuous;
+	u8 rsvd0;
+	u16 vlan_tag[32];
+} __packed;
+
+struct FWCMD_COMMON_ANON_176_RESPONSE {
+	u32 rsvd0;
+} __packed;
+
+union FWCMD_COMMON_ANON_175_PARAMS {
+	struct FWCMD_COMMON_NTWK_VLAN_CONFIG_REQUEST_PAYLOAD request;
+	struct FWCMD_COMMON_ANON_176_RESPONSE response;
+} __packed;
+
+/*
+ *  Sets VLAN tag filter. The MPU will merge the VLAN tag list from all
+ *  clients, including  the networking and storage functions. This command
+ *  may fail if the final vlan_tag array  (from all functions) is longer
+ *  than 32 entries.
+ */
+struct FWCMD_COMMON_NTWK_VLAN_CONFIG {
+	union FWCMD_HEADER header;
+	union FWCMD_COMMON_ANON_175_PARAMS params;
+} __packed;
+
+struct RING_DESTROY_REQUEST {
+	u16 ring_type;
+	u16 id;
+	u8 bypass_flush;
+	u8 rsvd0;
+	u16 rsvd1;
+} __packed;
+
+struct FWCMD_COMMON_ANON_190_RESPONSE {
+	u32 rsvd0;
+} __packed;
+
+union FWCMD_COMMON_ANON_189_PARAMS {
+	struct RING_DESTROY_REQUEST request;
+	struct FWCMD_COMMON_ANON_190_RESPONSE response;
+} __packed;
+/*
+ *  Command for destroying any ring. The connection(s) using the ring should
+ *  be quiesced  before destroying the ring.
+ */
+struct FWCMD_COMMON_RING_DESTROY {
+	union FWCMD_HEADER header;
+	union FWCMD_COMMON_ANON_189_PARAMS params;
+} __packed;
+
+struct FWCMD_COMMON_ANON_192_REQUEST {
+	u16 num_pages;
+	u16 rsvd0;
+	struct CQ_CONTEXT_AMAP context;
+	struct PHYS_ADDR pages[4];
+} __packed ;
+
+struct FWCMD_COMMON_ANON_193_RESPONSE {
+	u16 cq_id;
+} __packed ;
+
+union FWCMD_COMMON_ANON_191_PARAMS {
+	struct FWCMD_COMMON_ANON_192_REQUEST request;
+	struct FWCMD_COMMON_ANON_193_RESPONSE response;
+} __packed ;
+
+/*
+ *  Command for creating a completion queue. A Completion Queue must span
+ *  at least 1 page and  at most 4 pages. Each completion queue entry
+ *  is 16 bytes regardless of CQ entry format.  Thus the ring must be
+ *  at least 256 entries deep (corresponding to 1 page) and can be at
+ *   most 1024 entries deep (corresponding to 4 pages). The number of
+ *  pages posted must  contain the CQ ring size as encoded in the context.
+ *
+ */
+struct FWCMD_COMMON_CQ_CREATE {
+	union FWCMD_HEADER header;
+	union FWCMD_COMMON_ANON_191_PARAMS params;
+} __packed ;
+
+struct FWCMD_COMMON_ANON_198_REQUEST {
+	u16 num_pages;
+	u16 rsvd0;
+	struct EQ_CONTEXT_AMAP context;
+	struct PHYS_ADDR pages[8];
+} __packed ;
+
+struct FWCMD_COMMON_ANON_199_RESPONSE {
+	u16 eq_id;
+} __packed ;
+
+union FWCMD_COMMON_ANON_197_PARAMS {
+	struct FWCMD_COMMON_ANON_198_REQUEST request;
+	struct FWCMD_COMMON_ANON_199_RESPONSE response;
+} __packed ;
+
+/*
+ *  Command for creating a event queue. An Event Queue must span at least
+ *  1 page and at most  8 pages. The number of pages posted must contain
+ *  the EQ ring. The ring is defined by  the size of the EQ entries (encoded
+ *  in the context) and the number of EQ entries (also  encoded in the
+ *  context).
+ */
+struct FWCMD_COMMON_EQ_CREATE {
+	union  FWCMD_HEADER header;
+	union FWCMD_COMMON_ANON_197_PARAMS params;
+} __packed ;
+
+struct FWCMD_COMMON_ANON_201_REQUEST {
+	u16 cq_id;
+	u16 bcmc_cq_id;
+	u16 num_pages;
+	u16 rsvd0;
+	struct PHYS_ADDR pages[2];
+} __packed;
+
+struct FWCMD_COMMON_ANON_202_RESPONSE {
+	u16 id;
+} __packed;
+
+union FWCMD_COMMON_ANON_200_PARAMS {
+	struct FWCMD_COMMON_ANON_201_REQUEST request;
+	struct FWCMD_COMMON_ANON_202_RESPONSE response;
+} __packed;
+
+/*
+ *  Command for creating Ethernet receive ring.  An ERX ring contains ETH_RX_D
+ *  entries (8  bytes each). An ERX ring must be 1024 entries deep
+ *  (corresponding to 2 pages).
+ */
+struct FWCMD_COMMON_ETH_RX_CREATE {
+	union FWCMD_HEADER header;
+	union FWCMD_COMMON_ANON_200_PARAMS params;
+} __packed;
+
+struct FWCMD_COMMON_ANON_204_REQUEST {
+	u16 num_pages;
+	u8 ulp_num;
+	u8 type;
+	struct ETX_CONTEXT_AMAP context;
+	struct PHYS_ADDR pages[8];
+} __packed ;
+
+struct FWCMD_COMMON_ANON_205_RESPONSE {
+	u16 cid;
+	u8 ulp_num;
+	u8 rsvd0;
+} __packed ;
+
+union FWCMD_COMMON_ANON_203_PARAMS {
+	struct FWCMD_COMMON_ANON_204_REQUEST request;
+	struct FWCMD_COMMON_ANON_205_RESPONSE response;
+} __packed ;
+
+/*
+ *  Command for creating an Ethernet transmit ring.  An ETX ring contains
+ *  ETH_WRB entries (16  bytes each). An ETX ring must be at least 256
+ *  entries deep (corresponding to 1 page)  and at most 2k entries deep
+ *  (corresponding to 8 pages).
+ */
+struct FWCMD_COMMON_ETH_TX_CREATE {
+	union FWCMD_HEADER header;
+	union FWCMD_COMMON_ANON_203_PARAMS params;
+} __packed ;
+
+struct FWCMD_COMMON_ANON_222_REQUEST {
+	u16 num_pages;
+	u16 rsvd0;
+	struct MCC_RING_CONTEXT_AMAP context;
+	struct PHYS_ADDR pages[8];
+} __packed ;
+
+struct FWCMD_COMMON_ANON_223_RESPONSE {
+	u16 id;
+} __packed ;
+
+union FWCMD_COMMON_ANON_221_PARAMS {
+	struct FWCMD_COMMON_ANON_222_REQUEST request;
+	struct FWCMD_COMMON_ANON_223_RESPONSE response;
+} __packed ;
+
+/*
+ *  Command for creating the MCC ring. An MCC ring must be at least 16
+ *  entries deep  (corresponding to 1 page) and at most 128 entries deep
+ *  (corresponding to 8 pages).
+ */
+struct FWCMD_COMMON_MCC_CREATE {
+	union FWCMD_HEADER header;
+	union FWCMD_COMMON_ANON_221_PARAMS params;
+} __packed ;
+
+struct GET_QOS_IN {
+	u32 qos_params_rsvd;
+} __packed;
+
+struct GET_QOS_OUT {
+	u32 max_bits_per_second_NIC;
+	u32 max_packets_per_second_NIC;
+	u32 max_ios_per_second_iSCSI;
+	u32 max_bytes_per_second_iSCSI;
+	u16 domain_VLAN_tag;
+	u16 fabric_domain_ID;
+	u32 qos_params_oem[4];
+} __packed;
+
+union GET_QOS_PARAMS {
+	struct GET_QOS_IN request;
+	struct GET_QOS_OUT response;
+} __packed;
+
+/* QOS/Bandwidth settings per domain. Applicable only in VMs.  */
+struct FWCMD_COMMON_GET_QOS {
+	union FWCMD_HEADER header;
+	union GET_QOS_PARAMS params;
+} __packed;
+
+struct SET_QOS_IN {
+	u32 valid_flags;
+	u32 max_bits_per_second_NIC;
+	u32 max_packets_per_second_NIC;
+	u32 max_ios_per_second_iSCSI;
+	u32 max_bytes_per_second_iSCSI;
+	u16 domain_VLAN_tag;
+	u16 fabric_domain_ID;
+	u32 qos_params_oem[4];
+} __packed;
+
+struct SET_QOS_OUT {
+	u32 qos_params_rsvd;
+} __packed;
+
+union SET_QOS_PARAMS {
+	struct SET_QOS_IN request;
+	struct SET_QOS_OUT response;
+} __packed;
+
+/* QOS/Bandwidth settings per domain. Applicable only in VMs.  */
+struct FWCMD_COMMON_SET_QOS {
+	union FWCMD_HEADER header;
+	union SET_QOS_PARAMS params;
+} __packed;
+
+struct SET_FRAME_SIZE_IN {
+	u32 max_tx_frame_size;
+	u32 max_rx_frame_size;
+} __packed;
+
+struct SET_FRAME_SIZE_OUT {
+	u32 chip_max_tx_frame_size;
+	u32 chip_max_rx_frame_size;
+} __packed;
+
+union SET_FRAME_SIZE_PARAMS {
+	struct SET_FRAME_SIZE_IN request;
+	struct SET_FRAME_SIZE_OUT response;
+} __packed;
+
+/* Set frame size command. Only host domain may issue this command.  */
+struct FWCMD_COMMON_SET_FRAME_SIZE {
+	union FWCMD_HEADER header;
+	union SET_FRAME_SIZE_PARAMS params;
+} __packed;
+
+struct FORCE_FAILOVER_IN {
+	u32 move_to_port;
+	u32 failover_config;
+} __packed;
+
+struct FWCMD_COMMON_ANON_231_RESPONSE {
+	u32 rsvd0;
+} __packed;
+
+union FWCMD_COMMON_ANON_230_PARAMS {
+	struct FORCE_FAILOVER_IN request;
+	struct FWCMD_COMMON_ANON_231_RESPONSE response;
+} __packed;
+
+/*
+ *  Use this command to control failover in BladeEngine. It may be used
+ *  to failback to a  restored port or to forcibly move traffic from
+ *  one port to another. It may also be used  to enable or disable the
+ *  automatic failover feature. This command can only be issued by  domain
+ *  0.
+ */
+struct FWCMD_COMMON_FORCE_FAILOVER {
+	union FWCMD_HEADER header;
+	union FWCMD_COMMON_ANON_230_PARAMS params;
+} __packed;
+
+struct FWCMD_COMMON_ANON_240_REQUEST {
+	u64 context;
+} __packed;
+
+struct FWCMD_COMMON_ANON_241_RESPONSE {
+	u64 context;
+} __packed;
+
+union FWCMD_COMMON_ANON_239_PARAMS {
+	struct FWCMD_COMMON_ANON_240_REQUEST request;
+	struct FWCMD_COMMON_ANON_241_RESPONSE response;
+} __packed;
+
+/*
+ *  This command can be used by clients as a no-operation request. Typical
+ *  uses for drivers  are as a heartbeat mechanism, or deferred processing
+ *  catalyst. The ARM will always  complete this command with a good completion.
+ *  The 64-bit parameter is not touched by the  ARM processor.
+ */
+struct FWCMD_COMMON_NOP {
+	union FWCMD_HEADER header;
+	union FWCMD_COMMON_ANON_239_PARAMS params;
+} __packed;
+
+struct NTWK_RX_FILTER_SETTINGS {
+	u8 promiscuous;
+	u8 ip_cksum;
+	u8 tcp_cksum;
+	u8 udp_cksum;
+	u8 pass_err;
+	u8 pass_ckerr;
+	u8 strip_crc;
+	u8 mcast_en;
+	u8 bcast_en;
+	u8 mcast_promiscuous_en;
+	u8 unicast_en;
+	u8 vlan_promiscuous;
+} __packed;
+
+union FWCMD_COMMON_ANON_242_PARAMS {
+	struct NTWK_RX_FILTER_SETTINGS request;
+	struct NTWK_RX_FILTER_SETTINGS response;
+} __packed;
+
+/*
+ *  This command is used to modify the ethernet receive filter configuration.
+ *  Only domain 0  network function drivers may issue this command. The
+ *  applied configuration is returned in  the response payload. Note:
+ *  Some receive packet filter settings are global on  BladeEngine and
+ *  can affect both the storage and network function clients that the
+ *   BladeEngine hardware and firmware serve. Additionaly, depending
+ *  on the revision of  BladeEngine, some ethernet receive filter settings
+ *  are dependent on others. If a  dependency exists between settings
+ *  for the BladeEngine revision, and the command request  settings do
+ *  not meet the dependency requirement, the invalid settings will not
+ *  be  applied despite the comand succeeding. For example: a driver may
+ *  request to enable  broadcast packets, but not enable multicast packets.
+ *  On early revisions of BladeEngine,  there may be no distinction between
+ *  broadcast and multicast filters, so broadcast could  not be enabled
+ *  without enabling multicast. In this scenario, the comand would still
+ *   succeed, but the response payload would indicate the previously
+ *  configured broadcast  and multicast setting.
+ */
+struct FWCMD_COMMON_NTWK_RX_FILTER {
+	union FWCMD_HEADER header;
+	union FWCMD_COMMON_ANON_242_PARAMS params;
+} __packed;
+
+
+struct FWCMD_COMMON_ANON_244_REQUEST {
+	u32 rsvd0;
+} __packed;
+
+struct FWCMD_COMMON_GET_FW_VERSION_RESPONSE_PAYLOAD {
+	u8 firmware_version_string[32];
+	u8 fw_on_flash_version_string[32];
+} __packed;
+
+union FWCMD_COMMON_ANON_243_PARAMS {
+	struct FWCMD_COMMON_ANON_244_REQUEST request;
+	struct FWCMD_COMMON_GET_FW_VERSION_RESPONSE_PAYLOAD response;
+} __packed;
+
+/* This comand retrieves the firmware version.  */
+struct FWCMD_COMMON_GET_FW_VERSION {
+	union FWCMD_HEADER header;
+	union FWCMD_COMMON_ANON_243_PARAMS params;
+} __packed;
+
+struct FWCMD_COMMON_ANON_246_REQUEST {
+	u16 tx_flow_control;
+	u16 rx_flow_control;
+} __packed;
+
+struct FWCMD_COMMON_ANON_247_RESPONSE {
+	u32 rsvd0;
+} __packed;
+
+union FWCMD_COMMON_ANON_245_PARAMS {
+	struct FWCMD_COMMON_ANON_246_REQUEST request;
+	struct FWCMD_COMMON_ANON_247_RESPONSE response;
+} __packed;
+
+/*
+ *  This comand is used to program BladeEngine flow control behavior.
+ *  Only the host  networking driver is allowed to use this comand.
+ */
+struct FWCMD_COMMON_SET_FLOW_CONTROL {
+	union FWCMD_HEADER header;
+	union FWCMD_COMMON_ANON_245_PARAMS params;
+} __packed;
+
+struct FWCMD_COMMON_ANON_249_REQUEST {
+	u32 rsvd0;
+} __packed;
+
+struct FWCMD_COMMON_ANON_250_RESPONSE {
+	u16 tx_flow_control;
+	u16 rx_flow_control;
+} __packed;
+
+union FWCMD_COMMON_ANON_248_PARAMS {
+	struct FWCMD_COMMON_ANON_249_REQUEST request;
+	struct FWCMD_COMMON_ANON_250_RESPONSE response;
+} __packed;
+
+/* This comand is used to read BladeEngine flow control settings.  */
+struct FWCMD_COMMON_GET_FLOW_CONTROL {
+	union FWCMD_HEADER header;
+	union FWCMD_COMMON_ANON_248_PARAMS params;
+} __packed;
+
+struct EQ_DELAY_PARAMS {
+	u32 eq_id;
+	u32 delay_in_microseconds;
+} __packed;
+
+struct FWCMD_COMMON_ANON_257_REQUEST {
+	u32 num_eq;
+	u32 rsvd0;
+	struct EQ_DELAY_PARAMS delay[16];
+} __packed;
+
+struct FWCMD_COMMON_ANON_258_RESPONSE {
+	u32 delay_resolution_in_microseconds;
+	u32 delay_max_in_microseconds;
+} __packed;
+
+union MODIFY_EQ_DELAY_PARAMS {
+	struct FWCMD_COMMON_ANON_257_REQUEST request;
+	struct FWCMD_COMMON_ANON_258_RESPONSE response;
+} __packed;
+
+/* This comand changes the EQ delay for a given set of EQs.  */
+struct FWCMD_COMMON_MODIFY_EQ_DELAY {
+	union FWCMD_HEADER header;
+	union MODIFY_EQ_DELAY_PARAMS params;
+} __packed;
+
+struct FWCMD_COMMON_ANON_260_REQUEST {
+	u32 rsvd0;
+} __packed;
+
+struct BE_FIRMWARE_CONFIG {
+	u16 be_config_number;
+	u16 asic_revision;
+	u32 nic_ulp_mask;
+	u32 tulp_mask;
+	u32 iscsi_ulp_mask;
+	u32 rdma_ulp_mask;
+	u32 rsvd0[4];
+	u32 eth_tx_id_start;
+	u32 eth_tx_id_count;
+	u32 eth_rx_id_start;
+	u32 eth_rx_id_count;
+	u32 tpm_wrbq_id_start;
+	u32 tpm_wrbq_id_count;
+	u32 tpm_defq_id_start;
+	u32 tpm_defq_id_count;
+	u32 iscsi_wrbq_id_start;
+	u32 iscsi_wrbq_id_count;
+	u32 iscsi_defq_id_start;
+	u32 iscsi_defq_id_count;
+	u32 rdma_qp_id_start;
+	u32 rdma_qp_id_count;
+	u32 rsvd1[8];
+} __packed;
+
+union FWCMD_COMMON_ANON_259_PARAMS {
+	struct FWCMD_COMMON_ANON_260_REQUEST request;
+	struct BE_FIRMWARE_CONFIG response;
+} __packed;
+
+/*
+ *  This comand queries the current firmware configuration parameters.
+ *   The static  configuration type is defined by be_config_number. This
+ *  differentiates different  BladeEngine builds, such as iSCSI Initiator
+ *  versus iSCSI Target.  For a given static  configuration, the Upper
+ *  Layer Protocol (ULP) processors may be reconfigured to support  different
+ *  protocols. Each ULP processor supports one or more protocols. The
+ *  masks  indicate which processors are configured for each protocol.
+ *   For a given static  configuration, the number of TCP connections
+ *  supported for each protocol may vary. The  *_id_start and *_id_count
+ *  variables define a linear range of IDs that are available for  each
+ *  supported protocol. The *_id_count may be used by the driver to allocate
+ *  the  appropriate number of connection resources. The *_id_start may
+ *  be used to map the  arbitrary range of IDs to a zero-based range
+ *  of indices.
+ */
+struct FWCMD_COMMON_FIRMWARE_CONFIG {
+	union FWCMD_HEADER header;
+	union FWCMD_COMMON_ANON_259_PARAMS params;
+} __packed;
+
+struct FWCMD_COMMON_PORT_EQUALIZATION_PARAMS {
+	u32 emph_lev_sel_port0;
+	u32 emph_lev_sel_port1;
+	u8 xaui_vo_sel;
+	u8 xaui_state;
+	u16 rsvd0;
+	u32 xaui_eq_vector;
+} __packed;
+
+struct FWCMD_COMMON_ANON_262_REQUEST {
+	u32 rsvd0;
+} __packed;
+
+union FWCMD_COMMON_ANON_261_PARAMS {
+	struct FWCMD_COMMON_ANON_262_REQUEST request;
+	struct FWCMD_COMMON_PORT_EQUALIZATION_PARAMS response;
+} __packed;
+
+/*
+ *  This comand can be used to read XAUI equalization parameters. The
+ *  ARM firmware applies  default equalization parameters during initialization.
+ *  These parameters may be  customer-specific when derived from the
+ *  SEEPROM. See SEEPROM_DATA for equalization  specific fields.
+ */
+struct FWCMD_COMMON_GET_PORT_EQUALIZATION {
+	union FWCMD_HEADER header;
+	union FWCMD_COMMON_ANON_261_PARAMS params;
+} __packed;
+
+struct FWCMD_COMMON_ANON_264_RESPONSE {
+	u32 rsvd0;
+} __packed;
+
+union FWCMD_COMMON_ANON_263_PARAMS {
+	struct FWCMD_COMMON_PORT_EQUALIZATION_PARAMS request;
+	struct FWCMD_COMMON_ANON_264_RESPONSE response;
+} __packed;
+
+/*
+ *  This comand can be used to set XAUI equalization parameters. The ARM
+ *  firmware applies  default equalization parameters during initialization.
+ *  These parameters may be  customer-specific when derived from the
+ *  SEEPROM. See SEEPROM_DATA for equalization  specific fields.
+ */
+struct FWCMD_COMMON_SET_PORT_EQUALIZATION {
+	union FWCMD_HEADER header;
+	union FWCMD_COMMON_ANON_263_PARAMS params;
+} __packed;
+
+#endif /* __fwcmd_common_bmap_h__ */
diff --git a/drivers/staging/benet/fwcmd_eth_bmap.h b/drivers/staging/benet/fwcmd_eth_bmap.h
new file mode 100644
index 00000000000..234b179eace
--- /dev/null
+++ b/drivers/staging/benet/fwcmd_eth_bmap.h
@@ -0,0 +1,280 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __fwcmd_eth_bmap_h__
+#define __fwcmd_eth_bmap_h__
+#include "fwcmd_hdr_bmap.h"
+#include "fwcmd_types_bmap.h"
+
+struct MIB_ETH_STATISTICS_PARAMS_IN {
+	u32 rsvd0;
+} __packed;
+
+struct BE_RXF_STATS {
+	u32 p0recvdtotalbytesLSD;	/* DWORD 0 */
+	u32 p0recvdtotalbytesMSD;	/* DWORD 1 */
+	u32 p0recvdtotalframes;	/* DWORD 2 */
+	u32 p0recvdunicastframes;	/* DWORD 3 */
+	u32 p0recvdmulticastframes;	/* DWORD 4 */
+	u32 p0recvdbroadcastframes;	/* DWORD 5 */
+	u32 p0crcerrors;	/* DWORD 6 */
+	u32 p0alignmentsymerrs;	/* DWORD 7 */
+	u32 p0pauseframesrecvd;	/* DWORD 8 */
+	u32 p0controlframesrecvd;	/* DWORD 9 */
+	u32 p0inrangelenerrors;	/* DWORD 10 */
+	u32 p0outrangeerrors;	/* DWORD 11 */
+	u32 p0frametoolongerrors;	/* DWORD 12 */
+	u32 p0droppedaddressmatch;	/* DWORD 13 */
+	u32 p0droppedvlanmismatch;	/* DWORD 14 */
+	u32 p0ipdroppedtoosmall;	/* DWORD 15 */
+	u32 p0ipdroppedtooshort;	/* DWORD 16 */
+	u32 p0ipdroppedhdrtoosmall;	/* DWORD 17 */
+	u32 p0tcpdroppedlen;	/* DWORD 18 */
+	u32 p0droppedrunt;	/* DWORD 19 */
+	u32 p0recvd64;		/* DWORD 20 */
+	u32 p0recvd65_127;	/* DWORD 21 */
+	u32 p0recvd128_256;	/* DWORD 22 */
+	u32 p0recvd256_511;	/* DWORD 23 */
+	u32 p0recvd512_1023;	/* DWORD 24 */
+	u32 p0recvd1518_1522;	/* DWORD 25 */
+	u32 p0recvd1522_2047;	/* DWORD 26 */
+	u32 p0recvd2048_4095;	/* DWORD 27 */
+	u32 p0recvd4096_8191;	/* DWORD 28 */
+	u32 p0recvd8192_9216;	/* DWORD 29 */
+	u32 p0rcvdipcksmerrs;	/* DWORD 30 */
+	u32 p0recvdtcpcksmerrs;	/* DWORD 31 */
+	u32 p0recvdudpcksmerrs;	/* DWORD 32 */
+	u32 p0recvdnonrsspackets;	/* DWORD 33 */
+	u32 p0recvdippackets;	/* DWORD 34 */
+	u32 p0recvdchute1packets;	/* DWORD 35 */
+	u32 p0recvdchute2packets;	/* DWORD 36 */
+	u32 p0recvdchute3packets;	/* DWORD 37 */
+	u32 p0recvdipsecpackets;	/* DWORD 38 */
+	u32 p0recvdmanagementpackets;	/* DWORD 39 */
+	u32 p0xmitbyteslsd;	/* DWORD 40 */
+	u32 p0xmitbytesmsd;	/* DWORD 41 */
+	u32 p0xmitunicastframes;	/* DWORD 42 */
+	u32 p0xmitmulticastframes;	/* DWORD 43 */
+	u32 p0xmitbroadcastframes;	/* DWORD 44 */
+	u32 p0xmitpauseframes;	/* DWORD 45 */
+	u32 p0xmitcontrolframes;	/* DWORD 46 */
+	u32 p0xmit64;		/* DWORD 47 */
+	u32 p0xmit65_127;	/* DWORD 48 */
+	u32 p0xmit128_256;	/* DWORD 49 */
+	u32 p0xmit256_511;	/* DWORD 50 */
+	u32 p0xmit512_1023;	/* DWORD 51 */
+	u32 p0xmit1518_1522;	/* DWORD 52 */
+	u32 p0xmit1522_2047;	/* DWORD 53 */
+	u32 p0xmit2048_4095;	/* DWORD 54 */
+	u32 p0xmit4096_8191;	/* DWORD 55 */
+	u32 p0xmit8192_9216;	/* DWORD 56 */
+	u32 p0rxfifooverflowdropped;	/* DWORD 57 */
+	u32 p0ipseclookupfaileddropped;	/* DWORD 58 */
+	u32 p1recvdtotalbytesLSD;	/* DWORD 59 */
+	u32 p1recvdtotalbytesMSD;	/* DWORD 60 */
+	u32 p1recvdtotalframes;	/* DWORD 61 */
+	u32 p1recvdunicastframes;	/* DWORD 62 */
+	u32 p1recvdmulticastframes;	/* DWORD 63 */
+	u32 p1recvdbroadcastframes;	/* DWORD 64 */
+	u32 p1crcerrors;	/* DWORD 65 */
+	u32 p1alignmentsymerrs;	/* DWORD 66 */
+	u32 p1pauseframesrecvd;	/* DWORD 67 */
+	u32 p1controlframesrecvd;	/* DWORD 68 */
+	u32 p1inrangelenerrors;	/* DWORD 69 */
+	u32 p1outrangeerrors;	/* DWORD 70 */
+	u32 p1frametoolongerrors;	/* DWORD 71 */
+	u32 p1droppedaddressmatch;	/* DWORD 72 */
+	u32 p1droppedvlanmismatch;	/* DWORD 73 */
+	u32 p1ipdroppedtoosmall;	/* DWORD 74 */
+	u32 p1ipdroppedtooshort;	/* DWORD 75 */
+	u32 p1ipdroppedhdrtoosmall;	/* DWORD 76 */
+	u32 p1tcpdroppedlen;	/* DWORD 77 */
+	u32 p1droppedrunt;	/* DWORD 78 */
+	u32 p1recvd64;		/* DWORD 79 */
+	u32 p1recvd65_127;	/* DWORD 80 */
+	u32 p1recvd128_256;	/* DWORD 81 */
+	u32 p1recvd256_511;	/* DWORD 82 */
+	u32 p1recvd512_1023;	/* DWORD 83 */
+	u32 p1recvd1518_1522;	/* DWORD 84 */
+	u32 p1recvd1522_2047;	/* DWORD 85 */
+	u32 p1recvd2048_4095;	/* DWORD 86 */
+	u32 p1recvd4096_8191;	/* DWORD 87 */
+	u32 p1recvd8192_9216;	/* DWORD 88 */
+	u32 p1rcvdipcksmerrs;	/* DWORD 89 */
+	u32 p1recvdtcpcksmerrs;	/* DWORD 90 */
+	u32 p1recvdudpcksmerrs;	/* DWORD 91 */
+	u32 p1recvdnonrsspackets;	/* DWORD 92 */
+	u32 p1recvdippackets;	/* DWORD 93 */
+	u32 p1recvdchute1packets;	/* DWORD 94 */
+	u32 p1recvdchute2packets;	/* DWORD 95 */
+	u32 p1recvdchute3packets;	/* DWORD 96 */
+	u32 p1recvdipsecpackets;	/* DWORD 97 */
+	u32 p1recvdmanagementpackets;	/* DWORD 98 */
+	u32 p1xmitbyteslsd;	/* DWORD 99 */
+	u32 p1xmitbytesmsd;	/* DWORD 100 */
+	u32 p1xmitunicastframes;	/* DWORD 101 */
+	u32 p1xmitmulticastframes;	/* DWORD 102 */
+	u32 p1xmitbroadcastframes;	/* DWORD 103 */
+	u32 p1xmitpauseframes;	/* DWORD 104 */
+	u32 p1xmitcontrolframes;	/* DWORD 105 */
+	u32 p1xmit64;		/* DWORD 106 */
+	u32 p1xmit65_127;	/* DWORD 107 */
+	u32 p1xmit128_256;	/* DWORD 108 */
+	u32 p1xmit256_511;	/* DWORD 109 */
+	u32 p1xmit512_1023;	/* DWORD 110 */
+	u32 p1xmit1518_1522;	/* DWORD 111 */
+	u32 p1xmit1522_2047;	/* DWORD 112 */
+	u32 p1xmit2048_4095;	/* DWORD 113 */
+	u32 p1xmit4096_8191;	/* DWORD 114 */
+	u32 p1xmit8192_9216;	/* DWORD 115 */
+	u32 p1rxfifooverflowdropped;	/* DWORD 116 */
+	u32 p1ipseclookupfaileddropped;	/* DWORD 117 */
+	u32 pxdroppednopbuf;	/* DWORD 118 */
+	u32 pxdroppednotxpb;	/* DWORD 119 */
+	u32 pxdroppednoipsecbuf;	/* DWORD 120 */
+	u32 pxdroppednoerxdescr;	/* DWORD 121 */
+	u32 pxdroppednotpredescr;	/* DWORD 122 */
+	u32 pxrecvdmanagementportpackets;	/* DWORD 123 */
+	u32 pxrecvdmanagementportbytes;	/* DWORD 124 */
+	u32 pxrecvdmanagementportpauseframes;	/* DWORD 125 */
+	u32 pxrecvdmanagementporterrors;	/* DWORD 126 */
+	u32 pxxmitmanagementportpackets;	/* DWORD 127 */
+	u32 pxxmitmanagementportbytes;	/* DWORD 128 */
+	u32 pxxmitmanagementportpause;	/* DWORD 129 */
+	u32 pxxmitmanagementportrxfifooverflow;	/* DWORD 130 */
+	u32 pxrecvdipsecipcksmerrs;	/* DWORD 131 */
+	u32 pxrecvdtcpsecipcksmerrs;	/* DWORD 132 */
+	u32 pxrecvdudpsecipcksmerrs;	/* DWORD 133 */
+	u32 pxipsecrunt;	/* DWORD 134 */
+	u32 pxipsecaddressmismatchdropped;	/* DWORD 135 */
+	u32 pxipsecrxfifooverflowdropped;	/* DWORD 136 */
+	u32 pxipsecframestoolong;	/* DWORD 137 */
+	u32 pxipsectotalipframes;	/* DWORD 138 */
+	u32 pxipseciptoosmall;	/* DWORD 139 */
+	u32 pxipseciptooshort;	/* DWORD 140 */
+	u32 pxipseciphdrtoosmall;	/* DWORD 141 */
+	u32 pxipsectcphdrbad;	/* DWORD 142 */
+	u32 pxrecvdipsecchute1;	/* DWORD 143 */
+	u32 pxrecvdipsecchute2;	/* DWORD 144 */
+	u32 pxrecvdipsecchute3;	/* DWORD 145 */
+	u32 pxdropped7frags;	/* DWORD 146 */
+	u32 pxdroppedfrags;	/* DWORD 147 */
+	u32 pxdroppedinvalidfragring;	/* DWORD 148 */
+	u32 pxnumforwardedpackets;	/* DWORD 149 */
+} __packed;
+
+union MIB_ETH_STATISTICS_PARAMS {
+	struct MIB_ETH_STATISTICS_PARAMS_IN request;
+	struct BE_RXF_STATS response;
+} __packed;
+
+/*
+ *  Query ethernet statistics. All domains may issue this command. The
+ *  host domain drivers  may optionally reset internal statistic counters
+ *  with a query.
+ */
+struct FWCMD_ETH_GET_STATISTICS {
+	union FWCMD_HEADER header;
+	union MIB_ETH_STATISTICS_PARAMS params;
+} __packed;
+
+
+struct FWCMD_ETH_ANON_175_REQUEST {
+	u8 port0_promiscuous;
+	u8 port1_promiscuous;
+	u16 rsvd0;
+} __packed;
+
+struct FWCMD_ETH_ANON_176_RESPONSE {
+	u32 rsvd0;
+} __packed;
+
+union FWCMD_ETH_ANON_174_PARAMS {
+	struct FWCMD_ETH_ANON_175_REQUEST request;
+	struct FWCMD_ETH_ANON_176_RESPONSE response;
+} __packed;
+
+/* Enables/Disables promiscuous ethernet receive mode.  */
+struct FWCMD_ETH_PROMISCUOUS {
+	union FWCMD_HEADER header;
+	union FWCMD_ETH_ANON_174_PARAMS params;
+} __packed;
+
+struct FWCMD_ETH_ANON_178_REQUEST {
+	u32 new_fragsize_log2;
+} __packed;
+
+struct FWCMD_ETH_ANON_179_RESPONSE {
+	u32 actual_fragsize_log2;
+} __packed;
+
+union FWCMD_ETH_ANON_177_PARAMS {
+	struct FWCMD_ETH_ANON_178_REQUEST request;
+	struct FWCMD_ETH_ANON_179_RESPONSE response;
+} __packed;
+
+/*
+ *  Sets the Ethernet RX fragment size. Only host (domain 0) networking
+ *  drivers may issue  this command.  This call will fail for non-host
+ *  protection domains. In this situation the  MCC CQ status will indicate
+ *  a failure due to insufficient priviledges. The response  should be
+ *  ignored, and the driver should use the FWCMD_ETH_GET_FRAG_SIZE to
+ *  query the  existing ethernet receive fragment size. It must use this
+ *  fragment size for all  fragments in the ethernet receive ring.  If
+ *  the command succeeds, the driver must use the  frag size indicated
+ *  in the command response since the requested frag size may not be  applied
+ *  until the next reboot. When the requested fragsize matches the response
+ *   fragsize, this indicates the request was applied immediately.
+ */
+struct FWCMD_ETH_SET_RX_FRAG_SIZE {
+	union FWCMD_HEADER header;
+	union FWCMD_ETH_ANON_177_PARAMS params;
+} __packed;
+
+struct FWCMD_ETH_ANON_181_REQUEST {
+	u32 rsvd0;
+} __packed;
+
+struct FWCMD_ETH_ANON_182_RESPONSE {
+	u32 actual_fragsize_log2;
+} __packed;
+
+union FWCMD_ETH_ANON_180_PARAMS {
+	struct FWCMD_ETH_ANON_181_REQUEST request;
+	struct FWCMD_ETH_ANON_182_RESPONSE response;
+} __packed;
+
+/*
+ *  Queries the Ethernet RX fragment size. All domains may issue this
+ *  command.  The driver  should call this command to determine the minimum
+ *  required fragment size for the ethernet  RX ring buffers. Drivers
+ *  may choose to use a larger size for each fragment buffer, but  BladeEngine
+ *  will use up to the configured minimum required fragsize in each ethernet
+ *   receive fragment buffer. For example, if the ethernet receive fragment
+ *  size is  configured to 4kB, and a driver uses 8kB fragments, a 6kB
+ *  ethernet packet received by  BladeEngine will be split accross two
+ *  of the driver's receive framgents (4kB in one  fragment buffer, and
+ *  2kB in the subsequent fragment buffer).
+ */
+struct FWCMD_ETH_GET_RX_FRAG_SIZE {
+	union FWCMD_HEADER header;
+	union FWCMD_ETH_ANON_180_PARAMS params;
+} __packed;
+
+#endif /* __fwcmd_eth_bmap_h__ */
diff --git a/drivers/staging/benet/fwcmd_hdr_bmap.h b/drivers/staging/benet/fwcmd_hdr_bmap.h
new file mode 100644
index 00000000000..28b45328fe7
--- /dev/null
+++ b/drivers/staging/benet/fwcmd_hdr_bmap.h
@@ -0,0 +1,54 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __fwcmd_hdr_bmap_h__
+#define __fwcmd_hdr_bmap_h__
+
+struct FWCMD_REQUEST_HEADER {
+	u8 opcode;
+	u8 subsystem;
+	u8 port_number;
+	u8 domain;
+	u32 timeout;
+	u32 request_length;
+	u32 rsvd0;
+} __packed;
+
+struct FWCMD_RESPONSE_HEADER {
+	u8 opcode;
+	u8 subsystem;
+	u8 rsvd0;
+	u8 domain;
+	u8 status;
+	u8 additional_status;
+	u16 rsvd1;
+	u32 response_length;
+	u32 actual_response_length;
+} __packed;
+
+/*
+ *  The firmware/driver overwrites the input FWCMD_REQUEST_HEADER with
+ *  the output  FWCMD_RESPONSE_HEADER.
+ */
+union FWCMD_HEADER {
+	struct FWCMD_REQUEST_HEADER request;
+	struct FWCMD_RESPONSE_HEADER response;
+} __packed;
+
+#endif /* __fwcmd_hdr_bmap_h__ */
diff --git a/drivers/staging/benet/fwcmd_mcc.h b/drivers/staging/benet/fwcmd_mcc.h
new file mode 100644
index 00000000000..9eeca878c1f
--- /dev/null
+++ b/drivers/staging/benet/fwcmd_mcc.h
@@ -0,0 +1,94 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __fwcmd_mcc_amap_h__
+#define __fwcmd_mcc_amap_h__
+#include "fwcmd_opcodes.h"
+/*
+ * Where applicable, a WRB, may contain a list of Scatter-gather elements.
+ * Each element supports a 64 bit address and a 32bit length field.
+ */
+struct BE_MCC_SGE_AMAP {
+	u8 pa_lo[32];	/* DWORD 0 */
+	u8 pa_hi[32];	/* DWORD 1 */
+	u8 length[32];	/* DWORD 2 */
+} __packed;
+struct MCC_SGE_AMAP {
+	u32 dw[3];
+};
+/*
+ * The design of an MCC_SGE allows up to 19 elements to be embedded
+ * in a WRB, supporting 64KB data transfers (assuming a 4KB page size).
+ */
+struct BE_MCC_WRB_PAYLOAD_AMAP {
+	union {
+		struct BE_MCC_SGE_AMAP sgl[19];
+		u8 embedded[59][32];	/* DWORD 0 */
+	};
+} __packed;
+struct MCC_WRB_PAYLOAD_AMAP {
+	u32 dw[59];
+};
+
+/*
+ * This is the structure of the MCC Command WRB for commands
+ * sent to the Management Processing Unit (MPU). See section
+ * for usage in embedded and non-embedded modes.
+ */
+struct BE_MCC_WRB_AMAP {
+	u8 embedded;	/* DWORD 0 */
+	u8 rsvd0[2];	/* DWORD 0 */
+	u8 sge_count[5];	/* DWORD 0 */
+	u8 rsvd1[16];	/* DWORD 0 */
+	u8 special[8];	/* DWORD 0 */
+	u8 payload_length[32];	/* DWORD 1 */
+	u8 tag[2][32];	/* DWORD 2 */
+	u8 rsvd2[32];	/* DWORD 4 */
+	struct BE_MCC_WRB_PAYLOAD_AMAP payload;
+} __packed;
+struct MCC_WRB_AMAP {
+	u32 dw[64];
+};
+
+/*  This is the structure of the MCC Completion queue entry  */
+struct BE_MCC_CQ_ENTRY_AMAP {
+	u8 completion_status[16];	/* DWORD 0 */
+	u8 extended_status[16];	/* DWORD 0 */
+	u8 mcc_tag[2][32];	/* DWORD 1 */
+	u8 rsvd0[27];	/* DWORD 3 */
+	u8 consumed;	/* DWORD 3 */
+	u8 completed;	/* DWORD 3 */
+	u8 hpi_buffer_completion;	/* DWORD 3 */
+	u8 async_event;	/* DWORD 3 */
+	u8 valid;		/* DWORD 3 */
+} __packed;
+struct MCC_CQ_ENTRY_AMAP {
+	u32 dw[4];
+};
+
+/* Mailbox structures used by the MPU during bootstrap */
+struct BE_MCC_MAILBOX_AMAP {
+	struct BE_MCC_WRB_AMAP wrb;
+	struct BE_MCC_CQ_ENTRY_AMAP cq;
+} __packed;
+struct MCC_MAILBOX_AMAP {
+	u32 dw[68];
+};
+
+#endif /* __fwcmd_mcc_amap_h__ */
diff --git a/drivers/staging/benet/fwcmd_opcodes.h b/drivers/staging/benet/fwcmd_opcodes.h
new file mode 100644
index 00000000000..23d569386b4
--- /dev/null
+++ b/drivers/staging/benet/fwcmd_opcodes.h
@@ -0,0 +1,244 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __fwcmd_opcodes_amap_h__
+#define __fwcmd_opcodes_amap_h__
+
+/*
+ * --- FWCMD_SUBSYSTEMS ---
+ * The commands are grouped into the following subsystems. The subsystem
+ * code along with the opcode uniquely identify a particular fwcmd.
+ */
+#define FWCMD_SUBSYSTEM_RSVD  (0)	/* This subsystem is reserved. It is */
+						  /* never used. */
+#define FWCMD_SUBSYSTEM_COMMON (1)	/* CMDs in this group are common to
+					* all subsystems. See
+					* COMMON_SUBSYSTEM_OPCODES for opcodes
+					* and Common Host Configuration CMDs
+					* for the FWCMD descriptions.
+					*/
+#define FWCMD_SUBSYSTEM_COMMON_ISCSI    (2) /* CMDs in this group are */
+					/*
+					* common to Initiator and Target. See
+					* COMMON_ISCSI_SUBSYSTEM_OPCODES and
+					* Common iSCSI Initiator and Target
+					* CMDs for the command descriptions.
+					*/
+#define FWCMD_SUBSYSTEM_ETH             (3)	/* This subsystem is used to
+						execute  Ethernet commands.  */
+
+#define FWCMD_SUBSYSTEM_TPM             (4)	/* This subsystem is used
+						 to execute TPM  commands.  */
+#define FWCMD_SUBSYSTEM_PXE_UNDI        (5)	/* This subsystem is used
+						* to execute PXE
+						* and UNDI specific commands.
+						*/
+
+#define FWCMD_SUBSYSTEM_ISCSI_INI       (6)	/* This subsystem is used to
+						execute ISCSI Initiator
+						specific commands.
+						*/
+#define FWCMD_SUBSYSTEM_ISCSI_TGT       (7)	/* This subsystem is used
+						to execute iSCSI Target
+						specific commands.between
+						PTL and ARM firmware.
+						*/
+#define FWCMD_SUBSYSTEM_MILI_PTL        (8)	/* This subsystem is used to
+						execute iSCSI Target specific
+						commands.between MILI
+						and PTL.  */
+#define FWCMD_SUBSYSTEM_MILI_TMD        (9)	/* This subsystem is used to
+						execute iSCSI Target specific
+						commands between MILI
+						and TMD.  */
+#define FWCMD_SUBSYSTEM_PROXY           (11)	/* This subsystem is used
+						to execute proxied commands
+						within the host at the
+						explicit request of a
+						non priviledged domain.
+						This 'subsystem' is entirely
+						virtual from the controller
+						and firmware perspective as
+						it is implemented in host
+						drivers.
+						*/
+
+/*
+ * --- COMMON_SUBSYSTEM_OPCODES ---
+ * These opcodes are common to both networking and storage PCI
+ * functions. They are used to reserve resources and configure
+ * BladeEngine. These opcodes all use the FWCMD_SUBSYSTEM_COMMON
+ * subsystem code.
+ */
+#define OPCODE_COMMON_NTWK_MAC_QUERY    (1)
+#define SUBSYSTEM_COMMON_NTWK_MAC_QUERY (1)
+#define SUBSYSTEM_COMMON_NTWK_MAC_SET   (1)
+#define SUBSYSTEM_COMMON_NTWK_MULTICAST_SET (1)
+#define SUBSYSTEM_COMMON_NTWK_VLAN_CONFIG (1)
+#define SUBSYSTEM_COMMON_NTWK_LINK_STATUS_QUERY (1)
+#define SUBSYSTEM_COMMON_READ_FLASHROM  (1)
+#define SUBSYSTEM_COMMON_WRITE_FLASHROM (1)
+#define SUBSYSTEM_COMMON_QUERY_MAX_FWCMD_BUFFER_SIZE (1)
+#define SUBSYSTEM_COMMON_ADD_PAGE_TABLES (1)
+#define SUBSYSTEM_COMMON_REMOVE_PAGE_TABLES (1)
+#define SUBSYSTEM_COMMON_RING_DESTROY   (1)
+#define SUBSYSTEM_COMMON_CQ_CREATE      (1)
+#define SUBSYSTEM_COMMON_EQ_CREATE      (1)
+#define SUBSYSTEM_COMMON_ETH_RX_CREATE  (1)
+#define SUBSYSTEM_COMMON_ETH_TX_CREATE  (1)
+#define SUBSYSTEM_COMMON_ISCSI_DEFQ_CREATE (1)
+#define SUBSYSTEM_COMMON_ISCSI_WRBQ_CREATE (1)
+#define SUBSYSTEM_COMMON_MCC_CREATE     (1)
+#define SUBSYSTEM_COMMON_JELL_CONFIG    (1)
+#define SUBSYSTEM_COMMON_FORCE_FAILOVER (1)
+#define SUBSYSTEM_COMMON_ADD_TEMPLATE_HEADER_BUFFERS (1)
+#define SUBSYSTEM_COMMON_REMOVE_TEMPLATE_HEADER_BUFFERS (1)
+#define SUBSYSTEM_COMMON_POST_ZERO_BUFFER (1)
+#define SUBSYSTEM_COMMON_GET_QOS        (1)
+#define SUBSYSTEM_COMMON_SET_QOS        (1)
+#define SUBSYSTEM_COMMON_TCP_GET_STATISTICS (1)
+#define SUBSYSTEM_COMMON_SEEPROM_READ   (1)
+#define SUBSYSTEM_COMMON_TCP_STATE_QUERY (1)
+#define SUBSYSTEM_COMMON_GET_CNTL_ATTRIBUTES (1)
+#define SUBSYSTEM_COMMON_NOP            (1)
+#define SUBSYSTEM_COMMON_NTWK_RX_FILTER (1)
+#define SUBSYSTEM_COMMON_GET_FW_VERSION (1)
+#define SUBSYSTEM_COMMON_SET_FLOW_CONTROL (1)
+#define SUBSYSTEM_COMMON_GET_FLOW_CONTROL (1)
+#define SUBSYSTEM_COMMON_SET_TCP_PARAMETERS (1)
+#define SUBSYSTEM_COMMON_SET_FRAME_SIZE (1)
+#define SUBSYSTEM_COMMON_GET_FAT        (1)
+#define SUBSYSTEM_COMMON_MODIFY_EQ_DELAY (1)
+#define SUBSYSTEM_COMMON_FIRMWARE_CONFIG (1)
+#define SUBSYSTEM_COMMON_ENABLE_DISABLE_DOMAINS (1)
+#define SUBSYSTEM_COMMON_GET_DOMAIN_CONFIG (1)
+#define SUBSYSTEM_COMMON_SET_VLD_CONFIG (1)
+#define SUBSYSTEM_COMMON_GET_VLD_CONFIG (1)
+#define SUBSYSTEM_COMMON_GET_PORT_EQUALIZATION (1)
+#define SUBSYSTEM_COMMON_SET_PORT_EQUALIZATION (1)
+#define SUBSYSTEM_COMMON_RED_CONFIG     (1)
+#define OPCODE_COMMON_NTWK_MAC_SET      (2)
+#define OPCODE_COMMON_NTWK_MULTICAST_SET (3)
+#define OPCODE_COMMON_NTWK_VLAN_CONFIG  (4)
+#define OPCODE_COMMON_NTWK_LINK_STATUS_QUERY (5)
+#define OPCODE_COMMON_READ_FLASHROM     (6)
+#define OPCODE_COMMON_WRITE_FLASHROM    (7)
+#define OPCODE_COMMON_QUERY_MAX_FWCMD_BUFFER_SIZE (8)
+#define OPCODE_COMMON_ADD_PAGE_TABLES   (9)
+#define OPCODE_COMMON_REMOVE_PAGE_TABLES (10)
+#define OPCODE_COMMON_RING_DESTROY      (11)
+#define OPCODE_COMMON_CQ_CREATE         (12)
+#define OPCODE_COMMON_EQ_CREATE         (13)
+#define OPCODE_COMMON_ETH_RX_CREATE     (14)
+#define OPCODE_COMMON_ETH_TX_CREATE     (15)
+#define OPCODE_COMMON_NET_RESERVED0     (16)	/* Reserved */
+#define OPCODE_COMMON_NET_RESERVED1     (17)	/* Reserved */
+#define OPCODE_COMMON_NET_RESERVED2     (18)	/* Reserved */
+#define OPCODE_COMMON_ISCSI_DEFQ_CREATE (19)
+#define OPCODE_COMMON_ISCSI_WRBQ_CREATE (20)
+#define OPCODE_COMMON_MCC_CREATE        (21)
+#define OPCODE_COMMON_JELL_CONFIG       (22)
+#define OPCODE_COMMON_FORCE_FAILOVER    (23)
+#define OPCODE_COMMON_ADD_TEMPLATE_HEADER_BUFFERS (24)
+#define OPCODE_COMMON_REMOVE_TEMPLATE_HEADER_BUFFERS (25)
+#define OPCODE_COMMON_POST_ZERO_BUFFER  (26)
+#define OPCODE_COMMON_GET_QOS           (27)
+#define OPCODE_COMMON_SET_QOS           (28)
+#define OPCODE_COMMON_TCP_GET_STATISTICS (29)
+#define OPCODE_COMMON_SEEPROM_READ      (30)
+#define OPCODE_COMMON_TCP_STATE_QUERY   (31)
+#define OPCODE_COMMON_GET_CNTL_ATTRIBUTES (32)
+#define OPCODE_COMMON_NOP               (33)
+#define OPCODE_COMMON_NTWK_RX_FILTER    (34)
+#define OPCODE_COMMON_GET_FW_VERSION    (35)
+#define OPCODE_COMMON_SET_FLOW_CONTROL  (36)
+#define OPCODE_COMMON_GET_FLOW_CONTROL  (37)
+#define OPCODE_COMMON_SET_TCP_PARAMETERS (38)
+#define OPCODE_COMMON_SET_FRAME_SIZE    (39)
+#define OPCODE_COMMON_GET_FAT           (40)
+#define OPCODE_COMMON_MODIFY_EQ_DELAY   (41)
+#define OPCODE_COMMON_FIRMWARE_CONFIG   (42)
+#define OPCODE_COMMON_ENABLE_DISABLE_DOMAINS (43)
+#define OPCODE_COMMON_GET_DOMAIN_CONFIG (44)
+#define OPCODE_COMMON_SET_VLD_CONFIG    (45)
+#define OPCODE_COMMON_GET_VLD_CONFIG    (46)
+#define OPCODE_COMMON_GET_PORT_EQUALIZATION (47)
+#define OPCODE_COMMON_SET_PORT_EQUALIZATION (48)
+#define OPCODE_COMMON_RED_CONFIG        (49)
+
+
+
+/*
+ * --- ETH_SUBSYSTEM_OPCODES ---
+ * These opcodes are used for configuring the Ethernet interfaces. These
+ * opcodes all use the FWCMD_SUBSYSTEM_ETH subsystem code.
+ */
+#define OPCODE_ETH_RSS_CONFIG           (1)
+#define OPCODE_ETH_ACPI_CONFIG          (2)
+#define SUBSYSTEM_ETH_RSS_CONFIG        (3)
+#define SUBSYSTEM_ETH_ACPI_CONFIG       (3)
+#define OPCODE_ETH_PROMISCUOUS          (3)
+#define SUBSYSTEM_ETH_PROMISCUOUS       (3)
+#define SUBSYSTEM_ETH_GET_STATISTICS    (3)
+#define SUBSYSTEM_ETH_GET_RX_FRAG_SIZE  (3)
+#define SUBSYSTEM_ETH_SET_RX_FRAG_SIZE  (3)
+#define OPCODE_ETH_GET_STATISTICS       (4)
+#define OPCODE_ETH_GET_RX_FRAG_SIZE     (5)
+#define OPCODE_ETH_SET_RX_FRAG_SIZE     (6)
+
+
+
+
+
+/*
+ * --- MCC_STATUS_CODE ---
+ * These are the global status codes used by all subsystems
+ */
+#define MCC_STATUS_SUCCESS              (0)	/* Indicates a successful
+						completion of  the command */
+#define MCC_STATUS_INSUFFICIENT_PRIVILEGES (1)	/* The client does not have
+						sufficient privileges to
+						execute the command */
+#define MCC_STATUS_INVALID_PARAMETER    (2)	/* A parameter in the command
+						was invalid. The extended
+						status contains the index
+						of the parameter */
+#define MCC_STATUS_INSUFFICIENT_RESOURCES (3)	/* There are insufficient
+						chip resources to execute
+						the command */
+#define MCC_STATUS_QUEUE_FLUSHING       (4)	/* The command is completing
+						because the queue was
+						getting flushed */
+#define MCC_STATUS_DMA_FAILED           (5)	/* The command is completing
+						with a DMA error */
+
+/*
+ * --- MGMT_ERROR_CODES ---
+ * Error Codes returned in the status field of the FWCMD response header
+ */
+#define MGMT_STATUS_SUCCESS             (0)	/* The FWCMD completed
+						without errors */
+#define MGMT_STATUS_FAILED              (1)	/* Error status in the Status
+						field of  the
+						struct FWCMD_RESPONSE_HEADER */
+#define MGMT_STATUS_ILLEGAL_REQUEST     (2)	/* Invalid FWCMD opcode */
+#define MGMT_STATUS_ILLEGAL_FIELD       (3)	/* Invalid parameter in
+						the FWCMD  payload */
+
+#endif /* __fwcmd_opcodes_amap_h__ */
diff --git a/drivers/staging/benet/fwcmd_types_bmap.h b/drivers/staging/benet/fwcmd_types_bmap.h
new file mode 100644
index 00000000000..92217aff3a1
--- /dev/null
+++ b/drivers/staging/benet/fwcmd_types_bmap.h
@@ -0,0 +1,29 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __fwcmd_types_bmap_h__
+#define __fwcmd_types_bmap_h__
+
+/* MAC address format  */
+struct MAC_ADDRESS_FORMAT {
+	u16 SizeOfStructure;
+	u8 MACAddress[6];
+} __packed;
+
+#endif /* __fwcmd_types_bmap_h__ */
diff --git a/drivers/staging/benet/host_struct.h b/drivers/staging/benet/host_struct.h
new file mode 100644
index 00000000000..3de6722b980
--- /dev/null
+++ b/drivers/staging/benet/host_struct.h
@@ -0,0 +1,182 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __host_struct_amap_h__
+#define __host_struct_amap_h__
+#include "be_cm.h"
+#include "be_common.h"
+#include "descriptors.h"
+
+/* --- EQ_COMPLETION_MAJOR_CODE_ENUM --- */
+#define EQ_MAJOR_CODE_COMPLETION        (0)	/* Completion event on a */
+						  /* qcompletion ueue. */
+#define EQ_MAJOR_CODE_ETH               (1)	/* Affiliated Ethernet Event. */
+#define EQ_MAJOR_CODE_RESERVED          (2)	/* Reserved */
+#define EQ_MAJOR_CODE_RDMA              (3)	/* Affiliated RDMA Event. */
+#define EQ_MAJOR_CODE_ISCSI             (4)	/* Affiliated ISCSI Event */
+#define EQ_MAJOR_CODE_UNAFFILIATED      (5)	/* Unaffiliated Event */
+
+/* --- EQ_COMPLETION_MINOR_CODE_ENUM --- */
+#define EQ_MINOR_CODE_COMPLETION        (0)	/* Completion event on a */
+						  /* completion queue. */
+#define EQ_MINOR_CODE_OTHER             (1)	/* Other Event (TBD). */
+
+/* Queue Entry Definition for all 4 byte event queue types. */
+struct BE_EQ_ENTRY_AMAP {
+	u8 Valid;		/* DWORD 0 */
+	u8 MajorCode[3];	/* DWORD 0 */
+	u8 MinorCode[12];	/* DWORD 0 */
+	u8 ResourceID[16];	/* DWORD 0 */
+} __packed;
+struct EQ_ENTRY_AMAP {
+	u32 dw[1];
+};
+
+/*
+ * --- ETH_EVENT_CODE ---
+ * These codes are returned by the MPU when one of these events has occurred,
+ * and the event is configured to report to an Event Queue when an event
+ * is detected.
+ */
+#define ETH_EQ_LINK_STATUS              (0)	/* Link status change event */
+						  /* detected. */
+#define ETH_EQ_WATERMARK                (1)	/* watermark event detected. */
+#define ETH_EQ_MAGIC_PKT                (2)	/* magic pkt event detected. */
+#define ETH_EQ_ACPI_PKT0                (3)	/* ACPI interesting packet */
+						  /* detected. */
+#define ETH_EQ_ACPI_PKT1                (3)	/* ACPI interesting packet */
+						  /* detected. */
+#define ETH_EQ_ACPI_PKT2                (3)	/* ACPI interesting packet */
+						  /* detected. */
+#define ETH_EQ_ACPI_PKT3                (3)	/* ACPI interesting packet */
+						  /* detected. */
+
+/*
+ * --- ETH_TX_COMPL_STATUS_ENUM ---
+ * Status codes contained in Ethernet TX completion descriptors.
+ */
+#define ETH_COMP_VALID                  (0)
+#define ETH_COMP_ERROR                  (1)
+#define ETH_COMP_INVALID                (15)
+
+/*
+ * --- ETH_TX_COMPL_PORT_ENUM ---
+ * Port indicator contained in Ethernet TX completion descriptors.
+ */
+#define ETH_COMP_PORT0                  (0)
+#define ETH_COMP_PORT1                  (1)
+#define ETH_COMP_MGMT                   (2)
+
+/*
+ * --- ETH_TX_COMPL_CT_ENUM ---
+ * Completion type indicator contained in Ethernet TX completion descriptors.
+ */
+#define ETH_COMP_ETH                    (0)
+
+/*
+ * Work request block that the driver issues to the chip for
+ * Ethernet transmissions. All control fields must be valid in each WRB for
+ * a message. The controller, as specified by the flags, optionally writes
+ * an entry to the Completion Ring and generate an event.
+ */
+struct BE_ETH_WRB_AMAP {
+	u8 frag_pa_hi[32];	/* DWORD 0 */
+	u8 frag_pa_lo[32];	/* DWORD 1 */
+	u8 complete;	/* DWORD 2 */
+	u8 event;		/* DWORD 2 */
+	u8 crc;		/* DWORD 2 */
+	u8 forward;		/* DWORD 2 */
+	u8 ipsec;		/* DWORD 2 */
+	u8 mgmt;		/* DWORD 2 */
+	u8 ipcs;		/* DWORD 2 */
+	u8 udpcs;		/* DWORD 2 */
+	u8 tcpcs;		/* DWORD 2 */
+	u8 lso;		/* DWORD 2 */
+	u8 last;		/* DWORD 2 */
+	u8 vlan;		/* DWORD 2 */
+	u8 dbg[3];		/* DWORD 2 */
+	u8 hash_val[3];	/* DWORD 2 */
+	u8 lso_mss[14];	/* DWORD 2 */
+	u8 frag_len[16];	/* DWORD 3 */
+	u8 vlan_tag[16];	/* DWORD 3 */
+} __packed;
+struct ETH_WRB_AMAP {
+	u32 dw[4];
+};
+
+/* This is an Ethernet transmit completion descriptor */
+struct BE_ETH_TX_COMPL_AMAP {
+	u8 user_bytes[16];	/* DWORD 0 */
+	u8 nwh_bytes[8];	/* DWORD 0 */
+	u8 lso;		/* DWORD 0 */
+	u8 rsvd0[7];	/* DWORD 0 */
+	u8 wrb_index[16];	/* DWORD 1 */
+	u8 ct[2];		/* DWORD 1 */
+	u8 port[2];		/* DWORD 1 */
+	u8 rsvd1[8];	/* DWORD 1 */
+	u8 status[4];	/* DWORD 1 */
+	u8 rsvd2[16];	/* DWORD 2 */
+	u8 ringid[11];	/* DWORD 2 */
+	u8 hash_val[4];	/* DWORD 2 */
+	u8 valid;		/* DWORD 2 */
+	u8 rsvd3[32];	/* DWORD 3 */
+} __packed;
+struct ETH_TX_COMPL_AMAP {
+	u32 dw[4];
+};
+
+/* Ethernet Receive Buffer descriptor */
+struct BE_ETH_RX_D_AMAP {
+	u8 fragpa_hi[32];	/* DWORD 0 */
+	u8 fragpa_lo[32];	/* DWORD 1 */
+} __packed;
+struct ETH_RX_D_AMAP {
+	u32 dw[2];
+};
+
+/* This is an Ethernet Receive Completion Descriptor */
+struct BE_ETH_RX_COMPL_AMAP {
+	u8 vlan_tag[16];	/* DWORD 0 */
+	u8 pktsize[14];	/* DWORD 0 */
+	u8 port;		/* DWORD 0 */
+	u8 rsvd0;		/* DWORD 0 */
+	u8 err;		/* DWORD 1 */
+	u8 rsshp;		/* DWORD 1 */
+	u8 ipf;		/* DWORD 1 */
+	u8 tcpf;		/* DWORD 1 */
+	u8 udpf;		/* DWORD 1 */
+	u8 ipcksm;		/* DWORD 1 */
+	u8 tcpcksm;		/* DWORD 1 */
+	u8 udpcksm;		/* DWORD 1 */
+	u8 macdst[6];	/* DWORD 1 */
+	u8 vtp;		/* DWORD 1 */
+	u8 vtm;		/* DWORD 1 */
+	u8 fragndx[10];	/* DWORD 1 */
+	u8 ct[2];		/* DWORD 1 */
+	u8 ipsec;		/* DWORD 1 */
+	u8 numfrags[3];	/* DWORD 1 */
+	u8 rsvd1[31];	/* DWORD 2 */
+	u8 valid;		/* DWORD 2 */
+	u8 rsshash[32];	/* DWORD 3 */
+} __packed;
+struct ETH_RX_COMPL_AMAP {
+	u32 dw[4];
+};
+
+#endif /* __host_struct_amap_h__ */
diff --git a/drivers/staging/benet/hwlib.h b/drivers/staging/benet/hwlib.h
new file mode 100644
index 00000000000..afedf4dc590
--- /dev/null
+++ b/drivers/staging/benet/hwlib.h
@@ -0,0 +1,830 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+#ifndef __hwlib_h__
+#define __hwlib_h__
+
+#include <linux/module.h>
+#include <linux/io.h>
+#include <linux/list.h>
+#include <linux/spinlock.h>
+
+#include "regmap.h"		/* srcgen array map output */
+
+#include "asyncmesg.h"
+#include "fwcmd_opcodes.h"
+#include "post_codes.h"
+#include "fwcmd_mcc.h"
+
+#include "fwcmd_types_bmap.h"
+#include "fwcmd_common_bmap.h"
+#include "fwcmd_eth_bmap.h"
+#include "bestatus.h"
+/*
+ *
+ * Macros for reading/writing a protection domain or CSR registers
+ * in BladeEngine.
+ */
+#define PD_READ(fo, field)	ioread32((fo)->db_va + \
+		offsetof(struct BE_PROTECTION_DOMAIN_DBMAP_AMAP, field)/8)
+
+#define PD_WRITE(fo, field, val) iowrite32(val, (fo)->db_va + \
+		offsetof(struct BE_PROTECTION_DOMAIN_DBMAP_AMAP, field)/8)
+
+#define CSR_READ(fo, field)	ioread32((fo)->csr_va + \
+		offsetof(struct BE_BLADE_ENGINE_CSRMAP_AMAP, field)/8)
+
+#define CSR_WRITE(fo, field, val)	iowrite32(val, (fo)->csr_va + \
+		offsetof(struct BE_BLADE_ENGINE_CSRMAP_AMAP, field)/8)
+
+#define PCICFG0_READ(fo, field)	ioread32((fo)->pci_va + \
+		offsetof(struct BE_PCICFG0_CSRMAP_AMAP, field)/8)
+
+#define PCICFG0_WRITE(fo, field, val)	iowrite32(val, (fo)->pci_va + \
+		offsetof(struct BE_PCICFG0_CSRMAP_AMAP, field)/8)
+
+#define PCICFG1_READ(fo, field)	ioread32((fo)->pci_va + \
+		offsetof(struct BE_PCICFG1_CSRMAP_AMAP, field)/8)
+
+#define PCICFG1_WRITE(fo, field, val)	iowrite32(val, (fo)->pci_va + \
+		offsetof(struct BE_PCICFG1_CSRMAP_AMAP, field)/8)
+
+#ifdef BE_DEBUG
+#define ASSERT(c)       BUG_ON(!(c));
+#else
+#define ASSERT(c)
+#endif
+
+/* debug levels */
+enum BE_DEBUG_LEVELS {
+	DL_ALWAYS = 0,		/* cannot be masked */
+	DL_ERR = 0x1,		/* errors that should never happen */
+	DL_WARN = 0x2,		/* something questionable.
+				   recoverable errors */
+	DL_NOTE = 0x4,		/* infrequent, important debug info */
+	DL_INFO = 0x8,		/* debug information */
+	DL_VERBOSE = 0x10,	/* detailed info, such as buffer traces */
+	BE_DL_MIN_VALUE = 0x1,	/* this is the min value used */
+	BE_DL_MAX_VALUE = 0x80	/* this is the higheset value used */
+} ;
+
+extern unsigned int trace_level;
+
+#define TRACE(lm, fmt, args...)  {				\
+		if (trace_level & lm) {				\
+			printk(KERN_NOTICE "BE: %s:%d \n" fmt,	\
+			__FILE__ , __LINE__ , ## args);		\
+		}						\
+	}
+
+static inline unsigned int be_trace_set_level(unsigned int level)
+{
+	unsigned int old_level = trace_level;
+	trace_level = level;
+	return old_level;
+}
+
+#define be_trace_get_level() 	trace_level
+/*
+ * Returns number of pages spanned by the size of data
+ * starting at the given address.
+ */
+#define PAGES_SPANNED(_address, _size) \
+   ((u32)((((size_t)(_address) & (PAGE_SIZE - 1)) + \
+		(_size) + (PAGE_SIZE - 1)) >> PAGE_SHIFT))
+/* Byte offset into the page corresponding to given address */
+#define OFFSET_IN_PAGE(_addr_) ((size_t)(_addr_) & (PAGE_SIZE-1))
+
+/*
+ * circular subtract.
+ * Returns a - b assuming a circular number system, where a and b are
+ * in range (0, maxValue-1). If a==b, zero is returned so the
+ * highest value possible with this subtraction is maxValue-1.
+ */
+static inline u32 be_subc(u32 a, u32 b, u32 max)
+{
+	ASSERT(a <= max && b <= max);
+	ASSERT(max > 0);
+	return a >= b ? (a - b) : (max - b + a);
+}
+
+static inline u32 be_addc(u32 a, u32 b, u32 max)
+{
+	ASSERT(a < max);
+	ASSERT(max > 0);
+	return (max - a > b) ? (a + b) : (b + a - max);
+}
+
+/* descriptor for a physically contiguous memory used for ring */
+struct ring_desc {
+	u32 length;	/* length in bytes */
+	void *va; 	/* virtual address */
+	u64 pa;		/* bus address */
+} ;
+
+/*
+ * This structure stores information about a ring shared between hardware
+ * and software.  Each ring is allocated by the driver in the uncached
+ * extension and mapped into BladeEngine's unified table.
+ */
+struct mp_ring {
+	u32 pages;		/* queue size in pages */
+	u32 id;			/* queue id assigned by beklib */
+	u32 num;		/* number of elements in queue */
+	u32 cidx;		/* consumer index */
+	u32 pidx;		/* producer index -- not used by most rings */
+	u32 itemSize;		/* size in bytes of one object */
+
+	void *va;		/* The virtual address of the ring.
+				   This should be last to allow 32 & 64
+				   bit debugger extensions to work. */
+} ;
+
+/*-----------  amap bit filed get / set macros and functions -----*/
+/*
+ * Structures defined in the map header files (under fw/amap/) with names
+ * in the format BE_<name>_AMAP are pseudo structures with members
+ * of type u8. These structures are templates that are used in
+ * conjuntion with the structures with names in the format
+ * <name>_AMAP to calculate the bit masks and bit offsets to get or set
+ * bit fields in structures. The structures <name>_AMAP are arrays
+ * of 32 bits words and have the correct size.  The following macros
+ * provide convenient ways to get and set the various members
+ * in the structures without using strucctures with bit fields.
+ * Always use the macros AMAP_GET_BITS_PTR and AMAP_SET_BITS_PTR
+ * macros to extract and set various members.
+ */
+
+/*
+ * Returns the a bit mask for the register that is NOT shifted into location.
+ * That means return values always look like: 0x1, 0xFF, 0x7FF, etc...
+ */
+static inline u32 amap_mask(u32 bit_size)
+{
+	return bit_size == 32 ? 0xFFFFFFFF : (1 << bit_size) - 1;
+}
+
+#define AMAP_BIT_MASK(_struct_, field)       \
+	amap_mask(AMAP_BIT_SIZE(_struct_, field))
+
+/*
+ * non-optimized set bits function. First clears the bits and then assigns them.
+ * This does not require knowledge of the particular DWORD you are setting.
+ * e.g. AMAP_SET_BITS_PTR (struct, field1, &contextMemory, 123);
+ */
+static inline void
+amap_set(void *ptr, u32 dw_offset, u32 mask, u32 offset, u32 value)
+{
+	u32 *dw = (u32 *)ptr;
+	*(dw + dw_offset) &= ~(mask << offset);
+	*(dw + dw_offset) |= (mask & value) << offset;
+}
+
+#define AMAP_SET_BITS_PTR(_struct_, field, _structPtr_, val)	\
+	amap_set(_structPtr_, AMAP_WORD_OFFSET(_struct_, field),\
+		AMAP_BIT_MASK(_struct_, field),			\
+		AMAP_BIT_OFFSET(_struct_, field), val)
+/*
+ * Non-optimized routine that gets the bits without knowing the correct DWORD.
+ * e.g. fieldValue = AMAP_GET_BITS_PTR (struct, field1, &contextMemory);
+ */
+static inline u32
+amap_get(void *ptr, u32 dw_offset, u32 mask, u32 offset)
+{
+	u32 *dw = (u32 *)ptr;
+	return mask & (*(dw + dw_offset) >> offset);
+}
+#define AMAP_GET_BITS_PTR(_struct_, field, _structPtr_)			\
+	amap_get(_structPtr_, AMAP_WORD_OFFSET(_struct_, field),	\
+		AMAP_BIT_MASK(_struct_, field),				\
+		AMAP_BIT_OFFSET(_struct_, field))
+
+/* Returns 0-31 representing bit offset within a DWORD of a bitfield. */
+#define AMAP_BIT_OFFSET(_struct_, field)                  \
+	(offsetof(struct BE_ ## _struct_ ## _AMAP, field) % 32)
+
+/* Returns 0-n representing DWORD offset of bitfield within the structure. */
+#define AMAP_WORD_OFFSET(_struct_, field)  \
+		  (offsetof(struct BE_ ## _struct_ ## _AMAP, field)/32)
+
+/* Returns size of bitfield in bits. */
+#define AMAP_BIT_SIZE(_struct_, field) \
+		sizeof(((struct BE_ ## _struct_ ## _AMAP*)0)->field)
+
+struct be_mcc_wrb_response_copy {
+	u16 length;		/* bytes in response */
+	u16 fwcmd_offset;	/* offset within the wrb of the response */
+	void *va;		/* user's va to copy response into */
+
+} ;
+typedef void (*mcc_wrb_cqe_callback) (void *context, int status,
+				struct MCC_WRB_AMAP *optional_wrb);
+struct be_mcc_wrb_context {
+
+	mcc_wrb_cqe_callback internal_cb;	/* Function to call on
+						completion */
+	void *internal_cb_context;	/* Parameter to pass
+						   to completion function */
+
+	mcc_wrb_cqe_callback cb;	/* Function to call on completion */
+	void *cb_context;	/* Parameter to pass to completion function */
+
+	int *users_final_status;	/* pointer to a local
+						variable for synchronous
+						commands */
+	struct MCC_WRB_AMAP *wrb;	/* pointer to original wrb for embedded
+						commands only */
+	struct list_head next;	/* links context structs together in
+				   free list */
+
+	struct be_mcc_wrb_response_copy copy;	/* Optional parameters to copy
+					   embedded response to user's va */
+
+#if defined(BE_DEBUG)
+	u16 subsystem, opcode;	/* Track this FWCMD for debug builds. */
+	struct MCC_WRB_AMAP *ring_wrb;
+	u32 consumed_count;
+#endif
+} ;
+
+/*
+    Represents a function object for network or storage.  This
+    is used to manage per-function resources like MCC CQs, etc.
+*/
+struct be_function_object {
+
+	u32 magic;		/*!< magic for detecting memory corruption. */
+
+	/* PCI BAR mapped addresses */
+	u8 __iomem *csr_va;	/* CSR */
+	u8 __iomem *db_va;	/* Door Bell */
+	u8 __iomem *pci_va;	/* PCI config space */
+	u32 emulate;		/* if set, MPU is not available.
+				  Emulate everything.     */
+	u32 pend_queue_driving;	/* if set, drive the queued WRBs
+				   after releasing the WRB lock */
+
+	spinlock_t post_lock;	/* lock for verifying one thread posting wrbs */
+	spinlock_t cq_lock;	/* lock for verifying one thread
+				   processing cq */
+	spinlock_t mcc_context_lock;	/* lock for protecting mcc
+					   context free list */
+	unsigned long post_irq;
+	unsigned long cq_irq;
+
+	u32 type;
+	u32 pci_function_number;
+
+	struct be_mcc_object *mcc;	/* mcc rings. */
+
+	struct {
+		struct MCC_MAILBOX_AMAP *va;	/* VA to the mailbox */
+		u64 pa;	/* PA to the mailbox */
+		u32 length;	/* byte length of mailbox */
+
+		/* One default context struct used for posting at
+		 * least one MCC_WRB
+		 */
+		struct be_mcc_wrb_context default_context;
+		bool default_context_allocated;
+	} mailbox;
+
+	struct {
+
+		/* Wake on lans configured. */
+		u32 wol_bitmask;	/* bits 0,1,2,3 are set if
+					   corresponding index is enabled */
+	} config;
+
+
+	struct BE_FIRMWARE_CONFIG fw_config;
+} ;
+
+/*
+      Represents an Event Queue
+*/
+struct be_eq_object {
+	u32 magic;
+	atomic_t ref_count;
+
+	struct be_function_object *parent_function;
+
+	struct list_head eq_list;
+	struct list_head cq_list_head;
+
+	u32 eq_id;
+	void *cb_context;
+
+} ;
+
+/*
+    Manages a completion queue
+*/
+struct be_cq_object {
+	u32 magic;
+	atomic_t ref_count;
+
+	struct be_function_object *parent_function;
+	struct be_eq_object *eq_object;
+
+	struct list_head cq_list;
+	struct list_head cqlist_for_eq;
+
+	void *va;
+	u32 num_entries;
+
+	void *cb_context;
+
+	u32 cq_id;
+
+} ;
+
+/*
+    Manages an ethernet send queue
+*/
+struct be_ethsq_object {
+	u32 magic;
+
+	struct list_head list;
+
+	struct be_function_object *parent_function;
+	struct be_cq_object *cq_object;
+	u32 bid;
+
+} ;
+
+/*
+@brief
+    Manages an ethernet receive queue
+*/
+struct be_ethrq_object {
+	u32 magic;
+	struct list_head list;
+	struct be_function_object *parent_function;
+	u32 rid;
+	struct be_cq_object *cq_object;
+	struct be_cq_object *rss_cq_object[4];
+
+} ;
+
+/*
+    Manages an MCC
+*/
+typedef void (*mcc_async_event_callback) (void *context, u32 event_code,
+				void *event);
+struct be_mcc_object {
+	u32 magic;
+
+	struct be_function_object *parent_function;
+	struct list_head mcc_list;
+
+	struct be_cq_object *cq_object;
+
+	/* Async event callback for MCC CQ. */
+	mcc_async_event_callback async_cb;
+	void *async_context;
+
+	struct {
+		struct be_mcc_wrb_context *base;
+		u32 num;
+		struct list_head list_head;
+	} wrb_context;
+
+	struct {
+		struct ring_desc *rd;
+		struct mp_ring ring;
+	} sq;
+
+	struct {
+		struct mp_ring ring;
+	} cq;
+
+	u32 processing;		/* flag indicating that one thread
+				   is processing CQ */
+	u32 rearm;		/* doorbell rearm setting to make
+				   sure the active processing thread */
+	/* rearms the CQ if any of the threads requested it. */
+
+	struct list_head backlog;
+	u32 backlog_length;
+	u32 driving_backlog;
+	u32 consumed_index;
+
+} ;
+
+
+/* Queue context header -- the required software information for
+ * queueing a WRB.
+ */
+struct be_queue_driver_context {
+	mcc_wrb_cqe_callback internal_cb;	/* Function to call on
+						   completion */
+	void *internal_cb_context;	/* Parameter to pass
+						   to completion function */
+
+	mcc_wrb_cqe_callback cb;	/* Function to call on completion */
+	void *cb_context;	/* Parameter to pass to completion function */
+
+	struct be_mcc_wrb_response_copy copy;	/* Optional parameters to copy
+					   embedded response to user's va */
+	void *optional_fwcmd_va;
+	struct list_head list;
+	u32 bytes;
+} ;
+
+/*
+ * Common MCC WRB header that all commands require.
+ */
+struct be_mcc_wrb_header {
+	u8 rsvd[offsetof(struct BE_MCC_WRB_AMAP, payload)/8];
+} ;
+
+/*
+ * All non embedded commands supported by hwlib functions only allow
+ * 1 SGE.  This queue context handles them all.
+ */
+struct be_nonembedded_q_ctxt {
+	struct be_queue_driver_context context;
+	struct be_mcc_wrb_header wrb_header;
+	struct MCC_SGE_AMAP sge[1];
+} ;
+
+/*
+ * ------------------------------------------------------------------------
+ *  This section contains the specific queue struct for each command.
+ *  The user could always provide a be_generic_q_ctxt but this is a
+ *  rather large struct.  By using the specific struct, memory consumption
+ *  can be reduced.
+ * ------------------------------------------------------------------------
+ */
+
+struct be_link_status_q_ctxt {
+	struct be_queue_driver_context context;
+	struct be_mcc_wrb_header wrb_header;
+	struct FWCMD_COMMON_NTWK_LINK_STATUS_QUERY fwcmd;
+} ;
+
+struct be_multicast_q_ctxt {
+	struct be_queue_driver_context context;
+	struct be_mcc_wrb_header wrb_header;
+	struct FWCMD_COMMON_NTWK_MULTICAST_SET fwcmd;
+} ;
+
+
+struct be_vlan_q_ctxt {
+	struct be_queue_driver_context context;
+	struct be_mcc_wrb_header wrb_header;
+	struct FWCMD_COMMON_NTWK_VLAN_CONFIG fwcmd;
+} ;
+
+struct be_promiscuous_q_ctxt {
+	struct be_queue_driver_context context;
+	struct be_mcc_wrb_header wrb_header;
+	struct FWCMD_ETH_PROMISCUOUS fwcmd;
+} ;
+
+struct be_force_failover_q_ctxt {
+	struct be_queue_driver_context context;
+	struct be_mcc_wrb_header wrb_header;
+	struct FWCMD_COMMON_FORCE_FAILOVER fwcmd;
+} ;
+
+
+struct be_rxf_filter_q_ctxt {
+	struct be_queue_driver_context context;
+	struct be_mcc_wrb_header wrb_header;
+	struct FWCMD_COMMON_NTWK_RX_FILTER fwcmd;
+} ;
+
+struct be_eq_modify_delay_q_ctxt {
+	struct be_queue_driver_context context;
+	struct be_mcc_wrb_header wrb_header;
+	struct FWCMD_COMMON_MODIFY_EQ_DELAY fwcmd;
+} ;
+
+/*
+ * The generic context is the largest size that would be required.
+ * It is the software context plus an entire WRB.
+ */
+struct be_generic_q_ctxt {
+	struct be_queue_driver_context context;
+	struct be_mcc_wrb_header wrb_header;
+	struct MCC_WRB_PAYLOAD_AMAP payload;
+} ;
+
+/*
+ * Types for the BE_QUEUE_CONTEXT object.
+ */
+#define BE_QUEUE_INVALID	(0)
+#define BE_QUEUE_LINK_STATUS	(0xA006)
+#define BE_QUEUE_ETH_STATS	(0xA007)
+#define BE_QUEUE_TPM_STATS	(0xA008)
+#define BE_QUEUE_TCP_STATS	(0xA009)
+#define BE_QUEUE_MULTICAST	(0xA00A)
+#define BE_QUEUE_VLAN		(0xA00B)
+#define BE_QUEUE_RSS		(0xA00C)
+#define BE_QUEUE_FORCE_FAILOVER	(0xA00D)
+#define BE_QUEUE_PROMISCUOUS	(0xA00E)
+#define BE_QUEUE_WAKE_ON_LAN	(0xA00F)
+#define BE_QUEUE_NOP		(0xA010)
+
+/* --- BE_FUNCTION_ENUM --- */
+#define BE_FUNCTION_TYPE_ISCSI          (0)
+#define BE_FUNCTION_TYPE_NETWORK        (1)
+#define BE_FUNCTION_TYPE_ARM            (2)
+
+/* --- BE_ETH_TX_RING_TYPE_ENUM --- */
+#define BE_ETH_TX_RING_TYPE_FORWARDING  (1) 	/* Ether ring for forwarding */
+#define BE_ETH_TX_RING_TYPE_STANDARD    (2)	/* Ether ring for sending */
+						/* network packets. */
+#define BE_ETH_TX_RING_TYPE_BOUND       (3)	/* Ethernet ring for sending */
+						/* network packets, bound */
+						/* to a physical port. */
+/*
+ * ----------------------------------------------------------------------
+ *   API MACROS
+ * ----------------------------------------------------------------------
+ */
+#define BE_FWCMD_NAME(_short_name_)     struct FWCMD_##_short_name_
+#define BE_OPCODE_NAME(_short_name_)    OPCODE_##_short_name_
+#define BE_SUBSYSTEM_NAME(_short_name_) SUBSYSTEM_##_short_name_
+
+
+#define BE_PREPARE_EMBEDDED_FWCMD(_pfob_, _wrb_, _short_name_)	\
+	((BE_FWCMD_NAME(_short_name_) *)				\
+	be_function_prepare_embedded_fwcmd(_pfob_, _wrb_,	\
+		sizeof(BE_FWCMD_NAME(_short_name_)),		\
+		FIELD_SIZEOF(BE_FWCMD_NAME(_short_name_), params.request), \
+		FIELD_SIZEOF(BE_FWCMD_NAME(_short_name_), params.response), \
+		BE_OPCODE_NAME(_short_name_),				\
+		BE_SUBSYSTEM_NAME(_short_name_)));
+
+#define BE_PREPARE_NONEMBEDDED_FWCMD(_pfob_, _wrb_, _iva_, _ipa_, _short_name_)\
+	((BE_FWCMD_NAME(_short_name_) *)				\
+	be_function_prepare_nonembedded_fwcmd(_pfob_, _wrb_, (_iva_), (_ipa_), \
+		sizeof(BE_FWCMD_NAME(_short_name_)),		\
+		FIELD_SIZEOF(BE_FWCMD_NAME(_short_name_), params.request), \
+		FIELD_SIZEOF(BE_FWCMD_NAME(_short_name_), params.response), \
+		BE_OPCODE_NAME(_short_name_),				\
+		BE_SUBSYSTEM_NAME(_short_name_)));
+
+int be_function_object_create(u8 __iomem *csr_va, u8 __iomem *db_va,
+	u8 __iomem *pci_va, u32 function_type, struct ring_desc *mailbox_rd,
+	  struct be_function_object *pfob);
+
+int be_function_object_destroy(struct be_function_object *pfob);
+int be_function_cleanup(struct be_function_object *pfob);
+
+
+int be_function_get_fw_version(struct be_function_object *pfob,
+	struct FWCMD_COMMON_GET_FW_VERSION_RESPONSE_PAYLOAD *fw_version,
+	mcc_wrb_cqe_callback cb, void *cb_context);
+
+
+int be_eq_modify_delay(struct be_function_object *pfob,
+		   u32 num_eq, struct be_eq_object **eq_array,
+		   u32 *eq_delay_array, mcc_wrb_cqe_callback cb,
+		   void *cb_context,
+		   struct be_eq_modify_delay_q_ctxt *q_ctxt);
+
+
+
+int be_eq_create(struct be_function_object *pfob,
+	     struct ring_desc *rd, u32 eqe_size, u32 num_entries,
+	     u32 watermark, u32 timer_delay, struct be_eq_object *eq_object);
+
+int be_eq_destroy(struct be_eq_object *eq);
+
+int be_cq_create(struct be_function_object *pfob,
+	struct ring_desc *rd, u32 length,
+	bool solicited_eventable, bool no_delay,
+	u32 wm_thresh, struct be_eq_object *eq_object,
+	struct be_cq_object *cq_object);
+
+int be_cq_destroy(struct be_cq_object *cq);
+
+int be_mcc_ring_create(struct be_function_object *pfob,
+		   struct ring_desc *rd, u32 length,
+		   struct be_mcc_wrb_context *context_array,
+		   u32 num_context_entries,
+		   struct be_cq_object *cq, struct be_mcc_object *mcc);
+int be_mcc_ring_destroy(struct be_mcc_object *mcc_object);
+
+int be_mcc_process_cq(struct be_mcc_object *mcc_object, bool rearm);
+
+int be_mcc_add_async_event_callback(struct be_mcc_object *mcc_object,
+		mcc_async_event_callback cb, void *cb_context);
+
+int be_pci_soft_reset(struct be_function_object *pfob);
+
+
+int be_drive_POST(struct be_function_object *pfob);
+
+
+int be_eth_sq_create(struct be_function_object *pfob,
+		struct ring_desc *rd, u32 length_in_bytes,
+		u32 type, u32 ulp, struct be_cq_object *cq_object,
+		struct be_ethsq_object *eth_sq);
+
+struct be_eth_sq_parameters {
+	u32 port;
+	u32 rsvd0[2];
+} ;
+
+int be_eth_sq_create_ex(struct be_function_object *pfob,
+		    struct ring_desc *rd, u32 length_in_bytes,
+		    u32 type, u32 ulp, struct be_cq_object *cq_object,
+		    struct be_eth_sq_parameters *ex_parameters,
+		    struct be_ethsq_object *eth_sq);
+int be_eth_sq_destroy(struct be_ethsq_object *eth_sq);
+
+int be_eth_set_flow_control(struct be_function_object *pfob,
+			bool txfc_enable, bool rxfc_enable);
+
+int be_eth_get_flow_control(struct be_function_object *pfob,
+			bool *txfc_enable, bool *rxfc_enable);
+int be_eth_set_qos(struct be_function_object *pfob, u32 max_bps, u32 max_pps);
+
+int be_eth_get_qos(struct be_function_object *pfob, u32 *max_bps, u32 *max_pps);
+
+int be_eth_set_frame_size(struct be_function_object *pfob,
+		      u32 *tx_frame_size, u32 *rx_frame_size);
+
+int be_eth_rq_create(struct be_function_object *pfob,
+		 struct ring_desc *rd, struct be_cq_object *cq_object,
+		 struct be_cq_object *bcmc_cq_object,
+		 struct be_ethrq_object *eth_rq);
+
+int be_eth_rq_destroy(struct be_ethrq_object *eth_rq);
+
+int be_eth_rq_destroy_options(struct be_ethrq_object *eth_rq, bool flush,
+		mcc_wrb_cqe_callback cb, void *cb_context);
+int be_eth_rq_set_frag_size(struct be_function_object *pfob,
+		u32 new_frag_size_bytes, u32 *actual_frag_size_bytes);
+int be_eth_rq_get_frag_size(struct be_function_object *pfob,
+						u32 *frag_size_bytes);
+
+void *be_function_prepare_embedded_fwcmd(struct be_function_object *pfob,
+		   struct MCC_WRB_AMAP *wrb,
+		   u32 payload_length, u32 request_length,
+		   u32 response_length, u32 opcode, u32 subsystem);
+void *be_function_prepare_nonembedded_fwcmd(struct be_function_object *pfob,
+	struct MCC_WRB_AMAP *wrb, void *fwcmd_header_va, u64 fwcmd_header_pa,
+	u32 payload_length, u32 request_length, u32 response_length,
+	u32 opcode, u32 subsystem);
+
+
+struct MCC_WRB_AMAP *
+be_function_peek_mcc_wrb(struct be_function_object *pfob);
+
+int be_rxf_mac_address_read_write(struct be_function_object *pfob,
+	      bool port1, bool mac1, bool mgmt,
+	      bool write, bool permanent, u8 *mac_address,
+	      mcc_wrb_cqe_callback cb,
+	      void *cb_context);
+
+int be_rxf_multicast_config(struct be_function_object *pfob,
+			bool promiscuous, u32 num, u8 *mac_table,
+			mcc_wrb_cqe_callback cb,
+			void *cb_context,
+			struct be_multicast_q_ctxt *q_ctxt);
+
+int be_rxf_vlan_config(struct be_function_object *pfob,
+	   bool promiscuous, u32 num, u16 *vlan_tag_array,
+	   mcc_wrb_cqe_callback cb, void *cb_context,
+	   struct be_vlan_q_ctxt *q_ctxt);
+
+
+int be_rxf_link_status(struct be_function_object *pfob,
+		   struct BE_LINK_STATUS *link_status,
+		   mcc_wrb_cqe_callback cb,
+		   void *cb_context,
+		   struct be_link_status_q_ctxt *q_ctxt);
+
+
+int be_rxf_query_eth_statistics(struct be_function_object *pfob,
+		struct FWCMD_ETH_GET_STATISTICS *va_for_fwcmd,
+		u64 pa_for_fwcmd, mcc_wrb_cqe_callback cb,
+		void *cb_context,
+		struct be_nonembedded_q_ctxt *q_ctxt);
+
+int be_rxf_promiscuous(struct be_function_object *pfob,
+		   bool enable_port0, bool enable_port1,
+		   mcc_wrb_cqe_callback cb, void *cb_context,
+		   struct be_promiscuous_q_ctxt *q_ctxt);
+
+
+int be_rxf_filter_config(struct be_function_object *pfob,
+		     struct NTWK_RX_FILTER_SETTINGS *settings,
+		     mcc_wrb_cqe_callback cb,
+		     void *cb_context,
+		     struct be_rxf_filter_q_ctxt *q_ctxt);
+
+/*
+ * ------------------------------------------------------
+ *  internal functions used by hwlib
+ * ------------------------------------------------------
+ */
+
+
+int be_function_ring_destroy(struct be_function_object *pfob,
+		       u32 id, u32 ring_type, mcc_wrb_cqe_callback cb,
+		       void *cb_context,
+		       mcc_wrb_cqe_callback internal_cb,
+		       void *internal_callback_context);
+
+int be_function_post_mcc_wrb(struct be_function_object *pfob,
+		struct MCC_WRB_AMAP *wrb,
+		struct be_generic_q_ctxt *q_ctxt,
+		mcc_wrb_cqe_callback cb, void *cb_context,
+		mcc_wrb_cqe_callback internal_cb,
+		void *internal_cb_context, void *optional_fwcmd_va,
+		struct be_mcc_wrb_response_copy *response_copy);
+
+int be_function_queue_mcc_wrb(struct be_function_object *pfob,
+			  struct be_generic_q_ctxt *q_ctxt);
+
+/*
+ * ------------------------------------------------------
+ *  MCC QUEUE
+ * ------------------------------------------------------
+ */
+
+int be_mpu_init_mailbox(struct be_function_object *pfob, struct ring_desc *rd);
+
+
+struct MCC_WRB_AMAP *
+_be_mpu_peek_ring_wrb(struct be_mcc_object *mcc, bool driving_queue);
+
+struct be_mcc_wrb_context *
+_be_mcc_allocate_wrb_context(struct be_function_object *pfob);
+
+void _be_mcc_free_wrb_context(struct be_function_object *pfob,
+			 struct be_mcc_wrb_context *context);
+
+int _be_mpu_post_wrb_mailbox(struct be_function_object *pfob,
+	 struct MCC_WRB_AMAP *wrb, struct be_mcc_wrb_context *wrb_context);
+
+int _be_mpu_post_wrb_ring(struct be_mcc_object *mcc,
+	struct MCC_WRB_AMAP *wrb, struct be_mcc_wrb_context *wrb_context);
+
+void be_drive_mcc_wrb_queue(struct be_mcc_object *mcc);
+
+
+/*
+ * ------------------------------------------------------
+ *  Ring Sizes
+ * ------------------------------------------------------
+ */
+static inline u32 be_ring_encoding_to_length(u32 encoding, u32 object_size)
+{
+
+	ASSERT(encoding != 1);	/* 1 is rsvd */
+	ASSERT(encoding < 16);
+	ASSERT(object_size > 0);
+
+	if (encoding == 0)	/* 32k deep */
+		encoding = 16;
+
+	return (1 << (encoding - 1)) * object_size;
+}
+
+static inline
+u32 be_ring_length_to_encoding(u32 length_in_bytes, u32 object_size)
+{
+
+	u32 count, encoding;
+
+	ASSERT(object_size > 0);
+	ASSERT(length_in_bytes % object_size == 0);
+
+	count = length_in_bytes / object_size;
+
+	ASSERT(count > 1);
+	ASSERT(count <= 32 * 1024);
+	ASSERT(length_in_bytes <= 8 * PAGE_SIZE); /* max ring size in UT */
+
+	encoding = __ilog2_u32(count) + 1;
+
+	if (encoding == 16)
+		encoding = 0;	/* 32k deep */
+
+	return encoding;
+}
+
+void be_rd_to_pa_list(struct ring_desc *rd, struct PHYS_ADDR *pa_list,
+						u32 max_num);
+#endif /* __hwlib_h__ */
diff --git a/drivers/staging/benet/mpu.c b/drivers/staging/benet/mpu.c
new file mode 100644
index 00000000000..269cc11d305
--- /dev/null
+++ b/drivers/staging/benet/mpu.c
@@ -0,0 +1,1364 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+#include <linux/delay.h>
+#include "hwlib.h"
+#include "bestatus.h"
+
+static
+inline void mp_ring_create(struct mp_ring *ring, u32 num, u32 size, void *va)
+{
+	ASSERT(ring);
+	memset(ring, 0, sizeof(struct mp_ring));
+	ring->num = num;
+	ring->pages = DIV_ROUND_UP(num * size, PAGE_SIZE);
+	ring->itemSize = size;
+	ring->va = va;
+}
+
+/*
+ * -----------------------------------------------------------------------
+ * Interface for 2 index rings. i.e. consumer/producer rings
+ * --------------------------------------------------------------------------
+ */
+
+/* Returns number items pending on ring. */
+static inline u32 mp_ring_num_pending(struct mp_ring *ring)
+{
+	ASSERT(ring);
+	if (ring->num == 0)
+		return 0;
+	return be_subc(ring->pidx, ring->cidx, ring->num);
+}
+
+/* Returns number items free on ring. */
+static inline u32 mp_ring_num_empty(struct mp_ring *ring)
+{
+	ASSERT(ring);
+	return ring->num - 1 - mp_ring_num_pending(ring);
+}
+
+/* Consume 1 item */
+static inline void mp_ring_consume(struct mp_ring *ring)
+{
+	ASSERT(ring);
+	ASSERT(ring->pidx != ring->cidx);
+
+	ring->cidx = be_addc(ring->cidx, 1, ring->num);
+}
+
+/* Produce 1 item */
+static inline void mp_ring_produce(struct mp_ring *ring)
+{
+	ASSERT(ring);
+	ring->pidx = be_addc(ring->pidx, 1, ring->num);
+}
+
+/* Consume count items */
+static inline void mp_ring_consume_multiple(struct mp_ring *ring, u32 count)
+{
+	ASSERT(ring);
+	ASSERT(mp_ring_num_pending(ring) >= count);
+	ring->cidx = be_addc(ring->cidx, count, ring->num);
+}
+
+static inline void *mp_ring_item(struct mp_ring *ring, u32 index)
+{
+	ASSERT(ring);
+	ASSERT(index < ring->num);
+	ASSERT(ring->itemSize > 0);
+	return (u8 *) ring->va + index * ring->itemSize;
+}
+
+/* Ptr to produce item */
+static inline void *mp_ring_producer_ptr(struct mp_ring *ring)
+{
+	ASSERT(ring);
+	return mp_ring_item(ring, ring->pidx);
+}
+
+/*
+ * Returns a pointer to the current location in the ring.
+ * This is used for rings with 1 index.
+ */
+static inline void *mp_ring_current(struct mp_ring *ring)
+{
+	ASSERT(ring);
+	ASSERT(ring->pidx == 0);	/* not used */
+
+	return mp_ring_item(ring, ring->cidx);
+}
+
+/*
+ * Increment index for rings with only 1 index.
+ * This is used for rings with 1 index.
+ */
+static inline void *mp_ring_next(struct mp_ring *ring)
+{
+	ASSERT(ring);
+	ASSERT(ring->num > 0);
+	ASSERT(ring->pidx == 0);	/* not used */
+
+	ring->cidx = be_addc(ring->cidx, 1, ring->num);
+	return mp_ring_current(ring);
+}
+
+/*
+    This routine waits for a previously posted mailbox WRB to be completed.
+    Specifically it waits for the mailbox to say that it's ready to accept
+    more data by setting the LSB of the mailbox pd register to 1.
+
+    pcontroller      - The function object to post this data to
+
+    IRQL < DISPATCH_LEVEL
+*/
+static void be_mcc_mailbox_wait(struct be_function_object *pfob)
+{
+	struct MPU_MAILBOX_DB_AMAP mailbox_db;
+	u32 i = 0;
+	u32 ready;
+
+	if (pfob->emulate) {
+		/* No waiting for mailbox in emulated mode. */
+		return;
+	}
+
+	mailbox_db.dw[0] = PD_READ(pfob, mcc_bootstrap_db);
+	ready = AMAP_GET_BITS_PTR(MPU_MAILBOX_DB, ready, &mailbox_db);
+
+	while (ready == false) {
+		if ((++i & 0x3FFFF) == 0) {
+			TRACE(DL_WARN, "Waiting for mailbox ready - %dk polls",
+								i / 1000);
+		}
+		udelay(1);
+		mailbox_db.dw[0] = PD_READ(pfob, mcc_bootstrap_db);
+		ready = AMAP_GET_BITS_PTR(MPU_MAILBOX_DB, ready, &mailbox_db);
+	}
+}
+
+/*
+    This routine tells the MCC mailbox that there is data to processed
+    in the mailbox. It does this by setting the physical address for the
+    mailbox location and clearing the LSB.  This routine returns immediately
+    and does not wait for the WRB to be processed.
+
+    pcontroller      - The function object to post this data to
+
+    IRQL < DISPATCH_LEVEL
+
+*/
+static void be_mcc_mailbox_notify(struct be_function_object *pfob)
+{
+	struct MPU_MAILBOX_DB_AMAP mailbox_db;
+	u32 pa;
+
+	ASSERT(pfob->mailbox.pa);
+	ASSERT(pfob->mailbox.va);
+
+	/* If emulated, do not ring the mailbox */
+	if (pfob->emulate) {
+		TRACE(DL_WARN, "MPU disabled. Skipping mailbox notify.");
+		return;
+	}
+
+	/* form the higher bits in the address */
+	mailbox_db.dw[0] = 0;	/* init */
+	AMAP_SET_BITS_PTR(MPU_MAILBOX_DB, hi, &mailbox_db, 1);
+	AMAP_SET_BITS_PTR(MPU_MAILBOX_DB, ready, &mailbox_db, 0);
+
+	/* bits 34 to 63 */
+	pa = (u32) (pfob->mailbox.pa >> 34);
+	AMAP_SET_BITS_PTR(MPU_MAILBOX_DB, address, &mailbox_db, pa);
+
+	/* Wait for the MPU to be ready */
+	be_mcc_mailbox_wait(pfob);
+
+	/* Ring doorbell 1st time */
+	PD_WRITE(pfob, mcc_bootstrap_db, mailbox_db.dw[0]);
+
+	/* Wait for 1st write to be acknowledged. */
+	be_mcc_mailbox_wait(pfob);
+
+	/* lower bits 30 bits from 4th bit (bits 4 to 33)*/
+	pa = (u32) (pfob->mailbox.pa >> 4) & 0x3FFFFFFF;
+
+	AMAP_SET_BITS_PTR(MPU_MAILBOX_DB, hi, &mailbox_db, 0);
+	AMAP_SET_BITS_PTR(MPU_MAILBOX_DB, ready, &mailbox_db, 0);
+	AMAP_SET_BITS_PTR(MPU_MAILBOX_DB, address, &mailbox_db, pa);
+
+	/* Ring doorbell 2nd time */
+	PD_WRITE(pfob, mcc_bootstrap_db, mailbox_db.dw[0]);
+}
+
+/*
+    This routine tells the MCC mailbox that there is data to processed
+    in the mailbox. It does this by setting the physical address for the
+    mailbox location and clearing the LSB.  This routine spins until the
+    MPU writes a 1 into the LSB indicating that the data has been received
+    and is ready to be processed.
+
+    pcontroller      - The function object to post this data to
+
+    IRQL < DISPATCH_LEVEL
+*/
+static void
+be_mcc_mailbox_notify_and_wait(struct be_function_object *pfob)
+{
+	/*
+	 * Notify it
+	 */
+	be_mcc_mailbox_notify(pfob);
+	/*
+	 * Now wait for completion of WRB
+	 */
+	be_mcc_mailbox_wait(pfob);
+}
+
+void
+be_mcc_process_cqe(struct be_function_object *pfob,
+				struct MCC_CQ_ENTRY_AMAP *cqe)
+{
+	struct be_mcc_wrb_context *wrb_context = NULL;
+	u32 offset, status;
+	u8 *p;
+
+	ASSERT(cqe);
+	/*
+	 * A command completed.  Commands complete out-of-order.
+	 * Determine which command completed from the TAG.
+	 */
+	offset = offsetof(struct BE_MCC_CQ_ENTRY_AMAP, mcc_tag)/8;
+	p = (u8 *) cqe + offset;
+	wrb_context = (struct be_mcc_wrb_context *)(void *)(size_t)(*(u64 *)p);
+	ASSERT(wrb_context);
+
+	/*
+	 * Perform a response copy if requested.
+	 * Only copy data if the FWCMD is successful.
+	 */
+	status = AMAP_GET_BITS_PTR(MCC_CQ_ENTRY, completion_status, cqe);
+	if (status == MGMT_STATUS_SUCCESS && wrb_context->copy.length > 0) {
+		ASSERT(wrb_context->wrb);
+		ASSERT(wrb_context->copy.va);
+		p = (u8 *)wrb_context->wrb +
+				offsetof(struct BE_MCC_WRB_AMAP, payload)/8;
+		memcpy(wrb_context->copy.va,
+			  (u8 *)p + wrb_context->copy.fwcmd_offset,
+			  wrb_context->copy.length);
+	}
+
+	if (status)
+		status = BE_NOT_OK;
+	/* internal callback */
+	if (wrb_context->internal_cb) {
+		wrb_context->internal_cb(wrb_context->internal_cb_context,
+						status, wrb_context->wrb);
+	}
+
+	/* callback */
+	if (wrb_context->cb) {
+		wrb_context->cb(wrb_context->cb_context,
+					      status, wrb_context->wrb);
+	}
+	/* Free the context structure */
+	_be_mcc_free_wrb_context(pfob, wrb_context);
+}
+
+void be_drive_mcc_wrb_queue(struct be_mcc_object *mcc)
+{
+	struct be_function_object *pfob = NULL;
+	int status = BE_PENDING;
+	struct be_generic_q_ctxt *q_ctxt;
+	struct MCC_WRB_AMAP *wrb;
+	struct MCC_WRB_AMAP *queue_wrb;
+	u32 length, payload_length, sge_count, embedded;
+	unsigned long irql;
+
+	BUILD_BUG_ON((sizeof(struct be_generic_q_ctxt) <
+			  sizeof(struct be_queue_driver_context) +
+					sizeof(struct MCC_WRB_AMAP)));
+	pfob = mcc->parent_function;
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	if (mcc->driving_backlog) {
+		spin_unlock_irqrestore(&pfob->post_lock, irql);
+		if (pfob->pend_queue_driving && pfob->mcc) {
+			pfob->pend_queue_driving = 0;
+			be_drive_mcc_wrb_queue(pfob->mcc);
+		}
+		return;
+	}
+	/* Acquire the flag to limit 1 thread to redrive posts. */
+	mcc->driving_backlog = 1;
+
+	while (!list_empty(&mcc->backlog)) {
+		wrb = _be_mpu_peek_ring_wrb(mcc, true);	/* Driving the queue */
+		if (!wrb)
+			break;	/* No space in the ring yet. */
+		/* Get the next queued entry to process. */
+		q_ctxt = list_first_entry(&mcc->backlog,
+				struct be_generic_q_ctxt, context.list);
+		list_del(&q_ctxt->context.list);
+		pfob->mcc->backlog_length--;
+		/*
+		 * Compute the required length of the WRB.
+		 * Since the queue element may be smaller than
+		 * the complete WRB, copy only the required number of bytes.
+		 */
+		queue_wrb = (struct MCC_WRB_AMAP *) &q_ctxt->wrb_header;
+		embedded = AMAP_GET_BITS_PTR(MCC_WRB, embedded, queue_wrb);
+		if (embedded) {
+			payload_length = AMAP_GET_BITS_PTR(MCC_WRB,
+						payload_length, queue_wrb);
+			length = sizeof(struct be_mcc_wrb_header) +
+								payload_length;
+		} else {
+			sge_count = AMAP_GET_BITS_PTR(MCC_WRB, sge_count,
+								queue_wrb);
+			ASSERT(sge_count == 1); /* only 1 frag. */
+			length = sizeof(struct be_mcc_wrb_header) +
+			    sge_count * sizeof(struct MCC_SGE_AMAP);
+		}
+
+		/*
+		 * Truncate the length based on the size of the
+		 * queue element.  Some elements that have output parameters
+		 * can be smaller than the payload_length field would
+		 * indicate.  We really only need to copy the request
+		 * parameters, not the response.
+		 */
+		length = min(length, (u32) (q_ctxt->context.bytes -
+			offsetof(struct be_generic_q_ctxt, wrb_header)));
+
+		/* Copy the queue element WRB into the ring. */
+		memcpy(wrb, &q_ctxt->wrb_header, length);
+
+		/* Post the wrb.  This should not fail assuming we have
+		 * enough context structs. */
+		status = be_function_post_mcc_wrb(pfob, wrb, NULL,
+			   q_ctxt->context.cb, q_ctxt->context.cb_context,
+			   q_ctxt->context.internal_cb,
+			   q_ctxt->context.internal_cb_context,
+			   q_ctxt->context.optional_fwcmd_va,
+			   &q_ctxt->context.copy);
+
+		if (status == BE_SUCCESS) {
+			/*
+			 * Synchronous completion. Since it was queued,
+			 * we will invoke the callback.
+			 * To the user, this is an asynchronous request.
+			 */
+			spin_unlock_irqrestore(&pfob->post_lock, irql);
+			if (pfob->pend_queue_driving && pfob->mcc) {
+				pfob->pend_queue_driving = 0;
+				be_drive_mcc_wrb_queue(pfob->mcc);
+			}
+
+			ASSERT(q_ctxt->context.cb);
+
+			q_ctxt->context.cb(
+				q_ctxt->context.cb_context,
+						BE_SUCCESS, NULL);
+
+			spin_lock_irqsave(&pfob->post_lock, irql);
+
+		} else if (status != BE_PENDING) {
+			/*
+			 * Another resource failed.  Should never happen
+			 * if we have sufficient MCC_WRB_CONTEXT structs.
+			 * Return to head of the queue.
+			 */
+			TRACE(DL_WARN, "Failed to post a queued WRB. 0x%x",
+			      status);
+			list_add(&q_ctxt->context.list, &mcc->backlog);
+			pfob->mcc->backlog_length++;
+			break;
+		}
+	}
+
+	/* Free the flag to limit 1 thread to redrive posts. */
+	mcc->driving_backlog = 0;
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+}
+
+/* This function asserts that the WRB was consumed in order. */
+#ifdef BE_DEBUG
+u32 be_mcc_wrb_consumed_in_order(struct be_mcc_object *mcc,
+					struct MCC_CQ_ENTRY_AMAP *cqe)
+{
+	struct be_mcc_wrb_context *wrb_context = NULL;
+	u32 wrb_index;
+	u32 wrb_consumed_in_order;
+	u32 offset;
+	u8 *p;
+
+	ASSERT(cqe);
+	/*
+	 * A command completed.  Commands complete out-of-order.
+	 * Determine which command completed from the TAG.
+	 */
+	offset = offsetof(struct BE_MCC_CQ_ENTRY_AMAP, mcc_tag)/8;
+	p = (u8 *) cqe + offset;
+	wrb_context = (struct be_mcc_wrb_context *)(void *)(size_t)(*(u64 *)p);
+
+	ASSERT(wrb_context);
+
+	wrb_index = (u32) (((u64)(size_t)wrb_context->ring_wrb -
+		(u64)(size_t)mcc->sq.ring.va) / sizeof(struct MCC_WRB_AMAP));
+
+	ASSERT(wrb_index < mcc->sq.ring.num);
+
+	wrb_consumed_in_order = (u32) (wrb_index == mcc->consumed_index);
+	mcc->consumed_index = be_addc(mcc->consumed_index, 1, mcc->sq.ring.num);
+	return wrb_consumed_in_order;
+}
+#endif
+
+int be_mcc_process_cq(struct be_mcc_object *mcc, bool rearm)
+{
+	struct be_function_object *pfob = NULL;
+	struct MCC_CQ_ENTRY_AMAP *cqe;
+	struct CQ_DB_AMAP db;
+	struct mp_ring *cq_ring = &mcc->cq.ring;
+	struct mp_ring *mp_ring = &mcc->sq.ring;
+	u32 num_processed = 0;
+	u32 consumed = 0, valid, completed, cqe_consumed, async_event;
+
+	pfob = mcc->parent_function;
+
+	spin_lock_irqsave(&pfob->cq_lock, pfob->cq_irq);
+
+	/*
+	 * Verify that only one thread is processing the CQ at once.
+	 * We cannot hold the lock while processing the CQ due to
+	 * the callbacks into the OS.  Therefore, this flag is used
+	 * to control it.  If any of the threads want to
+	 * rearm the CQ, we need to honor that.
+	 */
+	if (mcc->processing != 0) {
+		mcc->rearm = mcc->rearm || rearm;
+		goto Error;
+	} else {
+		mcc->processing = 1;	/* lock processing for this thread. */
+		mcc->rearm = rearm;	/* set our rearm setting */
+	}
+
+	spin_unlock_irqrestore(&pfob->cq_lock, pfob->cq_irq);
+
+	cqe = mp_ring_current(cq_ring);
+	valid = AMAP_GET_BITS_PTR(MCC_CQ_ENTRY, valid, cqe);
+	while (valid) {
+
+		if (num_processed >= 8) {
+			/* coalesce doorbells, but free space in cq
+			 * ring while processing. */
+			db.dw[0] = 0;	/* clear */
+			AMAP_SET_BITS_PTR(CQ_DB, qid, &db, cq_ring->id);
+			AMAP_SET_BITS_PTR(CQ_DB, rearm, &db, false);
+			AMAP_SET_BITS_PTR(CQ_DB, event, &db, false);
+			AMAP_SET_BITS_PTR(CQ_DB, num_popped, &db,
+							num_processed);
+			num_processed = 0;
+
+			PD_WRITE(pfob, cq_db, db.dw[0]);
+		}
+
+		async_event = AMAP_GET_BITS_PTR(MCC_CQ_ENTRY, async_event, cqe);
+		if (async_event) {
+			/* This is an asynchronous event. */
+			struct ASYNC_EVENT_TRAILER_AMAP *async_trailer =
+			    (struct ASYNC_EVENT_TRAILER_AMAP *)
+			    ((u8 *) cqe + sizeof(struct MCC_CQ_ENTRY_AMAP) -
+			     sizeof(struct ASYNC_EVENT_TRAILER_AMAP));
+			u32 event_code;
+			async_event = AMAP_GET_BITS_PTR(ASYNC_EVENT_TRAILER,
+						async_event, async_trailer);
+			ASSERT(async_event == 1);
+
+
+			valid = AMAP_GET_BITS_PTR(ASYNC_EVENT_TRAILER,
+						valid, async_trailer);
+			ASSERT(valid == 1);
+
+			/* Call the async event handler if it is installed. */
+			if (mcc->async_cb) {
+				event_code =
+					AMAP_GET_BITS_PTR(ASYNC_EVENT_TRAILER,
+						event_code, async_trailer);
+				mcc->async_cb(mcc->async_context,
+					    (u32) event_code, (void *) cqe);
+			}
+
+		} else {
+			/* This is a completion entry. */
+
+			/* No vm forwarding in this driver. */
+
+			cqe_consumed = AMAP_GET_BITS_PTR(MCC_CQ_ENTRY,
+						consumed, cqe);
+			if (cqe_consumed) {
+				/*
+				 * A command on the MCC ring was consumed.
+				 * Update the consumer index.
+				 * These occur in order.
+				 */
+				ASSERT(be_mcc_wrb_consumed_in_order(mcc, cqe));
+				consumed++;
+			}
+
+			completed = AMAP_GET_BITS_PTR(MCC_CQ_ENTRY,
+					completed, cqe);
+			if (completed) {
+				/* A command completed.  Use tag to
+				 * determine which command.  */
+				be_mcc_process_cqe(pfob, cqe);
+			}
+		}
+
+		/* Reset the CQE */
+		AMAP_SET_BITS_PTR(MCC_CQ_ENTRY, valid, cqe, false);
+		num_processed++;
+
+		/* Update our tracking for the CQ ring. */
+		cqe = mp_ring_next(cq_ring);
+		valid = AMAP_GET_BITS_PTR(MCC_CQ_ENTRY, valid, cqe);
+	}
+
+	TRACE(DL_INFO, "num_processed:0x%x, and consumed:0x%x",
+	      num_processed, consumed);
+	/*
+	 * Grab the CQ lock to synchronize the "rearm" setting for
+	 * the doorbell, and for clearing the "processing" flag.
+	 */
+	spin_lock_irqsave(&pfob->cq_lock, pfob->cq_irq);
+
+	/*
+	 * Rearm the cq.  This is done based on the global mcc->rearm
+	 * flag which combines the rearm parameter from the current
+	 * call to process_cq and any other threads
+	 * that tried to process the CQ while this one was active.
+	 * This handles the situation where a sync. fwcmd was processing
+	 * the CQ while the interrupt/dpc tries to process it.
+	 * The sync process gets to continue -- but it is now
+	 * responsible for the rearming.
+	 */
+	if (num_processed > 0 || mcc->rearm == true) {
+		db.dw[0] = 0;	/* clear */
+		AMAP_SET_BITS_PTR(CQ_DB, qid, &db, cq_ring->id);
+		AMAP_SET_BITS_PTR(CQ_DB, rearm, &db, mcc->rearm);
+		AMAP_SET_BITS_PTR(CQ_DB, event, &db, false);
+		AMAP_SET_BITS_PTR(CQ_DB, num_popped, &db, num_processed);
+
+		PD_WRITE(pfob, cq_db, db.dw[0]);
+	}
+	/*
+	 * Update the consumer index after ringing the CQ doorbell.
+	 * We don't want another thread to post more WRBs before we
+	 * have CQ space available.
+	 */
+	mp_ring_consume_multiple(mp_ring, consumed);
+
+	/* Clear the processing flag. */
+	mcc->processing = 0;
+
+Error:
+	spin_unlock_irqrestore(&pfob->cq_lock, pfob->cq_irq);
+	/*
+	 * Use the local variable to detect if the current thread
+	 * holds the WRB post lock.  If rearm is false, this is
+	 * either a synchronous command, or the upper layer driver is polling
+	 * from a thread.  We do not drive the queue from that
+	 * context since the driver may hold the
+	 * wrb post lock already.
+	 */
+	if (rearm)
+		be_drive_mcc_wrb_queue(mcc);
+	else
+		pfob->pend_queue_driving = 1;
+
+	return BE_SUCCESS;
+}
+
+/*
+ *============================================================================
+ *                  P U B L I C  R O U T I N E S
+ *============================================================================
+ */
+
+/*
+    This routine creates an MCC object.  This object contains an MCC send queue
+    and a CQ private to the MCC.
+
+    pcontroller      - Handle to a function object
+
+    EqObject            - EQ object that will be used to dispatch this MCC
+
+    ppMccObject         - Pointer to an internal Mcc Object returned.
+
+    Returns BE_SUCCESS if successfull,, otherwise a useful error code
+	is returned.
+
+    IRQL < DISPATCH_LEVEL
+
+*/
+int
+be_mcc_ring_create(struct be_function_object *pfob,
+		   struct ring_desc *rd, u32 length,
+		   struct be_mcc_wrb_context *context_array,
+		   u32 num_context_entries,
+		   struct be_cq_object *cq, struct be_mcc_object *mcc)
+{
+	int status = 0;
+
+	struct FWCMD_COMMON_MCC_CREATE *fwcmd = NULL;
+	struct MCC_WRB_AMAP *wrb = NULL;
+	u32 num_entries_encoded, n, i;
+	void *va = NULL;
+	unsigned long irql;
+
+	if (length < sizeof(struct MCC_WRB_AMAP) * 2) {
+		TRACE(DL_ERR, "Invalid MCC ring length:%d", length);
+		return BE_NOT_OK;
+	}
+	/*
+	 * Reduce the actual ring size to be less than the number
+	 * of context entries.  This ensures that we run out of
+	 * ring WRBs first so the queuing works correctly.  We never
+	 * queue based on context structs.
+	 */
+	if (num_context_entries + 1 <
+			length / sizeof(struct MCC_WRB_AMAP) - 1) {
+
+		u32 max_length =
+		    (num_context_entries + 2) * sizeof(struct MCC_WRB_AMAP);
+
+		if (is_power_of_2(max_length))
+			length = __roundup_pow_of_two(max_length+1) / 2;
+		else
+			length = __roundup_pow_of_two(max_length) / 2;
+
+		ASSERT(length <= max_length);
+
+		TRACE(DL_WARN,
+			"MCC ring length reduced based on context entries."
+			" length:%d wrbs:%d context_entries:%d", length,
+			(int) (length / sizeof(struct MCC_WRB_AMAP)),
+			num_context_entries);
+	}
+
+	spin_lock_irqsave(&pfob->post_lock, irql);
+
+	num_entries_encoded =
+	    be_ring_length_to_encoding(length, sizeof(struct MCC_WRB_AMAP));
+
+	/* Init MCC object. */
+	memset(mcc, 0, sizeof(*mcc));
+	mcc->parent_function = pfob;
+	mcc->cq_object = cq;
+
+	INIT_LIST_HEAD(&mcc->backlog);
+
+	wrb = be_function_peek_mcc_wrb(pfob);
+	if (!wrb) {
+		ASSERT(wrb);
+		TRACE(DL_ERR, "No free MCC WRBs in create EQ.");
+		status = BE_STATUS_NO_MCC_WRB;
+		goto error;
+	}
+	/* Prepares an embedded fwcmd, including request/response sizes. */
+	fwcmd = BE_PREPARE_EMBEDDED_FWCMD(pfob, wrb, COMMON_MCC_CREATE);
+
+	fwcmd->params.request.num_pages = DIV_ROUND_UP(length, PAGE_SIZE);
+	/*
+	 * Program MCC ring context
+	 */
+	AMAP_SET_BITS_PTR(MCC_RING_CONTEXT, pdid,
+			&fwcmd->params.request.context, 0);
+	AMAP_SET_BITS_PTR(MCC_RING_CONTEXT, invalid,
+			&fwcmd->params.request.context, false);
+	AMAP_SET_BITS_PTR(MCC_RING_CONTEXT, ring_size,
+			&fwcmd->params.request.context, num_entries_encoded);
+
+	n = cq->cq_id;
+	AMAP_SET_BITS_PTR(MCC_RING_CONTEXT,
+				cq_id, &fwcmd->params.request.context, n);
+	be_rd_to_pa_list(rd, fwcmd->params.request.pages,
+				ARRAY_SIZE(fwcmd->params.request.pages));
+	/* Post the f/w command */
+	status = be_function_post_mcc_wrb(pfob, wrb, NULL, NULL, NULL,
+						NULL, NULL, fwcmd, NULL);
+	if (status != BE_SUCCESS) {
+		TRACE(DL_ERR, "MCC to create CQ failed.");
+		goto error;
+	}
+	/*
+	 * Create a linked list of context structures
+	 */
+	mcc->wrb_context.base = context_array;
+	mcc->wrb_context.num = num_context_entries;
+	INIT_LIST_HEAD(&mcc->wrb_context.list_head);
+	memset(context_array, 0,
+		    sizeof(struct be_mcc_wrb_context) * num_context_entries);
+	for (i = 0; i < mcc->wrb_context.num; i++) {
+		list_add_tail(&context_array[i].next,
+					&mcc->wrb_context.list_head);
+	}
+
+	/*
+	 *
+	 * Create an mcc_ring for tracking WRB hw ring
+	 */
+	va = rd->va;
+	ASSERT(va);
+	mp_ring_create(&mcc->sq.ring, length / sizeof(struct MCC_WRB_AMAP),
+				sizeof(struct MCC_WRB_AMAP), va);
+	mcc->sq.ring.id = fwcmd->params.response.id;
+	/*
+	 * Init a mcc_ring for tracking the MCC CQ.
+	 */
+	ASSERT(cq->va);
+	mp_ring_create(&mcc->cq.ring, cq->num_entries,
+		       sizeof(struct MCC_CQ_ENTRY_AMAP), cq->va);
+	mcc->cq.ring.id = cq->cq_id;
+
+	/* Force zeroing of CQ. */
+	memset(cq->va, 0, cq->num_entries * sizeof(struct MCC_CQ_ENTRY_AMAP));
+
+	/* Initialize debug index. */
+	mcc->consumed_index = 0;
+
+	atomic_inc(&cq->ref_count);
+	pfob->mcc = mcc;
+
+	TRACE(DL_INFO, "MCC ring created. id:%d bytes:%d cq_id:%d cq_entries:%d"
+	      " num_context:%d", mcc->sq.ring.id, length,
+	      cq->cq_id, cq->num_entries, num_context_entries);
+
+error:
+	spin_unlock_irqrestore(&pfob->post_lock, irql);
+	if (pfob->pend_queue_driving && pfob->mcc) {
+		pfob->pend_queue_driving = 0;
+		be_drive_mcc_wrb_queue(pfob->mcc);
+	}
+	return status;
+}
+
+/*
+    This routine destroys an MCC send queue
+
+    MccObject         - Internal Mcc Object to be destroyed.
+
+    Returns BE_SUCCESS if successfull, otherwise an error code is returned.
+
+    IRQL < DISPATCH_LEVEL
+
+    The caller of this routine must ensure that no other WRB may be posted
+    until this routine returns.
+
+*/
+int be_mcc_ring_destroy(struct be_mcc_object *mcc)
+{
+	int status = 0;
+	struct be_function_object *pfob = mcc->parent_function;
+
+
+	ASSERT(mcc->processing == 0);
+
+	/*
+	 * Remove the ring from the function object.
+	 * This transitions back to mailbox mode.
+	 */
+	pfob->mcc = NULL;
+
+	/* Send fwcmd to destroy the queue.  (Using the mailbox.) */
+	status = be_function_ring_destroy(mcc->parent_function, mcc->sq.ring.id,
+			     FWCMD_RING_TYPE_MCC, NULL, NULL, NULL, NULL);
+	ASSERT(status == 0);
+
+	/* Release the SQ reference to the CQ */
+	atomic_dec(&mcc->cq_object->ref_count);
+
+	return status;
+}
+
+static void
+mcc_wrb_sync_cb(void *context, int staus, struct MCC_WRB_AMAP *wrb)
+{
+	struct be_mcc_wrb_context *wrb_context =
+				(struct be_mcc_wrb_context *) context;
+	ASSERT(wrb_context);
+	*wrb_context->users_final_status = staus;
+}
+
+/*
+    This routine posts a command to the MCC send queue
+
+    mcc       - Internal Mcc Object to be destroyed.
+
+    wrb             - wrb to post.
+
+    Returns BE_SUCCESS if successfull, otherwise an error code is returned.
+
+    IRQL < DISPATCH_LEVEL if CompletionCallback is not NULL
+    IRQL <=DISPATCH_LEVEL if CompletionCallback is  NULL
+
+    If this routine is called with CompletionCallback != NULL the
+    call is considered to be asynchronous and will return as soon
+    as the WRB is posted to the MCC with BE_PENDING.
+
+    If CompletionCallback is NULL, then this routine will not return until
+    a completion for this MCC command has been processed.
+    If called at DISPATCH_LEVEL the CompletionCallback must be NULL.
+
+    This routine should only be called if the MPU has been boostraped past
+    mailbox mode.
+
+
+*/
+int
+_be_mpu_post_wrb_ring(struct be_mcc_object *mcc, struct MCC_WRB_AMAP *wrb,
+				struct be_mcc_wrb_context *wrb_context)
+{
+
+	struct MCC_WRB_AMAP *ring_wrb = NULL;
+	int status = BE_PENDING;
+	int final_status = BE_PENDING;
+	mcc_wrb_cqe_callback cb = NULL;
+	struct MCC_DB_AMAP mcc_db;
+	u32 embedded;
+
+	ASSERT(mp_ring_num_empty(&mcc->sq.ring) > 0);
+	/*
+	 * Input wrb is most likely the next wrb in the ring, since the client
+	 * can peek at the address.
+	 */
+	ring_wrb = mp_ring_producer_ptr(&mcc->sq.ring);
+	if (wrb != ring_wrb) {
+		/* If not equal, copy it into the ring. */
+		memcpy(ring_wrb, wrb, sizeof(struct MCC_WRB_AMAP));
+	}
+#ifdef BE_DEBUG
+	wrb_context->ring_wrb = ring_wrb;
+#endif
+	embedded = AMAP_GET_BITS_PTR(MCC_WRB, embedded, ring_wrb);
+	if (embedded) {
+		/* embedded commands will have the response within the WRB. */
+		wrb_context->wrb = ring_wrb;
+	} else {
+		/*
+		 * non-embedded commands will not have the response
+		 * within the WRB, and they may complete out-of-order.
+		 * The WRB will not be valid to inspect
+		 * during the completion.
+		 */
+		wrb_context->wrb = NULL;
+	}
+	cb = wrb_context->cb;
+
+	if (cb == NULL) {
+		/* Assign our internal callback if this is a
+		 * synchronous call. */
+		wrb_context->cb = mcc_wrb_sync_cb;
+		wrb_context->cb_context = wrb_context;
+		wrb_context->users_final_status = &final_status;
+	}
+	/* Increment producer index */
+
+	mcc_db.dw[0] = 0;		/* initialize */
+	AMAP_SET_BITS_PTR(MCC_DB, rid, &mcc_db, mcc->sq.ring.id);
+	AMAP_SET_BITS_PTR(MCC_DB, numPosted, &mcc_db, 1);
+
+	mp_ring_produce(&mcc->sq.ring);
+	PD_WRITE(mcc->parent_function, mpu_mcc_db, mcc_db.dw[0]);
+	TRACE(DL_INFO, "pidx: %x and cidx: %x.", mcc->sq.ring.pidx,
+	      mcc->sq.ring.cidx);
+
+	if (cb == NULL) {
+		int polls = 0;	/* At >= 1 us per poll   */
+		/* Wait until this command completes, polling the CQ. */
+		do {
+			TRACE(DL_INFO, "FWCMD submitted in the poll mode.");
+			/* Do not rearm CQ in this context. */
+			be_mcc_process_cq(mcc, false);
+
+			if (final_status == BE_PENDING) {
+				if ((++polls & 0x7FFFF) == 0) {
+					TRACE(DL_WARN,
+					      "Warning : polling MCC CQ for %d"
+					      "ms.", polls / 1000);
+				}
+
+				udelay(1);
+			}
+
+			/* final_status changed when the command completes */
+		} while (final_status == BE_PENDING);
+
+		status = final_status;
+	}
+
+	return status;
+}
+
+struct MCC_WRB_AMAP *
+_be_mpu_peek_ring_wrb(struct be_mcc_object *mcc, bool driving_queue)
+{
+	/* If we have queued items, do not allow a post to bypass the queue. */
+	if (!driving_queue && !list_empty(&mcc->backlog))
+		return NULL;
+
+	if (mp_ring_num_empty(&mcc->sq.ring) <= 0)
+		return NULL;
+	return (struct MCC_WRB_AMAP *) mp_ring_producer_ptr(&mcc->sq.ring);
+}
+
+int
+be_mpu_init_mailbox(struct be_function_object *pfob, struct ring_desc *mailbox)
+{
+	ASSERT(mailbox);
+	pfob->mailbox.va = mailbox->va;
+	pfob->mailbox.pa =  cpu_to_le64(mailbox->pa);
+	pfob->mailbox.length = mailbox->length;
+
+	ASSERT(((u32)(size_t)pfob->mailbox.va & 0xf) == 0);
+	ASSERT(((u32)(size_t)pfob->mailbox.pa & 0xf) == 0);
+	/*
+	 * Issue the WRB to set MPU endianness
+	 */
+	{
+		u64 *endian_check = (u64 *) (pfob->mailbox.va +
+				offsetof(struct BE_MCC_MAILBOX_AMAP, wrb)/8);
+		*endian_check = 0xFF1234FFFF5678FFULL;
+	}
+
+	be_mcc_mailbox_notify_and_wait(pfob);
+
+	return BE_SUCCESS;
+}
+
+
+/*
+    This routine posts a command to the MCC mailbox.
+
+    FuncObj         - Function Object to post the WRB on behalf of.
+    wrb             - wrb to post.
+    CompletionCallback  - Address of a callback routine to invoke once the WRB
+				is completed.
+    CompletionCallbackContext - Opaque context to be passed during the call to
+				the CompletionCallback.
+    Returns BE_SUCCESS if successfull, otherwise an error code is returned.
+
+    IRQL <=DISPATCH_LEVEL if CompletionCallback is  NULL
+
+    This routine will block until a completion for this MCC command has been
+    processed. If called at DISPATCH_LEVEL the CompletionCallback must be NULL.
+
+    This routine should only be called if the MPU has not been boostraped past
+    mailbox mode.
+*/
+int
+_be_mpu_post_wrb_mailbox(struct be_function_object *pfob,
+	 struct MCC_WRB_AMAP *wrb, struct be_mcc_wrb_context *wrb_context)
+{
+	struct MCC_MAILBOX_AMAP *mailbox = NULL;
+	struct MCC_WRB_AMAP *mb_wrb;
+	struct MCC_CQ_ENTRY_AMAP *mb_cq;
+	u32 offset, status;
+
+	ASSERT(pfob->mcc == NULL);
+	mailbox = pfob->mailbox.va;
+	ASSERT(mailbox);
+
+	offset = offsetof(struct BE_MCC_MAILBOX_AMAP, wrb)/8;
+	mb_wrb = (struct MCC_WRB_AMAP *) (u8 *)mailbox + offset;
+	if (mb_wrb != wrb) {
+		memset(mailbox, 0, sizeof(*mailbox));
+		memcpy(mb_wrb, wrb, sizeof(struct MCC_WRB_AMAP));
+	}
+	/* The callback can inspect the final WRB to get output parameters. */
+	wrb_context->wrb = mb_wrb;
+
+	be_mcc_mailbox_notify_and_wait(pfob);
+
+	/* A command completed.  Use tag to determine which command. */
+	offset = offsetof(struct BE_MCC_MAILBOX_AMAP, cq)/8;
+	mb_cq = (struct MCC_CQ_ENTRY_AMAP *) ((u8 *)mailbox + offset);
+	be_mcc_process_cqe(pfob, mb_cq);
+
+	status = AMAP_GET_BITS_PTR(MCC_CQ_ENTRY, completion_status, mb_cq);
+	if (status)
+		status = BE_NOT_OK;
+	return status;
+}
+
+struct be_mcc_wrb_context *
+_be_mcc_allocate_wrb_context(struct be_function_object *pfob)
+{
+	struct be_mcc_wrb_context *context = NULL;
+	unsigned long irq;
+
+	spin_lock_irqsave(&pfob->mcc_context_lock, irq);
+
+	if (!pfob->mailbox.default_context_allocated) {
+		/* Use the single default context that we
+		 * always have allocated. */
+		pfob->mailbox.default_context_allocated = true;
+		context = &pfob->mailbox.default_context;
+	} else if (pfob->mcc) {
+		/* Get a context from the free list. If any are available. */
+		if (!list_empty(&pfob->mcc->wrb_context.list_head)) {
+			context = list_first_entry(
+				&pfob->mcc->wrb_context.list_head,
+					 struct be_mcc_wrb_context, next);
+		}
+	}
+
+	spin_unlock_irqrestore(&pfob->mcc_context_lock, irq);
+
+	return context;
+}
+
+void
+_be_mcc_free_wrb_context(struct be_function_object *pfob,
+			 struct be_mcc_wrb_context *context)
+{
+	unsigned long irq;
+
+	ASSERT(context);
+	/*
+	 * Zero during free to try and catch any bugs where the context
+	 * is accessed after a free.
+	 */
+	memset(context, 0, sizeof(context));
+
+	spin_lock_irqsave(&pfob->mcc_context_lock, irq);
+
+	if (context == &pfob->mailbox.default_context) {
+		/* Free the default context. */
+		ASSERT(pfob->mailbox.default_context_allocated);
+		pfob->mailbox.default_context_allocated = false;
+	} else {
+		/* Add to free list. */
+		ASSERT(pfob->mcc);
+		list_add_tail(&context->next,
+				&pfob->mcc->wrb_context.list_head);
+	}
+
+	spin_unlock_irqrestore(&pfob->mcc_context_lock, irq);
+}
+
+int
+be_mcc_add_async_event_callback(struct be_mcc_object *mcc_object,
+		mcc_async_event_callback cb, void *cb_context)
+{
+	/* Lock against anyone trying to change the callback/context pointers
+	 * while being used. */
+	spin_lock_irqsave(&mcc_object->parent_function->cq_lock,
+		mcc_object->parent_function->cq_irq);
+
+	/* Assign the async callback. */
+	mcc_object->async_context = cb_context;
+	mcc_object->async_cb = cb;
+
+	spin_unlock_irqrestore(&mcc_object->parent_function->cq_lock,
+					mcc_object->parent_function->cq_irq);
+
+	return BE_SUCCESS;
+}
+
+#define MPU_EP_CONTROL 0
+#define MPU_EP_SEMAPHORE 0xac
+
+/*
+ *-------------------------------------------------------------------
+ * Function: be_wait_for_POST_complete
+ *   Waits until the BladeEngine POST completes (either in error or success).
+ * pfob -
+ * return status   - BE_SUCCESS (0) on success. Negative error code on failure.
+ *-------------------------------------------------------------------
+ */
+static int be_wait_for_POST_complete(struct be_function_object *pfob)
+{
+	struct MGMT_HBA_POST_STATUS_STRUCT_AMAP status;
+	int s;
+	u32 post_error, post_stage;
+
+	const u32 us_per_loop = 1000;	/* 1000us */
+	const u32 print_frequency_loops = 1000000 / us_per_loop;
+	const u32 max_loops = 60 * print_frequency_loops;
+	u32 loops = 0;
+
+	/*
+	 * Wait for arm fw indicating it is done or a fatal error happened.
+	 * Note: POST can take some time to complete depending on configuration
+	 * settings (consider ARM attempts to acquire an IP address
+	 * over DHCP!!!).
+	 *
+	 */
+	do {
+		status.dw[0] = ioread32(pfob->csr_va + MPU_EP_SEMAPHORE);
+		post_error = AMAP_GET_BITS_PTR(MGMT_HBA_POST_STATUS_STRUCT,
+						error, &status);
+		post_stage = AMAP_GET_BITS_PTR(MGMT_HBA_POST_STATUS_STRUCT,
+						stage, &status);
+		if (0 == (loops % print_frequency_loops)) {
+			/* Print current status */
+			TRACE(DL_INFO, "POST status = 0x%x (stage = 0x%x)",
+				status.dw[0], post_stage);
+		}
+		udelay(us_per_loop);
+	} while ((post_error != 1) &&
+		 (post_stage != POST_STAGE_ARMFW_READY) &&
+		 (++loops < max_loops));
+
+	if (post_error == 1) {
+		TRACE(DL_ERR, "POST error! Status = 0x%x (stage = 0x%x)",
+		      status.dw[0], post_stage);
+		s = BE_NOT_OK;
+	} else if (post_stage != POST_STAGE_ARMFW_READY) {
+		TRACE(DL_ERR, "POST time-out! Status = 0x%x (stage = 0x%x)",
+		      status.dw[0], post_stage);
+		s = BE_NOT_OK;
+	} else {
+		s = BE_SUCCESS;
+	}
+	return s;
+}
+
+/*
+ *-------------------------------------------------------------------
+ * Function: be_kickoff_and_wait_for_POST
+ *   Interacts with the BladeEngine management processor to initiate POST, and
+ *   subsequently waits until POST completes (either in error or success).
+ *   The caller must acquire the reset semaphore before initiating POST
+ *   to prevent multiple drivers interacting with the management processor.
+ *   Once POST is complete the caller must release the reset semaphore.
+ *   Callers who only want to wait for POST complete may call
+ *   be_wait_for_POST_complete.
+ * pfob -
+ * return status   - BE_SUCCESS (0) on success. Negative error code on failure.
+ *-------------------------------------------------------------------
+ */
+static int
+be_kickoff_and_wait_for_POST(struct be_function_object *pfob)
+{
+	struct MGMT_HBA_POST_STATUS_STRUCT_AMAP status;
+	int s;
+
+	const u32 us_per_loop = 1000;	/* 1000us */
+	const u32 print_frequency_loops = 1000000 / us_per_loop;
+	const u32 max_loops = 5 * print_frequency_loops;
+	u32 loops = 0;
+	u32 post_error, post_stage;
+
+	/* Wait for arm fw awaiting host ready or a fatal error happened. */
+	TRACE(DL_INFO, "Wait for BladeEngine ready to POST");
+	do {
+		status.dw[0] = ioread32(pfob->csr_va + MPU_EP_SEMAPHORE);
+		post_error = AMAP_GET_BITS_PTR(MGMT_HBA_POST_STATUS_STRUCT,
+						error, &status);
+		post_stage = AMAP_GET_BITS_PTR(MGMT_HBA_POST_STATUS_STRUCT,
+						stage, &status);
+		if (0 == (loops % print_frequency_loops)) {
+			/* Print current status */
+			TRACE(DL_INFO, "POST status = 0x%x (stage = 0x%x)",
+			      status.dw[0], post_stage);
+		}
+		udelay(us_per_loop);
+	} while ((post_error != 1) &&
+		 (post_stage < POST_STAGE_AWAITING_HOST_RDY) &&
+		 (++loops < max_loops));
+
+	if (post_error == 1) {
+		TRACE(DL_ERR, "Pre-POST error! Status = 0x%x (stage = 0x%x)",
+		      status.dw[0], post_stage);
+		s = BE_NOT_OK;
+	} else if (post_stage == POST_STAGE_AWAITING_HOST_RDY) {
+		iowrite32(POST_STAGE_HOST_RDY, pfob->csr_va + MPU_EP_SEMAPHORE);
+
+		/* Wait for POST to complete */
+		s = be_wait_for_POST_complete(pfob);
+	} else {
+		/*
+		 * Either a timeout waiting for host ready signal or POST has
+		 * moved ahead without requiring a host ready signal.
+		 * Might as well give POST a chance to complete
+		 * (or timeout again).
+		 */
+		s = be_wait_for_POST_complete(pfob);
+	}
+	return s;
+}
+
+/*
+ *-------------------------------------------------------------------
+ * Function: be_pci_soft_reset
+ *   This function is called to issue a BladeEngine soft reset.
+ *   Callers should acquire the soft reset semaphore before calling this
+ *   function. Additionaly, callers should ensure they cannot be pre-empted
+ *   while the routine executes. Upon completion of this routine, callers
+ *   should release the reset semaphore. This routine implicitly waits
+ *   for BladeEngine POST to complete.
+ * pfob -
+ * return status   - BE_SUCCESS (0) on success. Negative error code on failure.
+ *-------------------------------------------------------------------
+ */
+int be_pci_soft_reset(struct be_function_object *pfob)
+{
+	struct PCICFG_SOFT_RESET_CSR_AMAP soft_reset;
+	struct PCICFG_ONLINE0_CSR_AMAP pciOnline0;
+	struct PCICFG_ONLINE1_CSR_AMAP pciOnline1;
+	struct EP_CONTROL_CSR_AMAP epControlCsr;
+	int status = BE_SUCCESS;
+	u32 i, soft_reset_bit;
+
+	TRACE(DL_NOTE, "PCI reset...");
+
+	/* Issue soft reset #1 to get BladeEngine into a known state. */
+	soft_reset.dw[0] = PCICFG0_READ(pfob, soft_reset);
+	AMAP_SET_BITS_PTR(PCICFG_SOFT_RESET_CSR, softreset, soft_reset.dw, 1);
+	PCICFG0_WRITE(pfob, host_timer_int_ctrl, soft_reset.dw[0]);
+	/*
+	 * wait til soft reset is deasserted - hardware
+	 * deasserts after some time.
+	 */
+	i = 0;
+	do {
+		udelay(50);
+		soft_reset.dw[0] = PCICFG0_READ(pfob, soft_reset);
+		soft_reset_bit = AMAP_GET_BITS_PTR(PCICFG_SOFT_RESET_CSR,
+					softreset, soft_reset.dw);
+	} while (soft_reset_bit  && (i++ < 1024));
+	if (soft_reset_bit != 0) {
+		TRACE(DL_ERR, "Soft-reset #1 did not deassert as expected.");
+		status = BE_NOT_OK;
+		goto Error_label;
+	}
+	/* Mask everything  */
+	PCICFG0_WRITE(pfob, ue_status_low_mask, 0xFFFFFFFF);
+	PCICFG0_WRITE(pfob, ue_status_hi_mask, 0xFFFFFFFF);
+	/*
+	 * Set everything offline except MPU IRAM (it is offline with
+	 * the soft-reset, but soft-reset does not reset the PCICFG registers!)
+	 */
+	pciOnline0.dw[0] = 0;
+	pciOnline1.dw[0] = 0;
+	AMAP_SET_BITS_PTR(PCICFG_ONLINE1_CSR, mpu_iram_online,
+				pciOnline1.dw, 1);
+	PCICFG0_WRITE(pfob, online0, pciOnline0.dw[0]);
+	PCICFG0_WRITE(pfob, online1, pciOnline1.dw[0]);
+
+	udelay(20000);
+
+	/* Issue soft reset #2. */
+	AMAP_SET_BITS_PTR(PCICFG_SOFT_RESET_CSR, softreset, soft_reset.dw, 1);
+	PCICFG0_WRITE(pfob, host_timer_int_ctrl, soft_reset.dw[0]);
+	/*
+	 * wait til soft reset is deasserted - hardware
+	 * deasserts after some time.
+	 */
+	i = 0;
+	do {
+		udelay(50);
+		soft_reset.dw[0] = PCICFG0_READ(pfob, soft_reset);
+		soft_reset_bit = AMAP_GET_BITS_PTR(PCICFG_SOFT_RESET_CSR,
+					softreset, soft_reset.dw);
+	} while (soft_reset_bit  && (i++ < 1024));
+	if (soft_reset_bit != 0) {
+		TRACE(DL_ERR, "Soft-reset #1 did not deassert as expected.");
+		status = BE_NOT_OK;
+		goto Error_label;
+	}
+
+
+	udelay(20000);
+
+	/* Take MPU out of reset. */
+
+	epControlCsr.dw[0] = ioread32(pfob->csr_va + MPU_EP_CONTROL);
+	AMAP_SET_BITS_PTR(EP_CONTROL_CSR, CPU_reset, &epControlCsr, 0);
+	iowrite32((u32)epControlCsr.dw[0], pfob->csr_va + MPU_EP_CONTROL);
+
+	/* Kickoff BE POST and wait for completion */
+	status = be_kickoff_and_wait_for_POST(pfob);
+
+Error_label:
+	return status;
+}
+
+
+/*
+ *-------------------------------------------------------------------
+ * Function: be_pci_reset_required
+ *   This private function is called to detect if a host entity is
+ *   required to issue a PCI soft reset and subsequently drive
+ *   BladeEngine POST. Scenarios where this is required:
+ *   1) BIOS-less configuration
+ *   2) Hot-swap/plug/power-on
+ * pfob -
+ * return   true if a reset is required, false otherwise
+ *-------------------------------------------------------------------
+ */
+static bool be_pci_reset_required(struct be_function_object *pfob)
+{
+	struct MGMT_HBA_POST_STATUS_STRUCT_AMAP status;
+	bool do_reset = false;
+	u32 post_error, post_stage;
+
+	/*
+	 * Read the POST status register
+	 */
+	status.dw[0] = ioread32(pfob->csr_va + MPU_EP_SEMAPHORE);
+	post_error = AMAP_GET_BITS_PTR(MGMT_HBA_POST_STATUS_STRUCT, error,
+								&status);
+	post_stage = AMAP_GET_BITS_PTR(MGMT_HBA_POST_STATUS_STRUCT, stage,
+								&status);
+	if (post_stage <= POST_STAGE_AWAITING_HOST_RDY) {
+		/*
+		 * If BladeEngine is waiting for host ready indication,
+		 * we want to do a PCI reset.
+		 */
+		do_reset = true;
+	}
+
+	return do_reset;
+}
+
+/*
+ *-------------------------------------------------------------------
+ * Function: be_drive_POST
+ *   This function is called to drive BladeEngine POST. The
+ *   caller should ensure they cannot be pre-empted while this routine executes.
+ * pfob -
+ * return status   - BE_SUCCESS (0) on success. Negative error code on failure.
+ *-------------------------------------------------------------------
+ */
+int be_drive_POST(struct be_function_object *pfob)
+{
+	int status;
+
+	if (false != be_pci_reset_required(pfob)) {
+		/* PCI reset is needed (implicitly starts and waits for POST) */
+		status = be_pci_soft_reset(pfob);
+	} else {
+		/* No PCI reset is needed, start POST */
+		status = be_kickoff_and_wait_for_POST(pfob);
+	}
+
+	return status;
+}
diff --git a/drivers/staging/benet/mpu.h b/drivers/staging/benet/mpu.h
new file mode 100644
index 00000000000..41f3f87516e
--- /dev/null
+++ b/drivers/staging/benet/mpu.h
@@ -0,0 +1,74 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __mpu_amap_h__
+#define __mpu_amap_h__
+#include "ep.h"
+
+/* Provide control parameters for the Managment Processor Unit. */
+struct BE_MPU_CSRMAP_AMAP {
+	struct BE_EP_CSRMAP_AMAP ep;
+	u8 rsvd0[128];	/* DWORD 64 */
+	u8 rsvd1[32];	/* DWORD 68 */
+	u8 rsvd2[192];	/* DWORD 69 */
+	u8 rsvd3[192];	/* DWORD 75 */
+	u8 rsvd4[32];	/* DWORD 81 */
+	u8 rsvd5[32];	/* DWORD 82 */
+	u8 rsvd6[32];	/* DWORD 83 */
+	u8 rsvd7[32];	/* DWORD 84 */
+	u8 rsvd8[32];	/* DWORD 85 */
+	u8 rsvd9[32];	/* DWORD 86 */
+	u8 rsvd10[32];	/* DWORD 87 */
+	u8 rsvd11[32];	/* DWORD 88 */
+	u8 rsvd12[32];	/* DWORD 89 */
+	u8 rsvd13[32];	/* DWORD 90 */
+	u8 rsvd14[32];	/* DWORD 91 */
+	u8 rsvd15[32];	/* DWORD 92 */
+	u8 rsvd16[32];	/* DWORD 93 */
+	u8 rsvd17[32];	/* DWORD 94 */
+	u8 rsvd18[32];	/* DWORD 95 */
+	u8 rsvd19[32];	/* DWORD 96 */
+	u8 rsvd20[32];	/* DWORD 97 */
+	u8 rsvd21[32];	/* DWORD 98 */
+	u8 rsvd22[32];	/* DWORD 99 */
+	u8 rsvd23[32];	/* DWORD 100 */
+	u8 rsvd24[32];	/* DWORD 101 */
+	u8 rsvd25[32];	/* DWORD 102 */
+	u8 rsvd26[32];	/* DWORD 103 */
+	u8 rsvd27[32];	/* DWORD 104 */
+	u8 rsvd28[96];	/* DWORD 105 */
+	u8 rsvd29[32];	/* DWORD 108 */
+	u8 rsvd30[32];	/* DWORD 109 */
+	u8 rsvd31[32];	/* DWORD 110 */
+	u8 rsvd32[32];	/* DWORD 111 */
+	u8 rsvd33[32];	/* DWORD 112 */
+	u8 rsvd34[96];	/* DWORD 113 */
+	u8 rsvd35[32];	/* DWORD 116 */
+	u8 rsvd36[32];	/* DWORD 117 */
+	u8 rsvd37[32];	/* DWORD 118 */
+	u8 rsvd38[32];	/* DWORD 119 */
+	u8 rsvd39[32];	/* DWORD 120 */
+	u8 rsvd40[32];	/* DWORD 121 */
+	u8 rsvd41[134][32];	/* DWORD 122 */
+} __packed;
+struct MPU_CSRMAP_AMAP {
+	u32 dw[256];
+};
+
+#endif /* __mpu_amap_h__ */
diff --git a/drivers/staging/benet/mpu_context.h b/drivers/staging/benet/mpu_context.h
new file mode 100644
index 00000000000..8ce90f9c46c
--- /dev/null
+++ b/drivers/staging/benet/mpu_context.h
@@ -0,0 +1,46 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __mpu_context_amap_h__
+#define __mpu_context_amap_h__
+
+/*
+ * Management command and control ring context. The MPUs BTLR_CTRL1 CSR
+ * controls the writeback behavior of the producer and consumer index values.
+ */
+struct BE_MCC_RING_CONTEXT_AMAP {
+	u8 con_index[16];	/* DWORD 0 */
+	u8 ring_size[4];	/* DWORD 0 */
+	u8 cq_id[11];	/* DWORD 0 */
+	u8 rsvd0;		/* DWORD 0 */
+	u8 prod_index[16];	/* DWORD 1 */
+	u8 pdid[15];	/* DWORD 1 */
+	u8 invalid;		/* DWORD 1 */
+	u8 cmd_pending_current[7];	/* DWORD 2 */
+	u8 rsvd1[25];	/* DWORD 2 */
+	u8 hpi_port_cq_id[11];	/* DWORD 3 */
+	u8 rsvd2[5];	/* DWORD 3 */
+	u8 cmd_pending_max[7];	/* DWORD 3 */
+	u8 rsvd3[9];	/* DWORD 3 */
+} __packed;
+struct MCC_RING_CONTEXT_AMAP {
+	u32 dw[4];
+};
+
+#endif /* __mpu_context_amap_h__ */
diff --git a/drivers/staging/benet/pcicfg.h b/drivers/staging/benet/pcicfg.h
new file mode 100644
index 00000000000..7c15684adf4
--- /dev/null
+++ b/drivers/staging/benet/pcicfg.h
@@ -0,0 +1,825 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __pcicfg_amap_h__
+#define __pcicfg_amap_h__
+
+/* Vendor and Device ID Register. */
+struct BE_PCICFG_ID_CSR_AMAP {
+	u8 vendorid[16];	/* DWORD 0 */
+	u8 deviceid[16];	/* DWORD 0 */
+} __packed;
+struct PCICFG_ID_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* IO Bar Register. */
+struct BE_PCICFG_IOBAR_CSR_AMAP {
+	u8 iospace;		/* DWORD 0 */
+	u8 rsvd0[7];	/* DWORD 0 */
+	u8 iobar[24];	/* DWORD 0 */
+} __packed;
+struct PCICFG_IOBAR_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Memory BAR 0 Register. */
+struct BE_PCICFG_MEMBAR0_CSR_AMAP {
+	u8 memspace;	/* DWORD 0 */
+	u8 type[2];		/* DWORD 0 */
+	u8 pf;		/* DWORD 0 */
+	u8 rsvd0[10];	/* DWORD 0 */
+	u8 membar0[18];	/* DWORD 0 */
+} __packed;
+struct PCICFG_MEMBAR0_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Memory BAR 1 - Low Address Register. */
+struct BE_PCICFG_MEMBAR1_LO_CSR_AMAP {
+	u8 memspace;	/* DWORD 0 */
+	u8 type[2];		/* DWORD 0 */
+	u8 pf;		/* DWORD 0 */
+	u8 rsvd0[13];	/* DWORD 0 */
+	u8 membar1lo[15];	/* DWORD 0 */
+} __packed;
+struct PCICFG_MEMBAR1_LO_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Memory BAR 1 - High Address Register. */
+struct BE_PCICFG_MEMBAR1_HI_CSR_AMAP {
+	u8 membar1hi[32];	/* DWORD 0 */
+} __packed;
+struct PCICFG_MEMBAR1_HI_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Memory BAR 2 - Low Address Register. */
+struct BE_PCICFG_MEMBAR2_LO_CSR_AMAP {
+	u8 memspace;	/* DWORD 0 */
+	u8 type[2];		/* DWORD 0 */
+	u8 pf;		/* DWORD 0 */
+	u8 rsvd0[17];	/* DWORD 0 */
+	u8 membar2lo[11];	/* DWORD 0 */
+} __packed;
+struct PCICFG_MEMBAR2_LO_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Memory BAR 2 - High Address Register. */
+struct BE_PCICFG_MEMBAR2_HI_CSR_AMAP {
+	u8 membar2hi[32];	/* DWORD 0 */
+} __packed;
+struct PCICFG_MEMBAR2_HI_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Subsystem Vendor and ID (Function 0) Register. */
+struct BE_PCICFG_SUBSYSTEM_ID_F0_CSR_AMAP {
+	u8 subsys_vendor_id[16];	/* DWORD 0 */
+	u8 subsys_id[16];	/* DWORD 0 */
+} __packed;
+struct PCICFG_SUBSYSTEM_ID_F0_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Subsystem Vendor and ID (Function 1) Register. */
+struct BE_PCICFG_SUBSYSTEM_ID_F1_CSR_AMAP {
+	u8 subsys_vendor_id[16];	/* DWORD 0 */
+	u8 subsys_id[16];	/* DWORD 0 */
+} __packed;
+struct PCICFG_SUBSYSTEM_ID_F1_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Semaphore Register. */
+struct BE_PCICFG_SEMAPHORE_CSR_AMAP {
+	u8 locked;		/* DWORD 0 */
+	u8 rsvd0[31];	/* DWORD 0 */
+} __packed;
+struct PCICFG_SEMAPHORE_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Soft Reset Register. */
+struct BE_PCICFG_SOFT_RESET_CSR_AMAP {
+	u8 rsvd0[7];	/* DWORD 0 */
+	u8 softreset;	/* DWORD 0 */
+	u8 rsvd1[16];	/* DWORD 0 */
+	u8 nec_ll_rcvdetect_i[8];	/* DWORD 0 */
+} __packed;
+struct PCICFG_SOFT_RESET_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Unrecoverable Error Status (Low) Register. Each bit corresponds to
+ * an internal Unrecoverable Error.  These are set by hardware and may be
+ * cleared by writing a one to the respective bit(s) to be cleared.  Any
+ * bit being set that is also unmasked will result in Unrecoverable Error
+ * interrupt notification to the host CPU and/or Server Management chip
+ * and the transitioning of BladeEngine to an Offline state.
+ */
+struct BE_PCICFG_UE_STATUS_LOW_CSR_AMAP {
+	u8 cev_ue_status;	/* DWORD 0 */
+	u8 ctx_ue_status;	/* DWORD 0 */
+	u8 dbuf_ue_status;	/* DWORD 0 */
+	u8 erx_ue_status;	/* DWORD 0 */
+	u8 host_ue_status;	/* DWORD 0 */
+	u8 mpu_ue_status;	/* DWORD 0 */
+	u8 ndma_ue_status;	/* DWORD 0 */
+	u8 ptc_ue_status;	/* DWORD 0 */
+	u8 rdma_ue_status;	/* DWORD 0 */
+	u8 rxf_ue_status;	/* DWORD 0 */
+	u8 rxips_ue_status;	/* DWORD 0 */
+	u8 rxulp0_ue_status;	/* DWORD 0 */
+	u8 rxulp1_ue_status;	/* DWORD 0 */
+	u8 rxulp2_ue_status;	/* DWORD 0 */
+	u8 tim_ue_status;	/* DWORD 0 */
+	u8 tpost_ue_status;	/* DWORD 0 */
+	u8 tpre_ue_status;	/* DWORD 0 */
+	u8 txips_ue_status;	/* DWORD 0 */
+	u8 txulp0_ue_status;	/* DWORD 0 */
+	u8 txulp1_ue_status;	/* DWORD 0 */
+	u8 uc_ue_status;	/* DWORD 0 */
+	u8 wdma_ue_status;	/* DWORD 0 */
+	u8 txulp2_ue_status;	/* DWORD 0 */
+	u8 host1_ue_status;	/* DWORD 0 */
+	u8 p0_ob_link_ue_status;	/* DWORD 0 */
+	u8 p1_ob_link_ue_status;	/* DWORD 0 */
+	u8 host_gpio_ue_status;	/* DWORD 0 */
+	u8 mbox_netw_ue_status;	/* DWORD 0 */
+	u8 mbox_stor_ue_status;	/* DWORD 0 */
+	u8 axgmac0_ue_status;	/* DWORD 0 */
+	u8 axgmac1_ue_status;	/* DWORD 0 */
+	u8 mpu_intpend_ue_status;	/* DWORD 0 */
+} __packed;
+struct PCICFG_UE_STATUS_LOW_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Unrecoverable Error Status (High) Register. Each bit corresponds to
+ * an internal Unrecoverable Error.  These are set by hardware and may be
+ * cleared by writing a one to the respective bit(s) to be cleared.  Any
+ * bit being set that is also unmasked will result in Unrecoverable Error
+ * interrupt notification to the host CPU and/or Server Management chip;
+ * and the transitioning of BladeEngine to an Offline state.
+ */
+struct BE_PCICFG_UE_STATUS_HI_CSR_AMAP {
+	u8 jtag_ue_status;	/* DWORD 0 */
+	u8 lpcmemhost_ue_status;	/* DWORD 0 */
+	u8 mgmt_mac_ue_status;	/* DWORD 0 */
+	u8 mpu_iram_ue_status;	/* DWORD 0 */
+	u8 pcs0online_ue_status;	/* DWORD 0 */
+	u8 pcs1online_ue_status;	/* DWORD 0 */
+	u8 pctl0_ue_status;	/* DWORD 0 */
+	u8 pctl1_ue_status;	/* DWORD 0 */
+	u8 pmem_ue_status;	/* DWORD 0 */
+	u8 rr_ue_status;	/* DWORD 0 */
+	u8 rxpp_ue_status;	/* DWORD 0 */
+	u8 txpb_ue_status;	/* DWORD 0 */
+	u8 txp_ue_status;	/* DWORD 0 */
+	u8 xaui_ue_status;	/* DWORD 0 */
+	u8 arm_ue_status;	/* DWORD 0 */
+	u8 ipc_ue_status;	/* DWORD 0 */
+	u8 rsvd0[16];	/* DWORD 0 */
+} __packed;
+struct PCICFG_UE_STATUS_HI_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Unrecoverable Error Mask (Low) Register. Each bit, when set to one,
+ * will mask the associated Unrecoverable  Error status bit from notification
+ * of Unrecoverable Error to the host CPU and/or Server Managment chip and the
+ * transitioning of all BladeEngine units to an Offline state.
+ */
+struct BE_PCICFG_UE_STATUS_LOW_MASK_CSR_AMAP {
+	u8 cev_ue_mask;	/* DWORD 0 */
+	u8 ctx_ue_mask;	/* DWORD 0 */
+	u8 dbuf_ue_mask;	/* DWORD 0 */
+	u8 erx_ue_mask;	/* DWORD 0 */
+	u8 host_ue_mask;	/* DWORD 0 */
+	u8 mpu_ue_mask;	/* DWORD 0 */
+	u8 ndma_ue_mask;	/* DWORD 0 */
+	u8 ptc_ue_mask;	/* DWORD 0 */
+	u8 rdma_ue_mask;	/* DWORD 0 */
+	u8 rxf_ue_mask;	/* DWORD 0 */
+	u8 rxips_ue_mask;	/* DWORD 0 */
+	u8 rxulp0_ue_mask;	/* DWORD 0 */
+	u8 rxulp1_ue_mask;	/* DWORD 0 */
+	u8 rxulp2_ue_mask;	/* DWORD 0 */
+	u8 tim_ue_mask;	/* DWORD 0 */
+	u8 tpost_ue_mask;	/* DWORD 0 */
+	u8 tpre_ue_mask;	/* DWORD 0 */
+	u8 txips_ue_mask;	/* DWORD 0 */
+	u8 txulp0_ue_mask;	/* DWORD 0 */
+	u8 txulp1_ue_mask;	/* DWORD 0 */
+	u8 uc_ue_mask;	/* DWORD 0 */
+	u8 wdma_ue_mask;	/* DWORD 0 */
+	u8 txulp2_ue_mask;	/* DWORD 0 */
+	u8 host1_ue_mask;	/* DWORD 0 */
+	u8 p0_ob_link_ue_mask;	/* DWORD 0 */
+	u8 p1_ob_link_ue_mask;	/* DWORD 0 */
+	u8 host_gpio_ue_mask;	/* DWORD 0 */
+	u8 mbox_netw_ue_mask;	/* DWORD 0 */
+	u8 mbox_stor_ue_mask;	/* DWORD 0 */
+	u8 axgmac0_ue_mask;	/* DWORD 0 */
+	u8 axgmac1_ue_mask;	/* DWORD 0 */
+	u8 mpu_intpend_ue_mask;	/* DWORD 0 */
+} __packed;
+struct PCICFG_UE_STATUS_LOW_MASK_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Unrecoverable Error Mask (High) Register. Each bit, when set to one,
+ * will mask the associated Unrecoverable Error status bit from notification
+ * of Unrecoverable Error to the host CPU and/or Server Managment chip and the
+ * transitioning of all BladeEngine units to an Offline state.
+ */
+struct BE_PCICFG_UE_STATUS_HI_MASK_CSR_AMAP {
+	u8 jtag_ue_mask;	/* DWORD 0 */
+	u8 lpcmemhost_ue_mask;	/* DWORD 0 */
+	u8 mgmt_mac_ue_mask;	/* DWORD 0 */
+	u8 mpu_iram_ue_mask;	/* DWORD 0 */
+	u8 pcs0online_ue_mask;	/* DWORD 0 */
+	u8 pcs1online_ue_mask;	/* DWORD 0 */
+	u8 pctl0_ue_mask;	/* DWORD 0 */
+	u8 pctl1_ue_mask;	/* DWORD 0 */
+	u8 pmem_ue_mask;	/* DWORD 0 */
+	u8 rr_ue_mask;	/* DWORD 0 */
+	u8 rxpp_ue_mask;	/* DWORD 0 */
+	u8 txpb_ue_mask;	/* DWORD 0 */
+	u8 txp_ue_mask;	/* DWORD 0 */
+	u8 xaui_ue_mask;	/* DWORD 0 */
+	u8 arm_ue_mask;	/* DWORD 0 */
+	u8 ipc_ue_mask;	/* DWORD 0 */
+	u8 rsvd0[16];	/* DWORD 0 */
+} __packed;
+struct PCICFG_UE_STATUS_HI_MASK_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Online Control Register 0. This register controls various units within
+ * BladeEngine being in an Online or Offline state.
+ */
+struct BE_PCICFG_ONLINE0_CSR_AMAP {
+	u8 cev_online;	/* DWORD 0 */
+	u8 ctx_online;	/* DWORD 0 */
+	u8 dbuf_online;	/* DWORD 0 */
+	u8 erx_online;	/* DWORD 0 */
+	u8 host_online;	/* DWORD 0 */
+	u8 mpu_online;	/* DWORD 0 */
+	u8 ndma_online;	/* DWORD 0 */
+	u8 ptc_online;	/* DWORD 0 */
+	u8 rdma_online;	/* DWORD 0 */
+	u8 rxf_online;	/* DWORD 0 */
+	u8 rxips_online;	/* DWORD 0 */
+	u8 rxulp0_online;	/* DWORD 0 */
+	u8 rxulp1_online;	/* DWORD 0 */
+	u8 rxulp2_online;	/* DWORD 0 */
+	u8 tim_online;	/* DWORD 0 */
+	u8 tpost_online;	/* DWORD 0 */
+	u8 tpre_online;	/* DWORD 0 */
+	u8 txips_online;	/* DWORD 0 */
+	u8 txulp0_online;	/* DWORD 0 */
+	u8 txulp1_online;	/* DWORD 0 */
+	u8 uc_online;	/* DWORD 0 */
+	u8 wdma_online;	/* DWORD 0 */
+	u8 txulp2_online;	/* DWORD 0 */
+	u8 host1_online;	/* DWORD 0 */
+	u8 p0_ob_link_online;	/* DWORD 0 */
+	u8 p1_ob_link_online;	/* DWORD 0 */
+	u8 host_gpio_online;	/* DWORD 0 */
+	u8 mbox_netw_online;	/* DWORD 0 */
+	u8 mbox_stor_online;	/* DWORD 0 */
+	u8 axgmac0_online;	/* DWORD 0 */
+	u8 axgmac1_online;	/* DWORD 0 */
+	u8 mpu_intpend_online;	/* DWORD 0 */
+} __packed;
+struct PCICFG_ONLINE0_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Online Control Register 1. This register controls various units within
+ * BladeEngine being in an Online or Offline state.
+ */
+struct BE_PCICFG_ONLINE1_CSR_AMAP {
+	u8 jtag_online;	/* DWORD 0 */
+	u8 lpcmemhost_online;	/* DWORD 0 */
+	u8 mgmt_mac_online;	/* DWORD 0 */
+	u8 mpu_iram_online;	/* DWORD 0 */
+	u8 pcs0online_online;	/* DWORD 0 */
+	u8 pcs1online_online;	/* DWORD 0 */
+	u8 pctl0_online;	/* DWORD 0 */
+	u8 pctl1_online;	/* DWORD 0 */
+	u8 pmem_online;	/* DWORD 0 */
+	u8 rr_online;	/* DWORD 0 */
+	u8 rxpp_online;	/* DWORD 0 */
+	u8 txpb_online;	/* DWORD 0 */
+	u8 txp_online;	/* DWORD 0 */
+	u8 xaui_online;	/* DWORD 0 */
+	u8 arm_online;	/* DWORD 0 */
+	u8 ipc_online;	/* DWORD 0 */
+	u8 rsvd0[16];	/* DWORD 0 */
+} __packed;
+struct PCICFG_ONLINE1_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Host Timer Register. */
+struct BE_PCICFG_HOST_TIMER_INT_CTRL_CSR_AMAP {
+	u8 hosttimer[24];	/* DWORD 0 */
+	u8 hostintr;	/* DWORD 0 */
+	u8 rsvd0[7];	/* DWORD 0 */
+} __packed;
+struct PCICFG_HOST_TIMER_INT_CTRL_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* Scratchpad Register (for software use). */
+struct BE_PCICFG_SCRATCHPAD_CSR_AMAP {
+	u8 scratchpad[32];	/* DWORD 0 */
+} __packed;
+struct PCICFG_SCRATCHPAD_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* PCI Express Capabilities Register. */
+struct BE_PCICFG_PCIE_CAP_CSR_AMAP {
+	u8 capid[8];	/* DWORD 0 */
+	u8 nextcap[8];	/* DWORD 0 */
+	u8 capver[4];	/* DWORD 0 */
+	u8 devport[4];	/* DWORD 0 */
+	u8 rsvd0[6];	/* DWORD 0 */
+	u8 rsvd1[2];	/* DWORD 0 */
+} __packed;
+struct PCICFG_PCIE_CAP_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* PCI Express Device Capabilities Register. */
+struct BE_PCICFG_PCIE_DEVCAP_CSR_AMAP {
+	u8 payload[3];	/* DWORD 0 */
+	u8 rsvd0[3];	/* DWORD 0 */
+	u8 lo_lat[3];	/* DWORD 0 */
+	u8 l1_lat[3];	/* DWORD 0 */
+	u8 rsvd1[3];	/* DWORD 0 */
+	u8 rsvd2[3];	/* DWORD 0 */
+	u8 pwr_value[8];	/* DWORD 0 */
+	u8 pwr_scale[2];	/* DWORD 0 */
+	u8 rsvd3[4];	/* DWORD 0 */
+} __packed;
+struct PCICFG_PCIE_DEVCAP_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* PCI Express Device Control/Status Registers. */
+struct BE_PCICFG_PCIE_CONTROL_STATUS_CSR_AMAP {
+	u8 CorrErrReportEn;	/* DWORD 0 */
+	u8 NonFatalErrReportEn;	/* DWORD 0 */
+	u8 FatalErrReportEn;	/* DWORD 0 */
+	u8 UnsuppReqReportEn;	/* DWORD 0 */
+	u8 EnableRelaxOrder;	/* DWORD 0 */
+	u8 Max_Payload_Size[3];	/* DWORD 0 */
+	u8 ExtendTagFieldEnable;	/* DWORD 0 */
+	u8 PhantomFnEnable;	/* DWORD 0 */
+	u8 AuxPwrPMEnable;	/* DWORD 0 */
+	u8 EnableNoSnoop;	/* DWORD 0 */
+	u8 Max_Read_Req_Size[3];	/* DWORD 0 */
+	u8 rsvd0;		/* DWORD 0 */
+	u8 CorrErrDetect;	/* DWORD 0 */
+	u8 NonFatalErrDetect;	/* DWORD 0 */
+	u8 FatalErrDetect;	/* DWORD 0 */
+	u8 UnsuppReqDetect;	/* DWORD 0 */
+	u8 AuxPwrDetect;	/* DWORD 0 */
+	u8 TransPending;	/* DWORD 0 */
+	u8 rsvd1[10];	/* DWORD 0 */
+} __packed;
+struct PCICFG_PCIE_CONTROL_STATUS_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* PCI Express Link Capabilities Register. */
+struct BE_PCICFG_PCIE_LINK_CAP_CSR_AMAP {
+	u8 MaxLinkSpeed[4];	/* DWORD 0 */
+	u8 MaxLinkWidth[6];	/* DWORD 0 */
+	u8 ASPMSupport[2];	/* DWORD 0 */
+	u8 L0sExitLat[3];	/* DWORD 0 */
+	u8 L1ExitLat[3];	/* DWORD 0 */
+	u8 rsvd0[6];	/* DWORD 0 */
+	u8 PortNum[8];	/* DWORD 0 */
+} __packed;
+struct PCICFG_PCIE_LINK_CAP_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* PCI Express Link Status Register. */
+struct BE_PCICFG_PCIE_LINK_STATUS_CSR_AMAP {
+	u8 ASPMCtl[2];	/* DWORD 0 */
+	u8 rsvd0;		/* DWORD 0 */
+	u8 ReadCmplBndry;	/* DWORD 0 */
+	u8 LinkDisable;	/* DWORD 0 */
+	u8 RetrainLink;	/* DWORD 0 */
+	u8 CommonClkConfig;	/* DWORD 0 */
+	u8 ExtendSync;	/* DWORD 0 */
+	u8 rsvd1[8];	/* DWORD 0 */
+	u8 LinkSpeed[4];	/* DWORD 0 */
+	u8 NegLinkWidth[6];	/* DWORD 0 */
+	u8 LinkTrainErr;	/* DWORD 0 */
+	u8 LinkTrain;	/* DWORD 0 */
+	u8 SlotClkConfig;	/* DWORD 0 */
+	u8 rsvd2[3];	/* DWORD 0 */
+} __packed;
+struct PCICFG_PCIE_LINK_STATUS_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* PCI Express MSI Configuration Register. */
+struct BE_PCICFG_MSI_CSR_AMAP {
+	u8 capid[8];	/* DWORD 0 */
+	u8 nextptr[8];	/* DWORD 0 */
+	u8 tablesize[11];	/* DWORD 0 */
+	u8 rsvd0[3];	/* DWORD 0 */
+	u8 funcmask;	/* DWORD 0 */
+	u8 en;		/* DWORD 0 */
+} __packed;
+struct PCICFG_MSI_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* MSI-X Table Offset Register. */
+struct BE_PCICFG_MSIX_TABLE_CSR_AMAP {
+	u8 tablebir[3];	/* DWORD 0 */
+	u8 offset[29];	/* DWORD 0 */
+} __packed;
+struct PCICFG_MSIX_TABLE_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* MSI-X PBA Offset Register. */
+struct BE_PCICFG_MSIX_PBA_CSR_AMAP {
+	u8 pbabir[3];	/* DWORD 0 */
+	u8 offset[29];	/* DWORD 0 */
+} __packed;
+struct PCICFG_MSIX_PBA_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* PCI Express MSI-X Message Vector Control Register. */
+struct BE_PCICFG_MSIX_VECTOR_CONTROL_CSR_AMAP {
+	u8 vector_control;	/* DWORD 0 */
+	u8 rsvd0[31];	/* DWORD 0 */
+} __packed;
+struct PCICFG_MSIX_VECTOR_CONTROL_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* PCI Express MSI-X Message Data Register. */
+struct BE_PCICFG_MSIX_MSG_DATA_CSR_AMAP {
+	u8 data[16];	/* DWORD 0 */
+	u8 rsvd0[16];	/* DWORD 0 */
+} __packed;
+struct PCICFG_MSIX_MSG_DATA_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* PCI Express MSI-X Message Address Register - High Part. */
+struct BE_PCICFG_MSIX_MSG_ADDR_HI_CSR_AMAP {
+	u8 addr[32];	/* DWORD 0 */
+} __packed;
+struct PCICFG_MSIX_MSG_ADDR_HI_CSR_AMAP {
+	u32 dw[1];
+};
+
+/* PCI Express MSI-X Message Address Register - Low Part. */
+struct BE_PCICFG_MSIX_MSG_ADDR_LO_CSR_AMAP {
+	u8 rsvd0[2];	/* DWORD 0 */
+	u8 addr[30];	/* DWORD 0 */
+} __packed;
+struct PCICFG_MSIX_MSG_ADDR_LO_CSR_AMAP {
+	u32 dw[1];
+};
+
+struct BE_PCICFG_ANON_18_RSVD_AMAP {
+	u8 rsvd0[32];	/* DWORD 0 */
+} __packed;
+struct PCICFG_ANON_18_RSVD_AMAP {
+	u32 dw[1];
+};
+
+struct BE_PCICFG_ANON_19_RSVD_AMAP {
+	u8 rsvd0[32];	/* DWORD 0 */
+} __packed;
+struct PCICFG_ANON_19_RSVD_AMAP {
+	u32 dw[1];
+};
+
+struct BE_PCICFG_ANON_20_RSVD_AMAP {
+	u8 rsvd0[32];	/* DWORD 0 */
+	u8 rsvd1[25][32];	/* DWORD 1 */
+} __packed;
+struct PCICFG_ANON_20_RSVD_AMAP {
+	u32 dw[26];
+};
+
+struct BE_PCICFG_ANON_21_RSVD_AMAP {
+	u8 rsvd0[32];	/* DWORD 0 */
+	u8 rsvd1[1919][32];	/* DWORD 1 */
+} __packed;
+struct PCICFG_ANON_21_RSVD_AMAP {
+	u32 dw[1920];
+};
+
+struct BE_PCICFG_ANON_22_MESSAGE_AMAP {
+	struct BE_PCICFG_MSIX_VECTOR_CONTROL_CSR_AMAP vec_ctrl;
+	struct BE_PCICFG_MSIX_MSG_DATA_CSR_AMAP msg_data;
+	struct BE_PCICFG_MSIX_MSG_ADDR_HI_CSR_AMAP addr_hi;
+	struct BE_PCICFG_MSIX_MSG_ADDR_LO_CSR_AMAP addr_low;
+} __packed;
+struct PCICFG_ANON_22_MESSAGE_AMAP {
+	u32 dw[4];
+};
+
+struct BE_PCICFG_ANON_23_RSVD_AMAP {
+	u8 rsvd0[32];	/* DWORD 0 */
+	u8 rsvd1[895][32];	/* DWORD 1 */
+} __packed;
+struct PCICFG_ANON_23_RSVD_AMAP {
+	u32 dw[896];
+};
+
+/* These PCI Configuration Space registers are for the Storage  Function of
+ * BladeEngine (Function 0). In the memory map of the registers below their
+ * table,
+ */
+struct BE_PCICFG0_CSRMAP_AMAP {
+	struct BE_PCICFG_ID_CSR_AMAP id;
+	u8 rsvd0[32];	/* DWORD 1 */
+	u8 rsvd1[32];	/* DWORD 2 */
+	u8 rsvd2[32];	/* DWORD 3 */
+	struct BE_PCICFG_IOBAR_CSR_AMAP iobar;
+	struct BE_PCICFG_MEMBAR0_CSR_AMAP membar0;
+	struct BE_PCICFG_MEMBAR1_LO_CSR_AMAP membar1_lo;
+	struct BE_PCICFG_MEMBAR1_HI_CSR_AMAP membar1_hi;
+	struct BE_PCICFG_MEMBAR2_LO_CSR_AMAP membar2_lo;
+	struct BE_PCICFG_MEMBAR2_HI_CSR_AMAP membar2_hi;
+	u8 rsvd3[32];	/* DWORD 10 */
+	struct BE_PCICFG_SUBSYSTEM_ID_F0_CSR_AMAP subsystem_id;
+	u8 rsvd4[32];	/* DWORD 12 */
+	u8 rsvd5[32];	/* DWORD 13 */
+	u8 rsvd6[32];	/* DWORD 14 */
+	u8 rsvd7[32];	/* DWORD 15 */
+	struct BE_PCICFG_SEMAPHORE_CSR_AMAP semaphore[4];
+	struct BE_PCICFG_SOFT_RESET_CSR_AMAP soft_reset;
+	u8 rsvd8[32];	/* DWORD 21 */
+	struct BE_PCICFG_SCRATCHPAD_CSR_AMAP scratchpad;
+	u8 rsvd9[32];	/* DWORD 23 */
+	u8 rsvd10[32];	/* DWORD 24 */
+	u8 rsvd11[32];	/* DWORD 25 */
+	u8 rsvd12[32];	/* DWORD 26 */
+	u8 rsvd13[32];	/* DWORD 27 */
+	u8 rsvd14[2][32];	/* DWORD 28 */
+	u8 rsvd15[32];	/* DWORD 30 */
+	u8 rsvd16[32];	/* DWORD 31 */
+	u8 rsvd17[8][32];	/* DWORD 32 */
+	struct BE_PCICFG_UE_STATUS_LOW_CSR_AMAP ue_status_low;
+	struct BE_PCICFG_UE_STATUS_HI_CSR_AMAP ue_status_hi;
+	struct BE_PCICFG_UE_STATUS_LOW_MASK_CSR_AMAP ue_status_low_mask;
+	struct BE_PCICFG_UE_STATUS_HI_MASK_CSR_AMAP ue_status_hi_mask;
+	struct BE_PCICFG_ONLINE0_CSR_AMAP online0;
+	struct BE_PCICFG_ONLINE1_CSR_AMAP online1;
+	u8 rsvd18[32];	/* DWORD 46 */
+	u8 rsvd19[32];	/* DWORD 47 */
+	u8 rsvd20[32];	/* DWORD 48 */
+	u8 rsvd21[32];	/* DWORD 49 */
+	struct BE_PCICFG_HOST_TIMER_INT_CTRL_CSR_AMAP host_timer_int_ctrl;
+	u8 rsvd22[32];	/* DWORD 51 */
+	struct BE_PCICFG_PCIE_CAP_CSR_AMAP pcie_cap;
+	struct BE_PCICFG_PCIE_DEVCAP_CSR_AMAP pcie_devcap;
+	struct BE_PCICFG_PCIE_CONTROL_STATUS_CSR_AMAP pcie_control_status;
+	struct BE_PCICFG_PCIE_LINK_CAP_CSR_AMAP pcie_link_cap;
+	struct BE_PCICFG_PCIE_LINK_STATUS_CSR_AMAP pcie_link_status;
+	struct BE_PCICFG_MSI_CSR_AMAP msi;
+	struct BE_PCICFG_MSIX_TABLE_CSR_AMAP msix_table_offset;
+	struct BE_PCICFG_MSIX_PBA_CSR_AMAP msix_pba_offset;
+	u8 rsvd23[32];	/* DWORD 60 */
+	u8 rsvd24[32];	/* DWORD 61 */
+	u8 rsvd25[32];	/* DWORD 62 */
+	u8 rsvd26[32];	/* DWORD 63 */
+	u8 rsvd27[32];	/* DWORD 64 */
+	u8 rsvd28[32];	/* DWORD 65 */
+	u8 rsvd29[32];	/* DWORD 66 */
+	u8 rsvd30[32];	/* DWORD 67 */
+	u8 rsvd31[32];	/* DWORD 68 */
+	u8 rsvd32[32];	/* DWORD 69 */
+	u8 rsvd33[32];	/* DWORD 70 */
+	u8 rsvd34[32];	/* DWORD 71 */
+	u8 rsvd35[32];	/* DWORD 72 */
+	u8 rsvd36[32];	/* DWORD 73 */
+	u8 rsvd37[32];	/* DWORD 74 */
+	u8 rsvd38[32];	/* DWORD 75 */
+	u8 rsvd39[32];	/* DWORD 76 */
+	u8 rsvd40[32];	/* DWORD 77 */
+	u8 rsvd41[32];	/* DWORD 78 */
+	u8 rsvd42[32];	/* DWORD 79 */
+	u8 rsvd43[32];	/* DWORD 80 */
+	u8 rsvd44[32];	/* DWORD 81 */
+	u8 rsvd45[32];	/* DWORD 82 */
+	u8 rsvd46[32];	/* DWORD 83 */
+	u8 rsvd47[32];	/* DWORD 84 */
+	u8 rsvd48[32];	/* DWORD 85 */
+	u8 rsvd49[32];	/* DWORD 86 */
+	u8 rsvd50[32];	/* DWORD 87 */
+	u8 rsvd51[32];	/* DWORD 88 */
+	u8 rsvd52[32];	/* DWORD 89 */
+	u8 rsvd53[32];	/* DWORD 90 */
+	u8 rsvd54[32];	/* DWORD 91 */
+	u8 rsvd55[32];	/* DWORD 92 */
+	u8 rsvd56[832];	/* DWORD 93 */
+	u8 rsvd57[32];	/* DWORD 119 */
+	u8 rsvd58[32];	/* DWORD 120 */
+	u8 rsvd59[32];	/* DWORD 121 */
+	u8 rsvd60[32];	/* DWORD 122 */
+	u8 rsvd61[32];	/* DWORD 123 */
+	u8 rsvd62[32];	/* DWORD 124 */
+	u8 rsvd63[32];	/* DWORD 125 */
+	u8 rsvd64[32];	/* DWORD 126 */
+	u8 rsvd65[32];	/* DWORD 127 */
+	u8 rsvd66[61440];	/* DWORD 128 */
+	struct BE_PCICFG_ANON_22_MESSAGE_AMAP message[32];
+	u8 rsvd67[28672];	/* DWORD 2176 */
+	u8 rsvd68[32];	/* DWORD 3072 */
+	u8 rsvd69[1023][32];	/* DWORD 3073 */
+} __packed;
+struct PCICFG0_CSRMAP_AMAP {
+	u32 dw[4096];
+};
+
+struct BE_PCICFG_ANON_24_RSVD_AMAP {
+	u8 rsvd0[32];	/* DWORD 0 */
+} __packed;
+struct PCICFG_ANON_24_RSVD_AMAP {
+	u32 dw[1];
+};
+
+struct BE_PCICFG_ANON_25_RSVD_AMAP {
+	u8 rsvd0[32];	/* DWORD 0 */
+} __packed;
+struct PCICFG_ANON_25_RSVD_AMAP {
+	u32 dw[1];
+};
+
+struct BE_PCICFG_ANON_26_RSVD_AMAP {
+	u8 rsvd0[32];	/* DWORD 0 */
+} __packed;
+struct PCICFG_ANON_26_RSVD_AMAP {
+	u32 dw[1];
+};
+
+struct BE_PCICFG_ANON_27_RSVD_AMAP {
+	u8 rsvd0[32];	/* DWORD 0 */
+	u8 rsvd1[32];	/* DWORD 1 */
+} __packed;
+struct PCICFG_ANON_27_RSVD_AMAP {
+	u32 dw[2];
+};
+
+struct BE_PCICFG_ANON_28_RSVD_AMAP {
+	u8 rsvd0[32];	/* DWORD 0 */
+	u8 rsvd1[3][32];	/* DWORD 1 */
+} __packed;
+struct PCICFG_ANON_28_RSVD_AMAP {
+	u32 dw[4];
+};
+
+struct BE_PCICFG_ANON_29_RSVD_AMAP {
+	u8 rsvd0[32];	/* DWORD 0 */
+	u8 rsvd1[36][32];	/* DWORD 1 */
+} __packed;
+struct PCICFG_ANON_29_RSVD_AMAP {
+	u32 dw[37];
+};
+
+struct BE_PCICFG_ANON_30_RSVD_AMAP {
+	u8 rsvd0[32];	/* DWORD 0 */
+	u8 rsvd1[1930][32];	/* DWORD 1 */
+} __packed;
+struct PCICFG_ANON_30_RSVD_AMAP {
+	u32 dw[1931];
+};
+
+struct BE_PCICFG_ANON_31_MESSAGE_AMAP {
+	struct BE_PCICFG_MSIX_VECTOR_CONTROL_CSR_AMAP vec_ctrl;
+	struct BE_PCICFG_MSIX_MSG_DATA_CSR_AMAP msg_data;
+	struct BE_PCICFG_MSIX_MSG_ADDR_HI_CSR_AMAP addr_hi;
+	struct BE_PCICFG_MSIX_MSG_ADDR_LO_CSR_AMAP addr_low;
+} __packed;
+struct PCICFG_ANON_31_MESSAGE_AMAP {
+	u32 dw[4];
+};
+
+struct BE_PCICFG_ANON_32_RSVD_AMAP {
+	u8 rsvd0[32];	/* DWORD 0 */
+	u8 rsvd1[895][32];	/* DWORD 1 */
+} __packed;
+struct PCICFG_ANON_32_RSVD_AMAP {
+	u32 dw[896];
+};
+
+/* This PCI configuration space register map is for the  Networking Function of
+ * BladeEngine (Function 1).
+ */
+struct BE_PCICFG1_CSRMAP_AMAP {
+	struct BE_PCICFG_ID_CSR_AMAP id;
+	u8 rsvd0[32];	/* DWORD 1 */
+	u8 rsvd1[32];	/* DWORD 2 */
+	u8 rsvd2[32];	/* DWORD 3 */
+	struct BE_PCICFG_IOBAR_CSR_AMAP iobar;
+	struct BE_PCICFG_MEMBAR0_CSR_AMAP membar0;
+	struct BE_PCICFG_MEMBAR1_LO_CSR_AMAP membar1_lo;
+	struct BE_PCICFG_MEMBAR1_HI_CSR_AMAP membar1_hi;
+	struct BE_PCICFG_MEMBAR2_LO_CSR_AMAP membar2_lo;
+	struct BE_PCICFG_MEMBAR2_HI_CSR_AMAP membar2_hi;
+	u8 rsvd3[32];	/* DWORD 10 */
+	struct BE_PCICFG_SUBSYSTEM_ID_F1_CSR_AMAP subsystem_id;
+	u8 rsvd4[32];	/* DWORD 12 */
+	u8 rsvd5[32];	/* DWORD 13 */
+	u8 rsvd6[32];	/* DWORD 14 */
+	u8 rsvd7[32];	/* DWORD 15 */
+	struct BE_PCICFG_SEMAPHORE_CSR_AMAP semaphore[4];
+	struct BE_PCICFG_SOFT_RESET_CSR_AMAP soft_reset;
+	u8 rsvd8[32];	/* DWORD 21 */
+	struct BE_PCICFG_SCRATCHPAD_CSR_AMAP scratchpad;
+	u8 rsvd9[32];	/* DWORD 23 */
+	u8 rsvd10[32];	/* DWORD 24 */
+	u8 rsvd11[32];	/* DWORD 25 */
+	u8 rsvd12[32];	/* DWORD 26 */
+	u8 rsvd13[32];	/* DWORD 27 */
+	u8 rsvd14[2][32];	/* DWORD 28 */
+	u8 rsvd15[32];	/* DWORD 30 */
+	u8 rsvd16[32];	/* DWORD 31 */
+	u8 rsvd17[8][32];	/* DWORD 32 */
+	struct BE_PCICFG_UE_STATUS_LOW_CSR_AMAP ue_status_low;
+	struct BE_PCICFG_UE_STATUS_HI_CSR_AMAP ue_status_hi;
+	struct BE_PCICFG_UE_STATUS_LOW_MASK_CSR_AMAP ue_status_low_mask;
+	struct BE_PCICFG_UE_STATUS_HI_MASK_CSR_AMAP ue_status_hi_mask;
+	struct BE_PCICFG_ONLINE0_CSR_AMAP online0;
+	struct BE_PCICFG_ONLINE1_CSR_AMAP online1;
+	u8 rsvd18[32];	/* DWORD 46 */
+	u8 rsvd19[32];	/* DWORD 47 */
+	u8 rsvd20[32];	/* DWORD 48 */
+	u8 rsvd21[32];	/* DWORD 49 */
+	struct BE_PCICFG_HOST_TIMER_INT_CTRL_CSR_AMAP host_timer_int_ctrl;
+	u8 rsvd22[32];	/* DWORD 51 */
+	struct BE_PCICFG_PCIE_CAP_CSR_AMAP pcie_cap;
+	struct BE_PCICFG_PCIE_DEVCAP_CSR_AMAP pcie_devcap;
+	struct BE_PCICFG_PCIE_CONTROL_STATUS_CSR_AMAP pcie_control_status;
+	struct BE_PCICFG_PCIE_LINK_CAP_CSR_AMAP pcie_link_cap;
+	struct BE_PCICFG_PCIE_LINK_STATUS_CSR_AMAP pcie_link_status;
+	struct BE_PCICFG_MSI_CSR_AMAP msi;
+	struct BE_PCICFG_MSIX_TABLE_CSR_AMAP msix_table_offset;
+	struct BE_PCICFG_MSIX_PBA_CSR_AMAP msix_pba_offset;
+	u8 rsvd23[64];	/* DWORD 60 */
+	u8 rsvd24[32];	/* DWORD 62 */
+	u8 rsvd25[32];	/* DWORD 63 */
+	u8 rsvd26[32];	/* DWORD 64 */
+	u8 rsvd27[32];	/* DWORD 65 */
+	u8 rsvd28[32];	/* DWORD 66 */
+	u8 rsvd29[32];	/* DWORD 67 */
+	u8 rsvd30[32];	/* DWORD 68 */
+	u8 rsvd31[32];	/* DWORD 69 */
+	u8 rsvd32[32];	/* DWORD 70 */
+	u8 rsvd33[32];	/* DWORD 71 */
+	u8 rsvd34[32];	/* DWORD 72 */
+	u8 rsvd35[32];	/* DWORD 73 */
+	u8 rsvd36[32];	/* DWORD 74 */
+	u8 rsvd37[128];	/* DWORD 75 */
+	u8 rsvd38[32];	/* DWORD 79 */
+	u8 rsvd39[1184];	/* DWORD 80 */
+	u8 rsvd40[61792];	/* DWORD 117 */
+	struct BE_PCICFG_ANON_31_MESSAGE_AMAP message[32];
+	u8 rsvd41[28672];	/* DWORD 2176 */
+	u8 rsvd42[32];	/* DWORD 3072 */
+	u8 rsvd43[1023][32];	/* DWORD 3073 */
+} __packed;
+struct PCICFG1_CSRMAP_AMAP {
+	u32 dw[4096];
+};
+
+#endif /* __pcicfg_amap_h__ */
diff --git a/drivers/staging/benet/post_codes.h b/drivers/staging/benet/post_codes.h
new file mode 100644
index 00000000000..6d1621f5f5f
--- /dev/null
+++ b/drivers/staging/benet/post_codes.h
@@ -0,0 +1,111 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __post_codes_amap_h__
+#define __post_codes_amap_h__
+
+/* --- MGMT_HBA_POST_STAGE_ENUM --- */
+#define POST_STAGE_POWER_ON_RESET   (0)	/* State after a cold or warm boot. */
+#define POST_STAGE_AWAITING_HOST_RDY (1)	/* ARM boot code awaiting a
+						go-ahed from  the host. */
+#define POST_STAGE_HOST_RDY (2)	/* Host has given go-ahed to ARM. */
+#define POST_STAGE_BE_RESET (3)	/* Host wants to reset chip, this is a  chip
+						workaround  */
+#define POST_STAGE_SEEPROM_CS_START (256)	/* SEEPROM checksum
+						test start. */
+#define POST_STAGE_SEEPROM_CS_DONE  (257)	/* SEEPROM checksum test
+							done. */
+#define POST_STAGE_DDR_CONFIG_START (512)	/* DDR configuration start. */
+#define POST_STAGE_DDR_CONFIG_DONE  (513)	/* DDR configuration done. */
+#define POST_STAGE_DDR_CALIBRATE_START  (768)	/* DDR calibration start. */
+#define POST_STAGE_DDR_CALIBRATE_DONE   (769)	/* DDR calibration done. */
+#define POST_STAGE_DDR_TEST_START   (1024)	/* DDR memory test start. */
+#define POST_STAGE_DDR_TEST_DONE    (1025)	/* DDR memory test done. */
+#define POST_STAGE_REDBOOT_INIT_START   (1536)	/* Redboot starts execution. */
+#define POST_STAGE_REDBOOT_INIT_DONE (1537)	/* Redboot done execution. */
+#define POST_STAGE_FW_IMAGE_LOAD_START (1792)	/* Firmware image load to
+							DDR start. */
+#define POST_STAGE_FW_IMAGE_LOAD_DONE   (1793)	/* Firmware image load
+							to DDR done. */
+#define POST_STAGE_ARMFW_START          (2048)	/* ARMfw runtime code
+						starts execution. */
+#define POST_STAGE_DHCP_QUERY_START     (2304)	/* DHCP server query start. */
+#define POST_STAGE_DHCP_QUERY_DONE      (2305)	/* DHCP server query done. */
+#define POST_STAGE_BOOT_TARGET_DISCOVERY_START (2560)	/* Boot Target
+						Discovery Start. */
+#define POST_STAGE_BOOT_TARGET_DISCOVERY_DONE (2561)	/* Boot Target
+						Discovery Done. */
+#define POST_STAGE_RC_OPTION_SET        (2816)	/* Remote configuration
+						option is set in  SEEPROM  */
+#define POST_STAGE_SWITCH_LINK          (2817)	/* Wait for link up on switch */
+#define POST_STAGE_SEND_ICDS_MESSAGE    (2818)	/* Send the ICDS message
+						to switch */
+#define POST_STAGE_PERFROM_TFTP         (2819)	/* Download xml using TFTP */
+#define POST_STAGE_PARSE_XML            (2820)	/* Parse XML file */
+#define POST_STAGE_DOWNLOAD_IMAGE       (2821)	/* Download IMAGE from
+						TFTP server */
+#define POST_STAGE_FLASH_IMAGE          (2822)	/* Flash the IMAGE */
+#define POST_STAGE_RC_DONE              (2823)	/* Remote configuration
+						complete */
+#define POST_STAGE_REBOOT_SYSTEM        (2824)	/* Upgrade IMAGE done,
+						reboot required */
+#define POST_STAGE_MAC_ADDRESS          (3072)	/* MAC Address Check */
+#define POST_STAGE_ARMFW_READY          (49152)	/* ARMfw is done with POST
+						and ready. */
+#define POST_STAGE_ARMFW_UE             (61440)	/* ARMfw has asserted an
+						unrecoverable error. The
+						lower 3 hex digits of the
+						stage code identify the
+						unique error code.
+						*/
+
+/* This structure defines the format of the MPU semaphore
+ * register when used for POST.
+ */
+struct BE_MGMT_HBA_POST_STATUS_STRUCT_AMAP {
+	u8 stage[16];	/* DWORD 0 */
+	u8 rsvd0[10];	/* DWORD 0 */
+	u8 iscsi_driver_loaded;	/* DWORD 0 */
+	u8 option_rom_installed;	/* DWORD 0 */
+	u8 iscsi_ip_conflict;	/* DWORD 0 */
+	u8 iscsi_no_ip;	/* DWORD 0 */
+	u8 backup_fw;	/* DWORD 0 */
+	u8 error;		/* DWORD 0 */
+} __packed;
+struct MGMT_HBA_POST_STATUS_STRUCT_AMAP {
+	u32 dw[1];
+};
+
+/* --- MGMT_HBA_POST_DUMMY_BITS_ENUM --- */
+#define POST_BIT_ISCSI_LOADED           (26)
+#define POST_BIT_OPTROM_INST            (27)
+#define POST_BIT_BAD_IP_ADDR            (28)
+#define POST_BIT_NO_IP_ADDR             (29)
+#define POST_BIT_BACKUP_FW              (30)
+#define POST_BIT_ERROR                  (31)
+
+/* --- MGMT_HBA_POST_DUMMY_VALUES_ENUM --- */
+#define POST_ISCSI_DRIVER_LOADED        (67108864)
+#define POST_OPTROM_INSTALLED           (134217728)
+#define POST_ISCSI_IP_ADDRESS_CONFLICT  (268435456)
+#define POST_ISCSI_NO_IP_ADDRESS        (536870912)
+#define POST_BACKUP_FW_LOADED           (1073741824)
+#define POST_FATAL_ERROR                (2147483648)
+
+#endif /* __post_codes_amap_h__ */
diff --git a/drivers/staging/benet/regmap.h b/drivers/staging/benet/regmap.h
new file mode 100644
index 00000000000..e816ba210e8
--- /dev/null
+++ b/drivers/staging/benet/regmap.h
@@ -0,0 +1,68 @@
+/*
+ * Copyright (C) 2005 - 2008 ServerEngines
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.  The full GNU General
+ * Public License is included in this distribution in the file called COPYING.
+ *
+ * Contact Information:
+ * linux-drivers@serverengines.com
+ *
+ * ServerEngines
+ * 209 N. Fair Oaks Ave
+ * Sunnyvale, CA 94085
+ */
+/*
+ * Autogenerated by srcgen version: 0127
+ */
+#ifndef __regmap_amap_h__
+#define __regmap_amap_h__
+#include "pcicfg.h"
+#include "ep.h"
+#include "cev.h"
+#include "mpu.h"
+#include "doorbells.h"
+
+/*
+ * This is the control and status register map for BladeEngine, showing
+ * the relative size and offset of each sub-module. The CSR registers
+ * are identical for the network and storage PCI functions. The
+ * CSR map is shown below, followed by details of each block,
+ * in sub-sections.  The sub-sections begin with a description
+ * of CSRs that are instantiated in multiple blocks.
+ */
+struct BE_BLADE_ENGINE_CSRMAP_AMAP {
+	struct BE_MPU_CSRMAP_AMAP mpu;
+	u8 rsvd0[8192];	/* DWORD 256 */
+	u8 rsvd1[8192];	/* DWORD 512 */
+	struct BE_CEV_CSRMAP_AMAP cev;
+	u8 rsvd2[8192];	/* DWORD 1024 */
+	u8 rsvd3[8192];	/* DWORD 1280 */
+	u8 rsvd4[8192];	/* DWORD 1536 */
+	u8 rsvd5[8192];	/* DWORD 1792 */
+	u8 rsvd6[8192];	/* DWORD 2048 */
+	u8 rsvd7[8192];	/* DWORD 2304 */
+	u8 rsvd8[8192];	/* DWORD 2560 */
+	u8 rsvd9[8192];	/* DWORD 2816 */
+	u8 rsvd10[8192];	/* DWORD 3072 */
+	u8 rsvd11[8192];	/* DWORD 3328 */
+	u8 rsvd12[8192];	/* DWORD 3584 */
+	u8 rsvd13[8192];	/* DWORD 3840 */
+	u8 rsvd14[8192];	/* DWORD 4096 */
+	u8 rsvd15[8192];	/* DWORD 4352 */
+	u8 rsvd16[8192];	/* DWORD 4608 */
+	u8 rsvd17[8192];	/* DWORD 4864 */
+	u8 rsvd18[8192];	/* DWORD 5120 */
+	u8 rsvd19[8192];	/* DWORD 5376 */
+	u8 rsvd20[8192];	/* DWORD 5632 */
+	u8 rsvd21[8192];	/* DWORD 5888 */
+	u8 rsvd22[8192];	/* DWORD 6144 */
+	u8 rsvd23[17152][32];	/* DWORD 6400 */
+} __packed;
+struct BLADE_ENGINE_CSRMAP_AMAP {
+	u32 dw[23552];
+};
+
+#endif /* __regmap_amap_h__ */
diff --git a/drivers/staging/comedi/Kconfig b/drivers/staging/comedi/Kconfig
new file mode 100644
index 00000000000..b501bfb9c75
--- /dev/null
+++ b/drivers/staging/comedi/Kconfig
@@ -0,0 +1,27 @@
+config COMEDI
+	tristate "Data Acquision support (comedi)"
+	default N
+	---help---
+	  Enable support a wide range of data acquision devices
+	  for Linux.
+
+config COMEDI_RT
+	tristate "Comedi Real-time support"
+	depends on COMEDI && RT
+	default N
+	---help---
+	  Enable Real time support for the Comedi core.
+
+config COMEDI_PCI_DRIVERS
+	tristate "Comedi PCI drivers"
+	depends on COMEDI && PCI
+	default N
+	---help---
+	  Enable lots of comedi PCI drivers to be built
+
+config COMEDI_USB_DRIVERS
+	tristate "Comedi USB drivers"
+	depends on COMEDI && USB
+	default N
+	---help---
+	  Enable lots of comedi USB drivers to be built
diff --git a/drivers/staging/comedi/Makefile b/drivers/staging/comedi/Makefile
new file mode 100644
index 00000000000..afd1a19c1b8
--- /dev/null
+++ b/drivers/staging/comedi/Makefile
@@ -0,0 +1,17 @@
+obj-$(CONFIG_COMEDI) += comedi.o
+obj-$(CONFIG_COMEDI_RT) += comedi_rt.o
+
+obj-$(CONFIG_COMEDI)	+= kcomedilib/
+obj-$(CONFIG_COMEDI)	+= drivers/
+
+comedi-objs :=		\
+	comedi_fops.o	\
+	proc.o		\
+	range.o		\
+	drivers.o	\
+	comedi_compat32.o \
+	comedi_ksyms.o	\
+
+comedi_rt-objs :=	\
+	rt_pend_tq.o	\
+	rt.o
diff --git a/drivers/staging/comedi/TODO b/drivers/staging/comedi/TODO
new file mode 100644
index 00000000000..55781295846
--- /dev/null
+++ b/drivers/staging/comedi/TODO
@@ -0,0 +1,14 @@
+TODO:
+	- checkpatch.pl cleanups
+	- Lindent
+	- remove all wrappers
+	- remove typedefs
+	- audit userspace interface
+	- reserve major number
+	- cleanup the individual comedi drivers as well
+
+Please send patches to Greg Kroah-Hartman <greg@kroah.com> and
+copy:
+	Ian Abbott <abbotti@mev.co.uk>
+	Frank Mori Hess <fmhess@users.sourceforge.net>
+	David Schleef <ds@schleef.org>
diff --git a/drivers/staging/comedi/comedi.h b/drivers/staging/comedi/comedi.h
new file mode 100644
index 00000000000..36d2e1b01e7
--- /dev/null
+++ b/drivers/staging/comedi/comedi.h
@@ -0,0 +1,916 @@
+/*
+    include/comedi.h (installed as /usr/include/comedi.h)
+    header file for comedi
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1998-2001 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU Lesser General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#ifndef _COMEDI_H
+#define _COMEDI_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define COMEDI_MAJORVERSION	0
+#define COMEDI_MINORVERSION	7
+#define COMEDI_MICROVERSION	76
+#define VERSION	"0.7.76"
+
+/* comedi's major device number */
+#define COMEDI_MAJOR 98
+
+/*
+   maximum number of minor devices.  This can be increased, although
+   kernel structures are currently statically allocated, thus you
+   don't want this to be much more than you actually use.
+ */
+#define COMEDI_NDEVICES 16
+
+/* number of config options in the config structure */
+#define COMEDI_NDEVCONFOPTS 32
+/*length of nth chunk of firmware data*/
+#define COMEDI_DEVCONF_AUX_DATA3_LENGTH		25
+#define COMEDI_DEVCONF_AUX_DATA2_LENGTH		26
+#define COMEDI_DEVCONF_AUX_DATA1_LENGTH		27
+#define COMEDI_DEVCONF_AUX_DATA0_LENGTH		28
+#define COMEDI_DEVCONF_AUX_DATA_HI		29	/* most significant 32 bits of pointer address (if needed) */
+#define COMEDI_DEVCONF_AUX_DATA_LO		30	/* least significant 32 bits of pointer address */
+#define COMEDI_DEVCONF_AUX_DATA_LENGTH		31	/* total data length */
+
+/* max length of device and driver names */
+#define COMEDI_NAMELEN 20
+
+	typedef unsigned int lsampl_t;
+	typedef unsigned short sampl_t;
+
+/* packs and unpacks a channel/range number */
+
+#define CR_PACK(chan, rng, aref)		((((aref)&0x3)<<24) | (((rng)&0xff)<<16) | (chan))
+#define CR_PACK_FLAGS(chan, range, aref, flags)	(CR_PACK(chan, range, aref) | ((flags) & CR_FLAGS_MASK))
+
+#define CR_CHAN(a)	((a)&0xffff)
+#define CR_RANGE(a)	(((a)>>16)&0xff)
+#define CR_AREF(a)	(((a)>>24)&0x03)
+
+#define CR_FLAGS_MASK	0xfc000000
+#define CR_ALT_FILTER	(1<<26)
+#define CR_DITHER	CR_ALT_FILTER
+#define CR_DEGLITCH	CR_ALT_FILTER
+#define CR_ALT_SOURCE	(1<<27)
+#define CR_EDGE		(1<<30)
+#define CR_INVERT	(1<<31)
+
+#define AREF_GROUND	0x00	/* analog ref = analog ground */
+#define AREF_COMMON	0x01	/* analog ref = analog common */
+#define AREF_DIFF	0x02	/* analog ref = differential */
+#define AREF_OTHER	0x03	/* analog ref = other (undefined) */
+
+/* counters -- these are arbitrary values */
+#define GPCT_RESET		0x0001
+#define GPCT_SET_SOURCE		0x0002
+#define GPCT_SET_GATE		0x0004
+#define GPCT_SET_DIRECTION	0x0008
+#define GPCT_SET_OPERATION	0x0010
+#define GPCT_ARM		0x0020
+#define GPCT_DISARM		0x0040
+#define GPCT_GET_INT_CLK_FRQ	0x0080
+
+#define GPCT_INT_CLOCK		0x0001
+#define GPCT_EXT_PIN		0x0002
+#define GPCT_NO_GATE		0x0004
+#define GPCT_UP			0x0008
+#define GPCT_DOWN		0x0010
+#define GPCT_HWUD		0x0020
+#define GPCT_SIMPLE_EVENT	0x0040
+#define GPCT_SINGLE_PERIOD	0x0080
+#define GPCT_SINGLE_PW		0x0100
+#define GPCT_CONT_PULSE_OUT	0x0200
+#define GPCT_SINGLE_PULSE_OUT	0x0400
+
+/* instructions */
+
+#define INSN_MASK_WRITE		0x8000000
+#define INSN_MASK_READ		0x4000000
+#define INSN_MASK_SPECIAL	0x2000000
+
+#define INSN_READ		(0 | INSN_MASK_READ)
+#define INSN_WRITE		(1 | INSN_MASK_WRITE)
+#define INSN_BITS		(2 | INSN_MASK_READ|INSN_MASK_WRITE)
+#define INSN_CONFIG		(3 | INSN_MASK_READ|INSN_MASK_WRITE)
+#define INSN_GTOD		(4 | INSN_MASK_READ|INSN_MASK_SPECIAL)
+#define INSN_WAIT		(5 | INSN_MASK_WRITE|INSN_MASK_SPECIAL)
+#define INSN_INTTRIG		(6 | INSN_MASK_WRITE|INSN_MASK_SPECIAL)
+
+/* trigger flags */
+/* These flags are used in comedi_trig structures */
+
+#define TRIG_BOGUS	0x0001	/* do the motions */
+#define TRIG_DITHER	0x0002	/* enable dithering */
+#define TRIG_DEGLITCH	0x0004	/* enable deglitching */
+/*#define TRIG_RT       0x0008 */	/* perform op in real time */
+#define TRIG_CONFIG	0x0010	/* perform configuration, not triggering */
+#define TRIG_WAKE_EOS	0x0020	/* wake up on end-of-scan events */
+/*#define TRIG_WRITE    0x0040*/	/* write to bidirectional devices */
+
+/* command flags */
+/* These flags are used in comedi_cmd structures */
+
+#define CMDF_PRIORITY		0x00000008	/* try to use a real-time interrupt while performing command */
+
+#define TRIG_RT		CMDF_PRIORITY	/* compatibility definition */
+
+#define CMDF_WRITE		0x00000040
+#define TRIG_WRITE	CMDF_WRITE	/* compatibility definition */
+
+#define CMDF_RAWDATA		0x00000080
+
+#define COMEDI_EV_START		0x00040000
+#define COMEDI_EV_SCAN_BEGIN	0x00080000
+#define COMEDI_EV_CONVERT	0x00100000
+#define COMEDI_EV_SCAN_END	0x00200000
+#define COMEDI_EV_STOP		0x00400000
+
+#define TRIG_ROUND_MASK		0x00030000
+#define TRIG_ROUND_NEAREST	0x00000000
+#define TRIG_ROUND_DOWN		0x00010000
+#define TRIG_ROUND_UP		0x00020000
+#define TRIG_ROUND_UP_NEXT	0x00030000
+
+/* trigger sources */
+
+#define TRIG_ANY	0xffffffff
+#define TRIG_INVALID	0x00000000
+
+#define TRIG_NONE	0x00000001	/* never trigger */
+#define TRIG_NOW	0x00000002	/* trigger now + N ns */
+#define TRIG_FOLLOW	0x00000004	/* trigger on next lower level trig */
+#define TRIG_TIME	0x00000008	/* trigger at time N ns */
+#define TRIG_TIMER	0x00000010	/* trigger at rate N ns */
+#define TRIG_COUNT	0x00000020	/* trigger when count reaches N */
+#define TRIG_EXT	0x00000040	/* trigger on external signal N */
+#define TRIG_INT	0x00000080	/* trigger on comedi-internal signal N */
+#define TRIG_OTHER	0x00000100	/* driver defined */
+
+/* subdevice flags */
+
+#define SDF_BUSY	0x0001	/* device is busy */
+#define SDF_BUSY_OWNER	0x0002	/* device is busy with your job */
+#define SDF_LOCKED	0x0004	/* subdevice is locked */
+#define SDF_LOCK_OWNER	0x0008	/* you own lock */
+#define SDF_MAXDATA	0x0010	/* maxdata depends on channel */
+#define SDF_FLAGS	0x0020	/* flags depend on channel */
+#define SDF_RANGETYPE	0x0040	/* range type depends on channel */
+#define SDF_MODE0	0x0080	/* can do mode 0 */
+#define SDF_MODE1	0x0100	/* can do mode 1 */
+#define SDF_MODE2	0x0200	/* can do mode 2 */
+#define SDF_MODE3	0x0400	/* can do mode 3 */
+#define SDF_MODE4	0x0800	/* can do mode 4 */
+#define SDF_CMD		0x1000	/* can do commands (deprecated) */
+#define SDF_SOFT_CALIBRATED	0x2000	/* subdevice uses software calibration */
+#define SDF_CMD_WRITE		0x4000	/* can do output commands */
+#define SDF_CMD_READ		0x8000	/* can do input commands */
+
+#define SDF_READABLE	0x00010000	/* subdevice can be read (e.g. analog input) */
+#define SDF_WRITABLE	0x00020000	/* subdevice can be written (e.g. analog output) */
+#define SDF_WRITEABLE	SDF_WRITABLE	/* spelling error in API */
+#define SDF_INTERNAL	0x00040000	/* subdevice does not have externally visible lines */
+#define SDF_RT		0x00080000	/* DEPRECATED: subdevice is RT capable */
+#define SDF_GROUND	0x00100000	/* can do aref=ground */
+#define SDF_COMMON	0x00200000	/* can do aref=common */
+#define SDF_DIFF	0x00400000	/* can do aref=diff */
+#define SDF_OTHER	0x00800000	/* can do aref=other */
+#define SDF_DITHER	0x01000000	/* can do dithering */
+#define SDF_DEGLITCH	0x02000000	/* can do deglitching */
+#define SDF_MMAP	0x04000000	/* can do mmap() */
+#define SDF_RUNNING	0x08000000	/* subdevice is acquiring data */
+#define SDF_LSAMPL	0x10000000	/* subdevice uses 32-bit samples */
+#define SDF_PACKED	0x20000000	/* subdevice can do packed DIO */
+/* re recyle these flags for PWM */
+#define SDF_PWM_COUNTER SDF_MODE0       /* PWM can automatically switch off */
+#define SDF_PWM_HBRIDGE SDF_MODE1       /* PWM is signed (H-bridge) */
+
+
+
+/* subdevice types */
+
+enum comedi_subdevice_type {
+	COMEDI_SUBD_UNUSED,	/* unused by driver */
+	COMEDI_SUBD_AI,	/* analog input */
+	COMEDI_SUBD_AO,	/* analog output */
+	COMEDI_SUBD_DI,	/* digital input */
+	COMEDI_SUBD_DO,	/* digital output */
+	COMEDI_SUBD_DIO,	/* digital input/output */
+	COMEDI_SUBD_COUNTER,	/* counter */
+	COMEDI_SUBD_TIMER,	/* timer */
+	COMEDI_SUBD_MEMORY,	/* memory, EEPROM, DPRAM */
+	COMEDI_SUBD_CALIB,	/* calibration DACs */
+	COMEDI_SUBD_PROC,	/* processor, DSP */
+	COMEDI_SUBD_SERIAL,	/* serial IO */
+	COMEDI_SUBD_PWM         /* PWM */
+};
+
+/* configuration instructions */
+
+enum configuration_ids {
+	INSN_CONFIG_DIO_INPUT = 0,
+	INSN_CONFIG_DIO_OUTPUT = 1,
+	INSN_CONFIG_DIO_OPENDRAIN = 2,
+	INSN_CONFIG_ANALOG_TRIG = 16,
+/*	INSN_CONFIG_WAVEFORM = 17, */
+/*	INSN_CONFIG_TRIG = 18, */
+/*	INSN_CONFIG_COUNTER = 19, */
+	INSN_CONFIG_ALT_SOURCE = 20,
+	INSN_CONFIG_DIGITAL_TRIG = 21,
+	INSN_CONFIG_BLOCK_SIZE = 22,
+	INSN_CONFIG_TIMER_1 = 23,
+	INSN_CONFIG_FILTER = 24,
+	INSN_CONFIG_CHANGE_NOTIFY = 25,
+
+	 /*ALPHA*/ INSN_CONFIG_SERIAL_CLOCK = 26,
+	INSN_CONFIG_BIDIRECTIONAL_DATA = 27,
+	INSN_CONFIG_DIO_QUERY = 28,
+	INSN_CONFIG_PWM_OUTPUT = 29,
+	INSN_CONFIG_GET_PWM_OUTPUT = 30,
+	INSN_CONFIG_ARM = 31,
+	INSN_CONFIG_DISARM = 32,
+	INSN_CONFIG_GET_COUNTER_STATUS = 33,
+	INSN_CONFIG_RESET = 34,
+	INSN_CONFIG_GPCT_SINGLE_PULSE_GENERATOR = 1001,	/* Use CTR as single pulsegenerator */
+	INSN_CONFIG_GPCT_PULSE_TRAIN_GENERATOR = 1002,	/* Use CTR as pulsetraingenerator */
+	INSN_CONFIG_GPCT_QUADRATURE_ENCODER = 1003,	/* Use the counter as encoder */
+	INSN_CONFIG_SET_GATE_SRC = 2001,	/* Set gate source */
+	INSN_CONFIG_GET_GATE_SRC = 2002,	/* Get gate source */
+	INSN_CONFIG_SET_CLOCK_SRC = 2003,	/* Set master clock source */
+	INSN_CONFIG_GET_CLOCK_SRC = 2004,	/* Get master clock source */
+	INSN_CONFIG_SET_OTHER_SRC = 2005,	/* Set other source */
+/*	INSN_CONFIG_GET_OTHER_SRC = 2006,*/	/* Get other source */
+	INSN_CONFIG_GET_HARDWARE_BUFFER_SIZE,	/* Get size in bytes of
+						  subdevice's on-board fifos
+						  used during streaming
+						  input/output */
+	INSN_CONFIG_SET_COUNTER_MODE = 4097,
+	INSN_CONFIG_8254_SET_MODE = INSN_CONFIG_SET_COUNTER_MODE,	/* deprecated */
+	INSN_CONFIG_8254_READ_STATUS = 4098,
+	INSN_CONFIG_SET_ROUTING = 4099,
+	INSN_CONFIG_GET_ROUTING = 4109,
+/* PWM */
+	INSN_CONFIG_PWM_SET_PERIOD = 5000,   /* sets frequency */
+	INSN_CONFIG_PWM_GET_PERIOD = 5001,   /* gets frequency */
+	INSN_CONFIG_GET_PWM_STATUS = 5002,          /* is it running? */
+	INSN_CONFIG_PWM_SET_H_BRIDGE = 5003, /* sets H bridge: duty cycle and sign bit for a relay  at the same time*/
+	INSN_CONFIG_PWM_GET_H_BRIDGE = 5004  /* gets H bridge data: duty cycle and the sign bit */
+};
+
+enum comedi_io_direction {
+	COMEDI_INPUT = 0,
+	COMEDI_OUTPUT = 1,
+	COMEDI_OPENDRAIN = 2
+};
+
+enum comedi_support_level {
+	COMEDI_UNKNOWN_SUPPORT = 0,
+	COMEDI_SUPPORTED,
+	COMEDI_UNSUPPORTED
+};
+
+/* ioctls */
+
+#define CIO 'd'
+#define COMEDI_DEVCONFIG _IOW(CIO, 0, comedi_devconfig)
+#define COMEDI_DEVINFO _IOR(CIO, 1, comedi_devinfo)
+#define COMEDI_SUBDINFO _IOR(CIO, 2, comedi_subdinfo)
+#define COMEDI_CHANINFO _IOR(CIO, 3, comedi_chaninfo)
+#define COMEDI_TRIG _IOWR(CIO, 4, comedi_trig)
+#define COMEDI_LOCK _IO(CIO, 5)
+#define COMEDI_UNLOCK _IO(CIO, 6)
+#define COMEDI_CANCEL _IO(CIO, 7)
+#define COMEDI_RANGEINFO _IOR(CIO, 8, comedi_rangeinfo)
+#define COMEDI_CMD _IOR(CIO, 9, comedi_cmd)
+#define COMEDI_CMDTEST _IOR(CIO, 10, comedi_cmd)
+#define COMEDI_INSNLIST _IOR(CIO, 11, comedi_insnlist)
+#define COMEDI_INSN _IOR(CIO, 12, comedi_insn)
+#define COMEDI_BUFCONFIG _IOR(CIO, 13, comedi_bufconfig)
+#define COMEDI_BUFINFO _IOWR(CIO, 14, comedi_bufinfo)
+#define COMEDI_POLL _IO(CIO, 15)
+
+/* structures */
+
+typedef struct comedi_trig_struct comedi_trig;
+typedef struct comedi_cmd_struct comedi_cmd;
+typedef struct comedi_insn_struct comedi_insn;
+typedef struct comedi_insnlist_struct comedi_insnlist;
+typedef struct comedi_chaninfo_struct comedi_chaninfo;
+typedef struct comedi_subdinfo_struct comedi_subdinfo;
+typedef struct comedi_devinfo_struct comedi_devinfo;
+typedef struct comedi_devconfig_struct comedi_devconfig;
+typedef struct comedi_rangeinfo_struct comedi_rangeinfo;
+typedef struct comedi_krange_struct comedi_krange;
+typedef struct comedi_bufconfig_struct comedi_bufconfig;
+typedef struct comedi_bufinfo_struct comedi_bufinfo;
+
+struct comedi_trig_struct {
+	unsigned int subdev;	/* subdevice */
+	unsigned int mode;	/* mode */
+	unsigned int flags;
+	unsigned int n_chan;	/* number of channels */
+	unsigned int *chanlist;	/* channel/range list */
+	sampl_t *data;	/* data list, size depends on subd flags */
+	unsigned int n;	/* number of scans */
+	unsigned int trigsrc;
+	unsigned int trigvar;
+	unsigned int trigvar1;
+	unsigned int data_len;
+	unsigned int unused[3];
+};
+
+struct comedi_insn_struct {
+	unsigned int insn;
+	unsigned int n;
+	lsampl_t *data;
+	unsigned int subdev;
+	unsigned int chanspec;
+	unsigned int unused[3];
+};
+
+struct comedi_insnlist_struct {
+	unsigned int n_insns;
+	comedi_insn *insns;
+};
+
+struct comedi_cmd_struct {
+	unsigned int subdev;
+	unsigned int flags;
+
+	unsigned int start_src;
+	unsigned int start_arg;
+
+	unsigned int scan_begin_src;
+	unsigned int scan_begin_arg;
+
+	unsigned int convert_src;
+	unsigned int convert_arg;
+
+	unsigned int scan_end_src;
+	unsigned int scan_end_arg;
+
+	unsigned int stop_src;
+	unsigned int stop_arg;
+
+	unsigned int *chanlist;	/* channel/range list */
+	unsigned int chanlist_len;
+
+	sampl_t *data;	/* data list, size depends on subd flags */
+	unsigned int data_len;
+};
+
+struct comedi_chaninfo_struct {
+	unsigned int subdev;
+	lsampl_t *maxdata_list;
+	unsigned int *flaglist;
+	unsigned int *rangelist;
+	unsigned int unused[4];
+};
+
+struct comedi_rangeinfo_struct {
+	unsigned int range_type;
+	void *range_ptr;
+};
+
+struct comedi_krange_struct {
+	int min;	/* fixed point, multiply by 1e-6 */
+	int max;	/* fixed point, multiply by 1e-6 */
+	unsigned int flags;
+};
+
+
+struct comedi_subdinfo_struct {
+	unsigned int type;
+	unsigned int n_chan;
+	unsigned int subd_flags;
+	unsigned int timer_type;
+	unsigned int len_chanlist;
+	lsampl_t maxdata;
+	unsigned int flags;	/* channel flags */
+	unsigned int range_type;	/* lookup in kernel */
+	unsigned int settling_time_0;
+	unsigned insn_bits_support;	/* see support_level enum for values*/
+	unsigned int unused[8];
+};
+
+struct comedi_devinfo_struct {
+	unsigned int version_code;
+	unsigned int n_subdevs;
+	char driver_name[COMEDI_NAMELEN];
+	char board_name[COMEDI_NAMELEN];
+	int read_subdevice;
+	int write_subdevice;
+	int unused[30];
+};
+
+struct comedi_devconfig_struct {
+	char board_name[COMEDI_NAMELEN];
+	int options[COMEDI_NDEVCONFOPTS];
+};
+
+struct comedi_bufconfig_struct {
+	unsigned int subdevice;
+	unsigned int flags;
+
+	unsigned int maximum_size;
+	unsigned int size;
+
+	unsigned int unused[4];
+};
+
+struct comedi_bufinfo_struct {
+	unsigned int subdevice;
+	unsigned int bytes_read;
+
+	unsigned int buf_write_ptr;
+	unsigned int buf_read_ptr;
+	unsigned int buf_write_count;
+	unsigned int buf_read_count;
+
+	unsigned int bytes_written;
+
+	unsigned int unused[4];
+};
+
+/* range stuff */
+
+#define __RANGE(a, b)	((((a)&0xffff)<<16)|((b)&0xffff))
+
+#define RANGE_OFFSET(a)		(((a)>>16)&0xffff)
+#define RANGE_LENGTH(b)		((b)&0xffff)
+
+#define RF_UNIT(flags)		((flags)&0xff)
+#define RF_EXTERNAL		(1<<8)
+
+#define UNIT_volt		0
+#define UNIT_mA			1
+#define UNIT_none		2
+
+#define COMEDI_MIN_SPEED	((unsigned int)0xffffffff)
+
+/* callback stuff */
+/* only relevant to kernel modules. */
+
+#define COMEDI_CB_EOS		1	/* end of scan */
+#define COMEDI_CB_EOA		2	/* end of acquisition */
+#define COMEDI_CB_BLOCK		4	/* DEPRECATED: convenient block size */
+#define COMEDI_CB_EOBUF		8	/* DEPRECATED: end of buffer */
+#define COMEDI_CB_ERROR		16	/* card error during acquisition */
+#define COMEDI_CB_OVERFLOW	32	/* buffer overflow/underflow */
+
+/**********************************************************/
+/* everything after this line is ALPHA */
+/**********************************************************/
+
+/*
+  8254 specific configuration.
+
+  It supports two config commands:
+
+  0 ID: INSN_CONFIG_SET_COUNTER_MODE
+  1 8254 Mode
+    I8254_MODE0, I8254_MODE1, ..., I8254_MODE5
+    OR'ed with:
+    I8254_BCD, I8254_BINARY
+
+  0 ID: INSN_CONFIG_8254_READ_STATUS
+  1 <-- Status byte returned here.
+    B7 = Output
+    B6 = NULL Count
+    B5 - B0 Current mode.
+
+*/
+
+enum i8254_mode {
+	I8254_MODE0 = (0 << 1),	/* Interrupt on terminal count */
+	I8254_MODE1 = (1 << 1),	/* Hardware retriggerable one-shot */
+	I8254_MODE2 = (2 << 1),	/* Rate generator */
+	I8254_MODE3 = (3 << 1),	/* Square wave mode */
+	I8254_MODE4 = (4 << 1),	/* Software triggered strobe */
+	I8254_MODE5 = (5 << 1),	/* Hardware triggered strobe (retriggerable) */
+	I8254_BCD = 1,	/* use binary-coded decimal instead of binary (pretty useless) */
+	I8254_BINARY = 0
+};
+
+static inline unsigned NI_USUAL_PFI_SELECT(unsigned pfi_channel)
+{
+	if (pfi_channel < 10)
+		return 0x1 + pfi_channel;
+	else
+		return 0xb + pfi_channel;
+}
+static inline unsigned NI_USUAL_RTSI_SELECT(unsigned rtsi_channel)
+{
+	if (rtsi_channel < 7)
+		return 0xb + rtsi_channel;
+	else
+		return 0x1b;
+}
+/* mode bits for NI general-purpose counters, set with
+ * INSN_CONFIG_SET_COUNTER_MODE */
+#define NI_GPCT_COUNTING_MODE_SHIFT 16
+#define NI_GPCT_INDEX_PHASE_BITSHIFT 20
+#define NI_GPCT_COUNTING_DIRECTION_SHIFT 24
+enum ni_gpct_mode_bits {
+	NI_GPCT_GATE_ON_BOTH_EDGES_BIT = 0x4,
+	NI_GPCT_EDGE_GATE_MODE_MASK = 0x18,
+	NI_GPCT_EDGE_GATE_STARTS_STOPS_BITS = 0x0,
+	NI_GPCT_EDGE_GATE_STOPS_STARTS_BITS = 0x8,
+	NI_GPCT_EDGE_GATE_STARTS_BITS = 0x10,
+	NI_GPCT_EDGE_GATE_NO_STARTS_NO_STOPS_BITS = 0x18,
+	NI_GPCT_STOP_MODE_MASK = 0x60,
+	NI_GPCT_STOP_ON_GATE_BITS = 0x00,
+	NI_GPCT_STOP_ON_GATE_OR_TC_BITS = 0x20,
+	NI_GPCT_STOP_ON_GATE_OR_SECOND_TC_BITS = 0x40,
+	NI_GPCT_LOAD_B_SELECT_BIT = 0x80,
+	NI_GPCT_OUTPUT_MODE_MASK = 0x300,
+	NI_GPCT_OUTPUT_TC_PULSE_BITS = 0x100,
+	NI_GPCT_OUTPUT_TC_TOGGLE_BITS = 0x200,
+	NI_GPCT_OUTPUT_TC_OR_GATE_TOGGLE_BITS = 0x300,
+	NI_GPCT_HARDWARE_DISARM_MASK = 0xc00,
+	NI_GPCT_NO_HARDWARE_DISARM_BITS = 0x000,
+	NI_GPCT_DISARM_AT_TC_BITS = 0x400,
+	NI_GPCT_DISARM_AT_GATE_BITS = 0x800,
+	NI_GPCT_DISARM_AT_TC_OR_GATE_BITS = 0xc00,
+	NI_GPCT_LOADING_ON_TC_BIT = 0x1000,
+	NI_GPCT_LOADING_ON_GATE_BIT = 0x4000,
+	NI_GPCT_COUNTING_MODE_MASK = 0x7 << NI_GPCT_COUNTING_MODE_SHIFT,
+	NI_GPCT_COUNTING_MODE_NORMAL_BITS =
+		0x0 << NI_GPCT_COUNTING_MODE_SHIFT,
+	NI_GPCT_COUNTING_MODE_QUADRATURE_X1_BITS =
+		0x1 << NI_GPCT_COUNTING_MODE_SHIFT,
+	NI_GPCT_COUNTING_MODE_QUADRATURE_X2_BITS =
+		0x2 << NI_GPCT_COUNTING_MODE_SHIFT,
+	NI_GPCT_COUNTING_MODE_QUADRATURE_X4_BITS =
+		0x3 << NI_GPCT_COUNTING_MODE_SHIFT,
+	NI_GPCT_COUNTING_MODE_TWO_PULSE_BITS =
+		0x4 << NI_GPCT_COUNTING_MODE_SHIFT,
+	NI_GPCT_COUNTING_MODE_SYNC_SOURCE_BITS =
+		0x6 << NI_GPCT_COUNTING_MODE_SHIFT,
+	NI_GPCT_INDEX_PHASE_MASK = 0x3 << NI_GPCT_INDEX_PHASE_BITSHIFT,
+	NI_GPCT_INDEX_PHASE_LOW_A_LOW_B_BITS =
+		0x0 << NI_GPCT_INDEX_PHASE_BITSHIFT,
+	NI_GPCT_INDEX_PHASE_LOW_A_HIGH_B_BITS =
+		0x1 << NI_GPCT_INDEX_PHASE_BITSHIFT,
+	NI_GPCT_INDEX_PHASE_HIGH_A_LOW_B_BITS =
+		0x2 << NI_GPCT_INDEX_PHASE_BITSHIFT,
+	NI_GPCT_INDEX_PHASE_HIGH_A_HIGH_B_BITS =
+		0x3 << NI_GPCT_INDEX_PHASE_BITSHIFT,
+	NI_GPCT_INDEX_ENABLE_BIT = 0x400000,
+	NI_GPCT_COUNTING_DIRECTION_MASK =
+		0x3 << NI_GPCT_COUNTING_DIRECTION_SHIFT,
+	NI_GPCT_COUNTING_DIRECTION_DOWN_BITS =
+		0x00 << NI_GPCT_COUNTING_DIRECTION_SHIFT,
+	NI_GPCT_COUNTING_DIRECTION_UP_BITS =
+		0x1 << NI_GPCT_COUNTING_DIRECTION_SHIFT,
+	NI_GPCT_COUNTING_DIRECTION_HW_UP_DOWN_BITS =
+		0x2 << NI_GPCT_COUNTING_DIRECTION_SHIFT,
+	NI_GPCT_COUNTING_DIRECTION_HW_GATE_BITS =
+		0x3 << NI_GPCT_COUNTING_DIRECTION_SHIFT,
+	NI_GPCT_RELOAD_SOURCE_MASK = 0xc000000,
+	NI_GPCT_RELOAD_SOURCE_FIXED_BITS = 0x0,
+	NI_GPCT_RELOAD_SOURCE_SWITCHING_BITS = 0x4000000,
+	NI_GPCT_RELOAD_SOURCE_GATE_SELECT_BITS = 0x8000000,
+	NI_GPCT_OR_GATE_BIT = 0x10000000,
+	NI_GPCT_INVERT_OUTPUT_BIT = 0x20000000
+};
+
+/* Bits for setting a clock source with
+ * INSN_CONFIG_SET_CLOCK_SRC when using NI general-purpose counters. */
+enum ni_gpct_clock_source_bits {
+	NI_GPCT_CLOCK_SRC_SELECT_MASK = 0x3f,
+	NI_GPCT_TIMEBASE_1_CLOCK_SRC_BITS = 0x0,
+	NI_GPCT_TIMEBASE_2_CLOCK_SRC_BITS = 0x1,
+	NI_GPCT_TIMEBASE_3_CLOCK_SRC_BITS = 0x2,
+	NI_GPCT_LOGIC_LOW_CLOCK_SRC_BITS = 0x3,
+	NI_GPCT_NEXT_GATE_CLOCK_SRC_BITS = 0x4,
+	NI_GPCT_NEXT_TC_CLOCK_SRC_BITS = 0x5,
+	NI_GPCT_SOURCE_PIN_i_CLOCK_SRC_BITS = 0x6,	/* NI 660x-specific */
+	NI_GPCT_PXI10_CLOCK_SRC_BITS = 0x7,
+	NI_GPCT_PXI_STAR_TRIGGER_CLOCK_SRC_BITS = 0x8,
+	NI_GPCT_ANALOG_TRIGGER_OUT_CLOCK_SRC_BITS = 0x9,
+	NI_GPCT_PRESCALE_MODE_CLOCK_SRC_MASK = 0x30000000,
+	NI_GPCT_NO_PRESCALE_CLOCK_SRC_BITS = 0x0,
+	NI_GPCT_PRESCALE_X2_CLOCK_SRC_BITS = 0x10000000, /* divide source by 2 */
+	NI_GPCT_PRESCALE_X8_CLOCK_SRC_BITS = 0x20000000, /* divide source by 8 */
+	NI_GPCT_INVERT_CLOCK_SRC_BIT = 0x80000000
+};
+static inline unsigned NI_GPCT_SOURCE_PIN_CLOCK_SRC_BITS(unsigned n)
+{
+	/* NI 660x-specific */
+	return 0x10 + n;
+}
+static inline unsigned NI_GPCT_RTSI_CLOCK_SRC_BITS(unsigned n)
+{
+	return 0x18 + n;
+}
+static inline unsigned NI_GPCT_PFI_CLOCK_SRC_BITS(unsigned n)
+{
+	/* no pfi on NI 660x */
+	return 0x20 + n;
+}
+
+/* Possibilities for setting a gate source with
+INSN_CONFIG_SET_GATE_SRC when using NI general-purpose counters.
+May be bitwise-or'd with CR_EDGE or CR_INVERT. */
+enum ni_gpct_gate_select {
+	/* m-series gates */
+	NI_GPCT_TIMESTAMP_MUX_GATE_SELECT = 0x0,
+	NI_GPCT_AI_START2_GATE_SELECT = 0x12,
+	NI_GPCT_PXI_STAR_TRIGGER_GATE_SELECT = 0x13,
+	NI_GPCT_NEXT_OUT_GATE_SELECT = 0x14,
+	NI_GPCT_AI_START1_GATE_SELECT = 0x1c,
+	NI_GPCT_NEXT_SOURCE_GATE_SELECT = 0x1d,
+	NI_GPCT_ANALOG_TRIGGER_OUT_GATE_SELECT = 0x1e,
+	NI_GPCT_LOGIC_LOW_GATE_SELECT = 0x1f,
+	/* more gates for 660x */
+	NI_GPCT_SOURCE_PIN_i_GATE_SELECT = 0x100,
+	NI_GPCT_GATE_PIN_i_GATE_SELECT = 0x101,
+	/* more gates for 660x "second gate" */
+	NI_GPCT_UP_DOWN_PIN_i_GATE_SELECT = 0x201,
+	NI_GPCT_SELECTED_GATE_GATE_SELECT = 0x21e,
+	/* m-series "second gate" sources are unknown,
+	   we should add them here with an offset of 0x300 when known. */
+	NI_GPCT_DISABLED_GATE_SELECT = 0x8000,
+};
+static inline unsigned NI_GPCT_GATE_PIN_GATE_SELECT(unsigned n)
+{
+	return 0x102 + n;
+}
+static inline unsigned NI_GPCT_RTSI_GATE_SELECT(unsigned n)
+{
+	return NI_USUAL_RTSI_SELECT(n);
+}
+static inline unsigned NI_GPCT_PFI_GATE_SELECT(unsigned n)
+{
+	return NI_USUAL_PFI_SELECT(n);
+}
+static inline unsigned NI_GPCT_UP_DOWN_PIN_GATE_SELECT(unsigned n)
+{
+	return 0x202 + n;
+}
+
+/* Possibilities for setting a source with
+INSN_CONFIG_SET_OTHER_SRC when using NI general-purpose counters. */
+enum ni_gpct_other_index {
+	NI_GPCT_SOURCE_ENCODER_A,
+	NI_GPCT_SOURCE_ENCODER_B,
+	NI_GPCT_SOURCE_ENCODER_Z
+};
+enum ni_gpct_other_select {
+	/* m-series gates */
+	/* Still unknown, probably only need NI_GPCT_PFI_OTHER_SELECT */
+	NI_GPCT_DISABLED_OTHER_SELECT = 0x8000,
+};
+static inline unsigned NI_GPCT_PFI_OTHER_SELECT(unsigned n)
+{
+	return NI_USUAL_PFI_SELECT(n);
+}
+
+/* start sources for ni general-purpose counters for use with
+INSN_CONFIG_ARM */
+enum ni_gpct_arm_source {
+	NI_GPCT_ARM_IMMEDIATE = 0x0,
+	NI_GPCT_ARM_PAIRED_IMMEDIATE = 0x1,	/* Start both the counter and
+						   the adjacent paired counter
+						   simultaneously */
+	/* NI doesn't document bits for selecting hardware arm triggers.  If
+	 * the NI_GPCT_ARM_UNKNOWN bit is set, we will pass the least
+	 * significant bits (3 bits for 660x or 5 bits for m-series) through to
+	 * the hardware.  This will at least allow someone to figure out what
+	 * the bits do later. */
+	NI_GPCT_ARM_UNKNOWN = 0x1000,
+};
+
+/* digital filtering options for ni 660x for use with INSN_CONFIG_FILTER. */
+enum ni_gpct_filter_select {
+	NI_GPCT_FILTER_OFF = 0x0,
+	NI_GPCT_FILTER_TIMEBASE_3_SYNC = 0x1,
+	NI_GPCT_FILTER_100x_TIMEBASE_1 = 0x2,
+	NI_GPCT_FILTER_20x_TIMEBASE_1 = 0x3,
+	NI_GPCT_FILTER_10x_TIMEBASE_1 = 0x4,
+	NI_GPCT_FILTER_2x_TIMEBASE_1 = 0x5,
+	NI_GPCT_FILTER_2x_TIMEBASE_3 = 0x6
+};
+
+/* PFI digital filtering options for ni m-series for use with
+ * INSN_CONFIG_FILTER. */
+enum ni_pfi_filter_select {
+	NI_PFI_FILTER_OFF = 0x0,
+	NI_PFI_FILTER_125ns = 0x1,
+	NI_PFI_FILTER_6425ns = 0x2,
+	NI_PFI_FILTER_2550us = 0x3
+};
+
+/* master clock sources for ni mio boards and INSN_CONFIG_SET_CLOCK_SRC */
+enum ni_mio_clock_source {
+	NI_MIO_INTERNAL_CLOCK = 0,
+	NI_MIO_RTSI_CLOCK = 1,	/* doesn't work for m-series, use
+				   NI_MIO_PLL_RTSI_CLOCK() */
+	/* the NI_MIO_PLL_* sources are m-series only */
+	NI_MIO_PLL_PXI_STAR_TRIGGER_CLOCK = 2,
+	NI_MIO_PLL_PXI10_CLOCK = 3,
+	NI_MIO_PLL_RTSI0_CLOCK = 4
+};
+static inline unsigned NI_MIO_PLL_RTSI_CLOCK(unsigned rtsi_channel)
+{
+	return NI_MIO_PLL_RTSI0_CLOCK + rtsi_channel;
+}
+
+/* Signals which can be routed to an NI RTSI pin with INSN_CONFIG_SET_ROUTING.
+ The numbers assigned are not arbitrary, they correspond to the bits required
+ to program the board. */
+enum ni_rtsi_routing {
+	NI_RTSI_OUTPUT_ADR_START1 = 0,
+	NI_RTSI_OUTPUT_ADR_START2 = 1,
+	NI_RTSI_OUTPUT_SCLKG = 2,
+	NI_RTSI_OUTPUT_DACUPDN = 3,
+	NI_RTSI_OUTPUT_DA_START1 = 4,
+	NI_RTSI_OUTPUT_G_SRC0 = 5,
+	NI_RTSI_OUTPUT_G_GATE0 = 6,
+	NI_RTSI_OUTPUT_RGOUT0 = 7,
+	NI_RTSI_OUTPUT_RTSI_BRD_0 = 8,
+	NI_RTSI_OUTPUT_RTSI_OSC = 12	/* pre-m-series always have RTSI clock
+					   on line 7 */
+};
+static inline unsigned NI_RTSI_OUTPUT_RTSI_BRD(unsigned n)
+{
+	return NI_RTSI_OUTPUT_RTSI_BRD_0 + n;
+}
+
+/* Signals which can be routed to an NI PFI pin on an m-series board with
+ * INSN_CONFIG_SET_ROUTING.  These numbers are also returned by
+ * INSN_CONFIG_GET_ROUTING on pre-m-series boards, even though their routing
+ * cannot be changed.  The numbers assigned are not arbitrary, they correspond
+ * to the bits required to program the board. */
+enum ni_pfi_routing {
+	NI_PFI_OUTPUT_PFI_DEFAULT = 0,
+	NI_PFI_OUTPUT_AI_START1 = 1,
+	NI_PFI_OUTPUT_AI_START2 = 2,
+	NI_PFI_OUTPUT_AI_CONVERT = 3,
+	NI_PFI_OUTPUT_G_SRC1 = 4,
+	NI_PFI_OUTPUT_G_GATE1 = 5,
+	NI_PFI_OUTPUT_AO_UPDATE_N = 6,
+	NI_PFI_OUTPUT_AO_START1 = 7,
+	NI_PFI_OUTPUT_AI_START_PULSE = 8,
+	NI_PFI_OUTPUT_G_SRC0 = 9,
+	NI_PFI_OUTPUT_G_GATE0 = 10,
+	NI_PFI_OUTPUT_EXT_STROBE = 11,
+	NI_PFI_OUTPUT_AI_EXT_MUX_CLK = 12,
+	NI_PFI_OUTPUT_GOUT0 = 13,
+	NI_PFI_OUTPUT_GOUT1 = 14,
+	NI_PFI_OUTPUT_FREQ_OUT = 15,
+	NI_PFI_OUTPUT_PFI_DO = 16,
+	NI_PFI_OUTPUT_I_ATRIG = 17,
+	NI_PFI_OUTPUT_RTSI0 = 18,
+	NI_PFI_OUTPUT_PXI_STAR_TRIGGER_IN = 26,
+	NI_PFI_OUTPUT_SCXI_TRIG1 = 27,
+	NI_PFI_OUTPUT_DIO_CHANGE_DETECT_RTSI = 28,
+	NI_PFI_OUTPUT_CDI_SAMPLE = 29,
+	NI_PFI_OUTPUT_CDO_UPDATE = 30
+};
+static inline unsigned NI_PFI_OUTPUT_RTSI(unsigned rtsi_channel)
+{
+	return NI_PFI_OUTPUT_RTSI0 + rtsi_channel;
+}
+
+/* Signals which can be routed to output on a NI PFI pin on a 660x board
+ with INSN_CONFIG_SET_ROUTING.  The numbers assigned are
+ not arbitrary, they correspond to the bits required
+ to program the board.  Lines 0 to 7 can only be set to
+ NI_660X_PFI_OUTPUT_DIO.  Lines 32 to 39 can only be set to
+ NI_660X_PFI_OUTPUT_COUNTER. */
+enum ni_660x_pfi_routing {
+	NI_660X_PFI_OUTPUT_COUNTER = 1,	/* counter */
+	NI_660X_PFI_OUTPUT_DIO = 2,	/* static digital output */
+};
+
+/* NI External Trigger lines.  These values are not arbitrary, but are related
+ * to the bits required to program the board (offset by 1 for historical
+ * reasons). */
+static inline unsigned NI_EXT_PFI(unsigned pfi_channel)
+{
+	return NI_USUAL_PFI_SELECT(pfi_channel) - 1;
+}
+static inline unsigned NI_EXT_RTSI(unsigned rtsi_channel)
+{
+	return NI_USUAL_RTSI_SELECT(rtsi_channel) - 1;
+}
+
+/* status bits for INSN_CONFIG_GET_COUNTER_STATUS */
+enum comedi_counter_status_flags {
+	COMEDI_COUNTER_ARMED = 0x1,
+	COMEDI_COUNTER_COUNTING = 0x2,
+	COMEDI_COUNTER_TERMINAL_COUNT = 0x4,
+};
+
+/* Clock sources for CDIO subdevice on NI m-series boards.  Used as the
+ * scan_begin_arg for a comedi_command. These sources may also be bitwise-or'd
+ * with CR_INVERT to change polarity. */
+enum ni_m_series_cdio_scan_begin_src {
+	NI_CDIO_SCAN_BEGIN_SRC_GROUND = 0,
+	NI_CDIO_SCAN_BEGIN_SRC_AI_START = 18,
+	NI_CDIO_SCAN_BEGIN_SRC_AI_CONVERT = 19,
+	NI_CDIO_SCAN_BEGIN_SRC_PXI_STAR_TRIGGER = 20,
+	NI_CDIO_SCAN_BEGIN_SRC_G0_OUT = 28,
+	NI_CDIO_SCAN_BEGIN_SRC_G1_OUT = 29,
+	NI_CDIO_SCAN_BEGIN_SRC_ANALOG_TRIGGER = 30,
+	NI_CDIO_SCAN_BEGIN_SRC_AO_UPDATE = 31,
+	NI_CDIO_SCAN_BEGIN_SRC_FREQ_OUT = 32,
+	NI_CDIO_SCAN_BEGIN_SRC_DIO_CHANGE_DETECT_IRQ = 33
+};
+static inline unsigned NI_CDIO_SCAN_BEGIN_SRC_PFI(unsigned pfi_channel)
+{
+	return NI_USUAL_PFI_SELECT(pfi_channel);
+}
+static inline unsigned NI_CDIO_SCAN_BEGIN_SRC_RTSI(unsigned rtsi_channel)
+{
+	return NI_USUAL_RTSI_SELECT(rtsi_channel);
+}
+
+/* scan_begin_src for scan_begin_arg==TRIG_EXT with analog output command on NI
+ * boards.  These scan begin sources can also be bitwise-or'd with CR_INVERT to
+ * change polarity. */
+static inline unsigned NI_AO_SCAN_BEGIN_SRC_PFI(unsigned pfi_channel)
+{
+	return NI_USUAL_PFI_SELECT(pfi_channel);
+}
+static inline unsigned NI_AO_SCAN_BEGIN_SRC_RTSI(unsigned rtsi_channel)
+{
+	return NI_USUAL_RTSI_SELECT(rtsi_channel);
+}
+
+/* Bits for setting a clock source with
+ * INSN_CONFIG_SET_CLOCK_SRC when using NI frequency output subdevice. */
+enum ni_freq_out_clock_source_bits {
+	NI_FREQ_OUT_TIMEBASE_1_DIV_2_CLOCK_SRC,	/* 10 MHz */
+	NI_FREQ_OUT_TIMEBASE_2_CLOCK_SRC	/* 100 KHz */
+};
+
+/* Values for setting a clock source with INSN_CONFIG_SET_CLOCK_SRC for
+ * 8254 counter subdevices on Amplicon DIO boards (amplc_dio200 driver). */
+	enum amplc_dio_clock_source {
+		AMPLC_DIO_CLK_CLKN,	/* per channel external clock
+					   input/output pin (pin is only an
+					   input when clock source set to this
+					   value, otherwise it is an output) */
+		AMPLC_DIO_CLK_10MHZ,	/* 10 MHz internal clock */
+		AMPLC_DIO_CLK_1MHZ,	/* 1 MHz internal clock */
+		AMPLC_DIO_CLK_100KHZ,	/* 100 kHz internal clock */
+		AMPLC_DIO_CLK_10KHZ,	/* 10 kHz internal clock */
+		AMPLC_DIO_CLK_1KHZ,	/* 1 kHz internal clock */
+		AMPLC_DIO_CLK_OUTNM1,	/* output of preceding counter channel
+					   (for channel 0, preceding counter
+					   channel is channel 2 on preceding
+					   counter subdevice, for first counter
+					   subdevice, preceding counter
+					   subdevice is the last counter
+					   subdevice) */
+		AMPLC_DIO_CLK_EXT	/* per chip external input pin */
+	};
+
+/* Values for setting a gate source with INSN_CONFIG_SET_GATE_SRC for
+ * 8254 counter subdevices on Amplicon DIO boards (amplc_dio200 driver). */
+	enum amplc_dio_gate_source {
+		AMPLC_DIO_GAT_VCC,	/* internal high logic level */
+		AMPLC_DIO_GAT_GND,	/* internal low logic level */
+		AMPLC_DIO_GAT_GATN,	/* per channel external gate input */
+		AMPLC_DIO_GAT_NOUTNM2,	/* negated output of counter channel
+					   minus 2 (for channels 0 or 1,
+					   channel minus 2 is channel 1 or 2 on
+					   the preceding counter subdevice, for
+					   the first counter subdevice the
+					   preceding counter subdevice is the
+					   last counter subdevice) */
+		AMPLC_DIO_GAT_RESERVED4,
+		AMPLC_DIO_GAT_RESERVED5,
+		AMPLC_DIO_GAT_RESERVED6,
+		AMPLC_DIO_GAT_RESERVED7
+	};
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _COMEDI_H */
diff --git a/drivers/staging/comedi/comedi_compat32.c b/drivers/staging/comedi/comedi_compat32.c
new file mode 100644
index 00000000000..7d0116bcb9f
--- /dev/null
+++ b/drivers/staging/comedi/comedi_compat32.c
@@ -0,0 +1,597 @@
+/*
+    comedi/comedi_compat32.c
+    32-bit ioctl compatibility for 64-bit comedi kernel module.
+
+    Author: Ian Abbott, MEV Ltd. <abbotti@mev.co.uk>
+    Copyright (C) 2007 MEV Ltd. <http://www.mev.co.uk/>
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1997-2007 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#define __NO_VERSION__
+#include "comedi.h"
+#include <linux/smp_lock.h>
+#include <asm/uaccess.h>
+
+#include "comedi_compat32.h"
+
+#ifdef CONFIG_COMPAT
+
+#ifndef HAVE_COMPAT_IOCTL
+#include <linux/ioctl32.h>	/* for (un)register_ioctl32_conversion */
+#endif
+
+#define COMEDI32_CHANINFO _IOR(CIO,3,comedi32_chaninfo)
+#define COMEDI32_RANGEINFO _IOR(CIO,8,comedi32_rangeinfo)
+/* N.B. COMEDI32_CMD and COMEDI_CMD ought to use _IOWR, not _IOR.
+ * It's too late to change it now, but it only affects the command number. */
+#define COMEDI32_CMD _IOR(CIO,9,comedi32_cmd)
+/* N.B. COMEDI32_CMDTEST and COMEDI_CMDTEST ought to use _IOWR, not _IOR.
+ * It's too late to change it now, but it only affects the command number. */
+#define COMEDI32_CMDTEST _IOR(CIO,10,comedi32_cmd)
+#define COMEDI32_INSNLIST _IOR(CIO,11,comedi32_insnlist)
+#define COMEDI32_INSN _IOR(CIO,12,comedi32_insn)
+
+typedef struct comedi32_chaninfo_struct {
+	unsigned int subdev;
+	compat_uptr_t maxdata_list;	/* 32-bit 'lsampl_t *' */
+	compat_uptr_t flaglist;		/* 32-bit 'unsigned int *' */
+	compat_uptr_t rangelist;	/* 32-bit 'unsigned int *' */
+	unsigned int unused[4];
+} comedi32_chaninfo;
+
+typedef struct comedi32_rangeinfo_struct {
+	unsigned int range_type;
+	compat_uptr_t range_ptr;	/* 32-bit 'void *' */
+} comedi32_rangeinfo;
+
+typedef struct comedi32_cmd_struct {
+	unsigned int subdev;
+	unsigned int flags;
+	unsigned int start_src;
+	unsigned int start_arg;
+	unsigned int scan_begin_src;
+	unsigned int scan_begin_arg;
+	unsigned int convert_src;
+	unsigned int convert_arg;
+	unsigned int scan_end_src;
+	unsigned int scan_end_arg;
+	unsigned int stop_src;
+	unsigned int stop_arg;
+	compat_uptr_t chanlist;		/* 32-bit 'unsigned int *' */
+	unsigned int chanlist_len;
+	compat_uptr_t data;		/* 32-bit 'sampl_t *' */
+	unsigned int data_len;
+} comedi32_cmd;
+
+typedef struct comedi32_insn_struct {
+	unsigned int insn;
+	unsigned int n;
+	compat_uptr_t data;		/* 32-bit 'lsampl_t *' */
+	unsigned int subdev;
+	unsigned int chanspec;
+	unsigned int unused[3];
+} comedi32_insn;
+
+typedef struct comedi32_insnlist_struct {
+	unsigned int n_insns;
+	compat_uptr_t insns;		/* 32-bit 'comedi_insn *' */
+} comedi32_insnlist;
+
+/* Handle translated ioctl. */
+static int translated_ioctl(struct file *file, unsigned int cmd,
+		unsigned long arg)
+{
+	if (!file->f_op) {
+		return -ENOTTY;
+	}
+#ifdef HAVE_UNLOCKED_IOCTL
+	if (file->f_op->unlocked_ioctl) {
+		int rc = (int)(*file->f_op->unlocked_ioctl)(file, cmd, arg);
+		if (rc == -ENOIOCTLCMD) {
+			rc = -ENOTTY;
+		}
+		return rc;
+	}
+#endif
+	if (file->f_op->ioctl) {
+		int rc;
+		lock_kernel();
+		rc = (*file->f_op->ioctl)(file->f_dentry->d_inode,
+				file, cmd, arg);
+		unlock_kernel();
+		return rc;
+	}
+	return -ENOTTY;
+}
+
+/* Handle 32-bit COMEDI_CHANINFO ioctl. */
+static int compat_chaninfo(struct file *file, unsigned long arg)
+{
+	comedi_chaninfo __user *chaninfo;
+	comedi32_chaninfo __user *chaninfo32;
+	int err;
+	union {
+		unsigned int uint;
+		compat_uptr_t uptr;
+	} temp;
+
+	chaninfo32 = compat_ptr(arg);
+	chaninfo = compat_alloc_user_space(sizeof(*chaninfo));
+
+	/* Copy chaninfo structure.  Ignore unused members. */
+	if (!access_ok(VERIFY_READ, chaninfo32, sizeof(*chaninfo32))
+			|| !access_ok(VERIFY_WRITE, chaninfo,
+				sizeof(*chaninfo))) {
+		return -EFAULT;
+	}
+	err = 0;
+	err |= __get_user(temp.uint, &chaninfo32->subdev);
+	err |= __put_user(temp.uint, &chaninfo->subdev);
+	err |= __get_user(temp.uptr, &chaninfo32->maxdata_list);
+	err |= __put_user(compat_ptr(temp.uptr), &chaninfo->maxdata_list);
+	err |= __get_user(temp.uptr, &chaninfo32->flaglist);
+	err |= __put_user(compat_ptr(temp.uptr), &chaninfo->flaglist);
+	err |= __get_user(temp.uptr, &chaninfo32->rangelist);
+	err |= __put_user(compat_ptr(temp.uptr), &chaninfo->rangelist);
+	if (err) {
+		return -EFAULT;
+	}
+
+	return translated_ioctl(file, COMEDI_CHANINFO, (unsigned long)chaninfo);
+}
+
+/* Handle 32-bit COMEDI_RANGEINFO ioctl. */
+static int compat_rangeinfo(struct file *file, unsigned long arg)
+{
+	comedi_rangeinfo __user *rangeinfo;
+	comedi32_rangeinfo __user *rangeinfo32;
+	int err;
+	union {
+		unsigned int uint;
+		compat_uptr_t uptr;
+	} temp;
+
+	rangeinfo32 = compat_ptr(arg);
+	rangeinfo = compat_alloc_user_space(sizeof(*rangeinfo));
+
+	/* Copy rangeinfo structure. */
+	if (!access_ok(VERIFY_READ, rangeinfo32, sizeof(*rangeinfo32))
+			|| !access_ok(VERIFY_WRITE, rangeinfo,
+				sizeof(*rangeinfo))) {
+		return -EFAULT;
+	}
+	err = 0;
+	err |= __get_user(temp.uint, &rangeinfo32->range_type);
+	err |= __put_user(temp.uint, &rangeinfo->range_type);
+	err |= __get_user(temp.uptr, &rangeinfo32->range_ptr);
+	err |= __put_user(compat_ptr(temp.uptr), &rangeinfo->range_ptr);
+	if (err) {
+		return -EFAULT;
+	}
+
+	return translated_ioctl(file, COMEDI_RANGEINFO,
+			(unsigned long)rangeinfo);
+}
+
+/* Copy 32-bit cmd structure to native cmd structure. */
+static int get_compat_cmd(comedi_cmd __user *cmd,
+		comedi32_cmd __user *cmd32)
+{
+	int err;
+	union {
+		unsigned int uint;
+		compat_uptr_t uptr;
+	} temp;
+
+	/* Copy cmd structure. */
+	if (!access_ok(VERIFY_READ, cmd32, sizeof(*cmd32))
+			|| !access_ok(VERIFY_WRITE, cmd, sizeof(*cmd))) {
+		return -EFAULT;
+	}
+	err = 0;
+	err |= __get_user(temp.uint, &cmd32->subdev);
+	err |= __put_user(temp.uint, &cmd->subdev);
+	err |= __get_user(temp.uint, &cmd32->flags);
+	err |= __put_user(temp.uint, &cmd->flags);
+	err |= __get_user(temp.uint, &cmd32->start_src);
+	err |= __put_user(temp.uint, &cmd->start_src);
+	err |= __get_user(temp.uint, &cmd32->start_arg);
+	err |= __put_user(temp.uint, &cmd->start_arg);
+	err |= __get_user(temp.uint, &cmd32->scan_begin_src);
+	err |= __put_user(temp.uint, &cmd->scan_begin_src);
+	err |= __get_user(temp.uint, &cmd32->scan_begin_arg);
+	err |= __put_user(temp.uint, &cmd->scan_begin_arg);
+	err |= __get_user(temp.uint, &cmd32->convert_src);
+	err |= __put_user(temp.uint, &cmd->convert_src);
+	err |= __get_user(temp.uint, &cmd32->convert_arg);
+	err |= __put_user(temp.uint, &cmd->convert_arg);
+	err |= __get_user(temp.uint, &cmd32->scan_end_src);
+	err |= __put_user(temp.uint, &cmd->scan_end_src);
+	err |= __get_user(temp.uint, &cmd32->scan_end_arg);
+	err |= __put_user(temp.uint, &cmd->scan_end_arg);
+	err |= __get_user(temp.uint, &cmd32->stop_src);
+	err |= __put_user(temp.uint, &cmd->stop_src);
+	err |= __get_user(temp.uint, &cmd32->stop_arg);
+	err |= __put_user(temp.uint, &cmd->stop_arg);
+	err |= __get_user(temp.uptr, &cmd32->chanlist);
+	err |= __put_user(compat_ptr(temp.uptr), &cmd->chanlist);
+	err |= __get_user(temp.uint, &cmd32->chanlist_len);
+	err |= __put_user(temp.uint, &cmd->chanlist_len);
+	err |= __get_user(temp.uptr, &cmd32->data);
+	err |= __put_user(compat_ptr(temp.uptr), &cmd->data);
+	err |= __get_user(temp.uint, &cmd32->data_len);
+	err |= __put_user(temp.uint, &cmd->data_len);
+	return err ? -EFAULT : 0;
+}
+
+/* Copy native cmd structure to 32-bit cmd structure. */
+static int put_compat_cmd(comedi32_cmd __user *cmd32, comedi_cmd __user *cmd)
+{
+	int err;
+	unsigned int temp;
+
+	/* Copy back most of cmd structure. */
+	/* Assume the pointer values are already valid. */
+	/* (Could use ptr_to_compat() to set them, but that wasn't implemented
+	 * until kernel version 2.6.11.) */
+	if (!access_ok(VERIFY_READ, cmd, sizeof(*cmd))
+			|| !access_ok(VERIFY_WRITE, cmd32, sizeof(*cmd32))) {
+		return -EFAULT;
+	}
+	err = 0;
+	err |= __get_user(temp, &cmd->subdev);
+	err |= __put_user(temp, &cmd32->subdev);
+	err |= __get_user(temp, &cmd->flags);
+	err |= __put_user(temp, &cmd32->flags);
+	err |= __get_user(temp, &cmd->start_src);
+	err |= __put_user(temp, &cmd32->start_src);
+	err |= __get_user(temp, &cmd->start_arg);
+	err |= __put_user(temp, &cmd32->start_arg);
+	err |= __get_user(temp, &cmd->scan_begin_src);
+	err |= __put_user(temp, &cmd32->scan_begin_src);
+	err |= __get_user(temp, &cmd->scan_begin_arg);
+	err |= __put_user(temp, &cmd32->scan_begin_arg);
+	err |= __get_user(temp, &cmd->convert_src);
+	err |= __put_user(temp, &cmd32->convert_src);
+	err |= __get_user(temp, &cmd->convert_arg);
+	err |= __put_user(temp, &cmd32->convert_arg);
+	err |= __get_user(temp, &cmd->scan_end_src);
+	err |= __put_user(temp, &cmd32->scan_end_src);
+	err |= __get_user(temp, &cmd->scan_end_arg);
+	err |= __put_user(temp, &cmd32->scan_end_arg);
+	err |= __get_user(temp, &cmd->stop_src);
+	err |= __put_user(temp, &cmd32->stop_src);
+	err |= __get_user(temp, &cmd->stop_arg);
+	err |= __put_user(temp, &cmd32->stop_arg);
+	/* Assume chanlist pointer is unchanged. */
+	err |= __get_user(temp, &cmd->chanlist_len);
+	err |= __put_user(temp, &cmd32->chanlist_len);
+	/* Assume data pointer is unchanged. */
+	err |= __get_user(temp, &cmd->data_len);
+	err |= __put_user(temp, &cmd32->data_len);
+	return err ? -EFAULT : 0;
+}
+
+/* Handle 32-bit COMEDI_CMD ioctl. */
+static int compat_cmd(struct file *file, unsigned long arg)
+{
+	comedi_cmd __user *cmd;
+	comedi32_cmd __user *cmd32;
+	int rc;
+
+	cmd32 = compat_ptr(arg);
+	cmd = compat_alloc_user_space(sizeof(*cmd));
+
+	rc = get_compat_cmd(cmd, cmd32);
+	if (rc) {
+		return rc;
+	}
+
+	return translated_ioctl(file, COMEDI_CMD, (unsigned long)cmd);
+}
+
+/* Handle 32-bit COMEDI_CMDTEST ioctl. */
+static int compat_cmdtest(struct file *file, unsigned long arg)
+{
+	comedi_cmd __user *cmd;
+	comedi32_cmd __user *cmd32;
+	int rc, err;
+
+	cmd32 = compat_ptr(arg);
+	cmd = compat_alloc_user_space(sizeof(*cmd));
+
+	rc = get_compat_cmd(cmd, cmd32);
+	if (rc) {
+		return rc;
+	}
+
+	rc = translated_ioctl(file, COMEDI_CMDTEST, (unsigned long)cmd);
+	if (rc < 0) {
+		return rc;
+	}
+
+	err = put_compat_cmd(cmd32, cmd);
+	if (err) {
+		rc = err;
+	}
+	return rc;
+}
+
+/* Copy 32-bit insn structure to native insn structure. */
+static int get_compat_insn(comedi_insn __user *insn,
+		comedi32_insn __user *insn32)
+{
+	int err;
+	union {
+		unsigned int uint;
+		compat_uptr_t uptr;
+	} temp;
+
+	/* Copy insn structure.  Ignore the unused members. */
+	err = 0;
+	if (!access_ok(VERIFY_READ, insn32, sizeof(*insn32))
+			|| !access_ok(VERIFY_WRITE, insn, sizeof(*insn))) {
+		return -EFAULT;
+	}
+	err |= __get_user(temp.uint, &insn32->insn);
+	err |= __put_user(temp.uint, &insn->insn);
+	err |= __get_user(temp.uint, &insn32->n);
+	err |= __put_user(temp.uint, &insn->n);
+	err |= __get_user(temp.uptr, &insn32->data);
+	err |= __put_user(compat_ptr(temp.uptr), &insn->data);
+	err |= __get_user(temp.uint, &insn32->subdev);
+	err |= __put_user(temp.uint, &insn->subdev);
+	err |= __get_user(temp.uint, &insn32->chanspec);
+	err |= __put_user(temp.uint, &insn->chanspec);
+	return err ? -EFAULT : 0;
+}
+
+/* Handle 32-bit COMEDI_INSNLIST ioctl. */
+static int compat_insnlist(struct file *file, unsigned long arg)
+{
+	struct combined_insnlist {
+		comedi_insnlist insnlist;
+		comedi_insn insn[1];
+	} __user *s;
+	comedi32_insnlist __user *insnlist32;
+	comedi32_insn __user *insn32;
+	compat_uptr_t uptr;
+	unsigned int n_insns, n;
+	int err, rc;
+
+	insnlist32 = compat_ptr(arg);
+
+	/* Get 32-bit insnlist structure.  */
+	if (!access_ok(VERIFY_READ, insnlist32, sizeof(*insnlist32))) {
+		return -EFAULT;
+	}
+	err = 0;
+	err |= __get_user(n_insns, &insnlist32->n_insns);
+	err |= __get_user(uptr, &insnlist32->insns);
+	insn32 = compat_ptr(uptr);
+	if (err) {
+		return -EFAULT;
+	}
+
+	/* Allocate user memory to copy insnlist and insns into. */
+	s = compat_alloc_user_space(offsetof(struct combined_insnlist,
+				insn[n_insns]));
+
+	/* Set native insnlist structure. */
+	if (!access_ok(VERIFY_WRITE, &s->insnlist, sizeof(s->insnlist))) {
+		return -EFAULT;
+	}
+	err |= __put_user(n_insns, &s->insnlist.n_insns);
+	err |= __put_user(&s->insn[0], &s->insnlist.insns);
+	if (err) {
+		return -EFAULT;
+	}
+
+	/* Copy insn structures. */
+	for (n = 0; n < n_insns; n++) {
+		rc = get_compat_insn(&s->insn[n], &insn32[n]);
+		if (rc) {
+			return rc;
+		}
+	}
+
+	return translated_ioctl(file, COMEDI_INSNLIST,
+			(unsigned long)&s->insnlist);
+}
+
+/* Handle 32-bit COMEDI_INSN ioctl. */
+static int compat_insn(struct file *file, unsigned long arg)
+{
+	comedi_insn __user *insn;
+	comedi32_insn __user *insn32;
+	int rc;
+
+	insn32 = compat_ptr(arg);
+	insn = compat_alloc_user_space(sizeof(*insn));
+
+	rc = get_compat_insn(insn, insn32);
+	if (rc) {
+		return rc;
+	}
+
+	return translated_ioctl(file, COMEDI_INSN, (unsigned long)insn);
+}
+
+/* Process untranslated ioctl. */
+/* Returns -ENOIOCTLCMD for unrecognised ioctl codes. */
+static inline int raw_ioctl(struct file *file, unsigned int cmd,
+		unsigned long arg)
+{
+	int rc;
+
+	switch (cmd) {
+	case COMEDI_DEVCONFIG:
+	case COMEDI_DEVINFO:
+	case COMEDI_SUBDINFO:
+	case COMEDI_BUFCONFIG:
+	case COMEDI_BUFINFO:
+		/* Just need to translate the pointer argument. */
+		arg = (unsigned long)compat_ptr(arg);
+		rc = translated_ioctl(file, cmd, arg);
+		break;
+	case COMEDI_LOCK:
+	case COMEDI_UNLOCK:
+	case COMEDI_CANCEL:
+	case COMEDI_POLL:
+		/* No translation needed. */
+		rc = translated_ioctl(file, cmd, arg);
+		break;
+	case COMEDI32_CHANINFO:
+		rc = compat_chaninfo(file, arg);
+		break;
+	case COMEDI32_RANGEINFO:
+		rc = compat_rangeinfo(file, arg);
+		break;
+	case COMEDI32_CMD:
+		rc = compat_cmd(file, arg);
+		break;
+	case COMEDI32_CMDTEST:
+		rc = compat_cmdtest(file, arg);
+		break;
+	case COMEDI32_INSNLIST:
+		rc = compat_insnlist(file, arg);
+		break;
+	case COMEDI32_INSN:
+		rc = compat_insn(file, arg);
+		break;
+	default:
+		rc = -ENOIOCTLCMD;
+		break;
+	}
+	return rc;
+}
+
+#ifdef HAVE_COMPAT_IOCTL	/* defined in <linux/fs.h> 2.6.11 onwards */
+
+/* compat_ioctl file operation. */
+/* Returns -ENOIOCTLCMD for unrecognised ioctl codes. */
+long comedi_compat_ioctl(struct file *file, unsigned int cmd,
+		unsigned long arg)
+{
+	return raw_ioctl(file, cmd, arg);
+}
+
+#else /* HAVE_COMPAT_IOCTL */
+
+/*
+ * Brain-dead ioctl compatibility for 2.6.10 and earlier.
+ *
+ * It's brain-dead because cmd numbers need to be unique system-wide!
+ * The comedi driver could end up attempting to execute ioctls for non-Comedi
+ * devices because it registered the system-wide cmd code first.  Similarly,
+ * another driver could end up attempting to execute ioctls for a Comedi
+ * device because it registered the cmd code first.  Chaos ensues.
+ */
+
+/* Handler for all 32-bit ioctl codes registered by this driver. */
+static int mapped_ioctl(unsigned int fd, unsigned int cmd, unsigned long arg,
+		struct file *file)
+{
+	int rc;
+
+	/* Make sure we are dealing with a Comedi device. */
+	if (imajor(file->f_dentry->d_inode) != COMEDI_MAJOR) {
+		return -ENOTTY;
+	}
+	rc = raw_ioctl(file, cmd, arg);
+	/* Do not return -ENOIOCTLCMD. */
+	if (rc == -ENOIOCTLCMD) {
+		rc = -ENOTTY;
+	}
+	return rc;
+}
+
+struct ioctl32_map {
+	unsigned int cmd;
+	int (*handler)(unsigned int, unsigned int, unsigned long,
+			struct file *);
+	int registered;
+};
+
+static struct ioctl32_map comedi_ioctl32_map[] = {
+	{ COMEDI_DEVCONFIG, mapped_ioctl, 0 },
+	{ COMEDI_DEVINFO, mapped_ioctl, 0 },
+	{ COMEDI_SUBDINFO, mapped_ioctl, 0 },
+	{ COMEDI_BUFCONFIG, mapped_ioctl, 0 },
+	{ COMEDI_BUFINFO, mapped_ioctl, 0 },
+	{ COMEDI_LOCK, mapped_ioctl, 0 },
+	{ COMEDI_UNLOCK, mapped_ioctl, 0 },
+	{ COMEDI_CANCEL, mapped_ioctl, 0 },
+	{ COMEDI_POLL, mapped_ioctl, 0 },
+	{ COMEDI32_CHANINFO, mapped_ioctl, 0 },
+	{ COMEDI32_RANGEINFO, mapped_ioctl, 0 },
+	{ COMEDI32_CMD, mapped_ioctl, 0 },
+	{ COMEDI32_CMDTEST, mapped_ioctl, 0 },
+	{ COMEDI32_INSNLIST, mapped_ioctl, 0 },
+	{ COMEDI32_INSN, mapped_ioctl, 0 },
+};
+
+#define NUM_IOCTL32_MAPS ARRAY_SIZE(comedi_ioctl32_map)
+
+/* Register system-wide 32-bit ioctl handlers. */
+void comedi_register_ioctl32(void)
+{
+	int n, rc;
+
+	for (n = 0; n < NUM_IOCTL32_MAPS; n++) {
+		rc = register_ioctl32_conversion(comedi_ioctl32_map[n].cmd,
+				comedi_ioctl32_map[n].handler);
+		if (rc) {
+			printk(KERN_WARNING
+					"comedi: failed to register 32-bit "
+					"compatible ioctl handler for 0x%X - "
+					"expect bad things to happen!\n",
+					comedi_ioctl32_map[n].cmd);
+		}
+		comedi_ioctl32_map[n].registered = !rc;
+	}
+}
+
+/* Unregister system-wide 32-bit ioctl translations. */
+void comedi_unregister_ioctl32(void)
+{
+	int n, rc;
+
+	for (n = 0; n < NUM_IOCTL32_MAPS; n++) {
+		if (comedi_ioctl32_map[n].registered) {
+			rc = unregister_ioctl32_conversion(
+					comedi_ioctl32_map[n].cmd,
+					comedi_ioctl32_map[n].handler);
+			if (rc) {
+				printk(KERN_ERR
+					"comedi: failed to unregister 32-bit "
+					"compatible ioctl handler for 0x%X - "
+					"expect kernel Oops!\n",
+					comedi_ioctl32_map[n].cmd);
+			} else {
+				comedi_ioctl32_map[n].registered = 0;
+			}
+		}
+	}
+}
+
+#endif	/* HAVE_COMPAT_IOCTL */
+
+#endif	/* CONFIG_COMPAT */
diff --git a/drivers/staging/comedi/comedi_compat32.h b/drivers/staging/comedi/comedi_compat32.h
new file mode 100644
index 00000000000..0ca01642c16
--- /dev/null
+++ b/drivers/staging/comedi/comedi_compat32.h
@@ -0,0 +1,58 @@
+/*
+    comedi/comedi_compat32.h
+    32-bit ioctl compatibility for 64-bit comedi kernel module.
+
+    Author: Ian Abbott, MEV Ltd. <abbotti@mev.co.uk>
+    Copyright (C) 2007 MEV Ltd. <http://www.mev.co.uk/>
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1997-2007 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#ifndef _COMEDI_COMPAT32_H
+#define _COMEDI_COMPAT32_H
+
+#include <linux/compat.h>
+#include <linux/fs.h>	/* For HAVE_COMPAT_IOCTL and HAVE_UNLOCKED_IOCTL */
+
+#ifdef CONFIG_COMPAT
+
+#ifdef HAVE_COMPAT_IOCTL
+
+extern long comedi_compat_ioctl(struct file *file, unsigned int cmd,
+		unsigned long arg);
+#define comedi_register_ioctl32() do {} while (0)
+#define comedi_unregister_ioctl32() do {} while (0)
+
+#else /* HAVE_COMPAT_IOCTL */
+
+#define comedi_compat_ioctl 0	/* NULL */
+extern void comedi_register_ioctl32(void);
+extern void comedi_unregister_ioctl32(void);
+
+#endif /* HAVE_COMPAT_IOCTL */
+
+#else /* CONFIG_COMPAT */
+
+#define comedi_compat_ioctl 0	/* NULL */
+#define comedi_register_ioctl32() do {} while (0)
+#define comedi_unregister_ioctl32() do {} while (0)
+
+#endif /* CONFIG_COMPAT */
+
+#endif /* _COMEDI_COMPAT32_H */
diff --git a/drivers/staging/comedi/comedi_fops.c b/drivers/staging/comedi/comedi_fops.c
new file mode 100644
index 00000000000..018c964396d
--- /dev/null
+++ b/drivers/staging/comedi/comedi_fops.c
@@ -0,0 +1,2244 @@
+/*
+    comedi/comedi_fops.c
+    comedi kernel module
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1997-2000 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#undef DEBUG
+
+#define __NO_VERSION__
+#include "comedi_fops.h"
+#include "comedi_compat32.h"
+
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/fcntl.h>
+#include <linux/delay.h>
+#include <linux/ioport.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+#include <linux/kmod.h>
+#include <linux/poll.h>
+#include <linux/init.h>
+#include <linux/device.h>
+#include <linux/vmalloc.h>
+#include <linux/fs.h>
+#include "comedidev.h"
+#include <linux/cdev.h>
+
+#include <linux/io.h>
+#include <linux/uaccess.h>
+
+/* #include "kvmem.h" */
+
+MODULE_AUTHOR("http://www.comedi.org");
+MODULE_DESCRIPTION("Comedi core module");
+MODULE_LICENSE("GPL");
+
+#ifdef CONFIG_COMEDI_DEBUG
+int comedi_debug;
+module_param(comedi_debug, int, 0644);
+#endif
+
+static DEFINE_SPINLOCK(comedi_file_info_table_lock);
+static struct comedi_device_file_info
+    *comedi_file_info_table[COMEDI_NUM_MINORS];
+
+static int do_devconfig_ioctl(comedi_device *dev, comedi_devconfig *arg);
+static int do_bufconfig_ioctl(comedi_device *dev, void *arg);
+static int do_devinfo_ioctl(comedi_device *dev, comedi_devinfo *arg,
+			    struct file *file);
+static int do_subdinfo_ioctl(comedi_device *dev, comedi_subdinfo *arg,
+			     void *file);
+static int do_chaninfo_ioctl(comedi_device *dev, comedi_chaninfo *arg);
+static int do_bufinfo_ioctl(comedi_device *dev, void *arg);
+static int do_cmd_ioctl(comedi_device *dev, void *arg, void *file);
+static int do_lock_ioctl(comedi_device *dev, unsigned int arg, void *file);
+static int do_unlock_ioctl(comedi_device *dev, unsigned int arg, void *file);
+static int do_cancel_ioctl(comedi_device *dev, unsigned int arg, void *file);
+static int do_cmdtest_ioctl(comedi_device *dev, void *arg, void *file);
+static int do_insnlist_ioctl(comedi_device *dev, void *arg, void *file);
+static int do_insn_ioctl(comedi_device *dev, void *arg, void *file);
+static int do_poll_ioctl(comedi_device *dev, unsigned int subd, void *file);
+
+extern void do_become_nonbusy(comedi_device *dev, comedi_subdevice *s);
+static int do_cancel(comedi_device *dev, comedi_subdevice *s);
+
+static int comedi_fasync(int fd, struct file *file, int on);
+
+static int is_device_busy(comedi_device *dev);
+
+#ifdef HAVE_UNLOCKED_IOCTL
+static long comedi_unlocked_ioctl(struct file *file, unsigned int cmd,
+				  unsigned long arg)
+#else
+static int comedi_ioctl(struct inode *inode, struct file *file,
+			unsigned int cmd, unsigned long arg)
+#endif
+{
+	const unsigned minor = iminor(file->f_dentry->d_inode);
+	struct comedi_device_file_info *dev_file_info =
+	    comedi_get_device_file_info(minor);
+	comedi_device *dev = dev_file_info->device;
+	int rc;
+
+	mutex_lock(&dev->mutex);
+
+	/* Device config is special, because it must work on
+	 * an unconfigured device. */
+	if (cmd == COMEDI_DEVCONFIG) {
+		rc = do_devconfig_ioctl(dev, (void *)arg);
+		goto done;
+	}
+
+	if (!dev->attached) {
+		DPRINTK("no driver configured on /dev/comedi%i\n", dev->minor);
+		rc = -ENODEV;
+		goto done;
+	}
+
+	switch (cmd) {
+	case COMEDI_BUFCONFIG:
+		rc = do_bufconfig_ioctl(dev, (void *)arg);
+		break;
+	case COMEDI_DEVINFO:
+		rc = do_devinfo_ioctl(dev, (void *)arg, file);
+		break;
+	case COMEDI_SUBDINFO:
+		rc = do_subdinfo_ioctl(dev, (void *)arg, file);
+		break;
+	case COMEDI_CHANINFO:
+		rc = do_chaninfo_ioctl(dev, (void *)arg);
+		break;
+	case COMEDI_RANGEINFO:
+		rc = do_rangeinfo_ioctl(dev, (void *)arg);
+		break;
+	case COMEDI_BUFINFO:
+		rc = do_bufinfo_ioctl(dev, (void *)arg);
+		break;
+	case COMEDI_LOCK:
+		rc = do_lock_ioctl(dev, arg, file);
+		break;
+	case COMEDI_UNLOCK:
+		rc = do_unlock_ioctl(dev, arg, file);
+		break;
+	case COMEDI_CANCEL:
+		rc = do_cancel_ioctl(dev, arg, file);
+		break;
+	case COMEDI_CMD:
+		rc = do_cmd_ioctl(dev, (void *)arg, file);
+		break;
+	case COMEDI_CMDTEST:
+		rc = do_cmdtest_ioctl(dev, (void *)arg, file);
+		break;
+	case COMEDI_INSNLIST:
+		rc = do_insnlist_ioctl(dev, (void *)arg, file);
+		break;
+	case COMEDI_INSN:
+		rc = do_insn_ioctl(dev, (void *)arg, file);
+		break;
+	case COMEDI_POLL:
+		rc = do_poll_ioctl(dev, arg, file);
+		break;
+	default:
+		rc = -ENOTTY;
+		break;
+	}
+
+done:
+	mutex_unlock(&dev->mutex);
+	return rc;
+}
+
+/*
+	COMEDI_DEVCONFIG
+	device config ioctl
+
+	arg:
+		pointer to devconfig structure
+
+	reads:
+		devconfig structure at arg
+
+	writes:
+		none
+*/
+static int do_devconfig_ioctl(comedi_device *dev, comedi_devconfig *arg)
+{
+	comedi_devconfig it;
+	int ret;
+	unsigned char *aux_data = NULL;
+	int aux_len;
+
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+
+	if (arg == NULL) {
+		if (is_device_busy(dev))
+			return -EBUSY;
+		if (dev->attached) {
+			struct module *driver_module = dev->driver->module;
+			comedi_device_detach(dev);
+			module_put(driver_module);
+		}
+		return 0;
+	}
+
+	if (copy_from_user(&it, arg, sizeof(comedi_devconfig)))
+		return -EFAULT;
+
+	it.board_name[COMEDI_NAMELEN - 1] = 0;
+
+	if (comedi_aux_data(it.options, 0) &&
+	    it.options[COMEDI_DEVCONF_AUX_DATA_LENGTH]) {
+		int bit_shift;
+		aux_len = it.options[COMEDI_DEVCONF_AUX_DATA_LENGTH];
+		if (aux_len < 0)
+			return -EFAULT;
+
+		aux_data = vmalloc(aux_len);
+		if (!aux_data)
+			return -ENOMEM;
+
+		if (copy_from_user(aux_data,
+				   comedi_aux_data(it.options, 0), aux_len)) {
+			vfree(aux_data);
+			return -EFAULT;
+		}
+		it.options[COMEDI_DEVCONF_AUX_DATA_LO] =
+		    (unsigned long)aux_data;
+		if (sizeof(void *) > sizeof(int)) {
+			bit_shift = sizeof(int) * 8;
+			it.options[COMEDI_DEVCONF_AUX_DATA_HI] =
+			    ((unsigned long)aux_data) >> bit_shift;
+		} else
+			it.options[COMEDI_DEVCONF_AUX_DATA_HI] = 0;
+	}
+
+	ret = comedi_device_attach(dev, &it);
+	if (ret == 0) {
+		if (!try_module_get(dev->driver->module)) {
+			comedi_device_detach(dev);
+			return -ENOSYS;
+		}
+	}
+
+	if (aux_data)
+		vfree(aux_data);
+
+	return ret;
+}
+
+/*
+	COMEDI_BUFCONFIG
+	buffer configuration ioctl
+
+	arg:
+		pointer to bufconfig structure
+
+	reads:
+		bufconfig at arg
+
+	writes:
+		modified bufconfig at arg
+
+*/
+static int do_bufconfig_ioctl(comedi_device *dev, void *arg)
+{
+	comedi_bufconfig bc;
+	comedi_async *async;
+	comedi_subdevice *s;
+	int ret = 0;
+
+	if (copy_from_user(&bc, arg, sizeof(comedi_bufconfig)))
+		return -EFAULT;
+
+	if (bc.subdevice >= dev->n_subdevices || bc.subdevice < 0)
+		return -EINVAL;
+
+	s = dev->subdevices + bc.subdevice;
+	async = s->async;
+
+	if (!async) {
+		DPRINTK("subdevice does not have async capability\n");
+		bc.size = 0;
+		bc.maximum_size = 0;
+		goto copyback;
+	}
+
+	if (bc.maximum_size) {
+		if (!capable(CAP_SYS_ADMIN))
+			return -EPERM;
+
+		async->max_bufsize = bc.maximum_size;
+	}
+
+	if (bc.size) {
+		if (bc.size > async->max_bufsize)
+			return -EPERM;
+
+		if (s->busy) {
+			DPRINTK("subdevice is busy, cannot resize buffer\n");
+			return -EBUSY;
+		}
+		if (async->mmap_count) {
+			DPRINTK("subdevice is mmapped, cannot resize buffer\n");
+			return -EBUSY;
+		}
+
+		if (!async->prealloc_buf)
+			return -EINVAL;
+
+		/* make sure buffer is an integral number of pages
+		 * (we round up) */
+		bc.size = (bc.size + PAGE_SIZE - 1) & PAGE_MASK;
+
+		ret = comedi_buf_alloc(dev, s, bc.size);
+		if (ret < 0)
+			return ret;
+
+		if (s->buf_change) {
+			ret = s->buf_change(dev, s, bc.size);
+			if (ret < 0)
+				return ret;
+		}
+
+		DPRINTK("comedi%i subd %d buffer resized to %i bytes\n",
+			dev->minor, bc.subdevice, async->prealloc_bufsz);
+	}
+
+	bc.size = async->prealloc_bufsz;
+	bc.maximum_size = async->max_bufsize;
+
+copyback:
+	if (copy_to_user(arg, &bc, sizeof(comedi_bufconfig)))
+		return -EFAULT;
+
+	return 0;
+}
+
+/*
+	COMEDI_DEVINFO
+	device info ioctl
+
+	arg:
+		pointer to devinfo structure
+
+	reads:
+		none
+
+	writes:
+		devinfo structure
+
+*/
+static int do_devinfo_ioctl(comedi_device *dev, comedi_devinfo *arg,
+			    struct file *file)
+{
+	comedi_devinfo devinfo;
+	const unsigned minor = iminor(file->f_dentry->d_inode);
+	struct comedi_device_file_info *dev_file_info =
+	    comedi_get_device_file_info(minor);
+	comedi_subdevice *read_subdev =
+	    comedi_get_read_subdevice(dev_file_info);
+	comedi_subdevice *write_subdev =
+	    comedi_get_write_subdevice(dev_file_info);
+
+	memset(&devinfo, 0, sizeof(devinfo));
+
+	/* fill devinfo structure */
+	devinfo.version_code = COMEDI_VERSION_CODE;
+	devinfo.n_subdevs = dev->n_subdevices;
+	memcpy(devinfo.driver_name, dev->driver->driver_name, COMEDI_NAMELEN);
+	memcpy(devinfo.board_name, dev->board_name, COMEDI_NAMELEN);
+
+	if (read_subdev)
+		devinfo.read_subdevice = read_subdev - dev->subdevices;
+	else
+		devinfo.read_subdevice = -1;
+
+	if (write_subdev)
+		devinfo.write_subdevice = write_subdev - dev->subdevices;
+	else
+		devinfo.write_subdevice = -1;
+
+	if (copy_to_user(arg, &devinfo, sizeof(comedi_devinfo)))
+		return -EFAULT;
+
+	return 0;
+}
+
+/*
+	COMEDI_SUBDINFO
+	subdevice info ioctl
+
+	arg:
+		pointer to array of subdevice info structures
+
+	reads:
+		none
+
+	writes:
+		array of subdevice info structures at arg
+
+*/
+static int do_subdinfo_ioctl(comedi_device *dev, comedi_subdinfo *arg,
+			     void *file)
+{
+	int ret, i;
+	comedi_subdinfo *tmp, *us;
+	comedi_subdevice *s;
+
+	tmp = kcalloc(dev->n_subdevices, sizeof(comedi_subdinfo), GFP_KERNEL);
+	if (!tmp)
+		return -ENOMEM;
+
+	/* fill subdinfo structs */
+	for (i = 0; i < dev->n_subdevices; i++) {
+		s = dev->subdevices + i;
+		us = tmp + i;
+
+		us->type = s->type;
+		us->n_chan = s->n_chan;
+		us->subd_flags = s->subdev_flags;
+		if (comedi_get_subdevice_runflags(s) & SRF_RUNNING)
+			us->subd_flags |= SDF_RUNNING;
+#define TIMER_nanosec 5		/* backwards compatibility */
+		us->timer_type = TIMER_nanosec;
+		us->len_chanlist = s->len_chanlist;
+		us->maxdata = s->maxdata;
+		if (s->range_table) {
+			us->range_type =
+			    (i << 24) | (0 << 16) | (s->range_table->length);
+		} else {
+			us->range_type = 0;	/* XXX */
+		}
+		us->flags = s->flags;
+
+		if (s->busy)
+			us->subd_flags |= SDF_BUSY;
+		if (s->busy == file)
+			us->subd_flags |= SDF_BUSY_OWNER;
+		if (s->lock)
+			us->subd_flags |= SDF_LOCKED;
+		if (s->lock == file)
+			us->subd_flags |= SDF_LOCK_OWNER;
+		if (!s->maxdata && s->maxdata_list)
+			us->subd_flags |= SDF_MAXDATA;
+		if (s->flaglist)
+			us->subd_flags |= SDF_FLAGS;
+		if (s->range_table_list)
+			us->subd_flags |= SDF_RANGETYPE;
+		if (s->do_cmd)
+			us->subd_flags |= SDF_CMD;
+
+		if (s->insn_bits != &insn_inval)
+			us->insn_bits_support = COMEDI_SUPPORTED;
+		else
+			us->insn_bits_support = COMEDI_UNSUPPORTED;
+
+		us->settling_time_0 = s->settling_time_0;
+	}
+
+	ret = copy_to_user(arg, tmp,
+			   dev->n_subdevices * sizeof(comedi_subdinfo));
+
+	kfree(tmp);
+
+	return ret ? -EFAULT : 0;
+}
+
+/*
+	COMEDI_CHANINFO
+	subdevice info ioctl
+
+	arg:
+		pointer to chaninfo structure
+
+	reads:
+		chaninfo structure at arg
+
+	writes:
+		arrays at elements of chaninfo structure
+
+*/
+static int do_chaninfo_ioctl(comedi_device *dev, comedi_chaninfo *arg)
+{
+	comedi_subdevice *s;
+	comedi_chaninfo it;
+
+	if (copy_from_user(&it, arg, sizeof(comedi_chaninfo)))
+		return -EFAULT;
+
+	if (it.subdev >= dev->n_subdevices)
+		return -EINVAL;
+	s = dev->subdevices + it.subdev;
+
+	if (it.maxdata_list) {
+		if (s->maxdata || !s->maxdata_list)
+			return -EINVAL;
+		if (copy_to_user(it.maxdata_list, s->maxdata_list,
+				 s->n_chan * sizeof(lsampl_t)))
+			return -EFAULT;
+	}
+
+	if (it.flaglist) {
+		if (!s->flaglist)
+			return -EINVAL;
+		if (copy_to_user(it.flaglist, s->flaglist,
+				 s->n_chan * sizeof(unsigned int)))
+			return -EFAULT;
+	}
+
+	if (it.rangelist) {
+		int i;
+
+		if (!s->range_table_list)
+			return -EINVAL;
+		for (i = 0; i < s->n_chan; i++) {
+			int x;
+
+			x = (dev->minor << 28) | (it.subdev << 24) | (i << 16) |
+			    (s->range_table_list[i]->length);
+			put_user(x, it.rangelist + i);
+		}
+#if 0
+		if (copy_to_user(it.rangelist, s->range_type_list,
+				 s->n_chan*sizeof(unsigned int)))
+			return -EFAULT;
+#endif
+	}
+
+	return 0;
+}
+
+ /*
+    COMEDI_BUFINFO
+    buffer information ioctl
+
+    arg:
+    pointer to bufinfo structure
+
+    reads:
+    bufinfo at arg
+
+    writes:
+    modified bufinfo at arg
+
+  */
+static int do_bufinfo_ioctl(comedi_device *dev, void *arg)
+{
+	comedi_bufinfo bi;
+	comedi_subdevice *s;
+	comedi_async *async;
+
+	if (copy_from_user(&bi, arg, sizeof(comedi_bufinfo)))
+		return -EFAULT;
+
+	if (bi.subdevice >= dev->n_subdevices || bi.subdevice < 0)
+		return -EINVAL;
+
+	s = dev->subdevices + bi.subdevice;
+	async = s->async;
+
+	if (!async) {
+		DPRINTK("subdevice does not have async capability\n");
+		bi.buf_write_ptr = 0;
+		bi.buf_read_ptr = 0;
+		bi.buf_write_count = 0;
+		bi.buf_read_count = 0;
+		goto copyback;
+	}
+
+	if (bi.bytes_read && (s->subdev_flags & SDF_CMD_READ)) {
+		bi.bytes_read = comedi_buf_read_alloc(async, bi.bytes_read);
+		comedi_buf_read_free(async, bi.bytes_read);
+
+		if (!(comedi_get_subdevice_runflags(s) & (SRF_ERROR |
+							  SRF_RUNNING))
+		    && async->buf_write_count == async->buf_read_count) {
+			do_become_nonbusy(dev, s);
+		}
+	}
+
+	if (bi.bytes_written && (s->subdev_flags & SDF_CMD_WRITE)) {
+		bi.bytes_written =
+		    comedi_buf_write_alloc(async, bi.bytes_written);
+		comedi_buf_write_free(async, bi.bytes_written);
+	}
+
+	bi.buf_write_count = async->buf_write_count;
+	bi.buf_write_ptr = async->buf_write_ptr;
+	bi.buf_read_count = async->buf_read_count;
+	bi.buf_read_ptr = async->buf_read_ptr;
+
+copyback:
+	if (copy_to_user(arg, &bi, sizeof(comedi_bufinfo)))
+		return -EFAULT;
+
+	return 0;
+}
+
+static int parse_insn(comedi_device *dev, comedi_insn *insn, lsampl_t *data,
+		      void *file);
+/*
+ * 	COMEDI_INSNLIST
+ * 	synchronous instructions
+ *
+ * 	arg:
+ * 		pointer to sync cmd structure
+ *
+ * 	reads:
+ * 		sync cmd struct at arg
+ * 		instruction list
+ * 		data (for writes)
+ *
+ * 	writes:
+ * 		data (for reads)
+ */
+/* arbitrary limits */
+#define MAX_SAMPLES 256
+static int do_insnlist_ioctl(comedi_device *dev, void *arg, void *file)
+{
+	comedi_insnlist insnlist;
+	comedi_insn *insns = NULL;
+	lsampl_t *data = NULL;
+	int i = 0;
+	int ret = 0;
+
+	if (copy_from_user(&insnlist, arg, sizeof(comedi_insnlist)))
+		return -EFAULT;
+
+	data = kmalloc(sizeof(lsampl_t) * MAX_SAMPLES, GFP_KERNEL);
+	if (!data) {
+		DPRINTK("kmalloc failed\n");
+		ret = -ENOMEM;
+		goto error;
+	}
+
+	insns = kmalloc(sizeof(comedi_insn) * insnlist.n_insns, GFP_KERNEL);
+	if (!insns) {
+		DPRINTK("kmalloc failed\n");
+		ret = -ENOMEM;
+		goto error;
+	}
+
+	if (copy_from_user(insns, insnlist.insns,
+			   sizeof(comedi_insn) * insnlist.n_insns)) {
+		DPRINTK("copy_from_user failed\n");
+		ret = -EFAULT;
+		goto error;
+	}
+
+	for (i = 0; i < insnlist.n_insns; i++) {
+		if (insns[i].n > MAX_SAMPLES) {
+			DPRINTK("number of samples too large\n");
+			ret = -EINVAL;
+			goto error;
+		}
+		if (insns[i].insn & INSN_MASK_WRITE) {
+			if (copy_from_user(data, insns[i].data,
+					   insns[i].n * sizeof(lsampl_t))) {
+				DPRINTK("copy_from_user failed\n");
+				ret = -EFAULT;
+				goto error;
+			}
+		}
+		ret = parse_insn(dev, insns + i, data, file);
+		if (ret < 0)
+			goto error;
+		if (insns[i].insn & INSN_MASK_READ) {
+			if (copy_to_user(insns[i].data, data,
+					 insns[i].n * sizeof(lsampl_t))) {
+				DPRINTK("copy_to_user failed\n");
+				ret = -EFAULT;
+				goto error;
+			}
+		}
+		if (need_resched())
+			schedule();
+	}
+
+error:
+	kfree(insns);
+	kfree(data);
+
+	if (ret < 0)
+		return ret;
+	return i;
+}
+
+static int check_insn_config_length(comedi_insn *insn, lsampl_t *data)
+{
+	if (insn->n < 1)
+		return -EINVAL;
+
+	switch (data[0]) {
+	case INSN_CONFIG_DIO_OUTPUT:
+	case INSN_CONFIG_DIO_INPUT:
+	case INSN_CONFIG_DISARM:
+	case INSN_CONFIG_RESET:
+		if (insn->n == 1)
+			return 0;
+		break;
+	case INSN_CONFIG_ARM:
+	case INSN_CONFIG_DIO_QUERY:
+	case INSN_CONFIG_BLOCK_SIZE:
+	case INSN_CONFIG_FILTER:
+	case INSN_CONFIG_SERIAL_CLOCK:
+	case INSN_CONFIG_BIDIRECTIONAL_DATA:
+	case INSN_CONFIG_ALT_SOURCE:
+	case INSN_CONFIG_SET_COUNTER_MODE:
+	case INSN_CONFIG_8254_READ_STATUS:
+	case INSN_CONFIG_SET_ROUTING:
+	case INSN_CONFIG_GET_ROUTING:
+	case INSN_CONFIG_GET_PWM_STATUS:
+	case INSN_CONFIG_PWM_SET_PERIOD:
+	case INSN_CONFIG_PWM_GET_PERIOD:
+		if (insn->n == 2)
+			return 0;
+		break;
+	case INSN_CONFIG_SET_GATE_SRC:
+	case INSN_CONFIG_GET_GATE_SRC:
+	case INSN_CONFIG_SET_CLOCK_SRC:
+	case INSN_CONFIG_GET_CLOCK_SRC:
+	case INSN_CONFIG_SET_OTHER_SRC:
+	case INSN_CONFIG_GET_COUNTER_STATUS:
+	case INSN_CONFIG_PWM_SET_H_BRIDGE:
+	case INSN_CONFIG_PWM_GET_H_BRIDGE:
+	case INSN_CONFIG_GET_HARDWARE_BUFFER_SIZE:
+		if (insn->n == 3)
+			return 0;
+		break;
+	case INSN_CONFIG_PWM_OUTPUT:
+	case INSN_CONFIG_ANALOG_TRIG:
+		if (insn->n == 5)
+			return 0;
+		break;
+	/* by default we allow the insn since we don't have checks for
+	 * all possible cases yet */
+	default:
+		rt_printk("comedi: no check for data length of config insn id "
+			  "%i is implemented.\n"
+			  " Add a check to %s in %s.\n"
+			  " Assuming n=%i is correct.\n", data[0], __func__,
+			  __FILE__, insn->n);
+		return 0;
+		break;
+	}
+	return -EINVAL;
+}
+
+static int parse_insn(comedi_device *dev, comedi_insn *insn, lsampl_t *data,
+		      void *file)
+{
+	comedi_subdevice *s;
+	int ret = 0;
+	int i;
+
+	if (insn->insn & INSN_MASK_SPECIAL) {
+		/* a non-subdevice instruction */
+
+		switch (insn->insn) {
+		case INSN_GTOD:
+			{
+				struct timeval tv;
+
+				if (insn->n != 2) {
+					ret = -EINVAL;
+					break;
+				}
+
+				do_gettimeofday(&tv);
+				data[0] = tv.tv_sec;
+				data[1] = tv.tv_usec;
+				ret = 2;
+
+				break;
+			}
+		case INSN_WAIT:
+			if (insn->n != 1 || data[0] >= 100000) {
+				ret = -EINVAL;
+				break;
+			}
+			udelay(data[0] / 1000);
+			ret = 1;
+			break;
+		case INSN_INTTRIG:
+			if (insn->n != 1) {
+				ret = -EINVAL;
+				break;
+			}
+			if (insn->subdev >= dev->n_subdevices) {
+				DPRINTK("%d not usable subdevice\n",
+					insn->subdev);
+				ret = -EINVAL;
+				break;
+			}
+			s = dev->subdevices + insn->subdev;
+			if (!s->async) {
+				DPRINTK("no async\n");
+				ret = -EINVAL;
+				break;
+			}
+			if (!s->async->inttrig) {
+				DPRINTK("no inttrig\n");
+				ret = -EAGAIN;
+				break;
+			}
+			ret = s->async->inttrig(dev, s, insn->data[0]);
+			if (ret >= 0)
+				ret = 1;
+			break;
+		default:
+			DPRINTK("invalid insn\n");
+			ret = -EINVAL;
+			break;
+		}
+	} else {
+		/* a subdevice instruction */
+		lsampl_t maxdata;
+
+		if (insn->subdev >= dev->n_subdevices) {
+			DPRINTK("subdevice %d out of range\n", insn->subdev);
+			ret = -EINVAL;
+			goto out;
+		}
+		s = dev->subdevices + insn->subdev;
+
+		if (s->type == COMEDI_SUBD_UNUSED) {
+			DPRINTK("%d not usable subdevice\n", insn->subdev);
+			ret = -EIO;
+			goto out;
+		}
+
+		/* are we locked? (ioctl lock) */
+		if (s->lock && s->lock != file) {
+			DPRINTK("device locked\n");
+			ret = -EACCES;
+			goto out;
+		}
+
+		ret = check_chanlist(s, 1, &insn->chanspec);
+		if (ret < 0) {
+			ret = -EINVAL;
+			DPRINTK("bad chanspec\n");
+			goto out;
+		}
+
+		if (s->busy) {
+			ret = -EBUSY;
+			goto out;
+		}
+		/* This looks arbitrary.  It is. */
+		s->busy = &parse_insn;
+		switch (insn->insn) {
+		case INSN_READ:
+			ret = s->insn_read(dev, s, insn, data);
+			break;
+		case INSN_WRITE:
+			maxdata = s->maxdata_list
+			    ? s->maxdata_list[CR_CHAN(insn->chanspec)]
+			    : s->maxdata;
+			for (i = 0; i < insn->n; ++i) {
+				if (data[i] > maxdata) {
+					ret = -EINVAL;
+					DPRINTK("bad data value(s)\n");
+					break;
+				}
+			}
+			if (ret == 0)
+				ret = s->insn_write(dev, s, insn, data);
+			break;
+		case INSN_BITS:
+			if (insn->n != 2) {
+				ret = -EINVAL;
+				break;
+			}
+			ret = s->insn_bits(dev, s, insn, data);
+			break;
+		case INSN_CONFIG:
+			ret = check_insn_config_length(insn, data);
+			if (ret)
+				break;
+			ret = s->insn_config(dev, s, insn, data);
+			break;
+		default:
+			ret = -EINVAL;
+			break;
+		}
+
+		s->busy = NULL;
+	}
+
+out:
+	return ret;
+}
+
+/*
+ * 	COMEDI_INSN
+ * 	synchronous instructions
+ *
+ * 	arg:
+ * 		pointer to insn
+ *
+ * 	reads:
+ * 		comedi_insn struct at arg
+ * 		data (for writes)
+ *
+ * 	writes:
+ * 		data (for reads)
+ */
+static int do_insn_ioctl(comedi_device *dev, void *arg, void *file)
+{
+	comedi_insn insn;
+	lsampl_t *data = NULL;
+	int ret = 0;
+
+	data = kmalloc(sizeof(lsampl_t) * MAX_SAMPLES, GFP_KERNEL);
+	if (!data) {
+		ret = -ENOMEM;
+		goto error;
+	}
+
+	if (copy_from_user(&insn, arg, sizeof(comedi_insn))) {
+		ret = -EFAULT;
+		goto error;
+	}
+
+	/* This is where the behavior of insn and insnlist deviate. */
+	if (insn.n > MAX_SAMPLES)
+		insn.n = MAX_SAMPLES;
+	if (insn.insn & INSN_MASK_WRITE) {
+		if (copy_from_user(data, insn.data, insn.n * sizeof(lsampl_t))) {
+			ret = -EFAULT;
+			goto error;
+		}
+	}
+	ret = parse_insn(dev, &insn, data, file);
+	if (ret < 0)
+		goto error;
+	if (insn.insn & INSN_MASK_READ) {
+		if (copy_to_user(insn.data, data, insn.n * sizeof(lsampl_t))) {
+			ret = -EFAULT;
+			goto error;
+		}
+	}
+	ret = insn.n;
+
+error:
+	kfree(data);
+
+	return ret;
+}
+
+/*
+	COMEDI_CMD
+	command ioctl
+
+	arg:
+		pointer to cmd structure
+
+	reads:
+		cmd structure at arg
+		channel/range list
+
+	writes:
+		modified cmd structure at arg
+
+*/
+static int do_cmd_ioctl(comedi_device *dev, void *arg, void *file)
+{
+	comedi_cmd user_cmd;
+	comedi_subdevice *s;
+	comedi_async *async;
+	int ret = 0;
+	unsigned int *chanlist_saver = NULL;
+
+	if (copy_from_user(&user_cmd, arg, sizeof(comedi_cmd))) {
+		DPRINTK("bad cmd address\n");
+		return -EFAULT;
+	}
+	/* save user's chanlist pointer so it can be restored later */
+	chanlist_saver = user_cmd.chanlist;
+
+	if (user_cmd.subdev >= dev->n_subdevices) {
+		DPRINTK("%d no such subdevice\n", user_cmd.subdev);
+		return -ENODEV;
+	}
+
+	s = dev->subdevices + user_cmd.subdev;
+	async = s->async;
+
+	if (s->type == COMEDI_SUBD_UNUSED) {
+		DPRINTK("%d not valid subdevice\n", user_cmd.subdev);
+		return -EIO;
+	}
+
+	if (!s->do_cmd || !s->do_cmdtest || !s->async) {
+		DPRINTK("subdevice %i does not support commands\n",
+			user_cmd.subdev);
+		return -EIO;
+	}
+
+	/* are we locked? (ioctl lock) */
+	if (s->lock && s->lock != file) {
+		DPRINTK("subdevice locked\n");
+		return -EACCES;
+	}
+
+	/* are we busy? */
+	if (s->busy) {
+		DPRINTK("subdevice busy\n");
+		return -EBUSY;
+	}
+	s->busy = file;
+
+	/* make sure channel/gain list isn't too long */
+	if (user_cmd.chanlist_len > s->len_chanlist) {
+		DPRINTK("channel/gain list too long %u > %d\n",
+			user_cmd.chanlist_len, s->len_chanlist);
+		ret = -EINVAL;
+		goto cleanup;
+	}
+
+	/* make sure channel/gain list isn't too short */
+	if (user_cmd.chanlist_len < 1) {
+		DPRINTK("channel/gain list too short %u < 1\n",
+			user_cmd.chanlist_len);
+		ret = -EINVAL;
+		goto cleanup;
+	}
+
+	kfree(async->cmd.chanlist);
+	async->cmd = user_cmd;
+	async->cmd.data = NULL;
+	/* load channel/gain list */
+	async->cmd.chanlist =
+	    kmalloc(async->cmd.chanlist_len * sizeof(int), GFP_KERNEL);
+	if (!async->cmd.chanlist) {
+		DPRINTK("allocation failed\n");
+		ret = -ENOMEM;
+		goto cleanup;
+	}
+
+	if (copy_from_user(async->cmd.chanlist, user_cmd.chanlist,
+			   async->cmd.chanlist_len * sizeof(int))) {
+		DPRINTK("fault reading chanlist\n");
+		ret = -EFAULT;
+		goto cleanup;
+	}
+
+	/* make sure each element in channel/gain list is valid */
+	ret = check_chanlist(s, async->cmd.chanlist_len, async->cmd.chanlist);
+	if (ret < 0) {
+		DPRINTK("bad chanlist\n");
+		goto cleanup;
+	}
+
+	ret = s->do_cmdtest(dev, s, &async->cmd);
+
+	if (async->cmd.flags & TRIG_BOGUS || ret) {
+		DPRINTK("test returned %d\n", ret);
+		user_cmd = async->cmd;
+		/* restore chanlist pointer before copying back */
+		user_cmd.chanlist = chanlist_saver;
+		user_cmd.data = NULL;
+		if (copy_to_user(arg, &user_cmd, sizeof(comedi_cmd))) {
+			DPRINTK("fault writing cmd\n");
+			ret = -EFAULT;
+			goto cleanup;
+		}
+		ret = -EAGAIN;
+		goto cleanup;
+	}
+
+	if (!async->prealloc_bufsz) {
+		ret = -ENOMEM;
+		DPRINTK("no buffer (?)\n");
+		goto cleanup;
+	}
+
+	comedi_reset_async_buf(async);
+
+	async->cb_mask =
+	    COMEDI_CB_EOA | COMEDI_CB_BLOCK | COMEDI_CB_ERROR |
+	    COMEDI_CB_OVERFLOW;
+	if (async->cmd.flags & TRIG_WAKE_EOS)
+		async->cb_mask |= COMEDI_CB_EOS;
+
+	comedi_set_subdevice_runflags(s, ~0, SRF_USER | SRF_RUNNING);
+
+#ifdef CONFIG_COMEDI_RT
+	if (async->cmd.flags & TRIG_RT) {
+		if (comedi_switch_to_rt(dev) == 0)
+			comedi_set_subdevice_runflags(s, SRF_RT, SRF_RT);
+	}
+#endif
+
+	ret = s->do_cmd(dev, s);
+	if (ret == 0)
+		return 0;
+
+cleanup:
+	do_become_nonbusy(dev, s);
+
+	return ret;
+}
+
+/*
+	COMEDI_CMDTEST
+	command testing ioctl
+
+	arg:
+		pointer to cmd structure
+
+	reads:
+		cmd structure at arg
+		channel/range list
+
+	writes:
+		modified cmd structure at arg
+
+*/
+static int do_cmdtest_ioctl(comedi_device *dev, void *arg, void *file)
+{
+	comedi_cmd user_cmd;
+	comedi_subdevice *s;
+	int ret = 0;
+	unsigned int *chanlist = NULL;
+	unsigned int *chanlist_saver = NULL;
+
+	if (copy_from_user(&user_cmd, arg, sizeof(comedi_cmd))) {
+		DPRINTK("bad cmd address\n");
+		return -EFAULT;
+	}
+	/* save user's chanlist pointer so it can be restored later */
+	chanlist_saver = user_cmd.chanlist;
+
+	if (user_cmd.subdev >= dev->n_subdevices) {
+		DPRINTK("%d no such subdevice\n", user_cmd.subdev);
+		return -ENODEV;
+	}
+
+	s = dev->subdevices + user_cmd.subdev;
+	if (s->type == COMEDI_SUBD_UNUSED) {
+		DPRINTK("%d not valid subdevice\n", user_cmd.subdev);
+		return -EIO;
+	}
+
+	if (!s->do_cmd || !s->do_cmdtest) {
+		DPRINTK("subdevice %i does not support commands\n",
+			user_cmd.subdev);
+		return -EIO;
+	}
+
+	/* make sure channel/gain list isn't too long */
+	if (user_cmd.chanlist_len > s->len_chanlist) {
+		DPRINTK("channel/gain list too long %d > %d\n",
+			user_cmd.chanlist_len, s->len_chanlist);
+		ret = -EINVAL;
+		goto cleanup;
+	}
+
+	/* load channel/gain list */
+	if (user_cmd.chanlist) {
+		chanlist =
+		    kmalloc(user_cmd.chanlist_len * sizeof(int), GFP_KERNEL);
+		if (!chanlist) {
+			DPRINTK("allocation failed\n");
+			ret = -ENOMEM;
+			goto cleanup;
+		}
+
+		if (copy_from_user(chanlist, user_cmd.chanlist,
+				   user_cmd.chanlist_len * sizeof(int))) {
+			DPRINTK("fault reading chanlist\n");
+			ret = -EFAULT;
+			goto cleanup;
+		}
+
+		/* make sure each element in channel/gain list is valid */
+		ret = check_chanlist(s, user_cmd.chanlist_len, chanlist);
+		if (ret < 0) {
+			DPRINTK("bad chanlist\n");
+			goto cleanup;
+		}
+
+		user_cmd.chanlist = chanlist;
+	}
+
+	ret = s->do_cmdtest(dev, s, &user_cmd);
+
+	/* restore chanlist pointer before copying back */
+	user_cmd.chanlist = chanlist_saver;
+
+	if (copy_to_user(arg, &user_cmd, sizeof(comedi_cmd))) {
+		DPRINTK("bad cmd address\n");
+		ret = -EFAULT;
+		goto cleanup;
+	}
+cleanup:
+	kfree(chanlist);
+
+	return ret;
+}
+
+/*
+	COMEDI_LOCK
+	lock subdevice
+
+	arg:
+		subdevice number
+
+	reads:
+		none
+
+	writes:
+		none
+
+*/
+
+static int do_lock_ioctl(comedi_device *dev, unsigned int arg, void *file)
+{
+	int ret = 0;
+	unsigned long flags;
+	comedi_subdevice *s;
+
+	if (arg >= dev->n_subdevices)
+		return -EINVAL;
+	s = dev->subdevices + arg;
+
+	comedi_spin_lock_irqsave(&s->spin_lock, flags);
+	if (s->busy || s->lock)
+		ret = -EBUSY;
+	else
+		s->lock = file;
+	comedi_spin_unlock_irqrestore(&s->spin_lock, flags);
+
+	if (ret < 0)
+		return ret;
+
+#if 0
+	if (s->lock_f)
+		ret = s->lock_f(dev, s);
+#endif
+
+	return ret;
+}
+
+/*
+	COMEDI_UNLOCK
+	unlock subdevice
+
+	arg:
+		subdevice number
+
+	reads:
+		none
+
+	writes:
+		none
+
+	This function isn't protected by the semaphore, since
+	we already own the lock.
+*/
+static int do_unlock_ioctl(comedi_device *dev, unsigned int arg, void *file)
+{
+	comedi_subdevice *s;
+
+	if (arg >= dev->n_subdevices)
+		return -EINVAL;
+	s = dev->subdevices + arg;
+
+	if (s->busy)
+		return -EBUSY;
+
+	if (s->lock && s->lock != file)
+		return -EACCES;
+
+	if (s->lock == file) {
+#if 0
+		if (s->unlock)
+			s->unlock(dev, s);
+#endif
+
+		s->lock = NULL;
+	}
+
+	return 0;
+}
+
+/*
+	COMEDI_CANCEL
+	cancel acquisition ioctl
+
+	arg:
+		subdevice number
+
+	reads:
+		nothing
+
+	writes:
+		nothing
+
+*/
+static int do_cancel_ioctl(comedi_device *dev, unsigned int arg, void *file)
+{
+	comedi_subdevice *s;
+
+	if (arg >= dev->n_subdevices)
+		return -EINVAL;
+	s = dev->subdevices + arg;
+	if (s->async == NULL)
+		return -EINVAL;
+
+	if (s->lock && s->lock != file)
+		return -EACCES;
+
+	if (!s->busy)
+		return 0;
+
+	if (s->busy != file)
+		return -EBUSY;
+
+	return do_cancel(dev, s);
+}
+
+/*
+	COMEDI_POLL ioctl
+	instructs driver to synchronize buffers
+
+	arg:
+		subdevice number
+
+	reads:
+		nothing
+
+	writes:
+		nothing
+
+*/
+static int do_poll_ioctl(comedi_device *dev, unsigned int arg, void *file)
+{
+	comedi_subdevice *s;
+
+	if (arg >= dev->n_subdevices)
+		return -EINVAL;
+	s = dev->subdevices + arg;
+
+	if (s->lock && s->lock != file)
+		return -EACCES;
+
+	if (!s->busy)
+		return 0;
+
+	if (s->busy != file)
+		return -EBUSY;
+
+	if (s->poll)
+		return s->poll(dev, s);
+
+	return -EINVAL;
+}
+
+static int do_cancel(comedi_device *dev, comedi_subdevice *s)
+{
+	int ret = 0;
+
+	if ((comedi_get_subdevice_runflags(s) & SRF_RUNNING) && s->cancel)
+		ret = s->cancel(dev, s);
+
+	do_become_nonbusy(dev, s);
+
+	return ret;
+}
+
+void comedi_unmap(struct vm_area_struct *area)
+{
+	comedi_async *async;
+	comedi_device *dev;
+
+	async = area->vm_private_data;
+	dev = async->subdevice->device;
+
+	mutex_lock(&dev->mutex);
+	async->mmap_count--;
+	mutex_unlock(&dev->mutex);
+}
+
+static struct vm_operations_struct comedi_vm_ops = {
+	.close =	comedi_unmap,
+};
+
+static int comedi_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	const unsigned minor = iminor(file->f_dentry->d_inode);
+	struct comedi_device_file_info *dev_file_info =
+	    comedi_get_device_file_info(minor);
+	comedi_device *dev = dev_file_info->device;
+	comedi_async *async = NULL;
+	unsigned long start = vma->vm_start;
+	unsigned long size;
+	int n_pages;
+	int i;
+	int retval;
+	comedi_subdevice *s;
+
+	mutex_lock(&dev->mutex);
+	if (!dev->attached) {
+		DPRINTK("no driver configured on comedi%i\n", dev->minor);
+		retval = -ENODEV;
+		goto done;
+	}
+	if (vma->vm_flags & VM_WRITE)
+		s = comedi_get_write_subdevice(dev_file_info);
+	else
+		s = comedi_get_read_subdevice(dev_file_info);
+
+	if (s == NULL) {
+		retval = -EINVAL;
+		goto done;
+	}
+	async = s->async;
+	if (async == NULL) {
+		retval = -EINVAL;
+		goto done;
+	}
+
+	if (vma->vm_pgoff != 0) {
+		DPRINTK("comedi: mmap() offset must be 0.\n");
+		retval = -EINVAL;
+		goto done;
+	}
+
+	size = vma->vm_end - vma->vm_start;
+	if (size > async->prealloc_bufsz) {
+		retval = -EFAULT;
+		goto done;
+	}
+	if (size & (~PAGE_MASK)) {
+		retval = -EFAULT;
+		goto done;
+	}
+
+	n_pages = size >> PAGE_SHIFT;
+	for (i = 0; i < n_pages; ++i) {
+		if (remap_pfn_range(vma, start,
+				    page_to_pfn(virt_to_page(async->
+							     buf_page_list[i].
+							     virt_addr)),
+				    PAGE_SIZE, PAGE_SHARED)) {
+			retval = -EAGAIN;
+			goto done;
+		}
+		start += PAGE_SIZE;
+	}
+
+	vma->vm_ops = &comedi_vm_ops;
+	vma->vm_private_data = async;
+
+	async->mmap_count++;
+
+	retval = 0;
+done:
+	mutex_unlock(&dev->mutex);
+	return retval;
+}
+
+static unsigned int comedi_poll(struct file *file, poll_table *wait)
+{
+	unsigned int mask = 0;
+	const unsigned minor = iminor(file->f_dentry->d_inode);
+	struct comedi_device_file_info *dev_file_info =
+	    comedi_get_device_file_info(minor);
+	comedi_device *dev = dev_file_info->device;
+	comedi_subdevice *read_subdev;
+	comedi_subdevice *write_subdev;
+
+	mutex_lock(&dev->mutex);
+	if (!dev->attached) {
+		DPRINTK("no driver configured on comedi%i\n", dev->minor);
+		mutex_unlock(&dev->mutex);
+		return 0;
+	}
+
+	mask = 0;
+	read_subdev = comedi_get_read_subdevice(dev_file_info);
+	if (read_subdev) {
+		poll_wait(file, &read_subdev->async->wait_head, wait);
+		if (!read_subdev->busy
+		    || comedi_buf_read_n_available(read_subdev->async) > 0
+		    || !(comedi_get_subdevice_runflags(read_subdev) &
+			 SRF_RUNNING)) {
+			mask |= POLLIN | POLLRDNORM;
+		}
+	}
+	write_subdev = comedi_get_write_subdevice(dev_file_info);
+	if (write_subdev) {
+		poll_wait(file, &write_subdev->async->wait_head, wait);
+		comedi_buf_write_alloc(write_subdev->async,
+				       write_subdev->async->prealloc_bufsz);
+		if (!write_subdev->busy
+		    || !(comedi_get_subdevice_runflags(write_subdev) &
+			 SRF_RUNNING)
+		    || comedi_buf_write_n_allocated(write_subdev->async) >=
+		    bytes_per_sample(write_subdev->async->subdevice)) {
+			mask |= POLLOUT | POLLWRNORM;
+		}
+	}
+
+	mutex_unlock(&dev->mutex);
+	return mask;
+}
+
+static ssize_t comedi_write(struct file *file, const char *buf, size_t nbytes,
+			    loff_t *offset)
+{
+	comedi_subdevice *s;
+	comedi_async *async;
+	int n, m, count = 0, retval = 0;
+	DECLARE_WAITQUEUE(wait, current);
+	const unsigned minor = iminor(file->f_dentry->d_inode);
+	struct comedi_device_file_info *dev_file_info =
+	    comedi_get_device_file_info(minor);
+	comedi_device *dev = dev_file_info->device;
+
+	if (!dev->attached) {
+		DPRINTK("no driver configured on comedi%i\n", dev->minor);
+		retval = -ENODEV;
+		goto done;
+	}
+
+	s = comedi_get_write_subdevice(dev_file_info);
+	if (s == NULL) {
+		retval = -EIO;
+		goto done;
+	}
+	async = s->async;
+
+	if (!nbytes) {
+		retval = 0;
+		goto done;
+	}
+	if (!s->busy) {
+		retval = 0;
+		goto done;
+	}
+	if (s->busy != file) {
+		retval = -EACCES;
+		goto done;
+	}
+	add_wait_queue(&async->wait_head, &wait);
+	while (nbytes > 0 && !retval) {
+		set_current_state(TASK_INTERRUPTIBLE);
+
+		n = nbytes;
+
+		m = n;
+		if (async->buf_write_ptr + m > async->prealloc_bufsz)
+			m = async->prealloc_bufsz - async->buf_write_ptr;
+		comedi_buf_write_alloc(async, async->prealloc_bufsz);
+		if (m > comedi_buf_write_n_allocated(async))
+			m = comedi_buf_write_n_allocated(async);
+		if (m < n)
+			n = m;
+
+		if (n == 0) {
+			if (!(comedi_get_subdevice_runflags(s) & SRF_RUNNING)) {
+				if (comedi_get_subdevice_runflags(s) &
+				    SRF_ERROR) {
+					retval = -EPIPE;
+				} else {
+					retval = 0;
+				}
+				do_become_nonbusy(dev, s);
+				break;
+			}
+			if (file->f_flags & O_NONBLOCK) {
+				retval = -EAGAIN;
+				break;
+			}
+			if (signal_pending(current)) {
+				retval = -ERESTARTSYS;
+				break;
+			}
+			schedule();
+			if (!s->busy)
+				break;
+			if (s->busy != file) {
+				retval = -EACCES;
+				break;
+			}
+			continue;
+		}
+
+		m = copy_from_user(async->prealloc_buf + async->buf_write_ptr,
+				   buf, n);
+		if (m) {
+			n -= m;
+			retval = -EFAULT;
+		}
+		comedi_buf_write_free(async, n);
+
+		count += n;
+		nbytes -= n;
+
+		buf += n;
+		break;		/* makes device work like a pipe */
+	}
+	set_current_state(TASK_RUNNING);
+	remove_wait_queue(&async->wait_head, &wait);
+
+done:
+	return count ? count : retval;
+}
+
+static ssize_t comedi_read(struct file *file, char *buf, size_t nbytes,
+			   loff_t *offset)
+{
+	comedi_subdevice *s;
+	comedi_async *async;
+	int n, m, count = 0, retval = 0;
+	DECLARE_WAITQUEUE(wait, current);
+	const unsigned minor = iminor(file->f_dentry->d_inode);
+	struct comedi_device_file_info *dev_file_info =
+	    comedi_get_device_file_info(minor);
+	comedi_device *dev = dev_file_info->device;
+
+	if (!dev->attached) {
+		DPRINTK("no driver configured on comedi%i\n", dev->minor);
+		retval = -ENODEV;
+		goto done;
+	}
+
+	s = comedi_get_read_subdevice(dev_file_info);
+	if (s == NULL) {
+		retval = -EIO;
+		goto done;
+	}
+	async = s->async;
+	if (!nbytes) {
+		retval = 0;
+		goto done;
+	}
+	if (!s->busy) {
+		retval = 0;
+		goto done;
+	}
+	if (s->busy != file) {
+		retval = -EACCES;
+		goto done;
+	}
+
+	add_wait_queue(&async->wait_head, &wait);
+	while (nbytes > 0 && !retval) {
+		set_current_state(TASK_INTERRUPTIBLE);
+
+		n = nbytes;
+
+		m = comedi_buf_read_n_available(async);
+		/* printk("%d available\n",m); */
+		if (async->buf_read_ptr + m > async->prealloc_bufsz)
+			m = async->prealloc_bufsz - async->buf_read_ptr;
+		/* printk("%d contiguous\n",m); */
+		if (m < n)
+			n = m;
+
+		if (n == 0) {
+			if (!(comedi_get_subdevice_runflags(s) & SRF_RUNNING)) {
+				do_become_nonbusy(dev, s);
+				if (comedi_get_subdevice_runflags(s) &
+				    SRF_ERROR) {
+					retval = -EPIPE;
+				} else {
+					retval = 0;
+				}
+				break;
+			}
+			if (file->f_flags & O_NONBLOCK) {
+				retval = -EAGAIN;
+				break;
+			}
+			if (signal_pending(current)) {
+				retval = -ERESTARTSYS;
+				break;
+			}
+			schedule();
+			if (!s->busy) {
+				retval = 0;
+				break;
+			}
+			if (s->busy != file) {
+				retval = -EACCES;
+				break;
+			}
+			continue;
+		}
+		m = copy_to_user(buf, async->prealloc_buf +
+				 async->buf_read_ptr, n);
+		if (m) {
+			n -= m;
+			retval = -EFAULT;
+		}
+
+		comedi_buf_read_alloc(async, n);
+		comedi_buf_read_free(async, n);
+
+		count += n;
+		nbytes -= n;
+
+		buf += n;
+		break;		/* makes device work like a pipe */
+	}
+	if (!(comedi_get_subdevice_runflags(s) & (SRF_ERROR | SRF_RUNNING)) &&
+	    async->buf_read_count - async->buf_write_count == 0) {
+		do_become_nonbusy(dev, s);
+	}
+	set_current_state(TASK_RUNNING);
+	remove_wait_queue(&async->wait_head, &wait);
+
+done:
+	return count ? count : retval;
+}
+
+/*
+   This function restores a subdevice to an idle state.
+ */
+void do_become_nonbusy(comedi_device *dev, comedi_subdevice *s)
+{
+	comedi_async *async = s->async;
+
+	comedi_set_subdevice_runflags(s, SRF_RUNNING, 0);
+#ifdef CONFIG_COMEDI_RT
+	if (comedi_get_subdevice_runflags(s) & SRF_RT) {
+		comedi_switch_to_non_rt(dev);
+		comedi_set_subdevice_runflags(s, SRF_RT, 0);
+	}
+#endif
+	if (async) {
+		comedi_reset_async_buf(async);
+		async->inttrig = NULL;
+	} else {
+		printk(KERN_ERR
+		       "BUG: (?) do_become_nonbusy called with async=0\n");
+	}
+
+	s->busy = NULL;
+}
+
+static int comedi_open(struct inode *inode, struct file *file)
+{
+	char mod[32];
+	const unsigned minor = iminor(inode);
+	struct comedi_device_file_info *dev_file_info =
+	    comedi_get_device_file_info(minor);
+	comedi_device *dev = dev_file_info->device;
+	if (dev == NULL) {
+		DPRINTK("invalid minor number\n");
+		return -ENODEV;
+	}
+
+	/* This is slightly hacky, but we want module autoloading
+	 * to work for root.
+	 * case: user opens device, attached -> ok
+	 * case: user opens device, unattached, in_request_module=0 -> autoload
+	 * case: user opens device, unattached, in_request_module=1 -> fail
+	 * case: root opens device, attached -> ok
+	 * case: root opens device, unattached, in_request_module=1 -> ok
+	 *   (typically called from modprobe)
+	 * case: root opens device, unattached, in_request_module=0 -> autoload
+	 *
+	 * The last could be changed to "-> ok", which would deny root
+	 * autoloading.
+	 */
+	mutex_lock(&dev->mutex);
+	if (dev->attached)
+		goto ok;
+	if (!capable(CAP_SYS_MODULE) && dev->in_request_module) {
+		DPRINTK("in request module\n");
+		mutex_unlock(&dev->mutex);
+		return -ENODEV;
+	}
+	if (capable(CAP_SYS_MODULE) && dev->in_request_module)
+		goto ok;
+
+	dev->in_request_module = 1;
+
+	sprintf(mod, "char-major-%i-%i", COMEDI_MAJOR, dev->minor);
+#ifdef CONFIG_KMOD
+	mutex_unlock(&dev->mutex);
+	request_module(mod);
+	mutex_lock(&dev->mutex);
+#endif
+
+	dev->in_request_module = 0;
+
+	if (!dev->attached && !capable(CAP_SYS_MODULE)) {
+		DPRINTK("not attached and not CAP_SYS_MODULE\n");
+		mutex_unlock(&dev->mutex);
+		return -ENODEV;
+	}
+ok:
+	__module_get(THIS_MODULE);
+
+	if (dev->attached) {
+		if (!try_module_get(dev->driver->module)) {
+			module_put(THIS_MODULE);
+			mutex_unlock(&dev->mutex);
+			return -ENOSYS;
+		}
+	}
+
+	if (dev->attached && dev->use_count == 0 && dev->open)
+		dev->open(dev);
+
+	dev->use_count++;
+
+	mutex_unlock(&dev->mutex);
+
+	return 0;
+}
+
+static int comedi_close(struct inode *inode, struct file *file)
+{
+	const unsigned minor = iminor(inode);
+	struct comedi_device_file_info *dev_file_info =
+	    comedi_get_device_file_info(minor);
+	comedi_device *dev = dev_file_info->device;
+	comedi_subdevice *s = NULL;
+	int i;
+
+	mutex_lock(&dev->mutex);
+
+	if (dev->subdevices) {
+		for (i = 0; i < dev->n_subdevices; i++) {
+			s = dev->subdevices + i;
+
+			if (s->busy == file)
+				do_cancel(dev, s);
+			if (s->lock == file)
+				s->lock = NULL;
+		}
+	}
+	if (dev->attached && dev->use_count == 1 && dev->close)
+		dev->close(dev);
+
+	module_put(THIS_MODULE);
+	if (dev->attached)
+		module_put(dev->driver->module);
+
+	dev->use_count--;
+
+	mutex_unlock(&dev->mutex);
+
+	if (file->f_flags & FASYNC)
+		comedi_fasync(-1, file, 0);
+
+	return 0;
+}
+
+static int comedi_fasync(int fd, struct file *file, int on)
+{
+	const unsigned minor = iminor(file->f_dentry->d_inode);
+	struct comedi_device_file_info *dev_file_info =
+	    comedi_get_device_file_info(minor);
+
+	comedi_device *dev = dev_file_info->device;
+
+	return fasync_helper(fd, file, on, &dev->async_queue);
+}
+
+const struct file_operations comedi_fops = {
+      .owner =		THIS_MODULE,
+#ifdef HAVE_UNLOCKED_IOCTL
+      .unlocked_ioctl =	comedi_unlocked_ioctl,
+#else
+      .ioctl =		comedi_ioctl,
+#endif
+#ifdef HAVE_COMPAT_IOCTL
+      .compat_ioctl =	comedi_compat_ioctl,
+#endif
+      .open =		comedi_open,
+      .release =	comedi_close,
+      .read =		comedi_read,
+      .write =		comedi_write,
+      .mmap =		comedi_mmap,
+      .poll =		comedi_poll,
+      .fasync =		comedi_fasync,
+};
+
+struct class *comedi_class;
+static struct cdev comedi_cdev;
+
+static void comedi_cleanup_legacy_minors(void)
+{
+	unsigned i;
+
+	for (i = 0; i < COMEDI_NUM_LEGACY_MINORS; i++)
+		comedi_free_board_minor(i);
+}
+
+static int __init comedi_init(void)
+{
+	int i;
+	int retval;
+
+	printk(KERN_INFO "comedi: version " COMEDI_RELEASE
+	       " - http://www.comedi.org\n");
+
+	memset(comedi_file_info_table, 0,
+	       sizeof(struct comedi_device_file_info *) * COMEDI_NUM_MINORS);
+
+	retval = register_chrdev_region(MKDEV(COMEDI_MAJOR, 0),
+					COMEDI_NUM_MINORS, "comedi");
+	if (retval)
+		return -EIO;
+	cdev_init(&comedi_cdev, &comedi_fops);
+	comedi_cdev.owner = THIS_MODULE;
+	kobject_set_name(&comedi_cdev.kobj, "comedi");
+	if (cdev_add(&comedi_cdev, MKDEV(COMEDI_MAJOR, 0), COMEDI_NUM_MINORS)) {
+		unregister_chrdev_region(MKDEV(COMEDI_MAJOR, 0),
+					 COMEDI_NUM_MINORS);
+		return -EIO;
+	}
+	comedi_class = class_create(THIS_MODULE, "comedi");
+	if (IS_ERR(comedi_class)) {
+		printk("comedi: failed to create class");
+		cdev_del(&comedi_cdev);
+		unregister_chrdev_region(MKDEV(COMEDI_MAJOR, 0),
+					 COMEDI_NUM_MINORS);
+		return PTR_ERR(comedi_class);
+	}
+
+	/* XXX requires /proc interface */
+	comedi_proc_init();
+
+	/* create devices files for legacy/manual use */
+	for (i = 0; i < COMEDI_NUM_LEGACY_MINORS; i++) {
+		int minor;
+		minor = comedi_alloc_board_minor(NULL);
+		if (minor < 0) {
+			comedi_cleanup_legacy_minors();
+			cdev_del(&comedi_cdev);
+			unregister_chrdev_region(MKDEV(COMEDI_MAJOR, 0),
+						 COMEDI_NUM_MINORS);
+			return minor;
+		}
+	}
+
+	comedi_rt_init();
+
+	comedi_register_ioctl32();
+
+	return 0;
+}
+
+static void __exit comedi_cleanup(void)
+{
+	int i;
+
+	comedi_cleanup_legacy_minors();
+	for (i = 0; i < COMEDI_NUM_MINORS; ++i)
+		BUG_ON(comedi_file_info_table[i]);
+
+
+	class_destroy(comedi_class);
+	cdev_del(&comedi_cdev);
+	unregister_chrdev_region(MKDEV(COMEDI_MAJOR, 0), COMEDI_NUM_MINORS);
+
+	comedi_proc_cleanup();
+
+	comedi_rt_cleanup();
+
+	comedi_unregister_ioctl32();
+}
+
+module_init(comedi_init);
+module_exit(comedi_cleanup);
+
+void comedi_error(const comedi_device *dev, const char *s)
+{
+	rt_printk("comedi%d: %s: %s\n", dev->minor, dev->driver->driver_name,
+		  s);
+}
+
+void comedi_event(comedi_device *dev, comedi_subdevice *s)
+{
+	comedi_async *async = s->async;
+	unsigned runflags = 0;
+	unsigned runflags_mask = 0;
+
+	/* DPRINTK("comedi_event 0x%x\n",mask); */
+
+	if ((comedi_get_subdevice_runflags(s) & SRF_RUNNING) == 0)
+		return;
+
+	if (s->async->
+	    events & (COMEDI_CB_EOA | COMEDI_CB_ERROR | COMEDI_CB_OVERFLOW)) {
+		runflags_mask |= SRF_RUNNING;
+	}
+	/* remember if an error event has occured, so an error
+	 * can be returned the next time the user does a read() */
+	if (s->async->events & (COMEDI_CB_ERROR | COMEDI_CB_OVERFLOW)) {
+		runflags_mask |= SRF_ERROR;
+		runflags |= SRF_ERROR;
+	}
+	if (runflags_mask) {
+		/*sets SRF_ERROR and SRF_RUNNING together atomically */
+		comedi_set_subdevice_runflags(s, runflags_mask, runflags);
+	}
+
+	if (async->cb_mask & s->async->events) {
+		if (comedi_get_subdevice_runflags(s) & SRF_USER) {
+
+			if (dev->rt) {
+#ifdef CONFIG_COMEDI_RT
+				/* pend wake up */
+				comedi_rt_pend_wakeup(&async->wait_head);
+#else
+				printk
+				    ("BUG: comedi_event() code unreachable\n");
+#endif
+			} else {
+				wake_up_interruptible(&async->wait_head);
+				if (s->subdev_flags & SDF_CMD_READ) {
+					kill_fasync(&dev->async_queue, SIGIO,
+						    POLL_IN);
+				}
+				if (s->subdev_flags & SDF_CMD_WRITE) {
+					kill_fasync(&dev->async_queue, SIGIO,
+						    POLL_OUT);
+				}
+			}
+		} else {
+			if (async->cb_func)
+				async->cb_func(s->async->events, async->cb_arg);
+			/* XXX bug here.  If subdevice A is rt, and
+			 * subdevice B tries to callback to a normal
+			 * linux kernel function, it will be at the
+			 * wrong priority.  Since this isn't very
+			 * common, I'm not going to worry about it. */
+		}
+	}
+	s->async->events = 0;
+}
+
+void comedi_set_subdevice_runflags(comedi_subdevice *s, unsigned mask,
+				   unsigned bits)
+{
+	unsigned long flags;
+
+	comedi_spin_lock_irqsave(&s->spin_lock, flags);
+	s->runflags &= ~mask;
+	s->runflags |= (bits & mask);
+	comedi_spin_unlock_irqrestore(&s->spin_lock, flags);
+}
+
+unsigned comedi_get_subdevice_runflags(comedi_subdevice *s)
+{
+	unsigned long flags;
+	unsigned runflags;
+
+	comedi_spin_lock_irqsave(&s->spin_lock, flags);
+	runflags = s->runflags;
+	comedi_spin_unlock_irqrestore(&s->spin_lock, flags);
+	return runflags;
+}
+
+static int is_device_busy(comedi_device *dev)
+{
+	comedi_subdevice *s;
+	int i;
+
+	if (!dev->attached)
+		return 0;
+
+	for (i = 0; i < dev->n_subdevices; i++) {
+		s = dev->subdevices + i;
+		if (s->busy)
+			return 1;
+		if (s->async && s->async->mmap_count)
+			return 1;
+	}
+
+	return 0;
+}
+
+void comedi_device_init(comedi_device *dev)
+{
+	memset(dev, 0, sizeof(comedi_device));
+	spin_lock_init(&dev->spinlock);
+	mutex_init(&dev->mutex);
+	dev->minor = -1;
+}
+
+void comedi_device_cleanup(comedi_device *dev)
+{
+	if (dev == NULL)
+		return;
+	mutex_lock(&dev->mutex);
+	comedi_device_detach(dev);
+	mutex_unlock(&dev->mutex);
+	mutex_destroy(&dev->mutex);
+}
+
+int comedi_alloc_board_minor(struct device *hardware_device)
+{
+	unsigned long flags;
+	struct comedi_device_file_info *info;
+	device_create_result_type *csdev;
+	unsigned i;
+
+	info = kzalloc(sizeof(struct comedi_device_file_info), GFP_KERNEL);
+	if (info == NULL)
+		return -ENOMEM;
+	info->device = kzalloc(sizeof(comedi_device), GFP_KERNEL);
+	if (info->device == NULL) {
+		kfree(info);
+		return -ENOMEM;
+	}
+	comedi_device_init(info->device);
+	comedi_spin_lock_irqsave(&comedi_file_info_table_lock, flags);
+	for (i = 0; i < COMEDI_NUM_BOARD_MINORS; ++i) {
+		if (comedi_file_info_table[i] == NULL) {
+			comedi_file_info_table[i] = info;
+			break;
+		}
+	}
+	comedi_spin_unlock_irqrestore(&comedi_file_info_table_lock, flags);
+	if (i == COMEDI_NUM_BOARD_MINORS) {
+		comedi_device_cleanup(info->device);
+		kfree(info->device);
+		kfree(info);
+		rt_printk
+		    ("comedi: error: ran out of minor numbers for board device files.\n");
+		return -EBUSY;
+	}
+	info->device->minor = i;
+	csdev = COMEDI_DEVICE_CREATE(comedi_class, NULL,
+				     MKDEV(COMEDI_MAJOR, i), NULL,
+				     hardware_device, "comedi%i", i);
+	if (!IS_ERR(csdev))
+		info->device->class_dev = csdev;
+
+	return i;
+}
+
+void comedi_free_board_minor(unsigned minor)
+{
+	unsigned long flags;
+	struct comedi_device_file_info *info;
+
+	BUG_ON(minor >= COMEDI_NUM_BOARD_MINORS);
+	comedi_spin_lock_irqsave(&comedi_file_info_table_lock, flags);
+	info = comedi_file_info_table[minor];
+	comedi_file_info_table[minor] = NULL;
+	comedi_spin_unlock_irqrestore(&comedi_file_info_table_lock, flags);
+
+	if (info) {
+		comedi_device *dev = info->device;
+		if (dev) {
+			if (dev->class_dev) {
+				device_destroy(comedi_class,
+					       MKDEV(COMEDI_MAJOR, dev->minor));
+			}
+			comedi_device_cleanup(dev);
+			kfree(dev);
+		}
+		kfree(info);
+	}
+}
+
+int comedi_alloc_subdevice_minor(comedi_device *dev, comedi_subdevice *s)
+{
+	unsigned long flags;
+	struct comedi_device_file_info *info;
+	device_create_result_type *csdev;
+	unsigned i;
+
+	info = kmalloc(sizeof(struct comedi_device_file_info), GFP_KERNEL);
+	if (info == NULL)
+		return -ENOMEM;
+	info->device = dev;
+	info->read_subdevice = s;
+	info->write_subdevice = s;
+	comedi_spin_lock_irqsave(&comedi_file_info_table_lock, flags);
+	for (i = COMEDI_FIRST_SUBDEVICE_MINOR; i < COMEDI_NUM_BOARD_MINORS; ++i) {
+		if (comedi_file_info_table[i] == NULL) {
+			comedi_file_info_table[i] = info;
+			break;
+		}
+	}
+	comedi_spin_unlock_irqrestore(&comedi_file_info_table_lock, flags);
+	if (i == COMEDI_NUM_MINORS) {
+		kfree(info);
+		rt_printk
+		    ("comedi: error: ran out of minor numbers for board device files.\n");
+		return -EBUSY;
+	}
+	s->minor = i;
+	csdev = COMEDI_DEVICE_CREATE(comedi_class, dev->class_dev,
+				     MKDEV(COMEDI_MAJOR, i), NULL, NULL,
+				     "comedi%i_subd%i", dev->minor,
+				     (int)(s - dev->subdevices));
+	if (!IS_ERR(csdev))
+		s->class_dev = csdev;
+
+	return i;
+}
+
+void comedi_free_subdevice_minor(comedi_subdevice *s)
+{
+	unsigned long flags;
+	struct comedi_device_file_info *info;
+
+	if (s == NULL)
+		return;
+	if (s->minor < 0)
+		return;
+
+	BUG_ON(s->minor >= COMEDI_NUM_MINORS);
+	BUG_ON(s->minor < COMEDI_FIRST_SUBDEVICE_MINOR);
+
+	comedi_spin_lock_irqsave(&comedi_file_info_table_lock, flags);
+	info = comedi_file_info_table[s->minor];
+	comedi_file_info_table[s->minor] = NULL;
+	comedi_spin_unlock_irqrestore(&comedi_file_info_table_lock, flags);
+
+	if (s->class_dev) {
+		device_destroy(comedi_class, MKDEV(COMEDI_MAJOR, s->minor));
+		s->class_dev = NULL;
+	}
+	kfree(info);
+}
+
+struct comedi_device_file_info *comedi_get_device_file_info(unsigned minor)
+{
+	unsigned long flags;
+	struct comedi_device_file_info *info;
+
+	BUG_ON(minor >= COMEDI_NUM_MINORS);
+	comedi_spin_lock_irqsave(&comedi_file_info_table_lock, flags);
+	info = comedi_file_info_table[minor];
+	comedi_spin_unlock_irqrestore(&comedi_file_info_table_lock, flags);
+	return info;
+}
diff --git a/drivers/staging/comedi/comedi_fops.h b/drivers/staging/comedi/comedi_fops.h
new file mode 100644
index 00000000000..63f8df558e8
--- /dev/null
+++ b/drivers/staging/comedi/comedi_fops.h
@@ -0,0 +1,8 @@
+
+#ifndef _COMEDI_FOPS_H
+#define _COMEDI_FOPS_H
+
+extern struct class *comedi_class;
+extern const struct file_operations comedi_fops;
+
+#endif /* _COMEDI_FOPS_H */
diff --git a/drivers/staging/comedi/comedi_ksyms.c b/drivers/staging/comedi/comedi_ksyms.c
new file mode 100644
index 00000000000..90d57282efb
--- /dev/null
+++ b/drivers/staging/comedi/comedi_ksyms.c
@@ -0,0 +1,77 @@
+/*
+    module/exp_ioctl.c
+    exported comedi functions
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1997-8 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#define __NO_VERSION__
+#ifndef EXPORT_SYMTAB
+#define EXPORT_SYMTAB
+#endif
+
+#include "comedidev.h"
+
+/* for drivers */
+EXPORT_SYMBOL(comedi_driver_register);
+EXPORT_SYMBOL(comedi_driver_unregister);
+//EXPORT_SYMBOL(comedi_bufcheck);
+//EXPORT_SYMBOL(comedi_done);
+//EXPORT_SYMBOL(comedi_error_done);
+EXPORT_SYMBOL(comedi_error);
+//EXPORT_SYMBOL(comedi_eobuf);
+//EXPORT_SYMBOL(comedi_eos);
+EXPORT_SYMBOL(comedi_event);
+EXPORT_SYMBOL(comedi_get_subdevice_runflags);
+EXPORT_SYMBOL(comedi_set_subdevice_runflags);
+EXPORT_SYMBOL(range_bipolar10);
+EXPORT_SYMBOL(range_bipolar5);
+EXPORT_SYMBOL(range_bipolar2_5);
+EXPORT_SYMBOL(range_unipolar10);
+EXPORT_SYMBOL(range_unipolar5);
+EXPORT_SYMBOL(range_unknown);
+#ifdef CONFIG_COMEDI_RT
+EXPORT_SYMBOL(comedi_free_irq);
+EXPORT_SYMBOL(comedi_request_irq);
+EXPORT_SYMBOL(comedi_switch_to_rt);
+EXPORT_SYMBOL(comedi_switch_to_non_rt);
+EXPORT_SYMBOL(rt_pend_call);
+#endif
+#ifdef CONFIG_COMEDI_DEBUG
+EXPORT_SYMBOL(comedi_debug);
+#endif
+EXPORT_SYMBOL_GPL(comedi_alloc_board_minor);
+EXPORT_SYMBOL_GPL(comedi_free_board_minor);
+EXPORT_SYMBOL_GPL(comedi_pci_auto_config);
+EXPORT_SYMBOL_GPL(comedi_pci_auto_unconfig);
+
+/* for kcomedilib */
+EXPORT_SYMBOL(check_chanlist);
+EXPORT_SYMBOL_GPL(comedi_get_device_file_info);
+
+EXPORT_SYMBOL(comedi_buf_put);
+EXPORT_SYMBOL(comedi_buf_get);
+EXPORT_SYMBOL(comedi_buf_read_n_available);
+EXPORT_SYMBOL(comedi_buf_write_free);
+EXPORT_SYMBOL(comedi_buf_write_alloc);
+EXPORT_SYMBOL(comedi_buf_read_free);
+EXPORT_SYMBOL(comedi_buf_read_alloc);
+EXPORT_SYMBOL(comedi_buf_memcpy_to);
+EXPORT_SYMBOL(comedi_buf_memcpy_from);
+EXPORT_SYMBOL(comedi_reset_async_buf);
diff --git a/drivers/staging/comedi/comedi_rt.h b/drivers/staging/comedi/comedi_rt.h
new file mode 100644
index 00000000000..61852bf5adc
--- /dev/null
+++ b/drivers/staging/comedi/comedi_rt.h
@@ -0,0 +1,150 @@
+/*
+    module/comedi_rt.h
+    header file for real-time structures, variables, and constants
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1997-2000 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#ifndef _COMEDI_RT_H
+#define _COMEDI_RT_H
+
+#ifndef _COMEDIDEV_H
+#error comedi_rt.h should only be included by comedidev.h
+#endif
+
+#include <linux/version.h>
+#include <linux/kdev_t.h>
+#include <linux/slab.h>
+#include <linux/errno.h>
+#include <linux/spinlock.h>
+#include <linux/delay.h>
+
+#ifdef CONFIG_COMEDI_RT
+
+#ifdef CONFIG_COMEDI_RTAI
+#include <rtai.h>
+#include <rtai_sched.h>
+#include <rtai_version.h>
+#endif
+#ifdef CONFIG_COMEDI_RTL
+#include <rtl_core.h>
+#include <rtl_time.h>
+/* #ifdef RTLINUX_VERSION_CODE */
+#include <rtl_sync.h>
+/* #endif */
+#define rt_printk rtl_printf
+#endif
+#ifdef CONFIG_COMEDI_FUSION
+#define rt_printk(format, args...) printk(format , ## args)
+#endif /* CONFIG_COMEDI_FUSION */
+#ifdef CONFIG_PRIORITY_IRQ
+#define rt_printk printk
+#endif
+
+int comedi_request_irq(unsigned int irq, irqreturn_t(*handler) (int,
+		void *PT_REGS_ARG), unsigned long flags, const char *device,
+		comedi_device *dev_id);
+void comedi_free_irq(unsigned int irq, comedi_device *dev_id);
+void comedi_rt_init(void);
+void comedi_rt_cleanup(void);
+int comedi_switch_to_rt(comedi_device *dev);
+void comedi_switch_to_non_rt(comedi_device *dev);
+void comedi_rt_pend_wakeup(wait_queue_head_t *q);
+extern int rt_pend_call(void (*func) (int arg1, void *arg2), int arg1,
+	void *arg2);
+
+#else
+
+#define comedi_request_irq(a, b, c, d, e) request_irq(a, b, c, d, e)
+#define comedi_free_irq(a, b) free_irq(a, b)
+#define comedi_rt_init() do {} while (0)
+#define comedi_rt_cleanup() do {} while (0)
+#define comedi_switch_to_rt(a) (-1)
+#define comedi_switch_to_non_rt(a) do {} while (0)
+#define comedi_rt_pend_wakeup(a) do {} while (0)
+
+#define rt_printk(format, args...)	printk(format, ##args)
+
+#endif
+
+/* Define a spin_lock_irqsave function that will work with rt or without.
+ * Use inline functions instead of just macros to enforce some type checking.
+ */
+#define comedi_spin_lock_irqsave(lock_ptr, flags) \
+	(flags = __comedi_spin_lock_irqsave(lock_ptr))
+
+static inline unsigned long __comedi_spin_lock_irqsave(spinlock_t *lock_ptr)
+{
+	unsigned long flags;
+
+#if defined(CONFIG_COMEDI_RTAI)
+	flags = rt_spin_lock_irqsave(lock_ptr);
+
+#elif defined(CONFIG_COMEDI_RTL)
+	rtl_spin_lock_irqsave(lock_ptr, flags);
+
+#elif defined(CONFIG_COMEDI_RTL_V1)
+	rtl_spin_lock_irqsave(lock_ptr, flags);
+
+#elif defined(CONFIG_COMEDI_FUSION)
+	rthal_spin_lock_irqsave(lock_ptr, flags);
+#else
+	spin_lock_irqsave(lock_ptr, flags);
+
+#endif
+
+	return flags;
+}
+
+static inline void comedi_spin_unlock_irqrestore(spinlock_t *lock_ptr,
+	unsigned long flags)
+{
+
+#if defined(CONFIG_COMEDI_RTAI)
+	rt_spin_unlock_irqrestore(flags, lock_ptr);
+
+#elif defined(CONFIG_COMEDI_RTL)
+	rtl_spin_unlock_irqrestore(lock_ptr, flags);
+
+#elif defined(CONFIG_COMEDI_RTL_V1)
+	rtl_spin_unlock_irqrestore(lock_ptr, flags);
+#elif defined(CONFIG_COMEDI_FUSION)
+	rthal_spin_unlock_irqrestore(lock_ptr, flags);
+#else
+	spin_unlock_irqrestore(lock_ptr, flags);
+
+#endif
+
+}
+
+/* define a RT safe udelay */
+static inline void comedi_udelay(unsigned int usec)
+{
+#if defined(CONFIG_COMEDI_RTAI)
+	static const int nanosec_per_usec = 1000;
+	rt_busy_sleep(usec * nanosec_per_usec);
+#elif defined(CONFIG_COMEDI_RTL)
+	static const int nanosec_per_usec = 1000;
+	rtl_delay(usec * nanosec_per_usec);
+#else
+	udelay(usec);
+#endif
+}
+
+#endif
diff --git a/drivers/staging/comedi/comedidev.h b/drivers/staging/comedi/comedidev.h
new file mode 100644
index 00000000000..3735355d3c5
--- /dev/null
+++ b/drivers/staging/comedi/comedidev.h
@@ -0,0 +1,537 @@
+/*
+    include/linux/comedidev.h
+    header file for kernel-only structures, variables, and constants
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1997-2000 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#ifndef _COMEDIDEV_H
+#define _COMEDIDEV_H
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/version.h>
+#include <linux/kdev_t.h>
+#include <linux/slab.h>
+#include <linux/errno.h>
+#include <linux/spinlock.h>
+#include <linux/mutex.h>
+#include <linux/wait.h>
+#include <linux/mm.h>
+#include <linux/init.h>
+#include <linux/vmalloc.h>
+#include <linux/mm.h>
+#include "interrupt.h"
+#include <linux/dma-mapping.h>
+#include <linux/uaccess.h>
+#include <linux/io.h>
+
+#include "comedi.h"
+
+#define DPRINTK(format, args...)	do {		\
+	if (comedi_debug)				\
+		printk(KERN_DEBUG "comedi: " format , ## args);	\
+} while (0)
+
+#define COMEDI_VERSION(a, b, c) (((a) << 16) + ((b) << 8) + (c))
+#define COMEDI_VERSION_CODE COMEDI_VERSION(COMEDI_MAJORVERSION, COMEDI_MINORVERSION, COMEDI_MICROVERSION)
+#define COMEDI_RELEASE VERSION
+
+#define COMEDI_INITCLEANUP_NOMODULE(x)					\
+	static int __init x ## _init_module(void)			\
+		{return comedi_driver_register(&(x));}			\
+	static void __exit x ## _cleanup_module(void)			\
+		{comedi_driver_unregister(&(x));} 			\
+	module_init(x ## _init_module);					\
+	module_exit(x ## _cleanup_module);					\
+
+#define COMEDI_MODULE_MACROS						\
+	MODULE_AUTHOR("Comedi http://www.comedi.org");		\
+	MODULE_DESCRIPTION("Comedi low-level driver");			\
+	MODULE_LICENSE("GPL");						\
+
+#define COMEDI_INITCLEANUP(x)						\
+	COMEDI_MODULE_MACROS		\
+	COMEDI_INITCLEANUP_NOMODULE(x)
+
+#define COMEDI_PCI_INITCLEANUP_NOMODULE(comedi_driver, pci_id_table) \
+	static int __devinit comedi_driver ## _pci_probe(struct pci_dev *dev, \
+		const struct pci_device_id *ent) \
+	{ \
+		return comedi_pci_auto_config(dev, comedi_driver.driver_name); \
+	} \
+	static void __devexit comedi_driver ## _pci_remove(struct pci_dev *dev) \
+	{ \
+		comedi_pci_auto_unconfig(dev); \
+	} \
+	static struct pci_driver comedi_driver ## _pci_driver = \
+	{ \
+		.id_table = pci_id_table, \
+		.probe = &comedi_driver ## _pci_probe, \
+		.remove = __devexit_p(&comedi_driver ## _pci_remove) \
+	}; \
+	static int __init comedi_driver ## _init_module(void) \
+	{ \
+		int retval; \
+		retval = comedi_driver_register(&comedi_driver); \
+		if (retval < 0) \
+			return retval; \
+		comedi_driver ## _pci_driver.name = (char *)comedi_driver.driver_name; \
+		return pci_register_driver(&comedi_driver ## _pci_driver); \
+	} \
+	static void __exit comedi_driver ## _cleanup_module(void) \
+	{ \
+		pci_unregister_driver(&comedi_driver ## _pci_driver); \
+		comedi_driver_unregister(&comedi_driver); \
+	} \
+	module_init(comedi_driver ## _init_module); \
+	module_exit(comedi_driver ## _cleanup_module);
+
+#define COMEDI_PCI_INITCLEANUP(comedi_driver, pci_id_table) \
+	COMEDI_MODULE_MACROS \
+	COMEDI_PCI_INITCLEANUP_NOMODULE(comedi_driver, pci_id_table)
+
+#define PCI_VENDOR_ID_INOVA		0x104c
+#define PCI_VENDOR_ID_NATINST		0x1093
+#define PCI_VENDOR_ID_DATX		0x1116
+#define PCI_VENDOR_ID_COMPUTERBOARDS	0x1307
+#define PCI_VENDOR_ID_ADVANTECH		0x13fe
+#define PCI_VENDOR_ID_RTD		0x1435
+#define PCI_VENDOR_ID_AMPLICON		0x14dc
+#define PCI_VENDOR_ID_ADLINK		0x144a
+#define PCI_VENDOR_ID_ICP		0x104c
+#define PCI_VENDOR_ID_CONTEC		0x1221
+#define PCI_VENDOR_ID_MEILHAUS		0x1402
+
+#define COMEDI_NUM_MINORS 0x100
+#define COMEDI_NUM_LEGACY_MINORS 0x10
+#define COMEDI_NUM_BOARD_MINORS 0x30
+#define COMEDI_FIRST_SUBDEVICE_MINOR COMEDI_NUM_BOARD_MINORS
+
+typedef struct comedi_device_struct comedi_device;
+typedef struct comedi_subdevice_struct comedi_subdevice;
+typedef struct comedi_async_struct comedi_async;
+typedef struct comedi_driver_struct comedi_driver;
+typedef struct comedi_lrange_struct comedi_lrange;
+
+typedef struct device device_create_result_type;
+
+#define COMEDI_DEVICE_CREATE(cs, parent, devt, drvdata, device, fmt...) \
+	device_create(cs, ((parent) ? (parent) : (device)), devt, drvdata, fmt)
+
+struct comedi_subdevice_struct {
+	comedi_device *device;
+	int type;
+	int n_chan;
+	volatile int subdev_flags;
+	int len_chanlist;	/* maximum length of channel/gain list */
+
+	void *private;
+
+	comedi_async *async;
+
+	void *lock;
+	void *busy;
+	unsigned runflags;
+	spinlock_t spin_lock;
+
+	int io_bits;
+
+	lsampl_t maxdata;	/* if maxdata==0, use list */
+	const lsampl_t *maxdata_list;	/* list is channel specific */
+
+	unsigned int flags;
+	const unsigned int *flaglist;
+
+	unsigned int settling_time_0;
+
+	const comedi_lrange *range_table;
+	const comedi_lrange *const *range_table_list;
+
+	unsigned int *chanlist;	/* driver-owned chanlist (not used) */
+
+	int (*insn_read) (comedi_device *, comedi_subdevice *, comedi_insn *,
+		lsampl_t *);
+	int (*insn_write) (comedi_device *, comedi_subdevice *, comedi_insn *,
+		lsampl_t *);
+	int (*insn_bits) (comedi_device *, comedi_subdevice *, comedi_insn *,
+		lsampl_t *);
+	int (*insn_config) (comedi_device *, comedi_subdevice *, comedi_insn *,
+		lsampl_t *);
+
+	int (*do_cmd) (comedi_device *, comedi_subdevice *);
+	int (*do_cmdtest) (comedi_device *, comedi_subdevice *, comedi_cmd *);
+	int (*poll) (comedi_device *, comedi_subdevice *);
+	int (*cancel) (comedi_device *, comedi_subdevice *);
+	/* int (*do_lock)(comedi_device *,comedi_subdevice *); */
+	/* int (*do_unlock)(comedi_device *,comedi_subdevice *); */
+
+	/* called when the buffer changes */
+	int (*buf_change) (comedi_device *dev, comedi_subdevice *s,
+		unsigned long new_size);
+
+	void (*munge) (comedi_device *dev, comedi_subdevice *s, void *data,
+		unsigned int num_bytes, unsigned int start_chan_index);
+	enum dma_data_direction async_dma_dir;
+
+	unsigned int state;
+
+	device_create_result_type *class_dev;
+	int minor;
+};
+
+struct comedi_buf_page {
+	void *virt_addr;
+	dma_addr_t dma_addr;
+};
+
+struct comedi_async_struct {
+	comedi_subdevice *subdevice;
+
+	void *prealloc_buf;	/* pre-allocated buffer */
+	unsigned int prealloc_bufsz;	/* buffer size, in bytes */
+	struct comedi_buf_page *buf_page_list;	/* virtual and dma address of each page */
+	unsigned n_buf_pages;	/* num elements in buf_page_list */
+
+	unsigned int max_bufsize;	/* maximum buffer size, bytes */
+	unsigned int mmap_count;	/* current number of mmaps of prealloc_buf */
+
+	unsigned int buf_write_count;	/* byte count for writer (write completed) */
+	unsigned int buf_write_alloc_count;	/* byte count for writer (allocated for writing) */
+	unsigned int buf_read_count;	/* byte count for reader (read completed) */
+	unsigned int buf_read_alloc_count;	/* byte count for reader (allocated for reading) */
+
+	unsigned int buf_write_ptr;	/* buffer marker for writer */
+	unsigned int buf_read_ptr;	/* buffer marker for reader */
+
+	unsigned int cur_chan;	/* useless channel marker for interrupt */
+	/* number of bytes that have been received for current scan */
+	unsigned int scan_progress;
+	/* keeps track of where we are in chanlist as for munging */
+	unsigned int munge_chan;
+	/* number of bytes that have been munged */
+	unsigned int munge_count;
+	/* buffer marker for munging */
+	unsigned int munge_ptr;
+
+	unsigned int events;	/* events that have occurred */
+
+	comedi_cmd cmd;
+
+	wait_queue_head_t wait_head;
+
+	/* callback stuff */
+	unsigned int cb_mask;
+	int (*cb_func) (unsigned int flags, void *);
+	void *cb_arg;
+
+	int (*inttrig) (comedi_device *dev, comedi_subdevice *s,
+			unsigned int x);
+};
+
+struct comedi_driver_struct {
+	struct comedi_driver_struct *next;
+
+	const char *driver_name;
+	struct module *module;
+	int (*attach) (comedi_device *, comedi_devconfig *);
+	int (*detach) (comedi_device *);
+
+	/* number of elements in board_name and board_id arrays */
+	unsigned int num_names;
+	const char *const *board_name;
+	/* offset in bytes from one board name pointer to the next */
+	int offset;
+};
+
+struct comedi_device_struct {
+	int use_count;
+	comedi_driver *driver;
+	void *private;
+
+	device_create_result_type *class_dev;
+	int minor;
+	/* hw_dev is passed to dma_alloc_coherent when allocating async buffers
+	 * for subdevices that have async_dma_dir set to something other than
+	 * DMA_NONE */
+	struct device *hw_dev;
+
+	const char *board_name;
+	const void *board_ptr;
+	int attached;
+	int rt;
+	spinlock_t spinlock;
+	struct mutex mutex;
+	int in_request_module;
+
+	int n_subdevices;
+	comedi_subdevice *subdevices;
+
+	/* dumb */
+	unsigned long iobase;
+	unsigned int irq;
+
+	comedi_subdevice *read_subdev;
+	comedi_subdevice *write_subdev;
+
+	struct fasync_struct *async_queue;
+
+	void (*open) (comedi_device *dev);
+	void (*close) (comedi_device *dev);
+};
+
+struct comedi_device_file_info {
+	comedi_device *device;
+	comedi_subdevice *read_subdevice;
+	comedi_subdevice *write_subdevice;
+};
+
+#ifdef CONFIG_COMEDI_DEBUG
+extern int comedi_debug;
+#else
+static const int comedi_debug;
+#endif
+
+/*
+ * function prototypes
+ */
+
+void comedi_event(comedi_device *dev, comedi_subdevice *s);
+void comedi_error(const comedi_device *dev, const char *s);
+
+/* we can expand the number of bits used to encode devices/subdevices into
+ the minor number soon, after more distros support > 8 bit minor numbers
+ (like after Debian Etch gets released) */
+enum comedi_minor_bits {
+	COMEDI_DEVICE_MINOR_MASK = 0xf,
+	COMEDI_SUBDEVICE_MINOR_MASK = 0xf0
+};
+static const unsigned COMEDI_SUBDEVICE_MINOR_SHIFT = 4;
+static const unsigned COMEDI_SUBDEVICE_MINOR_OFFSET = 1;
+
+struct comedi_device_file_info *comedi_get_device_file_info(unsigned minor);
+
+static inline comedi_subdevice *comedi_get_read_subdevice(
+				const struct comedi_device_file_info *info)
+{
+	if (info->read_subdevice)
+		return info->read_subdevice;
+	if (info->device == NULL)
+		return NULL;
+	return info->device->read_subdev;
+}
+
+static inline comedi_subdevice *comedi_get_write_subdevice(
+				const struct comedi_device_file_info *info)
+{
+	if (info->write_subdevice)
+		return info->write_subdevice;
+	if (info->device == NULL)
+		return NULL;
+	return info->device->write_subdev;
+}
+
+void comedi_device_detach(comedi_device *dev);
+int comedi_device_attach(comedi_device *dev, comedi_devconfig *it);
+int comedi_driver_register(comedi_driver *);
+int comedi_driver_unregister(comedi_driver *);
+
+void init_polling(void);
+void cleanup_polling(void);
+void start_polling(comedi_device *);
+void stop_polling(comedi_device *);
+
+int comedi_buf_alloc(comedi_device *dev, comedi_subdevice *s, unsigned long
+	new_size);
+
+#ifdef CONFIG_PROC_FS
+void comedi_proc_init(void);
+void comedi_proc_cleanup(void);
+#else
+static inline void comedi_proc_init(void)
+{
+}
+static inline void comedi_proc_cleanup(void)
+{
+}
+#endif
+
+/* subdevice runflags */
+enum subdevice_runflags {
+	SRF_USER = 0x00000001,
+	SRF_RT = 0x00000002,
+	/* indicates an COMEDI_CB_ERROR event has occurred since the last
+	 * command was started */
+	SRF_ERROR = 0x00000004,
+	SRF_RUNNING = 0x08000000
+};
+
+/*
+   various internal comedi functions
+ */
+
+int do_rangeinfo_ioctl(comedi_device *dev, comedi_rangeinfo *arg);
+int check_chanlist(comedi_subdevice *s, int n, unsigned int *chanlist);
+void comedi_set_subdevice_runflags(comedi_subdevice *s, unsigned mask,
+	unsigned bits);
+unsigned comedi_get_subdevice_runflags(comedi_subdevice *s);
+int insn_inval(comedi_device *dev, comedi_subdevice *s,
+	comedi_insn *insn, lsampl_t *data);
+
+/* range stuff */
+
+#define RANGE(a, b)		{(a)*1e6, (b)*1e6, 0}
+#define RANGE_ext(a, b)		{(a)*1e6, (b)*1e6, RF_EXTERNAL}
+#define RANGE_mA(a, b)		{(a)*1e6, (b)*1e6, UNIT_mA}
+#define RANGE_unitless(a, b)	{(a)*1e6, (b)*1e6, 0}	/* XXX */
+#define BIP_RANGE(a)		{-(a)*1e6, (a)*1e6, 0}
+#define UNI_RANGE(a)		{0, (a)*1e6, 0}
+
+extern const comedi_lrange range_bipolar10;
+extern const comedi_lrange range_bipolar5;
+extern const comedi_lrange range_bipolar2_5;
+extern const comedi_lrange range_unipolar10;
+extern const comedi_lrange range_unipolar5;
+extern const comedi_lrange range_unknown;
+
+#define range_digital		range_unipolar5
+
+#if __GNUC__ >= 3
+#define GCC_ZERO_LENGTH_ARRAY
+#else
+#define GCC_ZERO_LENGTH_ARRAY 0
+#endif
+
+struct comedi_lrange_struct {
+	int length;
+	comedi_krange range[GCC_ZERO_LENGTH_ARRAY];
+};
+
+/* some silly little inline functions */
+
+static inline int alloc_subdevices(comedi_device *dev,
+				   unsigned int num_subdevices)
+{
+	unsigned i;
+
+	dev->n_subdevices = num_subdevices;
+	dev->subdevices =
+		kcalloc(num_subdevices, sizeof(comedi_subdevice), GFP_KERNEL);
+	if (!dev->subdevices)
+		return -ENOMEM;
+	for (i = 0; i < num_subdevices; ++i) {
+		dev->subdevices[i].device = dev;
+		dev->subdevices[i].async_dma_dir = DMA_NONE;
+		spin_lock_init(&dev->subdevices[i].spin_lock);
+		dev->subdevices[i].minor = -1;
+	}
+	return 0;
+}
+
+static inline int alloc_private(comedi_device *dev, int size)
+{
+	dev->private = kzalloc(size, GFP_KERNEL);
+	if (!dev->private)
+		return -ENOMEM;
+	return 0;
+}
+
+static inline unsigned int bytes_per_sample(const comedi_subdevice *subd)
+{
+	if (subd->subdev_flags & SDF_LSAMPL)
+		return sizeof(lsampl_t);
+	else
+		return sizeof(sampl_t);
+}
+
+/* must be used in attach to set dev->hw_dev if you wish to dma directly
+into comedi's buffer */
+static inline void comedi_set_hw_dev(comedi_device *dev, struct device *hw_dev)
+{
+	if (dev->hw_dev)
+		put_device(dev->hw_dev);
+
+	dev->hw_dev = hw_dev;
+	if (dev->hw_dev) {
+		dev->hw_dev = get_device(dev->hw_dev);
+		BUG_ON(dev->hw_dev == NULL);
+	}
+}
+
+int comedi_buf_put(comedi_async *async, sampl_t x);
+int comedi_buf_get(comedi_async *async, sampl_t *x);
+
+unsigned int comedi_buf_write_n_available(comedi_async *async);
+unsigned int comedi_buf_write_alloc(comedi_async *async, unsigned int nbytes);
+unsigned int comedi_buf_write_alloc_strict(comedi_async *async,
+	unsigned int nbytes);
+unsigned comedi_buf_write_free(comedi_async *async, unsigned int nbytes);
+unsigned comedi_buf_read_alloc(comedi_async *async, unsigned nbytes);
+unsigned comedi_buf_read_free(comedi_async *async, unsigned int nbytes);
+unsigned int comedi_buf_read_n_available(comedi_async *async);
+void comedi_buf_memcpy_to(comedi_async *async, unsigned int offset,
+	const void *source, unsigned int num_bytes);
+void comedi_buf_memcpy_from(comedi_async *async, unsigned int offset,
+	void *destination, unsigned int num_bytes);
+static inline unsigned comedi_buf_write_n_allocated(comedi_async *async)
+{
+	return async->buf_write_alloc_count - async->buf_write_count;
+}
+static inline unsigned comedi_buf_read_n_allocated(comedi_async *async)
+{
+	return async->buf_read_alloc_count - async->buf_read_count;
+}
+
+void comedi_reset_async_buf(comedi_async *async);
+
+static inline void *comedi_aux_data(int options[], int n)
+{
+	unsigned long address;
+	unsigned long addressLow;
+	int bit_shift;
+	if (sizeof(int) >= sizeof(void *))
+		address = options[COMEDI_DEVCONF_AUX_DATA_LO];
+	else {
+		address = options[COMEDI_DEVCONF_AUX_DATA_HI];
+		bit_shift = sizeof(int) * 8;
+		address <<= bit_shift;
+		addressLow = options[COMEDI_DEVCONF_AUX_DATA_LO];
+		addressLow &= (1UL << bit_shift) - 1;
+		address |= addressLow;
+	}
+	if (n >= 1)
+		address += options[COMEDI_DEVCONF_AUX_DATA0_LENGTH];
+	if (n >= 2)
+		address += options[COMEDI_DEVCONF_AUX_DATA1_LENGTH];
+	if (n >= 3)
+		address += options[COMEDI_DEVCONF_AUX_DATA2_LENGTH];
+	BUG_ON(n > 3);
+	return (void *)address;
+}
+
+int comedi_alloc_board_minor(struct device *hardware_device);
+void comedi_free_board_minor(unsigned minor);
+int comedi_alloc_subdevice_minor(comedi_device *dev, comedi_subdevice *s);
+void comedi_free_subdevice_minor(comedi_subdevice *s);
+int comedi_pci_auto_config(struct pci_dev *pcidev, const char *board_name);
+void comedi_pci_auto_unconfig(struct pci_dev *pcidev);
+
+#include "comedi_rt.h"
+
+#endif /* _COMEDIDEV_H */
diff --git a/drivers/staging/comedi/comedilib.h b/drivers/staging/comedi/comedilib.h
new file mode 100644
index 00000000000..fc5fc015726
--- /dev/null
+++ b/drivers/staging/comedi/comedilib.h
@@ -0,0 +1,192 @@
+/*
+    linux/include/comedilib.h
+    header file for kcomedilib
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1998-2001 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#ifndef _LINUX_COMEDILIB_H
+#define _LINUX_COMEDILIB_H
+
+#include "comedi.h"
+
+/* Kernel internal stuff.  Needed by real-time modules and such. */
+
+#ifndef __KERNEL__
+#error linux/comedilib.h should not be included by non-kernel-space code
+#endif
+
+/* exported functions */
+
+#ifndef KCOMEDILIB_DEPRECATED
+
+typedef void comedi_t;
+
+/* these functions may not be called at real-time priority */
+
+comedi_t *comedi_open(const char *path);
+int comedi_close(comedi_t *dev);
+
+/* these functions may be called at any priority, but may fail at
+   real-time priority */
+
+int comedi_lock(comedi_t *dev, unsigned int subdev);
+int comedi_unlock(comedi_t *dev, unsigned int subdev);
+
+/* these functions may be called at any priority, but you must hold
+   the lock for the subdevice */
+
+int comedi_loglevel(int loglevel);
+void comedi_perror(const char *s);
+char *comedi_strerror(int errnum);
+int comedi_errno(void);
+int comedi_fileno(comedi_t *dev);
+
+int comedi_cancel(comedi_t *dev, unsigned int subdev);
+int comedi_register_callback(comedi_t *dev, unsigned int subdev,
+	unsigned int mask, int (*cb) (unsigned int, void *), void *arg);
+
+int comedi_command(comedi_t *dev, comedi_cmd *cmd);
+int comedi_command_test(comedi_t *dev, comedi_cmd *cmd);
+int comedi_trigger(comedi_t *dev, unsigned int subdev, comedi_trig *it);
+int __comedi_trigger(comedi_t *dev, unsigned int subdev, comedi_trig *it);
+int comedi_data_write(comedi_t *dev, unsigned int subdev, unsigned int chan,
+	unsigned int range, unsigned int aref, lsampl_t data);
+int comedi_data_read(comedi_t *dev, unsigned int subdev, unsigned int chan,
+	unsigned int range, unsigned int aref, lsampl_t *data);
+int comedi_data_read_hint(comedi_t *dev, unsigned int subdev,
+	unsigned int chan, unsigned int range, unsigned int aref);
+int comedi_data_read_delayed(comedi_t *dev, unsigned int subdev,
+	unsigned int chan, unsigned int range, unsigned int aref,
+	lsampl_t *data, unsigned int nano_sec);
+int comedi_dio_config(comedi_t *dev, unsigned int subdev, unsigned int chan,
+	unsigned int io);
+int comedi_dio_read(comedi_t *dev, unsigned int subdev, unsigned int chan,
+	unsigned int *val);
+int comedi_dio_write(comedi_t *dev, unsigned int subdev, unsigned int chan,
+	unsigned int val);
+int comedi_dio_bitfield(comedi_t *dev, unsigned int subdev, unsigned int mask,
+	unsigned int *bits);
+int comedi_get_n_subdevices(comedi_t *dev);
+int comedi_get_version_code(comedi_t *dev);
+const char *comedi_get_driver_name(comedi_t *dev);
+const char *comedi_get_board_name(comedi_t *dev);
+int comedi_get_subdevice_type(comedi_t *dev, unsigned int subdevice);
+int comedi_find_subdevice_by_type(comedi_t *dev, int type, unsigned int subd);
+int comedi_get_n_channels(comedi_t *dev, unsigned int subdevice);
+lsampl_t comedi_get_maxdata(comedi_t *dev, unsigned int subdevice, unsigned
+	int chan);
+int comedi_get_n_ranges(comedi_t *dev, unsigned int subdevice, unsigned int
+	chan);
+int comedi_do_insn(comedi_t *dev, comedi_insn *insn);
+int comedi_poll(comedi_t *dev, unsigned int subdev);
+
+/* DEPRECATED functions */
+int comedi_get_rangetype(comedi_t *dev, unsigned int subdevice,
+	unsigned int chan);
+
+/* ALPHA functions */
+unsigned int comedi_get_subdevice_flags(comedi_t *dev, unsigned int subdevice);
+int comedi_get_len_chanlist(comedi_t *dev, unsigned int subdevice);
+int comedi_get_krange(comedi_t *dev, unsigned int subdevice, unsigned int
+	chan, unsigned int range, comedi_krange *krange);
+unsigned int comedi_get_buf_head_pos(comedi_t *dev, unsigned int subdevice);
+int comedi_set_user_int_count(comedi_t *dev, unsigned int subdevice,
+	unsigned int buf_user_count);
+int comedi_map(comedi_t *dev, unsigned int subdev, void *ptr);
+int comedi_unmap(comedi_t *dev, unsigned int subdev);
+int comedi_get_buffer_size(comedi_t *dev, unsigned int subdev);
+int comedi_mark_buffer_read(comedi_t *dev, unsigned int subdevice,
+	unsigned int num_bytes);
+int comedi_mark_buffer_written(comedi_t *d, unsigned int subdevice,
+	unsigned int num_bytes);
+int comedi_get_buffer_contents(comedi_t *dev, unsigned int subdevice);
+int comedi_get_buffer_offset(comedi_t *dev, unsigned int subdevice);
+
+#else
+
+/* these functions may not be called at real-time priority */
+
+int comedi_open(unsigned int minor);
+void comedi_close(unsigned int minor);
+
+/* these functions may be called at any priority, but may fail at
+   real-time priority */
+
+int comedi_lock(unsigned int minor, unsigned int subdev);
+int comedi_unlock(unsigned int minor, unsigned int subdev);
+
+/* these functions may be called at any priority, but you must hold
+   the lock for the subdevice */
+
+int comedi_cancel(unsigned int minor, unsigned int subdev);
+int comedi_register_callback(unsigned int minor, unsigned int subdev,
+	unsigned int mask, int (*cb) (unsigned int, void *), void *arg);
+
+int comedi_command(unsigned int minor, comedi_cmd *cmd);
+int comedi_command_test(unsigned int minor, comedi_cmd *cmd);
+int comedi_trigger(unsigned int minor, unsigned int subdev, comedi_trig *it);
+int __comedi_trigger(unsigned int minor, unsigned int subdev, comedi_trig *it);
+int comedi_data_write(unsigned int dev, unsigned int subdev, unsigned int chan,
+	unsigned int range, unsigned int aref, lsampl_t data);
+int comedi_data_read(unsigned int dev, unsigned int subdev, unsigned int chan,
+	unsigned int range, unsigned int aref, lsampl_t *data);
+int comedi_dio_config(unsigned int dev, unsigned int subdev, unsigned int chan,
+	unsigned int io);
+int comedi_dio_read(unsigned int dev, unsigned int subdev, unsigned int chan,
+	unsigned int *val);
+int comedi_dio_write(unsigned int dev, unsigned int subdev, unsigned int chan,
+	unsigned int val);
+int comedi_dio_bitfield(unsigned int dev, unsigned int subdev,
+	unsigned int mask, unsigned int *bits);
+int comedi_get_n_subdevices(unsigned int dev);
+int comedi_get_version_code(unsigned int dev);
+char *comedi_get_driver_name(unsigned int dev);
+char *comedi_get_board_name(unsigned int minor);
+int comedi_get_subdevice_type(unsigned int minor, unsigned int subdevice);
+int comedi_find_subdevice_by_type(unsigned int minor, int type,
+	unsigned int subd);
+int comedi_get_n_channels(unsigned int minor, unsigned int subdevice);
+lsampl_t comedi_get_maxdata(unsigned int minor, unsigned int subdevice, unsigned
+	int chan);
+int comedi_get_n_ranges(unsigned int minor, unsigned int subdevice, unsigned int
+	chan);
+int comedi_do_insn(unsigned int minor, comedi_insn *insn);
+int comedi_poll(unsigned int minor, unsigned int subdev);
+
+/* DEPRECATED functions */
+int comedi_get_rangetype(unsigned int minor, unsigned int subdevice,
+	unsigned int chan);
+
+/* ALPHA functions */
+unsigned int comedi_get_subdevice_flags(unsigned int minor, unsigned int
+	subdevice);
+int comedi_get_len_chanlist(unsigned int minor, unsigned int subdevice);
+int comedi_get_krange(unsigned int minor, unsigned int subdevice, unsigned int
+	chan, unsigned int range, comedi_krange *krange);
+unsigned int comedi_get_buf_head_pos(unsigned int minor, unsigned int
+	subdevice);
+int comedi_set_user_int_count(unsigned int minor, unsigned int subdevice,
+	unsigned int buf_user_count);
+int comedi_map(unsigned int minor, unsigned int subdev, void **ptr);
+int comedi_unmap(unsigned int minor, unsigned int subdev);
+
+#endif
+
+#endif
diff --git a/drivers/staging/comedi/drivers.c b/drivers/staging/comedi/drivers.c
new file mode 100644
index 00000000000..06372b227bb
--- /dev/null
+++ b/drivers/staging/comedi/drivers.c
@@ -0,0 +1,846 @@
+/*
+    module/drivers.c
+    functions for manipulating drivers
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1997-2000 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#define _GNU_SOURCE
+
+#define __NO_VERSION__
+#include "comedi_fops.h"
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/pci.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/fcntl.h>
+#include <linux/delay.h>
+#include <linux/ioport.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+#include "comedidev.h"
+#include "wrapper.h"
+#include <linux/highmem.h>	/* for SuSE brokenness */
+#include <linux/vmalloc.h>
+#include <linux/cdev.h>
+#include <linux/dma-mapping.h>
+
+#include <asm/io.h>
+#include <asm/system.h>
+
+static int postconfig(comedi_device * dev);
+static int insn_rw_emulate_bits(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data);
+static void *comedi_recognize(comedi_driver * driv, const char *name);
+static void comedi_report_boards(comedi_driver * driv);
+static int poll_invalid(comedi_device * dev, comedi_subdevice * s);
+int comedi_buf_alloc(comedi_device * dev, comedi_subdevice * s,
+	unsigned long new_size);
+
+comedi_driver *comedi_drivers;
+
+int comedi_modprobe(int minor)
+{
+	return -EINVAL;
+}
+
+static void cleanup_device(comedi_device * dev)
+{
+	int i;
+	comedi_subdevice *s;
+
+	if (dev->subdevices) {
+		for (i = 0; i < dev->n_subdevices; i++) {
+			s = dev->subdevices + i;
+			comedi_free_subdevice_minor(s);
+			if (s->async) {
+				comedi_buf_alloc(dev, s, 0);
+				kfree(s->async);
+			}
+		}
+		kfree(dev->subdevices);
+		dev->subdevices = NULL;
+		dev->n_subdevices = 0;
+	}
+	if (dev->private) {
+		kfree(dev->private);
+		dev->private = NULL;
+	}
+	dev->driver = 0;
+	dev->board_name = NULL;
+	dev->board_ptr = NULL;
+	dev->iobase = 0;
+	dev->irq = 0;
+	dev->read_subdev = NULL;
+	dev->write_subdev = NULL;
+	dev->open = NULL;
+	dev->close = NULL;
+	comedi_set_hw_dev(dev, NULL);
+}
+
+static void __comedi_device_detach(comedi_device * dev)
+{
+	dev->attached = 0;
+	if (dev->driver) {
+		dev->driver->detach(dev);
+	} else {
+		printk("BUG: dev->driver=NULL in comedi_device_detach()\n");
+	}
+	cleanup_device(dev);
+}
+
+void comedi_device_detach(comedi_device * dev)
+{
+	if (!dev->attached)
+		return;
+	__comedi_device_detach(dev);
+}
+
+int comedi_device_attach(comedi_device * dev, comedi_devconfig * it)
+{
+	comedi_driver *driv;
+	int ret;
+
+	if (dev->attached)
+		return -EBUSY;
+
+	for (driv = comedi_drivers; driv; driv = driv->next) {
+		if (!try_module_get(driv->module)) {
+			printk("comedi: failed to increment module count, skipping\n");
+			continue;
+		}
+		if (driv->num_names) {
+			dev->board_ptr = comedi_recognize(driv, it->board_name);
+			if (dev->board_ptr == NULL) {
+				module_put(driv->module);
+				continue;
+			}
+		} else {
+			if (strcmp(driv->driver_name, it->board_name)) {
+				module_put(driv->module);
+				continue;
+			}
+		}
+		//initialize dev->driver here so comedi_error() can be called from attach
+		dev->driver = driv;
+		ret = driv->attach(dev, it);
+		if (ret < 0) {
+			module_put(dev->driver->module);
+			__comedi_device_detach(dev);
+			return ret;
+		}
+		goto attached;
+	}
+
+	// recognize has failed if we get here
+	// report valid board names before returning error
+	for (driv = comedi_drivers; driv; driv = driv->next) {
+		if (!try_module_get(driv->module)) {
+			printk("comedi: failed to increment module count\n");
+			continue;
+		}
+		comedi_report_boards(driv);
+		module_put(driv->module);
+	}
+	return -EIO;
+
+attached:
+	/* do a little post-config cleanup */
+	ret = postconfig(dev);
+	module_put(dev->driver->module);
+	if (ret < 0) {
+		__comedi_device_detach(dev);
+		return ret;
+	}
+
+	if (!dev->board_name) {
+		printk("BUG: dev->board_name=<%p>\n", dev->board_name);
+		dev->board_name = "BUG";
+	}
+	smp_wmb();
+	dev->attached = 1;
+
+	return 0;
+}
+
+int comedi_driver_register(comedi_driver * driver)
+{
+	driver->next = comedi_drivers;
+	comedi_drivers = driver;
+
+	return 0;
+}
+
+int comedi_driver_unregister(comedi_driver * driver)
+{
+	comedi_driver *prev;
+	int i;
+
+	/* check for devices using this driver */
+	for (i = 0; i < COMEDI_NUM_BOARD_MINORS; i++) {
+		struct comedi_device_file_info *dev_file_info = comedi_get_device_file_info(i);
+		comedi_device *dev;
+
+		if(dev_file_info == NULL) continue;
+		dev = dev_file_info->device;
+
+		mutex_lock(&dev->mutex);
+		if (dev->attached && dev->driver == driver) {
+			if (dev->use_count)
+				printk("BUG! detaching device with use_count=%d\n", dev->use_count);
+			comedi_device_detach(dev);
+		}
+		mutex_unlock(&dev->mutex);
+	}
+
+	if (comedi_drivers == driver) {
+		comedi_drivers = driver->next;
+		return 0;
+	}
+
+	for (prev = comedi_drivers; prev->next; prev = prev->next) {
+		if (prev->next == driver) {
+			prev->next = driver->next;
+			return 0;
+		}
+	}
+	return -EINVAL;
+}
+
+static int postconfig(comedi_device * dev)
+{
+	int i;
+	comedi_subdevice *s;
+	comedi_async *async = NULL;
+	int ret;
+
+	for (i = 0; i < dev->n_subdevices; i++) {
+		s = dev->subdevices + i;
+
+		if (s->type == COMEDI_SUBD_UNUSED)
+			continue;
+
+		if (s->len_chanlist == 0)
+			s->len_chanlist = 1;
+
+		if (s->do_cmd) {
+			BUG_ON((s->subdev_flags & (SDF_CMD_READ |
+				SDF_CMD_WRITE)) == 0);
+			BUG_ON(!s->do_cmdtest);
+
+			async = kzalloc(sizeof(comedi_async), GFP_KERNEL);
+			if (async == NULL) {
+				printk("failed to allocate async struct\n");
+				return -ENOMEM;
+			}
+			init_waitqueue_head(&async->wait_head);
+			async->subdevice = s;
+			s->async = async;
+
+#define DEFAULT_BUF_MAXSIZE (64*1024)
+#define DEFAULT_BUF_SIZE (64*1024)
+
+			async->max_bufsize = DEFAULT_BUF_MAXSIZE;
+
+			async->prealloc_buf = NULL;
+			async->prealloc_bufsz = 0;
+			if (comedi_buf_alloc(dev, s, DEFAULT_BUF_SIZE) < 0) {
+				printk("Buffer allocation failed\n");
+				return -ENOMEM;
+			}
+			if (s->buf_change) {
+				ret = s->buf_change(dev, s, DEFAULT_BUF_SIZE);
+				if (ret < 0)
+					return ret;
+			}
+			comedi_alloc_subdevice_minor(dev, s);
+		}
+
+		if (!s->range_table && !s->range_table_list)
+			s->range_table = &range_unknown;
+
+		if (!s->insn_read && s->insn_bits)
+			s->insn_read = insn_rw_emulate_bits;
+		if (!s->insn_write && s->insn_bits)
+			s->insn_write = insn_rw_emulate_bits;
+
+		if (!s->insn_read)
+			s->insn_read = insn_inval;
+		if (!s->insn_write)
+			s->insn_write = insn_inval;
+		if (!s->insn_bits)
+			s->insn_bits = insn_inval;
+		if (!s->insn_config)
+			s->insn_config = insn_inval;
+
+		if (!s->poll)
+			s->poll = poll_invalid;
+	}
+
+	return 0;
+}
+
+// generic recognize function for drivers that register their supported board names
+void *comedi_recognize(comedi_driver * driv, const char *name)
+{
+	unsigned i;
+	const char *const *name_ptr = driv->board_name;
+	for (i = 0; i < driv->num_names; i++) {
+		if (strcmp(*name_ptr, name) == 0)
+			return (void *)name_ptr;
+		name_ptr =
+			(const char *const *)((const char *)name_ptr +
+			driv->offset);
+	}
+
+	return NULL;
+}
+
+void comedi_report_boards(comedi_driver * driv)
+{
+	unsigned int i;
+	const char *const *name_ptr;
+
+	printk("comedi: valid board names for %s driver are:\n",
+		driv->driver_name);
+
+	name_ptr = driv->board_name;
+	for (i = 0; i < driv->num_names; i++) {
+		printk(" %s\n", *name_ptr);
+		name_ptr = (const char **)((char *)name_ptr + driv->offset);
+	}
+
+	if (driv->num_names == 0)
+		printk(" %s\n", driv->driver_name);
+}
+
+static int poll_invalid(comedi_device * dev, comedi_subdevice * s)
+{
+	return -EINVAL;
+}
+
+int insn_inval(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data)
+{
+	return -EINVAL;
+}
+
+static int insn_rw_emulate_bits(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data)
+{
+	comedi_insn new_insn;
+	int ret;
+	static const unsigned channels_per_bitfield = 32;
+
+	unsigned chan = CR_CHAN(insn->chanspec);
+	const unsigned base_bitfield_channel =
+		(chan < channels_per_bitfield) ? 0 : chan;
+	lsampl_t new_data[2];
+	memset(new_data, 0, sizeof(new_data));
+	memset(&new_insn, 0, sizeof(new_insn));
+	new_insn.insn = INSN_BITS;
+	new_insn.chanspec = base_bitfield_channel;
+	new_insn.n = 2;
+	new_insn.data = new_data;
+	new_insn.subdev = insn->subdev;
+
+	if (insn->insn == INSN_WRITE) {
+		if (!(s->subdev_flags & SDF_WRITABLE))
+			return -EINVAL;
+		new_data[0] = 1 << (chan - base_bitfield_channel);	/* mask */
+		new_data[1] = data[0] ? (1 << (chan - base_bitfield_channel)) : 0;	/* bits */
+	}
+
+	ret = s->insn_bits(dev, s, &new_insn, new_data);
+	if (ret < 0)
+		return ret;
+
+	if (insn->insn == INSN_READ) {
+		data[0] = (new_data[1] >> (chan - base_bitfield_channel)) & 1;
+	}
+
+	return 1;
+}
+
+static inline unsigned long uvirt_to_kva(pgd_t * pgd, unsigned long adr)
+{
+	unsigned long ret = 0UL;
+	pmd_t *pmd;
+	pte_t *ptep, pte;
+	pud_t *pud;
+
+	if (!pgd_none(*pgd)) {
+		pud = pud_offset(pgd, adr);
+		pmd = pmd_offset(pud, adr);
+		if (!pmd_none(*pmd)) {
+			ptep = pte_offset_kernel(pmd, adr);
+			pte = *ptep;
+			if (pte_present(pte)) {
+				ret = (unsigned long)
+					page_address(pte_page(pte));
+				ret |= (adr & (PAGE_SIZE - 1));
+			}
+		}
+	}
+	return ret;
+}
+
+static inline unsigned long kvirt_to_kva(unsigned long adr)
+{
+	unsigned long va, kva;
+
+	va = adr;
+	kva = uvirt_to_kva(pgd_offset_k(va), va);
+
+	return kva;
+}
+
+int comedi_buf_alloc(comedi_device * dev, comedi_subdevice * s,
+	unsigned long new_size)
+{
+	comedi_async *async = s->async;
+
+	/* Round up new_size to multiple of PAGE_SIZE */
+	new_size = (new_size + PAGE_SIZE - 1) & PAGE_MASK;
+
+	/* if no change is required, do nothing */
+	if (async->prealloc_buf && async->prealloc_bufsz == new_size) {
+		return 0;
+	}
+	// deallocate old buffer
+	if (async->prealloc_buf) {
+		vunmap(async->prealloc_buf);
+		async->prealloc_buf = NULL;
+		async->prealloc_bufsz = 0;
+	}
+	if (async->buf_page_list) {
+		unsigned i;
+		for (i = 0; i < async->n_buf_pages; ++i) {
+			if (async->buf_page_list[i].virt_addr) {
+				mem_map_unreserve(virt_to_page(async->
+						buf_page_list[i].virt_addr));
+				if (s->async_dma_dir != DMA_NONE) {
+					dma_free_coherent(dev->hw_dev,
+						PAGE_SIZE,
+						async->buf_page_list[i].
+						virt_addr,
+						async->buf_page_list[i].
+						dma_addr);
+				} else {
+					free_page((unsigned long)async->
+						buf_page_list[i].virt_addr);
+				}
+			}
+		}
+		vfree(async->buf_page_list);
+		async->buf_page_list = NULL;
+		async->n_buf_pages = 0;
+	}
+	// allocate new buffer
+	if (new_size) {
+		unsigned i = 0;
+		unsigned n_pages = new_size >> PAGE_SHIFT;
+		struct page **pages = NULL;
+
+		async->buf_page_list =
+			vmalloc(sizeof(struct comedi_buf_page) * n_pages);
+		if (async->buf_page_list) {
+			memset(async->buf_page_list, 0,
+				sizeof(struct comedi_buf_page) * n_pages);
+			pages = vmalloc(sizeof(struct page *) * n_pages);
+		}
+		if (pages) {
+			for (i = 0; i < n_pages; i++) {
+				if (s->async_dma_dir != DMA_NONE) {
+					async->buf_page_list[i].virt_addr =
+						dma_alloc_coherent(dev->hw_dev,
+						PAGE_SIZE,
+						&async->buf_page_list[i].
+						dma_addr,
+						GFP_KERNEL | __GFP_COMP);
+				} else {
+					async->buf_page_list[i].virt_addr =
+						(void *)
+						get_zeroed_page(GFP_KERNEL);
+				}
+				if (async->buf_page_list[i].virt_addr == NULL) {
+					break;
+				}
+				mem_map_reserve(virt_to_page(async->
+						buf_page_list[i].virt_addr));
+				pages[i] =
+					virt_to_page(async->buf_page_list[i].
+					virt_addr);
+			}
+		}
+		if (i == n_pages) {
+			async->prealloc_buf =
+				vmap(pages, n_pages, VM_MAP,
+				PAGE_KERNEL_NOCACHE);
+		}
+		if (pages) {
+			vfree(pages);
+		}
+		if (async->prealloc_buf == NULL) {
+			/* Some allocation failed above. */
+			if (async->buf_page_list) {
+				for (i = 0; i < n_pages; i++) {
+					if (async->buf_page_list[i].virt_addr ==
+						NULL) {
+						break;
+					}
+					mem_map_unreserve(virt_to_page(async->
+							buf_page_list[i].
+							virt_addr));
+					if (s->async_dma_dir != DMA_NONE) {
+						dma_free_coherent(dev->hw_dev,
+							PAGE_SIZE,
+							async->buf_page_list[i].
+							virt_addr,
+							async->buf_page_list[i].
+							dma_addr);
+					} else {
+						free_page((unsigned long)async->
+							buf_page_list[i].
+							virt_addr);
+					}
+				}
+				vfree(async->buf_page_list);
+				async->buf_page_list = NULL;
+			}
+			return -ENOMEM;
+		}
+		async->n_buf_pages = n_pages;
+	}
+	async->prealloc_bufsz = new_size;
+
+	return 0;
+}
+
+/* munging is applied to data by core as it passes between user
+ * and kernel space */
+unsigned int comedi_buf_munge(comedi_async * async, unsigned int num_bytes)
+{
+	comedi_subdevice *s = async->subdevice;
+	unsigned int count = 0;
+	const unsigned num_sample_bytes = bytes_per_sample(s);
+
+	if (s->munge == NULL || (async->cmd.flags & CMDF_RAWDATA)) {
+		async->munge_count += num_bytes;
+		if ((int)(async->munge_count - async->buf_write_count) > 0)
+			BUG();
+		return num_bytes;
+	}
+	/* don't munge partial samples */
+	num_bytes -= num_bytes % num_sample_bytes;
+	while (count < num_bytes) {
+		int block_size;
+
+		block_size = num_bytes - count;
+		if (block_size < 0) {
+			rt_printk("%s: %s: bug! block_size is negative\n",
+				__FILE__, __FUNCTION__);
+			break;
+		}
+		if ((int)(async->munge_ptr + block_size -
+				async->prealloc_bufsz) > 0)
+			block_size = async->prealloc_bufsz - async->munge_ptr;
+
+		s->munge(s->device, s, async->prealloc_buf + async->munge_ptr,
+			block_size, async->munge_chan);
+
+		smp_wmb();	//barrier insures data is munged in buffer before munge_count is incremented
+
+		async->munge_chan += block_size / num_sample_bytes;
+		async->munge_chan %= async->cmd.chanlist_len;
+		async->munge_count += block_size;
+		async->munge_ptr += block_size;
+		async->munge_ptr %= async->prealloc_bufsz;
+		count += block_size;
+	}
+	if ((int)(async->munge_count - async->buf_write_count) > 0)
+		BUG();
+	return count;
+}
+
+unsigned int comedi_buf_write_n_available(comedi_async * async)
+{
+	unsigned int free_end;
+	unsigned int nbytes;
+
+	if (async == NULL)
+		return 0;
+
+	free_end = async->buf_read_count + async->prealloc_bufsz;
+	nbytes = free_end - async->buf_write_alloc_count;
+	nbytes -= nbytes % bytes_per_sample(async->subdevice);
+	/* barrier insures the read of buf_read_count in this
+	   query occurs before any following writes to the buffer which
+	   might be based on the return value from this query.
+	 */
+	smp_mb();
+	return nbytes;
+}
+
+/* allocates chunk for the writer from free buffer space */
+unsigned int comedi_buf_write_alloc(comedi_async * async, unsigned int nbytes)
+{
+	unsigned int free_end = async->buf_read_count + async->prealloc_bufsz;
+
+	if ((int)(async->buf_write_alloc_count + nbytes - free_end) > 0) {
+		nbytes = free_end - async->buf_write_alloc_count;
+	}
+	async->buf_write_alloc_count += nbytes;
+	/* barrier insures the read of buf_read_count above occurs before
+	   we write data to the write-alloc'ed buffer space */
+	smp_mb();
+	return nbytes;
+}
+
+/* allocates nothing unless it can completely fulfill the request */
+unsigned int comedi_buf_write_alloc_strict(comedi_async * async,
+	unsigned int nbytes)
+{
+	unsigned int free_end = async->buf_read_count + async->prealloc_bufsz;
+
+	if ((int)(async->buf_write_alloc_count + nbytes - free_end) > 0) {
+		nbytes = 0;
+	}
+	async->buf_write_alloc_count += nbytes;
+	/* barrier insures the read of buf_read_count above occurs before
+	   we write data to the write-alloc'ed buffer space */
+	smp_mb();
+	return nbytes;
+}
+
+/* transfers a chunk from writer to filled buffer space */
+unsigned comedi_buf_write_free(comedi_async * async, unsigned int nbytes)
+{
+	if ((int)(async->buf_write_count + nbytes -
+			async->buf_write_alloc_count) > 0) {
+		rt_printk
+			("comedi: attempted to write-free more bytes than have been write-allocated.\n");
+		nbytes = async->buf_write_alloc_count - async->buf_write_count;
+	}
+	async->buf_write_count += nbytes;
+	async->buf_write_ptr += nbytes;
+	comedi_buf_munge(async, async->buf_write_count - async->munge_count);
+	if (async->buf_write_ptr >= async->prealloc_bufsz) {
+		async->buf_write_ptr %= async->prealloc_bufsz;
+	}
+	return nbytes;
+}
+
+/* allocates a chunk for the reader from filled (and munged) buffer space */
+unsigned comedi_buf_read_alloc(comedi_async * async, unsigned nbytes)
+{
+	if ((int)(async->buf_read_alloc_count + nbytes - async->munge_count) >
+		0) {
+		nbytes = async->munge_count - async->buf_read_alloc_count;
+	}
+	async->buf_read_alloc_count += nbytes;
+	/* barrier insures read of munge_count occurs before we actually read
+	   data out of buffer */
+	smp_rmb();
+	return nbytes;
+}
+
+/* transfers control of a chunk from reader to free buffer space */
+unsigned comedi_buf_read_free(comedi_async * async, unsigned int nbytes)
+{
+	// barrier insures data has been read out of buffer before read count is incremented
+	smp_mb();
+	if ((int)(async->buf_read_count + nbytes -
+			async->buf_read_alloc_count) > 0) {
+		rt_printk
+			("comedi: attempted to read-free more bytes than have been read-allocated.\n");
+		nbytes = async->buf_read_alloc_count - async->buf_read_count;
+	}
+	async->buf_read_count += nbytes;
+	async->buf_read_ptr += nbytes;
+	async->buf_read_ptr %= async->prealloc_bufsz;
+	return nbytes;
+}
+
+void comedi_buf_memcpy_to(comedi_async * async, unsigned int offset,
+	const void *data, unsigned int num_bytes)
+{
+	unsigned int write_ptr = async->buf_write_ptr + offset;
+
+	if (write_ptr >= async->prealloc_bufsz)
+		write_ptr %= async->prealloc_bufsz;
+
+	while (num_bytes) {
+		unsigned int block_size;
+
+		if (write_ptr + num_bytes > async->prealloc_bufsz)
+			block_size = async->prealloc_bufsz - write_ptr;
+		else
+			block_size = num_bytes;
+
+		memcpy(async->prealloc_buf + write_ptr, data, block_size);
+
+		data += block_size;
+		num_bytes -= block_size;
+
+		write_ptr = 0;
+	}
+}
+
+void comedi_buf_memcpy_from(comedi_async * async, unsigned int offset,
+	void *dest, unsigned int nbytes)
+{
+	void *src;
+	unsigned int read_ptr = async->buf_read_ptr + offset;
+
+	if (read_ptr >= async->prealloc_bufsz)
+		read_ptr %= async->prealloc_bufsz;
+
+	while (nbytes) {
+		unsigned int block_size;
+
+		src = async->prealloc_buf + read_ptr;
+
+		if (nbytes >= async->prealloc_bufsz - read_ptr)
+			block_size = async->prealloc_bufsz - read_ptr;
+		else
+			block_size = nbytes;
+
+		memcpy(dest, src, block_size);
+		nbytes -= block_size;
+		dest += block_size;
+		read_ptr = 0;
+	}
+}
+
+unsigned int comedi_buf_read_n_available(comedi_async * async)
+{
+	unsigned num_bytes;
+
+	if (async == NULL)
+		return 0;
+	num_bytes = async->munge_count - async->buf_read_count;
+	/* barrier insures the read of munge_count in this
+	   query occurs before any following reads of the buffer which
+	   might be based on the return value from this query.
+	 */
+	smp_rmb();
+	return num_bytes;
+}
+
+int comedi_buf_get(comedi_async * async, sampl_t * x)
+{
+	unsigned int n = comedi_buf_read_n_available(async);
+
+	if (n < sizeof(sampl_t))
+		return 0;
+	comedi_buf_read_alloc(async, sizeof(sampl_t));
+	*x = *(sampl_t *) (async->prealloc_buf + async->buf_read_ptr);
+	comedi_buf_read_free(async, sizeof(sampl_t));
+	return 1;
+}
+
+int comedi_buf_put(comedi_async * async, sampl_t x)
+{
+	unsigned int n = comedi_buf_write_alloc_strict(async, sizeof(sampl_t));
+
+	if (n < sizeof(sampl_t)) {
+		async->events |= COMEDI_CB_ERROR;
+		return 0;
+	}
+	*(sampl_t *) (async->prealloc_buf + async->buf_write_ptr) = x;
+	comedi_buf_write_free(async, sizeof(sampl_t));
+	return 1;
+}
+
+void comedi_reset_async_buf(comedi_async * async)
+{
+	async->buf_write_alloc_count = 0;
+	async->buf_write_count = 0;
+	async->buf_read_alloc_count = 0;
+	async->buf_read_count = 0;
+
+	async->buf_write_ptr = 0;
+	async->buf_read_ptr = 0;
+
+	async->cur_chan = 0;
+	async->scan_progress = 0;
+	async->munge_chan = 0;
+	async->munge_count = 0;
+	async->munge_ptr = 0;
+
+	async->events = 0;
+}
+
+int comedi_auto_config(struct device *hardware_device, const char *board_name, const int *options, unsigned num_options)
+{
+	comedi_devconfig it;
+	int minor;
+	struct comedi_device_file_info *dev_file_info;
+	int retval;
+
+	minor = comedi_alloc_board_minor(hardware_device);
+	if(minor < 0) return minor;
+	dev_set_drvdata(hardware_device, (void*)(unsigned long)minor);
+
+	dev_file_info = comedi_get_device_file_info(minor);
+
+	memset(&it, 0, sizeof(it));
+	strncpy(it.board_name, board_name, COMEDI_NAMELEN);
+	it.board_name[COMEDI_NAMELEN - 1] = '\0';
+	BUG_ON(num_options > COMEDI_NDEVCONFOPTS);
+	memcpy(it.options, options, num_options * sizeof(int));
+
+	mutex_lock(&dev_file_info->device->mutex);
+	retval = comedi_device_attach(dev_file_info->device, &it);
+	mutex_unlock(&dev_file_info->device->mutex);
+	if(retval < 0)
+	{
+		comedi_free_board_minor(minor);
+	}
+	return retval;
+}
+
+void comedi_auto_unconfig(struct device *hardware_device)
+{
+	unsigned long minor = (unsigned long)dev_get_drvdata(hardware_device);
+
+	BUG_ON(minor >= COMEDI_NUM_BOARD_MINORS);
+
+	comedi_free_board_minor(minor);
+}
+
+int comedi_pci_auto_config(struct pci_dev *pcidev, const char *board_name)
+{
+	int options[2];
+
+	// pci bus
+	options[0] = pcidev->bus->number;
+	// pci slot
+	options[1] = PCI_SLOT(pcidev->devfn);
+
+	return comedi_auto_config(&pcidev->dev, board_name, options, sizeof(options) / sizeof(options[0]));
+}
+
+void comedi_pci_auto_unconfig(struct pci_dev *pcidev)
+{
+	comedi_auto_unconfig(&pcidev->dev);
+}
diff --git a/drivers/staging/comedi/drivers/Makefile b/drivers/staging/comedi/drivers/Makefile
new file mode 100644
index 00000000000..eb7a615e085
--- /dev/null
+++ b/drivers/staging/comedi/drivers/Makefile
@@ -0,0 +1,21 @@
+# Makefile for individual comedi drivers
+#
+
+# Comedi "helper" modules
+obj-$(CONFIG_COMEDI)			+= comedi_fc.o
+obj-$(CONFIG_COMEDI)			+= comedi_bond.o
+obj-$(CONFIG_COMEDI)			+= comedi_test.o
+obj-$(CONFIG_COMEDI)			+= comedi_parport.o
+
+# Comedi PCI drivers
+obj-$(CONFIG_COMEDI_PCI_DRIVERS)	+= mite.o
+obj-$(CONFIG_COMEDI_PCI_DRIVERS)	+= icp_multi.o
+obj-$(CONFIG_COMEDI_PCI_DRIVERS)	+= me_daq.o
+obj-$(CONFIG_COMEDI_PCI_DRIVERS)	+= me4000.o
+obj-$(CONFIG_COMEDI_PCI_DRIVERS)	+= rtd520.o
+obj-$(CONFIG_COMEDI_PCI_DRIVERS)	+= s626.o
+
+# Comedi USB drivers
+obj-$(CONFIG_COMEDI_USB_DRIVERS)	+= usbdux.o
+obj-$(CONFIG_COMEDI_USB_DRIVERS)	+= usbduxfast.o
+obj-$(CONFIG_COMEDI_USB_DRIVERS)	+= dt9812.o
diff --git a/drivers/staging/comedi/drivers/comedi_bond.c b/drivers/staging/comedi/drivers/comedi_bond.c
new file mode 100644
index 00000000000..9e5496f4f1a
--- /dev/null
+++ b/drivers/staging/comedi/drivers/comedi_bond.c
@@ -0,0 +1,535 @@
+/*
+    comedi/drivers/comedi_bond.c
+    A Comedi driver to 'bond' or merge multiple drivers and devices as one.
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 2000 David A. Schleef <ds@schleef.org>
+    Copyright (C) 2005 Calin A. Culianu <calin@ajvar.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+/*
+Driver: comedi_bond
+Description: A driver to 'bond' (merge) multiple subdevices from multiple
+	     devices together as one.
+Devices:
+Author: ds
+Updated: Mon, 10 Oct 00:18:25 -0500
+Status: works
+
+This driver allows you to 'bond' (merge) multiple comedi subdevices
+(coming from possibly difference boards and/or drivers) together.  For
+example, if you had a board with 2 different DIO subdevices, and
+another with 1 DIO subdevice, you could 'bond' them with this driver
+so that they look like one big fat DIO subdevice.  This makes writing
+applications slightly easier as you don't have to worry about managing
+different subdevices in the application -- you just worry about
+indexing one linear array of channel id's.
+
+Right now only DIO subdevices are supported as that's the personal itch
+I am scratching with this driver.  If you want to add support for AI and AO
+subdevs, go right on ahead and do so!
+
+Commands aren't supported -- although it would be cool if they were.
+
+Configuration Options:
+  List of comedi-minors to bond.  All subdevices of the same type
+  within each minor will be concatenated together in the order given here.
+*/
+
+/*
+ * The previous block comment is used to automatically generate
+ * documentation in Comedi and Comedilib.  The fields:
+ *
+ * Driver: the name of the driver
+ * Description: a short phrase describing the driver.  Don't list boards.
+ * Devices: a full list of the boards that attempt to be supported by
+ *   the driver.  Format is "(manufacturer) board name [comedi name]",
+ *   where comedi_name is the name that is used to configure the board.
+ *   See the comment near board_name: in the comedi_driver structure
+ *   below.  If (manufacturer) or [comedi name] is missing, the previous
+ *   value is used.
+ * Author: you
+ * Updated: date when the _documentation_ was last updated.  Use 'date -R'
+ *   to get a value for this.
+ * Status: a one-word description of the status.  Valid values are:
+ *   works - driver works correctly on most boards supported, and
+ *     passes comedi_test.
+ *   unknown - unknown.  Usually put there by ds.
+ *   experimental - may not work in any particular release.  Author
+ *     probably wants assistance testing it.
+ *   bitrotten - driver has not been update in a long time, probably
+ *     doesn't work, and probably is missing support for significant
+ *     Comedi interface features.
+ *   untested - author probably wrote it "blind", and is believed to
+ *     work, but no confirmation.
+ *
+ * These headers should be followed by a blank line, and any comments
+ * you wish to say about the driver.  The comment area is the place
+ * to put any known bugs, limitations, unsupported features, supported
+ * command triggers, whether or not commands are supported on particular
+ * subdevices, etc.
+ *
+ * Somewhere in the comment should be information about configuration
+ * options that are used with comedi_config.
+ */
+
+#include "../comedilib.h"
+#include "../comedidev.h"
+#include <linux/string.h>
+
+/* The maxiumum number of channels per subdevice. */
+#define MAX_CHANS 256
+
+#define MODULE_NAME "comedi_bond"
+#ifdef MODULE_LICENSE
+MODULE_LICENSE("GPL");
+#endif
+#ifndef STR
+#  define STR1(x) #x
+#  define STR(x) STR1(x)
+#endif
+
+static int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "If true, print extra cryptic debugging output useful"
+		 "only to developers.");
+
+#define LOG_MSG(x...) printk(KERN_INFO MODULE_NAME": "x)
+#define DEBUG(x...)							\
+	do {								\
+		if (debug)						\
+			printk(KERN_DEBUG MODULE_NAME": DEBUG: "x);	\
+	} while (0)
+#define WARNING(x...)  printk(KERN_WARNING MODULE_NAME ": WARNING: "x)
+#define ERROR(x...)  printk(KERN_ERR MODULE_NAME ": INTERNAL ERROR: "x)
+MODULE_AUTHOR("Calin A. Culianu");
+MODULE_DESCRIPTION(MODULE_NAME "A driver for COMEDI to bond multiple COMEDI "
+		   "devices together as one.  In the words of John Lennon: "
+		   "'And the world will live as one...'");
+
+/*
+ * Board descriptions for two imaginary boards.  Describing the
+ * boards in this way is optional, and completely driver-dependent.
+ * Some drivers use arrays such as this, other do not.
+ */
+struct BondingBoard {
+	const char *name;
+};
+
+static const struct BondingBoard bondingBoards[] = {
+	{
+		.name =	MODULE_NAME,
+	},
+};
+
+/*
+ * Useful for shorthand access to the particular board structure
+ */
+#define thisboard ((const struct BondingBoard *)dev->board_ptr)
+
+struct BondedDevice {
+	comedi_t *dev;
+	unsigned minor;
+	unsigned subdev;
+	unsigned subdev_type;
+	unsigned nchans;
+	unsigned chanid_offset;	/* The offset into our unified linear
+				   channel-id's of chanid 0 on this
+				   subdevice. */
+};
+
+/* this structure is for data unique to this hardware driver.  If
+   several hardware drivers keep similar information in this structure,
+   feel free to suggest moving the variable to the comedi_device struct.  */
+struct Private {
+# define MAX_BOARD_NAME 256
+	char name[MAX_BOARD_NAME];
+	struct BondedDevice **devs;
+	unsigned ndevs;
+	struct BondedDevice *chanIdDevMap[MAX_CHANS];
+	unsigned nchans;
+};
+
+/*
+ * most drivers define the following macro to make it easy to
+ * access the private structure.
+ */
+#define devpriv ((struct Private *)dev->private)
+
+/*
+ * The comedi_driver structure tells the Comedi core module
+ * which functions to call to configure/deconfigure (attach/detach)
+ * the board, and also about the kernel module that contains
+ * the device code.
+ */
+static int bonding_attach(comedi_device *dev, comedi_devconfig *it);
+static int bonding_detach(comedi_device *dev);
+/** Build Private array of all devices.. */
+static int doDevConfig(comedi_device *dev, comedi_devconfig *it);
+static void doDevUnconfig(comedi_device *dev);
+/* Ugly implementation of realloc that always copies memory around -- I'm lazy,
+ * what can I say?  I like to do wasteful memcopies.. :) */
+static void *Realloc(const void *ptr, size_t len, size_t old_len);
+
+static comedi_driver driver_bonding = {
+      .driver_name =	MODULE_NAME,
+      .module =		THIS_MODULE,
+      .attach =		bonding_attach,
+      .detach =		bonding_detach,
+	/* It is not necessary to implement the following members if you are
+	 * writing a driver for a ISA PnP or PCI card */
+	/* Most drivers will support multiple types of boards by
+	 * having an array of board structures.  These were defined
+	 * in skel_boards[] above.  Note that the element 'name'
+	 * was first in the structure -- Comedi uses this fact to
+	 * extract the name of the board without knowing any details
+	 * about the structure except for its length.
+	 * When a device is attached (by comedi_config), the name
+	 * of the device is given to Comedi, and Comedi tries to
+	 * match it by going through the list of board names.  If
+	 * there is a match, the address of the pointer is put
+	 * into dev->board_ptr and driver->attach() is called.
+	 *
+	 * Note that these are not necessary if you can determine
+	 * the type of board in software.  ISA PnP, PCI, and PCMCIA
+	 * devices are such boards.
+	 */
+      .board_name =	&bondingBoards[0].name,
+      .offset =		sizeof(struct BondingBoard),
+      .num_names =	sizeof(bondingBoards) / sizeof(struct BondingBoard),
+};
+
+static int bonding_dio_insn_bits(comedi_device *dev, comedi_subdevice *s,
+				 comedi_insn *insn, lsampl_t *data);
+static int bonding_dio_insn_config(comedi_device *dev, comedi_subdevice *s,
+				   comedi_insn *insn, lsampl_t *data);
+
+/*
+ * Attach is called by the Comedi core to configure the driver
+ * for a particular board.  If you specified a board_name array
+ * in the driver structure, dev->board_ptr contains that
+ * address.
+ */
+static int bonding_attach(comedi_device *dev, comedi_devconfig *it)
+{
+	comedi_subdevice *s;
+
+	LOG_MSG("comedi%d\n", dev->minor);
+
+	/*
+	 * Allocate the private structure area.  alloc_private() is a
+	 * convenient macro defined in comedidev.h.
+	 */
+	if (alloc_private(dev, sizeof(struct Private)) < 0)
+		return -ENOMEM;
+
+	/*
+	 * Setup our bonding from config params.. sets up our Private struct..
+	 */
+	if (!doDevConfig(dev, it))
+		return -EINVAL;
+
+	/*
+	 * Initialize dev->board_name.  Note that we can use the "thisboard"
+	 * macro now, since we just initialized it in the last line.
+	 */
+	dev->board_name = devpriv->name;
+
+	/*
+	 * Allocate the subdevice structures.  alloc_subdevice() is a
+	 * convenient macro defined in comedidev.h.
+	 */
+	if (alloc_subdevices(dev, 1) < 0)
+		return -ENOMEM;
+
+	s = dev->subdevices + 0;
+	s->type = COMEDI_SUBD_DIO;
+	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+	s->n_chan = devpriv->nchans;
+	s->maxdata = 1;
+	s->range_table = &range_digital;
+	s->insn_bits = bonding_dio_insn_bits;
+	s->insn_config = bonding_dio_insn_config;
+
+	LOG_MSG("attached with %u DIO channels coming from %u different "
+		"subdevices all bonded together.  "
+		"John Lennon would be proud!\n",
+		devpriv->nchans, devpriv->ndevs);
+
+	return 1;
+}
+
+/*
+ * _detach is called to deconfigure a device.  It should deallocate
+ * resources.
+ * This function is also called when _attach() fails, so it should be
+ * careful not to release resources that were not necessarily
+ * allocated by _attach().  dev->private and dev->subdevices are
+ * deallocated automatically by the core.
+ */
+static int bonding_detach(comedi_device *dev)
+{
+	LOG_MSG("comedi%d: remove\n", dev->minor);
+	doDevUnconfig(dev);
+	return 0;
+}
+
+/* DIO devices are slightly special.  Although it is possible to
+ * implement the insn_read/insn_write interface, it is much more
+ * useful to applications if you implement the insn_bits interface.
+ * This allows packed reading/writing of the DIO channels.  The
+ * comedi core can convert between insn_bits and insn_read/write */
+static int bonding_dio_insn_bits(comedi_device *dev, comedi_subdevice *s,
+				 comedi_insn *insn, lsampl_t *data)
+{
+#define LSAMPL_BITS (sizeof(lsampl_t)*8)
+	unsigned nchans = LSAMPL_BITS, num_done = 0, i;
+	if (insn->n != 2)
+		return -EINVAL;
+
+	if (devpriv->nchans < nchans)
+		nchans = devpriv->nchans;
+
+	/* The insn data is a mask in data[0] and the new data
+	 * in data[1], each channel cooresponding to a bit. */
+	for (i = 0; num_done < nchans && i < devpriv->ndevs; ++i) {
+		struct BondedDevice *bdev = devpriv->devs[i];
+		/* Grab the channel mask and data of only the bits corresponding
+		   to this subdevice.. need to shift them to zero position of
+		   course. */
+		/* Bits corresponding to this subdev. */
+		lsampl_t subdevMask = ((1 << bdev->nchans) - 1);
+		lsampl_t writeMask, dataBits;
+
+		/* Argh, we have >= LSAMPL_BITS chans.. take all bits */
+		if (bdev->nchans >= LSAMPL_BITS)
+			subdevMask = (lsampl_t) (-1);
+
+		writeMask = (data[0] >> num_done) & subdevMask;
+		dataBits = (data[1] >> num_done) & subdevMask;
+
+		/* Read/Write the new digital lines */
+		if (comedi_dio_bitfield(bdev->dev, bdev->subdev, writeMask,
+				&dataBits) != 2)
+			return -EINVAL;
+
+		/* Make room for the new bits in data[1], the return value */
+		data[1] &= ~(subdevMask << num_done);
+		/* Put the bits in the return value */
+		data[1] |= (dataBits & subdevMask) << num_done;
+		/* Save the new bits to the saved state.. */
+		s->state = data[1];
+
+		num_done += bdev->nchans;
+	}
+
+	return insn->n;
+}
+
+static int bonding_dio_insn_config(comedi_device *dev, comedi_subdevice *s,
+				   comedi_insn *insn, lsampl_t *data)
+{
+	int chan = CR_CHAN(insn->chanspec), ret, io_bits = s->io_bits;
+	unsigned int io;
+	struct BondedDevice *bdev;
+
+	if (chan < 0 || chan >= devpriv->nchans)
+		return -EINVAL;
+	bdev = devpriv->chanIdDevMap[chan];
+
+	/* The input or output configuration of each digital line is
+	 * configured by a special insn_config instruction.  chanspec
+	 * contains the channel to be changed, and data[0] contains the
+	 * value COMEDI_INPUT or COMEDI_OUTPUT. */
+	switch (data[0]) {
+	case INSN_CONFIG_DIO_OUTPUT:
+		io = COMEDI_OUTPUT;	/* is this really necessary? */
+		io_bits |= 1 << chan;
+		break;
+	case INSN_CONFIG_DIO_INPUT:
+		io = COMEDI_INPUT;	/* is this really necessary? */
+		io_bits &= ~(1 << chan);
+		break;
+	case INSN_CONFIG_DIO_QUERY:
+		data[1] =
+			(io_bits & (1 << chan)) ? COMEDI_OUTPUT : COMEDI_INPUT;
+		return insn->n;
+		break;
+	default:
+		return -EINVAL;
+		break;
+	}
+	/* 'real' channel id for this subdev.. */
+	chan -= bdev->chanid_offset;
+	ret = comedi_dio_config(bdev->dev, bdev->subdev, chan, io);
+	if (ret != 1)
+		return -EINVAL;
+	/* Finally, save the new io_bits values since we didn't get
+	   an error above. */
+	s->io_bits = io_bits;
+	return insn->n;
+}
+
+static void *Realloc(const void *oldmem, size_t newlen, size_t oldlen)
+{
+	void *newmem = kmalloc(newlen, GFP_KERNEL);
+
+	if (newmem && oldmem)
+		memcpy(newmem, oldmem, min(oldlen, newlen));
+	kfree(oldmem);
+	return newmem;
+}
+
+static int doDevConfig(comedi_device *dev, comedi_devconfig *it)
+{
+	int i;
+	comedi_t *devs_opened[COMEDI_NUM_BOARD_MINORS];
+
+	memset(devs_opened, 0, sizeof(devs_opened));
+	devpriv->name[0] = 0;;
+	/* Loop through all comedi devices specified on the command-line,
+	   building our device list */
+	for (i = 0; i < COMEDI_NDEVCONFOPTS && (!i || it->options[i]); ++i) {
+		char file[] = "/dev/comediXXXXXX";
+		int minor = it->options[i];
+		comedi_t *d;
+		int sdev = -1, nchans, tmp;
+		struct BondedDevice *bdev = NULL;
+
+		if (minor < 0 || minor > COMEDI_NUM_BOARD_MINORS) {
+			ERROR("Minor %d is invalid!\n", minor);
+			return 0;
+		}
+		if (minor == dev->minor) {
+			ERROR("Cannot bond this driver to itself!\n");
+			return 0;
+		}
+		if (devs_opened[minor]) {
+			ERROR("Minor %d specified more than once!\n", minor);
+			return 0;
+		}
+
+		snprintf(file, sizeof(file), "/dev/comedi%u", minor);
+		file[sizeof(file) - 1] = 0;
+
+		d = devs_opened[minor] = comedi_open(file);
+
+		if (!d) {
+			ERROR("Minor %u could not be opened\n", minor);
+			return 0;
+		}
+
+		/* Do DIO, as that's all we support now.. */
+		while ((sdev = comedi_find_subdevice_by_type(d, COMEDI_SUBD_DIO,
+					sdev + 1)) > -1) {
+			nchans = comedi_get_n_channels(d, sdev);
+			if (nchans <= 0) {
+				ERROR("comedi_get_n_channels() returned %d "
+				      "on minor %u subdev %d!\n",
+				      nchans, minor, sdev);
+				return 0;
+			}
+			bdev = kmalloc(sizeof(*bdev), GFP_KERNEL);
+			if (!bdev) {
+				ERROR("Out of memory.\n");
+				return 0;
+			}
+			bdev->dev = d;
+			bdev->minor = minor;
+			bdev->subdev = sdev;
+			bdev->subdev_type = COMEDI_SUBD_DIO;
+			bdev->nchans = nchans;
+			bdev->chanid_offset = devpriv->nchans;
+
+			/* map channel id's to BondedDevice * pointer.. */
+			while (nchans--)
+				devpriv->chanIdDevMap[devpriv->nchans++] = bdev;
+
+			/* Now put bdev pointer at end of devpriv->devs array
+			 * list.. */
+
+			/* ergh.. ugly.. we need to realloc :(  */
+			tmp = devpriv->ndevs * sizeof(bdev);
+			devpriv->devs =
+				Realloc(devpriv->devs,
+				++devpriv->ndevs * sizeof(bdev), tmp);
+			if (!devpriv->devs) {
+				ERROR("Could not allocate memory. "
+				      "Out of memory?");
+				return 0;
+			}
+
+			devpriv->devs[devpriv->ndevs - 1] = bdev;
+			{
+	/** Append dev:subdev to devpriv->name */
+				char buf[20];
+				int left =
+					MAX_BOARD_NAME - strlen(devpriv->name) -
+					1;
+				snprintf(buf, sizeof(buf), "%d:%d ", dev->minor,
+					bdev->subdev);
+				buf[sizeof(buf) - 1] = 0;
+				strncat(devpriv->name, buf, left);
+			}
+
+		}
+	}
+
+	if (!devpriv->nchans) {
+		ERROR("No channels found!\n");
+		return 0;
+	}
+
+	return 1;
+}
+
+static void doDevUnconfig(comedi_device *dev)
+{
+	unsigned long devs_closed = 0;
+
+	if (devpriv) {
+		while (devpriv->ndevs-- && devpriv->devs) {
+			struct BondedDevice *bdev;
+
+			bdev = devpriv->devs[devpriv->ndevs];
+			if (!bdev)
+				continue;
+			if (!(devs_closed & (0x1 << bdev->minor))) {
+				comedi_close(bdev->dev);
+				devs_closed |= (0x1 << bdev->minor);
+			}
+			kfree(bdev);
+		}
+		kfree(devpriv->devs);
+		devpriv->devs = NULL;
+		kfree(devpriv);
+		dev->private = NULL;
+	}
+}
+
+static int __init init(void)
+{
+	return comedi_driver_register(&driver_bonding);
+}
+
+static void __exit cleanup(void)
+{
+	comedi_driver_unregister(&driver_bonding);
+}
+
+module_init(init);
+module_exit(cleanup);
diff --git a/drivers/staging/comedi/drivers/comedi_fc.c b/drivers/staging/comedi/drivers/comedi_fc.c
new file mode 100644
index 00000000000..cd74dbe5417
--- /dev/null
+++ b/drivers/staging/comedi/drivers/comedi_fc.c
@@ -0,0 +1,118 @@
+/*
+    comedi/drivers/comedi_fc.c
+
+    This is a place for code driver writers wish to share between
+    two or more drivers.  fc is short
+    for frank-common.
+
+    Author:  Frank Mori Hess <fmhess@users.sourceforge.net>
+    Copyright (C) 2002 Frank Mori Hess
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+************************************************************************/
+
+#include "../comedidev.h"
+
+#include "comedi_fc.h"
+
+static void increment_scan_progress(comedi_subdevice *subd,
+				    unsigned int num_bytes)
+{
+	comedi_async *async = subd->async;
+	unsigned int scan_length = cfc_bytes_per_scan(subd);
+
+	async->scan_progress += num_bytes;
+	if (async->scan_progress >= scan_length) {
+		async->scan_progress %= scan_length;
+		async->events |= COMEDI_CB_EOS;
+	}
+}
+
+/* Writes an array of data points to comedi's buffer */
+unsigned int cfc_write_array_to_buffer(comedi_subdevice *subd, void *data,
+				       unsigned int num_bytes)
+{
+	comedi_async *async = subd->async;
+	unsigned int retval;
+
+	if (num_bytes == 0)
+		return 0;
+
+	retval = comedi_buf_write_alloc(async, num_bytes);
+	if (retval != num_bytes) {
+		rt_printk("comedi: buffer overrun\n");
+		async->events |= COMEDI_CB_OVERFLOW;
+		return 0;
+	}
+
+	comedi_buf_memcpy_to(async, 0, data, num_bytes);
+	comedi_buf_write_free(async, num_bytes);
+	increment_scan_progress(subd, num_bytes);
+	async->events |= COMEDI_CB_BLOCK;
+
+	return num_bytes;
+}
+EXPORT_SYMBOL(cfc_write_array_to_buffer);
+
+unsigned int cfc_read_array_from_buffer(comedi_subdevice *subd, void *data,
+					unsigned int num_bytes)
+{
+	comedi_async *async = subd->async;
+
+	if (num_bytes == 0)
+		return 0;
+
+	num_bytes = comedi_buf_read_alloc(async, num_bytes);
+	comedi_buf_memcpy_from(async, 0, data, num_bytes);
+	comedi_buf_read_free(async, num_bytes);
+	increment_scan_progress(subd, num_bytes);
+	async->events |= COMEDI_CB_BLOCK;
+
+	return num_bytes;
+}
+EXPORT_SYMBOL(cfc_read_array_from_buffer);
+
+unsigned int cfc_handle_events(comedi_device *dev, comedi_subdevice *subd)
+{
+	unsigned int events = subd->async->events;
+
+	if (events == 0)
+		return events;
+
+	if (events & (COMEDI_CB_EOA | COMEDI_CB_ERROR | COMEDI_CB_OVERFLOW))
+		subd->cancel(dev, subd);
+
+	comedi_event(dev, subd);
+
+	return events;
+}
+EXPORT_SYMBOL(cfc_handle_events);
+
+MODULE_AUTHOR("Frank Mori Hess <fmhess@users.sourceforge.net>");
+MODULE_DESCRIPTION("Shared functions for Comedi low-level drivers");
+MODULE_LICENSE("GPL");
+
+static int __init comedi_fc_init_module(void)
+{
+	return 0;
+}
+
+static void __exit comedi_fc_cleanup_module(void)
+{
+}
+
+module_init(comedi_fc_init_module);
+module_exit(comedi_fc_cleanup_module);
diff --git a/drivers/staging/comedi/drivers/comedi_fc.h b/drivers/staging/comedi/drivers/comedi_fc.h
new file mode 100644
index 00000000000..6952fe20f27
--- /dev/null
+++ b/drivers/staging/comedi/drivers/comedi_fc.h
@@ -0,0 +1,76 @@
+/*
+    comedi_fc.h
+
+    This is a place for code driver writers wish to share between
+    two or more drivers. These functions are meant to be used only
+    by drivers, they are NOT part of the kcomedilib API!
+
+    Author:  Frank Mori Hess <fmhess@users.sourceforge.net>
+    Copyright (C) 2002 Frank Mori Hess
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+************************************************************************/
+
+#ifndef _COMEDI_FC_H
+#define _COMEDI_FC_H
+
+#include "../comedidev.h"
+
+/* Writes an array of data points to comedi's buffer */
+extern unsigned int cfc_write_array_to_buffer(comedi_subdevice *subd,
+					      void *data,
+					      unsigned int num_bytes);
+
+static inline unsigned int cfc_write_to_buffer(comedi_subdevice *subd,
+					       sampl_t data)
+{
+	return cfc_write_array_to_buffer(subd, &data, sizeof(data));
+};
+
+static inline unsigned int cfc_write_long_to_buffer(comedi_subdevice *subd,
+						    lsampl_t data)
+{
+	return cfc_write_array_to_buffer(subd, &data, sizeof(data));
+};
+
+extern unsigned int cfc_read_array_from_buffer(comedi_subdevice *subd,
+					       void *data,
+					       unsigned int num_bytes);
+
+extern unsigned int cfc_handle_events(comedi_device *dev,
+				      comedi_subdevice *subd);
+
+static inline unsigned int cfc_bytes_per_scan(comedi_subdevice *subd)
+{
+	int num_samples;
+	int bits_per_sample;
+
+	switch (subd->type) {
+	case COMEDI_SUBD_DI:
+	case COMEDI_SUBD_DO:
+	case COMEDI_SUBD_DIO:
+		bits_per_sample = 8 * bytes_per_sample(subd);
+		num_samples = (subd->async->cmd.chanlist_len +
+			       bits_per_sample - 1) / bits_per_sample;
+		break;
+	default:
+		num_samples = subd->async->cmd.chanlist_len;
+		break;
+	}
+	return num_samples * bytes_per_sample(subd);
+}
+
+#endif /* _COMEDI_FC_H */
diff --git a/drivers/staging/comedi/drivers/comedi_parport.c b/drivers/staging/comedi/drivers/comedi_parport.c
new file mode 100644
index 00000000000..ba838fffa29
--- /dev/null
+++ b/drivers/staging/comedi/drivers/comedi_parport.c
@@ -0,0 +1,390 @@
+/*
+    comedi/drivers/comedi_parport.c
+    hardware driver for standard parallel port
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1998,2001 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+/*
+Driver: comedi_parport
+Description: Standard PC parallel port
+Author: ds
+Status: works in immediate mode
+Devices: [standard] parallel port (comedi_parport)
+Updated: Tue, 30 Apr 2002 21:11:45 -0700
+
+A cheap and easy way to get a few more digital I/O lines.  Steal
+additional parallel ports from old computers or your neighbors'
+computers.
+
+Option list:
+ 0: I/O port base for the parallel port.
+ 1: IRQ
+
+Parallel Port Lines:
+
+pin     subdev  chan    aka
+---     ------  ----    ---
+1       2       0       strobe
+2       0       0       data 0
+3       0       1       data 1
+4       0       2       data 2
+5       0       3       data 3
+6       0       4       data 4
+7       0       5       data 5
+8       0       6       data 6
+9       0       7       data 7
+10      1       3       acknowledge
+11      1       4       busy
+12      1       2       output
+13      1       1       printer selected
+14      2       1       auto LF
+15      1       0       error
+16      2       2       init
+17      2       3       select printer
+18-25   ground
+
+Notes:
+
+Subdevices 0 is digital I/O, subdevice 1 is digital input, and
+subdevice 2 is digital output.  Unlike other Comedi devices,
+subdevice 0 defaults to output.
+
+Pins 13 and 14 are inverted once by Comedi and once by the
+hardware, thus cancelling the effect.
+
+Pin 1 is a strobe, thus acts like one.  There's no way in software
+to change this, at least on a standard parallel port.
+
+Subdevice 3 pretends to be a digital input subdevice, but it always
+returns 0 when read.  However, if you run a command with
+scan_begin_src=TRIG_EXT, it uses pin 10 as a external triggering
+pin, which can be used to wake up tasks.
+*/
+/*
+   see http://www.beyondlogic.org/ for information.
+   or http://www.linux-magazin.de/ausgabe/1999/10/IO/io.html
+ */
+
+#include "../comedidev.h"
+#include <linux/ioport.h>
+
+#define PARPORT_SIZE 3
+
+#define PARPORT_A 0
+#define PARPORT_B 1
+#define PARPORT_C 2
+
+static int parport_attach(comedi_device *dev, comedi_devconfig *it);
+static int parport_detach(comedi_device *dev);
+static comedi_driver driver_parport = {
+      .driver_name =	"comedi_parport",
+      .module =		THIS_MODULE,
+      .attach =		parport_attach,
+      .detach =		parport_detach,
+};
+
+COMEDI_INITCLEANUP(driver_parport);
+
+struct parport_private {
+	unsigned int a_data;
+	unsigned int c_data;
+	int enable_irq;
+};
+#define devpriv ((struct parport_private *)(dev->private))
+
+static int parport_insn_a(comedi_device *dev, comedi_subdevice *s,
+			  comedi_insn *insn, lsampl_t *data)
+{
+	if (data[0]) {
+		devpriv->a_data &= ~data[0];
+		devpriv->a_data |= (data[0] & data[1]);
+
+		outb(devpriv->a_data, dev->iobase + PARPORT_A);
+	}
+
+	data[1] = inb(dev->iobase + PARPORT_A);
+
+	return 2;
+}
+
+static int parport_insn_config_a(comedi_device *dev, comedi_subdevice *s,
+				 comedi_insn *insn, lsampl_t *data)
+{
+	if (data[0]) {
+		s->io_bits = 0xff;
+		devpriv->c_data &= ~(1 << 5);
+	} else {
+		s->io_bits = 0;
+		devpriv->c_data |= (1 << 5);
+	}
+	outb(devpriv->c_data, dev->iobase + PARPORT_C);
+
+	return 1;
+}
+
+static int parport_insn_b(comedi_device *dev, comedi_subdevice *s,
+			  comedi_insn *insn, lsampl_t *data)
+{
+	if (data[0]) {
+		/* should writes be ignored? */
+		/* anyone??? */
+	}
+
+	data[1] = (inb(dev->iobase + PARPORT_B) >> 3);
+
+	return 2;
+}
+
+static int parport_insn_c(comedi_device *dev, comedi_subdevice *s,
+			  comedi_insn *insn, lsampl_t *data)
+{
+	data[0] &= 0x0f;
+	if (data[0]) {
+		devpriv->c_data &= ~data[0];
+		devpriv->c_data |= (data[0] & data[1]);
+
+		outb(devpriv->c_data, dev->iobase + PARPORT_C);
+	}
+
+	data[1] = devpriv->c_data & 0xf;
+
+	return 2;
+}
+
+static int parport_intr_insn(comedi_device *dev, comedi_subdevice *s,
+			     comedi_insn *insn, lsampl_t *data)
+{
+	if (insn->n < 1)
+		return -EINVAL;
+
+	data[1] = 0;
+	return 2;
+}
+
+static int parport_intr_cmdtest(comedi_device *dev, comedi_subdevice *s,
+				comedi_cmd *cmd)
+{
+	int err = 0;
+	int tmp;
+
+	/* step 1 */
+
+	tmp = cmd->start_src;
+	cmd->start_src &= TRIG_NOW;
+	if (!cmd->start_src || tmp != cmd->start_src)
+		err++;
+
+	tmp = cmd->scan_begin_src;
+	cmd->scan_begin_src &= TRIG_EXT;
+	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
+		err++;
+
+	tmp = cmd->convert_src;
+	cmd->convert_src &= TRIG_FOLLOW;
+	if (!cmd->convert_src || tmp != cmd->convert_src)
+		err++;
+
+	tmp = cmd->scan_end_src;
+	cmd->scan_end_src &= TRIG_COUNT;
+	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
+		err++;
+
+	tmp = cmd->stop_src;
+	cmd->stop_src &= TRIG_NONE;
+	if (!cmd->stop_src || tmp != cmd->stop_src)
+		err++;
+
+	if (err)
+		return 1;
+
+	/* step 2: ignored */
+
+	if (err)
+		return 2;
+
+	/* step 3: */
+
+	if (cmd->start_arg != 0) {
+		cmd->start_arg = 0;
+		err++;
+	}
+	if (cmd->scan_begin_arg != 0) {
+		cmd->scan_begin_arg = 0;
+		err++;
+	}
+	if (cmd->convert_arg != 0) {
+		cmd->convert_arg = 0;
+		err++;
+	}
+	if (cmd->scan_end_arg != 1) {
+		cmd->scan_end_arg = 1;
+		err++;
+	}
+	if (cmd->stop_arg != 0) {
+		cmd->stop_arg = 0;
+		err++;
+	}
+
+	if (err)
+		return 3;
+
+	/* step 4: ignored */
+
+	if (err)
+		return 4;
+
+	return 0;
+}
+
+static int parport_intr_cmd(comedi_device *dev, comedi_subdevice *s)
+{
+	devpriv->c_data |= 0x10;
+	outb(devpriv->c_data, dev->iobase + PARPORT_C);
+
+	devpriv->enable_irq = 1;
+
+	return 0;
+}
+
+static int parport_intr_cancel(comedi_device *dev, comedi_subdevice *s)
+{
+	printk(KERN_DEBUG "parport_intr_cancel()\n");
+
+	devpriv->c_data &= ~0x10;
+	outb(devpriv->c_data, dev->iobase + PARPORT_C);
+
+	devpriv->enable_irq = 0;
+
+	return 0;
+}
+
+static irqreturn_t parport_interrupt(int irq, void *d PT_REGS_ARG)
+{
+	comedi_device *dev = d;
+	comedi_subdevice *s = dev->subdevices + 3;
+
+	if (!devpriv->enable_irq) {
+		printk(KERN_ERR "comedi_parport: bogus irq, ignored\n");
+		return IRQ_NONE;
+	}
+
+	comedi_buf_put(s->async, 0);
+	s->async->events |= COMEDI_CB_BLOCK | COMEDI_CB_EOS;
+
+	comedi_event(dev, s);
+	return IRQ_HANDLED;
+}
+
+static int parport_attach(comedi_device *dev, comedi_devconfig *it)
+{
+	int ret;
+	unsigned int irq;
+	unsigned long iobase;
+	comedi_subdevice *s;
+
+	iobase = it->options[0];
+	printk(KERN_INFO "comedi%d: parport: 0x%04lx ", dev->minor, iobase);
+	if (!request_region(iobase, PARPORT_SIZE, "parport (comedi)")) {
+		printk("I/O port conflict\n");
+		return -EIO;
+	}
+	dev->iobase = iobase;
+
+	irq = it->options[1];
+	if (irq) {
+		printk(" irq=%u", irq);
+		ret = comedi_request_irq(irq, parport_interrupt, 0,
+			"comedi_parport", dev);
+		if (ret < 0) {
+			printk(" irq not available\n");
+			return -EINVAL;
+		}
+		dev->irq = irq;
+	}
+	dev->board_name = "parport";
+
+	ret = alloc_subdevices(dev, 4);
+	if (ret < 0)
+		return ret;
+	ret = alloc_private(dev, sizeof(struct parport_private));
+	if (ret < 0)
+		return ret;
+
+	s = dev->subdevices + 0;
+	s->type = COMEDI_SUBD_DIO;
+	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+	s->n_chan = 8;
+	s->maxdata = 1;
+	s->range_table = &range_digital;
+	s->insn_bits = parport_insn_a;
+	s->insn_config = parport_insn_config_a;
+
+	s = dev->subdevices + 1;
+	s->type = COMEDI_SUBD_DI;
+	s->subdev_flags = SDF_READABLE;
+	s->n_chan = 5;
+	s->maxdata = 1;
+	s->range_table = &range_digital;
+	s->insn_bits = parport_insn_b;
+
+	s = dev->subdevices + 2;
+	s->type = COMEDI_SUBD_DO;
+	s->subdev_flags = SDF_WRITABLE;
+	s->n_chan = 4;
+	s->maxdata = 1;
+	s->range_table = &range_digital;
+	s->insn_bits = parport_insn_c;
+
+	s = dev->subdevices + 3;
+	if (irq) {
+		dev->read_subdev = s;
+		s->type = COMEDI_SUBD_DI;
+		s->subdev_flags = SDF_READABLE | SDF_CMD_READ;
+		s->n_chan = 1;
+		s->maxdata = 1;
+		s->range_table = &range_digital;
+		s->insn_bits = parport_intr_insn;
+		s->do_cmdtest = parport_intr_cmdtest;
+		s->do_cmd = parport_intr_cmd;
+		s->cancel = parport_intr_cancel;
+	} else {
+		s->type = COMEDI_SUBD_UNUSED;
+	}
+
+	devpriv->a_data = 0;
+	outb(devpriv->a_data, dev->iobase + PARPORT_A);
+	devpriv->c_data = 0;
+	outb(devpriv->c_data, dev->iobase + PARPORT_C);
+
+	printk("\n");
+	return 1;
+}
+
+static int parport_detach(comedi_device *dev)
+{
+	printk("comedi%d: parport: remove\n", dev->minor);
+
+	if (dev->iobase)
+		release_region(dev->iobase, PARPORT_SIZE);
+
+	if (dev->irq)
+		comedi_free_irq(dev->irq, dev);
+
+	return 0;
+}
diff --git a/drivers/staging/comedi/drivers/comedi_pci.h b/drivers/staging/comedi/drivers/comedi_pci.h
new file mode 100644
index 00000000000..c14a036a053
--- /dev/null
+++ b/drivers/staging/comedi/drivers/comedi_pci.h
@@ -0,0 +1,60 @@
+/*
+    comedi/drivers/comedi_pci.h
+    Various PCI functions for drivers.
+
+    Copyright (C) 2007 MEV Ltd. <http://www.mev.co.uk/>
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 2000 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#ifndef _COMEDI_PCI_H_
+#define _COMEDI_PCI_H_
+
+#include <linux/pci.h>
+
+/*
+ * Enable the PCI device and request the regions.
+ */
+static inline int comedi_pci_enable(struct pci_dev *pdev, const char *res_name)
+{
+	int rc;
+
+	rc = pci_enable_device(pdev);
+	if (rc < 0)
+		return rc;
+
+	rc = pci_request_regions(pdev, res_name);
+	if (rc < 0)
+		pci_disable_device(pdev);
+
+	return rc;
+}
+
+/*
+ * Release the regions and disable the PCI device.
+ *
+ * This must be matched with a previous successful call to comedi_pci_enable().
+ */
+static inline void comedi_pci_disable(struct pci_dev *pdev)
+{
+	pci_release_regions(pdev);
+	pci_disable_device(pdev);
+}
+
+#endif
diff --git a/drivers/staging/comedi/drivers/comedi_test.c b/drivers/staging/comedi/drivers/comedi_test.c
new file mode 100644
index 00000000000..4b4c37d0748
--- /dev/null
+++ b/drivers/staging/comedi/drivers/comedi_test.c
@@ -0,0 +1,527 @@
+/*
+    comedi/drivers/comedi_test.c
+
+    Generates fake waveform signals that can be read through
+    the command interface.  It does _not_ read from any board;
+    it just generates deterministic waveforms.
+    Useful for various testing purposes.
+
+    Copyright (C) 2002 Joachim Wuttke <Joachim.Wuttke@icn.siemens.de>
+    Copyright (C) 2002 Frank Mori Hess <fmhess@users.sourceforge.net>
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 2000 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+************************************************************************/
+/*
+Driver: comedi_test
+Description: generates fake waveforms
+Author: Joachim Wuttke <Joachim.Wuttke@icn.siemens.de>, Frank Mori Hess
+  <fmhess@users.sourceforge.net>, ds
+Devices:
+Status: works
+Updated: Sat, 16 Mar 2002 17:34:48 -0800
+
+This driver is mainly for testing purposes, but can also be used to
+generate sample waveforms on systems that don't have data acquisition
+hardware.
+
+Configuration options:
+  [0] - Amplitude in microvolts for fake waveforms (default 1 volt)
+  [1] - Period in microseconds for fake waveforms (default 0.1 sec)
+
+Generates a sawtooth wave on channel 0, square wave on channel 1, additional
+waveforms could be added to other channels (currently they return flatline
+zero volts).
+
+*/
+
+#include "../comedidev.h"
+
+#include <asm/div64.h>
+
+#include "comedi_fc.h"
+
+/* Board descriptions */
+struct waveform_board {
+	const char *name;
+	int ai_chans;
+	int ai_bits;
+	int have_dio;
+};
+
+#define N_CHANS 8
+
+static const struct waveform_board waveform_boards[] = {
+	{
+		.name =		"comedi_test",
+		.ai_chans =	N_CHANS,
+		.ai_bits =	16,
+		.have_dio =	0,
+	},
+};
+
+#define thisboard ((const struct waveform_board *)dev->board_ptr)
+
+/* Data unique to this driver */
+struct waveform_private {
+	struct timer_list timer;
+	struct timeval last;	/* time at which last timer interrupt occured */
+	unsigned int uvolt_amplitude;	/* waveform amplitude in microvolts */
+	unsigned long usec_period;	/* waveform period in microseconds */
+	unsigned long usec_current; /* current time (modulo waveform period) */
+	unsigned long usec_remainder;	/* usec since last scan; */
+	unsigned long ai_count;	/* number of conversions remaining */
+	unsigned int scan_period;	/* scan period in usec */
+	unsigned int convert_period;	/* conversion period in usec */
+	unsigned timer_running:1;
+	lsampl_t ao_loopbacks[N_CHANS];
+};
+#define devpriv ((struct waveform_private *)dev->private)
+
+static int waveform_attach(comedi_device *dev, comedi_devconfig *it);
+static int waveform_detach(comedi_device *dev);
+static comedi_driver driver_waveform = {
+      .driver_name =	"comedi_test",
+      .module =		THIS_MODULE,
+      .attach =		waveform_attach,
+      .detach =		waveform_detach,
+      .board_name =	&waveform_boards[0].name,
+      .offset =		sizeof(struct waveform_board),
+      .num_names =	sizeof(waveform_boards) / sizeof(struct waveform_board),
+};
+
+COMEDI_INITCLEANUP(driver_waveform);
+
+static int waveform_ai_cmdtest(comedi_device *dev, comedi_subdevice *s,
+			       comedi_cmd *cmd);
+static int waveform_ai_cmd(comedi_device *dev, comedi_subdevice *s);
+static int waveform_ai_cancel(comedi_device *dev, comedi_subdevice *s);
+static int waveform_ai_insn_read(comedi_device *dev, comedi_subdevice *s,
+				 comedi_insn *insn, lsampl_t *data);
+static int waveform_ao_insn_write(comedi_device *dev, comedi_subdevice *s,
+				  comedi_insn *insn, lsampl_t *data);
+static sampl_t fake_sawtooth(comedi_device *dev, unsigned int range,
+			     unsigned long current_time);
+static sampl_t fake_squarewave(comedi_device *dev, unsigned int range,
+			       unsigned long current_time);
+static sampl_t fake_flatline(comedi_device *dev, unsigned int range,
+			     unsigned long current_time);
+static sampl_t fake_waveform(comedi_device *dev, unsigned int channel,
+			     unsigned int range, unsigned long current_time);
+
+/* 1000 nanosec in a microsec */
+static const int nano_per_micro = 1000;
+
+/* fake analog input ranges */
+static const comedi_lrange waveform_ai_ranges = {
+	2,
+	{
+			BIP_RANGE(10),
+			BIP_RANGE(5),
+		}
+};
+
+/*
+   This is the background routine used to generate arbitrary data.
+   It should run in the background; therefore it is scheduled by
+   a timer mechanism.
+*/
+static void waveform_ai_interrupt(unsigned long arg)
+{
+	comedi_device *dev = (comedi_device *) arg;
+	comedi_async *async = dev->read_subdev->async;
+	comedi_cmd *cmd = &async->cmd;
+	unsigned int i, j;
+	/* all times in microsec */
+	unsigned long elapsed_time;
+	unsigned int num_scans;
+	struct timeval now;
+
+	do_gettimeofday(&now);
+
+	elapsed_time =
+		1000000 * (now.tv_sec - devpriv->last.tv_sec) + now.tv_usec -
+		devpriv->last.tv_usec;
+	devpriv->last = now;
+	num_scans =
+		(devpriv->usec_remainder + elapsed_time) / devpriv->scan_period;
+	devpriv->usec_remainder =
+		(devpriv->usec_remainder + elapsed_time) % devpriv->scan_period;
+	async->events = 0;
+
+	for (i = 0; i < num_scans; i++) {
+		for (j = 0; j < cmd->chanlist_len; j++) {
+			cfc_write_to_buffer(dev->read_subdev,
+				fake_waveform(dev, CR_CHAN(cmd->chanlist[j]),
+					CR_RANGE(cmd->chanlist[j]),
+					devpriv->usec_current +
+					i * devpriv->scan_period +
+					j * devpriv->convert_period));
+		}
+		devpriv->ai_count++;
+		if (cmd->stop_src == TRIG_COUNT
+			&& devpriv->ai_count >= cmd->stop_arg) {
+			async->events |= COMEDI_CB_EOA;
+			break;
+		}
+	}
+
+	devpriv->usec_current += elapsed_time;
+	devpriv->usec_current %= devpriv->usec_period;
+
+	if ((async->events & COMEDI_CB_EOA) == 0 && devpriv->timer_running)
+		mod_timer(&devpriv->timer, jiffies + 1);
+	else
+		del_timer(&devpriv->timer);
+
+	comedi_event(dev, dev->read_subdev);
+}
+
+static int waveform_attach(comedi_device *dev, comedi_devconfig *it)
+{
+	comedi_subdevice *s;
+	int amplitude = it->options[0];
+	int period = it->options[1];
+	int i;
+
+	dev->board_name = thisboard->name;
+
+	if (alloc_private(dev, sizeof(struct waveform_private)) < 0)
+		return -ENOMEM;
+
+	/* set default amplitude and period */
+	if (amplitude <= 0)
+		amplitude = 1000000;	/* 1 volt */
+	if (period <= 0)
+		period = 100000;	/* 0.1 sec */
+
+	devpriv->uvolt_amplitude = amplitude;
+	devpriv->usec_period = period;
+
+	dev->n_subdevices = 2;
+	if (alloc_subdevices(dev, dev->n_subdevices) < 0)
+		return -ENOMEM;
+
+	s = dev->subdevices + 0;
+	dev->read_subdev = s;
+	/* analog input subdevice */
+	s->type = COMEDI_SUBD_AI;
+	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_CMD_READ;
+	s->n_chan = thisboard->ai_chans;
+	s->maxdata = (1 << thisboard->ai_bits) - 1;
+	s->range_table = &waveform_ai_ranges;
+	s->len_chanlist = s->n_chan * 2;
+	s->insn_read = waveform_ai_insn_read;
+	s->do_cmd = waveform_ai_cmd;
+	s->do_cmdtest = waveform_ai_cmdtest;
+	s->cancel = waveform_ai_cancel;
+
+	s = dev->subdevices + 1;
+	dev->write_subdev = s;
+	/* analog output subdevice (loopback) */
+	s->type = COMEDI_SUBD_AO;
+	s->subdev_flags = SDF_WRITEABLE | SDF_GROUND;
+	s->n_chan = thisboard->ai_chans;
+	s->maxdata = (1 << thisboard->ai_bits) - 1;
+	s->range_table = &waveform_ai_ranges;
+	s->len_chanlist = s->n_chan * 2;
+	s->insn_write = waveform_ao_insn_write;
+	s->do_cmd = NULL;
+	s->do_cmdtest = NULL;
+	s->cancel = NULL;
+
+	/* Our default loopback value is just a 0V flatline */
+	for (i = 0; i < s->n_chan; i++)
+		devpriv->ao_loopbacks[i] = s->maxdata / 2;
+
+	init_timer(&(devpriv->timer));
+	devpriv->timer.function = waveform_ai_interrupt;
+	devpriv->timer.data = (unsigned long)dev;
+
+	printk(KERN_INFO "comedi%d: comedi_test: "
+		"%i microvolt, %li microsecond waveform attached\n", dev->minor,
+		devpriv->uvolt_amplitude, devpriv->usec_period);
+	return 1;
+}
+
+static int waveform_detach(comedi_device *dev)
+{
+	printk("comedi%d: comedi_test: remove\n", dev->minor);
+
+	if (dev->private)
+		waveform_ai_cancel(dev, dev->read_subdev);
+
+	return 0;
+}
+
+static int waveform_ai_cmdtest(comedi_device *dev, comedi_subdevice *s,
+			       comedi_cmd *cmd)
+{
+	int err = 0;
+	int tmp;
+
+	/* step 1: make sure trigger sources are trivially valid */
+
+	tmp = cmd->start_src;
+	cmd->start_src &= TRIG_NOW;
+	if (!cmd->start_src || tmp != cmd->start_src)
+		err++;
+
+	tmp = cmd->scan_begin_src;
+	cmd->scan_begin_src &= TRIG_TIMER;
+	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
+		err++;
+
+	tmp = cmd->convert_src;
+	cmd->convert_src &= TRIG_NOW | TRIG_TIMER;
+	if (!cmd->convert_src || tmp != cmd->convert_src)
+		err++;
+
+	tmp = cmd->scan_end_src;
+	cmd->scan_end_src &= TRIG_COUNT;
+	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
+		err++;
+
+	tmp = cmd->stop_src;
+	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
+	if (!cmd->stop_src || tmp != cmd->stop_src)
+		err++;
+
+	if (err)
+		return 1;
+
+	/*
+	 * step 2: make sure trigger sources are unique and mutually compatible
+	 */
+
+	if (cmd->convert_src != TRIG_NOW && cmd->convert_src != TRIG_TIMER)
+		err++;
+	if (cmd->stop_src != TRIG_COUNT && cmd->stop_src != TRIG_NONE)
+		err++;
+
+	if (err)
+		return 2;
+
+	/* step 3: make sure arguments are trivially compatible */
+
+	if (cmd->start_arg != 0) {
+		cmd->start_arg = 0;
+		err++;
+	}
+	if (cmd->convert_src == TRIG_NOW) {
+		if (cmd->convert_arg != 0) {
+			cmd->convert_arg = 0;
+			err++;
+		}
+	}
+	if (cmd->scan_begin_src == TRIG_TIMER) {
+		if (cmd->scan_begin_arg < nano_per_micro) {
+			cmd->scan_begin_arg = nano_per_micro;
+			err++;
+		}
+		if (cmd->convert_src == TRIG_TIMER &&
+			cmd->scan_begin_arg <
+			cmd->convert_arg * cmd->chanlist_len) {
+			cmd->scan_begin_arg =
+				cmd->convert_arg * cmd->chanlist_len;
+			err++;
+		}
+	}
+	/*
+	 * XXX these checks are generic and should go in core if not there
+	 * already
+	 */
+	if (!cmd->chanlist_len) {
+		cmd->chanlist_len = 1;
+		err++;
+	}
+	if (cmd->scan_end_arg != cmd->chanlist_len) {
+		cmd->scan_end_arg = cmd->chanlist_len;
+		err++;
+	}
+
+	if (cmd->stop_src == TRIG_COUNT) {
+		if (!cmd->stop_arg) {
+			cmd->stop_arg = 1;
+			err++;
+		}
+	} else {		/* TRIG_NONE */
+		if (cmd->stop_arg != 0) {
+			cmd->stop_arg = 0;
+			err++;
+		}
+	}
+
+	if (err)
+		return 3;
+
+	/* step 4: fix up any arguments */
+
+	if (cmd->scan_begin_src == TRIG_TIMER) {
+		tmp = cmd->scan_begin_arg;
+		/* round to nearest microsec */
+		cmd->scan_begin_arg =
+			nano_per_micro * ((tmp +
+				(nano_per_micro / 2)) / nano_per_micro);
+		if (tmp != cmd->scan_begin_arg)
+			err++;
+	}
+	if (cmd->convert_src == TRIG_TIMER) {
+		tmp = cmd->convert_arg;
+		/* round to nearest microsec */
+		cmd->convert_arg =
+			nano_per_micro * ((tmp +
+				(nano_per_micro / 2)) / nano_per_micro);
+		if (tmp != cmd->convert_arg)
+			err++;
+	}
+
+	if (err)
+		return 4;
+
+	return 0;
+}
+
+static int waveform_ai_cmd(comedi_device *dev, comedi_subdevice *s)
+{
+	comedi_cmd *cmd = &s->async->cmd;
+
+	if (cmd->flags & TRIG_RT) {
+		comedi_error(dev,
+			"commands at RT priority not supported in this driver");
+		return -1;
+	}
+
+	devpriv->timer_running = 1;
+	devpriv->ai_count = 0;
+	devpriv->scan_period = cmd->scan_begin_arg / nano_per_micro;
+
+	if (cmd->convert_src == TRIG_NOW)
+		devpriv->convert_period = 0;
+	else if (cmd->convert_src == TRIG_TIMER)
+		devpriv->convert_period = cmd->convert_arg / nano_per_micro;
+	else {
+		comedi_error(dev, "bug setting conversion period");
+		return -1;
+	}
+
+	do_gettimeofday(&devpriv->last);
+	devpriv->usec_current = devpriv->last.tv_usec % devpriv->usec_period;
+	devpriv->usec_remainder = 0;
+
+	devpriv->timer.expires = jiffies + 1;
+	add_timer(&devpriv->timer);
+	return 0;
+}
+
+static int waveform_ai_cancel(comedi_device *dev, comedi_subdevice *s)
+{
+	devpriv->timer_running = 0;
+	del_timer(&devpriv->timer);
+	return 0;
+}
+
+static sampl_t fake_sawtooth(comedi_device *dev, unsigned int range_index,
+			     unsigned long current_time)
+{
+	comedi_subdevice *s = dev->read_subdev;
+	unsigned int offset = s->maxdata / 2;
+	u64 value;
+	const comedi_krange *krange = &s->range_table->range[range_index];
+	u64 binary_amplitude;
+
+	binary_amplitude = s->maxdata;
+	binary_amplitude *= devpriv->uvolt_amplitude;
+	do_div(binary_amplitude, krange->max - krange->min);
+
+	current_time %= devpriv->usec_period;
+	value = current_time;
+	value *= binary_amplitude * 2;
+	do_div(value, devpriv->usec_period);
+	value -= binary_amplitude;	/* get rid of sawtooth's dc offset */
+
+	return offset + value;
+}
+static sampl_t fake_squarewave(comedi_device *dev, unsigned int range_index,
+			       unsigned long current_time)
+{
+	comedi_subdevice *s = dev->read_subdev;
+	unsigned int offset = s->maxdata / 2;
+	u64 value;
+	const comedi_krange *krange = &s->range_table->range[range_index];
+	current_time %= devpriv->usec_period;
+
+	value = s->maxdata;
+	value *= devpriv->uvolt_amplitude;
+	do_div(value, krange->max - krange->min);
+
+	if (current_time < devpriv->usec_period / 2)
+		value *= -1;
+
+	return offset + value;
+}
+
+static sampl_t fake_flatline(comedi_device *dev, unsigned int range_index,
+			     unsigned long current_time)
+{
+	return dev->read_subdev->maxdata / 2;
+}
+
+/* generates a different waveform depending on what channel is read */
+static sampl_t fake_waveform(comedi_device *dev, unsigned int channel,
+			     unsigned int range, unsigned long current_time)
+{
+	enum {
+		SAWTOOTH_CHAN,
+		SQUARE_CHAN,
+	};
+	switch (channel) {
+	case SAWTOOTH_CHAN:
+		return fake_sawtooth(dev, range, current_time);
+		break;
+	case SQUARE_CHAN:
+		return fake_squarewave(dev, range, current_time);
+		break;
+	default:
+		break;
+	}
+
+	return fake_flatline(dev, range, current_time);
+}
+
+static int waveform_ai_insn_read(comedi_device *dev, comedi_subdevice *s,
+				 comedi_insn *insn, lsampl_t *data)
+{
+	int i, chan = CR_CHAN(insn->chanspec);
+
+	for (i = 0; i < insn->n; i++)
+		data[i] = devpriv->ao_loopbacks[chan];
+
+	return insn->n;
+}
+
+static int waveform_ao_insn_write(comedi_device *dev, comedi_subdevice *s,
+				  comedi_insn *insn, lsampl_t *data)
+{
+	int i, chan = CR_CHAN(insn->chanspec);
+
+	for (i = 0; i < insn->n; i++)
+		devpriv->ao_loopbacks[chan] = data[i];
+
+	return insn->n;
+}
diff --git a/drivers/staging/comedi/drivers/dt9812.c b/drivers/staging/comedi/drivers/dt9812.c
new file mode 100644
index 00000000000..f2d2173d721
--- /dev/null
+++ b/drivers/staging/comedi/drivers/dt9812.c
@@ -0,0 +1,1162 @@
+/*
+ * comedi/drivers/dt9812.c
+ *   COMEDI driver for DataTranslation DT9812 USB module
+ *
+ * Copyright (C) 2005 Anders Blomdell <anders.blomdell@control.lth.se>
+ *
+ * COMEDI - Linux Control and Measurement Device Interface
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+
+ *  This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ */
+
+/*
+Driver: dt9812
+Description: Data Translation DT9812 USB module
+Author: anders.blomdell@control.lth.se (Anders Blomdell)
+Status: in development
+Devices: [Data Translation] DT9812 (dt9812)
+Updated: Sun Nov 20 20:18:34 EST 2005
+
+This driver works, but bulk transfers not implemented. Might be a starting point
+for someone else. I found out too late that USB has too high latencies (>1 ms)
+for my needs.
+*/
+
+/*
+ * Nota Bene:
+ *   1. All writes to command pipe has to be 32 bytes (ISP1181B SHRTP=0 ?)
+ *   2. The DDK source (as of sep 2005) is in error regarding the
+ *      input MUX bits (example code says P4, but firmware schematics
+ *      says P1).
+ */
+
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/kref.h>
+#include <linux/uaccess.h>
+#include <linux/usb.h>
+
+#include "../comedidev.h"
+
+#define DT9812_DIAGS_BOARD_INFO_ADDR	0xFBFF
+#define DT9812_MAX_WRITE_CMD_PIPE_SIZE	32
+#define DT9812_MAX_READ_CMD_PIPE_SIZE	32
+
+/*
+ * See Silican Laboratories C8051F020/1/2/3 manual
+ */
+#define F020_SFR_P4			0x84
+#define F020_SFR_P1			0x90
+#define F020_SFR_P2			0xa0
+#define F020_SFR_P3			0xb0
+#define F020_SFR_AMX0CF			0xba
+#define F020_SFR_AMX0SL			0xbb
+#define F020_SFR_ADC0CF			0xbc
+#define F020_SFR_ADC0L			0xbe
+#define F020_SFR_ADC0H			0xbf
+#define F020_SFR_DAC0L			0xd2
+#define F020_SFR_DAC0H			0xd3
+#define F020_SFR_DAC0CN			0xd4
+#define F020_SFR_DAC1L			0xd5
+#define F020_SFR_DAC1H			0xd6
+#define F020_SFR_DAC1CN			0xd7
+#define F020_SFR_ADC0CN			0xe8
+
+#define F020_MASK_ADC0CF_AMP0GN0	0x01
+#define F020_MASK_ADC0CF_AMP0GN1	0x02
+#define F020_MASK_ADC0CF_AMP0GN2	0x04
+
+#define F020_MASK_ADC0CN_AD0EN		0x80
+#define F020_MASK_ADC0CN_AD0INT		0x20
+#define F020_MASK_ADC0CN_AD0BUSY	0x10
+
+#define F020_MASK_DACxCN_DACxEN		0x80
+
+enum {
+				/* A/D  D/A  DI  DO  CT */
+	DT9812_DEVID_DT9812_10,	/*  8    2   8   8   1  +/- 10V */
+	DT9812_DEVID_DT9812_2PT5,/* 8    2   8   8   1  0-2.44V */
+#if 0
+	DT9812_DEVID_DT9813,	/*  16   2   4   4   1  +/- 10V */
+	DT9812_DEVID_DT9814	/*  24   2   0   0   1  +/- 10V */
+#endif
+};
+
+enum dt9812_gain {
+	DT9812_GAIN_0PT25 = 1,
+	DT9812_GAIN_0PT5 = 2,
+	DT9812_GAIN_1 = 4,
+	DT9812_GAIN_2 = 8,
+	DT9812_GAIN_4 = 16,
+	DT9812_GAIN_8 = 32,
+	DT9812_GAIN_16 = 64,
+};
+
+enum {
+	DT9812_LEAST_USB_FIRMWARE_CMD_CODE = 0,
+	/* Write Flash memory */
+	DT9812_W_FLASH_DATA = 0,
+	/* Read Flash memory misc config info */
+	DT9812_R_FLASH_DATA = 1,
+
+	/*
+	 * Register read/write commands for processor
+	 */
+
+	/* Read a single byte of USB memory */
+	DT9812_R_SINGLE_BYTE_REG = 2,
+	/* Write a single byte of USB memory */
+	DT9812_W_SINGLE_BYTE_REG = 3,
+	/* Multiple Reads of USB memory */
+	DT9812_R_MULTI_BYTE_REG = 4,
+	/* Multiple Writes of USB memory */
+	DT9812_W_MULTI_BYTE_REG = 5,
+	/* Read, (AND) with mask, OR value, then write (single) */
+	DT9812_RMW_SINGLE_BYTE_REG = 6,
+	/* Read, (AND) with mask, OR value, then write (multiple) */
+	DT9812_RMW_MULTI_BYTE_REG = 7,
+
+	/*
+	 * Register read/write commands for SMBus
+	 */
+
+	/* Read a single byte of SMBus */
+	DT9812_R_SINGLE_BYTE_SMBUS = 8,
+	/* Write a single byte of SMBus */
+	DT9812_W_SINGLE_BYTE_SMBUS = 9,
+	/* Multiple Reads of SMBus */
+	DT9812_R_MULTI_BYTE_SMBUS = 10,
+	/* Multiple Writes of SMBus */
+	DT9812_W_MULTI_BYTE_SMBUS = 11,
+
+	/*
+	 * Register read/write commands for a device
+	 */
+
+	/* Read a single byte of a device */
+	DT9812_R_SINGLE_BYTE_DEV = 12,
+	/* Write a single byte of a device */
+	DT9812_W_SINGLE_BYTE_DEV = 13,
+	/* Multiple Reads of a device */
+	DT9812_R_MULTI_BYTE_DEV = 14,
+	/* Multiple Writes of a device */
+	DT9812_W_MULTI_BYTE_DEV = 15,
+
+	/* Not sure if we'll need this */
+	DT9812_W_DAC_THRESHOLD = 16,
+
+	/* Set interrupt on change mask */
+	DT9812_W_INT_ON_CHANGE_MASK = 17,
+
+	/* Write (or Clear) the CGL for the ADC */
+	DT9812_W_CGL = 18,
+	/* Multiple Reads of USB memory */
+	DT9812_R_MULTI_BYTE_USBMEM = 19,
+	/* Multiple Writes to USB memory */
+	DT9812_W_MULTI_BYTE_USBMEM = 20,
+
+	/* Issue a start command to a given subsystem */
+	DT9812_START_SUBSYSTEM = 21,
+	/* Issue a stop command to a given subsystem */
+	DT9812_STOP_SUBSYSTEM = 22,
+
+	/* calibrate the board using CAL_POT_CMD */
+	DT9812_CALIBRATE_POT = 23,
+	/* set the DAC FIFO size */
+	DT9812_W_DAC_FIFO_SIZE = 24,
+	/* Write or Clear the CGL for the DAC */
+	DT9812_W_CGL_DAC = 25,
+	/* Read a single value from a subsystem */
+	DT9812_R_SINGLE_VALUE_CMD = 26,
+	/* Write a single value to a subsystem */
+	DT9812_W_SINGLE_VALUE_CMD = 27,
+	/* Valid DT9812_USB_FIRMWARE_CMD_CODE's will be less than this number */
+	DT9812_MAX_USB_FIRMWARE_CMD_CODE,
+};
+
+struct dt9812_flash_data {
+	u16 numbytes;
+	u16 address;
+};
+
+#define DT9812_MAX_NUM_MULTI_BYTE_RDS  \
+    ((DT9812_MAX_WRITE_CMD_PIPE_SIZE - 4 - 1) / sizeof(u8))
+
+struct dt9812_read_multi {
+	u8 count;
+	u8 address[DT9812_MAX_NUM_MULTI_BYTE_RDS];
+};
+
+struct dt9812_write_byte {
+	u8 address;
+	u8 value;
+};
+
+#define DT9812_MAX_NUM_MULTI_BYTE_WRTS  \
+    ((DT9812_MAX_WRITE_CMD_PIPE_SIZE - 4 - 1) / \
+      sizeof(struct dt9812_write_byte))
+
+struct dt9812_write_multi {
+	u8 count;
+	struct dt9812_write_byte write[DT9812_MAX_NUM_MULTI_BYTE_WRTS];
+};
+
+struct dt9812_rmw_byte {
+	u8 address;
+	u8 and_mask;
+	u8 or_value;
+};
+
+#define DT9812_MAX_NUM_MULTI_BYTE_RMWS  \
+    ((DT9812_MAX_WRITE_CMD_PIPE_SIZE - 4 - 1) / sizeof(struct dt9812_rmw_byte))
+
+struct dt9812_rmw_multi {
+	u8 count;
+	struct dt9812_rmw_byte rmw[DT9812_MAX_NUM_MULTI_BYTE_RMWS];
+};
+
+struct dt9812_usb_cmd {
+	u32 cmd;
+	union {
+		struct dt9812_flash_data flash_data_info;
+		struct dt9812_read_multi read_multi_info;
+		struct dt9812_write_multi write_multi_info;
+		struct dt9812_rmw_multi rmw_multi_info;
+	} u;
+#if 0
+	WRITE_BYTE_INFO WriteByteInfo;
+	READ_BYTE_INFO ReadByteInfo;
+	WRITE_MULTI_INFO WriteMultiInfo;
+	READ_MULTI_INFO ReadMultiInfo;
+	RMW_BYTE_INFO RMWByteInfo;
+	RMW_MULTI_INFO RMWMultiInfo;
+	DAC_THRESHOLD_INFO DacThresholdInfo;
+	INT_ON_CHANGE_MASK_INFO IntOnChangeMaskInfo;
+	CGL_INFO CglInfo;
+	SUBSYSTEM_INFO SubsystemInfo;
+	CAL_POT_CMD CalPotCmd;
+	WRITE_DEV_BYTE_INFO WriteDevByteInfo;
+	READ_DEV_BYTE_INFO ReadDevByteInfo;
+	WRITE_DEV_MULTI_INFO WriteDevMultiInfo;
+	READ_DEV_MULTI_INFO ReadDevMultiInfo;
+	READ_SINGLE_VALUE_INFO ReadSingleValueInfo;
+	WRITE_SINGLE_VALUE_INFO WriteSingleValueInfo;
+#endif
+};
+
+#define DT9812_NUM_SLOTS	16
+
+static DECLARE_MUTEX(dt9812_mutex);
+
+static struct usb_device_id dt9812_table[] = {
+	{USB_DEVICE(0x0867, 0x9812)},
+	{ }			/* Terminating entry */
+};
+
+MODULE_DEVICE_TABLE(usb, dt9812_table);
+
+struct usb_dt9812 {
+	struct slot_dt9812 *slot;
+	struct usb_device *udev;
+	struct usb_interface *interface;
+	u16 vendor;
+	u16 product;
+	u16 device;
+	u32 serial;
+	struct {
+		__u8 addr;
+		size_t size;
+	} message_pipe, command_write, command_read, write_stream, read_stream;
+	struct kref kref;
+	u16 analog_out_shadow[2];
+	u8 digital_out_shadow;
+};
+
+struct comedi_dt9812 {
+	struct slot_dt9812 *slot;
+	u32 serial;
+};
+
+struct slot_dt9812 {
+	struct semaphore mutex;
+	u32 serial;
+	struct usb_dt9812 *usb;
+	struct comedi_dt9812 *comedi;
+};
+
+static const comedi_lrange dt9812_10_ain_range = { 1, {
+			BIP_RANGE(10),
+	}
+};
+
+static const comedi_lrange dt9812_2pt5_ain_range = { 1, {
+			UNI_RANGE(2.5),
+	}
+};
+
+static const comedi_lrange dt9812_10_aout_range = { 1, {
+			BIP_RANGE(10),
+	}
+};
+
+static const comedi_lrange dt9812_2pt5_aout_range = { 1, {
+			UNI_RANGE(2.5),
+	}
+};
+
+static struct slot_dt9812 dt9812[DT9812_NUM_SLOTS];
+
+/* Useful shorthand access to private data */
+#define devpriv ((struct comedi_dt9812 *)dev->private)
+
+static inline struct usb_dt9812 *to_dt9812_dev(struct kref *d)
+{
+	return container_of(d, struct usb_dt9812, kref);
+}
+
+static void dt9812_delete(struct kref *kref)
+{
+	struct usb_dt9812 *dev = to_dt9812_dev(kref);
+
+	usb_put_dev(dev->udev);
+	kfree(dev);
+}
+
+static int dt9812_read_info(struct usb_dt9812 *dev, int offset, void *buf,
+			    size_t buf_size)
+{
+	struct dt9812_usb_cmd cmd;
+	int count, retval;
+
+	cmd.cmd = cpu_to_le32(DT9812_R_FLASH_DATA);
+	cmd.u.flash_data_info.address =
+		cpu_to_le16(DT9812_DIAGS_BOARD_INFO_ADDR + offset);
+	cmd.u.flash_data_info.numbytes = cpu_to_le16(buf_size);
+
+	/* DT9812 only responds to 32 byte writes!! */
+	count = 32;
+	retval = usb_bulk_msg(dev->udev,
+			      usb_sndbulkpipe(dev->udev,
+					      dev->command_write.addr),
+			      &cmd, 32, &count, HZ * 1);
+	if (retval)
+		return retval;
+	retval = usb_bulk_msg(dev->udev,
+			      usb_rcvbulkpipe(dev->udev,
+					      dev->command_read.addr),
+			      buf, buf_size, &count, HZ * 1);
+	return retval;
+}
+
+static int dt9812_read_multiple_registers(struct usb_dt9812 *dev, int reg_count,
+					  u8 *address, u8 *value)
+{
+	struct dt9812_usb_cmd cmd;
+	int i, count, retval;
+
+	cmd.cmd = cpu_to_le32(DT9812_R_MULTI_BYTE_REG);
+	cmd.u.read_multi_info.count = reg_count;
+	for (i = 0; i < reg_count; i++)
+		cmd.u.read_multi_info.address[i] = address[i];
+
+	/* DT9812 only responds to 32 byte writes!! */
+	count = 32;
+	retval = usb_bulk_msg(dev->udev,
+			      usb_sndbulkpipe(dev->udev,
+					      dev->command_write.addr),
+			      &cmd, 32, &count, HZ * 1);
+	if (retval)
+		return retval;
+	retval = usb_bulk_msg(dev->udev,
+			      usb_rcvbulkpipe(dev->udev,
+					      dev->command_read.addr),
+			      value, reg_count, &count, HZ * 1);
+	return retval;
+}
+
+static int dt9812_write_multiple_registers(struct usb_dt9812 *dev,
+					   int reg_count, u8 *address,
+					   u8 *value)
+{
+	struct dt9812_usb_cmd cmd;
+	int i, count, retval;
+
+	cmd.cmd = cpu_to_le32(DT9812_W_MULTI_BYTE_REG);
+	cmd.u.read_multi_info.count = reg_count;
+	for (i = 0; i < reg_count; i++) {
+		cmd.u.write_multi_info.write[i].address = address[i];
+		cmd.u.write_multi_info.write[i].value = value[i];
+	}
+	/* DT9812 only responds to 32 byte writes!! */
+	retval = usb_bulk_msg(dev->udev,
+			      usb_sndbulkpipe(dev->udev,
+					      dev->command_write.addr),
+			      &cmd, 32, &count, HZ * 1);
+	return retval;
+}
+
+static int dt9812_rmw_multiple_registers(struct usb_dt9812 *dev, int reg_count,
+					 struct dt9812_rmw_byte *rmw)
+{
+	struct dt9812_usb_cmd cmd;
+	int i, count, retval;
+
+	cmd.cmd = cpu_to_le32(DT9812_RMW_MULTI_BYTE_REG);
+	cmd.u.rmw_multi_info.count = reg_count;
+	for (i = 0; i < reg_count; i++)
+		cmd.u.rmw_multi_info.rmw[i] = rmw[i];
+
+	/* DT9812 only responds to 32 byte writes!! */
+	retval = usb_bulk_msg(dev->udev,
+			      usb_sndbulkpipe(dev->udev,
+					      dev->command_write.addr),
+			      &cmd, 32, &count, HZ * 1);
+	return retval;
+}
+
+static int dt9812_digital_in(struct slot_dt9812 *slot, u8 *bits)
+{
+	int result = -ENODEV;
+
+	down(&slot->mutex);
+	if (slot->usb) {
+		u8 reg[2] = { F020_SFR_P3, F020_SFR_P1 };
+		u8 value[2];
+
+		result = dt9812_read_multiple_registers(slot->usb, 2, reg,
+							value);
+		if (result == 0) {
+			/*
+			 * bits 0-6 in F020_SFR_P3 are bits 0-6 in the digital
+			 * input port bit 3 in F020_SFR_P1 is bit 7 in the
+			 * digital input port
+			 */
+			*bits = (value[0] & 0x7f) | ((value[1] & 0x08) << 4);
+			/* printk("%2.2x, %2.2x -> %2.2x\n",
+				  value[0], value[1], *bits); */
+		}
+	}
+	up(&slot->mutex);
+
+	return result;
+}
+
+static int dt9812_digital_out(struct slot_dt9812 *slot, u8 bits)
+{
+	int result = -ENODEV;
+
+	down(&slot->mutex);
+	if (slot->usb) {
+		u8 reg[1];
+		u8 value[1];
+
+		reg[0] = F020_SFR_P2;
+		value[0] = bits;
+		result = dt9812_write_multiple_registers(slot->usb, 1, reg,
+							 value);
+		slot->usb->digital_out_shadow = bits;
+	}
+	up(&slot->mutex);
+	return result;
+}
+
+static int dt9812_digital_out_shadow(struct slot_dt9812 *slot, u8 *bits)
+{
+	int result = -ENODEV;
+
+	down(&slot->mutex);
+	if (slot->usb) {
+		*bits = slot->usb->digital_out_shadow;
+		result = 0;
+	}
+	up(&slot->mutex);
+	return result;
+}
+
+static void dt9812_configure_mux(struct usb_dt9812 *dev,
+				 struct dt9812_rmw_byte *rmw, int channel)
+{
+	if (dev->device == DT9812_DEVID_DT9812_10) {
+		/* In the DT9812/10V MUX is selected by P1.5-7 */
+		rmw->address = F020_SFR_P1;
+		rmw->and_mask = 0xe0;
+		rmw->or_value = channel << 5;
+	} else {
+		/* In the DT9812/2.5V, internal mux is selected by bits 0:2 */
+		rmw->address = F020_SFR_AMX0SL;
+		rmw->and_mask = 0xff;
+		rmw->or_value = channel & 0x07;
+	}
+}
+
+static void dt9812_configure_gain(struct usb_dt9812 *dev,
+				  struct dt9812_rmw_byte *rmw,
+				  enum dt9812_gain gain)
+{
+	if (dev->device == DT9812_DEVID_DT9812_10) {
+		/* In the DT9812/10V, there is an external gain of 0.5 */
+		gain <<= 1;
+	}
+
+	rmw->address = F020_SFR_ADC0CF;
+	rmw->and_mask = F020_MASK_ADC0CF_AMP0GN2 |
+			F020_MASK_ADC0CF_AMP0GN1 |
+			F020_MASK_ADC0CF_AMP0GN0;
+	switch (gain) {
+		/*
+		 * 000 -> Gain =  1
+		 * 001 -> Gain =  2
+		 * 010 -> Gain =  4
+		 * 011 -> Gain =  8
+		 * 10x -> Gain = 16
+		 * 11x -> Gain =  0.5
+		 */
+	case DT9812_GAIN_0PT5:
+		rmw->or_value = F020_MASK_ADC0CF_AMP0GN2 ||
+				F020_MASK_ADC0CF_AMP0GN1;
+		break;
+	case DT9812_GAIN_1:
+		rmw->or_value = 0x00;
+		break;
+	case DT9812_GAIN_2:
+		rmw->or_value = F020_MASK_ADC0CF_AMP0GN0;
+		break;
+	case DT9812_GAIN_4:
+		rmw->or_value = F020_MASK_ADC0CF_AMP0GN1;
+		break;
+	case DT9812_GAIN_8:
+		rmw->or_value = F020_MASK_ADC0CF_AMP0GN1 ||
+				F020_MASK_ADC0CF_AMP0GN0;
+		break;
+	case DT9812_GAIN_16:
+		rmw->or_value = F020_MASK_ADC0CF_AMP0GN2;
+		break;
+	default:
+		err("Illegal gain %d\n", gain);
+
+	}
+}
+
+static int dt9812_analog_in(struct slot_dt9812 *slot, int channel, u16 *value,
+			    enum dt9812_gain gain)
+{
+	struct dt9812_rmw_byte rmw[3];
+	u8 reg[3] = {
+		F020_SFR_ADC0CN,
+		F020_SFR_ADC0H,
+		F020_SFR_ADC0L
+	};
+	u8 val[3];
+	int result = -ENODEV;
+
+	down(&slot->mutex);
+	if (!slot->usb)
+		goto exit;
+
+	/* 1 select the gain */
+	dt9812_configure_gain(slot->usb, &rmw[0], gain);
+
+	/* 2 set the MUX to select the channel */
+	dt9812_configure_mux(slot->usb, &rmw[1], channel);
+
+	/* 3 start conversion */
+	rmw[2].address = F020_SFR_ADC0CN;
+	rmw[2].and_mask = 0xff;
+	rmw[2].or_value = F020_MASK_ADC0CN_AD0EN | F020_MASK_ADC0CN_AD0BUSY;
+
+	result = dt9812_rmw_multiple_registers(slot->usb, 3, rmw);
+	if (result)
+		goto exit;
+
+	/* read the status and ADC */
+	result = dt9812_read_multiple_registers(slot->usb, 3, reg, val);
+	if (result)
+		goto exit;
+	/*
+	 * An ADC conversion takes 16 SAR clocks cycles, i.e. about 9us.
+	 * Therefore, between the instant that AD0BUSY was set via
+	 * dt9812_rmw_multiple_registers and the read of AD0BUSY via
+	 * dt9812_read_multiple_registers, the conversion should be complete
+	 * since these two operations require two USB transactions each taking
+	 * at least a millisecond to complete.  However, lets make sure that
+	 * conversion is finished.
+	 */
+	if ((val[0] & (F020_MASK_ADC0CN_AD0INT | F020_MASK_ADC0CN_AD0BUSY)) ==
+	    F020_MASK_ADC0CN_AD0INT) {
+		switch (slot->usb->device) {
+		case DT9812_DEVID_DT9812_10:
+			/*
+			 * For DT9812-10V the personality module set the
+			 * encoding to 2's complement. Hence, convert it before
+			 * returning it
+			 */
+			*value = ((val[1] << 8) | val[2]) + 0x800;
+			break;
+		case DT9812_DEVID_DT9812_2PT5:
+			*value = (val[1] << 8) | val[2];
+			break;
+		}
+	}
+
+exit:
+	up(&slot->mutex);
+	return result;
+}
+
+static int dt9812_analog_out_shadow(struct slot_dt9812 *slot, int channel,
+				    u16 *value)
+{
+	int result = -ENODEV;
+
+	down(&slot->mutex);
+	if (slot->usb) {
+		*value = slot->usb->analog_out_shadow[channel];
+		result = 0;
+	}
+	up(&slot->mutex);
+
+	return result;
+}
+
+static int dt9812_analog_out(struct slot_dt9812 *slot, int channel, u16 value)
+{
+	int result = -ENODEV;
+
+	down(&slot->mutex);
+	if (slot->usb) {
+		struct dt9812_rmw_byte rmw[3];
+
+		switch (channel) {
+		case 0:
+			/* 1. Set DAC mode */
+			rmw[0].address = F020_SFR_DAC0CN;
+			rmw[0].and_mask = 0xff;
+			rmw[0].or_value = F020_MASK_DACxCN_DACxEN;
+
+			/* 2 load low byte of DAC value first */
+			rmw[1].address = F020_SFR_DAC0L;
+			rmw[1].and_mask = 0xff;
+			rmw[1].or_value = value & 0xff;
+
+			/* 3 load high byte of DAC value next to latch the
+			   12-bit value */
+			rmw[2].address = F020_SFR_DAC0H;
+			rmw[2].and_mask = 0xff;
+			rmw[2].or_value = (value >> 8) & 0xf;
+			break;
+
+		case 1:
+			/* 1. Set DAC mode */
+			rmw[0].address = F020_SFR_DAC1CN;
+			rmw[0].and_mask = 0xff;
+			rmw[0].or_value = F020_MASK_DACxCN_DACxEN;
+
+			/* 2 load low byte of DAC value first */
+			rmw[1].address = F020_SFR_DAC1L;
+			rmw[1].and_mask = 0xff;
+			rmw[1].or_value = value & 0xff;
+
+			/* 3 load high byte of DAC value next to latch the
+			   12-bit value */
+			rmw[2].address = F020_SFR_DAC1H;
+			rmw[2].and_mask = 0xff;
+			rmw[2].or_value = (value >> 8) & 0xf;
+			break;
+		}
+		result = dt9812_rmw_multiple_registers(slot->usb, 3, rmw);
+		slot->usb->analog_out_shadow[channel] = value;
+	}
+	up(&slot->mutex);
+
+	return result;
+}
+
+/*
+ * USB framework functions
+ */
+
+static int dt9812_probe(struct usb_interface *interface,
+			const struct usb_device_id *id)
+{
+	int retval = -ENOMEM;
+	struct usb_dt9812 *dev = NULL;
+	struct usb_host_interface *iface_desc;
+	struct usb_endpoint_descriptor *endpoint;
+	int i;
+	u8 fw;
+
+	/* allocate memory for our device state and initialize it */
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (dev == NULL) {
+		dev_err(&interface->dev, "Out of memory\n");
+		goto error;
+	}
+	kref_init(&dev->kref);
+
+	dev->udev = usb_get_dev(interface_to_usbdev(interface));
+	dev->interface = interface;
+
+	/* Check endpoints */
+	iface_desc = interface->cur_altsetting;
+
+	if (iface_desc->desc.bNumEndpoints != 5) {
+		err("Wrong number of endpints.");
+		retval = -ENODEV;
+		goto error;
+	}
+
+	for (i = 0; i < iface_desc->desc.bNumEndpoints; ++i) {
+		int direction = -1;
+		endpoint = &iface_desc->endpoint[i].desc;
+		switch (i) {
+		case 0:
+			direction = USB_DIR_IN;
+			dev->message_pipe.addr = endpoint->bEndpointAddress;
+			dev->message_pipe.size =
+					le16_to_cpu(endpoint->wMaxPacketSize);
+
+			break;
+		case 1:
+			direction = USB_DIR_OUT;
+			dev->command_write.addr = endpoint->bEndpointAddress;
+			dev->command_write.size =
+					le16_to_cpu(endpoint->wMaxPacketSize);
+			break;
+		case 2:
+			direction = USB_DIR_IN;
+			dev->command_read.addr = endpoint->bEndpointAddress;
+			dev->command_read.size =
+					le16_to_cpu(endpoint->wMaxPacketSize);
+			break;
+		case 3:
+			direction = USB_DIR_OUT;
+			dev->write_stream.addr = endpoint->bEndpointAddress;
+			dev->write_stream.size =
+					le16_to_cpu(endpoint->wMaxPacketSize);
+			break;
+		case 4:
+			direction = USB_DIR_IN;
+			dev->read_stream.addr = endpoint->bEndpointAddress;
+			dev->read_stream.size =
+					le16_to_cpu(endpoint->wMaxPacketSize);
+			break;
+		}
+		if ((endpoint->bEndpointAddress & USB_DIR_IN) != direction) {
+			dev_err(&interface->dev,
+				"Endpoint has wrong direction.\n");
+			retval = -ENODEV;
+			goto error;
+		}
+	}
+	if (dt9812_read_info(dev, 0, &fw, sizeof(fw)) != 0) {
+		/*
+		 * Seems like a configuration reset is necessary if driver is
+		 * reloaded while device is attached
+		 */
+		usb_reset_configuration(dev->udev);
+		for (i = 0; i < 10; i++) {
+			retval = dt9812_read_info(dev, 1, &fw, sizeof(fw));
+			if (retval == 0) {
+				dev_info(&interface->dev,
+					 "usb_reset_configuration succeded "
+					 "after %d iterations\n", i);
+				break;
+			}
+		}
+	}
+
+	if (dt9812_read_info(dev, 1, &dev->vendor, sizeof(dev->vendor)) != 0) {
+		err("Failed to read vendor.");
+		retval = -ENODEV;
+		goto error;
+	}
+	if (dt9812_read_info(dev, 3, &dev->product,
+			     sizeof(dev->product)) != 0) {
+		err("Failed to read product.");
+		retval = -ENODEV;
+		goto error;
+	}
+	if (dt9812_read_info(dev, 5, &dev->device, sizeof(dev->device)) != 0) {
+		err("Failed to read device.");
+		retval = -ENODEV;
+		goto error;
+	}
+	if (dt9812_read_info(dev, 7, &dev->serial, sizeof(dev->serial)) != 0) {
+		err("Failed to read serial.");
+		retval = -ENODEV;
+		goto error;
+	}
+
+	dev->vendor = le16_to_cpu(dev->vendor);
+	dev->product = le16_to_cpu(dev->product);
+	dev->device = le16_to_cpu(dev->device);
+	dev->serial = le32_to_cpu(dev->serial);
+	switch (dev->device) {
+	case DT9812_DEVID_DT9812_10:
+		dev->analog_out_shadow[0] = 0x0800;
+		dev->analog_out_shadow[1] = 0x800;
+		break;
+	case DT9812_DEVID_DT9812_2PT5:
+		dev->analog_out_shadow[0] = 0x0000;
+		dev->analog_out_shadow[1] = 0x0000;
+		break;
+	}
+	dev->digital_out_shadow = 0;
+
+	/* save our data pointer in this interface device */
+	usb_set_intfdata(interface, dev);
+
+	/* let the user know what node this device is now attached to */
+	dev_info(&interface->dev, "USB DT9812 (%4.4x.%4.4x.%4.4x) #0x%8.8x\n",
+		 dev->vendor, dev->product, dev->device, dev->serial);
+
+	down(&dt9812_mutex);
+	{
+		/* Find a slot for the USB device */
+		struct slot_dt9812 *first = NULL;
+		struct slot_dt9812 *best = NULL;
+
+		for (i = 0; i < DT9812_NUM_SLOTS; i++) {
+			if (!first && !dt9812[i].usb && dt9812[i].serial == 0)
+				first = &dt9812[i];
+			if (!best && dt9812[i].serial == dev->serial)
+				best = &dt9812[i];
+		}
+
+		if (!best)
+			best = first;
+
+		if (best) {
+			down(&best->mutex);
+			best->usb = dev;
+			dev->slot = best;
+			up(&best->mutex);
+		}
+	}
+	up(&dt9812_mutex);
+
+	return 0;
+
+error:
+	if (dev)
+		kref_put(&dev->kref, dt9812_delete);
+	return retval;
+}
+
+static void dt9812_disconnect(struct usb_interface *interface)
+{
+	struct usb_dt9812 *dev;
+	int minor = interface->minor;
+
+	down(&dt9812_mutex);
+	dev = usb_get_intfdata(interface);
+	if (dev->slot) {
+		down(&dev->slot->mutex);
+		dev->slot->usb = NULL;
+		up(&dev->slot->mutex);
+		dev->slot = NULL;
+	}
+	usb_set_intfdata(interface, NULL);
+	up(&dt9812_mutex);
+
+	/* queue final destruction */
+	kref_put(&dev->kref, dt9812_delete);
+
+	dev_info(&interface->dev, "USB Dt9812 #%d now disconnected\n", minor);
+}
+
+static struct usb_driver dt9812_usb_driver = {
+	.name = "dt9812",
+	.probe = dt9812_probe,
+	.disconnect = dt9812_disconnect,
+	.id_table = dt9812_table,
+};
+
+/*
+ * Comedi functions
+ */
+
+static void dt9812_comedi_open(comedi_device *dev)
+{
+	down(&devpriv->slot->mutex);
+	if (devpriv->slot->usb) {
+		/* We have an attached device, fill in current range info */
+		comedi_subdevice *s;
+
+		s = &dev->subdevices[0];
+		s->n_chan = 8;
+		s->maxdata = 1;
+
+		s = &dev->subdevices[1];
+		s->n_chan = 8;
+		s->maxdata = 1;
+
+		s = &dev->subdevices[2];
+		s->n_chan = 8;
+		switch (devpriv->slot->usb->device) {
+		case 0:{
+				s->maxdata = 4095;
+				s->range_table = &dt9812_10_ain_range;
+			}
+			break;
+		case 1:{
+				s->maxdata = 4095;
+				s->range_table = &dt9812_2pt5_ain_range;
+			}
+			break;
+		}
+
+		s = &dev->subdevices[3];
+		s->n_chan = 2;
+		switch (devpriv->slot->usb->device) {
+		case 0:{
+				s->maxdata = 4095;
+				s->range_table = &dt9812_10_aout_range;
+			}
+			break;
+		case 1:{
+				s->maxdata = 4095;
+				s->range_table = &dt9812_2pt5_aout_range;
+			}
+			break;
+		}
+	}
+	up(&devpriv->slot->mutex);
+}
+
+static int dt9812_di_rinsn(comedi_device *dev, comedi_subdevice *s,
+			   comedi_insn *insn, lsampl_t *data)
+{
+	int n;
+	u8 bits = 0;
+
+	dt9812_digital_in(devpriv->slot, &bits);
+	for (n = 0; n < insn->n; n++)
+		data[n] = ((1 << insn->chanspec) & bits) != 0;
+	return n;
+}
+
+static int dt9812_do_winsn(comedi_device *dev, comedi_subdevice *s,
+			   comedi_insn *insn, lsampl_t *data)
+{
+	int n;
+	u8 bits = 0;
+
+	dt9812_digital_out_shadow(devpriv->slot, &bits);
+	for (n = 0; n < insn->n; n++) {
+		u8 mask = 1 << insn->chanspec;
+
+		bits &= ~mask;
+		if (data[n])
+			bits |= mask;
+	}
+	dt9812_digital_out(devpriv->slot, bits);
+	return n;
+}
+
+static int dt9812_ai_rinsn(comedi_device *dev, comedi_subdevice *s,
+			   comedi_insn *insn, lsampl_t *data)
+{
+	int n;
+
+	for (n = 0; n < insn->n; n++) {
+		u16 value = 0;
+
+		dt9812_analog_in(devpriv->slot, insn->chanspec, &value,
+				 DT9812_GAIN_1);
+		data[n] = value;
+	}
+	return n;
+}
+
+static int dt9812_ao_rinsn(comedi_device *dev, comedi_subdevice *s,
+			   comedi_insn *insn, lsampl_t *data)
+{
+	int n;
+	u16 value;
+
+	for (n = 0; n < insn->n; n++) {
+		value = 0;
+		dt9812_analog_out_shadow(devpriv->slot, insn->chanspec, &value);
+		data[n] = value;
+	}
+	return n;
+}
+
+static int dt9812_ao_winsn(comedi_device *dev, comedi_subdevice *s,
+			   comedi_insn *insn, lsampl_t *data)
+{
+	int n;
+
+	for (n = 0; n < insn->n; n++)
+		dt9812_analog_out(devpriv->slot, insn->chanspec, data[n]);
+	return n;
+}
+
+static int dt9812_attach(comedi_device *dev, comedi_devconfig *it)
+{
+	int i;
+	comedi_subdevice *s;
+
+	dev->board_name = "dt9812";
+
+	if (alloc_private(dev, sizeof(struct comedi_dt9812)) < 0)
+		return -ENOMEM;
+
+	/*
+	 * Special open routine, since USB unit may be unattached at
+	 * comedi_config time, hence range can not be determined
+	 */
+	dev->open = dt9812_comedi_open;
+
+	devpriv->serial = it->options[0];
+
+	/* Allocate subdevices */
+	if (alloc_subdevices(dev, 4) < 0)
+		return -ENOMEM;
+
+	/* digital input subdevice */
+	s = dev->subdevices + 0;
+	s->type = COMEDI_SUBD_DI;
+	s->subdev_flags = SDF_READABLE;
+	s->n_chan = 0;
+	s->maxdata = 1;
+	s->range_table = &range_digital;
+	s->insn_read = &dt9812_di_rinsn;
+
+	/* digital output subdevice */
+	s = dev->subdevices + 1;
+	s->type = COMEDI_SUBD_DO;
+	s->subdev_flags = SDF_WRITEABLE;
+	s->n_chan = 0;
+	s->maxdata = 1;
+	s->range_table = &range_digital;
+	s->insn_write = &dt9812_do_winsn;
+
+	/* analog input subdevice */
+	s = dev->subdevices + 2;
+	s->type = COMEDI_SUBD_AI;
+	s->subdev_flags = SDF_READABLE | SDF_GROUND;
+	s->n_chan = 0;
+	s->maxdata = 1;
+	s->range_table = NULL;
+	s->insn_read = &dt9812_ai_rinsn;
+
+	/* analog output subdevice */
+	s = dev->subdevices + 3;
+	s->type = COMEDI_SUBD_AO;
+	s->subdev_flags = SDF_WRITEABLE;
+	s->n_chan = 0;
+	s->maxdata = 1;
+	s->range_table = NULL;
+	s->insn_write = &dt9812_ao_winsn;
+	s->insn_read = &dt9812_ao_rinsn;
+
+	printk(KERN_INFO "comedi%d: successfully attached to dt9812.\n",
+	       dev->minor);
+
+	down(&dt9812_mutex);
+	/* Find a slot for the comedi device */
+	{
+		struct slot_dt9812 *first = NULL;
+		struct slot_dt9812 *best = NULL;
+		for (i = 0; i < DT9812_NUM_SLOTS; i++) {
+			if (!first && !dt9812[i].comedi) {
+				/* First free slot from comedi side */
+				first = &dt9812[i];
+			}
+			if (!best &&
+			    dt9812[i].usb &&
+			    dt9812[i].usb->serial == devpriv->serial) {
+				/* We have an attaced device with matching ID */
+				best = &dt9812[i];
+			}
+		}
+		if (!best)
+			best = first;
+		if (best) {
+			down(&best->mutex);
+			best->comedi = devpriv;
+			best->serial = devpriv->serial;
+			devpriv->slot = best;
+			up(&best->mutex);
+		}
+	}
+	up(&dt9812_mutex);
+
+	return 0;
+}
+
+static int dt9812_detach(comedi_device *dev)
+{
+	return 0;
+}
+
+static comedi_driver dt9812_comedi_driver = {
+	.module = THIS_MODULE,
+	.driver_name = "dt9812",
+	.attach = dt9812_attach,
+	.detach = dt9812_detach,
+};
+
+static int __init usb_dt9812_init(void)
+{
+	int result, i;
+
+	/* Initialize all driver slots */
+	for (i = 0; i < DT9812_NUM_SLOTS; i++) {
+		init_MUTEX(&dt9812[i].mutex);
+		dt9812[i].serial = 0;
+		dt9812[i].usb = NULL;
+		dt9812[i].comedi = NULL;
+	}
+	dt9812[12].serial = 0x0;
+
+	/* register with the USB subsystem */
+	result = usb_register(&dt9812_usb_driver);
+	if (result) {
+		printk(KERN_ERR KBUILD_MODNAME
+		       ": usb_register failed. Error number %d\n", result);
+		return result;
+	}
+	/* register with comedi */
+	result = comedi_driver_register(&dt9812_comedi_driver);
+	if (result) {
+		usb_deregister(&dt9812_usb_driver);
+		err("comedi_driver_register failed. Error number %d", result);
+	}
+
+	return result;
+}
+
+static void __exit usb_dt9812_exit(void)
+{
+	/* unregister with comedi */
+	comedi_driver_unregister(&dt9812_comedi_driver);
+
+	/* deregister this driver with the USB subsystem */
+	usb_deregister(&dt9812_usb_driver);
+}
+
+module_init(usb_dt9812_init);
+module_exit(usb_dt9812_exit);
+
+MODULE_AUTHOR("Anders Blomdell <anders.blomdell@control.lth.se>");
+MODULE_DESCRIPTION("Comedi DT9812 driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/icp_multi.c b/drivers/staging/comedi/drivers/icp_multi.c
new file mode 100644
index 00000000000..59144d7cb0b
--- /dev/null
+++ b/drivers/staging/comedi/drivers/icp_multi.c
@@ -0,0 +1,1085 @@
+/*
+    comedi/drivers/icp_multi.c
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1997-2002 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+/*
+Driver: icp_multi
+Description: Inova ICP_MULTI
+Author: Anne Smorthit <anne.smorthit@sfwte.ch>
+Devices: [Inova] ICP_MULTI (icp_multi)
+Status: works
+
+The driver works for analog input and output and digital input and output.
+It does not work with interrupts or with the counters.  Currently no support
+for DMA.
+
+It has 16 single-ended or 8 differential Analogue Input channels with 12-bit
+resolution.  Ranges : 5V, 10V, +/-5V, +/-10V, 0..20mA and 4..20mA.  Input
+ranges can be individually programmed for each channel.  Voltage or current
+measurement is selected by jumper.
+
+There are 4 x 12-bit Analogue Outputs.  Ranges : 5V, 10V, +/-5V, +/-10V
+
+16 x Digital Inputs, 24V
+
+8 x Digital Outputs, 24V, 1A
+
+4 x 16-bit counters
+
+Options:
+ [0] - PCI bus number - if bus number and slot number are 0,
+                        then driver search for first unused card
+ [1] - PCI slot number
+*/
+
+#include "../comedidev.h"
+
+#include <linux/delay.h>
+#include <linux/pci.h>
+
+#include "icp_multi.h"
+
+#define DEVICE_ID	0x8000	/* Device ID */
+
+#define ICP_MULTI_EXTDEBUG
+
+// Hardware types of the cards
+#define TYPE_ICP_MULTI	0
+
+#define IORANGE_ICP_MULTI 	32
+
+#define ICP_MULTI_ADC_CSR	0	/* R/W: ADC command/status register */
+#define ICP_MULTI_AI		2	/* R:   Analogue input data */
+#define ICP_MULTI_DAC_CSR	4	/* R/W: DAC command/status register */
+#define ICP_MULTI_AO		6	/* R/W: Analogue output data */
+#define ICP_MULTI_DI		8	/* R/W: Digital inouts */
+#define ICP_MULTI_DO		0x0A	/* R/W: Digital outputs */
+#define ICP_MULTI_INT_EN	0x0C	/* R/W: Interrupt enable register */
+#define ICP_MULTI_INT_STAT	0x0E	/* R/W: Interrupt status register */
+#define ICP_MULTI_CNTR0		0x10	/* R/W: Counter 0 */
+#define ICP_MULTI_CNTR1		0x12	/* R/W: counter 1 */
+#define ICP_MULTI_CNTR2		0x14	/* R/W: Counter 2 */
+#define ICP_MULTI_CNTR3		0x16	/* R/W: Counter 3 */
+
+#define ICP_MULTI_SIZE		0x20	/* 32 bytes */
+
+// Define bits from ADC command/status register
+#define	ADC_ST		0x0001	/* Start ADC */
+#define	ADC_BSY		0x0001	/* ADC busy */
+#define ADC_BI		0x0010	/* Bipolar input range 1 = bipolar */
+#define ADC_RA		0x0020	/* Input range 0 = 5V, 1 = 10V */
+#define	ADC_DI		0x0040	/* Differential input mode 1 = differential */
+
+// Define bits from DAC command/status register
+#define	DAC_ST		0x0001	/* Start DAC */
+#define DAC_BSY		0x0001	/* DAC busy */
+#define	DAC_BI		0x0010	/* Bipolar input range 1 = bipolar */
+#define	DAC_RA		0x0020	/* Input range 0 = 5V, 1 = 10V */
+
+// Define bits from interrupt enable/status registers
+#define	ADC_READY	0x0001	/* A/d conversion ready interrupt */
+#define	DAC_READY	0x0002	/* D/a conversion ready interrupt */
+#define	DOUT_ERROR	0x0004	/* Digital output error interrupt */
+#define	DIN_STATUS	0x0008	/* Digital input status change interrupt */
+#define	CIE0		0x0010	/* Counter 0 overrun interrupt */
+#define	CIE1		0x0020	/* Counter 1 overrun interrupt */
+#define	CIE2		0x0040	/* Counter 2 overrun interrupt */
+#define	CIE3		0x0080	/* Counter 3 overrun interrupt */
+
+// Useful definitions
+#define	Status_IRQ	0x00ff	// All interrupts
+
+// Define analogue range
+static const comedi_lrange range_analog = { 4, {
+			UNI_RANGE(5),
+			UNI_RANGE(10),
+			BIP_RANGE(5),
+			BIP_RANGE(10)
+	}
+};
+
+static const char range_codes_analog[] = { 0x00, 0x20, 0x10, 0x30 };
+
+/*
+==============================================================================
+	Forward declarations
+==============================================================================
+*/
+static int icp_multi_attach(comedi_device * dev, comedi_devconfig * it);
+static int icp_multi_detach(comedi_device * dev);
+
+/*
+==============================================================================
+	Data & Structure declarations
+==============================================================================
+*/
+static unsigned short pci_list_builded = 0;	/*>0 list of card is known */
+
+typedef struct {
+	const char *name;	// driver name
+	int device_id;
+	int iorange;		// I/O range len
+	char have_irq;		// 1=card support IRQ
+	char cardtype;		// 0=ICP Multi
+	int n_aichan;		// num of A/D chans
+	int n_aichand;		// num of A/D chans in diff mode
+	int n_aochan;		// num of D/A chans
+	int n_dichan;		// num of DI chans
+	int n_dochan;		// num of DO chans
+	int n_ctrs;		// num of counters
+	int ai_maxdata;		// resolution of A/D
+	int ao_maxdata;		// resolution of D/A
+	const comedi_lrange *rangelist_ai;	// rangelist for A/D
+	const char *rangecode;	// range codes for programming
+	const comedi_lrange *rangelist_ao;	// rangelist for D/A
+} boardtype;
+
+static const boardtype boardtypes[] = {
+	{"icp_multi",		// Driver name
+			DEVICE_ID,	// PCI device ID
+			IORANGE_ICP_MULTI,	// I/O range length
+			1,	// 1=Card supports interrupts
+			TYPE_ICP_MULTI,	// Card type = ICP MULTI
+			16,	// Num of A/D channels
+			8,	// Num of A/D channels in diff mode
+			4,	// Num of D/A channels
+			16,	// Num of digital inputs
+			8,	// Num of digital outputs
+			4,	// Num of counters
+			0x0fff,	// Resolution of A/D
+			0x0fff,	// Resolution of D/A
+			&range_analog,	// Rangelist for A/D
+			range_codes_analog,	// Range codes for programming
+		&range_analog},	// Rangelist for D/A
+};
+
+#define n_boardtypes (sizeof(boardtypes)/sizeof(boardtype))
+
+static comedi_driver driver_icp_multi = {
+      driver_name:"icp_multi",
+      module:THIS_MODULE,
+      attach:icp_multi_attach,
+      detach:icp_multi_detach,
+      num_names:n_boardtypes,
+      board_name:&boardtypes[0].name,
+      offset:sizeof(boardtype),
+};
+
+COMEDI_INITCLEANUP(driver_icp_multi);
+
+typedef struct {
+	struct pcilst_struct *card;	// pointer to card
+	char valid;		// card is usable
+	void *io_addr;		// Pointer to mapped io address
+	resource_size_t phys_iobase;	// Physical io address
+	unsigned int AdcCmdStatus;	// ADC Command/Status register
+	unsigned int DacCmdStatus;	// DAC Command/Status register
+	unsigned int IntEnable;	// Interrupt Enable register
+	unsigned int IntStatus;	// Interrupt Status register
+	unsigned int act_chanlist[32];	// list of scaned channel
+	unsigned char act_chanlist_len;	// len of scanlist
+	unsigned char act_chanlist_pos;	// actual position in MUX list
+	unsigned int *ai_chanlist;	// actaul chanlist
+	sampl_t *ai_data;	// data buffer
+	sampl_t ao_data[4];	// data output buffer
+	sampl_t di_data;	// Digital input data
+	unsigned int do_data;	// Remember digital output data
+} icp_multi_private;
+
+#define devpriv ((icp_multi_private *)dev->private)
+#define this_board ((const boardtype *)dev->board_ptr)
+
+/*
+==============================================================================
+	More forward declarations
+==============================================================================
+*/
+
+#if 0
+static int check_channel_list(comedi_device * dev, comedi_subdevice * s,
+	unsigned int *chanlist, unsigned int n_chan);
+#endif
+static void setup_channel_list(comedi_device * dev, comedi_subdevice * s,
+	unsigned int *chanlist, unsigned int n_chan);
+static int icp_multi_reset(comedi_device * dev);
+
+/*
+==============================================================================
+	Functions
+==============================================================================
+*/
+
+/*
+==============================================================================
+
+	Name:	icp_multi_insn_read_ai
+
+	Description:
+		This function reads a single analogue input.
+
+	Parameters:
+		comedi_device *dev	Pointer to current device structure
+		comedi_subdevice *s	Pointer to current subdevice structure
+		comedi_insn *insn	Pointer to current comedi instruction
+		lsampl_t *data		Pointer to analogue input data
+
+	Returns:int			Nmuber of instructions executed
+
+==============================================================================
+*/
+static int icp_multi_insn_read_ai(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data)
+{
+	int n, timeout;
+
+#ifdef ICP_MULTI_EXTDEBUG
+	printk("icp multi EDBG: BGN: icp_multi_insn_read_ai(...)\n");
+#endif
+	// Disable A/D conversion ready interrupt
+	devpriv->IntEnable &= ~ADC_READY;
+	writew(devpriv->IntEnable, devpriv->io_addr + ICP_MULTI_INT_EN);
+
+	// Clear interrupt status
+	devpriv->IntStatus |= ADC_READY;
+	writew(devpriv->IntStatus, devpriv->io_addr + ICP_MULTI_INT_STAT);
+
+	// Set up appropriate channel, mode and range data, for specified channel
+	setup_channel_list(dev, s, &insn->chanspec, 1);
+
+#ifdef ICP_MULTI_EXTDEBUG
+	printk("icp_multi A ST=%4x IO=%p\n",
+		readw(devpriv->io_addr + ICP_MULTI_ADC_CSR),
+		devpriv->io_addr + ICP_MULTI_ADC_CSR);
+#endif
+
+	for (n = 0; n < insn->n; n++) {
+		// Set start ADC bit
+		devpriv->AdcCmdStatus |= ADC_ST;
+		writew(devpriv->AdcCmdStatus,
+			devpriv->io_addr + ICP_MULTI_ADC_CSR);
+		devpriv->AdcCmdStatus &= ~ADC_ST;
+
+#ifdef ICP_MULTI_EXTDEBUG
+		printk("icp multi B n=%d ST=%4x\n", n,
+			readw(devpriv->io_addr + ICP_MULTI_ADC_CSR));
+#endif
+
+		comedi_udelay(1);
+
+#ifdef ICP_MULTI_EXTDEBUG
+		printk("icp multi C n=%d ST=%4x\n", n,
+			readw(devpriv->io_addr + ICP_MULTI_ADC_CSR));
+#endif
+
+		// Wait for conversion to complete, or get fed up waiting
+		timeout = 100;
+		while (timeout--) {
+			if (!(readw(devpriv->io_addr +
+						ICP_MULTI_ADC_CSR) & ADC_BSY))
+				goto conv_finish;
+
+#ifdef ICP_MULTI_EXTDEBUG
+			if (!(timeout % 10))
+				printk("icp multi D n=%d tm=%d ST=%4x\n", n,
+					timeout,
+					readw(devpriv->io_addr +
+						ICP_MULTI_ADC_CSR));
+#endif
+
+			comedi_udelay(1);
+		}
+
+		// If we reach here, a timeout has occurred
+		comedi_error(dev, "A/D insn timeout");
+
+		// Disable interrupt
+		devpriv->IntEnable &= ~ADC_READY;
+		writew(devpriv->IntEnable, devpriv->io_addr + ICP_MULTI_INT_EN);
+
+		// Clear interrupt status
+		devpriv->IntStatus |= ADC_READY;
+		writew(devpriv->IntStatus,
+			devpriv->io_addr + ICP_MULTI_INT_STAT);
+
+		// Clear data received
+		data[n] = 0;
+
+#ifdef ICP_MULTI_EXTDEBUG
+		printk("icp multi EDBG: END: icp_multi_insn_read_ai(...) n=%d\n", n);
+#endif
+		return -ETIME;
+
+	      conv_finish:
+		data[n] =
+			(readw(devpriv->io_addr + ICP_MULTI_AI) >> 4) & 0x0fff;
+	}
+
+	// Disable interrupt
+	devpriv->IntEnable &= ~ADC_READY;
+	writew(devpriv->IntEnable, devpriv->io_addr + ICP_MULTI_INT_EN);
+
+	// Clear interrupt status
+	devpriv->IntStatus |= ADC_READY;
+	writew(devpriv->IntStatus, devpriv->io_addr + ICP_MULTI_INT_STAT);
+
+#ifdef ICP_MULTI_EXTDEBUG
+	printk("icp multi EDBG: END: icp_multi_insn_read_ai(...) n=%d\n", n);
+#endif
+	return n;
+}
+
+/*
+==============================================================================
+
+	Name:	icp_multi_insn_write_ao
+
+	Description:
+		This function writes a single analogue output.
+
+	Parameters:
+		comedi_device *dev	Pointer to current device structure
+		comedi_subdevice *s	Pointer to current subdevice structure
+		comedi_insn *insn	Pointer to current comedi instruction
+		lsampl_t *data		Pointer to analogue output data
+
+	Returns:int			Nmuber of instructions executed
+
+==============================================================================
+*/
+static int icp_multi_insn_write_ao(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data)
+{
+	int n, chan, range, timeout;
+
+#ifdef ICP_MULTI_EXTDEBUG
+	printk("icp multi EDBG: BGN: icp_multi_insn_write_ao(...)\n");
+#endif
+	// Disable D/A conversion ready interrupt
+	devpriv->IntEnable &= ~DAC_READY;
+	writew(devpriv->IntEnable, devpriv->io_addr + ICP_MULTI_INT_EN);
+
+	// Clear interrupt status
+	devpriv->IntStatus |= DAC_READY;
+	writew(devpriv->IntStatus, devpriv->io_addr + ICP_MULTI_INT_STAT);
+
+	// Get channel number and range
+	chan = CR_CHAN(insn->chanspec);
+	range = CR_RANGE(insn->chanspec);
+
+	// Set up range and channel data
+	// Bit 4 = 1 : Bipolar
+	// Bit 5 = 0 : 5V
+	// Bit 5 = 1 : 10V
+	// Bits 8-9 : Channel number
+	devpriv->DacCmdStatus &= 0xfccf;
+	devpriv->DacCmdStatus |= this_board->rangecode[range];
+	devpriv->DacCmdStatus |= (chan << 8);
+
+	writew(devpriv->DacCmdStatus, devpriv->io_addr + ICP_MULTI_DAC_CSR);
+
+	for (n = 0; n < insn->n; n++) {
+		// Wait for analogue output data register to be ready for new data, or get fed up waiting
+		timeout = 100;
+		while (timeout--) {
+			if (!(readw(devpriv->io_addr +
+						ICP_MULTI_DAC_CSR) & DAC_BSY))
+				goto dac_ready;
+
+#ifdef ICP_MULTI_EXTDEBUG
+			if (!(timeout % 10))
+				printk("icp multi A n=%d tm=%d ST=%4x\n", n,
+					timeout,
+					readw(devpriv->io_addr +
+						ICP_MULTI_DAC_CSR));
+#endif
+
+			comedi_udelay(1);
+		}
+
+		// If we reach here, a timeout has occurred
+		comedi_error(dev, "D/A insn timeout");
+
+		// Disable interrupt
+		devpriv->IntEnable &= ~DAC_READY;
+		writew(devpriv->IntEnable, devpriv->io_addr + ICP_MULTI_INT_EN);
+
+		// Clear interrupt status
+		devpriv->IntStatus |= DAC_READY;
+		writew(devpriv->IntStatus,
+			devpriv->io_addr + ICP_MULTI_INT_STAT);
+
+		// Clear data received
+		devpriv->ao_data[chan] = 0;
+
+#ifdef ICP_MULTI_EXTDEBUG
+		printk("icp multi EDBG: END: icp_multi_insn_write_ao(...) n=%d\n", n);
+#endif
+		return -ETIME;
+
+	      dac_ready:
+		// Write data to analogue output data register
+		writew(data[n], devpriv->io_addr + ICP_MULTI_AO);
+
+		// Set DAC_ST bit to write the data to selected channel
+		devpriv->DacCmdStatus |= DAC_ST;
+		writew(devpriv->DacCmdStatus,
+			devpriv->io_addr + ICP_MULTI_DAC_CSR);
+		devpriv->DacCmdStatus &= ~DAC_ST;
+
+		// Save analogue output data
+		devpriv->ao_data[chan] = data[n];
+	}
+
+#ifdef ICP_MULTI_EXTDEBUG
+	printk("icp multi EDBG: END: icp_multi_insn_write_ao(...) n=%d\n", n);
+#endif
+	return n;
+}
+
+/*
+==============================================================================
+
+	Name:	icp_multi_insn_read_ao
+
+	Description:
+		This function reads a single analogue output.
+
+	Parameters:
+		comedi_device *dev	Pointer to current device structure
+		comedi_subdevice *s	Pointer to current subdevice structure
+		comedi_insn *insn	Pointer to current comedi instruction
+		lsampl_t *data		Pointer to analogue output data
+
+	Returns:int			Nmuber of instructions executed
+
+==============================================================================
+*/
+static int icp_multi_insn_read_ao(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data)
+{
+	int n, chan;
+
+	// Get channel number
+	chan = CR_CHAN(insn->chanspec);
+
+	// Read analogue outputs
+	for (n = 0; n < insn->n; n++)
+		data[n] = devpriv->ao_data[chan];
+
+	return n;
+}
+
+/*
+==============================================================================
+
+	Name:	icp_multi_insn_bits_di
+
+	Description:
+		This function reads the digital inputs.
+
+	Parameters:
+		comedi_device *dev	Pointer to current device structure
+		comedi_subdevice *s	Pointer to current subdevice structure
+		comedi_insn *insn	Pointer to current comedi instruction
+		lsampl_t *data		Pointer to analogue output data
+
+	Returns:int			Nmuber of instructions executed
+
+==============================================================================
+*/
+static int icp_multi_insn_bits_di(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data)
+{
+	data[1] = readw(devpriv->io_addr + ICP_MULTI_DI);
+
+	return 2;
+}
+
+/*
+==============================================================================
+
+	Name:	icp_multi_insn_bits_do
+
+	Description:
+		This function writes the appropriate digital outputs.
+
+	Parameters:
+		comedi_device *dev	Pointer to current device structure
+		comedi_subdevice *s	Pointer to current subdevice structure
+		comedi_insn *insn	Pointer to current comedi instruction
+		lsampl_t *data		Pointer to analogue output data
+
+	Returns:int			Nmuber of instructions executed
+
+==============================================================================
+*/
+static int icp_multi_insn_bits_do(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data)
+{
+#ifdef ICP_MULTI_EXTDEBUG
+	printk("icp multi EDBG: BGN: icp_multi_insn_bits_do(...)\n");
+#endif
+
+	if (data[0]) {
+		s->state &= ~data[0];
+		s->state |= (data[0] & data[1]);
+
+		printk("Digital outputs = %4x \n", s->state);
+
+		writew(s->state, devpriv->io_addr + ICP_MULTI_DO);
+	}
+
+	data[1] = readw(devpriv->io_addr + ICP_MULTI_DI);
+
+#ifdef ICP_MULTI_EXTDEBUG
+	printk("icp multi EDBG: END: icp_multi_insn_bits_do(...)\n");
+#endif
+	return 2;
+}
+
+/*
+==============================================================================
+
+	Name:	icp_multi_insn_read_ctr
+
+	Description:
+		This function reads the specified counter.
+
+	Parameters:
+		comedi_device *dev	Pointer to current device structure
+		comedi_subdevice *s	Pointer to current subdevice structure
+		comedi_insn *insn	Pointer to current comedi instruction
+		lsampl_t *data		Pointer to counter data
+
+	Returns:int			Nmuber of instructions executed
+
+==============================================================================
+*/
+static int icp_multi_insn_read_ctr(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data)
+{
+	return 0;
+}
+
+/*
+==============================================================================
+
+	Name:	icp_multi_insn_write_ctr
+
+	Description:
+		This function write to the specified counter.
+
+	Parameters:
+		comedi_device *dev	Pointer to current device structure
+		comedi_subdevice *s	Pointer to current subdevice structure
+		comedi_insn *insn	Pointer to current comedi instruction
+		lsampl_t *data		Pointer to counter data
+
+	Returns:int			Nmuber of instructions executed
+
+==============================================================================
+*/
+static int icp_multi_insn_write_ctr(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data)
+{
+	return 0;
+}
+
+/*
+==============================================================================
+
+	Name:	interrupt_service_icp_multi
+
+	Description:
+		This function is the interrupt service routine for all
+		interrupts generated by the icp multi board.
+
+	Parameters:
+		int irq
+		void *d			Pointer to current device
+
+==============================================================================
+*/
+static irqreturn_t interrupt_service_icp_multi(int irq, void *d PT_REGS_ARG)
+{
+	comedi_device *dev = d;
+	int int_no;
+
+#ifdef ICP_MULTI_EXTDEBUG
+	printk("icp multi EDBG: BGN: interrupt_service_icp_multi(%d,...)\n",
+		irq);
+#endif
+
+	// Is this interrupt from our board?
+	int_no = readw(devpriv->io_addr + ICP_MULTI_INT_STAT) & Status_IRQ;
+	if (!int_no)
+		// No, exit
+		return IRQ_NONE;
+
+#ifdef ICP_MULTI_EXTDEBUG
+	printk("icp multi EDBG: interrupt_service_icp_multi() ST: %4x\n",
+		readw(devpriv->io_addr + ICP_MULTI_INT_STAT));
+#endif
+
+	// Determine which interrupt is active & handle it
+	switch (int_no) {
+	case ADC_READY:
+		break;
+	case DAC_READY:
+		break;
+	case DOUT_ERROR:
+		break;
+	case DIN_STATUS:
+		break;
+	case CIE0:
+		break;
+	case CIE1:
+		break;
+	case CIE2:
+		break;
+	case CIE3:
+		break;
+	default:
+		break;
+
+	}
+
+#ifdef ICP_MULTI_EXTDEBUG
+	printk("icp multi EDBG: END: interrupt_service_icp_multi(...)\n");
+#endif
+	return IRQ_HANDLED;
+}
+
+#if 0
+/*
+==============================================================================
+
+	Name:	check_channel_list
+
+	Description:
+		This function checks if the channel list, provided by user
+		is built correctly
+
+	Parameters:
+		comedi_device *dev	Pointer to current sevice structure
+		comedi_subdevice *s	Pointer to current subdevice structure
+		unsigned int *chanlist	Pointer to packed channel list
+		unsigned int n_chan	Number of channels to scan
+
+	Returns:int 0 = failure
+		    1 = success
+
+==============================================================================
+*/
+static int check_channel_list(comedi_device * dev, comedi_subdevice * s,
+	unsigned int *chanlist, unsigned int n_chan)
+{
+	unsigned int i;
+
+#ifdef ICP_MULTI_EXTDEBUG
+	printk("icp multi EDBG:  check_channel_list(...,%d)\n", n_chan);
+#endif
+	// Check that we at least have one channel to check
+	if (n_chan < 1) {
+		comedi_error(dev, "range/channel list is empty!");
+		return 0;
+	}
+	// Check all channels
+	for (i = 0; i < n_chan; i++) {
+		// Check that channel number is < maximum
+		if (CR_AREF(chanlist[i]) == AREF_DIFF) {
+			if (CR_CHAN(chanlist[i]) > this_board->n_aichand) {
+				comedi_error(dev,
+					"Incorrect differential ai channel number");
+				return 0;
+			}
+		} else {
+			if (CR_CHAN(chanlist[i]) > this_board->n_aichan) {
+				comedi_error(dev,
+					"Incorrect ai channel number");
+				return 0;
+			}
+		}
+	}
+	return 1;
+}
+#endif
+
+/*
+==============================================================================
+
+	Name:	setup_channel_list
+
+	Description:
+		This function sets the appropriate channel selection,
+		differential input mode and range bits in the ADC Command/
+		Status register.
+
+	Parameters:
+		comedi_device *dev	Pointer to current sevice structure
+		comedi_subdevice *s	Pointer to current subdevice structure
+		unsigned int *chanlist	Pointer to packed channel list
+		unsigned int n_chan	Number of channels to scan
+
+	Returns:Void
+
+==============================================================================
+*/
+static void setup_channel_list(comedi_device * dev, comedi_subdevice * s,
+	unsigned int *chanlist, unsigned int n_chan)
+{
+	unsigned int i, range, chanprog;
+	unsigned int diff;
+
+#ifdef ICP_MULTI_EXTDEBUG
+	printk("icp multi EDBG:  setup_channel_list(...,%d)\n", n_chan);
+#endif
+	devpriv->act_chanlist_len = n_chan;
+	devpriv->act_chanlist_pos = 0;
+
+	for (i = 0; i < n_chan; i++) {
+		// Get channel
+		chanprog = CR_CHAN(chanlist[i]);
+
+		// Determine if it is a differential channel (Bit 15  = 1)
+		if (CR_AREF(chanlist[i]) == AREF_DIFF) {
+			diff = 1;
+			chanprog &= 0x0007;
+		} else {
+			diff = 0;
+			chanprog &= 0x000f;
+		}
+
+		// Clear channel, range and input mode bits in A/D command/status register
+		devpriv->AdcCmdStatus &= 0xf00f;
+
+		// Set channel number and differential mode status bit
+		if (diff) {
+			// Set channel number, bits 9-11 & mode, bit 6
+			devpriv->AdcCmdStatus |= (chanprog << 9);
+			devpriv->AdcCmdStatus |= ADC_DI;
+		} else
+			// Set channel number, bits 8-11
+			devpriv->AdcCmdStatus |= (chanprog << 8);
+
+		// Get range for current channel
+		range = this_board->rangecode[CR_RANGE(chanlist[i])];
+		// Set range. bits 4-5
+		devpriv->AdcCmdStatus |= range;
+
+		/* Output channel, range, mode to ICP Multi */
+		writew(devpriv->AdcCmdStatus,
+			devpriv->io_addr + ICP_MULTI_ADC_CSR);
+
+#ifdef ICP_MULTI_EXTDEBUG
+		printk("GS: %2d. [%4x]=%4x %4x\n", i, chanprog, range,
+			devpriv->act_chanlist[i]);
+#endif
+	}
+
+}
+
+/*
+==============================================================================
+
+	Name:	icp_multi_reset
+
+	Description:
+		This function resets the icp multi device to a 'safe' state
+
+	Parameters:
+		comedi_device *dev	Pointer to current sevice structure
+
+	Returns:int	0 = success
+
+==============================================================================
+*/
+static int icp_multi_reset(comedi_device * dev)
+{
+	unsigned int i;
+
+#ifdef ICP_MULTI_EXTDEBUG
+	printk("icp_multi EDBG: BGN: icp_multi_reset(...)\n");
+#endif
+	// Clear INT enables and requests
+	writew(0, devpriv->io_addr + ICP_MULTI_INT_EN);
+	writew(0x00ff, devpriv->io_addr + ICP_MULTI_INT_STAT);
+
+	if (this_board->n_aochan)
+		// Set DACs to 0..5V range and 0V output
+		for (i = 0; i < this_board->n_aochan; i++) {
+			devpriv->DacCmdStatus &= 0xfcce;
+
+			// Set channel number
+			devpriv->DacCmdStatus |= (i << 8);
+
+			// Output 0V
+			writew(0, devpriv->io_addr + ICP_MULTI_AO);
+
+			// Set start conversion bit
+			devpriv->DacCmdStatus |= DAC_ST;
+
+			// Output to command / status register
+			writew(devpriv->DacCmdStatus,
+				devpriv->io_addr + ICP_MULTI_DAC_CSR);
+
+			// Delay to allow DAC time to recover
+			comedi_udelay(1);
+		}
+	// Digital outputs to 0
+	writew(0, devpriv->io_addr + ICP_MULTI_DO);
+
+#ifdef ICP_MULTI_EXTDEBUG
+	printk("icp multi EDBG: END: icp_multi_reset(...)\n");
+#endif
+	return 0;
+}
+
+/*
+==============================================================================
+
+	Name:	icp_multi_attach
+
+	Description:
+		This function sets up all the appropriate data for the current
+		device.
+
+	Parameters:
+		comedi_device *dev	Pointer to current device structure
+		comedi_devconfig *it	Pointer to current device configuration
+
+	Returns:int	0 = success
+
+==============================================================================
+*/
+static int icp_multi_attach(comedi_device * dev, comedi_devconfig * it)
+{
+	comedi_subdevice *s;
+	int ret, subdev, n_subdevices;
+	unsigned int irq;
+	struct pcilst_struct *card = NULL;
+	resource_size_t io_addr[5], iobase;
+	unsigned char pci_bus, pci_slot, pci_func;
+
+	printk("icp_multi EDBG: BGN: icp_multi_attach(...)\n");
+
+	// Alocate private data storage space
+	if ((ret = alloc_private(dev, sizeof(icp_multi_private))) < 0)
+		return ret;
+
+	// Initialise list of PCI cards in system, if not already done so
+	if (pci_list_builded++ == 0) {
+		pci_card_list_init(PCI_VENDOR_ID_ICP,
+#ifdef ICP_MULTI_EXTDEBUG
+			1
+#else
+			0
+#endif
+			);
+	}
+
+	printk("Anne's comedi%d: icp_multi: board=%s", dev->minor,
+		this_board->name);
+
+	if ((card = select_and_alloc_pci_card(PCI_VENDOR_ID_ICP,
+				this_board->device_id, it->options[0],
+				it->options[1])) == NULL)
+		return -EIO;
+
+	devpriv->card = card;
+
+	if ((pci_card_data(card, &pci_bus, &pci_slot, &pci_func, &io_addr[0],
+				&irq)) < 0) {
+		printk(" - Can't get configuration data!\n");
+		return -EIO;
+	}
+
+	iobase = io_addr[2];
+	devpriv->phys_iobase = iobase;
+
+	printk(", b:s:f=%d:%d:%d, io=0x%8llx \n", pci_bus, pci_slot, pci_func,
+		(unsigned long long)iobase);
+
+	devpriv->io_addr = ioremap(iobase, ICP_MULTI_SIZE);
+
+	if (devpriv->io_addr == NULL) {
+		printk("ioremap failed.\n");
+		return -ENOMEM;
+	}
+#ifdef ICP_MULTI_EXTDEBUG
+	printk("0x%08llx mapped to %p, ", (unsigned long long)iobase,
+		devpriv->io_addr);
+#endif
+
+	dev->board_name = this_board->name;
+
+	n_subdevices = 0;
+	if (this_board->n_aichan)
+		n_subdevices++;
+	if (this_board->n_aochan)
+		n_subdevices++;
+	if (this_board->n_dichan)
+		n_subdevices++;
+	if (this_board->n_dochan)
+		n_subdevices++;
+	if (this_board->n_ctrs)
+		n_subdevices++;
+
+	if ((ret = alloc_subdevices(dev, n_subdevices)) < 0) {
+		return ret;
+	}
+
+	icp_multi_reset(dev);
+
+	if (this_board->have_irq) {
+		if (irq) {
+			if (comedi_request_irq(irq, interrupt_service_icp_multi,
+					IRQF_SHARED, "Inova Icp Multi", dev)) {
+				printk(", unable to allocate IRQ %u, DISABLING IT", irq);
+				irq = 0;	/* Can't use IRQ */
+			} else
+				printk(", irq=%u", irq);
+		} else
+			printk(", IRQ disabled");
+	} else
+		irq = 0;
+
+	dev->irq = irq;
+
+	printk(".\n");
+
+	subdev = 0;
+
+	if (this_board->n_aichan) {
+		s = dev->subdevices + subdev;
+		dev->read_subdev = s;
+		s->type = COMEDI_SUBD_AI;
+		s->subdev_flags = SDF_READABLE | SDF_COMMON | SDF_GROUND;
+		if (this_board->n_aichand)
+			s->subdev_flags |= SDF_DIFF;
+		s->n_chan = this_board->n_aichan;
+		s->maxdata = this_board->ai_maxdata;
+		s->len_chanlist = this_board->n_aichan;
+		s->range_table = this_board->rangelist_ai;
+		s->insn_read = icp_multi_insn_read_ai;
+		subdev++;
+	}
+
+	if (this_board->n_aochan) {
+		s = dev->subdevices + subdev;
+		s->type = COMEDI_SUBD_AO;
+		s->subdev_flags = SDF_WRITABLE | SDF_GROUND | SDF_COMMON;
+		s->n_chan = this_board->n_aochan;
+		s->maxdata = this_board->ao_maxdata;
+		s->len_chanlist = this_board->n_aochan;
+		s->range_table = this_board->rangelist_ao;
+		s->insn_write = icp_multi_insn_write_ao;
+		s->insn_read = icp_multi_insn_read_ao;
+		subdev++;
+	}
+
+	if (this_board->n_dichan) {
+		s = dev->subdevices + subdev;
+		s->type = COMEDI_SUBD_DI;
+		s->subdev_flags = SDF_READABLE;
+		s->n_chan = this_board->n_dichan;
+		s->maxdata = 1;
+		s->len_chanlist = this_board->n_dichan;
+		s->range_table = &range_digital;
+		s->io_bits = 0;
+		s->insn_bits = icp_multi_insn_bits_di;
+		subdev++;
+	}
+
+	if (this_board->n_dochan) {
+		s = dev->subdevices + subdev;
+		s->type = COMEDI_SUBD_DO;
+		s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
+		s->n_chan = this_board->n_dochan;
+		s->maxdata = 1;
+		s->len_chanlist = this_board->n_dochan;
+		s->range_table = &range_digital;
+		s->io_bits = (1 << this_board->n_dochan) - 1;
+		s->state = 0;
+		s->insn_bits = icp_multi_insn_bits_do;
+		subdev++;
+	}
+
+	if (this_board->n_ctrs) {
+		s = dev->subdevices + subdev;
+		s->type = COMEDI_SUBD_COUNTER;
+		s->subdev_flags = SDF_WRITABLE | SDF_GROUND | SDF_COMMON;
+		s->n_chan = this_board->n_ctrs;
+		s->maxdata = 0xffff;
+		s->len_chanlist = this_board->n_ctrs;
+		s->state = 0;
+		s->insn_read = icp_multi_insn_read_ctr;
+		s->insn_write = icp_multi_insn_write_ctr;
+		subdev++;
+	}
+
+	devpriv->valid = 1;
+
+#ifdef ICP_MULTI_EXTDEBUG
+	printk("icp multi EDBG: END: icp_multi_attach(...)\n");
+#endif
+
+	return 0;
+}
+
+/*
+==============================================================================
+
+	Name:	icp_multi_detach
+
+	Description:
+		This function releases all the resources used by the current
+		device.
+
+	Parameters:
+		comedi_device *dev	Pointer to current device structure
+
+	Returns:int	0 = success
+
+==============================================================================
+*/
+static int icp_multi_detach(comedi_device * dev)
+{
+
+	if (dev->private)
+		if (devpriv->valid)
+			icp_multi_reset(dev);
+
+	if (dev->irq)
+		comedi_free_irq(dev->irq, dev);
+
+	if (dev->private && devpriv->io_addr)
+		iounmap(devpriv->io_addr);
+
+	if (dev->private && devpriv->card)
+		pci_card_free(devpriv->card);
+
+	if (--pci_list_builded == 0) {
+		pci_card_list_cleanup(PCI_VENDOR_ID_ICP);
+	}
+
+	return 0;
+}
diff --git a/drivers/staging/comedi/drivers/icp_multi.h b/drivers/staging/comedi/drivers/icp_multi.h
new file mode 100644
index 00000000000..6df4a8d15ff
--- /dev/null
+++ b/drivers/staging/comedi/drivers/icp_multi.h
@@ -0,0 +1,278 @@
+/*
+    comedi/drivers/icp_multi.h
+
+    Stuff for ICP Multi
+
+    Author: Anne Smorthit <anne.smorthit@sfwte.ch>
+
+*/
+
+#ifndef _ICP_MULTI_H_
+#define _ICP_MULTI_H_
+
+#include "../comedidev.h"
+#include "comedi_pci.h"
+
+/****************************************************************************/
+
+struct pcilst_struct {
+	struct pcilst_struct *next;
+	int used;
+	struct pci_dev *pcidev;
+	unsigned short vendor;
+	unsigned short device;
+	unsigned char pci_bus;
+	unsigned char pci_slot;
+	unsigned char pci_func;
+	resource_size_t io_addr[5];
+	unsigned int irq;
+};
+
+struct pcilst_struct *inova_devices;	// ptr to root list of all Inova devices
+
+/****************************************************************************/
+
+static void pci_card_list_init(unsigned short pci_vendor, char display);
+static void pci_card_list_cleanup(unsigned short pci_vendor);
+static struct pcilst_struct *find_free_pci_card_by_device(unsigned short
+	vendor_id, unsigned short device_id);
+static int find_free_pci_card_by_position(unsigned short vendor_id,
+	unsigned short device_id, unsigned short pci_bus,
+	unsigned short pci_slot, struct pcilst_struct **card);
+static struct pcilst_struct *select_and_alloc_pci_card(unsigned short vendor_id,
+	unsigned short device_id, unsigned short pci_bus,
+	unsigned short pci_slot);
+
+static int pci_card_alloc(struct pcilst_struct *amcc);
+static int pci_card_free(struct pcilst_struct *amcc);
+static void pci_card_list_display(void);
+static int pci_card_data(struct pcilst_struct *amcc,
+	unsigned char *pci_bus, unsigned char *pci_slot,
+	unsigned char *pci_func, resource_size_t * io_addr, unsigned int *irq);
+
+/****************************************************************************/
+
+/* build list of Inova cards in this system */
+static void pci_card_list_init(unsigned short pci_vendor, char display)
+{
+	struct pci_dev *pcidev;
+	struct pcilst_struct *inova, *last;
+	int i;
+
+	inova_devices = NULL;
+	last = NULL;
+
+	for (pcidev = pci_get_device(PCI_ANY_ID, PCI_ANY_ID, NULL);
+		pcidev != NULL;
+		pcidev = pci_get_device(PCI_ANY_ID, PCI_ANY_ID, pcidev)) {
+		if (pcidev->vendor == pci_vendor) {
+			inova = kmalloc(sizeof(*inova), GFP_KERNEL);
+			if (!inova) {
+				printk("icp_multi: pci_card_list_init: allocation failed\n");
+				pci_dev_put(pcidev);
+				break;
+			}
+			memset(inova, 0, sizeof(*inova));
+
+			inova->pcidev = pci_dev_get(pcidev);
+			if (last) {
+				last->next = inova;
+			} else {
+				inova_devices = inova;
+			}
+			last = inova;
+
+			inova->vendor = pcidev->vendor;
+			inova->device = pcidev->device;
+			inova->pci_bus = pcidev->bus->number;
+			inova->pci_slot = PCI_SLOT(pcidev->devfn);
+			inova->pci_func = PCI_FUNC(pcidev->devfn);
+			/* Note: resources may be invalid if PCI device
+			 * not enabled, but they are corrected in
+			 * pci_card_alloc. */
+			for (i = 0; i < 5; i++)
+				inova->io_addr[i] =
+					pci_resource_start(pcidev, i);
+			inova->irq = pcidev->irq;
+		}
+	}
+
+	if (display)
+		pci_card_list_display();
+}
+
+/****************************************************************************/
+/* free up list of amcc cards in this system */
+static void pci_card_list_cleanup(unsigned short pci_vendor)
+{
+	struct pcilst_struct *inova, *next;
+
+	for (inova = inova_devices; inova; inova = next) {
+		next = inova->next;
+		pci_dev_put(inova->pcidev);
+		kfree(inova);
+	}
+
+	inova_devices = NULL;
+}
+
+/****************************************************************************/
+/* find first unused card with this device_id */
+static struct pcilst_struct *find_free_pci_card_by_device(unsigned short
+	vendor_id, unsigned short device_id)
+{
+	struct pcilst_struct *inova, *next;
+
+	for (inova = inova_devices; inova; inova = next) {
+		next = inova->next;
+		if ((!inova->used) && (inova->device == device_id)
+			&& (inova->vendor == vendor_id))
+			return inova;
+
+	}
+
+	return NULL;
+}
+
+/****************************************************************************/
+/* find card on requested position */
+static int find_free_pci_card_by_position(unsigned short vendor_id,
+	unsigned short device_id, unsigned short pci_bus,
+	unsigned short pci_slot, struct pcilst_struct **card)
+{
+	struct pcilst_struct *inova, *next;
+
+	*card = NULL;
+	for (inova = inova_devices; inova; inova = next) {
+		next = inova->next;
+		if ((inova->vendor == vendor_id) && (inova->device == device_id)
+			&& (inova->pci_bus == pci_bus)
+			&& (inova->pci_slot == pci_slot)) {
+			if (!(inova->used)) {
+				*card = inova;
+				return 0;	// ok, card is found
+			} else {
+				return 2;	// card exist but is used
+			}
+		}
+	}
+
+	return 1;		// no card found
+}
+
+/****************************************************************************/
+/* mark card as used */
+static int pci_card_alloc(struct pcilst_struct *inova)
+{
+	int i;
+
+	if (!inova) {
+		rt_printk(" - BUG!! inova is NULL!\n");
+		return -1;
+	}
+
+	if (inova->used)
+		return 1;
+	if (comedi_pci_enable(inova->pcidev, "icp_multi")) {
+		rt_printk(" - Can't enable PCI device and request regions!\n");
+		return -1;
+	}
+	/* Resources will be accurate now. */
+	for (i = 0; i < 5; i++)
+		inova->io_addr[i] = pci_resource_start(inova->pcidev, i);
+	inova->irq = inova->pcidev->irq;
+	inova->used = 1;
+	return 0;
+}
+
+/****************************************************************************/
+/* mark card as free */
+static int pci_card_free(struct pcilst_struct *inova)
+{
+	if (!inova)
+		return -1;
+
+	if (!inova->used)
+		return 1;
+	inova->used = 0;
+	comedi_pci_disable(inova->pcidev);
+	return 0;
+}
+
+/****************************************************************************/
+/* display list of found cards */
+static void pci_card_list_display(void)
+{
+	struct pcilst_struct *inova, *next;
+
+	printk("Anne's List of pci cards\n");
+	printk("bus:slot:func vendor device io_inova io_daq irq used\n");
+
+	for (inova = inova_devices; inova; inova = next) {
+		next = inova->next;
+		printk("%2d   %2d   %2d  0x%4x 0x%4x   0x%8llx 0x%8llx  %2u  %2d\n", inova->pci_bus, inova->pci_slot, inova->pci_func, inova->vendor, inova->device, (unsigned long long)inova->io_addr[0], (unsigned long long)inova->io_addr[2], inova->irq, inova->used);
+
+	}
+}
+
+/****************************************************************************/
+/* return all card information for driver */
+static int pci_card_data(struct pcilst_struct *inova,
+	unsigned char *pci_bus, unsigned char *pci_slot,
+	unsigned char *pci_func, resource_size_t * io_addr, unsigned int *irq)
+{
+	int i;
+
+	if (!inova)
+		return -1;
+	*pci_bus = inova->pci_bus;
+	*pci_slot = inova->pci_slot;
+	*pci_func = inova->pci_func;
+	for (i = 0; i < 5; i++)
+		io_addr[i] = inova->io_addr[i];
+	*irq = inova->irq;
+	return 0;
+}
+
+/****************************************************************************/
+/* select and alloc card */
+static struct pcilst_struct *select_and_alloc_pci_card(unsigned short vendor_id,
+	unsigned short device_id, unsigned short pci_bus,
+	unsigned short pci_slot)
+{
+	struct pcilst_struct *card;
+	int err;
+
+	if ((pci_bus < 1) & (pci_slot < 1)) {	// use autodetection
+		if ((card = find_free_pci_card_by_device(vendor_id,
+					device_id)) == NULL) {
+			rt_printk(" - Unused card not found in system!\n");
+			return NULL;
+		}
+	} else {
+		switch (find_free_pci_card_by_position(vendor_id, device_id,
+				pci_bus, pci_slot, &card)) {
+		case 1:
+			rt_printk
+				(" - Card not found on requested position b:s %d:%d!\n",
+				pci_bus, pci_slot);
+			return NULL;
+		case 2:
+			rt_printk
+				(" - Card on requested position is used b:s %d:%d!\n",
+				pci_bus, pci_slot);
+			return NULL;
+		}
+	}
+
+	if ((err = pci_card_alloc(card)) != 0) {
+		if (err > 0)
+			rt_printk(" - Can't allocate card!\n");
+		/* else: error already printed. */
+		return NULL;
+	}
+
+	return card;
+}
+
+#endif
diff --git a/drivers/staging/comedi/drivers/me4000.c b/drivers/staging/comedi/drivers/me4000.c
new file mode 100644
index 00000000000..b432aa7d764
--- /dev/null
+++ b/drivers/staging/comedi/drivers/me4000.c
@@ -0,0 +1,2362 @@
+/*
+   comedi/drivers/me4000.c
+   Source code for the Meilhaus ME-4000 board family.
+
+   COMEDI - Linux Control and Measurement Device Interface
+   Copyright (C) 2000 David A. Schleef <ds@schleef.org>
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation; either version 2 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program; if not, write to the Free Software
+   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+ */
+/*
+Driver: me4000
+Description: Meilhaus ME-4000 series boards
+Devices: [Meilhaus] ME-4650 (me4000), ME-4670i, ME-4680, ME-4680i, ME-4680is
+Author: gg (Guenter Gebhardt <g.gebhardt@meilhaus.com>)
+Updated: Mon, 18 Mar 2002 15:34:01 -0800
+Status: broken (no support for loading firmware)
+
+Supports:
+
+    - Analog Input
+    - Analog Output
+    - Digital I/O
+    - Counter
+
+Configuration Options:
+
+    [0] - PCI bus number (optional)
+    [1] - PCI slot number (optional)
+
+    If bus/slot is not specified, the first available PCI
+    device will be used.
+
+The firmware required by these boards is available in the
+comedi_nonfree_firmware tarball available from
+http://www.comedi.org.  However, the driver's support for
+loading the firmware through comedi_config is currently
+broken.
+
+ */
+
+#include "../comedidev.h"
+
+#include <linux/delay.h>
+#include <linux/list.h>
+#include <linux/spinlock.h>
+
+#include "comedi_pci.h"
+#include "me4000.h"
+#if 0
+/* file removed due to GPL incompatibility */
+#include "me4000_fw.h"
+#endif
+
+/*=============================================================================
+  PCI device table.
+  This is used by modprobe to translate PCI IDs to drivers.
+  ===========================================================================*/
+
+static DEFINE_PCI_DEVICE_TABLE(me4000_pci_table) = {
+	{PCI_VENDOR_ID_MEILHAUS, 0x4650, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, 0x4660, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, 0x4661, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, 0x4662, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, 0x4663, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, 0x4670, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, 0x4671, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, 0x4672, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, 0x4673, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, 0x4680, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, 0x4681, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, 0x4682, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, 0x4683, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+
+	{0}
+};
+
+MODULE_DEVICE_TABLE(pci, me4000_pci_table);
+
+static const me4000_board_t me4000_boards[] = {
+	{"ME-4650", 0x4650, {0, 0}, {16, 0, 0, 0}, {4}, {0}},
+
+	{"ME-4660", 0x4660, {0, 0}, {32, 0, 16, 0}, {4}, {3}},
+	{"ME-4660i", 0x4661, {0, 0}, {32, 0, 16, 0}, {4}, {3}},
+	{"ME-4660s", 0x4662, {0, 0}, {32, 8, 16, 0}, {4}, {3}},
+	{"ME-4660is", 0x4663, {0, 0}, {32, 8, 16, 0}, {4}, {3}},
+
+	{"ME-4670", 0x4670, {4, 0}, {32, 0, 16, 1}, {4}, {3}},
+	{"ME-4670i", 0x4671, {4, 0}, {32, 0, 16, 1}, {4}, {3}},
+	{"ME-4670s", 0x4672, {4, 0}, {32, 8, 16, 1}, {4}, {3}},
+	{"ME-4670is", 0x4673, {4, 0}, {32, 8, 16, 1}, {4}, {3}},
+
+	{"ME-4680", 0x4680, {4, 4}, {32, 0, 16, 1}, {4}, {3}},
+	{"ME-4680i", 0x4681, {4, 4}, {32, 0, 16, 1}, {4}, {3}},
+	{"ME-4680s", 0x4682, {4, 4}, {32, 8, 16, 1}, {4}, {3}},
+	{"ME-4680is", 0x4683, {4, 4}, {32, 8, 16, 1}, {4}, {3}},
+
+	{0},
+};
+
+#define ME4000_BOARD_VERSIONS (sizeof(me4000_boards) / sizeof(me4000_board_t) - 1)
+
+/*-----------------------------------------------------------------------------
+  Comedi function prototypes
+  ---------------------------------------------------------------------------*/
+static int me4000_attach(comedi_device * dev, comedi_devconfig * it);
+static int me4000_detach(comedi_device * dev);
+static comedi_driver driver_me4000 = {
+      driver_name:"me4000",
+      module:THIS_MODULE,
+      attach:me4000_attach,
+      detach:me4000_detach,
+};
+
+/*-----------------------------------------------------------------------------
+  Meilhaus function prototypes
+  ---------------------------------------------------------------------------*/
+static int me4000_probe(comedi_device * dev, comedi_devconfig * it);
+static int get_registers(comedi_device * dev, struct pci_dev *pci_dev_p);
+static int init_board_info(comedi_device * dev, struct pci_dev *pci_dev_p);
+static int init_ao_context(comedi_device * dev);
+static int init_ai_context(comedi_device * dev);
+static int init_dio_context(comedi_device * dev);
+static int init_cnt_context(comedi_device * dev);
+static int xilinx_download(comedi_device * dev);
+static int reset_board(comedi_device * dev);
+
+static int me4000_dio_insn_bits(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data);
+
+static int me4000_dio_insn_config(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data);
+
+static int cnt_reset(comedi_device * dev, unsigned int channel);
+
+static int cnt_config(comedi_device * dev,
+	unsigned int channel, unsigned int mode);
+
+static int me4000_cnt_insn_config(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data);
+
+static int me4000_cnt_insn_write(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data);
+
+static int me4000_cnt_insn_read(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data);
+
+static int me4000_ai_insn_read(comedi_device * dev,
+	comedi_subdevice * subdevice, comedi_insn * insn, lsampl_t * data);
+
+static int me4000_ai_cancel(comedi_device * dev, comedi_subdevice * s);
+
+static int ai_check_chanlist(comedi_device * dev,
+	comedi_subdevice * s, comedi_cmd * cmd);
+
+static int ai_round_cmd_args(comedi_device * dev,
+	comedi_subdevice * s,
+	comedi_cmd * cmd,
+	unsigned int *init_ticks,
+	unsigned int *scan_ticks, unsigned int *chan_ticks);
+
+static int ai_prepare(comedi_device * dev,
+	comedi_subdevice * s,
+	comedi_cmd * cmd,
+	unsigned int init_ticks,
+	unsigned int scan_ticks, unsigned int chan_ticks);
+
+static int ai_write_chanlist(comedi_device * dev,
+	comedi_subdevice * s, comedi_cmd * cmd);
+
+static irqreturn_t me4000_ai_isr(int irq, void *dev_id PT_REGS_ARG);
+
+static int me4000_ai_do_cmd_test(comedi_device * dev,
+	comedi_subdevice * s, comedi_cmd * cmd);
+
+static int me4000_ai_do_cmd(comedi_device * dev, comedi_subdevice * s);
+
+static int me4000_ao_insn_write(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data);
+
+static int me4000_ao_insn_read(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data);
+
+/*-----------------------------------------------------------------------------
+  Meilhaus inline functions
+  ---------------------------------------------------------------------------*/
+
+static inline void me4000_outb(comedi_device * dev, unsigned char value,
+	unsigned long port)
+{
+	PORT_PDEBUG("--> 0x%02X port 0x%04lX\n", value, port);
+	outb(value, port);
+}
+
+static inline void me4000_outl(comedi_device * dev, unsigned long value,
+	unsigned long port)
+{
+	PORT_PDEBUG("--> 0x%08lX port 0x%04lX\n", value, port);
+	outl(value, port);
+}
+
+static inline unsigned long me4000_inl(comedi_device * dev, unsigned long port)
+{
+	unsigned long value;
+	value = inl(port);
+	PORT_PDEBUG("<-- 0x%08lX port 0x%04lX\n", value, port);
+	return value;
+}
+
+static inline unsigned char me4000_inb(comedi_device * dev, unsigned long port)
+{
+	unsigned char value;
+	value = inb(port);
+	PORT_PDEBUG("<-- 0x%08X port 0x%04lX\n", value, port);
+	return value;
+}
+
+static const comedi_lrange me4000_ai_range = {
+	4,
+	{
+			UNI_RANGE(2.5),
+			UNI_RANGE(10),
+			BIP_RANGE(2.5),
+			BIP_RANGE(10),
+		}
+};
+
+static const comedi_lrange me4000_ao_range = {
+	1,
+	{
+			BIP_RANGE(10),
+		}
+};
+
+static int me4000_attach(comedi_device * dev, comedi_devconfig * it)
+{
+	comedi_subdevice *s;
+	int result;
+
+	CALL_PDEBUG("In me4000_attach()\n");
+
+	result = me4000_probe(dev, it);
+	if (result)
+		return result;
+
+	/*
+	 * Allocate the subdevice structures.  alloc_subdevice() is a
+	 * convenient macro defined in comedidev.h.  It relies on
+	 * n_subdevices being set correctly.
+	 */
+	if (alloc_subdevices(dev, 4) < 0)
+		return -ENOMEM;
+
+    /*=========================================================================
+      Analog input subdevice
+      ========================================================================*/
+
+	s = dev->subdevices + 0;
+
+	if (thisboard->ai.count) {
+		s->type = COMEDI_SUBD_AI;
+		s->subdev_flags =
+			SDF_READABLE | SDF_COMMON | SDF_GROUND | SDF_DIFF;
+		s->n_chan = thisboard->ai.count;
+		s->maxdata = 0xFFFF;	// 16 bit ADC
+		s->len_chanlist = ME4000_AI_CHANNEL_LIST_COUNT;
+		s->range_table = &me4000_ai_range;
+		s->insn_read = me4000_ai_insn_read;
+
+		if (info->irq > 0) {
+			if (comedi_request_irq(info->irq, me4000_ai_isr,
+					IRQF_SHARED, "ME-4000", dev)) {
+				printk("comedi%d: me4000: me4000_attach(): Unable to allocate irq\n", dev->minor);
+			} else {
+				dev->read_subdev = s;
+				s->subdev_flags |= SDF_CMD_READ;
+				s->cancel = me4000_ai_cancel;
+				s->do_cmdtest = me4000_ai_do_cmd_test;
+				s->do_cmd = me4000_ai_do_cmd;
+			}
+		} else {
+			printk(KERN_WARNING
+				"comedi%d: me4000: me4000_attach(): No interrupt available\n",
+				dev->minor);
+		}
+	} else {
+		s->type = COMEDI_SUBD_UNUSED;
+	}
+
+    /*=========================================================================
+      Analog output subdevice
+      ========================================================================*/
+
+	s = dev->subdevices + 1;
+
+	if (thisboard->ao.count) {
+		s->type = COMEDI_SUBD_AO;
+		s->subdev_flags = SDF_WRITEABLE | SDF_COMMON | SDF_GROUND;
+		s->n_chan = thisboard->ao.count;
+		s->maxdata = 0xFFFF;	// 16 bit DAC
+		s->range_table = &me4000_ao_range;
+		s->insn_write = me4000_ao_insn_write;
+		s->insn_read = me4000_ao_insn_read;
+	} else {
+		s->type = COMEDI_SUBD_UNUSED;
+	}
+
+    /*=========================================================================
+      Digital I/O subdevice
+      ========================================================================*/
+
+	s = dev->subdevices + 2;
+
+	if (thisboard->dio.count) {
+		s->type = COMEDI_SUBD_DIO;
+		s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+		s->n_chan = thisboard->dio.count * 8;
+		s->maxdata = 1;
+		s->range_table = &range_digital;
+		s->insn_bits = me4000_dio_insn_bits;
+		s->insn_config = me4000_dio_insn_config;
+	} else {
+		s->type = COMEDI_SUBD_UNUSED;
+	}
+
+	/*
+	 * Check for optoisolated ME-4000 version. If one the first
+	 * port is a fixed output port and the second is a fixed input port.
+	 */
+	if (!me4000_inl(dev, info->dio_context.dir_reg)) {
+		s->io_bits |= 0xFF;
+		me4000_outl(dev, ME4000_DIO_CTRL_BIT_MODE_0,
+			info->dio_context.dir_reg);
+	}
+
+    /*=========================================================================
+      Counter subdevice
+      ========================================================================*/
+
+	s = dev->subdevices + 3;
+
+	if (thisboard->cnt.count) {
+		s->type = COMEDI_SUBD_COUNTER;
+		s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+		s->n_chan = thisboard->cnt.count;
+		s->maxdata = 0xFFFF;	// 16 bit counters
+		s->insn_read = me4000_cnt_insn_read;
+		s->insn_write = me4000_cnt_insn_write;
+		s->insn_config = me4000_cnt_insn_config;
+	} else {
+		s->type = COMEDI_SUBD_UNUSED;
+	}
+
+	return 0;
+}
+
+static int me4000_probe(comedi_device * dev, comedi_devconfig * it)
+{
+	struct pci_dev *pci_device;
+	int result, i;
+	me4000_board_t *board;
+
+	CALL_PDEBUG("In me4000_probe()\n");
+
+	/* Allocate private memory */
+	if (alloc_private(dev, sizeof(me4000_info_t)) < 0) {
+		return -ENOMEM;
+	}
+	/*
+	 * Probe the device to determine what device in the series it is.
+	 */
+	for (pci_device = pci_get_device(PCI_ANY_ID, PCI_ANY_ID, NULL);
+		pci_device != NULL;
+		pci_device =
+		pci_get_device(PCI_ANY_ID, PCI_ANY_ID, pci_device)) {
+		if (pci_device->vendor == PCI_VENDOR_ID_MEILHAUS) {
+			for (i = 0; i < ME4000_BOARD_VERSIONS; i++) {
+				if (me4000_boards[i].device_id ==
+					pci_device->device) {
+					/* Was a particular bus/slot requested? */
+					if ((it->options[0] != 0)
+						|| (it->options[1] != 0)) {
+						/* Are we on the wrong bus/slot? */
+						if (pci_device->bus->number !=
+							it->options[0]
+							|| PCI_SLOT(pci_device->
+								devfn) !=
+							it->options[1]) {
+							continue;
+						}
+					}
+					dev->board_ptr = me4000_boards + i;
+					board = (me4000_board_t *) dev->
+						board_ptr;
+					info->pci_dev_p = pci_device;
+					goto found;
+				}
+			}
+		}
+	}
+
+	printk(KERN_ERR
+		"comedi%d: me4000: me4000_probe(): No supported board found (req. bus/slot : %d/%d)\n",
+		dev->minor, it->options[0], it->options[1]);
+	return -ENODEV;
+
+      found:
+
+	printk(KERN_INFO
+		"comedi%d: me4000: me4000_probe(): Found %s at PCI bus %d, slot %d\n",
+		dev->minor, me4000_boards[i].name, pci_device->bus->number,
+		PCI_SLOT(pci_device->devfn));
+
+	/* Set data in device structure */
+	dev->board_name = board->name;
+
+	/* Enable PCI device and request regions */
+	result = comedi_pci_enable(pci_device, dev->board_name);
+	if (result) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_probe(): Cannot enable PCI device and request I/O regions\n",
+			dev->minor);
+		return result;
+	}
+
+	/* Get the PCI base registers */
+	result = get_registers(dev, pci_device);
+	if (result) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_probe(): Cannot get registers\n",
+			dev->minor);
+		return result;
+	}
+	/* Initialize board info */
+	result = init_board_info(dev, pci_device);
+	if (result) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_probe(): Cannot init baord info\n",
+			dev->minor);
+		return result;
+	}
+
+	/* Init analog output context */
+	result = init_ao_context(dev);
+	if (result) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_probe(): Cannot init ao context\n",
+			dev->minor);
+		return result;
+	}
+
+	/* Init analog input context */
+	result = init_ai_context(dev);
+	if (result) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_probe(): Cannot init ai context\n",
+			dev->minor);
+		return result;
+	}
+
+	/* Init digital I/O context */
+	result = init_dio_context(dev);
+	if (result) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_probe(): Cannot init dio context\n",
+			dev->minor);
+		return result;
+	}
+
+	/* Init counter context */
+	result = init_cnt_context(dev);
+	if (result) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_probe(): Cannot init cnt context\n",
+			dev->minor);
+		return result;
+	}
+
+	/* Download the xilinx firmware */
+	result = xilinx_download(dev);
+	if (result) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_probe(): Can't download firmware\n",
+			dev->minor);
+		return result;
+	}
+
+	/* Make a hardware reset */
+	result = reset_board(dev);
+	if (result) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_probe(): Can't reset board\n",
+			dev->minor);
+		return result;
+	}
+
+	return 0;
+}
+
+static int get_registers(comedi_device * dev, struct pci_dev *pci_dev_p)
+{
+
+	CALL_PDEBUG("In get_registers()\n");
+
+    /*--------------------------- plx regbase ---------------------------------*/
+
+	info->plx_regbase = pci_resource_start(pci_dev_p, 1);
+	if (info->plx_regbase == 0) {
+		printk(KERN_ERR
+			"comedi%d: me4000: get_registers(): PCI base address 1 is not available\n",
+			dev->minor);
+		return -ENODEV;
+	}
+	info->plx_regbase_size = pci_resource_len(pci_dev_p, 1);
+
+    /*--------------------------- me4000 regbase ------------------------------*/
+
+	info->me4000_regbase = pci_resource_start(pci_dev_p, 2);
+	if (info->me4000_regbase == 0) {
+		printk(KERN_ERR
+			"comedi%d: me4000: get_registers(): PCI base address 2 is not available\n",
+			dev->minor);
+		return -ENODEV;
+	}
+	info->me4000_regbase_size = pci_resource_len(pci_dev_p, 2);
+
+    /*--------------------------- timer regbase ------------------------------*/
+
+	info->timer_regbase = pci_resource_start(pci_dev_p, 3);
+	if (info->timer_regbase == 0) {
+		printk(KERN_ERR
+			"comedi%d: me4000: get_registers(): PCI base address 3 is not available\n",
+			dev->minor);
+		return -ENODEV;
+	}
+	info->timer_regbase_size = pci_resource_len(pci_dev_p, 3);
+
+    /*--------------------------- program regbase ------------------------------*/
+
+	info->program_regbase = pci_resource_start(pci_dev_p, 5);
+	if (info->program_regbase == 0) {
+		printk(KERN_ERR
+			"comedi%d: me4000: get_registers(): PCI base address 5 is not available\n",
+			dev->minor);
+		return -ENODEV;
+	}
+	info->program_regbase_size = pci_resource_len(pci_dev_p, 5);
+
+	return 0;
+}
+
+static int init_board_info(comedi_device * dev, struct pci_dev *pci_dev_p)
+{
+	int result;
+
+	CALL_PDEBUG("In init_board_info()\n");
+
+	/* Init spin locks */
+	//spin_lock_init(&info->preload_lock);
+	//spin_lock_init(&info->ai_ctrl_lock);
+
+	/* Get the serial number */
+	result = pci_read_config_dword(pci_dev_p, 0x2C, &info->serial_no);
+	if (result != PCIBIOS_SUCCESSFUL) {
+		return result;
+	}
+
+	/* Get the hardware revision */
+	result = pci_read_config_byte(pci_dev_p, 0x08, &info->hw_revision);
+	if (result != PCIBIOS_SUCCESSFUL) {
+		return result;
+	}
+
+	/* Get the vendor id */
+	info->vendor_id = pci_dev_p->vendor;
+
+	/* Get the device id */
+	info->device_id = pci_dev_p->device;
+
+	/* Get the irq assigned to the board */
+	info->irq = pci_dev_p->irq;
+
+	return 0;
+}
+
+static int init_ao_context(comedi_device * dev)
+{
+	int i;
+
+	CALL_PDEBUG("In init_ao_context()\n");
+
+	for (i = 0; i < thisboard->ao.count; i++) {
+		//spin_lock_init(&info->ao_context[i].use_lock);
+		info->ao_context[i].irq = info->irq;
+
+		switch (i) {
+		case 0:
+			info->ao_context[i].ctrl_reg =
+				info->me4000_regbase + ME4000_AO_00_CTRL_REG;
+			info->ao_context[i].status_reg =
+				info->me4000_regbase + ME4000_AO_00_STATUS_REG;
+			info->ao_context[i].fifo_reg =
+				info->me4000_regbase + ME4000_AO_00_FIFO_REG;
+			info->ao_context[i].single_reg =
+				info->me4000_regbase + ME4000_AO_00_SINGLE_REG;
+			info->ao_context[i].timer_reg =
+				info->me4000_regbase + ME4000_AO_00_TIMER_REG;
+			info->ao_context[i].irq_status_reg =
+				info->me4000_regbase + ME4000_IRQ_STATUS_REG;
+			info->ao_context[i].preload_reg =
+				info->me4000_regbase + ME4000_AO_LOADSETREG_XX;
+			break;
+		case 1:
+			info->ao_context[i].ctrl_reg =
+				info->me4000_regbase + ME4000_AO_01_CTRL_REG;
+			info->ao_context[i].status_reg =
+				info->me4000_regbase + ME4000_AO_01_STATUS_REG;
+			info->ao_context[i].fifo_reg =
+				info->me4000_regbase + ME4000_AO_01_FIFO_REG;
+			info->ao_context[i].single_reg =
+				info->me4000_regbase + ME4000_AO_01_SINGLE_REG;
+			info->ao_context[i].timer_reg =
+				info->me4000_regbase + ME4000_AO_01_TIMER_REG;
+			info->ao_context[i].irq_status_reg =
+				info->me4000_regbase + ME4000_IRQ_STATUS_REG;
+			info->ao_context[i].preload_reg =
+				info->me4000_regbase + ME4000_AO_LOADSETREG_XX;
+			break;
+		case 2:
+			info->ao_context[i].ctrl_reg =
+				info->me4000_regbase + ME4000_AO_02_CTRL_REG;
+			info->ao_context[i].status_reg =
+				info->me4000_regbase + ME4000_AO_02_STATUS_REG;
+			info->ao_context[i].fifo_reg =
+				info->me4000_regbase + ME4000_AO_02_FIFO_REG;
+			info->ao_context[i].single_reg =
+				info->me4000_regbase + ME4000_AO_02_SINGLE_REG;
+			info->ao_context[i].timer_reg =
+				info->me4000_regbase + ME4000_AO_02_TIMER_REG;
+			info->ao_context[i].irq_status_reg =
+				info->me4000_regbase + ME4000_IRQ_STATUS_REG;
+			info->ao_context[i].preload_reg =
+				info->me4000_regbase + ME4000_AO_LOADSETREG_XX;
+			break;
+		case 3:
+			info->ao_context[i].ctrl_reg =
+				info->me4000_regbase + ME4000_AO_03_CTRL_REG;
+			info->ao_context[i].status_reg =
+				info->me4000_regbase + ME4000_AO_03_STATUS_REG;
+			info->ao_context[i].fifo_reg =
+				info->me4000_regbase + ME4000_AO_03_FIFO_REG;
+			info->ao_context[i].single_reg =
+				info->me4000_regbase + ME4000_AO_03_SINGLE_REG;
+			info->ao_context[i].timer_reg =
+				info->me4000_regbase + ME4000_AO_03_TIMER_REG;
+			info->ao_context[i].irq_status_reg =
+				info->me4000_regbase + ME4000_IRQ_STATUS_REG;
+			info->ao_context[i].preload_reg =
+				info->me4000_regbase + ME4000_AO_LOADSETREG_XX;
+			break;
+		default:
+			break;
+		}
+	}
+
+	return 0;
+}
+
+static int init_ai_context(comedi_device * dev)
+{
+
+	CALL_PDEBUG("In init_ai_context()\n");
+
+	info->ai_context.irq = info->irq;
+
+	info->ai_context.ctrl_reg = info->me4000_regbase + ME4000_AI_CTRL_REG;
+	info->ai_context.status_reg =
+		info->me4000_regbase + ME4000_AI_STATUS_REG;
+	info->ai_context.channel_list_reg =
+		info->me4000_regbase + ME4000_AI_CHANNEL_LIST_REG;
+	info->ai_context.data_reg = info->me4000_regbase + ME4000_AI_DATA_REG;
+	info->ai_context.chan_timer_reg =
+		info->me4000_regbase + ME4000_AI_CHAN_TIMER_REG;
+	info->ai_context.chan_pre_timer_reg =
+		info->me4000_regbase + ME4000_AI_CHAN_PRE_TIMER_REG;
+	info->ai_context.scan_timer_low_reg =
+		info->me4000_regbase + ME4000_AI_SCAN_TIMER_LOW_REG;
+	info->ai_context.scan_timer_high_reg =
+		info->me4000_regbase + ME4000_AI_SCAN_TIMER_HIGH_REG;
+	info->ai_context.scan_pre_timer_low_reg =
+		info->me4000_regbase + ME4000_AI_SCAN_PRE_TIMER_LOW_REG;
+	info->ai_context.scan_pre_timer_high_reg =
+		info->me4000_regbase + ME4000_AI_SCAN_PRE_TIMER_HIGH_REG;
+	info->ai_context.start_reg = info->me4000_regbase + ME4000_AI_START_REG;
+	info->ai_context.irq_status_reg =
+		info->me4000_regbase + ME4000_IRQ_STATUS_REG;
+	info->ai_context.sample_counter_reg =
+		info->me4000_regbase + ME4000_AI_SAMPLE_COUNTER_REG;
+
+	return 0;
+}
+
+static int init_dio_context(comedi_device * dev)
+{
+
+	CALL_PDEBUG("In init_dio_context()\n");
+
+	info->dio_context.dir_reg = info->me4000_regbase + ME4000_DIO_DIR_REG;
+	info->dio_context.ctrl_reg = info->me4000_regbase + ME4000_DIO_CTRL_REG;
+	info->dio_context.port_0_reg =
+		info->me4000_regbase + ME4000_DIO_PORT_0_REG;
+	info->dio_context.port_1_reg =
+		info->me4000_regbase + ME4000_DIO_PORT_1_REG;
+	info->dio_context.port_2_reg =
+		info->me4000_regbase + ME4000_DIO_PORT_2_REG;
+	info->dio_context.port_3_reg =
+		info->me4000_regbase + ME4000_DIO_PORT_3_REG;
+
+	return 0;
+}
+
+static int init_cnt_context(comedi_device * dev)
+{
+
+	CALL_PDEBUG("In init_cnt_context()\n");
+
+	info->cnt_context.ctrl_reg = info->timer_regbase + ME4000_CNT_CTRL_REG;
+	info->cnt_context.counter_0_reg =
+		info->timer_regbase + ME4000_CNT_COUNTER_0_REG;
+	info->cnt_context.counter_1_reg =
+		info->timer_regbase + ME4000_CNT_COUNTER_1_REG;
+	info->cnt_context.counter_2_reg =
+		info->timer_regbase + ME4000_CNT_COUNTER_2_REG;
+
+	return 0;
+}
+
+#define FIRMWARE_NOT_AVAILABLE 1
+#if FIRMWARE_NOT_AVAILABLE
+extern unsigned char *xilinx_firm;
+#endif
+
+static int xilinx_download(comedi_device * dev)
+{
+	u32 value = 0;
+	wait_queue_head_t queue;
+	int idx = 0;
+	int size = 0;
+
+	CALL_PDEBUG("In xilinx_download()\n");
+
+	init_waitqueue_head(&queue);
+
+	/*
+	 * Set PLX local interrupt 2 polarity to high.
+	 * Interrupt is thrown by init pin of xilinx.
+	 */
+	outl(0x10, info->plx_regbase + PLX_INTCSR);
+
+	/* Set /CS and /WRITE of the Xilinx */
+	value = inl(info->plx_regbase + PLX_ICR);
+	value |= 0x100;
+	outl(value, info->plx_regbase + PLX_ICR);
+
+	/* Init Xilinx with CS1 */
+	inb(info->program_regbase + 0xC8);
+
+	/* Wait until /INIT pin is set */
+	udelay(20);
+	if (!inl(info->plx_regbase + PLX_INTCSR) & 0x20) {
+		printk(KERN_ERR
+			"comedi%d: me4000: xilinx_download(): Can't init Xilinx\n",
+			dev->minor);
+		return -EIO;
+	}
+
+	/* Reset /CS and /WRITE of the Xilinx */
+	value = inl(info->plx_regbase + PLX_ICR);
+	value &= ~0x100;
+	outl(value, info->plx_regbase + PLX_ICR);
+	if (FIRMWARE_NOT_AVAILABLE) {
+		comedi_error(dev,
+			"xilinx firmware unavailable due to licensing, aborting");
+		return -EIO;
+	} else {
+		/* Download Xilinx firmware */
+		size = (xilinx_firm[0] << 24) + (xilinx_firm[1] << 16) +
+			(xilinx_firm[2] << 8) + xilinx_firm[3];
+		udelay(10);
+
+		for (idx = 0; idx < size; idx++) {
+			outb(xilinx_firm[16 + idx], info->program_regbase);
+			udelay(10);
+
+			/* Check if BUSY flag is low */
+			if (inl(info->plx_regbase + PLX_ICR) & 0x20) {
+				printk(KERN_ERR
+					"comedi%d: me4000: xilinx_download(): Xilinx is still busy (idx = %d)\n",
+					dev->minor, idx);
+				return -EIO;
+			}
+		}
+	}
+
+	/* If done flag is high download was successful */
+	if (inl(info->plx_regbase + PLX_ICR) & 0x4) {
+	} else {
+		printk(KERN_ERR
+			"comedi%d: me4000: xilinx_download(): DONE flag is not set\n",
+			dev->minor);
+		printk(KERN_ERR
+			"comedi%d: me4000: xilinx_download(): Download not succesful\n",
+			dev->minor);
+		return -EIO;
+	}
+
+	/* Set /CS and /WRITE */
+	value = inl(info->plx_regbase + PLX_ICR);
+	value |= 0x100;
+	outl(value, info->plx_regbase + PLX_ICR);
+
+	return 0;
+}
+
+static int reset_board(comedi_device * dev)
+{
+	unsigned long icr;
+
+	CALL_PDEBUG("In reset_board()\n");
+
+	/* Make a hardware reset */
+	icr = me4000_inl(dev, info->plx_regbase + PLX_ICR);
+	icr |= 0x40000000;
+	me4000_outl(dev, icr, info->plx_regbase + PLX_ICR);
+	icr &= ~0x40000000;
+	me4000_outl(dev, icr, info->plx_regbase + PLX_ICR);
+
+	/* 0x8000 to the DACs means an output voltage of 0V */
+	me4000_outl(dev, 0x8000,
+		info->me4000_regbase + ME4000_AO_00_SINGLE_REG);
+	me4000_outl(dev, 0x8000,
+		info->me4000_regbase + ME4000_AO_01_SINGLE_REG);
+	me4000_outl(dev, 0x8000,
+		info->me4000_regbase + ME4000_AO_02_SINGLE_REG);
+	me4000_outl(dev, 0x8000,
+		info->me4000_regbase + ME4000_AO_03_SINGLE_REG);
+
+	/* Set both stop bits in the analog input control register */
+	me4000_outl(dev,
+		ME4000_AI_CTRL_BIT_IMMEDIATE_STOP | ME4000_AI_CTRL_BIT_STOP,
+		info->me4000_regbase + ME4000_AI_CTRL_REG);
+
+	/* Set both stop bits in the analog output control register */
+	me4000_outl(dev,
+		ME4000_AO_CTRL_BIT_IMMEDIATE_STOP | ME4000_AO_CTRL_BIT_STOP,
+		info->me4000_regbase + ME4000_AO_00_CTRL_REG);
+	me4000_outl(dev,
+		ME4000_AO_CTRL_BIT_IMMEDIATE_STOP | ME4000_AO_CTRL_BIT_STOP,
+		info->me4000_regbase + ME4000_AO_01_CTRL_REG);
+	me4000_outl(dev,
+		ME4000_AO_CTRL_BIT_IMMEDIATE_STOP | ME4000_AO_CTRL_BIT_STOP,
+		info->me4000_regbase + ME4000_AO_02_CTRL_REG);
+	me4000_outl(dev,
+		ME4000_AO_CTRL_BIT_IMMEDIATE_STOP | ME4000_AO_CTRL_BIT_STOP,
+		info->me4000_regbase + ME4000_AO_03_CTRL_REG);
+
+	/* Enable interrupts on the PLX */
+	me4000_outl(dev, 0x43, info->plx_regbase + PLX_INTCSR);
+
+	/* Set the adustment register for AO demux */
+	me4000_outl(dev, ME4000_AO_DEMUX_ADJUST_VALUE,
+		info->me4000_regbase + ME4000_AO_DEMUX_ADJUST_REG);
+
+	/* Set digital I/O direction for port 0 to output on isolated versions */
+	if (!(me4000_inl(dev, info->me4000_regbase + ME4000_DIO_DIR_REG) & 0x1)) {
+		me4000_outl(dev, 0x1,
+			info->me4000_regbase + ME4000_DIO_CTRL_REG);
+	}
+
+	return 0;
+}
+
+static int me4000_detach(comedi_device * dev)
+{
+	CALL_PDEBUG("In me4000_detach()\n");
+
+	if (info) {
+		if (info->pci_dev_p) {
+			reset_board(dev);
+			if (info->plx_regbase) {
+				comedi_pci_disable(info->pci_dev_p);
+			}
+			pci_dev_put(info->pci_dev_p);
+		}
+	}
+
+	return 0;
+}
+
+/*=============================================================================
+  Analog input section
+  ===========================================================================*/
+
+static int me4000_ai_insn_read(comedi_device * dev,
+	comedi_subdevice * subdevice, comedi_insn * insn, lsampl_t * data)
+{
+
+	int chan = CR_CHAN(insn->chanspec);
+	int rang = CR_RANGE(insn->chanspec);
+	int aref = CR_AREF(insn->chanspec);
+
+	unsigned long entry = 0;
+	unsigned long tmp;
+	long lval;
+
+	CALL_PDEBUG("In me4000_ai_insn_read()\n");
+
+	if (insn->n == 0) {
+		return 0;
+	} else if (insn->n > 1) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ai_insn_read(): Invalid instruction length %d\n",
+			dev->minor, insn->n);
+		return -EINVAL;
+	}
+
+	switch (rang) {
+	case 0:
+		entry |= ME4000_AI_LIST_RANGE_UNIPOLAR_2_5;
+		break;
+	case 1:
+		entry |= ME4000_AI_LIST_RANGE_UNIPOLAR_10;
+		break;
+	case 2:
+		entry |= ME4000_AI_LIST_RANGE_BIPOLAR_2_5;
+		break;
+	case 3:
+		entry |= ME4000_AI_LIST_RANGE_BIPOLAR_10;
+		break;
+	default:
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ai_insn_read(): Invalid range specified\n",
+			dev->minor);
+		return -EINVAL;
+	}
+
+	switch (aref) {
+	case AREF_GROUND:
+	case AREF_COMMON:
+		if (chan >= thisboard->ai.count) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_insn_read(): Analog input is not available\n",
+				dev->minor);
+			return -EINVAL;
+		}
+		entry |= ME4000_AI_LIST_INPUT_SINGLE_ENDED | chan;
+		break;
+
+	case AREF_DIFF:
+		if (rang == 0 || rang == 1) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_insn_read(): Range must be bipolar when aref = diff\n",
+				dev->minor);
+			return -EINVAL;
+		}
+
+		if (chan >= thisboard->ai.diff_count) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_insn_read(): Analog input is not available\n",
+				dev->minor);
+			return -EINVAL;
+		}
+		entry |= ME4000_AI_LIST_INPUT_DIFFERENTIAL | chan;
+		break;
+	default:
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ai_insn_read(): Invalid aref specified\n",
+			dev->minor);
+		return -EINVAL;
+	}
+
+	entry |= ME4000_AI_LIST_LAST_ENTRY;
+
+	/* Clear channel list, data fifo and both stop bits */
+	tmp = me4000_inl(dev, info->ai_context.ctrl_reg);
+	tmp &= ~(ME4000_AI_CTRL_BIT_CHANNEL_FIFO |
+		ME4000_AI_CTRL_BIT_DATA_FIFO |
+		ME4000_AI_CTRL_BIT_STOP | ME4000_AI_CTRL_BIT_IMMEDIATE_STOP);
+	me4000_outl(dev, tmp, info->ai_context.ctrl_reg);
+
+	/* Set the acquisition mode to single */
+	tmp &= ~(ME4000_AI_CTRL_BIT_MODE_0 | ME4000_AI_CTRL_BIT_MODE_1 |
+		ME4000_AI_CTRL_BIT_MODE_2);
+	me4000_outl(dev, tmp, info->ai_context.ctrl_reg);
+
+	/* Enable channel list and data fifo */
+	tmp |= ME4000_AI_CTRL_BIT_CHANNEL_FIFO | ME4000_AI_CTRL_BIT_DATA_FIFO;
+	me4000_outl(dev, tmp, info->ai_context.ctrl_reg);
+
+	/* Generate channel list entry */
+	me4000_outl(dev, entry, info->ai_context.channel_list_reg);
+
+	/* Set the timer to maximum sample rate */
+	me4000_outl(dev, ME4000_AI_MIN_TICKS, info->ai_context.chan_timer_reg);
+	me4000_outl(dev, ME4000_AI_MIN_TICKS,
+		info->ai_context.chan_pre_timer_reg);
+
+	/* Start conversion by dummy read */
+	me4000_inl(dev, info->ai_context.start_reg);
+
+	/* Wait until ready */
+	udelay(10);
+	if (!(me4000_inl(dev, info->ai_context.
+				status_reg) & ME4000_AI_STATUS_BIT_EF_DATA)) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ai_insn_read(): Value not available after wait\n",
+			dev->minor);
+		return -EIO;
+	}
+
+	/* Read value from data fifo */
+	lval = me4000_inl(dev, info->ai_context.data_reg) & 0xFFFF;
+	data[0] = lval ^ 0x8000;
+
+	return 1;
+}
+
+static int me4000_ai_cancel(comedi_device * dev, comedi_subdevice * s)
+{
+	unsigned long tmp;
+
+	CALL_PDEBUG("In me4000_ai_cancel()\n");
+
+	/* Stop any running conversion */
+	tmp = me4000_inl(dev, info->ai_context.ctrl_reg);
+	tmp &= ~(ME4000_AI_CTRL_BIT_STOP | ME4000_AI_CTRL_BIT_IMMEDIATE_STOP);
+	me4000_outl(dev, tmp, info->ai_context.ctrl_reg);
+
+	/* Clear the control register */
+	me4000_outl(dev, 0x0, info->ai_context.ctrl_reg);
+
+	return 0;
+}
+
+static int ai_check_chanlist(comedi_device * dev,
+	comedi_subdevice * s, comedi_cmd * cmd)
+{
+	int aref;
+	int i;
+
+	CALL_PDEBUG("In ai_check_chanlist()\n");
+
+	/* Check whether a channel list is available */
+	if (!cmd->chanlist_len) {
+		printk(KERN_ERR
+			"comedi%d: me4000: ai_check_chanlist(): No channel list available\n",
+			dev->minor);
+		return -EINVAL;
+	}
+
+	/* Check the channel list size */
+	if (cmd->chanlist_len > ME4000_AI_CHANNEL_LIST_COUNT) {
+		printk(KERN_ERR
+			"comedi%d: me4000: ai_check_chanlist(): Channel list is to large\n",
+			dev->minor);
+		return -EINVAL;
+	}
+
+	/* Check the pointer */
+	if (!cmd->chanlist) {
+		printk(KERN_ERR
+			"comedi%d: me4000: ai_check_chanlist(): NULL pointer to channel list\n",
+			dev->minor);
+		return -EFAULT;
+	}
+
+	/* Check whether aref is equal for all entries */
+	aref = CR_AREF(cmd->chanlist[0]);
+	for (i = 0; i < cmd->chanlist_len; i++) {
+		if (CR_AREF(cmd->chanlist[i]) != aref) {
+			printk(KERN_ERR
+				"comedi%d: me4000: ai_check_chanlist(): Mode is not equal for all entries\n",
+				dev->minor);
+			return -EINVAL;
+		}
+	}
+
+	/* Check whether channels are available for this ending */
+	if (aref == SDF_DIFF) {
+		for (i = 0; i < cmd->chanlist_len; i++) {
+			if (CR_CHAN(cmd->chanlist[i]) >=
+				thisboard->ai.diff_count) {
+				printk(KERN_ERR
+					"comedi%d: me4000: ai_check_chanlist(): Channel number to high\n",
+					dev->minor);
+				return -EINVAL;
+			}
+		}
+	} else {
+		for (i = 0; i < cmd->chanlist_len; i++) {
+			if (CR_CHAN(cmd->chanlist[i]) >= thisboard->ai.count) {
+				printk(KERN_ERR
+					"comedi%d: me4000: ai_check_chanlist(): Channel number to high\n",
+					dev->minor);
+				return -EINVAL;
+			}
+		}
+	}
+
+	/* Check if bipolar is set for all entries when in differential mode */
+	if (aref == SDF_DIFF) {
+		for (i = 0; i < cmd->chanlist_len; i++) {
+			if (CR_RANGE(cmd->chanlist[i]) != 1 &&
+				CR_RANGE(cmd->chanlist[i]) != 2) {
+				printk(KERN_ERR
+					"comedi%d: me4000: ai_check_chanlist(): Bipolar is not selected in differential mode\n",
+					dev->minor);
+				return -EINVAL;
+			}
+		}
+	}
+
+	return 0;
+}
+
+static int ai_round_cmd_args(comedi_device * dev,
+	comedi_subdevice * s,
+	comedi_cmd * cmd,
+	unsigned int *init_ticks,
+	unsigned int *scan_ticks, unsigned int *chan_ticks)
+{
+
+	int rest;
+
+	CALL_PDEBUG("In ai_round_cmd_args()\n");
+
+	*init_ticks = 0;
+	*scan_ticks = 0;
+	*chan_ticks = 0;
+
+	PDEBUG("ai_round_cmd_arg(): start_arg = %d\n", cmd->start_arg);
+	PDEBUG("ai_round_cmd_arg(): scan_begin_arg = %d\n",
+		cmd->scan_begin_arg);
+	PDEBUG("ai_round_cmd_arg(): convert_arg = %d\n", cmd->convert_arg);
+
+	if (cmd->start_arg) {
+		*init_ticks = (cmd->start_arg * 33) / 1000;
+		rest = (cmd->start_arg * 33) % 1000;
+
+		if (cmd->flags & TRIG_ROUND_NEAREST) {
+			if (rest > 33) {
+				(*init_ticks)++;
+			}
+		} else if (cmd->flags & TRIG_ROUND_UP) {
+			if (rest)
+				(*init_ticks)++;
+		}
+	}
+
+	if (cmd->scan_begin_arg) {
+		*scan_ticks = (cmd->scan_begin_arg * 33) / 1000;
+		rest = (cmd->scan_begin_arg * 33) % 1000;
+
+		if (cmd->flags & TRIG_ROUND_NEAREST) {
+			if (rest > 33) {
+				(*scan_ticks)++;
+			}
+		} else if (cmd->flags & TRIG_ROUND_UP) {
+			if (rest)
+				(*scan_ticks)++;
+		}
+	}
+
+	if (cmd->convert_arg) {
+		*chan_ticks = (cmd->convert_arg * 33) / 1000;
+		rest = (cmd->convert_arg * 33) % 1000;
+
+		if (cmd->flags & TRIG_ROUND_NEAREST) {
+			if (rest > 33) {
+				(*chan_ticks)++;
+			}
+		} else if (cmd->flags & TRIG_ROUND_UP) {
+			if (rest)
+				(*chan_ticks)++;
+		}
+	}
+
+	PDEBUG("ai_round_cmd_args(): init_ticks = %d\n", *init_ticks);
+	PDEBUG("ai_round_cmd_args(): scan_ticks = %d\n", *scan_ticks);
+	PDEBUG("ai_round_cmd_args(): chan_ticks = %d\n", *chan_ticks);
+
+	return 0;
+}
+
+static void ai_write_timer(comedi_device * dev,
+	unsigned int init_ticks,
+	unsigned int scan_ticks, unsigned int chan_ticks)
+{
+
+	CALL_PDEBUG("In ai_write_timer()\n");
+
+	me4000_outl(dev, init_ticks - 1,
+		info->ai_context.scan_pre_timer_low_reg);
+	me4000_outl(dev, 0x0, info->ai_context.scan_pre_timer_high_reg);
+
+	if (scan_ticks) {
+		me4000_outl(dev, scan_ticks - 1,
+			info->ai_context.scan_timer_low_reg);
+		me4000_outl(dev, 0x0, info->ai_context.scan_timer_high_reg);
+	}
+
+	me4000_outl(dev, chan_ticks - 1, info->ai_context.chan_pre_timer_reg);
+	me4000_outl(dev, chan_ticks - 1, info->ai_context.chan_timer_reg);
+}
+
+static int ai_prepare(comedi_device * dev,
+	comedi_subdevice * s,
+	comedi_cmd * cmd,
+	unsigned int init_ticks,
+	unsigned int scan_ticks, unsigned int chan_ticks)
+{
+
+	unsigned long tmp = 0;
+
+	CALL_PDEBUG("In ai_prepare()\n");
+
+	/* Write timer arguments */
+	ai_write_timer(dev, init_ticks, scan_ticks, chan_ticks);
+
+	/* Reset control register */
+	me4000_outl(dev, tmp, info->ai_context.ctrl_reg);
+
+	/* Start sources */
+	if ((cmd->start_src == TRIG_EXT &&
+			cmd->scan_begin_src == TRIG_TIMER &&
+			cmd->convert_src == TRIG_TIMER) ||
+		(cmd->start_src == TRIG_EXT &&
+			cmd->scan_begin_src == TRIG_FOLLOW &&
+			cmd->convert_src == TRIG_TIMER)) {
+		tmp = ME4000_AI_CTRL_BIT_MODE_1 |
+			ME4000_AI_CTRL_BIT_CHANNEL_FIFO |
+			ME4000_AI_CTRL_BIT_DATA_FIFO;
+	} else if (cmd->start_src == TRIG_EXT &&
+		cmd->scan_begin_src == TRIG_EXT &&
+		cmd->convert_src == TRIG_TIMER) {
+		tmp = ME4000_AI_CTRL_BIT_MODE_2 |
+			ME4000_AI_CTRL_BIT_CHANNEL_FIFO |
+			ME4000_AI_CTRL_BIT_DATA_FIFO;
+	} else if (cmd->start_src == TRIG_EXT &&
+		cmd->scan_begin_src == TRIG_EXT &&
+		cmd->convert_src == TRIG_EXT) {
+		tmp = ME4000_AI_CTRL_BIT_MODE_0 |
+			ME4000_AI_CTRL_BIT_MODE_1 |
+			ME4000_AI_CTRL_BIT_CHANNEL_FIFO |
+			ME4000_AI_CTRL_BIT_DATA_FIFO;
+	} else {
+		tmp = ME4000_AI_CTRL_BIT_MODE_0 |
+			ME4000_AI_CTRL_BIT_CHANNEL_FIFO |
+			ME4000_AI_CTRL_BIT_DATA_FIFO;
+	}
+
+	/* Stop triggers */
+	if (cmd->stop_src == TRIG_COUNT) {
+		me4000_outl(dev, cmd->chanlist_len * cmd->stop_arg,
+			info->ai_context.sample_counter_reg);
+		tmp |= ME4000_AI_CTRL_BIT_HF_IRQ | ME4000_AI_CTRL_BIT_SC_IRQ;
+	} else if (cmd->stop_src == TRIG_NONE &&
+		cmd->scan_end_src == TRIG_COUNT) {
+		me4000_outl(dev, cmd->scan_end_arg,
+			info->ai_context.sample_counter_reg);
+		tmp |= ME4000_AI_CTRL_BIT_HF_IRQ | ME4000_AI_CTRL_BIT_SC_IRQ;
+	} else {
+		tmp |= ME4000_AI_CTRL_BIT_HF_IRQ;
+	}
+
+	/* Write the setup to the control register */
+	me4000_outl(dev, tmp, info->ai_context.ctrl_reg);
+
+	/* Write the channel list */
+	ai_write_chanlist(dev, s, cmd);
+
+	return 0;
+}
+
+static int ai_write_chanlist(comedi_device * dev,
+	comedi_subdevice * s, comedi_cmd * cmd)
+{
+	unsigned int entry;
+	unsigned int chan;
+	unsigned int rang;
+	unsigned int aref;
+	int i;
+
+	CALL_PDEBUG("In ai_write_chanlist()\n");
+
+	for (i = 0; i < cmd->chanlist_len; i++) {
+		chan = CR_CHAN(cmd->chanlist[i]);
+		rang = CR_RANGE(cmd->chanlist[i]);
+		aref = CR_AREF(cmd->chanlist[i]);
+
+		entry = chan;
+
+		if (rang == 0) {
+			entry |= ME4000_AI_LIST_RANGE_UNIPOLAR_2_5;
+		} else if (rang == 1) {
+			entry |= ME4000_AI_LIST_RANGE_UNIPOLAR_10;
+		} else if (rang == 2) {
+			entry |= ME4000_AI_LIST_RANGE_BIPOLAR_2_5;
+		} else {
+			entry |= ME4000_AI_LIST_RANGE_BIPOLAR_10;
+		}
+
+		if (aref == SDF_DIFF) {
+			entry |= ME4000_AI_LIST_INPUT_DIFFERENTIAL;
+		} else {
+			entry |= ME4000_AI_LIST_INPUT_SINGLE_ENDED;
+		}
+
+		me4000_outl(dev, entry, info->ai_context.channel_list_reg);
+	}
+
+	return 0;
+}
+
+static int me4000_ai_do_cmd(comedi_device * dev, comedi_subdevice * s)
+{
+	int err;
+	unsigned int init_ticks = 0;
+	unsigned int scan_ticks = 0;
+	unsigned int chan_ticks = 0;
+	comedi_cmd *cmd = &s->async->cmd;
+
+	CALL_PDEBUG("In me4000_ai_do_cmd()\n");
+
+	/* Reset the analog input */
+	err = me4000_ai_cancel(dev, s);
+	if (err)
+		return err;
+
+	/* Round the timer arguments */
+	err = ai_round_cmd_args(dev,
+		s, cmd, &init_ticks, &scan_ticks, &chan_ticks);
+	if (err)
+		return err;
+
+	/* Prepare the AI for acquisition */
+	err = ai_prepare(dev, s, cmd, init_ticks, scan_ticks, chan_ticks);
+	if (err)
+		return err;
+
+	/* Start acquistion by dummy read */
+	me4000_inl(dev, info->ai_context.start_reg);
+
+	return 0;
+}
+
+/*
+ * me4000_ai_do_cmd_test():
+ *
+ * The demo cmd.c in ./comedilib/demo specifies 6 return values:
+ * - success
+ * - invalid source
+ * - source conflict
+ * - invalid argument
+ * - argument conflict
+ * - invalid chanlist
+ * So I tried to adopt this scheme.
+ */
+static int me4000_ai_do_cmd_test(comedi_device * dev,
+	comedi_subdevice * s, comedi_cmd * cmd)
+{
+
+	unsigned int init_ticks;
+	unsigned int chan_ticks;
+	unsigned int scan_ticks;
+	int err = 0;
+
+	CALL_PDEBUG("In me4000_ai_do_cmd_test()\n");
+
+	PDEBUG("me4000_ai_do_cmd_test(): subdev         = %d\n", cmd->subdev);
+	PDEBUG("me4000_ai_do_cmd_test(): flags          = %08X\n", cmd->flags);
+	PDEBUG("me4000_ai_do_cmd_test(): start_src      = %08X\n",
+		cmd->start_src);
+	PDEBUG("me4000_ai_do_cmd_test(): start_arg      = %d\n",
+		cmd->start_arg);
+	PDEBUG("me4000_ai_do_cmd_test(): scan_begin_src = %08X\n",
+		cmd->scan_begin_src);
+	PDEBUG("me4000_ai_do_cmd_test(): scan_begin_arg = %d\n",
+		cmd->scan_begin_arg);
+	PDEBUG("me4000_ai_do_cmd_test(): convert_src    = %08X\n",
+		cmd->convert_src);
+	PDEBUG("me4000_ai_do_cmd_test(): convert_arg    = %d\n",
+		cmd->convert_arg);
+	PDEBUG("me4000_ai_do_cmd_test(): scan_end_src   = %08X\n",
+		cmd->scan_end_src);
+	PDEBUG("me4000_ai_do_cmd_test(): scan_end_arg   = %d\n",
+		cmd->scan_end_arg);
+	PDEBUG("me4000_ai_do_cmd_test(): stop_src       = %08X\n",
+		cmd->stop_src);
+	PDEBUG("me4000_ai_do_cmd_test(): stop_arg       = %d\n", cmd->stop_arg);
+	PDEBUG("me4000_ai_do_cmd_test(): chanlist       = %d\n",
+		(unsigned int)cmd->chanlist);
+	PDEBUG("me4000_ai_do_cmd_test(): chanlist_len   = %d\n",
+		cmd->chanlist_len);
+
+	/* Only rounding flags are implemented */
+	cmd->flags &= TRIG_ROUND_NEAREST | TRIG_ROUND_UP | TRIG_ROUND_DOWN;
+
+	/* Round the timer arguments */
+	ai_round_cmd_args(dev, s, cmd, &init_ticks, &scan_ticks, &chan_ticks);
+
+	/*
+	 * Stage 1. Check if the trigger sources are generally valid.
+	 */
+	switch (cmd->start_src) {
+	case TRIG_NOW:
+	case TRIG_EXT:
+		break;
+	case TRIG_ANY:
+		cmd->start_src &= TRIG_NOW | TRIG_EXT;
+		err++;
+		break;
+	default:
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid start source\n",
+			dev->minor);
+		cmd->start_src = TRIG_NOW;
+		err++;
+	}
+	switch (cmd->scan_begin_src) {
+	case TRIG_FOLLOW:
+	case TRIG_TIMER:
+	case TRIG_EXT:
+		break;
+	case TRIG_ANY:
+		cmd->scan_begin_src &= TRIG_FOLLOW | TRIG_TIMER | TRIG_EXT;
+		err++;
+		break;
+	default:
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid scan begin source\n",
+			dev->minor);
+		cmd->scan_begin_src = TRIG_FOLLOW;
+		err++;
+	}
+	switch (cmd->convert_src) {
+	case TRIG_TIMER:
+	case TRIG_EXT:
+		break;
+	case TRIG_ANY:
+		cmd->convert_src &= TRIG_TIMER | TRIG_EXT;
+		err++;
+		break;
+	default:
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid convert source\n",
+			dev->minor);
+		cmd->convert_src = TRIG_TIMER;
+		err++;
+	}
+	switch (cmd->scan_end_src) {
+	case TRIG_NONE:
+	case TRIG_COUNT:
+		break;
+	case TRIG_ANY:
+		cmd->scan_end_src &= TRIG_NONE | TRIG_COUNT;
+		err++;
+		break;
+	default:
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid scan end source\n",
+			dev->minor);
+		cmd->scan_end_src = TRIG_NONE;
+		err++;
+	}
+	switch (cmd->stop_src) {
+	case TRIG_NONE:
+	case TRIG_COUNT:
+		break;
+	case TRIG_ANY:
+		cmd->stop_src &= TRIG_NONE | TRIG_COUNT;
+		err++;
+		break;
+	default:
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid stop source\n",
+			dev->minor);
+		cmd->stop_src = TRIG_NONE;
+		err++;
+	}
+	if (err) {
+		return 1;
+	}
+
+	/*
+	 * Stage 2. Check for trigger source conflicts.
+	 */
+	if (cmd->start_src == TRIG_NOW &&
+		cmd->scan_begin_src == TRIG_TIMER &&
+		cmd->convert_src == TRIG_TIMER) {
+	} else if (cmd->start_src == TRIG_NOW &&
+		cmd->scan_begin_src == TRIG_FOLLOW &&
+		cmd->convert_src == TRIG_TIMER) {
+	} else if (cmd->start_src == TRIG_EXT &&
+		cmd->scan_begin_src == TRIG_TIMER &&
+		cmd->convert_src == TRIG_TIMER) {
+	} else if (cmd->start_src == TRIG_EXT &&
+		cmd->scan_begin_src == TRIG_FOLLOW &&
+		cmd->convert_src == TRIG_TIMER) {
+	} else if (cmd->start_src == TRIG_EXT &&
+		cmd->scan_begin_src == TRIG_EXT &&
+		cmd->convert_src == TRIG_TIMER) {
+	} else if (cmd->start_src == TRIG_EXT &&
+		cmd->scan_begin_src == TRIG_EXT &&
+		cmd->convert_src == TRIG_EXT) {
+	} else {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid start trigger combination\n",
+			dev->minor);
+		cmd->start_src = TRIG_NOW;
+		cmd->scan_begin_src = TRIG_FOLLOW;
+		cmd->convert_src = TRIG_TIMER;
+		err++;
+	}
+
+	if (cmd->stop_src == TRIG_NONE && cmd->scan_end_src == TRIG_NONE) {
+	} else if (cmd->stop_src == TRIG_COUNT &&
+		cmd->scan_end_src == TRIG_NONE) {
+	} else if (cmd->stop_src == TRIG_NONE &&
+		cmd->scan_end_src == TRIG_COUNT) {
+	} else if (cmd->stop_src == TRIG_COUNT &&
+		cmd->scan_end_src == TRIG_COUNT) {
+	} else {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid stop trigger combination\n",
+			dev->minor);
+		cmd->stop_src = TRIG_NONE;
+		cmd->scan_end_src = TRIG_NONE;
+		err++;
+	}
+	if (err) {
+		return 2;
+	}
+
+	/*
+	 * Stage 3. Check if arguments are generally valid.
+	 */
+	if (cmd->chanlist_len < 1) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ai_do_cmd_test(): No channel list\n",
+			dev->minor);
+		cmd->chanlist_len = 1;
+		err++;
+	}
+	if (init_ticks < 66) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ai_do_cmd_test(): Start arg to low\n",
+			dev->minor);
+		cmd->start_arg = 2000;
+		err++;
+	}
+	if (scan_ticks && scan_ticks < 67) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ai_do_cmd_test(): Scan begin arg to low\n",
+			dev->minor);
+		cmd->scan_begin_arg = 2031;
+		err++;
+	}
+	if (chan_ticks < 66) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ai_do_cmd_test(): Convert arg to low\n",
+			dev->minor);
+		cmd->convert_arg = 2000;
+		err++;
+	}
+	if (err) {
+		return 3;
+	}
+
+	/*
+	 * Stage 4. Check for argument conflicts.
+	 */
+	if (cmd->start_src == TRIG_NOW &&
+		cmd->scan_begin_src == TRIG_TIMER &&
+		cmd->convert_src == TRIG_TIMER) {
+
+		/* Check timer arguments */
+		if (init_ticks < ME4000_AI_MIN_TICKS) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid start arg\n",
+				dev->minor);
+			cmd->start_arg = 2000;	// 66 ticks at least
+			err++;
+		}
+		if (chan_ticks < ME4000_AI_MIN_TICKS) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid convert arg\n",
+				dev->minor);
+			cmd->convert_arg = 2000;	// 66 ticks at least
+			err++;
+		}
+		if (scan_ticks <= cmd->chanlist_len * chan_ticks) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid scan end arg\n",
+				dev->minor);
+			cmd->scan_end_arg = 2000 * cmd->chanlist_len + 31;	// At least one tick more
+			err++;
+		}
+	} else if (cmd->start_src == TRIG_NOW &&
+		cmd->scan_begin_src == TRIG_FOLLOW &&
+		cmd->convert_src == TRIG_TIMER) {
+
+		/* Check timer arguments */
+		if (init_ticks < ME4000_AI_MIN_TICKS) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid start arg\n",
+				dev->minor);
+			cmd->start_arg = 2000;	// 66 ticks at least
+			err++;
+		}
+		if (chan_ticks < ME4000_AI_MIN_TICKS) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid convert arg\n",
+				dev->minor);
+			cmd->convert_arg = 2000;	// 66 ticks at least
+			err++;
+		}
+	} else if (cmd->start_src == TRIG_EXT &&
+		cmd->scan_begin_src == TRIG_TIMER &&
+		cmd->convert_src == TRIG_TIMER) {
+
+		/* Check timer arguments */
+		if (init_ticks < ME4000_AI_MIN_TICKS) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid start arg\n",
+				dev->minor);
+			cmd->start_arg = 2000;	// 66 ticks at least
+			err++;
+		}
+		if (chan_ticks < ME4000_AI_MIN_TICKS) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid convert arg\n",
+				dev->minor);
+			cmd->convert_arg = 2000;	// 66 ticks at least
+			err++;
+		}
+		if (scan_ticks <= cmd->chanlist_len * chan_ticks) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid scan end arg\n",
+				dev->minor);
+			cmd->scan_end_arg = 2000 * cmd->chanlist_len + 31;	// At least one tick more
+			err++;
+		}
+	} else if (cmd->start_src == TRIG_EXT &&
+		cmd->scan_begin_src == TRIG_FOLLOW &&
+		cmd->convert_src == TRIG_TIMER) {
+
+		/* Check timer arguments */
+		if (init_ticks < ME4000_AI_MIN_TICKS) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid start arg\n",
+				dev->minor);
+			cmd->start_arg = 2000;	// 66 ticks at least
+			err++;
+		}
+		if (chan_ticks < ME4000_AI_MIN_TICKS) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid convert arg\n",
+				dev->minor);
+			cmd->convert_arg = 2000;	// 66 ticks at least
+			err++;
+		}
+	} else if (cmd->start_src == TRIG_EXT &&
+		cmd->scan_begin_src == TRIG_EXT &&
+		cmd->convert_src == TRIG_TIMER) {
+
+		/* Check timer arguments */
+		if (init_ticks < ME4000_AI_MIN_TICKS) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid start arg\n",
+				dev->minor);
+			cmd->start_arg = 2000;	// 66 ticks at least
+			err++;
+		}
+		if (chan_ticks < ME4000_AI_MIN_TICKS) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid convert arg\n",
+				dev->minor);
+			cmd->convert_arg = 2000;	// 66 ticks at least
+			err++;
+		}
+	} else if (cmd->start_src == TRIG_EXT &&
+		cmd->scan_begin_src == TRIG_EXT &&
+		cmd->convert_src == TRIG_EXT) {
+
+		/* Check timer arguments */
+		if (init_ticks < ME4000_AI_MIN_TICKS) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid start arg\n",
+				dev->minor);
+			cmd->start_arg = 2000;	// 66 ticks at least
+			err++;
+		}
+	}
+	if (cmd->stop_src == TRIG_COUNT) {
+		if (cmd->stop_arg == 0) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid stop arg\n",
+				dev->minor);
+			cmd->stop_arg = 1;
+			err++;
+		}
+	}
+	if (cmd->scan_end_src == TRIG_COUNT) {
+		if (cmd->scan_end_arg == 0) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_do_cmd_test(): Invalid scan end arg\n",
+				dev->minor);
+			cmd->scan_end_arg = 1;
+			err++;
+		}
+	}
+	if (err) {
+		return 4;
+	}
+
+	/*
+	 * Stage 5. Check the channel list.
+	 */
+	if (ai_check_chanlist(dev, s, cmd))
+		return 5;
+
+	return 0;
+}
+
+static irqreturn_t me4000_ai_isr(int irq, void *dev_id PT_REGS_ARG)
+{
+	unsigned int tmp;
+	comedi_device *dev = dev_id;
+	comedi_subdevice *s = dev->subdevices;
+	me4000_ai_context_t *ai_context = &info->ai_context;
+	int i;
+	int c = 0;
+	long lval;
+
+	ISR_PDEBUG("me4000_ai_isr() is executed\n");
+
+	if (!dev->attached) {
+		ISR_PDEBUG("me4000_ai_isr() premature interrupt\n");
+		return IRQ_NONE;
+	}
+
+	/* Reset all events */
+	s->async->events = 0;
+
+	/* Check if irq number is right */
+	if (irq != ai_context->irq) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ai_isr(): Incorrect interrupt num: %d\n",
+			dev->minor, irq);
+		return IRQ_HANDLED;
+	}
+
+	if (me4000_inl(dev,
+			ai_context->
+			irq_status_reg) & ME4000_IRQ_STATUS_BIT_AI_HF) {
+		ISR_PDEBUG
+			("me4000_ai_isr(): Fifo half full interrupt occured\n");
+
+		/* Read status register to find out what happened */
+		tmp = me4000_inl(dev, ai_context->ctrl_reg);
+
+		if (!(tmp & ME4000_AI_STATUS_BIT_FF_DATA) &&
+			!(tmp & ME4000_AI_STATUS_BIT_HF_DATA) &&
+			(tmp & ME4000_AI_STATUS_BIT_EF_DATA)) {
+			ISR_PDEBUG("me4000_ai_isr(): Fifo full\n");
+			c = ME4000_AI_FIFO_COUNT;
+
+			/* FIFO overflow, so stop conversion and disable all interrupts */
+			tmp |= ME4000_AI_CTRL_BIT_IMMEDIATE_STOP;
+			tmp &= ~(ME4000_AI_CTRL_BIT_HF_IRQ |
+				ME4000_AI_CTRL_BIT_SC_IRQ);
+			me4000_outl(dev, tmp, ai_context->ctrl_reg);
+
+			s->async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
+
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_isr(): FIFO overflow\n",
+				dev->minor);
+		} else if ((tmp & ME4000_AI_STATUS_BIT_FF_DATA)
+			&& !(tmp & ME4000_AI_STATUS_BIT_HF_DATA)
+			&& (tmp & ME4000_AI_STATUS_BIT_EF_DATA)) {
+			ISR_PDEBUG("me4000_ai_isr(): Fifo half full\n");
+
+			s->async->events |= COMEDI_CB_BLOCK;
+
+			c = ME4000_AI_FIFO_COUNT / 2;
+		} else {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_isr(): Can't determine state of fifo\n",
+				dev->minor);
+			c = 0;
+
+			/* Undefined state, so stop conversion and disable all interrupts */
+			tmp |= ME4000_AI_CTRL_BIT_IMMEDIATE_STOP;
+			tmp &= ~(ME4000_AI_CTRL_BIT_HF_IRQ |
+				ME4000_AI_CTRL_BIT_SC_IRQ);
+			me4000_outl(dev, tmp, ai_context->ctrl_reg);
+
+			s->async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
+
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_ai_isr(): Undefined FIFO state\n",
+				dev->minor);
+		}
+
+		ISR_PDEBUG("me4000_ai_isr(): Try to read %d values\n", c);
+
+		for (i = 0; i < c; i++) {
+			/* Read value from data fifo */
+			lval = inl(ai_context->data_reg) & 0xFFFF;
+			lval ^= 0x8000;
+
+			if (!comedi_buf_put(s->async, lval)) {
+				/* Buffer overflow, so stop conversion and disable all interrupts */
+				tmp |= ME4000_AI_CTRL_BIT_IMMEDIATE_STOP;
+				tmp &= ~(ME4000_AI_CTRL_BIT_HF_IRQ |
+					ME4000_AI_CTRL_BIT_SC_IRQ);
+				me4000_outl(dev, tmp, ai_context->ctrl_reg);
+
+				s->async->events |= COMEDI_CB_OVERFLOW;
+
+				printk(KERN_ERR
+					"comedi%d: me4000: me4000_ai_isr(): Buffer overflow\n",
+					dev->minor);
+
+				break;
+			}
+		}
+
+		/* Work is done, so reset the interrupt */
+		ISR_PDEBUG("me4000_ai_isr(): Reset fifo half full interrupt\n");
+		tmp |= ME4000_AI_CTRL_BIT_HF_IRQ_RESET;
+		me4000_outl(dev, tmp, ai_context->ctrl_reg);
+		tmp &= ~ME4000_AI_CTRL_BIT_HF_IRQ_RESET;
+		me4000_outl(dev, tmp, ai_context->ctrl_reg);
+	}
+
+	if (me4000_inl(dev,
+			ai_context->
+			irq_status_reg) & ME4000_IRQ_STATUS_BIT_SC) {
+		ISR_PDEBUG
+			("me4000_ai_isr(): Sample counter interrupt occured\n");
+
+		s->async->events |= COMEDI_CB_BLOCK | COMEDI_CB_EOA;
+
+		/* Acquisition is complete, so stop conversion and disable all interrupts */
+		tmp = me4000_inl(dev, ai_context->ctrl_reg);
+		tmp |= ME4000_AI_CTRL_BIT_IMMEDIATE_STOP;
+		tmp &= ~(ME4000_AI_CTRL_BIT_HF_IRQ | ME4000_AI_CTRL_BIT_SC_IRQ);
+		me4000_outl(dev, tmp, ai_context->ctrl_reg);
+
+		/* Poll data until fifo empty */
+		while (inl(ai_context->ctrl_reg) & ME4000_AI_STATUS_BIT_EF_DATA) {
+			/* Read value from data fifo */
+			lval = inl(ai_context->data_reg) & 0xFFFF;
+			lval ^= 0x8000;
+
+			if (!comedi_buf_put(s->async, lval)) {
+				printk(KERN_ERR
+					"comedi%d: me4000: me4000_ai_isr(): Buffer overflow\n",
+					dev->minor);
+				s->async->events |= COMEDI_CB_OVERFLOW;
+				break;
+			}
+		}
+
+		/* Work is done, so reset the interrupt */
+		ISR_PDEBUG
+			("me4000_ai_isr(): Reset interrupt from sample counter\n");
+		tmp |= ME4000_AI_CTRL_BIT_SC_IRQ_RESET;
+		me4000_outl(dev, tmp, ai_context->ctrl_reg);
+		tmp &= ~ME4000_AI_CTRL_BIT_SC_IRQ_RESET;
+		me4000_outl(dev, tmp, ai_context->ctrl_reg);
+	}
+
+	ISR_PDEBUG("me4000_ai_isr(): Events = 0x%X\n", s->async->events);
+
+	if (s->async->events)
+		comedi_event(dev, s);
+
+	return IRQ_HANDLED;
+}
+
+/*=============================================================================
+  Analog output section
+  ===========================================================================*/
+
+static int me4000_ao_insn_write(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data)
+{
+
+	int chan = CR_CHAN(insn->chanspec);
+	int rang = CR_RANGE(insn->chanspec);
+	int aref = CR_AREF(insn->chanspec);
+	unsigned long tmp;
+
+	CALL_PDEBUG("In me4000_ao_insn_write()\n");
+
+	if (insn->n == 0) {
+		return 0;
+	} else if (insn->n > 1) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ao_insn_write(): Invalid instruction length %d\n",
+			dev->minor, insn->n);
+		return -EINVAL;
+	}
+
+	if (chan >= thisboard->ao.count) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ao_insn_write(): Invalid channel %d\n",
+			dev->minor, insn->n);
+		return -EINVAL;
+	}
+
+	if (rang != 0) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ao_insn_write(): Invalid range %d\n",
+			dev->minor, insn->n);
+		return -EINVAL;
+	}
+
+	if (aref != AREF_GROUND && aref != AREF_COMMON) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_ao_insn_write(): Invalid aref %d\n",
+			dev->minor, insn->n);
+		return -EINVAL;
+	}
+
+	/* Stop any running conversion */
+	tmp = me4000_inl(dev, info->ao_context[chan].ctrl_reg);
+	tmp |= ME4000_AO_CTRL_BIT_IMMEDIATE_STOP;
+	me4000_outl(dev, tmp, info->ao_context[chan].ctrl_reg);
+
+	/* Clear control register and set to single mode */
+	me4000_outl(dev, 0x0, info->ao_context[chan].ctrl_reg);
+
+	/* Write data value */
+	me4000_outl(dev, data[0], info->ao_context[chan].single_reg);
+
+	/* Store in the mirror */
+	info->ao_context[chan].mirror = data[0];
+
+	return 1;
+}
+
+static int me4000_ao_insn_read(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data)
+{
+	int chan = CR_CHAN(insn->chanspec);
+
+	if (insn->n == 0) {
+		return 0;
+	} else if (insn->n > 1) {
+		printk("comedi%d: me4000: me4000_ao_insn_read(): Invalid instruction length\n", dev->minor);
+		return -EINVAL;
+	}
+
+	data[0] = info->ao_context[chan].mirror;
+
+	return 1;
+}
+
+/*=============================================================================
+  Digital I/O section
+  ===========================================================================*/
+
+static int me4000_dio_insn_bits(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data)
+{
+
+	CALL_PDEBUG("In me4000_dio_insn_bits()\n");
+
+	/* Length of data must be 2 (mask and new data, see below) */
+	if (insn->n == 0) {
+		return 0;
+	}
+	if (insn->n != 2) {
+		printk("comedi%d: me4000: me4000_dio_insn_bits(): Invalid instruction length\n", dev->minor);
+		return -EINVAL;
+	}
+
+	/*
+	 * The insn data consists of a mask in data[0] and the new data
+	 * in data[1]. The mask defines which bits we are concerning about.
+	 * The new data must be anded with the mask.
+	 * Each channel corresponds to a bit.
+	 */
+	if (data[0]) {
+		/* Check if requested ports are configured for output */
+		if ((s->io_bits & data[0]) != data[0])
+			return -EIO;
+
+		s->state &= ~data[0];
+		s->state |= data[0] & data[1];
+
+		/* Write out the new digital output lines */
+		me4000_outl(dev, (s->state >> 0) & 0xFF,
+			info->dio_context.port_0_reg);
+		me4000_outl(dev, (s->state >> 8) & 0xFF,
+			info->dio_context.port_1_reg);
+		me4000_outl(dev, (s->state >> 16) & 0xFF,
+			info->dio_context.port_2_reg);
+		me4000_outl(dev, (s->state >> 24) & 0xFF,
+			info->dio_context.port_3_reg);
+	}
+
+	/* On return, data[1] contains the value of
+	   the digital input and output lines. */
+	data[1] =
+		((me4000_inl(dev, info->dio_context.port_0_reg) & 0xFF) << 0) |
+		((me4000_inl(dev, info->dio_context.port_1_reg) & 0xFF) << 8) |
+		((me4000_inl(dev, info->dio_context.port_2_reg) & 0xFF) << 16) |
+		((me4000_inl(dev, info->dio_context.port_3_reg) & 0xFF) << 24);
+
+	return 2;
+}
+
+static int me4000_dio_insn_config(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data)
+{
+	unsigned long tmp;
+	int chan = CR_CHAN(insn->chanspec);
+
+	CALL_PDEBUG("In me4000_dio_insn_config()\n");
+
+	if (data[0] == INSN_CONFIG_DIO_QUERY) {
+		data[1] =
+			(s->
+			io_bits & (1 << chan)) ? COMEDI_OUTPUT : COMEDI_INPUT;
+		return insn->n;
+	}
+
+	/*
+	 * The input or output configuration of each digital line is
+	 * configured by a special insn_config instruction.  chanspec
+	 * contains the channel to be changed, and data[0] contains the
+	 * value COMEDI_INPUT or COMEDI_OUTPUT.
+	 * On the ME-4000 it is only possible to switch port wise (8 bit)
+	 */
+
+	tmp = me4000_inl(dev, info->dio_context.ctrl_reg);
+
+	if (data[0] == COMEDI_OUTPUT) {
+		if (chan < 8) {
+			s->io_bits |= 0xFF;
+			tmp &= ~(ME4000_DIO_CTRL_BIT_MODE_0 |
+				ME4000_DIO_CTRL_BIT_MODE_1);
+			tmp |= ME4000_DIO_CTRL_BIT_MODE_0;
+		} else if (chan < 16) {
+			/*
+			 * Chech for optoisolated ME-4000 version. If one the first
+			 * port is a fixed output port and the second is a fixed input port.
+			 */
+			if (!me4000_inl(dev, info->dio_context.dir_reg))
+				return -ENODEV;
+
+			s->io_bits |= 0xFF00;
+			tmp &= ~(ME4000_DIO_CTRL_BIT_MODE_2 |
+				ME4000_DIO_CTRL_BIT_MODE_3);
+			tmp |= ME4000_DIO_CTRL_BIT_MODE_2;
+		} else if (chan < 24) {
+			s->io_bits |= 0xFF0000;
+			tmp &= ~(ME4000_DIO_CTRL_BIT_MODE_4 |
+				ME4000_DIO_CTRL_BIT_MODE_5);
+			tmp |= ME4000_DIO_CTRL_BIT_MODE_4;
+		} else if (chan < 32) {
+			s->io_bits |= 0xFF000000;
+			tmp &= ~(ME4000_DIO_CTRL_BIT_MODE_6 |
+				ME4000_DIO_CTRL_BIT_MODE_7);
+			tmp |= ME4000_DIO_CTRL_BIT_MODE_6;
+		} else {
+			return -EINVAL;
+		}
+	} else {
+		if (chan < 8) {
+			/*
+			 * Chech for optoisolated ME-4000 version. If one the first
+			 * port is a fixed output port and the second is a fixed input port.
+			 */
+			if (!me4000_inl(dev, info->dio_context.dir_reg))
+				return -ENODEV;
+
+			s->io_bits &= ~0xFF;
+			tmp &= ~(ME4000_DIO_CTRL_BIT_MODE_0 |
+				ME4000_DIO_CTRL_BIT_MODE_1);
+		} else if (chan < 16) {
+			s->io_bits &= ~0xFF00;
+			tmp &= ~(ME4000_DIO_CTRL_BIT_MODE_2 |
+				ME4000_DIO_CTRL_BIT_MODE_3);
+		} else if (chan < 24) {
+			s->io_bits &= ~0xFF0000;
+			tmp &= ~(ME4000_DIO_CTRL_BIT_MODE_4 |
+				ME4000_DIO_CTRL_BIT_MODE_5);
+		} else if (chan < 32) {
+			s->io_bits &= ~0xFF000000;
+			tmp &= ~(ME4000_DIO_CTRL_BIT_MODE_6 |
+				ME4000_DIO_CTRL_BIT_MODE_7);
+		} else {
+			return -EINVAL;
+		}
+	}
+
+	me4000_outl(dev, tmp, info->dio_context.ctrl_reg);
+
+	return 1;
+}
+
+/*=============================================================================
+  Counter section
+  ===========================================================================*/
+
+static int cnt_reset(comedi_device * dev, unsigned int channel)
+{
+
+	CALL_PDEBUG("In cnt_reset()\n");
+
+	switch (channel) {
+	case 0:
+		me4000_outb(dev, 0x30, info->cnt_context.ctrl_reg);
+		me4000_outb(dev, 0x00, info->cnt_context.counter_0_reg);
+		me4000_outb(dev, 0x00, info->cnt_context.counter_0_reg);
+		break;
+	case 1:
+		me4000_outb(dev, 0x70, info->cnt_context.ctrl_reg);
+		me4000_outb(dev, 0x00, info->cnt_context.counter_1_reg);
+		me4000_outb(dev, 0x00, info->cnt_context.counter_1_reg);
+		break;
+	case 2:
+		me4000_outb(dev, 0xB0, info->cnt_context.ctrl_reg);
+		me4000_outb(dev, 0x00, info->cnt_context.counter_2_reg);
+		me4000_outb(dev, 0x00, info->cnt_context.counter_2_reg);
+		break;
+	default:
+		printk(KERN_ERR
+			"comedi%d: me4000: cnt_reset(): Invalid channel\n",
+			dev->minor);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int cnt_config(comedi_device * dev, unsigned int channel,
+	unsigned int mode)
+{
+	int tmp = 0;
+
+	CALL_PDEBUG("In cnt_config()\n");
+
+	switch (channel) {
+	case 0:
+		tmp |= ME4000_CNT_COUNTER_0;
+		break;
+	case 1:
+		tmp |= ME4000_CNT_COUNTER_1;
+		break;
+	case 2:
+		tmp |= ME4000_CNT_COUNTER_2;
+		break;
+	default:
+		printk(KERN_ERR
+			"comedi%d: me4000: cnt_config(): Invalid channel\n",
+			dev->minor);
+		return -EINVAL;
+	}
+
+	switch (mode) {
+	case 0:
+		tmp |= ME4000_CNT_MODE_0;
+		break;
+	case 1:
+		tmp |= ME4000_CNT_MODE_1;
+		break;
+	case 2:
+		tmp |= ME4000_CNT_MODE_2;
+		break;
+	case 3:
+		tmp |= ME4000_CNT_MODE_3;
+		break;
+	case 4:
+		tmp |= ME4000_CNT_MODE_4;
+		break;
+	case 5:
+		tmp |= ME4000_CNT_MODE_5;
+		break;
+	default:
+		printk(KERN_ERR
+			"comedi%d: me4000: cnt_config(): Invalid counter mode\n",
+			dev->minor);
+		return -EINVAL;
+	}
+
+	/* Write the control word */
+	tmp |= 0x30;
+	me4000_outb(dev, tmp, info->cnt_context.ctrl_reg);
+
+	return 0;
+}
+
+static int me4000_cnt_insn_config(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data)
+{
+
+	int err;
+
+	CALL_PDEBUG("In me4000_cnt_insn_config()\n");
+
+	switch (data[0]) {
+	case GPCT_RESET:
+		if (insn->n != 1) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_cnt_insn_config(): Invalid instruction length%d\n",
+				dev->minor, insn->n);
+			return -EINVAL;
+		}
+
+		err = cnt_reset(dev, insn->chanspec);
+		if (err)
+			return err;
+		break;
+	case GPCT_SET_OPERATION:
+		if (insn->n != 2) {
+			printk(KERN_ERR
+				"comedi%d: me4000: me4000_cnt_insn_config(): Invalid instruction length%d\n",
+				dev->minor, insn->n);
+			return -EINVAL;
+		}
+
+		err = cnt_config(dev, insn->chanspec, data[1]);
+		if (err)
+			return err;
+		break;
+	default:
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_cnt_insn_config(): Invalid instruction\n",
+			dev->minor);
+		return -EINVAL;
+	}
+
+	return 2;
+}
+
+static int me4000_cnt_insn_read(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data)
+{
+
+	unsigned short tmp;
+
+	CALL_PDEBUG("In me4000_cnt_insn_read()\n");
+
+	if (insn->n == 0) {
+		return 0;
+	}
+	if (insn->n > 1) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_cnt_insn_read(): Invalid instruction length %d\n",
+			dev->minor, insn->n);
+		return -EINVAL;
+	}
+
+	switch (insn->chanspec) {
+	case 0:
+		tmp = me4000_inb(dev, info->cnt_context.counter_0_reg);
+		data[0] = tmp;
+		tmp = me4000_inb(dev, info->cnt_context.counter_0_reg);
+		data[0] |= tmp << 8;
+		break;
+	case 1:
+		tmp = me4000_inb(dev, info->cnt_context.counter_1_reg);
+		data[0] = tmp;
+		tmp = me4000_inb(dev, info->cnt_context.counter_1_reg);
+		data[0] |= tmp << 8;
+		break;
+	case 2:
+		tmp = me4000_inb(dev, info->cnt_context.counter_2_reg);
+		data[0] = tmp;
+		tmp = me4000_inb(dev, info->cnt_context.counter_2_reg);
+		data[0] |= tmp << 8;
+		break;
+	default:
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_cnt_insn_read(): Invalid channel %d\n",
+			dev->minor, insn->chanspec);
+		return -EINVAL;
+	}
+
+	return 1;
+}
+
+static int me4000_cnt_insn_write(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data)
+{
+
+	unsigned short tmp;
+
+	CALL_PDEBUG("In me4000_cnt_insn_write()\n");
+
+	if (insn->n == 0) {
+		return 0;
+	} else if (insn->n > 1) {
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_cnt_insn_write(): Invalid instruction length %d\n",
+			dev->minor, insn->n);
+		return -EINVAL;
+	}
+
+	switch (insn->chanspec) {
+	case 0:
+		tmp = data[0] & 0xFF;
+		me4000_outb(dev, tmp, info->cnt_context.counter_0_reg);
+		tmp = (data[0] >> 8) & 0xFF;
+		me4000_outb(dev, tmp, info->cnt_context.counter_0_reg);
+		break;
+	case 1:
+		tmp = data[0] & 0xFF;
+		me4000_outb(dev, tmp, info->cnt_context.counter_1_reg);
+		tmp = (data[0] >> 8) & 0xFF;
+		me4000_outb(dev, tmp, info->cnt_context.counter_1_reg);
+		break;
+	case 2:
+		tmp = data[0] & 0xFF;
+		me4000_outb(dev, tmp, info->cnt_context.counter_2_reg);
+		tmp = (data[0] >> 8) & 0xFF;
+		me4000_outb(dev, tmp, info->cnt_context.counter_2_reg);
+		break;
+	default:
+		printk(KERN_ERR
+			"comedi%d: me4000: me4000_cnt_insn_write(): Invalid channel %d\n",
+			dev->minor, insn->chanspec);
+		return -EINVAL;
+	}
+
+	return 1;
+}
+
+COMEDI_PCI_INITCLEANUP(driver_me4000, me4000_pci_table);
diff --git a/drivers/staging/comedi/drivers/me4000.h b/drivers/staging/comedi/drivers/me4000.h
new file mode 100644
index 00000000000..f12b8873ec3
--- /dev/null
+++ b/drivers/staging/comedi/drivers/me4000.h
@@ -0,0 +1,446 @@
+/*
+    me4000.h
+    Register descriptions and defines for the ME-4000 board family
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1998-9 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#ifndef _ME4000_H_
+#define _ME4000_H_
+
+/*=============================================================================
+  Debug section
+  ===========================================================================*/
+
+#undef ME4000_CALL_DEBUG	// Debug function entry and exit
+#undef ME4000_PORT_DEBUG	// Debug port access
+#undef ME4000_ISR_DEBUG		// Debug the interrupt service routine
+#undef ME4000_DEBUG		// General purpose debug masseges
+
+#ifdef ME4000_CALL_DEBUG
+#undef CALL_PDEBUG
+#define CALL_PDEBUG(fmt, args...) printk(KERN_DEBUG"comedi%d: me4000: " fmt, dev->minor, ##args)
+#else
+# define CALL_PDEBUG(fmt, args...)	// no debugging, do nothing
+#endif
+
+#ifdef ME4000_PORT_DEBUG
+#undef PORT_PDEBUG
+#define PORT_PDEBUG(fmt, args...) printk(KERN_DEBUG"comedi%d: me4000: " fmt, dev->minor,  ##args)
+#else
+#define PORT_PDEBUG(fmt, args...)	// no debugging, do nothing
+#endif
+
+#ifdef ME4000_ISR_DEBUG
+#undef ISR_PDEBUG
+#define ISR_PDEBUG(fmt, args...) printk(KERN_DEBUG"comedi%d: me4000: " fmt, dev->minor,  ##args)
+#else
+#define ISR_PDEBUG(fmt, args...)	// no debugging, do nothing
+#endif
+
+#ifdef ME4000_DEBUG
+#undef PDEBUG
+#define PDEBUG(fmt, args...) printk(KERN_DEBUG"comedi%d: me4000: " fmt, dev->minor,  ##args)
+#else
+#define PDEBUG(fmt, args...)	// no debugging, do nothing
+#endif
+
+/*=============================================================================
+  PCI vendor and device IDs
+  ===========================================================================*/
+
+#define PCI_VENDOR_ID_MEILHAUS 0x1402
+
+#define PCI_DEVICE_ID_MEILHAUS_ME4650	0x4650	// Low Cost version
+
+#define PCI_DEVICE_ID_MEILHAUS_ME4660	0x4660	// Standard version
+#define PCI_DEVICE_ID_MEILHAUS_ME4660I	0x4661	// Isolated version
+#define PCI_DEVICE_ID_MEILHAUS_ME4660S	0x4662	// Standard version with Sample and Hold
+#define PCI_DEVICE_ID_MEILHAUS_ME4660IS	0x4663	// Isolated version with Sample and Hold
+
+#define PCI_DEVICE_ID_MEILHAUS_ME4670	0x4670	// Standard version
+#define PCI_DEVICE_ID_MEILHAUS_ME4670I	0x4671	// Isolated version
+#define PCI_DEVICE_ID_MEILHAUS_ME4670S	0x4672	// Standard version with Sample and Hold
+#define PCI_DEVICE_ID_MEILHAUS_ME4670IS	0x4673	// Isolated version with Sample and Hold
+
+#define PCI_DEVICE_ID_MEILHAUS_ME4680	0x4680	// Standard version
+#define PCI_DEVICE_ID_MEILHAUS_ME4680I	0x4681	// Isolated version
+#define PCI_DEVICE_ID_MEILHAUS_ME4680S	0x4682	// Standard version with Sample and Hold
+#define PCI_DEVICE_ID_MEILHAUS_ME4680IS	0x4683	// Isolated version with Sample and Hold
+
+/*=============================================================================
+  ME-4000 base register offsets
+  ===========================================================================*/
+
+#define ME4000_AO_00_CTRL_REG			0x00	// R/W
+#define ME4000_AO_00_STATUS_REG			0x04	// R/_
+#define ME4000_AO_00_FIFO_REG			0x08	// _/W
+#define ME4000_AO_00_SINGLE_REG			0x0C	// R/W
+#define ME4000_AO_00_TIMER_REG			0x10	// _/W
+
+#define ME4000_AO_01_CTRL_REG			0x18	// R/W
+#define ME4000_AO_01_STATUS_REG			0x1C	// R/_
+#define ME4000_AO_01_FIFO_REG			0x20	// _/W
+#define ME4000_AO_01_SINGLE_REG			0x24	// R/W
+#define ME4000_AO_01_TIMER_REG			0x28	// _/W
+
+#define ME4000_AO_02_CTRL_REG			0x30	// R/W
+#define ME4000_AO_02_STATUS_REG			0x34	// R/_
+#define ME4000_AO_02_FIFO_REG			0x38	// _/W
+#define ME4000_AO_02_SINGLE_REG			0x3C	// R/W
+#define ME4000_AO_02_TIMER_REG			0x40	// _/W
+
+#define ME4000_AO_03_CTRL_REG			0x48	// R/W
+#define ME4000_AO_03_STATUS_REG			0x4C	// R/_
+#define ME4000_AO_03_FIFO_REG			0x50	// _/W
+#define ME4000_AO_03_SINGLE_REG			0x54	// R/W
+#define ME4000_AO_03_TIMER_REG			0x58	// _/W
+
+#define ME4000_AI_CTRL_REG			0x74	// _/W
+#define ME4000_AI_STATUS_REG			0x74	// R/_
+#define ME4000_AI_CHANNEL_LIST_REG		0x78	// _/W
+#define ME4000_AI_DATA_REG			0x7C	// R/_
+#define ME4000_AI_CHAN_TIMER_REG		0x80	// _/W
+#define ME4000_AI_CHAN_PRE_TIMER_REG		0x84	// _/W
+#define ME4000_AI_SCAN_TIMER_LOW_REG		0x88	// _/W
+#define ME4000_AI_SCAN_TIMER_HIGH_REG		0x8C	// _/W
+#define ME4000_AI_SCAN_PRE_TIMER_LOW_REG	0x90	// _/W
+#define ME4000_AI_SCAN_PRE_TIMER_HIGH_REG	0x94	// _/W
+#define ME4000_AI_START_REG			0x98	// R/_
+
+#define ME4000_IRQ_STATUS_REG			0x9C	// R/_
+
+#define ME4000_DIO_PORT_0_REG			0xA0	// R/W
+#define ME4000_DIO_PORT_1_REG			0xA4	// R/W
+#define ME4000_DIO_PORT_2_REG			0xA8	// R/W
+#define ME4000_DIO_PORT_3_REG			0xAC	// R/W
+#define ME4000_DIO_DIR_REG			0xB0	// R/W
+
+#define ME4000_AO_LOADSETREG_XX			0xB4	// R/W
+
+#define ME4000_DIO_CTRL_REG			0xB8	// R/W
+
+#define ME4000_AO_DEMUX_ADJUST_REG		0xBC	// -/W
+
+#define ME4000_AI_SAMPLE_COUNTER_REG		0xC0	// _/W
+
+/*=============================================================================
+  Value to adjust Demux
+  ===========================================================================*/
+
+#define ME4000_AO_DEMUX_ADJUST_VALUE            0x4C
+
+/*=============================================================================
+  Counter base register offsets
+  ===========================================================================*/
+
+#define ME4000_CNT_COUNTER_0_REG		0x00
+#define ME4000_CNT_COUNTER_1_REG		0x01
+#define ME4000_CNT_COUNTER_2_REG		0x02
+#define ME4000_CNT_CTRL_REG			0x03
+
+/*=============================================================================
+  PLX base register offsets
+  ===========================================================================*/
+
+#define PLX_INTCSR	0x4C	// Interrupt control and status register
+#define PLX_ICR		0x50	// Initialization control register
+
+/*=============================================================================
+  Bits for the PLX_ICSR register
+  ===========================================================================*/
+
+#define PLX_INTCSR_LOCAL_INT1_EN             0x01	// If set, local interrupt 1 is enabled (r/w)
+#define PLX_INTCSR_LOCAL_INT1_POL            0x02	// If set, local interrupt 1 polarity is active high (r/w)
+#define PLX_INTCSR_LOCAL_INT1_STATE          0x04	// If set, local interrupt 1 is active (r/_)
+#define PLX_INTCSR_LOCAL_INT2_EN             0x08	// If set, local interrupt 2 is enabled (r/w)
+#define PLX_INTCSR_LOCAL_INT2_POL            0x10	// If set, local interrupt 2 polarity is active high (r/w)
+#define PLX_INTCSR_LOCAL_INT2_STATE          0x20	// If set, local interrupt 2 is active  (r/_)
+#define PLX_INTCSR_PCI_INT_EN                0x40	// If set, PCI interrupt is enabled (r/w)
+#define PLX_INTCSR_SOFT_INT                  0x80	// If set, a software interrupt is generated (r/w)
+
+/*=============================================================================
+  Bits for the PLX_ICR register
+  ===========================================================================*/
+
+#define PLX_ICR_BIT_EEPROM_CLOCK_SET		0x01000000
+#define PLX_ICR_BIT_EEPROM_CHIP_SELECT		0x02000000
+#define PLX_ICR_BIT_EEPROM_WRITE		0x04000000
+#define PLX_ICR_BIT_EEPROM_READ			0x08000000
+#define PLX_ICR_BIT_EEPROM_VALID		0x10000000
+
+#define PLX_ICR_MASK_EEPROM			0x1F000000
+
+#define EEPROM_DELAY				1
+
+/*=============================================================================
+  Bits for the ME4000_AO_CTRL_REG register
+  ===========================================================================*/
+
+#define ME4000_AO_CTRL_BIT_MODE_0		0x001
+#define ME4000_AO_CTRL_BIT_MODE_1		0x002
+#define ME4000_AO_CTRL_MASK_MODE		0x003
+#define ME4000_AO_CTRL_BIT_STOP			0x004
+#define ME4000_AO_CTRL_BIT_ENABLE_FIFO		0x008
+#define ME4000_AO_CTRL_BIT_ENABLE_EX_TRIG	0x010
+#define ME4000_AO_CTRL_BIT_EX_TRIG_EDGE		0x020
+#define ME4000_AO_CTRL_BIT_IMMEDIATE_STOP	0x080
+#define ME4000_AO_CTRL_BIT_ENABLE_DO		0x100
+#define ME4000_AO_CTRL_BIT_ENABLE_IRQ		0x200
+#define ME4000_AO_CTRL_BIT_RESET_IRQ		0x400
+
+/*=============================================================================
+  Bits for the ME4000_AO_STATUS_REG register
+  ===========================================================================*/
+
+#define ME4000_AO_STATUS_BIT_FSM		0x01
+#define ME4000_AO_STATUS_BIT_FF			0x02
+#define ME4000_AO_STATUS_BIT_HF			0x04
+#define ME4000_AO_STATUS_BIT_EF			0x08
+
+/*=============================================================================
+  Bits for the ME4000_AI_CTRL_REG register
+  ===========================================================================*/
+
+#define ME4000_AI_CTRL_BIT_MODE_0		0x00000001
+#define ME4000_AI_CTRL_BIT_MODE_1		0x00000002
+#define ME4000_AI_CTRL_BIT_MODE_2		0x00000004
+#define ME4000_AI_CTRL_BIT_SAMPLE_HOLD		0x00000008
+#define ME4000_AI_CTRL_BIT_IMMEDIATE_STOP	0x00000010
+#define ME4000_AI_CTRL_BIT_STOP			0x00000020
+#define ME4000_AI_CTRL_BIT_CHANNEL_FIFO		0x00000040
+#define ME4000_AI_CTRL_BIT_DATA_FIFO		0x00000080
+#define ME4000_AI_CTRL_BIT_FULLSCALE		0x00000100
+#define ME4000_AI_CTRL_BIT_OFFSET		0x00000200
+#define ME4000_AI_CTRL_BIT_EX_TRIG_ANALOG	0x00000400
+#define ME4000_AI_CTRL_BIT_EX_TRIG		0x00000800
+#define ME4000_AI_CTRL_BIT_EX_TRIG_FALLING	0x00001000
+#define ME4000_AI_CTRL_BIT_EX_IRQ		0x00002000
+#define ME4000_AI_CTRL_BIT_EX_IRQ_RESET		0x00004000
+#define ME4000_AI_CTRL_BIT_LE_IRQ		0x00008000
+#define ME4000_AI_CTRL_BIT_LE_IRQ_RESET		0x00010000
+#define ME4000_AI_CTRL_BIT_HF_IRQ		0x00020000
+#define ME4000_AI_CTRL_BIT_HF_IRQ_RESET		0x00040000
+#define ME4000_AI_CTRL_BIT_SC_IRQ		0x00080000
+#define ME4000_AI_CTRL_BIT_SC_IRQ_RESET		0x00100000
+#define ME4000_AI_CTRL_BIT_SC_RELOAD		0x00200000
+#define ME4000_AI_CTRL_BIT_EX_TRIG_BOTH		0x80000000
+
+/*=============================================================================
+  Bits for the ME4000_AI_STATUS_REG register
+  ===========================================================================*/
+
+#define ME4000_AI_STATUS_BIT_EF_CHANNEL		0x00400000
+#define ME4000_AI_STATUS_BIT_HF_CHANNEL		0x00800000
+#define ME4000_AI_STATUS_BIT_FF_CHANNEL		0x01000000
+#define ME4000_AI_STATUS_BIT_EF_DATA		0x02000000
+#define ME4000_AI_STATUS_BIT_HF_DATA		0x04000000
+#define ME4000_AI_STATUS_BIT_FF_DATA		0x08000000
+#define ME4000_AI_STATUS_BIT_LE			0x10000000
+#define ME4000_AI_STATUS_BIT_FSM		0x20000000
+
+/*=============================================================================
+  Bits for the ME4000_IRQ_STATUS_REG register
+  ===========================================================================*/
+
+#define ME4000_IRQ_STATUS_BIT_EX		0x01
+#define ME4000_IRQ_STATUS_BIT_LE		0x02
+#define ME4000_IRQ_STATUS_BIT_AI_HF		0x04
+#define ME4000_IRQ_STATUS_BIT_AO_0_HF		0x08
+#define ME4000_IRQ_STATUS_BIT_AO_1_HF		0x10
+#define ME4000_IRQ_STATUS_BIT_AO_2_HF		0x20
+#define ME4000_IRQ_STATUS_BIT_AO_3_HF		0x40
+#define ME4000_IRQ_STATUS_BIT_SC		0x80
+
+/*=============================================================================
+  Bits for the ME4000_DIO_CTRL_REG register
+  ===========================================================================*/
+
+#define ME4000_DIO_CTRL_BIT_MODE_0		0x0001
+#define ME4000_DIO_CTRL_BIT_MODE_1		0x0002
+#define ME4000_DIO_CTRL_BIT_MODE_2		0x0004
+#define ME4000_DIO_CTRL_BIT_MODE_3		0x0008
+#define ME4000_DIO_CTRL_BIT_MODE_4		0x0010
+#define ME4000_DIO_CTRL_BIT_MODE_5		0x0020
+#define ME4000_DIO_CTRL_BIT_MODE_6		0x0040
+#define ME4000_DIO_CTRL_BIT_MODE_7		0x0080
+
+#define ME4000_DIO_CTRL_BIT_FUNCTION_0		0x0100
+#define ME4000_DIO_CTRL_BIT_FUNCTION_1		0x0200
+
+#define ME4000_DIO_CTRL_BIT_FIFO_HIGH_0		0x0400
+#define ME4000_DIO_CTRL_BIT_FIFO_HIGH_1		0x0800
+#define ME4000_DIO_CTRL_BIT_FIFO_HIGH_2		0x1000
+#define ME4000_DIO_CTRL_BIT_FIFO_HIGH_3		0x2000
+
+/*=============================================================================
+  Information about the hardware capabilities
+  ===========================================================================*/
+
+typedef struct me4000_ao_info {
+	int count;
+	int fifo_count;
+} me4000_ao_info_t;
+
+typedef struct me4000_ai_info {
+	int count;
+	int sh_count;
+	int diff_count;
+	int ex_trig_analog;
+} me4000_ai_info_t;
+
+typedef struct me4000_dio_info {
+	int count;
+} me4000_dio_info_t;
+
+typedef struct me4000_cnt_info {
+	int count;
+} me4000_cnt_info_t;
+
+typedef struct me4000_board {
+	const char *name;
+	unsigned short device_id;
+	me4000_ao_info_t ao;
+	me4000_ai_info_t ai;
+	me4000_dio_info_t dio;
+	me4000_cnt_info_t cnt;
+} me4000_board_t;
+
+#define thisboard ((const me4000_board_t *)dev->board_ptr)
+
+/*=============================================================================
+  Global board and subdevice information structures
+  ===========================================================================*/
+
+typedef struct me4000_ao_context {
+	int irq;
+
+	unsigned long mirror;	// Store the last written value
+
+	unsigned long ctrl_reg;
+	unsigned long status_reg;
+	unsigned long fifo_reg;
+	unsigned long single_reg;
+	unsigned long timer_reg;
+	unsigned long irq_status_reg;
+	unsigned long preload_reg;
+} me4000_ao_context_t;
+
+typedef struct me4000_ai_context {
+	int irq;
+
+	unsigned long ctrl_reg;
+	unsigned long status_reg;
+	unsigned long channel_list_reg;
+	unsigned long data_reg;
+	unsigned long chan_timer_reg;
+	unsigned long chan_pre_timer_reg;
+	unsigned long scan_timer_low_reg;
+	unsigned long scan_timer_high_reg;
+	unsigned long scan_pre_timer_low_reg;
+	unsigned long scan_pre_timer_high_reg;
+	unsigned long start_reg;
+	unsigned long irq_status_reg;
+	unsigned long sample_counter_reg;
+} me4000_ai_context_t;
+
+typedef struct me4000_dio_context {
+	unsigned long dir_reg;
+	unsigned long ctrl_reg;
+	unsigned long port_0_reg;
+	unsigned long port_1_reg;
+	unsigned long port_2_reg;
+	unsigned long port_3_reg;
+} me4000_dio_context_t;
+
+typedef struct me4000_cnt_context {
+	unsigned long ctrl_reg;
+	unsigned long counter_0_reg;
+	unsigned long counter_1_reg;
+	unsigned long counter_2_reg;
+} me4000_cnt_context_t;
+
+typedef struct me4000_info {
+	unsigned long plx_regbase;	// PLX configuration space base address
+	unsigned long me4000_regbase;	// Base address of the ME4000
+	unsigned long timer_regbase;	// Base address of the timer circuit
+	unsigned long program_regbase;	// Base address to set the program pin for the xilinx
+
+	unsigned long plx_regbase_size;	// PLX register set space
+	unsigned long me4000_regbase_size;	// ME4000 register set space
+	unsigned long timer_regbase_size;	// Timer circuit register set space
+	unsigned long program_regbase_size;	// Size of program base address of the ME4000
+
+	unsigned int serial_no;	// Serial number of the board
+	unsigned char hw_revision;	// Hardware revision of the board
+	unsigned short vendor_id;	// Meilhaus vendor id
+	unsigned short device_id;	// Device id
+
+	struct pci_dev *pci_dev_p;	// General PCI information
+
+	unsigned int irq;	// IRQ assigned from the PCI BIOS
+
+	struct me4000_ai_context ai_context;	// Analog input  specific context
+	struct me4000_ao_context ao_context[4];	// Vector with analog output specific context
+	struct me4000_dio_context dio_context;	// Digital I/O specific context
+	struct me4000_cnt_context cnt_context;	// Counter specific context
+} me4000_info_t;
+
+#define info	((me4000_info_t *)dev->private)
+
+/*-----------------------------------------------------------------------------
+  Defines for analog input
+ ----------------------------------------------------------------------------*/
+
+/* General stuff */
+#define ME4000_AI_FIFO_COUNT			2048
+
+#define ME4000_AI_MIN_TICKS			66
+#define ME4000_AI_MIN_SAMPLE_TIME		2000	// Minimum sample time [ns]
+#define ME4000_AI_BASE_FREQUENCY		(unsigned int) 33E6
+
+/* Channel list defines and masks */
+#define ME4000_AI_CHANNEL_LIST_COUNT		1024
+
+#define ME4000_AI_LIST_INPUT_SINGLE_ENDED	0x000
+#define ME4000_AI_LIST_INPUT_DIFFERENTIAL	0x020
+
+#define ME4000_AI_LIST_RANGE_BIPOLAR_10		0x000
+#define ME4000_AI_LIST_RANGE_BIPOLAR_2_5	0x040
+#define ME4000_AI_LIST_RANGE_UNIPOLAR_10	0x080
+#define ME4000_AI_LIST_RANGE_UNIPOLAR_2_5	0x0C0
+
+#define ME4000_AI_LIST_LAST_ENTRY		0x100
+
+/*-----------------------------------------------------------------------------
+  Defines for counters
+ ----------------------------------------------------------------------------*/
+
+#define ME4000_CNT_COUNTER_0  0x00
+#define ME4000_CNT_COUNTER_1  0x40
+#define ME4000_CNT_COUNTER_2  0x80
+
+#define ME4000_CNT_MODE_0     0x00	// Change state if zero crossing
+#define ME4000_CNT_MODE_1     0x02	// Retriggerable One-Shot
+#define ME4000_CNT_MODE_2     0x04	// Asymmetrical divider
+#define ME4000_CNT_MODE_3     0x06	// Symmetrical divider
+#define ME4000_CNT_MODE_4     0x08	// Counter start by software trigger
+#define ME4000_CNT_MODE_5     0x0A	// Counter start by hardware trigger
+
+#endif
diff --git a/drivers/staging/comedi/drivers/me_daq.c b/drivers/staging/comedi/drivers/me_daq.c
new file mode 100644
index 00000000000..6accec20a0f
--- /dev/null
+++ b/drivers/staging/comedi/drivers/me_daq.c
@@ -0,0 +1,845 @@
+/*
+
+   comedi/drivers/me_daq.c
+
+   Hardware driver for Meilhaus data acquisition cards:
+
+     ME-2000i, ME-2600i, ME-3000vm1
+
+   Copyright (C) 2002 Michael Hillmann <hillmann@syscongroup.de>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+*/
+
+/*
+Driver: me_daq
+Description: Meilhaus PCI data acquisition cards
+Author: Michael Hillmann <hillmann@syscongroup.de>
+Devices: [Meilhaus] ME-2600i (me_daq), ME-2000i
+Status: experimental
+
+Supports:
+
+    Analog Output
+
+Configuration options:
+
+    [0] - PCI bus number (optional)
+    [1] - PCI slot number (optional)
+
+    If bus/slot is not specified, the first available PCI
+    device will be used.
+
+The 2600 requires a firmware upload, which can be accomplished
+using the -i or --init-data option of comedi_config.
+The firmware can be
+found in the comedi_nonfree_firmware tarball available
+from http://www.comedi.org
+
+*/
+
+#include "../comedidev.h"
+
+#include "comedi_pci.h"
+
+/*#include "me2600_fw.h" */
+
+#define ME_DRIVER_NAME		"me_daq"
+
+#define ME2000_DEVICE_ID	0x2000
+#define ME2600_DEVICE_ID	0x2600
+
+#define PLX_INTCSR		0x4C	/* PLX interrupt status register */
+#define XILINX_DOWNLOAD_RESET	0x42	/* Xilinx registers */
+
+#define ME_CONTROL_1			0x0000	/* - | W */
+#define   INTERRUPT_ENABLE		(1<<15)
+#define   COUNTER_B_IRQ			(1<<12)
+#define   COUNTER_A_IRQ			(1<<11)
+#define   CHANLIST_READY_IRQ		(1<<10)
+#define   EXT_IRQ			(1<<9)
+#define   ADFIFO_HALFFULL_IRQ		(1<<8)
+#define   SCAN_COUNT_ENABLE		(1<<5)
+#define   SIMULTANEOUS_ENABLE		(1<<4)
+#define   TRIGGER_FALLING_EDGE		(1<<3)
+#define   CONTINUOUS_MODE		(1<<2)
+#define   DISABLE_ADC			(0<<0)
+#define   SOFTWARE_TRIGGERED_ADC	(1<<0)
+#define   SCAN_TRIGGERED_ADC		(2<<0)
+#define   EXT_TRIGGERED_ADC		(3<<0)
+#define ME_ADC_START			0x0000	/* R | - */
+#define ME_CONTROL_2			0x0002	/* - | W */
+#define   ENABLE_ADFIFO			(1<<10)
+#define   ENABLE_CHANLIST		(1<<9)
+#define   ENABLE_PORT_B			(1<<7)
+#define   ENABLE_PORT_A			(1<<6)
+#define   ENABLE_COUNTER_B		(1<<4)
+#define   ENABLE_COUNTER_A		(1<<3)
+#define   ENABLE_DAC			(1<<1)
+#define   BUFFERED_DAC			(1<<0)
+#define ME_DAC_UPDATE			0x0002	/* R | - */
+#define ME_STATUS			0x0004	/* R | - */
+#define   COUNTER_B_IRQ_PENDING		(1<<12)
+#define   COUNTER_A_IRQ_PENDING		(1<<11)
+#define   CHANLIST_READY_IRQ_PENDING	(1<<10)
+#define   EXT_IRQ_PENDING		(1<<9)
+#define   ADFIFO_HALFFULL_IRQ_PENDING	(1<<8)
+#define   ADFIFO_FULL			(1<<4)
+#define   ADFIFO_HALFFULL		(1<<3)
+#define   ADFIFO_EMPTY			(1<<2)
+#define   CHANLIST_FULL			(1<<1)
+#define   FST_ACTIVE			(1<<0)
+#define ME_RESET_INTERRUPT		0x0004	/* - | W */
+#define ME_DIO_PORT_A			0x0006	/* R | W */
+#define ME_DIO_PORT_B			0x0008	/* R | W */
+#define ME_TIMER_DATA_0			0x000A	/* - | W */
+#define ME_TIMER_DATA_1			0x000C	/* - | W */
+#define ME_TIMER_DATA_2			0x000E	/* - | W */
+#define ME_CHANNEL_LIST			0x0010	/* - | W */
+#define   ADC_UNIPOLAR			(1<<6)
+#define   ADC_GAIN_0			(0<<4)
+#define   ADC_GAIN_1			(1<<4)
+#define   ADC_GAIN_2			(2<<4)
+#define   ADC_GAIN_3			(3<<4)
+#define ME_READ_AD_FIFO			0x0010	/* R | - */
+#define ME_DAC_CONTROL			0x0012	/* - | W */
+#define   DAC_UNIPOLAR_D		(0<<4)
+#define   DAC_BIPOLAR_D			(1<<4)
+#define   DAC_UNIPOLAR_C		(0<<5)
+#define   DAC_BIPOLAR_C			(1<<5)
+#define   DAC_UNIPOLAR_B		(0<<6)
+#define   DAC_BIPOLAR_B			(1<<6)
+#define   DAC_UNIPOLAR_A		(0<<7)
+#define   DAC_BIPOLAR_A			(1<<7)
+#define   DAC_GAIN_0_D			(0<<8)
+#define   DAC_GAIN_1_D			(1<<8)
+#define   DAC_GAIN_0_C			(0<<9)
+#define   DAC_GAIN_1_C			(1<<9)
+#define   DAC_GAIN_0_B			(0<<10)
+#define   DAC_GAIN_1_B			(1<<10)
+#define   DAC_GAIN_0_A			(0<<11)
+#define   DAC_GAIN_1_A			(1<<11)
+#define ME_DAC_CONTROL_UPDATE		0x0012	/* R | - */
+#define ME_DAC_DATA_A			0x0014	/* - | W */
+#define ME_DAC_DATA_B			0x0016	/* - | W */
+#define ME_DAC_DATA_C			0x0018	/* - | W */
+#define ME_DAC_DATA_D			0x001A	/* - | W */
+#define ME_COUNTER_ENDDATA_A		0x001C	/* - | W */
+#define ME_COUNTER_ENDDATA_B		0x001E	/* - | W */
+#define ME_COUNTER_STARTDATA_A		0x0020	/* - | W */
+#define ME_COUNTER_VALUE_A		0x0020	/* R | - */
+#define ME_COUNTER_STARTDATA_B		0x0022	/* - | W */
+#define ME_COUNTER_VALUE_B		0x0022	/* R | - */
+
+/* Function prototypes */
+static int me_attach(comedi_device *dev, comedi_devconfig *it);
+static int me_detach(comedi_device *dev);
+
+static const comedi_lrange me2000_ai_range = {
+	8,
+	{
+		BIP_RANGE(10),
+		BIP_RANGE(5),
+		BIP_RANGE(2.5),
+		BIP_RANGE(1.25),
+		UNI_RANGE(10),
+		UNI_RANGE(5),
+		UNI_RANGE(2.5),
+		UNI_RANGE(1.25)
+	}
+};
+
+static const comedi_lrange me2600_ai_range = {
+	8,
+	{
+			BIP_RANGE(10),
+			BIP_RANGE(5),
+			BIP_RANGE(2.5),
+			BIP_RANGE(1.25),
+			UNI_RANGE(10),
+			UNI_RANGE(5),
+			UNI_RANGE(2.5),
+			UNI_RANGE(1.25)
+		}
+};
+
+static const comedi_lrange me2600_ao_range = {
+	3,
+	{
+			BIP_RANGE(10),
+			BIP_RANGE(5),
+			UNI_RANGE(10)
+		}
+};
+
+static DEFINE_PCI_DEVICE_TABLE(me_pci_table) = {
+	{PCI_VENDOR_ID_MEILHAUS, ME2600_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID, 0, 0,
+		0},
+	{PCI_VENDOR_ID_MEILHAUS, ME2000_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID, 0, 0,
+		0},
+	{0}
+};
+
+MODULE_DEVICE_TABLE(pci, me_pci_table);
+
+/* Board specification structure */
+struct me_board {
+	const char *name;	/* driver name */
+	int device_id;
+	int ao_channel_nbr;	/* DA config */
+	int ao_resolution;
+	int ao_resolution_mask;
+	const comedi_lrange *ao_range_list;
+	int ai_channel_nbr;	/* AD config */
+	int ai_resolution;
+	int ai_resolution_mask;
+	const comedi_lrange *ai_range_list;
+	int dio_channel_nbr;	/* DIO config */
+};
+
+static const struct me_board me_boards[] = {
+	{
+		/* -- ME-2600i -- */
+		.name = 		ME_DRIVER_NAME,
+		.device_id =		ME2600_DEVICE_ID,
+		/* Analog Output */
+		.ao_channel_nbr =	4,
+		.ao_resolution =	12,
+		.ao_resolution_mask =	0x0fff,
+		.ao_range_list =	&me2600_ao_range,
+		.ai_channel_nbr =	16,
+		/* Analog Input */
+		.ai_resolution =	12,
+		.ai_resolution_mask =	0x0fff,
+		.ai_range_list =	&me2600_ai_range,
+		.dio_channel_nbr =	32,
+		},
+	{
+		/* -- ME-2000i -- */
+		.name =			ME_DRIVER_NAME,
+		.device_id =		ME2000_DEVICE_ID,
+		/* Analog Output */
+		.ao_channel_nbr =	0,
+		.ao_resolution =	0,
+		.ao_resolution_mask =	0,
+		.ao_range_list =	NULL,
+		.ai_channel_nbr =	16,
+		/* Analog Input */
+		.ai_resolution =	12,
+		.ai_resolution_mask =	0x0fff,
+		.ai_range_list =	&me2000_ai_range,
+		.dio_channel_nbr =	32,
+		}
+};
+
+#define me_board_nbr (sizeof(me_boards)/sizeof(struct me_board))
+
+static comedi_driver me_driver = {
+      .driver_name =	ME_DRIVER_NAME,
+      .module =		THIS_MODULE,
+      .attach =		me_attach,
+      .detach =		me_detach,
+};
+COMEDI_PCI_INITCLEANUP(me_driver, me_pci_table);
+
+/* Private data structure */
+struct me_private_data {
+	struct pci_dev *pci_device;
+	void __iomem *plx_regbase;	/* PLX configuration base address */
+	void __iomem *me_regbase;	/* Base address of the Meilhaus card */
+	unsigned long plx_regbase_size;	/* Size of PLX configuration space */
+	unsigned long me_regbase_size;	/* Size of Meilhaus space */
+
+	unsigned short control_1;	/* Mirror of CONTROL_1 register */
+	unsigned short control_2;	/* Mirror of CONTROL_2 register */
+	unsigned short dac_control;	/* Mirror of the DAC_CONTROL register */
+	int ao_readback[4];	/* Mirror of analog output data */
+};
+
+#define dev_private ((struct me_private_data *)dev->private)
+
+/*
+ * ------------------------------------------------------------------
+ *
+ * Helpful functions
+ *
+ * ------------------------------------------------------------------
+ */
+static inline void sleep(unsigned sec)
+{
+	current->state = TASK_INTERRUPTIBLE;
+	schedule_timeout(sec * HZ);
+}
+
+/*
+ * ------------------------------------------------------------------
+ *
+ * DIGITAL INPUT/OUTPUT SECTION
+ *
+ * ------------------------------------------------------------------
+ */
+static int me_dio_insn_config(comedi_device *dev, comedi_subdevice *s,
+			      comedi_insn *insn, lsampl_t *data)
+{
+	int bits;
+	int mask = 1 << CR_CHAN(insn->chanspec);
+
+	/* calculate port */
+	if (mask & 0x0000ffff) {	/* Port A in use */
+		bits = 0x0000ffff;
+
+		/* Enable Port A */
+		dev_private->control_2 |= ENABLE_PORT_A;
+		writew(dev_private->control_2,
+			dev_private->me_regbase + ME_CONTROL_2);
+	} else {		/* Port B in use */
+
+		bits = 0xffff0000;
+
+		/* Enable Port B */
+		dev_private->control_2 |= ENABLE_PORT_B;
+		writew(dev_private->control_2,
+			dev_private->me_regbase + ME_CONTROL_2);
+	}
+
+	if (data[0]) {
+		/* Config port as output */
+		s->io_bits |= bits;
+	} else {
+		/* Config port as input */
+		s->io_bits &= ~bits;
+	}
+
+	return 1;
+}
+
+/* Digital instant input/outputs */
+static int me_dio_insn_bits(comedi_device *dev, comedi_subdevice *s,
+			    comedi_insn *insn, lsampl_t *data)
+{
+	unsigned int mask = data[0];
+	s->state &= ~mask;
+	s->state |= (mask & data[1]);
+
+	mask &= s->io_bits;
+	if (mask & 0x0000ffff) {	/* Port A */
+		writew((s->state & 0xffff),
+			dev_private->me_regbase + ME_DIO_PORT_A);
+	} else {
+		data[1] &= ~0x0000ffff;
+		data[1] |= readw(dev_private->me_regbase + ME_DIO_PORT_A);
+	}
+
+	if (mask & 0xffff0000) {	/* Port B */
+		writew(((s->state >> 16) & 0xffff),
+			dev_private->me_regbase + ME_DIO_PORT_B);
+	} else {
+		data[1] &= ~0xffff0000;
+		data[1] |= readw(dev_private->me_regbase + ME_DIO_PORT_B) << 16;
+	}
+
+	return 2;
+}
+
+/*
+ * ------------------------------------------------------------------
+ *
+ * ANALOG INPUT SECTION
+ *
+ * ------------------------------------------------------------------
+ */
+
+/* Analog instant input */
+static int me_ai_insn_read(comedi_device *dev, comedi_subdevice *subdevice,
+			   comedi_insn *insn, lsampl_t *data)
+{
+	unsigned short value;
+	int chan = CR_CHAN((&insn->chanspec)[0]);
+	int rang = CR_RANGE((&insn->chanspec)[0]);
+	int aref = CR_AREF((&insn->chanspec)[0]);
+	int i;
+
+	/* stop any running conversion */
+	dev_private->control_1 &= 0xFFFC;
+	writew(dev_private->control_1, dev_private->me_regbase + ME_CONTROL_1);
+
+	/* clear chanlist and ad fifo */
+	dev_private->control_2 &= ~(ENABLE_ADFIFO | ENABLE_CHANLIST);
+	writew(dev_private->control_2, dev_private->me_regbase + ME_CONTROL_2);
+
+	/* reset any pending interrupt */
+	writew(0x00, dev_private->me_regbase + ME_RESET_INTERRUPT);
+
+	/* enable the chanlist and ADC fifo */
+	dev_private->control_2 |= (ENABLE_ADFIFO | ENABLE_CHANLIST);
+	writew(dev_private->control_2, dev_private->me_regbase + ME_CONTROL_2);
+
+	/* write to channel list fifo */
+	/* b3:b0 are the channel number */
+	value = chan & 0x0f;
+	/* b5:b4 are the channel gain */
+	value |= (rang & 0x03) << 4;
+	/* b6 channel polarity */
+	value |= (rang & 0x04) << 4;
+	/* b7 single or differential */
+	value |= ((aref & AREF_DIFF) ? 0x80 : 0);
+	writew(value & 0xff, dev_private->me_regbase + ME_CHANNEL_LIST);
+
+	/* set ADC mode to software trigger */
+	dev_private->control_1 |= SOFTWARE_TRIGGERED_ADC;
+	writew(dev_private->control_1, dev_private->me_regbase + ME_CONTROL_1);
+
+	/* start conversion by reading from ADC_START */
+	readw(dev_private->me_regbase + ME_ADC_START);
+
+	/* wait for ADC fifo not empty flag */
+	for (i = 100000; i > 0; i--)
+		if (!(readw(dev_private->me_regbase + ME_STATUS) & 0x0004))
+			break;
+
+	/* get value from ADC fifo */
+	if (i) {
+		data[0] =
+			(readw(dev_private->me_regbase +
+				ME_READ_AD_FIFO) ^ 0x800) & 0x0FFF;
+	} else {
+		printk(KERN_ERR "comedi%d: Cannot get single value\n",
+		       dev->minor);
+		return -EIO;
+	}
+
+	/* stop any running conversion */
+	dev_private->control_1 &= 0xFFFC;
+	writew(dev_private->control_1, dev_private->me_regbase + ME_CONTROL_1);
+
+	return 1;
+}
+
+/*
+ * ------------------------------------------------------------------
+ *
+ * HARDWARE TRIGGERED ANALOG INPUT SECTION
+ *
+ * ------------------------------------------------------------------
+ */
+
+/* Cancel analog input autoscan */
+static int me_ai_cancel(comedi_device *dev, comedi_subdevice *s)
+{
+	/* disable interrupts */
+
+	/* stop any running conversion */
+	dev_private->control_1 &= 0xFFFC;
+	writew(dev_private->control_1, dev_private->me_regbase + ME_CONTROL_1);
+
+	return 0;
+}
+
+/* Test analog input command */
+static int me_ai_do_cmd_test(comedi_device *dev, comedi_subdevice *s,
+			     comedi_cmd *cmd)
+{
+	return 0;
+}
+
+/* Analog input command */
+static int me_ai_do_cmd(comedi_device *dev, comedi_subdevice *subdevice)
+{
+	return 0;
+}
+
+/*
+ * ------------------------------------------------------------------
+ *
+ * ANALOG OUTPUT SECTION
+ *
+ * ------------------------------------------------------------------
+ */
+
+/* Analog instant output */
+static int me_ao_insn_write(comedi_device *dev, comedi_subdevice *s,
+			    comedi_insn *insn, lsampl_t *data)
+{
+	int chan;
+	int rang;
+	int i;
+
+	/* Enable all DAC */
+	dev_private->control_2 |= ENABLE_DAC;
+	writew(dev_private->control_2, dev_private->me_regbase + ME_CONTROL_2);
+
+	/* and set DAC to "buffered" mode */
+	dev_private->control_2 |= BUFFERED_DAC;
+	writew(dev_private->control_2, dev_private->me_regbase + ME_CONTROL_2);
+
+	/* Set dac-control register */
+	for (i = 0; i < insn->n; i++) {
+		chan = CR_CHAN((&insn->chanspec)[i]);
+		rang = CR_RANGE((&insn->chanspec)[i]);
+
+		/* clear bits for this channel */
+		dev_private->dac_control &= ~(0x0880 >> chan);
+		if (rang == 0)
+			dev_private->dac_control |=
+				((DAC_BIPOLAR_A | DAC_GAIN_1_A) >> chan);
+		else if (rang == 1)
+			dev_private->dac_control |=
+				((DAC_BIPOLAR_A | DAC_GAIN_0_A) >> chan);
+	}
+	writew(dev_private->dac_control,
+		dev_private->me_regbase + ME_DAC_CONTROL);
+
+	/* Update dac-control register */
+	readw(dev_private->me_regbase + ME_DAC_CONTROL_UPDATE);
+
+	/* Set data register */
+	for (i = 0; i < insn->n; i++) {
+		chan = CR_CHAN((&insn->chanspec)[i]);
+		writew((data[0] & s->maxdata),
+			dev_private->me_regbase + ME_DAC_DATA_A + (chan << 1));
+		dev_private->ao_readback[chan] = (data[0] & s->maxdata);
+	}
+
+	/* Update dac with data registers */
+	readw(dev_private->me_regbase + ME_DAC_UPDATE);
+
+	return i;
+}
+
+/* Analog output readback */
+static int me_ao_insn_read(comedi_device *dev, comedi_subdevice *s,
+			   comedi_insn *insn, lsampl_t *data)
+{
+	int i;
+
+	for (i = 0; i < insn->n; i++) {
+		data[i] =
+			dev_private->ao_readback[CR_CHAN((&insn->chanspec)[i])];
+	}
+
+	return 1;
+}
+
+/*
+ * ------------------------------------------------------------------
+ *
+ * INITIALISATION SECTION
+ *
+ * ------------------------------------------------------------------
+ */
+
+/* Xilinx firmware download for card: ME-2600i */
+static int me2600_xilinx_download(comedi_device *dev,
+				  unsigned char *me2600_firmware,
+				  unsigned int length)
+{
+	unsigned int value;
+	unsigned int file_length;
+	unsigned int i;
+
+	/* disable irq's on PLX */
+	writel(0x00, dev_private->plx_regbase + PLX_INTCSR);
+
+	/* First, make a dummy read to reset xilinx */
+	value = readw(dev_private->me_regbase + XILINX_DOWNLOAD_RESET);
+
+	/* Wait until reset is over */
+	sleep(1);
+
+	/* Write a dummy value to Xilinx */
+	writeb(0x00, dev_private->me_regbase + 0x0);
+	sleep(1);
+
+	/*
+	 * Format of the firmware
+	 * Build longs from the byte-wise coded header
+	 * Byte 1-3:   length of the array
+	 * Byte 4-7:   version
+	 * Byte 8-11:  date
+	 * Byte 12-15: reserved
+	 */
+	if (length < 16)
+		return -EINVAL;
+	file_length = (((unsigned int)me2600_firmware[0] & 0xff) << 24) +
+		      (((unsigned int)me2600_firmware[1] & 0xff) << 16) +
+		      (((unsigned int)me2600_firmware[2] & 0xff) << 8) +
+		      ((unsigned int)me2600_firmware[3] & 0xff);
+
+	/*
+	 * Loop for writing firmware byte by byte to xilinx
+	 * Firmware data start at offfset 16
+	 */
+	for (i = 0; i < file_length; i++)
+		writeb((me2600_firmware[16 + i] & 0xff),
+			dev_private->me_regbase + 0x0);
+
+	/* Write 5 dummy values to xilinx */
+	for (i = 0; i < 5; i++)
+		writeb(0x00, dev_private->me_regbase + 0x0);
+
+	/* Test if there was an error during download -> INTB was thrown */
+	value = readl(dev_private->plx_regbase + PLX_INTCSR);
+	if (value & 0x20) {
+		/* Disable interrupt */
+		writel(0x00, dev_private->plx_regbase + PLX_INTCSR);
+		printk(KERN_ERR "comedi%d: Xilinx download failed\n",
+		       dev->minor);
+		return -EIO;
+	}
+
+	/* Wait until the Xilinx is ready for real work */
+	sleep(1);
+
+	/* Enable PLX-Interrupts */
+	writel(0x43, dev_private->plx_regbase + PLX_INTCSR);
+
+	return 0;
+}
+
+/* Reset device */
+static int me_reset(comedi_device *dev)
+{
+	/* Reset board */
+	writew(0x00, dev_private->me_regbase + ME_CONTROL_1);
+	writew(0x00, dev_private->me_regbase + ME_CONTROL_2);
+	writew(0x00, dev_private->me_regbase + ME_RESET_INTERRUPT);
+	writew(0x00, dev_private->me_regbase + ME_DAC_CONTROL);
+
+	/* Save values in the board context */
+	dev_private->dac_control = 0;
+	dev_private->control_1 = 0;
+	dev_private->control_2 = 0;
+
+	return 0;
+}
+
+/*
+ * Attach
+ *
+ * - Register PCI device
+ * - Declare device driver capability
+ */
+static int me_attach(comedi_device *dev, comedi_devconfig *it)
+{
+	struct pci_dev *pci_device;
+	comedi_subdevice *subdevice;
+	struct me_board *board;
+	resource_size_t plx_regbase_tmp;
+	unsigned long plx_regbase_size_tmp;
+	resource_size_t me_regbase_tmp;
+	unsigned long me_regbase_size_tmp;
+	resource_size_t swap_regbase_tmp;
+	unsigned long swap_regbase_size_tmp;
+	resource_size_t regbase_tmp;
+	int result, error, i;
+
+	/* Allocate private memory */
+	if (alloc_private(dev, sizeof(struct me_private_data)) < 0)
+		return -ENOMEM;
+
+	/* Probe the device to determine what device in the series it is. */
+	for (pci_device = pci_get_device(PCI_ANY_ID, PCI_ANY_ID, NULL);
+		pci_device != NULL;
+		pci_device =
+		pci_get_device(PCI_ANY_ID, PCI_ANY_ID, pci_device)) {
+		if (pci_device->vendor == PCI_VENDOR_ID_MEILHAUS) {
+			for (i = 0; i < me_board_nbr; i++) {
+				if (me_boards[i].device_id ==
+					pci_device->device) {
+					/*
+					 * was a particular bus/slot requested?
+					 */
+					if ((it->options[0] != 0)
+						|| (it->options[1] != 0)) {
+						/*
+						 * are we on the wrong bus/slot?
+						 */
+						if (pci_device->bus->number !=
+							it->options[0]
+							|| PCI_SLOT(pci_device->
+								devfn) !=
+							it->options[1]) {
+							continue;
+						}
+					}
+
+					dev->board_ptr = me_boards + i;
+					board = (struct me_board *) dev->
+						board_ptr;
+					dev_private->pci_device = pci_device;
+					goto found;
+				}
+			}
+		}
+	}
+
+	printk(KERN_ERR
+	       "comedi%d: no supported board found! (req. bus/slot : %d/%d)\n",
+	       dev->minor, it->options[0], it->options[1]);
+	return -EIO;
+
+found:
+	printk(KERN_INFO "comedi%d: found %s at PCI bus %d, slot %d\n",
+		dev->minor, me_boards[i].name,
+		pci_device->bus->number, PCI_SLOT(pci_device->devfn));
+
+	/* Enable PCI device and request PCI regions */
+	if (comedi_pci_enable(pci_device, ME_DRIVER_NAME) < 0) {
+		printk(KERN_ERR "comedi%d: Failed to enable PCI device and "
+		       "request regions\n", dev->minor);
+		return -EIO;
+	}
+
+	/* Set data in device structure */
+	dev->board_name = board->name;
+
+	/* Read PLX register base address [PCI_BASE_ADDRESS #0]. */
+	plx_regbase_tmp = pci_resource_start(pci_device, 0);
+	plx_regbase_size_tmp = pci_resource_len(pci_device, 0);
+	dev_private->plx_regbase =
+		ioremap(plx_regbase_tmp, plx_regbase_size_tmp);
+	dev_private->plx_regbase_size = plx_regbase_size_tmp;
+	if (!dev_private->plx_regbase) {
+		printk("comedi%d: Failed to remap I/O memory\n", dev->minor);
+		return -ENOMEM;
+	}
+
+	/* Read Swap base address [PCI_BASE_ADDRESS #5]. */
+
+	swap_regbase_tmp = pci_resource_start(pci_device, 5);
+	swap_regbase_size_tmp = pci_resource_len(pci_device, 5);
+
+	if (!swap_regbase_tmp)
+		printk(KERN_ERR "comedi%d: Swap not present\n", dev->minor);
+
+	/*---------------------------------------------- Workaround start ---*/
+	if (plx_regbase_tmp & 0x0080) {
+		printk(KERN_ERR "comedi%d: PLX-Bug detected\n", dev->minor);
+
+		if (swap_regbase_tmp) {
+			regbase_tmp = plx_regbase_tmp;
+			plx_regbase_tmp = swap_regbase_tmp;
+			swap_regbase_tmp = regbase_tmp;
+
+			result = pci_write_config_dword(pci_device,
+				PCI_BASE_ADDRESS_0, plx_regbase_tmp);
+			if (result != PCIBIOS_SUCCESSFUL)
+				return -EIO;
+
+			result = pci_write_config_dword(pci_device,
+				PCI_BASE_ADDRESS_5, swap_regbase_tmp);
+			if (result != PCIBIOS_SUCCESSFUL)
+				return -EIO;
+		} else {
+			plx_regbase_tmp -= 0x80;
+			result = pci_write_config_dword(pci_device,
+				PCI_BASE_ADDRESS_0, plx_regbase_tmp);
+			if (result != PCIBIOS_SUCCESSFUL)
+				return -EIO;
+		}
+	}
+	/*--------------------------------------------- Workaround end -----*/
+
+	/* Read Meilhaus register base address [PCI_BASE_ADDRESS #2]. */
+
+	me_regbase_tmp = pci_resource_start(pci_device, 2);
+	me_regbase_size_tmp = pci_resource_len(pci_device, 2);
+	dev_private->me_regbase_size = me_regbase_size_tmp;
+	dev_private->me_regbase = ioremap(me_regbase_tmp, me_regbase_size_tmp);
+	if (!dev_private->me_regbase) {
+		printk(KERN_ERR "comedi%d: Failed to remap I/O memory\n",
+		       dev->minor);
+		return -ENOMEM;
+	}
+	/* Download firmware and reset card */
+	if (board->device_id == ME2600_DEVICE_ID) {
+		unsigned char *aux_data;
+		int aux_len;
+
+		aux_data = comedi_aux_data(it->options, 0);
+		aux_len = it->options[COMEDI_DEVCONF_AUX_DATA_LENGTH];
+
+		if (!aux_data || aux_len < 1) {
+			comedi_error(dev, "You must provide me2600 firmware "
+				     "using the --init-data option of "
+				     "comedi_config");
+			return -EINVAL;
+		}
+		me2600_xilinx_download(dev, aux_data, aux_len);
+	}
+
+	me_reset(dev);
+
+	/* device driver capabilities */
+	error = alloc_subdevices(dev, 3);
+	if (error < 0)
+		return error;
+
+	subdevice = dev->subdevices + 0;
+	subdevice->type = COMEDI_SUBD_AI;
+	subdevice->subdev_flags = SDF_READABLE | SDF_COMMON | SDF_CMD_READ;
+	subdevice->n_chan = board->ai_channel_nbr;
+	subdevice->maxdata = board->ai_resolution_mask;
+	subdevice->len_chanlist = board->ai_channel_nbr;
+	subdevice->range_table = board->ai_range_list;
+	subdevice->cancel = me_ai_cancel;
+	subdevice->insn_read = me_ai_insn_read;
+	subdevice->do_cmdtest = me_ai_do_cmd_test;
+	subdevice->do_cmd = me_ai_do_cmd;
+
+	subdevice = dev->subdevices + 1;
+	subdevice->type = COMEDI_SUBD_AO;
+	subdevice->subdev_flags = SDF_WRITEABLE | SDF_COMMON;
+	subdevice->n_chan = board->ao_channel_nbr;
+	subdevice->maxdata = board->ao_resolution_mask;
+	subdevice->len_chanlist = board->ao_channel_nbr;
+	subdevice->range_table = board->ao_range_list;
+	subdevice->insn_read = me_ao_insn_read;
+	subdevice->insn_write = me_ao_insn_write;
+
+	subdevice = dev->subdevices + 2;
+	subdevice->type = COMEDI_SUBD_DIO;
+	subdevice->subdev_flags = SDF_READABLE | SDF_WRITEABLE;
+	subdevice->n_chan = board->dio_channel_nbr;
+	subdevice->maxdata = 1;
+	subdevice->len_chanlist = board->dio_channel_nbr;
+	subdevice->range_table = &range_digital;
+	subdevice->insn_bits = me_dio_insn_bits;
+	subdevice->insn_config = me_dio_insn_config;
+	subdevice->io_bits = 0;
+
+	printk(KERN_INFO "comedi%d: "ME_DRIVER_NAME" attached.\n", dev->minor);
+	return 0;
+}
+
+/* Detach */
+static int me_detach(comedi_device *dev)
+{
+	if (dev_private) {
+		if (dev_private->me_regbase) {
+			me_reset(dev);
+			iounmap(dev_private->me_regbase);
+		}
+		if (dev_private->plx_regbase)
+			iounmap(dev_private->plx_regbase);
+		if (dev_private->pci_device) {
+			if (dev_private->plx_regbase_size)
+				comedi_pci_disable(dev_private->pci_device);
+
+			pci_dev_put(dev_private->pci_device);
+		}
+	}
+	return 0;
+}
diff --git a/drivers/staging/comedi/drivers/mite.c b/drivers/staging/comedi/drivers/mite.c
new file mode 100644
index 00000000000..9cc527424d0
--- /dev/null
+++ b/drivers/staging/comedi/drivers/mite.c
@@ -0,0 +1,809 @@
+/*
+    comedi/drivers/mite.c
+    Hardware driver for NI Mite PCI interface chip
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1997-2002 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+/*
+	The PCI-MIO E series driver was originally written by
+	Tomasz Motylewski <...>, and ported to comedi by ds.
+
+	References for specifications:
+
+	   321747b.pdf  Register Level Programmer Manual (obsolete)
+	   321747c.pdf  Register Level Programmer Manual (new)
+	   DAQ-STC reference manual
+
+	Other possibly relevant info:
+
+	   320517c.pdf  User manual (obsolete)
+	   320517f.pdf  User manual (new)
+	   320889a.pdf  delete
+	   320906c.pdf  maximum signal ratings
+	   321066a.pdf  about 16x
+	   321791a.pdf  discontinuation of at-mio-16e-10 rev. c
+	   321808a.pdf  about at-mio-16e-10 rev P
+	   321837a.pdf  discontinuation of at-mio-16de-10 rev d
+	   321838a.pdf  about at-mio-16de-10 rev N
+
+	ISSUES:
+
+*/
+
+//#define USE_KMALLOC
+
+#include "mite.h"
+
+#include "comedi_fc.h"
+#include "comedi_pci.h"
+#include "../comedidev.h"
+
+#include <asm/system.h>
+
+#define PCI_MITE_SIZE		4096
+#define PCI_DAQ_SIZE		4096
+#define PCI_DAQ_SIZE_660X       8192
+
+MODULE_LICENSE("GPL");
+
+struct mite_struct *mite_devices = NULL;
+
+#define TOP_OF_PAGE(x) ((x)|(~(PAGE_MASK)))
+
+void mite_init(void)
+{
+	struct pci_dev *pcidev;
+	struct mite_struct *mite;
+
+	for (pcidev = pci_get_device(PCI_ANY_ID, PCI_ANY_ID, NULL);
+		pcidev != NULL;
+		pcidev = pci_get_device(PCI_ANY_ID, PCI_ANY_ID, pcidev)) {
+		if (pcidev->vendor == PCI_VENDOR_ID_NATINST) {
+			unsigned i;
+
+			mite = kzalloc(sizeof(*mite), GFP_KERNEL);
+			if (!mite) {
+				printk("mite: allocation failed\n");
+				pci_dev_put(pcidev);
+				return;
+			}
+			spin_lock_init(&mite->lock);
+			mite->pcidev = pci_dev_get(pcidev);
+			for (i = 0; i < MAX_MITE_DMA_CHANNELS; ++i) {
+				mite->channels[i].mite = mite;
+				mite->channels[i].channel = i;
+				mite->channels[i].done = 1;
+			}
+			mite->next = mite_devices;
+			mite_devices = mite;
+		}
+	}
+}
+
+static void dump_chip_signature(u32 csigr_bits)
+{
+	printk("mite: version = %i, type = %i, mite mode = %i, interface mode = %i\n", mite_csigr_version(csigr_bits), mite_csigr_type(csigr_bits), mite_csigr_mmode(csigr_bits), mite_csigr_imode(csigr_bits));
+	printk("mite: num channels = %i, write post fifo depth = %i, wins = %i, iowins = %i\n", mite_csigr_dmac(csigr_bits), mite_csigr_wpdep(csigr_bits), mite_csigr_wins(csigr_bits), mite_csigr_iowins(csigr_bits));
+}
+
+unsigned mite_fifo_size(struct mite_struct * mite, unsigned channel)
+{
+	unsigned fcr_bits = readl(mite->mite_io_addr +
+		MITE_FCR(channel));
+	unsigned empty_count = (fcr_bits >> 16) & 0xff;
+	unsigned full_count = fcr_bits & 0xff;
+	return empty_count + full_count;
+}
+
+int mite_setup2(struct mite_struct *mite, unsigned use_iodwbsr_1)
+{
+	unsigned long length;
+	resource_size_t addr;
+	int i;
+	u32 csigr_bits;
+	unsigned unknown_dma_burst_bits;
+
+	if (comedi_pci_enable(mite->pcidev, "mite")) {
+		printk("error enabling mite and requesting io regions\n");
+		return -EIO;
+	}
+	pci_set_master(mite->pcidev);
+
+	addr = pci_resource_start(mite->pcidev, 0);
+	mite->mite_phys_addr = addr;
+	mite->mite_io_addr = ioremap(addr, PCI_MITE_SIZE);
+	if (!mite->mite_io_addr) {
+		printk("failed to remap mite io memory address\n");
+		return -ENOMEM;
+	}
+	printk("MITE:0x%08llx mapped to %p ",
+		(unsigned long long)mite->mite_phys_addr, mite->mite_io_addr);
+
+	addr = pci_resource_start(mite->pcidev, 1);
+	mite->daq_phys_addr = addr;
+	length = pci_resource_len(mite->pcidev, 1);
+	// In case of a 660x board, DAQ size is 8k instead of 4k (see as shown by lspci output)
+	mite->daq_io_addr = ioremap(mite->daq_phys_addr, length);
+	if (!mite->daq_io_addr) {
+		printk("failed to remap daq io memory address\n");
+		return -ENOMEM;
+	}
+	printk("DAQ:0x%08llx mapped to %p\n",
+		(unsigned long long)mite->daq_phys_addr, mite->daq_io_addr);
+
+	if (use_iodwbsr_1) {
+		writel(0, mite->mite_io_addr + MITE_IODWBSR);
+		printk("mite: using I/O Window Base Size register 1\n");
+		writel(mite->
+			daq_phys_addr | WENAB |
+			MITE_IODWBSR_1_WSIZE_bits(length),
+			mite->mite_io_addr + MITE_IODWBSR_1);
+		writel(0, mite->mite_io_addr + MITE_IODWCR_1);
+	} else {
+		writel(mite->daq_phys_addr | WENAB,
+			mite->mite_io_addr + MITE_IODWBSR);
+	}
+	/* make sure dma bursts work.  I got this from running a bus analyzer
+	   on a pxi-6281 and a pxi-6713.  6713 powered up with register value
+	   of 0x61f and bursts worked.  6281 powered up with register value of
+	   0x1f and bursts didn't work.  The NI windows driver reads the register,
+	   then does a bitwise-or of 0x600 with it and writes it back.
+	 */
+	unknown_dma_burst_bits =
+		readl(mite->mite_io_addr + MITE_UNKNOWN_DMA_BURST_REG);
+	unknown_dma_burst_bits |= UNKNOWN_DMA_BURST_ENABLE_BITS;
+	writel(unknown_dma_burst_bits,
+		mite->mite_io_addr + MITE_UNKNOWN_DMA_BURST_REG);
+
+	csigr_bits = readl(mite->mite_io_addr + MITE_CSIGR);
+	mite->num_channels = mite_csigr_dmac(csigr_bits);
+	if (mite->num_channels > MAX_MITE_DMA_CHANNELS) {
+		printk("mite: bug? chip claims to have %i dma channels.  Setting to %i.\n", mite->num_channels, MAX_MITE_DMA_CHANNELS);
+		mite->num_channels = MAX_MITE_DMA_CHANNELS;
+	}
+	dump_chip_signature(csigr_bits);
+	for (i = 0; i < mite->num_channels; i++) {
+		writel(CHOR_DMARESET, mite->mite_io_addr + MITE_CHOR(i));
+		/* disable interrupts */
+		writel(CHCR_CLR_DMA_IE | CHCR_CLR_LINKP_IE | CHCR_CLR_SAR_IE |
+			CHCR_CLR_DONE_IE | CHCR_CLR_MRDY_IE | CHCR_CLR_DRDY_IE |
+			CHCR_CLR_LC_IE | CHCR_CLR_CONT_RB_IE,
+			mite->mite_io_addr + MITE_CHCR(i));
+	}
+	mite->fifo_size = mite_fifo_size(mite, 0);
+	printk("mite: fifo size is %i.\n", mite->fifo_size);
+	mite->used = 1;
+
+	return 0;
+}
+
+int mite_setup(struct mite_struct *mite)
+{
+	return mite_setup2(mite, 0);
+}
+
+void mite_cleanup(void)
+{
+	struct mite_struct *mite, *next;
+
+	for (mite = mite_devices; mite; mite = next) {
+		pci_dev_put(mite->pcidev);
+		next = mite->next;
+		kfree(mite);
+	}
+}
+
+void mite_unsetup(struct mite_struct *mite)
+{
+	//unsigned long offset, start, length;
+
+	if (!mite)
+		return;
+
+	if (mite->mite_io_addr) {
+		iounmap(mite->mite_io_addr);
+		mite->mite_io_addr = NULL;
+	}
+	if (mite->daq_io_addr) {
+		iounmap(mite->daq_io_addr);
+		mite->daq_io_addr = NULL;
+	}
+	if (mite->mite_phys_addr) {
+		comedi_pci_disable(mite->pcidev);
+		mite->mite_phys_addr = 0;
+	}
+
+	mite->used = 0;
+}
+
+void mite_list_devices(void)
+{
+	struct mite_struct *mite, *next;
+
+	printk("Available NI device IDs:");
+	if (mite_devices)
+		for (mite = mite_devices; mite; mite = next) {
+			next = mite->next;
+			printk(" 0x%04x", mite_device_id(mite));
+			if (mite->used)
+				printk("(used)");
+		}
+	printk("\n");
+
+}
+
+struct mite_channel *mite_request_channel_in_range(struct mite_struct *mite,
+	struct mite_dma_descriptor_ring *ring, unsigned min_channel,
+	unsigned max_channel)
+{
+	int i;
+	unsigned long flags;
+	struct mite_channel *channel = NULL;
+
+	// spin lock so mite_release_channel can be called safely from interrupts
+	comedi_spin_lock_irqsave(&mite->lock, flags);
+	for (i = min_channel; i <= max_channel; ++i) {
+		if (mite->channel_allocated[i] == 0) {
+			mite->channel_allocated[i] = 1;
+			channel = &mite->channels[i];
+			channel->ring = ring;
+			break;
+		}
+	}
+	comedi_spin_unlock_irqrestore(&mite->lock, flags);
+	return channel;
+}
+
+void mite_release_channel(struct mite_channel *mite_chan)
+{
+	struct mite_struct *mite = mite_chan->mite;
+	unsigned long flags;
+
+	// spin lock to prevent races with mite_request_channel
+	comedi_spin_lock_irqsave(&mite->lock, flags);
+	if (mite->channel_allocated[mite_chan->channel]) {
+		mite_dma_disarm(mite_chan);
+		mite_dma_reset(mite_chan);
+/* disable all channel's interrupts (do it after disarm/reset so
+MITE_CHCR reg isn't changed while dma is still active!) */
+		writel(CHCR_CLR_DMA_IE | CHCR_CLR_LINKP_IE |
+			CHCR_CLR_SAR_IE | CHCR_CLR_DONE_IE |
+			CHCR_CLR_MRDY_IE | CHCR_CLR_DRDY_IE |
+			CHCR_CLR_LC_IE | CHCR_CLR_CONT_RB_IE,
+			mite->mite_io_addr + MITE_CHCR(mite_chan->channel));
+		mite->channel_allocated[mite_chan->channel] = 0;
+		mite_chan->ring = NULL;
+		mmiowb();
+	}
+	comedi_spin_unlock_irqrestore(&mite->lock, flags);
+}
+
+void mite_dma_arm(struct mite_channel *mite_chan)
+{
+	struct mite_struct *mite = mite_chan->mite;
+	int chor;
+	unsigned long flags;
+
+	MDPRINTK("mite_dma_arm ch%i\n", channel);
+	/* memory barrier is intended to insure any twiddling with the buffer
+	   is done before writing to the mite to arm dma transfer */
+	smp_mb();
+	/* arm */
+	chor = CHOR_START;
+	comedi_spin_lock_irqsave(&mite->lock, flags);
+	mite_chan->done = 0;
+	writel(chor, mite->mite_io_addr + MITE_CHOR(mite_chan->channel));
+	mmiowb();
+	comedi_spin_unlock_irqrestore(&mite->lock, flags);
+//      mite_dma_tcr(mite, channel);
+}
+
+/**************************************/
+
+int mite_buf_change(struct mite_dma_descriptor_ring *ring, comedi_async * async)
+{
+	unsigned int n_links;
+	int i;
+
+	if (ring->descriptors) {
+		dma_free_coherent(ring->hw_dev,
+			ring->n_links * sizeof(struct mite_dma_descriptor),
+			ring->descriptors, ring->descriptors_dma_addr);
+	}
+	ring->descriptors = NULL;
+	ring->descriptors_dma_addr = 0;
+	ring->n_links = 0;
+
+	if (async->prealloc_bufsz == 0) {
+		return 0;
+	}
+	n_links = async->prealloc_bufsz >> PAGE_SHIFT;
+
+	MDPRINTK("ring->hw_dev=%p, n_links=0x%04x\n", ring->hw_dev, n_links);
+
+	ring->descriptors =
+		dma_alloc_coherent(ring->hw_dev,
+		n_links * sizeof(struct mite_dma_descriptor),
+		&ring->descriptors_dma_addr, GFP_KERNEL);
+	if (!ring->descriptors) {
+		printk("mite: ring buffer allocation failed\n");
+		return -ENOMEM;
+	}
+	ring->n_links = n_links;
+
+	for (i = 0; i < n_links; i++) {
+		ring->descriptors[i].count = cpu_to_le32(PAGE_SIZE);
+		ring->descriptors[i].addr =
+			cpu_to_le32(async->buf_page_list[i].dma_addr);
+		ring->descriptors[i].next =
+			cpu_to_le32(ring->descriptors_dma_addr + (i +
+				1) * sizeof(struct mite_dma_descriptor));
+	}
+	ring->descriptors[n_links - 1].next =
+		cpu_to_le32(ring->descriptors_dma_addr);
+	/* barrier is meant to insure that all the writes to the dma descriptors
+	   have completed before the dma controller is commanded to read them */
+	smp_wmb();
+	return 0;
+}
+
+void mite_prep_dma(struct mite_channel *mite_chan,
+	unsigned int num_device_bits, unsigned int num_memory_bits)
+{
+	unsigned int chor, chcr, mcr, dcr, lkcr;
+	struct mite_struct *mite = mite_chan->mite;
+
+	MDPRINTK("mite_prep_dma ch%i\n", mite_chan->channel);
+
+	/* reset DMA and FIFO */
+	chor = CHOR_DMARESET | CHOR_FRESET;
+	writel(chor, mite->mite_io_addr + MITE_CHOR(mite_chan->channel));
+
+	/* short link chaining mode */
+	chcr = CHCR_SET_DMA_IE | CHCR_LINKSHORT | CHCR_SET_DONE_IE |
+		CHCR_BURSTEN;
+	/*
+	 * Link Complete Interrupt: interrupt every time a link
+	 * in MITE_RING is completed. This can generate a lot of
+	 * extra interrupts, but right now we update the values
+	 * of buf_int_ptr and buf_int_count at each interrupt.  A
+	 * better method is to poll the MITE before each user
+	 * "read()" to calculate the number of bytes available.
+	 */
+	chcr |= CHCR_SET_LC_IE;
+	if (num_memory_bits == 32 && num_device_bits == 16) {
+		/* Doing a combined 32 and 16 bit byteswap gets the 16 bit samples into the fifo in the right order.
+		   Tested doing 32 bit memory to 16 bit device transfers to the analog out of a pxi-6281,
+		   which has mite version = 1, type = 4.  This also works for dma reads from the counters
+		   on e-series boards.  */
+		chcr |= CHCR_BYTE_SWAP_DEVICE | CHCR_BYTE_SWAP_MEMORY;
+	}
+	if (mite_chan->dir == COMEDI_INPUT) {
+		chcr |= CHCR_DEV_TO_MEM;
+	}
+	writel(chcr, mite->mite_io_addr + MITE_CHCR(mite_chan->channel));
+
+	/* to/from memory */
+	mcr = CR_RL(64) | CR_ASEQUP;
+	switch (num_memory_bits) {
+	case 8:
+		mcr |= CR_PSIZE8;
+		break;
+	case 16:
+		mcr |= CR_PSIZE16;
+		break;
+	case 32:
+		mcr |= CR_PSIZE32;
+		break;
+	default:
+		rt_printk
+			("mite: bug! invalid mem bit width for dma transfer\n");
+		break;
+	}
+	writel(mcr, mite->mite_io_addr + MITE_MCR(mite_chan->channel));
+
+	/* from/to device */
+	dcr = CR_RL(64) | CR_ASEQUP;
+	dcr |= CR_PORTIO | CR_AMDEVICE | CR_REQSDRQ(mite_chan->channel);
+	switch (num_device_bits) {
+	case 8:
+		dcr |= CR_PSIZE8;
+		break;
+	case 16:
+		dcr |= CR_PSIZE16;
+		break;
+	case 32:
+		dcr |= CR_PSIZE32;
+		break;
+	default:
+		rt_printk
+			("mite: bug! invalid dev bit width for dma transfer\n");
+		break;
+	}
+	writel(dcr, mite->mite_io_addr + MITE_DCR(mite_chan->channel));
+
+	/* reset the DAR */
+	writel(0, mite->mite_io_addr + MITE_DAR(mite_chan->channel));
+
+	/* the link is 32bits */
+	lkcr = CR_RL(64) | CR_ASEQUP | CR_PSIZE32;
+	writel(lkcr, mite->mite_io_addr + MITE_LKCR(mite_chan->channel));
+
+	/* starting address for link chaining */
+	writel(mite_chan->ring->descriptors_dma_addr,
+		mite->mite_io_addr + MITE_LKAR(mite_chan->channel));
+
+	MDPRINTK("exit mite_prep_dma\n");
+}
+
+u32 mite_device_bytes_transferred(struct mite_channel *mite_chan)
+{
+	struct mite_struct *mite = mite_chan->mite;
+	return readl(mite->mite_io_addr + MITE_DAR(mite_chan->channel));
+}
+
+u32 mite_bytes_in_transit(struct mite_channel * mite_chan)
+{
+	struct mite_struct *mite = mite_chan->mite;
+	return readl(mite->mite_io_addr +
+		MITE_FCR(mite_chan->channel)) & 0x000000FF;
+}
+
+// returns lower bound for number of bytes transferred from device to memory
+u32 mite_bytes_written_to_memory_lb(struct mite_channel * mite_chan)
+{
+	u32 device_byte_count;
+
+	device_byte_count = mite_device_bytes_transferred(mite_chan);
+	return device_byte_count - mite_bytes_in_transit(mite_chan);
+}
+
+// returns upper bound for number of bytes transferred from device to memory
+u32 mite_bytes_written_to_memory_ub(struct mite_channel * mite_chan)
+{
+	u32 in_transit_count;
+
+	in_transit_count = mite_bytes_in_transit(mite_chan);
+	return mite_device_bytes_transferred(mite_chan) - in_transit_count;
+}
+
+// returns lower bound for number of bytes read from memory for transfer to device
+u32 mite_bytes_read_from_memory_lb(struct mite_channel * mite_chan)
+{
+	u32 device_byte_count;
+
+	device_byte_count = mite_device_bytes_transferred(mite_chan);
+	return device_byte_count + mite_bytes_in_transit(mite_chan);
+}
+
+// returns upper bound for number of bytes read from memory for transfer to device
+u32 mite_bytes_read_from_memory_ub(struct mite_channel * mite_chan)
+{
+	u32 in_transit_count;
+
+	in_transit_count = mite_bytes_in_transit(mite_chan);
+	return mite_device_bytes_transferred(mite_chan) + in_transit_count;
+}
+
+unsigned mite_dma_tcr(struct mite_channel *mite_chan)
+{
+	struct mite_struct *mite = mite_chan->mite;
+	int tcr;
+	int lkar;
+
+	lkar = readl(mite->mite_io_addr + MITE_LKAR(mite_chan->channel));
+	tcr = readl(mite->mite_io_addr + MITE_TCR(mite_chan->channel));
+	MDPRINTK("mite_dma_tcr ch%i, lkar=0x%08x tcr=%d\n", mite_chan->channel,
+		lkar, tcr);
+
+	return tcr;
+}
+
+void mite_dma_disarm(struct mite_channel *mite_chan)
+{
+	struct mite_struct *mite = mite_chan->mite;
+	unsigned chor;
+
+	/* disarm */
+	chor = CHOR_ABORT;
+	writel(chor, mite->mite_io_addr + MITE_CHOR(mite_chan->channel));
+}
+
+int mite_sync_input_dma(struct mite_channel *mite_chan, comedi_async * async)
+{
+	int count;
+	unsigned int nbytes, old_alloc_count;
+	const unsigned bytes_per_scan = cfc_bytes_per_scan(async->subdevice);
+
+	old_alloc_count = async->buf_write_alloc_count;
+	// write alloc as much as we can
+	comedi_buf_write_alloc(async, async->prealloc_bufsz);
+
+	nbytes = mite_bytes_written_to_memory_lb(mite_chan);
+	if ((int)(mite_bytes_written_to_memory_ub(mite_chan) -
+			old_alloc_count) > 0) {
+		rt_printk("mite: DMA overwrite of free area\n");
+		async->events |= COMEDI_CB_OVERFLOW;
+		return -1;
+	}
+
+	count = nbytes - async->buf_write_count;
+	/* it's possible count will be negative due to
+	 * conservative value returned by mite_bytes_written_to_memory_lb */
+	if (count <= 0) {
+		return 0;
+	}
+	comedi_buf_write_free(async, count);
+
+	async->scan_progress += count;
+	if (async->scan_progress >= bytes_per_scan) {
+		async->scan_progress %= bytes_per_scan;
+		async->events |= COMEDI_CB_EOS;
+	}
+	async->events |= COMEDI_CB_BLOCK;
+	return 0;
+}
+
+int mite_sync_output_dma(struct mite_channel *mite_chan, comedi_async * async)
+{
+	int count;
+	u32 nbytes_ub, nbytes_lb;
+	unsigned int old_alloc_count;
+	u32 stop_count =
+		async->cmd.stop_arg * cfc_bytes_per_scan(async->subdevice);
+
+	old_alloc_count = async->buf_read_alloc_count;
+	// read alloc as much as we can
+	comedi_buf_read_alloc(async, async->prealloc_bufsz);
+	nbytes_lb = mite_bytes_read_from_memory_lb(mite_chan);
+	if (async->cmd.stop_src == TRIG_COUNT &&
+		(int)(nbytes_lb - stop_count) > 0)
+		nbytes_lb = stop_count;
+	nbytes_ub = mite_bytes_read_from_memory_ub(mite_chan);
+	if (async->cmd.stop_src == TRIG_COUNT &&
+		(int)(nbytes_ub - stop_count) > 0)
+		nbytes_ub = stop_count;
+	if ((int)(nbytes_ub - old_alloc_count) > 0) {
+		rt_printk("mite: DMA underrun\n");
+		async->events |= COMEDI_CB_OVERFLOW;
+		return -1;
+	}
+	count = nbytes_lb - async->buf_read_count;
+	if (count <= 0) {
+		return 0;
+	}
+	if (count) {
+		comedi_buf_read_free(async, count);
+		async->events |= COMEDI_CB_BLOCK;
+	}
+	return 0;
+}
+
+unsigned mite_get_status(struct mite_channel *mite_chan)
+{
+	struct mite_struct *mite = mite_chan->mite;
+	unsigned status;
+	unsigned long flags;
+
+	comedi_spin_lock_irqsave(&mite->lock, flags);
+	status = readl(mite->mite_io_addr + MITE_CHSR(mite_chan->channel));
+	if (status & CHSR_DONE) {
+		mite_chan->done = 1;
+		writel(CHOR_CLRDONE,
+			mite->mite_io_addr + MITE_CHOR(mite_chan->channel));
+	}
+	mmiowb();
+	comedi_spin_unlock_irqrestore(&mite->lock, flags);
+	return status;
+}
+
+int mite_done(struct mite_channel *mite_chan)
+{
+	struct mite_struct *mite = mite_chan->mite;
+	unsigned long flags;
+	int done;
+
+	mite_get_status(mite_chan);
+	comedi_spin_lock_irqsave(&mite->lock, flags);
+	done = mite_chan->done;
+	comedi_spin_unlock_irqrestore(&mite->lock, flags);
+	return done;
+}
+
+#ifdef DEBUG_MITE
+
+static void mite_decode(char **bit_str, unsigned int bits);
+
+/* names of bits in mite registers */
+
+static const char *const mite_CHOR_strings[] = {
+	"start", "cont", "stop", "abort",
+	"freset", "clrlc", "clrrb", "clrdone",
+	"clr_lpause", "set_lpause", "clr_send_tc",
+	"set_send_tc", "12", "13", "14",
+	"15", "16", "17", "18",
+	"19", "20", "21", "22",
+	"23", "24", "25", "26",
+	"27", "28", "29", "30",
+	"dmareset",
+};
+
+static const char *const mite_CHCR_strings[] = {
+	"continue", "ringbuff", "2", "3",
+	"4", "5", "6", "7",
+	"8", "9", "10", "11",
+	"12", "13", "bursten", "fifodis",
+	"clr_cont_rb_ie", "set_cont_rb_ie", "clr_lc_ie", "set_lc_ie",
+	"clr_drdy_ie", "set_drdy_ie", "clr_mrdy_ie", "set_mrdy_ie",
+	"clr_done_ie", "set_done_ie", "clr_sar_ie", "set_sar_ie",
+	"clr_linkp_ie", "set_linkp_ie", "clr_dma_ie", "set_dma_ie",
+};
+
+static const char *const mite_MCR_strings[] = {
+	"amdevice", "1", "2", "3",
+	"4", "5", "portio", "portvxi",
+	"psizebyte", "psizehalf (byte & half = word)", "aseqxp1", "11",
+	"12", "13", "blocken", "berhand",
+	"reqsintlim/reqs0", "reqs1", "reqs2", "rd32",
+	"rd512", "rl1", "rl2", "rl8",
+	"24", "25", "26", "27",
+	"28", "29", "30", "stopen",
+};
+
+static const char *const mite_DCR_strings[] = {
+	"amdevice", "1", "2", "3",
+	"4", "5", "portio", "portvxi",
+	"psizebyte", "psizehalf (byte & half = word)", "aseqxp1", "aseqxp2",
+	"aseqxp8", "13", "blocken", "berhand",
+	"reqsintlim", "reqs1", "reqs2", "rd32",
+	"rd512", "rl1", "rl2", "rl8",
+	"23", "24", "25", "27",
+	"28", "wsdevc", "wsdevs", "rwdevpack",
+};
+
+static const char *const mite_LKCR_strings[] = {
+	"amdevice", "1", "2", "3",
+	"4", "5", "portio", "portvxi",
+	"psizebyte", "psizehalf (byte & half = word)", "asequp", "aseqdown",
+	"12", "13", "14", "berhand",
+	"16", "17", "18", "rd32",
+	"rd512", "rl1", "rl2", "rl8",
+	"24", "25", "26", "27",
+	"28", "29", "30", "chngend",
+};
+
+static const char *const mite_CHSR_strings[] = {
+	"d.err0", "d.err1", "m.err0", "m.err1",
+	"l.err0", "l.err1", "drq0", "drq1",
+	"end", "xferr", "operr0", "operr1",
+	"stops", "habort", "sabort", "error",
+	"16", "conts_rb", "18", "linkc",
+	"20", "drdy", "22", "mrdy",
+	"24", "done", "26", "sars",
+	"28", "lpauses", "30", "int",
+};
+
+void mite_dump_regs(struct mite_channel *mite_chan)
+{
+	unsigned long mite_io_addr =
+		(unsigned long)mite_chan->mite->mite_io_addr;
+	unsigned long addr = 0;
+	unsigned long temp = 0;
+
+	printk("mite_dump_regs ch%i\n", mite_chan->channel);
+	printk("mite address is  =0x%08lx\n", mite_io_addr);
+
+	addr = mite_io_addr + MITE_CHOR(channel);
+	printk("mite status[CHOR]at 0x%08lx =0x%08lx\n", addr, temp =
+		readl(addr));
+	mite_decode(mite_CHOR_strings, temp);
+	addr = mite_io_addr + MITE_CHCR(channel);
+	printk("mite status[CHCR]at 0x%08lx =0x%08lx\n", addr, temp =
+		readl(addr));
+	mite_decode(mite_CHCR_strings, temp);
+	addr = mite_io_addr + MITE_TCR(channel);
+	printk("mite status[TCR] at 0x%08lx =0x%08x\n", addr, readl(addr));
+	addr = mite_io_addr + MITE_MCR(channel);
+	printk("mite status[MCR] at 0x%08lx =0x%08lx\n", addr, temp =
+		readl(addr));
+	mite_decode(mite_MCR_strings, temp);
+
+	addr = mite_io_addr + MITE_MAR(channel);
+	printk("mite status[MAR] at 0x%08lx =0x%08x\n", addr, readl(addr));
+	addr = mite_io_addr + MITE_DCR(channel);
+	printk("mite status[DCR] at 0x%08lx =0x%08lx\n", addr, temp =
+		readl(addr));
+	mite_decode(mite_DCR_strings, temp);
+	addr = mite_io_addr + MITE_DAR(channel);
+	printk("mite status[DAR] at 0x%08lx =0x%08x\n", addr, readl(addr));
+	addr = mite_io_addr + MITE_LKCR(channel);
+	printk("mite status[LKCR]at 0x%08lx =0x%08lx\n", addr, temp =
+		readl(addr));
+	mite_decode(mite_LKCR_strings, temp);
+	addr = mite_io_addr + MITE_LKAR(channel);
+	printk("mite status[LKAR]at 0x%08lx =0x%08x\n", addr, readl(addr));
+
+	addr = mite_io_addr + MITE_CHSR(channel);
+	printk("mite status[CHSR]at 0x%08lx =0x%08lx\n", addr, temp =
+		readl(addr));
+	mite_decode(mite_CHSR_strings, temp);
+	addr = mite_io_addr + MITE_FCR(channel);
+	printk("mite status[FCR] at 0x%08lx =0x%08x\n\n", addr, readl(addr));
+}
+
+static void mite_decode(char **bit_str, unsigned int bits)
+{
+	int i;
+
+	for (i = 31; i >= 0; i--) {
+		if (bits & (1 << i)) {
+			printk(" %s", bit_str[i]);
+		}
+	}
+	printk("\n");
+}
+#endif
+
+#ifdef MODULE
+int __init init_module(void)
+{
+	mite_init();
+	mite_list_devices();
+
+	return 0;
+}
+
+void __exit cleanup_module(void)
+{
+	mite_cleanup();
+}
+
+EXPORT_SYMBOL(mite_dma_tcr);
+EXPORT_SYMBOL(mite_dma_arm);
+EXPORT_SYMBOL(mite_dma_disarm);
+EXPORT_SYMBOL(mite_sync_input_dma);
+EXPORT_SYMBOL(mite_sync_output_dma);
+EXPORT_SYMBOL(mite_setup);
+EXPORT_SYMBOL(mite_setup2);
+EXPORT_SYMBOL(mite_unsetup);
+#if 0
+EXPORT_SYMBOL(mite_kvmem_segment_load);
+EXPORT_SYMBOL(mite_ll_from_kvmem);
+EXPORT_SYMBOL(mite_setregs);
+#endif
+EXPORT_SYMBOL(mite_devices);
+EXPORT_SYMBOL(mite_list_devices);
+EXPORT_SYMBOL(mite_request_channel_in_range);
+EXPORT_SYMBOL(mite_release_channel);
+EXPORT_SYMBOL(mite_prep_dma);
+EXPORT_SYMBOL(mite_buf_change);
+EXPORT_SYMBOL(mite_bytes_written_to_memory_lb);
+EXPORT_SYMBOL(mite_bytes_written_to_memory_ub);
+EXPORT_SYMBOL(mite_bytes_read_from_memory_lb);
+EXPORT_SYMBOL(mite_bytes_read_from_memory_ub);
+EXPORT_SYMBOL(mite_bytes_in_transit);
+EXPORT_SYMBOL(mite_get_status);
+EXPORT_SYMBOL(mite_done);
+#ifdef DEBUG_MITE
+EXPORT_SYMBOL(mite_decode);
+EXPORT_SYMBOL(mite_dump_regs);
+#endif
+
+#endif
diff --git a/drivers/staging/comedi/drivers/mite.h b/drivers/staging/comedi/drivers/mite.h
new file mode 100644
index 00000000000..b84eafa6ff2
--- /dev/null
+++ b/drivers/staging/comedi/drivers/mite.h
@@ -0,0 +1,453 @@
+/*
+    module/mite.h
+    Hardware driver for NI Mite PCI interface chip
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1999 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#ifndef _MITE_H_
+#define _MITE_H_
+
+#include <linux/pci.h>
+#include "../comedidev.h"
+
+#define PCI_VENDOR_ID_NATINST		0x1093
+
+// #define DEBUG_MITE
+#define PCIMIO_COMPAT
+
+#ifdef DEBUG_MITE
+#define MDPRINTK(format,args...)	printk(format , ## args )
+#else
+#define MDPRINTK(format,args...)
+#endif
+
+#define MAX_MITE_DMA_CHANNELS 8
+
+struct mite_dma_descriptor {
+	u32 count;
+	u32 addr;
+	u32 next;
+	u32 dar;
+};
+
+struct mite_dma_descriptor_ring {
+	struct device *hw_dev;
+	unsigned int n_links;
+	struct mite_dma_descriptor *descriptors;
+	dma_addr_t descriptors_dma_addr;
+};
+
+struct mite_channel {
+	struct mite_struct *mite;
+	unsigned channel;
+	int dir;
+	int done;
+	struct mite_dma_descriptor_ring *ring;
+};
+
+struct mite_struct {
+	struct mite_struct *next;
+	int used;
+
+	struct pci_dev *pcidev;
+	resource_size_t mite_phys_addr;
+	void *mite_io_addr;
+	resource_size_t daq_phys_addr;
+	void *daq_io_addr;
+
+	struct mite_channel channels[MAX_MITE_DMA_CHANNELS];
+	short channel_allocated[MAX_MITE_DMA_CHANNELS];
+	int num_channels;
+	unsigned fifo_size;
+	spinlock_t lock;
+};
+
+static inline struct mite_dma_descriptor_ring *mite_alloc_ring(struct
+	mite_struct *mite)
+{
+	struct mite_dma_descriptor_ring *ring =
+		kmalloc(sizeof(struct mite_dma_descriptor_ring), GFP_KERNEL);
+	if (ring == NULL)
+		return ring;
+	ring->hw_dev = get_device(&mite->pcidev->dev);
+	if (ring->hw_dev == NULL) {
+		kfree(ring);
+		return NULL;
+	}
+	ring->n_links = 0;
+	ring->descriptors = NULL;
+	ring->descriptors_dma_addr = 0;
+	return ring;
+};
+
+static inline void mite_free_ring(struct mite_dma_descriptor_ring *ring)
+{
+	if (ring) {
+		if (ring->descriptors) {
+			dma_free_coherent(ring->hw_dev,
+				ring->n_links *
+				sizeof(struct mite_dma_descriptor),
+				ring->descriptors, ring->descriptors_dma_addr);
+		}
+		put_device(ring->hw_dev);
+		kfree(ring);
+	}
+};
+
+extern struct mite_struct *mite_devices;
+
+static inline unsigned int mite_irq(struct mite_struct *mite)
+{
+	return mite->pcidev->irq;
+};
+static inline unsigned int mite_device_id(struct mite_struct *mite)
+{
+	return mite->pcidev->device;
+};
+
+void mite_init(void);
+void mite_cleanup(void);
+int mite_setup(struct mite_struct *mite);
+int mite_setup2(struct mite_struct *mite, unsigned use_iodwbsr_1);
+void mite_unsetup(struct mite_struct *mite);
+void mite_list_devices(void);
+struct mite_channel *mite_request_channel_in_range(struct mite_struct *mite,
+	struct mite_dma_descriptor_ring *ring, unsigned min_channel,
+	unsigned max_channel);
+static inline struct mite_channel *mite_request_channel(struct mite_struct
+	*mite, struct mite_dma_descriptor_ring *ring)
+{
+	return mite_request_channel_in_range(mite, ring, 0,
+		mite->num_channels - 1);
+}
+void mite_release_channel(struct mite_channel *mite_chan);
+
+unsigned mite_dma_tcr(struct mite_channel *mite_chan);
+void mite_dma_arm(struct mite_channel *mite_chan);
+void mite_dma_disarm(struct mite_channel *mite_chan);
+int mite_sync_input_dma(struct mite_channel *mite_chan, comedi_async * async);
+int mite_sync_output_dma(struct mite_channel *mite_chan, comedi_async * async);
+u32 mite_bytes_written_to_memory_lb(struct mite_channel *mite_chan);
+u32 mite_bytes_written_to_memory_ub(struct mite_channel *mite_chan);
+u32 mite_bytes_read_from_memory_lb(struct mite_channel *mite_chan);
+u32 mite_bytes_read_from_memory_ub(struct mite_channel *mite_chan);
+u32 mite_bytes_in_transit(struct mite_channel *mite_chan);
+unsigned mite_get_status(struct mite_channel *mite_chan);
+int mite_done(struct mite_channel *mite_chan);
+
+#if 0
+unsigned long mite_ll_from_kvmem(struct mite_struct *mite, comedi_async * async,
+	int len);
+void mite_setregs(struct mite_struct *mite, unsigned long ll_start, int chan,
+	int dir);
+#endif
+
+void mite_prep_dma(struct mite_channel *mite_chan,
+	unsigned int num_device_bits, unsigned int num_memory_bits);
+int mite_buf_change(struct mite_dma_descriptor_ring *ring,
+	comedi_async * async);
+
+#ifdef DEBUG_MITE
+void mite_print_chsr(unsigned int chsr);
+void mite_dump_regs(struct mite_channel *mite_chan);
+#endif
+
+static inline int CHAN_OFFSET(int channel)
+{
+	return 0x500 + 0x100 * channel;
+};
+
+enum mite_registers {
+	/* The bits 0x90180700 in MITE_UNKNOWN_DMA_BURST_REG can be
+	   written and read back.  The bits 0x1f always read as 1.
+	   The rest always read as zero. */
+	MITE_UNKNOWN_DMA_BURST_REG = 0x28,
+	MITE_IODWBSR = 0xc0,	//IO Device Window Base Size Register
+	MITE_IODWBSR_1 = 0xc4,	// IO Device Window Base Size Register 1
+	MITE_IODWCR_1 = 0xf4,
+	MITE_PCI_CONFIG_OFFSET = 0x300,
+	MITE_CSIGR = 0x460	//chip signature
+};
+static inline int MITE_CHOR(int channel)	// channel operation
+{
+	return CHAN_OFFSET(channel) + 0x0;
+};
+static inline int MITE_CHCR(int channel)	// channel control
+{
+	return CHAN_OFFSET(channel) + 0x4;
+};
+static inline int MITE_TCR(int channel)	// transfer count
+{
+	return CHAN_OFFSET(channel) + 0x8;
+};
+static inline int MITE_MCR(int channel)	// memory configuration
+{
+	return CHAN_OFFSET(channel) + 0xc;
+};
+static inline int MITE_MAR(int channel)	// memory address
+{
+	return CHAN_OFFSET(channel) + 0x10;
+};
+static inline int MITE_DCR(int channel)	// device configuration
+{
+	return CHAN_OFFSET(channel) + 0x14;
+};
+static inline int MITE_DAR(int channel)	// device address
+{
+	return CHAN_OFFSET(channel) + 0x18;
+};
+static inline int MITE_LKCR(int channel)	// link configuration
+{
+	return CHAN_OFFSET(channel) + 0x1c;
+};
+static inline int MITE_LKAR(int channel)	// link address
+{
+	return CHAN_OFFSET(channel) + 0x20;
+};
+static inline int MITE_LLKAR(int channel)	// see mite section of tnt5002 manual
+{
+	return CHAN_OFFSET(channel) + 0x24;
+};
+static inline int MITE_BAR(int channel)	// base address
+{
+	return CHAN_OFFSET(channel) + 0x28;
+};
+static inline int MITE_BCR(int channel)	// base count
+{
+	return CHAN_OFFSET(channel) + 0x2c;
+};
+static inline int MITE_SAR(int channel)	// ? address
+{
+	return CHAN_OFFSET(channel) + 0x30;
+};
+static inline int MITE_WSCR(int channel)	// ?
+{
+	return CHAN_OFFSET(channel) + 0x34;
+};
+static inline int MITE_WSER(int channel)	// ?
+{
+	return CHAN_OFFSET(channel) + 0x38;
+};
+static inline int MITE_CHSR(int channel)	// channel status
+{
+	return CHAN_OFFSET(channel) + 0x3c;
+};
+static inline int MITE_FCR(int channel)	// fifo count
+{
+	return CHAN_OFFSET(channel) + 0x40;
+};
+
+enum MITE_IODWBSR_bits {
+	WENAB = 0x80,		// window enable
+};
+
+static inline unsigned MITE_IODWBSR_1_WSIZE_bits(unsigned size)
+{
+	unsigned order = 0;
+	while (size >>= 1)
+		++order;
+	BUG_ON(order < 1);
+	return (order - 1) & 0x1f;
+}
+
+enum MITE_UNKNOWN_DMA_BURST_bits {
+	UNKNOWN_DMA_BURST_ENABLE_BITS = 0x600
+};
+
+static inline int mite_csigr_version(u32 csigr_bits)
+{
+	return csigr_bits & 0xf;
+};
+static inline int mite_csigr_type(u32 csigr_bits)
+{				// original mite = 0, minimite = 1
+	return (csigr_bits >> 4) & 0xf;
+};
+static inline int mite_csigr_mmode(u32 csigr_bits)
+{				// mite mode, minimite = 1
+	return (csigr_bits >> 8) & 0x3;
+};
+static inline int mite_csigr_imode(u32 csigr_bits)
+{				// cpu port interface mode, pci = 0x3
+	return (csigr_bits >> 12) & 0x3;
+};
+static inline int mite_csigr_dmac(u32 csigr_bits)
+{				// number of dma channels
+	return (csigr_bits >> 16) & 0xf;
+};
+static inline int mite_csigr_wpdep(u32 csigr_bits)
+{				// write post fifo depth
+	unsigned int wpdep_bits = (csigr_bits >> 20) & 0x7;
+	if (wpdep_bits == 0)
+		return 0;
+	else
+		return 1 << (wpdep_bits - 1);
+};
+static inline int mite_csigr_wins(u32 csigr_bits)
+{
+	return (csigr_bits >> 24) & 0x1f;
+};
+static inline int mite_csigr_iowins(u32 csigr_bits)
+{				// number of io windows
+	return (csigr_bits >> 29) & 0x7;
+};
+
+enum MITE_MCR_bits {
+	MCRPON = 0,
+};
+
+enum MITE_DCR_bits {
+	DCR_NORMAL = (1 << 29),
+	DCRPON = 0,
+};
+
+enum MITE_CHOR_bits {
+	CHOR_DMARESET = (1 << 31),
+	CHOR_SET_SEND_TC = (1 << 11),
+	CHOR_CLR_SEND_TC = (1 << 10),
+	CHOR_SET_LPAUSE = (1 << 9),
+	CHOR_CLR_LPAUSE = (1 << 8),
+	CHOR_CLRDONE = (1 << 7),
+	CHOR_CLRRB = (1 << 6),
+	CHOR_CLRLC = (1 << 5),
+	CHOR_FRESET = (1 << 4),
+	CHOR_ABORT = (1 << 3),	/* stop without emptying fifo */
+	CHOR_STOP = (1 << 2),	/* stop after emptying fifo */
+	CHOR_CONT = (1 << 1),
+	CHOR_START = (1 << 0),
+	CHOR_PON = (CHOR_CLR_SEND_TC | CHOR_CLR_LPAUSE),
+};
+
+enum MITE_CHCR_bits {
+	CHCR_SET_DMA_IE = (1 << 31),
+	CHCR_CLR_DMA_IE = (1 << 30),
+	CHCR_SET_LINKP_IE = (1 << 29),
+	CHCR_CLR_LINKP_IE = (1 << 28),
+	CHCR_SET_SAR_IE = (1 << 27),
+	CHCR_CLR_SAR_IE = (1 << 26),
+	CHCR_SET_DONE_IE = (1 << 25),
+	CHCR_CLR_DONE_IE = (1 << 24),
+	CHCR_SET_MRDY_IE = (1 << 23),
+	CHCR_CLR_MRDY_IE = (1 << 22),
+	CHCR_SET_DRDY_IE = (1 << 21),
+	CHCR_CLR_DRDY_IE = (1 << 20),
+	CHCR_SET_LC_IE = (1 << 19),
+	CHCR_CLR_LC_IE = (1 << 18),
+	CHCR_SET_CONT_RB_IE = (1 << 17),
+	CHCR_CLR_CONT_RB_IE = (1 << 16),
+	CHCR_FIFODIS = (1 << 15),
+	CHCR_FIFO_ON = 0,
+	CHCR_BURSTEN = (1 << 14),
+	CHCR_NO_BURSTEN = 0,
+	CHCR_BYTE_SWAP_DEVICE = (1 << 6),
+	CHCR_BYTE_SWAP_MEMORY = (1 << 4),
+	CHCR_DIR = (1 << 3),
+	CHCR_DEV_TO_MEM = CHCR_DIR,
+	CHCR_MEM_TO_DEV = 0,
+	CHCR_NORMAL = (0 << 0),
+	CHCR_CONTINUE = (1 << 0),
+	CHCR_RINGBUFF = (2 << 0),
+	CHCR_LINKSHORT = (4 << 0),
+	CHCR_LINKLONG = (5 << 0),
+	CHCRPON =
+		(CHCR_CLR_DMA_IE | CHCR_CLR_LINKP_IE | CHCR_CLR_SAR_IE |
+		CHCR_CLR_DONE_IE | CHCR_CLR_MRDY_IE | CHCR_CLR_DRDY_IE |
+		CHCR_CLR_LC_IE | CHCR_CLR_CONT_RB_IE),
+};
+
+enum ConfigRegister_bits {
+	CR_REQS_MASK = 0x7 << 16,
+	CR_ASEQDONT = 0x0 << 10,
+	CR_ASEQUP = 0x1 << 10,
+	CR_ASEQDOWN = 0x2 << 10,
+	CR_ASEQ_MASK = 0x3 << 10,
+	CR_PSIZE8 = (1 << 8),
+	CR_PSIZE16 = (2 << 8),
+	CR_PSIZE32 = (3 << 8),
+	CR_PORTCPU = (0 << 6),
+	CR_PORTIO = (1 << 6),
+	CR_PORTVXI = (2 << 6),
+	CR_PORTMXI = (3 << 6),
+	CR_AMDEVICE = (1 << 0),
+};
+static inline int CR_REQS(int source)
+{
+	return (source & 0x7) << 16;
+};
+static inline int CR_REQSDRQ(unsigned drq_line)
+{
+	/* This also works on m-series when
+	   using channels (drq_line) 4 or 5. */
+	return CR_REQS((drq_line & 0x3) | 0x4);
+}
+static inline int CR_RL(unsigned int retry_limit)
+{
+	int value = 0;
+
+	while (retry_limit) {
+		retry_limit >>= 1;
+		value++;
+	}
+	if (value > 0x7)
+		rt_printk("comedi: bug! retry_limit too large\n");
+	return (value & 0x7) << 21;
+}
+
+enum CHSR_bits {
+	CHSR_INT = (1 << 31),
+	CHSR_LPAUSES = (1 << 29),
+	CHSR_SARS = (1 << 27),
+	CHSR_DONE = (1 << 25),
+	CHSR_MRDY = (1 << 23),
+	CHSR_DRDY = (1 << 21),
+	CHSR_LINKC = (1 << 19),
+	CHSR_CONTS_RB = (1 << 17),
+	CHSR_ERROR = (1 << 15),
+	CHSR_SABORT = (1 << 14),
+	CHSR_HABORT = (1 << 13),
+	CHSR_STOPS = (1 << 12),
+	CHSR_OPERR_mask = (3 << 10),
+	CHSR_OPERR_NOERROR = (0 << 10),
+	CHSR_OPERR_FIFOERROR = (1 << 10),
+	CHSR_OPERR_LINKERROR = (1 << 10),	/* ??? */
+	CHSR_XFERR = (1 << 9),
+	CHSR_END = (1 << 8),
+	CHSR_DRQ1 = (1 << 7),
+	CHSR_DRQ0 = (1 << 6),
+	CHSR_LxERR_mask = (3 << 4),
+	CHSR_LBERR = (1 << 4),
+	CHSR_LRERR = (2 << 4),
+	CHSR_LOERR = (3 << 4),
+	CHSR_MxERR_mask = (3 << 2),
+	CHSR_MBERR = (1 << 2),
+	CHSR_MRERR = (2 << 2),
+	CHSR_MOERR = (3 << 2),
+	CHSR_DxERR_mask = (3 << 0),
+	CHSR_DBERR = (1 << 0),
+	CHSR_DRERR = (2 << 0),
+	CHSR_DOERR = (3 << 0),
+};
+
+static inline void mite_dma_reset(struct mite_channel *mite_chan)
+{
+	writel(CHOR_DMARESET | CHOR_FRESET,
+		mite_chan->mite->mite_io_addr + MITE_CHOR(mite_chan->channel));
+};
+
+#endif
diff --git a/drivers/staging/comedi/drivers/plx9080.h b/drivers/staging/comedi/drivers/plx9080.h
new file mode 100644
index 00000000000..a5a1a6808c5
--- /dev/null
+++ b/drivers/staging/comedi/drivers/plx9080.h
@@ -0,0 +1,429 @@
+/* plx9080.h
+ *
+ * Copyright (C) 2002,2003 Frank Mori Hess <fmhess@users.sourceforge.net>
+ *
+ * I modified this file from the plx9060.h header for the
+ * wanXL device driver in the linux kernel,
+ * for the register offsets and bit definitions.  Made minor modifications,
+ * added plx9080 registers and
+ * stripped out stuff that was specifically for the wanXL driver.
+ * Note: I've only made sure the definitions are correct as far
+ * as I make use of them.  There are still various plx9060-isms
+ * left in this header file.
+ *
+ ********************************************************************
+ *
+ * Copyright (C) 1999 RG Studio s.c., http://www.rgstudio.com.pl/
+ * Written by Krzysztof Halasa <khc@rgstudio.com.pl>
+ *
+ * Portions (C) SBE Inc., used by permission.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version
+ * 2 of the License, or (at your option) any later version.
+ */
+
+#ifndef __COMEDI_PLX9080_H
+#define __COMEDI_PLX9080_H
+
+// descriptor block used for chained dma transfers
+struct plx_dma_desc {
+	volatile uint32_t pci_start_addr;
+	volatile uint32_t local_start_addr;
+	/* transfer_size is in bytes, only first 23 bits of register are used */
+	volatile uint32_t transfer_size;
+	/* address of next descriptor (quad word aligned), plus some
+	 * additional bits (see PLX_DMA0_DESCRIPTOR_REG) */
+	volatile uint32_t next;
+};
+
+/**********************************************************************
+**            Register Offsets and Bit Definitions
+**
+** Note: All offsets zero relative.  IE. Some standard base address
+** must be added to the Register Number to properly access the register.
+**
+**********************************************************************/
+
+#define PLX_LAS0RNG_REG         0x0000	/* L, Local Addr Space 0 Range Register */
+#define PLX_LAS1RNG_REG         0x00f0	/* L, Local Addr Space 1 Range Register */
+#define  LRNG_IO           0x00000001	/* Map to: 1=I/O, 0=Mem */
+#define  LRNG_ANY32        0x00000000	/* Locate anywhere in 32 bit */
+#define  LRNG_LT1MB        0x00000002	/* Locate in 1st meg */
+#define  LRNG_ANY64        0x00000004	/* Locate anywhere in 64 bit */
+#define  LRNG_MEM_MASK     0xfffffff0	// bits that specify range for memory io
+#define  LRNG_IO_MASK     0xfffffffa	// bits that specify range for normal io
+
+#define PLX_LAS0MAP_REG         0x0004	/* L, Local Addr Space 0 Remap Register */
+#define PLX_LAS1MAP_REG         0x00f4	/* L, Local Addr Space 1 Remap Register */
+#define  LMAP_EN           0x00000001	/* Enable slave decode */
+#define  LMAP_MEM_MASK     0xfffffff0	// bits that specify decode for memory io
+#define  LMAP_IO_MASK     0xfffffffa	// bits that specify decode bits for normal io
+
+/* Mode/Arbitration Register.
+*/
+#define PLX_MARB_REG         0x8	/* L, Local Arbitration Register */
+#define PLX_DMAARB_REG      0xac
+enum marb_bits {
+	MARB_LLT_MASK = 0x000000ff,	/* Local Bus Latency Timer */
+	MARB_LPT_MASK = 0x0000ff00,	/* Local Bus Pause Timer */
+	MARB_LTEN = 0x00010000,	/* Latency Timer Enable */
+	MARB_LPEN = 0x00020000,	/* Pause Timer Enable */
+	MARB_BREQ = 0x00040000,	/* Local Bus BREQ Enable */
+	MARB_DMA_PRIORITY_MASK = 0x00180000,
+	MARB_LBDS_GIVE_UP_BUS_MODE = 0x00200000,	/* local bus direct slave give up bus mode */
+	MARB_DS_LLOCK_ENABLE = 0x00400000,	/* direct slave LLOCKo# enable */
+	MARB_PCI_REQUEST_MODE = 0x00800000,
+	MARB_PCIv21_MODE = 0x01000000,	/* pci specification v2.1 mode */
+	MARB_PCI_READ_NO_WRITE_MODE = 0x02000000,
+	MARB_PCI_READ_WITH_WRITE_FLUSH_MODE = 0x04000000,
+	MARB_GATE_TIMER_WITH_BREQ = 0x08000000,	/* gate local bus latency timer with BREQ */
+	MARB_PCI_READ_NO_FLUSH_MODE = 0x10000000,
+	MARB_USE_SUBSYSTEM_IDS = 0x20000000,
+};
+
+#define PLX_BIGEND_REG 0xc
+enum bigend_bits {
+	BIGEND_CONFIG = 0x1,	/* use big endian ordering for configuration register accesses */
+	BIGEND_DIRECT_MASTER = 0x2,
+	BIGEND_DIRECT_SLAVE_LOCAL0 = 0x4,
+	BIGEND_ROM = 0x8,
+	BIGEND_BYTE_LANE = 0x10,	/* use byte lane consisting of most significant bits instead of least significant */
+	BIGEND_DIRECT_SLAVE_LOCAL1 = 0x20,
+	BIGEND_DMA1 = 0x40,
+	BIGEND_DMA0 = 0x80,
+};
+
+/* Note: The Expansion ROM  stuff is only relevant to the PC environment.
+**       This expansion ROM code is executed by the host CPU at boot time.
+**       For this reason no bit definitions are provided here.
+*/
+#define PLX_ROMRNG_REG         0x0010	/* L, Expn ROM Space Range Register */
+#define PLX_ROMMAP_REG         0x0014	/* L, Local Addr Space Range Register */
+
+#define PLX_REGION0_REG         0x0018	/* L, Local Bus Region 0 Descriptor */
+#define  RGN_WIDTH         0x00000002	/* Local bus width bits */
+#define  RGN_8BITS         0x00000000	/* 08 bit Local Bus */
+#define  RGN_16BITS        0x00000001	/* 16 bit Local Bus */
+#define  RGN_32BITS        0x00000002	/* 32 bit Local Bus */
+#define  RGN_MWS           0x0000003C	/* Memory Access Wait States */
+#define  RGN_0MWS          0x00000000
+#define  RGN_1MWS          0x00000004
+#define  RGN_2MWS          0x00000008
+#define  RGN_3MWS          0x0000000C
+#define  RGN_4MWS          0x00000010
+#define  RGN_6MWS          0x00000018
+#define  RGN_8MWS          0x00000020
+#define  RGN_MRE           0x00000040	/* Memory Space Ready Input Enable */
+#define  RGN_MBE           0x00000080	/* Memory Space Bterm Input Enable */
+#define  RGN_READ_PREFETCH_DISABLE 0x00000100
+#define  RGN_ROM_PREFETCH_DISABLE 0x00000200
+#define  RGN_READ_PREFETCH_COUNT_ENABLE 0x00000400
+#define  RGN_RWS           0x003C0000	/* Expn ROM Wait States */
+#define  RGN_RRE           0x00400000	/* ROM Space Ready Input Enable */
+#define  RGN_RBE           0x00800000	/* ROM Space Bterm Input Enable */
+#define  RGN_MBEN          0x01000000	/* Memory Space Burst Enable */
+#define  RGN_RBEN          0x04000000	/* ROM Space Burst Enable */
+#define  RGN_THROT         0x08000000	/* De-assert TRDY when FIFO full */
+#define  RGN_TRD           0xF0000000	/* Target Ready Delay /8 */
+
+#define PLX_REGION1_REG         0x00f8	/* L, Local Bus Region 1 Descriptor */
+
+#define PLX_DMRNG_REG          0x001C	/* L, Direct Master Range Register */
+
+#define PLX_LBAPMEM_REG        0x0020	/* L, Lcl Base Addr for PCI mem space */
+
+#define PLX_LBAPIO_REG         0x0024	/* L, Lcl Base Addr for PCI I/O space */
+
+#define PLX_DMMAP_REG          0x0028	/* L, Direct Master Remap Register */
+#define  DMM_MAE           0x00000001	/* Direct Mstr Memory Acc Enable */
+#define  DMM_IAE           0x00000002	/* Direct Mstr I/O Acc Enable */
+#define  DMM_LCK           0x00000004	/* LOCK Input Enable */
+#define  DMM_PF4           0x00000008	/* Prefetch 4 Mode Enable */
+#define  DMM_THROT         0x00000010	/* Assert IRDY when read FIFO full */
+#define  DMM_PAF0          0x00000000	/* Programmable Almost fill level */
+#define  DMM_PAF1          0x00000020	/* Programmable Almost fill level */
+#define  DMM_PAF2          0x00000040	/* Programmable Almost fill level */
+#define  DMM_PAF3          0x00000060	/* Programmable Almost fill level */
+#define  DMM_PAF4          0x00000080	/* Programmable Almost fill level */
+#define  DMM_PAF5          0x000000A0	/* Programmable Almost fill level */
+#define  DMM_PAF6          0x000000C0	/* Programmable Almost fill level */
+#define  DMM_PAF7          0x000000D0	/* Programmable Almost fill level */
+#define  DMM_MAP           0xFFFF0000	/* Remap Address Bits */
+
+#define PLX_CAR_REG            0x002C	/* L, Configuration Address Register */
+#define  CAR_CT0           0x00000000	/* Config Type 0 */
+#define  CAR_CT1           0x00000001	/* Config Type 1 */
+#define  CAR_REG           0x000000FC	/* Register Number Bits */
+#define  CAR_FUN           0x00000700	/* Function Number Bits */
+#define  CAR_DEV           0x0000F800	/* Device Number Bits */
+#define  CAR_BUS           0x00FF0000	/* Bus Number Bits */
+#define  CAR_CFG           0x80000000	/* Config Spc Access Enable */
+
+#define PLX_DBR_IN_REG         0x0060	/* L, PCI to Local Doorbell Register */
+
+#define PLX_DBR_OUT_REG        0x0064	/* L, Local to PCI Doorbell Register */
+
+#define PLX_INTRCS_REG         0x0068	/* L, Interrupt Control/Status Reg */
+#define  ICS_AERR          0x00000001	/* Assert LSERR on ABORT */
+#define  ICS_PERR          0x00000002	/* Assert LSERR on Parity Error */
+#define  ICS_SERR          0x00000004	/* Generate PCI SERR# */
+#define  ICS_MBIE          0x00000008	// mailbox interrupt enable
+#define  ICS_PIE           0x00000100	/* PCI Interrupt Enable */
+#define  ICS_PDIE          0x00000200	/* PCI Doorbell Interrupt Enable */
+#define  ICS_PAIE          0x00000400	/* PCI Abort Interrupt Enable */
+#define  ICS_PLIE          0x00000800	/* PCI Local Int Enable */
+#define  ICS_RAE           0x00001000	/* Retry Abort Enable */
+#define  ICS_PDIA          0x00002000	/* PCI Doorbell Interrupt Active */
+#define  ICS_PAIA          0x00004000	/* PCI Abort Interrupt Active */
+#define  ICS_LIA           0x00008000	/* Local Interrupt Active */
+#define  ICS_LIE           0x00010000	/* Local Interrupt Enable */
+#define  ICS_LDIE          0x00020000	/* Local Doorbell Int Enable */
+#define  ICS_DMA0_E        0x00040000	/* DMA #0 Interrupt Enable */
+#define  ICS_DMA1_E        0x00080000	/* DMA #1 Interrupt Enable */
+#define  ICS_LDIA          0x00100000	/* Local Doorbell Int Active */
+#define  ICS_DMA0_A        0x00200000	/* DMA #0 Interrupt Active */
+#define  ICS_DMA1_A        0x00400000	/* DMA #1 Interrupt Active */
+#define  ICS_BIA           0x00800000	/* BIST Interrupt Active */
+#define  ICS_TA_DM         0x01000000	/* Target Abort - Direct Master */
+#define  ICS_TA_DMA0       0x02000000	/* Target Abort - DMA #0 */
+#define  ICS_TA_DMA1       0x04000000	/* Target Abort - DMA #1 */
+#define  ICS_TA_RA         0x08000000	/* Target Abort - Retry Timeout */
+#define  ICS_MBIA(x)       (0x10000000 << ((x) & 0x3))	// mailbox x is active
+
+#define PLX_CONTROL_REG        0x006C	/* L, EEPROM Cntl & PCI Cmd Codes */
+#define  CTL_RDMA          0x0000000E	/* DMA Read Command */
+#define  CTL_WDMA          0x00000070	/* DMA Write Command */
+#define  CTL_RMEM          0x00000600	/* Memory Read Command */
+#define  CTL_WMEM          0x00007000	/* Memory Write Command */
+#define  CTL_USERO         0x00010000	/* USERO output pin control bit */
+#define  CTL_USERI         0x00020000	/* USERI input pin bit */
+#define  CTL_EE_CLK        0x01000000	/* EEPROM Clock line */
+#define  CTL_EE_CS         0x02000000	/* EEPROM Chip Select */
+#define  CTL_EE_W          0x04000000	/* EEPROM Write bit */
+#define  CTL_EE_R          0x08000000	/* EEPROM Read bit */
+#define  CTL_EECHK         0x10000000	/* EEPROM Present bit */
+#define  CTL_EERLD         0x20000000	/* EEPROM Reload Register */
+#define  CTL_RESET         0x40000000	/* !! Adapter Reset !! */
+#define  CTL_READY         0x80000000	/* Local Init Done */
+
+#define PLX_ID_REG	0x70	// hard-coded plx vendor and device ids
+
+#define PLX_REVISION_REG	0x74	// silicon revision
+
+#define PLX_DMA0_MODE_REG	0x80	// dma channel 0 mode register
+#define PLX_DMA1_MODE_REG	0x94	// dma channel 0 mode register
+#define  PLX_LOCAL_BUS_16_WIDE_BITS	0x1
+#define  PLX_LOCAL_BUS_32_WIDE_BITS	0x3
+#define  PLX_LOCAL_BUS_WIDTH_MASK	0x3
+#define  PLX_DMA_EN_READYIN_BIT	0x40	// enable ready in input
+#define  PLX_EN_BTERM_BIT	0x80	// enable BTERM# input
+#define  PLX_DMA_LOCAL_BURST_EN_BIT	0x100	// enable local burst mode
+#define  PLX_EN_CHAIN_BIT	0x200	// enables chaining
+#define  PLX_EN_DMA_DONE_INTR_BIT	0x400	// enables interrupt on dma done
+#define  PLX_LOCAL_ADDR_CONST_BIT	0x800	// hold local address constant (don't increment)
+#define  PLX_DEMAND_MODE_BIT	0x1000	// enables demand-mode for dma transfer
+#define  PLX_EOT_ENABLE_BIT	0x4000
+#define  PLX_STOP_MODE_BIT 0x8000
+#define  PLX_DMA_INTR_PCI_BIT	0x20000	// routes dma interrupt to pci bus (instead of local bus)
+
+#define PLX_DMA0_PCI_ADDRESS_REG	0x84	// pci address that dma transfers start at
+#define PLX_DMA1_PCI_ADDRESS_REG	0x98
+
+#define PLX_DMA0_LOCAL_ADDRESS_REG	0x88	// local address that dma transfers start at
+#define PLX_DMA1_LOCAL_ADDRESS_REG	0x9c
+
+#define PLX_DMA0_TRANSFER_SIZE_REG	0x8c	// number of bytes to transfer (first 23 bits)
+#define PLX_DMA1_TRANSFER_SIZE_REG	0xa0
+
+#define PLX_DMA0_DESCRIPTOR_REG	0x90	// descriptor pointer register
+#define PLX_DMA1_DESCRIPTOR_REG	0xa4
+#define  PLX_DESC_IN_PCI_BIT	0x1	// descriptor is located in pci space (not local space)
+#define  PLX_END_OF_CHAIN_BIT	0x2	// end of chain bit
+#define  PLX_INTR_TERM_COUNT	0x4	// interrupt when this descriptor's transfer is finished
+#define  PLX_XFER_LOCAL_TO_PCI 0x8	// transfer from local to pci bus (not pci to local)
+
+#define PLX_DMA0_CS_REG	0xa8	// command status register
+#define PLX_DMA1_CS_REG	0xa9
+#define  PLX_DMA_EN_BIT	0x1	// enable dma channel
+#define  PLX_DMA_START_BIT	0x2	// start dma transfer
+#define  PLX_DMA_ABORT_BIT	0x4	// abort dma transfer
+#define  PLX_CLEAR_DMA_INTR_BIT	0x8	// clear dma interrupt
+#define  PLX_DMA_DONE_BIT	0x10	// transfer done status bit
+
+#define PLX_DMA0_THRESHOLD_REG	0xb0	// command status register
+
+/*
+ * Accesses near the end of memory can cause the PLX chip
+ * to pre-fetch data off of end-of-ram.  Limit the size of
+ * memory so host-side accesses cannot occur.
+ */
+
+#define PLX_PREFETCH   32
+
+/*
+ * The PCI Interface, via the PCI-9060 Chip, has up to eight (8) Mailbox
+ * Registers.  The PUTS (Power-Up Test Suite) handles the board-side
+ * interface/interaction using the first 4 registers.  Specifications for
+ * the use of the full PUTS' command and status interface is contained
+ * within a separate SBE PUTS Manual.  The Host-Side Device Driver only
+ * uses a subset of the full PUTS interface.
+ */
+
+/*****************************************/
+/***    MAILBOX #(-1) - MEM ACCESS STS ***/
+/*****************************************/
+
+#define MBX_STS_VALID      0x57584744	/* 'WXGD' */
+#define MBX_STS_DILAV      0x44475857	/* swapped = 'DGXW' */
+
+/*****************************************/
+/***    MAILBOX #0  -  PUTS STATUS     ***/
+/*****************************************/
+
+#define MBX_STS_MASK       0x000000ff	/* PUTS Status Register bits */
+#define MBX_STS_TMASK      0x0000000f	/* register bits for TEST number */
+
+#define MBX_STS_PCIRESET   0x00000100	/* Host issued PCI reset request */
+#define MBX_STS_BUSY       0x00000080	/* PUTS is in progress */
+#define MBX_STS_ERROR      0x00000040	/* PUTS has failed */
+#define MBX_STS_RESERVED   0x000000c0	/* Undefined -> status in transition.
+					   We are in process of changing
+					   bits; we SET Error bit before
+					   RESET of Busy bit */
+
+#define MBX_RESERVED_5     0x00000020	/* FYI: reserved/unused bit */
+#define MBX_RESERVED_4     0x00000010	/* FYI: reserved/unused bit */
+
+/******************************************/
+/***    MAILBOX #1  -  PUTS COMMANDS    ***/
+/******************************************/
+
+/*
+ * Any attempt to execute an unimplement command results in the PUTS
+ * interface executing a NOOP and continuing as if the offending command
+ * completed normally.  Note: this supplies a simple method to interrogate
+ * mailbox command processing functionality.
+ */
+
+#define MBX_CMD_MASK       0xffff0000	/* PUTS Command Register bits */
+
+#define MBX_CMD_ABORTJ     0x85000000	/* abort and jump */
+#define MBX_CMD_RESETP     0x86000000	/* reset and pause at start */
+#define MBX_CMD_PAUSE      0x87000000	/* pause immediately */
+#define MBX_CMD_PAUSEC     0x88000000	/* pause on completion */
+#define MBX_CMD_RESUME     0x89000000	/* resume operation */
+#define MBX_CMD_STEP       0x8a000000	/* single step tests */
+
+#define MBX_CMD_BSWAP      0x8c000000	/* identify byte swap scheme */
+#define MBX_CMD_BSWAP_0    0x8c000000	/* use scheme 0 */
+#define MBX_CMD_BSWAP_1    0x8c000001	/* use scheme 1 */
+
+#define MBX_CMD_SETHMS     0x8d000000	/* setup host memory access window
+					   size */
+#define MBX_CMD_SETHBA     0x8e000000	/* setup host memory access base
+					   address */
+#define MBX_CMD_MGO        0x8f000000	/* perform memory setup and continue
+					   (IE. Done) */
+#define MBX_CMD_NOOP       0xFF000000	/* dummy, illegal command */
+
+/*****************************************/
+/***    MAILBOX #2  -  MEMORY SIZE     ***/
+/*****************************************/
+
+#define MBX_MEMSZ_MASK     0xffff0000	/* PUTS Memory Size Register bits */
+
+#define MBX_MEMSZ_128KB    0x00020000	/* 128 kilobyte board */
+#define MBX_MEMSZ_256KB    0x00040000	/* 256 kilobyte board */
+#define MBX_MEMSZ_512KB    0x00080000	/* 512 kilobyte board */
+#define MBX_MEMSZ_1MB      0x00100000	/* 1 megabyte board */
+#define MBX_MEMSZ_2MB      0x00200000	/* 2 megabyte board */
+#define MBX_MEMSZ_4MB      0x00400000	/* 4 megabyte board */
+#define MBX_MEMSZ_8MB      0x00800000	/* 8 megabyte board */
+#define MBX_MEMSZ_16MB     0x01000000	/* 16 megabyte board */
+
+/***************************************/
+/***    MAILBOX #2  -  BOARD TYPE    ***/
+/***************************************/
+
+#define MBX_BTYPE_MASK          0x0000ffff	/* PUTS Board Type Register */
+#define MBX_BTYPE_FAMILY_MASK   0x0000ff00	/* PUTS Board Family Register */
+#define MBX_BTYPE_SUBTYPE_MASK  0x000000ff	/* PUTS Board Subtype */
+
+#define MBX_BTYPE_PLX9060       0x00000100	/* PLX family type */
+#define MBX_BTYPE_PLX9080       0x00000300	/* PLX wanXL100s family type */
+
+#define MBX_BTYPE_WANXL_4       0x00000104	/* wanXL400, 4-port */
+#define MBX_BTYPE_WANXL_2       0x00000102	/* wanXL200, 2-port */
+#define MBX_BTYPE_WANXL_1s      0x00000301	/* wanXL100s, 1-port */
+#define MBX_BTYPE_WANXL_1t      0x00000401	/* wanXL100T1, 1-port */
+
+/*****************************************/
+/***    MAILBOX #3  -  SHMQ MAILBOX    ***/
+/*****************************************/
+
+#define MBX_SMBX_MASK           0x000000ff	/* PUTS SHMQ Mailbox bits */
+
+/***************************************/
+/***    GENERIC HOST-SIDE DRIVER     ***/
+/***************************************/
+
+#define MBX_ERR    0
+#define MBX_OK     1
+
+/* mailbox check routine - type of testing */
+#define MBXCHK_STS      0x00	/* check for PUTS status */
+#define MBXCHK_NOWAIT   0x01	/* dont care about PUTS status */
+
+/* system allocates this many bytes for address mapping mailbox space */
+#define MBX_ADDR_SPACE_360 0x80	/* wanXL100s/200/400 */
+#define MBX_ADDR_MASK_360 (MBX_ADDR_SPACE_360-1)
+
+static inline int plx9080_abort_dma(void *iobase, unsigned int channel)
+{
+	void *dma_cs_addr;
+	uint8_t dma_status;
+	const int timeout = 10000;
+	unsigned int i;
+
+	if (channel)
+		dma_cs_addr = iobase + PLX_DMA1_CS_REG;
+	else
+		dma_cs_addr = iobase + PLX_DMA0_CS_REG;
+
+	// abort dma transfer if necessary
+	dma_status = readb(dma_cs_addr);
+	if ((dma_status & PLX_DMA_EN_BIT) == 0) {
+		return 0;
+	}
+	// wait to make sure done bit is zero
+	for (i = 0; (dma_status & PLX_DMA_DONE_BIT) && i < timeout; i++) {
+		comedi_udelay(1);
+		dma_status = readb(dma_cs_addr);
+	}
+	if (i == timeout) {
+		rt_printk
+			("plx9080: cancel() timed out waiting for dma %i done clear\n",
+			channel);
+		return -ETIMEDOUT;
+	}
+	// disable and abort channel
+	writeb(PLX_DMA_ABORT_BIT, dma_cs_addr);
+	// wait for dma done bit
+	dma_status = readb(dma_cs_addr);
+	for (i = 0; (dma_status & PLX_DMA_DONE_BIT) == 0 && i < timeout; i++) {
+		comedi_udelay(1);
+		dma_status = readb(dma_cs_addr);
+	}
+	if (i == timeout) {
+		rt_printk
+			("plx9080: cancel() timed out waiting for dma %i done set\n",
+			channel);
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+#endif /* __COMEDI_PLX9080_H */
diff --git a/drivers/staging/comedi/drivers/rtd520.c b/drivers/staging/comedi/drivers/rtd520.c
new file mode 100644
index 00000000000..65d5242a258
--- /dev/null
+++ b/drivers/staging/comedi/drivers/rtd520.c
@@ -0,0 +1,2283 @@
+/*
+    comedi/drivers/rtd520.c
+    Comedi driver for Real Time Devices (RTD) PCI4520/DM7520
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 2001 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+*/
+/*
+Driver: rtd520
+Description: Real Time Devices PCI4520/DM7520
+Author: Dan Christian
+Devices: [Real Time Devices] DM7520HR-1 (rtd520), DM7520HR-8,
+  PCI4520, PCI4520-8
+Status: Works.  Only tested on DM7520-8.  Not SMP safe.
+
+Configuration options:
+  [0] - PCI bus of device (optional)
+          If bus/slot is not specified, the first available PCI
+          device will be used.
+  [1] - PCI slot of device (optional)
+*/
+/*
+    Created by Dan Christian, NASA Ames Research Center.
+
+    The PCI4520 is a PCI card.  The DM7520 is a PC/104-plus card.
+    Both have:
+    8/16 12 bit ADC with FIFO and channel gain table
+    8 bits high speed digital out (for external MUX) (or 8 in or 8 out)
+    8 bits high speed digital in with FIFO and interrupt on change (or 8 IO)
+    2 12 bit DACs with FIFOs
+    2 bits output
+    2 bits input
+    bus mastering DMA
+    timers: ADC sample, pacer, burst, about, delay, DA1, DA2
+    sample counter
+    3 user timer/counters (8254)
+    external interrupt
+
+    The DM7520 has slightly fewer features (fewer gain steps).
+
+    These boards can support external multiplexors and multi-board
+    synchronization, but this driver doesn't support that.
+
+    Board docs: http://www.rtdusa.com/PC104/DM/analog%20IO/dm7520.htm
+    Data sheet: http://www.rtdusa.com/pdf/dm7520.pdf
+    Example source: http://www.rtdusa.com/examples/dm/dm7520.zip
+    Call them and ask for the register level manual.
+    PCI chip: http://www.plxtech.com/products/toolbox/9080.htm
+
+    Notes:
+    This board is memory mapped.  There is some IO stuff, but it isn't needed.
+
+    I use a pretty loose naming style within the driver (rtd_blah).
+    All externally visible names should be rtd520_blah.
+    I use camelCase for structures (and inside them).
+    I may also use upper CamelCase for function names (old habit).
+
+    This board is somewhat related to the RTD PCI4400 board.
+
+    I borrowed heavily from the ni_mio_common, ni_atmio16d, mite, and
+    das1800, since they have the best documented code.  Driver
+    cb_pcidas64.c uses the same DMA controller.
+
+    As far as I can tell, the About interrupt doesnt work if Sample is
+    also enabled.  It turns out that About really isn't needed, since
+    we always count down samples read.
+
+    There was some timer/counter code, but it didn't follow the right API.
+
+*/
+
+/*
+  driver status:
+
+  Analog-In supports instruction and command mode.
+
+  With DMA, you can sample at 1.15Mhz with 70% idle on a 400Mhz K6-2
+  (single channel, 64K read buffer).  I get random system lockups when
+  using DMA with ALI-15xx based systems.  I haven't been able to test
+  any other chipsets.  The lockups happen soon after the start of an
+  acquistion, not in the middle of a long run.
+
+  Without DMA, you can do 620Khz sampling with 20% idle on a 400Mhz K6-2
+  (with a 256K read buffer).
+
+  Digital-IO and Analog-Out only support instruction mode.
+
+*/
+
+#include <linux/delay.h>
+
+#include "../comedidev.h"
+#include "comedi_pci.h"
+
+#define DRV_NAME "rtd520"
+
+/*======================================================================
+  Driver specific stuff (tunable)
+======================================================================*/
+/* Enable this to test the new DMA support. You may get hard lock ups */
+/*#define USE_DMA*/
+
+/* We really only need 2 buffers.  More than that means being much
+   smarter about knowing which ones are full. */
+#define DMA_CHAIN_COUNT 2	/* max DMA segments/buffers in a ring (min 2) */
+
+/* Target period for periodic transfers.  This sets the user read latency. */
+/* Note: There are certain rates where we give this up and transfer 1/2 FIFO */
+/* If this is too low, efficiency is poor */
+#define TRANS_TARGET_PERIOD 10000000	/* 10 ms (in nanoseconds) */
+
+/* Set a practical limit on how long a list to support (affects memory use) */
+/* The board support a channel list up to the FIFO length (1K or 8K) */
+#define RTD_MAX_CHANLIST	128	/* max channel list that we allow */
+
+/* tuning for ai/ao instruction done polling */
+#ifdef FAST_SPIN
+#define WAIT_QUIETLY		/* as nothing, spin on done bit */
+#define RTD_ADC_TIMEOUT	66000	/* 2 msec at 33mhz bus rate */
+#define RTD_DAC_TIMEOUT	66000
+#define RTD_DMA_TIMEOUT	33000	/* 1 msec */
+#else
+/* by delaying, power and electrical noise are reduced somewhat */
+#define WAIT_QUIETLY	comedi_udelay (1)
+#define RTD_ADC_TIMEOUT	2000	/* in usec */
+#define RTD_DAC_TIMEOUT	2000	/* in usec */
+#define RTD_DMA_TIMEOUT	1000	/* in usec */
+#endif
+
+/*======================================================================
+  Board specific stuff
+======================================================================*/
+
+/* registers  */
+#define PCI_VENDOR_ID_RTD	0x1435
+/*
+  The board has three memory windows: las0, las1, and lcfg (the PCI chip)
+  Las1 has the data and can be burst DMAed 32bits at a time.
+*/
+#define LCFG_PCIINDEX	0
+/* PCI region 1 is a 256 byte IO space mapping.  Use??? */
+#define LAS0_PCIINDEX	2	/* PCI memory resources */
+#define LAS1_PCIINDEX	3
+#define LCFG_PCISIZE	0x100
+#define LAS0_PCISIZE	0x200
+#define LAS1_PCISIZE	0x10
+
+#define RTD_CLOCK_RATE	8000000	/* 8Mhz onboard clock */
+#define RTD_CLOCK_BASE	125	/* clock period in ns */
+
+/* Note: these speed are slower than the spec, but fit the counter resolution*/
+#define RTD_MAX_SPEED	1625	/* when sampling, in nanoseconds */
+/* max speed if we don't have to wait for settling */
+#define RTD_MAX_SPEED_1	875	/* if single channel, in nanoseconds */
+
+#define RTD_MIN_SPEED	2097151875	/* (24bit counter) in nanoseconds */
+/* min speed when only 1 channel (no burst counter) */
+#define RTD_MIN_SPEED_1	5000000	/* 200Hz, in nanoseconds */
+
+#include "rtd520.h"
+#include "plx9080.h"
+
+/* Setup continuous ring of 1/2 FIFO transfers.  See RTD manual p91 */
+#define DMA_MODE_BITS (\
+		       PLX_LOCAL_BUS_16_WIDE_BITS \
+		       | PLX_DMA_EN_READYIN_BIT \
+		       | PLX_DMA_LOCAL_BURST_EN_BIT \
+		       | PLX_EN_CHAIN_BIT \
+		       | PLX_DMA_INTR_PCI_BIT \
+		       | PLX_LOCAL_ADDR_CONST_BIT \
+		       | PLX_DEMAND_MODE_BIT)
+
+#define DMA_TRANSFER_BITS (\
+/* descriptors in PCI memory*/ 	PLX_DESC_IN_PCI_BIT \
+/* interrupt at end of block */ | PLX_INTR_TERM_COUNT \
+/* from board to PCI */		| PLX_XFER_LOCAL_TO_PCI)
+
+/*======================================================================
+  Comedi specific stuff
+======================================================================*/
+
+/*
+  The board has 3 input modes and the gains of 1,2,4,...32 (, 64, 128)
+*/
+static const comedi_lrange rtd_ai_7520_range = { 18, {
+			/* +-5V input range gain steps */
+			BIP_RANGE(5.0),
+			BIP_RANGE(5.0 / 2),
+			BIP_RANGE(5.0 / 4),
+			BIP_RANGE(5.0 / 8),
+			BIP_RANGE(5.0 / 16),
+			BIP_RANGE(5.0 / 32),
+			/* +-10V input range gain steps */
+			BIP_RANGE(10.0),
+			BIP_RANGE(10.0 / 2),
+			BIP_RANGE(10.0 / 4),
+			BIP_RANGE(10.0 / 8),
+			BIP_RANGE(10.0 / 16),
+			BIP_RANGE(10.0 / 32),
+			/* +10V input range gain steps */
+			UNI_RANGE(10.0),
+			UNI_RANGE(10.0 / 2),
+			UNI_RANGE(10.0 / 4),
+			UNI_RANGE(10.0 / 8),
+			UNI_RANGE(10.0 / 16),
+			UNI_RANGE(10.0 / 32),
+
+	}
+};
+
+/* PCI4520 has two more gains (6 more entries) */
+static const comedi_lrange rtd_ai_4520_range = { 24, {
+			/* +-5V input range gain steps */
+			BIP_RANGE(5.0),
+			BIP_RANGE(5.0 / 2),
+			BIP_RANGE(5.0 / 4),
+			BIP_RANGE(5.0 / 8),
+			BIP_RANGE(5.0 / 16),
+			BIP_RANGE(5.0 / 32),
+			BIP_RANGE(5.0 / 64),
+			BIP_RANGE(5.0 / 128),
+			/* +-10V input range gain steps */
+			BIP_RANGE(10.0),
+			BIP_RANGE(10.0 / 2),
+			BIP_RANGE(10.0 / 4),
+			BIP_RANGE(10.0 / 8),
+			BIP_RANGE(10.0 / 16),
+			BIP_RANGE(10.0 / 32),
+			BIP_RANGE(10.0 / 64),
+			BIP_RANGE(10.0 / 128),
+			/* +10V input range gain steps */
+			UNI_RANGE(10.0),
+			UNI_RANGE(10.0 / 2),
+			UNI_RANGE(10.0 / 4),
+			UNI_RANGE(10.0 / 8),
+			UNI_RANGE(10.0 / 16),
+			UNI_RANGE(10.0 / 32),
+			UNI_RANGE(10.0 / 64),
+			UNI_RANGE(10.0 / 128),
+	}
+};
+
+/* Table order matches range values */
+static const comedi_lrange rtd_ao_range = { 4, {
+			RANGE(0, 5),
+			RANGE(0, 10),
+			RANGE(-5, 5),
+			RANGE(-10, 10),
+	}
+};
+
+/*
+  Board descriptions
+ */
+typedef struct rtdBoard_struct {
+	const char *name;	/* must be first */
+	int device_id;
+	int aiChans;
+	int aiBits;
+	int aiMaxGain;
+	int range10Start;	/* start of +-10V range */
+	int rangeUniStart;	/* start of +10V range */
+} rtdBoard;
+
+static const rtdBoard rtd520Boards[] = {
+	{
+	      name:	"DM7520",
+	      device_id:0x7520,
+	      aiChans:	16,
+	      aiBits:	12,
+	      aiMaxGain:32,
+	      range10Start:6,
+	      rangeUniStart:12,
+		},
+	{
+	      name:	"PCI4520",
+	      device_id:0x4520,
+	      aiChans:	16,
+	      aiBits:	12,
+	      aiMaxGain:128,
+	      range10Start:8,
+	      rangeUniStart:16,
+		},
+};
+
+static DEFINE_PCI_DEVICE_TABLE(rtd520_pci_table) = {
+	{PCI_VENDOR_ID_RTD, 0x7520, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_RTD, 0x4520, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{0}
+};
+
+MODULE_DEVICE_TABLE(pci, rtd520_pci_table);
+
+/*
+ * Useful for shorthand access to the particular board structure
+ */
+#define thisboard ((const rtdBoard *)dev->board_ptr)
+
+/*
+   This structure is for data unique to this hardware driver.
+   This is also unique for each board in the system.
+*/
+typedef struct {
+	/* memory mapped board structures */
+	void *las0;
+	void *las1;
+	void *lcfg;
+
+	unsigned long intCount;	/* interrupt count */
+	long aiCount;		/* total transfer size (samples) */
+	int transCount;		/* # to tranfer data. 0->1/2FIFO */
+	int flags;		/* flag event modes */
+
+	/* PCI device info */
+	struct pci_dev *pci_dev;
+	int got_regions;	/* non-zero if PCI regions owned */
+
+	/* channel list info */
+	/* chanBipolar tracks whether a channel is bipolar (and needs +2048) */
+	unsigned char chanBipolar[RTD_MAX_CHANLIST / 8];	/* bit array */
+
+	/* read back data */
+	lsampl_t aoValue[2];	/* Used for AO read back */
+
+	/* timer gate (when enabled) */
+	u8 utcGate[4];		/* 1 extra allows simple range check */
+
+	/* shadow registers affect other registers, but cant be read back */
+	/* The macros below update these on writes */
+	u16 intMask;		/* interrupt mask */
+	u16 intClearMask;	/* interrupt clear mask */
+	u8 utcCtrl[4];		/* crtl mode for 3 utc + read back */
+	u8 dioStatus;		/* could be read back (dio0Ctrl) */
+#ifdef USE_DMA
+	/* Always DMA 1/2 FIFO.  Buffer (dmaBuff?) is (at least) twice that size.
+	   After transferring, interrupt processes 1/2 FIFO and passes to comedi */
+	s16 dma0Offset;		/* current processing offset (0, 1/2) */
+	uint16_t *dma0Buff[DMA_CHAIN_COUNT];	/* DMA buffers (for ADC) */
+	dma_addr_t dma0BuffPhysAddr[DMA_CHAIN_COUNT];	/* physical addresses */
+	struct plx_dma_desc *dma0Chain;	/* DMA descriptor ring for dmaBuff */
+	dma_addr_t dma0ChainPhysAddr;	/* physical addresses */
+	/* shadow registers */
+	u8 dma0Control;
+	u8 dma1Control;
+#endif				/* USE_DMA */
+	unsigned fifoLen;
+} rtdPrivate;
+
+/* bit defines for "flags" */
+#define SEND_EOS	0x01	/* send End Of Scan events */
+#define DMA0_ACTIVE	0x02	/* DMA0 is active */
+#define DMA1_ACTIVE	0x04	/* DMA1 is active */
+
+/* Macros for accessing channel list bit array */
+#define CHAN_ARRAY_TEST(array,index) \
+	(((array)[(index)/8] >> ((index) & 0x7)) & 0x1)
+#define CHAN_ARRAY_SET(array,index) \
+	(((array)[(index)/8] |= 1 << ((index) & 0x7)))
+#define CHAN_ARRAY_CLEAR(array,index) \
+	(((array)[(index)/8] &= ~(1 << ((index) & 0x7))))
+
+/*
+ * most drivers define the following macro to make it easy to
+ * access the private structure.
+ */
+#define devpriv ((rtdPrivate *)dev->private)
+
+/* Macros to access registers */
+
+/* Reset board */
+#define RtdResetBoard(dev) \
+    writel (0, devpriv->las0+LAS0_BOARD_RESET)
+
+/* Reset channel gain table read pointer */
+#define RtdResetCGT(dev) \
+    writel (0, devpriv->las0+LAS0_CGT_RESET)
+
+/* Reset channel gain table read and write pointers */
+#define RtdClearCGT(dev) \
+    writel (0, devpriv->las0+LAS0_CGT_CLEAR)
+
+/* Reset channel gain table read and write pointers */
+#define RtdEnableCGT(dev,v) \
+    writel ((v > 0) ? 1 : 0, devpriv->las0+LAS0_CGT_ENABLE)
+
+/* Write channel gain table entry */
+#define RtdWriteCGTable(dev,v) \
+    writel (v, devpriv->las0+LAS0_CGT_WRITE)
+
+/* Write Channel Gain Latch */
+#define RtdWriteCGLatch(dev,v) \
+    writel (v, devpriv->las0+LAS0_CGL_WRITE)
+
+/* Reset ADC FIFO */
+#define RtdAdcClearFifo(dev) \
+    writel (0, devpriv->las0+LAS0_ADC_FIFO_CLEAR)
+
+/* Set ADC start conversion source select (write only) */
+#define RtdAdcConversionSource(dev,v) \
+    writel (v, devpriv->las0+LAS0_ADC_CONVERSION)
+
+/* Set burst start source select (write only) */
+#define RtdBurstStartSource(dev,v) \
+    writel (v, devpriv->las0+LAS0_BURST_START)
+
+/* Set Pacer start source select (write only) */
+#define RtdPacerStartSource(dev,v) \
+    writel (v, devpriv->las0+LAS0_PACER_START)
+
+/* Set Pacer stop source select (write only) */
+#define RtdPacerStopSource(dev,v) \
+    writel (v, devpriv->las0+LAS0_PACER_STOP)
+
+/* Set Pacer clock source select (write only) 0=external 1=internal */
+#define RtdPacerClockSource(dev,v) \
+    writel ((v > 0) ? 1 : 0, devpriv->las0+LAS0_PACER_SELECT)
+
+/* Set sample counter source select (write only) */
+#define RtdAdcSampleCounterSource(dev,v) \
+    writel (v, devpriv->las0+LAS0_ADC_SCNT_SRC)
+
+/* Set Pacer trigger mode select (write only) 0=single cycle, 1=repeat */
+#define RtdPacerTriggerMode(dev,v) \
+    writel ((v > 0) ? 1 : 0, devpriv->las0+LAS0_PACER_REPEAT)
+
+/* Set About counter stop enable (write only) */
+#define RtdAboutStopEnable(dev,v) \
+    writel ((v > 0) ? 1 : 0, devpriv->las0+LAS0_ACNT_STOP_ENABLE)
+
+/* Set external trigger polarity (write only) 0=positive edge, 1=negative */
+#define RtdTriggerPolarity(dev,v) \
+    writel ((v > 0) ? 1 : 0, devpriv->las0+LAS0_ETRG_POLARITY)
+
+/* Start single ADC conversion */
+#define RtdAdcStart(dev) \
+    writew (0, devpriv->las0+LAS0_ADC)
+
+/* Read one ADC data value (12bit (with sign extend) as 16bit) */
+/* Note: matches what DMA would get.  Actual value >> 3 */
+#define RtdAdcFifoGet(dev) \
+    readw (devpriv->las1+LAS1_ADC_FIFO)
+
+/* Read two ADC data values (DOESNT WORK) */
+#define RtdAdcFifoGet2(dev) \
+    readl (devpriv->las1+LAS1_ADC_FIFO)
+
+/* FIFO status */
+#define RtdFifoStatus(dev) \
+    readl (devpriv->las0+LAS0_ADC)
+
+/* pacer start/stop read=start, write=stop*/
+#define RtdPacerStart(dev) \
+    readl (devpriv->las0+LAS0_PACER)
+#define RtdPacerStop(dev) \
+    writel (0, devpriv->las0+LAS0_PACER)
+
+/* Interrupt status */
+#define RtdInterruptStatus(dev) \
+    readw (devpriv->las0+LAS0_IT)
+
+/* Interrupt mask */
+#define RtdInterruptMask(dev,v) \
+    writew ((devpriv->intMask = (v)),devpriv->las0+LAS0_IT)
+
+/* Interrupt status clear (only bits set in mask) */
+#define RtdInterruptClear(dev) \
+    readw (devpriv->las0+LAS0_CLEAR)
+
+/* Interrupt clear mask */
+#define RtdInterruptClearMask(dev,v) \
+    writew ((devpriv->intClearMask = (v)), devpriv->las0+LAS0_CLEAR)
+
+/* Interrupt overrun status */
+#define RtdInterruptOverrunStatus(dev) \
+    readl (devpriv->las0+LAS0_OVERRUN)
+
+/* Interrupt overrun clear */
+#define RtdInterruptOverrunClear(dev) \
+    writel (0, devpriv->las0+LAS0_OVERRUN)
+
+/* Pacer counter, 24bit */
+#define RtdPacerCount(dev) \
+    readl (devpriv->las0+LAS0_PCLK)
+#define RtdPacerCounter(dev,v) \
+    writel ((v) & 0xffffff,devpriv->las0+LAS0_PCLK)
+
+/* Burst counter, 10bit */
+#define RtdBurstCount(dev) \
+    readl (devpriv->las0+LAS0_BCLK)
+#define RtdBurstCounter(dev,v) \
+    writel ((v) & 0x3ff,devpriv->las0+LAS0_BCLK)
+
+/* Delay counter, 16bit */
+#define RtdDelayCount(dev) \
+    readl (devpriv->las0+LAS0_DCLK)
+#define RtdDelayCounter(dev,v) \
+    writel ((v) & 0xffff, devpriv->las0+LAS0_DCLK)
+
+/* About counter, 16bit */
+#define RtdAboutCount(dev) \
+    readl (devpriv->las0+LAS0_ACNT)
+#define RtdAboutCounter(dev,v) \
+    writel ((v) & 0xffff, devpriv->las0+LAS0_ACNT)
+
+/* ADC sample counter, 10bit */
+#define RtdAdcSampleCount(dev) \
+    readl (devpriv->las0+LAS0_ADC_SCNT)
+#define RtdAdcSampleCounter(dev,v) \
+    writel ((v) & 0x3ff, devpriv->las0+LAS0_ADC_SCNT)
+
+/* User Timer/Counter (8254) */
+#define RtdUtcCounterGet(dev,n) \
+    readb (devpriv->las0 \
+        + ((n <= 0) ? LAS0_UTC0 : ((1 == n) ? LAS0_UTC1 : LAS0_UTC2)))
+
+#define RtdUtcCounterPut(dev,n,v) \
+    writeb ((v) & 0xff, devpriv->las0 \
+        + ((n <= 0) ? LAS0_UTC0 : ((1 == n) ? LAS0_UTC1 : LAS0_UTC2)))
+
+/* Set UTC (8254) control byte  */
+#define RtdUtcCtrlPut(dev,n,v) \
+    writeb (devpriv->utcCtrl[(n) & 3] = (((n) & 3) << 6) | ((v) & 0x3f), \
+      devpriv->las0 + LAS0_UTC_CTRL)
+
+/* Set UTCn clock source (write only) */
+#define RtdUtcClockSource(dev,n,v) \
+    writew (v, devpriv->las0 \
+        + ((n <= 0) ? LAS0_UTC0_CLOCK : \
+           ((1 == n) ? LAS0_UTC1_CLOCK : LAS0_UTC2_CLOCK)))
+
+/* Set UTCn gate source (write only) */
+#define RtdUtcGateSource(dev,n,v) \
+    writew (v, devpriv->las0 \
+        + ((n <= 0) ? LAS0_UTC0_GATE : \
+           ((1 == n) ? LAS0_UTC1_GATE : LAS0_UTC2_GATE)))
+
+/* User output N source select (write only) */
+#define RtdUsrOutSource(dev,n,v) \
+    writel (v,devpriv->las0+((n <= 0) ? LAS0_UOUT0_SELECT : LAS0_UOUT1_SELECT))
+
+/* Digital IO */
+#define RtdDio0Read(dev) \
+    (readw (devpriv->las0+LAS0_DIO0) & 0xff)
+#define RtdDio0Write(dev,v) \
+    writew ((v) & 0xff, devpriv->las0+LAS0_DIO0)
+
+#define RtdDio1Read(dev) \
+    (readw (devpriv->las0+LAS0_DIO1) & 0xff)
+#define RtdDio1Write(dev,v) \
+    writew ((v) & 0xff, devpriv->las0+LAS0_DIO1)
+
+#define RtdDioStatusRead(dev) \
+    (readw (devpriv->las0+LAS0_DIO_STATUS) & 0xff)
+#define RtdDioStatusWrite(dev,v) \
+    writew ((devpriv->dioStatus = (v)), devpriv->las0+LAS0_DIO_STATUS)
+
+#define RtdDio0CtrlRead(dev) \
+    (readw (devpriv->las0+LAS0_DIO0_CTRL) & 0xff)
+#define RtdDio0CtrlWrite(dev,v) \
+    writew ((v) & 0xff, devpriv->las0+LAS0_DIO0_CTRL)
+
+/* Digital to Analog converter */
+/* Write one data value (sign + 12bit + marker bits) */
+/* Note: matches what DMA would put.  Actual value << 3 */
+#define RtdDacFifoPut(dev,n,v) \
+    writew ((v), devpriv->las1 +(((n) == 0) ? LAS1_DAC1_FIFO : LAS1_DAC2_FIFO))
+
+/* Start single DAC conversion */
+#define RtdDacUpdate(dev,n) \
+    writew (0, devpriv->las0 +(((n) == 0) ? LAS0_DAC1 : LAS0_DAC2))
+
+/* Start single DAC conversion on both DACs */
+#define RtdDacBothUpdate(dev) \
+    writew (0, devpriv->las0+LAS0_DAC)
+
+/* Set DAC output type and range */
+#define RtdDacRange(dev,n,v) \
+    writew ((v) & 7, devpriv->las0 \
+	+(((n) == 0) ? LAS0_DAC1_CTRL : LAS0_DAC2_CTRL))
+
+/* Reset DAC FIFO */
+#define RtdDacClearFifo(dev,n) \
+    writel (0, devpriv->las0+(((n) == 0) ? LAS0_DAC1_RESET : LAS0_DAC2_RESET))
+
+/* Set source for DMA 0 (write only, shadow?) */
+#define RtdDma0Source(dev,n) \
+    writel ((n) & 0xf, devpriv->las0+LAS0_DMA0_SRC)
+
+/* Set source for DMA 1 (write only, shadow?) */
+#define RtdDma1Source(dev,n) \
+    writel ((n) & 0xf, devpriv->las0+LAS0_DMA1_SRC)
+
+/* Reset board state for DMA 0 */
+#define RtdDma0Reset(dev) \
+    writel (0, devpriv->las0+LAS0_DMA0_RESET)
+
+/* Reset board state for DMA 1 */
+#define RtdDma1Reset(dev) \
+    writel (0, devpriv->las0+LAS0_DMA1_SRC)
+
+/* PLX9080 interrupt mask and status */
+#define RtdPlxInterruptRead(dev) \
+    readl (devpriv->lcfg+LCFG_ITCSR)
+#define RtdPlxInterruptWrite(dev,v) \
+    writel (v, devpriv->lcfg+LCFG_ITCSR)
+
+/* Set  mode for DMA 0 */
+#define RtdDma0Mode(dev,m) \
+    writel ((m), devpriv->lcfg+LCFG_DMAMODE0)
+
+/* Set PCI address for DMA 0 */
+#define RtdDma0PciAddr(dev,a) \
+    writel ((a), devpriv->lcfg+LCFG_DMAPADR0)
+
+/* Set local address for DMA 0 */
+#define RtdDma0LocalAddr(dev,a) \
+    writel ((a), devpriv->lcfg+LCFG_DMALADR0)
+
+/* Set byte count for DMA 0 */
+#define RtdDma0Count(dev,c) \
+    writel ((c), devpriv->lcfg+LCFG_DMASIZ0)
+
+/* Set next descriptor for DMA 0 */
+#define RtdDma0Next(dev,a) \
+    writel ((a), devpriv->lcfg+LCFG_DMADPR0)
+
+/* Set  mode for DMA 1 */
+#define RtdDma1Mode(dev,m) \
+    writel ((m), devpriv->lcfg+LCFG_DMAMODE1)
+
+/* Set PCI address for DMA 1 */
+#define RtdDma1PciAddr(dev,a) \
+    writel ((a), devpriv->lcfg+LCFG_DMAADR1)
+
+/* Set local address for DMA 1 */
+#define RtdDma1LocalAddr(dev,a) \
+    writel ((a), devpriv->lcfg+LCFG_DMALADR1)
+
+/* Set byte count for DMA 1 */
+#define RtdDma1Count(dev,c) \
+    writel ((c), devpriv->lcfg+LCFG_DMASIZ1)
+
+/* Set next descriptor for DMA 1 */
+#define RtdDma1Next(dev,a) \
+    writel ((a), devpriv->lcfg+LCFG_DMADPR1)
+
+/* Set control for DMA 0 (write only, shadow?) */
+#define RtdDma0Control(dev,n) \
+    writeb (devpriv->dma0Control = (n), devpriv->lcfg+LCFG_DMACSR0)
+
+/* Get status for DMA 0 */
+#define RtdDma0Status(dev) \
+    readb (devpriv->lcfg+LCFG_DMACSR0)
+
+/* Set control for DMA 1 (write only, shadow?) */
+#define RtdDma1Control(dev,n) \
+    writeb (devpriv->dma1Control = (n), devpriv->lcfg+LCFG_DMACSR1)
+
+/* Get status for DMA 1 */
+#define RtdDma1Status(dev) \
+    readb (devpriv->lcfg+LCFG_DMACSR1)
+
+/*
+ * The comedi_driver structure tells the Comedi core module
+ * which functions to call to configure/deconfigure (attac/detach)
+ * the board, and also about the kernel module that contains
+ * the device code.
+ */
+static int rtd_attach(comedi_device * dev, comedi_devconfig * it);
+static int rtd_detach(comedi_device * dev);
+
+static comedi_driver rtd520Driver = {
+      driver_name: DRV_NAME,
+      module:THIS_MODULE,
+      attach:rtd_attach,
+      detach:rtd_detach,
+};
+
+static int rtd_ai_rinsn(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data);
+static int rtd_ao_winsn(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data);
+static int rtd_ao_rinsn(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data);
+static int rtd_dio_insn_bits(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data);
+static int rtd_dio_insn_config(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data);
+static int rtd_ai_cmdtest(comedi_device * dev, comedi_subdevice * s,
+	comedi_cmd * cmd);
+static int rtd_ai_cmd(comedi_device * dev, comedi_subdevice * s);
+static int rtd_ai_cancel(comedi_device * dev, comedi_subdevice * s);
+//static int rtd_ai_poll (comedi_device *dev,comedi_subdevice *s);
+static int rtd_ns_to_timer(unsigned int *ns, int roundMode);
+static irqreturn_t rtd_interrupt(int irq, void *d PT_REGS_ARG);
+static int rtd520_probe_fifo_depth(comedi_device *dev);
+
+/*
+ * Attach is called by the Comedi core to configure the driver
+ * for a particular board.  If you specified a board_name array
+ * in the driver structure, dev->board_ptr contains that
+ * address.
+ */
+static int rtd_attach(comedi_device * dev, comedi_devconfig * it)
+{				/* board name and options flags */
+	comedi_subdevice *s;
+	struct pci_dev *pcidev;
+	int ret;
+	resource_size_t physLas0;	/* configuation */
+	resource_size_t physLas1;	/* data area */
+	resource_size_t physLcfg;	/* PLX9080 */
+#ifdef USE_DMA
+	int index;
+#endif
+
+	printk("comedi%d: rtd520 attaching.\n", dev->minor);
+
+#if defined (CONFIG_COMEDI_DEBUG) && defined (USE_DMA)
+	/* You can set this a load time: modprobe comedi comedi_debug=1 */
+	if (0 == comedi_debug)	/* force DMA debug printks */
+		comedi_debug = 1;
+#endif
+
+	/*
+	 * Allocate the private structure area.  alloc_private() is a
+	 * convenient macro defined in comedidev.h.
+	 */
+	if (alloc_private(dev, sizeof(rtdPrivate)) < 0)
+		return -ENOMEM;
+
+	/*
+	 * Probe the device to determine what device in the series it is.
+	 */
+	for (pcidev = pci_get_device(PCI_VENDOR_ID_RTD, PCI_ANY_ID, NULL);
+		pcidev != NULL;
+		pcidev = pci_get_device(PCI_VENDOR_ID_RTD, PCI_ANY_ID, pcidev)) {
+		int i;
+
+		if (it->options[0] || it->options[1]) {
+			if (pcidev->bus->number != it->options[0]
+				|| PCI_SLOT(pcidev->devfn) !=
+				it->options[1]) {
+				continue;
+			}
+		}
+		for(i = 0; i < sizeof(rtd520Boards) / sizeof(rtd520Boards[0]); ++i)
+		{
+			if(pcidev->device == rtd520Boards[i].device_id)
+			{
+				dev->board_ptr = &rtd520Boards[i];
+				break;
+			}
+		}
+		if(dev->board_ptr) break;	/* found one */
+	}
+	if (!pcidev) {
+		if (it->options[0] && it->options[1]) {
+			printk("No RTD card at bus=%d slot=%d.\n",
+				it->options[0], it->options[1]);
+		} else {
+			printk("No RTD card found.\n");
+		}
+		return -EIO;
+	}
+	devpriv->pci_dev = pcidev;
+	dev->board_name = thisboard->name;
+
+	if ((ret = comedi_pci_enable(pcidev, DRV_NAME)) < 0) {
+		printk("Failed to enable PCI device and request regions.\n");
+		return ret;
+	}
+	devpriv->got_regions = 1;
+
+	/*
+	 * Initialize base addresses
+	 */
+	/* Get the physical address from PCI config */
+	physLas0 = pci_resource_start(devpriv->pci_dev, LAS0_PCIINDEX);
+	physLas1 = pci_resource_start(devpriv->pci_dev, LAS1_PCIINDEX);
+	physLcfg = pci_resource_start(devpriv->pci_dev, LCFG_PCIINDEX);
+	/* Now have the kernel map this into memory */
+	/* ASSUME page aligned */
+	devpriv->las0 = ioremap_nocache(physLas0, LAS0_PCISIZE);
+	devpriv->las1 = ioremap_nocache(physLas1, LAS1_PCISIZE);
+	devpriv->lcfg = ioremap_nocache(physLcfg, LCFG_PCISIZE);
+
+	if (!devpriv->las0 || !devpriv->las1 || !devpriv->lcfg) {
+		return -ENOMEM;
+	}
+
+	DPRINTK("%s: LAS0=%llx, LAS1=%llx, CFG=%llx.\n", dev->board_name,
+		(unsigned long long)physLas0, (unsigned long long)physLas1,
+		(unsigned long long)physLcfg);
+	{			/* The RTD driver does this */
+		unsigned char pci_latency;
+		u16 revision;
+		/*uint32_t epld_version; */
+
+		pci_read_config_word(devpriv->pci_dev, PCI_REVISION_ID,
+			&revision);
+		DPRINTK("%s: PCI revision %d.\n", dev->board_name, revision);
+
+		pci_read_config_byte(devpriv->pci_dev,
+			PCI_LATENCY_TIMER, &pci_latency);
+		if (pci_latency < 32) {
+			printk("%s: PCI latency changed from %d to %d\n",
+				dev->board_name, pci_latency, 32);
+			pci_write_config_byte(devpriv->pci_dev,
+				PCI_LATENCY_TIMER, 32);
+		} else {
+			DPRINTK("rtd520: PCI latency = %d\n", pci_latency);
+		}
+
+		/* Undocumented EPLD version (doesnt match RTD driver results) */
+		/*DPRINTK ("rtd520: Reading epld from %p\n",
+		   devpriv->las0+0);
+		   epld_version = readl (devpriv->las0+0);
+		   if ((epld_version & 0xF0) >> 4 == 0x0F) {
+		   DPRINTK("rtd520: pre-v8 EPLD. (%x)\n", epld_version);
+		   } else {
+		   DPRINTK("rtd520: EPLD version %x.\n", epld_version >> 4);
+		   } */
+	}
+
+	/* Show board configuration */
+	printk("%s:", dev->board_name);
+
+	/*
+	 * Allocate the subdevice structures.  alloc_subdevice() is a
+	 * convenient macro defined in comedidev.h.
+	 */
+	if (alloc_subdevices(dev, 4) < 0) {
+		return -ENOMEM;
+	}
+
+	s = dev->subdevices + 0;
+	dev->read_subdev = s;
+	/* analog input subdevice */
+	s->type = COMEDI_SUBD_AI;
+	s->subdev_flags =
+		SDF_READABLE | SDF_GROUND | SDF_COMMON | SDF_DIFF |
+		SDF_CMD_READ;
+	s->n_chan = thisboard->aiChans;
+	s->maxdata = (1 << thisboard->aiBits) - 1;
+	if (thisboard->aiMaxGain <= 32) {
+		s->range_table = &rtd_ai_7520_range;
+	} else {
+		s->range_table = &rtd_ai_4520_range;
+	}
+	s->len_chanlist = RTD_MAX_CHANLIST;	/* devpriv->fifoLen */
+	s->insn_read = rtd_ai_rinsn;
+	s->do_cmd = rtd_ai_cmd;
+	s->do_cmdtest = rtd_ai_cmdtest;
+	s->cancel = rtd_ai_cancel;
+	/*s->poll = rtd_ai_poll; *//* not ready yet */
+
+	s = dev->subdevices + 1;
+	/* analog output subdevice */
+	s->type = COMEDI_SUBD_AO;
+	s->subdev_flags = SDF_WRITABLE;
+	s->n_chan = 2;
+	s->maxdata = (1 << thisboard->aiBits) - 1;
+	s->range_table = &rtd_ao_range;
+	s->insn_write = rtd_ao_winsn;
+	s->insn_read = rtd_ao_rinsn;
+
+	s = dev->subdevices + 2;
+	/* digital i/o subdevice */
+	s->type = COMEDI_SUBD_DIO;
+	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+	/* we only support port 0 right now.  Ignoring port 1 and user IO */
+	s->n_chan = 8;
+	s->maxdata = 1;
+	s->range_table = &range_digital;
+	s->insn_bits = rtd_dio_insn_bits;
+	s->insn_config = rtd_dio_insn_config;
+
+	/* timer/counter subdevices (not currently supported) */
+	s = dev->subdevices + 3;
+	s->type = COMEDI_SUBD_COUNTER;
+	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+	s->n_chan = 3;
+	s->maxdata = 0xffff;
+
+	/* initialize board, per RTD spec */
+	/* also, initialize shadow registers */
+	RtdResetBoard(dev);
+	comedi_udelay(100);	/* needed? */
+	RtdPlxInterruptWrite(dev, 0);
+	RtdInterruptMask(dev, 0);	/* and sets shadow */
+	RtdInterruptClearMask(dev, ~0);	/* and sets shadow */
+	RtdInterruptClear(dev);	/* clears bits set by mask */
+	RtdInterruptOverrunClear(dev);
+	RtdClearCGT(dev);
+	RtdAdcClearFifo(dev);
+	RtdDacClearFifo(dev, 0);
+	RtdDacClearFifo(dev, 1);
+	/* clear digital IO fifo */
+	RtdDioStatusWrite(dev, 0);	/* safe state, set shadow */
+	RtdUtcCtrlPut(dev, 0, 0x30);	/* safe state, set shadow */
+	RtdUtcCtrlPut(dev, 1, 0x30);	/* safe state, set shadow */
+	RtdUtcCtrlPut(dev, 2, 0x30);	/* safe state, set shadow */
+	RtdUtcCtrlPut(dev, 3, 0);	/* safe state, set shadow */
+	/* TODO: set user out source ??? */
+
+	/* check if our interrupt is available and get it */
+	if ((ret = comedi_request_irq(devpriv->pci_dev->irq, rtd_interrupt,
+				IRQF_SHARED, DRV_NAME, dev)) < 0) {
+		printk("Could not get interrupt! (%u)\n",
+			devpriv->pci_dev->irq);
+		return ret;
+	}
+	dev->irq = devpriv->pci_dev->irq;
+	printk("( irq=%u )", dev->irq);
+
+	ret = rtd520_probe_fifo_depth(dev);
+	if(ret < 0) {
+		return ret;
+	}
+	devpriv->fifoLen = ret;
+	printk("( fifoLen=%d )", devpriv->fifoLen);
+
+#ifdef USE_DMA
+	if (dev->irq > 0) {
+		printk("( DMA buff=%d )\n", DMA_CHAIN_COUNT);
+		/* The PLX9080 has 2 DMA controllers, but there could be 4 sources:
+		   ADC, digital, DAC1, and DAC2.  Since only the ADC supports cmd mode
+		   right now, this isn't an issue (yet) */
+		devpriv->dma0Offset = 0;
+
+		for (index = 0; index < DMA_CHAIN_COUNT; index++) {
+			devpriv->dma0Buff[index] =
+				pci_alloc_consistent(devpriv->pci_dev,
+				sizeof(u16) * devpriv->fifoLen / 2,
+				&devpriv->dma0BuffPhysAddr[index]);
+			if (devpriv->dma0Buff[index] == NULL) {
+				ret = -ENOMEM;
+				goto rtd_attach_die_error;
+			}
+			/*DPRINTK ("buff[%d] @ %p virtual, %x PCI\n",
+			   index,
+			   devpriv->dma0Buff[index], devpriv->dma0BuffPhysAddr[index]); */
+		}
+
+		/* setup DMA descriptor ring (use cpu_to_le32 for byte ordering?) */
+		devpriv->dma0Chain =
+			pci_alloc_consistent(devpriv->pci_dev,
+			sizeof(struct plx_dma_desc) * DMA_CHAIN_COUNT,
+			&devpriv->dma0ChainPhysAddr);
+		for (index = 0; index < DMA_CHAIN_COUNT; index++) {
+			devpriv->dma0Chain[index].pci_start_addr =
+				devpriv->dma0BuffPhysAddr[index];
+			devpriv->dma0Chain[index].local_start_addr =
+				DMALADDR_ADC;
+			devpriv->dma0Chain[index].transfer_size =
+				sizeof(u16) * devpriv->fifoLen / 2;
+			devpriv->dma0Chain[index].next =
+				(devpriv->dma0ChainPhysAddr + ((index +
+						1) % (DMA_CHAIN_COUNT))
+				* sizeof(devpriv->dma0Chain[0]))
+				| DMA_TRANSFER_BITS;
+			/*DPRINTK ("ring[%d] @%lx PCI: %x, local: %x, N: 0x%x, next: %x\n",
+			   index,
+			   ((long)devpriv->dma0ChainPhysAddr
+			   + (index * sizeof(devpriv->dma0Chain[0]))),
+			   devpriv->dma0Chain[index].pci_start_addr,
+			   devpriv->dma0Chain[index].local_start_addr,
+			   devpriv->dma0Chain[index].transfer_size,
+			   devpriv->dma0Chain[index].next); */
+		}
+
+		if (devpriv->dma0Chain == NULL) {
+			ret = -ENOMEM;
+			goto rtd_attach_die_error;
+		}
+
+		RtdDma0Mode(dev, DMA_MODE_BITS);
+		RtdDma0Source(dev, DMAS_ADFIFO_HALF_FULL);	/* set DMA trigger source */
+	} else {
+		printk("( no IRQ->no DMA )");
+	}
+#endif /* USE_DMA */
+
+	if (dev->irq) {		/* enable plx9080 interrupts */
+		RtdPlxInterruptWrite(dev, ICS_PIE | ICS_PLIE);
+	}
+
+	printk("\ncomedi%d: rtd520 driver attached.\n", dev->minor);
+
+	return 1;
+
+#if 0
+	/* hit an error, clean up memory and return ret */
+//rtd_attach_die_error:
+#ifdef USE_DMA
+	for (index = 0; index < DMA_CHAIN_COUNT; index++) {
+		if (NULL != devpriv->dma0Buff[index]) {	/* free buffer memory */
+			pci_free_consistent(devpriv->pci_dev,
+				sizeof(u16) * devpriv->fifoLen / 2,
+				devpriv->dma0Buff[index],
+				devpriv->dma0BuffPhysAddr[index]);
+			devpriv->dma0Buff[index] = NULL;
+		}
+	}
+	if (NULL != devpriv->dma0Chain) {
+		pci_free_consistent(devpriv->pci_dev,
+			sizeof(struct plx_dma_desc)
+			* DMA_CHAIN_COUNT,
+			devpriv->dma0Chain, devpriv->dma0ChainPhysAddr);
+		devpriv->dma0Chain = NULL;
+	}
+#endif /* USE_DMA */
+	/* subdevices and priv are freed by the core */
+	if (dev->irq) {
+		/* disable interrupt controller */
+		RtdPlxInterruptWrite(dev, RtdPlxInterruptRead(dev)
+			& ~(ICS_PLIE | ICS_DMA0_E | ICS_DMA1_E));
+		comedi_free_irq(dev->irq, dev);
+	}
+
+	/* release all regions that were allocated */
+	if (devpriv->las0) {
+		iounmap(devpriv->las0);
+	}
+	if (devpriv->las1) {
+		iounmap(devpriv->las1);
+	}
+	if (devpriv->lcfg) {
+		iounmap(devpriv->lcfg);
+	}
+	if (devpriv->pci_dev) {
+		pci_dev_put(devpriv->pci_dev);
+	}
+	return ret;
+#endif
+}
+
+/*
+ * _detach is called to deconfigure a device.  It should deallocate
+ * resources.
+ * This function is also called when _attach() fails, so it should be
+ * careful not to release resources that were not necessarily
+ * allocated by _attach().  dev->private and dev->subdevices are
+ * deallocated automatically by the core.
+ */
+static int rtd_detach(comedi_device * dev)
+{
+#ifdef USE_DMA
+	int index;
+#endif
+
+	DPRINTK("comedi%d: rtd520: removing (%ld ints)\n",
+		dev->minor, (devpriv ? devpriv->intCount : 0L));
+	if (devpriv && devpriv->lcfg) {
+		DPRINTK("(int status 0x%x, overrun status 0x%x, fifo status 0x%x)...\n", 0xffff & RtdInterruptStatus(dev), 0xffff & RtdInterruptOverrunStatus(dev), (0xffff & RtdFifoStatus(dev)) ^ 0x6666);
+	}
+
+	if (devpriv) {
+		/* Shut down any board ops by resetting it */
+#ifdef USE_DMA
+		if (devpriv->lcfg) {
+			RtdDma0Control(dev, 0);	/* disable DMA */
+			RtdDma1Control(dev, 0);	/* disable DMA */
+			RtdPlxInterruptWrite(dev, ICS_PIE | ICS_PLIE);
+		}
+#endif /* USE_DMA */
+		if (devpriv->las0) {
+			RtdResetBoard(dev);
+			RtdInterruptMask(dev, 0);
+			RtdInterruptClearMask(dev, ~0);
+			RtdInterruptClear(dev);	/* clears bits set by mask */
+		}
+#ifdef USE_DMA
+		/* release DMA */
+		for (index = 0; index < DMA_CHAIN_COUNT; index++) {
+			if (NULL != devpriv->dma0Buff[index]) {
+				pci_free_consistent(devpriv->pci_dev,
+					sizeof(u16) * devpriv->fifoLen / 2,
+					devpriv->dma0Buff[index],
+					devpriv->dma0BuffPhysAddr[index]);
+				devpriv->dma0Buff[index] = NULL;
+			}
+		}
+		if (NULL != devpriv->dma0Chain) {
+			pci_free_consistent(devpriv->pci_dev,
+				sizeof(struct plx_dma_desc) * DMA_CHAIN_COUNT,
+				devpriv->dma0Chain, devpriv->dma0ChainPhysAddr);
+			devpriv->dma0Chain = NULL;
+		}
+#endif /* USE_DMA */
+
+		/* release IRQ */
+		if (dev->irq) {
+			/* disable interrupt controller */
+			RtdPlxInterruptWrite(dev, RtdPlxInterruptRead(dev)
+				& ~(ICS_PLIE | ICS_DMA0_E | ICS_DMA1_E));
+			comedi_free_irq(dev->irq, dev);
+		}
+
+		/* release all regions that were allocated */
+		if (devpriv->las0) {
+			iounmap(devpriv->las0);
+		}
+		if (devpriv->las1) {
+			iounmap(devpriv->las1);
+		}
+		if (devpriv->lcfg) {
+			iounmap(devpriv->lcfg);
+		}
+		if (devpriv->pci_dev) {
+			if (devpriv->got_regions) {
+				comedi_pci_disable(devpriv->pci_dev);
+			}
+			pci_dev_put(devpriv->pci_dev);
+		}
+	}
+
+	printk("comedi%d: rtd520: removed.\n", dev->minor);
+
+	return 0;
+}
+
+/*
+  Convert a single comedi channel-gain entry to a RTD520 table entry
+*/
+static unsigned short rtdConvertChanGain(comedi_device * dev,
+	unsigned int comediChan, int chanIndex)
+{				/* index in channel list */
+	unsigned int chan, range, aref;
+	unsigned short r = 0;
+
+	chan = CR_CHAN(comediChan);
+	range = CR_RANGE(comediChan);
+	aref = CR_AREF(comediChan);
+
+	r |= chan & 0xf;
+
+	/* Note: we also setup the channel list bipolar flag array */
+	if (range < thisboard->range10Start) {	/* first batch are +-5 */
+		r |= 0x000;	/* +-5 range */
+		r |= (range & 0x7) << 4;	/* gain */
+		CHAN_ARRAY_SET(devpriv->chanBipolar, chanIndex);
+	} else if (range < thisboard->rangeUniStart) {	/* second batch are +-10 */
+		r |= 0x100;	/* +-10 range */
+		r |= ((range - thisboard->range10Start) & 0x7) << 4;	/* gain */
+		CHAN_ARRAY_SET(devpriv->chanBipolar, chanIndex);
+	} else {		/* last batch is +10 */
+		r |= 0x200;	/* +10 range */
+		r |= ((range - thisboard->rangeUniStart) & 0x7) << 4;	/* gain */
+		CHAN_ARRAY_CLEAR(devpriv->chanBipolar, chanIndex);
+	}
+
+	switch (aref) {
+	case AREF_GROUND:	/* on-board ground */
+		break;
+
+	case AREF_COMMON:
+		r |= 0x80;	/* ref external analog common */
+		break;
+
+	case AREF_DIFF:
+		r |= 0x400;	/* differential inputs */
+		break;
+
+	case AREF_OTHER:	/* ??? */
+		break;
+	}
+	/*printk ("chan=%d r=%d a=%d -> 0x%x\n",
+	   chan, range, aref, r); */
+	return r;
+}
+
+/*
+  Setup the channel-gain table from a comedi list
+*/
+static void rtd_load_channelgain_list(comedi_device * dev,
+	unsigned int n_chan, unsigned int *list)
+{
+	if (n_chan > 1) {	/* setup channel gain table */
+		int ii;
+		RtdClearCGT(dev);
+		RtdEnableCGT(dev, 1);	/* enable table */
+		for (ii = 0; ii < n_chan; ii++) {
+			RtdWriteCGTable(dev, rtdConvertChanGain(dev, list[ii],
+					ii));
+		}
+	} else {		/* just use the channel gain latch */
+		RtdEnableCGT(dev, 0);	/* disable table, enable latch */
+		RtdWriteCGLatch(dev, rtdConvertChanGain(dev, list[0], 0));
+	}
+}
+
+/* determine fifo size by doing adc conversions until the fifo half
+empty status flag clears */
+static int rtd520_probe_fifo_depth(comedi_device *dev)
+{
+	lsampl_t chanspec = CR_PACK(0, 0, AREF_GROUND);
+	unsigned i;
+	static const unsigned limit = 0x2000;
+	unsigned fifo_size = 0;
+
+	RtdAdcClearFifo(dev);
+	rtd_load_channelgain_list(dev, 1, &chanspec);
+	RtdAdcConversionSource(dev, 0);	/* software */
+	/* convert  samples */
+	for (i = 0; i < limit; ++i) {
+		unsigned fifo_status;
+		/* trigger conversion */
+		RtdAdcStart(dev);
+		comedi_udelay(1);
+		fifo_status = RtdFifoStatus(dev);
+		if((fifo_status & FS_ADC_HEMPTY) == 0) {
+			fifo_size = 2 * i;
+			break;
+		}
+	}
+	if(i == limit)
+	{
+		rt_printk("\ncomedi: %s: failed to probe fifo size.\n", DRV_NAME);
+		return -EIO;
+	}
+	RtdAdcClearFifo(dev);
+	if(fifo_size != 0x400 || fifo_size != 0x2000)
+	{
+		rt_printk("\ncomedi: %s: unexpected fifo size of %i, expected 1024 or 8192.\n",
+			DRV_NAME, fifo_size);
+		return -EIO;
+	}
+	return fifo_size;
+}
+
+/*
+  "instructions" read/write data in "one-shot" or "software-triggered"
+  mode (simplest case).
+  This doesnt use interrupts.
+
+  Note, we don't do any settling delays.  Use a instruction list to
+  select, delay, then read.
+ */
+static int rtd_ai_rinsn(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data)
+{
+	int n, ii;
+	int stat;
+
+	/* clear any old fifo data */
+	RtdAdcClearFifo(dev);
+
+	/* write channel to multiplexer and clear channel gain table */
+	rtd_load_channelgain_list(dev, 1, &insn->chanspec);
+
+	/* set conversion source */
+	RtdAdcConversionSource(dev, 0);	/* software */
+
+	/* convert n samples */
+	for (n = 0; n < insn->n; n++) {
+		s16 d;
+		/* trigger conversion */
+		RtdAdcStart(dev);
+
+		for (ii = 0; ii < RTD_ADC_TIMEOUT; ++ii) {
+			stat = RtdFifoStatus(dev);
+			if (stat & FS_ADC_NOT_EMPTY)	/* 1 -> not empty */
+				break;
+			WAIT_QUIETLY;
+		}
+		if (ii >= RTD_ADC_TIMEOUT) {
+			DPRINTK("rtd520: Error: ADC never finished! FifoStatus=0x%x\n", stat ^ 0x6666);
+			return -ETIMEDOUT;
+		}
+
+		/* read data */
+		d = RtdAdcFifoGet(dev);	/* get 2s comp value */
+		/*printk ("rtd520: Got 0x%x after %d usec\n", d, ii+1); */
+		d = d >> 3;	/* low 3 bits are marker lines */
+		if (CHAN_ARRAY_TEST(devpriv->chanBipolar, 0)) {
+			data[n] = d + 2048;	/* convert to comedi unsigned data */
+		} else {
+			data[n] = d;
+		}
+	}
+
+	/* return the number of samples read/written */
+	return n;
+}
+
+/*
+  Get what we know is there.... Fast!
+  This uses 1/2 the bus cycles of read_dregs (below).
+
+  The manual claims that we can do a lword read, but it doesn't work here.
+*/
+static int ai_read_n(comedi_device * dev, comedi_subdevice * s, int count)
+{
+	int ii;
+
+	for (ii = 0; ii < count; ii++) {
+		sampl_t sample;
+		s16 d;
+
+		if (0 == devpriv->aiCount) {	/* done */
+			d = RtdAdcFifoGet(dev);	/* Read N and discard */
+			continue;
+		}
+#if 0
+		if (0 == (RtdFifoStatus(dev) & FS_ADC_NOT_EMPTY)) {	/* DEBUG */
+			DPRINTK("comedi: READ OOPS on %d of %d\n", ii + 1,
+				count);
+			break;
+		}
+#endif
+		d = RtdAdcFifoGet(dev);	/* get 2s comp value */
+
+		d = d >> 3;	/* low 3 bits are marker lines */
+		if (CHAN_ARRAY_TEST(devpriv->chanBipolar, s->async->cur_chan)) {
+			sample = d + 2048;	/* convert to comedi unsigned data */
+		} else {
+			sample = d;
+		}
+		if (!comedi_buf_put(s->async, sample))
+			return -1;
+
+		if (devpriv->aiCount > 0)	/* < 0, means read forever */
+			devpriv->aiCount--;
+	}
+	return 0;
+}
+
+/*
+  unknown amout of data is waiting in fifo.
+*/
+static int ai_read_dregs(comedi_device * dev, comedi_subdevice * s)
+{
+	while (RtdFifoStatus(dev) & FS_ADC_NOT_EMPTY) {	/* 1 -> not empty */
+		sampl_t sample;
+		s16 d = RtdAdcFifoGet(dev);	/* get 2s comp value */
+
+		if (0 == devpriv->aiCount) {	/* done */
+			continue;	/* read rest */
+		}
+
+		d = d >> 3;	/* low 3 bits are marker lines */
+		if (CHAN_ARRAY_TEST(devpriv->chanBipolar, s->async->cur_chan)) {
+			sample = d + 2048;	/* convert to comedi unsigned data */
+		} else {
+			sample = d;
+		}
+		if (!comedi_buf_put(s->async, sample))
+			return -1;
+
+		if (devpriv->aiCount > 0)	/* < 0, means read forever */
+			devpriv->aiCount--;
+	}
+	return 0;
+}
+
+#ifdef USE_DMA
+/*
+  Terminate a DMA transfer and wait for everything to quiet down
+*/
+void abort_dma(comedi_device * dev, unsigned int channel)
+{				/* DMA channel 0, 1 */
+	unsigned long dma_cs_addr;	/* the control/status register */
+	uint8_t status;
+	unsigned int ii;
+	//unsigned long flags;
+
+	dma_cs_addr = (unsigned long)devpriv->lcfg
+		+ ((channel == 0) ? LCFG_DMACSR0 : LCFG_DMACSR1);
+
+	// spinlock for plx dma control/status reg
+	//comedi_spin_lock_irqsave( &dev->spinlock, flags );
+
+	// abort dma transfer if necessary
+	status = readb(dma_cs_addr);
+	if ((status & PLX_DMA_EN_BIT) == 0) {	/* not enabled (Error?) */
+		DPRINTK("rtd520: AbortDma on non-active channel %d (0x%x)\n",
+			channel, status);
+		goto abortDmaExit;
+	}
+
+	/* wait to make sure done bit is zero (needed?) */
+	for (ii = 0; (status & PLX_DMA_DONE_BIT) && ii < RTD_DMA_TIMEOUT; ii++) {
+		WAIT_QUIETLY;
+		status = readb(dma_cs_addr);
+	}
+	if (status & PLX_DMA_DONE_BIT) {
+		printk("rtd520: Timeout waiting for dma %i done clear\n",
+			channel);
+		goto abortDmaExit;
+	}
+
+	/* disable channel (required) */
+	writeb(0, dma_cs_addr);
+	comedi_udelay(1);	/* needed?? */
+	/* set abort bit for channel */
+	writeb(PLX_DMA_ABORT_BIT, dma_cs_addr);
+
+	// wait for dma done bit to be set
+	status = readb(dma_cs_addr);
+	for (ii = 0;
+		(status & PLX_DMA_DONE_BIT) == 0 && ii < RTD_DMA_TIMEOUT;
+		ii++) {
+		status = readb(dma_cs_addr);
+		WAIT_QUIETLY;
+	}
+	if ((status & PLX_DMA_DONE_BIT) == 0) {
+		printk("rtd520: Timeout waiting for dma %i done set\n",
+			channel);
+	}
+
+      abortDmaExit:
+	//comedi_spin_unlock_irqrestore( &dev->spinlock, flags );
+}
+
+/*
+  Process what is in the DMA transfer buffer and pass to comedi
+  Note: this is not re-entrant
+*/
+static int ai_process_dma(comedi_device * dev, comedi_subdevice * s)
+{
+	int ii, n;
+	s16 *dp;
+
+	if (devpriv->aiCount == 0)	/* transfer already complete */
+		return 0;
+
+	dp = devpriv->dma0Buff[devpriv->dma0Offset];
+	for (ii = 0; ii < devpriv->fifoLen / 2;) {	/* convert samples */
+		sampl_t sample;
+
+		if (CHAN_ARRAY_TEST(devpriv->chanBipolar, s->async->cur_chan)) {
+			sample = (*dp >> 3) + 2048;	/* convert to comedi unsigned data */
+		} else {
+			sample = *dp >> 3;	/* low 3 bits are marker lines */
+		}
+		*dp++ = sample;	/* put processed value back */
+
+		if (++s->async->cur_chan >= s->async->cmd.chanlist_len)
+			s->async->cur_chan = 0;
+
+		++ii;		/* number ready to transfer */
+		if (devpriv->aiCount > 0) {	/* < 0, means read forever */
+			if (--devpriv->aiCount == 0) {	/* done */
+				/*DPRINTK ("rtd520: Final %d samples\n", ii); */
+				break;
+			}
+		}
+	}
+
+	/* now pass the whole array to the comedi buffer */
+	dp = devpriv->dma0Buff[devpriv->dma0Offset];
+	n = comedi_buf_write_alloc(s->async, ii * sizeof(s16));
+	if (n < (ii * sizeof(s16))) {	/* any residual is an error */
+		DPRINTK("rtd520:ai_process_dma buffer overflow %d samples!\n",
+			ii - (n / sizeof(s16)));
+		s->async->events |= COMEDI_CB_ERROR;
+		return -1;
+	}
+	comedi_buf_memcpy_to(s->async, 0, dp, n);
+	comedi_buf_write_free(s->async, n);
+
+	/* always at least 1 scan -- 1/2 FIFO is larger than our max scan list */
+	s->async->events |= COMEDI_CB_BLOCK | COMEDI_CB_EOS;
+
+	if (++devpriv->dma0Offset >= DMA_CHAIN_COUNT) {	/* next buffer */
+		devpriv->dma0Offset = 0;
+	}
+	return 0;
+}
+#endif /* USE_DMA */
+
+/*
+  Handle all rtd520 interrupts.
+  Runs atomically and is never re-entered.
+  This is a "slow handler";  other interrupts may be active.
+  The data conversion may someday happen in a "bottom half".
+*/
+static irqreturn_t rtd_interrupt(int irq,	/* interrupt number (ignored) */
+	void *d			/* our data */
+	PT_REGS_ARG)
+{				/* cpu context (ignored) */
+	comedi_device *dev = d;	/* must be called "dev" for devpriv */
+	u16 status;
+	u16 fifoStatus;
+	comedi_subdevice *s = dev->subdevices + 0;	/* analog in subdevice */
+
+	if (!dev->attached) {
+		return IRQ_NONE;
+	}
+
+	devpriv->intCount++;	/* DEBUG statistics */
+
+	fifoStatus = RtdFifoStatus(dev);
+	/* check for FIFO full, this automatically halts the ADC! */
+	if (!(fifoStatus & FS_ADC_NOT_FULL)) {	/* 0 -> full */
+		DPRINTK("rtd520: FIFO full! fifo_status=0x%x\n", (fifoStatus ^ 0x6666) & 0x7777);	/* should be all 0s */
+		goto abortTransfer;
+	}
+#ifdef USE_DMA
+	if (devpriv->flags & DMA0_ACTIVE) {	/* Check DMA */
+		u32 istatus = RtdPlxInterruptRead(dev);
+
+		if (istatus & ICS_DMA0_A) {
+			if (ai_process_dma(dev, s) < 0) {
+				DPRINTK("rtd520: comedi read buffer overflow (DMA) with %ld to go!\n", devpriv->aiCount);
+				RtdDma0Control(dev,
+					(devpriv->
+						dma0Control &
+						~PLX_DMA_START_BIT)
+					| PLX_CLEAR_DMA_INTR_BIT);
+				goto abortTransfer;
+			}
+
+			/*DPRINTK ("rtd520: DMA transfer: %ld to go, istatus %x\n",
+			   devpriv->aiCount, istatus); */
+			RtdDma0Control(dev,
+				(devpriv->dma0Control & ~PLX_DMA_START_BIT)
+				| PLX_CLEAR_DMA_INTR_BIT);
+			if (0 == devpriv->aiCount) {	/* counted down */
+				DPRINTK("rtd520: Samples Done (DMA).\n");
+				goto transferDone;
+			}
+			comedi_event(dev, s);
+		} else {
+			/*DPRINTK ("rtd520: No DMA ready: istatus %x\n", istatus); */
+		}
+	}
+	/* Fall through and check for other interrupt sources */
+#endif /* USE_DMA */
+
+	status = RtdInterruptStatus(dev);
+	/* if interrupt was not caused by our board, or handled above */
+	if (0 == status) {
+		return IRQ_HANDLED;
+	}
+
+	if (status & IRQM_ADC_ABOUT_CNT) {	/* sample count -> read FIFO */
+		/* since the priority interrupt controller may have queued a sample
+		   counter interrupt, even though we have already finished,
+		   we must handle the possibility that there is no data here */
+		if (!(fifoStatus & FS_ADC_HEMPTY)) {	/* 0 -> 1/2 full */
+			/*DPRINTK("rtd520: Sample int, reading 1/2FIFO.  fifo_status 0x%x\n",
+			   (fifoStatus ^ 0x6666) & 0x7777); */
+			if (ai_read_n(dev, s, devpriv->fifoLen / 2) < 0) {
+				DPRINTK("rtd520: comedi read buffer overflow (1/2FIFO) with %ld to go!\n", devpriv->aiCount);
+				goto abortTransfer;
+			}
+			if (0 == devpriv->aiCount) {	/* counted down */
+				DPRINTK("rtd520: Samples Done (1/2). fifo_status was 0x%x\n", (fifoStatus ^ 0x6666) & 0x7777);	/* should be all 0s */
+				goto transferDone;
+			}
+			comedi_event(dev, s);
+		} else if (devpriv->transCount > 0) {	/* read often */
+			/*DPRINTK("rtd520: Sample int, reading %d  fifo_status 0x%x\n",
+			   devpriv->transCount, (fifoStatus ^ 0x6666) & 0x7777); */
+			if (fifoStatus & FS_ADC_NOT_EMPTY) {	/* 1 -> not empty */
+				if (ai_read_n(dev, s, devpriv->transCount) < 0) {
+					DPRINTK("rtd520: comedi read buffer overflow (N) with %ld to go!\n", devpriv->aiCount);
+					goto abortTransfer;
+				}
+				if (0 == devpriv->aiCount) {	/* counted down */
+					DPRINTK("rtd520: Samples Done (N). fifo_status was 0x%x\n", (fifoStatus ^ 0x6666) & 0x7777);
+					goto transferDone;
+				}
+				comedi_event(dev, s);
+			}
+		} else {	/* wait for 1/2 FIFO (old) */
+			DPRINTK("rtd520: Sample int.  Wait for 1/2. fifo_status 0x%x\n", (fifoStatus ^ 0x6666) & 0x7777);
+		}
+	} else {
+		DPRINTK("rtd520: unknown interrupt source!\n");
+	}
+
+	if (0xffff & RtdInterruptOverrunStatus(dev)) {	/* interrupt overrun */
+		DPRINTK("rtd520: Interrupt overrun with %ld to go! over_status=0x%x\n", devpriv->aiCount, 0xffff & RtdInterruptOverrunStatus(dev));
+		goto abortTransfer;
+	}
+
+	/* clear the interrupt */
+	RtdInterruptClearMask(dev, status);
+	RtdInterruptClear(dev);
+	return IRQ_HANDLED;
+
+      abortTransfer:
+	RtdAdcClearFifo(dev);	/* clears full flag */
+	s->async->events |= COMEDI_CB_ERROR;
+	devpriv->aiCount = 0;	/* stop and don't transfer any more */
+	/* fall into transferDone */
+
+      transferDone:
+	RtdPacerStopSource(dev, 0);	/* stop on SOFTWARE stop */
+	RtdPacerStop(dev);	/* Stop PACER */
+	RtdAdcConversionSource(dev, 0);	/* software trigger only */
+	RtdInterruptMask(dev, 0);	/* mask out SAMPLE */
+#ifdef USE_DMA
+	if (devpriv->flags & DMA0_ACTIVE) {
+		RtdPlxInterruptWrite(dev,	/* disable any more interrupts */
+			RtdPlxInterruptRead(dev) & ~ICS_DMA0_E);
+		abort_dma(dev, 0);
+		devpriv->flags &= ~DMA0_ACTIVE;
+		/* if Using DMA, then we should have read everything by now */
+		if (devpriv->aiCount > 0) {
+			DPRINTK("rtd520: Lost DMA data! %ld remain\n",
+				devpriv->aiCount);
+		}
+	}
+#endif /* USE_DMA */
+
+	if (devpriv->aiCount > 0) {	/* there shouldn't be anything left */
+		fifoStatus = RtdFifoStatus(dev);
+		DPRINTK("rtd520: Finishing up. %ld remain, fifoStat=%x\n", devpriv->aiCount, (fifoStatus ^ 0x6666) & 0x7777);	/* should read all 0s */
+		ai_read_dregs(dev, s);	/* read anything left in FIFO */
+	}
+
+	s->async->events |= COMEDI_CB_EOA;	/* signal end to comedi */
+	comedi_event(dev, s);
+
+	/* clear the interrupt */
+	status = RtdInterruptStatus(dev);
+	RtdInterruptClearMask(dev, status);
+	RtdInterruptClear(dev);
+
+	fifoStatus = RtdFifoStatus(dev);	/* DEBUG */
+	DPRINTK("rtd520: Acquisition complete. %ld ints, intStat=%x, overStat=%x\n", devpriv->intCount, status, 0xffff & RtdInterruptOverrunStatus(dev));
+
+	return IRQ_HANDLED;
+}
+
+#if 0
+/*
+  return the number of samples available
+*/
+static int rtd_ai_poll(comedi_device * dev, comedi_subdevice * s)
+{
+	/* TODO: This needs to mask interrupts, read_dregs, and then re-enable */
+	/* Not sure what to do if DMA is active */
+	return s->async->buf_write_count - s->async->buf_read_count;
+}
+#endif
+
+/*
+  cmdtest tests a particular command to see if it is valid.
+  Using the cmdtest ioctl, a user can create a valid cmd
+  and then have it executed by the cmd ioctl (asyncronously).
+
+  cmdtest returns 1,2,3,4 or 0, depending on which tests
+  the command passes.
+*/
+
+static int rtd_ai_cmdtest(comedi_device * dev,
+	comedi_subdevice * s, comedi_cmd * cmd)
+{
+	int err = 0;
+	int tmp;
+
+	/* step 1: make sure trigger sources are trivially valid */
+
+	tmp = cmd->start_src;
+	cmd->start_src &= TRIG_NOW;
+	if (!cmd->start_src || tmp != cmd->start_src) {
+		err++;
+	}
+
+	tmp = cmd->scan_begin_src;
+	cmd->scan_begin_src &= TRIG_TIMER | TRIG_EXT;
+	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src) {
+		err++;
+	}
+
+	tmp = cmd->convert_src;
+	cmd->convert_src &= TRIG_TIMER | TRIG_EXT;
+	if (!cmd->convert_src || tmp != cmd->convert_src) {
+		err++;
+	}
+
+	tmp = cmd->scan_end_src;
+	cmd->scan_end_src &= TRIG_COUNT;
+	if (!cmd->scan_end_src || tmp != cmd->scan_end_src) {
+		err++;
+	}
+
+	tmp = cmd->stop_src;
+	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
+	if (!cmd->stop_src || tmp != cmd->stop_src) {
+		err++;
+	}
+
+	if (err)
+		return 1;
+
+	/* step 2: make sure trigger sources are unique
+	   and mutually compatible */
+	/* note that mutual compatiblity is not an issue here */
+	if (cmd->scan_begin_src != TRIG_TIMER &&
+		cmd->scan_begin_src != TRIG_EXT) {
+		err++;
+	}
+	if (cmd->convert_src != TRIG_TIMER && cmd->convert_src != TRIG_EXT) {
+		err++;
+	}
+	if (cmd->stop_src != TRIG_COUNT && cmd->stop_src != TRIG_NONE) {
+		err++;
+	}
+
+	if (err) {
+		return 2;
+	}
+
+	/* step 3: make sure arguments are trivially compatible */
+
+	if (cmd->start_arg != 0) {
+		cmd->start_arg = 0;
+		err++;
+	}
+
+	if (cmd->scan_begin_src == TRIG_TIMER) {
+		/* Note: these are time periods, not actual rates */
+		if (1 == cmd->chanlist_len) {	/* no scanning */
+			if (cmd->scan_begin_arg < RTD_MAX_SPEED_1) {
+				cmd->scan_begin_arg = RTD_MAX_SPEED_1;
+				rtd_ns_to_timer(&cmd->scan_begin_arg,
+					TRIG_ROUND_UP);
+				err++;
+			}
+			if (cmd->scan_begin_arg > RTD_MIN_SPEED_1) {
+				cmd->scan_begin_arg = RTD_MIN_SPEED_1;
+				rtd_ns_to_timer(&cmd->scan_begin_arg,
+					TRIG_ROUND_DOWN);
+				err++;
+			}
+		} else {
+			if (cmd->scan_begin_arg < RTD_MAX_SPEED) {
+				cmd->scan_begin_arg = RTD_MAX_SPEED;
+				rtd_ns_to_timer(&cmd->scan_begin_arg,
+					TRIG_ROUND_UP);
+				err++;
+			}
+			if (cmd->scan_begin_arg > RTD_MIN_SPEED) {
+				cmd->scan_begin_arg = RTD_MIN_SPEED;
+				rtd_ns_to_timer(&cmd->scan_begin_arg,
+					TRIG_ROUND_DOWN);
+				err++;
+			}
+		}
+	} else {
+		/* external trigger */
+		/* should be level/edge, hi/lo specification here */
+		/* should specify multiple external triggers */
+		if (cmd->scan_begin_arg > 9) {
+			cmd->scan_begin_arg = 9;
+			err++;
+		}
+	}
+	if (cmd->convert_src == TRIG_TIMER) {
+		if (1 == cmd->chanlist_len) {	/* no scanning */
+			if (cmd->convert_arg < RTD_MAX_SPEED_1) {
+				cmd->convert_arg = RTD_MAX_SPEED_1;
+				rtd_ns_to_timer(&cmd->convert_arg,
+					TRIG_ROUND_UP);
+				err++;
+			}
+			if (cmd->convert_arg > RTD_MIN_SPEED_1) {
+				cmd->convert_arg = RTD_MIN_SPEED_1;
+				rtd_ns_to_timer(&cmd->convert_arg,
+					TRIG_ROUND_DOWN);
+				err++;
+			}
+		} else {
+			if (cmd->convert_arg < RTD_MAX_SPEED) {
+				cmd->convert_arg = RTD_MAX_SPEED;
+				rtd_ns_to_timer(&cmd->convert_arg,
+					TRIG_ROUND_UP);
+				err++;
+			}
+			if (cmd->convert_arg > RTD_MIN_SPEED) {
+				cmd->convert_arg = RTD_MIN_SPEED;
+				rtd_ns_to_timer(&cmd->convert_arg,
+					TRIG_ROUND_DOWN);
+				err++;
+			}
+		}
+	} else {
+		/* external trigger */
+		/* see above */
+		if (cmd->convert_arg > 9) {
+			cmd->convert_arg = 9;
+			err++;
+		}
+	}
+
+#if 0
+	if (cmd->scan_end_arg != cmd->chanlist_len) {
+		cmd->scan_end_arg = cmd->chanlist_len;
+		err++;
+	}
+#endif
+	if (cmd->stop_src == TRIG_COUNT) {
+		/* TODO check for rounding error due to counter wrap */
+
+	} else {
+		/* TRIG_NONE */
+		if (cmd->stop_arg != 0) {
+			cmd->stop_arg = 0;
+			err++;
+		}
+	}
+
+	if (err) {
+		return 3;
+	}
+
+	/* step 4: fix up any arguments */
+
+	if (cmd->chanlist_len > RTD_MAX_CHANLIST) {
+		cmd->chanlist_len = RTD_MAX_CHANLIST;
+		err++;
+	}
+	if (cmd->scan_begin_src == TRIG_TIMER) {
+		tmp = cmd->scan_begin_arg;
+		rtd_ns_to_timer(&cmd->scan_begin_arg,
+			cmd->flags & TRIG_ROUND_MASK);
+		if (tmp != cmd->scan_begin_arg) {
+			err++;
+		}
+	}
+	if (cmd->convert_src == TRIG_TIMER) {
+		tmp = cmd->convert_arg;
+		rtd_ns_to_timer(&cmd->convert_arg,
+			cmd->flags & TRIG_ROUND_MASK);
+		if (tmp != cmd->convert_arg) {
+			err++;
+		}
+		if (cmd->scan_begin_src == TRIG_TIMER
+			&& (cmd->scan_begin_arg
+				< (cmd->convert_arg * cmd->scan_end_arg))) {
+			cmd->scan_begin_arg =
+				cmd->convert_arg * cmd->scan_end_arg;
+			err++;
+		}
+	}
+
+	if (err) {
+		return 4;
+	}
+
+	return 0;
+}
+
+/*
+  Execute a analog in command with many possible triggering options.
+  The data get stored in the async structure of the subdevice.
+  This is usually done by an interrupt handler.
+  Userland gets to the data using read calls.
+*/
+static int rtd_ai_cmd(comedi_device * dev, comedi_subdevice * s)
+{
+	comedi_cmd *cmd = &s->async->cmd;
+	int timer;
+
+	/* stop anything currently running */
+	RtdPacerStopSource(dev, 0);	/* stop on SOFTWARE stop */
+	RtdPacerStop(dev);	/* make sure PACER is stopped */
+	RtdAdcConversionSource(dev, 0);	/* software trigger only */
+	RtdInterruptMask(dev, 0);
+#ifdef USE_DMA
+	if (devpriv->flags & DMA0_ACTIVE) {	/* cancel anything running */
+		RtdPlxInterruptWrite(dev,	/* disable any more interrupts */
+			RtdPlxInterruptRead(dev) & ~ICS_DMA0_E);
+		abort_dma(dev, 0);
+		devpriv->flags &= ~DMA0_ACTIVE;
+		if (RtdPlxInterruptRead(dev) & ICS_DMA0_A) {	/*clear pending int */
+			RtdDma0Control(dev, PLX_CLEAR_DMA_INTR_BIT);
+		}
+	}
+	RtdDma0Reset(dev);	/* reset onboard state */
+#endif /* USE_DMA */
+	RtdAdcClearFifo(dev);	/* clear any old data */
+	RtdInterruptOverrunClear(dev);
+	devpriv->intCount = 0;
+
+	if (!dev->irq) {	/* we need interrupts for this */
+		DPRINTK("rtd520: ERROR! No interrupt available!\n");
+		return -ENXIO;
+	}
+
+	/* start configuration */
+	/* load channel list and reset CGT */
+	rtd_load_channelgain_list(dev, cmd->chanlist_len, cmd->chanlist);
+
+	/* setup the common case and override if needed */
+	if (cmd->chanlist_len > 1) {
+		/*DPRINTK ("rtd520: Multi channel setup\n"); */
+		RtdPacerStartSource(dev, 0);	/* software triggers pacer */
+		RtdBurstStartSource(dev, 1);	/* PACER triggers burst */
+		RtdAdcConversionSource(dev, 2);	/* BURST triggers ADC */
+	} else {		/* single channel */
+		/*DPRINTK ("rtd520: single channel setup\n"); */
+		RtdPacerStartSource(dev, 0);	/* software triggers pacer */
+		RtdAdcConversionSource(dev, 1);	/* PACER triggers ADC */
+	}
+	RtdAboutCounter(dev, devpriv->fifoLen / 2 - 1);	/* 1/2 FIFO */
+
+	if (TRIG_TIMER == cmd->scan_begin_src) {
+		/* scan_begin_arg is in nanoseconds */
+		/* find out how many samples to wait before transferring */
+		if (cmd->flags & TRIG_WAKE_EOS) {
+			/* this may generate un-sustainable interrupt rates */
+			/* the application is responsible for doing the right thing */
+			devpriv->transCount = cmd->chanlist_len;
+			devpriv->flags |= SEND_EOS;
+		} else {
+			/* arrange to transfer data periodically */
+			devpriv->transCount
+				=
+				(TRANS_TARGET_PERIOD * cmd->chanlist_len) /
+				cmd->scan_begin_arg;
+			if (devpriv->transCount < cmd->chanlist_len) {
+				/* tranfer after each scan (and avoid 0) */
+				devpriv->transCount = cmd->chanlist_len;
+			} else {	/* make a multiple of scan length */
+				devpriv->transCount =
+					(devpriv->transCount +
+					cmd->chanlist_len - 1)
+					/ cmd->chanlist_len;
+				devpriv->transCount *= cmd->chanlist_len;
+			}
+			devpriv->flags |= SEND_EOS;
+		}
+		if (devpriv->transCount >= (devpriv->fifoLen / 2)) {
+			/* out of counter range, use 1/2 fifo instead */
+			devpriv->transCount = 0;
+			devpriv->flags &= ~SEND_EOS;
+		} else {
+			/* interrupt for each tranfer */
+			RtdAboutCounter(dev, devpriv->transCount - 1);
+		}
+
+		DPRINTK("rtd520: scanLen=%d tranferCount=%d fifoLen=%d\n  scanTime(ns)=%d flags=0x%x\n", cmd->chanlist_len, devpriv->transCount, devpriv->fifoLen, cmd->scan_begin_arg, devpriv->flags);
+	} else {		/* unknown timing, just use 1/2 FIFO */
+		devpriv->transCount = 0;
+		devpriv->flags &= ~SEND_EOS;
+	}
+	RtdPacerClockSource(dev, 1);	/* use INTERNAL 8Mhz clock source */
+	RtdAboutStopEnable(dev, 1);	/* just interrupt, dont stop */
+
+	/* BUG??? these look like enumerated values, but they are bit fields */
+
+	/* First, setup when to stop */
+	switch (cmd->stop_src) {
+	case TRIG_COUNT:	/* stop after N scans */
+		devpriv->aiCount = cmd->stop_arg * cmd->chanlist_len;
+		if ((devpriv->transCount > 0)
+			&& (devpriv->transCount > devpriv->aiCount)) {
+			devpriv->transCount = devpriv->aiCount;
+		}
+		break;
+
+	case TRIG_NONE:	/* stop when cancel is called */
+		devpriv->aiCount = -1;	/* read forever */
+		break;
+
+	default:
+		DPRINTK("rtd520: Warning! ignoring stop_src mode %d\n",
+			cmd->stop_src);
+	}
+
+	/* Scan timing */
+	switch (cmd->scan_begin_src) {
+	case TRIG_TIMER:	/* periodic scanning */
+		timer = rtd_ns_to_timer(&cmd->scan_begin_arg,
+			TRIG_ROUND_NEAREST);
+		/* set PACER clock */
+		/*DPRINTK ("rtd520: loading %d into pacer\n", timer); */
+		RtdPacerCounter(dev, timer);
+
+		break;
+
+	case TRIG_EXT:
+		RtdPacerStartSource(dev, 1);	/* EXTERNALy trigger pacer */
+		break;
+
+	default:
+		DPRINTK("rtd520: Warning! ignoring scan_begin_src mode %d\n",
+			cmd->scan_begin_src);
+	}
+
+	/* Sample timing within a scan */
+	switch (cmd->convert_src) {
+	case TRIG_TIMER:	/* periodic */
+		if (cmd->chanlist_len > 1) {	/* only needed for multi-channel */
+			timer = rtd_ns_to_timer(&cmd->convert_arg,
+				TRIG_ROUND_NEAREST);
+			/* setup BURST clock */
+			/*DPRINTK ("rtd520: loading %d into burst\n", timer); */
+			RtdBurstCounter(dev, timer);
+		}
+
+		break;
+
+	case TRIG_EXT:		/* external */
+		RtdBurstStartSource(dev, 2);	/* EXTERNALy trigger burst */
+		break;
+
+	default:
+		DPRINTK("rtd520: Warning! ignoring convert_src mode %d\n",
+			cmd->convert_src);
+	}
+	/* end configuration */
+
+	/* This doesn't seem to work.  There is no way to clear an interrupt
+	   that the priority controller has queued! */
+	RtdInterruptClearMask(dev, ~0);	/* clear any existing flags */
+	RtdInterruptClear(dev);
+
+	/* TODO: allow multiple interrupt sources */
+	if (devpriv->transCount > 0) {	/* transfer every N samples */
+		RtdInterruptMask(dev, IRQM_ADC_ABOUT_CNT);
+		DPRINTK("rtd520: Transferring every %d\n", devpriv->transCount);
+	} else {		/* 1/2 FIFO transfers */
+#ifdef USE_DMA
+		devpriv->flags |= DMA0_ACTIVE;
+
+		/* point to first transfer in ring */
+		devpriv->dma0Offset = 0;
+		RtdDma0Mode(dev, DMA_MODE_BITS);
+		RtdDma0Next(dev,	/* point to first block */
+			devpriv->dma0Chain[DMA_CHAIN_COUNT - 1].next);
+		RtdDma0Source(dev, DMAS_ADFIFO_HALF_FULL);	/* set DMA trigger source */
+
+		RtdPlxInterruptWrite(dev,	/* enable interrupt */
+			RtdPlxInterruptRead(dev) | ICS_DMA0_E);
+		/* Must be 2 steps.  See PLX app note about "Starting a DMA transfer" */
+		RtdDma0Control(dev, PLX_DMA_EN_BIT);	/* enable DMA (clear INTR?) */
+		RtdDma0Control(dev, PLX_DMA_EN_BIT | PLX_DMA_START_BIT);	/*start DMA */
+		DPRINTK("rtd520: Using DMA0 transfers. plxInt %x RtdInt %x\n",
+			RtdPlxInterruptRead(dev), devpriv->intMask);
+#else /* USE_DMA */
+		RtdInterruptMask(dev, IRQM_ADC_ABOUT_CNT);
+		DPRINTK("rtd520: Transferring every 1/2 FIFO\n");
+#endif /* USE_DMA */
+	}
+
+	/* BUG: start_src is ASSUMED to be TRIG_NOW */
+	/* BUG? it seems like things are running before the "start" */
+	RtdPacerStart(dev);	/* Start PACER */
+	return 0;
+}
+
+/*
+  Stop a running data aquisition.
+*/
+static int rtd_ai_cancel(comedi_device * dev, comedi_subdevice * s)
+{
+	u16 status;
+
+	RtdPacerStopSource(dev, 0);	/* stop on SOFTWARE stop */
+	RtdPacerStop(dev);	/* Stop PACER */
+	RtdAdcConversionSource(dev, 0);	/* software trigger only */
+	RtdInterruptMask(dev, 0);
+	devpriv->aiCount = 0;	/* stop and don't transfer any more */
+#ifdef USE_DMA
+	if (devpriv->flags & DMA0_ACTIVE) {
+		RtdPlxInterruptWrite(dev,	/* disable any more interrupts */
+			RtdPlxInterruptRead(dev) & ~ICS_DMA0_E);
+		abort_dma(dev, 0);
+		devpriv->flags &= ~DMA0_ACTIVE;
+	}
+#endif /* USE_DMA */
+	status = RtdInterruptStatus(dev);
+	DPRINTK("rtd520: Acquisition canceled. %ld ints, intStat=%x, overStat=%x\n", devpriv->intCount, status, 0xffff & RtdInterruptOverrunStatus(dev));
+	return 0;
+}
+
+/*
+  Given a desired period and the clock period (both in ns),
+  return the proper counter value (divider-1).
+  Sets the original period to be the true value.
+  Note: you have to check if the value is larger than the counter range!
+*/
+static int rtd_ns_to_timer_base(unsigned int *nanosec,	/* desired period (in ns) */
+	int round_mode, int base)
+{				/* clock period (in ns) */
+	int divider;
+
+	switch (round_mode) {
+	case TRIG_ROUND_NEAREST:
+	default:
+		divider = (*nanosec + base / 2) / base;
+		break;
+	case TRIG_ROUND_DOWN:
+		divider = (*nanosec) / base;
+		break;
+	case TRIG_ROUND_UP:
+		divider = (*nanosec + base - 1) / base;
+		break;
+	}
+	if (divider < 2)
+		divider = 2;	/* min is divide by 2 */
+
+	/* Note: we don't check for max, because different timers
+	   have different ranges */
+
+	*nanosec = base * divider;
+	return divider - 1;	/* countdown is divisor+1 */
+}
+
+/*
+  Given a desired period (in ns),
+  return the proper counter value (divider-1) for the internal clock.
+  Sets the original period to be the true value.
+*/
+static int rtd_ns_to_timer(unsigned int *ns, int round_mode)
+{
+	return rtd_ns_to_timer_base(ns, round_mode, RTD_CLOCK_BASE);
+}
+
+/*
+  Output one (or more) analog values to a single port as fast as possible.
+*/
+static int rtd_ao_winsn(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data)
+{
+	int i;
+	int chan = CR_CHAN(insn->chanspec);
+	int range = CR_RANGE(insn->chanspec);
+
+	/* Configure the output range (table index matches the range values) */
+	RtdDacRange(dev, chan, range);
+
+	/* Writing a list of values to an AO channel is probably not
+	 * very useful, but that's how the interface is defined. */
+	for (i = 0; i < insn->n; ++i) {
+		int val = data[i] << 3;
+		int stat = 0;	/* initialize to avoid bogus warning */
+		int ii;
+
+		/* VERIFY: comedi range and offset conversions */
+
+		if ((range > 1)	/* bipolar */
+			&&(data[i] < 2048)) {
+			/* offset and sign extend */
+			val = (((int)data[i]) - 2048) << 3;
+		} else {	/* unipolor */
+			val = data[i] << 3;
+		}
+
+		DPRINTK("comedi: rtd520 DAC chan=%d range=%d writing %d as 0x%x\n", chan, range, data[i], val);
+
+		/* a typical programming sequence */
+		RtdDacFifoPut(dev, chan, val);	/* put the value in */
+		RtdDacUpdate(dev, chan);	/* trigger the conversion */
+
+		devpriv->aoValue[chan] = data[i];	/* save for read back */
+
+		for (ii = 0; ii < RTD_DAC_TIMEOUT; ++ii) {
+			stat = RtdFifoStatus(dev);
+			/* 1 -> not empty */
+			if (stat & ((0 == chan) ? FS_DAC1_NOT_EMPTY :
+					FS_DAC2_NOT_EMPTY))
+				break;
+			WAIT_QUIETLY;
+		}
+		if (ii >= RTD_DAC_TIMEOUT) {
+			DPRINTK("rtd520: Error: DAC never finished! FifoStatus=0x%x\n", stat ^ 0x6666);
+			return -ETIMEDOUT;
+		}
+	}
+
+	/* return the number of samples read/written */
+	return i;
+}
+
+/* AO subdevices should have a read insn as well as a write insn.
+ * Usually this means copying a value stored in devpriv. */
+static int rtd_ao_rinsn(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data)
+{
+	int i;
+	int chan = CR_CHAN(insn->chanspec);
+
+	for (i = 0; i < insn->n; i++) {
+		data[i] = devpriv->aoValue[chan];
+	}
+
+	return i;
+}
+
+/*
+   Write a masked set of bits and the read back the port.
+   We track what the bits should be (i.e. we don't read the port first).
+
+   DIO devices are slightly special.  Although it is possible to
+ * implement the insn_read/insn_write interface, it is much more
+ * useful to applications if you implement the insn_bits interface.
+ * This allows packed reading/writing of the DIO channels.  The
+ * comedi core can convert between insn_bits and insn_read/write
+ */
+static int rtd_dio_insn_bits(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data)
+{
+	if (insn->n != 2)
+		return -EINVAL;
+
+	/* The insn data is a mask in data[0] and the new data
+	 * in data[1], each channel cooresponding to a bit. */
+	if (data[0]) {
+		s->state &= ~data[0];
+		s->state |= data[0] & data[1];
+
+		/* Write out the new digital output lines */
+		RtdDio0Write(dev, s->state);
+	}
+	/* on return, data[1] contains the value of the digital
+	 * input lines. */
+	data[1] = RtdDio0Read(dev);
+
+	/*DPRINTK("rtd520:port_0 wrote: 0x%x read: 0x%x\n", s->state, data[1]); */
+
+	return 2;
+}
+
+/*
+  Configure one bit on a IO port as Input or Output (hence the name :-).
+*/
+static int rtd_dio_insn_config(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data)
+{
+	int chan = CR_CHAN(insn->chanspec);
+
+	/* The input or output configuration of each digital line is
+	 * configured by a special insn_config instruction.  chanspec
+	 * contains the channel to be changed, and data[0] contains the
+	 * value COMEDI_INPUT or COMEDI_OUTPUT. */
+	switch (data[0]) {
+	case INSN_CONFIG_DIO_OUTPUT:
+		s->io_bits |= 1 << chan;	/* 1 means Out */
+		break;
+	case INSN_CONFIG_DIO_INPUT:
+		s->io_bits &= ~(1 << chan);
+		break;
+	case INSN_CONFIG_DIO_QUERY:
+		data[1] =
+			(s->
+			io_bits & (1 << chan)) ? COMEDI_OUTPUT : COMEDI_INPUT;
+		return insn->n;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	DPRINTK("rtd520: port_0_direction=0x%x (1 means out)\n", s->io_bits);
+	/* TODO support digital match interrupts and strobes */
+	RtdDioStatusWrite(dev, 0x01);	/* make Dio0Ctrl point to direction */
+	RtdDio0CtrlWrite(dev, s->io_bits);	/* set direction 1 means Out */
+	RtdDioStatusWrite(dev, 0);	/* make Dio0Ctrl clear interrupts */
+
+	/* port1 can only be all input or all output */
+
+	/* there are also 2 user input lines and 2 user output lines */
+
+	return 1;
+}
+
+/*
+ * A convenient macro that defines init_module() and cleanup_module(),
+ * as necessary.
+ */
+COMEDI_PCI_INITCLEANUP(rtd520Driver, rtd520_pci_table);
diff --git a/drivers/staging/comedi/drivers/rtd520.h b/drivers/staging/comedi/drivers/rtd520.h
new file mode 100644
index 00000000000..0eb50b8e605
--- /dev/null
+++ b/drivers/staging/comedi/drivers/rtd520.h
@@ -0,0 +1,412 @@
+/*
+    comedi/drivers/rtd520.h
+    Comedi driver defines for Real Time Devices (RTD) PCI4520/DM7520
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 2001 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+*/
+
+/*
+    Created by Dan Christian, NASA Ames Research Center.
+    See board notes in rtd520.c
+*/
+
+/*
+  LAS0 Runtime Area
+  Local Address Space 0 Offset		Read Function	Write Function
+*/
+#define LAS0_SPARE_00    0x0000	// -                               -
+#define LAS0_SPARE_04    0x0004	// -                               -
+#define LAS0_USER_IO     0x0008	// Read User Inputs                Write User Outputs
+#define LAS0_SPARE_0C    0x000C	// -                               -
+#define LAS0_ADC         0x0010	// Read FIFO Status                Software A/D Start
+#define LAS0_DAC1        0x0014	// -                               Software D/A1 Update
+#define LAS0_DAC2        0x0018	// -                               Software D/A2 Update
+#define LAS0_SPARE_1C    0x001C	// -                               -
+#define LAS0_SPARE_20    0x0020	// -                               -
+#define LAS0_DAC         0x0024	// -                               Software Simultaneous D/A1 and D/A2 Update
+#define LAS0_PACER       0x0028	// Software Pacer Start            Software Pacer Stop
+#define LAS0_TIMER       0x002C	// Read Timer Counters Status      HDIN Software Trigger
+#define LAS0_IT          0x0030	// Read Interrupt Status           Write Interrupt Enable Mask Register
+#define LAS0_CLEAR       0x0034	// Clear ITs set by Clear Mask     Set Interrupt Clear Mask
+#define LAS0_OVERRUN     0x0038	// Read pending interrupts         Clear Overrun Register
+#define LAS0_SPARE_3C    0x003C	// -                               -
+
+/*
+  LAS0 Runtime Area Timer/Counter,Dig.IO
+  Name			Local Address			Function
+*/
+#define LAS0_PCLK        0x0040	// Pacer Clock value (24bit)             Pacer Clock load (24bit)
+#define LAS0_BCLK        0x0044	// Burst Clock value (10bit)             Burst Clock load (10bit)
+#define LAS0_ADC_SCNT    0x0048	// A/D Sample counter value (10bit)      A/D Sample counter load (10bit)
+#define LAS0_DAC1_UCNT   0x004C	// D/A1 Update counter value (10 bit)    D/A1 Update counter load (10bit)
+#define LAS0_DAC2_UCNT   0x0050	// D/A2 Update counter value (10 bit)    D/A2 Update counter load (10bit)
+#define LAS0_DCNT        0x0054	// Delay counter value (16 bit)          Delay counter load (16bit)
+#define LAS0_ACNT        0x0058	// About counter value (16 bit)          About counter load (16bit)
+#define LAS0_DAC_CLK     0x005C	// DAC clock value (16bit)               DAC clock load (16bit)
+#define LAS0_UTC0        0x0060	// 8254 TC Counter 0 User TC 0 value     Load count in TC Counter 0
+#define LAS0_UTC1        0x0064	// 8254 TC Counter 1 User TC 1 value     Load count in TC Counter 1
+#define LAS0_UTC2        0x0068	// 8254 TC Counter 2 User TC 2 value     Load count in TC Counter 2
+#define LAS0_UTC_CTRL    0x006C	// 8254 TC Control Word                  Program counter mode for TC
+#define LAS0_DIO0        0x0070	// Digital I/O Port 0 Read Port          Digital I/O Port 0 Write Port
+#define LAS0_DIO1        0x0074	// Digital I/O Port 1 Read Port          Digital I/O Port 1 Write Port
+#define LAS0_DIO0_CTRL   0x0078	// Clear digital IRQ status flag/read    Clear digital chip/program Port 0
+#define LAS0_DIO_STATUS  0x007C	// Read Digital I/O Status word          Program digital control register &
+
+/*
+  LAS0 Setup Area
+  Name			Local Address			Function
+*/
+#define LAS0_BOARD_RESET        0x0100	// Board reset
+#define LAS0_DMA0_SRC           0x0104	// DMA 0 Sources select
+#define LAS0_DMA1_SRC           0x0108	// DMA 1 Sources select
+#define LAS0_ADC_CONVERSION     0x010C	// A/D Conversion Signal select
+#define LAS0_BURST_START        0x0110	// Burst Clock Start Trigger select
+#define LAS0_PACER_START        0x0114	// Pacer Clock Start Trigger select
+#define LAS0_PACER_STOP         0x0118	// Pacer Clock Stop Trigger select
+#define LAS0_ACNT_STOP_ENABLE   0x011C	// About Counter Stop Enable
+#define LAS0_PACER_REPEAT       0x0120	// Pacer Start Trigger Mode select
+#define LAS0_DIN_START          0x0124	// High Speed Digital Input Sampling Signal select
+#define LAS0_DIN_FIFO_CLEAR     0x0128	// Digital Input FIFO Clear
+#define LAS0_ADC_FIFO_CLEAR     0x012C	// A/D FIFO Clear
+#define LAS0_CGT_WRITE          0x0130	// Channel Gain Table Write
+#define LAS0_CGL_WRITE          0x0134	// Channel Gain Latch Write
+#define LAS0_CG_DATA            0x0138	// Digital Table Write
+#define LAS0_CGT_ENABLE		0x013C	// Channel Gain Table Enable
+#define LAS0_CG_ENABLE          0x0140	// Digital Table Enable
+#define LAS0_CGT_PAUSE          0x0144	// Table Pause Enable
+#define LAS0_CGT_RESET          0x0148	// Reset Channel Gain Table
+#define LAS0_CGT_CLEAR          0x014C	// Clear Channel Gain Table
+#define LAS0_DAC1_CTRL          0x0150	// D/A1 output type/range
+#define LAS0_DAC1_SRC           0x0154	// D/A1 update source
+#define LAS0_DAC1_CYCLE         0x0158	// D/A1 cycle mode
+#define LAS0_DAC1_RESET         0x015C	// D/A1 FIFO reset
+#define LAS0_DAC1_FIFO_CLEAR    0x0160	// D/A1 FIFO clear
+#define LAS0_DAC2_CTRL          0x0164	// D/A2 output type/range
+#define LAS0_DAC2_SRC           0x0168	// D/A2 update source
+#define LAS0_DAC2_CYCLE         0x016C	// D/A2 cycle mode
+#define LAS0_DAC2_RESET         0x0170	// D/A2 FIFO reset
+#define LAS0_DAC2_FIFO_CLEAR    0x0174	// D/A2 FIFO clear
+#define LAS0_ADC_SCNT_SRC       0x0178	// A/D Sample Counter Source select
+#define LAS0_PACER_SELECT       0x0180	// Pacer Clock select
+#define LAS0_SBUS0_SRC          0x0184	// SyncBus 0 Source select
+#define LAS0_SBUS0_ENABLE       0x0188	// SyncBus 0 enable
+#define LAS0_SBUS1_SRC          0x018C	// SyncBus 1 Source select
+#define LAS0_SBUS1_ENABLE       0x0190	// SyncBus 1 enable
+#define LAS0_SBUS2_SRC          0x0198	// SyncBus 2 Source select
+#define LAS0_SBUS2_ENABLE       0x019C	// SyncBus 2 enable
+#define LAS0_ETRG_POLARITY      0x01A4	// External Trigger polarity select
+#define LAS0_EINT_POLARITY      0x01A8	// External Interrupt polarity select
+#define LAS0_UTC0_CLOCK         0x01AC	// UTC0 Clock select
+#define LAS0_UTC0_GATE          0x01B0	// UTC0 Gate select
+#define LAS0_UTC1_CLOCK         0x01B4	// UTC1 Clock select
+#define LAS0_UTC1_GATE          0x01B8	// UTC1 Gate select
+#define LAS0_UTC2_CLOCK         0x01BC	// UTC2 Clock select
+#define LAS0_UTC2_GATE          0x01C0	// UTC2 Gate select
+#define LAS0_UOUT0_SELECT       0x01C4	// User Output 0 source select
+#define LAS0_UOUT1_SELECT       0x01C8	// User Output 1 source select
+#define LAS0_DMA0_RESET         0x01CC	// DMA0 Request state machine reset
+#define LAS0_DMA1_RESET         0x01D0	// DMA1 Request state machine reset
+
+/*
+  LAS1
+  Name			Local Address			Function
+*/
+#define LAS1_ADC_FIFO            0x0000	// Read A/D FIFO (16bit) -
+#define LAS1_HDIO_FIFO           0x0004	// Read High Speed Digital Input FIFO (16bit) -
+#define LAS1_DAC1_FIFO           0x0008	// - Write D/A1 FIFO (16bit)
+#define LAS1_DAC2_FIFO           0x000C	// - Write D/A2 FIFO (16bit)
+
+/*
+  LCFG: PLX 9080 local config & runtime registers
+  Name			Local Address			Function
+*/
+#define LCFG_ITCSR              0x0068	// INTCSR, Interrupt Control/Status Register
+#define LCFG_DMAMODE0           0x0080	// DMA Channel 0 Mode Register
+#define LCFG_DMAPADR0           0x0084	// DMA Channel 0 PCI Address Register
+#define LCFG_DMALADR0           0x0088	// DMA Channel 0 Local Address Reg
+#define LCFG_DMASIZ0            0x008C	// DMA Channel 0 Transfer Size (Bytes) Register
+#define LCFG_DMADPR0            0x0090	// DMA Channel 0 Descriptor Pointer Register
+#define LCFG_DMAMODE1           0x0094	// DMA Channel 1 Mode Register
+#define LCFG_DMAPADR1           0x0098	// DMA Channel 1 PCI Address Register
+#define LCFG_DMALADR1           0x009C	// DMA Channel 1 Local Address Register
+#define LCFG_DMASIZ1            0x00A0	// DMA Channel 1 Transfer Size (Bytes) Register
+#define LCFG_DMADPR1            0x00A4	// DMA Channel 1 Descriptor Pointer Register
+#define LCFG_DMACSR0            0x00A8	// DMA Channel 0 Command/Status Register
+#define LCFG_DMACSR1            0x00A9	// DMA Channel 0 Command/Status Register
+#define LCFG_DMAARB             0x00AC	// DMA Arbitration Register
+#define LCFG_DMATHR             0x00B0	// DMA Threshold Register
+
+/*======================================================================
+  Resister bit definitions
+======================================================================*/
+
+// FIFO Status Word Bits (RtdFifoStatus)
+#define FS_DAC1_NOT_EMPTY    0x0001	// D0  - DAC1 FIFO not empty
+#define FS_DAC1_HEMPTY   0x0002	// D1  - DAC1 FIFO half empty
+#define FS_DAC1_NOT_FULL     0x0004	// D2  - DAC1 FIFO not full
+#define FS_DAC2_NOT_EMPTY    0x0010	// D4  - DAC2 FIFO not empty
+#define FS_DAC2_HEMPTY   0x0020	// D5  - DAC2 FIFO half empty
+#define FS_DAC2_NOT_FULL     0x0040	// D6  - DAC2 FIFO not full
+#define FS_ADC_NOT_EMPTY     0x0100	// D8  - ADC FIFO not empty
+#define FS_ADC_HEMPTY    0x0200	// D9  - ADC FIFO half empty
+#define FS_ADC_NOT_FULL      0x0400	// D10 - ADC FIFO not full
+#define FS_DIN_NOT_EMPTY     0x1000	// D12 - DIN FIFO not empty
+#define FS_DIN_HEMPTY    0x2000	// D13 - DIN FIFO half empty
+#define FS_DIN_NOT_FULL      0x4000	// D14 - DIN FIFO not full
+
+// Timer Status Word Bits (GetTimerStatus)
+#define TS_PCLK_GATE   0x0001
+// D0 - Pacer Clock Gate [0 - gated, 1 - enabled]
+#define TS_BCLK_GATE   0x0002
+// D1 - Burst Clock Gate [0 - disabled, 1 - running]
+#define TS_DCNT_GATE   0x0004
+// D2 - Pacer Clock Delayed Start Trigger [0 - delay over, 1 - delay in
+// progress]
+#define TS_ACNT_GATE   0x0008
+// D3 - Pacer Clock About Trigger [0 - completed, 1 - in progress]
+#define TS_PCLK_RUN    0x0010
+// D4 - Pacer Clock Shutdown Flag [0 - Pacer Clock cannot be start
+// triggered only by Software Pacer Start Command, 1 - Pacer Clock can
+// be start triggered]
+
+// External Trigger polarity select
+// External Interrupt polarity select
+#define POL_POSITIVE         0x0	// positive edge
+#define POL_NEGATIVE         0x1	// negative edge
+
+// User Output Signal select (SetUout0Source, SetUout1Source)
+#define UOUT_ADC                0x0	// A/D Conversion Signal
+#define UOUT_DAC1               0x1	// D/A1 Update
+#define UOUT_DAC2               0x2	// D/A2 Update
+#define UOUT_SOFTWARE           0x3	// Software Programmable
+
+// Pacer clock select (SetPacerSource)
+#define PCLK_INTERNAL           1	// Internal Pacer Clock
+#define PCLK_EXTERNAL           0	// External Pacer Clock
+
+// A/D Sample Counter Sources (SetAdcntSource, SetupSampleCounter)
+#define ADC_SCNT_CGT_RESET         0x0	// needs restart with StartPacer
+#define ADC_SCNT_FIFO_WRITE        0x1
+
+// A/D Conversion Signal Select (for SetConversionSelect)
+#define ADC_START_SOFTWARE         0x0	// Software A/D Start
+#define ADC_START_PCLK             0x1	// Pacer Clock (Ext. Int. see Func.509)
+#define ADC_START_BCLK             0x2	// Burst Clock
+#define ADC_START_DIGITAL_IT       0x3	// Digital Interrupt
+#define ADC_START_DAC1_MARKER1     0x4	// D/A 1 Data Marker 1
+#define ADC_START_DAC2_MARKER1     0x5	// D/A 2 Data Marker 1
+#define ADC_START_SBUS0            0x6	// SyncBus 0
+#define ADC_START_SBUS1            0x7	// SyncBus 1
+#define ADC_START_SBUS2            0x8	// SyncBus 2
+
+// Burst Clock start trigger select (SetBurstStart)
+#define BCLK_START_SOFTWARE        0x0	// Software A/D Start (StartBurst)
+#define BCLK_START_PCLK            0x1	// Pacer Clock
+#define BCLK_START_ETRIG           0x2	// External Trigger
+#define BCLK_START_DIGITAL_IT      0x3	// Digital Interrupt
+#define BCLK_START_SBUS0           0x4	// SyncBus 0
+#define BCLK_START_SBUS1           0x5	// SyncBus 1
+#define BCLK_START_SBUS2           0x6	// SyncBus 2
+
+// Pacer Clock start trigger select (SetPacerStart)
+#define PCLK_START_SOFTWARE        0x0	// Software Pacer Start (StartPacer)
+#define PCLK_START_ETRIG           0x1	// External trigger
+#define PCLK_START_DIGITAL_IT      0x2	// Digital interrupt
+#define PCLK_START_UTC2            0x3	// User TC 2 out
+#define PCLK_START_SBUS0           0x4	// SyncBus 0
+#define PCLK_START_SBUS1           0x5	// SyncBus 1
+#define PCLK_START_SBUS2           0x6	// SyncBus 2
+#define PCLK_START_D_SOFTWARE      0x8	// Delayed Software Pacer Start
+#define PCLK_START_D_ETRIG         0x9	// Delayed external trigger
+#define PCLK_START_D_DIGITAL_IT    0xA	// Delayed digital interrupt
+#define PCLK_START_D_UTC2          0xB	// Delayed User TC 2 out
+#define PCLK_START_D_SBUS0         0xC	// Delayed SyncBus 0
+#define PCLK_START_D_SBUS1         0xD	// Delayed SyncBus 1
+#define PCLK_START_D_SBUS2         0xE	// Delayed SyncBus 2
+#define PCLK_START_ETRIG_GATED     0xF	// External Trigger Gated controlled mode
+
+// Pacer Clock Stop Trigger select (SetPacerStop)
+#define PCLK_STOP_SOFTWARE         0x0	// Software Pacer Stop (StopPacer)
+#define PCLK_STOP_ETRIG            0x1	// External Trigger
+#define PCLK_STOP_DIGITAL_IT       0x2	// Digital Interrupt
+#define PCLK_STOP_ACNT             0x3	// About Counter
+#define PCLK_STOP_UTC2             0x4	// User TC2 out
+#define PCLK_STOP_SBUS0            0x5	// SyncBus 0
+#define PCLK_STOP_SBUS1            0x6	// SyncBus 1
+#define PCLK_STOP_SBUS2            0x7	// SyncBus 2
+#define PCLK_STOP_A_SOFTWARE       0x8	// About Software Pacer Stop
+#define PCLK_STOP_A_ETRIG          0x9	// About External Trigger
+#define PCLK_STOP_A_DIGITAL_IT     0xA	// About Digital Interrupt
+#define PCLK_STOP_A_UTC2           0xC	// About User TC2 out
+#define PCLK_STOP_A_SBUS0          0xD	// About SyncBus 0
+#define PCLK_STOP_A_SBUS1          0xE	// About SyncBus 1
+#define PCLK_STOP_A_SBUS2          0xF	// About SyncBus 2
+
+// About Counter Stop Enable
+#define ACNT_STOP                  0x0	// stop enable
+#define ACNT_NO_STOP               0x1	// stop disabled
+
+// DAC update source (SetDAC1Start & SetDAC2Start)
+#define DAC_START_SOFTWARE         0x0	// Software Update
+#define DAC_START_CGT              0x1	// CGT controlled Update
+#define DAC_START_DAC_CLK          0x2	// D/A Clock
+#define DAC_START_EPCLK            0x3	// External Pacer Clock
+#define DAC_START_SBUS0            0x4	// SyncBus 0
+#define DAC_START_SBUS1            0x5	// SyncBus 1
+#define DAC_START_SBUS2            0x6	// SyncBus 2
+
+// DAC Cycle Mode (SetDAC1Cycle, SetDAC2Cycle, SetupDAC)
+#define DAC_CYCLE_SINGLE           0x0	// not cycle
+#define DAC_CYCLE_MULTI            0x1	// cycle
+
+// 8254 Operation Modes (Set8254Mode, SetupTimerCounter)
+#define M8254_EVENT_COUNTER        0	// Event Counter
+#define M8254_HW_ONE_SHOT          1	// Hardware-Retriggerable One-Shot
+#define M8254_RATE_GENERATOR       2	// Rate Generator
+#define M8254_SQUARE_WAVE          3	// Square Wave Mode
+#define M8254_SW_STROBE            4	// Software Triggered Strobe
+#define M8254_HW_STROBE            5	// Hardware Triggered Strobe (Retriggerable)
+
+// User Timer/Counter 0 Clock Select (SetUtc0Clock)
+#define CUTC0_8MHZ                 0x0	// 8MHz
+#define CUTC0_EXT_TC_CLOCK1        0x1	// Ext. TC Clock 1
+#define CUTC0_EXT_TC_CLOCK2        0x2	// Ext. TC Clock 2
+#define CUTC0_EXT_PCLK             0x3	// Ext. Pacer Clock
+
+// User Timer/Counter 1 Clock Select (SetUtc1Clock)
+#define CUTC1_8MHZ                 0x0	// 8MHz
+#define CUTC1_EXT_TC_CLOCK1        0x1	// Ext. TC Clock 1
+#define CUTC1_EXT_TC_CLOCK2        0x2	// Ext. TC Clock 2
+#define CUTC1_EXT_PCLK             0x3	// Ext. Pacer Clock
+#define CUTC1_UTC0_OUT             0x4	// User Timer/Counter 0 out
+#define CUTC1_DIN_SIGNAL           0x5	// High-Speed Digital Input   Sampling signal
+
+// User Timer/Counter 2 Clock Select (SetUtc2Clock)
+#define CUTC2_8MHZ                 0x0	// 8MHz
+#define CUTC2_EXT_TC_CLOCK1        0x1	// Ext. TC Clock 1
+#define CUTC2_EXT_TC_CLOCK2        0x2	// Ext. TC Clock 2
+#define CUTC2_EXT_PCLK             0x3	// Ext. Pacer Clock
+#define CUTC2_UTC1_OUT             0x4	// User Timer/Counter 1 out
+
+// User Timer/Counter 0 Gate Select (SetUtc0Gate)
+#define GUTC0_NOT_GATED            0x0	// Not gated
+#define GUTC0_GATED                0x1	// Gated
+#define GUTC0_EXT_TC_GATE1         0x2	// Ext. TC Gate 1
+#define GUTC0_EXT_TC_GATE2         0x3	// Ext. TC Gate 2
+
+// User Timer/Counter 1 Gate Select (SetUtc1Gate)
+#define GUTC1_NOT_GATED            0x0	// Not gated
+#define GUTC1_GATED                0x1	// Gated
+#define GUTC1_EXT_TC_GATE1         0x2	// Ext. TC Gate 1
+#define GUTC1_EXT_TC_GATE2         0x3	// Ext. TC Gate 2
+#define GUTC1_UTC0_OUT             0x4	// User Timer/Counter 0 out
+
+// User Timer/Counter 2 Gate Select (SetUtc2Gate)
+#define GUTC2_NOT_GATED            0x0	// Not gated
+#define GUTC2_GATED                0x1	// Gated
+#define GUTC2_EXT_TC_GATE1         0x2	// Ext. TC Gate 1
+#define GUTC2_EXT_TC_GATE2         0x3	// Ext. TC Gate 2
+#define GUTC2_UTC1_OUT             0x4	// User Timer/Counter 1 out
+
+// Interrupt Source Masks (SetITMask, ClearITMask, GetITStatus)
+#define IRQM_ADC_FIFO_WRITE        0x0001	// ADC FIFO Write
+#define IRQM_CGT_RESET             0x0002	// Reset CGT
+#define IRQM_CGT_PAUSE             0x0008	// Pause CGT
+#define IRQM_ADC_ABOUT_CNT         0x0010	// About Counter out
+#define IRQM_ADC_DELAY_CNT         0x0020	// Delay Counter out
+#define IRQM_ADC_SAMPLE_CNT	   0x0040	// ADC Sample Counter
+#define IRQM_DAC1_UCNT             0x0080	// DAC1 Update Counter
+#define IRQM_DAC2_UCNT             0x0100	// DAC2 Update Counter
+#define IRQM_UTC1                  0x0200	// User TC1 out
+#define IRQM_UTC1_INV              0x0400	// User TC1 out, inverted
+#define IRQM_UTC2                  0x0800	// User TC2 out
+#define IRQM_DIGITAL_IT            0x1000	// Digital Interrupt
+#define IRQM_EXTERNAL_IT           0x2000	// External Interrupt
+#define IRQM_ETRIG_RISING          0x4000	// External Trigger rising-edge
+#define IRQM_ETRIG_FALLING         0x8000	// External Trigger falling-edge
+
+// DMA Request Sources (LAS0)
+#define DMAS_DISABLED              0x0	// DMA Disabled
+#define DMAS_ADC_SCNT              0x1	// ADC Sample Counter
+#define DMAS_DAC1_UCNT             0x2	// D/A1 Update Counter
+#define DMAS_DAC2_UCNT             0x3	// D/A2 Update Counter
+#define DMAS_UTC1                  0x4	// User TC1 out
+#define DMAS_ADFIFO_HALF_FULL      0x8	// A/D FIFO half full
+#define DMAS_DAC1_FIFO_HALF_EMPTY  0x9	// D/A1 FIFO half empty
+#define DMAS_DAC2_FIFO_HALF_EMPTY  0xA	// D/A2 FIFO half empty
+
+// DMA Local Addresses   (0x40000000+LAS1 offset)
+#define DMALADDR_ADC       0x40000000	// A/D FIFO
+#define DMALADDR_HDIN      0x40000004	// High Speed Digital Input FIFO
+#define DMALADDR_DAC1      0x40000008	// D/A1 FIFO
+#define DMALADDR_DAC2      0x4000000C	// D/A2 FIFO
+
+// Port 0 compare modes (SetDIO0CompareMode)
+#define DIO_MODE_EVENT     0	// Event Mode
+#define DIO_MODE_MATCH     1	// Match Mode
+
+// Digital Table Enable (Port 1 disable)
+#define DTBL_DISABLE       0	// Enable Digital Table
+#define DTBL_ENABLE        1	// Disable Digital Table
+
+// Sampling Signal for High Speed Digital Input (SetHdinStart)
+#define HDIN_SOFTWARE      0x0	// Software Trigger
+#define HDIN_ADC           0x1	// A/D Conversion Signal
+#define HDIN_UTC0          0x2	// User TC out 0
+#define HDIN_UTC1          0x3	// User TC out 1
+#define HDIN_UTC2          0x4	// User TC out 2
+#define HDIN_EPCLK         0x5	// External Pacer Clock
+#define HDIN_ETRG          0x6	// External Trigger
+
+// Channel Gain Table / Channel Gain Latch
+#define CSC_LATCH          0	// Channel Gain Latch mode
+#define CSC_CGT            1	// Channel Gain Table mode
+
+// Channel Gain Table Pause Enable
+#define CGT_PAUSE_DISABLE  0	// Channel Gain Table Pause Disable
+#define CGT_PAUSE_ENABLE   1	// Channel Gain Table Pause Enable
+
+// DAC output type/range (p63)
+#define AOUT_UNIP5         0	// 0..+5 Volt
+#define AOUT_UNIP10        1	// 0..+10 Volt
+#define AOUT_BIP5          2	// -5..+5 Volt
+#define AOUT_BIP10         3	// -10..+10 Volt
+
+// Ghannel Gain Table field definitions (p61)
+// Gain
+#define GAIN1              0
+#define GAIN2              1
+#define GAIN4              2
+#define GAIN8              3
+#define GAIN16             4
+#define GAIN32             5
+#define GAIN64             6
+#define GAIN128            7
+
+// Input range/polarity
+#define AIN_BIP5           0	// -5..+5 Volt
+#define AIN_BIP10          1	// -10..+10 Volt
+#define AIN_UNIP10         2	// 0..+10 Volt
+
+// non referenced single ended select bit
+#define NRSE_AGND          0	// AGND referenced SE input
+#define NRSE_AINS          1	// AIN SENSE referenced SE input
+
+// single ended vs differential
+#define GND_SE		0	// Single-Ended
+#define GND_DIFF	1	// Differential
diff --git a/drivers/staging/comedi/drivers/s626.c b/drivers/staging/comedi/drivers/s626.c
new file mode 100644
index 00000000000..469ee8c474c
--- /dev/null
+++ b/drivers/staging/comedi/drivers/s626.c
@@ -0,0 +1,3254 @@
+/*
+  comedi/drivers/s626.c
+  Sensoray s626 Comedi driver
+
+  COMEDI - Linux Control and Measurement Device Interface
+  Copyright (C) 2000 David A. Schleef <ds@schleef.org>
+
+  Based on Sensoray Model 626 Linux driver Version 0.2
+  Copyright (C) 2002-2004 Sensoray Co., Inc.
+
+  This program is free software; you can redistribute it and/or modify
+  it under the terms of the GNU General Public License as published by
+  the Free Software Foundation; either version 2 of the License, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; if not, write to the Free Software
+  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+/*
+Driver: s626
+Description: Sensoray 626 driver
+Devices: [Sensoray] 626 (s626)
+Authors: Gianluca Palli <gpalli@deis.unibo.it>,
+Updated: Fri, 15 Feb 2008 10:28:42 +0000
+Status: experimental
+
+Configuration options:
+  [0] - PCI bus of device (optional)
+  [1] - PCI slot of device (optional)
+  If bus/slot is not specified, the first supported
+  PCI device found will be used.
+
+INSN_CONFIG instructions:
+  analog input:
+   none
+
+  analog output:
+   none
+
+  digital channel:
+   s626 has 3 dio subdevices (2,3 and 4) each with 16 i/o channels
+   supported configuration options:
+   INSN_CONFIG_DIO_QUERY
+   COMEDI_INPUT
+   COMEDI_OUTPUT
+
+  encoder:
+   Every channel must be configured before reading.
+
+   Example code
+
+   insn.insn=INSN_CONFIG;   //configuration instruction
+   insn.n=1;                //number of operation (must be 1)
+   insn.data=&initialvalue; //initial value loaded into encoder
+                            //during configuration
+   insn.subdev=5;           //encoder subdevice
+   insn.chanspec=CR_PACK(encoder_channel,0,AREF_OTHER); //encoder_channel
+                                                        //to configure
+
+   comedi_do_insn(cf,&insn); //executing configuration
+*/
+
+#include <linux/kernel.h>
+#include <linux/types.h>
+
+#include "../comedidev.h"
+
+#include "comedi_pci.h"
+
+#include "comedi_fc.h"
+#include "s626.h"
+
+MODULE_AUTHOR("Gianluca Palli <gpalli@deis.unibo.it>");
+MODULE_DESCRIPTION("Sensoray 626 Comedi driver module");
+MODULE_LICENSE("GPL");
+
+typedef struct s626_board_struct {
+	const char *name;
+	int ai_chans;
+	int ai_bits;
+	int ao_chans;
+	int ao_bits;
+	int dio_chans;
+	int dio_banks;
+	int enc_chans;
+} s626_board;
+
+static const s626_board s626_boards[] = {
+	{
+	      name:	"s626",
+	      ai_chans:S626_ADC_CHANNELS,
+	      ai_bits:	14,
+	      ao_chans:S626_DAC_CHANNELS,
+	      ao_bits:	13,
+	      dio_chans:S626_DIO_CHANNELS,
+	      dio_banks:S626_DIO_BANKS,
+	      enc_chans:S626_ENCODER_CHANNELS,
+		}
+};
+
+#define thisboard ((const s626_board *)dev->board_ptr)
+#define PCI_VENDOR_ID_S626 0x1131
+#define PCI_DEVICE_ID_S626 0x7146
+
+static DEFINE_PCI_DEVICE_TABLE(s626_pci_table) = {
+	{PCI_VENDOR_ID_S626, PCI_DEVICE_ID_S626, PCI_ANY_ID, PCI_ANY_ID, 0, 0,
+		0},
+	{0}
+};
+
+MODULE_DEVICE_TABLE(pci, s626_pci_table);
+
+static int s626_attach(comedi_device * dev, comedi_devconfig * it);
+static int s626_detach(comedi_device * dev);
+
+static comedi_driver driver_s626 = {
+      driver_name:"s626",
+      module:THIS_MODULE,
+      attach:s626_attach,
+      detach:s626_detach,
+};
+
+typedef struct {
+	struct pci_dev *pdev;
+	void *base_addr;
+	int got_regions;
+	short allocatedBuf;
+	uint8_t ai_cmd_running;	// ai_cmd is running
+	uint8_t ai_continous;	// continous aquisition
+	int ai_sample_count;	// number of samples to aquire
+	unsigned int ai_sample_timer;	// time between samples in
+	// units of the timer
+	int ai_convert_count;	// conversion counter
+	unsigned int ai_convert_timer;	// time between conversion in
+	// units of the timer
+	uint16_t CounterIntEnabs;	//Counter interrupt enable
+	//mask for MISC2 register.
+	uint8_t AdcItems;	//Number of items in ADC poll
+	//list.
+	DMABUF RPSBuf;		//DMA buffer used to hold ADC
+	//(RPS1) program.
+	DMABUF ANABuf;		//DMA buffer used to receive
+	//ADC data and hold DAC data.
+	uint32_t *pDacWBuf;	//Pointer to logical adrs of
+	//DMA buffer used to hold DAC
+	//data.
+	uint16_t Dacpol;	//Image of DAC polarity
+	//register.
+	uint8_t TrimSetpoint[12];	//Images of TrimDAC setpoints.
+	//registers.
+	uint16_t ChargeEnabled;	//Image of MISC2 Battery
+	//Charge Enabled (0 or
+	//WRMISC2_CHARGE_ENABLE).
+	uint16_t WDInterval;	//Image of MISC2 watchdog
+	//interval control bits.
+	uint32_t I2CAdrs;	//I2C device address for
+	//onboard EEPROM (board rev
+	//dependent).
+	//  short         I2Cards;
+	lsampl_t ao_readback[S626_DAC_CHANNELS];
+} s626_private;
+
+typedef struct {
+	uint16_t RDDIn;
+	uint16_t WRDOut;
+	uint16_t RDEdgSel;
+	uint16_t WREdgSel;
+	uint16_t RDCapSel;
+	uint16_t WRCapSel;
+	uint16_t RDCapFlg;
+	uint16_t RDIntSel;
+	uint16_t WRIntSel;
+} dio_private;
+
+static dio_private dio_private_A = {
+      RDDIn:LP_RDDINA,
+      WRDOut:LP_WRDOUTA,
+      RDEdgSel:LP_RDEDGSELA,
+      WREdgSel:LP_WREDGSELA,
+      RDCapSel:LP_RDCAPSELA,
+      WRCapSel:LP_WRCAPSELA,
+      RDCapFlg:LP_RDCAPFLGA,
+      RDIntSel:LP_RDINTSELA,
+      WRIntSel:LP_WRINTSELA,
+};
+
+static dio_private dio_private_B = {
+      RDDIn:LP_RDDINB,
+      WRDOut:LP_WRDOUTB,
+      RDEdgSel:LP_RDEDGSELB,
+      WREdgSel:LP_WREDGSELB,
+      RDCapSel:LP_RDCAPSELB,
+      WRCapSel:LP_WRCAPSELB,
+      RDCapFlg:LP_RDCAPFLGB,
+      RDIntSel:LP_RDINTSELB,
+      WRIntSel:LP_WRINTSELB,
+};
+
+static dio_private dio_private_C = {
+      RDDIn:LP_RDDINC,
+      WRDOut:LP_WRDOUTC,
+      RDEdgSel:LP_RDEDGSELC,
+      WREdgSel:LP_WREDGSELC,
+      RDCapSel:LP_RDCAPSELC,
+      WRCapSel:LP_WRCAPSELC,
+      RDCapFlg:LP_RDCAPFLGC,
+      RDIntSel:LP_RDINTSELC,
+      WRIntSel:LP_WRINTSELC,
+};
+
+/* to group dio devices (48 bits mask and data are not allowed ???)
+static dio_private *dio_private_word[]={
+  &dio_private_A,
+  &dio_private_B,
+  &dio_private_C,
+};
+*/
+
+#define devpriv ((s626_private *)dev->private)
+#define diopriv ((dio_private *)s->private)
+
+COMEDI_PCI_INITCLEANUP_NOMODULE(driver_s626, s626_pci_table);
+
+//ioctl routines
+static int s626_ai_insn_config(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data);
+/* static int s626_ai_rinsn(comedi_device *dev,comedi_subdevice *s,comedi_insn *insn,lsampl_t *data); */
+static int s626_ai_insn_read(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data);
+static int s626_ai_cmd(comedi_device * dev, comedi_subdevice * s);
+static int s626_ai_cmdtest(comedi_device * dev, comedi_subdevice * s,
+	comedi_cmd * cmd);
+static int s626_ai_cancel(comedi_device * dev, comedi_subdevice * s);
+static int s626_ao_winsn(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data);
+static int s626_ao_rinsn(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data);
+static int s626_dio_insn_bits(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data);
+static int s626_dio_insn_config(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data);
+static int s626_dio_set_irq(comedi_device * dev, unsigned int chan);
+static int s626_dio_reset_irq(comedi_device * dev, unsigned int gruop,
+	unsigned int mask);
+static int s626_dio_clear_irq(comedi_device * dev);
+static int s626_enc_insn_config(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data);
+static int s626_enc_insn_read(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data);
+static int s626_enc_insn_write(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data);
+static int s626_ns_to_timer(int *nanosec, int round_mode);
+static int s626_ai_load_polllist(uint8_t * ppl, comedi_cmd * cmd);
+static int s626_ai_inttrig(comedi_device * dev, comedi_subdevice * s,
+	unsigned int trignum);
+static irqreturn_t s626_irq_handler(int irq, void *d PT_REGS_ARG);
+static lsampl_t s626_ai_reg_to_uint(int data);
+/* static lsampl_t s626_uint_to_reg(comedi_subdevice *s, int data); */
+
+//end ioctl routines
+
+//internal routines
+static void s626_dio_init(comedi_device * dev);
+static void ResetADC(comedi_device * dev, uint8_t * ppl);
+static void LoadTrimDACs(comedi_device * dev);
+static void WriteTrimDAC(comedi_device * dev, uint8_t LogicalChan,
+	uint8_t DacData);
+static uint8_t I2Cread(comedi_device * dev, uint8_t addr);
+static uint32_t I2Chandshake(comedi_device * dev, uint32_t val);
+static void SetDAC(comedi_device * dev, uint16_t chan, short dacdata);
+static void SendDAC(comedi_device * dev, uint32_t val);
+static void WriteMISC2(comedi_device * dev, uint16_t NewImage);
+static void DEBItransfer(comedi_device * dev);
+static uint16_t DEBIread(comedi_device * dev, uint16_t addr);
+static void DEBIwrite(comedi_device * dev, uint16_t addr, uint16_t wdata);
+static void DEBIreplace(comedi_device * dev, uint16_t addr, uint16_t mask,
+	uint16_t wdata);
+static void CloseDMAB(comedi_device * dev, DMABUF * pdma, size_t bsize);
+
+// COUNTER OBJECT ------------------------------------------------
+typedef struct enc_private_struct {
+	// Pointers to functions that differ for A and B counters:
+	uint16_t(*GetEnable) (comedi_device * dev, struct enc_private_struct *);	//Return clock enable.
+	uint16_t(*GetIntSrc) (comedi_device * dev, struct enc_private_struct *);	//Return interrupt source.
+	uint16_t(*GetLoadTrig) (comedi_device * dev, struct enc_private_struct *);	//Return preload trigger source.
+	uint16_t(*GetMode) (comedi_device * dev, struct enc_private_struct *);	//Return standardized operating mode.
+	void (*PulseIndex) (comedi_device * dev, struct enc_private_struct *);	//Generate soft index strobe.
+	void (*SetEnable) (comedi_device * dev, struct enc_private_struct *, uint16_t enab);	//Program clock enable.
+	void (*SetIntSrc) (comedi_device * dev, struct enc_private_struct *, uint16_t IntSource);	//Program interrupt source.
+	void (*SetLoadTrig) (comedi_device * dev, struct enc_private_struct *, uint16_t Trig);	//Program preload trigger source.
+	void (*SetMode) (comedi_device * dev, struct enc_private_struct *, uint16_t Setup, uint16_t DisableIntSrc);	//Program standardized operating mode.
+	void (*ResetCapFlags) (comedi_device * dev, struct enc_private_struct *);	//Reset event capture flags.
+
+	uint16_t MyCRA;		//   Address of CRA register.
+	uint16_t MyCRB;		//   Address of CRB register.
+	uint16_t MyLatchLsw;	//   Address of Latch least-significant-word
+	//   register.
+	uint16_t MyEventBits[4];	//   Bit translations for IntSrc -->RDMISC2.
+} enc_private;			//counter object
+
+#define encpriv ((enc_private *)(dev->subdevices+5)->private)
+
+//counters routines
+static void s626_timer_load(comedi_device * dev, enc_private * k, int tick);
+static uint32_t ReadLatch(comedi_device * dev, enc_private * k);
+static void ResetCapFlags_A(comedi_device * dev, enc_private * k);
+static void ResetCapFlags_B(comedi_device * dev, enc_private * k);
+static uint16_t GetMode_A(comedi_device * dev, enc_private * k);
+static uint16_t GetMode_B(comedi_device * dev, enc_private * k);
+static void SetMode_A(comedi_device * dev, enc_private * k, uint16_t Setup,
+	uint16_t DisableIntSrc);
+static void SetMode_B(comedi_device * dev, enc_private * k, uint16_t Setup,
+	uint16_t DisableIntSrc);
+static void SetEnable_A(comedi_device * dev, enc_private * k, uint16_t enab);
+static void SetEnable_B(comedi_device * dev, enc_private * k, uint16_t enab);
+static uint16_t GetEnable_A(comedi_device * dev, enc_private * k);
+static uint16_t GetEnable_B(comedi_device * dev, enc_private * k);
+static void SetLatchSource(comedi_device * dev, enc_private * k,
+	uint16_t value);
+/* static uint16_t GetLatchSource(comedi_device *dev, enc_private *k ); */
+static void SetLoadTrig_A(comedi_device * dev, enc_private * k, uint16_t Trig);
+static void SetLoadTrig_B(comedi_device * dev, enc_private * k, uint16_t Trig);
+static uint16_t GetLoadTrig_A(comedi_device * dev, enc_private * k);
+static uint16_t GetLoadTrig_B(comedi_device * dev, enc_private * k);
+static void SetIntSrc_B(comedi_device * dev, enc_private * k,
+	uint16_t IntSource);
+static void SetIntSrc_A(comedi_device * dev, enc_private * k,
+	uint16_t IntSource);
+static uint16_t GetIntSrc_A(comedi_device * dev, enc_private * k);
+static uint16_t GetIntSrc_B(comedi_device * dev, enc_private * k);
+/* static void SetClkMult(comedi_device *dev, enc_private *k, uint16_t value ) ; */
+/* static uint16_t GetClkMult(comedi_device *dev, enc_private *k ) ; */
+/* static void SetIndexPol(comedi_device *dev, enc_private *k, uint16_t value ); */
+/* static uint16_t GetClkPol(comedi_device *dev, enc_private *k ) ; */
+/* static void SetIndexSrc( comedi_device *dev,enc_private *k, uint16_t value );  */
+/* static uint16_t GetClkSrc( comedi_device *dev,enc_private *k );  */
+/* static void SetIndexSrc( comedi_device *dev,enc_private *k, uint16_t value );  */
+/* static uint16_t GetIndexSrc( comedi_device *dev,enc_private *k );  */
+static void PulseIndex_A(comedi_device * dev, enc_private * k);
+static void PulseIndex_B(comedi_device * dev, enc_private * k);
+static void Preload(comedi_device * dev, enc_private * k, uint32_t value);
+static void CountersInit(comedi_device * dev);
+//end internal routines
+
+/////////////////////////////////////////////////////////////////////////
+// Counter objects constructor.
+
+// Counter overflow/index event flag masks for RDMISC2.
+#define INDXMASK(C)		( 1 << ( ( (C) > 2 ) ? ( (C) * 2 - 1 ) : ( (C) * 2 +  4 ) ) )
+#define OVERMASK(C)		( 1 << ( ( (C) > 2 ) ? ( (C) * 2 + 5 ) : ( (C) * 2 + 10 ) ) )
+#define EVBITS(C)		{ 0, OVERMASK(C), INDXMASK(C), OVERMASK(C) | INDXMASK(C) }
+
+// Translation table to map IntSrc into equivalent RDMISC2 event flag
+// bits.
+//static const uint16_t EventBits[][4] = { EVBITS(0), EVBITS(1), EVBITS(2), EVBITS(3), EVBITS(4), EVBITS(5) };
+
+/* enc_private; */
+static enc_private enc_private_data[] = {
+	{
+	      GetEnable:GetEnable_A,
+	      GetIntSrc:GetIntSrc_A,
+	      GetLoadTrig:GetLoadTrig_A,
+	      GetMode:	GetMode_A,
+	      PulseIndex:PulseIndex_A,
+	      SetEnable:SetEnable_A,
+	      SetIntSrc:SetIntSrc_A,
+	      SetLoadTrig:SetLoadTrig_A,
+	      SetMode:	SetMode_A,
+	      ResetCapFlags:ResetCapFlags_A,
+	      MyCRA:	LP_CR0A,
+	      MyCRB:	LP_CR0B,
+	      MyLatchLsw:LP_CNTR0ALSW,
+	      MyEventBits:EVBITS(0),
+		},
+	{
+	      GetEnable:GetEnable_A,
+	      GetIntSrc:GetIntSrc_A,
+	      GetLoadTrig:GetLoadTrig_A,
+	      GetMode:	GetMode_A,
+	      PulseIndex:PulseIndex_A,
+	      SetEnable:SetEnable_A,
+	      SetIntSrc:SetIntSrc_A,
+	      SetLoadTrig:SetLoadTrig_A,
+	      SetMode:	SetMode_A,
+	      ResetCapFlags:ResetCapFlags_A,
+	      MyCRA:	LP_CR1A,
+	      MyCRB:	LP_CR1B,
+	      MyLatchLsw:LP_CNTR1ALSW,
+	      MyEventBits:EVBITS(1),
+		},
+	{
+	      GetEnable:GetEnable_A,
+	      GetIntSrc:GetIntSrc_A,
+	      GetLoadTrig:GetLoadTrig_A,
+	      GetMode:	GetMode_A,
+	      PulseIndex:PulseIndex_A,
+	      SetEnable:SetEnable_A,
+	      SetIntSrc:SetIntSrc_A,
+	      SetLoadTrig:SetLoadTrig_A,
+	      SetMode:	SetMode_A,
+	      ResetCapFlags:ResetCapFlags_A,
+	      MyCRA:	LP_CR2A,
+	      MyCRB:	LP_CR2B,
+	      MyLatchLsw:LP_CNTR2ALSW,
+	      MyEventBits:EVBITS(2),
+		},
+	{
+	      GetEnable:GetEnable_B,
+	      GetIntSrc:GetIntSrc_B,
+	      GetLoadTrig:GetLoadTrig_B,
+	      GetMode:	GetMode_B,
+	      PulseIndex:PulseIndex_B,
+	      SetEnable:SetEnable_B,
+	      SetIntSrc:SetIntSrc_B,
+	      SetLoadTrig:SetLoadTrig_B,
+	      SetMode:	SetMode_B,
+	      ResetCapFlags:ResetCapFlags_B,
+	      MyCRA:	LP_CR0A,
+	      MyCRB:	LP_CR0B,
+	      MyLatchLsw:LP_CNTR0BLSW,
+	      MyEventBits:EVBITS(3),
+		},
+	{
+	      GetEnable:GetEnable_B,
+	      GetIntSrc:GetIntSrc_B,
+	      GetLoadTrig:GetLoadTrig_B,
+	      GetMode:	GetMode_B,
+	      PulseIndex:PulseIndex_B,
+	      SetEnable:SetEnable_B,
+	      SetIntSrc:SetIntSrc_B,
+	      SetLoadTrig:SetLoadTrig_B,
+	      SetMode:	SetMode_B,
+	      ResetCapFlags:ResetCapFlags_B,
+	      MyCRA:	LP_CR1A,
+	      MyCRB:	LP_CR1B,
+	      MyLatchLsw:LP_CNTR1BLSW,
+	      MyEventBits:EVBITS(4),
+		},
+	{
+	      GetEnable:GetEnable_B,
+	      GetIntSrc:GetIntSrc_B,
+	      GetLoadTrig:GetLoadTrig_B,
+	      GetMode:	GetMode_B,
+	      PulseIndex:PulseIndex_B,
+	      SetEnable:SetEnable_B,
+	      SetIntSrc:SetIntSrc_B,
+	      SetLoadTrig:SetLoadTrig_B,
+	      SetMode:	SetMode_B,
+	      ResetCapFlags:ResetCapFlags_B,
+	      MyCRA:	LP_CR2A,
+	      MyCRB:	LP_CR2B,
+	      MyLatchLsw:LP_CNTR2BLSW,
+	      MyEventBits:EVBITS(5),
+		},
+};
+
+// enab/disable a function or test status bit(s) that are accessed
+// through Main Control Registers 1 or 2.
+#define MC_ENABLE( REGADRS, CTRLWORD )	writel(  ( (uint32_t)( CTRLWORD ) << 16 ) | (uint32_t)( CTRLWORD ),devpriv->base_addr+( REGADRS ) )
+
+#define MC_DISABLE( REGADRS, CTRLWORD )	writel(  (uint32_t)( CTRLWORD ) << 16 , devpriv->base_addr+( REGADRS ) )
+
+#define MC_TEST( REGADRS, CTRLWORD )	( ( readl(devpriv->base_addr+( REGADRS )) & CTRLWORD ) != 0 )
+
+/* #define WR7146(REGARDS,CTRLWORD)
+    writel(CTRLWORD,(uint32_t)(devpriv->base_addr+(REGARDS))) */
+#define WR7146(REGARDS,CTRLWORD) writel(CTRLWORD,devpriv->base_addr+(REGARDS))
+
+/* #define RR7146(REGARDS)
+    readl((uint32_t)(devpriv->base_addr+(REGARDS))) */
+#define RR7146(REGARDS)		readl(devpriv->base_addr+(REGARDS))
+
+#define BUGFIX_STREG(REGADRS)   ( REGADRS - 4 )
+
+// Write a time slot control record to TSL2.
+#define VECTPORT( VECTNUM )		(P_TSL2 + ( (VECTNUM) << 2 ))
+#define SETVECT( VECTNUM, VECTVAL )	WR7146(VECTPORT( VECTNUM ), (VECTVAL))
+
+// Code macros used for constructing I2C command bytes.
+#define I2C_B2(ATTR,VAL)	( ( (ATTR) << 6 ) | ( (VAL) << 24 ) )
+#define I2C_B1(ATTR,VAL)	( ( (ATTR) << 4 ) | ( (VAL) << 16 ) )
+#define I2C_B0(ATTR,VAL)	( ( (ATTR) << 2 ) | ( (VAL) <<  8 ) )
+
+static const comedi_lrange s626_range_table = { 2, {
+			RANGE(-5, 5),
+			RANGE(-10, 10),
+	}
+};
+
+static int s626_attach(comedi_device * dev, comedi_devconfig * it)
+{
+/*   uint8_t	PollList; */
+/*   uint16_t	AdcData; */
+/*   uint16_t	StartVal; */
+/*   uint16_t	index; */
+/*   unsigned int data[16]; */
+	int result;
+	int i;
+	int ret;
+	resource_size_t resourceStart;
+	dma_addr_t appdma;
+	comedi_subdevice *s;
+	struct pci_dev *pdev;
+
+	if (alloc_private(dev, sizeof(s626_private)) < 0)
+		return -ENOMEM;
+
+	for (pdev = pci_get_device(PCI_VENDOR_ID_S626, PCI_DEVICE_ID_S626,
+			NULL); pdev != NULL;
+		pdev = pci_get_device(PCI_VENDOR_ID_S626,
+			PCI_DEVICE_ID_S626, pdev)) {
+		if (it->options[0] || it->options[1]) {
+			if (pdev->bus->number == it->options[0] &&
+				PCI_SLOT(pdev->devfn) == it->options[1]) {
+				/* matches requested bus/slot */
+				break;
+			}
+		} else {
+			/* no bus/slot specified */
+			break;
+		}
+	}
+	devpriv->pdev = pdev;
+
+	if (pdev == NULL) {
+		printk("s626_attach: Board not present!!!\n");
+		return -ENODEV;
+	}
+
+	if ((result = comedi_pci_enable(pdev, "s626")) < 0) {
+		printk("s626_attach: comedi_pci_enable fails\n");
+		return -ENODEV;
+	}
+	devpriv->got_regions = 1;
+
+	resourceStart = pci_resource_start(devpriv->pdev, 0);
+
+	devpriv->base_addr = ioremap(resourceStart, SIZEOF_ADDRESS_SPACE);
+	if (devpriv->base_addr == NULL) {
+		printk("s626_attach: IOREMAP failed\n");
+		return -ENODEV;
+	}
+
+	if (devpriv->base_addr) {
+		//disable master interrupt
+		writel(0, devpriv->base_addr + P_IER);
+
+		//soft reset
+		writel(MC1_SOFT_RESET, devpriv->base_addr + P_MC1);
+
+		//DMA FIXME DMA//
+		DEBUG("s626_attach: DMA ALLOCATION\n");
+
+		//adc buffer allocation
+		devpriv->allocatedBuf = 0;
+
+		if ((devpriv->ANABuf.LogicalBase =
+				pci_alloc_consistent(devpriv->pdev, DMABUF_SIZE,
+					&appdma)) == NULL) {
+			printk("s626_attach: DMA Memory mapping error\n");
+			return -ENOMEM;
+		}
+
+		devpriv->ANABuf.PhysicalBase = appdma;
+
+		DEBUG("s626_attach: AllocDMAB ADC Logical=%p, bsize=%d, Physical=0x%x\n", devpriv->ANABuf.LogicalBase, DMABUF_SIZE, (uint32_t) devpriv->ANABuf.PhysicalBase);
+
+		devpriv->allocatedBuf++;
+
+		if ((devpriv->RPSBuf.LogicalBase =
+				pci_alloc_consistent(devpriv->pdev, DMABUF_SIZE,
+					&appdma)) == NULL) {
+			printk("s626_attach: DMA Memory mapping error\n");
+			return -ENOMEM;
+		}
+
+		devpriv->RPSBuf.PhysicalBase = appdma;
+
+		DEBUG("s626_attach: AllocDMAB RPS Logical=%p, bsize=%d, Physical=0x%x\n", devpriv->RPSBuf.LogicalBase, DMABUF_SIZE, (uint32_t) devpriv->RPSBuf.PhysicalBase);
+
+		devpriv->allocatedBuf++;
+
+	}
+
+	dev->board_ptr = s626_boards;
+	dev->board_name = thisboard->name;
+
+	if (alloc_subdevices(dev, 6) < 0)
+		return -ENOMEM;
+
+	dev->iobase = (unsigned long)devpriv->base_addr;
+	dev->irq = devpriv->pdev->irq;
+
+	//set up interrupt handler
+	if (dev->irq == 0) {
+		printk(" unknown irq (bad)\n");
+	} else {
+		if ((ret = comedi_request_irq(dev->irq, s626_irq_handler,
+					IRQF_SHARED, "s626", dev)) < 0) {
+			printk(" irq not available\n");
+			dev->irq = 0;
+		}
+	}
+
+	DEBUG("s626_attach: -- it opts  %d,%d -- \n",
+		it->options[0], it->options[1]);
+
+	s = dev->subdevices + 0;
+	/* analog input subdevice */
+	dev->read_subdev = s;
+	/* we support single-ended (ground) and differential */
+	s->type = COMEDI_SUBD_AI;
+	s->subdev_flags = SDF_READABLE | SDF_DIFF | SDF_CMD_READ;
+	s->n_chan = thisboard->ai_chans;
+	s->maxdata = (0xffff >> 2);
+	s->range_table = &s626_range_table;
+	s->len_chanlist = thisboard->ai_chans;	/* This is the maximum chanlist
+						   length that the board can
+						   handle */
+	s->insn_config = s626_ai_insn_config;
+	s->insn_read = s626_ai_insn_read;
+	s->do_cmd = s626_ai_cmd;
+	s->do_cmdtest = s626_ai_cmdtest;
+	s->cancel = s626_ai_cancel;
+
+	s = dev->subdevices + 1;
+	/* analog output subdevice */
+	s->type = COMEDI_SUBD_AO;
+	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
+	s->n_chan = thisboard->ao_chans;
+	s->maxdata = (0x3fff);
+	s->range_table = &range_bipolar10;
+	s->insn_write = s626_ao_winsn;
+	s->insn_read = s626_ao_rinsn;
+
+	s = dev->subdevices + 2;
+	/* digital I/O subdevice */
+	s->type = COMEDI_SUBD_DIO;
+	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
+	s->n_chan = S626_DIO_CHANNELS;
+	s->maxdata = 1;
+	s->io_bits = 0xffff;
+	s->private = &dio_private_A;
+	s->range_table = &range_digital;
+	s->insn_config = s626_dio_insn_config;
+	s->insn_bits = s626_dio_insn_bits;
+
+	s = dev->subdevices + 3;
+	/* digital I/O subdevice */
+	s->type = COMEDI_SUBD_DIO;
+	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
+	s->n_chan = 16;
+	s->maxdata = 1;
+	s->io_bits = 0xffff;
+	s->private = &dio_private_B;
+	s->range_table = &range_digital;
+	s->insn_config = s626_dio_insn_config;
+	s->insn_bits = s626_dio_insn_bits;
+
+	s = dev->subdevices + 4;
+	/* digital I/O subdevice */
+	s->type = COMEDI_SUBD_DIO;
+	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
+	s->n_chan = 16;
+	s->maxdata = 1;
+	s->io_bits = 0xffff;
+	s->private = &dio_private_C;
+	s->range_table = &range_digital;
+	s->insn_config = s626_dio_insn_config;
+	s->insn_bits = s626_dio_insn_bits;
+
+	s = dev->subdevices + 5;
+	/* encoder (counter) subdevice */
+	s->type = COMEDI_SUBD_COUNTER;
+	s->subdev_flags = SDF_WRITABLE | SDF_READABLE | SDF_LSAMPL;
+	s->n_chan = thisboard->enc_chans;
+	s->private = enc_private_data;
+	s->insn_config = s626_enc_insn_config;
+	s->insn_read = s626_enc_insn_read;
+	s->insn_write = s626_enc_insn_write;
+	s->maxdata = 0xffffff;
+	s->range_table = &range_unknown;
+
+	//stop ai_command
+	devpriv->ai_cmd_running = 0;
+
+	if (devpriv->base_addr && (devpriv->allocatedBuf == 2)) {
+		dma_addr_t pPhysBuf;
+		uint16_t chan;
+
+		// enab DEBI and audio pins, enable I2C interface.
+		MC_ENABLE(P_MC1, MC1_DEBI | MC1_AUDIO | MC1_I2C);
+		// Configure DEBI operating mode.
+		WR7146(P_DEBICFG, DEBI_CFG_SLAVE16	// Local bus is 16
+			// bits wide.
+			| (DEBI_TOUT << DEBI_CFG_TOUT_BIT)	// Declare DEBI
+			// transfer timeout
+			// interval.
+			| DEBI_SWAP	// Set up byte lane
+			// steering.
+			| DEBI_CFG_INTEL);	// Intel-compatible
+		// local bus (DEBI
+		// never times out).
+		DEBUG("s626_attach: %d debi init -- %d\n",
+			DEBI_CFG_SLAVE16 | (DEBI_TOUT << DEBI_CFG_TOUT_BIT) |
+			DEBI_SWAP | DEBI_CFG_INTEL,
+			DEBI_CFG_INTEL | DEBI_CFG_TOQ | DEBI_CFG_INCQ |
+			DEBI_CFG_16Q);
+
+		//DEBI INIT S626 WR7146( P_DEBICFG, DEBI_CFG_INTEL | DEBI_CFG_TOQ
+		//| DEBI_CFG_INCQ| DEBI_CFG_16Q); //end
+
+		// Paging is disabled.
+		WR7146(P_DEBIPAGE, DEBI_PAGE_DISABLE);	// Disable MMU paging.
+
+		// Init GPIO so that ADC Start* is negated.
+		WR7146(P_GPIO, GPIO_BASE | GPIO1_HI);
+
+		//IsBoardRevA is a boolean that indicates whether the board is
+		//RevA.
+
+		// VERSION 2.01 CHANGE: REV A & B BOARDS NOW SUPPORTED BY DYNAMIC
+		// EEPROM ADDRESS SELECTION.  Initialize the I2C interface, which
+		// is used to access the onboard serial EEPROM.  The EEPROM's I2C
+		// DeviceAddress is hardwired to a value that is dependent on the
+		// 626 board revision.  On all board revisions, the EEPROM stores
+		// TrimDAC calibration constants for analog I/O.  On RevB and
+		// higher boards, the DeviceAddress is hardwired to 0 to enable
+		// the EEPROM to also store the PCI SubVendorID and SubDeviceID;
+		// this is the address at which the SAA7146 expects a
+		// configuration EEPROM to reside.  On RevA boards, the EEPROM
+		// device address, which is hardwired to 4, prevents the SAA7146
+		// from retrieving PCI sub-IDs, so the SAA7146 uses its built-in
+		// default values, instead.
+
+		//    devpriv->I2Cards= IsBoardRevA ? 0xA8 : 0xA0; // Set I2C EEPROM
+		// DeviceType (0xA0)
+		// and DeviceAddress<<1.
+
+		devpriv->I2CAdrs = 0xA0;	// I2C device address for onboard
+		// eeprom(revb)
+
+		// Issue an I2C ABORT command to halt any I2C operation in
+		//progress and reset BUSY flag.
+		WR7146(P_I2CSTAT, I2C_CLKSEL | I2C_ABORT);	// Write I2C control:
+		// abort any I2C
+		// activity.
+		MC_ENABLE(P_MC2, MC2_UPLD_IIC);	// Invoke command
+		// upload
+		while ((RR7146(P_MC2) & MC2_UPLD_IIC) == 0) ;	// and wait for
+		// upload to
+		// complete.
+
+		// Per SAA7146 data sheet, write to STATUS reg twice to reset all
+		// I2C error flags.
+		for (i = 0; i < 2; i++) {
+			WR7146(P_I2CSTAT, I2C_CLKSEL);	// Write I2C control: reset
+			// error flags.
+			MC_ENABLE(P_MC2, MC2_UPLD_IIC);	// Invoke command upload
+			while (!MC_TEST(P_MC2, MC2_UPLD_IIC)) ;	//   and wait for
+			//   upload to
+			//   complete.
+		}
+
+		// Init audio interface functional attributes: set DAC/ADC serial
+		// clock rates, invert DAC serial clock so that DAC data setup
+		// times are satisfied, enable DAC serial clock out.
+		WR7146(P_ACON2, ACON2_INIT);
+
+		// Set up TSL1 slot list, which is used to control the
+		// accumulation of ADC data: RSD1 = shift data in on SD1.  SIB_A1
+		// = store data uint8_t at next available location in FB BUFFER1
+		// register.
+		WR7146(P_TSL1, RSD1 | SIB_A1);	// Fetch ADC high data
+		// uint8_t.
+		WR7146(P_TSL1 + 4, RSD1 | SIB_A1 | EOS);	// Fetch ADC low data
+		// uint8_t; end of
+		// TSL1.
+
+		// enab TSL1 slot list so that it executes all the time.
+		WR7146(P_ACON1, ACON1_ADCSTART);
+
+		// Initialize RPS registers used for ADC.
+
+		//Physical start of RPS program.
+		WR7146(P_RPSADDR1, (uint32_t) devpriv->RPSBuf.PhysicalBase);
+
+		WR7146(P_RPSPAGE1, 0);	// RPS program performs no
+		// explicit mem writes.
+		WR7146(P_RPS1_TOUT, 0);	// Disable RPS timeouts.
+
+		// SAA7146 BUG WORKAROUND.  Initialize SAA7146 ADC interface to a
+		// known state by invoking ADCs until FB BUFFER 1 register shows
+		// that it is correctly receiving ADC data.  This is necessary
+		// because the SAA7146 ADC interface does not start up in a
+		// defined state after a PCI reset.
+
+/*     PollList = EOPL;			// Create a simple polling */
+/* 					// list for analog input */
+/* 					// channel 0. */
+/*     ResetADC( dev, &PollList ); */
+
+/*     s626_ai_rinsn(dev,dev->subdevices,NULL,data); //( &AdcData ); // */
+/* 						  //Get initial ADC */
+/* 						  //value. */
+
+/*     StartVal = data[0]; */
+
+/*     // VERSION 2.01 CHANGE: TIMEOUT ADDED TO PREVENT HANGED EXECUTION. */
+/*     // Invoke ADCs until the new ADC value differs from the initial */
+/*     // value or a timeout occurs.  The timeout protects against the */
+/*     // possibility that the driver is restarting and the ADC data is a */
+/*     // fixed value resulting from the applied ADC analog input being */
+/*     // unusually quiet or at the rail. */
+
+/*     for ( index = 0; index < 500; index++ ) */
+/*       { */
+/* 	s626_ai_rinsn(dev,dev->subdevices,NULL,data); */
+/* 	AdcData = data[0];	//ReadADC(  &AdcData ); */
+/* 	if ( AdcData != StartVal ) */
+/* 	  break; */
+/*       } */
+
+		// end initADC
+
+		// init the DAC interface
+
+		// Init Audio2's output DMAC attributes: burst length = 1 DWORD,
+		// threshold = 1 DWORD.
+		WR7146(P_PCI_BT_A, 0);
+
+		// Init Audio2's output DMA physical addresses.  The protection
+		// address is set to 1 DWORD past the base address so that a
+		// single DWORD will be transferred each time a DMA transfer is
+		// enabled.
+
+		pPhysBuf =
+			devpriv->ANABuf.PhysicalBase +
+			(DAC_WDMABUF_OS * sizeof(uint32_t));
+
+		WR7146(P_BASEA2_OUT, (uint32_t) pPhysBuf);	// Buffer base adrs.
+		WR7146(P_PROTA2_OUT, (uint32_t) (pPhysBuf + sizeof(uint32_t)));	// Protection address.
+
+		// Cache Audio2's output DMA buffer logical address.  This is
+		// where DAC data is buffered for A2 output DMA transfers.
+		devpriv->pDacWBuf =
+			(uint32_t *) devpriv->ANABuf.LogicalBase +
+			DAC_WDMABUF_OS;
+
+		// Audio2's output channels does not use paging.  The protection
+		// violation handling bit is set so that the DMAC will
+		// automatically halt and its PCI address pointer will be reset
+		// when the protection address is reached.
+		WR7146(P_PAGEA2_OUT, 8);
+
+		// Initialize time slot list 2 (TSL2), which is used to control
+		// the clock generation for and serialization of data to be sent
+		// to the DAC devices.  Slot 0 is a NOP that is used to trap TSL
+		// execution; this permits other slots to be safely modified
+		// without first turning off the TSL sequencer (which is
+		// apparently impossible to do).  Also, SD3 (which is driven by a
+		// pull-up resistor) is shifted in and stored to the MSB of
+		// FB_BUFFER2 to be used as evidence that the slot sequence has
+		// not yet finished executing.
+		SETVECT(0, XSD2 | RSD3 | SIB_A2 | EOS);	// Slot 0: Trap TSL
+		// execution, shift 0xFF
+		// into FB_BUFFER2.
+
+		// Initialize slot 1, which is constant.  Slot 1 causes a DWORD to
+		// be transferred from audio channel 2's output FIFO to the FIFO's
+		// output buffer so that it can be serialized and sent to the DAC
+		// during subsequent slots.  All remaining slots are dynamically
+		// populated as required by the target DAC device.
+		SETVECT(1, LF_A2);	// Slot 1: Fetch DWORD from Audio2's
+		// output FIFO.
+
+		// Start DAC's audio interface (TSL2) running.
+		WR7146(P_ACON1, ACON1_DACSTART);
+
+		////////////////////////////////////////////////////////
+
+		// end init DAC interface
+
+		// Init Trim DACs to calibrated values.  Do it twice because the
+		// SAA7146 audio channel does not always reset properly and
+		// sometimes causes the first few TrimDAC writes to malfunction.
+
+		LoadTrimDACs(dev);
+		LoadTrimDACs(dev);	// Insurance.
+
+		//////////////////////////////////////////////////////////////////
+		// Manually init all gate array hardware in case this is a soft
+		// reset (we have no way of determining whether this is a warm or
+		// cold start).  This is necessary because the gate array will
+		// reset only in response to a PCI hard reset; there is no soft
+		// reset function.
+
+		// Init all DAC outputs to 0V and init all DAC setpoint and
+		// polarity images.
+		for (chan = 0; chan < S626_DAC_CHANNELS; chan++)
+			SetDAC(dev, chan, 0);
+
+		// Init image of WRMISC2 Battery Charger Enabled control bit.
+		// This image is used when the state of the charger control bit,
+		// which has no direct hardware readback mechanism, is queried.
+		devpriv->ChargeEnabled = 0;
+
+		// Init image of watchdog timer interval in WRMISC2.  This image
+		// maintains the value of the control bits of MISC2 are
+		// continuously reset to zero as long as the WD timer is disabled.
+		devpriv->WDInterval = 0;
+
+		// Init Counter Interrupt enab mask for RDMISC2.  This mask is
+		// applied against MISC2 when testing to determine which timer
+		// events are requesting interrupt service.
+		devpriv->CounterIntEnabs = 0;
+
+		// Init counters.
+		CountersInit(dev);
+
+		// Without modifying the state of the Battery Backup enab, disable
+		// the watchdog timer, set DIO channels 0-5 to operate in the
+		// standard DIO (vs. counter overflow) mode, disable the battery
+		// charger, and reset the watchdog interval selector to zero.
+		WriteMISC2(dev, (uint16_t) (DEBIread(dev,
+					LP_RDMISC2) & MISC2_BATT_ENABLE));
+
+		// Initialize the digital I/O subsystem.
+		s626_dio_init(dev);
+
+		//enable interrupt test
+		// writel(IRQ_GPIO3 | IRQ_RPS1,devpriv->base_addr+P_IER);
+	}
+
+	DEBUG("s626_attach: comedi%d s626 attached %04x\n", dev->minor,
+		(uint32_t) devpriv->base_addr);
+
+	return 1;
+}
+
+static lsampl_t s626_ai_reg_to_uint(int data)
+{
+	lsampl_t tempdata;
+
+	tempdata = (data >> 18);
+	if (tempdata & 0x2000)
+		tempdata &= 0x1fff;
+	else
+		tempdata += (1 << 13);
+
+	return tempdata;
+}
+
+/* static lsampl_t s626_uint_to_reg(comedi_subdevice *s, int data){ */
+/*   return 0; */
+/* } */
+
+static irqreturn_t s626_irq_handler(int irq, void *d PT_REGS_ARG)
+{
+	comedi_device *dev = d;
+	comedi_subdevice *s;
+	comedi_cmd *cmd;
+	enc_private *k;
+	unsigned long flags;
+	int32_t *readaddr;
+	uint32_t irqtype, irqstatus;
+	int i = 0;
+	sampl_t tempdata;
+	uint8_t group;
+	uint16_t irqbit;
+
+	DEBUG("s626_irq_handler: interrupt request recieved!!!\n");
+
+	if (dev->attached == 0)
+		return IRQ_NONE;
+	// lock to avoid race with comedi_poll
+	comedi_spin_lock_irqsave(&dev->spinlock, flags);
+
+	//save interrupt enable register state
+	irqstatus = readl(devpriv->base_addr + P_IER);
+
+	//read interrupt type
+	irqtype = readl(devpriv->base_addr + P_ISR);
+
+	//disable master interrupt
+	writel(0, devpriv->base_addr + P_IER);
+
+	//clear interrupt
+	writel(irqtype, devpriv->base_addr + P_ISR);
+
+	//do somethings
+	DEBUG("s626_irq_handler: interrupt type %d\n", irqtype);
+
+	switch (irqtype) {
+	case IRQ_RPS1:		// end_of_scan occurs
+
+		DEBUG("s626_irq_handler: RPS1 irq detected\n");
+
+		// manage ai subdevice
+		s = dev->subdevices;
+		cmd = &(s->async->cmd);
+
+		// Init ptr to DMA buffer that holds new ADC data.  We skip the
+		// first uint16_t in the buffer because it contains junk data from
+		// the final ADC of the previous poll list scan.
+		readaddr = (int32_t *) devpriv->ANABuf.LogicalBase + 1;
+
+		// get the data and hand it over to comedi
+		for (i = 0; i < (s->async->cmd.chanlist_len); i++) {
+			// Convert ADC data to 16-bit integer values and copy to application
+			// buffer.
+			tempdata = s626_ai_reg_to_uint((int)*readaddr);
+			readaddr++;
+
+			//put data into read buffer
+			// comedi_buf_put(s->async, tempdata);
+			if (cfc_write_to_buffer(s, tempdata) == 0)
+				printk("s626_irq_handler: cfc_write_to_buffer error!\n");
+
+			DEBUG("s626_irq_handler: ai channel %d acquired: %d\n",
+				i, tempdata);
+		}
+
+		//end of scan occurs
+		s->async->events |= COMEDI_CB_EOS;
+
+		if (!(devpriv->ai_continous))
+			devpriv->ai_sample_count--;
+		if (devpriv->ai_sample_count <= 0) {
+			devpriv->ai_cmd_running = 0;
+
+			// Stop RPS program.
+			MC_DISABLE(P_MC1, MC1_ERPS1);
+
+			//send end of acquisition
+			s->async->events |= COMEDI_CB_EOA;
+
+			//disable master interrupt
+			irqstatus = 0;
+		}
+
+		if (devpriv->ai_cmd_running && cmd->scan_begin_src == TRIG_EXT) {
+			DEBUG("s626_irq_handler: enable interrupt on dio channel %d\n", cmd->scan_begin_arg);
+
+			s626_dio_set_irq(dev, cmd->scan_begin_arg);
+
+			DEBUG("s626_irq_handler: External trigger is set!!!\n");
+		}
+		// tell comedi that data is there
+		DEBUG("s626_irq_handler: events %d\n", s->async->events);
+		comedi_event(dev, s);
+		break;
+	case IRQ_GPIO3:	//check dio and conter interrupt
+
+		DEBUG("s626_irq_handler: GPIO3 irq detected\n");
+
+		// manage ai subdevice
+		s = dev->subdevices;
+		cmd = &(s->async->cmd);
+
+		//s626_dio_clear_irq(dev);
+
+		for (group = 0; group < S626_DIO_BANKS; group++) {
+			irqbit = 0;
+			//read interrupt type
+			irqbit = DEBIread(dev,
+				((dio_private *) (dev->subdevices + 2 +
+						group)->private)->RDCapFlg);
+
+			//check if interrupt is generated from dio channels
+			if (irqbit) {
+				s626_dio_reset_irq(dev, group, irqbit);
+				DEBUG("s626_irq_handler: check interrupt on dio group %d %d\n", group, i);
+				if (devpriv->ai_cmd_running) {
+					//check if interrupt is an ai acquisition start trigger
+					if ((irqbit >> (cmd->start_arg -
+								(16 * group)))
+						== 1
+						&& cmd->start_src == TRIG_EXT) {
+						DEBUG("s626_irq_handler: Edge capture interrupt recieved from channel %d\n", cmd->start_arg);
+
+						// Start executing the RPS program.
+						MC_ENABLE(P_MC1, MC1_ERPS1);
+
+						DEBUG("s626_irq_handler: aquisition start triggered!!!\n");
+
+						if (cmd->scan_begin_src ==
+							TRIG_EXT) {
+							DEBUG("s626_ai_cmd: enable interrupt on dio channel %d\n", cmd->scan_begin_arg);
+
+							s626_dio_set_irq(dev,
+								cmd->
+								scan_begin_arg);
+
+							DEBUG("s626_irq_handler: External scan trigger is set!!!\n");
+						}
+					}
+					if ((irqbit >> (cmd->scan_begin_arg -
+								(16 * group)))
+						== 1
+						&& cmd->scan_begin_src ==
+						TRIG_EXT) {
+						DEBUG("s626_irq_handler: Edge capture interrupt recieved from channel %d\n", cmd->scan_begin_arg);
+
+						// Trigger ADC scan loop start by setting RPS Signal 0.
+						MC_ENABLE(P_MC2, MC2_ADC_RPS);
+
+						DEBUG("s626_irq_handler: scan triggered!!! %d\n", devpriv->ai_sample_count);
+						if (cmd->convert_src ==
+							TRIG_EXT) {
+
+							DEBUG("s626_ai_cmd: enable interrupt on dio channel %d group %d\n", cmd->convert_arg - (16 * group), group);
+
+							devpriv->
+								ai_convert_count
+								=
+								cmd->
+								chanlist_len;
+
+							s626_dio_set_irq(dev,
+								cmd->
+								convert_arg);
+
+							DEBUG("s626_irq_handler: External convert trigger is set!!!\n");
+						}
+
+						if (cmd->convert_src ==
+							TRIG_TIMER) {
+							k = &encpriv[5];
+							devpriv->
+								ai_convert_count
+								=
+								cmd->
+								chanlist_len;
+							k->SetEnable(dev, k,
+								CLKENAB_ALWAYS);
+						}
+					}
+					if ((irqbit >> (cmd->convert_arg -
+								(16 * group)))
+						== 1
+						&& cmd->convert_src ==
+						TRIG_EXT) {
+						DEBUG("s626_irq_handler: Edge capture interrupt recieved from channel %d\n", cmd->convert_arg);
+
+						// Trigger ADC scan loop start by setting RPS Signal 0.
+						MC_ENABLE(P_MC2, MC2_ADC_RPS);
+
+						DEBUG("s626_irq_handler: adc convert triggered!!!\n");
+
+						devpriv->ai_convert_count--;
+
+						if (devpriv->ai_convert_count >
+							0) {
+
+							DEBUG("s626_ai_cmd: enable interrupt on dio channel %d group %d\n", cmd->convert_arg - (16 * group), group);
+
+							s626_dio_set_irq(dev,
+								cmd->
+								convert_arg);
+
+							DEBUG("s626_irq_handler: External trigger is set!!!\n");
+						}
+					}
+				}
+				break;
+			}
+		}
+
+		//read interrupt type
+		irqbit = DEBIread(dev, LP_RDMISC2);
+
+		//check interrupt on counters
+		DEBUG("s626_irq_handler: check counters interrupt %d\n",
+			irqbit);
+
+		if (irqbit & IRQ_COINT1A) {
+			DEBUG("s626_irq_handler: interrupt on counter 1A overflow\n");
+			k = &encpriv[0];
+
+			//clear interrupt capture flag
+			k->ResetCapFlags(dev, k);
+		}
+		if (irqbit & IRQ_COINT2A) {
+			DEBUG("s626_irq_handler: interrupt on counter 2A overflow\n");
+			k = &encpriv[1];
+
+			//clear interrupt capture flag
+			k->ResetCapFlags(dev, k);
+		}
+		if (irqbit & IRQ_COINT3A) {
+			DEBUG("s626_irq_handler: interrupt on counter 3A overflow\n");
+			k = &encpriv[2];
+
+			//clear interrupt capture flag
+			k->ResetCapFlags(dev, k);
+		}
+		if (irqbit & IRQ_COINT1B) {
+			DEBUG("s626_irq_handler: interrupt on counter 1B overflow\n");
+			k = &encpriv[3];
+
+			//clear interrupt capture flag
+			k->ResetCapFlags(dev, k);
+		}
+		if (irqbit & IRQ_COINT2B) {
+			DEBUG("s626_irq_handler: interrupt on counter 2B overflow\n");
+			k = &encpriv[4];
+
+			//clear interrupt capture flag
+			k->ResetCapFlags(dev, k);
+
+			if (devpriv->ai_convert_count > 0) {
+				devpriv->ai_convert_count--;
+				if (devpriv->ai_convert_count == 0)
+					k->SetEnable(dev, k, CLKENAB_INDEX);
+
+				if (cmd->convert_src == TRIG_TIMER) {
+					DEBUG("s626_irq_handler: conver timer trigger!!! %d\n", devpriv->ai_convert_count);
+
+					// Trigger ADC scan loop start by setting RPS Signal 0.
+					MC_ENABLE(P_MC2, MC2_ADC_RPS);
+				}
+			}
+		}
+		if (irqbit & IRQ_COINT3B) {
+			DEBUG("s626_irq_handler: interrupt on counter 3B overflow\n");
+			k = &encpriv[5];
+
+			//clear interrupt capture flag
+			k->ResetCapFlags(dev, k);
+
+			if (cmd->scan_begin_src == TRIG_TIMER) {
+				DEBUG("s626_irq_handler: scan timer trigger!!!\n");
+
+				// Trigger ADC scan loop start by setting RPS Signal 0.
+				MC_ENABLE(P_MC2, MC2_ADC_RPS);
+			}
+
+			if (cmd->convert_src == TRIG_TIMER) {
+				DEBUG("s626_irq_handler: convert timer trigger is set\n");
+				k = &encpriv[4];
+				devpriv->ai_convert_count = cmd->chanlist_len;
+				k->SetEnable(dev, k, CLKENAB_ALWAYS);
+			}
+		}
+	}
+
+	//enable interrupt
+	writel(irqstatus, devpriv->base_addr + P_IER);
+
+	DEBUG("s626_irq_handler: exit interrupt service routine.\n");
+
+	comedi_spin_unlock_irqrestore(&dev->spinlock, flags);
+	return IRQ_HANDLED;
+}
+
+static int s626_detach(comedi_device * dev)
+{
+	if (devpriv) {
+		//stop ai_command
+		devpriv->ai_cmd_running = 0;
+
+		if (devpriv->base_addr) {
+			//interrupt mask
+			WR7146(P_IER, 0);	// Disable master interrupt.
+			WR7146(P_ISR, IRQ_GPIO3 | IRQ_RPS1);	// Clear board's IRQ status flag.
+
+			// Disable the watchdog timer and battery charger.
+			WriteMISC2(dev, 0);
+
+			// Close all interfaces on 7146 device.
+			WR7146(P_MC1, MC1_SHUTDOWN);
+			WR7146(P_ACON1, ACON1_BASE);
+
+			CloseDMAB(dev, &devpriv->RPSBuf, DMABUF_SIZE);
+			CloseDMAB(dev, &devpriv->ANABuf, DMABUF_SIZE);
+		}
+
+		if (dev->irq) {
+			comedi_free_irq(dev->irq, dev);
+		}
+
+		if (devpriv->base_addr) {
+			iounmap(devpriv->base_addr);
+		}
+
+		if (devpriv->pdev) {
+			if (devpriv->got_regions) {
+				comedi_pci_disable(devpriv->pdev);
+			}
+			pci_dev_put(devpriv->pdev);
+		}
+	}
+
+	DEBUG("s626_detach: S626 detached!\n");
+
+	return 0;
+}
+
+/*
+ * this functions build the RPS program for hardware driven acquistion
+ */
+void ResetADC(comedi_device * dev, uint8_t * ppl)
+{
+	register uint32_t *pRPS;
+	uint32_t JmpAdrs;
+	uint16_t i;
+	uint16_t n;
+	uint32_t LocalPPL;
+	comedi_cmd *cmd = &(dev->subdevices->async->cmd);
+
+	// Stop RPS program in case it is currently running.
+	MC_DISABLE(P_MC1, MC1_ERPS1);
+
+	// Set starting logical address to write RPS commands.
+	pRPS = (uint32_t *) devpriv->RPSBuf.LogicalBase;
+
+	// Initialize RPS instruction pointer.
+	WR7146(P_RPSADDR1, (uint32_t) devpriv->RPSBuf.PhysicalBase);
+
+	// Construct RPS program in RPSBuf DMA buffer
+
+	if (cmd != NULL && cmd->scan_begin_src != TRIG_FOLLOW) {
+		DEBUG("ResetADC: scan_begin pause inserted\n");
+		// Wait for Start trigger.
+		*pRPS++ = RPS_PAUSE | RPS_SIGADC;
+		*pRPS++ = RPS_CLRSIGNAL | RPS_SIGADC;
+	}
+	// SAA7146 BUG WORKAROUND Do a dummy DEBI Write.  This is necessary
+	// because the first RPS DEBI Write following a non-RPS DEBI write
+	// seems to always fail.  If we don't do this dummy write, the ADC
+	// gain might not be set to the value required for the first slot in
+	// the poll list; the ADC gain would instead remain unchanged from
+	// the previously programmed value.
+	*pRPS++ = RPS_LDREG | (P_DEBICMD >> 2);	// Write DEBI Write command
+	// and address to shadow RAM.
+	*pRPS++ = DEBI_CMD_WRWORD | LP_GSEL;
+	*pRPS++ = RPS_LDREG | (P_DEBIAD >> 2);	// Write DEBI immediate data
+	// to shadow RAM:
+	*pRPS++ = GSEL_BIPOLAR5V;	// arbitrary immediate data
+	// value.
+	*pRPS++ = RPS_CLRSIGNAL | RPS_DEBI;	// Reset "shadow RAM
+	// uploaded" flag.
+	*pRPS++ = RPS_UPLOAD | RPS_DEBI;	// Invoke shadow RAM upload.
+	*pRPS++ = RPS_PAUSE | RPS_DEBI;	// Wait for shadow upload to finish.
+
+	// Digitize all slots in the poll list. This is implemented as a
+	// for loop to limit the slot count to 16 in case the application
+	// forgot to set the EOPL flag in the final slot.
+	for (devpriv->AdcItems = 0; devpriv->AdcItems < 16; devpriv->AdcItems++) {
+		// Convert application's poll list item to private board class
+		// format.  Each app poll list item is an uint8_t with form
+		// (EOPL,x,x,RANGE,CHAN<3:0>), where RANGE code indicates 0 =
+		// +-10V, 1 = +-5V, and EOPL = End of Poll List marker.
+		LocalPPL =
+			(*ppl << 8) | (*ppl & 0x10 ? GSEL_BIPOLAR5V :
+			GSEL_BIPOLAR10V);
+
+		// Switch ADC analog gain.
+		*pRPS++ = RPS_LDREG | (P_DEBICMD >> 2);	// Write DEBI command
+		// and address to
+		// shadow RAM.
+		*pRPS++ = DEBI_CMD_WRWORD | LP_GSEL;
+		*pRPS++ = RPS_LDREG | (P_DEBIAD >> 2);	// Write DEBI
+		// immediate data to
+		// shadow RAM.
+		*pRPS++ = LocalPPL;
+		*pRPS++ = RPS_CLRSIGNAL | RPS_DEBI;	// Reset "shadow RAM uploaded"
+		// flag.
+		*pRPS++ = RPS_UPLOAD | RPS_DEBI;	// Invoke shadow RAM upload.
+		*pRPS++ = RPS_PAUSE | RPS_DEBI;	// Wait for shadow upload to
+		// finish.
+
+		// Select ADC analog input channel.
+		*pRPS++ = RPS_LDREG | (P_DEBICMD >> 2);	// Write DEBI command
+		// and address to
+		// shadow RAM.
+		*pRPS++ = DEBI_CMD_WRWORD | LP_ISEL;
+		*pRPS++ = RPS_LDREG | (P_DEBIAD >> 2);	// Write DEBI
+		// immediate data to
+		// shadow RAM.
+		*pRPS++ = LocalPPL;
+		*pRPS++ = RPS_CLRSIGNAL | RPS_DEBI;	// Reset "shadow RAM uploaded"
+		// flag.
+		*pRPS++ = RPS_UPLOAD | RPS_DEBI;	// Invoke shadow RAM upload.
+		*pRPS++ = RPS_PAUSE | RPS_DEBI;	// Wait for shadow upload to
+		// finish.
+
+		// Delay at least 10 microseconds for analog input settling.
+		// Instead of padding with NOPs, we use RPS_JUMP instructions
+		// here; this allows us to produce a longer delay than is
+		// possible with NOPs because each RPS_JUMP flushes the RPS'
+		// instruction prefetch pipeline.
+		JmpAdrs =
+			(uint32_t) devpriv->RPSBuf.PhysicalBase +
+			(uint32_t) ((unsigned long)pRPS -
+			(unsigned long)devpriv->RPSBuf.LogicalBase);
+		for (i = 0; i < (10 * RPSCLK_PER_US / 2); i++) {
+			JmpAdrs += 8;	// Repeat to implement time delay:
+			*pRPS++ = RPS_JUMP;	// Jump to next RPS instruction.
+			*pRPS++ = JmpAdrs;
+		}
+
+		if (cmd != NULL && cmd->convert_src != TRIG_NOW) {
+			DEBUG("ResetADC: convert pause inserted\n");
+			// Wait for Start trigger.
+			*pRPS++ = RPS_PAUSE | RPS_SIGADC;
+			*pRPS++ = RPS_CLRSIGNAL | RPS_SIGADC;
+		}
+		// Start ADC by pulsing GPIO1.
+		*pRPS++ = RPS_LDREG | (P_GPIO >> 2);	// Begin ADC Start pulse.
+		*pRPS++ = GPIO_BASE | GPIO1_LO;
+		*pRPS++ = RPS_NOP;
+		// VERSION 2.03 CHANGE: STRETCH OUT ADC START PULSE.
+		*pRPS++ = RPS_LDREG | (P_GPIO >> 2);	// End ADC Start pulse.
+		*pRPS++ = GPIO_BASE | GPIO1_HI;
+
+		// Wait for ADC to complete (GPIO2 is asserted high when ADC not
+		// busy) and for data from previous conversion to shift into FB
+		// BUFFER 1 register.
+		*pRPS++ = RPS_PAUSE | RPS_GPIO2;	// Wait for ADC done.
+
+		// Transfer ADC data from FB BUFFER 1 register to DMA buffer.
+		*pRPS++ = RPS_STREG | (BUGFIX_STREG(P_FB_BUFFER1) >> 2);
+		*pRPS++ =
+			(uint32_t) devpriv->ANABuf.PhysicalBase +
+			(devpriv->AdcItems << 2);
+
+		// If this slot's EndOfPollList flag is set, all channels have
+		// now been processed.
+		if (*ppl++ & EOPL) {
+			devpriv->AdcItems++;	// Adjust poll list item count.
+			break;	// Exit poll list processing loop.
+		}
+	}
+	DEBUG("ResetADC: ADC items %d \n", devpriv->AdcItems);
+
+	// VERSION 2.01 CHANGE: DELAY CHANGED FROM 250NS to 2US.  Allow the
+	// ADC to stabilize for 2 microseconds before starting the final
+	// (dummy) conversion.  This delay is necessary to allow sufficient
+	// time between last conversion finished and the start of the dummy
+	// conversion.  Without this delay, the last conversion's data value
+	// is sometimes set to the previous conversion's data value.
+	for (n = 0; n < (2 * RPSCLK_PER_US); n++)
+		*pRPS++ = RPS_NOP;
+
+	// Start a dummy conversion to cause the data from the last
+	// conversion of interest to be shifted in.
+	*pRPS++ = RPS_LDREG | (P_GPIO >> 2);	// Begin ADC Start pulse.
+	*pRPS++ = GPIO_BASE | GPIO1_LO;
+	*pRPS++ = RPS_NOP;
+	// VERSION 2.03 CHANGE: STRETCH OUT ADC START PULSE.
+	*pRPS++ = RPS_LDREG | (P_GPIO >> 2);	// End ADC Start pulse.
+	*pRPS++ = GPIO_BASE | GPIO1_HI;
+
+	// Wait for the data from the last conversion of interest to arrive
+	// in FB BUFFER 1 register.
+	*pRPS++ = RPS_PAUSE | RPS_GPIO2;	// Wait for ADC done.
+
+	// Transfer final ADC data from FB BUFFER 1 register to DMA buffer.
+	*pRPS++ = RPS_STREG | (BUGFIX_STREG(P_FB_BUFFER1) >> 2);	//
+	*pRPS++ =
+		(uint32_t) devpriv->ANABuf.PhysicalBase +
+		(devpriv->AdcItems << 2);
+
+	// Indicate ADC scan loop is finished.
+	// *pRPS++= RPS_CLRSIGNAL | RPS_SIGADC ;  // Signal ReadADC() that scan is done.
+
+	//invoke interrupt
+	if (devpriv->ai_cmd_running == 1) {
+		DEBUG("ResetADC: insert irq in ADC RPS task\n");
+		*pRPS++ = RPS_IRQ;
+	}
+	// Restart RPS program at its beginning.
+	*pRPS++ = RPS_JUMP;	// Branch to start of RPS program.
+	*pRPS++ = (uint32_t) devpriv->RPSBuf.PhysicalBase;
+
+	// End of RPS program build
+	// ------------------------------------------------------------
+}
+
+/* TO COMPLETE, IF NECESSARY */
+static int s626_ai_insn_config(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data)
+{
+
+	return -EINVAL;
+}
+
+/* static int s626_ai_rinsn(comedi_device *dev,comedi_subdevice *s,comedi_insn *insn,lsampl_t *data) */
+/* { */
+/*   register uint8_t	i; */
+/*   register int32_t	*readaddr; */
+
+/*   DEBUG("as626_ai_rinsn: ai_rinsn enter \n");  */
+
+/*   // Trigger ADC scan loop start by setting RPS Signal 0. */
+/*   MC_ENABLE( P_MC2, MC2_ADC_RPS ); */
+
+/*   // Wait until ADC scan loop is finished (RPS Signal 0 reset). */
+/*   while ( MC_TEST( P_MC2, MC2_ADC_RPS ) ); */
+
+/*   // Init ptr to DMA buffer that holds new ADC data.  We skip the */
+/*   // first uint16_t in the buffer because it contains junk data from */
+/*   // the final ADC of the previous poll list scan. */
+/*   readaddr = (uint32_t *)devpriv->ANABuf.LogicalBase + 1; */
+
+/*   // Convert ADC data to 16-bit integer values and copy to application */
+/*   // buffer.	 */
+/*   for ( i = 0; i < devpriv->AdcItems; i++ ) { */
+/*     *data = s626_ai_reg_to_uint( *readaddr++ ); */
+/*     DEBUG("s626_ai_rinsn: data %d \n",*data); */
+/*     data++; */
+/*   } */
+
+/*   DEBUG("s626_ai_rinsn: ai_rinsn escape \n"); */
+/*   return i; */
+/* } */
+
+static int s626_ai_insn_read(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data)
+{
+	uint16_t chan = CR_CHAN(insn->chanspec);
+	uint16_t range = CR_RANGE(insn->chanspec);
+	uint16_t AdcSpec = 0;
+	uint32_t GpioImage;
+	int n;
+
+/*   //interrupt call test  */
+/*   writel(IRQ_GPIO3,devpriv->base_addr+P_PSR); //Writing a logical 1 */
+/* 					     //into any of the RPS_PSR */
+/* 					     //bits causes the */
+/* 					     //corresponding interrupt */
+/* 					     //to be generated if */
+/* 					     //enabled */
+
+	DEBUG("s626_ai_insn_read: entering\n");
+
+	// Convert application's ADC specification into form
+	// appropriate for register programming.
+	if (range == 0)
+		AdcSpec = (chan << 8) | (GSEL_BIPOLAR5V);
+	else
+		AdcSpec = (chan << 8) | (GSEL_BIPOLAR10V);
+
+	// Switch ADC analog gain.
+	DEBIwrite(dev, LP_GSEL, AdcSpec);	// Set gain.
+
+	// Select ADC analog input channel.
+	DEBIwrite(dev, LP_ISEL, AdcSpec);	// Select channel.
+
+	for (n = 0; n < insn->n; n++) {
+
+		// Delay 10 microseconds for analog input settling.
+		comedi_udelay(10);
+
+		// Start ADC by pulsing GPIO1 low.
+		GpioImage = RR7146(P_GPIO);
+		// Assert ADC Start command
+		WR7146(P_GPIO, GpioImage & ~GPIO1_HI);
+		//   and stretch it out.
+		WR7146(P_GPIO, GpioImage & ~GPIO1_HI);
+		WR7146(P_GPIO, GpioImage & ~GPIO1_HI);
+		// Negate ADC Start command.
+		WR7146(P_GPIO, GpioImage | GPIO1_HI);
+
+		// Wait for ADC to complete (GPIO2 is asserted high when
+		// ADC not busy) and for data from previous conversion to
+		// shift into FB BUFFER 1 register.
+
+		// Wait for ADC done.
+		while (!(RR7146(P_PSR) & PSR_GPIO2)) ;
+
+		// Fetch ADC data.
+		if (n != 0)
+			data[n - 1] = s626_ai_reg_to_uint(RR7146(P_FB_BUFFER1));
+
+		// Allow the ADC to stabilize for 4 microseconds before
+		// starting the next (final) conversion.  This delay is
+		// necessary to allow sufficient time between last
+		// conversion finished and the start of the next
+		// conversion.  Without this delay, the last conversion's
+		// data value is sometimes set to the previous
+		// conversion's data value.
+		comedi_udelay(4);
+	}
+
+	// Start a dummy conversion to cause the data from the
+	// previous conversion to be shifted in.
+	GpioImage = RR7146(P_GPIO);
+
+	//Assert ADC Start command
+	WR7146(P_GPIO, GpioImage & ~GPIO1_HI);
+	//   and stretch it out.
+	WR7146(P_GPIO, GpioImage & ~GPIO1_HI);
+	WR7146(P_GPIO, GpioImage & ~GPIO1_HI);
+	// Negate ADC Start command.
+	WR7146(P_GPIO, GpioImage | GPIO1_HI);
+
+	// Wait for the data to arrive in FB BUFFER 1 register.
+
+	// Wait for ADC done.
+	while (!(RR7146(P_PSR) & PSR_GPIO2)) ;
+
+	// Fetch ADC data from audio interface's input shift
+	// register.
+
+	// Fetch ADC data.
+	if (n != 0)
+		data[n - 1] = s626_ai_reg_to_uint(RR7146(P_FB_BUFFER1));
+
+	DEBUG("s626_ai_insn_read: samples %d, data %d\n", n, data[n - 1]);
+
+	return n;
+}
+
+static int s626_ai_load_polllist(uint8_t * ppl, comedi_cmd * cmd)
+{
+
+	int n;
+
+	for (n = 0; n < cmd->chanlist_len; n++) {
+		if (CR_RANGE((cmd->chanlist)[n]) == 0)
+			ppl[n] = (CR_CHAN((cmd->chanlist)[n])) | (RANGE_5V);
+		else
+			ppl[n] = (CR_CHAN((cmd->chanlist)[n])) | (RANGE_10V);
+	}
+	ppl[n - 1] |= EOPL;
+
+	return n;
+}
+
+static int s626_ai_inttrig(comedi_device * dev, comedi_subdevice * s,
+	unsigned int trignum)
+{
+	if (trignum != 0)
+		return -EINVAL;
+
+	DEBUG("s626_ai_inttrig: trigger adc start...");
+
+	// Start executing the RPS program.
+	MC_ENABLE(P_MC1, MC1_ERPS1);
+
+	s->async->inttrig = NULL;
+
+	DEBUG(" done\n");
+
+	return 1;
+}
+
+/*  TO COMPLETE  */
+static int s626_ai_cmd(comedi_device * dev, comedi_subdevice * s)
+{
+
+	uint8_t ppl[16];
+	comedi_cmd *cmd = &s->async->cmd;
+	enc_private *k;
+	int tick;
+
+	DEBUG("s626_ai_cmd: entering command function\n");
+
+	if (devpriv->ai_cmd_running) {
+		printk("s626_ai_cmd: Another ai_cmd is running %d\n",
+			dev->minor);
+		return -EBUSY;
+	}
+	//disable interrupt
+	writel(0, devpriv->base_addr + P_IER);
+
+	//clear interrupt request
+	writel(IRQ_RPS1 | IRQ_GPIO3, devpriv->base_addr + P_ISR);
+
+	//clear any pending interrupt
+	s626_dio_clear_irq(dev);
+	//  s626_enc_clear_irq(dev);
+
+	//reset ai_cmd_running flag
+	devpriv->ai_cmd_running = 0;
+
+	// test if cmd is valid
+	if (cmd == NULL) {
+		DEBUG("s626_ai_cmd: NULL command\n");
+		return -EINVAL;
+	} else {
+		DEBUG("s626_ai_cmd: command recieved!!!\n");
+	}
+
+	if (dev->irq == 0) {
+		comedi_error(dev,
+			"s626_ai_cmd: cannot run command without an irq");
+		return -EIO;
+	}
+
+	s626_ai_load_polllist(ppl, cmd);
+	devpriv->ai_cmd_running = 1;
+	devpriv->ai_convert_count = 0;
+
+	switch (cmd->scan_begin_src) {
+	case TRIG_FOLLOW:
+		break;
+	case TRIG_TIMER:
+		// set a conter to generate adc trigger at scan_begin_arg interval
+		k = &encpriv[5];
+		tick = s626_ns_to_timer((int *)&cmd->scan_begin_arg,
+			cmd->flags & TRIG_ROUND_MASK);
+
+		//load timer value and enable interrupt
+		s626_timer_load(dev, k, tick);
+		k->SetEnable(dev, k, CLKENAB_ALWAYS);
+
+		DEBUG("s626_ai_cmd: scan trigger timer is set with value %d\n",
+			tick);
+
+		break;
+	case TRIG_EXT:
+		// set the digital line and interrupt for scan trigger
+		if (cmd->start_src != TRIG_EXT)
+			s626_dio_set_irq(dev, cmd->scan_begin_arg);
+
+		DEBUG("s626_ai_cmd: External scan trigger is set!!!\n");
+
+		break;
+	}
+
+	switch (cmd->convert_src) {
+	case TRIG_NOW:
+		break;
+	case TRIG_TIMER:
+		// set a conter to generate adc trigger at convert_arg interval
+		k = &encpriv[4];
+		tick = s626_ns_to_timer((int *)&cmd->convert_arg,
+			cmd->flags & TRIG_ROUND_MASK);
+
+		//load timer value and enable interrupt
+		s626_timer_load(dev, k, tick);
+		k->SetEnable(dev, k, CLKENAB_INDEX);
+
+		DEBUG("s626_ai_cmd: convert trigger timer is set with value %d\n", tick);
+		break;
+	case TRIG_EXT:
+		// set the digital line and interrupt for convert trigger
+		if (cmd->scan_begin_src != TRIG_EXT
+			&& cmd->start_src == TRIG_EXT)
+			s626_dio_set_irq(dev, cmd->convert_arg);
+
+		DEBUG("s626_ai_cmd: External convert trigger is set!!!\n");
+
+		break;
+	}
+
+	switch (cmd->stop_src) {
+	case TRIG_COUNT:
+		// data arrives as one packet
+		devpriv->ai_sample_count = cmd->stop_arg;
+		devpriv->ai_continous = 0;
+		break;
+	case TRIG_NONE:
+		// continous aquisition
+		devpriv->ai_continous = 1;
+		devpriv->ai_sample_count = 0;
+		break;
+	}
+
+	ResetADC(dev, ppl);
+
+	switch (cmd->start_src) {
+	case TRIG_NOW:
+		// Trigger ADC scan loop start by setting RPS Signal 0.
+		// MC_ENABLE( P_MC2, MC2_ADC_RPS );
+
+		// Start executing the RPS program.
+		MC_ENABLE(P_MC1, MC1_ERPS1);
+
+		DEBUG("s626_ai_cmd: ADC triggered\n");
+		s->async->inttrig = NULL;
+		break;
+	case TRIG_EXT:
+		//configure DIO channel for acquisition trigger
+		s626_dio_set_irq(dev, cmd->start_arg);
+
+		DEBUG("s626_ai_cmd: External start trigger is set!!!\n");
+
+		s->async->inttrig = NULL;
+		break;
+	case TRIG_INT:
+		s->async->inttrig = s626_ai_inttrig;
+		break;
+	}
+
+	//enable interrupt
+	writel(IRQ_GPIO3 | IRQ_RPS1, devpriv->base_addr + P_IER);
+
+	DEBUG("s626_ai_cmd: command function terminated\n");
+
+	return 0;
+}
+
+static int s626_ai_cmdtest(comedi_device * dev, comedi_subdevice * s,
+	comedi_cmd * cmd)
+{
+	int err = 0;
+	int tmp;
+
+	/* cmdtest tests a particular command to see if it is valid.  Using
+	 * the cmdtest ioctl, a user can create a valid cmd and then have it
+	 * executes by the cmd ioctl.
+	 *
+	 * cmdtest returns 1,2,3,4 or 0, depending on which tests the
+	 * command passes. */
+
+	/* step 1: make sure trigger sources are trivially valid */
+
+	tmp = cmd->start_src;
+	cmd->start_src &= TRIG_NOW | TRIG_INT | TRIG_EXT;
+	if (!cmd->start_src || tmp != cmd->start_src)
+		err++;
+
+	tmp = cmd->scan_begin_src;
+	cmd->scan_begin_src &= TRIG_TIMER | TRIG_EXT | TRIG_FOLLOW;
+	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
+		err++;
+
+	tmp = cmd->convert_src;
+	cmd->convert_src &= TRIG_TIMER | TRIG_EXT | TRIG_NOW;
+	if (!cmd->convert_src || tmp != cmd->convert_src)
+		err++;
+
+	tmp = cmd->scan_end_src;
+	cmd->scan_end_src &= TRIG_COUNT;
+	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
+		err++;
+
+	tmp = cmd->stop_src;
+	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
+	if (!cmd->stop_src || tmp != cmd->stop_src)
+		err++;
+
+	if (err)
+		return 1;
+
+	/* step 2: make sure trigger sources are unique and mutually
+	   compatible */
+
+	/* note that mutual compatiblity is not an issue here */
+	if (cmd->scan_begin_src != TRIG_TIMER &&
+		cmd->scan_begin_src != TRIG_EXT
+		&& cmd->scan_begin_src != TRIG_FOLLOW)
+		err++;
+	if (cmd->convert_src != TRIG_TIMER &&
+		cmd->convert_src != TRIG_EXT && cmd->convert_src != TRIG_NOW)
+		err++;
+	if (cmd->stop_src != TRIG_COUNT && cmd->stop_src != TRIG_NONE)
+		err++;
+
+	if (err)
+		return 2;
+
+	/* step 3: make sure arguments are trivially compatible */
+
+	if (cmd->start_src != TRIG_EXT && cmd->start_arg != 0) {
+		cmd->start_arg = 0;
+		err++;
+	}
+
+	if (cmd->start_src == TRIG_EXT && cmd->start_arg < 0) {
+		cmd->start_arg = 0;
+		err++;
+	}
+
+	if (cmd->start_src == TRIG_EXT && cmd->start_arg > 39) {
+		cmd->start_arg = 39;
+		err++;
+	}
+
+	if (cmd->scan_begin_src == TRIG_EXT && cmd->scan_begin_arg < 0) {
+		cmd->scan_begin_arg = 0;
+		err++;
+	}
+
+	if (cmd->scan_begin_src == TRIG_EXT && cmd->scan_begin_arg > 39) {
+		cmd->scan_begin_arg = 39;
+		err++;
+	}
+
+	if (cmd->convert_src == TRIG_EXT && cmd->convert_arg < 0) {
+		cmd->convert_arg = 0;
+		err++;
+	}
+
+	if (cmd->convert_src == TRIG_EXT && cmd->convert_arg > 39) {
+		cmd->convert_arg = 39;
+		err++;
+	}
+#define MAX_SPEED	200000	/* in nanoseconds */
+#define MIN_SPEED	2000000000	/* in nanoseconds */
+
+	if (cmd->scan_begin_src == TRIG_TIMER) {
+		if (cmd->scan_begin_arg < MAX_SPEED) {
+			cmd->scan_begin_arg = MAX_SPEED;
+			err++;
+		}
+		if (cmd->scan_begin_arg > MIN_SPEED) {
+			cmd->scan_begin_arg = MIN_SPEED;
+			err++;
+		}
+	} else {
+		/* external trigger */
+		/* should be level/edge, hi/lo specification here */
+		/* should specify multiple external triggers */
+/*     if(cmd->scan_begin_arg>9){ */
+/*       cmd->scan_begin_arg=9; */
+/*       err++; */
+/*     } */
+	}
+	if (cmd->convert_src == TRIG_TIMER) {
+		if (cmd->convert_arg < MAX_SPEED) {
+			cmd->convert_arg = MAX_SPEED;
+			err++;
+		}
+		if (cmd->convert_arg > MIN_SPEED) {
+			cmd->convert_arg = MIN_SPEED;
+			err++;
+		}
+	} else {
+		/* external trigger */
+		/* see above */
+/*     if(cmd->convert_arg>9){ */
+/*       cmd->convert_arg=9; */
+/*       err++; */
+/*     } */
+	}
+
+	if (cmd->scan_end_arg != cmd->chanlist_len) {
+		cmd->scan_end_arg = cmd->chanlist_len;
+		err++;
+	}
+	if (cmd->stop_src == TRIG_COUNT) {
+		if (cmd->stop_arg > 0x00ffffff) {
+			cmd->stop_arg = 0x00ffffff;
+			err++;
+		}
+	} else {
+		/* TRIG_NONE */
+		if (cmd->stop_arg != 0) {
+			cmd->stop_arg = 0;
+			err++;
+		}
+	}
+
+	if (err)
+		return 3;
+
+	/* step 4: fix up any arguments */
+
+	if (cmd->scan_begin_src == TRIG_TIMER) {
+		tmp = cmd->scan_begin_arg;
+		s626_ns_to_timer((int *)&cmd->scan_begin_arg,
+			cmd->flags & TRIG_ROUND_MASK);
+		if (tmp != cmd->scan_begin_arg)
+			err++;
+	}
+	if (cmd->convert_src == TRIG_TIMER) {
+		tmp = cmd->convert_arg;
+		s626_ns_to_timer((int *)&cmd->convert_arg,
+			cmd->flags & TRIG_ROUND_MASK);
+		if (tmp != cmd->convert_arg)
+			err++;
+		if (cmd->scan_begin_src == TRIG_TIMER &&
+			cmd->scan_begin_arg <
+			cmd->convert_arg * cmd->scan_end_arg) {
+			cmd->scan_begin_arg =
+				cmd->convert_arg * cmd->scan_end_arg;
+			err++;
+		}
+	}
+
+	if (err)
+		return 4;
+
+	return 0;
+}
+
+static int s626_ai_cancel(comedi_device * dev, comedi_subdevice * s)
+{
+	// Stop RPS program in case it is currently running.
+	MC_DISABLE(P_MC1, MC1_ERPS1);
+
+	//disable master interrupt
+	writel(0, devpriv->base_addr + P_IER);
+
+	devpriv->ai_cmd_running = 0;
+
+	return 0;
+}
+
+/* This function doesn't require a particular form, this is just what
+ * happens to be used in some of the drivers.  It should convert ns
+ * nanoseconds to a counter value suitable for programming the device.
+ * Also, it should adjust ns so that it cooresponds to the actual time
+ * that the device will use. */
+static int s626_ns_to_timer(int *nanosec, int round_mode)
+{
+	int divider, base;
+
+	base = 500;		//2MHz internal clock
+
+	switch (round_mode) {
+	case TRIG_ROUND_NEAREST:
+	default:
+		divider = (*nanosec + base / 2) / base;
+		break;
+	case TRIG_ROUND_DOWN:
+		divider = (*nanosec) / base;
+		break;
+	case TRIG_ROUND_UP:
+		divider = (*nanosec + base - 1) / base;
+		break;
+	}
+
+	*nanosec = base * divider;
+	return divider - 1;
+}
+
+static int s626_ao_winsn(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data)
+{
+
+	int i;
+	uint16_t chan = CR_CHAN(insn->chanspec);
+	int16_t dacdata;
+
+	for (i = 0; i < insn->n; i++) {
+		dacdata = (int16_t) data[i];
+		devpriv->ao_readback[CR_CHAN(insn->chanspec)] = data[i];
+		dacdata -= (0x1fff);
+
+		SetDAC(dev, chan, dacdata);
+	}
+
+	return i;
+}
+
+static int s626_ao_rinsn(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data)
+{
+	int i;
+
+	for (i = 0; i < insn->n; i++) {
+		data[i] = devpriv->ao_readback[CR_CHAN(insn->chanspec)];
+	}
+
+	return i;
+}
+
+/////////////////////////////////////////////////////////////////////
+///////////////  DIGITAL I/O FUNCTIONS  /////////////////////////////
+/////////////////////////////////////////////////////////////////////
+// All DIO functions address a group of DIO channels by means of
+// "group" argument.  group may be 0, 1 or 2, which correspond to DIO
+// ports A, B and C, respectively.
+/////////////////////////////////////////////////////////////////////
+
+static void s626_dio_init(comedi_device * dev)
+{
+	uint16_t group;
+	comedi_subdevice *s;
+
+	// Prepare to treat writes to WRCapSel as capture disables.
+	DEBIwrite(dev, LP_MISC1, MISC1_NOEDCAP);
+
+	// For each group of sixteen channels ...
+	for (group = 0; group < S626_DIO_BANKS; group++) {
+		s = dev->subdevices + 2 + group;
+		DEBIwrite(dev, diopriv->WRIntSel, 0);	// Disable all interrupts.
+		DEBIwrite(dev, diopriv->WRCapSel, 0xFFFF);	// Disable all event
+		// captures.
+		DEBIwrite(dev, diopriv->WREdgSel, 0);	// Init all DIOs to
+		// default edge
+		// polarity.
+		DEBIwrite(dev, diopriv->WRDOut, 0);	// Program all outputs
+		// to inactive state.
+	}
+	DEBUG("s626_dio_init: DIO initialized \n");
+}
+
+/* DIO devices are slightly special.  Although it is possible to
+ * implement the insn_read/insn_write interface, it is much more
+ * useful to applications if you implement the insn_bits interface.
+ * This allows packed reading/writing of the DIO channels.  The comedi
+ * core can convert between insn_bits and insn_read/write */
+
+static int s626_dio_insn_bits(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data)
+{
+
+	/* Length of data must be 2 (mask and new data, see below) */
+	if (insn->n == 0) {
+		return 0;
+	}
+	if (insn->n != 2) {
+		printk("comedi%d: s626: s626_dio_insn_bits(): Invalid instruction length\n", dev->minor);
+		return -EINVAL;
+	}
+
+	/*
+	 * The insn data consists of a mask in data[0] and the new data in
+	 * data[1]. The mask defines which bits we are concerning about.
+	 * The new data must be anded with the mask.  Each channel
+	 * corresponds to a bit.
+	 */
+	if (data[0]) {
+		/* Check if requested ports are configured for output */
+		if ((s->io_bits & data[0]) != data[0])
+			return -EIO;
+
+		s->state &= ~data[0];
+		s->state |= data[0] & data[1];
+
+		/* Write out the new digital output lines */
+
+		DEBIwrite(dev, diopriv->WRDOut, s->state);
+	}
+	data[1] = DEBIread(dev, diopriv->RDDIn);
+
+	return 2;
+}
+
+static int s626_dio_insn_config(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data)
+{
+
+	switch (data[0]) {
+	case INSN_CONFIG_DIO_QUERY:
+		data[1] =
+			(s->io_bits & (1 << CR_CHAN(insn->
+					chanspec))) ? COMEDI_OUTPUT :
+			COMEDI_INPUT;
+		return insn->n;
+		break;
+	case COMEDI_INPUT:
+		s->io_bits &= ~(1 << CR_CHAN(insn->chanspec));
+		break;
+	case COMEDI_OUTPUT:
+		s->io_bits |= 1 << CR_CHAN(insn->chanspec);
+		break;
+	default:
+		return -EINVAL;
+		break;
+	}
+	DEBIwrite(dev, diopriv->WRDOut, s->io_bits);
+
+	return 1;
+}
+
+static int s626_dio_set_irq(comedi_device * dev, unsigned int chan)
+{
+	unsigned int group;
+	unsigned int bitmask;
+	unsigned int status;
+
+	//select dio bank
+	group = chan / 16;
+	bitmask = 1 << (chan - (16 * group));
+	DEBUG("s626_dio_set_irq: enable interrupt on dio channel %d group %d\n",
+		chan - (16 * group), group);
+
+	//set channel to capture positive edge
+	status = DEBIread(dev,
+		((dio_private *) (dev->subdevices + 2 +
+				group)->private)->RDEdgSel);
+	DEBIwrite(dev,
+		((dio_private *) (dev->subdevices + 2 +
+				group)->private)->WREdgSel, bitmask | status);
+
+	//enable interrupt on selected channel
+	status = DEBIread(dev,
+		((dio_private *) (dev->subdevices + 2 +
+				group)->private)->RDIntSel);
+	DEBIwrite(dev,
+		((dio_private *) (dev->subdevices + 2 +
+				group)->private)->WRIntSel, bitmask | status);
+
+	//enable edge capture write command
+	DEBIwrite(dev, LP_MISC1, MISC1_EDCAP);
+
+	//enable edge capture on selected channel
+	status = DEBIread(dev,
+		((dio_private *) (dev->subdevices + 2 +
+				group)->private)->RDCapSel);
+	DEBIwrite(dev,
+		((dio_private *) (dev->subdevices + 2 +
+				group)->private)->WRCapSel, bitmask | status);
+
+	return 0;
+}
+
+static int s626_dio_reset_irq(comedi_device * dev, unsigned int group,
+	unsigned int mask)
+{
+	DEBUG("s626_dio_reset_irq: disable  interrupt on dio channel %d group %d\n", mask, group);
+
+	//disable edge capture write command
+	DEBIwrite(dev, LP_MISC1, MISC1_NOEDCAP);
+
+	//enable edge capture on selected channel
+	DEBIwrite(dev,
+		((dio_private *) (dev->subdevices + 2 +
+				group)->private)->WRCapSel, mask);
+
+	return 0;
+}
+
+static int s626_dio_clear_irq(comedi_device * dev)
+{
+	unsigned int group;
+
+	//disable edge capture write command
+	DEBIwrite(dev, LP_MISC1, MISC1_NOEDCAP);
+
+	for (group = 0; group < S626_DIO_BANKS; group++) {
+		//clear pending events and interrupt
+		DEBIwrite(dev,
+			((dio_private *) (dev->subdevices + 2 +
+					group)->private)->WRCapSel, 0xffff);
+	}
+
+	return 0;
+}
+
+/* Now this function initializes the value of the counter (data[0])
+   and set the subdevice. To complete with trigger and interrupt
+   configuration */
+static int s626_enc_insn_config(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data)
+{
+	uint16_t Setup = (LOADSRC_INDX << BF_LOADSRC) |	// Preload upon
+		// index.
+		(INDXSRC_SOFT << BF_INDXSRC) |	// Disable hardware index.
+		(CLKSRC_COUNTER << BF_CLKSRC) |	// Operating mode is Counter.
+		(CLKPOL_POS << BF_CLKPOL) |	// Active high clock.
+		//( CNTDIR_UP << BF_CLKPOL ) |      // Count direction is Down.
+		(CLKMULT_1X << BF_CLKMULT) |	// Clock multiplier is 1x.
+		(CLKENAB_INDEX << BF_CLKENAB);
+	/*   uint16_t DisableIntSrc=TRUE; */
+	// uint32_t Preloadvalue;              //Counter initial value
+	uint16_t valueSrclatch = LATCHSRC_AB_READ;
+	uint16_t enab = CLKENAB_ALWAYS;
+	enc_private *k = &encpriv[CR_CHAN(insn->chanspec)];
+
+	DEBUG("s626_enc_insn_config: encoder config\n");
+
+	//  (data==NULL) ? (Preloadvalue=0) : (Preloadvalue=data[0]);
+
+	k->SetMode(dev, k, Setup, TRUE);
+	Preload(dev, k, *(insn->data));
+	k->PulseIndex(dev, k);
+	SetLatchSource(dev, k, valueSrclatch);
+	k->SetEnable(dev, k, (uint16_t) (enab != 0));
+
+	return insn->n;
+}
+
+static int s626_enc_insn_read(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data)
+{
+
+	int n;
+	enc_private *k = &encpriv[CR_CHAN(insn->chanspec)];
+
+	DEBUG("s626_enc_insn_read: encoder read channel %d \n",
+		CR_CHAN(insn->chanspec));
+
+	for (n = 0; n < insn->n; n++)
+		data[n] = ReadLatch(dev, k);
+
+	DEBUG("s626_enc_insn_read: encoder sample %d\n", data[n]);
+
+	return n;
+}
+
+static int s626_enc_insn_write(comedi_device * dev, comedi_subdevice * s,
+	comedi_insn * insn, lsampl_t * data)
+{
+
+	enc_private *k = &encpriv[CR_CHAN(insn->chanspec)];
+
+	DEBUG("s626_enc_insn_write: encoder write channel %d \n",
+		CR_CHAN(insn->chanspec));
+
+	// Set the preload register
+	Preload(dev, k, data[0]);
+
+	// Software index pulse forces the preload register to load
+	// into the counter
+	k->SetLoadTrig(dev, k, 0);
+	k->PulseIndex(dev, k);
+	k->SetLoadTrig(dev, k, 2);
+
+	DEBUG("s626_enc_insn_write: End encoder write\n");
+
+	return 1;
+}
+
+static void s626_timer_load(comedi_device * dev, enc_private * k, int tick)
+{
+	uint16_t Setup = (LOADSRC_INDX << BF_LOADSRC) |	// Preload upon
+		// index.
+		(INDXSRC_SOFT << BF_INDXSRC) |	// Disable hardware index.
+		(CLKSRC_TIMER << BF_CLKSRC) |	// Operating mode is Timer.
+		(CLKPOL_POS << BF_CLKPOL) |	// Active high clock.
+		(CNTDIR_DOWN << BF_CLKPOL) |	// Count direction is Down.
+		(CLKMULT_1X << BF_CLKMULT) |	// Clock multiplier is 1x.
+		(CLKENAB_INDEX << BF_CLKENAB);
+	uint16_t valueSrclatch = LATCHSRC_A_INDXA;
+	//  uint16_t enab=CLKENAB_ALWAYS;
+
+	k->SetMode(dev, k, Setup, FALSE);
+
+	// Set the preload register
+	Preload(dev, k, tick);
+
+	// Software index pulse forces the preload register to load
+	// into the counter
+	k->SetLoadTrig(dev, k, 0);
+	k->PulseIndex(dev, k);
+
+	//set reload on counter overflow
+	k->SetLoadTrig(dev, k, 1);
+
+	//set interrupt on overflow
+	k->SetIntSrc(dev, k, INTSRC_OVER);
+
+	SetLatchSource(dev, k, valueSrclatch);
+	//  k->SetEnable(dev,k,(uint16_t)(enab != 0));
+}
+
+///////////////////////////////////////////////////////////////////////
+/////////////////////  DAC FUNCTIONS /////////////////////////////////
+///////////////////////////////////////////////////////////////////////
+
+// Slot 0 base settings.
+#define VECT0	( XSD2 | RSD3 | SIB_A2 )	// Slot 0 always shifts in
+					 // 0xFF and store it to
+					 // FB_BUFFER2.
+
+// TrimDac LogicalChan-to-PhysicalChan mapping table.
+static uint8_t trimchan[] = { 10, 9, 8, 3, 2, 7, 6, 1, 0, 5, 4 };
+
+// TrimDac LogicalChan-to-EepromAdrs mapping table.
+static uint8_t trimadrs[] =
+	{ 0x40, 0x41, 0x42, 0x50, 0x51, 0x52, 0x53, 0x60, 0x61, 0x62, 0x63 };
+
+static void LoadTrimDACs(comedi_device * dev)
+{
+	register uint8_t i;
+
+	// Copy TrimDac setpoint values from EEPROM to TrimDacs.
+	for (i = 0; i < (sizeof(trimchan) / sizeof(trimchan[0])); i++)
+		WriteTrimDAC(dev, i, I2Cread(dev, trimadrs[i]));
+}
+
+static void WriteTrimDAC(comedi_device * dev, uint8_t LogicalChan,
+	uint8_t DacData)
+{
+	uint32_t chan;
+
+	// Save the new setpoint in case the application needs to read it back later.
+	devpriv->TrimSetpoint[LogicalChan] = (uint8_t) DacData;
+
+	// Map logical channel number to physical channel number.
+	chan = (uint32_t) trimchan[LogicalChan];
+
+	// Set up TSL2 records for TrimDac write operation.  All slots shift
+	// 0xFF in from pulled-up SD3 so that the end of the slot sequence
+	// can be detected.
+	SETVECT(2, XSD2 | XFIFO_1 | WS3);	// Slot 2: Send high uint8_t
+	// to target TrimDac.
+	SETVECT(3, XSD2 | XFIFO_0 | WS3);	// Slot 3: Send low uint8_t to
+	// target TrimDac.
+	SETVECT(4, XSD2 | XFIFO_3 | WS1);	// Slot 4: Send NOP high
+	// uint8_t to DAC0 to keep
+	// clock running.
+	SETVECT(5, XSD2 | XFIFO_2 | WS1 | EOS);	// Slot 5: Send NOP low
+	// uint8_t to DAC0.
+
+	// Construct and transmit target DAC's serial packet: ( 0000 AAAA
+	// ),( DDDD DDDD ),( 0x00 ),( 0x00 ) where A<3:0> is the DAC
+	// channel's address, and D<7:0> is the DAC setpoint.  Append a WORD
+	// value (that writes a channel 0 NOP command to a non-existent main
+	// DAC channel) that serves to keep the clock running after the
+	// packet has been sent to the target DAC.
+
+	SendDAC(dev, ((uint32_t) chan << 8)	// Address the DAC channel
+		// within the trimdac device.
+		| (uint32_t) DacData);	// Include DAC setpoint data.
+}
+
+/////////////////////////////////////////////////////////////////////////
+////////////////  EEPROM ACCESS FUNCTIONS  //////////////////////////////
+/////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////
+// Read uint8_t from EEPROM.
+
+static uint8_t I2Cread(comedi_device * dev, uint8_t addr)
+{
+	uint8_t rtnval;
+
+	// Send EEPROM target address.
+	if (I2Chandshake(dev, I2C_B2(I2C_ATTRSTART, I2CW)	// Byte2 = I2C
+			// command:
+			// write to
+			// I2C EEPROM
+			// device.
+			| I2C_B1(I2C_ATTRSTOP, addr)	// Byte1 = EEPROM
+			// internal target
+			// address.
+			| I2C_B0(I2C_ATTRNOP, 0)))	// Byte0 = Not
+		// sent.
+	{
+		// Abort function and declare error if handshake failed.
+		DEBUG("I2Cread: error handshake I2Cread  a\n");
+		return 0;
+	}
+	// Execute EEPROM read.
+	if (I2Chandshake(dev, I2C_B2(I2C_ATTRSTART, I2CR)	// Byte2 = I2C
+			// command: read
+			// from I2C EEPROM
+			// device.
+			| I2C_B1(I2C_ATTRSTOP, 0)	// Byte1 receives
+			// uint8_t from
+			// EEPROM.
+			| I2C_B0(I2C_ATTRNOP, 0)))	// Byte0 = Not
+		// sent.
+	{
+		// Abort function and declare error if handshake failed.
+		DEBUG("I2Cread: error handshake I2Cread b\n");
+		return 0;
+	}
+	// Return copy of EEPROM value.
+	rtnval = (uint8_t) (RR7146(P_I2CCTRL) >> 16);
+	return rtnval;
+}
+
+static uint32_t I2Chandshake(comedi_device * dev, uint32_t val)
+{
+	// Write I2C command to I2C Transfer Control shadow register.
+	WR7146(P_I2CCTRL, val);
+
+	// Upload I2C shadow registers into working registers and wait for
+	// upload confirmation.
+
+	MC_ENABLE(P_MC2, MC2_UPLD_IIC);
+	while (!MC_TEST(P_MC2, MC2_UPLD_IIC)) ;
+
+	// Wait until I2C bus transfer is finished or an error occurs.
+	while ((RR7146(P_I2CCTRL) & (I2C_BUSY | I2C_ERR)) == I2C_BUSY) ;
+
+	// Return non-zero if I2C error occured.
+	return RR7146(P_I2CCTRL) & I2C_ERR;
+
+}
+
+// Private helper function: Write setpoint to an application DAC channel.
+
+static void SetDAC(comedi_device * dev, uint16_t chan, short dacdata)
+{
+	register uint16_t signmask;
+	register uint32_t WSImage;
+
+	// Adjust DAC data polarity and set up Polarity Control Register
+	// image.
+	signmask = 1 << chan;
+	if (dacdata < 0) {
+		dacdata = -dacdata;
+		devpriv->Dacpol |= signmask;
+	} else
+		devpriv->Dacpol &= ~signmask;
+
+	// Limit DAC setpoint value to valid range.
+	if ((uint16_t) dacdata > 0x1FFF)
+		dacdata = 0x1FFF;
+
+	// Set up TSL2 records (aka "vectors") for DAC update.  Vectors V2
+	// and V3 transmit the setpoint to the target DAC.  V4 and V5 send
+	// data to a non-existent TrimDac channel just to keep the clock
+	// running after sending data to the target DAC.  This is necessary
+	// to eliminate the clock glitch that would otherwise occur at the
+	// end of the target DAC's serial data stream.  When the sequence
+	// restarts at V0 (after executing V5), the gate array automatically
+	// disables gating for the DAC clock and all DAC chip selects.
+	WSImage = (chan & 2) ? WS1 : WS2;	// Choose DAC chip select to
+	// be asserted.
+	SETVECT(2, XSD2 | XFIFO_1 | WSImage);	// Slot 2: Transmit high
+	// data byte to target DAC.
+	SETVECT(3, XSD2 | XFIFO_0 | WSImage);	// Slot 3: Transmit low data
+	// byte to target DAC.
+	SETVECT(4, XSD2 | XFIFO_3 | WS3);	// Slot 4: Transmit to
+	// non-existent TrimDac
+	// channel to keep clock
+	SETVECT(5, XSD2 | XFIFO_2 | WS3 | EOS);	// Slot 5: running after
+	// writing target DAC's
+	// low data byte.
+
+	// Construct and transmit target DAC's serial packet: ( A10D DDDD
+	// ),( DDDD DDDD ),( 0x0F ),( 0x00 ) where A is chan<0>, and D<12:0>
+	// is the DAC setpoint.  Append a WORD value (that writes to a
+	// non-existent TrimDac channel) that serves to keep the clock
+	// running after the packet has been sent to the target DAC.
+	SendDAC(dev, 0x0F000000	//Continue clock after target DAC
+		//data (write to non-existent
+		//trimdac).
+		| 0x00004000	// Address the two main dual-DAC
+		// devices (TSL's chip select enables
+		// target device).
+		| ((uint32_t) (chan & 1) << 15)	// Address the DAC
+		// channel within the
+		// device.
+		| (uint32_t) dacdata);	// Include DAC setpoint data.
+
+}
+
+////////////////////////////////////////////////////////
+// Private helper function: Transmit serial data to DAC via Audio
+// channel 2.  Assumes: (1) TSL2 slot records initialized, and (2)
+// Dacpol contains valid target image.
+
+static void SendDAC(comedi_device * dev, uint32_t val)
+{
+
+	// START THE SERIAL CLOCK RUNNING -------------
+
+	// Assert DAC polarity control and enable gating of DAC serial clock
+	// and audio bit stream signals.  At this point in time we must be
+	// assured of being in time slot 0.  If we are not in slot 0, the
+	// serial clock and audio stream signals will be disabled; this is
+	// because the following DEBIwrite statement (which enables signals
+	// to be passed through the gate array) would execute before the
+	// trailing edge of WS1/WS3 (which turns off the signals), thus
+	// causing the signals to be inactive during the DAC write.
+	DEBIwrite(dev, LP_DACPOL, devpriv->Dacpol);
+
+	// TRANSFER OUTPUT DWORD VALUE INTO A2'S OUTPUT FIFO ----------------
+
+	// Copy DAC setpoint value to DAC's output DMA buffer.
+
+	//WR7146( (uint32_t)devpriv->pDacWBuf, val );
+	*devpriv->pDacWBuf = val;
+
+	// enab the output DMA transfer.  This will cause the DMAC to copy
+	// the DAC's data value to A2's output FIFO.  The DMA transfer will
+	// then immediately terminate because the protection address is
+	// reached upon transfer of the first DWORD value.
+	MC_ENABLE(P_MC1, MC1_A2OUT);
+
+	// While the DMA transfer is executing ...
+
+	// Reset Audio2 output FIFO's underflow flag (along with any other
+	// FIFO underflow/overflow flags).  When set, this flag will
+	// indicate that we have emerged from slot 0.
+	WR7146(P_ISR, ISR_AFOU);
+
+	// Wait for the DMA transfer to finish so that there will be data
+	// available in the FIFO when time slot 1 tries to transfer a DWORD
+	// from the FIFO to the output buffer register.  We test for DMA
+	// Done by polling the DMAC enable flag; this flag is automatically
+	// cleared when the transfer has finished.
+	while ((RR7146(P_MC1) & MC1_A2OUT) != 0) ;
+
+	// START THE OUTPUT STREAM TO THE TARGET DAC --------------------
+
+	// FIFO data is now available, so we enable execution of time slots
+	// 1 and higher by clearing the EOS flag in slot 0.  Note that SD3
+	// will be shifted in and stored in FB_BUFFER2 for end-of-slot-list
+	// detection.
+	SETVECT(0, XSD2 | RSD3 | SIB_A2);
+
+	// Wait for slot 1 to execute to ensure that the Packet will be
+	// transmitted.  This is detected by polling the Audio2 output FIFO
+	// underflow flag, which will be set when slot 1 execution has
+	// finished transferring the DAC's data DWORD from the output FIFO
+	// to the output buffer register.
+	while ((RR7146(P_SSR) & SSR_AF2_OUT) == 0) ;
+
+	// Set up to trap execution at slot 0 when the TSL sequencer cycles
+	// back to slot 0 after executing the EOS in slot 5.  Also,
+	// simultaneously shift out and in the 0x00 that is ALWAYS the value
+	// stored in the last byte to be shifted out of the FIFO's DWORD
+	// buffer register.
+	SETVECT(0, XSD2 | XFIFO_2 | RSD2 | SIB_A2 | EOS);
+
+	// WAIT FOR THE TRANSACTION TO FINISH -----------------------
+
+	// Wait for the TSL to finish executing all time slots before
+	// exiting this function.  We must do this so that the next DAC
+	// write doesn't start, thereby enabling clock/chip select signals:
+	// 1. Before the TSL sequence cycles back to slot 0, which disables
+	// the clock/cs signal gating and traps slot // list execution.  If
+	// we have not yet finished slot 5 then the clock/cs signals are
+	// still gated and we have // not finished transmitting the stream.
+	// 2. While slots 2-5 are executing due to a late slot 0 trap.  In
+	// this case, the slot sequence is currently // repeating, but with
+	// clock/cs signals disabled.  We must wait for slot 0 to trap
+	// execution before setting // up the next DAC setpoint DMA transfer
+	// and enabling the clock/cs signals.  To detect the end of slot 5,
+	// we test for the FB_BUFFER2 MSB contents to be equal to 0xFF.  If
+	// the TSL has not yet finished executing slot 5 ...
+	if ((RR7146(P_FB_BUFFER2) & 0xFF000000) != 0) {
+		// The trap was set on time and we are still executing somewhere
+		// in slots 2-5, so we now wait for slot 0 to execute and trap
+		// TSL execution.  This is detected when FB_BUFFER2 MSB changes
+		// from 0xFF to 0x00, which slot 0 causes to happen by shifting
+		// out/in on SD2 the 0x00 that is always referenced by slot 5.
+		while ((RR7146(P_FB_BUFFER2) & 0xFF000000) != 0) ;
+	}
+	// Either (1) we were too late setting the slot 0 trap; the TSL
+	// sequencer restarted slot 0 before we could set the EOS trap flag,
+	// or (2) we were not late and execution is now trapped at slot 0.
+	// In either case, we must now change slot 0 so that it will store
+	// value 0xFF (instead of 0x00) to FB_BUFFER2 next time it executes.
+	// In order to do this, we reprogram slot 0 so that it will shift in
+	// SD3, which is driven only by a pull-up resistor.
+	SETVECT(0, RSD3 | SIB_A2 | EOS);
+
+	// Wait for slot 0 to execute, at which time the TSL is setup for
+	// the next DAC write.  This is detected when FB_BUFFER2 MSB changes
+	// from 0x00 to 0xFF.
+	while ((RR7146(P_FB_BUFFER2) & 0xFF000000) == 0) ;
+}
+
+static void WriteMISC2(comedi_device * dev, uint16_t NewImage)
+{
+	DEBIwrite(dev, LP_MISC1, MISC1_WENABLE);	// enab writes to
+	// MISC2 register.
+	DEBIwrite(dev, LP_WRMISC2, NewImage);	// Write new image to MISC2.
+	DEBIwrite(dev, LP_MISC1, MISC1_WDISABLE);	// Disable writes to MISC2.
+}
+
+/////////////////////////////////////////////////////////////////////
+// Initialize the DEBI interface for all transfers.
+
+static uint16_t DEBIread(comedi_device * dev, uint16_t addr)
+{
+	uint16_t retval;
+
+	// Set up DEBI control register value in shadow RAM.
+	WR7146(P_DEBICMD, DEBI_CMD_RDWORD | addr);
+
+	// Execute the DEBI transfer.
+	DEBItransfer(dev);
+
+	// Fetch target register value.
+	retval = (uint16_t) RR7146(P_DEBIAD);
+
+	// Return register value.
+	return retval;
+}
+
+// Execute a DEBI transfer.  This must be called from within a
+// critical section.
+static void DEBItransfer(comedi_device * dev)
+{
+	// Initiate upload of shadow RAM to DEBI control register.
+	MC_ENABLE(P_MC2, MC2_UPLD_DEBI);
+
+	// Wait for completion of upload from shadow RAM to DEBI control
+	// register.
+	while (!MC_TEST(P_MC2, MC2_UPLD_DEBI)) ;
+
+	// Wait until DEBI transfer is done.
+	while (RR7146(P_PSR) & PSR_DEBI_S) ;
+}
+
+// Write a value to a gate array register.
+static void DEBIwrite(comedi_device * dev, uint16_t addr, uint16_t wdata)
+{
+
+	// Set up DEBI control register value in shadow RAM.
+	WR7146(P_DEBICMD, DEBI_CMD_WRWORD | addr);
+	WR7146(P_DEBIAD, wdata);
+
+	// Execute the DEBI transfer.
+	DEBItransfer(dev);
+}
+
+/////////////////////////////////////////////////////////////////////////////
+// Replace the specified bits in a gate array register.  Imports: mask
+// specifies bits that are to be preserved, wdata is new value to be
+// or'd with the masked original.
+static void DEBIreplace(comedi_device * dev, uint16_t addr, uint16_t mask,
+	uint16_t wdata)
+{
+
+	// Copy target gate array register into P_DEBIAD register.
+	WR7146(P_DEBICMD, DEBI_CMD_RDWORD | addr);	// Set up DEBI control
+	// reg value in shadow
+	// RAM.
+	DEBItransfer(dev);	// Execute the DEBI
+	// Read transfer.
+
+	// Write back the modified image.
+	WR7146(P_DEBICMD, DEBI_CMD_WRWORD | addr);	// Set up DEBI control
+	// reg value in shadow
+	// RAM.
+
+	WR7146(P_DEBIAD, wdata | ((uint16_t) RR7146(P_DEBIAD) & mask));	// Modify the register image.
+	DEBItransfer(dev);	// Execute the DEBI Write transfer.
+}
+
+static void CloseDMAB(comedi_device * dev, DMABUF * pdma, size_t bsize)
+{
+	void *vbptr;
+	dma_addr_t vpptr;
+
+	DEBUG("CloseDMAB: Entering S626DRV_CloseDMAB():\n");
+	if (pdma == NULL)
+		return;
+	//find the matching allocation from the board struct
+
+	vbptr = pdma->LogicalBase;
+	vpptr = pdma->PhysicalBase;
+	if (vbptr) {
+		pci_free_consistent(devpriv->pdev, bsize, vbptr, vpptr);
+		pdma->LogicalBase = 0;
+		pdma->PhysicalBase = 0;
+
+		DEBUG("CloseDMAB(): Logical=%p, bsize=%d, Physical=0x%x\n",
+			vbptr, bsize, (uint32_t) vpptr);
+	}
+}
+
+////////////////////////////////////////////////////////////////////////
+/////////////////  COUNTER FUNCTIONS  //////////////////////////////////
+////////////////////////////////////////////////////////////////////////
+// All counter functions address a specific counter by means of the
+// "Counter" argument, which is a logical counter number.  The Counter
+// argument may have any of the following legal values: 0=0A, 1=1A,
+// 2=2A, 3=0B, 4=1B, 5=2B.
+////////////////////////////////////////////////////////////////////////
+
+// Forward declarations for functions that are common to both A and B
+// counters:
+
+/////////////////////////////////////////////////////////////////////
+//////////////////// PRIVATE COUNTER FUNCTIONS  /////////////////////
+/////////////////////////////////////////////////////////////////////
+
+/////////////////////////////////////////////////////////////////
+// Read a counter's output latch.
+
+static uint32_t ReadLatch(comedi_device * dev, enc_private * k)
+{
+	register uint32_t value;
+	//DEBUG FIXME DEBUG("ReadLatch: Read Latch enter\n");
+
+	// Latch counts and fetch LSW of latched counts value.
+	value = (uint32_t) DEBIread(dev, k->MyLatchLsw);
+
+	// Fetch MSW of latched counts and combine with LSW.
+	value |= ((uint32_t) DEBIread(dev, k->MyLatchLsw + 2) << 16);
+
+	// DEBUG FIXME DEBUG("ReadLatch: Read Latch exit\n");
+
+	// Return latched counts.
+	return value;
+}
+
+///////////////////////////////////////////////////////////////////
+// Reset a counter's index and overflow event capture flags.
+
+static void ResetCapFlags_A(comedi_device * dev, enc_private * k)
+{
+	DEBIreplace(dev, k->MyCRB, (uint16_t) (~CRBMSK_INTCTRL),
+		CRBMSK_INTRESETCMD | CRBMSK_INTRESET_A);
+}
+
+static void ResetCapFlags_B(comedi_device * dev, enc_private * k)
+{
+	DEBIreplace(dev, k->MyCRB, (uint16_t) (~CRBMSK_INTCTRL),
+		CRBMSK_INTRESETCMD | CRBMSK_INTRESET_B);
+}
+
+/////////////////////////////////////////////////////////////////////////
+// Return counter setup in a format (COUNTER_SETUP) that is consistent
+// for both A and B counters.
+
+static uint16_t GetMode_A(comedi_device * dev, enc_private * k)
+{
+	register uint16_t cra;
+	register uint16_t crb;
+	register uint16_t setup;
+
+	// Fetch CRA and CRB register images.
+	cra = DEBIread(dev, k->MyCRA);
+	crb = DEBIread(dev, k->MyCRB);
+
+	// Populate the standardized counter setup bit fields.  Note:
+	// IndexSrc is restricted to ENC_X or IndxPol.
+	setup = ((cra & STDMSK_LOADSRC)	// LoadSrc  = LoadSrcA.
+		| ((crb << (STDBIT_LATCHSRC - CRBBIT_LATCHSRC)) & STDMSK_LATCHSRC)	// LatchSrc = LatchSrcA.
+		| ((cra << (STDBIT_INTSRC - CRABIT_INTSRC_A)) & STDMSK_INTSRC)	// IntSrc   = IntSrcA.
+		| ((cra << (STDBIT_INDXSRC - (CRABIT_INDXSRC_A + 1))) & STDMSK_INDXSRC)	// IndxSrc  = IndxSrcA<1>.
+		| ((cra >> (CRABIT_INDXPOL_A - STDBIT_INDXPOL)) & STDMSK_INDXPOL)	// IndxPol  = IndxPolA.
+		| ((crb >> (CRBBIT_CLKENAB_A - STDBIT_CLKENAB)) & STDMSK_CLKENAB));	// ClkEnab  = ClkEnabA.
+
+	// Adjust mode-dependent parameters.
+	if (cra & (2 << CRABIT_CLKSRC_A))	// If Timer mode (ClkSrcA<1> == 1):
+		setup |= ((CLKSRC_TIMER << STDBIT_CLKSRC)	//   Indicate Timer mode.
+			| ((cra << (STDBIT_CLKPOL - CRABIT_CLKSRC_A)) & STDMSK_CLKPOL)	//   Set ClkPol to indicate count direction (ClkSrcA<0>).
+			| (MULT_X1 << STDBIT_CLKMULT));	//   ClkMult must be 1x in Timer mode.
+
+	else			// If Counter mode (ClkSrcA<1> == 0):
+		setup |= ((CLKSRC_COUNTER << STDBIT_CLKSRC)	//   Indicate Counter mode.
+			| ((cra >> (CRABIT_CLKPOL_A - STDBIT_CLKPOL)) & STDMSK_CLKPOL)	//   Pass through ClkPol.
+			| (((cra & CRAMSK_CLKMULT_A) == (MULT_X0 << CRABIT_CLKMULT_A)) ?	//   Force ClkMult to 1x if not legal, else pass through.
+				(MULT_X1 << STDBIT_CLKMULT) :
+				((cra >> (CRABIT_CLKMULT_A -
+							STDBIT_CLKMULT)) &
+					STDMSK_CLKMULT)));
+
+	// Return adjusted counter setup.
+	return setup;
+}
+
+static uint16_t GetMode_B(comedi_device * dev, enc_private * k)
+{
+	register uint16_t cra;
+	register uint16_t crb;
+	register uint16_t setup;
+
+	// Fetch CRA and CRB register images.
+	cra = DEBIread(dev, k->MyCRA);
+	crb = DEBIread(dev, k->MyCRB);
+
+	// Populate the standardized counter setup bit fields.  Note:
+	// IndexSrc is restricted to ENC_X or IndxPol.
+	setup = (((crb << (STDBIT_INTSRC - CRBBIT_INTSRC_B)) & STDMSK_INTSRC)	// IntSrc   = IntSrcB.
+		| ((crb << (STDBIT_LATCHSRC - CRBBIT_LATCHSRC)) & STDMSK_LATCHSRC)	// LatchSrc = LatchSrcB.
+		| ((crb << (STDBIT_LOADSRC - CRBBIT_LOADSRC_B)) & STDMSK_LOADSRC)	// LoadSrc  = LoadSrcB.
+		| ((crb << (STDBIT_INDXPOL - CRBBIT_INDXPOL_B)) & STDMSK_INDXPOL)	// IndxPol  = IndxPolB.
+		| ((crb >> (CRBBIT_CLKENAB_B - STDBIT_CLKENAB)) & STDMSK_CLKENAB)	// ClkEnab  = ClkEnabB.
+		| ((cra >> ((CRABIT_INDXSRC_B + 1) - STDBIT_INDXSRC)) & STDMSK_INDXSRC));	// IndxSrc  = IndxSrcB<1>.
+
+	// Adjust mode-dependent parameters.
+	if ((crb & CRBMSK_CLKMULT_B) == (MULT_X0 << CRBBIT_CLKMULT_B))	// If Extender mode (ClkMultB == MULT_X0):
+		setup |= ((CLKSRC_EXTENDER << STDBIT_CLKSRC)	//   Indicate Extender mode.
+			| (MULT_X1 << STDBIT_CLKMULT)	//   Indicate multiplier is 1x.
+			| ((cra >> (CRABIT_CLKSRC_B - STDBIT_CLKPOL)) & STDMSK_CLKPOL));	//   Set ClkPol equal to Timer count direction (ClkSrcB<0>).
+
+	else if (cra & (2 << CRABIT_CLKSRC_B))	// If Timer mode (ClkSrcB<1> == 1):
+		setup |= ((CLKSRC_TIMER << STDBIT_CLKSRC)	//   Indicate Timer mode.
+			| (MULT_X1 << STDBIT_CLKMULT)	//   Indicate multiplier is 1x.
+			| ((cra >> (CRABIT_CLKSRC_B - STDBIT_CLKPOL)) & STDMSK_CLKPOL));	//   Set ClkPol equal to Timer count direction (ClkSrcB<0>).
+
+	else			// If Counter mode (ClkSrcB<1> == 0):
+		setup |= ((CLKSRC_COUNTER << STDBIT_CLKSRC)	//   Indicate Timer mode.
+			| ((crb >> (CRBBIT_CLKMULT_B - STDBIT_CLKMULT)) & STDMSK_CLKMULT)	//   Clock multiplier is passed through.
+			| ((crb << (STDBIT_CLKPOL - CRBBIT_CLKPOL_B)) & STDMSK_CLKPOL));	//   Clock polarity is passed through.
+
+	// Return adjusted counter setup.
+	return setup;
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+// Set the operating mode for the specified counter.  The setup
+// parameter is treated as a COUNTER_SETUP data type.  The following
+// parameters are programmable (all other parms are ignored): ClkMult,
+// ClkPol, ClkEnab, IndexSrc, IndexPol, LoadSrc.
+
+static void SetMode_A(comedi_device * dev, enc_private * k, uint16_t Setup,
+	uint16_t DisableIntSrc)
+{
+	register uint16_t cra;
+	register uint16_t crb;
+	register uint16_t setup = Setup;	// Cache the Standard Setup.
+
+	// Initialize CRA and CRB images.
+	cra = ((setup & CRAMSK_LOADSRC_A)	// Preload trigger is passed through.
+		| ((setup & STDMSK_INDXSRC) >> (STDBIT_INDXSRC - (CRABIT_INDXSRC_A + 1))));	// IndexSrc is restricted to ENC_X or IndxPol.
+
+	crb = (CRBMSK_INTRESETCMD | CRBMSK_INTRESET_A	// Reset any pending CounterA event captures.
+		| ((setup & STDMSK_CLKENAB) << (CRBBIT_CLKENAB_A - STDBIT_CLKENAB)));	// Clock enable is passed through.
+
+	// Force IntSrc to Disabled if DisableIntSrc is asserted.
+	if (!DisableIntSrc)
+		cra |= ((setup & STDMSK_INTSRC) >> (STDBIT_INTSRC -
+				CRABIT_INTSRC_A));
+
+	// Populate all mode-dependent attributes of CRA & CRB images.
+	switch ((setup & STDMSK_CLKSRC) >> STDBIT_CLKSRC) {
+	case CLKSRC_EXTENDER:	// Extender Mode: Force to Timer mode
+		// (Extender valid only for B counters).
+
+	case CLKSRC_TIMER:	// Timer Mode:
+		cra |= ((2 << CRABIT_CLKSRC_A)	//   ClkSrcA<1> selects system clock
+			| ((setup & STDMSK_CLKPOL) >> (STDBIT_CLKPOL - CRABIT_CLKSRC_A))	//     with count direction (ClkSrcA<0>) obtained from ClkPol.
+			| (1 << CRABIT_CLKPOL_A)	//   ClkPolA behaves as always-on clock enable.
+			| (MULT_X1 << CRABIT_CLKMULT_A));	//   ClkMult must be 1x.
+		break;
+
+	default:		// Counter Mode:
+		cra |= (CLKSRC_COUNTER	//   Select ENC_C and ENC_D as clock/direction inputs.
+			| ((setup & STDMSK_CLKPOL) << (CRABIT_CLKPOL_A - STDBIT_CLKPOL))	//   Clock polarity is passed through.
+			| (((setup & STDMSK_CLKMULT) == (MULT_X0 << STDBIT_CLKMULT)) ?	//   Force multiplier to x1 if not legal, otherwise pass through.
+				(MULT_X1 << CRABIT_CLKMULT_A) :
+				((setup & STDMSK_CLKMULT) << (CRABIT_CLKMULT_A -
+						STDBIT_CLKMULT))));
+	}
+
+	// Force positive index polarity if IndxSrc is software-driven only,
+	// otherwise pass it through.
+	if (~setup & STDMSK_INDXSRC)
+		cra |= ((setup & STDMSK_INDXPOL) << (CRABIT_INDXPOL_A -
+				STDBIT_INDXPOL));
+
+	// If IntSrc has been forced to Disabled, update the MISC2 interrupt
+	// enable mask to indicate the counter interrupt is disabled.
+	if (DisableIntSrc)
+		devpriv->CounterIntEnabs &= ~k->MyEventBits[3];
+
+	// While retaining CounterB and LatchSrc configurations, program the
+	// new counter operating mode.
+	DEBIreplace(dev, k->MyCRA, CRAMSK_INDXSRC_B | CRAMSK_CLKSRC_B, cra);
+	DEBIreplace(dev, k->MyCRB,
+		(uint16_t) (~(CRBMSK_INTCTRL | CRBMSK_CLKENAB_A)), crb);
+}
+
+static void SetMode_B(comedi_device * dev, enc_private * k, uint16_t Setup,
+	uint16_t DisableIntSrc)
+{
+	register uint16_t cra;
+	register uint16_t crb;
+	register uint16_t setup = Setup;	// Cache the Standard Setup.
+
+	// Initialize CRA and CRB images.
+	cra = ((setup & STDMSK_INDXSRC) << ((CRABIT_INDXSRC_B + 1) - STDBIT_INDXSRC));	// IndexSrc field is restricted to ENC_X or IndxPol.
+
+	crb = (CRBMSK_INTRESETCMD | CRBMSK_INTRESET_B	// Reset event captures and disable interrupts.
+		| ((setup & STDMSK_CLKENAB) << (CRBBIT_CLKENAB_B - STDBIT_CLKENAB))	// Clock enable is passed through.
+		| ((setup & STDMSK_LOADSRC) >> (STDBIT_LOADSRC - CRBBIT_LOADSRC_B)));	// Preload trigger source is passed through.
+
+	// Force IntSrc to Disabled if DisableIntSrc is asserted.
+	if (!DisableIntSrc)
+		crb |= ((setup & STDMSK_INTSRC) >> (STDBIT_INTSRC -
+				CRBBIT_INTSRC_B));
+
+	// Populate all mode-dependent attributes of CRA & CRB images.
+	switch ((setup & STDMSK_CLKSRC) >> STDBIT_CLKSRC) {
+	case CLKSRC_TIMER:	// Timer Mode:
+		cra |= ((2 << CRABIT_CLKSRC_B)	//   ClkSrcB<1> selects system clock
+			| ((setup & STDMSK_CLKPOL) << (CRABIT_CLKSRC_B - STDBIT_CLKPOL)));	//     with direction (ClkSrcB<0>) obtained from ClkPol.
+		crb |= ((1 << CRBBIT_CLKPOL_B)	//   ClkPolB behaves as always-on clock enable.
+			| (MULT_X1 << CRBBIT_CLKMULT_B));	//   ClkMultB must be 1x.
+		break;
+
+	case CLKSRC_EXTENDER:	// Extender Mode:
+		cra |= ((2 << CRABIT_CLKSRC_B)	//   ClkSrcB source is OverflowA (same as "timer")
+			| ((setup & STDMSK_CLKPOL) << (CRABIT_CLKSRC_B - STDBIT_CLKPOL)));	//     with direction obtained from ClkPol.
+		crb |= ((1 << CRBBIT_CLKPOL_B)	//   ClkPolB controls IndexB -- always set to active.
+			| (MULT_X0 << CRBBIT_CLKMULT_B));	//   ClkMultB selects OverflowA as the clock source.
+		break;
+
+	default:		// Counter Mode:
+		cra |= (CLKSRC_COUNTER << CRABIT_CLKSRC_B);	//   Select ENC_C and ENC_D as clock/direction inputs.
+		crb |= (((setup & STDMSK_CLKPOL) >> (STDBIT_CLKPOL - CRBBIT_CLKPOL_B))	//   ClkPol is passed through.
+			| (((setup & STDMSK_CLKMULT) == (MULT_X0 << STDBIT_CLKMULT)) ?	//   Force ClkMult to x1 if not legal, otherwise pass through.
+				(MULT_X1 << CRBBIT_CLKMULT_B) :
+				((setup & STDMSK_CLKMULT) << (CRBBIT_CLKMULT_B -
+						STDBIT_CLKMULT))));
+	}
+
+	// Force positive index polarity if IndxSrc is software-driven only,
+	// otherwise pass it through.
+	if (~setup & STDMSK_INDXSRC)
+		crb |= ((setup & STDMSK_INDXPOL) >> (STDBIT_INDXPOL -
+				CRBBIT_INDXPOL_B));
+
+	// If IntSrc has been forced to Disabled, update the MISC2 interrupt
+	// enable mask to indicate the counter interrupt is disabled.
+	if (DisableIntSrc)
+		devpriv->CounterIntEnabs &= ~k->MyEventBits[3];
+
+	// While retaining CounterA and LatchSrc configurations, program the
+	// new counter operating mode.
+	DEBIreplace(dev, k->MyCRA,
+		(uint16_t) (~(CRAMSK_INDXSRC_B | CRAMSK_CLKSRC_B)), cra);
+	DEBIreplace(dev, k->MyCRB, CRBMSK_CLKENAB_A | CRBMSK_LATCHSRC, crb);
+}
+
+////////////////////////////////////////////////////////////////////////
+// Return/set a counter's enable.  enab: 0=always enabled, 1=enabled by index.
+
+static void SetEnable_A(comedi_device * dev, enc_private * k, uint16_t enab)
+{
+	DEBUG("SetEnable_A: SetEnable_A enter 3541\n");
+	DEBIreplace(dev, k->MyCRB,
+		(uint16_t) (~(CRBMSK_INTCTRL | CRBMSK_CLKENAB_A)),
+		(uint16_t) (enab << CRBBIT_CLKENAB_A));
+}
+
+static void SetEnable_B(comedi_device * dev, enc_private * k, uint16_t enab)
+{
+	DEBIreplace(dev, k->MyCRB,
+		(uint16_t) (~(CRBMSK_INTCTRL | CRBMSK_CLKENAB_B)),
+		(uint16_t) (enab << CRBBIT_CLKENAB_B));
+}
+
+static uint16_t GetEnable_A(comedi_device * dev, enc_private * k)
+{
+	return (DEBIread(dev, k->MyCRB) >> CRBBIT_CLKENAB_A) & 1;
+}
+
+static uint16_t GetEnable_B(comedi_device * dev, enc_private * k)
+{
+	return (DEBIread(dev, k->MyCRB) >> CRBBIT_CLKENAB_B) & 1;
+}
+
+////////////////////////////////////////////////////////////////////////
+// Return/set a counter pair's latch trigger source.  0: On read
+// access, 1: A index latches A, 2: B index latches B, 3: A overflow
+// latches B.
+
+static void SetLatchSource(comedi_device * dev, enc_private * k, uint16_t value)
+{
+	DEBUG("SetLatchSource: SetLatchSource enter 3550 \n");
+	DEBIreplace(dev, k->MyCRB,
+		(uint16_t) (~(CRBMSK_INTCTRL | CRBMSK_LATCHSRC)),
+		(uint16_t) (value << CRBBIT_LATCHSRC));
+
+	DEBUG("SetLatchSource: SetLatchSource exit \n");
+}
+
+/* static uint16_t GetLatchSource(comedi_device *dev, enc_private *k ) */
+/* { */
+/*   return ( DEBIread( dev, k->MyCRB) >> CRBBIT_LATCHSRC ) & 3; */
+/* } */
+
+/////////////////////////////////////////////////////////////////////////
+// Return/set the event that will trigger transfer of the preload
+// register into the counter.  0=ThisCntr_Index, 1=ThisCntr_Overflow,
+// 2=OverflowA (B counters only), 3=disabled.
+
+static void SetLoadTrig_A(comedi_device * dev, enc_private * k, uint16_t Trig)
+{
+	DEBIreplace(dev, k->MyCRA, (uint16_t) (~CRAMSK_LOADSRC_A),
+		(uint16_t) (Trig << CRABIT_LOADSRC_A));
+}
+
+static void SetLoadTrig_B(comedi_device * dev, enc_private * k, uint16_t Trig)
+{
+	DEBIreplace(dev, k->MyCRB,
+		(uint16_t) (~(CRBMSK_LOADSRC_B | CRBMSK_INTCTRL)),
+		(uint16_t) (Trig << CRBBIT_LOADSRC_B));
+}
+
+static uint16_t GetLoadTrig_A(comedi_device * dev, enc_private * k)
+{
+	return (DEBIread(dev, k->MyCRA) >> CRABIT_LOADSRC_A) & 3;
+}
+
+static uint16_t GetLoadTrig_B(comedi_device * dev, enc_private * k)
+{
+	return (DEBIread(dev, k->MyCRB) >> CRBBIT_LOADSRC_B) & 3;
+}
+
+////////////////////
+// Return/set counter interrupt source and clear any captured
+// index/overflow events.  IntSource: 0=Disabled, 1=OverflowOnly,
+// 2=IndexOnly, 3=IndexAndOverflow.
+
+static void SetIntSrc_A(comedi_device * dev, enc_private * k,
+	uint16_t IntSource)
+{
+	// Reset any pending counter overflow or index captures.
+	DEBIreplace(dev, k->MyCRB, (uint16_t) (~CRBMSK_INTCTRL),
+		CRBMSK_INTRESETCMD | CRBMSK_INTRESET_A);
+
+	// Program counter interrupt source.
+	DEBIreplace(dev, k->MyCRA, ~CRAMSK_INTSRC_A,
+		(uint16_t) (IntSource << CRABIT_INTSRC_A));
+
+	// Update MISC2 interrupt enable mask.
+	devpriv->CounterIntEnabs =
+		(devpriv->CounterIntEnabs & ~k->MyEventBits[3]) | k->
+		MyEventBits[IntSource];
+}
+
+static void SetIntSrc_B(comedi_device * dev, enc_private * k,
+	uint16_t IntSource)
+{
+	uint16_t crb;
+
+	// Cache writeable CRB register image.
+	crb = DEBIread(dev, k->MyCRB) & ~CRBMSK_INTCTRL;
+
+	// Reset any pending counter overflow or index captures.
+	DEBIwrite(dev, k->MyCRB,
+		(uint16_t) (crb | CRBMSK_INTRESETCMD | CRBMSK_INTRESET_B));
+
+	// Program counter interrupt source.
+	DEBIwrite(dev, k->MyCRB,
+		(uint16_t) ((crb & ~CRBMSK_INTSRC_B) | (IntSource <<
+				CRBBIT_INTSRC_B)));
+
+	// Update MISC2 interrupt enable mask.
+	devpriv->CounterIntEnabs =
+		(devpriv->CounterIntEnabs & ~k->MyEventBits[3]) | k->
+		MyEventBits[IntSource];
+}
+
+static uint16_t GetIntSrc_A(comedi_device * dev, enc_private * k)
+{
+	return (DEBIread(dev, k->MyCRA) >> CRABIT_INTSRC_A) & 3;
+}
+
+static uint16_t GetIntSrc_B(comedi_device * dev, enc_private * k)
+{
+	return (DEBIread(dev, k->MyCRB) >> CRBBIT_INTSRC_B) & 3;
+}
+
+/////////////////////////////////////////////////////////////////////////
+// Return/set the clock multiplier.
+
+/* static void SetClkMult(comedi_device *dev, enc_private *k, uint16_t value )  */
+/* { */
+/*   k->SetMode(dev, k, (uint16_t)( ( k->GetMode(dev, k ) & ~STDMSK_CLKMULT ) | ( value << STDBIT_CLKMULT ) ), FALSE ); */
+/* } */
+
+/* static uint16_t GetClkMult(comedi_device *dev, enc_private *k )  */
+/* { */
+/*   return ( k->GetMode(dev, k ) >> STDBIT_CLKMULT ) & 3; */
+/* } */
+
+/* ////////////////////////////////////////////////////////////////////////// */
+/* // Return/set the clock polarity. */
+
+/* static void SetClkPol( comedi_device *dev,enc_private *k, uint16_t value )  */
+/* { */
+/*   k->SetMode(dev, k, (uint16_t)( ( k->GetMode(dev, k ) & ~STDMSK_CLKPOL ) | ( value << STDBIT_CLKPOL ) ), FALSE ); */
+/* } */
+
+/* static uint16_t GetClkPol(comedi_device *dev, enc_private *k )  */
+/* { */
+/*   return ( k->GetMode(dev, k ) >> STDBIT_CLKPOL ) & 1; */
+/* } */
+
+/* /////////////////////////////////////////////////////////////////////// */
+/* // Return/set the clock source. */
+
+/* static void SetClkSrc( comedi_device *dev,enc_private *k, uint16_t value )  */
+/* { */
+/*   k->SetMode(dev, k, (uint16_t)( ( k->GetMode(dev, k ) & ~STDMSK_CLKSRC ) | ( value << STDBIT_CLKSRC ) ), FALSE ); */
+/* } */
+
+/* static uint16_t GetClkSrc( comedi_device *dev,enc_private *k )  */
+/* { */
+/*   return ( k->GetMode(dev, k ) >> STDBIT_CLKSRC ) & 3; */
+/* } */
+
+/* //////////////////////////////////////////////////////////////////////// */
+/* // Return/set the index polarity. */
+
+/* static void SetIndexPol(comedi_device *dev, enc_private *k, uint16_t value )  */
+/* { */
+/*   k->SetMode(dev, k, (uint16_t)( ( k->GetMode(dev, k ) & ~STDMSK_INDXPOL ) | ( (value != 0) << STDBIT_INDXPOL ) ), FALSE ); */
+/* } */
+
+/* static uint16_t GetIndexPol(comedi_device *dev, enc_private *k )  */
+/* { */
+/*   return ( k->GetMode(dev, k ) >> STDBIT_INDXPOL ) & 1; */
+/* } */
+
+/* //////////////////////////////////////////////////////////////////////// */
+/* // Return/set the index source. */
+
+/* static void SetIndexSrc(comedi_device *dev, enc_private *k, uint16_t value )  */
+/* { */
+/*   DEBUG("SetIndexSrc: set index src enter 3700\n"); */
+/*   k->SetMode(dev, k, (uint16_t)( ( k->GetMode(dev, k ) & ~STDMSK_INDXSRC ) | ( (value != 0) << STDBIT_INDXSRC ) ), FALSE ); */
+/* } */
+
+/* static uint16_t GetIndexSrc(comedi_device *dev, enc_private *k )  */
+/* { */
+/*   return ( k->GetMode(dev, k ) >> STDBIT_INDXSRC ) & 1; */
+/* } */
+
+///////////////////////////////////////////////////////////////////
+// Generate an index pulse.
+
+static void PulseIndex_A(comedi_device * dev, enc_private * k)
+{
+	register uint16_t cra;
+
+	DEBUG("PulseIndex_A: pulse index enter\n");
+
+	cra = DEBIread(dev, k->MyCRA);	// Pulse index.
+	DEBIwrite(dev, k->MyCRA, (uint16_t) (cra ^ CRAMSK_INDXPOL_A));
+	DEBUG("PulseIndex_A: pulse index step1\n");
+	DEBIwrite(dev, k->MyCRA, cra);
+}
+
+static void PulseIndex_B(comedi_device * dev, enc_private * k)
+{
+	register uint16_t crb;
+
+	crb = DEBIread(dev, k->MyCRB) & ~CRBMSK_INTCTRL;	// Pulse index.
+	DEBIwrite(dev, k->MyCRB, (uint16_t) (crb ^ CRBMSK_INDXPOL_B));
+	DEBIwrite(dev, k->MyCRB, crb);
+}
+
+/////////////////////////////////////////////////////////
+// Write value into counter preload register.
+
+static void Preload(comedi_device * dev, enc_private * k, uint32_t value)
+{
+	DEBUG("Preload: preload enter\n");
+	DEBIwrite(dev, (uint16_t) (k->MyLatchLsw), (uint16_t) value);	// Write value to preload register.
+	DEBUG("Preload: preload step 1\n");
+	DEBIwrite(dev, (uint16_t) (k->MyLatchLsw + 2),
+		(uint16_t) (value >> 16));
+}
+
+static void CountersInit(comedi_device * dev)
+{
+	int chan;
+	enc_private *k;
+	uint16_t Setup = (LOADSRC_INDX << BF_LOADSRC) |	// Preload upon
+		// index.
+		(INDXSRC_SOFT << BF_INDXSRC) |	// Disable hardware index.
+		(CLKSRC_COUNTER << BF_CLKSRC) |	// Operating mode is counter.
+		(CLKPOL_POS << BF_CLKPOL) |	// Active high clock.
+		(CNTDIR_UP << BF_CLKPOL) |	// Count direction is up.
+		(CLKMULT_1X << BF_CLKMULT) |	// Clock multiplier is 1x.
+		(CLKENAB_INDEX << BF_CLKENAB);	// Enabled by index
+
+	// Disable all counter interrupts and clear any captured counter events.
+	for (chan = 0; chan < S626_ENCODER_CHANNELS; chan++) {
+		k = &encpriv[chan];
+		k->SetMode(dev, k, Setup, TRUE);
+		k->SetIntSrc(dev, k, 0);
+		k->ResetCapFlags(dev, k);
+		k->SetEnable(dev, k, CLKENAB_ALWAYS);
+	}
+	DEBUG("CountersInit: counters initialized \n");
+
+}
diff --git a/drivers/staging/comedi/drivers/s626.h b/drivers/staging/comedi/drivers/s626.h
new file mode 100644
index 00000000000..11d8b1ceb0b
--- /dev/null
+++ b/drivers/staging/comedi/drivers/s626.h
@@ -0,0 +1,802 @@
+/*
+  comedi/drivers/s626.h
+  Sensoray s626 Comedi driver, header file
+
+  COMEDI - Linux Control and Measurement Device Interface
+  Copyright (C) 2000 David A. Schleef <ds@schleef.org>
+
+  Based on Sensoray Model 626 Linux driver Version 0.2
+  Copyright (C) 2002-2004 Sensoray Co., Inc.
+
+  This program is free software; you can redistribute it and/or modify
+  it under the terms of the GNU General Public License as published by
+  the Free Software Foundation; either version 2 of the License, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; if not, write to the Free Software
+  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+/*
+  Driver: s626.o (s626.ko)
+  Description: Sensoray 626 driver
+  Devices: Sensoray s626
+  Authors: Gianluca Palli <gpalli@deis.unibo.it>,
+  Updated: Thu, 12 Jul 2005
+  Status: experimental
+
+  Configuration Options:
+  analog input:
+   none
+
+  analog output:
+   none
+
+  digital channel:
+   s626 has 3 dio subdevices (2,3 and 4) each with 16 i/o channels
+   supported configuration options:
+   INSN_CONFIG_DIO_QUERY
+   COMEDI_INPUT
+   COMEDI_OUTPUT
+
+  encoder:
+   Every channel must be configured before reading.
+
+   Example code
+
+   insn.insn=INSN_CONFIG;   //configuration instruction
+   insn.n=1;                //number of operation (must be 1)
+   insn.data=&initialvalue; //initial value loaded into encoder
+                            //during configuration
+   insn.subdev=5;           //encoder subdevice
+   insn.chanspec=CR_PACK(encoder_channel,0,AREF_OTHER); //encoder_channel
+                                                        //to configure
+
+   comedi_do_insn(cf,&insn); //executing configuration
+*/
+
+#ifdef _DEBUG_
+#define DEBUG(...);        rt_printk(__VA_ARGS__);
+#else
+#define DEBUG(...)
+#endif
+
+#if !defined(TRUE)
+#define TRUE    (1)
+#endif
+
+#if !defined(FALSE)
+#define FALSE   (0)
+#endif
+
+#if !defined(EXTERN)
+#if defined(__cplusplus)
+#define EXTERN extern "C"
+#else
+#define EXTERN extern
+#endif
+#endif
+
+#if !defined(INLINE)
+#define INLINE static __inline
+#endif
+
+/////////////////////////////////////////////////////
+#include<linux/slab.h>
+
+#define S626_SIZE 0x0200
+#define SIZEOF_ADDRESS_SPACE		0x0200
+#define DMABUF_SIZE			4096	// 4k pages
+
+#define S626_ADC_CHANNELS       16
+#define S626_DAC_CHANNELS       4
+#define S626_ENCODER_CHANNELS   6
+#define S626_DIO_CHANNELS       48
+#define S626_DIO_BANKS		3	// Number of DIO groups.
+#define S626_DIO_EXTCHANS	40	// Number of
+					// extended-capability
+					// DIO channels.
+
+#define NUM_TRIMDACS	12	// Number of valid TrimDAC channels.
+
+// PCI bus interface types.
+#define INTEL				1	// Intel bus type.
+#define MOTOROLA			2	// Motorola bus type.
+
+//////////////////////////////////////////////////////////
+
+//////////////////////////////////////////////////////////
+#define PLATFORM		INTEL	// *** SELECT PLATFORM TYPE ***
+//////////////////////////////////////////////////////////
+
+#define RANGE_5V                0x10	// +/-5V range
+#define RANGE_10V               0x00	// +/-10V range
+
+#define EOPL			0x80	// End of ADC poll list marker.
+#define GSEL_BIPOLAR5V		0x00F0	// LP_GSEL setting for 5V bipolar range.
+#define GSEL_BIPOLAR10V		0x00A0	// LP_GSEL setting for 10V bipolar range.
+
+// Error codes that must be visible to this base class.
+#define ERR_ILLEGAL_PARM	0x00010000	// Illegal function parameter value was specified.
+#define ERR_I2C			0x00020000	// I2C error.
+#define ERR_COUNTERSETUP	0x00200000	// Illegal setup specified for counter channel.
+#define ERR_DEBI_TIMEOUT	0x00400000	// DEBI transfer timed out.
+
+// Organization (physical order) and size (in DWORDs) of logical DMA buffers contained by ANA_DMABUF.
+#define ADC_DMABUF_DWORDS	40	// ADC DMA buffer must hold 16 samples, plus pre/post garbage samples.
+#define DAC_WDMABUF_DWORDS	1	// DAC output DMA buffer holds a single sample.
+
+// All remaining space in 4KB DMA buffer is available for the RPS1 program.
+
+// Address offsets, in DWORDS, from base of DMA buffer.
+#define DAC_WDMABUF_OS		ADC_DMABUF_DWORDS
+
+// Interrupt enab bit in ISR and IER.
+#define IRQ_GPIO3		0x00000040	// IRQ enable for GPIO3.
+#define IRQ_RPS1                0x10000000
+#define ISR_AFOU		0x00000800	// Audio fifo
+						// under/overflow
+						// detected.
+#define IRQ_COINT1A             0x0400	// conter 1A overflow
+						// interrupt mask
+#define IRQ_COINT1B             0x0800	// conter 1B overflow
+						// interrupt mask
+#define IRQ_COINT2A             0x1000	// conter 2A overflow
+						// interrupt mask
+#define IRQ_COINT2B             0x2000	// conter 2B overflow
+						// interrupt mask
+#define IRQ_COINT3A             0x4000	// conter 3A overflow
+						// interrupt mask
+#define IRQ_COINT3B             0x8000	// conter 3B overflow
+						// interrupt mask
+
+// RPS command codes.
+#define RPS_CLRSIGNAL		0x00000000	// CLEAR SIGNAL
+#define RPS_SETSIGNAL		0x10000000	// SET SIGNAL
+#define RPS_NOP			0x00000000	// NOP
+#define RPS_PAUSE		0x20000000	// PAUSE
+#define RPS_UPLOAD		0x40000000	// UPLOAD
+#define RPS_JUMP		0x80000000	// JUMP
+#define RPS_LDREG		0x90000100	// LDREG (1 uint32_t only)
+#define RPS_STREG		0xA0000100	// STREG (1 uint32_t only)
+#define RPS_STOP		0x50000000	// STOP
+#define RPS_IRQ                 0x60000000	// IRQ
+
+#define RPS_LOGICAL_OR		0x08000000	// Logical OR conditionals.
+#define RPS_INVERT		0x04000000	// Test for negated semaphores.
+#define RPS_DEBI		0x00000002	// DEBI done
+
+#define RPS_SIG0		0x00200000	// RPS semaphore 0 (used by ADC).
+#define RPS_SIG1		0x00400000	// RPS semaphore 1 (used by DAC).
+#define RPS_SIG2		0x00800000	// RPS semaphore 2 (not used).
+#define RPS_GPIO2		0x00080000	// RPS GPIO2
+#define RPS_GPIO3		0x00100000	// RPS GPIO3
+
+#define RPS_SIGADC		RPS_SIG0	// Trigger/status for ADC's RPS program.
+#define RPS_SIGDAC		RPS_SIG1	// Trigger/status for DAC's RPS program.
+
+// RPS clock parameters.
+#define RPSCLK_SCALAR		8	// This is apparent ratio of PCI/RPS clks (undocumented!!).
+#define RPSCLK_PER_US		( 33 / RPSCLK_SCALAR )	// Number of RPS clocks in one microsecond.
+
+// Event counter source addresses.
+#define SBA_RPS_A0		0x27	// Time of RPS0 busy, in PCI clocks.
+
+// GPIO constants.
+#define GPIO_BASE		0x10004000	// GPIO 0,2,3 = inputs, GPIO3 = IRQ; GPIO1 = out.
+#define GPIO1_LO		0x00000000	// GPIO1 set to LOW.
+#define GPIO1_HI		0x00001000	// GPIO1 set to HIGH.
+
+// Primary Status Register (PSR) constants.
+#define PSR_DEBI_E		0x00040000	// DEBI event flag.
+#define PSR_DEBI_S		0x00080000	// DEBI status flag.
+#define PSR_A2_IN		0x00008000	// Audio output DMA2 protection address reached.
+#define PSR_AFOU		0x00000800	// Audio FIFO under/overflow detected.
+#define PSR_GPIO2		0x00000020	// GPIO2 input pin: 0=AdcBusy, 1=AdcIdle.
+#define PSR_EC0S		0x00000001	// Event counter 0 threshold reached.
+
+// Secondary Status Register (SSR) constants.
+#define SSR_AF2_OUT		0x00000200	// Audio 2 output FIFO under/overflow detected.
+
+// Master Control Register 1 (MC1) constants.
+#define MC1_SOFT_RESET		0x80000000	// Invoke 7146 soft reset.
+#define MC1_SHUTDOWN		0x3FFF0000	// Shut down all MC1-controlled enables.
+
+#define MC1_ERPS1		0x2000	// enab/disable RPS task 1.
+#define MC1_ERPS0		0x1000	// enab/disable RPS task 0.
+#define MC1_DEBI		0x0800	// enab/disable DEBI pins.
+#define MC1_AUDIO		0x0200	// enab/disable audio port pins.
+#define MC1_I2C			0x0100	// enab/disable I2C interface.
+#define MC1_A2OUT		0x0008	// enab/disable transfer on A2 out.
+#define MC1_A2IN		0x0004	// enab/disable transfer on A2 in.
+#define MC1_A1IN		0x0001	// enab/disable transfer on A1 in.
+
+// Master Control Register 2 (MC2) constants.
+#define MC2_UPLD_DEBIq		0x00020002	// Upload DEBI registers.
+#define MC2_UPLD_IICq		0x00010001	// Upload I2C registers.
+#define MC2_RPSSIG2_ONq		0x20002000	// Assert RPS_SIG2.
+#define MC2_RPSSIG1_ONq		0x10001000	// Assert RPS_SIG1.
+#define MC2_RPSSIG0_ONq		0x08000800	// Assert RPS_SIG0.
+#define MC2_UPLD_DEBI_MASKq	0x00000002	// Upload DEBI mask.
+#define MC2_UPLD_IIC_MASKq	0x00000001	// Upload I2C mask.
+#define MC2_RPSSIG2_MASKq	0x00002000	// RPS_SIG2 bit mask.
+#define MC2_RPSSIG1_MASKq	0x00001000	// RPS_SIG1 bit mask.
+#define MC2_RPSSIG0_MASKq	0x00000800	// RPS_SIG0 bit mask.
+
+#define MC2_DELAYTRIG_4USq	MC2_RPSSIG1_ON
+#define MC2_DELAYBUSY_4USq	MC2_RPSSIG1_MASK
+
+#define	MC2_DELAYTRIG_6USq	MC2_RPSSIG2_ON
+#define MC2_DELAYBUSY_6USq	MC2_RPSSIG2_MASK
+
+#define MC2_UPLD_DEBI		0x0002	// Upload DEBI.
+#define MC2_UPLD_IIC		0x0001	// Upload I2C.
+#define MC2_RPSSIG2		0x2000	// RPS signal 2 (not used).
+#define MC2_RPSSIG1		0x1000	// RPS signal 1 (DAC RPS busy).
+#define MC2_RPSSIG0		0x0800	// RPS signal 0 (ADC RPS busy).
+
+#define MC2_ADC_RPS		MC2_RPSSIG0	// ADC RPS busy.
+#define MC2_DAC_RPS		MC2_RPSSIG1	// DAC RPS busy.
+
+///////////////////oldies///////////
+#define MC2_UPLD_DEBIQ		0x00020002	// Upload DEBI registers.
+#define MC2_UPLD_IICQ		0x00010001	// Upload I2C registers.
+////////////////////////////////////////
+
+// PCI BUS (SAA7146) REGISTER ADDRESS OFFSETS ////////////////////////
+#define P_PCI_BT_A		0x004C	// Audio DMA
+						// burst/threshold
+						// control.
+#define P_DEBICFG               0x007C	// DEBI configuration.
+#define P_DEBICMD               0x0080	// DEBI command.
+#define P_DEBIPAGE              0x0084	// DEBI page.
+#define P_DEBIAD                0x0088	// DEBI target address.
+#define P_I2CCTRL               0x008C	// I2C control.
+#define P_I2CSTAT               0x0090	// I2C status.
+#define P_BASEA2_IN		0x00AC	// Audio input 2 base
+						// physical DMAbuf
+						// address.
+#define P_PROTA2_IN		0x00B0	// Audio input 2
+						// physical DMAbuf
+						// protection address.
+#define P_PAGEA2_IN		0x00B4	// Audio input 2
+						// paging attributes.
+#define P_BASEA2_OUT		0x00B8	// Audio output 2 base
+						// physical DMAbuf
+						// address.
+#define P_PROTA2_OUT		0x00BC	// Audio output 2
+						// physical DMAbuf
+						// protection address.
+#define P_PAGEA2_OUT		0x00C0	// Audio output 2
+						// paging attributes.
+#define P_RPSPAGE0              0x00C4	// RPS0 page.
+#define P_RPSPAGE1              0x00C8	// RPS1 page.
+#define P_RPS0_TOUT		0x00D4	// RPS0 time-out.
+#define P_RPS1_TOUT		0x00D8	// RPS1 time-out.
+#define P_IER                   0x00DC	// Interrupt enable.
+#define P_GPIO                  0x00E0	// General-purpose I/O.
+#define P_EC1SSR		0x00E4	// Event counter set 1
+						// source select.
+#define P_ECT1R			0x00EC	// Event counter
+						// threshold set 1.
+#define P_ACON1                 0x00F4	// Audio control 1.
+#define P_ACON2                 0x00F8	// Audio control 2.
+#define P_MC1                   0x00FC	// Master control 1.
+#define P_MC2                   0x0100	// Master control 2.
+#define P_RPSADDR0              0x0104	// RPS0 instruction pointer.
+#define P_RPSADDR1              0x0108	// RPS1 instruction pointer.
+#define P_ISR                   0x010C	// Interrupt status.
+#define P_PSR                   0x0110	// Primary status.
+#define P_SSR                   0x0114	// Secondary status.
+#define P_EC1R			0x0118	// Event counter set 1.
+#define P_ADP4			0x0138	// Logical audio DMA
+						// pointer of audio
+						// input FIFO A2_IN.
+#define P_FB_BUFFER1            0x0144	// Audio feedback buffer 1.
+#define P_FB_BUFFER2            0x0148	// Audio feedback buffer 2.
+#define P_TSL1                  0x0180	// Audio time slot list 1.
+#define P_TSL2                  0x01C0	// Audio time slot list 2.
+
+// LOCAL BUS (GATE ARRAY) REGISTER ADDRESS OFFSETS /////////////////
+// Analog I/O registers:
+#define LP_DACPOL		0x0082	//  Write DAC polarity.
+#define LP_GSEL			0x0084	//  Write ADC gain.
+#define LP_ISEL			0x0086	//  Write ADC channel select.
+// Digital I/O (write only):
+#define LP_WRINTSELA		0x0042	//  Write A interrupt enable.
+#define LP_WREDGSELA		0x0044	//  Write A edge selection.
+#define LP_WRCAPSELA		0x0046	//  Write A capture enable.
+#define LP_WRDOUTA		0x0048	//  Write A digital output.
+#define LP_WRINTSELB		0x0052	//  Write B interrupt enable.
+#define LP_WREDGSELB		0x0054	//  Write B edge selection.
+#define LP_WRCAPSELB		0x0056	//  Write B capture enable.
+#define LP_WRDOUTB		0x0058	//  Write B digital output.
+#define LP_WRINTSELC		0x0062	//  Write C interrupt enable.
+#define LP_WREDGSELC		0x0064	//  Write C edge selection.
+#define LP_WRCAPSELC		0x0066	//  Write C capture enable.
+#define LP_WRDOUTC		0x0068	//  Write C digital output.
+
+// Digital I/O (read only):
+#define LP_RDDINA		0x0040	//  Read digital input.
+#define LP_RDCAPFLGA		0x0048	//  Read edges captured.
+#define LP_RDINTSELA		0x004A	//  Read interrupt
+						//  enable register.
+#define LP_RDEDGSELA		0x004C	//  Read edge
+						//  selection
+						//  register.
+#define LP_RDCAPSELA		0x004E	//  Read capture
+						//  enable register.
+#define LP_RDDINB		0x0050	//  Read digital input.
+#define LP_RDCAPFLGB		0x0058	//  Read edges captured.
+#define LP_RDINTSELB		0x005A	//  Read interrupt
+						//  enable register.
+#define LP_RDEDGSELB		0x005C	//  Read edge
+						//  selection
+						//  register.
+#define LP_RDCAPSELB		0x005E	//  Read capture
+						//  enable register.
+#define LP_RDDINC		0x0060	//  Read digital input.
+#define LP_RDCAPFLGC		0x0068	//  Read edges captured.
+#define LP_RDINTSELC		0x006A	//  Read interrupt
+						//  enable register.
+#define LP_RDEDGSELC		0x006C	//  Read edge
+						//  selection
+						//  register.
+#define LP_RDCAPSELC		0x006E	//  Read capture
+						//  enable register.
+// Counter Registers (read/write):
+#define LP_CR0A			0x0000	//  0A setup register.
+#define LP_CR0B			0x0002	//  0B setup register.
+#define LP_CR1A			0x0004	//  1A setup register.
+#define LP_CR1B			0x0006	//  1B setup register.
+#define LP_CR2A			0x0008	//  2A setup register.
+#define LP_CR2B			0x000A	//  2B setup register.
+// Counter PreLoad (write) and Latch (read) Registers:
+#define	LP_CNTR0ALSW		0x000C	//  0A lsw.
+#define	LP_CNTR0AMSW		0x000E	//  0A msw.
+#define	LP_CNTR0BLSW		0x0010	//  0B lsw.
+#define	LP_CNTR0BMSW		0x0012	//  0B msw.
+#define	LP_CNTR1ALSW		0x0014	//  1A lsw.
+#define	LP_CNTR1AMSW		0x0016	//  1A msw.
+#define	LP_CNTR1BLSW		0x0018	//  1B lsw.
+#define	LP_CNTR1BMSW		0x001A	//  1B msw.
+#define	LP_CNTR2ALSW		0x001C	//  2A lsw.
+#define	LP_CNTR2AMSW		0x001E	//  2A msw.
+#define	LP_CNTR2BLSW		0x0020	//  2B lsw.
+#define	LP_CNTR2BMSW		0x0022	//  2B msw.
+// Miscellaneous Registers (read/write):
+#define LP_MISC1		0x0088	//  Read/write Misc1.
+#define LP_WRMISC2		0x0090	//  Write Misc2.
+#define LP_RDMISC2		0x0082	//  Read Misc2.
+
+// Bit masks for MISC1 register that are the same for reads and writes.
+#define MISC1_WENABLE		0x8000	// enab writes to
+						// MISC2 (except Clear
+						// Watchdog bit).
+#define MISC1_WDISABLE		0x0000	// Disable writes to MISC2.
+#define MISC1_EDCAP		0x1000	// enab edge capture
+						// on DIO chans
+						// specified by
+						// LP_WRCAPSELx.
+#define MISC1_NOEDCAP		0x0000	// Disable edge
+						// capture on
+						// specified DIO
+						// chans.
+
+// Bit masks for MISC1 register reads.
+#define RDMISC1_WDTIMEOUT	0x4000	// Watchdog timer timed out.
+
+// Bit masks for MISC2 register writes.
+#define WRMISC2_WDCLEAR		0x8000	// Reset watchdog
+						// timer to zero.
+#define WRMISC2_CHARGE_ENABLE	0x4000	// enab battery
+						// trickle charging.
+
+// Bit masks for MISC2 register that are the same for reads and writes.
+#define MISC2_BATT_ENABLE	0x0008	// Backup battery enable.
+#define MISC2_WDENABLE		0x0004	// Watchdog timer enable.
+#define MISC2_WDPERIOD_MASK	0x0003	// Watchdog interval
+						// select mask.
+
+// Bit masks for ACON1 register.
+#define A2_RUN			0x40000000	// Run A2 based on TSL2.
+#define A1_RUN			0x20000000	// Run A1 based on TSL1.
+#define A1_SWAP			0x00200000	// Use big-endian for A1.
+#define A2_SWAP			0x00100000	// Use big-endian for A2.
+#define WS_MODES		0x00019999	// WS0 = TSL1 trigger
+						// input, WS1-WS4 =
+						// CS* outputs.
+
+#if PLATFORM == INTEL		// Base ACON1 config:
+						// always run A1 based
+						// on TSL1.
+#define ACON1_BASE		( WS_MODES | A1_RUN )
+#elif PLATFORM == MOTOROLA
+#define ACON1_BASE		( WS_MODES | A1_RUN | A1_SWAP | A2_SWAP )
+#endif
+
+#define ACON1_ADCSTART		ACON1_BASE	// Start ADC: run A1
+						// based on TSL1.
+#define ACON1_DACSTART		( ACON1_BASE | A2_RUN )	// Start
+							// transmit to
+							// DAC: run A2
+							// based on
+							// TSL2.
+#define ACON1_DACSTOP		ACON1_BASE	// Halt A2.
+
+// Bit masks for ACON2 register.
+#define A1_CLKSRC_BCLK1		0x00000000	// A1 bit rate = BCLK1 (ADC).
+#define A2_CLKSRC_X1		0x00800000	// A2 bit rate = ACLK/1 (DACs).
+#define A2_CLKSRC_X2		0x00C00000	// A2 bit rate = ACLK/2 (DACs).
+#define A2_CLKSRC_X4		0x01400000	// A2 bit rate = ACLK/4 (DACs).
+#define INVERT_BCLK2		0x00100000	// Invert BCLK2 (DACs).
+#define BCLK2_OE		0x00040000	// enab BCLK2 (DACs).
+#define ACON2_XORMASK		0x000C0000	// XOR mask for ACON2
+						// active-low bits.
+
+#define ACON2_INIT		( ACON2_XORMASK ^ ( A1_CLKSRC_BCLK1 | A2_CLKSRC_X2 | INVERT_BCLK2 | BCLK2_OE ) )
+
+// Bit masks for timeslot records.
+#define WS1		     	0x40000000	// WS output to assert.
+#define WS2		     	0x20000000
+#define WS3		     	0x10000000
+#define WS4		     	0x08000000
+#define RSD1			0x01000000	// Shift A1 data in on SD1.
+#define SDW_A1			0x00800000	// Store rcv'd char at
+						// next char slot of
+						// DWORD1 buffer.
+#define SIB_A1			0x00400000	// Store rcv'd char at
+						// next char slot of
+						// FB1 buffer.
+#define SF_A1			0x00200000	// Write unsigned long
+						// buffer to input
+						// FIFO.
+
+//Select parallel-to-serial converter's data source:
+#define XFIFO_0			0x00000000	//   Data fifo byte 0.
+#define XFIFO_1			0x00000010	//   Data fifo byte 1.
+#define XFIFO_2			0x00000020	//   Data fifo byte 2.
+#define XFIFO_3			0x00000030	//   Data fifo byte 3.
+#define XFB0			0x00000040	//   FB_BUFFER byte 0.
+#define XFB1			0x00000050	//   FB_BUFFER byte 1.
+#define XFB2			0x00000060	//   FB_BUFFER byte 2.
+#define XFB3			0x00000070	//   FB_BUFFER byte 3.
+#define SIB_A2			0x00000200	// Store next dword
+						// from A2's input
+						// shifter to FB2
+						// buffer.
+#define SF_A2			0x00000100	// Store next dword
+						// from A2's input
+						// shifter to its
+						// input fifo.
+#define LF_A2			0x00000080	// Load next dword
+						// from A2's output
+						// fifo into its
+						// output dword
+						// buffer.
+#define XSD2			0x00000008	// Shift data out on SD2.
+#define RSD3			0x00001800	// Shift data in on SD3.
+#define RSD2			0x00001000	// Shift data in on SD2.
+#define LOW_A2			0x00000002	// Drive last SD low
+						// for 7 clks, then
+						// tri-state.
+#define EOS		     	0x00000001	// End of superframe.
+
+//////////////////////
+
+// I2C configuration constants.
+#define I2C_CLKSEL		0x0400	// I2C bit rate =
+						// PCIclk/480 = 68.75
+						// KHz.
+#define I2C_BITRATE		68.75	// I2C bus data bit
+						// rate (determined by
+						// I2C_CLKSEL) in KHz.
+#define I2C_WRTIME		15.0	// Worst case time,in
+						// msec, for EEPROM
+						// internal write op.
+
+// I2C manifest constants.
+
+// Max retries to wait for EEPROM write.
+#define I2C_RETRIES		( I2C_WRTIME * I2C_BITRATE / 9.0 )
+#define I2C_ERR			0x0002	// I2C control/status
+						// flag ERROR.
+#define I2C_BUSY		0x0001	// I2C control/status
+						// flag BUSY.
+#define I2C_ABORT		0x0080	// I2C status flag ABORT.
+#define I2C_ATTRSTART		0x3	// I2C attribute START.
+#define I2C_ATTRCONT		0x2	// I2C attribute CONT.
+#define I2C_ATTRSTOP		0x1	// I2C attribute STOP.
+#define I2C_ATTRNOP		0x0	// I2C attribute NOP.
+
+// I2C read command  | EEPROM address.
+#define I2CR			( devpriv->I2CAdrs | 1 )
+
+// I2C write command | EEPROM address.
+#define I2CW			( devpriv->I2CAdrs )
+
+// Code macros used for constructing I2C command bytes.
+#define I2C_B2(ATTR,VAL)	( ( (ATTR) << 6 ) | ( (VAL) << 24 ) )
+#define I2C_B1(ATTR,VAL)	( ( (ATTR) << 4 ) | ( (VAL) << 16 ) )
+#define I2C_B0(ATTR,VAL)	( ( (ATTR) << 2 ) | ( (VAL) <<  8 ) )
+
+////////////////////////////////////////////////////////
+//oldest
+#define P_DEBICFGq              0x007C	// DEBI configuration.
+#define P_DEBICMDq              0x0080	// DEBI command.
+#define P_DEBIPAGEq             0x0084	// DEBI page.
+#define P_DEBIADq               0x0088	// DEBI target address.
+
+#define DEBI_CFG_TOQ		0x03C00000	// timeout (15 PCI cycles)
+#define DEBI_CFG_FASTQ		0x10000000	// fast mode enable
+#define DEBI_CFG_16Q		0x00080000	// 16-bit access enable
+#define DEBI_CFG_INCQ		0x00040000	// enable address increment
+#define DEBI_CFG_TIMEROFFQ	0x00010000	// disable timer
+#define DEBI_CMD_RDQ		0x00050000	// read immediate 2 bytes
+#define DEBI_CMD_WRQ		0x00040000	// write immediate 2 bytes
+#define DEBI_PAGE_DISABLEQ	0x00000000	// paging disable
+
+///////////////////////////////////////////
+// DEBI command constants.
+#define DEBI_CMD_SIZE16		( 2 << 17 )	// Transfer size is
+						// always 2 bytes.
+#define DEBI_CMD_READ		0x00010000	// Read operation.
+#define DEBI_CMD_WRITE		0x00000000	// Write operation.
+
+// Read immediate 2 bytes.
+#define DEBI_CMD_RDWORD		( DEBI_CMD_READ  | DEBI_CMD_SIZE16 )
+
+// Write immediate 2 bytes.
+#define DEBI_CMD_WRWORD		( DEBI_CMD_WRITE | DEBI_CMD_SIZE16 )
+
+// DEBI configuration constants.
+#define DEBI_CFG_XIRQ_EN	0x80000000	// enab external
+						// interrupt on GPIO3.
+#define DEBI_CFG_XRESUME	0x40000000	// Resume block
+						// transfer when XIRQ
+						// deasserted.
+#define DEBI_CFG_FAST		0x10000000	// Fast mode enable.
+
+// 4-bit field that specifies DEBI timeout value in PCI clock cycles:
+#define DEBI_CFG_TOUT_BIT	22	//   Finish DEBI cycle after
+					//   this many clocks.
+
+// 2-bit field that specifies Endian byte lane steering:
+#define DEBI_CFG_SWAP_NONE	0x00000000	//   Straight - don't
+						//   swap any bytes
+						//   (Intel).
+#define DEBI_CFG_SWAP_2		0x00100000	//   2-byte swap (Motorola).
+#define DEBI_CFG_SWAP_4		0x00200000	//   4-byte swap.
+#define DEBI_CFG_16		0x00080000	// Slave is able to
+						// serve 16-bit
+						// cycles.
+
+#define DEBI_CFG_SLAVE16	0x00080000	// Slave is able to
+						// serve 16-bit
+						// cycles.
+#define DEBI_CFG_INC		0x00040000	// enab address
+						// increment for block
+						// transfers.
+#define DEBI_CFG_INTEL		0x00020000	// Intel style local bus.
+#define DEBI_CFG_TIMEROFF	0x00010000	// Disable timer.
+
+#if PLATFORM == INTEL
+
+#define DEBI_TOUT		7	// Wait 7 PCI clocks
+						// (212 ns) before
+						// polling RDY.
+
+// Intel byte lane steering (pass through all byte lanes).
+#define DEBI_SWAP		DEBI_CFG_SWAP_NONE
+
+#elif PLATFORM == MOTOROLA
+
+#define DEBI_TOUT		15	// Wait 15 PCI clocks (454 ns)
+					// maximum before timing out.
+#define DEBI_SWAP		DEBI_CFG_SWAP_2	// Motorola byte lane steering.
+
+#endif
+
+// DEBI page table constants.
+#define DEBI_PAGE_DISABLE	0x00000000	// Paging disable.
+
+///////////////////EXTRA FROM OTHER SANSORAY  * .h////////
+
+// LoadSrc values:
+#define LOADSRC_INDX		0	// Preload core in response to
+					// Index.
+#define LOADSRC_OVER		1	// Preload core in response to
+					// Overflow.
+#define LOADSRCB_OVERA		2	// Preload B core in response
+					// to A Overflow.
+#define LOADSRC_NONE		3	// Never preload core.
+
+// IntSrc values:
+#define INTSRC_NONE 		0	// Interrupts disabled.
+#define INTSRC_OVER 		1	// Interrupt on Overflow.
+#define INTSRC_INDX 		2	// Interrupt on Index.
+#define INTSRC_BOTH 		3	// Interrupt on Index or Overflow.
+
+// LatchSrc values:
+#define LATCHSRC_AB_READ	0	// Latch on read.
+#define LATCHSRC_A_INDXA	1	// Latch A on A Index.
+#define LATCHSRC_B_INDXB	2	// Latch B on B Index.
+#define LATCHSRC_B_OVERA	3	// Latch B on A Overflow.
+
+// IndxSrc values:
+#define INDXSRC_HARD		0	// Hardware or software index.
+#define INDXSRC_SOFT		1	// Software index only.
+
+// IndxPol values:
+#define INDXPOL_POS 		0	// Index input is active high.
+#define INDXPOL_NEG 		1	// Index input is active low.
+
+// ClkSrc values:
+#define CLKSRC_COUNTER		0	// Counter mode.
+#define CLKSRC_TIMER		2	// Timer mode.
+#define CLKSRC_EXTENDER		3	// Extender mode.
+
+// ClkPol values:
+#define CLKPOL_POS		0	// Counter/Extender clock is
+					// active high.
+#define CLKPOL_NEG		1	// Counter/Extender clock is
+					// active low.
+#define CNTDIR_UP		0	// Timer counts up.
+#define CNTDIR_DOWN 		1	// Timer counts down.
+
+// ClkEnab values:
+#define CLKENAB_ALWAYS		0	// Clock always enabled.
+#define CLKENAB_INDEX		1	// Clock is enabled by index.
+
+// ClkMult values:
+#define CLKMULT_4X 		0	// 4x clock multiplier.
+#define CLKMULT_2X 		1	// 2x clock multiplier.
+#define CLKMULT_1X 		2	// 1x clock multiplier.
+
+// Bit Field positions in COUNTER_SETUP structure:
+#define BF_LOADSRC		9	// Preload trigger.
+#define BF_INDXSRC		7	// Index source.
+#define BF_INDXPOL		6	// Index polarity.
+#define BF_CLKSRC		4	// Clock source.
+#define BF_CLKPOL		3	// Clock polarity/count direction.
+#define BF_CLKMULT		1	// Clock multiplier.
+#define BF_CLKENAB		0	// Clock enable.
+
+// Enumerated counter operating modes specified by ClkSrc bit field in
+// a COUNTER_SETUP.
+
+#define CLKSRC_COUNTER		0	// Counter: ENC_C clock, ENC_D
+					// direction.
+#define CLKSRC_TIMER		2	// Timer: SYS_C clock,
+					// direction specified by
+					// ClkPol.
+#define CLKSRC_EXTENDER		3	// Extender: OVR_A clock,
+					// ENC_D direction.
+
+// Enumerated counter clock multipliers.
+
+#define MULT_X0			0x0003	// Supports no multipliers;
+					// fixed physical multiplier =
+					// 3.
+#define MULT_X1			0x0002	// Supports multiplier x1;
+					// fixed physical multiplier =
+					// 2.
+#define MULT_X2			0x0001	// Supports multipliers x1,
+					// x2; physical multipliers =
+					// 1 or 2.
+#define MULT_X4			0x0000	// Supports multipliers x1,
+					// x2, x4; physical
+					// multipliers = 0, 1 or 2.
+
+// Sanity-check limits for parameters.
+
+#define NUM_COUNTERS		6	// Maximum valid counter
+					// logical channel number.
+#define NUM_INTSOURCES		4
+#define NUM_LATCHSOURCES	4
+#define NUM_CLKMULTS		4
+#define NUM_CLKSOURCES		4
+#define NUM_CLKPOLS		2
+#define NUM_INDEXPOLS		2
+#define NUM_INDEXSOURCES	2
+#define NUM_LOADTRIGS		4
+
+// Bit field positions in CRA and CRB counter control registers.
+
+// Bit field positions in CRA:
+#define CRABIT_INDXSRC_B	14	//   B index source.
+#define CRABIT_CLKSRC_B		12	//   B clock source.
+#define CRABIT_INDXPOL_A	11	//   A index polarity.
+#define CRABIT_LOADSRC_A	 9	//   A preload trigger.
+#define CRABIT_CLKMULT_A	 7	//   A clock multiplier.
+#define CRABIT_INTSRC_A		 5	//   A interrupt source.
+#define CRABIT_CLKPOL_A		 4	//   A clock polarity.
+#define CRABIT_INDXSRC_A	 2	//   A index source.
+#define CRABIT_CLKSRC_A		 0	//   A clock source.
+
+// Bit field positions in CRB:
+#define CRBBIT_INTRESETCMD	15	//   Interrupt reset command.
+#define CRBBIT_INTRESET_B	14	//   B interrupt reset enable.
+#define CRBBIT_INTRESET_A	13	//   A interrupt reset enable.
+#define CRBBIT_CLKENAB_A	12	//   A clock enable.
+#define CRBBIT_INTSRC_B		10	//   B interrupt source.
+#define CRBBIT_LATCHSRC		 8	//   A/B latch source.
+#define CRBBIT_LOADSRC_B	 6	//   B preload trigger.
+#define CRBBIT_CLKMULT_B	 3	//   B clock multiplier.
+#define CRBBIT_CLKENAB_B	 2	//   B clock enable.
+#define CRBBIT_INDXPOL_B	 1	//   B index polarity.
+#define CRBBIT_CLKPOL_B		 0	//   B clock polarity.
+
+// Bit field masks for CRA and CRB.
+
+#define CRAMSK_INDXSRC_B	( (uint16_t)( 3 << CRABIT_INDXSRC_B) )
+#define CRAMSK_CLKSRC_B		( (uint16_t)( 3 << CRABIT_CLKSRC_B) )
+#define CRAMSK_INDXPOL_A	( (uint16_t)( 1 << CRABIT_INDXPOL_A) )
+#define CRAMSK_LOADSRC_A	( (uint16_t)( 3 << CRABIT_LOADSRC_A) )
+#define CRAMSK_CLKMULT_A	( (uint16_t)( 3 << CRABIT_CLKMULT_A) )
+#define CRAMSK_INTSRC_A		( (uint16_t)( 3 << CRABIT_INTSRC_A) )
+#define CRAMSK_CLKPOL_A		( (uint16_t)( 3 << CRABIT_CLKPOL_A) )
+#define CRAMSK_INDXSRC_A	( (uint16_t)( 3 << CRABIT_INDXSRC_A) )
+#define CRAMSK_CLKSRC_A		( (uint16_t)( 3 << CRABIT_CLKSRC_A) )
+
+#define CRBMSK_INTRESETCMD	( (uint16_t)( 1 << CRBBIT_INTRESETCMD) )
+#define CRBMSK_INTRESET_B	( (uint16_t)( 1 << CRBBIT_INTRESET_B) )
+#define CRBMSK_INTRESET_A	( (uint16_t)( 1 << CRBBIT_INTRESET_A) )
+#define CRBMSK_CLKENAB_A	( (uint16_t)( 1 << CRBBIT_CLKENAB_A) )
+#define CRBMSK_INTSRC_B		( (uint16_t)( 3 << CRBBIT_INTSRC_B) )
+#define CRBMSK_LATCHSRC		( (uint16_t)( 3 << CRBBIT_LATCHSRC) )
+#define CRBMSK_LOADSRC_B	( (uint16_t)( 3 << CRBBIT_LOADSRC_B) )
+#define CRBMSK_CLKMULT_B	( (uint16_t)( 3 << CRBBIT_CLKMULT_B) )
+#define CRBMSK_CLKENAB_B	( (uint16_t)( 1 << CRBBIT_CLKENAB_B) )
+#define CRBMSK_INDXPOL_B	( (uint16_t)( 1 << CRBBIT_INDXPOL_B) )
+#define CRBMSK_CLKPOL_B		( (uint16_t)( 1 << CRBBIT_CLKPOL_B) )
+
+#define CRBMSK_INTCTRL		( CRBMSK_INTRESETCMD | CRBMSK_INTRESET_A | CRBMSK_INTRESET_B )	// Interrupt reset control bits.
+
+// Bit field positions for standardized SETUP structure.
+
+#define STDBIT_INTSRC		13
+#define STDBIT_LATCHSRC		11
+#define STDBIT_LOADSRC		 9
+#define STDBIT_INDXSRC		 7
+#define STDBIT_INDXPOL		 6
+#define STDBIT_CLKSRC		 4
+#define STDBIT_CLKPOL		 3
+#define STDBIT_CLKMULT		 1
+#define STDBIT_CLKENAB		 0
+
+// Bit field masks for standardized SETUP structure.
+
+#define STDMSK_INTSRC		( (uint16_t)( 3 << STDBIT_INTSRC   ) )
+#define STDMSK_LATCHSRC		( (uint16_t)( 3 << STDBIT_LATCHSRC ) )
+#define STDMSK_LOADSRC		( (uint16_t)( 3 << STDBIT_LOADSRC  ) )
+#define STDMSK_INDXSRC		( (uint16_t)( 1 << STDBIT_INDXSRC  ) )
+#define STDMSK_INDXPOL		( (uint16_t)( 1 << STDBIT_INDXPOL  ) )
+#define STDMSK_CLKSRC		( (uint16_t)( 3 << STDBIT_CLKSRC   ) )
+#define STDMSK_CLKPOL		( (uint16_t)( 1 << STDBIT_CLKPOL   ) )
+#define STDMSK_CLKMULT		( (uint16_t)( 3 << STDBIT_CLKMULT  ) )
+#define STDMSK_CLKENAB		( (uint16_t)( 1 << STDBIT_CLKENAB  ) )
+
+//////////////////////////////////////////////////////////
+
+/* typedef struct indexCounter */
+/* { */
+/*   unsigned int ao; */
+/*   unsigned int ai; */
+/*   unsigned int digout; */
+/*   unsigned int digin; */
+/*   unsigned int enc; */
+/* }CallCounter; */
+
+typedef struct bufferDMA {
+	dma_addr_t PhysicalBase;
+	void *LogicalBase;
+	uint32_t DMAHandle;
+} DMABUF;
diff --git a/drivers/staging/comedi/drivers/usbdux.c b/drivers/staging/comedi/drivers/usbdux.c
new file mode 100644
index 00000000000..35138257be7
--- /dev/null
+++ b/drivers/staging/comedi/drivers/usbdux.c
@@ -0,0 +1,2932 @@
+#define DRIVER_VERSION "v2.1"
+#define DRIVER_AUTHOR "Bernd Porr, BerndPorr@f2s.com"
+#define DRIVER_DESC "Stirling/ITL USB-DUX -- Bernd.Porr@f2s.com"
+/*
+   comedi/drivers/usbdux.c
+   Copyright (C) 2003-2007 Bernd Porr, Bernd.Porr@f2s.com
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation; either version 2 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program; if not, write to the Free Software
+   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+ */
+/*
+Driver: usbdux
+Description: University of Stirling USB DAQ & INCITE Technology Limited
+Devices: [ITL] USB-DUX (usbdux.o)
+Author: Bernd Porr <BerndPorr@f2s.com>
+Updated: 25 Nov 2007
+Status: Testing
+Configuration options:
+  You have to upload firmware with the -i option. The
+  firmware is usually installed under /usr/share/usb or
+  /usr/local/share/usb or /lib/firmware.
+
+Connection scheme for the counter at the digital port:
+  0=/CLK0, 1=UP/DOWN0, 2=RESET0, 4=/CLK1, 5=UP/DOWN1, 6=RESET1.
+  The sampling rate of the counter is approximately 500Hz.
+
+Please note that under USB2.0 the length of the channel list determines
+the max sampling rate. If you sample only one channel you get 8kHz
+sampling rate. If you sample two channels you get 4kHz and so on.
+*/
+/*
+ * I must give credit here to Chris Baugher who
+ * wrote the driver for AT-MIO-16d. I used some parts of this
+ * driver. I also must give credits to David Brownell
+ * who supported me with the USB development.
+ *
+ * Bernd Porr
+ *
+ *
+ * Revision history:
+ * 0.94: D/A output should work now with any channel list combinations
+ * 0.95: .owner commented out for kernel vers below 2.4.19
+ *       sanity checks in ai/ao_cmd
+ * 0.96: trying to get it working with 2.6, moved all memory alloc to comedi's
+ *       attach final USB IDs
+ *       moved memory allocation completely to the corresponding comedi
+ *       functions firmware upload is by fxload and no longer by comedi (due to
+ *       enumeration)
+ * 0.97: USB IDs received, adjusted table
+ * 0.98: SMP, locking, memroy alloc: moved all usb memory alloc
+ *       to the usb subsystem and moved all comedi related memory
+ *       alloc to comedi.
+ *       | kernel | registration | usbdux-usb | usbdux-comedi | comedi |
+ * 0.99: USB 2.0: changed protocol to isochronous transfer
+ *                IRQ transfer is too buggy and too risky in 2.0
+ *                for the high speed ISO transfer is now a working version
+ *                available
+ * 0.99b: Increased the iso transfer buffer for high sp.to 10 buffers. Some VIA
+ *        chipsets miss out IRQs. Deeper buffering is needed.
+ * 1.00: full USB 2.0 support for the A/D converter. Now: max 8kHz sampling
+ *       rate.
+ *       Firmware vers 1.00 is needed for this.
+ *       Two 16 bit up/down/reset counter with a sampling rate of 1kHz
+ *       And loads of cleaning up, in particular streamlining the
+ *       bulk transfers.
+ * 1.1:  moved EP4 transfers to EP1 to make space for a PWM output on EP4
+ * 1.2:  added PWM suport via EP4
+ * 2.0:  PWM seems to be stable and is not interfering with the other functions
+ * 2.1:  changed PWM API
+ *
+ */
+
+/* generates loads of debug info */
+/* #define NOISY_DUX_DEBUGBUG */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/input.h>
+#include <linux/usb.h>
+#include <linux/smp_lock.h>
+#include <linux/fcntl.h>
+#include <linux/compiler.h>
+
+#include "../comedidev.h"
+
+#define BOARDNAME "usbdux"
+
+/* timeout for the USB-transfer */
+#define EZTIMEOUT 30
+
+/* constants for "firmware" upload and download */
+#define USBDUXSUB_FIRMWARE 0xA0
+#define VENDOR_DIR_IN  0xC0
+#define VENDOR_DIR_OUT 0x40
+
+/* internal adresses of the 8051 processor */
+#define USBDUXSUB_CPUCS 0xE600
+
+/*
+ * the minor device number, major is 180 only for debugging purposes and to
+ * upload special firmware (programming the eeprom etc) which is not compatible
+ * with the comedi framwork
+ */
+#define USBDUXSUB_MINOR 32
+
+/* max lenghth of the transfer-buffer for software upload */
+#define TB_LEN 0x2000
+
+/* Input endpoint number: ISO/IRQ */
+#define ISOINEP           6
+
+/* Output endpoint number: ISO/IRQ */
+#define ISOOUTEP          2
+
+/* This EP sends DUX commands to USBDUX */
+#define COMMAND_OUT_EP     1
+
+/* This EP receives the DUX commands from USBDUX */
+#define COMMAND_IN_EP        8
+
+/* Output endpoint for PWM */
+#define PWM_EP         4
+
+/* 300Hz max frequ under PWM */
+#define MIN_PWM_PERIOD  ((long)(1E9/300))
+
+/* Default PWM frequency */
+#define PWM_DEFAULT_PERIOD ((long)(1E9/100))
+
+/* Number of channels */
+#define NUMCHANNELS       8
+
+/* Size of one A/D value */
+#define SIZEADIN          ((sizeof(int16_t)))
+
+/*
+ * Size of the input-buffer IN BYTES
+ * Always multiple of 8 for 8 microframes which is needed in the highspeed mode
+ */
+#define SIZEINBUF         ((8*SIZEADIN))
+
+/* 16 bytes. */
+#define SIZEINSNBUF       16
+
+/* Number of DA channels */
+#define NUMOUTCHANNELS    8
+
+/* size of one value for the D/A converter: channel and value */
+#define SIZEDAOUT          ((sizeof(int8_t)+sizeof(int16_t)))
+
+/*
+ * Size of the output-buffer in bytes
+ * Actually only the first 4 triplets are used but for the
+ * high speed mode we need to pad it to 8 (microframes).
+ */
+#define SIZEOUTBUF         ((8*SIZEDAOUT))
+
+/*
+ * Size of the buffer for the dux commands: just now max size is determined
+ * by the analogue out + command byte + panic bytes...
+ */
+#define SIZEOFDUXBUFFER    ((8*SIZEDAOUT+2))
+
+/* Number of in-URBs which receive the data: min=2 */
+#define NUMOFINBUFFERSFULL     5
+
+/* Number of out-URBs which send the data: min=2 */
+#define NUMOFOUTBUFFERSFULL    5
+
+/* Number of in-URBs which receive the data: min=5 */
+/* must have more buffers due to buggy USB ctr */
+#define NUMOFINBUFFERSHIGH     10
+
+/* Number of out-URBs which send the data: min=5 */
+/* must have more buffers due to buggy USB ctr */
+#define NUMOFOUTBUFFERSHIGH    10
+
+/* Total number of usbdux devices */
+#define NUMUSBDUX             16
+
+/* Analogue in subdevice */
+#define SUBDEV_AD             0
+
+/* Analogue out subdevice */
+#define SUBDEV_DA             1
+
+/* Digital I/O */
+#define SUBDEV_DIO            2
+
+/* counter */
+#define SUBDEV_COUNTER        3
+
+/* timer aka pwm output */
+#define SUBDEV_PWM            4
+
+/* number of retries to get the right dux command */
+#define RETRIES 10
+
+/**************************************************/
+/* comedi constants */
+static const comedi_lrange range_usbdux_ai_range = { 4, {
+			BIP_RANGE(4.096),
+			BIP_RANGE(4.096 / 2),
+			UNI_RANGE(4.096),
+			UNI_RANGE(4.096 / 2)
+	}
+};
+
+static const comedi_lrange range_usbdux_ao_range = { 2, {
+			BIP_RANGE(4.096),
+			UNI_RANGE(4.096),
+	}
+};
+
+/*
+ * private structure of one subdevice
+ */
+
+/*
+ * This is the structure which holds all the data of
+ * this driver one sub device just now: A/D
+ */
+struct usbduxsub {
+	/* attached? */
+	int attached;
+	/* is it associated with a subdevice? */
+	int probed;
+	/* pointer to the usb-device */
+	struct usb_device *usbdev;
+	/* actual number of in-buffers */
+	int numOfInBuffers;
+	/* actual number of out-buffers */
+	int numOfOutBuffers;
+	/* ISO-transfer handling: buffers */
+	struct urb **urbIn;
+	struct urb **urbOut;
+	/* pwm-transfer handling */
+	struct urb *urbPwm;
+	/* PWM period */
+	lsampl_t pwmPeriod;
+	/* PWM internal delay for the GPIF in the FX2 */
+	int8_t pwmDelay;
+	/* size of the PWM buffer which holds the bit pattern */
+	int sizePwmBuf;
+	/* input buffer for the ISO-transfer */
+	int16_t *inBuffer;
+	/* input buffer for single insn */
+	int16_t *insnBuffer;
+	/* output buffer for single DA outputs */
+	int16_t *outBuffer;
+	/* interface number */
+	int ifnum;
+	/* interface structure in 2.6 */
+	struct usb_interface *interface;
+	/* comedi device for the interrupt context */
+	comedi_device *comedidev;
+	/* is it USB_SPEED_HIGH or not? */
+	short int high_speed;
+	/* asynchronous command is running */
+	short int ai_cmd_running;
+	short int ao_cmd_running;
+	/* pwm is running */
+	short int pwm_cmd_running;
+	/* continous aquisition */
+	short int ai_continous;
+	short int ao_continous;
+	/* number of samples to aquire */
+	int ai_sample_count;
+	int ao_sample_count;
+	/* time between samples in units of the timer */
+	unsigned int ai_timer;
+	unsigned int ao_timer;
+	/* counter between aquisitions */
+	unsigned int ai_counter;
+	unsigned int ao_counter;
+	/* interval in frames/uframes */
+	unsigned int ai_interval;
+	/* D/A commands */
+	int8_t *dac_commands;
+	/* commands */
+	int8_t *dux_commands;
+	struct semaphore sem;
+};
+
+/*
+ * The pointer to the private usb-data of the driver is also the private data
+ * for the comedi-device.  This has to be global as the usb subsystem needs
+ * global variables. The other reason is that this structure must be there
+ * _before_ any comedi command is issued. The usb subsystem must be initialised
+ * before comedi can access it.
+ */
+static struct usbduxsub usbduxsub[NUMUSBDUX];
+
+static DECLARE_MUTEX(start_stop_sem);
+
+/*
+ * Stops the data acquision
+ * It should be safe to call this function from any context
+ */
+static int usbduxsub_unlink_InURBs(struct usbduxsub *usbduxsub_tmp)
+{
+	int i = 0;
+	int err = 0;
+
+	if (usbduxsub_tmp && usbduxsub_tmp->urbIn) {
+		for (i = 0; i < usbduxsub_tmp->numOfInBuffers; i++) {
+			if (usbduxsub_tmp->urbIn[i]) {
+				/* We wait here until all transfers have been
+				 * cancelled. */
+				usb_kill_urb(usbduxsub_tmp->urbIn[i]);
+			}
+			dev_dbg(&usbduxsub_tmp->interface->dev,
+				"comedi: usbdux: unlinked InURB %d, err=%d\n",
+				i, err);
+		}
+	}
+	return err;
+}
+
+/*
+ * This will stop a running acquisition operation
+ * Is called from within this driver from both the
+ * interrupt context and from comedi
+ */
+static int usbdux_ai_stop(struct usbduxsub *this_usbduxsub, int do_unlink)
+{
+	int ret = 0;
+
+	if (!this_usbduxsub) {
+		dev_err(&this_usbduxsub->interface->dev,
+			"comedi?: usbdux_ai_stop: this_usbduxsub=NULL!\n");
+		return -EFAULT;
+	}
+	dev_dbg(&this_usbduxsub->interface->dev, "comedi: usbdux_ai_stop\n");
+
+	if (do_unlink) {
+		/* stop aquistion */
+		ret = usbduxsub_unlink_InURBs(this_usbduxsub);
+	}
+
+	this_usbduxsub->ai_cmd_running = 0;
+
+	return ret;
+}
+
+/*
+ * This will cancel a running acquisition operation.
+ * This is called by comedi but never from inside the driver.
+ */
+static int usbdux_ai_cancel(comedi_device *dev, comedi_subdevice *s)
+{
+	struct usbduxsub *this_usbduxsub;
+	int res = 0;
+
+	/* force unlink of all urbs */
+	this_usbduxsub = dev->private;
+	if (!this_usbduxsub)
+		return -EFAULT;
+
+	dev_dbg(&this_usbduxsub->interface->dev, "comedi: usbdux_ai_cancel\n");
+
+	/* prevent other CPUs from submitting new commands just now */
+	down(&this_usbduxsub->sem);
+	if (!(this_usbduxsub->probed)) {
+		up(&this_usbduxsub->sem);
+		return -ENODEV;
+	}
+	/* unlink only if the urb really has been submitted */
+	res = usbdux_ai_stop(this_usbduxsub, this_usbduxsub->ai_cmd_running);
+	up(&this_usbduxsub->sem);
+	return res;
+}
+
+/* analogue IN - interrupt service routine */
+static void usbduxsub_ai_IsocIrq(struct urb *urb)
+{
+	int i, err, n;
+	struct usbduxsub *this_usbduxsub;
+	comedi_device *this_comedidev;
+	comedi_subdevice *s;
+
+	/* the context variable points to the subdevice */
+	this_comedidev = urb->context;
+	/* the private structure of the subdevice is struct usbduxsub */
+	this_usbduxsub = this_comedidev->private;
+	/* subdevice which is the AD converter */
+	s = this_comedidev->subdevices + SUBDEV_AD;
+
+	/* first we test if something unusual has just happened */
+	switch (urb->status) {
+	case 0:
+		/* copy the result in the transfer buffer */
+		memcpy(this_usbduxsub->inBuffer,
+			urb->transfer_buffer, SIZEINBUF);
+		break;
+	case -EILSEQ:
+		/* error in the ISOchronous data */
+		/* we don't copy the data into the transfer buffer */
+		/* and recycle the last data byte */
+		dev_dbg(&urb->dev->dev,
+			"comedi%d: usbdux: CRC error in ISO IN stream.\n",
+			this_usbduxsub->comedidev->minor);
+
+		break;
+
+	case -ECONNRESET:
+	case -ENOENT:
+	case -ESHUTDOWN:
+	case -ECONNABORTED:
+		/* happens after an unlink command */
+		if (this_usbduxsub->ai_cmd_running) {
+			/* we are still running a command */
+			/* tell this comedi */
+			s->async->events |= COMEDI_CB_EOA;
+			s->async->events |= COMEDI_CB_ERROR;
+			comedi_event(this_usbduxsub->comedidev, s);
+			/* stop the transfer w/o unlink */
+			usbdux_ai_stop(this_usbduxsub, 0);
+		}
+		return;
+
+	default:
+		/* a real error on the bus */
+		/* pass error to comedi if we are really running a command */
+		if (this_usbduxsub->ai_cmd_running) {
+			dev_err(&urb->dev->dev,
+				"Non-zero urb status received in ai intr "
+				"context: %d\n", urb->status);
+			s->async->events |= COMEDI_CB_EOA;
+			s->async->events |= COMEDI_CB_ERROR;
+			comedi_event(this_usbduxsub->comedidev, s);
+			/* don't do an unlink here */
+			usbdux_ai_stop(this_usbduxsub, 0);
+		}
+		return;
+	}
+
+	/*
+	 * at this point we are reasonably sure that nothing dodgy has happened
+	 * are we running a command?
+	 */
+	if (unlikely((!(this_usbduxsub->ai_cmd_running)))) {
+		/*
+		 * not running a command, do not continue execution if no
+		 * asynchronous command is running in particular not resubmit
+		 */
+		return;
+	}
+
+	urb->dev = this_usbduxsub->usbdev;
+
+	/* resubmit the urb */
+	err = usb_submit_urb(urb, GFP_ATOMIC);
+	if (unlikely(err < 0)) {
+		dev_err(&urb->dev->dev,
+			"comedi_: urb resubmit failed in int-context! err=%d\n",
+			err);
+		if (err == -EL2NSYNC)
+			dev_err(&urb->dev->dev,
+				"buggy USB host controller or bug in IRQ "
+				"handler!\n");
+		s->async->events |= COMEDI_CB_EOA;
+		s->async->events |= COMEDI_CB_ERROR;
+		comedi_event(this_usbduxsub->comedidev, s);
+		/* don't do an unlink here */
+		usbdux_ai_stop(this_usbduxsub, 0);
+		return;
+	}
+
+	this_usbduxsub->ai_counter--;
+	if (likely(this_usbduxsub->ai_counter > 0))
+		return;
+
+	/* timer zero, transfer measurements to comedi */
+	this_usbduxsub->ai_counter = this_usbduxsub->ai_timer;
+
+	/* test, if we transmit only a fixed number of samples */
+	if (!(this_usbduxsub->ai_continous)) {
+		/* not continous, fixed number of samples */
+		this_usbduxsub->ai_sample_count--;
+		/* all samples received? */
+		if (this_usbduxsub->ai_sample_count < 0) {
+			/* prevent a resubmit next time */
+			usbdux_ai_stop(this_usbduxsub, 0);
+			/* say comedi that the acquistion is over */
+			s->async->events |= COMEDI_CB_EOA;
+			comedi_event(this_usbduxsub->comedidev, s);
+			return;
+		}
+	}
+	/* get the data from the USB bus and hand it over to comedi */
+	n = s->async->cmd.chanlist_len;
+	for (i = 0; i < n; i++) {
+		/* transfer data */
+		if (CR_RANGE(s->async->cmd.chanlist[i]) <= 1) {
+			comedi_buf_put
+				(s->async,
+				le16_to_cpu(this_usbduxsub->
+					inBuffer[i]) ^ 0x800);
+		} else {
+			comedi_buf_put
+				(s->async,
+				le16_to_cpu(this_usbduxsub->inBuffer[i]));
+		}
+	}
+	/* tell comedi that data is there */
+	comedi_event(this_usbduxsub->comedidev, s);
+}
+
+static int usbduxsub_unlink_OutURBs(struct usbduxsub *usbduxsub_tmp)
+{
+	int i = 0;
+	int err = 0;
+
+	if (usbduxsub_tmp && usbduxsub_tmp->urbOut) {
+		for (i = 0; i < usbduxsub_tmp->numOfOutBuffers; i++) {
+			if (usbduxsub_tmp->urbOut[i])
+				usb_kill_urb(usbduxsub_tmp->urbOut[i]);
+
+			dev_dbg(&usbduxsub_tmp->interface->dev,
+				"comedi: usbdux: unlinked OutURB %d: res=%d\n",
+				i, err);
+		}
+	}
+	return err;
+}
+
+/* This will cancel a running acquisition operation
+ * in any context.
+ */
+static int usbdux_ao_stop(struct usbduxsub *this_usbduxsub, int do_unlink)
+{
+	int ret = 0;
+
+	if (!this_usbduxsub)
+		return -EFAULT;
+	dev_dbg(&this_usbduxsub->interface->dev, "comedi: usbdux_ao_cancel\n");
+
+	if (do_unlink)
+		ret = usbduxsub_unlink_OutURBs(this_usbduxsub);
+
+	this_usbduxsub->ao_cmd_running = 0;
+
+	return ret;
+}
+
+/* force unlink, is called by comedi */
+static int usbdux_ao_cancel(comedi_device *dev, comedi_subdevice *s)
+{
+	struct usbduxsub *this_usbduxsub = dev->private;
+	int res = 0;
+
+	if (!this_usbduxsub)
+		return -EFAULT;
+
+	/* prevent other CPUs from submitting a command just now */
+	down(&this_usbduxsub->sem);
+	if (!(this_usbduxsub->probed)) {
+		up(&this_usbduxsub->sem);
+		return -ENODEV;
+	}
+	/* unlink only if it is really running */
+	res = usbdux_ao_stop(this_usbduxsub, this_usbduxsub->ao_cmd_running);
+	up(&this_usbduxsub->sem);
+	return res;
+}
+
+static void usbduxsub_ao_IsocIrq(struct urb *urb)
+{
+	int i, ret;
+	int8_t *datap;
+	struct usbduxsub *this_usbduxsub;
+	comedi_device *this_comedidev;
+	comedi_subdevice *s;
+
+	/* the context variable points to the subdevice */
+	this_comedidev = urb->context;
+	/* the private structure of the subdevice is struct usbduxsub */
+	this_usbduxsub = this_comedidev->private;
+
+	s = this_comedidev->subdevices + SUBDEV_DA;
+
+	switch (urb->status) {
+	case 0:
+		/* success */
+		break;
+
+	case -ECONNRESET:
+	case -ENOENT:
+	case -ESHUTDOWN:
+	case -ECONNABORTED:
+		/* after an unlink command, unplug, ... etc */
+		/* no unlink needed here. Already shutting down. */
+		if (this_usbduxsub->ao_cmd_running) {
+			s->async->events |= COMEDI_CB_EOA;
+			comedi_event(this_usbduxsub->comedidev, s);
+			usbdux_ao_stop(this_usbduxsub, 0);
+		}
+		return;
+
+	default:
+		/* a real error */
+		if (this_usbduxsub->ao_cmd_running) {
+			dev_err(&urb->dev->dev,
+				"comedi_: Non-zero urb status received in ao "
+				"intr context: %d\n", urb->status);
+			s->async->events |= COMEDI_CB_ERROR;
+			s->async->events |= COMEDI_CB_EOA;
+			comedi_event(this_usbduxsub->comedidev, s);
+			/* we do an unlink if we are in the high speed mode */
+			usbdux_ao_stop(this_usbduxsub, 0);
+		}
+		return;
+	}
+
+	/* are we actually running? */
+	if (!(this_usbduxsub->ao_cmd_running))
+		return;
+
+	/* normal operation: executing a command in this subdevice */
+	this_usbduxsub->ao_counter--;
+	if (this_usbduxsub->ao_counter <= 0) {
+		/* timer zero */
+		this_usbduxsub->ao_counter = this_usbduxsub->ao_timer;
+
+		/* handle non continous aquisition */
+		if (!(this_usbduxsub->ao_continous)) {
+			/* fixed number of samples */
+			this_usbduxsub->ao_sample_count--;
+			if (this_usbduxsub->ao_sample_count < 0) {
+				/* all samples transmitted */
+				usbdux_ao_stop(this_usbduxsub, 0);
+				s->async->events |= COMEDI_CB_EOA;
+				comedi_event(this_usbduxsub->comedidev, s);
+				/* no resubmit of the urb */
+				return;
+			}
+		}
+		/* transmit data to the USB bus */
+		((uint8_t *) (urb->transfer_buffer))[0] =
+			s->async->cmd.chanlist_len;
+		for (i = 0; i < s->async->cmd.chanlist_len; i++) {
+			sampl_t temp;
+			if (i >= NUMOUTCHANNELS)
+				break;
+
+			/* pointer to the DA */
+			datap =
+			       (&(((int8_t *)urb->transfer_buffer)[i * 3 + 1]));
+			/* get the data from comedi */
+			ret = comedi_buf_get(s->async, &temp);
+			datap[0] = temp;
+			datap[1] = temp >> 8;
+			datap[2] = this_usbduxsub->dac_commands[i];
+			/* printk("data[0]=%x, data[1]=%x, data[2]=%x\n", */
+			/* datap[0],datap[1],datap[2]); */
+			if (ret < 0) {
+				dev_err(&urb->dev->dev,
+					"comedi: buffer underflow\n");
+				s->async->events |= COMEDI_CB_EOA;
+				s->async->events |= COMEDI_CB_OVERFLOW;
+			}
+			/* transmit data to comedi */
+			s->async->events |= COMEDI_CB_BLOCK;
+			comedi_event(this_usbduxsub->comedidev, s);
+		}
+	}
+	urb->transfer_buffer_length = SIZEOUTBUF;
+	urb->dev = this_usbduxsub->usbdev;
+	urb->status = 0;
+	if (this_usbduxsub->ao_cmd_running) {
+		if (this_usbduxsub->high_speed) {
+			/* uframes */
+			urb->interval = 8;
+		} else {
+			/* frames */
+			urb->interval = 1;
+		}
+		urb->number_of_packets = 1;
+		urb->iso_frame_desc[0].offset = 0;
+		urb->iso_frame_desc[0].length = SIZEOUTBUF;
+		urb->iso_frame_desc[0].status = 0;
+		ret = usb_submit_urb(urb, GFP_ATOMIC);
+		if (ret < 0) {
+			dev_err(&urb->dev->dev,
+				"comedi_: ao urb resubm failed in int-cont. "
+				"ret=%d", ret);
+			if (ret == EL2NSYNC)
+				dev_err(&urb->dev->dev,
+					"buggy USB host controller or bug in "
+					"IRQ handling!\n");
+
+			s->async->events |= COMEDI_CB_EOA;
+			s->async->events |= COMEDI_CB_ERROR;
+			comedi_event(this_usbduxsub->comedidev, s);
+			/* don't do an unlink here */
+			usbdux_ao_stop(this_usbduxsub, 0);
+		}
+	}
+}
+
+static int usbduxsub_start(struct usbduxsub *usbduxsub)
+{
+	int errcode = 0;
+	uint8_t local_transfer_buffer[16];
+
+	if (usbduxsub->probed) {
+		/* 7f92 to zero */
+		local_transfer_buffer[0] = 0;
+		errcode = usb_control_msg(usbduxsub->usbdev,
+			/* create a pipe for a control transfer */
+			usb_sndctrlpipe(usbduxsub->usbdev, 0),
+			/* bRequest, "Firmware" */
+			USBDUXSUB_FIRMWARE,
+			/* bmRequestType */
+			VENDOR_DIR_OUT,
+			/* Value */
+			USBDUXSUB_CPUCS,
+			/* Index */
+			0x0000,
+			/* address of the transfer buffer */
+			local_transfer_buffer,
+			/* Length */
+			1,
+			/* Timeout */
+			EZTIMEOUT);
+		if (errcode < 0) {
+			dev_err(&usbduxsub->interface->dev,
+				"comedi_: control msg failed (start)\n");
+			return errcode;
+		}
+	}
+	return 0;
+}
+
+static int usbduxsub_stop(struct usbduxsub *usbduxsub)
+{
+	int errcode = 0;
+
+	uint8_t local_transfer_buffer[16];
+	if (usbduxsub->probed) {
+		/* 7f92 to one */
+		local_transfer_buffer[0] = 1;
+		errcode = usb_control_msg(usbduxsub->usbdev,
+			usb_sndctrlpipe(usbduxsub->usbdev, 0),
+			/* bRequest, "Firmware" */
+			USBDUXSUB_FIRMWARE,
+			/* bmRequestType */
+			VENDOR_DIR_OUT,
+			/* Value */
+			USBDUXSUB_CPUCS,
+			/* Index */
+			0x0000, local_transfer_buffer,
+			/* Length */
+			1,
+			/* Timeout */
+			EZTIMEOUT);
+		if (errcode < 0) {
+			dev_err(&usbduxsub->interface->dev,
+				"comedi_: control msg failed (stop)\n");
+			return errcode;
+		}
+	}
+	return 0;
+}
+
+static int usbduxsub_upload(struct usbduxsub *usbduxsub,
+			    uint8_t *local_transfer_buffer,
+			    unsigned int startAddr, unsigned int len)
+{
+	int errcode;
+
+	if (usbduxsub->probed) {
+		dev_dbg(&usbduxsub->interface->dev,
+			"comedi%d: usbdux: uploading %d bytes"
+			" to addr %d, first byte=%d.\n",
+			usbduxsub->comedidev->minor, len,
+			startAddr, local_transfer_buffer[0]);
+		errcode = usb_control_msg(usbduxsub->usbdev,
+			usb_sndctrlpipe(usbduxsub->usbdev, 0),
+			/* brequest, firmware */
+			USBDUXSUB_FIRMWARE,
+			/* bmRequestType */
+			VENDOR_DIR_OUT,
+			/* value */
+			startAddr,
+			/* index */
+			0x0000,
+			/* our local safe buffer */
+			local_transfer_buffer,
+			/* length */
+			len,
+			/* timeout */
+			EZTIMEOUT);
+		dev_dbg(&usbduxsub->interface->dev,
+			"comedi_: result=%d\n", errcode);
+		if (errcode < 0) {
+			dev_err(&usbduxsub->interface->dev,
+				"comedi_: upload failed\n");
+			return errcode;
+		}
+	} else {
+		/* no device on the bus for this index */
+		return -EFAULT;
+	}
+	return 0;
+}
+
+static int firmwareUpload(struct usbduxsub *usbduxsub, uint8_t *firmwareBinary,
+			  int sizeFirmware)
+{
+	int ret;
+
+	if (!firmwareBinary)
+		return 0;
+
+	ret = usbduxsub_stop(usbduxsub);
+	if (ret < 0) {
+		dev_err(&usbduxsub->interface->dev,
+			"comedi_: can not stop firmware\n");
+		return ret;
+	}
+	ret = usbduxsub_upload(usbduxsub, firmwareBinary, 0, sizeFirmware);
+	if (ret < 0) {
+		dev_err(&usbduxsub->interface->dev,
+			"comedi_: firmware upload failed\n");
+		return ret;
+	}
+	ret = usbduxsub_start(usbduxsub);
+	if (ret < 0) {
+		dev_err(&usbduxsub->interface->dev,
+			"comedi_: can not start firmware\n");
+		return ret;
+	}
+	return 0;
+}
+
+static int usbduxsub_submit_InURBs(struct usbduxsub *usbduxsub)
+{
+	int i, errFlag;
+
+	if (!usbduxsub)
+		return -EFAULT;
+
+	/* Submit all URBs and start the transfer on the bus */
+	for (i = 0; i < usbduxsub->numOfInBuffers; i++) {
+		/* in case of a resubmission after an unlink... */
+		usbduxsub->urbIn[i]->interval = usbduxsub->ai_interval;
+		usbduxsub->urbIn[i]->context = usbduxsub->comedidev;
+		usbduxsub->urbIn[i]->dev = usbduxsub->usbdev;
+		usbduxsub->urbIn[i]->status = 0;
+		usbduxsub->urbIn[i]->transfer_flags = URB_ISO_ASAP;
+		dev_dbg(&usbduxsub->interface->dev,
+			"comedi%d: submitting in-urb[%d]: %p,%p intv=%d\n",
+			usbduxsub->comedidev->minor, i,
+			(usbduxsub->urbIn[i]->context),
+			(usbduxsub->urbIn[i]->dev),
+			(usbduxsub->urbIn[i]->interval));
+		errFlag = usb_submit_urb(usbduxsub->urbIn[i], GFP_ATOMIC);
+		if (errFlag) {
+			dev_err(&usbduxsub->interface->dev,
+				"comedi_: ai: usb_submit_urb(%d) error %d\n",
+				i, errFlag);
+			return errFlag;
+		}
+	}
+	return 0;
+}
+
+static int usbduxsub_submit_OutURBs(struct usbduxsub *usbduxsub)
+{
+	int i, errFlag;
+
+	if (!usbduxsub)
+		return -EFAULT;
+
+	for (i = 0; i < usbduxsub->numOfOutBuffers; i++) {
+		dev_dbg(&usbduxsub->interface->dev,
+			"comedi_: submitting out-urb[%d]\n", i);
+		/* in case of a resubmission after an unlink... */
+		usbduxsub->urbOut[i]->context = usbduxsub->comedidev;
+		usbduxsub->urbOut[i]->dev = usbduxsub->usbdev;
+		usbduxsub->urbOut[i]->status = 0;
+		usbduxsub->urbOut[i]->transfer_flags = URB_ISO_ASAP;
+		errFlag = usb_submit_urb(usbduxsub->urbOut[i], GFP_ATOMIC);
+		if (errFlag) {
+			dev_err(&usbduxsub->interface->dev,
+				"comedi_: ao: usb_submit_urb(%d) error %d\n",
+				i, errFlag);
+			return errFlag;
+		}
+	}
+	return 0;
+}
+
+static int usbdux_ai_cmdtest(comedi_device *dev, comedi_subdevice *s,
+			     comedi_cmd *cmd)
+{
+	int err = 0, tmp, i;
+	unsigned int tmpTimer;
+	struct usbduxsub *this_usbduxsub = dev->private;
+
+	if (!(this_usbduxsub->probed))
+		return -ENODEV;
+
+	dev_dbg(&this_usbduxsub->interface->dev,
+		"comedi%d: usbdux_ai_cmdtest\n", dev->minor);
+
+	/* make sure triggers are valid */
+	/* Only immediate triggers are allowed */
+	tmp = cmd->start_src;
+	cmd->start_src &= TRIG_NOW | TRIG_INT;
+	if (!cmd->start_src || tmp != cmd->start_src)
+		err++;
+
+	/* trigger should happen timed */
+	tmp = cmd->scan_begin_src;
+	/* start a new _scan_ with a timer */
+	cmd->scan_begin_src &= TRIG_TIMER;
+	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
+		err++;
+
+	/* scanning is continous */
+	tmp = cmd->convert_src;
+	cmd->convert_src &= TRIG_NOW;
+	if (!cmd->convert_src || tmp != cmd->convert_src)
+		err++;
+
+	/* issue a trigger when scan is finished and start a new scan */
+	tmp = cmd->scan_end_src;
+	cmd->scan_end_src &= TRIG_COUNT;
+	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
+		err++;
+
+	/* trigger at the end of count events or not, stop condition or not */
+	tmp = cmd->stop_src;
+	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
+	if (!cmd->stop_src || tmp != cmd->stop_src)
+		err++;
+
+	if (err)
+		return 1;
+
+	/*
+	 * step 2: make sure trigger sources are unique and mutually compatible
+	 * note that mutual compatiblity is not an issue here
+	 */
+	if (cmd->scan_begin_src != TRIG_FOLLOW &&
+		cmd->scan_begin_src != TRIG_EXT &&
+		cmd->scan_begin_src != TRIG_TIMER)
+		err++;
+	if (cmd->stop_src != TRIG_COUNT && cmd->stop_src != TRIG_NONE)
+		err++;
+
+	if (err)
+		return 2;
+
+	/* step 3: make sure arguments are trivially compatible */
+	if (cmd->start_arg != 0) {
+		cmd->start_arg = 0;
+		err++;
+	}
+
+	if (cmd->scan_begin_src == TRIG_FOLLOW) {
+		/* internal trigger */
+		if (cmd->scan_begin_arg != 0) {
+			cmd->scan_begin_arg = 0;
+			err++;
+		}
+	}
+
+	if (cmd->scan_begin_src == TRIG_TIMER) {
+		if (this_usbduxsub->high_speed) {
+			/*
+			 * In high speed mode microframes are possible.
+			 * However, during one microframe we can roughly
+			 * sample one channel. Thus, the more channels
+			 * are in the channel list the more time we need.
+			 */
+			i = 1;
+			/* find a power of 2 for the number of channels */
+			while (i < (cmd->chanlist_len))
+				i = i * 2;
+
+			if (cmd->scan_begin_arg < (1000000 / 8 * i)) {
+				cmd->scan_begin_arg = 1000000 / 8 * i;
+				err++;
+			}
+			/* now calc the real sampling rate with all the
+			 * rounding errors */
+			tmpTimer =
+				((unsigned int)(cmd->scan_begin_arg / 125000)) *
+				125000;
+			if (cmd->scan_begin_arg != tmpTimer) {
+				cmd->scan_begin_arg = tmpTimer;
+				err++;
+			}
+		} else {
+			/* full speed */
+			/* 1kHz scans every USB frame */
+			if (cmd->scan_begin_arg < 1000000) {
+				cmd->scan_begin_arg = 1000000;
+				err++;
+			}
+			/*
+			 * calc the real sampling rate with the rounding errors
+			 */
+			tmpTimer = ((unsigned int)(cmd->scan_begin_arg /
+					1000000)) * 1000000;
+			if (cmd->scan_begin_arg != tmpTimer) {
+				cmd->scan_begin_arg = tmpTimer;
+				err++;
+			}
+		}
+	}
+	/* the same argument */
+	if (cmd->scan_end_arg != cmd->chanlist_len) {
+		cmd->scan_end_arg = cmd->chanlist_len;
+		err++;
+	}
+
+	if (cmd->stop_src == TRIG_COUNT) {
+		/* any count is allowed */
+	} else {
+		/* TRIG_NONE */
+		if (cmd->stop_arg != 0) {
+			cmd->stop_arg = 0;
+			err++;
+		}
+	}
+
+	if (err)
+		return 3;
+
+	return 0;
+}
+
+/*
+ * creates the ADC command for the MAX1271
+ * range is the range value from comedi
+ */
+static int8_t create_adc_command(unsigned int chan, int range)
+{
+	int8_t p = (range <= 1);
+	int8_t r = ((range % 2) == 0);
+	return (chan << 4) | ((p == 1) << 2) | ((r == 1) << 3);
+}
+
+/* bulk transfers to usbdux */
+
+#define SENDADCOMMANDS            0
+#define SENDDACOMMANDS            1
+#define SENDDIOCONFIGCOMMAND      2
+#define SENDDIOBITSCOMMAND        3
+#define SENDSINGLEAD              4
+#define READCOUNTERCOMMAND        5
+#define WRITECOUNTERCOMMAND       6
+#define SENDPWMON                 7
+#define SENDPWMOFF                8
+
+static int send_dux_commands(struct usbduxsub *this_usbduxsub, int cmd_type)
+{
+	int result, nsent;
+
+	this_usbduxsub->dux_commands[0] = cmd_type;
+#ifdef NOISY_DUX_DEBUGBUG
+	printk(KERN_DEBUG "comedi%d: usbdux: dux_commands: ",
+		this_usbduxsub->comedidev->minor);
+	for (result = 0; result < SIZEOFDUXBUFFER; result++)
+		printk(" %02x", this_usbduxsub->dux_commands[result]);
+	printk("\n");
+#endif
+	result = usb_bulk_msg(this_usbduxsub->usbdev,
+			      usb_sndbulkpipe(this_usbduxsub->usbdev,
+					      COMMAND_OUT_EP),
+			      this_usbduxsub->dux_commands, SIZEOFDUXBUFFER,
+			      &nsent, 10);
+	if (result < 0)
+		dev_err(&this_usbduxsub->interface->dev, "comedi%d: "
+			"could not transmit dux_command to the usb-device, "
+			"err=%d\n", this_usbduxsub->comedidev->minor, result);
+
+	return result;
+}
+
+static int receive_dux_commands(struct usbduxsub *this_usbduxsub, int command)
+{
+	int result = (-EFAULT);
+	int nrec;
+	int i;
+
+	for (i = 0; i < RETRIES; i++) {
+		result = usb_bulk_msg(this_usbduxsub->usbdev,
+				      usb_rcvbulkpipe(this_usbduxsub->usbdev,
+						      COMMAND_IN_EP),
+				      this_usbduxsub->insnBuffer, SIZEINSNBUF,
+				      &nrec, 1);
+		if (result < 0) {
+			dev_err(&this_usbduxsub->interface->dev, "comedi%d: "
+				"insn: USB error %d while receiving DUX command"
+				"\n", this_usbduxsub->comedidev->minor, result);
+			return result;
+		}
+		if (le16_to_cpu(this_usbduxsub->insnBuffer[0]) == command)
+			return result;
+	}
+	/* this is only reached if the data has been requested a couple of
+	 * times */
+	dev_err(&this_usbduxsub->interface->dev, "comedi%d: insn: "
+		"wrong data returned from firmware: want cmd %d, got cmd %d.\n",
+		this_usbduxsub->comedidev->minor, command,
+		le16_to_cpu(this_usbduxsub->insnBuffer[0]));
+	return -EFAULT;
+}
+
+static int usbdux_ai_inttrig(comedi_device *dev, comedi_subdevice *s,
+			     unsigned int trignum)
+{
+	int ret;
+	struct usbduxsub *this_usbduxsub = dev->private;
+	if (!this_usbduxsub)
+		return -EFAULT;
+
+	down(&this_usbduxsub->sem);
+	if (!(this_usbduxsub->probed)) {
+		up(&this_usbduxsub->sem);
+		return -ENODEV;
+	}
+	dev_dbg(&this_usbduxsub->interface->dev,
+		"comedi%d: usbdux_ai_inttrig\n", dev->minor);
+
+	if (trignum != 0) {
+		dev_err(&this_usbduxsub->interface->dev,
+			"comedi%d: usbdux_ai_inttrig: invalid trignum\n",
+			dev->minor);
+		up(&this_usbduxsub->sem);
+		return -EINVAL;
+	}
+	if (!(this_usbduxsub->ai_cmd_running)) {
+		this_usbduxsub->ai_cmd_running = 1;
+		ret = usbduxsub_submit_InURBs(this_usbduxsub);
+		if (ret < 0) {
+			dev_err(&this_usbduxsub->interface->dev,
+				"comedi%d: usbdux_ai_inttrig: "
+				"urbSubmit: err=%d\n", dev->minor, ret);
+			this_usbduxsub->ai_cmd_running = 0;
+			up(&this_usbduxsub->sem);
+			return ret;
+		}
+		s->async->inttrig = NULL;
+	} else {
+		dev_err(&this_usbduxsub->interface->dev,
+			"comedi%d: ai_inttrig but acqu is already running\n",
+			dev->minor);
+	}
+	up(&this_usbduxsub->sem);
+	return 1;
+}
+
+static int usbdux_ai_cmd(comedi_device *dev, comedi_subdevice *s)
+{
+	comedi_cmd *cmd = &s->async->cmd;
+	unsigned int chan, range;
+	int i, ret;
+	struct usbduxsub *this_usbduxsub = dev->private;
+	int result;
+
+	if (!this_usbduxsub)
+		return -EFAULT;
+
+	dev_dbg(&this_usbduxsub->interface->dev,
+		"comedi%d: usbdux_ai_cmd\n", dev->minor);
+
+	/* block other CPUs from starting an ai_cmd */
+	down(&this_usbduxsub->sem);
+
+	if (!(this_usbduxsub->probed)) {
+		up(&this_usbduxsub->sem);
+		return -ENODEV;
+	}
+	if (this_usbduxsub->ai_cmd_running) {
+		dev_err(&this_usbduxsub->interface->dev, "comedi%d: "
+			"ai_cmd not possible. Another ai_cmd is running.\n",
+			dev->minor);
+		up(&this_usbduxsub->sem);
+		return -EBUSY;
+	}
+	/* set current channel of the running aquisition to zero */
+	s->async->cur_chan = 0;
+
+	this_usbduxsub->dux_commands[1] = cmd->chanlist_len;
+	for (i = 0; i < cmd->chanlist_len; ++i) {
+		chan = CR_CHAN(cmd->chanlist[i]);
+		range = CR_RANGE(cmd->chanlist[i]);
+		if (i >= NUMCHANNELS) {
+			dev_err(&this_usbduxsub->interface->dev,
+				"comedi%d: channel list too long\n",
+				dev->minor);
+			break;
+		}
+		this_usbduxsub->dux_commands[i + 2] =
+			create_adc_command(chan, range);
+	}
+
+	dev_dbg(&this_usbduxsub->interface->dev,
+		"comedi %d: sending commands to the usb device: size=%u\n",
+		dev->minor, NUMCHANNELS);
+
+	result = send_dux_commands(this_usbduxsub, SENDADCOMMANDS);
+	if (result < 0) {
+		up(&this_usbduxsub->sem);
+		return result;
+	}
+
+	if (this_usbduxsub->high_speed) {
+		/*
+		 * every channel gets a time window of 125us. Thus, if we
+		 * sample all 8 channels we need 1ms. If we sample only one
+		 * channel we need only 125us
+		 */
+		this_usbduxsub->ai_interval = 1;
+		/* find a power of 2 for the interval */
+		while ((this_usbduxsub->ai_interval) < (cmd->chanlist_len)) {
+			this_usbduxsub->ai_interval =
+				(this_usbduxsub->ai_interval) * 2;
+		}
+		this_usbduxsub->ai_timer = cmd->scan_begin_arg / (125000 *
+			(this_usbduxsub->ai_interval));
+	} else {
+		/* interval always 1ms */
+		this_usbduxsub->ai_interval = 1;
+		this_usbduxsub->ai_timer = cmd->scan_begin_arg / 1000000;
+	}
+	if (this_usbduxsub->ai_timer < 1) {
+		dev_err(&this_usbduxsub->interface->dev, "comedi%d: ai_cmd: "
+			"timer=%d, scan_begin_arg=%d. "
+			"Not properly tested by cmdtest?\n", dev->minor,
+			this_usbduxsub->ai_timer, cmd->scan_begin_arg);
+		up(&this_usbduxsub->sem);
+		return -EINVAL;
+	}
+	this_usbduxsub->ai_counter = this_usbduxsub->ai_timer;
+
+	if (cmd->stop_src == TRIG_COUNT) {
+		/* data arrives as one packet */
+		this_usbduxsub->ai_sample_count = cmd->stop_arg;
+		this_usbduxsub->ai_continous = 0;
+	} else {
+		/* continous aquisition */
+		this_usbduxsub->ai_continous = 1;
+		this_usbduxsub->ai_sample_count = 0;
+	}
+
+	if (cmd->start_src == TRIG_NOW) {
+		/* enable this acquisition operation */
+		this_usbduxsub->ai_cmd_running = 1;
+		ret = usbduxsub_submit_InURBs(this_usbduxsub);
+		if (ret < 0) {
+			this_usbduxsub->ai_cmd_running = 0;
+			/* fixme: unlink here?? */
+			up(&this_usbduxsub->sem);
+			return ret;
+		}
+		s->async->inttrig = NULL;
+	} else {
+		/* TRIG_INT */
+		/* don't enable the acquision operation */
+		/* wait for an internal signal */
+		s->async->inttrig = usbdux_ai_inttrig;
+	}
+	up(&this_usbduxsub->sem);
+	return 0;
+}
+
+/* Mode 0 is used to get a single conversion on demand */
+static int usbdux_ai_insn_read(comedi_device *dev, comedi_subdevice *s,
+			       comedi_insn *insn, lsampl_t *data)
+{
+	int i;
+	lsampl_t one = 0;
+	int chan, range;
+	int err;
+	struct usbduxsub *this_usbduxsub = dev->private;
+
+	if (!this_usbduxsub)
+		return 0;
+
+	dev_dbg(&this_usbduxsub->interface->dev,
+		"comedi%d: ai_insn_read, insn->n=%d, insn->subdev=%d\n",
+		dev->minor, insn->n, insn->subdev);
+
+	down(&this_usbduxsub->sem);
+	if (!(this_usbduxsub->probed)) {
+		up(&this_usbduxsub->sem);
+		return -ENODEV;
+	}
+	if (this_usbduxsub->ai_cmd_running) {
+		dev_err(&this_usbduxsub->interface->dev,
+			"comedi%d: ai_insn_read not possible. "
+			"Async Command is running.\n", dev->minor);
+		up(&this_usbduxsub->sem);
+		return 0;
+	}
+
+	/* sample one channel */
+	chan = CR_CHAN(insn->chanspec);
+	range = CR_RANGE(insn->chanspec);
+	/* set command for the first channel */
+	this_usbduxsub->dux_commands[1] = create_adc_command(chan, range);
+
+	/* adc commands */
+	err = send_dux_commands(this_usbduxsub, SENDSINGLEAD);
+	if (err < 0) {
+		up(&this_usbduxsub->sem);
+		return err;
+	}
+
+	for (i = 0; i < insn->n; i++) {
+		err = receive_dux_commands(this_usbduxsub, SENDSINGLEAD);
+		if (err < 0) {
+			up(&this_usbduxsub->sem);
+			return 0;
+		}
+		one = le16_to_cpu(this_usbduxsub->insnBuffer[1]);
+		if (CR_RANGE(insn->chanspec) <= 1)
+			one = one ^ 0x800;
+
+		data[i] = one;
+	}
+	up(&this_usbduxsub->sem);
+	return i;
+}
+
+/************************************/
+/* analog out */
+
+static int usbdux_ao_insn_read(comedi_device *dev, comedi_subdevice *s,
+			       comedi_insn *insn, lsampl_t *data)
+{
+	int i;
+	int chan = CR_CHAN(insn->chanspec);
+	struct usbduxsub *this_usbduxsub = dev->private;
+
+	if (!this_usbduxsub)
+		return -EFAULT;
+
+	down(&this_usbduxsub->sem);
+	if (!(this_usbduxsub->probed)) {
+		up(&this_usbduxsub->sem);
+		return -ENODEV;
+	}
+	for (i = 0; i < insn->n; i++)
+		data[i] = this_usbduxsub->outBuffer[chan];
+
+	up(&this_usbduxsub->sem);
+	return i;
+}
+
+static int usbdux_ao_insn_write(comedi_device *dev, comedi_subdevice *s,
+				comedi_insn *insn, lsampl_t *data)
+{
+	int i, err;
+	int chan = CR_CHAN(insn->chanspec);
+	struct usbduxsub *this_usbduxsub = dev->private;
+
+	if (!this_usbduxsub)
+		return -EFAULT;
+
+	dev_dbg(&this_usbduxsub->interface->dev,
+		"comedi%d: ao_insn_write\n", dev->minor);
+
+	down(&this_usbduxsub->sem);
+	if (!(this_usbduxsub->probed)) {
+		up(&this_usbduxsub->sem);
+		return -ENODEV;
+	}
+	if (this_usbduxsub->ao_cmd_running) {
+		dev_err(&this_usbduxsub->interface->dev,
+			"comedi%d: ao_insn_write: "
+			"ERROR: asynchronous ao_cmd is running\n", dev->minor);
+		up(&this_usbduxsub->sem);
+		return 0;
+	}
+
+	for (i = 0; i < insn->n; i++) {
+		dev_dbg(&this_usbduxsub->interface->dev,
+			"comedi%d: ao_insn_write: data[chan=%d,i=%d]=%d\n",
+			dev->minor, chan, i, data[i]);
+
+		/* number of channels: 1 */
+		this_usbduxsub->dux_commands[1] = 1;
+		/* one 16 bit value */
+		*((int16_t *) (this_usbduxsub->dux_commands + 2)) =
+			cpu_to_le16(data[i]);
+		this_usbduxsub->outBuffer[chan] = data[i];
+		/* channel number */
+		this_usbduxsub->dux_commands[4] = (chan << 6);
+		err = send_dux_commands(this_usbduxsub, SENDDACOMMANDS);
+		if (err < 0) {
+			up(&this_usbduxsub->sem);
+			return err;
+		}
+	}
+	up(&this_usbduxsub->sem);
+
+	return i;
+}
+
+static int usbdux_ao_inttrig(comedi_device *dev, comedi_subdevice *s,
+			     unsigned int trignum)
+{
+	int ret;
+	struct usbduxsub *this_usbduxsub = dev->private;
+
+	if (!this_usbduxsub)
+		return -EFAULT;
+
+	down(&this_usbduxsub->sem);
+	if (!(this_usbduxsub->probed)) {
+		up(&this_usbduxsub->sem);
+		return -ENODEV;
+	}
+	if (trignum != 0) {
+		dev_err(&this_usbduxsub->interface->dev,
+			"comedi%d: usbdux_ao_inttrig: invalid trignum\n",
+			dev->minor);
+		return -EINVAL;
+	}
+	if (!(this_usbduxsub->ao_cmd_running)) {
+		this_usbduxsub->ao_cmd_running = 1;
+		ret = usbduxsub_submit_OutURBs(this_usbduxsub);
+		if (ret < 0) {
+			dev_err(&this_usbduxsub->interface->dev,
+				"comedi%d: usbdux_ao_inttrig: submitURB: "
+				"err=%d\n", dev->minor, ret);
+			this_usbduxsub->ao_cmd_running = 0;
+			up(&this_usbduxsub->sem);
+			return ret;
+		}
+		s->async->inttrig = NULL;
+	} else {
+		dev_err(&this_usbduxsub->interface->dev,
+			"comedi%d: ao_inttrig but acqu is already running.\n",
+			dev->minor);
+	}
+	up(&this_usbduxsub->sem);
+	return 1;
+}
+
+static int usbdux_ao_cmdtest(comedi_device *dev, comedi_subdevice *s,
+			     comedi_cmd *cmd)
+{
+	int err = 0, tmp;
+	struct usbduxsub *this_usbduxsub = dev->private;
+
+	if (!this_usbduxsub)
+		return -EFAULT;
+
+	if (!(this_usbduxsub->probed))
+		return -ENODEV;
+
+	dev_dbg(&this_usbduxsub->interface->dev,
+		"comedi%d: usbdux_ao_cmdtest\n", dev->minor);
+
+	/* make sure triggers are valid */
+	/* Only immediate triggers are allowed */
+	tmp = cmd->start_src;
+	cmd->start_src &= TRIG_NOW | TRIG_INT;
+	if (!cmd->start_src || tmp != cmd->start_src)
+		err++;
+
+	/* trigger should happen timed */
+	tmp = cmd->scan_begin_src;
+	/* just now we scan also in the high speed mode every frame */
+	/* this is due to ehci driver limitations */
+	if (0) {		/* (this_usbduxsub->high_speed) */
+		/* start immidiately a new scan */
+		/* the sampling rate is set by the coversion rate */
+		cmd->scan_begin_src &= TRIG_FOLLOW;
+	} else {
+		/* start a new scan (output at once) with a timer */
+		cmd->scan_begin_src &= TRIG_TIMER;
+	}
+	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
+		err++;
+
+	/* scanning is continous */
+	tmp = cmd->convert_src;
+	/* we always output at 1kHz just now all channels at once */
+	if (0) {		/* (this_usbduxsub->high_speed) */
+		/*
+		 * in usb-2.0 only one conversion it tranmitted but with 8kHz/n
+		 */
+		cmd->convert_src &= TRIG_TIMER;
+	} else {
+		/* all conversion events happen simultaneously with a rate of
+		 * 1kHz/n */
+		cmd->convert_src &= TRIG_NOW;
+	}
+	if (!cmd->convert_src || tmp != cmd->convert_src)
+		err++;
+
+	/* issue a trigger when scan is finished and start a new scan */
+	tmp = cmd->scan_end_src;
+	cmd->scan_end_src &= TRIG_COUNT;
+	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
+		err++;
+
+	/* trigger at the end of count events or not, stop condition or not */
+	tmp = cmd->stop_src;
+	cmd->stop_src &= TRIG_COUNT | TRIG_NONE;
+	if (!cmd->stop_src || tmp != cmd->stop_src)
+		err++;
+
+	if (err)
+		return 1;
+
+	/*
+	 * step 2: make sure trigger sources are unique and mutually compatible
+	 * note that mutual compatiblity is not an issue here
+	 */
+	if (cmd->scan_begin_src != TRIG_FOLLOW &&
+		cmd->scan_begin_src != TRIG_EXT &&
+		cmd->scan_begin_src != TRIG_TIMER)
+		err++;
+	if (cmd->stop_src != TRIG_COUNT && cmd->stop_src != TRIG_NONE)
+		err++;
+
+	if (err)
+		return 2;
+
+	/* step 3: make sure arguments are trivially compatible */
+
+	if (cmd->start_arg != 0) {
+		cmd->start_arg = 0;
+		err++;
+	}
+
+	if (cmd->scan_begin_src == TRIG_FOLLOW) {
+		/* internal trigger */
+		if (cmd->scan_begin_arg != 0) {
+			cmd->scan_begin_arg = 0;
+			err++;
+		}
+	}
+
+	if (cmd->scan_begin_src == TRIG_TIMER) {
+		/* timer */
+		if (cmd->scan_begin_arg < 1000000) {
+			cmd->scan_begin_arg = 1000000;
+			err++;
+		}
+	}
+	/* not used now, is for later use */
+	if (cmd->convert_src == TRIG_TIMER) {
+		if (cmd->convert_arg < 125000) {
+			cmd->convert_arg = 125000;
+			err++;
+		}
+	}
+
+	/* the same argument */
+	if (cmd->scan_end_arg != cmd->chanlist_len) {
+		cmd->scan_end_arg = cmd->chanlist_len;
+		err++;
+	}
+
+	if (cmd->stop_src == TRIG_COUNT) {
+		/* any count is allowed */
+	} else {
+		/* TRIG_NONE */
+		if (cmd->stop_arg != 0) {
+			cmd->stop_arg = 0;
+			err++;
+		}
+	}
+
+	dev_dbg(&this_usbduxsub->interface->dev, "comedi%d: err=%d, "
+		"scan_begin_src=%d, scan_begin_arg=%d, convert_src=%d, "
+		"convert_arg=%d\n", dev->minor, err, cmd->scan_begin_src,
+		cmd->scan_begin_arg, cmd->convert_src, cmd->convert_arg);
+
+	if (err)
+		return 3;
+
+	return 0;
+}
+
+static int usbdux_ao_cmd(comedi_device *dev, comedi_subdevice *s)
+{
+	comedi_cmd *cmd = &s->async->cmd;
+	unsigned int chan, gain;
+	int i, ret;
+	struct usbduxsub *this_usbduxsub = dev->private;
+
+	if (!this_usbduxsub)
+		return -EFAULT;
+
+	down(&this_usbduxsub->sem);
+	if (!(this_usbduxsub->probed)) {
+		up(&this_usbduxsub->sem);
+		return -ENODEV;
+	}
+	dev_dbg(&this_usbduxsub->interface->dev,
+		"comedi%d: %s\n", dev->minor, __func__);
+
+	/* set current channel of the running aquisition to zero */
+	s->async->cur_chan = 0;
+	for (i = 0; i < cmd->chanlist_len; ++i) {
+		chan = CR_CHAN(cmd->chanlist[i]);
+		gain = CR_RANGE(cmd->chanlist[i]);
+		if (i >= NUMOUTCHANNELS) {
+			dev_err(&this_usbduxsub->interface->dev,
+				"comedi%d: %s: channel list too long\n",
+				dev->minor, __func__);
+			break;
+		}
+		this_usbduxsub->dac_commands[i] = (chan << 6);
+		dev_dbg(&this_usbduxsub->interface->dev,
+			"comedi%d: dac command for ch %d is %x\n",
+			dev->minor, i, this_usbduxsub->dac_commands[i]);
+	}
+
+	/* we count in steps of 1ms (125us) */
+	/* 125us mode not used yet */
+	if (0) {		/* (this_usbduxsub->high_speed) */
+		/* 125us */
+		/* timing of the conversion itself: every 125 us */
+		this_usbduxsub->ao_timer = cmd->convert_arg / 125000;
+	} else {
+		/* 1ms */
+		/* timing of the scan: we get all channels at once */
+		this_usbduxsub->ao_timer = cmd->scan_begin_arg / 1000000;
+		dev_dbg(&this_usbduxsub->interface->dev,
+			"comedi%d: scan_begin_src=%d, scan_begin_arg=%d, "
+			"convert_src=%d, convert_arg=%d\n", dev->minor,
+			cmd->scan_begin_src, cmd->scan_begin_arg,
+			cmd->convert_src, cmd->convert_arg);
+		dev_dbg(&this_usbduxsub->interface->dev,
+			"comedi%d: ao_timer=%d (ms)\n",
+			dev->minor, this_usbduxsub->ao_timer);
+		if (this_usbduxsub->ao_timer < 1) {
+			dev_err(&this_usbduxsub->interface->dev,
+				"comedi%d: usbdux: ao_timer=%d, "
+				"scan_begin_arg=%d. "
+				"Not properly tested by cmdtest?\n",
+				dev->minor, this_usbduxsub->ao_timer,
+				cmd->scan_begin_arg);
+			up(&this_usbduxsub->sem);
+			return -EINVAL;
+		}
+	}
+	this_usbduxsub->ao_counter = this_usbduxsub->ao_timer;
+
+	if (cmd->stop_src == TRIG_COUNT) {
+		/* not continous */
+		/* counter */
+		/* high speed also scans everything at once */
+		if (0) {	/* (this_usbduxsub->high_speed) */
+			this_usbduxsub->ao_sample_count =
+				(cmd->stop_arg) * (cmd->scan_end_arg);
+		} else {
+			/* there's no scan as the scan has been */
+			/* perf inside the FX2 */
+			/* data arrives as one packet */
+			this_usbduxsub->ao_sample_count = cmd->stop_arg;
+		}
+		this_usbduxsub->ao_continous = 0;
+	} else {
+		/* continous aquisition */
+		this_usbduxsub->ao_continous = 1;
+		this_usbduxsub->ao_sample_count = 0;
+	}
+
+	if (cmd->start_src == TRIG_NOW) {
+		/* enable this acquisition operation */
+		this_usbduxsub->ao_cmd_running = 1;
+		ret = usbduxsub_submit_OutURBs(this_usbduxsub);
+		if (ret < 0) {
+			this_usbduxsub->ao_cmd_running = 0;
+			/* fixme: unlink here?? */
+			up(&this_usbduxsub->sem);
+			return ret;
+		}
+		s->async->inttrig = NULL;
+	} else {
+		/* TRIG_INT */
+		/* submit the urbs later */
+		/* wait for an internal signal */
+		s->async->inttrig = usbdux_ao_inttrig;
+	}
+
+	up(&this_usbduxsub->sem);
+	return 0;
+}
+
+static int usbdux_dio_insn_config(comedi_device *dev, comedi_subdevice *s,
+				  comedi_insn *insn, lsampl_t *data)
+{
+	int chan = CR_CHAN(insn->chanspec);
+
+	/* The input or output configuration of each digital line is
+	 * configured by a special insn_config instruction.  chanspec
+	 * contains the channel to be changed, and data[0] contains the
+	 * value COMEDI_INPUT or COMEDI_OUTPUT. */
+
+	switch (data[0]) {
+	case INSN_CONFIG_DIO_OUTPUT:
+		s->io_bits |= 1 << chan;	/* 1 means Out */
+		break;
+	case INSN_CONFIG_DIO_INPUT:
+		s->io_bits &= ~(1 << chan);
+		break;
+	case INSN_CONFIG_DIO_QUERY:
+		data[1] =
+			(s->
+			io_bits & (1 << chan)) ? COMEDI_OUTPUT : COMEDI_INPUT;
+		break;
+	default:
+		return -EINVAL;
+		break;
+	}
+	/* we don't tell the firmware here as it would take 8 frames */
+	/* to submit the information. We do it in the insn_bits. */
+	return insn->n;
+}
+
+static int usbdux_dio_insn_bits(comedi_device *dev, comedi_subdevice *s,
+				comedi_insn *insn, lsampl_t *data)
+{
+
+	struct usbduxsub *this_usbduxsub = dev->private;
+	int err;
+
+	if (!this_usbduxsub)
+		return -EFAULT;
+
+
+	if (insn->n != 2)
+		return -EINVAL;
+
+	down(&this_usbduxsub->sem);
+
+	if (!(this_usbduxsub->probed)) {
+		up(&this_usbduxsub->sem);
+		return -ENODEV;
+	}
+
+	/* The insn data is a mask in data[0] and the new data
+	 * in data[1], each channel cooresponding to a bit. */
+	s->state &= ~data[0];
+	s->state |= data[0] & data[1];
+	this_usbduxsub->dux_commands[1] = s->io_bits;
+	this_usbduxsub->dux_commands[2] = s->state;
+
+	/* This command also tells the firmware to return */
+	/* the digital input lines */
+	err = send_dux_commands(this_usbduxsub, SENDDIOBITSCOMMAND);
+	if (err < 0) {
+		up(&this_usbduxsub->sem);
+		return err;
+	}
+	err = receive_dux_commands(this_usbduxsub, SENDDIOBITSCOMMAND);
+	if (err < 0) {
+		up(&this_usbduxsub->sem);
+		return err;
+	}
+
+	data[1] = le16_to_cpu(this_usbduxsub->insnBuffer[1]);
+	up(&this_usbduxsub->sem);
+	return 2;
+}
+
+/* reads the 4 counters, only two are used just now */
+static int usbdux_counter_read(comedi_device *dev, comedi_subdevice *s,
+			       comedi_insn *insn, lsampl_t *data)
+{
+	struct usbduxsub *this_usbduxsub = dev->private;
+	int chan = insn->chanspec;
+	int err;
+
+	if (!this_usbduxsub)
+		return -EFAULT;
+
+	down(&this_usbduxsub->sem);
+
+	if (!(this_usbduxsub->probed)) {
+		up(&this_usbduxsub->sem);
+		return -ENODEV;
+	}
+
+	err = send_dux_commands(this_usbduxsub, READCOUNTERCOMMAND);
+	if (err < 0) {
+		up(&this_usbduxsub->sem);
+		return err;
+	}
+
+	err = receive_dux_commands(this_usbduxsub, READCOUNTERCOMMAND);
+	if (err < 0) {
+		up(&this_usbduxsub->sem);
+		return err;
+	}
+
+	data[0] = le16_to_cpu(this_usbduxsub->insnBuffer[chan + 1]);
+	up(&this_usbduxsub->sem);
+	return 1;
+}
+
+static int usbdux_counter_write(comedi_device *dev, comedi_subdevice *s,
+				comedi_insn *insn, lsampl_t *data)
+{
+	struct usbduxsub *this_usbduxsub = dev->private;
+	int err;
+
+	if (!this_usbduxsub)
+		return -EFAULT;
+
+	down(&this_usbduxsub->sem);
+
+	if (!(this_usbduxsub->probed)) {
+		up(&this_usbduxsub->sem);
+		return -ENODEV;
+	}
+
+	this_usbduxsub->dux_commands[1] = insn->chanspec;
+	*((int16_t *) (this_usbduxsub->dux_commands + 2)) = cpu_to_le16(*data);
+
+	err = send_dux_commands(this_usbduxsub, WRITECOUNTERCOMMAND);
+	if (err < 0) {
+		up(&this_usbduxsub->sem);
+		return err;
+	}
+
+	up(&this_usbduxsub->sem);
+
+	return 1;
+}
+
+static int usbdux_counter_config(comedi_device *dev, comedi_subdevice *s,
+				 comedi_insn *insn, lsampl_t *data)
+{
+	/* nothing to do so far */
+	return 2;
+}
+
+/***********************************/
+/* PWM */
+
+static int usbduxsub_unlink_PwmURBs(struct usbduxsub *usbduxsub_tmp)
+{
+	int err = 0;
+
+	if (usbduxsub_tmp && usbduxsub_tmp->urbPwm) {
+		if (usbduxsub_tmp->urbPwm)
+			usb_kill_urb(usbduxsub_tmp->urbPwm);
+		dev_dbg(&usbduxsub_tmp->interface->dev,
+			"comedi: unlinked PwmURB: res=%d\n", err);
+	}
+	return err;
+}
+
+/* This cancels a running acquisition operation
+ * in any context.
+ */
+static int usbdux_pwm_stop(struct usbduxsub *this_usbduxsub, int do_unlink)
+{
+	int ret = 0;
+
+	if (!this_usbduxsub)
+		return -EFAULT;
+
+	dev_dbg(&this_usbduxsub->interface->dev, "comedi: %s\n", __func__);
+	if (do_unlink)
+		ret = usbduxsub_unlink_PwmURBs(this_usbduxsub);
+
+
+	this_usbduxsub->pwm_cmd_running = 0;
+
+	return ret;
+}
+
+/* force unlink - is called by comedi */
+static int usbdux_pwm_cancel(comedi_device *dev, comedi_subdevice *s)
+{
+	struct usbduxsub *this_usbduxsub = dev->private;
+	int res = 0;
+
+	/* unlink only if it is really running */
+	res = usbdux_pwm_stop(this_usbduxsub, this_usbduxsub->pwm_cmd_running);
+
+	dev_dbg(&this_usbduxsub->interface->dev,
+		"comedi %d: sending pwm off command to the usb device.\n",
+		dev->minor);
+	res = send_dux_commands(this_usbduxsub, SENDPWMOFF);
+	if (res < 0)
+		return res;
+
+	return res;
+}
+
+static void usbduxsub_pwm_irq(struct urb *urb)
+{
+	int ret;
+	struct usbduxsub *this_usbduxsub;
+	comedi_device *this_comedidev;
+	comedi_subdevice *s;
+
+	/* printk(KERN_DEBUG "PWM: IRQ\n"); */
+
+	/* the context variable points to the subdevice */
+	this_comedidev = urb->context;
+	/* the private structure of the subdevice is struct usbduxsub */
+	this_usbduxsub = this_comedidev->private;
+
+	s = this_comedidev->subdevices + SUBDEV_DA;
+
+	switch (urb->status) {
+	case 0:
+		/* success */
+		break;
+
+	case -ECONNRESET:
+	case -ENOENT:
+	case -ESHUTDOWN:
+	case -ECONNABORTED:
+		/*
+		 * after an unlink command, unplug, ... etc
+		 * no unlink needed here. Already shutting down.
+		 */
+		if (this_usbduxsub->pwm_cmd_running)
+			usbdux_pwm_stop(this_usbduxsub, 0);
+
+		return;
+
+	default:
+		/* a real error */
+		if (this_usbduxsub->pwm_cmd_running) {
+			dev_err(&this_usbduxsub->interface->dev,
+				"comedi_: Non-zero urb status received in "
+				"pwm intr context: %d\n", urb->status);
+			usbdux_pwm_stop(this_usbduxsub, 0);
+		}
+		return;
+	}
+
+	/* are we actually running? */
+	if (!(this_usbduxsub->pwm_cmd_running))
+		return;
+
+	urb->transfer_buffer_length = this_usbduxsub->sizePwmBuf;
+	urb->dev = this_usbduxsub->usbdev;
+	urb->status = 0;
+	if (this_usbduxsub->pwm_cmd_running) {
+		ret = usb_submit_urb(urb, GFP_ATOMIC);
+		if (ret < 0) {
+			dev_err(&this_usbduxsub->interface->dev,
+				"comedi_: pwm urb resubm failed in int-cont. "
+				"ret=%d", ret);
+			if (ret == EL2NSYNC)
+				dev_err(&this_usbduxsub->interface->dev,
+					"buggy USB host controller or bug in "
+					"IRQ handling!\n");
+
+			/* don't do an unlink here */
+			usbdux_pwm_stop(this_usbduxsub, 0);
+		}
+	}
+}
+
+static int usbduxsub_submit_PwmURBs(struct usbduxsub *usbduxsub)
+{
+	int errFlag;
+
+	if (!usbduxsub)
+		return -EFAULT;
+
+	dev_dbg(&usbduxsub->interface->dev, "comedi_: submitting pwm-urb\n");
+
+	/* in case of a resubmission after an unlink... */
+	usb_fill_bulk_urb(usbduxsub->urbPwm,
+		usbduxsub->usbdev,
+		usb_sndbulkpipe(usbduxsub->usbdev, PWM_EP),
+		usbduxsub->urbPwm->transfer_buffer,
+		usbduxsub->sizePwmBuf, usbduxsub_pwm_irq, usbduxsub->comedidev);
+
+	errFlag = usb_submit_urb(usbduxsub->urbPwm, GFP_ATOMIC);
+	if (errFlag) {
+		dev_err(&usbduxsub->interface->dev,
+			"comedi_: usbdux: pwm: usb_submit_urb error %d\n",
+			errFlag);
+		return errFlag;
+	}
+	return 0;
+}
+
+static int usbdux_pwm_period(comedi_device *dev, comedi_subdevice *s,
+			     lsampl_t period)
+{
+	struct usbduxsub *this_usbduxsub = dev->private;
+	int fx2delay = 255;
+
+	if (period < MIN_PWM_PERIOD) {
+		dev_err(&this_usbduxsub->interface->dev,
+			"comedi%d: illegal period setting for pwm.\n",
+			dev->minor);
+		return -EAGAIN;
+	} else {
+		fx2delay = period / ((int)(6*512*(1.0/0.033))) - 6;
+		if (fx2delay > 255) {
+			dev_err(&this_usbduxsub->interface->dev,
+				"comedi%d: period %d for pwm is too low.\n",
+			       dev->minor, period);
+			return -EAGAIN;
+		}
+	}
+	this_usbduxsub->pwmDelay = fx2delay;
+	this_usbduxsub->pwmPeriod = period;
+	dev_dbg(&this_usbduxsub->interface->dev, "%s: frequ=%d, period=%d\n",
+		__func__, period, fx2delay);
+	return 0;
+}
+
+/* is called from insn so there's no need to do all the sanity checks */
+static int usbdux_pwm_start(comedi_device *dev, comedi_subdevice *s)
+{
+	int ret, i;
+	struct usbduxsub *this_usbduxsub = dev->private;
+
+	dev_dbg(&this_usbduxsub->interface->dev, "comedi%d: %s\n",
+		dev->minor, __func__);
+
+	if (this_usbduxsub->pwm_cmd_running) {
+		/* already running */
+		return 0;
+	}
+
+	this_usbduxsub->dux_commands[1] = ((int8_t) this_usbduxsub->pwmDelay);
+	ret = send_dux_commands(this_usbduxsub, SENDPWMON);
+	if (ret < 0)
+		return ret;
+
+	/* initalise the buffer */
+	for (i = 0; i < this_usbduxsub->sizePwmBuf; i++)
+		((char *)(this_usbduxsub->urbPwm->transfer_buffer))[i] = 0;
+
+	this_usbduxsub->pwm_cmd_running = 1;
+	ret = usbduxsub_submit_PwmURBs(this_usbduxsub);
+	if (ret < 0) {
+		this_usbduxsub->pwm_cmd_running = 0;
+		return ret;
+	}
+	return 0;
+}
+
+/* generates the bit pattern for PWM with the optional sign bit */
+static int usbdux_pwm_pattern(comedi_device *dev, comedi_subdevice *s,
+			      int channel, lsampl_t value, lsampl_t sign)
+{
+	struct usbduxsub *this_usbduxsub = dev->private;
+	int i, szbuf;
+	char *pBuf;
+	char pwm_mask;
+	char sgn_mask;
+	char c;
+
+	if (!this_usbduxsub)
+		return -EFAULT;
+
+	/* this is the DIO bit which carries the PWM data */
+	pwm_mask = (1 << channel);
+	/* this is the DIO bit which carries the optional direction bit */
+	sgn_mask = (16 << channel);
+	/* this is the buffer which will be filled with the with bit */
+	/* pattern for one period */
+	szbuf = this_usbduxsub->sizePwmBuf;
+	pBuf = (char *)(this_usbduxsub->urbPwm->transfer_buffer);
+	for (i = 0; i < szbuf; i++) {
+		c = *pBuf;
+		/* reset bits */
+		c = c & (~pwm_mask);
+		/* set the bit as long as the index is lower than the value */
+		if (i < value)
+			c = c | pwm_mask;
+		/* set the optional sign bit for a relay */
+		if (!sign) {
+			/* positive value */
+			c = c & (~sgn_mask);
+		} else {
+			/* negative value */
+			c = c | sgn_mask;
+		}
+		*(pBuf++) = c;
+	}
+	return 1;
+}
+
+static int usbdux_pwm_write(comedi_device *dev, comedi_subdevice *s,
+			    comedi_insn *insn, lsampl_t *data)
+{
+	struct usbduxsub *this_usbduxsub = dev->private;
+
+	if (!this_usbduxsub)
+		return -EFAULT;
+
+	if ((insn->n) != 1) {
+		/*
+		 * doesn't make sense to have more than one value here because
+		 * it would just overwrite the PWM buffer a couple of times
+		 */
+		return -EINVAL;
+	}
+
+	/*
+	 * the sign is set via a special INSN only, this gives us 8 bits for
+	 * normal operation
+	 * relay sign 0 by default
+	 */
+	return usbdux_pwm_pattern(dev, s, CR_CHAN(insn->chanspec),
+				  data[0], 0);
+}
+
+static int usbdux_pwm_read(comedi_device *x1, comedi_subdevice *x2,
+			   comedi_insn *x3, lsampl_t *x4)
+{
+	/* not needed */
+	return -EINVAL;
+};
+
+/* switches on/off PWM */
+static int usbdux_pwm_config(comedi_device *dev, comedi_subdevice *s,
+			     comedi_insn *insn, lsampl_t *data)
+{
+	struct usbduxsub *this_usbduxsub = dev->private;
+	switch (data[0]) {
+	case INSN_CONFIG_ARM:
+		/* switch it on */
+		dev_dbg(&this_usbduxsub->interface->dev,
+			"comedi%d: %s: pwm on\n", dev->minor, __func__);
+		/*
+		 * if not zero the PWM is limited to a certain time which is
+		 * not supported here
+		 */
+		if (data[1] != 0)
+			return -EINVAL;
+		return usbdux_pwm_start(dev, s);
+	case INSN_CONFIG_DISARM:
+		dev_dbg(&this_usbduxsub->interface->dev,
+			"comedi%d: %s: pwm off\n", dev->minor, __func__);
+		return usbdux_pwm_cancel(dev, s);
+	case INSN_CONFIG_GET_PWM_STATUS:
+		/*
+		 * to check if the USB transmission has failed or in case PWM
+		 * was limited to n cycles to check if it has terminated
+		 */
+		data[1] = this_usbduxsub->pwm_cmd_running;
+		return 0;
+	case INSN_CONFIG_PWM_SET_PERIOD:
+		dev_dbg(&this_usbduxsub->interface->dev,
+			"comedi%d: %s: setting period\n", dev->minor, __func__);
+		return usbdux_pwm_period(dev, s, data[1]);
+	case INSN_CONFIG_PWM_GET_PERIOD:
+		data[1] = this_usbduxsub->pwmPeriod;
+		return 0;
+	case INSN_CONFIG_PWM_SET_H_BRIDGE:
+		/* value in the first byte and the sign in the second for a
+		   relay */
+		return usbdux_pwm_pattern(dev, s,
+					  /* the channel number */
+					  CR_CHAN(insn->chanspec),
+					  /* actual PWM data */
+					  data[1],
+					  /* just a sign */
+					  (data[2] != 0));
+	case INSN_CONFIG_PWM_GET_H_BRIDGE:
+		/* values are not kept in this driver, nothing to return here */
+		return -EINVAL;
+	}
+	return -EINVAL;
+}
+
+/* end of PWM */
+/*****************************************************************/
+
+static void tidy_up(struct usbduxsub *usbduxsub_tmp)
+{
+	int i;
+
+	if (!usbduxsub_tmp)
+		return;
+	dev_dbg(&usbduxsub_tmp->interface->dev, "comedi_: tiding up\n");
+
+	/* shows the usb subsystem that the driver is down */
+	if (usbduxsub_tmp->interface)
+		usb_set_intfdata(usbduxsub_tmp->interface, NULL);
+
+	usbduxsub_tmp->probed = 0;
+
+	if (usbduxsub_tmp->urbIn) {
+		if (usbduxsub_tmp->ai_cmd_running) {
+			usbduxsub_tmp->ai_cmd_running = 0;
+			usbduxsub_unlink_InURBs(usbduxsub_tmp);
+		}
+		for (i = 0; i < usbduxsub_tmp->numOfInBuffers; i++) {
+			kfree(usbduxsub_tmp->urbIn[i]->transfer_buffer);
+			usbduxsub_tmp->urbIn[i]->transfer_buffer = NULL;
+			usb_kill_urb(usbduxsub_tmp->urbIn[i]);
+			usb_free_urb(usbduxsub_tmp->urbIn[i]);
+			usbduxsub_tmp->urbIn[i] = NULL;
+		}
+		kfree(usbduxsub_tmp->urbIn);
+		usbduxsub_tmp->urbIn = NULL;
+	}
+	if (usbduxsub_tmp->urbOut) {
+		if (usbduxsub_tmp->ao_cmd_running) {
+			usbduxsub_tmp->ao_cmd_running = 0;
+			usbduxsub_unlink_OutURBs(usbduxsub_tmp);
+		}
+		for (i = 0; i < usbduxsub_tmp->numOfOutBuffers; i++) {
+			if (usbduxsub_tmp->urbOut[i]->transfer_buffer) {
+				kfree(usbduxsub_tmp->urbOut[i]->
+					transfer_buffer);
+				usbduxsub_tmp->urbOut[i]->transfer_buffer =
+					NULL;
+			}
+			if (usbduxsub_tmp->urbOut[i]) {
+				usb_kill_urb(usbduxsub_tmp->urbOut[i]);
+				usb_free_urb(usbduxsub_tmp->urbOut[i]);
+				usbduxsub_tmp->urbOut[i] = NULL;
+			}
+		}
+		kfree(usbduxsub_tmp->urbOut);
+		usbduxsub_tmp->urbOut = NULL;
+	}
+	if (usbduxsub_tmp->urbPwm) {
+		if (usbduxsub_tmp->pwm_cmd_running) {
+			usbduxsub_tmp->pwm_cmd_running = 0;
+			usbduxsub_unlink_PwmURBs(usbduxsub_tmp);
+		}
+		kfree(usbduxsub_tmp->urbPwm->transfer_buffer);
+		usbduxsub_tmp->urbPwm->transfer_buffer = NULL;
+		usb_kill_urb(usbduxsub_tmp->urbPwm);
+		usb_free_urb(usbduxsub_tmp->urbPwm);
+		usbduxsub_tmp->urbPwm = NULL;
+	}
+	kfree(usbduxsub_tmp->inBuffer);
+	usbduxsub_tmp->inBuffer = NULL;
+	kfree(usbduxsub_tmp->insnBuffer);
+	usbduxsub_tmp->insnBuffer = NULL;
+	kfree(usbduxsub_tmp->inBuffer);
+	usbduxsub_tmp->inBuffer = NULL;
+	kfree(usbduxsub_tmp->dac_commands);
+	usbduxsub_tmp->dac_commands = NULL;
+	kfree(usbduxsub_tmp->dux_commands);
+	usbduxsub_tmp->dux_commands = NULL;
+	usbduxsub_tmp->ai_cmd_running = 0;
+	usbduxsub_tmp->ao_cmd_running = 0;
+	usbduxsub_tmp->pwm_cmd_running = 0;
+}
+
+static unsigned hex2unsigned(char *h)
+{
+	unsigned hi, lo;
+
+	if (h[0] > '9')
+		hi = h[0] - 'A' + 0x0a;
+	else
+		hi = h[0] - '0';
+
+	if (h[1] > '9')
+		lo = h[1] - 'A' + 0x0a;
+	else
+		lo = h[1] - '0';
+
+	return hi * 0x10 + lo;
+}
+
+/* for FX2 */
+#define FIRMWARE_MAX_LEN 0x2000
+
+/* taken from David Brownell's fxload and adjusted for this driver */
+static int read_firmware(struct usbduxsub *usbduxsub, void *firmwarePtr,
+			 long size)
+{
+	struct device *dev = &usbduxsub->interface->dev;
+	int i = 0;
+	unsigned char *fp = (char *)firmwarePtr;
+	unsigned char *firmwareBinary = NULL;
+	int res = 0;
+	int maxAddr = 0;
+
+	firmwareBinary = kzalloc(FIRMWARE_MAX_LEN, GFP_KERNEL);
+	if (!firmwareBinary) {
+		dev_err(dev, "comedi_: mem alloc for firmware failed\n");
+		return -ENOMEM;
+	}
+
+	for (;;) {
+		char buf[256], *cp;
+		char type;
+		int len;
+		int idx, off;
+		int j = 0;
+
+		/* get one line */
+		while ((i < size) && (fp[i] != 13) && (fp[i] != 10)) {
+			buf[j] = fp[i];
+			i++;
+			j++;
+			if (j >= sizeof(buf)) {
+				dev_err(dev, "comedi_: bogus firmware file!\n");
+				return -1;
+			}
+		}
+		/* get rid of LF/CR/... */
+		while ((i < size) && ((fp[i] == 13) || (fp[i] == 10)
+				|| (fp[i] == 0))) {
+			i++;
+		}
+
+		buf[j] = 0;
+		/* dev_dbg(dev, "comedi_: buf=%s\n", buf); */
+
+		/*
+		 * EXTENSION:
+		 * "# comment-till-end-of-line", for copyrights etc
+		 */
+		if (buf[0] == '#')
+			continue;
+
+		if (buf[0] != ':') {
+			dev_err(dev, "comedi_: upload: not an ihex record: %s",
+				buf);
+			return -EFAULT;
+		}
+
+		/* Read the length field (up to 16 bytes) */
+		len = hex2unsigned(buf + 1);
+
+		/* Read the target offset */
+		off = (hex2unsigned(buf + 3) * 0x0100) + hex2unsigned(buf + 5);
+
+		if ((off + len) > maxAddr)
+			maxAddr = off + len;
+
+
+		if (maxAddr >= FIRMWARE_MAX_LEN) {
+			dev_err(dev, "comedi_: firmware upload goes "
+				"beyond FX2 RAM boundaries.\n");
+			return -EFAULT;
+		}
+		/* dev_dbg(dev, "comedi_: off=%x, len=%x:\n", off, len); */
+
+		/* Read the record type */
+		type = hex2unsigned(buf + 7);
+
+		/* If this is an EOF record, then make it so. */
+		if (type == 1)
+			break;
+
+
+		if (type != 0) {
+			dev_err(dev, "comedi_: unsupported record type: %u\n",
+				type);
+			return -EFAULT;
+		}
+
+		for (idx = 0, cp = buf + 9; idx < len; idx += 1, cp += 2) {
+			firmwareBinary[idx + off] = hex2unsigned(cp);
+			/*printk("%02x ",firmwareBinary[idx+off]); */
+		}
+		/*printk("\n"); */
+
+		if (i >= size) {
+			dev_err(dev, "comedi_: unexpected end of hex file\n");
+			break;
+		}
+
+	}
+	res = firmwareUpload(usbduxsub, firmwareBinary, maxAddr + 1);
+	kfree(firmwareBinary);
+	return res;
+}
+
+/* allocate memory for the urbs and initialise them */
+static int usbduxsub_probe(struct usb_interface *uinterf,
+			   const struct usb_device_id *id)
+{
+	struct usb_device *udev = interface_to_usbdev(uinterf);
+	struct device *dev = &uinterf->dev;
+	int i;
+	int index;
+
+	dev_dbg(dev, "comedi_: usbdux_: "
+		"finding a free structure for the usb-device\n");
+
+	down(&start_stop_sem);
+	/* look for a free place in the usbdux array */
+	index = -1;
+	for (i = 0; i < NUMUSBDUX; i++) {
+		if (!(usbduxsub[i].probed)) {
+			index = i;
+			break;
+		}
+	}
+
+	/* no more space */
+	if (index == -1) {
+		dev_err(dev, "Too many usbdux-devices connected.\n");
+		up(&start_stop_sem);
+		return -EMFILE;
+	}
+	dev_dbg(dev, "comedi_: usbdux: "
+		"usbduxsub[%d] is ready to connect to comedi.\n", index);
+
+	init_MUTEX(&(usbduxsub[index].sem));
+	/* save a pointer to the usb device */
+	usbduxsub[index].usbdev = udev;
+
+	/* 2.6: save the interface itself */
+	usbduxsub[index].interface = uinterf;
+	/* get the interface number from the interface */
+	usbduxsub[index].ifnum = uinterf->altsetting->desc.bInterfaceNumber;
+	/* hand the private data over to the usb subsystem */
+	/* will be needed for disconnect */
+	usb_set_intfdata(uinterf, &(usbduxsub[index]));
+
+	dev_dbg(dev, "comedi_: usbdux: ifnum=%d\n", usbduxsub[index].ifnum);
+
+	/* test if it is high speed (USB 2.0) */
+	usbduxsub[index].high_speed =
+		(usbduxsub[index].usbdev->speed == USB_SPEED_HIGH);
+
+	/* create space for the commands of the DA converter */
+	usbduxsub[index].dac_commands = kzalloc(NUMOUTCHANNELS, GFP_KERNEL);
+	if (!usbduxsub[index].dac_commands) {
+		dev_err(dev, "comedi_: usbdux: "
+			"error alloc space for dac commands\n");
+		tidy_up(&(usbduxsub[index]));
+		up(&start_stop_sem);
+		return -ENOMEM;
+	}
+	/* create space for the commands going to the usb device */
+	usbduxsub[index].dux_commands = kzalloc(SIZEOFDUXBUFFER, GFP_KERNEL);
+	if (!usbduxsub[index].dux_commands) {
+		dev_err(dev, "comedi_: usbdux: "
+			"error alloc space for dac commands\n");
+		tidy_up(&(usbduxsub[index]));
+		up(&start_stop_sem);
+		return -ENOMEM;
+	}
+	/* create space for the in buffer and set it to zero */
+	usbduxsub[index].inBuffer = kzalloc(SIZEINBUF, GFP_KERNEL);
+	if (!(usbduxsub[index].inBuffer)) {
+		dev_err(dev, "comedi_: usbdux: "
+			"could not alloc space for inBuffer\n");
+		tidy_up(&(usbduxsub[index]));
+		up(&start_stop_sem);
+		return -ENOMEM;
+	}
+	/* create space of the instruction buffer */
+	usbduxsub[index].insnBuffer = kzalloc(SIZEINSNBUF, GFP_KERNEL);
+	if (!(usbduxsub[index].insnBuffer)) {
+		dev_err(dev, "comedi_: usbdux: "
+			"could not alloc space for insnBuffer\n");
+		tidy_up(&(usbduxsub[index]));
+		up(&start_stop_sem);
+		return -ENOMEM;
+	}
+	/* create space for the outbuffer */
+	usbduxsub[index].outBuffer = kzalloc(SIZEOUTBUF, GFP_KERNEL);
+	if (!(usbduxsub[index].outBuffer)) {
+		dev_err(dev, "comedi_: usbdux: "
+			"could not alloc space for outBuffer\n");
+		tidy_up(&(usbduxsub[index]));
+		up(&start_stop_sem);
+		return -ENOMEM;
+	}
+	/* setting to alternate setting 3: enabling iso ep and bulk ep. */
+	i = usb_set_interface(usbduxsub[index].usbdev,
+			      usbduxsub[index].ifnum, 3);
+	if (i < 0) {
+		dev_err(dev, "comedi_: usbdux%d: "
+			"could not set alternate setting 3 in high speed.\n",
+			index);
+		tidy_up(&(usbduxsub[index]));
+		up(&start_stop_sem);
+		return -ENODEV;
+	}
+	if (usbduxsub[index].high_speed)
+		usbduxsub[index].numOfInBuffers = NUMOFINBUFFERSHIGH;
+	else
+		usbduxsub[index].numOfInBuffers = NUMOFINBUFFERSFULL;
+
+	usbduxsub[index].urbIn =
+		kzalloc(sizeof(struct urb *) * usbduxsub[index].numOfInBuffers,
+		GFP_KERNEL);
+	if (!(usbduxsub[index].urbIn)) {
+		dev_err(dev, "comedi_: usbdux: Could not alloc. urbIn array\n");
+		tidy_up(&(usbduxsub[index]));
+		up(&start_stop_sem);
+		return -ENOMEM;
+	}
+	for (i = 0; i < usbduxsub[index].numOfInBuffers; i++) {
+		/* one frame: 1ms */
+		usbduxsub[index].urbIn[i] = usb_alloc_urb(1, GFP_KERNEL);
+		if (usbduxsub[index].urbIn[i] == NULL) {
+			dev_err(dev, "comedi_: usbdux%d: "
+				"Could not alloc. urb(%d)\n", index, i);
+			tidy_up(&(usbduxsub[index]));
+			up(&start_stop_sem);
+			return -ENOMEM;
+		}
+		usbduxsub[index].urbIn[i]->dev = usbduxsub[index].usbdev;
+		/* will be filled later with a pointer to the comedi-device */
+		/* and ONLY then the urb should be submitted */
+		usbduxsub[index].urbIn[i]->context = NULL;
+		usbduxsub[index].urbIn[i]->pipe =
+			usb_rcvisocpipe(usbduxsub[index].usbdev, ISOINEP);
+		usbduxsub[index].urbIn[i]->transfer_flags = URB_ISO_ASAP;
+		usbduxsub[index].urbIn[i]->transfer_buffer =
+			kzalloc(SIZEINBUF, GFP_KERNEL);
+		if (!(usbduxsub[index].urbIn[i]->transfer_buffer)) {
+			dev_err(dev, "comedi_: usbdux%d: "
+				"could not alloc. transb.\n", index);
+			tidy_up(&(usbduxsub[index]));
+			up(&start_stop_sem);
+			return -ENOMEM;
+		}
+		usbduxsub[index].urbIn[i]->complete = usbduxsub_ai_IsocIrq;
+		usbduxsub[index].urbIn[i]->number_of_packets = 1;
+		usbduxsub[index].urbIn[i]->transfer_buffer_length = SIZEINBUF;
+		usbduxsub[index].urbIn[i]->iso_frame_desc[0].offset = 0;
+		usbduxsub[index].urbIn[i]->iso_frame_desc[0].length = SIZEINBUF;
+	}
+
+	/* out */
+	if (usbduxsub[index].high_speed)
+		usbduxsub[index].numOfOutBuffers = NUMOFOUTBUFFERSHIGH;
+	else
+		usbduxsub[index].numOfOutBuffers = NUMOFOUTBUFFERSFULL;
+
+	usbduxsub[index].urbOut =
+		kzalloc(sizeof(struct urb *) * usbduxsub[index].numOfOutBuffers,
+		GFP_KERNEL);
+	if (!(usbduxsub[index].urbOut)) {
+		dev_err(dev, "comedi_: usbdux: "
+			"Could not alloc. urbOut array\n");
+		tidy_up(&(usbduxsub[index]));
+		up(&start_stop_sem);
+		return -ENOMEM;
+	}
+	for (i = 0; i < usbduxsub[index].numOfOutBuffers; i++) {
+		/* one frame: 1ms */
+		usbduxsub[index].urbOut[i] = usb_alloc_urb(1, GFP_KERNEL);
+		if (usbduxsub[index].urbOut[i] == NULL) {
+			dev_err(dev, "comedi_: usbdux%d: "
+				"Could not alloc. urb(%d)\n", index, i);
+			tidy_up(&(usbduxsub[index]));
+			up(&start_stop_sem);
+			return -ENOMEM;
+		}
+		usbduxsub[index].urbOut[i]->dev = usbduxsub[index].usbdev;
+		/* will be filled later with a pointer to the comedi-device */
+		/* and ONLY then the urb should be submitted */
+		usbduxsub[index].urbOut[i]->context = NULL;
+		usbduxsub[index].urbOut[i]->pipe =
+			usb_sndisocpipe(usbduxsub[index].usbdev, ISOOUTEP);
+		usbduxsub[index].urbOut[i]->transfer_flags = URB_ISO_ASAP;
+		usbduxsub[index].urbOut[i]->transfer_buffer =
+			kzalloc(SIZEOUTBUF, GFP_KERNEL);
+		if (!(usbduxsub[index].urbOut[i]->transfer_buffer)) {
+			dev_err(dev, "comedi_: usbdux%d: "
+				"could not alloc. transb.\n", index);
+			tidy_up(&(usbduxsub[index]));
+			up(&start_stop_sem);
+			return -ENOMEM;
+		}
+		usbduxsub[index].urbOut[i]->complete = usbduxsub_ao_IsocIrq;
+		usbduxsub[index].urbOut[i]->number_of_packets = 1;
+		usbduxsub[index].urbOut[i]->transfer_buffer_length = SIZEOUTBUF;
+		usbduxsub[index].urbOut[i]->iso_frame_desc[0].offset = 0;
+		usbduxsub[index].urbOut[i]->iso_frame_desc[0].length =
+			SIZEOUTBUF;
+		if (usbduxsub[index].high_speed) {
+			/* uframes */
+			usbduxsub[index].urbOut[i]->interval = 8;
+		} else {
+			/* frames */
+			usbduxsub[index].urbOut[i]->interval = 1;
+		}
+	}
+
+	/* pwm */
+	if (usbduxsub[index].high_speed) {
+		/* max bulk ep size in high speed */
+		usbduxsub[index].sizePwmBuf = 512;
+		usbduxsub[index].urbPwm = usb_alloc_urb(0, GFP_KERNEL);
+		if (usbduxsub[index].urbPwm == NULL) {
+			dev_err(dev, "comedi_: usbdux%d: "
+				"Could not alloc. pwm urb\n", index);
+			tidy_up(&(usbduxsub[index]));
+			up(&start_stop_sem);
+			return -ENOMEM;
+		}
+		usbduxsub[index].urbPwm->transfer_buffer =
+			kzalloc(usbduxsub[index].sizePwmBuf, GFP_KERNEL);
+		if (!(usbduxsub[index].urbPwm->transfer_buffer)) {
+			dev_err(dev, "comedi_: usbdux%d: "
+				"could not alloc. transb. for pwm\n", index);
+			tidy_up(&(usbduxsub[index]));
+			up(&start_stop_sem);
+			return -ENOMEM;
+		}
+	} else {
+		usbduxsub[index].urbPwm = NULL;
+		usbduxsub[index].sizePwmBuf = 0;
+	}
+
+	usbduxsub[index].ai_cmd_running = 0;
+	usbduxsub[index].ao_cmd_running = 0;
+	usbduxsub[index].pwm_cmd_running = 0;
+
+	/* we've reached the bottom of the function */
+	usbduxsub[index].probed = 1;
+	up(&start_stop_sem);
+	dev_info(dev, "comedi_: usbdux%d "
+		 "has been successfully initialised.\n", index);
+	/* success */
+	return 0;
+}
+
+static void usbduxsub_disconnect(struct usb_interface *intf)
+{
+	struct usbduxsub *usbduxsub_tmp = usb_get_intfdata(intf);
+	struct usb_device *udev = interface_to_usbdev(intf);
+
+	if (!usbduxsub_tmp) {
+		dev_err(&intf->dev,
+			"comedi_: disconnect called with null pointer.\n");
+		return;
+	}
+	if (usbduxsub_tmp->usbdev != udev) {
+		dev_err(&intf->dev,
+			"comedi_: BUG! called with wrong ptr!!!\n");
+		return;
+	}
+	down(&start_stop_sem);
+	down(&usbduxsub_tmp->sem);
+	tidy_up(usbduxsub_tmp);
+	up(&usbduxsub_tmp->sem);
+	up(&start_stop_sem);
+	dev_dbg(&intf->dev, "comedi_: disconnected from the usb\n");
+}
+
+/* is called when comedi-config is called */
+static int usbdux_attach(comedi_device *dev, comedi_devconfig *it)
+{
+	int ret;
+	int index;
+	int i;
+	struct usbduxsub *udev;
+
+	comedi_subdevice *s = NULL;
+	dev->private = NULL;
+
+	down(&start_stop_sem);
+	/* find a valid device which has been detected by the probe function of
+	 * the usb */
+	index = -1;
+	for (i = 0; i < NUMUSBDUX; i++) {
+		if ((usbduxsub[i].probed) && (!usbduxsub[i].attached)) {
+			index = i;
+			break;
+		}
+	}
+
+	if (index < 0) {
+		printk(KERN_ERR "comedi%d: usbdux: error: attach failed, no "
+		       "usbdux devs connected to the usb bus.\n", dev->minor);
+		up(&start_stop_sem);
+		return -ENODEV;
+	}
+
+	udev = &usbduxsub[index];
+	down(&udev->sem);
+	/* pointer back to the corresponding comedi device */
+	udev->comedidev = dev;
+
+	/* trying to upload the firmware into the chip */
+	if (comedi_aux_data(it->options, 0) &&
+		it->options[COMEDI_DEVCONF_AUX_DATA_LENGTH]) {
+		read_firmware(udev, comedi_aux_data(it->options, 0),
+			      it->options[COMEDI_DEVCONF_AUX_DATA_LENGTH]);
+	}
+
+	dev->board_name = BOARDNAME;
+
+	/* set number of subdevices */
+	if (udev->high_speed) {
+		/* with pwm */
+		dev->n_subdevices = 5;
+	} else {
+		/* without pwm */
+		dev->n_subdevices = 4;
+	}
+
+	/* allocate space for the subdevices */
+	ret = alloc_subdevices(dev, dev->n_subdevices);
+	if (ret < 0) {
+		dev_err(&udev->interface->dev,
+			"comedi%d: error alloc space for subdev\n", dev->minor);
+		up(&start_stop_sem);
+		return ret;
+	}
+
+	dev_info(&udev->interface->dev,
+		"comedi%d: usb-device %d is attached to comedi.\n",
+		dev->minor, index);
+	/* private structure is also simply the usb-structure */
+	dev->private = udev;
+
+	/* the first subdevice is the A/D converter */
+	s = dev->subdevices + SUBDEV_AD;
+	/* the URBs get the comedi subdevice */
+	/* which is responsible for reading */
+	/* this is the subdevice which reads data */
+	dev->read_subdev = s;
+	/* the subdevice receives as private structure the */
+	/* usb-structure */
+	s->private = NULL;
+	/* analog input */
+	s->type = COMEDI_SUBD_AI;
+	/* readable and ref is to ground */
+	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_CMD_READ;
+	/* 8 channels */
+	s->n_chan = 8;
+	/* length of the channellist */
+	s->len_chanlist = 8;
+	/* callback functions */
+	s->insn_read = usbdux_ai_insn_read;
+	s->do_cmdtest = usbdux_ai_cmdtest;
+	s->do_cmd = usbdux_ai_cmd;
+	s->cancel = usbdux_ai_cancel;
+	/* max value from the A/D converter (12bit) */
+	s->maxdata = 0xfff;
+	/* range table to convert to physical units */
+	s->range_table = (&range_usbdux_ai_range);
+
+	/* analog out */
+	s = dev->subdevices + SUBDEV_DA;
+	/* analog out */
+	s->type = COMEDI_SUBD_AO;
+	/* backward pointer */
+	dev->write_subdev = s;
+	/* the subdevice receives as private structure the */
+	/* usb-structure */
+	s->private = NULL;
+	/* are writable */
+	s->subdev_flags = SDF_WRITABLE | SDF_GROUND | SDF_CMD_WRITE;
+	/* 4 channels */
+	s->n_chan = 4;
+	/* length of the channellist */
+	s->len_chanlist = 4;
+	/* 12 bit resolution */
+	s->maxdata = 0x0fff;
+	/* bipolar range */
+	s->range_table = (&range_usbdux_ao_range);
+	/* callback */
+	s->do_cmdtest = usbdux_ao_cmdtest;
+	s->do_cmd = usbdux_ao_cmd;
+	s->cancel = usbdux_ao_cancel;
+	s->insn_read = usbdux_ao_insn_read;
+	s->insn_write = usbdux_ao_insn_write;
+
+	/* digital I/O */
+	s = dev->subdevices + SUBDEV_DIO;
+	s->type = COMEDI_SUBD_DIO;
+	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
+	s->n_chan = 8;
+	s->maxdata = 1;
+	s->range_table = (&range_digital);
+	s->insn_bits = usbdux_dio_insn_bits;
+	s->insn_config = usbdux_dio_insn_config;
+	/* we don't use it */
+	s->private = NULL;
+
+	/* counter */
+	s = dev->subdevices + SUBDEV_COUNTER;
+	s->type = COMEDI_SUBD_COUNTER;
+	s->subdev_flags = SDF_WRITABLE | SDF_READABLE;
+	s->n_chan = 4;
+	s->maxdata = 0xFFFF;
+	s->insn_read = usbdux_counter_read;
+	s->insn_write = usbdux_counter_write;
+	s->insn_config = usbdux_counter_config;
+
+	if (udev->high_speed) {
+		/* timer / pwm */
+		s = dev->subdevices + SUBDEV_PWM;
+		s->type = COMEDI_SUBD_PWM;
+		s->subdev_flags = SDF_WRITABLE | SDF_PWM_HBRIDGE;
+		s->n_chan = 8;
+		/* this defines the max duty cycle resolution */
+		s->maxdata = udev->sizePwmBuf;
+		s->insn_write = usbdux_pwm_write;
+		s->insn_read = usbdux_pwm_read;
+		s->insn_config = usbdux_pwm_config;
+		usbdux_pwm_period(dev, s, PWM_DEFAULT_PERIOD);
+	}
+	/* finally decide that it's attached */
+	udev->attached = 1;
+
+	up(&udev->sem);
+
+	up(&start_stop_sem);
+
+	dev_info(&udev->interface->dev, "comedi%d: attached to usbdux.\n",
+		 dev->minor);
+
+	return 0;
+}
+
+static int usbdux_detach(comedi_device *dev)
+{
+	struct usbduxsub *usbduxsub_tmp;
+
+	if (!dev) {
+		printk(KERN_ERR
+			"comedi?: usbdux: detach without dev variable...\n");
+		return -EFAULT;
+	}
+
+	usbduxsub_tmp = dev->private;
+	if (!usbduxsub_tmp) {
+		printk(KERN_ERR
+			"comedi?: usbdux: detach without ptr to usbduxsub[]\n");
+		return -EFAULT;
+	}
+
+	dev_dbg(&usbduxsub_tmp->interface->dev, "comedi%d: detach usb device\n",
+		dev->minor);
+
+	down(&usbduxsub_tmp->sem);
+	/* Don't allow detach to free the private structure */
+	/* It's one entry of of usbduxsub[] */
+	dev->private = NULL;
+	usbduxsub_tmp->attached = 0;
+	usbduxsub_tmp->comedidev = NULL;
+	dev_dbg(&usbduxsub_tmp->interface->dev,
+		"comedi%d: detach: successfully removed\n", dev->minor);
+	up(&usbduxsub_tmp->sem);
+	return 0;
+}
+
+/* main driver struct */
+static comedi_driver driver_usbdux = {
+      .driver_name =	"usbdux",
+      .module =		THIS_MODULE,
+      .attach =		usbdux_attach,
+      .detach =		usbdux_detach,
+};
+
+static void init_usb_devices(void)
+{
+	int index;
+
+	/* all devices entries are invalid to begin with */
+	/* they will become valid by the probe function */
+	/* and then finally by the attach-function */
+	for (index = 0; index < NUMUSBDUX; index++) {
+		memset(&(usbduxsub[index]), 0x00, sizeof(usbduxsub[index]));
+		init_MUTEX(&(usbduxsub[index].sem));
+	}
+}
+
+/* Table with the USB-devices: just now only testing IDs */
+static struct usb_device_id usbduxsub_table[] = {
+	{USB_DEVICE(0x13d8, 0x0001) },
+	{USB_DEVICE(0x13d8, 0x0002) },
+	{}			/* Terminating entry */
+};
+
+MODULE_DEVICE_TABLE(usb, usbduxsub_table);
+
+/* The usbduxsub-driver */
+static struct usb_driver usbduxsub_driver = {
+      .name =		BOARDNAME,
+      .probe =		usbduxsub_probe,
+      .disconnect =	usbduxsub_disconnect,
+      .id_table =	usbduxsub_table,
+};
+
+/* Can't use the nice macro as I have also to initialise the USB */
+/* subsystem: */
+/* registering the usb-system _and_ the comedi-driver */
+static int init_usbdux(void)
+{
+	printk(KERN_INFO KBUILD_MODNAME ": "
+	       DRIVER_VERSION ":" DRIVER_DESC "\n");
+	init_usb_devices();
+	usb_register(&usbduxsub_driver);
+	comedi_driver_register(&driver_usbdux);
+	return 0;
+}
+
+/* deregistering the comedi driver and the usb-subsystem */
+static void exit_usbdux(void)
+{
+	comedi_driver_unregister(&driver_usbdux);
+	usb_deregister(&usbduxsub_driver);
+}
+
+module_init(init_usbdux);
+module_exit(exit_usbdux);
+
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/usbduxfast.c b/drivers/staging/comedi/drivers/usbduxfast.c
new file mode 100644
index 00000000000..3a00ff0cfc5
--- /dev/null
+++ b/drivers/staging/comedi/drivers/usbduxfast.c
@@ -0,0 +1,1778 @@
+#define DRIVER_VERSION "v0.99a"
+#define DRIVER_AUTHOR "Bernd Porr, BerndPorr@f2s.com"
+#define DRIVER_DESC "USB-DUXfast, BerndPorr@f2s.com"
+/*
+   comedi/drivers/usbduxfast.c
+   Copyright (C) 2004 Bernd Porr, Bernd.Porr@f2s.com
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation; either version 2 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program; if not, write to the Free Software
+   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+*/
+
+/*
+Driver: usbduxfast
+Description: ITL USB-DUXfast
+Devices: [ITL] USB-DUX (usbduxfast.o)
+Author: Bernd Porr <BerndPorr@f2s.com>
+Updated: 04 Dec 2006
+Status: testing
+*/
+
+/*
+ * I must give credit here to Chris Baugher who
+ * wrote the driver for AT-MIO-16d. I used some parts of this
+ * driver. I also must give credits to David Brownell
+ * who supported me with the USB development.
+ *
+ * Bernd Porr
+ *
+ *
+ * Revision history:
+ * 0.9: Dropping the first data packet which seems to be from the last transfer.
+ *      Buffer overflows in the FX2 are handed over to comedi.
+ * 0.92: Dropping now 4 packets. The quad buffer has to be emptied.
+ *       Added insn command basically for testing. Sample rate is 1MHz/16ch=62.5kHz
+ * 0.99: Ian Abbott pointed out a bug which has been corrected. Thanks!
+ * 0.99a: added external trigger.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/input.h>
+#include <linux/usb.h>
+#include <linux/smp_lock.h>
+#include <linux/fcntl.h>
+#include <linux/compiler.h>
+#include "comedi_fc.h"
+#include "../comedidev.h"
+
+// (un)comment this if you want to have debug info.
+//#define CONFIG_COMEDI_DEBUG
+#undef  CONFIG_COMEDI_DEBUG
+
+#define BOARDNAME "usbduxfast"
+
+// timeout for the USB-transfer
+#define EZTIMEOUT 30
+
+// constants for "firmware" upload and download
+#define USBDUXFASTSUB_FIRMWARE 0xA0
+#define VENDOR_DIR_IN  0xC0
+#define VENDOR_DIR_OUT 0x40
+
+// internal adresses of the 8051 processor
+#define USBDUXFASTSUB_CPUCS 0xE600
+
+// max lenghth of the transfer-buffer for software upload
+#define TB_LEN 0x2000
+
+// Input endpoint number
+#define BULKINEP           6
+
+// Endpoint for the A/D channellist: bulk OUT
+#define CHANNELLISTEP     4
+
+// Number of channels
+#define NUMCHANNELS       32
+
+// size of the waveform descriptor
+#define WAVESIZE          0x20
+
+// Size of one A/D value
+#define SIZEADIN          ((sizeof(int16_t)))
+
+// Size of the input-buffer IN BYTES
+#define SIZEINBUF         512
+
+// 16 bytes.
+#define SIZEINSNBUF       512
+
+// Size of the buffer for the dux commands
+#define SIZEOFDUXBUFFER    256	// bytes
+
+// Number of in-URBs which receive the data: min=5
+#define NUMOFINBUFFERSHIGH     10
+
+// Total number of usbduxfast devices
+#define NUMUSBDUXFAST             16
+
+// Number of subdevices
+#define N_SUBDEVICES          1
+
+// Analogue in subdevice
+#define SUBDEV_AD             0
+
+// min delay steps for more than one channel
+// basically when the mux gives up. ;-)
+#define MIN_SAMPLING_PERIOD 9	// steps at 30MHz in the FX2
+
+// Max number of 1/30MHz delay steps:
+#define MAX_SAMPLING_PERIOD 500
+
+// Number of received packets to ignore before we start handing data over to comedi.
+// It's quad buffering and we have to ignore 4 packets.
+#define PACKETS_TO_IGNORE 4
+
+/////////////////////////////////////////////
+// comedi constants
+static const comedi_lrange range_usbduxfast_ai_range = { 2, {
+			BIP_RANGE(0.75),
+			BIP_RANGE(0.5),
+	}
+};
+
+/*
+ * private structure of one subdevice
+ */
+
+// This is the structure which holds all the data of this driver
+// one sub device just now: A/D
+typedef struct {
+	// attached?
+	int attached;
+	// is it associated with a subdevice?
+	int probed;
+	// pointer to the usb-device
+	struct usb_device *usbdev;
+	// BULK-transfer handling: urb
+	struct urb *urbIn;
+	int8_t *transfer_buffer;
+	// input buffer for single insn
+	int16_t *insnBuffer;
+	// interface number
+	int ifnum;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0)
+	// interface structure in 2.6
+	struct usb_interface *interface;
+#endif
+	// comedi device for the interrupt context
+	comedi_device *comedidev;
+	// asynchronous command is running
+	short int ai_cmd_running;
+	// continous aquisition
+	short int ai_continous;
+	// number of samples to aquire
+	long int ai_sample_count;
+	// commands
+	uint8_t *dux_commands;
+	// counter which ignores the first buffers
+	int ignore;
+	struct semaphore sem;
+} usbduxfastsub_t;
+
+// The pointer to the private usb-data of the driver
+// is also the private data for the comedi-device.
+// This has to be global as the usb subsystem needs
+// global variables. The other reason is that this
+// structure must be there _before_ any comedi
+// command is issued. The usb subsystem must be
+// initialised before comedi can access it.
+static usbduxfastsub_t usbduxfastsub[NUMUSBDUXFAST];
+
+static DECLARE_MUTEX(start_stop_sem);
+
+// bulk transfers to usbduxfast
+
+#define SENDADCOMMANDS            0
+#define SENDINITEP6               1
+
+static int send_dux_commands(usbduxfastsub_t * this_usbduxfastsub, int cmd_type)
+{
+	int result, nsent;
+	this_usbduxfastsub->dux_commands[0] = cmd_type;
+#ifdef CONFIG_COMEDI_DEBUG
+	int i;
+	printk("comedi%d: usbduxfast: dux_commands: ",
+		this_usbduxfastsub->comedidev->minor);
+	for (i = 0; i < SIZEOFDUXBUFFER; i++) {
+		printk(" %02x", this_usbduxfastsub->dux_commands[i]);
+	}
+	printk("\n");
+#endif
+	result = usb_bulk_msg(this_usbduxfastsub->usbdev,
+			      usb_sndbulkpipe(this_usbduxfastsub->usbdev,
+					      CHANNELLISTEP),
+			      this_usbduxfastsub->dux_commands, SIZEOFDUXBUFFER,
+			      &nsent, 10000);
+	if (result < 0) {
+		printk("comedi%d: could not transmit dux_commands to the usb-device, err=%d\n", this_usbduxfastsub->comedidev->minor, result);
+	}
+	return result;
+}
+
+// Stops the data acquision
+// It should be safe to call this function from any context
+static int usbduxfastsub_unlink_InURBs(usbduxfastsub_t * usbduxfastsub_tmp)
+{
+	int j = 0;
+	int err = 0;
+
+	if (usbduxfastsub_tmp && usbduxfastsub_tmp->urbIn) {
+		usbduxfastsub_tmp->ai_cmd_running = 0;
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,8)
+		j = usb_unlink_urb(usbduxfastsub_tmp->urbIn);
+		if (j < 0) {
+			err = j;
+		}
+#else
+		// waits until a running transfer is over
+		usb_kill_urb(usbduxfastsub_tmp->urbIn);
+		j = 0;
+#endif
+	}
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi: usbduxfast: unlinked InURB: res=%d\n", j);
+#endif
+	return err;
+}
+
+/* This will stop a running acquisition operation */
+// Is called from within this driver from both the
+// interrupt context and from comedi
+static int usbduxfast_ai_stop(usbduxfastsub_t * this_usbduxfastsub,
+	int do_unlink)
+{
+	int ret = 0;
+
+	if (!this_usbduxfastsub) {
+		printk("comedi?: usbduxfast_ai_stop: this_usbduxfastsub=NULL!\n");
+		return -EFAULT;
+	}
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi: usbduxfast_ai_stop\n");
+#endif
+
+	this_usbduxfastsub->ai_cmd_running = 0;
+
+	if (do_unlink) {
+		// stop aquistion
+		ret = usbduxfastsub_unlink_InURBs(this_usbduxfastsub);
+	}
+
+	return ret;
+}
+
+// This will cancel a running acquisition operation.
+// This is called by comedi but never from inside the
+// driver.
+static int usbduxfast_ai_cancel(comedi_device * dev, comedi_subdevice * s)
+{
+	usbduxfastsub_t *this_usbduxfastsub;
+	int res = 0;
+
+	// force unlink of all urbs
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi: usbduxfast_ai_cancel\n");
+#endif
+	this_usbduxfastsub = dev->private;
+	if (!this_usbduxfastsub) {
+		printk("comedi: usbduxfast_ai_cancel: this_usbduxfastsub=NULL\n");
+		return -EFAULT;
+	}
+	down(&this_usbduxfastsub->sem);
+	if (!(this_usbduxfastsub->probed)) {
+		up(&this_usbduxfastsub->sem);
+		return -ENODEV;
+	}
+	// unlink
+	res = usbduxfast_ai_stop(this_usbduxfastsub, 1);
+	up(&this_usbduxfastsub->sem);
+
+	return res;
+}
+
+// analogue IN
+// interrupt service routine
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0)
+static void usbduxfastsub_ai_Irq(struct urb *urb)
+#else
+static void usbduxfastsub_ai_Irq(struct urb *urb PT_REGS_ARG)
+#endif
+{
+	int n, err;
+	usbduxfastsub_t *this_usbduxfastsub;
+	comedi_device *this_comedidev;
+	comedi_subdevice *s;
+	uint16_t *p;
+
+	// sanity checks
+	// is the urb there?
+	if (!urb) {
+		printk("comedi_: usbduxfast_: ao int-handler called with urb=NULL!\n");
+		return;
+	}
+	// the context variable points to the subdevice
+	this_comedidev = urb->context;
+	if (!this_comedidev) {
+		printk("comedi_: usbduxfast_: urb context is a NULL pointer!\n");
+		return;
+	}
+	// the private structure of the subdevice is usbduxfastsub_t
+	this_usbduxfastsub = this_comedidev->private;
+	if (!this_usbduxfastsub) {
+		printk("comedi_: usbduxfast_: private of comedi subdev is a NULL pointer!\n");
+		return;
+	}
+	// are we running a command?
+	if (unlikely(!(this_usbduxfastsub->ai_cmd_running))) {
+		// not running a command
+		// do not continue execution if no asynchronous command is running
+		// in particular not resubmit
+		return;
+	}
+
+	if (unlikely(!(this_usbduxfastsub->attached))) {
+		// no comedi device there
+		return;
+	}
+	// subdevice which is the AD converter
+	s = this_comedidev->subdevices + SUBDEV_AD;
+
+	// first we test if something unusual has just happened
+	switch (urb->status) {
+	case 0:
+		break;
+
+		// happens after an unlink command or when the device is plugged out
+	case -ECONNRESET:
+	case -ENOENT:
+	case -ESHUTDOWN:
+	case -ECONNABORTED:
+		// tell this comedi
+		s->async->events |= COMEDI_CB_EOA;
+		s->async->events |= COMEDI_CB_ERROR;
+		comedi_event(this_usbduxfastsub->comedidev, s);
+		// stop the transfer w/o unlink
+		usbduxfast_ai_stop(this_usbduxfastsub, 0);
+		return;
+
+	default:
+		printk("comedi%d: usbduxfast: non-zero urb status received in ai intr context: %d\n", this_usbduxfastsub->comedidev->minor, urb->status);
+		s->async->events |= COMEDI_CB_EOA;
+		s->async->events |= COMEDI_CB_ERROR;
+		comedi_event(this_usbduxfastsub->comedidev, s);
+		usbduxfast_ai_stop(this_usbduxfastsub, 0);
+		return;
+	}
+
+	p = urb->transfer_buffer;
+	if (!this_usbduxfastsub->ignore) {
+		if (!(this_usbduxfastsub->ai_continous)) {
+			// not continous, fixed number of samples
+			n = urb->actual_length / sizeof(uint16_t);
+			if (unlikely(this_usbduxfastsub->ai_sample_count < n)) {
+				// we have send only a fraction of the bytes received
+				cfc_write_array_to_buffer(s,
+					urb->transfer_buffer,
+					this_usbduxfastsub->ai_sample_count *
+					sizeof(uint16_t));
+				usbduxfast_ai_stop(this_usbduxfastsub, 0);
+				// say comedi that the acquistion is over
+				s->async->events |= COMEDI_CB_EOA;
+				comedi_event(this_usbduxfastsub->comedidev, s);
+				return;
+			}
+			this_usbduxfastsub->ai_sample_count -= n;
+		}
+		// write the full buffer to comedi
+		cfc_write_array_to_buffer(s,
+			urb->transfer_buffer, urb->actual_length);
+
+		// tell comedi that data is there
+		comedi_event(this_usbduxfastsub->comedidev, s);
+
+	} else {
+		// ignore this packet
+		this_usbduxfastsub->ignore--;
+	}
+
+	// command is still running
+	// resubmit urb for BULK transfer
+	urb->dev = this_usbduxfastsub->usbdev;
+	urb->status = 0;
+	err = usb_submit_urb(urb, GFP_ATOMIC);
+	if (err < 0) {
+		printk("comedi%d: usbduxfast: urb resubm failed: %d",
+			this_usbduxfastsub->comedidev->minor, err);
+		s->async->events |= COMEDI_CB_EOA;
+		s->async->events |= COMEDI_CB_ERROR;
+		comedi_event(this_usbduxfastsub->comedidev, s);
+		usbduxfast_ai_stop(this_usbduxfastsub, 0);
+	}
+}
+
+static int usbduxfastsub_start(usbduxfastsub_t * usbduxfastsub)
+{
+	int errcode = 0;
+	unsigned char local_transfer_buffer[16];
+
+	if (usbduxfastsub->probed) {
+		// 7f92 to zero
+		local_transfer_buffer[0] = 0;
+		errcode = usb_control_msg(usbduxfastsub->usbdev,
+			// create a pipe for a control transfer
+			usb_sndctrlpipe(usbduxfastsub->usbdev, 0),
+			// bRequest, "Firmware"
+			USBDUXFASTSUB_FIRMWARE,
+			// bmRequestType
+			VENDOR_DIR_OUT,
+			// Value
+			USBDUXFASTSUB_CPUCS,
+			// Index
+			0x0000,
+			// address of the transfer buffer
+			local_transfer_buffer,
+			// Length
+			1,
+			// Timeout
+			EZTIMEOUT);
+		if (errcode < 0) {
+			printk("comedi_: usbduxfast_: control msg failed (start)\n");
+			return errcode;
+		}
+	}
+	return 0;
+}
+
+static int usbduxfastsub_stop(usbduxfastsub_t * usbduxfastsub)
+{
+	int errcode = 0;
+
+	unsigned char local_transfer_buffer[16];
+	if (usbduxfastsub->probed) {
+		// 7f92 to one
+		local_transfer_buffer[0] = 1;
+		errcode = usb_control_msg(usbduxfastsub->usbdev,
+			usb_sndctrlpipe(usbduxfastsub->usbdev, 0),
+			// bRequest, "Firmware"
+			USBDUXFASTSUB_FIRMWARE,
+			// bmRequestType
+			VENDOR_DIR_OUT,
+			// Value
+			USBDUXFASTSUB_CPUCS,
+			// Index
+			0x0000, local_transfer_buffer,
+			// Length
+			1,
+			// Timeout
+			EZTIMEOUT);
+		if (errcode < 0) {
+			printk("comedi_: usbduxfast: control msg failed (stop)\n");
+			return errcode;
+		}
+	}
+	return 0;
+}
+
+static int usbduxfastsub_upload(usbduxfastsub_t * usbduxfastsub,
+	unsigned char *local_transfer_buffer,
+	unsigned int startAddr, unsigned int len)
+{
+	int errcode;
+
+	if (usbduxfastsub->probed) {
+#ifdef CONFIG_COMEDI_DEBUG
+		printk("comedi%d: usbduxfast: uploading %d bytes",
+			usbduxfastsub->comedidev->minor, len);
+		printk(" to addr %d, first byte=%d.\n",
+			startAddr, local_transfer_buffer[0]);
+#endif
+		errcode = usb_control_msg(usbduxfastsub->usbdev,
+			usb_sndctrlpipe(usbduxfastsub->usbdev, 0),
+			// brequest, firmware
+			USBDUXFASTSUB_FIRMWARE,
+			// bmRequestType
+			VENDOR_DIR_OUT,
+			// value
+			startAddr,
+			// index
+			0x0000,
+			// our local safe buffer
+			local_transfer_buffer,
+			// length
+			len,
+			// timeout
+			EZTIMEOUT);
+#ifdef CONFIG_COMEDI_DEBUG
+		printk("comedi_: usbduxfast: result=%d\n", errcode);
+#endif
+		if (errcode < 0) {
+			printk("comedi_: usbduxfast: uppload failed\n");
+			return errcode;
+		}
+	} else {
+		// no device on the bus for this index
+		return -EFAULT;
+	}
+	return 0;
+}
+
+int firmwareUpload(usbduxfastsub_t * usbduxfastsub,
+	unsigned char *firmwareBinary, int sizeFirmware)
+{
+	int ret;
+
+	if (!firmwareBinary) {
+		return 0;
+	}
+	ret = usbduxfastsub_stop(usbduxfastsub);
+	if (ret < 0) {
+		printk("comedi_: usbduxfast: can not stop firmware\n");
+		return ret;
+	}
+	ret = usbduxfastsub_upload(usbduxfastsub,
+		firmwareBinary, 0, sizeFirmware);
+	if (ret < 0) {
+		printk("comedi_: usbduxfast: firmware upload failed\n");
+		return ret;
+	}
+	ret = usbduxfastsub_start(usbduxfastsub);
+	if (ret < 0) {
+		printk("comedi_: usbduxfast: can not start firmware\n");
+		return ret;
+	}
+	return 0;
+}
+
+int usbduxfastsub_submit_InURBs(usbduxfastsub_t * usbduxfastsub)
+{
+	int errFlag;
+
+	if (!usbduxfastsub) {
+		return -EFAULT;
+	}
+	usb_fill_bulk_urb(usbduxfastsub->urbIn,
+		usbduxfastsub->usbdev,
+		usb_rcvbulkpipe(usbduxfastsub->usbdev, BULKINEP),
+		usbduxfastsub->transfer_buffer,
+		SIZEINBUF, usbduxfastsub_ai_Irq, usbduxfastsub->comedidev);
+
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi%d: usbduxfast: submitting in-urb: %x,%x\n",
+		usbduxfastsub->comedidev->minor,
+		(int)(usbduxfastsub->urbIn->context),
+		(int)(usbduxfastsub->urbIn->dev));
+#endif
+	errFlag = usb_submit_urb(usbduxfastsub->urbIn, GFP_ATOMIC);
+	if (errFlag) {
+		printk("comedi_: usbduxfast: ai: usb_submit_urb error %d\n",
+			errFlag);
+		return errFlag;
+	}
+	return 0;
+}
+
+static int usbduxfast_ai_cmdtest(comedi_device * dev,
+	comedi_subdevice * s, comedi_cmd * cmd)
+{
+	int err = 0, stop_mask = 0;
+	long int steps, tmp = 0;
+	int minSamplPer;
+	usbduxfastsub_t *this_usbduxfastsub = dev->private;
+	if (!(this_usbduxfastsub->probed)) {
+		return -ENODEV;
+	}
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi%d: usbduxfast_ai_cmdtest\n", dev->minor);
+	printk("comedi%d: usbduxfast: convert_arg=%u scan_begin_arg=%u\n",
+		dev->minor, cmd->convert_arg, cmd->scan_begin_arg);
+#endif
+	/* step 1: make sure trigger sources are trivially valid */
+
+	tmp = cmd->start_src;
+	cmd->start_src &= TRIG_NOW | TRIG_EXT | TRIG_INT;
+	if (!cmd->start_src || tmp != cmd->start_src)
+		err++;
+
+	tmp = cmd->scan_begin_src;
+	cmd->scan_begin_src &= TRIG_TIMER | TRIG_FOLLOW | TRIG_EXT;
+	if (!cmd->scan_begin_src || tmp != cmd->scan_begin_src)
+		err++;
+
+	tmp = cmd->convert_src;
+	cmd->convert_src &= TRIG_TIMER | TRIG_EXT;
+	if (!cmd->convert_src || tmp != cmd->convert_src)
+		err++;
+
+	tmp = cmd->scan_end_src;
+	cmd->scan_end_src &= TRIG_COUNT;
+	if (!cmd->scan_end_src || tmp != cmd->scan_end_src)
+		err++;
+
+	tmp = cmd->stop_src;
+	stop_mask = TRIG_COUNT | TRIG_NONE;
+	cmd->stop_src &= stop_mask;
+	if (!cmd->stop_src || tmp != cmd->stop_src)
+		err++;
+
+	if (err)
+		return 1;
+
+	/* step 2: make sure trigger sources are unique and mutually compatible */
+
+	if (cmd->start_src != TRIG_NOW &&
+		cmd->start_src != TRIG_EXT && cmd->start_src != TRIG_INT)
+		err++;
+	if (cmd->scan_begin_src != TRIG_TIMER &&
+		cmd->scan_begin_src != TRIG_FOLLOW &&
+		cmd->scan_begin_src != TRIG_EXT)
+		err++;
+	if (cmd->convert_src != TRIG_TIMER && cmd->convert_src != TRIG_EXT)
+		err++;
+	if (cmd->stop_src != TRIG_COUNT &&
+		cmd->stop_src != TRIG_EXT && cmd->stop_src != TRIG_NONE)
+		err++;
+
+	// can't have external stop and start triggers at once
+	if (cmd->start_src == TRIG_EXT && cmd->stop_src == TRIG_EXT)
+		err++;
+
+	if (err)
+		return 2;
+
+	/* step 3: make sure arguments are trivially compatible */
+
+	if (cmd->start_src == TRIG_NOW && cmd->start_arg != 0) {
+		cmd->start_arg = 0;
+		err++;
+	}
+
+	if (!cmd->chanlist_len) {
+		err++;
+	}
+	if (cmd->scan_end_arg != cmd->chanlist_len) {
+		cmd->scan_end_arg = cmd->chanlist_len;
+		err++;
+	}
+
+	if (cmd->chanlist_len == 1) {
+		minSamplPer = 1;
+	} else {
+		minSamplPer = MIN_SAMPLING_PERIOD;
+	}
+
+	if (cmd->convert_src == TRIG_TIMER) {
+		steps = cmd->convert_arg * 30;
+		if (steps < (minSamplPer * 1000)) {
+			steps = minSamplPer * 1000;
+		}
+		if (steps > (MAX_SAMPLING_PERIOD * 1000)) {
+			steps = MAX_SAMPLING_PERIOD * 1000;
+		}
+		// calc arg again
+		tmp = steps / 30;
+		if (cmd->convert_arg != tmp) {
+			cmd->convert_arg = tmp;
+			err++;
+		}
+	}
+
+	if (cmd->scan_begin_src == TRIG_TIMER) {
+		err++;
+	}
+	// stop source
+	switch (cmd->stop_src) {
+	case TRIG_COUNT:
+		if (!cmd->stop_arg) {
+			cmd->stop_arg = 1;
+			err++;
+		}
+		break;
+	case TRIG_NONE:
+		if (cmd->stop_arg != 0) {
+			cmd->stop_arg = 0;
+			err++;
+		}
+		break;
+		// TRIG_EXT doesn't care since it doesn't trigger off a numbered channel
+	default:
+		break;
+	}
+
+	if (err)
+		return 3;
+
+	/* step 4: fix up any arguments */
+
+	return 0;
+
+}
+
+static int usbduxfast_ai_inttrig(comedi_device * dev,
+	comedi_subdevice * s, unsigned int trignum)
+{
+	int ret;
+	usbduxfastsub_t *this_usbduxfastsub = dev->private;
+	if (!this_usbduxfastsub) {
+		return -EFAULT;
+	}
+	down(&this_usbduxfastsub->sem);
+	if (!(this_usbduxfastsub->probed)) {
+		up(&this_usbduxfastsub->sem);
+		return -ENODEV;
+	}
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi%d: usbduxfast_ai_inttrig\n", dev->minor);
+#endif
+
+	if (trignum != 0) {
+		printk("comedi%d: usbduxfast_ai_inttrig: invalid trignum\n",
+			dev->minor);
+		up(&this_usbduxfastsub->sem);
+		return -EINVAL;
+	}
+	if (!(this_usbduxfastsub->ai_cmd_running)) {
+		this_usbduxfastsub->ai_cmd_running = 1;
+		ret = usbduxfastsub_submit_InURBs(this_usbduxfastsub);
+		if (ret < 0) {
+			printk("comedi%d: usbduxfast_ai_inttrig: urbSubmit: err=%d\n", dev->minor, ret);
+			this_usbduxfastsub->ai_cmd_running = 0;
+			up(&this_usbduxfastsub->sem);
+			return ret;
+		}
+		s->async->inttrig = NULL;
+	} else {
+		printk("comedi%d: ai_inttrig but acqu is already running\n",
+			dev->minor);
+	}
+	up(&this_usbduxfastsub->sem);
+	return 1;
+}
+
+// offsets for the GPIF bytes
+// the first byte is the command byte
+#define LENBASE 1+0x00
+#define OPBASE  1+0x08
+#define OUTBASE 1+0x10
+#define LOGBASE 1+0x18
+
+static int usbduxfast_ai_cmd(comedi_device * dev, comedi_subdevice * s)
+{
+	comedi_cmd *cmd = &s->async->cmd;
+	unsigned int chan, gain, rngmask = 0xff;
+	int i, j, ret;
+	usbduxfastsub_t *this_usbduxfastsub = dev->private;
+	int result;
+	long steps, steps_tmp;
+
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi%d: usbduxfast_ai_cmd\n", dev->minor);
+#endif
+	if (!this_usbduxfastsub) {
+		return -EFAULT;
+	}
+	down(&this_usbduxfastsub->sem);
+	if (!(this_usbduxfastsub->probed)) {
+		up(&this_usbduxfastsub->sem);
+		return -ENODEV;
+	}
+	if (this_usbduxfastsub->ai_cmd_running) {
+		printk("comedi%d: ai_cmd not possible. Another ai_cmd is running.\n", dev->minor);
+		up(&this_usbduxfastsub->sem);
+		return -EBUSY;
+	}
+	// set current channel of the running aquisition to zero
+	s->async->cur_chan = 0;
+
+	// ignore the first buffers from the device if there is an error condition
+	this_usbduxfastsub->ignore = PACKETS_TO_IGNORE;
+
+	if (cmd->chanlist_len > 0) {
+		gain = CR_RANGE(cmd->chanlist[0]);
+		for (i = 0; i < cmd->chanlist_len; ++i) {
+			chan = CR_CHAN(cmd->chanlist[i]);
+			if (chan != i) {
+				printk("comedi%d: cmd is accepting only consecutive channels.\n", dev->minor);
+				up(&this_usbduxfastsub->sem);
+				return -EINVAL;
+			}
+			if ((gain != CR_RANGE(cmd->chanlist[i]))
+				&& (cmd->chanlist_len > 3)) {
+				printk("comedi%d: the gain must be the same for all channels.\n", dev->minor);
+				up(&this_usbduxfastsub->sem);
+				return -EINVAL;
+			}
+			if (i >= NUMCHANNELS) {
+				printk("comedi%d: channel list too long\n",
+					dev->minor);
+				break;
+			}
+		}
+	}
+	steps = 0;
+	if (cmd->scan_begin_src == TRIG_TIMER) {
+		printk("comedi%d: usbduxfast: scan_begin_src==TRIG_TIMER not valid.\n", dev->minor);
+		up(&this_usbduxfastsub->sem);
+		return -EINVAL;
+	}
+	if (cmd->convert_src == TRIG_TIMER) {
+		steps = (cmd->convert_arg * 30) / 1000;
+	}
+	if ((steps < MIN_SAMPLING_PERIOD) && (cmd->chanlist_len != 1)) {
+		printk("comedi%d: usbduxfast: ai_cmd: steps=%ld, scan_begin_arg=%d. Not properly tested by cmdtest?\n", dev->minor, steps, cmd->scan_begin_arg);
+		up(&this_usbduxfastsub->sem);
+		return -EINVAL;
+	}
+	if (steps > MAX_SAMPLING_PERIOD) {
+		printk("comedi%d: usbduxfast: ai_cmd: sampling rate too low.\n",
+			dev->minor);
+		up(&this_usbduxfastsub->sem);
+		return -EINVAL;
+	}
+	if ((cmd->start_src == TRIG_EXT) && (cmd->chanlist_len != 1)
+		&& (cmd->chanlist_len != 16)) {
+		printk("comedi%d: usbduxfast: ai_cmd: TRIG_EXT only with 1 or 16 channels possible.\n", dev->minor);
+		up(&this_usbduxfastsub->sem);
+		return -EINVAL;
+	}
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi%d: usbduxfast: steps=%ld, convert_arg=%u, ai_timer=%u\n",
+		dev->minor,
+		steps, cmd->convert_arg, this_usbduxfastsub->ai_timer);
+#endif
+
+	switch (cmd->chanlist_len) {
+		// one channel
+	case 1:
+		if (CR_RANGE(cmd->chanlist[0]) > 0)
+			rngmask = 0xff - 0x04;
+		else
+			rngmask = 0xff;
+
+		// for external trigger: looping in this state until the RDY0 pin
+		// becomes zero
+		if (cmd->start_src == TRIG_EXT) {	// we loop here until ready has been set
+			this_usbduxfastsub->dux_commands[LENBASE + 0] = 0x01;	// branch back to state 0
+			this_usbduxfastsub->dux_commands[OPBASE + 0] = 0x01;	// deceision state w/o data
+			this_usbduxfastsub->dux_commands[OUTBASE + 0] =
+				0xFF & rngmask;
+			this_usbduxfastsub->dux_commands[LOGBASE + 0] = 0x00;	// RDY0 = 0
+		} else {	// we just proceed to state 1
+			this_usbduxfastsub->dux_commands[LENBASE + 0] = 1;
+			this_usbduxfastsub->dux_commands[OPBASE + 0] = 0;
+			this_usbduxfastsub->dux_commands[OUTBASE + 0] =
+				0xFF & rngmask;
+			this_usbduxfastsub->dux_commands[LOGBASE + 0] = 0;
+		}
+
+		if (steps < MIN_SAMPLING_PERIOD) {
+			// for fast single channel aqu without mux
+			if (steps <= 1) {
+				// we just stay here at state 1 and rexecute the same state
+				// this gives us 30MHz sampling rate
+				this_usbduxfastsub->dux_commands[LENBASE + 1] = 0x89;	// branch back to state 1
+				this_usbduxfastsub->dux_commands[OPBASE + 1] = 0x03;	// deceision state with data
+				this_usbduxfastsub->dux_commands[OUTBASE + 1] =
+					0xFF & rngmask;
+				this_usbduxfastsub->dux_commands[LOGBASE + 1] = 0xFF;	// doesn't matter
+			} else {
+				// we loop through two states: data and delay: max rate is 15Mhz
+				this_usbduxfastsub->dux_commands[LENBASE + 1] =
+					steps - 1;
+				this_usbduxfastsub->dux_commands[OPBASE + 1] = 0x02;	// data
+				this_usbduxfastsub->dux_commands[OUTBASE + 1] =
+					0xFF & rngmask;
+				this_usbduxfastsub->dux_commands[LOGBASE + 1] = 0;	// doesn't matter
+
+				this_usbduxfastsub->dux_commands[LENBASE + 2] = 0x09;	// branch back to state 1
+				this_usbduxfastsub->dux_commands[OPBASE + 2] = 0x01;	// deceision state w/o data
+				this_usbduxfastsub->dux_commands[OUTBASE + 2] =
+					0xFF & rngmask;
+				this_usbduxfastsub->dux_commands[LOGBASE + 2] = 0xFF;	// doesn't matter
+			}
+		} else {
+			// we loop through 3 states: 2x delay and 1x data. This gives a min
+			// sampling rate of 60kHz.
+
+			// we have 1 state with duration 1
+			steps = steps - 1;
+
+			// do the first part of the delay
+			this_usbduxfastsub->dux_commands[LENBASE + 1] =
+				steps / 2;
+			this_usbduxfastsub->dux_commands[OPBASE + 1] = 0;
+			this_usbduxfastsub->dux_commands[OUTBASE + 1] =
+				0xFF & rngmask;
+			this_usbduxfastsub->dux_commands[LOGBASE + 1] = 0;
+
+			// and the second part
+			this_usbduxfastsub->dux_commands[LENBASE + 2] =
+				steps - steps / 2;
+			this_usbduxfastsub->dux_commands[OPBASE + 2] = 0;
+			this_usbduxfastsub->dux_commands[OUTBASE + 2] =
+				0xFF & rngmask;
+			this_usbduxfastsub->dux_commands[LOGBASE + 2] = 0;
+
+			// get the data and branch back
+			this_usbduxfastsub->dux_commands[LENBASE + 3] = 0x09;	// branch back to state 1
+			this_usbduxfastsub->dux_commands[OPBASE + 3] = 0x03;	// deceision state w data
+			this_usbduxfastsub->dux_commands[OUTBASE + 3] =
+				0xFF & rngmask;
+			this_usbduxfastsub->dux_commands[LOGBASE + 3] = 0xFF;	// doesn't matter
+		}
+		break;
+
+	case 2:
+		// two channels
+		// commit data to the FIFO
+		if (CR_RANGE(cmd->chanlist[0]) > 0)
+			rngmask = 0xff - 0x04;
+		else
+			rngmask = 0xff;
+		this_usbduxfastsub->dux_commands[LENBASE + 0] = 1;
+		this_usbduxfastsub->dux_commands[OPBASE + 0] = 0x02;	// data
+		this_usbduxfastsub->dux_commands[OUTBASE + 0] = 0xFF & rngmask;
+		this_usbduxfastsub->dux_commands[LOGBASE + 0] = 0;
+
+		// we have 1 state with duration 1: state 0
+		steps_tmp = steps - 1;
+
+		if (CR_RANGE(cmd->chanlist[1]) > 0)
+			rngmask = 0xff - 0x04;
+		else
+			rngmask = 0xff;
+		// do the first part of the delay
+		this_usbduxfastsub->dux_commands[LENBASE + 1] = steps_tmp / 2;
+		this_usbduxfastsub->dux_commands[OPBASE + 1] = 0;
+		this_usbduxfastsub->dux_commands[OUTBASE + 1] = 0xFE & rngmask;	//count
+		this_usbduxfastsub->dux_commands[LOGBASE + 1] = 0;
+
+		// and the second part
+		this_usbduxfastsub->dux_commands[LENBASE + 2] =
+			steps_tmp - steps_tmp / 2;
+		this_usbduxfastsub->dux_commands[OPBASE + 2] = 0;
+		this_usbduxfastsub->dux_commands[OUTBASE + 2] = 0xFF & rngmask;
+		this_usbduxfastsub->dux_commands[LOGBASE + 2] = 0;
+
+		this_usbduxfastsub->dux_commands[LENBASE + 3] = 1;
+		this_usbduxfastsub->dux_commands[OPBASE + 3] = 0x02;	// data
+		this_usbduxfastsub->dux_commands[OUTBASE + 3] = 0xFF & rngmask;
+		this_usbduxfastsub->dux_commands[LOGBASE + 3] = 0;
+
+		// we have 2 states with duration 1: step 6 and the IDLE state
+		steps_tmp = steps - 2;
+
+		if (CR_RANGE(cmd->chanlist[0]) > 0)
+			rngmask = 0xff - 0x04;
+		else
+			rngmask = 0xff;
+		// do the first part of the delay
+		this_usbduxfastsub->dux_commands[LENBASE + 4] = steps_tmp / 2;
+		this_usbduxfastsub->dux_commands[OPBASE + 4] = 0;
+		this_usbduxfastsub->dux_commands[OUTBASE + 4] = (0xFF - 0x02) & rngmask;	//reset
+		this_usbduxfastsub->dux_commands[LOGBASE + 4] = 0;
+
+		// and the second part
+		this_usbduxfastsub->dux_commands[LENBASE + 5] =
+			steps_tmp - steps_tmp / 2;
+		this_usbduxfastsub->dux_commands[OPBASE + 5] = 0;
+		this_usbduxfastsub->dux_commands[OUTBASE + 5] = 0xFF & rngmask;
+		this_usbduxfastsub->dux_commands[LOGBASE + 5] = 0;
+
+		this_usbduxfastsub->dux_commands[LENBASE + 6] = 1;
+		this_usbduxfastsub->dux_commands[OPBASE + 6] = 0;
+		this_usbduxfastsub->dux_commands[OUTBASE + 6] = 0xFF & rngmask;
+		this_usbduxfastsub->dux_commands[LOGBASE + 6] = 0;
+		break;
+
+	case 3:
+		// three channels
+		for (j = 0; j < 1; j++) {
+			if (CR_RANGE(cmd->chanlist[j]) > 0)
+				rngmask = 0xff - 0x04;
+			else
+				rngmask = 0xff;
+			// commit data to the FIFO and do the first part of the delay
+			this_usbduxfastsub->dux_commands[LENBASE + j * 2] =
+				steps / 2;
+			this_usbduxfastsub->dux_commands[OPBASE + j * 2] = 0x02;	// data
+			this_usbduxfastsub->dux_commands[OUTBASE + j * 2] = 0xFF & rngmask;	// no change
+			this_usbduxfastsub->dux_commands[LOGBASE + j * 2] = 0;
+
+			if (CR_RANGE(cmd->chanlist[j + 1]) > 0)
+				rngmask = 0xff - 0x04;
+			else
+				rngmask = 0xff;
+			// do the second part of the delay
+			this_usbduxfastsub->dux_commands[LENBASE + j * 2 + 1] =
+				steps - steps / 2;
+			this_usbduxfastsub->dux_commands[OPBASE + j * 2 + 1] = 0;	// no data
+			this_usbduxfastsub->dux_commands[OUTBASE + j * 2 + 1] = 0xFE & rngmask;	//count
+			this_usbduxfastsub->dux_commands[LOGBASE + j * 2 + 1] =
+				0;
+		}
+
+		// 2 steps with duration 1: the idele step and step 6:
+		steps_tmp = steps - 2;
+		// commit data to the FIFO and do the first part of the delay
+		this_usbduxfastsub->dux_commands[LENBASE + 4] = steps_tmp / 2;
+		this_usbduxfastsub->dux_commands[OPBASE + 4] = 0x02;	// data
+		this_usbduxfastsub->dux_commands[OUTBASE + 4] = 0xFF & rngmask;	// no change
+		this_usbduxfastsub->dux_commands[LOGBASE + 4] = 0;
+
+		if (CR_RANGE(cmd->chanlist[0]) > 0)
+			rngmask = 0xff - 0x04;
+		else
+			rngmask = 0xff;
+		// do the second part of the delay
+		this_usbduxfastsub->dux_commands[LENBASE + 5] =
+			steps_tmp - steps_tmp / 2;
+		this_usbduxfastsub->dux_commands[OPBASE + 5] = 0;	// no data
+		this_usbduxfastsub->dux_commands[OUTBASE + 5] = (0xFF - 0x02) & rngmask;	// reset
+		this_usbduxfastsub->dux_commands[LOGBASE + 5] = 0;
+
+		this_usbduxfastsub->dux_commands[LENBASE + 6] = 1;
+		this_usbduxfastsub->dux_commands[OPBASE + 6] = 0;
+		this_usbduxfastsub->dux_commands[OUTBASE + 6] = 0xFF & rngmask;
+		this_usbduxfastsub->dux_commands[LOGBASE + 6] = 0;
+
+	case 16:
+		if (CR_RANGE(cmd->chanlist[0]) > 0)
+			rngmask = 0xff - 0x04;
+		else
+			rngmask = 0xff;
+		if (cmd->start_src == TRIG_EXT) {	// we loop here until ready has been set
+			this_usbduxfastsub->dux_commands[LENBASE + 0] = 0x01;	// branch back to state 0
+			this_usbduxfastsub->dux_commands[OPBASE + 0] = 0x01;	// deceision state w/o data
+			this_usbduxfastsub->dux_commands[OUTBASE + 0] = (0xFF - 0x02) & rngmask;	// reset
+			this_usbduxfastsub->dux_commands[LOGBASE + 0] = 0x00;	// RDY0 = 0
+		} else {	// we just proceed to state 1
+			this_usbduxfastsub->dux_commands[LENBASE + 0] = 255;	// 30us reset pulse
+			this_usbduxfastsub->dux_commands[OPBASE + 0] = 0;
+			this_usbduxfastsub->dux_commands[OUTBASE + 0] = (0xFF - 0x02) & rngmask;	// reset
+			this_usbduxfastsub->dux_commands[LOGBASE + 0] = 0;
+		}
+
+		// commit data to the FIFO
+		this_usbduxfastsub->dux_commands[LENBASE + 1] = 1;
+		this_usbduxfastsub->dux_commands[OPBASE + 1] = 0x02;	// data
+		this_usbduxfastsub->dux_commands[OUTBASE + 1] = 0xFF & rngmask;
+		this_usbduxfastsub->dux_commands[LOGBASE + 1] = 0;
+
+		// we have 2 states with duration 1
+		steps = steps - 2;
+
+		// do the first part of the delay
+		this_usbduxfastsub->dux_commands[LENBASE + 2] = steps / 2;
+		this_usbduxfastsub->dux_commands[OPBASE + 2] = 0;
+		this_usbduxfastsub->dux_commands[OUTBASE + 2] = 0xFE & rngmask;
+		this_usbduxfastsub->dux_commands[LOGBASE + 2] = 0;
+
+		// and the second part
+		this_usbduxfastsub->dux_commands[LENBASE + 3] =
+			steps - steps / 2;
+		this_usbduxfastsub->dux_commands[OPBASE + 3] = 0;
+		this_usbduxfastsub->dux_commands[OUTBASE + 3] = 0xFF & rngmask;
+		this_usbduxfastsub->dux_commands[LOGBASE + 3] = 0;
+
+		this_usbduxfastsub->dux_commands[LENBASE + 4] = 0x09;	// branch back to state 1
+		this_usbduxfastsub->dux_commands[OPBASE + 4] = 0x01;	// deceision state w/o data
+		this_usbduxfastsub->dux_commands[OUTBASE + 4] = 0xFF & rngmask;
+		this_usbduxfastsub->dux_commands[LOGBASE + 4] = 0xFF;	// doesn't matter
+
+		break;
+
+	default:
+		printk("comedi %d: unsupported combination of channels\n",
+			dev->minor);
+		up(&this_usbduxfastsub->sem);
+		return -EFAULT;
+	}
+
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi %d: sending commands to the usb device\n", dev->minor);
+#endif
+	// 0 means that the AD commands are sent
+	result = send_dux_commands(this_usbduxfastsub, SENDADCOMMANDS);
+	if (result < 0) {
+		printk("comedi%d: adc command could not be submitted. Aborting...\n", dev->minor);
+		up(&this_usbduxfastsub->sem);
+		return result;
+	}
+	if (cmd->stop_src == TRIG_COUNT) {
+		this_usbduxfastsub->ai_sample_count =
+			(cmd->stop_arg) * (cmd->scan_end_arg);
+		if (usbduxfastsub->ai_sample_count < 1) {
+			printk("comedi%d: (cmd->stop_arg)*(cmd->scan_end_arg)<1, aborting.\n", dev->minor);
+			up(&this_usbduxfastsub->sem);
+			return -EFAULT;
+		}
+		this_usbduxfastsub->ai_continous = 0;
+	} else {
+		// continous aquisition
+		this_usbduxfastsub->ai_continous = 1;
+		this_usbduxfastsub->ai_sample_count = 0;
+	}
+
+	if ((cmd->start_src == TRIG_NOW) || (cmd->start_src == TRIG_EXT)) {
+		// enable this acquisition operation
+		this_usbduxfastsub->ai_cmd_running = 1;
+		ret = usbduxfastsub_submit_InURBs(this_usbduxfastsub);
+		if (ret < 0) {
+			this_usbduxfastsub->ai_cmd_running = 0;
+			// fixme: unlink here??
+			up(&this_usbduxfastsub->sem);
+			return ret;
+		}
+		s->async->inttrig = NULL;
+	} else {
+		/* TRIG_INT */
+		// don't enable the acquision operation
+		// wait for an internal signal
+		s->async->inttrig = usbduxfast_ai_inttrig;
+	}
+	up(&this_usbduxfastsub->sem);
+
+	return 0;
+}
+
+/* Mode 0 is used to get a single conversion on demand */
+static int usbduxfast_ai_insn_read(comedi_device * dev,
+	comedi_subdevice * s, comedi_insn * insn, lsampl_t * data)
+{
+	int i, j, n, actual_length;
+	int chan, range, rngmask;
+	int err;
+	usbduxfastsub_t *usbduxfastsub = dev->private;
+
+	if (!usbduxfastsub) {
+		printk("comedi%d: ai_insn_read: no usb dev.\n", dev->minor);
+		return -ENODEV;
+	}
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi%d: ai_insn_read, insn->n=%d, insn->subdev=%d\n",
+		dev->minor, insn->n, insn->subdev);
+#endif
+	down(&usbduxfastsub->sem);
+	if (!(usbduxfastsub->probed)) {
+		up(&usbduxfastsub->sem);
+		return -ENODEV;
+	}
+	if (usbduxfastsub->ai_cmd_running) {
+		printk("comedi%d: ai_insn_read not possible. Async Command is running.\n", dev->minor);
+		up(&usbduxfastsub->sem);
+		return -EBUSY;
+	}
+	// sample one channel
+	chan = CR_CHAN(insn->chanspec);
+	range = CR_RANGE(insn->chanspec);
+	// set command for the first channel
+
+	if (range > 0)
+		rngmask = 0xff - 0x04;
+	else
+		rngmask = 0xff;
+	// commit data to the FIFO
+	usbduxfastsub->dux_commands[LENBASE + 0] = 1;
+	usbduxfastsub->dux_commands[OPBASE + 0] = 0x02;	// data
+	usbduxfastsub->dux_commands[OUTBASE + 0] = 0xFF & rngmask;
+	usbduxfastsub->dux_commands[LOGBASE + 0] = 0;
+
+	// do the first part of the delay
+	usbduxfastsub->dux_commands[LENBASE + 1] = 12;
+	usbduxfastsub->dux_commands[OPBASE + 1] = 0;
+	usbduxfastsub->dux_commands[OUTBASE + 1] = 0xFE & rngmask;
+	usbduxfastsub->dux_commands[LOGBASE + 1] = 0;
+
+	usbduxfastsub->dux_commands[LENBASE + 2] = 1;
+	usbduxfastsub->dux_commands[OPBASE + 2] = 0;
+	usbduxfastsub->dux_commands[OUTBASE + 2] = 0xFE & rngmask;
+	usbduxfastsub->dux_commands[LOGBASE + 2] = 0;
+
+	usbduxfastsub->dux_commands[LENBASE + 3] = 1;
+	usbduxfastsub->dux_commands[OPBASE + 3] = 0;
+	usbduxfastsub->dux_commands[OUTBASE + 3] = 0xFE & rngmask;
+	usbduxfastsub->dux_commands[LOGBASE + 3] = 0;
+
+	usbduxfastsub->dux_commands[LENBASE + 4] = 1;
+	usbduxfastsub->dux_commands[OPBASE + 4] = 0;
+	usbduxfastsub->dux_commands[OUTBASE + 4] = 0xFE & rngmask;
+	usbduxfastsub->dux_commands[LOGBASE + 4] = 0;
+
+	// second part
+	usbduxfastsub->dux_commands[LENBASE + 5] = 12;
+	usbduxfastsub->dux_commands[OPBASE + 5] = 0;
+	usbduxfastsub->dux_commands[OUTBASE + 5] = 0xFF & rngmask;
+	usbduxfastsub->dux_commands[LOGBASE + 5] = 0;
+
+	usbduxfastsub->dux_commands[LENBASE + 6] = 1;
+	usbduxfastsub->dux_commands[OPBASE + 6] = 0;
+	usbduxfastsub->dux_commands[OUTBASE + 6] = 0xFF & rngmask;
+	usbduxfastsub->dux_commands[LOGBASE + 0] = 0;
+
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi %d: sending commands to the usb device\n", dev->minor);
+#endif
+	// 0 means that the AD commands are sent
+	err = send_dux_commands(usbduxfastsub, SENDADCOMMANDS);
+	if (err < 0) {
+		printk("comedi%d: adc command could not be submitted. Aborting...\n", dev->minor);
+		up(&usbduxfastsub->sem);
+		return err;
+	}
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi%d: usbduxfast: submitting in-urb: %x,%x\n",
+		usbduxfastsub->comedidev->minor,
+		(int)(usbduxfastsub->urbIn->context),
+		(int)(usbduxfastsub->urbIn->dev));
+#endif
+	for (i = 0; i < PACKETS_TO_IGNORE; i++) {
+		err = usb_bulk_msg(usbduxfastsub->usbdev,
+				   usb_rcvbulkpipe(usbduxfastsub->usbdev,
+						   BULKINEP),
+				   usbduxfastsub->transfer_buffer, SIZEINBUF,
+				   &actual_length, 10000);
+		if (err < 0) {
+			printk("comedi%d: insn timeout. No data.\n",
+				dev->minor);
+			up(&usbduxfastsub->sem);
+			return err;
+		}
+	}
+	// data points
+	for (i = 0; i < insn->n;) {
+		err = usb_bulk_msg(usbduxfastsub->usbdev,
+				   usb_rcvbulkpipe(usbduxfastsub->usbdev,
+						   BULKINEP),
+				   usbduxfastsub->transfer_buffer, SIZEINBUF,
+				   &actual_length, 10000);
+		if (err < 0) {
+			printk("comedi%d: insn data error: %d\n",
+				dev->minor, err);
+			up(&usbduxfastsub->sem);
+			return err;
+		}
+		n = actual_length / sizeof(uint16_t);
+		if ((n % 16) != 0) {
+			printk("comedi%d: insn data packet corrupted.\n",
+				dev->minor);
+			up(&usbduxfastsub->sem);
+			return -EINVAL;
+		}
+		for (j = chan; (j < n) && (i < insn->n); j = j + 16) {
+			data[i] =
+				((uint16_t *) (usbduxfastsub->
+					transfer_buffer))[j];
+			i++;
+		}
+	}
+	up(&usbduxfastsub->sem);
+	return i;
+}
+
+static unsigned hex2unsigned(char *h)
+{
+	unsigned hi, lo;
+	if (h[0] > '9') {
+		hi = h[0] - 'A' + 0x0a;
+	} else {
+		hi = h[0] - '0';
+	}
+	if (h[1] > '9') {
+		lo = h[1] - 'A' + 0x0a;
+	} else {
+		lo = h[1] - '0';
+	}
+	return hi * 0x10 + lo;
+}
+
+// for FX2
+#define FIRMWARE_MAX_LEN 0x2000
+
+// taken from David Brownell's fxload and adjusted for this driver
+static int read_firmware(usbduxfastsub_t * usbduxfastsub, void *firmwarePtr,
+	long size)
+{
+	int i = 0;
+	unsigned char *fp = (char *)firmwarePtr;
+	unsigned char *firmwareBinary = NULL;
+	int res = 0;
+	int maxAddr = 0;
+
+	firmwareBinary = kmalloc(FIRMWARE_MAX_LEN, GFP_KERNEL);
+	if (!firmwareBinary) {
+		printk("comedi_: usbduxfast: mem alloc for firmware failed\n");
+		return -ENOMEM;
+	}
+
+	for (;;) {
+		char buf[256], *cp;
+		char type;
+		int len;
+		int idx, off;
+		int j = 0;
+
+		// get one line
+		while ((i < size) && (fp[i] != 13) && (fp[i] != 10)) {
+			buf[j] = fp[i];
+			i++;
+			j++;
+			if (j >= sizeof(buf)) {
+				printk("comedi_: usbduxfast: bogus firmware file!\n");
+				return -1;
+			}
+		}
+		// get rid of LF/CR/...
+		while ((i < size) && ((fp[i] == 13) || (fp[i] == 10)
+				|| (fp[i] == 0))) {
+			i++;
+		}
+
+		buf[j] = 0;
+		//printk("comedi_: buf=%s\n",buf);
+
+		/* EXTENSION: "# comment-till-end-of-line", for copyrights etc */
+		if (buf[0] == '#')
+			continue;
+
+		if (buf[0] != ':') {
+			printk("comedi_: usbduxfast: upload: not an ihex record: %s", buf);
+			return -EFAULT;
+		}
+
+		/* Read the length field (up to 16 bytes) */
+		len = hex2unsigned(buf + 1);
+
+		/* Read the target offset */
+		off = (hex2unsigned(buf + 3) * 0x0100) + hex2unsigned(buf + 5);
+
+		if ((off + len) > maxAddr) {
+			maxAddr = off + len;
+		}
+
+		if (maxAddr >= FIRMWARE_MAX_LEN) {
+			printk("comedi_: usbduxfast: firmware upload goes beyond FX2 RAM boundaries.");
+			return -EFAULT;
+		}
+		//printk("comedi_: usbduxfast: off=%x, len=%x:",off,len);
+
+		/* Read the record type */
+		type = hex2unsigned(buf + 7);
+
+		/* If this is an EOF record, then make it so. */
+		if (type == 1) {
+			break;
+		}
+
+		if (type != 0) {
+			printk("comedi_: usbduxfast: unsupported record type: %u\n", type);
+			return -EFAULT;
+		}
+
+		for (idx = 0, cp = buf + 9; idx < len; idx += 1, cp += 2) {
+			firmwareBinary[idx + off] = hex2unsigned(cp);
+			//printk("%02x ",firmwareBinary[idx+off]);
+		}
+		//printk("\n");
+
+		if (i >= size) {
+			printk("comedi_: usbduxfast: unexpected end of hex file\n");
+			break;
+		}
+
+	}
+	res = firmwareUpload(usbduxfastsub, firmwareBinary, maxAddr + 1);
+	kfree(firmwareBinary);
+	return res;
+}
+
+static void tidy_up(usbduxfastsub_t * usbduxfastsub_tmp)
+{
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi_: usbduxfast: tiding up\n");
+#endif
+	if (!usbduxfastsub_tmp) {
+		return;
+	}
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0)
+	// shows the usb subsystem that the driver is down
+	if (usbduxfastsub_tmp->interface) {
+		usb_set_intfdata(usbduxfastsub_tmp->interface, NULL);
+	}
+#endif
+
+	usbduxfastsub_tmp->probed = 0;
+
+	if (usbduxfastsub_tmp->urbIn) {
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,8)
+		// waits until a running transfer is over
+		// thus, under 2.4 hotplugging while a command
+		// is running is not safe
+		usb_kill_urb(usbduxfastsub_tmp->urbIn);
+#endif
+		if (usbduxfastsub_tmp->transfer_buffer) {
+			kfree(usbduxfastsub_tmp->transfer_buffer);
+			usbduxfastsub_tmp->transfer_buffer = NULL;
+		}
+		usb_free_urb(usbduxfastsub_tmp->urbIn);
+		usbduxfastsub_tmp->urbIn = NULL;
+	}
+	if (usbduxfastsub_tmp->insnBuffer) {
+		kfree(usbduxfastsub_tmp->insnBuffer);
+		usbduxfastsub_tmp->insnBuffer = NULL;
+	}
+	if (usbduxfastsub_tmp->dux_commands) {
+		kfree(usbduxfastsub_tmp->dux_commands);
+		usbduxfastsub_tmp->dux_commands = NULL;
+	}
+	usbduxfastsub_tmp->ai_cmd_running = 0;
+}
+
+// allocate memory for the urbs and initialise them
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0)
+static void *usbduxfastsub_probe(struct usb_device *udev,
+	unsigned int interfnum, const struct usb_device_id *id)
+{
+#else
+static int usbduxfastsub_probe(struct usb_interface *uinterf,
+	const struct usb_device_id *id)
+{
+	struct usb_device *udev = interface_to_usbdev(uinterf);
+#endif
+	int i;
+	int index;
+
+	if (udev->speed != USB_SPEED_HIGH) {
+		printk("comedi_: usbduxfast_: This driver needs USB 2.0 to operate. Aborting...\n");
+		return -ENODEV;
+	}
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi_: usbduxfast_: finding a free structure for the usb-device\n");
+#endif
+	down(&start_stop_sem);
+	// look for a free place in the usbduxfast array
+	index = -1;
+	for (i = 0; i < NUMUSBDUXFAST; i++) {
+		if (!(usbduxfastsub[i].probed)) {
+			index = i;
+			break;
+		}
+	}
+
+	// no more space
+	if (index == -1) {
+		printk("Too many usbduxfast-devices connected.\n");
+		up(&start_stop_sem);
+		return -EMFILE;
+	}
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi_: usbduxfast: usbduxfastsub[%d] is ready to connect to comedi.\n", index);
+#endif
+
+	init_MUTEX(&(usbduxfastsub[index].sem));
+	// save a pointer to the usb device
+	usbduxfastsub[index].usbdev = udev;
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0)
+	// save the interface number
+	usbduxfastsub[index].ifnum = interfnum;
+#else
+	// 2.6: save the interface itself
+	usbduxfastsub[index].interface = uinterf;
+	// get the interface number from the interface
+	usbduxfastsub[index].ifnum = uinterf->altsetting->desc.bInterfaceNumber;
+	// hand the private data over to the usb subsystem
+	// will be needed for disconnect
+	usb_set_intfdata(uinterf, &(usbduxfastsub[index]));
+#endif
+
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi_: usbduxfast: ifnum=%d\n", usbduxfastsub[index].ifnum);
+#endif
+	// create space for the commands going to the usb device
+	usbduxfastsub[index].dux_commands = kmalloc(SIZEOFDUXBUFFER,
+		GFP_KERNEL);
+	if (!usbduxfastsub[index].dux_commands) {
+		printk("comedi_: usbduxfast: error alloc space for dac commands\n");
+		tidy_up(&(usbduxfastsub[index]));
+		up(&start_stop_sem);
+		return -ENOMEM;
+	}
+	// create space of the instruction buffer
+	usbduxfastsub[index].insnBuffer = kmalloc(SIZEINSNBUF, GFP_KERNEL);
+	if (!(usbduxfastsub[index].insnBuffer)) {
+		printk("comedi_: usbduxfast: could not alloc space for insnBuffer\n");
+		tidy_up(&(usbduxfastsub[index]));
+		up(&start_stop_sem);
+		return -ENOMEM;
+	}
+	// setting to alternate setting 1: enabling bulk ep
+	i = usb_set_interface(usbduxfastsub[index].usbdev,
+		usbduxfastsub[index].ifnum, 1);
+	if (i < 0) {
+		printk("comedi_: usbduxfast%d: could not switch to alternate setting 1.\n", index);
+		tidy_up(&(usbduxfastsub[index]));
+		up(&start_stop_sem);
+		return -ENODEV;
+	}
+	usbduxfastsub[index].urbIn = usb_alloc_urb(0, GFP_KERNEL);
+	if (usbduxfastsub[index].urbIn == NULL) {
+		printk("comedi_: usbduxfast%d: Could not alloc. urb\n", index);
+		tidy_up(&(usbduxfastsub[index]));
+		up(&start_stop_sem);
+		return -ENOMEM;
+	}
+	usbduxfastsub[index].transfer_buffer = kmalloc(SIZEINBUF, GFP_KERNEL);
+	if (!(usbduxfastsub[index].transfer_buffer)) {
+		printk("comedi_: usbduxfast%d: could not alloc. transb.\n",
+			index);
+		tidy_up(&(usbduxfastsub[index]));
+		up(&start_stop_sem);
+		return -ENOMEM;
+	}
+	// we've reached the bottom of the function
+	usbduxfastsub[index].probed = 1;
+	up(&start_stop_sem);
+	printk("comedi_: usbduxfast%d has been successfully initialized.\n",
+		index);
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0)
+	return (void *)(&usbduxfastsub[index]);
+#else
+	// success
+	return 0;
+#endif
+}
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0)
+static void usbduxfastsub_disconnect(struct usb_device *udev, void *ptr)
+{
+	usbduxfastsub_t *usbduxfastsub_tmp = (usbduxfastsub_t *) ptr;
+#else
+static void usbduxfastsub_disconnect(struct usb_interface *intf)
+{
+	usbduxfastsub_t *usbduxfastsub_tmp = usb_get_intfdata(intf);
+	struct usb_device *udev = interface_to_usbdev(intf);
+#endif
+	if (!usbduxfastsub_tmp) {
+		printk("comedi_: usbduxfast: disconnect called with null pointer.\n");
+		return;
+	}
+	if (usbduxfastsub_tmp->usbdev != udev) {
+		printk("comedi_: usbduxfast: BUG! called with wrong ptr!!!\n");
+		return;
+	}
+	down(&start_stop_sem);
+	down(&usbduxfastsub_tmp->sem);
+	tidy_up(usbduxfastsub_tmp);
+	up(&usbduxfastsub_tmp->sem);
+	up(&start_stop_sem);
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi_: usbduxfast: disconnected from the usb\n");
+#endif
+}
+
+// is called when comedi-config is called
+static int usbduxfast_attach(comedi_device * dev, comedi_devconfig * it)
+{
+	int ret;
+	int index;
+	int i;
+	comedi_subdevice *s = NULL;
+	dev->private = NULL;
+
+	down(&start_stop_sem);
+	// find a valid device which has been detected by the probe function of the usb
+	index = -1;
+	for (i = 0; i < NUMUSBDUXFAST; i++) {
+		if ((usbduxfastsub[i].probed) && (!usbduxfastsub[i].attached)) {
+			index = i;
+			break;
+		}
+	}
+
+	if (index < 0) {
+		printk("comedi%d: usbduxfast: error: attach failed, no usbduxfast devs connected to the usb bus.\n", dev->minor);
+		up(&start_stop_sem);
+		return -ENODEV;
+	}
+
+	down(&(usbduxfastsub[index].sem));
+	// pointer back to the corresponding comedi device
+	usbduxfastsub[index].comedidev = dev;
+
+	// trying to upload the firmware into the chip
+	if (comedi_aux_data(it->options, 0) &&
+		it->options[COMEDI_DEVCONF_AUX_DATA_LENGTH]) {
+		read_firmware(usbduxfastsub,
+			comedi_aux_data(it->options, 0),
+			it->options[COMEDI_DEVCONF_AUX_DATA_LENGTH]);
+	}
+
+	dev->board_name = BOARDNAME;
+
+	/* set number of subdevices */
+	dev->n_subdevices = N_SUBDEVICES;
+
+	// allocate space for the subdevices
+	if ((ret = alloc_subdevices(dev, N_SUBDEVICES)) < 0) {
+		printk("comedi%d: usbduxfast: error alloc space for subdev\n",
+			dev->minor);
+		up(&start_stop_sem);
+		return ret;
+	}
+
+	printk("comedi%d: usbduxfast: usb-device %d is attached to comedi.\n",
+		dev->minor, index);
+	// private structure is also simply the usb-structure
+	dev->private = usbduxfastsub + index;
+	// the first subdevice is the A/D converter
+	s = dev->subdevices + SUBDEV_AD;
+	// the URBs get the comedi subdevice
+	// which is responsible for reading
+	// this is the subdevice which reads data
+	dev->read_subdev = s;
+	// the subdevice receives as private structure the
+	// usb-structure
+	s->private = NULL;
+	// analog input
+	s->type = COMEDI_SUBD_AI;
+	// readable and ref is to ground
+	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_CMD_READ;
+	// 16 channels
+	s->n_chan = 16;
+	// length of the channellist
+	s->len_chanlist = 16;
+	// callback functions
+	s->insn_read = usbduxfast_ai_insn_read;
+	s->do_cmdtest = usbduxfast_ai_cmdtest;
+	s->do_cmd = usbduxfast_ai_cmd;
+	s->cancel = usbduxfast_ai_cancel;
+	// max value from the A/D converter (12bit+1 bit for overflow)
+	s->maxdata = 0x1000;
+	// range table to convert to physical units
+	s->range_table = &range_usbduxfast_ai_range;
+
+	// finally decide that it's attached
+	usbduxfastsub[index].attached = 1;
+
+	up(&(usbduxfastsub[index].sem));
+
+	up(&start_stop_sem);
+
+	printk("comedi%d: successfully attached to usbduxfast.\n", dev->minor);
+
+	return 0;
+}
+
+static int usbduxfast_detach(comedi_device * dev)
+{
+	usbduxfastsub_t *usbduxfastsub_tmp;
+
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi%d: usbduxfast: detach usb device\n", dev->minor);
+#endif
+
+	if (!dev) {
+		printk("comedi?: usbduxfast: detach without dev variable...\n");
+		return -EFAULT;
+	}
+
+	usbduxfastsub_tmp = dev->private;
+	if (!usbduxfastsub_tmp) {
+		printk("comedi?: usbduxfast: detach without ptr to usbduxfastsub[]\n");
+		return -EFAULT;
+	}
+
+	down(&usbduxfastsub_tmp->sem);
+	down(&start_stop_sem);
+	// Don't allow detach to free the private structure
+	// It's one entry of of usbduxfastsub[]
+	dev->private = NULL;
+	usbduxfastsub_tmp->attached = 0;
+	usbduxfastsub_tmp->comedidev = NULL;
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi%d: usbduxfast: detach: successfully removed\n",
+		dev->minor);
+#endif
+	up(&start_stop_sem);
+	up(&usbduxfastsub_tmp->sem);
+	return 0;
+}
+
+/* main driver struct */
+static comedi_driver driver_usbduxfast = {
+      driver_name:"usbduxfast",
+      module:THIS_MODULE,
+      attach:usbduxfast_attach,
+      detach:usbduxfast_detach,
+};
+
+static void init_usb_devices(void)
+{
+	int index;
+#ifdef CONFIG_COMEDI_DEBUG
+	printk("comedi_: usbduxfast: setting all possible devs to invalid\n");
+#endif
+	// all devices entries are invalid to begin with
+	// they will become valid by the probe function
+	// and then finally by the attach-function
+	for (index = 0; index < NUMUSBDUXFAST; index++) {
+		memset(&(usbduxfastsub[index]), 0x00,
+			sizeof(usbduxfastsub[index]));
+		init_MUTEX(&(usbduxfastsub[index].sem));
+	}
+}
+
+// Table with the USB-devices: just now only testing IDs
+static struct usb_device_id usbduxfastsub_table[] = {
+	//        { USB_DEVICE(0x4b4, 0x8613), //testing
+	//        },
+	{USB_DEVICE(0x13d8, 0x0010)	//real ID
+		},
+	{USB_DEVICE(0x13d8, 0x0011)	//real ID
+		},
+	{}			/* Terminating entry */
+};
+
+MODULE_DEVICE_TABLE(usb, usbduxfastsub_table);
+
+// The usbduxfastsub-driver
+static struct usb_driver usbduxfastsub_driver = {
+#ifdef COMEDI_HAVE_USB_DRIVER_OWNER
+      owner:THIS_MODULE,
+#endif
+      name:BOARDNAME,
+      probe:usbduxfastsub_probe,
+      disconnect:usbduxfastsub_disconnect,
+      id_table:usbduxfastsub_table,
+};
+
+// Can't use the nice macro as I have also to initialise the USB
+// subsystem:
+// registering the usb-system _and_ the comedi-driver
+static int init_usbduxfast(void)
+{
+	printk(KERN_INFO KBUILD_MODNAME ": "
+	       DRIVER_VERSION ":" DRIVER_DESC "\n");
+	init_usb_devices();
+	usb_register(&usbduxfastsub_driver);
+	comedi_driver_register(&driver_usbduxfast);
+	return 0;
+}
+
+// deregistering the comedi driver and the usb-subsystem
+static void exit_usbduxfast(void)
+{
+	comedi_driver_unregister(&driver_usbduxfast);
+	usb_deregister(&usbduxfastsub_driver);
+}
+
+module_init(init_usbduxfast);
+module_exit(exit_usbduxfast);
+
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/interrupt.h b/drivers/staging/comedi/interrupt.h
new file mode 100644
index 00000000000..3038e461948
--- /dev/null
+++ b/drivers/staging/comedi/interrupt.h
@@ -0,0 +1,60 @@
+/*
+    linux/interrupt.h compatibility header
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __COMPAT_LINUX_INTERRUPT_H_
+#define __COMPAT_LINUX_INTERRUPT_H_
+
+#include <linux/interrupt.h>
+
+#include <linux/version.h>
+
+#ifndef IRQ_NONE
+typedef void irqreturn_t;
+#define IRQ_NONE
+#define IRQ_HANDLED
+#define IRQ_RETVAL(x) (void)(x)
+#endif
+
+#ifndef IRQF_DISABLED
+#define IRQF_DISABLED           SA_INTERRUPT
+#define IRQF_SAMPLE_RANDOM      SA_SAMPLE_RANDOM
+#define IRQF_SHARED             SA_SHIRQ
+#define IRQF_PROBE_SHARED       SA_PROBEIRQ
+#define IRQF_PERCPU             SA_PERCPU
+#ifdef SA_TRIGGER_MASK
+#define IRQF_TRIGGER_NONE       0
+#define IRQF_TRIGGER_LOW        SA_TRIGGER_LOW
+#define IRQF_TRIGGER_HIGH       SA_TRIGGER_HIGH
+#define IRQF_TRIGGER_FALLING    SA_TRIGGER_FALLING
+#define IRQF_TRIGGER_RISING     SA_TRIGGER_RISING
+#define IRQF_TRIGGER_MASK       SA_TRIGGER_MASK
+#else
+#define IRQF_TRIGGER_NONE       0
+#define IRQF_TRIGGER_LOW        0
+#define IRQF_TRIGGER_HIGH       0
+#define IRQF_TRIGGER_FALLING    0
+#define IRQF_TRIGGER_RISING     0
+#define IRQF_TRIGGER_MASK       0
+#endif
+#endif
+
+#define PT_REGS_ARG
+#define PT_REGS_CALL
+#define PT_REGS_NULL
+
+#endif
diff --git a/drivers/staging/comedi/kcomedilib/Makefile b/drivers/staging/comedi/kcomedilib/Makefile
new file mode 100644
index 00000000000..ffcc9ad32ad
--- /dev/null
+++ b/drivers/staging/comedi/kcomedilib/Makefile
@@ -0,0 +1,8 @@
+obj-$(CONFIG_COMEDI)	+= kcomedilib.o
+
+kcomedilib-objs :=				\
+			data.o			\
+			ksyms.o			\
+			dio.o			\
+			kcomedilib_main.o	\
+			get.o
diff --git a/drivers/staging/comedi/kcomedilib/data.c b/drivers/staging/comedi/kcomedilib/data.c
new file mode 100644
index 00000000000..79aec204115
--- /dev/null
+++ b/drivers/staging/comedi/kcomedilib/data.c
@@ -0,0 +1,89 @@
+/*
+    kcomedilib/data.c
+    implements comedi_data_*() functions
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 2000 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#include "../comedi.h"
+#include "../comedilib.h"
+#include "../comedidev.h"	/* for comedi_udelay() */
+
+#include <linux/string.h>
+
+int comedi_data_write(comedi_t * dev, unsigned int subdev, unsigned int chan,
+	unsigned int range, unsigned int aref, lsampl_t data)
+{
+	comedi_insn insn;
+
+	memset(&insn, 0, sizeof(insn));
+	insn.insn = INSN_WRITE;
+	insn.n = 1;
+	insn.data = &data;
+	insn.subdev = subdev;
+	insn.chanspec = CR_PACK(chan, range, aref);
+
+	return comedi_do_insn(dev, &insn);
+}
+
+int comedi_data_read(comedi_t * dev, unsigned int subdev, unsigned int chan,
+	unsigned int range, unsigned int aref, lsampl_t * data)
+{
+	comedi_insn insn;
+
+	memset(&insn, 0, sizeof(insn));
+	insn.insn = INSN_READ;
+	insn.n = 1;
+	insn.data = data;
+	insn.subdev = subdev;
+	insn.chanspec = CR_PACK(chan, range, aref);
+
+	return comedi_do_insn(dev, &insn);
+}
+
+int comedi_data_read_hint(comedi_t * dev, unsigned int subdev,
+	unsigned int chan, unsigned int range, unsigned int aref)
+{
+	comedi_insn insn;
+	lsampl_t dummy_data;
+
+	memset(&insn, 0, sizeof(insn));
+	insn.insn = INSN_READ;
+	insn.n = 0;
+	insn.data = &dummy_data;
+	insn.subdev = subdev;
+	insn.chanspec = CR_PACK(chan, range, aref);
+
+	return comedi_do_insn(dev, &insn);
+}
+
+int comedi_data_read_delayed(comedi_t * dev, unsigned int subdev,
+	unsigned int chan, unsigned int range, unsigned int aref,
+	lsampl_t * data, unsigned int nano_sec)
+{
+	int retval;
+
+	retval = comedi_data_read_hint(dev, subdev, chan, range, aref);
+	if (retval < 0)
+		return retval;
+
+	comedi_udelay((nano_sec + 999) / 1000);
+
+	return comedi_data_read(dev, subdev, chan, range, aref, data);
+}
diff --git a/drivers/staging/comedi/kcomedilib/dio.c b/drivers/staging/comedi/kcomedilib/dio.c
new file mode 100644
index 00000000000..a9f488aed36
--- /dev/null
+++ b/drivers/staging/comedi/kcomedilib/dio.c
@@ -0,0 +1,95 @@
+/*
+    kcomedilib/dio.c
+    implements comedi_dio_*() functions
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 2000 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#include "../comedi.h"
+#include "../comedilib.h"
+
+#include <linux/string.h>
+
+int comedi_dio_config(comedi_t * dev, unsigned int subdev, unsigned int chan,
+	unsigned int io)
+{
+	comedi_insn insn;
+
+	memset(&insn, 0, sizeof(insn));
+	insn.insn = INSN_CONFIG;
+	insn.n = 1;
+	insn.data = &io;
+	insn.subdev = subdev;
+	insn.chanspec = CR_PACK(chan, 0, 0);
+
+	return comedi_do_insn(dev, &insn);
+}
+
+int comedi_dio_read(comedi_t * dev, unsigned int subdev, unsigned int chan,
+	unsigned int *val)
+{
+	comedi_insn insn;
+
+	memset(&insn, 0, sizeof(insn));
+	insn.insn = INSN_READ;
+	insn.n = 1;
+	insn.data = val;
+	insn.subdev = subdev;
+	insn.chanspec = CR_PACK(chan, 0, 0);
+
+	return comedi_do_insn(dev, &insn);
+}
+
+int comedi_dio_write(comedi_t * dev, unsigned int subdev, unsigned int chan,
+	unsigned int val)
+{
+	comedi_insn insn;
+
+	memset(&insn, 0, sizeof(insn));
+	insn.insn = INSN_WRITE;
+	insn.n = 1;
+	insn.data = &val;
+	insn.subdev = subdev;
+	insn.chanspec = CR_PACK(chan, 0, 0);
+
+	return comedi_do_insn(dev, &insn);
+}
+
+int comedi_dio_bitfield(comedi_t * dev, unsigned int subdev, unsigned int mask,
+	unsigned int *bits)
+{
+	comedi_insn insn;
+	lsampl_t data[2];
+	int ret;
+
+	memset(&insn, 0, sizeof(insn));
+	insn.insn = INSN_BITS;
+	insn.n = 2;
+	insn.data = data;
+	insn.subdev = subdev;
+
+	data[0] = mask;
+	data[1] = *bits;
+
+	ret = comedi_do_insn(dev, &insn);
+
+	*bits = data[1];
+
+	return ret;
+}
diff --git a/drivers/staging/comedi/kcomedilib/get.c b/drivers/staging/comedi/kcomedilib/get.c
new file mode 100644
index 00000000000..2004ad4480c
--- /dev/null
+++ b/drivers/staging/comedi/kcomedilib/get.c
@@ -0,0 +1,294 @@
+/*
+    kcomedilib/get.c
+    a comedlib interface for kernel modules
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1997-2000 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#define __NO_VERSION__
+#include "../comedi.h"
+#include "../comedilib.h"
+#include "../comedidev.h"
+
+int comedi_get_n_subdevices(comedi_t * d)
+{
+	comedi_device *dev = (comedi_device *) d;
+
+	return dev->n_subdevices;
+}
+
+int comedi_get_version_code(comedi_t * d)
+{
+	return COMEDI_VERSION_CODE;
+}
+
+const char *comedi_get_driver_name(comedi_t * d)
+{
+	comedi_device *dev = (comedi_device *) d;
+
+	return dev->driver->driver_name;
+}
+
+const char *comedi_get_board_name(comedi_t * d)
+{
+	comedi_device *dev = (comedi_device *) d;
+
+	return dev->board_name;
+}
+
+int comedi_get_subdevice_type(comedi_t * d, unsigned int subdevice)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s = dev->subdevices + subdevice;
+
+	return s->type;
+}
+
+unsigned int comedi_get_subdevice_flags(comedi_t * d, unsigned int subdevice)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s = dev->subdevices + subdevice;
+
+	return s->subdev_flags;
+}
+
+int comedi_find_subdevice_by_type(comedi_t * d, int type, unsigned int subd)
+{
+	comedi_device *dev = (comedi_device *) d;
+
+	if (subd > dev->n_subdevices)
+		return -ENODEV;
+
+	for (; subd < dev->n_subdevices; subd++) {
+		if (dev->subdevices[subd].type == type)
+			return subd;
+	}
+	return -1;
+}
+
+int comedi_get_n_channels(comedi_t * d, unsigned int subdevice)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s = dev->subdevices + subdevice;
+
+	return s->n_chan;
+}
+
+int comedi_get_len_chanlist(comedi_t * d, unsigned int subdevice)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s = dev->subdevices + subdevice;
+
+	return s->len_chanlist;
+}
+
+lsampl_t comedi_get_maxdata(comedi_t * d, unsigned int subdevice,
+	unsigned int chan)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s = dev->subdevices + subdevice;
+
+	if (s->maxdata_list)
+		return s->maxdata_list[chan];
+
+	return s->maxdata;
+}
+
+#ifdef KCOMEDILIB_DEPRECATED
+int comedi_get_rangetype(comedi_t * d, unsigned int subdevice,
+	unsigned int chan)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s = dev->subdevices + subdevice;
+	int ret;
+
+	if (s->range_table_list) {
+		ret = s->range_table_list[chan]->length;
+	} else {
+		ret = s->range_table->length;
+	}
+
+	ret = ret | (dev->minor << 28) | (subdevice << 24) | (chan << 16);
+
+	return ret;
+}
+#endif
+
+int comedi_get_n_ranges(comedi_t * d, unsigned int subdevice, unsigned int chan)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s = dev->subdevices + subdevice;
+	int ret;
+
+	if (s->range_table_list) {
+		ret = s->range_table_list[chan]->length;
+	} else {
+		ret = s->range_table->length;
+	}
+
+	return ret;
+}
+
+/*
+ * ALPHA (non-portable)
+*/
+int comedi_get_krange(comedi_t * d, unsigned int subdevice, unsigned int chan,
+	unsigned int range, comedi_krange * krange)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s = dev->subdevices + subdevice;
+	const comedi_lrange *lr;
+
+	if (s->range_table_list) {
+		lr = s->range_table_list[chan];
+	} else {
+		lr = s->range_table;
+	}
+	if (range >= lr->length) {
+		return -EINVAL;
+	}
+	memcpy(krange, lr->range + range, sizeof(comedi_krange));
+
+	return 0;
+}
+
+/*
+ * ALPHA (may be renamed)
+*/
+unsigned int comedi_get_buf_head_pos(comedi_t * d, unsigned int subdevice)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s = dev->subdevices + subdevice;
+	comedi_async *async;
+
+	async = s->async;
+	if (async == NULL)
+		return 0;
+
+	return async->buf_write_count;
+}
+
+int comedi_get_buffer_contents(comedi_t * d, unsigned int subdevice)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s = dev->subdevices + subdevice;
+	comedi_async *async;
+	unsigned int num_bytes;
+
+	if (subdevice >= dev->n_subdevices)
+		return -1;
+	async = s->async;
+	if (async == NULL)
+		return 0;
+	num_bytes = comedi_buf_read_n_available(s->async);
+	return num_bytes;
+}
+
+/*
+ * ALPHA
+*/
+int comedi_set_user_int_count(comedi_t * d, unsigned int subdevice,
+	unsigned int buf_user_count)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s = dev->subdevices + subdevice;
+	comedi_async *async;
+	int num_bytes;
+
+	async = s->async;
+	if (async == NULL)
+		return -1;
+
+	num_bytes = buf_user_count - async->buf_read_count;
+	if (num_bytes < 0)
+		return -1;
+	comedi_buf_read_alloc(async, num_bytes);
+	comedi_buf_read_free(async, num_bytes);
+
+	return 0;
+}
+
+int comedi_mark_buffer_read(comedi_t * d, unsigned int subdevice,
+	unsigned int num_bytes)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s = dev->subdevices + subdevice;
+	comedi_async *async;
+
+	if (subdevice >= dev->n_subdevices)
+		return -1;
+	async = s->async;
+	if (async == NULL)
+		return -1;
+
+	comedi_buf_read_alloc(async, num_bytes);
+	comedi_buf_read_free(async, num_bytes);
+
+	return 0;
+}
+
+int comedi_mark_buffer_written(comedi_t * d, unsigned int subdevice,
+	unsigned int num_bytes)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s = dev->subdevices + subdevice;
+	comedi_async *async;
+	int bytes_written;
+
+	if (subdevice >= dev->n_subdevices)
+		return -1;
+	async = s->async;
+	if (async == NULL)
+		return -1;
+	bytes_written = comedi_buf_write_alloc(async, num_bytes);
+	comedi_buf_write_free(async, bytes_written);
+	if (bytes_written != num_bytes)
+		return -1;
+	return 0;
+}
+
+int comedi_get_buffer_size(comedi_t * d, unsigned int subdev)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s = dev->subdevices + subdev;
+	comedi_async *async;
+
+	if (subdev >= dev->n_subdevices)
+		return -1;
+	async = s->async;
+	if (async == NULL)
+		return 0;
+
+	return async->prealloc_bufsz;
+}
+
+int comedi_get_buffer_offset(comedi_t * d, unsigned int subdevice)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s = dev->subdevices + subdevice;
+	comedi_async *async;
+
+	if (subdevice >= dev->n_subdevices)
+		return -1;
+	async = s->async;
+	if (async == NULL)
+		return 0;
+
+	return async->buf_read_ptr;
+}
diff --git a/drivers/staging/comedi/kcomedilib/kcomedilib_main.c b/drivers/staging/comedi/kcomedilib/kcomedilib_main.c
new file mode 100644
index 00000000000..510fbdcec49
--- /dev/null
+++ b/drivers/staging/comedi/kcomedilib/kcomedilib_main.c
@@ -0,0 +1,567 @@
+/*
+    kcomedilib/kcomedilib.c
+    a comedlib interface for kernel modules
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1997-2000 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#define __NO_VERSION__
+#include <linux/module.h>
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/fcntl.h>
+#include <linux/delay.h>
+#include <linux/ioport.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+#include <asm/io.h>
+
+#include "../comedi.h"
+#include "../comedilib.h"
+#include "../comedidev.h"
+
+MODULE_AUTHOR("David Schleef <ds@schleef.org>");
+MODULE_DESCRIPTION("Comedi kernel library");
+MODULE_LICENSE("GPL");
+
+comedi_t *comedi_open(const char *filename)
+{
+	struct comedi_device_file_info *dev_file_info;
+	comedi_device *dev;
+	unsigned int minor;
+
+	if (strncmp(filename, "/dev/comedi", 11) != 0)
+		return NULL;
+
+	minor = simple_strtoul(filename + 11, NULL, 0);
+
+	if (minor >= COMEDI_NUM_BOARD_MINORS)
+		return NULL;
+
+	dev_file_info = comedi_get_device_file_info(minor);
+	if(dev_file_info == NULL)
+		return NULL;
+	dev = dev_file_info->device;
+
+	if(dev == NULL || !dev->attached)
+		return NULL;
+
+	if (!try_module_get(dev->driver->module))
+		return NULL;
+
+	return (comedi_t *) dev;
+}
+
+comedi_t *comedi_open_old(unsigned int minor)
+{
+	struct comedi_device_file_info *dev_file_info;
+	comedi_device *dev;
+
+	if (minor >= COMEDI_NUM_MINORS)
+		return NULL;
+
+	dev_file_info = comedi_get_device_file_info(minor);
+	if(dev_file_info == NULL)
+		return NULL;
+	dev = dev_file_info->device;
+
+	if(dev == NULL || !dev->attached)
+		return NULL;
+
+	return (comedi_t *) dev;
+}
+
+int comedi_close(comedi_t * d)
+{
+	comedi_device *dev = (comedi_device *) d;
+
+	module_put(dev->driver->module);
+
+	return 0;
+}
+
+int comedi_loglevel(int newlevel)
+{
+	return 0;
+}
+
+void comedi_perror(const char *message)
+{
+	rt_printk("%s: unknown error\n", message);
+}
+
+char *comedi_strerror(int err)
+{
+	return "unknown error";
+}
+
+int comedi_fileno(comedi_t * d)
+{
+	comedi_device *dev = (comedi_device *) d;
+
+	/* return something random */
+	return dev->minor;
+}
+
+int comedi_command(comedi_t * d, comedi_cmd * cmd)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s;
+	comedi_async *async;
+	unsigned runflags;
+
+	if (cmd->subdev >= dev->n_subdevices)
+		return -ENODEV;
+
+	s = dev->subdevices + cmd->subdev;
+	if (s->type == COMEDI_SUBD_UNUSED)
+		return -EIO;
+
+	async = s->async;
+	if (async == NULL)
+		return -ENODEV;
+
+	if (s->busy)
+		return -EBUSY;
+	s->busy = d;
+
+	if (async->cb_mask & COMEDI_CB_EOS)
+		cmd->flags |= TRIG_WAKE_EOS;
+
+	async->cmd = *cmd;
+
+	runflags = SRF_RUNNING;
+
+#ifdef CONFIG_COMEDI_RT
+	if (comedi_switch_to_rt(dev) == 0)
+		runflags |= SRF_RT;
+#endif
+	comedi_set_subdevice_runflags(s, ~0, runflags);
+
+	comedi_reset_async_buf(async);
+
+	return s->do_cmd(dev, s);
+}
+
+int comedi_command_test(comedi_t * d, comedi_cmd * cmd)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s;
+
+	if (cmd->subdev >= dev->n_subdevices)
+		return -ENODEV;
+
+	s = dev->subdevices + cmd->subdev;
+	if (s->type == COMEDI_SUBD_UNUSED)
+		return -EIO;
+
+	if (s->async == NULL)
+		return -ENODEV;
+
+	return s->do_cmdtest(dev, s, cmd);
+}
+
+/*
+ *	COMEDI_INSN
+ *	perform an instruction
+ */
+int comedi_do_insn(comedi_t * d, comedi_insn * insn)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s;
+	int ret = 0;
+
+	if (insn->insn & INSN_MASK_SPECIAL) {
+		switch (insn->insn) {
+		case INSN_GTOD:
+			{
+				struct timeval tv;
+
+				do_gettimeofday(&tv);
+				insn->data[0] = tv.tv_sec;
+				insn->data[1] = tv.tv_usec;
+				ret = 2;
+
+				break;
+			}
+		case INSN_WAIT:
+			/* XXX isn't the value supposed to be nanosecs? */
+			if (insn->n != 1 || insn->data[0] >= 100) {
+				ret = -EINVAL;
+				break;
+			}
+			comedi_udelay(insn->data[0]);
+			ret = 1;
+			break;
+		case INSN_INTTRIG:
+			if (insn->n != 1) {
+				ret = -EINVAL;
+				break;
+			}
+			if (insn->subdev >= dev->n_subdevices) {
+				rt_printk("%d not usable subdevice\n",
+					insn->subdev);
+				ret = -EINVAL;
+				break;
+			}
+			s = dev->subdevices + insn->subdev;
+			if (!s->async) {
+				rt_printk("no async\n");
+				ret = -EINVAL;
+				break;
+			}
+			if (!s->async->inttrig) {
+				rt_printk("no inttrig\n");
+				ret = -EAGAIN;
+				break;
+			}
+			ret = s->async->inttrig(dev, s, insn->data[0]);
+			if (ret >= 0)
+				ret = 1;
+			break;
+		default:
+			ret = -EINVAL;
+		}
+	} else {
+		/* a subdevice instruction */
+		if (insn->subdev >= dev->n_subdevices) {
+			ret = -EINVAL;
+			goto error;
+		}
+		s = dev->subdevices + insn->subdev;
+
+		if (s->type == COMEDI_SUBD_UNUSED) {
+			rt_printk("%d not useable subdevice\n", insn->subdev);
+			ret = -EIO;
+			goto error;
+		}
+
+		/* XXX check lock */
+
+		if ((ret = check_chanlist(s, 1, &insn->chanspec)) < 0) {
+			rt_printk("bad chanspec\n");
+			ret = -EINVAL;
+			goto error;
+		}
+
+		if (s->busy) {
+			ret = -EBUSY;
+			goto error;
+		}
+		s->busy = d;
+
+		switch (insn->insn) {
+		case INSN_READ:
+			ret = s->insn_read(dev, s, insn, insn->data);
+			break;
+		case INSN_WRITE:
+			ret = s->insn_write(dev, s, insn, insn->data);
+			break;
+		case INSN_BITS:
+			ret = s->insn_bits(dev, s, insn, insn->data);
+			break;
+		case INSN_CONFIG:
+			/* XXX should check instruction length */
+			ret = s->insn_config(dev, s, insn, insn->data);
+			break;
+		default:
+			ret = -EINVAL;
+			break;
+		}
+
+		s->busy = NULL;
+	}
+	if (ret < 0)
+		goto error;
+#if 0
+	/* XXX do we want this? -- abbotti #if'ed it out for now. */
+	if (ret != insn->n) {
+		rt_printk("BUG: result of insn != insn.n\n");
+		ret = -EINVAL;
+		goto error;
+	}
+#endif
+      error:
+
+	return ret;
+}
+
+/*
+	COMEDI_LOCK
+	lock subdevice
+
+	arg:
+		subdevice number
+
+	reads:
+		none
+
+	writes:
+		none
+
+	necessary locking:
+	- ioctl/rt lock  (this type)
+	- lock while subdevice busy
+	- lock while subdevice being programmed
+
+*/
+int comedi_lock(comedi_t * d, unsigned int subdevice)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s;
+	unsigned long flags;
+	int ret = 0;
+
+	if (subdevice >= dev->n_subdevices) {
+		return -EINVAL;
+	}
+	s = dev->subdevices + subdevice;
+
+	comedi_spin_lock_irqsave(&s->spin_lock, flags);
+
+	if (s->busy) {
+		ret = -EBUSY;
+	} else {
+		if (s->lock) {
+			ret = -EBUSY;
+		} else {
+			s->lock = d;
+		}
+	}
+
+	comedi_spin_unlock_irqrestore(&s->spin_lock, flags);
+
+	return ret;
+}
+
+/*
+	COMEDI_UNLOCK
+	unlock subdevice
+
+	arg:
+		subdevice number
+
+	reads:
+		none
+
+	writes:
+		none
+
+*/
+int comedi_unlock(comedi_t * d, unsigned int subdevice)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s;
+	unsigned long flags;
+	comedi_async *async;
+	int ret;
+
+	if (subdevice >= dev->n_subdevices) {
+		return -EINVAL;
+	}
+	s = dev->subdevices + subdevice;
+
+	async = s->async;
+
+	comedi_spin_lock_irqsave(&s->spin_lock, flags);
+
+	if (s->busy) {
+		ret = -EBUSY;
+	} else if (s->lock && s->lock != (void *)d) {
+		ret = -EACCES;
+	} else {
+		s->lock = NULL;
+
+		if (async) {
+			async->cb_mask = 0;
+			async->cb_func = NULL;
+			async->cb_arg = NULL;
+		}
+
+		ret = 0;
+	}
+
+	comedi_spin_unlock_irqrestore(&s->spin_lock, flags);
+
+	return ret;
+}
+
+/*
+	COMEDI_CANCEL
+	cancel acquisition ioctl
+
+	arg:
+		subdevice number
+
+	reads:
+		nothing
+
+	writes:
+		nothing
+
+*/
+int comedi_cancel(comedi_t * d, unsigned int subdevice)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s;
+	int ret = 0;
+
+	if (subdevice >= dev->n_subdevices) {
+		return -EINVAL;
+	}
+	s = dev->subdevices + subdevice;
+
+	if (s->lock && s->lock != d)
+		return -EACCES;
+
+#if 0
+	if (!s->busy)
+		return 0;
+
+	if (s->busy != d)
+		return -EBUSY;
+#endif
+
+	if (!s->cancel || !s->async)
+		return -EINVAL;
+
+	if ((ret = s->cancel(dev, s)))
+		return ret;
+
+#ifdef CONFIG_COMEDI_RT
+	if (comedi_get_subdevice_runflags(s) & SRF_RT) {
+		comedi_switch_to_non_rt(dev);
+	}
+#endif
+	comedi_set_subdevice_runflags(s, SRF_RUNNING | SRF_RT, 0);
+	s->async->inttrig = NULL;
+	s->busy = NULL;
+
+	return 0;
+}
+
+/*
+   registration of callback functions
+ */
+int comedi_register_callback(comedi_t * d, unsigned int subdevice,
+	unsigned int mask, int (*cb) (unsigned int, void *), void *arg)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s;
+	comedi_async *async;
+
+	if (subdevice >= dev->n_subdevices) {
+		return -EINVAL;
+	}
+	s = dev->subdevices + subdevice;
+
+	async = s->async;
+	if (s->type == COMEDI_SUBD_UNUSED || !async)
+		return -EIO;
+
+	/* are we locked? (ioctl lock) */
+	if (s->lock && s->lock != d)
+		return -EACCES;
+
+	/* are we busy? */
+	if (s->busy)
+		return -EBUSY;
+
+	if (!mask) {
+		async->cb_mask = 0;
+		async->cb_func = NULL;
+		async->cb_arg = NULL;
+	} else {
+		async->cb_mask = mask;
+		async->cb_func = cb;
+		async->cb_arg = arg;
+	}
+
+	return 0;
+}
+
+int comedi_poll(comedi_t * d, unsigned int subdevice)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s = dev->subdevices;
+	comedi_async *async;
+
+	if (subdevice >= dev->n_subdevices) {
+		return -EINVAL;
+	}
+	s = dev->subdevices + subdevice;
+
+	async = s->async;
+	if (s->type == COMEDI_SUBD_UNUSED || !async)
+		return -EIO;
+
+	/* are we locked? (ioctl lock) */
+	if (s->lock && s->lock != d)
+		return -EACCES;
+
+	/* are we running? XXX wrong? */
+	if (!s->busy)
+		return -EIO;
+
+	return s->poll(dev, s);
+}
+
+/* WARNING: not portable */
+int comedi_map(comedi_t * d, unsigned int subdevice, void *ptr)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s;
+
+	if (subdevice >= dev->n_subdevices) {
+		return -EINVAL;
+	}
+	s = dev->subdevices + subdevice;
+
+	if (!s->async)
+		return -EINVAL;
+
+	if (ptr) {
+		*((void **)ptr) = s->async->prealloc_buf;
+	}
+
+	/* XXX no reference counting */
+
+	return 0;
+}
+
+/* WARNING: not portable */
+int comedi_unmap(comedi_t * d, unsigned int subdevice)
+{
+	comedi_device *dev = (comedi_device *) d;
+	comedi_subdevice *s;
+
+	if (subdevice >= dev->n_subdevices) {
+		return -EINVAL;
+	}
+	s = dev->subdevices + subdevice;
+
+	if (!s->async)
+		return -EINVAL;
+
+	/* XXX no reference counting */
+
+	return 0;
+}
diff --git a/drivers/staging/comedi/kcomedilib/ksyms.c b/drivers/staging/comedi/kcomedilib/ksyms.c
new file mode 100644
index 00000000000..76b45063a9f
--- /dev/null
+++ b/drivers/staging/comedi/kcomedilib/ksyms.c
@@ -0,0 +1,144 @@
+/*
+    comedi/kcomedilib/ksyms.c
+    a comedlib interface for kernel modules
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1997-2001 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#ifndef EXPORT_SYMTAB
+#define EXPORT_SYMTAB
+#endif
+
+#include "../comedi.h"
+#include "../comedilib.h"
+#include "../comedidev.h"
+
+#include <linux/module.h>
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/fcntl.h>
+#include <linux/delay.h>
+#include <linux/ioport.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+
+#if LINUX_VERSION_CODE >= 0x020200
+
+/* functions specific to kcomedilib */
+
+EXPORT_SYMBOL(comedi_register_callback);
+EXPORT_SYMBOL(comedi_get_krange);
+EXPORT_SYMBOL(comedi_get_buf_head_pos);
+EXPORT_SYMBOL(comedi_set_user_int_count);
+EXPORT_SYMBOL(comedi_map);
+EXPORT_SYMBOL(comedi_unmap);
+
+/* This list comes from user-space comedilib, to show which
+ * functions are not ported yet. */
+
+EXPORT_SYMBOL(comedi_open);
+EXPORT_SYMBOL(comedi_close);
+
+/* logging */
+EXPORT_SYMBOL(comedi_loglevel);
+EXPORT_SYMBOL(comedi_perror);
+EXPORT_SYMBOL(comedi_strerror);
+//EXPORT_SYMBOL(comedi_errno);
+EXPORT_SYMBOL(comedi_fileno);
+
+/* device queries */
+EXPORT_SYMBOL(comedi_get_n_subdevices);
+EXPORT_SYMBOL(comedi_get_version_code);
+EXPORT_SYMBOL(comedi_get_driver_name);
+EXPORT_SYMBOL(comedi_get_board_name);
+
+/* subdevice queries */
+EXPORT_SYMBOL(comedi_get_subdevice_type);
+EXPORT_SYMBOL(comedi_find_subdevice_by_type);
+EXPORT_SYMBOL(comedi_get_subdevice_flags);
+EXPORT_SYMBOL(comedi_get_n_channels);
+//EXPORT_SYMBOL(comedi_range_is_chan_specific);
+//EXPORT_SYMBOL(comedi_maxdata_is_chan_specific);
+
+/* channel queries */
+EXPORT_SYMBOL(comedi_get_maxdata);
+#ifdef KCOMEDILIB_DEPRECATED
+EXPORT_SYMBOL(comedi_get_rangetype);
+#endif
+EXPORT_SYMBOL(comedi_get_n_ranges);
+//EXPORT_SYMBOL(comedi_find_range);
+
+/* buffer queries */
+EXPORT_SYMBOL(comedi_get_buffer_size);
+//EXPORT_SYMBOL(comedi_get_max_buffer_size);
+//EXPORT_SYMBOL(comedi_set_buffer_size);
+EXPORT_SYMBOL(comedi_get_buffer_contents);
+EXPORT_SYMBOL(comedi_get_buffer_offset);
+
+/* low-level stuff */
+//EXPORT_SYMBOL(comedi_trigger);
+//EXPORT_SYMBOL(comedi_do_insnlist);
+EXPORT_SYMBOL(comedi_do_insn);
+EXPORT_SYMBOL(comedi_lock);
+EXPORT_SYMBOL(comedi_unlock);
+
+/* physical units */
+//EXPORT_SYMBOL(comedi_to_phys);
+//EXPORT_SYMBOL(comedi_from_phys);
+
+/* synchronous stuff */
+EXPORT_SYMBOL(comedi_data_read);
+EXPORT_SYMBOL(comedi_data_read_hint);
+EXPORT_SYMBOL(comedi_data_read_delayed);
+EXPORT_SYMBOL(comedi_data_write);
+EXPORT_SYMBOL(comedi_dio_config);
+EXPORT_SYMBOL(comedi_dio_read);
+EXPORT_SYMBOL(comedi_dio_write);
+EXPORT_SYMBOL(comedi_dio_bitfield);
+
+/* slowly varying stuff */
+//EXPORT_SYMBOL(comedi_sv_init);
+//EXPORT_SYMBOL(comedi_sv_update);
+//EXPORT_SYMBOL(comedi_sv_measure);
+
+/* commands */
+//EXPORT_SYMBOL(comedi_get_cmd_src_mask);
+//EXPORT_SYMBOL(comedi_get_cmd_generic_timed);
+EXPORT_SYMBOL(comedi_cancel);
+EXPORT_SYMBOL(comedi_command);
+EXPORT_SYMBOL(comedi_command_test);
+EXPORT_SYMBOL(comedi_poll);
+
+/* buffer configuration */
+EXPORT_SYMBOL(comedi_mark_buffer_read);
+EXPORT_SYMBOL(comedi_mark_buffer_written);
+
+//EXPORT_SYMBOL(comedi_get_range);
+EXPORT_SYMBOL(comedi_get_len_chanlist);
+
+/* deprecated */
+//EXPORT_SYMBOL(comedi_get_timer);
+//EXPORT_SYMBOL(comedi_timed_1chan);
+
+/* alpha */
+//EXPORT_SYMBOL(comedi_set_global_oor_behavior);
+
+#endif
diff --git a/drivers/staging/comedi/pci_ids.h b/drivers/staging/comedi/pci_ids.h
new file mode 100644
index 00000000000..c61ba90f960
--- /dev/null
+++ b/drivers/staging/comedi/pci_ids.h
@@ -0,0 +1,31 @@
+/***************************************************************************
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ ***************************************************************************/
+
+#ifndef __COMPAT_LINUX_PCI_IDS_H
+#define __COMPAT_LINUX_PCI_IDS_H
+
+#include <linux/pci_ids.h>
+
+#ifndef PCI_VENDOR_ID_AMCC
+#define PCI_VENDOR_ID_AMCC	0x10e8
+#endif
+
+#ifndef PCI_VENDOR_ID_CBOARDS
+#define PCI_VENDOR_ID_CBOARDS	0x1307
+#endif
+
+#ifndef PCI_VENDOR_ID_QUANCOM
+#define PCI_VENDOR_ID_QUANCOM	0x8008
+#endif
+
+#ifndef PCI_DEVICE_ID_QUANCOM_GPIB
+#define PCI_DEVICE_ID_QUANCOM_GPIB	0x3302
+#endif
+
+#endif // __COMPAT_LINUX_PCI_IDS_H
diff --git a/drivers/staging/comedi/proc.c b/drivers/staging/comedi/proc.c
new file mode 100644
index 00000000000..5a2b72d8757
--- /dev/null
+++ b/drivers/staging/comedi/proc.c
@@ -0,0 +1,102 @@
+/*
+    module/proc.c
+    /proc interface for comedi
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1998 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+/*
+	This is some serious bloatware.
+
+	Taken from Dave A.'s PCL-711 driver, 'cuz I thought it
+	was cool.
+*/
+
+#define __NO_VERSION__
+#include "comedidev.h"
+#include <linux/proc_fs.h>
+//#include <linux/string.h>
+
+int comedi_read_procmem(char *buf, char **start, off_t offset, int len,
+	int *eof, void *data);
+
+extern comedi_driver *comedi_drivers;
+
+int comedi_read_procmem(char *buf, char **start, off_t offset, int len,
+	int *eof, void *data)
+{
+	int i;
+	int devices_q = 0;
+	int l = 0;
+	comedi_driver *driv;
+
+	l += sprintf(buf + l,
+		"comedi version " COMEDI_RELEASE "\n"
+		"format string: %s\n",
+		"\"%2d: %-20s %-20s %4d\",i,driver_name,board_name,n_subdevices");
+
+	for (i = 0; i < COMEDI_NUM_BOARD_MINORS; i++) {
+		struct comedi_device_file_info *dev_file_info = comedi_get_device_file_info(i);
+		comedi_device *dev;
+
+		if(dev_file_info == NULL) continue;
+		dev = dev_file_info->device;
+
+		if (dev->attached) {
+			devices_q = 1;
+			l += sprintf(buf + l, "%2d: %-20s %-20s %4d\n",
+				i,
+				dev->driver->driver_name,
+				dev->board_name, dev->n_subdevices);
+		}
+	}
+	if (!devices_q) {
+		l += sprintf(buf + l, "no devices\n");
+	}
+
+	for (driv = comedi_drivers; driv; driv = driv->next) {
+		l += sprintf(buf + l, "%s:\n", driv->driver_name);
+		for (i = 0; i < driv->num_names; i++) {
+			l += sprintf(buf + l, " %s\n",
+				*(char **)((char *)driv->board_name +
+					i * driv->offset));
+		}
+		if (!driv->num_names) {
+			l += sprintf(buf + l, " %s\n", driv->driver_name);
+		}
+	}
+
+	return l;
+}
+
+#ifdef CONFIG_PROC_FS
+void comedi_proc_init(void)
+{
+	struct proc_dir_entry *comedi_proc;
+
+	comedi_proc = create_proc_entry("comedi", S_IFREG | S_IRUGO, 0);
+	if (comedi_proc)
+		comedi_proc->read_proc = comedi_read_procmem;
+}
+
+void comedi_proc_cleanup(void)
+{
+	remove_proc_entry("comedi", 0);
+}
+#endif
diff --git a/drivers/staging/comedi/range.c b/drivers/staging/comedi/range.c
new file mode 100644
index 00000000000..61dc3cd6a9f
--- /dev/null
+++ b/drivers/staging/comedi/range.c
@@ -0,0 +1,161 @@
+/*
+    module/range.c
+    comedi routines for voltage ranges
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1997-8 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#include "comedidev.h"
+#include <asm/uaccess.h>
+
+const comedi_lrange range_bipolar10 = { 1, {BIP_RANGE(10)} };
+const comedi_lrange range_bipolar5 = { 1, {BIP_RANGE(5)} };
+const comedi_lrange range_bipolar2_5 = { 1, {BIP_RANGE(2.5)} };
+const comedi_lrange range_unipolar10 = { 1, {UNI_RANGE(10)} };
+const comedi_lrange range_unipolar5 = { 1, {UNI_RANGE(5)} };
+const comedi_lrange range_unknown = { 1, {{0, 1000000, UNIT_none}} };
+
+/*
+   	COMEDI_RANGEINFO
+	range information ioctl
+
+	arg:
+		pointer to rangeinfo structure
+
+	reads:
+		range info structure
+
+	writes:
+		n comedi_krange structures to rangeinfo->range_ptr
+*/
+int do_rangeinfo_ioctl(comedi_device * dev, comedi_rangeinfo * arg)
+{
+	comedi_rangeinfo it;
+	int subd, chan;
+	const comedi_lrange *lr;
+	comedi_subdevice *s;
+
+	if (copy_from_user(&it, arg, sizeof(comedi_rangeinfo)))
+		return -EFAULT;
+	subd = (it.range_type >> 24) & 0xf;
+	chan = (it.range_type >> 16) & 0xff;
+
+	if (!dev->attached)
+		return -EINVAL;
+	if (subd >= dev->n_subdevices)
+		return -EINVAL;
+	s = dev->subdevices + subd;
+	if (s->range_table) {
+		lr = s->range_table;
+	} else if (s->range_table_list) {
+		if (chan >= s->n_chan)
+			return -EINVAL;
+		lr = s->range_table_list[chan];
+	} else {
+		return -EINVAL;
+	}
+
+	if (RANGE_LENGTH(it.range_type) != lr->length) {
+		DPRINTK("wrong length %d should be %d (0x%08x)\n",
+			RANGE_LENGTH(it.range_type), lr->length, it.range_type);
+		return -EINVAL;
+	}
+
+	if (copy_to_user(it.range_ptr, lr->range,
+			sizeof(comedi_krange) * lr->length))
+		return -EFAULT;
+
+	return 0;
+}
+
+static int aref_invalid(comedi_subdevice * s, unsigned int chanspec)
+{
+	unsigned int aref;
+
+	// disable reporting invalid arefs... maybe someday
+	return 0;
+
+	aref = CR_AREF(chanspec);
+	switch (aref) {
+	case AREF_DIFF:
+		if (s->subdev_flags & SDF_DIFF)
+			return 0;
+		break;
+	case AREF_COMMON:
+		if (s->subdev_flags & SDF_COMMON)
+			return 0;
+		break;
+	case AREF_GROUND:
+		if (s->subdev_flags & SDF_GROUND)
+			return 0;
+		break;
+	case AREF_OTHER:
+		if (s->subdev_flags & SDF_OTHER)
+			return 0;
+		break;
+	default:
+		break;
+	}
+	DPRINTK("subdevice does not support aref %i", aref);
+	return 1;
+}
+
+/*
+   This function checks each element in a channel/gain list to make
+   make sure it is valid.
+*/
+int check_chanlist(comedi_subdevice * s, int n, unsigned int *chanlist)
+{
+	int i;
+	int chan;
+
+	if (s->range_table) {
+		for (i = 0; i < n; i++)
+			if (CR_CHAN(chanlist[i]) >= s->n_chan ||
+				CR_RANGE(chanlist[i]) >= s->range_table->length
+				|| aref_invalid(s, chanlist[i])) {
+				rt_printk
+					("bad chanlist[%d]=0x%08x n_chan=%d range length=%d\n",
+					i, chanlist[i], s->n_chan,
+					s->range_table->length);
+#if 0
+				for (i = 0; i < n; i++) {
+					printk("[%d]=0x%08x\n", i, chanlist[i]);
+				}
+#endif
+				return -EINVAL;
+			}
+	} else if (s->range_table_list) {
+		for (i = 0; i < n; i++) {
+			chan = CR_CHAN(chanlist[i]);
+			if (chan >= s->n_chan ||
+				CR_RANGE(chanlist[i]) >=
+				s->range_table_list[chan]->length
+				|| aref_invalid(s, chanlist[i])) {
+				rt_printk("bad chanlist[%d]=0x%08x\n", i,
+					chanlist[i]);
+				return -EINVAL;
+			}
+		}
+	} else {
+		rt_printk("comedi: (bug) no range type list!\n");
+		return -EINVAL;
+	}
+	return 0;
+}
diff --git a/drivers/staging/comedi/rt.c b/drivers/staging/comedi/rt.c
new file mode 100644
index 00000000000..385b81b94ac
--- /dev/null
+++ b/drivers/staging/comedi/rt.c
@@ -0,0 +1,412 @@
+/*
+    comedi/rt.c
+    comedi kernel module
+
+    COMEDI - Linux Control and Measurement Device Interface
+    Copyright (C) 1997-2000 David A. Schleef <ds@schleef.org>
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+
+*/
+
+#undef DEBUG
+
+#define __NO_VERSION__
+#include <linux/comedidev.h>
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/fcntl.h>
+#include <linux/delay.h>
+#include <linux/ioport.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+#include <asm/io.h>
+
+#include "rt_pend_tq.h"
+
+#ifdef CONFIG_COMEDI_RTAI
+#include <rtai.h>
+#endif
+
+#ifdef CONFIG_COMEDI_FUSION
+#include <nucleus/asm/hal.h>
+#endif
+
+#ifdef CONFIG_COMEDI_RTL
+#include <rtl_core.h>
+#include <rtl_sync.h>
+#endif
+
+struct comedi_irq_struct {
+	int rt;
+	int irq;
+	 irqreturn_t(*handler) (int irq, void *dev_id PT_REGS_ARG);
+	unsigned long flags;
+	const char *device;
+	comedi_device *dev_id;
+};
+
+static int comedi_rt_get_irq(struct comedi_irq_struct *it);
+static int comedi_rt_release_irq(struct comedi_irq_struct *it);
+
+static struct comedi_irq_struct *comedi_irqs[NR_IRQS];
+
+int comedi_request_irq(unsigned irq, irqreturn_t(*handler) (int,
+		void *PT_REGS_ARG), unsigned long flags, const char *device,
+	comedi_device * dev_id)
+{
+	struct comedi_irq_struct *it;
+	int ret;
+	/* null shared interrupt flag, since rt interrupt handlers do not
+	 * support it, and this version of comedi_request_irq() is only
+	 * called for kernels with rt support */
+	unsigned long unshared_flags = flags & ~IRQF_SHARED;
+
+	ret = request_irq(irq, handler, unshared_flags, device, dev_id);
+	if (ret < 0) {
+		// we failed, so fall back on allowing shared interrupt (which we won't ever make RT)
+		if (flags & IRQF_SHARED) {
+			rt_printk
+				("comedi: cannot get unshared interrupt, will not use RT interrupts.\n");
+			ret = request_irq(irq, handler, flags, device, dev_id);
+		}
+		if (ret < 0) {
+			return ret;
+		}
+	} else {
+		it = kzalloc(sizeof(struct comedi_irq_struct), GFP_KERNEL);
+		if (!it)
+			return -ENOMEM;
+
+		it->handler = handler;
+		it->irq = irq;
+		it->dev_id = dev_id;
+		it->device = device;
+		it->flags = unshared_flags;
+		comedi_irqs[irq] = it;
+	}
+	return 0;
+}
+
+void comedi_free_irq(unsigned int irq, comedi_device * dev_id)
+{
+	struct comedi_irq_struct *it;
+
+	free_irq(irq, dev_id);
+
+	it = comedi_irqs[irq];
+	if (it == NULL)
+		return;
+
+	if (it->rt) {
+		printk("real-time IRQ allocated at board removal (ignore)\n");
+		comedi_rt_release_irq(it);
+	}
+
+	kfree(it);
+	comedi_irqs[irq] = NULL;
+}
+
+int comedi_switch_to_rt(comedi_device * dev)
+{
+	struct comedi_irq_struct *it;
+	unsigned long flags;
+
+	it = comedi_irqs[dev->irq];
+	/* drivers might not be using an interrupt for commands,
+	   or we might not have been able to get an unshared irq */
+	if (it == NULL)
+		return -1;
+
+	comedi_spin_lock_irqsave(&dev->spinlock, flags);
+
+	if (!dev->rt)
+		comedi_rt_get_irq(it);
+
+	dev->rt++;
+	it->rt = 1;
+
+	comedi_spin_unlock_irqrestore(&dev->spinlock, flags);
+
+	return 0;
+}
+
+void comedi_switch_to_non_rt(comedi_device * dev)
+{
+	struct comedi_irq_struct *it;
+	unsigned long flags;
+
+	it = comedi_irqs[dev->irq];
+	if (it == NULL)
+		return;
+
+	comedi_spin_lock_irqsave(&dev->spinlock, flags);
+
+	dev->rt--;
+	if (!dev->rt)
+		comedi_rt_release_irq(it);
+
+	it->rt = 0;
+
+	comedi_spin_unlock_irqrestore(&dev->spinlock, flags);
+}
+
+void wake_up_int_handler(int arg1, void *arg2)
+{
+	wake_up_interruptible((wait_queue_head_t *) arg2);
+}
+
+void comedi_rt_pend_wakeup(wait_queue_head_t * q)
+{
+	rt_pend_call(wake_up_int_handler, 0, q);
+}
+
+/* RTAI section */
+#ifdef CONFIG_COMEDI_RTAI
+
+#ifndef HAVE_RT_REQUEST_IRQ_WITH_ARG
+#define DECLARE_VOID_IRQ(irq) \
+static void handle_void_irq_ ## irq (void){ handle_void_irq(irq);}
+
+static void handle_void_irq(int irq)
+{
+	struct comedi_irq_struct *it;
+
+	it = comedi_irqs[irq];
+	if (it == NULL) {
+		rt_printk("comedi: null irq struct?\n");
+		return;
+	}
+	it->handler(irq, it->dev_id PT_REGS_NULL);
+	rt_enable_irq(irq);	//needed by rtai-adeos, seems like it shouldn't hurt earlier versions
+}
+
+DECLARE_VOID_IRQ(0);
+DECLARE_VOID_IRQ(1);
+DECLARE_VOID_IRQ(2);
+DECLARE_VOID_IRQ(3);
+DECLARE_VOID_IRQ(4);
+DECLARE_VOID_IRQ(5);
+DECLARE_VOID_IRQ(6);
+DECLARE_VOID_IRQ(7);
+DECLARE_VOID_IRQ(8);
+DECLARE_VOID_IRQ(9);
+DECLARE_VOID_IRQ(10);
+DECLARE_VOID_IRQ(11);
+DECLARE_VOID_IRQ(12);
+DECLARE_VOID_IRQ(13);
+DECLARE_VOID_IRQ(14);
+DECLARE_VOID_IRQ(15);
+DECLARE_VOID_IRQ(16);
+DECLARE_VOID_IRQ(17);
+DECLARE_VOID_IRQ(18);
+DECLARE_VOID_IRQ(19);
+DECLARE_VOID_IRQ(20);
+DECLARE_VOID_IRQ(21);
+DECLARE_VOID_IRQ(22);
+DECLARE_VOID_IRQ(23);
+
+typedef void (*V_FP_V) (void);
+static V_FP_V handle_void_irq_ptrs[] = {
+	handle_void_irq_0,
+	handle_void_irq_1,
+	handle_void_irq_2,
+	handle_void_irq_3,
+	handle_void_irq_4,
+	handle_void_irq_5,
+	handle_void_irq_6,
+	handle_void_irq_7,
+	handle_void_irq_8,
+	handle_void_irq_9,
+	handle_void_irq_10,
+	handle_void_irq_11,
+	handle_void_irq_12,
+	handle_void_irq_13,
+	handle_void_irq_14,
+	handle_void_irq_15,
+	handle_void_irq_16,
+	handle_void_irq_17,
+	handle_void_irq_18,
+	handle_void_irq_19,
+	handle_void_irq_20,
+	handle_void_irq_21,
+	handle_void_irq_22,
+	handle_void_irq_23,
+};
+
+static int comedi_rt_get_irq(struct comedi_irq_struct *it)
+{
+	rt_request_global_irq(it->irq, handle_void_irq_ptrs[it->irq]);
+	rt_startup_irq(it->irq);
+
+	return 0;
+}
+
+static int comedi_rt_release_irq(struct comedi_irq_struct *it)
+{
+	rt_shutdown_irq(it->irq);
+	rt_free_global_irq(it->irq);
+	return 0;
+}
+#else
+
+static int comedi_rt_get_irq(struct comedi_irq_struct *it)
+{
+	int ret;
+
+	ret = rt_request_global_irq_arg(it->irq, it->handler, it->flags,
+		it->device, it->dev_id);
+	if (ret < 0) {
+		rt_printk("rt_request_global_irq_arg() returned %d\n", ret);
+		return ret;
+	}
+	rt_startup_irq(it->irq);
+
+	return 0;
+}
+
+static int comedi_rt_release_irq(struct comedi_irq_struct *it)
+{
+	rt_shutdown_irq(it->irq);
+	rt_free_global_irq(it->irq);
+	return 0;
+}
+#endif
+
+void comedi_rt_init(void)
+{
+	rt_mount_rtai();
+	rt_pend_tq_init();
+}
+
+void comedi_rt_cleanup(void)
+{
+	rt_umount_rtai();
+	rt_pend_tq_cleanup();
+}
+
+#endif
+
+/* Fusion section */
+#ifdef CONFIG_COMEDI_FUSION
+
+static void fusion_handle_irq(unsigned int irq, void *cookie)
+{
+	struct comedi_irq_struct *it = cookie;
+
+	it->handler(irq, it->dev_id PT_REGS_NULL);
+	rthal_irq_enable(irq);
+}
+
+static int comedi_rt_get_irq(struct comedi_irq_struct *it)
+{
+	rthal_irq_request(it->irq, fusion_handle_irq, it);
+	rthal_irq_enable(it->irq);
+	return 0;
+}
+
+static int comedi_rt_release_irq(struct comedi_irq_struct *it)
+{
+	rthal_irq_disable(it->irq);
+	rthal_irq_release(it->irq);
+	return 0;
+}
+
+void comedi_rt_init(void)
+{
+	rt_pend_tq_init();
+}
+
+void comedi_rt_cleanup(void)
+{
+	rt_pend_tq_cleanup();
+}
+
+#endif /*CONFIG_COMEDI_FUSION */
+
+/* RTLinux section */
+#ifdef CONFIG_COMEDI_RTL
+
+static unsigned int handle_rtl_irq(unsigned int irq PT_REGS_ARG)
+{
+	struct comedi_irq_struct *it;
+
+	it = comedi_irqs[irq];
+	if (it == NULL)
+		return 0;
+	it->handler(irq, it->dev_id PT_REGS_NULL);
+	rtl_hard_enable_irq(irq);
+	return 0;
+}
+
+static int comedi_rt_get_irq(struct comedi_irq_struct *it)
+{
+	rtl_request_global_irq(it->irq, handle_rtl_irq);
+	return 0;
+}
+
+static int comedi_rt_release_irq(struct comedi_irq_struct *it)
+{
+	rtl_free_global_irq(it->irq);
+	return 0;
+}
+
+void comedi_rt_init(void)
+{
+	rt_pend_tq_init();
+}
+
+void comedi_rt_cleanup(void)
+{
+	rt_pend_tq_cleanup();
+}
+
+#endif
+
+#ifdef CONFIG_COMEDI_PIRQ
+static int comedi_rt_get_irq(struct comedi_irq_struct *it)
+{
+	int ret;
+
+	free_irq(it->irq, it->dev_id);
+	ret = request_irq(it->irq, it->handler, it->flags | SA_PRIORITY,
+		it->device, it->dev_id);
+
+	return ret;
+}
+
+static int comedi_rt_release_irq(struct comedi_irq_struct *it)
+{
+	int ret;
+
+	free_irq(it->irq, it->dev_id);
+	ret = request_irq(it->irq, it->handler, it->flags,
+		it->device, it->dev_id);
+
+	return ret;
+}
+
+void comedi_rt_init(void)
+{
+	//rt_pend_tq_init();
+}
+
+void comedi_rt_cleanup(void)
+{
+	//rt_pend_tq_cleanup();
+}
+#endif
diff --git a/drivers/staging/comedi/rt_pend_tq.c b/drivers/staging/comedi/rt_pend_tq.c
new file mode 100644
index 00000000000..995f076e0af
--- /dev/null
+++ b/drivers/staging/comedi/rt_pend_tq.c
@@ -0,0 +1,113 @@
+#define __NO_VERSION__
+/* rt_pend_tq.c */
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/sched.h>
+#include "comedidev.h"	// for rt spinlocks
+#include "rt_pend_tq.h"
+#ifdef CONFIG_COMEDI_RTAI
+#include <rtai.h>
+#endif
+#ifdef CONFIG_COMEDI_FUSION
+#include <nucleus/asm/hal.h>
+#endif
+#ifdef CONFIG_COMEDI_RTL
+#include <rtl_core.h>
+#endif
+
+#ifdef standalone
+#include <linux/module.h>
+#define rt_pend_tq_init init_module
+#define rt_pend_tq_cleanup cleanup_module
+#endif
+
+volatile static struct rt_pend_tq rt_pend_tq[RT_PEND_TQ_SIZE];
+volatile static struct rt_pend_tq *volatile rt_pend_head = rt_pend_tq,
+	*volatile rt_pend_tail = rt_pend_tq;
+int rt_pend_tq_irq = 0;
+spinlock_t rt_pend_tq_lock = SPIN_LOCK_UNLOCKED;
+
+// WARNING: following code not checked against race conditions yet.
+#define INC_CIRCULAR_PTR(ptr,begin,size) do {if(++(ptr)>=(begin)+(size)) (ptr)=(begin); } while(0)
+#define DEC_CIRCULAR_PTR(ptr,begin,size) do {if(--(ptr)<(begin)) (ptr)=(begin)+(size)-1; } while(0)
+
+int rt_pend_call(void (*func) (int arg1, void *arg2), int arg1, void *arg2)
+{
+	unsigned long flags;
+
+	if (func == NULL)
+		return -EINVAL;
+	if (rt_pend_tq_irq <= 0)
+		return -ENODEV;
+	comedi_spin_lock_irqsave(&rt_pend_tq_lock, flags);
+	INC_CIRCULAR_PTR(rt_pend_head, rt_pend_tq, RT_PEND_TQ_SIZE);
+	if (rt_pend_head == rt_pend_tail) {
+		// overflow, we just refuse to take this request
+		DEC_CIRCULAR_PTR(rt_pend_head, rt_pend_tq, RT_PEND_TQ_SIZE);
+		comedi_spin_unlock_irqrestore(&rt_pend_tq_lock, flags);
+		return -EAGAIN;
+	}
+	rt_pend_head->func = func;
+	rt_pend_head->arg1 = arg1;
+	rt_pend_head->arg2 = arg2;
+	comedi_spin_unlock_irqrestore(&rt_pend_tq_lock, flags);
+#ifdef CONFIG_COMEDI_RTAI
+	rt_pend_linux_srq(rt_pend_tq_irq);
+#endif
+#ifdef CONFIG_COMEDI_FUSION
+	rthal_apc_schedule(rt_pend_tq_irq);
+#endif
+#ifdef CONFIG_COMEDI_RTL
+	rtl_global_pend_irq(rt_pend_tq_irq);
+
+#endif
+	return 0;
+}
+
+#ifdef CONFIG_COMEDI_RTAI
+void rt_pend_irq_handler(void)
+#elif defined(CONFIG_COMEDI_FUSION)
+void rt_pend_irq_handler(void *cookie)
+#elif defined(CONFIG_COMEDI_RTL)
+void rt_pend_irq_handler(int irq, void *dev PT_REGS_ARG)
+#endif
+{
+	while (rt_pend_head != rt_pend_tail) {
+		INC_CIRCULAR_PTR(rt_pend_tail, rt_pend_tq, RT_PEND_TQ_SIZE);
+		rt_pend_tail->func(rt_pend_tail->arg1, rt_pend_tail->arg2);
+	}
+}
+
+int rt_pend_tq_init(void)
+{
+	rt_pend_head = rt_pend_tail = rt_pend_tq;
+#ifdef CONFIG_COMEDI_RTAI
+	rt_pend_tq_irq = rt_request_srq(0, rt_pend_irq_handler, NULL);
+#endif
+#ifdef CONFIG_COMEDI_FUSION
+	rt_pend_tq_irq =
+		rthal_apc_alloc("comedi APC", rt_pend_irq_handler, NULL);
+#endif
+#ifdef CONFIG_COMEDI_RTL
+	rt_pend_tq_irq = rtl_get_soft_irq(rt_pend_irq_handler, "rt_pend_irq");
+#endif
+	if (rt_pend_tq_irq > 0)
+		printk("rt_pend_tq: RT bottom half scheduler initialized OK\n");
+	else
+		printk("rt_pend_tq: rtl_get_soft_irq failed\n");
+	return 0;
+}
+
+void rt_pend_tq_cleanup(void)
+{
+	printk("rt_pend_tq: unloading\n");
+#ifdef CONFIG_COMEDI_RTAI
+	rt_free_srq(rt_pend_tq_irq);
+#endif
+#ifdef CONFIG_COMEDI_FUSION
+	rthal_apc_free(rt_pend_tq_irq);
+#endif
+#ifdef CONFIG_COMEDI_RTL
+	free_irq(rt_pend_tq_irq, NULL);
+#endif
+}
diff --git a/drivers/staging/comedi/rt_pend_tq.h b/drivers/staging/comedi/rt_pend_tq.h
new file mode 100644
index 00000000000..01ed71bf409
--- /dev/null
+++ b/drivers/staging/comedi/rt_pend_tq.h
@@ -0,0 +1,10 @@
+#define RT_PEND_TQ_SIZE 16
+struct rt_pend_tq {
+	void (*func) (int arg1, void *arg2);
+	int arg1;
+	void *arg2;
+};
+extern int rt_pend_call(void (*func) (int arg1, void *arg2), int arg1,
+	void *arg2);
+extern int rt_pend_tq_init(void);
+extern void rt_pend_tq_cleanup(void);
diff --git a/drivers/staging/comedi/wrapper.h b/drivers/staging/comedi/wrapper.h
new file mode 100644
index 00000000000..77fc673900e
--- /dev/null
+++ b/drivers/staging/comedi/wrapper.h
@@ -0,0 +1,25 @@
+/*
+    linux/wrapper.h compatibility header
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program; if not, write to the Free Software
+    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __COMPAT_LINUX_WRAPPER_H_
+#define __COMPAT_LINUX_WRAPPER_H_
+
+#define mem_map_reserve(p)      set_bit(PG_reserved, &((p)->flags))
+#define mem_map_unreserve(p)    clear_bit(PG_reserved, &((p)->flags))
+
+#endif /* __COMPAT_LINUX_WRAPPER_H_ */
diff --git a/drivers/staging/echo/TODO b/drivers/staging/echo/TODO
index 1ca09afd603..f6d8580a0ba 100644
--- a/drivers/staging/echo/TODO
+++ b/drivers/staging/echo/TODO
@@ -1,7 +1,5 @@
 TODO:
 	- checkpatch.pl cleanups
-	- Lindent
-	- typedef removals
 	- handle bit_operations.h (merge in or make part of common code?)
 	- remove proc interface, only use echo.h interface (proc interface is
 	  racy and not correct.)
diff --git a/drivers/staging/echo/echo.h b/drivers/staging/echo/echo.h
index 9fb9543c4f1..34fb816b15c 100644
--- a/drivers/staging/echo/echo.h
+++ b/drivers/staging/echo/echo.h
@@ -149,8 +149,8 @@ struct oslec_state {
 	int Lbgn, Lbgn_acc, Lbgn_upper, Lbgn_upper_acc;
 
 	/* foreground and background filter states */
-	fir16_state_t fir_state;
-	fir16_state_t fir_state_bg;
+	struct fir16_state_t fir_state;
+	struct fir16_state_t fir_state_bg;
 	int16_t *fir_taps16[2];
 
 	/* DC blocking filter states */
diff --git a/drivers/staging/echo/fir.h b/drivers/staging/echo/fir.h
index 5645cb1b2f9..d35f16805f4 100644
--- a/drivers/staging/echo/fir.h
+++ b/drivers/staging/echo/fir.h
@@ -72,37 +72,37 @@
     16 bit integer FIR descriptor. This defines the working state for a single
     instance of an FIR filter using 16 bit integer coefficients.
 */
-typedef struct {
+struct fir16_state_t {
 	int taps;
 	int curr_pos;
 	const int16_t *coeffs;
 	int16_t *history;
-} fir16_state_t;
+};
 
 /*!
     32 bit integer FIR descriptor. This defines the working state for a single
     instance of an FIR filter using 32 bit integer coefficients, and filtering
     16 bit integer data.
 */
-typedef struct {
+struct fir32_state_t {
 	int taps;
 	int curr_pos;
 	const int32_t *coeffs;
 	int16_t *history;
-} fir32_state_t;
+};
 
 /*!
     Floating point FIR descriptor. This defines the working state for a single
     instance of an FIR filter using floating point coefficients and data.
 */
-typedef struct {
+struct fir_float_state_t {
 	int taps;
 	int curr_pos;
 	const float *coeffs;
 	float *history;
-} fir_float_state_t;
+};
 
-static __inline__ const int16_t *fir16_create(fir16_state_t * fir,
+static __inline__ const int16_t *fir16_create(struct fir16_state_t *fir,
 					      const int16_t * coeffs, int taps)
 {
 	fir->taps = taps;
@@ -116,7 +116,7 @@ static __inline__ const int16_t *fir16_create(fir16_state_t * fir,
 	return fir->history;
 }
 
-static __inline__ void fir16_flush(fir16_state_t * fir)
+static __inline__ void fir16_flush(struct fir16_state_t *fir)
 {
 #if defined(USE_MMX)  ||  defined(USE_SSE2) || defined(__bfin__)
 	memset(fir->history, 0, 2 * fir->taps * sizeof(int16_t));
@@ -125,7 +125,7 @@ static __inline__ void fir16_flush(fir16_state_t * fir)
 #endif
 }
 
-static __inline__ void fir16_free(fir16_state_t * fir)
+static __inline__ void fir16_free(struct fir16_state_t *fir)
 {
 	kfree(fir->history);
 }
@@ -157,19 +157,19 @@ static inline int32_t dot_asm(short *x, short *y, int len)
 }
 #endif
 
-static __inline__ int16_t fir16(fir16_state_t * fir, int16_t sample)
+static __inline__ int16_t fir16(struct fir16_state_t *fir, int16_t sample)
 {
 	int32_t y;
 #if defined(USE_MMX)
 	int i;
-	mmx_t *mmx_coeffs;
-	mmx_t *mmx_hist;
+	union mmx_t *mmx_coeffs;
+	union mmx_t *mmx_hist;
 
 	fir->history[fir->curr_pos] = sample;
 	fir->history[fir->curr_pos + fir->taps] = sample;
 
-	mmx_coeffs = (mmx_t *) fir->coeffs;
-	mmx_hist = (mmx_t *) & fir->history[fir->curr_pos];
+	mmx_coeffs = (union mmx_t *)fir->coeffs;
+	mmx_hist = (union mmx_t *)&fir->history[fir->curr_pos];
 	i = fir->taps;
 	pxor_r2r(mm4, mm4);
 	/* 8 samples per iteration, so the filter must be a multiple of 8 long. */
@@ -193,14 +193,14 @@ static __inline__ int16_t fir16(fir16_state_t * fir, int16_t sample)
 	emms();
 #elif defined(USE_SSE2)
 	int i;
-	xmm_t *xmm_coeffs;
-	xmm_t *xmm_hist;
+	union xmm_t *xmm_coeffs;
+	union xmm_t *xmm_hist;
 
 	fir->history[fir->curr_pos] = sample;
 	fir->history[fir->curr_pos + fir->taps] = sample;
 
-	xmm_coeffs = (xmm_t *) fir->coeffs;
-	xmm_hist = (xmm_t *) & fir->history[fir->curr_pos];
+	xmm_coeffs = (union xmm_t *)fir->coeffs;
+	xmm_hist = (union xmm_t *)&fir->history[fir->curr_pos];
 	i = fir->taps;
 	pxor_r2r(xmm4, xmm4);
 	/* 16 samples per iteration, so the filter must be a multiple of 16 long. */
@@ -250,7 +250,7 @@ static __inline__ int16_t fir16(fir16_state_t * fir, int16_t sample)
 	return (int16_t) (y >> 15);
 }
 
-static __inline__ const int16_t *fir32_create(fir32_state_t * fir,
+static __inline__ const int16_t *fir32_create(struct fir32_state_t *fir,
 					      const int32_t * coeffs, int taps)
 {
 	fir->taps = taps;
@@ -260,17 +260,17 @@ static __inline__ const int16_t *fir32_create(fir32_state_t * fir,
 	return fir->history;
 }
 
-static __inline__ void fir32_flush(fir32_state_t * fir)
+static __inline__ void fir32_flush(struct fir32_state_t *fir)
 {
 	memset(fir->history, 0, fir->taps * sizeof(int16_t));
 }
 
-static __inline__ void fir32_free(fir32_state_t * fir)
+static __inline__ void fir32_free(struct fir32_state_t *fir)
 {
 	kfree(fir->history);
 }
 
-static __inline__ int16_t fir32(fir32_state_t * fir, int16_t sample)
+static __inline__ int16_t fir32(struct fir32_state_t *fir, int16_t sample)
 {
 	int i;
 	int32_t y;
diff --git a/drivers/staging/echo/mmx.h b/drivers/staging/echo/mmx.h
index 35412efe61c..44e5cfebc18 100644
--- a/drivers/staging/echo/mmx.h
+++ b/drivers/staging/echo/mmx.h
@@ -27,7 +27,7 @@
  * values by ULL, lest they be truncated by the compiler)
  */
 
-typedef union {
+union mmx_t {
 	long long q;		/* Quadword (64-bit) value */
 	unsigned long long uq;	/* Unsigned Quadword */
 	int d[2];		/* 2 Doubleword (32-bit) values */
@@ -37,12 +37,12 @@ typedef union {
 	char b[8];		/* 8 Byte (8-bit) values */
 	unsigned char ub[8];	/* 8 Unsigned Byte */
 	float s[2];		/* Single-precision (32-bit) value */
-} mmx_t;			/* On an 8-byte (64-bit) boundary */
+};				/* On an 8-byte (64-bit) boundary */
 
 /* SSE registers */
-typedef union {
+union xmm_t {
 	char b[16];
-} xmm_t;
+};
 
 #define         mmx_i2r(op,imm,reg) \
         __asm__ __volatile__ (#op " %0, %%" #reg \
diff --git a/drivers/staging/epl/Benchmark.h b/drivers/staging/epl/Benchmark.h
new file mode 100644
index 00000000000..62dee3b1437
--- /dev/null
+++ b/drivers/staging/epl/Benchmark.h
@@ -0,0 +1,437 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  header file for benchmarking
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: Benchmark.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    ...
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/08/16 d.k.:  start of implementation
+
+****************************************************************************/
+
+#ifndef _BENCHMARK_H_
+#define _BENCHMARK_H_
+
+#include "global.h"
+
+#if (TARGET_SYSTEM == _NO_OS_) && (DEV_SYSTEM == _DEV_GNU_CF548X_)
+#include "common.h"
+
+#elif (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
+
+//    #include <linux/config.h>
+#include <linux/kernel.h>
+
+#ifdef CONFIG_COLDFIRE
+#include <asm/coldfire.h>
+#include <asm/m5485gpio.h>
+
+#define BENCHMARK_SET(x)    MCF_GPIO_PODR_PCIBG |= (1 << (x))	// (x+1)
+#define BENCHMARK_RESET(x)  MCF_GPIO_PODR_PCIBG &= ~(1 << (x))	// (x+1)
+#define BENCHMARK_TOGGLE(x) MCF_GPIO_PODR_PCIBR ^= (1 << (x - 5))
+#else
+#undef BENCHMARK_MODULES
+#define BENCHMARK_MODULES           0x00000000
+#endif
+
+#else
+    // disable Benchmarking
+#undef BENCHMARK_MODULES
+#define BENCHMARK_MODULES               0x00000000
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#ifndef BENCHMARK_MODULES
+#define BENCHMARK_MODULES                   0x00000000
+#endif
+
+#define BENCHMARK_MOD_01                    0x00000001
+#define BENCHMARK_MOD_02                    0x00000002
+#define BENCHMARK_MOD_03                    0x00000004
+#define BENCHMARK_MOD_04                    0x00000008
+#define BENCHMARK_MOD_05                    0x00000010
+#define BENCHMARK_MOD_06                    0x00000020
+#define BENCHMARK_MOD_07                    0x00000040
+#define BENCHMARK_MOD_08                    0x00000080
+#define BENCHMARK_MOD_09                    0x00000100
+#define BENCHMARK_MOD_10                    0x00000200
+#define BENCHMARK_MOD_11                    0x00000400
+#define BENCHMARK_MOD_12                    0x00000800
+#define BENCHMARK_MOD_13                    0x00001000
+#define BENCHMARK_MOD_14                    0x00002000
+#define BENCHMARK_MOD_15                    0x00004000
+#define BENCHMARK_MOD_16                    0x00008000
+#define BENCHMARK_MOD_17                    0x00010000
+#define BENCHMARK_MOD_18                    0x00020000
+#define BENCHMARK_MOD_19                    0x00040000
+#define BENCHMARK_MOD_20                    0x00080000
+#define BENCHMARK_MOD_21                    0x00100000
+#define BENCHMARK_MOD_22                    0x00200000
+#define BENCHMARK_MOD_23                    0x00400000
+#define BENCHMARK_MOD_24                    0x00800000
+#define BENCHMARK_MOD_25                    0x01000000
+#define BENCHMARK_MOD_26                    0x02000000
+#define BENCHMARK_MOD_27                    0x04000000
+#define BENCHMARK_MOD_28                    0x08000000
+#define BENCHMARK_MOD_29                    0x10000000
+#define BENCHMARK_MOD_30                    0x20000000
+#define BENCHMARK_MOD_31                    0x40000000
+#define BENCHMARK_MOD_32                    0x80000000
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_01)
+#define BENCHMARK_MOD_01_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_01_RESET(x)       BENCHMARK_RESET(x)
+#define BENCHMARK_MOD_01_TOGGLE(x)      BENCHMARK_TOGGLE(x)
+#else
+#define BENCHMARK_MOD_01_SET(x)
+#define BENCHMARK_MOD_01_RESET(x)
+#define BENCHMARK_MOD_01_TOGGLE(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_02)
+#define BENCHMARK_MOD_02_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_02_RESET(x)       BENCHMARK_RESET(x)
+#define BENCHMARK_MOD_02_TOGGLE(x)      BENCHMARK_TOGGLE(x)
+#else
+#define BENCHMARK_MOD_02_SET(x)
+#define BENCHMARK_MOD_02_RESET(x)
+#define BENCHMARK_MOD_02_TOGGLE(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_03)
+#define BENCHMARK_MOD_03_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_03_RESET(x)       BENCHMARK_RESET(x)
+#define BENCHMARK_MOD_03_TOGGLE(x)      BENCHMARK_TOGGLE(x)
+#else
+#define BENCHMARK_MOD_03_SET(x)
+#define BENCHMARK_MOD_03_RESET(x)
+#define BENCHMARK_MOD_03_TOGGLE(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_04)
+#define BENCHMARK_MOD_04_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_04_RESET(x)       BENCHMARK_RESET(x)
+#define BENCHMARK_MOD_04_TOGGLE(x)      BENCHMARK_TOGGLE(x)
+#else
+#define BENCHMARK_MOD_04_SET(x)
+#define BENCHMARK_MOD_04_RESET(x)
+#define BENCHMARK_MOD_04_TOGGLE(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_05)
+#define BENCHMARK_MOD_05_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_05_RESET(x)       BENCHMARK_RESET(x)
+#define BENCHMARK_MOD_05_TOGGLE(x)      BENCHMARK_TOGGLE(x)
+#else
+#define BENCHMARK_MOD_05_SET(x)
+#define BENCHMARK_MOD_05_RESET(x)
+#define BENCHMARK_MOD_05_TOGGLE(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_06)
+#define BENCHMARK_MOD_06_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_06_RESET(x)       BENCHMARK_RESET(x)
+#define BENCHMARK_MOD_06_TOGGLE(x)      BENCHMARK_TOGGLE(x)
+#else
+#define BENCHMARK_MOD_06_SET(x)
+#define BENCHMARK_MOD_06_RESET(x)
+#define BENCHMARK_MOD_06_TOGGLE(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_07)
+#define BENCHMARK_MOD_07_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_07_RESET(x)       BENCHMARK_RESET(x)
+#define BENCHMARK_MOD_07_TOGGLE(x)      BENCHMARK_TOGGLE(x)
+#else
+#define BENCHMARK_MOD_07_SET(x)
+#define BENCHMARK_MOD_07_RESET(x)
+#define BENCHMARK_MOD_07_TOGGLE(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_08)
+#define BENCHMARK_MOD_08_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_08_RESET(x)       BENCHMARK_RESET(x)
+#define BENCHMARK_MOD_08_TOGGLE(x)      BENCHMARK_TOGGLE(x)
+#else
+#define BENCHMARK_MOD_08_SET(x)
+#define BENCHMARK_MOD_08_RESET(x)
+#define BENCHMARK_MOD_08_TOGGLE(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_09)
+#define BENCHMARK_MOD_09_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_09_RESET(x)       BENCHMARK_RESET(x)
+#define BENCHMARK_MOD_09_TOGGLE(x)      BENCHMARK_TOGGLE(x)
+#else
+#define BENCHMARK_MOD_09_SET(x)
+#define BENCHMARK_MOD_09_RESET(x)
+#define BENCHMARK_MOD_09_TOGGLE(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_10)
+#define BENCHMARK_MOD_10_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_10_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_10_SET(x)
+#define BENCHMARK_MOD_10_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_11)
+#define BENCHMARK_MOD_11_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_11_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_11_SET(x)
+#define BENCHMARK_MOD_11_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_12)
+#define BENCHMARK_MOD_12_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_12_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_12_SET(x)
+#define BENCHMARK_MOD_12_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_13)
+#define BENCHMARK_MOD_13_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_13_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_13_SET(x)
+#define BENCHMARK_MOD_13_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_14)
+#define BENCHMARK_MOD_14_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_14_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_14_SET(x)
+#define BENCHMARK_MOD_14_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_15)
+#define BENCHMARK_MOD_15_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_15_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_15_SET(x)
+#define BENCHMARK_MOD_15_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_16)
+#define BENCHMARK_MOD_16_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_16_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_16_SET(x)
+#define BENCHMARK_MOD_16_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_17)
+#define BENCHMARK_MOD_17_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_17_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_17_SET(x)
+#define BENCHMARK_MOD_17_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_18)
+#define BENCHMARK_MOD_18_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_18_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_18_SET(x)
+#define BENCHMARK_MOD_18_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_19)
+#define BENCHMARK_MOD_19_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_19_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_19_SET(x)
+#define BENCHMARK_MOD_19_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_20)
+#define BENCHMARK_MOD_20_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_20_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_20_SET(x)
+#define BENCHMARK_MOD_20_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_21)
+#define BENCHMARK_MOD_21_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_21_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_21_SET(x)
+#define BENCHMARK_MOD_21_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_22)
+#define BENCHMARK_MOD_22_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_22_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_22_SET(x)
+#define BENCHMARK_MOD_22_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_23)
+#define BENCHMARK_MOD_23_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_23_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_23_SET(x)
+#define BENCHMARK_MOD_23_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_24)
+#define BENCHMARK_MOD_24_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_24_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_24_SET(x)
+#define BENCHMARK_MOD_24_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_25)
+#define BENCHMARK_MOD_25_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_25_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_25_SET(x)
+#define BENCHMARK_MOD_25_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_26)
+#define BENCHMARK_MOD_26_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_26_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_26_SET(x)
+#define BENCHMARK_MOD_26_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_27)
+#define BENCHMARK_MOD_27_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_27_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_27_SET(x)
+#define BENCHMARK_MOD_27_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_28)
+#define BENCHMARK_MOD_28_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_28_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_28_SET(x)
+#define BENCHMARK_MOD_28_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_29)
+#define BENCHMARK_MOD_29_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_29_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_29_SET(x)
+#define BENCHMARK_MOD_29_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_30)
+#define BENCHMARK_MOD_30_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_30_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_30_SET(x)
+#define BENCHMARK_MOD_30_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_31)
+#define BENCHMARK_MOD_31_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_31_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_31_SET(x)
+#define BENCHMARK_MOD_31_RESET(x)
+#endif
+
+#if (BENCHMARK_MODULES & BENCHMARK_MOD_32)
+#define BENCHMARK_MOD_32_SET(x)         BENCHMARK_SET(x)
+#define BENCHMARK_MOD_32_RESET(x)       BENCHMARK_RESET(x)
+#else
+#define BENCHMARK_MOD_32_SET(x)
+#define BENCHMARK_MOD_32_RESET(x)
+#endif
+
+//---------------------------------------------------------------------------
+// modul global types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+#endif // _BENCHMARK_H_
diff --git a/drivers/staging/epl/Debug.h b/drivers/staging/epl/Debug.h
new file mode 100644
index 00000000000..05de9d541fd
--- /dev/null
+++ b/drivers/staging/epl/Debug.h
@@ -0,0 +1,734 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  Debug interface
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: Debug.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    ...
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+****************************************************************************/
+
+#ifndef _DEBUG_H_
+#define _DEBUG_H_
+
+#include "global.h"
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// global const defines
+//---------------------------------------------------------------------------
+
+// These definitions are important for level-debug traces.
+// A macro DEBUG_GLB_LVL() defines the current debug-level using following bis.
+// If the corresponding bit is set then trace message will be printed out
+// (only if NDEBUG is not defined). The upper debug-levels are reserved for
+// the debug-levels ALWAYS, ERROR and ASSERT.
+#define DEBUG_LVL_01                    0x00000001
+#define DEBUG_LVL_02                    0x00000002
+#define DEBUG_LVL_03                    0x00000004
+#define DEBUG_LVL_04                    0x00000008
+#define DEBUG_LVL_05                    0x00000010
+#define DEBUG_LVL_06                    0x00000020
+#define DEBUG_LVL_07                    0x00000040
+#define DEBUG_LVL_08                    0x00000080
+#define DEBUG_LVL_09                    0x00000100
+#define DEBUG_LVL_10                    0x00000200
+#define DEBUG_LVL_11                    0x00000400
+#define DEBUG_LVL_12                    0x00000800
+#define DEBUG_LVL_13                    0x00001000
+#define DEBUG_LVL_14                    0x00002000
+#define DEBUG_LVL_15                    0x00004000
+#define DEBUG_LVL_16                    0x00008000
+#define DEBUG_LVL_17                    0x00010000
+#define DEBUG_LVL_18                    0x00020000
+#define DEBUG_LVL_19                    0x00040000
+#define DEBUG_LVL_20                    0x00080000
+#define DEBUG_LVL_21                    0x00100000
+#define DEBUG_LVL_22                    0x00200000
+#define DEBUG_LVL_23                    0x00400000
+#define DEBUG_LVL_24                    0x00800000
+#define DEBUG_LVL_25                    0x01000000
+#define DEBUG_LVL_26                    0x02000000
+#define DEBUG_LVL_27                    0x04000000
+#define DEBUG_LVL_28                    0x08000000
+#define DEBUG_LVL_29                    0x10000000
+#define DEBUG_LVL_ASSERT                0x20000000
+#define DEBUG_LVL_ERROR                 0x40000000
+#define DEBUG_LVL_ALWAYS                0x80000000
+
+//---------------------------------------------------------------------------
+// global types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// global vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// global function prototypes
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// global macros
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// this macro defines a version string
+
+
+//---------------------------------------------------------------------------
+// this macro defines a build info string (e.g. for using in printf())
+#define DEBUG_MAKE_BUILD_INFO(prefix,product,prodid,descr,verstr,author) "\n" \
+    prefix "***************************************************\n" \
+    prefix "Project:   " product ", " prodid                  "\n" \
+    prefix "Descript.: " descr                                "\n" \
+    prefix "Author:    " author                               "\n" \
+    prefix "Date:      " __DATE__                             "\n" \
+    prefix "Version:   " verstr                               "\n" \
+    prefix "***************************************************\n\n"
+
+//---------------------------------------------------------------------------
+// The default debug-level is: ERROR and ALWAYS.
+// You can define an other debug-level in project settings.
+#ifndef DEF_DEBUG_LVL
+#define DEF_DEBUG_LVL                   (DEBUG_LVL_ALWAYS | DEBUG_LVL_ERROR)
+#endif
+#ifndef DEBUG_GLB_LVL
+#define DEBUG_GLB_LVL()                 (DEF_DEBUG_LVL)
+#endif
+
+//---------------------------------------------------------------------------
+#if (DEV_SYSTEM == _DEV_WIN32_) && defined (TRACE_MSG)
+
+    // For WIN32 the macro DEBUG_TRACE0 can be defined as function call TraceLvl()
+    // or as macro TRACE().
+    //
+    // Here the parameter 'lvl' can be used with more than one
+    // debug-level (using OR).
+    //
+    // Example: DEBUG_TRACE1(DEBUG_LVL_30 | DEBUG_LVL_02, "Hello %d", bCount);
+
+#define DEBUG_TRACE0(lvl,str)               TraceLvl((lvl),str)
+#define DEBUG_TRACE1(lvl,str,p1)            TraceLvl((lvl),str,p1)
+#define DEBUG_TRACE2(lvl,str,p1,p2)         TraceLvl((lvl),str,p1,p2)
+#define DEBUG_TRACE3(lvl,str,p1,p2,p3)      TraceLvl((lvl),str,p1,p2,p3)
+#define DEBUG_TRACE4(lvl,str,p1,p2,p3,p4)   TraceLvl((lvl),str,p1,p2,p3,p4)
+#define DEBUG_GLB_LVL()                     dwDebugLevel_g
+
+#else
+
+    // At microcontrollers we do reduce the memory usage by deleting DEBUG_TRACE-lines
+    // (compiler does delete the lines).
+    //
+    // Here the parameter 'lvl' can only be used with one debug-level.
+    //
+    // Example: DEBUG_TRACE1(DEBUG_LVL_ERROR, "error code %d", dwRet);
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_ALWAYS)
+#define DEBUG_LVL_ALWAYS_TRACE0(str)                TRACE0(str)
+#define DEBUG_LVL_ALWAYS_TRACE1(str,p1)             TRACE1(str,p1)
+#define DEBUG_LVL_ALWAYS_TRACE2(str,p1,p2)          TRACE2(str,p1,p2)
+#define DEBUG_LVL_ALWAYS_TRACE3(str,p1,p2,p3)       TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_ALWAYS_TRACE4(str,p1,p2,p3,p4)    TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_ALWAYS_TRACE0(str)
+#define DEBUG_LVL_ALWAYS_TRACE1(str,p1)
+#define DEBUG_LVL_ALWAYS_TRACE2(str,p1,p2)
+#define DEBUG_LVL_ALWAYS_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_ALWAYS_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_ERROR)
+#define DEBUG_LVL_ERROR_TRACE0(str)                 TRACE0(str)
+#define DEBUG_LVL_ERROR_TRACE1(str,p1)              TRACE1(str,p1)
+#define DEBUG_LVL_ERROR_TRACE2(str,p1,p2)           TRACE2(str,p1,p2)
+#define DEBUG_LVL_ERROR_TRACE3(str,p1,p2,p3)        TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_ERROR_TRACE4(str,p1,p2,p3,p4)     TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_ERROR_TRACE0(str)
+#define DEBUG_LVL_ERROR_TRACE1(str,p1)
+#define DEBUG_LVL_ERROR_TRACE2(str,p1,p2)
+#define DEBUG_LVL_ERROR_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_ERROR_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_ASSERT)
+#define DEBUG_LVL_ASSERT_TRACE0(str)                TRACE0(str)
+#define DEBUG_LVL_ASSERT_TRACE1(str,p1)             TRACE1(str,p1)
+#define DEBUG_LVL_ASSERT_TRACE2(str,p1,p2)          TRACE2(str,p1,p2)
+#define DEBUG_LVL_ASSERT_TRACE3(str,p1,p2,p3)       TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_ASSERT_TRACE4(str,p1,p2,p3,p4)    TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_ASSERT_TRACE0(str)
+#define DEBUG_LVL_ASSERT_TRACE1(str,p1)
+#define DEBUG_LVL_ASSERT_TRACE2(str,p1,p2)
+#define DEBUG_LVL_ASSERT_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_ASSERT_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_29)
+#define DEBUG_LVL_29_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_29_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_29_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_29_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_29_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_29_TRACE0(str)
+#define DEBUG_LVL_29_TRACE1(str,p1)
+#define DEBUG_LVL_29_TRACE2(str,p1,p2)
+#define DEBUG_LVL_29_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_29_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_28)
+#define DEBUG_LVL_28_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_28_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_28_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_28_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_28_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_28_TRACE0(str)
+#define DEBUG_LVL_28_TRACE1(str,p1)
+#define DEBUG_LVL_28_TRACE2(str,p1,p2)
+#define DEBUG_LVL_28_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_28_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_27)
+#define DEBUG_LVL_27_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_27_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_27_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_27_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_27_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_27_TRACE0(str)
+#define DEBUG_LVL_27_TRACE1(str,p1)
+#define DEBUG_LVL_27_TRACE2(str,p1,p2)
+#define DEBUG_LVL_27_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_27_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_26)
+#define DEBUG_LVL_26_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_26_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_26_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_26_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_26_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_26_TRACE0(str)
+#define DEBUG_LVL_26_TRACE1(str,p1)
+#define DEBUG_LVL_26_TRACE2(str,p1,p2)
+#define DEBUG_LVL_26_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_26_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_25)
+#define DEBUG_LVL_25_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_25_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_25_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_25_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_25_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_25_TRACE0(str)
+#define DEBUG_LVL_25_TRACE1(str,p1)
+#define DEBUG_LVL_25_TRACE2(str,p1,p2)
+#define DEBUG_LVL_25_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_25_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_24)
+#define DEBUG_LVL_24_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_24_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_24_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_24_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_24_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_24_TRACE0(str)
+#define DEBUG_LVL_24_TRACE1(str,p1)
+#define DEBUG_LVL_24_TRACE2(str,p1,p2)
+#define DEBUG_LVL_24_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_24_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_23)
+#define DEBUG_LVL_23_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_23_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_23_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_23_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_23_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_23_TRACE0(str)
+#define DEBUG_LVL_23_TRACE1(str,p1)
+#define DEBUG_LVL_23_TRACE2(str,p1,p2)
+#define DEBUG_LVL_23_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_23_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_22)
+#define DEBUG_LVL_22_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_22_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_22_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_22_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_22_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_22_TRACE0(str)
+#define DEBUG_LVL_22_TRACE1(str,p1)
+#define DEBUG_LVL_22_TRACE2(str,p1,p2)
+#define DEBUG_LVL_22_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_22_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_21)
+#define DEBUG_LVL_21_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_21_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_21_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_21_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_21_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_21_TRACE0(str)
+#define DEBUG_LVL_21_TRACE1(str,p1)
+#define DEBUG_LVL_21_TRACE2(str,p1,p2)
+#define DEBUG_LVL_21_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_21_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_20)
+#define DEBUG_LVL_20_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_20_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_20_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_20_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_20_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_20_TRACE0(str)
+#define DEBUG_LVL_20_TRACE1(str,p1)
+#define DEBUG_LVL_20_TRACE2(str,p1,p2)
+#define DEBUG_LVL_20_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_20_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_19)
+#define DEBUG_LVL_19_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_19_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_19_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_19_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_19_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_19_TRACE0(str)
+#define DEBUG_LVL_19_TRACE1(str,p1)
+#define DEBUG_LVL_19_TRACE2(str,p1,p2)
+#define DEBUG_LVL_19_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_19_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_18)
+#define DEBUG_LVL_18_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_18_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_18_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_18_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_18_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_18_TRACE0(str)
+#define DEBUG_LVL_18_TRACE1(str,p1)
+#define DEBUG_LVL_18_TRACE2(str,p1,p2)
+#define DEBUG_LVL_18_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_18_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_17)
+#define DEBUG_LVL_17_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_17_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_17_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_17_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_17_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_17_TRACE0(str)
+#define DEBUG_LVL_17_TRACE1(str,p1)
+#define DEBUG_LVL_17_TRACE2(str,p1,p2)
+#define DEBUG_LVL_17_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_17_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_16)
+#define DEBUG_LVL_16_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_16_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_16_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_16_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_16_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_16_TRACE0(str)
+#define DEBUG_LVL_16_TRACE1(str,p1)
+#define DEBUG_LVL_16_TRACE2(str,p1,p2)
+#define DEBUG_LVL_16_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_16_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_15)
+#define DEBUG_LVL_15_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_15_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_15_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_15_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_15_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_15_TRACE0(str)
+#define DEBUG_LVL_15_TRACE1(str,p1)
+#define DEBUG_LVL_15_TRACE2(str,p1,p2)
+#define DEBUG_LVL_15_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_15_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_14)
+#define DEBUG_LVL_14_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_14_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_14_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_14_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_14_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_14_TRACE0(str)
+#define DEBUG_LVL_14_TRACE1(str,p1)
+#define DEBUG_LVL_14_TRACE2(str,p1,p2)
+#define DEBUG_LVL_14_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_14_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_13)
+#define DEBUG_LVL_13_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_13_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_13_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_13_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_13_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_13_TRACE0(str)
+#define DEBUG_LVL_13_TRACE1(str,p1)
+#define DEBUG_LVL_13_TRACE2(str,p1,p2)
+#define DEBUG_LVL_13_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_13_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_12)
+#define DEBUG_LVL_12_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_12_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_12_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_12_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_12_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_12_TRACE0(str)
+#define DEBUG_LVL_12_TRACE1(str,p1)
+#define DEBUG_LVL_12_TRACE2(str,p1,p2)
+#define DEBUG_LVL_12_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_12_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_11)
+#define DEBUG_LVL_11_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_11_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_11_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_11_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_11_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_11_TRACE0(str)
+#define DEBUG_LVL_11_TRACE1(str,p1)
+#define DEBUG_LVL_11_TRACE2(str,p1,p2)
+#define DEBUG_LVL_11_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_11_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_10)
+#define DEBUG_LVL_10_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_10_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_10_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_10_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_10_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_10_TRACE0(str)
+#define DEBUG_LVL_10_TRACE1(str,p1)
+#define DEBUG_LVL_10_TRACE2(str,p1,p2)
+#define DEBUG_LVL_10_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_10_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_09)
+#define DEBUG_LVL_09_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_09_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_09_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_09_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_09_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_09_TRACE0(str)
+#define DEBUG_LVL_09_TRACE1(str,p1)
+#define DEBUG_LVL_09_TRACE2(str,p1,p2)
+#define DEBUG_LVL_09_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_09_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_08)
+#define DEBUG_LVL_08_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_08_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_08_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_08_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_08_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_08_TRACE0(str)
+#define DEBUG_LVL_08_TRACE1(str,p1)
+#define DEBUG_LVL_08_TRACE2(str,p1,p2)
+#define DEBUG_LVL_08_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_08_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_07)
+#define DEBUG_LVL_07_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_07_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_07_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_07_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_07_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_07_TRACE0(str)
+#define DEBUG_LVL_07_TRACE1(str,p1)
+#define DEBUG_LVL_07_TRACE2(str,p1,p2)
+#define DEBUG_LVL_07_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_07_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_06)
+#define DEBUG_LVL_06_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_06_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_06_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_06_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_06_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_06_TRACE0(str)
+#define DEBUG_LVL_06_TRACE1(str,p1)
+#define DEBUG_LVL_06_TRACE2(str,p1,p2)
+#define DEBUG_LVL_06_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_06_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_05)
+#define DEBUG_LVL_05_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_05_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_05_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_05_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_05_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_05_TRACE0(str)
+#define DEBUG_LVL_05_TRACE1(str,p1)
+#define DEBUG_LVL_05_TRACE2(str,p1,p2)
+#define DEBUG_LVL_05_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_05_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_04)
+#define DEBUG_LVL_04_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_04_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_04_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_04_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_04_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_04_TRACE0(str)
+#define DEBUG_LVL_04_TRACE1(str,p1)
+#define DEBUG_LVL_04_TRACE2(str,p1,p2)
+#define DEBUG_LVL_04_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_04_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_03)
+#define DEBUG_LVL_03_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_03_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_03_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_03_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_03_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_03_TRACE0(str)
+#define DEBUG_LVL_03_TRACE1(str,p1)
+#define DEBUG_LVL_03_TRACE2(str,p1,p2)
+#define DEBUG_LVL_03_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_03_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_02)
+#define DEBUG_LVL_02_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_02_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_02_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_02_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_02_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_02_TRACE0(str)
+#define DEBUG_LVL_02_TRACE1(str,p1)
+#define DEBUG_LVL_02_TRACE2(str,p1,p2)
+#define DEBUG_LVL_02_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_02_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#if (DEBUG_GLB_LVL() & DEBUG_LVL_01)
+#define DEBUG_LVL_01_TRACE0(str)                    TRACE0(str)
+#define DEBUG_LVL_01_TRACE1(str,p1)                 TRACE1(str,p1)
+#define DEBUG_LVL_01_TRACE2(str,p1,p2)              TRACE2(str,p1,p2)
+#define DEBUG_LVL_01_TRACE3(str,p1,p2,p3)           TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_01_TRACE4(str,p1,p2,p3,p4)        TRACE4(str,p1,p2,p3,p4)
+#else
+#define DEBUG_LVL_01_TRACE0(str)
+#define DEBUG_LVL_01_TRACE1(str,p1)
+#define DEBUG_LVL_01_TRACE2(str,p1,p2)
+#define DEBUG_LVL_01_TRACE3(str,p1,p2,p3)
+#define DEBUG_LVL_01_TRACE4(str,p1,p2,p3,p4)
+#endif
+
+#define DEBUG_TRACE0(lvl,str)                           lvl##_TRACE0(str)
+#define DEBUG_TRACE1(lvl,str,p1)                        lvl##_TRACE1(str,p1)
+#define DEBUG_TRACE2(lvl,str,p1,p2)                     lvl##_TRACE2(str,p1,p2)
+#define DEBUG_TRACE3(lvl,str,p1,p2,p3)                  lvl##_TRACE3(str,p1,p2,p3)
+#define DEBUG_TRACE4(lvl,str,p1,p2,p3,p4)               lvl##_TRACE4(str,p1,p2,p3,p4)
+
+#endif
+
+//---------------------------------------------------------------------------
+// The macro DEBUG_DUMP_DATA() can be used with the same debug-levels to dump
+// out data bytes. Function DumpData() has to be included.
+// NOTE: DUMP_DATA has to be defined in project settings.
+#if (!defined (NDEBUG) && defined (DUMP_DATA)) || (DEV_SYSTEM == _DEV_WIN32_)
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+	void DumpData(char *szStr_p, BYTE MEM * pbData_p, WORD wSize_p);
+
+#ifdef __cplusplus
+}				// von extern "C"
+#endif
+#define DEBUG_DUMP_DATA(lvl,str,ptr,siz)    if ((DEBUG_GLB_LVL() & (lvl))==(lvl)) \
+                                                    DumpData (str, (BYTE MEM*) (ptr), (WORD) (siz));
+#else
+
+#define DEBUG_DUMP_DATA(lvl,str,ptr,siz)
+
+#endif
+
+//---------------------------------------------------------------------------
+// The macro DEBUG_ASSERT() can be used to print out an error string if the
+// parametered expresion does not result TRUE.
+// NOTE: If DEBUG_KEEP_ASSERT is defined, then DEBUG_ASSERT-line will not be
+//       deleted from compiler (in release version too).
+#if !defined (NDEBUG) || defined (DEBUG_KEEP_ASSERT)
+
+#if (DEV_SYSTEM == _DEV_WIN32_)
+
+	// For WIN32 process will be killed after closing message box.
+
+#define DEBUG_ASSERT0(expr,str)         if (!(expr ) && ((DEBUG_GLB_LVL() & DEBUG_LVL_ASSERT)!=0)) { \
+                                                    MessageBox (NULL, \
+                                                        "Assertion failed: line " __LINE__ " file " __FILE__ \
+                                                        "\n    -> " str "\n"); \
+                                                    ExitProcess (-1); }
+
+#define DEBUG_ASSERT1(expr,str,p1)      if (!(expr ) && ((DEBUG_GLB_LVL() & DEBUG_LVL_ASSERT)!=0)) { \
+                                                    MessageBox (NULL, \
+                                                        "Assertion failed: line " __LINE__ " file " __FILE__ \
+                                                        "\n    -> " str "\n"); \
+                                                    ExitProcess (-1); }
+
+#else
+
+	// For microcontrollers process will be stopped using endless loop.
+
+#define DEBUG_ASSERT0(expr,str)         if (!(expr )) { \
+                                                    DEBUG_LVL_ASSERT_TRACE3 ( \
+                                                        "Assertion failed: line %d file '%s'\n" \
+                                                        "    -> '%s'\n", __LINE__, __FILE__, str); \
+                                                    while (1); }
+
+#define DEBUG_ASSERT1(expr,str,p1)      if (!(expr )) { \
+                                                    DEBUG_LVL_ASSERT_TRACE4 ( \
+                                                        "Assertion failed: line %d file '%s'\n" \
+                                                        "    -> '%s'\n" \
+                                                        "    -> 0x%08lX\n", __LINE__, __FILE__, str, (DWORD) p1); \
+                                                    while (1); }
+
+#endif
+
+#else
+
+#define DEBUG_ASSERT0(expr,str)
+#define DEBUG_ASSERT1(expr,str,p1)
+
+#endif
+
+//---------------------------------------------------------------------------
+// The macro DEBUG_ONLY() implements code, if NDEBUG is not defined.
+#if !defined (DEBUG_ONLY)
+#if !defined (NDEBUG)
+
+#define DEBUG_ONLY(expr)    expr
+
+#else
+
+#define DEBUG_ONLY(expr)
+
+#endif
+#endif
+
+#endif // _DEBUG_H_
diff --git a/drivers/staging/epl/Edrv8139.c b/drivers/staging/epl/Edrv8139.c
new file mode 100644
index 00000000000..88ab4a4f102
--- /dev/null
+++ b/drivers/staging/epl/Edrv8139.c
@@ -0,0 +1,1252 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  Ethernet driver for Realtek RTL8139 chips
+                except the RTL8139C+, because it has a different
+                Tx descriptor handling.
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: Edrv8139.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.10 $  $Date: 2008/11/21 09:00:38 $
+
+                $State: Exp $
+
+                Build Environment:
+                Dev C++ and GNU-Compiler for m68k
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2008/02/05 d.k.:   start of implementation
+
+****************************************************************************/
+
+#include "global.h"
+#include "EplInc.h"
+#include "edrv.h"
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/pci.h>
+#include <linux/interrupt.h>
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/major.h>
+#include <linux/version.h>
+#include <asm/io.h>
+#include <asm/uaccess.h>
+#include <asm/atomic.h>
+#include <asm/irq.h>
+#include <linux/sched.h>
+#include <linux/delay.h>
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+// Buffer handling:
+// All buffers are created statically (i.e. at compile time resp. at
+// initialisation via kmalloc() ) and not dynamically on request (i.e. via
+// EdrvAllocTxMsgBuffer().
+// EdrvAllocTxMsgBuffer() searches for an unused buffer which is large enough.
+// EdrvInit() may allocate some buffers with sizes less than maximum frame
+// size (i.e. 1514 bytes), e.g. for SoC, SoA, StatusResponse, IdentResponse,
+// NMT requests / commands. The less the size of the buffer the less the
+// number of the buffer.
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#ifndef EDRV_MAX_TX_BUFFERS
+#define EDRV_MAX_TX_BUFFERS     20
+#endif
+
+#define EDRV_MAX_FRAME_SIZE     0x600
+
+#define EDRV_RX_BUFFER_SIZE     0x8610	// 32 kB + 16 Byte + 1,5 kB (WRAP is enabled)
+#define EDRV_RX_BUFFER_LENGTH   (EDRV_RX_BUFFER_SIZE & 0xF800)	// buffer size cut down to 2 kB alignment
+
+#define EDRV_TX_BUFFER_SIZE     (EDRV_MAX_TX_BUFFERS * EDRV_MAX_FRAME_SIZE)	// n * (MTU + 14 + 4)
+
+#define DRV_NAME                "epl"
+
+#define EDRV_REGW_INT_MASK      0x3C	// interrupt mask register
+#define EDRV_REGW_INT_STATUS    0x3E	// interrupt status register
+#define EDRV_REGW_INT_ROK       0x0001	// Receive OK interrupt
+#define EDRV_REGW_INT_RER       0x0002	// Receive error interrupt
+#define EDRV_REGW_INT_TOK       0x0004	// Transmit OK interrupt
+#define EDRV_REGW_INT_TER       0x0008	// Transmit error interrupt
+#define EDRV_REGW_INT_RXOVW     0x0010	// Rx buffer overflow interrupt
+#define EDRV_REGW_INT_PUN       0x0020	// Packet underrun/ link change interrupt
+#define EDRV_REGW_INT_FOVW      0x0040	// Rx FIFO overflow interrupt
+#define EDRV_REGW_INT_LENCHG    0x2000	// Cable length change interrupt
+#define EDRV_REGW_INT_TIMEOUT   0x4000	// Time out interrupt
+#define EDRV_REGW_INT_SERR      0x8000	// System error interrupt
+#define EDRV_REGW_INT_MASK_DEF  (EDRV_REGW_INT_ROK \
+                                 | EDRV_REGW_INT_RER \
+                                 | EDRV_REGW_INT_TOK \
+                                 | EDRV_REGW_INT_TER \
+                                 | EDRV_REGW_INT_RXOVW \
+                                 | EDRV_REGW_INT_FOVW \
+                                 | EDRV_REGW_INT_PUN \
+                                 | EDRV_REGW_INT_TIMEOUT \
+                                 | EDRV_REGW_INT_SERR)	// default interrupt mask
+
+#define EDRV_REGB_COMMAND       0x37	// command register
+#define EDRV_REGB_COMMAND_RST   0x10
+#define EDRV_REGB_COMMAND_RE    0x08
+#define EDRV_REGB_COMMAND_TE    0x04
+#define EDRV_REGB_COMMAND_BUFE  0x01
+
+#define EDRV_REGB_CMD9346       0x50	// 93C46 command register
+#define EDRV_REGB_CMD9346_LOCK  0x00	// lock configuration registers
+#define EDRV_REGB_CMD9346_UNLOCK 0xC0	// unlock configuration registers
+
+#define EDRV_REGDW_RCR          0x44	// Rx configuration register
+#define EDRV_REGDW_RCR_NO_FTH   0x0000E000	// no receive FIFO threshold
+#define EDRV_REGDW_RCR_RBLEN32K 0x00001000	// 32 kB receive buffer
+#define EDRV_REGDW_RCR_MXDMAUNL 0x00000700	// unlimited maximum DMA burst size
+#define EDRV_REGDW_RCR_NOWRAP   0x00000080	// do not wrap frame at end of buffer
+#define EDRV_REGDW_RCR_AER      0x00000020	// accept error frames (CRC, alignment, collided)
+#define EDRV_REGDW_RCR_AR       0x00000010	// accept runt
+#define EDRV_REGDW_RCR_AB       0x00000008	// accept broadcast frames
+#define EDRV_REGDW_RCR_AM       0x00000004	// accept multicast frames
+#define EDRV_REGDW_RCR_APM      0x00000002	// accept physical match frames
+#define EDRV_REGDW_RCR_AAP      0x00000001	// accept all frames
+#define EDRV_REGDW_RCR_DEF      (EDRV_REGDW_RCR_NO_FTH \
+                                 | EDRV_REGDW_RCR_RBLEN32K \
+                                 | EDRV_REGDW_RCR_MXDMAUNL \
+                                 | EDRV_REGDW_RCR_NOWRAP \
+                                 | EDRV_REGDW_RCR_AB \
+                                 | EDRV_REGDW_RCR_AM \
+                                 | EDRV_REGDW_RCR_APM)	// default value
+
+#define EDRV_REGDW_TCR          0x40	// Tx configuration register
+#define EDRV_REGDW_TCR_VER_MASK 0x7CC00000	// mask for hardware version
+#define EDRV_REGDW_TCR_VER_C    0x74000000	// RTL8139C
+#define EDRV_REGDW_TCR_VER_D    0x74400000	// RTL8139D
+#define EDRV_REGDW_TCR_IFG96    0x03000000	// default interframe gap (960 ns)
+#define EDRV_REGDW_TCR_CRC      0x00010000	// disable appending of CRC by the controller
+#define EDRV_REGDW_TCR_MXDMAUNL 0x00000700	// maximum DMA burst size of 2048 b
+#define EDRV_REGDW_TCR_TXRETRY  0x00000000	// 16 retries
+#define EDRV_REGDW_TCR_DEF      (EDRV_REGDW_TCR_IFG96 \
+                                 | EDRV_REGDW_TCR_MXDMAUNL \
+                                 | EDRV_REGDW_TCR_TXRETRY)
+
+#define EDRV_REGW_MULINT        0x5C	// multiple interrupt select register
+
+#define EDRV_REGDW_MPC          0x4C	// missed packet counter register
+
+#define EDRV_REGDW_TSAD0        0x20	// Transmit start address of descriptor 0
+#define EDRV_REGDW_TSAD1        0x24	// Transmit start address of descriptor 1
+#define EDRV_REGDW_TSAD2        0x28	// Transmit start address of descriptor 2
+#define EDRV_REGDW_TSAD3        0x2C	// Transmit start address of descriptor 3
+#define EDRV_REGDW_TSD0         0x10	// Transmit status of descriptor 0
+#define EDRV_REGDW_TSD_CRS      0x80000000	// Carrier sense lost
+#define EDRV_REGDW_TSD_TABT     0x40000000	// Transmit Abort
+#define EDRV_REGDW_TSD_OWC      0x20000000	// Out of window collision
+#define EDRV_REGDW_TSD_TXTH_DEF 0x00020000	// Transmit FIFO threshold of 64 bytes
+#define EDRV_REGDW_TSD_TOK      0x00008000	// Transmit OK
+#define EDRV_REGDW_TSD_TUN      0x00004000	// Transmit FIFO underrun
+#define EDRV_REGDW_TSD_OWN      0x00002000	// Owner
+
+#define EDRV_REGDW_RBSTART      0x30	// Receive buffer start address
+
+#define EDRV_REGW_CAPR          0x38	// Current address of packet read
+
+#define EDRV_REGDW_IDR0         0x00	// ID register 0
+#define EDRV_REGDW_IDR4         0x04	// ID register 4
+
+#define EDRV_REGDW_MAR0         0x08	// Multicast address register 0
+#define EDRV_REGDW_MAR4         0x0C	// Multicast address register 4
+
+// defines for the status word in the receive buffer
+#define EDRV_RXSTAT_MAR         0x8000	// Multicast address received
+#define EDRV_RXSTAT_PAM         0x4000	// Physical address matched
+#define EDRV_RXSTAT_BAR         0x2000	// Broadcast address received
+#define EDRV_RXSTAT_ISE         0x0020	// Invalid symbol error
+#define EDRV_RXSTAT_RUNT        0x0010	// Runt packet received
+#define EDRV_RXSTAT_LONG        0x0008	// Long packet
+#define EDRV_RXSTAT_CRC         0x0004	// CRC error
+#define EDRV_RXSTAT_FAE         0x0002	// Frame alignment error
+#define EDRV_RXSTAT_ROK         0x0001	// Receive OK
+
+#define EDRV_REGDW_WRITE(dwReg, dwVal)  writel(dwVal, EdrvInstance_l.m_pIoAddr + dwReg)
+#define EDRV_REGW_WRITE(dwReg, wVal)    writew(wVal, EdrvInstance_l.m_pIoAddr + dwReg)
+#define EDRV_REGB_WRITE(dwReg, bVal)    writeb(bVal, EdrvInstance_l.m_pIoAddr + dwReg)
+#define EDRV_REGDW_READ(dwReg)          readl(EdrvInstance_l.m_pIoAddr + dwReg)
+#define EDRV_REGW_READ(dwReg)           readw(EdrvInstance_l.m_pIoAddr + dwReg)
+#define EDRV_REGB_READ(dwReg)           readb(EdrvInstance_l.m_pIoAddr + dwReg)
+
+// TracePoint support for realtime-debugging
+#ifdef _DBG_TRACE_POINTS_
+void PUBLIC TgtDbgSignalTracePoint(BYTE bTracePointNumber_p);
+void PUBLIC TgtDbgPostTraceValue(DWORD dwTraceValue_p);
+#define TGT_DBG_SIGNAL_TRACE_POINT(p)   TgtDbgSignalTracePoint(p)
+#define TGT_DBG_POST_TRACE_VALUE(v)     TgtDbgPostTraceValue(v)
+#else
+#define TGT_DBG_SIGNAL_TRACE_POINT(p)
+#define TGT_DBG_POST_TRACE_VALUE(v)
+#endif
+
+#define EDRV_COUNT_SEND                 TGT_DBG_SIGNAL_TRACE_POINT(2)
+#define EDRV_COUNT_TIMEOUT              TGT_DBG_SIGNAL_TRACE_POINT(3)
+#define EDRV_COUNT_PCI_ERR              TGT_DBG_SIGNAL_TRACE_POINT(4)
+#define EDRV_COUNT_TX                   TGT_DBG_SIGNAL_TRACE_POINT(5)
+#define EDRV_COUNT_RX                   TGT_DBG_SIGNAL_TRACE_POINT(6)
+#define EDRV_COUNT_LATECOLLISION        TGT_DBG_SIGNAL_TRACE_POINT(10)
+#define EDRV_COUNT_TX_COL_RL            TGT_DBG_SIGNAL_TRACE_POINT(11)
+#define EDRV_COUNT_TX_FUN               TGT_DBG_SIGNAL_TRACE_POINT(12)
+#define EDRV_COUNT_TX_ERR               TGT_DBG_SIGNAL_TRACE_POINT(13)
+#define EDRV_COUNT_RX_CRC               TGT_DBG_SIGNAL_TRACE_POINT(14)
+#define EDRV_COUNT_RX_ERR               TGT_DBG_SIGNAL_TRACE_POINT(15)
+#define EDRV_COUNT_RX_FOVW              TGT_DBG_SIGNAL_TRACE_POINT(16)
+#define EDRV_COUNT_RX_PUN               TGT_DBG_SIGNAL_TRACE_POINT(17)
+#define EDRV_COUNT_RX_FAE               TGT_DBG_SIGNAL_TRACE_POINT(18)
+#define EDRV_COUNT_RX_OVW               TGT_DBG_SIGNAL_TRACE_POINT(19)
+
+#define EDRV_TRACE_CAPR(x)              TGT_DBG_POST_TRACE_VALUE(((x) & 0xFFFF) | 0x06000000)
+#define EDRV_TRACE_RX_CRC(x)            TGT_DBG_POST_TRACE_VALUE(((x) & 0xFFFF) | 0x0E000000)
+#define EDRV_TRACE_RX_ERR(x)            TGT_DBG_POST_TRACE_VALUE(((x) & 0xFFFF) | 0x0F000000)
+#define EDRV_TRACE_RX_PUN(x)            TGT_DBG_POST_TRACE_VALUE(((x) & 0xFFFF) | 0x11000000)
+#define EDRV_TRACE(x)                   TGT_DBG_POST_TRACE_VALUE(((x) & 0xFFFF0000) | 0x0000FEC0)
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+/*
+typedef struct
+{
+    BOOL            m_fUsed;
+    unsigned int    m_uiSize;
+    MCD_bufDescFec *m_pBufDescr;
+
+} tEdrvTxBufferIntern;
+*/
+
+// Private structure
+typedef struct {
+	struct pci_dev *m_pPciDev;	// pointer to PCI device structure
+	void *m_pIoAddr;	// pointer to register space of Ethernet controller
+	BYTE *m_pbRxBuf;	// pointer to Rx buffer
+	dma_addr_t m_pRxBufDma;
+	BYTE *m_pbTxBuf;	// pointer to Tx buffer
+	dma_addr_t m_pTxBufDma;
+	BOOL m_afTxBufUsed[EDRV_MAX_TX_BUFFERS];
+	unsigned int m_uiCurTxDesc;
+
+	tEdrvInitParam m_InitParam;
+	tEdrvTxBuffer *m_pLastTransmittedTxBuffer;
+
+} tEdrvInstance;
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+static int EdrvInitOne(struct pci_dev *pPciDev,
+		       const struct pci_device_id *pId);
+
+static void EdrvRemoveOne(struct pci_dev *pPciDev);
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+// buffers and buffer descriptors and pointers
+
+static struct pci_device_id aEdrvPciTbl[] = {
+	{0x10ec, 0x8139, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{0,}
+};
+
+MODULE_DEVICE_TABLE(pci, aEdrvPciTbl);
+
+static tEdrvInstance EdrvInstance_l;
+
+static struct pci_driver EdrvDriver = {
+	.name = DRV_NAME,
+	.id_table = aEdrvPciTbl,
+	.probe = EdrvInitOne,
+	.remove = EdrvRemoveOne,
+};
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  <edrv>                                              */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description:
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   D E F I N I T I O N S                          //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+static BYTE EdrvCalcHash(BYTE * pbMAC_p);
+
+//---------------------------------------------------------------------------
+//
+// Function:    EdrvInit
+//
+// Description: function for init of the Ethernet controller
+//
+// Parameters:  pEdrvInitParam_p    = pointer to struct including the init-parameters
+//
+// Returns:     Errorcode           = kEplSuccessful
+//                                  = kEplNoResource
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel EdrvInit(tEdrvInitParam * pEdrvInitParam_p)
+{
+	tEplKernel Ret;
+	int iResult;
+
+	Ret = kEplSuccessful;
+
+	// clear instance structure
+	EPL_MEMSET(&EdrvInstance_l, 0, sizeof(EdrvInstance_l));
+
+	// save the init data
+	EdrvInstance_l.m_InitParam = *pEdrvInitParam_p;
+
+	// register PCI driver
+	iResult = pci_register_driver(&EdrvDriver);
+	if (iResult != 0) {
+		printk("%s pci_register_driver failed with %d\n", __FUNCTION__,
+		       iResult);
+		Ret = kEplNoResource;
+		goto Exit;
+	}
+
+	if (EdrvInstance_l.m_pPciDev == NULL) {
+		printk("%s m_pPciDev=NULL\n", __FUNCTION__);
+		Ret = kEplNoResource;
+		goto Exit;
+	}
+	// read MAC address from controller
+	printk("%s local MAC = ", __FUNCTION__);
+	for (iResult = 0; iResult < 6; iResult++) {
+		pEdrvInitParam_p->m_abMyMacAddr[iResult] =
+		    EDRV_REGB_READ((EDRV_REGDW_IDR0 + iResult));
+		printk("%02X ",
+		       (unsigned int)pEdrvInitParam_p->m_abMyMacAddr[iResult]);
+	}
+	printk("\n");
+
+      Exit:
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EdrvShutdown
+//
+// Description: Shutdown the Ethernet controller
+//
+// Parameters:  void
+//
+// Returns:     Errorcode   = kEplSuccessful
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel EdrvShutdown(void)
+{
+
+	// unregister PCI driver
+	printk("%s calling pci_unregister_driver()\n", __FUNCTION__);
+	pci_unregister_driver(&EdrvDriver);
+
+	return kEplSuccessful;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EdrvDefineRxMacAddrEntry
+//
+// Description: Set a multicast entry into the Ethernet controller
+//
+// Parameters:  pbMacAddr_p     = pointer to multicast entry to set
+//
+// Returns:     Errorcode       = kEplSuccessful
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel EdrvDefineRxMacAddrEntry(BYTE * pbMacAddr_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	DWORD dwData;
+	BYTE bHash;
+
+	bHash = EdrvCalcHash(pbMacAddr_p);
+/*
+    dwData = ether_crc(6, pbMacAddr_p);
+
+    printk("EdrvDefineRxMacAddrEntry('%02X:%02X:%02X:%02X:%02X:%02X') hash = %u / %u  ether_crc = 0x%08lX\n",
+        (WORD) pbMacAddr_p[0], (WORD) pbMacAddr_p[1], (WORD) pbMacAddr_p[2],
+        (WORD) pbMacAddr_p[3], (WORD) pbMacAddr_p[4], (WORD) pbMacAddr_p[5],
+        (WORD) bHash, (WORD) (dwData >> 26), dwData);
+*/
+	if (bHash > 31) {
+		dwData = EDRV_REGDW_READ(EDRV_REGDW_MAR4);
+		dwData |= 1 << (bHash - 32);
+		EDRV_REGDW_WRITE(EDRV_REGDW_MAR4, dwData);
+	} else {
+		dwData = EDRV_REGDW_READ(EDRV_REGDW_MAR0);
+		dwData |= 1 << bHash;
+		EDRV_REGDW_WRITE(EDRV_REGDW_MAR0, dwData);
+	}
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EdrvUndefineRxMacAddrEntry
+//
+// Description: Reset a multicast entry in the Ethernet controller
+//
+// Parameters:  pbMacAddr_p     = pointer to multicast entry to reset
+//
+// Returns:     Errorcode       = kEplSuccessful
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel EdrvUndefineRxMacAddrEntry(BYTE * pbMacAddr_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	DWORD dwData;
+	BYTE bHash;
+
+	bHash = EdrvCalcHash(pbMacAddr_p);
+
+	if (bHash > 31) {
+		dwData = EDRV_REGDW_READ(EDRV_REGDW_MAR4);
+		dwData &= ~(1 << (bHash - 32));
+		EDRV_REGDW_WRITE(EDRV_REGDW_MAR4, dwData);
+	} else {
+		dwData = EDRV_REGDW_READ(EDRV_REGDW_MAR0);
+		dwData &= ~(1 << bHash);
+		EDRV_REGDW_WRITE(EDRV_REGDW_MAR0, dwData);
+	}
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EdrvAllocTxMsgBuffer
+//
+// Description: Register a Tx-Buffer
+//
+// Parameters:  pBuffer_p   = pointer to Buffer structure
+//
+// Returns:     Errorcode   = kEplSuccessful
+//                          = kEplEdrvNoFreeBufEntry
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel EdrvAllocTxMsgBuffer(tEdrvTxBuffer * pBuffer_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	DWORD i;
+
+	if (pBuffer_p->m_uiMaxBufferLen > EDRV_MAX_FRAME_SIZE) {
+		Ret = kEplEdrvNoFreeBufEntry;
+		goto Exit;
+	}
+	// search a free Tx buffer with appropriate size
+	for (i = 0; i < EDRV_MAX_TX_BUFFERS; i++) {
+		if (EdrvInstance_l.m_afTxBufUsed[i] == FALSE) {
+			// free channel found
+			EdrvInstance_l.m_afTxBufUsed[i] = TRUE;
+			pBuffer_p->m_uiBufferNumber = i;
+			pBuffer_p->m_pbBuffer =
+			    EdrvInstance_l.m_pbTxBuf +
+			    (i * EDRV_MAX_FRAME_SIZE);
+			pBuffer_p->m_uiMaxBufferLen = EDRV_MAX_FRAME_SIZE;
+			break;
+		}
+	}
+	if (i >= EDRV_MAX_TX_BUFFERS) {
+		Ret = kEplEdrvNoFreeBufEntry;
+		goto Exit;
+	}
+
+      Exit:
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EdrvReleaseTxMsgBuffer
+//
+// Description: Register a Tx-Buffer
+//
+// Parameters:  pBuffer_p   = pointer to Buffer structure
+//
+// Returns:     Errorcode   = kEplSuccessful
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel EdrvReleaseTxMsgBuffer(tEdrvTxBuffer * pBuffer_p)
+{
+	unsigned int uiBufferNumber;
+
+	uiBufferNumber = pBuffer_p->m_uiBufferNumber;
+
+	if (uiBufferNumber < EDRV_MAX_TX_BUFFERS) {
+		EdrvInstance_l.m_afTxBufUsed[uiBufferNumber] = FALSE;
+	}
+
+	return kEplSuccessful;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EdrvSendTxMsg
+//
+// Description: immediately starts the transmission of the buffer
+//
+// Parameters:  pBuffer_p   = buffer descriptor to transmit
+//
+// Returns:     Errorcode   = kEplSuccessful
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel EdrvSendTxMsg(tEdrvTxBuffer * pBuffer_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	unsigned int uiBufferNumber;
+	DWORD dwTemp;
+
+	uiBufferNumber = pBuffer_p->m_uiBufferNumber;
+
+	if ((uiBufferNumber >= EDRV_MAX_TX_BUFFERS)
+	    || (EdrvInstance_l.m_afTxBufUsed[uiBufferNumber] == FALSE)) {
+		Ret = kEplEdrvBufNotExisting;
+		goto Exit;
+	}
+
+	if (EdrvInstance_l.m_pLastTransmittedTxBuffer != NULL) {	// transmission is already active
+		Ret = kEplInvalidOperation;
+		dwTemp =
+		    EDRV_REGDW_READ((EDRV_REGDW_TSD0 +
+				     (EdrvInstance_l.m_uiCurTxDesc *
+				      sizeof(DWORD))));
+		printk("%s InvOp TSD%u = 0x%08lX", __FUNCTION__,
+		       EdrvInstance_l.m_uiCurTxDesc, dwTemp);
+		printk("  Cmd = 0x%02X\n",
+		       (WORD) EDRV_REGB_READ(EDRV_REGB_COMMAND));
+		goto Exit;
+	}
+	// save pointer to buffer structure for TxHandler
+	EdrvInstance_l.m_pLastTransmittedTxBuffer = pBuffer_p;
+
+	EDRV_COUNT_SEND;
+
+	// pad with zeros if necessary, because controller does not do it
+	if (pBuffer_p->m_uiTxMsgLen < MIN_ETH_SIZE) {
+		EPL_MEMSET(pBuffer_p->m_pbBuffer + pBuffer_p->m_uiTxMsgLen, 0,
+			   MIN_ETH_SIZE - pBuffer_p->m_uiTxMsgLen);
+		pBuffer_p->m_uiTxMsgLen = MIN_ETH_SIZE;
+	}
+	// set DMA address of buffer
+	EDRV_REGDW_WRITE((EDRV_REGDW_TSAD0 +
+			  (EdrvInstance_l.m_uiCurTxDesc * sizeof(DWORD))),
+			 (EdrvInstance_l.m_pTxBufDma +
+			  (uiBufferNumber * EDRV_MAX_FRAME_SIZE)));
+	dwTemp =
+	    EDRV_REGDW_READ((EDRV_REGDW_TSAD0 +
+			     (EdrvInstance_l.m_uiCurTxDesc * sizeof(DWORD))));
+//    printk("%s TSAD%u = 0x%08lX", __FUNCTION__, EdrvInstance_l.m_uiCurTxDesc, dwTemp);
+
+	// start transmission
+	EDRV_REGDW_WRITE((EDRV_REGDW_TSD0 +
+			  (EdrvInstance_l.m_uiCurTxDesc * sizeof(DWORD))),
+			 (EDRV_REGDW_TSD_TXTH_DEF | pBuffer_p->m_uiTxMsgLen));
+	dwTemp =
+	    EDRV_REGDW_READ((EDRV_REGDW_TSD0 +
+			     (EdrvInstance_l.m_uiCurTxDesc * sizeof(DWORD))));
+//    printk(" TSD%u = 0x%08lX / 0x%08lX\n", EdrvInstance_l.m_uiCurTxDesc, dwTemp, (DWORD)(EDRV_REGDW_TSD_TXTH_DEF | pBuffer_p->m_uiTxMsgLen));
+
+      Exit:
+	return Ret;
+}
+
+#if 0
+//---------------------------------------------------------------------------
+//
+// Function:    EdrvTxMsgReady
+//
+// Description: starts copying the buffer to the ethernet controller's FIFO
+//
+// Parameters:  pbBuffer_p - bufferdescriptor to transmit
+//
+// Returns:     Errorcode - kEplSuccessful
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel EdrvTxMsgReady(tEdrvTxBuffer * pBuffer_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	unsigned int uiBufferNumber;
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EdrvTxMsgStart
+//
+// Description: starts transmission of the ethernet controller's FIFO
+//
+// Parameters:  pbBuffer_p - bufferdescriptor to transmit
+//
+// Returns:     Errorcode - kEplSuccessful
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel EdrvTxMsgStart(tEdrvTxBuffer * pBuffer_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	return Ret;
+}
+#endif
+
+//---------------------------------------------------------------------------
+//
+// Function:    EdrvReinitRx
+//
+// Description: reinitialize the Rx process, because of error
+//
+// Parameters:  void
+//
+// Returns:     void
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static void EdrvReinitRx(void)
+{
+	BYTE bCmd;
+
+	// simply switch off and on the receiver
+	// this will reset the CAPR register
+	bCmd = EDRV_REGB_READ(EDRV_REGB_COMMAND);
+	EDRV_REGB_WRITE(EDRV_REGB_COMMAND, (bCmd & ~EDRV_REGB_COMMAND_RE));
+	EDRV_REGB_WRITE(EDRV_REGB_COMMAND, bCmd);
+
+	// set receive configuration register
+	EDRV_REGDW_WRITE(EDRV_REGDW_RCR, EDRV_REGDW_RCR_DEF);
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:     EdrvInterruptHandler
+//
+// Description:  interrupt handler
+//
+// Parameters:   void
+//
+// Returns:      void
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#if 0
+void EdrvInterruptHandler(void)
+{
+}
+#endif
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,19)
+static int TgtEthIsr(int nIrqNum_p, void *ppDevInstData_p)
+#else
+static int TgtEthIsr(int nIrqNum_p, void *ppDevInstData_p,
+		     struct pt_regs *ptRegs_p)
+#endif
+{
+//    EdrvInterruptHandler();
+	tEdrvRxBuffer RxBuffer;
+	tEdrvTxBuffer *pTxBuffer;
+	WORD wStatus;
+	DWORD dwTxStatus;
+	DWORD dwRxStatus;
+	WORD wCurRx;
+	BYTE *pbRxBuf;
+	unsigned int uiLength;
+	int iHandled = IRQ_HANDLED;
+
+//    printk("¤");
+
+	// read the interrupt status
+	wStatus = EDRV_REGW_READ(EDRV_REGW_INT_STATUS);
+
+	// acknowledge the interrupts
+	EDRV_REGW_WRITE(EDRV_REGW_INT_STATUS, wStatus);
+
+	if (wStatus == 0) {
+		iHandled = IRQ_NONE;
+		goto Exit;
+	}
+	// process tasks
+	if ((wStatus & (EDRV_REGW_INT_TER | EDRV_REGW_INT_TOK)) != 0) {	// transmit interrupt
+
+		if (EdrvInstance_l.m_pbTxBuf == NULL) {
+			printk("%s Tx buffers currently not allocated\n",
+			       __FUNCTION__);
+			goto Exit;
+		}
+		// read transmit status
+		dwTxStatus =
+		    EDRV_REGDW_READ((EDRV_REGDW_TSD0 +
+				     (EdrvInstance_l.m_uiCurTxDesc *
+				      sizeof(DWORD))));
+		if ((dwTxStatus & (EDRV_REGDW_TSD_TOK | EDRV_REGDW_TSD_TABT | EDRV_REGDW_TSD_TUN)) != 0) {	// transmit finished
+			EdrvInstance_l.m_uiCurTxDesc =
+			    (EdrvInstance_l.m_uiCurTxDesc + 1) & 0x03;
+			pTxBuffer = EdrvInstance_l.m_pLastTransmittedTxBuffer;
+			EdrvInstance_l.m_pLastTransmittedTxBuffer = NULL;
+
+			if ((dwTxStatus & EDRV_REGDW_TSD_TOK) != 0) {
+				EDRV_COUNT_TX;
+			} else if ((dwTxStatus & EDRV_REGDW_TSD_TUN) != 0) {
+				EDRV_COUNT_TX_FUN;
+			} else {	// assume EDRV_REGDW_TSD_TABT
+				EDRV_COUNT_TX_COL_RL;
+			}
+
+//            printk("T");
+			if (pTxBuffer != NULL) {
+				// call Tx handler of Data link layer
+				EdrvInstance_l.m_InitParam.
+				    m_pfnTxHandler(pTxBuffer);
+			}
+		} else {
+			EDRV_COUNT_TX_ERR;
+		}
+	}
+
+	if ((wStatus & (EDRV_REGW_INT_RER | EDRV_REGW_INT_FOVW | EDRV_REGW_INT_RXOVW | EDRV_REGW_INT_PUN)) != 0) {	// receive error interrupt
+
+		if ((wStatus & EDRV_REGW_INT_FOVW) != 0) {
+			EDRV_COUNT_RX_FOVW;
+		} else if ((wStatus & EDRV_REGW_INT_RXOVW) != 0) {
+			EDRV_COUNT_RX_OVW;
+		} else if ((wStatus & EDRV_REGW_INT_PUN) != 0) {	// Packet underrun
+			EDRV_TRACE_RX_PUN(wStatus);
+			EDRV_COUNT_RX_PUN;
+		} else {	/*if ((wStatus & EDRV_REGW_INT_RER) != 0) */
+
+			EDRV_TRACE_RX_ERR(wStatus);
+			EDRV_COUNT_RX_ERR;
+		}
+
+		// reinitialize Rx process
+		EdrvReinitRx();
+	}
+
+	if ((wStatus & EDRV_REGW_INT_ROK) != 0) {	// receive interrupt
+
+		if (EdrvInstance_l.m_pbRxBuf == NULL) {
+			printk("%s Rx buffers currently not allocated\n",
+			       __FUNCTION__);
+			goto Exit;
+		}
+		// read current offset in receive buffer
+		wCurRx =
+		    (EDRV_REGW_READ(EDRV_REGW_CAPR) +
+		     0x10) % EDRV_RX_BUFFER_LENGTH;
+
+		while ((EDRV_REGB_READ(EDRV_REGB_COMMAND) & EDRV_REGB_COMMAND_BUFE) == 0) {	// frame available
+
+			// calculate pointer to current frame in receive buffer
+			pbRxBuf = EdrvInstance_l.m_pbRxBuf + wCurRx;
+
+			// read receive status DWORD
+			dwRxStatus = le32_to_cpu(*((DWORD *) pbRxBuf));
+
+			// calculate length of received frame
+			uiLength = dwRxStatus >> 16;
+
+			if (uiLength == 0xFFF0) {	// frame is unfinished (maybe early Rx interrupt is active)
+				break;
+			}
+
+			if ((dwRxStatus & EDRV_RXSTAT_ROK) == 0) {	// error occured while receiving this frame
+				// ignore it
+				if ((dwRxStatus & EDRV_RXSTAT_FAE) != 0) {
+					EDRV_COUNT_RX_FAE;
+				} else if ((dwRxStatus & EDRV_RXSTAT_CRC) != 0) {
+					EDRV_TRACE_RX_CRC(dwRxStatus);
+					EDRV_COUNT_RX_CRC;
+				} else {
+					EDRV_TRACE_RX_ERR(dwRxStatus);
+					EDRV_COUNT_RX_ERR;
+				}
+
+				// reinitialize Rx process
+				EdrvReinitRx();
+
+				break;
+			} else {	// frame is OK
+				RxBuffer.m_BufferInFrame =
+				    kEdrvBufferLastInFrame;
+				RxBuffer.m_uiRxMsgLen = uiLength - ETH_CRC_SIZE;
+				RxBuffer.m_pbBuffer =
+				    pbRxBuf + sizeof(dwRxStatus);
+
+//                printk("R");
+				EDRV_COUNT_RX;
+
+				// call Rx handler of Data link layer
+				EdrvInstance_l.m_InitParam.
+				    m_pfnRxHandler(&RxBuffer);
+			}
+
+			// calulate new offset (DWORD aligned)
+			wCurRx =
+			    (WORD) ((wCurRx + uiLength + sizeof(dwRxStatus) +
+				     3) & ~0x3);
+			EDRV_TRACE_CAPR(wCurRx - 0x10);
+			EDRV_REGW_WRITE(EDRV_REGW_CAPR, wCurRx - 0x10);
+
+			// reread current offset in receive buffer
+			wCurRx =
+			    (EDRV_REGW_READ(EDRV_REGW_CAPR) +
+			     0x10) % EDRV_RX_BUFFER_LENGTH;
+
+		}
+	}
+
+	if ((wStatus & EDRV_REGW_INT_SERR) != 0) {	// PCI error
+		EDRV_COUNT_PCI_ERR;
+	}
+
+	if ((wStatus & EDRV_REGW_INT_TIMEOUT) != 0) {	// Timeout
+		EDRV_COUNT_TIMEOUT;
+	}
+
+      Exit:
+	return iHandled;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EdrvInitOne
+//
+// Description: initializes one PCI device
+//
+// Parameters:  pPciDev             = pointer to corresponding PCI device structure
+//              pId                 = PCI device ID
+//
+// Returns:     (int)               = error code
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static int EdrvInitOne(struct pci_dev *pPciDev, const struct pci_device_id *pId)
+{
+	int iResult = 0;
+	DWORD dwTemp;
+
+	if (EdrvInstance_l.m_pPciDev != NULL) {	// Edrv is already connected to a PCI device
+		printk("%s device %s discarded\n", __FUNCTION__,
+		       pci_name(pPciDev));
+		iResult = -ENODEV;
+		goto Exit;
+	}
+
+	if (pPciDev->revision >= 0x20) {
+		printk
+		    ("%s device %s is an enhanced 8139C+ version, which is not supported\n",
+		     __FUNCTION__, pci_name(pPciDev));
+		iResult = -ENODEV;
+		goto Exit;
+	}
+
+	EdrvInstance_l.m_pPciDev = pPciDev;
+
+	// enable device
+	printk("%s enable device\n", __FUNCTION__);
+	iResult = pci_enable_device(pPciDev);
+	if (iResult != 0) {
+		goto Exit;
+	}
+
+	if ((pci_resource_flags(pPciDev, 1) & IORESOURCE_MEM) == 0) {
+		iResult = -ENODEV;
+		goto Exit;
+	}
+
+	printk("%s request regions\n", __FUNCTION__);
+	iResult = pci_request_regions(pPciDev, DRV_NAME);
+	if (iResult != 0) {
+		goto Exit;
+	}
+
+	printk("%s ioremap\n", __FUNCTION__);
+	EdrvInstance_l.m_pIoAddr =
+	    ioremap(pci_resource_start(pPciDev, 1),
+		    pci_resource_len(pPciDev, 1));
+	if (EdrvInstance_l.m_pIoAddr == NULL) {	// remap of controller's register space failed
+		iResult = -EIO;
+		goto Exit;
+	}
+	// enable PCI busmaster
+	printk("%s enable busmaster\n", __FUNCTION__);
+	pci_set_master(pPciDev);
+
+	// reset controller
+	printk("%s reset controller\n", __FUNCTION__);
+	EDRV_REGB_WRITE(EDRV_REGB_COMMAND, EDRV_REGB_COMMAND_RST);
+
+	// wait until reset has finished
+	for (iResult = 500; iResult > 0; iResult--) {
+		if ((EDRV_REGB_READ(EDRV_REGB_COMMAND) & EDRV_REGB_COMMAND_RST)
+		    == 0) {
+			break;
+		}
+
+		schedule_timeout(10);
+	}
+
+	// check hardware version, i.e. chip ID
+	dwTemp = EDRV_REGDW_READ(EDRV_REGDW_TCR);
+	if (((dwTemp & EDRV_REGDW_TCR_VER_MASK) != EDRV_REGDW_TCR_VER_C)
+	    && ((dwTemp & EDRV_REGDW_TCR_VER_MASK) != EDRV_REGDW_TCR_VER_D)) {	// unsupported chip
+		printk("%s Unsupported chip! TCR = 0x%08lX\n", __FUNCTION__,
+		       dwTemp);
+		iResult = -ENODEV;
+		goto Exit;
+	}
+	// disable interrupts
+	printk("%s disable interrupts\n", __FUNCTION__);
+	EDRV_REGW_WRITE(EDRV_REGW_INT_MASK, 0);
+	// acknowledge all pending interrupts
+	EDRV_REGW_WRITE(EDRV_REGW_INT_STATUS,
+			EDRV_REGW_READ(EDRV_REGW_INT_STATUS));
+
+	// install interrupt handler
+	printk("%s install interrupt handler\n", __FUNCTION__);
+	iResult =
+	    request_irq(pPciDev->irq, TgtEthIsr, IRQF_SHARED,
+			DRV_NAME /*pPciDev->dev.name */ , pPciDev);
+	if (iResult != 0) {
+		goto Exit;
+	}
+
+/*
+    // unlock configuration registers
+    printk("%s unlock configuration registers\n", __FUNCTION__);
+    EDRV_REGB_WRITE(EDRV_REGB_CMD9346, EDRV_REGB_CMD9346_UNLOCK);
+
+    // check if user specified a MAC address
+    printk("%s check specified MAC address\n", __FUNCTION__);
+    for (iResult = 0; iResult < 6; iResult++)
+    {
+        if (EdrvInstance_l.m_InitParam.m_abMyMacAddr[iResult] != 0)
+        {
+            printk("%s set local MAC address\n", __FUNCTION__);
+            // write this MAC address to controller
+            EDRV_REGDW_WRITE(EDRV_REGDW_IDR0,
+                le32_to_cpu(*((DWORD*)&EdrvInstance_l.m_InitParam.m_abMyMacAddr[0])));
+            dwTemp = EDRV_REGDW_READ(EDRV_REGDW_IDR0);
+
+            EDRV_REGDW_WRITE(EDRV_REGDW_IDR4,
+                le32_to_cpu(*((DWORD*)&EdrvInstance_l.m_InitParam.m_abMyMacAddr[4])));
+            dwTemp = EDRV_REGDW_READ(EDRV_REGDW_IDR4);
+            break;
+        }
+    }
+    iResult = 0;
+
+    // lock configuration registers
+    EDRV_REGB_WRITE(EDRV_REGB_CMD9346, EDRV_REGB_CMD9346_LOCK);
+*/
+
+	// allocate buffers
+	printk("%s allocate buffers\n", __FUNCTION__);
+	EdrvInstance_l.m_pbTxBuf =
+	    pci_alloc_consistent(pPciDev, EDRV_TX_BUFFER_SIZE,
+				 &EdrvInstance_l.m_pTxBufDma);
+	if (EdrvInstance_l.m_pbTxBuf == NULL) {
+		iResult = -ENOMEM;
+		goto Exit;
+	}
+
+	EdrvInstance_l.m_pbRxBuf =
+	    pci_alloc_consistent(pPciDev, EDRV_RX_BUFFER_SIZE,
+				 &EdrvInstance_l.m_pRxBufDma);
+	if (EdrvInstance_l.m_pbRxBuf == NULL) {
+		iResult = -ENOMEM;
+		goto Exit;
+	}
+	// reset pointers for Tx buffers
+	printk("%s reset pointers fo Tx buffers\n", __FUNCTION__);
+	EDRV_REGDW_WRITE(EDRV_REGDW_TSAD0, 0);
+	dwTemp = EDRV_REGDW_READ(EDRV_REGDW_TSAD0);
+	EDRV_REGDW_WRITE(EDRV_REGDW_TSAD1, 0);
+	dwTemp = EDRV_REGDW_READ(EDRV_REGDW_TSAD1);
+	EDRV_REGDW_WRITE(EDRV_REGDW_TSAD2, 0);
+	dwTemp = EDRV_REGDW_READ(EDRV_REGDW_TSAD2);
+	EDRV_REGDW_WRITE(EDRV_REGDW_TSAD3, 0);
+	dwTemp = EDRV_REGDW_READ(EDRV_REGDW_TSAD3);
+
+	printk("    Command = 0x%02X\n",
+	       (WORD) EDRV_REGB_READ(EDRV_REGB_COMMAND));
+
+	// set pointer for receive buffer in controller
+	printk("%s set pointer to Rx buffer\n", __FUNCTION__);
+	EDRV_REGDW_WRITE(EDRV_REGDW_RBSTART, EdrvInstance_l.m_pRxBufDma);
+
+	// enable transmitter and receiver
+	printk("%s enable Tx and Rx", __FUNCTION__);
+	EDRV_REGB_WRITE(EDRV_REGB_COMMAND,
+			(EDRV_REGB_COMMAND_RE | EDRV_REGB_COMMAND_TE));
+	printk("  Command = 0x%02X\n",
+	       (WORD) EDRV_REGB_READ(EDRV_REGB_COMMAND));
+
+	// clear missed packet counter to enable Rx/Tx process
+	EDRV_REGDW_WRITE(EDRV_REGDW_MPC, 0);
+
+	// set transmit configuration register
+	printk("%s set Tx conf register", __FUNCTION__);
+	EDRV_REGDW_WRITE(EDRV_REGDW_TCR, EDRV_REGDW_TCR_DEF);
+	printk(" = 0x%08X\n", EDRV_REGDW_READ(EDRV_REGDW_TCR));
+
+	// set receive configuration register
+	printk("%s set Rx conf register", __FUNCTION__);
+	EDRV_REGDW_WRITE(EDRV_REGDW_RCR, EDRV_REGDW_RCR_DEF);
+	printk(" = 0x%08X\n", EDRV_REGDW_READ(EDRV_REGDW_RCR));
+
+	// reset multicast MAC address filter
+	EDRV_REGDW_WRITE(EDRV_REGDW_MAR0, 0);
+	dwTemp = EDRV_REGDW_READ(EDRV_REGDW_MAR0);
+	EDRV_REGDW_WRITE(EDRV_REGDW_MAR4, 0);
+	dwTemp = EDRV_REGDW_READ(EDRV_REGDW_MAR4);
+
+/*
+    // enable transmitter and receiver
+    printk("%s enable Tx and Rx", __FUNCTION__);
+    EDRV_REGB_WRITE(EDRV_REGB_COMMAND, (EDRV_REGB_COMMAND_RE | EDRV_REGB_COMMAND_TE));
+    printk("  Command = 0x%02X\n", (WORD) EDRV_REGB_READ(EDRV_REGB_COMMAND));
+*/
+	// disable early interrupts
+	EDRV_REGW_WRITE(EDRV_REGW_MULINT, 0);
+
+	// enable interrupts
+	printk("%s enable interrupts\n", __FUNCTION__);
+	EDRV_REGW_WRITE(EDRV_REGW_INT_MASK, EDRV_REGW_INT_MASK_DEF);
+
+      Exit:
+	printk("%s finished with %d\n", __FUNCTION__, iResult);
+	return iResult;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EdrvRemoveOne
+//
+// Description: shuts down one PCI device
+//
+// Parameters:  pPciDev             = pointer to corresponding PCI device structure
+//
+// Returns:     (void)
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static void EdrvRemoveOne(struct pci_dev *pPciDev)
+{
+
+	if (EdrvInstance_l.m_pPciDev != pPciDev) {	// trying to remove unknown device
+		BUG_ON(EdrvInstance_l.m_pPciDev != pPciDev);
+		goto Exit;
+	}
+	// disable transmitter and receiver
+	EDRV_REGB_WRITE(EDRV_REGB_COMMAND, 0);
+
+	// disable interrupts
+	EDRV_REGW_WRITE(EDRV_REGW_INT_MASK, 0);
+
+	// remove interrupt handler
+	free_irq(pPciDev->irq, pPciDev);
+
+	// free buffers
+	if (EdrvInstance_l.m_pbTxBuf != NULL) {
+		pci_free_consistent(pPciDev, EDRV_TX_BUFFER_SIZE,
+				    EdrvInstance_l.m_pbTxBuf,
+				    EdrvInstance_l.m_pTxBufDma);
+		EdrvInstance_l.m_pbTxBuf = NULL;
+	}
+
+	if (EdrvInstance_l.m_pbRxBuf != NULL) {
+		pci_free_consistent(pPciDev, EDRV_RX_BUFFER_SIZE,
+				    EdrvInstance_l.m_pbRxBuf,
+				    EdrvInstance_l.m_pRxBufDma);
+		EdrvInstance_l.m_pbRxBuf = NULL;
+	}
+	// unmap controller's register space
+	if (EdrvInstance_l.m_pIoAddr != NULL) {
+		iounmap(EdrvInstance_l.m_pIoAddr);
+	}
+	// disable the PCI device
+	pci_disable_device(pPciDev);
+
+	// release memory regions
+	pci_release_regions(pPciDev);
+
+	EdrvInstance_l.m_pPciDev = NULL;
+
+      Exit:;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EdrvCalcHash
+//
+// Description: function calculates the entry for the hash-table from MAC
+//              address
+//
+// Parameters:  pbMAC_p - pointer to MAC address
+//
+// Returns:     hash value
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#define HASH_BITS              6	// used bits in hash
+#define CRC32_POLY    0x04C11DB6	//
+//#define CRC32_POLY    0xEDB88320  //
+// G(x) = x32 + x26 + x23 + x22 + x16 + x12 + x11 + x10 + x8 + x7 + x5 + x4 + x2 + x + 1
+
+static BYTE EdrvCalcHash(BYTE * pbMAC_p)
+{
+	DWORD dwByteCounter;
+	DWORD dwBitCounter;
+	DWORD dwData;
+	DWORD dwCrc;
+	DWORD dwCarry;
+	BYTE *pbData;
+	BYTE bHash;
+
+	pbData = pbMAC_p;
+
+	// calculate crc32 value of mac address
+	dwCrc = 0xFFFFFFFF;
+
+	for (dwByteCounter = 0; dwByteCounter < 6; dwByteCounter++) {
+		dwData = *pbData;
+		pbData++;
+		for (dwBitCounter = 0; dwBitCounter < 8;
+		     dwBitCounter++, dwData >>= 1) {
+			dwCarry = (((dwCrc >> 31) ^ dwData) & 1);
+			dwCrc = dwCrc << 1;
+			if (dwCarry != 0) {
+				dwCrc = (dwCrc ^ CRC32_POLY) | dwCarry;
+			}
+		}
+	}
+
+//    printk("MyCRC = 0x%08lX\n", dwCrc);
+	// only upper 6 bits (HASH_BITS) are used
+	// which point to specific bit in the hash registers
+	bHash = (BYTE) ((dwCrc >> (32 - HASH_BITS)) & 0x3f);
+
+	return bHash;
+}
diff --git a/drivers/staging/epl/EdrvFec.h b/drivers/staging/epl/EdrvFec.h
new file mode 100644
index 00000000000..5f252fbed74
--- /dev/null
+++ b/drivers/staging/epl/EdrvFec.h
@@ -0,0 +1,114 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  interface for ethernetdriver
+                "fast ethernet controller" (FEC)
+                freescale coldfire MCF528x and compatible FEC
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EdrvFec.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.3 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                Dev C++ and GNU-Compiler for m68k
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2005/08/01 m.b.:   start of implementation
+
+****************************************************************************/
+
+#ifndef _EDRVFEC_H_
+#define _EDRVFEC_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+// do this in config header
+#define TARGET_HARDWARE TGTHW_SPLC_CF54
+
+// base addresses
+#if ((TARGET_HARDWARE & TGT_CPU_MASK_) == TGT_CPU_5282)
+
+#elif ((TARGET_HARDWARE & TGT_CPU_MASK_) == TGT_CPU_5485)
+
+#else
+
+#error 'ERROR: Target was never implemented!'
+
+#endif
+
+//---------------------------------------------------------------------------
+// types
+//---------------------------------------------------------------------------
+
+// Rx and Tx buffer descriptor format
+typedef struct {
+	WORD m_wStatus;		// control / status  ---  used by edrv, do not change in application
+	WORD m_wLength;		// transfer length
+	BYTE *m_pbData;		// buffer address
+} tBufferDescr;
+
+#if ((TARGET_HARDWARE & TGT_CPU_MASK_) == TGT_CPU_5282)
+
+#elif ((TARGET_HARDWARE & TGT_CPU_MASK_) == TGT_CPU_5485)
+
+#endif
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#endif // #ifndef _EDRV_FEC_H_
diff --git a/drivers/staging/epl/EdrvFec5282.h b/drivers/staging/epl/EdrvFec5282.h
new file mode 100644
index 00000000000..a16bb1d4da7
--- /dev/null
+++ b/drivers/staging/epl/EdrvFec5282.h
@@ -0,0 +1,340 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  interface for ethernetdriver
+                "fast ethernet controller" (FEC)
+                freescale coldfire MCF528x and compatible FEC
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EdrvFec5282.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.3 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                Dev C++ and GNU-Compiler for m68k
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2005/08/01 m.b.:   start of implementation
+
+****************************************************************************/
+
+#ifndef _EDRVFEC_H_
+#define _EDRVFEC_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+// base addresses
+#define FEC0_ADDR 0x0000
+#define FEC1_ADDR 0x0000	//tbd
+
+// control / status registers
+#define FEC_EIR                 0x1004	// interrupt event register
+#define FEC_EIMR                0x1008	// interrupt mask register
+#define FEC_RDAR                0x1010	// receive descriptor active register
+#define FEC_TDAR                0x1014	// transmit descriptor active register
+#define FEC_ECR                 0x1024	// ethernet control register
+#define FEC_MMFR                0x1040	// MII data register
+#define FEC_MSCR                0x1044	// MII speed register
+#define FEC_MIBC                0x1064	// MIB control/status register
+#define FEC_RCR                 0x1084	// receive control register
+#define FEC_TCR                 0x10C4	// transmit control register
+#define FEC_PALR                0x10E4	// physical address low register
+#define FEC_PAUR                0x10E8	// physical address high + type register
+#define FEC_OPD                 0x10EC	// opcode + pause register
+#define FEC_IAUR                0x1118	// upper 32 bit of individual hash table
+#define FEC_IALR                0x111C	// lower 32 bit of individual hash table
+#define FEC_GAUR                0x1120	// upper 32 bit of group hash table
+#define FEC_GALR                0x1124	// lower 32 bit of group hash table
+#define FEC_TFWR                0x1144	// transmit FIFO watermark
+#define FEC_FRBR                0x114C	// FIFO receive bound register
+#define FEC_FRSR                0x1150	// FIFO receive FIFO start register
+#define FEC_ERDSR               0x1180	// pointer to receive descriptor ring
+#define FEC_ETDSR               0x1184	// pointer to transmit descriptor ring
+#define FEC_EMRBR               0x1188	// maximum receive buffer size
+
+// mib block counters memory map
+#define FEC_RMON_T_DROP         0x1200	// count of frames not counted correctly
+#define FEC_RMON_T_PACKETS      0x1204	// RMON tx packet count
+#define FEC_RMON_T_BC_PKT       0x1208	// RMON tx broadcast packets
+#define FEC_RMON_T_MC_PKT       0x120C	// RMON tx multicast packets
+#define FEC_RMON_T_CRC_ALIGN    0x1210	// RMON tx packets w CRC/align error
+#define FEC_RMON_T_UNDERSIZE    0x1214	// RMON tx packets < 64 bytes, good CRC
+#define FEC_RMON_T_OVERSIZE     0x1218	// RMON tx packets > MAX_FL bytes, good CRC
+#define FEC_RMON_T_FRAG         0x121C	// RMON tx packets < 64 bytes, bad CRC
+#define FEC_RMON_T_JAB          0x1220	// RMON tx packets > MAX_FL bytes, bad CRC
+#define FEC_RMON_T_COL          0x1224	// RMON tx collision count
+#define FEC_RMON_T_P64          0x1228	// RMON tx           64 byte packets
+#define FEC_RMON_T_P65TO127     0x122C	// RMON tx   65 to  127 byte packets
+#define FEC_RMON_T_P128TO255    0x1230	// RMON tx  128 to  255 byte packets
+#define FEC_RMON_T_P256TO511    0x1234	// RMON tx  256 to  511 byte packets
+#define FEC_RMON_T_P512TO1023   0x1238	// RMON tx  512 to 1023 byte packets
+#define FEC_RMON_T_P1024TO2047  0x123C	// RMON tx 1024 to 2047 byte packets
+#define FEC_RMON_T_P_GTE2048    0x1240	// RMON tx w > 2048 bytes
+#define FEC_RMON_T_OCTETS       0x1244	// RMON tx octets
+#define FEC_IEEE_T_DROP         0x1248	// count of frames not counted correctly
+#define FEC_IEEE_T_FRAME_OK     0x124C	// frames transmitted OK
+#define FEC_IEEE_T_1COL         0x1250	// frames transmitted with single collision
+#define FEC_IEEE_T_MCOL         0x1254	// frames transmitted with multiple collisions
+#define FEC_IEEE_T_DEF          0x1258	// frames transmitted after deferral delay
+#define FEC_IEEE_T_LCOL         0x125C	// frames transmitted with late collisions
+#define FEC_IEEE_T_EXCOL        0x1260	// frames transmitted with excessive collisions
+#define FEC_IEEE_T_MACERR       0x1264	// frames transmitted with tx-FIFO underrun
+#define FEC_IEEE_T_CSERR        0x1268	// frames transmitted with carrier sense error
+#define FEC_IEEE_T_SQE          0x126C	// frames transmitted with SQE error
+#define FEC_IEEE_T_FDXFC        0x1270	// flow control pause frames transmitted
+#define FEC_IEEE_T_OCTETS_OK    0x1274	// octet count for frames transmitted w/o error
+#define FEC_RMON_R_PACKETS      0x1284	// RMON rx packet count
+#define FEC_RMON_R_BC_PKT       0x1288	// RMON rx broadcast packets
+#define FEC_RMON_R_MC_PKT       0x128C	// RMON rx multicast packets
+#define FEC_RMON_R_CRC_ALIGN    0x1290	// RMON rx packets w CRC/align error
+#define FEC_RMON_R_UNDERSIZE    0x1294	// RMON rx packets < 64 bytes, good CRC
+#define FEC_RMON_R_OVERSIZE     0x1298	// RMON rx packets > MAX_FL bytes, good CRC
+#define FEC_RMON_R_FRAG         0x129C	// RMON rx packets < 64 bytes, bad CRC
+#define FEC_RMON_R_JAB          0x12A0	// RMON rx packets > MAX_FL bytes, bad CRC
+#define FEC_RMON_R_RESVD_0      0x12A4	//
+#define FEC_RMON_R_P64          0x12A8	// RMON rx           64 byte packets
+#define FEC_RMON_R_P65T0127     0x12AC	// RMON rx   65 to  127 byte packets
+#define FEC_RMON_R_P128TO255    0x12B0	// RMON rx  128 to  255 byte packets
+#define FEC_RMON_R_P256TO511    0x12B4	// RMON rx  256 to  511 byte packets
+#define FEC_RMON_R_P512TO1023   0x12B8	// RMON rx  512 to 1023 byte packets
+#define FEC_RMON_R_P1024TO2047  0x12BC	// RMON rx 1024 to 2047 byte packets
+#define FEC_RMON_R_GTE2048      0x12C0	// RMON rx w > 2048 bytes
+#define FEC_RMON_R_OCTETS       0x12C4	// RMON rx octets
+#define FEC_IEEE_R_DROP         0x12C8	// count of frames not counted correctly
+#define FEC_IEEE_R_FRAME_OK     0x12CC	// frames received OK
+#define FEC_IEEE_R_CRC          0x12D0	// frames received with CRC error
+#define FEC_IEEE_R_ALIGN        0x12D4	// frames received with alignment error
+#define FEC_IEEE_R_MACERR       0x12D8	// receive FIFO overflow count
+#define FEC_IEEE_R_FDXFC        0x12DC	// flow control pause frames received
+#define FEC_IEEE_R_OCTETS_OK    0x12E0	// octet count for frames rcvd w/o error
+
+// register bit definitions and macros
+#define FEC_EIR_UN              (0x00080000)
+#define FEC_EIR_RL              (0x00100000)
+#define FEC_EIR_LC              (0x00200000)
+#define FEC_EIR_EBERR           (0x00400000)
+#define FEC_EIR_MII             (0x00800000)
+#define FEC_EIR_RXB             (0x01000000)
+#define FEC_EIR_RXF             (0x02000000)
+#define FEC_EIR_TXB             (0x04000000)
+#define FEC_EIR_TXF             (0x08000000)
+#define FEC_EIR_GRA             (0x10000000)
+#define FEC_EIR_BABT            (0x20000000)
+#define FEC_EIR_BABR            (0x40000000)
+#define FEC_EIR_HBERR           (0x80000000)
+
+#define FEC_EIMR_UN             (0x00080000)
+#define FEC_EIMR_RL             (0x00100000)
+#define FEC_EIMR_LC             (0x00200000)
+#define FEC_EIMR_EBERR          (0x00400000)
+#define FEC_EIMR_MII            (0x00800000)
+#define FEC_EIMR_RXB            (0x01000000)
+#define FEC_EIMR_RXF            (0x02000000)
+#define FEC_EIMR_TXB            (0x04000000)
+#define FEC_EIMR_TXF            (0x08000000)
+#define FEC_EIMR_GRA            (0x10000000)
+#define FEC_EIMR_BABT           (0x20000000)
+#define FEC_EIMR_BABR           (0x40000000)
+#define FEC_EIMR_HBERR          (0x80000000)
+
+#define FEC_RDAR_R_DES_ACTIVE   (0x01000000)
+
+#define FEC_TDAR_X_DES_ACTIVE   (0x01000000)
+
+#define FEC_ECR_RESET           (0x00000001)
+#define FEC_ECR_ETHER_EN        (0x00000002)
+
+#define FEC_MMFR_DATA(x)        (((x) & 0xFFFF))
+#define FEC_MMFR_TA             (0x00020000)
+#define FEC_MMFR_RA(x)          (((x) & 0x1F) << 18)
+#define FEC_MMFR_PA(x)          (((x) & 0x1F) << 23)
+#define FEC_MMFR_OP_WR          (0x10000000)
+#define FEC_MMFR_OP_RD          (0x20000000)
+#define FEC_MMFR_ST             (0x40000000)
+
+#define FEC_MSCR_MII_SPEED(x)   (((x) & 0x1F) << 1)
+#define FEC_MSCR_DIS_PREAMBLE   (0x00000008)
+
+#define FEC_MIBC_MIB_IDLE       (0x40000000)
+#define FEC_MIBC_MIB_DISABLE    (0x80000000)
+
+#define FEC_RCR_LOOP            (0x00000001)
+#define FEC_RCR_DRT             (0x00000002)
+#define FEC_RCR_MII_MODE        (0x00000004)
+#define FEC_RCR_PROM            (0x00000008)
+#define FEC_RCR_BC_REJ          (0x00000010)
+#define FEC_RCR_FCE             (0x00000020)
+#define FEC_RCR_MAX_FL(x)       (((x) & 0x07FF) << 16)
+
+#define FEC_TCR_GTS             (0x00000001)
+#define FEC_TCR_HBC             (0x00000002)
+#define FEC_TCR_FDEN            (0x00000004)
+#define FEC_TCR_TFC_PAUSE       (0x00000008)
+#define FEC_TCR_RFC_PAUSE       (0x00000010)
+
+#define FEC_PALR_BYTE3(x)       (((x) & 0xFF) <<  0)
+#define FEC_PALR_BYTE2(x)       (((x) & 0xFF) <<  8)
+#define FEC_PALR_BYTE1(x)       (((x) & 0xFF) << 16)
+#define FEC_PALR_BYTE0(x)       (((x) & 0xFF) << 24)
+
+//#define FEC_PAUR_TYPE(x)        (((x) & 0xFFFF) <<  0)
+#define FEC_PAUR_BYTE5(x)       (((x) &   0xFF) << 16)
+#define FEC_PAUR_BYTE4(x)       (((x) &   0xFF) << 24)
+
+#define FEC_OPD_PAUSE_DUR(x)    (((x) & 0xFFFF))
+//#define FEC_OPD_OPCODE(x)       (((x) & 0xFFFF) << 16)
+
+//m.b.
+#define FEC_IAUR_BYTE7(x)       (((x) & 0xFF) <<  0)
+#define FEC_IAUR_BYTE6(x)       (((x) & 0xFF) <<  8)
+#define FEC_IAUR_BYTE5(x)       (((x) & 0xFF) << 16)
+#define FEC_IAUR_BYTE4(x)       (((x) & 0xFF) << 24)
+
+#define FEC_IALR_BYTE3(x)       (((x) & 0xFF) <<  0)
+#define FEC_IALR_BYTE2(x)       (((x) & 0xFF) <<  8)
+#define FEC_IALR_BYTE1(x)       (((x) & 0xFF) << 16)
+#define FEC_IALR_BYTE0(x)       (((x) & 0xFF) << 24)
+
+#define FEC_GAUR_BYTE7(x)       (((x) & 0xFF) <<  0)
+#define FEC_GAUR_BYTE6(x)       (((x) & 0xFF) <<  8)
+#define FEC_GAUR_BYTE5(x)       (((x) & 0xFF) << 16)
+#define FEC_GAUR_BYTE4(x)       (((x) & 0xFF) << 24)
+
+#define FEC_GALR_BYTE3(x)       (((x) & 0xFF) <<  0)
+#define FEC_GALR_BYTE2(x)       (((x) & 0xFF) <<  8)
+#define FEC_GALR_BYTE1(x)       (((x) & 0xFF) << 16)
+#define FEC_GALR_BYTE0(x)       (((x) & 0xFF) << 24)
+// ^^^^
+
+#define FEC_TFWR_X_WMRK_64      (0x00000001)
+#define FEC_TFWR_X_WMRK_128     (0x00000002)
+#define FEC_TFWR_X_WMRK_192     (0x00000003)
+
+//m.b.
+#define FEC_FRBR_R_BOUND(x)     (((x) & 0xFF) << 2)
+
+//m.b.
+#define FEC_FRSR_R_FSTART(x)    (((x) & 0xFF) << 2)
+
+//m.b.
+#define FEC_ERDSR_R_DES_START(x)  (((x) & 0x3FFFFFFF) << 2)
+
+//m.b.
+#define FEC_ETSDR_X_DES_START(x)  (((x) & 0x3FFFFFFF) << 2)
+
+#define FEC_EMRBR_R_BUF_SIZE(x) (((x) & 0x7F) <<  4)
+
+#define FEC_RxBD_TR             0x0001
+#define FEC_RxBD_OV             0x0002
+#define FEC_RxBD_CR             0x0004
+#define FEC_RxBD_NO             0x0010
+#define FEC_RxBD_LG             0x0020
+#define FEC_RxBD_MC             0x0040
+#define FEC_RxBD_BC             0x0080
+#define FEC_RxBD_M              0x0100
+#define FEC_RxBD_L              0x0800
+#define FEC_RxBD_R02            0x1000
+#define FEC_RxBD_W              0x2000
+#define FEC_RxBD_R01            0x4000
+#define FEC_RxBD_INUSE          0x4000
+#define FEC_RxBD_E              0x8000
+
+//m.b.
+//#define FEC_TxBD_CSL            0x0001
+//#define FEC_TxBD_UN             0x0002
+//#define FEC_TxBD_RL             0x0040
+//#define FEC_TxBD_LC             0x0080
+//#define FEC_TxBD_HB             0x0100
+//#define FEC_TxBD_DEF            0x0200
+#define FEC_TxBD_ABC            0x0200
+// ^^^^
+#define FEC_TxBD_TC             0x0400
+#define FEC_TxBD_L              0x0800
+#define FEC_TxBD_TO2            0x1000
+#define FEC_TxBD_W              0x2000
+#define FEC_TxBD_TO1            0x4000
+#define FEC_TxBD_INUSE          0x4000
+#define FEC_TxBD_R              0x8000
+
+//---------------------------------------------------------------------------
+// types
+//---------------------------------------------------------------------------
+
+// Rx and Tx buffer descriptor format
+typedef struct {
+	WORD m_wStatus;		// control / status  ---  used by edrv, do not change in application
+	WORD m_wLength;		// transfer length
+	BYTE *m_pbData;		// buffer address
+} tBufferDescr;
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#if (NO_OF_INSTANCES > 1)
+#define ECI_WRITE_DW_REG(off,val)       (*(DWORD *)(void *)(&__IPSBAR[off]) = val)
+#define ECI_READ_DW_REG(off)            (*(DWORD *)(void *)(&__IPSBAR[off]))
+#else
+#if (EDRV_USED_ETH_CTRL == 0)
+#define ECI_WRITE_DW_REG(off,val)       (*(DWORD *)(void *)(&__IPSBAR[FEC0_ADDR+off]) = val)
+#define ECI_READ_DW_REG(off)            (*(DWORD *)(void *)(&__IPSBAR[FEC0_ADDR+off]))
+#else
+#define ECI_WRITE_DW_REG(off,val)       (*(DWORD *)(void *)(&__IPSBAR[FEC1_ADDR+off]) = val)
+#define ECI_READ_DW_REG(off)            (*(DWORD *)(void *)(&__IPSBAR[FEC1_ADDR+off]))
+#endif
+#endif
+
+#endif // #ifndef _EDRV_FEC_H_
diff --git a/drivers/staging/epl/EdrvSim.h b/drivers/staging/epl/EdrvSim.h
new file mode 100644
index 00000000000..39300e321d6
--- /dev/null
+++ b/drivers/staging/epl/EdrvSim.h
@@ -0,0 +1,89 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  interface for ethernet driver simulation
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EdrvSim.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.3 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                Dev C++ and GNU-Compiler for m68k
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/15 d.k.:   start of implementation
+
+****************************************************************************/
+
+#ifndef _EDRVSIM_H_
+#define _EDRVSIM_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+void EdrvRxInterruptHandler(BYTE bBufferInFrame_p, BYTE * pbEthernetData_p,
+			    WORD wDataLen_p);
+
+#endif // #ifndef _EDRVSIM_H_
diff --git a/drivers/staging/epl/Epl.h b/drivers/staging/epl/Epl.h
new file mode 100644
index 00000000000..be60f771b63
--- /dev/null
+++ b/drivers/staging/epl/Epl.h
@@ -0,0 +1,273 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for EPL API layer
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: Epl.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.8 $  $Date: 2008/11/17 16:40:39 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/05/22 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_API_H_
+#define _EPL_API_H_
+
+#include "EplInc.h"
+#include "EplSdo.h"
+#include "EplObd.h"
+#include "EplLed.h"
+#include "EplEvent.h"
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+typedef struct {
+	unsigned int m_uiNodeId;
+	tEplNmtState m_NmtState;
+	tEplNmtNodeEvent m_NodeEvent;
+	WORD m_wErrorCode;	// EPL error code if m_NodeEvent == kEplNmtNodeEventError
+	BOOL m_fMandatory;
+
+} tEplApiEventNode;
+
+typedef struct {
+	tEplNmtState m_NmtState;	// local NMT state
+	tEplNmtBootEvent m_BootEvent;
+	WORD m_wErrorCode;	// EPL error code if m_BootEvent == kEplNmtBootEventError
+
+} tEplApiEventBoot;
+
+typedef struct {
+	tEplLedType m_LedType;	// type of the LED (e.g. Status or Error)
+	BOOL m_fOn;		// state of the LED (e.g. on or off)
+
+} tEplApiEventLed;
+
+typedef enum {
+	kEplApiEventNmtStateChange = 0x10,	// m_NmtStateChange
+//    kEplApiEventRequestNmt     = 0x11,    // m_bNmtCmd
+	kEplApiEventCriticalError = 0x12,	// m_InternalError, Stack halted
+	kEplApiEventWarning = 0x13,	// m_InternalError, Stack running
+	kEplApiEventNode = 0x20,	// m_Node
+	kEplApiEventBoot = 0x21,	// m_Boot
+	kEplApiEventSdo = 0x62,	// m_Sdo
+	kEplApiEventObdAccess = 0x69,	// m_ObdCbParam
+	kEplApiEventLed = 0x70,	// m_Led
+
+} tEplApiEventType;
+
+typedef union {
+	tEplEventNmtStateChange m_NmtStateChange;
+	tEplEventError m_InternalError;
+	tEplSdoComFinished m_Sdo;
+	tEplObdCbParam m_ObdCbParam;
+	tEplApiEventNode m_Node;
+	tEplApiEventBoot m_Boot;
+	tEplApiEventLed m_Led;
+
+} tEplApiEventArg;
+
+typedef tEplKernel(PUBLIC ROM * tEplApiCbEvent) (tEplApiEventType EventType_p,	// IN: event type (enum)
+						 tEplApiEventArg * pEventArg_p,	// IN: event argument (union)
+						 void GENERIC * pUserArg_p);
+
+typedef struct {
+	unsigned int m_uiSizeOfStruct;
+	BOOL m_fAsyncOnly;	// do not need to register PRes
+	unsigned int m_uiNodeId;	// local node ID
+	BYTE m_abMacAddress[6];	// local MAC address
+
+	// 0x1F82: NMT_FeatureFlags_U32
+	DWORD m_dwFeatureFlags;
+	// Cycle Length (0x1006: NMT_CycleLen_U32) in [us]
+	DWORD m_dwCycleLen;	// required for error detection
+	// 0x1F98: NMT_CycleTiming_REC
+	// 0x1F98.1: IsochrTxMaxPayload_U16
+	unsigned int m_uiIsochrTxMaxPayload;	// const
+	// 0x1F98.2: IsochrRxMaxPayload_U16
+	unsigned int m_uiIsochrRxMaxPayload;	// const
+	// 0x1F98.3: PResMaxLatency_U32
+	DWORD m_dwPresMaxLatency;	// const in [ns], only required for IdentRes
+	// 0x1F98.4: PReqActPayloadLimit_U16
+	unsigned int m_uiPreqActPayloadLimit;	// required for initialisation (+28 bytes)
+	// 0x1F98.5: PResActPayloadLimit_U16
+	unsigned int m_uiPresActPayloadLimit;	// required for initialisation of Pres frame (+28 bytes)
+	// 0x1F98.6: ASndMaxLatency_U32
+	DWORD m_dwAsndMaxLatency;	// const in [ns], only required for IdentRes
+	// 0x1F98.7: MultiplCycleCnt_U8
+	unsigned int m_uiMultiplCycleCnt;	// required for error detection
+	// 0x1F98.8: AsyncMTU_U16
+	unsigned int m_uiAsyncMtu;	// required to set up max frame size
+	// 0x1F98.9: Prescaler_U16
+	unsigned int m_uiPrescaler;	// required for sync
+	// $$$ Multiplexed Slot
+
+	// 0x1C14: DLL_LossOfFrameTolerance_U32 in [ns]
+	DWORD m_dwLossOfFrameTolerance;
+
+	// 0x1F8A: NMT_MNCycleTiming_REC
+	// 0x1F8A.1: WaitSoCPReq_U32 in [ns]
+	DWORD m_dwWaitSocPreq;
+
+	// 0x1F8A.2: AsyncSlotTimeout_U32 in [ns]
+	DWORD m_dwAsyncSlotTimeout;
+
+	DWORD m_dwDeviceType;	// NMT_DeviceType_U32
+	DWORD m_dwVendorId;	// NMT_IdentityObject_REC.VendorId_U32
+	DWORD m_dwProductCode;	// NMT_IdentityObject_REC.ProductCode_U32
+	DWORD m_dwRevisionNumber;	// NMT_IdentityObject_REC.RevisionNo_U32
+	DWORD m_dwSerialNumber;	// NMT_IdentityObject_REC.SerialNo_U32
+	QWORD m_qwVendorSpecificExt1;
+	DWORD m_dwVerifyConfigurationDate;	// CFM_VerifyConfiguration_REC.ConfDate_U32
+	DWORD m_dwVerifyConfigurationTime;	// CFM_VerifyConfiguration_REC.ConfTime_U32
+	DWORD m_dwApplicationSwDate;	// PDL_LocVerApplSw_REC.ApplSwDate_U32 on programmable device or date portion of NMT_ManufactSwVers_VS on non-programmable device
+	DWORD m_dwApplicationSwTime;	// PDL_LocVerApplSw_REC.ApplSwTime_U32 on programmable device or time portion of NMT_ManufactSwVers_VS on non-programmable device
+	DWORD m_dwIpAddress;
+	DWORD m_dwSubnetMask;
+	DWORD m_dwDefaultGateway;
+	BYTE m_sHostname[32];
+	BYTE m_abVendorSpecificExt2[48];
+
+	char *m_pszDevName;	// NMT_ManufactDevName_VS (0x1008/0 local OD)
+	char *m_pszHwVersion;	// NMT_ManufactHwVers_VS  (0x1009/0 local OD)
+	char *m_pszSwVersion;	// NMT_ManufactSwVers_VS  (0x100A/0 local OD)
+
+	tEplApiCbEvent m_pfnCbEvent;
+	void *m_pEventUserArg;
+	tEplSyncCb m_pfnCbSync;
+
+} tEplApiInitParam;
+
+typedef struct {
+	void *m_pImage;
+	unsigned int m_uiSize;
+
+} tEplApiProcessImage;
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplApiInitialize(tEplApiInitParam * pInitParam_p);
+
+tEplKernel PUBLIC EplApiShutdown(void);
+
+tEplKernel PUBLIC EplApiReadObject(tEplSdoComConHdl * pSdoComConHdl_p,
+				   unsigned int uiNodeId_p,
+				   unsigned int uiIndex_p,
+				   unsigned int uiSubindex_p,
+				   void *pDstData_le_p,
+				   unsigned int *puiSize_p,
+				   tEplSdoType SdoType_p, void *pUserArg_p);
+
+tEplKernel PUBLIC EplApiWriteObject(tEplSdoComConHdl * pSdoComConHdl_p,
+				    unsigned int uiNodeId_p,
+				    unsigned int uiIndex_p,
+				    unsigned int uiSubindex_p,
+				    void *pSrcData_le_p,
+				    unsigned int uiSize_p,
+				    tEplSdoType SdoType_p, void *pUserArg_p);
+
+tEplKernel PUBLIC EplApiFreeSdoChannel(tEplSdoComConHdl SdoComConHdl_p);
+
+tEplKernel PUBLIC EplApiReadLocalObject(unsigned int uiIndex_p,
+					unsigned int uiSubindex_p,
+					void *pDstData_p,
+					unsigned int *puiSize_p);
+
+tEplKernel PUBLIC EplApiWriteLocalObject(unsigned int uiIndex_p,
+					 unsigned int uiSubindex_p,
+					 void *pSrcData_p,
+					 unsigned int uiSize_p);
+
+tEplKernel PUBLIC EplApiCbObdAccess(tEplObdCbParam MEM * pParam_p);
+
+tEplKernel PUBLIC EplApiLinkObject(unsigned int uiObjIndex_p,
+				   void *pVar_p,
+				   unsigned int *puiVarEntries_p,
+				   tEplObdSize * pEntrySize_p,
+				   unsigned int uiFirstSubindex_p);
+
+tEplKernel PUBLIC EplApiExecNmtCommand(tEplNmtEvent NmtEvent_p);
+
+tEplKernel PUBLIC EplApiProcess(void);
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+tEplKernel PUBLIC EplApiMnTriggerStateChange(unsigned int uiNodeId_p,
+					     tEplNmtNodeCommand NodeCommand_p);
+#endif
+
+tEplKernel PUBLIC EplApiGetIdentResponse(unsigned int uiNodeId_p,
+					 tEplIdentResponse **
+					 ppIdentResponse_p);
+
+// functions for process image will be implemented in separate file
+tEplKernel PUBLIC EplApiProcessImageSetup(void);
+tEplKernel PUBLIC EplApiProcessImageExchangeIn(tEplApiProcessImage * pPI_p);
+tEplKernel PUBLIC EplApiProcessImageExchangeOut(tEplApiProcessImage * pPI_p);
+
+#endif // #ifndef _EPL_API_H_
diff --git a/drivers/staging/epl/EplAmi.h b/drivers/staging/epl/EplAmi.h
new file mode 100644
index 00000000000..6fa04a4f0a1
--- /dev/null
+++ b/drivers/staging/epl/EplAmi.h
@@ -0,0 +1,362 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  Definitions for Abstract Memory Interface
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplAmi.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.2 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                GCC
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+   06.03.2000  -rs
+               Implementation
+
+   16.09.2002  -as
+               To save code space the functions AmiSetByte and AmiGetByte
+               are replaced by macros. For targets which assign BYTE by
+               an 16Bit type, the definition of macros must changed to
+               functions.
+
+   23.02.2005  r.d.:
+               Functions included for extended data types such as UNSIGNED24,
+               UNSIGNED40, ...
+
+   13.06.2006  d.k.:
+               Extended the interface for EPL with the different functions
+               for little endian and big endian
+
+****************************************************************************/
+
+#ifndef _EPLAMI_H_
+#define _EPLAMI_H_
+
+#if ((DEV_SYSTEM & _DEV_64BIT_SUPPORT_) == 0)
+//    #ifdef USE_VAR64
+#error 'ERROR: development system does not support 64 bit operations!'
+//    #endif
+#endif
+
+//---------------------------------------------------------------------------
+//  types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+//  Prototypen
+//---------------------------------------------------------------------------
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#if (TARGET_SYSTEM == _WIN32_)
+#if defined(INLINE_FUNCTION_DEF)
+#undef  INLINE_FUNCTION
+#define INLINE_FUNCTION     INLINE_FUNCTION_DEF
+#define INLINE_ENABLED      TRUE
+#define EPL_AMI_INLINED
+#include "../EplStack/amix86.c"
+#endif
+
+#elif (TARGET_SYSTEM == _LINUX_)
+#if defined(__m68k__)		// it is an big endian machine
+#if defined(INLINE_FUNCTION_DEF)
+#undef  INLINE_FUNCTION
+#define INLINE_FUNCTION     INLINE_FUNCTION_DEF
+#define INLINE_ENABLED      TRUE
+#define EPL_AMI_INLINED
+#include "../EplStack/amibe.c"
+#endif
+#endif
+#endif
+
+//---------------------------------------------------------------------------
+//
+// write functions
+//
+// To save code space the function AmiSetByte is replaced by
+// an macro.
+// void   PUBLIC  AmiSetByte  (void FAR* pAddr_p, BYTE bByteVal_p);
+
+#define AmiSetByteToBe(pAddr_p, bByteVal_p)  {*(BYTE FAR*)(pAddr_p) = (bByteVal_p);}
+#define AmiSetByteToLe(pAddr_p, bByteVal_p)  {*(BYTE FAR*)(pAddr_p) = (bByteVal_p);}
+
+#if !defined(INLINE_ENABLED)
+	void PUBLIC AmiSetWordToBe(void FAR * pAddr_p, WORD wWordVal_p);
+	void PUBLIC AmiSetDwordToBe(void FAR * pAddr_p, DWORD dwDwordVal_p);
+	void PUBLIC AmiSetWordToLe(void FAR * pAddr_p, WORD wWordVal_p);
+	void PUBLIC AmiSetDwordToLe(void FAR * pAddr_p, DWORD dwDwordVal_p);
+#endif
+
+//---------------------------------------------------------------------------
+//
+// read functions
+//
+// To save code space the function AmiGetByte is replaced by
+// an macro.
+// BYTE   PUBLIC  AmiGetByte  (void FAR* pAddr_p);
+
+#define AmiGetByteFromBe(pAddr_p)  (*(BYTE FAR*)(pAddr_p))
+#define AmiGetByteFromLe(pAddr_p)  (*(BYTE FAR*)(pAddr_p))
+
+#if !defined(INLINE_ENABLED)
+
+	WORD PUBLIC AmiGetWordFromBe(void FAR * pAddr_p);
+	DWORD PUBLIC AmiGetDwordFromBe(void FAR * pAddr_p);
+	WORD PUBLIC AmiGetWordFromLe(void FAR * pAddr_p);
+	DWORD PUBLIC AmiGetDwordFromLe(void FAR * pAddr_p);
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetDword24()
+//
+// Description: sets a 24 bit value to a buffer
+//
+// Parameters:  pAddr_p         = pointer to destination buffer
+//              dwDwordVal_p    = value to set
+//
+// Return:      void
+//
+//---------------------------------------------------------------------------
+
+	void PUBLIC AmiSetDword24ToBe(void FAR * pAddr_p, DWORD dwDwordVal_p);
+	void PUBLIC AmiSetDword24ToLe(void FAR * pAddr_p, DWORD dwDwordVal_p);
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetDword24()
+//
+// Description: reads a 24 bit value from a buffer
+//
+// Parameters:  pAddr_p         = pointer to source buffer
+//
+// Return:      DWORD           = read value
+//
+//---------------------------------------------------------------------------
+
+	DWORD PUBLIC AmiGetDword24FromBe(void FAR * pAddr_p);
+	DWORD PUBLIC AmiGetDword24FromLe(void FAR * pAddr_p);
+
+//#ifdef USE_VAR64
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetQword40()
+//
+// Description: sets a 40 bit value to a buffer
+//
+// Parameters:  pAddr_p         = pointer to destination buffer
+//              qwQwordVal_p    = quadruple word value
+//
+// Return:      void
+//
+//---------------------------------------------------------------------------
+
+	void PUBLIC AmiSetQword40ToBe(void FAR * pAddr_p, QWORD qwQwordVal_p);
+	void PUBLIC AmiSetQword40ToLe(void FAR * pAddr_p, QWORD qwQwordVal_p);
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetQword40()
+//
+// Description: reads a 40 bit value from a buffer
+//
+// Parameters:  pAddr_p         = pointer to source buffer
+//
+// Return:      QWORD
+//
+//---------------------------------------------------------------------------
+
+	QWORD PUBLIC AmiGetQword40FromBe(void FAR * pAddr_p);
+	QWORD PUBLIC AmiGetQword40FromLe(void FAR * pAddr_p);
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetQword48()
+//
+// Description: sets a 48 bit value to a buffer
+//
+// Parameters:  pAddr_p         = pointer to destination buffer
+//              qwQwordVal_p    = quadruple word value
+//
+// Return:      void
+//
+//---------------------------------------------------------------------------
+
+	void PUBLIC AmiSetQword48ToBe(void FAR * pAddr_p, QWORD qwQwordVal_p);
+	void PUBLIC AmiSetQword48ToLe(void FAR * pAddr_p, QWORD qwQwordVal_p);
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetQword48()
+//
+// Description: reads a 48 bit value from a buffer
+//
+// Parameters:  pAddr_p         = pointer to source buffer
+//
+// Return:      QWORD
+//
+//---------------------------------------------------------------------------
+
+	QWORD PUBLIC AmiGetQword48FromBe(void FAR * pAddr_p);
+	QWORD PUBLIC AmiGetQword48FromLe(void FAR * pAddr_p);
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetQword56()
+//
+// Description: sets a 56 bit value to a buffer
+//
+// Parameters:  pAddr_p         = pointer to destination buffer
+//              qwQwordVal_p    = quadruple word value
+//
+// Return:      void
+//
+//---------------------------------------------------------------------------
+
+	void PUBLIC AmiSetQword56ToBe(void FAR * pAddr_p, QWORD qwQwordVal_p);
+	void PUBLIC AmiSetQword56ToLe(void FAR * pAddr_p, QWORD qwQwordVal_p);
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetQword56()
+//
+// Description: reads a 56 bit value from a buffer
+//
+// Parameters:  pAddr_p         = pointer to source buffer
+//
+// Return:      QWORD
+//
+//---------------------------------------------------------------------------
+
+	QWORD PUBLIC AmiGetQword56FromBe(void FAR * pAddr_p);
+	QWORD PUBLIC AmiGetQword56FromLe(void FAR * pAddr_p);
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetQword64()
+//
+// Description: sets a 64 bit value to a buffer
+//
+// Parameters:  pAddr_p         = pointer to destination buffer
+//              qwQwordVal_p    = quadruple word value
+//
+// Return:      void
+//
+//---------------------------------------------------------------------------
+
+	void PUBLIC AmiSetQword64ToBe(void FAR * pAddr_p, QWORD qwQwordVal_p);
+	void PUBLIC AmiSetQword64ToLe(void FAR * pAddr_p, QWORD qwQwordVal_p);
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetQword64()
+//
+// Description: reads a 64 bit value from a buffer
+//
+// Parameters:  pAddr_p         = pointer to source buffer
+//
+// Return:      void
+//
+//---------------------------------------------------------------------------
+
+	QWORD PUBLIC AmiGetQword64FromBe(void FAR * pAddr_p);
+	QWORD PUBLIC AmiGetQword64FromLe(void FAR * pAddr_p);
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetTimeOfDay()
+//
+// Description: sets a TIME_OF_DAY (CANopen) value to a buffer
+//
+// Parameters:  pAddr_p         = pointer to destination buffer
+//              pTimeOfDay_p    = pointer to struct TIME_OF_DAY
+//
+// Return:      void
+//
+//---------------------------------------------------------------------------
+
+	void PUBLIC AmiSetTimeOfDay(void FAR * pAddr_p,
+				    tTimeOfDay FAR * pTimeOfDay_p);
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetTimeOfDay()
+//
+// Description: reads a TIME_OF_DAY (CANopen) value from a buffer
+//
+// Parameters:  pAddr_p         = pointer to source buffer
+//              pTimeOfDay_p    = pointer to struct TIME_OF_DAY
+//
+// Return:      void
+//
+//---------------------------------------------------------------------------
+
+	void PUBLIC AmiGetTimeOfDay(void FAR * pAddr_p,
+				    tTimeOfDay FAR * pTimeOfDay_p);
+
+#endif
+
+#undef  INLINE_ENABLED		// disable actual inlining of functions
+#define EPL_AMI_INCLUDED
+
+#ifdef __cplusplus
+}
+#endif
+#endif				// ifndef _EPLAMI_H_
+// Die letzte Zeile muß unbedingt eine leere Zeile sein, weil manche Compiler// damit ein Problem haben, wenn das nicht so ist (z.B. GNU oder Borland C++ Builder).
diff --git a/drivers/staging/epl/EplApiGeneric.c b/drivers/staging/epl/EplApiGeneric.c
new file mode 100644
index 00000000000..ae19e34cd7b
--- /dev/null
+++ b/drivers/staging/epl/EplApiGeneric.c
@@ -0,0 +1,2060 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for generic EPL API module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplApiGeneric.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.21 $  $Date: 2008/11/21 09:00:38 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/09/05 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#include "Epl.h"
+#include "kernel/EplDllk.h"
+#include "kernel/EplErrorHandlerk.h"
+#include "kernel/EplEventk.h"
+#include "kernel/EplNmtk.h"
+#include "kernel/EplObdk.h"
+#include "kernel/EplTimerk.h"
+#include "kernel/EplDllkCal.h"
+#include "kernel/EplPdokCal.h"
+#include "user/EplDlluCal.h"
+#include "user/EplLedu.h"
+#include "user/EplNmtCnu.h"
+#include "user/EplNmtMnu.h"
+#include "user/EplSdoComu.h"
+#include "user/EplIdentu.h"
+#include "user/EplStatusu.h"
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0)
+#include "kernel/EplPdok.h"
+#endif
+
+#include "SharedBuff.h"
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) == 0)
+#error "EPL API layer needs EPL module OBDK!"
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  EplApi                                              */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description:
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   D E F I N I T I O N S                          //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+typedef struct {
+	tEplApiInitParam m_InitParam;
+
+} tEplApiInstance;
+
+//---------------------------------------------------------------------------
+// local vars
+//---------------------------------------------------------------------------
+
+static tEplApiInstance EplApiInstance_g;
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+// NMT state change event callback function
+static tEplKernel PUBLIC EplApiCbNmtStateChange(tEplEventNmtStateChange
+						NmtStateChange_p);
+
+// update DLL configuration from OD
+static tEplKernel PUBLIC EplApiUpdateDllConfig(BOOL fUpdateIdentity_p);
+
+// update OD from init param
+static tEplKernel PUBLIC EplApiUpdateObd(void);
+
+// process events from user event queue
+static tEplKernel PUBLIC EplApiProcessEvent(tEplEvent * pEplEvent_p);
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+// callback function of SDO module
+static tEplKernel PUBLIC EplApiCbSdoCon(tEplSdoComFinished * pSdoComFinished_p);
+#endif
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+// callback functions of NmtMnu module
+static tEplKernel PUBLIC EplApiCbNodeEvent(unsigned int uiNodeId_p,
+					   tEplNmtNodeEvent NodeEvent_p,
+					   tEplNmtState NmtState_p,
+					   WORD wErrorCode_p,
+					   BOOL fMandatory_p);
+
+static tEplKernel PUBLIC EplApiCbBootEvent(tEplNmtBootEvent BootEvent_p,
+					   tEplNmtState NmtState_p,
+					   WORD wErrorCode_p);
+#endif
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_LEDU)) != 0)
+// callback function of Ledu module
+static tEplKernel PUBLIC EplApiCbLedStateChange(tEplLedType LedType_p,
+						BOOL fOn_p);
+#endif
+
+// OD initialization function (implemented in Objdict.c)
+tEplKernel PUBLIC EplObdInitRam(tEplObdInitParam MEM * pInitParam_p);
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplApiInitialize()
+//
+// Description: add and initialize new instance of EPL stack.
+//              After return from this function the application must start
+//              the NMT state machine via
+//              EplApiExecNmtCommand(kEplNmtEventSwReset)
+//              and thereby the whole EPL stack :-)
+//
+// Parameters:  pInitParam_p            = initialisation parameters
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplApiInitialize(tEplApiInitParam * pInitParam_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplObdInitParam ObdInitParam;
+	tEplDllkInitParam DllkInitParam;
+#ifndef EPL_NO_FIFO
+	tShbError ShbError;
+#endif
+
+	// reset instance structure
+	EPL_MEMSET(&EplApiInstance_g, 0, sizeof(EplApiInstance_g));
+
+	EPL_MEMCPY(&EplApiInstance_g.m_InitParam, pInitParam_p,
+		   min(sizeof(tEplApiInitParam),
+		       pInitParam_p->m_uiSizeOfStruct));
+
+	// check event callback function pointer
+	if (EplApiInstance_g.m_InitParam.m_pfnCbEvent == NULL) {	// application must always have an event callback function
+		Ret = kEplApiInvalidParam;
+		goto Exit;
+	}
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+	// init OD
+// FIXME
+//    Ret = EplObdInitRam(&ObdInitParam);
+//    if (Ret != kEplSuccessful)
+//    {
+//        goto Exit;
+//    }
+
+	// initialize EplObd module
+	Ret = EplObdInit(&ObdInitParam);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#endif
+
+#ifndef EPL_NO_FIFO
+	ShbError = ShbInit();
+	if (ShbError != kShbOk) {
+		Ret = kEplNoResource;
+		goto Exit;
+	}
+#endif
+
+	// initialize EplEventk module
+	Ret = EplEventkInit(EplApiInstance_g.m_InitParam.m_pfnCbSync);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// initialize EplEventu module
+	Ret = EplEventuInit(EplApiProcessEvent);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// init EplTimerk module
+	Ret = EplTimerkInit();
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// initialize EplNmtk module before DLL
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTK)) != 0)
+	Ret = EplNmtkInit();
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#endif
+
+	// initialize EplDllk module
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+	EPL_MEMCPY(DllkInitParam.m_be_abSrcMac,
+		   EplApiInstance_g.m_InitParam.m_abMacAddress, 6);
+	Ret = EplDllkAddInstance(&DllkInitParam);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// initialize EplErrorHandlerk module
+	Ret = EplErrorHandlerkInit();
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// initialize EplDllkCal module
+	Ret = EplDllkCalAddInstance();
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#endif
+
+	// initialize EplDlluCal module
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLU)) != 0)
+	Ret = EplDlluCalAddInstance();
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#endif
+
+	// initialize EplPdok module
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0)
+	Ret = EplPdokAddInstance();
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	Ret = EplPdokCalAddInstance();
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#endif
+
+	// initialize EplNmtCnu module
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_CN)) != 0)
+	Ret = EplNmtCnuAddInstance(EplApiInstance_g.m_InitParam.m_uiNodeId);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#endif
+
+	// initialize EplNmtu module
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTU)) != 0)
+	Ret = EplNmtuInit();
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// register NMT event callback function
+	Ret = EplNmtuRegisterStateChangeCb(EplApiCbNmtStateChange);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#endif
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+	// initialize EplNmtMnu module
+	Ret = EplNmtMnuInit(EplApiCbNodeEvent, EplApiCbBootEvent);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// initialize EplIdentu module
+	Ret = EplIdentuInit();
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// initialize EplStatusu module
+	Ret = EplStatusuInit();
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#endif
+
+	// initialize EplLedu module
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_LEDU)) != 0)
+	Ret = EplLeduInit(EplApiCbLedStateChange);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#endif
+
+	// init SDO module
+#if ((((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOS)) != 0) || \
+     (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0))
+	// init sdo command layer
+	Ret = EplSdoComInit();
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#endif
+
+	// the application must start NMT state machine
+	// via EplApiExecNmtCommand(kEplNmtEventSwReset)
+	// and thereby the whole EPL stack
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplApiShutdown()
+//
+// Description: deletes an instance of EPL stack
+//
+// Parameters:  (none)
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplApiShutdown(void)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	// $$$ d.k.: check if NMT state is NMT_GS_OFF
+
+	// $$$ d.k.: maybe delete event queues at first, but this implies that
+	//           no other module must not use the event queues for communication
+	//           during shutdown.
+
+	// delete instance for all modules
+
+	// deinitialize EplSdoCom module
+#if ((((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOS)) != 0) || \
+     (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0))
+	Ret = EplSdoComDelInstance();
+//    PRINTF1("EplSdoComDelInstance():  0x%X\n", Ret);
+#endif
+
+	// deinitialize EplLedu module
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_LEDU)) != 0)
+	Ret = EplLeduDelInstance();
+//    PRINTF1("EplLeduDelInstance():    0x%X\n", Ret);
+#endif
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+	// deinitialize EplNmtMnu module
+	Ret = EplNmtMnuDelInstance();
+//    PRINTF1("EplNmtMnuDelInstance():  0x%X\n", Ret);
+
+	// deinitialize EplIdentu module
+	Ret = EplIdentuDelInstance();
+//    PRINTF1("EplIdentuDelInstance():  0x%X\n", Ret);
+
+	// deinitialize EplStatusu module
+	Ret = EplStatusuDelInstance();
+//    PRINTF1("EplStatusuDelInstance():  0x%X\n", Ret);
+#endif
+
+	// deinitialize EplNmtCnu module
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_CN)) != 0)
+	Ret = EplNmtCnuDelInstance();
+//    PRINTF1("EplNmtCnuDelInstance():  0x%X\n", Ret);
+#endif
+
+	// deinitialize EplNmtu module
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTU)) != 0)
+	Ret = EplNmtuDelInstance();
+//    PRINTF1("EplNmtuDelInstance():    0x%X\n", Ret);
+#endif
+
+	// deinitialize EplDlluCal module
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLU)) != 0)
+	Ret = EplDlluCalDelInstance();
+//    PRINTF1("EplDlluCalDelInstance(): 0x%X\n", Ret);
+
+#endif
+
+	// deinitialize EplEventu module
+	Ret = EplEventuDelInstance();
+//    PRINTF1("EplEventuDelInstance():  0x%X\n", Ret);
+
+	// deinitialize EplNmtk module
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTK)) != 0)
+	Ret = EplNmtkDelInstance();
+//    PRINTF1("EplNmtkDelInstance():    0x%X\n", Ret);
+#endif
+
+	// deinitialize EplDllk module
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+	Ret = EplDllkDelInstance();
+//    PRINTF1("EplDllkDelInstance():    0x%X\n", Ret);
+
+	// deinitialize EplDllkCal module
+	Ret = EplDllkCalDelInstance();
+//    PRINTF1("EplDllkCalDelInstance(): 0x%X\n", Ret);
+#endif
+
+	// deinitialize EplEventk module
+	Ret = EplEventkDelInstance();
+//    PRINTF1("EplEventkDelInstance():  0x%X\n", Ret);
+
+	// deinitialize EplTimerk module
+	Ret = EplTimerkDelInstance();
+//    PRINTF1("EplTimerkDelInstance():  0x%X\n", Ret);
+
+#ifndef EPL_NO_FIFO
+	ShbExit();
+#endif
+
+	return Ret;
+}
+
+//----------------------------------------------------------------------------
+// Function:    EplApiExecNmtCommand()
+//
+// Description: executes a NMT command, i.e. post the NMT command/event to the
+//              NMTk module. NMT commands which are not appropriate in the current
+//              NMT state are silently ignored. Please keep in mind that the
+//              NMT state may change until the NMT command is actually executed.
+//
+// Parameters:  NmtEvent_p              = NMT command/event
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//----------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplApiExecNmtCommand(tEplNmtEvent NmtEvent_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTU)) != 0)
+	Ret = EplNmtuNmtEvent(NmtEvent_p);
+#endif
+
+	return Ret;
+}
+
+//----------------------------------------------------------------------------
+// Function:    EplApiLinkObject()
+//
+// Description: Function maps array of application variables onto specified object in OD
+//
+// Parameters:  uiObjIndex_p            = Function maps variables for this object index
+//              pVar_p                  = Pointer to data memory area for the specified object
+//              puiVarEntries_p         = IN: pointer to number of entries to map
+//                                        OUT: pointer to number of actually used entries
+//              pEntrySize_p            = IN: pointer to size of one entry;
+//                                            if size is zero, the actual size will be read from OD
+//                                        OUT: pointer to entire size of all entries mapped
+//              uiFirstSubindex_p       = This is the first subindex to be mapped.
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//----------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplApiLinkObject(unsigned int uiObjIndex_p,
+				   void *pVar_p,
+				   unsigned int *puiVarEntries_p,
+				   tEplObdSize * pEntrySize_p,
+				   unsigned int uiFirstSubindex_p)
+{
+	BYTE bVarEntries;
+	BYTE bIndexEntries;
+	BYTE MEM *pbData;
+	unsigned int uiSubindex;
+	tEplVarParam VarParam;
+	tEplObdSize EntrySize;
+	tEplObdSize UsedSize;
+
+	tEplKernel RetCode = kEplSuccessful;
+
+	if ((pVar_p == NULL)
+	    || (puiVarEntries_p == NULL)
+	    || (*puiVarEntries_p == 0)
+	    || (pEntrySize_p == NULL)) {
+		RetCode = kEplApiInvalidParam;
+		goto Exit;
+	}
+
+	pbData = (BYTE MEM *) pVar_p;
+	bVarEntries = (BYTE) * puiVarEntries_p;
+	UsedSize = 0;
+
+	// init VarParam structure with default values
+	VarParam.m_uiIndex = uiObjIndex_p;
+	VarParam.m_ValidFlag = kVarValidAll;
+
+	if (uiFirstSubindex_p != 0) {	// check if object exists by reading subindex 0x00,
+		// because user wants to link a variable to a subindex unequal 0x00
+		// read number of entries
+		EntrySize = (tEplObdSize) sizeof(bIndexEntries);
+		RetCode = EplObdReadEntry(uiObjIndex_p,
+					  0x00,
+					  (void GENERIC *)&bIndexEntries,
+					  &EntrySize);
+
+		if ((RetCode != kEplSuccessful) || (bIndexEntries == 0x00)) {
+			// Object doesn't exist or invalid entry number
+			RetCode = kEplObdIndexNotExist;
+			goto Exit;
+		}
+	} else {		// user wants to link a variable to subindex 0x00
+		// that's OK
+		bIndexEntries = 0;
+	}
+
+	// Correct number of entries if number read from OD is greater
+	// than the specified number.
+	// This is done, so that we do not set more entries than subindexes the
+	// object actually has.
+	if ((bIndexEntries > (bVarEntries + uiFirstSubindex_p - 1)) &&
+	    (bVarEntries != 0x00)) {
+		bIndexEntries = (BYTE) (bVarEntries + uiFirstSubindex_p - 1);
+	}
+	// map entries
+	for (uiSubindex = uiFirstSubindex_p; uiSubindex <= bIndexEntries;
+	     uiSubindex++) {
+		// if passed entry size is 0, then get size from OD
+		if (*pEntrySize_p == 0x00) {
+			// read entry size
+			EntrySize = EplObdGetDataSize(uiObjIndex_p, uiSubindex);
+
+			if (EntrySize == 0x00) {
+				// invalid entry size (maybe object doesn't exist or entry of type DOMAIN is empty)
+				RetCode = kEplObdSubindexNotExist;
+				break;
+			}
+		} else {	// use passed entry size
+			EntrySize = *pEntrySize_p;
+		}
+
+		VarParam.m_uiSubindex = uiSubindex;
+
+		// set pointer to user var
+		VarParam.m_Size = EntrySize;
+		VarParam.m_pData = pbData;
+
+		UsedSize += EntrySize;
+		pbData += EntrySize;
+
+		RetCode = EplObdDefineVar(&VarParam);
+		if (RetCode != kEplSuccessful) {
+			break;
+		}
+	}
+
+	// set number of mapped entries and entry size
+	*puiVarEntries_p = ((bIndexEntries - uiFirstSubindex_p) + 1);
+	*pEntrySize_p = UsedSize;
+
+      Exit:
+
+	return (RetCode);
+
+}
+
+// ----------------------------------------------------------------------------
+//
+// Function:    EplApiReadObject()
+//
+// Description: reads the specified entry from the OD of the specified node.
+//              If this node is a remote node, it performs a SDO transfer, which
+//              means this function returns kEplApiTaskDeferred and the application
+//              is informed via the event callback function when the task is completed.
+//
+// Parameters:  pSdoComConHdl_p         = INOUT: pointer to SDO connection handle (may be NULL in case of local OD access)
+//              uiNodeId_p              = IN: node ID (0 = itself)
+//              uiIndex_p               = IN: index of object in OD
+//              uiSubindex_p            = IN: sub-index of object in OD
+//              pDstData_le_p           = OUT: pointer to data in little endian
+//              puiSize_p               = INOUT: pointer to size of data
+//              SdoType_p               = IN: type of SDO transfer
+//              pUserArg_p              = IN: user-definable argument pointer,
+//                                            which will be passed to the event callback function
+//
+// Return:      tEplKernel              = error code
+//
+// ----------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplApiReadObject(tEplSdoComConHdl * pSdoComConHdl_p,
+				   unsigned int uiNodeId_p,
+				   unsigned int uiIndex_p,
+				   unsigned int uiSubindex_p,
+				   void *pDstData_le_p,
+				   unsigned int *puiSize_p,
+				   tEplSdoType SdoType_p, void *pUserArg_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	if ((uiIndex_p == 0) || (pDstData_le_p == NULL) || (puiSize_p == NULL)
+	    || (*puiSize_p == 0)) {
+		Ret = kEplApiInvalidParam;
+		goto Exit;
+	}
+
+	if (uiNodeId_p == 0 || uiNodeId_p == EplObdGetNodeId()) {	// local OD access can be performed
+		tEplObdSize ObdSize;
+
+		ObdSize = (tEplObdSize) * puiSize_p;
+		Ret =
+		    EplObdReadEntryToLe(uiIndex_p, uiSubindex_p, pDstData_le_p,
+					&ObdSize);
+		*puiSize_p = (unsigned int)ObdSize;
+	} else {		// perform SDO transfer
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+		tEplSdoComTransParamByIndex TransParamByIndex;
+//    tEplSdoComConHdl            SdoComConHdl;
+
+		// check if application provides space for handle
+		if (pSdoComConHdl_p == NULL) {
+			Ret = kEplApiInvalidParam;
+			goto Exit;
+//            pSdoComConHdl_p = &SdoComConHdl;
+		}
+		// init command layer connection
+		Ret = EplSdoComDefineCon(pSdoComConHdl_p, uiNodeId_p,	// target node id
+					 SdoType_p);	// SDO type
+		if ((Ret != kEplSuccessful) && (Ret != kEplSdoComHandleExists)) {
+			goto Exit;
+		}
+		TransParamByIndex.m_pData = pDstData_le_p;
+		TransParamByIndex.m_SdoAccessType = kEplSdoAccessTypeRead;
+		TransParamByIndex.m_SdoComConHdl = *pSdoComConHdl_p;
+		TransParamByIndex.m_uiDataSize = *puiSize_p;
+		TransParamByIndex.m_uiIndex = uiIndex_p;
+		TransParamByIndex.m_uiSubindex = uiSubindex_p;
+		TransParamByIndex.m_pfnSdoFinishedCb = EplApiCbSdoCon;
+		TransParamByIndex.m_pUserArg = pUserArg_p;
+
+		Ret = EplSdoComInitTransferByIndex(&TransParamByIndex);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+		Ret = kEplApiTaskDeferred;
+
+#else
+		Ret = kEplApiInvalidParam;
+#endif
+	}
+
+      Exit:
+	return Ret;
+}
+
+// ----------------------------------------------------------------------------
+//
+// Function:    EplApiWriteObject()
+//
+// Description: writes the specified entry to the OD of the specified node.
+//              If this node is a remote node, it performs a SDO transfer, which
+//              means this function returns kEplApiTaskDeferred and the application
+//              is informed via the event callback function when the task is completed.
+//
+// Parameters:  pSdoComConHdl_p         = INOUT: pointer to SDO connection handle (may be NULL in case of local OD access)
+//              uiNodeId_p              = IN: node ID (0 = itself)
+//              uiIndex_p               = IN: index of object in OD
+//              uiSubindex_p            = IN: sub-index of object in OD
+//              pSrcData_le_p           = IN: pointer to data in little endian
+//              uiSize_p                = IN: size of data in bytes
+//              SdoType_p               = IN: type of SDO transfer
+//              pUserArg_p              = IN: user-definable argument pointer,
+//                                            which will be passed to the event callback function
+//
+// Return:      tEplKernel              = error code
+//
+// ----------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplApiWriteObject(tEplSdoComConHdl * pSdoComConHdl_p,
+				    unsigned int uiNodeId_p,
+				    unsigned int uiIndex_p,
+				    unsigned int uiSubindex_p,
+				    void *pSrcData_le_p,
+				    unsigned int uiSize_p,
+				    tEplSdoType SdoType_p, void *pUserArg_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	if ((uiIndex_p == 0) || (pSrcData_le_p == NULL) || (uiSize_p == 0)) {
+		Ret = kEplApiInvalidParam;
+		goto Exit;
+	}
+
+	if (uiNodeId_p == 0 || uiNodeId_p == EplObdGetNodeId()) {	// local OD access can be performed
+
+		Ret =
+		    EplObdWriteEntryFromLe(uiIndex_p, uiSubindex_p,
+					   pSrcData_le_p, uiSize_p);
+	} else {		// perform SDO transfer
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+		tEplSdoComTransParamByIndex TransParamByIndex;
+//    tEplSdoComConHdl            SdoComConHdl;
+
+		// check if application provides space for handle
+		if (pSdoComConHdl_p == NULL) {
+			Ret = kEplApiInvalidParam;
+			goto Exit;
+//            pSdoComConHdl_p = &SdoComConHdl;
+		}
+		// d.k.: How to recycle command layer connection?
+		//       Try to redefine it, which will return kEplSdoComHandleExists
+		//       and the existing command layer handle.
+		//       If the returned handle is busy, EplSdoComInitTransferByIndex()
+		//       will return with error.
+		// $$$ d.k.: Collisions may occur with Configuration Manager, if both the application and
+		//           Configuration Manager, are trying to communicate with the very same node.
+		//     possible solution: disallow communication by application if Configuration Manager is busy
+
+		// init command layer connection
+		Ret = EplSdoComDefineCon(pSdoComConHdl_p, uiNodeId_p,	// target node id
+					 SdoType_p);	// SDO type
+		if ((Ret != kEplSuccessful) && (Ret != kEplSdoComHandleExists)) {
+			goto Exit;
+		}
+		TransParamByIndex.m_pData = pSrcData_le_p;
+		TransParamByIndex.m_SdoAccessType = kEplSdoAccessTypeWrite;
+		TransParamByIndex.m_SdoComConHdl = *pSdoComConHdl_p;
+		TransParamByIndex.m_uiDataSize = uiSize_p;
+		TransParamByIndex.m_uiIndex = uiIndex_p;
+		TransParamByIndex.m_uiSubindex = uiSubindex_p;
+		TransParamByIndex.m_pfnSdoFinishedCb = EplApiCbSdoCon;
+		TransParamByIndex.m_pUserArg = pUserArg_p;
+
+		Ret = EplSdoComInitTransferByIndex(&TransParamByIndex);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+		Ret = kEplApiTaskDeferred;
+
+#else
+		Ret = kEplApiInvalidParam;
+#endif
+	}
+
+      Exit:
+	return Ret;
+}
+
+// ----------------------------------------------------------------------------
+//
+// Function:    EplApiFreeSdoChannel()
+//
+// Description: frees the specified SDO channel.
+//              This function must be called after each call to EplApiReadObject()/EplApiWriteObject()
+//              which returns kEplApiTaskDeferred and the application
+//              is informed via the event callback function when the task is completed.
+//
+// Parameters:  SdoComConHdl_p          = IN: SDO connection handle
+//
+// Return:      tEplKernel              = error code
+//
+// ----------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplApiFreeSdoChannel(tEplSdoComConHdl SdoComConHdl_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+
+	// init command layer connection
+	Ret = EplSdoComUndefineCon(SdoComConHdl_p);
+
+#else
+	Ret = kEplApiInvalidParam;
+#endif
+
+	return Ret;
+}
+
+// ----------------------------------------------------------------------------
+//
+// Function:    EplApiReadLocalObject()
+//
+// Description: reads the specified entry from the local OD.
+//
+// Parameters:  uiIndex_p               = IN: index of object in OD
+//              uiSubindex_p            = IN: sub-index of object in OD
+//              pDstData_p              = OUT: pointer to data in platform byte order
+//              puiSize_p               = INOUT: pointer to size of data
+//
+// Return:      tEplKernel              = error code
+//
+// ----------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplApiReadLocalObject(unsigned int uiIndex_p,
+					unsigned int uiSubindex_p,
+					void *pDstData_p,
+					unsigned int *puiSize_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplObdSize ObdSize;
+
+	ObdSize = (tEplObdSize) * puiSize_p;
+	Ret = EplObdReadEntry(uiIndex_p, uiSubindex_p, pDstData_p, &ObdSize);
+	*puiSize_p = (unsigned int)ObdSize;
+
+	return Ret;
+}
+
+// ----------------------------------------------------------------------------
+//
+// Function:    EplApiWriteLocalObject()
+//
+// Description: writes the specified entry to the local OD.
+//
+// Parameters:  uiIndex_p               = IN: index of object in OD
+//              uiSubindex_p            = IN: sub-index of object in OD
+//              pSrcData_p              = IN: pointer to data in platform byte order
+//              uiSize_p                = IN: size of data in bytes
+//
+// Return:      tEplKernel              = error code
+//
+// ----------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplApiWriteLocalObject(unsigned int uiIndex_p,
+					 unsigned int uiSubindex_p,
+					 void *pSrcData_p,
+					 unsigned int uiSize_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	Ret =
+	    EplObdWriteEntry(uiIndex_p, uiSubindex_p, pSrcData_p,
+			     (tEplObdSize) uiSize_p);
+
+	return Ret;
+}
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+// ----------------------------------------------------------------------------
+//
+// Function:    EplApiMnTriggerStateChange()
+//
+// Description: triggers the specified node command for the specified node.
+//
+// Parameters:  uiNodeId_p              = node ID for which the node command will be executed
+//              NodeCommand_p           = node command
+//
+// Return:      tEplKernel              = error code
+//
+// ----------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplApiMnTriggerStateChange(unsigned int uiNodeId_p,
+					     tEplNmtNodeCommand NodeCommand_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	Ret = EplNmtMnuTriggerStateChange(uiNodeId_p, NodeCommand_p);
+
+	return Ret;
+}
+
+#endif // (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplApiCbObdAccess
+//
+// Description: callback function for OD accesses
+//
+// Parameters:  pParam_p                = OBD parameter
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplApiCbObdAccess(tEplObdCbParam MEM * pParam_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+#if (EPL_API_OBD_FORWARD_EVENT != FALSE)
+	tEplApiEventArg EventArg;
+
+	// call user callback
+	// must be disabled for EplApiLinuxKernel.c, because of reentrancy problem
+	// for local OD access. This is not so bad as user callback function in
+	// application does not use OD callbacks at the moment.
+	EventArg.m_ObdCbParam = *pParam_p;
+	Ret = EplApiInstance_g.m_InitParam.m_pfnCbEvent(kEplApiEventObdAccess,
+							&EventArg,
+							EplApiInstance_g.
+							m_InitParam.
+							m_pEventUserArg);
+#endif
+
+	switch (pParam_p->m_uiIndex) {
+		//case 0x1006:    // NMT_CycleLen_U32 (valid on reset)
+	case 0x1C14:		// DLL_LossOfFrameTolerance_U32
+		//case 0x1F98:    // NMT_CycleTiming_REC (valid on reset)
+		{
+			if (pParam_p->m_ObdEvent == kEplObdEvPostWrite) {
+				// update DLL configuration
+				Ret = EplApiUpdateDllConfig(FALSE);
+			}
+			break;
+		}
+
+	case 0x1020:		// CFM_VerifyConfiguration_REC.ConfId_U32 != 0
+		{
+			if ((pParam_p->m_ObdEvent == kEplObdEvPostWrite)
+			    && (pParam_p->m_uiSubIndex == 3)
+			    && (*((DWORD *) pParam_p->m_pArg) != 0)) {
+				DWORD dwVerifyConfInvalid = 0;
+				// set CFM_VerifyConfiguration_REC.VerifyConfInvalid_U32 to 0
+				Ret =
+				    EplObdWriteEntry(0x1020, 4,
+						     &dwVerifyConfInvalid, 4);
+				// ignore any error because this objekt is optional
+				Ret = kEplSuccessful;
+			}
+			break;
+		}
+
+	case 0x1F9E:		// NMT_ResetCmd_U8
+		{
+			if (pParam_p->m_ObdEvent == kEplObdEvPreWrite) {
+				BYTE bNmtCommand;
+
+				bNmtCommand = *((BYTE *) pParam_p->m_pArg);
+				// check value range
+				switch ((tEplNmtCommand) bNmtCommand) {
+				case kEplNmtCmdResetNode:
+				case kEplNmtCmdResetCommunication:
+				case kEplNmtCmdResetConfiguration:
+				case kEplNmtCmdSwReset:
+				case kEplNmtCmdInvalidService:
+					// valid command identifier specified
+					break;
+
+				default:
+					pParam_p->m_dwAbortCode =
+					    EPL_SDOAC_VALUE_RANGE_EXCEEDED;
+					Ret = kEplObdAccessViolation;
+					break;
+				}
+			} else if (pParam_p->m_ObdEvent == kEplObdEvPostWrite) {
+				BYTE bNmtCommand;
+
+				bNmtCommand = *((BYTE *) pParam_p->m_pArg);
+				// check value range
+				switch ((tEplNmtCommand) bNmtCommand) {
+				case kEplNmtCmdResetNode:
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTU)) != 0)
+					Ret =
+					    EplNmtuNmtEvent
+					    (kEplNmtEventResetNode);
+#endif
+					break;
+
+				case kEplNmtCmdResetCommunication:
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTU)) != 0)
+					Ret =
+					    EplNmtuNmtEvent
+					    (kEplNmtEventResetCom);
+#endif
+					break;
+
+				case kEplNmtCmdResetConfiguration:
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTU)) != 0)
+					Ret =
+					    EplNmtuNmtEvent
+					    (kEplNmtEventResetConfig);
+#endif
+					break;
+
+				case kEplNmtCmdSwReset:
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTU)) != 0)
+					Ret =
+					    EplNmtuNmtEvent
+					    (kEplNmtEventSwReset);
+#endif
+					break;
+
+				case kEplNmtCmdInvalidService:
+					break;
+
+				default:
+					pParam_p->m_dwAbortCode =
+					    EPL_SDOAC_VALUE_RANGE_EXCEEDED;
+					Ret = kEplObdAccessViolation;
+					break;
+				}
+			}
+			break;
+		}
+
+	default:
+		break;
+	}
+
+//Exit:
+	return Ret;
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplApiProcessEvent
+//
+// Description: processes events from event queue and forwards these to
+//              the application's event callback function
+//
+// Parameters:  pEplEvent_p =   pointer to event
+//
+// Returns:     tEplKernel  = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel PUBLIC EplApiProcessEvent(tEplEvent * pEplEvent_p)
+{
+	tEplKernel Ret;
+	tEplEventError *pEventError;
+	tEplApiEventType EventType;
+
+	Ret = kEplSuccessful;
+
+	// process event
+	switch (pEplEvent_p->m_EventType) {
+		// error event
+	case kEplEventTypeError:
+		{
+			pEventError = (tEplEventError *) pEplEvent_p->m_pArg;
+			switch (pEventError->m_EventSource) {
+				// treat the errors from the following sources as critical
+			case kEplEventSourceEventk:
+			case kEplEventSourceEventu:
+			case kEplEventSourceDllk:
+				{
+					EventType = kEplApiEventCriticalError;
+					// halt the stack by entering NMT state Off
+					Ret =
+					    EplNmtuNmtEvent
+					    (kEplNmtEventCriticalError);
+					break;
+				}
+
+				// the other errors are just warnings
+			default:
+				{
+					EventType = kEplApiEventWarning;
+					break;
+				}
+			}
+
+			// call user callback
+			Ret =
+			    EplApiInstance_g.m_InitParam.m_pfnCbEvent(EventType,
+								      (tEplApiEventArg
+								       *)
+								      pEventError,
+								      EplApiInstance_g.
+								      m_InitParam.
+								      m_pEventUserArg);
+			// discard error from callback function, because this could generate an endless loop
+			Ret = kEplSuccessful;
+			break;
+		}
+
+		// at present, there are no other events for this module
+	default:
+		break;
+	}
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplApiCbNmtStateChange
+//
+// Description: callback function for NMT state changes
+//
+// Parameters:  NmtStateChange_p        = NMT state change event
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel PUBLIC EplApiCbNmtStateChange(tEplEventNmtStateChange
+						NmtStateChange_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	BYTE bNmtState;
+	tEplApiEventArg EventArg;
+
+	// save NMT state in OD
+	bNmtState = (BYTE) NmtStateChange_p.m_NewNmtState;
+	Ret = EplObdWriteEntry(0x1F8C, 0, &bNmtState, 1);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// do work which must be done in that state
+	switch (NmtStateChange_p.m_NewNmtState) {
+		// EPL stack is not running
+	case kEplNmtGsOff:
+		break;
+
+		// first init of the hardware
+	case kEplNmtGsInitialising:
+#if 0
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) != 0)
+		// configure SDO via UDP (i.e. bind it to the EPL ethernet interface)
+		Ret =
+		    EplSdoUdpuConfig(EplApiInstance_g.m_InitParam.m_dwIpAddress,
+				     EPL_C_SDO_EPL_PORT);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+#endif
+#endif
+
+		break;
+
+		// init of the manufacturer-specific profile area and the
+		// standardised device profile area
+	case kEplNmtGsResetApplication:
+		{
+			// reset application part of OD
+			Ret = EplObdAccessOdPart(kEplObdPartApp,
+						 kEplObdDirLoad);
+			if (Ret != kEplSuccessful) {
+				goto Exit;
+			}
+
+			break;
+		}
+
+		// init of the communication profile area
+	case kEplNmtGsResetCommunication:
+		{
+			// reset communication part of OD
+			Ret = EplObdAccessOdPart(kEplObdPartGen,
+						 kEplObdDirLoad);
+
+			if (Ret != kEplSuccessful) {
+				goto Exit;
+			}
+			// $$$ d.k.: update OD only if OD was not loaded from non-volatile memory
+			Ret = EplApiUpdateObd();
+			if (Ret != kEplSuccessful) {
+				goto Exit;
+			}
+
+			break;
+		}
+
+		// build the configuration with infos from OD
+	case kEplNmtGsResetConfiguration:
+		{
+
+			Ret = EplApiUpdateDllConfig(TRUE);
+			if (Ret != kEplSuccessful) {
+				goto Exit;
+			}
+
+			break;
+		}
+
+		//-----------------------------------------------------------
+		// CN part of the state machine
+
+		// node liste for EPL-Frames and check timeout
+	case kEplNmtCsNotActive:
+		{
+			// indicate completion of reset in NMT_ResetCmd_U8
+			bNmtState = (BYTE) kEplNmtCmdInvalidService;
+			Ret = EplObdWriteEntry(0x1F9E, 0, &bNmtState, 1);
+			if (Ret != kEplSuccessful) {
+				goto Exit;
+			}
+
+			break;
+		}
+
+		// node process only async frames
+	case kEplNmtCsPreOperational1:
+		{
+			break;
+		}
+
+		// node process isochronus and asynchronus frames
+	case kEplNmtCsPreOperational2:
+		{
+			break;
+		}
+
+		// node should be configured und application is ready
+	case kEplNmtCsReadyToOperate:
+		{
+			break;
+		}
+
+		// normal work state
+	case kEplNmtCsOperational:
+		{
+			break;
+		}
+
+		// node stopped by MN
+		// -> only process asynchronus frames
+	case kEplNmtCsStopped:
+		{
+			break;
+		}
+
+		// no EPL cycle
+		// -> normal ethernet communication
+	case kEplNmtCsBasicEthernet:
+		{
+			break;
+		}
+
+		//-----------------------------------------------------------
+		// MN part of the state machine
+
+		// node listens for EPL-Frames and check timeout
+	case kEplNmtMsNotActive:
+		{
+			break;
+		}
+
+		// node processes only async frames
+	case kEplNmtMsPreOperational1:
+		{
+			break;
+		}
+
+		// node processes isochronous and asynchronous frames
+	case kEplNmtMsPreOperational2:
+		{
+			break;
+		}
+
+		// node should be configured und application is ready
+	case kEplNmtMsReadyToOperate:
+		{
+			break;
+		}
+
+		// normal work state
+	case kEplNmtMsOperational:
+		{
+			break;
+		}
+
+		// no EPL cycle
+		// -> normal ethernet communication
+	case kEplNmtMsBasicEthernet:
+		{
+			break;
+		}
+
+	default:
+		{
+			TRACE0
+			    ("EplApiCbNmtStateChange(): unhandled NMT state\n");
+		}
+	}
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_LEDU)) != 0)
+	// forward event to Led module
+	Ret = EplLeduCbNmtStateChange(NmtStateChange_p);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#endif
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+	// forward event to NmtMn module
+	Ret = EplNmtMnuCbNmtStateChange(NmtStateChange_p);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#endif
+
+	// call user callback
+	EventArg.m_NmtStateChange = NmtStateChange_p;
+	Ret =
+	    EplApiInstance_g.m_InitParam.
+	    m_pfnCbEvent(kEplApiEventNmtStateChange, &EventArg,
+			 EplApiInstance_g.m_InitParam.m_pEventUserArg);
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplApiUpdateDllConfig
+//
+// Description: update configuration of DLL
+//
+// Parameters:  fUpdateIdentity_p       = TRUE, if identity must be updated
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel PUBLIC EplApiUpdateDllConfig(BOOL fUpdateIdentity_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplDllConfigParam DllConfigParam;
+	tEplDllIdentParam DllIdentParam;
+	tEplObdSize ObdSize;
+	WORD wTemp;
+	BYTE bTemp;
+
+	// configure Dll
+	EPL_MEMSET(&DllConfigParam, 0, sizeof(DllConfigParam));
+	DllConfigParam.m_uiNodeId = EplObdGetNodeId();
+
+	// Cycle Length (0x1006: NMT_CycleLen_U32) in [us]
+	ObdSize = 4;
+	Ret =
+	    EplObdReadEntry(0x1006, 0, &DllConfigParam.m_dwCycleLen, &ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// 0x1F82: NMT_FeatureFlags_U32
+	ObdSize = 4;
+	Ret =
+	    EplObdReadEntry(0x1F82, 0, &DllConfigParam.m_dwFeatureFlags,
+			    &ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// d.k. There is no dependance between FeatureFlags and async-only CN
+	DllConfigParam.m_fAsyncOnly = EplApiInstance_g.m_InitParam.m_fAsyncOnly;
+
+	// 0x1C14: DLL_LossOfFrameTolerance_U32 in [ns]
+	ObdSize = 4;
+	Ret =
+	    EplObdReadEntry(0x1C14, 0, &DllConfigParam.m_dwLossOfFrameTolerance,
+			    &ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// 0x1F98: NMT_CycleTiming_REC
+	// 0x1F98.1: IsochrTxMaxPayload_U16
+	ObdSize = 2;
+	Ret = EplObdReadEntry(0x1F98, 1, &wTemp, &ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	DllConfigParam.m_uiIsochrTxMaxPayload = wTemp;
+
+	// 0x1F98.2: IsochrRxMaxPayload_U16
+	ObdSize = 2;
+	Ret = EplObdReadEntry(0x1F98, 2, &wTemp, &ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	DllConfigParam.m_uiIsochrRxMaxPayload = wTemp;
+
+	// 0x1F98.3: PResMaxLatency_U32
+	ObdSize = 4;
+	Ret =
+	    EplObdReadEntry(0x1F98, 3, &DllConfigParam.m_dwPresMaxLatency,
+			    &ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// 0x1F98.4: PReqActPayloadLimit_U16
+	ObdSize = 2;
+	Ret = EplObdReadEntry(0x1F98, 4, &wTemp, &ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	DllConfigParam.m_uiPreqActPayloadLimit = wTemp;
+
+	// 0x1F98.5: PResActPayloadLimit_U16
+	ObdSize = 2;
+	Ret = EplObdReadEntry(0x1F98, 5, &wTemp, &ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	DllConfigParam.m_uiPresActPayloadLimit = wTemp;
+
+	// 0x1F98.6: ASndMaxLatency_U32
+	ObdSize = 4;
+	Ret =
+	    EplObdReadEntry(0x1F98, 6, &DllConfigParam.m_dwAsndMaxLatency,
+			    &ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// 0x1F98.7: MultiplCycleCnt_U8
+	ObdSize = 1;
+	Ret = EplObdReadEntry(0x1F98, 7, &bTemp, &ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	DllConfigParam.m_uiMultiplCycleCnt = bTemp;
+
+	// 0x1F98.8: AsyncMTU_U16
+	ObdSize = 2;
+	Ret = EplObdReadEntry(0x1F98, 8, &wTemp, &ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	DllConfigParam.m_uiAsyncMtu = wTemp;
+
+	// $$$ Prescaler
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+	// 0x1F8A.1: WaitSoCPReq_U32 in [ns]
+	ObdSize = 4;
+	Ret =
+	    EplObdReadEntry(0x1F8A, 1, &DllConfigParam.m_dwWaitSocPreq,
+			    &ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// 0x1F8A.2: AsyncSlotTimeout_U32 in [ns] (optional)
+	ObdSize = 4;
+	Ret =
+	    EplObdReadEntry(0x1F8A, 2, &DllConfigParam.m_dwAsyncSlotTimeout,
+			    &ObdSize);
+/*    if(Ret != kEplSuccessful)
+    {
+        goto Exit;
+    }*/
+#endif
+
+	DllConfigParam.m_uiSizeOfStruct = sizeof(DllConfigParam);
+	Ret = EplDllkConfig(&DllConfigParam);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	if (fUpdateIdentity_p != FALSE) {
+		// configure Identity
+		EPL_MEMSET(&DllIdentParam, 0, sizeof(DllIdentParam));
+		ObdSize = 4;
+		Ret =
+		    EplObdReadEntry(0x1000, 0, &DllIdentParam.m_dwDeviceType,
+				    &ObdSize);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+
+		ObdSize = 4;
+		Ret =
+		    EplObdReadEntry(0x1018, 1, &DllIdentParam.m_dwVendorId,
+				    &ObdSize);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+		ObdSize = 4;
+		Ret =
+		    EplObdReadEntry(0x1018, 2, &DllIdentParam.m_dwProductCode,
+				    &ObdSize);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+		ObdSize = 4;
+		Ret =
+		    EplObdReadEntry(0x1018, 3,
+				    &DllIdentParam.m_dwRevisionNumber,
+				    &ObdSize);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+		ObdSize = 4;
+		Ret =
+		    EplObdReadEntry(0x1018, 4, &DllIdentParam.m_dwSerialNumber,
+				    &ObdSize);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+
+		DllIdentParam.m_dwIpAddress =
+		    EplApiInstance_g.m_InitParam.m_dwIpAddress;
+		DllIdentParam.m_dwSubnetMask =
+		    EplApiInstance_g.m_InitParam.m_dwSubnetMask;
+		EPL_MEMCPY(DllIdentParam.m_sHostname,
+			   EplApiInstance_g.m_InitParam.m_sHostname,
+			   sizeof(DllIdentParam.m_sHostname));
+
+		ObdSize = 4;
+		Ret =
+		    EplObdReadEntry(0x1020, 1,
+				    &DllIdentParam.m_dwVerifyConfigurationDate,
+				    &ObdSize);
+		// ignore any error, because this object is optional
+
+		ObdSize = 4;
+		Ret =
+		    EplObdReadEntry(0x1020, 2,
+				    &DllIdentParam.m_dwVerifyConfigurationTime,
+				    &ObdSize);
+		// ignore any error, because this object is optional
+
+		// $$$ d.k.: fill rest of ident structure
+
+		DllIdentParam.m_uiSizeOfStruct = sizeof(DllIdentParam);
+		Ret = EplDllkSetIdentity(&DllIdentParam);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplApiUpdateObd
+//
+// Description: update OD from init param
+//
+// Parameters:  (none)
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel PUBLIC EplApiUpdateObd(void)
+{
+	tEplKernel Ret = kEplSuccessful;
+	WORD wTemp;
+	BYTE bTemp;
+
+	// set node id in OD
+	Ret = EplObdSetNodeId(EplApiInstance_g.m_InitParam.m_uiNodeId,	// node id
+			      kEplObdNodeIdHardware);	// set by hardware
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	if (EplApiInstance_g.m_InitParam.m_dwCycleLen != -1) {
+		Ret =
+		    EplObdWriteEntry(0x1006, 0,
+				     &EplApiInstance_g.m_InitParam.m_dwCycleLen,
+				     4);
+/*    if(Ret != kEplSuccessful)
+    {
+        goto Exit;
+    }*/
+	}
+
+	if (EplApiInstance_g.m_InitParam.m_dwLossOfFrameTolerance != -1) {
+		Ret =
+		    EplObdWriteEntry(0x1C14, 0,
+				     &EplApiInstance_g.m_InitParam.
+				     m_dwLossOfFrameTolerance, 4);
+		/*        if(Ret != kEplSuccessful)
+		   {
+		   goto Exit;
+		   } */
+	}
+	// d.k. There is no dependance between FeatureFlags and async-only CN.
+	if (EplApiInstance_g.m_InitParam.m_dwFeatureFlags != -1) {
+		Ret =
+		    EplObdWriteEntry(0x1F82, 0,
+				     &EplApiInstance_g.m_InitParam.
+				     m_dwFeatureFlags, 4);
+		/*    if(Ret != kEplSuccessful)
+		   {
+		   goto Exit;
+		   } */
+	}
+
+	wTemp = (WORD) EplApiInstance_g.m_InitParam.m_uiIsochrTxMaxPayload;
+	Ret = EplObdWriteEntry(0x1F98, 1, &wTemp, 2);
+/*    if(Ret != kEplSuccessful)
+    {
+        goto Exit;
+    }*/
+
+	wTemp = (WORD) EplApiInstance_g.m_InitParam.m_uiIsochrRxMaxPayload;
+	Ret = EplObdWriteEntry(0x1F98, 2, &wTemp, 2);
+/*    if(Ret != kEplSuccessful)
+    {
+        goto Exit;
+    }*/
+
+	Ret =
+	    EplObdWriteEntry(0x1F98, 3,
+			     &EplApiInstance_g.m_InitParam.m_dwPresMaxLatency,
+			     4);
+/*    if(Ret != kEplSuccessful)
+    {
+        goto Exit;
+    }*/
+
+	if (EplApiInstance_g.m_InitParam.m_uiPreqActPayloadLimit <=
+	    EPL_C_DLL_ISOCHR_MAX_PAYL) {
+		wTemp =
+		    (WORD) EplApiInstance_g.m_InitParam.m_uiPreqActPayloadLimit;
+		Ret = EplObdWriteEntry(0x1F98, 4, &wTemp, 2);
+/*    if(Ret != kEplSuccessful)
+    {
+        goto Exit;
+    }*/
+	}
+
+	if (EplApiInstance_g.m_InitParam.m_uiPresActPayloadLimit <=
+	    EPL_C_DLL_ISOCHR_MAX_PAYL) {
+		wTemp =
+		    (WORD) EplApiInstance_g.m_InitParam.m_uiPresActPayloadLimit;
+		Ret = EplObdWriteEntry(0x1F98, 5, &wTemp, 2);
+/*    if(Ret != kEplSuccessful)
+    {
+        goto Exit;
+    }*/
+	}
+
+	Ret =
+	    EplObdWriteEntry(0x1F98, 6,
+			     &EplApiInstance_g.m_InitParam.m_dwAsndMaxLatency,
+			     4);
+/*    if(Ret != kEplSuccessful)
+    {
+        goto Exit;
+    }*/
+
+	if (EplApiInstance_g.m_InitParam.m_uiMultiplCycleCnt <= 0xFF) {
+		bTemp = (BYTE) EplApiInstance_g.m_InitParam.m_uiMultiplCycleCnt;
+		Ret = EplObdWriteEntry(0x1F98, 7, &bTemp, 1);
+/*    if(Ret != kEplSuccessful)
+    {
+        goto Exit;
+    }*/
+	}
+
+	if (EplApiInstance_g.m_InitParam.m_uiAsyncMtu <=
+	    EPL_C_DLL_MAX_ASYNC_MTU) {
+		wTemp = (WORD) EplApiInstance_g.m_InitParam.m_uiAsyncMtu;
+		Ret = EplObdWriteEntry(0x1F98, 8, &wTemp, 2);
+/*    if(Ret != kEplSuccessful)
+    {
+        goto Exit;
+    }*/
+	}
+
+	if (EplApiInstance_g.m_InitParam.m_uiPrescaler <= 1000) {
+		wTemp = (WORD) EplApiInstance_g.m_InitParam.m_uiPrescaler;
+		Ret = EplObdWriteEntry(0x1F98, 9, &wTemp, 2);
+		// ignore return code
+		Ret = kEplSuccessful;
+	}
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+	if (EplApiInstance_g.m_InitParam.m_dwWaitSocPreq != -1) {
+		Ret =
+		    EplObdWriteEntry(0x1F8A, 1,
+				     &EplApiInstance_g.m_InitParam.
+				     m_dwWaitSocPreq, 4);
+		/*        if(Ret != kEplSuccessful)
+		   {
+		   goto Exit;
+		   } */
+	}
+
+	if ((EplApiInstance_g.m_InitParam.m_dwAsyncSlotTimeout != 0)
+	    && (EplApiInstance_g.m_InitParam.m_dwAsyncSlotTimeout != -1)) {
+		Ret =
+		    EplObdWriteEntry(0x1F8A, 2,
+				     &EplApiInstance_g.m_InitParam.
+				     m_dwAsyncSlotTimeout, 4);
+		/*        if(Ret != kEplSuccessful)
+		   {
+		   goto Exit;
+		   } */
+	}
+#endif
+
+	// configure Identity
+	if (EplApiInstance_g.m_InitParam.m_dwDeviceType != -1) {
+		Ret =
+		    EplObdWriteEntry(0x1000, 0,
+				     &EplApiInstance_g.m_InitParam.
+				     m_dwDeviceType, 4);
+/*        if(Ret != kEplSuccessful)
+        {
+            goto Exit;
+        }*/
+	}
+
+	if (EplApiInstance_g.m_InitParam.m_dwVendorId != -1) {
+		Ret =
+		    EplObdWriteEntry(0x1018, 1,
+				     &EplApiInstance_g.m_InitParam.m_dwVendorId,
+				     4);
+/*        if(Ret != kEplSuccessful)
+        {
+            goto Exit;
+        }*/
+	}
+
+	if (EplApiInstance_g.m_InitParam.m_dwProductCode != -1) {
+		Ret =
+		    EplObdWriteEntry(0x1018, 2,
+				     &EplApiInstance_g.m_InitParam.
+				     m_dwProductCode, 4);
+/*        if(Ret != kEplSuccessful)
+        {
+            goto Exit;
+        }*/
+	}
+
+	if (EplApiInstance_g.m_InitParam.m_dwRevisionNumber != -1) {
+		Ret =
+		    EplObdWriteEntry(0x1018, 3,
+				     &EplApiInstance_g.m_InitParam.
+				     m_dwRevisionNumber, 4);
+/*        if(Ret != kEplSuccessful)
+        {
+            goto Exit;
+        }*/
+	}
+
+	if (EplApiInstance_g.m_InitParam.m_dwSerialNumber != -1) {
+		Ret =
+		    EplObdWriteEntry(0x1018, 4,
+				     &EplApiInstance_g.m_InitParam.
+				     m_dwSerialNumber, 4);
+/*        if(Ret != kEplSuccessful)
+        {
+            goto Exit;
+        }*/
+	}
+
+	if (EplApiInstance_g.m_InitParam.m_pszDevName != NULL) {
+		// write Device Name (0x1008)
+		Ret =
+		    EplObdWriteEntry(0x1008, 0,
+				     (void GENERIC *)EplApiInstance_g.
+				     m_InitParam.m_pszDevName,
+				     (tEplObdSize) strlen(EplApiInstance_g.
+							  m_InitParam.
+							  m_pszDevName));
+/*        if (Ret != kEplSuccessful)
+        {
+            goto Exit;
+        }*/
+	}
+
+	if (EplApiInstance_g.m_InitParam.m_pszHwVersion != NULL) {
+		// write Hardware version (0x1009)
+		Ret =
+		    EplObdWriteEntry(0x1009, 0,
+				     (void GENERIC *)EplApiInstance_g.
+				     m_InitParam.m_pszHwVersion,
+				     (tEplObdSize) strlen(EplApiInstance_g.
+							  m_InitParam.
+							  m_pszHwVersion));
+/*        if (Ret != kEplSuccessful)
+        {
+            goto Exit;
+        }*/
+	}
+
+	if (EplApiInstance_g.m_InitParam.m_pszSwVersion != NULL) {
+		// write Software version (0x100A)
+		Ret =
+		    EplObdWriteEntry(0x100A, 0,
+				     (void GENERIC *)EplApiInstance_g.
+				     m_InitParam.m_pszSwVersion,
+				     (tEplObdSize) strlen(EplApiInstance_g.
+							  m_InitParam.
+							  m_pszSwVersion));
+/*        if (Ret != kEplSuccessful)
+        {
+            goto Exit;
+        }*/
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplApiCbSdoCon
+//
+// Description: callback function for SDO transfers
+//
+// Parameters:  pSdoComFinished_p       = SDO parameter
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+static tEplKernel PUBLIC EplApiCbSdoCon(tEplSdoComFinished * pSdoComFinished_p)
+{
+	tEplKernel Ret;
+	tEplApiEventArg EventArg;
+
+	Ret = kEplSuccessful;
+
+	// call user callback
+	EventArg.m_Sdo = *pSdoComFinished_p;
+	Ret = EplApiInstance_g.m_InitParam.m_pfnCbEvent(kEplApiEventSdo,
+							&EventArg,
+							EplApiInstance_g.
+							m_InitParam.
+							m_pEventUserArg);
+
+	return Ret;
+
+}
+#endif
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplApiCbNodeEvent
+//
+// Description: callback function for node events
+//
+// Parameters:  uiNodeId_p              = node ID of the CN
+//              NodeEvent_p             = event from the specified CN
+//              NmtState_p              = current NMT state of the CN
+//              wErrorCode_p            = EPL error code if NodeEvent_p==kEplNmtNodeEventError
+//              fMandatory_p            = flag if CN is mandatory
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel PUBLIC EplApiCbNodeEvent(unsigned int uiNodeId_p,
+					   tEplNmtNodeEvent NodeEvent_p,
+					   tEplNmtState NmtState_p,
+					   WORD wErrorCode_p, BOOL fMandatory_p)
+{
+	tEplKernel Ret;
+	tEplApiEventArg EventArg;
+
+	Ret = kEplSuccessful;
+
+	// call user callback
+	EventArg.m_Node.m_uiNodeId = uiNodeId_p;
+	EventArg.m_Node.m_NodeEvent = NodeEvent_p;
+	EventArg.m_Node.m_NmtState = NmtState_p;
+	EventArg.m_Node.m_wErrorCode = wErrorCode_p;
+	EventArg.m_Node.m_fMandatory = fMandatory_p;
+
+	Ret = EplApiInstance_g.m_InitParam.m_pfnCbEvent(kEplApiEventNode,
+							&EventArg,
+							EplApiInstance_g.
+							m_InitParam.
+							m_pEventUserArg);
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplApiCbBootEvent
+//
+// Description: callback function for boot events
+//
+// Parameters:  BootEvent_p             = event from the boot-up process
+//              NmtState_p              = current local NMT state
+//              wErrorCode_p            = EPL error code if BootEvent_p==kEplNmtBootEventError
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel PUBLIC EplApiCbBootEvent(tEplNmtBootEvent BootEvent_p,
+					   tEplNmtState NmtState_p,
+					   WORD wErrorCode_p)
+{
+	tEplKernel Ret;
+	tEplApiEventArg EventArg;
+
+	Ret = kEplSuccessful;
+
+	// call user callback
+	EventArg.m_Boot.m_BootEvent = BootEvent_p;
+	EventArg.m_Boot.m_NmtState = NmtState_p;
+	EventArg.m_Boot.m_wErrorCode = wErrorCode_p;
+
+	Ret = EplApiInstance_g.m_InitParam.m_pfnCbEvent(kEplApiEventBoot,
+							&EventArg,
+							EplApiInstance_g.
+							m_InitParam.
+							m_pEventUserArg);
+
+	return Ret;
+
+}
+
+#endif // (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_LEDU)) != 0)
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplApiCbLedStateChange
+//
+// Description: callback function for LED change events.
+//
+// Parameters:  LedType_p       = type of LED
+//              fOn_p           = state of LED
+//
+// Returns:     tEplKernel      = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel PUBLIC EplApiCbLedStateChange(tEplLedType LedType_p,
+						BOOL fOn_p)
+{
+	tEplKernel Ret;
+	tEplApiEventArg EventArg;
+
+	Ret = kEplSuccessful;
+
+	// call user callback
+	EventArg.m_Led.m_LedType = LedType_p;
+	EventArg.m_Led.m_fOn = fOn_p;
+
+	Ret = EplApiInstance_g.m_InitParam.m_pfnCbEvent(kEplApiEventLed,
+							&EventArg,
+							EplApiInstance_g.
+							m_InitParam.
+							m_pEventUserArg);
+
+	return Ret;
+
+}
+
+#endif
+
+// EOF
diff --git a/drivers/staging/epl/EplApiLinux.h b/drivers/staging/epl/EplApiLinux.h
new file mode 100644
index 00000000000..92cd12532a6
--- /dev/null
+++ b/drivers/staging/epl/EplApiLinux.h
@@ -0,0 +1,141 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for EPL API layer for Linux (kernel and user space)
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplApiLinux.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/08/25 12:17:41 $
+
+                $State: Exp $
+
+                Build Environment:
+                KEIL uVision 2
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/10/11 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_API_LINUX_H_
+#define _EPL_API_LINUX_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#define EPLLIN_DEV_NAME     "epl"	// used for "/dev" and "/proc" entry
+
+//---------------------------------------------------------------------------
+//  Commands for <ioctl>
+//---------------------------------------------------------------------------
+
+#define EPLLIN_CMD_INITIALIZE               0	// ulArg_p ~ tEplApiInitParam*
+#define EPLLIN_CMD_PI_IN                    1	// ulArg_p ~ tEplApiProcessImage*
+#define EPLLIN_CMD_PI_OUT                   2	// ulArg_p ~ tEplApiProcessImage*
+#define EPLLIN_CMD_WRITE_OBJECT             3	// ulArg_p ~ tEplLinSdoObject*
+#define EPLLIN_CMD_READ_OBJECT              4	// ulArg_p ~ tEplLinSdoObject*
+#define EPLLIN_CMD_WRITE_LOCAL_OBJECT       5	// ulArg_p ~ tEplLinLocalObject*
+#define EPLLIN_CMD_READ_LOCAL_OBJECT        6	// ulArg_p ~ tEplLinLocalObject*
+#define EPLLIN_CMD_FREE_SDO_CHANNEL         7	// ulArg_p ~ tEplSdoComConHdl
+#define EPLLIN_CMD_NMT_COMMAND              8	// ulArg_p ~ tEplNmtEvent
+#define EPLLIN_CMD_GET_EVENT                9	// ulArg_p ~ tEplLinEvent*
+#define EPLLIN_CMD_MN_TRIGGER_STATE_CHANGE 10	// ulArg_p ~ tEplLinNodeCmdObject*
+#define EPLLIN_CMD_PI_SETUP                11	// ulArg_p ~ 0
+#define EPLLIN_CMD_SHUTDOWN                12	// ulArg_p ~ 0
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+typedef struct {
+	unsigned int m_uiEventArgSize;
+	tEplApiEventArg *m_pEventArg;
+	tEplApiEventType *m_pEventType;
+	tEplKernel m_RetCbEvent;
+
+} tEplLinEvent;
+
+typedef struct {
+	tEplSdoComConHdl m_SdoComConHdl;
+	BOOL m_fValidSdoComConHdl;
+	unsigned int m_uiNodeId;
+	unsigned int m_uiIndex;
+	unsigned int m_uiSubindex;
+	void *m_le_pData;
+	unsigned int m_uiSize;
+	tEplSdoType m_SdoType;
+	void *m_pUserArg;
+
+} tEplLinSdoObject;
+
+typedef struct {
+	unsigned int m_uiIndex;
+	unsigned int m_uiSubindex;
+	void *m_pData;
+	unsigned int m_uiSize;
+
+} tEplLinLocalObject;
+
+typedef struct {
+	unsigned int m_uiNodeId;
+	tEplNmtNodeCommand m_NodeCommand;
+
+} tEplLinNodeCmdObject;
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#endif // #ifndef _EPL_API_LINUX_H_
diff --git a/drivers/staging/epl/EplApiLinuxKernel.c b/drivers/staging/epl/EplApiLinuxKernel.c
new file mode 100644
index 00000000000..05ca0628d6a
--- /dev/null
+++ b/drivers/staging/epl/EplApiLinuxKernel.c
@@ -0,0 +1,1260 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  Linux kernel module as wrapper of EPL API layer,
+                i.e. counterpart to a Linux application
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplApiLinuxKernel.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.9 $  $Date: 2008/11/21 09:00:38 $
+
+                $State: Exp $
+
+                Build Environment:
+                GNU-Compiler for m68k
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/10/11 d.k.:  Initial Version
+  2008/04/10 m.u.:  Changed to new char driver init
+
+****************************************************************************/
+
+// kernel modul and driver
+
+//#include <linux/version.h>
+//#include <linux/config.h>
+
+#include <linux/module.h>
+#include <linux/fs.h>
+#include <linux/cdev.h>
+#include <linux/types.h>
+
+//#include <linux/module.h>
+//#include <linux/kernel.h>
+//#include <linux/init.h>
+//#include <linux/errno.h>
+
+// scheduling
+#include <linux/sched.h>
+
+// memory access
+#include <asm/uaccess.h>
+#include <linux/vmalloc.h>
+
+#ifdef CONFIG_DEVFS_FS
+#include <linux/major.h>
+#include <linux/devfs_fs_kernel.h>
+#endif
+
+#include "Epl.h"
+#include "EplApiLinux.h"
+//#include "kernel/EplPdokCal.h"
+#include "proc_fs.h"
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0)
+    // remove ("make invisible") obsolete symbols for kernel versions 2.6
+    // and higher
+#define MOD_INC_USE_COUNT
+#define MOD_DEC_USE_COUNT
+#define EXPORT_NO_SYMBOLS
+#else
+#error "This driver needs a 2.6.x kernel or higher"
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+// Metainformation
+MODULE_LICENSE("Dual BSD/GPL");
+#ifdef MODULE_AUTHOR
+MODULE_AUTHOR("Daniel.Krueger@SYSTEC-electronic.com");
+MODULE_DESCRIPTION("EPL API driver");
+#endif
+
+//---------------------------------------------------------------------------
+//  Configuration
+//---------------------------------------------------------------------------
+
+#define EPLLIN_DRV_NAME     "systec_epl"	// used for <register_chrdev>
+
+//---------------------------------------------------------------------------
+//  Constant definitions
+//---------------------------------------------------------------------------
+
+// TracePoint support for realtime-debugging
+#ifdef _DBG_TRACE_POINTS_
+void PUBLIC TgtDbgSignalTracePoint(BYTE bTracePointNumber_p);
+#define TGT_DBG_SIGNAL_TRACE_POINT(p)   TgtDbgSignalTracePoint(p)
+#else
+#define TGT_DBG_SIGNAL_TRACE_POINT(p)
+#endif
+
+#define EVENT_STATE_INIT        0
+#define EVENT_STATE_IOCTL       1	// ioctl entered and ready to receive EPL event
+#define EVENT_STATE_READY       2	// EPL event can be forwarded to user application
+#define EVENT_STATE_TERM        3	// terminate processing
+
+#define EPL_STATE_NOTOPEN       0
+#define EPL_STATE_NOTINIT       1
+#define EPL_STATE_RUNNING       2
+#define EPL_STATE_SHUTDOWN      3
+
+//---------------------------------------------------------------------------
+//  Global variables
+//---------------------------------------------------------------------------
+
+#ifdef CONFIG_DEVFS_FS
+
+    // driver major number
+static int nDrvMajorNumber_g;
+
+#else
+
+    // device number (major and minor)
+static dev_t nDevNum_g;
+static struct cdev *pEpl_cdev_g;
+
+#endif
+
+static volatile unsigned int uiEplState_g = EPL_STATE_NOTOPEN;
+
+static struct semaphore SemaphoreCbEvent_g;	// semaphore for EplLinCbEvent
+static wait_queue_head_t WaitQueueCbEvent_g;	// wait queue EplLinCbEvent
+static wait_queue_head_t WaitQueueProcess_g;	// wait queue for EplApiProcess (user process)
+static wait_queue_head_t WaitQueueRelease_g;	// wait queue for EplLinRelease
+static atomic_t AtomicEventState_g = ATOMIC_INIT(EVENT_STATE_INIT);
+static tEplApiEventType EventType_g;	// event type (enum)
+static tEplApiEventArg *pEventArg_g;	// event argument (union)
+static tEplKernel RetCbEvent_g;	// return code from event callback function
+static wait_queue_head_t WaitQueueCbSync_g;	// wait queue EplLinCbSync
+static wait_queue_head_t WaitQueuePI_In_g;	// wait queue for EplApiProcessImageExchangeIn (user process)
+static atomic_t AtomicSyncState_g = ATOMIC_INIT(EVENT_STATE_INIT);
+
+//---------------------------------------------------------------------------
+//  Local types
+//---------------------------------------------------------------------------
+
+typedef struct {
+	void *m_pUserArg;
+	void *m_pData;
+
+} tEplLinSdoBufHeader;
+
+//---------------------------------------------------------------------------
+//  Local variables
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+//  Prototypes of internal functions
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplLinCbEvent(tEplApiEventType EventType_p,	// IN: event type (enum)
+				tEplApiEventArg * pEventArg_p,	// IN: event argument (union)
+				void GENERIC * pUserArg_p);
+
+tEplKernel PUBLIC EplLinCbSync(void);
+
+static int __init EplLinInit(void);
+static void __exit EplLinExit(void);
+
+static int EplLinOpen(struct inode *pDeviceFile_p, struct file *pInstance_p);
+static int EplLinRelease(struct inode *pDeviceFile_p, struct file *pInstance_p);
+static ssize_t EplLinRead(struct file *pInstance_p, char *pDstBuff_p,
+			  size_t BuffSize_p, loff_t * pFileOffs_p);
+static ssize_t EplLinWrite(struct file *pInstance_p, const char *pSrcBuff_p,
+			   size_t BuffSize_p, loff_t * pFileOffs_p);
+static int EplLinIoctl(struct inode *pDeviceFile_p, struct file *pInstance_p,
+		       unsigned int uiIoctlCmd_p, unsigned long ulArg_p);
+
+//---------------------------------------------------------------------------
+//  Kernel Module specific Data Structures
+//---------------------------------------------------------------------------
+
+EXPORT_NO_SYMBOLS;
+
+module_init(EplLinInit);
+module_exit(EplLinExit);
+
+static struct file_operations EplLinFileOps_g = {
+	.owner = THIS_MODULE,
+	.open = EplLinOpen,
+	.release = EplLinRelease,
+	.read = EplLinRead,
+	.write = EplLinWrite,
+	.ioctl = EplLinIoctl,
+
+};
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//  Initailize Driver
+//---------------------------------------------------------------------------
+//  -> insmod driver
+//---------------------------------------------------------------------------
+
+static int __init EplLinInit(void)
+{
+
+	tEplKernel EplRet;
+	int iErr;
+	int iRet;
+#ifdef CONFIG_DEVFS_FS
+	int nMinorNumber;
+#endif
+
+	TRACE0("EPL: + EplLinInit...\n");
+	TRACE2("EPL:   Driver build: %s / %s\n", __DATE__, __TIME__);
+
+	iRet = 0;
+
+	// initialize global variables
+	atomic_set(&AtomicEventState_g, EVENT_STATE_INIT);
+	sema_init(&SemaphoreCbEvent_g, 1);
+	init_waitqueue_head(&WaitQueueCbEvent_g);
+	init_waitqueue_head(&WaitQueueProcess_g);
+	init_waitqueue_head(&WaitQueueRelease_g);
+
+#ifdef CONFIG_DEVFS_FS
+
+	// register character device handler
+	TRACE2("EPL:   Installing Driver '%s', Version %s...\n",
+	       EPLLIN_DRV_NAME, EPL_PRODUCT_VERSION);
+	TRACE0("EPL:   (using dynamic major number assignment)\n");
+	nDrvMajorNumber_g =
+	    register_chrdev(0, EPLLIN_DRV_NAME, &EplLinFileOps_g);
+	if (nDrvMajorNumber_g != 0) {
+		TRACE2
+		    ("EPL:   Driver '%s' installed successful, assigned MajorNumber=%d\n",
+		     EPLLIN_DRV_NAME, nDrvMajorNumber_g);
+	} else {
+		TRACE1
+		    ("EPL:   ERROR: Driver '%s' is unable to get a free MajorNumber!\n",
+		     EPLLIN_DRV_NAME);
+		iRet = -EIO;
+		goto Exit;
+	}
+
+	// create device node in DEVFS
+	nMinorNumber = 0;
+	TRACE1("EPL:   Creating device node '/dev/%s'...\n", EPLLIN_DEV_NAME);
+	iErr =
+	    devfs_mk_cdev(MKDEV(nDrvMajorNumber_g, nMinorNumber),
+			  S_IFCHR | S_IRUGO | S_IWUGO, EPLLIN_DEV_NAME);
+	if (iErr == 0) {
+		TRACE1("EPL:   Device node '/dev/%s' created successful.\n",
+		       EPLLIN_DEV_NAME);
+	} else {
+		TRACE1("EPL:   ERROR: unable to create device node '/dev/%s'\n",
+		       EPLLIN_DEV_NAME);
+		iRet = -EIO;
+		goto Exit;
+	}
+
+#else
+
+	// register character device handler
+	// only one Minor required
+	TRACE2("EPL:   Installing Driver '%s', Version %s...\n",
+	       EPLLIN_DRV_NAME, EPL_PRODUCT_VERSION);
+	iRet = alloc_chrdev_region(&nDevNum_g, 0, 1, EPLLIN_DRV_NAME);
+	if (iRet == 0) {
+		TRACE2
+		    ("EPL:   Driver '%s' installed successful, assigned MajorNumber=%d\n",
+		     EPLLIN_DRV_NAME, MAJOR(nDevNum_g));
+	} else {
+		TRACE1
+		    ("EPL:   ERROR: Driver '%s' is unable to get a free MajorNumber!\n",
+		     EPLLIN_DRV_NAME);
+		iRet = -EIO;
+		goto Exit;
+	}
+
+	// register cdev structure
+	pEpl_cdev_g = cdev_alloc();
+	pEpl_cdev_g->ops = &EplLinFileOps_g;
+	pEpl_cdev_g->owner = THIS_MODULE;
+	iErr = cdev_add(pEpl_cdev_g, nDevNum_g, 1);
+	if (iErr) {
+		TRACE2("EPL:   ERROR %d: Driver '%s' could not be added!\n",
+		       iErr, EPLLIN_DRV_NAME);
+		iRet = -EIO;
+		goto Exit;
+	}
+#endif
+
+	// create device node in PROCFS
+	EplRet = EplLinProcInit();
+	if (EplRet != kEplSuccessful) {
+		goto Exit;
+	}
+
+      Exit:
+
+	TRACE1("EPL: - EplLinInit (iRet=%d)\n", iRet);
+	return (iRet);
+
+}
+
+//---------------------------------------------------------------------------
+//  Remove Driver
+//---------------------------------------------------------------------------
+//  -> rmmod driver
+//---------------------------------------------------------------------------
+
+static void __exit EplLinExit(void)
+{
+
+	tEplKernel EplRet;
+
+	// delete instance for all modules
+//    EplRet = EplApiShutdown();
+//    printk("EplApiShutdown():  0x%X\n", EplRet);
+
+	// deinitialize proc fs
+	EplRet = EplLinProcFree();
+	printk("EplLinProcFree():        0x%X\n", EplRet);
+
+	TRACE0("EPL: + EplLinExit...\n");
+
+#ifdef CONFIG_DEVFS_FS
+
+	// remove device node from DEVFS
+	devfs_remove(EPLLIN_DEV_NAME);
+	TRACE1("EPL:   Device node '/dev/%s' removed.\n", EPLLIN_DEV_NAME);
+
+	// unregister character device handler
+	unregister_chrdev(nDrvMajorNumber_g, EPLLIN_DRV_NAME);
+
+#else
+
+	// remove cdev structure
+	cdev_del(pEpl_cdev_g);
+
+	// unregister character device handler
+	unregister_chrdev_region(nDevNum_g, 1);
+
+#endif
+
+	TRACE1("EPL:   Driver '%s' removed.\n", EPLLIN_DRV_NAME);
+
+	TRACE0("EPL: - EplLinExit\n");
+
+}
+
+//---------------------------------------------------------------------------
+//  Open Driver
+//---------------------------------------------------------------------------
+//  -> open("/dev/driver", O_RDWR)...
+//---------------------------------------------------------------------------
+
+static int EplLinOpen(struct inode *pDeviceFile_p,	// information about the device to open
+		      struct file *pInstance_p)	// information about driver instance
+{
+
+	int iRet;
+
+	TRACE0("EPL: + EplLinOpen...\n");
+
+	MOD_INC_USE_COUNT;
+
+	if (uiEplState_g != EPL_STATE_NOTOPEN) {	// stack already initialized
+		iRet = -EALREADY;
+	} else {
+		atomic_set(&AtomicEventState_g, EVENT_STATE_INIT);
+		sema_init(&SemaphoreCbEvent_g, 1);
+		init_waitqueue_head(&WaitQueueCbEvent_g);
+		init_waitqueue_head(&WaitQueueProcess_g);
+		init_waitqueue_head(&WaitQueueRelease_g);
+		atomic_set(&AtomicSyncState_g, EVENT_STATE_INIT);
+		init_waitqueue_head(&WaitQueueCbSync_g);
+		init_waitqueue_head(&WaitQueuePI_In_g);
+
+		uiEplState_g = EPL_STATE_NOTINIT;
+		iRet = 0;
+	}
+
+	TRACE1("EPL: - EplLinOpen (iRet=%d)\n", iRet);
+	return (iRet);
+
+}
+
+//---------------------------------------------------------------------------
+//  Close Driver
+//---------------------------------------------------------------------------
+//  -> close(device)...
+//---------------------------------------------------------------------------
+
+static int EplLinRelease(struct inode *pDeviceFile_p,	// information about the device to open
+			 struct file *pInstance_p)	// information about driver instance
+{
+
+	tEplKernel EplRet = kEplSuccessful;
+	int iRet;
+
+	TRACE0("EPL: + EplLinRelease...\n");
+
+	if (uiEplState_g != EPL_STATE_NOTINIT) {
+		// pass control to sync kernel thread, but signal termination
+		atomic_set(&AtomicSyncState_g, EVENT_STATE_TERM);
+		wake_up_interruptible(&WaitQueueCbSync_g);
+		wake_up_interruptible(&WaitQueuePI_In_g);
+
+		// pass control to event queue kernel thread
+		atomic_set(&AtomicEventState_g, EVENT_STATE_TERM);
+		wake_up_interruptible(&WaitQueueCbEvent_g);
+
+		if (uiEplState_g == EPL_STATE_RUNNING) {	// post NmtEventSwitchOff
+			EplRet = EplApiExecNmtCommand(kEplNmtEventSwitchOff);
+
+		}
+
+		if (EplRet == kEplSuccessful) {
+			TRACE0("EPL:   waiting for NMT_GS_OFF\n");
+			wait_event_interruptible(WaitQueueRelease_g,
+						 (uiEplState_g ==
+						  EPL_STATE_SHUTDOWN));
+		} else {	// post NmtEventSwitchOff failed
+			TRACE0("EPL:   event post failed\n");
+		}
+
+		// $$$ d.k.: What if waiting was interrupted by signal?
+
+		TRACE0("EPL:   call EplApiShutdown()\n");
+		// EPL stack can be safely shut down
+		// delete instance for all EPL modules
+		EplRet = EplApiShutdown();
+		printk("EplApiShutdown():  0x%X\n", EplRet);
+	}
+
+	uiEplState_g = EPL_STATE_NOTOPEN;
+	iRet = 0;
+
+	MOD_DEC_USE_COUNT;
+
+	TRACE1("EPL: - EplLinRelease (iRet=%d)\n", iRet);
+	return (iRet);
+
+}
+
+//---------------------------------------------------------------------------
+//  Read Data from Driver
+//---------------------------------------------------------------------------
+//  -> read(...)
+//---------------------------------------------------------------------------
+
+static ssize_t EplLinRead(struct file *pInstance_p,	// information about driver instance
+			  char *pDstBuff_p,	// address of buffer to fill with data
+			  size_t BuffSize_p,	// length of the buffer
+			  loff_t * pFileOffs_p)	// offset in the file
+{
+
+	int iRet;
+
+	TRACE0("EPL: + EplLinRead...\n");
+
+	TRACE0("EPL:   Sorry, this operation isn't supported.\n");
+	iRet = -EINVAL;
+
+	TRACE1("EPL: - EplLinRead (iRet=%d)\n", iRet);
+	return (iRet);
+
+}
+
+//---------------------------------------------------------------------------
+//  Write Data to Driver
+//---------------------------------------------------------------------------
+//  -> write(...)
+//---------------------------------------------------------------------------
+
+static ssize_t EplLinWrite(struct file *pInstance_p,	// information about driver instance
+			   const char *pSrcBuff_p,	// address of buffer to get data from
+			   size_t BuffSize_p,	// length of the buffer
+			   loff_t * pFileOffs_p)	// offset in the file
+{
+
+	int iRet;
+
+	TRACE0("EPL: + EplLinWrite...\n");
+
+	TRACE0("EPL:   Sorry, this operation isn't supported.\n");
+	iRet = -EINVAL;
+
+	TRACE1("EPL: - EplLinWrite (iRet=%d)\n", iRet);
+	return (iRet);
+
+}
+
+//---------------------------------------------------------------------------
+//  Generic Access to Driver
+//---------------------------------------------------------------------------
+//  -> ioctl(...)
+//---------------------------------------------------------------------------
+
+static int EplLinIoctl(struct inode *pDeviceFile_p,	// information about the device to open
+		       struct file *pInstance_p,	// information about driver instance
+		       unsigned int uiIoctlCmd_p,	// Ioctl command to execute
+		       unsigned long ulArg_p)	// Ioctl command specific argument/parameter
+{
+
+	tEplKernel EplRet;
+	int iErr;
+	int iRet;
+
+//    TRACE1("EPL: + EplLinIoctl (uiIoctlCmd_p=%d)...\n", uiIoctlCmd_p);
+
+	iRet = -EINVAL;
+
+	switch (uiIoctlCmd_p) {
+		// ----------------------------------------------------------
+	case EPLLIN_CMD_INITIALIZE:
+		{
+			tEplApiInitParam EplApiInitParam;
+
+			iErr =
+			    copy_from_user(&EplApiInitParam,
+					   (const void *)ulArg_p,
+					   sizeof(EplApiInitParam));
+			if (iErr != 0) {
+				iRet = -EIO;
+				goto Exit;
+			}
+
+			EplApiInitParam.m_pfnCbEvent = EplLinCbEvent;
+			EplApiInitParam.m_pfnCbSync = EplLinCbSync;
+
+			EplRet = EplApiInitialize(&EplApiInitParam);
+
+			uiEplState_g = EPL_STATE_RUNNING;
+
+			iRet = (int)EplRet;
+			break;
+		}
+
+		// ----------------------------------------------------------
+	case EPLLIN_CMD_SHUTDOWN:
+		{		// shutdown the threads
+
+			// pass control to sync kernel thread, but signal termination
+			atomic_set(&AtomicSyncState_g, EVENT_STATE_TERM);
+			wake_up_interruptible(&WaitQueueCbSync_g);
+			wake_up_interruptible(&WaitQueuePI_In_g);
+
+			// pass control to event queue kernel thread
+			atomic_set(&AtomicEventState_g, EVENT_STATE_TERM);
+			wake_up_interruptible(&WaitQueueCbEvent_g);
+
+			if (uiEplState_g == EPL_STATE_RUNNING) {	// post NmtEventSwitchOff
+				EplRet =
+				    EplApiExecNmtCommand(kEplNmtEventSwitchOff);
+
+			}
+
+			iRet = 0;
+			break;
+		}
+
+		// ----------------------------------------------------------
+	case EPLLIN_CMD_READ_LOCAL_OBJECT:
+		{
+			tEplLinLocalObject LocalObject;
+			void *pData;
+
+			iErr =
+			    copy_from_user(&LocalObject, (const void *)ulArg_p,
+					   sizeof(LocalObject));
+			if (iErr != 0) {
+				iRet = -EIO;
+				goto Exit;
+			}
+
+			if ((LocalObject.m_pData == NULL)
+			    || (LocalObject.m_uiSize == 0)) {
+				iRet = (int)kEplApiInvalidParam;
+				goto Exit;
+			}
+
+			pData = vmalloc(LocalObject.m_uiSize);
+			if (pData == NULL) {	// no memory available
+				iRet = -ENOMEM;
+				goto Exit;
+			}
+
+			EplRet =
+			    EplApiReadLocalObject(LocalObject.m_uiIndex,
+						  LocalObject.m_uiSubindex,
+						  pData, &LocalObject.m_uiSize);
+
+			if (EplRet == kEplSuccessful) {
+				iErr =
+				    copy_to_user(LocalObject.m_pData, pData,
+						 LocalObject.m_uiSize);
+
+				vfree(pData);
+
+				if (iErr != 0) {
+					iRet = -EIO;
+					goto Exit;
+				}
+				// return actual size (LocalObject.m_uiSize)
+				iErr = put_user(LocalObject.m_uiSize,
+						(unsigned int *)(ulArg_p +
+								 (unsigned long)
+								 &LocalObject.
+								 m_uiSize -
+								 (unsigned long)
+								 &LocalObject));
+				if (iErr != 0) {
+					iRet = -EIO;
+					goto Exit;
+				}
+
+			} else {
+				vfree(pData);
+			}
+
+			iRet = (int)EplRet;
+			break;
+		}
+
+		// ----------------------------------------------------------
+	case EPLLIN_CMD_WRITE_LOCAL_OBJECT:
+		{
+			tEplLinLocalObject LocalObject;
+			void *pData;
+
+			iErr =
+			    copy_from_user(&LocalObject, (const void *)ulArg_p,
+					   sizeof(LocalObject));
+			if (iErr != 0) {
+				iRet = -EIO;
+				goto Exit;
+			}
+
+			if ((LocalObject.m_pData == NULL)
+			    || (LocalObject.m_uiSize == 0)) {
+				iRet = (int)kEplApiInvalidParam;
+				goto Exit;
+			}
+
+			pData = vmalloc(LocalObject.m_uiSize);
+			if (pData == NULL) {	// no memory available
+				iRet = -ENOMEM;
+				goto Exit;
+			}
+			iErr =
+			    copy_from_user(pData, LocalObject.m_pData,
+					   LocalObject.m_uiSize);
+			if (iErr != 0) {
+				iRet = -EIO;
+				goto Exit;
+			}
+
+			EplRet =
+			    EplApiWriteLocalObject(LocalObject.m_uiIndex,
+						   LocalObject.m_uiSubindex,
+						   pData, LocalObject.m_uiSize);
+
+			vfree(pData);
+
+			iRet = (int)EplRet;
+			break;
+		}
+
+	case EPLLIN_CMD_READ_OBJECT:
+		{
+			tEplLinSdoObject SdoObject;
+			void *pData;
+			tEplLinSdoBufHeader *pBufHeader;
+			tEplSdoComConHdl *pSdoComConHdl;
+
+			iErr =
+			    copy_from_user(&SdoObject, (const void *)ulArg_p,
+					   sizeof(SdoObject));
+			if (iErr != 0) {
+				iRet = -EIO;
+				goto Exit;
+			}
+
+			if ((SdoObject.m_le_pData == NULL)
+			    || (SdoObject.m_uiSize == 0)) {
+				iRet = (int)kEplApiInvalidParam;
+				goto Exit;
+			}
+
+			pBufHeader =
+			    (tEplLinSdoBufHeader *)
+			    vmalloc(sizeof(tEplLinSdoBufHeader) +
+				    SdoObject.m_uiSize);
+			if (pBufHeader == NULL) {	// no memory available
+				iRet = -ENOMEM;
+				goto Exit;
+			}
+			// initiate temporary buffer
+			pBufHeader->m_pUserArg = SdoObject.m_pUserArg;	// original user argument pointer
+			pBufHeader->m_pData = SdoObject.m_le_pData;	// original data pointer from app
+			pData = pBufHeader + sizeof(tEplLinSdoBufHeader);
+
+			if (SdoObject.m_fValidSdoComConHdl != FALSE) {
+				pSdoComConHdl = &SdoObject.m_SdoComConHdl;
+			} else {
+				pSdoComConHdl = NULL;
+			}
+
+			EplRet =
+			    EplApiReadObject(pSdoComConHdl,
+					     SdoObject.m_uiNodeId,
+					     SdoObject.m_uiIndex,
+					     SdoObject.m_uiSubindex, pData,
+					     &SdoObject.m_uiSize,
+					     SdoObject.m_SdoType, pBufHeader);
+
+			// return actual SDO handle (SdoObject.m_SdoComConHdl)
+			iErr = put_user(SdoObject.m_SdoComConHdl,
+					(unsigned int *)(ulArg_p +
+							 (unsigned long)
+							 &SdoObject.
+							 m_SdoComConHdl -
+							 (unsigned long)
+							 &SdoObject));
+			if (iErr != 0) {
+				iRet = -EIO;
+				goto Exit;
+			}
+
+			if (EplRet == kEplSuccessful) {
+				iErr =
+				    copy_to_user(SdoObject.m_le_pData, pData,
+						 SdoObject.m_uiSize);
+
+				vfree(pBufHeader);
+
+				if (iErr != 0) {
+					iRet = -EIO;
+					goto Exit;
+				}
+				// return actual size (SdoObject.m_uiSize)
+				iErr = put_user(SdoObject.m_uiSize,
+						(unsigned int *)(ulArg_p +
+								 (unsigned long)
+								 &SdoObject.
+								 m_uiSize -
+								 (unsigned long)
+								 &SdoObject));
+				if (iErr != 0) {
+					iRet = -EIO;
+					goto Exit;
+				}
+			} else if (EplRet != kEplApiTaskDeferred) {	// error ocurred
+				vfree(pBufHeader);
+				if (iErr != 0) {
+					iRet = -EIO;
+					goto Exit;
+				}
+			}
+
+			iRet = (int)EplRet;
+			break;
+		}
+
+	case EPLLIN_CMD_WRITE_OBJECT:
+		{
+			tEplLinSdoObject SdoObject;
+			void *pData;
+			tEplLinSdoBufHeader *pBufHeader;
+			tEplSdoComConHdl *pSdoComConHdl;
+
+			iErr =
+			    copy_from_user(&SdoObject, (const void *)ulArg_p,
+					   sizeof(SdoObject));
+			if (iErr != 0) {
+				iRet = -EIO;
+				goto Exit;
+			}
+
+			if ((SdoObject.m_le_pData == NULL)
+			    || (SdoObject.m_uiSize == 0)) {
+				iRet = (int)kEplApiInvalidParam;
+				goto Exit;
+			}
+
+			pBufHeader =
+			    (tEplLinSdoBufHeader *)
+			    vmalloc(sizeof(tEplLinSdoBufHeader) +
+				    SdoObject.m_uiSize);
+			if (pBufHeader == NULL) {	// no memory available
+				iRet = -ENOMEM;
+				goto Exit;
+			}
+			// initiate temporary buffer
+			pBufHeader->m_pUserArg = SdoObject.m_pUserArg;	// original user argument pointer
+			pBufHeader->m_pData = SdoObject.m_le_pData;	// original data pointer from app
+			pData = pBufHeader + sizeof(tEplLinSdoBufHeader);
+
+			iErr =
+			    copy_from_user(pData, SdoObject.m_le_pData,
+					   SdoObject.m_uiSize);
+
+			if (iErr != 0) {
+				iRet = -EIO;
+				goto Exit;
+			}
+
+			if (SdoObject.m_fValidSdoComConHdl != FALSE) {
+				pSdoComConHdl = &SdoObject.m_SdoComConHdl;
+			} else {
+				pSdoComConHdl = NULL;
+			}
+
+			EplRet =
+			    EplApiWriteObject(pSdoComConHdl,
+					      SdoObject.m_uiNodeId,
+					      SdoObject.m_uiIndex,
+					      SdoObject.m_uiSubindex, pData,
+					      SdoObject.m_uiSize,
+					      SdoObject.m_SdoType, pBufHeader);
+
+			// return actual SDO handle (SdoObject.m_SdoComConHdl)
+			iErr = put_user(SdoObject.m_SdoComConHdl,
+					(unsigned int *)(ulArg_p +
+							 (unsigned long)
+							 &SdoObject.
+							 m_SdoComConHdl -
+							 (unsigned long)
+							 &SdoObject));
+			if (iErr != 0) {
+				iRet = -EIO;
+				goto Exit;
+			}
+
+			if (EplRet != kEplApiTaskDeferred) {	// succeeded or error ocurred, but task not deferred
+				vfree(pBufHeader);
+			}
+
+			iRet = (int)EplRet;
+			break;
+		}
+
+		// ----------------------------------------------------------
+	case EPLLIN_CMD_FREE_SDO_CHANNEL:
+		{
+			// forward SDO handle to EPL stack
+			EplRet =
+			    EplApiFreeSdoChannel((tEplSdoComConHdl) ulArg_p);
+
+			iRet = (int)EplRet;
+			break;
+		}
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+		// ----------------------------------------------------------
+	case EPLLIN_CMD_MN_TRIGGER_STATE_CHANGE:
+		{
+			tEplLinNodeCmdObject NodeCmdObject;
+
+			iErr =
+			    copy_from_user(&NodeCmdObject,
+					   (const void *)ulArg_p,
+					   sizeof(NodeCmdObject));
+			if (iErr != 0) {
+				iRet = -EIO;
+				goto Exit;
+			}
+
+			EplRet =
+			    EplApiMnTriggerStateChange(NodeCmdObject.m_uiNodeId,
+						       NodeCmdObject.
+						       m_NodeCommand);
+			iRet = (int)EplRet;
+			break;
+		}
+#endif
+
+		// ----------------------------------------------------------
+	case EPLLIN_CMD_GET_EVENT:
+		{
+			tEplLinEvent Event;
+
+			// save event structure
+			iErr =
+			    copy_from_user(&Event, (const void *)ulArg_p,
+					   sizeof(Event));
+			if (iErr != 0) {
+				iRet = -EIO;
+				goto Exit;
+			}
+			// save return code from application's event callback function
+			RetCbEvent_g = Event.m_RetCbEvent;
+
+			if (RetCbEvent_g == kEplShutdown) {
+				// pass control to event queue kernel thread, but signal termination
+				atomic_set(&AtomicEventState_g,
+					   EVENT_STATE_TERM);
+				wake_up_interruptible(&WaitQueueCbEvent_g);
+				// exit with error -> EplApiProcess() will leave the infinite loop
+				iRet = 1;
+				goto Exit;
+			}
+			// pass control to event queue kernel thread
+			atomic_set(&AtomicEventState_g, EVENT_STATE_IOCTL);
+			wake_up_interruptible(&WaitQueueCbEvent_g);
+
+			// fall asleep itself in own wait queue
+			iErr = wait_event_interruptible(WaitQueueProcess_g,
+							(atomic_read
+							 (&AtomicEventState_g)
+							 == EVENT_STATE_READY)
+							||
+							(atomic_read
+							 (&AtomicEventState_g)
+							 == EVENT_STATE_TERM));
+			if (iErr != 0) {	// waiting was interrupted by signal
+				// pass control to event queue kernel thread, but signal termination
+				atomic_set(&AtomicEventState_g,
+					   EVENT_STATE_TERM);
+				wake_up_interruptible(&WaitQueueCbEvent_g);
+				// exit with this error -> EplApiProcess() will leave the infinite loop
+				iRet = iErr;
+				goto Exit;
+			} else if (atomic_read(&AtomicEventState_g) == EVENT_STATE_TERM) {	// termination in progress
+				// pass control to event queue kernel thread, but signal termination
+				wake_up_interruptible(&WaitQueueCbEvent_g);
+				// exit with this error -> EplApiProcess() will leave the infinite loop
+				iRet = 1;
+				goto Exit;
+			}
+			// copy event to user space
+			iErr =
+			    copy_to_user(Event.m_pEventType, &EventType_g,
+					 sizeof(EventType_g));
+			if (iErr != 0) {	// not all data could be copied
+				iRet = -EIO;
+				goto Exit;
+			}
+			// $$$ d.k. perform SDO event processing
+			if (EventType_g == kEplApiEventSdo) {
+				void *pData;
+				tEplLinSdoBufHeader *pBufHeader;
+
+				pBufHeader =
+				    (tEplLinSdoBufHeader *) pEventArg_g->m_Sdo.
+				    m_pUserArg;
+				pData =
+				    pBufHeader + sizeof(tEplLinSdoBufHeader);
+
+				if (pEventArg_g->m_Sdo.m_SdoAccessType ==
+				    kEplSdoAccessTypeRead) {
+					// copy read data to user space
+					iErr =
+					    copy_to_user(pBufHeader->m_pData,
+							 pData,
+							 pEventArg_g->m_Sdo.
+							 m_uiTransferredByte);
+					if (iErr != 0) {	// not all data could be copied
+						iRet = -EIO;
+						goto Exit;
+					}
+				}
+				pEventArg_g->m_Sdo.m_pUserArg =
+				    pBufHeader->m_pUserArg;
+				vfree(pBufHeader);
+			}
+
+			iErr =
+			    copy_to_user(Event.m_pEventArg, pEventArg_g,
+					 min(sizeof(tEplApiEventArg),
+					     Event.m_uiEventArgSize));
+			if (iErr != 0) {	// not all data could be copied
+				iRet = -EIO;
+				goto Exit;
+			}
+			// return to EplApiProcess(), which will call the application's event callback function
+			iRet = 0;
+
+			break;
+		}
+
+		// ----------------------------------------------------------
+	case EPLLIN_CMD_PI_SETUP:
+		{
+			EplRet = EplApiProcessImageSetup();
+			iRet = (int)EplRet;
+
+			break;
+		}
+
+		// ----------------------------------------------------------
+	case EPLLIN_CMD_PI_IN:
+		{
+			tEplApiProcessImage ProcessImageIn;
+
+			// save process image structure
+			iErr =
+			    copy_from_user(&ProcessImageIn,
+					   (const void *)ulArg_p,
+					   sizeof(ProcessImageIn));
+			if (iErr != 0) {
+				iRet = -EIO;
+				goto Exit;
+			}
+			// pass control to event queue kernel thread
+			atomic_set(&AtomicSyncState_g, EVENT_STATE_IOCTL);
+
+			// fall asleep itself in own wait queue
+			iErr = wait_event_interruptible(WaitQueuePI_In_g,
+							(atomic_read
+							 (&AtomicSyncState_g) ==
+							 EVENT_STATE_READY)
+							||
+							(atomic_read
+							 (&AtomicSyncState_g) ==
+							 EVENT_STATE_TERM));
+			if (iErr != 0) {	// waiting was interrupted by signal
+				// pass control to sync kernel thread, but signal termination
+				atomic_set(&AtomicSyncState_g,
+					   EVENT_STATE_TERM);
+				wake_up_interruptible(&WaitQueueCbSync_g);
+				// exit with this error -> application will leave the infinite loop
+				iRet = iErr;
+				goto Exit;
+			} else if (atomic_read(&AtomicSyncState_g) == EVENT_STATE_TERM) {	// termination in progress
+				// pass control to sync kernel thread, but signal termination
+				wake_up_interruptible(&WaitQueueCbSync_g);
+				// exit with this error -> application will leave the infinite loop
+				iRet = 1;
+				goto Exit;
+			}
+			// exchange process image
+			EplRet = EplApiProcessImageExchangeIn(&ProcessImageIn);
+
+			// return to EplApiProcessImageExchangeIn()
+			iRet = (int)EplRet;
+
+			break;
+		}
+
+		// ----------------------------------------------------------
+	case EPLLIN_CMD_PI_OUT:
+		{
+			tEplApiProcessImage ProcessImageOut;
+
+			// save process image structure
+			iErr =
+			    copy_from_user(&ProcessImageOut,
+					   (const void *)ulArg_p,
+					   sizeof(ProcessImageOut));
+			if (iErr != 0) {
+				iRet = -EIO;
+				goto Exit;
+			}
+
+			if (atomic_read(&AtomicSyncState_g) !=
+			    EVENT_STATE_READY) {
+				iRet = (int)kEplInvalidOperation;
+				goto Exit;
+			}
+			// exchange process image
+			EplRet =
+			    EplApiProcessImageExchangeOut(&ProcessImageOut);
+
+			// pass control to sync kernel thread
+			atomic_set(&AtomicSyncState_g, EVENT_STATE_TERM);
+			wake_up_interruptible(&WaitQueueCbSync_g);
+
+			// return to EplApiProcessImageExchangeout()
+			iRet = (int)EplRet;
+
+			break;
+		}
+
+		// ----------------------------------------------------------
+	case EPLLIN_CMD_NMT_COMMAND:
+		{
+			// forward NMT command to EPL stack
+			EplRet = EplApiExecNmtCommand((tEplNmtEvent) ulArg_p);
+
+			iRet = (int)EplRet;
+
+			break;
+		}
+
+		// ----------------------------------------------------------
+	default:
+		{
+			break;
+		}
+	}
+
+      Exit:
+
+//    TRACE1("EPL: - EplLinIoctl (iRet=%d)\n", iRet);
+	return (iRet);
+
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+tEplKernel PUBLIC EplLinCbEvent(tEplApiEventType EventType_p,	// IN: event type (enum)
+				tEplApiEventArg * pEventArg_p,	// IN: event argument (union)
+				void GENERIC * pUserArg_p)
+{
+	tEplKernel EplRet = kEplSuccessful;
+	int iErr;
+
+	// block any further call to this function, i.e. enter critical section
+	iErr = down_interruptible(&SemaphoreCbEvent_g);
+	if (iErr != 0) {	// waiting was interrupted by signal
+		EplRet = kEplShutdown;
+		goto Exit;
+	}
+	// wait for EplApiProcess() to call ioctl
+	// normally it should be waiting already for us to pass a new event
+	iErr = wait_event_interruptible(WaitQueueCbEvent_g,
+					(atomic_read(&AtomicEventState_g) ==
+					 EVENT_STATE_IOCTL)
+					|| (atomic_read(&AtomicEventState_g) ==
+					    EVENT_STATE_TERM));
+	if ((iErr != 0) || (atomic_read(&AtomicEventState_g) == EVENT_STATE_TERM)) {	// waiting was interrupted by signal
+		EplRet = kEplShutdown;
+		goto LeaveCriticalSection;
+	}
+	// save event information for ioctl
+	EventType_g = EventType_p;
+	pEventArg_g = pEventArg_p;
+
+	// pass control to application's event callback function, i.e. EplApiProcess()
+	atomic_set(&AtomicEventState_g, EVENT_STATE_READY);
+	wake_up_interruptible(&WaitQueueProcess_g);
+
+	// now, the application's event callback function processes the event
+
+	// wait for completion of application's event callback function, i.e. EplApiProcess() calls ioctl again
+	iErr = wait_event_interruptible(WaitQueueCbEvent_g,
+					(atomic_read(&AtomicEventState_g) ==
+					 EVENT_STATE_IOCTL)
+					|| (atomic_read(&AtomicEventState_g) ==
+					    EVENT_STATE_TERM));
+	if ((iErr != 0) || (atomic_read(&AtomicEventState_g) == EVENT_STATE_TERM)) {	// waiting was interrupted by signal
+		EplRet = kEplShutdown;
+		goto LeaveCriticalSection;
+	}
+	// read return code from application's event callback function
+	EplRet = RetCbEvent_g;
+
+      LeaveCriticalSection:
+	up(&SemaphoreCbEvent_g);
+
+      Exit:
+	// check if NMT_GS_OFF is reached
+	if (EventType_p == kEplApiEventNmtStateChange) {
+		if (pEventArg_p->m_NmtStateChange.m_NewNmtState == kEplNmtGsOff) {	// NMT state machine was shut down
+			TRACE0("EPL:   EplLinCbEvent(NMT_GS_OFF)\n");
+			uiEplState_g = EPL_STATE_SHUTDOWN;
+			atomic_set(&AtomicEventState_g, EVENT_STATE_TERM);
+			wake_up(&WaitQueueRelease_g);
+		} else {	// NMT state machine is running
+			uiEplState_g = EPL_STATE_RUNNING;
+		}
+	}
+
+	return EplRet;
+}
+
+tEplKernel PUBLIC EplLinCbSync(void)
+{
+	tEplKernel EplRet = kEplSuccessful;
+	int iErr;
+
+	// check if user process waits for sync
+	if (atomic_read(&AtomicSyncState_g) == EVENT_STATE_IOCTL) {
+		// pass control to application, i.e. EplApiProcessImageExchangeIn()
+		atomic_set(&AtomicSyncState_g, EVENT_STATE_READY);
+		wake_up_interruptible(&WaitQueuePI_In_g);
+
+		// now, the application processes the sync event
+
+		// wait for call of EplApiProcessImageExchangeOut()
+		iErr = wait_event_interruptible(WaitQueueCbSync_g,
+						(atomic_read(&AtomicSyncState_g)
+						 == EVENT_STATE_IOCTL)
+						||
+						(atomic_read(&AtomicSyncState_g)
+						 == EVENT_STATE_TERM));
+		if ((iErr != 0) || (atomic_read(&AtomicEventState_g) == EVENT_STATE_IOCTL)) {	// waiting was interrupted by signal or application called wrong function
+			EplRet = kEplShutdown;
+		}
+	} else {		// application is currently not waiting for sync
+		// continue without interruption
+		// TPDO are set valid by caller (i.e. EplEventkProcess())
+	}
+
+	TGT_DBG_SIGNAL_TRACE_POINT(1);
+
+	return EplRet;
+}
+
+// EOF
diff --git a/drivers/staging/epl/EplApiProcessImage.c b/drivers/staging/epl/EplApiProcessImage.c
new file mode 100644
index 00000000000..2b2fdf229a7
--- /dev/null
+++ b/drivers/staging/epl/EplApiProcessImage.c
@@ -0,0 +1,347 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for EPL API module (process image)
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplApiProcessImage.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.7 $  $Date: 2008/11/13 17:13:09 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/10/10 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#include "Epl.h"
+//#include "kernel/EplPdokCal.h"
+
+#if (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
+#include <asm/uaccess.h>
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  EplApi                                              */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description:
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   D E F I N I T I O N S                          //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+#if ((EPL_API_PROCESS_IMAGE_SIZE_IN > 0) || (EPL_API_PROCESS_IMAGE_SIZE_OUT > 0))
+typedef struct {
+#if EPL_API_PROCESS_IMAGE_SIZE_IN > 0
+	BYTE m_abProcessImageInput[EPL_API_PROCESS_IMAGE_SIZE_IN];
+#endif
+#if EPL_API_PROCESS_IMAGE_SIZE_OUT > 0
+	BYTE m_abProcessImageOutput[EPL_API_PROCESS_IMAGE_SIZE_OUT];
+#endif
+
+} tEplApiProcessImageInstance;
+
+//---------------------------------------------------------------------------
+// local vars
+//---------------------------------------------------------------------------
+
+static tEplApiProcessImageInstance EplApiProcessImageInstance_g;
+#endif
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplApiProcessImageSetup()
+//
+// Description: sets up static process image
+//
+// Parameters:  (none)
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplApiProcessImageSetup(void)
+{
+	tEplKernel Ret = kEplSuccessful;
+#if ((EPL_API_PROCESS_IMAGE_SIZE_IN > 0) || (EPL_API_PROCESS_IMAGE_SIZE_OUT > 0))
+	unsigned int uiVarEntries;
+	tEplObdSize ObdSize;
+#endif
+
+#if EPL_API_PROCESS_IMAGE_SIZE_IN > 0
+	uiVarEntries = EPL_API_PROCESS_IMAGE_SIZE_IN;
+	ObdSize = 1;
+	Ret = EplApiLinkObject(0x2000,
+			       EplApiProcessImageInstance_g.
+			       m_abProcessImageInput, &uiVarEntries, &ObdSize,
+			       1);
+
+	uiVarEntries = EPL_API_PROCESS_IMAGE_SIZE_IN;
+	ObdSize = 1;
+	Ret = EplApiLinkObject(0x2001,
+			       EplApiProcessImageInstance_g.
+			       m_abProcessImageInput, &uiVarEntries, &ObdSize,
+			       1);
+
+	ObdSize = 2;
+	uiVarEntries = EPL_API_PROCESS_IMAGE_SIZE_IN / ObdSize;
+	Ret = EplApiLinkObject(0x2010,
+			       EplApiProcessImageInstance_g.
+			       m_abProcessImageInput, &uiVarEntries, &ObdSize,
+			       1);
+
+	ObdSize = 2;
+	uiVarEntries = EPL_API_PROCESS_IMAGE_SIZE_IN / ObdSize;
+	Ret = EplApiLinkObject(0x2011,
+			       EplApiProcessImageInstance_g.
+			       m_abProcessImageInput, &uiVarEntries, &ObdSize,
+			       1);
+
+	ObdSize = 4;
+	uiVarEntries = EPL_API_PROCESS_IMAGE_SIZE_IN / ObdSize;
+	Ret = EplApiLinkObject(0x2020,
+			       EplApiProcessImageInstance_g.
+			       m_abProcessImageInput, &uiVarEntries, &ObdSize,
+			       1);
+
+	ObdSize = 4;
+	uiVarEntries = EPL_API_PROCESS_IMAGE_SIZE_IN / ObdSize;
+	Ret = EplApiLinkObject(0x2021,
+			       EplApiProcessImageInstance_g.
+			       m_abProcessImageInput, &uiVarEntries, &ObdSize,
+			       1);
+#endif
+
+#if EPL_API_PROCESS_IMAGE_SIZE_OUT > 0
+	uiVarEntries = EPL_API_PROCESS_IMAGE_SIZE_OUT;
+	ObdSize = 1;
+	Ret = EplApiLinkObject(0x2030,
+			       EplApiProcessImageInstance_g.
+			       m_abProcessImageOutput, &uiVarEntries, &ObdSize,
+			       1);
+
+	uiVarEntries = EPL_API_PROCESS_IMAGE_SIZE_OUT;
+	ObdSize = 1;
+	Ret = EplApiLinkObject(0x2031,
+			       EplApiProcessImageInstance_g.
+			       m_abProcessImageOutput, &uiVarEntries, &ObdSize,
+			       1);
+
+	ObdSize = 2;
+	uiVarEntries = EPL_API_PROCESS_IMAGE_SIZE_OUT / ObdSize;
+	Ret = EplApiLinkObject(0x2040,
+			       EplApiProcessImageInstance_g.
+			       m_abProcessImageOutput, &uiVarEntries, &ObdSize,
+			       1);
+
+	ObdSize = 2;
+	uiVarEntries = EPL_API_PROCESS_IMAGE_SIZE_OUT / ObdSize;
+	Ret = EplApiLinkObject(0x2041,
+			       EplApiProcessImageInstance_g.
+			       m_abProcessImageOutput, &uiVarEntries, &ObdSize,
+			       1);
+
+	ObdSize = 4;
+	uiVarEntries = EPL_API_PROCESS_IMAGE_SIZE_OUT / ObdSize;
+	Ret = EplApiLinkObject(0x2050,
+			       EplApiProcessImageInstance_g.
+			       m_abProcessImageOutput, &uiVarEntries, &ObdSize,
+			       1);
+
+	ObdSize = 4;
+	uiVarEntries = EPL_API_PROCESS_IMAGE_SIZE_OUT / ObdSize;
+	Ret = EplApiLinkObject(0x2051,
+			       EplApiProcessImageInstance_g.
+			       m_abProcessImageOutput, &uiVarEntries, &ObdSize,
+			       1);
+#endif
+
+	return Ret;
+}
+
+//----------------------------------------------------------------------------
+// Function:    EplApiProcessImageExchangeIn()
+//
+// Description: replaces passed input process image with the one of EPL stack
+//
+// Parameters:  pPI_p                   = input process image
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//----------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplApiProcessImageExchangeIn(tEplApiProcessImage * pPI_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+#if EPL_API_PROCESS_IMAGE_SIZE_IN > 0
+#if (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
+	copy_to_user(pPI_p->m_pImage,
+		     EplApiProcessImageInstance_g.m_abProcessImageInput,
+		     min(pPI_p->m_uiSize,
+			 sizeof(EplApiProcessImageInstance_g.
+				m_abProcessImageInput)));
+#else
+	EPL_MEMCPY(pPI_p->m_pImage,
+		   EplApiProcessImageInstance_g.m_abProcessImageInput,
+		   min(pPI_p->m_uiSize,
+		       sizeof(EplApiProcessImageInstance_g.
+			      m_abProcessImageInput)));
+#endif
+#endif
+
+	return Ret;
+}
+
+//----------------------------------------------------------------------------
+// Function:    EplApiProcessImageExchangeOut()
+//
+// Description: copies passed output process image to EPL stack.
+//
+// Parameters:  pPI_p                   = output process image
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//----------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplApiProcessImageExchangeOut(tEplApiProcessImage * pPI_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+#if EPL_API_PROCESS_IMAGE_SIZE_OUT > 0
+#if (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
+	copy_from_user(EplApiProcessImageInstance_g.m_abProcessImageOutput,
+		       pPI_p->m_pImage,
+		       min(pPI_p->m_uiSize,
+			   sizeof(EplApiProcessImageInstance_g.
+				  m_abProcessImageOutput)));
+#else
+	EPL_MEMCPY(EplApiProcessImageInstance_g.m_abProcessImageOutput,
+		   pPI_p->m_pImage,
+		   min(pPI_p->m_uiSize,
+		       sizeof(EplApiProcessImageInstance_g.
+			      m_abProcessImageOutput)));
+#endif
+#endif
+
+	return Ret;
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+// EOF
diff --git a/drivers/staging/epl/EplCfg.h b/drivers/staging/epl/EplCfg.h
new file mode 100644
index 00000000000..38e958a042a
--- /dev/null
+++ b/drivers/staging/epl/EplCfg.h
@@ -0,0 +1,196 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  configuration file
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplCfg.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    ...
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/06    k.t.: Start of Implementation
+
+****************************************************************************/
+
+#ifndef _EPLCFG_H_
+#define _EPLCFG_H_
+
+// =========================================================================
+// generic defines which for whole EPL Stack
+// =========================================================================
+#define EPL_USE_DELETEINST_FUNC TRUE
+
+// needed to support datatypes over 32 bit by global.h
+#define USE_VAR64
+
+// EPL_MAX_INSTANCES specifies count of instances of all EPL modules.
+// If it is greater than 1 the first parameter of all
+// functions is the instance number.
+#define EPL_MAX_INSTANCES               1
+
+// This defines the target hardware. Here is encoded wich CPU and wich external
+// peripherals are connected. For possible values refere to target.h. If
+// necessary value is not available EPL stack has to
+// be adapted and tested.
+#define TARGET_HARDWARE                 TGTHW_PC_WRAPP
+
+// use no FIFOs, make direct calls
+//#define EPL_NO_FIFO
+
+// use no IPC between user- and kernelspace modules, make direct calls
+#define EPL_NO_USER_KERNEL
+
+#ifndef BENCHMARK_MODULES
+#define BENCHMARK_MODULES       0	//0xEE800042L
+#endif
+
+// Default defug level:
+// Only debug traces of these modules will be compiled which flags are set in define DEF_DEBUG_LVL.
+#ifndef DEF_DEBUG_LVL
+#define DEF_DEBUG_LVL           0xEC000000L
+#endif
+//   EPL_DBGLVL_OBD         =   0x00000004L
+// * EPL_DBGLVL_ASSERT      =   0x20000000L
+// * EPL_DBGLVL_ERROR       =   0x40000000L
+// * EPL_DBGLVL_ALWAYS      =   0x80000000L
+
+// EPL_MODULE_INTEGRATION defines all modules which are included in
+// EPL application. Please add or delete modules for your application.
+#define EPL_MODULE_INTEGRATION EPL_MODULE_OBDK \
+                               | EPL_MODULE_PDOK \
+                               | EPL_MODULE_NMT_MN \
+                               | EPL_MODULE_SDOS \
+                               | EPL_MODULE_SDOC \
+                               | EPL_MODULE_SDO_ASND \
+                               | EPL_MODULE_SDO_UDP \
+                               | EPL_MODULE_NMT_CN \
+                               | EPL_MODULE_NMTU \
+                               | EPL_MODULE_NMTK \
+                               | EPL_MODULE_DLLK \
+                               | EPL_MODULE_DLLU \
+                               | EPL_MODULE_VETH
+//                               | EPL_MODULE_OBDU
+
+// =========================================================================
+// EPL ethernet driver (Edrv) specific defines
+// =========================================================================
+
+// switch this define to TRUE if Edrv supports fast tx frames
+#define EDRV_FAST_TXFRAMES              FALSE
+//#define EDRV_FAST_TXFRAMES              TRUE
+
+// switch this define to TRUE if Edrv supports early receive interrupts
+#define EDRV_EARLY_RX_INT               FALSE
+//#define EDRV_EARLY_RX_INT               TRUE
+
+// enables setting of several port pins for benchmarking purposes
+#define EDRV_BENCHMARK                  FALSE
+//#define EDRV_BENCHMARK                  TRUE // MCF_GPIO_PODR_PCIBR
+
+// Call Tx handler (i.e. EplDllCbFrameTransmitted()) already if DMA has finished,
+// otherwise call the Tx handler if frame was actually transmitted over ethernet.
+#define EDRV_DMA_TX_HANDLER             FALSE
+//#define EDRV_DMA_TX_HANDLER             TRUE
+
+// number of used ethernet controller
+//#define EDRV_USED_ETH_CTRL              1
+
+// =========================================================================
+// Data Link Layer (DLL) specific defines
+// =========================================================================
+
+// switch this define to TRUE if Edrv supports fast tx frames
+// and DLL shall pass PRes as ready to Edrv after SoC
+#define EPL_DLL_PRES_READY_AFTER_SOC    FALSE
+//#define EPL_DLL_PRES_READY_AFTER_SOC    TRUE
+
+// switch this define to TRUE if Edrv supports fast tx frames
+// and DLL shall pass PRes as ready to Edrv after SoA
+#define EPL_DLL_PRES_READY_AFTER_SOA    FALSE
+//#define EPL_DLL_PRES_READY_AFTER_SOA    TRUE
+
+// =========================================================================
+// OBD specific defines
+// =========================================================================
+
+// switch this define to TRUE if Epl should compare object range
+// automaticly
+#define EPL_OBD_CHECK_OBJECT_RANGE          FALSE
+//#define EPL_OBD_CHECK_OBJECT_RANGE          TRUE
+
+// set this define to TRUE if there are strings or domains in OD, which
+// may be changed in object size and/or object data pointer by its object
+// callback function (called event kObdEvWrStringDomain)
+//#define EPL_OBD_USE_STRING_DOMAIN_IN_RAM    FALSE
+#define EPL_OBD_USE_STRING_DOMAIN_IN_RAM    TRUE
+
+#define EPL_OBD_USE_VARIABLE_SUBINDEX_TAB TRUE
+
+// =========================================================================
+// Timer module specific defines
+// =========================================================================
+
+// if TRUE it uses the Timer module implementation of EPL user also in EPL kernel
+#define EPL_TIMER_USE_USER              TRUE
+
+// if TRUE the high resolution timer module will be used
+#define EPL_TIMER_USE_HIGHRES              TRUE
+//#define EPL_TIMER_USE_HIGHRES              FALSE
+
+#endif //_EPLCFG_H_
diff --git a/drivers/staging/epl/EplDef.h b/drivers/staging/epl/EplDef.h
new file mode 100644
index 00000000000..1dc8108449c
--- /dev/null
+++ b/drivers/staging/epl/EplDef.h
@@ -0,0 +1,355 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for EPL default constants
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplDef.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.15 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/05/22 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_DEF_H_
+#define _EPL_DEF_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#define EPL_C_ADR_BROADCAST         0xFF	// EPL broadcast address
+#define EPL_C_ADR_DIAG_DEF_NODE_ID  0xFD	// EPL default address of dignostic device
+#define EPL_C_ADR_DUMMY_NODE_ID     0xFC	// EPL dummy node address
+#define EPL_C_ADR_INVALID           0x00	// invalid EPL address
+#define EPL_C_ADR_MN_DEF_NODE_ID    0xF0	// EPL default address of MN
+#define EPL_C_ADR_RT1_DEF_NODE_ID   0xFE	// EPL default address of router type 1
+#define EPL_C_DLL_ASND_PRIO_NMTRQST 7	// increased ASnd request priority to be used by NMT Requests
+#define EPL_C_DLL_ASND_PRIO_STD     0	// standard ASnd request priority
+#define EPL_C_DLL_ETHERTYPE_EPL     0x88AB
+#define EPL_C_DLL_ISOCHR_MAX_PAYL   1490	// Byte: maximum size of PReq and PRes payload data, requires C_IP_MAX_MTU
+#define EPL_C_DLL_MAX_ASYNC_MTU     1500	// Byte: maximum asynchronous payload in bytes
+#define EPL_C_DLL_MAX_PAYL_OFFSET   1499	// Byte: maximum offset of Ethernet frame payload, requires C_IP_MAX_MTU
+#define EPL_C_DLL_MAX_RS            7
+#define EPL_C_DLL_MIN_ASYNC_MTU     282	// Byte: minimum asynchronous payload in bytes.
+#define EPL_C_DLL_MIN_PAYL_OFFSET   45	// Byte: minimum offset of Ethernet frame payload
+#define EPL_C_DLL_MULTICAST_ASND    0x01111E000004LL	// EPL ASnd multicast MAC address, canonical form
+#define EPL_C_DLL_MULTICAST_PRES    0x01111E000002LL	// EPL PRes multicast MAC address, canonical form
+#define EPL_C_DLL_MULTICAST_SOA     0x01111E000003LL	// EPL SoA multicast MAC address, canonical form
+#define EPL_C_DLL_MULTICAST_SOC     0x01111E000001LL	// EPL Soc multicast MAC address, canonical form
+#define EPL_C_DLL_PREOP1_START_CYCLES 10	// number of unassigning SoA frames at start of NMT_MS_PRE_OPERATIONAL_1
+#define EPL_C_DLL_T_BITTIME         10	// ns: Transmission time per bit on 100 Mbit/s network
+#define EPL_C_DLL_T_EPL_PDO_HEADER  10	// Byte: size of PReq and PRes EPL PDO message header
+#define EPL_C_DLL_T_ETH2_WRAPPER    18	// Byte: size of Ethernet type II wrapper consisting of header and checksum
+#define EPL_C_DLL_T_IFG             640	// ns: Ethernet Interframe Gap
+#define EPL_C_DLL_T_MIN_FRAME       5120	// ns: Size of minimum Ethernet frame (without preamble)
+#define EPL_C_DLL_T_PREAMBLE        960	// ns: Size of Ethernet frame preamble
+
+#define EPL_C_DLL_MINSIZE_SOC       36	// minimum size of SoC without padding and CRC
+#define EPL_C_DLL_MINSIZE_PREQ      60	// minimum size of PRec without CRC
+#define EPL_C_DLL_MINSIZE_PRES      60	// minimum size of PRes without CRC
+#define EPL_C_DLL_MINSIZE_SOA       24	// minimum size of SoA without padding and CRC
+#define EPL_C_DLL_MINSIZE_IDENTRES  176	// minimum size of IdentResponse without CRC
+#define EPL_C_DLL_MINSIZE_STATUSRES 72	// minimum size of StatusResponse without CRC
+#define EPL_C_DLL_MINSIZE_NMTCMD    20	// minimum size of NmtCommand without CommandData, padding and CRC
+#define EPL_C_DLL_MINSIZE_NMTCMDEXT 52	// minimum size of NmtCommand without padding and CRC
+#define EPL_C_DLL_MINSIZE_NMTREQ    20	// minimum size of NmtRequest without CommandData, padding and CRC
+#define EPL_C_DLL_MINSIZE_NMTREQEXT 52	// minimum size of NmtRequest without padding and CRC
+
+#define EPL_C_ERR_MONITOR_DELAY     10	// Error monitoring start delay (not used in DS 1.0.0)
+#define EPL_C_IP_ADR_INVALID        0x00000000L	// invalid IP address (0.0.0.0) used to indicate no change
+#define EPL_C_IP_INVALID_MTU        0	// Byte: invalid MTU size used to indicate no change
+#define EPL_C_IP_MAX_MTU            1518	// Byte: maximum size in bytes of the IP stack which must be processed.
+#define EPL_C_IP_MIN_MTU            300	// Byte: minimum size in bytes of the IP stack which must be processed.
+#define EPL_C_NMT_STATE_TOLERANCE   5	// Cycles: maximum reaction time to NMT state commands
+#define EPL_C_NMT_STATREQ_CYCLE     5	// sec: StatusRequest cycle time to be applied to AsyncOnly CNs
+#define EPL_C_SDO_EPL_PORT          3819
+
+#define EPL_C_DLL_MAX_ASND_SERVICE_IDS  5	// see tEplDllAsndServiceId in EplDll.h
+
+// Default configuration
+// ======================
+
+#ifndef EPL_D_PDO_Granularity_U8
+#define EPL_D_PDO_Granularity_U8    8	// minimum size of objects to be mapped in bits UNSIGNED8 O O 1 1
+#endif
+
+#ifndef EPL_NMT_MAX_NODE_ID
+#define EPL_NMT_MAX_NODE_ID         254	// maximum node-ID
+#endif
+
+#ifndef EPL_D_NMT_MaxCNNumber_U8
+#define EPL_D_NMT_MaxCNNumber_U8    239	// maximum number of supported regular CNs in the Node ID range 1 .. 239 UNSIGNED8 O O 239 239
+#endif
+
+// defines for EPL API layer static process image
+#ifndef EPL_API_PROCESS_IMAGE_SIZE_IN
+#define EPL_API_PROCESS_IMAGE_SIZE_IN   0
+#endif
+
+#ifndef EPL_API_PROCESS_IMAGE_SIZE_OUT
+#define EPL_API_PROCESS_IMAGE_SIZE_OUT  0
+#endif
+
+// configure whether OD access events shall be forwarded
+// to user callback function.
+// Because of reentrancy for local OD accesses, this has to be disabled
+// when application resides in other address space as the stack (e.g. if
+// EplApiLinuxUser.c and EplApiLinuxKernel.c are used)
+#ifndef EPL_API_OBD_FORWARD_EVENT
+#define EPL_API_OBD_FORWARD_EVENT       TRUE
+#endif
+
+#ifndef EPL_OBD_MAX_STRING_SIZE
+#define EPL_OBD_MAX_STRING_SIZE        32	// is used for objects 0x1008/0x1009/0x100A
+#endif
+
+#ifndef EPL_OBD_USE_STORE_RESTORE
+#define EPL_OBD_USE_STORE_RESTORE       FALSE
+#endif
+
+#ifndef EPL_OBD_CHECK_OBJECT_RANGE
+#define EPL_OBD_CHECK_OBJECT_RANGE      TRUE
+#endif
+
+#ifndef EPL_OBD_USE_STRING_DOMAIN_IN_RAM
+#define EPL_OBD_USE_STRING_DOMAIN_IN_RAM    TRUE
+#endif
+
+#ifndef EPL_OBD_USE_VARIABLE_SUBINDEX_TAB
+#define EPL_OBD_USE_VARIABLE_SUBINDEX_TAB   TRUE
+#endif
+
+#ifndef EPL_OBD_USE_KERNEL
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) == 0)
+#define EPL_OBD_USE_KERNEL                  TRUE
+#else
+#define EPL_OBD_USE_KERNEL                  FALSE
+#endif
+#endif
+
+#ifndef EPL_OBD_INCLUDE_A000_TO_DEVICE_PART
+#define EPL_OBD_INCLUDE_A000_TO_DEVICE_PART FALSE
+#endif
+
+#ifndef EPL_VETH_NAME
+#define EPL_VETH_NAME       "epl"	// name of net device in Linux
+#endif
+
+/*
+#define EPL_D_CFG_ConfigManager_BOOL // Ability of a MN node to perform Configuration Manager functions BOOLEAN O - N -
+#define EPL_D_CFM_VerifyConf_BOOL   // Support of objects CFM_VerifyConfiguration_REC, CFM_ExpConfDateList_AU32, CFM_ExpConfTimeList_AU32 BOOLEAN O O N N
+#define EPL_D_CFM_VerifyConfId_BOOL // Support of objects CFM_VerifyConfiguration_REC.ConfId_U32 and CFM_ExpConfIdList_AU32 BOOLEAN O O N N
+#define EPL_D_DLL_CNFeatureIsochr_BOOL // CN’s ability to perform isochronous functions BOOLEAN - O - Y
+#define EPL_D_DLL_CNFeatureMultiplex_BOOL // node’s ability to perform control of multiplexed isochronous communication BOOLEAN - O - N
+#define EPL_D_DLL_FeatureCN_BOOL // node’s ability to perform CN functions BOOLEAN O O Y Y
+#define EPL_D_DLL_FeatureMN_BOOL // node’s ability to perform MN functions BOOLEAN M O - N
+#define EPL_D_DLL_MNFeatureMultiplex_BOOL // MN’s ability to perform control of multiplexed isochronous communication BOOLEAN O - Y -
+#define EPL_D_DLL_MNFeaturePResTx_BOOL // MN’s ability to transmit PRes BOOLEAN O - Y -
+#define EPL_D_NMT_ASndRxMaxPayload_U16 // size of ASnd frame receive buffer UNSIGNED16 M M - -
+#define EPL_D_NMT_ASndTxMaxPayload_U16 // size of ASnd frame transmit buffer UNSIGNED16 M M - -
+#define EPL_D_NMT_CNASnd2SoC_U32 // minimum delay between end of reception of ASnd and start of reception of SoC UNSIGNED32 - M - -
+#define EPL_D_NMT_CNASndMaxLatency_U32 // delay between end of SoA reception and start of ASnd transmission UNSIGNED32 - M - -
+#define EPL_D_NMT_CNPResMaxLatency_U32 // delay between end of PReq reception and start of PRes transmission UNSIGNED32 - M - -
+#define EPL_D_NMT_CNSoC2PReq_U32 // CN SoC handling maximum time, a subsequent PReq won’t be handled before SoC handling was finished UNSIGNED32 - M - -
+#define EPL_D_NMT_DeviceType_U32 // Device Type ID UNSIGNED32 M M - -
+#define EPL_D_NMT_EPLVers_U8 EPL // Version implemented by the device UNSIGNED8 M M - -
+#define EPL_D_NMT_ExtStateCmd_BOOL // abitilty to support Extended NMT State Commands BOOLEAN O O Y Y
+#define EPL_D_NMT_InfoSvc_BOOL // ability to support NMT Info Services BOOLEAN O - Y -
+#define EPL_D_NMT_InterfaceAddr_Xh_OSTR // Physical Address of Interface No. Xh OCTET_STRING M M - -
+#define EPL_D_NMT_InterfaceDescr_Xh_VSTR // Description text of Interface No. Xh VISIBLE_STRINGM M - -
+#define EPL_D_NMT_InterfaceMtu_Xh_U32 // MTU of Interface No. Xh UNSIGNED32 M M - -
+#define EPL_D_NMT_InterfaceType_Xh_U8 // Type of Interface No. Xh UNSIGNED8 M M - -
+#define EPL_D_NMT_IsochrRxMaxPayload_U16 // size of isochronous frame receive buffer UNSIGNED16 M M - -
+#define EPL_D_NMT_IsochrTxMaxPayload_U16 // size of isochronous frame transmit buffer UNSIGNED16 M M - -
+#define EPL_D_NMT_ManufactDevName_VS // Manufacturer Device Name VISIBLE_STRING O O - -
+#define EPL_D_NMT_ManufactHwVers_VS // Manufacturer HW version VISIBLE_STRING O O - -
+#define EPL_D_NMT_ManufactSwVers_VS // Manufacturer SW version VISIBLE_STRING O O - -
+#define EPL_D_NMT_MaxCNNodeID_U8 // maximum Node ID available for regular CNs the entry provides an upper limit to the NodeID available for cross traffic PDO reception from a regular CN UNSIGNED8 O O 239 239
+#define EPL_D_NMT_MaxCNNumber_U8 // maximum number of supported regular CNs in the Node ID range 1 .. 239 UNSIGNED8 O O 239 239
+#define EPL_D_NMT_MaxHeartbeats_U8 // number of guard channels UNSIGNED8 O O 254 254
+#define EPL_D_NMT_MNASnd2SoC_U32 // minimum delay between end of reception of ASnd and start of transmission of SoC UNSIGNED32 M - - -
+#define EPL_D_NMT_MNMultiplCycMax_U8 // maximum number of EPL cycles per multiplexed cycle UNSIGNED8 O - 0 -
+#define EPL_D_NMT_MNPRes2PReq_U32 // delay between end of PRes reception and start of PReq transmission UNSIGNED32 M - - -
+#define EPL_D_NMT_MNPRes2PRes_U32 // delay between end of reception of PRes from CNn and start of transmission of PRes by MN UNSIGNED32 M - - -
+#define EPL_D_NMT_MNPResRx2SoA_U32 // delay between end of reception of PRes from CNn and start of transmission of SoA by MN UNSIGNED32 M - - -
+#define EPL_D_NMT_MNPResTx2SoA_U32 // delay between end of PRes transmission by MN and start of transmission of SoA by MN UNSIGNED32 M - - -
+#define EPL_D_NMT_MNSoA2ASndTx_U32 // delay between end of transmission of SoA and start of transmission of ASnd by MN UNSIGNED32 M - - -
+#define EPL_D_NMT_MNSoC2PReq_U32 // MN minimum delay between end of SoC transmission and start of PReq transmission UNSIGNED32 M - - -
+#define EPL_D_NMT_NMTSvcViaUDPIP_BOOL // Ability of a node to perform NMT services via UDP/IP BOOLEAN O - Y -
+#define EPL_D_NMT_NodeIDByHW_BOOL // Ability of a node to support NodeID setup by HW BOOLEAN O O Y Y
+#define EPL_D_NMT_NodeIDBySW_BOOL // Ability of a node to support NodeID setup by SW BOOLEAN O O N N
+#define EPL_D_NMT_ProductCode_U32 // Identity Object Product Code UNSIGNED32 M M - -
+#define EPL_D_NMT_RevisionNo_U32 // Identity Object Revision Number UNSIGNED32 M M - -
+#define EPL_D_NMT_SerialNo_U32 // Identity Object Serial Number UNSIGNED32 M M - -
+#define EPL_D_NMT_SimpleBoot_BOOL // Ability of a MN node to perform Simple Boot Process, if not set Indivual Boot Process shall be proviced BOOLEAN M - - -
+#define EPL_D_NMT_VendorID_U32 // Identity Object Vendor ID UNSIGNED32 M M - -
+#define EPL_D_NWL_Forward_BOOL // Ability of node to forward datagrams BOOLEAN O O N N
+#define EPL_D_NWL_IPSupport_BOOL // Ability of the node cummunicate via IP BOOLEAN - - Y Y
+#define EPL_D_PDO_DynamicMapping_BOOL // Ability of a node to perform dynamic PDO mapping BOOLEAN O O Y Y
+#define EPL_D_PDO_MaxDescrMem_U32 // maximum cumulative memory consumption of TPDO and RPDO describing objects in byte UNSIGNED32 O O MAX_U32 MAX_U32
+#define EPL_D_PDO_RPDOChannels_U8 // number of supported RPDO channels UNSIGNED8 O O 256 256
+#define EPL_D_PDO_RPDOMaxMem_U32 // Maximum memory available for RPDO data per EPL cycle in byte UNSIGNED32 O O MAX_U32 MAX_U32
+#define EPL_D_PDO_RPDOObjects_U8 // Number of supported mapped objects per RPDO channel UNSIGNED8 O O 254 254
+#define EPL_D_PDO_TPDOChannels_U8 // number of supported TPDO channels UNSIGNED8 O - 256 -
+#define EPL_D_PDO_TPDOMaxMem_U32 // Maximum memory available for TPDO data per EPL cycle in byte UNSIGNED32 O O MAX_U32 MAX_U32
+#define EPL_D_PDO_TPDOObjects_U8 // Number of supported mapped objects per TPDO channel UNSIGNED8 O O 254 254
+#define EPL_D_SDO_ViaASnd_BOOL // Ability of a CN to perform SDO transfer by EPL ASnd BOOLEAN - M - -
+#define EPL_D_SDO_ViaPDO_BOOL // Ability of a node to perform SDO transfer by PDO BOOLEAN O O N N
+#define EPL_D_SDO_ViaUDPIP_BOOL // Ability of a CN to perform SDO transfer by UDP/IP BOOLEAN - M - -
+#define EPL_D_SYN_OptimizedSync_BOOL // Ability of node to perform optimized synchronisation BOOLEAN O O N N
+*/
+
+// Emergency error codes
+// ======================
+#define EPL_E_NO_ERROR                  0x0000
+// 0xFxxx manufacturer specific error codes
+#define EPL_E_NMT_NO_IDENT_RES          0xF001
+#define EPL_E_NMT_NO_STATUS_RES         0xF002
+
+// 0x816x HW errors
+#define EPL_E_DLL_BAD_PHYS_MODE         0x8161
+#define EPL_E_DLL_COLLISION             0x8162
+#define EPL_E_DLL_COLLISION_TH          0x8163
+#define EPL_E_DLL_CRC_TH                0x8164
+#define EPL_E_DLL_LOSS_OF_LINK          0x8165
+#define EPL_E_DLL_MAC_BUFFER            0x8166
+// 0x82xx Protocol errors
+#define EPL_E_DLL_ADDRESS_CONFLICT      0x8201
+#define EPL_E_DLL_MULTIPLE_MN           0x8202
+// 0x821x Frame size errors
+#define EPL_E_PDO_SHORT_RX              0x8210
+#define EPL_E_PDO_MAP_VERS              0x8211
+#define EPL_E_NMT_ASND_MTU_DIF          0x8212
+#define EPL_E_NMT_ASND_MTU_LIM          0x8213
+#define EPL_E_NMT_ASND_TX_LIM           0x8214
+// 0x823x Timing errors
+#define EPL_E_NMT_CYCLE_LEN             0x8231
+#define EPL_E_DLL_CYCLE_EXCEED          0x8232
+#define EPL_E_DLL_CYCLE_EXCEED_TH       0x8233
+#define EPL_E_NMT_IDLE_LIM              0x8234
+#define EPL_E_DLL_JITTER_TH             0x8235
+#define EPL_E_DLL_LATE_PRES_TH          0x8236
+#define EPL_E_NMT_PREQ_CN               0x8237
+#define EPL_E_NMT_PREQ_LIM              0x8238
+#define EPL_E_NMT_PRES_CN               0x8239
+#define EPL_E_NMT_PRES_RX_LIM           0x823A
+#define EPL_E_NMT_PRES_TX_LIM           0x823B
+// 0x824x Frame errors
+#define EPL_E_DLL_INVALID_FORMAT        0x8241
+#define EPL_E_DLL_LOSS_PREQ_TH          0x8242
+#define EPL_E_DLL_LOSS_PRES_TH          0x8243
+#define EPL_E_DLL_LOSS_SOA_TH           0x8244
+#define EPL_E_DLL_LOSS_SOC_TH           0x8245
+// 0x84xx BootUp Errors
+#define EPL_E_NMT_BA1                   0x8410	// other MN in MsNotActive active
+#define EPL_E_NMT_BA1_NO_MN_SUPPORT     0x8411	// MN is not supported
+#define EPL_E_NMT_BPO1                  0x8420	// mandatory CN was not found or failed in BootStep1
+#define EPL_E_NMT_BPO1_GET_IDENT        0x8421	// IdentRes was not received
+#define EPL_E_NMT_BPO1_DEVICE_TYPE      0x8422	// wrong device type
+#define EPL_E_NMT_BPO1_VENDOR_ID        0x8423	// wrong vendor ID
+#define EPL_E_NMT_BPO1_PRODUCT_CODE     0x8424	// wrong product code
+#define EPL_E_NMT_BPO1_REVISION_NO      0x8425	// wrong revision number
+#define EPL_E_NMT_BPO1_SERIAL_NO        0x8426	// wrong serial number
+#define EPL_E_NMT_BPO1_CF_VERIFY        0x8428	// verification of configuration failed
+#define EPL_E_NMT_BPO2                  0x8430	// mandatory CN failed in BootStep2
+#define EPL_E_NMT_BRO                   0x8440	// CheckCommunication failed for mandatory CN
+#define EPL_E_NMT_WRONG_STATE           0x8480	// mandatory CN has wrong NMT state
+
+// Defines for object 0x1F80 NMT_StartUp_U32
+// ==========================================
+#define EPL_NMTST_STARTALLNODES         0x00000002L	// Bit 1
+#define EPL_NMTST_NO_AUTOSTART          0x00000004L	// Bit 2
+#define EPL_NMTST_NO_STARTNODE          0x00000008L	// Bit 3
+#define EPL_NMTST_RESETALL_MAND_CN      0x00000010L	// Bit 4
+#define EPL_NMTST_STOPALL_MAND_CN       0x00000040L	// Bit 6
+#define EPL_NMTST_NO_AUTOPREOP2         0x00000080L	// Bit 7
+#define EPL_NMTST_NO_AUTOREADYTOOP      0x00000100L	// Bit 8
+#define EPL_NMTST_EXT_CNIDENTCHECK      0x00000200L	// Bit 9
+#define EPL_NMTST_SWVERSIONCHECK        0x00000400L	// Bit 10
+#define EPL_NMTST_CONFCHECK             0x00000800L	// Bit 11
+#define EPL_NMTST_NO_RETURN_PREOP1      0x00001000L	// Bit 12
+#define EPL_NMTST_BASICETHERNET         0x00002000L	// Bit 13
+
+// Defines for object 0x1F81 NMT_NodeAssignment_AU32
+// ==================================================
+#define EPL_NODEASSIGN_NODE_EXISTS      0x00000001L	// Bit 0
+#define EPL_NODEASSIGN_NODE_IS_CN       0x00000002L	// Bit 1
+#define EPL_NODEASSIGN_START_CN         0x00000004L	// Bit 2
+#define EPL_NODEASSIGN_MANDATORY_CN     0x00000008L	// Bit 3
+#define EPL_NODEASSIGN_KEEPALIVE        0x00000010L	//currently not used in EPL V2 standard
+#define EPL_NODEASSIGN_SWVERSIONCHECK   0x00000020L	// Bit 5
+#define EPL_NODEASSIGN_SWUPDATE         0x00000040L	// Bit 6
+#define EPL_NODEASSIGN_ASYNCONLY_NODE   0x00000100L	// Bit 8
+#define EPL_NODEASSIGN_MULTIPLEXED_CN   0x00000200L	// Bit 9
+#define EPL_NODEASSIGN_RT1              0x00000400L	// Bit 10
+#define EPL_NODEASSIGN_RT2              0x00000800L	// Bit 11
+#define EPL_NODEASSIGN_MN_PRES          0x00001000L	// Bit 12
+#define EPL_NODEASSIGN_VALID            0x80000000L	// Bit 31
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#endif // #ifndef _EPL_DEF_H_
diff --git a/drivers/staging/epl/EplDll.h b/drivers/staging/epl/EplDll.h
new file mode 100644
index 00000000000..36657f2daf8
--- /dev/null
+++ b/drivers/staging/epl/EplDll.h
@@ -0,0 +1,205 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for DLL module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplDll.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/08 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_DLL_H_
+#define _EPL_DLL_H_
+
+#include "EplInc.h"
+#include "EplFrame.h"
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#ifndef EPL_DLL_MAX_ASND_SERVICE_ID
+#define EPL_DLL_MAX_ASND_SERVICE_ID (EPL_C_DLL_MAX_ASND_SERVICE_IDS + 1)	// last is kEplDllAsndSdo == 5
+#endif
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+typedef enum {
+	kEplDllAsndNotDefined = 0x00,
+	kEplDllAsndIdentResponse = 0x01,
+	kEplDllAsndStatusResponse = 0x02,
+	kEplDllAsndNmtRequest = 0x03,
+	kEplDllAsndNmtCommand = 0x04,
+	kEplDllAsndSdo = 0x05
+} tEplDllAsndServiceId;
+
+typedef enum {
+	kEplDllAsndFilterNone = 0x00,
+	kEplDllAsndFilterLocal = 0x01,	// receive only ASnd frames with local or broadcast node ID
+	kEplDllAsndFilterAny = 0x02,	// receive any ASnd frame
+} tEplDllAsndFilter;
+
+typedef enum {
+	kEplDllReqServiceNo = 0x00,
+	kEplDllReqServiceIdent = 0x01,
+	kEplDllReqServiceStatus = 0x02,
+	kEplDllReqServiceNmtRequest = 0x03,
+	kEplDllReqServiceUnspecified = 0xFF,
+
+} tEplDllReqServiceId;
+
+typedef enum {
+	kEplDllAsyncReqPrioNmt = 0x07,	// PRIO_NMT_REQUEST
+	kEplDllAsyncReqPrio6 = 0x06,
+	kEplDllAsyncReqPrio5 = 0x05,
+	kEplDllAsyncReqPrio4 = 0x04,
+	kEplDllAsyncReqPrioGeneric = 0x03,	// PRIO_GENERIC_REQUEST
+	kEplDllAsyncReqPrio2 = 0x02,	// till WSP 0.1.3: PRIO_ABOVE_GENERIC
+	kEplDllAsyncReqPrio1 = 0x01,	// till WSP 0.1.3: PRIO_BELOW_GENERIC
+	kEplDllAsyncReqPrio0 = 0x00,	// till WSP 0.1.3: PRIO_GENERIC_REQUEST
+
+} tEplDllAsyncReqPriority;
+
+typedef struct {
+	unsigned int m_uiFrameSize;
+	tEplFrame *m_pFrame;
+	tEplNetTime m_NetTime;
+
+} tEplFrameInfo;
+
+typedef struct {
+	unsigned int m_uiSizeOfStruct;
+	BOOL m_fAsyncOnly;	// do not need to register PRes-Frame
+	unsigned int m_uiNodeId;	// local node ID
+
+	// 0x1F82: NMT_FeatureFlags_U32
+	DWORD m_dwFeatureFlags;
+	// Cycle Length (0x1006: NMT_CycleLen_U32) in [us]
+	DWORD m_dwCycleLen;	// required for error detection
+	// 0x1F98: NMT_CycleTiming_REC
+	// 0x1F98.1: IsochrTxMaxPayload_U16
+	unsigned int m_uiIsochrTxMaxPayload;	// const
+	// 0x1F98.2: IsochrRxMaxPayload_U16
+	unsigned int m_uiIsochrRxMaxPayload;	// const
+	// 0x1F98.3: PResMaxLatency_U32
+	DWORD m_dwPresMaxLatency;	// const in [ns], only required for IdentRes
+	// 0x1F98.4: PReqActPayloadLimit_U16
+	unsigned int m_uiPreqActPayloadLimit;	// required for initialisation (+24 bytes)
+	// 0x1F98.5: PResActPayloadLimit_U16
+	unsigned int m_uiPresActPayloadLimit;	// required for initialisation of Pres frame (+24 bytes)
+	// 0x1F98.6: ASndMaxLatency_U32
+	DWORD m_dwAsndMaxLatency;	// const in [ns], only required for IdentRes
+	// 0x1F98.7: MultiplCycleCnt_U8
+	unsigned int m_uiMultiplCycleCnt;	// required for error detection
+	// 0x1F98.8: AsyncMTU_U16
+	unsigned int m_uiAsyncMtu;	// required to set up max frame size
+	// $$$ 0x1F98.9: Prescaler_U16
+	// $$$ Multiplexed Slot
+
+	// 0x1C14: DLL_LossOfFrameTolerance_U32 in [ns]
+	DWORD m_dwLossOfFrameTolerance;
+
+	// 0x1F8A: NMT_MNCycleTiming_REC
+	// 0x1F8A.1: WaitSoCPReq_U32 in [ns]
+	DWORD m_dwWaitSocPreq;
+
+	// 0x1F8A.2: AsyncSlotTimeout_U32 in [ns]
+	DWORD m_dwAsyncSlotTimeout;
+
+} tEplDllConfigParam;
+
+typedef struct {
+	unsigned int m_uiSizeOfStruct;
+	DWORD m_dwDeviceType;	// NMT_DeviceType_U32
+	DWORD m_dwVendorId;	// NMT_IdentityObject_REC.VendorId_U32
+	DWORD m_dwProductCode;	// NMT_IdentityObject_REC.ProductCode_U32
+	DWORD m_dwRevisionNumber;	// NMT_IdentityObject_REC.RevisionNo_U32
+	DWORD m_dwSerialNumber;	// NMT_IdentityObject_REC.SerialNo_U32
+	QWORD m_qwVendorSpecificExt1;
+	DWORD m_dwVerifyConfigurationDate;	// CFM_VerifyConfiguration_REC.ConfDate_U32
+	DWORD m_dwVerifyConfigurationTime;	// CFM_VerifyConfiguration_REC.ConfTime_U32
+	DWORD m_dwApplicationSwDate;	// PDL_LocVerApplSw_REC.ApplSwDate_U32 on programmable device or date portion of NMT_ManufactSwVers_VS on non-programmable device
+	DWORD m_dwApplicationSwTime;	// PDL_LocVerApplSw_REC.ApplSwTime_U32 on programmable device or time portion of NMT_ManufactSwVers_VS on non-programmable device
+	DWORD m_dwIpAddress;
+	DWORD m_dwSubnetMask;
+	DWORD m_dwDefaultGateway;
+	BYTE m_sHostname[32];
+	BYTE m_abVendorSpecificExt2[48];
+
+} tEplDllIdentParam;
+
+typedef struct {
+	unsigned int m_uiNodeId;
+	WORD m_wPreqPayloadLimit;	// object 0x1F8B: NMT_MNPReqPayloadLimitList_AU16
+	WORD m_wPresPayloadLimit;	// object 0x1F8D: NMT_PResPayloadLimitList_AU16
+	DWORD m_dwPresTimeout;	// object 0x1F92: NMT_MNCNPResTimeout_AU32
+
+} tEplDllNodeInfo;
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#endif // #ifndef _EPL_DLL_H_
diff --git a/drivers/staging/epl/EplDllCal.h b/drivers/staging/epl/EplDllCal.h
new file mode 100644
index 00000000000..24460087e98
--- /dev/null
+++ b/drivers/staging/epl/EplDllCal.h
@@ -0,0 +1,123 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for DLL Communication Abstraction Layer module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplDllCal.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/20 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_DLLCAL_H_
+#define _EPL_DLLCAL_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+/*#ifndef EPL_DLLCAL_BUFFER_ID_RX
+#define EPL_DLLCAL_BUFFER_ID_RX    "EplSblDllCalRx"
+#endif
+
+#ifndef EPL_DLLCAL_BUFFER_SIZE_RX
+#define EPL_DLLCAL_BUFFER_SIZE_RX  32767
+#endif
+*/
+#ifndef EPL_DLLCAL_BUFFER_ID_TX_NMT
+#define EPL_DLLCAL_BUFFER_ID_TX_NMT     "EplSblDllCalTxNmt"
+#endif
+
+#ifndef EPL_DLLCAL_BUFFER_SIZE_TX_NMT
+#define EPL_DLLCAL_BUFFER_SIZE_TX_NMT   32767
+#endif
+
+#ifndef EPL_DLLCAL_BUFFER_ID_TX_GEN
+#define EPL_DLLCAL_BUFFER_ID_TX_GEN     "EplSblDllCalTxGen"
+#endif
+
+#ifndef EPL_DLLCAL_BUFFER_SIZE_TX_GEN
+#define EPL_DLLCAL_BUFFER_SIZE_TX_GEN   32767
+#endif
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+typedef struct {
+	tEplDllAsndServiceId m_ServiceId;
+	tEplDllAsndFilter m_Filter;
+
+} tEplDllCalAsndServiceIdFilter;
+
+typedef struct {
+	tEplDllReqServiceId m_Service;
+	unsigned int m_uiNodeId;
+	BYTE m_bSoaFlag1;
+
+} tEplDllCalIssueRequest;
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#endif // #ifndef _EPL_DLLKCAL_H_
diff --git a/drivers/staging/epl/EplDllk.c b/drivers/staging/epl/EplDllk.c
new file mode 100644
index 00000000000..9e22641055c
--- /dev/null
+++ b/drivers/staging/epl/EplDllk.c
@@ -0,0 +1,4054 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for kernel DLL module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplDllk.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.21 $  $Date: 2008/11/13 17:13:09 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/12 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#include "kernel/EplDllk.h"
+#include "kernel/EplDllkCal.h"
+#include "kernel/EplEventk.h"
+#include "kernel/EplNmtk.h"
+#include "edrv.h"
+#include "Benchmark.h"
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0)
+#include "kernel/EplPdok.h"
+#endif
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_VETH)) != 0)
+#include "kernel/VirtualEthernet.h"
+#endif
+
+//#if EPL_TIMER_USE_HIGHRES != FALSE
+#include "kernel/EplTimerHighResk.h"
+//#endif
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTK)) == 0)
+#error "EPL module DLLK needs EPL module NMTK!"
+#endif
+
+#if (EPL_DLL_PRES_READY_AFTER_SOA != FALSE) && (EPL_DLL_PRES_READY_AFTER_SOC != FALSE)
+#error "EPL module DLLK: select only one of EPL_DLL_PRES_READY_AFTER_SOA and EPL_DLL_PRES_READY_AFTER_SOC."
+#endif
+
+#if ((EPL_DLL_PRES_READY_AFTER_SOA != FALSE) || (EPL_DLL_PRES_READY_AFTER_SOC != FALSE)) \
+    && (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) == 0)
+#error "EPL module DLLK: currently, EPL_DLL_PRES_READY_AFTER_* is not supported if EPL_MODULE_NMT_MN is enabled."
+#endif
+
+#if (EDRV_FAST_TXFRAMES == FALSE) && \
+    ((EPL_DLL_PRES_READY_AFTER_SOA != FALSE) || (EPL_DLL_PRES_READY_AFTER_SOC != FALSE))
+#error "EPL module DLLK: EPL_DLL_PRES_READY_AFTER_* is enabled, but not EDRV_FAST_TXFRAMES."
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+// TracePoint support for realtime-debugging
+#ifdef _DBG_TRACE_POINTS_
+void PUBLIC TgtDbgSignalTracePoint(BYTE bTracePointNumber_p);
+void PUBLIC TgtDbgPostTraceValue(DWORD dwTraceValue_p);
+#define TGT_DBG_SIGNAL_TRACE_POINT(p)   TgtDbgSignalTracePoint(p)
+#define TGT_DBG_POST_TRACE_VALUE(v)     TgtDbgPostTraceValue(v)
+#else
+#define TGT_DBG_SIGNAL_TRACE_POINT(p)
+#define TGT_DBG_POST_TRACE_VALUE(v)
+#endif
+#define EPL_DLLK_DBG_POST_TRACE_VALUE(Event_p, uiNodeId_p, wErrorCode_p) \
+    TGT_DBG_POST_TRACE_VALUE((kEplEventSinkDllk << 28) | (Event_p << 24) \
+                             | (uiNodeId_p << 16) | wErrorCode_p)
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  EplDllk                                             */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description:
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   D E F I N I T I O N S                          //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+// defines for indexes of tEplDllInstance.m_pTxFrameInfo
+#define EPL_DLLK_TXFRAME_IDENTRES   0	// IdentResponse on CN / MN
+#define EPL_DLLK_TXFRAME_STATUSRES  1	// StatusResponse on CN / MN
+#define EPL_DLLK_TXFRAME_NMTREQ     2	// NMT Request from FIFO on CN / MN
+#define EPL_DLLK_TXFRAME_NONEPL     3	// non-EPL frame from FIFO on CN / MN
+#define EPL_DLLK_TXFRAME_PRES       4	// PRes on CN / MN
+#define EPL_DLLK_TXFRAME_SOC        5	// SoC on MN
+#define EPL_DLLK_TXFRAME_SOA        6	// SoA on MN
+#define EPL_DLLK_TXFRAME_PREQ       7	// PReq on MN
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+#define EPL_DLLK_TXFRAME_COUNT      (7 + EPL_D_NMT_MaxCNNumber_U8 + 2)	// on MN: 7 + MaxPReq of regular CNs + 1 Diag + 1 Router
+#else
+#define EPL_DLLK_TXFRAME_COUNT      5	// on CN: 5
+#endif
+
+#define EPL_DLLK_BUFLEN_EMPTY       0	// buffer is empty
+#define EPL_DLLK_BUFLEN_FILLING     1	// just the buffer is being filled
+#define EPL_DLLK_BUFLEN_MIN         60	// minimum ethernet frame length
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+typedef enum {
+	kEplDllGsInit = 0x00,	// MN/CN: initialisation (< PreOp2)
+	kEplDllCsWaitPreq = 0x01,	// CN: wait for PReq frame
+	kEplDllCsWaitSoc = 0x02,	// CN: wait for SoC frame
+	kEplDllCsWaitSoa = 0x03,	// CN: wait for SoA frame
+	kEplDllMsNonCyclic = 0x04,	// MN: reduced EPL cycle (PreOp1)
+	kEplDllMsWaitSocTrig = 0x05,	// MN: wait for SoC trigger (cycle timer)
+	kEplDllMsWaitPreqTrig = 0x06,	// MN: wait for (first) PReq trigger (WaitSoCPReq_U32)
+	kEplDllMsWaitPres = 0x07,	// MN: wait for PRes frame from CN
+	kEplDllMsWaitSoaTrig = 0x08,	// MN: wait for SoA trigger (PRes transmitted)
+	kEplDllMsWaitAsndTrig = 0x09,	// MN: wait for ASnd trigger (SoA transmitted)
+	kEplDllMsWaitAsnd = 0x0A,	// MN: wait for ASnd frame if SoA contained invitation
+
+} tEplDllState;
+
+typedef struct {
+	BYTE m_be_abSrcMac[6];
+	tEdrvTxBuffer *m_pTxBuffer;	// Buffers for Tx-Frames
+	unsigned int m_uiMaxTxFrames;
+	BYTE m_bFlag1;		// Flag 1 with EN, EC for PRes, StatusRes
+	BYTE m_bMnFlag1;	// Flag 1 with EA, ER from PReq, SoA of MN
+	BYTE m_bFlag2;		// Flag 2 with PR and RS for PRes, StatusRes, IdentRes
+	tEplDllConfigParam m_DllConfigParam;
+	tEplDllIdentParam m_DllIdentParam;
+	tEplDllState m_DllState;
+	tEplDllkCbAsync m_pfnCbAsync;
+	tEplDllAsndFilter m_aAsndFilter[EPL_DLL_MAX_ASND_SERVICE_ID];
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+	tEplDllkNodeInfo *m_pFirstNodeInfo;
+	tEplDllkNodeInfo *m_pCurNodeInfo;
+	tEplDllkNodeInfo m_aNodeInfo[EPL_NMT_MAX_NODE_ID];
+	tEplDllReqServiceId m_LastReqServiceId;
+	unsigned int m_uiLastTargetNodeId;
+#endif
+
+#if EPL_TIMER_USE_HIGHRES != FALSE
+	tEplTimerHdl m_TimerHdlCycle;	// used for EPL cycle monitoring on CN and generation on MN
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+	tEplTimerHdl m_TimerHdlResponse;	// used for CN response monitoring
+#endif				//(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+#endif
+
+	unsigned int m_uiCycleCount;	// cycle counter (needed for multiplexed cycle support)
+	unsigned long long m_ullFrameTimeout;	// frame timeout (cycle length + loss of frame tolerance)
+
+} tEplDllkInstance;
+
+//---------------------------------------------------------------------------
+// local vars
+//---------------------------------------------------------------------------
+
+// if no dynamic memory allocation shall be used
+// define structures statically
+static tEplDllkInstance EplDllkInstance_g;
+
+static tEdrvTxBuffer aEplDllkTxBuffer_l[EPL_DLLK_TXFRAME_COUNT];
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+// change DLL state on event
+static tEplKernel EplDllkChangeState(tEplNmtEvent NmtEvent_p,
+				     tEplNmtState NmtState_p);
+
+// called from EdrvInterruptHandler()
+static void EplDllkCbFrameReceived(tEdrvRxBuffer * pRxBuffer_p);
+
+// called from EdrvInterruptHandler()
+static void EplDllkCbFrameTransmitted(tEdrvTxBuffer * pTxBuffer_p);
+
+// check frame and set missing information
+static tEplKernel EplDllkCheckFrame(tEplFrame * pFrame_p,
+				    unsigned int uiFrameSize_p);
+
+// called by high resolution timer module to monitor EPL cycle as CN
+#if EPL_TIMER_USE_HIGHRES != FALSE
+static tEplKernel PUBLIC EplDllkCbCnTimer(tEplTimerEventArg * pEventArg_p);
+#endif
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+// MN: returns internal node info structure
+static tEplDllkNodeInfo *EplDllkGetNodeInfo(unsigned int uiNodeId_p);
+
+// transmit SoA
+static tEplKernel EplDllkMnSendSoa(tEplNmtState NmtState_p,
+				   tEplDllState * pDllStateProposed_p,
+				   BOOL fEnableInvitation_p);
+
+static tEplKernel EplDllkMnSendSoc(void);
+
+static tEplKernel EplDllkMnSendPreq(tEplNmtState NmtState_p,
+				    tEplDllState * pDllStateProposed_p);
+
+static tEplKernel EplDllkAsyncFrameNotReceived(tEplDllReqServiceId
+					       ReqServiceId_p,
+					       unsigned int uiNodeId_p);
+
+static tEplKernel PUBLIC EplDllkCbMnTimerCycle(tEplTimerEventArg * pEventArg_p);
+
+static tEplKernel PUBLIC EplDllkCbMnTimerResponse(tEplTimerEventArg *
+						  pEventArg_p);
+
+#endif
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkAddInstance()
+//
+// Description: add and initialize new instance of EPL stack
+//
+// Parameters:  pInitParam_p            = initialisation parameters like MAC address
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkAddInstance(tEplDllkInitParam * pInitParam_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	unsigned int uiIndex;
+	tEdrvInitParam EdrvInitParam;
+
+	// reset instance structure
+	EPL_MEMSET(&EplDllkInstance_g, 0, sizeof(EplDllkInstance_g));
+
+#if EPL_TIMER_USE_HIGHRES != FALSE
+	Ret = EplTimerHighReskInit();
+	if (Ret != kEplSuccessful) {	// error occured while initializing high resolution timer module
+		goto Exit;
+	}
+#endif
+
+	// if dynamic memory allocation available
+	// allocate instance structure
+	// allocate TPDO and RPDO table with default size
+
+	// initialize and link pointers in instance structure to frame tables
+	EplDllkInstance_g.m_pTxBuffer = aEplDllkTxBuffer_l;
+	EplDllkInstance_g.m_uiMaxTxFrames =
+	    sizeof(aEplDllkTxBuffer_l) / sizeof(tEdrvTxBuffer);
+
+	// initialize state
+	EplDllkInstance_g.m_DllState = kEplDllGsInit;
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+	// set up node info structure
+	for (uiIndex = 0; uiIndex < tabentries(EplDllkInstance_g.m_aNodeInfo);
+	     uiIndex++) {
+		EplDllkInstance_g.m_aNodeInfo[uiIndex].m_uiNodeId = uiIndex + 1;
+		EplDllkInstance_g.m_aNodeInfo[uiIndex].m_wPresPayloadLimit =
+		    0xFFFF;
+	}
+#endif
+
+	// initialize Edrv
+	EPL_MEMCPY(EdrvInitParam.m_abMyMacAddr, pInitParam_p->m_be_abSrcMac, 6);
+	EdrvInitParam.m_pfnRxHandler = EplDllkCbFrameReceived;
+	EdrvInitParam.m_pfnTxHandler = EplDllkCbFrameTransmitted;
+	Ret = EdrvInit(&EdrvInitParam);
+	if (Ret != kEplSuccessful) {	// error occured while initializing ethernet driver
+		goto Exit;
+	}
+	// copy local MAC address from Ethernet driver back to local instance structure
+	// because Ethernet driver may have read it from controller EEPROM
+	EPL_MEMCPY(EplDllkInstance_g.m_be_abSrcMac, EdrvInitParam.m_abMyMacAddr,
+		   6);
+	EPL_MEMCPY(pInitParam_p->m_be_abSrcMac, EdrvInitParam.m_abMyMacAddr, 6);
+
+	// initialize TxBuffer array
+	for (uiIndex = 0; uiIndex < EplDllkInstance_g.m_uiMaxTxFrames;
+	     uiIndex++) {
+		EplDllkInstance_g.m_pTxBuffer[uiIndex].m_pbBuffer = NULL;
+	}
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_VETH)) != 0)
+	Ret = VEthAddInstance(pInitParam_p);
+#endif
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkDelInstance()
+//
+// Description: deletes an instance of EPL stack
+//
+// Parameters:  (none)
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkDelInstance(void)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	// reset state
+	EplDllkInstance_g.m_DllState = kEplDllGsInit;
+
+#if EPL_TIMER_USE_HIGHRES != FALSE
+	Ret = EplTimerHighReskDelInstance();
+#endif
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_VETH)) != 0)
+	Ret = VEthDelInstance();
+#endif
+
+	Ret = EdrvShutdown();
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCreateTxFrame
+//
+// Description: creates the buffer for a Tx frame and registers it to the
+//              ethernet driver
+//
+// Parameters:  puiHandle_p             = OUT: handle to frame buffer
+//              ppFrame_p               = OUT: pointer to pointer of EPL frame
+//              puiFrameSize_p          = IN/OUT: pointer to size of frame
+//                                        returned size is always equal or larger than
+//                                        requested size, if that is not possible
+//                                        an error will be returned
+//              MsgType_p               = EPL message type
+//              ServiceId_p             = Service ID in case of ASnd frame, otherwise
+//                                        kEplDllAsndNotDefined
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkCreateTxFrame(unsigned int *puiHandle_p,
+				tEplFrame ** ppFrame_p,
+				unsigned int *puiFrameSize_p,
+				tEplMsgType MsgType_p,
+				tEplDllAsndServiceId ServiceId_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplFrame *pTxFrame;
+	unsigned int uiHandle = EplDllkInstance_g.m_uiMaxTxFrames;
+	tEdrvTxBuffer *pTxBuffer = NULL;
+
+	if (MsgType_p == kEplMsgTypeAsnd) {
+		// search for fixed Tx buffers
+		if (ServiceId_p == kEplDllAsndIdentResponse) {
+			uiHandle = EPL_DLLK_TXFRAME_IDENTRES;
+		} else if (ServiceId_p == kEplDllAsndStatusResponse) {
+			uiHandle = EPL_DLLK_TXFRAME_STATUSRES;
+		} else if ((ServiceId_p == kEplDllAsndNmtRequest)
+			   || (ServiceId_p == kEplDllAsndNmtCommand)) {
+			uiHandle = EPL_DLLK_TXFRAME_NMTREQ;
+		}
+
+		if (uiHandle >= EplDllkInstance_g.m_uiMaxTxFrames) {	// look for free entry
+			uiHandle = EPL_DLLK_TXFRAME_PREQ;
+			pTxBuffer = &EplDllkInstance_g.m_pTxBuffer[uiHandle];
+			for (; uiHandle < EplDllkInstance_g.m_uiMaxTxFrames;
+			     uiHandle++, pTxBuffer++) {
+				if (pTxBuffer->m_pbBuffer == NULL) {	// free entry found
+					break;
+				}
+			}
+		}
+	} else if (MsgType_p == kEplMsgTypeNonEpl) {
+		uiHandle = EPL_DLLK_TXFRAME_NONEPL;
+	} else if (MsgType_p == kEplMsgTypePres) {
+		uiHandle = EPL_DLLK_TXFRAME_PRES;
+	} else if (MsgType_p == kEplMsgTypeSoc) {
+		uiHandle = EPL_DLLK_TXFRAME_SOC;
+	} else if (MsgType_p == kEplMsgTypeSoa) {
+		uiHandle = EPL_DLLK_TXFRAME_SOA;
+	} else {		// look for free entry
+		uiHandle = EPL_DLLK_TXFRAME_PREQ;
+		pTxBuffer = &EplDllkInstance_g.m_pTxBuffer[uiHandle];
+		for (; uiHandle < EplDllkInstance_g.m_uiMaxTxFrames;
+		     uiHandle++, pTxBuffer++) {
+			if (pTxBuffer->m_pbBuffer == NULL) {	// free entry found
+				break;
+			}
+		}
+		if (pTxBuffer->m_pbBuffer != NULL) {
+			Ret = kEplEdrvNoFreeBufEntry;
+			goto Exit;
+		}
+	}
+
+	// test if requested entry is free
+	pTxBuffer = &EplDllkInstance_g.m_pTxBuffer[uiHandle];
+	if (pTxBuffer->m_pbBuffer != NULL) {	// entry is not free
+		Ret = kEplEdrvNoFreeBufEntry;
+		goto Exit;
+	}
+	// setup Tx buffer
+	pTxBuffer->m_EplMsgType = MsgType_p;
+	pTxBuffer->m_uiMaxBufferLen = *puiFrameSize_p;
+
+	Ret = EdrvAllocTxMsgBuffer(pTxBuffer);
+	if (Ret != kEplSuccessful) {	// error occured while registering Tx frame
+		goto Exit;
+	}
+	// because buffer size may be larger than requested
+	// memorize real length of frame
+	pTxBuffer->m_uiTxMsgLen = *puiFrameSize_p;
+
+	// fill whole frame with 0
+	EPL_MEMSET(pTxBuffer->m_pbBuffer, 0, pTxBuffer->m_uiMaxBufferLen);
+
+	pTxFrame = (tEplFrame *) pTxBuffer->m_pbBuffer;
+
+	if (MsgType_p != kEplMsgTypeNonEpl) {	// fill out Frame only if it is an EPL frame
+		// ethertype
+		AmiSetWordToBe(&pTxFrame->m_be_wEtherType,
+			       EPL_C_DLL_ETHERTYPE_EPL);
+		// source node ID
+		AmiSetByteToLe(&pTxFrame->m_le_bSrcNodeId,
+			       (BYTE) EplDllkInstance_g.m_DllConfigParam.
+			       m_uiNodeId);
+		// source MAC address
+		EPL_MEMCPY(&pTxFrame->m_be_abSrcMac[0],
+			   &EplDllkInstance_g.m_be_abSrcMac[0], 6);
+		switch (MsgType_p) {
+		case kEplMsgTypeAsnd:
+			// destination MAC address
+			AmiSetQword48ToBe(&pTxFrame->m_be_abDstMac[0],
+					  EPL_C_DLL_MULTICAST_ASND);
+			// destination node ID
+			switch (ServiceId_p) {
+			case kEplDllAsndIdentResponse:
+			case kEplDllAsndStatusResponse:
+				{	// IdentResponses and StatusResponses are Broadcast
+					AmiSetByteToLe(&pTxFrame->
+						       m_le_bDstNodeId,
+						       (BYTE)
+						       EPL_C_ADR_BROADCAST);
+					break;
+				}
+
+			default:
+				break;
+			}
+			// ASnd Service ID
+			AmiSetByteToLe(&pTxFrame->m_Data.m_Asnd.m_le_bServiceId,
+				       ServiceId_p);
+			break;
+
+		case kEplMsgTypeSoc:
+			// destination MAC address
+			AmiSetQword48ToBe(&pTxFrame->m_be_abDstMac[0],
+					  EPL_C_DLL_MULTICAST_SOC);
+			// destination node ID
+			AmiSetByteToLe(&pTxFrame->m_le_bDstNodeId,
+				       (BYTE) EPL_C_ADR_BROADCAST);
+			// reset Flags
+			//AmiSetByteToLe(&pTxFrame->m_Data.m_Soc.m_le_bFlag1, (BYTE) 0);
+			//AmiSetByteToLe(&pTxFrame->m_Data.m_Soc.m_le_bFlag2, (BYTE) 0);
+			break;
+
+		case kEplMsgTypeSoa:
+			// destination MAC address
+			AmiSetQword48ToBe(&pTxFrame->m_be_abDstMac[0],
+					  EPL_C_DLL_MULTICAST_SOA);
+			// destination node ID
+			AmiSetByteToLe(&pTxFrame->m_le_bDstNodeId,
+				       (BYTE) EPL_C_ADR_BROADCAST);
+			// reset Flags
+			//AmiSetByteToLe(&pTxFrame->m_Data.m_Soa.m_le_bFlag1, (BYTE) 0);
+			//AmiSetByteToLe(&pTxFrame->m_Data.m_Soa.m_le_bFlag2, (BYTE) 0);
+			// EPL profile version
+			AmiSetByteToLe(&pTxFrame->m_Data.m_Soa.m_le_bEplVersion,
+				       (BYTE) EPL_SPEC_VERSION);
+			break;
+
+		case kEplMsgTypePres:
+			// destination MAC address
+			AmiSetQword48ToBe(&pTxFrame->m_be_abDstMac[0],
+					  EPL_C_DLL_MULTICAST_PRES);
+			// destination node ID
+			AmiSetByteToLe(&pTxFrame->m_le_bDstNodeId,
+				       (BYTE) EPL_C_ADR_BROADCAST);
+			// reset Flags
+			//AmiSetByteToLe(&pTxFrame->m_Data.m_Pres.m_le_bFlag1, (BYTE) 0);
+			//AmiSetByteToLe(&pTxFrame->m_Data.m_Pres.m_le_bFlag2, (BYTE) 0);
+			// PDO size
+			//AmiSetWordToLe(&pTxFrame->m_Data.m_Pres.m_le_wSize, 0);
+			break;
+
+		case kEplMsgTypePreq:
+			// reset Flags
+			//AmiSetByteToLe(&pTxFrame->m_Data.m_Preq.m_le_bFlag1, (BYTE) 0);
+			//AmiSetByteToLe(&pTxFrame->m_Data.m_Preq.m_le_bFlag2, (BYTE) 0);
+			// PDO size
+			//AmiSetWordToLe(&pTxFrame->m_Data.m_Preq.m_le_wSize, 0);
+			break;
+
+		default:
+			break;
+		}
+		// EPL message type
+		AmiSetByteToLe(&pTxFrame->m_le_bMessageType, (BYTE) MsgType_p);
+	}
+
+	*ppFrame_p = pTxFrame;
+	*puiFrameSize_p = pTxBuffer->m_uiMaxBufferLen;
+	*puiHandle_p = uiHandle;
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkDeleteTxFrame
+//
+// Description: deletes the buffer for a Tx frame and frees it in the
+//              ethernet driver
+//
+// Parameters:  uiHandle_p              = IN: handle to frame buffer
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkDeleteTxFrame(unsigned int uiHandle_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEdrvTxBuffer *pTxBuffer = NULL;
+
+	if (uiHandle_p >= EplDllkInstance_g.m_uiMaxTxFrames) {	// handle is not valid
+		Ret = kEplDllIllegalHdl;
+		goto Exit;
+	}
+
+	pTxBuffer = &EplDllkInstance_g.m_pTxBuffer[uiHandle_p];
+
+	// mark buffer as free so that frame will not be send in future anymore
+	// $$$ d.k. What's up with running transmissions?
+	pTxBuffer->m_uiTxMsgLen = EPL_DLLK_BUFLEN_EMPTY;
+	pTxBuffer->m_pbBuffer = NULL;
+
+	// delete Tx buffer
+	Ret = EdrvReleaseTxMsgBuffer(pTxBuffer);
+	if (Ret != kEplSuccessful) {	// error occured while releasing Tx frame
+		goto Exit;
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkProcess
+//
+// Description: process the passed event
+//
+// Parameters:  pEvent_p                = event to be processed
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkProcess(tEplEvent * pEvent_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplFrame *pTxFrame;
+	tEdrvTxBuffer *pTxBuffer;
+	unsigned int uiHandle;
+	unsigned int uiFrameSize;
+	BYTE abMulticastMac[6];
+	tEplDllAsyncReqPriority AsyncReqPriority;
+	unsigned int uiFrameCount;
+	tEplNmtState NmtState;
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0)
+	tEplFrameInfo FrameInfo;
+#endif
+
+	switch (pEvent_p->m_EventType) {
+	case kEplEventTypeDllkCreate:
+		{
+			// $$$ reset ethernet driver
+
+			NmtState = *((tEplNmtState *) pEvent_p->m_pArg);
+
+			// initialize flags for PRes and StatusRes
+			EplDllkInstance_g.m_bFlag1 = EPL_FRAME_FLAG1_EC;
+			EplDllkInstance_g.m_bMnFlag1 = 0;
+			EplDllkInstance_g.m_bFlag2 = 0;
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+			// initialize linked node list
+			EplDllkInstance_g.m_pFirstNodeInfo = NULL;
+#endif
+
+			// register TxFrames in Edrv
+
+			// IdentResponse
+			uiFrameSize = EPL_C_DLL_MINSIZE_IDENTRES;
+			Ret =
+			    EplDllkCreateTxFrame(&uiHandle, &pTxFrame,
+						 &uiFrameSize, kEplMsgTypeAsnd,
+						 kEplDllAsndIdentResponse);
+			if (Ret != kEplSuccessful) {	// error occured while registering Tx frame
+				goto Exit;
+			}
+			// EPL profile version
+			AmiSetByteToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+				       m_IdentResponse.m_le_bEplProfileVersion,
+				       (BYTE) EPL_SPEC_VERSION);
+			// FeatureFlags
+			AmiSetDwordToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+					m_IdentResponse.m_le_dwFeatureFlags,
+					EplDllkInstance_g.m_DllConfigParam.
+					m_dwFeatureFlags);
+			// MTU
+			AmiSetWordToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+				       m_IdentResponse.m_le_wMtu,
+				       (WORD) EplDllkInstance_g.
+				       m_DllConfigParam.m_uiAsyncMtu);
+			// PollInSize
+			AmiSetWordToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+				       m_IdentResponse.m_le_wPollInSize,
+				       (WORD) EplDllkInstance_g.
+				       m_DllConfigParam.
+				       m_uiPreqActPayloadLimit);
+			// PollOutSize
+			AmiSetWordToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+				       m_IdentResponse.m_le_wPollOutSize,
+				       (WORD) EplDllkInstance_g.
+				       m_DllConfigParam.
+				       m_uiPresActPayloadLimit);
+			// ResponseTime / PresMaxLatency
+			AmiSetDwordToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+					m_IdentResponse.m_le_dwResponseTime,
+					EplDllkInstance_g.m_DllConfigParam.
+					m_dwPresMaxLatency);
+			// DeviceType
+			AmiSetDwordToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+					m_IdentResponse.m_le_dwDeviceType,
+					EplDllkInstance_g.m_DllIdentParam.
+					m_dwDeviceType);
+			// VendorId
+			AmiSetDwordToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+					m_IdentResponse.m_le_dwVendorId,
+					EplDllkInstance_g.m_DllIdentParam.
+					m_dwVendorId);
+			// ProductCode
+			AmiSetDwordToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+					m_IdentResponse.m_le_dwProductCode,
+					EplDllkInstance_g.m_DllIdentParam.
+					m_dwProductCode);
+			// RevisionNumber
+			AmiSetDwordToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+					m_IdentResponse.m_le_dwRevisionNumber,
+					EplDllkInstance_g.m_DllIdentParam.
+					m_dwRevisionNumber);
+			// SerialNumber
+			AmiSetDwordToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+					m_IdentResponse.m_le_dwSerialNumber,
+					EplDllkInstance_g.m_DllIdentParam.
+					m_dwSerialNumber);
+			// VendorSpecificExt1
+			AmiSetQword64ToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+					  m_IdentResponse.
+					  m_le_qwVendorSpecificExt1,
+					  EplDllkInstance_g.m_DllIdentParam.
+					  m_qwVendorSpecificExt1);
+			// VerifyConfigurationDate
+			AmiSetDwordToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+					m_IdentResponse.
+					m_le_dwVerifyConfigurationDate,
+					EplDllkInstance_g.m_DllIdentParam.
+					m_dwVerifyConfigurationDate);
+			// VerifyConfigurationTime
+			AmiSetDwordToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+					m_IdentResponse.
+					m_le_dwVerifyConfigurationTime,
+					EplDllkInstance_g.m_DllIdentParam.
+					m_dwVerifyConfigurationTime);
+			// ApplicationSwDate
+			AmiSetDwordToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+					m_IdentResponse.
+					m_le_dwApplicationSwDate,
+					EplDllkInstance_g.m_DllIdentParam.
+					m_dwApplicationSwDate);
+			// ApplicationSwTime
+			AmiSetDwordToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+					m_IdentResponse.
+					m_le_dwApplicationSwTime,
+					EplDllkInstance_g.m_DllIdentParam.
+					m_dwApplicationSwTime);
+			// IPAddress
+			AmiSetDwordToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+					m_IdentResponse.m_le_dwIpAddress,
+					EplDllkInstance_g.m_DllIdentParam.
+					m_dwIpAddress);
+			// SubnetMask
+			AmiSetDwordToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+					m_IdentResponse.m_le_dwSubnetMask,
+					EplDllkInstance_g.m_DllIdentParam.
+					m_dwSubnetMask);
+			// DefaultGateway
+			AmiSetDwordToLe(&pTxFrame->m_Data.m_Asnd.m_Payload.
+					m_IdentResponse.m_le_dwDefaultGateway,
+					EplDllkInstance_g.m_DllIdentParam.
+					m_dwDefaultGateway);
+			// HostName
+			EPL_MEMCPY(&pTxFrame->m_Data.m_Asnd.m_Payload.
+				   m_IdentResponse.m_le_sHostname[0],
+				   &EplDllkInstance_g.m_DllIdentParam.
+				   m_sHostname[0],
+				   sizeof(EplDllkInstance_g.m_DllIdentParam.
+					  m_sHostname));
+			// VendorSpecificExt2
+			EPL_MEMCPY(&pTxFrame->m_Data.m_Asnd.m_Payload.
+				   m_IdentResponse.m_le_abVendorSpecificExt2[0],
+				   &EplDllkInstance_g.m_DllIdentParam.
+				   m_abVendorSpecificExt2[0],
+				   sizeof(EplDllkInstance_g.m_DllIdentParam.
+					  m_abVendorSpecificExt2));
+
+			// StatusResponse
+			uiFrameSize = EPL_C_DLL_MINSIZE_STATUSRES;
+			Ret =
+			    EplDllkCreateTxFrame(&uiHandle, &pTxFrame,
+						 &uiFrameSize, kEplMsgTypeAsnd,
+						 kEplDllAsndStatusResponse);
+			if (Ret != kEplSuccessful) {	// error occured while registering Tx frame
+				goto Exit;
+			}
+			// PRes $$$ maybe move this to PDO module
+			if ((EplDllkInstance_g.m_DllConfigParam.m_fAsyncOnly ==
+			     FALSE)
+			    && (EplDllkInstance_g.m_DllConfigParam.m_uiPresActPayloadLimit >= 36)) {	// it is not configured as async-only CN,
+				// so take part in isochronous phase and register PRes frame
+				uiFrameSize =
+				    EplDllkInstance_g.m_DllConfigParam.
+				    m_uiPresActPayloadLimit + 24;
+				Ret =
+				    EplDllkCreateTxFrame(&uiHandle, &pTxFrame,
+							 &uiFrameSize,
+							 kEplMsgTypePres,
+							 kEplDllAsndNotDefined);
+				if (Ret != kEplSuccessful) {	// error occured while registering Tx frame
+					goto Exit;
+				}
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0)
+				// initially encode TPDO -> inform PDO module
+				FrameInfo.m_pFrame = pTxFrame;
+				FrameInfo.m_uiFrameSize = uiFrameSize;
+				Ret = EplPdokCbPdoTransmitted(&FrameInfo);
+#endif
+				// reset cycle counter
+				EplDllkInstance_g.m_uiCycleCount = 0;
+			} else {	// it is an async-only CN
+				// fool EplDllkChangeState() to think that PRes was not expected
+				EplDllkInstance_g.m_uiCycleCount = 1;
+			}
+
+			// NMT request
+			uiFrameSize = EPL_C_IP_MAX_MTU;
+			Ret =
+			    EplDllkCreateTxFrame(&uiHandle, &pTxFrame,
+						 &uiFrameSize, kEplMsgTypeAsnd,
+						 kEplDllAsndNmtRequest);
+			if (Ret != kEplSuccessful) {	// error occured while registering Tx frame
+				goto Exit;
+			}
+			// mark Tx buffer as empty
+			EplDllkInstance_g.m_pTxBuffer[uiHandle].m_uiTxMsgLen =
+			    EPL_DLLK_BUFLEN_EMPTY;
+
+			// non-EPL frame
+			uiFrameSize = EPL_C_IP_MAX_MTU;
+			Ret =
+			    EplDllkCreateTxFrame(&uiHandle, &pTxFrame,
+						 &uiFrameSize,
+						 kEplMsgTypeNonEpl,
+						 kEplDllAsndNotDefined);
+			if (Ret != kEplSuccessful) {	// error occured while registering Tx frame
+				goto Exit;
+			}
+			// mark Tx buffer as empty
+			EplDllkInstance_g.m_pTxBuffer[uiHandle].m_uiTxMsgLen =
+			    EPL_DLLK_BUFLEN_EMPTY;
+
+			// register multicast MACs in ethernet driver
+			AmiSetQword48ToBe(&abMulticastMac[0],
+					  EPL_C_DLL_MULTICAST_SOC);
+			Ret = EdrvDefineRxMacAddrEntry(abMulticastMac);
+			AmiSetQword48ToBe(&abMulticastMac[0],
+					  EPL_C_DLL_MULTICAST_SOA);
+			Ret = EdrvDefineRxMacAddrEntry(abMulticastMac);
+			AmiSetQword48ToBe(&abMulticastMac[0],
+					  EPL_C_DLL_MULTICAST_PRES);
+			Ret = EdrvDefineRxMacAddrEntry(abMulticastMac);
+			AmiSetQword48ToBe(&abMulticastMac[0],
+					  EPL_C_DLL_MULTICAST_ASND);
+			Ret = EdrvDefineRxMacAddrEntry(abMulticastMac);
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+			if (NmtState >= kEplNmtMsNotActive) {	// local node is MN
+				unsigned int uiIndex;
+
+				// SoC
+				uiFrameSize = EPL_C_DLL_MINSIZE_SOC;
+				Ret =
+				    EplDllkCreateTxFrame(&uiHandle, &pTxFrame,
+							 &uiFrameSize,
+							 kEplMsgTypeSoc,
+							 kEplDllAsndNotDefined);
+				if (Ret != kEplSuccessful) {	// error occured while registering Tx frame
+					goto Exit;
+				}
+				// SoA
+				uiFrameSize = EPL_C_DLL_MINSIZE_SOA;
+				Ret =
+				    EplDllkCreateTxFrame(&uiHandle, &pTxFrame,
+							 &uiFrameSize,
+							 kEplMsgTypeSoa,
+							 kEplDllAsndNotDefined);
+				if (Ret != kEplSuccessful) {	// error occured while registering Tx frame
+					goto Exit;
+				}
+
+				for (uiIndex = 0;
+				     uiIndex <
+				     tabentries(EplDllkInstance_g.m_aNodeInfo);
+				     uiIndex++) {
+//                    EplDllkInstance_g.m_aNodeInfo[uiIndex].m_uiNodeId = uiIndex + 1;
+					EplDllkInstance_g.m_aNodeInfo[uiIndex].
+					    m_wPresPayloadLimit =
+					    (WORD) EplDllkInstance_g.
+					    m_DllConfigParam.
+					    m_uiIsochrRxMaxPayload;
+				}
+
+				// calculate cycle length
+				EplDllkInstance_g.m_ullFrameTimeout = 1000LL
+				    *
+				    ((unsigned long long)EplDllkInstance_g.
+				     m_DllConfigParam.m_dwCycleLen);
+			}
+#endif //(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+			Ret = EplDllkCalAsyncClearBuffer();
+
+			break;
+		}
+
+	case kEplEventTypeDllkDestroy:
+		{
+			// destroy all data structures
+
+			NmtState = *((tEplNmtState *) pEvent_p->m_pArg);
+
+			// delete Tx frames
+			Ret = EplDllkDeleteTxFrame(EPL_DLLK_TXFRAME_IDENTRES);
+			if (Ret != kEplSuccessful) {	// error occured while deregistering Tx frame
+				goto Exit;
+			}
+
+			Ret = EplDllkDeleteTxFrame(EPL_DLLK_TXFRAME_STATUSRES);
+			if (Ret != kEplSuccessful) {	// error occured while deregistering Tx frame
+				goto Exit;
+			}
+
+			Ret = EplDllkDeleteTxFrame(EPL_DLLK_TXFRAME_PRES);
+			if (Ret != kEplSuccessful) {	// error occured while deregistering Tx frame
+				goto Exit;
+			}
+
+			Ret = EplDllkDeleteTxFrame(EPL_DLLK_TXFRAME_NMTREQ);
+			if (Ret != kEplSuccessful) {	// error occured while deregistering Tx frame
+				goto Exit;
+			}
+
+			Ret = EplDllkDeleteTxFrame(EPL_DLLK_TXFRAME_NONEPL);
+			if (Ret != kEplSuccessful) {	// error occured while deregistering Tx frame
+				goto Exit;
+			}
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+			if (NmtState >= kEplNmtMsNotActive) {	// local node was MN
+				unsigned int uiIndex;
+
+				Ret =
+				    EplDllkDeleteTxFrame(EPL_DLLK_TXFRAME_SOC);
+				if (Ret != kEplSuccessful) {	// error occured while deregistering Tx frame
+					goto Exit;
+				}
+
+				Ret =
+				    EplDllkDeleteTxFrame(EPL_DLLK_TXFRAME_SOA);
+				if (Ret != kEplSuccessful) {	// error occured while deregistering Tx frame
+					goto Exit;
+				}
+
+				for (uiIndex = 0;
+				     uiIndex <
+				     tabentries(EplDllkInstance_g.m_aNodeInfo);
+				     uiIndex++) {
+					if (EplDllkInstance_g.
+					    m_aNodeInfo[uiIndex].
+					    m_pPreqTxBuffer != NULL) {
+						uiHandle =
+						    EplDllkInstance_g.
+						    m_aNodeInfo[uiIndex].
+						    m_pPreqTxBuffer -
+						    EplDllkInstance_g.
+						    m_pTxBuffer;
+						EplDllkInstance_g.
+						    m_aNodeInfo[uiIndex].
+						    m_pPreqTxBuffer = NULL;
+						Ret =
+						    EplDllkDeleteTxFrame
+						    (uiHandle);
+						if (Ret != kEplSuccessful) {	// error occured while deregistering Tx frame
+							goto Exit;
+						}
+
+					}
+					EplDllkInstance_g.m_aNodeInfo[uiIndex].
+					    m_wPresPayloadLimit = 0xFFFF;
+				}
+			}
+#endif //(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+			// deregister multicast MACs in ethernet driver
+			AmiSetQword48ToBe(&abMulticastMac[0],
+					  EPL_C_DLL_MULTICAST_SOC);
+			Ret = EdrvUndefineRxMacAddrEntry(abMulticastMac);
+			AmiSetQword48ToBe(&abMulticastMac[0],
+					  EPL_C_DLL_MULTICAST_SOA);
+			Ret = EdrvUndefineRxMacAddrEntry(abMulticastMac);
+			AmiSetQword48ToBe(&abMulticastMac[0],
+					  EPL_C_DLL_MULTICAST_PRES);
+			Ret = EdrvUndefineRxMacAddrEntry(abMulticastMac);
+			AmiSetQword48ToBe(&abMulticastMac[0],
+					  EPL_C_DLL_MULTICAST_ASND);
+			Ret = EdrvUndefineRxMacAddrEntry(abMulticastMac);
+
+			// delete timer
+#if EPL_TIMER_USE_HIGHRES != FALSE
+			Ret =
+			    EplTimerHighReskDeleteTimer(&EplDllkInstance_g.
+							m_TimerHdlCycle);
+#endif
+
+			break;
+		}
+
+	case kEplEventTypeDllkFillTx:
+		{
+			// fill TxBuffer of specified priority with new frame if empty
+
+			pTxFrame = NULL;
+			AsyncReqPriority =
+			    *((tEplDllAsyncReqPriority *) pEvent_p->m_pArg);
+			switch (AsyncReqPriority) {
+			case kEplDllAsyncReqPrioNmt:	// NMT request priority
+				{
+					pTxBuffer =
+					    &EplDllkInstance_g.
+					    m_pTxBuffer
+					    [EPL_DLLK_TXFRAME_NMTREQ];
+					if (pTxBuffer->m_pbBuffer != NULL) {	// NmtRequest does exist
+						// check if frame is empty and not being filled
+						if (pTxBuffer->m_uiTxMsgLen ==
+						    EPL_DLLK_BUFLEN_EMPTY) {
+							// mark Tx buffer as filling is in process
+							pTxBuffer->
+							    m_uiTxMsgLen =
+							    EPL_DLLK_BUFLEN_FILLING;
+							// set max buffer size as input parameter
+							uiFrameSize =
+							    pTxBuffer->
+							    m_uiMaxBufferLen;
+							// copy frame from shared loop buffer to Tx buffer
+							Ret =
+							    EplDllkCalAsyncGetTxFrame
+							    (pTxBuffer->
+							     m_pbBuffer,
+							     &uiFrameSize,
+							     AsyncReqPriority);
+							if (Ret ==
+							    kEplSuccessful) {
+								pTxFrame =
+								    (tEplFrame
+								     *)
+								    pTxBuffer->
+								    m_pbBuffer;
+								Ret =
+								    EplDllkCheckFrame
+								    (pTxFrame,
+								     uiFrameSize);
+
+								// set buffer valid
+								pTxBuffer->
+								    m_uiTxMsgLen
+								    =
+								    uiFrameSize;
+							} else if (Ret == kEplDllAsyncTxBufferEmpty) {	// empty Tx buffer is not a real problem
+								// so just ignore it
+								Ret =
+								    kEplSuccessful;
+								// mark Tx buffer as empty
+								pTxBuffer->
+								    m_uiTxMsgLen
+								    =
+								    EPL_DLLK_BUFLEN_EMPTY;
+							}
+						}
+					}
+					break;
+				}
+
+			default:	// generic priority
+				{
+					pTxBuffer =
+					    &EplDllkInstance_g.
+					    m_pTxBuffer
+					    [EPL_DLLK_TXFRAME_NONEPL];
+					if (pTxBuffer->m_pbBuffer != NULL) {	// non-EPL frame does exist
+						// check if frame is empty and not being filled
+						if (pTxBuffer->m_uiTxMsgLen ==
+						    EPL_DLLK_BUFLEN_EMPTY) {
+							// mark Tx buffer as filling is in process
+							pTxBuffer->
+							    m_uiTxMsgLen =
+							    EPL_DLLK_BUFLEN_FILLING;
+							// set max buffer size as input parameter
+							uiFrameSize =
+							    pTxBuffer->
+							    m_uiMaxBufferLen;
+							// copy frame from shared loop buffer to Tx buffer
+							Ret =
+							    EplDllkCalAsyncGetTxFrame
+							    (pTxBuffer->
+							     m_pbBuffer,
+							     &uiFrameSize,
+							     AsyncReqPriority);
+							if (Ret ==
+							    kEplSuccessful) {
+								pTxFrame =
+								    (tEplFrame
+								     *)
+								    pTxBuffer->
+								    m_pbBuffer;
+								Ret =
+								    EplDllkCheckFrame
+								    (pTxFrame,
+								     uiFrameSize);
+
+								// set buffer valid
+								pTxBuffer->
+								    m_uiTxMsgLen
+								    =
+								    uiFrameSize;
+							} else if (Ret == kEplDllAsyncTxBufferEmpty) {	// empty Tx buffer is not a real problem
+								// so just ignore it
+								Ret =
+								    kEplSuccessful;
+								// mark Tx buffer as empty
+								pTxBuffer->
+								    m_uiTxMsgLen
+								    =
+								    EPL_DLLK_BUFLEN_EMPTY;
+							}
+						}
+					}
+					break;
+				}
+			}
+
+			NmtState = EplNmtkGetNmtState();
+
+			if ((NmtState == kEplNmtCsBasicEthernet) || (NmtState == kEplNmtMsBasicEthernet)) {	// send frame immediately
+				if (pTxFrame != NULL) {	// frame is present
+					// padding is done by Edrv or ethernet controller
+					Ret = EdrvSendTxMsg(pTxBuffer);
+				} else {	// no frame moved to TxBuffer
+					// check if TxBuffers contain unsent frames
+					if (EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_NMTREQ].m_uiTxMsgLen > EPL_DLLK_BUFLEN_EMPTY) {	// NMT request Tx buffer contains a frame
+						Ret =
+						    EdrvSendTxMsg
+						    (&EplDllkInstance_g.
+						     m_pTxBuffer
+						     [EPL_DLLK_TXFRAME_NMTREQ]);
+					} else if (EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_NONEPL].m_uiTxMsgLen > EPL_DLLK_BUFLEN_EMPTY) {	// non-EPL Tx buffer contains a frame
+						Ret =
+						    EdrvSendTxMsg
+						    (&EplDllkInstance_g.
+						     m_pTxBuffer
+						     [EPL_DLLK_TXFRAME_NONEPL]);
+					}
+					if (Ret == kEplInvalidOperation) {	// ignore error if caused by already active transmission
+						Ret = kEplSuccessful;
+					}
+				}
+				// reset PRes flag 2
+				EplDllkInstance_g.m_bFlag2 = 0;
+			} else {
+				// update Flag 2 (PR, RS)
+				Ret =
+				    EplDllkCalAsyncGetTxCount(&AsyncReqPriority,
+							      &uiFrameCount);
+				if (AsyncReqPriority == kEplDllAsyncReqPrioNmt) {	// non-empty FIFO with hightest priority is for NMT requests
+					if (EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_NMTREQ].m_uiTxMsgLen > EPL_DLLK_BUFLEN_EMPTY) {	// NMT request Tx buffer contains a frame
+						// add one more frame
+						uiFrameCount++;
+					}
+				} else {	// non-empty FIFO with highest priority is for generic frames
+					if (EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_NMTREQ].m_uiTxMsgLen > EPL_DLLK_BUFLEN_EMPTY) {	// NMT request Tx buffer contains a frame
+						// use NMT request FIFO, because of higher priority
+						uiFrameCount = 1;
+						AsyncReqPriority =
+						    kEplDllAsyncReqPrioNmt;
+					} else if (EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_NONEPL].m_uiTxMsgLen > EPL_DLLK_BUFLEN_EMPTY) {	// non-EPL Tx buffer contains a frame
+						// use NMT request FIFO, because of higher priority
+						// add one more frame
+						uiFrameCount++;
+					}
+				}
+
+				if (uiFrameCount > 7) {	// limit frame request to send counter to 7
+					uiFrameCount = 7;
+				}
+				if (uiFrameCount > 0) {
+					EplDllkInstance_g.m_bFlag2 =
+					    (BYTE) (((AsyncReqPriority <<
+						      EPL_FRAME_FLAG2_PR_SHIFT)
+						     & EPL_FRAME_FLAG2_PR)
+						    | (uiFrameCount &
+						       EPL_FRAME_FLAG2_RS));
+				} else {
+					EplDllkInstance_g.m_bFlag2 = 0;
+				}
+			}
+
+			break;
+		}
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+	case kEplEventTypeDllkStartReducedCycle:
+		{
+			// start the reduced cycle by programming the cycle timer
+			// it is issued by NMT MN module, when PreOp1 is entered
+
+			// clear the asynchronous queues
+			Ret = EplDllkCalAsyncClearQueues();
+
+			// reset cycle counter (everytime a SoA is triggerd in PreOp1 the counter is incremented
+			// and when it reaches EPL_C_DLL_PREOP1_START_CYCLES the SoA may contain invitations)
+			EplDllkInstance_g.m_uiCycleCount = 0;
+
+			// remove any CN from isochronous phase
+			while (EplDllkInstance_g.m_pFirstNodeInfo != NULL) {
+				EplDllkDeleteNode(EplDllkInstance_g.
+						  m_pFirstNodeInfo->m_uiNodeId);
+			}
+
+			// change state to NonCyclic,
+			// hence EplDllkChangeState() will not ignore the next call
+			EplDllkInstance_g.m_DllState = kEplDllMsNonCyclic;
+
+#if EPL_TIMER_USE_HIGHRES != FALSE
+			if (EplDllkInstance_g.m_DllConfigParam.
+			    m_dwAsyncSlotTimeout != 0) {
+				Ret =
+				    EplTimerHighReskModifyTimerNs
+				    (&EplDllkInstance_g.m_TimerHdlCycle,
+				     EplDllkInstance_g.m_DllConfigParam.
+				     m_dwAsyncSlotTimeout,
+				     EplDllkCbMnTimerCycle, 0L, FALSE);
+			}
+#endif
+
+			break;
+		}
+#endif
+
+#if EPL_DLL_PRES_READY_AFTER_SOA != FALSE
+	case kEplEventTypeDllkPresReady:
+		{
+			// post PRes to transmit FIFO
+
+			NmtState = EplNmtkGetNmtState();
+
+			if (NmtState != kEplNmtCsBasicEthernet) {
+				// Does PRes exist?
+				if (EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_PRES].m_pbBuffer != NULL) {	// PRes does exist
+					pTxFrame =
+					    (tEplFrame *) EplDllkInstance_g.
+					    m_pTxBuffer[EPL_DLLK_TXFRAME_PRES].
+					    m_pbBuffer;
+					// update frame (NMT state, RD, RS, PR, MS, EN flags)
+					if (NmtState < kEplNmtCsPreOperational2) {	// NMT state is not PreOp2, ReadyToOp or Op
+						// fake NMT state PreOp2, because PRes will be sent only in PreOp2 or greater
+						NmtState =
+						    kEplNmtCsPreOperational2;
+					}
+					AmiSetByteToLe(&pTxFrame->m_Data.m_Pres.
+						       m_le_bNmtStatus,
+						       (BYTE) NmtState);
+					AmiSetByteToLe(&pTxFrame->m_Data.m_Pres.
+						       m_le_bFlag2,
+						       EplDllkInstance_g.
+						       m_bFlag2);
+					if (NmtState != kEplNmtCsOperational) {	// mark PDO as invalid in NMT state Op
+						// $$$ reset only RD flag; set other flags appropriately
+						AmiSetByteToLe(&pTxFrame->
+							       m_Data.m_Pres.
+							       m_le_bFlag1, 0);
+					}
+					// $$$ make function that updates Pres, StatusRes
+					// mark PRes frame as ready for transmission
+					Ret =
+					    EdrvTxMsgReady(&EplDllkInstance_g.
+							   m_pTxBuffer
+							   [EPL_DLLK_TXFRAME_PRES]);
+				}
+			}
+
+			break;
+		}
+#endif
+	default:
+		{
+			ASSERTMSG(FALSE,
+				  "EplDllkProcess(): unhandled event type!\n");
+		}
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkConfig
+//
+// Description: configure parameters of DLL
+//
+// Parameters:  pDllConfigParam_p       = configuration parameters
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkConfig(tEplDllConfigParam * pDllConfigParam_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+// d.k. check of NMT state disabled, because CycleLen is programmed at run time by MN without reset of CN
+/*tEplNmtState    NmtState;
+
+    NmtState = EplNmtkGetNmtState();
+
+    if (NmtState > kEplNmtGsResetConfiguration)
+    {   // only allowed in state DLL_GS_INIT
+        Ret = kEplInvalidOperation;
+        goto Exit;
+    }
+*/
+	EPL_MEMCPY(&EplDllkInstance_g.m_DllConfigParam, pDllConfigParam_p,
+		   (pDllConfigParam_p->m_uiSizeOfStruct <
+		    sizeof(tEplDllConfigParam) ? pDllConfigParam_p->
+		    m_uiSizeOfStruct : sizeof(tEplDllConfigParam)));
+
+	if ((EplDllkInstance_g.m_DllConfigParam.m_dwCycleLen != 0)
+	    && (EplDllkInstance_g.m_DllConfigParam.m_dwLossOfFrameTolerance != 0)) {	// monitor EPL cycle, calculate frame timeout
+		EplDllkInstance_g.m_ullFrameTimeout = (1000LL
+						       *
+						       ((unsigned long long)
+							EplDllkInstance_g.
+							m_DllConfigParam.
+							m_dwCycleLen))
+		    +
+		    ((unsigned long long)EplDllkInstance_g.m_DllConfigParam.
+		     m_dwLossOfFrameTolerance);
+	} else {
+		EplDllkInstance_g.m_ullFrameTimeout = 0LL;
+	}
+
+	if (EplDllkInstance_g.m_DllConfigParam.m_fAsyncOnly != FALSE) {	// it is configured as async-only CN
+		// disable multiplexed cycle, that m_uiCycleCount will not be incremented spuriously on SoC
+		EplDllkInstance_g.m_DllConfigParam.m_uiMultiplCycleCnt = 0;
+	}
+//Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkSetIdentity
+//
+// Description: configure identity of local node for IdentResponse
+//
+// Parameters:  pDllIdentParam_p        = identity
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkSetIdentity(tEplDllIdentParam * pDllIdentParam_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	EPL_MEMCPY(&EplDllkInstance_g.m_DllIdentParam, pDllIdentParam_p,
+		   (pDllIdentParam_p->m_uiSizeOfStruct <
+		    sizeof(tEplDllIdentParam) ? pDllIdentParam_p->
+		    m_uiSizeOfStruct : sizeof(tEplDllIdentParam)));
+
+	// $$$ if IdentResponse frame exists update it
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkRegAsyncHandler
+//
+// Description: registers handler for non-EPL frames
+//
+// Parameters:  pfnDllkCbAsync_p        = pointer to callback function
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkRegAsyncHandler(tEplDllkCbAsync pfnDllkCbAsync_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	if (EplDllkInstance_g.m_pfnCbAsync == NULL) {	// no handler registered yet
+		EplDllkInstance_g.m_pfnCbAsync = pfnDllkCbAsync_p;
+	} else {		// handler already registered
+		Ret = kEplDllCbAsyncRegistered;
+	}
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkDeregAsyncHandler
+//
+// Description: deregisters handler for non-EPL frames
+//
+// Parameters:  pfnDllkCbAsync_p        = pointer to callback function
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkDeregAsyncHandler(tEplDllkCbAsync pfnDllkCbAsync_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	if (EplDllkInstance_g.m_pfnCbAsync == pfnDllkCbAsync_p) {	// same handler is registered
+		// deregister it
+		EplDllkInstance_g.m_pfnCbAsync = NULL;
+	} else {		// wrong handler or no handler registered
+		Ret = kEplDllCbAsyncRegistered;
+	}
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkSetAsndServiceIdFilter()
+//
+// Description: sets the specified node ID filter for the specified
+//              AsndServiceId. It registers C_DLL_MULTICAST_ASND in ethernet
+//              driver if any AsndServiceId is open.
+//
+// Parameters:  ServiceId_p             = ASnd Service ID
+//              Filter_p                = node ID filter
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkSetAsndServiceIdFilter(tEplDllAsndServiceId ServiceId_p,
+					 tEplDllAsndFilter Filter_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	if (ServiceId_p < tabentries(EplDllkInstance_g.m_aAsndFilter)) {
+		EplDllkInstance_g.m_aAsndFilter[ServiceId_p] = Filter_p;
+	}
+
+	return Ret;
+}
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkSetFlag1OfNode()
+//
+// Description: sets Flag1 (for PReq and SoA) of the specified node ID.
+//
+// Parameters:  uiNodeId_p              = node ID
+//              bSoaFlag1_p             = flag1
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkSetFlag1OfNode(unsigned int uiNodeId_p, BYTE bSoaFlag1_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplDllkNodeInfo *pNodeInfo;
+
+	pNodeInfo = EplDllkGetNodeInfo(uiNodeId_p);
+	if (pNodeInfo == NULL) {	// no node info structure available
+		Ret = kEplDllNoNodeInfo;
+		goto Exit;
+	}
+	// store flag1 in internal node info structure
+	pNodeInfo->m_bSoaFlag1 = bSoaFlag1_p;
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkGetFirstNodeInfo()
+//
+// Description: returns first info structure of first node in isochronous phase.
+//              It is only useful for ErrorHandlerk module.
+//
+// Parameters:  ppNodeInfo_p            = pointer to pointer of internal node info structure
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkGetFirstNodeInfo(tEplDllkNodeInfo ** ppNodeInfo_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	*ppNodeInfo_p = EplDllkInstance_g.m_pFirstNodeInfo;
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkAddNode()
+//
+// Description: adds the specified node to the isochronous phase.
+//
+// Parameters:  pNodeInfo_p             = pointer of node info structure
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkAddNode(tEplDllNodeInfo * pNodeInfo_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplDllkNodeInfo *pIntNodeInfo;
+	tEplDllkNodeInfo **ppIntNodeInfo;
+	unsigned int uiHandle;
+	tEplFrame *pFrame;
+	unsigned int uiFrameSize;
+
+	pIntNodeInfo = EplDllkGetNodeInfo(pNodeInfo_p->m_uiNodeId);
+	if (pIntNodeInfo == NULL) {	// no node info structure available
+		Ret = kEplDllNoNodeInfo;
+		goto Exit;
+	}
+
+	EPL_DLLK_DBG_POST_TRACE_VALUE(kEplEventTypeDllkAddNode,
+				      pNodeInfo_p->m_uiNodeId, 0);
+
+	// copy node configuration
+	pIntNodeInfo->m_dwPresTimeout = pNodeInfo_p->m_dwPresTimeout;
+	pIntNodeInfo->m_wPresPayloadLimit = pNodeInfo_p->m_wPresPayloadLimit;
+
+	// $$$ d.k.: actually add node only if MN. On CN it is sufficient to update the node configuration
+	if (pNodeInfo_p->m_uiNodeId == EplDllkInstance_g.m_DllConfigParam.m_uiNodeId) {	// we shall send PRes ourself
+		// insert our node at the end of the list
+		ppIntNodeInfo = &EplDllkInstance_g.m_pFirstNodeInfo;
+		while ((*ppIntNodeInfo != NULL)
+		       && ((*ppIntNodeInfo)->m_pNextNodeInfo != NULL)) {
+			ppIntNodeInfo = &(*ppIntNodeInfo)->m_pNextNodeInfo;
+		}
+		if (*ppIntNodeInfo != NULL) {
+			if ((*ppIntNodeInfo)->m_uiNodeId == pNodeInfo_p->m_uiNodeId) {	// node was already added to list
+				// $$$ d.k. maybe this should be an error
+				goto Exit;
+			} else {	// add our node at the end of the list
+				ppIntNodeInfo =
+				    &(*ppIntNodeInfo)->m_pNextNodeInfo;
+			}
+		}
+		// set "PReq"-TxBuffer to PRes-TxBuffer
+		pIntNodeInfo->m_pPreqTxBuffer =
+		    &EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_PRES];
+	} else {		// normal CN shall be added to isochronous phase
+		// insert node into list in ascending order
+		ppIntNodeInfo = &EplDllkInstance_g.m_pFirstNodeInfo;
+		while ((*ppIntNodeInfo != NULL)
+		       && ((*ppIntNodeInfo)->m_uiNodeId <
+			   pNodeInfo_p->m_uiNodeId)
+		       && ((*ppIntNodeInfo)->m_uiNodeId !=
+			   EplDllkInstance_g.m_DllConfigParam.m_uiNodeId)) {
+			ppIntNodeInfo = &(*ppIntNodeInfo)->m_pNextNodeInfo;
+		}
+		if ((*ppIntNodeInfo != NULL) && ((*ppIntNodeInfo)->m_uiNodeId == pNodeInfo_p->m_uiNodeId)) {	// node was already added to list
+			// $$$ d.k. maybe this should be an error
+			goto Exit;
+		}
+	}
+
+	// initialize elements of internal node info structure
+	pIntNodeInfo->m_bSoaFlag1 = 0;
+	pIntNodeInfo->m_fSoftDelete = FALSE;
+	pIntNodeInfo->m_NmtState = kEplNmtCsNotActive;
+	if (pIntNodeInfo->m_pPreqTxBuffer == NULL) {	// create TxBuffer entry
+		uiFrameSize = pNodeInfo_p->m_wPreqPayloadLimit + 24;
+		Ret =
+		    EplDllkCreateTxFrame(&uiHandle, &pFrame, &uiFrameSize,
+					 kEplMsgTypePreq,
+					 kEplDllAsndNotDefined);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+		pIntNodeInfo->m_pPreqTxBuffer =
+		    &EplDllkInstance_g.m_pTxBuffer[uiHandle];
+		AmiSetByteToLe(&pFrame->m_le_bDstNodeId,
+			       (BYTE) pNodeInfo_p->m_uiNodeId);
+
+		// set up destination MAC address
+		EPL_MEMCPY(pFrame->m_be_abDstMac, pIntNodeInfo->m_be_abMacAddr,
+			   6);
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0)
+		{
+			tEplFrameInfo FrameInfo;
+
+			// initially encode TPDO -> inform PDO module
+			FrameInfo.m_pFrame = pFrame;
+			FrameInfo.m_uiFrameSize = uiFrameSize;
+			Ret = EplPdokCbPdoTransmitted(&FrameInfo);
+		}
+#endif
+	}
+	pIntNodeInfo->m_ulDllErrorEvents = 0L;
+	// add node to list
+	pIntNodeInfo->m_pNextNodeInfo = *ppIntNodeInfo;
+	*ppIntNodeInfo = pIntNodeInfo;
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkDeleteNode()
+//
+// Description: removes the specified node from the isochronous phase.
+//
+// Parameters:  uiNodeId_p              = node ID
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkDeleteNode(unsigned int uiNodeId_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplDllkNodeInfo *pIntNodeInfo;
+	tEplDllkNodeInfo **ppIntNodeInfo;
+	unsigned int uiHandle;
+
+	pIntNodeInfo = EplDllkGetNodeInfo(uiNodeId_p);
+	if (pIntNodeInfo == NULL) {	// no node info structure available
+		Ret = kEplDllNoNodeInfo;
+		goto Exit;
+	}
+
+	EPL_DLLK_DBG_POST_TRACE_VALUE(kEplEventTypeDllkDelNode, uiNodeId_p, 0);
+
+	// search node in whole list
+	ppIntNodeInfo = &EplDllkInstance_g.m_pFirstNodeInfo;
+	while ((*ppIntNodeInfo != NULL)
+	       && ((*ppIntNodeInfo)->m_uiNodeId != uiNodeId_p)) {
+		ppIntNodeInfo = &(*ppIntNodeInfo)->m_pNextNodeInfo;
+	}
+	if ((*ppIntNodeInfo == NULL) || ((*ppIntNodeInfo)->m_uiNodeId != uiNodeId_p)) {	// node was not found in list
+		// $$$ d.k. maybe this should be an error
+		goto Exit;
+	}
+	// remove node from list
+	*ppIntNodeInfo = pIntNodeInfo->m_pNextNodeInfo;
+
+	if ((pIntNodeInfo->m_pPreqTxBuffer != NULL)
+	    && (pIntNodeInfo->m_pPreqTxBuffer != &EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_PRES])) {	// delete TxBuffer entry
+		uiHandle =
+		    pIntNodeInfo->m_pPreqTxBuffer -
+		    EplDllkInstance_g.m_pTxBuffer;
+		pIntNodeInfo->m_pPreqTxBuffer = NULL;
+		Ret = EplDllkDeleteTxFrame(uiHandle);
+/*        if (Ret != kEplSuccessful)
+        {
+            goto Exit;
+        }*/
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkSoftDeleteNode()
+//
+// Description: removes the specified node not immediately from the isochronous phase.
+//              Instead the will be removed after error (late/loss PRes) without
+//              charging the error.
+//
+// Parameters:  uiNodeId_p              = node ID
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkSoftDeleteNode(unsigned int uiNodeId_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplDllkNodeInfo *pIntNodeInfo;
+
+	pIntNodeInfo = EplDllkGetNodeInfo(uiNodeId_p);
+	if (pIntNodeInfo == NULL) {	// no node info structure available
+		Ret = kEplDllNoNodeInfo;
+		goto Exit;
+	}
+
+	EPL_DLLK_DBG_POST_TRACE_VALUE(kEplEventTypeDllkSoftDelNode,
+				      uiNodeId_p, 0);
+
+	pIntNodeInfo->m_fSoftDelete = TRUE;
+
+      Exit:
+	return Ret;
+}
+
+#endif //(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkChangeState
+//
+// Description: change DLL state on event and diagnose some communication errors
+//
+// Parameters:  NmtEvent_p              = DLL event (wrapped in NMT event)
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplDllkChangeState(tEplNmtEvent NmtEvent_p,
+				     tEplNmtState NmtState_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplEvent Event;
+	tEplErrorHandlerkEvent DllEvent;
+
+	DllEvent.m_ulDllErrorEvents = 0;
+	DllEvent.m_uiNodeId = 0;
+	DllEvent.m_NmtState = NmtState_p;
+
+	switch (NmtState_p) {
+	case kEplNmtGsOff:
+	case kEplNmtGsInitialising:
+	case kEplNmtGsResetApplication:
+	case kEplNmtGsResetCommunication:
+	case kEplNmtGsResetConfiguration:
+	case kEplNmtCsBasicEthernet:
+		// enter DLL_GS_INIT
+		EplDllkInstance_g.m_DllState = kEplDllGsInit;
+		break;
+
+	case kEplNmtCsNotActive:
+	case kEplNmtCsPreOperational1:
+		// reduced EPL cycle is active
+		if (NmtEvent_p == kEplNmtEventDllCeSoc) {	// SoC received
+			// enter DLL_CS_WAIT_PREQ
+			EplDllkInstance_g.m_DllState = kEplDllCsWaitPreq;
+		} else {
+			// enter DLL_GS_INIT
+			EplDllkInstance_g.m_DllState = kEplDllGsInit;
+		}
+		break;
+
+	case kEplNmtCsPreOperational2:
+	case kEplNmtCsReadyToOperate:
+	case kEplNmtCsOperational:
+		// full EPL cycle is active
+
+		switch (EplDllkInstance_g.m_DllState) {
+		case kEplDllCsWaitPreq:
+			switch (NmtEvent_p) {
+				// DLL_CT2
+			case kEplNmtEventDllCePreq:
+				// enter DLL_CS_WAIT_SOA
+				DllEvent.m_ulDllErrorEvents |=
+				    EPL_DLL_ERR_CN_RECVD_PREQ;
+				EplDllkInstance_g.m_DllState = kEplDllCsWaitSoa;
+				break;
+
+				// DLL_CT8
+			case kEplNmtEventDllCeFrameTimeout:
+				if (NmtState_p == kEplNmtCsPreOperational2) {	// ignore frame timeout in PreOp2,
+					// because the previously configured cycle len
+					// may be wrong.
+					// 2008/10/15 d.k. If it would not be ignored,
+					// we would go cyclically to PreOp1 and on next
+					// SoC back to PreOp2.
+					break;
+				}
+				// report DLL_CEV_LOSS_SOC and DLL_CEV_LOSS_SOA
+				DllEvent.m_ulDllErrorEvents |=
+				    EPL_DLL_ERR_CN_LOSS_SOA |
+				    EPL_DLL_ERR_CN_LOSS_SOC;
+
+				// enter DLL_CS_WAIT_SOC
+				EplDllkInstance_g.m_DllState = kEplDllCsWaitSoc;
+				break;
+
+			case kEplNmtEventDllCeSoa:
+				// check if multiplexed and PReq should have been received in this cycle
+				// and if >= NMT_CS_READY_TO_OPERATE
+				if ((EplDllkInstance_g.m_uiCycleCount == 0)
+				    && (NmtState_p >= kEplNmtCsReadyToOperate)) {	// report DLL_CEV_LOSS_OF_PREQ
+					DllEvent.m_ulDllErrorEvents |=
+					    EPL_DLL_ERR_CN_LOSS_PREQ;
+				}
+				// enter DLL_CS_WAIT_SOC
+				EplDllkInstance_g.m_DllState = kEplDllCsWaitSoc;
+				break;
+
+				// DLL_CT7
+			case kEplNmtEventDllCeSoc:
+			case kEplNmtEventDllCeAsnd:
+				// report DLL_CEV_LOSS_SOA
+				DllEvent.m_ulDllErrorEvents |=
+				    EPL_DLL_ERR_CN_LOSS_SOA;
+
+			case kEplNmtEventDllCePres:
+			default:
+				// remain in this state
+				break;
+			}
+			break;
+
+		case kEplDllCsWaitSoc:
+			switch (NmtEvent_p) {
+				// DLL_CT1
+			case kEplNmtEventDllCeSoc:
+				// start of cycle and isochronous phase
+				// enter DLL_CS_WAIT_PREQ
+				EplDllkInstance_g.m_DllState =
+				    kEplDllCsWaitPreq;
+				break;
+
+				// DLL_CT4
+//                        case kEplNmtEventDllCePres:
+			case kEplNmtEventDllCeFrameTimeout:
+				if (NmtState_p == kEplNmtCsPreOperational2) {	// ignore frame timeout in PreOp2,
+					// because the previously configured cycle len
+					// may be wrong.
+					// 2008/10/15 d.k. If it would not be ignored,
+					// we would go cyclically to PreOp1 and on next
+					// SoC back to PreOp2.
+					break;
+				}
+				// fall through
+
+			case kEplNmtEventDllCePreq:
+			case kEplNmtEventDllCeSoa:
+				// report DLL_CEV_LOSS_SOC
+				DllEvent.m_ulDllErrorEvents |=
+				    EPL_DLL_ERR_CN_LOSS_SOC;
+
+			case kEplNmtEventDllCeAsnd:
+			default:
+				// remain in this state
+				break;
+			}
+			break;
+
+		case kEplDllCsWaitSoa:
+			switch (NmtEvent_p) {
+			case kEplNmtEventDllCeFrameTimeout:
+				// DLL_CT3
+				if (NmtState_p == kEplNmtCsPreOperational2) {	// ignore frame timeout in PreOp2,
+					// because the previously configured cycle len
+					// may be wrong.
+					// 2008/10/15 d.k. If it would not be ignored,
+					// we would go cyclically to PreOp1 and on next
+					// SoC back to PreOp2.
+					break;
+				}
+				// fall through
+
+			case kEplNmtEventDllCePreq:
+				// report DLL_CEV_LOSS_SOC and DLL_CEV_LOSS_SOA
+				DllEvent.m_ulDllErrorEvents |=
+				    EPL_DLL_ERR_CN_LOSS_SOA |
+				    EPL_DLL_ERR_CN_LOSS_SOC;
+
+			case kEplNmtEventDllCeSoa:
+				// enter DLL_CS_WAIT_SOC
+				EplDllkInstance_g.m_DllState = kEplDllCsWaitSoc;
+				break;
+
+				// DLL_CT9
+			case kEplNmtEventDllCeSoc:
+				// report DLL_CEV_LOSS_SOA
+				DllEvent.m_ulDllErrorEvents |=
+				    EPL_DLL_ERR_CN_LOSS_SOA;
+
+				// enter DLL_CS_WAIT_PREQ
+				EplDllkInstance_g.m_DllState =
+				    kEplDllCsWaitPreq;
+				break;
+
+				// DLL_CT10
+			case kEplNmtEventDllCeAsnd:
+				// report DLL_CEV_LOSS_SOA
+				DllEvent.m_ulDllErrorEvents |=
+				    EPL_DLL_ERR_CN_LOSS_SOA;
+
+			case kEplNmtEventDllCePres:
+			default:
+				// remain in this state
+				break;
+			}
+			break;
+
+		case kEplDllGsInit:
+			// enter DLL_CS_WAIT_PREQ
+			EplDllkInstance_g.m_DllState = kEplDllCsWaitPreq;
+			break;
+
+		default:
+			break;
+		}
+		break;
+
+	case kEplNmtCsStopped:
+		// full EPL cycle is active, but without PReq/PRes
+
+		switch (EplDllkInstance_g.m_DllState) {
+		case kEplDllCsWaitPreq:
+			switch (NmtEvent_p) {
+				// DLL_CT2
+			case kEplNmtEventDllCePreq:
+				// enter DLL_CS_WAIT_SOA
+				EplDllkInstance_g.m_DllState = kEplDllCsWaitSoa;
+				break;
+
+				// DLL_CT8
+			case kEplNmtEventDllCeFrameTimeout:
+				// report DLL_CEV_LOSS_SOC and DLL_CEV_LOSS_SOA
+				DllEvent.m_ulDllErrorEvents |=
+				    EPL_DLL_ERR_CN_LOSS_SOA |
+				    EPL_DLL_ERR_CN_LOSS_SOC;
+
+			case kEplNmtEventDllCeSoa:
+				// NMT_CS_STOPPED active
+				// it is Ok if no PReq was received
+
+				// enter DLL_CS_WAIT_SOC
+				EplDllkInstance_g.m_DllState = kEplDllCsWaitSoc;
+				break;
+
+				// DLL_CT7
+			case kEplNmtEventDllCeSoc:
+			case kEplNmtEventDllCeAsnd:
+				// report DLL_CEV_LOSS_SOA
+				DllEvent.m_ulDllErrorEvents |=
+				    EPL_DLL_ERR_CN_LOSS_SOA;
+
+			case kEplNmtEventDllCePres:
+			default:
+				// remain in this state
+				break;
+			}
+			break;
+
+		case kEplDllCsWaitSoc:
+			switch (NmtEvent_p) {
+				// DLL_CT1
+			case kEplNmtEventDllCeSoc:
+				// start of cycle and isochronous phase
+				// enter DLL_CS_WAIT_SOA
+				EplDllkInstance_g.m_DllState = kEplDllCsWaitSoa;
+				break;
+
+				// DLL_CT4
+//                        case kEplNmtEventDllCePres:
+			case kEplNmtEventDllCePreq:
+			case kEplNmtEventDllCeSoa:
+			case kEplNmtEventDllCeFrameTimeout:
+				// report DLL_CEV_LOSS_SOC
+				DllEvent.m_ulDllErrorEvents |=
+				    EPL_DLL_ERR_CN_LOSS_SOC;
+
+			case kEplNmtEventDllCeAsnd:
+			default:
+				// remain in this state
+				break;
+			}
+			break;
+
+		case kEplDllCsWaitSoa:
+			switch (NmtEvent_p) {
+				// DLL_CT3
+			case kEplNmtEventDllCeFrameTimeout:
+				// report DLL_CEV_LOSS_SOC and DLL_CEV_LOSS_SOA
+				DllEvent.m_ulDllErrorEvents |=
+				    EPL_DLL_ERR_CN_LOSS_SOA |
+				    EPL_DLL_ERR_CN_LOSS_SOC;
+
+			case kEplNmtEventDllCeSoa:
+				// enter DLL_CS_WAIT_SOC
+				EplDllkInstance_g.m_DllState = kEplDllCsWaitSoc;
+				break;
+
+				// DLL_CT9
+			case kEplNmtEventDllCeSoc:
+				// report DLL_CEV_LOSS_SOA
+				DllEvent.m_ulDllErrorEvents |=
+				    EPL_DLL_ERR_CN_LOSS_SOA;
+				// remain in DLL_CS_WAIT_SOA
+				break;
+
+				// DLL_CT10
+			case kEplNmtEventDllCeAsnd:
+				// report DLL_CEV_LOSS_SOA
+				DllEvent.m_ulDllErrorEvents |=
+				    EPL_DLL_ERR_CN_LOSS_SOA;
+
+			case kEplNmtEventDllCePreq:
+				// NMT_CS_STOPPED active and we do not expect any PReq
+				// so just ignore it
+			case kEplNmtEventDllCePres:
+			default:
+				// remain in this state
+				break;
+			}
+			break;
+
+		case kEplDllGsInit:
+		default:
+			// enter DLL_CS_WAIT_PREQ
+			EplDllkInstance_g.m_DllState = kEplDllCsWaitSoa;
+			break;
+		}
+		break;
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+	case kEplNmtMsNotActive:
+	case kEplNmtMsBasicEthernet:
+		break;
+
+	case kEplNmtMsPreOperational1:
+		// reduced EPL cycle is active
+		if (EplDllkInstance_g.m_DllState != kEplDllMsNonCyclic) {	// stop cycle timer
+#if EPL_TIMER_USE_HIGHRES != FALSE
+			Ret =
+			    EplTimerHighReskDeleteTimer(&EplDllkInstance_g.
+							m_TimerHdlCycle);
+#endif
+			EplDllkInstance_g.m_DllState = kEplDllMsNonCyclic;
+
+			// stop further processing,
+			// because it will be restarted by NMT MN module
+			break;
+		}
+
+		switch (NmtEvent_p) {
+		case kEplNmtEventDllMeSocTrig:
+		case kEplNmtEventDllCeAsnd:
+			{	// because of reduced EPL cycle SoA shall be triggered, not SoC
+				tEplDllState DummyDllState;
+
+				Ret =
+				    EplDllkAsyncFrameNotReceived
+				    (EplDllkInstance_g.m_LastReqServiceId,
+				     EplDllkInstance_g.m_uiLastTargetNodeId);
+
+				// go ahead and send SoA
+				Ret = EplDllkMnSendSoa(NmtState_p,
+						       &DummyDllState,
+						       (EplDllkInstance_g.
+							m_uiCycleCount >=
+							EPL_C_DLL_PREOP1_START_CYCLES));
+				// increment cycle counter to detect if EPL_C_DLL_PREOP1_START_CYCLES empty cycles are elapsed
+				EplDllkInstance_g.m_uiCycleCount++;
+
+				// reprogram timer
+#if EPL_TIMER_USE_HIGHRES != FALSE
+				if (EplDllkInstance_g.m_DllConfigParam.
+				    m_dwAsyncSlotTimeout != 0) {
+					Ret =
+					    EplTimerHighReskModifyTimerNs
+					    (&EplDllkInstance_g.m_TimerHdlCycle,
+					     EplDllkInstance_g.m_DllConfigParam.
+					     m_dwAsyncSlotTimeout,
+					     EplDllkCbMnTimerCycle, 0L, FALSE);
+				}
+#endif
+				break;
+			}
+
+		default:
+			break;
+		}
+		break;
+
+	case kEplNmtMsPreOperational2:
+	case kEplNmtMsReadyToOperate:
+	case kEplNmtMsOperational:
+		// full EPL cycle is active
+		switch (NmtEvent_p) {
+		case kEplNmtEventDllMeSocTrig:
+			{
+				// update cycle counter
+				if (EplDllkInstance_g.m_DllConfigParam.m_uiMultiplCycleCnt > 0) {	// multiplexed cycle active
+					EplDllkInstance_g.m_uiCycleCount =
+					    (EplDllkInstance_g.m_uiCycleCount +
+					     1) %
+					    EplDllkInstance_g.m_DllConfigParam.
+					    m_uiMultiplCycleCnt;
+					// $$$ check multiplexed cycle restart
+					//     -> toggle MC flag
+					//     -> change node linked list
+				} else {	// non-multiplexed cycle active
+					// start with first node in isochronous phase
+					EplDllkInstance_g.m_pCurNodeInfo = NULL;
+				}
+
+				switch (EplDllkInstance_g.m_DllState) {
+				case kEplDllMsNonCyclic:
+					{	// start continuous cycle timer
+#if EPL_TIMER_USE_HIGHRES != FALSE
+						Ret =
+						    EplTimerHighReskModifyTimerNs
+						    (&EplDllkInstance_g.
+						     m_TimerHdlCycle,
+						     EplDllkInstance_g.
+						     m_ullFrameTimeout,
+						     EplDllkCbMnTimerCycle, 0L,
+						     TRUE);
+#endif
+						// continue with sending SoC
+					}
+
+				case kEplDllMsWaitAsnd:
+				case kEplDllMsWaitSocTrig:
+					{	// if m_LastReqServiceId is still valid,
+						// SoA was not correctly answered
+						// and user part has to be informed
+						Ret =
+						    EplDllkAsyncFrameNotReceived
+						    (EplDllkInstance_g.
+						     m_LastReqServiceId,
+						     EplDllkInstance_g.
+						     m_uiLastTargetNodeId);
+
+						// send SoC
+						Ret = EplDllkMnSendSoc();
+
+						// new DLL state
+						EplDllkInstance_g.m_DllState =
+						    kEplDllMsWaitPreqTrig;
+
+						// start WaitSoCPReq Timer
+#if EPL_TIMER_USE_HIGHRES != FALSE
+						Ret =
+						    EplTimerHighReskModifyTimerNs
+						    (&EplDllkInstance_g.
+						     m_TimerHdlResponse,
+						     EplDllkInstance_g.
+						     m_DllConfigParam.
+						     m_dwWaitSocPreq,
+						     EplDllkCbMnTimerResponse,
+						     0L, FALSE);
+#endif
+						break;
+					}
+
+				default:
+					{	// wrong DLL state / cycle time exceeded
+						DllEvent.m_ulDllErrorEvents |=
+						    EPL_DLL_ERR_MN_CYCTIMEEXCEED;
+						EplDllkInstance_g.m_DllState =
+						    kEplDllMsWaitSocTrig;
+						break;
+					}
+				}
+
+				break;
+			}
+
+		case kEplNmtEventDllMePresTimeout:
+			{
+
+				switch (EplDllkInstance_g.m_DllState) {
+				case kEplDllMsWaitPres:
+					{	// PRes not received
+
+						if (EplDllkInstance_g.m_pCurNodeInfo->m_fSoftDelete == FALSE) {	// normal isochronous CN
+							DllEvent.
+							    m_ulDllErrorEvents
+							    |=
+							    EPL_DLL_ERR_MN_CN_LOSS_PRES;
+							DllEvent.m_uiNodeId =
+							    EplDllkInstance_g.
+							    m_pCurNodeInfo->
+							    m_uiNodeId;
+						} else {	// CN shall be deleted softly
+							Event.m_EventSink =
+							    kEplEventSinkDllkCal;
+							Event.m_EventType =
+							    kEplEventTypeDllkSoftDelNode;
+							// $$$ d.k. set Event.m_NetTime to current time
+							Event.m_uiSize =
+							    sizeof(unsigned
+								   int);
+							Event.m_pArg =
+							    &EplDllkInstance_g.
+							    m_pCurNodeInfo->
+							    m_uiNodeId;
+							Ret =
+							    EplEventkPost
+							    (&Event);
+						}
+
+						// continue with sending next PReq
+					}
+
+				case kEplDllMsWaitPreqTrig:
+					{
+						// send next PReq
+						Ret =
+						    EplDllkMnSendPreq
+						    (NmtState_p,
+						     &EplDllkInstance_g.
+						     m_DllState);
+
+						break;
+					}
+
+				default:
+					{	// wrong DLL state
+						break;
+					}
+				}
+
+				break;
+			}
+
+		case kEplNmtEventDllCePres:
+			{
+
+				switch (EplDllkInstance_g.m_DllState) {
+				case kEplDllMsWaitPres:
+					{	// PRes received
+						// send next PReq
+						Ret =
+						    EplDllkMnSendPreq
+						    (NmtState_p,
+						     &EplDllkInstance_g.
+						     m_DllState);
+
+						break;
+					}
+
+				default:
+					{	// wrong DLL state
+						break;
+					}
+				}
+
+				break;
+			}
+
+		case kEplNmtEventDllMeSoaTrig:
+			{
+
+				switch (EplDllkInstance_g.m_DllState) {
+				case kEplDllMsWaitSoaTrig:
+					{	// MN PRes sent
+						// send SoA
+						Ret =
+						    EplDllkMnSendSoa(NmtState_p,
+								     &EplDllkInstance_g.
+								     m_DllState,
+								     TRUE);
+
+						break;
+					}
+
+				default:
+					{	// wrong DLL state
+						break;
+					}
+				}
+
+				break;
+			}
+
+		case kEplNmtEventDllCeAsnd:
+			{	// ASnd has been received, but it may be not the requested one
+/*
+                    // report if SoA was correctly answered
+                    Ret = EplDllkAsyncFrameNotReceived(EplDllkInstance_g.m_LastReqServiceId,
+                                                       EplDllkInstance_g.m_uiLastTargetNodeId);
+*/
+				if (EplDllkInstance_g.m_DllState ==
+				    kEplDllMsWaitAsnd) {
+					EplDllkInstance_g.m_DllState =
+					    kEplDllMsWaitSocTrig;
+				}
+				break;
+			}
+
+		default:
+			break;
+		}
+		break;
+#endif //(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+	default:
+		break;
+	}
+
+	if (DllEvent.m_ulDllErrorEvents != 0) {	// error event set -> post it to error handler
+		Event.m_EventSink = kEplEventSinkErrk;
+		Event.m_EventType = kEplEventTypeDllError;
+		// $$$ d.k. set Event.m_NetTime to current time
+		Event.m_uiSize = sizeof(DllEvent);
+		Event.m_pArg = &DllEvent;
+		Ret = EplEventkPost(&Event);
+	}
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCbFrameReceived()
+//
+// Description: called from EdrvInterruptHandler()
+//
+// Parameters:  pRxBuffer_p             = receive buffer structure
+//
+// Returns:     (none)
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static void EplDllkCbFrameReceived(tEdrvRxBuffer * pRxBuffer_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplNmtState NmtState;
+	tEplNmtEvent NmtEvent = kEplNmtEventNoEvent;
+	tEplEvent Event;
+	tEplFrame *pFrame;
+	tEplFrame *pTxFrame;
+	tEdrvTxBuffer *pTxBuffer = NULL;
+	tEplFrameInfo FrameInfo;
+	tEplMsgType MsgType;
+	tEplDllReqServiceId ReqServiceId;
+	unsigned int uiAsndServiceId;
+	unsigned int uiNodeId;
+	BYTE bFlag1;
+
+	BENCHMARK_MOD_02_SET(3);
+	NmtState = EplNmtkGetNmtState();
+
+	if (NmtState <= kEplNmtGsResetConfiguration) {
+		goto Exit;
+	}
+
+	pFrame = (tEplFrame *) pRxBuffer_p->m_pbBuffer;
+
+#if EDRV_EARLY_RX_INT != FALSE
+	switch (pRxBuffer_p->m_BufferInFrame) {
+	case kEdrvBufferFirstInFrame:
+		{
+			MsgType =
+			    (tEplMsgType) AmiGetByteFromLe(&pFrame->
+							   m_le_bMessageType);
+			if (MsgType == kEplMsgTypePreq) {
+				if (EplDllkInstance_g.m_DllState == kEplDllCsWaitPreq) {	// PReq expected and actually received
+					// d.k.: The condition above is sufficent, because EPL cycle is active
+					//       and no non-EPL frame shall be received in isochronous phase.
+					// start transmission PRes
+					// $$$ What if Tx buffer is invalid?
+					pTxBuffer =
+					    &EplDllkInstance_g.
+					    m_pTxBuffer[EPL_DLLK_TXFRAME_PRES];
+#if (EPL_DLL_PRES_READY_AFTER_SOA != FALSE) || (EPL_DLL_PRES_READY_AFTER_SOC != FALSE)
+					Ret = EdrvTxMsgStart(pTxBuffer);
+#else
+					pTxFrame =
+					    (tEplFrame *) pTxBuffer->m_pbBuffer;
+					// update frame (NMT state, RD, RS, PR, MS, EN flags)
+					AmiSetByteToLe(&pTxFrame->m_Data.m_Pres.
+						       m_le_bNmtStatus,
+						       (BYTE) NmtState);
+					AmiSetByteToLe(&pTxFrame->m_Data.m_Pres.
+						       m_le_bFlag2,
+						       EplDllkInstance_g.
+						       m_bFlag2);
+					if (NmtState != kEplNmtCsOperational) {	// mark PDO as invalid in NMT state Op
+						// $$$ reset only RD flag; set other flags appropriately
+						AmiSetByteToLe(&pTxFrame->
+							       m_Data.m_Pres.
+							       m_le_bFlag1, 0);
+					}
+					// $$$ make function that updates Pres, StatusRes
+					// send PRes frame
+					Ret = EdrvSendTxMsg(pTxBuffer);
+#endif
+				}
+			}
+			goto Exit;
+		}
+
+	case kEdrvBufferMiddleInFrame:
+		{
+			goto Exit;
+		}
+
+	case kEdrvBufferLastInFrame:
+		{
+			break;
+		}
+	}
+#endif
+
+	FrameInfo.m_pFrame = pFrame;
+	FrameInfo.m_uiFrameSize = pRxBuffer_p->m_uiRxMsgLen;
+	FrameInfo.m_NetTime.m_dwNanoSec = pRxBuffer_p->m_NetTime.m_dwNanoSec;
+	FrameInfo.m_NetTime.m_dwSec = pRxBuffer_p->m_NetTime.m_dwSec;
+
+	if (AmiGetWordFromBe(&pFrame->m_be_wEtherType) != EPL_C_DLL_ETHERTYPE_EPL) {	// non-EPL frame
+		//TRACE2("EplDllkCbFrameReceived: pfnCbAsync=0x%p SrcMAC=0x%llx\n", EplDllkInstance_g.m_pfnCbAsync, AmiGetQword48FromBe(pFrame->m_be_abSrcMac));
+		if (EplDllkInstance_g.m_pfnCbAsync != NULL) {	// handler for async frames is registered
+			EplDllkInstance_g.m_pfnCbAsync(&FrameInfo);
+		}
+
+		goto Exit;
+	}
+
+	MsgType = (tEplMsgType) AmiGetByteFromLe(&pFrame->m_le_bMessageType);
+	switch (MsgType) {
+	case kEplMsgTypePreq:
+		{
+			// PReq frame
+			// d.k.: (we assume that this PReq frame is intended for us and don't check DstNodeId)
+			if (AmiGetByteFromLe(&pFrame->m_le_bDstNodeId) != EplDllkInstance_g.m_DllConfigParam.m_uiNodeId) {	// this PReq is not intended for us
+				goto Exit;
+			}
+			NmtEvent = kEplNmtEventDllCePreq;
+
+			if (NmtState >= kEplNmtMsNotActive) {	// MN is active -> wrong msg type
+				break;
+			}
+#if EDRV_EARLY_RX_INT == FALSE
+			if (NmtState >= kEplNmtCsPreOperational2) {	// respond to and process PReq frames only in PreOp2, ReadyToOp and Op
+				// Does PRes exist?
+				pTxBuffer =
+				    &EplDllkInstance_g.
+				    m_pTxBuffer[EPL_DLLK_TXFRAME_PRES];
+				if (pTxBuffer->m_pbBuffer != NULL) {	// PRes does exist
+#if (EPL_DLL_PRES_READY_AFTER_SOA != FALSE) || (EPL_DLL_PRES_READY_AFTER_SOC != FALSE)
+					EdrvTxMsgStart(pTxBuffer);
+#else
+					pTxFrame =
+					    (tEplFrame *) pTxBuffer->m_pbBuffer;
+					// update frame (NMT state, RD, RS, PR, MS, EN flags)
+					AmiSetByteToLe(&pTxFrame->m_Data.m_Pres.
+						       m_le_bNmtStatus,
+						       (BYTE) NmtState);
+					AmiSetByteToLe(&pTxFrame->m_Data.m_Pres.
+						       m_le_bFlag2,
+						       EplDllkInstance_g.
+						       m_bFlag2);
+					bFlag1 =
+					    AmiGetByteFromLe(&pFrame->m_Data.
+							     m_Preq.
+							     m_le_bFlag1);
+					// save EA flag
+					EplDllkInstance_g.m_bMnFlag1 =
+					    (EplDllkInstance_g.
+					     m_bMnFlag1 & ~EPL_FRAME_FLAG1_EA)
+					    | (bFlag1 & EPL_FRAME_FLAG1_EA);
+					// preserve MS flag
+					bFlag1 &= EPL_FRAME_FLAG1_MS;
+					// add EN flag from Error signaling module
+					bFlag1 |=
+					    EplDllkInstance_g.
+					    m_bFlag1 & EPL_FRAME_FLAG1_EN;
+					if (NmtState != kEplNmtCsOperational) {	// mark PDO as invalid in NMT state Op
+						// reset only RD flag
+						AmiSetByteToLe(&pTxFrame->
+							       m_Data.m_Pres.
+							       m_le_bFlag1,
+							       bFlag1);
+					} else {	// leave RD flag untouched
+						AmiSetByteToLe(&pTxFrame->
+							       m_Data.m_Pres.
+							       m_le_bFlag1,
+							       (AmiGetByteFromLe
+								(&pTxFrame->
+								 m_Data.m_Pres.
+								 m_le_bFlag1) &
+								EPL_FRAME_FLAG1_RD)
+							       | bFlag1);
+					}
+					// $$$ update EPL_DLL_PRES_READY_AFTER_* code
+					// send PRes frame
+					Ret = EdrvSendTxMsg(pTxBuffer);
+					if (Ret != kEplSuccessful) {
+						goto Exit;
+					}
+#endif
+				}
+#endif
+				// inform PDO module
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0)
+				if (NmtState >= kEplNmtCsReadyToOperate) {	// inform PDO module only in ReadyToOp and Op
+					if (NmtState != kEplNmtCsOperational) {
+						// reset RD flag and all other flags, but that does not matter, because they were processed above
+						AmiSetByteToLe(&pFrame->m_Data.
+							       m_Preq.
+							       m_le_bFlag1, 0);
+					}
+					// compares real frame size and PDO size
+					if ((unsigned
+					     int)(AmiGetWordFromLe(&pFrame->
+								   m_Data.
+								   m_Preq.
+								   m_le_wSize) +
+						  24)
+					    > FrameInfo.m_uiFrameSize) {	// format error
+						tEplErrorHandlerkEvent DllEvent;
+
+						DllEvent.m_ulDllErrorEvents =
+						    EPL_DLL_ERR_INVALID_FORMAT;
+						DllEvent.m_uiNodeId =
+						    AmiGetByteFromLe(&pFrame->
+								     m_le_bSrcNodeId);
+						DllEvent.m_NmtState = NmtState;
+						Event.m_EventSink =
+						    kEplEventSinkErrk;
+						Event.m_EventType =
+						    kEplEventTypeDllError;
+						Event.m_NetTime =
+						    FrameInfo.m_NetTime;
+						Event.m_uiSize =
+						    sizeof(DllEvent);
+						Event.m_pArg = &DllEvent;
+						Ret = EplEventkPost(&Event);
+						break;
+					}
+					// forward PReq frame as RPDO to PDO module
+					Ret = EplPdokCbPdoReceived(&FrameInfo);
+
+				}
+#if (EPL_DLL_PRES_READY_AFTER_SOC != FALSE)
+				if (pTxBuffer->m_pbBuffer != NULL) {	// PRes does exist
+					// inform PDO module about PRes after PReq
+					FrameInfo.m_pFrame =
+					    (tEplFrame *) pTxBuffer->m_pbBuffer;
+					FrameInfo.m_uiFrameSize =
+					    pTxBuffer->m_uiMaxBufferLen;
+					Ret =
+					    EplPdokCbPdoTransmitted(&FrameInfo);
+				}
+#endif
+#endif
+
+#if EDRV_EARLY_RX_INT == FALSE
+				// $$$ inform emergency protocol handling (error signaling module) about flags
+			}
+#endif
+
+			// reset cycle counter
+			EplDllkInstance_g.m_uiCycleCount = 0;
+
+			break;
+		}
+
+	case kEplMsgTypePres:
+		{
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+			tEplDllkNodeInfo *pIntNodeInfo;
+			tEplHeartbeatEvent HeartbeatEvent;
+#endif
+
+			// PRes frame
+			NmtEvent = kEplNmtEventDllCePres;
+
+			uiNodeId = AmiGetByteFromLe(&pFrame->m_le_bSrcNodeId);
+
+			if ((NmtState >= kEplNmtCsPreOperational2)
+			    && (NmtState <= kEplNmtCsOperational)) {	// process PRes frames only in PreOp2, ReadyToOp and Op of CN
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+				pIntNodeInfo = EplDllkGetNodeInfo(uiNodeId);
+				if (pIntNodeInfo == NULL) {	// no node info structure available
+					Ret = kEplDllNoNodeInfo;
+					goto Exit;
+				}
+			} else if (EplDllkInstance_g.m_DllState == kEplDllMsWaitPres) {	// or process PRes frames in MsWaitPres
+
+				pIntNodeInfo = EplDllkInstance_g.m_pCurNodeInfo;
+				if ((pIntNodeInfo == NULL) || (pIntNodeInfo->m_uiNodeId != uiNodeId)) {	// ignore PRes, because it is from wrong CN
+					// $$$ maybe post event to NmtMn module
+					goto Exit;
+				}
+				// forward Flag2 to asynchronous scheduler
+				bFlag1 =
+				    AmiGetByteFromLe(&pFrame->m_Data.m_Asnd.
+						     m_Payload.m_StatusResponse.
+						     m_le_bFlag2);
+				Ret =
+				    EplDllkCalAsyncSetPendingRequests(uiNodeId,
+								      ((tEplDllAsyncReqPriority) ((bFlag1 & EPL_FRAME_FLAG2_PR) >> EPL_FRAME_FLAG2_PR_SHIFT)), (bFlag1 & EPL_FRAME_FLAG2_RS));
+
+#endif
+			} else {	// ignore PRes, because it was received in wrong NMT state
+				// but execute EplDllkChangeState() and post event to NMT module
+				break;
+			}
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+			{	// check NMT state of CN
+				HeartbeatEvent.m_wErrorCode = EPL_E_NO_ERROR;
+				HeartbeatEvent.m_NmtState =
+				    (tEplNmtState) (AmiGetByteFromLe
+						    (&pFrame->m_Data.m_Pres.
+						     m_le_bNmtStatus) |
+						    EPL_NMT_TYPE_CS);
+				if (pIntNodeInfo->m_NmtState != HeartbeatEvent.m_NmtState) {	// NMT state of CN has changed -> post event to NmtMnu module
+					if (pIntNodeInfo->m_fSoftDelete == FALSE) {	// normal isochronous CN
+						HeartbeatEvent.m_uiNodeId =
+						    uiNodeId;
+						Event.m_EventSink =
+						    kEplEventSinkNmtMnu;
+						Event.m_EventType =
+						    kEplEventTypeHeartbeat;
+						Event.m_uiSize =
+						    sizeof(HeartbeatEvent);
+						Event.m_pArg = &HeartbeatEvent;
+					} else {	// CN shall be deleted softly
+						Event.m_EventSink =
+						    kEplEventSinkDllkCal;
+						Event.m_EventType =
+						    kEplEventTypeDllkSoftDelNode;
+						Event.m_uiSize =
+						    sizeof(unsigned int);
+						Event.m_pArg =
+						    &pIntNodeInfo->m_uiNodeId;
+					}
+					Event.m_NetTime = FrameInfo.m_NetTime;
+					Ret = EplEventkPost(&Event);
+
+					// save current NMT state of CN in internal node structure
+					pIntNodeInfo->m_NmtState =
+					    HeartbeatEvent.m_NmtState;
+				}
+			}
+#endif
+
+			// inform PDO module
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0)
+			if ((NmtState != kEplNmtCsPreOperational2)
+			    && (NmtState != kEplNmtMsPreOperational2)) {	// inform PDO module only in ReadyToOp and Op
+				// compare real frame size and PDO size?
+				if (((unsigned
+				      int)(AmiGetWordFromLe(&pFrame->m_Data.
+							    m_Pres.m_le_wSize) +
+					   24)
+				     > FrameInfo.m_uiFrameSize)
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+				    ||
+				    (AmiGetWordFromLe
+				     (&pFrame->m_Data.m_Pres.m_le_wSize) >
+				     pIntNodeInfo->m_wPresPayloadLimit)
+#endif
+				    ) {	// format error
+					tEplErrorHandlerkEvent DllEvent;
+
+					DllEvent.m_ulDllErrorEvents =
+					    EPL_DLL_ERR_INVALID_FORMAT;
+					DllEvent.m_uiNodeId = uiNodeId;
+					DllEvent.m_NmtState = NmtState;
+					Event.m_EventSink = kEplEventSinkErrk;
+					Event.m_EventType =
+					    kEplEventTypeDllError;
+					Event.m_NetTime = FrameInfo.m_NetTime;
+					Event.m_uiSize = sizeof(DllEvent);
+					Event.m_pArg = &DllEvent;
+					Ret = EplEventkPost(&Event);
+					break;
+				}
+				if ((NmtState != kEplNmtCsOperational)
+				    && (NmtState != kEplNmtMsOperational)) {
+					// reset RD flag and all other flags, but that does not matter, because they were processed above
+					AmiSetByteToLe(&pFrame->m_Data.m_Pres.
+						       m_le_bFlag1, 0);
+				}
+				Ret = EplPdokCbPdoReceived(&FrameInfo);
+			}
+#endif
+
+			break;
+		}
+
+	case kEplMsgTypeSoc:
+		{
+			// SoC frame
+			NmtEvent = kEplNmtEventDllCeSoc;
+
+			if (NmtState >= kEplNmtMsNotActive) {	// MN is active -> wrong msg type
+				break;
+			}
+#if EPL_DLL_PRES_READY_AFTER_SOC != FALSE
+			// post PRes to transmit FIFO of the ethernet controller, but don't start
+			// transmission over bus
+			pTxBuffer =
+			    &EplDllkInstance_g.
+			    m_pTxBuffer[EPL_DLLK_TXFRAME_PRES];
+			// Does PRes exist?
+			if (pTxBuffer->m_pbBuffer != NULL) {	// PRes does exist
+				pTxFrame = (tEplFrame *) pTxBuffer->m_pbBuffer;
+				// update frame (NMT state, RD, RS, PR, MS, EN flags)
+				if (NmtState < kEplNmtCsPreOperational2) {	// NMT state is not PreOp2, ReadyToOp or Op
+					// fake NMT state PreOp2, because PRes will be sent only in PreOp2 or greater
+					NmtState = kEplNmtCsPreOperational2;
+				}
+				AmiSetByteToLe(&pTxFrame->m_Data.m_Pres.
+					       m_le_bNmtStatus,
+					       (BYTE) NmtState);
+				AmiSetByteToLe(&pTxFrame->m_Data.m_Pres.
+					       m_le_bFlag2,
+					       EplDllkInstance_g.m_bFlag2);
+				if (NmtState != kEplNmtCsOperational) {	// mark PDO as invalid in NMT state Op
+					// $$$ reset only RD flag; set other flags appropriately
+					AmiSetByteToLe(&pTxFrame->m_Data.m_Pres.
+						       m_le_bFlag1, 0);
+				}
+				// $$$ make function that updates Pres, StatusRes
+				// mark PRes frame as ready for transmission
+				Ret = EdrvTxMsgReady(pTxBuffer);
+			}
+#endif
+
+			if (NmtState >= kEplNmtCsPreOperational2) {	// SoC frames only in PreOp2, ReadyToOp and Op
+				// trigger synchronous task
+				Event.m_EventSink = kEplEventSinkSync;
+				Event.m_EventType = kEplEventTypeSync;
+				Event.m_uiSize = 0;
+				Ret = EplEventkPost(&Event);
+
+				// update cycle counter
+				if (EplDllkInstance_g.m_DllConfigParam.m_uiMultiplCycleCnt > 0) {	// multiplexed cycle active
+					EplDllkInstance_g.m_uiCycleCount =
+					    (EplDllkInstance_g.m_uiCycleCount +
+					     1) %
+					    EplDllkInstance_g.m_DllConfigParam.
+					    m_uiMultiplCycleCnt;
+				}
+			}
+			// reprogram timer
+#if EPL_TIMER_USE_HIGHRES != FALSE
+			if (EplDllkInstance_g.m_ullFrameTimeout != 0) {
+				Ret =
+				    EplTimerHighReskModifyTimerNs
+				    (&EplDllkInstance_g.m_TimerHdlCycle,
+				     EplDllkInstance_g.m_ullFrameTimeout,
+				     EplDllkCbCnTimer, 0L, FALSE);
+			}
+#endif
+
+			break;
+		}
+
+	case kEplMsgTypeSoa:
+		{
+			// SoA frame
+			NmtEvent = kEplNmtEventDllCeSoa;
+
+			if (NmtState >= kEplNmtMsNotActive) {	// MN is active -> wrong msg type
+				break;
+			}
+
+			pTxFrame = NULL;
+
+			if ((NmtState & EPL_NMT_SUPERSTATE_MASK) != EPL_NMT_CS_EPLMODE) {	// do not respond, if NMT state is < PreOp1 (i.e. not EPL_MODE)
+				break;
+			}
+			// check TargetNodeId
+			uiNodeId =
+			    AmiGetByteFromLe(&pFrame->m_Data.m_Soa.
+					     m_le_bReqServiceTarget);
+			if (uiNodeId == EplDllkInstance_g.m_DllConfigParam.m_uiNodeId) {	// local node is the target of the current request
+
+				// check ServiceId
+				ReqServiceId =
+				    (tEplDllReqServiceId)
+				    AmiGetByteFromLe(&pFrame->m_Data.m_Soa.
+						     m_le_bReqServiceId);
+				if (ReqServiceId == kEplDllReqServiceStatus) {	// StatusRequest
+					if (EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_STATUSRES].m_pbBuffer != NULL) {	// StatusRes does exist
+
+						pTxFrame =
+						    (tEplFrame *)
+						    EplDllkInstance_g.
+						    m_pTxBuffer
+						    [EPL_DLLK_TXFRAME_STATUSRES].
+						    m_pbBuffer;
+						// update StatusRes frame (NMT state, EN, EC, RS, PR flags)
+						AmiSetByteToLe(&pTxFrame->
+							       m_Data.m_Asnd.
+							       m_Payload.
+							       m_StatusResponse.
+							       m_le_bNmtStatus,
+							       (BYTE) NmtState);
+						AmiSetByteToLe(&pTxFrame->
+							       m_Data.m_Asnd.
+							       m_Payload.
+							       m_StatusResponse.
+							       m_le_bFlag1,
+							       EplDllkInstance_g.
+							       m_bFlag1);
+						AmiSetByteToLe(&pTxFrame->
+							       m_Data.m_Asnd.
+							       m_Payload.
+							       m_StatusResponse.
+							       m_le_bFlag2,
+							       EplDllkInstance_g.
+							       m_bFlag2);
+						// send StatusRes
+						Ret =
+						    EdrvSendTxMsg
+						    (&EplDllkInstance_g.
+						     m_pTxBuffer
+						     [EPL_DLLK_TXFRAME_STATUSRES]);
+						if (Ret != kEplSuccessful) {
+							goto Exit;
+						}
+						TGT_DBG_SIGNAL_TRACE_POINT(8);
+
+						// update error signaling
+						bFlag1 =
+						    AmiGetByteFromLe(&pFrame->
+								     m_Data.
+								     m_Soa.
+								     m_le_bFlag1);
+						if (((bFlag1 ^ EplDllkInstance_g.m_bMnFlag1) & EPL_FRAME_FLAG1_ER) != 0) {	// exception reset flag was changed by MN
+							// assume same state for EC in next cycle (clear all other bits)
+							if ((bFlag1 &
+							     EPL_FRAME_FLAG1_ER)
+							    != 0) {
+								// set EC and reset rest
+								EplDllkInstance_g.
+								    m_bFlag1 =
+								    EPL_FRAME_FLAG1_EC;
+							} else {
+								// reset complete flag 1 (including EC and EN)
+								EplDllkInstance_g.
+								    m_bFlag1 =
+								    0;
+							}
+						}
+						// save flag 1 from MN for Status request response cycle
+						EplDllkInstance_g.m_bMnFlag1 =
+						    bFlag1;
+					}
+				} else if (ReqServiceId == kEplDllReqServiceIdent) {	// IdentRequest
+					if (EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_IDENTRES].m_pbBuffer != NULL) {	// IdentRes does exist
+						pTxFrame =
+						    (tEplFrame *)
+						    EplDllkInstance_g.
+						    m_pTxBuffer
+						    [EPL_DLLK_TXFRAME_IDENTRES].
+						    m_pbBuffer;
+						// update IdentRes frame (NMT state, RS, PR flags)
+						AmiSetByteToLe(&pTxFrame->
+							       m_Data.m_Asnd.
+							       m_Payload.
+							       m_IdentResponse.
+							       m_le_bNmtStatus,
+							       (BYTE) NmtState);
+						AmiSetByteToLe(&pTxFrame->
+							       m_Data.m_Asnd.
+							       m_Payload.
+							       m_IdentResponse.
+							       m_le_bFlag2,
+							       EplDllkInstance_g.
+							       m_bFlag2);
+						// send IdentRes
+						Ret =
+						    EdrvSendTxMsg
+						    (&EplDllkInstance_g.
+						     m_pTxBuffer
+						     [EPL_DLLK_TXFRAME_IDENTRES]);
+						if (Ret != kEplSuccessful) {
+							goto Exit;
+						}
+						TGT_DBG_SIGNAL_TRACE_POINT(7);
+					}
+				} else if (ReqServiceId == kEplDllReqServiceNmtRequest) {	// NmtRequest
+					if (EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_NMTREQ].m_pbBuffer != NULL) {	// NmtRequest does exist
+						// check if frame is not empty and not being filled
+						if (EplDllkInstance_g.
+						    m_pTxBuffer
+						    [EPL_DLLK_TXFRAME_NMTREQ].
+						    m_uiTxMsgLen >
+						    EPL_DLLK_BUFLEN_FILLING) {
+							/*if (EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_NMTREQ].m_uiTxMsgLen < EPL_DLLK_BUFLEN_MIN)
+							   {   // pad frame
+							   EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_NMTREQ].m_uiTxMsgLen = EPL_DLLK_BUFLEN_MIN;
+							   } */
+							// memorize transmission
+							pTxFrame =
+							    (tEplFrame *) 1;
+							// send NmtRequest
+							Ret =
+							    EdrvSendTxMsg
+							    (&EplDllkInstance_g.
+							     m_pTxBuffer
+							     [EPL_DLLK_TXFRAME_NMTREQ]);
+							if (Ret !=
+							    kEplSuccessful) {
+								goto Exit;
+							}
+
+						}
+					}
+
+				} else if (ReqServiceId == kEplDllReqServiceUnspecified) {	// unspecified invite
+					if (EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_NONEPL].m_pbBuffer != NULL) {	// non-EPL frame does exist
+						// check if frame is not empty and not being filled
+						if (EplDllkInstance_g.
+						    m_pTxBuffer
+						    [EPL_DLLK_TXFRAME_NONEPL].
+						    m_uiTxMsgLen >
+						    EPL_DLLK_BUFLEN_FILLING) {
+							/*if (EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_NMTREQ].m_uiTxMsgLen < EPL_DLLK_BUFLEN_MIN)
+							   {   // pad frame
+							   EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_NMTREQ].m_uiTxMsgLen = EPL_DLLK_BUFLEN_MIN;
+							   } */
+							// memorize transmission
+							pTxFrame =
+							    (tEplFrame *) 1;
+							// send non-EPL frame
+							Ret =
+							    EdrvSendTxMsg
+							    (&EplDllkInstance_g.
+							     m_pTxBuffer
+							     [EPL_DLLK_TXFRAME_NONEPL]);
+							if (Ret !=
+							    kEplSuccessful) {
+								goto Exit;
+							}
+
+						}
+					}
+
+				} else if (ReqServiceId == kEplDllReqServiceNo) {	// no async service requested -> do nothing
+				}
+			}
+#if EPL_DLL_PRES_READY_AFTER_SOA != FALSE
+			if (pTxFrame == NULL) {	// signal process function readiness of PRes frame
+				Event.m_EventSink = kEplEventSinkDllk;
+				Event.m_EventType = kEplEventTypeDllkPresReady;
+				Event.m_uiSize = 0;
+				Event.m_pArg = NULL;
+				Ret = EplEventkPost(&Event);
+			}
+#endif
+
+			// inform PDO module
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0)
+//            Ret = EplPdokCbSoa(&FrameInfo);
+#endif
+
+			// $$$ put SrcNodeId, NMT state and NetTime as HeartbeatEvent into eventqueue
+
+			// $$$ inform emergency protocol handling about flags
+			break;
+		}
+
+	case kEplMsgTypeAsnd:
+		{
+			// ASnd frame
+			NmtEvent = kEplNmtEventDllCeAsnd;
+
+			// ASnd service registered?
+			uiAsndServiceId =
+			    (unsigned int)AmiGetByteFromLe(&pFrame->m_Data.
+							   m_Asnd.
+							   m_le_bServiceId);
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+			if ((EplDllkInstance_g.m_DllState >= kEplDllMsNonCyclic)
+			    &&
+			    ((((tEplDllAsndServiceId) uiAsndServiceId) ==
+			      kEplDllAsndStatusResponse)
+			     || (((tEplDllAsndServiceId) uiAsndServiceId) == kEplDllAsndIdentResponse))) {	// StatusRes or IdentRes received
+				uiNodeId =
+				    AmiGetByteFromLe(&pFrame->m_le_bSrcNodeId);
+				if ((EplDllkInstance_g.m_LastReqServiceId ==
+				     ((tEplDllReqServiceId) uiAsndServiceId))
+				    && (uiNodeId == EplDllkInstance_g.m_uiLastTargetNodeId)) {	// mark request as responded
+					EplDllkInstance_g.m_LastReqServiceId =
+					    kEplDllReqServiceNo;
+				}
+				if (((tEplDllAsndServiceId) uiAsndServiceId) == kEplDllAsndIdentResponse) {	// memorize MAC address of CN for PReq
+					tEplDllkNodeInfo *pIntNodeInfo;
+
+					pIntNodeInfo =
+					    EplDllkGetNodeInfo(uiNodeId);
+					if (pIntNodeInfo == NULL) {	// no node info structure available
+						Ret = kEplDllNoNodeInfo;
+					} else {
+						EPL_MEMCPY(pIntNodeInfo->
+							   m_be_abMacAddr,
+							   pFrame->
+							   m_be_abSrcMac, 6);
+					}
+				}
+				// forward Flag2 to asynchronous scheduler
+				bFlag1 =
+				    AmiGetByteFromLe(&pFrame->m_Data.m_Asnd.
+						     m_Payload.m_StatusResponse.
+						     m_le_bFlag2);
+				Ret =
+				    EplDllkCalAsyncSetPendingRequests(uiNodeId,
+								      ((tEplDllAsyncReqPriority) ((bFlag1 & EPL_FRAME_FLAG2_PR) >> EPL_FRAME_FLAG2_PR_SHIFT)), (bFlag1 & EPL_FRAME_FLAG2_RS));
+			}
+#endif
+
+			if (uiAsndServiceId < EPL_DLL_MAX_ASND_SERVICE_ID) {	// ASnd service ID is valid
+				if (EplDllkInstance_g.m_aAsndFilter[uiAsndServiceId] == kEplDllAsndFilterAny) {	// ASnd service ID is registered
+					// forward frame via async receive FIFO to userspace
+					Ret =
+					    EplDllkCalAsyncFrameReceived
+					    (&FrameInfo);
+				} else if (EplDllkInstance_g.m_aAsndFilter[uiAsndServiceId] == kEplDllAsndFilterLocal) {	// ASnd service ID is registered, but only local node ID or broadcasts
+					// shall be forwarded
+					uiNodeId =
+					    AmiGetByteFromLe(&pFrame->
+							     m_le_bDstNodeId);
+					if ((uiNodeId ==
+					     EplDllkInstance_g.m_DllConfigParam.
+					     m_uiNodeId)
+					    || (uiNodeId == EPL_C_ADR_BROADCAST)) {	// ASnd frame is intended for us
+						// forward frame via async receive FIFO to userspace
+						Ret =
+						    EplDllkCalAsyncFrameReceived
+						    (&FrameInfo);
+					}
+				}
+			}
+			break;
+		}
+
+	default:
+		{
+			break;
+		}
+	}
+
+	if (NmtEvent != kEplNmtEventNoEvent) {	// event for DLL and NMT state machine generated
+		Ret = EplDllkChangeState(NmtEvent, NmtState);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+
+		if ((NmtEvent != kEplNmtEventDllCeAsnd)
+		    && ((NmtState <= kEplNmtCsPreOperational1) || (NmtEvent != kEplNmtEventDllCePres))) {	// NMT state machine is not interested in ASnd frames and PRes frames when not CsNotActive or CsPreOp1
+			// inform NMT module
+			Event.m_EventSink = kEplEventSinkNmtk;
+			Event.m_EventType = kEplEventTypeNmtEvent;
+			Event.m_uiSize = sizeof(NmtEvent);
+			Event.m_pArg = &NmtEvent;
+			Ret = EplEventkPost(&Event);
+		}
+	}
+
+      Exit:
+	if (Ret != kEplSuccessful) {
+		DWORD dwArg;
+
+		BENCHMARK_MOD_02_TOGGLE(9);
+
+		dwArg = EplDllkInstance_g.m_DllState | (NmtEvent << 8);
+
+		// Error event for API layer
+		Ret = EplEventkPostError(kEplEventSourceDllk,
+					 Ret, sizeof(dwArg), &dwArg);
+	}
+	BENCHMARK_MOD_02_RESET(3);
+	return;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCbFrameTransmitted()
+//
+// Description: called from EdrvInterruptHandler().
+//              It signals
+//
+// Parameters:  pRxBuffer_p             = receive buffer structure
+//
+// Returns:     (none)
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static void EplDllkCbFrameTransmitted(tEdrvTxBuffer * pTxBuffer_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplEvent Event;
+	tEplDllAsyncReqPriority Priority;
+	tEplNmtState NmtState;
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0) \
+    && (EPL_DLL_PRES_READY_AFTER_SOC == FALSE)
+	tEplFrameInfo FrameInfo;
+#endif
+
+	NmtState = EplNmtkGetNmtState();
+
+	if (NmtState <= kEplNmtGsResetConfiguration) {
+		goto Exit;
+	}
+
+	if ((pTxBuffer_p - EplDllkInstance_g.m_pTxBuffer) == EPL_DLLK_TXFRAME_NMTREQ) {	// frame from NMT request FIFO sent
+		// mark Tx-buffer as empty
+		pTxBuffer_p->m_uiTxMsgLen = EPL_DLLK_BUFLEN_EMPTY;
+
+		// post event to DLL
+		Priority = kEplDllAsyncReqPrioNmt;
+		Event.m_EventSink = kEplEventSinkDllk;
+		Event.m_EventType = kEplEventTypeDllkFillTx;
+		EPL_MEMSET(&Event.m_NetTime, 0x00, sizeof(Event.m_NetTime));
+		Event.m_pArg = &Priority;
+		Event.m_uiSize = sizeof(Priority);
+		Ret = EplEventkPost(&Event);
+	} else if ((pTxBuffer_p - EplDllkInstance_g.m_pTxBuffer) == EPL_DLLK_TXFRAME_NONEPL) {	// frame from generic priority FIFO sent
+		// mark Tx-buffer as empty
+		pTxBuffer_p->m_uiTxMsgLen = EPL_DLLK_BUFLEN_EMPTY;
+
+		// post event to DLL
+		Priority = kEplDllAsyncReqPrioGeneric;
+		Event.m_EventSink = kEplEventSinkDllk;
+		Event.m_EventType = kEplEventTypeDllkFillTx;
+		EPL_MEMSET(&Event.m_NetTime, 0x00, sizeof(Event.m_NetTime));
+		Event.m_pArg = &Priority;
+		Event.m_uiSize = sizeof(Priority);
+		Ret = EplEventkPost(&Event);
+	}
+#if ((((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0) \
+    && (EPL_DLL_PRES_READY_AFTER_SOC == FALSE)) \
+    || (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+	else if ((pTxBuffer_p->m_EplMsgType == kEplMsgTypePreq)
+		 || (pTxBuffer_p->m_EplMsgType == kEplMsgTypePres)) {	// PRes resp. PReq frame sent
+
+#if ((((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0) \
+            && (EPL_DLL_PRES_READY_AFTER_SOC == FALSE))
+		{
+			// inform PDO module
+			FrameInfo.m_pFrame =
+			    (tEplFrame *) pTxBuffer_p->m_pbBuffer;
+			FrameInfo.m_uiFrameSize = pTxBuffer_p->m_uiMaxBufferLen;
+			Ret = EplPdokCbPdoTransmitted(&FrameInfo);
+		}
+#endif
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+		{
+			// if own Pres on MN, trigger SoA
+			if ((NmtState >= kEplNmtMsPreOperational2)
+			    && (pTxBuffer_p ==
+				&EplDllkInstance_g.
+				m_pTxBuffer[EPL_DLLK_TXFRAME_PRES])) {
+				Ret =
+				    EplDllkChangeState(kEplNmtEventDllMeSoaTrig,
+						       NmtState);
+			}
+		}
+#endif
+
+#if EPL_DLL_PRES_READY_AFTER_SOA != FALSE
+		goto Exit;
+#endif
+	}
+#endif
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+	else if (pTxBuffer_p->m_EplMsgType == kEplMsgTypeSoa) {	// SoA frame sent
+		tEplNmtEvent NmtEvent = kEplNmtEventDllMeSoaSent;
+
+		// check if we are invited
+		if (EplDllkInstance_g.m_uiLastTargetNodeId ==
+		    EplDllkInstance_g.m_DllConfigParam.m_uiNodeId) {
+			tEplFrame *pTxFrame;
+
+			if (EplDllkInstance_g.m_LastReqServiceId == kEplDllReqServiceStatus) {	// StatusRequest
+				if (EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_STATUSRES].m_pbBuffer != NULL) {	// StatusRes does exist
+
+					pTxFrame =
+					    (tEplFrame *) EplDllkInstance_g.
+					    m_pTxBuffer
+					    [EPL_DLLK_TXFRAME_STATUSRES].
+					    m_pbBuffer;
+					// update StatusRes frame (NMT state, EN, EC, RS, PR flags)
+					AmiSetByteToLe(&pTxFrame->m_Data.m_Asnd.
+						       m_Payload.
+						       m_StatusResponse.
+						       m_le_bNmtStatus,
+						       (BYTE) NmtState);
+					AmiSetByteToLe(&pTxFrame->m_Data.m_Asnd.
+						       m_Payload.
+						       m_StatusResponse.
+						       m_le_bFlag1,
+						       EplDllkInstance_g.
+						       m_bFlag1);
+					AmiSetByteToLe(&pTxFrame->m_Data.m_Asnd.
+						       m_Payload.
+						       m_StatusResponse.
+						       m_le_bFlag2,
+						       EplDllkInstance_g.
+						       m_bFlag2);
+					// send StatusRes
+					Ret =
+					    EdrvSendTxMsg(&EplDllkInstance_g.
+							  m_pTxBuffer
+							  [EPL_DLLK_TXFRAME_STATUSRES]);
+					if (Ret != kEplSuccessful) {
+						goto Exit;
+					}
+					TGT_DBG_SIGNAL_TRACE_POINT(8);
+
+				}
+			} else if (EplDllkInstance_g.m_LastReqServiceId == kEplDllReqServiceIdent) {	// IdentRequest
+				if (EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_IDENTRES].m_pbBuffer != NULL) {	// IdentRes does exist
+					pTxFrame =
+					    (tEplFrame *) EplDllkInstance_g.
+					    m_pTxBuffer
+					    [EPL_DLLK_TXFRAME_IDENTRES].
+					    m_pbBuffer;
+					// update IdentRes frame (NMT state, RS, PR flags)
+					AmiSetByteToLe(&pTxFrame->m_Data.m_Asnd.
+						       m_Payload.
+						       m_IdentResponse.
+						       m_le_bNmtStatus,
+						       (BYTE) NmtState);
+					AmiSetByteToLe(&pTxFrame->m_Data.m_Asnd.
+						       m_Payload.
+						       m_IdentResponse.
+						       m_le_bFlag2,
+						       EplDllkInstance_g.
+						       m_bFlag2);
+					// send IdentRes
+					Ret =
+					    EdrvSendTxMsg(&EplDllkInstance_g.
+							  m_pTxBuffer
+							  [EPL_DLLK_TXFRAME_IDENTRES]);
+					if (Ret != kEplSuccessful) {
+						goto Exit;
+					}
+					TGT_DBG_SIGNAL_TRACE_POINT(7);
+				}
+			} else if (EplDllkInstance_g.m_LastReqServiceId == kEplDllReqServiceNmtRequest) {	// NmtRequest
+				if (EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_NMTREQ].m_pbBuffer != NULL) {	// NmtRequest does exist
+					// check if frame is not empty and not being filled
+					if (EplDllkInstance_g.
+					    m_pTxBuffer
+					    [EPL_DLLK_TXFRAME_NMTREQ].
+					    m_uiTxMsgLen >
+					    EPL_DLLK_BUFLEN_FILLING) {
+						// check if this frame is a NMT command,
+						// then forward this frame back to NmtMnu module,
+						// because it needs the time, when this frame is
+						// actually sent, to start the timer for monitoring
+						// the NMT state change.
+
+						pTxFrame =
+						    (tEplFrame *)
+						    EplDllkInstance_g.
+						    m_pTxBuffer
+						    [EPL_DLLK_TXFRAME_NMTREQ].
+						    m_pbBuffer;
+						if ((AmiGetByteFromLe
+						     (&pTxFrame->
+						      m_le_bMessageType)
+						     == (BYTE) kEplMsgTypeAsnd)
+						    &&
+						    (AmiGetByteFromLe
+						     (&pTxFrame->m_Data.m_Asnd.
+						      m_le_bServiceId)
+						     == (BYTE) kEplDllAsndNmtCommand)) {	// post event directly to NmtMnu module
+							Event.m_EventSink =
+							    kEplEventSinkNmtMnu;
+							Event.m_EventType =
+							    kEplEventTypeNmtMnuNmtCmdSent;
+							Event.m_uiSize =
+							    EplDllkInstance_g.
+							    m_pTxBuffer
+							    [EPL_DLLK_TXFRAME_NMTREQ].
+							    m_uiTxMsgLen;
+							Event.m_pArg = pTxFrame;
+							Ret =
+							    EplEventkPost
+							    (&Event);
+
+						}
+						// send NmtRequest
+						Ret =
+						    EdrvSendTxMsg
+						    (&EplDllkInstance_g.
+						     m_pTxBuffer
+						     [EPL_DLLK_TXFRAME_NMTREQ]);
+						if (Ret != kEplSuccessful) {
+							goto Exit;
+						}
+
+					}
+				}
+
+			} else if (EplDllkInstance_g.m_LastReqServiceId == kEplDllReqServiceUnspecified) {	// unspecified invite
+				if (EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_NONEPL].m_pbBuffer != NULL) {	// non-EPL frame does exist
+					// check if frame is not empty and not being filled
+					if (EplDllkInstance_g.
+					    m_pTxBuffer
+					    [EPL_DLLK_TXFRAME_NONEPL].
+					    m_uiTxMsgLen >
+					    EPL_DLLK_BUFLEN_FILLING) {
+						// send non-EPL frame
+						Ret =
+						    EdrvSendTxMsg
+						    (&EplDllkInstance_g.
+						     m_pTxBuffer
+						     [EPL_DLLK_TXFRAME_NONEPL]);
+						if (Ret != kEplSuccessful) {
+							goto Exit;
+						}
+
+					}
+				}
+			}
+			// ASnd frame was sent, remove the request
+			EplDllkInstance_g.m_LastReqServiceId =
+			    kEplDllReqServiceNo;
+		}
+		// forward event to ErrorHandler and PDO module
+		Event.m_EventSink = kEplEventSinkNmtk;
+		Event.m_EventType = kEplEventTypeNmtEvent;
+		Event.m_uiSize = sizeof(NmtEvent);
+		Event.m_pArg = &NmtEvent;
+		Ret = EplEventkPost(&Event);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+	}
+#endif
+
+#if EPL_DLL_PRES_READY_AFTER_SOA != FALSE
+	else {			// d.k.: Why that else? on CN it is entered on IdentRes and StatusRes
+		goto Exit;
+	}
+
+	// signal process function readiness of PRes frame
+	Event.m_EventSink = kEplEventSinkDllk;
+	Event.m_EventType = kEplEventTypeDllkPresReady;
+	Event.m_uiSize = 0;
+	Event.m_pArg = NULL;
+	Ret = EplEventkPost(&Event);
+
+#endif
+
+      Exit:
+	if (Ret != kEplSuccessful) {
+		DWORD dwArg;
+
+		BENCHMARK_MOD_02_TOGGLE(9);
+
+		dwArg =
+		    EplDllkInstance_g.m_DllState | (pTxBuffer_p->
+						    m_EplMsgType << 16);
+
+		// Error event for API layer
+		Ret = EplEventkPostError(kEplEventSourceDllk,
+					 Ret, sizeof(dwArg), &dwArg);
+	}
+
+	return;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCheckFrame()
+//
+// Description: check frame and set missing information
+//
+// Parameters:  pFrame_p                = ethernet frame
+//              uiFrameSize_p           = size of frame
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplDllkCheckFrame(tEplFrame * pFrame_p,
+				    unsigned int uiFrameSize_p)
+{
+	tEplMsgType MsgType;
+	WORD wEtherType;
+
+	// check frame
+	if (pFrame_p != NULL) {
+		// check SrcMAC
+		if (AmiGetQword48FromBe(pFrame_p->m_be_abSrcMac) == 0) {
+			// source MAC address
+			EPL_MEMCPY(&pFrame_p->m_be_abSrcMac[0],
+				   &EplDllkInstance_g.m_be_abSrcMac[0], 6);
+		}
+		// check ethertype
+		wEtherType = AmiGetWordFromBe(&pFrame_p->m_be_wEtherType);
+		if (wEtherType == 0) {
+			// assume EPL frame
+			wEtherType = EPL_C_DLL_ETHERTYPE_EPL;
+			AmiSetWordToBe(&pFrame_p->m_be_wEtherType, wEtherType);
+		}
+
+		if (wEtherType == EPL_C_DLL_ETHERTYPE_EPL) {
+			// source node ID
+			AmiSetByteToLe(&pFrame_p->m_le_bSrcNodeId,
+				       (BYTE) EplDllkInstance_g.
+				       m_DllConfigParam.m_uiNodeId);
+
+			// check message type
+			MsgType =
+			    AmiGetByteFromLe(&pFrame_p->m_le_bMessageType);
+			if (MsgType == 0) {
+				MsgType = kEplMsgTypeAsnd;
+				AmiSetByteToLe(&pFrame_p->m_le_bMessageType,
+					       (BYTE) MsgType);
+			}
+
+			if (MsgType == kEplMsgTypeAsnd) {
+				// destination MAC address
+				AmiSetQword48ToBe(&pFrame_p->m_be_abDstMac[0],
+						  EPL_C_DLL_MULTICAST_ASND);
+			}
+
+		}
+	}
+
+	return kEplSuccessful;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCbCnTimer()
+//
+// Description: called by timer module. It monitors the EPL cycle when it is a CN.
+//
+// Parameters:  pEventArg_p             = timer event argument
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+#if EPL_TIMER_USE_HIGHRES != FALSE
+static tEplKernel PUBLIC EplDllkCbCnTimer(tEplTimerEventArg * pEventArg_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplNmtState NmtState;
+
+#if EPL_TIMER_USE_HIGHRES != FALSE
+	if (pEventArg_p->m_TimerHdl != EplDllkInstance_g.m_TimerHdlCycle) {	// zombie callback
+		// just exit
+		goto Exit;
+	}
+#endif
+
+	NmtState = EplNmtkGetNmtState();
+
+	if (NmtState <= kEplNmtGsResetConfiguration) {
+		goto Exit;
+	}
+
+	Ret = EplDllkChangeState(kEplNmtEventDllCeFrameTimeout, NmtState);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// 2008/10/15 d.k. reprogramming of timer not necessary,
+	// because it will be programmed, when SoC is received.
+/*
+    // reprogram timer
+#if EPL_TIMER_USE_HIGHRES != FALSE
+    if ((NmtState > kEplNmtCsPreOperational1)
+        && (EplDllkInstance_g.m_ullFrameTimeout != 0))
+    {
+        Ret = EplTimerHighReskModifyTimerNs(&EplDllkInstance_g.m_TimerHdlCycle, EplDllkInstance_g.m_ullFrameTimeout, EplDllkCbCnTimer, 0L, FALSE);
+    }
+#endif
+*/
+
+      Exit:
+	if (Ret != kEplSuccessful) {
+		DWORD dwArg;
+
+		BENCHMARK_MOD_02_TOGGLE(9);
+
+		dwArg =
+		    EplDllkInstance_g.
+		    m_DllState | (kEplNmtEventDllCeFrameTimeout << 8);
+
+		// Error event for API layer
+		Ret = EplEventkPostError(kEplEventSourceDllk,
+					 Ret, sizeof(dwArg), &dwArg);
+	}
+
+	return Ret;
+}
+#endif
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCbMnTimerCycle()
+//
+// Description: called by timer module. It triggers the SoC when it is a MN.
+//
+// Parameters:  pEventArg_p             = timer event argument
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel PUBLIC EplDllkCbMnTimerCycle(tEplTimerEventArg * pEventArg_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplNmtState NmtState;
+
+#if EPL_TIMER_USE_HIGHRES != FALSE
+	if (pEventArg_p->m_TimerHdl != EplDllkInstance_g.m_TimerHdlCycle) {	// zombie callback
+		// just exit
+		goto Exit;
+	}
+#endif
+
+	NmtState = EplNmtkGetNmtState();
+
+	if (NmtState <= kEplNmtGsResetConfiguration) {
+		goto Exit;
+	}
+
+	Ret = EplDllkChangeState(kEplNmtEventDllMeSocTrig, NmtState);
+
+      Exit:
+	if (Ret != kEplSuccessful) {
+		DWORD dwArg;
+
+		BENCHMARK_MOD_02_TOGGLE(9);
+
+		dwArg =
+		    EplDllkInstance_g.
+		    m_DllState | (kEplNmtEventDllMeSocTrig << 8);
+
+		// Error event for API layer
+		Ret = EplEventkPostError(kEplEventSourceDllk,
+					 Ret, sizeof(dwArg), &dwArg);
+	}
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCbMnTimerResponse()
+//
+// Description: called by timer module. It monitors the PRes timeout.
+//
+// Parameters:  pEventArg_p             = timer event argument
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel PUBLIC EplDllkCbMnTimerResponse(tEplTimerEventArg *
+						  pEventArg_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplNmtState NmtState;
+
+#if EPL_TIMER_USE_HIGHRES != FALSE
+	if (pEventArg_p->m_TimerHdl != EplDllkInstance_g.m_TimerHdlResponse) {	// zombie callback
+		// just exit
+		goto Exit;
+	}
+#endif
+
+	NmtState = EplNmtkGetNmtState();
+
+	if (NmtState <= kEplNmtGsResetConfiguration) {
+		goto Exit;
+	}
+
+	Ret = EplDllkChangeState(kEplNmtEventDllMePresTimeout, NmtState);
+
+      Exit:
+	if (Ret != kEplSuccessful) {
+		DWORD dwArg;
+
+		BENCHMARK_MOD_02_TOGGLE(9);
+
+		dwArg =
+		    EplDllkInstance_g.
+		    m_DllState | (kEplNmtEventDllMePresTimeout << 8);
+
+		// Error event for API layer
+		Ret = EplEventkPostError(kEplEventSourceDllk,
+					 Ret, sizeof(dwArg), &dwArg);
+	}
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkGetNodeInfo()
+//
+// Description: returns node info structure of the specified node.
+//
+// Parameters:  uiNodeId_p              = node ID
+//
+// Returns:     tEplDllkNodeInfo*       = pointer to internal node info structure
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplDllkNodeInfo *EplDllkGetNodeInfo(unsigned int uiNodeId_p)
+{
+	// $$$ d.k.: use hash algorithm to retrieve the appropriate node info structure
+	//           if size of array is less than 254.
+	uiNodeId_p--;		// node ID starts at 1 but array at 0
+	if (uiNodeId_p >= tabentries(EplDllkInstance_g.m_aNodeInfo)) {
+		return NULL;
+	} else {
+		return &EplDllkInstance_g.m_aNodeInfo[uiNodeId_p];
+	}
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkMnSendSoa()
+//
+// Description: it updates and transmits the SoA.
+//
+// Parameters:  NmtState_p              = current NMT state
+//              pDllStateProposed_p     = proposed DLL state
+//              fEnableInvitation_p     = enable invitation for asynchronous phase
+//                                        it will be disabled for EPL_C_DLL_PREOP1_START_CYCLES SoAs
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplDllkMnSendSoa(tEplNmtState NmtState_p,
+				   tEplDllState * pDllStateProposed_p,
+				   BOOL fEnableInvitation_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEdrvTxBuffer *pTxBuffer = NULL;
+	tEplFrame *pTxFrame;
+	tEplDllkNodeInfo *pNodeInfo;
+
+	*pDllStateProposed_p = kEplDllMsNonCyclic;
+
+	pTxBuffer = &EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_SOA];
+	if (pTxBuffer->m_pbBuffer != NULL) {	// SoA does exist
+		pTxFrame = (tEplFrame *) pTxBuffer->m_pbBuffer;
+
+		if (fEnableInvitation_p != FALSE) {	// fetch target of asynchronous phase
+			if (EplDllkInstance_g.m_bFlag2 == 0) {	// own queues are empty
+				EplDllkInstance_g.m_LastReqServiceId =
+				    kEplDllReqServiceNo;
+			} else if (((tEplDllAsyncReqPriority) (EplDllkInstance_g.m_bFlag2 >> EPL_FRAME_FLAG2_PR_SHIFT)) == kEplDllAsyncReqPrioNmt) {	// frames in own NMT request queue available
+				EplDllkInstance_g.m_LastReqServiceId =
+				    kEplDllReqServiceNmtRequest;
+			} else {
+				EplDllkInstance_g.m_LastReqServiceId =
+				    kEplDllReqServiceUnspecified;
+			}
+			Ret =
+			    EplDllkCalAsyncGetSoaRequest(&EplDllkInstance_g.
+							 m_LastReqServiceId,
+							 &EplDllkInstance_g.
+							 m_uiLastTargetNodeId);
+			if (Ret != kEplSuccessful) {
+				goto Exit;
+			}
+			if (EplDllkInstance_g.m_LastReqServiceId != kEplDllReqServiceNo) {	// asynchronous phase will be assigned to one node
+				if (EplDllkInstance_g.m_uiLastTargetNodeId == EPL_C_ADR_INVALID) {	// exchange invalid node ID with local node ID
+					EplDllkInstance_g.m_uiLastTargetNodeId =
+					    EplDllkInstance_g.m_DllConfigParam.
+					    m_uiNodeId;
+					// d.k. DLL state WaitAsndTrig is not helpful;
+					//      so just step over to WaitSocTrig,
+					//      because own ASnd is sent automatically in CbFrameTransmitted() after SoA.
+					//*pDllStateProposed_p = kEplDllMsWaitAsndTrig;
+					*pDllStateProposed_p =
+					    kEplDllMsWaitSocTrig;
+				} else {	// assignment to CN
+					*pDllStateProposed_p =
+					    kEplDllMsWaitAsnd;
+				}
+
+				pNodeInfo =
+				    EplDllkGetNodeInfo(EplDllkInstance_g.
+						       m_uiLastTargetNodeId);
+				if (pNodeInfo == NULL) {	// no node info structure available
+					Ret = kEplDllNoNodeInfo;
+					goto Exit;
+				}
+				// update frame (EA, ER flags)
+				AmiSetByteToLe(&pTxFrame->m_Data.m_Soa.
+					       m_le_bFlag1,
+					       pNodeInfo->
+					       m_bSoaFlag1 & (EPL_FRAME_FLAG1_EA
+							      |
+							      EPL_FRAME_FLAG1_ER));
+			} else {	// no assignment of asynchronous phase
+				*pDllStateProposed_p = kEplDllMsWaitSocTrig;
+				EplDllkInstance_g.m_uiLastTargetNodeId =
+				    EPL_C_ADR_INVALID;
+			}
+
+			// update frame (target)
+			AmiSetByteToLe(&pTxFrame->m_Data.m_Soa.
+				       m_le_bReqServiceId,
+				       (BYTE) EplDllkInstance_g.
+				       m_LastReqServiceId);
+			AmiSetByteToLe(&pTxFrame->m_Data.m_Soa.
+				       m_le_bReqServiceTarget,
+				       (BYTE) EplDllkInstance_g.
+				       m_uiLastTargetNodeId);
+
+		} else {	// invite nobody
+			// update frame (target)
+			AmiSetByteToLe(&pTxFrame->m_Data.m_Soa.
+				       m_le_bReqServiceId, (BYTE) 0);
+			AmiSetByteToLe(&pTxFrame->m_Data.m_Soa.
+				       m_le_bReqServiceTarget, (BYTE) 0);
+		}
+
+		// update frame (NMT state)
+		AmiSetByteToLe(&pTxFrame->m_Data.m_Soa.m_le_bNmtStatus,
+			       (BYTE) NmtState_p);
+
+		// send SoA frame
+		Ret = EdrvSendTxMsg(pTxBuffer);
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkMnSendSoc()
+//
+// Description: it updates and transmits the SoA.
+//
+// Parameters:  (none)
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplDllkMnSendSoc(void)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEdrvTxBuffer *pTxBuffer = NULL;
+	tEplFrame *pTxFrame;
+	tEplEvent Event;
+
+	pTxBuffer = &EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_SOC];
+	if (pTxBuffer->m_pbBuffer != NULL) {	// SoC does exist
+		pTxFrame = (tEplFrame *) pTxBuffer->m_pbBuffer;
+
+		// $$$ update NetTime
+
+		// send SoC frame
+		Ret = EdrvSendTxMsg(pTxBuffer);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+		// trigger synchronous task
+		Event.m_EventSink = kEplEventSinkSync;
+		Event.m_EventType = kEplEventTypeSync;
+		Event.m_uiSize = 0;
+		Ret = EplEventkPost(&Event);
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkMnSendPreq()
+//
+// Description: it updates and transmits the PReq for the next isochronous CN
+//              or own PRes if enabled.
+//
+// Parameters:  NmtState_p              = current NMT state
+//              pDllStateProposed_p     = proposed DLL state
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplDllkMnSendPreq(tEplNmtState NmtState_p,
+				    tEplDllState * pDllStateProposed_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEdrvTxBuffer *pTxBuffer = NULL;
+	tEplFrame *pTxFrame;
+	BYTE bFlag1 = 0;
+
+	if (EplDllkInstance_g.m_pCurNodeInfo == NULL) {	// start with first isochronous CN
+		EplDllkInstance_g.m_pCurNodeInfo =
+		    EplDllkInstance_g.m_pFirstNodeInfo;
+	} else {		// iterate to next isochronous CN
+		EplDllkInstance_g.m_pCurNodeInfo =
+		    EplDllkInstance_g.m_pCurNodeInfo->m_pNextNodeInfo;
+	}
+
+	if (EplDllkInstance_g.m_pCurNodeInfo == NULL) {	// last isochronous CN reached
+		Ret = EplDllkMnSendSoa(NmtState_p, pDllStateProposed_p, TRUE);
+		goto Exit;
+	} else {
+		pTxBuffer = EplDllkInstance_g.m_pCurNodeInfo->m_pPreqTxBuffer;
+		bFlag1 =
+		    EplDllkInstance_g.m_pCurNodeInfo->
+		    m_bSoaFlag1 & EPL_FRAME_FLAG1_EA;
+		*pDllStateProposed_p = kEplDllMsWaitPres;
+
+		// start PRes Timer
+		// $$$ d.k.: maybe move this call to CbFrameTransmitted(), because the time should run from there
+#if EPL_TIMER_USE_HIGHRES != FALSE
+		Ret =
+		    EplTimerHighReskModifyTimerNs(&EplDllkInstance_g.
+						  m_TimerHdlResponse,
+						  EplDllkInstance_g.
+						  m_pCurNodeInfo->
+						  m_dwPresTimeout,
+						  EplDllkCbMnTimerResponse, 0L,
+						  FALSE);
+#endif
+	}
+
+	if (pTxBuffer == NULL) {	// PReq does not exist
+		Ret = kEplDllTxBufNotReady;
+		goto Exit;
+	}
+
+	pTxFrame = (tEplFrame *) pTxBuffer->m_pbBuffer;
+
+	if (pTxFrame != NULL) {	// PReq does exist
+		if (NmtState_p == kEplNmtMsOperational) {	// leave RD flag untouched
+			bFlag1 |=
+			    AmiGetByteFromLe(&pTxFrame->m_Data.m_Preq.
+					     m_le_bFlag1) & EPL_FRAME_FLAG1_RD;
+		}
+
+		if (pTxBuffer == &EplDllkInstance_g.m_pTxBuffer[EPL_DLLK_TXFRAME_PRES]) {	// PRes of MN will be sent
+			// update NMT state
+			AmiSetByteToLe(&pTxFrame->m_Data.m_Pres.m_le_bNmtStatus,
+				       (BYTE) NmtState_p);
+			*pDllStateProposed_p = kEplDllMsWaitSoaTrig;
+		}
+		// $$$ d.k. set EPL_FRAME_FLAG1_MS if necessary
+		// update frame (Flag1)
+		AmiSetByteToLe(&pTxFrame->m_Data.m_Preq.m_le_bFlag1, bFlag1);
+
+		// calculate frame size from payload size
+		pTxBuffer->m_uiTxMsgLen =
+		    AmiGetWordFromLe(&pTxFrame->m_Data.m_Preq.m_le_wSize) + 24;
+
+		// send PReq frame
+		Ret = EdrvSendTxMsg(pTxBuffer);
+	} else {
+		Ret = kEplDllTxFrameInvalid;
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkAsyncFrameNotReceived()
+//
+// Description: passes empty ASnd frame to receive FIFO.
+//              It will be called only for frames with registered AsndServiceIds
+//              (only kEplDllAsndFilterAny).
+//
+// Parameters:  none
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplDllkAsyncFrameNotReceived(tEplDllReqServiceId
+					       ReqServiceId_p,
+					       unsigned int uiNodeId_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	BYTE abBuffer[18];
+	tEplFrame *pFrame = (tEplFrame *) abBuffer;
+	tEplFrameInfo FrameInfo;
+
+	// check if previous SoA invitation was not answered
+	switch (ReqServiceId_p) {
+	case kEplDllReqServiceIdent:
+	case kEplDllReqServiceStatus:
+		// ASnd service registered?
+		if (EplDllkInstance_g.m_aAsndFilter[ReqServiceId_p] == kEplDllAsndFilterAny) {	// ASnd service ID is registered
+			AmiSetByteToLe(&pFrame->m_le_bSrcNodeId,
+				       (BYTE) uiNodeId_p);
+			// EPL MsgType ASnd
+			AmiSetByteToLe(&pFrame->m_le_bMessageType,
+				       (BYTE) kEplMsgTypeAsnd);
+			// ASnd Service ID
+			AmiSetByteToLe(&pFrame->m_Data.m_Asnd.m_le_bServiceId,
+				       (BYTE) ReqServiceId_p);
+			// create frame info structure
+			FrameInfo.m_pFrame = pFrame;
+			FrameInfo.m_uiFrameSize = 18;	// empty non existing ASnd frame
+			// forward frame via async receive FIFO to userspace
+			Ret = EplDllkCalAsyncFrameReceived(&FrameInfo);
+		}
+		break;
+	default:
+		// no invitation issued or it was successfully answered or it is uninteresting
+		break;
+	}
+
+	return Ret;
+}
+
+#endif //(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+#endif // #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+// EOF
diff --git a/drivers/staging/epl/EplDllkCal.c b/drivers/staging/epl/EplDllkCal.c
new file mode 100644
index 00000000000..359083ebe12
--- /dev/null
+++ b/drivers/staging/epl/EplDllkCal.c
@@ -0,0 +1,1260 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for kernel DLL Communication Abstraction Layer module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplDllkCal.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.7 $  $Date: 2008/11/13 17:13:09 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/15 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#include "kernel/EplDllkCal.h"
+#include "kernel/EplDllk.h"
+#include "kernel/EplEventk.h"
+
+#include "EplDllCal.h"
+#ifndef EPL_NO_FIFO
+#include "SharedBuff.h"
+#endif
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#ifndef min
+#define min(a,b)            (((a) < (b)) ? (a) : (b))
+#endif
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  EplDllkCal                                          */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description:
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   D E F I N I T I O N S                          //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#define EPL_DLLKCAL_MAX_QUEUES  5	// CnGenReq, CnNmtReq, {MnGenReq, MnNmtReq}, MnIdentReq, MnStatusReq
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+typedef struct {
+#ifndef EPL_NO_FIFO
+//    tShbInstance    m_ShbInstanceRx;      // FIFO for Rx ASnd frames
+	tShbInstance m_ShbInstanceTxNmt;	// FIFO for Tx frames with NMT request priority
+	tShbInstance m_ShbInstanceTxGen;	// FIFO for Tx frames with generic priority
+#else
+	unsigned int m_uiFrameSizeNmt;
+	BYTE m_abFrameNmt[1500];
+	unsigned int m_uiFrameSizeGen;
+	BYTE m_abFrameGen[1500];
+#endif
+
+	tEplDllkCalStatistics m_Statistics;
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+	// IdentRequest queue with CN node IDs
+	unsigned int m_auiQueueIdentReq[EPL_D_NMT_MaxCNNumber_U8 + 1];	// 1 entry is reserved to distinguish between full and empty
+	unsigned int m_uiWriteIdentReq;
+	unsigned int m_uiReadIdentReq;
+
+	// StatusRequest queue with CN node IDs
+	unsigned int m_auiQueueStatusReq[EPL_D_NMT_MaxCNNumber_U8 + 1];	// 1 entry is reserved to distinguish between full and empty
+	unsigned int m_uiWriteStatusReq;
+	unsigned int m_uiReadStatusReq;
+
+	unsigned int m_auiQueueCnRequests[254 * 2];
+	// first 254 entries represent the generic requests of the corresponding node
+	// second 254 entries represent the NMT requests of the corresponding node
+	unsigned int m_uiNextQueueCnRequest;
+	unsigned int m_uiNextRequestQueue;
+#endif
+
+} tEplDllkCalInstance;
+
+//---------------------------------------------------------------------------
+// local vars
+//---------------------------------------------------------------------------
+
+// if no dynamic memory allocation shall be used
+// define structures statically
+static tEplDllkCalInstance EplDllkCalInstance_g;
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCalAddInstance()
+//
+// Description: add and initialize new instance of DLL CAL module
+//
+// Parameters:  none
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkCalAddInstance()
+{
+	tEplKernel Ret = kEplSuccessful;
+#ifndef EPL_NO_FIFO
+	tShbError ShbError;
+	unsigned int fShbNewCreated;
+
+/*    ShbError = ShbCirAllocBuffer (EPL_DLLCAL_BUFFER_SIZE_RX, EPL_DLLCAL_BUFFER_ID_RX,
+        &EplDllkCalInstance_g.m_ShbInstanceRx, &fShbNewCreated);
+    // returns kShbOk, kShbOpenMismatch, kShbOutOfMem or kShbInvalidArg
+
+    if (ShbError != kShbOk)
+    {
+        Ret = kEplNoResource;
+    }
+*/
+	ShbError =
+	    ShbCirAllocBuffer(EPL_DLLCAL_BUFFER_SIZE_TX_NMT,
+			      EPL_DLLCAL_BUFFER_ID_TX_NMT,
+			      &EplDllkCalInstance_g.m_ShbInstanceTxNmt,
+			      &fShbNewCreated);
+	// returns kShbOk, kShbOpenMismatch, kShbOutOfMem or kShbInvalidArg
+
+	if (ShbError != kShbOk) {
+		Ret = kEplNoResource;
+	}
+
+/*    ShbError = ShbCirSetSignalHandlerNewData (EplDllkCalInstance_g.m_ShbInstanceTxNmt, EplDllkCalTxNmtSignalHandler, kShbPriorityNormal);
+    // returns kShbOk, kShbAlreadySignaling or kShbInvalidArg
+
+    if (ShbError != kShbOk)
+    {
+        Ret = kEplNoResource;
+    }
+*/
+	ShbError =
+	    ShbCirAllocBuffer(EPL_DLLCAL_BUFFER_SIZE_TX_GEN,
+			      EPL_DLLCAL_BUFFER_ID_TX_GEN,
+			      &EplDllkCalInstance_g.m_ShbInstanceTxGen,
+			      &fShbNewCreated);
+	// returns kShbOk, kShbOpenMismatch, kShbOutOfMem or kShbInvalidArg
+
+	if (ShbError != kShbOk) {
+		Ret = kEplNoResource;
+	}
+
+/*    ShbError = ShbCirSetSignalHandlerNewData (EplDllkCalInstance_g.m_ShbInstanceTxGen, EplDllkCalTxGenSignalHandler, kShbPriorityNormal);
+    // returns kShbOk, kShbAlreadySignaling or kShbInvalidArg
+
+    if (ShbError != kShbOk)
+    {
+        Ret = kEplNoResource;
+    }
+*/
+#else
+	EplDllkCalInstance_g.m_uiFrameSizeNmt = 0;
+	EplDllkCalInstance_g.m_uiFrameSizeGen = 0;
+#endif
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCalDelInstance()
+//
+// Description: deletes instance of DLL CAL module
+//
+// Parameters:  none
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkCalDelInstance()
+{
+	tEplKernel Ret = kEplSuccessful;
+#ifndef EPL_NO_FIFO
+	tShbError ShbError;
+
+/*    ShbError = ShbCirReleaseBuffer (EplDllkCalInstance_g.m_ShbInstanceRx);
+    if (ShbError != kShbOk)
+    {
+        Ret = kEplNoResource;
+    }
+    EplDllkCalInstance_g.m_ShbInstanceRx = NULL;
+*/
+	ShbError = ShbCirReleaseBuffer(EplDllkCalInstance_g.m_ShbInstanceTxNmt);
+	if (ShbError != kShbOk) {
+		Ret = kEplNoResource;
+	}
+	EplDllkCalInstance_g.m_ShbInstanceTxNmt = NULL;
+
+	ShbError = ShbCirReleaseBuffer(EplDllkCalInstance_g.m_ShbInstanceTxGen);
+	if (ShbError != kShbOk) {
+		Ret = kEplNoResource;
+	}
+	EplDllkCalInstance_g.m_ShbInstanceTxGen = NULL;
+
+#else
+	EplDllkCalInstance_g.m_uiFrameSizeNmt = 0;
+	EplDllkCalInstance_g.m_uiFrameSizeGen = 0;
+#endif
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCalProcess
+//
+// Description: process the passed configuration
+//
+// Parameters:  pEvent_p                = event containing configuration options
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkCalProcess(tEplEvent * pEvent_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	switch (pEvent_p->m_EventType) {
+	case kEplEventTypeDllkServFilter:
+		{
+			tEplDllCalAsndServiceIdFilter *pServFilter;
+
+			pServFilter =
+			    (tEplDllCalAsndServiceIdFilter *) pEvent_p->m_pArg;
+			Ret =
+			    EplDllkSetAsndServiceIdFilter(pServFilter->
+							  m_ServiceId,
+							  pServFilter->
+							  m_Filter);
+			break;
+		}
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+	case kEplEventTypeDllkIssueReq:
+		{
+			tEplDllCalIssueRequest *pIssueReq;
+
+			pIssueReq = (tEplDllCalIssueRequest *) pEvent_p->m_pArg;
+			Ret =
+			    EplDllkCalIssueRequest(pIssueReq->m_Service,
+						   pIssueReq->m_uiNodeId,
+						   pIssueReq->m_bSoaFlag1);
+			break;
+		}
+
+	case kEplEventTypeDllkAddNode:
+		{
+			tEplDllNodeInfo *pNodeInfo;
+
+			pNodeInfo = (tEplDllNodeInfo *) pEvent_p->m_pArg;
+			Ret = EplDllkAddNode(pNodeInfo);
+			break;
+		}
+
+	case kEplEventTypeDllkDelNode:
+		{
+			unsigned int *puiNodeId;
+
+			puiNodeId = (unsigned int *)pEvent_p->m_pArg;
+			Ret = EplDllkDeleteNode(*puiNodeId);
+			break;
+		}
+
+	case kEplEventTypeDllkSoftDelNode:
+		{
+			unsigned int *puiNodeId;
+
+			puiNodeId = (unsigned int *)pEvent_p->m_pArg;
+			Ret = EplDllkSoftDeleteNode(*puiNodeId);
+			break;
+		}
+#endif
+
+	case kEplEventTypeDllkIdentity:
+		{
+			tEplDllIdentParam *pIdentParam;
+
+			pIdentParam = (tEplDllIdentParam *) pEvent_p->m_pArg;
+			if (pIdentParam->m_uiSizeOfStruct > pEvent_p->m_uiSize) {
+				pIdentParam->m_uiSizeOfStruct =
+				    pEvent_p->m_uiSize;
+			}
+			Ret = EplDllkSetIdentity(pIdentParam);
+			break;
+		}
+
+	case kEplEventTypeDllkConfig:
+		{
+			tEplDllConfigParam *pConfigParam;
+
+			pConfigParam = (tEplDllConfigParam *) pEvent_p->m_pArg;
+			if (pConfigParam->m_uiSizeOfStruct > pEvent_p->m_uiSize) {
+				pConfigParam->m_uiSizeOfStruct =
+				    pEvent_p->m_uiSize;
+			}
+			Ret = EplDllkConfig(pConfigParam);
+			break;
+		}
+
+	default:
+		break;
+	}
+
+//Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCalAsyncGetTxCount()
+//
+// Description: returns count of Tx frames of FIFO with highest priority
+//
+// Parameters:  none
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkCalAsyncGetTxCount(tEplDllAsyncReqPriority * pPriority_p,
+				     unsigned int *puiCount_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+#ifndef EPL_NO_FIFO
+	tShbError ShbError;
+	unsigned long ulFrameCount;
+
+	// get frame count of Tx FIFO with NMT request priority
+	ShbError =
+	    ShbCirGetReadBlockCount(EplDllkCalInstance_g.m_ShbInstanceTxNmt,
+				    &ulFrameCount);
+	// returns kShbOk, kShbInvalidArg
+
+	// error handling
+	if (ShbError != kShbOk) {
+		Ret = kEplNoResource;
+		goto Exit;
+	}
+
+	if (ulFrameCount >
+	    EplDllkCalInstance_g.m_Statistics.m_ulMaxTxFrameCountNmt) {
+		EplDllkCalInstance_g.m_Statistics.m_ulMaxTxFrameCountNmt =
+		    ulFrameCount;
+	}
+
+	if (ulFrameCount != 0) {	// NMT requests are in queue
+		*pPriority_p = kEplDllAsyncReqPrioNmt;
+		*puiCount_p = (unsigned int)ulFrameCount;
+		goto Exit;
+	}
+	// get frame count of Tx FIFO with generic priority
+	ShbError =
+	    ShbCirGetReadBlockCount(EplDllkCalInstance_g.m_ShbInstanceTxGen,
+				    &ulFrameCount);
+	// returns kShbOk, kShbInvalidArg
+
+	// error handling
+	if (ShbError != kShbOk) {
+		Ret = kEplNoResource;
+		goto Exit;
+	}
+
+	if (ulFrameCount >
+	    EplDllkCalInstance_g.m_Statistics.m_ulMaxTxFrameCountGen) {
+		EplDllkCalInstance_g.m_Statistics.m_ulMaxTxFrameCountGen =
+		    ulFrameCount;
+	}
+
+	*pPriority_p = kEplDllAsyncReqPrioGeneric;
+	*puiCount_p = (unsigned int)ulFrameCount;
+
+      Exit:
+#else
+	if (EplDllkCalInstance_g.m_uiFrameSizeNmt > 0) {
+		*pPriority_p = kEplDllAsyncReqPrioNmt;
+		*puiCount_p = 1;
+	} else if (EplDllkCalInstance_g.m_uiFrameSizeGen > 0) {
+		*pPriority_p = kEplDllAsyncReqPrioGeneric;
+		*puiCount_p = 1;
+	} else {
+		*pPriority_p = kEplDllAsyncReqPrioGeneric;
+		*puiCount_p = 0;
+	}
+#endif
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCalAsyncGetTxFrame()
+//
+// Description: returns Tx frames from FIFO with specified priority
+//
+// Parameters:  pFrame_p                = IN: pointer to buffer
+//              puiFrameSize_p          = IN: max size of buffer
+//                                        OUT: actual size of frame
+//              Priority_p              = IN: priority
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkCalAsyncGetTxFrame(void *pFrame_p,
+				     unsigned int *puiFrameSize_p,
+				     tEplDllAsyncReqPriority Priority_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+#ifndef EPL_NO_FIFO
+	tShbError ShbError;
+	unsigned long ulFrameSize;
+
+	switch (Priority_p) {
+	case kEplDllAsyncReqPrioNmt:	// NMT request priority
+		ShbError =
+		    ShbCirReadDataBlock(EplDllkCalInstance_g.m_ShbInstanceTxNmt,
+					(BYTE *) pFrame_p, *puiFrameSize_p,
+					&ulFrameSize);
+		// returns kShbOk, kShbDataTruncated, kShbInvalidArg, kShbNoReadableData
+		break;
+
+	default:		// generic priority
+		ShbError =
+		    ShbCirReadDataBlock(EplDllkCalInstance_g.m_ShbInstanceTxGen,
+					(BYTE *) pFrame_p, *puiFrameSize_p,
+					&ulFrameSize);
+		// returns kShbOk, kShbDataTruncated, kShbInvalidArg, kShbNoReadableData
+		break;
+
+	}
+
+	// error handling
+	if (ShbError != kShbOk) {
+		if (ShbError == kShbNoReadableData) {
+			Ret = kEplDllAsyncTxBufferEmpty;
+		} else {	// other error
+			Ret = kEplNoResource;
+		}
+		goto Exit;
+	}
+
+	*puiFrameSize_p = (unsigned int)ulFrameSize;
+
+      Exit:
+#else
+	switch (Priority_p) {
+	case kEplDllAsyncReqPrioNmt:	// NMT request priority
+		*puiFrameSize_p =
+		    min(*puiFrameSize_p, EplDllkCalInstance_g.m_uiFrameSizeNmt);
+		EPL_MEMCPY(pFrame_p, EplDllkCalInstance_g.m_abFrameNmt,
+			   *puiFrameSize_p);
+		EplDllkCalInstance_g.m_uiFrameSizeNmt = 0;
+		break;
+
+	default:		// generic priority
+		*puiFrameSize_p =
+		    min(*puiFrameSize_p, EplDllkCalInstance_g.m_uiFrameSizeGen);
+		EPL_MEMCPY(pFrame_p, EplDllkCalInstance_g.m_abFrameGen,
+			   *puiFrameSize_p);
+		EplDllkCalInstance_g.m_uiFrameSizeGen = 0;
+		break;
+	}
+
+#endif
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCalAsyncFrameReceived()
+//
+// Description: passes ASnd frame to receive FIFO.
+//              It will be called only for frames with registered AsndServiceIds.
+//
+// Parameters:  none
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkCalAsyncFrameReceived(tEplFrameInfo * pFrameInfo_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplEvent Event;
+
+	Event.m_EventSink = kEplEventSinkDlluCal;
+	Event.m_EventType = kEplEventTypeAsndRx;
+	Event.m_pArg = pFrameInfo_p->m_pFrame;
+	Event.m_uiSize = pFrameInfo_p->m_uiFrameSize;
+	// pass NetTime of frame to userspace
+	Event.m_NetTime = pFrameInfo_p->m_NetTime;
+
+	Ret = EplEventkPost(&Event);
+	if (Ret != kEplSuccessful) {
+		EplDllkCalInstance_g.m_Statistics.m_ulCurRxFrameCount++;
+	} else {
+		EplDllkCalInstance_g.m_Statistics.m_ulMaxRxFrameCount++;
+	}
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCalAsyncSend()
+//
+// Description: puts the given frame into the transmit FIFO with the specified
+//              priority.
+//
+// Parameters:  pFrameInfo_p            = frame info structure
+//              Priority_p              = priority
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkCalAsyncSend(tEplFrameInfo * pFrameInfo_p,
+			       tEplDllAsyncReqPriority Priority_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplEvent Event;
+#ifndef EPL_NO_FIFO
+	tShbError ShbError;
+
+	switch (Priority_p) {
+	case kEplDllAsyncReqPrioNmt:	// NMT request priority
+		ShbError =
+		    ShbCirWriteDataBlock(EplDllkCalInstance_g.
+					 m_ShbInstanceTxNmt,
+					 pFrameInfo_p->m_pFrame,
+					 pFrameInfo_p->m_uiFrameSize);
+		// returns kShbOk, kShbExceedDataSizeLimit, kShbBufferFull, kShbInvalidArg
+		break;
+
+	default:		// generic priority
+		ShbError =
+		    ShbCirWriteDataBlock(EplDllkCalInstance_g.
+					 m_ShbInstanceTxGen,
+					 pFrameInfo_p->m_pFrame,
+					 pFrameInfo_p->m_uiFrameSize);
+		// returns kShbOk, kShbExceedDataSizeLimit, kShbBufferFull, kShbInvalidArg
+		break;
+
+	}
+
+	// error handling
+	switch (ShbError) {
+	case kShbOk:
+		break;
+
+	case kShbExceedDataSizeLimit:
+		Ret = kEplDllAsyncTxBufferFull;
+		break;
+
+	case kShbBufferFull:
+		Ret = kEplDllAsyncTxBufferFull;
+		break;
+
+	case kShbInvalidArg:
+	default:
+		Ret = kEplNoResource;
+		break;
+	}
+
+#else
+
+	switch (Priority_p) {
+	case kEplDllAsyncReqPrioNmt:	// NMT request priority
+		if (EplDllkCalInstance_g.m_uiFrameSizeNmt == 0) {
+			EPL_MEMCPY(EplDllkCalInstance_g.m_abFrameNmt,
+				   pFrameInfo_p->m_pFrame,
+				   pFrameInfo_p->m_uiFrameSize);
+			EplDllkCalInstance_g.m_uiFrameSizeNmt =
+			    pFrameInfo_p->m_uiFrameSize;
+		} else {
+			Ret = kEplDllAsyncTxBufferFull;
+			goto Exit;
+		}
+		break;
+
+	default:		// generic priority
+		if (EplDllkCalInstance_g.m_uiFrameSizeGen == 0) {
+			EPL_MEMCPY(EplDllkCalInstance_g.m_abFrameGen,
+				   pFrameInfo_p->m_pFrame,
+				   pFrameInfo_p->m_uiFrameSize);
+			EplDllkCalInstance_g.m_uiFrameSizeGen =
+			    pFrameInfo_p->m_uiFrameSize;
+		} else {
+			Ret = kEplDllAsyncTxBufferFull;
+			goto Exit;
+		}
+		break;
+	}
+
+#endif
+
+	// post event to DLL
+	Event.m_EventSink = kEplEventSinkDllk;
+	Event.m_EventType = kEplEventTypeDllkFillTx;
+	EPL_MEMSET(&Event.m_NetTime, 0x00, sizeof(Event.m_NetTime));
+	Event.m_pArg = &Priority_p;
+	Event.m_uiSize = sizeof(Priority_p);
+	Ret = EplEventkPost(&Event);
+
+#ifdef EPL_NO_FIFO
+      Exit:
+#endif
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCalAsyncClearBuffer()
+//
+// Description: clears the transmit buffer
+//
+// Parameters:  (none)
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkCalAsyncClearBuffer(void)
+{
+	tEplKernel Ret = kEplSuccessful;
+#ifndef EPL_NO_FIFO
+	tShbError ShbError;
+
+	ShbError =
+	    ShbCirResetBuffer(EplDllkCalInstance_g.m_ShbInstanceTxNmt, 1000,
+			      NULL);
+	ShbError =
+	    ShbCirResetBuffer(EplDllkCalInstance_g.m_ShbInstanceTxGen, 1000,
+			      NULL);
+
+#else
+	EplDllkCalInstance_g.m_uiFrameSizeNmt = 0;
+	EplDllkCalInstance_g.m_uiFrameSizeGen = 0;
+#endif
+
+//    EPL_MEMSET(&EplDllkCalInstance_g.m_Statistics, 0, sizeof (tEplDllkCalStatistics));
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCalAsyncClearQueues()
+//
+// Description: clears the transmit buffer
+//
+// Parameters:  (none)
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+tEplKernel EplDllkCalAsyncClearQueues(void)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	// clear MN asynchronous queues
+	EplDllkCalInstance_g.m_uiNextQueueCnRequest = 0;
+	EplDllkCalInstance_g.m_uiNextRequestQueue = 0;
+	EplDllkCalInstance_g.m_uiReadIdentReq = 0;
+	EplDllkCalInstance_g.m_uiWriteIdentReq = 0;
+	EplDllkCalInstance_g.m_uiReadStatusReq = 0;
+	EplDllkCalInstance_g.m_uiWriteStatusReq = 0;
+
+	return Ret;
+}
+#endif
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCalGetStatistics()
+//
+// Description: returns statistics of the asynchronous queues.
+//
+// Parameters:  ppStatistics            = statistics structure
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkCalGetStatistics(tEplDllkCalStatistics ** ppStatistics)
+{
+	tEplKernel Ret = kEplSuccessful;
+#ifndef EPL_NO_FIFO
+	tShbError ShbError;
+
+	ShbError =
+	    ShbCirGetReadBlockCount(EplDllkCalInstance_g.m_ShbInstanceTxNmt,
+				    &EplDllkCalInstance_g.m_Statistics.
+				    m_ulCurTxFrameCountNmt);
+	ShbError =
+	    ShbCirGetReadBlockCount(EplDllkCalInstance_g.m_ShbInstanceTxGen,
+				    &EplDllkCalInstance_g.m_Statistics.
+				    m_ulCurTxFrameCountGen);
+//    ShbError = ShbCirGetReadBlockCount (EplDllkCalInstance_g.m_ShbInstanceRx, &EplDllkCalInstance_g.m_Statistics.m_ulCurRxFrameCount);
+
+#else
+	if (EplDllkCalInstance_g.m_uiFrameSizeNmt > 0) {
+		EplDllkCalInstance_g.m_Statistics.m_ulCurTxFrameCountNmt = 1;
+	} else {
+		EplDllkCalInstance_g.m_Statistics.m_ulCurTxFrameCountNmt = 0;
+	}
+	if (EplDllkCalInstance_g.m_uiFrameSizeGen > 0) {
+		EplDllkCalInstance_g.m_Statistics.m_ulCurTxFrameCountGen = 1;
+	} else {
+		EplDllkCalInstance_g.m_Statistics.m_ulCurTxFrameCountGen = 0;
+	}
+#endif
+
+	*ppStatistics = &EplDllkCalInstance_g.m_Statistics;
+	return Ret;
+}
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCalIssueRequest()
+//
+// Description: issues a StatusRequest or a IdentRequest to the specified node.
+//
+// Parameters:  Service_p               = request service ID
+//              uiNodeId_p              = node ID
+//              bSoaFlag1_p             = flag1 for this node (transmit in SoA and PReq)
+//                                        If 0xFF this flag is ignored.
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkCalIssueRequest(tEplDllReqServiceId Service_p,
+				  unsigned int uiNodeId_p, BYTE bSoaFlag1_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	if (bSoaFlag1_p != 0xFF) {
+		Ret = EplDllkSetFlag1OfNode(uiNodeId_p, bSoaFlag1_p);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+	}
+	// add node to appropriate request queue
+	switch (Service_p) {
+	case kEplDllReqServiceIdent:
+		{
+			if (((EplDllkCalInstance_g.m_uiWriteIdentReq +
+			      1) %
+			     tabentries(EplDllkCalInstance_g.
+					m_auiQueueIdentReq))
+			    == EplDllkCalInstance_g.m_uiReadIdentReq) {	// queue is full
+				Ret = kEplDllAsyncTxBufferFull;
+				goto Exit;
+			}
+			EplDllkCalInstance_g.
+			    m_auiQueueIdentReq[EplDllkCalInstance_g.
+					       m_uiWriteIdentReq] = uiNodeId_p;
+			EplDllkCalInstance_g.m_uiWriteIdentReq =
+			    (EplDllkCalInstance_g.m_uiWriteIdentReq +
+			     1) %
+			    tabentries(EplDllkCalInstance_g.m_auiQueueIdentReq);
+			break;
+		}
+
+	case kEplDllReqServiceStatus:
+		{
+			if (((EplDllkCalInstance_g.m_uiWriteStatusReq +
+			      1) %
+			     tabentries(EplDllkCalInstance_g.
+					m_auiQueueStatusReq))
+			    == EplDllkCalInstance_g.m_uiReadStatusReq) {	// queue is full
+				Ret = kEplDllAsyncTxBufferFull;
+				goto Exit;
+			}
+			EplDllkCalInstance_g.
+			    m_auiQueueStatusReq[EplDllkCalInstance_g.
+						m_uiWriteStatusReq] =
+			    uiNodeId_p;
+			EplDllkCalInstance_g.m_uiWriteStatusReq =
+			    (EplDllkCalInstance_g.m_uiWriteStatusReq +
+			     1) %
+			    tabentries(EplDllkCalInstance_g.
+				       m_auiQueueStatusReq);
+			break;
+		}
+
+	default:
+		{
+			Ret = kEplDllInvalidParam;
+			goto Exit;
+		}
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCalAsyncGetSoaRequest()
+//
+// Description: returns next request for SoA. This function is called by DLLk module.
+//
+// Parameters:  pReqServiceId_p         = pointer to request service ID
+//                                        IN: available request for MN NMT or generic request queue (Flag2.PR)
+//                                            or kEplDllReqServiceNo if queues are empty
+//                                        OUT: next request
+//              puiNodeId_p             = OUT: pointer to node ID of next request
+//                                             = EPL_C_ADR_INVALID, if request is self addressed
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkCalAsyncGetSoaRequest(tEplDllReqServiceId * pReqServiceId_p,
+					unsigned int *puiNodeId_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	unsigned int uiCount;
+
+//    *pReqServiceId_p = kEplDllReqServiceNo;
+
+	for (uiCount = EPL_DLLKCAL_MAX_QUEUES; uiCount > 0; uiCount--) {
+		switch (EplDllkCalInstance_g.m_uiNextRequestQueue) {
+		case 0:
+			{	// CnGenReq
+				for (;
+				     EplDllkCalInstance_g.
+				     m_uiNextQueueCnRequest <
+				     (tabentries
+				      (EplDllkCalInstance_g.
+				       m_auiQueueCnRequests) / 2);
+				     EplDllkCalInstance_g.
+				     m_uiNextQueueCnRequest++) {
+					if (EplDllkCalInstance_g.m_auiQueueCnRequests[EplDllkCalInstance_g.m_uiNextQueueCnRequest] > 0) {	// non empty queue found
+						// remove one request from queue
+						EplDllkCalInstance_g.
+						    m_auiQueueCnRequests
+						    [EplDllkCalInstance_g.
+						     m_uiNextQueueCnRequest]--;
+						*puiNodeId_p =
+						    EplDllkCalInstance_g.
+						    m_uiNextQueueCnRequest + 1;
+						*pReqServiceId_p =
+						    kEplDllReqServiceUnspecified;
+						EplDllkCalInstance_g.
+						    m_uiNextQueueCnRequest++;
+						if (EplDllkCalInstance_g.m_uiNextQueueCnRequest >= (tabentries(EplDllkCalInstance_g.m_auiQueueCnRequests) / 2)) {	// last node reached
+							// continue with CnNmtReq queue at next SoA
+							EplDllkCalInstance_g.
+							    m_uiNextRequestQueue
+							    = 1;
+						}
+						goto Exit;
+					}
+				}
+				// all CnGenReq queues are empty -> continue with CnNmtReq queue
+				EplDllkCalInstance_g.m_uiNextRequestQueue = 1;
+				break;
+			}
+
+		case 1:
+			{	// CnNmtReq
+				for (;
+				     EplDllkCalInstance_g.
+				     m_uiNextQueueCnRequest <
+				     tabentries(EplDllkCalInstance_g.
+						m_auiQueueCnRequests);
+				     EplDllkCalInstance_g.
+				     m_uiNextQueueCnRequest++) {
+					if (EplDllkCalInstance_g.m_auiQueueCnRequests[EplDllkCalInstance_g.m_uiNextQueueCnRequest] > 0) {	// non empty queue found
+						// remove one request from queue
+						EplDllkCalInstance_g.
+						    m_auiQueueCnRequests
+						    [EplDllkCalInstance_g.
+						     m_uiNextQueueCnRequest]--;
+						*puiNodeId_p =
+						    EplDllkCalInstance_g.
+						    m_uiNextQueueCnRequest + 1 -
+						    (tabentries
+						     (EplDllkCalInstance_g.
+						      m_auiQueueCnRequests) /
+						     2);
+						*pReqServiceId_p =
+						    kEplDllReqServiceNmtRequest;
+						EplDllkCalInstance_g.
+						    m_uiNextQueueCnRequest++;
+						if (EplDllkCalInstance_g.m_uiNextQueueCnRequest > tabentries(EplDllkCalInstance_g.m_auiQueueCnRequests)) {	// last node reached
+							// restart CnGenReq queue
+							EplDllkCalInstance_g.
+							    m_uiNextQueueCnRequest
+							    = 0;
+							// continue with MnGenReq queue at next SoA
+							EplDllkCalInstance_g.
+							    m_uiNextRequestQueue
+							    = 2;
+						}
+						goto Exit;
+					}
+				}
+				// restart CnGenReq queue
+				EplDllkCalInstance_g.m_uiNextQueueCnRequest = 0;
+				// all CnNmtReq queues are empty -> continue with MnGenReq queue
+				EplDllkCalInstance_g.m_uiNextRequestQueue = 2;
+				break;
+			}
+
+		case 2:
+			{	// MnNmtReq and MnGenReq
+				// next queue will be MnIdentReq queue
+				EplDllkCalInstance_g.m_uiNextRequestQueue = 3;
+				if (*pReqServiceId_p != kEplDllReqServiceNo) {
+					*puiNodeId_p = EPL_C_ADR_INVALID;	// DLLk must exchange this with the actual node ID
+					goto Exit;
+				}
+				break;
+			}
+
+		case 3:
+			{	// MnIdentReq
+				// next queue will be MnStatusReq queue
+				EplDllkCalInstance_g.m_uiNextRequestQueue = 4;
+				if (EplDllkCalInstance_g.m_uiReadIdentReq != EplDllkCalInstance_g.m_uiWriteIdentReq) {	// queue is not empty
+					*puiNodeId_p =
+					    EplDllkCalInstance_g.
+					    m_auiQueueIdentReq
+					    [EplDllkCalInstance_g.
+					     m_uiReadIdentReq];
+					EplDllkCalInstance_g.m_uiReadIdentReq =
+					    (EplDllkCalInstance_g.
+					     m_uiReadIdentReq +
+					     1) %
+					    tabentries(EplDllkCalInstance_g.
+						       m_auiQueueIdentReq);
+					*pReqServiceId_p =
+					    kEplDllReqServiceIdent;
+					goto Exit;
+				}
+				break;
+			}
+
+		case 4:
+			{	// MnStatusReq
+				// next queue will be CnGenReq queue
+				EplDllkCalInstance_g.m_uiNextRequestQueue = 0;
+				if (EplDllkCalInstance_g.m_uiReadStatusReq != EplDllkCalInstance_g.m_uiWriteStatusReq) {	// queue is not empty
+					*puiNodeId_p =
+					    EplDllkCalInstance_g.
+					    m_auiQueueStatusReq
+					    [EplDllkCalInstance_g.
+					     m_uiReadStatusReq];
+					EplDllkCalInstance_g.m_uiReadStatusReq =
+					    (EplDllkCalInstance_g.
+					     m_uiReadStatusReq +
+					     1) %
+					    tabentries(EplDllkCalInstance_g.
+						       m_auiQueueStatusReq);
+					*pReqServiceId_p =
+					    kEplDllReqServiceStatus;
+					goto Exit;
+				}
+				break;
+			}
+
+		}
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDllkCalAsyncSetPendingRequests()
+//
+// Description: sets the pending asynchronous frame requests of the specified node.
+//              This will add the node to the asynchronous request scheduler.
+//
+// Parameters:  uiNodeId_p              = node ID
+//              AsyncReqPrio_p          = asynchronous request priority
+//              uiCount_p               = count of asynchronous frames
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDllkCalAsyncSetPendingRequests(unsigned int uiNodeId_p,
+					     tEplDllAsyncReqPriority
+					     AsyncReqPrio_p,
+					     unsigned int uiCount_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	// add node to appropriate request queue
+	switch (AsyncReqPrio_p) {
+	case kEplDllAsyncReqPrioNmt:
+		{
+			uiNodeId_p--;
+			if (uiNodeId_p >=
+			    (tabentries
+			     (EplDllkCalInstance_g.m_auiQueueCnRequests) / 2)) {
+				Ret = kEplDllInvalidParam;
+				goto Exit;
+			}
+			uiNodeId_p +=
+			    tabentries(EplDllkCalInstance_g.
+				       m_auiQueueCnRequests) / 2;
+			EplDllkCalInstance_g.m_auiQueueCnRequests[uiNodeId_p] =
+			    uiCount_p;
+			break;
+		}
+
+	default:
+		{
+			uiNodeId_p--;
+			if (uiNodeId_p >=
+			    (tabentries
+			     (EplDllkCalInstance_g.m_auiQueueCnRequests) / 2)) {
+				Ret = kEplDllInvalidParam;
+				goto Exit;
+			}
+			EplDllkCalInstance_g.m_auiQueueCnRequests[uiNodeId_p] =
+			    uiCount_p;
+			break;
+		}
+	}
+
+      Exit:
+	return Ret;
+}
+#endif //(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//  Callback handler for new data signaling
+//---------------------------------------------------------------------------
+
+#ifndef EPL_NO_FIFO
+/*static void  EplDllkCalTxNmtSignalHandler (
+    tShbInstance pShbRxInstance_p,
+    unsigned long ulDataSize_p)
+{
+tEplKernel      Ret = kEplSuccessful;
+tEplEvent       Event;
+tEplDllAsyncReqPriority Priority;
+#ifndef EPL_NO_FIFO
+tShbError   ShbError;
+unsigned long   ulBlockCount;
+
+    ShbError = ShbCirGetReadBlockCount (EplDllkCalInstance_g.m_ShbInstanceTxNmt, &ulBlockCount);
+    if (ulBlockCount > EplDllkCalInstance_g.m_Statistics.m_ulMaxTxFrameCountNmt)
+    {
+        EplDllkCalInstance_g.m_Statistics.m_ulMaxTxFrameCountNmt = ulBlockCount;
+    }
+
+#endif
+
+    // post event to DLL
+    Priority = kEplDllAsyncReqPrioNmt;
+    Event.m_EventSink = kEplEventSinkDllk;
+    Event.m_EventType = kEplEventTypeDllkFillTx;
+    EPL_MEMSET(&Event.m_NetTime, 0x00, sizeof(Event.m_NetTime));
+    Event.m_pArg = &Priority;
+    Event.m_uiSize = sizeof(Priority);
+    Ret = EplEventkPost(&Event);
+
+}
+
+static void  EplDllkCalTxGenSignalHandler (
+    tShbInstance pShbRxInstance_p,
+    unsigned long ulDataSize_p)
+{
+tEplKernel      Ret = kEplSuccessful;
+tEplEvent       Event;
+tEplDllAsyncReqPriority Priority;
+#ifndef EPL_NO_FIFO
+tShbError   ShbError;
+unsigned long   ulBlockCount;
+
+    ShbError = ShbCirGetReadBlockCount (EplDllkCalInstance_g.m_ShbInstanceTxGen, &ulBlockCount);
+    if (ulBlockCount > EplDllkCalInstance_g.m_Statistics.m_ulMaxTxFrameCountGen)
+    {
+        EplDllkCalInstance_g.m_Statistics.m_ulMaxTxFrameCountGen = ulBlockCount;
+    }
+
+#endif
+
+    // post event to DLL
+    Priority = kEplDllAsyncReqPrioGeneric;
+    Event.m_EventSink = kEplEventSinkDllk;
+    Event.m_EventType = kEplEventTypeDllkFillTx;
+    EPL_MEMSET(&Event.m_NetTime, 0x00, sizeof(Event.m_NetTime));
+    Event.m_pArg = &Priority;
+    Event.m_uiSize = sizeof(Priority);
+    Ret = EplEventkPost(&Event);
+
+}
+*/
+#endif
+
+#endif // #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+
+// EOF
diff --git a/drivers/staging/epl/EplDlluCal.c b/drivers/staging/epl/EplDlluCal.c
new file mode 100644
index 00000000000..7f4a1745023
--- /dev/null
+++ b/drivers/staging/epl/EplDlluCal.c
@@ -0,0 +1,529 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for DLL Communication Abstraction Layer module in EPL user part
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplDlluCal.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.7 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/20 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#include "user/EplDlluCal.h"
+#include "user/EplEventu.h"
+
+#include "EplDllCal.h"
+
+// include only if direct call between user- and kernelspace is enabled
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+#include "kernel/EplDllkCal.h"
+#endif
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLU)) != 0)
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  EplDlluCal                                          */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description:
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   D E F I N I T I O N S                          //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+typedef struct {
+	tEplDlluCbAsnd m_apfnDlluCbAsnd[EPL_DLL_MAX_ASND_SERVICE_ID];
+
+} tEplDlluCalInstance;
+
+//---------------------------------------------------------------------------
+// local vars
+//---------------------------------------------------------------------------
+
+// if no dynamic memory allocation shall be used
+// define structures statically
+static tEplDlluCalInstance EplDlluCalInstance_g;
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+static tEplKernel EplDlluCalSetAsndServiceIdFilter(tEplDllAsndServiceId
+						   ServiceId_p,
+						   tEplDllAsndFilter Filter_p);
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDlluCalAddInstance()
+//
+// Description: add and initialize new instance of DLL CAL module
+//
+// Parameters:  none
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDlluCalAddInstance()
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	// reset instance structure
+	EPL_MEMSET(&EplDlluCalInstance_g, 0, sizeof(EplDlluCalInstance_g));
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDlluCalDelInstance()
+//
+// Description: deletes an instance of DLL CAL module
+//
+// Parameters:  none
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDlluCalDelInstance()
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	// reset instance structure
+	EPL_MEMSET(&EplDlluCalInstance_g, 0, sizeof(EplDlluCalInstance_g));
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDlluCalProcess
+//
+// Description: process the passed asynch frame
+//
+// Parameters:  pEvent_p                = event containing frame to be processed
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDlluCalProcess(tEplEvent * pEvent_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplMsgType MsgType;
+	unsigned int uiAsndServiceId;
+	tEplFrameInfo FrameInfo;
+
+	if (pEvent_p->m_EventType == kEplEventTypeAsndRx) {
+		FrameInfo.m_pFrame = (tEplFrame *) pEvent_p->m_pArg;
+		FrameInfo.m_uiFrameSize = pEvent_p->m_uiSize;
+		// extract NetTime
+		FrameInfo.m_NetTime = pEvent_p->m_NetTime;
+
+		MsgType =
+		    (tEplMsgType) AmiGetByteFromLe(&FrameInfo.m_pFrame->
+						   m_le_bMessageType);
+		if (MsgType != kEplMsgTypeAsnd) {
+			Ret = kEplInvalidOperation;
+			goto Exit;
+		}
+
+		uiAsndServiceId =
+		    (unsigned int)AmiGetByteFromLe(&FrameInfo.m_pFrame->m_Data.
+						   m_Asnd.m_le_bServiceId);
+		if (uiAsndServiceId < EPL_DLL_MAX_ASND_SERVICE_ID) {	// ASnd service ID is valid
+			if (EplDlluCalInstance_g.m_apfnDlluCbAsnd[uiAsndServiceId] != NULL) {	// handler was registered
+				Ret =
+				    EplDlluCalInstance_g.
+				    m_apfnDlluCbAsnd[uiAsndServiceId]
+				    (&FrameInfo);
+			}
+		}
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDlluCalRegAsndService()
+//
+// Description: registers the specified handler for the specified
+//              AsndServiceId with the specified node ID filter.
+//
+// Parameters:  ServiceId_p             = ASnd Service ID
+//              pfnDlluCbAsnd_p         = callback function
+//              Filter_p                = node ID filter
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDlluCalRegAsndService(tEplDllAsndServiceId ServiceId_p,
+				    tEplDlluCbAsnd pfnDlluCbAsnd_p,
+				    tEplDllAsndFilter Filter_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	if (ServiceId_p < tabentries(EplDlluCalInstance_g.m_apfnDlluCbAsnd)) {
+		// memorize function pointer
+		EplDlluCalInstance_g.m_apfnDlluCbAsnd[ServiceId_p] =
+		    pfnDlluCbAsnd_p;
+
+		if (pfnDlluCbAsnd_p == NULL) {	// close filter
+			Filter_p = kEplDllAsndFilterNone;
+		}
+		// set filter in DLL module in kernel part
+		Ret = EplDlluCalSetAsndServiceIdFilter(ServiceId_p, Filter_p);
+
+	}
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDlluCalAsyncSend()
+//
+// Description: sends the frame with the specified priority.
+//
+// Parameters:  pFrameInfo_p            = frame
+//                                        m_uiFrameSize does not include the
+//                                        ethernet header (14 bytes)
+//              Priority_p              = priority
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDlluCalAsyncSend(tEplFrameInfo * pFrameInfo_p,
+			       tEplDllAsyncReqPriority Priority_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+	pFrameInfo_p->m_uiFrameSize += 14;	// add size of ethernet header
+	Ret = EplDllkCalAsyncSend(pFrameInfo_p, Priority_p);
+#else
+	Ret = kEplSuccessful;
+#endif
+
+	return Ret;
+}
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDlluCalIssueRequest()
+//
+// Description: issues a StatusRequest or a IdentRequest to the specified node.
+//
+// Parameters:  Service_p               = request service ID
+//              uiNodeId_p              = node ID
+//              bSoaFlag1_p             = flag1 for this node (transmit in SoA and PReq)
+//                                        If 0xFF this flag is ignored.
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDlluCalIssueRequest(tEplDllReqServiceId Service_p,
+				  unsigned int uiNodeId_p, BYTE bSoaFlag1_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	// add node to appropriate request queue
+	switch (Service_p) {
+	case kEplDllReqServiceIdent:
+	case kEplDllReqServiceStatus:
+		{
+			tEplEvent Event;
+			tEplDllCalIssueRequest IssueReq;
+
+			Event.m_EventSink = kEplEventSinkDllkCal;
+			Event.m_EventType = kEplEventTypeDllkIssueReq;
+			IssueReq.m_Service = Service_p;
+			IssueReq.m_uiNodeId = uiNodeId_p;
+			IssueReq.m_bSoaFlag1 = bSoaFlag1_p;
+			Event.m_pArg = &IssueReq;
+			Event.m_uiSize = sizeof(IssueReq);
+
+			Ret = EplEventuPost(&Event);
+			break;
+		}
+
+	default:
+		{
+			Ret = kEplDllInvalidParam;
+			goto Exit;
+		}
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDlluCalAddNode()
+//
+// Description: adds the specified node to the isochronous phase.
+//
+// Parameters:  pNodeInfo_p             = pointer of node info structure
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDlluCalAddNode(tEplDllNodeInfo * pNodeInfo_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplEvent Event;
+
+	Event.m_EventSink = kEplEventSinkDllkCal;
+	Event.m_EventType = kEplEventTypeDllkAddNode;
+	Event.m_pArg = pNodeInfo_p;
+	Event.m_uiSize = sizeof(tEplDllNodeInfo);
+
+	Ret = EplEventuPost(&Event);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDlluCalDeleteNode()
+//
+// Description: removes the specified node from the isochronous phase.
+//
+// Parameters:  uiNodeId_p              = node ID
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDlluCalDeleteNode(unsigned int uiNodeId_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplEvent Event;
+
+	Event.m_EventSink = kEplEventSinkDllkCal;
+	Event.m_EventType = kEplEventTypeDllkDelNode;
+	Event.m_pArg = &uiNodeId_p;
+	Event.m_uiSize = sizeof(uiNodeId_p);
+
+	Ret = EplEventuPost(&Event);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDlluCalSoftDeleteNode()
+//
+// Description: removes the specified node softly from the isochronous phase.
+//
+// Parameters:  uiNodeId_p              = node ID
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplDlluCalSoftDeleteNode(unsigned int uiNodeId_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplEvent Event;
+
+	Event.m_EventSink = kEplEventSinkDllkCal;
+	Event.m_EventType = kEplEventTypeDllkSoftDelNode;
+	Event.m_pArg = &uiNodeId_p;
+	Event.m_uiSize = sizeof(uiNodeId_p);
+
+	Ret = EplEventuPost(&Event);
+
+	return Ret;
+}
+
+#endif // (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplDlluCalSetAsndServiceIdFilter()
+//
+// Description: forwards call to EplDllkSetAsndServiceIdFilter() in kernel part
+//
+// Parameters:  ServiceId_p             = ASnd Service ID
+//              Filter_p                = node ID filter
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplDlluCalSetAsndServiceIdFilter(tEplDllAsndServiceId
+						   ServiceId_p,
+						   tEplDllAsndFilter Filter_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplEvent Event;
+	tEplDllCalAsndServiceIdFilter ServFilter;
+
+	Event.m_EventSink = kEplEventSinkDllkCal;
+	Event.m_EventType = kEplEventTypeDllkServFilter;
+	ServFilter.m_ServiceId = ServiceId_p;
+	ServFilter.m_Filter = Filter_p;
+	Event.m_pArg = &ServFilter;
+	Event.m_uiSize = sizeof(ServFilter);
+
+	Ret = EplEventuPost(&Event);
+
+	return Ret;
+}
+
+#endif // (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLU)) != 0)
+
+// EOF
diff --git a/drivers/staging/epl/EplErrDef.h b/drivers/staging/epl/EplErrDef.h
new file mode 100644
index 00000000000..2aee12cfd28
--- /dev/null
+++ b/drivers/staging/epl/EplErrDef.h
@@ -0,0 +1,294 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  definitions for all EPL-function return codes
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplErrDef.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.9 $  $Date: 2008/06/23 14:56:33 $
+
+                $State: Exp $
+
+                Build Environment:
+                    all
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2005/12/05 -as:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_ERRORDEF_H_
+#define _EPL_ERRORDEF_H_
+
+//---------------------------------------------------------------------------
+// return codes
+//---------------------------------------------------------------------------
+
+typedef enum {
+	// area for generic errors 0x0000 - 0x000F
+	kEplSuccessful = 0x0000,	// no error/successful run
+	kEplIllegalInstance = 0x0001,	// the called Instanz does not exist
+	kEplInvalidInstanceParam = 0x0002,	//
+	kEplNoFreeInstance = 0x0003,	// XxxAddInstance was called but no free instance is available
+	kEplWrongSignature = 0x0004,	// wrong signature while writing to object 0x1010 or 0x1011
+	kEplInvalidOperation = 0x0005,	// operation not allowed in this situation
+	kEplInvalidNodeId = 0x0007,	// invalid NodeId was specified
+	kEplNoResource = 0x0008,	// resource could not be created (Windows, PxROS, ...)
+	kEplShutdown = 0x0009,	// stack is shutting down
+	kEplReject = 0x000A,	// reject the subsequent command
+
+	// area for EDRV module 0x0010 - 0x001F
+//    kEplEdrvNoFrame             = 0x0010,       // no CAN message was received
+//    kEplEdrvMsgHigh             = 0x0011,       // CAN message with high priority was received
+//    kEplEdrvMsgLow              = 0x0012,       // CAN message with low priority was received
+	kEplEdrvInitError = 0x0013,	// initialisation error
+	kEplEdrvNoFreeBufEntry = 0x0014,	// no free entry in internal buffer table for Tx frames
+	kEplEdrvBufNotExisting = 0x0015,	// specified Tx buffer does not exist
+//    kEplEdrvNoFreeChannel       = 0x0014,       // CAN controller has not a free channel
+//    kEplEdrvTxBuffHighOverrun   = 0x0015,       // buffer for high priority CAN transmit messages has overrun
+//    kEplEdrvTxBuffLowOverrun    = 0x0016,       // buffer for low priority CAN transmit messages has overrun
+//    kEplEdrvIllegalBdi          = 0x0017,       // unsupported baudrate within baudrate table
+//    kEplEdrvBusy                = 0x0018,       // remote frame can not be updated because no bus contact or CAN
+	// transmission is activ
+//    kEplEdrvInvalidDriverType   = 0x0019,       // (PC: Windows or Linux) invalid driver type
+//    kEplEdrvDriverNotFound      = 0x001A,       // (PC: Windows or Linux) driver (DLL) could not be found
+//    kEplEdrvInvalidBaseAddress  = 0x001B,       // (PC: Windows or Linux) driver could not found the CAN controller
+//    kEplEdrvInvalidParam        = 0x001C,       // invalid param in function call
+
+	// area for COB module 0x0020 - 0x002F
+/*    kEplCobNoFreeEntry          = 0x0020,       // no free entry in RX- or TX-COB table
+    kEplCobAlreadyExist         = 0x0021,       // COB-ID already exists in RX- resp. TX-COB table
+    */
+	kEplDllIllegalHdl = 0x0022,	// illegal handle for a TxFrame was passed
+	kEplDllCbAsyncRegistered = 0x0023,	// handler for non-EPL frames was already registered before
+//    kEplDllAsyncRxBufferFull    = 0x0024,       // receive buffer for asynchronous frames is full
+	kEplDllAsyncTxBufferEmpty = 0x0025,	// transmit buffer for asynchronous frames is empty
+	kEplDllAsyncTxBufferFull = 0x0026,	// transmit buffer for asynchronous frames is full
+	kEplDllNoNodeInfo = 0x0027,	// MN: too less space in the internal node info structure
+	kEplDllInvalidParam = 0x0028,	// invalid parameters passed to function
+	kEplDllTxBufNotReady = 0x002E,	// TxBuffer (e.g. for PReq) is not ready yet
+	kEplDllTxFrameInvalid = 0x002F,	// TxFrame (e.g. for PReq) is invalid or does not exist
+/*    kEplCobIllegalCanId         = 0x0023,       // COB-ID is not allowed (like 0x000 is reserved for NMT, ...)
+    kEplCobInvalidCanId         = 0x0024,       // COB-ID is switched off
+    kEplCobCdrvStateSet         = 0x0025,       // at least one bit of CAN driver state is set
+    kEplCobNoFreeEntryHighBuf   = 0x0026,       // no free entry in high priotity RX- or TX-COB table
+    kEplCobOwnId                = 0x0027,       // COB-ID already exists in own module which calls CobDefine() or CobCheck()
+*/
+	// area for OBD module 0x0030 - 0x003F
+	kEplObdIllegalPart = 0x0030,	// unknown OD part
+	kEplObdIndexNotExist = 0x0031,	// object index does not exist in OD
+	kEplObdSubindexNotExist = 0x0032,	// subindex does not exist in object index
+	kEplObdReadViolation = 0x0033,	// read access to a write-only object
+	kEplObdWriteViolation = 0x0034,	// write access to a read-only object
+	kEplObdAccessViolation = 0x0035,	// access not allowed
+	kEplObdUnknownObjectType = 0x0036,	// object type not defined/known
+	kEplObdVarEntryNotExist = 0x0037,	// object does not contain VarEntry structure
+	kEplObdValueTooLow = 0x0038,	// value to write to an object is too low
+	kEplObdValueTooHigh = 0x0039,	// value to write to an object is too high
+	kEplObdValueLengthError = 0x003A,	// value to write is to long or to short
+//    kEplObdIllegalFloat         = 0x003B,       // illegal float variable
+//    kEplObdWrongOdBuilderKey    = 0x003F,       // OD was generated with demo version of tool ODBuilder
+
+	// area for NMT module 0x0040 - 0x004F
+	kEplNmtUnknownCommand = 0x0040,	// unknown NMT command
+	kEplNmtInvalidFramePointer = 0x0041,	// pointer to the frame is not valid
+	kEplNmtInvalidEvent = 0x0042,	// invalid event send to NMT-modul
+	kEplNmtInvalidState = 0x0043,	// unknown state in NMT-State-Maschine
+	kEplNmtInvalidParam = 0x0044,	// invalid parameters specified
+
+	// area for SDO/UDP module 0x0050 - 0x005F
+	kEplSdoUdpMissCb = 0x0050,	// missing callback-function pointer during inti of
+	// module
+	kEplSdoUdpNoSocket = 0x0051,	// error during init of socket
+	kEplSdoUdpSocketError = 0x0052,	// error during usage of socket
+	kEplSdoUdpThreadError = 0x0053,	// error during start of listen thread
+	kEplSdoUdpNoFreeHandle = 0x0054,	// no free connection handle for Udp
+	kEplSdoUdpSendError = 0x0055,	// Error during send of frame
+	kEplSdoUdpInvalidHdl = 0x0056,	// the connection handle is invalid
+
+	// area for SDO Sequence layer module 0x0060 - 0x006F
+	kEplSdoSeqMissCb = 0x0060,	// no callback-function assign
+	kEplSdoSeqNoFreeHandle = 0x0061,	// no free handle for connection
+	kEplSdoSeqInvalidHdl = 0x0062,	// invalid handle in SDO sequence layer
+	kEplSdoSeqUnsupportedProt = 0x0063,	// unsupported Protocol selected
+	kEplSdoSeqNoFreeHistory = 0x0064,	// no free entry in history
+	kEplSdoSeqFrameSizeError = 0x0065,	// the size of the frames is not correct
+	kEplSdoSeqRequestAckNeeded = 0x0066,	// indeicates that the history buffer is full
+	// and a ack request is needed
+	kEplSdoSeqInvalidFrame = 0x0067,	// frame not valid
+	kEplSdoSeqConnectionBusy = 0x0068,	// connection is busy -> retry later
+	kEplSdoSeqInvalidEvent = 0x0069,	// invalid event received
+
+	// area for SDO Command Layer Module 0x0070 - 0x007F
+	kEplSdoComUnsupportedProt = 0x0070,	// unsupported Protocol selected
+	kEplSdoComNoFreeHandle = 0x0071,	// no free handle for connection
+	kEplSdoComInvalidServiceType = 0x0072,	// invalid SDO service type specified
+	kEplSdoComInvalidHandle = 0x0073,	// handle invalid
+	kEplSdoComInvalidSendType = 0x0074,	// the stated to of frame to send is
+	// not possible
+	kEplSdoComNotResponsible = 0x0075,	// internal error: command layer handle is
+	// not responsible for this event from sequence layer
+	kEplSdoComHandleExists = 0x0076,	// handle to same node already exists
+	kEplSdoComHandleBusy = 0x0077,	// transfer via this handle is already running
+	kEplSdoComInvalidParam = 0x0078,	// invalid parameters passed to function
+
+	// area for EPL Event-Modul 0x0080 - 0x008F
+	kEplEventUnknownSink = 0x0080,	// unknown sink for event
+	kEplEventPostError = 0x0081,	// error during post of event
+
+	// area for EPL Timer Modul 0x0090 - 0x009F
+	kEplTimerInvalidHandle = 0x0090,	// invalid handle for timer
+	kEplTimerNoTimerCreated = 0x0091,	// no timer was created caused by
+	// an error
+
+	// area for EPL SDO/Asnd Module 0x00A0 - 0x0AF
+	kEplSdoAsndInvalidNodeId = 0x00A0,	//0 node id is invalid
+	kEplSdoAsndNoFreeHandle = 0x00A1,	// no free handle for connection
+	kEplSdoAsndInvalidHandle = 0x00A2,	// handle for connection is invalid
+
+	// area for PDO module 0x00B0 - 0x00BF
+	kEplPdoNotExist = 0x00B0,	// selected PDO does not exist
+	kEplPdoLengthExceeded = 0x00B1,	// length of PDO mapping exceedes 64 bis
+	kEplPdoGranularityMismatch = 0x00B2,	// configured PDO granularity is not equal to supported granularity
+	kEplPdoInitError = 0x00B3,	// error during initialisation of PDO module
+	kEplPdoErrorPdoEncode = 0x00B4,	// error during encoding a PDO
+	kEplPdoErrorPdoDecode = 0x00B5,	// error during decoding a PDO
+	kEplPdoErrorSend = 0x00B6,	// error during sending a PDO
+	kEplPdoErrorSyncWin = 0x00B7,	// the SYNC window runs out during sending SYNC-PDOs
+	kEplPdoErrorMapp = 0x00B8,	// invalid PDO mapping
+	kEplPdoVarNotFound = 0x00B9,	// variable was not found in function PdoSignalVar()
+	kEplPdoErrorEmcyPdoLen = 0x00BA,	// the length of a received PDO is unequal to the expected value
+	kEplPdoWriteConstObject = 0x00BB,	// constant object can not be written
+	// (only TxType, Inhibit-, Event Time for CANopen Kit)
+
+	// area for LSS slave module
+/*    kEplLsssResetNode           = 0x0080,       // NMT command "reset node" has to be processed after LSS configuration
+                                                // new of NodeId
+    kEplLsssInvalidNodeId       = 0x0081,       // no valid NodeId is configured -> wait until it is configured with
+                                                // LSS service before calling CcmConnectToNet()
+*/
+	// area for emergency consumer module 0x0090 - 0x009F
+/*    kEplEmccNoFreeProducerEntry = 0x0090,       // no free entry to add a Emergency Producer
+    kEplEmccNodeIdNotExist      = 0x0091,       // selected NodeId was never added
+    kEplEmccNodeIdInvalid       = 0x0092,       // selected NodeId is outside of range (0x01 until 0x7F)
+    kEplEmccNodeIdExist         = 0x0093,       // selected NodeId already exist
+*/
+	// area for dynamic OD 0x00A0 - 0x00AF
+/*    kEplDynNoMemory             = 0x00A0,       // no memory available
+    kEplDynInvalidConfig        = 0x00A1,       // invalid configuration in segment container
+*/
+	// area for hertbeat consumer module 0x00B0 - 0x00BF
+/*    kEplHbcEntryNotExist        = 0x00B0,       // Heartbeat Producer node not configured
+    kEplHbcEntryAlreadyExist    = 0x00B1,       // NodeId was already defined in heartbeat consumer table (object 0x1016)
+*/
+	// Configuration manager module 0x00C0 - 0x00CF
+	kEplCfgMaConfigError = 0x00C0,	// error in configuration manager
+	kEplCfgMaSdocTimeOutError = 0x00C1,	// error in configuration manager, Sdo timeout
+	kEplCfgMaInvalidDcf = 0x00C2,	// configration file not valid
+	kEplCfgMaUnsupportedDcf = 0x00C3,	// unsupported Dcf format
+	kEplCfgMaConfigWithErrors = 0x00C4,	// configuration finished with errors
+	kEplCfgMaNoFreeConfig = 0x00C5,	// no free configuration entry
+	kEplCfgMaNoConfigData = 0x00C6,	// no configuration data present
+	kEplCfgMaUnsuppDatatypeDcf = 0x00C7,	// unsupported datatype found in dcf
+	// -> this entry was not configured
+
+	// area for LSS master module 0x00D0 - 0x00DF
+/*    kEplLssmIllegalMode         = 0x00D0,       // illegal LSS mode (operation / configuration)
+    kEplLssmIllegalState        = 0x00D1,       // function was called in illegal state of LSS master
+    kEplLssmBusy                = 0x00D2,       // LSS process is busy with an previous service
+    kEplLssmIllegalCmd          = 0x00D3,       // illegal command code was set for function LssmInquireIdentity()
+    kEplLssmTimeout             = 0x00D4,       // LSS slave did not answer a LSS service
+    kEplLssmErrorInConfirm      = 0x00D5,       // LSS slave replied an error code for a LSS service
+*/
+	// area for CCM modules 0x00E0 - 0xEF
+/*    kEplCcmStoreUnvalidState    = 0x00E0,       // memory device not available due device state
+    kEplCcmStoreHwError         = 0x00E1,       // hw error due device access
+*/
+	// area for SRDO module 0x0100 - 0x011F
+/*    kEplSrdoNotExist            = 0x0100,       // selected SRDO does not exist
+    kEplSrdoGranularityMismatch = 0x0101,       // configured SRDO granularity is not equal to supported granularity
+    kEplSrdoCfgTimingError      = 0x0102,       // configuration is not ok (Timing)
+    kEplSrdoCfgIdError          = 0x0103,       // configuration is not ok (CobIds)
+    kEplSrdoCfgCrcError         = 0x0104,       // configuration is not ok (CRC)
+    kEplSrdoNmtError            = 0x0105,       // an action was tried in a wrong NMT state
+    kEplSrdoInvalidCfg          = 0x0106,       // an action was tried with an invald SRDO configuration
+    kEplSrdoInvalid             = 0x0107,       // an action was tried with an invald SRDO
+    kEplSrdoRxTxConflict        = 0x0108,       // an transmission was tried with an receive SRDO (or the other way)
+    kEplSrdoIllegalCanId        = 0x0109,       // the CanId is invalid
+    kEplSrdoCanIdAlreadyInUse   = 0x010A,       // the CanId is already in use
+    kEplSrdoNotInOrder          = 0x010B,       // the two messages of a SRDO are not in order
+    kEplSrdoSctTimeout          = 0x010C,       // timeout of SCT
+    kEplSrdoSrvtTimeout         = 0x010D,       // timeout of SRVT
+    kEplSrdoCanIdNotValid       = 0x010E,       // one of received CAN-IDs are not equal to configured one
+    kEplSrdoDlcNotValid         = 0x010F,       // one of received CAN-DLC are not equal to configured one
+    kEplSrdoErrorMapp           = 0x0110,       // wrong values in mapping found
+    kEplSrdoDataError           = 0x0111,       // data of CAN messages are not invers
+    kEplSrdoLengthExceeded      = 0x0112,       // length of SRDO mapping exceedes 64 bit per CAN-message
+    kEplSrdoNotHandledInApp     = 0x0113,       // the SRDO error was not handled in AppSrdoError()
+    kEplSrdoOverrun             = 0x0114        // a RxSRDO was received but the pevious one was not else processed
+*/
+
+	kEplApiTaskDeferred = 0x0140,	// EPL performs task in background and informs the application (or vice-versa), when it is finished
+	kEplApiInvalidParam = 0x0142,	// passed invalid parameters to a function (e.g. invalid node id)
+
+	// area untill 0x07FF is reserved
+	// area for user application from 0x0800 to 0x7FFF
+
+} tEplKernel;
+
+#endif
+//EOF
+
+// Die letzte Zeile muß unbedingt eine leere Zeile sein, weil manche Compiler
+// damit ein Problem haben, wenn das nicht so ist (z.B. GNU oder Borland C++ Builder).
diff --git a/drivers/staging/epl/EplErrorHandlerk.c b/drivers/staging/epl/EplErrorHandlerk.c
new file mode 100644
index 00000000000..d12521fc99a
--- /dev/null
+++ b/drivers/staging/epl/EplErrorHandlerk.c
@@ -0,0 +1,810 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for error handler module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplErrorHandlerk.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.9 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/10/02 d.k.:   start of the implementation
+
+****************************************************************************/
+
+#include "kernel/EplErrorHandlerk.h"
+#include "EplNmt.h"
+#include "kernel/EplEventk.h"
+#include "kernel/EplObdk.h"	// function prototyps of the EplOBD-Modul
+#include "kernel/EplDllk.h"
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) == 0)
+#error "EPL ErrorHandler module needs EPL module OBDK!"
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+typedef struct {
+	DWORD m_dwCumulativeCnt;	// subindex 1
+	DWORD m_dwThresholdCnt;	// subindex 2
+	DWORD m_dwThreshold;	// subindex 3
+
+} tEplErrorHandlerkErrorCounter;
+
+typedef struct {
+	tEplErrorHandlerkErrorCounter m_CnLossSoc;	// object 0x1C0B
+	tEplErrorHandlerkErrorCounter m_CnLossPreq;	// object 0x1C0D
+	tEplErrorHandlerkErrorCounter m_CnCrcErr;	// object 0x1C0F
+	unsigned long m_ulDllErrorEvents;
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+	tEplErrorHandlerkErrorCounter m_MnCrcErr;	// object 0x1C00
+	tEplErrorHandlerkErrorCounter m_MnCycTimeExceed;	// object 0x1C02
+	DWORD m_adwMnCnLossPresCumCnt[254];	// object 0x1C07
+	DWORD m_adwMnCnLossPresThrCnt[254];	// object 0x1C08
+	DWORD m_adwMnCnLossPresThreshold[254];	// object 0x1C09
+	BOOL m_afMnCnLossPresEvent[254];
+#endif
+
+} tEplErrorHandlerkInstance;
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+static tEplErrorHandlerkInstance EplErrorHandlerkInstance_g;
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+static tEplKernel EplErrorHandlerkLinkErrorCounter(tEplErrorHandlerkErrorCounter
+						   * pErrorCounter_p,
+						   unsigned int uiIndex_p);
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+static tEplKernel EplErrorHandlerkLinkArray(DWORD * pdwValue_p,
+					    unsigned int uiValueCount_p,
+					    unsigned int uiIndex_p);
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  <Epl-Kernelspace-Error-Handler>                     */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description:
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplErrorHandlerkInit
+//
+// Description: function initialize the first instance
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:      tEpKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplErrorHandlerkInit(void)
+{
+	tEplKernel Ret;
+
+	Ret = EplErrorHandlerkAddInstance();
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplErrorHandlerkAddInstance
+//
+// Description: function add one more instance
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:      tEpKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplErrorHandlerkAddInstance(void)
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// reset only event variable,
+	// all other instance members are reset by OD or may keep their current value
+	// d.k.: this is necessary for the cumulative counters, which shall not be reset
+	EplErrorHandlerkInstance_g.m_ulDllErrorEvents = 0;
+
+	// link counters to OD
+	// $$$ d.k. if OD resides in userspace, fetch pointer to shared memory,
+	//          which shall have the same structure as the instance (needs to be declared globally).
+	//          Other idea: error counter shall belong to the process image
+	//          (reset of counters by SDO write are a little bit tricky).
+
+	Ret =
+	    EplErrorHandlerkLinkErrorCounter(&EplErrorHandlerkInstance_g.
+					     m_CnLossSoc, 0x1C0B);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	Ret =
+	    EplErrorHandlerkLinkErrorCounter(&EplErrorHandlerkInstance_g.
+					     m_CnLossPreq, 0x1C0D);
+	// ignore return code, because object 0x1C0D is conditional
+
+	Ret =
+	    EplErrorHandlerkLinkErrorCounter(&EplErrorHandlerkInstance_g.
+					     m_CnCrcErr, 0x1C0F);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+	Ret =
+	    EplErrorHandlerkLinkErrorCounter(&EplErrorHandlerkInstance_g.
+					     m_MnCrcErr, 0x1C00);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	Ret =
+	    EplErrorHandlerkLinkErrorCounter(&EplErrorHandlerkInstance_g.
+					     m_MnCycTimeExceed, 0x1C02);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	Ret =
+	    EplErrorHandlerkLinkArray(EplErrorHandlerkInstance_g.
+				      m_adwMnCnLossPresCumCnt,
+				      tabentries(EplErrorHandlerkInstance_g.
+						 m_adwMnCnLossPresCumCnt),
+				      0x1C07);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	Ret =
+	    EplErrorHandlerkLinkArray(EplErrorHandlerkInstance_g.
+				      m_adwMnCnLossPresThrCnt,
+				      tabentries(EplErrorHandlerkInstance_g.
+						 m_adwMnCnLossPresThrCnt),
+				      0x1C08);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	Ret =
+	    EplErrorHandlerkLinkArray(EplErrorHandlerkInstance_g.
+				      m_adwMnCnLossPresThreshold,
+				      tabentries(EplErrorHandlerkInstance_g.
+						 m_adwMnCnLossPresThreshold),
+				      0x1C09);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#endif
+
+      Exit:
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplErrorHandlerkDelInstance
+//
+// Description: function delete instance an free the bufferstructure
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:      tEpKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplErrorHandlerkDelInstance()
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplErrorHandlerkProcess
+//
+// Description: processes error events from DLL
+//
+//
+//
+// Parameters:  pEvent_p = pointer to event-structur from buffer
+//
+//
+// Returns:      tEpKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplErrorHandlerkProcess(tEplEvent * pEvent_p)
+{
+	tEplKernel Ret;
+	unsigned long ulDllErrorEvents;
+	tEplEvent Event;
+	tEplNmtEvent NmtEvent;
+
+	Ret = kEplSuccessful;
+
+	// check m_EventType
+	switch (pEvent_p->m_EventType) {
+	case kEplEventTypeDllError:
+		{
+			tEplErrorHandlerkEvent *pErrHandlerEvent =
+			    (tEplErrorHandlerkEvent *) pEvent_p->m_pArg;
+
+			ulDllErrorEvents = pErrHandlerEvent->m_ulDllErrorEvents;
+
+			// check the several error events
+			if ((EplErrorHandlerkInstance_g.m_CnLossSoc.
+			     m_dwThreshold > 0)
+			    && ((ulDllErrorEvents & EPL_DLL_ERR_CN_LOSS_SOC) != 0)) {	// loss of SoC event occured
+				// increment cumulative counter by 1
+				EplErrorHandlerkInstance_g.m_CnLossSoc.
+				    m_dwCumulativeCnt++;
+				// increment threshold counter by 8
+				EplErrorHandlerkInstance_g.m_CnLossSoc.
+				    m_dwThresholdCnt += 8;
+				if (EplErrorHandlerkInstance_g.m_CnLossSoc.m_dwThresholdCnt >= EplErrorHandlerkInstance_g.m_CnLossSoc.m_dwThreshold) {	// threshold is reached
+					// $$$ d.k.: generate error history entry E_DLL_LOSS_SOC_TH
+
+					// post event to NMT state machine
+					NmtEvent = kEplNmtEventNmtCycleError;
+					Event.m_EventSink = kEplEventSinkNmtk;
+					Event.m_EventType =
+					    kEplEventTypeNmtEvent;
+					Event.m_pArg = &NmtEvent;
+					Event.m_uiSize = sizeof(NmtEvent);
+					Ret = EplEventkPost(&Event);
+				}
+				EplErrorHandlerkInstance_g.m_ulDllErrorEvents |=
+				    EPL_DLL_ERR_CN_LOSS_SOC;
+			}
+
+			if ((EplErrorHandlerkInstance_g.m_CnLossPreq.
+			     m_dwThreshold > 0)
+			    && ((ulDllErrorEvents & EPL_DLL_ERR_CN_LOSS_PREQ) != 0)) {	// loss of PReq event occured
+				// increment cumulative counter by 1
+				EplErrorHandlerkInstance_g.m_CnLossPreq.
+				    m_dwCumulativeCnt++;
+				// increment threshold counter by 8
+				EplErrorHandlerkInstance_g.m_CnLossPreq.
+				    m_dwThresholdCnt += 8;
+				if (EplErrorHandlerkInstance_g.m_CnLossPreq.m_dwThresholdCnt >= EplErrorHandlerkInstance_g.m_CnLossPreq.m_dwThreshold) {	// threshold is reached
+					// $$$ d.k.: generate error history entry E_DLL_LOSS_PREQ_TH
+
+					// post event to NMT state machine
+					NmtEvent = kEplNmtEventNmtCycleError;
+					Event.m_EventSink = kEplEventSinkNmtk;
+					Event.m_EventType =
+					    kEplEventTypeNmtEvent;
+					Event.m_pArg = &NmtEvent;
+					Event.m_uiSize = sizeof(NmtEvent);
+					Ret = EplEventkPost(&Event);
+				}
+			}
+
+			if ((EplErrorHandlerkInstance_g.m_CnLossPreq.
+			     m_dwThresholdCnt > 0)
+			    && ((ulDllErrorEvents & EPL_DLL_ERR_CN_RECVD_PREQ) != 0)) {	// PReq correctly received
+				// decrement threshold counter by 1
+				EplErrorHandlerkInstance_g.m_CnLossPreq.
+				    m_dwThresholdCnt--;
+			}
+
+			if ((EplErrorHandlerkInstance_g.m_CnCrcErr.
+			     m_dwThreshold > 0)
+			    && ((ulDllErrorEvents & EPL_DLL_ERR_CN_CRC) != 0)) {	// CRC error event occured
+				// increment cumulative counter by 1
+				EplErrorHandlerkInstance_g.m_CnCrcErr.
+				    m_dwCumulativeCnt++;
+				// increment threshold counter by 8
+				EplErrorHandlerkInstance_g.m_CnCrcErr.
+				    m_dwThresholdCnt += 8;
+				if (EplErrorHandlerkInstance_g.m_CnCrcErr.m_dwThresholdCnt >= EplErrorHandlerkInstance_g.m_CnCrcErr.m_dwThreshold) {	// threshold is reached
+					// $$$ d.k.: generate error history entry E_DLL_CRC_TH
+
+					// post event to NMT state machine
+					NmtEvent = kEplNmtEventNmtCycleError;
+					Event.m_EventSink = kEplEventSinkNmtk;
+					Event.m_EventType =
+					    kEplEventTypeNmtEvent;
+					Event.m_pArg = &NmtEvent;
+					Event.m_uiSize = sizeof(NmtEvent);
+					Ret = EplEventkPost(&Event);
+				}
+				EplErrorHandlerkInstance_g.m_ulDllErrorEvents |=
+				    EPL_DLL_ERR_CN_CRC;
+			}
+
+			if ((ulDllErrorEvents & EPL_DLL_ERR_INVALID_FORMAT) != 0) {	// invalid format error occured (only direct reaction)
+				// $$$ d.k.: generate error history entry E_DLL_INVALID_FORMAT
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+				if (pErrHandlerEvent->m_NmtState >= kEplNmtMsNotActive) {	// MN is active
+					if (pErrHandlerEvent->m_uiNodeId != 0) {
+						tEplHeartbeatEvent
+						    HeartbeatEvent;
+
+						// remove node from isochronous phase
+						Ret =
+						    EplDllkDeleteNode
+						    (pErrHandlerEvent->
+						     m_uiNodeId);
+
+						// inform NmtMnu module about state change, which shall send NMT command ResetNode to this CN
+						HeartbeatEvent.m_uiNodeId =
+						    pErrHandlerEvent->
+						    m_uiNodeId;
+						HeartbeatEvent.m_NmtState =
+						    kEplNmtCsNotActive;
+						HeartbeatEvent.m_wErrorCode =
+						    EPL_E_DLL_INVALID_FORMAT;
+						Event.m_EventSink =
+						    kEplEventSinkNmtMnu;
+						Event.m_EventType =
+						    kEplEventTypeHeartbeat;
+						Event.m_uiSize =
+						    sizeof(HeartbeatEvent);
+						Event.m_pArg = &HeartbeatEvent;
+						Ret = EplEventkPost(&Event);
+					}
+					// $$$ and else should lead to InternComError
+				} else
+#endif
+				{	// CN is active
+					// post event to NMT state machine
+					NmtEvent = kEplNmtEventInternComError;
+					Event.m_EventSink = kEplEventSinkNmtk;
+					Event.m_EventType =
+					    kEplEventTypeNmtEvent;
+					Event.m_pArg = &NmtEvent;
+					Event.m_uiSize = sizeof(NmtEvent);
+					Ret = EplEventkPost(&Event);
+				}
+			}
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+			if ((EplErrorHandlerkInstance_g.m_MnCrcErr.
+			     m_dwThreshold > 0)
+			    && ((ulDllErrorEvents & EPL_DLL_ERR_MN_CRC) != 0)) {	// CRC error event occured
+				// increment cumulative counter by 1
+				EplErrorHandlerkInstance_g.m_MnCrcErr.
+				    m_dwCumulativeCnt++;
+				// increment threshold counter by 8
+				EplErrorHandlerkInstance_g.m_MnCrcErr.
+				    m_dwThresholdCnt += 8;
+				if (EplErrorHandlerkInstance_g.m_MnCrcErr.m_dwThresholdCnt >= EplErrorHandlerkInstance_g.m_MnCrcErr.m_dwThreshold) {	// threshold is reached
+					// $$$ d.k.: generate error history entry E_DLL_CRC_TH
+
+					// post event to NMT state machine
+					NmtEvent = kEplNmtEventNmtCycleError;
+					Event.m_EventSink = kEplEventSinkNmtk;
+					Event.m_EventType =
+					    kEplEventTypeNmtEvent;
+					Event.m_pArg = &NmtEvent;
+					Event.m_uiSize = sizeof(NmtEvent);
+					Ret = EplEventkPost(&Event);
+				}
+				EplErrorHandlerkInstance_g.m_ulDllErrorEvents |=
+				    EPL_DLL_ERR_MN_CRC;
+			}
+
+			if ((EplErrorHandlerkInstance_g.m_MnCycTimeExceed.
+			     m_dwThreshold > 0)
+			    && ((ulDllErrorEvents & EPL_DLL_ERR_MN_CYCTIMEEXCEED) != 0)) {	// cycle time exceeded event occured
+				// increment cumulative counter by 1
+				EplErrorHandlerkInstance_g.m_MnCycTimeExceed.
+				    m_dwCumulativeCnt++;
+				// increment threshold counter by 8
+				EplErrorHandlerkInstance_g.m_MnCycTimeExceed.
+				    m_dwThresholdCnt += 8;
+				if (EplErrorHandlerkInstance_g.m_MnCycTimeExceed.m_dwThresholdCnt >= EplErrorHandlerkInstance_g.m_MnCycTimeExceed.m_dwThreshold) {	// threshold is reached
+					// $$$ d.k.: generate error history entry E_DLL_CYCLE_EXCEED_TH
+
+					// post event to NMT state machine
+					NmtEvent = kEplNmtEventNmtCycleError;
+					Event.m_EventSink = kEplEventSinkNmtk;
+					Event.m_EventType =
+					    kEplEventTypeNmtEvent;
+					Event.m_pArg = &NmtEvent;
+					Event.m_uiSize = sizeof(NmtEvent);
+					Ret = EplEventkPost(&Event);
+				}
+				// $$$ d.k.: else generate error history entry E_DLL_CYCLE_EXCEED
+				EplErrorHandlerkInstance_g.m_ulDllErrorEvents |=
+				    EPL_DLL_ERR_MN_CYCTIMEEXCEED;
+			}
+
+			if ((ulDllErrorEvents & EPL_DLL_ERR_MN_CN_LOSS_PRES) != 0) {	// CN loss PRes event occured
+				unsigned int uiNodeId;
+
+				uiNodeId = pErrHandlerEvent->m_uiNodeId - 1;
+				if ((uiNodeId <
+				     tabentries(EplErrorHandlerkInstance_g.
+						m_adwMnCnLossPresCumCnt))
+				    && (EplErrorHandlerkInstance_g.
+					m_adwMnCnLossPresThreshold[uiNodeId] >
+					0)) {
+					// increment cumulative counter by 1
+					EplErrorHandlerkInstance_g.
+					    m_adwMnCnLossPresCumCnt[uiNodeId]++;
+					// increment threshold counter by 8
+					EplErrorHandlerkInstance_g.
+					    m_adwMnCnLossPresThrCnt[uiNodeId] +=
+					    8;
+					if (EplErrorHandlerkInstance_g.
+					    m_adwMnCnLossPresThrCnt[uiNodeId]
+					    >= EplErrorHandlerkInstance_g.m_adwMnCnLossPresThreshold[uiNodeId]) {	// threshold is reached
+						tEplHeartbeatEvent
+						    HeartbeatEvent;
+
+						// $$$ d.k.: generate error history entry E_DLL_LOSS_PRES_TH
+
+						// remove node from isochronous phase
+						Ret =
+						    EplDllkDeleteNode
+						    (pErrHandlerEvent->
+						     m_uiNodeId);
+
+						// inform NmtMnu module about state change, which shall send NMT command ResetNode to this CN
+						HeartbeatEvent.m_uiNodeId =
+						    pErrHandlerEvent->
+						    m_uiNodeId;
+						HeartbeatEvent.m_NmtState =
+						    kEplNmtCsNotActive;
+						HeartbeatEvent.m_wErrorCode =
+						    EPL_E_DLL_LOSS_PRES_TH;
+						Event.m_EventSink =
+						    kEplEventSinkNmtMnu;
+						Event.m_EventType =
+						    kEplEventTypeHeartbeat;
+						Event.m_uiSize =
+						    sizeof(HeartbeatEvent);
+						Event.m_pArg = &HeartbeatEvent;
+						Ret = EplEventkPost(&Event);
+					}
+					EplErrorHandlerkInstance_g.
+					    m_afMnCnLossPresEvent[uiNodeId] =
+					    TRUE;
+				}
+			}
+#endif
+
+			break;
+		}
+
+		// NMT event
+	case kEplEventTypeNmtEvent:
+		{
+			if ((*(tEplNmtEvent *) pEvent_p->m_pArg) == kEplNmtEventDllCeSoa) {	// SoA event of CN -> decrement threshold counters
+
+				if ((EplErrorHandlerkInstance_g.m_ulDllErrorEvents & EPL_DLL_ERR_CN_LOSS_SOC) == 0) {	// decrement loss of SoC threshold counter, because it didn't occur last cycle
+					if (EplErrorHandlerkInstance_g.
+					    m_CnLossSoc.m_dwThresholdCnt > 0) {
+						EplErrorHandlerkInstance_g.
+						    m_CnLossSoc.
+						    m_dwThresholdCnt--;
+					}
+				}
+
+				if ((EplErrorHandlerkInstance_g.m_ulDllErrorEvents & EPL_DLL_ERR_CN_CRC) == 0) {	// decrement CRC threshold counter, because it didn't occur last cycle
+					if (EplErrorHandlerkInstance_g.
+					    m_CnCrcErr.m_dwThresholdCnt > 0) {
+						EplErrorHandlerkInstance_g.
+						    m_CnCrcErr.
+						    m_dwThresholdCnt--;
+					}
+				}
+			}
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+			else if ((*(tEplNmtEvent *) pEvent_p->m_pArg) == kEplNmtEventDllMeSoaSent) {	// SoA event of MN -> decrement threshold counters
+				tEplDllkNodeInfo *pIntNodeInfo;
+				unsigned int uiNodeId;
+
+				Ret = EplDllkGetFirstNodeInfo(&pIntNodeInfo);
+				if (Ret != kEplSuccessful) {
+					break;
+				}
+				// iterate through node info structure list
+				while (pIntNodeInfo != NULL) {
+					uiNodeId = pIntNodeInfo->m_uiNodeId - 1;
+					if (uiNodeId <
+					    tabentries
+					    (EplErrorHandlerkInstance_g.
+					     m_adwMnCnLossPresCumCnt)) {
+						if (EplErrorHandlerkInstance_g.
+						    m_afMnCnLossPresEvent
+						    [uiNodeId] == FALSE) {
+							if (EplErrorHandlerkInstance_g.m_adwMnCnLossPresThrCnt[uiNodeId] > 0) {
+								EplErrorHandlerkInstance_g.
+								    m_adwMnCnLossPresThrCnt
+								    [uiNodeId]--;
+							}
+						} else {
+							EplErrorHandlerkInstance_g.
+							    m_afMnCnLossPresEvent
+							    [uiNodeId] = FALSE;
+						}
+					}
+					pIntNodeInfo =
+					    pIntNodeInfo->m_pNextNodeInfo;
+				}
+
+				if ((EplErrorHandlerkInstance_g.m_ulDllErrorEvents & EPL_DLL_ERR_MN_CRC) == 0) {	// decrement CRC threshold counter, because it didn't occur last cycle
+					if (EplErrorHandlerkInstance_g.
+					    m_MnCrcErr.m_dwThresholdCnt > 0) {
+						EplErrorHandlerkInstance_g.
+						    m_MnCrcErr.
+						    m_dwThresholdCnt--;
+					}
+				}
+
+				if ((EplErrorHandlerkInstance_g.m_ulDllErrorEvents & EPL_DLL_ERR_MN_CYCTIMEEXCEED) == 0) {	// decrement cycle exceed threshold counter, because it didn't occur last cycle
+					if (EplErrorHandlerkInstance_g.
+					    m_MnCycTimeExceed.m_dwThresholdCnt >
+					    0) {
+						EplErrorHandlerkInstance_g.
+						    m_MnCycTimeExceed.
+						    m_dwThresholdCnt--;
+					}
+				}
+			}
+#endif
+
+			// reset error events
+			EplErrorHandlerkInstance_g.m_ulDllErrorEvents = 0L;
+
+			break;
+		}
+
+		// unknown type
+	default:
+		{
+		}
+
+	}			// end of switch(pEvent_p->m_EventType)
+
+	return Ret;
+
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplErrorHandlerkLinkErrorCounter
+//
+// Description: link specified error counter structure to OD entry
+//
+// Parameters:  pErrorCounter_p         = pointer to error counter structure
+//              uiIndex_p               = OD index
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplErrorHandlerkLinkErrorCounter(tEplErrorHandlerkErrorCounter
+						   * pErrorCounter_p,
+						   unsigned int uiIndex_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplVarParam VarParam;
+
+	VarParam.m_pData = &pErrorCounter_p->m_dwCumulativeCnt;
+	VarParam.m_Size = sizeof(DWORD);
+	VarParam.m_uiIndex = uiIndex_p;
+	VarParam.m_uiSubindex = 0x01;
+	VarParam.m_ValidFlag = kVarValidAll;
+	Ret = EplObdDefineVar(&VarParam);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	VarParam.m_pData = &pErrorCounter_p->m_dwThresholdCnt;
+	VarParam.m_Size = sizeof(DWORD);
+	VarParam.m_uiIndex = uiIndex_p;
+	VarParam.m_uiSubindex = 0x02;
+	VarParam.m_ValidFlag = kVarValidAll;
+	Ret = EplObdDefineVar(&VarParam);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	VarParam.m_pData = &pErrorCounter_p->m_dwThreshold;
+	VarParam.m_Size = sizeof(DWORD);
+	VarParam.m_uiIndex = uiIndex_p;
+	VarParam.m_uiSubindex = 0x03;
+	VarParam.m_ValidFlag = kVarValidAll;
+	Ret = EplObdDefineVar(&VarParam);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplErrorHandlerkLinkErrorCounter
+//
+// Description: link specified error counter structure to OD entry
+//
+// Parameters:  pErrorCounter_p         = pointer to error counter structure
+//              uiIndex_p               = OD index
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+static tEplKernel EplErrorHandlerkLinkArray(DWORD * pdwValue_p,
+					    unsigned int uiValueCount_p,
+					    unsigned int uiIndex_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplVarParam VarParam;
+	tEplObdSize EntrySize;
+	BYTE bIndexEntries;
+
+	EntrySize = (tEplObdSize) sizeof(bIndexEntries);
+	Ret = EplObdReadEntry(uiIndex_p,
+			      0x00, (void GENERIC *)&bIndexEntries, &EntrySize);
+
+	if ((Ret != kEplSuccessful) || (bIndexEntries == 0x00)) {
+		// Object doesn't exist or invalid entry number
+		Ret = kEplObdIndexNotExist;
+		goto Exit;
+	}
+
+	if (bIndexEntries < uiValueCount_p) {
+		uiValueCount_p = bIndexEntries;
+	}
+
+	VarParam.m_Size = sizeof(DWORD);
+	VarParam.m_uiIndex = uiIndex_p;
+	VarParam.m_ValidFlag = kVarValidAll;
+
+	for (VarParam.m_uiSubindex = 0x01;
+	     VarParam.m_uiSubindex <= uiValueCount_p; VarParam.m_uiSubindex++) {
+		VarParam.m_pData = pdwValue_p;
+		Ret = EplObdDefineVar(&VarParam);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+		pdwValue_p++;
+	}
+
+      Exit:
+	return Ret;
+}
+#endif //(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+#endif //(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+
+// EOF
diff --git a/drivers/staging/epl/EplEvent.h b/drivers/staging/epl/EplEvent.h
new file mode 100644
index 00000000000..b6dc1b9b2ab
--- /dev/null
+++ b/drivers/staging/epl/EplEvent.h
@@ -0,0 +1,279 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for event module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplEvent.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.8 $  $Date: 2008/11/17 16:40:39 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/12 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_EVENT_H_
+#define _EPL_EVENT_H_
+
+#include "EplInc.h"
+#include "EplNmt.h"
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+// name and size of event queues
+#define EPL_EVENT_NAME_SHB_KERNEL_TO_USER   "ShbKernelToUser"
+#ifndef EPL_EVENT_SIZE_SHB_KERNEL_TO_USER
+#define EPL_EVENT_SIZE_SHB_KERNEL_TO_USER   32768	// 32 kByte
+#endif
+
+#define EPL_EVENT_NAME_SHB_USER_TO_KERNEL   "ShbUserToKernel"
+#ifndef EPL_EVENT_SIZE_SHB_USER_TO_KERNEL
+#define EPL_EVENT_SIZE_SHB_USER_TO_KERNEL   32768	// 32 kByte
+#endif
+
+// max size of event argument
+#ifndef EPL_MAX_EVENT_ARG_SIZE
+#define EPL_MAX_EVENT_ARG_SIZE      256	// because of PDO
+#endif
+
+#define EPL_DLL_ERR_MN_CRC           0x00000001L	// object 0x1C00
+#define EPL_DLL_ERR_MN_COLLISION     0x00000002L	// object 0x1C01
+#define EPL_DLL_ERR_MN_CYCTIMEEXCEED 0x00000004L	// object 0x1C02
+#define EPL_DLL_ERR_MN_LOSS_LINK     0x00000008L	// object 0x1C03
+#define EPL_DLL_ERR_MN_CN_LATE_PRES  0x00000010L	// objects 0x1C04-0x1C06
+#define EPL_DLL_ERR_MN_CN_LOSS_PRES  0x00000080L	// objects 0x1C07-0x1C09
+#define EPL_DLL_ERR_CN_COLLISION     0x00000400L	// object 0x1C0A
+#define EPL_DLL_ERR_CN_LOSS_SOC      0x00000800L	// object 0x1C0B
+#define EPL_DLL_ERR_CN_LOSS_SOA      0x00001000L	// object 0x1C0C
+#define EPL_DLL_ERR_CN_LOSS_PREQ     0x00002000L	// object 0x1C0D
+#define EPL_DLL_ERR_CN_RECVD_PREQ    0x00004000L	// decrement object 0x1C0D/2
+#define EPL_DLL_ERR_CN_SOC_JITTER    0x00008000L	// object 0x1C0E
+#define EPL_DLL_ERR_CN_CRC           0x00010000L	// object 0x1C0F
+#define EPL_DLL_ERR_CN_LOSS_LINK     0x00020000L	// object 0x1C10
+#define EPL_DLL_ERR_MN_LOSS_STATRES  0x00040000L	// objects 0x1C15-0x1C17 (should be operated by NmtMnu module)
+#define EPL_DLL_ERR_BAD_PHYS_MODE    0x00080000L	// no object
+#define EPL_DLL_ERR_MAC_BUFFER       0x00100000L	// no object (NMT_GT6)
+#define EPL_DLL_ERR_INVALID_FORMAT   0x00200000L	// no object (NMT_GT6)
+#define EPL_DLL_ERR_ADDRESS_CONFLICT 0x00400000L	// no object (remove CN from configuration)
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+// EventType determines the argument of the event
+typedef enum {
+	kEplEventTypeNmtEvent = 0x01,	// NMT event
+	// arg is pointer to tEplNmtEvent
+	kEplEventTypePdoRx = 0x02,	// PDO frame received event (PRes/PReq)
+	// arg is pointer to tEplFrame
+	kEplEventTypePdoTx = 0x03,	// PDO frame transmitted event (PRes/PReq)
+	// arg is pointer to tEplFrameInfo
+	kEplEventTypePdoSoa = 0x04,	// SoA frame received event (isochronous phase completed)
+	// arg is pointer to nothing
+	kEplEventTypeSync = 0x05,	// Sync event (e.g. SoC or anticipated SoC)
+	// arg is pointer to nothing
+	kEplEventTypeTimer = 0x06,	// Timer event
+	// arg is pointer to tEplTimerEventArg
+	kEplEventTypeHeartbeat = 0x07,	// Heartbeat event
+	// arg is pointer to tEplHeartbeatEvent
+	kEplEventTypeDllkCreate = 0x08,	// DLL kernel create event
+	// arg is pointer to the new tEplNmtState
+	kEplEventTypeDllkDestroy = 0x09,	// DLL kernel destroy event
+	// arg is pointer to the old tEplNmtState
+	kEplEventTypeDllkFillTx = 0x0A,	// DLL kernel fill TxBuffer event
+	// arg is pointer to tEplDllAsyncReqPriority
+	kEplEventTypeDllkPresReady = 0x0B,	// DLL kernel PRes ready event
+	// arg is pointer to nothing
+	kEplEventTypeError = 0x0C,	// Error event for API layer
+	// arg is pointer to tEplEventError
+	kEplEventTypeNmtStateChange = 0x0D,	// indicate change of NMT-State
+	// arg is pointer to tEplEventNmtStateChange
+	kEplEventTypeDllError = 0x0E,	// DLL error event for Error handler
+	// arg is pointer to tEplErrorHandlerkEvent
+	kEplEventTypeAsndRx = 0x0F,	// received ASnd frame for DLL user module
+	// arg is pointer to tEplFrame
+	kEplEventTypeDllkServFilter = 0x10,	// configure ServiceIdFilter
+	// arg is pointer to tEplDllCalServiceIdFilter
+	kEplEventTypeDllkIdentity = 0x11,	// configure Identity
+	// arg is pointer to tEplDllIdentParam
+	kEplEventTypeDllkConfig = 0x12,	// configure ConfigParam
+	// arg is pointer to tEplDllConfigParam
+	kEplEventTypeDllkIssueReq = 0x13,	// issue Ident/Status request
+	// arg is pointer to tEplDllCalIssueRequest
+	kEplEventTypeDllkAddNode = 0x14,	// add node to isochronous phase
+	// arg is pointer to tEplDllNodeInfo
+	kEplEventTypeDllkDelNode = 0x15,	// remove node from isochronous phase
+	// arg is pointer to unsigned int
+	kEplEventTypeDllkSoftDelNode = 0x16,	// remove node softly from isochronous phase
+	// arg is pointer to unsigned int
+	kEplEventTypeDllkStartReducedCycle = 0x17,	// start reduced EPL cycle on MN
+	// arg is pointer to nothing
+	kEplEventTypeNmtMnuNmtCmdSent = 0x18,	// NMT command was actually sent
+	// arg is pointer to tEplFrame
+
+} tEplEventType;
+
+// EventSink determines the consumer of the event
+typedef enum {
+	kEplEventSinkSync = 0x00,	// Sync event for application or kernel EPL module
+	kEplEventSinkNmtk = 0x01,	// events for Nmtk module
+	kEplEventSinkDllk = 0x02,	// events for Dllk module
+	kEplEventSinkDlluCal = 0x03,	// events for DlluCal module
+	kEplEventSinkDllkCal = 0x04,	// events for DllkCal module
+	kEplEventSinkPdok = 0x05,	// events for Pdok module
+	kEplEventSinkNmtu = 0x06,	// events for Nmtu module
+	kEplEventSinkErrk = 0x07,	// events for Error handler module
+	kEplEventSinkErru = 0x08,	// events for Error signaling module
+	kEplEventSinkSdoAsySeq = 0x09,	// events for asyncronous SDO Sequence Layer module
+	kEplEventSinkNmtMnu = 0x0A,	// events for NmtMnu module
+	kEplEventSinkLedu = 0x0B,	// events for Ledu module
+	kEplEventSinkApi = 0x0F,	// events for API module
+
+} tEplEventSink;
+
+// EventSource determines the source of an errorevent
+typedef enum {
+	// kernelspace modules
+	kEplEventSourceDllk = 0x01,	// Dllk module
+	kEplEventSourceNmtk = 0x02,	// Nmtk module
+	kEplEventSourceObdk = 0x03,	// Obdk module
+	kEplEventSourcePdok = 0x04,	// Pdok module
+	kEplEventSourceTimerk = 0x05,	// Timerk module
+	kEplEventSourceEventk = 0x06,	// Eventk module
+	kEplEventSourceSyncCb = 0x07,	// sync-Cb
+	kEplEventSourceErrk = 0x08,	// Error handler module
+
+	// userspace modules
+	kEplEventSourceDllu = 0x10,	// Dllu module
+	kEplEventSourceNmtu = 0x11,	// Nmtu module
+	kEplEventSourceNmtCnu = 0x12,	// NmtCnu module
+	kEplEventSourceNmtMnu = 0x13,	// NmtMnu module
+	kEplEventSourceObdu = 0x14,	// Obdu module
+	kEplEventSourceSdoUdp = 0x15,	// Sdo/Udp module
+	kEplEventSourceSdoAsnd = 0x16,	// Sdo/Asnd module
+	kEplEventSourceSdoAsySeq = 0x17,	// Sdo asynchronus Sequence Layer module
+	kEplEventSourceSdoCom = 0x18,	// Sdo command layer module
+	kEplEventSourceTimeru = 0x19,	// Timeru module
+	kEplEventSourceCfgMau = 0x1A,	// CfgMau module
+	kEplEventSourceEventu = 0x1B,	// Eventu module
+	kEplEventSourceEplApi = 0x1C,	// Api module
+	kEplEventSourceLedu = 0x1D,	// Ledu module
+
+} tEplEventSource;
+
+// structure of EPL event (element order must not be changed!)
+typedef struct {
+	tEplEventType m_EventType /*:28 */ ;	// event type
+	tEplEventSink m_EventSink /*:4 */ ;	// event sink
+	tEplNetTime m_NetTime;	// timestamp
+	unsigned int m_uiSize;	// size of argument
+	void *m_pArg;		// argument of event
+
+} tEplEvent;
+
+// short structure of EPL event without argument and its size (element order must not be changed!)
+typedef struct {
+	tEplEventType m_EventType /*:28 */ ;	// event type
+	tEplEventSink m_EventSink /*:4 */ ;	// event sink
+	tEplNetTime m_NetTime;	// timestamp
+
+} tEplEventShort;
+
+typedef struct {
+	unsigned int m_uiIndex;
+	unsigned int m_uiSubIndex;
+
+} tEplEventObdError;
+
+// structure for kEplEventTypeError
+typedef struct {
+	tEplEventSource m_EventSource;	// module which posted this error event
+	tEplKernel m_EplError;	// EPL error which occured
+	union {
+		BYTE m_bArg;
+		DWORD m_dwArg;
+		tEplEventSource m_EventSource;	// from Eventk/u module (originating error source)
+		tEplEventObdError m_ObdError;	// from Obd module
+//        tEplErrHistoryEntry     m_HistoryEntry; // from Nmtk/u module
+
+	} m_Arg;
+
+} tEplEventError;
+
+// structure for kEplEventTypeDllError
+typedef struct {
+	unsigned long m_ulDllErrorEvents;	// EPL_DLL_ERR_*
+	unsigned int m_uiNodeId;
+	tEplNmtState m_NmtState;
+
+} tEplErrorHandlerkEvent;
+
+// callback function to get informed about sync event
+typedef tEplKernel(PUBLIC * tEplSyncCb) (void);
+
+// callback function for generic events
+typedef tEplKernel(PUBLIC * tEplProcessEventCb) (tEplEvent * pEplEvent_p);
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#endif // #ifndef _EPL_EVENT_H_
diff --git a/drivers/staging/epl/EplEventk.c b/drivers/staging/epl/EplEventk.c
new file mode 100644
index 00000000000..8068a6c5a0d
--- /dev/null
+++ b/drivers/staging/epl/EplEventk.c
@@ -0,0 +1,853 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for Epl-Kernelspace-Event-Modul
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplEventk.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.9 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/20 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "kernel/EplEventk.h"
+#include "kernel/EplNmtk.h"
+#include "kernel/EplDllk.h"
+#include "kernel/EplDllkCal.h"
+#include "kernel/EplErrorHandlerk.h"
+#include "Benchmark.h"
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0)
+#include "kernel/EplPdok.h"
+#include "kernel/EplPdokCal.h"
+#endif
+
+#ifdef EPL_NO_FIFO
+#include "user/EplEventu.h"
+#else
+#include "SharedBuff.h"
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+// TracePoint support for realtime-debugging
+#ifdef _DBG_TRACE_POINTS_
+void PUBLIC TgtDbgSignalTracePoint(BYTE bTracePointNumber_p);
+void PUBLIC TgtDbgPostTraceValue(DWORD dwTraceValue_p);
+#define TGT_DBG_SIGNAL_TRACE_POINT(p)   TgtDbgSignalTracePoint(p)
+#define TGT_DBG_POST_TRACE_VALUE(v)     TgtDbgPostTraceValue(v)
+#else
+#define TGT_DBG_SIGNAL_TRACE_POINT(p)
+#define TGT_DBG_POST_TRACE_VALUE(v)
+#endif
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+typedef struct {
+#ifndef EPL_NO_FIFO
+	tShbInstance m_pShbKernelToUserInstance;
+	tShbInstance m_pShbUserToKernelInstance;
+#else
+
+#endif
+	tEplSyncCb m_pfnCbSync;
+	unsigned int m_uiUserToKernelFullCount;
+
+} tEplEventkInstance;
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+static tEplEventkInstance EplEventkInstance_g;
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+// callback function for incoming events
+#ifndef EPL_NO_FIFO
+static void EplEventkRxSignalHandlerCb(tShbInstance pShbRxInstance_p,
+				       unsigned long ulDataSize_p);
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  <Epl-Kernelspace-Event>                             */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description:
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplEventkInit
+//
+// Description: function initializes the first instance
+//
+// Parameters:  pfnCbSync_p = callback-function for sync event
+//
+// Returns:     tEpKernel   = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplEventkInit(tEplSyncCb pfnCbSync_p)
+{
+	tEplKernel Ret;
+
+	Ret = EplEventkAddInstance(pfnCbSync_p);
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplEventkAddInstance
+//
+// Description: function adds one more instance
+//
+// Parameters:  pfnCbSync_p = callback-function for sync event
+//
+// Returns:     tEpKernel   = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplEventkAddInstance(tEplSyncCb pfnCbSync_p)
+{
+	tEplKernel Ret;
+#ifndef EPL_NO_FIFO
+	tShbError ShbError;
+	unsigned int fShbNewCreated;
+#endif
+
+	Ret = kEplSuccessful;
+
+	// init instance structure
+	EplEventkInstance_g.m_uiUserToKernelFullCount = 0;
+
+	// save cb-function
+	EplEventkInstance_g.m_pfnCbSync = pfnCbSync_p;
+
+#ifndef EPL_NO_FIFO
+	// init shared loop buffer
+	// kernel -> user
+	ShbError = ShbCirAllocBuffer(EPL_EVENT_SIZE_SHB_KERNEL_TO_USER,
+				     EPL_EVENT_NAME_SHB_KERNEL_TO_USER,
+				     &EplEventkInstance_g.
+				     m_pShbKernelToUserInstance,
+				     &fShbNewCreated);
+	if (ShbError != kShbOk) {
+		EPL_DBGLVL_EVENTK_TRACE1
+		    ("EplEventkAddInstance(): ShbCirAllocBuffer(K2U) -> 0x%X\n",
+		     ShbError);
+		Ret = kEplNoResource;
+		goto Exit;
+	}
+	// user -> kernel
+	ShbError = ShbCirAllocBuffer(EPL_EVENT_SIZE_SHB_USER_TO_KERNEL,
+				     EPL_EVENT_NAME_SHB_USER_TO_KERNEL,
+				     &EplEventkInstance_g.
+				     m_pShbUserToKernelInstance,
+				     &fShbNewCreated);
+	if (ShbError != kShbOk) {
+		EPL_DBGLVL_EVENTK_TRACE1
+		    ("EplEventkAddInstance(): ShbCirAllocBuffer(U2K) -> 0x%X\n",
+		     ShbError);
+		Ret = kEplNoResource;
+		goto Exit;
+	}
+	// register eventhandler
+	ShbError =
+	    ShbCirSetSignalHandlerNewData(EplEventkInstance_g.
+					  m_pShbUserToKernelInstance,
+					  EplEventkRxSignalHandlerCb,
+					  kshbPriorityHigh);
+	if (ShbError != kShbOk) {
+		EPL_DBGLVL_EVENTK_TRACE1
+		    ("EplEventkAddInstance(): ShbCirSetSignalHandlerNewData(U2K) -> 0x%X\n",
+		     ShbError);
+		Ret = kEplNoResource;
+		goto Exit;
+	}
+
+      Exit:
+#endif
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplEventkDelInstance
+//
+// Description: function deletes instance and frees the buffers
+//
+// Parameters:  void
+//
+// Returns:     tEpKernel   = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplEventkDelInstance()
+{
+	tEplKernel Ret;
+#ifndef EPL_NO_FIFO
+	tShbError ShbError;
+#endif
+
+	Ret = kEplSuccessful;
+
+#ifndef EPL_NO_FIFO
+	// set eventhandler to NULL
+	ShbError =
+	    ShbCirSetSignalHandlerNewData(EplEventkInstance_g.
+					  m_pShbUserToKernelInstance, NULL,
+					  kShbPriorityNormal);
+	if (ShbError != kShbOk) {
+		EPL_DBGLVL_EVENTK_TRACE1
+		    ("EplEventkDelInstance(): ShbCirSetSignalHandlerNewData(U2K) -> 0x%X\n",
+		     ShbError);
+		Ret = kEplNoResource;
+	}
+	// free buffer User -> Kernel
+	ShbError =
+	    ShbCirReleaseBuffer(EplEventkInstance_g.m_pShbUserToKernelInstance);
+	if (ShbError != kShbOk) {
+		EPL_DBGLVL_EVENTK_TRACE1
+		    ("EplEventkDelInstance(): ShbCirReleaseBuffer(U2K) -> 0x%X\n",
+		     ShbError);
+		Ret = kEplNoResource;
+	} else {
+		EplEventkInstance_g.m_pShbUserToKernelInstance = NULL;
+	}
+
+	// free buffer  Kernel -> User
+	ShbError =
+	    ShbCirReleaseBuffer(EplEventkInstance_g.m_pShbKernelToUserInstance);
+	if (ShbError != kShbOk) {
+		EPL_DBGLVL_EVENTK_TRACE1
+		    ("EplEventkDelInstance(): ShbCirReleaseBuffer(K2U) -> 0x%X\n",
+		     ShbError);
+		Ret = kEplNoResource;
+	} else {
+		EplEventkInstance_g.m_pShbKernelToUserInstance = NULL;
+	}
+#endif
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplEventkProcess
+//
+// Description: Kernelthread that dispatches events in kernel part
+//
+// Parameters:  pEvent_p    = pointer to event-structure from buffer
+//
+// Returns:     tEpKernel   = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplEventkProcess(tEplEvent * pEvent_p)
+{
+	tEplKernel Ret;
+	tEplEventSource EventSource;
+
+	Ret = kEplSuccessful;
+
+	// error handling if event queue is full
+	if (EplEventkInstance_g.m_uiUserToKernelFullCount > 0) {	// UserToKernel event queue has run out of space -> kEplNmtEventInternComError
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTK)) != 0)
+		tEplEvent Event;
+		tEplNmtEvent NmtEvent;
+#endif
+#ifndef EPL_NO_FIFO
+		tShbError ShbError;
+#endif
+
+		// directly call NMTk process function, because event queue is full
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTK)) != 0)
+		NmtEvent = kEplNmtEventInternComError;
+		Event.m_EventSink = kEplEventSinkNmtk;
+		Event.m_NetTime.m_dwNanoSec = 0;
+		Event.m_NetTime.m_dwSec = 0;
+		Event.m_EventType = kEplEventTypeNmtEvent;
+		Event.m_pArg = &NmtEvent;
+		Event.m_uiSize = sizeof(NmtEvent);
+		Ret = EplNmtkProcess(&Event);
+#endif
+
+		// NMT state machine changed to reset (i.e. NMT_GS_RESET_COMMUNICATION)
+		// now, it is safe to reset the counter and empty the event queue
+#ifndef EPL_NO_FIFO
+		ShbError =
+		    ShbCirResetBuffer(EplEventkInstance_g.
+				      m_pShbUserToKernelInstance, 1000, NULL);
+#endif
+
+		EplEventkInstance_g.m_uiUserToKernelFullCount = 0;
+		TGT_DBG_SIGNAL_TRACE_POINT(22);
+
+		// also discard the current event (it doesn't matter if we lose another event)
+		goto Exit;
+	}
+	// check m_EventSink
+	switch (pEvent_p->m_EventSink) {
+	case kEplEventSinkSync:
+		{
+			if (EplEventkInstance_g.m_pfnCbSync != NULL) {
+				Ret = EplEventkInstance_g.m_pfnCbSync();
+				if (Ret == kEplSuccessful) {
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0)
+					// mark TPDOs as valid
+					Ret = EplPdokCalSetTpdosValid(TRUE);
+#endif
+				} else if ((Ret != kEplReject)
+					   && (Ret != kEplShutdown)) {
+					EventSource = kEplEventSourceSyncCb;
+
+					// Error event for API layer
+					EplEventkPostError
+					    (kEplEventSourceEventk, Ret,
+					     sizeof(EventSource), &EventSource);
+				}
+			}
+			break;
+		}
+
+		// NMT-Kernel-Modul
+	case kEplEventSinkNmtk:
+		{
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTK)) != 0)
+			Ret = EplNmtkProcess(pEvent_p);
+			if ((Ret != kEplSuccessful) && (Ret != kEplShutdown)) {
+				EventSource = kEplEventSourceNmtk;
+
+				// Error event for API layer
+				EplEventkPostError(kEplEventSourceEventk,
+						   Ret,
+						   sizeof(EventSource),
+						   &EventSource);
+			}
+#endif
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+			if ((pEvent_p->m_EventType == kEplEventTypeNmtEvent)
+			    &&
+			    ((*((tEplNmtEvent *) pEvent_p->m_pArg) ==
+			      kEplNmtEventDllCeSoa)
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+			     || (*((tEplNmtEvent *) pEvent_p->m_pArg) ==
+				 kEplNmtEventDllMeSoaSent)
+#endif
+			    )) {	// forward SoA event to error handler
+				Ret = EplErrorHandlerkProcess(pEvent_p);
+				if ((Ret != kEplSuccessful)
+				    && (Ret != kEplShutdown)) {
+					EventSource = kEplEventSourceErrk;
+
+					// Error event for API layer
+					EplEventkPostError
+					    (kEplEventSourceEventk, Ret,
+					     sizeof(EventSource), &EventSource);
+				}
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0)
+				// forward SoA event to PDO module
+				pEvent_p->m_EventType = kEplEventTypePdoSoa;
+				Ret = EplPdokProcess(pEvent_p);
+				if ((Ret != kEplSuccessful)
+				    && (Ret != kEplShutdown)) {
+					EventSource = kEplEventSourcePdok;
+
+					// Error event for API layer
+					EplEventkPostError
+					    (kEplEventSourceEventk, Ret,
+					     sizeof(EventSource), &EventSource);
+				}
+#endif
+
+			}
+			break;
+#endif
+		}
+
+		// events for Dllk module
+	case kEplEventSinkDllk:
+		{
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+			Ret = EplDllkProcess(pEvent_p);
+			if ((Ret != kEplSuccessful) && (Ret != kEplShutdown)) {
+				EventSource = kEplEventSourceDllk;
+
+				// Error event for API layer
+				EplEventkPostError(kEplEventSourceEventk,
+						   Ret,
+						   sizeof(EventSource),
+						   &EventSource);
+			}
+#endif
+			break;
+		}
+
+		// events for DllkCal module
+	case kEplEventSinkDllkCal:
+		{
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+			Ret = EplDllkCalProcess(pEvent_p);
+			if ((Ret != kEplSuccessful) && (Ret != kEplShutdown)) {
+				EventSource = kEplEventSourceDllk;
+
+				// Error event for API layer
+				EplEventkPostError(kEplEventSourceEventk,
+						   Ret,
+						   sizeof(EventSource),
+						   &EventSource);
+			}
+#endif
+			break;
+		}
+
+		//
+	case kEplEventSinkPdok:
+		{
+			// PDO-Module
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0)
+			Ret = EplPdokProcess(pEvent_p);
+			if ((Ret != kEplSuccessful) && (Ret != kEplShutdown)) {
+				EventSource = kEplEventSourcePdok;
+
+				// Error event for API layer
+				EplEventkPostError(kEplEventSourceEventk,
+						   Ret,
+						   sizeof(EventSource),
+						   &EventSource);
+			}
+#endif
+			break;
+		}
+
+		// events for Error handler module
+	case kEplEventSinkErrk:
+		{
+			// only call error handler if DLL is present
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+			Ret = EplErrorHandlerkProcess(pEvent_p);
+			if ((Ret != kEplSuccessful) && (Ret != kEplShutdown)) {
+				EventSource = kEplEventSourceErrk;
+
+				// Error event for API layer
+				EplEventkPostError(kEplEventSourceEventk,
+						   Ret,
+						   sizeof(EventSource),
+						   &EventSource);
+			}
+			break;
+#endif
+		}
+
+		// unknown sink
+	default:
+		{
+			Ret = kEplEventUnknownSink;
+		}
+
+	}			// end of switch(pEvent_p->m_EventSink)
+
+      Exit:
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplEventkPost
+//
+// Description: post events from kernel part
+//
+// Parameters:  pEvent_p    = pointer to event-structure from buffer
+//
+// Returns:     tEpKernel   = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplEventkPost(tEplEvent * pEvent_p)
+{
+	tEplKernel Ret;
+#ifndef EPL_NO_FIFO
+	tShbError ShbError;
+	tShbCirChunk ShbCirChunk;
+	unsigned long ulDataSize;
+	unsigned int fBufferCompleted;
+#endif
+
+	Ret = kEplSuccessful;
+
+	// the event must be posted by using the abBuffer
+	// it is neede because the Argument must by copied
+	// to the buffer too and not only the pointer
+
+#ifndef EPL_NO_FIFO
+	// 2006/08/03 d.k.: Event and argument are posted as separate chunks to the event queue.
+	ulDataSize =
+	    sizeof(tEplEvent) +
+	    ((pEvent_p->m_pArg != NULL) ? pEvent_p->m_uiSize : 0);
+#endif
+
+	// decide in which buffer the event have to write
+	switch (pEvent_p->m_EventSink) {
+		// kernelspace modules
+	case kEplEventSinkSync:
+	case kEplEventSinkNmtk:
+	case kEplEventSinkDllk:
+	case kEplEventSinkDllkCal:
+	case kEplEventSinkPdok:
+	case kEplEventSinkErrk:
+		{
+#ifndef EPL_NO_FIFO
+			// post message
+			BENCHMARK_MOD_27_SET(2);
+			ShbError =
+			    ShbCirAllocDataBlock(EplEventkInstance_g.
+						 m_pShbUserToKernelInstance,
+						 &ShbCirChunk, ulDataSize);
+			switch (ShbError) {
+			case kShbOk:
+				break;
+
+			case kShbBufferFull:
+				{
+					EplEventkInstance_g.
+					    m_uiUserToKernelFullCount++;
+					Ret = kEplEventPostError;
+					goto Exit;
+				}
+
+			default:
+				{
+					EPL_DBGLVL_EVENTK_TRACE1
+					    ("EplEventkPost(): ShbCirAllocDataBlock(U2K) -> 0x%X\n",
+					     ShbError);
+					Ret = kEplEventPostError;
+					goto Exit;
+				}
+			}
+			ShbError =
+			    ShbCirWriteDataChunk(EplEventkInstance_g.
+						 m_pShbUserToKernelInstance,
+						 &ShbCirChunk, pEvent_p,
+						 sizeof(tEplEvent),
+						 &fBufferCompleted);
+			if (ShbError != kShbOk) {
+				EPL_DBGLVL_EVENTK_TRACE1
+				    ("EplEventkPost(): ShbCirWriteDataChunk(U2K) -> 0x%X\n",
+				     ShbError);
+				Ret = kEplEventPostError;
+				goto Exit;
+			}
+			if (fBufferCompleted == FALSE) {
+				ShbError =
+				    ShbCirWriteDataChunk(EplEventkInstance_g.
+							 m_pShbUserToKernelInstance,
+							 &ShbCirChunk,
+							 pEvent_p->m_pArg,
+							 (unsigned long)
+							 pEvent_p->m_uiSize,
+							 &fBufferCompleted);
+				if ((ShbError != kShbOk)
+				    || (fBufferCompleted == FALSE)) {
+					EPL_DBGLVL_EVENTK_TRACE1
+					    ("EplEventkPost(): ShbCirWriteDataChunk2(U2K) -> 0x%X\n",
+					     ShbError);
+					Ret = kEplEventPostError;
+					goto Exit;
+				}
+			}
+			BENCHMARK_MOD_27_RESET(2);
+
+#else
+			Ret = EplEventkProcess(pEvent_p);
+#endif
+
+			break;
+		}
+
+		// userspace modules
+	case kEplEventSinkNmtu:
+	case kEplEventSinkNmtMnu:
+	case kEplEventSinkSdoAsySeq:
+	case kEplEventSinkApi:
+	case kEplEventSinkDlluCal:
+	case kEplEventSinkErru:
+		{
+#ifndef EPL_NO_FIFO
+			// post message
+//            BENCHMARK_MOD_27_SET(3);    // 74 µs until reset
+			ShbError =
+			    ShbCirAllocDataBlock(EplEventkInstance_g.
+						 m_pShbKernelToUserInstance,
+						 &ShbCirChunk, ulDataSize);
+			if (ShbError != kShbOk) {
+				EPL_DBGLVL_EVENTK_TRACE1
+				    ("EplEventkPost(): ShbCirAllocDataBlock(K2U) -> 0x%X\n",
+				     ShbError);
+				Ret = kEplEventPostError;
+				goto Exit;
+			}
+			ShbError =
+			    ShbCirWriteDataChunk(EplEventkInstance_g.
+						 m_pShbKernelToUserInstance,
+						 &ShbCirChunk, pEvent_p,
+						 sizeof(tEplEvent),
+						 &fBufferCompleted);
+			if (ShbError != kShbOk) {
+				EPL_DBGLVL_EVENTK_TRACE1
+				    ("EplEventkPost(): ShbCirWriteDataChunk(K2U) -> 0x%X\n",
+				     ShbError);
+				Ret = kEplEventPostError;
+				goto Exit;
+			}
+			if (fBufferCompleted == FALSE) {
+				ShbError =
+				    ShbCirWriteDataChunk(EplEventkInstance_g.
+							 m_pShbKernelToUserInstance,
+							 &ShbCirChunk,
+							 pEvent_p->m_pArg,
+							 (unsigned long)
+							 pEvent_p->m_uiSize,
+							 &fBufferCompleted);
+				if ((ShbError != kShbOk)
+				    || (fBufferCompleted == FALSE)) {
+					EPL_DBGLVL_EVENTK_TRACE1
+					    ("EplEventkPost(): ShbCirWriteDataChunk2(K2U) -> 0x%X\n",
+					     ShbError);
+					Ret = kEplEventPostError;
+					goto Exit;
+				}
+			}
+//            BENCHMARK_MOD_27_RESET(3);  // 82 µs until ShbCirGetReadDataSize() in EplEventu
+
+#else
+			Ret = EplEventuProcess(pEvent_p);
+#endif
+
+			break;
+		}
+
+	default:
+		{
+			Ret = kEplEventUnknownSink;
+		}
+
+	}			// end of switch(pEvent_p->m_EventSink)
+
+#ifndef EPL_NO_FIFO
+      Exit:
+#endif
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplEventkPostError
+//
+// Description: post error event from kernel part to API layer
+//
+// Parameters:  EventSource_p   = source-module of the error event
+//              EplError_p      = code of occured error
+//              ArgSize_p       = size of the argument
+//              pArg_p          = pointer to the argument
+//
+// Returns:     tEpKernel       = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplEventkPostError(tEplEventSource EventSource_p,
+				     tEplKernel EplError_p,
+				     unsigned int uiArgSize_p, void *pArg_p)
+{
+	tEplKernel Ret;
+	BYTE abBuffer[EPL_MAX_EVENT_ARG_SIZE];
+	tEplEventError *pEventError = (tEplEventError *) abBuffer;
+	tEplEvent EplEvent;
+
+	Ret = kEplSuccessful;
+
+	// create argument
+	pEventError->m_EventSource = EventSource_p;
+	pEventError->m_EplError = EplError_p;
+	EPL_MEMCPY(&pEventError->m_Arg, pArg_p, uiArgSize_p);
+
+	// create event
+	EplEvent.m_EventType = kEplEventTypeError;
+	EplEvent.m_EventSink = kEplEventSinkApi;
+	EPL_MEMSET(&EplEvent.m_NetTime, 0x00, sizeof(EplEvent.m_NetTime));
+	EplEvent.m_uiSize =
+	    (sizeof(EventSource_p) + sizeof(EplError_p) + uiArgSize_p);
+	EplEvent.m_pArg = &abBuffer[0];
+
+	// post errorevent
+	Ret = EplEventkPost(&EplEvent);
+
+	return Ret;
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplEventkRxSignalHandlerCb()
+//
+// Description: Callback-function for events from user and kernel part
+//
+// Parameters:  pShbRxInstance_p    = Instance-pointer of buffer
+//              ulDataSize_p        = size of data
+//
+// Returns: void
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+#ifndef EPL_NO_FIFO
+static void EplEventkRxSignalHandlerCb(tShbInstance pShbRxInstance_p,
+				       unsigned long ulDataSize_p)
+{
+	tEplEvent *pEplEvent;
+	tShbError ShbError;
+//unsigned long   ulBlockCount;
+//unsigned long   ulDataSize;
+	BYTE abDataBuffer[sizeof(tEplEvent) + EPL_MAX_EVENT_ARG_SIZE];
+	// d.k.: abDataBuffer contains the complete tEplEvent structure
+	//       and behind this the argument
+
+	TGT_DBG_SIGNAL_TRACE_POINT(20);
+
+	BENCHMARK_MOD_27_RESET(0);
+	// copy data from event queue
+	ShbError = ShbCirReadDataBlock(pShbRxInstance_p,
+				       &abDataBuffer[0],
+				       sizeof(abDataBuffer), &ulDataSize_p);
+	if (ShbError != kShbOk) {
+		// error goto exit
+		goto Exit;
+	}
+	// resolve the pointer to the event structure
+	pEplEvent = (tEplEvent *) abDataBuffer;
+	// set Datasize
+	pEplEvent->m_uiSize = (ulDataSize_p - sizeof(tEplEvent));
+	if (pEplEvent->m_uiSize > 0) {
+		// set pointer to argument
+		pEplEvent->m_pArg = &abDataBuffer[sizeof(tEplEvent)];
+	} else {
+		//set pointer to NULL
+		pEplEvent->m_pArg = NULL;
+	}
+
+	BENCHMARK_MOD_27_SET(0);
+	// call processfunction
+	EplEventkProcess(pEplEvent);
+
+      Exit:
+	return;
+}
+#endif
+
+// EOF
diff --git a/drivers/staging/epl/EplEventu.c b/drivers/staging/epl/EplEventu.c
new file mode 100644
index 00000000000..815f9a87abf
--- /dev/null
+++ b/drivers/staging/epl/EplEventu.c
@@ -0,0 +1,814 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for Epl-Userspace-Event-Modul
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplEventu.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.8 $  $Date: 2008/11/17 16:40:39 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/20 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "user/EplEventu.h"
+#include "user/EplNmtu.h"
+#include "user/EplNmtMnu.h"
+#include "user/EplSdoAsySequ.h"
+#include "user/EplDlluCal.h"
+#include "user/EplLedu.h"
+#include "Benchmark.h"
+
+#ifdef EPL_NO_FIFO
+#include "kernel/EplEventk.h"
+#else
+#include "SharedBuff.h"
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+// TracePoint support for realtime-debugging
+#ifdef _DBG_TRACE_POINTS_
+void PUBLIC TgtDbgSignalTracePoint(BYTE bTracePointNumber_p);
+void PUBLIC TgtDbgPostTraceValue(DWORD dwTraceValue_p);
+#define TGT_DBG_SIGNAL_TRACE_POINT(p)   TgtDbgSignalTracePoint(p)
+#define TGT_DBG_POST_TRACE_VALUE(v)     TgtDbgPostTraceValue(v)
+#else
+#define TGT_DBG_SIGNAL_TRACE_POINT(p)
+#define TGT_DBG_POST_TRACE_VALUE(v)
+#endif
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+typedef struct {
+#ifndef EPL_NO_FIFO
+	tShbInstance m_pShbKernelToUserInstance;
+	tShbInstance m_pShbUserToKernelInstance;
+#endif
+	tEplProcessEventCb m_pfnApiProcessEventCb;
+
+} tEplEventuInstance;
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+//#ifndef EPL_NO_FIFO
+static tEplEventuInstance EplEventuInstance_g;
+//#endif
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+#ifndef EPL_NO_FIFO
+// callback function for incomming events
+static void EplEventuRxSignalHandlerCb(tShbInstance pShbRxInstance_p,
+				       unsigned long ulDataSize_p);
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  <Epl-User-Event>                                    */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description:
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplEventuInit
+//
+// Description: function initialize the first instance
+//
+//
+//
+// Parameters:  pfnApiProcessEventCb_p  = function pointer for API event callback
+//
+//
+// Returns:      tEpKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplEventuInit(tEplProcessEventCb pfnApiProcessEventCb_p)
+{
+	tEplKernel Ret;
+
+	Ret = EplEventuAddInstance(pfnApiProcessEventCb_p);
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplEventuAddInstance
+//
+// Description: function add one more instance
+//
+//
+//
+// Parameters:  pfnApiProcessEventCb_p  = function pointer for API event callback
+//
+//
+// Returns:      tEpKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplEventuAddInstance(tEplProcessEventCb
+				       pfnApiProcessEventCb_p)
+{
+	tEplKernel Ret;
+#ifndef EPL_NO_FIFO
+	tShbError ShbError;
+	unsigned int fShbNewCreated;
+#endif
+
+	Ret = kEplSuccessful;
+
+	// init instance variables
+	EplEventuInstance_g.m_pfnApiProcessEventCb = pfnApiProcessEventCb_p;
+
+#ifndef EPL_NO_FIFO
+	// init shared loop buffer
+	// kernel -> user
+	ShbError = ShbCirAllocBuffer(EPL_EVENT_SIZE_SHB_KERNEL_TO_USER,
+				     EPL_EVENT_NAME_SHB_KERNEL_TO_USER,
+				     &EplEventuInstance_g.
+				     m_pShbKernelToUserInstance,
+				     &fShbNewCreated);
+	if (ShbError != kShbOk) {
+		EPL_DBGLVL_EVENTK_TRACE1
+		    ("EplEventuAddInstance(): ShbCirAllocBuffer(K2U) -> 0x%X\n",
+		     ShbError);
+		Ret = kEplNoResource;
+		goto Exit;
+	}
+
+	// user -> kernel
+	ShbError = ShbCirAllocBuffer(EPL_EVENT_SIZE_SHB_USER_TO_KERNEL,
+				     EPL_EVENT_NAME_SHB_USER_TO_KERNEL,
+				     &EplEventuInstance_g.
+				     m_pShbUserToKernelInstance,
+				     &fShbNewCreated);
+	if (ShbError != kShbOk) {
+		EPL_DBGLVL_EVENTK_TRACE1
+		    ("EplEventuAddInstance(): ShbCirAllocBuffer(U2K) -> 0x%X\n",
+		     ShbError);
+		Ret = kEplNoResource;
+		goto Exit;
+	}
+	// register eventhandler
+	ShbError =
+	    ShbCirSetSignalHandlerNewData(EplEventuInstance_g.
+					  m_pShbKernelToUserInstance,
+					  EplEventuRxSignalHandlerCb,
+					  kShbPriorityNormal);
+	if (ShbError != kShbOk) {
+		EPL_DBGLVL_EVENTK_TRACE1
+		    ("EplEventuAddInstance(): ShbCirSetSignalHandlerNewData(K2U) -> 0x%X\n",
+		     ShbError);
+		Ret = kEplNoResource;
+		goto Exit;
+	}
+
+      Exit:
+#endif
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplEventuDelInstance
+//
+// Description: function delete instance an free the bufferstructure
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:      tEpKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplEventuDelInstance()
+{
+	tEplKernel Ret;
+#ifndef EPL_NO_FIFO
+	tShbError ShbError;
+#endif
+
+	Ret = kEplSuccessful;
+
+#ifndef EPL_NO_FIFO
+	// set eventhandler to NULL
+	ShbError =
+	    ShbCirSetSignalHandlerNewData(EplEventuInstance_g.
+					  m_pShbKernelToUserInstance, NULL,
+					  kShbPriorityNormal);
+	if (ShbError != kShbOk) {
+		EPL_DBGLVL_EVENTK_TRACE1
+		    ("EplEventuDelInstance(): ShbCirSetSignalHandlerNewData(K2U) -> 0x%X\n",
+		     ShbError);
+		Ret = kEplNoResource;
+	}
+	// free buffer User -> Kernel
+	ShbError =
+	    ShbCirReleaseBuffer(EplEventuInstance_g.m_pShbUserToKernelInstance);
+	if ((ShbError != kShbOk) && (ShbError != kShbMemUsedByOtherProcs)) {
+		EPL_DBGLVL_EVENTK_TRACE1
+		    ("EplEventuDelInstance(): ShbCirReleaseBuffer(U2K) -> 0x%X\n",
+		     ShbError);
+		Ret = kEplNoResource;
+	} else {
+		EplEventuInstance_g.m_pShbUserToKernelInstance = NULL;
+	}
+
+	// free buffer  Kernel -> User
+	ShbError =
+	    ShbCirReleaseBuffer(EplEventuInstance_g.m_pShbKernelToUserInstance);
+	if ((ShbError != kShbOk) && (ShbError != kShbMemUsedByOtherProcs)) {
+		EPL_DBGLVL_EVENTK_TRACE1
+		    ("EplEventuDelInstance(): ShbCirReleaseBuffer(K2U) -> 0x%X\n",
+		     ShbError);
+		Ret = kEplNoResource;
+	} else {
+		EplEventuInstance_g.m_pShbKernelToUserInstance = NULL;
+	}
+
+#endif
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplEventuProcess
+//
+// Description: Kernelthread that dispatches events in kernelspace
+//
+//
+//
+// Parameters:  pEvent_p = pointer to event-structur from buffer
+//
+//
+// Returns:      tEpKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplEventuProcess(tEplEvent * pEvent_p)
+{
+	tEplKernel Ret;
+	tEplEventSource EventSource;
+
+	Ret = kEplSuccessful;
+
+	// check m_EventSink
+	switch (pEvent_p->m_EventSink) {
+		// NMT-User-Module
+	case kEplEventSinkNmtu:
+		{
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTU)) != 0)
+			Ret = EplNmtuProcessEvent(pEvent_p);
+			if ((Ret != kEplSuccessful) && (Ret != kEplShutdown)) {
+				EventSource = kEplEventSourceNmtu;
+
+				// Error event for API layer
+				EplEventuPostError(kEplEventSourceEventu,
+						   Ret,
+						   sizeof(EventSource),
+						   &EventSource);
+			}
+#endif
+			break;
+		}
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+		// NMT-MN-User-Module
+	case kEplEventSinkNmtMnu:
+		{
+			Ret = EplNmtMnuProcessEvent(pEvent_p);
+			if ((Ret != kEplSuccessful) && (Ret != kEplShutdown)) {
+				EventSource = kEplEventSourceNmtMnu;
+
+				// Error event for API layer
+				EplEventuPostError(kEplEventSourceEventu,
+						   Ret,
+						   sizeof(EventSource),
+						   &EventSource);
+			}
+			break;
+		}
+#endif
+
+#if ((((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)   \
+     || (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOS)) != 0))
+		// events for asynchronus SDO Sequence Layer
+	case kEplEventSinkSdoAsySeq:
+		{
+			Ret = EplSdoAsySeqProcessEvent(pEvent_p);
+			if ((Ret != kEplSuccessful) && (Ret != kEplShutdown)) {
+				EventSource = kEplEventSourceSdoAsySeq;
+
+				// Error event for API layer
+				EplEventuPostError(kEplEventSourceEventu,
+						   Ret,
+						   sizeof(EventSource),
+						   &EventSource);
+			}
+			break;
+		}
+#endif
+
+		// LED user part module
+	case kEplEventSinkLedu:
+		{
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_LEDU)) != 0)
+			Ret = EplLeduProcessEvent(pEvent_p);
+			if ((Ret != kEplSuccessful) && (Ret != kEplShutdown)) {
+				EventSource = kEplEventSourceLedu;
+
+				// Error event for API layer
+				EplEventuPostError(kEplEventSourceEventu,
+						   Ret,
+						   sizeof(EventSource),
+						   &EventSource);
+			}
+#endif
+			break;
+		}
+
+		// event for EPL api
+	case kEplEventSinkApi:
+		{
+			if (EplEventuInstance_g.m_pfnApiProcessEventCb != NULL) {
+				Ret =
+				    EplEventuInstance_g.
+				    m_pfnApiProcessEventCb(pEvent_p);
+				if ((Ret != kEplSuccessful)
+				    && (Ret != kEplShutdown)) {
+					EventSource = kEplEventSourceEplApi;
+
+					// Error event for API layer
+					EplEventuPostError
+					    (kEplEventSourceEventu, Ret,
+					     sizeof(EventSource), &EventSource);
+				}
+			}
+			break;
+
+		}
+
+	case kEplEventSinkDlluCal:
+		{
+			Ret = EplDlluCalProcess(pEvent_p);
+			if ((Ret != kEplSuccessful) && (Ret != kEplShutdown)) {
+				EventSource = kEplEventSourceDllu;
+
+				// Error event for API layer
+				EplEventuPostError(kEplEventSourceEventu,
+						   Ret,
+						   sizeof(EventSource),
+						   &EventSource);
+			}
+			break;
+
+		}
+
+	case kEplEventSinkErru:
+		{
+			/*
+			   Ret = EplErruProcess(pEvent_p);
+			   if ((Ret != kEplSuccessful) && (Ret != kEplShutdown))
+			   {
+			   EventSource = kEplEventSourceErru;
+
+			   // Error event for API layer
+			   EplEventuPostError(kEplEventSourceEventu,
+			   Ret,
+			   sizeof(EventSource),
+			   &EventSource);
+			   }
+			 */
+			break;
+
+		}
+
+		// unknown sink
+	default:
+		{
+			Ret = kEplEventUnknownSink;
+		}
+
+	}			// end of switch(pEvent_p->m_EventSink)
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplEventuPost
+//
+// Description: post events from userspace
+//
+//
+//
+// Parameters:  pEvent_p = pointer to event-structur from buffer
+//
+//
+// Returns:      tEpKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplEventuPost(tEplEvent * pEvent_p)
+{
+	tEplKernel Ret;
+#ifndef EPL_NO_FIFO
+	tShbError ShbError;
+	tShbCirChunk ShbCirChunk;
+	unsigned long ulDataSize;
+	unsigned int fBufferCompleted;
+#endif
+
+	Ret = kEplSuccessful;
+
+#ifndef EPL_NO_FIFO
+	// 2006/08/03 d.k.: Event and argument are posted as separate chunks to the event queue.
+	ulDataSize =
+	    sizeof(tEplEvent) +
+	    ((pEvent_p->m_pArg != NULL) ? pEvent_p->m_uiSize : 0);
+#endif
+
+	// decide in which buffer the event have to write
+	switch (pEvent_p->m_EventSink) {
+		// kernelspace modules
+	case kEplEventSinkSync:
+	case kEplEventSinkNmtk:
+	case kEplEventSinkDllk:
+	case kEplEventSinkDllkCal:
+	case kEplEventSinkPdok:
+	case kEplEventSinkErrk:
+		{
+#ifndef EPL_NO_FIFO
+			// post message
+			ShbError =
+			    ShbCirAllocDataBlock(EplEventuInstance_g.
+						 m_pShbUserToKernelInstance,
+						 &ShbCirChunk, ulDataSize);
+			if (ShbError != kShbOk) {
+				EPL_DBGLVL_EVENTK_TRACE1
+				    ("EplEventuPost(): ShbCirAllocDataBlock(U2K) -> 0x%X\n",
+				     ShbError);
+				Ret = kEplEventPostError;
+				goto Exit;
+			}
+			ShbError =
+			    ShbCirWriteDataChunk(EplEventuInstance_g.
+						 m_pShbUserToKernelInstance,
+						 &ShbCirChunk, pEvent_p,
+						 sizeof(tEplEvent),
+						 &fBufferCompleted);
+			if (ShbError != kShbOk) {
+				EPL_DBGLVL_EVENTK_TRACE1
+				    ("EplEventuPost(): ShbCirWriteDataChunk(U2K) -> 0x%X\n",
+				     ShbError);
+				Ret = kEplEventPostError;
+				goto Exit;
+			}
+			if (fBufferCompleted == FALSE) {
+				ShbError =
+				    ShbCirWriteDataChunk(EplEventuInstance_g.
+							 m_pShbUserToKernelInstance,
+							 &ShbCirChunk,
+							 pEvent_p->m_pArg,
+							 (unsigned long)
+							 pEvent_p->m_uiSize,
+							 &fBufferCompleted);
+				if ((ShbError != kShbOk)
+				    || (fBufferCompleted == FALSE)) {
+					EPL_DBGLVL_EVENTK_TRACE1
+					    ("EplEventuPost(): ShbCirWriteDataChunk2(U2K) -> 0x%X\n",
+					     ShbError);
+					Ret = kEplEventPostError;
+					goto Exit;
+				}
+			}
+#else
+			Ret = EplEventkProcess(pEvent_p);
+#endif
+
+			break;
+		}
+
+		// userspace modules
+	case kEplEventSinkNmtMnu:
+	case kEplEventSinkNmtu:
+	case kEplEventSinkSdoAsySeq:
+	case kEplEventSinkApi:
+	case kEplEventSinkDlluCal:
+	case kEplEventSinkErru:
+	case kEplEventSinkLedu:
+		{
+#ifndef EPL_NO_FIFO
+			// post message
+			ShbError =
+			    ShbCirAllocDataBlock(EplEventuInstance_g.
+						 m_pShbKernelToUserInstance,
+						 &ShbCirChunk, ulDataSize);
+			if (ShbError != kShbOk) {
+				EPL_DBGLVL_EVENTK_TRACE1
+				    ("EplEventuPost(): ShbCirAllocDataBlock(K2U) -> 0x%X\n",
+				     ShbError);
+				Ret = kEplEventPostError;
+				goto Exit;
+			}
+			ShbError =
+			    ShbCirWriteDataChunk(EplEventuInstance_g.
+						 m_pShbKernelToUserInstance,
+						 &ShbCirChunk, pEvent_p,
+						 sizeof(tEplEvent),
+						 &fBufferCompleted);
+			if (ShbError != kShbOk) {
+				EPL_DBGLVL_EVENTK_TRACE1
+				    ("EplEventuPost(): ShbCirWriteDataChunk(K2U) -> 0x%X\n",
+				     ShbError);
+				Ret = kEplEventPostError;
+				goto Exit;
+			}
+			if (fBufferCompleted == FALSE) {
+				ShbError =
+				    ShbCirWriteDataChunk(EplEventuInstance_g.
+							 m_pShbKernelToUserInstance,
+							 &ShbCirChunk,
+							 pEvent_p->m_pArg,
+							 (unsigned long)
+							 pEvent_p->m_uiSize,
+							 &fBufferCompleted);
+				if ((ShbError != kShbOk)
+				    || (fBufferCompleted == FALSE)) {
+					EPL_DBGLVL_EVENTK_TRACE1
+					    ("EplEventuPost(): ShbCirWriteDataChunk2(K2U) -> 0x%X\n",
+					     ShbError);
+					Ret = kEplEventPostError;
+					goto Exit;
+				}
+			}
+#else
+			Ret = EplEventuProcess(pEvent_p);
+#endif
+
+			break;
+		}
+
+	default:
+		{
+			Ret = kEplEventUnknownSink;
+		}
+
+	}			// end of switch(pEvent_p->m_EventSink)
+
+#ifndef EPL_NO_FIFO
+      Exit:
+#endif
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplEventuPostError
+//
+// Description: post errorevent from userspace
+//
+//
+//
+// Parameters:  EventSource_p   = source-module of the errorevent
+//              EplError_p     = code of occured error
+//              uiArgSize_p     = size of the argument
+//              pArg_p          = pointer to the argument
+//
+//
+// Returns:      tEpKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplEventuPostError(tEplEventSource EventSource_p,
+				     tEplKernel EplError_p,
+				     unsigned int uiArgSize_p, void *pArg_p)
+{
+	tEplKernel Ret;
+	BYTE abBuffer[EPL_MAX_EVENT_ARG_SIZE];
+	tEplEventError *pEventError = (tEplEventError *) abBuffer;
+	tEplEvent EplEvent;
+
+	Ret = kEplSuccessful;
+
+	// create argument
+	pEventError->m_EventSource = EventSource_p;
+	pEventError->m_EplError = EplError_p;
+	EPL_MEMCPY(&pEventError->m_Arg, pArg_p, uiArgSize_p);
+
+	// create event
+	EplEvent.m_EventType = kEplEventTypeError;
+	EplEvent.m_EventSink = kEplEventSinkApi;
+	EPL_MEMSET(&EplEvent.m_NetTime, 0x00, sizeof(EplEvent.m_NetTime));
+	EplEvent.m_uiSize =
+	    (sizeof(EventSource_p) + sizeof(EplError_p) + uiArgSize_p);
+	EplEvent.m_pArg = &abBuffer[0];
+
+	// post errorevent
+	Ret = EplEventuPost(&EplEvent);
+
+	return Ret;
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplEventuRxSignalHandlerCb()
+//
+// Description: Callback-function for evets from kernelspace
+//
+//
+//
+// Parameters:  pShbRxInstance_p    = Instance-pointer for buffer
+//              ulDataSize_p        = size of data
+//
+//
+// Returns: void
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#ifndef EPL_NO_FIFO
+static void EplEventuRxSignalHandlerCb(tShbInstance pShbRxInstance_p,
+				       unsigned long ulDataSize_p)
+{
+	tEplEvent *pEplEvent;
+	tShbError ShbError;
+//unsigned long   ulBlockCount;
+//unsigned long   ulDataSize;
+	BYTE abDataBuffer[sizeof(tEplEvent) + EPL_MAX_EVENT_ARG_SIZE];
+	// d.k.: abDataBuffer contains the complete tEplEvent structure
+	//       and behind this the argument
+
+	TGT_DBG_SIGNAL_TRACE_POINT(21);
+
+// d.k. not needed because it is already done in SharedBuff
+/*    do
+    {
+        BENCHMARK_MOD_28_SET(1);    // 4 µs until reset
+        // get messagesize
+        ShbError = ShbCirGetReadDataSize (pShbRxInstance_p, &ulDataSize);
+        if(ShbError != kShbOk)
+        {
+            // error goto exit
+            goto Exit;
+        }
+
+        BENCHMARK_MOD_28_RESET(1);  // 14 µs until set
+*/
+	// copy data from event queue
+	ShbError = ShbCirReadDataBlock(pShbRxInstance_p,
+				       &abDataBuffer[0],
+				       sizeof(abDataBuffer), &ulDataSize_p);
+	if (ShbError != kShbOk) {
+		// error goto exit
+		goto Exit;
+	}
+	// resolve the pointer to the event structure
+	pEplEvent = (tEplEvent *) abDataBuffer;
+	// set Datasize
+	pEplEvent->m_uiSize = (ulDataSize_p - sizeof(tEplEvent));
+	if (pEplEvent->m_uiSize > 0) {
+		// set pointer to argument
+		pEplEvent->m_pArg = &abDataBuffer[sizeof(tEplEvent)];
+	} else {
+		//set pointer to NULL
+		pEplEvent->m_pArg = NULL;
+	}
+
+	BENCHMARK_MOD_28_SET(1);
+	// call processfunction
+	EplEventuProcess(pEplEvent);
+
+	BENCHMARK_MOD_28_RESET(1);
+	// read number of left messages to process
+// d.k. not needed because it is already done in SharedBuff
+/*        ShbError = ShbCirGetReadBlockCount (pShbRxInstance_p, &ulBlockCount);
+        if (ShbError != kShbOk)
+        {
+            // error goto exit
+            goto Exit;
+        }
+    } while (ulBlockCount > 0);
+*/
+      Exit:
+	return;
+}
+#endif
+
+// EOF
diff --git a/drivers/staging/epl/EplFrame.h b/drivers/staging/epl/EplFrame.h
new file mode 100644
index 00000000000..9a7f8b9f594
--- /dev/null
+++ b/drivers/staging/epl/EplFrame.h
@@ -0,0 +1,344 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for EPL frames
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplFrame.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/06/23 14:56:33 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/05/22 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_FRAME_H_
+#define _EPL_FRAME_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+// defines for EplFrame.m_wFlag
+#define EPL_FRAME_FLAG1_RD          0x01	// ready                                    (PReq, PRes)
+#define EPL_FRAME_FLAG1_ER          0x02	// exception reset (error signalling)       (SoA)
+#define EPL_FRAME_FLAG1_EA          0x04	// exception acknowledge (error signalling) (PReq, SoA)
+#define EPL_FRAME_FLAG1_EC          0x08	// exception clear (error signalling)       (StatusRes)
+#define EPL_FRAME_FLAG1_EN          0x10	// exception new (error signalling)         (PRes, StatusRes)
+#define EPL_FRAME_FLAG1_MS          0x20	// multiplexed slot                         (PReq)
+#define EPL_FRAME_FLAG1_PS          0x40	// prescaled slot                           (SoC)
+#define EPL_FRAME_FLAG1_MC          0x80	// multiplexed cycle completed              (SoC)
+#define EPL_FRAME_FLAG2_RS          0x07	// number of pending requests to send       (PRes, StatusRes, IdentRes)
+#define EPL_FRAME_FLAG2_PR          0x38	// priority of requested asynch. frame      (PRes, StatusRes, IdentRes)
+#define EPL_FRAME_FLAG2_PR_SHIFT    3	// shift of priority of requested asynch. frame
+
+// error history/status entry types
+#define EPL_ERR_ENTRYTYPE_STATUS        0x8000
+#define EPL_ERR_ENTRYTYPE_HISTORY       0x0000
+#define EPL_ERR_ENTRYTYPE_EMCY          0x4000
+#define EPL_ERR_ENTRYTYPE_MODE_ACTIVE   0x1000
+#define EPL_ERR_ENTRYTYPE_MODE_CLEARED  0x2000
+#define EPL_ERR_ENTRYTYPE_MODE_OCCURRED 0x3000
+#define EPL_ERR_ENTRYTYPE_MODE_MASK     0x3000
+#define EPL_ERR_ENTRYTYPE_PROF_VENDOR   0x0001
+#define EPL_ERR_ENTRYTYPE_PROF_EPL      0x0002
+#define EPL_ERR_ENTRYTYPE_PROF_MASK     0x0FFF
+
+// defines for EPL version / PDO version
+#define EPL_VERSION_SUB             0x0F	// sub version
+#define EPL_VERSION_MAIN            0xF0	// main version
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+// $$$ d.k.: move this definition to global.h
+// byte-align structures
+#ifdef _MSC_VER
+#    pragma pack( push, packing )
+#    pragma pack( 1 )
+#    define PACK_STRUCT
+#elif defined( __GNUC__ )
+#    define PACK_STRUCT    __attribute__((packed))
+#else
+#    error you must byte-align these structures with the appropriate compiler directives
+#endif
+
+typedef struct {
+	// Offset 17
+	BYTE m_le_bRes1;	// reserved
+	// Offset 18
+	BYTE m_le_bFlag1;	// Flags: MC, PS
+	// Offset 19
+	BYTE m_le_bFlag2;	// Flags: res
+	// Offset 20
+	tEplNetTime m_le_NetTime;	// supported if D_NMT_NetTimeIsRealTime_BOOL is set
+	// Offset 28
+	QWORD m_le_RelativeTime;	// in us (supported if D_NMT_RelativeTime_BOOL is set)
+
+} PACK_STRUCT tEplSocFrame;
+
+typedef struct {
+	// Offset 17
+	BYTE m_le_bRes1;	// reserved
+	// Offset 18
+	BYTE m_le_bFlag1;	// Flags: MS, EA, RD
+	// Offset 19
+	BYTE m_le_bFlag2;	// Flags: res
+	// Offset 20
+	BYTE m_le_bPdoVersion;
+	// Offset 21
+	BYTE m_le_bRes2;	// reserved
+	// Offset 22
+	WORD m_le_wSize;
+	// Offset 24
+	BYTE m_le_abPayload[256 /*D_NMT_IsochrRxMaxPayload_U16 */ ];
+
+} PACK_STRUCT tEplPreqFrame;
+
+typedef struct {
+	// Offset 17
+	BYTE m_le_bNmtStatus;	// NMT state
+	// Offset 18
+	BYTE m_le_bFlag1;	// Flags: MS, EN, RD
+	// Offset 19
+	BYTE m_le_bFlag2;	// Flags: PR, RS
+	// Offset 20
+	BYTE m_le_bPdoVersion;
+	// Offset 21
+	BYTE m_le_bRes2;	// reserved
+	// Offset 22
+	WORD m_le_wSize;
+	// Offset 24
+	BYTE m_le_abPayload[256	/*D_NMT_IsochrRxMaxPayload_U16
+				   / D_NMT_IsochrTxMaxPayload_U16 */ ];
+
+} PACK_STRUCT tEplPresFrame;
+
+typedef struct {
+	// Offset 17
+	BYTE m_le_bNmtStatus;	// NMT state
+	// Offset 18
+	BYTE m_le_bFlag1;	// Flags: EA, ER
+	// Offset 19
+	BYTE m_le_bFlag2;	// Flags: res
+	// Offset 20
+	BYTE m_le_bReqServiceId;
+	// Offset 21
+	BYTE m_le_bReqServiceTarget;
+	// Offset 22
+	BYTE m_le_bEplVersion;
+
+} PACK_STRUCT tEplSoaFrame;
+
+typedef struct {
+	WORD m_wEntryType;
+	WORD m_wErrorCode;
+	tEplNetTime m_TimeStamp;
+	BYTE m_abAddInfo[8];
+
+} PACK_STRUCT tEplErrHistoryEntry;
+
+typedef struct {
+	// Offset 18
+	BYTE m_le_bFlag1;	// Flags: EN, EC
+	BYTE m_le_bFlag2;	// Flags: PR, RS
+	BYTE m_le_bNmtStatus;	// NMT state
+	BYTE m_le_bRes1[3];
+	QWORD m_le_qwStaticError;	// static error bit field
+	tEplErrHistoryEntry m_le_aErrHistoryEntry[14];
+
+} PACK_STRUCT tEplStatusResponse;
+
+typedef struct {
+	// Offset 18
+	BYTE m_le_bFlag1;	// Flags: res
+	BYTE m_le_bFlag2;	// Flags: PR, RS
+	BYTE m_le_bNmtStatus;	// NMT state
+	BYTE m_le_bIdentRespFlags;	// Flags: FW
+	BYTE m_le_bEplProfileVersion;
+	BYTE m_le_bRes1;
+	DWORD m_le_dwFeatureFlags;	// NMT_FeatureFlags_U32
+	WORD m_le_wMtu;		// NMT_CycleTiming_REC.AsyncMTU_U16: C_IP_MIN_MTU - C_IP_MAX_MTU
+	WORD m_le_wPollInSize;	// NMT_CycleTiming_REC.PReqActPayload_U16
+	WORD m_le_wPollOutSize;	// NMT_CycleTiming_REC.PResActPayload_U16
+	DWORD m_le_dwResponseTime;	// NMT_CycleTiming_REC.PResMaxLatency_U32
+	WORD m_le_wRes2;
+	DWORD m_le_dwDeviceType;	// NMT_DeviceType_U32
+	DWORD m_le_dwVendorId;	// NMT_IdentityObject_REC.VendorId_U32
+	DWORD m_le_dwProductCode;	// NMT_IdentityObject_REC.ProductCode_U32
+	DWORD m_le_dwRevisionNumber;	// NMT_IdentityObject_REC.RevisionNo_U32
+	DWORD m_le_dwSerialNumber;	// NMT_IdentityObject_REC.SerialNo_U32
+	QWORD m_le_qwVendorSpecificExt1;
+	DWORD m_le_dwVerifyConfigurationDate;	// CFM_VerifyConfiguration_REC.ConfDate_U32
+	DWORD m_le_dwVerifyConfigurationTime;	// CFM_VerifyConfiguration_REC.ConfTime_U32
+	DWORD m_le_dwApplicationSwDate;	// PDL_LocVerApplSw_REC.ApplSwDate_U32 on programmable device or date portion of NMT_ManufactSwVers_VS on non-programmable device
+	DWORD m_le_dwApplicationSwTime;	// PDL_LocVerApplSw_REC.ApplSwTime_U32 on programmable device or time portion of NMT_ManufactSwVers_VS on non-programmable device
+	DWORD m_le_dwIpAddress;
+	DWORD m_le_dwSubnetMask;
+	DWORD m_le_dwDefaultGateway;
+	BYTE m_le_sHostname[32];
+	BYTE m_le_abVendorSpecificExt2[48];
+
+} PACK_STRUCT tEplIdentResponse;
+
+typedef struct {
+	// Offset 18
+	BYTE m_le_bNmtCommandId;
+	BYTE m_le_bRes1;
+	BYTE m_le_abNmtCommandData[32];
+
+} PACK_STRUCT tEplNmtCommandService;
+
+typedef struct {
+	BYTE m_le_bReserved;
+	BYTE m_le_bTransactionId;
+	BYTE m_le_bFlags;
+	BYTE m_le_bCommandId;
+	WORD m_le_wSegmentSize;
+	WORD m_le_wReserved;
+	BYTE m_le_abCommandData[8];	// just reserve a minimum number of bytes as a placeholder
+
+} PACK_STRUCT tEplAsySdoCom;
+
+// asynchronous SDO Sequence Header
+typedef struct {
+	BYTE m_le_bRecSeqNumCon;
+	BYTE m_le_bSendSeqNumCon;
+	BYTE m_le_abReserved[2];
+	tEplAsySdoCom m_le_abSdoSeqPayload;
+
+} PACK_STRUCT tEplAsySdoSeq;
+
+typedef struct {
+	// Offset 18
+	BYTE m_le_bNmtCommandId;
+	BYTE m_le_bTargetNodeId;
+	BYTE m_le_abNmtCommandData[32];
+
+} PACK_STRUCT tEplNmtRequestService;
+
+typedef union {
+	// Offset 18
+	tEplStatusResponse m_StatusResponse;
+	tEplIdentResponse m_IdentResponse;
+	tEplNmtCommandService m_NmtCommandService;
+	tEplNmtRequestService m_NmtRequestService;
+	tEplAsySdoSeq m_SdoSequenceFrame;
+	BYTE m_le_abPayload[256	/*D_NMT_ASndTxMaxPayload_U16
+				   / D_NMT_ASndRxMaxPayload_U16 */ ];
+
+} tEplAsndPayload;
+
+typedef struct {
+	// Offset 17
+	BYTE m_le_bServiceId;
+	// Offset 18
+	tEplAsndPayload m_Payload;
+
+} PACK_STRUCT tEplAsndFrame;
+
+typedef union {
+	// Offset 17
+	tEplSocFrame m_Soc;
+	tEplPreqFrame m_Preq;
+	tEplPresFrame m_Pres;
+	tEplSoaFrame m_Soa;
+	tEplAsndFrame m_Asnd;
+
+} tEplFrameData;
+
+typedef struct {
+	// Offset 0
+	BYTE m_be_abDstMac[6];	// MAC address of the addressed nodes
+	// Offset 6
+	BYTE m_be_abSrcMac[6];	// MAC address of the transmitting node
+	// Offset 12
+	WORD m_be_wEtherType;	// Ethernet message type (big endian)
+	// Offset 14
+	BYTE m_le_bMessageType;	// EPL message type
+	// Offset 15
+	BYTE m_le_bDstNodeId;	// EPL node ID of the addressed nodes
+	// Offset 16
+	BYTE m_le_bSrcNodeId;	// EPL node ID of the transmitting node
+	// Offset 17
+	tEplFrameData m_Data;
+
+} PACK_STRUCT tEplFrame;
+
+// un-byte-align structures
+#ifdef _MSC_VER
+#    pragma pack( pop, packing )
+#endif
+
+typedef enum {
+	kEplMsgTypeNonEpl = 0x00,
+	kEplMsgTypeSoc = 0x01,
+	kEplMsgTypePreq = 0x03,
+	kEplMsgTypePres = 0x04,
+	kEplMsgTypeSoa = 0x05,
+	kEplMsgTypeAsnd = 0x06,
+
+} tEplMsgType;
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#endif // #ifndef _EPL_FRAME_H_
diff --git a/drivers/staging/epl/EplIdentu.c b/drivers/staging/epl/EplIdentu.c
new file mode 100644
index 00000000000..ce59ef09ccd
--- /dev/null
+++ b/drivers/staging/epl/EplIdentu.c
@@ -0,0 +1,488 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for Identu-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplIdentu.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.8 $  $Date: 2008/11/21 09:00:38 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/11/15 d.k.:   start of the implementation
+
+****************************************************************************/
+
+#include "user/EplIdentu.h"
+#include "user/EplDlluCal.h"
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  <xxxxx>                                             */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description:
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   D E F I N I T I O N S                          //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+typedef struct {
+	tEplIdentResponse *m_apIdentResponse[254];	// the IdentResponse are managed dynamically
+	tEplIdentuCbResponse m_apfnCbResponse[254];
+
+} tEplIdentuInstance;
+
+//---------------------------------------------------------------------------
+// local vars
+//---------------------------------------------------------------------------
+
+static tEplIdentuInstance EplIdentuInstance_g;
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+static tEplKernel PUBLIC EplIdentuCbIdentResponse(tEplFrameInfo * pFrameInfo_p);
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplIdentuInit
+//
+// Description: init first instance of the module
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT tEplKernel PUBLIC EplIdentuInit()
+{
+	tEplKernel Ret;
+
+	Ret = EplIdentuAddInstance();
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplIdentuAddInstance
+//
+// Description: init other instances of the module
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT tEplKernel PUBLIC EplIdentuAddInstance()
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// reset instance structure
+	EPL_MEMSET(&EplIdentuInstance_g, 0, sizeof(EplIdentuInstance_g));
+
+	// register IdentResponse callback function
+	Ret =
+	    EplDlluCalRegAsndService(kEplDllAsndIdentResponse,
+				     EplIdentuCbIdentResponse,
+				     kEplDllAsndFilterAny);
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplIdentuDelInstance
+//
+// Description: delete instance
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT tEplKernel PUBLIC EplIdentuDelInstance()
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// deregister IdentResponse callback function
+	Ret =
+	    EplDlluCalRegAsndService(kEplDllAsndIdentResponse, NULL,
+				     kEplDllAsndFilterNone);
+
+	Ret = EplIdentuReset();
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplIdentuReset
+//
+// Description: resets this instance
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT tEplKernel PUBLIC EplIdentuReset()
+{
+	tEplKernel Ret;
+	int iIndex;
+
+	Ret = kEplSuccessful;
+
+	for (iIndex = 0;
+	     iIndex < tabentries(EplIdentuInstance_g.m_apIdentResponse);
+	     iIndex++) {
+		if (EplIdentuInstance_g.m_apIdentResponse[iIndex] != NULL) {	// free memory
+			EPL_FREE(EplIdentuInstance_g.m_apIdentResponse[iIndex]);
+		}
+	}
+
+	EPL_MEMSET(&EplIdentuInstance_g, 0, sizeof(EplIdentuInstance_g));
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplIdentuGetIdentResponse
+//
+// Description: returns the IdentResponse for the specified node.
+//
+// Parameters:  uiNodeId_p                  = IN: node ID
+//              ppIdentResponse_p           = OUT: pointer to pointer of IdentResponse
+//                                            equals NULL, if no IdentResponse available
+//
+// Return:      tEplKernel                  = error code
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplIdentuGetIdentResponse(unsigned int uiNodeId_p,
+					    tEplIdentResponse **
+					    ppIdentResponse_p)
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// decrement node ID, because array is zero based
+	uiNodeId_p--;
+	if (uiNodeId_p < tabentries(EplIdentuInstance_g.m_apIdentResponse)) {
+		*ppIdentResponse_p =
+		    EplIdentuInstance_g.m_apIdentResponse[uiNodeId_p];
+	} else {		// invalid node ID specified
+		*ppIdentResponse_p = NULL;
+		Ret = kEplInvalidNodeId;
+	}
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplIdentuRequestIdentResponse
+//
+// Description: returns the IdentResponse for the specified node.
+//
+// Parameters:  uiNodeId_p                  = IN: node ID
+//              pfnCbResponse_p             = IN: function pointer to callback function
+//                                            which will be called if IdentResponse is received
+//
+// Return:      tEplKernel                  = error code
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplIdentuRequestIdentResponse(unsigned int uiNodeId_p,
+						tEplIdentuCbResponse
+						pfnCbResponse_p)
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// decrement node ID, because array is zero based
+	uiNodeId_p--;
+	if (uiNodeId_p < tabentries(EplIdentuInstance_g.m_apfnCbResponse)) {
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+		if (EplIdentuInstance_g.m_apfnCbResponse[uiNodeId_p] != NULL) {	// request already issued (maybe by someone else)
+			Ret = kEplInvalidOperation;
+		} else {
+			EplIdentuInstance_g.m_apfnCbResponse[uiNodeId_p] =
+			    pfnCbResponse_p;
+			Ret =
+			    EplDlluCalIssueRequest(kEplDllReqServiceIdent,
+						   (uiNodeId_p + 1), 0xFF);
+		}
+#else
+		Ret = kEplInvalidOperation;
+#endif
+	} else {		// invalid node ID specified
+		Ret = kEplInvalidNodeId;
+	}
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplIdentuGetRunningRequests
+//
+// Description: returns a bit field with the running requests for node-ID 1-32
+//              just for debugging purposes
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT DWORD PUBLIC EplIdentuGetRunningRequests(void)
+{
+	DWORD dwReqs = 0;
+	unsigned int uiIndex;
+
+	for (uiIndex = 0; uiIndex < 32; uiIndex++) {
+		if (EplIdentuInstance_g.m_apfnCbResponse[uiIndex] != NULL) {
+			dwReqs |= (1 << uiIndex);
+		}
+	}
+
+	return dwReqs;
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplIdentuCbIdentResponse
+//
+// Description: callback funktion for IdentResponse
+//
+//
+//
+// Parameters:  pFrameInfo_p            = Frame with the IdentResponse
+//
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel PUBLIC EplIdentuCbIdentResponse(tEplFrameInfo * pFrameInfo_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	unsigned int uiNodeId;
+	unsigned int uiIndex;
+	tEplIdentuCbResponse pfnCbResponse;
+
+	uiNodeId = AmiGetByteFromLe(&pFrameInfo_p->m_pFrame->m_le_bSrcNodeId);
+
+	uiIndex = uiNodeId - 1;
+
+	if (uiIndex < tabentries(EplIdentuInstance_g.m_apfnCbResponse)) {
+		// memorize pointer to callback function
+		pfnCbResponse = EplIdentuInstance_g.m_apfnCbResponse[uiIndex];
+		// reset callback function pointer so that caller may issue next request immediately
+		EplIdentuInstance_g.m_apfnCbResponse[uiIndex] = NULL;
+
+		if (pFrameInfo_p->m_uiFrameSize < EPL_C_DLL_MINSIZE_IDENTRES) {	// IdentResponse not received or it has invalid size
+			if (pfnCbResponse == NULL) {	// response was not requested
+				goto Exit;
+			}
+			Ret = pfnCbResponse(uiNodeId, NULL);
+		} else {	// IdentResponse received
+			if (EplIdentuInstance_g.m_apIdentResponse[uiIndex] == NULL) {	// memory for IdentResponse must be allocated
+				EplIdentuInstance_g.m_apIdentResponse[uiIndex] =
+				    EPL_MALLOC(sizeof(tEplIdentResponse));
+				if (EplIdentuInstance_g.m_apIdentResponse[uiIndex] == NULL) {	// malloc failed
+					if (pfnCbResponse == NULL) {	// response was not requested
+						goto Exit;
+					}
+					Ret =
+					    pfnCbResponse(uiNodeId,
+							  &pFrameInfo_p->
+							  m_pFrame->m_Data.
+							  m_Asnd.m_Payload.
+							  m_IdentResponse);
+					goto Exit;
+				}
+			}
+			// copy IdentResponse to instance structure
+			EPL_MEMCPY(EplIdentuInstance_g.
+				   m_apIdentResponse[uiIndex],
+				   &pFrameInfo_p->m_pFrame->m_Data.m_Asnd.
+				   m_Payload.m_IdentResponse,
+				   sizeof(tEplIdentResponse));
+			if (pfnCbResponse == NULL) {	// response was not requested
+				goto Exit;
+			}
+			Ret =
+			    pfnCbResponse(uiNodeId,
+					  EplIdentuInstance_g.
+					  m_apIdentResponse[uiIndex]);
+		}
+	}
+
+      Exit:
+	return Ret;
+}
+
+// EOF
diff --git a/drivers/staging/epl/EplInc.h b/drivers/staging/epl/EplInc.h
new file mode 100644
index 00000000000..77f93d14416
--- /dev/null
+++ b/drivers/staging/epl/EplInc.h
@@ -0,0 +1,385 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  basic include file for internal EPL stack modules
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplInc.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.8 $  $Date: 2008/11/17 16:40:39 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/05/22 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_INC_H_
+#define _EPL_INC_H_
+
+// ============================================================================
+// include files
+// ============================================================================
+#if defined(WIN32) || defined(_WIN32)
+
+#ifdef UNDER_RTSS
+	// RTX header
+#include <windows.h>
+#include <process.h>
+#include <rtapi.h>
+
+#elif __BORLANDC__
+	// borland C header
+#include <windows.h>
+#include <process.h>
+
+#elif WINCE
+#include <windows.h>
+
+#else
+	// MSVC needs to include windows.h at first
+	// the following defines ar necessary for function prototypes for waitable timers
+#define _WIN32_WINDOWS 0x0401
+#define _WIN32_WINNT   0x0400
+#include <windows.h>
+#include <process.h>
+#endif
+
+#endif
+
+// defines for module integration
+// possible other include file needed
+// These constants defines modules which can be included in the Epl application.
+// Use this constants for define EPL_MODULE_INTEGRATION in file EplCfg.h.
+#define EPL_MODULE_OBDK        0x00000001L	// OBD kernel part module
+#define EPL_MODULE_PDOK        0x00000002L	// PDO kernel part module
+#define EPL_MODULE_NMT_MN      0x00000004L	// NMT MN module
+#define EPL_MODULE_SDOS        0x00000008L	// SDO Server module
+#define EPL_MODULE_SDOC        0x00000010L	// SDO Client module
+#define EPL_MODULE_SDO_ASND    0x00000020L	// SDO over Asnd module
+#define EPL_MODULE_SDO_UDP     0x00000040L	// SDO over UDP module
+#define EPL_MODULE_SDO_PDO     0x00000080L	// SDO in PDO module
+#define EPL_MODULE_NMT_CN      0x00000100L	// NMT CN module
+#define EPL_MODULE_NMTU        0x00000200L	// NMT user part module
+#define EPL_MODULE_NMTK        0x00000400L	// NMT kernel part module
+#define EPL_MODULE_DLLK        0x00000800L	// DLL kernel part module
+#define EPL_MODULE_DLLU        0x00001000L	// DLL user part module
+#define EPL_MODULE_OBDU        0x00002000L	// OBD user part module
+#define EPL_MODULE_CFGMA       0x00004000L	// Configuartioan Manager module
+#define EPL_MODULE_VETH        0x00008000L	// virtual ethernet driver module
+#define EPL_MODULE_PDOU        0x00010000L	// PDO user part module
+#define EPL_MODULE_LEDU        0x00020000L	// LED user part module
+
+#include "EplCfg.h"		// EPL configuration file (configuration from application)
+
+#include "global.h"		// global definitions
+
+#include "EplDef.h"		// EPL configuration file (default configuration)
+#include "EplInstDef.h"		// defines macros for instance types and table
+#include "Debug.h"		// debug definitions
+
+#include "EplErrDef.h"		// EPL error codes for API funtions
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+// IEEE 1588 conformant net time structure
+typedef struct {
+	DWORD m_dwSec;
+	DWORD m_dwNanoSec;
+
+} tEplNetTime;
+
+#include "EplTarget.h"		// target specific functions and definitions
+
+#include "EplAmi.h"
+
+// -------------------------------------------------------------------------
+// macros
+// -------------------------------------------------------------------------
+
+#define EPL_SPEC_VERSION                    0x20	// ETHERNET Powerlink V. 2.0
+#define EPL_STACK_VERSION(ver,rev,rel)      ((((DWORD)(ver)) & 0xFF)|((((DWORD)(rev))&0xFF)<<8)|(((DWORD)(rel))<<16))
+#define EPL_OBJ1018_VERSION(ver,rev,rel)    ((((DWORD)(ver))<<16) |(((DWORD)(rev))&0xFFFF))
+#define EPL_STRING_VERSION(ver,rev,rel)     "V" #ver "." #rev " r" #rel
+
+#include "EplVersion.h"
+
+// defines for EPL FeatureFlags
+#define EPL_FEATURE_ISOCHR          0x00000001
+#define EPL_FEATURE_SDO_UDP         0x00000002
+#define EPL_FEATURE_SDO_ASND        0x00000004
+#define EPL_FEATURE_SDO_PDO         0x00000008
+#define EPL_FEATURE_NMT_INFO        0x00000010
+#define EPL_FEATURE_NMT_EXT         0x00000020
+#define EPL_FEATURE_PDO_DYN         0x00000040
+#define EPL_FEATURE_NMT_UDP         0x00000080
+#define EPL_FEATURE_CFGMA           0x00000100
+#define EPL_FEATURE_DLL_MULTIPLEX   0x00000200
+#define EPL_FEATURE_NODEID_SW       0x00000400
+#define EPL_FEATURE_NMT_BASICETH    0x00000800
+#define EPL_FEATURE_RT1             0x00001000
+#define EPL_FEATURE_RT2             0x00002000
+
+// generate EPL NMT_FeatureFlags_U32
+#ifndef EPL_DEF_FEATURE_ISOCHR
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+#define EPL_DEF_FEATURE_ISOCHR          (EPL_FEATURE_ISOCHR)
+#else
+#define EPL_DEF_FEATURE_ISOCHR          0
+#endif
+#endif
+
+#ifndef EPL_DEF_FEATURE_SDO_ASND
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_ASND)) != 0)
+#define EPL_DEF_FEATURE_SDO_ASND        (EPL_FEATURE_SDO_ASND)
+#else
+#define EPL_DEF_FEATURE_SDO_ASND        0
+#endif
+#endif
+
+#ifndef EPL_DEF_FEATURE_SDO_UDP
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) != 0)
+#define EPL_DEF_FEATURE_SDO_UDP         (EPL_FEATURE_SDO_UDP)
+#else
+#define EPL_DEF_FEATURE_SDO_UDP         0
+#endif
+#endif
+
+#ifndef EPL_DEF_FEATURE_SDO_PDO
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_PDO)) != 0)
+#define EPL_DEF_FEATURE_SDO_PDO         (EPL_FEATURE_SDO_PDO)
+#else
+#define EPL_DEF_FEATURE_SDO_PDO         0
+#endif
+#endif
+
+#ifndef EPL_DEF_FEATURE_PDO_DYN
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0)
+#define EPL_DEF_FEATURE_PDO_DYN         (EPL_FEATURE_PDO_DYN)
+#else
+#define EPL_DEF_FEATURE_PDO_DYN         0
+#endif
+#endif
+
+#ifndef EPL_DEF_FEATURE_CFGMA
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_CFGMA)) != 0)
+#define EPL_DEF_FEATURE_CFGMA           (EPL_FEATURE_CFGMA)
+#else
+#define EPL_DEF_FEATURE_CFGMA           0
+#endif
+#endif
+
+#define EPL_DEF_FEATURE_FLAGS                   (EPL_DEF_FEATURE_ISOCHR \
+                                                | EPL_DEF_FEATURE_SDO_ASND \
+                                                | EPL_DEF_FEATURE_SDO_UDP \
+                                                | EPL_DEF_FEATURE_SDO_PDO \
+                                                | EPL_DEF_FEATURE_PDO_DYN \
+                                                | EPL_DEF_FEATURE_CFGMA)
+
+#ifndef tabentries
+#define tabentries(a)   (sizeof(a)/sizeof(*(a)))
+#endif
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+// definitions for DLL export
+#if ((DEV_SYSTEM == _DEV_WIN32_) || (DEV_SYSTEM == _DEV_WIN_CE_)) && defined (COP_LIB)
+
+#define EPLDLLEXPORT    __declspec (dllexport)
+
+#else
+
+#define EPLDLLEXPORT
+
+#endif
+
+// ============================================================================
+// common debug macros
+// ============================================================================
+// for using macro DEBUG_TRACEx()
+//
+// Example:
+//      DEBUG_TRACE1 (EPL_DBGLVL_OBD, "Value is %d\n" , wObjectIndex);
+//
+// This message only will be printed if:
+//      - NDEBUG is not defined     AND !!!
+//      - flag 0x00000004L is set in DEF_DEBUG_LVL (can be defined in copcfg.h)
+//
+// default level is defined in copdef.h
+
+// debug-level and TRACE-macros         // standard-level   // flags for DEF_DEBUG_LVL
+#define EPL_DBGLVL_EDRV                 DEBUG_LVL_01	// 0x00000001L
+#define EPL_DBGLVL_EDRV_TRACE0          DEBUG_LVL_01_TRACE0
+#define EPL_DBGLVL_EDRV_TRACE1          DEBUG_LVL_01_TRACE1
+#define EPL_DBGLVL_EDRV_TRACE2          DEBUG_LVL_01_TRACE2
+#define EPL_DBGLVL_EDRV_TRACE3          DEBUG_LVL_01_TRACE3
+#define EPL_DBGLVL_EDRV_TRACE4          DEBUG_LVL_01_TRACE4
+
+#define EPL_DBGLVL_DLL                  DEBUG_LVL_02	// 0x00000002L
+#define EPL_DBGLVL_DLL_TRACE0           DEBUG_LVL_02_TRACE0
+#define EPL_DBGLVL_DLL_TRACE1           DEBUG_LVL_02_TRACE1
+#define EPL_DBGLVL_DLL_TRACE2           DEBUG_LVL_02_TRACE2
+#define EPL_DBGLVL_DLL_TRACE3           DEBUG_LVL_02_TRACE3
+#define EPL_DBGLVL_DLL_TRACE4           DEBUG_LVL_02_TRACE4
+
+#define EPL_DBGLVL_OBD                  DEBUG_LVL_03	// 0x00000004L
+#define EPL_DBGLVL_OBD_TRACE0           DEBUG_LVL_03_TRACE0
+#define EPL_DBGLVL_OBD_TRACE1           DEBUG_LVL_03_TRACE1
+#define EPL_DBGLVL_OBD_TRACE2           DEBUG_LVL_03_TRACE2
+#define EPL_DBGLVL_OBD_TRACE3           DEBUG_LVL_03_TRACE3
+#define EPL_DBGLVL_OBD_TRACE4           DEBUG_LVL_03_TRACE4
+
+#define EPL_DBGLVL_NMTK                 DEBUG_LVL_04	// 0x00000008L
+#define EPL_DBGLVL_NMTK_TRACE0          DEBUG_LVL_04_TRACE0
+#define EPL_DBGLVL_NMTK_TRACE1          DEBUG_LVL_04_TRACE1
+#define EPL_DBGLVL_NMTK_TRACE2          DEBUG_LVL_04_TRACE2
+#define EPL_DBGLVL_NMTK_TRACE3          DEBUG_LVL_04_TRACE3
+#define EPL_DBGLVL_NMTK_TRACE4          DEBUG_LVL_04_TRACE4
+
+#define EPL_DBGLVL_NMTCN                DEBUG_LVL_05	// 0x00000010L
+#define EPL_DBGLVL_NMTCN_TRACE0         DEBUG_LVL_05_TRACE0
+#define EPL_DBGLVL_NMTCN_TRACE1         DEBUG_LVL_05_TRACE1
+#define EPL_DBGLVL_NMTCN_TRACE2         DEBUG_LVL_05_TRACE2
+#define EPL_DBGLVL_NMTCN_TRACE3         DEBUG_LVL_05_TRACE3
+#define EPL_DBGLVL_NMTCN_TRACE4         DEBUG_LVL_05_TRACE4
+
+#define EPL_DBGLVL_NMTU                 DEBUG_LVL_06	// 0x00000020L
+#define EPL_DBGLVL_NMTU_TRACE0          DEBUG_LVL_06_TRACE0
+#define EPL_DBGLVL_NMTU_TRACE1          DEBUG_LVL_06_TRACE1
+#define EPL_DBGLVL_NMTU_TRACE2          DEBUG_LVL_06_TRACE2
+#define EPL_DBGLVL_NMTU_TRACE3          DEBUG_LVL_06_TRACE3
+#define EPL_DBGLVL_NMTU_TRACE4          DEBUG_LVL_06_TRACE4
+
+#define EPL_DBGLVL_NMTMN                DEBUG_LVL_07	// 0x00000040L
+#define EPL_DBGLVL_NMTMN_TRACE0         DEBUG_LVL_07_TRACE0
+#define EPL_DBGLVL_NMTMN_TRACE1         DEBUG_LVL_07_TRACE1
+#define EPL_DBGLVL_NMTMN_TRACE2         DEBUG_LVL_07_TRACE2
+#define EPL_DBGLVL_NMTMN_TRACE3         DEBUG_LVL_07_TRACE3
+#define EPL_DBGLVL_NMTMN_TRACE4         DEBUG_LVL_07_TRACE4
+
+//...
+
+#define EPL_DBGLVL_SDO                  DEBUG_LVL_25	// 0x01000000
+#define EPL_DBGLVL_SDO_TRACE0           DEBUG_LVL_25_TRACE0
+#define EPL_DBGLVL_SDO_TRACE1           DEBUG_LVL_25_TRACE1
+#define EPL_DBGLVL_SDO_TRACE2           DEBUG_LVL_25_TRACE2
+#define EPL_DBGLVL_SDO_TRACE3           DEBUG_LVL_25_TRACE3
+#define EPL_DBGLVL_SDO_TRACE4           DEBUG_LVL_25_TRACE4
+
+#define EPL_DBGLVL_VETH                 DEBUG_LVL_26	// 0x02000000
+#define EPL_DBGLVL_VETH_TRACE0          DEBUG_LVL_26_TRACE0
+#define EPL_DBGLVL_VETH_TRACE1          DEBUG_LVL_26_TRACE1
+#define EPL_DBGLVL_VETH_TRACE2          DEBUG_LVL_26_TRACE2
+#define EPL_DBGLVL_VETH_TRACE3          DEBUG_LVL_26_TRACE3
+#define EPL_DBGLVL_VETH_TRACE4          DEBUG_LVL_26_TRACE4
+
+#define EPL_DBGLVL_EVENTK               DEBUG_LVL_27	// 0x04000000
+#define EPL_DBGLVL_EVENTK_TRACE0        DEBUG_LVL_27_TRACE0
+#define EPL_DBGLVL_EVENTK_TRACE1        DEBUG_LVL_27_TRACE1
+#define EPL_DBGLVL_EVENTK_TRACE2        DEBUG_LVL_27_TRACE2
+#define EPL_DBGLVL_EVENTK_TRACE3        DEBUG_LVL_27_TRACE3
+#define EPL_DBGLVL_EVENTK_TRACE4        DEBUG_LVL_27_TRACE4
+
+#define EPL_DBGLVL_EVENTU               DEBUG_LVL_28	// 0x08000000
+#define EPL_DBGLVL_EVENTU_TRACE0        DEBUG_LVL_28_TRACE0
+#define EPL_DBGLVL_EVENTU_TRACE1        DEBUG_LVL_28_TRACE1
+#define EPL_DBGLVL_EVENTU_TRACE2        DEBUG_LVL_28_TRACE2
+#define EPL_DBGLVL_EVENTU_TRACE3        DEBUG_LVL_28_TRACE3
+#define EPL_DBGLVL_EVENTU_TRACE4        DEBUG_LVL_28_TRACE4
+
+// SharedBuff
+#define EPL_DBGLVL_SHB                  DEBUG_LVL_29	// 0x10000000
+#define EPL_DBGLVL_SHB_TRACE0           DEBUG_LVL_29_TRACE0
+#define EPL_DBGLVL_SHB_TRACE1           DEBUG_LVL_29_TRACE1
+#define EPL_DBGLVL_SHB_TRACE2           DEBUG_LVL_29_TRACE2
+#define EPL_DBGLVL_SHB_TRACE3           DEBUG_LVL_29_TRACE3
+#define EPL_DBGLVL_SHB_TRACE4           DEBUG_LVL_29_TRACE4
+
+#define EPL_DBGLVL_ASSERT               DEBUG_LVL_ASSERT	// 0x20000000L
+#define EPL_DBGLVL_ASSERT_TRACE0        DEBUG_LVL_ASSERT_TRACE0
+#define EPL_DBGLVL_ASSERT_TRACE1        DEBUG_LVL_ASSERT_TRACE1
+#define EPL_DBGLVL_ASSERT_TRACE2        DEBUG_LVL_ASSERT_TRACE2
+#define EPL_DBGLVL_ASSERT_TRACE3        DEBUG_LVL_ASSERT_TRACE3
+#define EPL_DBGLVL_ASSERT_TRACE4        DEBUG_LVL_ASSERT_TRACE4
+
+#define EPL_DBGLVL_ERROR                DEBUG_LVL_ERROR	// 0x40000000L
+#define EPL_DBGLVL_ERROR_TRACE0         DEBUG_LVL_ERROR_TRACE0
+#define EPL_DBGLVL_ERROR_TRACE1         DEBUG_LVL_ERROR_TRACE1
+#define EPL_DBGLVL_ERROR_TRACE2         DEBUG_LVL_ERROR_TRACE2
+#define EPL_DBGLVL_ERROR_TRACE3         DEBUG_LVL_ERROR_TRACE3
+#define EPL_DBGLVL_ERROR_TRACE4         DEBUG_LVL_ERROR_TRACE4
+
+#define EPL_DBGLVL_ALWAYS               DEBUG_LVL_ALWAYS	// 0x80000000L
+#define EPL_DBGLVL_ALWAYS_TRACE0        DEBUG_LVL_ALWAYS_TRACE0
+#define EPL_DBGLVL_ALWAYS_TRACE1        DEBUG_LVL_ALWAYS_TRACE1
+#define EPL_DBGLVL_ALWAYS_TRACE2        DEBUG_LVL_ALWAYS_TRACE2
+#define EPL_DBGLVL_ALWAYS_TRACE3        DEBUG_LVL_ALWAYS_TRACE3
+#define EPL_DBGLVL_ALWAYS_TRACE4        DEBUG_LVL_ALWAYS_TRACE4
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#endif // #ifndef _EPL_INC_H_
diff --git a/drivers/staging/epl/EplInstDef.h b/drivers/staging/epl/EplInstDef.h
new file mode 100644
index 00000000000..89efbf27826
--- /dev/null
+++ b/drivers/staging/epl/EplInstDef.h
@@ -0,0 +1,377 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  definitions for generating instances
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplInstDef.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    ...
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  r.d.: first implementation
+
+****************************************************************************/
+
+#ifndef _EPLINSTDEF_H_
+#define _EPLINSTDEF_H_
+
+// =========================================================================
+// types and macros for generating instances
+// =========================================================================
+
+typedef enum {
+	kStateUnused = 0,
+	kStateDeleted = 1,
+	kStateUsed = 0xFF
+} tInstState;
+
+//------------------------------------------------------------------------------------------
+
+typedef void MEM *tEplPtrInstance;
+typedef BYTE tEplInstanceHdl;
+
+// define const for illegale values
+#define CCM_ILLINSTANCE      NULL
+#define CCM_ILLINSTANCE_HDL  0xFF
+
+//------------------------------------------------------------------------------------------
+// if more than one instance then use this macros
+#if (EPL_MAX_INSTANCES > 1)
+
+    //--------------------------------------------------------------------------------------
+    // macro definition for instance table definition
+    //--------------------------------------------------------------------------------------
+
+    // memory attributes for instance table
+#define INST_NEAR		// faster access to variables
+#define INST_FAR		// variables wich have to located in xdata
+#define STATIC			// prevent warnings for variables with same name
+
+#define INSTANCE_TYPE_BEGIN     typedef struct {
+#define INSTANCE_TYPE_END       } tEplInstanceInfo;
+
+    //--------------------------------------------------------------------------------------
+    // macro definition for API interface
+    //--------------------------------------------------------------------------------------
+
+    // declaration:
+
+    // macros for declaration within function header or prototype of API functions
+#define CCM_DECL_INSTANCE_HDL                   tEplInstanceHdl InstanceHandle
+#define CCM_DECL_INSTANCE_HDL_                  tEplInstanceHdl InstanceHandle,
+
+    // macros for declaration of pointer to instance handle within function header or prototype of API functions
+#define CCM_DECL_PTR_INSTANCE_HDL               tEplInstanceHdl MEM* pInstanceHandle
+#define CCM_DECL_PTR_INSTANCE_HDL_              tEplInstanceHdl MEM* pInstanceHandle,
+
+    // macros for declaration instance as lokacl variable within functions
+#define CCM_DECL_INSTANCE_PTR_LOCAL             tCcmInstanceInfo MEM* pInstance;
+#define CCM_DECL_PTR_INSTANCE_HDL_LOCAL         tEplInstanceHdl  MEM* pInstanceHandle;
+
+    // reference:
+
+    // macros for reference of instance handle for function parameters
+#define CCM_INSTANCE_HDL                        InstanceHandle
+#define CCM_INSTANCE_HDL_                       InstanceHandle,
+
+    // macros for reference of instance parameter for function parameters
+#define CCM_INSTANCE_PARAM(par)                 par
+#define CCM_INSTANCE_PARAM_(par)                par,
+
+    // macros for reference of instance parameter for writing or reading values
+#define CCM_INST_ENTRY                          (*((tEplPtrInstance)pInstance))
+
+    // processing:
+
+    // macros for process instance handle
+#define CCM_CHECK_INSTANCE_HDL()                if (InstanceHandle >= EPL_MAX_INSTANCES) \
+                                                        {return (kEplIllegalInstance);}
+
+    // macros for process pointer to instance handle
+#define CCM_CHECK_PTR_INSTANCE_HDL()            if (pInstanceHandle == NULL) \
+                                                        {return (kEplInvalidInstanceParam);}
+
+    // This macro returned the handle and pointer to next free instance.
+#define CCM_GET_FREE_INSTANCE_AND_HDL()         pInstance = CcmGetFreeInstanceAndHandle (pInstanceHandle); \
+                                                    ASSERT (*pInstanceHandle != CCM_ILLINSTANCE_HDL);
+
+#define CCM_CHECK_INSTANCE_PTR()                if (pInstance == CCM_ILLINSTANCE) \
+                                                        {return (kEplNoFreeInstance);}
+
+#define CCM_GET_INSTANCE_PTR()                  pInstance = CcmGetInstancePtr (InstanceHandle);
+#define CCM_GET_FREE_INSTANCE_PTR()             pInstance = GetFreeInstance (); \
+                                                    ASSERT (pInstance != CCM_ILLINSTANCE);
+
+    //--------------------------------------------------------------------------------------
+    // macro definition for stack interface
+    //--------------------------------------------------------------------------------------
+
+    // macros for declaration within the function header, prototype or local var list
+    // Declaration of pointers within function paramater list must defined as void MEM*
+    // pointer.
+#define EPL_MCO_DECL_INSTANCE_PTR                   void MEM* pInstance
+#define EPL_MCO_DECL_INSTANCE_PTR_                  void MEM* pInstance,
+#define EPL_MCO_DECL_INSTANCE_PTR_LOCAL             tEplPtrInstance  pInstance;
+
+    // macros for reference of pointer to instance
+    // These macros are used for parameter passing to called function.
+#define EPL_MCO_INSTANCE_PTR                        pInstance
+#define EPL_MCO_INSTANCE_PTR_                       pInstance,
+#define EPL_MCO_ADDR_INSTANCE_PTR_                  &pInstance,
+
+    // macro for access of struct members of one instance
+    // An access to a member of instance table must be casted by the local
+    // defined type of instance table.
+#define EPL_MCO_INST_ENTRY                          (*(tEplPtrInstance)pInstance)
+#define EPL_MCO_GLB_VAR(var)                        (((tEplPtrInstance)pInstance)->var)
+
+    // macros for process pointer to instance
+#define EPL_MCO_GET_INSTANCE_PTR()                  pInstance = (tEplPtrInstance) GetInstancePtr (InstanceHandle);
+#define EPL_MCO_GET_FREE_INSTANCE_PTR()             pInstance = (tEplPtrInstance) GetFreeInstance (); \
+                                                    ASSERT (pInstance != CCM_ILLINSTANCE);
+
+    // This macro should be used to check the passed pointer to an public function
+#define EPL_MCO_CHECK_INSTANCE_STATE()              ASSERT (pInstance != NULL); \
+                                                    ASSERT (((tEplPtrInstance)pInstance)->m_InstState == kStateUsed);
+
+    // macros for declaration of pointer to instance pointer
+#define EPL_MCO_DECL_PTR_INSTANCE_PTR               void MEM*  MEM* pInstancePtr
+#define EPL_MCO_DECL_PTR_INSTANCE_PTR_              void MEM*  MEM* pInstancePtr,
+
+    // macros for reference of pointer to instance pointer
+    // These macros are used for parameter passing to called function.
+#define EPL_MCO_PTR_INSTANCE_PTR                    pInstancePtr
+#define EPL_MCO_PTR_INSTANCE_PTR_                   pInstancePtr,
+
+    // macros for process pointer to instance pointer
+#define EPL_MCO_CHECK_PTR_INSTANCE_PTR()            ASSERT (pInstancePtr != NULL);
+#define EPL_MCO_SET_PTR_INSTANCE_PTR()              (*pInstancePtr = pInstance);
+
+#define EPL_MCO_INSTANCE_PARAM(a)                   (a)
+#define EPL_MCO_INSTANCE_PARAM_(a)                  (a),
+#define EPL_MCO_INSTANCE_PARAM_IDX_()               EPL_MCO_INSTANCE_PARAM_ (EPL_MCO_GLB_VAR (m_bInstIndex))
+#define EPL_MCO_INSTANCE_PARAM_IDX()                EPL_MCO_INSTANCE_PARAM (EPL_MCO_GLB_VAR (m_bInstIndex))
+#define EPL_MCO_WRITE_INSTANCE_STATE(a)             EPL_MCO_GLB_VAR (m_InstState) = a;
+
+    // this macro deletes all instance entries as unused
+#define EPL_MCO_DELETE_INSTANCE_TABLE()                                    \
+    {                                                                      \
+        tEplInstanceInfo MEM*   pInstance       = &aEplInstanceTable_g[0]; \
+        tFastByte               InstNumber      = 0;                       \
+        tFastByte               i               = EPL_MAX_INSTANCES;       \
+        do {                                                               \
+            pInstance->m_InstState = (BYTE) kStateUnused;                  \
+            pInstance->m_bInstIndex = (BYTE) InstNumber;                   \
+            pInstance++; InstNumber++; i--;                                \
+        } while (i != 0);                                                  \
+    }
+
+    // definition of functions which has to be defined in each module of CANopen stack
+#define EPL_MCO_DEFINE_INSTANCE_FCT() \
+        static tEplPtrInstance GetInstancePtr (tEplInstanceHdl InstHandle_p);  \
+        static tEplPtrInstance GetFreeInstance (void);
+#define EPL_MCO_DECL_INSTANCE_FCT()                                            \
+        static tEplPtrInstance GetInstancePtr (tEplInstanceHdl InstHandle_p) { \
+            return &aEplInstanceTable_g[InstHandle_p]; }                       \
+        static tEplPtrInstance GetFreeInstance (void) {                        \
+            tEplInstanceInfo MEM*   pInstance   = &aEplInstanceTable_g[0];     \
+            tFastByte               i           = EPL_MAX_INSTANCES;           \
+            do { if (pInstance->m_InstState != kStateUsed) {                   \
+                    return (tEplPtrInstance) pInstance; }                      \
+                pInstance++; i--; }                                            \
+            while (i != 0);                                                    \
+            return CCM_ILLINSTANCE; }
+
+    // this macro defines the instance table. Each entry is reserved for an instance of CANopen.
+#define EPL_MCO_DECL_INSTANCE_VAR() \
+        static tEplInstanceInfo MEM aEplInstanceTable_g [EPL_MAX_INSTANCES];
+
+    // this macro defines member variables in instance table which are needed in
+    // all modules of Epl stack
+#define EPL_MCO_DECL_INSTANCE_MEMBER() \
+        STATIC  BYTE                            m_InstState; \
+        STATIC  BYTE                            m_bInstIndex;
+
+#define EPL_MCO_INSTANCE_PARAM_IDX_()           EPL_MCO_INSTANCE_PARAM_ (EPL_MCO_GLB_VAR (m_bInstIndex))
+#define EPL_MCO_INSTANCE_PARAM_IDX()            EPL_MCO_INSTANCE_PARAM (EPL_MCO_GLB_VAR (m_bInstIndex))
+
+#else // only one instance is used
+
+    // Memory attributes for instance table.
+#define INST_NEAR   NEAR	// faster access to variables
+#define INST_FAR    MEM		// variables wich have to located in xdata
+#define STATIC      static	// prevent warnings for variables with same name
+
+#define INSTANCE_TYPE_BEGIN
+#define INSTANCE_TYPE_END
+
+// macros for declaration, initializing and member access for instance handle
+// This class of macros are used by API function to inform CCM-modul which
+// instance is to be used.
+
+    // macros for reference of instance handle
+    // These macros are used for parameter passing to CANopen API function.
+#define CCM_INSTANCE_HDL
+#define CCM_INSTANCE_HDL_
+
+#define CCM_DECL_INSTANCE_PTR_LOCAL
+
+    // macros for declaration within the function header or prototype
+#define CCM_DECL_INSTANCE_HDL                   void
+#define CCM_DECL_INSTANCE_HDL_
+
+    // macros for process instance handle
+#define CCM_CHECK_INSTANCE_HDL()
+
+    // macros for declaration of pointer to instance handle
+#define CCM_DECL_PTR_INSTANCE_HDL               void
+#define CCM_DECL_PTR_INSTANCE_HDL_
+
+    // macros for process pointer to instance handle
+#define CCM_CHECK_PTR_INSTANCE_HDL()
+
+    // This macro returned the handle and pointer to next free instance.
+#define CCM_GET_FREE_INSTANCE_AND_HDL()
+
+#define CCM_CHECK_INSTANCE_PTR()
+
+#define CCM_GET_INSTANCE_PTR()
+#define CCM_GET_FREE_INSTANCE_PTR()
+
+#define CCM_INSTANCE_PARAM(par)
+#define CCM_INSTANCE_PARAM_(par)
+
+#define CCM_INST_ENTRY                          aCcmInstanceTable_g[0]
+
+// macros for declaration, initializing and member access for instance pointer
+// This class of macros are used by CANopen internal function to point to one instance.
+
+    // macros for declaration within the function header, prototype or local var list
+#define EPL_MCO_DECL_INSTANCE_PTR                   void
+#define EPL_MCO_DECL_INSTANCE_PTR_
+#define EPL_MCO_DECL_INSTANCE_PTR_LOCAL
+
+    // macros for reference of pointer to instance
+    // These macros are used for parameter passing to called function.
+#define EPL_MCO_INSTANCE_PTR
+#define EPL_MCO_INSTANCE_PTR_
+#define EPL_MCO_ADDR_INSTANCE_PTR_
+
+    // macros for process pointer to instance
+#define EPL_MCO_GET_INSTANCE_PTR()
+#define EPL_MCO_GET_FREE_INSTANCE_PTR()
+
+    // This macro should be used to check the passed pointer to an public function
+#define EPL_MCO_CHECK_INSTANCE_STATE()
+
+    // macros for declaration of pointer to instance pointer
+#define EPL_MCO_DECL_PTR_INSTANCE_PTR               void
+#define EPL_MCO_DECL_PTR_INSTANCE_PTR_
+
+    // macros for reference of pointer to instance pointer
+    // These macros are used for parameter passing to called function.
+#define EPL_MCO_PTR_INSTANCE_PTR
+#define EPL_MCO_PTR_INSTANCE_PTR_
+
+    // macros for process pointer to instance pointer
+#define EPL_MCO_CHECK_PTR_INSTANCE_PTR()
+#define EPL_MCO_SET_PTR_INSTANCE_PTR()
+
+#define EPL_MCO_INSTANCE_PARAM(a)
+#define EPL_MCO_INSTANCE_PARAM_(a)
+#define EPL_MCO_INSTANCE_PARAM_IDX_()
+#define EPL_MCO_INSTANCE_PARAM_IDX()
+
+    // macro for access of struct members of one instance
+#define EPL_MCO_INST_ENTRY                          aEplInstanceTable_g[0]
+#define EPL_MCO_GLB_VAR(var)                        (var)
+#define EPL_MCO_WRITE_INSTANCE_STATE(a)
+
+    // this macro deletes all instance entries as unused
+#define EPL_MCO_DELETE_INSTANCE_TABLE()
+
+    // definition of functions which has to be defined in each module of CANopen stack
+#define EPL_MCO_DEFINE_INSTANCE_FCT()
+#define EPL_MCO_DECL_INSTANCE_FCT()
+
+    // this macro defines the instance table. Each entry is reserved for an instance of CANopen.
+#define EPL_MCO_DECL_INSTANCE_VAR()
+
+    // this macro defines member variables in instance table which are needed in
+    // all modules of CANopen stack
+#define EPL_MCO_DECL_INSTANCE_MEMBER()
+
+#endif
+
+/*
+#if (CDRV_MAX_INSTANCES > 1)
+
+    #define CDRV_REENTRANT                          REENTRANT
+
+#else
+
+    #define CDRV_REENTRANT
+
+#endif
+*/
+
+#endif // _EPLINSTDEF_H_
+
+// Die letzte Zeile muß unbedingt eine leere Zeile sein, weil manche Compiler
+// damit ein Problem haben, wenn das nicht so ist (z.B. GNU oder Borland C++ Builder).
diff --git a/drivers/staging/epl/EplLed.h b/drivers/staging/epl/EplLed.h
new file mode 100644
index 00000000000..6a29aed303a
--- /dev/null
+++ b/drivers/staging/epl/EplLed.h
@@ -0,0 +1,92 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for status and error LED
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplLed.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.1 $  $Date: 2008/11/17 16:40:39 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2008/11/17 d.k.:   start of the implementation
+
+****************************************************************************/
+
+#ifndef _EPLLED_H_
+#define _EPLLED_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+typedef enum {
+	kEplLedTypeStatus = 0x00,
+	kEplLedTypeError = 0x01,
+
+} tEplLedType;
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#endif // #ifndef _EPLLED_H_
diff --git a/drivers/staging/epl/EplNmt.h b/drivers/staging/epl/EplNmt.h
new file mode 100644
index 00000000000..4c11e5bc74a
--- /dev/null
+++ b/drivers/staging/epl/EplNmt.h
@@ -0,0 +1,230 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  global include file for EPL-NMT-Modules
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplNmt.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.6 $  $Date: 2008/11/17 16:40:39 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/09 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#ifndef _EPLNMT_H_
+#define _EPLNMT_H_
+
+#include "EplInc.h"
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+// define super-states and masks to identify a super-state
+#define EPL_NMT_GS_POWERED          0x0008	// super state
+#define EPL_NMT_GS_INITIALISATION   0x0009	// super state
+#define EPL_NMT_GS_COMMUNICATING    0x000C	// super state
+#define EPL_NMT_CS_EPLMODE          0x000D	// super state
+#define EPL_NMT_MS_EPLMODE          0x000D	// super state
+
+#define EPL_NMT_SUPERSTATE_MASK     0x000F	// mask to select state
+
+#define EPL_NMT_TYPE_UNDEFINED      0x0000	// type of NMT state is still undefined
+#define EPL_NMT_TYPE_CS             0x0100	// CS type of NMT state
+#define EPL_NMT_TYPE_MS             0x0200	// MS type of NMT state
+#define EPL_NMT_TYPE_MASK           0x0300	// mask to select type of NMT state (i.e. CS or MS)
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+// the lower Byte of the NMT-State is encoded
+// like the values in the EPL-Standard
+// the higher byte is used to encode MN
+// (Bit 1 of the higher byte = 1) or CN (Bit 0 of the
+// higher byte  = 1)
+// the super-states are not mentioned in this
+// enum because they are no real states
+// --> there are masks defined to indentify the
+// super-states
+
+typedef enum {
+	kEplNmtGsOff = 0x0000,
+	kEplNmtGsInitialising = 0x0019,
+	kEplNmtGsResetApplication = 0x0029,
+	kEplNmtGsResetCommunication = 0x0039,
+	kEplNmtGsResetConfiguration = 0x0079,
+	kEplNmtCsNotActive = 0x011C,
+	kEplNmtCsPreOperational1 = 0x011D,
+	kEplNmtCsStopped = 0x014D,
+	kEplNmtCsPreOperational2 = 0x015D,
+	kEplNmtCsReadyToOperate = 0x016D,
+	kEplNmtCsOperational = 0x01FD,
+	kEplNmtCsBasicEthernet = 0x011E,
+	kEplNmtMsNotActive = 0x021C,
+	kEplNmtMsPreOperational1 = 0x021D,
+	kEplNmtMsPreOperational2 = 0x025D,
+	kEplNmtMsReadyToOperate = 0x026D,
+	kEplNmtMsOperational = 0x02FD,
+	kEplNmtMsBasicEthernet = 0x021E
+} tEplNmtState;
+
+// NMT-events
+typedef enum {
+	// Events from DLL
+	// Events defined by EPL V2 specification
+	kEplNmtEventNoEvent = 0x00,
+//    kEplNmtEventDllMePres           =   0x01,
+	kEplNmtEventDllMePresTimeout = 0x02,
+//    kEplNmtEventDllMeAsnd           =   0x03,
+//    kEplNmtEventDllMeAsndTimeout    =   0x04,
+	kEplNmtEventDllMeSoaSent = 0x04,
+	kEplNmtEventDllMeSocTrig = 0x05,
+	kEplNmtEventDllMeSoaTrig = 0x06,
+	kEplNmtEventDllCeSoc = 0x07,
+	kEplNmtEventDllCePreq = 0x08,
+	kEplNmtEventDllCePres = 0x09,
+	kEplNmtEventDllCeSoa = 0x0A,
+	kEplNmtEventDllCeAsnd = 0x0B,
+	kEplNmtEventDllCeFrameTimeout = 0x0C,
+
+	// Events triggered by NMT-Commands
+	kEplNmtEventSwReset = 0x10,	// NMT_GT1, NMT_GT2, NMT_GT8
+	kEplNmtEventResetNode = 0x11,
+	kEplNmtEventResetCom = 0x12,
+	kEplNmtEventResetConfig = 0x13,
+	kEplNmtEventEnterPreOperational2 = 0x14,
+	kEplNmtEventEnableReadyToOperate = 0x15,
+	kEplNmtEventStartNode = 0x16,	// NMT_CT7
+	kEplNmtEventStopNode = 0x17,
+
+	// Events triggered by higher layer
+	kEplNmtEventEnterResetApp = 0x20,
+	kEplNmtEventEnterResetCom = 0x21,
+	kEplNmtEventInternComError = 0x22,	// NMT_GT6, internal communication error -> enter ResetCommunication
+	kEplNmtEventEnterResetConfig = 0x23,
+	kEplNmtEventEnterCsNotActive = 0x24,
+	kEplNmtEventEnterMsNotActive = 0x25,
+	kEplNmtEventTimerBasicEthernet = 0x26,	// NMT_CT3; timer triggered state change (NotActive -> BasicEth)
+	kEplNmtEventTimerMsPreOp1 = 0x27,	// enter PreOp1 on MN (NotActive -> MsPreOp1)
+	kEplNmtEventNmtCycleError = 0x28,	// NMT_CT11, NMT_MT6; error during cycle -> enter PreOp1
+	kEplNmtEventTimerMsPreOp2 = 0x29,	// enter PreOp2 on MN (MsPreOp1 -> MsPreOp2 if kEplNmtEventAllMandatoryCNIdent)
+	kEplNmtEventAllMandatoryCNIdent = 0x2A,	// enter PreOp2 on MN if kEplNmtEventTimerMsPreOp2
+	kEplNmtEventEnterReadyToOperate = 0x2B,	// application ready for the state ReadyToOp
+	kEplNmtEventEnterMsOperational = 0x2C,	// enter Operational on MN
+	kEplNmtEventSwitchOff = 0x2D,	// enter state Off
+	kEplNmtEventCriticalError = 0x2E,	// enter state Off because of critical error
+
+} tEplNmtEvent;
+
+// type for argument of event kEplEventTypeNmtStateChange
+typedef struct {
+	tEplNmtState m_NewNmtState;
+	tEplNmtEvent m_NmtEvent;
+
+} tEplEventNmtStateChange;
+
+// structure for kEplEventTypeHeartbeat
+typedef struct {
+	unsigned int m_uiNodeId;	// NodeId
+	tEplNmtState m_NmtState;	// NMT state (remember distinguish between MN / CN)
+	WORD m_wErrorCode;	// EPL error code in case of NMT state NotActive
+
+} tEplHeartbeatEvent;
+
+typedef enum {
+	kEplNmtNodeEventFound = 0x00,
+	kEplNmtNodeEventUpdateSw = 0x01,	// application shall update software on CN
+	kEplNmtNodeEventCheckConf = 0x02,	// application / Configuration Manager shall check and update configuration on CN
+	kEplNmtNodeEventUpdateConf = 0x03,	// application / Configuration Manager shall update configuration on CN (check was done by NmtMn module)
+	kEplNmtNodeEventVerifyConf = 0x04,	// application / Configuration Manager shall verify configuration of CN
+	kEplNmtNodeEventReadyToStart = 0x05,	// issued if EPL_NMTST_NO_STARTNODE set
+	// application must call EplNmtMnuSendNmtCommand(kEplNmtCmdStartNode) manually
+	kEplNmtNodeEventNmtState = 0x06,
+	kEplNmtNodeEventError = 0x07,	// NMT error of CN
+
+} tEplNmtNodeEvent;
+
+typedef enum {
+	kEplNmtNodeCommandBoot = 0x01,	// if EPL_NODEASSIGN_START_CN not set it must be issued after kEplNmtNodeEventFound
+	kEplNmtNodeCommandSwOk = 0x02,	// application updated software on CN successfully
+	kEplNmtNodeCommandSwUpdated = 0x03,	// application updated software on CN successfully
+	kEplNmtNodeCommandConfOk = 0x04,	// application / Configuration Manager has updated configuration on CN successfully
+	kEplNmtNodeCommandConfReset = 0x05,	// application / Configuration Manager has updated configuration on CN successfully
+	// and CN needs ResetConf so that the configuration gets actived
+	kEplNmtNodeCommandConfErr = 0x06,	// application / Configuration Manager failed on updating configuration on CN
+	kEplNmtNodeCommandStart = 0x07,	// if EPL_NMTST_NO_STARTNODE set it must be issued after kEplNmtNodeEventReadyToStart
+
+} tEplNmtNodeCommand;
+
+typedef enum {
+	kEplNmtBootEventBootStep1Finish = 0x00,	// PreOp2 is possible
+	kEplNmtBootEventBootStep2Finish = 0x01,	// ReadyToOp is possible
+	kEplNmtBootEventCheckComFinish = 0x02,	// Operational is possible
+	kEplNmtBootEventOperational = 0x03,	// all mandatory CNs are Operational
+	kEplNmtBootEventError = 0x04,	// boot process halted because of an error
+
+} tEplNmtBootEvent;
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#endif // #ifndef _EPLNMT_H_
diff --git a/drivers/staging/epl/EplNmtCnu.c b/drivers/staging/epl/EplNmtCnu.c
new file mode 100644
index 00000000000..f2f46da08c7
--- /dev/null
+++ b/drivers/staging/epl/EplNmtCnu.c
@@ -0,0 +1,704 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for NMT-CN-Userspace-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplNmtCnu.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.6 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/09 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "EplInc.h"
+#include "user/EplNmtCnu.h"
+#include "user/EplDlluCal.h"
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_CN)) != 0)
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+typedef struct {
+	unsigned int m_uiNodeId;
+	tEplNmtuCheckEventCallback m_pfnCheckEventCb;
+
+} tEplNmtCnuInstance;
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+static tEplNmtCnuInstance EplNmtCnuInstance_g;
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+static tEplNmtCommand EplNmtCnuGetNmtCommand(tEplFrameInfo * pFrameInfo_p);
+
+static BOOL EplNmtCnuNodeIdList(BYTE * pbNmtCommandDate_p);
+
+static tEplKernel PUBLIC EplNmtCnuCommandCb(tEplFrameInfo * pFrameInfo_p);
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtCnuInit
+//
+// Description: init the first instance of the module
+//
+//
+//
+// Parameters:      uiNodeId_p = NodeId of the local node
+//
+//
+// Returns:         tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtCnuInit(unsigned int uiNodeId_p)
+{
+	tEplKernel Ret;
+
+	Ret = EplNmtCnuAddInstance(uiNodeId_p);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtCnuAddInstance
+//
+// Description: init the add new instance of the module
+//
+//
+//
+// Parameters:      uiNodeId_p = NodeId of the local node
+//
+//
+// Returns:         tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtCnuAddInstance(unsigned int uiNodeId_p)
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// reset instance structure
+	EPL_MEMSET(&EplNmtCnuInstance_g, 0, sizeof(EplNmtCnuInstance_g));
+
+	// save nodeid
+	EplNmtCnuInstance_g.m_uiNodeId = uiNodeId_p;
+
+	// register callback-function for NMT-commands
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLU)) != 0)
+	Ret = EplDlluCalRegAsndService(kEplDllAsndNmtCommand,
+				       EplNmtCnuCommandCb,
+				       kEplDllAsndFilterLocal);
+#endif
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtCnuDelInstance
+//
+// Description: delte instance of the module
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:         tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtCnuDelInstance()
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLU)) != 0)
+	// deregister callback function from DLL
+	Ret = EplDlluCalRegAsndService(kEplDllAsndNmtCommand,
+				       NULL, kEplDllAsndFilterNone);
+#endif
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtCnuSendNmtRequest
+//
+// Description: Send an NMT-Request to the MN
+//
+//
+//
+// Parameters:      uiNodeId_p = NodeId of the local node
+//                  NmtCommand_p = requested NMT-Command
+//
+//
+// Returns:         tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtCnuSendNmtRequest(unsigned int uiNodeId_p,
+						       tEplNmtCommand
+						       NmtCommand_p)
+{
+	tEplKernel Ret;
+	tEplFrameInfo NmtRequestFrameInfo;
+	tEplFrame NmtRequestFrame;
+
+	Ret = kEplSuccessful;
+
+	// build frame
+	EPL_MEMSET(&NmtRequestFrame.m_be_abDstMac[0], 0x00, sizeof(NmtRequestFrame.m_be_abDstMac));	// set by DLL
+	EPL_MEMSET(&NmtRequestFrame.m_be_abSrcMac[0], 0x00, sizeof(NmtRequestFrame.m_be_abSrcMac));	// set by DLL
+	AmiSetWordToBe(&NmtRequestFrame.m_be_wEtherType,
+		       EPL_C_DLL_ETHERTYPE_EPL);
+	AmiSetByteToLe(&NmtRequestFrame.m_le_bDstNodeId, (BYTE) EPL_C_ADR_MN_DEF_NODE_ID);	// node id of the MN
+	AmiSetByteToLe(&NmtRequestFrame.m_le_bMessageType,
+		       (BYTE) kEplMsgTypeAsnd);
+	AmiSetByteToLe(&NmtRequestFrame.m_Data.m_Asnd.m_le_bServiceId,
+		       (BYTE) kEplDllAsndNmtRequest);
+	AmiSetByteToLe(&NmtRequestFrame.m_Data.m_Asnd.m_Payload.
+		       m_NmtRequestService.m_le_bNmtCommandId,
+		       (BYTE) NmtCommand_p);
+	AmiSetByteToLe(&NmtRequestFrame.m_Data.m_Asnd.m_Payload.m_NmtRequestService.m_le_bTargetNodeId, (BYTE) uiNodeId_p);	// target for the nmt command
+	EPL_MEMSET(&NmtRequestFrame.m_Data.m_Asnd.m_Payload.m_NmtRequestService.
+		   m_le_abNmtCommandData[0], 0x00,
+		   sizeof(NmtRequestFrame.m_Data.m_Asnd.m_Payload.
+			  m_NmtRequestService.m_le_abNmtCommandData));
+
+	// build info-structure
+	NmtRequestFrameInfo.m_NetTime.m_dwNanoSec = 0;
+	NmtRequestFrameInfo.m_NetTime.m_dwSec = 0;
+	NmtRequestFrameInfo.m_pFrame = &NmtRequestFrame;
+	NmtRequestFrameInfo.m_uiFrameSize = EPL_C_DLL_MINSIZE_NMTREQ;	// sizeof(NmtRequestFrame);
+
+	// send NMT-Request
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLU)) != 0)
+	Ret = EplDlluCalAsyncSend(&NmtRequestFrameInfo,	// pointer to frameinfo
+				  kEplDllAsyncReqPrioNmt);	// priority
+#endif
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtCnuRegisterStateChangeCb
+//
+// Description: register Callback-function go get informed about a
+//              NMT-Change-State-Event
+//
+//
+//
+// Parameters:  pfnEplNmtStateChangeCb_p = functionpointer
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT tEplKernel PUBLIC
+EplNmtCnuRegisterCheckEventCb(tEplNmtuCheckEventCallback
+			      pfnEplNmtCheckEventCb_p)
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// save callback-function in modul global var
+	EplNmtCnuInstance_g.m_pfnCheckEventCb = pfnEplNmtCheckEventCb_p;
+
+	return Ret;
+
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtCnuCommandCb
+//
+// Description: callback funktion for NMT-Commands
+//
+//
+//
+// Parameters:      pFrameInfo_p = Frame with the NMT-Commando
+//
+//
+// Returns:         tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static tEplKernel PUBLIC EplNmtCnuCommandCb(tEplFrameInfo * pFrameInfo_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplNmtCommand NmtCommand;
+	BOOL fNodeIdInList;
+	tEplNmtEvent NmtEvent = kEplNmtEventNoEvent;
+
+	if (pFrameInfo_p == NULL) {
+		Ret = kEplNmtInvalidFramePointer;
+		goto Exit;
+	}
+
+	NmtCommand = EplNmtCnuGetNmtCommand(pFrameInfo_p);
+
+	// check NMT-Command
+	switch (NmtCommand) {
+
+		//------------------------------------------------------------------------
+		// plain NMT state commands
+	case kEplNmtCmdStartNode:
+		{		// send NMT-Event to state maschine kEplNmtEventStartNode
+			NmtEvent = kEplNmtEventStartNode;
+			break;
+		}
+
+	case kEplNmtCmdStopNode:
+		{		// send NMT-Event to state maschine kEplNmtEventStopNode
+			NmtEvent = kEplNmtEventStopNode;
+			break;
+		}
+
+	case kEplNmtCmdEnterPreOperational2:
+		{		// send NMT-Event to state maschine kEplNmtEventEnterPreOperational2
+			NmtEvent = kEplNmtEventEnterPreOperational2;
+			break;
+		}
+
+	case kEplNmtCmdEnableReadyToOperate:
+		{		// send NMT-Event to state maschine kEplNmtEventEnableReadyToOperate
+			NmtEvent = kEplNmtEventEnableReadyToOperate;
+			break;
+		}
+
+	case kEplNmtCmdResetNode:
+		{		// send NMT-Event to state maschine kEplNmtEventResetNode
+			NmtEvent = kEplNmtEventResetNode;
+			break;
+		}
+
+	case kEplNmtCmdResetCommunication:
+		{		// send NMT-Event to state maschine kEplNmtEventResetCom
+			NmtEvent = kEplNmtEventResetCom;
+			break;
+		}
+
+	case kEplNmtCmdResetConfiguration:
+		{		// send NMT-Event to state maschine kEplNmtEventResetConfig
+			NmtEvent = kEplNmtEventResetConfig;
+			break;
+		}
+
+	case kEplNmtCmdSwReset:
+		{		// send NMT-Event to state maschine kEplNmtEventSwReset
+			NmtEvent = kEplNmtEventSwReset;
+			break;
+		}
+
+		//------------------------------------------------------------------------
+		// extended NMT state commands
+
+	case kEplNmtCmdStartNodeEx:
+		{
+			// check if own nodeid is in EPL node list
+			fNodeIdInList =
+			    EplNmtCnuNodeIdList(&
+						(pFrameInfo_p->m_pFrame->m_Data.
+						 m_Asnd.m_Payload.
+						 m_NmtCommandService.
+						 m_le_abNmtCommandData[0]));
+			if (fNodeIdInList != FALSE) {	// own nodeid in list
+				// send event to process command
+				NmtEvent = kEplNmtEventStartNode;
+			}
+			break;
+		}
+
+	case kEplNmtCmdStopNodeEx:
+		{		// check if own nodeid is in EPL node list
+			fNodeIdInList =
+			    EplNmtCnuNodeIdList(&pFrameInfo_p->m_pFrame->m_Data.
+						m_Asnd.m_Payload.
+						m_NmtCommandService.
+						m_le_abNmtCommandData[0]);
+			if (fNodeIdInList != FALSE) {	// own nodeid in list
+				// send event to process command
+				NmtEvent = kEplNmtEventStopNode;
+			}
+			break;
+		}
+
+	case kEplNmtCmdEnterPreOperational2Ex:
+		{		// check if own nodeid is in EPL node list
+			fNodeIdInList =
+			    EplNmtCnuNodeIdList(&pFrameInfo_p->m_pFrame->m_Data.
+						m_Asnd.m_Payload.
+						m_NmtCommandService.
+						m_le_abNmtCommandData[0]);
+			if (fNodeIdInList != FALSE) {	// own nodeid in list
+				// send event to process command
+				NmtEvent = kEplNmtEventEnterPreOperational2;
+			}
+			break;
+		}
+
+	case kEplNmtCmdEnableReadyToOperateEx:
+		{		// check if own nodeid is in EPL node list
+			fNodeIdInList =
+			    EplNmtCnuNodeIdList(&pFrameInfo_p->m_pFrame->m_Data.
+						m_Asnd.m_Payload.
+						m_NmtCommandService.
+						m_le_abNmtCommandData[0]);
+			if (fNodeIdInList != FALSE) {	// own nodeid in list
+				// send event to process command
+				NmtEvent = kEplNmtEventEnableReadyToOperate;
+			}
+			break;
+		}
+
+	case kEplNmtCmdResetNodeEx:
+		{		// check if own nodeid is in EPL node list
+			fNodeIdInList =
+			    EplNmtCnuNodeIdList(&pFrameInfo_p->m_pFrame->m_Data.
+						m_Asnd.m_Payload.
+						m_NmtCommandService.
+						m_le_abNmtCommandData[0]);
+			if (fNodeIdInList != FALSE) {	// own nodeid in list
+				// send event to process command
+				NmtEvent = kEplNmtEventResetNode;
+			}
+			break;
+		}
+
+	case kEplNmtCmdResetCommunicationEx:
+		{		// check if own nodeid is in EPL node list
+			fNodeIdInList =
+			    EplNmtCnuNodeIdList(&pFrameInfo_p->m_pFrame->m_Data.
+						m_Asnd.m_Payload.
+						m_NmtCommandService.
+						m_le_abNmtCommandData[0]);
+			if (fNodeIdInList != FALSE) {	// own nodeid in list
+				// send event to process command
+				NmtEvent = kEplNmtEventResetCom;
+			}
+			break;
+		}
+
+	case kEplNmtCmdResetConfigurationEx:
+		{		// check if own nodeid is in EPL node list
+			fNodeIdInList =
+			    EplNmtCnuNodeIdList(&pFrameInfo_p->m_pFrame->m_Data.
+						m_Asnd.m_Payload.
+						m_NmtCommandService.
+						m_le_abNmtCommandData[0]);
+			if (fNodeIdInList != FALSE) {	// own nodeid in list
+				// send event to process command
+				NmtEvent = kEplNmtEventResetConfig;
+			}
+			break;
+		}
+
+	case kEplNmtCmdSwResetEx:
+		{		// check if own nodeid is in EPL node list
+			fNodeIdInList =
+			    EplNmtCnuNodeIdList(&pFrameInfo_p->m_pFrame->m_Data.
+						m_Asnd.m_Payload.
+						m_NmtCommandService.
+						m_le_abNmtCommandData[0]);
+			if (fNodeIdInList != FALSE) {	// own nodeid in list
+				// send event to process command
+				NmtEvent = kEplNmtEventSwReset;
+			}
+			break;
+		}
+
+		//------------------------------------------------------------------------
+		// NMT managing commands
+
+		// TODO: add functions to process managing command (optional)
+
+	case kEplNmtCmdNetHostNameSet:
+		{
+			break;
+		}
+
+	case kEplNmtCmdFlushArpEntry:
+		{
+			break;
+		}
+
+		//------------------------------------------------------------------------
+		// NMT info services
+
+		// TODO: forward event with infos to the application (optional)
+
+	case kEplNmtCmdPublishConfiguredCN:
+		{
+			break;
+		}
+
+	case kEplNmtCmdPublishActiveCN:
+		{
+			break;
+		}
+
+	case kEplNmtCmdPublishPreOperational1:
+		{
+			break;
+		}
+
+	case kEplNmtCmdPublishPreOperational2:
+		{
+			break;
+		}
+
+	case kEplNmtCmdPublishReadyToOperate:
+		{
+			break;
+		}
+
+	case kEplNmtCmdPublishOperational:
+		{
+			break;
+		}
+
+	case kEplNmtCmdPublishStopped:
+		{
+			break;
+		}
+
+	case kEplNmtCmdPublishEmergencyNew:
+		{
+			break;
+		}
+
+	case kEplNmtCmdPublishTime:
+		{
+			break;
+		}
+
+		//-----------------------------------------------------------------------
+		// error from MN
+		// -> requested command not supported by MN
+	case kEplNmtCmdInvalidService:
+		{
+
+			// TODO: errorevent to application
+			break;
+		}
+
+		//------------------------------------------------------------------------
+		// default
+	default:
+		{
+			Ret = kEplNmtUnknownCommand;
+			goto Exit;
+		}
+
+	}			// end of switch(NmtCommand)
+
+	if (NmtEvent != kEplNmtEventNoEvent) {
+		if (EplNmtCnuInstance_g.m_pfnCheckEventCb != NULL) {
+			Ret = EplNmtCnuInstance_g.m_pfnCheckEventCb(NmtEvent);
+			if (Ret == kEplReject) {
+				Ret = kEplSuccessful;
+				goto Exit;
+			} else if (Ret != kEplSuccessful) {
+				goto Exit;
+			}
+		}
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTU)) != 0)
+		Ret = EplNmtuNmtEvent(NmtEvent);
+#endif
+	}
+
+      Exit:
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtCnuGetNmtCommand()
+//
+// Description: returns the NMT-Command from the frame
+//
+//
+//
+// Parameters:      pFrameInfo_p = pointer to the Frame
+//                                 with the NMT-Command
+//
+//
+// Returns:         tEplNmtCommand = NMT-Command
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static tEplNmtCommand EplNmtCnuGetNmtCommand(tEplFrameInfo * pFrameInfo_p)
+{
+	tEplNmtCommand NmtCommand;
+	tEplNmtCommandService *pNmtCommandService;
+
+	pNmtCommandService =
+	    &pFrameInfo_p->m_pFrame->m_Data.m_Asnd.m_Payload.
+	    m_NmtCommandService;
+
+	NmtCommand =
+	    (tEplNmtCommand) AmiGetByteFromLe(&pNmtCommandService->
+					      m_le_bNmtCommandId);
+
+	return NmtCommand;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtCnuNodeIdList()
+//
+// Description: check if the own nodeid is set in EPL Node List
+//
+//
+//
+// Parameters:      pbNmtCommandDate_p = pointer to the data of the NMT Command
+//
+//
+// Returns:         BOOL = TRUE if nodeid is set in EPL Node List
+//                         FALSE if nodeid not set in EPL Node List
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static BOOL EplNmtCnuNodeIdList(BYTE * pbNmtCommandDate_p)
+{
+	BOOL fNodeIdInList;
+	unsigned int uiByteOffset;
+	BYTE bBitOffset;
+	BYTE bNodeListByte;
+
+	// get byte-offset of the own nodeid in NodeIdList
+	// devide though 8
+	uiByteOffset = (unsigned int)(EplNmtCnuInstance_g.m_uiNodeId >> 3);
+	// get bitoffset
+	bBitOffset = (BYTE) EplNmtCnuInstance_g.m_uiNodeId % 8;
+
+	bNodeListByte = AmiGetByteFromLe(&pbNmtCommandDate_p[uiByteOffset]);
+	if ((bNodeListByte & bBitOffset) == 0) {
+		fNodeIdInList = FALSE;
+	} else {
+		fNodeIdInList = TRUE;
+	}
+
+	return fNodeIdInList;
+}
+
+#endif // #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_CN)) != 0)
+
+// EOF
diff --git a/drivers/staging/epl/EplNmtMnu.c b/drivers/staging/epl/EplNmtMnu.c
new file mode 100644
index 00000000000..4ed0b6ce487
--- /dev/null
+++ b/drivers/staging/epl/EplNmtMnu.c
@@ -0,0 +1,2835 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for NMT-MN-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplNmtMnu.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.18 $  $Date: 2008/11/19 09:52:24 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/09 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "user/EplNmtMnu.h"
+#include "user/EplTimeru.h"
+#include "user/EplIdentu.h"
+#include "user/EplStatusu.h"
+#include "user/EplObdu.h"
+#include "user/EplDlluCal.h"
+#include "Benchmark.h"
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) == 0) && (EPL_OBD_USE_KERNEL == FALSE)
+#error "EPL NmtMnu module needs EPL module OBDU or OBDK!"
+#endif
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   D E F I N I T I O N S                          //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+// TracePoint support for realtime-debugging
+#ifdef _DBG_TRACE_POINTS_
+void PUBLIC TgtDbgSignalTracePoint(BYTE bTracePointNumber_p);
+void PUBLIC TgtDbgPostTraceValue(DWORD dwTraceValue_p);
+#define TGT_DBG_SIGNAL_TRACE_POINT(p)   TgtDbgSignalTracePoint(p)
+#define TGT_DBG_POST_TRACE_VALUE(v)     TgtDbgPostTraceValue(v)
+#else
+#define TGT_DBG_SIGNAL_TRACE_POINT(p)
+#define TGT_DBG_POST_TRACE_VALUE(v)
+#endif
+#define EPL_NMTMNU_DBG_POST_TRACE_VALUE(Event_p, uiNodeId_p, wErrorCode_p) \
+    TGT_DBG_POST_TRACE_VALUE((kEplEventSinkNmtMnu << 28) | (Event_p << 24) \
+                             | (uiNodeId_p << 16) | wErrorCode_p)
+
+// defines for flags in node info structure
+#define EPL_NMTMNU_NODE_FLAG_ISOCHRON       0x0001	// CN is being accessed isochronously
+#define EPL_NMTMNU_NODE_FLAG_NOT_SCANNED    0x0002	// CN was not scanned once -> decrement SignalCounter and reset flag
+#define EPL_NMTMNU_NODE_FLAG_HALTED         0x0004	// boot process for this CN is halted
+#define EPL_NMTMNU_NODE_FLAG_NMT_CMD_ISSUED 0x0008	// NMT command was just issued, wrong NMT states will be tolerated
+#define EPL_NMTMNU_NODE_FLAG_COUNT_STATREQ  0x0300	// counter for StatusRequest timer handle
+#define EPL_NMTMNU_NODE_FLAG_COUNT_LONGER   0x0C00	// counter for longer timeouts timer handle
+#define EPL_NMTMNU_NODE_FLAG_INC_STATREQ    0x0100	// increment for StatusRequest timer handle
+#define EPL_NMTMNU_NODE_FLAG_INC_LONGER     0x0400	// increment for longer timeouts timer handle
+		    // These counters will be incremented at every timer start
+		    // and copied to timerarg. When the timer event occures
+		    // both will be compared and if unequal the timer event
+		    // will be discarded, because it is an old one.
+
+// defines for timer arguments to draw a distinction between serveral events
+#define EPL_NMTMNU_TIMERARG_NODE_MASK   0x000000FFL	// mask that contains the node-ID
+#define EPL_NMTMNU_TIMERARG_IDENTREQ    0x00010000L	// timer event is for IdentRequest
+#define EPL_NMTMNU_TIMERARG_STATREQ     0x00020000L	// timer event is for StatusRequest
+#define EPL_NMTMNU_TIMERARG_LONGER      0x00040000L	// timer event is for longer timeouts
+#define EPL_NMTMNU_TIMERARG_STATE_MON   0x00080000L	// timer event for StatusRequest to monitor execution of NMT state changes
+#define EPL_NMTMNU_TIMERARG_COUNT_SR    0x00000300L	// counter for StatusRequest
+#define EPL_NMTMNU_TIMERARG_COUNT_LO    0x00000C00L	// counter for longer timeouts
+		    // The counters must have the same position as in the node flags above.
+
+#define EPL_NMTMNU_SET_FLAGS_TIMERARG_STATREQ(pNodeInfo_p, uiNodeId_p, TimerArg_p) \
+    pNodeInfo_p->m_wFlags = \
+        ((pNodeInfo_p->m_wFlags + EPL_NMTMNU_NODE_FLAG_INC_STATREQ) \
+         & EPL_NMTMNU_NODE_FLAG_COUNT_STATREQ) \
+        | (pNodeInfo_p->m_wFlags & ~EPL_NMTMNU_NODE_FLAG_COUNT_STATREQ); \
+    TimerArg_p.m_ulArg = EPL_NMTMNU_TIMERARG_STATREQ | uiNodeId_p | \
+        (pNodeInfo_p->m_wFlags & EPL_NMTMNU_NODE_FLAG_COUNT_STATREQ); \
+    TimerArg_p.m_EventSink = kEplEventSinkNmtMnu;
+
+#define EPL_NMTMNU_SET_FLAGS_TIMERARG_IDENTREQ(pNodeInfo_p, uiNodeId_p, TimerArg_p) \
+    pNodeInfo_p->m_wFlags = \
+        ((pNodeInfo_p->m_wFlags + EPL_NMTMNU_NODE_FLAG_INC_STATREQ) \
+         & EPL_NMTMNU_NODE_FLAG_COUNT_STATREQ) \
+        | (pNodeInfo_p->m_wFlags & ~EPL_NMTMNU_NODE_FLAG_COUNT_STATREQ); \
+    TimerArg_p.m_ulArg = EPL_NMTMNU_TIMERARG_IDENTREQ | uiNodeId_p | \
+        (pNodeInfo_p->m_wFlags & EPL_NMTMNU_NODE_FLAG_COUNT_STATREQ); \
+    TimerArg_p.m_EventSink = kEplEventSinkNmtMnu;
+
+#define EPL_NMTMNU_SET_FLAGS_TIMERARG_LONGER(pNodeInfo_p, uiNodeId_p, TimerArg_p) \
+    pNodeInfo_p->m_wFlags = \
+        ((pNodeInfo_p->m_wFlags + EPL_NMTMNU_NODE_FLAG_INC_LONGER) \
+         & EPL_NMTMNU_NODE_FLAG_COUNT_LONGER) \
+        | (pNodeInfo_p->m_wFlags & ~EPL_NMTMNU_NODE_FLAG_COUNT_LONGER); \
+    TimerArg_p.m_ulArg = EPL_NMTMNU_TIMERARG_LONGER | uiNodeId_p | \
+        (pNodeInfo_p->m_wFlags & EPL_NMTMNU_NODE_FLAG_COUNT_LONGER); \
+    TimerArg_p.m_EventSink = kEplEventSinkNmtMnu;
+
+#define EPL_NMTMNU_SET_FLAGS_TIMERARG_STATE_MON(pNodeInfo_p, uiNodeId_p, TimerArg_p) \
+    pNodeInfo_p->m_wFlags = \
+        ((pNodeInfo_p->m_wFlags + EPL_NMTMNU_NODE_FLAG_INC_STATREQ) \
+         & EPL_NMTMNU_NODE_FLAG_COUNT_STATREQ) \
+        | (pNodeInfo_p->m_wFlags & ~EPL_NMTMNU_NODE_FLAG_COUNT_STATREQ); \
+    TimerArg_p.m_ulArg = EPL_NMTMNU_TIMERARG_STATE_MON | uiNodeId_p | \
+        (pNodeInfo_p->m_wFlags & EPL_NMTMNU_NODE_FLAG_COUNT_STATREQ); \
+    TimerArg_p.m_EventSink = kEplEventSinkNmtMnu;
+
+// defines for global flags
+#define EPL_NMTMNU_FLAG_HALTED          0x0001	// boot process is halted
+#define EPL_NMTMNU_FLAG_APP_INFORMED    0x0002	// application was informed about possible NMT state change
+
+// return pointer to node info structure for specified node ID
+// d.k. may be replaced by special (hash) function if node ID array is smaller than 254
+#define EPL_NMTMNU_GET_NODEINFO(uiNodeId_p) (&EplNmtMnuInstance_g.m_aNodeInfo[uiNodeId_p - 1])
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+typedef enum {
+	kEplNmtMnuIntNodeEventNoIdentResponse = 0x00,
+	kEplNmtMnuIntNodeEventIdentResponse = 0x01,
+	kEplNmtMnuIntNodeEventBoot = 0x02,
+	kEplNmtMnuIntNodeEventExecReset = 0x03,
+	kEplNmtMnuIntNodeEventConfigured = 0x04,
+	kEplNmtMnuIntNodeEventNoStatusResponse = 0x05,
+	kEplNmtMnuIntNodeEventStatusResponse = 0x06,
+	kEplNmtMnuIntNodeEventHeartbeat = 0x07,
+	kEplNmtMnuIntNodeEventNmtCmdSent = 0x08,
+	kEplNmtMnuIntNodeEventTimerIdentReq = 0x09,
+	kEplNmtMnuIntNodeEventTimerStatReq = 0x0A,
+	kEplNmtMnuIntNodeEventTimerStateMon = 0x0B,
+	kEplNmtMnuIntNodeEventTimerLonger = 0x0C,
+	kEplNmtMnuIntNodeEventError = 0x0D,
+
+} tEplNmtMnuIntNodeEvent;
+
+typedef enum {
+	kEplNmtMnuNodeStateUnknown = 0x00,
+	kEplNmtMnuNodeStateIdentified = 0x01,
+	kEplNmtMnuNodeStateResetConf = 0x02,	// CN reset after configuration update
+	kEplNmtMnuNodeStateConfigured = 0x03,	// BootStep1 completed
+	kEplNmtMnuNodeStateReadyToOp = 0x04,	// BootStep2 completed
+	kEplNmtMnuNodeStateComChecked = 0x05,	// Communication checked successfully
+	kEplNmtMnuNodeStateOperational = 0x06,	// CN is in NMT state OPERATIONAL
+
+} tEplNmtMnuNodeState;
+
+typedef struct {
+	tEplTimerHdl m_TimerHdlStatReq;	// timer to delay StatusRequests and IdentRequests
+	tEplTimerHdl m_TimerHdlLonger;	// 2nd timer for NMT command EnableReadyToOp and CheckCommunication
+	tEplNmtMnuNodeState m_NodeState;	// internal node state (kind of sub state of NMT state)
+	DWORD m_dwNodeCfg;	// subindex from 0x1F81
+	WORD m_wFlags;		// flags: CN is being accessed isochronously
+
+} tEplNmtMnuNodeInfo;
+
+typedef struct {
+	tEplNmtMnuNodeInfo m_aNodeInfo[EPL_NMT_MAX_NODE_ID];
+	tEplTimerHdl m_TimerHdlNmtState;	// timeout for stay in NMT state
+	unsigned int m_uiMandatorySlaveCount;
+	unsigned int m_uiSignalSlaveCount;
+	unsigned long m_ulStatusRequestDelay;	// in [ms] (object 0x1006 * EPL_C_NMT_STATREQ_CYCLE)
+	unsigned long m_ulTimeoutReadyToOp;	// in [ms] (object 0x1F89/5)
+	unsigned long m_ulTimeoutCheckCom;	// in [ms] (object 0x1006 * MultiplexedCycleCount)
+	WORD m_wFlags;		// global flags
+	DWORD m_dwNmtStartup;	// object 0x1F80 NMT_StartUp_U32
+	tEplNmtMnuCbNodeEvent m_pfnCbNodeEvent;
+	tEplNmtMnuCbBootEvent m_pfnCbBootEvent;
+
+} tEplNmtMnuInstance;
+
+//---------------------------------------------------------------------------
+// local vars
+//---------------------------------------------------------------------------
+
+static tEplNmtMnuInstance EplNmtMnuInstance_g;
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+static tEplKernel PUBLIC EplNmtMnuCbNmtRequest(tEplFrameInfo * pFrameInfo_p);
+
+static tEplKernel PUBLIC EplNmtMnuCbIdentResponse(unsigned int uiNodeId_p,
+						  tEplIdentResponse *
+						  pIdentResponse_p);
+
+static tEplKernel PUBLIC EplNmtMnuCbStatusResponse(unsigned int uiNodeId_p,
+						   tEplStatusResponse *
+						   pStatusResponse_p);
+
+static tEplKernel EplNmtMnuCheckNmtState(unsigned int uiNodeId_p,
+					 tEplNmtMnuNodeInfo * pNodeInfo_p,
+					 tEplNmtState NodeNmtState_p,
+					 WORD wErrorCode_p,
+					 tEplNmtState LocalNmtState_p);
+
+static tEplKernel EplNmtMnuStartBootStep1(void);
+
+static tEplKernel EplNmtMnuStartBootStep2(void);
+
+static tEplKernel EplNmtMnuStartCheckCom(void);
+
+static tEplKernel EplNmtMnuNodeBootStep2(unsigned int uiNodeId_p,
+					 tEplNmtMnuNodeInfo * pNodeInfo_p);
+
+static tEplKernel EplNmtMnuNodeCheckCom(unsigned int uiNodeId_p,
+					tEplNmtMnuNodeInfo * pNodeInfo_p);
+
+static tEplKernel EplNmtMnuStartNodes(void);
+
+static tEplKernel EplNmtMnuProcessInternalEvent(unsigned int uiNodeId_p,
+						tEplNmtState NodeNmtState_p,
+						WORD wErrorCode_p,
+						tEplNmtMnuIntNodeEvent
+						NodeEvent_p);
+
+static tEplKernel EplNmtMnuReset(void);
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuInit
+//
+// Description: init first instance of the module
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplNmtMnuInit(tEplNmtMnuCbNodeEvent pfnCbNodeEvent_p,
+			 tEplNmtMnuCbBootEvent pfnCbBootEvent_p)
+{
+	tEplKernel Ret;
+
+	Ret = EplNmtMnuAddInstance(pfnCbNodeEvent_p, pfnCbBootEvent_p);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuAddInstance
+//
+// Description: init other instances of the module
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplNmtMnuAddInstance(tEplNmtMnuCbNodeEvent pfnCbNodeEvent_p,
+				tEplNmtMnuCbBootEvent pfnCbBootEvent_p)
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// reset instance structure
+	EPL_MEMSET(&EplNmtMnuInstance_g, 0, sizeof(EplNmtMnuInstance_g));
+
+	if ((pfnCbNodeEvent_p == NULL) || (pfnCbBootEvent_p == NULL)) {
+		Ret = kEplNmtInvalidParam;
+		goto Exit;
+	}
+	EplNmtMnuInstance_g.m_pfnCbNodeEvent = pfnCbNodeEvent_p;
+	EplNmtMnuInstance_g.m_pfnCbBootEvent = pfnCbBootEvent_p;
+
+	// initialize StatusRequest delay
+	EplNmtMnuInstance_g.m_ulStatusRequestDelay = 5000L;
+
+	// register NmtMnResponse callback function
+	Ret =
+	    EplDlluCalRegAsndService(kEplDllAsndNmtRequest,
+				     EplNmtMnuCbNmtRequest,
+				     kEplDllAsndFilterLocal);
+
+      Exit:
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuDelInstance
+//
+// Description: delete instance
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplNmtMnuDelInstance()
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// deregister NmtMnResponse callback function
+	Ret =
+	    EplDlluCalRegAsndService(kEplDllAsndNmtRequest, NULL,
+				     kEplDllAsndFilterNone);
+
+	Ret = EplNmtMnuReset();
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuSendNmtCommandEx
+//
+// Description: sends the specified NMT command to the specified node.
+//
+// Parameters:  uiNodeId_p              = node ID to which the NMT command will be sent
+//              NmtCommand_p            = NMT command
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplNmtMnuSendNmtCommandEx(unsigned int uiNodeId_p,
+				     tEplNmtCommand NmtCommand_p,
+				     void *pNmtCommandData_p,
+				     unsigned int uiDataSize_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplFrameInfo FrameInfo;
+	BYTE abBuffer[EPL_C_DLL_MINSIZE_NMTCMDEXT];
+	tEplFrame *pFrame = (tEplFrame *) abBuffer;
+	BOOL fSoftDeleteNode = FALSE;
+
+	if ((uiNodeId_p == 0) || (uiNodeId_p > EPL_C_ADR_BROADCAST)) {	// invalid node ID specified
+		Ret = kEplInvalidNodeId;
+		goto Exit;
+	}
+
+	if ((pNmtCommandData_p != NULL)
+	    && (uiDataSize_p >
+		(EPL_C_DLL_MINSIZE_NMTCMDEXT - EPL_C_DLL_MINSIZE_NMTCMD))) {
+		Ret = kEplNmtInvalidParam;
+		goto Exit;
+	}
+	// $$$ d.k. may be check in future versions if the caller wants to perform prohibited state transitions
+	//     the CN should not perform these transitions, but the expected NMT state will be changed and never fullfilled.
+
+	// build frame
+	EPL_MEMSET(pFrame, 0x00, sizeof(abBuffer));
+	AmiSetByteToLe(&pFrame->m_le_bDstNodeId, (BYTE) uiNodeId_p);
+	AmiSetByteToLe(&pFrame->m_Data.m_Asnd.m_le_bServiceId,
+		       (BYTE) kEplDllAsndNmtCommand);
+	AmiSetByteToLe(&pFrame->m_Data.m_Asnd.m_Payload.m_NmtCommandService.
+		       m_le_bNmtCommandId, (BYTE) NmtCommand_p);
+	if ((pNmtCommandData_p != NULL) && (uiDataSize_p > 0)) {	// copy command data to frame
+		EPL_MEMCPY(&pFrame->m_Data.m_Asnd.m_Payload.m_NmtCommandService.
+			   m_le_abNmtCommandData[0], pNmtCommandData_p,
+			   uiDataSize_p);
+	}
+	// build info structure
+	FrameInfo.m_NetTime.m_dwNanoSec = 0;
+	FrameInfo.m_NetTime.m_dwSec = 0;
+	FrameInfo.m_pFrame = pFrame;
+	FrameInfo.m_uiFrameSize = sizeof(abBuffer);
+
+	// send NMT-Request
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLU)) != 0)
+	Ret = EplDlluCalAsyncSend(&FrameInfo,	// pointer to frameinfo
+				  kEplDllAsyncReqPrioNmt);	// priority
+#endif
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	EPL_DBGLVL_NMTMN_TRACE2("NMTCmd(%02X->%02X)\n", NmtCommand_p,
+				uiNodeId_p);
+
+	switch (NmtCommand_p) {
+	case kEplNmtCmdStartNode:
+	case kEplNmtCmdEnterPreOperational2:
+	case kEplNmtCmdEnableReadyToOperate:
+		{
+			// nothing left to do,
+			// because any further processing is done
+			// when the NMT command is actually sent
+			goto Exit;
+		}
+
+	case kEplNmtCmdStopNode:
+		{
+			fSoftDeleteNode = TRUE;
+			break;
+		}
+
+	case kEplNmtCmdResetNode:
+	case kEplNmtCmdResetCommunication:
+	case kEplNmtCmdResetConfiguration:
+	case kEplNmtCmdSwReset:
+		{
+			break;
+		}
+
+	default:
+		goto Exit;
+	}
+
+	// remove CN from isochronous phase;
+	// This must be done here and not when NMT command is actually sent
+	// because it will be too late and may cause unwanted errors
+	if (uiNodeId_p != EPL_C_ADR_BROADCAST) {
+		if (fSoftDeleteNode == FALSE) {	// remove CN immediately from isochronous phase
+			Ret = EplDlluCalDeleteNode(uiNodeId_p);
+		} else {	// remove CN from isochronous phase softly
+			Ret = EplDlluCalSoftDeleteNode(uiNodeId_p);
+		}
+	} else {		// do it for all active CNs
+		for (uiNodeId_p = 1;
+		     uiNodeId_p <= tabentries(EplNmtMnuInstance_g.m_aNodeInfo);
+		     uiNodeId_p++) {
+			if ((EPL_NMTMNU_GET_NODEINFO(uiNodeId_p)->
+			     m_dwNodeCfg & (EPL_NODEASSIGN_NODE_IS_CN |
+					    EPL_NODEASSIGN_NODE_EXISTS)) != 0) {
+				if (fSoftDeleteNode == FALSE) {	// remove CN immediately from isochronous phase
+					Ret = EplDlluCalDeleteNode(uiNodeId_p);
+				} else {	// remove CN from isochronous phase softly
+					Ret =
+					    EplDlluCalSoftDeleteNode
+					    (uiNodeId_p);
+				}
+			}
+		}
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuSendNmtCommand
+//
+// Description: sends the specified NMT command to the specified node.
+//
+// Parameters:  uiNodeId_p              = node ID to which the NMT command will be sent
+//              NmtCommand_p            = NMT command
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplNmtMnuSendNmtCommand(unsigned int uiNodeId_p,
+				   tEplNmtCommand NmtCommand_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	Ret = EplNmtMnuSendNmtCommandEx(uiNodeId_p, NmtCommand_p, NULL, 0);
+
+//Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuTriggerStateChange
+//
+// Description: triggers the specified node command for the specified node.
+//
+// Parameters:  uiNodeId_p              = node ID for which the node command will be executed
+//              NodeCommand_p           = node command
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplNmtMnuTriggerStateChange(unsigned int uiNodeId_p,
+				       tEplNmtNodeCommand NodeCommand_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplNmtMnuIntNodeEvent NodeEvent;
+	tEplObdSize ObdSize;
+	BYTE bNmtState;
+	WORD wErrorCode = EPL_E_NO_ERROR;
+
+	if ((uiNodeId_p == 0) || (uiNodeId_p >= EPL_C_ADR_BROADCAST)) {
+		Ret = kEplInvalidNodeId;
+		goto Exit;
+	}
+
+	switch (NodeCommand_p) {
+	case kEplNmtNodeCommandBoot:
+		{
+			NodeEvent = kEplNmtMnuIntNodeEventBoot;
+			break;
+		}
+
+	case kEplNmtNodeCommandConfOk:
+		{
+			NodeEvent = kEplNmtMnuIntNodeEventConfigured;
+			break;
+		}
+
+	case kEplNmtNodeCommandConfErr:
+		{
+			NodeEvent = kEplNmtMnuIntNodeEventError;
+			wErrorCode = EPL_E_NMT_BPO1_CF_VERIFY;
+			break;
+		}
+
+	case kEplNmtNodeCommandConfReset:
+		{
+			NodeEvent = kEplNmtMnuIntNodeEventExecReset;
+			break;
+		}
+
+	default:
+		{		// invalid node command
+			goto Exit;
+		}
+	}
+
+	// fetch current NMT state
+	ObdSize = 1;
+	Ret = EplObduReadEntry(0x1F8E, uiNodeId_p, &bNmtState, &ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	Ret = EplNmtMnuProcessInternalEvent(uiNodeId_p,
+					    (tEplNmtState) (bNmtState |
+							    EPL_NMT_TYPE_CS),
+					    wErrorCode, NodeEvent);
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuCbNmtStateChange
+//
+// Description: callback function for NMT state changes
+//
+// Parameters:  NmtStateChange_p        = NMT state change event
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplNmtMnuCbNmtStateChange(tEplEventNmtStateChange
+					    NmtStateChange_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	// do work which must be done in that state
+	switch (NmtStateChange_p.m_NewNmtState) {
+		// EPL stack is not running
+/*        case kEplNmtGsOff:
+            break;
+
+        // first init of the hardware
+        case kEplNmtGsInitialising:
+            break;
+
+        // init of the manufacturer-specific profile area and the
+        // standardised device profile area
+        case kEplNmtGsResetApplication:
+        {
+            break;
+        }
+
+        // init of the communication profile area
+        case kEplNmtGsResetCommunication:
+        {
+            break;
+        }
+*/
+		// build the configuration with infos from OD
+	case kEplNmtGsResetConfiguration:
+		{
+			DWORD dwTimeout;
+			tEplObdSize ObdSize;
+
+			// read object 0x1F80 NMT_StartUp_U32
+			ObdSize = 4;
+			Ret =
+			    EplObduReadEntry(0x1F80, 0,
+					     &EplNmtMnuInstance_g.
+					     m_dwNmtStartup, &ObdSize);
+			if (Ret != kEplSuccessful) {
+				break;
+			}
+			// compute StatusReqDelay = object 0x1006 * EPL_C_NMT_STATREQ_CYCLE
+			ObdSize = sizeof(dwTimeout);
+			Ret = EplObduReadEntry(0x1006, 0, &dwTimeout, &ObdSize);
+			if (Ret != kEplSuccessful) {
+				break;
+			}
+			if (dwTimeout != 0L) {
+				EplNmtMnuInstance_g.m_ulStatusRequestDelay =
+				    dwTimeout * EPL_C_NMT_STATREQ_CYCLE / 1000L;
+				if (EplNmtMnuInstance_g.
+				    m_ulStatusRequestDelay == 0L) {
+					EplNmtMnuInstance_g.m_ulStatusRequestDelay = 1L;	// at least 1 ms
+				}
+				// $$$ fetch and use MultiplexedCycleCount from OD
+				EplNmtMnuInstance_g.m_ulTimeoutCheckCom =
+				    dwTimeout * EPL_C_NMT_STATREQ_CYCLE / 1000L;
+				if (EplNmtMnuInstance_g.m_ulTimeoutCheckCom ==
+				    0L) {
+					EplNmtMnuInstance_g.m_ulTimeoutCheckCom = 1L;	// at least 1 ms
+				}
+			}
+			// fetch ReadyToOp Timeout from OD
+			ObdSize = sizeof(dwTimeout);
+			Ret = EplObduReadEntry(0x1F89, 5, &dwTimeout, &ObdSize);
+			if (Ret != kEplSuccessful) {
+				break;
+			}
+			if (dwTimeout != 0L) {
+				// convert [us] to [ms]
+				dwTimeout /= 1000L;
+				if (dwTimeout == 0L) {
+					dwTimeout = 1L;	// at least 1 ms
+				}
+				EplNmtMnuInstance_g.m_ulTimeoutReadyToOp =
+				    dwTimeout;
+			} else {
+				EplNmtMnuInstance_g.m_ulTimeoutReadyToOp = 0L;
+			}
+			break;
+		}
+/*
+        //-----------------------------------------------------------
+        // CN part of the state machine
+
+        // node liste for EPL-Frames and check timeout
+        case kEplNmtCsNotActive:
+        {
+            break;
+        }
+
+        // node process only async frames
+        case kEplNmtCsPreOperational1:
+        {
+            break;
+        }
+
+        // node process isochronus and asynchronus frames
+        case kEplNmtCsPreOperational2:
+        {
+            break;
+        }
+
+        // node should be configured und application is ready
+        case kEplNmtCsReadyToOperate:
+        {
+            break;
+        }
+
+        // normal work state
+        case kEplNmtCsOperational:
+        {
+            break;
+        }
+
+        // node stopped by MN
+        // -> only process asynchronus frames
+        case kEplNmtCsStopped:
+        {
+            break;
+        }
+
+        // no EPL cycle
+        // -> normal ethernet communication
+        case kEplNmtCsBasicEthernet:
+        {
+            break;
+        }
+*/
+		//-----------------------------------------------------------
+		// MN part of the state machine
+
+		// node listens for EPL-Frames and check timeout
+	case kEplNmtMsNotActive:
+		{
+			break;
+		}
+
+		// node processes only async frames
+	case kEplNmtMsPreOperational1:
+		{
+			DWORD dwTimeout;
+			tEplTimerArg TimerArg;
+			tEplObdSize ObdSize;
+			tEplEvent Event;
+
+			// clear global flags, e.g. reenable boot process
+			EplNmtMnuInstance_g.m_wFlags = 0;
+
+			// reset IdentResponses and running IdentRequests and StatusRequests
+			Ret = EplIdentuReset();
+			Ret = EplStatusuReset();
+
+			// reset timers
+			Ret = EplNmtMnuReset();
+
+			// 2008/11/18 d.k. reset internal node info is not necessary,
+			//                 because timer flags are important and other
+			//                 things are reset by EplNmtMnuStartBootStep1().
+/*
+            EPL_MEMSET(EplNmtMnuInstance_g.m_aNodeInfo,
+                       0,
+                       sizeof (EplNmtMnuInstance_g.m_aNodeInfo));
+*/
+
+			// inform DLL about NMT state change,
+			// so that it can clear the asynchonous queues and start the reduced cycle
+			Event.m_EventSink = kEplEventSinkDllk;
+			Event.m_EventType = kEplEventTypeDllkStartReducedCycle;
+			EPL_MEMSET(&Event.m_NetTime, 0x00,
+				   sizeof(Event.m_NetTime));
+			Event.m_pArg = NULL;
+			Event.m_uiSize = 0;
+			Ret = EplEventuPost(&Event);
+			if (Ret != kEplSuccessful) {
+				break;
+			}
+			// reset all nodes
+			// d.k.: skip this step if was just done before, e.g. because of a ResetNode command from a diagnostic node
+			if (NmtStateChange_p.m_NmtEvent ==
+			    kEplNmtEventTimerMsPreOp1) {
+				BENCHMARK_MOD_07_TOGGLE(9);
+
+				EPL_NMTMNU_DBG_POST_TRACE_VALUE(0,
+								EPL_C_ADR_BROADCAST,
+								kEplNmtCmdResetNode);
+
+				Ret =
+				    EplNmtMnuSendNmtCommand(EPL_C_ADR_BROADCAST,
+							    kEplNmtCmdResetNode);
+				if (Ret != kEplSuccessful) {
+					break;
+				}
+			}
+			// start network scan
+			Ret = EplNmtMnuStartBootStep1();
+
+			// start timer for 0x1F89/2 MNTimeoutPreOp1_U32
+			ObdSize = sizeof(dwTimeout);
+			Ret = EplObduReadEntry(0x1F89, 2, &dwTimeout, &ObdSize);
+			if (Ret != kEplSuccessful) {
+				break;
+			}
+			if (dwTimeout != 0L) {
+				dwTimeout /= 1000L;
+				if (dwTimeout == 0L) {
+					dwTimeout = 1L;	// at least 1 ms
+				}
+				TimerArg.m_EventSink = kEplEventSinkNmtMnu;
+				TimerArg.m_ulArg = 0;
+				Ret =
+				    EplTimeruModifyTimerMs(&EplNmtMnuInstance_g.
+							   m_TimerHdlNmtState,
+							   dwTimeout, TimerArg);
+			}
+			break;
+		}
+
+		// node processes isochronous and asynchronous frames
+	case kEplNmtMsPreOperational2:
+		{
+			// add identified CNs to isochronous phase
+			// send EnableReadyToOp to all identified CNs
+			Ret = EplNmtMnuStartBootStep2();
+
+			// wait for NMT state change of CNs
+			break;
+		}
+
+		// node should be configured und application is ready
+	case kEplNmtMsReadyToOperate:
+		{
+			// check if PRes of CNs are OK
+			// d.k. that means wait CycleLength * MultiplexCycleCount (i.e. start timer)
+			//      because Dllk checks PRes of CNs automatically in ReadyToOp
+			Ret = EplNmtMnuStartCheckCom();
+			break;
+		}
+
+		// normal work state
+	case kEplNmtMsOperational:
+		{
+			// send StartNode to CNs
+			// wait for NMT state change of CNs
+			Ret = EplNmtMnuStartNodes();
+			break;
+		}
+
+		// no EPL cycle
+		// -> normal ethernet communication
+	case kEplNmtMsBasicEthernet:
+		{
+			break;
+		}
+
+	default:
+		{
+//            TRACE0("EplNmtMnuCbNmtStateChange(): unhandled NMT state\n");
+		}
+	}
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuCbCheckEvent
+//
+// Description: callback funktion for NMT events before they are actually executed.
+//              The EPL API layer must forward NMT events from NmtCnu module.
+//              This module will reject some NMT commands while MN.
+//
+// Parameters:  NmtEvent_p              = outstanding NMT event for approval
+//
+// Returns:     tEplKernel              = error code
+//                      kEplReject      = reject the NMT event
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplNmtMnuCbCheckEvent(tEplNmtEvent NmtEvent_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtuProcessEvent
+//
+// Description: processes events from event queue
+//
+// Parameters:  pEvent_p        = pointer to event
+//
+// Returns:     tEplKernel      = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtMnuProcessEvent(tEplEvent * pEvent_p)
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// process event
+	switch (pEvent_p->m_EventType) {
+		// timer event
+	case kEplEventTypeTimer:
+		{
+			tEplTimerEventArg *pTimerEventArg =
+			    (tEplTimerEventArg *) pEvent_p->m_pArg;
+			unsigned int uiNodeId;
+
+			uiNodeId =
+			    (unsigned int)(pTimerEventArg->
+					   m_ulArg &
+					   EPL_NMTMNU_TIMERARG_NODE_MASK);
+			if (uiNodeId != 0) {
+				tEplObdSize ObdSize;
+				BYTE bNmtState;
+				tEplNmtMnuNodeInfo *pNodeInfo;
+
+				pNodeInfo = EPL_NMTMNU_GET_NODEINFO(uiNodeId);
+
+				ObdSize = 1;
+				Ret =
+				    EplObduReadEntry(0x1F8E, uiNodeId,
+						     &bNmtState, &ObdSize);
+				if (Ret != kEplSuccessful) {
+					break;
+				}
+
+				if ((pTimerEventArg->
+				     m_ulArg & EPL_NMTMNU_TIMERARG_IDENTREQ) !=
+				    0L) {
+					if ((pNodeInfo->
+					     m_wFlags &
+					     EPL_NMTMNU_NODE_FLAG_COUNT_STATREQ)
+					    != (pTimerEventArg->m_ulArg & EPL_NMTMNU_TIMERARG_COUNT_SR)) {	// this is an old (already deleted or modified) timer
+						// but not the current timer
+						// so discard it
+						EPL_NMTMNU_DBG_POST_TRACE_VALUE
+						    (kEplNmtMnuIntNodeEventTimerIdentReq,
+						     uiNodeId,
+						     ((pNodeInfo->
+						       m_NodeState << 8)
+						      | 0xFF));
+
+						break;
+					}
+/*
+                    EPL_NMTMNU_DBG_POST_TRACE_VALUE(kEplNmtMnuIntNodeEventTimerIdentReq,
+                                                    uiNodeId,
+                                                    ((pNodeInfo->m_NodeState << 8)
+                                                     | 0x80
+                                                     | ((pNodeInfo->m_wFlags & EPL_NMTMNU_NODE_FLAG_COUNT_STATREQ) >> 6)
+                                                     | ((pTimerEventArg->m_ulArg & EPL_NMTMNU_TIMERARG_COUNT_SR) >> 8)));
+*/
+					Ret =
+					    EplNmtMnuProcessInternalEvent
+					    (uiNodeId,
+					     (tEplNmtState) (bNmtState |
+							     EPL_NMT_TYPE_CS),
+					     EPL_E_NO_ERROR,
+					     kEplNmtMnuIntNodeEventTimerIdentReq);
+				}
+
+				else if ((pTimerEventArg->
+					  m_ulArg & EPL_NMTMNU_TIMERARG_STATREQ)
+					 != 0L) {
+					if ((pNodeInfo->
+					     m_wFlags &
+					     EPL_NMTMNU_NODE_FLAG_COUNT_STATREQ)
+					    != (pTimerEventArg->m_ulArg & EPL_NMTMNU_TIMERARG_COUNT_SR)) {	// this is an old (already deleted or modified) timer
+						// but not the current timer
+						// so discard it
+						EPL_NMTMNU_DBG_POST_TRACE_VALUE
+						    (kEplNmtMnuIntNodeEventTimerStatReq,
+						     uiNodeId,
+						     ((pNodeInfo->
+						       m_NodeState << 8)
+						      | 0xFF));
+
+						break;
+					}
+/*
+                    EPL_NMTMNU_DBG_POST_TRACE_VALUE(kEplNmtMnuIntNodeEventTimerStatReq,
+                                                    uiNodeId,
+                                                    ((pNodeInfo->m_NodeState << 8)
+                                                     | 0x80
+                                                     | ((pNodeInfo->m_wFlags & EPL_NMTMNU_NODE_FLAG_COUNT_STATREQ) >> 6)
+                                                     | ((pTimerEventArg->m_ulArg & EPL_NMTMNU_TIMERARG_COUNT_SR) >> 8)));
+*/
+					Ret =
+					    EplNmtMnuProcessInternalEvent
+					    (uiNodeId,
+					     (tEplNmtState) (bNmtState |
+							     EPL_NMT_TYPE_CS),
+					     EPL_E_NO_ERROR,
+					     kEplNmtMnuIntNodeEventTimerStatReq);
+				}
+
+				else if ((pTimerEventArg->
+					  m_ulArg &
+					  EPL_NMTMNU_TIMERARG_STATE_MON) !=
+					 0L) {
+					if ((pNodeInfo->
+					     m_wFlags &
+					     EPL_NMTMNU_NODE_FLAG_COUNT_STATREQ)
+					    != (pTimerEventArg->m_ulArg & EPL_NMTMNU_TIMERARG_COUNT_SR)) {	// this is an old (already deleted or modified) timer
+						// but not the current timer
+						// so discard it
+						EPL_NMTMNU_DBG_POST_TRACE_VALUE
+						    (kEplNmtMnuIntNodeEventTimerStateMon,
+						     uiNodeId,
+						     ((pNodeInfo->
+						       m_NodeState << 8)
+						      | 0xFF));
+
+						break;
+					}
+/*
+                    EPL_NMTMNU_DBG_POST_TRACE_VALUE(kEplNmtMnuIntNodeEventTimerStatReq,
+                                                    uiNodeId,
+                                                    ((pNodeInfo->m_NodeState << 8)
+                                                     | 0x80
+                                                     | ((pNodeInfo->m_wFlags & EPL_NMTMNU_NODE_FLAG_COUNT_STATREQ) >> 6)
+                                                     | ((pTimerEventArg->m_ulArg & EPL_NMTMNU_TIMERARG_COUNT_SR) >> 8)));
+*/
+					Ret =
+					    EplNmtMnuProcessInternalEvent
+					    (uiNodeId,
+					     (tEplNmtState) (bNmtState |
+							     EPL_NMT_TYPE_CS),
+					     EPL_E_NO_ERROR,
+					     kEplNmtMnuIntNodeEventTimerStateMon);
+				}
+
+				else if ((pTimerEventArg->
+					  m_ulArg & EPL_NMTMNU_TIMERARG_LONGER)
+					 != 0L) {
+					if ((pNodeInfo->
+					     m_wFlags &
+					     EPL_NMTMNU_NODE_FLAG_COUNT_LONGER)
+					    != (pTimerEventArg->m_ulArg & EPL_NMTMNU_TIMERARG_COUNT_LO)) {	// this is an old (already deleted or modified) timer
+						// but not the current timer
+						// so discard it
+						EPL_NMTMNU_DBG_POST_TRACE_VALUE
+						    (kEplNmtMnuIntNodeEventTimerLonger,
+						     uiNodeId,
+						     ((pNodeInfo->
+						       m_NodeState << 8)
+						      | 0xFF));
+
+						break;
+					}
+/*
+                    EPL_NMTMNU_DBG_POST_TRACE_VALUE(kEplNmtMnuIntNodeEventTimerLonger,
+                                                    uiNodeId,
+                                                    ((pNodeInfo->m_NodeState << 8)
+                                                     | 0x80
+                                                     | ((pNodeInfo->m_wFlags & EPL_NMTMNU_NODE_FLAG_COUNT_LONGER) >> 6)
+                                                     | ((pTimerEventArg->m_ulArg & EPL_NMTMNU_TIMERARG_COUNT_LO) >> 8)));
+*/
+					Ret =
+					    EplNmtMnuProcessInternalEvent
+					    (uiNodeId,
+					     (tEplNmtState) (bNmtState |
+							     EPL_NMT_TYPE_CS),
+					     EPL_E_NO_ERROR,
+					     kEplNmtMnuIntNodeEventTimerLonger);
+				}
+
+			} else {	// global timer event
+			}
+			break;
+		}
+
+	case kEplEventTypeHeartbeat:
+		{
+			tEplHeartbeatEvent *pHeartbeatEvent =
+			    (tEplHeartbeatEvent *) pEvent_p->m_pArg;
+
+			Ret =
+			    EplNmtMnuProcessInternalEvent(pHeartbeatEvent->
+							  m_uiNodeId,
+							  pHeartbeatEvent->
+							  m_NmtState,
+							  pHeartbeatEvent->
+							  m_wErrorCode,
+							  kEplNmtMnuIntNodeEventHeartbeat);
+			break;
+		}
+
+	case kEplEventTypeNmtMnuNmtCmdSent:
+		{
+			tEplFrame *pFrame = (tEplFrame *) pEvent_p->m_pArg;
+			unsigned int uiNodeId;
+			tEplNmtCommand NmtCommand;
+			BYTE bNmtState;
+
+			uiNodeId = AmiGetByteFromLe(&pFrame->m_le_bDstNodeId);
+			NmtCommand =
+			    (tEplNmtCommand) AmiGetByteFromLe(&pFrame->m_Data.
+							      m_Asnd.m_Payload.
+							      m_NmtCommandService.
+							      m_le_bNmtCommandId);
+
+			switch (NmtCommand) {
+			case kEplNmtCmdStartNode:
+				bNmtState =
+				    (BYTE) (kEplNmtCsOperational & 0xFF);
+				break;
+
+			case kEplNmtCmdStopNode:
+				bNmtState = (BYTE) (kEplNmtCsStopped & 0xFF);
+				break;
+
+			case kEplNmtCmdEnterPreOperational2:
+				bNmtState =
+				    (BYTE) (kEplNmtCsPreOperational2 & 0xFF);
+				break;
+
+			case kEplNmtCmdEnableReadyToOperate:
+				// d.k. do not change expected node state, because of DS 1.0.0 7.3.1.2.1 Plain NMT State Command
+				//      and because node may not change NMT state within EPL_C_NMT_STATE_TOLERANCE
+				bNmtState =
+				    (BYTE) (kEplNmtCsPreOperational2 & 0xFF);
+				break;
+
+			case kEplNmtCmdResetNode:
+			case kEplNmtCmdResetCommunication:
+			case kEplNmtCmdResetConfiguration:
+			case kEplNmtCmdSwReset:
+				bNmtState = (BYTE) (kEplNmtCsNotActive & 0xFF);
+				// EplNmtMnuProcessInternalEvent() sets internal node state to kEplNmtMnuNodeStateUnknown
+				// after next unresponded IdentRequest/StatusRequest
+				break;
+
+			default:
+				goto Exit;
+			}
+
+			// process as internal event which update expected NMT state in OD
+			if (uiNodeId != EPL_C_ADR_BROADCAST) {
+				Ret = EplNmtMnuProcessInternalEvent(uiNodeId,
+								    (tEplNmtState)
+								    (bNmtState |
+								     EPL_NMT_TYPE_CS),
+								    0,
+								    kEplNmtMnuIntNodeEventNmtCmdSent);
+
+			} else {	// process internal event for all active nodes (except myself)
+
+				for (uiNodeId = 1;
+				     uiNodeId <=
+				     tabentries(EplNmtMnuInstance_g.
+						m_aNodeInfo); uiNodeId++) {
+					if ((EPL_NMTMNU_GET_NODEINFO(uiNodeId)->
+					     m_dwNodeCfg &
+					     (EPL_NODEASSIGN_NODE_IS_CN |
+					      EPL_NODEASSIGN_NODE_EXISTS)) !=
+					    0) {
+						Ret =
+						    EplNmtMnuProcessInternalEvent
+						    (uiNodeId,
+						     (tEplNmtState) (bNmtState |
+								     EPL_NMT_TYPE_CS),
+						     0,
+						     kEplNmtMnuIntNodeEventNmtCmdSent);
+
+						if (Ret != kEplSuccessful) {
+							goto Exit;
+						}
+					}
+				}
+			}
+
+			break;
+		}
+
+	default:
+		{
+			Ret = kEplNmtInvalidEvent;
+		}
+
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuGetRunningTimerStatReq
+//
+// Description: returns a bit field with running StatReq timers
+//              just for debugging purposes
+//
+// Parameters:  (none)
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplNmtMnuGetDiagnosticInfo(unsigned int
+					     *puiMandatorySlaveCount_p,
+					     unsigned int
+					     *puiSignalSlaveCount_p,
+					     WORD * pwFlags_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	if ((puiMandatorySlaveCount_p == NULL)
+	    || (puiSignalSlaveCount_p == NULL)
+	    || (pwFlags_p == NULL)) {
+		Ret = kEplNmtInvalidParam;
+		goto Exit;
+	}
+
+	*puiMandatorySlaveCount_p = EplNmtMnuInstance_g.m_uiMandatorySlaveCount;
+	*puiSignalSlaveCount_p = EplNmtMnuInstance_g.m_uiSignalSlaveCount;
+	*pwFlags_p = EplNmtMnuInstance_g.m_wFlags;
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuGetRunningTimerStatReq
+//
+// Description: returns a bit field with running StatReq timers
+//              just for debugging purposes
+//
+// Parameters:  (none)
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//
+//---------------------------------------------------------------------------
+/*
+DWORD EplNmtMnuGetRunningTimerStatReq(void)
+{
+tEplKernel      Ret = kEplSuccessful;
+unsigned int    uiIndex;
+tEplNmtMnuNodeInfo* pNodeInfo;
+
+    pNodeInfo = EplNmtMnuInstance_g.m_aNodeInfo;
+    for (uiIndex = 1; uiIndex <= tabentries(EplNmtMnuInstance_g.m_aNodeInfo); uiIndex++, pNodeInfo++)
+    {
+        if (pNodeInfo->m_NodeState == kEplNmtMnuNodeStateConfigured)
+        {
+            // reset flag "scanned once"
+            pNodeInfo->m_wFlags &= ~EPL_NMTMNU_NODE_FLAG_SCANNED;
+
+            Ret = EplNmtMnuNodeBootStep2(uiIndex, pNodeInfo);
+            if (Ret != kEplSuccessful)
+            {
+                goto Exit;
+            }
+            EplNmtMnuInstance_g.m_uiSignalSlaveCount++;
+            // signal slave counter shall be decremented if StatusRequest was sent once to a CN
+            // mandatory slave counter shall be decremented if mandatory CN is ReadyToOp
+        }
+    }
+
+Exit:
+    return Ret;
+}
+*/
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuCbNmtRequest
+//
+// Description: callback funktion for NmtRequest
+//
+// Parameters:  pFrameInfo_p            = Frame with the NmtRequest
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel PUBLIC EplNmtMnuCbNmtRequest(tEplFrameInfo * pFrameInfo_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	// $$$ perform NMTRequest
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuCbIdentResponse
+//
+// Description: callback funktion for IdentResponse
+//
+// Parameters:  uiNodeId_p              = node ID for which IdentReponse was received
+//              pIdentResponse_p        = pointer to IdentResponse
+//                                        is NULL if node did not answer
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel PUBLIC EplNmtMnuCbIdentResponse(unsigned int uiNodeId_p,
+						  tEplIdentResponse *
+						  pIdentResponse_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	if (pIdentResponse_p == NULL) {	// node did not answer
+		Ret = EplNmtMnuProcessInternalEvent(uiNodeId_p, kEplNmtCsNotActive, EPL_E_NMT_NO_IDENT_RES,	// was EPL_E_NO_ERROR
+						    kEplNmtMnuIntNodeEventNoIdentResponse);
+	} else {		// node answered IdentRequest
+		tEplObdSize ObdSize;
+		DWORD dwDevType;
+		WORD wErrorCode = EPL_E_NO_ERROR;
+		tEplNmtState NmtState =
+		    (tEplNmtState) (AmiGetByteFromLe
+				    (&pIdentResponse_p->
+				     m_le_bNmtStatus) | EPL_NMT_TYPE_CS);
+
+		// check IdentResponse $$$ move to ProcessIntern, because this function may be called also if CN
+
+		// check DeviceType (0x1F84)
+		ObdSize = 4;
+		Ret =
+		    EplObduReadEntry(0x1F84, uiNodeId_p, &dwDevType, &ObdSize);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+		if (dwDevType != 0L) {	// actually compare it with DeviceType from IdentResponse
+			if (AmiGetDwordFromLe(&pIdentResponse_p->m_le_dwDeviceType) != dwDevType) {	// wrong DeviceType
+				NmtState = kEplNmtCsNotActive;
+				wErrorCode = EPL_E_NMT_BPO1_DEVICE_TYPE;
+			}
+		}
+
+		Ret = EplNmtMnuProcessInternalEvent(uiNodeId_p,
+						    NmtState,
+						    wErrorCode,
+						    kEplNmtMnuIntNodeEventIdentResponse);
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuCbStatusResponse
+//
+// Description: callback funktion for StatusResponse
+//
+// Parameters:  uiNodeId_p              = node ID for which IdentReponse was received
+//              pIdentResponse_p        = pointer to IdentResponse
+//                                        is NULL if node did not answer
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel PUBLIC EplNmtMnuCbStatusResponse(unsigned int uiNodeId_p,
+						   tEplStatusResponse *
+						   pStatusResponse_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	if (pStatusResponse_p == NULL) {	// node did not answer
+		Ret = EplNmtMnuProcessInternalEvent(uiNodeId_p, kEplNmtCsNotActive, EPL_E_NMT_NO_STATUS_RES,	// was EPL_E_NO_ERROR
+						    kEplNmtMnuIntNodeEventNoStatusResponse);
+	} else {		// node answered StatusRequest
+		Ret = EplNmtMnuProcessInternalEvent(uiNodeId_p,
+						    (tEplNmtState)
+						    (AmiGetByteFromLe
+						     (&pStatusResponse_p->
+						      m_le_bNmtStatus) |
+						     EPL_NMT_TYPE_CS),
+						    EPL_E_NO_ERROR,
+						    kEplNmtMnuIntNodeEventStatusResponse);
+	}
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuStartBootStep1
+//
+// Description: starts BootStep1
+//
+// Parameters:  (none)
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplNmtMnuStartBootStep1(void)
+{
+	tEplKernel Ret = kEplSuccessful;
+	unsigned int uiSubIndex;
+	unsigned int uiLocalNodeId;
+	DWORD dwNodeCfg;
+	tEplObdSize ObdSize;
+
+	// $$$ d.k.: save current time for 0x1F89/2 MNTimeoutPreOp1_U32
+
+	// start network scan
+	EplNmtMnuInstance_g.m_uiMandatorySlaveCount = 0;
+	EplNmtMnuInstance_g.m_uiSignalSlaveCount = 0;
+	// check 0x1F81
+	uiLocalNodeId = EplObduGetNodeId();
+	for (uiSubIndex = 1; uiSubIndex <= 254; uiSubIndex++) {
+		ObdSize = 4;
+		Ret =
+		    EplObduReadEntry(0x1F81, uiSubIndex, &dwNodeCfg, &ObdSize);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+		if (uiSubIndex != uiLocalNodeId) {
+			// reset flags "not scanned" and "isochronous"
+			EPL_NMTMNU_GET_NODEINFO(uiSubIndex)->m_wFlags &=
+			    ~(EPL_NMTMNU_NODE_FLAG_ISOCHRON |
+			      EPL_NMTMNU_NODE_FLAG_NOT_SCANNED);
+
+			if (uiSubIndex == EPL_C_ADR_DIAG_DEF_NODE_ID) {	// diagnostic node must be scanned by MN in any case
+				dwNodeCfg |=
+				    (EPL_NODEASSIGN_NODE_IS_CN |
+				     EPL_NODEASSIGN_NODE_EXISTS);
+				// and it must be isochronously accessed
+				dwNodeCfg &= ~EPL_NODEASSIGN_ASYNCONLY_NODE;
+			}
+			// save node config in local node info structure
+			EPL_NMTMNU_GET_NODEINFO(uiSubIndex)->m_dwNodeCfg =
+			    dwNodeCfg;
+			EPL_NMTMNU_GET_NODEINFO(uiSubIndex)->m_NodeState =
+			    kEplNmtMnuNodeStateUnknown;
+
+			if ((dwNodeCfg & (EPL_NODEASSIGN_NODE_IS_CN | EPL_NODEASSIGN_NODE_EXISTS)) != 0) {	// node is configured as CN
+				// identify the node
+				Ret =
+				    EplIdentuRequestIdentResponse(uiSubIndex,
+								  EplNmtMnuCbIdentResponse);
+				if (Ret != kEplSuccessful) {
+					goto Exit;
+				}
+				// set flag "not scanned"
+				EPL_NMTMNU_GET_NODEINFO(uiSubIndex)->m_wFlags |=
+				    EPL_NMTMNU_NODE_FLAG_NOT_SCANNED;
+				EplNmtMnuInstance_g.m_uiSignalSlaveCount++;
+				// signal slave counter shall be decremented if IdentRequest was sent once to a CN
+
+				if ((dwNodeCfg & EPL_NODEASSIGN_MANDATORY_CN) != 0) {	// node is a mandatory CN
+					EplNmtMnuInstance_g.
+					    m_uiMandatorySlaveCount++;
+					// mandatory slave counter shall be decremented if mandatory CN was configured successfully
+				}
+			}
+		} else {	// subindex of MN
+			if ((dwNodeCfg & (EPL_NODEASSIGN_MN_PRES | EPL_NODEASSIGN_NODE_EXISTS)) != 0) {	// MN shall send PRes
+				tEplDllNodeInfo DllNodeInfo;
+
+				EPL_MEMSET(&DllNodeInfo, 0,
+					   sizeof(DllNodeInfo));
+				DllNodeInfo.m_uiNodeId = uiLocalNodeId;
+
+				Ret = EplDlluCalAddNode(&DllNodeInfo);
+			}
+		}
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuStartBootStep2
+//
+// Description: starts BootStep2.
+//              That means add nodes to isochronous phase and send
+//              NMT EnableReadyToOp.
+//
+// Parameters:  (none)
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplNmtMnuStartBootStep2(void)
+{
+	tEplKernel Ret = kEplSuccessful;
+	unsigned int uiIndex;
+	tEplNmtMnuNodeInfo *pNodeInfo;
+
+	if ((EplNmtMnuInstance_g.m_wFlags & EPL_NMTMNU_FLAG_HALTED) == 0) {	// boot process is not halted
+		// add nodes to isochronous phase and send NMT EnableReadyToOp
+		EplNmtMnuInstance_g.m_uiMandatorySlaveCount = 0;
+		EplNmtMnuInstance_g.m_uiSignalSlaveCount = 0;
+		// reset flag that application was informed about possible state change
+		EplNmtMnuInstance_g.m_wFlags &= ~EPL_NMTMNU_FLAG_APP_INFORMED;
+
+		pNodeInfo = EplNmtMnuInstance_g.m_aNodeInfo;
+		for (uiIndex = 1;
+		     uiIndex <= tabentries(EplNmtMnuInstance_g.m_aNodeInfo);
+		     uiIndex++, pNodeInfo++) {
+			if (pNodeInfo->m_NodeState ==
+			    kEplNmtMnuNodeStateConfigured) {
+				Ret =
+				    EplNmtMnuNodeBootStep2(uiIndex, pNodeInfo);
+				if (Ret != kEplSuccessful) {
+					goto Exit;
+				}
+				// set flag "not scanned"
+				pNodeInfo->m_wFlags |=
+				    EPL_NMTMNU_NODE_FLAG_NOT_SCANNED;
+
+				EplNmtMnuInstance_g.m_uiSignalSlaveCount++;
+				// signal slave counter shall be decremented if StatusRequest was sent once to a CN
+
+				if ((pNodeInfo->m_dwNodeCfg & EPL_NODEASSIGN_MANDATORY_CN) != 0) {	// node is a mandatory CN
+					EplNmtMnuInstance_g.
+					    m_uiMandatorySlaveCount++;
+				}
+				// mandatory slave counter shall be decremented if mandatory CN is ReadyToOp
+			}
+		}
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuNodeBootStep2
+//
+// Description: starts BootStep2 for the specified node.
+//              This means the CN is added to isochronous phase if not
+//              async-only and it gets the NMT command EnableReadyToOp.
+//              The CN must be in node state Configured, when it enters
+//              BootStep2. When BootStep2 finishes, the CN is in node state
+//              ReadyToOp.
+//              If TimeoutReadyToOp in object 0x1F89/5 is configured,
+//              TimerHdlLonger will be started with this timeout.
+//
+// Parameters:  uiNodeId_p              = node ID
+//              pNodeInfo_p             = pointer to internal node info structure
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplNmtMnuNodeBootStep2(unsigned int uiNodeId_p,
+					 tEplNmtMnuNodeInfo * pNodeInfo_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplDllNodeInfo DllNodeInfo;
+	DWORD dwNodeCfg;
+	tEplObdSize ObdSize;
+	tEplTimerArg TimerArg;
+
+	dwNodeCfg = pNodeInfo_p->m_dwNodeCfg;
+	if ((dwNodeCfg & EPL_NODEASSIGN_ASYNCONLY_NODE) == 0) {	// add node to isochronous phase
+		DllNodeInfo.m_uiNodeId = uiNodeId_p;
+		ObdSize = 4;
+		Ret =
+		    EplObduReadEntry(0x1F92, uiNodeId_p,
+				     &DllNodeInfo.m_dwPresTimeout, &ObdSize);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+
+		ObdSize = 2;
+		Ret =
+		    EplObduReadEntry(0x1F8B, uiNodeId_p,
+				     &DllNodeInfo.m_wPreqPayloadLimit,
+				     &ObdSize);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+
+		ObdSize = 2;
+		Ret =
+		    EplObduReadEntry(0x1F8D, uiNodeId_p,
+				     &DllNodeInfo.m_wPresPayloadLimit,
+				     &ObdSize);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+
+		pNodeInfo_p->m_wFlags |= EPL_NMTMNU_NODE_FLAG_ISOCHRON;
+
+		Ret = EplDlluCalAddNode(&DllNodeInfo);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+
+	}
+
+	EPL_NMTMNU_DBG_POST_TRACE_VALUE(0,
+					uiNodeId_p,
+					kEplNmtCmdEnableReadyToOperate);
+
+	Ret =
+	    EplNmtMnuSendNmtCommand(uiNodeId_p, kEplNmtCmdEnableReadyToOperate);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	if (EplNmtMnuInstance_g.m_ulTimeoutReadyToOp != 0L) {	// start timer
+		// when the timer expires the CN must be ReadyToOp
+		EPL_NMTMNU_SET_FLAGS_TIMERARG_LONGER(pNodeInfo_p, uiNodeId_p,
+						     TimerArg);
+//        TimerArg.m_EventSink = kEplEventSinkNmtMnu;
+//        TimerArg.m_ulArg = EPL_NMTMNU_TIMERARG_LONGER | uiNodeId_p;
+		Ret =
+		    EplTimeruModifyTimerMs(&pNodeInfo_p->m_TimerHdlLonger,
+					   EplNmtMnuInstance_g.
+					   m_ulTimeoutReadyToOp, TimerArg);
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuStartCheckCom
+//
+// Description: starts CheckCommunication
+//
+// Parameters:  (none)
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplNmtMnuStartCheckCom(void)
+{
+	tEplKernel Ret = kEplSuccessful;
+	unsigned int uiIndex;
+	tEplNmtMnuNodeInfo *pNodeInfo;
+
+	if ((EplNmtMnuInstance_g.m_wFlags & EPL_NMTMNU_FLAG_HALTED) == 0) {	// boot process is not halted
+		// wait some time and check that no communication error occurs
+		EplNmtMnuInstance_g.m_uiMandatorySlaveCount = 0;
+		EplNmtMnuInstance_g.m_uiSignalSlaveCount = 0;
+		// reset flag that application was informed about possible state change
+		EplNmtMnuInstance_g.m_wFlags &= ~EPL_NMTMNU_FLAG_APP_INFORMED;
+
+		pNodeInfo = EplNmtMnuInstance_g.m_aNodeInfo;
+		for (uiIndex = 1;
+		     uiIndex <= tabentries(EplNmtMnuInstance_g.m_aNodeInfo);
+		     uiIndex++, pNodeInfo++) {
+			if (pNodeInfo->m_NodeState ==
+			    kEplNmtMnuNodeStateReadyToOp) {
+				Ret = EplNmtMnuNodeCheckCom(uiIndex, pNodeInfo);
+				if (Ret == kEplReject) {	// timer was started
+					// wait until it expires
+					if ((pNodeInfo->m_dwNodeCfg & EPL_NODEASSIGN_MANDATORY_CN) != 0) {	// node is a mandatory CN
+						EplNmtMnuInstance_g.
+						    m_uiMandatorySlaveCount++;
+					}
+				} else if (Ret != kEplSuccessful) {
+					goto Exit;
+				}
+				// set flag "not scanned"
+				pNodeInfo->m_wFlags |=
+				    EPL_NMTMNU_NODE_FLAG_NOT_SCANNED;
+
+				EplNmtMnuInstance_g.m_uiSignalSlaveCount++;
+				// signal slave counter shall be decremented if timeout elapsed and regardless of an error
+				// mandatory slave counter shall be decremented if timeout elapsed and no error occured
+			}
+		}
+	}
+
+	Ret = kEplSuccessful;
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuNodeCheckCom
+//
+// Description: checks communication of the specified node.
+//              That means wait some time and if no error occured everything
+//              is OK.
+//
+// Parameters:  uiNodeId_p              = node ID
+//              pNodeInfo_p             = pointer to internal node info structure
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplNmtMnuNodeCheckCom(unsigned int uiNodeId_p,
+					tEplNmtMnuNodeInfo * pNodeInfo_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	DWORD dwNodeCfg;
+	tEplTimerArg TimerArg;
+
+	dwNodeCfg = pNodeInfo_p->m_dwNodeCfg;
+	if (((dwNodeCfg & EPL_NODEASSIGN_ASYNCONLY_NODE) == 0)
+	    && (EplNmtMnuInstance_g.m_ulTimeoutCheckCom != 0L)) {	// CN is not async-only and timeout for CheckCom was set
+
+		// check communication,
+		// that means wait some time and if no error occured everything is OK;
+
+		// start timer (when the timer expires the CN must be still ReadyToOp)
+		EPL_NMTMNU_SET_FLAGS_TIMERARG_LONGER(pNodeInfo_p, uiNodeId_p,
+						     TimerArg);
+//        TimerArg.m_EventSink = kEplEventSinkNmtMnu;
+//        TimerArg.m_ulArg = EPL_NMTMNU_TIMERARG_LONGER | uiNodeId_p;
+		Ret =
+		    EplTimeruModifyTimerMs(&pNodeInfo_p->m_TimerHdlLonger,
+					   EplNmtMnuInstance_g.
+					   m_ulTimeoutCheckCom, TimerArg);
+
+		// update mandatory slave counter, because timer was started
+		if (Ret == kEplSuccessful) {
+			Ret = kEplReject;
+		}
+	} else {		// timer was not started
+		// assume everything is OK
+		pNodeInfo_p->m_NodeState = kEplNmtMnuNodeStateComChecked;
+	}
+
+//Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuStartNodes
+//
+// Description: really starts all nodes which are ReadyToOp and CheckCom did not fail
+//
+// Parameters:  (none)
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplNmtMnuStartNodes(void)
+{
+	tEplKernel Ret = kEplSuccessful;
+	unsigned int uiIndex;
+	tEplNmtMnuNodeInfo *pNodeInfo;
+
+	if ((EplNmtMnuInstance_g.m_wFlags & EPL_NMTMNU_FLAG_HALTED) == 0) {	// boot process is not halted
+		// send NMT command Start Node
+		EplNmtMnuInstance_g.m_uiMandatorySlaveCount = 0;
+		EplNmtMnuInstance_g.m_uiSignalSlaveCount = 0;
+		// reset flag that application was informed about possible state change
+		EplNmtMnuInstance_g.m_wFlags &= ~EPL_NMTMNU_FLAG_APP_INFORMED;
+
+		pNodeInfo = EplNmtMnuInstance_g.m_aNodeInfo;
+		for (uiIndex = 1;
+		     uiIndex <= tabentries(EplNmtMnuInstance_g.m_aNodeInfo);
+		     uiIndex++, pNodeInfo++) {
+			if (pNodeInfo->m_NodeState ==
+			    kEplNmtMnuNodeStateComChecked) {
+				if ((EplNmtMnuInstance_g.
+				     m_dwNmtStartup & EPL_NMTST_STARTALLNODES)
+				    == 0) {
+					EPL_NMTMNU_DBG_POST_TRACE_VALUE(0,
+									uiIndex,
+									kEplNmtCmdStartNode);
+
+					Ret =
+					    EplNmtMnuSendNmtCommand(uiIndex,
+								    kEplNmtCmdStartNode);
+					if (Ret != kEplSuccessful) {
+						goto Exit;
+					}
+				}
+
+				if ((pNodeInfo->m_dwNodeCfg & EPL_NODEASSIGN_MANDATORY_CN) != 0) {	// node is a mandatory CN
+					EplNmtMnuInstance_g.
+					    m_uiMandatorySlaveCount++;
+				}
+				// set flag "not scanned"
+				pNodeInfo->m_wFlags |=
+				    EPL_NMTMNU_NODE_FLAG_NOT_SCANNED;
+
+				EplNmtMnuInstance_g.m_uiSignalSlaveCount++;
+				// signal slave counter shall be decremented if StatusRequest was sent once to a CN
+				// mandatory slave counter shall be decremented if mandatory CN is OPERATIONAL
+			}
+		}
+
+		// $$$ inform application if EPL_NMTST_NO_STARTNODE is set
+
+		if ((EplNmtMnuInstance_g.
+		     m_dwNmtStartup & EPL_NMTST_STARTALLNODES) != 0) {
+			EPL_NMTMNU_DBG_POST_TRACE_VALUE(0, EPL_C_ADR_BROADCAST,
+							kEplNmtCmdStartNode);
+
+			Ret =
+			    EplNmtMnuSendNmtCommand(EPL_C_ADR_BROADCAST,
+						    kEplNmtCmdStartNode);
+			if (Ret != kEplSuccessful) {
+				goto Exit;
+			}
+		}
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuProcessInternalEvent
+//
+// Description: processes internal node events
+//
+// Parameters:  uiNodeId_p              = node ID
+//              NodeNmtState_p          = NMT state of CN
+//              NodeEvent_p             = occured events
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplNmtMnuProcessInternalEvent(unsigned int uiNodeId_p,
+						tEplNmtState NodeNmtState_p,
+						WORD wErrorCode_p,
+						tEplNmtMnuIntNodeEvent
+						NodeEvent_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplNmtState NmtState;
+	tEplNmtMnuNodeInfo *pNodeInfo;
+	tEplTimerArg TimerArg;
+
+	pNodeInfo = EPL_NMTMNU_GET_NODEINFO(uiNodeId_p);
+	NmtState = EplNmtuGetNmtState();
+	if (NmtState <= kEplNmtMsNotActive) {	// MN is not active
+		goto Exit;
+	}
+
+	switch (NodeEvent_p) {
+	case kEplNmtMnuIntNodeEventIdentResponse:
+		{
+			BYTE bNmtState;
+
+			EPL_NMTMNU_DBG_POST_TRACE_VALUE(NodeEvent_p,
+							uiNodeId_p,
+							pNodeInfo->m_NodeState);
+
+			if (pNodeInfo->m_NodeState !=
+			    kEplNmtMnuNodeStateResetConf) {
+				pNodeInfo->m_NodeState =
+				    kEplNmtMnuNodeStateIdentified;
+			}
+			// reset flags ISOCHRON and NMT_CMD_ISSUED
+			pNodeInfo->m_wFlags &= ~(EPL_NMTMNU_NODE_FLAG_ISOCHRON
+						 |
+						 EPL_NMTMNU_NODE_FLAG_NMT_CMD_ISSUED);
+
+			if ((NmtState == kEplNmtMsPreOperational1)
+			    &&
+			    ((pNodeInfo->
+			      m_wFlags & EPL_NMTMNU_NODE_FLAG_NOT_SCANNED) !=
+			     0)) {
+				// decrement only signal slave count
+				EplNmtMnuInstance_g.m_uiSignalSlaveCount--;
+				pNodeInfo->m_wFlags &=
+				    ~EPL_NMTMNU_NODE_FLAG_NOT_SCANNED;
+			}
+			// update object 0x1F8F NMT_MNNodeExpState_AU8 to PreOp1 (even if local state >= PreOp2)
+			bNmtState = (BYTE) (kEplNmtCsPreOperational1 & 0xFF);
+			Ret =
+			    EplObduWriteEntry(0x1F8F, uiNodeId_p, &bNmtState,
+					      1);
+
+			// check NMT state of CN
+			Ret =
+			    EplNmtMnuCheckNmtState(uiNodeId_p, pNodeInfo,
+						   NodeNmtState_p, wErrorCode_p,
+						   NmtState);
+			if (Ret != kEplSuccessful) {
+				if (Ret == kEplReject) {
+					Ret = kEplSuccessful;
+				}
+				break;
+			}
+			// request StatusResponse immediately,
+			// because we want a fast boot-up of CNs
+			Ret =
+			    EplStatusuRequestStatusResponse(uiNodeId_p,
+							    EplNmtMnuCbStatusResponse);
+			if (Ret != kEplSuccessful) {
+				EPL_NMTMNU_DBG_POST_TRACE_VALUE(NodeEvent_p,
+								uiNodeId_p,
+								Ret);
+
+				if (Ret == kEplInvalidOperation) {	// the only situation when this should happen is, when
+					// StatusResponse was already requested from within
+					// the StatReq timer event.
+					// so ignore this error.
+					Ret = kEplSuccessful;
+				} else {
+					break;
+				}
+			}
+
+			if (pNodeInfo->m_NodeState !=
+			    kEplNmtMnuNodeStateResetConf) {
+				// inform application
+				Ret =
+				    EplNmtMnuInstance_g.
+				    m_pfnCbNodeEvent(uiNodeId_p,
+						     kEplNmtNodeEventFound,
+						     NodeNmtState_p,
+						     EPL_E_NO_ERROR,
+						     (pNodeInfo->
+						      m_dwNodeCfg &
+						      EPL_NODEASSIGN_MANDATORY_CN)
+						     != 0);
+				if (Ret == kEplReject) {	// interrupt boot process on user request
+					EPL_NMTMNU_DBG_POST_TRACE_VALUE
+					    (NodeEvent_p, uiNodeId_p,
+					     ((pNodeInfo->m_NodeState << 8)
+					      | Ret));
+
+					Ret = kEplSuccessful;
+					break;
+				} else if (Ret != kEplSuccessful) {
+					EPL_NMTMNU_DBG_POST_TRACE_VALUE
+					    (NodeEvent_p, uiNodeId_p,
+					     ((pNodeInfo->m_NodeState << 8)
+					      | Ret));
+
+					break;
+				}
+			}
+			// continue BootStep1
+		}
+
+	case kEplNmtMnuIntNodeEventBoot:
+		{
+
+			// $$$ check identification (vendor ID, product code, revision no, serial no)
+
+			if (pNodeInfo->m_NodeState ==
+			    kEplNmtMnuNodeStateIdentified) {
+				// $$$ check software
+
+				// check/start configuration
+				// inform application
+				Ret =
+				    EplNmtMnuInstance_g.
+				    m_pfnCbNodeEvent(uiNodeId_p,
+						     kEplNmtNodeEventCheckConf,
+						     NodeNmtState_p,
+						     EPL_E_NO_ERROR,
+						     (pNodeInfo->
+						      m_dwNodeCfg &
+						      EPL_NODEASSIGN_MANDATORY_CN)
+						     != 0);
+				if (Ret == kEplReject) {	// interrupt boot process on user request
+					EPL_NMTMNU_DBG_POST_TRACE_VALUE
+					    (kEplNmtMnuIntNodeEventBoot,
+					     uiNodeId_p,
+					     ((pNodeInfo->m_NodeState << 8)
+					      | Ret));
+
+					Ret = kEplSuccessful;
+					break;
+				} else if (Ret != kEplSuccessful) {
+					EPL_NMTMNU_DBG_POST_TRACE_VALUE
+					    (kEplNmtMnuIntNodeEventBoot,
+					     uiNodeId_p,
+					     ((pNodeInfo->m_NodeState << 8)
+					      | Ret));
+
+					break;
+				}
+			} else if (pNodeInfo->m_NodeState != kEplNmtMnuNodeStateResetConf) {	// wrong CN state
+				// ignore event
+				break;
+			}
+			// $$$ d.k.: currently we assume configuration is OK
+
+			// continue BootStep1
+		}
+
+	case kEplNmtMnuIntNodeEventConfigured:
+		{
+			if ((pNodeInfo->m_NodeState !=
+			     kEplNmtMnuNodeStateIdentified)
+			    && (pNodeInfo->m_NodeState != kEplNmtMnuNodeStateResetConf)) {	// wrong CN state
+				// ignore event
+				break;
+			}
+
+			pNodeInfo->m_NodeState = kEplNmtMnuNodeStateConfigured;
+
+			if (NmtState == kEplNmtMsPreOperational1) {
+				if ((pNodeInfo->m_dwNodeCfg & EPL_NODEASSIGN_MANDATORY_CN) != 0) {	// decrement mandatory CN counter
+					EplNmtMnuInstance_g.
+					    m_uiMandatorySlaveCount--;
+				}
+			} else {
+				// put optional node to next step (BootStep2)
+				Ret =
+				    EplNmtMnuNodeBootStep2(uiNodeId_p,
+							   pNodeInfo);
+			}
+			break;
+		}
+
+	case kEplNmtMnuIntNodeEventNoIdentResponse:
+		{
+			if ((NmtState == kEplNmtMsPreOperational1)
+			    &&
+			    ((pNodeInfo->
+			      m_wFlags & EPL_NMTMNU_NODE_FLAG_NOT_SCANNED) !=
+			     0)) {
+				// decrement only signal slave count
+				EplNmtMnuInstance_g.m_uiSignalSlaveCount--;
+				pNodeInfo->m_wFlags &=
+				    ~EPL_NMTMNU_NODE_FLAG_NOT_SCANNED;
+			}
+
+			if (pNodeInfo->m_NodeState !=
+			    kEplNmtMnuNodeStateResetConf) {
+				pNodeInfo->m_NodeState =
+				    kEplNmtMnuNodeStateUnknown;
+			}
+			// $$$ d.k. check start time for 0x1F89/2 MNTimeoutPreOp1_U32
+			// $$$ d.k. check individual timeout 0x1F89/6 MNIdentificationTimeout_U32
+			// if mandatory node and timeout elapsed -> halt boot procedure
+			// trigger IdentRequest again (if >= PreOp2, after delay)
+			if (NmtState >= kEplNmtMsPreOperational2) {	// start timer
+				EPL_NMTMNU_SET_FLAGS_TIMERARG_IDENTREQ
+				    (pNodeInfo, uiNodeId_p, TimerArg);
+//                TimerArg.m_EventSink = kEplEventSinkNmtMnu;
+//                TimerArg.m_ulArg = EPL_NMTMNU_TIMERARG_IDENTREQ | uiNodeId_p;
+/*
+                EPL_NMTMNU_DBG_POST_TRACE_VALUE(kEplNmtMnuIntNodeEventNoIdentResponse,
+                                                uiNodeId_p,
+                                                ((pNodeInfo->m_NodeState << 8)
+                                                 | 0x80
+                                                 | ((pNodeInfo->m_wFlags & EPL_NMTMNU_NODE_FLAG_COUNT_STATREQ) >> 6)
+                                                 | ((TimerArg.m_ulArg & EPL_NMTMNU_TIMERARG_COUNT_SR) >> 8)));
+*/
+				Ret =
+				    EplTimeruModifyTimerMs(&pNodeInfo->
+							   m_TimerHdlStatReq,
+							   EplNmtMnuInstance_g.
+							   m_ulStatusRequestDelay,
+							   TimerArg);
+			} else {	// trigger IdentRequest immediately
+				Ret =
+				    EplIdentuRequestIdentResponse(uiNodeId_p,
+								  EplNmtMnuCbIdentResponse);
+			}
+			break;
+		}
+
+	case kEplNmtMnuIntNodeEventStatusResponse:
+		{
+			if ((NmtState >= kEplNmtMsPreOperational2)
+			    &&
+			    ((pNodeInfo->
+			      m_wFlags & EPL_NMTMNU_NODE_FLAG_NOT_SCANNED) !=
+			     0)) {
+				// decrement only signal slave count if checked once for ReadyToOp, CheckCom, Operational
+				EplNmtMnuInstance_g.m_uiSignalSlaveCount--;
+				pNodeInfo->m_wFlags &=
+				    ~EPL_NMTMNU_NODE_FLAG_NOT_SCANNED;
+			}
+			// check NMT state of CN
+			Ret =
+			    EplNmtMnuCheckNmtState(uiNodeId_p, pNodeInfo,
+						   NodeNmtState_p, wErrorCode_p,
+						   NmtState);
+			if (Ret != kEplSuccessful) {
+				if (Ret == kEplReject) {
+					Ret = kEplSuccessful;
+				}
+				break;
+			}
+
+			if (NmtState == kEplNmtMsPreOperational1) {
+				// request next StatusResponse immediately
+				Ret =
+				    EplStatusuRequestStatusResponse(uiNodeId_p,
+								    EplNmtMnuCbStatusResponse);
+				if (Ret != kEplSuccessful) {
+					EPL_NMTMNU_DBG_POST_TRACE_VALUE
+					    (NodeEvent_p, uiNodeId_p, Ret);
+				}
+
+			} else if ((pNodeInfo->m_wFlags & EPL_NMTMNU_NODE_FLAG_ISOCHRON) == 0) {	// start timer
+				// not isochronously accessed CN (e.g. async-only or stopped CN)
+				EPL_NMTMNU_SET_FLAGS_TIMERARG_STATREQ(pNodeInfo,
+								      uiNodeId_p,
+								      TimerArg);
+//                TimerArg.m_EventSink = kEplEventSinkNmtMnu;
+//                TimerArg.m_ulArg = EPL_NMTMNU_TIMERARG_STATREQ | uiNodeId_p;
+/*
+                EPL_NMTMNU_DBG_POST_TRACE_VALUE(kEplNmtMnuIntNodeEventStatusResponse,
+                                                uiNodeId_p,
+                                                ((pNodeInfo->m_NodeState << 8)
+                                                 | 0x80
+                                                 | ((pNodeInfo->m_wFlags & EPL_NMTMNU_NODE_FLAG_COUNT_STATREQ) >> 6)
+                                                 | ((TimerArg.m_ulArg & EPL_NMTMNU_TIMERARG_COUNT_SR) >> 8)));
+*/
+				Ret =
+				    EplTimeruModifyTimerMs(&pNodeInfo->
+							   m_TimerHdlStatReq,
+							   EplNmtMnuInstance_g.
+							   m_ulStatusRequestDelay,
+							   TimerArg);
+			}
+
+			break;
+		}
+
+	case kEplNmtMnuIntNodeEventNoStatusResponse:
+		{
+			// function CheckNmtState sets node state to unknown if necessary
+/*
+            if ((NmtState >= kEplNmtMsPreOperational2)
+                && ((pNodeInfo->m_wFlags & EPL_NMTMNU_NODE_FLAG_NOT_SCANNED) != 0))
+            {
+                // decrement only signal slave count if checked once for ReadyToOp, CheckCom, Operational
+                EplNmtMnuInstance_g.m_uiSignalSlaveCount--;
+                pNodeInfo->m_wFlags &= ~EPL_NMTMNU_NODE_FLAG_NOT_SCANNED;
+            }
+*/
+			// check NMT state of CN
+			Ret =
+			    EplNmtMnuCheckNmtState(uiNodeId_p, pNodeInfo,
+						   NodeNmtState_p, wErrorCode_p,
+						   NmtState);
+			if (Ret != kEplSuccessful) {
+				if (Ret == kEplReject) {
+					Ret = kEplSuccessful;
+				}
+				break;
+			}
+
+			break;
+		}
+
+	case kEplNmtMnuIntNodeEventError:
+		{		// currently only issued on kEplNmtNodeCommandConfErr
+
+			if (pNodeInfo->m_NodeState != kEplNmtMnuNodeStateIdentified) {	// wrong CN state
+				// ignore event
+				break;
+			}
+			// check NMT state of CN
+			Ret =
+			    EplNmtMnuCheckNmtState(uiNodeId_p, pNodeInfo,
+						   kEplNmtCsNotActive,
+						   wErrorCode_p, NmtState);
+			if (Ret != kEplSuccessful) {
+				if (Ret == kEplReject) {
+					Ret = kEplSuccessful;
+				}
+				break;
+			}
+
+			break;
+		}
+
+	case kEplNmtMnuIntNodeEventExecReset:
+		{
+			if (pNodeInfo->m_NodeState != kEplNmtMnuNodeStateIdentified) {	// wrong CN state
+				// ignore event
+				break;
+			}
+
+			pNodeInfo->m_NodeState = kEplNmtMnuNodeStateResetConf;
+
+			EPL_NMTMNU_DBG_POST_TRACE_VALUE(NodeEvent_p,
+							uiNodeId_p,
+							(((NodeNmtState_p &
+							   0xFF) << 8)
+							 |
+							 kEplNmtCmdResetConfiguration));
+
+			// send NMT reset configuration to CN for activation of configuration
+			Ret =
+			    EplNmtMnuSendNmtCommand(uiNodeId_p,
+						    kEplNmtCmdResetConfiguration);
+
+			break;
+		}
+
+	case kEplNmtMnuIntNodeEventHeartbeat:
+		{
+/*
+            if ((NmtState >= kEplNmtMsPreOperational2)
+                && ((pNodeInfo->m_wFlags & EPL_NMTMNU_NODE_FLAG_NOT_SCANNED) != 0))
+            {
+                // decrement only signal slave count if checked once for ReadyToOp, CheckCom, Operational
+                EplNmtMnuInstance_g.m_uiSignalSlaveCount--;
+                pNodeInfo->m_wFlags &= ~EPL_NMTMNU_NODE_FLAG_NOT_SCANNED;
+            }
+*/
+			// check NMT state of CN
+			Ret =
+			    EplNmtMnuCheckNmtState(uiNodeId_p, pNodeInfo,
+						   NodeNmtState_p, wErrorCode_p,
+						   NmtState);
+			if (Ret != kEplSuccessful) {
+				if (Ret == kEplReject) {
+					Ret = kEplSuccessful;
+				}
+				break;
+			}
+
+			break;
+		}
+
+	case kEplNmtMnuIntNodeEventTimerIdentReq:
+		{
+			EPL_DBGLVL_NMTMN_TRACE1
+			    ("TimerStatReq->IdentReq(%02X)\n", uiNodeId_p);
+			// trigger IdentRequest again
+			Ret =
+			    EplIdentuRequestIdentResponse(uiNodeId_p,
+							  EplNmtMnuCbIdentResponse);
+			if (Ret != kEplSuccessful) {
+				EPL_NMTMNU_DBG_POST_TRACE_VALUE(NodeEvent_p,
+								uiNodeId_p,
+								(((NodeNmtState_p & 0xFF) << 8)
+								 | Ret));
+				if (Ret == kEplInvalidOperation) {	// this can happen because of a bug in EplTimeruLinuxKernel.c
+					// so ignore this error.
+					Ret = kEplSuccessful;
+				}
+			}
+
+			break;
+		}
+
+	case kEplNmtMnuIntNodeEventTimerStateMon:
+		{
+			// reset NMT state change flag
+			// because from now on the CN must have the correct NMT state
+			pNodeInfo->m_wFlags &=
+			    ~EPL_NMTMNU_NODE_FLAG_NMT_CMD_ISSUED;
+
+			// continue with normal StatReq processing
+		}
+
+	case kEplNmtMnuIntNodeEventTimerStatReq:
+		{
+			EPL_DBGLVL_NMTMN_TRACE1("TimerStatReq->StatReq(%02X)\n",
+						uiNodeId_p);
+			// request next StatusResponse
+			Ret =
+			    EplStatusuRequestStatusResponse(uiNodeId_p,
+							    EplNmtMnuCbStatusResponse);
+			if (Ret != kEplSuccessful) {
+				EPL_NMTMNU_DBG_POST_TRACE_VALUE(NodeEvent_p,
+								uiNodeId_p,
+								(((NodeNmtState_p & 0xFF) << 8)
+								 | Ret));
+				if (Ret == kEplInvalidOperation) {	// the only situation when this should happen is, when
+					// StatusResponse was already requested while processing
+					// event IdentResponse.
+					// so ignore this error.
+					Ret = kEplSuccessful;
+				}
+			}
+
+			break;
+		}
+
+	case kEplNmtMnuIntNodeEventTimerLonger:
+		{
+			switch (pNodeInfo->m_NodeState) {
+			case kEplNmtMnuNodeStateConfigured:
+				{	// node should be ReadyToOp but it is not
+
+					// check NMT state which shall be intentionally wrong, so that ERROR_TREATMENT will be started
+					Ret =
+					    EplNmtMnuCheckNmtState(uiNodeId_p,
+								   pNodeInfo,
+								   kEplNmtCsNotActive,
+								   EPL_E_NMT_BPO2,
+								   NmtState);
+					if (Ret != kEplSuccessful) {
+						if (Ret == kEplReject) {
+							Ret = kEplSuccessful;
+						}
+						break;
+					}
+
+					break;
+				}
+
+			case kEplNmtMnuNodeStateReadyToOp:
+				{	// CheckCom finished successfully
+
+					pNodeInfo->m_NodeState =
+					    kEplNmtMnuNodeStateComChecked;
+
+					if ((pNodeInfo->
+					     m_wFlags &
+					     EPL_NMTMNU_NODE_FLAG_NOT_SCANNED)
+					    != 0) {
+						// decrement only signal slave count if checked once for ReadyToOp, CheckCom, Operational
+						EplNmtMnuInstance_g.
+						    m_uiSignalSlaveCount--;
+						pNodeInfo->m_wFlags &=
+						    ~EPL_NMTMNU_NODE_FLAG_NOT_SCANNED;
+					}
+
+					if ((pNodeInfo->
+					     m_dwNodeCfg &
+					     EPL_NODEASSIGN_MANDATORY_CN) !=
+					    0) {
+						// decrement mandatory slave counter
+						EplNmtMnuInstance_g.
+						    m_uiMandatorySlaveCount--;
+					}
+					if (NmtState != kEplNmtMsReadyToOperate) {
+						EPL_NMTMNU_DBG_POST_TRACE_VALUE
+						    (NodeEvent_p, uiNodeId_p,
+						     (((NodeNmtState_p & 0xFF)
+						       << 8)
+						      | kEplNmtCmdStartNode));
+
+						// start optional CN
+						Ret =
+						    EplNmtMnuSendNmtCommand
+						    (uiNodeId_p,
+						     kEplNmtCmdStartNode);
+					}
+					break;
+				}
+
+			default:
+				{
+					break;
+				}
+			}
+			break;
+		}
+
+	case kEplNmtMnuIntNodeEventNmtCmdSent:
+		{
+			BYTE bNmtState;
+
+			// update expected NMT state with the one that results
+			// from the sent NMT command
+			bNmtState = (BYTE) (NodeNmtState_p & 0xFF);
+
+			// write object 0x1F8F NMT_MNNodeExpState_AU8
+			Ret =
+			    EplObduWriteEntry(0x1F8F, uiNodeId_p, &bNmtState,
+					      1);
+			if (Ret != kEplSuccessful) {
+				goto Exit;
+			}
+
+			if (NodeNmtState_p == kEplNmtCsNotActive) {	// restart processing with IdentRequest
+				EPL_NMTMNU_SET_FLAGS_TIMERARG_IDENTREQ
+				    (pNodeInfo, uiNodeId_p, TimerArg);
+			} else {	// monitor NMT state change with StatusRequest after
+				// the corresponding delay;
+				// until then wrong NMT states will be ignored
+				EPL_NMTMNU_SET_FLAGS_TIMERARG_STATE_MON
+				    (pNodeInfo, uiNodeId_p, TimerArg);
+
+				// set NMT state change flag
+				pNodeInfo->m_wFlags |=
+				    EPL_NMTMNU_NODE_FLAG_NMT_CMD_ISSUED;
+			}
+
+			Ret =
+			    EplTimeruModifyTimerMs(&pNodeInfo->
+						   m_TimerHdlStatReq,
+						   EplNmtMnuInstance_g.
+						   m_ulStatusRequestDelay,
+						   TimerArg);
+
+			// finish processing, because NmtState_p is the expected and not the current state
+			goto Exit;
+		}
+
+	default:
+		{
+			break;
+		}
+	}
+
+	// check if network is ready to change local NMT state and this was not done before
+	if ((EplNmtMnuInstance_g.m_wFlags & (EPL_NMTMNU_FLAG_HALTED | EPL_NMTMNU_FLAG_APP_INFORMED)) == 0) {	// boot process is not halted
+		switch (NmtState) {
+		case kEplNmtMsPreOperational1:
+			{
+				if ((EplNmtMnuInstance_g.m_uiSignalSlaveCount ==
+				     0)
+				    && (EplNmtMnuInstance_g.m_uiMandatorySlaveCount == 0)) {	// all optional CNs scanned once and all mandatory CNs configured successfully
+					EplNmtMnuInstance_g.m_wFlags |=
+					    EPL_NMTMNU_FLAG_APP_INFORMED;
+					// inform application
+					Ret =
+					    EplNmtMnuInstance_g.
+					    m_pfnCbBootEvent
+					    (kEplNmtBootEventBootStep1Finish,
+					     NmtState, EPL_E_NO_ERROR);
+					if (Ret != kEplSuccessful) {
+						if (Ret == kEplReject) {
+							// wait for application
+							Ret = kEplSuccessful;
+						}
+						break;
+					}
+					// enter PreOp2
+					Ret =
+					    EplNmtuNmtEvent
+					    (kEplNmtEventAllMandatoryCNIdent);
+				}
+				break;
+			}
+
+		case kEplNmtMsPreOperational2:
+			{
+				if ((EplNmtMnuInstance_g.m_uiSignalSlaveCount ==
+				     0)
+				    && (EplNmtMnuInstance_g.m_uiMandatorySlaveCount == 0)) {	// all optional CNs checked once for ReadyToOp and all mandatory CNs are ReadyToOp
+					EplNmtMnuInstance_g.m_wFlags |=
+					    EPL_NMTMNU_FLAG_APP_INFORMED;
+					// inform application
+					Ret =
+					    EplNmtMnuInstance_g.
+					    m_pfnCbBootEvent
+					    (kEplNmtBootEventBootStep2Finish,
+					     NmtState, EPL_E_NO_ERROR);
+					if (Ret != kEplSuccessful) {
+						if (Ret == kEplReject) {
+							// wait for application
+							Ret = kEplSuccessful;
+						}
+						break;
+					}
+					// enter ReadyToOp
+					Ret =
+					    EplNmtuNmtEvent
+					    (kEplNmtEventEnterReadyToOperate);
+				}
+				break;
+			}
+
+		case kEplNmtMsReadyToOperate:
+			{
+				if ((EplNmtMnuInstance_g.m_uiSignalSlaveCount ==
+				     0)
+				    && (EplNmtMnuInstance_g.m_uiMandatorySlaveCount == 0)) {	// all CNs checked for errorless communication
+					EplNmtMnuInstance_g.m_wFlags |=
+					    EPL_NMTMNU_FLAG_APP_INFORMED;
+					// inform application
+					Ret =
+					    EplNmtMnuInstance_g.
+					    m_pfnCbBootEvent
+					    (kEplNmtBootEventCheckComFinish,
+					     NmtState, EPL_E_NO_ERROR);
+					if (Ret != kEplSuccessful) {
+						if (Ret == kEplReject) {
+							// wait for application
+							Ret = kEplSuccessful;
+						}
+						break;
+					}
+					// enter Operational
+					Ret =
+					    EplNmtuNmtEvent
+					    (kEplNmtEventEnterMsOperational);
+				}
+				break;
+			}
+
+		case kEplNmtMsOperational:
+			{
+				if ((EplNmtMnuInstance_g.m_uiSignalSlaveCount ==
+				     0)
+				    && (EplNmtMnuInstance_g.m_uiMandatorySlaveCount == 0)) {	// all optional CNs scanned once and all mandatory CNs are OPERATIONAL
+					EplNmtMnuInstance_g.m_wFlags |=
+					    EPL_NMTMNU_FLAG_APP_INFORMED;
+					// inform application
+					Ret =
+					    EplNmtMnuInstance_g.
+					    m_pfnCbBootEvent
+					    (kEplNmtBootEventOperational,
+					     NmtState, EPL_E_NO_ERROR);
+					if (Ret != kEplSuccessful) {
+						if (Ret == kEplReject) {
+							// ignore error code
+							Ret = kEplSuccessful;
+						}
+						break;
+					}
+				}
+				break;
+			}
+
+		default:
+			{
+				break;
+			}
+		}
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuCheckNmtState
+//
+// Description: checks the NMT state, i.e. evaluates it with object 0x1F8F
+//              NMT_MNNodeExpState_AU8 and updates object 0x1F8E
+//              NMT_MNNodeCurrState_AU8.
+//              It manipulates m_NodeState in internal node info structure.
+//
+// Parameters:  uiNodeId_p              = node ID
+//              NodeNmtState_p          = NMT state of CN
+//
+// Returns:     tEplKernel              = error code
+//                  kEplReject          = CN was in wrong state and has been reset
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplNmtMnuCheckNmtState(unsigned int uiNodeId_p,
+					 tEplNmtMnuNodeInfo * pNodeInfo_p,
+					 tEplNmtState NodeNmtState_p,
+					 WORD wErrorCode_p,
+					 tEplNmtState LocalNmtState_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplObdSize ObdSize;
+	BYTE bNmtState;
+	BYTE bNmtStatePrev;
+	tEplNmtState ExpNmtState;
+
+	ObdSize = 1;
+	// read object 0x1F8F NMT_MNNodeExpState_AU8
+	Ret = EplObduReadEntry(0x1F8F, uiNodeId_p, &bNmtState, &ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// compute expected NMT state
+	ExpNmtState = (tEplNmtState) (bNmtState | EPL_NMT_TYPE_CS);
+	// compute BYTE of current NMT state
+	bNmtState = ((BYTE) NodeNmtState_p & 0xFF);
+
+	if (ExpNmtState == kEplNmtCsNotActive) {	// ignore the current state, because the CN shall be not active
+		Ret = kEplReject;
+		goto Exit;
+	} else if ((ExpNmtState == kEplNmtCsPreOperational2)
+		   && (NodeNmtState_p == kEplNmtCsReadyToOperate)) {	// CN switched to ReadyToOp
+		// delete timer for timeout handling
+		Ret = EplTimeruDeleteTimer(&pNodeInfo_p->m_TimerHdlLonger);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+		pNodeInfo_p->m_NodeState = kEplNmtMnuNodeStateReadyToOp;
+
+		// update object 0x1F8F NMT_MNNodeExpState_AU8 to ReadyToOp
+		Ret = EplObduWriteEntry(0x1F8F, uiNodeId_p, &bNmtState, 1);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+
+		if ((pNodeInfo_p->m_dwNodeCfg & EPL_NODEASSIGN_MANDATORY_CN) != 0) {	// node is a mandatory CN -> decrement counter
+			EplNmtMnuInstance_g.m_uiMandatorySlaveCount--;
+		}
+		if (LocalNmtState_p >= kEplNmtMsReadyToOperate) {	// start procedure CheckCommunication for this node
+			Ret = EplNmtMnuNodeCheckCom(uiNodeId_p, pNodeInfo_p);
+			if (Ret != kEplSuccessful) {
+				goto Exit;
+			}
+
+			if ((LocalNmtState_p == kEplNmtMsOperational)
+			    && (pNodeInfo_p->m_NodeState ==
+				kEplNmtMnuNodeStateComChecked)) {
+				EPL_NMTMNU_DBG_POST_TRACE_VALUE(0, uiNodeId_p,
+								(((NodeNmtState_p & 0xFF) << 8)
+								 |
+								 kEplNmtCmdStartNode));
+
+				// immediately start optional CN, because communication is always OK (e.g. async-only CN)
+				Ret =
+				    EplNmtMnuSendNmtCommand(uiNodeId_p,
+							    kEplNmtCmdStartNode);
+				if (Ret != kEplSuccessful) {
+					goto Exit;
+				}
+			}
+		}
+
+	} else if ((ExpNmtState == kEplNmtCsReadyToOperate)
+		   && (NodeNmtState_p == kEplNmtCsOperational)) {	// CN switched to OPERATIONAL
+		pNodeInfo_p->m_NodeState = kEplNmtMnuNodeStateOperational;
+
+		if ((pNodeInfo_p->m_dwNodeCfg & EPL_NODEASSIGN_MANDATORY_CN) != 0) {	// node is a mandatory CN -> decrement counter
+			EplNmtMnuInstance_g.m_uiMandatorySlaveCount--;
+		}
+
+	} else if ((ExpNmtState != NodeNmtState_p)
+		   && !((ExpNmtState == kEplNmtCsPreOperational1)
+			&& (NodeNmtState_p == kEplNmtCsPreOperational2))) {	// CN is not in expected NMT state (without the exceptions above)
+		WORD wbeErrorCode;
+
+		if ((pNodeInfo_p->
+		     m_wFlags & EPL_NMTMNU_NODE_FLAG_NOT_SCANNED) != 0) {
+			// decrement only signal slave count if checked once
+			EplNmtMnuInstance_g.m_uiSignalSlaveCount--;
+			pNodeInfo_p->m_wFlags &=
+			    ~EPL_NMTMNU_NODE_FLAG_NOT_SCANNED;
+		}
+
+		if (pNodeInfo_p->m_NodeState == kEplNmtMnuNodeStateUnknown) {	// CN is already in state unknown, which means that it got
+			// NMT reset command earlier
+			goto Exit;
+		}
+		// -> CN is in wrong NMT state
+		pNodeInfo_p->m_NodeState = kEplNmtMnuNodeStateUnknown;
+
+		if (wErrorCode_p == 0) {	// assume wrong NMT state error
+			if ((pNodeInfo_p->m_wFlags & EPL_NMTMNU_NODE_FLAG_NMT_CMD_ISSUED) != 0) {	// NMT command has been just issued;
+				// ignore wrong NMT state until timer expires;
+				// other errors like LOSS_PRES_TH are still processed
+				goto Exit;
+			}
+
+			wErrorCode_p = EPL_E_NMT_WRONG_STATE;
+		}
+
+		BENCHMARK_MOD_07_TOGGLE(9);
+
+		// $$$ start ERROR_TREATMENT and inform application
+		Ret = EplNmtMnuInstance_g.m_pfnCbNodeEvent(uiNodeId_p,
+							   kEplNmtNodeEventError,
+							   NodeNmtState_p,
+							   wErrorCode_p,
+							   (pNodeInfo_p->
+							    m_dwNodeCfg &
+							    EPL_NODEASSIGN_MANDATORY_CN)
+							   != 0);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+
+		EPL_NMTMNU_DBG_POST_TRACE_VALUE(0,
+						uiNodeId_p,
+						(((NodeNmtState_p & 0xFF) << 8)
+						 | kEplNmtCmdResetNode));
+
+		// reset CN
+		// store error code in NMT command data for diagnostic purpose
+		AmiSetWordToLe(&wbeErrorCode, wErrorCode_p);
+		Ret =
+		    EplNmtMnuSendNmtCommandEx(uiNodeId_p, kEplNmtCmdResetNode,
+					      &wbeErrorCode,
+					      sizeof(wbeErrorCode));
+		if (Ret == kEplSuccessful) {
+			Ret = kEplReject;
+		}
+
+		goto Exit;
+	}
+	// check if NMT_MNNodeCurrState_AU8 has to be changed
+	ObdSize = 1;
+	Ret = EplObduReadEntry(0x1F8E, uiNodeId_p, &bNmtStatePrev, &ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	if (bNmtState != bNmtStatePrev) {
+		// update object 0x1F8E NMT_MNNodeCurrState_AU8
+		Ret = EplObduWriteEntry(0x1F8E, uiNodeId_p, &bNmtState, 1);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+		Ret = EplNmtMnuInstance_g.m_pfnCbNodeEvent(uiNodeId_p,
+							   kEplNmtNodeEventNmtState,
+							   NodeNmtState_p,
+							   wErrorCode_p,
+							   (pNodeInfo_p->
+							    m_dwNodeCfg &
+							    EPL_NODEASSIGN_MANDATORY_CN)
+							   != 0);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtMnuReset
+//
+// Description: reset internal structures, e.g. timers
+//
+// Parameters:  void
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplNmtMnuReset(void)
+{
+	tEplKernel Ret;
+	int iIndex;
+
+	Ret = EplTimeruDeleteTimer(&EplNmtMnuInstance_g.m_TimerHdlNmtState);
+
+	for (iIndex = 1; iIndex <= tabentries(EplNmtMnuInstance_g.m_aNodeInfo);
+	     iIndex++) {
+		// delete timer handles
+		Ret =
+		    EplTimeruDeleteTimer(&EPL_NMTMNU_GET_NODEINFO(iIndex)->
+					 m_TimerHdlStatReq);
+		Ret =
+		    EplTimeruDeleteTimer(&EPL_NMTMNU_GET_NODEINFO(iIndex)->
+					 m_TimerHdlLonger);
+	}
+
+	return Ret;
+}
+
+#endif // #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+// EOF
diff --git a/drivers/staging/epl/EplNmtk.c b/drivers/staging/epl/EplNmtk.c
new file mode 100644
index 00000000000..4e2d0e15e94
--- /dev/null
+++ b/drivers/staging/epl/EplNmtk.c
@@ -0,0 +1,1842 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for NMT-Kernelspace-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplNmtk.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.12 $  $Date: 2008/11/13 17:13:09 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/09 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "kernel/EplNmtk.h"
+#include "kernel/EplTimerk.h"
+
+#include "kernel/EplDllk.h"	// for EplDllkProcess()
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTK)) != 0)
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+// TracePoint support for realtime-debugging
+#ifdef _DBG_TRACE_POINTS_
+void PUBLIC TgtDbgSignalTracePoint(BYTE bTracePointNumber_p);
+void PUBLIC TgtDbgPostTraceValue(DWORD dwTraceValue_p);
+#define TGT_DBG_SIGNAL_TRACE_POINT(p)   TgtDbgSignalTracePoint(p)
+#define TGT_DBG_POST_TRACE_VALUE(v)     TgtDbgPostTraceValue(v)
+#else
+#define TGT_DBG_SIGNAL_TRACE_POINT(p)
+#define TGT_DBG_POST_TRACE_VALUE(v)
+#endif
+#define EPL_NMTK_DBG_POST_TRACE_VALUE(NmtEvent_p, OldNmtState_p, NewNmtState_p) \
+    TGT_DBG_POST_TRACE_VALUE((kEplEventSinkNmtk << 28) | (NmtEvent_p << 16) \
+                             | ((OldNmtState_p & 0xFF) << 8) \
+                             | (NewNmtState_p & 0xFF))
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+// struct for instance table
+INSTANCE_TYPE_BEGIN EPL_MCO_DECL_INSTANCE_MEMBER()
+
+STATIC volatile tEplNmtState INST_FAR m_NmtState;
+STATIC volatile BOOL INST_FAR m_fEnableReadyToOperate;
+STATIC volatile BOOL INST_FAR m_fAppReadyToOperate;
+STATIC volatile BOOL INST_FAR m_fTimerMsPreOp2;
+STATIC volatile BOOL INST_FAR m_fAllMandatoryCNIdent;
+STATIC volatile BOOL INST_FAR m_fFrozen;
+
+INSTANCE_TYPE_END
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+// This macro replace the unspecific pointer to an instance through
+// the modul specific type for the local instance table. This macro
+// must defined in each modul.
+//#define tEplPtrInstance             tEplInstanceInfo MEM*
+EPL_MCO_DECL_INSTANCE_VAR()
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+EPL_MCO_DEFINE_INSTANCE_FCT()
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  <NMT_Kernel-Module>                                 */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description: This module realize the NMT-State-Machine of the EPL-Stack
+//
+//
+/***************************************************************************/
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+//---------------------------------------------------------------------------
+//
+// Function:        EplNmtkInit
+//
+// Description: initializes the first instance
+//
+//
+//
+// Parameters:  EPL_MCO_DECL_PTR_INSTANCE_PTR = Instance pointer
+//              uiNodeId_p = Node Id of the lokal node
+//
+//
+// Returns:     tEplKernel  =   Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtkInit(EPL_MCO_DECL_PTR_INSTANCE_PTR)
+{
+	tEplKernel Ret;
+
+	Ret = EplNmtkAddInstance(EPL_MCO_PTR_INSTANCE_PTR);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:        EplNmtkAddInstance
+//
+// Description: adds a new instance
+//
+//
+//
+// Parameters:  EPL_MCO_DECL_PTR_INSTANCE_PTR = Instance pointer
+//
+//
+// Returns:     tEplKernel  =   Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtkAddInstance(EPL_MCO_DECL_PTR_INSTANCE_PTR)
+{
+	EPL_MCO_DECL_INSTANCE_PTR_LOCAL tEplKernel Ret;
+//tEplEvent               Event;
+//tEplEventNmtStateChange NmtStateChange;
+
+	// check if pointer to instance pointer valid
+	// get free instance and set the globale instance pointer
+	// set also the instance addr to parameterlist
+	EPL_MCO_CHECK_PTR_INSTANCE_PTR();
+	EPL_MCO_GET_FREE_INSTANCE_PTR();
+	EPL_MCO_SET_PTR_INSTANCE_PTR();
+
+	// sign instance as used
+	EPL_MCO_WRITE_INSTANCE_STATE(kStateUsed);
+
+	Ret = kEplSuccessful;
+
+	// initialize intern vaiables
+	// 2006/07/31 d.k.: set NMT-State to kEplNmtGsOff
+	EPL_MCO_GLB_VAR(m_NmtState) = kEplNmtGsOff;
+	// set NMT-State to kEplNmtGsInitialising
+	//EPL_MCO_GLB_VAR(m_NmtState) = kEplNmtGsInitialising;
+
+	// set flags to FALSE
+	EPL_MCO_GLB_VAR(m_fEnableReadyToOperate) = FALSE;
+	EPL_MCO_GLB_VAR(m_fAppReadyToOperate) = FALSE;
+	EPL_MCO_GLB_VAR(m_fTimerMsPreOp2) = FALSE;
+	EPL_MCO_GLB_VAR(m_fAllMandatoryCNIdent) = FALSE;
+	EPL_MCO_GLB_VAR(m_fFrozen) = FALSE;
+
+//    EPL_MCO_GLB_VAR(m_TimerHdl) = 0;
+
+	// inform higher layer about state change
+	// 2006/07/31 d.k.: The EPL API layer/application has to start NMT state
+	//                  machine via NmtEventSwReset after initialisation of
+	//                  all modules has been completed. DLL has to be initialised
+	//                  after NMTk because NMT state shall not be uninitialised
+	//                  at that time.
+/*    NmtStateChange.m_NewNmtState = EPL_MCO_GLB_VAR(m_NmtState);
+    NmtStateChange.m_NmtEvent = kEplNmtEventNoEvent;
+    Event.m_EventSink = kEplEventSinkNmtu;
+    Event.m_EventType = kEplEventTypeNmtStateChange;
+    EPL_MEMSET(&Event.m_NetTime, 0x00, sizeof(Event.m_NetTime));
+    Event.m_pArg = &NmtStateChange;
+    Event.m_uiSize = sizeof(NmtStateChange);
+    Ret = EplEventkPost(&Event);
+*/
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:        EplNmtkDelInstance
+//
+// Description: delete instance
+//
+//
+//
+// Parameters:  EPL_MCO_DECL_PTR_INSTANCE_PTR = Instance pointer
+//
+//
+// Returns:     tEplKernel  =   Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#if (EPL_USE_DELETEINST_FUNC != FALSE)
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtkDelInstance(EPL_MCO_DECL_PTR_INSTANCE_PTR)
+{
+	tEplKernel Ret = kEplSuccessful;
+	// check for all API function if instance is valid
+	EPL_MCO_CHECK_INSTANCE_STATE();
+
+	// set NMT-State to kEplNmtGsOff
+	EPL_MCO_GLB_VAR(m_NmtState) = kEplNmtGsOff;
+
+	// sign instance as unused
+	EPL_MCO_WRITE_INSTANCE_STATE(kStateUnused);
+
+	// delete timer
+//    Ret = EplTimerkDeleteTimer(&EPL_MCO_GLB_VAR(m_TimerHdl));
+
+	return Ret;
+}
+#endif // (EPL_USE_DELETEINST_FUNC != FALSE)
+
+//---------------------------------------------------------------------------
+//
+// Function:        EplNmtkProcess
+//
+// Description: main process function
+//              -> process NMT-State-Maschine und read NMT-Events from Queue
+//
+//
+//
+// Parameters:  EPL_MCO_DECL_PTR_INSTANCE_PTR_ = Instance pointer
+//              pEvent_p    =   Epl-Event with NMT-event to process
+//
+//
+// Returns:     tEplKernel  =   Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtkProcess(EPL_MCO_DECL_PTR_INSTANCE_PTR_
+					      tEplEvent * pEvent_p)
+{
+	tEplKernel Ret;
+	tEplNmtState OldNmtState;
+	tEplNmtEvent NmtEvent;
+	tEplEvent Event;
+	tEplEventNmtStateChange NmtStateChange;
+
+	// check for all API function if instance is valid
+	EPL_MCO_CHECK_INSTANCE_STATE();
+
+	Ret = kEplSuccessful;
+
+	switch (pEvent_p->m_EventType) {
+	case kEplEventTypeNmtEvent:
+		{
+			NmtEvent = *((tEplNmtEvent *) pEvent_p->m_pArg);
+			break;
+		}
+
+	case kEplEventTypeTimer:
+		{
+			NmtEvent =
+			    (tEplNmtEvent) ((tEplTimerEventArg *) pEvent_p->
+					    m_pArg)->m_ulArg;
+			break;
+		}
+	default:
+		{
+			Ret = kEplNmtInvalidEvent;
+			goto Exit;
+		}
+	}
+
+	// save NMT-State
+	// needed for later comparison to
+	// inform hgher layer about state change
+	OldNmtState = EPL_MCO_GLB_VAR(m_NmtState);
+
+	// NMT-State-Maschine
+	switch (EPL_MCO_GLB_VAR(m_NmtState)) {
+		//-----------------------------------------------------------
+		// general part of the statemaschine
+
+		// first init of the hardware
+	case kEplNmtGsOff:
+		{
+			// leave this state only if higher layer says so
+			if (NmtEvent == kEplNmtEventSwReset) {	// new state kEplNmtGsInitialising
+				EPL_MCO_GLB_VAR(m_NmtState) =
+				    kEplNmtGsInitialising;
+			}
+			break;
+		}
+
+		// first init of the hardware
+	case kEplNmtGsInitialising:
+		{
+			// leave this state only if higher layer says so
+
+			// check events
+			switch (NmtEvent) {
+				// 2006/07/31 d.k.: react also on NMT reset commands in ResetApp state
+				// NMT Command SwitchOff
+			case kEplNmtEventCriticalError:
+			case kEplNmtEventSwitchOff:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsOff;
+					break;
+				}
+
+				// new state kEplNmtGsResetApplication
+			case kEplNmtEventEnterResetApp:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetApplication;
+					break;
+				}
+
+			default:
+				{
+					break;
+				}
+			}
+			break;
+		}
+
+		// init of the manufacturer-specific profile area and the
+		// standardised device profile area
+	case kEplNmtGsResetApplication:
+		{
+			// check events
+			switch (NmtEvent) {
+				// 2006/07/31 d.k.: react also on NMT reset commands in ResetApp state
+				// NMT Command SwitchOff
+			case kEplNmtEventCriticalError:
+			case kEplNmtEventSwitchOff:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsOff;
+					break;
+				}
+
+				// NMT Command SwReset
+			case kEplNmtEventSwReset:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsInitialising;
+					break;
+				}
+
+				// leave this state only if higher layer
+				// say so
+			case kEplNmtEventEnterResetCom:
+				{
+					// new state kEplNmtGsResetCommunication
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+			default:
+				{
+					break;
+				}
+			}
+			break;
+		}
+
+		// init of the communication profile area
+	case kEplNmtGsResetCommunication:
+		{
+			// check events
+			switch (NmtEvent) {
+				// 2006/07/31 d.k.: react also on NMT reset commands in ResetComm state
+				// NMT Command SwitchOff
+			case kEplNmtEventCriticalError:
+			case kEplNmtEventSwitchOff:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsOff;
+					break;
+				}
+
+				// NMT Command SwReset
+			case kEplNmtEventSwReset:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsInitialising;
+					break;
+				}
+
+				// NMT Command ResetNode
+			case kEplNmtEventResetNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetApplication;
+					break;
+				}
+
+				// leave this state only if higher layer
+				// say so
+			case kEplNmtEventEnterResetConfig:
+				{
+					// new state kEplNmtGsResetCommunication
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetConfiguration;
+					break;
+				}
+
+			default:
+				{
+					break;
+				}
+			}
+			break;
+		}
+
+		// build the configuration with infos from OD
+	case kEplNmtGsResetConfiguration:
+		{
+			// reset flags
+			EPL_MCO_GLB_VAR(m_fEnableReadyToOperate) = FALSE;
+			EPL_MCO_GLB_VAR(m_fAppReadyToOperate) = FALSE;
+			EPL_MCO_GLB_VAR(m_fFrozen) = FALSE;
+
+			// check events
+			switch (NmtEvent) {
+				// 2006/07/31 d.k.: react also on NMT reset commands in ResetConf state
+				// NMT Command SwitchOff
+			case kEplNmtEventCriticalError:
+			case kEplNmtEventSwitchOff:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsOff;
+					break;
+				}
+
+				// NMT Command SwReset
+			case kEplNmtEventSwReset:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsInitialising;
+					break;
+				}
+
+				// NMT Command ResetNode
+			case kEplNmtEventResetNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetApplication;
+					break;
+				}
+
+				// NMT Command ResetCommunication
+			case kEplNmtEventResetCom:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+				// leave this state only if higher layer says so
+			case kEplNmtEventEnterCsNotActive:
+				{	// Node should be CN
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtCsNotActive;
+					break;
+
+				}
+
+			case kEplNmtEventEnterMsNotActive:
+				{	// Node should be CN
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) == 0)
+					// no MN functionality
+					// TODO: -create error E_NMT_BA1_NO_MN_SUPPORT
+					EPL_MCO_GLB_VAR(m_fFrozen) = TRUE;
+#else
+
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtMsNotActive;
+#endif
+					break;
+
+				}
+
+			default:
+				{
+					break;
+				}
+			}
+			break;
+		}
+
+		//-----------------------------------------------------------
+		// CN part of the statemaschine
+
+		// node liste for EPL-Frames and check timeout
+	case kEplNmtCsNotActive:
+		{
+
+			// check events
+			switch (NmtEvent) {
+				// 2006/07/31 d.k.: react also on NMT reset commands in NotActive state
+				// NMT Command SwitchOff
+			case kEplNmtEventCriticalError:
+			case kEplNmtEventSwitchOff:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsOff;
+					break;
+				}
+
+				// NMT Command SwReset
+			case kEplNmtEventSwReset:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsInitialising;
+//                    Ret = EplTimerkDeleteTimer(&EPL_MCO_GLB_VAR(m_TimerHdl));
+					break;
+				}
+
+				// NMT Command ResetNode
+			case kEplNmtEventResetNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetApplication;
+//                    Ret = EplTimerkDeleteTimer(&EPL_MCO_GLB_VAR(m_TimerHdl));
+					break;
+				}
+
+				// NMT Command ResetCommunication
+				// or internal Communication error
+			case kEplNmtEventResetCom:
+			case kEplNmtEventInternComError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+//                    Ret = EplTimerkDeleteTimer(&EPL_MCO_GLB_VAR(m_TimerHdl));
+					break;
+				}
+
+				// NMT Command Reset Configuration
+			case kEplNmtEventResetConfig:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetConfiguration;
+//                    Ret = EplTimerkDeleteTimer(&EPL_MCO_GLB_VAR(m_TimerHdl));
+					break;
+				}
+
+				// see if SoA or SoC received
+				// k.t. 20.07.2006: only SoA forces change of state
+				// see EPL V2 DS 1.0.0 p.267
+				// case kEplNmtEventDllCeSoc:
+			case kEplNmtEventDllCeSoa:
+				{	// new state PRE_OPERATIONAL1
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtCsPreOperational1;
+//                    Ret = EplTimerkDeleteTimer(&EPL_MCO_GLB_VAR(m_TimerHdl));
+					break;
+				}
+				// timeout for SoA and Soc
+			case kEplNmtEventTimerBasicEthernet:
+				{
+					// new state BASIC_ETHERNET
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtCsBasicEthernet;
+					break;
+				}
+
+			default:
+				{
+					break;
+				}
+			}	// end of switch(NmtEvent)
+
+			break;
+		}
+
+		// node processes only async frames
+	case kEplNmtCsPreOperational1:
+		{
+
+			// check events
+			switch (NmtEvent) {
+				// NMT Command SwitchOff
+			case kEplNmtEventCriticalError:
+			case kEplNmtEventSwitchOff:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsOff;
+					break;
+				}
+
+				// NMT Command SwReset
+			case kEplNmtEventSwReset:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsInitialising;
+					break;
+				}
+
+				// NMT Command ResetNode
+			case kEplNmtEventResetNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetApplication;
+					break;
+				}
+
+				// NMT Command ResetCommunication
+				// or internal Communication error
+			case kEplNmtEventResetCom:
+			case kEplNmtEventInternComError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+				// NMT Command Reset Configuration
+			case kEplNmtEventResetConfig:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetConfiguration;
+					break;
+				}
+
+				// NMT Command StopNode
+			case kEplNmtEventStopNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtCsStopped;
+					break;
+				}
+
+				// check if SoC received
+			case kEplNmtEventDllCeSoc:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtCsPreOperational2;
+					break;
+				}
+
+			default:
+				{
+					break;
+				}
+
+			}	// end of switch(NmtEvent)
+
+			break;
+		}
+
+		// node processes isochronous and asynchronous frames
+	case kEplNmtCsPreOperational2:
+		{
+			// check events
+			switch (NmtEvent) {
+				// NMT Command SwitchOff
+			case kEplNmtEventCriticalError:
+			case kEplNmtEventSwitchOff:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsOff;
+					break;
+				}
+
+				// NMT Command SwReset
+			case kEplNmtEventSwReset:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsInitialising;
+					break;
+				}
+
+				// NMT Command ResetNode
+			case kEplNmtEventResetNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetApplication;
+					break;
+				}
+
+				// NMT Command ResetCommunication
+				// or internal Communication error
+			case kEplNmtEventResetCom:
+			case kEplNmtEventInternComError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+				// NMT Command Reset Configuration
+			case kEplNmtEventResetConfig:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetConfiguration;
+					break;
+				}
+
+				// NMT Command StopNode
+			case kEplNmtEventStopNode:
+				{
+					// reset flags
+					EPL_MCO_GLB_VAR(m_fEnableReadyToOperate)
+					    = FALSE;
+					EPL_MCO_GLB_VAR(m_fAppReadyToOperate) =
+					    FALSE;
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtCsStopped;
+					break;
+				}
+
+				// error occured
+			case kEplNmtEventNmtCycleError:
+				{
+					// reset flags
+					EPL_MCO_GLB_VAR(m_fEnableReadyToOperate)
+					    = FALSE;
+					EPL_MCO_GLB_VAR(m_fAppReadyToOperate) =
+					    FALSE;
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtCsPreOperational1;
+					break;
+				}
+
+				// check if application is ready to operate
+			case kEplNmtEventEnterReadyToOperate:
+				{
+					// check if command NMTEnableReadyToOperate from MN was received
+					if (EPL_MCO_GLB_VAR(m_fEnableReadyToOperate) == TRUE) {	// reset flags
+						EPL_MCO_GLB_VAR
+						    (m_fEnableReadyToOperate) =
+						    FALSE;
+						EPL_MCO_GLB_VAR
+						    (m_fAppReadyToOperate) =
+						    FALSE;
+						// change state
+						EPL_MCO_GLB_VAR(m_NmtState) =
+						    kEplNmtCsReadyToOperate;
+					} else {	// set Flag
+						EPL_MCO_GLB_VAR
+						    (m_fAppReadyToOperate) =
+						    TRUE;
+					}
+					break;
+				}
+
+				// NMT Commando EnableReadyToOperate
+			case kEplNmtEventEnableReadyToOperate:
+				{
+					// check if application is ready
+					if (EPL_MCO_GLB_VAR(m_fAppReadyToOperate) == TRUE) {	// reset flags
+						EPL_MCO_GLB_VAR
+						    (m_fEnableReadyToOperate) =
+						    FALSE;
+						EPL_MCO_GLB_VAR
+						    (m_fAppReadyToOperate) =
+						    FALSE;
+						// change state
+						EPL_MCO_GLB_VAR(m_NmtState) =
+						    kEplNmtCsReadyToOperate;
+					} else {	// set Flag
+						EPL_MCO_GLB_VAR
+						    (m_fEnableReadyToOperate) =
+						    TRUE;
+					}
+					break;
+				}
+
+			default:
+				{
+					break;
+				}
+
+			}	// end of switch(NmtEvent)
+			break;
+		}
+
+		// node should be configured und application is ready
+	case kEplNmtCsReadyToOperate:
+		{
+			// check events
+			switch (NmtEvent) {
+				// NMT Command SwitchOff
+			case kEplNmtEventCriticalError:
+			case kEplNmtEventSwitchOff:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsOff;
+					break;
+				}
+
+				// NMT Command SwReset
+			case kEplNmtEventSwReset:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsInitialising;
+					break;
+				}
+
+				// NMT Command ResetNode
+			case kEplNmtEventResetNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetApplication;
+					break;
+				}
+
+				// NMT Command ResetCommunication
+				// or internal Communication error
+			case kEplNmtEventResetCom:
+			case kEplNmtEventInternComError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+				// NMT Command ResetConfiguration
+			case kEplNmtEventResetConfig:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetConfiguration;
+					break;
+				}
+
+				// NMT Command StopNode
+			case kEplNmtEventStopNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtCsStopped;
+					break;
+				}
+
+				// error occured
+			case kEplNmtEventNmtCycleError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtCsPreOperational1;
+					break;
+				}
+
+				// NMT Command StartNode
+			case kEplNmtEventStartNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtCsOperational;
+					break;
+				}
+
+			default:
+				{
+					break;
+				}
+
+			}	// end of switch(NmtEvent)
+			break;
+		}
+
+		// normal work state
+	case kEplNmtCsOperational:
+		{
+
+			// check events
+			switch (NmtEvent) {
+				// NMT Command SwitchOff
+			case kEplNmtEventCriticalError:
+			case kEplNmtEventSwitchOff:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsOff;
+					break;
+				}
+
+				// NMT Command SwReset
+			case kEplNmtEventSwReset:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsInitialising;
+					break;
+				}
+
+				// NMT Command ResetNode
+			case kEplNmtEventResetNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetApplication;
+					break;
+				}
+
+				// NMT Command ResetCommunication
+				// or internal Communication error
+			case kEplNmtEventResetCom:
+			case kEplNmtEventInternComError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+				// NMT Command ResetConfiguration
+			case kEplNmtEventResetConfig:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetConfiguration;
+					break;
+				}
+
+				// NMT Command StopNode
+			case kEplNmtEventStopNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtCsStopped;
+					break;
+				}
+
+				// NMT Command EnterPreOperational2
+			case kEplNmtEventEnterPreOperational2:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtCsPreOperational2;
+					break;
+				}
+
+				// error occured
+			case kEplNmtEventNmtCycleError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtCsPreOperational1;
+					break;
+				}
+
+			default:
+				{
+					break;
+				}
+
+			}	// end of switch(NmtEvent)
+			break;
+		}
+
+		// node stopped by MN
+		// -> only process asynchronous frames
+	case kEplNmtCsStopped:
+		{
+			// check events
+			switch (NmtEvent) {
+				// NMT Command SwitchOff
+			case kEplNmtEventCriticalError:
+			case kEplNmtEventSwitchOff:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsOff;
+					break;
+				}
+
+				// NMT Command SwReset
+			case kEplNmtEventSwReset:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsInitialising;
+					break;
+				}
+
+				// NMT Command ResetNode
+			case kEplNmtEventResetNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetApplication;
+					break;
+				}
+
+				// NMT Command ResetCommunication
+				// or internal Communication error
+			case kEplNmtEventResetCom:
+			case kEplNmtEventInternComError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+				// NMT Command ResetConfiguration
+			case kEplNmtEventResetConfig:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetConfiguration;
+					break;
+				}
+
+				// NMT Command EnterPreOperational2
+			case kEplNmtEventEnterPreOperational2:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtCsPreOperational2;
+					break;
+				}
+
+				// error occured
+			case kEplNmtEventNmtCycleError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtCsPreOperational1;
+					break;
+				}
+
+			default:
+				{
+					break;
+				}
+
+			}	// end of switch(NmtEvent)
+			break;
+		}
+
+		// no epl cycle
+		// -> normal ethernet communication
+	case kEplNmtCsBasicEthernet:
+		{
+			// check events
+			switch (NmtEvent) {
+				// NMT Command SwitchOff
+			case kEplNmtEventCriticalError:
+			case kEplNmtEventSwitchOff:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsOff;
+					break;
+				}
+
+				// NMT Command SwReset
+			case kEplNmtEventSwReset:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsInitialising;
+					break;
+				}
+
+				// NMT Command ResetNode
+			case kEplNmtEventResetNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetApplication;
+					break;
+				}
+
+				// NMT Command ResetCommunication
+				// or internal Communication error
+			case kEplNmtEventResetCom:
+			case kEplNmtEventInternComError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+				// NMT Command ResetConfiguration
+			case kEplNmtEventResetConfig:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetConfiguration;
+					break;
+				}
+
+				// error occured
+				// d.k.: how does this error occur? on CRC errors
+/*                case kEplNmtEventNmtCycleError:
+                {
+                    EPL_MCO_GLB_VAR(m_NmtState) = kEplNmtCsPreOperational1;
+                    break;
+                }
+*/
+			case kEplNmtEventDllCeSoc:
+			case kEplNmtEventDllCePreq:
+			case kEplNmtEventDllCePres:
+			case kEplNmtEventDllCeSoa:
+				{	// Epl-Frame on net -> stop any communication
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtCsPreOperational1;
+					break;
+				}
+
+			default:
+				{
+					break;
+				}
+
+			}	// end of switch(NmtEvent)
+
+			break;
+		}
+
+		//-----------------------------------------------------------
+		// MN part of the statemaschine
+
+		// MN listen to network
+		// -> if no EPL traffic go to next state
+	case kEplNmtMsNotActive:
+		{
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) == 0)
+			// no MN functionality
+			// TODO: -create error E_NMT_BA1_NO_MN_SUPPORT
+			EPL_MCO_GLB_VAR(m_fFrozen) = TRUE;
+#else
+
+			// check events
+			switch (NmtEvent) {
+				// NMT Command SwitchOff
+			case kEplNmtEventCriticalError:
+			case kEplNmtEventSwitchOff:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsOff;
+					break;
+				}
+
+				// NMT Command SwReset
+			case kEplNmtEventSwReset:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsInitialising;
+					break;
+				}
+
+				// NMT Command ResetNode
+			case kEplNmtEventResetNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetApplication;
+					break;
+				}
+
+				// NMT Command ResetCommunication
+				// or internal Communication error
+			case kEplNmtEventResetCom:
+			case kEplNmtEventInternComError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+				// NMT Command ResetConfiguration
+			case kEplNmtEventResetConfig:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetConfiguration;
+					break;
+				}
+
+				// EPL frames received
+			case kEplNmtEventDllCeSoc:
+			case kEplNmtEventDllCeSoa:
+				{	// other MN in network
+					// $$$ d.k.: generate error history entry
+					EPL_MCO_GLB_VAR(m_fFrozen) = TRUE;
+					break;
+				}
+
+				// timeout event
+			case kEplNmtEventTimerBasicEthernet:
+				{
+					if (EPL_MCO_GLB_VAR(m_fFrozen) == FALSE) {	// new state BasicEthernet
+						EPL_MCO_GLB_VAR(m_NmtState) =
+						    kEplNmtMsBasicEthernet;
+					}
+					break;
+				}
+
+				// timeout event
+			case kEplNmtEventTimerMsPreOp1:
+				{
+					if (EPL_MCO_GLB_VAR(m_fFrozen) == FALSE) {	// new state PreOp1
+						EPL_MCO_GLB_VAR(m_NmtState) =
+						    kEplNmtMsPreOperational1;
+						EPL_MCO_GLB_VAR
+						    (m_fTimerMsPreOp2) = FALSE;
+						EPL_MCO_GLB_VAR
+						    (m_fAllMandatoryCNIdent) =
+						    FALSE;
+
+					}
+					break;
+				}
+
+			default:
+				{
+					break;
+				}
+
+			}	// end of switch(NmtEvent)
+
+#endif // ((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) == 0)
+
+			break;
+		}
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+		// MN process reduces epl cycle
+	case kEplNmtMsPreOperational1:
+		{
+			// check events
+			switch (NmtEvent) {
+				// NMT Command SwitchOff
+			case kEplNmtEventCriticalError:
+			case kEplNmtEventSwitchOff:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsOff;
+					break;
+				}
+
+				// NMT Command SwReset
+			case kEplNmtEventSwReset:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsInitialising;
+					break;
+				}
+
+				// NMT Command ResetNode
+			case kEplNmtEventResetNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetApplication;
+					break;
+				}
+
+				// NMT Command ResetCommunication
+				// or internal Communication error
+			case kEplNmtEventResetCom:
+			case kEplNmtEventInternComError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+				// NMT Command ResetConfiguration
+			case kEplNmtEventResetConfig:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetConfiguration;
+					break;
+				}
+
+				// EPL frames received
+			case kEplNmtEventDllCeSoc:
+			case kEplNmtEventDllCeSoa:
+				{	// other MN in network
+					// $$$ d.k.: generate error history entry
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+				// error occured
+				// d.k. MSPreOp1->CSPreOp1: nonsense -> keep state
+				/*
+				   case kEplNmtEventNmtCycleError:
+				   {
+				   EPL_MCO_GLB_VAR(m_NmtState) = kEplNmtCsPreOperational1;
+				   break;
+				   }
+				 */
+
+			case kEplNmtEventAllMandatoryCNIdent:
+				{	// all mandatory CN identified
+					if (EPL_MCO_GLB_VAR(m_fTimerMsPreOp2) !=
+					    FALSE) {
+						EPL_MCO_GLB_VAR(m_NmtState) =
+						    kEplNmtMsPreOperational2;
+					} else {
+						EPL_MCO_GLB_VAR
+						    (m_fAllMandatoryCNIdent) =
+						    TRUE;
+					}
+					break;
+				}
+
+			case kEplNmtEventTimerMsPreOp2:
+				{	// residence time for PreOp1 is elapsed
+					if (EPL_MCO_GLB_VAR
+					    (m_fAllMandatoryCNIdent) != FALSE) {
+						EPL_MCO_GLB_VAR(m_NmtState) =
+						    kEplNmtMsPreOperational2;
+					} else {
+						EPL_MCO_GLB_VAR
+						    (m_fTimerMsPreOp2) = TRUE;
+					}
+					break;
+				}
+
+			default:
+				{
+					break;
+				}
+
+			}	// end of switch(NmtEvent)
+			break;
+		}
+
+		// MN process full epl cycle
+	case kEplNmtMsPreOperational2:
+		{
+			// check events
+			switch (NmtEvent) {
+				// NMT Command SwitchOff
+			case kEplNmtEventCriticalError:
+			case kEplNmtEventSwitchOff:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsOff;
+					break;
+				}
+
+				// NMT Command SwReset
+			case kEplNmtEventSwReset:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsInitialising;
+					break;
+				}
+
+				// NMT Command ResetNode
+			case kEplNmtEventResetNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetApplication;
+					break;
+				}
+
+				// NMT Command ResetCommunication
+				// or internal Communication error
+			case kEplNmtEventResetCom:
+			case kEplNmtEventInternComError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+				// NMT Command ResetConfiguration
+			case kEplNmtEventResetConfig:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetConfiguration;
+					break;
+				}
+
+				// EPL frames received
+			case kEplNmtEventDllCeSoc:
+			case kEplNmtEventDllCeSoa:
+				{	// other MN in network
+					// $$$ d.k.: generate error history entry
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+				// error occured
+			case kEplNmtEventNmtCycleError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtMsPreOperational1;
+					break;
+				}
+
+			case kEplNmtEventEnterReadyToOperate:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtMsReadyToOperate;
+					break;
+				}
+
+			default:
+				{
+					break;
+				}
+
+			}	// end of switch(NmtEvent)
+
+			break;
+		}
+
+		// all madatory nodes ready to operate
+		// -> MN process full epl cycle
+	case kEplNmtMsReadyToOperate:
+		{
+
+			// check events
+			switch (NmtEvent) {
+				// NMT Command SwitchOff
+			case kEplNmtEventCriticalError:
+			case kEplNmtEventSwitchOff:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsOff;
+					break;
+				}
+
+				// NMT Command SwReset
+			case kEplNmtEventSwReset:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsInitialising;
+					break;
+				}
+
+				// NMT Command ResetNode
+			case kEplNmtEventResetNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetApplication;
+					break;
+				}
+
+				// NMT Command ResetCommunication
+				// or internal Communication error
+			case kEplNmtEventResetCom:
+			case kEplNmtEventInternComError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+				// NMT Command ResetConfiguration
+			case kEplNmtEventResetConfig:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetConfiguration;
+					break;
+				}
+
+				// EPL frames received
+			case kEplNmtEventDllCeSoc:
+			case kEplNmtEventDllCeSoa:
+				{	// other MN in network
+					// $$$ d.k.: generate error history entry
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+				// error occured
+			case kEplNmtEventNmtCycleError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtMsPreOperational1;
+					break;
+				}
+
+			case kEplNmtEventEnterMsOperational:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtMsOperational;
+					break;
+				}
+
+			default:
+				{
+					break;
+				}
+
+			}	// end of switch(NmtEvent)
+
+			break;
+		}
+
+		// normal eplcycle processing
+	case kEplNmtMsOperational:
+		{
+			// check events
+			switch (NmtEvent) {
+				// NMT Command SwitchOff
+			case kEplNmtEventCriticalError:
+			case kEplNmtEventSwitchOff:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsOff;
+					break;
+				}
+
+				// NMT Command SwReset
+			case kEplNmtEventSwReset:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsInitialising;
+					break;
+				}
+
+				// NMT Command ResetNode
+			case kEplNmtEventResetNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetApplication;
+					break;
+				}
+
+				// NMT Command ResetCommunication
+				// or internal Communication error
+			case kEplNmtEventResetCom:
+			case kEplNmtEventInternComError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+				// NMT Command ResetConfiguration
+			case kEplNmtEventResetConfig:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetConfiguration;
+					break;
+				}
+
+				// EPL frames received
+			case kEplNmtEventDllCeSoc:
+			case kEplNmtEventDllCeSoa:
+				{	// other MN in network
+					// $$$ d.k.: generate error history entry
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+				// error occured
+			case kEplNmtEventNmtCycleError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtMsPreOperational1;
+					break;
+				}
+
+			default:
+				{
+					break;
+				}
+
+			}	// end of switch(NmtEvent)
+			break;
+		}
+
+		//  normal ethernet traffic
+	case kEplNmtMsBasicEthernet:
+		{
+
+			// check events
+			switch (NmtEvent) {
+				// NMT Command SwitchOff
+			case kEplNmtEventCriticalError:
+			case kEplNmtEventSwitchOff:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsOff;
+					break;
+				}
+
+				// NMT Command SwReset
+			case kEplNmtEventSwReset:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsInitialising;
+					break;
+				}
+
+				// NMT Command ResetNode
+			case kEplNmtEventResetNode:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetApplication;
+					break;
+				}
+
+				// NMT Command ResetCommunication
+				// or internal Communication error
+			case kEplNmtEventResetCom:
+			case kEplNmtEventInternComError:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+				// NMT Command ResetConfiguration
+			case kEplNmtEventResetConfig:
+				{
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetConfiguration;
+					break;
+				}
+
+				// EPL frames received
+			case kEplNmtEventDllCeSoc:
+			case kEplNmtEventDllCeSoa:
+				{	// other MN in network
+					// $$$ d.k.: generate error history entry
+					EPL_MCO_GLB_VAR(m_NmtState) =
+					    kEplNmtGsResetCommunication;
+					break;
+				}
+
+				// error occured
+				// d.k. BE->PreOp1 on cycle error? No
+/*                case kEplNmtEventNmtCycleError:
+                {
+                    EPL_MCO_GLB_VAR(m_NmtState) = kEplNmtCsPreOperational1;
+                    break;
+                }
+*/
+			default:
+				{
+					break;
+				}
+
+			}	// end of switch(NmtEvent)
+			break;
+		}
+#endif //#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+	default:
+		{
+			//DEBUG_EPL_DBGLVL_NMTK_TRACE0(EPL_DBGLVL_NMT ,"Error in EplNmtProcess: Unknown NMT-State");
+			//EPL_MCO_GLB_VAR(m_NmtState) = kEplNmtGsResetApplication;
+			Ret = kEplNmtInvalidState;
+			goto Exit;
+		}
+
+	}			// end of switch(NmtEvent)
+
+	// inform higher layer about State-Change if needed
+	if (OldNmtState != EPL_MCO_GLB_VAR(m_NmtState)) {
+		EPL_NMTK_DBG_POST_TRACE_VALUE(NmtEvent, OldNmtState,
+					      EPL_MCO_GLB_VAR(m_NmtState));
+
+		// d.k.: memorize NMT state before posting any events
+		NmtStateChange.m_NewNmtState = EPL_MCO_GLB_VAR(m_NmtState);
+
+		// inform DLL
+		if ((OldNmtState > kEplNmtGsResetConfiguration)
+		    && (EPL_MCO_GLB_VAR(m_NmtState) <=
+			kEplNmtGsResetConfiguration)) {
+			// send DLL DEINIT
+			Event.m_EventSink = kEplEventSinkDllk;
+			Event.m_EventType = kEplEventTypeDllkDestroy;
+			EPL_MEMSET(&Event.m_NetTime, 0x00,
+				   sizeof(Event.m_NetTime));
+			Event.m_pArg = &OldNmtState;
+			Event.m_uiSize = sizeof(OldNmtState);
+			// d.k.: directly call DLLk process function, because
+			//       1. execution of process function is still synchonized and serialized,
+			//       2. it is the same as without event queues (i.e. well tested),
+			//       3. DLLk will get those necessary events even if event queue is full,
+			//       4. event queue is very inefficient
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+			Ret = EplDllkProcess(&Event);
+#else
+			Ret = EplEventkPost(&Event);
+#endif
+		} else if ((OldNmtState <= kEplNmtGsResetConfiguration)
+			   && (EPL_MCO_GLB_VAR(m_NmtState) >
+			       kEplNmtGsResetConfiguration)) {
+			// send DLL INIT
+			Event.m_EventSink = kEplEventSinkDllk;
+			Event.m_EventType = kEplEventTypeDllkCreate;
+			EPL_MEMSET(&Event.m_NetTime, 0x00,
+				   sizeof(Event.m_NetTime));
+			Event.m_pArg = &NmtStateChange.m_NewNmtState;
+			Event.m_uiSize = sizeof(NmtStateChange.m_NewNmtState);
+			// d.k.: directly call DLLk process function, because
+			//       1. execution of process function is still synchonized and serialized,
+			//       2. it is the same as without event queues (i.e. well tested),
+			//       3. DLLk will get those necessary events even if event queue is full
+			//       4. event queue is very inefficient
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+			Ret = EplDllkProcess(&Event);
+#else
+			Ret = EplEventkPost(&Event);
+#endif
+		} else
+		    if ((EPL_MCO_GLB_VAR(m_NmtState) == kEplNmtCsBasicEthernet)
+			|| (EPL_MCO_GLB_VAR(m_NmtState) ==
+			    kEplNmtMsBasicEthernet)) {
+			tEplDllAsyncReqPriority AsyncReqPriority;
+
+			// send DLL Fill Async Tx Buffer, because state BasicEthernet was entered
+			Event.m_EventSink = kEplEventSinkDllk;
+			Event.m_EventType = kEplEventTypeDllkFillTx;
+			EPL_MEMSET(&Event.m_NetTime, 0x00,
+				   sizeof(Event.m_NetTime));
+			AsyncReqPriority = kEplDllAsyncReqPrioGeneric;
+			Event.m_pArg = &AsyncReqPriority;
+			Event.m_uiSize = sizeof(AsyncReqPriority);
+			// d.k.: directly call DLLk process function, because
+			//       1. execution of process function is still synchonized and serialized,
+			//       2. it is the same as without event queues (i.e. well tested),
+			//       3. DLLk will get those necessary events even if event queue is full
+			//       4. event queue is very inefficient
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+			Ret = EplDllkProcess(&Event);
+#else
+			Ret = EplEventkPost(&Event);
+#endif
+		}
+		// inform higher layer about state change
+		NmtStateChange.m_NmtEvent = NmtEvent;
+		Event.m_EventSink = kEplEventSinkNmtu;
+		Event.m_EventType = kEplEventTypeNmtStateChange;
+		EPL_MEMSET(&Event.m_NetTime, 0x00, sizeof(Event.m_NetTime));
+		Event.m_pArg = &NmtStateChange;
+		Event.m_uiSize = sizeof(NmtStateChange);
+		Ret = EplEventkPost(&Event);
+		EPL_DBGLVL_NMTK_TRACE2
+		    ("EplNmtkProcess(NMT-Event = 0x%04X): New NMT-State = 0x%03X\n",
+		     NmtEvent, NmtStateChange.m_NewNmtState);
+
+	}
+
+      Exit:
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtkGetNmtState
+//
+// Description: return the actuell NMT-State and the bits
+//              to for MN- or CN-mode
+//
+//
+//
+// Parameters:  EPL_MCO_DECL_PTR_INSTANCE_PTR_ = Instancepointer
+//
+//
+// Returns:     tEplNmtState = NMT-State
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplNmtState PUBLIC
+EplNmtkGetNmtState(EPL_MCO_DECL_PTR_INSTANCE_PTR)
+{
+	tEplNmtState NmtState;
+
+	NmtState = EPL_MCO_GLB_VAR(m_NmtState);
+
+	return NmtState;
+
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   D E F I N I T I O N S                          //
+//                                                                         //
+//=========================================================================//
+EPL_MCO_DECL_INSTANCE_FCT()
+//---------------------------------------------------------------------------
+//
+// Function:
+//
+// Description:
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#endif // #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTK)) != 0)
+// EOF
diff --git a/drivers/staging/epl/EplNmtkCal.c b/drivers/staging/epl/EplNmtkCal.c
new file mode 100644
index 00000000000..4ad71a71e01
--- /dev/null
+++ b/drivers/staging/epl/EplNmtkCal.c
@@ -0,0 +1,149 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for communication abstraction layer of the
+                NMT-Kernel-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplNmtkCal.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.3 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                KEIL uVision 2
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/16 -k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "kernel/EplNmtkCal.h"
+
+// TODO: init function needed to prepare EplNmtkGetNmtState for
+//       io-controll-call from EplNmtuCal-Modul
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:
+//
+// Description:
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:
+//
+// Description:
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+// EOF
diff --git a/drivers/staging/epl/EplNmtu.c b/drivers/staging/epl/EplNmtu.c
new file mode 100644
index 00000000000..3de16a1eff5
--- /dev/null
+++ b/drivers/staging/epl/EplNmtu.c
@@ -0,0 +1,708 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for NMT-Userspace-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplNmtu.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.8 $  $Date: 2008/11/10 17:17:42 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/09 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "EplInc.h"
+#include "user/EplNmtu.h"
+#include "user/EplObdu.h"
+#include "user/EplTimeru.h"
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTK)) != 0)
+#include "kernel/EplNmtk.h"
+#endif
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTU)) != 0)
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+typedef struct {
+	tEplNmtuStateChangeCallback m_pfnNmtChangeCb;
+	tEplTimerHdl m_TimerHdl;
+
+} tEplNmtuInstance;
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+static tEplNmtuInstance EplNmtuInstance_g;
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtuInit
+//
+// Description: init first instance of the module
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtuInit()
+{
+	tEplKernel Ret;
+
+	Ret = EplNmtuAddInstance();
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtuAddInstance
+//
+// Description: init other instances of the module
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtuAddInstance()
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	EplNmtuInstance_g.m_pfnNmtChangeCb = NULL;
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtuDelInstance
+//
+// Description: delete instance
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtuDelInstance()
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	EplNmtuInstance_g.m_pfnNmtChangeCb = NULL;
+
+	// delete timer
+	Ret = EplTimeruDeleteTimer(&EplNmtuInstance_g.m_TimerHdl);
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtuNmtEvent
+//
+// Description: sends the NMT-Event to the NMT-State-Maschine
+//
+//
+//
+// Parameters:  NmtEvent_p  = NMT-Event to send
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtuNmtEvent(tEplNmtEvent NmtEvent_p)
+{
+	tEplKernel Ret;
+	tEplEvent Event;
+
+	Event.m_EventSink = kEplEventSinkNmtk;
+	Event.m_NetTime.m_dwNanoSec = 0;
+	Event.m_NetTime.m_dwSec = 0;
+	Event.m_EventType = kEplEventTypeNmtEvent;
+	Event.m_pArg = &NmtEvent_p;
+	Event.m_uiSize = sizeof(NmtEvent_p);
+
+	Ret = EplEventuPost(&Event);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtuGetNmtState
+//
+// Description: returns the actuell NMT-State
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplNmtState  = NMT-State
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplNmtState PUBLIC EplNmtuGetNmtState()
+{
+	tEplNmtState NmtState;
+
+	// $$$ call function of communication abstraction layer
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTK)) != 0)
+	NmtState = EplNmtkGetNmtState();
+#else
+	NmtState = 0;
+#endif
+
+	return NmtState;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtuProcessEvent
+//
+// Description: processes events from event queue
+//
+//
+//
+// Parameters:  pEplEvent_p =   pointer to event
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtuProcessEvent(tEplEvent * pEplEvent_p)
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// process event
+	switch (pEplEvent_p->m_EventType) {
+		// state change of NMT-Module
+	case kEplEventTypeNmtStateChange:
+		{
+			tEplEventNmtStateChange *pNmtStateChange;
+
+			// delete timer
+			Ret =
+			    EplTimeruDeleteTimer(&EplNmtuInstance_g.m_TimerHdl);
+
+			pNmtStateChange =
+			    (tEplEventNmtStateChange *) pEplEvent_p->m_pArg;
+
+			// call cb-functions to inform higher layer
+			if (EplNmtuInstance_g.m_pfnNmtChangeCb != NULL) {
+				Ret =
+				    EplNmtuInstance_g.
+				    m_pfnNmtChangeCb(*pNmtStateChange);
+			}
+
+			if (Ret == kEplSuccessful) {	// everything is OK, so switch to next state if necessary
+				switch (pNmtStateChange->m_NewNmtState) {
+					// EPL stack is not running
+				case kEplNmtGsOff:
+					break;
+
+					// first init of the hardware
+				case kEplNmtGsInitialising:
+					{
+						Ret =
+						    EplNmtuNmtEvent
+						    (kEplNmtEventEnterResetApp);
+						break;
+					}
+
+					// init of the manufacturer-specific profile area and the
+					// standardised device profile area
+				case kEplNmtGsResetApplication:
+					{
+						Ret =
+						    EplNmtuNmtEvent
+						    (kEplNmtEventEnterResetCom);
+						break;
+					}
+
+					// init of the communication profile area
+				case kEplNmtGsResetCommunication:
+					{
+						Ret =
+						    EplNmtuNmtEvent
+						    (kEplNmtEventEnterResetConfig);
+						break;
+					}
+
+					// build the configuration with infos from OD
+				case kEplNmtGsResetConfiguration:
+					{
+						unsigned int uiNodeId;
+
+						// get node ID from OD
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0) || (EPL_OBD_USE_KERNEL != FALSE)
+						uiNodeId =
+						    EplObduGetNodeId
+						    (EPL_MCO_PTR_INSTANCE_PTR);
+#else
+						uiNodeId = 0;
+#endif
+						//check node ID if not should be master or slave
+						if (uiNodeId == EPL_C_ADR_MN_DEF_NODE_ID) {	// node shall be MN
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+							Ret =
+							    EplNmtuNmtEvent
+							    (kEplNmtEventEnterMsNotActive);
+#else
+							TRACE0
+							    ("EplNmtuProcess(): no MN functionality implemented\n");
+#endif
+						} else {	// node shall be CN
+							Ret =
+							    EplNmtuNmtEvent
+							    (kEplNmtEventEnterCsNotActive);
+						}
+						break;
+					}
+
+					//-----------------------------------------------------------
+					// CN part of the state machine
+
+					// node listens for EPL-Frames and check timeout
+				case kEplNmtCsNotActive:
+					{
+						DWORD dwBuffer;
+						tEplObdSize ObdSize;
+						tEplTimerArg TimerArg;
+
+						// create timer to switch automatically to BasicEthernet if no MN available in network
+
+						// read NMT_CNBasicEthernetTimerout_U32 from OD
+						ObdSize = sizeof(dwBuffer);
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0) || (EPL_OBD_USE_KERNEL != FALSE)
+						Ret =
+						    EplObduReadEntry
+						    (EPL_MCO_PTR_INSTANCE_PTR_
+						     0x1F99, 0x00, &dwBuffer,
+						     &ObdSize);
+#else
+						Ret = kEplObdIndexNotExist;
+#endif
+						if (Ret != kEplSuccessful) {
+							break;
+						}
+						if (dwBuffer != 0) {	// BasicEthernet is enabled
+							// convert us into ms
+							dwBuffer =
+							    dwBuffer / 1000;
+							if (dwBuffer == 0) {	// timer was below one ms
+								// set one ms
+								dwBuffer = 1;
+							}
+							TimerArg.m_EventSink =
+							    kEplEventSinkNmtk;
+							TimerArg.m_ulArg =
+							    (unsigned long)
+							    kEplNmtEventTimerBasicEthernet;
+							Ret =
+							    EplTimeruModifyTimerMs
+							    (&EplNmtuInstance_g.
+							     m_TimerHdl,
+							     (unsigned long)
+							     dwBuffer,
+							     TimerArg);
+							// potential error is forwarded to event queue which generates error event
+						}
+						break;
+					}
+
+					// node processes only async frames
+				case kEplNmtCsPreOperational1:
+					{
+						break;
+					}
+
+					// node processes isochronous and asynchronous frames
+				case kEplNmtCsPreOperational2:
+					{
+						Ret =
+						    EplNmtuNmtEvent
+						    (kEplNmtEventEnterReadyToOperate);
+						break;
+					}
+
+					// node should be configured und application is ready
+				case kEplNmtCsReadyToOperate:
+					{
+						break;
+					}
+
+					// normal work state
+				case kEplNmtCsOperational:
+					{
+						break;
+					}
+
+					// node stopped by MN
+					// -> only process asynchronous frames
+				case kEplNmtCsStopped:
+					{
+						break;
+					}
+
+					// no EPL cycle
+					// -> normal ethernet communication
+				case kEplNmtCsBasicEthernet:
+					{
+						break;
+					}
+
+					//-----------------------------------------------------------
+					// MN part of the state machine
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+					// node listens for EPL-Frames and check timeout
+				case kEplNmtMsNotActive:
+					{
+						DWORD dwBuffer;
+						tEplObdSize ObdSize;
+						tEplTimerArg TimerArg;
+
+						// create timer to switch automatically to BasicEthernet/PreOp1 if no other MN active in network
+
+						// check NMT_StartUp_U32.Bit13
+						// read NMT_StartUp_U32 from OD
+						ObdSize = sizeof(dwBuffer);
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0) || (EPL_OBD_USE_KERNEL != FALSE)
+						Ret =
+						    EplObduReadEntry
+						    (EPL_MCO_PTR_INSTANCE_PTR_
+						     0x1F80, 0x00, &dwBuffer,
+						     &ObdSize);
+#else
+						Ret = kEplObdIndexNotExist;
+#endif
+						if (Ret != kEplSuccessful) {
+							break;
+						}
+
+						if ((dwBuffer & EPL_NMTST_BASICETHERNET) == 0) {	// NMT_StartUp_U32.Bit13 == 0
+							// new state PreOperational1
+							TimerArg.m_ulArg =
+							    (unsigned long)
+							    kEplNmtEventTimerMsPreOp1;
+						} else {	// NMT_StartUp_U32.Bit13 == 1
+							// new state BasicEthernet
+							TimerArg.m_ulArg =
+							    (unsigned long)
+							    kEplNmtEventTimerBasicEthernet;
+						}
+
+						// read NMT_BootTime_REC.MNWaitNotAct_U32 from OD
+						ObdSize = sizeof(dwBuffer);
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0) || (EPL_OBD_USE_KERNEL != FALSE)
+						Ret =
+						    EplObduReadEntry
+						    (EPL_MCO_PTR_INSTANCE_PTR_
+						     0x1F89, 0x01, &dwBuffer,
+						     &ObdSize);
+#else
+						Ret = kEplObdIndexNotExist;
+#endif
+						if (Ret != kEplSuccessful) {
+							break;
+						}
+						// convert us into ms
+						dwBuffer = dwBuffer / 1000;
+						if (dwBuffer == 0) {	// timer was below one ms
+							// set one ms
+							dwBuffer = 1;
+						}
+						TimerArg.m_EventSink =
+						    kEplEventSinkNmtk;
+						Ret =
+						    EplTimeruModifyTimerMs
+						    (&EplNmtuInstance_g.
+						     m_TimerHdl,
+						     (unsigned long)dwBuffer,
+						     TimerArg);
+						// potential error is forwarded to event queue which generates error event
+						break;
+					}
+
+					// node processes only async frames
+				case kEplNmtMsPreOperational1:
+					{
+						DWORD dwBuffer = 0;
+						tEplObdSize ObdSize;
+						tEplTimerArg TimerArg;
+
+						// create timer to switch automatically to PreOp2 if MN identified all mandatory CNs
+
+						// read NMT_BootTime_REC.MNWaitPreOp1_U32 from OD
+						ObdSize = sizeof(dwBuffer);
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0) || (EPL_OBD_USE_KERNEL != FALSE)
+						Ret =
+						    EplObduReadEntry
+						    (EPL_MCO_PTR_INSTANCE_PTR_
+						     0x1F89, 0x03, &dwBuffer,
+						     &ObdSize);
+						if (Ret != kEplSuccessful) {
+							// ignore error, because this timeout is optional
+							dwBuffer = 0;
+						}
+#endif
+						if (dwBuffer == 0) {	// delay is deactivated
+							// immediately post timer event
+							Ret =
+							    EplNmtuNmtEvent
+							    (kEplNmtEventTimerMsPreOp2);
+							break;
+						}
+						// convert us into ms
+						dwBuffer = dwBuffer / 1000;
+						if (dwBuffer == 0) {	// timer was below one ms
+							// set one ms
+							dwBuffer = 1;
+						}
+						TimerArg.m_EventSink =
+						    kEplEventSinkNmtk;
+						TimerArg.m_ulArg =
+						    (unsigned long)
+						    kEplNmtEventTimerMsPreOp2;
+						Ret =
+						    EplTimeruModifyTimerMs
+						    (&EplNmtuInstance_g.
+						     m_TimerHdl,
+						     (unsigned long)dwBuffer,
+						     TimerArg);
+						// potential error is forwarded to event queue which generates error event
+						break;
+					}
+
+					// node processes isochronous and asynchronous frames
+				case kEplNmtMsPreOperational2:
+					{
+						break;
+					}
+
+					// node should be configured und application is ready
+				case kEplNmtMsReadyToOperate:
+					{
+						break;
+					}
+
+					// normal work state
+				case kEplNmtMsOperational:
+					{
+						break;
+					}
+
+					// no EPL cycle
+					// -> normal ethernet communication
+				case kEplNmtMsBasicEthernet:
+					{
+						break;
+					}
+#endif // (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+				default:
+					{
+						TRACE1
+						    ("EplNmtuProcess(): unhandled NMT state 0x%X\n",
+						     pNmtStateChange->
+						     m_NewNmtState);
+					}
+				}
+			} else if (Ret == kEplReject) {	// application wants to change NMT state itself
+				// it's OK
+				Ret = kEplSuccessful;
+			}
+
+			EPL_DBGLVL_NMTU_TRACE0
+			    ("EplNmtuProcessEvent(): NMT-State-Maschine announce change of NMT State\n");
+			break;
+		}
+
+	default:
+		{
+			Ret = kEplNmtInvalidEvent;
+		}
+
+	}
+
+//Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtuRegisterStateChangeCb
+//
+// Description: register Callback-function go get informed about a
+//              NMT-Change-State-Event
+//
+//
+//
+// Parameters:  pfnEplNmtStateChangeCb_p = functionpointer
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC
+EplNmtuRegisterStateChangeCb(tEplNmtuStateChangeCallback
+			     pfnEplNmtStateChangeCb_p)
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// save callback-function in modul global var
+	EplNmtuInstance_g.m_pfnNmtChangeCb = pfnEplNmtStateChangeCb_p;
+
+	return Ret;
+
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:
+//
+// Description:
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+#endif // #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTU)) != 0)
+
+// EOF
diff --git a/drivers/staging/epl/EplNmtuCal.c b/drivers/staging/epl/EplNmtuCal.c
new file mode 100644
index 00000000000..4a29ef58bf9
--- /dev/null
+++ b/drivers/staging/epl/EplNmtuCal.c
@@ -0,0 +1,158 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for communication abstraction layer of the
+                NMT-Userspace-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplNmtuCal.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                KEIL uVision 2
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/16 -k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "user/EplNmtuCal.h"
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplNmtkCalGetNmtState
+//
+// Description: return current NMT-State
+//              -> encapsulate access to kernelspace
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplNmtState = current NMT-State
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplNmtState PUBLIC EplNmtkCalGetNmtState()
+{
+	tEplNmtState NmtState;
+	// for test direkt call for EplNmtkGetNmtState()
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTK)) != 0)
+	NmtState = EplNmtkGetNmtState();
+#else
+	NmtState = 0;
+#endif
+	return NmtState;
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:
+//
+// Description:
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+// EOF
diff --git a/drivers/staging/epl/EplObd.c b/drivers/staging/epl/EplObd.c
new file mode 100644
index 00000000000..efbb1967a5d
--- /dev/null
+++ b/drivers/staging/epl/EplObd.c
@@ -0,0 +1,3262 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for api function of EplOBD-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplObd.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.12 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                Microsoft VC7
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/02 k.t.:   start of the implementation, version 1.00
+		     ->based on CANopen OBD-Modul
+
+****************************************************************************/
+
+#include "EplInc.h"
+#include "kernel/EplObdk.h"	// function prototyps of the EplOBD-Modul
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+// float definitions and macros
+#define _SHIFTED_EXPONENT_MASK_SP   0xff
+#define _BIAS_SP                    126
+#define T_SP                        23
+#define EXPONENT_DENORM_SP          (-_BIAS_SP)
+#define BASE_TO_THE_T_SP            ((float) 8388608.0)
+#define GET_EXPONENT_SP(x)          ((((x) >> T_SP) & _SHIFTED_EXPONENT_MASK_SP) - _BIAS_SP)
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+// struct for instance table
+INSTANCE_TYPE_BEGIN EPL_MCO_DECL_INSTANCE_MEMBER()
+
+STATIC tEplObdInitParam INST_FAR m_ObdInitParam;
+STATIC tEplObdStoreLoadObjCallback INST_NEAR m_fpStoreLoadObjCallback;
+
+INSTANCE_TYPE_END
+// decomposition of float
+typedef union {
+	tEplObdReal32 m_flRealPart;
+	int m_nIntegerPart;
+
+} tEplObdRealParts;
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+// This macro replace the unspecific pointer to an instance through
+// the modul specific type for the local instance table. This macro
+// must defined in each modul.
+//#define tEplPtrInstance             tEplInstanceInfo MEM*
+
+EPL_MCO_DECL_INSTANCE_VAR()
+
+BYTE MEM abEplObdTrashObject_g[8];
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+EPL_MCO_DEFINE_INSTANCE_FCT()
+
+static tEplKernel EplObdCallObjectCallback(EPL_MCO_DECL_INSTANCE_PTR_
+					   tEplObdCallback fpCallback_p,
+					   tEplObdCbParam MEM * pCbParam_p);
+
+static tEplObdSize EplObdGetDataSizeIntern(tEplObdSubEntryPtr pSubIndexEntry_p);
+
+static tEplObdSize EplObdGetStrLen(void *pObjData_p,
+				   tEplObdSize ObjLen_p, tEplObdType ObjType_p);
+
+#if (EPL_OBD_CHECK_OBJECT_RANGE != FALSE)
+static tEplKernel EplObdCheckObjectRange(tEplObdSubEntryPtr pSubindexEntry_p,
+					 void *pData_p);
+#endif
+
+static tEplKernel EplObdGetVarEntry(tEplObdSubEntryPtr pSubindexEntry_p,
+				    tEplObdVarEntry MEM ** ppVarEntry_p);
+
+static tEplKernel EplObdGetEntry(EPL_MCO_DECL_INSTANCE_PTR_
+				 unsigned int uiIndex_p,
+				 unsigned int uiSubindex_p,
+				 tEplObdEntryPtr * ppObdEntry_p,
+				 tEplObdSubEntryPtr * ppObdSubEntry_p);
+
+static tEplObdSize EplObdGetObjectSize(tEplObdSubEntryPtr pSubIndexEntry_p);
+
+static tEplKernel EplObdGetIndexIntern(tEplObdInitParam MEM * pInitParam_p,
+				       unsigned int uiIndex_p,
+				       tEplObdEntryPtr * ppObdEntry_p);
+
+static tEplKernel EplObdGetSubindexIntern(tEplObdEntryPtr pObdEntry_p,
+					  unsigned int uiSubIndex_p,
+					  tEplObdSubEntryPtr * ppObdSubEntry_p);
+
+static tEplKernel EplObdAccessOdPartIntern(EPL_MCO_DECL_INSTANCE_PTR_
+					   tEplObdPart CurrentOdPart_p,
+					   tEplObdEntryPtr pObdEnty_p,
+					   tEplObdDir Direction_p);
+
+static void *EplObdGetObjectDefaultPtr(tEplObdSubEntryPtr pSubIndexEntry_p);
+static void MEM *EplObdGetObjectCurrentPtr(tEplObdSubEntryPtr pSubIndexEntry_p);
+
+#if (EPL_OBD_USE_STORE_RESTORE != FALSE)
+
+static tEplKernel EplObdCallStoreCallback(EPL_MCO_DECL_INSTANCE_PTR_
+					  tEplObdCbStoreParam MEM *
+					  pCbStoreParam_p);
+
+#endif // (EPL_OBD_USE_STORE_RESTORE != FALSE)
+
+static void EplObdCopyObjectData(void MEM * pDstData_p,
+				 void *pSrcData_p,
+				 tEplObdSize ObjSize_p, tEplObdType ObjType_p);
+
+void *EplObdGetObjectDataPtrIntern(tEplObdSubEntryPtr pSubindexEntry_p);
+
+static tEplKernel EplObdIsNumericalIntern(tEplObdSubEntryPtr pObdSubEntry_p,
+					  BOOL * pfEntryNumerical_p);
+
+static tEplKernel PUBLIC EplObdWriteEntryPre(EPL_MCO_DECL_INSTANCE_PTR_
+					     unsigned int uiIndex_p,
+					     unsigned int uiSubIndex_p,
+					     void *pSrcData_p,
+					     void **ppDstData_p,
+					     tEplObdSize Size_p,
+					     tEplObdEntryPtr * ppObdEntry_p,
+					     tEplObdSubEntryPtr * ppSubEntry_p,
+					     tEplObdCbParam MEM * pCbParam_p,
+					     tEplObdSize * pObdSize_p);
+
+static tEplKernel PUBLIC EplObdWriteEntryPost(EPL_MCO_DECL_INSTANCE_PTR_
+					      tEplObdEntryPtr pObdEntry_p,
+					      tEplObdSubEntryPtr pSubEntry_p,
+					      tEplObdCbParam MEM * pCbParam_p,
+					      void *pSrcData_p,
+					      void *pDstData_p,
+					      tEplObdSize ObdSize_p);
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdInit()
+//
+// Description: initializes the first instance
+//
+// Parameters:  pInitParam_p    = init parameter
+//
+// Return:      tEplKernel      =   errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT tEplKernel PUBLIC EplObdInit(EPL_MCO_DECL_PTR_INSTANCE_PTR_
+					  tEplObdInitParam MEM * pInitParam_p)
+{
+
+	tEplKernel Ret;
+	EPL_MCO_DELETE_INSTANCE_TABLE();
+
+	if (pInitParam_p == NULL) {
+		Ret = kEplSuccessful;
+		goto Exit;
+	}
+
+	Ret = EplObdAddInstance(EPL_MCO_PTR_INSTANCE_PTR_ pInitParam_p);
+
+      Exit:
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdAddInstance()
+//
+// Description: adds a new instance
+//
+// Parameters:  pInitParam_p
+//
+// Return:      tEplKernel
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT tEplKernel PUBLIC EplObdAddInstance(EPL_MCO_DECL_PTR_INSTANCE_PTR_
+						 tEplObdInitParam MEM *
+						 pInitParam_p)
+{
+
+	EPL_MCO_DECL_INSTANCE_PTR_LOCAL tEplKernel Ret;
+
+	// check if pointer to instance pointer valid
+	// get free instance and set the globale instance pointer
+	// set also the instance addr to parameterlist
+	EPL_MCO_CHECK_PTR_INSTANCE_PTR();
+	EPL_MCO_GET_FREE_INSTANCE_PTR();
+	EPL_MCO_SET_PTR_INSTANCE_PTR();
+
+	// save init parameters
+	EPL_MEMCPY(&EPL_MCO_GLB_VAR(m_ObdInitParam), pInitParam_p,
+		   sizeof(tEplObdInitParam));
+
+	// clear callback function for command LOAD and STORE
+	EPL_MCO_GLB_VAR(m_fpStoreLoadObjCallback) = NULL;
+
+	// sign instance as used
+	EPL_MCO_WRITE_INSTANCE_STATE(kStateUsed);
+
+	// initialize object dictionary
+	// so all all VarEntries will be initialized to trash object and default values will be set to current data
+	Ret = EplObdAccessOdPart(EPL_MCO_INSTANCE_PTR_
+				 kEplObdPartAll, kEplObdDirInit);
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdDeleteInstance()
+//
+// Description: delete instance
+//
+// Parameters:  EPL_MCO_DECL_INSTANCE_PTR
+//
+// Return:      tEplKernel
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#if (EPL_USE_DELETEINST_FUNC != FALSE)
+EPLDLLEXPORT tEplKernel PUBLIC EplObdDeleteInstance(EPL_MCO_DECL_INSTANCE_PTR)
+{
+	// check for all API function if instance is valid
+	EPL_MCO_CHECK_INSTANCE_STATE();
+
+	// sign instance as unused
+	EPL_MCO_WRITE_INSTANCE_STATE(kStateUnused);
+
+	return kEplSuccessful;
+
+}
+#endif // (EPL_USE_DELETEINST_FUNC != FALSE)
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdWriteEntry()
+//
+// Description: Function writes data to an OBD entry. Strings
+//              are stored with added '\0' character.
+//
+// Parameters:  EPL_MCO_DECL_INSTANCE_PTR_
+//              uiIndex_p       =   Index of the OD entry
+//              uiSubIndex_p    =   Subindex of the OD Entry
+//              pSrcData_p      =   Pointer to the data to write
+//              Size_p          =   Size of the data in Byte
+//
+// Return:      tEplKernel      =   Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT tEplKernel PUBLIC EplObdWriteEntry(EPL_MCO_DECL_INSTANCE_PTR_
+						unsigned int uiIndex_p,
+						unsigned int uiSubIndex_p,
+						void *pSrcData_p,
+						tEplObdSize Size_p)
+{
+
+	tEplKernel Ret;
+	tEplObdEntryPtr pObdEntry;
+	tEplObdSubEntryPtr pSubEntry;
+	tEplObdCbParam MEM CbParam;
+	void MEM *pDstData;
+	tEplObdSize ObdSize;
+
+	Ret = EplObdWriteEntryPre(EPL_MCO_INSTANCE_PTR_
+				  uiIndex_p,
+				  uiSubIndex_p,
+				  pSrcData_p,
+				  &pDstData,
+				  Size_p,
+				  &pObdEntry, &pSubEntry, &CbParam, &ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	Ret = EplObdWriteEntryPost(EPL_MCO_INSTANCE_PTR_
+				   pObdEntry,
+				   pSubEntry,
+				   &CbParam, pSrcData_p, pDstData, ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+      Exit:
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdReadEntry()
+//
+// Description: The function reads an object entry. The application
+//              can always read the data even if attrib kEplObdAccRead
+//              is not set. The attrib is only checked up for SDO transfer.
+//
+// Parameters:  EPL_MCO_DECL_INSTANCE_PTR_
+//              uiIndex_p       = Index oof the OD entry to read
+//              uiSubIndex_p    = Subindex to read
+//              pDstData_p      = pointer to the buffer for data
+//              Offset_p        = offset in data for read access
+//              pSize_p         = IN: Size of the buffer
+//                                OUT: number of readed Bytes
+//
+// Return:      tEplKernel
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT tEplKernel PUBLIC EplObdReadEntry(EPL_MCO_DECL_INSTANCE_PTR_
+					       unsigned int uiIndex_p,
+					       unsigned int uiSubIndex_p,
+					       void *pDstData_p,
+					       tEplObdSize * pSize_p)
+{
+
+	tEplKernel Ret;
+	tEplObdEntryPtr pObdEntry;
+	tEplObdSubEntryPtr pSubEntry;
+	tEplObdCbParam MEM CbParam;
+	void *pSrcData;
+	tEplObdSize ObdSize;
+
+	// check for all API function if instance is valid
+	EPL_MCO_CHECK_INSTANCE_STATE();
+
+	ASSERT(pDstData_p != NULL);
+	ASSERT(pSize_p != NULL);
+
+	// get address of index and subindex entry
+	Ret = EplObdGetEntry(EPL_MCO_INSTANCE_PTR_
+			     uiIndex_p, uiSubIndex_p, &pObdEntry, &pSubEntry);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// get pointer to object data
+	pSrcData = EplObdGetObjectDataPtrIntern(pSubEntry);
+
+	// check source pointer
+	if (pSrcData == NULL) {
+		Ret = kEplObdReadViolation;
+		goto Exit;
+	}
+	//------------------------------------------------------------------------
+	// address of source data to structure of callback parameters
+	// so callback function can change this data before reading
+	CbParam.m_uiIndex = uiIndex_p;
+	CbParam.m_uiSubIndex = uiSubIndex_p;
+	CbParam.m_pArg = pSrcData;
+	CbParam.m_ObdEvent = kEplObdEvPreRead;
+	Ret = EplObdCallObjectCallback(EPL_MCO_INSTANCE_PTR_
+				       pObdEntry->m_fpCallback, &CbParam);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// get size of data and check if application has reserved enough memory
+	ObdSize = EplObdGetDataSizeIntern(pSubEntry);
+	// check if offset given and calc correct number of bytes to read
+	if (*pSize_p < ObdSize) {
+		Ret = kEplObdValueLengthError;
+		goto Exit;
+	}
+	// read value from object
+	EPL_MEMCPY(pDstData_p, pSrcData, ObdSize);
+	*pSize_p = ObdSize;
+
+	// write address of destination data to structure of callback parameters
+	// so callback function can change this data after reading
+	CbParam.m_pArg = pDstData_p;
+	CbParam.m_ObdEvent = kEplObdEvPostRead;
+	Ret = EplObdCallObjectCallback(EPL_MCO_INSTANCE_PTR_
+				       pObdEntry->m_fpCallback, &CbParam);
+
+      Exit:
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdAccessOdPart()
+//
+// Description: restores default values of one part of OD
+//
+// Parameters:  ObdPart_p
+//              Direction_p
+//
+// Return:      tEplKernel
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT tEplKernel PUBLIC EplObdAccessOdPart(EPL_MCO_DECL_INSTANCE_PTR_
+						  tEplObdPart ObdPart_p,
+						  tEplObdDir Direction_p)
+{
+
+	tEplKernel Ret = kEplSuccessful;
+	BOOL fPartFount;
+	tEplObdEntryPtr pObdEntry;
+
+	// check for all API function if instance is valid
+	EPL_MCO_CHECK_INSTANCE_STATE();
+
+	//  part always has to be unequal to NULL
+	pObdEntry = EPL_MCO_GLB_VAR(m_ObdInitParam.m_pPart);
+	ASSERTMSG(pObdEntry != NULL,
+		  "EplObdAccessOdPart(): no  OD part is defined!\n");
+
+	// if ObdPart_p is not valid fPartFound keeps FALSE and function returns kEplObdIllegalPart
+	fPartFount = FALSE;
+
+	// access to  part
+	if ((ObdPart_p & kEplObdPartGen) != 0) {
+		fPartFount = TRUE;
+
+		Ret = EplObdAccessOdPartIntern(EPL_MCO_INSTANCE_PTR_
+					       kEplObdPartGen, pObdEntry,
+					       Direction_p);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+	}
+	// access to manufacturer part
+	pObdEntry = EPL_MCO_GLB_VAR(m_ObdInitParam.m_pManufacturerPart);
+
+	if (((ObdPart_p & kEplObdPartMan) != 0) && (pObdEntry != NULL)) {
+		fPartFount = TRUE;
+
+		Ret = EplObdAccessOdPartIntern(EPL_MCO_INSTANCE_PTR_
+					       kEplObdPartMan, pObdEntry,
+					       Direction_p);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+	}
+	// access to device part
+	pObdEntry = EPL_MCO_GLB_VAR(m_ObdInitParam.m_pDevicePart);
+
+	if (((ObdPart_p & kEplObdPartDev) != 0) && (pObdEntry != NULL)) {
+		fPartFount = TRUE;
+
+		Ret = EplObdAccessOdPartIntern(EPL_MCO_INSTANCE_PTR_
+					       kEplObdPartDev, pObdEntry,
+					       Direction_p);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+	}
+#if (defined (EPL_OBD_USER_OD) && (EPL_OBD_USER_OD != FALSE))
+	{
+		// access to user part
+		pObdEntry = EPL_MCO_GLB_VAR(m_ObdInitParam.m_pUserPart);
+
+		if (((ObdPart_p & kEplObdPartUsr) != 0) && (pObdEntry != NULL)) {
+			fPartFount = TRUE;
+
+			Ret = EplObdAccessOdPartIntern(EPL_MCO_INSTANCE_PTR_
+						       kEplObdPartUsr,
+						       pObdEntry, Direction_p);
+			if (Ret != kEplSuccessful) {
+				goto Exit;
+			}
+		}
+	}
+#endif
+
+	// no access to an OD part was done? illegal OD part was specified!
+	if (fPartFount == FALSE) {
+		Ret = kEplObdIllegalPart;
+	}
+
+      Exit:
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdDefineVar()
+//
+// Description: defines a variable in OD
+//
+// Parameters:  pEplVarParam_p
+//
+// Return:      tEplKernel
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT tEplKernel PUBLIC EplObdDefineVar(EPL_MCO_DECL_INSTANCE_PTR_
+					       tEplVarParam MEM * pVarParam_p)
+{
+
+	tEplKernel Ret;
+	tEplObdVarEntry MEM *pVarEntry;
+	tEplVarParamValid VarValid;
+	tEplObdSubEntryPtr pSubindexEntry;
+
+	// check for all API function if instance is valid
+	EPL_MCO_CHECK_INSTANCE_STATE();
+
+	ASSERT(pVarParam_p != NULL);	// is not allowed to be NULL
+
+	// get address of subindex entry
+	Ret = EplObdGetEntry(EPL_MCO_INSTANCE_PTR_
+			     pVarParam_p->m_uiIndex,
+			     pVarParam_p->m_uiSubindex, NULL, &pSubindexEntry);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// get var entry
+	Ret = EplObdGetVarEntry(pSubindexEntry, &pVarEntry);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	VarValid = pVarParam_p->m_ValidFlag;
+
+	// copy only this values, which valid flag is set
+	if ((VarValid & kVarValidSize) != 0) {
+		if (pSubindexEntry->m_Type != kEplObdTypDomain) {
+			tEplObdSize DataSize;
+
+			// check passed size parameter
+			DataSize = EplObdGetObjectSize(pSubindexEntry);
+			if (DataSize != pVarParam_p->m_Size) {	// size of variable does not match
+				Ret = kEplObdValueLengthError;
+				goto Exit;
+			}
+		} else {	// size can be set only for objects of type DOMAIN
+			pVarEntry->m_Size = pVarParam_p->m_Size;
+		}
+	}
+
+	if ((VarValid & kVarValidData) != 0) {
+		pVarEntry->m_pData = pVarParam_p->m_pData;
+	}
+/*
+    #if (EPL_PDO_USE_STATIC_MAPPING == FALSE)
+    {
+        if ((VarValid & kVarValidCallback) != 0)
+        {
+           pVarEntry->m_fpCallback = pVarParam_p->m_fpCallback;
+        }
+
+        if ((VarValid & kVarValidArg) != 0)
+        {
+           pVarEntry->m_pArg = pVarParam_p->m_pArg;
+        }
+    }
+    #endif
+*/
+	// Ret is already set to kEplSuccessful from ObdGetVarIntern()
+
+      Exit:
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdGetObjectDataPtr()
+//
+// Description: It returnes the current data pointer. But if object is an
+//              constant object it returnes the default pointer.
+//
+// Parameters:  uiIndex_p    =   Index of the entry
+//              uiSubindex_p =   Subindex of the entry
+//
+// Return:      void *    = pointer to object data
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT void *PUBLIC EplObdGetObjectDataPtr(EPL_MCO_DECL_INSTANCE_PTR_
+						 unsigned int uiIndex_p,
+						 unsigned int uiSubIndex_p)
+{
+	tEplKernel Ret;
+	void *pData;
+	tEplObdEntryPtr pObdEntry;
+	tEplObdSubEntryPtr pObdSubEntry;
+
+	// get pointer to index structure
+	Ret = EplObdGetIndexIntern(&EPL_MCO_GLB_VAR(m_ObdInitParam),
+				   uiIndex_p, &pObdEntry);
+	if (Ret != kEplSuccessful) {
+		pData = NULL;
+		goto Exit;
+	}
+	// get pointer to subindex structure
+	Ret = EplObdGetSubindexIntern(pObdEntry, uiSubIndex_p, &pObdSubEntry);
+	if (Ret != kEplSuccessful) {
+		pData = NULL;
+		goto Exit;
+	}
+	// get Datapointer
+	pData = EplObdGetObjectDataPtrIntern(pObdSubEntry);
+
+      Exit:
+	return pData;
+
+}
+
+#if (defined (EPL_OBD_USER_OD) && (EPL_OBD_USER_OD != FALSE))
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdRegisterUserOd()
+//
+// Description: function registers the user OD
+//
+// Parameters:  pUserOd_p   =pointer to user ODd
+//
+// Return:     tEplKernel = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdRegisterUserOd(EPL_MCO_DECL_INSTANCE_PTR_
+						    tEplObdEntryPtr pUserOd_p)
+{
+
+	EPL_MCO_CHECK_INSTANCE_STATE();
+
+	EPL_MCO_GLB_VAR(m_ObdInitParam.m_pUserPart) = pUserOd_p;
+
+	return kEplSuccessful;
+
+}
+
+#endif
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdInitVarEntry()
+//
+// Description: function to initialize VarEntry dependened on object type
+//
+// Parameters:  pVarEntry_p = pointer to var entry structure
+//              Type_p      = object type
+//              ObdSize_p   = size of object data
+//
+// Returns:     none
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT void PUBLIC EplObdInitVarEntry(EPL_MCO_DECL_INSTANCE_PTR_
+					    tEplObdVarEntry MEM * pVarEntry_p,
+					    tEplObdType Type_p,
+					    tEplObdSize ObdSize_p)
+{
+/*
+    #if (EPL_PDO_USE_STATIC_MAPPING == FALSE)
+    {
+        // reset pointer to VAR callback and argument
+        pVarEntry_p->m_fpCallback  = NULL;
+        pVarEntry_p->m_pArg = NULL;
+    }
+    #endif
+*/
+
+// 10-dec-2004 r.d.: this function will not be used for strings
+	if ((Type_p == kEplObdTypDomain))
+//         (bType_p == kEplObdTypVString) /* ||
+//         (bType_p == kEplObdTypOString) ||
+//         (bType_p == kEplObdTypUString)    */ )
+	{
+		// variables which are defined as DOMAIN or VSTRING should not point to
+		// trash object, because this trash object contains only 8 bytes. DOMAINS or
+		// STRINGS can be longer.
+		pVarEntry_p->m_pData = NULL;
+		pVarEntry_p->m_Size = 0;
+	} else {
+		// set address to variable data to trash object
+		// This prevents an access violation if user forgets to call EplObdDefineVar()
+		// for this variable but mappes it in a PDO.
+		pVarEntry_p->m_pData = &abEplObdTrashObject_g[0];
+		pVarEntry_p->m_Size = ObdSize_p;
+	}
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdGetDataSize()
+//
+// Description: function to initialize VarEntry dependened on object type
+//
+//              gets the data size of an object
+//              for string objects it returnes the string length
+//
+// Parameters:  EPL_MCO_DECL_INSTANCE_PTR_ = Instancepointer
+//              uiIndex_p   =   Index
+//              uiSubIndex_p=   Subindex
+//
+// Return:      tEplObdSize
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplObdSize PUBLIC EplObdGetDataSize(EPL_MCO_DECL_INSTANCE_PTR_
+						  unsigned int uiIndex_p,
+						  unsigned int uiSubIndex_p)
+{
+	tEplKernel Ret;
+	tEplObdSize ObdSize;
+	tEplObdEntryPtr pObdEntry;
+	tEplObdSubEntryPtr pObdSubEntry;
+
+	// get pointer to index structure
+	Ret = EplObdGetIndexIntern(&EPL_MCO_GLB_VAR(m_ObdInitParam),
+				   uiIndex_p, &pObdEntry);
+	if (Ret != kEplSuccessful) {
+		ObdSize = 0;
+		goto Exit;
+	}
+	// get pointer to subindex structure
+	Ret = EplObdGetSubindexIntern(pObdEntry, uiSubIndex_p, &pObdSubEntry);
+	if (Ret != kEplSuccessful) {
+		ObdSize = 0;
+		goto Exit;
+	}
+	// get size
+	ObdSize = EplObdGetDataSizeIntern(pObdSubEntry);
+      Exit:
+	return ObdSize;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdGetNodeId()
+//
+// Description: function returns nodeid from entry 0x1F93
+//
+//
+// Parameters:  EPL_MCO_DECL_INSTANCE_PTR = Instancepointer
+//
+// Return:      unsigned int = Node Id
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT unsigned int PUBLIC EplObdGetNodeId(EPL_MCO_DECL_INSTANCE_PTR)
+{
+	tEplKernel Ret;
+	tEplObdSize ObdSize;
+	BYTE bNodeId;
+
+	bNodeId = 0;
+	ObdSize = sizeof(bNodeId);
+	Ret = EplObdReadEntry(EPL_MCO_PTR_INSTANCE_PTR_
+			      EPL_OBD_NODE_ID_INDEX,
+			      EPL_OBD_NODE_ID_SUBINDEX, &bNodeId, &ObdSize);
+	if (Ret != kEplSuccessful) {
+		bNodeId = EPL_C_ADR_INVALID;
+		goto Exit;
+	}
+
+      Exit:
+	return (unsigned int)bNodeId;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdSetNodeId()
+//
+// Description: function sets nodeid in entry 0x1F93
+//
+//
+// Parameters:  EPL_MCO_DECL_INSTANCE_PTR_ = Instancepointer
+//              uiNodeId_p  =   Node Id to set
+//              NodeIdType_p=   Type on which way the Node Id was set
+//
+// Return:      tEplKernel = Errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdSetNodeId(EPL_MCO_DECL_PTR_INSTANCE_PTR_
+					       unsigned int uiNodeId_p,
+					       tEplObdNodeIdType NodeIdType_p)
+{
+	tEplKernel Ret;
+	tEplObdSize ObdSize;
+	BYTE fHwBool;
+	BYTE bNodeId;
+
+	// check Node Id
+	if (uiNodeId_p == EPL_C_ADR_INVALID) {
+		Ret = kEplInvalidNodeId;
+		goto Exit;
+	}
+	bNodeId = (BYTE) uiNodeId_p;
+	ObdSize = sizeof(BYTE);
+	// write NodeId to OD entry
+	Ret = EplObdWriteEntry(EPL_MCO_PTR_INSTANCE_PTR_
+			       EPL_OBD_NODE_ID_INDEX,
+			       EPL_OBD_NODE_ID_SUBINDEX, &bNodeId, ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// set HWBOOL-Flag in Subindex EPL_OBD_NODE_ID_HWBOOL_SUBINDEX
+	switch (NodeIdType_p) {
+		// type unknown
+	case kEplObdNodeIdUnknown:
+		{
+			fHwBool = OBD_FALSE;
+			break;
+		}
+
+	case kEplObdNodeIdSoftware:
+		{
+			fHwBool = OBD_FALSE;
+			break;
+		}
+
+	case kEplObdNodeIdHardware:
+		{
+			fHwBool = OBD_TRUE;
+			break;
+		}
+
+	default:
+		{
+			fHwBool = OBD_FALSE;
+		}
+
+	}			// end of switch (NodeIdType_p)
+
+	// write flag
+	ObdSize = sizeof(fHwBool);
+	Ret = EplObdWriteEntry(EPL_MCO_PTR_INSTANCE_PTR
+			       EPL_OBD_NODE_ID_INDEX,
+			       EPL_OBD_NODE_ID_HWBOOL_SUBINDEX,
+			       &fHwBool, ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdIsNumerical()
+//
+// Description: function checks if a entry is numerical or not
+//
+//
+// Parameters:  EPL_MCO_DECL_INSTANCE_PTR_ = Instancepointer
+//              uiIndex_p           = Index
+//              uiSubIndex_p        = Subindex
+//              pfEntryNumerical_p  = pointer to BOOL for returnvalue
+//                                  -> TRUE if entry a numerical value
+//                                  -> FALSE if entry not a numerical value
+//
+// Return:      tEplKernel = Errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdIsNumerical(EPL_MCO_DECL_INSTANCE_PTR_
+						 unsigned int uiIndex_p,
+						 unsigned int uiSubIndex_p,
+						 BOOL * pfEntryNumerical_p)
+{
+	tEplKernel Ret;
+	tEplObdEntryPtr pObdEntry;
+	tEplObdSubEntryPtr pObdSubEntry;
+
+	// get pointer to index structure
+	Ret = EplObdGetIndexIntern(&EPL_MCO_GLB_VAR(m_ObdInitParam),
+				   uiIndex_p, &pObdEntry);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// get pointer to subindex structure
+	Ret = EplObdGetSubindexIntern(pObdEntry, uiSubIndex_p, &pObdSubEntry);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	Ret = EplObdIsNumericalIntern(pObdSubEntry, pfEntryNumerical_p);
+
+      Exit:
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdReadEntryToLe()
+//
+// Description: The function reads an object entry from the byteoder
+//              of the system to the little endian byteorder for numerical values.
+//              For other types a normal read will be processed. This is usefull for
+//              the PDO and SDO module. The application
+//              can always read the data even if attrib kEplObdAccRead
+//              is not set. The attrib is only checked up for SDO transfer.
+//
+// Parameters:  EPL_MCO_DECL_INSTANCE_PTR_
+//              uiIndex_p       = Index of the OD entry to read
+//              uiSubIndex_p    = Subindex to read
+//              pDstData_p      = pointer to the buffer for data
+//              Offset_p        = offset in data for read access
+//              pSize_p         = IN: Size of the buffer
+//                                OUT: number of readed Bytes
+//
+// Return:      tEplKernel
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdReadEntryToLe(EPL_MCO_DECL_INSTANCE_PTR_
+						   unsigned int uiIndex_p,
+						   unsigned int uiSubIndex_p,
+						   void *pDstData_p,
+						   tEplObdSize * pSize_p)
+{
+	tEplKernel Ret;
+	tEplObdEntryPtr pObdEntry;
+	tEplObdSubEntryPtr pSubEntry;
+	tEplObdCbParam MEM CbParam;
+	void *pSrcData;
+	tEplObdSize ObdSize;
+
+	// check for all API function if instance is valid
+	EPL_MCO_CHECK_INSTANCE_STATE();
+
+	ASSERT(pDstData_p != NULL);
+	ASSERT(pSize_p != NULL);
+
+	// get address of index and subindex entry
+	Ret = EplObdGetEntry(EPL_MCO_INSTANCE_PTR_
+			     uiIndex_p, uiSubIndex_p, &pObdEntry, &pSubEntry);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// get pointer to object data
+	pSrcData = EplObdGetObjectDataPtrIntern(pSubEntry);
+
+	// check source pointer
+	if (pSrcData == NULL) {
+		Ret = kEplObdReadViolation;
+		goto Exit;
+	}
+	//------------------------------------------------------------------------
+	// address of source data to structure of callback parameters
+	// so callback function can change this data before reading
+	CbParam.m_uiIndex = uiIndex_p;
+	CbParam.m_uiSubIndex = uiSubIndex_p;
+	CbParam.m_pArg = pSrcData;
+	CbParam.m_ObdEvent = kEplObdEvPreRead;
+	Ret = EplObdCallObjectCallback(EPL_MCO_INSTANCE_PTR_
+				       pObdEntry->m_fpCallback, &CbParam);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// get size of data and check if application has reserved enough memory
+	ObdSize = EplObdGetDataSizeIntern(pSubEntry);
+	// check if offset given and calc correct number of bytes to read
+	if (*pSize_p < ObdSize) {
+		Ret = kEplObdValueLengthError;
+		goto Exit;
+	}
+	// check if numerical type
+	switch (pSubEntry->m_Type) {
+		//-----------------------------------------------
+		// types without ami
+	case kEplObdTypVString:
+	case kEplObdTypOString:
+	case kEplObdTypDomain:
+	default:
+		{
+			// read value from object
+			EPL_MEMCPY(pDstData_p, pSrcData, ObdSize);
+			break;
+		}
+
+		//-----------------------------------------------
+		// numerical type which needs ami-write
+		// 8 bit or smaller values
+	case kEplObdTypBool:
+	case kEplObdTypInt8:
+	case kEplObdTypUInt8:
+		{
+			AmiSetByteToLe(pDstData_p, *((BYTE *) pSrcData));
+			break;
+		}
+
+		// 16 bit values
+	case kEplObdTypInt16:
+	case kEplObdTypUInt16:
+		{
+			AmiSetWordToLe(pDstData_p, *((WORD *) pSrcData));
+			break;
+		}
+
+		// 24 bit values
+	case kEplObdTypInt24:
+	case kEplObdTypUInt24:
+		{
+			AmiSetDword24ToLe(pDstData_p, *((DWORD *) pSrcData));
+			break;
+		}
+
+		// 32 bit values
+	case kEplObdTypInt32:
+	case kEplObdTypUInt32:
+	case kEplObdTypReal32:
+		{
+			AmiSetDwordToLe(pDstData_p, *((DWORD *) pSrcData));
+			break;
+		}
+
+		// 40 bit values
+	case kEplObdTypInt40:
+	case kEplObdTypUInt40:
+		{
+			AmiSetQword40ToLe(pDstData_p, *((QWORD *) pSrcData));
+			break;
+		}
+
+		// 48 bit values
+	case kEplObdTypInt48:
+	case kEplObdTypUInt48:
+		{
+			AmiSetQword48ToLe(pDstData_p, *((QWORD *) pSrcData));
+			break;
+		}
+
+		// 56 bit values
+	case kEplObdTypInt56:
+	case kEplObdTypUInt56:
+		{
+			AmiSetQword56ToLe(pDstData_p, *((QWORD *) pSrcData));
+			break;
+		}
+
+		// 64 bit values
+	case kEplObdTypInt64:
+	case kEplObdTypUInt64:
+	case kEplObdTypReal64:
+		{
+			AmiSetQword64ToLe(pDstData_p, *((QWORD *) pSrcData));
+			break;
+		}
+
+		// time of day
+	case kEplObdTypTimeOfDay:
+	case kEplObdTypTimeDiff:
+		{
+			AmiSetTimeOfDay(pDstData_p, ((tTimeOfDay *) pSrcData));
+			break;
+		}
+
+	}			// end of switch(pSubEntry->m_Type)
+
+	*pSize_p = ObdSize;
+
+	// write address of destination data to structure of callback parameters
+	// so callback function can change this data after reading
+	CbParam.m_pArg = pDstData_p;
+	CbParam.m_ObdEvent = kEplObdEvPostRead;
+	Ret = EplObdCallObjectCallback(EPL_MCO_INSTANCE_PTR_
+				       pObdEntry->m_fpCallback, &CbParam);
+
+      Exit:
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdWriteEntryFromLe()
+//
+// Description: Function writes data to an OBD entry from a source with
+//              little endian byteorder to the od with system specuific
+//              byteorder. Not numerical values will only by copied. Strings
+//              are stored with added '\0' character.
+//
+// Parameters:  EPL_MCO_DECL_INSTANCE_PTR_
+//              uiIndex_p       =   Index of the OD entry
+//              uiSubIndex_p    =   Subindex of the OD Entry
+//              pSrcData_p      =   Pointer to the data to write
+//              Size_p          =   Size of the data in Byte
+//
+// Return:      tEplKernel      =   Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdWriteEntryFromLe(EPL_MCO_DECL_INSTANCE_PTR_
+						      unsigned int uiIndex_p,
+						      unsigned int uiSubIndex_p,
+						      void *pSrcData_p,
+						      tEplObdSize Size_p)
+{
+	tEplKernel Ret;
+	tEplObdEntryPtr pObdEntry;
+	tEplObdSubEntryPtr pSubEntry;
+	tEplObdCbParam MEM CbParam;
+	void MEM *pDstData;
+	tEplObdSize ObdSize;
+	QWORD qwBuffer;
+	void *pBuffer = &qwBuffer;
+
+	Ret = EplObdWriteEntryPre(EPL_MCO_INSTANCE_PTR_
+				  uiIndex_p,
+				  uiSubIndex_p,
+				  pSrcData_p,
+				  &pDstData,
+				  Size_p,
+				  &pObdEntry, &pSubEntry, &CbParam, &ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	// check if numerical type
+	switch (pSubEntry->m_Type) {
+		//-----------------------------------------------
+		// types without ami
+	default:
+		{		// do nothing, i.e. use the given source pointer
+			pBuffer = pSrcData_p;
+			break;
+		}
+
+		//-----------------------------------------------
+		// numerical type which needs ami-write
+		// 8 bit or smaller values
+	case kEplObdTypBool:
+	case kEplObdTypInt8:
+	case kEplObdTypUInt8:
+		{
+			*((BYTE *) pBuffer) = AmiGetByteFromLe(pSrcData_p);
+			break;
+		}
+
+		// 16 bit values
+	case kEplObdTypInt16:
+	case kEplObdTypUInt16:
+		{
+			*((WORD *) pBuffer) = AmiGetWordFromLe(pSrcData_p);
+			break;
+		}
+
+		// 24 bit values
+	case kEplObdTypInt24:
+	case kEplObdTypUInt24:
+		{
+			*((DWORD *) pBuffer) = AmiGetDword24FromLe(pSrcData_p);
+			break;
+		}
+
+		// 32 bit values
+	case kEplObdTypInt32:
+	case kEplObdTypUInt32:
+	case kEplObdTypReal32:
+		{
+			*((DWORD *) pBuffer) = AmiGetDwordFromLe(pSrcData_p);
+			break;
+		}
+
+		// 40 bit values
+	case kEplObdTypInt40:
+	case kEplObdTypUInt40:
+		{
+			*((QWORD *) pBuffer) = AmiGetQword40FromLe(pSrcData_p);
+			break;
+		}
+
+		// 48 bit values
+	case kEplObdTypInt48:
+	case kEplObdTypUInt48:
+		{
+			*((QWORD *) pBuffer) = AmiGetQword48FromLe(pSrcData_p);
+			break;
+		}
+
+		// 56 bit values
+	case kEplObdTypInt56:
+	case kEplObdTypUInt56:
+		{
+			*((QWORD *) pBuffer) = AmiGetQword56FromLe(pSrcData_p);
+			break;
+		}
+
+		// 64 bit values
+	case kEplObdTypInt64:
+	case kEplObdTypUInt64:
+	case kEplObdTypReal64:
+		{
+			*((QWORD *) pBuffer) = AmiGetQword64FromLe(pSrcData_p);
+			break;
+		}
+
+		// time of day
+	case kEplObdTypTimeOfDay:
+	case kEplObdTypTimeDiff:
+		{
+			AmiGetTimeOfDay(pBuffer, ((tTimeOfDay *) pSrcData_p));
+			break;
+		}
+
+	}			// end of switch(pSubEntry->m_Type)
+
+	Ret = EplObdWriteEntryPost(EPL_MCO_INSTANCE_PTR_
+				   pObdEntry,
+				   pSubEntry,
+				   &CbParam, pBuffer, pDstData, ObdSize);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+      Exit:
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdGetAccessType()
+//
+// Description: Function returns accesstype of the entry
+//
+// Parameters:  EPL_MCO_DECL_INSTANCE_PTR_
+//              uiIndex_p       =   Index of the OD entry
+//              uiSubIndex_p    =   Subindex of the OD Entry
+//              pAccessTyp_p    =   pointer to buffer to store accesstype
+//
+// Return:      tEplKernel     =   errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdGetAccessType(EPL_MCO_DECL_INSTANCE_PTR_
+						   unsigned int uiIndex_p,
+						   unsigned int uiSubIndex_p,
+						   tEplObdAccess * pAccessTyp_p)
+{
+	tEplKernel Ret;
+	tEplObdEntryPtr pObdEntry;
+	tEplObdSubEntryPtr pObdSubEntry;
+
+	// get pointer to index structure
+	Ret = EplObdGetIndexIntern(&EPL_MCO_GLB_VAR(m_ObdInitParam),
+				   uiIndex_p, &pObdEntry);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// get pointer to subindex structure
+	Ret = EplObdGetSubindexIntern(pObdEntry, uiSubIndex_p, &pObdSubEntry);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// get accessType
+	*pAccessTyp_p = pObdSubEntry->m_Access;
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdSearchVarEntry()
+//
+// Description: gets variable from OD
+//
+// Parameters:  uiIndex_p       =   index of the var entry to search
+//              uiSubindex_p    =   subindex of var entry to search
+//              ppVarEntry_p    =   pointer to the pointer to the varentry
+//
+// Return:      tEplKernel
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplObdSearchVarEntry(EPL_MCO_DECL_INSTANCE_PTR_
+				       unsigned int uiIndex_p,
+				       unsigned int uiSubindex_p,
+				       tEplObdVarEntry MEM ** ppVarEntry_p)
+{
+
+	tEplKernel Ret;
+	tEplObdSubEntryPtr pSubindexEntry;
+
+	// check for all API function if instance is valid
+	EPL_MCO_CHECK_INSTANCE_STATE();
+
+	// get address of subindex entry
+	Ret = EplObdGetEntry(EPL_MCO_INSTANCE_PTR_
+			     uiIndex_p, uiSubindex_p, NULL, &pSubindexEntry);
+	if (Ret == kEplSuccessful) {
+		// get var entry
+		Ret = EplObdGetVarEntry(pSubindexEntry, ppVarEntry_p);
+	}
+
+	return Ret;
+
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   D E F I N I T I O N S                          //
+//                                                                         //
+//=========================================================================//
+
+EPL_MCO_DECL_INSTANCE_FCT()
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdCallObjectCallback()
+//
+// Description: calls callback function of an object or of a variable
+//
+// Parameters:  fpCallback_p
+//              pCbParam_p
+//
+// Return:      tEplKernel
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static tEplKernel EplObdCallObjectCallback(EPL_MCO_DECL_INSTANCE_PTR_
+					   tEplObdCallback fpCallback_p,
+					   tEplObdCbParam MEM * pCbParam_p)
+{
+
+	tEplKernel Ret;
+	tEplObdCallback MEM fpCallback;
+
+	// check for all API function if instance is valid
+	EPL_MCO_CHECK_INSTANCE_STATE();
+
+	ASSERT(pCbParam_p != NULL);
+
+	Ret = kEplSuccessful;
+
+	// check address of callback function before calling it
+	if (fpCallback_p != NULL) {
+		// KEIL C51 V6.01 has a bug.
+		// Therefore the parameter fpCallback_p has to be copied in local variable fpCallback.
+		fpCallback = fpCallback_p;
+
+		// call callback function for this object
+		Ret = fpCallback(EPL_MCO_INSTANCE_PARAM_IDX_()
+				 pCbParam_p);
+	}
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdGetDataSizeIntern()
+//
+// Description: gets the data size of an object
+//              for string objects it returnes the string length
+//
+// Parameters:  pSubIndexEntry_p
+//
+// Return:      tEplObdSize
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplObdSize EplObdGetDataSizeIntern(tEplObdSubEntryPtr pSubIndexEntry_p)
+{
+
+	tEplObdSize DataSize;
+	void MEM *pData;
+
+	// If OD entry is defined by macro EPL_OBD_SUBINDEX_ROM_VSTRING
+	// then the current pointer is always NULL. The function
+	// returns the length of default string.
+	DataSize = EplObdGetObjectSize(pSubIndexEntry_p);
+
+	if (pSubIndexEntry_p->m_Type == kEplObdTypVString) {
+		// The pointer to current value can be received from EplObdGetObjectCurrentPtr()
+		pData =
+		    ((void MEM *)EplObdGetObjectCurrentPtr(pSubIndexEntry_p));
+		if (pData != NULL) {
+			DataSize =
+			    EplObdGetStrLen((void *)pData, DataSize,
+					    pSubIndexEntry_p->m_Type);
+		}
+
+	}
+
+	return DataSize;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdGetStrLen()
+//
+// Description: The function calculates the length of string. The '\0'
+//              character is included!!
+//
+// Parameters:  pObjData_p          = pointer to string
+//              ObjLen_p            = max. length of objectr entry
+//              bObjType_p          = object type (VSTRING, ...)
+//
+// Returns:     string length + 1
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplObdSize EplObdGetStrLen(void *pObjData_p,
+				   tEplObdSize ObjLen_p, tEplObdType ObjType_p)
+{
+
+	tEplObdSize StrLen = 0;
+	BYTE *pbString;
+
+	if (pObjData_p == NULL) {
+		goto Exit;
+	}
+	//----------------------------------------
+	// Visible String: data format byte
+	if (ObjType_p == kEplObdTypVString) {
+		pbString = pObjData_p;
+
+		for (StrLen = 0; StrLen < ObjLen_p; StrLen++) {
+			if (*pbString == '\0') {
+				StrLen++;
+				break;
+			}
+
+			pbString++;
+		}
+	}
+	//----------------------------------------
+	// other string types ...
+
+      Exit:
+	return (StrLen);
+
+}
+
+#if (EPL_OBD_CHECK_OBJECT_RANGE != FALSE)
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdCheckObjectRange()
+//
+// Description: function to check value range of object data
+//
+// NOTICE: The pointer of data (pData_p) must point out to an even address,
+//         if ObjType is unequal to kEplObdTypInt8 or kEplObdTypUInt8! But it is
+//         always realiced because pointer m_pDefault points always to an
+//         array of the SPECIFIED type.
+//
+// Parameters:  pSubindexEntry_p
+//              pData_p
+//
+// Return:      tEplKernel
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplObdCheckObjectRange(tEplObdSubEntryPtr pSubindexEntry_p,
+					 void *pData_p)
+{
+
+	tEplKernel Ret;
+	void *pRangeData;
+
+	ASSERTMSG(pSubindexEntry_p != NULL,
+		  "EplObdCheckObjectRange(): no address to subindex struct!\n");
+
+	Ret = kEplSuccessful;
+
+	// check if data range has to be checked
+	if ((pSubindexEntry_p->m_Access & kEplObdAccRange) == 0) {
+		goto Exit;
+	}
+	// get address of default data
+	pRangeData = pSubindexEntry_p->m_pDefault;
+
+	// jump to called object type
+	switch ((tEplObdType) pSubindexEntry_p->m_Type) {
+		// -----------------------------------------------------------------
+		// ObdType kEplObdTypBool will not be checked because there are only
+		// two possible values 0 or 1.
+
+		// -----------------------------------------------------------------
+		// ObdTypes which has to be check up because numerical values
+	case kEplObdTypInt8:
+
+		// switch to lower limit
+		pRangeData = ((tEplObdInteger8 *) pRangeData) + 1;
+
+		// check if value is to low
+		if (*((tEplObdInteger8 *) pData_p) <
+		    *((tEplObdInteger8 *) pRangeData)) {
+			Ret = kEplObdValueTooLow;
+			break;
+		}
+		// switch to higher limit
+		pRangeData = ((tEplObdInteger8 *) pRangeData) + 1;
+
+		// check if value is to high
+		if (*((tEplObdInteger8 *) pData_p) >
+		    *((tEplObdInteger8 *) pRangeData)) {
+			Ret = kEplObdValueTooHigh;
+		}
+
+		break;
+
+	case kEplObdTypUInt8:
+
+		// switch to lower limit
+		pRangeData = ((tEplObdUnsigned8 *) pRangeData) + 1;
+
+		// check if value is to low
+		if (*((tEplObdUnsigned8 *) pData_p) <
+		    *((tEplObdUnsigned8 *) pRangeData)) {
+			Ret = kEplObdValueTooLow;
+			break;
+		}
+		// switch to higher limit
+		pRangeData = ((tEplObdUnsigned8 *) pRangeData) + 1;
+
+		// check if value is to high
+		if (*((tEplObdUnsigned8 *) pData_p) >
+		    *((tEplObdUnsigned8 *) pRangeData)) {
+			Ret = kEplObdValueTooHigh;
+		}
+
+		break;
+
+	case kEplObdTypInt16:
+
+		// switch to lower limit
+		pRangeData = ((tEplObdInteger16 *) pRangeData) + 1;
+
+		// check if value is to low
+		if (*((tEplObdInteger16 *) pData_p) <
+		    *((tEplObdInteger16 *) pRangeData)) {
+			Ret = kEplObdValueTooLow;
+			break;
+		}
+		// switch to higher limit
+		pRangeData = ((tEplObdInteger16 *) pRangeData) + 1;
+
+		// check if value is to high
+		if (*((tEplObdInteger16 *) pData_p) >
+		    *((tEplObdInteger16 *) pRangeData)) {
+			Ret = kEplObdValueTooHigh;
+		}
+
+		break;
+
+	case kEplObdTypUInt16:
+
+		// switch to lower limit
+		pRangeData = ((tEplObdUnsigned16 *) pRangeData) + 1;
+
+		// check if value is to low
+		if (*((tEplObdUnsigned16 *) pData_p) <
+		    *((tEplObdUnsigned16 *) pRangeData)) {
+			Ret = kEplObdValueTooLow;
+			break;
+		}
+		// switch to higher limit
+		pRangeData = ((tEplObdUnsigned16 *) pRangeData) + 1;
+
+		// check if value is to high
+		if (*((tEplObdUnsigned16 *) pData_p) >
+		    *((tEplObdUnsigned16 *) pRangeData)) {
+			Ret = kEplObdValueTooHigh;
+		}
+
+		break;
+
+	case kEplObdTypInt32:
+
+		// switch to lower limit
+		pRangeData = ((tEplObdInteger32 *) pRangeData) + 1;
+
+		// check if value is to low
+		if (*((tEplObdInteger32 *) pData_p) <
+		    *((tEplObdInteger32 *) pRangeData)) {
+			Ret = kEplObdValueTooLow;
+			break;
+		}
+		// switch to higher limit
+		pRangeData = ((tEplObdInteger32 *) pRangeData) + 1;
+
+		// check if value is to high
+		if (*((tEplObdInteger32 *) pData_p) >
+		    *((tEplObdInteger32 *) pRangeData)) {
+			Ret = kEplObdValueTooHigh;
+		}
+
+		break;
+
+	case kEplObdTypUInt32:
+
+		// switch to lower limit
+		pRangeData = ((tEplObdUnsigned32 *) pRangeData) + 1;
+
+		// check if value is to low
+		if (*((tEplObdUnsigned32 *) pData_p) <
+		    *((tEplObdUnsigned32 *) pRangeData)) {
+			Ret = kEplObdValueTooLow;
+			break;
+		}
+		// switch to higher limit
+		pRangeData = ((tEplObdUnsigned32 *) pRangeData) + 1;
+
+		// check if value is to high
+		if (*((tEplObdUnsigned32 *) pData_p) >
+		    *((tEplObdUnsigned32 *) pRangeData)) {
+			Ret = kEplObdValueTooHigh;
+		}
+
+		break;
+
+	case kEplObdTypReal32:
+
+		// switch to lower limit
+		pRangeData = ((tEplObdReal32 *) pRangeData) + 1;
+
+		// check if value is to low
+		if (*((tEplObdReal32 *) pData_p) <
+		    *((tEplObdReal32 *) pRangeData)) {
+			Ret = kEplObdValueTooLow;
+			break;
+		}
+		// switch to higher limit
+		pRangeData = ((tEplObdReal32 *) pRangeData) + 1;
+
+		// check if value is to high
+		if (*((tEplObdReal32 *) pData_p) >
+		    *((tEplObdReal32 *) pRangeData)) {
+			Ret = kEplObdValueTooHigh;
+		}
+
+		break;
+
+		// -----------------------------------------------------------------
+	case kEplObdTypInt40:
+	case kEplObdTypInt48:
+	case kEplObdTypInt56:
+	case kEplObdTypInt64:
+
+		// switch to lower limit
+		pRangeData = ((signed QWORD *)pRangeData) + 1;
+
+		// check if value is to low
+		if (*((signed QWORD *)pData_p) < *((signed QWORD *)pRangeData)) {
+			Ret = kEplObdValueTooLow;
+			break;
+		}
+		// switch to higher limit
+		pRangeData = ((signed QWORD *)pRangeData) + 1;
+
+		// check if value is to high
+		if (*((signed QWORD *)pData_p) > *((signed QWORD *)pRangeData)) {
+			Ret = kEplObdValueTooHigh;
+		}
+
+		break;
+
+		// -----------------------------------------------------------------
+	case kEplObdTypUInt40:
+	case kEplObdTypUInt48:
+	case kEplObdTypUInt56:
+	case kEplObdTypUInt64:
+
+		// switch to lower limit
+		pRangeData = ((unsigned QWORD *)pRangeData) + 1;
+
+		// check if value is to low
+		if (*((unsigned QWORD *)pData_p) <
+		    *((unsigned QWORD *)pRangeData)) {
+			Ret = kEplObdValueTooLow;
+			break;
+		}
+		// switch to higher limit
+		pRangeData = ((unsigned QWORD *)pRangeData) + 1;
+
+		// check if value is to high
+		if (*((unsigned QWORD *)pData_p) >
+		    *((unsigned QWORD *)pRangeData)) {
+			Ret = kEplObdValueTooHigh;
+		}
+
+		break;
+
+		// -----------------------------------------------------------------
+	case kEplObdTypReal64:
+
+		// switch to lower limit
+		pRangeData = ((tEplObdReal64 *) pRangeData) + 1;
+
+		// check if value is to low
+		if (*((tEplObdReal64 *) pData_p) <
+		    *((tEplObdReal64 *) pRangeData)) {
+			Ret = kEplObdValueTooLow;
+			break;
+		}
+		// switch to higher limit
+		pRangeData = ((tEplObdReal64 *) pRangeData) + 1;
+
+		// check if value is to high
+		if (*((tEplObdReal64 *) pData_p) >
+		    *((tEplObdReal64 *) pRangeData)) {
+			Ret = kEplObdValueTooHigh;
+		}
+
+		break;
+
+		// -----------------------------------------------------------------
+	case kEplObdTypTimeOfDay:
+	case kEplObdTypTimeDiff:
+		break;
+
+		// -----------------------------------------------------------------
+		// ObdTypes kEplObdTypXString and kEplObdTypDomain can not be checkt because
+		// they have no numerical value.
+	default:
+
+		Ret = kEplObdUnknownObjectType;
+		break;
+	}
+
+      Exit:
+
+	return Ret;
+
+}
+#endif // (EPL_OBD_CHECK_OBJECT_RANGE != FALSE)
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdWriteEntryPre()
+//
+// Description: Function prepares write of data to an OBD entry. Strings
+//              are stored with added '\0' character.
+//
+// Parameters:  EPL_MCO_DECL_INSTANCE_PTR_
+//              uiIndex_p       =   Index of the OD entry
+//              uiSubIndex_p    =   Subindex of the OD Entry
+//              pSrcData_p      =   Pointer to the data to write
+//              Size_p          =   Size of the data in Byte
+//
+// Return:      tEplKernel      =   Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel PUBLIC EplObdWriteEntryPre(EPL_MCO_DECL_INSTANCE_PTR_
+					     unsigned int uiIndex_p,
+					     unsigned int uiSubIndex_p,
+					     void *pSrcData_p,
+					     void **ppDstData_p,
+					     tEplObdSize Size_p,
+					     tEplObdEntryPtr * ppObdEntry_p,
+					     tEplObdSubEntryPtr * ppSubEntry_p,
+					     tEplObdCbParam MEM * pCbParam_p,
+					     tEplObdSize * pObdSize_p)
+{
+
+	tEplKernel Ret;
+	tEplObdEntryPtr pObdEntry;
+	tEplObdSubEntryPtr pSubEntry;
+	tEplObdAccess Access;
+	void MEM *pDstData;
+	tEplObdSize ObdSize;
+	BOOL fEntryNumerical;
+
+#if (EPL_OBD_USE_STRING_DOMAIN_IN_RAM != FALSE)
+	tEplObdVStringDomain MEM MemVStringDomain;
+	void MEM *pCurrData;
+#endif
+
+	// check for all API function if instance is valid
+	EPL_MCO_CHECK_INSTANCE_STATE();
+
+	ASSERT(pSrcData_p != NULL);	// should never be NULL
+
+	//------------------------------------------------------------------------
+	// get address of index and subindex entry
+	Ret = EplObdGetEntry(EPL_MCO_INSTANCE_PTR_
+			     uiIndex_p, uiSubIndex_p, &pObdEntry, &pSubEntry);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// get pointer to object data
+	pDstData = (void MEM *)EplObdGetObjectDataPtrIntern(pSubEntry);
+
+	Access = (tEplObdAccess) pSubEntry->m_Access;
+
+	// check access for write
+	// access violation if adress to current value is NULL
+	if (((Access & kEplObdAccConst) != 0) || (pDstData == NULL)) {
+		Ret = kEplObdAccessViolation;
+		goto Exit;
+	}
+	//------------------------------------------------------------------------
+	// get size of object
+	// -as ObdSize = ObdGetObjectSize (pSubEntry);
+
+	//------------------------------------------------------------------------
+	// To use the same callback function for ObdWriteEntry as well as for
+	// an SDO download call at first (kEplObdEvPre...) the callback function
+	// with the argument pointer to object size.
+	pCbParam_p->m_uiIndex = uiIndex_p;
+	pCbParam_p->m_uiSubIndex = uiSubIndex_p;
+
+	// Because object size and object pointer are
+	// adapted by user callback function, re-read
+	// this values.
+	ObdSize = EplObdGetObjectSize(pSubEntry);
+	pDstData = (void MEM *)EplObdGetObjectDataPtrIntern(pSubEntry);
+
+	// 09-dec-2004 r.d.:
+	//      Function EplObdWriteEntry() calls new event kEplObdEvWrStringDomain
+	//      for String or Domain which lets called module directly change
+	//      the data pointer or size. This prevents a recursive call to
+	//      the callback function if it calls EplObdGetEntry().
+#if (EPL_OBD_USE_STRING_DOMAIN_IN_RAM != FALSE)
+	if ((pSubEntry->m_Type == kEplObdTypVString) ||
+	    (pSubEntry->m_Type == kEplObdTypDomain) ||
+	    (pSubEntry->m_Type == kEplObdTypOString)) {
+		if (pSubEntry->m_Type == kEplObdTypVString) {
+			// reserve one byte for 0-termination
+			// -as ObdSize -= 1;
+			Size_p += 1;
+		}
+		// fill out new arg-struct
+		MemVStringDomain.m_DownloadSize = Size_p;
+		MemVStringDomain.m_ObjSize = ObdSize;
+		MemVStringDomain.m_pData = pDstData;
+
+		pCbParam_p->m_ObdEvent = kEplObdEvWrStringDomain;
+		pCbParam_p->m_pArg = &MemVStringDomain;
+		//  call user callback
+		Ret = EplObdCallObjectCallback(EPL_MCO_INSTANCE_PTR_
+					       pObdEntry->m_fpCallback,
+					       pCbParam_p);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+		// write back new settings
+		pCurrData = pSubEntry->m_pCurrent;
+		if ((pSubEntry->m_Type == kEplObdTypVString)
+		    || (pSubEntry->m_Type == kEplObdTypOString)) {
+			((tEplObdVString MEM *) pCurrData)->m_Size =
+			    MemVStringDomain.m_ObjSize;
+			((tEplObdVString MEM *) pCurrData)->m_pString =
+			    MemVStringDomain.m_pData;
+		} else		// if (pSdosTableEntry_p->m_bObjType == kEplObdTypDomain)
+		{
+			((tEplObdVarEntry MEM *) pCurrData)->m_Size =
+			    MemVStringDomain.m_ObjSize;
+			((tEplObdVarEntry MEM *) pCurrData)->m_pData =
+			    (void MEM *)MemVStringDomain.m_pData;
+		}
+
+		// Because object size and object pointer are
+		// adapted by user callback function, re-read
+		// this values.
+		ObdSize = MemVStringDomain.m_ObjSize;
+		pDstData = (void MEM *)MemVStringDomain.m_pData;
+	}
+#endif //#if (OBD_USE_STRING_DOMAIN_IN_RAM != FALSE)
+
+	// 07-dec-2004 r.d.: size from application is needed because callback function can change the object size
+	// -as 16.11.04 CbParam.m_pArg     = &ObdSize;
+	// 09-dec-2004 r.d.: CbParam.m_pArg     = &Size_p;
+	pCbParam_p->m_pArg = &ObdSize;
+	pCbParam_p->m_ObdEvent = kEplObdEvInitWrite;
+	Ret = EplObdCallObjectCallback(EPL_MCO_INSTANCE_PTR_
+				       pObdEntry->m_fpCallback, pCbParam_p);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	if (Size_p > ObdSize) {
+		Ret = kEplObdValueLengthError;
+		goto Exit;
+	}
+
+	if (pSubEntry->m_Type == kEplObdTypVString) {
+		if (((char MEM *)pSrcData_p)[Size_p - 1] == '\0') {	// last byte of source string contains null character
+
+			// reserve one byte in destination for 0-termination
+			Size_p -= 1;
+		} else if (Size_p >= ObdSize) {	// source string is not 0-terminated
+			// and destination buffer is too short
+			Ret = kEplObdValueLengthError;
+			goto Exit;
+		}
+	}
+
+	Ret = EplObdIsNumericalIntern(pSubEntry, &fEntryNumerical);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+	if ((fEntryNumerical != FALSE)
+	    && (Size_p != ObdSize)) {
+		// type is numerical, therefor size has to fit, but it does not.
+		Ret = kEplObdValueLengthError;
+		goto Exit;
+	}
+	// use given size, because non-numerical objects can be written with shorter values
+	ObdSize = Size_p;
+
+	// set output parameters
+	*pObdSize_p = ObdSize;
+	*ppObdEntry_p = pObdEntry;
+	*ppSubEntry_p = pSubEntry;
+	*ppDstData_p = pDstData;
+
+	// all checks are done
+	// the caller may now convert the numerial source value to platform byte order in a temporary buffer
+
+      Exit:
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdWriteEntryPost()
+//
+// Description: Function finishes write of data to an OBD entry. Strings
+//              are stored with added '\0' character.
+//
+// Parameters:  EPL_MCO_DECL_INSTANCE_PTR_
+//              uiIndex_p       =   Index of the OD entry
+//              uiSubIndex_p    =   Subindex of the OD Entry
+//              pSrcData_p      =   Pointer to the data to write
+//              Size_p          =   Size of the data in Byte
+//
+// Return:      tEplKernel      =   Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel PUBLIC EplObdWriteEntryPost(EPL_MCO_DECL_INSTANCE_PTR_
+					      tEplObdEntryPtr pObdEntry_p,
+					      tEplObdSubEntryPtr pSubEntry_p,
+					      tEplObdCbParam MEM * pCbParam_p,
+					      void *pSrcData_p,
+					      void *pDstData_p,
+					      tEplObdSize ObdSize_p)
+{
+
+	tEplKernel Ret;
+
+	// caller converted the source value to platform byte order
+	// now the range of the value may be checked
+
+#if (EPL_OBD_CHECK_OBJECT_RANGE != FALSE)
+	{
+		// check data range
+		Ret = EplObdCheckObjectRange(pSubEntry_p, pSrcData_p);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+	}
+#endif
+
+	// now call user callback function to check value
+	// write address of source data to structure of callback parameters
+	// so callback function can check this data
+	pCbParam_p->m_pArg = pSrcData_p;
+	pCbParam_p->m_ObdEvent = kEplObdEvPreWrite;
+	Ret = EplObdCallObjectCallback(EPL_MCO_INSTANCE_PTR_
+				       pObdEntry_p->m_fpCallback, pCbParam_p);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	// copy object data to OBD
+	EPL_MEMCPY(pDstData_p, pSrcData_p, ObdSize_p);
+
+	// terminate string with 0
+	if (pSubEntry_p->m_Type == kEplObdTypVString) {
+		((char MEM *)pDstData_p)[ObdSize_p] = '\0';
+	}
+	// write address of destination to structure of callback parameters
+	// so callback function can change data subsequently
+	pCbParam_p->m_pArg = pDstData_p;
+	pCbParam_p->m_ObdEvent = kEplObdEvPostWrite;
+	Ret = EplObdCallObjectCallback(EPL_MCO_INSTANCE_PTR_
+				       pObdEntry_p->m_fpCallback, pCbParam_p);
+
+      Exit:
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdGetObjectSize()
+//
+// Description: function to get size of object
+//              The function determines if an object type an fixed data type (BYTE, WORD, ...)
+//              or non fixed object (string, domain). This information is used to decide
+//              if download data are stored temporary or not. For objects with fixed data length
+//              and types a value range checking can process.
+//              For strings the function returns the whole object size not the
+//              length of string.
+//
+// Parameters:  pSubIndexEntry_p
+//
+// Return:      tEplObdSize
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplObdSize EplObdGetObjectSize(tEplObdSubEntryPtr pSubIndexEntry_p)
+{
+
+	tEplObdSize DataSize = 0;
+	void *pData;
+
+	switch (pSubIndexEntry_p->m_Type) {
+		// -----------------------------------------------------------------
+	case kEplObdTypBool:
+
+		DataSize = 1;
+		break;
+
+		// -----------------------------------------------------------------
+		// ObdTypes which has to be check because numerical values
+	case kEplObdTypInt8:
+		DataSize = sizeof(tEplObdInteger8);
+		break;
+
+		// -----------------------------------------------------------------
+	case kEplObdTypUInt8:
+		DataSize = sizeof(tEplObdUnsigned8);
+		break;
+
+		// -----------------------------------------------------------------
+	case kEplObdTypInt16:
+		DataSize = sizeof(tEplObdInteger16);
+		break;
+
+		// -----------------------------------------------------------------
+	case kEplObdTypUInt16:
+		DataSize = sizeof(tEplObdUnsigned16);
+		break;
+
+		// -----------------------------------------------------------------
+	case kEplObdTypInt32:
+		DataSize = sizeof(tEplObdInteger32);
+		break;
+
+		// -----------------------------------------------------------------
+	case kEplObdTypUInt32:
+		DataSize = sizeof(tEplObdUnsigned32);
+		break;
+
+		// -----------------------------------------------------------------
+	case kEplObdTypReal32:
+		DataSize = sizeof(tEplObdReal32);
+		break;
+
+		// -----------------------------------------------------------------
+		// ObdTypes which has to be not checked because not NUM values
+	case kEplObdTypDomain:
+
+		pData = (void *)pSubIndexEntry_p->m_pCurrent;
+		if ((void MEM *)pData != (void MEM *)NULL) {
+			DataSize = ((tEplObdVarEntry MEM *) pData)->m_Size;
+		}
+		break;
+
+		// -----------------------------------------------------------------
+	case kEplObdTypVString:
+		//case kEplObdTypUString:
+
+		// If OD entry is defined by macro EPL_OBD_SUBINDEX_ROM_VSTRING
+		// then the current pointer is always NULL. The function
+		// returns the length of default string.
+		pData = (void *)pSubIndexEntry_p->m_pCurrent;
+		if ((void MEM *)pData != (void MEM *)NULL) {
+			// The max. size of strings defined by STRING-Macro is stored in
+			// tEplObdVString of current value.
+			// (types tEplObdVString, tEplObdOString and tEplObdUString has the same members)
+			DataSize = ((tEplObdVString MEM *) pData)->m_Size;
+		} else {
+			// The current position is not decleared. The string
+			// is located in ROM, therefor use default pointer.
+			pData = (void *)pSubIndexEntry_p->m_pDefault;
+			if ((CONST void ROM *)pData != (CONST void ROM *)NULL) {
+				// The max. size of strings defined by STRING-Macro is stored in
+				// tEplObdVString of default value.
+				DataSize =
+				    ((CONST tEplObdVString ROM *) pData)->
+				    m_Size;
+			}
+		}
+
+		break;
+
+		// -----------------------------------------------------------------
+	case kEplObdTypOString:
+
+		pData = (void *)pSubIndexEntry_p->m_pCurrent;
+		if ((void MEM *)pData != (void MEM *)NULL) {
+			// The max. size of strings defined by STRING-Macro is stored in
+			// tEplObdVString of current value.
+			// (types tEplObdVString, tEplObdOString and tEplObdUString has the same members)
+			DataSize = ((tEplObdOString MEM *) pData)->m_Size;
+		} else {
+			// The current position is not decleared. The string
+			// is located in ROM, therefor use default pointer.
+			pData = (void *)pSubIndexEntry_p->m_pDefault;
+			if ((CONST void ROM *)pData != (CONST void ROM *)NULL) {
+				// The max. size of strings defined by STRING-Macro is stored in
+				// tEplObdVString of default value.
+				DataSize =
+				    ((CONST tEplObdOString ROM *) pData)->
+				    m_Size;
+			}
+		}
+		break;
+
+		// -----------------------------------------------------------------
+	case kEplObdTypInt24:
+	case kEplObdTypUInt24:
+
+		DataSize = 3;
+		break;
+
+		// -----------------------------------------------------------------
+	case kEplObdTypInt40:
+	case kEplObdTypUInt40:
+
+		DataSize = 5;
+		break;
+
+		// -----------------------------------------------------------------
+	case kEplObdTypInt48:
+	case kEplObdTypUInt48:
+
+		DataSize = 6;
+		break;
+
+		// -----------------------------------------------------------------
+	case kEplObdTypInt56:
+	case kEplObdTypUInt56:
+
+		DataSize = 7;
+		break;
+
+		// -----------------------------------------------------------------
+	case kEplObdTypInt64:
+	case kEplObdTypUInt64:
+	case kEplObdTypReal64:
+
+		DataSize = 8;
+		break;
+
+		// -----------------------------------------------------------------
+	case kEplObdTypTimeOfDay:
+	case kEplObdTypTimeDiff:
+
+		DataSize = 6;
+		break;
+
+		// -----------------------------------------------------------------
+	default:
+		break;
+	}
+
+	return DataSize;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdGetObjectDefaultPtr()
+//
+// Description: function to get the default pointer (type specific)
+//
+// Parameters:  pSubIndexEntry_p    = pointer to subindex structure
+//
+// Returns:     (void *)   = pointer to default value
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static void *EplObdGetObjectDefaultPtr(tEplObdSubEntryPtr pSubIndexEntry_p)
+{
+
+	void *pDefault;
+	tEplObdType Type;
+
+	ASSERTMSG(pSubIndexEntry_p != NULL,
+		  "EplObdGetObjectDefaultPtr(): pointer to SubEntry not valid!\n");
+
+	// get address to default data from default pointer
+	pDefault = pSubIndexEntry_p->m_pDefault;
+	if (pDefault != NULL) {
+		// there are some special types, whose default pointer always is NULL or has to get from other structure
+		// get type from subindex structure
+		Type = pSubIndexEntry_p->m_Type;
+
+		// check if object type is a string value
+		if ((Type == kEplObdTypVString)	/* ||
+						   (Type == kEplObdTypUString) */ ) {
+
+			// EPL_OBD_SUBINDEX_RAM_VSTRING
+			//    tEplObdSize         m_Size;       --> size of default string
+			//    char *    m_pDefString; --> pointer to  default string
+			//    char *    m_pString;    --> pointer to string in RAM
+			//
+			pDefault =
+			    (void *)((tEplObdVString *) pDefault)->m_pString;
+		} else if (Type == kEplObdTypOString) {
+			pDefault =
+			    (void *)((tEplObdOString *) pDefault)->m_pString;
+		}
+	}
+
+	return pDefault;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdGetVarEntry()
+//
+// Description: gets a variable entry of an object
+//
+// Parameters:  pSubindexEntry_p
+//              ppVarEntry_p
+//
+// Return:      tCopKernel
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplObdGetVarEntry(tEplObdSubEntryPtr pSubindexEntry_p,
+				    tEplObdVarEntry MEM ** ppVarEntry_p)
+{
+
+	tEplKernel Ret = kEplObdVarEntryNotExist;
+
+	ASSERT(ppVarEntry_p != NULL);	// is not allowed to be NULL
+	ASSERT(pSubindexEntry_p != NULL);
+
+	// check VAR-Flag - only this object points to variables
+	if ((pSubindexEntry_p->m_Access & kEplObdAccVar) != 0) {
+		// check if object is an array
+		if ((pSubindexEntry_p->m_Access & kEplObdAccArray) != 0) {
+			*ppVarEntry_p =
+			    &((tEplObdVarEntry MEM *) pSubindexEntry_p->
+			      m_pCurrent)[pSubindexEntry_p->m_uiSubIndex - 1];
+		} else {
+			*ppVarEntry_p =
+			    (tEplObdVarEntry MEM *) pSubindexEntry_p->
+			    m_pCurrent;
+		}
+
+		Ret = kEplSuccessful;
+	}
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdGetEntry()
+//
+// Description: gets a index entry from OD
+//
+// Parameters:  uiIndex_p       =   Index number
+//              uiSubindex_p    =   Subindex number
+//              ppObdEntry_p    =   pointer to the pointer to the entry
+//              ppObdSubEntry_p =   pointer to the pointer to the subentry
+//
+// Return:      tEplKernel
+
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplObdGetEntry(EPL_MCO_DECL_INSTANCE_PTR_
+				 unsigned int uiIndex_p,
+				 unsigned int uiSubindex_p,
+				 tEplObdEntryPtr * ppObdEntry_p,
+				 tEplObdSubEntryPtr * ppObdSubEntry_p)
+{
+
+	tEplObdEntryPtr pObdEntry;
+	tEplObdCbParam MEM CbParam;
+	tEplKernel Ret;
+
+	// check for all API function if instance is valid
+	EPL_MCO_CHECK_INSTANCE_STATE();
+
+	//------------------------------------------------------------------------
+	// get address of entry of index
+	Ret =
+	    EplObdGetIndexIntern(&EPL_MCO_GLB_VAR(m_ObdInitParam), uiIndex_p,
+				 &pObdEntry);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	//------------------------------------------------------------------------
+	// get address of entry of subindex
+	Ret = EplObdGetSubindexIntern(pObdEntry, uiSubindex_p, ppObdSubEntry_p);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+	//------------------------------------------------------------------------
+	// call callback function to inform user/stack that an object will be searched
+	// if the called module returnes an error then we abort the searching with kEplObdIndexNotExist
+	CbParam.m_uiIndex = uiIndex_p;
+	CbParam.m_uiSubIndex = uiSubindex_p;
+	CbParam.m_pArg = NULL;
+	CbParam.m_ObdEvent = kEplObdEvCheckExist;
+	Ret = EplObdCallObjectCallback(EPL_MCO_INSTANCE_PTR_
+				       pObdEntry->m_fpCallback, &CbParam);
+	if (Ret != kEplSuccessful) {
+		Ret = kEplObdIndexNotExist;
+		goto Exit;
+	}
+	//------------------------------------------------------------------------
+	// it is allowed to set ppObdEntry_p to NULL
+	// if so, no address will be written to calling function
+	if (ppObdEntry_p != NULL) {
+		*ppObdEntry_p = pObdEntry;
+	}
+
+      Exit:
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdGetObjectCurrentPtr()
+//
+// Description: function to get Current pointer (type specific)
+//
+// Parameters:  pSubIndexEntry_p
+//
+// Return:      void MEM*
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static void MEM *EplObdGetObjectCurrentPtr(tEplObdSubEntryPtr pSubIndexEntry_p)
+{
+
+	void MEM *pData;
+	unsigned int uiArrayIndex;
+	tEplObdSize Size;
+
+	pData = pSubIndexEntry_p->m_pCurrent;
+
+	// check if constant object
+	if (pData != NULL) {
+		// check if object is an array
+		if ((pSubIndexEntry_p->m_Access & kEplObdAccArray) != 0) {
+			// calculate correct data pointer
+			uiArrayIndex = pSubIndexEntry_p->m_uiSubIndex - 1;
+			if ((pSubIndexEntry_p->m_Access & kEplObdAccVar) != 0) {
+				Size = sizeof(tEplObdVarEntry);
+			} else {
+				Size = EplObdGetObjectSize(pSubIndexEntry_p);
+			}
+			pData = ((BYTE MEM *) pData) + (Size * uiArrayIndex);
+		}
+		// check if VarEntry
+		if ((pSubIndexEntry_p->m_Access & kEplObdAccVar) != 0) {
+			// The data pointer is stored in VarEntry->pData
+			pData = ((tEplObdVarEntry MEM *) pData)->m_pData;
+		}
+		// the default pointer is stored for strings in tEplObdVString
+		else if ((pSubIndexEntry_p->m_Type == kEplObdTypVString)	/* ||
+										   (pSubIndexEntry_p->m_Type == kEplObdTypUString)    */
+			 ) {
+			pData =
+			    (void MEM *)((tEplObdVString MEM *) pData)->
+			    m_pString;
+		} else if (pSubIndexEntry_p->m_Type == kEplObdTypOString) {
+			pData =
+			    (void MEM *)((tEplObdOString MEM *) pData)->
+			    m_pString;
+		}
+	}
+
+	return pData;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdGetIndexIntern()
+//
+// Description: gets a index entry from OD
+//
+// Parameters:  pInitParam_p
+//              uiIndex_p
+//              ppObdEntry_p
+//
+// Return:      tEplKernel
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplObdGetIndexIntern(tEplObdInitParam MEM * pInitParam_p,
+				       unsigned int uiIndex_p,
+				       tEplObdEntryPtr * ppObdEntry_p)
+{
+
+	tEplObdEntryPtr pObdEntry;
+	tEplKernel Ret;
+	unsigned int uiIndex;
+
+#if (defined (EPL_OBD_USER_OD) && (EPL_OBD_USER_OD != FALSE))
+
+	unsigned int nLoop;
+
+	// if user OD is used then objekts also has to be searched in user OD
+	// there is less code need if we do this in a loop
+	nLoop = 2;
+
+#endif
+
+	ASSERTMSG(ppObdEntry_p != NULL,
+		  "EplObdGetIndexIntern(): pointer to index entry is NULL!\n");
+
+	Ret = kEplObdIndexNotExist;
+
+	// get start address of OD part
+	// start address depends on object index because
+	// object dictionary is divided in 3 parts
+	if ((uiIndex_p >= 0x1000) && (uiIndex_p < 0x2000)) {
+		pObdEntry = pInitParam_p->m_pPart;
+	} else if ((uiIndex_p >= 0x2000) && (uiIndex_p < 0x6000)) {
+		pObdEntry = pInitParam_p->m_pManufacturerPart;
+	}
+	// index range 0xA000 to 0xFFFF is reserved for DSP-405
+	// DS-301 defines that range 0x6000 to 0x9FFF (!!!) is stored if "store" was written to 0x1010/3.
+	// Therefore default configuration is OBD_INCLUDE_A000_TO_DEVICE_PART = FALSE.
+	// But a CANopen Application which does not implement dynamic OD or user-OD but wants to use static objets 0xA000...
+	// should set OBD_INCLUDE_A000_TO_DEVICE_PART to TRUE.
+
+#if (EPL_OBD_INCLUDE_A000_TO_DEVICE_PART == FALSE)
+	else if ((uiIndex_p >= 0x6000) && (uiIndex_p < 0x9FFF))
+#else
+	else if ((uiIndex_p >= 0x6000) && (uiIndex_p < 0xFFFF))
+#endif
+	{
+		pObdEntry = pInitParam_p->m_pDevicePart;
+	}
+
+#if (defined (EPL_OBD_USER_OD) && (EPL_OBD_USER_OD != FALSE))
+
+	// if index does not match in static OD then index only has to be searched in user OD
+	else {
+		// begin from first entry of user OD part
+		pObdEntry = pInitParam_p->m_pUserPart;
+
+		// no user OD is available
+		if (pObdEntry == NULL) {
+			goto Exit;
+		}
+		// loop must only run once
+		nLoop = 1;
+	}
+
+	do {
+
+#else
+
+	// no user OD is available
+	// so other object can be found in OD
+	else {
+		Ret = kEplObdIllegalPart;
+		goto Exit;
+	}
+
+#endif
+
+	// note:
+	// The end of Index table is marked with m_uiIndex = 0xFFFF.
+	// If this function will be called with wIndex_p = 0xFFFF, entry
+	// should not be found. Therefor it is important to use
+	// while{} instead of do{}while !!!
+
+	// get first index of index table
+	uiIndex = pObdEntry->m_uiIndex;
+
+	// search Index in OD part
+	while (uiIndex != EPL_OBD_TABLE_INDEX_END) {
+		// go to the end of this function if index is found
+		if (uiIndex_p == uiIndex) {
+			// write address of OD entry to calling function
+			*ppObdEntry_p = pObdEntry;
+			Ret = kEplSuccessful;
+			goto Exit;
+		}
+		// objects are sorted in OD
+		// if the current index in OD is greater than the index which is to search then break loop
+		// in this case user OD has to be search too
+		if (uiIndex_p < uiIndex) {
+			break;
+		}
+		// next entry in index table
+		pObdEntry++;
+
+		// get next index of index table
+		uiIndex = pObdEntry->m_uiIndex;
+	}
+
+#if (defined (EPL_OBD_USER_OD) && (EPL_OBD_USER_OD != FALSE))
+
+	// begin from first entry of user OD part
+	pObdEntry = pInitParam_p->m_pUserPart;
+
+	// no user OD is available
+	if (pObdEntry == NULL) {
+		goto Exit;
+	}
+	// switch next loop for user OD
+	nLoop--;
+
+}
+
+while (nLoop > 0) ;
+
+#endif
+
+    // in this line Index was not found
+
+Exit:
+
+return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdGetSubindexIntern()
+//
+// Description: gets a subindex entry from a index entry
+//
+// Parameters:  pObdEntry_p
+//              bSubIndex_p
+//              ppObdSubEntry_p
+//
+// Return:      tEplKernel
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplObdGetSubindexIntern(tEplObdEntryPtr pObdEntry_p,
+					  unsigned int uiSubIndex_p,
+					  tEplObdSubEntryPtr * ppObdSubEntry_p)
+{
+
+	tEplObdSubEntryPtr pSubEntry;
+	unsigned int nSubIndexCount;
+	tEplKernel Ret;
+
+	ASSERTMSG(pObdEntry_p != NULL,
+		  "EplObdGetSubindexIntern(): pointer to index is NULL!\n");
+	ASSERTMSG(ppObdSubEntry_p != NULL,
+		  "EplObdGetSubindexIntern(): pointer to subindex is NULL!\n");
+
+	Ret = kEplObdSubindexNotExist;
+
+	// get start address of subindex table and count of subindices
+	pSubEntry = pObdEntry_p->m_pSubIndex;
+	nSubIndexCount = pObdEntry_p->m_uiCount;
+	ASSERTMSG((pSubEntry != NULL) && (nSubIndexCount > 0), "ObdGetSubindexIntern(): invalid subindex table within index table!\n");	// should never be NULL
+
+	// search subindex in subindex table
+	while (nSubIndexCount > 0) {
+		// check if array is found
+		if ((pSubEntry->m_Access & kEplObdAccArray) != 0) {
+			// check if subindex is in range
+			if (uiSubIndex_p < pObdEntry_p->m_uiCount) {
+				// update subindex number (subindex entry of an array is always in RAM !!!)
+				pSubEntry->m_uiSubIndex = uiSubIndex_p;
+				*ppObdSubEntry_p = pSubEntry;
+				Ret = kEplSuccessful;
+				goto Exit;
+			}
+		}
+		// go to the end of this function if subindex is found
+		else if (uiSubIndex_p == pSubEntry->m_uiSubIndex) {
+			*ppObdSubEntry_p = pSubEntry;
+			Ret = kEplSuccessful;
+			goto Exit;
+		}
+		// objects are sorted in OD
+		// if the current subindex in OD is greater than the subindex which is to search then break loop
+		// in this case user OD has to be search too
+		if (uiSubIndex_p < pSubEntry->m_uiSubIndex) {
+			break;
+		}
+
+		pSubEntry++;
+		nSubIndexCount--;
+	}
+
+	// in this line SubIndex was not fount
+
+      Exit:
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdSetStoreLoadObjCallback()
+//
+// Description: function set address to callbackfunction for command Store and Load
+//
+// Parameters:  fpCallback_p
+//
+// Return:      tEplKernel
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#if (EPL_OBD_USE_STORE_RESTORE != FALSE)
+EPLDLLEXPORT tEplKernel PUBLIC
+EplObdSetStoreLoadObjCallback(EPL_MCO_DECL_INSTANCE_PTR_
+			      tEplObdStoreLoadObjCallback fpCallback_p)
+{
+
+	EPL_MCO_CHECK_INSTANCE_STATE();
+
+	// set new address of callback function
+	EPL_MCO_GLB_VAR(m_fpStoreLoadObjCallback) = fpCallback_p;
+
+	return kEplSuccessful;
+
+}
+#endif // (EPL_OBD_USE_STORE_RESTORE != FALSE)
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdAccessOdPartIntern()
+//
+// Description: runs through OD and executes a job
+//
+// Parameters:  CurrentOdPart_p
+//              pObdEnty_p
+//              Direction_p     = what is to do (load values from flash or EEPROM, store, ...)
+//
+// Return:      tEplKernel
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplObdAccessOdPartIntern(EPL_MCO_DECL_INSTANCE_PTR_
+					   tEplObdPart CurrentOdPart_p,
+					   tEplObdEntryPtr pObdEnty_p,
+					   tEplObdDir Direction_p)
+{
+
+	tEplObdSubEntryPtr pSubIndex;
+	unsigned int nSubIndexCount;
+	tEplObdAccess Access;
+	void MEM *pDstData;
+	void *pDefault;
+	tEplObdSize ObjSize;
+	tEplKernel Ret;
+	tEplObdCbStoreParam MEM CbStore;
+	tEplObdVarEntry MEM *pVarEntry;
+
+	ASSERT(pObdEnty_p != NULL);
+
+	Ret = kEplSuccessful;
+
+	// prepare structure for STORE RESTORE callback function
+	CbStore.m_bCurrentOdPart = (BYTE) CurrentOdPart_p;
+	CbStore.m_pData = NULL;
+	CbStore.m_ObjSize = 0;
+
+	// command of first action depends on direction to access
+#if (EPL_OBD_USE_STORE_RESTORE != FALSE)
+	if (Direction_p == kEplObdDirLoad) {
+		CbStore.m_bCommand = (BYTE) kEplObdCommOpenRead;
+
+		// call callback function for previous command
+		Ret = EplObdCallStoreCallback(EPL_MCO_INSTANCE_PTR_ & CbStore);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+		// set command for index and subindex loop
+		CbStore.m_bCommand = (BYTE) kEplObdCommReadObj;
+	} else if (Direction_p == kEplObdDirStore) {
+		CbStore.m_bCommand = (BYTE) kEplObdCommOpenWrite;
+
+		// call callback function for previous command
+		Ret = EplObdCallStoreCallback(EPL_MCO_INSTANCE_PTR_ & CbStore);
+		if (Ret != kEplSuccessful) {
+			goto Exit;
+		}
+		// set command for index and subindex loop
+		CbStore.m_bCommand = (BYTE) kEplObdCommWriteObj;
+	}
+#endif // (EPL_OBD_USE_STORE_RESTORE != FALSE)
+
+	// we should not restore the OD values here
+	// the next NMT command "Reset Node" or "Reset Communication" resets the OD data
+	if (Direction_p != kEplObdDirRestore) {
+		// walk through OD part till end is found
+		while (pObdEnty_p->m_uiIndex != EPL_OBD_TABLE_INDEX_END) {
+			// get address to subindex table and count of subindices
+			pSubIndex = pObdEnty_p->m_pSubIndex;
+			nSubIndexCount = pObdEnty_p->m_uiCount;
+			ASSERT((pSubIndex != NULL) && (nSubIndexCount > 0));	// should never be NULL
+
+			// walk through subindex table till all subinices were restored
+			while (nSubIndexCount != 0) {
+				Access = (tEplObdAccess) pSubIndex->m_Access;
+
+				// get pointer to current and default data
+				pDefault = EplObdGetObjectDefaultPtr(pSubIndex);
+				pDstData = EplObdGetObjectCurrentPtr(pSubIndex);
+
+				// NOTE (for kEplObdTypVString):
+				//      The function returnes the max. number of bytes for a
+				//      current string.
+				//      r.d.: For stings the default-size will be read in other lines following (kEplObdDirInit).
+				ObjSize = EplObdGetObjectSize(pSubIndex);
+
+				// switch direction of OD access
+				switch (Direction_p) {
+					// --------------------------------------------------------------------------
+					// VarEntry structures has to be initialized
+				case kEplObdDirInit:
+
+					// If VAR-Flag is set, m_pCurrent means not address of data
+					// but address of tEplObdVarEntry. Address of data has to be get from
+					// this structure.
+					if ((Access & kEplObdAccVar) != 0) {
+						EplObdGetVarEntry(pSubIndex,
+								  &pVarEntry);
+						EplObdInitVarEntry(pVarEntry,
+								   pSubIndex->
+								   m_Type,
+								   ObjSize);
+/*
+                            if ((Access & kEplObdAccArray) == 0)
+                            {
+                                EplObdInitVarEntry (pSubIndex->m_pCurrent, pSubIndex->m_Type, ObjSize);
+                            }
+                            else
+                            {
+                                EplObdInitVarEntry ((tEplObdVarEntry MEM*) (((BYTE MEM*) pSubIndex->m_pCurrent) + (sizeof (tEplObdVarEntry) * pSubIndex->m_uiSubIndex)),
+                                    pSubIndex->m_Type, ObjSize);
+                            }
+*/
+						// at this time no application variable is defined !!!
+						// therefore data can not be copied.
+						break;
+					} else if (pSubIndex->m_Type ==
+						   kEplObdTypVString) {
+						// If pointer m_pCurrent is not equal to NULL then the
+						// string was defined with EPL_OBD_SUBINDEX_RAM_VSTRING. The current
+						// pointer points to struct tEplObdVString located in MEM.
+						// The element size includes the max. number of
+						// bytes. The element m_pString includes the pointer
+						// to string in MEM. The memory location of default string
+						// must be copied to memory location of current string.
+
+						pDstData =
+						    pSubIndex->m_pCurrent;
+						if (pDstData != NULL) {
+							// 08-dec-2004: code optimization !!!
+							//              entries ((tEplObdVStringDef ROM*) pSubIndex->m_pDefault)->m_pString
+							//              and ((tEplObdVStringDef ROM*) pSubIndex->m_pDefault)->m_Size were read
+							//              twice. thats not necessary!
+
+							// For copying data we have to set the destination pointer to the real RAM string. This
+							// pointer to RAM string is located in default string info structure. (translated r.d.)
+							pDstData =
+							    (void MEM
+							     *)((tEplObdVStringDef ROM *) pSubIndex->m_pDefault)->m_pString;
+							ObjSize =
+							    ((tEplObdVStringDef
+							      ROM *) pSubIndex->
+							     m_pDefault)->
+							    m_Size;
+
+							((tEplObdVString MEM *)
+							 pSubIndex->
+							 m_pCurrent)->
+				     m_pString = pDstData;
+							((tEplObdVString MEM *)
+							 pSubIndex->
+							 m_pCurrent)->m_Size =
+				     ObjSize;
+						}
+
+					} else if (pSubIndex->m_Type ==
+						   kEplObdTypOString) {
+						pDstData =
+						    pSubIndex->m_pCurrent;
+						if (pDstData != NULL) {
+							// 08-dec-2004: code optimization !!!
+							//              entries ((tEplObdOStringDef ROM*) pSubIndex->m_pDefault)->m_pString
+							//              and ((tEplObdOStringDef ROM*) pSubIndex->m_pDefault)->m_Size were read
+							//              twice. thats not necessary!
+
+							// For copying data we have to set the destination pointer to the real RAM string. This
+							// pointer to RAM string is located in default string info structure. (translated r.d.)
+							pDstData =
+							    (void MEM
+							     *)((tEplObdOStringDef ROM *) pSubIndex->m_pDefault)->m_pString;
+							ObjSize =
+							    ((tEplObdOStringDef
+							      ROM *) pSubIndex->
+							     m_pDefault)->
+							    m_Size;
+
+							((tEplObdOString MEM *)
+							 pSubIndex->
+							 m_pCurrent)->
+				     m_pString = pDstData;
+							((tEplObdOString MEM *)
+							 pSubIndex->
+							 m_pCurrent)->m_Size =
+				     ObjSize;
+						}
+
+					}
+
+					// no break !! because copy of data has to done too.
+
+					// --------------------------------------------------------------------------
+					// all objects has to be restored with default values
+				case kEplObdDirRestore:
+
+					// 09-dec-2004 r.d.: optimization! the same code for kEplObdDirRestore and kEplObdDirLoad
+					//                   is replaced to function ObdCopyObjectData() with a new parameter.
+
+					// restore object data for init phase
+					EplObdCopyObjectData(pDstData, pDefault,
+							     ObjSize,
+							     pSubIndex->m_Type);
+					break;
+
+					// --------------------------------------------------------------------------
+					// objects with attribute kEplObdAccStore has to be load from EEPROM or from a file
+				case kEplObdDirLoad:
+
+					// restore object data for init phase
+					EplObdCopyObjectData(pDstData, pDefault,
+							     ObjSize,
+							     pSubIndex->m_Type);
+
+					// no break !! because callback function has to be called too.
+
+					// --------------------------------------------------------------------------
+					// objects with attribute kEplObdAccStore has to be stored in EEPROM or in a file
+				case kEplObdDirStore:
+
+					// when attribute kEplObdAccStore is set, then call callback function
+#if (EPL_OBD_USE_STORE_RESTORE != FALSE)
+					if ((Access & kEplObdAccStore) != 0) {
+						// fill out data pointer and size of data
+						CbStore.m_pData = pDstData;
+						CbStore.m_ObjSize = ObjSize;
+
+						// call callback function for read or write object
+						Ret =
+						    ObdCallStoreCallback
+						    (EPL_MCO_INSTANCE_PTR_ &
+						     CbStore);
+						if (Ret != kEplSuccessful) {
+							goto Exit;
+						}
+					}
+#endif // (EPL_OBD_USE_STORE_RESTORE != FALSE)
+					break;
+
+					// --------------------------------------------------------------------------
+					// if OD Builder key has to be checked no access to subindex and data should be made
+				case kEplObdDirOBKCheck:
+
+					// no break !! because we want to break the second loop too.
+
+					// --------------------------------------------------------------------------
+					// unknown Direction
+				default:
+
+					// so we can break the second loop earler
+					nSubIndexCount = 1;
+					break;
+				}
+
+				nSubIndexCount--;
+
+				// next subindex entry
+				if ((Access & kEplObdAccArray) == 0) {
+					pSubIndex++;
+					if ((nSubIndexCount > 0)
+					    &&
+					    ((pSubIndex->
+					      m_Access & kEplObdAccArray) !=
+					     0)) {
+						// next subindex points to an array
+						// reset subindex number
+						pSubIndex->m_uiSubIndex = 1;
+					}
+				} else {
+					if (nSubIndexCount > 0) {
+						// next subindex points to an array
+						// increment subindex number
+						pSubIndex->m_uiSubIndex++;
+					}
+				}
+			}
+
+			// next index entry
+			pObdEnty_p++;
+		}
+	}
+	// -----------------------------------------------------------------------------------------
+	// command of last action depends on direction to access
+	if (Direction_p == kEplObdDirOBKCheck) {
+
+		goto Exit;
+	}
+#if (EPL_OBD_USE_STORE_RESTORE != FALSE)
+	else {
+		if (Direction_p == kEplObdDirLoad) {
+			CbStore.m_bCommand = (BYTE) kEplObdCommCloseRead;
+		} else if (Direction_p == kEplObdDirStore) {
+			CbStore.m_bCommand = (BYTE) kEplObdCommCloseWrite;
+		} else if (Direction_p == kEplObdDirRestore) {
+			CbStore.m_bCommand = (BYTE) kEplObdCommClear;
+		} else {
+			goto Exit;
+		}
+
+		// call callback function for last command
+		Ret = EplObdCallStoreCallback(EPL_MCO_INSTANCE_PTR_ & CbStore);
+	}
+#endif // (EPL_OBD_USE_STORE_RESTORE != FALSE)
+
+//    goto Exit;
+
+      Exit:
+
+	return Ret;
+
+}
+
+// ----------------------------------------------------------------------------
+// Function:    EplObdCopyObjectData()
+//
+// Description: checks pointers to object data and copy them from source to destination
+//
+// Parameters:  pDstData_p              = destination pointer
+//              pSrcData_p              = source pointer
+//              ObjSize_p               = size of object
+//              ObjType_p               =
+//
+// Returns:     tEplKernel              = error code
+// ----------------------------------------------------------------------------
+
+static void EplObdCopyObjectData(void MEM * pDstData_p,
+				 void *pSrcData_p,
+				 tEplObdSize ObjSize_p, tEplObdType ObjType_p)
+{
+
+	tEplObdSize StrSize = 0;
+
+	// it is allowed to set default and current address to NULL (nothing to copy)
+	if (pDstData_p != NULL) {
+
+		if (ObjType_p == kEplObdTypVString) {
+			// The function calculates the really number of characters of string. The
+			// object entry size can be bigger as string size of default string.
+			// The '\0'-termination is included. A string with no characters has a
+			// size of 1.
+			StrSize =
+			    EplObdGetStrLen((void *)pSrcData_p, ObjSize_p,
+					    kEplObdTypVString);
+
+			// If the string length is greater than or equal to the entry size in OD then only copy
+			// entry size - 1 and always set the '\0'-termination.
+			if (StrSize >= ObjSize_p) {
+				StrSize = ObjSize_p - 1;
+			}
+		}
+
+		if (pSrcData_p != NULL) {
+			// copy data
+			EPL_MEMCPY(pDstData_p, pSrcData_p, ObjSize_p);
+
+			if (ObjType_p == kEplObdTypVString) {
+				((char MEM *)pDstData_p)[StrSize] = '\0';
+			}
+		}
+	}
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdIsNumericalIntern()
+//
+// Description: function checks if a entry is numerical or not
+//
+//
+// Parameters:  EPL_MCO_DECL_INSTANCE_PTR_ = Instancepointer
+//              uiIndex_p           = Index
+//              uiSubIndex_p        = Subindex
+//              pfEntryNumerical_p  = pointer to BOOL for returnvalue
+//                                  -> TRUE if entry a numerical value
+//                                  -> FALSE if entry not a numerical value
+//
+// Return:      tEplKernel = Errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static tEplKernel EplObdIsNumericalIntern(tEplObdSubEntryPtr pObdSubEntry_p,
+					  BOOL * pfEntryNumerical_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	// get Type
+	if ((pObdSubEntry_p->m_Type == kEplObdTypVString)
+	    || (pObdSubEntry_p->m_Type == kEplObdTypOString)
+	    || (pObdSubEntry_p->m_Type == kEplObdTypDomain)) {	// not numerical types
+		*pfEntryNumerical_p = FALSE;
+	} else {		// numerical types
+		*pfEntryNumerical_p = TRUE;
+	}
+
+	return Ret;
+
+}
+
+// -------------------------------------------------------------------------
+// function to classify object type (fixed/non fixed)
+// -------------------------------------------------------------------------
+
+// ----------------------------------------------------------------------------
+// Function:    EplObdCallStoreCallback()
+//
+// Description: checks address to callback function and calles it when unequal
+//              to NULL
+//
+// Parameters:  EPL_MCO_DECL_INSTANCE_PTR_ = (instance pointer)
+//              pCbStoreParam_p        = address to callback parameters
+//
+// Returns:     tEplKernel             = error code
+// ----------------------------------------------------------------------------
+#if (EPL_OBD_USE_STORE_RESTORE != FALSE)
+static tEplKernel EplObdCallStoreCallback(EPL_MCO_DECL_INSTANCE_PTR_
+					  tEplObdCbStoreParam MEM *
+					  pCbStoreParam_p)
+{
+
+	tEplKernel Ret = kEplSuccessful;
+
+	ASSERT(pCbStoreParam_p != NULL);
+
+	// check if function pointer is NULL - if so, no callback should be called
+	if (EPL_MCO_GLB_VAR(m_fpStoreLoadObjCallback) != NULL) {
+		Ret =
+		    EPL_MCO_GLB_VAR(m_fpStoreLoadObjCallback)
+		    (EPL_MCO_INSTANCE_PARAM_IDX_()
+		     pCbStoreParam_p);
+	}
+
+	return Ret;
+
+}
+#endif // (EPL_OBD_USE_STORE_RESTORE != FALSE)
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdGetObjectDataPtrIntern()
+//
+// Description: Function gets the data pointer of an object.
+//              It returnes the current data pointer. But if object is an
+//              constant object it returnes the default pointer.
+//
+// Parameters:  pSubindexEntry_p = pointer to subindex entry
+//
+// Return:      void *    = pointer to object data
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+void *EplObdGetObjectDataPtrIntern(tEplObdSubEntryPtr pSubindexEntry_p)
+{
+
+	void *pData;
+	tEplObdAccess Access;
+
+	ASSERTMSG(pSubindexEntry_p != NULL,
+		  "EplObdGetObjectDataPtrIntern(): pointer to SubEntry not valid!\n");
+
+	// there are are some objects whose data pointer has to get from other structure
+	// get access type for this object
+	Access = pSubindexEntry_p->m_Access;
+
+	// If object has access type = const,
+	// for data only exists default values.
+	if ((Access & kEplObdAccConst) != 0) {
+		// The pointer to defualt value can be received from ObdGetObjectDefaultPtr()
+		pData = ((void *)EplObdGetObjectDefaultPtr(pSubindexEntry_p));
+	} else {
+		// The pointer to current value can be received from ObdGetObjectCurrentPtr()
+		pData = ((void *)EplObdGetObjectCurrentPtr(pSubindexEntry_p));
+	}
+
+	return pData;
+
+}
+#endif // end of #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+// EOF
diff --git a/drivers/staging/epl/EplObd.h b/drivers/staging/epl/EplObd.h
new file mode 100644
index 00000000000..88cc11e34cc
--- /dev/null
+++ b/drivers/staging/epl/EplObd.h
@@ -0,0 +1,464 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for api function of EplOBD-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplObd.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                Microsoft VC7
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/02 k.t.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#include "EplInc.h"
+
+#ifndef _EPLOBD_H_
+#define _EPLOBD_H_
+
+// ============================================================================
+// defines
+// ============================================================================
+
+#define EPL_OBD_TABLE_INDEX_END     0xFFFF
+
+// for the usage of BOOLEAN in OD
+#define OBD_TRUE    0x01
+#define OBD_FALSE   0x00
+
+// default OD index for Node id
+#define EPL_OBD_NODE_ID_INDEX               0x1F93
+// default subindex for NodeId in OD
+#define EPL_OBD_NODE_ID_SUBINDEX            0x01
+// default subindex for NodeIDByHW_BOOL
+#define EPL_OBD_NODE_ID_HWBOOL_SUBINDEX     0x02
+
+// ============================================================================
+// enums
+// ============================================================================
+
+// directions for access to object dictionary
+typedef enum {
+	kEplObdDirInit = 0x00,	// initialising after power on
+	kEplObdDirStore = 0x01,	// store all object values to non volatile memory
+	kEplObdDirLoad = 0x02,	// load all object values from non volatile memory
+	kEplObdDirRestore = 0x03,	// deletes non volatile memory (restore)
+	kEplObdDirOBKCheck = 0xFF	// reserved
+} tEplObdDir;
+
+// commands for store
+typedef enum {
+	kEplObdCommNothing = 0x00,
+	kEplObdCommOpenWrite = 0x01,
+	kEplObdCommWriteObj = 0x02,
+	kEplObdCommCloseWrite = 0x03,
+	kEplObdCommOpenRead = 0x04,
+	kEplObdCommReadObj = 0x05,
+	kEplObdCommCloseRead = 0x06,
+	kEplObdCommClear = 0x07,
+	kEplObdCommUnknown = 0xFF
+} tEplObdCommand;
+
+//-----------------------------------------------------------------------------------------------------------
+// events of object callback function
+typedef enum {
+//                                                                                                      m_pArg points to
+//                                                                                                    ---------------------
+	kEplObdEvCheckExist = 0x06,	// checking if object does exist (reading and writing)    NULL
+	kEplObdEvPreRead = 0x00,	// before reading an object                               source data buffer in OD
+	kEplObdEvPostRead = 0x01,	// after reading an object                                destination data buffer from caller
+	kEplObdEvWrStringDomain = 0x07,	// event for changing string/domain data pointer or size  struct tEplObdVStringDomain in RAM
+	kEplObdEvInitWrite = 0x04,	// initializes writing an object (checking object size)   size of object in OD (tEplObdSize)
+	kEplObdEvPreWrite = 0x02,	// before writing an object                               source data buffer from caller
+	kEplObdEvPostWrite = 0x03,	// after writing an object                                destination data buffer in OD
+//    kEplObdEvAbortSdo              = 0x05     // after an abort of an SDO transfer
+
+} tEplObdEvent;
+
+// part of OD (bit oriented)
+typedef unsigned int tEplObdPart;
+
+#define kEplObdPartNo          0x00	// nothing
+#define kEplObdPartGen         0x01	//  part      (0x1000 - 0x1FFF)
+#define kEplObdPartMan         0x02	// manufacturer part (0x2000 - 0x5FFF)
+#define kEplObdPartDev         0x04	// device part       (0x6000 - 0x9FFF)
+#define kEplObdPartUsr         0x08	// dynamic part e.g. for ICE61131-3
+
+// combinations
+#define kEplObdPartApp         (              kEplObdPartMan | kEplObdPartDev | kEplObdPartUsr)	// manufacturer and device part (0x2000 - 0x9FFF) and user OD
+#define kEplObdPartAll         (kEplObdPartGen | kEplObdPartMan | kEplObdPartDev | kEplObdPartUsr)	// whole OD
+
+//-----------------------------------------------------------------------------------------------------------
+// access types for objects
+// must be a difine because bit-flags
+typedef unsigned int tEplObdAccess;
+
+#define kEplObdAccRead         0x01	// object can be read
+#define kEplObdAccWrite        0x02	// object can be written
+#define kEplObdAccConst        0x04	// object contains a constant value
+#define kEplObdAccPdo          0x08	// object can be mapped in a PDO
+#define kEplObdAccArray        0x10	// object contains an array of numerical values
+#define kEplObdAccRange        0x20	// object contains lower and upper limit
+#define kEplObdAccVar          0x40	// object data is placed in application
+#define kEplObdAccStore        0x80	// object data can be stored to non volatile memory
+
+// combinations (not all combinations are required)
+#define kEplObdAccR            (0            | 0          | 0            | 0          | 0            | 0            | kEplObdAccRead)
+#define kEplObdAccW            (0            | 0          | 0            | 0          | 0            | kEplObdAccWrite | 0          )
+#define kEplObdAccRW           (0            | 0          | 0            | 0          | 0            | kEplObdAccWrite | kEplObdAccRead)
+#define kEplObdAccCR           (0            | 0          | 0            | 0          | kEplObdAccConst | 0            | kEplObdAccRead)
+#define kEplObdAccGR           (0            | 0          | kEplObdAccRange | 0          | 0            | 0            | kEplObdAccRead)
+#define kEplObdAccGW           (0            | 0          | kEplObdAccRange | 0          | 0            | kEplObdAccWrite | 0          )
+#define kEplObdAccGRW          (0            | 0          | kEplObdAccRange | 0          | 0            | kEplObdAccWrite | kEplObdAccRead)
+#define kEplObdAccVR           (0            | kEplObdAccVar | 0            | 0          | 0            | 0            | kEplObdAccRead)
+#define kEplObdAccVW           (0            | kEplObdAccVar | 0            | 0          | 0            | kEplObdAccWrite | 0          )
+#define kEplObdAccVRW          (0            | kEplObdAccVar | 0            | 0          | 0            | kEplObdAccWrite | kEplObdAccRead)
+#define kEplObdAccVPR          (0            | kEplObdAccVar | 0            | kEplObdAccPdo | 0            | 0            | kEplObdAccRead)
+#define kEplObdAccVPW          (0            | kEplObdAccVar | 0            | kEplObdAccPdo | 0            | kEplObdAccWrite | 0          )
+#define kEplObdAccVPRW         (0            | kEplObdAccVar | 0            | kEplObdAccPdo | 0            | kEplObdAccWrite | kEplObdAccRead)
+#define kEplObdAccVGR          (0            | kEplObdAccVar | kEplObdAccRange | 0          | 0            | 0            | kEplObdAccRead)
+#define kEplObdAccVGW          (0            | kEplObdAccVar | kEplObdAccRange | 0          | 0            | kEplObdAccWrite | 0          )
+#define kEplObdAccVGRW         (0            | kEplObdAccVar | kEplObdAccRange | 0          | 0            | kEplObdAccWrite | kEplObdAccRead)
+#define kEplObdAccVGPR         (0            | kEplObdAccVar | kEplObdAccRange | kEplObdAccPdo | 0            | 0            | kEplObdAccRead)
+#define kEplObdAccVGPW         (0            | kEplObdAccVar | kEplObdAccRange | kEplObdAccPdo | 0            | kEplObdAccWrite | 0          )
+#define kEplObdAccVGPRW        (0            | kEplObdAccVar | kEplObdAccRange | kEplObdAccPdo | 0            | kEplObdAccWrite | kEplObdAccRead)
+#define kEplObdAccSR           (kEplObdAccStore | 0          | 0            | 0          | 0            | 0            | kEplObdAccRead)
+#define kEplObdAccSW           (kEplObdAccStore | 0          | 0            | 0          | 0            | kEplObdAccWrite | 0          )
+#define kEplObdAccSRW          (kEplObdAccStore | 0          | 0            | 0          | 0            | kEplObdAccWrite | kEplObdAccRead)
+#define kEplObdAccSCR          (kEplObdAccStore | 0          | 0            | 0          | kEplObdAccConst | 0            | kEplObdAccRead)
+#define kEplObdAccSGR          (kEplObdAccStore | 0          | kEplObdAccRange | 0          | 0            | 0            | kEplObdAccRead)
+#define kEplObdAccSGW          (kEplObdAccStore | 0          | kEplObdAccRange | 0          | 0            | kEplObdAccWrite | 0          )
+#define kEplObdAccSGRW         (kEplObdAccStore | 0          | kEplObdAccRange | 0          | 0            | kEplObdAccWrite | kEplObdAccRead)
+#define kEplObdAccSVR          (kEplObdAccStore | kEplObdAccVar | 0            | 0          | 0            | 0            | kEplObdAccRead)
+#define kEplObdAccSVW          (kEplObdAccStore | kEplObdAccVar | 0            | 0          | 0            | kEplObdAccWrite | 0          )
+#define kEplObdAccSVRW         (kEplObdAccStore | kEplObdAccVar | 0            | 0          | 0            | kEplObdAccWrite | kEplObdAccRead)
+#define kEplObdAccSVPR         (kEplObdAccStore | kEplObdAccVar | 0            | kEplObdAccPdo | 0            | 0            | kEplObdAccRead)
+#define kEplObdAccSVPW         (kEplObdAccStore | kEplObdAccVar | 0            | kEplObdAccPdo | 0            | kEplObdAccWrite | 0          )
+#define kEplObdAccSVPRW        (kEplObdAccStore | kEplObdAccVar | 0            | kEplObdAccPdo | 0            | kEplObdAccWrite | kEplObdAccRead)
+#define kEplObdAccSVGR         (kEplObdAccStore | kEplObdAccVar | kEplObdAccRange | 0          | 0            | 0            | kEplObdAccRead)
+#define kEplObdAccSVGW         (kEplObdAccStore | kEplObdAccVar | kEplObdAccRange | 0          | 0            | kEplObdAccWrite | 0          )
+#define kEplObdAccSVGRW        (kEplObdAccStore | kEplObdAccVar | kEplObdAccRange | 0          | 0            | kEplObdAccWrite | kEplObdAccRead)
+#define kEplObdAccSVGPR        (kEplObdAccStore | kEplObdAccVar | kEplObdAccRange | kEplObdAccPdo | 0            | 0            | kEplObdAccRead)
+#define kEplObdAccSVGPW        (kEplObdAccStore | kEplObdAccVar | kEplObdAccRange | kEplObdAccPdo | 0            | kEplObdAccWrite | 0          )
+#define kEplObdAccSVGPRW       (kEplObdAccStore | kEplObdAccVar | kEplObdAccRange | kEplObdAccPdo | 0            | kEplObdAccWrite | kEplObdAccRead)
+
+typedef unsigned int tEplObdSize;	// For all objects as objects size are used an unsigned int.
+
+// -------------------------------------------------------------------------
+// types for data types defined in DS301
+// -------------------------------------------------------------------------
+
+// types of objects in object dictionary
+// DS-301 defines these types as WORD
+typedef enum {
+// types which are always supported
+	kEplObdTypBool = 0x0001,
+
+	kEplObdTypInt8 = 0x0002,
+	kEplObdTypInt16 = 0x0003,
+	kEplObdTypInt32 = 0x0004,
+	kEplObdTypUInt8 = 0x0005,
+	kEplObdTypUInt16 = 0x0006,
+	kEplObdTypUInt32 = 0x0007,
+	kEplObdTypReal32 = 0x0008,
+	kEplObdTypVString = 0x0009,
+	kEplObdTypOString = 0x000A,
+	kEplObdTypDomain = 0x000F,
+
+	kEplObdTypInt24 = 0x0010,
+	kEplObdTypUInt24 = 0x0016,
+
+	kEplObdTypReal64 = 0x0011,
+	kEplObdTypInt40 = 0x0012,
+	kEplObdTypInt48 = 0x0013,
+	kEplObdTypInt56 = 0x0014,
+	kEplObdTypInt64 = 0x0015,
+	kEplObdTypUInt40 = 0x0018,
+	kEplObdTypUInt48 = 0x0019,
+	kEplObdTypUInt56 = 0x001A,
+	kEplObdTypUInt64 = 0x001B,
+	kEplObdTypTimeOfDay = 0x000C,
+	kEplObdTypTimeDiff = 0x000D
+} tEplObdType;
+// other types are not supported in this version
+
+// -------------------------------------------------------------------------
+// types for data types defined in DS301
+// -------------------------------------------------------------------------
+
+typedef unsigned char tEplObdBoolean;	// 0001
+typedef signed char tEplObdInteger8;	// 0002
+typedef signed short int tEplObdInteger16;	// 0003
+typedef signed long tEplObdInteger32;	// 0004
+typedef unsigned char tEplObdUnsigned8;	// 0005
+typedef unsigned short int tEplObdUnsigned16;	// 0006
+typedef unsigned long tEplObdUnsigned32;	// 0007
+typedef float tEplObdReal32;	// 0008
+typedef unsigned char tEplObdDomain;	// 000F
+typedef signed long tEplObdInteger24;	// 0010
+typedef unsigned long tEplObdUnsigned24;	// 0016
+
+typedef signed QWORD tEplObdInteger40;	// 0012
+typedef signed QWORD tEplObdInteger48;	// 0013
+typedef signed QWORD tEplObdInteger56;	// 0014
+typedef signed QWORD tEplObdInteger64;	// 0015
+
+typedef unsigned QWORD tEplObdUnsigned40;	// 0018
+typedef unsigned QWORD tEplObdUnsigned48;	// 0019
+typedef unsigned QWORD tEplObdUnsigned56;	// 001A
+typedef unsigned QWORD tEplObdUnsigned64;	// 001B
+
+typedef double tEplObdReal64;	// 0011
+
+typedef tTimeOfDay tEplObdTimeOfDay;	// 000C
+typedef tTimeOfDay tEplObdTimeDifference;	// 000D
+
+// -------------------------------------------------------------------------
+// structur for defining a variable
+// -------------------------------------------------------------------------
+// -------------------------------------------------------------------------
+typedef enum {
+	kVarValidSize = 0x01,
+	kVarValidData = 0x02,
+//    kVarValidCallback       = 0x04,
+//    kVarValidArg            = 0x08,
+
+	kVarValidAll = 0x03	// currently only size and data are implemented and used
+} tEplVarParamValid;
+
+typedef tEplKernel(PUBLIC ROM * tEplVarCallback) (CCM_DECL_INSTANCE_HDL_
+						  void *pParam_p);
+
+typedef struct {
+	tEplVarParamValid m_ValidFlag;
+	unsigned int m_uiIndex;
+	unsigned int m_uiSubindex;
+	tEplObdSize m_Size;
+	void MEM *m_pData;
+//    tEplVarCallback     m_fpCallback;
+//    void *       m_pArg;
+
+} tEplVarParam;
+
+typedef struct {
+	void MEM *m_pData;
+	tEplObdSize m_Size;
+/*
+    #if (EPL_PDO_USE_STATIC_MAPPING == FALSE)
+        tEplVarCallback    m_fpCallback;
+        void *   m_pArg;
+    #endif
+*/
+} tEplObdVarEntry;
+
+typedef struct {
+	tEplObdSize m_Size;
+	BYTE *m_pString;
+
+} tEplObdOString;		// 000C
+
+typedef struct {
+	tEplObdSize m_Size;
+	char *m_pString;
+} tEplObdVString;		// 000D
+
+typedef struct {
+	tEplObdSize m_Size;
+	char *m_pDefString;	// $$$ d.k. it is unused, so we could delete it
+	char *m_pString;
+
+} tEplObdVStringDef;
+
+typedef struct {
+	tEplObdSize m_Size;
+	BYTE *m_pDefString;	// $$$ d.k. it is unused, so we could delete it
+	BYTE *m_pString;
+
+} tEplObdOStringDef;
+
+//r.d. parameter struct for changing object size and/or pointer to data of Strings or Domains
+typedef struct {
+	tEplObdSize m_DownloadSize;	// download size from SDO or APP
+	tEplObdSize m_ObjSize;	// current object size from OD - should be changed from callback function
+	void *m_pData;		// current object ptr  from OD - should be changed from callback function
+
+} tEplObdVStringDomain;		// 000D
+
+// ============================================================================
+// types
+// ============================================================================
+// -------------------------------------------------------------------------
+// subindexstruct
+// -------------------------------------------------------------------------
+
+// Change not the order for this struct!!!
+typedef struct {
+	unsigned int m_uiSubIndex;
+	tEplObdType m_Type;
+	tEplObdAccess m_Access;
+	void *m_pDefault;
+	void MEM *m_pCurrent;	// points always to RAM
+
+} tEplObdSubEntry;
+
+// r.d.: has always to be  because new OBD-Macros for arrays
+typedef tEplObdSubEntry *tEplObdSubEntryPtr;
+
+// -------------------------------------------------------------------------
+// callback function for objdictionary modul
+// -------------------------------------------------------------------------
+
+// parameters for callback function
+typedef struct {
+	tEplObdEvent m_ObdEvent;
+	unsigned int m_uiIndex;
+	unsigned int m_uiSubIndex;
+	void *m_pArg;
+	DWORD m_dwAbortCode;
+
+} tEplObdCbParam;
+
+// define type for callback function: pParam_p points to tEplObdCbParam
+typedef tEplKernel(PUBLIC ROM * tEplObdCallback) (CCM_DECL_INSTANCE_HDL_
+						  tEplObdCbParam MEM *
+						  pParam_p);
+
+// do not change the order for this struct!!!
+
+typedef struct {
+	unsigned int m_uiIndex;
+	tEplObdSubEntryPtr m_pSubIndex;
+	unsigned int m_uiCount;
+	tEplObdCallback m_fpCallback;	// function is called back if object access
+
+} tEplObdEntry;
+
+// allways  pointer
+typedef tEplObdEntry *tEplObdEntryPtr;
+
+// -------------------------------------------------------------------------
+// structur to initialize OBD module
+// -------------------------------------------------------------------------
+
+typedef struct {
+	tEplObdEntryPtr m_pPart;
+	tEplObdEntryPtr m_pManufacturerPart;
+	tEplObdEntryPtr m_pDevicePart;
+
+#if (defined (EPL_OBD_USER_OD) && (EPL_OBD_USER_OD != FALSE))
+
+	tEplObdEntryPtr m_pUserPart;
+
+#endif
+
+} tEplObdInitParam;
+
+// -------------------------------------------------------------------------
+// structur for parameters of STORE RESTORE command
+// -------------------------------------------------------------------------
+
+typedef struct {
+	tEplObdCommand m_bCommand;
+	tEplObdPart m_bCurrentOdPart;
+	void MEM *m_pData;
+	tEplObdSize m_ObjSize;
+
+} tEplObdCbStoreParam;
+
+typedef tEplKernel(PUBLIC ROM * tInitTabEntryCallback) (void MEM * pTabEntry_p,
+							unsigned int
+							uiObjIndex_p);
+
+typedef tEplKernel(PUBLIC ROM *
+		   tEplObdStoreLoadObjCallback) (CCM_DECL_INSTANCE_HDL_
+						 tEplObdCbStoreParam MEM *
+						 pCbStoreParam_p);
+
+// -------------------------------------------------------------------------
+// this stucture is used for parameters for function ObdInitModuleTab()
+// -------------------------------------------------------------------------
+typedef struct {
+	unsigned int m_uiLowerObjIndex;	// lower limit of ObjIndex
+	unsigned int m_uiUpperObjIndex;	// upper limit of ObjIndex
+	tInitTabEntryCallback m_fpInitTabEntry;	// will be called if ObjIndex was found
+	void MEM *m_pTabBase;	// base address of table
+	unsigned int m_uiEntrySize;	// size of table entry      // 25-feb-2005 r.d.: expansion from BYTE to WORD necessary for PDO bit mapping
+	unsigned int m_uiMaxEntries;	// max. tabel entries
+
+} tEplObdModulTabParam;
+
+//-------------------------------------------------------------------
+//  enum for function EplObdSetNodeId
+//-------------------------------------------------------------------
+typedef enum {
+	kEplObdNodeIdUnknown = 0x00,	// unknown how the node id was set
+	kEplObdNodeIdSoftware = 0x01,	// node id set by software
+	kEplObdNodeIdHardware = 0x02	// node id set by hardware
+} tEplObdNodeIdType;
+
+// ============================================================================
+// global variables
+// ============================================================================
+
+// ============================================================================
+// public functions
+// ============================================================================
+
+#endif // #ifndef _EPLOBD_H_
diff --git a/drivers/staging/epl/EplObdMacro.h b/drivers/staging/epl/EplObdMacro.h
new file mode 100644
index 00000000000..23f2ad80dde
--- /dev/null
+++ b/drivers/staging/epl/EplObdMacro.h
@@ -0,0 +1,354 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for macros of EplOBD-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplObdMacro.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/05 k.t.:   start of the implementation
+                    -> based on CANopen ObdMacro.h
+
+****************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#if defined (EPL_OBD_DEFINE_MACRO)
+
+    //-------------------------------------------------------------------------------------------
+#if defined (EPL_OBD_CREATE_ROM_DATA)
+
+//        #pragma message ("EPL_OBD_CREATE_ROM_DATA")
+
+#define EPL_OBD_BEGIN()                                                         static  DWORD  dwObd_OBK_g = 0x0000;
+#define EPL_OBD_END()
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_BEGIN_PART_GENERIC()
+#define EPL_OBD_BEGIN_PART_MANUFACTURER()
+#define EPL_OBD_BEGIN_PART_DEVICE()
+#define EPL_OBD_END_PART()
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_BEGIN_INDEX_RAM(ind,cnt,call)
+#define EPL_OBD_END_INDEX(ind)
+#define EPL_OBD_RAM_INDEX_RAM_ARRAY(ind,cnt,call,typ,acc,dtyp,name,def)         static  tEplObdUnsigned8    xDef##ind##_0x00_g = (cnt); \
+                                                                                        static  dtyp  xDef##ind##_0x01_g = (def);
+#define EPL_OBD_RAM_INDEX_RAM_VARARRAY(ind,cnt,call,typ,acc,dtyp,name,def)      static  tEplObdUnsigned8    xDef##ind##_0x00_g = (cnt); \
+                                                                                        static  dtyp  xDef##ind##_0x01_g = (def);
+#define EPL_OBD_RAM_INDEX_RAM_VARARRAY_NOINIT(ind,cnt,call,typ,acc,dtyp,name)   static  tEplObdUnsigned8    xDef##ind##_0x00_g = (cnt);
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_SUBINDEX_RAM_VAR(ind,sub,typ,acc,dtyp,name,val)                 static  dtyp  xDef##ind##_##sub##_g        = val;
+#define EPL_OBD_SUBINDEX_RAM_VAR_RG(ind,sub,typ,acc,dtyp,name,val,low,high)     static  dtyp  xDef##ind##_##sub##_g[3]     = {val,low,high};
+#define EPL_OBD_SUBINDEX_RAM_VAR_NOINIT(ind,sub,typ,acc,dtyp,name)
+#define EPL_OBD_SUBINDEX_RAM_VSTRING(ind,sub,acc,name,size,val)                 static char  MEM szCur##ind##_##sub##_g[size+1]; \
+                                                                                        static  tEplObdVStringDef  xDef##ind##_##sub##_g = {size, val, szCur##ind##_##sub##_g};
+
+#define EPL_OBD_SUBINDEX_RAM_OSTRING(ind,sub,acc,name,size)                     static  BYTE  MEM bCur##ind##_##sub##_g[size]; \
+                                                                                        static  tEplObdOStringDef  xDef##ind##_##sub##_g = {size, ((BYTE*)""), bCur##ind##_##sub##_g};
+#define EPL_OBD_SUBINDEX_RAM_DOMAIN(ind,sub,acc,name)
+#define EPL_OBD_SUBINDEX_RAM_USERDEF(ind,sub,typ,acc,dtyp,name,val)             static  dtyp  xDef##ind##_##sub##_g        = val;
+#define EPL_OBD_SUBINDEX_RAM_USERDEF_RG(ind,sub,typ,acc,dtyp,name,val,low,high) static  dtyp  xDef##ind##_##sub##_g[3]     = {val,low,high};
+#define EPL_OBD_SUBINDEX_RAM_USERDEF_NOINIT(ind,sub,typ,acc,dtyp,name)
+
+//-------------------------------------------------------------------------------------------
+#elif defined (EPL_OBD_CREATE_RAM_DATA)
+
+//        #pragma message ("EPL_OBD_CREATE_RAM_DATA")
+
+#define EPL_OBD_BEGIN()
+#define EPL_OBD_END()
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_BEGIN_PART_GENERIC()
+#define EPL_OBD_BEGIN_PART_MANUFACTURER()
+#define EPL_OBD_BEGIN_PART_DEVICE()
+#define EPL_OBD_END_PART()
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_BEGIN_INDEX_RAM(ind,cnt,call)
+#define EPL_OBD_END_INDEX(ind)
+#define EPL_OBD_RAM_INDEX_RAM_ARRAY(ind,cnt,call,typ,acc,dtyp,name,def)         static dtyp         MEM axCur##ind##_g[cnt];
+#define EPL_OBD_RAM_INDEX_RAM_VARARRAY(ind,cnt,call,typ,acc,dtyp,name,def)      static tEplObdVarEntry MEM aVarEntry##ind##_g[cnt];
+#define EPL_OBD_RAM_INDEX_RAM_VARARRAY_NOINIT(ind,cnt,call,typ,acc,dtyp,name)   static tEplObdVarEntry MEM aVarEntry##ind##_g[cnt];
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_SUBINDEX_RAM_VAR(ind,sub,typ,acc,dtyp,name,val)                 static dtyp         MEM xCur##ind##_##sub##_g;
+#define EPL_OBD_SUBINDEX_RAM_VAR_RG(ind,sub,typ,acc,dtyp,name,val,low,high)     static dtyp         MEM xCur##ind##_##sub##_g;
+#define EPL_OBD_SUBINDEX_RAM_VSTRING(ind,sub,acc,name,size,val)                 static tEplObdVString  MEM xCur##ind##_##sub##_g;
+#define EPL_OBD_SUBINDEX_RAM_OSTRING(ind,sub,acc,name,size)                     static tEplObdOString  MEM xCur##ind##_##sub##_g;
+#define EPL_OBD_SUBINDEX_RAM_VAR_NOINIT(ind,sub,typ,acc,dtyp,name)              static dtyp         MEM xCur##ind##_##sub##_g;
+#define EPL_OBD_SUBINDEX_RAM_DOMAIN(ind,sub,acc,name)                           static tEplObdVarEntry MEM VarEntry##ind##_##sub##_g;
+#define EPL_OBD_SUBINDEX_RAM_USERDEF(ind,sub,typ,acc,dtyp,name,val)             static tEplObdVarEntry MEM VarEntry##ind##_##sub##_g;
+#define EPL_OBD_SUBINDEX_RAM_USERDEF_RG(ind,sub,typ,acc,dtyp,name,val,low,high) static tEplObdVarEntry MEM VarEntry##ind##_##sub##_g;
+#define EPL_OBD_SUBINDEX_RAM_USERDEF_NOINIT(ind,sub,typ,acc,dtyp,name)          static tEplObdVarEntry MEM VarEntry##ind##_##sub##_g;
+
+    //-------------------------------------------------------------------------------------------
+#elif defined (EPL_OBD_CREATE_SUBINDEX_TAB)
+
+//        #pragma message ("EPL_OBD_CREATE_SUBINDEX_TAB")
+
+#define EPL_OBD_BEGIN()
+#define EPL_OBD_END()
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_BEGIN_PART_GENERIC()
+#define EPL_OBD_BEGIN_PART_MANUFACTURER()
+#define EPL_OBD_BEGIN_PART_DEVICE()
+#define EPL_OBD_END_PART()
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_BEGIN_INDEX_RAM(ind,cnt,call)                                   static tEplObdSubEntry MEM aObdSubEntry##ind##Ram_g[cnt]= {
+#define EPL_OBD_END_INDEX(ind)                                                  EPL_OBD_END_SUBINDEX()};
+#define EPL_OBD_RAM_INDEX_RAM_ARRAY(ind,cnt,call,typ,acc,dtyp,name,def)         static tEplObdSubEntry MEM aObdSubEntry##ind##Ram_g[]= { \
+                                                                                        {0, kEplObdTypUInt8, kEplObdAccCR,          &xDef##ind##_0x00_g,   NULL}, \
+                                                                                        {1, typ,          (acc)|kEplObdAccArray, &xDef##ind##_0x01_g,   &axCur##ind##_g[0]}, \
+                                                                                        EPL_OBD_END_SUBINDEX()};
+#define EPL_OBD_RAM_INDEX_RAM_VARARRAY(ind,cnt,call,typ,acc,dtyp,name,def)      static tEplObdSubEntry MEM aObdSubEntry##ind##Ram_g[]= { \
+                                                                                        {0, kEplObdTypUInt8, kEplObdAccCR,                     &xDef##ind##_0x00_g,   NULL}, \
+                                                                                        {1, typ,          (acc)|kEplObdAccArray|kEplObdAccVar, &xDef##ind##_0x01_g,   &aVarEntry##ind##_g[0]}, \
+                                                                                        EPL_OBD_END_SUBINDEX()};
+#define EPL_OBD_RAM_INDEX_RAM_VARARRAY_NOINIT(ind,cnt,call,typ,acc,dtyp,name)   static tEplObdSubEntry MEM aObdSubEntry##ind##Ram_g[]= { \
+                                                                                        {0, kEplObdTypUInt8, kEplObdAccCR,                     &xDef##ind##_0x00_g,   NULL}, \
+                                                                                        {1, typ,          (acc)|kEplObdAccArray|kEplObdAccVar, NULL,                  &aVarEntry##ind##_g[0]}, \
+                                                                                        EPL_OBD_END_SUBINDEX()};
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_SUBINDEX_RAM_VAR(ind,sub,typ,acc,dtyp,name,val)                 {sub,typ,            (acc),                        &xDef##ind##_##sub##_g,   &xCur##ind##_##sub##_g},
+#define EPL_OBD_SUBINDEX_RAM_VAR_RG(ind,sub,typ,acc,dtyp,name,val,low,high)     {sub,typ,            (acc)|kEplObdAccRange,           &xDef##ind##_##sub##_g[0],&xCur##ind##_##sub##_g},
+#define EPL_OBD_SUBINDEX_RAM_VAR_NOINIT(ind,sub,typ,acc,dtyp,name)              {sub,typ,            (acc),                        NULL,   &xCur##ind##_##sub##_g},
+#define EPL_OBD_SUBINDEX_RAM_VSTRING(ind,sub,acc,name,size,val)                 {sub,kEplObdTypVString,(acc)/*|kEplObdAccVar*/,         &xDef##ind##_##sub##_g,   &xCur##ind##_##sub##_g},
+#define EPL_OBD_SUBINDEX_RAM_OSTRING(ind,sub,acc,name,size)                     {sub,kEplObdTypOString,(acc)/*|kEplObdAccVar*/,         &xDef##ind##_##sub##_g,   &xCur##ind##_##sub##_g},
+#define EPL_OBD_SUBINDEX_RAM_DOMAIN(ind,sub,acc,name)                           {sub,kEplObdTypDomain, (acc)|kEplObdAccVar,             NULL,                     &VarEntry##ind##_##sub##_g},
+#define EPL_OBD_SUBINDEX_RAM_USERDEF(ind,sub,typ,acc,dtyp,name,val)             {sub,typ,           (acc)|kEplObdAccVar,             &xDef##ind##_##sub##_g,   &VarEntry##ind##_##sub##_g},
+#define EPL_OBD_SUBINDEX_RAM_USERDEF_RG(ind,sub,typ,acc,dtyp,name,val,low,high) {sub,typ,           (acc)|kEplObdAccVar|kEplObdAccRange,&xDef##ind##_##sub##_g[0],&VarEntry##ind##_##sub##_g},
+#define EPL_OBD_SUBINDEX_RAM_USERDEF_NOINIT(ind,sub,typ,acc,dtyp,name)          {sub,typ,           (acc)|kEplObdAccVar,             NULL,    &VarEntry##ind##_##sub##_g},
+
+    //-------------------------------------------------------------------------------------------
+#elif defined (EPL_OBD_CREATE_INDEX_TAB)
+
+//        #pragma message ("EPL_OBD_CREATE_INDEX_TAB")
+
+#define EPL_OBD_BEGIN()
+#define EPL_OBD_END()
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_BEGIN_PART_GENERIC()                                                   static  tEplObdEntry  aObdTab_g[]      = {
+#define EPL_OBD_BEGIN_PART_MANUFACTURER()                                       static  tEplObdEntry  aObdTabManufacturer_g[] = {
+#define EPL_OBD_BEGIN_PART_DEVICE()                                             static  tEplObdEntry  aObdTabDevice_g[]       = {
+#define EPL_OBD_END_PART()                                                      {EPL_OBD_TABLE_INDEX_END,(tEplObdSubEntryPtr)&dwObd_OBK_g,0,NULL}};
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_BEGIN_INDEX_RAM(ind,cnt,call)                                   {ind,(tEplObdSubEntryPtr)&aObdSubEntry##ind##Ram_g[0],cnt,(tEplObdCallback)call},
+#define EPL_OBD_END_INDEX(ind)
+#define EPL_OBD_RAM_INDEX_RAM_ARRAY(ind,cnt,call,typ,acc,dtyp,name,def)         {ind,(tEplObdSubEntryPtr)&aObdSubEntry##ind##Ram_g[0],(cnt)+1,(tEplObdCallback)call},
+#define EPL_OBD_RAM_INDEX_RAM_VARARRAY(ind,cnt,call,typ,acc,dtyp,name,def)      {ind,(tEplObdSubEntryPtr)&aObdSubEntry##ind##Ram_g[0],(cnt)+1,(tEplObdCallback)call},
+#define EPL_OBD_RAM_INDEX_RAM_VARARRAY_NOINIT(ind,cnt,call,typ,acc,dtyp,name)   {ind,(tEplObdSubEntryPtr)&aObdSubEntry##ind##Ram_g[0],(cnt)+1,(tEplObdCallback)call},
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_SUBINDEX_RAM_VAR(ind,sub,typ,acc,dtyp,name,val)
+#define EPL_OBD_SUBINDEX_RAM_VAR_RG(ind,sub,typ,acc,dtyp,name,val,low,high)
+#define EPL_OBD_SUBINDEX_RAM_VSTRING(ind,sub,acc,name,size,val)
+#define EPL_OBD_SUBINDEX_RAM_OSTRING(ind,sub,acc,name,size)
+#define EPL_OBD_SUBINDEX_RAM_VAR_NOINIT(ind,sub,typ,acc,dtyp,name)
+#define EPL_OBD_SUBINDEX_RAM_DOMAIN(ind,sub,acc,name)
+#define EPL_OBD_SUBINDEX_RAM_USERDEF(ind,sub,typ,acc,dtyp,name,val)
+#define EPL_OBD_SUBINDEX_RAM_USERDEF_RG(ind,sub,typ,acc,dtyp,name,val,low,high)
+#define EPL_OBD_SUBINDEX_RAM_USERDEF_NOINIT(ind,sub,typ,acc,dtyp,name)
+
+	    //-------------------------------------------------------------------------------------------
+#elif defined (EPL_OBD_CREATE_INIT_FUNCTION)
+
+//        #pragma message ("EPL_OBD_CREATE_INIT_FUNCTION")
+
+#define EPL_OBD_BEGIN()
+#define EPL_OBD_END()
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_BEGIN_PART_GENERIC()                                                   pInitParam->m_pPart      = (tEplObdEntryPtr) &aObdTab_g[0];
+#define EPL_OBD_BEGIN_PART_MANUFACTURER()                                       pInitParam->m_pManufacturerPart = (tEplObdEntryPtr) &aObdTabManufacturer_g[0];
+#define EPL_OBD_BEGIN_PART_DEVICE()                                             pInitParam->m_pDevicePart       = (tEplObdEntryPtr) &aObdTabDevice_g[0];
+#define EPL_OBD_END_PART()
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_BEGIN_INDEX_RAM(ind,cnt,call)
+#define EPL_OBD_END_INDEX(ind)
+#define EPL_OBD_RAM_INDEX_RAM_ARRAY(ind,cnt,call,typ,acc,dtyp,name,def)
+#define EPL_OBD_RAM_INDEX_RAM_VARARRAY(ind,cnt,call,typ,acc,dtyp,name,def)
+#define EPL_OBD_RAM_INDEX_RAM_VARARRAY_NOINIT(ind,cnt,call,typ,acc,dtyp,name)
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_SUBINDEX_RAM_VAR(ind,sub,typ,acc,dtyp,name,val)
+#define EPL_OBD_SUBINDEX_RAM_VAR_RG(ind,sub,typ,acc,dtyp,name,val,low,high)
+#define EPL_OBD_SUBINDEX_RAM_VSTRING(ind,sub,acc,name,size,val)
+#define EPL_OBD_SUBINDEX_RAM_OSTRING(ind,sub,acc,name,size)
+#define EPL_OBD_SUBINDEX_RAM_VAR_NOINIT(ind,sub,typ,acc,dtyp,name)
+#define EPL_OBD_SUBINDEX_RAM_DOMAIN(ind,sub,acc,name)
+#define EPL_OBD_SUBINDEX_RAM_USERDEF(ind,sub,typ,acc,dtyp,name,val)
+#define EPL_OBD_SUBINDEX_RAM_USERDEF_RG(ind,sub,typ,acc,dtyp,name,val,low,high)
+#define EPL_OBD_SUBINDEX_RAM_USERDEF_NOINIT(ind,sub,typ,acc,dtyp,name)
+
+    //-------------------------------------------------------------------------------------------
+#elif defined (EPL_OBD_CREATE_INIT_SUBINDEX)
+
+//        #pragma message ("EPL_OBD_CREATE_INIT_SUBINDEX")
+
+#define EPL_OBD_BEGIN()
+#define EPL_OBD_END()
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_BEGIN_PART_GENERIC()
+#define EPL_OBD_BEGIN_PART_MANUFACTURER()
+#define EPL_OBD_BEGIN_PART_DEVICE()
+#define EPL_OBD_END_PART()
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_BEGIN_INDEX_RAM(ind,cnt,call)	//CCM_SUBINDEX_RAM_ONLY (EPL_MEMCPY (&aObdSubEntry##ind##Ram_g[0],&aObdSubEntry##ind##Rom_g[0],sizeof(aObdSubEntry##ind##Ram_g)));
+#define EPL_OBD_END_INDEX(ind)
+#define EPL_OBD_RAM_INDEX_RAM_ARRAY(ind,cnt,call,typ,acc,dtyp,name,def)	//EPL_MEMCPY (&aObdSubEntry##ind##Ram_g[0],&aObdSubEntry##ind##Rom_g[0],sizeof(aObdSubEntry##ind##Ram_g));
+#define EPL_OBD_RAM_INDEX_RAM_VARARRAY(ind,cnt,call,typ,acc,dtyp,name,def)	//EPL_MEMCPY (&aObdSubEntry##ind##Ram_g[0],&aObdSubEntry##ind##Rom_g[0],sizeof(aObdSubEntry##ind##Ram_g));
+#define EPL_OBD_RAM_INDEX_RAM_VARARRAY_NOINIT(ind,cnt,call,typ,acc,dtyp,name)	//EPL_MEMCPY (&aObdSubEntry##ind##Ram_g[0],&aObdSubEntry##ind##Rom_g[0],sizeof(aObdSubEntry##ind##Ram_g));
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_SUBINDEX_RAM_VAR(ind,sub,typ,acc,dtyp,name,val)
+#define EPL_OBD_SUBINDEX_RAM_VAR_RG(ind,sub,typ,acc,dtyp,name,val,low,high)
+#define EPL_OBD_SUBINDEX_RAM_VSTRING(ind,sub,acc,name,size,val)
+#define EPL_OBD_SUBINDEX_RAM_OSTRING(ind,sub,acc,name,size)
+#define EPL_OBD_SUBINDEX_RAM_VAR_NOINIT(ind,sub,typ,acc,dtyp,name)
+#define EPL_OBD_SUBINDEX_RAM_DOMAIN(ind,sub,acc,name)
+#define EPL_OBD_SUBINDEX_RAM_USERDEF(ind,sub,typ,acc,dtyp,name,val)
+#define EPL_OBD_SUBINDEX_RAM_USERDEF_RG(ind,sub,typ,acc,dtyp,name,val,low,high)
+#define EPL_OBD_SUBINDEX_RAM_USERDEF_NOINIT(ind,sub,typ,acc,dtyp,name)
+
+    //-------------------------------------------------------------------------------------------
+#else
+
+//        #pragma message ("ELSE OF DEFINE")
+
+#define EPL_OBD_BEGIN()
+#define EPL_OBD_END()
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_BEGIN_PART_GENERIC()
+#define EPL_OBD_BEGIN_PART_MANUFACTURER()
+#define EPL_OBD_BEGIN_PART_DEVICE()
+#define EPL_OBD_END_PART()
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_BEGIN_INDEX_RAM(ind,cnt,call)
+#define EPL_OBD_END_INDEX(ind)
+#define EPL_OBD_RAM_INDEX_RAM_ARRAY(ind,cnt,call,typ,acc,dtyp,name,def)
+#define EPL_OBD_RAM_INDEX_RAM_VARARRAY(ind,cnt,call,typ,acc,dtyp,name,def)
+#define EPL_OBD_RAM_INDEX_RAM_VARARRAY_NOINIT(ind,cnt,call,typ,acc,dtyp,name)
+
+	//---------------------------------------------------------------------------------------
+#define EPL_OBD_SUBINDEX_RAM_VAR(ind,sub,typ,acc,dtyp,name,val)
+#define EPL_OBD_SUBINDEX_RAM_VAR_RG(ind,sub,typ,acc,dtyp,name,val,low,high)
+#define EPL_OBD_SUBINDEX_RAM_VSTRING(ind,sub,acc,name,sizes,val)
+#define EPL_OBD_SUBINDEX_RAM_OSTRING(ind,sub,acc,name,size)
+#define EPL_OBD_SUBINDEX_RAM_VAR_NOINIT(ind,sub,typ,acc,dtyp,name)
+#define EPL_OBD_SUBINDEX_RAM_DOMAIN(ind,sub,acc,name)
+#define EPL_OBD_SUBINDEX_RAM_USERDEF(ind,sub,typ,acc,dtyp,name,val)
+#define EPL_OBD_SUBINDEX_RAM_USERDEF_RG(ind,sub,typ,acc,dtyp,name,val,low,high)
+#define EPL_OBD_SUBINDEX_RAM_USERDEF_NOINIT(ind,sub,typ,acc,dtyp,name)
+
+#endif
+
+    //-------------------------------------------------------------------------------------------
+#elif defined (EPL_OBD_UNDEFINE_MACRO)
+
+//    #pragma message ("EPL_OBD_UNDEFINE_MACRO")
+
+#undef EPL_OBD_BEGIN
+#undef EPL_OBD_END
+
+    //---------------------------------------------------------------------------------------
+#undef EPL_OBD_BEGIN_PART_GENERIC
+#undef EPL_OBD_BEGIN_PART_MANUFACTURER
+#undef EPL_OBD_BEGIN_PART_DEVICE
+#undef EPL_OBD_END_PART
+
+    //---------------------------------------------------------------------------------------
+#undef EPL_OBD_BEGIN_INDEX_RAM
+#undef EPL_OBD_END_INDEX
+#undef EPL_OBD_RAM_INDEX_RAM_ARRAY
+#undef EPL_OBD_RAM_INDEX_RAM_VARARRAY
+#undef EPL_OBD_RAM_INDEX_RAM_VARARRAY_NOINIT
+
+    //---------------------------------------------------------------------------------------
+#undef EPL_OBD_SUBINDEX_RAM_VAR
+#undef EPL_OBD_SUBINDEX_RAM_VAR_RG
+#undef EPL_OBD_SUBINDEX_RAM_VSTRING
+#undef EPL_OBD_SUBINDEX_RAM_OSTRING
+#undef EPL_OBD_SUBINDEX_RAM_VAR_NOINIT
+#undef EPL_OBD_SUBINDEX_RAM_DOMAIN
+#undef EPL_OBD_SUBINDEX_RAM_USERDEF
+#undef EPL_OBD_SUBINDEX_RAM_USERDEF_RG
+#undef EPL_OBD_SUBINDEX_RAM_USERDEF_NOINIT
+
+#else
+
+#error "nothing defined"
+
+#endif
diff --git a/drivers/staging/epl/EplObdkCal.c b/drivers/staging/epl/EplObdkCal.c
new file mode 100644
index 00000000000..4c9af89719e
--- /dev/null
+++ b/drivers/staging/epl/EplObdkCal.c
@@ -0,0 +1,147 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for communication abstraction layer
+                for the Epl-Obd-Kernelspace-Modul
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplObdkCal.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.3 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                KEIL uVision 2
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/19 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "EplInc.h"
+#include "kernel/EplObdkCal.h"
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:
+//
+// Description:
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:
+//
+// Description:
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+// EOF
diff --git a/drivers/staging/epl/EplObdu.c b/drivers/staging/epl/EplObdu.c
new file mode 100644
index 00000000000..218d152897c
--- /dev/null
+++ b/drivers/staging/epl/EplObdu.c
@@ -0,0 +1,517 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for Epl-Obd-Userspace-module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplObdu.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/19 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "EplInc.h"
+#include "user/EplObdu.h"
+#include "user/EplObduCal.h"
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0)
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduWriteEntry()
+//
+// Description: Function writes data to an OBD entry. Strings
+//              are stored with added '\0' character.
+//
+// Parameters:  uiIndex_p       =   Index of the OD entry
+//              uiSubIndex_p    =   Subindex of the OD Entry
+//              pSrcData_p      =   Pointer to the data to write
+//              Size_p          =   Size of the data in Byte
+//
+// Return:      tEplKernel      =   Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduWriteEntry(unsigned int uiIndex_p,
+						 unsigned int uiSubIndex_p,
+						 void *pSrcData_p,
+						 tEplObdSize Size_p)
+{
+	tEplKernel Ret;
+
+	Ret = EplObduCalWriteEntry(uiIndex_p, uiSubIndex_p, pSrcData_p, Size_p);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduReadEntry()
+//
+// Description: The function reads an object entry. The application
+//              can always read the data even if attrib kEplObdAccRead
+//              is not set. The attrib is only checked up for SDO transfer.
+//
+// Parameters:  uiIndex_p       = Index oof the OD entry to read
+//              uiSubIndex_p    = Subindex to read
+//              pDstData_p      = pointer to the buffer for data
+//              Offset_p        = offset in data for read access
+//              pSize_p         = IN: Size of the buffer
+//                                OUT: number of readed Bytes
+//
+// Return:      tEplKernel      =   errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduReadEntry(unsigned int uiIndex_p,
+						unsigned int uiSubIndex_p,
+						void *pDstData_p,
+						tEplObdSize * pSize_p)
+{
+	tEplKernel Ret;
+
+	Ret = EplObduCalReadEntry(uiIndex_p, uiSubIndex_p, pDstData_p, pSize_p);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdAccessOdPart()
+//
+// Description: restores default values of one part of OD
+//
+// Parameters:  ObdPart_p       = od-part to reset
+//              Direction_p     = directory flag for
+//
+// Return:      tEplKernel  = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduAccessOdPart(tEplObdPart ObdPart_p,
+						   tEplObdDir Direction_p)
+{
+	tEplKernel Ret;
+
+	Ret = EplObduCalAccessOdPart(ObdPart_p, Direction_p);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduDefineVar()
+//
+// Description: defines a variable in OD
+//
+// Parameters:  pEplVarParam_p = varentry
+//
+// Return:      tEplKernel  =   errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduDefineVar(tEplVarParam MEM * pVarParam_p)
+{
+	tEplKernel Ret;
+
+	Ret = EplObduCalDefineVar(pVarParam_p);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduGetObjectDataPtr()
+//
+// Description: It returnes the current data pointer. But if object is an
+//              constant object it returnes the default pointer.
+//
+// Parameters:  uiIndex_p    =   Index of the entry
+//              uiSubindex_p =   Subindex of the entry
+//
+// Return:      void *    = pointer to object data
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT void *PUBLIC EplObduGetObjectDataPtr(unsigned int uiIndex_p,
+						  unsigned int uiSubIndex_p)
+{
+	void *pData;
+
+	pData = EplObduCalGetObjectDataPtr(uiIndex_p, uiSubIndex_p);
+
+	return pData;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduRegisterUserOd()
+//
+// Description: function registers the user OD
+//
+// Parameters:  pUserOd_p   =pointer to user ODd
+//
+// Return:     tEplKernel = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#if (defined (EPL_OBD_USER_OD) && (EPL_OBD_USER_OD != FALSE))
+EPLDLLEXPORT tEplKernel PUBLIC EplObduRegisterUserOd(tEplObdEntryPtr pUserOd_p)
+{
+	tEplKernel Ret;
+
+	Ret = EplObduCalRegisterUserOd(pUserOd_p);
+
+	return Ret;
+
+}
+#endif
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduInitVarEntry()
+//
+// Description: function to initialize VarEntry dependened on object type
+//
+// Parameters:  pVarEntry_p = pointer to var entry structure
+//              bType_p     = object type
+//              ObdSize_p   = size of object data
+//
+// Returns:     none
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT void PUBLIC EplObduInitVarEntry(tEplObdVarEntry MEM * pVarEntry_p,
+					     BYTE bType_p,
+					     tEplObdSize ObdSize_p)
+{
+	EplObduCalInitVarEntry(pVarEntry_p, bType_p, ObdSize_p);
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduGetDataSize()
+//
+// Description: function to initialize VarEntry dependened on object type
+//
+//              gets the data size of an object
+//              for string objects it returnes the string length
+//
+// Parameters:  uiIndex_p   =   Index
+//              uiSubIndex_p=   Subindex
+//
+// Return:      tEplObdSize
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplObdSize PUBLIC EplObduGetDataSize(unsigned int uiIndex_p,
+						   unsigned int uiSubIndex_p)
+{
+	tEplObdSize Size;
+
+	Size = EplObduCalGetDataSize(uiIndex_p, uiSubIndex_p);
+
+	return Size;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduGetNodeId()
+//
+// Description: function returns nodeid from entry 0x1F93
+//
+//
+// Parameters:
+//
+// Return:      unsigned int = Node Id
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT unsigned int PUBLIC EplObduGetNodeId()
+{
+	unsigned int uiNodeId;
+
+	uiNodeId = EplObduCalGetNodeId();
+
+	return uiNodeId;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduSetNodeId()
+//
+// Description: function sets nodeid in entry 0x1F93
+//
+//
+// Parameters:  uiNodeId_p  =   Node Id to set
+//              NodeIdType_p=   Type on which way the Node Id was set
+//
+// Return:      tEplKernel = Errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduSetNodeId(unsigned int uiNodeId_p,
+						tEplObdNodeIdType NodeIdType_p)
+{
+	tEplKernel Ret;
+
+	Ret = EplObduCalSetNodeId(uiNodeId_p, NodeIdType_p);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduGetAccessType()
+//
+// Description: Function returns accesstype of the entry
+//
+// Parameters:  uiIndex_p       =   Index of the OD entry
+//              uiSubIndex_p    =   Subindex of the OD Entry
+//              pAccessTyp_p    =   pointer to buffer to store accesstyp
+//
+// Return:      tEplKernel      =   Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduGetAccessType(unsigned int uiIndex_p,
+						    unsigned int uiSubIndex_p,
+						    tEplObdAccess *
+						    pAccessTyp_p)
+{
+	tEplObdAccess AccessType;
+
+	AccessType =
+	    EplObduCalGetAccessType(uiIndex_p, uiSubIndex_p, pAccessTyp_p);
+
+	return AccessType;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObdReaduEntryToLe()
+//
+// Description: The function reads an object entry from the byteoder
+//              of the system to the little endian byteorder for numeric values.
+//              For other types a normal read will be processed. This is usefull for
+//              the PDO and SDO module. The application
+//              can always read the data even if attrib kEplObdAccRead
+//              is not set. The attrib is only checked up for SDO transfer.
+//
+// Parameters:  EPL_MCO_DECL_INSTANCE_PTR_
+//              uiIndex_p       = Index of the OD entry to read
+//              uiSubIndex_p    = Subindex to read
+//              pDstData_p      = pointer to the buffer for data
+//              Offset_p        = offset in data for read access
+//              pSize_p         = IN: Size of the buffer
+//                                OUT: number of readed Bytes
+//
+// Return:      tEplKernel
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduReadEntryToLe(unsigned int uiIndex_p,
+						    unsigned int uiSubIndex_p,
+						    void *pDstData_p,
+						    tEplObdSize * pSize_p)
+{
+	tEplKernel Ret;
+
+	Ret =
+	    EplObduCalReadEntryToLe(uiIndex_p, uiSubIndex_p, pDstData_p,
+				    pSize_p);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduWriteEntryFromLe()
+//
+// Description: Function writes data to an OBD entry from a source with
+//              little endian byteorder to the od with system specuific
+//              byteorder. Not numeric values will only by copied. Strings
+//              are stored with added '\0' character.
+//
+// Parameters:  EPL_MCO_DECL_INSTANCE_PTR_
+//              uiIndex_p       =   Index of the OD entry
+//              uiSubIndex_p    =   Subindex of the OD Entry
+//              pSrcData_p      =   Pointer to the data to write
+//              Size_p          =   Size of the data in Byte
+//
+// Return:      tEplKernel      =   Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduWriteEntryFromLe(unsigned int uiIndex_p,
+						       unsigned int
+						       uiSubIndex_p,
+						       void *pSrcData_p,
+						       tEplObdSize Size_p)
+{
+	tEplKernel Ret;
+
+	Ret =
+	    EplObduCalWriteEntryFromLe(uiIndex_p, uiSubIndex_p, pSrcData_p,
+				       Size_p);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduSearchVarEntry()
+//
+// Description: gets variable from OD
+//
+// Parameters:  uiIndex_p       =   index of the var entry to search
+//              uiSubindex_p    =   subindex of var entry to search
+//              ppVarEntry_p    =   pointer to the pointer to the varentry
+//
+// Return:      tEplKernel
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduSearchVarEntry(EPL_MCO_DECL_INSTANCE_PTR_
+						     unsigned int uiIndex_p,
+						     unsigned int uiSubindex_p,
+						     tEplObdVarEntry MEM **
+						     ppVarEntry_p)
+{
+	tEplKernel Ret;
+
+	Ret = EplObduCalSearchVarEntry(uiIndex_p, uiSubindex_p, ppVarEntry_p);
+
+	return Ret;
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:
+//
+// Description:
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+#endif // #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0)
+
+// EOF
diff --git a/drivers/staging/epl/EplObduCal.c b/drivers/staging/epl/EplObduCal.c
new file mode 100644
index 00000000000..85b3df0886b
--- /dev/null
+++ b/drivers/staging/epl/EplObduCal.c
@@ -0,0 +1,558 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for communication abstraction layer
+                for the Epl-Obd-Userspace-Modul
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplObduCal.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.6 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/19 k.t.:   start of the implementation
+
+****************************************************************************/
+#include "EplInc.h"
+#include "user/EplObduCal.h"
+#include "kernel/EplObdk.h"
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0) && (EPL_OBD_USE_KERNEL != FALSE)
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduCalWriteEntry()
+//
+// Description: Function encapsulate access of function EplObdWriteEntry
+//
+// Parameters:  uiIndex_p       =   Index of the OD entry
+//              uiSubIndex_p    =   Subindex of the OD Entry
+//              pSrcData_p      =   Pointer to the data to write
+//              Size_p          =   Size of the data in Byte
+//
+// Return:      tEplKernel      =   Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduCalWriteEntry(unsigned int uiIndex_p,
+						    unsigned int uiSubIndex_p,
+						    void *pSrcData_p,
+						    tEplObdSize Size_p)
+{
+	tEplKernel Ret;
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+	Ret = EplObdWriteEntry(uiIndex_p, uiSubIndex_p, pSrcData_p, Size_p);
+#else
+	Ret = kEplSuccessful;
+#endif
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduCalReadEntry()
+//
+// Description: Function encapsulate access of function EplObdReadEntry
+//
+// Parameters:  uiIndex_p       = Index oof the OD entry to read
+//              uiSubIndex_p    = Subindex to read
+//              pDstData_p      = pointer to the buffer for data
+//              Offset_p        = offset in data for read access
+//              pSize_p         = IN: Size of the buffer
+//                                OUT: number of readed Bytes
+//
+// Return:      tEplKernel      =   errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduCalReadEntry(unsigned int uiIndex_p,
+						   unsigned int uiSubIndex_p,
+						   void *pDstData_p,
+						   tEplObdSize * pSize_p)
+{
+	tEplKernel Ret;
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+	Ret = EplObdReadEntry(uiIndex_p, uiSubIndex_p, pDstData_p, pSize_p);
+#else
+	Ret = kEplSuccessful;
+#endif
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduCalAccessOdPart()
+//
+// Description: Function encapsulate access of function EplObdAccessOdPart
+//
+// Parameters:  ObdPart_p       = od-part to reset
+//              Direction_p     = directory flag for
+//
+// Return:      tEplKernel  = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduCalAccessOdPart(tEplObdPart ObdPart_p,
+						      tEplObdDir Direction_p)
+{
+	tEplKernel Ret;
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+	Ret = EplObdAccessOdPart(ObdPart_p, Direction_p);
+#else
+	Ret = kEplSuccessful;
+#endif
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduCalDefineVar()
+//
+// Description: Function encapsulate access of function EplObdDefineVar
+//
+// Parameters:  pEplVarParam_p = varentry
+//
+// Return:      tEplKernel  =   errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduCalDefineVar(tEplVarParam MEM *
+						   pVarParam_p)
+{
+	tEplKernel Ret;
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+	Ret = EplObdDefineVar(pVarParam_p);
+#else
+	Ret = kEplSuccessful;
+#endif
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduCalGetObjectDataPtr()
+//
+// Description: Function encapsulate access of function EplObdGetObjectDataPtr
+//
+// Parameters:  uiIndex_p    =   Index of the entry
+//              uiSubindex_p =   Subindex of the entry
+//
+// Return:      void *    = pointer to object data
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT void *PUBLIC EplObduCalGetObjectDataPtr(unsigned int uiIndex_p,
+						     unsigned int uiSubIndex_p)
+{
+	void *pData;
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+	pData = EplObdGetObjectDataPtr(uiIndex_p, uiSubIndex_p);
+#else
+	pData = NULL;
+#endif
+
+	return pData;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduCalRegisterUserOd()
+//
+// Description: Function encapsulate access of function EplObdRegisterUserOd
+//
+// Parameters:  pUserOd_p   = pointer to user OD
+//
+// Return:     tEplKernel = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#if (defined (EPL_OBD_USER_OD) && (EPL_OBD_USER_OD != FALSE))
+EPLDLLEXPORT tEplKernel PUBLIC EplObduCalRegisterUserOd(tEplObdEntryPtr
+							pUserOd_p)
+{
+	tEplKernel Ret;
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+	Ret = EplObdRegisterUserOd(pUserOd_p);
+#else
+	Ret = kEplSuccessful;
+#endif
+
+	return Ret;
+
+}
+#endif
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduCalInitVarEntry()
+//
+// Description: Function encapsulate access of function EplObdInitVarEntry
+//
+// Parameters:  pVarEntry_p = pointer to var entry structure
+//              bType_p     = object type
+//              ObdSize_p   = size of object data
+//
+// Returns:     none
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT void PUBLIC EplObduCalInitVarEntry(tEplObdVarEntry MEM *
+						pVarEntry_p, BYTE bType_p,
+						tEplObdSize ObdSize_p)
+{
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+	EplObdInitVarEntry(pVarEntry_p, bType_p, ObdSize_p);
+#endif
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduCalGetDataSize()
+//
+// Description: Function encapsulate access of function EplObdGetDataSize
+//
+//              gets the data size of an object
+//              for string objects it returnes the string length
+//
+// Parameters:  uiIndex_p   =   Index
+//              uiSubIndex_p=   Subindex
+//
+// Return:      tEplObdSize
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplObdSize PUBLIC EplObduCalGetDataSize(unsigned int uiIndex_p,
+						      unsigned int uiSubIndex_p)
+{
+	tEplObdSize Size;
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+	Size = EplObdGetDataSize(uiIndex_p, uiSubIndex_p);
+#else
+	Size = 0;
+#endif
+
+	return Size;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduCalGetNodeId()
+//
+// Description: Function encapsulate access of function EplObdGetNodeId
+//
+//
+// Parameters:
+//
+// Return:      unsigned int = Node Id
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT unsigned int PUBLIC EplObduCalGetNodeId()
+{
+	unsigned int uiNodeId;
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+	uiNodeId = EplObdGetNodeId();
+#else
+	uiNodeId = 0;
+#endif
+
+	return uiNodeId;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduCalSetNodeId()
+//
+// Description: Function encapsulate access of function EplObdSetNodeId
+//
+//
+// Parameters:  uiNodeId_p  =   Node Id to set
+//              NodeIdType_p=   Type on which way the Node Id was set
+//
+// Return:      tEplKernel = Errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduCalSetNodeId(unsigned int uiNodeId_p,
+						   tEplObdNodeIdType
+						   NodeIdType_p)
+{
+	tEplKernel Ret;
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+	Ret = EplObdSetNodeId(uiNodeId_p, NodeIdType_p);
+#else
+	Ret = kEplSuccessful;
+#endif
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduCalGetAccessType()
+//
+// Description: Function encapsulate access of function EplObdGetAccessType
+//
+// Parameters:  uiIndex_p       =   Index of the OD entry
+//              uiSubIndex_p    =   Subindex of the OD Entry
+//              pAccessTyp_p    =   pointer to buffer to store accesstype
+//
+// Return:      tEplKernel      =   errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduCalGetAccessType(unsigned int uiIndex_p,
+						       unsigned int
+						       uiSubIndex_p,
+						       tEplObdAccess *
+						       pAccessTyp_p)
+{
+	tEplObdAccess AccesType;
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+	AccesType = EplObdGetAccessType(uiIndex_p, uiSubIndex_p, pAccessTyp_p);
+#else
+	AccesType = 0;
+#endif
+
+	return AccesType;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduCalReadEntryToLe()
+//
+// Description: Function encapsulate access of function EplObdReadEntryToLe
+//
+// Parameters:  uiIndex_p       = Index of the OD entry to read
+//              uiSubIndex_p    = Subindex to read
+//              pDstData_p      = pointer to the buffer for data
+//              Offset_p        = offset in data for read access
+//              pSize_p         = IN: Size of the buffer
+//                                OUT: number of readed Bytes
+//
+// Return:      tEplKernel
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduCalReadEntryToLe(unsigned int uiIndex_p,
+						       unsigned int
+						       uiSubIndex_p,
+						       void *pDstData_p,
+						       tEplObdSize * pSize_p)
+{
+	tEplKernel Ret;
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+	Ret = EplObdReadEntryToLe(uiIndex_p, uiSubIndex_p, pDstData_p, pSize_p);
+#else
+	Ret = kEplSuccessful;
+#endif
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduCalWriteEntryFromLe()
+//
+// Description: Function encapsulate access of function EplObdWriteEntryFromLe
+//
+// Parameters:  uiIndex_p       =   Index of the OD entry
+//              uiSubIndex_p    =   Subindex of the OD Entry
+//              pSrcData_p      =   Pointer to the data to write
+//              Size_p          =   Size of the data in Byte
+//
+// Return:      tEplKernel      =   Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduCalWriteEntryFromLe(unsigned int
+							  uiIndex_p,
+							  unsigned int
+							  uiSubIndex_p,
+							  void *pSrcData_p,
+							  tEplObdSize Size_p)
+{
+	tEplKernel Ret;
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+	Ret =
+	    EplObdWriteEntryFromLe(uiIndex_p, uiSubIndex_p, pSrcData_p, Size_p);
+#else
+	Ret = kEplSuccessful;
+#endif
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplObduCalSearchVarEntry()
+//
+// Description: gets variable from OD
+//
+// Parameters:  uiIndex_p       =   index of the var entry to search
+//              uiSubindex_p    =   subindex of var entry to search
+//              ppVarEntry_p    =   pointer to the pointer to the varentry
+//
+// Return:      tEplKernel
+//
+// State:
+//
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC
+EplObduCalSearchVarEntry(EPL_MCO_DECL_INSTANCE_PTR_ unsigned int uiIndex_p,
+			 unsigned int uiSubindex_p,
+			 tEplObdVarEntry MEM ** ppVarEntry_p)
+{
+	tEplKernel Ret;
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+	Ret = EplObdSearchVarEntry(uiIndex_p, uiSubindex_p, ppVarEntry_p);
+#else
+	Ret = kEplSuccessful;
+#endif
+	return Ret;
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:
+//
+// Description:
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+#endif //(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0)
+
+// EOF
diff --git a/drivers/staging/epl/EplObjDef.h b/drivers/staging/epl/EplObjDef.h
new file mode 100644
index 00000000000..7713125a3e8
--- /dev/null
+++ b/drivers/staging/epl/EplObjDef.h
@@ -0,0 +1,208 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  defines objdict dictionary
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplObjDef.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/06    k.t.:   take ObjDef.h from CANopen and modify for EPL
+
+****************************************************************************/
+
+#ifndef _EPLOBJDEF_H_
+#define _EPLOBJDEF_H_
+
+//---------------------------------------------------------------------------
+// security checks
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// macros to help building OD
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+#if (defined (EPL_OBD_USE_VARIABLE_SUBINDEX_TAB) && (EPL_OBD_USE_VARIABLE_SUBINDEX_TAB != FALSE))
+
+#define CCM_SUBINDEX_RAM_ONLY(a)    a;
+#define CCM_SUBINDEX_RAM_ONEOF(a,b) a
+
+#else
+
+#define CCM_SUBINDEX_RAM_ONLY(a)
+#define CCM_SUBINDEX_RAM_ONEOF(a,b) b
+
+#endif
+
+//---------------------------------------------------------------------------
+// To prevent unused memory in subindex tables we need this macro.
+// But not all compilers support to preset the last struct value followed by a comma.
+// Compilers which does not support a comma after last struct value has to place in a dummy subindex.
+#if ((DEV_SYSTEM & _DEV_COMMA_EXT_) != 0)
+
+#define EPL_OBD_END_SUBINDEX()
+#define EPL_OBD_MAX_ARRAY_SUBENTRIES    2
+
+#else
+
+#define EPL_OBD_END_SUBINDEX()          {0,0,0,NULL,NULL}
+#define EPL_OBD_MAX_ARRAY_SUBENTRIES    3
+
+#endif
+
+//---------------------------------------------------------------------------
+//---------------------------------------------------------------------------
+// globale vars
+//---------------------------------------------------------------------------
+//---------------------------------------------------------------------------
+
+// -------------------------------------------------------------------------
+// creation of data in ROM memory
+// -------------------------------------------------------------------------
+#define EPL_OBD_CREATE_ROM_DATA
+#include "objdict.h"
+#undef EPL_OBD_CREATE_ROM_DATA
+
+// -------------------------------------------------------------------------
+// creation of data in RAM memory
+// -------------------------------------------------------------------------
+
+#define EPL_OBD_CREATE_RAM_DATA
+#include "objdict.h"
+#undef EPL_OBD_CREATE_RAM_DATA
+
+// -------------------------------------------------------------------------
+// creation of subindex tables in ROM and RAM
+// -------------------------------------------------------------------------
+
+#define EPL_OBD_CREATE_SUBINDEX_TAB
+#include "objdict.h"
+#undef EPL_OBD_CREATE_SUBINDEX_TAB
+
+// -------------------------------------------------------------------------
+// creation of index tables for generic, manufacturer and device part
+// -------------------------------------------------------------------------
+
+#define EPL_OBD_CREATE_INDEX_TAB
+#include "objdict.h"
+#undef EPL_OBD_CREATE_INDEX_TAB
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+// ----------------------------------------------------------------------------
+//
+// Function:    EPL_OBD_INIT_RAM_NAME()
+//
+// Description: function to initialize object dictionary
+//
+// Parameters:  pInitParam_p    = pointer to init param struct of Epl
+//
+// Returns:     tEplKernel      = error code
+//
+// State:
+//
+// ----------------------------------------------------------------------------
+
+EPLDLLEXPORT tEplKernel PUBLIC EPL_OBD_INIT_RAM_NAME(tEplObdInitParam MEM *
+						     pInitParam_p)
+{
+
+	tEplObdInitParam MEM *pInitParam = pInitParam_p;
+
+	// check if pointer to parameter structure is valid
+	// if not then only copy subindex tables below
+	if (pInitParam != NULL) {
+		// at first delete all parameters (all pointers will be set zu NULL)
+		EPL_MEMSET(pInitParam, 0, sizeof(tEplObdInitParam));
+
+#define EPL_OBD_CREATE_INIT_FUNCTION
+		{
+			// inserts code to init pointer to index tables
+#include "objdict.h"
+		}
+#undef EPL_OBD_CREATE_INIT_FUNCTION
+
+#if (defined (EPL_OBD_USER_OD) && (EPL_OBD_USER_OD != FALSE))
+		{
+			// to begin no user OD is defined
+			pInitParam_p->m_pUserPart = NULL;
+		}
+#endif
+	}
+#define EPL_OBD_CREATE_INIT_SUBINDEX
+	{
+		// inserts code to copy subindex tables
+#include "objdict.h"
+	}
+#undef EPL_OBD_CREATE_INIT_SUBINDEX
+
+	return kEplSuccessful;
+
+}
+
+#endif // _EPLOBJDEF_H_
+
+// Die letzte Zeile muß unbedingt eine leere Zeile sein, weil manche Compiler
+// damit ein Problem haben, wenn das nicht so ist (z.B. GNU oder Borland C++ Builder).
diff --git a/drivers/staging/epl/EplPdo.h b/drivers/staging/epl/EplPdo.h
new file mode 100644
index 00000000000..d22ac86e85b
--- /dev/null
+++ b/drivers/staging/epl/EplPdo.h
@@ -0,0 +1,117 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for PDO module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplPdo.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/05/22 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_PDO_H_
+#define _EPL_PDO_H_
+
+#include "EplInc.h"
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+// invalid PDO-NodeId
+#define EPL_PDO_INVALID_NODE_ID     0xFF
+// NodeId for PReq RPDO
+#define EPL_PDO_PREQ_NODE_ID        0x00
+// NodeId for PRes TPDO
+#define EPL_PDO_PRES_NODE_ID        0x00
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+typedef struct {
+	void *m_pVar;
+	WORD m_wOffset;		// in Bits
+	WORD m_wSize;		// in Bits
+	BOOL m_fNumeric;	// numeric value -> use AMI functions
+
+} tEplPdoMapping;
+
+typedef struct {
+	unsigned int m_uiSizeOfStruct;
+	unsigned int m_uiPdoId;
+	unsigned int m_uiNodeId;
+	// 0xFF=invalid, RPDO: 0x00=PReq, localNodeId=PRes, remoteNodeId=PRes
+	//               TPDO: 0x00=PRes, MN: CnNodeId=PReq
+
+	BOOL m_fTxRx;
+	BYTE m_bMappingVersion;
+	unsigned int m_uiMaxMappingEntries;	// maximum number of mapping entries, i.e. size of m_aPdoMapping
+	tEplPdoMapping m_aPdoMapping[1];
+
+} tEplPdoParam;
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#endif // #ifndef _EPL_PDO_H_
diff --git a/drivers/staging/epl/EplPdok.c b/drivers/staging/epl/EplPdok.c
new file mode 100644
index 00000000000..15999b4f575
--- /dev/null
+++ b/drivers/staging/epl/EplPdok.c
@@ -0,0 +1,694 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for kernel PDO module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplPdok.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.8 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/05/22 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#include "kernel/EplPdok.h"
+#include "kernel/EplPdokCal.h"
+#include "kernel/EplEventk.h"
+#include "kernel/EplObdk.h"
+
+#if (DEV_SYSTEM == _DEV_GNU_CF548X_)
+#include "plccore.h"
+#define PDO_LED 0x08
+#endif
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0)
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) == 0)
+
+#error 'ERROR: Missing DLLk-Modul!'
+
+#endif
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) == 0)
+
+#error 'ERROR: Missing OBDk-Modul!'
+
+#endif
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#define EPL_PDOK_OBD_IDX_RX_COMM_PARAM  0x1400
+#define EPL_PDOK_OBD_IDX_RX_MAPP_PARAM  0x1600
+#define EPL_PDOK_OBD_IDX_TX_COMM_PARAM  0x1800
+#define EPL_PDOK_OBD_IDX_TX_MAPP_PARAM  0x1A00
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  EplPdok                                             */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description:
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   D E F I N I T I O N S                          //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplPdokAddInstance()
+//
+// Description: add and initialize new instance of EPL stack
+//
+// Parameters:  none
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplPdokAddInstance(void)
+{
+
+	return kEplSuccessful;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplPdokDelInstance()
+//
+// Description: deletes an instance of EPL stack
+//
+// Parameters:  none
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplPdokDelInstance(void)
+{
+
+	return kEplSuccessful;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplPdokCbPdoReceived
+//
+// Description: This function is called by DLL if PRes or PReq frame was
+//              received. It posts the frame to the event queue.
+//              It is called in states NMT_CS_READY_TO_OPERATE and NMT_CS_OPERATIONAL.
+//              The passed PDO needs not to be valid.
+//
+// Parameters:  pFrameInfo_p            = pointer to frame info structure
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplPdokCbPdoReceived(tEplFrameInfo * pFrameInfo_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplEvent Event;
+
+#if (DEV_SYSTEM == _DEV_GNU_CF548X_)
+	// reset LED
+//    MCF_GPIO_PODR_PCIBG &= ~PDO_LED;  // Level
+#endif
+
+	Event.m_EventSink = kEplEventSinkPdok;
+	Event.m_EventType = kEplEventTypePdoRx;
+	// limit copied data to size of PDO (because from some CNs the frame is larger than necessary)
+	Event.m_uiSize = AmiGetWordFromLe(&pFrameInfo_p->m_pFrame->m_Data.m_Pres.m_le_wSize) + 24;	// pFrameInfo_p->m_uiFrameSize;
+	Event.m_pArg = pFrameInfo_p->m_pFrame;
+	Ret = EplEventkPost(&Event);
+
+#if (DEV_SYSTEM == _DEV_GNU_CF548X_)
+	// set LED
+//    MCF_GPIO_PODR_PCIBG |= PDO_LED;  // Level
+#endif
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplPdokCbPdoTransmitted
+//
+// Description: This function is called by DLL if PRes or PReq frame was
+//              sent. It posts the pointer to the frame to the event queue.
+//              It is called in NMT_CS_PRE_OPERATIONAL_2,
+//              NMT_CS_READY_TO_OPERATE and NMT_CS_OPERATIONAL.
+//
+// Parameters:  pFrameInfo_p            = pointer to frame info structure
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplPdokCbPdoTransmitted(tEplFrameInfo * pFrameInfo_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplEvent Event;
+
+#if (DEV_SYSTEM == _DEV_GNU_CF548X_)
+	// reset LED
+	MCF_GPIO_PODR_PCIBG &= ~PDO_LED;	// Level
+#endif
+
+	Event.m_EventSink = kEplEventSinkPdok;
+	Event.m_EventType = kEplEventTypePdoTx;
+	Event.m_uiSize = sizeof(tEplFrameInfo);
+	Event.m_pArg = pFrameInfo_p;
+	Ret = EplEventkPost(&Event);
+
+#if (DEV_SYSTEM == _DEV_GNU_CF548X_)
+	// set LED
+	MCF_GPIO_PODR_PCIBG |= PDO_LED;	// Level
+#endif
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplPdokCbSoa
+//
+// Description: This function is called by DLL if SoA frame was
+//              received resp. sent. It posts this event to the event queue.
+//
+// Parameters:  pFrameInfo_p            = pointer to frame info structure
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplPdokCbSoa(tEplFrameInfo * pFrameInfo_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplEvent Event;
+
+	Event.m_EventSink = kEplEventSinkPdok;
+	Event.m_EventType = kEplEventTypePdoSoa;
+	Event.m_uiSize = 0;
+	Event.m_pArg = NULL;
+	Ret = EplEventkPost(&Event);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplPdokProcess
+//
+// Description: This function processes all received and transmitted PDOs.
+//              This function must not be interrupted by any other task
+//              except ISRs (like the ethernet driver ISR, which may call
+//              EplPdokCbFrameReceived() or EplPdokCbFrameTransmitted()).
+//
+// Parameters:  pEvent_p                = pointer to event structure
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplPdokProcess(tEplEvent * pEvent_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	WORD wPdoSize;
+	WORD wBitOffset;
+	WORD wBitSize;
+	WORD wVarSize;
+	QWORD qwObjectMapping;
+	BYTE bMappSubindex;
+	BYTE bObdSubindex;
+	WORD wObdMappIndex;
+	WORD wObdCommIndex;
+	WORD wPdoId;
+	BYTE bObdData;
+	BYTE bObjectCount;
+	BYTE bFrameData;
+	BOOL fValid;
+	tEplObdSize ObdSize;
+	tEplFrame *pFrame;
+	tEplFrameInfo *pFrameInfo;
+	unsigned int uiNodeId;
+	tEplMsgType MsgType;
+
+	// 0xFF=invalid, RPDO: 0x00=PReq, localNodeId=PRes, remoteNodeId=PRes
+	//               TPDO: 0x00=PRes, MN: CnNodeId=PReq
+
+	switch (pEvent_p->m_EventType) {
+	case kEplEventTypePdoRx:	// RPDO received
+		pFrame = (tEplFrame *) pEvent_p->m_pArg;
+
+		// check if received RPDO is valid
+		bFrameData =
+		    AmiGetByteFromLe(&pFrame->m_Data.m_Pres.m_le_bFlag1);
+		if ((bFrameData & EPL_FRAME_FLAG1_RD) == 0) {	// RPDO invalid
+			goto Exit;
+		}
+		// retrieve EPL message type
+		MsgType = AmiGetByteFromLe(&pFrame->m_le_bMessageType);
+		if (MsgType == kEplMsgTypePreq) {	// RPDO is PReq frame
+			uiNodeId = EPL_PDO_PREQ_NODE_ID;	// 0x00
+		} else {	// RPDO is PRes frame
+			// retrieve node ID
+			uiNodeId = AmiGetByteFromLe(&pFrame->m_le_bSrcNodeId);
+		}
+
+		// search for appropriate valid RPDO in OD
+		wObdMappIndex = EPL_PDOK_OBD_IDX_RX_MAPP_PARAM;
+		for (wObdCommIndex = EPL_PDOK_OBD_IDX_RX_COMM_PARAM;
+		     wObdCommIndex < (EPL_PDOK_OBD_IDX_RX_COMM_PARAM + 0x00FF);
+		     wObdCommIndex++, wObdMappIndex++) {
+			ObdSize = 1;
+			// read node ID from OD
+			Ret =
+			    EplObdReadEntry(wObdCommIndex, 0x01, &bObdData,
+					    &ObdSize);
+			if ((Ret == kEplObdIndexNotExist)
+			    || (Ret == kEplObdSubindexNotExist)
+			    || (Ret == kEplObdIllegalPart)) {	// PDO does not exist; last PDO reached
+				Ret = kEplSuccessful;
+				goto Exit;
+			} else if (Ret != kEplSuccessful) {	// other fatal error occured
+				goto Exit;
+			}
+			// entry read successfully
+			if (bObdData != uiNodeId) {	// node ID does not equal - wrong PDO, try next PDO in OD
+				continue;
+			}
+			ObdSize = 1;
+			// read number of mapped objects from OD; this indicates if the PDO is valid
+			Ret =
+			    EplObdReadEntry(wObdMappIndex, 0x00, &bObjectCount,
+					    &ObdSize);
+			if ((Ret == kEplObdIndexNotExist)
+			    || (Ret == kEplObdSubindexNotExist)
+			    || (Ret == kEplObdIllegalPart)) {	// PDO does not exist; last PDO reached
+				Ret = kEplSuccessful;
+				goto Exit;
+			} else if (Ret != kEplSuccessful) {	// other fatal error occured
+				goto Exit;
+			}
+			// entry read successfully
+			if (bObjectCount == 0) {	// PDO in OD not valid, try next PDO in OD
+				continue;
+			}
+
+			ObdSize = 1;
+			// check PDO mapping version
+			Ret =
+			    EplObdReadEntry(wObdCommIndex, 0x02, &bObdData,
+					    &ObdSize);
+			if (Ret != kEplSuccessful) {	// other fatal error occured
+				goto Exit;
+			}
+			// entry read successfully
+			// retrieve PDO version from frame
+			bFrameData =
+			    AmiGetByteFromLe(&pFrame->m_Data.m_Pres.
+					     m_le_bPdoVersion);
+			if ((bObdData & EPL_VERSION_MAIN) != (bFrameData & EPL_VERSION_MAIN)) {	// PDO versions do not match
+				// $$$ raise PDO error
+				// termiate processing of this RPDO
+				goto Exit;
+			}
+			// valid RPDO found
+
+			// retrieve PDO size
+			wPdoSize =
+			    AmiGetWordFromLe(&pFrame->m_Data.m_Pres.m_le_wSize);
+
+			// process mapping
+			for (bMappSubindex = 1; bMappSubindex <= bObjectCount;
+			     bMappSubindex++) {
+				ObdSize = 8;	// QWORD
+				// read object mapping from OD
+				Ret =
+				    EplObdReadEntry(wObdMappIndex,
+						    bMappSubindex,
+						    &qwObjectMapping, &ObdSize);
+				if (Ret != kEplSuccessful) {	// other fatal error occured
+					goto Exit;
+				}
+				// check if object mapping entry is valid, i.e. unequal zero, because "empty" entries are allowed
+				if (qwObjectMapping == 0) {	// invalid entry, continue with next entry
+					continue;
+				}
+				// decode object mapping
+				wObdCommIndex =
+				    (WORD) (qwObjectMapping &
+					    0x000000000000FFFFLL);
+				bObdSubindex =
+				    (BYTE) ((qwObjectMapping &
+					     0x0000000000FF0000LL) >> 16);
+				wBitOffset =
+				    (WORD) ((qwObjectMapping &
+					     0x0000FFFF00000000LL) >> 32);
+				wBitSize =
+				    (WORD) ((qwObjectMapping &
+					     0xFFFF000000000000LL) >> 48);
+
+				// check if object exceeds PDO size
+				if (((wBitOffset + wBitSize) >> 3) > wPdoSize) {	// wrong object mapping; PDO size is too low
+					// $$$ raise PDO error
+					// terminate processing of this RPDO
+					goto Exit;
+				}
+				// copy object from RPDO to process/OD variable
+				ObdSize = wBitSize >> 3;
+				Ret =
+				    EplObdWriteEntryFromLe(wObdCommIndex,
+							   bObdSubindex,
+							   &pFrame->m_Data.
+							   m_Pres.
+							   m_le_abPayload[(wBitOffset >> 3)], ObdSize);
+				if (Ret != kEplSuccessful) {	// other fatal error occured
+					goto Exit;
+				}
+
+			}
+
+			// processing finished successfully
+			goto Exit;
+		}
+		break;
+
+	case kEplEventTypePdoTx:	// TPDO transmitted
+		pFrameInfo = (tEplFrameInfo *) pEvent_p->m_pArg;
+		pFrame = pFrameInfo->m_pFrame;
+
+		// set TPDO invalid, so that only fully processed TPDOs are sent as valid
+		bFrameData =
+		    AmiGetByteFromLe(&pFrame->m_Data.m_Pres.m_le_bFlag1);
+		AmiSetByteToLe(&pFrame->m_Data.m_Pres.m_le_bFlag1,
+			       (bFrameData & ~EPL_FRAME_FLAG1_RD));
+
+		// retrieve EPL message type
+		MsgType = AmiGetByteFromLe(&pFrame->m_le_bMessageType);
+		if (MsgType == kEplMsgTypePres) {	// TPDO is PRes frame
+			uiNodeId = EPL_PDO_PRES_NODE_ID;	// 0x00
+		} else {	// TPDO is PReq frame
+			// retrieve node ID
+			uiNodeId = AmiGetByteFromLe(&pFrame->m_le_bDstNodeId);
+		}
+
+		// search for appropriate valid TPDO in OD
+		wObdMappIndex = EPL_PDOK_OBD_IDX_TX_MAPP_PARAM;
+		wObdCommIndex = EPL_PDOK_OBD_IDX_TX_COMM_PARAM;
+		for (wPdoId = 0;; wPdoId++, wObdCommIndex++, wObdMappIndex++) {
+			ObdSize = 1;
+			// read node ID from OD
+			Ret =
+			    EplObdReadEntry(wObdCommIndex, 0x01, &bObdData,
+					    &ObdSize);
+			if ((Ret == kEplObdIndexNotExist)
+			    || (Ret == kEplObdSubindexNotExist)
+			    || (Ret == kEplObdIllegalPart)) {	// PDO does not exist; last PDO reached
+				Ret = kEplSuccessful;
+				goto Exit;
+			} else if (Ret != kEplSuccessful) {	// other fatal error occured
+				goto Exit;
+			}
+			// entry read successfully
+			if (bObdData != uiNodeId) {	// node ID does not equal - wrong PDO, try next PDO in OD
+				continue;
+			}
+			ObdSize = 1;
+			// read number of mapped objects from OD; this indicates if the PDO is valid
+			Ret =
+			    EplObdReadEntry(wObdMappIndex, 0x00, &bObjectCount,
+					    &ObdSize);
+			if ((Ret == kEplObdIndexNotExist)
+			    || (Ret == kEplObdSubindexNotExist)
+			    || (Ret == kEplObdIllegalPart)) {	// PDO does not exist; last PDO reached
+				Ret = kEplSuccessful;
+				goto Exit;
+			} else if (Ret != kEplSuccessful) {	// other fatal error occured
+				goto Exit;
+			}
+			// entry read successfully
+			if (bObjectCount == 0) {	// PDO in OD not valid, try next PDO in OD
+				continue;
+			}
+			// valid TPDO found
+
+			ObdSize = 1;
+			// get PDO mapping version from OD
+			Ret =
+			    EplObdReadEntry(wObdCommIndex, 0x02, &bObdData,
+					    &ObdSize);
+			if (Ret != kEplSuccessful) {	// other fatal error occured
+				goto Exit;
+			}
+			// entry read successfully
+			// set PDO version in frame
+			AmiSetByteToLe(&pFrame->m_Data.m_Pres.m_le_bPdoVersion,
+				       bObdData);
+
+			// calculate PDO size
+			wPdoSize = 0;
+
+			// process mapping
+			for (bMappSubindex = 1; bMappSubindex <= bObjectCount;
+			     bMappSubindex++) {
+				ObdSize = 8;	// QWORD
+				// read object mapping from OD
+				Ret =
+				    EplObdReadEntry(wObdMappIndex,
+						    bMappSubindex,
+						    &qwObjectMapping, &ObdSize);
+				if (Ret != kEplSuccessful) {	// other fatal error occured
+					goto Exit;
+				}
+				// check if object mapping entry is valid, i.e. unequal zero, because "empty" entries are allowed
+				if (qwObjectMapping == 0) {	// invalid entry, continue with next entry
+					continue;
+				}
+				// decode object mapping
+				wObdCommIndex =
+				    (WORD) (qwObjectMapping &
+					    0x000000000000FFFFLL);
+				bObdSubindex =
+				    (BYTE) ((qwObjectMapping &
+					     0x0000000000FF0000LL) >> 16);
+				wBitOffset =
+				    (WORD) ((qwObjectMapping &
+					     0x0000FFFF00000000LL) >> 32);
+				wBitSize =
+				    (WORD) ((qwObjectMapping &
+					     0xFFFF000000000000LL) >> 48);
+
+				// calculate max PDO size
+				ObdSize = wBitSize >> 3;
+				wVarSize = (wBitOffset >> 3) + (WORD) ObdSize;
+				if ((unsigned int)(wVarSize + 24) > pFrameInfo->m_uiFrameSize) {	// TPDO is too short
+					// $$$ raise PDO error, set Ret
+					goto Exit;
+				}
+				if (wVarSize > wPdoSize) {	// memorize new PDO size
+					wPdoSize = wVarSize;
+				}
+				// copy object from process/OD variable to TPDO
+				Ret =
+				    EplObdReadEntryToLe(wObdCommIndex,
+							bObdSubindex,
+							&pFrame->m_Data.m_Pres.
+							m_le_abPayload[(wBitOffset >> 3)], &ObdSize);
+				if (Ret != kEplSuccessful) {	// other fatal error occured
+					goto Exit;
+				}
+
+			}
+
+			// set PDO size in frame
+			AmiSetWordToLe(&pFrame->m_Data.m_Pres.m_le_wSize,
+				       wPdoSize);
+
+			Ret = EplPdokCalAreTpdosValid(&fValid);
+			if (fValid != FALSE) {
+				// set TPDO valid
+				bFrameData =
+				    AmiGetByteFromLe(&pFrame->m_Data.m_Pres.
+						     m_le_bFlag1);
+				AmiSetByteToLe(&pFrame->m_Data.m_Pres.
+					       m_le_bFlag1,
+					       (bFrameData |
+						EPL_FRAME_FLAG1_RD));
+			}
+			// processing finished successfully
+
+			goto Exit;
+		}
+		break;
+
+	case kEplEventTypePdoSoa:	// SoA received
+
+		// invalidate TPDOs
+		Ret = EplPdokCalSetTpdosValid(FALSE);
+		break;
+
+	default:
+		{
+			ASSERTMSG(FALSE,
+				  "EplPdokProcess(): unhandled event type!\n");
+		}
+	}
+
+      Exit:
+	return Ret;
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:
+//
+// Description:
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+#endif // #if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0)
+
+// EOF
diff --git a/drivers/staging/epl/EplPdokCal.c b/drivers/staging/epl/EplPdokCal.c
new file mode 100644
index 00000000000..f44c4757800
--- /dev/null
+++ b/drivers/staging/epl/EplPdokCal.c
@@ -0,0 +1,266 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for kernel PDO Communication Abstraction Layer module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplPdokCal.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.6 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/27 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#include "kernel/EplPdokCal.h"
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOK)) != 0)
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  EplPdokCal                                          */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description:
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   D E F I N I T I O N S                          //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+typedef struct {
+	BOOL m_fTpdosValid;
+
+} tEplPdokCalInstance;
+
+//---------------------------------------------------------------------------
+// local vars
+//---------------------------------------------------------------------------
+
+static tEplPdokCalInstance EplPdokCalInstance_g;
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplPdokCalAddInstance()
+//
+// Description: add and initialize new instance of EPL stack
+//
+// Parameters:  none
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplPdokCalAddInstance(void)
+{
+
+	EPL_MEMSET(&EplPdokCalInstance_g, 0, sizeof(EplPdokCalInstance_g));
+
+	return kEplSuccessful;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplPdokCalDelInstance()
+//
+// Description: deletes an instance of EPL stack
+//
+// Parameters:  none
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplPdokCalDelInstance(void)
+{
+
+	return kEplSuccessful;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplPdokCalSetTpdosValid()
+//
+// Description: This function sets the validity flag for TPDOs to the
+//              specified value.
+//
+// Parameters:  fValid_p                = validity flag
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplPdokCalSetTpdosValid(BOOL fValid_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	EplPdokCalInstance_g.m_fTpdosValid = fValid_p;
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplPdokCalAreTpdosValid()
+//
+// Description: This function returns the validity flag for TPDOs.
+//
+// Parameters:  pfValid_p               = OUT: validity flag
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplPdokCalAreTpdosValid(BOOL * pfValid_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	*pfValid_p = EplPdokCalInstance_g.m_fTpdosValid;
+
+	return Ret;
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:
+//
+// Description:
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+#endif
+
+// EOF
diff --git a/drivers/staging/epl/EplPdou.c b/drivers/staging/epl/EplPdou.c
new file mode 100644
index 00000000000..e7b10653a0e
--- /dev/null
+++ b/drivers/staging/epl/EplPdou.c
@@ -0,0 +1,565 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for user PDO module
+                Currently, this module just implements a OD callback function
+                to check if the PDO configuration is valid.
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplPdou.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/05/22 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#include "EplInc.h"
+//#include "user/EplPdouCal.h"
+#include "user/EplObdu.h"
+#include "user/EplPdou.h"
+#include "EplSdoAc.h"
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOU)) != 0)
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) == 0) && (EPL_OBD_USE_KERNEL == FALSE)
+#error "EPL PDOu module needs EPL module OBDU or OBDK!"
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#define EPL_PDOU_OBD_IDX_RX_COMM_PARAM  0x1400
+#define EPL_PDOU_OBD_IDX_RX_MAPP_PARAM  0x1600
+#define EPL_PDOU_OBD_IDX_TX_COMM_PARAM  0x1800
+#define EPL_PDOU_OBD_IDX_TX_MAPP_PARAM  0x1A00
+#define EPL_PDOU_OBD_IDX_MAPP_PARAM     0x0200
+#define EPL_PDOU_OBD_IDX_MASK           0xFF00
+#define EPL_PDOU_PDO_ID_MASK            0x00FF
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  EplPdou                                             */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description:
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   D E F I N I T I O N S                          //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+static tEplKernel EplPdouCheckPdoValidity(tEplObdCbParam MEM * pParam_p,
+					  unsigned int uiIndex_p);
+
+static void EplPdouDecodeObjectMapping(QWORD qwObjectMapping_p,
+				       unsigned int *puiIndex_p,
+				       unsigned int *puiSubIndex_p,
+				       unsigned int *puiBitOffset_p,
+				       unsigned int *puiBitSize_p);
+
+static tEplKernel EplPdouCheckObjectMapping(QWORD qwObjectMapping_p,
+					    tEplObdAccess AccessType_p,
+					    DWORD * pdwAbortCode_p,
+					    unsigned int *puiPdoSize_p);
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplPdouAddInstance()
+//
+// Description: add and initialize new instance of EPL stack
+//
+// Parameters:  none
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplPdouAddInstance(void)
+{
+
+	return kEplSuccessful;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplPdouDelInstance()
+//
+// Description: deletes an instance of EPL stack
+//
+// Parameters:  none
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel EplPdouDelInstance(void)
+{
+
+	return kEplSuccessful;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplPdouCbObdAccess
+//
+// Description: callback function for OD accesses
+//
+// Parameters:  pParam_p                = OBD parameter
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplPdouCbObdAccess(tEplObdCbParam MEM * pParam_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	unsigned int uiPdoId;
+	unsigned int uiIndexType;
+	tEplObdSize ObdSize;
+	BYTE bObjectCount;
+	QWORD qwObjectMapping;
+	tEplObdAccess AccessType;
+	BYTE bMappSubindex;
+	unsigned int uiCurPdoSize;
+	WORD wMaxPdoSize;
+	unsigned int uiSubIndex;
+
+	// fetch PDO ID
+	uiPdoId = pParam_p->m_uiIndex & EPL_PDOU_PDO_ID_MASK;
+
+	// fetch object index type
+	uiIndexType = pParam_p->m_uiIndex & EPL_PDOU_OBD_IDX_MASK;
+
+	if (pParam_p->m_ObdEvent != kEplObdEvPreWrite) {	// read accesses, post write events etc. are OK
+		pParam_p->m_dwAbortCode = 0;
+		goto Exit;
+	}
+	// check index type
+	switch (uiIndexType) {
+	case EPL_PDOU_OBD_IDX_RX_COMM_PARAM:
+		// RPDO communication parameter accessed
+	case EPL_PDOU_OBD_IDX_TX_COMM_PARAM:
+		{		// TPDO communication parameter accessed
+			Ret = EplPdouCheckPdoValidity(pParam_p,
+						      (EPL_PDOU_OBD_IDX_MAPP_PARAM
+						       | pParam_p->m_uiIndex));
+			if (Ret != kEplSuccessful) {	// PDO is valid or does not exist
+				goto Exit;
+			}
+
+			goto Exit;
+		}
+
+	case EPL_PDOU_OBD_IDX_RX_MAPP_PARAM:
+		{		// RPDO mapping parameter accessed
+
+			AccessType = kEplObdAccWrite;
+			break;
+		}
+
+	case EPL_PDOU_OBD_IDX_TX_MAPP_PARAM:
+		{		// TPDO mapping parameter accessed
+
+			AccessType = kEplObdAccRead;
+			break;
+		}
+
+	default:
+		{		// this callback function is only for
+			// PDO mapping and communication parameters
+			pParam_p->m_dwAbortCode = EPL_SDOAC_GENERAL_ERROR;
+			goto Exit;
+		}
+	}
+
+	// RPDO and TPDO mapping parameter accessed
+
+	if (pParam_p->m_uiSubIndex == 0) {	// object mapping count accessed
+
+		// PDO is enabled or disabled
+		bObjectCount = *((BYTE *) pParam_p->m_pArg);
+
+		if (bObjectCount == 0) {	// PDO shall be disabled
+
+			// that is always possible
+			goto Exit;
+		}
+		// PDO shall be enabled
+		// it should have been disabled for this operation
+		Ret = EplPdouCheckPdoValidity(pParam_p, pParam_p->m_uiIndex);
+		if (Ret != kEplSuccessful) {	// PDO is valid or does not exist
+			goto Exit;
+		}
+
+		if (AccessType == kEplObdAccWrite) {
+			uiSubIndex = 0x04;	// PReqActPayloadLimit_U16
+		} else {
+			uiSubIndex = 0x05;	// PResActPayloadLimit_U16
+		}
+
+		// fetch maximum PDO size from Object 1F98h: NMT_CycleTiming_REC
+		ObdSize = sizeof(wMaxPdoSize);
+		Ret =
+		    EplObduReadEntry(0x1F98, uiSubIndex, &wMaxPdoSize,
+				     &ObdSize);
+		if (Ret != kEplSuccessful) {	// other fatal error occured
+			pParam_p->m_dwAbortCode = EPL_SDOAC_GENERAL_ERROR;
+			goto Exit;
+		}
+		// check all objectmappings
+		for (bMappSubindex = 1; bMappSubindex <= bObjectCount;
+		     bMappSubindex++) {
+			// read object mapping from OD
+			ObdSize = sizeof(qwObjectMapping);	// QWORD
+			Ret = EplObduReadEntry(pParam_p->m_uiIndex,
+					       bMappSubindex, &qwObjectMapping,
+					       &ObdSize);
+			if (Ret != kEplSuccessful) {	// other fatal error occured
+				pParam_p->m_dwAbortCode =
+				    EPL_SDOAC_GENERAL_ERROR;
+				goto Exit;
+			}
+			// check object mapping
+			Ret = EplPdouCheckObjectMapping(qwObjectMapping,
+							AccessType,
+							&pParam_p->
+							m_dwAbortCode,
+							&uiCurPdoSize);
+			if (Ret != kEplSuccessful) {	// illegal object mapping
+				goto Exit;
+			}
+
+			if (uiCurPdoSize > wMaxPdoSize) {	// mapping exceeds object size
+				pParam_p->m_dwAbortCode =
+				    EPL_SDOAC_GENERAL_ERROR;
+				Ret = kEplPdoVarNotFound;
+			}
+
+		}
+
+	} else {		// ObjectMapping
+		Ret = EplPdouCheckPdoValidity(pParam_p, pParam_p->m_uiIndex);
+		if (Ret != kEplSuccessful) {	// PDO is valid or does not exist
+			goto Exit;
+		}
+		// check existence of object and validity of object length
+
+		qwObjectMapping = *((QWORD *) pParam_p->m_pArg);
+
+		Ret = EplPdouCheckObjectMapping(qwObjectMapping,
+						AccessType,
+						&pParam_p->m_dwAbortCode,
+						&uiCurPdoSize);
+
+	}
+
+      Exit:
+	return Ret;
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplPdouCheckPdoValidity
+//
+// Description: check if PDO is valid
+//
+// Parameters:  pParam_p                = OBD parameter
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplPdouCheckPdoValidity(tEplObdCbParam MEM * pParam_p,
+					  unsigned int uiIndex_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplObdSize ObdSize;
+	BYTE bObjectCount;
+
+	ObdSize = 1;
+	// read number of mapped objects from OD; this indicates if the PDO is valid
+	Ret = EplObduReadEntry(uiIndex_p, 0x00, &bObjectCount, &ObdSize);
+	if (Ret != kEplSuccessful) {	// other fatal error occured
+		pParam_p->m_dwAbortCode =
+		    EPL_SDOAC_GEN_INTERNAL_INCOMPATIBILITY;
+		goto Exit;
+	}
+	// entry read successfully
+	if (bObjectCount != 0) {	// PDO in OD is still valid
+		pParam_p->m_dwAbortCode = EPL_SDOAC_GEN_PARAM_INCOMPATIBILITY;
+		Ret = kEplPdoNotExist;
+		goto Exit;
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplPdouDecodeObjectMapping
+//
+// Description: decodes the given object mapping entry into index, subindex,
+//              bit offset and bit size.
+//
+// Parameters:  qwObjectMapping_p       = object mapping entry
+//              puiIndex_p              = [OUT] pointer to object index
+//              puiSubIndex_p           = [OUT] pointer to subindex
+//              puiBitOffset_p          = [OUT] pointer to bit offset
+//              puiBitSize_p            = [OUT] pointer to bit size
+//
+// Returns:     (void)
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static void EplPdouDecodeObjectMapping(QWORD qwObjectMapping_p,
+				       unsigned int *puiIndex_p,
+				       unsigned int *puiSubIndex_p,
+				       unsigned int *puiBitOffset_p,
+				       unsigned int *puiBitSize_p)
+{
+	*puiIndex_p = (unsigned int)
+	    (qwObjectMapping_p & 0x000000000000FFFFLL);
+
+	*puiSubIndex_p = (unsigned int)
+	    ((qwObjectMapping_p & 0x0000000000FF0000LL) >> 16);
+
+	*puiBitOffset_p = (unsigned int)
+	    ((qwObjectMapping_p & 0x0000FFFF00000000LL) >> 32);
+
+	*puiBitSize_p = (unsigned int)
+	    ((qwObjectMapping_p & 0xFFFF000000000000LL) >> 48);
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplPdouCheckObjectMapping
+//
+// Description: checks the given object mapping entry.
+//
+// Parameters:  qwObjectMapping_p       = object mapping entry
+//              AccessType_p            = access type to mapped object:
+//                                        write = RPDO and read = TPDO
+//              puiPdoSize_p            = [OUT] pointer to covered PDO size
+//                                        (offset + size) in byte;
+//                                        0 if mapping failed
+//              pdwAbortCode_p          = [OUT] pointer to SDO abort code;
+//                                        0 if mapping is possible
+//
+// Returns:     tEplKernel              = error code
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static tEplKernel EplPdouCheckObjectMapping(QWORD qwObjectMapping_p,
+					    tEplObdAccess AccessType_p,
+					    DWORD * pdwAbortCode_p,
+					    unsigned int *puiPdoSize_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplObdSize ObdSize;
+	unsigned int uiIndex;
+	unsigned int uiSubIndex;
+	unsigned int uiBitOffset;
+	unsigned int uiBitSize;
+	tEplObdAccess AccessType;
+	BOOL fNumerical;
+
+	if (qwObjectMapping_p == 0) {	// discard zero value
+		*puiPdoSize_p = 0;
+		goto Exit;
+	}
+	// decode object mapping
+	EplPdouDecodeObjectMapping(qwObjectMapping_p,
+				   &uiIndex,
+				   &uiSubIndex, &uiBitOffset, &uiBitSize);
+
+	if ((uiBitOffset & 0x7) != 0x0) {	// bit mapping is not supported
+		*pdwAbortCode_p = EPL_SDOAC_GENERAL_ERROR;
+		Ret = kEplPdoGranularityMismatch;
+		goto Exit;
+	}
+
+	if ((uiBitSize & 0x7) != 0x0) {	// bit mapping is not supported
+		*pdwAbortCode_p = EPL_SDOAC_GENERAL_ERROR;
+		Ret = kEplPdoGranularityMismatch;
+		goto Exit;
+	}
+	// check access type
+	Ret = EplObduGetAccessType(uiIndex, uiSubIndex, &AccessType);
+	if (Ret != kEplSuccessful) {	// entry doesn't exist
+		*pdwAbortCode_p = EPL_SDOAC_OBJECT_NOT_EXIST;
+		goto Exit;
+	}
+
+	if ((AccessType & kEplObdAccPdo) == 0) {	// object is not mappable
+		*pdwAbortCode_p = EPL_SDOAC_OBJECT_NOT_MAPPABLE;
+		Ret = kEplPdoVarNotFound;
+		goto Exit;
+	}
+
+	if ((AccessType & AccessType_p) == 0) {	// object is not writeable (RPDO) or readable (TPDO) respectively
+		*pdwAbortCode_p = EPL_SDOAC_OBJECT_NOT_MAPPABLE;
+		Ret = kEplPdoVarNotFound;
+		goto Exit;
+	}
+
+	ObdSize = EplObduGetDataSize(uiIndex, uiSubIndex);
+	if (ObdSize < (uiBitSize >> 3)) {	// object does not exist or has smaller size
+		*pdwAbortCode_p = EPL_SDOAC_GENERAL_ERROR;
+		Ret = kEplPdoVarNotFound;
+	}
+
+	Ret = EplObduIsNumerical(uiIndex, uiSubIndex, &fNumerical);
+	if (Ret != kEplSuccessful) {	// entry doesn't exist
+		*pdwAbortCode_p = EPL_SDOAC_OBJECT_NOT_EXIST;
+		goto Exit;
+	}
+
+	if ((fNumerical != FALSE)
+	    && ((uiBitSize >> 3) != ObdSize)) {
+		// object is numerical,
+		// therefor size has to fit, but it does not.
+		*pdwAbortCode_p = EPL_SDOAC_GENERAL_ERROR;
+		Ret = kEplPdoVarNotFound;
+		goto Exit;
+	}
+	// calucaled needed PDO size
+	*puiPdoSize_p = (uiBitOffset >> 3) + (uiBitSize >> 3);
+
+      Exit:
+	return Ret;
+}
+
+#endif // #if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOU)) != 0)
+
+// EOF
diff --git a/drivers/staging/epl/EplSdo.h b/drivers/staging/epl/EplSdo.h
new file mode 100644
index 00000000000..1cb3f2de4a3
--- /dev/null
+++ b/drivers/staging/epl/EplSdo.h
@@ -0,0 +1,245 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for api function of the sdo module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplSdo.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.6 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+ 2006/06/26 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "EplInc.h"
+#include "EplFrame.h"
+#include "EplSdoAc.h"
+
+#ifndef _EPLSDO_H_
+#define _EPLSDO_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+// global defines
+#ifndef EPL_SDO_MAX_PAYLOAD
+#define EPL_SDO_MAX_PAYLOAD     256
+#endif
+
+// handle between Protocol Abstraction Layer and asynchronous SDO Sequence Layer
+#define EPL_SDO_UDP_HANDLE      0x8000
+#define EPL_SDO_ASND_HANDLE     0x4000
+#define EPL_SDO_ASY_HANDLE_MASK 0xC000
+#define EPL_SDO_ASY_INVALID_HDL 0x3FFF
+
+// handle between  SDO Sequence Layer and sdo command layer
+#define EPL_SDO_ASY_HANDLE      0x8000
+#define EPL_SDO_PDO_HANDLE      0x4000
+#define EPL_SDO_SEQ_HANDLE_MASK 0xC000
+#define EPL_SDO_SEQ_INVALID_HDL 0x3FFF
+
+#define EPL_ASND_HEADER_SIZE        4
+//#define EPL_SEQ_HEADER_SIZE         4
+#define EPL_ETHERNET_HEADER_SIZE    14
+
+#define EPL_SEQ_NUM_MASK            0xFC
+
+// size for send buffer and history
+#define EPL_MAX_SDO_FRAME_SIZE      EPL_C_IP_MIN_MTU
+// size for receive frame
+// -> needed because SND-Kit sends up to 1518 Byte
+//    without Sdo-Command: Maximum Segment Size
+#define EPL_MAX_SDO_REC_FRAME_SIZE  EPL_C_IP_MAX_MTU
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+// handle between Protocol Abstraction Layer and asynchronuus SDO Sequence Layer
+typedef unsigned int tEplSdoConHdl;
+
+// callback function pointer for Protocol Abstraction Layer to call
+// asynchronuus SDO Sequence Layer
+typedef tEplKernel(PUBLIC * tEplSequLayerReceiveCb) (tEplSdoConHdl ConHdl_p,
+						     tEplAsySdoSeq *
+						     pSdoSeqData_p,
+						     unsigned int uiDataSize_p);
+
+// handle between asynchronuus SDO Sequence Layer and SDO Command layer
+typedef unsigned int tEplSdoSeqConHdl;
+
+// callback function pointer for asynchronuus SDO Sequence Layer to call
+// SDO Command layer for received data
+typedef tEplKernel(PUBLIC *
+		   tEplSdoComReceiveCb) (tEplSdoSeqConHdl SdoSeqConHdl_p,
+					 tEplAsySdoCom * pAsySdoCom_p,
+					 unsigned int uiDataSize_p);
+
+// status of connection
+typedef enum {
+	kAsySdoConStateConnected = 0x00,
+	kAsySdoConStateInitError = 0x01,
+	kAsySdoConStateConClosed = 0x02,
+	kAsySdoConStateAckReceived = 0x03,
+	kAsySdoConStateFrameSended = 0x04,
+	kAsySdoConStateTimeout = 0x05
+} tEplAsySdoConState;
+
+// callback function pointer for asynchronuus SDO Sequence Layer to call
+// SDO Command layer for connection status
+typedef tEplKernel(PUBLIC * tEplSdoComConCb) (tEplSdoSeqConHdl SdoSeqConHdl_p,
+					      tEplAsySdoConState
+					      AsySdoConState_p);
+
+// handle between  SDO Command layer and application
+typedef unsigned int tEplSdoComConHdl;
+
+// status of connection
+typedef enum {
+	kEplSdoComTransferNotActive = 0x00,
+	kEplSdoComTransferRunning = 0x01,
+	kEplSdoComTransferTxAborted = 0x02,
+	kEplSdoComTransferRxAborted = 0x03,
+	kEplSdoComTransferFinished = 0x04,
+	kEplSdoComTransferLowerLayerAbort = 0x05
+} tEplSdoComConState;
+
+// SDO Services and Command-Ids from DS 1.0.0 p.152
+typedef enum {
+	kEplSdoServiceNIL = 0x00,
+	kEplSdoServiceWriteByIndex = 0x01,
+	kEplSdoServiceReadByIndex = 0x02
+	    //--------------------------------
+	    // the following services are optional and
+	    // not supported now
+/*
+    kEplSdoServiceWriteAllByIndex   = 0x03,
+    kEplSdoServiceReadAllByIndex    = 0x04,
+    kEplSdoServiceWriteByName       = 0x05,
+    kEplSdoServiceReadByName        = 0x06,
+
+    kEplSdoServiceFileWrite         = 0x20,
+    kEplSdoServiceFileRead          = 0x21,
+
+    kEplSdoServiceWriteMultiByIndex = 0x31,
+    kEplSdoServiceReadMultiByIndex  = 0x32,
+
+    kEplSdoServiceMaxSegSize        = 0x70
+
+    // 0x80 - 0xFF manufacturer specific
+
+ */
+} tEplSdoServiceType;
+
+// describes if read or write access
+typedef enum {
+	kEplSdoAccessTypeRead = 0x00,
+	kEplSdoAccessTypeWrite = 0x01
+} tEplSdoAccessType;
+
+typedef enum {
+	kEplSdoTypeAuto = 0x00,
+	kEplSdoTypeUdp = 0x01,
+	kEplSdoTypeAsnd = 0x02,
+	kEplSdoTypePdo = 0x03
+} tEplSdoType;
+
+typedef enum {
+	kEplSdoTransAuto = 0x00,
+	kEplSdoTransExpedited = 0x01,
+	kEplSdoTransSegmented = 0x02
+} tEplSdoTransType;
+
+// structure to inform application about finish of SDO transfer
+typedef struct {
+	tEplSdoComConHdl m_SdoComConHdl;
+	tEplSdoComConState m_SdoComConState;
+	DWORD m_dwAbortCode;
+	tEplSdoAccessType m_SdoAccessType;
+	unsigned int m_uiNodeId;	// NodeId of the target
+	unsigned int m_uiTargetIndex;	// index which was accessed
+	unsigned int m_uiTargetSubIndex;	// subindex which was accessed
+	unsigned int m_uiTransferredByte;	// number of bytes transferred
+	void *m_pUserArg;	// user definable argument pointer
+
+} tEplSdoComFinished;
+
+// callback function pointer to inform application about connection
+typedef tEplKernel(PUBLIC * tEplSdoFinishedCb) (tEplSdoComFinished *
+						pSdoComFinished_p);
+
+// structure to init SDO transfer to Read or Write by Index
+typedef struct {
+	tEplSdoComConHdl m_SdoComConHdl;
+	unsigned int m_uiIndex;
+	unsigned int m_uiSubindex;
+	void *m_pData;
+	unsigned int m_uiDataSize;
+	unsigned int m_uiTimeout;	// not used in this version
+	tEplSdoAccessType m_SdoAccessType;
+	tEplSdoFinishedCb m_pfnSdoFinishedCb;
+	void *m_pUserArg;	// user definable argument pointer
+
+} tEplSdoComTransParamByIndex;
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#endif // #ifndef _EPLSDO_H_
diff --git a/drivers/staging/epl/EplSdoAc.h b/drivers/staging/epl/EplSdoAc.h
new file mode 100644
index 00000000000..400fb38ce3e
--- /dev/null
+++ b/drivers/staging/epl/EplSdoAc.h
@@ -0,0 +1,111 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  definitions for SDO Abort codes
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplSdoAc.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    ...
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/30 k.t.: first implementation
+
+****************************************************************************/
+
+#ifndef _EPLSDOAC_H_
+#define _EPLSDOAC_H_
+
+// =========================================================================
+// SDO abort codes
+// =========================================================================
+
+#define    EPL_SDOAC_TIME_OUT                            0x05040000L
+#define    EPL_SDOAC_UNKNOWN_COMMAND_SPECIFIER           0x05040001L
+#define    EPL_SDOAC_INVALID_BLOCK_SIZE                  0x05040002L
+#define    EPL_SDOAC_INVALID_SEQUENCE_NUMBER             0x05040003L
+#define    EPL_SDOAC_OUT_OF_MEMORY                       0x05040005L
+#define    EPL_SDOAC_UNSUPPORTED_ACCESS                  0x06010000L
+#define    EPL_SDOAC_READ_TO_WRITE_ONLY_OBJ              0x06010001L
+#define    EPL_SDOAC_WRITE_TO_READ_ONLY_OBJ              0x06010002L
+#define    EPL_SDOAC_OBJECT_NOT_EXIST                    0x06020000L
+#define    EPL_SDOAC_OBJECT_NOT_MAPPABLE                 0x06040041L
+#define    EPL_SDOAC_PDO_LENGTH_EXCEEDED                 0x06040042L
+#define    EPL_SDOAC_GEN_PARAM_INCOMPATIBILITY           0x06040043L
+#define    EPL_SDOAC_INVALID_HEARTBEAT_DEC               0x06040044L
+#define    EPL_SDOAC_GEN_INTERNAL_INCOMPATIBILITY        0x06040047L
+#define    EPL_SDOAC_ACCESS_FAILED_DUE_HW_ERROR          0x06060000L
+#define    EPL_SDOAC_DATA_TYPE_LENGTH_NOT_MATCH          0x06070010L
+#define    EPL_SDOAC_DATA_TYPE_LENGTH_TOO_HIGH           0x06070012L
+#define    EPL_SDOAC_DATA_TYPE_LENGTH_TOO_LOW            0x06070013L
+#define    EPL_SDOAC_SUB_INDEX_NOT_EXIST                 0x06090011L
+#define    EPL_SDOAC_VALUE_RANGE_EXCEEDED                0x06090030L
+#define    EPL_SDOAC_VALUE_RANGE_TOO_HIGH                0x06090031L
+#define    EPL_SDOAC_VALUE_RANGE_TOO_LOW                 0x06090032L
+#define    EPL_SDOAC_MAX_VALUE_LESS_MIN_VALUE            0x06090036L
+#define    EPL_SDOAC_GENERAL_ERROR                       0x08000000L
+#define    EPL_SDOAC_DATA_NOT_TRANSF_OR_STORED           0x08000020L
+#define    EPL_SDOAC_DATA_NOT_TRANSF_DUE_LOCAL_CONTROL   0x08000021L
+#define    EPL_SDOAC_DATA_NOT_TRANSF_DUE_DEVICE_STATE    0x08000022L
+#define    EPL_SDOAC_OBJECT_DICTIONARY_NOT_EXIST         0x08000023L
+#define    EPL_SDOAC_CONFIG_DATA_EMPTY                   0x08000024L
+
+#endif // _EPLSDOAC_H_
+
+// Die letzte Zeile muß unbedingt eine leere Zeile sein, weil manche Compiler
+// damit ein Problem haben, wenn das nicht so ist (z.B. GNU oder Borland C++ Builder).
diff --git a/drivers/staging/epl/EplSdoAsndu.c b/drivers/staging/epl/EplSdoAsndu.c
new file mode 100644
index 00000000000..05a00c9a731
--- /dev/null
+++ b/drivers/staging/epl/EplSdoAsndu.c
@@ -0,0 +1,483 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for SDO/Asnd-Protocolabstractionlayer module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplSdoAsndu.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.7 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/07/07 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "user/EplSdoAsndu.h"
+#include "user/EplDlluCal.h"
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_ASND)) != 0)
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#ifndef EPL_SDO_MAX_CONNECTION_ASND
+#define EPL_SDO_MAX_CONNECTION_ASND 5
+#endif
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+// instance table
+typedef struct {
+	unsigned int m_auiSdoAsndConnection[EPL_SDO_MAX_CONNECTION_ASND];
+	tEplSequLayerReceiveCb m_fpSdoAsySeqCb;
+
+} tEplSdoAsndInstance;
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+static tEplSdoAsndInstance SdoAsndInstance_g;
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplSdoAsnduCb(tEplFrameInfo * pFrameInfo_p);
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  <EPL SDO-Asnd Protocolabstraction layer>            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description: EPL SDO-Asnd Protocolabstraction layer
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoAsnduInit
+//
+// Description: init first instance of the module
+//
+//
+//
+// Parameters:  pReceiveCb_p    =   functionpointer to Sdo-Sequence layer
+//                                  callback-function
+//
+//
+// Returns:     tEplKernel  = Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoAsnduInit(tEplSequLayerReceiveCb fpReceiveCb_p)
+{
+	tEplKernel Ret;
+
+	Ret = EplSdoAsnduAddInstance(fpReceiveCb_p);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoAsnduAddInstance
+//
+// Description: init additional instance of the module
+//
+//
+//
+// Parameters:  pReceiveCb_p    =   functionpointer to Sdo-Sequence layer
+//                                  callback-function
+//
+//
+// Returns:     tEplKernel  = Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoAsnduAddInstance(tEplSequLayerReceiveCb fpReceiveCb_p)
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// init control structure
+	EPL_MEMSET(&SdoAsndInstance_g, 0x00, sizeof(SdoAsndInstance_g));
+
+	// save pointer to callback-function
+	if (fpReceiveCb_p != NULL) {
+		SdoAsndInstance_g.m_fpSdoAsySeqCb = fpReceiveCb_p;
+	} else {
+		Ret = kEplSdoUdpMissCb;
+	}
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLU)) != 0)
+	Ret = EplDlluCalRegAsndService(kEplDllAsndSdo,
+				       EplSdoAsnduCb, kEplDllAsndFilterLocal);
+#endif
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoAsnduDelInstance
+//
+// Description: del instance of the module
+//              del socket and del Listen-Thread
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoAsnduDelInstance()
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLU)) != 0)
+	// deregister callback function from DLL
+	Ret = EplDlluCalRegAsndService(kEplDllAsndSdo,
+				       NULL, kEplDllAsndFilterNone);
+#endif
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoAsnduInitCon
+//
+// Description: init a new connect
+//
+//
+//
+// Parameters:  pSdoConHandle_p = pointer for the new connection handle
+//              uiTargetNodeId_p = NodeId of the target node
+//
+//
+// Returns:     tEplKernel  = Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoAsnduInitCon(tEplSdoConHdl * pSdoConHandle_p,
+				     unsigned int uiTargetNodeId_p)
+{
+	tEplKernel Ret;
+	unsigned int uiCount;
+	unsigned int uiFreeCon;
+	unsigned int *puiConnection;
+
+	Ret = kEplSuccessful;
+
+	if ((uiTargetNodeId_p == EPL_C_ADR_INVALID)
+	    || (uiTargetNodeId_p >= EPL_C_ADR_BROADCAST)) {
+		Ret = kEplSdoAsndInvalidNodeId;
+		goto Exit;
+	}
+	// get free entry in control structure
+	uiCount = 0;
+	uiFreeCon = EPL_SDO_MAX_CONNECTION_ASND;
+	puiConnection = &SdoAsndInstance_g.m_auiSdoAsndConnection[0];
+	while (uiCount < EPL_SDO_MAX_CONNECTION_ASND) {
+		if (*puiConnection == uiTargetNodeId_p) {	// existing connection to target node found
+			// save handle for higher layer
+			*pSdoConHandle_p = (uiCount | EPL_SDO_ASND_HANDLE);
+
+			goto Exit;
+		} else if (*puiConnection == 0) {	// free entry-> save target nodeId
+			uiFreeCon = uiCount;
+		}
+		uiCount++;
+		puiConnection++;
+	}
+
+	if (uiFreeCon == EPL_SDO_MAX_CONNECTION_ASND) {
+		// no free connection
+		Ret = kEplSdoAsndNoFreeHandle;
+	} else {
+		puiConnection =
+		    &SdoAsndInstance_g.m_auiSdoAsndConnection[uiFreeCon];
+		*puiConnection = uiTargetNodeId_p;
+		// save handle for higher layer
+		*pSdoConHandle_p = (uiFreeCon | EPL_SDO_ASND_HANDLE);
+
+		goto Exit;
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoAsnduSendData
+//
+// Description: send data using exisiting connection
+//
+//
+//
+// Parameters:  SdoConHandle_p  = connection handle
+//              pSrcData_p      = pointer to data
+//              dwDataSize_p    = number of databyte
+//                                  -> without asnd-header!!!
+//
+// Returns:     tEplKernel  = Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoAsnduSendData(tEplSdoConHdl SdoConHandle_p,
+				      tEplFrame * pSrcData_p,
+				      DWORD dwDataSize_p)
+{
+	tEplKernel Ret;
+	unsigned int uiArray;
+	tEplFrameInfo FrameInfo;
+
+	Ret = kEplSuccessful;
+
+	uiArray = (SdoConHandle_p & ~EPL_SDO_ASY_HANDLE_MASK);
+
+	if (uiArray > EPL_SDO_MAX_CONNECTION_ASND) {
+		Ret = kEplSdoAsndInvalidHandle;
+		goto Exit;
+	}
+	// fillout Asnd header
+	// own node id not needed -> filled by DLL
+
+	// set message type
+	AmiSetByteToLe(&pSrcData_p->m_le_bMessageType, (BYTE) kEplMsgTypeAsnd);	// ASnd == 0x06
+	// target node id
+	AmiSetByteToLe(&pSrcData_p->m_le_bDstNodeId,
+		       (BYTE) SdoAsndInstance_g.
+		       m_auiSdoAsndConnection[uiArray]);
+	// set source-nodeid (filled by DLL 0)
+	AmiSetByteToLe(&pSrcData_p->m_le_bSrcNodeId, 0x00);
+
+	// calc size
+	dwDataSize_p += EPL_ASND_HEADER_SIZE;
+
+	// send function of DLL
+	FrameInfo.m_uiFrameSize = dwDataSize_p;
+	FrameInfo.m_pFrame = pSrcData_p;
+	EPL_MEMSET(&FrameInfo.m_NetTime, 0x00, sizeof(tEplNetTime));
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLU)) != 0)
+	Ret = EplDlluCalAsyncSend(&FrameInfo, kEplDllAsyncReqPrioGeneric);
+#endif
+
+      Exit:
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoAsnduDelCon
+//
+// Description: delete connection from intern structure
+//
+//
+//
+// Parameters:  SdoConHandle_p  = connection handle
+//
+// Returns:     tEplKernel  = Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoAsnduDelCon(tEplSdoConHdl SdoConHandle_p)
+{
+	tEplKernel Ret;
+	unsigned int uiArray;
+
+	Ret = kEplSuccessful;
+
+	uiArray = (SdoConHandle_p & ~EPL_SDO_ASY_HANDLE_MASK);
+	// check parameter
+	if (uiArray > EPL_SDO_MAX_CONNECTION_ASND) {
+		Ret = kEplSdoAsndInvalidHandle;
+		goto Exit;
+	}
+	// set target nodeId to 0
+	SdoAsndInstance_g.m_auiSdoAsndConnection[uiArray] = 0;
+
+      Exit:
+	return Ret;
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoAsnduCb
+//
+// Description: callback function for SDO ASnd frames
+//
+//
+//
+// Parameters:      pFrameInfo_p = Frame with SDO payload
+//
+//
+// Returns:         tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoAsnduCb(tEplFrameInfo * pFrameInfo_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	unsigned int uiCount;
+	unsigned int *puiConnection;
+	unsigned int uiNodeId;
+	unsigned int uiFreeEntry = 0xFFFF;
+	tEplSdoConHdl SdoConHdl;
+	tEplFrame *pFrame;
+
+	pFrame = pFrameInfo_p->m_pFrame;
+
+	uiNodeId = AmiGetByteFromLe(&pFrame->m_le_bSrcNodeId);
+
+	// search corresponding entry in control structure
+	uiCount = 0;
+	puiConnection = &SdoAsndInstance_g.m_auiSdoAsndConnection[0];
+	while (uiCount < EPL_SDO_MAX_CONNECTION_ASND) {
+		if (uiNodeId == *puiConnection) {
+			break;
+		} else if ((*puiConnection == 0)
+			   && (uiFreeEntry == 0xFFFF)) {	// free entry
+			uiFreeEntry = uiCount;
+		}
+		uiCount++;
+		puiConnection++;
+	}
+
+	if (uiCount == EPL_SDO_MAX_CONNECTION_ASND) {
+		if (uiFreeEntry != 0xFFFF) {
+			puiConnection =
+			    &SdoAsndInstance_g.
+			    m_auiSdoAsndConnection[uiFreeEntry];
+			*puiConnection = uiNodeId;
+			uiCount = uiFreeEntry;
+		} else {
+			EPL_DBGLVL_SDO_TRACE0
+			    ("EplSdoAsnduCb(): no free handle\n");
+			goto Exit;
+		}
+	}
+//    if (uiNodeId == *puiConnection)
+	{			// entry found or created
+		SdoConHdl = (uiCount | EPL_SDO_ASND_HANDLE);
+
+		SdoAsndInstance_g.m_fpSdoAsySeqCb(SdoConHdl,
+						  &pFrame->m_Data.m_Asnd.
+						  m_Payload.m_SdoSequenceFrame,
+						  (pFrameInfo_p->m_uiFrameSize -
+						   18));
+	}
+
+      Exit:
+	return Ret;
+
+}
+
+#endif // end of #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_ASND)) != 0)
+// EOF
diff --git a/drivers/staging/epl/EplSdoAsySequ.c b/drivers/staging/epl/EplSdoAsySequ.c
new file mode 100644
index 00000000000..991c6be880c
--- /dev/null
+++ b/drivers/staging/epl/EplSdoAsySequ.c
@@ -0,0 +1,2522 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for asychronous SDO Sequence Layer module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplSdoAsySequ.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.10 $  $Date: 2008/11/13 17:13:09 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/26 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "user/EplSdoAsySequ.h"
+
+#if ((((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) == 0) &&\
+     (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_ASND)) == 0)   )
+
+#error 'ERROR: At least UDP or Asnd module needed!'
+
+#endif
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#define EPL_SDO_HISTORY_SIZE        5
+
+#ifndef EPL_MAX_SDO_SEQ_CON
+#define EPL_MAX_SDO_SEQ_CON         10
+#endif
+
+#define EPL_SEQ_DEFAULT_TIMEOUT     5000	// in [ms] => 5 sec
+
+#define EPL_SEQ_RETRY_COUNT         5	// => max. Timeout 30 sec
+
+#define EPL_SEQ_NUM_THRESHOLD       100	// threshold which distinguishes between old and new sequence numbers
+
+// define frame with size of Asnd-Header-, SDO Sequenze Header size, SDO Command header
+// and Ethernet-Header size
+#define EPL_SEQ_FRAME_SIZE          24
+// size of the header of the asynchronus SDO Sequence layer
+#define EPL_SEQ_HEADER_SIZE         4
+
+// buffersize for one frame in history
+#define EPL_SEQ_HISTROY_FRAME_SIZE  EPL_MAX_SDO_FRAME_SIZE
+
+// mask to get scon and rcon
+#define EPL_ASY_SDO_CON_MASK        0x03
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+// events for processfunction
+typedef enum {
+	kAsySdoSeqEventNoEvent = 0x00,	// no Event
+	kAsySdoSeqEventInitCon = 0x01,	// init connection
+	kAsySdoSeqEventFrameRec = 0x02,	// frame received
+	kAsySdoSeqEventFrameSend = 0x03,	// frame to send
+	kAsySdoSeqEventTimeout = 0x04,	// Timeout for connection
+	kAsySdoSeqEventCloseCon = 0x05	// higher layer close connection
+} tEplAsySdoSeqEvent;
+
+// structure for History-Buffer
+typedef struct {
+	BYTE m_bFreeEntries;
+	BYTE m_bWrite;		// index of the next free buffer entry
+	BYTE m_bAck;		// index of the next message which should become acknowledged
+	BYTE m_bRead;		// index between m_bAck and m_bWrite to the next message for retransmission
+	BYTE m_aabHistoryFrame[EPL_SDO_HISTORY_SIZE]
+	    [EPL_SEQ_HISTROY_FRAME_SIZE];
+	unsigned int m_auiFrameSize[EPL_SDO_HISTORY_SIZE];
+
+} tEplAsySdoConHistory;
+
+// state of the statemaschine
+typedef enum {
+	kEplAsySdoStateIdle = 0x00,
+	kEplAsySdoStateInit1 = 0x01,
+	kEplAsySdoStateInit2 = 0x02,
+	kEplAsySdoStateInit3 = 0x03,
+	kEplAsySdoStateConnected = 0x04,
+	kEplAsySdoStateWaitAck = 0x05
+} tEplAsySdoState;
+
+// connection control structure
+typedef struct {
+	tEplSdoConHdl m_ConHandle;
+	tEplAsySdoState m_SdoState;
+	BYTE m_bRecSeqNum;	// name from view of the communication partner
+	BYTE m_bSendSeqNum;	// name from view of the communication partner
+	tEplAsySdoConHistory m_SdoConHistory;
+	tEplTimerHdl m_EplTimerHdl;
+	unsigned int m_uiRetryCount;	// retry counter
+	unsigned int m_uiUseCount;	// one sequence layer connection may be used by
+	// multiple command layer connections
+
+} tEplAsySdoSeqCon;
+
+// instance structure
+typedef struct {
+	tEplAsySdoSeqCon m_AsySdoConnection[EPL_MAX_SDO_SEQ_CON];
+	tEplSdoComReceiveCb m_fpSdoComReceiveCb;
+	tEplSdoComConCb m_fpSdoComConCb;
+
+#if defined(WIN32) || defined(_WIN32)
+	LPCRITICAL_SECTION m_pCriticalSection;
+	CRITICAL_SECTION m_CriticalSection;
+
+	LPCRITICAL_SECTION m_pCriticalSectionReceive;
+	CRITICAL_SECTION m_CriticalSectionReceive;
+#endif
+
+} tEplAsySdoSequInstance;
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+static tEplAsySdoSequInstance AsySdoSequInstance_g;
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+static tEplKernel EplSdoAsySeqProcess(unsigned int uiHandle_p,
+				      unsigned int uiDataSize_p,
+				      tEplFrame * pData_p,
+				      tEplAsySdoSeq * pRecFrame_p,
+				      tEplAsySdoSeqEvent Event_p);
+
+static tEplKernel EplSdoAsySeqSendIntern(tEplAsySdoSeqCon * pAsySdoSeqCon_p,
+					 unsigned int uiDataSize_p,
+					 tEplFrame * pData_p,
+					 BOOL fFrameInHistory);
+
+static tEplKernel EplSdoAsySeqSendLowerLayer(tEplAsySdoSeqCon * pAsySdoSeqCon_p,
+					     unsigned int uiDataSize_p,
+					     tEplFrame * pEplFrame_p);
+
+tEplKernel PUBLIC EplSdoAsyReceiveCb(tEplSdoConHdl ConHdl_p,
+				     tEplAsySdoSeq * pSdoSeqData_p,
+				     unsigned int uiDataSize_p);
+
+static tEplKernel EplSdoAsyInitHistory(void);
+
+static tEplKernel EplSdoAsyAddFrameToHistory(tEplAsySdoSeqCon * pAsySdoSeqCon_p,
+					     tEplFrame * pFrame_p,
+					     unsigned int uiSize_p);
+
+static tEplKernel EplSdoAsyAckFrameToHistory(tEplAsySdoSeqCon * pAsySdoSeqCon_p,
+					     BYTE bRecSeqNumber_p);
+
+static tEplKernel EplSdoAsyReadFromHistory(tEplAsySdoSeqCon * pAsySdoSeqCon_p,
+					   tEplFrame ** ppFrame_p,
+					   unsigned int *puiSize_p,
+					   BOOL fInitRead);
+
+static unsigned int EplSdoAsyGetFreeEntriesFromHistory(tEplAsySdoSeqCon *
+						       pAsySdoSeqCon_p);
+
+static tEplKernel EplSdoAsySeqSetTimer(tEplAsySdoSeqCon * pAsySdoSeqCon_p,
+				       unsigned long ulTimeout);
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  <EPL asychronus SDO Sequence layer>                 */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description: this module contains the asynchronus SDO Sequence Layer for
+//              the EPL SDO service
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoAsySeqInit
+//
+// Description: init first instance
+//
+//
+//
+// Parameters:  fpSdoComCb_p    = callback function to inform Command layer
+//                                about new frames
+//              fpSdoComConCb_p = callback function to inform command layer
+//                                about connection state
+//
+//
+// Returns:     tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoAsySeqInit(tEplSdoComReceiveCb fpSdoComCb_p,
+				   tEplSdoComConCb fpSdoComConCb_p)
+{
+	tEplKernel Ret;
+
+	Ret = EplSdoAsySeqAddInstance(fpSdoComCb_p, fpSdoComConCb_p);
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoAsySeqAddInstance
+//
+// Description: init following instances
+//
+//
+//
+// Parameters:  fpSdoComCb_p    = callback function to inform Command layer
+//                                about new frames
+//              fpSdoComConCb_p = callback function to inform command layer
+//                                about connection state
+//
+// Returns:     tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoAsySeqAddInstance(tEplSdoComReceiveCb fpSdoComCb_p,
+					  tEplSdoComConCb fpSdoComConCb_p)
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// check functionpointer
+	if (fpSdoComCb_p == NULL) {
+		Ret = kEplSdoSeqMissCb;
+		goto Exit;
+	} else {
+		AsySdoSequInstance_g.m_fpSdoComReceiveCb = fpSdoComCb_p;
+	}
+
+	// check functionpointer
+	if (fpSdoComConCb_p == NULL) {
+		Ret = kEplSdoSeqMissCb;
+		goto Exit;
+	} else {
+		AsySdoSequInstance_g.m_fpSdoComConCb = fpSdoComConCb_p;
+	}
+
+	// set controllstructure to 0
+	EPL_MEMSET(&AsySdoSequInstance_g.m_AsySdoConnection[0], 0x00,
+		   sizeof(AsySdoSequInstance_g.m_AsySdoConnection));
+
+	// init History
+	Ret = EplSdoAsyInitHistory();
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#if defined(WIN32) || defined(_WIN32)
+	// create critical section for process function
+	AsySdoSequInstance_g.m_pCriticalSection =
+	    &AsySdoSequInstance_g.m_CriticalSection;
+	InitializeCriticalSection(AsySdoSequInstance_g.m_pCriticalSection);
+
+	// init critical section for receive cb function
+	AsySdoSequInstance_g.m_pCriticalSectionReceive =
+	    &AsySdoSequInstance_g.m_CriticalSectionReceive;
+	InitializeCriticalSection(AsySdoSequInstance_g.
+				  m_pCriticalSectionReceive);
+#endif
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) != 0)
+	// init lower layer
+	Ret = EplSdoUdpuAddInstance(EplSdoAsyReceiveCb);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#endif
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_ASND)) != 0)
+	// init lower layer
+	Ret = EplSdoAsnduAddInstance(EplSdoAsyReceiveCb);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#endif
+
+      Exit:
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoAsySeqDelInstance
+//
+// Description: delete instances
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoAsySeqDelInstance()
+{
+	tEplKernel Ret;
+	unsigned int uiCount;
+	tEplAsySdoSeqCon *pAsySdoSeqCon;
+
+	Ret = kEplSuccessful;
+
+	// delete timer of open connections
+	uiCount = 0;
+	pAsySdoSeqCon = &AsySdoSequInstance_g.m_AsySdoConnection[0];
+	while (uiCount < EPL_MAX_SDO_SEQ_CON) {
+		if (pAsySdoSeqCon->m_ConHandle != 0) {
+			EplTimeruDeleteTimer(&pAsySdoSeqCon->m_EplTimerHdl);
+		}
+		uiCount++;
+		pAsySdoSeqCon++;
+	}
+
+#if defined(WIN32) || defined(_WIN32)
+	// delete critical section for process function
+	DeleteCriticalSection(AsySdoSequInstance_g.m_pCriticalSection);
+#endif
+
+	// set instance-table to 0
+	EPL_MEMSET(&AsySdoSequInstance_g, 0x00, sizeof(AsySdoSequInstance_g));
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) != 0)
+	// delete lower layer
+	Ret = EplSdoUdpuDelInstance();
+#endif
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_ASND)) != 0)
+	// delete lower layer
+	Ret = EplSdoAsnduDelInstance();
+#endif
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoAsySeqInitCon
+//
+// Description: start initialization of a sequence layer connection.
+//              It tries to reuse an existing connection to the same node.
+//
+//
+// Parameters:  pSdoSeqConHdl_p = pointer to the variable for the connection handle
+//              uiNodeId_p      = Node Id of the target
+//              SdoType          = Type of the SDO connection
+//
+//
+// Returns:     tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoAsySeqInitCon(tEplSdoSeqConHdl * pSdoSeqConHdl_p,
+				      unsigned int uiNodeId_p,
+				      tEplSdoType SdoType)
+{
+	tEplKernel Ret;
+	unsigned int uiCount;
+	unsigned int uiFreeCon;
+	tEplSdoConHdl ConHandle;
+	tEplAsySdoSeqCon *pAsySdoSeqCon;
+	Ret = kEplSuccessful;
+
+	// check SdoType
+	// call init function of the protcol abstraction layer
+	// which tries to find an existing connection to the same node
+	switch (SdoType) {
+		// SDO over UDP
+	case kEplSdoTypeUdp:
+		{
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) != 0)
+			Ret = EplSdoUdpuInitCon(&ConHandle, uiNodeId_p);
+			if (Ret != kEplSuccessful) {
+				goto Exit;
+			}
+#else
+			Ret = kEplSdoSeqUnsupportedProt;
+#endif
+			break;
+		}
+
+		// SDO over Asnd
+	case kEplSdoTypeAsnd:
+		{
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_ASND)) != 0)
+			Ret = EplSdoAsnduInitCon(&ConHandle, uiNodeId_p);
+			if (Ret != kEplSuccessful) {
+				goto Exit;
+			}
+#else
+			Ret = kEplSdoSeqUnsupportedProt;
+#endif
+			break;
+		}
+
+		// unsupported protocols
+		// -> auto should be replaced by command layer
+	case kEplSdoTypeAuto:
+	case kEplSdoTypePdo:
+	default:
+		{
+			Ret = kEplSdoSeqUnsupportedProt;
+			goto Exit;
+		}
+
+	}			// end of switch(SdoType)
+
+	// find existing connection to the same node or find empty entry for connection
+	uiCount = 0;
+	uiFreeCon = EPL_MAX_SDO_SEQ_CON;
+	pAsySdoSeqCon = &AsySdoSequInstance_g.m_AsySdoConnection[0];
+
+	while (uiCount < EPL_MAX_SDO_SEQ_CON) {
+		if (pAsySdoSeqCon->m_ConHandle == ConHandle) {	// existing connection found
+			break;
+		}
+		if (pAsySdoSeqCon->m_ConHandle == 0) {
+			uiFreeCon = uiCount;
+		}
+		uiCount++;
+		pAsySdoSeqCon++;
+	}
+
+	if (uiCount == EPL_MAX_SDO_SEQ_CON) {
+		if (uiFreeCon == EPL_MAX_SDO_SEQ_CON) {	// no free entry found
+			switch (SdoType) {
+				// SDO over UDP
+			case kEplSdoTypeUdp:
+				{
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) != 0)
+					Ret = EplSdoUdpuDelCon(ConHandle);
+					if (Ret != kEplSuccessful) {
+						goto Exit;
+					}
+#endif
+					break;
+				}
+
+				// SDO over Asnd
+			case kEplSdoTypeAsnd:
+				{
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_ASND)) != 0)
+					Ret = EplSdoAsnduDelCon(ConHandle);
+					if (Ret != kEplSuccessful) {
+						goto Exit;
+					}
+#endif
+					break;
+				}
+
+				// unsupported protocols
+				// -> auto should be replaced by command layer
+			case kEplSdoTypeAuto:
+			case kEplSdoTypePdo:
+			default:
+				{
+					Ret = kEplSdoSeqUnsupportedProt;
+					goto Exit;
+				}
+
+			}	// end of switch(SdoType)
+
+			Ret = kEplSdoSeqNoFreeHandle;
+			goto Exit;
+		} else {	// free entry found
+			pAsySdoSeqCon =
+			    &AsySdoSequInstance_g.m_AsySdoConnection[uiFreeCon];
+			pAsySdoSeqCon->m_ConHandle = ConHandle;
+			uiCount = uiFreeCon;
+		}
+	}
+	// set handle
+	*pSdoSeqConHdl_p = (uiCount | EPL_SDO_ASY_HANDLE);
+
+	// increment use counter
+	pAsySdoSeqCon->m_uiUseCount++;
+
+	// call intern process function
+	Ret = EplSdoAsySeqProcess(uiCount,
+				  0, NULL, NULL, kAsySdoSeqEventInitCon);
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoAsySeqSendData
+//
+// Description: send sata unsing a established connection
+//
+//
+//
+// Parameters:  pSdoSeqConHdl_p = connection handle
+//              uiDataSize_p    = Size of Frame to send
+//                                  -> wihtout SDO sequence layer header, Asnd header
+//                                     and ethernetnet
+//                                  ==> SDO Sequence layer payload
+//              SdoType          = Type of the SDO connection
+//
+//
+// Returns:     tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoAsySeqSendData(tEplSdoSeqConHdl SdoSeqConHdl_p,
+				       unsigned int uiDataSize_p,
+				       tEplFrame * pabData_p)
+{
+	tEplKernel Ret;
+	unsigned int uiHandle;
+
+	uiHandle = (SdoSeqConHdl_p & ~EPL_SDO_SEQ_HANDLE_MASK);
+
+	// check if connection ready
+	if (AsySdoSequInstance_g.m_AsySdoConnection[uiHandle].m_SdoState ==
+	    kEplAsySdoStateIdle) {
+		// no connection with this handle
+		Ret = kEplSdoSeqInvalidHdl;
+		goto Exit;
+	} else if (AsySdoSequInstance_g.m_AsySdoConnection[uiHandle].
+		   m_SdoState != kEplAsySdoStateConnected) {
+		Ret = kEplSdoSeqConnectionBusy;
+		goto Exit;
+	}
+
+	Ret = EplSdoAsySeqProcess(uiHandle,
+				  uiDataSize_p,
+				  pabData_p, NULL, kAsySdoSeqEventFrameSend);
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoAsySeqProcessEvent
+//
+// Description: function processes extern events
+//              -> later needed for timeout controll with timer-module
+//
+//
+//
+// Parameters:  pEvent_p = pointer to event
+//
+//
+// Returns:     tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoAsySeqProcessEvent(tEplEvent * pEvent_p)
+{
+	tEplKernel Ret;
+	tEplTimerEventArg *pTimerEventArg;
+	tEplAsySdoSeqCon *pAsySdoSeqCon;
+	tEplTimerHdl EplTimerHdl;
+	unsigned int uiCount;
+
+	Ret = kEplSuccessful;
+	// check parameter
+	if (pEvent_p == NULL) {
+		Ret = kEplSdoSeqInvalidEvent;
+		goto Exit;
+	}
+
+	if (pEvent_p->m_EventType != kEplEventTypeTimer) {
+		Ret = kEplSdoSeqInvalidEvent;
+		goto Exit;
+	}
+	// get timerhdl
+	pTimerEventArg = (tEplTimerEventArg *) pEvent_p->m_pArg;
+	EplTimerHdl = pTimerEventArg->m_TimerHdl;
+
+	// get pointer to intern control structure of connection
+	if (pTimerEventArg->m_ulArg == 0) {
+		goto Exit;
+	}
+	pAsySdoSeqCon = (tEplAsySdoSeqCon *) pTimerEventArg->m_ulArg;
+
+	// check if time is current
+	if (EplTimerHdl != pAsySdoSeqCon->m_EplTimerHdl) {
+		// delete timer
+		EplTimeruDeleteTimer(&EplTimerHdl);
+		goto Exit;
+	}
+	// delete timer
+	EplTimeruDeleteTimer(&pAsySdoSeqCon->m_EplTimerHdl);
+
+	// get indexnumber of control structure
+	uiCount = 0;
+	while ((&AsySdoSequInstance_g.m_AsySdoConnection[uiCount]) !=
+	       pAsySdoSeqCon) {
+		uiCount++;
+		if (uiCount > EPL_MAX_SDO_SEQ_CON) {
+			goto Exit;
+		}
+	}
+
+	// process event and call processfunction if needed
+	Ret = EplSdoAsySeqProcess(uiCount,
+				  0, NULL, NULL, kAsySdoSeqEventTimeout);
+
+      Exit:
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoAsySeqDelCon
+//
+// Description: del and close one connection
+//
+//
+//
+// Parameters:  SdoSeqConHdl_p = handle of connection
+//
+//
+// Returns:     tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoAsySeqDelCon(tEplSdoSeqConHdl SdoSeqConHdl_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	unsigned int uiHandle;
+	tEplAsySdoSeqCon *pAsySdoSeqCon;
+
+	uiHandle = (SdoSeqConHdl_p & ~EPL_SDO_SEQ_HANDLE_MASK);
+
+	// check if handle invalid
+	if (uiHandle >= EPL_MAX_SDO_SEQ_CON) {
+		Ret = kEplSdoSeqInvalidHdl;
+		goto Exit;
+	}
+	// get pointer to connection
+	pAsySdoSeqCon = &AsySdoSequInstance_g.m_AsySdoConnection[uiHandle];
+
+	// decrement use counter
+	pAsySdoSeqCon->m_uiUseCount--;
+
+	if (pAsySdoSeqCon->m_uiUseCount == 0) {
+		// process close in processfunction
+		Ret = EplSdoAsySeqProcess(uiHandle,
+					  0,
+					  NULL, NULL, kAsySdoSeqEventCloseCon);
+
+		//check protocol
+		if ((pAsySdoSeqCon->m_ConHandle & EPL_SDO_ASY_HANDLE_MASK) ==
+		    EPL_SDO_UDP_HANDLE) {
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) != 0)
+			// call close function of lower layer
+			EplSdoUdpuDelCon(pAsySdoSeqCon->m_ConHandle);
+#endif // end of #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) != 0)
+		} else {
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_ASND)) != 0)
+			// call close function of lower layer
+			EplSdoAsnduDelCon(pAsySdoSeqCon->m_ConHandle);
+#endif // end of #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_ASND)) != 0)
+		}
+
+		// delete timer
+		EplTimeruDeleteTimer(&pAsySdoSeqCon->m_EplTimerHdl);
+
+		// clean controllstructure
+		EPL_MEMSET(pAsySdoSeqCon, 0x00, sizeof(tEplAsySdoSeqCon));
+		pAsySdoSeqCon->m_SdoConHistory.m_bFreeEntries =
+		    EPL_SDO_HISTORY_SIZE;
+	}
+
+      Exit:
+	return Ret;
+
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplEplSdoAsySeqProcess
+//
+// Description: intern function to process the asynchronus SDO Sequence Layer
+//              state maschine
+//
+//
+//
+// Parameters:  uiHandle_p      = index of the control structure of the connection
+//              uiDataSize_p    = size of data frame to process (can be 0)
+//                                  -> without size of sequence header and Asnd header!!!
+//
+//              pData_p         = pointer to frame to send (can be NULL)
+//              pRecFrame_p     = pointer to received frame (can be NULL)
+//              Event_p         = Event to process
+//
+//
+//
+// Returns:     tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static tEplKernel EplSdoAsySeqProcess(unsigned int uiHandle_p,
+				      unsigned int uiDataSize_p,
+				      tEplFrame * pData_p,
+				      tEplAsySdoSeq * pRecFrame_p,
+				      tEplAsySdoSeqEvent Event_p)
+{
+	tEplKernel Ret;
+	unsigned int uiFrameSize;
+	tEplFrame *pEplFrame;
+	tEplAsySdoSeqCon *pAsySdoSeqCon;
+	tEplSdoSeqConHdl SdoSeqConHdl;
+	unsigned int uiFreeEntries;
+
+#if defined(WIN32) || defined(_WIN32)
+	// enter  critical section for process function
+	EnterCriticalSection(AsySdoSequInstance_g.m_pCriticalSection);
+#endif
+
+	Ret = kEplSuccessful;
+
+	// get handle for hinger layer
+	SdoSeqConHdl = uiHandle_p | EPL_SDO_ASY_HANDLE;
+
+	// check if handle invalid
+	if ((SdoSeqConHdl & ~EPL_SDO_SEQ_HANDLE_MASK) ==
+	    EPL_SDO_SEQ_INVALID_HDL) {
+		Ret = kEplSdoSeqInvalidHdl;
+		goto Exit;
+	}
+	// get pointer to connection
+	pAsySdoSeqCon = &AsySdoSequInstance_g.m_AsySdoConnection[uiHandle_p];
+
+	// check size
+	if ((pData_p == NULL) && (pRecFrame_p == NULL) && (uiDataSize_p != 0)) {
+		Ret = kEplSdoSeqInvalidFrame;
+		goto Exit;
+	}
+	// check state
+	switch (pAsySdoSeqCon->m_SdoState) {
+		// idle state
+	case kEplAsySdoStateIdle:
+		{
+			// check event
+			switch (Event_p) {
+				// new connection
+				// -> send init frame and change to
+				// kEplAsySdoStateInit1
+			case kAsySdoSeqEventInitCon:
+				{
+					// set sending scon to 1
+					pAsySdoSeqCon->m_bRecSeqNum = 0x01;
+					// set set send rcon to 0
+					pAsySdoSeqCon->m_bSendSeqNum = 0x00;
+					Ret =
+					    EplSdoAsySeqSendIntern
+					    (pAsySdoSeqCon, 0, NULL, FALSE);
+					if (Ret != kEplSuccessful) {
+						goto Exit;
+					}
+					// change state
+					pAsySdoSeqCon->m_SdoState =
+					    kEplAsySdoStateInit1;
+
+					// set timer
+					Ret =
+					    EplSdoAsySeqSetTimer(pAsySdoSeqCon,
+								 EPL_SEQ_DEFAULT_TIMEOUT);
+
+					break;
+				}
+
+				// init con from extern
+				// check rcon and scon
+				// -> send answer
+			case kAsySdoSeqEventFrameRec:
+				{
+/*
+                    PRINTF3("%s scon=%u rcon=%u\n",
+                            __FUNCTION__,
+                            pRecFrame_p->m_le_bSendSeqNumCon,
+                            pRecFrame_p->m_le_bRecSeqNumCon);
+*/
+					// check if scon == 1 and rcon == 0
+					if (((pRecFrame_p->
+					      m_le_bRecSeqNumCon &
+					      EPL_ASY_SDO_CON_MASK) == 0x00)
+					    &&
+					    ((pRecFrame_p->
+					      m_le_bSendSeqNumCon &
+					      EPL_ASY_SDO_CON_MASK) == 0x01)) {
+						// save sequence numbers
+						pAsySdoSeqCon->m_bRecSeqNum =
+						    AmiGetByteFromLe
+						    (&pRecFrame_p->
+						     m_le_bRecSeqNumCon);
+						pAsySdoSeqCon->m_bSendSeqNum =
+						    AmiGetByteFromLe
+						    (&pRecFrame_p->
+						     m_le_bSendSeqNumCon);
+						// create answer and send answer
+						// set rcon to 1 (in send direction own scon)
+						pAsySdoSeqCon->m_bRecSeqNum++;
+						Ret =
+						    EplSdoAsySeqSendIntern
+						    (pAsySdoSeqCon, 0, NULL,
+						     FALSE);
+						if (Ret != kEplSuccessful) {
+							goto Exit;
+						}
+						// change state to kEplAsySdoStateInit2
+						pAsySdoSeqCon->m_SdoState =
+						    kEplAsySdoStateInit2;
+
+						// set timer
+						Ret =
+						    EplSdoAsySeqSetTimer
+						    (pAsySdoSeqCon,
+						     EPL_SEQ_DEFAULT_TIMEOUT);
+					} else {	// error -> close
+						// delete timer
+						EplTimeruDeleteTimer
+						    (&pAsySdoSeqCon->
+						     m_EplTimerHdl);
+						if (((pRecFrame_p->
+						      m_le_bRecSeqNumCon &
+						      EPL_ASY_SDO_CON_MASK) !=
+						     0x00)
+						    || ((pRecFrame_p->m_le_bSendSeqNumCon & EPL_ASY_SDO_CON_MASK) != 0x00)) {	// d.k. only answer with close message if the message sent was not a close message
+							// save sequence numbers
+							pAsySdoSeqCon->
+							    m_bRecSeqNum =
+							    AmiGetByteFromLe
+							    (&pRecFrame_p->
+							     m_le_bRecSeqNumCon);
+							pAsySdoSeqCon->
+							    m_bSendSeqNum =
+							    AmiGetByteFromLe
+							    (&pRecFrame_p->
+							     m_le_bSendSeqNumCon);
+							// set rcon and scon to 0
+							pAsySdoSeqCon->
+							    m_bSendSeqNum &=
+							    EPL_SEQ_NUM_MASK;
+							pAsySdoSeqCon->
+							    m_bRecSeqNum &=
+							    EPL_SEQ_NUM_MASK;
+							// send frame
+							EplSdoAsySeqSendIntern
+							    (pAsySdoSeqCon, 0,
+							     NULL, FALSE);
+						}
+						// call Command Layer Cb
+						AsySdoSequInstance_g.
+						    m_fpSdoComConCb
+						    (SdoSeqConHdl,
+						     kAsySdoConStateInitError);
+					}
+					break;
+				}
+
+			default:
+				// d.k. do nothing
+				break;
+
+			}	// end of switch(Event_p)
+			break;
+		}
+
+		// init connection step 1
+		// wait for frame with scon = 1
+		// and rcon = 1
+	case kEplAsySdoStateInit1:
+		{
+//            PRINTF0("EplSdoAsySequ: StateInit1\n");
+
+			// check event
+			switch (Event_p) {
+				// frame received
+			case kAsySdoSeqEventFrameRec:
+				{
+					// check scon == 1 and rcon == 1
+					if (((pRecFrame_p->
+					      m_le_bRecSeqNumCon &
+					      EPL_ASY_SDO_CON_MASK) == 0x01)
+					    && ((pRecFrame_p->m_le_bSendSeqNumCon & EPL_ASY_SDO_CON_MASK) == 0x01)) {	// create answer own scon = 2
+						// save sequence numbers
+						pAsySdoSeqCon->m_bRecSeqNum =
+						    AmiGetByteFromLe
+						    (&pRecFrame_p->
+						     m_le_bRecSeqNumCon);
+						pAsySdoSeqCon->m_bSendSeqNum =
+						    AmiGetByteFromLe
+						    (&pRecFrame_p->
+						     m_le_bSendSeqNumCon);
+
+						pAsySdoSeqCon->m_bRecSeqNum++;
+						Ret =
+						    EplSdoAsySeqSendIntern
+						    (pAsySdoSeqCon, 0, NULL,
+						     FALSE);
+						if (Ret != kEplSuccessful) {
+							goto Exit;
+						}
+						// change state to kEplAsySdoStateInit3
+						pAsySdoSeqCon->m_SdoState =
+						    kEplAsySdoStateInit3;
+
+						// set timer
+						Ret =
+						    EplSdoAsySeqSetTimer
+						    (pAsySdoSeqCon,
+						     EPL_SEQ_DEFAULT_TIMEOUT);
+
+					}
+					// check if scon == 1 and rcon == 0, i.e. other side wants me to be server
+					else if (((pRecFrame_p->
+						   m_le_bRecSeqNumCon &
+						   EPL_ASY_SDO_CON_MASK) ==
+						  0x00)
+						 &&
+						 ((pRecFrame_p->
+						   m_le_bSendSeqNumCon &
+						   EPL_ASY_SDO_CON_MASK) ==
+						  0x01)) {
+						// save sequence numbers
+						pAsySdoSeqCon->m_bRecSeqNum =
+						    AmiGetByteFromLe
+						    (&pRecFrame_p->
+						     m_le_bRecSeqNumCon);
+						pAsySdoSeqCon->m_bSendSeqNum =
+						    AmiGetByteFromLe
+						    (&pRecFrame_p->
+						     m_le_bSendSeqNumCon);
+						// create answer and send answer
+						// set rcon to 1 (in send direction own scon)
+						pAsySdoSeqCon->m_bRecSeqNum++;
+						Ret =
+						    EplSdoAsySeqSendIntern
+						    (pAsySdoSeqCon, 0, NULL,
+						     FALSE);
+						if (Ret != kEplSuccessful) {
+							goto Exit;
+						}
+						// change state to kEplAsySdoStateInit2
+						pAsySdoSeqCon->m_SdoState =
+						    kEplAsySdoStateInit2;
+
+						// set timer
+						Ret =
+						    EplSdoAsySeqSetTimer
+						    (pAsySdoSeqCon,
+						     EPL_SEQ_DEFAULT_TIMEOUT);
+					} else {	// error -> Close
+						pAsySdoSeqCon->m_SdoState =
+						    kEplAsySdoStateIdle;
+						// delete timer
+						EplTimeruDeleteTimer
+						    (&pAsySdoSeqCon->
+						     m_EplTimerHdl);
+						if (((pRecFrame_p->
+						      m_le_bRecSeqNumCon &
+						      EPL_ASY_SDO_CON_MASK) !=
+						     0x00)
+						    || ((pRecFrame_p->m_le_bSendSeqNumCon & EPL_ASY_SDO_CON_MASK) != 0x00)) {	// d.k. only answer with close message if the message sent was not a close message
+							// save sequence numbers
+							pAsySdoSeqCon->
+							    m_bRecSeqNum =
+							    AmiGetByteFromLe
+							    (&pRecFrame_p->
+							     m_le_bRecSeqNumCon);
+							pAsySdoSeqCon->
+							    m_bSendSeqNum =
+							    AmiGetByteFromLe
+							    (&pRecFrame_p->
+							     m_le_bSendSeqNumCon);
+
+							// set rcon and scon to 0
+							pAsySdoSeqCon->
+							    m_bSendSeqNum &=
+							    EPL_SEQ_NUM_MASK;
+							pAsySdoSeqCon->
+							    m_bRecSeqNum &=
+							    EPL_SEQ_NUM_MASK;
+							// send frame
+							EplSdoAsySeqSendIntern
+							    (pAsySdoSeqCon, 0,
+							     NULL, FALSE);
+						}
+						// call Command Layer Cb
+						AsySdoSequInstance_g.
+						    m_fpSdoComConCb
+						    (SdoSeqConHdl,
+						     kAsySdoConStateInitError);
+					}
+					break;
+				}
+
+				// timeout
+			case kAsySdoSeqEventTimeout:
+				{	// error -> Close
+					pAsySdoSeqCon->m_SdoState =
+					    kEplAsySdoStateIdle;
+
+					// set rcon and scon to 0
+					pAsySdoSeqCon->m_bSendSeqNum &=
+					    EPL_SEQ_NUM_MASK;
+					pAsySdoSeqCon->m_bRecSeqNum &=
+					    EPL_SEQ_NUM_MASK;
+					// send frame
+					EplSdoAsySeqSendIntern(pAsySdoSeqCon,
+							       0, NULL, FALSE);
+					// call Command Layer Cb
+					AsySdoSequInstance_g.
+					    m_fpSdoComConCb(SdoSeqConHdl,
+							    kAsySdoConStateInitError);
+					break;
+				}
+
+			default:
+				// d.k. do nothing
+				break;
+
+			}	// end of switch(Event_p)
+			break;
+		}
+
+		// init connection step 2
+	case kEplAsySdoStateInit2:
+		{
+//            PRINTF0("EplSdoAsySequ: StateInit2\n");
+
+			// check event
+			switch (Event_p) {
+				// frame received
+			case kAsySdoSeqEventFrameRec:
+				{
+					// check scon == 2 and rcon == 1
+					if (((pRecFrame_p->
+					      m_le_bRecSeqNumCon &
+					      EPL_ASY_SDO_CON_MASK) == 0x01)
+					    && ((pRecFrame_p->m_le_bSendSeqNumCon & EPL_ASY_SDO_CON_MASK) == 0x02)) {	// create answer own rcon = 2
+						// save sequence numbers
+						pAsySdoSeqCon->m_bRecSeqNum =
+						    AmiGetByteFromLe
+						    (&pRecFrame_p->
+						     m_le_bRecSeqNumCon);
+						pAsySdoSeqCon->m_bSendSeqNum =
+						    AmiGetByteFromLe
+						    (&pRecFrame_p->
+						     m_le_bSendSeqNumCon);
+
+						pAsySdoSeqCon->m_bRecSeqNum++;
+						Ret =
+						    EplSdoAsySeqSendIntern
+						    (pAsySdoSeqCon, 0, NULL,
+						     FALSE);
+						if (Ret != kEplSuccessful) {
+							goto Exit;
+						}
+						// change state to kEplAsySdoStateConnected
+						pAsySdoSeqCon->m_SdoState =
+						    kEplAsySdoStateConnected;
+
+						// set timer
+						Ret =
+						    EplSdoAsySeqSetTimer
+						    (pAsySdoSeqCon,
+						     EPL_SEQ_DEFAULT_TIMEOUT);
+
+						// call Command Layer Cb
+						AsySdoSequInstance_g.
+						    m_fpSdoComConCb
+						    (SdoSeqConHdl,
+						     kAsySdoConStateConnected);
+
+					}
+					// check scon == 1 and rcon == 1, i.e. other side wants me to initiate the connection
+					else if (((pRecFrame_p->
+						   m_le_bRecSeqNumCon &
+						   EPL_ASY_SDO_CON_MASK) ==
+						  0x01)
+						 &&
+						 ((pRecFrame_p->
+						   m_le_bSendSeqNumCon &
+						   EPL_ASY_SDO_CON_MASK) ==
+						  0x01)) {
+						// save sequence numbers
+						pAsySdoSeqCon->m_bRecSeqNum =
+						    AmiGetByteFromLe
+						    (&pRecFrame_p->
+						     m_le_bRecSeqNumCon);
+						pAsySdoSeqCon->m_bSendSeqNum =
+						    AmiGetByteFromLe
+						    (&pRecFrame_p->
+						     m_le_bSendSeqNumCon);
+						// create answer and send answer
+						// set rcon to 1 (in send direction own scon)
+						pAsySdoSeqCon->m_bRecSeqNum++;
+						Ret =
+						    EplSdoAsySeqSendIntern
+						    (pAsySdoSeqCon, 0, NULL,
+						     FALSE);
+						if (Ret != kEplSuccessful) {
+							goto Exit;
+						}
+						// set timer
+						Ret =
+						    EplSdoAsySeqSetTimer
+						    (pAsySdoSeqCon,
+						     EPL_SEQ_DEFAULT_TIMEOUT);
+						// change state to kEplAsySdoStateInit3
+						pAsySdoSeqCon->m_SdoState =
+						    kEplAsySdoStateInit3;
+
+					} else {	// error -> Close
+						pAsySdoSeqCon->m_SdoState =
+						    kEplAsySdoStateIdle;
+						// delete timer
+						EplTimeruDeleteTimer
+						    (&pAsySdoSeqCon->
+						     m_EplTimerHdl);
+						if (((pRecFrame_p->
+						      m_le_bRecSeqNumCon &
+						      EPL_ASY_SDO_CON_MASK) !=
+						     0x00)
+						    || ((pRecFrame_p->m_le_bSendSeqNumCon & EPL_ASY_SDO_CON_MASK) != 0x00)) {	// d.k. only answer with close message if the message sent was not a close message
+							// save sequence numbers
+							pAsySdoSeqCon->
+							    m_bRecSeqNum =
+							    AmiGetByteFromLe
+							    (&pRecFrame_p->
+							     m_le_bRecSeqNumCon);
+							pAsySdoSeqCon->
+							    m_bSendSeqNum =
+							    AmiGetByteFromLe
+							    (&pRecFrame_p->
+							     m_le_bSendSeqNumCon);
+							// set rcon and scon to 0
+							pAsySdoSeqCon->
+							    m_bSendSeqNum &=
+							    EPL_SEQ_NUM_MASK;
+							pAsySdoSeqCon->
+							    m_bRecSeqNum &=
+							    EPL_SEQ_NUM_MASK;
+							// send frame
+							EplSdoAsySeqSendIntern
+							    (pAsySdoSeqCon, 0,
+							     NULL, FALSE);
+						}
+						// call Command Layer Cb
+						AsySdoSequInstance_g.
+						    m_fpSdoComConCb
+						    (SdoSeqConHdl,
+						     kAsySdoConStateInitError);
+					}
+					break;
+				}
+
+				// timeout
+			case kAsySdoSeqEventTimeout:
+				{	// error -> Close
+					pAsySdoSeqCon->m_SdoState =
+					    kEplAsySdoStateIdle;
+					// set rcon and scon to 0
+					pAsySdoSeqCon->m_bSendSeqNum &=
+					    EPL_SEQ_NUM_MASK;
+					pAsySdoSeqCon->m_bRecSeqNum &=
+					    EPL_SEQ_NUM_MASK;
+					// send frame
+					EplSdoAsySeqSendIntern(pAsySdoSeqCon,
+							       0, NULL, FALSE);
+
+					// call Command Layer Cb
+					AsySdoSequInstance_g.
+					    m_fpSdoComConCb(SdoSeqConHdl,
+							    kAsySdoConStateInitError);
+					break;
+				}
+
+			default:
+				// d.k. do nothing
+				break;
+
+			}	// end of switch(Event_p)
+			break;
+		}
+
+		// init connection step 3
+	case kEplAsySdoStateInit3:
+		{
+			// check event
+			switch (Event_p) {
+				// frame received
+			case kAsySdoSeqEventFrameRec:
+				{
+					// check scon == 2 and rcon == 2
+					if (((pRecFrame_p->
+					      m_le_bRecSeqNumCon &
+					      EPL_ASY_SDO_CON_MASK) == 0x02)
+					    &&
+					    ((pRecFrame_p->
+					      m_le_bSendSeqNumCon &
+					      EPL_ASY_SDO_CON_MASK) == 0x02)) {
+						// save sequence numbers
+						pAsySdoSeqCon->m_bRecSeqNum =
+						    AmiGetByteFromLe
+						    (&pRecFrame_p->
+						     m_le_bRecSeqNumCon);
+						pAsySdoSeqCon->m_bSendSeqNum =
+						    AmiGetByteFromLe
+						    (&pRecFrame_p->
+						     m_le_bSendSeqNumCon);
+						// change state to kEplAsySdoStateConnected
+						pAsySdoSeqCon->m_SdoState =
+						    kEplAsySdoStateConnected;
+
+						// set timer
+						Ret =
+						    EplSdoAsySeqSetTimer
+						    (pAsySdoSeqCon,
+						     EPL_SEQ_DEFAULT_TIMEOUT);
+						// call Command Layer Cb
+						AsySdoSequInstance_g.
+						    m_fpSdoComConCb
+						    (SdoSeqConHdl,
+						     kAsySdoConStateConnected);
+
+					}
+					// check scon == 2 and rcon == 1
+					else if (((pRecFrame_p->
+						   m_le_bRecSeqNumCon &
+						   EPL_ASY_SDO_CON_MASK) ==
+						  0x01)
+						 && ((pRecFrame_p->m_le_bSendSeqNumCon & EPL_ASY_SDO_CON_MASK) == 0x02)) {	// create answer own rcon = 2
+						// save sequence numbers
+						pAsySdoSeqCon->m_bRecSeqNum =
+						    AmiGetByteFromLe
+						    (&pRecFrame_p->
+						     m_le_bRecSeqNumCon);
+						pAsySdoSeqCon->m_bSendSeqNum =
+						    AmiGetByteFromLe
+						    (&pRecFrame_p->
+						     m_le_bSendSeqNumCon);
+
+						pAsySdoSeqCon->m_bRecSeqNum++;
+						Ret =
+						    EplSdoAsySeqSendIntern
+						    (pAsySdoSeqCon, 0, NULL,
+						     FALSE);
+						if (Ret != kEplSuccessful) {
+							goto Exit;
+						}
+						// change state to kEplAsySdoStateConnected
+						pAsySdoSeqCon->m_SdoState =
+						    kEplAsySdoStateConnected;
+
+						// set timer
+						Ret =
+						    EplSdoAsySeqSetTimer
+						    (pAsySdoSeqCon,
+						     EPL_SEQ_DEFAULT_TIMEOUT);
+
+						// call Command Layer Cb
+						AsySdoSequInstance_g.
+						    m_fpSdoComConCb
+						    (SdoSeqConHdl,
+						     kAsySdoConStateConnected);
+
+					} else {	// error -> Close
+						pAsySdoSeqCon->m_SdoState =
+						    kEplAsySdoStateIdle;
+						// delete timer
+						EplTimeruDeleteTimer
+						    (&pAsySdoSeqCon->
+						     m_EplTimerHdl);
+						if (((pRecFrame_p->
+						      m_le_bRecSeqNumCon &
+						      EPL_ASY_SDO_CON_MASK) !=
+						     0x00)
+						    || ((pRecFrame_p->m_le_bSendSeqNumCon & EPL_ASY_SDO_CON_MASK) != 0x00)) {	// d.k. only answer with close message if the message sent was not a close message
+							// save sequence numbers
+							pAsySdoSeqCon->
+							    m_bRecSeqNum =
+							    AmiGetByteFromLe
+							    (&pRecFrame_p->
+							     m_le_bRecSeqNumCon);
+							pAsySdoSeqCon->
+							    m_bSendSeqNum =
+							    AmiGetByteFromLe
+							    (&pRecFrame_p->
+							     m_le_bSendSeqNumCon);
+							// set rcon and scon to 0
+							pAsySdoSeqCon->
+							    m_bSendSeqNum &=
+							    EPL_SEQ_NUM_MASK;
+							pAsySdoSeqCon->
+							    m_bRecSeqNum &=
+							    EPL_SEQ_NUM_MASK;
+							// send frame
+							EplSdoAsySeqSendIntern
+							    (pAsySdoSeqCon, 0,
+							     NULL, FALSE);
+						}
+						// call Command Layer Cb
+						AsySdoSequInstance_g.
+						    m_fpSdoComConCb
+						    (SdoSeqConHdl,
+						     kAsySdoConStateInitError);
+					}
+					break;
+				}
+
+				// timeout
+			case kAsySdoSeqEventTimeout:
+				{	// error -> Close
+					pAsySdoSeqCon->m_SdoState =
+					    kEplAsySdoStateIdle;
+					// set rcon and scon to 0
+					pAsySdoSeqCon->m_bSendSeqNum &=
+					    EPL_SEQ_NUM_MASK;
+					pAsySdoSeqCon->m_bRecSeqNum &=
+					    EPL_SEQ_NUM_MASK;
+					// send frame
+					EplSdoAsySeqSendIntern(pAsySdoSeqCon,
+							       0, NULL, FALSE);
+
+					// call Command Layer Cb
+					AsySdoSequInstance_g.
+					    m_fpSdoComConCb(SdoSeqConHdl,
+							    kAsySdoConStateInitError);
+					break;
+				}
+
+			default:
+				// d.k. do nothing
+				break;
+
+			}	// end of switch(Event_p)
+			break;
+		}
+
+		// connection established
+	case kEplAsySdoStateConnected:
+		{
+			// check event
+			switch (Event_p) {
+
+				// frame to send
+			case kAsySdoSeqEventFrameSend:
+				{
+					// set timer
+					Ret =
+					    EplSdoAsySeqSetTimer(pAsySdoSeqCon,
+								 EPL_SEQ_DEFAULT_TIMEOUT);
+					// check if data frame or ack
+					if (pData_p == NULL) {	// send ack
+						// inc scon
+						//pAsySdoSeqCon->m_bRecSeqNum += 4;
+						Ret =
+						    EplSdoAsySeqSendIntern
+						    (pAsySdoSeqCon, 0, NULL,
+						     FALSE);
+						if (Ret != kEplSuccessful) {
+							goto Exit;
+						}
+					} else {	// send dataframe
+						// increment send sequence number
+						pAsySdoSeqCon->m_bRecSeqNum +=
+						    4;
+						Ret =
+						    EplSdoAsySeqSendIntern
+						    (pAsySdoSeqCon,
+						     uiDataSize_p, pData_p,
+						     TRUE);
+						if (Ret == kEplSdoSeqRequestAckNeeded) {	// request ack
+							// change state to wait ack
+							pAsySdoSeqCon->
+							    m_SdoState =
+							    kEplAsySdoStateWaitAck;
+							// set Ret to kEplSuccessful, because no error
+							// for higher layer
+							Ret = kEplSuccessful;
+
+						} else if (Ret !=
+							   kEplSuccessful) {
+							goto Exit;
+						} else {
+							// call Command Layer Cb
+							AsySdoSequInstance_g.
+							    m_fpSdoComConCb
+							    (SdoSeqConHdl,
+							     kAsySdoConStateFrameSended);
+						}
+					}
+					break;
+				}	// end of case kAsySdoSeqEventFrameSend
+
+				// frame received
+			case kAsySdoSeqEventFrameRec:
+				{
+					BYTE bSendSeqNumCon =
+					    AmiGetByteFromLe(&pRecFrame_p->
+							     m_le_bSendSeqNumCon);
+
+					// set timer
+					Ret =
+					    EplSdoAsySeqSetTimer(pAsySdoSeqCon,
+								 EPL_SEQ_DEFAULT_TIMEOUT);
+					// check scon
+					switch (bSendSeqNumCon &
+						EPL_ASY_SDO_CON_MASK) {
+						// close from other node
+					case 0:
+					case 1:
+						{
+							// return to idle
+							pAsySdoSeqCon->
+							    m_SdoState =
+							    kEplAsySdoStateIdle;
+							// delete timer
+							EplTimeruDeleteTimer
+							    (&pAsySdoSeqCon->
+							     m_EplTimerHdl);
+							// call Command Layer Cb
+							AsySdoSequInstance_g.
+							    m_fpSdoComConCb
+							    (SdoSeqConHdl,
+							     kAsySdoConStateConClosed);
+
+							break;
+						}
+
+						// Request Ack or Error Ack
+						// possible contain data
+					case 3:
+						// normal frame
+					case 2:
+						{
+							if ((AmiGetByteFromLe
+							     (&pRecFrame_p->
+							      m_le_bRecSeqNumCon)
+							     &
+							     EPL_ASY_SDO_CON_MASK)
+							    == 3) {
+//                                PRINTF0("EplSdoAsySequ: error response received\n");
+
+								// error response (retransmission request)
+								// resend frames from history
+
+								// read frame from history
+								Ret =
+								    EplSdoAsyReadFromHistory
+								    (pAsySdoSeqCon,
+								     &pEplFrame,
+								     &uiFrameSize,
+								     TRUE);
+								if (Ret !=
+								    kEplSuccessful)
+								{
+									goto Exit;
+								}
+
+								while ((pEplFrame != NULL)
+								       &&
+								       (uiFrameSize
+									!= 0)) {
+									// send frame
+									Ret =
+									    EplSdoAsySeqSendLowerLayer
+									    (pAsySdoSeqCon,
+									     uiFrameSize,
+									     pEplFrame);
+									if (Ret
+									    !=
+									    kEplSuccessful)
+									{
+										goto Exit;
+									}
+									// read next frame from history
+									Ret =
+									    EplSdoAsyReadFromHistory
+									    (pAsySdoSeqCon,
+									     &pEplFrame,
+									     &uiFrameSize,
+									     FALSE);
+									if (Ret
+									    !=
+									    kEplSuccessful)
+									{
+										goto Exit;
+									}
+								}	// end of while((pabFrame != NULL)
+							}	// end of if (error response)
+
+							if (((pAsySdoSeqCon->m_bSendSeqNum + 4) & EPL_SEQ_NUM_MASK) == (bSendSeqNumCon & EPL_SEQ_NUM_MASK)) {	// next frame of sequence received
+								// save send sequence number (without ack request)
+								pAsySdoSeqCon->
+								    m_bSendSeqNum
+								    =
+								    bSendSeqNumCon
+								    & ~0x01;
+
+								// check if ack or data-frame
+								//ignore ack -> already processed
+								if (uiDataSize_p
+								    >
+								    EPL_SEQ_HEADER_SIZE)
+								{
+									AsySdoSequInstance_g.
+									    m_fpSdoComReceiveCb
+									    (SdoSeqConHdl,
+									     ((tEplAsySdoCom *) & pRecFrame_p->m_le_abSdoSeqPayload), (uiDataSize_p - EPL_SEQ_HEADER_SIZE));
+									// call Command Layer Cb
+									AsySdoSequInstance_g.
+									    m_fpSdoComConCb
+									    (SdoSeqConHdl,
+									     kAsySdoConStateFrameSended);
+
+								} else {
+									// call Command Layer Cb
+									AsySdoSequInstance_g.
+									    m_fpSdoComConCb
+									    (SdoSeqConHdl,
+									     kAsySdoConStateAckReceived);
+								}
+							} else if (((bSendSeqNumCon - pAsySdoSeqCon->m_bSendSeqNum - 4) & EPL_SEQ_NUM_MASK) < EPL_SEQ_NUM_THRESHOLD) {	// frame of sequence was lost,
+								// because difference of received and old value
+								// is less then halve of the values range.
+
+								// send error frame with own rcon = 3
+								pAsySdoSeqCon->
+								    m_bSendSeqNum
+								    |= 0x03;
+								Ret =
+								    EplSdoAsySeqSendIntern
+								    (pAsySdoSeqCon,
+								     0, NULL,
+								     FALSE);
+								// restore send sequence number
+								pAsySdoSeqCon->
+								    m_bSendSeqNum
+								    =
+								    (pAsySdoSeqCon->
+								     m_bSendSeqNum
+								     &
+								     EPL_SEQ_NUM_MASK)
+								    | 0x02;
+								if (Ret !=
+								    kEplSuccessful)
+								{
+									goto Exit;
+								}
+								// break here, because a requested acknowledge
+								// was sent implicitly above
+								break;
+							}
+							// else, ignore repeated frame
+
+							if ((bSendSeqNumCon & EPL_ASY_SDO_CON_MASK) == 3) {	// ack request received
+
+								// create ack with own scon = 2
+								Ret =
+								    EplSdoAsySeqSendIntern
+								    (pAsySdoSeqCon,
+								     0, NULL,
+								     FALSE);
+								if (Ret !=
+								    kEplSuccessful)
+								{
+									goto Exit;
+								}
+							}
+
+							break;
+						}
+
+					}	// switch(pAsySdoSeqCon->m_bSendSeqNum & EPL_ASY_SDO_CON_MASK)
+					break;
+				}	// end of case kAsySdoSeqEventFrameRec:
+
+				//close event from higher layer
+			case kAsySdoSeqEventCloseCon:
+				{
+					pAsySdoSeqCon->m_SdoState =
+					    kEplAsySdoStateIdle;
+					// set rcon and scon to 0
+					pAsySdoSeqCon->m_bSendSeqNum &=
+					    EPL_SEQ_NUM_MASK;
+					pAsySdoSeqCon->m_bRecSeqNum &=
+					    EPL_SEQ_NUM_MASK;
+					// send frame
+					EplSdoAsySeqSendIntern(pAsySdoSeqCon,
+							       0, NULL, FALSE);
+
+					// delete timer
+					EplTimeruDeleteTimer(&pAsySdoSeqCon->
+							     m_EplTimerHdl);
+					// call Command Layer Cb is not necessary, because the event came from there
+//                    AsySdoSequInstance_g.m_fpSdoComConCb(SdoSeqConHdl,
+//                                                            kAsySdoConStateInitError);
+					break;
+				}
+
+				// timeout
+			case kAsySdoSeqEventTimeout:
+				{
+
+					uiFreeEntries =
+					    EplSdoAsyGetFreeEntriesFromHistory
+					    (pAsySdoSeqCon);
+					if ((uiFreeEntries <
+					     EPL_SDO_HISTORY_SIZE)
+					    && (pAsySdoSeqCon->m_uiRetryCount < EPL_SEQ_RETRY_COUNT)) {	// unacknowlegded frames in history
+						// and retry counter not exceeded
+
+						// resend data with acknowledge request
+
+						// increment retry counter
+						pAsySdoSeqCon->m_uiRetryCount++;
+
+						// set timer
+						Ret =
+						    EplSdoAsySeqSetTimer
+						    (pAsySdoSeqCon,
+						     EPL_SEQ_DEFAULT_TIMEOUT);
+
+						// read first frame from history
+						Ret =
+						    EplSdoAsyReadFromHistory
+						    (pAsySdoSeqCon, &pEplFrame,
+						     &uiFrameSize, TRUE);
+						if (Ret != kEplSuccessful) {
+							goto Exit;
+						}
+
+						if ((pEplFrame != NULL)
+						    && (uiFrameSize != 0)) {
+
+							// set ack request in scon
+							AmiSetByteToLe
+							    (&pEplFrame->m_Data.
+							     m_Asnd.m_Payload.
+							     m_SdoSequenceFrame.
+							     m_le_bSendSeqNumCon,
+							     AmiGetByteFromLe
+							     (&pEplFrame->
+							      m_Data.m_Asnd.
+							      m_Payload.
+							      m_SdoSequenceFrame.
+							      m_le_bSendSeqNumCon)
+							     | 0x03);
+
+							// send frame
+							Ret =
+							    EplSdoAsySeqSendLowerLayer
+							    (pAsySdoSeqCon,
+							     uiFrameSize,
+							     pEplFrame);
+							if (Ret !=
+							    kEplSuccessful) {
+								goto Exit;
+							}
+
+						}
+					} else {
+						// timeout, because of no traffic -> Close
+						pAsySdoSeqCon->m_SdoState =
+						    kEplAsySdoStateIdle;
+						// set rcon and scon to 0
+						pAsySdoSeqCon->m_bSendSeqNum &=
+						    EPL_SEQ_NUM_MASK;
+						pAsySdoSeqCon->m_bRecSeqNum &=
+						    EPL_SEQ_NUM_MASK;
+						// send frame
+						EplSdoAsySeqSendIntern
+						    (pAsySdoSeqCon, 0, NULL,
+						     FALSE);
+
+						// call Command Layer Cb
+						AsySdoSequInstance_g.
+						    m_fpSdoComConCb
+						    (SdoSeqConHdl,
+						     kAsySdoConStateTimeout);
+					}
+
+					break;
+				}
+
+			default:
+				// d.k. do nothing
+				break;
+
+			}	// end of switch(Event_p)
+			break;
+		}
+
+		// wait for Acknowledge (history buffer full)
+	case kEplAsySdoStateWaitAck:
+		{
+			PRINTF0("EplSdoAsySequ: StateWaitAck\n");
+
+			// set timer
+			Ret = EplSdoAsySeqSetTimer(pAsySdoSeqCon,
+						   EPL_SEQ_DEFAULT_TIMEOUT);
+
+			//TODO: retry of acknowledge
+			if (Event_p == kAsySdoSeqEventFrameRec) {
+				// check rcon
+				switch (pRecFrame_p->
+					m_le_bRecSeqNumCon &
+					EPL_ASY_SDO_CON_MASK) {
+					// close-frome other node
+				case 0:
+					{
+						// return to idle
+						pAsySdoSeqCon->m_SdoState =
+						    kEplAsySdoStateIdle;
+						// delete timer
+						EplTimeruDeleteTimer
+						    (&pAsySdoSeqCon->
+						     m_EplTimerHdl);
+						// call Command Layer Cb
+						AsySdoSequInstance_g.
+						    m_fpSdoComConCb
+						    (SdoSeqConHdl,
+						     kAsySdoConStateConClosed);
+
+						break;
+					}
+
+					// normal frame
+				case 2:
+					{
+						// should be ack
+						// -> change to state kEplAsySdoStateConnected
+						pAsySdoSeqCon->m_SdoState =
+						    kEplAsySdoStateConnected;
+						// call Command Layer Cb
+						AsySdoSequInstance_g.
+						    m_fpSdoComConCb
+						    (SdoSeqConHdl,
+						     kAsySdoConStateAckReceived);
+						// send data to higher layer if needed
+						if (uiDataSize_p >
+						    EPL_SEQ_HEADER_SIZE) {
+							AsySdoSequInstance_g.
+							    m_fpSdoComReceiveCb
+							    (SdoSeqConHdl,
+							     ((tEplAsySdoCom *)
+							      & pRecFrame_p->
+							      m_le_abSdoSeqPayload),
+							     (uiDataSize_p -
+							      EPL_SEQ_HEADER_SIZE));
+						}
+						break;
+					}
+
+					// Request Ack or Error Ack
+				case 3:
+					{
+						// -> change to state kEplAsySdoStateConnected
+						pAsySdoSeqCon->m_SdoState =
+						    kEplAsySdoStateConnected;
+
+						if (pRecFrame_p->m_le_bRecSeqNumCon == pAsySdoSeqCon->m_bRecSeqNum) {	// ack request
+							// -> send ack
+							// save sequence numbers
+							pAsySdoSeqCon->
+							    m_bRecSeqNum =
+							    AmiGetByteFromLe
+							    (&pRecFrame_p->
+							     m_le_bRecSeqNumCon);
+							pAsySdoSeqCon->
+							    m_bSendSeqNum =
+							    AmiGetByteFromLe
+							    (&pRecFrame_p->
+							     m_le_bSendSeqNumCon);
+
+							// create answer own rcon = 2
+							pAsySdoSeqCon->
+							    m_bRecSeqNum--;
+
+							// check if ack or data-frame
+							if (uiDataSize_p >
+							    EPL_SEQ_HEADER_SIZE)
+							{
+								AsySdoSequInstance_g.
+								    m_fpSdoComReceiveCb
+								    (SdoSeqConHdl,
+								     ((tEplAsySdoCom *) & pRecFrame_p->m_le_abSdoSeqPayload), (uiDataSize_p - EPL_SEQ_HEADER_SIZE));
+								// call Command Layer Cb
+								AsySdoSequInstance_g.
+								    m_fpSdoComConCb
+								    (SdoSeqConHdl,
+								     kAsySdoConStateFrameSended);
+
+							} else {
+								Ret =
+								    EplSdoAsySeqSendIntern
+								    (pAsySdoSeqCon,
+								     0, NULL,
+								     FALSE);
+								if (Ret !=
+								    kEplSuccessful)
+								{
+									goto Exit;
+								}
+							}
+
+						} else {
+							// error ack
+							// resend frames from history
+
+							// read frame from history
+							Ret =
+							    EplSdoAsyReadFromHistory
+							    (pAsySdoSeqCon,
+							     &pEplFrame,
+							     &uiFrameSize,
+							     TRUE);
+							while ((pEplFrame !=
+								NULL)
+							       && (uiFrameSize
+								   != 0)) {
+								// send frame
+								Ret =
+								    EplSdoAsySeqSendLowerLayer
+								    (pAsySdoSeqCon,
+								     uiFrameSize,
+								     pEplFrame);
+								if (Ret !=
+								    kEplSuccessful)
+								{
+									goto Exit;
+								}
+								// read next frame
+
+								// read frame from history
+								Ret =
+								    EplSdoAsyReadFromHistory
+								    (pAsySdoSeqCon,
+								     &pEplFrame,
+								     &uiFrameSize,
+								     FALSE);
+							}	// end of while((pabFrame != NULL)
+						}
+						break;
+					}
+				}	// end of switch(pRecFrame_p->m_le_bRecSeqNumCon & EPL_ASY_SDO_CON_MASK)
+
+			} else if (Event_p == kAsySdoSeqEventTimeout) {	// error -> Close
+				pAsySdoSeqCon->m_SdoState = kEplAsySdoStateIdle;
+				// set rcon and scon to 0
+				pAsySdoSeqCon->m_bSendSeqNum &=
+				    EPL_SEQ_NUM_MASK;
+				pAsySdoSeqCon->m_bRecSeqNum &= EPL_SEQ_NUM_MASK;
+				// send frame
+				EplSdoAsySeqSendIntern(pAsySdoSeqCon,
+						       0, NULL, FALSE);
+
+				// call Command Layer Cb
+				AsySdoSequInstance_g.
+				    m_fpSdoComConCb(SdoSeqConHdl,
+						    kAsySdoConStateTimeout);
+			}
+
+			break;
+		}
+
+		// unknown state
+	default:
+		{
+			EPL_DBGLVL_SDO_TRACE0
+			    ("Error: Unknown State in EplSdoAsySeqProcess\n");
+
+		}
+	}			// end of switch(pAsySdoSeqCon->m_SdoState)
+
+      Exit:
+
+#if defined(WIN32) || defined(_WIN32)
+	// leave critical section for process function
+	LeaveCriticalSection(AsySdoSequInstance_g.m_pCriticalSection);
+#endif
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoAsySeqSendIntern
+//
+// Description: intern function to create and send a frame
+//              -> if uiDataSize_p == 0 create a frame with infos from
+//                 pAsySdoSeqCon_p
+//
+//
+//
+// Parameters:  pAsySdoSeqCon_p = pointer to control structure of the connection
+//              uiDataSize_p    = size of data frame to process (can be 0)
+//                                  -> without size of sequence header and Asnd header!!!
+//              pData_p         = pointer to frame to process (can be NULL)
+//              fFrameInHistory = if TRUE frame is saved to history else not
+//
+//
+//
+// Returns:     tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static tEplKernel EplSdoAsySeqSendIntern(tEplAsySdoSeqCon * pAsySdoSeqCon_p,
+					 unsigned int uiDataSize_p,
+					 tEplFrame * pData_p,
+					 BOOL fFrameInHistory_p)
+{
+	tEplKernel Ret;
+	BYTE abFrame[EPL_SEQ_FRAME_SIZE];
+	tEplFrame *pEplFrame;
+	unsigned int uiFreeEntries;
+
+	if (pData_p == NULL) {	// set pointer to own frame
+		EPL_MEMSET(&abFrame[0], 0x00, sizeof(abFrame));
+		pEplFrame = (tEplFrame *) & abFrame[0];
+	} else {		// set pointer to frame from calling function
+		pEplFrame = pData_p;
+	}
+
+	if (fFrameInHistory_p != FALSE) {
+		// check if only one free entry in history buffer
+		uiFreeEntries =
+		    EplSdoAsyGetFreeEntriesFromHistory(pAsySdoSeqCon_p);
+		if (uiFreeEntries == 1) {	// request an acknowledge in dataframe
+			// own scon = 3
+			pAsySdoSeqCon_p->m_bRecSeqNum |= 0x03;
+		}
+	}
+	// fillin header informations
+	// set service id sdo
+	AmiSetByteToLe(&pEplFrame->m_Data.m_Asnd.m_le_bServiceId, 0x05);
+	AmiSetByteToLe(&pEplFrame->m_Data.m_Asnd.m_Payload.m_SdoSequenceFrame.
+		       m_le_abReserved, 0x00);
+	// set receive sequence number and rcon
+	AmiSetByteToLe(&pEplFrame->m_Data.m_Asnd.m_Payload.m_SdoSequenceFrame.
+		       m_le_bRecSeqNumCon, pAsySdoSeqCon_p->m_bSendSeqNum);
+	// set send sequence number and scon
+	AmiSetByteToLe(&pEplFrame->m_Data.m_Asnd.m_Payload.m_SdoSequenceFrame.
+		       m_le_bSendSeqNumCon, pAsySdoSeqCon_p->m_bRecSeqNum);
+
+	// add size
+	uiDataSize_p += EPL_SEQ_HEADER_SIZE;
+
+	// forward frame to appropriate lower layer
+	Ret = EplSdoAsySeqSendLowerLayer(pAsySdoSeqCon_p, uiDataSize_p, pEplFrame);	// pointer to frame
+
+	// check if all allright
+	if ((Ret == kEplSuccessful)
+	    && (fFrameInHistory_p != FALSE)) {
+		// set own scon to 2 if needed
+		if ((pAsySdoSeqCon_p->m_bRecSeqNum & 0x03) == 0x03) {
+			pAsySdoSeqCon_p->m_bRecSeqNum--;
+		}
+		// save frame to history
+		Ret = EplSdoAsyAddFrameToHistory(pAsySdoSeqCon_p,
+						 pEplFrame, uiDataSize_p);
+		if (Ret == kEplSdoSeqNoFreeHistory) {	// request Ack needed
+			Ret = kEplSdoSeqRequestAckNeeded;
+		}
+
+	}
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoAsySeqSendLowerLayer
+//
+// Description: intern function to send a previously created frame to lower layer
+//
+// Parameters:  pAsySdoSeqCon_p = pointer to control structure of the connection
+//              uiDataSize_p    = size of data frame to process (can be 0)
+//                                  -> without size of Asnd header!!!
+//              pData_p         = pointer to frame to process (can be NULL)
+//
+// Returns:     tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static tEplKernel EplSdoAsySeqSendLowerLayer(tEplAsySdoSeqCon * pAsySdoSeqCon_p,
+					     unsigned int uiDataSize_p,
+					     tEplFrame * pEplFrame_p)
+{
+	tEplKernel Ret;
+
+	// call send-function
+	// check handle for UDP or Asnd
+	if ((pAsySdoSeqCon_p->m_ConHandle & EPL_SDO_ASY_HANDLE_MASK) == EPL_SDO_UDP_HANDLE) {	// send over UDP
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) != 0)
+		Ret = EplSdoUdpuSendData(pAsySdoSeqCon_p->m_ConHandle, pEplFrame_p,	// pointer to frame
+					 uiDataSize_p);
+#else
+		Ret = kEplSdoSeqUnsupportedProt;
+#endif
+
+	} else if ((pAsySdoSeqCon_p->m_ConHandle & EPL_SDO_ASY_HANDLE_MASK) == EPL_SDO_ASND_HANDLE) {	// ASND
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_ASND)) != 0)
+		Ret = EplSdoAsnduSendData(pAsySdoSeqCon_p->m_ConHandle, pEplFrame_p,	// pointer to frame
+					  uiDataSize_p);
+#else
+		Ret = kEplSdoSeqUnsupportedProt;
+#endif
+	} else {		// error
+		Ret = kEplSdoSeqInvalidHdl;
+	}
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:        EplSdoAsyReceiveCb
+//
+// Description:     callback-function for received frames from lower layer
+//
+//
+//
+// Parameters:      ConHdl_p        = handle of the connection
+//                  pSdoSeqData_p   = pointer to frame
+//                  uiDataSize_p    = size of frame
+//
+//
+// Returns:         tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoAsyReceiveCb(tEplSdoConHdl ConHdl_p,
+				     tEplAsySdoSeq * pSdoSeqData_p,
+				     unsigned int uiDataSize_p)
+{
+	tEplKernel Ret;
+	unsigned int uiCount = 0;
+	unsigned int uiFreeEntry = EPL_MAX_SDO_SEQ_CON;
+	tEplAsySdoSeqCon *pAsySdoSeqCon;
+
+#if defined(WIN32) || defined(_WIN32)
+	// enter  critical section
+	EnterCriticalSection(AsySdoSequInstance_g.m_pCriticalSectionReceive);
+#endif
+
+	EPL_DBGLVL_SDO_TRACE2("Handle: 0x%x , First Databyte 0x%x\n", ConHdl_p,
+			      ((BYTE *) pSdoSeqData_p)[0]);
+
+	// search controll structure for this connection
+	pAsySdoSeqCon = &AsySdoSequInstance_g.m_AsySdoConnection[uiCount];
+	while (uiCount < EPL_MAX_SDO_SEQ_CON) {
+		if (pAsySdoSeqCon->m_ConHandle == ConHdl_p) {
+			break;
+		} else if ((pAsySdoSeqCon->m_ConHandle == 0)
+			   && (uiFreeEntry == EPL_MAX_SDO_SEQ_CON)) {
+			// free entry
+			uiFreeEntry = uiCount;
+		}
+		uiCount++;
+		pAsySdoSeqCon++;
+	}
+
+	if (uiCount == EPL_MAX_SDO_SEQ_CON) {	// new connection
+		if (uiFreeEntry == EPL_MAX_SDO_SEQ_CON) {
+			Ret = kEplSdoSeqNoFreeHandle;
+			goto Exit;
+		} else {
+			pAsySdoSeqCon =
+			    &AsySdoSequInstance_g.
+			    m_AsySdoConnection[uiFreeEntry];
+			// save handle from lower layer
+			pAsySdoSeqCon->m_ConHandle = ConHdl_p;
+			// increment use counter
+			pAsySdoSeqCon->m_uiUseCount++;
+			uiCount = uiFreeEntry;
+		}
+	}
+	// call history ack function
+	Ret = EplSdoAsyAckFrameToHistory(pAsySdoSeqCon,
+					 (AmiGetByteFromLe
+					  (&pSdoSeqData_p->
+					   m_le_bRecSeqNumCon) &
+					  EPL_SEQ_NUM_MASK));
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#if defined(WIN32) || defined(_WIN32)
+	// leave critical section
+	LeaveCriticalSection(AsySdoSequInstance_g.m_pCriticalSectionReceive);
+#endif
+
+	// call process function with pointer of frame and event kAsySdoSeqEventFrameRec
+	Ret = EplSdoAsySeqProcess(uiCount,
+				  uiDataSize_p,
+				  NULL, pSdoSeqData_p, kAsySdoSeqEventFrameRec);
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:        EplSdoAsyInitHistory
+//
+// Description:     inti function for history buffer
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:         tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static tEplKernel EplSdoAsyInitHistory(void)
+{
+	tEplKernel Ret;
+	unsigned int uiCount;
+
+	Ret = kEplSuccessful;
+	// init m_bFreeEntries in history-buffer
+	for (uiCount = 0; uiCount < EPL_MAX_SDO_SEQ_CON; uiCount++) {
+		AsySdoSequInstance_g.m_AsySdoConnection[uiCount].
+		    m_SdoConHistory.m_bFreeEntries = EPL_SDO_HISTORY_SIZE;
+	}
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:        EplSdoAsyAddFrameToHistory
+//
+// Description:     function to add a frame to the history buffer
+//
+//
+//
+// Parameters:      pAsySdoSeqCon_p = pointer to control structure of this connection
+//                  pFrame_p        = pointer to frame
+//                  uiSize_p        = size of the frame
+//                                     -> without size of the ethernet header
+//                                        and the asnd header
+//
+// Returns:         tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static tEplKernel EplSdoAsyAddFrameToHistory(tEplAsySdoSeqCon * pAsySdoSeqCon_p,
+					     tEplFrame * pFrame_p,
+					     unsigned int uiSize_p)
+{
+	tEplKernel Ret;
+	tEplAsySdoConHistory *pHistory;
+
+	Ret = kEplSuccessful;
+
+	// add frame to history buffer
+
+	// check size
+	// $$$ d.k. EPL_SEQ_HISTORY_FRAME_SIZE includes the header size, but uiSize_p does not!!!
+	if (uiSize_p > EPL_SEQ_HISTROY_FRAME_SIZE) {
+		Ret = kEplSdoSeqFrameSizeError;
+		goto Exit;
+	}
+	// save pointer to history
+	pHistory = &pAsySdoSeqCon_p->m_SdoConHistory;
+
+	// check if a free entry is available
+	if (pHistory->m_bFreeEntries > 0) {	// write message in free entry
+		EPL_MEMCPY(&
+			   ((tEplFrame *) pHistory->
+			    m_aabHistoryFrame[pHistory->m_bWrite])->
+			   m_le_bMessageType, &pFrame_p->m_le_bMessageType,
+			   uiSize_p + EPL_ASND_HEADER_SIZE);
+		// store size
+		pHistory->m_auiFrameSize[pHistory->m_bWrite] = uiSize_p;
+
+		// decremend number of free bufferentries
+		pHistory->m_bFreeEntries--;
+
+		// increment writeindex
+		pHistory->m_bWrite++;
+
+		// check if write-index run over array-boarder
+		if (pHistory->m_bWrite == EPL_SDO_HISTORY_SIZE) {
+			pHistory->m_bWrite = 0;
+		}
+
+	} else {		// no free entry
+		Ret = kEplSdoSeqNoFreeHistory;
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:        EplSdoAsyAckFrameToHistory
+//
+// Description:     function to delete acknowledged frames fron history buffer
+//
+//
+//
+// Parameters:      pAsySdoSeqCon_p = pointer to control structure of this connection
+//                  bRecSeqNumber_p = receive sequence number of the received frame
+//
+//
+// Returns:         tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static tEplKernel EplSdoAsyAckFrameToHistory(tEplAsySdoSeqCon * pAsySdoSeqCon_p,
+					     BYTE bRecSeqNumber_p)
+{
+	tEplKernel Ret;
+	tEplAsySdoConHistory *pHistory;
+	BYTE bAckIndex;
+	BYTE bCurrentSeqNum;
+
+	Ret = kEplSuccessful;
+
+	// get pointer to history buffer
+	pHistory = &pAsySdoSeqCon_p->m_SdoConHistory;
+
+	// release all acknowledged frames from history buffer
+
+	// check if there are entries in history
+	if (pHistory->m_bFreeEntries < EPL_SDO_HISTORY_SIZE) {
+		bAckIndex = pHistory->m_bAck;
+		do {
+			bCurrentSeqNum =
+			    (((tEplFrame *) pHistory->
+			      m_aabHistoryFrame[bAckIndex])->m_Data.m_Asnd.
+			     m_Payload.m_SdoSequenceFrame.
+			     m_le_bSendSeqNumCon & EPL_SEQ_NUM_MASK);
+			if (((bRecSeqNumber_p -
+			      bCurrentSeqNum) & EPL_SEQ_NUM_MASK)
+			    < EPL_SEQ_NUM_THRESHOLD) {
+				pHistory->m_auiFrameSize[bAckIndex] = 0;
+				bAckIndex++;
+				pHistory->m_bFreeEntries++;
+				if (bAckIndex == EPL_SDO_HISTORY_SIZE) {	// read index run over array-boarder
+					bAckIndex = 0;
+				}
+			} else {	// nothing to do anymore,
+				// because any further frame in history has larger sequence
+				// number than the acknowledge
+				goto Exit;
+			}
+		}
+		while ((((bRecSeqNumber_p - 1 -
+			  bCurrentSeqNum) & EPL_SEQ_NUM_MASK)
+			< EPL_SEQ_NUM_THRESHOLD)
+		       && (pHistory->m_bWrite != bAckIndex));
+
+		// store local read-index to global var
+		pHistory->m_bAck = bAckIndex;
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:        EplSdoAsyReadFromHistory
+//
+// Description:     function to one frame from history
+//
+//
+//
+// Parameters:      pAsySdoSeqCon_p = pointer to control structure of this connection
+//                  ppFrame_p       = pointer to pointer to the buffer of the stored frame
+//                  puiSize_p       = OUT: size of the frame
+//                  fInitRead       = bool which indicate a start of retransmission
+//                                      -> return last not acknowledged message if TRUE
+//
+//
+// Returns:         tEplKernel = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static tEplKernel EplSdoAsyReadFromHistory(tEplAsySdoSeqCon * pAsySdoSeqCon_p,
+					   tEplFrame ** ppFrame_p,
+					   unsigned int *puiSize_p,
+					   BOOL fInitRead_p)
+{
+	tEplKernel Ret;
+	tEplAsySdoConHistory *pHistory;
+
+	Ret = kEplSuccessful;
+
+	// read one message from History
+
+	// get pointer to history buffer
+	pHistory = &pAsySdoSeqCon_p->m_SdoConHistory;
+
+	// check if init
+	if (fInitRead_p != FALSE) {	// initialize read index to the index which shall be acknowledged next
+		pHistory->m_bRead = pHistory->m_bAck;
+	}
+	// check if entries are available for reading
+	if ((pHistory->m_bFreeEntries < EPL_SDO_HISTORY_SIZE)
+	    && (pHistory->m_bWrite != pHistory->m_bRead)) {
+//        PRINTF4("EplSdoAsyReadFromHistory(): init = %d, read = %u, write = %u, ack = %u", (int) fInitRead_p, (WORD)pHistory->m_bRead, (WORD)pHistory->m_bWrite, (WORD)pHistory->m_bAck);
+//        PRINTF2(", free entries = %u, next frame size = %u\n", (WORD)pHistory->m_bFreeEntries, pHistory->m_auiFrameSize[pHistory->m_bRead]);
+
+		// return pointer to stored frame
+		*ppFrame_p =
+		    (tEplFrame *) pHistory->m_aabHistoryFrame[pHistory->
+							      m_bRead];
+
+		// save size
+		*puiSize_p = pHistory->m_auiFrameSize[pHistory->m_bRead];
+
+		pHistory->m_bRead++;
+		if (pHistory->m_bRead == EPL_SDO_HISTORY_SIZE) {
+			pHistory->m_bRead = 0;
+		}
+
+	} else {
+//        PRINTF3("EplSdoAsyReadFromHistory(): read = %u, ack = %u, free entries = %u, no frame\n", (WORD)pHistory->m_bRead, (WORD)pHistory->m_bAck, (WORD)pHistory->m_bFreeEntries);
+
+		// no more frames to send
+		// return null pointer
+		*ppFrame_p = NULL;
+
+		*puiSize_p = 0;
+	}
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:        EplSdoAsyGetFreeEntriesFromHistory
+//
+// Description:     function returns the number of free histroy entries
+//
+//
+//
+// Parameters:      pAsySdoSeqCon_p = pointer to control structure of this connection
+//
+//
+// Returns:         unsigned int    = number of free entries
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static unsigned int EplSdoAsyGetFreeEntriesFromHistory(tEplAsySdoSeqCon *
+						       pAsySdoSeqCon_p)
+{
+	unsigned int uiFreeEntries;
+
+	uiFreeEntries =
+	    (unsigned int)pAsySdoSeqCon_p->m_SdoConHistory.m_bFreeEntries;
+
+	return uiFreeEntries;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:        EplSdoAsySeqSetTimer
+//
+// Description:     function sets or modify timer in timermosule
+//
+//
+//
+// Parameters:      pAsySdoSeqCon_p = pointer to control structure of this connection
+//                  ulTimeout       = timeout in ms
+//
+//
+// Returns:         unsigned int    = number of free entries
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static tEplKernel EplSdoAsySeqSetTimer(tEplAsySdoSeqCon * pAsySdoSeqCon_p,
+				       unsigned long ulTimeout)
+{
+	tEplKernel Ret;
+	tEplTimerArg TimerArg;
+
+	TimerArg.m_EventSink = kEplEventSinkSdoAsySeq;
+	TimerArg.m_ulArg = (unsigned long)pAsySdoSeqCon_p;
+
+	if (pAsySdoSeqCon_p->m_EplTimerHdl == 0) {	// create new timer
+		Ret = EplTimeruSetTimerMs(&pAsySdoSeqCon_p->m_EplTimerHdl,
+					  ulTimeout, TimerArg);
+	} else {		// modify exisiting timer
+		Ret = EplTimeruModifyTimerMs(&pAsySdoSeqCon_p->m_EplTimerHdl,
+					     ulTimeout, TimerArg);
+
+	}
+
+	return Ret;
+}
+
+// EOF
diff --git a/drivers/staging/epl/EplSdoComu.c b/drivers/staging/epl/EplSdoComu.c
new file mode 100644
index 00000000000..ce0eb33f4c4
--- /dev/null
+++ b/drivers/staging/epl/EplSdoComu.c
@@ -0,0 +1,3346 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for SDO Command Layer module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplSdoComu.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.14 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/26 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "user/EplSdoComu.h"
+
+#if ((((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOS)) == 0) &&\
+     (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) == 0)   )
+
+#error 'ERROR: At least SDO Server or SDO Client should be activate!'
+
+#endif
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOS)) != 0)
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) == 0) && (EPL_OBD_USE_KERNEL == FALSE)
+
+#error 'ERROR: SDO Server needs OBDu module!'
+
+#endif
+
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#ifndef EPL_MAX_SDO_COM_CON
+#define EPL_MAX_SDO_COM_CON         5
+#endif
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+// intern events
+typedef enum {
+	kEplSdoComConEventSendFirst = 0x00,	// first frame to send
+	kEplSdoComConEventRec = 0x01,	// frame received
+	kEplSdoComConEventConEstablished = 0x02,	// connection established
+	kEplSdoComConEventConClosed = 0x03,	// connection closed
+	kEplSdoComConEventAckReceived = 0x04,	// acknowledge received by lower layer
+	// -> continue sending
+	kEplSdoComConEventFrameSended = 0x05,	// lower has send a frame
+	kEplSdoComConEventInitError = 0x06,	// error duringinitialisiation
+	// of the connection
+	kEplSdoComConEventTimeout = 0x07	// timeout in lower layer
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+	    ,
+
+	kEplSdoComConEventInitCon = 0x08,	// init connection (only client)
+	kEplSdoComConEventAbort = 0x09	// abort sdo transfer (only client)
+#endif
+} tEplSdoComConEvent;
+
+typedef enum {
+	kEplSdoComSendTypeReq = 0x00,	// send a request
+	kEplSdoComSendTypeAckRes = 0x01,	// send a resonse without data
+	kEplSdoComSendTypeRes = 0x02,	// send response with data
+	kEplSdoComSendTypeAbort = 0x03	// send abort
+} tEplSdoComSendType;
+
+// state of the state maschine
+typedef enum {
+	// General State
+	kEplSdoComStateIdle = 0x00,	// idle state
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOS)) != 0)
+	// Server States
+	kEplSdoComStateServerSegmTrans = 0x01,	// send following frames
+#endif
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+	// Client States
+	kEplSdoComStateClientWaitInit = 0x10,	// wait for init connection
+	// on lower layer
+	kEplSdoComStateClientConnected = 0x11,	// connection established
+	kEplSdoComStateClientSegmTrans = 0x12	// send following frames
+#endif
+} tEplSdoComState;
+
+// control structure for transaction
+typedef struct {
+	tEplSdoSeqConHdl m_SdoSeqConHdl;	// if != 0 -> entry used
+	tEplSdoComState m_SdoComState;
+	BYTE m_bTransactionId;
+	unsigned int m_uiNodeId;	// NodeId of the target
+	// -> needed to reinit connection
+	//    after timeout
+	tEplSdoTransType m_SdoTransType;	// Auto, Expedited, Segmented
+	tEplSdoServiceType m_SdoServiceType;	// WriteByIndex, ReadByIndex
+	tEplSdoType m_SdoProtType;	// protocol layer: Auto, Udp, Asnd, Pdo
+	BYTE *m_pData;		// pointer to data
+	unsigned int m_uiTransSize;	// number of bytes
+	// to transfer
+	unsigned int m_uiTransferredByte;	// number of bytes
+	// already transferred
+	tEplSdoFinishedCb m_pfnTransferFinished;	// callback function of the
+	// application
+	// -> called in the end of
+	//    the SDO transfer
+	void *m_pUserArg;	// user definable argument pointer
+
+	DWORD m_dwLastAbortCode;	// save the last abort code
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+	// only for client
+	unsigned int m_uiTargetIndex;	// index to access
+	unsigned int m_uiTargetSubIndex;	// subiondex to access
+
+	// for future use
+	unsigned int m_uiTimeout;	// timeout for this connection
+
+#endif
+
+} tEplSdoComCon;
+
+// instance table
+typedef struct {
+	tEplSdoComCon m_SdoComCon[EPL_MAX_SDO_COM_CON];
+
+#if defined(WIN32) || defined(_WIN32)
+	LPCRITICAL_SECTION m_pCriticalSection;
+	CRITICAL_SECTION m_CriticalSection;
+#endif
+
+} tEplSdoComInstance;
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+static tEplSdoComInstance SdoComInstance_g;
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoComReceiveCb(tEplSdoSeqConHdl SdoSeqConHdl_p,
+				     tEplAsySdoCom * pAsySdoCom_p,
+				     unsigned int uiDataSize_p);
+
+tEplKernel PUBLIC EplSdoComConCb(tEplSdoSeqConHdl SdoSeqConHdl_p,
+				 tEplAsySdoConState AsySdoConState_p);
+
+static tEplKernel EplSdoComSearchConIntern(tEplSdoSeqConHdl SdoSeqConHdl_p,
+					   tEplSdoComConEvent SdoComConEvent_p,
+					   tEplAsySdoCom * pAsySdoCom_p);
+
+static tEplKernel EplSdoComProcessIntern(tEplSdoComConHdl SdoComCon_p,
+					 tEplSdoComConEvent SdoComConEvent_p,
+					 tEplAsySdoCom * pAsySdoCom_p);
+
+static tEplKernel EplSdoComTransferFinished(tEplSdoComConHdl SdoComCon_p,
+					    tEplSdoComCon * pSdoComCon_p,
+					    tEplSdoComConState
+					    SdoComConState_p);
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOS)) != 0)
+static tEplKernel EplSdoComServerInitReadByIndex(tEplSdoComCon * pSdoComCon_p,
+						 tEplAsySdoCom * pAsySdoCom_p);
+
+static tEplKernel EplSdoComServerSendFrameIntern(tEplSdoComCon * pSdoComCon_p,
+						 unsigned int uiIndex_p,
+						 unsigned int uiSubIndex_p,
+						 tEplSdoComSendType SendType_p);
+
+static tEplKernel EplSdoComServerInitWriteByIndex(tEplSdoComCon * pSdoComCon_p,
+						  tEplAsySdoCom * pAsySdoCom_p);
+#endif
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+
+static tEplKernel EplSdoComClientSend(tEplSdoComCon * pSdoComCon_p);
+
+static tEplKernel EplSdoComClientProcessFrame(tEplSdoComConHdl SdoComCon_p,
+					      tEplAsySdoCom * pAsySdoCom_p);
+
+static tEplKernel EplSdoComClientSendAbort(tEplSdoComCon * pSdoComCon_p,
+					   DWORD dwAbortCode_p);
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  <SDO Command Layer>                                 */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description: SDO Command layer Modul
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoComInit
+//
+// Description: Init first instance of the module
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoComInit(void)
+{
+	tEplKernel Ret;
+
+	Ret = EplSdoComAddInstance();
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoComAddInstance
+//
+// Description: Init additional instance of the module
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoComAddInstance(void)
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// init controll structure
+	EPL_MEMSET(&SdoComInstance_g, 0x00, sizeof(SdoComInstance_g));
+
+	// init instance of lower layer
+	Ret = EplSdoAsySeqAddInstance(EplSdoComReceiveCb, EplSdoComConCb);
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+#if defined(WIN32) || defined(_WIN32)
+	// create critical section for process function
+	SdoComInstance_g.m_pCriticalSection =
+	    &SdoComInstance_g.m_CriticalSection;
+	InitializeCriticalSection(SdoComInstance_g.m_pCriticalSection);
+#endif
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoComDelInstance
+//
+// Description: delete instance of the module
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoComDelInstance(void)
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+#if defined(WIN32) || defined(_WIN32)
+	// delete critical section for process function
+	DeleteCriticalSection(SdoComInstance_g.m_pCriticalSection);
+#endif
+
+	Ret = EplSdoAsySeqDelInstance();
+	if (Ret != kEplSuccessful) {
+		goto Exit;
+	}
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoComDefineCon
+//
+// Description: function defines a SDO connection to another node
+//              -> init lower layer and returns a handle for the connection.
+//              Two client connections to the same node via the same protocol
+//              are not allowed. If this function detects such a situation
+//              it will return kEplSdoComHandleExists and the handle of
+//              the existing connection in pSdoComConHdl_p.
+//              Using of existing server connections is possible.
+//
+// Parameters:  pSdoComConHdl_p     = pointer to the buffer of the handle
+//              uiTargetNodeId_p    = NodeId of the targetnode
+//              ProtType_p          = type of protocol to use for connection
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+tEplKernel PUBLIC EplSdoComDefineCon(tEplSdoComConHdl * pSdoComConHdl_p,
+				     unsigned int uiTargetNodeId_p,
+				     tEplSdoType ProtType_p)
+{
+	tEplKernel Ret;
+	unsigned int uiCount;
+	unsigned int uiFreeHdl;
+	tEplSdoComCon *pSdoComCon;
+
+	// check Parameter
+	ASSERT(pSdoComConHdl_p != NULL);
+
+	// check NodeId
+	if ((uiTargetNodeId_p == EPL_C_ADR_INVALID)
+	    || (uiTargetNodeId_p >= EPL_C_ADR_BROADCAST)) {
+		Ret = kEplInvalidNodeId;
+
+	}
+	// search free control structure
+	pSdoComCon = &SdoComInstance_g.m_SdoComCon[0];
+	uiCount = 0;
+	uiFreeHdl = EPL_MAX_SDO_COM_CON;
+	while (uiCount < EPL_MAX_SDO_COM_CON) {
+		if (pSdoComCon->m_SdoSeqConHdl == 0) {	// free entry
+			uiFreeHdl = uiCount;
+		} else if ((pSdoComCon->m_uiNodeId == uiTargetNodeId_p)
+			   && (pSdoComCon->m_SdoProtType == ProtType_p)) {	// existing client connection with same node ID and same protocol type
+			*pSdoComConHdl_p = uiCount;
+			Ret = kEplSdoComHandleExists;
+			goto Exit;
+		}
+		uiCount++;
+		pSdoComCon++;
+	}
+
+	if (uiFreeHdl == EPL_MAX_SDO_COM_CON) {
+		Ret = kEplSdoComNoFreeHandle;
+		goto Exit;
+	}
+
+	pSdoComCon = &SdoComInstance_g.m_SdoComCon[uiFreeHdl];
+	// save handle for application
+	*pSdoComConHdl_p = uiFreeHdl;
+	// save parameters
+	pSdoComCon->m_SdoProtType = ProtType_p;
+	pSdoComCon->m_uiNodeId = uiTargetNodeId_p;
+
+	// set Transaction Id
+	pSdoComCon->m_bTransactionId = 0;
+
+	// check protocol
+	switch (ProtType_p) {
+		// udp
+	case kEplSdoTypeUdp:
+		{
+			// call connection int function of lower layer
+			Ret = EplSdoAsySeqInitCon(&pSdoComCon->m_SdoSeqConHdl,
+						  pSdoComCon->m_uiNodeId,
+						  kEplSdoTypeUdp);
+			if (Ret != kEplSuccessful) {
+				goto Exit;
+			}
+			break;
+		}
+
+		// Asend
+	case kEplSdoTypeAsnd:
+		{
+			// call connection int function of lower layer
+			Ret = EplSdoAsySeqInitCon(&pSdoComCon->m_SdoSeqConHdl,
+						  pSdoComCon->m_uiNodeId,
+						  kEplSdoTypeAsnd);
+			if (Ret != kEplSuccessful) {
+				goto Exit;
+			}
+			break;
+		}
+
+		// Pdo -> not supported
+	case kEplSdoTypePdo:
+	default:
+		{
+			Ret = kEplSdoComUnsupportedProt;
+			goto Exit;
+		}
+	}			// end of switch(m_ProtType_p)
+
+	// call process function
+	Ret = EplSdoComProcessIntern(uiFreeHdl,
+				     kEplSdoComConEventInitCon, NULL);
+
+      Exit:
+	return Ret;
+}
+#endif
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoComInitTransferByIndex
+//
+// Description: function init SDO Transfer for a defined connection
+//
+//
+//
+// Parameters:  SdoComTransParam_p    = Structure with parameters for connection
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+tEplKernel PUBLIC EplSdoComInitTransferByIndex(tEplSdoComTransParamByIndex *
+					       pSdoComTransParam_p)
+{
+	tEplKernel Ret;
+	tEplSdoComCon *pSdoComCon;
+
+	// check parameter
+	if ((pSdoComTransParam_p->m_uiSubindex >= 0xFF)
+	    || (pSdoComTransParam_p->m_uiIndex == 0)
+	    || (pSdoComTransParam_p->m_uiIndex > 0xFFFF)
+	    || (pSdoComTransParam_p->m_pData == NULL)
+	    || (pSdoComTransParam_p->m_uiDataSize == 0)) {
+		Ret = kEplSdoComInvalidParam;
+		goto Exit;
+	}
+
+	if (pSdoComTransParam_p->m_SdoComConHdl >= EPL_MAX_SDO_COM_CON) {
+		Ret = kEplSdoComInvalidHandle;
+		goto Exit;
+	}
+	// get pointer to control structure of connection
+	pSdoComCon =
+	    &SdoComInstance_g.m_SdoComCon[pSdoComTransParam_p->m_SdoComConHdl];
+
+	// check if handle ok
+	if (pSdoComCon->m_SdoSeqConHdl == 0) {
+		Ret = kEplSdoComInvalidHandle;
+		goto Exit;
+	}
+	// check if command layer is idle
+	if ((pSdoComCon->m_uiTransferredByte + pSdoComCon->m_uiTransSize) > 0) {	// handle is not idle
+		Ret = kEplSdoComHandleBusy;
+		goto Exit;
+	}
+	// save parameter
+	// callback function for end of transfer
+	pSdoComCon->m_pfnTransferFinished =
+	    pSdoComTransParam_p->m_pfnSdoFinishedCb;
+	pSdoComCon->m_pUserArg = pSdoComTransParam_p->m_pUserArg;
+
+	// set type of SDO command
+	if (pSdoComTransParam_p->m_SdoAccessType == kEplSdoAccessTypeRead) {
+		pSdoComCon->m_SdoServiceType = kEplSdoServiceReadByIndex;
+	} else {
+		pSdoComCon->m_SdoServiceType = kEplSdoServiceWriteByIndex;
+
+	}
+	// save pointer to data
+	pSdoComCon->m_pData = pSdoComTransParam_p->m_pData;
+	// maximal bytes to transfer
+	pSdoComCon->m_uiTransSize = pSdoComTransParam_p->m_uiDataSize;
+	// bytes already transfered
+	pSdoComCon->m_uiTransferredByte = 0;
+
+	// reset parts of control structure
+	pSdoComCon->m_dwLastAbortCode = 0;
+	pSdoComCon->m_SdoTransType = kEplSdoTransAuto;
+	// save timeout
+	//pSdoComCon->m_uiTimeout = SdoComTransParam_p.m_uiTimeout;
+
+	// save index and subindex
+	pSdoComCon->m_uiTargetIndex = pSdoComTransParam_p->m_uiIndex;
+	pSdoComCon->m_uiTargetSubIndex = pSdoComTransParam_p->m_uiSubindex;
+
+	// call process function
+	Ret = EplSdoComProcessIntern(pSdoComTransParam_p->m_SdoComConHdl, kEplSdoComConEventSendFirst,	// event to start transfer
+				     NULL);
+
+      Exit:
+	return Ret;
+
+}
+#endif
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoComUndefineCon
+//
+// Description: function undefine a SDO connection
+//
+//
+//
+// Parameters:  SdoComConHdl_p    = handle for the connection
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+tEplKernel PUBLIC EplSdoComUndefineCon(tEplSdoComConHdl SdoComConHdl_p)
+{
+	tEplKernel Ret;
+	tEplSdoComCon *pSdoComCon;
+
+	Ret = kEplSuccessful;
+
+	if (SdoComConHdl_p >= EPL_MAX_SDO_COM_CON) {
+		Ret = kEplSdoComInvalidHandle;
+		goto Exit;
+	}
+	// get pointer to control structure
+	pSdoComCon = &SdoComInstance_g.m_SdoComCon[SdoComConHdl_p];
+
+	// $$$ d.k. abort a running transfer before closing the sequence layer
+
+	if (((pSdoComCon->m_SdoSeqConHdl & ~EPL_SDO_SEQ_HANDLE_MASK) !=
+	     EPL_SDO_SEQ_INVALID_HDL)
+	    && (pSdoComCon->m_SdoSeqConHdl != 0)) {
+		// close connection in lower layer
+		switch (pSdoComCon->m_SdoProtType) {
+		case kEplSdoTypeAsnd:
+		case kEplSdoTypeUdp:
+			{
+				Ret =
+				    EplSdoAsySeqDelCon(pSdoComCon->
+						       m_SdoSeqConHdl);
+				break;
+			}
+
+		case kEplSdoTypePdo:
+		case kEplSdoTypeAuto:
+		default:
+			{
+				Ret = kEplSdoComUnsupportedProt;
+				goto Exit;
+			}
+
+		}		// end of switch(pSdoComCon->m_SdoProtType)
+	}
+
+	// clean controll structure
+	EPL_MEMSET(pSdoComCon, 0x00, sizeof(tEplSdoComCon));
+      Exit:
+	return Ret;
+}
+#endif
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoComGetState
+//
+// Description: function returns the state fo the connection
+//
+//
+//
+// Parameters:  SdoComConHdl_p    = handle for the connection
+//              pSdoComFinished_p = pointer to structur for sdo state
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+tEplKernel PUBLIC EplSdoComGetState(tEplSdoComConHdl SdoComConHdl_p,
+				    tEplSdoComFinished * pSdoComFinished_p)
+{
+	tEplKernel Ret;
+	tEplSdoComCon *pSdoComCon;
+
+	Ret = kEplSuccessful;
+
+	if (SdoComConHdl_p >= EPL_MAX_SDO_COM_CON) {
+		Ret = kEplSdoComInvalidHandle;
+		goto Exit;
+	}
+	// get pointer to control structure
+	pSdoComCon = &SdoComInstance_g.m_SdoComCon[SdoComConHdl_p];
+
+	// check if handle ok
+	if (pSdoComCon->m_SdoSeqConHdl == 0) {
+		Ret = kEplSdoComInvalidHandle;
+		goto Exit;
+	}
+
+	pSdoComFinished_p->m_pUserArg = pSdoComCon->m_pUserArg;
+	pSdoComFinished_p->m_uiNodeId = pSdoComCon->m_uiNodeId;
+	pSdoComFinished_p->m_uiTargetIndex = pSdoComCon->m_uiTargetIndex;
+	pSdoComFinished_p->m_uiTargetSubIndex = pSdoComCon->m_uiTargetSubIndex;
+	pSdoComFinished_p->m_uiTransferredByte =
+	    pSdoComCon->m_uiTransferredByte;
+	pSdoComFinished_p->m_dwAbortCode = pSdoComCon->m_dwLastAbortCode;
+	pSdoComFinished_p->m_SdoComConHdl = SdoComConHdl_p;
+	if (pSdoComCon->m_SdoServiceType == kEplSdoServiceWriteByIndex) {
+		pSdoComFinished_p->m_SdoAccessType = kEplSdoAccessTypeWrite;
+	} else {
+		pSdoComFinished_p->m_SdoAccessType = kEplSdoAccessTypeRead;
+	}
+
+	if (pSdoComCon->m_dwLastAbortCode != 0) {	// sdo abort
+		pSdoComFinished_p->m_SdoComConState =
+		    kEplSdoComTransferRxAborted;
+
+		// delete abort code
+		pSdoComCon->m_dwLastAbortCode = 0;
+
+	} else if ((pSdoComCon->m_SdoSeqConHdl & ~EPL_SDO_SEQ_HANDLE_MASK) == EPL_SDO_SEQ_INVALID_HDL) {	// check state
+		pSdoComFinished_p->m_SdoComConState =
+		    kEplSdoComTransferLowerLayerAbort;
+	} else if (pSdoComCon->m_SdoComState == kEplSdoComStateClientWaitInit) {
+		// finished
+		pSdoComFinished_p->m_SdoComConState =
+		    kEplSdoComTransferNotActive;
+	} else if (pSdoComCon->m_uiTransSize == 0) {	// finished
+		pSdoComFinished_p->m_SdoComConState =
+		    kEplSdoComTransferFinished;
+	}
+
+      Exit:
+	return Ret;
+
+}
+#endif
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoComSdoAbort
+//
+// Description: function abort a sdo transfer
+//
+//
+//
+// Parameters:  SdoComConHdl_p    = handle for the connection
+//              dwAbortCode_p     = abort code
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+tEplKernel PUBLIC EplSdoComSdoAbort(tEplSdoComConHdl SdoComConHdl_p,
+				    DWORD dwAbortCode_p)
+{
+	tEplKernel Ret;
+	tEplSdoComCon *pSdoComCon;
+
+	if (SdoComConHdl_p >= EPL_MAX_SDO_COM_CON) {
+		Ret = kEplSdoComInvalidHandle;
+		goto Exit;
+	}
+	// get pointer to control structure of connection
+	pSdoComCon = &SdoComInstance_g.m_SdoComCon[SdoComConHdl_p];
+
+	// check if handle ok
+	if (pSdoComCon->m_SdoSeqConHdl == 0) {
+		Ret = kEplSdoComInvalidHandle;
+		goto Exit;
+	}
+	// save pointer to abort code
+	pSdoComCon->m_pData = (BYTE *) & dwAbortCode_p;
+
+	Ret = EplSdoComProcessIntern(SdoComConHdl_p,
+				     kEplSdoComConEventAbort,
+				     (tEplAsySdoCom *) NULL);
+
+      Exit:
+	return Ret;
+}
+#endif
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:        EplSdoComReceiveCb
+//
+// Description:     callback function for SDO Sequence Layer
+//                  -> indicates new data
+//
+//
+//
+// Parameters:      SdoSeqConHdl_p = Handle for connection
+//                  pAsySdoCom_p   = pointer to data
+//                  uiDataSize_p   = size of data ($$$ not used yet, but it should)
+//
+//
+// Returns:
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoComReceiveCb(tEplSdoSeqConHdl SdoSeqConHdl_p,
+				     tEplAsySdoCom * pAsySdoCom_p,
+				     unsigned int uiDataSize_p)
+{
+	tEplKernel Ret;
+
+	// search connection internally
+	Ret = EplSdoComSearchConIntern(SdoSeqConHdl_p,
+				       kEplSdoComConEventRec, pAsySdoCom_p);
+
+	EPL_DBGLVL_SDO_TRACE3
+	    ("EplSdoComReceiveCb SdoSeqConHdl: 0x%X, First Byte of pAsySdoCom_p: 0x%02X, uiDataSize_p: 0x%04X\n",
+	     SdoSeqConHdl_p, (WORD) pAsySdoCom_p->m_le_abCommandData[0],
+	     uiDataSize_p);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:        EplSdoComConCb
+//
+// Description:     callback function called by SDO Sequence Layer to inform
+//                  command layer about state change of connection
+//
+//
+//
+// Parameters:      SdoSeqConHdl_p      = Handle of the connection
+//                  AsySdoConState_p    = Event of the connection
+//
+//
+// Returns:         tEplKernel  = Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoComConCb(tEplSdoSeqConHdl SdoSeqConHdl_p,
+				 tEplAsySdoConState AsySdoConState_p)
+{
+	tEplKernel Ret;
+	tEplSdoComConEvent SdoComConEvent = kEplSdoComConEventSendFirst;
+
+	Ret = kEplSuccessful;
+
+	// check state
+	switch (AsySdoConState_p) {
+	case kAsySdoConStateConnected:
+		{
+			EPL_DBGLVL_SDO_TRACE0("Connection established\n");
+			SdoComConEvent = kEplSdoComConEventConEstablished;
+			// start transmission if needed
+			break;
+		}
+
+	case kAsySdoConStateInitError:
+		{
+			EPL_DBGLVL_SDO_TRACE0("Error during initialisation\n");
+			SdoComConEvent = kEplSdoComConEventInitError;
+			// inform app about error and close sequence layer handle
+			break;
+		}
+
+	case kAsySdoConStateConClosed:
+		{
+			EPL_DBGLVL_SDO_TRACE0("Connection closed\n");
+			SdoComConEvent = kEplSdoComConEventConClosed;
+			// close sequence layer handle
+			break;
+		}
+
+	case kAsySdoConStateAckReceived:
+		{
+			EPL_DBGLVL_SDO_TRACE0("Acknowlage received\n");
+			SdoComConEvent = kEplSdoComConEventAckReceived;
+			// continue transmission
+			break;
+		}
+
+	case kAsySdoConStateFrameSended:
+		{
+			EPL_DBGLVL_SDO_TRACE0("One Frame sent\n");
+			SdoComConEvent = kEplSdoComConEventFrameSended;
+			// to continue transmission
+			break;
+
+		}
+
+	case kAsySdoConStateTimeout:
+		{
+			EPL_DBGLVL_SDO_TRACE0("Timeout\n");
+			SdoComConEvent = kEplSdoComConEventTimeout;
+			// close sequence layer handle
+			break;
+
+		}
+	}			// end of switch(AsySdoConState_p)
+
+	Ret = EplSdoComSearchConIntern(SdoSeqConHdl_p,
+				       SdoComConEvent, (tEplAsySdoCom *) NULL);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:        EplSdoComSearchConIntern
+//
+// Description:     search a Sdo Sequence Layer connection handle in the
+//                  control structure of the Command Layer
+//
+// Parameters:      SdoSeqConHdl_p     = Handle to search
+//                  SdoComConEvent_p = event to process
+//                  pAsySdoCom_p     = pointer to received frame
+//
+// Returns:         tEplKernel
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static tEplKernel EplSdoComSearchConIntern(tEplSdoSeqConHdl SdoSeqConHdl_p,
+					   tEplSdoComConEvent SdoComConEvent_p,
+					   tEplAsySdoCom * pAsySdoCom_p)
+{
+	tEplKernel Ret;
+	tEplSdoComCon *pSdoComCon;
+	tEplSdoComConHdl HdlCount;
+	tEplSdoComConHdl HdlFree;
+
+	Ret = kEplSdoComNotResponsible;
+
+	// get pointer to first element of the array
+	pSdoComCon = &SdoComInstance_g.m_SdoComCon[0];
+	HdlCount = 0;
+	HdlFree = 0xFFFF;
+	while (HdlCount < EPL_MAX_SDO_COM_CON) {
+		if (pSdoComCon->m_SdoSeqConHdl == SdoSeqConHdl_p) {	// matching command layer handle found
+			Ret = EplSdoComProcessIntern(HdlCount,
+						     SdoComConEvent_p,
+						     pAsySdoCom_p);
+		} else if ((pSdoComCon->m_SdoSeqConHdl == 0)
+			   && (HdlFree == 0xFFFF)) {
+			HdlFree = HdlCount;
+		}
+
+		pSdoComCon++;
+		HdlCount++;
+	}
+
+	if (Ret == kEplSdoComNotResponsible) {	// no responsible command layer handle found
+		if (HdlFree == 0xFFFF) {	// no free handle
+			// delete connection immediately
+			// 2008/04/14 m.u./d.k. This connection actually does not exist.
+			//                      pSdoComCon is invalid.
+			// Ret = EplSdoAsySeqDelCon(pSdoComCon->m_SdoSeqConHdl);
+			Ret = kEplSdoComNoFreeHandle;
+		} else {	// create new handle
+			HdlCount = HdlFree;
+			pSdoComCon = &SdoComInstance_g.m_SdoComCon[HdlCount];
+			pSdoComCon->m_SdoSeqConHdl = SdoSeqConHdl_p;
+			Ret = EplSdoComProcessIntern(HdlCount,
+						     SdoComConEvent_p,
+						     pAsySdoCom_p);
+		}
+	}
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:        EplSdoComProcessIntern
+//
+// Description:     search a Sdo Sequence Layer connection handle in the
+//                  control structer of the Command Layer
+//
+//
+//
+// Parameters:      SdoComCon_p     = index of control structure of connection
+//                  SdoComConEvent_p = event to process
+//                  pAsySdoCom_p     = pointer to received frame
+//
+// Returns:         tEplKernel  =  errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static tEplKernel EplSdoComProcessIntern(tEplSdoComConHdl SdoComCon_p,
+					 tEplSdoComConEvent SdoComConEvent_p,
+					 tEplAsySdoCom * pAsySdoCom_p)
+{
+	tEplKernel Ret;
+	tEplSdoComCon *pSdoComCon;
+	BYTE bFlag;
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOS)) != 0)
+	DWORD dwAbortCode;
+	unsigned int uiSize;
+#endif
+
+#if defined(WIN32) || defined(_WIN32)
+	// enter  critical section for process function
+	EnterCriticalSection(SdoComInstance_g.m_pCriticalSection);
+	EPL_DBGLVL_SDO_TRACE0
+	    ("\n\tEnterCiticalSection EplSdoComProcessIntern\n\n");
+#endif
+
+	Ret = kEplSuccessful;
+
+	// get pointer to control structure
+	pSdoComCon = &SdoComInstance_g.m_SdoComCon[SdoComCon_p];
+
+	// process state maschine
+	switch (pSdoComCon->m_SdoComState) {
+		// idle state
+	case kEplSdoComStateIdle:
+		{
+			// check events
+			switch (SdoComConEvent_p) {
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+				// init con for client
+			case kEplSdoComConEventInitCon:
+				{
+
+					// call of the init function already
+					// processed in EplSdoComDefineCon()
+					// only change state to kEplSdoComStateClientWaitInit
+					pSdoComCon->m_SdoComState =
+					    kEplSdoComStateClientWaitInit;
+					break;
+				}
+#endif
+
+				// int con for server
+			case kEplSdoComConEventRec:
+				{
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOS)) != 0)
+					// check if init of an transfer and no SDO abort
+					if ((pAsySdoCom_p->m_le_bFlags & 0x80) == 0) {	// SDO request
+						if ((pAsySdoCom_p->m_le_bFlags & 0x40) == 0) {	// no SDO abort
+							// save tansaction id
+							pSdoComCon->
+							    m_bTransactionId =
+							    AmiGetByteFromLe
+							    (&pAsySdoCom_p->
+							     m_le_bTransactionId);
+							// check command
+							switch (pAsySdoCom_p->
+								m_le_bCommandId)
+							{
+							case kEplSdoServiceNIL:
+								{	// simply acknowlegde NIL command on sequence layer
+
+									Ret =
+									    EplSdoAsySeqSendData
+									    (pSdoComCon->
+									     m_SdoSeqConHdl,
+									     0,
+									     (tEplFrame
+									      *)
+									     NULL);
+
+									break;
+								}
+
+							case kEplSdoServiceReadByIndex:
+								{	// read by index
+
+									// search entry an start transfer
+									EplSdoComServerInitReadByIndex
+									    (pSdoComCon,
+									     pAsySdoCom_p);
+									// check next state
+									if (pSdoComCon->m_uiTransSize == 0) {	// ready -> stay idle
+										pSdoComCon->
+										    m_SdoComState
+										    =
+										    kEplSdoComStateIdle;
+										// reset abort code
+										pSdoComCon->
+										    m_dwLastAbortCode
+										    =
+										    0;
+									} else {	// segmented transfer
+										pSdoComCon->
+										    m_SdoComState
+										    =
+										    kEplSdoComStateServerSegmTrans;
+									}
+
+									break;
+								}
+
+							case kEplSdoServiceWriteByIndex:
+								{
+
+									// search entry an start write
+									EplSdoComServerInitWriteByIndex
+									    (pSdoComCon,
+									     pAsySdoCom_p);
+									// check next state
+									if (pSdoComCon->m_uiTransSize == 0) {	// already -> stay idle
+										pSdoComCon->
+										    m_SdoComState
+										    =
+										    kEplSdoComStateIdle;
+										// reset abort code
+										pSdoComCon->
+										    m_dwLastAbortCode
+										    =
+										    0;
+									} else {	// segmented transfer
+										pSdoComCon->
+										    m_SdoComState
+										    =
+										    kEplSdoComStateServerSegmTrans;
+									}
+
+									break;
+								}
+
+							default:
+								{
+									//  unsupported command
+									//       -> abort senden
+									dwAbortCode
+									    =
+									    EPL_SDOAC_UNKNOWN_COMMAND_SPECIFIER;
+									// send abort
+									pSdoComCon->
+									    m_pData
+									    =
+									    (BYTE
+									     *)
+									    &
+									    dwAbortCode;
+									Ret =
+									    EplSdoComServerSendFrameIntern
+									    (pSdoComCon,
+									     0,
+									     0,
+									     kEplSdoComSendTypeAbort);
+
+								}
+
+							}	// end of switch(pAsySdoCom_p->m_le_bCommandId)
+						}
+					} else {	// this command layer handle is not responsible
+						// (wrong direction or wrong transaction ID)
+						Ret = kEplSdoComNotResponsible;
+						goto Exit;
+					}
+#endif // end of #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOS)) != 0)
+
+					break;
+				}
+
+				// connection closed
+			case kEplSdoComConEventInitError:
+			case kEplSdoComConEventTimeout:
+			case kEplSdoComConEventConClosed:
+				{
+					Ret =
+					    EplSdoAsySeqDelCon(pSdoComCon->
+							       m_SdoSeqConHdl);
+					// clean control structure
+					EPL_MEMSET(pSdoComCon, 0x00,
+						   sizeof(tEplSdoComCon));
+					break;
+				}
+
+			default:
+				// d.k. do nothing
+				break;
+			}	// end of switch(SdoComConEvent_p)
+			break;
+		}
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOS)) != 0)
+		//-------------------------------------------------------------------------
+		// SDO Server part
+		// segmented transfer
+	case kEplSdoComStateServerSegmTrans:
+		{
+			// check events
+			switch (SdoComConEvent_p) {
+				// send next frame
+			case kEplSdoComConEventAckReceived:
+			case kEplSdoComConEventFrameSended:
+				{
+					// check if it is a read
+					if (pSdoComCon->m_SdoServiceType ==
+					    kEplSdoServiceReadByIndex) {
+						// send next frame
+						EplSdoComServerSendFrameIntern
+						    (pSdoComCon, 0, 0,
+						     kEplSdoComSendTypeRes);
+						// if all send -> back to idle
+						if (pSdoComCon->m_uiTransSize == 0) {	// back to idle
+							pSdoComCon->
+							    m_SdoComState =
+							    kEplSdoComStateIdle;
+							// reset abort code
+							pSdoComCon->
+							    m_dwLastAbortCode =
+							    0;
+						}
+
+					}
+					break;
+				}
+
+				// process next frame
+			case kEplSdoComConEventRec:
+				{
+					// check if the frame is a SDO response and has the right transaction ID
+					bFlag =
+					    AmiGetByteFromLe(&pAsySdoCom_p->
+							     m_le_bFlags);
+					if (((bFlag & 0x80) != 0)
+					    &&
+					    (AmiGetByteFromLe
+					     (&pAsySdoCom_p->
+					      m_le_bTransactionId) ==
+					     pSdoComCon->m_bTransactionId)) {
+						// check if it is a abort
+						if ((bFlag & 0x40) != 0) {	// SDO abort
+							// clear control structure
+							pSdoComCon->
+							    m_uiTransSize = 0;
+							pSdoComCon->
+							    m_uiTransferredByte
+							    = 0;
+							// change state
+							pSdoComCon->
+							    m_SdoComState =
+							    kEplSdoComStateIdle;
+							// reset abort code
+							pSdoComCon->
+							    m_dwLastAbortCode =
+							    0;
+							// d.k.: do not execute anything further on this command
+							break;
+						}
+						// check if it is a write
+						if (pSdoComCon->
+						    m_SdoServiceType ==
+						    kEplSdoServiceWriteByIndex)
+						{
+							// write data to OD
+							uiSize =
+							    AmiGetWordFromLe
+							    (&pAsySdoCom_p->
+							     m_le_wSegmentSize);
+							if (pSdoComCon->
+							    m_dwLastAbortCode ==
+							    0) {
+								EPL_MEMCPY
+								    (pSdoComCon->
+								     m_pData,
+								     &pAsySdoCom_p->
+								     m_le_abCommandData
+								     [0],
+								     uiSize);
+							}
+							// update counter
+							pSdoComCon->
+							    m_uiTransferredByte
+							    += uiSize;
+							pSdoComCon->
+							    m_uiTransSize -=
+							    uiSize;
+
+							// update pointer
+							if (pSdoComCon->
+							    m_dwLastAbortCode ==
+							    0) {
+								( /*(BYTE*) */
+								 pSdoComCon->
+								 m_pData) +=
+						      uiSize;
+							}
+							// check end of transfer
+							if ((pAsySdoCom_p->m_le_bFlags & 0x30) == 0x30) {	// transfer ready
+								pSdoComCon->
+								    m_uiTransSize
+								    = 0;
+
+								if (pSdoComCon->
+								    m_dwLastAbortCode
+								    == 0) {
+									// send response
+									// send next frame
+									EplSdoComServerSendFrameIntern
+									    (pSdoComCon,
+									     0,
+									     0,
+									     kEplSdoComSendTypeRes);
+									// if all send -> back to idle
+									if (pSdoComCon->m_uiTransSize == 0) {	// back to idle
+										pSdoComCon->
+										    m_SdoComState
+										    =
+										    kEplSdoComStateIdle;
+										// reset abort code
+										pSdoComCon->
+										    m_dwLastAbortCode
+										    =
+										    0;
+									}
+								} else {	// send dabort code
+									// send abort
+									pSdoComCon->
+									    m_pData
+									    =
+									    (BYTE
+									     *)
+									    &
+									    pSdoComCon->
+									    m_dwLastAbortCode;
+									Ret =
+									    EplSdoComServerSendFrameIntern
+									    (pSdoComCon,
+									     0,
+									     0,
+									     kEplSdoComSendTypeAbort);
+
+									// reset abort code
+									pSdoComCon->
+									    m_dwLastAbortCode
+									    = 0;
+
+								}
+							} else {
+								// send acknowledge without any Command layer data
+								Ret =
+								    EplSdoAsySeqSendData
+								    (pSdoComCon->
+								     m_SdoSeqConHdl,
+								     0,
+								     (tEplFrame
+								      *) NULL);
+							}
+						}
+					} else {	// this command layer handle is not responsible
+						// (wrong direction or wrong transaction ID)
+						Ret = kEplSdoComNotResponsible;
+						goto Exit;
+					}
+					break;
+				}
+
+				// connection closed
+			case kEplSdoComConEventInitError:
+			case kEplSdoComConEventTimeout:
+			case kEplSdoComConEventConClosed:
+				{
+					Ret =
+					    EplSdoAsySeqDelCon(pSdoComCon->
+							       m_SdoSeqConHdl);
+					// clean control structure
+					EPL_MEMSET(pSdoComCon, 0x00,
+						   sizeof(tEplSdoComCon));
+					break;
+				}
+
+			default:
+				// d.k. do nothing
+				break;
+			}	// end of switch(SdoComConEvent_p)
+
+			break;
+		}
+#endif // endif of #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOS)) != 0)
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+		//-------------------------------------------------------------------------
+		// SDO Client part
+		// wait for finish of establishing connection
+	case kEplSdoComStateClientWaitInit:
+		{
+
+			// if connection handle is invalid reinit connection
+			// d.k.: this will be done only on new events (i.e. InitTransfer)
+			if ((pSdoComCon->
+			     m_SdoSeqConHdl & ~EPL_SDO_SEQ_HANDLE_MASK) ==
+			    EPL_SDO_SEQ_INVALID_HDL) {
+				// check kind of connection to reinit
+				// check protocol
+				switch (pSdoComCon->m_SdoProtType) {
+					// udp
+				case kEplSdoTypeUdp:
+					{
+						// call connection int function of lower layer
+						Ret =
+						    EplSdoAsySeqInitCon
+						    (&pSdoComCon->
+						     m_SdoSeqConHdl,
+						     pSdoComCon->m_uiNodeId,
+						     kEplSdoTypeUdp);
+						if (Ret != kEplSuccessful) {
+							goto Exit;
+						}
+						break;
+					}
+
+					// Asend -> not supported
+				case kEplSdoTypeAsnd:
+					{
+						// call connection int function of lower layer
+						Ret =
+						    EplSdoAsySeqInitCon
+						    (&pSdoComCon->
+						     m_SdoSeqConHdl,
+						     pSdoComCon->m_uiNodeId,
+						     kEplSdoTypeAsnd);
+						if (Ret != kEplSuccessful) {
+							goto Exit;
+						}
+						break;
+					}
+
+					// Pdo -> not supported
+				case kEplSdoTypePdo:
+				default:
+					{
+						Ret = kEplSdoComUnsupportedProt;
+						goto Exit;
+					}
+				}	// end of switch(m_ProtType_p)
+				// d.k.: reset transaction ID, because new sequence layer connection was initialized
+				// $$$ d.k. is this really necessary?
+				//pSdoComCon->m_bTransactionId = 0;
+			}
+			// check events
+			switch (SdoComConEvent_p) {
+				// connection established
+			case kEplSdoComConEventConEstablished:
+				{
+					//send first frame if needed
+					if ((pSdoComCon->m_uiTransSize > 0)
+					    && (pSdoComCon->m_uiTargetIndex != 0)) {	// start SDO transfer
+						Ret =
+						    EplSdoComClientSend
+						    (pSdoComCon);
+						if (Ret != kEplSuccessful) {
+							goto Exit;
+						}
+						// check if segemted transfer
+						if (pSdoComCon->
+						    m_SdoTransType ==
+						    kEplSdoTransSegmented) {
+							pSdoComCon->
+							    m_SdoComState =
+							    kEplSdoComStateClientSegmTrans;
+							goto Exit;
+						}
+					}
+					// goto state kEplSdoComStateClientConnected
+					pSdoComCon->m_SdoComState =
+					    kEplSdoComStateClientConnected;
+					goto Exit;
+				}
+
+			case kEplSdoComConEventSendFirst:
+				{
+					// infos for transfer already saved by function EplSdoComInitTransferByIndex
+					break;
+				}
+
+			case kEplSdoComConEventConClosed:
+			case kEplSdoComConEventInitError:
+			case kEplSdoComConEventTimeout:
+				{
+					// close sequence layer handle
+					Ret =
+					    EplSdoAsySeqDelCon(pSdoComCon->
+							       m_SdoSeqConHdl);
+					pSdoComCon->m_SdoSeqConHdl |=
+					    EPL_SDO_SEQ_INVALID_HDL;
+					// call callback function
+					if (SdoComConEvent_p ==
+					    kEplSdoComConEventTimeout) {
+						pSdoComCon->m_dwLastAbortCode =
+						    EPL_SDOAC_TIME_OUT;
+					} else {
+						pSdoComCon->m_dwLastAbortCode =
+						    0;
+					}
+					Ret =
+					    EplSdoComTransferFinished
+					    (SdoComCon_p, pSdoComCon,
+					     kEplSdoComTransferLowerLayerAbort);
+					// d.k.: do not clean control structure
+					break;
+				}
+
+			default:
+				// d.k. do nothing
+				break;
+
+			}	// end of  switch(SdoComConEvent_p)
+			break;
+		}
+
+		// connected
+	case kEplSdoComStateClientConnected:
+		{
+			// check events
+			switch (SdoComConEvent_p) {
+				// send a frame
+			case kEplSdoComConEventSendFirst:
+			case kEplSdoComConEventAckReceived:
+			case kEplSdoComConEventFrameSended:
+				{
+					Ret = EplSdoComClientSend(pSdoComCon);
+					if (Ret != kEplSuccessful) {
+						goto Exit;
+					}
+					// check if read transfer finished
+					if ((pSdoComCon->m_uiTransSize == 0)
+					    && (pSdoComCon->
+						m_uiTransferredByte != 0)
+					    && (pSdoComCon->m_SdoServiceType ==
+						kEplSdoServiceReadByIndex)) {
+						// inc transaction id
+						pSdoComCon->m_bTransactionId++;
+						// call callback of application
+						pSdoComCon->m_dwLastAbortCode =
+						    0;
+						Ret =
+						    EplSdoComTransferFinished
+						    (SdoComCon_p, pSdoComCon,
+						     kEplSdoComTransferFinished);
+
+						goto Exit;
+					}
+					// check if segemted transfer
+					if (pSdoComCon->m_SdoTransType ==
+					    kEplSdoTransSegmented) {
+						pSdoComCon->m_SdoComState =
+						    kEplSdoComStateClientSegmTrans;
+						goto Exit;
+					}
+					break;
+				}
+
+				// frame received
+			case kEplSdoComConEventRec:
+				{
+					// check if the frame is a SDO response and has the right transaction ID
+					bFlag =
+					    AmiGetByteFromLe(&pAsySdoCom_p->
+							     m_le_bFlags);
+					if (((bFlag & 0x80) != 0)
+					    &&
+					    (AmiGetByteFromLe
+					     (&pAsySdoCom_p->
+					      m_le_bTransactionId) ==
+					     pSdoComCon->m_bTransactionId)) {
+						// check if abort or not
+						if ((bFlag & 0x40) != 0) {
+							// send acknowledge without any Command layer data
+							Ret =
+							    EplSdoAsySeqSendData
+							    (pSdoComCon->
+							     m_SdoSeqConHdl, 0,
+							     (tEplFrame *)
+							     NULL);
+							// inc transaction id
+							pSdoComCon->
+							    m_bTransactionId++;
+							// save abort code
+							pSdoComCon->
+							    m_dwLastAbortCode =
+							    AmiGetDwordFromLe
+							    (&pAsySdoCom_p->
+							     m_le_abCommandData
+							     [0]);
+							// call callback of application
+							Ret =
+							    EplSdoComTransferFinished
+							    (SdoComCon_p,
+							     pSdoComCon,
+							     kEplSdoComTransferRxAborted);
+
+							goto Exit;
+						} else {	// normal frame received
+							// check frame
+							Ret =
+							    EplSdoComClientProcessFrame
+							    (SdoComCon_p,
+							     pAsySdoCom_p);
+
+							// check if transfer ready
+							if (pSdoComCon->
+							    m_uiTransSize ==
+							    0) {
+								// send acknowledge without any Command layer data
+								Ret =
+								    EplSdoAsySeqSendData
+								    (pSdoComCon->
+								     m_SdoSeqConHdl,
+								     0,
+								     (tEplFrame
+								      *) NULL);
+								// inc transaction id
+								pSdoComCon->
+								    m_bTransactionId++;
+								// call callback of application
+								pSdoComCon->
+								    m_dwLastAbortCode
+								    = 0;
+								Ret =
+								    EplSdoComTransferFinished
+								    (SdoComCon_p,
+								     pSdoComCon,
+								     kEplSdoComTransferFinished);
+
+								goto Exit;
+							}
+
+						}
+					} else {	// this command layer handle is not responsible
+						// (wrong direction or wrong transaction ID)
+						Ret = kEplSdoComNotResponsible;
+						goto Exit;
+					}
+					break;
+				}
+
+				// connection closed event go back to kEplSdoComStateClientWaitInit
+			case kEplSdoComConEventConClosed:
+				{	// connection closed by communication partner
+					// close sequence layer handle
+					Ret =
+					    EplSdoAsySeqDelCon(pSdoComCon->
+							       m_SdoSeqConHdl);
+					// set handle to invalid and enter kEplSdoComStateClientWaitInit
+					pSdoComCon->m_SdoSeqConHdl |=
+					    EPL_SDO_SEQ_INVALID_HDL;
+					// change state
+					pSdoComCon->m_SdoComState =
+					    kEplSdoComStateClientWaitInit;
+
+					// call callback of application
+					pSdoComCon->m_dwLastAbortCode = 0;
+					Ret =
+					    EplSdoComTransferFinished
+					    (SdoComCon_p, pSdoComCon,
+					     kEplSdoComTransferLowerLayerAbort);
+
+					goto Exit;
+
+					break;
+				}
+
+				// abort to send from higher layer
+			case kEplSdoComConEventAbort:
+				{
+					EplSdoComClientSendAbort(pSdoComCon,
+								 *((DWORD *)
+								   pSdoComCon->
+								   m_pData));
+
+					// inc transaction id
+					pSdoComCon->m_bTransactionId++;
+					// call callback of application
+					pSdoComCon->m_dwLastAbortCode =
+					    *((DWORD *) pSdoComCon->m_pData);
+					Ret =
+					    EplSdoComTransferFinished
+					    (SdoComCon_p, pSdoComCon,
+					     kEplSdoComTransferTxAborted);
+
+					break;
+				}
+
+			case kEplSdoComConEventInitError:
+			case kEplSdoComConEventTimeout:
+				{
+					// close sequence layer handle
+					Ret =
+					    EplSdoAsySeqDelCon(pSdoComCon->
+							       m_SdoSeqConHdl);
+					pSdoComCon->m_SdoSeqConHdl |=
+					    EPL_SDO_SEQ_INVALID_HDL;
+					// change state
+					pSdoComCon->m_SdoComState =
+					    kEplSdoComStateClientWaitInit;
+					// call callback of application
+					pSdoComCon->m_dwLastAbortCode =
+					    EPL_SDOAC_TIME_OUT;
+					Ret =
+					    EplSdoComTransferFinished
+					    (SdoComCon_p, pSdoComCon,
+					     kEplSdoComTransferLowerLayerAbort);
+
+				}
+
+			default:
+				// d.k. do nothing
+				break;
+
+			}	// end of switch(SdoComConEvent_p)
+
+			break;
+		}
+
+		// process segmented transfer
+	case kEplSdoComStateClientSegmTrans:
+		{
+			// check events
+			switch (SdoComConEvent_p) {
+				// sned a frame
+			case kEplSdoComConEventSendFirst:
+			case kEplSdoComConEventAckReceived:
+			case kEplSdoComConEventFrameSended:
+				{
+					Ret = EplSdoComClientSend(pSdoComCon);
+					if (Ret != kEplSuccessful) {
+						goto Exit;
+					}
+					// check if read transfer finished
+					if ((pSdoComCon->m_uiTransSize == 0)
+					    && (pSdoComCon->m_SdoServiceType ==
+						kEplSdoServiceReadByIndex)) {
+						// inc transaction id
+						pSdoComCon->m_bTransactionId++;
+						// change state
+						pSdoComCon->m_SdoComState =
+						    kEplSdoComStateClientConnected;
+						// call callback of application
+						pSdoComCon->m_dwLastAbortCode =
+						    0;
+						Ret =
+						    EplSdoComTransferFinished
+						    (SdoComCon_p, pSdoComCon,
+						     kEplSdoComTransferFinished);
+
+						goto Exit;
+					}
+
+					break;
+				}
+
+				// frame received
+			case kEplSdoComConEventRec:
+				{
+					// check if the frame is a response
+					bFlag =
+					    AmiGetByteFromLe(&pAsySdoCom_p->
+							     m_le_bFlags);
+					if (((bFlag & 0x80) != 0)
+					    &&
+					    (AmiGetByteFromLe
+					     (&pAsySdoCom_p->
+					      m_le_bTransactionId) ==
+					     pSdoComCon->m_bTransactionId)) {
+						// check if abort or not
+						if ((bFlag & 0x40) != 0) {
+							// send acknowledge without any Command layer data
+							Ret =
+							    EplSdoAsySeqSendData
+							    (pSdoComCon->
+							     m_SdoSeqConHdl, 0,
+							     (tEplFrame *)
+							     NULL);
+							// inc transaction id
+							pSdoComCon->
+							    m_bTransactionId++;
+							// change state
+							pSdoComCon->
+							    m_SdoComState =
+							    kEplSdoComStateClientConnected;
+							// save abort code
+							pSdoComCon->
+							    m_dwLastAbortCode =
+							    AmiGetDwordFromLe
+							    (&pAsySdoCom_p->
+							     m_le_abCommandData
+							     [0]);
+							// call callback of application
+							Ret =
+							    EplSdoComTransferFinished
+							    (SdoComCon_p,
+							     pSdoComCon,
+							     kEplSdoComTransferRxAborted);
+
+							goto Exit;
+						} else {	// normal frame received
+							// check frame
+							Ret =
+							    EplSdoComClientProcessFrame
+							    (SdoComCon_p,
+							     pAsySdoCom_p);
+
+							// check if transfer ready
+							if (pSdoComCon->
+							    m_uiTransSize ==
+							    0) {
+								// send acknowledge without any Command layer data
+								Ret =
+								    EplSdoAsySeqSendData
+								    (pSdoComCon->
+								     m_SdoSeqConHdl,
+								     0,
+								     (tEplFrame
+								      *) NULL);
+								// inc transaction id
+								pSdoComCon->
+								    m_bTransactionId++;
+								// change state
+								pSdoComCon->
+								    m_SdoComState
+								    =
+								    kEplSdoComStateClientConnected;
+								// call callback of application
+								pSdoComCon->
+								    m_dwLastAbortCode
+								    = 0;
+								Ret =
+								    EplSdoComTransferFinished
+								    (SdoComCon_p,
+								     pSdoComCon,
+								     kEplSdoComTransferFinished);
+
+							}
+
+						}
+					}
+					break;
+				}
+
+				// connection closed event go back to kEplSdoComStateClientWaitInit
+			case kEplSdoComConEventConClosed:
+				{	// connection closed by communication partner
+					// close sequence layer handle
+					Ret =
+					    EplSdoAsySeqDelCon(pSdoComCon->
+							       m_SdoSeqConHdl);
+					// set handle to invalid and enter kEplSdoComStateClientWaitInit
+					pSdoComCon->m_SdoSeqConHdl |=
+					    EPL_SDO_SEQ_INVALID_HDL;
+					// change state
+					pSdoComCon->m_SdoComState =
+					    kEplSdoComStateClientWaitInit;
+					// inc transaction id
+					pSdoComCon->m_bTransactionId++;
+					// call callback of application
+					pSdoComCon->m_dwLastAbortCode = 0;
+					Ret =
+					    EplSdoComTransferFinished
+					    (SdoComCon_p, pSdoComCon,
+					     kEplSdoComTransferFinished);
+
+					break;
+				}
+
+				// abort to send from higher layer
+			case kEplSdoComConEventAbort:
+				{
+					EplSdoComClientSendAbort(pSdoComCon,
+								 *((DWORD *)
+								   pSdoComCon->
+								   m_pData));
+
+					// inc transaction id
+					pSdoComCon->m_bTransactionId++;
+					// change state
+					pSdoComCon->m_SdoComState =
+					    kEplSdoComStateClientConnected;
+					// call callback of application
+					pSdoComCon->m_dwLastAbortCode =
+					    *((DWORD *) pSdoComCon->m_pData);
+					Ret =
+					    EplSdoComTransferFinished
+					    (SdoComCon_p, pSdoComCon,
+					     kEplSdoComTransferTxAborted);
+
+					break;
+				}
+
+			case kEplSdoComConEventInitError:
+			case kEplSdoComConEventTimeout:
+				{
+					// close sequence layer handle
+					Ret =
+					    EplSdoAsySeqDelCon(pSdoComCon->
+							       m_SdoSeqConHdl);
+					pSdoComCon->m_SdoSeqConHdl |=
+					    EPL_SDO_SEQ_INVALID_HDL;
+					// change state
+					pSdoComCon->m_SdoComState =
+					    kEplSdoComStateClientWaitInit;
+					// call callback of application
+					pSdoComCon->m_dwLastAbortCode =
+					    EPL_SDOAC_TIME_OUT;
+					Ret =
+					    EplSdoComTransferFinished
+					    (SdoComCon_p, pSdoComCon,
+					     kEplSdoComTransferLowerLayerAbort);
+
+				}
+
+			default:
+				// d.k. do nothing
+				break;
+
+			}	// end of switch(SdoComConEvent_p)
+
+			break;
+		}
+#endif // endo of #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+
+	}			// end of switch(pSdoComCon->m_SdoComState)
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+      Exit:
+#endif
+
+#if defined(WIN32) || defined(_WIN32)
+	// leave critical section for process function
+	EPL_DBGLVL_SDO_TRACE0
+	    ("\n\tLeaveCriticalSection EplSdoComProcessIntern\n\n");
+	LeaveCriticalSection(SdoComInstance_g.m_pCriticalSection);
+
+#endif
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:        EplSdoComServerInitReadByIndex
+//
+// Description:    function start the processing of an read by index command
+//
+//
+//
+// Parameters:      pSdoComCon_p     = pointer to control structure of connection
+//                  pAsySdoCom_p     = pointer to received frame
+//
+// Returns:         tEplKernel  =  errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOS)) != 0)
+static tEplKernel EplSdoComServerInitReadByIndex(tEplSdoComCon * pSdoComCon_p,
+						 tEplAsySdoCom * pAsySdoCom_p)
+{
+	tEplKernel Ret;
+	unsigned int uiIndex;
+	unsigned int uiSubindex;
+	tEplObdSize EntrySize;
+	tEplObdAccess AccessType;
+	DWORD dwAbortCode;
+
+	dwAbortCode = 0;
+
+	// a init of a read could not be a segmented transfer
+	// -> no variable part of header
+
+	// get index and subindex
+	uiIndex = AmiGetWordFromLe(&pAsySdoCom_p->m_le_abCommandData[0]);
+	uiSubindex = AmiGetByteFromLe(&pAsySdoCom_p->m_le_abCommandData[2]);
+
+	// check accesstype of entry
+	// existens of entry
+//#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0)
+	Ret = EplObduGetAccessType(uiIndex, uiSubindex, &AccessType);
+/*#else
+    Ret = kEplObdSubindexNotExist;
+    AccessType = 0;
+#endif*/
+	if (Ret == kEplObdSubindexNotExist) {	// subentry doesn't exist
+		dwAbortCode = EPL_SDOAC_SUB_INDEX_NOT_EXIST;
+		// send abort
+		pSdoComCon_p->m_pData = (BYTE *) & dwAbortCode;
+		Ret = EplSdoComServerSendFrameIntern(pSdoComCon_p,
+						     uiIndex,
+						     uiSubindex,
+						     kEplSdoComSendTypeAbort);
+		goto Exit;
+	} else if (Ret != kEplSuccessful) {	// entry doesn't exist
+		dwAbortCode = EPL_SDOAC_OBJECT_NOT_EXIST;
+		// send abort
+		pSdoComCon_p->m_pData = (BYTE *) & dwAbortCode;
+		Ret = EplSdoComServerSendFrameIntern(pSdoComCon_p,
+						     uiIndex,
+						     uiSubindex,
+						     kEplSdoComSendTypeAbort);
+		goto Exit;
+	}
+	// compare accesstype must be read or const
+	if (((AccessType & kEplObdAccRead) == 0)
+	    && ((AccessType & kEplObdAccConst) == 0)) {
+
+		if ((AccessType & kEplObdAccWrite) != 0) {
+			// entry read a write only object
+			dwAbortCode = EPL_SDOAC_READ_TO_WRITE_ONLY_OBJ;
+		} else {
+			dwAbortCode = EPL_SDOAC_UNSUPPORTED_ACCESS;
+		}
+		// send abort
+		pSdoComCon_p->m_pData = (BYTE *) & dwAbortCode;
+		Ret = EplSdoComServerSendFrameIntern(pSdoComCon_p,
+						     uiIndex,
+						     uiSubindex,
+						     kEplSdoComSendTypeAbort);
+		goto Exit;
+	}
+	// save service
+	pSdoComCon_p->m_SdoServiceType = kEplSdoServiceReadByIndex;
+
+	// get size of object to see iof segmented or expedited transfer
+//#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0)
+	EntrySize = EplObduGetDataSize(uiIndex, uiSubindex);
+/*#else
+    EntrySize = 0;
+#endif*/
+	if (EntrySize > EPL_SDO_MAX_PAYLOAD) {	// segmented transfer
+		pSdoComCon_p->m_SdoTransType = kEplSdoTransSegmented;
+		// get pointer to object-entry data
+//#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0)
+		pSdoComCon_p->m_pData =
+		    EplObduGetObjectDataPtr(uiIndex, uiSubindex);
+//#endif
+	} else {		// expedited transfer
+		pSdoComCon_p->m_SdoTransType = kEplSdoTransExpedited;
+	}
+
+	pSdoComCon_p->m_uiTransSize = EntrySize;
+	pSdoComCon_p->m_uiTransferredByte = 0;
+
+	Ret = EplSdoComServerSendFrameIntern(pSdoComCon_p,
+					     uiIndex,
+					     uiSubindex, kEplSdoComSendTypeRes);
+	if (Ret != kEplSuccessful) {
+		// error -> abort
+		dwAbortCode = EPL_SDOAC_GENERAL_ERROR;
+		// send abort
+		pSdoComCon_p->m_pData = (BYTE *) & dwAbortCode;
+		Ret = EplSdoComServerSendFrameIntern(pSdoComCon_p,
+						     uiIndex,
+						     uiSubindex,
+						     kEplSdoComSendTypeAbort);
+		goto Exit;
+	}
+
+      Exit:
+	return Ret;
+}
+#endif
+
+//---------------------------------------------------------------------------
+//
+// Function:        EplSdoComServerSendFrameIntern();
+//
+// Description:    function creats and send a frame for server
+//
+//
+//
+// Parameters:      pSdoComCon_p     = pointer to control structure of connection
+//                  uiIndex_p        = index to send if expedited transfer else 0
+//                  uiSubIndex_p     = subindex to send if expedited transfer else 0
+//                  SendType_p       = to of frame to send
+//
+// Returns:         tEplKernel  =  errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOS)) != 0)
+static tEplKernel EplSdoComServerSendFrameIntern(tEplSdoComCon * pSdoComCon_p,
+						 unsigned int uiIndex_p,
+						 unsigned int uiSubIndex_p,
+						 tEplSdoComSendType SendType_p)
+{
+	tEplKernel Ret;
+	BYTE abFrame[EPL_MAX_SDO_FRAME_SIZE];
+	tEplFrame *pFrame;
+	tEplAsySdoCom *pCommandFrame;
+	unsigned int uiSizeOfFrame;
+	BYTE bFlag;
+
+	Ret = kEplSuccessful;
+
+	pFrame = (tEplFrame *) & abFrame[0];
+
+	EPL_MEMSET(&abFrame[0], 0x00, sizeof(abFrame));
+
+	// build generic part of frame
+	// get pointer to command layerpart of frame
+	pCommandFrame =
+	    &pFrame->m_Data.m_Asnd.m_Payload.m_SdoSequenceFrame.
+	    m_le_abSdoSeqPayload;
+	AmiSetByteToLe(&pCommandFrame->m_le_bCommandId,
+		       pSdoComCon_p->m_SdoServiceType);
+	AmiSetByteToLe(&pCommandFrame->m_le_bTransactionId,
+		       pSdoComCon_p->m_bTransactionId);
+
+	// set size to header size
+	uiSizeOfFrame = 8;
+
+	// check SendType
+	switch (SendType_p) {
+		// requestframe to send
+	case kEplSdoComSendTypeReq:
+		{
+			// nothing to do for server
+			//-> error
+			Ret = kEplSdoComInvalidSendType;
+			break;
+		}
+
+		// response without data to send
+	case kEplSdoComSendTypeAckRes:
+		{
+			// set response flag
+			AmiSetByteToLe(&pCommandFrame->m_le_bFlags, 0x80);
+
+			// send frame
+			Ret = EplSdoAsySeqSendData(pSdoComCon_p->m_SdoSeqConHdl,
+						   uiSizeOfFrame, pFrame);
+
+			break;
+		}
+
+		// responsframe to send
+	case kEplSdoComSendTypeRes:
+		{
+			// set response flag
+			bFlag = AmiGetByteFromLe(&pCommandFrame->m_le_bFlags);
+			bFlag |= 0x80;
+			AmiSetByteToLe(&pCommandFrame->m_le_bFlags, bFlag);
+
+			// check type of resonse
+			if (pSdoComCon_p->m_SdoTransType == kEplSdoTransExpedited) {	// Expedited transfer
+				// copy data in frame
+//#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0)
+				Ret = EplObduReadEntryToLe(uiIndex_p,
+							   uiSubIndex_p,
+							   &pCommandFrame->
+							   m_le_abCommandData
+							   [0],
+							   (tEplObdSize *) &
+							   pSdoComCon_p->
+							   m_uiTransSize);
+				if (Ret != kEplSuccessful) {
+					goto Exit;
+				}
+//#endif
+
+				// set size of frame
+				AmiSetWordToLe(&pCommandFrame->
+					       m_le_wSegmentSize,
+					       (WORD) pSdoComCon_p->
+					       m_uiTransSize);
+
+				// correct byte-counter
+				uiSizeOfFrame += pSdoComCon_p->m_uiTransSize;
+				pSdoComCon_p->m_uiTransferredByte +=
+				    pSdoComCon_p->m_uiTransSize;
+				pSdoComCon_p->m_uiTransSize = 0;
+
+				// send frame
+				uiSizeOfFrame += pSdoComCon_p->m_uiTransSize;
+				Ret =
+				    EplSdoAsySeqSendData(pSdoComCon_p->
+							 m_SdoSeqConHdl,
+							 uiSizeOfFrame, pFrame);
+			} else if (pSdoComCon_p->m_SdoTransType == kEplSdoTransSegmented) {	// segmented transfer
+				// distinguish between init, segment and complete
+				if (pSdoComCon_p->m_uiTransferredByte == 0) {	// init
+					// set init flag
+					bFlag =
+					    AmiGetByteFromLe(&pCommandFrame->
+							     m_le_bFlags);
+					bFlag |= 0x10;
+					AmiSetByteToLe(&pCommandFrame->
+						       m_le_bFlags, bFlag);
+					// init variable header
+					AmiSetDwordToLe(&pCommandFrame->
+							m_le_abCommandData[0],
+							pSdoComCon_p->
+							m_uiTransSize);
+					// copy data in frame
+					EPL_MEMCPY(&pCommandFrame->
+						   m_le_abCommandData[4],
+						   pSdoComCon_p->m_pData,
+						   (EPL_SDO_MAX_PAYLOAD - 4));
+
+					// correct byte-counter
+					pSdoComCon_p->m_uiTransSize -=
+					    (EPL_SDO_MAX_PAYLOAD - 4);
+					pSdoComCon_p->m_uiTransferredByte +=
+					    (EPL_SDO_MAX_PAYLOAD - 4);
+					// move data pointer
+					pSdoComCon_p->m_pData +=
+					    (EPL_SDO_MAX_PAYLOAD - 4);
+
+					// set segment size
+					AmiSetWordToLe(&pCommandFrame->
+						       m_le_wSegmentSize,
+						       (EPL_SDO_MAX_PAYLOAD -
+							4));
+
+					// send frame
+					uiSizeOfFrame += EPL_SDO_MAX_PAYLOAD;
+					Ret =
+					    EplSdoAsySeqSendData(pSdoComCon_p->
+								 m_SdoSeqConHdl,
+								 uiSizeOfFrame,
+								 pFrame);
+
+				} else
+				    if ((pSdoComCon_p->m_uiTransferredByte > 0)
+					&& (pSdoComCon_p->m_uiTransSize > EPL_SDO_MAX_PAYLOAD)) {	// segment
+					// set segment flag
+					bFlag =
+					    AmiGetByteFromLe(&pCommandFrame->
+							     m_le_bFlags);
+					bFlag |= 0x20;
+					AmiSetByteToLe(&pCommandFrame->
+						       m_le_bFlags, bFlag);
+
+					// copy data in frame
+					EPL_MEMCPY(&pCommandFrame->
+						   m_le_abCommandData[0],
+						   pSdoComCon_p->m_pData,
+						   EPL_SDO_MAX_PAYLOAD);
+
+					// correct byte-counter
+					pSdoComCon_p->m_uiTransSize -=
+					    EPL_SDO_MAX_PAYLOAD;
+					pSdoComCon_p->m_uiTransferredByte +=
+					    EPL_SDO_MAX_PAYLOAD;
+					// move data pointer
+					pSdoComCon_p->m_pData +=
+					    EPL_SDO_MAX_PAYLOAD;
+
+					// set segment size
+					AmiSetWordToLe(&pCommandFrame->
+						       m_le_wSegmentSize,
+						       EPL_SDO_MAX_PAYLOAD);
+
+					// send frame
+					uiSizeOfFrame += EPL_SDO_MAX_PAYLOAD;
+					Ret =
+					    EplSdoAsySeqSendData(pSdoComCon_p->
+								 m_SdoSeqConHdl,
+								 uiSizeOfFrame,
+								 pFrame);
+				} else {
+					if ((pSdoComCon_p->m_uiTransSize == 0)
+					    && (pSdoComCon_p->
+						m_SdoServiceType !=
+						kEplSdoServiceWriteByIndex)) {
+						goto Exit;
+					}
+					// complete
+					// set segment complete flag
+					bFlag =
+					    AmiGetByteFromLe(&pCommandFrame->
+							     m_le_bFlags);
+					bFlag |= 0x30;
+					AmiSetByteToLe(&pCommandFrame->
+						       m_le_bFlags, bFlag);
+
+					// copy data in frame
+					EPL_MEMCPY(&pCommandFrame->
+						   m_le_abCommandData[0],
+						   pSdoComCon_p->m_pData,
+						   pSdoComCon_p->m_uiTransSize);
+
+					// correct byte-counter
+					pSdoComCon_p->m_uiTransferredByte +=
+					    pSdoComCon_p->m_uiTransSize;
+
+					// move data pointer
+					pSdoComCon_p->m_pData +=
+					    pSdoComCon_p->m_uiTransSize;
+
+					// set segment size
+					AmiSetWordToLe(&pCommandFrame->
+						       m_le_wSegmentSize,
+						       (WORD) pSdoComCon_p->
+						       m_uiTransSize);
+
+					// send frame
+					uiSizeOfFrame +=
+					    pSdoComCon_p->m_uiTransSize;
+					pSdoComCon_p->m_uiTransSize = 0;
+					Ret =
+					    EplSdoAsySeqSendData(pSdoComCon_p->
+								 m_SdoSeqConHdl,
+								 uiSizeOfFrame,
+								 pFrame);
+				}
+
+			}
+			break;
+		}
+		// abort to send
+	case kEplSdoComSendTypeAbort:
+		{
+			// set response and abort flag
+			bFlag = AmiGetByteFromLe(&pCommandFrame->m_le_bFlags);
+			bFlag |= 0xC0;
+			AmiSetByteToLe(&pCommandFrame->m_le_bFlags, bFlag);
+
+			// copy abortcode to frame
+			AmiSetDwordToLe(&pCommandFrame->m_le_abCommandData[0],
+					*((DWORD *) pSdoComCon_p->m_pData));
+
+			// set size of segment
+			AmiSetWordToLe(&pCommandFrame->m_le_wSegmentSize,
+				       sizeof(DWORD));
+
+			// update counter
+			pSdoComCon_p->m_uiTransferredByte = sizeof(DWORD);
+			pSdoComCon_p->m_uiTransSize = 0;
+
+			// calc framesize
+			uiSizeOfFrame += sizeof(DWORD);
+			Ret = EplSdoAsySeqSendData(pSdoComCon_p->m_SdoSeqConHdl,
+						   uiSizeOfFrame, pFrame);
+			break;
+		}
+	}			// end of switch(SendType_p)
+
+      Exit:
+	return Ret;
+}
+#endif
+//---------------------------------------------------------------------------
+//
+// Function:        EplSdoComServerInitWriteByIndex
+//
+// Description:    function start the processing of an write by index command
+//
+//
+//
+// Parameters:      pSdoComCon_p     = pointer to control structure of connection
+//                  pAsySdoCom_p     = pointer to received frame
+//
+// Returns:         tEplKernel  =  errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOS)) != 0)
+static tEplKernel EplSdoComServerInitWriteByIndex(tEplSdoComCon * pSdoComCon_p,
+						  tEplAsySdoCom * pAsySdoCom_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	unsigned int uiIndex;
+	unsigned int uiSubindex;
+	unsigned int uiBytesToTransfer;
+	tEplObdSize EntrySize;
+	tEplObdAccess AccessType;
+	DWORD dwAbortCode;
+	BYTE *pbSrcData;
+
+	dwAbortCode = 0;
+
+	// a init of a write
+	// -> variable part of header possible
+
+	// check if expedited or segmented transfer
+	if ((pAsySdoCom_p->m_le_bFlags & 0x30) == 0x10) {	// initiate segmented transfer
+		pSdoComCon_p->m_SdoTransType = kEplSdoTransSegmented;
+		// get index and subindex
+		uiIndex =
+		    AmiGetWordFromLe(&pAsySdoCom_p->m_le_abCommandData[4]);
+		uiSubindex =
+		    AmiGetByteFromLe(&pAsySdoCom_p->m_le_abCommandData[6]);
+		// get source-pointer for copy
+		pbSrcData = &pAsySdoCom_p->m_le_abCommandData[8];
+		// save size
+		pSdoComCon_p->m_uiTransSize =
+		    AmiGetDwordFromLe(&pAsySdoCom_p->m_le_abCommandData[0]);
+
+	} else if ((pAsySdoCom_p->m_le_bFlags & 0x30) == 0x00) {	// expedited transfer
+		pSdoComCon_p->m_SdoTransType = kEplSdoTransExpedited;
+		// get index and subindex
+		uiIndex =
+		    AmiGetWordFromLe(&pAsySdoCom_p->m_le_abCommandData[0]);
+		uiSubindex =
+		    AmiGetByteFromLe(&pAsySdoCom_p->m_le_abCommandData[2]);
+		// get source-pointer for copy
+		pbSrcData = &pAsySdoCom_p->m_le_abCommandData[4];
+		// save size
+		pSdoComCon_p->m_uiTransSize =
+		    AmiGetWordFromLe(&pAsySdoCom_p->m_le_wSegmentSize);
+		// subtract header
+		pSdoComCon_p->m_uiTransSize -= 4;
+
+	} else {
+		// just ignore any other transfer type
+		goto Exit;
+	}
+
+	// check accesstype of entry
+	// existens of entry
+//#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0)
+	Ret = EplObduGetAccessType(uiIndex, uiSubindex, &AccessType);
+/*#else
+    Ret = kEplObdSubindexNotExist;
+    AccessType = 0;
+#endif*/
+	if (Ret == kEplObdSubindexNotExist) {	// subentry doesn't exist
+		pSdoComCon_p->m_dwLastAbortCode = EPL_SDOAC_SUB_INDEX_NOT_EXIST;
+		// send abort
+		// d.k. This is wrong: k.t. not needed send abort on end of write
+		/*pSdoComCon_p->m_pData = (BYTE*)pSdoComCon_p->m_dwLastAbortCode;
+		   Ret = EplSdoComServerSendFrameIntern(pSdoComCon_p,
+		   uiIndex,
+		   uiSubindex,
+		   kEplSdoComSendTypeAbort); */
+		goto Abort;
+	} else if (Ret != kEplSuccessful) {	// entry doesn't exist
+		pSdoComCon_p->m_dwLastAbortCode = EPL_SDOAC_OBJECT_NOT_EXIST;
+		// send abort
+		// d.k. This is wrong: k.t. not needed send abort on end of write
+		/*
+		   pSdoComCon_p->m_pData = (BYTE*)&dwAbortCode;
+		   Ret = EplSdoComServerSendFrameIntern(pSdoComCon_p,
+		   uiIndex,
+		   uiSubindex,
+		   kEplSdoComSendTypeAbort); */
+		goto Abort;
+	}
+	// compare accesstype must be read
+	if ((AccessType & kEplObdAccWrite) == 0) {
+
+		if ((AccessType & kEplObdAccRead) != 0) {
+			// entry write a read only object
+			pSdoComCon_p->m_dwLastAbortCode =
+			    EPL_SDOAC_WRITE_TO_READ_ONLY_OBJ;
+		} else {
+			pSdoComCon_p->m_dwLastAbortCode =
+			    EPL_SDOAC_UNSUPPORTED_ACCESS;
+		}
+		// send abort
+		// d.k. This is wrong: k.t. not needed send abort on end of write
+		/*pSdoComCon_p->m_pData = (BYTE*)&dwAbortCode;
+		   Ret = EplSdoComServerSendFrameIntern(pSdoComCon_p,
+		   uiIndex,
+		   uiSubindex,
+		   kEplSdoComSendTypeAbort); */
+		goto Abort;
+	}
+	// save service
+	pSdoComCon_p->m_SdoServiceType = kEplSdoServiceWriteByIndex;
+
+	pSdoComCon_p->m_uiTransferredByte = 0;
+
+	// write data to OD
+	if (pSdoComCon_p->m_SdoTransType == kEplSdoTransExpedited) {	// expedited transfer
+		// size checking is done by EplObduWriteEntryFromLe()
+
+//#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0)
+		Ret = EplObduWriteEntryFromLe(uiIndex,
+					      uiSubindex,
+					      pbSrcData,
+					      pSdoComCon_p->m_uiTransSize);
+		switch (Ret) {
+		case kEplSuccessful:
+			{
+				break;
+			}
+
+		case kEplObdAccessViolation:
+			{
+				pSdoComCon_p->m_dwLastAbortCode =
+				    EPL_SDOAC_UNSUPPORTED_ACCESS;
+				// send abort
+				goto Abort;
+			}
+
+		case kEplObdValueLengthError:
+			{
+				pSdoComCon_p->m_dwLastAbortCode =
+				    EPL_SDOAC_DATA_TYPE_LENGTH_NOT_MATCH;
+				// send abort
+				goto Abort;
+			}
+
+		case kEplObdValueTooHigh:
+			{
+				pSdoComCon_p->m_dwLastAbortCode =
+				    EPL_SDOAC_VALUE_RANGE_TOO_HIGH;
+				// send abort
+				goto Abort;
+			}
+
+		case kEplObdValueTooLow:
+			{
+				pSdoComCon_p->m_dwLastAbortCode =
+				    EPL_SDOAC_VALUE_RANGE_TOO_LOW;
+				// send abort
+				goto Abort;
+			}
+
+		default:
+			{
+				pSdoComCon_p->m_dwLastAbortCode =
+				    EPL_SDOAC_GENERAL_ERROR;
+				// send abort
+				goto Abort;
+			}
+		}
+//#endif
+		// send command acknowledge
+		Ret = EplSdoComServerSendFrameIntern(pSdoComCon_p,
+						     0,
+						     0,
+						     kEplSdoComSendTypeAckRes);
+
+		pSdoComCon_p->m_uiTransSize = 0;
+		goto Exit;
+	} else {
+		// get size of the object to check if it fits
+		// because we directly write to the destination memory
+		// d.k. no one calls the user OD callback function
+
+		//#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0)
+		EntrySize = EplObduGetDataSize(uiIndex, uiSubindex);
+		/*#else
+		   EntrySize = 0;
+		   #endif */
+		if (EntrySize < pSdoComCon_p->m_uiTransSize) {	// parameter too big
+			pSdoComCon_p->m_dwLastAbortCode =
+			    EPL_SDOAC_DATA_TYPE_LENGTH_TOO_HIGH;
+			// send abort
+			// d.k. This is wrong: k.t. not needed send abort on end of write
+			/*pSdoComCon_p->m_pData = (BYTE*)&dwAbortCode;
+			   Ret = EplSdoComServerSendFrameIntern(pSdoComCon_p,
+			   uiIndex,
+			   uiSubindex,
+			   kEplSdoComSendTypeAbort); */
+			goto Abort;
+		}
+
+		uiBytesToTransfer =
+		    AmiGetWordFromLe(&pAsySdoCom_p->m_le_wSegmentSize);
+		// eleminate header (Command header (8) + variable part (4) + Command header (4))
+		uiBytesToTransfer -= 16;
+		// get pointer to object entry
+//#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0)
+		pSdoComCon_p->m_pData = EplObduGetObjectDataPtr(uiIndex,
+								uiSubindex);
+//#endif
+		if (pSdoComCon_p->m_pData == NULL) {
+			pSdoComCon_p->m_dwLastAbortCode =
+			    EPL_SDOAC_GENERAL_ERROR;
+			// send abort
+			// d.k. This is wrong: k.t. not needed send abort on end of write
+/*            pSdoComCon_p->m_pData = (BYTE*)&pSdoComCon_p->m_dwLastAbortCode;
+            Ret = EplSdoComServerSendFrameIntern(pSdoComCon_p,
+                                        uiIndex,
+                                        uiSubindex,
+                                        kEplSdoComSendTypeAbort);*/
+			goto Abort;
+		}
+		// copy data
+		EPL_MEMCPY(pSdoComCon_p->m_pData, pbSrcData, uiBytesToTransfer);
+
+		// update internal counter
+		pSdoComCon_p->m_uiTransferredByte = uiBytesToTransfer;
+		pSdoComCon_p->m_uiTransSize -= uiBytesToTransfer;
+
+		// update target pointer
+		( /*(BYTE*) */ pSdoComCon_p->m_pData) += uiBytesToTransfer;
+
+		// send acknowledge without any Command layer data
+		Ret = EplSdoAsySeqSendData(pSdoComCon_p->m_SdoSeqConHdl,
+					   0, (tEplFrame *) NULL);
+		goto Exit;
+	}
+
+      Abort:
+	if (pSdoComCon_p->m_dwLastAbortCode != 0) {
+		// send abort
+		pSdoComCon_p->m_pData =
+		    (BYTE *) & pSdoComCon_p->m_dwLastAbortCode;
+		Ret =
+		    EplSdoComServerSendFrameIntern(pSdoComCon_p, uiIndex,
+						   uiSubindex,
+						   kEplSdoComSendTypeAbort);
+
+		// reset abort code
+		pSdoComCon_p->m_dwLastAbortCode = 0;
+		pSdoComCon_p->m_uiTransSize = 0;
+		goto Exit;
+	}
+
+      Exit:
+	return Ret;
+}
+#endif
+
+//---------------------------------------------------------------------------
+//
+// Function:        EplSdoComClientSend
+//
+// Description:    function starts an sdo transfer an send all further frames
+//
+//
+//
+// Parameters:      pSdoComCon_p     = pointer to control structure of connection
+//
+// Returns:         tEplKernel  =  errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+static tEplKernel EplSdoComClientSend(tEplSdoComCon * pSdoComCon_p)
+{
+	tEplKernel Ret;
+	BYTE abFrame[EPL_MAX_SDO_FRAME_SIZE];
+	tEplFrame *pFrame;
+	tEplAsySdoCom *pCommandFrame;
+	unsigned int uiSizeOfFrame;
+	BYTE bFlags;
+	BYTE *pbPayload;
+
+	Ret = kEplSuccessful;
+
+	pFrame = (tEplFrame *) & abFrame[0];
+
+	EPL_MEMSET(&abFrame[0], 0x00, sizeof(abFrame));
+
+	// build generic part of frame
+	// get pointer to command layerpart of frame
+	pCommandFrame =
+	    &pFrame->m_Data.m_Asnd.m_Payload.m_SdoSequenceFrame.
+	    m_le_abSdoSeqPayload;
+	AmiSetByteToLe(&pCommandFrame->m_le_bCommandId,
+		       pSdoComCon_p->m_SdoServiceType);
+	AmiSetByteToLe(&pCommandFrame->m_le_bTransactionId,
+		       pSdoComCon_p->m_bTransactionId);
+
+	// set size constant part of header
+	uiSizeOfFrame = 8;
+
+	// check if first frame to send -> command header needed
+	if (pSdoComCon_p->m_uiTransSize > 0) {
+		if (pSdoComCon_p->m_uiTransferredByte == 0) {	// start SDO transfer
+			// check if segmented or expedited transfer
+			// only for write commands
+			switch (pSdoComCon_p->m_SdoServiceType) {
+			case kEplSdoServiceReadByIndex:
+				{	// first frame of read access always expedited
+					pSdoComCon_p->m_SdoTransType =
+					    kEplSdoTransExpedited;
+					pbPayload =
+					    &pCommandFrame->
+					    m_le_abCommandData[0];
+					// fill rest of header
+					AmiSetWordToLe(&pCommandFrame->
+						       m_le_wSegmentSize, 4);
+
+					// create command header
+					AmiSetWordToLe(pbPayload,
+						       (WORD) pSdoComCon_p->
+						       m_uiTargetIndex);
+					pbPayload += 2;
+					AmiSetByteToLe(pbPayload,
+						       (BYTE) pSdoComCon_p->
+						       m_uiTargetSubIndex);
+					// calc size
+					uiSizeOfFrame += 4;
+
+					// set pSdoComCon_p->m_uiTransferredByte to one
+					pSdoComCon_p->m_uiTransferredByte = 1;
+					break;
+				}
+
+			case kEplSdoServiceWriteByIndex:
+				{
+					if (pSdoComCon_p->m_uiTransSize > EPL_SDO_MAX_PAYLOAD) {	// segmented transfer
+						// -> variable part of header needed
+						// save that transfer is segmented
+						pSdoComCon_p->m_SdoTransType =
+						    kEplSdoTransSegmented;
+						// fill variable part of header
+						AmiSetDwordToLe(&pCommandFrame->
+								m_le_abCommandData
+								[0],
+								pSdoComCon_p->
+								m_uiTransSize);
+						// set pointer to real payload
+						pbPayload =
+						    &pCommandFrame->
+						    m_le_abCommandData[4];
+						// fill rest of header
+						AmiSetWordToLe(&pCommandFrame->
+							       m_le_wSegmentSize,
+							       EPL_SDO_MAX_PAYLOAD);
+						bFlags = 0x10;
+						AmiSetByteToLe(&pCommandFrame->
+							       m_le_bFlags,
+							       bFlags);
+						// create command header
+						AmiSetWordToLe(pbPayload,
+							       (WORD)
+							       pSdoComCon_p->
+							       m_uiTargetIndex);
+						pbPayload += 2;
+						AmiSetByteToLe(pbPayload,
+							       (BYTE)
+							       pSdoComCon_p->
+							       m_uiTargetSubIndex);
+						// on byte for reserved
+						pbPayload += 2;
+						// calc size
+						uiSizeOfFrame +=
+						    EPL_SDO_MAX_PAYLOAD;
+
+						// copy payload
+						EPL_MEMCPY(pbPayload,
+							   pSdoComCon_p->
+							   m_pData,
+							   (EPL_SDO_MAX_PAYLOAD
+							    - 8));
+						pSdoComCon_p->m_pData +=
+						    (EPL_SDO_MAX_PAYLOAD - 8);
+						// correct intern counter
+						pSdoComCon_p->m_uiTransSize -=
+						    (EPL_SDO_MAX_PAYLOAD - 8);
+						pSdoComCon_p->
+						    m_uiTransferredByte =
+						    (EPL_SDO_MAX_PAYLOAD - 8);
+
+					} else {	// expedited trandsfer
+						// save that transfer is expedited
+						pSdoComCon_p->m_SdoTransType =
+						    kEplSdoTransExpedited;
+						pbPayload =
+						    &pCommandFrame->
+						    m_le_abCommandData[0];
+
+						// create command header
+						AmiSetWordToLe(pbPayload,
+							       (WORD)
+							       pSdoComCon_p->
+							       m_uiTargetIndex);
+						pbPayload += 2;
+						AmiSetByteToLe(pbPayload,
+							       (BYTE)
+							       pSdoComCon_p->
+							       m_uiTargetSubIndex);
+						// + 2 -> one byte for subindex and one byte reserved
+						pbPayload += 2;
+						// copy data
+						EPL_MEMCPY(pbPayload,
+							   pSdoComCon_p->
+							   m_pData,
+							   pSdoComCon_p->
+							   m_uiTransSize);
+						// calc size
+						uiSizeOfFrame +=
+						    (4 +
+						     pSdoComCon_p->
+						     m_uiTransSize);
+						// fill rest of header
+						AmiSetWordToLe(&pCommandFrame->
+							       m_le_wSegmentSize,
+							       (WORD) (4 +
+								       pSdoComCon_p->
+								       m_uiTransSize));
+
+						pSdoComCon_p->
+						    m_uiTransferredByte =
+						    pSdoComCon_p->m_uiTransSize;
+						pSdoComCon_p->m_uiTransSize = 0;
+					}
+					break;
+				}
+
+			case kEplSdoServiceNIL:
+			default:
+				// invalid service requested
+				Ret = kEplSdoComInvalidServiceType;
+				goto Exit;
+			}	// end of switch(pSdoComCon_p->m_SdoServiceType)
+		} else		// (pSdoComCon_p->m_uiTransferredByte > 0)
+		{		// continue SDO transfer
+			switch (pSdoComCon_p->m_SdoServiceType) {
+				// for expedited read is nothing to do
+				// -> server sends data
+
+			case kEplSdoServiceWriteByIndex:
+				{	// send next frame
+					if (pSdoComCon_p->m_SdoTransType ==
+					    kEplSdoTransSegmented) {
+						if (pSdoComCon_p->m_uiTransSize > EPL_SDO_MAX_PAYLOAD) {	// next segment
+							pbPayload =
+							    &pCommandFrame->
+							    m_le_abCommandData
+							    [0];
+							// fill rest of header
+							AmiSetWordToLe
+							    (&pCommandFrame->
+							     m_le_wSegmentSize,
+							     EPL_SDO_MAX_PAYLOAD);
+							bFlags = 0x20;
+							AmiSetByteToLe
+							    (&pCommandFrame->
+							     m_le_bFlags,
+							     bFlags);
+							// copy data
+							EPL_MEMCPY(pbPayload,
+								   pSdoComCon_p->
+								   m_pData,
+								   EPL_SDO_MAX_PAYLOAD);
+							pSdoComCon_p->m_pData +=
+							    EPL_SDO_MAX_PAYLOAD;
+							// correct intern counter
+							pSdoComCon_p->
+							    m_uiTransSize -=
+							    EPL_SDO_MAX_PAYLOAD;
+							pSdoComCon_p->
+							    m_uiTransferredByte
+							    =
+							    EPL_SDO_MAX_PAYLOAD;
+							// calc size
+							uiSizeOfFrame +=
+							    EPL_SDO_MAX_PAYLOAD;
+
+						} else {	// end of transfer
+							pbPayload =
+							    &pCommandFrame->
+							    m_le_abCommandData
+							    [0];
+							// fill rest of header
+							AmiSetWordToLe
+							    (&pCommandFrame->
+							     m_le_wSegmentSize,
+							     (WORD)
+							     pSdoComCon_p->
+							     m_uiTransSize);
+							bFlags = 0x30;
+							AmiSetByteToLe
+							    (&pCommandFrame->
+							     m_le_bFlags,
+							     bFlags);
+							// copy data
+							EPL_MEMCPY(pbPayload,
+								   pSdoComCon_p->
+								   m_pData,
+								   pSdoComCon_p->
+								   m_uiTransSize);
+							pSdoComCon_p->m_pData +=
+							    pSdoComCon_p->
+							    m_uiTransSize;
+							// calc size
+							uiSizeOfFrame +=
+							    pSdoComCon_p->
+							    m_uiTransSize;
+							// correct intern counter
+							pSdoComCon_p->
+							    m_uiTransSize = 0;
+							pSdoComCon_p->
+							    m_uiTransferredByte
+							    =
+							    pSdoComCon_p->
+							    m_uiTransSize;
+
+						}
+					} else {
+						goto Exit;
+					}
+					break;
+				}
+			default:
+				{
+					goto Exit;
+				}
+			}	// end of switch(pSdoComCon_p->m_SdoServiceType)
+		}
+	} else {
+		goto Exit;
+	}
+
+	// call send function of lower layer
+	switch (pSdoComCon_p->m_SdoProtType) {
+	case kEplSdoTypeAsnd:
+	case kEplSdoTypeUdp:
+		{
+			Ret = EplSdoAsySeqSendData(pSdoComCon_p->m_SdoSeqConHdl,
+						   uiSizeOfFrame, pFrame);
+			break;
+		}
+
+	default:
+		{
+			Ret = kEplSdoComUnsupportedProt;
+		}
+	}			// end of switch(pSdoComCon_p->m_SdoProtType)
+
+      Exit:
+	return Ret;
+
+}
+#endif
+//---------------------------------------------------------------------------
+//
+// Function:        EplSdoComClientProcessFrame
+//
+// Description:    function process a received frame
+//
+//
+//
+// Parameters:      SdoComCon_p      = connection handle
+//                  pAsySdoCom_p     = pointer to frame to process
+//
+// Returns:         tEplKernel  =  errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+static tEplKernel EplSdoComClientProcessFrame(tEplSdoComConHdl SdoComCon_p,
+					      tEplAsySdoCom * pAsySdoCom_p)
+{
+	tEplKernel Ret;
+	BYTE bBuffer;
+	unsigned int uiBuffer;
+	unsigned int uiDataSize;
+	unsigned long ulBuffer;
+	tEplSdoComCon *pSdoComCon;
+
+	Ret = kEplSuccessful;
+
+	// get pointer to control structure
+	pSdoComCon = &SdoComInstance_g.m_SdoComCon[SdoComCon_p];
+
+	// check if transaction Id fit
+	bBuffer = AmiGetByteFromLe(&pAsySdoCom_p->m_le_bTransactionId);
+	if (pSdoComCon->m_bTransactionId != bBuffer) {
+		// incorrect transaction id
+
+		// if running transfer
+		if ((pSdoComCon->m_uiTransferredByte != 0)
+		    && (pSdoComCon->m_uiTransSize != 0)) {
+			pSdoComCon->m_dwLastAbortCode = EPL_SDOAC_GENERAL_ERROR;
+			// -> send abort
+			EplSdoComClientSendAbort(pSdoComCon,
+						 pSdoComCon->m_dwLastAbortCode);
+			// call callback of application
+			Ret =
+			    EplSdoComTransferFinished(SdoComCon_p, pSdoComCon,
+						      kEplSdoComTransferTxAborted);
+		}
+
+	} else {		// check if correct command
+		bBuffer = AmiGetByteFromLe(&pAsySdoCom_p->m_le_bCommandId);
+		if (pSdoComCon->m_SdoServiceType != bBuffer) {
+			// incorrect command
+			// if running transfer
+			if ((pSdoComCon->m_uiTransferredByte != 0)
+			    && (pSdoComCon->m_uiTransSize != 0)) {
+				pSdoComCon->m_dwLastAbortCode =
+				    EPL_SDOAC_GENERAL_ERROR;
+				// -> send abort
+				EplSdoComClientSendAbort(pSdoComCon,
+							 pSdoComCon->
+							 m_dwLastAbortCode);
+				// call callback of application
+				Ret =
+				    EplSdoComTransferFinished(SdoComCon_p,
+							      pSdoComCon,
+							      kEplSdoComTransferTxAborted);
+			}
+
+		} else {	// switch on command
+			switch (pSdoComCon->m_SdoServiceType) {
+			case kEplSdoServiceWriteByIndex:
+				{	// check if confirmation from server
+					// nothing more to do
+					break;
+				}
+
+			case kEplSdoServiceReadByIndex:
+				{	// check if it is an segmented or an expedited transfer
+					bBuffer =
+					    AmiGetByteFromLe(&pAsySdoCom_p->
+							     m_le_bFlags);
+					// mask uninteressting bits
+					bBuffer &= 0x30;
+					switch (bBuffer) {
+						// expedited transfer
+					case 0x00:
+						{
+							// check size of buffer
+							uiBuffer =
+							    AmiGetWordFromLe
+							    (&pAsySdoCom_p->
+							     m_le_wSegmentSize);
+							if (uiBuffer > pSdoComCon->m_uiTransSize) {	// buffer provided by the application is to small
+								// copy only a part
+								uiDataSize =
+								    pSdoComCon->
+								    m_uiTransSize;
+							} else {	// buffer fits
+								uiDataSize =
+								    uiBuffer;
+							}
+
+							// copy data
+							EPL_MEMCPY(pSdoComCon->
+								   m_pData,
+								   &pAsySdoCom_p->
+								   m_le_abCommandData
+								   [0],
+								   uiDataSize);
+
+							// correct counter
+							pSdoComCon->
+							    m_uiTransSize = 0;
+							pSdoComCon->
+							    m_uiTransferredByte
+							    = uiDataSize;
+							break;
+						}
+
+						// start of a segmented transfer
+					case 0x10:
+						{	// get total size of transfer
+							ulBuffer =
+							    AmiGetDwordFromLe
+							    (&pAsySdoCom_p->
+							     m_le_abCommandData
+							     [0]);
+							if (ulBuffer <= pSdoComCon->m_uiTransSize) {	// buffer fit
+								pSdoComCon->
+								    m_uiTransSize
+								    =
+								    (unsigned
+								     int)
+								    ulBuffer;
+							} else {	// buffer to small
+								// send abort
+								pSdoComCon->
+								    m_dwLastAbortCode
+								    =
+								    EPL_SDOAC_DATA_TYPE_LENGTH_TOO_HIGH;
+								// -> send abort
+								EplSdoComClientSendAbort
+								    (pSdoComCon,
+								     pSdoComCon->
+								     m_dwLastAbortCode);
+								// call callback of application
+								Ret =
+								    EplSdoComTransferFinished
+								    (SdoComCon_p,
+								     pSdoComCon,
+								     kEplSdoComTransferRxAborted);
+								goto Exit;
+							}
+
+							// get segment size
+							// check size of buffer
+							uiBuffer =
+							    AmiGetWordFromLe
+							    (&pAsySdoCom_p->
+							     m_le_wSegmentSize);
+							// subtract size of vaiable header from datasize
+							uiBuffer -= 4;
+							// copy data
+							EPL_MEMCPY(pSdoComCon->
+								   m_pData,
+								   &pAsySdoCom_p->
+								   m_le_abCommandData
+								   [4],
+								   uiBuffer);
+
+							// correct counter an pointer
+							pSdoComCon->m_pData +=
+							    uiBuffer;
+							pSdoComCon->
+							    m_uiTransferredByte
+							    += uiBuffer;
+							pSdoComCon->
+							    m_uiTransSize -=
+							    uiBuffer;
+
+							break;
+						}
+
+						// segment
+					case 0x20:
+						{
+							// get segment size
+							// check size of buffer
+							uiBuffer =
+							    AmiGetWordFromLe
+							    (&pAsySdoCom_p->
+							     m_le_wSegmentSize);
+							// check if data to copy fit to buffer
+							if (uiBuffer >= pSdoComCon->m_uiTransSize) {	// to much data
+								uiBuffer =
+								    (pSdoComCon->
+								     m_uiTransSize
+								     - 1);
+							}
+							// copy data
+							EPL_MEMCPY(pSdoComCon->
+								   m_pData,
+								   &pAsySdoCom_p->
+								   m_le_abCommandData
+								   [0],
+								   uiBuffer);
+
+							// correct counter an pointer
+							pSdoComCon->m_pData +=
+							    uiBuffer;
+							pSdoComCon->
+							    m_uiTransferredByte
+							    += uiBuffer;
+							pSdoComCon->
+							    m_uiTransSize -=
+							    uiBuffer;
+							break;
+						}
+
+						// last segment
+					case 0x30:
+						{
+							// get segment size
+							// check size of buffer
+							uiBuffer =
+							    AmiGetWordFromLe
+							    (&pAsySdoCom_p->
+							     m_le_wSegmentSize);
+							// check if data to copy fit to buffer
+							if (uiBuffer > pSdoComCon->m_uiTransSize) {	// to much data
+								uiBuffer =
+								    (pSdoComCon->
+								     m_uiTransSize
+								     - 1);
+							}
+							// copy data
+							EPL_MEMCPY(pSdoComCon->
+								   m_pData,
+								   &pAsySdoCom_p->
+								   m_le_abCommandData
+								   [0],
+								   uiBuffer);
+
+							// correct counter an pointer
+							pSdoComCon->m_pData +=
+							    uiBuffer;
+							pSdoComCon->
+							    m_uiTransferredByte
+							    += uiBuffer;
+							pSdoComCon->
+							    m_uiTransSize = 0;
+
+							break;
+						}
+					}	// end of switch(bBuffer & 0x30)
+
+					break;
+				}
+
+			case kEplSdoServiceNIL:
+			default:
+				// invalid service requested
+				// $$$ d.k. What should we do?
+				break;
+			}	// end of switch(pSdoComCon->m_SdoServiceType)
+		}
+	}
+
+      Exit:
+	return Ret;
+}
+#endif
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoComClientSendAbort
+//
+// Description: function send a abort message
+//
+//
+//
+// Parameters:  pSdoComCon_p     = pointer to control structure of connection
+//              dwAbortCode_p    = Sdo abort code
+//
+// Returns:     tEplKernel  =  errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+static tEplKernel EplSdoComClientSendAbort(tEplSdoComCon * pSdoComCon_p,
+					   DWORD dwAbortCode_p)
+{
+	tEplKernel Ret;
+	BYTE abFrame[EPL_MAX_SDO_FRAME_SIZE];
+	tEplFrame *pFrame;
+	tEplAsySdoCom *pCommandFrame;
+	unsigned int uiSizeOfFrame;
+
+	Ret = kEplSuccessful;
+
+	pFrame = (tEplFrame *) & abFrame[0];
+
+	EPL_MEMSET(&abFrame[0], 0x00, sizeof(abFrame));
+
+	// build generic part of frame
+	// get pointer to command layerpart of frame
+	pCommandFrame =
+	    &pFrame->m_Data.m_Asnd.m_Payload.m_SdoSequenceFrame.
+	    m_le_abSdoSeqPayload;
+	AmiSetByteToLe(&pCommandFrame->m_le_bCommandId,
+		       pSdoComCon_p->m_SdoServiceType);
+	AmiSetByteToLe(&pCommandFrame->m_le_bTransactionId,
+		       pSdoComCon_p->m_bTransactionId);
+
+	uiSizeOfFrame = 8;
+
+	// set response and abort flag
+	pCommandFrame->m_le_bFlags |= 0x40;
+
+	// copy abortcode to frame
+	AmiSetDwordToLe(&pCommandFrame->m_le_abCommandData[0], dwAbortCode_p);
+
+	// set size of segment
+	AmiSetWordToLe(&pCommandFrame->m_le_wSegmentSize, sizeof(DWORD));
+
+	// update counter
+	pSdoComCon_p->m_uiTransferredByte = sizeof(DWORD);
+	pSdoComCon_p->m_uiTransSize = 0;
+
+	// calc framesize
+	uiSizeOfFrame += sizeof(DWORD);
+
+	// save abort code
+	pSdoComCon_p->m_dwLastAbortCode = dwAbortCode_p;
+
+	// call send function of lower layer
+	switch (pSdoComCon_p->m_SdoProtType) {
+	case kEplSdoTypeAsnd:
+	case kEplSdoTypeUdp:
+		{
+			Ret = EplSdoAsySeqSendData(pSdoComCon_p->m_SdoSeqConHdl,
+						   uiSizeOfFrame, pFrame);
+			break;
+		}
+
+	default:
+		{
+			Ret = kEplSdoComUnsupportedProt;
+		}
+	}			// end of switch(pSdoComCon_p->m_SdoProtType)
+
+	return Ret;
+}
+#endif
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoComTransferFinished
+//
+// Description: calls callback function of application if available
+//              and clears entry in control structure
+//
+// Parameters:  pSdoComCon_p     = pointer to control structure of connection
+//              SdoComConState_p = state of SDO transfer
+//
+// Returns:     tEplKernel  =  errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static tEplKernel EplSdoComTransferFinished(tEplSdoComConHdl SdoComCon_p,
+					    tEplSdoComCon * pSdoComCon_p,
+					    tEplSdoComConState SdoComConState_p)
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	if (pSdoComCon_p->m_pfnTransferFinished != NULL) {
+		tEplSdoFinishedCb pfnTransferFinished;
+		tEplSdoComFinished SdoComFinished;
+
+		SdoComFinished.m_pUserArg = pSdoComCon_p->m_pUserArg;
+		SdoComFinished.m_uiNodeId = pSdoComCon_p->m_uiNodeId;
+		SdoComFinished.m_uiTargetIndex = pSdoComCon_p->m_uiTargetIndex;
+		SdoComFinished.m_uiTargetSubIndex =
+		    pSdoComCon_p->m_uiTargetSubIndex;
+		SdoComFinished.m_uiTransferredByte =
+		    pSdoComCon_p->m_uiTransferredByte;
+		SdoComFinished.m_dwAbortCode = pSdoComCon_p->m_dwLastAbortCode;
+		SdoComFinished.m_SdoComConHdl = SdoComCon_p;
+		SdoComFinished.m_SdoComConState = SdoComConState_p;
+		if (pSdoComCon_p->m_SdoServiceType ==
+		    kEplSdoServiceWriteByIndex) {
+			SdoComFinished.m_SdoAccessType = kEplSdoAccessTypeWrite;
+		} else {
+			SdoComFinished.m_SdoAccessType = kEplSdoAccessTypeRead;
+		}
+
+		// reset transfer state so this handle is not busy anymore
+		pSdoComCon_p->m_uiTransferredByte = 0;
+		pSdoComCon_p->m_uiTransSize = 0;
+
+		pfnTransferFinished = pSdoComCon_p->m_pfnTransferFinished;
+		// delete function pointer to inform application only once for each transfer
+		pSdoComCon_p->m_pfnTransferFinished = NULL;
+
+		// call application's callback function
+		pfnTransferFinished(&SdoComFinished);
+
+	}
+
+	return Ret;
+}
+
+// EOF
diff --git a/drivers/staging/epl/EplSdoUdpu.c b/drivers/staging/epl/EplSdoUdpu.c
new file mode 100644
index 00000000000..be52233b3ee
--- /dev/null
+++ b/drivers/staging/epl/EplSdoUdpu.c
@@ -0,0 +1,790 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for SDO/UDP-Protocolabstractionlayer module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplSdoUdpu.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.8 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/26 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "user/EplSdoUdpu.h"
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) != 0)
+
+#if (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
+#include "SocketLinuxKernel.h"
+#include <linux/completion.h>
+#include <linux/sched.h>
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#ifndef EPL_SDO_MAX_CONNECTION_UDP
+#define EPL_SDO_MAX_CONNECTION_UDP  5
+#endif
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+typedef struct {
+	unsigned long m_ulIpAddr;	// in network byte order
+	unsigned int m_uiPort;	// in network byte order
+
+} tEplSdoUdpCon;
+
+// instance table
+typedef struct {
+	tEplSdoUdpCon m_aSdoAbsUdpConnection[EPL_SDO_MAX_CONNECTION_UDP];
+	tEplSequLayerReceiveCb m_fpSdoAsySeqCb;
+	SOCKET m_UdpSocket;
+
+#if (TARGET_SYSTEM == _WIN32_)
+	HANDLE m_ThreadHandle;
+	LPCRITICAL_SECTION m_pCriticalSection;
+	CRITICAL_SECTION m_CriticalSection;
+
+#elif (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
+	struct completion m_CompletionUdpThread;
+	int m_ThreadHandle;
+	int m_iTerminateThread;
+#endif
+
+} tEplSdoUdpInstance;
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+static tEplSdoUdpInstance SdoUdpInstance_g;
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+#if (TARGET_SYSTEM == _WIN32_)
+static DWORD PUBLIC EplSdoUdpThread(LPVOID lpParameter);
+
+#elif (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
+static int EplSdoUdpThread(void *pArg_p);
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  <EPL-SDO-UDP-Layer>                                 */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description: Protocolabstraction layer for UDP
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoUdpuInit
+//
+// Description: init first instance of the module
+//
+//
+//
+// Parameters:  pReceiveCb_p    =   functionpointer to Sdo-Sequence layer
+//                                  callback-function
+//
+//
+// Returns:     tEplKernel  = Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoUdpuInit(tEplSequLayerReceiveCb fpReceiveCb_p)
+{
+	tEplKernel Ret;
+
+	Ret = EplSdoUdpuAddInstance(fpReceiveCb_p);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoUdpuAddInstance
+//
+// Description: init additional instance of the module
+//              înit socket and start Listen-Thread
+//
+//
+//
+// Parameters:  pReceiveCb_p    =   functionpointer to Sdo-Sequence layer
+//                                  callback-function
+//
+//
+// Returns:     tEplKernel  = Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoUdpuAddInstance(tEplSequLayerReceiveCb fpReceiveCb_p)
+{
+	tEplKernel Ret;
+
+#if (TARGET_SYSTEM == _WIN32_)
+	int iError;
+	WSADATA Wsa;
+
+#endif
+
+	// set instance variables to 0
+	EPL_MEMSET(&SdoUdpInstance_g, 0x00, sizeof(SdoUdpInstance_g));
+
+	Ret = kEplSuccessful;
+
+	// save pointer to callback-function
+	if (fpReceiveCb_p != NULL) {
+		SdoUdpInstance_g.m_fpSdoAsySeqCb = fpReceiveCb_p;
+	} else {
+		Ret = kEplSdoUdpMissCb;
+		goto Exit;
+	}
+
+#if (TARGET_SYSTEM == _WIN32_)
+	// start winsock2 for win32
+	// windows specific start of socket
+	iError = WSAStartup(MAKEWORD(2, 0), &Wsa);
+	if (iError != 0) {
+		Ret = kEplSdoUdpNoSocket;
+		goto Exit;
+	}
+	// create critical section for acccess of instnace variables
+	SdoUdpInstance_g.m_pCriticalSection =
+	    &SdoUdpInstance_g.m_CriticalSection;
+	InitializeCriticalSection(SdoUdpInstance_g.m_pCriticalSection);
+
+#elif (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
+	init_completion(&SdoUdpInstance_g.m_CompletionUdpThread);
+	SdoUdpInstance_g.m_iTerminateThread = 0;
+#endif
+
+	SdoUdpInstance_g.m_ThreadHandle = 0;
+	SdoUdpInstance_g.m_UdpSocket = INVALID_SOCKET;
+
+	Ret = EplSdoUdpuConfig(INADDR_ANY, 0);
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoUdpuDelInstance
+//
+// Description: del instance of the module
+//              del socket and del Listen-Thread
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoUdpuDelInstance()
+{
+	tEplKernel Ret;
+
+#if (TARGET_SYSTEM == _WIN32_)
+	BOOL fTermError;
+#endif
+
+	Ret = kEplSuccessful;
+
+	if (SdoUdpInstance_g.m_ThreadHandle != 0) {	// listen thread was started
+		// close thread
+#if (TARGET_SYSTEM == _WIN32_)
+		fTermError =
+		    TerminateThread(SdoUdpInstance_g.m_ThreadHandle, 0);
+		if (fTermError == FALSE) {
+			Ret = kEplSdoUdpThreadError;
+			goto Exit;
+		}
+#elif (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
+		SdoUdpInstance_g.m_iTerminateThread = 1;
+		/* kill_proc(SdoUdpInstance_g.m_ThreadHandle, SIGTERM, 1 ); */
+		send_sig(SIGTERM, SdoUdpInstance_g.m_ThreadHandle, 1);
+		wait_for_completion(&SdoUdpInstance_g.m_CompletionUdpThread);
+#endif
+
+		SdoUdpInstance_g.m_ThreadHandle = 0;
+	}
+
+	if (SdoUdpInstance_g.m_UdpSocket != INVALID_SOCKET) {
+		// close socket
+		closesocket(SdoUdpInstance_g.m_UdpSocket);
+		SdoUdpInstance_g.m_UdpSocket = INVALID_SOCKET;
+	}
+#if (TARGET_SYSTEM == _WIN32_)
+	// delete critical section
+	DeleteCriticalSection(SdoUdpInstance_g.m_pCriticalSection);
+#endif
+
+#if (TARGET_SYSTEM == _WIN32_)
+	// for win 32
+	WSACleanup();
+#endif
+
+#if (TARGET_SYSTEM == _WIN32_)
+      Exit:
+#endif
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoUdpuConfig
+//
+// Description: reconfigurate socket with new IP-Address
+//              -> needed for NMT ResetConfiguration
+//
+// Parameters:  ulIpAddr_p      = IpAddress in platform byte order
+//              uiPort_p        = port number in platform byte order
+//
+//
+// Returns:     tEplKernel  = Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoUdpuConfig(unsigned long ulIpAddr_p,
+				   unsigned int uiPort_p)
+{
+	tEplKernel Ret;
+	struct sockaddr_in Addr;
+	int iError;
+
+#if (TARGET_SYSTEM == _WIN32_)
+	BOOL fTermError;
+	unsigned long ulThreadId;
+#endif
+
+	Ret = kEplSuccessful;
+
+	if (uiPort_p == 0) {	// set UDP port to default port number
+		uiPort_p = EPL_C_SDO_EPL_PORT;
+	} else if (uiPort_p > 65535) {
+		Ret = kEplSdoUdpSocketError;
+		goto Exit;
+	}
+
+	if (SdoUdpInstance_g.m_ThreadHandle != 0) {	// listen thread was started
+
+		// close old thread
+#if (TARGET_SYSTEM == _WIN32_)
+		fTermError =
+		    TerminateThread(SdoUdpInstance_g.m_ThreadHandle, 0);
+		if (fTermError == FALSE) {
+			Ret = kEplSdoUdpThreadError;
+			goto Exit;
+		}
+#elif (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
+		SdoUdpInstance_g.m_iTerminateThread = 1;
+		/* kill_proc(SdoUdpInstance_g.m_ThreadHandle, SIGTERM, 1 ); */
+		send_sig(SIGTERM, SdoUdpInstance_g.m_ThreadHandle, 1);
+		wait_for_completion(&SdoUdpInstance_g.m_CompletionUdpThread);
+		SdoUdpInstance_g.m_iTerminateThread = 0;
+#endif
+
+		SdoUdpInstance_g.m_ThreadHandle = 0;
+	}
+
+	if (SdoUdpInstance_g.m_UdpSocket != INVALID_SOCKET) {
+		// close socket
+		iError = closesocket(SdoUdpInstance_g.m_UdpSocket);
+		SdoUdpInstance_g.m_UdpSocket = INVALID_SOCKET;
+		if (iError != 0) {
+			Ret = kEplSdoUdpSocketError;
+			goto Exit;
+		}
+	}
+	// create Socket
+	SdoUdpInstance_g.m_UdpSocket = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
+	if (SdoUdpInstance_g.m_UdpSocket == INVALID_SOCKET) {
+		Ret = kEplSdoUdpNoSocket;
+		EPL_DBGLVL_SDO_TRACE0("EplSdoUdpuConfig: socket() failed\n");
+		goto Exit;
+	}
+	// bind socket
+	Addr.sin_family = AF_INET;
+	Addr.sin_port = htons((unsigned short)uiPort_p);
+	Addr.sin_addr.s_addr = htonl(ulIpAddr_p);
+	iError =
+	    bind(SdoUdpInstance_g.m_UdpSocket, (struct sockaddr *)&Addr,
+		 sizeof(Addr));
+	if (iError < 0) {
+		//iError = WSAGetLastError();
+		EPL_DBGLVL_SDO_TRACE1
+		    ("EplSdoUdpuConfig: bind() finished with %i\n", iError);
+		Ret = kEplSdoUdpNoSocket;
+		goto Exit;
+	}
+	// create Listen-Thread
+#if (TARGET_SYSTEM == _WIN32_)
+	// for win32
+
+	// create thread
+	SdoUdpInstance_g.m_ThreadHandle = CreateThread(NULL,
+						       0,
+						       EplSdoUdpThread,
+						       &SdoUdpInstance_g,
+						       0, &ulThreadId);
+	if (SdoUdpInstance_g.m_ThreadHandle == NULL) {
+		Ret = kEplSdoUdpThreadError;
+		goto Exit;
+	}
+#elif (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
+
+	SdoUdpInstance_g.m_ThreadHandle =
+	    kernel_thread(EplSdoUdpThread, &SdoUdpInstance_g, CLONE_KERNEL);
+	if (SdoUdpInstance_g.m_ThreadHandle == 0) {
+		Ret = kEplSdoUdpThreadError;
+		goto Exit;
+	}
+#endif
+
+      Exit:
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoUdpuInitCon
+//
+// Description: init a new connect
+//
+//
+//
+// Parameters:  pSdoConHandle_p = pointer for the new connection handle
+//              uiTargetNodeId_p = NodeId of the target node
+//
+//
+// Returns:     tEplKernel  = Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoUdpuInitCon(tEplSdoConHdl * pSdoConHandle_p,
+				    unsigned int uiTargetNodeId_p)
+{
+	tEplKernel Ret;
+	unsigned int uiCount;
+	unsigned int uiFreeCon;
+	tEplSdoUdpCon *pSdoUdpCon;
+
+	Ret = kEplSuccessful;
+
+	// get free entry in control structure
+	uiCount = 0;
+	uiFreeCon = EPL_SDO_MAX_CONNECTION_UDP;
+	pSdoUdpCon = &SdoUdpInstance_g.m_aSdoAbsUdpConnection[0];
+	while (uiCount < EPL_SDO_MAX_CONNECTION_UDP) {
+		if ((pSdoUdpCon->m_ulIpAddr & htonl(0xFF)) == htonl(uiTargetNodeId_p)) {	// existing connection to target node found
+			// set handle
+			*pSdoConHandle_p = (uiCount | EPL_SDO_UDP_HANDLE);
+
+			goto Exit;
+		} else if ((pSdoUdpCon->m_ulIpAddr == 0)
+			   && (pSdoUdpCon->m_uiPort == 0)) {
+			uiFreeCon = uiCount;
+		}
+		uiCount++;
+		pSdoUdpCon++;
+	}
+
+	if (uiFreeCon == EPL_SDO_MAX_CONNECTION_UDP) {
+		// error no free handle
+		Ret = kEplSdoUdpNoFreeHandle;
+	} else {
+		pSdoUdpCon =
+		    &SdoUdpInstance_g.m_aSdoAbsUdpConnection[uiFreeCon];
+		// save infos for connection
+		pSdoUdpCon->m_uiPort = htons(EPL_C_SDO_EPL_PORT);
+		pSdoUdpCon->m_ulIpAddr = htonl(0xC0A86400 | uiTargetNodeId_p);	// 192.168.100.uiTargetNodeId_p
+
+		// set handle
+		*pSdoConHandle_p = (uiFreeCon | EPL_SDO_UDP_HANDLE);
+
+	}
+
+      Exit:
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoUdpuSendData
+//
+// Description: send data using exisiting connection
+//
+//
+//
+// Parameters:  SdoConHandle_p  = connection handle
+//              pSrcData_p      = pointer to data
+//              dwDataSize_p    = number of databyte
+//                                  -> without asend-header!!!
+//
+// Returns:     tEplKernel  = Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoUdpuSendData(tEplSdoConHdl SdoConHandle_p,
+				     tEplFrame * pSrcData_p, DWORD dwDataSize_p)
+{
+	tEplKernel Ret;
+	int iError;
+	unsigned int uiArray;
+	struct sockaddr_in Addr;
+
+	Ret = kEplSuccessful;
+
+	uiArray = (SdoConHandle_p & ~EPL_SDO_ASY_HANDLE_MASK);
+	if (uiArray >= EPL_SDO_MAX_CONNECTION_UDP) {
+		Ret = kEplSdoUdpInvalidHdl;
+		goto Exit;
+	}
+	//set message type
+	AmiSetByteToLe(&pSrcData_p->m_le_bMessageType, 0x06);	// SDO
+	// target node id (for Udp = 0)
+	AmiSetByteToLe(&pSrcData_p->m_le_bDstNodeId, 0x00);
+	// set source-nodeid (for Udp = 0)
+	AmiSetByteToLe(&pSrcData_p->m_le_bSrcNodeId, 0x00);
+
+	// calc size
+	dwDataSize_p += EPL_ASND_HEADER_SIZE;
+
+	// call sendto
+	Addr.sin_family = AF_INET;
+#if (TARGET_SYSTEM == _WIN32_)
+	// enter  critical section for process function
+	EnterCriticalSection(SdoUdpInstance_g.m_pCriticalSection);
+#endif
+
+	Addr.sin_port =
+	    (unsigned short)SdoUdpInstance_g.m_aSdoAbsUdpConnection[uiArray].
+	    m_uiPort;
+	Addr.sin_addr.s_addr =
+	    SdoUdpInstance_g.m_aSdoAbsUdpConnection[uiArray].m_ulIpAddr;
+
+#if (TARGET_SYSTEM == _WIN32_)
+	// leave critical section for process function
+	LeaveCriticalSection(SdoUdpInstance_g.m_pCriticalSection);
+#endif
+
+	iError = sendto(SdoUdpInstance_g.m_UdpSocket,	// sockethandle
+			(const char *)&pSrcData_p->m_le_bMessageType,	// data to send
+			dwDataSize_p,	// number of bytes to send
+			0,	// flags
+			(struct sockaddr *)&Addr,	// target
+			sizeof(struct sockaddr_in));	// sizeof targetadress
+	if (iError < 0) {
+		EPL_DBGLVL_SDO_TRACE1
+		    ("EplSdoUdpuSendData: sendto() finished with %i\n", iError);
+		Ret = kEplSdoUdpSendError;
+		goto Exit;
+	}
+
+      Exit:
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoUdpuDelCon
+//
+// Description: delete connection from intern structure
+//
+//
+//
+// Parameters:  SdoConHandle_p  = connection handle
+//
+// Returns:     tEplKernel  = Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoUdpuDelCon(tEplSdoConHdl SdoConHandle_p)
+{
+	tEplKernel Ret;
+	unsigned int uiArray;
+
+	uiArray = (SdoConHandle_p & ~EPL_SDO_ASY_HANDLE_MASK);
+
+	if (uiArray >= EPL_SDO_MAX_CONNECTION_UDP) {
+		Ret = kEplSdoUdpInvalidHdl;
+		goto Exit;
+	} else {
+		Ret = kEplSuccessful;
+	}
+
+	// delete connection
+	SdoUdpInstance_g.m_aSdoAbsUdpConnection[uiArray].m_ulIpAddr = 0;
+	SdoUdpInstance_g.m_aSdoAbsUdpConnection[uiArray].m_uiPort = 0;
+
+      Exit:
+	return Ret;
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:        EplSdoUdpThread
+//
+// Description:     thread check socket for new data
+//
+//
+//
+// Parameters:      lpParameter = pointer to parameter type tEplSdoUdpThreadPara
+//
+//
+// Returns:         DWORD   =   errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+#if (TARGET_SYSTEM == _WIN32_)
+static DWORD PUBLIC EplSdoUdpThread(LPVOID lpParameter)
+#elif (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
+static int EplSdoUdpThread(void *pArg_p)
+#endif
+{
+
+	tEplSdoUdpInstance *pInstance;
+	struct sockaddr_in RemoteAddr;
+	int iError;
+	int iCount;
+	int iFreeEntry;
+	BYTE abBuffer[EPL_MAX_SDO_REC_FRAME_SIZE];
+	unsigned int uiSize;
+	tEplSdoConHdl SdoConHdl;
+
+#if (TARGET_SYSTEM == _WIN32_)
+	pInstance = (tEplSdoUdpInstance *) lpParameter;
+
+	for (;;)
+#elif (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
+	pInstance = (tEplSdoUdpInstance *) pArg_p;
+	daemonize("EplSdoUdpThread");
+	allow_signal(SIGTERM);
+
+	for (; pInstance->m_iTerminateThread == 0;)
+#endif
+
+	{
+		// wait for data
+		uiSize = sizeof(struct sockaddr);
+		iError = recvfrom(pInstance->m_UdpSocket,	// Socket
+				  (char *)&abBuffer[0],	// buffer for data
+				  sizeof(abBuffer),	// size of the buffer
+				  0,	// flags
+				  (struct sockaddr *)&RemoteAddr,
+				  (int *)&uiSize);
+#if (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
+		if (iError == -ERESTARTSYS) {
+			break;
+		}
+#endif
+		if (iError > 0) {
+			// get handle for higher layer
+			iCount = 0;
+			iFreeEntry = 0xFFFF;
+#if (TARGET_SYSTEM == _WIN32_)
+			// enter  critical section for process function
+			EnterCriticalSection(SdoUdpInstance_g.
+					     m_pCriticalSection);
+#endif
+			while (iCount < EPL_SDO_MAX_CONNECTION_UDP) {
+				// check if this connection is already known
+				if ((pInstance->m_aSdoAbsUdpConnection[iCount].
+				     m_ulIpAddr == RemoteAddr.sin_addr.s_addr)
+				    && (pInstance->
+					m_aSdoAbsUdpConnection[iCount].
+					m_uiPort == RemoteAddr.sin_port)) {
+					break;
+				}
+
+				if ((pInstance->m_aSdoAbsUdpConnection[iCount].
+				     m_ulIpAddr == 0)
+				    && (pInstance->
+					m_aSdoAbsUdpConnection[iCount].
+					m_uiPort == 0)
+				    && (iFreeEntry == 0xFFFF))
+				{
+					iFreeEntry = iCount;
+				}
+
+				iCount++;
+			}
+
+			if (iCount == EPL_SDO_MAX_CONNECTION_UDP) {
+				// connection unknown
+				// see if there is a free handle
+				if (iFreeEntry != 0xFFFF) {
+					// save adress infos
+					pInstance->
+					    m_aSdoAbsUdpConnection[iFreeEntry].
+					    m_ulIpAddr =
+					    RemoteAddr.sin_addr.s_addr;
+					pInstance->
+					    m_aSdoAbsUdpConnection[iFreeEntry].
+					    m_uiPort = RemoteAddr.sin_port;
+#if (TARGET_SYSTEM == _WIN32_)
+					// leave critical section for process function
+					LeaveCriticalSection(SdoUdpInstance_g.
+							     m_pCriticalSection);
+#endif
+					// call callback
+					SdoConHdl = iFreeEntry;
+					SdoConHdl |= EPL_SDO_UDP_HANDLE;
+					// offset 4 -> start of SDO Sequence header
+					pInstance->m_fpSdoAsySeqCb(SdoConHdl,
+								   (tEplAsySdoSeq
+								    *) &
+								   abBuffer[4],
+								   (iError -
+								    4));
+				} else {
+					EPL_DBGLVL_SDO_TRACE0
+					    ("Error in EplSdoUdpThread() no free handle\n");
+#if (TARGET_SYSTEM == _WIN32_)
+					// leave critical section for process function
+					LeaveCriticalSection(SdoUdpInstance_g.
+							     m_pCriticalSection);
+#endif
+				}
+
+			} else {
+				// known connection
+				// call callback with correct handle
+				SdoConHdl = iCount;
+				SdoConHdl |= EPL_SDO_UDP_HANDLE;
+#if (TARGET_SYSTEM == _WIN32_)
+				// leave critical section for process function
+				LeaveCriticalSection(SdoUdpInstance_g.
+						     m_pCriticalSection);
+#endif
+				// offset 4 -> start of SDO Sequence header
+				pInstance->m_fpSdoAsySeqCb(SdoConHdl,
+							   (tEplAsySdoSeq *) &
+							   abBuffer[4],
+							   (iError - 4));
+			}
+		}		// end of  if(iError!=SOCKET_ERROR)
+	}			// end of for(;;)
+
+#if (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
+	complete_and_exit(&SdoUdpInstance_g.m_CompletionUdpThread, 0);
+#endif
+
+	return 0;
+}
+
+#endif // end of #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) != 0)
+
+// EOF
diff --git a/drivers/staging/epl/EplStatusu.c b/drivers/staging/epl/EplStatusu.c
new file mode 100644
index 00000000000..689f9124ae2
--- /dev/null
+++ b/drivers/staging/epl/EplStatusu.c
@@ -0,0 +1,380 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for Statusu-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplStatusu.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/11/15 d.k.:   start of the implementation
+
+****************************************************************************/
+
+#include "user/EplStatusu.h"
+#include "user/EplDlluCal.h"
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  <xxxxx>                                             */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description:
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   D E F I N I T I O N S                          //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+typedef struct {
+	tEplStatusuCbResponse m_apfnCbResponse[254];
+
+} tEplStatusuInstance;
+
+//---------------------------------------------------------------------------
+// local vars
+//---------------------------------------------------------------------------
+
+static tEplStatusuInstance EplStatusuInstance_g;
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+static tEplKernel PUBLIC EplStatusuCbStatusResponse(tEplFrameInfo *
+						    pFrameInfo_p);
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplStatusuInit
+//
+// Description: init first instance of the module
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT tEplKernel PUBLIC EplStatusuInit()
+{
+	tEplKernel Ret;
+
+	Ret = EplStatusuAddInstance();
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplStatusuAddInstance
+//
+// Description: init other instances of the module
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT tEplKernel PUBLIC EplStatusuAddInstance()
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// reset instance structure
+	EPL_MEMSET(&EplStatusuInstance_g, 0, sizeof(EplStatusuInstance_g));
+
+	// register StatusResponse callback function
+	Ret =
+	    EplDlluCalRegAsndService(kEplDllAsndStatusResponse,
+				     EplStatusuCbStatusResponse,
+				     kEplDllAsndFilterAny);
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplStatusuDelInstance
+//
+// Description: delete instance
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT tEplKernel PUBLIC EplStatusuDelInstance()
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// deregister StatusResponse callback function
+	Ret =
+	    EplDlluCalRegAsndService(kEplDllAsndStatusResponse, NULL,
+				     kEplDllAsndFilterNone);
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplStatusuReset
+//
+// Description: resets this instance
+//
+// Parameters:
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+EPLDLLEXPORT tEplKernel PUBLIC EplStatusuReset()
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// reset instance structure
+	EPL_MEMSET(&EplStatusuInstance_g, 0, sizeof(EplStatusuInstance_g));
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplStatusuRequestStatusResponse
+//
+// Description: returns the StatusResponse for the specified node.
+//
+// Parameters:  uiNodeId_p                  = IN: node ID
+//              pfnCbResponse_p             = IN: function pointer to callback function
+//                                            which will be called if StatusResponse is received
+//
+// Return:      tEplKernel                  = error code
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplStatusuRequestStatusResponse(unsigned int uiNodeId_p,
+						  tEplStatusuCbResponse
+						  pfnCbResponse_p)
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// decrement node ID, because array is zero based
+	uiNodeId_p--;
+	if (uiNodeId_p < tabentries(EplStatusuInstance_g.m_apfnCbResponse)) {
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+		if (EplStatusuInstance_g.m_apfnCbResponse[uiNodeId_p] != NULL) {	// request already issued (maybe by someone else)
+			Ret = kEplInvalidOperation;
+		} else {
+			EplStatusuInstance_g.m_apfnCbResponse[uiNodeId_p] =
+			    pfnCbResponse_p;
+			Ret =
+			    EplDlluCalIssueRequest(kEplDllReqServiceStatus,
+						   (uiNodeId_p + 1), 0xFF);
+		}
+#else
+		Ret = kEplInvalidOperation;
+#endif
+	} else {		// invalid node ID specified
+		Ret = kEplInvalidNodeId;
+	}
+
+	return Ret;
+
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplStatusuCbStatusResponse
+//
+// Description: callback funktion for StatusResponse
+//
+//
+//
+// Parameters:  pFrameInfo_p            = Frame with the StatusResponse
+//
+//
+// Returns:     tEplKernel              = error code
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static tEplKernel PUBLIC EplStatusuCbStatusResponse(tEplFrameInfo *
+						    pFrameInfo_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	unsigned int uiNodeId;
+	unsigned int uiIndex;
+	tEplStatusuCbResponse pfnCbResponse;
+
+	uiNodeId = AmiGetByteFromLe(&pFrameInfo_p->m_pFrame->m_le_bSrcNodeId);
+
+	uiIndex = uiNodeId - 1;
+
+	if (uiIndex < tabentries(EplStatusuInstance_g.m_apfnCbResponse)) {
+		// memorize pointer to callback function
+		pfnCbResponse = EplStatusuInstance_g.m_apfnCbResponse[uiIndex];
+		if (pfnCbResponse == NULL) {	// response was not requested
+			goto Exit;
+		}
+		// reset callback function pointer so that caller may issue next request
+		EplStatusuInstance_g.m_apfnCbResponse[uiIndex] = NULL;
+
+		if (pFrameInfo_p->m_uiFrameSize < EPL_C_DLL_MINSIZE_STATUSRES) {	// StatusResponse not received or it has invalid size
+			Ret = pfnCbResponse(uiNodeId, NULL);
+		} else {	// StatusResponse received
+			Ret =
+			    pfnCbResponse(uiNodeId,
+					  &pFrameInfo_p->m_pFrame->m_Data.
+					  m_Asnd.m_Payload.m_StatusResponse);
+		}
+	}
+
+      Exit:
+	return Ret;
+}
+
+// EOF
diff --git a/drivers/staging/epl/EplTarget.h b/drivers/staging/epl/EplTarget.h
new file mode 100644
index 00000000000..b2b66f82c03
--- /dev/null
+++ b/drivers/staging/epl/EplTarget.h
@@ -0,0 +1,233 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for target api function
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplTarget.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2005/12/05 -as:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPLTARGET_H_
+#define _EPLTARGET_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+// =========================================================================
+// macros for memory access (depends on target system)
+// =========================================================================
+
+// NOTE:
+// The following macros are used to combine standard library definitions. Some
+// applications needs to use one common library function (e.g. memcpy()). So
+// you can set (or change) it here.
+
+#if (TARGET_SYSTEM == _WIN32_)
+
+#define _WIN32_WINDOWS 0x0401
+#define _WIN32_WINNT   0x0400
+
+#include <stdlib.h>
+#include <stdio.h>
+
+    //29.11.2004 f.j. sonst ist memcpy und memset unbekannt
+#include <string.h>
+
+#define EPL_MEMCPY(dst,src,siz)     memcpy((void*)(dst),(const void*)(src),(size_t)(siz));
+#define EPL_MEMSET(dst,val,siz)     memset((void*)(dst),(int)(val),(size_t)(siz));
+
+    // f.j.: die Funktionen für <MemAlloc> und <MemFree> sind in WinMem.c definiert
+    //definition der Prototypen
+void FAR *MemAlloc(DWORD dwMemSize_p);
+void MemFree(void FAR * pMem_p);
+
+#define EPL_MALLOC(siz)             malloc((size_t)(siz))
+#define EPL_FREE(ptr)               free((void *)ptr)
+
+#ifndef PRINTF0
+void trace(const char *fmt, ...);
+#define PRINTF                      TRACE
+#define PRINTF0(arg)                TRACE0(arg)
+#define PRINTF1(arg,p1)             TRACE1(arg,p1)
+#define PRINTF2(arg,p1,p2)          TRACE2(arg,p1,p2)
+#define PRINTF3(arg,p1,p2,p3)       TRACE3(arg,p1,p2,p3)
+#define PRINTF4(arg,p1,p2,p3,p4)    TRACE4(arg,p1,p2,p3,p4)
+	//#define PRINTF                      printf
+	//#define PRINTF0(arg)                PRINTF(arg)
+	//#define PRINTF1(arg,p1)             PRINTF(arg,p1)
+	//#define PRINTF2(arg,p1,p2)          PRINTF(arg,p1,p2)
+	//#define PRINTF3(arg,p1,p2,p3)       PRINTF(arg,p1,p2,p3)
+	//#define PRINTF4(arg,p1,p2,p3,p4)    PRINTF(arg,p1,p2,p3,p4)
+#endif
+
+#ifdef ASSERTMSG
+#undef ASSERTMSG
+#endif
+
+#define ASSERTMSG(expr,string)  if (!(expr)) { \
+                                        MessageBox (NULL, string, "Assertion failed", MB_OK | MB_ICONERROR); \
+                                        exit (-1);}
+
+#elif (TARGET_SYSTEM == _NO_OS_)
+
+#include <stdlib.h>
+#include <stdio.h>
+
+    //29.11.2004 f.j. sonst ist memcpy und memset unbekannt
+//    #include <string.h>
+
+#define EPL_MEMCPY(dst,src,siz)     memcpy((void*)(dst),(const void*)(src),(size_t)(siz));
+#define EPL_MEMSET(dst,val,siz)     memset((void*)(dst),(int)(val),(size_t)(siz));
+
+#define EPL_MALLOC(siz)             malloc((size_t)(siz))
+#define EPL_FREE(ptr)               free((void *)ptr)
+
+#ifndef PRINTF0
+#define PRINTF                      TRACE
+#define PRINTF0(arg)                TRACE0(arg)
+#define PRINTF1(arg,p1)             TRACE1(arg,p1)
+#define PRINTF2(arg,p1,p2)          TRACE2(arg,p1,p2)
+#define PRINTF3(arg,p1,p2,p3)       TRACE3(arg,p1,p2,p3)
+#define PRINTF4(arg,p1,p2,p3,p4)    TRACE4(arg,p1,p2,p3,p4)
+	//#define PRINTF                      printf
+	//#define PRINTF0(arg)                PRINTF(arg)
+	//#define PRINTF1(arg,p1)             PRINTF(arg,p1)
+	//#define PRINTF2(arg,p1,p2)          PRINTF(arg,p1,p2)
+	//#define PRINTF3(arg,p1,p2,p3)       PRINTF(arg,p1,p2,p3)
+	//#define PRINTF4(arg,p1,p2,p3,p4)    PRINTF(arg,p1,p2,p3,p4)
+#endif
+
+#elif (TARGET_SYSTEM == _LINUX_)
+
+#ifndef __KERNEL__
+#include <stdlib.h>
+#include <stdio.h>
+#else
+//        #include <linux/config.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/major.h>
+#include <linux/version.h>
+#endif
+
+    //29.11.2004 f.j. sonst ist memcpy und memset unbekannt
+//    #include <string.h>
+
+#define EPL_MEMCPY(dst,src,siz)     memcpy((void*)(dst),(const void*)(src),(size_t)(siz));
+#define EPL_MEMSET(dst,val,siz)     memset((void*)(dst),(int)(val),(size_t)(siz));
+
+#ifndef __KERNEL__
+#define EPL_MALLOC(siz)             malloc((size_t)(siz))
+#define EPL_FREE(ptr)               free((void *)ptr)
+#else
+#define EPL_MALLOC(siz)             kmalloc((size_t)(siz), GFP_KERNEL)
+#define EPL_FREE(ptr)               kfree((void *)ptr)
+#endif
+
+#ifndef PRINTF0
+#define PRINTF                      TRACE
+#define PRINTF0(arg)                TRACE0(arg)
+#define PRINTF1(arg,p1)             TRACE1(arg,p1)
+#define PRINTF2(arg,p1,p2)          TRACE2(arg,p1,p2)
+#define PRINTF3(arg,p1,p2,p3)       TRACE3(arg,p1,p2,p3)
+#define PRINTF4(arg,p1,p2,p3,p4)    TRACE4(arg,p1,p2,p3,p4)
+	//#define PRINTF                      printf
+	//#define PRINTF0(arg)                PRINTF(arg)
+	//#define PRINTF1(arg,p1)             PRINTF(arg,p1)
+	//#define PRINTF2(arg,p1,p2)          PRINTF(arg,p1,p2)
+	//#define PRINTF3(arg,p1,p2,p3)       PRINTF(arg,p1,p2,p3)
+	//#define PRINTF4(arg,p1,p2,p3,p4)    PRINTF(arg,p1,p2,p3,p4)
+#endif
+
+#endif
+
+#define EPL_TGT_INTMASK_ETH     0x0001	// ethernet interrupt
+#define EPL_TGT_INTMASK_DMA     0x0002	// DMA interrupt
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+// currently no Timer functions are needed by EPL stack
+// so they are not implemented yet
+//void  PUBLIC TgtTimerInit(void);
+//DWORD PUBLIC TgtGetTickCount(void);
+//void PUBLIC TgtGetNetTime(tEplNetTime * pNetTime_p);
+
+// functions for ethernet driver
+tEplKernel PUBLIC TgtInitEthIsr(void);
+void PUBLIC TgtFreeEthIsr(void);
+void PUBLIC TgtEnableGlobalInterrupt(BYTE fEnable_p);
+void PUBLIC TgtEnableEthInterrupt0(BYTE fEnable_p,
+				   unsigned int uiInterruptMask_p);
+void PUBLIC TgtEnableEthInterrupt1(BYTE fEnable_p,
+				   unsigned int uiInterruptMask_p);
+
+#endif // #ifndef _EPLTARGET_H_
diff --git a/drivers/staging/epl/EplTimer.h b/drivers/staging/epl/EplTimer.h
new file mode 100644
index 00000000000..facbfd8740e
--- /dev/null
+++ b/drivers/staging/epl/EplTimer.h
@@ -0,0 +1,117 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for Epl Timer-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplTimer.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/07/06 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "EplInc.h"
+#include "EplEvent.h"
+
+#ifndef _EPLTIMER_H_
+#define _EPLTIMER_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+// type for timer handle
+typedef unsigned long tEplTimerHdl;
+
+typedef struct {
+	tEplEventSink m_EventSink;
+	unsigned long m_ulArg;	// d.k.: converted to unsigned long because
+	// it is never accessed as a pointer by the
+	// timer module and the data the
+	// pointer points to is not saved in any way.
+	// It is just a value. The user is responsible
+	// to store the data statically and convert
+	// the pointer between address spaces.
+
+} tEplTimerArg;
+
+typedef struct {
+	tEplTimerHdl m_TimerHdl;
+	unsigned long m_ulArg;	// d.k.: converted to unsigned long because
+	// it is never accessed as a pointer by the
+	// timer module and the data the
+	// pointer points to is not saved in any way.
+	// It is just a value.
+
+} tEplTimerEventArg;
+
+typedef tEplKernel(PUBLIC * tEplTimerkCallback) (tEplTimerEventArg *
+						 pEventArg_p);
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#endif // #ifndef _EPLTIMER_H_
diff --git a/drivers/staging/epl/EplTimeruLinuxKernel.c b/drivers/staging/epl/EplTimeruLinuxKernel.c
new file mode 100644
index 00000000000..08820d18405
--- /dev/null
+++ b/drivers/staging/epl/EplTimeruLinuxKernel.c
@@ -0,0 +1,446 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for EPL User Timermodule for Linux kernel module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplTimeruLinuxKernel.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.6 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                KEIL uVision 2
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/09/12 d.k.:   start of the implementation
+
+****************************************************************************/
+
+#include "user/EplTimeru.h"
+#include <linux/timer.h>
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+typedef struct {
+	struct timer_list m_Timer;
+	tEplTimerArg TimerArgument;
+
+} tEplTimeruData;
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+static void PUBLIC EplTimeruCbMs(unsigned long ulParameter_p);
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  <Epl Userspace-Timermodule for Linux Kernel>              */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description: Epl Userspace-Timermodule for Linux Kernel
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimeruInit
+//
+// Description: function inits first instance
+//
+// Parameters:  void
+//
+// Returns:     tEplKernel  = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplTimeruInit()
+{
+	tEplKernel Ret;
+
+	Ret = EplTimeruAddInstance();
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimeruAddInstance
+//
+// Description: function inits additional instance
+//
+// Parameters:  void
+//
+// Returns:     tEplKernel  = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplTimeruAddInstance()
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimeruDelInstance
+//
+// Description: function deletes instance
+//              -> under Linux nothing to do
+//              -> no instance table needed
+//
+// Parameters:  void
+//
+// Returns:     tEplKernel  = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplTimeruDelInstance()
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimeruSetTimerMs
+//
+// Description: function creates a timer and returns the corresponding handle
+//
+// Parameters:  pTimerHdl_p = pointer to a buffer to fill in the handle
+//              ulTime_p    = time for timer in ms
+//              Argument_p  = argument for timer
+//
+// Returns:     tEplKernel  = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplTimeruSetTimerMs(tEplTimerHdl * pTimerHdl_p,
+				      unsigned long ulTime_p,
+				      tEplTimerArg Argument_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplTimeruData *pData;
+
+	// check pointer to handle
+	if (pTimerHdl_p == NULL) {
+		Ret = kEplTimerInvalidHandle;
+		goto Exit;
+	}
+
+	pData = (tEplTimeruData *) EPL_MALLOC(sizeof(tEplTimeruData));
+	if (pData == NULL) {
+		Ret = kEplNoResource;
+		goto Exit;
+	}
+
+	init_timer(&pData->m_Timer);
+	pData->m_Timer.function = EplTimeruCbMs;
+	pData->m_Timer.data = (unsigned long)pData;
+	pData->m_Timer.expires = jiffies + ulTime_p * HZ / 1000;
+
+	EPL_MEMCPY(&pData->TimerArgument, &Argument_p, sizeof(tEplTimerArg));
+
+	add_timer(&pData->m_Timer);
+
+	*pTimerHdl_p = (tEplTimerHdl) pData;
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimeruModifyTimerMs
+//
+// Description: function changes a timer and returns the corresponding handle
+//
+// Parameters:  pTimerHdl_p = pointer to a buffer to fill in the handle
+//              ulTime_p    = time for timer in ms
+//              Argument_p  = argument for timer
+//
+// Returns:     tEplKernel  = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplTimeruModifyTimerMs(tEplTimerHdl * pTimerHdl_p,
+					 unsigned long ulTime_p,
+					 tEplTimerArg Argument_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplTimeruData *pData;
+
+	// check pointer to handle
+	if (pTimerHdl_p == NULL) {
+		Ret = kEplTimerInvalidHandle;
+		goto Exit;
+	}
+	// check handle itself, i.e. was the handle initialized before
+	if (*pTimerHdl_p == 0) {
+		Ret = EplTimeruSetTimerMs(pTimerHdl_p, ulTime_p, Argument_p);
+		goto Exit;
+	}
+	pData = (tEplTimeruData *) * pTimerHdl_p;
+	if ((tEplTimeruData *) pData->m_Timer.data != pData) {
+		Ret = kEplTimerInvalidHandle;
+		goto Exit;
+	}
+
+	mod_timer(&pData->m_Timer, (jiffies + ulTime_p * HZ / 1000));
+
+	// copy the TimerArg after the timer is restarted,
+	// so that a timer occured immediately before mod_timer
+	// won't use the new TimerArg and
+	// therefore the old timer cannot be distinguished from the new one.
+	// But if the new timer is too fast, it may get lost.
+	EPL_MEMCPY(&pData->TimerArgument, &Argument_p, sizeof(tEplTimerArg));
+
+	// check if timer is really running
+	if (timer_pending(&pData->m_Timer) == 0) {	// timer is not running
+		// retry starting it
+		add_timer(&pData->m_Timer);
+	}
+	// set handle to pointer of tEplTimeruData
+//    *pTimerHdl_p = (tEplTimerHdl) pData;
+
+      Exit:
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimeruDeleteTimer
+//
+// Description: function deletes a timer
+//
+// Parameters:  pTimerHdl_p = pointer to a buffer to fill in the handle
+//
+// Returns:     tEplKernel  = errorcode
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplTimeruDeleteTimer(tEplTimerHdl * pTimerHdl_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplTimeruData *pData;
+
+	// check pointer to handle
+	if (pTimerHdl_p == NULL) {
+		Ret = kEplTimerInvalidHandle;
+		goto Exit;
+	}
+	// check handle itself, i.e. was the handle initialized before
+	if (*pTimerHdl_p == 0) {
+		Ret = kEplSuccessful;
+		goto Exit;
+	}
+	pData = (tEplTimeruData *) * pTimerHdl_p;
+	if ((tEplTimeruData *) pData->m_Timer.data != pData) {
+		Ret = kEplTimerInvalidHandle;
+		goto Exit;
+	}
+
+/*    if (del_timer(&pData->m_Timer) == 1)
+    {
+        kfree(pData);
+    }
+*/
+	// try to delete the timer
+	del_timer(&pData->m_Timer);
+	// free memory in any case
+	kfree(pData);
+
+	// uninitialize handle
+	*pTimerHdl_p = 0;
+
+      Exit:
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimeruIsTimerActive
+//
+// Description: checks if the timer referenced by the handle is currently
+//              active.
+//
+// Parameters:  TimerHdl_p  = handle of the timer to check
+//
+// Returns:     BOOL        = TRUE, if active;
+//                            FALSE, otherwise
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+BOOL PUBLIC EplTimeruIsTimerActive(tEplTimerHdl TimerHdl_p)
+{
+	BOOL fActive = FALSE;
+	tEplTimeruData *pData;
+
+	// check handle itself, i.e. was the handle initialized before
+	if (TimerHdl_p == 0) {	// timer was not created yet, so it is not active
+		goto Exit;
+	}
+	pData = (tEplTimeruData *) TimerHdl_p;
+	if ((tEplTimeruData *) pData->m_Timer.data != pData) {	// invalid timer
+		goto Exit;
+	}
+	// check if timer is running
+	if (timer_pending(&pData->m_Timer) == 0) {	// timer is not running
+		goto Exit;
+	}
+
+	fActive = TRUE;
+
+      Exit:
+	return fActive;
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimeruCbMs
+//
+// Description: function to process timer
+//
+//
+//
+// Parameters:  lpParameter = pointer to structur of type tEplTimeruData
+//
+//
+// Returns:     (none)
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static void PUBLIC EplTimeruCbMs(unsigned long ulParameter_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplTimeruData *pData;
+	tEplEvent EplEvent;
+	tEplTimerEventArg TimerEventArg;
+
+	pData = (tEplTimeruData *) ulParameter_p;
+
+	// call event function
+	TimerEventArg.m_TimerHdl = (tEplTimerHdl) pData;
+	TimerEventArg.m_ulArg = pData->TimerArgument.m_ulArg;
+
+	EplEvent.m_EventSink = pData->TimerArgument.m_EventSink;
+	EplEvent.m_EventType = kEplEventTypeTimer;
+	EPL_MEMSET(&EplEvent.m_NetTime, 0x00, sizeof(tEplNetTime));
+	EplEvent.m_pArg = &TimerEventArg;
+	EplEvent.m_uiSize = sizeof(TimerEventArg);
+
+	Ret = EplEventuPost(&EplEvent);
+
+	// d.k. do not free memory, user has to call EplTimeruDeleteTimer()
+	//kfree(pData);
+
+}
+
+// EOF
diff --git a/drivers/staging/epl/EplTimeruNull.c b/drivers/staging/epl/EplTimeruNull.c
new file mode 100644
index 00000000000..40ce403cbd0
--- /dev/null
+++ b/drivers/staging/epl/EplTimeruNull.c
@@ -0,0 +1,312 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for Epl Userspace-Timermodule NULL-Implementation
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplTimeruNull.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.3 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                KEIL uVision 2
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/07/06 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "user/EplTimeru.h"
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  <Epl Userspace-Timermodule NULL-Implementation>     */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description: Epl Userspace-Timermodule NULL-Implementation
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimeruInit
+//
+// Description: function init first instance
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplTimeruInit()
+{
+	tEplKernel Ret;
+
+	Ret = EplTimeruAddInstance();
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimeruAddInstance
+//
+// Description: function init additional instance
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplTimeruAddInstance()
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimeruDelInstance
+//
+// Description: function delte instance
+//              -> under Win32 nothing to do
+//              -> no instnace table needed
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplTimeruDelInstance()
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimeruSetTimerMs
+//
+// Description: function create a timer and return a handle to the pointer
+//
+//
+//
+// Parameters:  pTimerHdl_p = pointer to a buffer to fill in the handle
+//              ulTime_p    = time for timer in ms
+//              Argument_p  = argument for timer
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplTimeruSetTimerMs(tEplTimerHdl * pTimerHdl_p,
+				      unsigned long ulTime_p,
+				      tEplTimerArg Argument_p)
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// check handle
+	if (pTimerHdl_p == NULL) {
+		Ret = kEplTimerInvalidHandle;
+		goto Exit;
+	}
+
+      Exit:
+	return Ret;
+}
+
+ //---------------------------------------------------------------------------
+//
+// Function:    EplTimeruModifyTimerMs
+//
+// Description: function change a timer and return a handle to the pointer
+//
+//
+//
+// Parameters:  pTimerHdl_p = pointer to a buffer to fill in the handle
+//              ulTime_p    = time for timer in ms
+//              Argument_p  = argument for timer
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplTimeruModifyTimerMs(tEplTimerHdl * pTimerHdl_p,
+					 unsigned long ulTime_p,
+					 tEplTimerArg Argument_p)
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// check parameter
+	if (pTimerHdl_p == NULL) {
+		Ret = kEplTimerInvalidHandle;
+		goto Exit;
+	}
+
+      Exit:
+	return Ret;
+}
+
+ //---------------------------------------------------------------------------
+//
+// Function:    EplTimeruDeleteTimer
+//
+// Description: function delte a timer
+//
+//
+//
+// Parameters:  pTimerHdl_p = pointer to a buffer to fill in the handle
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplTimeruDeleteTimer(tEplTimerHdl * pTimerHdl_p)
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// check parameter
+	if (pTimerHdl_p == NULL) {
+		Ret = kEplTimerInvalidHandle;
+		goto Exit;
+	}
+	// set handle invalide
+	*pTimerHdl_p = 0;
+
+      Exit:
+	return Ret;
+
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+// EOF
diff --git a/drivers/staging/epl/EplTimeruWin32.c b/drivers/staging/epl/EplTimeruWin32.c
new file mode 100644
index 00000000000..a967b4e59d4
--- /dev/null
+++ b/drivers/staging/epl/EplTimeruWin32.c
@@ -0,0 +1,513 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  source file for Epl Userspace-Timermodule for Win32
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplTimeruWin32.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/07/06 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "user/EplTimeru.h"
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+typedef struct {
+	tEplTimerArg TimerArgument;
+	HANDLE DelteHandle;
+	unsigned long ulTimeout;
+
+} tEplTimeruThread;
+
+typedef struct {
+	LPCRITICAL_SECTION m_pCriticalSection;
+	CRITICAL_SECTION m_CriticalSection;
+} tEplTimeruInstance;
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+static tEplTimeruInstance EplTimeruInstance_g;
+static tEplTimeruThread ThreadData_l;
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+DWORD PUBLIC EplSdoTimeruThreadms(LPVOID lpParameter);
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          C L A S S  <Epl Userspace-Timermodule for Win32>               */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+//
+// Description: Epl Userspace-Timermodule for Win32
+//
+//
+/***************************************************************************/
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimeruInit
+//
+// Description: function init first instance
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplTimeruInit()
+{
+	tEplKernel Ret;
+
+	Ret = EplTimeruAddInstance();
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimeruAddInstance
+//
+// Description: function init additional instance
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplTimeruAddInstance()
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	// create critical section
+	EplTimeruInstance_g.m_pCriticalSection =
+	    &EplTimeruInstance_g.m_CriticalSection;
+	InitializeCriticalSection(EplTimeruInstance_g.m_pCriticalSection);
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimeruDelInstance
+//
+// Description: function delte instance
+//              -> under Win32 nothing to do
+//              -> no instnace table needed
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplTimeruDelInstance()
+{
+	tEplKernel Ret;
+
+	Ret = kEplSuccessful;
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimeruSetTimerMs
+//
+// Description: function create a timer and return a handle to the pointer
+//
+//
+//
+// Parameters:  pTimerHdl_p = pointer to a buffer to fill in the handle
+//              ulTime_p    = time for timer in ms
+//              Argument_p  = argument for timer
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplTimeruSetTimerMs(tEplTimerHdl * pTimerHdl_p,
+				      unsigned long ulTime_p,
+				      tEplTimerArg Argument_p)
+{
+	tEplKernel Ret;
+	HANDLE DeleteHandle;
+	HANDLE ThreadHandle;
+	DWORD ThreadId;
+
+	Ret = kEplSuccessful;
+
+	// check handle
+	if (pTimerHdl_p == NULL) {
+		Ret = kEplTimerInvalidHandle;
+		goto Exit;
+	}
+	// enter  critical section
+	EnterCriticalSection(EplTimeruInstance_g.m_pCriticalSection);
+
+	// first create event to delete timer
+	DeleteHandle = CreateEvent(NULL, FALSE, FALSE, NULL);
+	if (DeleteHandle == NULL) {
+		Ret = kEplTimerNoTimerCreated;
+		goto Exit;
+	}
+	// set handle for caller
+	*pTimerHdl_p = (tEplTimerHdl) DeleteHandle;
+
+	// fill data for thread
+	ThreadData_l.DelteHandle = DeleteHandle;
+	EPL_MEMCPY(&ThreadData_l.TimerArgument, &Argument_p,
+		   sizeof(tEplTimerArg));
+	ThreadData_l.ulTimeout = ulTime_p;
+
+	// create thread to create waitable timer and wait for timer
+	ThreadHandle = CreateThread(NULL,
+				    0,
+				    EplSdoTimeruThreadms,
+				    &ThreadData_l, 0, &ThreadId);
+	if (ThreadHandle == NULL) {
+		// leave critical section
+		LeaveCriticalSection(EplTimeruInstance_g.m_pCriticalSection);
+
+		// delte handle
+		CloseHandle(DeleteHandle);
+
+		Ret = kEplTimerNoTimerCreated;
+		goto Exit;
+	}
+
+      Exit:
+	return Ret;
+}
+
+ //---------------------------------------------------------------------------
+//
+// Function:    EplTimeruModifyTimerMs
+//
+// Description: function change a timer and return a handle to the pointer
+//
+//
+//
+// Parameters:  pTimerHdl_p = pointer to a buffer to fill in the handle
+//              ulTime_p    = time for timer in ms
+//              Argument_p  = argument for timer
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplTimeruModifyTimerMs(tEplTimerHdl * pTimerHdl_p,
+					 unsigned long ulTime_p,
+					 tEplTimerArg Argument_p)
+{
+	tEplKernel Ret;
+	HANDLE DeleteHandle;
+	HANDLE ThreadHandle;
+	DWORD ThreadId;
+
+	Ret = kEplSuccessful;
+
+	// check parameter
+	if (pTimerHdl_p == NULL) {
+		Ret = kEplTimerInvalidHandle;
+		goto Exit;
+	}
+
+	DeleteHandle = (HANDLE) (*pTimerHdl_p);
+
+	// set event to end timer task for this timer
+	SetEvent(DeleteHandle);
+
+	// create new timer
+	// first create event to delete timer
+	DeleteHandle = CreateEvent(NULL, FALSE, FALSE, NULL);
+	if (DeleteHandle == NULL) {
+		Ret = kEplTimerNoTimerCreated;
+		goto Exit;
+	}
+	// set handle for caller
+	*pTimerHdl_p = (tEplTimerHdl) DeleteHandle;
+
+	// enter  critical section
+	EnterCriticalSection(EplTimeruInstance_g.m_pCriticalSection);
+
+	// fill data for thread
+	ThreadData_l.DelteHandle = DeleteHandle;
+	EPL_MEMCPY(&ThreadData_l.TimerArgument, &Argument_p,
+		   sizeof(tEplTimerArg));
+	ThreadData_l.ulTimeout = ulTime_p;
+
+	// create thread to create waitable timer and wait for timer
+	ThreadHandle = CreateThread(NULL,
+				    0,
+				    EplSdoTimeruThreadms,
+				    &ThreadData_l, 0, &ThreadId);
+	if (ThreadHandle == NULL) {
+		// leave critical section
+		LeaveCriticalSection(EplTimeruInstance_g.m_pCriticalSection);
+
+		// delte handle
+
+		Ret = kEplTimerNoTimerCreated;
+		goto Exit;
+	}
+
+      Exit:
+	return Ret;
+}
+
+ //---------------------------------------------------------------------------
+//
+// Function:    EplTimeruDeleteTimer
+//
+// Description: function delte a timer
+//
+//
+//
+// Parameters:  pTimerHdl_p = pointer to a buffer to fill in the handle
+//
+//
+// Returns:     tEplKernel  = errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplTimeruDeleteTimer(tEplTimerHdl * pTimerHdl_p)
+{
+	tEplKernel Ret;
+	HANDLE DeleteHandle;
+
+	Ret = kEplSuccessful;
+
+	// check parameter
+	if (pTimerHdl_p == NULL) {
+		Ret = kEplTimerInvalidHandle;
+		goto Exit;
+	}
+
+	DeleteHandle = (HANDLE) (*pTimerHdl_p);
+
+	// set event to end timer task for this timer
+	SetEvent(DeleteHandle);
+
+	// set handle invalide
+	*pTimerHdl_p = 0;
+
+      Exit:
+	return Ret;
+
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplSdoTimeruThreadms
+//
+// Description: function to process timer as thread
+//
+//
+//
+// Parameters:  lpParameter = pointer to structur of type tEplTimeruThread
+//
+//
+// Returns:     DWORD = Errorcode
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+DWORD PUBLIC EplSdoTimeruThreadms(LPVOID lpParameter)
+{
+	tEplKernel Ret;
+	tEplTimeruThread *pThreadData;
+	HANDLE aHandles[2];
+	BOOL fReturn;
+	LARGE_INTEGER TimeoutTime;
+	unsigned long ulEvent;
+	tEplEvent EplEvent;
+	tEplTimeruThread ThreadData;
+	tEplTimerEventArg TimerEventArg;
+
+	Ret = kEplSuccessful;
+
+	// get pointer to data
+	pThreadData = (tEplTimeruThread *) lpParameter;
+	// copy thread data
+	EPL_MEMCPY(&ThreadData, pThreadData, sizeof(ThreadData));
+	pThreadData = &ThreadData;
+
+	// leave critical section
+	LeaveCriticalSection(EplTimeruInstance_g.m_pCriticalSection);
+
+	// create waitable timer
+	aHandles[1] = CreateWaitableTimer(NULL, FALSE, NULL);
+	if (aHandles[1] == NULL) {
+		Ret = kEplTimerNoTimerCreated;
+		goto Exit;
+	}
+	// set timer
+	// set timeout interval -> needed to be negativ
+	// -> because relative timeout
+	// -> multiply by 10000 for 100 ns timebase of function
+	TimeoutTime.QuadPart = (((long long)pThreadData->ulTimeout) * -10000);
+	fReturn = SetWaitableTimer(aHandles[1],
+				   &TimeoutTime, 0, NULL, NULL, FALSE);
+	if (fReturn == 0) {
+		Ret = kEplTimerNoTimerCreated;
+		goto Exit;
+	}
+	// save delte event handle in handle array
+	aHandles[0] = pThreadData->DelteHandle;
+
+	// wait for one of the events
+	ulEvent = WaitForMultipleObjects(2, &aHandles[0], FALSE, INFINITE);
+	if (ulEvent == WAIT_OBJECT_0) {	// delte event
+
+		// close handels
+		CloseHandle(aHandles[1]);
+		// terminate thread
+		goto Exit;
+	} else if (ulEvent == (WAIT_OBJECT_0 + 1)) {	// timer event
+		// call event function
+		TimerEventArg.m_TimerHdl =
+		    (tEplTimerHdl) pThreadData->DelteHandle;
+		TimerEventArg.m_ulArg = pThreadData->TimerArgument.m_ulArg;
+
+		EplEvent.m_EventSink = pThreadData->TimerArgument.m_EventSink;
+		EplEvent.m_EventType = kEplEventTypeTimer;
+		EPL_MEMSET(&EplEvent.m_NetTime, 0x00, sizeof(tEplNetTime));
+		EplEvent.m_pArg = &TimerEventArg;
+		EplEvent.m_uiSize = sizeof(TimerEventArg);
+
+		Ret = EplEventuPost(&EplEvent);
+
+		// close handels
+		CloseHandle(aHandles[1]);
+		// terminate thread
+		goto Exit;
+
+	} else {		// error
+		ulEvent = GetLastError();
+		TRACE1("Error in WaitForMultipleObjects Errorcode: 0x%x\n",
+		       ulEvent);
+		// terminate thread
+		goto Exit;
+	}
+
+      Exit:
+	return Ret;
+}
+
+// EOF
diff --git a/drivers/staging/epl/EplVersion.h b/drivers/staging/epl/EplVersion.h
new file mode 100644
index 00000000000..75570d56b86
--- /dev/null
+++ b/drivers/staging/epl/EplVersion.h
@@ -0,0 +1,98 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  This file defines the EPL version for the stack, as string
+                and for object 0x1018 within object dictionary.
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplVersion.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.6 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    all
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+****************************************************************************/
+
+#ifndef _EPL_VERSION_H_
+#define _EPL_VERSION_H_
+
+// NOTE:
+// All version macros should contain the same version number. But do not use
+// defines instead of the numbers. Because the macro EPL_STRING_VERSION() can not
+// convert a define to a string.
+//
+// Format: maj.min.build
+//         maj            = major version
+//             min        = minor version (will be set to 0 if major version will be incremented)
+//                 build  = current build (will be set to 0 if minor version will be incremented)
+//
+#define DEFINED_STACK_VERSION       EPL_STACK_VERSION   (1, 3, 0)
+#define DEFINED_OBJ1018_VERSION     EPL_OBJ1018_VERSION (1, 3, 0)
+#define DEFINED_STRING_VERSION      EPL_STRING_VERSION  (1, 3, 0)
+
+// -----------------------------------------------------------------------------
+#define EPL_PRODUCT_NAME            "EPL V2"
+#define EPL_PRODUCT_VERSION         DEFINED_STRING_VERSION
+#define EPL_PRODUCT_MANUFACTURER    "SYS TEC electronic GmbH"
+
+#define EPL_PRODUCT_KEY         "SO-1083"
+#define EPL_PRODUCT_DESCRIPTION "openPOWERLINK Protocol Stack Source"
+
+#endif // _EPL_VERSION_H_
+
+// Die letzte Zeile muß unbedingt eine leere Zeile sein, weil manche Compiler
+// damit ein Problem haben, wenn das nicht so ist (z.B. GNU oder Borland C++ Builder).
diff --git a/drivers/staging/epl/Kconfig b/drivers/staging/epl/Kconfig
new file mode 100644
index 00000000000..9f939d5874a
--- /dev/null
+++ b/drivers/staging/epl/Kconfig
@@ -0,0 +1,6 @@
+config EPL
+	tristate "openPOWERLINK protocol stack"
+	depends on NET && HIGH_RES_TIMERS && X86
+	default N
+	---help---
+	  Enable support for the openPOWERLINK network protocol stack.
diff --git a/drivers/staging/epl/Makefile b/drivers/staging/epl/Makefile
new file mode 100644
index 00000000000..a2c824187d2
--- /dev/null
+++ b/drivers/staging/epl/Makefile
@@ -0,0 +1,41 @@
+obj-$(CONFIG_EPL)	+= epl.o
+
+epl-objs := 	\
+	EplApiGeneric.o		\
+	EplApiLinuxKernel.o	\
+	EplApiProcessImage.o	\
+	EplDllk.o		\
+	EplDllkCal.o		\
+	EplDlluCal.o		\
+	EplErrorHandlerk.o	\
+	EplEventk.o		\
+	EplEventu.o		\
+	EplIdentu.o		\
+	EplNmtCnu.o		\
+	EplNmtk.o		\
+	EplNmtkCal.o		\
+	EplNmtMnu.o		\
+	EplNmtu.o		\
+	EplNmtuCal.o		\
+	EplObd.o		\
+	EplObdkCal.o		\
+	EplObdu.o		\
+	EplObduCal.o		\
+	EplPdok.o		\
+	EplPdokCal.o		\
+	EplPdou.o		\
+	EplSdoAsndu.o		\
+	EplSdoAsySequ.o		\
+	EplSdoComu.o		\
+	EplSdoUdpu.o		\
+	EplStatusu.o		\
+	EplTimeruLinuxKernel.o	\
+	amix86.o		\
+	SharedBuff.o		\
+	ShbIpc-LinuxKernel.o	\
+	TimerHighReskX86.o	\
+	VirtualEthernetLinux.o	\
+	SocketLinuxKernel.o	\
+	proc_fs.o		\
+	demo_main.o		\
+	Edrv8139.o		\
diff --git a/drivers/staging/epl/SharedBuff.c b/drivers/staging/epl/SharedBuff.c
new file mode 100644
index 00000000000..9fb09d6bc28
--- /dev/null
+++ b/drivers/staging/epl/SharedBuff.c
@@ -0,0 +1,1799 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      Project independend shared buffer (linear + circular)
+
+  Description:  Implementation of platform independend part for the
+                shared buffer
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+  2006/06/27 -rs:   V 1.00 (initial version)
+
+****************************************************************************/
+
+#if defined(WIN32) || defined(_WIN32)
+
+#ifdef UNDER_RTSS
+	// RTX header
+#include <windows.h>
+#include <process.h>
+#include <rtapi.h>
+
+#elif __BORLANDC__
+	// borland C header
+#include <windows.h>
+#include <process.h>
+
+#elif WINCE
+#include <windows.h>
+
+#else
+	// MSVC needs to include windows.h at first
+	// the following defines ar necessary for function prototypes for waitable timers
+#define _WIN32_WINDOWS 0x0401
+#define _WIN32_WINNT   0x0400
+#include <windows.h>
+#include <process.h>
+#endif
+
+#endif
+
+#include "global.h"
+#include "SharedBuff.h"
+#include "ShbIpc.h"
+
+// d.k. Linux kernel modules needs other header files for memcpy()
+#if (TARGET_SYSTEM == _LINUX_) && defined(__KERNEL__)
+#include <linux/string.h>
+#else
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+#if (!defined(SHAREDBUFF_INLINED)) || defined(INLINE_ENABLED)
+
+//---------------------------------------------------------------------------
+//  Configuration
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+//  Constant definitions
+//---------------------------------------------------------------------------
+
+#define SBC_MAGIC_ID    0x53424323	// magic ID ("SBC#")
+#define SBL_MAGIC_ID    0x53424C23	// magic ID ("SBL#")
+
+//---------------------------------------------------------------------------
+//  Local types
+//---------------------------------------------------------------------------
+
+// structure to administrate circular shared buffer head
+typedef struct {
+	unsigned long m_ShbCirMagicID;	// magic ID ("SBC#")
+	unsigned long m_ulBufferTotalSize;	// over-all size of complete buffer
+	unsigned long m_ulBufferDataSize;	// size of complete data area
+	unsigned long m_ulWrIndex;	// current write index (set bevore write)
+	unsigned long m_ulRdIndex;	// current read index (set after read)
+	unsigned long m_ulNumOfWriteJobs;	// number of currently (parallel running) write operations
+	unsigned long m_ulDataInUse;	// currently used buffer size (incl. uncompleted write operations)
+	unsigned long m_ulDataApended;	// buffer size of complete new written but not yet readable data (in case of m_ulNumOfWriteJobs>1)
+	unsigned long m_ulBlocksApended;	// number of complete new written but not yet readable data blocks (in case of m_ulNumOfWriteJobs>1)
+	unsigned long m_ulDataReadable;	// buffer size with readable (complete written) data
+	unsigned long m_ulBlocksReadable;	// number of readable (complete written) data blocks
+	tShbCirSigHndlrNewData m_pfnSigHndlrNewData;	// application handler to signal new data
+	unsigned int m_fBufferLocked;	// TRUE if buffer is locked (because of pending reset request)
+	tShbCirSigHndlrReset m_pfnSigHndlrReset;	// application handler to signal buffer reset is done
+	unsigned char m_Data;	// start of data area (the real data size is unknown at this time)
+
+} tShbCirBuff;
+
+// structure to administrate linear shared buffer head
+typedef struct {
+	unsigned int m_ShbLinMagicID;	// magic ID ("SBL#")
+	unsigned long m_ulBufferTotalSize;	// over-all size of complete buffer
+	unsigned long m_ulBufferDataSize;	// size of complete data area
+	unsigned char m_Data;	// start of data area (the real data size is unknown at this time)
+
+} tShbLinBuff;
+
+// type to save size of a single data block inside the circular shared buffer
+typedef struct {
+	unsigned int m_uiFullBlockSize:28;	// a single block must not exceed a length of 256MByte :-)
+	unsigned int m_uiAlignFillBytes:4;
+
+} tShbCirBlockSize;
+
+#define SBC_BLOCK_ALIGNMENT                  4	// alignment must *not* be lower than sizeof(tShbCirBlockSize)!
+#define SBC_MAX_BLOCK_SIZE         ((1<<28)-1)	// = (2^28 - 1) = (256MByte - 1) -> should be enought for real life :-)
+
+#define SBL_BLOCK_ALIGNMENT                  4
+#define SBL_MAX_BLOCK_SIZE         ((1<<28)-1)	// = (2^28 - 1) = (256MByte - 1) -> should be enought for real life :-)
+
+//---------------------------------------------------------------------------
+//  Global variables
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+//  Local variables
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+//  Prototypes of internal functions
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+//  Get pointer to Circular Shared Buffer
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbCirBuff *ShbCirGetBuffer(tShbInstance pShbInstance_p)
+{
+
+	tShbCirBuff *pShbCirBuff;
+
+	pShbCirBuff = (tShbCirBuff *) ShbIpcGetShMemPtr(pShbInstance_p);
+	ASSERT(pShbCirBuff->m_ShbCirMagicID == SBC_MAGIC_ID);
+
+	return (pShbCirBuff);
+
+}
+
+//---------------------------------------------------------------------------
+//  Get pointer to Linear Shared Buffer
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbLinBuff *ShbLinGetBuffer(tShbInstance pShbInstance_p)
+{
+
+	tShbLinBuff *pShbLinBuff;
+
+	pShbLinBuff = (tShbLinBuff *) ShbIpcGetShMemPtr(pShbInstance_p);
+	ASSERT(pShbLinBuff->m_ShbLinMagicID == SBL_MAGIC_ID);
+
+	return (pShbLinBuff);
+
+}
+
+// not inlined internal functions
+int ShbCirSignalHandlerNewData(tShbInstance pShbInstance_p);
+void ShbCirSignalHandlerReset(tShbInstance pShbInstance_p,
+			      unsigned int fTimeOut_p);
+
+#endif
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+#if !defined(INLINE_ENABLED)
+// not inlined external functions
+
+//---------------------------------------------------------------------------
+//  Initialize Shared Buffer Module
+//---------------------------------------------------------------------------
+
+tShbError ShbInit(void)
+{
+
+	tShbError ShbError;
+
+	ShbError = ShbIpcInit();
+
+	return (ShbError);
+
+}
+
+//---------------------------------------------------------------------------
+//  Deinitialize Shared Buffer Module
+//---------------------------------------------------------------------------
+
+tShbError ShbExit(void)
+{
+
+	tShbError ShbError;
+
+	ShbError = ShbIpcExit();
+
+	return (ShbError);
+
+}
+
+//-------------------------------------------------------------------------//
+//                                                                         //
+//          C i r c u l a r   S h a r e d   B u f f e r                    //
+//                                                                         //
+//-------------------------------------------------------------------------//
+
+//---------------------------------------------------------------------------
+//  Allocate Circular Shared Buffer
+//---------------------------------------------------------------------------
+
+tShbError ShbCirAllocBuffer(unsigned long ulBufferSize_p,
+			    const char *pszBufferID_p,
+			    tShbInstance * ppShbInstance_p,
+			    unsigned int *pfShbNewCreated_p)
+{
+
+	tShbInstance pShbInstance;
+	tShbCirBuff *pShbCirBuff;
+	unsigned int fShbNewCreated;
+	unsigned long ulBufferDataSize;
+	unsigned long ulBufferTotalSize;
+	tShbError ShbError;
+
+	// check arguments
+	if ((ulBufferSize_p == 0) || (ppShbInstance_p == NULL)) {
+		return (kShbInvalidArg);
+	}
+
+	// calculate length of memory to allocate
+	ulBufferDataSize =
+	    (ulBufferSize_p +
+	     (SBC_BLOCK_ALIGNMENT - 1)) & ~(SBC_BLOCK_ALIGNMENT - 1);
+	ulBufferTotalSize = ulBufferDataSize + sizeof(tShbCirBuff);
+
+	// allocate a new or open an existing shared buffer
+	ShbError = ShbIpcAllocBuffer(ulBufferTotalSize, pszBufferID_p,
+				     &pShbInstance, &fShbNewCreated);
+	if (ShbError != kShbOk) {
+		goto Exit;
+	}
+
+	if (pShbInstance == NULL) {
+		ShbError = kShbOutOfMem;
+		goto Exit;
+	}
+
+	// get pointer to shared buffer
+	pShbCirBuff = (tShbCirBuff *) ShbIpcGetShMemPtr(pShbInstance);
+
+	// if the shared buffer was new created, than this process has
+	// to initialize it, otherwise the buffer is already in use
+	// and *must not* be reseted
+	if (fShbNewCreated) {
+#ifndef NDEBUG
+		{
+			memset(pShbCirBuff, 0xCC, ulBufferTotalSize);
+		}
+#endif
+
+		pShbCirBuff->m_ShbCirMagicID = SBC_MAGIC_ID;
+		pShbCirBuff->m_ulBufferTotalSize = ulBufferTotalSize;
+		pShbCirBuff->m_ulBufferDataSize = ulBufferDataSize;
+		pShbCirBuff->m_ulWrIndex = 0;
+		pShbCirBuff->m_ulRdIndex = 0;
+		pShbCirBuff->m_ulNumOfWriteJobs = 0;
+		pShbCirBuff->m_ulDataInUse = 0;
+		pShbCirBuff->m_ulDataApended = 0;
+		pShbCirBuff->m_ulBlocksApended = 0;
+		pShbCirBuff->m_ulDataReadable = 0;
+		pShbCirBuff->m_ulBlocksReadable = 0;
+		pShbCirBuff->m_pfnSigHndlrNewData = NULL;
+		pShbCirBuff->m_fBufferLocked = FALSE;
+		pShbCirBuff->m_pfnSigHndlrReset = NULL;
+	} else {
+		if (pShbCirBuff->m_ShbCirMagicID != SBC_MAGIC_ID) {
+			ShbError = kShbInvalidBufferType;
+			goto Exit;
+		}
+	}
+
+      Exit:
+
+	*ppShbInstance_p = pShbInstance;
+	*pfShbNewCreated_p = fShbNewCreated;
+
+	return (ShbError);
+
+}
+
+//---------------------------------------------------------------------------
+//  Release Circular Shared Buffer
+//---------------------------------------------------------------------------
+
+tShbError ShbCirReleaseBuffer(tShbInstance pShbInstance_p)
+{
+
+	tShbError ShbError;
+
+	// check arguments
+	if (pShbInstance_p == NULL) {
+		ShbError = kShbOk;
+		goto Exit;
+	}
+
+	ShbError = ShbIpcReleaseBuffer(pShbInstance_p);
+
+      Exit:
+
+	return (ShbError);
+
+}
+
+#endif // !defined(INLINE_ENABLED)
+
+#if (!defined(SHAREDBUFF_INLINED)) || defined(INLINE_ENABLED)
+
+//---------------------------------------------------------------------------
+//  Reset Circular Shared Buffer
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbCirResetBuffer(tShbInstance pShbInstance_p,
+					    unsigned long ulTimeOut_p,
+					    tShbCirSigHndlrReset
+					    pfnSignalHandlerReset_p)
+{
+
+	tShbCirBuff *pShbCirBuff;
+	unsigned long ulNumOfWriteJobs = 0;	// d.k. GCC complains about uninitialized variable otherwise
+	tShbError ShbError;
+
+	// check arguments
+	if (pShbInstance_p == NULL) {
+		ShbError = kShbInvalidArg;
+		goto Exit;
+	}
+
+	pShbCirBuff = ShbCirGetBuffer(pShbInstance_p);
+	ShbError = kShbOk;
+
+	if (pShbCirBuff->m_ShbCirMagicID != SBC_MAGIC_ID) {
+		ShbError = kShbInvalidBufferType;
+		goto Exit;
+	}
+
+	// start reset job by setting request request in buffer header
+	ShbIpcEnterAtomicSection(pShbInstance_p);
+	{
+		if (!pShbCirBuff->m_fBufferLocked) {
+			ulNumOfWriteJobs = pShbCirBuff->m_ulNumOfWriteJobs;
+
+			pShbCirBuff->m_fBufferLocked = TRUE;
+			pShbCirBuff->m_pfnSigHndlrReset =
+			    pfnSignalHandlerReset_p;
+		} else {
+			ShbError = kShbAlreadyReseting;
+		}
+	}
+	ShbIpcLeaveAtomicSection(pShbInstance_p);
+
+	if (ShbError != kShbOk) {
+		goto Exit;
+	}
+
+	// if there is currently no running write operation then reset buffer
+	// immediately, otherwise wait until the last write job is ready by
+	// starting a signal process
+	if (ulNumOfWriteJobs == 0) {
+		// there is currently no running write operation
+		// -> reset buffer immediately
+		ShbCirSignalHandlerReset(pShbInstance_p, FALSE);
+		ShbError = kShbOk;
+	} else {
+		// there is currently at least one running write operation
+		// -> starting signal process to wait until the last write job is ready
+		ShbError =
+		    ShbIpcStartSignalingJobReady(pShbInstance_p, ulTimeOut_p,
+						 ShbCirSignalHandlerReset);
+	}
+
+      Exit:
+
+	return (ShbError);
+
+}
+
+//---------------------------------------------------------------------------
+//  Write data block to Circular Shared Buffer
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbCirWriteDataBlock(tShbInstance pShbInstance_p,
+					       const void *pSrcDataBlock_p,
+					       unsigned long ulDataBlockSize_p)
+{
+
+	tShbCirBuff *pShbCirBuff;
+	tShbCirBlockSize ShbCirBlockSize;
+	unsigned int uiFullBlockSize;
+	unsigned int uiAlignFillBytes;
+	unsigned char *pShbCirDataPtr;
+	unsigned char *pScrDataPtr;
+	unsigned long ulDataSize;
+	unsigned long ulChunkSize;
+	unsigned long ulWrIndex = 0;	// d.k. GCC complains about uninitialized variable otherwise
+	unsigned int fSignalNewData;
+	unsigned int fSignalReset;
+	tShbError ShbError;
+	tShbError ShbError2;
+	int fRes;
+
+	// check arguments
+	if (pShbInstance_p == NULL) {
+		ShbError = kShbInvalidArg;
+		goto Exit;
+	}
+
+	if ((pSrcDataBlock_p == NULL) || (ulDataBlockSize_p == 0)) {
+		// nothing to do here
+		ShbError = kShbOk;
+		goto Exit;
+	}
+
+	if (ulDataBlockSize_p > SBC_MAX_BLOCK_SIZE) {
+		ShbError = kShbExceedDataSizeLimit;
+		goto Exit;
+	}
+
+	pShbCirBuff = ShbCirGetBuffer(pShbInstance_p);
+	pScrDataPtr = (unsigned char *)pSrcDataBlock_p;
+	fSignalNewData = FALSE;
+	fSignalReset = FALSE;
+	ShbError = kShbOk;
+
+	if (pShbCirBuff->m_ShbCirMagicID != SBC_MAGIC_ID) {
+		ShbError = kShbInvalidBufferType;
+		goto Exit;
+	}
+
+	// calculate data block size in circular buffer
+	ulDataSize =
+	    (ulDataBlockSize_p +
+	     (SBC_BLOCK_ALIGNMENT - 1)) & ~(SBC_BLOCK_ALIGNMENT - 1);
+	uiFullBlockSize = ulDataSize + sizeof(tShbCirBlockSize);	// data size + header
+	uiAlignFillBytes = ulDataSize - ulDataBlockSize_p;
+
+	ShbCirBlockSize.m_uiFullBlockSize = uiFullBlockSize;
+	ShbCirBlockSize.m_uiAlignFillBytes = uiAlignFillBytes;
+
+	// reserve the needed memory for the write operation to do now
+	// and make necessary adjustments in the circular buffer header
+	ShbIpcEnterAtomicSection(pShbInstance_p);
+	{
+		// check if there is sufficient memory available to store
+		// the new data
+		fRes =
+		    uiFullBlockSize <=
+		    (pShbCirBuff->m_ulBufferDataSize -
+		     pShbCirBuff->m_ulDataInUse);
+		if (fRes) {
+			// set write pointer for the write operation to do now
+			// to the current write pointer of the circular buffer
+			ulWrIndex = pShbCirBuff->m_ulWrIndex;
+
+			// reserve the needed memory for the write operation to do now
+			pShbCirBuff->m_ulDataInUse += uiFullBlockSize;
+
+			// set new write pointer behind the reserved memory
+			// for the write operation to do now
+			pShbCirBuff->m_ulWrIndex += uiFullBlockSize;
+			pShbCirBuff->m_ulWrIndex %=
+			    pShbCirBuff->m_ulBufferDataSize;
+
+			// increment number of currently (parallel running)
+			// write operations
+			pShbCirBuff->m_ulNumOfWriteJobs++;
+		}
+	}
+	ShbIpcLeaveAtomicSection(pShbInstance_p);
+
+	if (!fRes) {
+		ShbError = kShbBufferFull;
+		goto Exit;
+	}
+
+	// copy the data to the circular buffer
+	// (the copy process itself will be done outside of any
+	// critical/locked section)
+	pShbCirDataPtr = &pShbCirBuff->m_Data;	// ptr to start of data area
+
+	// write real size of current block (incl. alignment fill bytes)
+	*(tShbCirBlockSize *) (pShbCirDataPtr + ulWrIndex) = ShbCirBlockSize;
+	ulWrIndex += sizeof(tShbCirBlockSize);
+	ulWrIndex %= pShbCirBuff->m_ulBufferDataSize;
+
+	if (ulWrIndex + ulDataBlockSize_p <= pShbCirBuff->m_ulBufferDataSize) {
+		// linear write operation
+		memcpy(pShbCirDataPtr + ulWrIndex, pScrDataPtr,
+		       ulDataBlockSize_p);
+	} else {
+		// wrap-around write operation
+		ulChunkSize = pShbCirBuff->m_ulBufferDataSize - ulWrIndex;
+		memcpy(pShbCirDataPtr + ulWrIndex, pScrDataPtr, ulChunkSize);
+		memcpy(pShbCirDataPtr, pScrDataPtr + ulChunkSize,
+		       ulDataBlockSize_p - ulChunkSize);
+	}
+
+	// adjust header information for circular buffer with properties
+	// of the wiritten data block
+	ShbIpcEnterAtomicSection(pShbInstance_p);
+	{
+		pShbCirBuff->m_ulDataApended += uiFullBlockSize;
+		pShbCirBuff->m_ulBlocksApended++;
+
+		// decrement number of currently (parallel running) write operations
+		if (!--pShbCirBuff->m_ulNumOfWriteJobs) {
+			// if there is no other write process running then
+			// set new size of readable (complete written) data and
+			// adjust number of readable blocks
+			pShbCirBuff->m_ulDataReadable +=
+			    pShbCirBuff->m_ulDataApended;
+			pShbCirBuff->m_ulBlocksReadable +=
+			    pShbCirBuff->m_ulBlocksApended;
+
+			pShbCirBuff->m_ulDataApended = 0;
+			pShbCirBuff->m_ulBlocksApended = 0;
+
+			fSignalNewData = TRUE;
+			fSignalReset = pShbCirBuff->m_fBufferLocked;
+		}
+	}
+	ShbIpcLeaveAtomicSection(pShbInstance_p);
+
+	// signal new data event to a potentially reading application
+	if (fSignalNewData) {
+		ShbError2 = ShbIpcSignalNewData(pShbInstance_p);
+		if (ShbError == kShbOk) {
+			ShbError = ShbError2;
+		}
+	}
+	// signal that the last write job has been finished to allow
+	// a waiting application to reset the buffer now
+	if (fSignalReset) {
+		ShbError2 = ShbIpcSignalJobReady(pShbInstance_p);
+		if (ShbError == kShbOk) {
+			ShbError = ShbError2;
+		}
+	}
+
+      Exit:
+
+	return (ShbError);
+
+}
+
+//---------------------------------------------------------------------------
+//  Allocate block within the Circular Shared Buffer for chunk writing
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbCirAllocDataBlock(tShbInstance pShbInstance_p,
+					       tShbCirChunk * pShbCirChunk_p,
+					       unsigned long ulDataBufferSize_p)
+{
+
+	tShbCirBuff *pShbCirBuff;
+	tShbCirBlockSize ShbCirBlockSize;
+	unsigned int uiFullBlockSize;
+	unsigned int uiAlignFillBytes;
+	unsigned char *pShbCirDataPtr;
+	unsigned long ulDataSize;
+	unsigned long ulWrIndex = 0;	// d.k. GCC complains about uninitialized variable otherwise
+	tShbError ShbError;
+	int fRes;
+
+	// check arguments
+	if ((pShbInstance_p == NULL) || (pShbCirChunk_p == NULL)) {
+		ShbError = kShbInvalidArg;
+		goto Exit;
+	}
+
+	if (ulDataBufferSize_p == 0) {
+		ShbError = kShbInvalidArg;
+		goto Exit;
+	}
+
+	if (ulDataBufferSize_p > SBC_MAX_BLOCK_SIZE) {
+		ShbError = kShbExceedDataSizeLimit;
+		goto Exit;
+	}
+
+	pShbCirBuff = ShbCirGetBuffer(pShbInstance_p);
+	ShbError = kShbOk;
+
+	if (pShbCirBuff->m_ShbCirMagicID != SBC_MAGIC_ID) {
+		ShbError = kShbInvalidBufferType;
+		goto Exit;
+	}
+
+	// calculate data block size in circular buffer
+	ulDataSize =
+	    (ulDataBufferSize_p +
+	     (SBC_BLOCK_ALIGNMENT - 1)) & ~(SBC_BLOCK_ALIGNMENT - 1);
+	uiFullBlockSize = ulDataSize + sizeof(tShbCirBlockSize);	// data size + header
+	uiAlignFillBytes = ulDataSize - ulDataBufferSize_p;
+
+	ShbCirBlockSize.m_uiFullBlockSize = uiFullBlockSize;
+	ShbCirBlockSize.m_uiAlignFillBytes = uiAlignFillBytes;
+
+	// reserve the needed memory for the write operation to do now
+	// and make necessary adjustments in the circular buffer header
+	ShbIpcEnterAtomicSection(pShbInstance_p);
+	{
+		// check if there is sufficient memory available to store
+		// the new data
+		fRes =
+		    (uiFullBlockSize <=
+		     (pShbCirBuff->m_ulBufferDataSize -
+		      pShbCirBuff->m_ulDataInUse));
+		if (fRes) {
+			// set write pointer for the write operation to do now
+			// to the current write pointer of the circular buffer
+			ulWrIndex = pShbCirBuff->m_ulWrIndex;
+
+			// reserve the needed memory for the write operation to do now
+			pShbCirBuff->m_ulDataInUse += uiFullBlockSize;
+
+			// set new write pointer behind the reserved memory
+			// for the write operation to do now
+			pShbCirBuff->m_ulWrIndex += uiFullBlockSize;
+			pShbCirBuff->m_ulWrIndex %=
+			    pShbCirBuff->m_ulBufferDataSize;
+
+			// increment number of currently (parallel running)
+			// write operations
+			pShbCirBuff->m_ulNumOfWriteJobs++;
+		}
+	}
+	ShbIpcLeaveAtomicSection(pShbInstance_p);
+
+	if (!fRes) {
+		ShbError = kShbBufferFull;
+		goto Exit;
+	}
+
+	// setup header information for allocated buffer
+	pShbCirDataPtr = &pShbCirBuff->m_Data;	// ptr to start of data area
+
+	// write real size of current block (incl. alignment fill bytes)
+	*(tShbCirBlockSize *) (pShbCirDataPtr + ulWrIndex) = ShbCirBlockSize;
+	ulWrIndex += sizeof(tShbCirBlockSize);
+	ulWrIndex %= pShbCirBuff->m_ulBufferDataSize;
+
+	// setup chunk descriptor
+	pShbCirChunk_p->m_uiFullBlockSize = uiFullBlockSize;
+	pShbCirChunk_p->m_ulAvailableSize = ulDataBufferSize_p;
+	pShbCirChunk_p->m_ulWrIndex = ulWrIndex;
+	pShbCirChunk_p->m_fBufferCompleted = FALSE;
+
+      Exit:
+
+	return (ShbError);
+
+}
+
+//---------------------------------------------------------------------------
+//  Write data chunk into an allocated buffer of the Circular Shared Buffer
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbCirWriteDataChunk(tShbInstance pShbInstance_p,
+					       tShbCirChunk * pShbCirChunk_p,
+					       const void *pSrcDataChunk_p,
+					       unsigned long ulDataChunkSize_p,
+					       unsigned int
+					       *pfBufferCompleted_p)
+{
+
+	tShbCirBuff *pShbCirBuff;
+	unsigned char *pShbCirDataPtr;
+	unsigned char *pScrDataPtr;
+	unsigned long ulSubChunkSize;
+	unsigned long ulWrIndex;
+	unsigned int fBufferCompleted;
+	unsigned int fSignalNewData;
+	unsigned int fSignalReset;
+	tShbError ShbError;
+	tShbError ShbError2;
+
+	// check arguments
+	if ((pShbInstance_p == NULL) || (pShbCirChunk_p == NULL)
+	    || (pfBufferCompleted_p == NULL)) {
+		ShbError = kShbInvalidArg;
+		goto Exit;
+	}
+
+	if ((pSrcDataChunk_p == NULL) || (ulDataChunkSize_p == 0)) {
+		// nothing to do here
+		ShbError = kShbOk;
+		goto Exit;
+	}
+
+	if (pShbCirChunk_p->m_fBufferCompleted) {
+		ShbError = kShbBufferAlreadyCompleted;
+		goto Exit;
+	}
+
+	if (ulDataChunkSize_p > pShbCirChunk_p->m_ulAvailableSize) {
+		ShbError = kShbExceedDataSizeLimit;
+		goto Exit;
+	}
+
+	pShbCirBuff = ShbCirGetBuffer(pShbInstance_p);
+	pScrDataPtr = (unsigned char *)pSrcDataChunk_p;
+	fSignalNewData = FALSE;
+	fSignalReset = FALSE;
+	ShbError = kShbOk;
+
+	if (pShbCirBuff->m_ShbCirMagicID != SBC_MAGIC_ID) {
+		ShbError = kShbInvalidBufferType;
+		goto Exit;
+	}
+
+	ulWrIndex = pShbCirChunk_p->m_ulWrIndex;
+
+	// copy the data to the circular buffer
+	// (the copy process itself will be done outside of any
+	// critical/locked section)
+	pShbCirDataPtr = &pShbCirBuff->m_Data;	// ptr to start of data area
+
+	if (ulWrIndex + ulDataChunkSize_p <= pShbCirBuff->m_ulBufferDataSize) {
+		// linear write operation
+		memcpy(pShbCirDataPtr + ulWrIndex, pScrDataPtr,
+		       ulDataChunkSize_p);
+	} else {
+		// wrap-around write operation
+		ulSubChunkSize = pShbCirBuff->m_ulBufferDataSize - ulWrIndex;
+		memcpy(pShbCirDataPtr + ulWrIndex, pScrDataPtr, ulSubChunkSize);
+		memcpy(pShbCirDataPtr, pScrDataPtr + ulSubChunkSize,
+		       ulDataChunkSize_p - ulSubChunkSize);
+	}
+
+	// adjust chunk descriptor
+	ulWrIndex += ulDataChunkSize_p;
+	ulWrIndex %= pShbCirBuff->m_ulBufferDataSize;
+
+	pShbCirChunk_p->m_ulAvailableSize -= ulDataChunkSize_p;
+	pShbCirChunk_p->m_ulWrIndex = ulWrIndex;
+
+	fBufferCompleted = (pShbCirChunk_p->m_ulAvailableSize == 0);
+	pShbCirChunk_p->m_fBufferCompleted = fBufferCompleted;
+
+	// if the complete allocated buffer is filled with data then
+	// adjust header information for circular buffer with properties
+	// of the wiritten data block
+	if (fBufferCompleted) {
+		ShbIpcEnterAtomicSection(pShbInstance_p);
+		{
+			pShbCirBuff->m_ulDataApended +=
+			    pShbCirChunk_p->m_uiFullBlockSize;
+			pShbCirBuff->m_ulBlocksApended++;
+
+			// decrement number of currently (parallel running) write operations
+			if (!--pShbCirBuff->m_ulNumOfWriteJobs) {
+				// if there is no other write process running then
+				// set new size of readable (complete written) data and
+				// adjust number of readable blocks
+				pShbCirBuff->m_ulDataReadable +=
+				    pShbCirBuff->m_ulDataApended;
+				pShbCirBuff->m_ulBlocksReadable +=
+				    pShbCirBuff->m_ulBlocksApended;
+
+				pShbCirBuff->m_ulDataApended = 0;
+				pShbCirBuff->m_ulBlocksApended = 0;
+
+				fSignalNewData = TRUE;
+				fSignalReset = pShbCirBuff->m_fBufferLocked;
+			}
+		}
+		ShbIpcLeaveAtomicSection(pShbInstance_p);
+	}
+
+	// signal new data event to a potentially reading application
+	if (fSignalNewData) {
+		ShbError2 = ShbIpcSignalNewData(pShbInstance_p);
+		if (ShbError == kShbOk) {
+			ShbError = ShbError2;
+		}
+	}
+	// signal that the last write job has been finished to allow
+	// a waiting application to reset the buffer now
+	if (fSignalReset) {
+		ShbError2 = ShbIpcSignalJobReady(pShbInstance_p);
+		if (ShbError == kShbOk) {
+			ShbError = ShbError2;
+		}
+	}
+
+	*pfBufferCompleted_p = fBufferCompleted;
+
+      Exit:
+
+	return (ShbError);
+
+}
+
+//---------------------------------------------------------------------------
+//  Read data block from Circular Shared Buffer
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbCirReadDataBlock(tShbInstance pShbInstance_p,
+					      void *pDstDataBlock_p,
+					      unsigned long ulRdBuffSize_p,
+					      unsigned long *pulDataBlockSize_p)
+{
+
+	tShbCirBuff *pShbCirBuff;
+	tShbCirBlockSize ShbCirBlockSize;
+	unsigned long ulDataReadable;
+	unsigned char *pShbCirDataPtr;
+	unsigned char *pDstDataPtr;
+	unsigned long ulDataSize = 0;	// d.k. GCC complains about uninitialized variable otherwise
+	unsigned long ulChunkSize;
+	unsigned long ulRdIndex;
+	tShbError ShbError;
+
+	// check arguments
+	if ((pShbInstance_p == NULL) || (pulDataBlockSize_p == NULL)) {
+		return (kShbInvalidArg);
+	}
+
+	if ((pDstDataBlock_p == NULL) || (ulRdBuffSize_p == 0)) {
+		// nothing to do here
+		ShbError = kShbOk;
+		goto Exit;
+	}
+
+	ShbError = kShbOk;
+	pShbCirBuff = ShbCirGetBuffer(pShbInstance_p);
+	pDstDataPtr = (unsigned char *)pDstDataBlock_p;
+	ulDataSize = 0;
+
+	if (pShbCirBuff->m_ShbCirMagicID != SBC_MAGIC_ID) {
+		ShbError = kShbInvalidBufferType;
+		goto Exit;
+	}
+
+	// get total number of readable bytes for the whole circular buffer
+	ShbIpcEnterAtomicSection(pShbInstance_p);
+	{
+		ulDataReadable = pShbCirBuff->m_ulDataReadable;
+	}
+	ShbIpcLeaveAtomicSection(pShbInstance_p);
+
+	// if there are readable data available, then there must be at least
+	// one complete readable data block
+	if (ulDataReadable > 0) {
+		// get pointer to start of data area and current read index
+		pShbCirDataPtr = &pShbCirBuff->m_Data;	// ptr to start of data area
+		ulRdIndex = pShbCirBuff->m_ulRdIndex;
+
+		// get real size of current block (incl. alignment fill bytes)
+		ShbCirBlockSize =
+		    *(tShbCirBlockSize *) (pShbCirDataPtr + ulRdIndex);
+		ulRdIndex += sizeof(tShbCirBlockSize);
+		ulRdIndex %= pShbCirBuff->m_ulBufferDataSize;
+
+		// get size of user data inside the current block
+		ulDataSize =
+		    ShbCirBlockSize.m_uiFullBlockSize -
+		    ShbCirBlockSize.m_uiAlignFillBytes;
+		ulDataSize -= sizeof(tShbCirBlockSize);
+	}
+
+	// ulDataSize = MIN(ulDataSize, ulRdBuffSize_p);
+	if (ulDataSize > ulRdBuffSize_p) {
+		ulDataSize = ulRdBuffSize_p;
+		ShbError = kShbDataTruncated;
+	}
+
+	if (ulDataSize == 0) {
+		// nothing to do here
+		ShbError = kShbNoReadableData;
+		goto Exit;
+	}
+
+	// copy the data from the circular buffer
+	// (the copy process itself will be done outside of any
+	// critical/locked section)
+	if (ulRdIndex + ulDataSize <= pShbCirBuff->m_ulBufferDataSize) {
+		// linear read operation
+		memcpy(pDstDataPtr, pShbCirDataPtr + ulRdIndex, ulDataSize);
+	} else {
+		// wrap-around read operation
+		ulChunkSize = pShbCirBuff->m_ulBufferDataSize - ulRdIndex;
+		memcpy(pDstDataPtr, pShbCirDataPtr + ulRdIndex, ulChunkSize);
+		memcpy(pDstDataPtr + ulChunkSize, pShbCirDataPtr,
+		       ulDataSize - ulChunkSize);
+	}
+
+#ifndef NDEBUG
+	{
+		tShbCirBlockSize ClrShbCirBlockSize;
+
+		if (ulRdIndex + ulDataSize <= pShbCirBuff->m_ulBufferDataSize) {
+			// linear buffer
+			memset(pShbCirDataPtr + ulRdIndex, 0xDD, ulDataSize);
+		} else {
+			// wrap-around read operation
+			ulChunkSize =
+			    pShbCirBuff->m_ulBufferDataSize - ulRdIndex;
+			memset(pShbCirDataPtr + ulRdIndex, 0xDD, ulChunkSize);
+			memset(pShbCirDataPtr, 0xDD, ulDataSize - ulChunkSize);
+		}
+
+		ClrShbCirBlockSize.m_uiFullBlockSize = /*(unsigned int) */ -1;	// -1 = xFFFFFFF
+		ClrShbCirBlockSize.m_uiAlignFillBytes = /*(unsigned int) */ -1;	// -1 = Fxxxxxxx
+		*(tShbCirBlockSize *) (pShbCirDataPtr +
+				       pShbCirBuff->m_ulRdIndex) =
+		    ClrShbCirBlockSize;
+	}
+#endif // #ifndef NDEBUG
+
+	// set new size of readable data, data in use, new read index
+	// and adjust number of readable blocks
+	ShbIpcEnterAtomicSection(pShbInstance_p);
+	{
+		pShbCirBuff->m_ulDataInUse -= ShbCirBlockSize.m_uiFullBlockSize;
+		pShbCirBuff->m_ulDataReadable -=
+		    ShbCirBlockSize.m_uiFullBlockSize;
+		pShbCirBuff->m_ulBlocksReadable--;
+
+		//$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
+		if ((pShbCirBuff->m_ulDataInUse == 0)
+		    && (pShbCirBuff->m_ulDataReadable == 0)) {
+			ASSERT(pShbCirBuff->m_ulBlocksReadable == 0);
+
+			pShbCirBuff->m_ulWrIndex = 0;
+			pShbCirBuff->m_ulRdIndex = 0;
+		} else
+			//$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
+		{
+			pShbCirBuff->m_ulRdIndex +=
+			    ShbCirBlockSize.m_uiFullBlockSize;
+			pShbCirBuff->m_ulRdIndex %=
+			    pShbCirBuff->m_ulBufferDataSize;
+		}
+	}
+	ShbIpcLeaveAtomicSection(pShbInstance_p);
+
+      Exit:
+
+	*pulDataBlockSize_p = ulDataSize;
+
+	return (ShbError);
+
+}
+
+//---------------------------------------------------------------------------
+//  Get data size of next readable block from Circular Shared Buffer
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbCirGetReadDataSize(tShbInstance pShbInstance_p,
+						unsigned long
+						*pulDataBlockSize_p)
+{
+
+	tShbCirBuff *pShbCirBuff;
+	unsigned long ulDataReadable;
+	unsigned char *pShbCirDataPtr;
+	tShbCirBlockSize ShbCirBlockSize;
+	unsigned long ulDataSize;
+	tShbError ShbError;
+
+	// check arguments
+	if ((pShbInstance_p == NULL) || (pulDataBlockSize_p == NULL)) {
+		return (kShbInvalidArg);
+	}
+
+	pShbCirBuff = ShbCirGetBuffer(pShbInstance_p);
+	ulDataSize = 0;
+	ShbError = kShbOk;
+
+	if (pShbCirBuff->m_ShbCirMagicID != SBC_MAGIC_ID) {
+		ShbError = kShbInvalidBufferType;
+		goto Exit;
+	}
+
+	// get total number of readable bytes for the whole circular buffer
+	ShbIpcEnterAtomicSection(pShbInstance_p);
+	{
+		ulDataReadable = pShbCirBuff->m_ulDataReadable;
+	}
+	ShbIpcLeaveAtomicSection(pShbInstance_p);
+
+	// if there are readable data available, then there must be at least
+	// one complete readable data block
+	if (ulDataReadable > 0) {
+		pShbCirDataPtr =
+		    &pShbCirBuff->m_Data + pShbCirBuff->m_ulRdIndex;
+
+		// get real size of current block (incl. alignment fill bytes)
+		ShbCirBlockSize = *(tShbCirBlockSize *) pShbCirDataPtr;
+
+		// get size of user data inside the current block
+		ulDataSize =
+		    ShbCirBlockSize.m_uiFullBlockSize -
+		    ShbCirBlockSize.m_uiAlignFillBytes;
+		ulDataSize -= sizeof(tShbCirBlockSize);
+	}
+
+      Exit:
+
+	*pulDataBlockSize_p = ulDataSize;
+
+	return (ShbError);
+
+}
+
+//---------------------------------------------------------------------------
+//  Get number of readable blocks from Circular Shared Buffer
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbCirGetReadBlockCount(tShbInstance pShbInstance_p,
+						  unsigned long
+						  *pulDataBlockCount_p)
+{
+
+	tShbCirBuff *pShbCirBuff;
+	unsigned long ulBlockCount;
+	tShbError ShbError;
+
+	// check arguments
+	if ((pShbInstance_p == NULL) || (pulDataBlockCount_p == NULL)) {
+		ShbError = kShbInvalidArg;
+		goto Exit;
+	}
+
+	pShbCirBuff = ShbCirGetBuffer(pShbInstance_p);
+	ulBlockCount = 0;
+	ShbError = kShbOk;
+
+	if (pShbCirBuff->m_ShbCirMagicID != SBC_MAGIC_ID) {
+		ShbError = kShbInvalidBufferType;
+		goto Exit;
+	}
+
+	ShbIpcEnterAtomicSection(pShbInstance_p);
+	{
+		ulBlockCount = pShbCirBuff->m_ulBlocksReadable;
+	}
+	ShbIpcLeaveAtomicSection(pShbInstance_p);
+
+	*pulDataBlockCount_p = ulBlockCount;
+
+      Exit:
+
+	return (ShbError);
+
+}
+
+//---------------------------------------------------------------------------
+//  Set application handler to signal new data for Circular Shared Buffer
+//  d.k.: new parameter priority as enum
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbCirSetSignalHandlerNewData(tShbInstance
+							pShbInstance_p,
+							tShbCirSigHndlrNewData
+							pfnSignalHandlerNewData_p,
+							tShbPriority
+							ShbPriority_p)
+{
+
+	tShbCirBuff *pShbCirBuff;
+	tShbError ShbError;
+
+	// check arguments
+	if (pShbInstance_p == NULL) {
+		ShbError = kShbInvalidArg;
+		goto Exit;
+	}
+
+	pShbCirBuff = ShbCirGetBuffer(pShbInstance_p);
+	ShbError = kShbOk;
+
+	if (pShbCirBuff->m_ShbCirMagicID != SBC_MAGIC_ID) {
+		ShbError = kShbInvalidBufferType;
+		goto Exit;
+	}
+
+	if (pfnSignalHandlerNewData_p != NULL) {
+		// set a new signal handler
+		if (pShbCirBuff->m_pfnSigHndlrNewData != NULL) {
+			ShbError = kShbAlreadySignaling;
+			goto Exit;
+		}
+
+		pShbCirBuff->m_pfnSigHndlrNewData = pfnSignalHandlerNewData_p;
+		ShbError =
+		    ShbIpcStartSignalingNewData(pShbInstance_p,
+						ShbCirSignalHandlerNewData,
+						ShbPriority_p);
+	} else {
+		// remove existing signal handler
+		ShbError = ShbIpcStopSignalingNewData(pShbInstance_p);
+		if (pShbCirBuff->m_pfnSigHndlrNewData != NULL) {
+			pShbCirBuff->m_pfnSigHndlrNewData(pShbInstance_p, 0);
+		}
+		pShbCirBuff->m_pfnSigHndlrNewData = NULL;
+	}
+
+      Exit:
+
+	return (ShbError);
+
+}
+
+#endif
+
+#if !defined(INLINE_ENABLED)
+
+//---------------------------------------------------------------------------
+//  DEBUG: Trace Circular Shared Buffer
+//---------------------------------------------------------------------------
+
+#ifndef NDEBUG
+tShbError ShbCirTraceBuffer(tShbInstance pShbInstance_p)
+{
+
+	tShbCirBuff *pShbCirBuff;
+	char szMagigID[sizeof(SBC_MAGIC_ID) + 1];
+	tShbCirBlockSize ShbCirBlockSize;
+	unsigned long ulDataReadable;
+	unsigned char *pShbCirDataPtr;
+	unsigned long ulBlockIndex;
+	unsigned int nBlockCount;
+	unsigned long ulDataSize;
+	unsigned long ulChunkSize;
+	unsigned long ulRdIndex;
+	tShbError ShbError;
+
+	TRACE0("\n\n##### Circular Shared Buffer #####\n");
+
+	// check arguments
+	if (pShbInstance_p == NULL) {
+		TRACE1("\nERROR: invalid buffer address (0x%08lX)\n",
+		       (unsigned long)pShbInstance_p);
+		ShbError = kShbInvalidArg;
+		goto Exit;
+	}
+
+	pShbCirBuff = ShbCirGetBuffer(pShbInstance_p);
+	ShbError = kShbOk;
+
+	if (pShbCirBuff->m_ShbCirMagicID != SBC_MAGIC_ID) {
+		ShbError = kShbInvalidBufferType;
+		goto Exit;
+	}
+
+	*(unsigned long *)&szMagigID[0] = pShbCirBuff->m_ShbCirMagicID;
+	szMagigID[sizeof(SBC_MAGIC_ID)] = '\0';
+
+	ShbIpcEnterAtomicSection(pShbInstance_p);
+	{
+		TRACE1("\nBuffer Address:   0x%08lX\n",
+		       (unsigned long)pShbCirBuff);
+
+		TRACE0("\nHeader Info:");
+		TRACE2("\nMagigID:          '%s' (%08lX)", szMagigID,
+		       pShbCirBuff->m_ShbCirMagicID);
+		TRACE1("\nBufferTotalSize:  %4lu [Bytes]",
+		       pShbCirBuff->m_ulBufferTotalSize);
+		TRACE1("\nBufferDataSize:   %4lu [Bytes]",
+		       pShbCirBuff->m_ulBufferDataSize);
+		TRACE1("\nWrIndex:          %4lu", pShbCirBuff->m_ulWrIndex);
+		TRACE1("\nRdIndex:          %4lu", pShbCirBuff->m_ulRdIndex);
+		TRACE1("\nNumOfWriteJobs:   %4lu",
+		       pShbCirBuff->m_ulNumOfWriteJobs);
+		TRACE1("\nDataInUse:        %4lu [Bytes]",
+		       pShbCirBuff->m_ulDataInUse);
+		TRACE1("\nDataApended:      %4lu [Bytes]",
+		       pShbCirBuff->m_ulDataApended);
+		TRACE1("\nBlocksApended:    %4lu",
+		       pShbCirBuff->m_ulBlocksApended);
+		TRACE1("\nDataReadable:     %4lu [Bytes]",
+		       pShbCirBuff->m_ulDataReadable);
+		TRACE1("\nBlocksReadable:   %4lu",
+		       pShbCirBuff->m_ulBlocksReadable);
+		TRACE1("\nSigHndlrNewData:  %08lX",
+		       (unsigned long)pShbCirBuff->m_pfnSigHndlrNewData);
+		TRACE1("\nBufferLocked:     %d", pShbCirBuff->m_fBufferLocked);
+		TRACE1("\nSigHndlrReset:    %08lX",
+		       (unsigned long)pShbCirBuff->m_pfnSigHndlrReset);
+
+		ShbTraceDump(&pShbCirBuff->m_Data,
+			     pShbCirBuff->m_ulBufferDataSize, 0x00000000L,
+			     "\nData Area:");
+
+		ulDataReadable = pShbCirBuff->m_ulDataReadable;
+		nBlockCount = 1;
+		ulBlockIndex = pShbCirBuff->m_ulRdIndex;
+
+		while (ulDataReadable > 0) {
+			TRACE1("\n\n--- Block #%u ---", nBlockCount);
+
+			// get pointer to start of data area and current read index
+			pShbCirDataPtr = &pShbCirBuff->m_Data;	// ptr to start of data area
+			ulRdIndex = ulBlockIndex;
+
+			// get real size of current block (incl. alignment fill bytes)
+			ShbCirBlockSize =
+			    *(tShbCirBlockSize *) (pShbCirDataPtr + ulRdIndex);
+			ulRdIndex += sizeof(tShbCirBlockSize);
+			ulRdIndex %= pShbCirBuff->m_ulBufferDataSize;
+
+			// get size of user data inside the current block
+			ulDataSize =
+			    ShbCirBlockSize.m_uiFullBlockSize -
+			    ShbCirBlockSize.m_uiAlignFillBytes;
+			ulDataSize -= sizeof(tShbCirBlockSize);
+
+			TRACE1
+			    ("\nFull Data Size:       %4u [Bytes] (incl. header and alignment fill bytes)",
+			     ShbCirBlockSize.m_uiFullBlockSize);
+			TRACE1("\nUser Data Size:       %4lu [Bytes]",
+			       ulDataSize);
+			TRACE1("\nAlignment Fill Bytes: %4u [Bytes]",
+			       ShbCirBlockSize.m_uiAlignFillBytes);
+
+			if (ulRdIndex + ulDataSize <=
+			    pShbCirBuff->m_ulBufferDataSize) {
+				// linear data buffer
+				ShbTraceDump(pShbCirDataPtr + ulRdIndex,
+					     ulDataSize, 0x00000000L, NULL);
+			} else {
+				// wrap-around data buffer
+				ulChunkSize =
+				    pShbCirBuff->m_ulBufferDataSize - ulRdIndex;
+				ShbTraceDump(pShbCirDataPtr + ulRdIndex,
+					     ulChunkSize, 0x00000000L, NULL);
+				ShbTraceDump(pShbCirDataPtr,
+					     ulDataSize - ulChunkSize,
+					     ulChunkSize, NULL);
+			}
+
+			nBlockCount++;
+
+			ulBlockIndex += ShbCirBlockSize.m_uiFullBlockSize;
+			ulBlockIndex %= pShbCirBuff->m_ulBufferDataSize;
+
+			ulDataReadable -= ShbCirBlockSize.m_uiFullBlockSize;
+		}
+
+		ASSERT(pShbCirBuff->m_ulBlocksReadable == nBlockCount - 1);
+	}
+	ShbIpcLeaveAtomicSection(pShbInstance_p);
+
+      Exit:
+
+	return (ShbError);
+
+}
+#endif
+
+//-------------------------------------------------------------------------//
+//                                                                         //
+//          L i n e a r   S h a r e d   B u f f e r                        //
+//                                                                         //
+//-------------------------------------------------------------------------//
+
+//---------------------------------------------------------------------------
+//  Allocate Linear Shared Buffer
+//---------------------------------------------------------------------------
+
+tShbError ShbLinAllocBuffer(unsigned long ulBufferSize_p,
+			    const char *pszBufferID_p,
+			    tShbInstance * ppShbInstance_p,
+			    unsigned int *pfShbNewCreated_p)
+{
+
+	tShbInstance pShbInstance;
+	tShbLinBuff *pShbLinBuff;
+	unsigned int fShbNewCreated;
+	unsigned long ulBufferDataSize;
+	unsigned long ulBufferTotalSize;
+	tShbError ShbError;
+
+	// check arguments
+	if ((ulBufferSize_p == 0) || (ppShbInstance_p == NULL)) {
+		return (kShbInvalidArg);
+	}
+
+	// calculate length of memory to allocate
+	ulBufferDataSize =
+	    (ulBufferSize_p +
+	     (SBL_BLOCK_ALIGNMENT - 1)) & ~(SBL_BLOCK_ALIGNMENT - 1);
+	ulBufferTotalSize = ulBufferDataSize + sizeof(tShbLinBuff);
+
+	// allocate a new or open an existing shared buffer
+	ShbError = ShbIpcAllocBuffer(ulBufferTotalSize, pszBufferID_p,
+				     &pShbInstance, &fShbNewCreated);
+	if (ShbError != kShbOk) {
+		goto Exit;
+	}
+
+	if (pShbInstance == NULL) {
+		ShbError = kShbOutOfMem;
+		goto Exit;
+	}
+
+	// get pointer to shared buffer
+	pShbLinBuff = (tShbLinBuff *) ShbIpcGetShMemPtr(pShbInstance);
+
+	// if the shared buffer was new created, than this process has
+	// to initialize it, otherwise the buffer is already in use
+	// and *must not* be reseted
+	if (fShbNewCreated) {
+#ifndef NDEBUG
+		{
+			memset(pShbLinBuff, 0xCC, ulBufferTotalSize);
+		}
+#endif
+
+		pShbLinBuff->m_ShbLinMagicID = SBL_MAGIC_ID;
+		pShbLinBuff->m_ulBufferTotalSize = ulBufferTotalSize;
+		pShbLinBuff->m_ulBufferDataSize = ulBufferDataSize;
+	} else {
+		if (pShbLinBuff->m_ShbLinMagicID != SBL_MAGIC_ID) {
+			ShbError = kShbInvalidBufferType;
+			goto Exit;
+		}
+	}
+
+      Exit:
+
+	*ppShbInstance_p = pShbInstance;
+	*pfShbNewCreated_p = fShbNewCreated;
+
+	return (ShbError);
+
+}
+
+//---------------------------------------------------------------------------
+//  Release Linear Shared Buffer
+//---------------------------------------------------------------------------
+
+tShbError ShbLinReleaseBuffer(tShbInstance pShbInstance_p)
+{
+
+	tShbError ShbError;
+
+	// check arguments
+	if (pShbInstance_p == NULL) {
+		ShbError = kShbOk;
+		goto Exit;
+	}
+
+	ShbError = ShbIpcReleaseBuffer(pShbInstance_p);
+
+      Exit:
+
+	return (ShbError);
+
+}
+
+#endif // !defined(INLINE_ENABLED)
+
+#if (!defined(SHAREDBUFF_INLINED)) || defined(INLINE_ENABLED)
+
+//---------------------------------------------------------------------------
+//  Write data block to Linear Shared Buffer
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbLinWriteDataBlock(tShbInstance pShbInstance_p,
+					       unsigned long ulDstBufferOffs_p,
+					       const void *pSrcDataBlock_p,
+					       unsigned long ulDataBlockSize_p)
+{
+
+	tShbLinBuff *pShbLinBuff;
+	unsigned char *pShbLinDataPtr;
+	unsigned char *pScrDataPtr;
+	unsigned long ulBufferDataSize;
+	tShbError ShbError;
+
+	// check arguments
+	if (pShbInstance_p == NULL) {
+		ShbError = kShbInvalidArg;
+		goto Exit;
+	}
+
+	if ((pSrcDataBlock_p == NULL) || (ulDataBlockSize_p == 0)) {
+		// nothing to do here
+		ShbError = kShbOk;
+		goto Exit;
+	}
+
+	if (ulDataBlockSize_p > SBL_MAX_BLOCK_SIZE) {
+		ShbError = kShbExceedDataSizeLimit;
+		goto Exit;
+	}
+
+	pShbLinBuff = ShbLinGetBuffer(pShbInstance_p);
+	pScrDataPtr = (unsigned char *)pSrcDataBlock_p;
+	ShbError = kShbOk;
+
+	if (pShbLinBuff->m_ShbLinMagicID != SBL_MAGIC_ID) {
+		ShbError = kShbInvalidBufferType;
+		goto Exit;
+	}
+
+	// check if offeset and size for the write operation matches with
+	// the size of the shared buffer
+	ulBufferDataSize = pShbLinBuff->m_ulBufferDataSize;
+	if ((ulDstBufferOffs_p > ulBufferDataSize) ||
+	    (ulDataBlockSize_p > ulBufferDataSize) ||
+	    ((ulDstBufferOffs_p + ulDataBlockSize_p) > ulBufferDataSize)) {
+		ShbError = kShbDataOutsideBufferArea;
+		goto Exit;
+	}
+
+	// copy the data to the linear buffer
+	// (the copy process will be done inside of any critical/locked section)
+	pShbLinDataPtr = &pShbLinBuff->m_Data;	// ptr to start of data area
+	pShbLinDataPtr += ulDstBufferOffs_p;
+
+	ShbIpcEnterAtomicSection(pShbInstance_p);
+	{
+		memcpy(pShbLinDataPtr, pScrDataPtr, ulDataBlockSize_p);
+	}
+	ShbIpcLeaveAtomicSection(pShbInstance_p);
+
+      Exit:
+
+	return (ShbError);
+
+}
+
+//---------------------------------------------------------------------------
+//  Read data block from Linear Shared Buffer
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbLinReadDataBlock(tShbInstance pShbInstance_p,
+					      void *pDstDataBlock_p,
+					      unsigned long ulSrcBufferOffs_p,
+					      unsigned long ulDataBlockSize_p)
+{
+
+	tShbLinBuff *pShbLinBuff;
+	unsigned char *pShbLinDataPtr;
+	unsigned char *pDstDataPtr;
+	unsigned long ulBufferDataSize;
+	tShbError ShbError;
+
+	// check arguments
+	if (pShbInstance_p == NULL) {
+		ShbError = kShbInvalidArg;
+		goto Exit;
+	}
+
+	if ((pDstDataBlock_p == NULL) || (ulDataBlockSize_p == 0)) {
+		// nothing to do here
+		ShbError = kShbOk;
+		goto Exit;
+	}
+
+	if (ulDataBlockSize_p > SBL_MAX_BLOCK_SIZE) {
+		ShbError = kShbExceedDataSizeLimit;
+		goto Exit;
+	}
+
+	pShbLinBuff = ShbLinGetBuffer(pShbInstance_p);
+	pDstDataPtr = (unsigned char *)pDstDataBlock_p;
+	ShbError = kShbOk;
+
+	if (pShbLinBuff->m_ShbLinMagicID != SBL_MAGIC_ID) {
+		ShbError = kShbInvalidBufferType;
+		goto Exit;
+	}
+
+	// check if offeset and size for the read operation matches with
+	// the size of the shared buffer
+	ulBufferDataSize = pShbLinBuff->m_ulBufferDataSize;
+	if ((ulSrcBufferOffs_p > ulBufferDataSize) ||
+	    (ulDataBlockSize_p > ulBufferDataSize) ||
+	    ((ulSrcBufferOffs_p + ulDataBlockSize_p) > ulBufferDataSize)) {
+		ShbError = kShbDataOutsideBufferArea;
+		goto Exit;
+	}
+
+	// copy the data to the linear buffer
+	// (the copy process will be done inside of any critical/locked section)
+	pShbLinDataPtr = &pShbLinBuff->m_Data;	// ptr to start of data area
+	pShbLinDataPtr += ulSrcBufferOffs_p;
+
+	ShbIpcEnterAtomicSection(pShbInstance_p);
+	{
+		memcpy(pDstDataPtr, pShbLinDataPtr, ulDataBlockSize_p);
+	}
+	ShbIpcLeaveAtomicSection(pShbInstance_p);
+
+      Exit:
+
+	return (ShbError);
+
+}
+
+#endif
+
+#if !defined(INLINE_ENABLED)
+
+//---------------------------------------------------------------------------
+//  DEBUG: Trace Linear Shared Buffer
+//---------------------------------------------------------------------------
+
+#ifndef NDEBUG
+tShbError ShbLinTraceBuffer(tShbInstance pShbInstance_p)
+{
+
+	tShbLinBuff *pShbLinBuff;
+	char szMagigID[sizeof(SBL_MAGIC_ID) + 1];
+	tShbError ShbError;
+
+	TRACE0("\n\n##### Linear Shared Buffer #####\n");
+
+	// check arguments
+	if (pShbInstance_p == NULL) {
+		TRACE1("\nERROR: invalid buffer address (0x%08lX)\n",
+		       (unsigned long)pShbInstance_p);
+		ShbError = kShbInvalidArg;
+		goto Exit;
+	}
+
+	pShbLinBuff = ShbLinGetBuffer(pShbInstance_p);
+	ShbError = kShbOk;
+
+	if (pShbLinBuff->m_ShbLinMagicID != SBL_MAGIC_ID) {
+		ShbError = kShbInvalidBufferType;
+		goto Exit;
+	}
+
+	*(unsigned int *)&szMagigID[0] = pShbLinBuff->m_ShbLinMagicID;
+	szMagigID[sizeof(SBL_MAGIC_ID)] = '\0';
+
+	ShbIpcEnterAtomicSection(pShbInstance_p);
+	{
+		TRACE1("\nBuffer Address:   0x%08lX\n",
+		       (unsigned long)pShbLinBuff);
+
+		TRACE0("\nHeader Info:");
+		TRACE2("\nMagigID:          '%s' (%08X)", szMagigID,
+		       pShbLinBuff->m_ShbLinMagicID);
+		TRACE1("\nBufferTotalSize:  %4lu [Bytes]",
+		       pShbLinBuff->m_ulBufferTotalSize);
+		TRACE1("\nBufferDataSize:   %4lu [Bytes]",
+		       pShbLinBuff->m_ulBufferDataSize);
+
+		ShbTraceDump(&pShbLinBuff->m_Data,
+			     pShbLinBuff->m_ulBufferDataSize, 0x00000000L,
+			     "\nData Area:");
+	}
+	ShbIpcLeaveAtomicSection(pShbInstance_p);
+
+      Exit:
+
+	return (ShbError);
+
+}
+#endif
+
+//---------------------------------------------------------------------------
+//  Dump buffer contents
+//---------------------------------------------------------------------------
+
+#ifndef NDEBUG
+tShbError ShbTraceDump(const unsigned char *pabStartAddr_p,
+		       unsigned long ulDataSize_p,
+		       unsigned long ulAddrOffset_p, const char *pszInfoText_p)
+{
+
+	const unsigned char *pabBuffData;
+	unsigned long ulBuffSize;
+	unsigned char bData;
+	int nRow;
+	int nCol;
+
+	// get pointer to buffer and length of buffer
+	pabBuffData = pabStartAddr_p;
+	ulBuffSize = ulDataSize_p;
+
+	if (pszInfoText_p != NULL) {
+		TRACE0(pszInfoText_p);
+	}
+	// dump buffer contents
+	for (nRow = 0;; nRow++) {
+		TRACE1("\n%08lX:   ",
+		       (unsigned long)(nRow * 0x10) + ulAddrOffset_p);
+
+		for (nCol = 0; nCol < 16; nCol++) {
+			if ((unsigned long)nCol < ulBuffSize) {
+				TRACE1("%02X ",
+				       (unsigned int)*(pabBuffData + nCol));
+			} else {
+				TRACE0("   ");
+			}
+		}
+
+		TRACE0(" ");
+
+		for (nCol = 0; nCol < 16; nCol++) {
+			bData = *pabBuffData++;
+			if ((unsigned long)nCol < ulBuffSize) {
+				if ((bData >= 0x20) && (bData < 0x7F)) {
+					TRACE1("%c", bData);
+				} else {
+					TRACE0(".");
+				}
+			} else {
+				TRACE0(" ");
+			}
+		}
+
+		if (ulBuffSize > 16) {
+			ulBuffSize -= 16;
+		} else {
+			break;
+		}
+	}
+
+	return (kShbOk);
+
+}
+#endif // #ifndef NDEBUG
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//  Handler to signal new data event for Circular Shared Buffer
+//---------------------------------------------------------------------------
+
+int ShbCirSignalHandlerNewData(tShbInstance pShbInstance_p)
+{
+
+	tShbCirBuff *pShbCirBuff;
+	unsigned long ulDataSize;
+	unsigned long ulBlockCount;
+	tShbError ShbError;
+
+	// check arguments
+	if (pShbInstance_p == NULL) {
+		return FALSE;
+	}
+
+	pShbCirBuff = ShbCirGetBuffer(pShbInstance_p);
+	ShbError = kShbOk;
+
+	if (pShbCirBuff->m_ShbCirMagicID != SBC_MAGIC_ID) {
+		return FALSE;
+	}
+
+	// call application handler
+	if (pShbCirBuff->m_pfnSigHndlrNewData != NULL) {
+/*        do
+        {*/
+		ShbError = ShbCirGetReadDataSize(pShbInstance_p, &ulDataSize);
+		if ((ulDataSize > 0) && (ShbError == kShbOk)) {
+			pShbCirBuff->m_pfnSigHndlrNewData(pShbInstance_p,
+							  ulDataSize);
+		}
+
+		ShbError =
+		    ShbCirGetReadBlockCount(pShbInstance_p, &ulBlockCount);
+/*        }
+        while ((ulBlockCount > 0) && (ShbError == kShbOk));*/
+	}
+	// Return TRUE if there are pending blocks.
+	// In that case ShbIpc tries to call this function again immediately if there
+	// is no other filled shared buffer with higher priority.
+	return ((ulBlockCount > 0) && (ShbError == kShbOk));
+
+}
+
+//---------------------------------------------------------------------------
+//  Handler to reset Circular Shared Buffer
+//---------------------------------------------------------------------------
+
+void ShbCirSignalHandlerReset(tShbInstance pShbInstance_p,
+			      unsigned int fTimeOut_p)
+{
+
+	tShbCirBuff *pShbCirBuff;
+
+	// check arguments
+	if (pShbInstance_p == NULL) {
+		return;
+	}
+
+	pShbCirBuff = ShbCirGetBuffer(pShbInstance_p);
+	if (pShbCirBuff->m_ShbCirMagicID != SBC_MAGIC_ID) {
+		return;
+	}
+
+	// reset buffer header
+	if (!fTimeOut_p) {
+		ShbIpcEnterAtomicSection(pShbInstance_p);
+		{
+			pShbCirBuff->m_ulWrIndex = 0;
+			pShbCirBuff->m_ulRdIndex = 0;
+			pShbCirBuff->m_ulNumOfWriteJobs = 0;
+			pShbCirBuff->m_ulDataInUse = 0;
+			pShbCirBuff->m_ulDataApended = 0;
+			pShbCirBuff->m_ulBlocksApended = 0;
+			pShbCirBuff->m_ulDataReadable = 0;
+			pShbCirBuff->m_ulBlocksReadable = 0;
+		}
+		ShbIpcLeaveAtomicSection(pShbInstance_p);
+
+#ifndef NDEBUG
+		{
+			memset(&pShbCirBuff->m_Data, 0xCC,
+			       pShbCirBuff->m_ulBufferDataSize);
+		}
+#endif
+	}
+
+	// call application handler
+	if (pShbCirBuff->m_pfnSigHndlrReset != NULL) {
+		pShbCirBuff->m_pfnSigHndlrReset(pShbInstance_p, fTimeOut_p);
+	}
+
+	// unlock buffer
+	ShbIpcEnterAtomicSection(pShbInstance_p);
+	{
+		pShbCirBuff->m_fBufferLocked = FALSE;
+		pShbCirBuff->m_pfnSigHndlrReset = NULL;
+	}
+	ShbIpcLeaveAtomicSection(pShbInstance_p);
+
+	return;
+
+}
+
+#endif
+
+// EOF
diff --git a/drivers/staging/epl/SharedBuff.h b/drivers/staging/epl/SharedBuff.h
new file mode 100644
index 00000000000..0ec1b4b9e6a
--- /dev/null
+++ b/drivers/staging/epl/SharedBuff.h
@@ -0,0 +1,204 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      Project independend shared buffer (linear + circular)
+
+  Description:  Declaration of platform independend part for the
+                shared buffer
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+  2006/06/27 -rs:   V 1.00 (initial version)
+
+****************************************************************************/
+
+#ifndef _SHAREDBUFF_H_
+#define _SHAREDBUFF_H_
+
+//---------------------------------------------------------------------------
+//  Type definitions
+//---------------------------------------------------------------------------
+
+typedef enum {
+	kShbOk = 0,
+	kShbNoReadableData = 1,
+	kShbDataTruncated = 2,
+	kShbBufferFull = 3,
+	kShbDataOutsideBufferArea = 4,
+	kShbBufferAlreadyCompleted = 5,
+	kShbMemUsedByOtherProcs = 6,
+	kShbOpenMismatch = 7,
+	kShbInvalidBufferType = 8,
+	kShbInvalidArg = 9,
+	kShbBufferInvalid = 10,
+	kShbOutOfMem = 11,
+	kShbAlreadyReseting = 12,
+	kShbAlreadySignaling = 13,
+	kShbExceedDataSizeLimit = 14,
+
+} tShbError;
+
+// 2006/08/24 d.k.: Priority for threads (new data, job signaling)
+typedef enum {
+	kShbPriorityLow = 0,
+	kShbPriorityNormal = 1,
+	kshbPriorityHigh = 2
+} tShbPriority;
+
+typedef struct {
+	unsigned int m_uiFullBlockSize;	// real size of allocated block (incl. alignment fill bytes)
+	unsigned long m_ulAvailableSize;	// still available size for data
+	unsigned long m_ulWrIndex;	// current write index
+	unsigned int m_fBufferCompleted;	// TRUE if allocated block is complete filled with data
+
+} tShbCirChunk;
+
+typedef void *tShbInstance;
+
+typedef void (*tShbCirSigHndlrNewData) (tShbInstance pShbInstance_p,
+					unsigned long ulDataBlockSize_p);
+typedef void (*tShbCirSigHndlrReset) (tShbInstance pShbInstance_p,
+				      unsigned int fTimeOut_p);
+
+//---------------------------------------------------------------------------
+//  Prototypes
+//---------------------------------------------------------------------------
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*#if defined(INLINE_FUNCTION_DEF)
+    #undef  INLINE_FUNCTION
+    #define INLINE_FUNCTION     INLINE_FUNCTION_DEF
+    #define INLINE_ENABLED      TRUE
+    #define SHAREDBUFF_INLINED
+    #include "SharedBuff.c"
+#endif
+*/
+
+	tShbError ShbInit(void);
+	tShbError ShbExit(void);
+
+// Circular Shared Buffer
+	tShbError ShbCirAllocBuffer(unsigned long ulBufferSize_p,
+				    const char *pszBufferID_p,
+				    tShbInstance * ppShbInstance_p,
+				    unsigned int *pfShbNewCreated_p);
+	tShbError ShbCirReleaseBuffer(tShbInstance pShbInstance_p);
+
+#if !defined(INLINE_ENABLED)
+
+	tShbError ShbCirResetBuffer(tShbInstance pShbInstance_p,
+				    unsigned long ulTimeOut_p,
+				    tShbCirSigHndlrReset
+				    pfnSignalHandlerReset_p);
+	tShbError ShbCirWriteDataBlock(tShbInstance pShbInstance_p,
+				       const void *pSrcDataBlock_p,
+				       unsigned long ulDataBlockSize_p);
+	tShbError ShbCirAllocDataBlock(tShbInstance pShbInstance_p,
+				       tShbCirChunk * pShbCirChunk_p,
+				       unsigned long ulDataBufferSize_p);
+	tShbError ShbCirWriteDataChunk(tShbInstance pShbInstance_p,
+				       tShbCirChunk * pShbCirChunk_p,
+				       const void *pSrcDataChunk_p,
+				       unsigned long ulDataChunkSize_p,
+				       unsigned int *pfBufferCompleted_p);
+	tShbError ShbCirReadDataBlock(tShbInstance pShbInstance_p,
+				      void *pDstDataBlock_p,
+				      unsigned long ulRdBuffSize_p,
+				      unsigned long *pulDataBlockSize_p);
+	tShbError ShbCirGetReadDataSize(tShbInstance pShbInstance_p,
+					unsigned long *pulDataBlockSize_p);
+	tShbError ShbCirGetReadBlockCount(tShbInstance pShbInstance_p,
+					  unsigned long *pulDataBlockCount_p);
+	tShbError ShbCirSetSignalHandlerNewData(tShbInstance pShbInstance_p,
+						tShbCirSigHndlrNewData
+						pfnShbSignalHandlerNewData_p,
+						tShbPriority ShbPriority_p);
+
+#endif
+
+// Linear Shared Buffer
+	tShbError ShbLinAllocBuffer(unsigned long ulBufferSize_p,
+				    const char *pszBufferID_p,
+				    tShbInstance * ppShbInstance_p,
+				    unsigned int *pfShbNewCreated_p);
+	tShbError ShbLinReleaseBuffer(tShbInstance pShbInstance_p);
+
+#if !defined(INLINE_ENABLED)
+
+	tShbError ShbLinWriteDataBlock(tShbInstance pShbInstance_p,
+				       unsigned long ulDstBufferOffs_p,
+				       const void *pSrcDataBlock_p,
+				       unsigned long ulDataBlockSize_p);
+	tShbError ShbLinReadDataBlock(tShbInstance pShbInstance_p,
+				      void *pDstDataBlock_p,
+				      unsigned long ulSrcBufferOffs_p,
+				      unsigned long ulDataBlockSize_p);
+
+#endif
+
+#ifndef NDEBUG
+	tShbError ShbCirTraceBuffer(tShbInstance pShbInstance_p);
+	tShbError ShbLinTraceBuffer(tShbInstance pShbInstance_p);
+	tShbError ShbTraceDump(const unsigned char *pabStartAddr_p,
+			       unsigned long ulDataSize_p,
+			       unsigned long ulAddrOffset_p,
+			       const char *pszInfoText_p);
+#else
+#define ShbCirTraceBuffer(p0)
+#define ShbLinTraceBuffer(p0)
+#define ShbTraceDump(p0, p1, p2, p3)
+#endif
+
+#undef  INLINE_ENABLED		// disable actual inlining of functions
+#undef  INLINE_FUNCTION
+#define INLINE_FUNCTION		// define INLINE_FUNCTION to nothing
+
+#ifdef __cplusplus
+}
+#endif
+#endif				// #ifndef _SHAREDBUFF_H_
diff --git a/drivers/staging/epl/ShbIpc-LinuxKernel.c b/drivers/staging/epl/ShbIpc-LinuxKernel.c
new file mode 100644
index 00000000000..1d3cb3f13de
--- /dev/null
+++ b/drivers/staging/epl/ShbIpc-LinuxKernel.c
@@ -0,0 +1,966 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      Project independend shared buffer (linear + circular)
+
+  Description:  Implementation of platform specific part for the
+                shared buffer
+                (Implementation for Linux KernelSpace)
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+  2006/06/28 -rs:   V 1.00 (initial version)
+
+****************************************************************************/
+
+#include "global.h"
+#include "SharedBuff.h"
+#include "ShbIpc.h"
+#include "ShbLinuxKernel.h"
+#include "Debug.h"
+
+#include <linux/string.h>
+#include <linux/module.h>
+#include <asm/processor.h>
+//#include <linux/vmalloc.h>
+#include <linux/sched.h>
+#include <linux/param.h>
+#include <linux/spinlock.h>
+#include <linux/wait.h>
+#include <linux/completion.h>
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+#if (!defined(SHBIPC_INLINED)) || defined(SHBIPC_INLINE_ENABLED)
+
+//---------------------------------------------------------------------------
+//  Configuration
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+//  Constant definitions
+//---------------------------------------------------------------------------
+
+#define MAX_LEN_BUFFER_ID       256
+
+#define TIMEOUT_ENTER_ATOMIC    1000	// (ms) for debgging: INFINITE
+#define TIMEOUT_TERM_THREAD     1000
+#define INFINITE                3600
+
+#define SBI_MAGIC_ID            0x5342492B	// magic ID ("SBI+")
+#define SBH_MAGIC_ID            0x5342482A	// magic ID ("SBH*")
+
+#define INVALID_ID              -1
+
+#define TABLE_SIZE              10
+
+//---------------------------------------------------------------------------
+//  Local types
+//---------------------------------------------------------------------------
+
+// This structure is the common header for the shared memory region used
+// by all processes attached this shared memory. It includes common
+// information to administrate/manage the shared buffer from a couple of
+// separated processes (e.g. the refernce counter). This structure is
+// located at the start of the shared memory region itself and exists
+// consequently only one times per shared memory instance.
+typedef struct {
+
+	unsigned long m_ulShMemSize;
+	unsigned long m_ulRefCount;
+	int m_iBufferId;
+//    int                 m_iUserSpaceMem;           //0 for userspace mem   !=0 kernelspace mem
+	spinlock_t m_SpinlockBuffAccess;
+	BOOL m_fNewData;
+	BOOL m_fJobReady;
+	wait_queue_head_t m_WaitQueueNewData;
+	wait_queue_head_t m_WaitQueueJobReady;
+
+#ifndef NDEBUG
+	unsigned long m_ulOwnerProcID;
+#endif
+
+} tShbMemHeader;
+
+// This structure is the "external entry point" from a separate process
+// to get access to a shared buffer. This structure includes all platform
+// resp. target specific information to administrate/manage the shared
+// buffer from a separate process. Every process attached to the shared
+// buffer has its own runtime instance of this structure with its individual
+// runtime data (e.g. the scope of an event handle is limitted to the
+// owner process only). The structure member <m_pShbMemHeader> points
+// to the (process specific) start address of the shared memory region
+// itself.
+typedef struct {
+	unsigned long m_SbiMagicID;	// magic ID ("SBI+")
+//    void*               m_pSharedMem;
+	int m_tThreadNewDataId;
+	long m_lThreadNewDataNice;	// nice value of the new data thread
+	int m_tThreadJobReadyId;
+	unsigned long m_ulFlagsBuffAccess;	// d.k. moved from tShbMemHeader, because each
+	// process needs to store the interrupt flags separately
+	tSigHndlrNewData m_pfnSigHndlrNewData;
+	unsigned long m_ulTimeOutJobReady;
+	tSigHndlrJobReady m_pfnSigHndlrJobReady;
+	tShbMemHeader *m_pShbMemHeader;
+	int m_iThreadTermFlag;
+	struct completion m_CompletionNewData;
+/*
+    struct semaphore    *m_pSemBuffAccess;
+    struct semaphore    *m_pSemNewData;
+    struct semaphore    *m_pSemStopSignalingNewData;
+    struct semaphore    *m_pSemJobReady;
+*/
+#ifndef NDEBUG
+	unsigned long m_ulThreadIDNewData;
+	unsigned long m_ulThreadIDJobReady;
+#endif
+} tShbMemInst;
+
+//---------------------------------------------------------------------------
+//  Prototypes of internal functions
+//---------------------------------------------------------------------------
+
+//tShbMemInst*            ShbIpcGetShbMemInst         (tShbInstance pShbInstance_p);
+//tShbMemHeader*          ShbIpcGetShbMemHeader       (tShbMemInst* pShbMemInst_p);
+
+//---------------------------------------------------------------------------
+//  Get pointer to process local information structure
+//---------------------------------------------------------------------------
+
+static inline tShbMemInst *ShbIpcGetShbMemInst(tShbInstance pShbInstance_p)
+{
+
+	tShbMemInst *pShbMemInst;
+
+	pShbMemInst = (tShbMemInst *) pShbInstance_p;
+
+	return (pShbMemInst);
+
+}
+
+//---------------------------------------------------------------------------
+//  Get pointer to shared memory header
+//---------------------------------------------------------------------------
+
+static inline tShbMemHeader *ShbIpcGetShbMemHeader(tShbMemInst * pShbMemInst_p)
+{
+
+	tShbMemHeader *pShbMemHeader;
+
+	pShbMemHeader = pShbMemInst_p->m_pShbMemHeader;
+
+	return (pShbMemHeader);
+
+}
+
+//  Get pointer to process local information structure
+//#define ShbIpcGetShbMemInst(pShbInstance_p) ((tShbMemInst*)pShbInstance_p)
+
+//  Get pointer to shared memory header
+//#define ShbIpcGetShbMemHeader(pShbMemInst_p) (pShbMemInst_p->m_pShbMemHeader)
+
+// not inlined internal functions
+int ShbIpcThreadSignalNewData(void *pvThreadParam_p);
+int ShbIpcThreadSignalJobReady(void *pvThreadParam_p);
+#endif
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+#if !defined(SHBIPC_INLINE_ENABLED)
+struct sShbMemTable *psMemTableElementFirst_g;
+
+static void *ShbIpcAllocPrivateMem(unsigned long ulMemSize_p);
+static int ShbIpcFindListElement(int iBufferId,
+				 struct sShbMemTable
+				 **ppsReturnMemTableElement);
+static void ShbIpcAppendListElement(struct sShbMemTable *sNewMemTableElement);
+static void ShbIpcDeleteListElement(int iBufferId);
+static void ShbIpcCrc32GenTable(unsigned long aulCrcTable[256]);
+static unsigned long ShbIpcCrc32GetCrc(const char *pcString,
+				       unsigned long aulCrcTable[256]);
+
+#endif
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+#if !defined(SHBIPC_INLINE_ENABLED)
+// not inlined external functions
+
+//---------------------------------------------------------------------------
+//  Initialize IPC for Shared Buffer Module
+//---------------------------------------------------------------------------
+
+tShbError ShbIpcInit(void)
+{
+	psMemTableElementFirst_g = NULL;
+	return (kShbOk);
+
+}
+
+//---------------------------------------------------------------------------
+//  Deinitialize IPC for Shared Buffer Module
+//---------------------------------------------------------------------------
+
+tShbError ShbIpcExit(void)
+{
+
+	return (kShbOk);
+
+}
+
+//---------------------------------------------------------------------------
+//  Allocate Shared Buffer
+//---------------------------------------------------------------------------
+
+tShbError ShbIpcAllocBuffer(unsigned long ulBufferSize_p,
+			    const char *pszBufferID_p,
+			    tShbInstance * ppShbInstance_p,
+			    unsigned int *pfShbNewCreated_p)
+{
+	tShbError ShbError;
+	int iBufferId = 0;
+	unsigned long ulCrc32 = 0;
+	unsigned int uiFirstProcess = 0;
+	unsigned long ulShMemSize;
+	tShbMemHeader *pShbMemHeader;
+	tShbMemInst *pShbMemInst = NULL;
+	tShbInstance pShbInstance;
+	unsigned int fShMemNewCreated = FALSE;
+	void *pSharedMem = NULL;
+	unsigned long aulCrcTable[256];
+	struct sShbMemTable *psMemTableElement;
+
+	DEBUG_LVL_29_TRACE0("ShbIpcAllocBuffer \n");
+	ulShMemSize = ulBufferSize_p + sizeof(tShbMemHeader);
+
+	//create Buffer ID
+	ShbIpcCrc32GenTable(aulCrcTable);
+	ulCrc32 = ShbIpcCrc32GetCrc(pszBufferID_p, aulCrcTable);
+	iBufferId = ulCrc32;
+	DEBUG_LVL_29_TRACE2
+	    ("ShbIpcAllocBuffer BufferSize:%d sizeof(tShb..):%d\n",
+	     ulBufferSize_p, sizeof(tShbMemHeader));
+	DEBUG_LVL_29_TRACE2("ShbIpcAllocBuffer BufferId:%d MemSize:%d\n",
+			    iBufferId, ulShMemSize);
+	//---------------------------------------------------------------
+	// (1) open an existing or create a new shared memory
+	//---------------------------------------------------------------
+	//test if buffer already exists
+	if (ShbIpcFindListElement(iBufferId, &psMemTableElement) == 0) {
+		//Buffer already exists
+		fShMemNewCreated = FALSE;
+		pSharedMem = psMemTableElement->m_pBuffer;
+		DEBUG_LVL_29_TRACE1
+		    ("ShbIpcAllocBuffer attach Buffer at:%p Id:%d\n",
+		     pSharedMem);
+		uiFirstProcess = 1;
+	} else {
+		//create new Buffer
+		fShMemNewCreated = TRUE;
+		uiFirstProcess = 0;
+		pSharedMem = kmalloc(ulShMemSize, GFP_KERNEL);
+		DEBUG_LVL_29_TRACE2
+		    ("ShbIpcAllocBuffer Create New Buffer at:%p Id:%d\n",
+		     pSharedMem, iBufferId);
+		if (pSharedMem == NULL) {
+			//unable to create mem
+			ShbError = kShbOutOfMem;
+			goto Exit;
+		}
+		DEBUG_LVL_29_TRACE0("ShbIpcAllocBuffer create semas\n");
+		// append Element to Mem Table
+		psMemTableElement =
+		    kmalloc(sizeof(struct sShbMemTable), GFP_KERNEL);
+		psMemTableElement->m_iBufferId = iBufferId;
+		psMemTableElement->m_pBuffer = pSharedMem;
+		psMemTableElement->m_psNextMemTableElement = NULL;
+		ShbIpcAppendListElement(psMemTableElement);
+	}
+
+	DEBUG_LVL_29_TRACE0("ShbIpcAllocBuffer update header\n");
+	//update header
+	pShbMemHeader = (tShbMemHeader *) pSharedMem;
+	DEBUG_LVL_29_TRACE1
+	    ("ShbIpcAllocBuffer 0 pShbMemHeader->m_ulShMemSize: %d\n",
+	     pShbMemHeader->m_ulShMemSize);
+	// allocate a memory block from process specific mempool to save
+	// process local information to administrate/manage the shared buffer
+	DEBUG_LVL_29_TRACE0("ShbIpcAllocBuffer alloc private mem\n");
+	pShbMemInst =
+	    (tShbMemInst *) ShbIpcAllocPrivateMem(sizeof(tShbMemInst));
+	if (pShbMemInst == NULL) {
+		ShbError = kShbOutOfMem;
+		goto Exit;
+	}
+	// reset complete header to default values
+	//pShbMemInst->m_SbiMagicID                             = SBI_MAGIC_ID;
+//    pShbMemInst->m_pSharedMem                               = pSharedMem;
+	pShbMemInst->m_tThreadNewDataId = INVALID_ID;
+	pShbMemInst->m_tThreadJobReadyId = INVALID_ID;
+	pShbMemInst->m_pfnSigHndlrNewData = NULL;
+	pShbMemInst->m_ulTimeOutJobReady = 0;
+	pShbMemInst->m_pfnSigHndlrJobReady = NULL;
+	pShbMemInst->m_pShbMemHeader = pShbMemHeader;
+	pShbMemInst->m_iThreadTermFlag = 0;
+
+	// initialize completion etc.
+	init_completion(&pShbMemInst->m_CompletionNewData);
+
+	ShbError = kShbOk;
+	if (fShMemNewCreated) {
+		// this process was the first who wanted to use the shared memory,
+		// so a new shared memory was created
+		// -> setup new header information inside the shared memory region
+		//    itself
+		pShbMemHeader->m_ulShMemSize = ulShMemSize;
+		pShbMemHeader->m_ulRefCount = 1;
+		pShbMemHeader->m_iBufferId = iBufferId;
+		// initialize spinlock
+		spin_lock_init(&pShbMemHeader->m_SpinlockBuffAccess);
+		// initialize wait queues
+		init_waitqueue_head(&pShbMemHeader->m_WaitQueueNewData);
+		init_waitqueue_head(&pShbMemHeader->m_WaitQueueJobReady);
+	} else {
+		// any other process has created the shared memory and this
+		// process only has to attach to it
+		// -> check and update existing header information inside the
+		//    shared memory region itself
+		if (pShbMemHeader->m_ulShMemSize != ulShMemSize) {
+			ShbError = kShbOpenMismatch;
+			goto Exit;
+		}
+		pShbMemHeader->m_ulRefCount++;
+	}
+
+      Exit:
+	pShbInstance = (tShbInstance *) pShbMemInst;
+	*pfShbNewCreated_p = fShMemNewCreated;
+	*ppShbInstance_p = pShbInstance;
+	return (ShbError);
+
+}
+
+//---------------------------------------------------------------------------
+//  Release Shared Buffer
+//---------------------------------------------------------------------------
+
+tShbError ShbIpcReleaseBuffer(tShbInstance pShbInstance_p)
+{
+	tShbMemInst *pShbMemInst;
+	tShbMemHeader *pShbMemHeader;
+	tShbError ShbError;
+	tShbError ShbError2;
+
+	DEBUG_LVL_26_TRACE1("ShbIpcReleaseBuffer(%p)\n", pShbInstance_p);
+	if (pShbInstance_p == NULL) {
+		return (kShbOk);
+	}
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance_p);
+	pShbMemHeader = ShbIpcGetShbMemHeader(pShbMemInst);
+
+	// stop threads in any case, because they are bound to that specific instance
+	ShbError2 = ShbIpcStopSignalingNewData(pShbInstance_p);
+	// d.k.: Whats up with JobReady thread?
+	//       Just wake it up, but without setting the semaphore variable
+	wake_up_interruptible(&pShbMemHeader->m_WaitQueueJobReady);
+
+	if (!--pShbMemHeader->m_ulRefCount) {
+		ShbError = kShbOk;
+		// delete mem table element
+		ShbIpcDeleteListElement(pShbMemHeader->m_iBufferId);
+		// delete shared mem
+		kfree(pShbMemInst->m_pShbMemHeader);
+	} else {
+		ShbError = kShbMemUsedByOtherProcs;
+	}
+	//delete privat mem
+	kfree(pShbMemInst);
+	return (ShbError);
+}
+
+#endif // !defined(SHBIPC_INLINE_ENABLED)
+
+#if (!defined(SHBIPC_INLINED)) || defined(SHBIPC_INLINE_ENABLED)
+
+//---------------------------------------------------------------------------
+//  Enter atomic section for Shared Buffer access
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbIpcEnterAtomicSection(tShbInstance pShbInstance_p)
+{
+
+	tShbMemInst *pShbMemInst;
+	tShbMemHeader *pShbMemHeader;
+	tShbError ShbError = kShbOk;
+
+	if (pShbInstance_p == NULL) {
+		ShbError = kShbInvalidArg;
+		goto Exit;
+	}
+	DEBUG_LVL_29_TRACE0("enter atomic\n");
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance_p);
+	pShbMemHeader = ShbIpcGetShbMemHeader(pShbMemInst);
+
+	// lock interrupts
+	spin_lock_irqsave(&pShbMemHeader->m_SpinlockBuffAccess,
+			  pShbMemInst->m_ulFlagsBuffAccess);
+
+      Exit:
+	return ShbError;
+
+}
+
+//---------------------------------------------------------------------------
+//  Leave atomic section for Shared Buffer access
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbIpcLeaveAtomicSection(tShbInstance pShbInstance_p)
+{
+
+	tShbMemInst *pShbMemInst;
+	tShbMemHeader *pShbMemHeader;
+	tShbError ShbError = kShbOk;
+
+	if (pShbInstance_p == NULL) {
+		ShbError = kShbInvalidArg;
+		goto Exit;
+	}
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance_p);
+	pShbMemHeader = ShbIpcGetShbMemHeader(pShbMemInst);
+	// unlock interrupts
+	spin_unlock_irqrestore(&pShbMemHeader->m_SpinlockBuffAccess,
+			       pShbMemInst->m_ulFlagsBuffAccess);
+
+      Exit:
+	DEBUG_LVL_29_TRACE0("Leave Atomic \n");
+	return ShbError;
+
+}
+
+//---------------------------------------------------------------------------
+//  Start signaling of new data (called from reading process)
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbIpcStartSignalingNewData(tShbInstance
+						      pShbInstance_p,
+						      tSigHndlrNewData
+						      pfnSignalHandlerNewData_p,
+						      tShbPriority
+						      ShbPriority_p)
+{
+	tShbMemInst *pShbMemInst;
+	tShbMemHeader *pShbMemHeader;
+	tShbError ShbError;
+
+	DEBUG_LVL_29_TRACE0("------->ShbIpcStartSignalingNewData\n");
+	if ((pShbInstance_p == NULL) || (pfnSignalHandlerNewData_p == NULL)) {
+		return (kShbInvalidArg);
+	}
+
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance_p);
+	pShbMemHeader = ShbIpcGetShbMemHeader(pShbMemInst);
+	ShbError = kShbOk;
+
+	if ((pShbMemInst->m_tThreadNewDataId != INVALID_ID)
+	    || (pShbMemInst->m_pfnSigHndlrNewData != NULL)) {
+		ShbError = kShbAlreadySignaling;
+		goto Exit;
+	}
+	DEBUG_LVL_26_TRACE2
+	    ("ShbIpcStartSignalingNewData(%p) m_pfnSigHndlrNewData = %p\n",
+	     pShbInstance_p, pfnSignalHandlerNewData_p);
+	pShbMemInst->m_pfnSigHndlrNewData = pfnSignalHandlerNewData_p;
+	pShbMemHeader->m_fNewData = FALSE;
+	pShbMemInst->m_iThreadTermFlag = 0;
+
+	switch (ShbPriority_p) {
+	case kShbPriorityLow:
+		pShbMemInst->m_lThreadNewDataNice = -2;
+		break;
+
+	case kShbPriorityNormal:
+		pShbMemInst->m_lThreadNewDataNice = -9;
+		break;
+
+	case kshbPriorityHigh:
+		pShbMemInst->m_lThreadNewDataNice = -20;
+		break;
+
+	}
+
+	//create thread for signalling new data
+	pShbMemInst->m_tThreadNewDataId =
+	    kernel_thread(ShbIpcThreadSignalNewData, pShbInstance_p,
+			  CLONE_KERNEL);
+
+      Exit:
+	return ShbError;
+
+}
+
+//---------------------------------------------------------------------------
+//  Stop signaling of new data (called from reading process)
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbIpcStopSignalingNewData(tShbInstance
+						     pShbInstance_p)
+{
+	tShbMemInst *pShbMemInst;
+	tShbMemHeader *pShbMemHeader;
+	tShbError ShbError;
+
+	DEBUG_LVL_29_TRACE0("------->ShbIpcStopSignalingNewData\n");
+	if (pShbInstance_p == NULL) {
+		return (kShbInvalidArg);
+	}
+	ShbError = kShbOk;
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance_p);
+	pShbMemHeader = ShbIpcGetShbMemHeader(pShbMemInst);
+
+	DEBUG_LVL_26_TRACE2
+	    ("ShbIpcStopSignalingNewData(%p) pfnSignHndlrNewData=%p\n",
+	     pShbInstance_p, pShbMemInst->m_pfnSigHndlrNewData);
+	if (pShbMemInst->m_pfnSigHndlrNewData != NULL) {	// signal handler was set before
+		int iErr;
+		//set termination flag in mem header
+		pShbMemInst->m_iThreadTermFlag = 1;
+
+		// check if thread is still running at all by sending the null-signal to this thread
+		/* iErr = kill_proc(pShbMemInst->m_tThreadNewDataId, 0, 1); */
+		iErr = send_sig(0, pShbMemInst->m_tThreadNewDataId, 1);
+		if (iErr == 0) {
+			// wake up thread, because it is still running
+			wake_up_interruptible(&pShbMemHeader->
+					      m_WaitQueueNewData);
+
+			//wait for termination of thread
+			wait_for_completion(&pShbMemInst->m_CompletionNewData);
+		}
+
+		pShbMemInst->m_pfnSigHndlrNewData = NULL;
+		pShbMemInst->m_tThreadNewDataId = INVALID_ID;
+	}
+
+	return ShbError;
+
+}
+
+//---------------------------------------------------------------------------
+//  Signal new data (called from writing process)
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbIpcSignalNewData(tShbInstance pShbInstance_p)
+{
+	tShbMemHeader *pShbMemHeader;
+
+	if (pShbInstance_p == NULL) {
+		return (kShbInvalidArg);
+	}
+	pShbMemHeader =
+	    ShbIpcGetShbMemHeader(ShbIpcGetShbMemInst(pShbInstance_p));
+	//set semaphore
+	pShbMemHeader->m_fNewData = TRUE;
+	DEBUG_LVL_29_TRACE0("ShbIpcSignalNewData set Sem -> New Data\n");
+
+	wake_up_interruptible(&pShbMemHeader->m_WaitQueueNewData);
+	return (kShbOk);
+}
+
+//---------------------------------------------------------------------------
+//  Start signaling for job ready (called from waiting process)
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbIpcStartSignalingJobReady(tShbInstance
+						       pShbInstance_p,
+						       unsigned long
+						       ulTimeOut_p,
+						       tSigHndlrJobReady
+						       pfnSignalHandlerJobReady_p)
+{
+	tShbMemInst *pShbMemInst;
+	tShbMemHeader *pShbMemHeader;
+	tShbError ShbError;
+
+	if ((pShbInstance_p == NULL) || (pfnSignalHandlerJobReady_p == NULL)) {
+		return (kShbInvalidArg);
+	}
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance_p);
+	pShbMemHeader = ShbIpcGetShbMemHeader(pShbMemInst);
+
+	ShbError = kShbOk;
+	if ((pShbMemInst->m_tThreadJobReadyId != INVALID_ID)
+	    || (pShbMemInst->m_pfnSigHndlrJobReady != NULL)) {
+		ShbError = kShbAlreadySignaling;
+		goto Exit;
+	}
+	pShbMemInst->m_ulTimeOutJobReady = ulTimeOut_p;
+	pShbMemInst->m_pfnSigHndlrJobReady = pfnSignalHandlerJobReady_p;
+	pShbMemHeader->m_fJobReady = FALSE;
+	//create thread for signalling new data
+	pShbMemInst->m_tThreadJobReadyId =
+	    kernel_thread(ShbIpcThreadSignalJobReady, pShbInstance_p,
+			  CLONE_KERNEL);
+      Exit:
+	return ShbError;
+}
+
+//---------------------------------------------------------------------------
+//  Signal job ready (called from executing process)
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbIpcSignalJobReady(tShbInstance pShbInstance_p)
+{
+	tShbMemHeader *pShbMemHeader;
+
+	DEBUG_LVL_29_TRACE0("ShbIpcSignalJobReady\n");
+	if (pShbInstance_p == NULL) {
+		return (kShbInvalidArg);
+	}
+	pShbMemHeader =
+	    ShbIpcGetShbMemHeader(ShbIpcGetShbMemInst(pShbInstance_p));
+	//set semaphore
+	pShbMemHeader->m_fJobReady = TRUE;
+	DEBUG_LVL_29_TRACE0("ShbIpcSignalJobReady set Sem -> Job Ready \n");
+
+	wake_up_interruptible(&pShbMemHeader->m_WaitQueueJobReady);
+	return (kShbOk);
+}
+
+//---------------------------------------------------------------------------
+//  Get pointer to common used share memory area
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION void *ShbIpcGetShMemPtr(tShbInstance pShbInstance_p)
+{
+
+	tShbMemHeader *pShbMemHeader;
+	void *pShbShMemPtr;
+
+	pShbMemHeader =
+	    ShbIpcGetShbMemHeader(ShbIpcGetShbMemInst(pShbInstance_p));
+	if (pShbMemHeader != NULL) {
+		pShbShMemPtr = (BYTE *) pShbMemHeader + sizeof(tShbMemHeader);
+	} else {
+		pShbShMemPtr = NULL;
+	}
+
+	return (pShbShMemPtr);
+
+}
+
+#endif
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+#if !defined(SHBIPC_INLINE_ENABLED)
+
+//---------------------------------------------------------------------------
+//  Get pointer to process local information structure
+//---------------------------------------------------------------------------
+
+/*tShbMemInst*  ShbIpcGetShbMemInst (
+    tShbInstance pShbInstance_p)
+{
+
+tShbMemInst*  pShbMemInst;
+
+    pShbMemInst = (tShbMemInst*)pShbInstance_p;
+
+    return (pShbMemInst);
+
+}
+*/
+
+//---------------------------------------------------------------------------
+//  Get pointer to shared memory header
+//---------------------------------------------------------------------------
+
+/*tShbMemHeader*  ShbIpcGetShbMemHeader (
+    tShbMemInst* pShbMemInst_p)
+{
+
+tShbMemHeader*  pShbMemHeader;
+
+    pShbMemHeader = pShbMemInst_p->m_pShbMemHeader;
+
+    return (pShbMemHeader);
+
+}
+*/
+
+//---------------------------------------------------------------------------
+//  Allocate a memory block from process specific mempool
+//---------------------------------------------------------------------------
+
+static void *ShbIpcAllocPrivateMem(unsigned long ulMemSize_p)
+{
+	tShbError ShbError;
+	void *pMem;
+
+	DEBUG_LVL_29_TRACE0("ShbIpcAllocPrivateMem \n");
+	//get private mem
+	pMem = kmalloc(ulMemSize_p, GFP_KERNEL);
+	if (pMem == NULL) {
+		//unable to create mem
+		ShbError = kShbOutOfMem;
+		goto Exit;
+	}
+      Exit:
+	return (pMem);
+
+}
+
+//---------------------------------------------------------------------------
+//  Thread for new data signaling
+//---------------------------------------------------------------------------
+
+int ShbIpcThreadSignalNewData(void *pvThreadParam_p)
+{
+	tShbInstance pShbInstance;
+	tShbMemInst *pShbMemInst;
+	tShbMemHeader *pShbMemHeader;
+	int iRetVal = -1;
+	int fCallAgain;
+
+	daemonize("ShbND%p", pvThreadParam_p);
+	allow_signal(SIGTERM);
+	pShbInstance = (tShbMemInst *) pvThreadParam_p;
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance);
+	pShbMemHeader = ShbIpcGetShbMemHeader(pShbMemInst);
+
+	DEBUG_LVL_26_TRACE1("ShbIpcThreadSignalNewData(%p)\n", pvThreadParam_p);
+
+	set_user_nice(current, pShbMemInst->m_lThreadNewDataNice);
+
+//            DEBUG_LVL_29_TRACE1("ShbIpcThreadSignalNewData wait for New Data Sem %p\n",pShbMemInst->m_pSemNewData);
+	do {
+		iRetVal =
+		    wait_event_interruptible(pShbMemHeader->m_WaitQueueNewData,
+					     (pShbMemInst->m_iThreadTermFlag !=
+					      0)
+					     || (pShbMemHeader->m_fNewData !=
+						 FALSE));
+
+		if (iRetVal != 0) {	// signal pending
+			break;
+		}
+
+		if (pShbMemHeader->m_fNewData != FALSE) {
+			pShbMemHeader->m_fNewData = FALSE;
+			do {
+				fCallAgain =
+				    pShbMemInst->
+				    m_pfnSigHndlrNewData(pShbInstance);
+				// call scheduler, which will execute any task with higher priority
+				schedule();
+			} while (fCallAgain != FALSE);
+		}
+	} while (pShbMemInst->m_iThreadTermFlag == 0);
+	DEBUG_LVL_29_TRACE0("ShbIpcThreadSignalNewData terminated \n");
+	//set thread completed
+	complete_and_exit(&pShbMemInst->m_CompletionNewData, 0);
+	return 0;
+}
+
+//---------------------------------------------------------------------------
+//  Thread for new data Job Ready signaling
+//---------------------------------------------------------------------------
+
+int ShbIpcThreadSignalJobReady(void *pvThreadParam_p)
+{
+	tShbInstance pShbInstance;
+	tShbMemInst *pShbMemInst;
+	tShbMemHeader *pShbMemHeader;
+	long lTimeOut;
+	int iRetVal = -1;
+
+	daemonize("ShbJR%p", pvThreadParam_p);
+	allow_signal(SIGTERM);
+	pShbInstance = (tShbMemInst *) pvThreadParam_p;
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance);
+	pShbMemHeader = ShbIpcGetShbMemHeader(pShbMemInst);
+
+	DEBUG_LVL_29_TRACE0
+	    ("ShbIpcThreadSignalJobReady wait for job ready Sem\n");
+	if (pShbMemInst->m_ulTimeOutJobReady != 0) {
+		lTimeOut = (long)pShbMemInst->m_ulTimeOutJobReady;
+		//wait for job ready semaphore
+		iRetVal =
+		    wait_event_interruptible_timeout(pShbMemHeader->
+						     m_WaitQueueJobReady,
+						     (pShbMemHeader->
+						      m_fJobReady != FALSE),
+						     lTimeOut);
+	} else {
+		//wait for job ready semaphore
+		iRetVal =
+		    wait_event_interruptible(pShbMemHeader->m_WaitQueueJobReady,
+					     (pShbMemHeader->m_fJobReady !=
+					      FALSE));
+	}
+
+	if (pShbMemInst->m_pfnSigHndlrJobReady != NULL) {
+		//call Handler
+		pShbMemInst->m_pfnSigHndlrJobReady(pShbInstance,
+						   !pShbMemHeader->m_fJobReady);
+	}
+
+	pShbMemInst->m_pfnSigHndlrJobReady = NULL;
+	return 0;
+}
+
+//Build the crc table
+static void ShbIpcCrc32GenTable(unsigned long aulCrcTable[256])
+{
+	unsigned long ulCrc, ulPoly;
+	int iIndexI, iIndexJ;
+
+	ulPoly = 0xEDB88320L;
+	for (iIndexI = 0; iIndexI < 256; iIndexI++) {
+		ulCrc = iIndexI;
+		for (iIndexJ = 8; iIndexJ > 0; iIndexJ--) {
+			if (ulCrc & 1) {
+				ulCrc = (ulCrc >> 1) ^ ulPoly;
+			} else {
+				ulCrc >>= 1;
+			}
+		}
+		aulCrcTable[iIndexI] = ulCrc;
+	}
+}
+
+//Calculate the crc value
+static unsigned long ShbIpcCrc32GetCrc(const char *pcString,
+				       unsigned long aulCrcTable[256])
+{
+	unsigned long ulCrc;
+	int iIndex;
+
+	ulCrc = 0xFFFFFFFF;
+	for (iIndex = 0; iIndex < strlen(pcString); iIndex++) {
+		ulCrc =
+		    ((ulCrc >> 8) & 0x00FFFFFF) ^
+		    aulCrcTable[(ulCrc ^ pcString[iIndex]) & 0xFF];
+	}
+	return (ulCrc ^ 0xFFFFFFFF);
+
+}
+
+static void ShbIpcAppendListElement(struct sShbMemTable *psNewMemTableElement)
+{
+	struct sShbMemTable *psMemTableElement = psMemTableElementFirst_g;
+	psNewMemTableElement->m_psNextMemTableElement = NULL;
+
+	if (psMemTableElementFirst_g != NULL) {	/* sind Elemente vorhanden */
+		while (psMemTableElement->m_psNextMemTableElement != NULL) {	/* suche das letzte Element */
+			psMemTableElement =
+			    psMemTableElement->m_psNextMemTableElement;
+		}
+		psMemTableElement->m_psNextMemTableElement = psNewMemTableElement;	/*  Haenge das Element hinten an */
+	} else {		/* wenn die liste leer ist, bin ich das erste Element */
+		psMemTableElementFirst_g = psNewMemTableElement;
+	}
+}
+
+static int ShbIpcFindListElement(int iBufferId,
+				 struct sShbMemTable **ppsReturnMemTableElement)
+{
+	struct sShbMemTable *psMemTableElement = psMemTableElementFirst_g;
+	while (psMemTableElement != NULL) {
+		if (psMemTableElement->m_iBufferId == iBufferId) {
+//printk("ShbIpcFindListElement Buffer at:%p Id:%d\n",psMemTableElement->m_pBuffer,psMemTableElement->m_iBufferId);
+			*ppsReturnMemTableElement = psMemTableElement;
+//printk("ShbIpcFindListElement Buffer at:%p Id:%d\n",(*ppsReturnMemTableElement)->m_pBuffer,(*ppsReturnMemTableElement)->m_iBufferId);
+			return 0;
+		}
+		psMemTableElement = psMemTableElement->m_psNextMemTableElement;
+	}
+	return -1;
+}
+
+static void ShbIpcDeleteListElement(int iBufferId)
+{
+	struct sShbMemTable *psMemTableElement = psMemTableElementFirst_g;
+	struct sShbMemTable *psMemTableElementOld = psMemTableElementFirst_g;
+	if (psMemTableElement != NULL) {
+		while ((psMemTableElement != NULL)
+		       && (psMemTableElement->m_iBufferId != iBufferId)) {
+			psMemTableElementOld = psMemTableElement;
+			psMemTableElement =
+			    psMemTableElement->m_psNextMemTableElement;
+		}
+		if (psMemTableElement != NULL) {
+			if (psMemTableElement != psMemTableElementFirst_g) {
+				psMemTableElementOld->m_psNextMemTableElement =
+				    psMemTableElement->m_psNextMemTableElement;
+				kfree(psMemTableElement);
+			} else {
+				kfree(psMemTableElement);
+				psMemTableElementFirst_g = NULL;
+			}
+
+		}
+	}
+
+}
+
+#endif
diff --git a/drivers/staging/epl/ShbIpc-Win32.c b/drivers/staging/epl/ShbIpc-Win32.c
new file mode 100644
index 00000000000..b9181471ae0
--- /dev/null
+++ b/drivers/staging/epl/ShbIpc-Win32.c
@@ -0,0 +1,1202 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      Project independend shared buffer (linear + circular)
+
+  Description:  Implementation of platform specific part for the
+                shared buffer
+                (Implementation for Win32)
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+  2006/06/27 -rs:   V 1.00 (initial version)
+
+****************************************************************************/
+
+#define WINVER       0x0400	// #defines necessary for usage of
+#define _WIN32_WINNT 0x0400	// function <SignalObjectAndWait>
+
+#include <windows.h>
+#include <stdio.h>
+#include "global.h"
+#include "sharedbuff.h"
+#include "shbipc.h"
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+#if (!defined(SHBIPC_INLINED)) || defined(SHBIPC_INLINE_ENABLED)
+
+//---------------------------------------------------------------------------
+//  Configuration
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+//  Constant definitions
+//---------------------------------------------------------------------------
+
+#define MAX_LEN_BUFFER_ID       MAX_PATH
+
+#define IDX_EVENT_NEW_DATA      0
+#define IDX_EVENT_TERM_REQU     1
+#define IDX_EVENT_TERM_RESP     2
+
+#define NAME_MUTEX_BUFF_ACCESS  "BuffAccess"
+#define NAME_EVENT_NEW_DATA     "NewData"
+#define NAME_EVENT_TERM_REQU    "TermRequ"
+#define NAME_EVENT_TERM_RESP    "TermResp"
+#define NAME_EVENT_JOB_READY    "JobReady"
+
+#define TIMEOUT_ENTER_ATOMIC    1000	// for debgging: INFINITE
+#define TIMEOUT_TERM_THREAD     2000
+
+#define SBI_MAGIC_ID            0x5342492B	// magic ID ("SBI+")
+#define SBH_MAGIC_ID            0x5342482A	// magic ID ("SBH*")
+
+//---------------------------------------------------------------------------
+//  Local types
+//---------------------------------------------------------------------------
+
+// This structure is the common header for the shared memory region used
+// by all processes attached this shared memory. It includes common
+// information to administrate/manage the shared buffer from a couple of
+// separated processes (e.g. the refernce counter). This structure is
+// located at the start of the shared memory region itself and exists
+// consequently only one times per shared memory instance.
+typedef struct {
+	unsigned long m_SbhMagicID;	// magic ID ("SBH*")
+	unsigned long m_ulShMemSize;
+	unsigned long m_ulRefCount;
+	char m_szBufferID[MAX_LEN_BUFFER_ID];
+
+#ifndef NDEBUG
+	unsigned long m_ulOwnerProcID;
+#endif
+
+} tShbMemHeader;
+
+// This structure is the "external entry point" from a separate process
+// to get access to a shared buffer. This structure includes all platform
+// resp. target specific information to administrate/manage the shared
+// buffer from a separate process. Every process attached to the shared
+// buffer has its own runtime instance of this structure with its individual
+// runtime data (e.g. the scope of an event handle is limitted to the
+// owner process only). The structure member <m_pShbMemHeader> points
+// to the (process specific) start address of the shared memory region
+// itself.
+typedef struct {
+	unsigned long m_SbiMagicID;	// magic ID ("SBI+")
+	HANDLE m_hSharedMem;
+	HANDLE m_hMutexBuffAccess;
+	HANDLE m_hThreadNewData;	// thraed to signal that new data are available
+	HANDLE m_ahEventNewData[3];	// IDX_EVENT_NEW_DATA + IDX_EVENT_TERM_REQU + ID_EVENT_TERM_RESP
+	tSigHndlrNewData m_pfnSigHndlrNewData;
+	HANDLE m_hThreadJobReady;	// thread to signal that a job/operation is ready now (e.g. reset buffer)
+	HANDLE m_hEventJobReady;
+	unsigned long m_ulTimeOutJobReady;
+	tSigHndlrJobReady m_pfnSigHndlrJobReady;
+	tShbMemHeader *m_pShbMemHeader;
+
+#ifndef NDEBUG
+	unsigned long m_ulThreadIDNewData;
+	unsigned long m_ulThreadIDJobReady;
+#endif
+
+} tShbMemInst;
+
+//---------------------------------------------------------------------------
+//  Global variables
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+//  Local variables
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+//  Prototypes of internal functions
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+//  Get pointer to process local information structure
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbMemInst *ShbIpcGetShbMemInst(tShbInstance pShbInstance_p)
+{
+
+	tShbMemInst *pShbMemInst;
+
+	pShbMemInst = (tShbMemInst *) pShbInstance_p;
+	ASSERT(pShbMemInst->m_SbiMagicID == SBI_MAGIC_ID);
+
+	return (pShbMemInst);
+
+}
+
+//---------------------------------------------------------------------------
+//  Get pointer to shared memory header
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbMemHeader *ShbIpcGetShbMemHeader(tShbInstance
+						     pShbInstance_p)
+{
+
+	tShbMemInst *pShbMemInst;
+	tShbMemHeader *pShbMemHeader;
+
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance_p);
+	pShbMemHeader = pShbMemInst->m_pShbMemHeader;
+	ASSERT(pShbMemHeader->m_SbhMagicID == SBH_MAGIC_ID);
+
+	return (pShbMemHeader);
+
+}
+
+// not inlined internal functions
+DWORD WINAPI ShbIpcThreadSignalNewData(LPVOID pvThreadParam_p);
+DWORD WINAPI ShbIpcThreadSignalJobReady(LPVOID pvThreadParam_p);
+const char *ShbIpcGetUniformObjectName(const char *pszEventJobName_p,
+				       const char *pszBufferID_p,
+				       BOOL fGlobalObject_p);
+
+#endif
+
+#if !defined(SHBIPC_INLINE_ENABLED)
+// true internal functions (not inlined)
+static void *ShbIpcAllocPrivateMem(unsigned long ulMemSize_p);
+static void ShbIpcReleasePrivateMem(void *pMem_p);
+#endif
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+#if !defined(SHBIPC_INLINE_ENABLED)
+// not inlined external functions
+
+//---------------------------------------------------------------------------
+//  Initialize IPC for Shared Buffer Module
+//---------------------------------------------------------------------------
+
+tShbError ShbIpcInit(void)
+{
+
+	return (kShbOk);
+
+}
+
+//---------------------------------------------------------------------------
+//  Deinitialize IPC for Shared Buffer Module
+//---------------------------------------------------------------------------
+
+tShbError ShbIpcExit(void)
+{
+
+	return (kShbOk);
+
+}
+
+//---------------------------------------------------------------------------
+//  Allocate Shared Buffer
+//---------------------------------------------------------------------------
+
+tShbError ShbIpcAllocBuffer(unsigned long ulBufferSize_p,
+			    const char *pszBufferID_p,
+			    tShbInstance * ppShbInstance_p,
+			    unsigned int *pfShbNewCreated_p)
+{
+
+	HANDLE hSharedMem;
+	LPVOID pSharedMem;
+	unsigned long ulShMemSize;
+	tShbMemInst *pShbMemInst;
+	tShbMemHeader *pShbMemHeader;
+	tShbInstance pShbInstance;
+	unsigned int fShMemNewCreated;
+	const char *pszObjectName;
+	HANDLE hMutexBuffAccess;
+	HANDLE hEventNewData;
+	HANDLE hEventJobReady;
+	tShbError ShbError;
+
+	ulShMemSize = ulBufferSize_p + sizeof(tShbMemHeader);
+	pSharedMem = NULL;
+	pShbInstance = NULL;
+	fShMemNewCreated = FALSE;
+	ShbError = kShbOk;
+
+	//---------------------------------------------------------------
+	// (1) open an existing or create a new shared memory
+	//---------------------------------------------------------------
+	// try to open an already existing shared memory
+	// (created by an another process)
+	hSharedMem = OpenFileMapping(FILE_MAP_ALL_ACCESS,	// DWORD dwDesiredAccess
+				     FALSE,	// BOOL bInheritHandle
+				     pszBufferID_p);	// LPCTSTR lpName
+	if (hSharedMem != NULL) {
+		// a shared memory already exists
+		fShMemNewCreated = FALSE;
+	} else {
+		// it seams that this process is the first who wants to use the
+		// shared memory, so it has to create a new shared memory
+		hSharedMem = CreateFileMapping(INVALID_HANDLE_VALUE,	// HANDLE hFile
+					       NULL,	// LPSECURITY_ATTRIBUTES lpAttributes
+					       PAGE_READWRITE,	// DWORD flProtect
+					       0,	// DWORD dwMaximumSizeHigh
+					       ulShMemSize,	// DWORD dwMaximumSizeLow
+					       pszBufferID_p);	// LPCTSTR lpName
+
+		fShMemNewCreated = TRUE;
+	}
+
+	if (hSharedMem == NULL) {
+		ShbError = kShbOutOfMem;
+		goto Exit;
+	}
+
+	//---------------------------------------------------------------
+	// (2) get the pointer to the shared memory
+	//---------------------------------------------------------------
+	pSharedMem = MapViewOfFile(hSharedMem,	// HANDLE hFileMappingObject
+				   FILE_MAP_ALL_ACCESS,	// DWORD dwDesiredAccess,
+				   0,	// DWORD dwFileOffsetHigh,
+				   0,	// DWORD dwFileOffsetLow,
+				   ulShMemSize);	// SIZE_T dwNumberOfBytesToMap
+
+	if (pSharedMem == NULL) {
+		ShbError = kShbOutOfMem;
+		goto Exit;
+	}
+
+	//---------------------------------------------------------------
+	// (3) setup or update header and management information
+	//---------------------------------------------------------------
+	pShbMemHeader = (tShbMemHeader *) pSharedMem;
+
+	// allocate a memory block from process specific mempool to save
+	// process local information to administrate/manage the shared buffer
+	pShbMemInst =
+	    (tShbMemInst *) ShbIpcAllocPrivateMem(sizeof(tShbMemInst));
+	if (pShbMemInst == NULL) {
+		ShbError = kShbOutOfMem;
+		goto Exit;
+	}
+	// reset complete header to default values
+	pShbMemInst->m_SbiMagicID = SBI_MAGIC_ID;
+	pShbMemInst->m_hSharedMem = hSharedMem;
+	pShbMemInst->m_hMutexBuffAccess = INVALID_HANDLE_VALUE;
+	pShbMemInst->m_hThreadNewData = INVALID_HANDLE_VALUE;
+	pShbMemInst->m_ahEventNewData[IDX_EVENT_NEW_DATA] =
+	    INVALID_HANDLE_VALUE;
+	pShbMemInst->m_ahEventNewData[IDX_EVENT_TERM_REQU] =
+	    INVALID_HANDLE_VALUE;
+	pShbMemInst->m_ahEventNewData[IDX_EVENT_TERM_RESP] =
+	    INVALID_HANDLE_VALUE;
+	pShbMemInst->m_pfnSigHndlrNewData = NULL;
+	pShbMemInst->m_hThreadJobReady = INVALID_HANDLE_VALUE;
+	pShbMemInst->m_hEventJobReady = INVALID_HANDLE_VALUE;
+	pShbMemInst->m_ulTimeOutJobReady = 0;
+	pShbMemInst->m_pfnSigHndlrJobReady = NULL;
+	pShbMemInst->m_pShbMemHeader = pShbMemHeader;
+
+#ifndef NDEBUG
+	{
+		pShbMemInst->m_ulThreadIDNewData = 0;
+		pShbMemInst->m_ulThreadIDJobReady = 0;
+	}
+#endif
+
+	// create mutex for buffer access
+	pszObjectName =
+	    ShbIpcGetUniformObjectName(NAME_MUTEX_BUFF_ACCESS, pszBufferID_p,
+				       TRUE);
+	hMutexBuffAccess = CreateMutex(NULL,	// LPSECURITY_ATTRIBUTES lpMutexAttributes
+				       FALSE,	// BOOL bInitialOwner
+				       pszObjectName);	// LPCTSTR lpName
+	pShbMemInst->m_hMutexBuffAccess = hMutexBuffAccess;
+	ASSERT(pShbMemInst->m_hMutexBuffAccess != NULL);
+
+	// The EventNewData is used for signaling of new data after a write
+	// operation (SetEvent) as well as for waiting for new data on the
+	// reader side (WaitForMultipleObjects). Because it's not known if
+	// this process will be read or write data, the event will be
+	// always created here.
+	pszObjectName =
+	    ShbIpcGetUniformObjectName(NAME_EVENT_NEW_DATA, pszBufferID_p,
+				       TRUE);
+	hEventNewData = CreateEvent(NULL,	// LPSECURITY_ATTRIBUTES lpEventAttributes
+				    FALSE,	// BOOL bManualReset
+				    FALSE,	// BOOL bInitialState
+				    pszObjectName);	// LPCTSTR lpName
+	pShbMemInst->m_ahEventNewData[IDX_EVENT_NEW_DATA] = hEventNewData;
+	ASSERT(pShbMemInst->m_ahEventNewData[IDX_EVENT_NEW_DATA] != NULL);
+
+	// The EventJobReady is used for signaling that a job is done (SetEvent)
+	// as well as for waiting for finishing of a job (WaitForMultipleObjects).
+	// Because it's not known if this process will signal or wait, the event
+	// will be always created here.
+	pszObjectName =
+	    ShbIpcGetUniformObjectName(NAME_EVENT_JOB_READY, pszBufferID_p,
+				       TRUE);
+	hEventJobReady = CreateEvent(NULL,	// LPSECURITY_ATTRIBUTES lpEventAttributes
+				     FALSE,	// BOOL bManualReset
+				     FALSE,	// BOOL bInitialState
+				     pszObjectName);	// LPCTSTR lpName
+	pShbMemInst->m_hEventJobReady = hEventJobReady;
+	ASSERT(pShbMemInst->m_hEventJobReady != NULL);
+
+	if (fShMemNewCreated) {
+		// this process was the first who wanted to use the shared memory,
+		// so a new shared memory was created
+		// -> setup new header information inside the shared memory region
+		//    itself
+		pShbMemHeader->m_SbhMagicID = SBH_MAGIC_ID;
+		pShbMemHeader->m_ulShMemSize = ulShMemSize;
+		pShbMemHeader->m_ulRefCount = 1;
+		strncpy(pShbMemHeader->m_szBufferID, pszBufferID_p,
+			sizeof(pShbMemHeader->m_szBufferID) - 1);
+
+#ifndef NDEBUG
+		{
+			pShbMemHeader->m_ulOwnerProcID = GetCurrentProcessId();
+		}
+#endif
+	} else {
+		// any other process has created the shared memory and this
+		// process has only attached to it
+		// -> check and update existing header information inside the
+		//    shared memory region itself
+		if (pShbMemHeader->m_ulShMemSize != ulShMemSize) {
+			ShbError = kShbOpenMismatch;
+			goto Exit;
+		}
+#ifndef NDEBUG
+		{
+			if (strncmp
+			    (pShbMemHeader->m_szBufferID, pszBufferID_p,
+			     sizeof(pShbMemHeader->m_szBufferID) - 1)) {
+				ShbError = kShbOpenMismatch;
+				goto Exit;
+			}
+		}
+#endif
+
+		pShbMemHeader->m_ulRefCount++;
+	}
+
+	// set abstarct "handle" for returning to application
+	pShbInstance = (tShbInstance *) pShbMemInst;
+
+      Exit:
+
+	if (ShbError != kShbOk) {
+		if (pShbMemInst != NULL) {
+			ShbIpcReleasePrivateMem(pShbMemInst);
+		}
+		if (pSharedMem != NULL) {
+			UnmapViewOfFile(pSharedMem);
+		}
+		if (hSharedMem != NULL) {
+			CloseHandle(hSharedMem);
+		}
+	}
+
+	*pfShbNewCreated_p = fShMemNewCreated;
+	*ppShbInstance_p = pShbInstance;
+
+	return (ShbError);
+
+}
+
+//---------------------------------------------------------------------------
+//  Release Shared Buffer
+//---------------------------------------------------------------------------
+
+tShbError ShbIpcReleaseBuffer(tShbInstance pShbInstance_p)
+{
+
+	tShbMemInst *pShbMemInst;
+	tShbMemHeader *pShbMemHeader;
+	HANDLE hEventNewData;
+	HANDLE hMutexBuffAccess;
+	tShbError ShbError;
+	tShbError ShbError2;
+
+	if (pShbInstance_p == NULL) {
+		return (kShbOk);
+	}
+
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance_p);
+	pShbMemHeader = ShbIpcGetShbMemHeader(pShbInstance_p);
+
+	if (!--pShbMemHeader->m_ulRefCount) {
+		ShbError = kShbOk;
+	} else {
+		ShbError = kShbMemUsedByOtherProcs;
+	}
+
+	ShbError2 = ShbIpcStopSignalingNewData(pShbInstance_p);
+	hEventNewData = pShbMemInst->m_ahEventNewData[IDX_EVENT_NEW_DATA];
+	if (hEventNewData != INVALID_HANDLE_VALUE) {
+		CloseHandle(hEventNewData);
+		pShbMemInst->m_ahEventNewData[IDX_EVENT_NEW_DATA] =
+		    INVALID_HANDLE_VALUE;
+	}
+
+	hMutexBuffAccess = pShbMemInst->m_hMutexBuffAccess;
+	if (hMutexBuffAccess != INVALID_HANDLE_VALUE) {
+		CloseHandle(hMutexBuffAccess);
+		pShbMemInst->m_hMutexBuffAccess = INVALID_HANDLE_VALUE;
+	}
+
+	UnmapViewOfFile(pShbMemHeader);
+	if (pShbMemInst->m_hSharedMem != INVALID_HANDLE_VALUE) {
+		CloseHandle(pShbMemInst->m_hSharedMem);
+		pShbMemInst->m_hSharedMem = INVALID_HANDLE_VALUE;
+	}
+
+	ShbIpcReleasePrivateMem(pShbMemInst);
+
+	if (ShbError == kShbOk) {
+		ShbError = ShbError2;
+	}
+
+	return (ShbError);
+
+}
+
+#endif // !defined(SHBIPC_INLINE_ENABLED)
+
+#if (!defined(SHBIPC_INLINED)) || defined(SHBIPC_INLINE_ENABLED)
+
+//---------------------------------------------------------------------------
+//  Enter atomic section for Shared Buffer access
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbIpcEnterAtomicSection(tShbInstance pShbInstance_p)
+{
+
+	tShbMemInst *pShbMemInst;
+	HANDLE hMutexBuffAccess;
+	DWORD dwWaitResult;
+	tShbError ShbError;
+
+	if (pShbInstance_p == NULL) {
+		return (kShbInvalidArg);
+	}
+
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance_p);
+	ShbError = kShbOk;
+
+	hMutexBuffAccess = pShbMemInst->m_hMutexBuffAccess;
+	if (hMutexBuffAccess != INVALID_HANDLE_VALUE) {
+		dwWaitResult =
+		    WaitForSingleObject(hMutexBuffAccess, TIMEOUT_ENTER_ATOMIC);
+		switch (dwWaitResult) {
+		case WAIT_OBJECT_0 + 0:
+			{
+				break;
+			}
+
+		case WAIT_TIMEOUT:
+			{
+				TRACE0
+				    ("\nShbIpcEnterAtomicSection(): WAIT_TIMEOUT");
+				ASSERT(0);
+				ShbError = kShbBufferInvalid;
+				break;
+			}
+
+		case WAIT_ABANDONED:
+			{
+				TRACE0
+				    ("\nShbIpcEnterAtomicSection(): WAIT_ABANDONED");
+				ASSERT(0);
+				ShbError = kShbBufferInvalid;
+				break;
+			}
+
+		case WAIT_FAILED:
+			{
+				TRACE1
+				    ("\nShbIpcEnterAtomicSection(): WAIT_FAILED -> LastError=%ld",
+				     GetLastError());
+				ASSERT(0);
+				ShbError = kShbBufferInvalid;
+				break;
+			}
+
+		default:
+			{
+				TRACE1
+				    ("\nShbIpcEnterAtomicSection(): unknown error -> LastError=%ld",
+				     GetLastError());
+				ASSERT(0);
+				ShbError = kShbBufferInvalid;
+				break;
+			}
+		}
+	} else {
+		ShbError = kShbBufferInvalid;
+	}
+
+	return (ShbError);
+
+}
+
+//---------------------------------------------------------------------------
+//  Leave atomic section for Shared Buffer access
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbIpcLeaveAtomicSection(tShbInstance pShbInstance_p)
+{
+
+	tShbMemInst *pShbMemInst;
+	HANDLE hMutexBuffAccess;
+	BOOL fRes;
+	tShbError ShbError;
+
+	if (pShbInstance_p == NULL) {
+		return (kShbInvalidArg);
+	}
+
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance_p);
+	ShbError = kShbOk;
+
+	hMutexBuffAccess = pShbMemInst->m_hMutexBuffAccess;
+	if (hMutexBuffAccess != INVALID_HANDLE_VALUE) {
+		fRes = ReleaseMutex(hMutexBuffAccess);
+		ASSERT(fRes);
+	} else {
+		ShbError = kShbBufferInvalid;
+	}
+
+	return (ShbError);
+
+}
+
+//---------------------------------------------------------------------------
+//  Start signaling of new data (called from reading process)
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbIpcStartSignalingNewData(tShbInstance
+						      pShbInstance_p,
+						      tSigHndlrNewData
+						      pfnSignalHandlerNewData_p,
+						      tShbPriority
+						      ShbPriority_p)
+{
+
+	tShbMemInst *pShbMemInst;
+	tShbMemHeader *pShbMemHeader;
+	const char *pszObjectName;
+	HANDLE hEventTermRequ;
+	HANDLE hEventTermResp;
+	HANDLE hThreadNewData;
+	unsigned long ulThreadIDNewData;
+	tShbError ShbError;
+	int iPriority;
+
+	if ((pShbInstance_p == NULL) || (pfnSignalHandlerNewData_p == NULL)) {
+		return (kShbInvalidArg);
+	}
+
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance_p);
+	pShbMemHeader = ShbIpcGetShbMemHeader(pShbInstance_p);
+	ShbError = kShbOk;
+
+	if ((pShbMemInst->m_hThreadNewData != INVALID_HANDLE_VALUE) ||
+	    (pShbMemInst->m_ahEventNewData[IDX_EVENT_TERM_REQU] !=
+	     INVALID_HANDLE_VALUE)
+	    || (pShbMemInst->m_ahEventNewData[IDX_EVENT_TERM_RESP] !=
+		INVALID_HANDLE_VALUE)
+	    || (pShbMemInst->m_pfnSigHndlrNewData != NULL)) {
+		ShbError = kShbAlreadySignaling;
+		goto Exit;
+	}
+
+	pShbMemInst->m_pfnSigHndlrNewData = pfnSignalHandlerNewData_p;
+
+	// Because the event <pShbMemInst->m_ahEventNewData[IDX_EVENT_NEW_DATA]>
+	// is used for signaling of new data after a write operation too (using
+	// SetEvent), it is always created here (see <ShbIpcAllocBuffer>).
+
+	pszObjectName =
+	    ShbIpcGetUniformObjectName(NAME_EVENT_TERM_REQU,
+				       pShbMemHeader->m_szBufferID, FALSE);
+	hEventTermRequ = CreateEvent(NULL,	// LPSECURITY_ATTRIBUTES lpEventAttributes
+				     FALSE,	// BOOL bManualReset
+				     FALSE,	// BOOL bInitialState
+				     pszObjectName);	// LPCTSTR lpName
+	pShbMemInst->m_ahEventNewData[IDX_EVENT_TERM_REQU] = hEventTermRequ;
+	ASSERT(pShbMemInst->m_ahEventNewData[IDX_EVENT_TERM_REQU] != NULL);
+
+	pszObjectName =
+	    ShbIpcGetUniformObjectName(NAME_EVENT_TERM_RESP,
+				       pShbMemHeader->m_szBufferID, FALSE);
+	hEventTermResp = CreateEvent(NULL,	// LPSECURITY_ATTRIBUTES lpEventAttributes
+				     FALSE,	// BOOL bManualReset
+				     FALSE,	// BOOL bInitialState
+				     pszObjectName);	// LPCTSTR lpName
+	pShbMemInst->m_ahEventNewData[IDX_EVENT_TERM_RESP] = hEventTermResp;
+	ASSERT(pShbMemInst->m_ahEventNewData[IDX_EVENT_TERM_RESP] != NULL);
+
+	hThreadNewData = CreateThread(NULL,	// LPSECURITY_ATTRIBUTES lpThreadAttributes
+				      0,	// SIZE_T dwStackSize
+				      ShbIpcThreadSignalNewData,	// LPTHREAD_START_ROUTINE lpStartAddress
+				      pShbInstance_p,	// LPVOID lpParameter
+				      0,	// DWORD dwCreationFlags
+				      &ulThreadIDNewData);	// LPDWORD lpThreadId
+
+	switch (ShbPriority_p) {
+	case kShbPriorityLow:
+		iPriority = THREAD_PRIORITY_BELOW_NORMAL;
+		break;
+
+	case kShbPriorityNormal:
+		iPriority = THREAD_PRIORITY_NORMAL;
+		break;
+
+	case kshbPriorityHigh:
+		iPriority = THREAD_PRIORITY_ABOVE_NORMAL;
+		break;
+
+	}
+
+	ASSERT(pShbMemInst->m_hThreadNewData != NULL);
+
+	SetThreadPriority(hThreadNewData, iPriority);
+
+	pShbMemInst->m_hThreadNewData = hThreadNewData;
+
+#ifndef NDEBUG
+	{
+		pShbMemInst->m_ulThreadIDNewData = ulThreadIDNewData;
+	}
+#endif
+
+      Exit:
+
+	return (ShbError);
+
+}
+
+//---------------------------------------------------------------------------
+//  Stop signaling of new data (called from reading process)
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbIpcStopSignalingNewData(tShbInstance
+						     pShbInstance_p)
+{
+
+	tShbMemInst *pShbMemInst;
+	HANDLE hEventTermRequ;
+	HANDLE hEventTermResp;
+	DWORD dwWaitResult;
+
+	if (pShbInstance_p == NULL) {
+		return (kShbInvalidArg);
+	}
+
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance_p);
+
+	// terminate new data signaling thread
+	// (set event <hEventTermRequ> to wakeup the thread and dispose it
+	// to exit, then wait for confirmation using event <hEventTermResp>)
+	hEventTermRequ = pShbMemInst->m_ahEventNewData[IDX_EVENT_TERM_REQU];
+	hEventTermResp = pShbMemInst->m_ahEventNewData[IDX_EVENT_TERM_RESP];
+	if ((hEventTermRequ != INVALID_HANDLE_VALUE) &&
+	    (hEventTermResp != INVALID_HANDLE_VALUE)) {
+		TRACE0("\nShbIpcStopSignalingNewData(): enter wait state");
+		dwWaitResult = SignalObjectAndWait(hEventTermRequ,	// HANDLE hObjectToSignal
+						   hEventTermResp,	// HANDLE hObjectToWaitOn
+						   TIMEOUT_TERM_THREAD,	// DWORD dwMilliseconds
+						   FALSE);	// BOOL bAlertable
+		TRACE0
+		    ("\nShbIpcStopSignalingNewData(): wait state leaved: ---> ");
+		switch (dwWaitResult) {
+		case WAIT_OBJECT_0 + 0:	// event "new data signaling thread terminated"
+			{
+				TRACE0("Event = WAIT_OBJECT_0+0");
+				break;
+			}
+
+		default:
+			{
+				TRACE0("Unhandled Event");
+				ASSERT(0);
+				break;
+			}
+		}
+	}
+
+	if (pShbMemInst->m_hThreadNewData != INVALID_HANDLE_VALUE) {
+		CloseHandle(pShbMemInst->m_hThreadNewData);
+		pShbMemInst->m_hThreadNewData = INVALID_HANDLE_VALUE;
+	}
+
+	if (pShbMemInst->m_ahEventNewData[IDX_EVENT_TERM_REQU] !=
+	    INVALID_HANDLE_VALUE) {
+		CloseHandle(pShbMemInst->m_ahEventNewData[IDX_EVENT_TERM_REQU]);
+		pShbMemInst->m_ahEventNewData[IDX_EVENT_TERM_REQU] =
+		    INVALID_HANDLE_VALUE;
+	}
+
+	if (pShbMemInst->m_ahEventNewData[IDX_EVENT_TERM_RESP] !=
+	    INVALID_HANDLE_VALUE) {
+		CloseHandle(pShbMemInst->m_ahEventNewData[IDX_EVENT_TERM_RESP]);
+		pShbMemInst->m_ahEventNewData[IDX_EVENT_TERM_RESP] =
+		    INVALID_HANDLE_VALUE;
+	}
+
+	pShbMemInst->m_pfnSigHndlrNewData = NULL;
+
+	return (kShbOk);
+
+}
+
+//---------------------------------------------------------------------------
+//  Signal new data (called from writing process)
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbIpcSignalNewData(tShbInstance pShbInstance_p)
+{
+
+	tShbMemInst *pShbMemInst;
+	HANDLE hEventNewData;
+	BOOL fRes;
+
+	// TRACE0("\nShbIpcSignalNewData(): enter\n");
+
+	if (pShbInstance_p == NULL) {
+		return (kShbInvalidArg);
+	}
+
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance_p);
+
+	ASSERT(pShbMemInst->m_ahEventNewData[IDX_EVENT_NEW_DATA] !=
+	       INVALID_HANDLE_VALUE);
+	hEventNewData = pShbMemInst->m_ahEventNewData[IDX_EVENT_NEW_DATA];
+	if (hEventNewData != INVALID_HANDLE_VALUE) {
+		fRes = SetEvent(hEventNewData);
+		// TRACE1("\nShbIpcSignalNewData(): EventNewData set (Result=%d)\n", (int)fRes);
+		ASSERT(fRes);
+	}
+	// TRACE0("\nShbIpcSignalNewData(): leave\n");
+	return (kShbOk);
+
+}
+
+//---------------------------------------------------------------------------
+//  Start signaling for job ready (called from waiting process)
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbIpcStartSignalingJobReady(tShbInstance
+						       pShbInstance_p,
+						       unsigned long
+						       ulTimeOut_p,
+						       tSigHndlrJobReady
+						       pfnSignalHandlerJobReady_p)
+{
+
+	tShbMemInst *pShbMemInst;
+	tShbMemHeader *pShbMemHeader;
+	HANDLE hThreadJobReady;
+	unsigned long ulThreadIDJobReady;
+	tShbError ShbError;
+
+	if ((pShbInstance_p == NULL) || (pfnSignalHandlerJobReady_p == NULL)) {
+		return (kShbInvalidArg);
+	}
+
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance_p);
+	pShbMemHeader = ShbIpcGetShbMemHeader(pShbInstance_p);
+	ShbError = kShbOk;
+
+	if ((pShbMemInst->m_hThreadJobReady != INVALID_HANDLE_VALUE) ||
+	    (pShbMemInst->m_pfnSigHndlrJobReady != NULL)) {
+		ShbError = kShbAlreadySignaling;
+		goto Exit;
+	}
+
+	pShbMemInst->m_ulTimeOutJobReady = ulTimeOut_p;
+	pShbMemInst->m_pfnSigHndlrJobReady = pfnSignalHandlerJobReady_p;
+
+	// Because the event <pShbMemInst->m_ahEventJobReady> is used for
+	// signaling of a finished job too (using SetEvent), it is always
+	// created here (see <ShbIpcAllocBuffer>).
+
+	hThreadJobReady = CreateThread(NULL,	// LPSECURITY_ATTRIBUTES lpThreadAttributes
+				       0,	// SIZE_T dwStackSize
+				       ShbIpcThreadSignalJobReady,	// LPTHREAD_START_ROUTINE lpStartAddress
+				       pShbInstance_p,	// LPVOID lpParameter
+				       0,	// DWORD dwCreationFlags
+				       &ulThreadIDJobReady);	// LPDWORD lpThreadId
+
+	pShbMemInst->m_hThreadJobReady = hThreadJobReady;
+	ASSERT(pShbMemInst->m_hThreadJobReady != NULL);
+
+#ifndef NDEBUG
+	{
+		pShbMemInst->m_ulThreadIDJobReady = ulThreadIDJobReady;
+	}
+#endif
+
+      Exit:
+
+	return (ShbError);
+
+}
+
+//---------------------------------------------------------------------------
+//  Signal job ready (called from executing process)
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION tShbError ShbIpcSignalJobReady(tShbInstance pShbInstance_p)
+{
+
+	tShbMemInst *pShbMemInst;
+	HANDLE hEventJobReady;
+	BOOL fRes;
+
+	// TRACE0("\nShbIpcSignalJobReady(): enter\n");
+
+	if (pShbInstance_p == NULL) {
+		return (kShbInvalidArg);
+	}
+
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance_p);
+
+	ASSERT(pShbMemInst->m_hEventJobReady != INVALID_HANDLE_VALUE);
+	hEventJobReady = pShbMemInst->m_hEventJobReady;
+	if (hEventJobReady != INVALID_HANDLE_VALUE) {
+		fRes = SetEvent(hEventJobReady);
+		// TRACE1("\nShbIpcSignalJobReady(): EventJobReady set (Result=%d)\n", (int)fRes);
+		ASSERT(fRes);
+	}
+	// TRACE0("\nShbIpcSignalJobReady(): leave\n");
+	return (kShbOk);
+
+}
+
+//---------------------------------------------------------------------------
+//  Get pointer to common used share memory area
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION void *ShbIpcGetShMemPtr(tShbInstance pShbInstance_p)
+{
+
+	tShbMemHeader *pShbMemHeader;
+	void *pShbShMemPtr;
+
+	pShbMemHeader = ShbIpcGetShbMemHeader(pShbInstance_p);
+	if (pShbMemHeader != NULL) {
+		pShbShMemPtr = (BYTE *) pShbMemHeader + sizeof(tShbMemHeader);
+	} else {
+		pShbShMemPtr = NULL;
+	}
+
+	return (pShbShMemPtr);
+
+}
+
+#endif
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+#if !defined(SHBIPC_INLINE_ENABLED)
+
+//---------------------------------------------------------------------------
+//  Allocate a memory block from process specific mempool
+//---------------------------------------------------------------------------
+
+static void *ShbIpcAllocPrivateMem(unsigned long ulMemSize_p)
+{
+
+	HGLOBAL hMem;
+	void *pMem;
+
+	hMem = GlobalAlloc(GMEM_FIXED, ulMemSize_p + sizeof(HGLOBAL));
+	pMem = GlobalLock(hMem);
+	if (pMem != NULL) {
+		*(HGLOBAL *) pMem = hMem;
+		(BYTE *) pMem += sizeof(HGLOBAL);
+	}
+
+#ifndef NDEBUG
+	{
+		memset(pMem, 0xaa, ulMemSize_p);
+	}
+#endif
+
+	return (pMem);
+
+}
+
+//---------------------------------------------------------------------------
+//  Release a memory block from process specific mempool
+//---------------------------------------------------------------------------
+
+static void ShbIpcReleasePrivateMem(void *pMem_p)
+{
+
+	HGLOBAL hMem;
+
+	if (pMem_p == NULL) {
+		return;
+	}
+
+	(BYTE *) pMem_p -= sizeof(HGLOBAL);
+	hMem = *(HGLOBAL *) pMem_p;
+
+	GlobalUnlock(hMem);
+	GlobalFree(hMem);
+
+	return;
+
+}
+
+//---------------------------------------------------------------------------
+//  Create uniform object name (needed for inter-process communication)
+//---------------------------------------------------------------------------
+
+const char *ShbIpcGetUniformObjectName(const char *pszObjectJobName_p,
+				       const char *pszBufferID_p,
+				       BOOL fGlobalObject_p)
+{
+
+	static char szObjectName[MAX_PATH];
+	char szObjectPrefix[MAX_PATH];
+
+	if (fGlobalObject_p) {
+		strncpy(szObjectPrefix, "Global\\", sizeof(szObjectPrefix));
+	} else {
+		_snprintf(szObjectPrefix, sizeof(szObjectPrefix), "PID%08lX_",
+			  (unsigned long)GetCurrentProcessId());
+	}
+
+	_snprintf(szObjectName, sizeof(szObjectName), "%s%s#%s",
+		  szObjectPrefix, pszBufferID_p, pszObjectJobName_p);
+
+	return (szObjectName);
+
+}
+
+//---------------------------------------------------------------------------
+//  Thread for new data signaling
+//---------------------------------------------------------------------------
+
+DWORD WINAPI ShbIpcThreadSignalNewData(LPVOID pvThreadParam_p)
+{
+
+	tShbInstance pShbInstance;
+	tShbMemInst *pShbMemInst;
+	DWORD dwWaitResult;
+	BOOL fTermRequ;
+	int fCallAgain;
+
+	TRACE1
+	    ("\nShbIpcThreadSignalNewData(): SignalThread started (pShbInstance=0x%08lX)\n",
+	     (DWORD) pvThreadParam_p);
+
+	pShbInstance = (tShbMemInst *) pvThreadParam_p;
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance);
+	fTermRequ = FALSE;
+
+	do {
+		ASSERT((pShbMemInst->m_ahEventNewData[0] !=
+			INVALID_HANDLE_VALUE)
+		       && (pShbMemInst->m_ahEventNewData[0] != NULL));
+		ASSERT((pShbMemInst->m_ahEventNewData[1] !=
+			INVALID_HANDLE_VALUE)
+		       && (pShbMemInst->m_ahEventNewData[1] != NULL));
+
+		TRACE0("\nShbIpcThreadSignalNewData(): enter wait state");
+		dwWaitResult = WaitForMultipleObjects(2,	// DWORD nCount
+						      pShbMemInst->m_ahEventNewData,	// const HANDLE* lpHandles
+						      FALSE,	// BOOL bWaitAll
+						      INFINITE);	// DWORD dwMilliseconds
+		TRACE0
+		    ("\nShbIpcThreadSignalNewData(): wait state leaved: ---> ");
+		switch (dwWaitResult) {
+		case WAIT_OBJECT_0 + 0:	// event "new data"
+			{
+				TRACE0("Event = WAIT_OBJECT_0+0");
+				if (pShbMemInst->m_pfnSigHndlrNewData != NULL) {
+					TRACE0
+					    ("\nShbIpcThreadSignalNewData(): calling SignalHandlerNewData");
+					do {
+						fCallAgain =
+						    pShbMemInst->
+						    m_pfnSigHndlrNewData
+						    (pShbInstance);
+						// d.k.: try to run any shared buffer which has higher priority.
+						//           under Windows this is not really necessary because the Windows scheduler
+						//           already preempts tasks with lower priority.
+					} while (fCallAgain != FALSE);
+				}
+				break;
+			}
+
+		case WAIT_OBJECT_0 + 1:	// event "terminate"
+			{
+				TRACE0("Event = WAIT_OBJECT_0+1");
+				fTermRequ = TRUE;
+				break;
+			}
+
+		default:
+			{
+				TRACE0("Unhandled Event");
+				ASSERT(0);
+				fTermRequ = TRUE;
+				break;
+			}
+		}
+	}
+	while (!fTermRequ);
+
+	if (pShbMemInst->m_ahEventNewData[IDX_EVENT_TERM_RESP] !=
+	    INVALID_HANDLE_VALUE) {
+		SetEvent(pShbMemInst->m_ahEventNewData[IDX_EVENT_TERM_RESP]);
+	}
+
+	TRACE1
+	    ("\nShbIpcThreadSignalNewData(): SignalThread terminated (pShbInstance=0x%08lX)\n",
+	     (DWORD) pShbInstance);
+
+	ExitThread(0);
+
+}
+
+//---------------------------------------------------------------------------
+//  Thread for new data signaling
+//---------------------------------------------------------------------------
+
+DWORD WINAPI ShbIpcThreadSignalJobReady(LPVOID pvThreadParam_p)
+{
+
+	tShbInstance *pShbInstance;
+	tShbMemInst *pShbMemInst;
+	DWORD ulTimeOut;
+	DWORD dwWaitResult;
+	unsigned int fTimeOut;
+
+	TRACE1
+	    ("\nShbIpcThreadSignalJobReady(): SignalThread started (pShbInstance=0x%08lX)\n",
+	     (DWORD) pvThreadParam_p);
+
+	pShbInstance = (tShbInstance *) pvThreadParam_p;
+	pShbMemInst = ShbIpcGetShbMemInst(pShbInstance);
+	fTimeOut = FALSE;
+
+	if (pShbMemInst->m_ulTimeOutJobReady != 0) {
+		ulTimeOut = pShbMemInst->m_ulTimeOutJobReady;
+	} else {
+		ulTimeOut = INFINITE;
+	}
+
+	ASSERT((pShbMemInst->m_hEventJobReady != INVALID_HANDLE_VALUE)
+	       && (pShbMemInst->m_hEventJobReady != NULL));
+
+	TRACE0("\nShbIpcThreadSignalJobReady(): enter wait state");
+	dwWaitResult = WaitForSingleObject(pShbMemInst->m_hEventJobReady,	// HANDLE hHandle
+					   ulTimeOut);	// DWORD dwMilliseconds
+	TRACE0("\nShbIpcThreadSignalJobReady(): wait state leaved: ---> ");
+	switch (dwWaitResult) {
+	case WAIT_OBJECT_0 + 0:	// event "new data"
+		{
+			TRACE0("Event = WAIT_OBJECT_0+0");
+			fTimeOut = FALSE;
+			break;
+		}
+
+	case WAIT_TIMEOUT:
+		{
+			TRACE0("\nEvent = WAIT_TIMEOUT");
+			fTimeOut = TRUE;
+			// ASSERT(0);
+			break;
+		}
+
+	default:
+		{
+			TRACE0("Unhandled Event");
+			fTimeOut = TRUE;
+			ASSERT(0);
+			break;
+		}
+	}
+
+	if (pShbMemInst->m_pfnSigHndlrJobReady != NULL) {
+		TRACE0
+		    ("\nShbIpcThreadSignalJobReady(): calling SignalHandlerJobReady");
+		pShbMemInst->m_pfnSigHndlrJobReady(pShbInstance, fTimeOut);
+	}
+
+	pShbMemInst->m_hThreadJobReady = INVALID_HANDLE_VALUE;
+	pShbMemInst->m_pfnSigHndlrJobReady = NULL;
+
+	TRACE1
+	    ("\nShbIpcThreadSignalJobReady(): SignalThread terminated (pShbInstance=0x%08lX)\n",
+	     (DWORD) pShbInstance);
+
+	ExitThread(0);
+
+}
+
+#endif
+
+// EOF
diff --git a/drivers/staging/epl/ShbIpc.h b/drivers/staging/epl/ShbIpc.h
new file mode 100644
index 00000000000..cad8846a082
--- /dev/null
+++ b/drivers/staging/epl/ShbIpc.h
@@ -0,0 +1,125 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      Project independend shared buffer (linear + circular)
+
+  Description:  Declaration of platform specific part for the
+                shared buffer
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+  2006/06/27 -rs:   V 1.00 (initial version)
+
+****************************************************************************/
+
+#ifndef _SHBIPC_H_
+#define _SHBIPC_H_
+
+//---------------------------------------------------------------------------
+//  Type definitions
+//---------------------------------------------------------------------------
+
+typedef int (*tSigHndlrNewData) (tShbInstance pShbInstance_p);
+typedef void (*tSigHndlrJobReady) (tShbInstance pShbInstance_p,
+				   unsigned int fTimeOut_p);
+
+#if (TARGET_SYSTEM == _WIN32_)
+#if defined(INLINE_FUNCTION_DEF)
+#undef  INLINE_FUNCTION
+#define INLINE_FUNCTION     INLINE_FUNCTION_DEF
+#define SHBIPC_INLINE_ENABLED      TRUE
+#define SHBIPC_INLINED
+#include "ShbIpc-Win32.c"
+#endif
+
+#elif (TARGET_SYSTEM == _LINUX_)
+#if defined(INLINE_FUNCTION_DEF)
+#undef  INLINE_FUNCTION
+#define INLINE_FUNCTION     INLINE_FUNCTION_DEF
+#define SHBIPC_INLINE_ENABLED      TRUE
+#define SHBIPC_INLINED
+#include "ShbIpc-LinuxKernel.c"
+#endif
+#endif
+
+//---------------------------------------------------------------------------
+//  Prototypes
+//---------------------------------------------------------------------------
+
+tShbError ShbIpcInit(void);
+tShbError ShbIpcExit(void);
+
+tShbError ShbIpcAllocBuffer(unsigned long ulBufferSize_p,
+			    const char *pszBufferID_p,
+			    tShbInstance * ppShbInstance_p,
+			    unsigned int *pfShbNewCreated_p);
+tShbError ShbIpcReleaseBuffer(tShbInstance pShbInstance_p);
+
+#if !defined(SHBIPC_INLINE_ENABLED)
+
+tShbError ShbIpcEnterAtomicSection(tShbInstance pShbInstance_p);
+tShbError ShbIpcLeaveAtomicSection(tShbInstance pShbInstance_p);
+
+tShbError ShbIpcStartSignalingNewData(tShbInstance pShbInstance_p,
+				      tSigHndlrNewData
+				      pfnSignalHandlerNewData_p,
+				      tShbPriority ShbPriority_p);
+tShbError ShbIpcStopSignalingNewData(tShbInstance pShbInstance_p);
+tShbError ShbIpcSignalNewData(tShbInstance pShbInstance_p);
+
+tShbError ShbIpcStartSignalingJobReady(tShbInstance pShbInstance_p,
+				       unsigned long ulTimeOut_p,
+				       tSigHndlrJobReady
+				       pfnSignalHandlerJobReady_p);
+tShbError ShbIpcSignalJobReady(tShbInstance pShbInstance_p);
+
+void *ShbIpcGetShMemPtr(tShbInstance pShbInstance_p);
+#endif
+
+#undef  SHBIPC_INLINE_ENABLED	// disable actual inlining of functions
+#undef  INLINE_FUNCTION
+#define INLINE_FUNCTION		// define INLINE_FUNCTION to nothing
+
+#endif // #ifndef _SHBIPC_H_
diff --git a/drivers/staging/epl/ShbLinuxKernel.h b/drivers/staging/epl/ShbLinuxKernel.h
new file mode 100644
index 00000000000..812702add4f
--- /dev/null
+++ b/drivers/staging/epl/ShbLinuxKernel.h
@@ -0,0 +1,68 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      Project independend shared buffer (linear + circular)
+
+  Description:  Declaration of platform specific part for the
+                shared buffer
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+  2006/07/20 -rs:   V 1.00 (initial version)
+
+****************************************************************************/
+
+#ifndef _SHBLINUXKERNEL_H_
+#define _SHBLINUXKERNEL_H_
+
+struct sShbMemTable {
+	int m_iBufferId;
+	void *m_pBuffer;
+	struct sShbMemTable *m_psNextMemTableElement;
+};
+
+extern struct sShbMemTable *psMemTableElementFirst_g;
+
+#endif // _SHBLINUXKERNEL_H_
diff --git a/drivers/staging/epl/SocketLinuxKernel.c b/drivers/staging/epl/SocketLinuxKernel.c
new file mode 100644
index 00000000000..562bc4a3e56
--- /dev/null
+++ b/drivers/staging/epl/SocketLinuxKernel.c
@@ -0,0 +1,197 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  Wrapper for BSD socket API for Linux kernel
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: SocketLinuxKernel.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.3 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                Dev C++ and GNU-Compiler for m68k
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/08/25 d.k.:   start of implementation
+
+****************************************************************************/
+
+#include <linux/net.h>
+#include <linux/in.h>
+#include "SocketLinuxKernel.h"
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+//  Kernel Module specific Data Structures
+//---------------------------------------------------------------------------
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:
+//
+// Description:
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+SOCKET socket(int af, int type, int protocol)
+{
+	int rc;
+	SOCKET socket;
+
+	rc = sock_create_kern(af, type, protocol, &socket);
+	if (rc < 0) {
+		socket = NULL;
+		goto Exit;
+	}
+
+      Exit:
+	return socket;
+}
+
+int bind(SOCKET socket_p, const struct sockaddr *addr, int addrlen)
+{
+	int rc;
+
+	rc = socket_p->ops->bind(socket_p, (struct sockaddr *)addr, addrlen);
+
+	return rc;
+}
+
+int closesocket(SOCKET socket_p)
+{
+	sock_release(socket_p);
+
+	return 0;
+}
+
+int recvfrom(SOCKET socket_p, char *buf, int len, int flags,
+	     struct sockaddr *from, int *fromlen)
+{
+	int rc;
+	struct msghdr msg;
+	struct kvec iov;
+
+	msg.msg_control = NULL;
+	msg.msg_controllen = 0;
+	msg.msg_name = from;	// will be struct sock_addr
+	msg.msg_namelen = *fromlen;
+	iov.iov_len = len;
+	iov.iov_base = buf;
+
+	rc = kernel_recvmsg(socket_p, &msg, &iov, 1, iov.iov_len, 0);
+
+	return rc;
+}
+
+int sendto(SOCKET socket_p, const char *buf, int len, int flags,
+	   const struct sockaddr *to, int tolen)
+{
+	int rc;
+	struct msghdr msg;
+	struct kvec iov;
+
+	msg.msg_control = NULL;
+	msg.msg_controllen = 0;
+	msg.msg_name = (struct sockaddr *)to;	// will be struct sock_addr
+	msg.msg_namelen = tolen;
+	msg.msg_flags = 0;
+	iov.iov_len = len;
+	iov.iov_base = (char *)buf;
+
+	rc = kernel_sendmsg(socket_p, &msg, &iov, 1, len);
+
+	return rc;
+}
+
+// EOF
diff --git a/drivers/staging/epl/SocketLinuxKernel.h b/drivers/staging/epl/SocketLinuxKernel.h
new file mode 100644
index 00000000000..6e1d6198960
--- /dev/null
+++ b/drivers/staging/epl/SocketLinuxKernel.h
@@ -0,0 +1,105 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for BSD socket API for Linux kernel
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: SocketLinuxKernel.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.3 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                KEIL uVision 2
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/08/25 d.k.:   start of the implementation
+
+****************************************************************************/
+
+#ifndef _SOCKETLINUXKERNEL_H_
+#define _SOCKETLINUXKERNEL_H_
+
+#include <linux/net.h>
+#include <linux/in.h>
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#define INVALID_SOCKET  0
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+typedef struct socket *SOCKET;
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+int bind(SOCKET s, const struct sockaddr *addr, int addrlen);
+
+int closesocket(SOCKET s);
+
+int recvfrom(SOCKET s, char *buf, int len, int flags, struct sockaddr *from,
+	     int *fromlen);
+
+int sendto(SOCKET s, const char *buf, int len, int flags,
+	   const struct sockaddr *to, int tolen);
+
+SOCKET socket(int af, int type, int protocol);
+
+#endif // #ifndef _SOCKETLINUXKERNEL_H_
diff --git a/drivers/staging/epl/TimerHighReskX86.c b/drivers/staging/epl/TimerHighReskX86.c
new file mode 100644
index 00000000000..82eee4702aa
--- /dev/null
+++ b/drivers/staging/epl/TimerHighReskX86.c
@@ -0,0 +1,522 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  target specific implementation of
+                high resolution timer module for X86 under Linux
+                The Linux kernel has to be compiled with high resolution
+                timers enabled. This is done by configuring the kernel
+                with CONFIG_HIGH_RES_TIMERS enabled.
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: TimerHighReskX86.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/04/17 21:38:01 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GNU
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+****************************************************************************/
+
+#include "EplInc.h"
+#include "kernel/EplTimerHighResk.h"
+#include "Benchmark.h"
+
+//#include <linux/config.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/hrtimer.h>
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#define TIMER_COUNT           2	/* max 15 timers selectable */
+#define TIMER_MIN_VAL_SINGLE  5000	/* min 5us */
+#define TIMER_MIN_VAL_CYCLE   100000	/* min 100us */
+
+#define PROVE_OVERRUN
+
+#ifndef CONFIG_HIGH_RES_TIMERS
+#error "Kernel symbol CONFIG_HIGH_RES_TIMERS is required."
+#endif
+
+// TracePoint support for realtime-debugging
+#ifdef _DBG_TRACE_POINTS_
+void PUBLIC TgtDbgSignalTracePoint(BYTE bTracePointNumber_p);
+void PUBLIC TgtDbgPostTraceValue(DWORD dwTraceValue_p);
+#define TGT_DBG_SIGNAL_TRACE_POINT(p)   TgtDbgSignalTracePoint(p)
+#define TGT_DBG_POST_TRACE_VALUE(v)     TgtDbgPostTraceValue(v)
+#else
+#define TGT_DBG_SIGNAL_TRACE_POINT(p)
+#define TGT_DBG_POST_TRACE_VALUE(v)
+#endif
+#define HRT_DBG_POST_TRACE_VALUE(Event_p, uiNodeId_p, wErrorCode_p) \
+    TGT_DBG_POST_TRACE_VALUE((0xE << 28) | (Event_p << 24) \
+                             | (uiNodeId_p << 16) | wErrorCode_p)
+
+#define TIMERHDL_MASK         0x0FFFFFFF
+#define TIMERHDL_SHIFT        28
+#define HDL_TO_IDX(Hdl)       ((Hdl >> TIMERHDL_SHIFT) - 1)
+#define HDL_INIT(Idx)         ((Idx + 1) << TIMERHDL_SHIFT)
+#define HDL_INC(Hdl)          (((Hdl + 1) & TIMERHDL_MASK) \
+                               | (Hdl & ~TIMERHDL_MASK))
+
+//---------------------------------------------------------------------------
+// modul global types
+//---------------------------------------------------------------------------
+
+typedef struct {
+	tEplTimerEventArg m_EventArg;
+	tEplTimerkCallback m_pfnCallback;
+	struct hrtimer m_Timer;
+	BOOL m_fContinuously;
+	unsigned long long m_ullPeriod;
+
+} tEplTimerHighReskTimerInfo;
+
+typedef struct {
+	tEplTimerHighReskTimerInfo m_aTimerInfo[TIMER_COUNT];
+
+} tEplTimerHighReskInstance;
+
+//---------------------------------------------------------------------------
+// local vars
+//---------------------------------------------------------------------------
+
+static tEplTimerHighReskInstance EplTimerHighReskInstance_l;
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+enum hrtimer_restart EplTimerHighReskCallback(struct hrtimer *pTimer_p);
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimerHighReskInit()
+//
+// Description: initializes the high resolution timer module.
+//
+// Parameters:  void
+//
+// Return:      tEplKernel      = error code
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplTimerHighReskInit(void)
+{
+	tEplKernel Ret;
+
+	Ret = EplTimerHighReskAddInstance();
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimerHighReskAddInstance()
+//
+// Description: initializes the high resolution timer module.
+//
+// Parameters:  void
+//
+// Return:      tEplKernel      = error code
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplTimerHighReskAddInstance(void)
+{
+	tEplKernel Ret;
+	unsigned int uiIndex;
+
+	Ret = kEplSuccessful;
+
+	EPL_MEMSET(&EplTimerHighReskInstance_l, 0,
+		   sizeof(EplTimerHighReskInstance_l));
+
+#ifndef CONFIG_HIGH_RES_TIMERS
+	printk
+	    ("EplTimerHighResk: Kernel symbol CONFIG_HIGH_RES_TIMERS is required.\n");
+	Ret = kEplNoResource;
+	return Ret;
+#endif
+
+	/*
+	 * Initialize hrtimer structures for all usable timers.
+	 */
+	for (uiIndex = 0; uiIndex < TIMER_COUNT; uiIndex++) {
+		tEplTimerHighReskTimerInfo *pTimerInfo;
+		struct hrtimer *pTimer;
+
+		pTimerInfo = &EplTimerHighReskInstance_l.m_aTimerInfo[uiIndex];
+		pTimer = &pTimerInfo->m_Timer;
+		hrtimer_init(pTimer, CLOCK_MONOTONIC, HRTIMER_MODE_ABS);
+
+		pTimer->function = EplTimerHighReskCallback;
+	}
+
+	return Ret;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimerHighReskDelInstance()
+//
+// Description: shuts down the high resolution timer module.
+//
+// Parameters:  void
+//
+// Return:      tEplKernel      = error code
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplTimerHighReskDelInstance(void)
+{
+	tEplTimerHighReskTimerInfo *pTimerInfo;
+	tEplKernel Ret;
+	unsigned int uiIndex;
+
+	Ret = kEplSuccessful;
+
+	for (uiIndex = 0; uiIndex < TIMER_COUNT; uiIndex++) {
+		pTimerInfo = &EplTimerHighReskInstance_l.m_aTimerInfo[0];
+		pTimerInfo->m_pfnCallback = NULL;
+		pTimerInfo->m_EventArg.m_TimerHdl = 0;
+		/*
+		 * In this case we can not just try to cancel the timer.
+		 * We actually have to wait until its callback function
+		 * has returned.
+		 */
+		hrtimer_cancel(&pTimerInfo->m_Timer);
+	}
+
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimerHighReskModifyTimerNs()
+//
+// Description: modifies the timeout of the timer with the specified handle.
+//              If the handle the pointer points to is zero, the timer must
+//              be created first.
+//              If it is not possible to stop the old timer,
+//              this function always assures that the old timer does not
+//              trigger the callback function with the same handle as the new
+//              timer. That means the callback function must check the passed
+//              handle with the one returned by this function. If these are
+//              unequal, the call can be discarded.
+//
+// Parameters:  pTimerHdl_p     = pointer to timer handle
+//              ullTimeNs_p     = relative timeout in [ns]
+//              pfnCallback_p   = callback function, which is called mutual
+//                                exclusive with the Edrv callback functions
+//                                (Rx and Tx).
+//              ulArgument_p    = user-specific argument
+//              fContinuously_p = if TRUE, callback function will be called
+//                                continuously;
+//                                otherwise, it is a oneshot timer.
+//
+// Return:      tEplKernel      = error code
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplTimerHighReskModifyTimerNs(tEplTimerHdl * pTimerHdl_p,
+						unsigned long long ullTimeNs_p,
+						tEplTimerkCallback
+						pfnCallback_p,
+						unsigned long ulArgument_p,
+						BOOL fContinuously_p)
+{
+	tEplKernel Ret;
+	unsigned int uiIndex;
+	tEplTimerHighReskTimerInfo *pTimerInfo;
+	ktime_t RelTime;
+
+	Ret = kEplSuccessful;
+
+	// check pointer to handle
+	if (pTimerHdl_p == NULL) {
+		Ret = kEplTimerInvalidHandle;
+		goto Exit;
+	}
+
+	if (*pTimerHdl_p == 0) {	// no timer created yet
+
+		// search free timer info structure
+		pTimerInfo = &EplTimerHighReskInstance_l.m_aTimerInfo[0];
+		for (uiIndex = 0; uiIndex < TIMER_COUNT;
+		     uiIndex++, pTimerInfo++) {
+			if (pTimerInfo->m_EventArg.m_TimerHdl == 0) {	// free structure found
+				break;
+			}
+		}
+		if (uiIndex >= TIMER_COUNT) {	// no free structure found
+			Ret = kEplTimerNoTimerCreated;
+			goto Exit;
+		}
+
+		pTimerInfo->m_EventArg.m_TimerHdl = HDL_INIT(uiIndex);
+	} else {
+		uiIndex = HDL_TO_IDX(*pTimerHdl_p);
+		if (uiIndex >= TIMER_COUNT) {	// invalid handle
+			Ret = kEplTimerInvalidHandle;
+			goto Exit;
+		}
+
+		pTimerInfo = &EplTimerHighReskInstance_l.m_aTimerInfo[uiIndex];
+	}
+
+	/*
+	 * increment timer handle
+	 * (if timer expires right after this statement, the user
+	 * would detect an unknown timer handle and discard it)
+	 */
+	pTimerInfo->m_EventArg.m_TimerHdl =
+	    HDL_INC(pTimerInfo->m_EventArg.m_TimerHdl);
+	*pTimerHdl_p = pTimerInfo->m_EventArg.m_TimerHdl;
+
+	// reject too small time values
+	if ((fContinuously_p && (ullTimeNs_p < TIMER_MIN_VAL_CYCLE))
+	    || (!fContinuously_p && (ullTimeNs_p < TIMER_MIN_VAL_SINGLE))) {
+		Ret = kEplTimerNoTimerCreated;
+		goto Exit;
+	}
+
+	pTimerInfo->m_EventArg.m_ulArg = ulArgument_p;
+	pTimerInfo->m_pfnCallback = pfnCallback_p;
+	pTimerInfo->m_fContinuously = fContinuously_p;
+	pTimerInfo->m_ullPeriod = ullTimeNs_p;
+
+	/*
+	 * HRTIMER_MODE_REL does not influence general handling of this timer.
+	 * It only sets relative mode for this start operation.
+	 * -> Expire time is calculated by: Now + RelTime
+	 * hrtimer_start also skips pending timer events.
+	 * The state HRTIMER_STATE_CALLBACK is ignored.
+	 * We have to cope with that in our callback function.
+	 */
+	RelTime = ktime_add_ns(ktime_set(0, 0), ullTimeNs_p);
+	hrtimer_start(&pTimerInfo->m_Timer, RelTime, HRTIMER_MODE_REL);
+
+      Exit:
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimerHighReskDeleteTimer()
+//
+// Description: deletes the timer with the specified handle. Afterward the
+//              handle is set to zero.
+//
+// Parameters:  pTimerHdl_p     = pointer to timer handle
+//
+// Return:      tEplKernel      = error code
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplTimerHighReskDeleteTimer(tEplTimerHdl * pTimerHdl_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	unsigned int uiIndex;
+	tEplTimerHighReskTimerInfo *pTimerInfo;
+
+	// check pointer to handle
+	if (pTimerHdl_p == NULL) {
+		Ret = kEplTimerInvalidHandle;
+		goto Exit;
+	}
+
+	if (*pTimerHdl_p == 0) {	// no timer created yet
+		goto Exit;
+	} else {
+		uiIndex = HDL_TO_IDX(*pTimerHdl_p);
+		if (uiIndex >= TIMER_COUNT) {	// invalid handle
+			Ret = kEplTimerInvalidHandle;
+			goto Exit;
+		}
+		pTimerInfo = &EplTimerHighReskInstance_l.m_aTimerInfo[uiIndex];
+		if (pTimerInfo->m_EventArg.m_TimerHdl != *pTimerHdl_p) {	// invalid handle
+			goto Exit;
+		}
+	}
+
+	*pTimerHdl_p = 0;
+	pTimerInfo->m_EventArg.m_TimerHdl = 0;
+	pTimerInfo->m_pfnCallback = NULL;
+
+	/*
+	 * Three return cases of hrtimer_try_to_cancel have to be tracked:
+	 *  1 - timer has been removed
+	 *  0 - timer was not active
+	 *      We need not do anything. hrtimer timers just consist of
+	 *      a hrtimer struct, which we might enqueue in the hrtimers
+	 *      event list by calling hrtimer_start().
+	 *      If a timer is not enqueued, it is not present in hrtimers.
+	 * -1 - callback function is running
+	 *      In this case we have to ensure that the timer is not
+	 *      continuously restarted. This has been done by clearing
+	 *      its handle.
+	 */
+	hrtimer_try_to_cancel(&pTimerInfo->m_Timer);
+
+      Exit:
+	return Ret;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    EplTimerHighReskCallback()
+//
+// Description: Callback function commonly used for all timers.
+//
+// Parameters:  pTimer_p = pointer to hrtimer
+//
+// Return:
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+enum hrtimer_restart EplTimerHighReskCallback(struct hrtimer *pTimer_p)
+{
+	unsigned int uiIndex;
+	tEplTimerHighReskTimerInfo *pTimerInfo;
+	tEplTimerHdl OrgTimerHdl;
+	enum hrtimer_restart Ret;
+
+	BENCHMARK_MOD_24_SET(4);
+
+	Ret = HRTIMER_NORESTART;
+	pTimerInfo =
+	    container_of(pTimer_p, tEplTimerHighReskTimerInfo, m_Timer);
+	uiIndex = HDL_TO_IDX(pTimerInfo->m_EventArg.m_TimerHdl);
+	if (uiIndex >= TIMER_COUNT) {	// invalid handle
+		goto Exit;
+	}
+
+	/*
+	 * We store the timer handle before calling the callback function
+	 * as the timer can be modified inside it.
+	 */
+	OrgTimerHdl = pTimerInfo->m_EventArg.m_TimerHdl;
+
+	if (pTimerInfo->m_pfnCallback != NULL) {
+		pTimerInfo->m_pfnCallback(&pTimerInfo->m_EventArg);
+	}
+
+	if (pTimerInfo->m_fContinuously) {
+		ktime_t Interval;
+#ifdef PROVE_OVERRUN
+		ktime_t Now;
+		unsigned long Overruns;
+#endif
+
+		if (OrgTimerHdl != pTimerInfo->m_EventArg.m_TimerHdl) {
+			/* modified timer has already been restarted */
+			goto Exit;
+		}
+#ifdef PROVE_OVERRUN
+		Now = ktime_get();
+		Interval =
+		    ktime_add_ns(ktime_set(0, 0), pTimerInfo->m_ullPeriod);
+		Overruns = hrtimer_forward(pTimer_p, Now, Interval);
+		if (Overruns > 1) {
+			printk
+			    ("EplTimerHighResk: Continuous timer (handle 0x%lX) had to skip %lu interval(s)!\n",
+			     pTimerInfo->m_EventArg.m_TimerHdl, Overruns - 1);
+		}
+#else
+		pTimer_p->expires = ktime_add_ns(pTimer_p->expires,
+						 pTimerInfo->m_ullPeriod);
+#endif
+
+		Ret = HRTIMER_RESTART;
+	}
+
+      Exit:
+	BENCHMARK_MOD_24_RESET(4);
+	return Ret;
+}
diff --git a/drivers/staging/epl/VirtualEthernetLinux.c b/drivers/staging/epl/VirtualEthernetLinux.c
new file mode 100644
index 00000000000..5d838dbf73a
--- /dev/null
+++ b/drivers/staging/epl/VirtualEthernetLinux.c
@@ -0,0 +1,342 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  Virtual Ethernet Driver for Linux
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: VirtualEthernetLinux.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.8 $  $Date: 2008/11/20 17:06:51 $
+
+                $State: Exp $
+
+                Build Environment:
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/12 -ar:   start of the implementation, version 1.00
+
+  2006/09/18 d.k.:  integration into EPL DLLk module
+
+  ToDo:
+
+  void netif_carrier_off(struct net_device *dev);
+  void netif_carrier_on(struct net_device *dev);
+
+****************************************************************************/
+
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/if_arp.h>
+#include <net/arp.h>
+
+#include <net/protocol.h>
+#include <net/pkt_sched.h>
+#include <linux/if_ether.h>
+#include <linux/in.h>
+#include <linux/ip.h>
+#include <linux/udp.h>
+#include <linux/mm.h>
+#include <linux/types.h>
+#include <linux/skbuff.h>	/* for struct sk_buff */
+
+#include "kernel/VirtualEthernet.h"
+#include "kernel/EplDllkCal.h"
+#include "kernel/EplDllk.h"
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_VETH)) != 0)
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#ifndef EPL_VETH_TX_TIMEOUT
+//#define EPL_VETH_TX_TIMEOUT (2*HZ)
+#define EPL_VETH_TX_TIMEOUT 0	// d.k.: we use no timeout
+#endif
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+static struct net_device *pVEthNetDevice_g = NULL;
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+static int VEthOpen(struct net_device *pNetDevice_p);
+static int VEthClose(struct net_device *pNetDevice_p);
+static int VEthXmit(struct sk_buff *pSkb_p, struct net_device *pNetDevice_p);
+static struct net_device_stats *VEthGetStats(struct net_device *pNetDevice_p);
+static void VEthTimeout(struct net_device *pNetDevice_p);
+static tEplKernel VEthRecvFrame(tEplFrameInfo * pFrameInfo_p);
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:
+//
+// Description:
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+static int VEthOpen(struct net_device *pNetDevice_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	//open the device
+//	struct net_device_stats* pStats = netdev_priv(pNetDevice_p);
+
+	//start the interface queue for the network subsystem
+	netif_start_queue(pNetDevice_p);
+
+	// register callback function in DLL
+	Ret = EplDllkRegAsyncHandler(VEthRecvFrame);
+
+	EPL_DBGLVL_VETH_TRACE1
+	    ("VEthOpen: EplDllkRegAsyncHandler returned 0x%02X\n", Ret);
+
+	return 0;
+}
+
+static int VEthClose(struct net_device *pNetDevice_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	EPL_DBGLVL_VETH_TRACE0("VEthClose\n");
+
+	Ret = EplDllkDeregAsyncHandler(VEthRecvFrame);
+
+	//stop the interface queue for the network subsystem
+	netif_stop_queue(pNetDevice_p);
+	return 0;
+}
+
+static int VEthXmit(struct sk_buff *pSkb_p, struct net_device *pNetDevice_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	tEplFrameInfo FrameInfo;
+
+	//transmit function
+	struct net_device_stats *pStats = netdev_priv(pNetDevice_p);
+
+	//save timestemp
+	pNetDevice_p->trans_start = jiffies;
+
+	FrameInfo.m_pFrame = (tEplFrame *) pSkb_p->data;
+	FrameInfo.m_uiFrameSize = pSkb_p->len;
+
+	//call send fkt on DLL
+	Ret = EplDllkCalAsyncSend(&FrameInfo, kEplDllAsyncReqPrioGeneric);
+	if (Ret != kEplSuccessful) {
+		EPL_DBGLVL_VETH_TRACE1
+		    ("VEthXmit: EplDllkCalAsyncSend returned 0x%02X\n", Ret);
+		netif_stop_queue(pNetDevice_p);
+		goto Exit;
+	} else {
+		EPL_DBGLVL_VETH_TRACE0("VEthXmit: frame passed to DLL\n");
+		dev_kfree_skb(pSkb_p);
+
+		//set stats for the device
+		pStats->tx_packets++;
+		pStats->tx_bytes += FrameInfo.m_uiFrameSize;
+	}
+
+      Exit:
+	return 0;
+
+}
+
+static struct net_device_stats *VEthGetStats(struct net_device *pNetDevice_p)
+{
+	EPL_DBGLVL_VETH_TRACE0("VEthGetStats\n");
+
+	return netdev_priv(pNetDevice_p);
+}
+
+static void VEthTimeout(struct net_device *pNetDevice_p)
+{
+	EPL_DBGLVL_VETH_TRACE0("VEthTimeout(\n");
+
+	// $$$ d.k.: move to extra function, which is called by DLL when new space is available in TxFifo
+	if (netif_queue_stopped(pNetDevice_p)) {
+		netif_wake_queue(pNetDevice_p);
+	}
+}
+
+static tEplKernel VEthRecvFrame(tEplFrameInfo * pFrameInfo_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+	struct net_device *pNetDevice = pVEthNetDevice_g;
+	struct net_device_stats *pStats = netdev_priv(pNetDevice);
+	struct sk_buff *pSkb;
+
+	EPL_DBGLVL_VETH_TRACE1("VEthRecvFrame: FrameSize=%u\n",
+			       pFrameInfo_p->m_uiFrameSize);
+
+	pSkb = dev_alloc_skb(pFrameInfo_p->m_uiFrameSize + 2);
+	if (pSkb == NULL) {
+		pStats->rx_dropped++;
+		goto Exit;
+	}
+	pSkb->dev = pNetDevice;
+
+	skb_reserve(pSkb, 2);
+
+	memcpy((void *)skb_put(pSkb, pFrameInfo_p->m_uiFrameSize),
+	       pFrameInfo_p->m_pFrame, pFrameInfo_p->m_uiFrameSize);
+
+	pSkb->protocol = eth_type_trans(pSkb, pNetDevice);
+	pSkb->ip_summed = CHECKSUM_UNNECESSARY;
+
+	// call netif_rx with skb
+	netif_rx(pSkb);
+
+	EPL_DBGLVL_VETH_TRACE1("VEthRecvFrame: SrcMAC=0x%llx\n",
+			       AmiGetQword48FromBe(pFrameInfo_p->m_pFrame->
+						   m_be_abSrcMac));
+
+	// update receive statistics
+	pStats->rx_packets++;
+	pStats->rx_bytes += pFrameInfo_p->m_uiFrameSize;
+
+      Exit:
+	return Ret;
+}
+
+tEplKernel PUBLIC VEthAddInstance(tEplDllkInitParam * pInitParam_p)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	// allocate net device structure with priv pointing to stats structure
+	pVEthNetDevice_g =
+	    alloc_netdev(sizeof(struct net_device_stats), EPL_VETH_NAME,
+			 ether_setup);
+//    pVEthNetDevice_g = alloc_etherdev(sizeof (struct net_device_stats));
+
+	if (pVEthNetDevice_g == NULL) {
+		Ret = kEplNoResource;
+		goto Exit;
+	}
+
+	pVEthNetDevice_g->open = VEthOpen;
+	pVEthNetDevice_g->stop = VEthClose;
+	pVEthNetDevice_g->get_stats = VEthGetStats;
+	pVEthNetDevice_g->hard_start_xmit = VEthXmit;
+	pVEthNetDevice_g->tx_timeout = VEthTimeout;
+	pVEthNetDevice_g->watchdog_timeo = EPL_VETH_TX_TIMEOUT;
+	pVEthNetDevice_g->destructor = free_netdev;
+
+	// copy own MAC address to net device structure
+	memcpy(pVEthNetDevice_g->dev_addr, pInitParam_p->m_be_abSrcMac, 6);
+
+	//register VEth to the network subsystem
+	if (register_netdev(pVEthNetDevice_g)) {
+		EPL_DBGLVL_VETH_TRACE0
+		    ("VEthAddInstance: Could not register VEth...\n");
+	} else {
+		EPL_DBGLVL_VETH_TRACE0
+		    ("VEthAddInstance: Register VEth successfull...\n");
+	}
+
+      Exit:
+	return Ret;
+}
+
+tEplKernel PUBLIC VEthDelInstance(void)
+{
+	tEplKernel Ret = kEplSuccessful;
+
+	if (pVEthNetDevice_g != NULL) {
+		//unregister VEth from the network subsystem
+		unregister_netdev(pVEthNetDevice_g);
+		// destructor was set to free_netdev,
+		// so we do not need to call free_netdev here
+		pVEthNetDevice_g = NULL;
+	}
+
+	return Ret;
+}
+
+#endif // (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_VETH)) != 0)
diff --git a/drivers/staging/epl/amix86.c b/drivers/staging/epl/amix86.c
new file mode 100644
index 00000000000..9f742384dac
--- /dev/null
+++ b/drivers/staging/epl/amix86.c
@@ -0,0 +1,905 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  Abstract Memory Interface for x86 compatible
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: amix86.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.3 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    ...
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  r.s.: first implemetation
+
+  2006-06-13  d.k.: duplicate functions for little endian and big endian
+
+****************************************************************************/
+
+//#include "global.h"
+//#include "EplAmi.h"
+#include "EplInc.h"
+
+#if (!defined(EPL_AMI_INLINED)) || defined(INLINE_ENABLED)
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+typedef struct {
+	WORD m_wWord;
+
+} twStruct;
+
+typedef struct {
+	DWORD m_dwDword;
+
+} tdwStruct;
+
+typedef struct {
+	QWORD m_qwQword;
+
+} tqwStruct;
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetXXXToBe()
+//
+// Description: writes the specified value to the absolute address in
+//              big endian
+//
+// Parameters:  pAddr_p                 = absolute address
+//              xXXXVal_p               = value
+//
+// Returns:     (none)
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+//------------< write BYTE in big endian >--------------------------
+/*
+void  PUBLIC  AmiSetByteToBe (void FAR* pAddr_p, BYTE bByteVal_p)
+{
+
+   *(BYTE FAR*)pAddr_p = bByteVal_p;
+
+}
+*/
+
+//------------< write WORD in big endian >--------------------------
+
+INLINE_FUNCTION void PUBLIC AmiSetWordToBe(void FAR * pAddr_p, WORD wWordVal_p)
+{
+	twStruct FAR *pwStruct;
+	twStruct wValue;
+
+	wValue.m_wWord = (WORD) ((wWordVal_p & 0x00FF) << 8);	//LSB to MSB
+	wValue.m_wWord |= (WORD) ((wWordVal_p & 0xFF00) >> 8);	//MSB to LSB
+
+	pwStruct = (twStruct FAR *) pAddr_p;
+	pwStruct->m_wWord = wValue.m_wWord;
+
+}
+
+//------------< write DWORD in big endian >-------------------------
+
+INLINE_FUNCTION void PUBLIC AmiSetDwordToBe(void FAR * pAddr_p,
+					    DWORD dwDwordVal_p)
+{
+	tdwStruct FAR *pdwStruct;
+	tdwStruct dwValue;
+
+	dwValue.m_dwDword = ((dwDwordVal_p & 0x000000FF) << 24);	//LSB to MSB
+	dwValue.m_dwDword |= ((dwDwordVal_p & 0x0000FF00) << 8);
+	dwValue.m_dwDword |= ((dwDwordVal_p & 0x00FF0000) >> 8);
+	dwValue.m_dwDword |= ((dwDwordVal_p & 0xFF000000) >> 24);	//MSB to LSB
+
+	pdwStruct = (tdwStruct FAR *) pAddr_p;
+	pdwStruct->m_dwDword = dwValue.m_dwDword;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetXXXToLe()
+//
+// Description: writes the specified value to the absolute address in
+//              little endian
+//
+// Parameters:  pAddr_p                 = absolute address
+//              xXXXVal_p               = value
+//
+// Returns:     (none)
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+//------------< write BYTE in little endian >--------------------------
+/*
+void  PUBLIC  AmiSetByteToLe (void FAR* pAddr_p, BYTE bByteVal_p)
+{
+
+   *(BYTE FAR*)pAddr_p = bByteVal_p;
+
+}
+*/
+
+//------------< write WORD in little endian >--------------------------
+
+INLINE_FUNCTION void PUBLIC AmiSetWordToLe(void FAR * pAddr_p, WORD wWordVal_p)
+{
+	twStruct FAR *pwStruct;
+
+	pwStruct = (twStruct FAR *) pAddr_p;
+	pwStruct->m_wWord = wWordVal_p;
+
+}
+
+//------------< write DWORD in little endian >-------------------------
+
+INLINE_FUNCTION void PUBLIC AmiSetDwordToLe(void FAR * pAddr_p,
+					    DWORD dwDwordVal_p)
+{
+	tdwStruct FAR *pdwStruct;
+
+	pdwStruct = (tdwStruct FAR *) pAddr_p;
+	pdwStruct->m_dwDword = dwDwordVal_p;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetXXXFromBe()
+//
+// Description: reads the specified value from the absolute address in
+//              big endian
+//
+// Parameters:  pAddr_p                 = absolute address
+//
+// Returns:     XXX                     = value
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+//------------< read BYTE in big endian >---------------------------
+/*
+BYTE  PUBLIC  AmiGetByteFromBe (void FAR* pAddr_p)
+{
+
+   return ( *(BYTE FAR*)pAddr_p );
+
+}
+*/
+
+//------------< read WORD in big endian >---------------------------
+
+INLINE_FUNCTION WORD PUBLIC AmiGetWordFromBe(void FAR * pAddr_p)
+{
+	twStruct FAR *pwStruct;
+	twStruct wValue;
+
+	pwStruct = (twStruct FAR *) pAddr_p;
+
+	wValue.m_wWord = (WORD) ((pwStruct->m_wWord & 0x00FF) << 8);	//LSB to MSB
+	wValue.m_wWord |= (WORD) ((pwStruct->m_wWord & 0xFF00) >> 8);	//MSB to LSB
+
+	return (wValue.m_wWord);
+
+}
+
+//------------< read DWORD in big endian >--------------------------
+
+INLINE_FUNCTION DWORD PUBLIC AmiGetDwordFromBe(void FAR * pAddr_p)
+{
+	tdwStruct FAR *pdwStruct;
+	tdwStruct dwValue;
+
+	pdwStruct = (tdwStruct FAR *) pAddr_p;
+
+	dwValue.m_dwDword = ((pdwStruct->m_dwDword & 0x000000FF) << 24);	//LSB to MSB
+	dwValue.m_dwDword |= ((pdwStruct->m_dwDword & 0x0000FF00) << 8);
+	dwValue.m_dwDword |= ((pdwStruct->m_dwDword & 0x00FF0000) >> 8);
+	dwValue.m_dwDword |= ((pdwStruct->m_dwDword & 0xFF000000) >> 24);	//MSB to LSB
+
+	return (dwValue.m_dwDword);
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetXXXFromLe()
+//
+// Description: reads the specified value from the absolute address in
+//              little endian
+//
+// Parameters:  pAddr_p                 = absolute address
+//
+// Returns:     XXX                     = value
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+//------------< read BYTE in little endian >---------------------------
+/*
+BYTE  PUBLIC  AmiGetByteFromLe (void FAR* pAddr_p)
+{
+
+   return ( *(BYTE FAR*)pAddr_p );
+
+}
+*/
+
+//------------< read WORD in little endian >---------------------------
+
+INLINE_FUNCTION WORD PUBLIC AmiGetWordFromLe(void FAR * pAddr_p)
+{
+	twStruct FAR *pwStruct;
+
+	pwStruct = (twStruct FAR *) pAddr_p;
+	return (pwStruct->m_wWord);
+
+}
+
+//------------< read DWORD in little endian >--------------------------
+
+INLINE_FUNCTION DWORD PUBLIC AmiGetDwordFromLe(void FAR * pAddr_p)
+{
+	tdwStruct FAR *pdwStruct;
+
+	pdwStruct = (tdwStruct FAR *) pAddr_p;
+	return (pdwStruct->m_dwDword);
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetDword24ToBe()
+//
+// Description: sets a 24 bit value to a buffer in big endian
+//
+// Parameters:  pAddr_p         = pointer to destination buffer
+//              dwDwordVal_p    = value to set
+//
+// Return:      void
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION void PUBLIC AmiSetDword24ToBe(void FAR * pAddr_p,
+					      DWORD dwDwordVal_p)
+{
+
+	((BYTE FAR *) pAddr_p)[0] = ((BYTE FAR *) & dwDwordVal_p)[2];
+	((BYTE FAR *) pAddr_p)[1] = ((BYTE FAR *) & dwDwordVal_p)[1];
+	((BYTE FAR *) pAddr_p)[2] = ((BYTE FAR *) & dwDwordVal_p)[0];
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetDword24ToLe()
+//
+// Description: sets a 24 bit value to a buffer in little endian
+//
+// Parameters:  pAddr_p         = pointer to destination buffer
+//              dwDwordVal_p    = value to set
+//
+// Return:      void
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION void PUBLIC AmiSetDword24ToLe(void FAR * pAddr_p,
+					      DWORD dwDwordVal_p)
+{
+
+	((BYTE FAR *) pAddr_p)[0] = ((BYTE FAR *) & dwDwordVal_p)[0];
+	((BYTE FAR *) pAddr_p)[1] = ((BYTE FAR *) & dwDwordVal_p)[1];
+	((BYTE FAR *) pAddr_p)[2] = ((BYTE FAR *) & dwDwordVal_p)[2];
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetDword24FromBe()
+//
+// Description: reads a 24 bit value from a buffer in big endian
+//
+// Parameters:  pAddr_p         = pointer to source buffer
+//
+// Return:      DWORD           = read value
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION DWORD PUBLIC AmiGetDword24FromBe(void FAR * pAddr_p)
+{
+
+	tdwStruct dwStruct;
+
+	dwStruct.m_dwDword = AmiGetDwordFromBe(pAddr_p);
+	dwStruct.m_dwDword >>= 8;
+
+	return (dwStruct.m_dwDword);
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetDword24FromLe()
+//
+// Description: reads a 24 bit value from a buffer in little endian
+//
+// Parameters:  pAddr_p         = pointer to source buffer
+//
+// Return:      DWORD           = read value
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION DWORD PUBLIC AmiGetDword24FromLe(void FAR * pAddr_p)
+{
+
+	tdwStruct dwStruct;
+
+	dwStruct.m_dwDword = AmiGetDwordFromLe(pAddr_p);
+	dwStruct.m_dwDword &= 0x00FFFFFF;
+
+	return (dwStruct.m_dwDword);
+
+}
+
+//#ifdef USE_VAR64
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetQword64ToBe()
+//
+// Description: sets a 64 bit value to a buffer in big endian
+//
+// Parameters:  pAddr_p         = pointer to destination buffer
+//              qwQwordVal_p    = quadruple word value
+//
+// Return:      void
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION void PUBLIC AmiSetQword64ToBe(void FAR * pAddr_p,
+					      QWORD qwQwordVal_p)
+{
+
+	((BYTE FAR *) pAddr_p)[0] = ((BYTE FAR *) & qwQwordVal_p)[7];
+	((BYTE FAR *) pAddr_p)[1] = ((BYTE FAR *) & qwQwordVal_p)[6];
+	((BYTE FAR *) pAddr_p)[2] = ((BYTE FAR *) & qwQwordVal_p)[5];
+	((BYTE FAR *) pAddr_p)[3] = ((BYTE FAR *) & qwQwordVal_p)[4];
+	((BYTE FAR *) pAddr_p)[4] = ((BYTE FAR *) & qwQwordVal_p)[3];
+	((BYTE FAR *) pAddr_p)[5] = ((BYTE FAR *) & qwQwordVal_p)[2];
+	((BYTE FAR *) pAddr_p)[6] = ((BYTE FAR *) & qwQwordVal_p)[1];
+	((BYTE FAR *) pAddr_p)[7] = ((BYTE FAR *) & qwQwordVal_p)[0];
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetQword64ToLe()
+//
+// Description: sets a 64 bit value to a buffer in little endian
+//
+// Parameters:  pAddr_p         = pointer to destination buffer
+//              qwQwordVal_p    = quadruple word value
+//
+// Return:      void
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION void PUBLIC AmiSetQword64ToLe(void FAR * pAddr_p,
+					      QWORD qwQwordVal_p)
+{
+
+	QWORD FAR *pqwDst;
+
+	pqwDst = (QWORD FAR *) pAddr_p;
+	*pqwDst = qwQwordVal_p;
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetQword64FromBe()
+//
+// Description: reads a 64 bit value from a buffer in big endian
+//
+// Parameters:  pAddr_p         = pointer to source buffer
+//
+// Return:      void
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION QWORD PUBLIC AmiGetQword64FromBe(void FAR * pAddr_p)
+{
+
+	tqwStruct qwStruct;
+
+	((BYTE FAR *) & qwStruct.m_qwQword)[0] = ((BYTE FAR *) pAddr_p)[7];
+	((BYTE FAR *) & qwStruct.m_qwQword)[1] = ((BYTE FAR *) pAddr_p)[6];
+	((BYTE FAR *) & qwStruct.m_qwQword)[2] = ((BYTE FAR *) pAddr_p)[5];
+	((BYTE FAR *) & qwStruct.m_qwQword)[3] = ((BYTE FAR *) pAddr_p)[4];
+	((BYTE FAR *) & qwStruct.m_qwQword)[4] = ((BYTE FAR *) pAddr_p)[3];
+	((BYTE FAR *) & qwStruct.m_qwQword)[5] = ((BYTE FAR *) pAddr_p)[2];
+	((BYTE FAR *) & qwStruct.m_qwQword)[6] = ((BYTE FAR *) pAddr_p)[1];
+	((BYTE FAR *) & qwStruct.m_qwQword)[7] = ((BYTE FAR *) pAddr_p)[0];
+
+	return (qwStruct.m_qwQword);
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetQword64FromLe()
+//
+// Description: reads a 64 bit value from a buffer in little endian
+//
+// Parameters:  pAddr_p         = pointer to source buffer
+//
+// Return:      void
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION QWORD PUBLIC AmiGetQword64FromLe(void FAR * pAddr_p)
+{
+
+	tqwStruct FAR *pqwStruct;
+	tqwStruct qwStruct;
+
+	pqwStruct = (tqwStruct FAR *) pAddr_p;
+	qwStruct.m_qwQword = pqwStruct->m_qwQword;
+
+	return (qwStruct.m_qwQword);
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetQword40ToBe()
+//
+// Description: sets a 40 bit value to a buffer in big endian
+//
+// Parameters:  pAddr_p         = pointer to destination buffer
+//              qwQwordVal_p    = quadruple word value
+//
+// Return:      void
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION void PUBLIC AmiSetQword40ToBe(void FAR * pAddr_p,
+					      QWORD qwQwordVal_p)
+{
+
+	((BYTE FAR *) pAddr_p)[0] = ((BYTE FAR *) & qwQwordVal_p)[4];
+	((BYTE FAR *) pAddr_p)[1] = ((BYTE FAR *) & qwQwordVal_p)[3];
+	((BYTE FAR *) pAddr_p)[2] = ((BYTE FAR *) & qwQwordVal_p)[2];
+	((BYTE FAR *) pAddr_p)[3] = ((BYTE FAR *) & qwQwordVal_p)[1];
+	((BYTE FAR *) pAddr_p)[4] = ((BYTE FAR *) & qwQwordVal_p)[0];
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetQword40ToLe()
+//
+// Description: sets a 40 bit value to a buffer in little endian
+//
+// Parameters:  pAddr_p         = pointer to destination buffer
+//              qwQwordVal_p    = quadruple word value
+//
+// Return:      void
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION void PUBLIC AmiSetQword40ToLe(void FAR * pAddr_p,
+					      QWORD qwQwordVal_p)
+{
+
+	((DWORD FAR *) pAddr_p)[0] = ((DWORD FAR *) & qwQwordVal_p)[0];
+	((BYTE FAR *) pAddr_p)[4] = ((BYTE FAR *) & qwQwordVal_p)[4];
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetQword40FromBe()
+//
+// Description: reads a 40 bit value from a buffer in big endian
+//
+// Parameters:  pAddr_p         = pointer to source buffer
+//
+// Return:      QWORD
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION QWORD PUBLIC AmiGetQword40FromBe(void FAR * pAddr_p)
+{
+
+	tqwStruct qwStruct;
+
+	qwStruct.m_qwQword = AmiGetQword64FromBe(pAddr_p);
+	qwStruct.m_qwQword >>= 24;
+
+	return (qwStruct.m_qwQword);
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetQword40FromLe()
+//
+// Description: reads a 40 bit value from a buffer in little endian
+//
+// Parameters:  pAddr_p         = pointer to source buffer
+//
+// Return:      QWORD
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION QWORD PUBLIC AmiGetQword40FromLe(void FAR * pAddr_p)
+{
+
+	tqwStruct qwStruct;
+
+	qwStruct.m_qwQword = AmiGetQword64FromLe(pAddr_p);
+	qwStruct.m_qwQword &= 0x000000FFFFFFFFFFLL;
+
+	return (qwStruct.m_qwQword);
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetQword48ToBe()
+//
+// Description: sets a 48 bit value to a buffer in big endian
+//
+// Parameters:  pAddr_p         = pointer to destination buffer
+//              qwQwordVal_p    = quadruple word value
+//
+// Return:      void
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION void PUBLIC AmiSetQword48ToBe(void FAR * pAddr_p,
+					      QWORD qwQwordVal_p)
+{
+
+	((BYTE FAR *) pAddr_p)[0] = ((BYTE FAR *) & qwQwordVal_p)[5];
+	((BYTE FAR *) pAddr_p)[1] = ((BYTE FAR *) & qwQwordVal_p)[4];
+	((BYTE FAR *) pAddr_p)[2] = ((BYTE FAR *) & qwQwordVal_p)[3];
+	((BYTE FAR *) pAddr_p)[3] = ((BYTE FAR *) & qwQwordVal_p)[2];
+	((BYTE FAR *) pAddr_p)[4] = ((BYTE FAR *) & qwQwordVal_p)[1];
+	((BYTE FAR *) pAddr_p)[5] = ((BYTE FAR *) & qwQwordVal_p)[0];
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetQword48ToLe()
+//
+// Description: sets a 48 bit value to a buffer in little endian
+//
+// Parameters:  pAddr_p         = pointer to destination buffer
+//              qwQwordVal_p    = quadruple word value
+//
+// Return:      void
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION void PUBLIC AmiSetQword48ToLe(void FAR * pAddr_p,
+					      QWORD qwQwordVal_p)
+{
+
+	((DWORD FAR *) pAddr_p)[0] = ((DWORD FAR *) & qwQwordVal_p)[0];
+	((WORD FAR *) pAddr_p)[2] = ((WORD FAR *) & qwQwordVal_p)[2];
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetQword48FromBe()
+//
+// Description: reads a 48 bit value from a buffer in big endian
+//
+// Parameters:  pAddr_p         = pointer to source buffer
+//
+// Return:      QWORD
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION QWORD PUBLIC AmiGetQword48FromBe(void FAR * pAddr_p)
+{
+
+	tqwStruct qwStruct;
+
+	qwStruct.m_qwQword = AmiGetQword64FromBe(pAddr_p);
+	qwStruct.m_qwQword >>= 16;
+
+	return (qwStruct.m_qwQword);
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetQword48FromLe()
+//
+// Description: reads a 48 bit value from a buffer in little endian
+//
+// Parameters:  pAddr_p         = pointer to source buffer
+//
+// Return:      QWORD
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION QWORD PUBLIC AmiGetQword48FromLe(void FAR * pAddr_p)
+{
+
+	tqwStruct qwStruct;
+
+	qwStruct.m_qwQword = AmiGetQword64FromLe(pAddr_p);
+	qwStruct.m_qwQword &= 0x0000FFFFFFFFFFFFLL;
+
+	return (qwStruct.m_qwQword);
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetQword56ToBe()
+//
+// Description: sets a 56 bit value to a buffer in big endian
+//
+// Parameters:  pAddr_p         = pointer to destination buffer
+//              qwQwordVal_p    = quadruple word value
+//
+// Return:      void
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION void PUBLIC AmiSetQword56ToBe(void FAR * pAddr_p,
+					      QWORD qwQwordVal_p)
+{
+
+	((BYTE FAR *) pAddr_p)[0] = ((BYTE FAR *) & qwQwordVal_p)[6];
+	((BYTE FAR *) pAddr_p)[1] = ((BYTE FAR *) & qwQwordVal_p)[5];
+	((BYTE FAR *) pAddr_p)[2] = ((BYTE FAR *) & qwQwordVal_p)[4];
+	((BYTE FAR *) pAddr_p)[3] = ((BYTE FAR *) & qwQwordVal_p)[3];
+	((BYTE FAR *) pAddr_p)[4] = ((BYTE FAR *) & qwQwordVal_p)[2];
+	((BYTE FAR *) pAddr_p)[5] = ((BYTE FAR *) & qwQwordVal_p)[1];
+	((BYTE FAR *) pAddr_p)[6] = ((BYTE FAR *) & qwQwordVal_p)[0];
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetQword56ToLe()
+//
+// Description: sets a 56 bit value to a buffer in little endian
+//
+// Parameters:  pAddr_p         = pointer to destination buffer
+//              qwQwordVal_p    = quadruple word value
+//
+// Return:      void
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION void PUBLIC AmiSetQword56ToLe(void FAR * pAddr_p,
+					      QWORD qwQwordVal_p)
+{
+
+	((DWORD FAR *) pAddr_p)[0] = ((DWORD FAR *) & qwQwordVal_p)[0];
+	((WORD FAR *) pAddr_p)[2] = ((WORD FAR *) & qwQwordVal_p)[2];
+	((BYTE FAR *) pAddr_p)[6] = ((BYTE FAR *) & qwQwordVal_p)[6];
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetQword56FromBe()
+//
+// Description: reads a 56 bit value from a buffer in big endian
+//
+// Parameters:  pAddr_p         = pointer to source buffer
+//
+// Return:      QWORD
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION QWORD PUBLIC AmiGetQword56FromBe(void FAR * pAddr_p)
+{
+
+	tqwStruct qwStruct;
+
+	qwStruct.m_qwQword = AmiGetQword64FromBe(pAddr_p);
+	qwStruct.m_qwQword >>= 8;
+
+	return (qwStruct.m_qwQword);
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetQword56FromLe()
+//
+// Description: reads a 56 bit value from a buffer in little endian
+//
+// Parameters:  pAddr_p         = pointer to source buffer
+//
+// Return:      QWORD
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION QWORD PUBLIC AmiGetQword56FromLe(void FAR * pAddr_p)
+{
+
+	tqwStruct qwStruct;
+
+	qwStruct.m_qwQword = AmiGetQword64FromLe(pAddr_p);
+	qwStruct.m_qwQword &= 0x00FFFFFFFFFFFFFFLL;
+
+	return (qwStruct.m_qwQword);
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiSetTimeOfDay()
+//
+// Description: sets a TIME_OF_DAY (CANopen) value to a buffer
+//
+// Parameters:  pAddr_p         = pointer to destination buffer
+//              pTimeOfDay_p    = pointer to struct TIME_OF_DAY
+//
+// Return:      void
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION void PUBLIC AmiSetTimeOfDay(void FAR * pAddr_p,
+					    tTimeOfDay FAR * pTimeOfDay_p)
+{
+
+	AmiSetDwordToLe(((BYTE FAR *) pAddr_p),
+			pTimeOfDay_p->m_dwMs & 0x0FFFFFFF);
+	AmiSetWordToLe(((BYTE FAR *) pAddr_p) + 4, pTimeOfDay_p->m_wDays);
+
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AmiGetTimeOfDay()
+//
+// Description: reads a TIME_OF_DAY (CANopen) value from a buffer
+//
+// Parameters:  pAddr_p         = pointer to source buffer
+//              pTimeOfDay_p    = pointer to struct TIME_OF_DAY
+//
+// Return:      void
+//
+// State:       not tested
+//
+//---------------------------------------------------------------------------
+
+INLINE_FUNCTION void PUBLIC AmiGetTimeOfDay(void FAR * pAddr_p,
+					    tTimeOfDay FAR * pTimeOfDay_p)
+{
+
+	pTimeOfDay_p->m_dwMs =
+	    AmiGetDwordFromLe(((BYTE FAR *) pAddr_p)) & 0x0FFFFFFF;
+	pTimeOfDay_p->m_wDays = AmiGetWordFromLe(((BYTE FAR *) pAddr_p) + 4);
+
+}
+
+#endif
+
+// EOF
+
+// Die letzte Zeile muß unbedingt eine leere Zeile sein, weil manche Compiler
+// damit ein Problem haben, wenn das nicht so ist (z.B. GNU oder Borland C++ Builder).
diff --git a/drivers/staging/epl/demo_main.c b/drivers/staging/epl/demo_main.c
new file mode 100644
index 00000000000..263fa042291
--- /dev/null
+++ b/drivers/staging/epl/demo_main.c
@@ -0,0 +1,961 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  demoapplication for EPL MN (with SDO over UDP)
+                under Linux on X86 with RTL8139 Ethernet controller
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: demo_main.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.10 $  $Date: 2008/11/19 18:11:43 $
+
+                $State: Exp $
+
+                Build Environment:
+                GCC
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/09/01 d.k.:   start of implementation
+
+****************************************************************************/
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/major.h>
+#include <linux/version.h>
+#include <asm/io.h>
+#include <asm/uaccess.h>
+#include <asm/atomic.h>
+#include <linux/sched.h>
+#include <linux/kmod.h>
+#include <linux/slab.h>
+#include <linux/pci.h>
+#include <linux/proc_fs.h>
+
+#include "Epl.h"
+#include "proc_fs.h"
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0)
+    // remove ("make invisible") obsolete symbols for kernel versions 2.6
+    // and higher
+#define MOD_INC_USE_COUNT
+#define MOD_DEC_USE_COUNT
+#define EXPORT_NO_SYMBOLS
+#else
+#error "This driver needs a 2.6.x kernel or higher"
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+// Metainformation
+MODULE_LICENSE("Dual BSD/GPL");
+#ifdef MODULE_AUTHOR
+MODULE_AUTHOR("Daniel.Krueger@SYSTEC-electronic.com");
+MODULE_DESCRIPTION("EPL MN demo");
+#endif
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+// TracePoint support for realtime-debugging
+#ifdef _DBG_TRACE_POINTS_
+void PUBLIC TgtDbgSignalTracePoint(BYTE bTracePointNumber_p);
+#define TGT_DBG_SIGNAL_TRACE_POINT(p)   TgtDbgSignalTracePoint(p)
+#else
+#define TGT_DBG_SIGNAL_TRACE_POINT(p)
+#endif
+
+#define NODEID      0xF0	//=> MN
+#define CYCLE_LEN   5000	// [us]
+#define IP_ADDR     0xc0a86401	// 192.168.100.1
+#define SUBNET_MASK 0xFFFFFF00	// 255.255.255.0
+#define HOSTNAME    "SYS TEC electronic EPL Stack    "
+#define IF_ETH      EPL_VETH_NAME
+
+// LIGHT EFFECT
+#define DEFAULT_MAX_CYCLE_COUNT 20	// 6 is very fast
+#define APP_DEFAULT_MODE        0x01
+#define APP_LED_COUNT           5	// number of LEDs in one row
+#define APP_LED_MASK            ((1 << APP_LED_COUNT) - 1)
+#define APP_DOUBLE_LED_MASK     ((1 << (APP_LED_COUNT * 2)) - 1)
+#define APP_MODE_COUNT          5
+#define APP_MODE_MASK           ((1 << APP_MODE_COUNT) - 1)
+
+//---------------------------------------------------------------------------
+// local types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// modul globale vars
+//---------------------------------------------------------------------------
+
+CONST BYTE abMacAddr[] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
+
+BYTE bVarIn1_l;
+BYTE bVarOut1_l;
+BYTE bVarOut1Old_l;
+BYTE bModeSelect_l;		// state of the pushbuttons to select the mode
+BYTE bSpeedSelect_l;		// state of the pushbuttons to increase/decrease the speed
+BYTE bSpeedSelectOld_l;		// old state of the pushbuttons
+DWORD dwLeds_l;			// current state of all LEDs
+BYTE bLedsRow1_l;		// current state of the LEDs in row 1
+BYTE bLedsRow2_l;		// current state of the LEDs in row 2
+BYTE abSelect_l[3];		// pushbuttons from CNs
+
+DWORD dwMode_l;			// current mode
+int iCurCycleCount_l;		// current cycle count
+int iMaxCycleCount_l;		// maximum cycle count (i.e. number of cycles until next light movement step)
+int iToggle;			// indicates the light movement direction
+
+BYTE abDomain_l[3000];
+
+static wait_queue_head_t WaitQueueShutdown_g;	// wait queue for tEplNmtEventSwitchOff
+static atomic_t AtomicShutdown_g = ATOMIC_INIT(FALSE);
+
+static DWORD dw_le_CycleLen_g;
+
+static uint uiNodeId_g = EPL_C_ADR_INVALID;
+module_param_named(nodeid, uiNodeId_g, uint, 0);
+
+static uint uiCycleLen_g = CYCLE_LEN;
+module_param_named(cyclelen, uiCycleLen_g, uint, 0);
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+// This function is the entry point for your object dictionary. It is defined
+// in OBJDICT.C by define EPL_OBD_INIT_RAM_NAME. Use this function name to define
+// this function prototype here. If you want to use more than one Epl
+// instances then the function name of each object dictionary has to differ.
+
+tEplKernel PUBLIC EplObdInitRam(tEplObdInitParam MEM * pInitParam_p);
+
+tEplKernel PUBLIC AppCbEvent(tEplApiEventType EventType_p,	// IN: event type (enum)
+			     tEplApiEventArg * pEventArg_p,	// IN: event argument (union)
+			     void GENERIC * pUserArg_p);
+
+tEplKernel PUBLIC AppCbSync(void);
+
+static int __init EplLinInit(void);
+static void __exit EplLinExit(void);
+
+//---------------------------------------------------------------------------
+//  Kernel Module specific Data Structures
+//---------------------------------------------------------------------------
+
+EXPORT_NO_SYMBOLS;
+
+//module_init(EplLinInit);
+//module_exit(EplLinExit);
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:
+//
+// Description:
+//
+//
+//
+// Parameters:
+//
+//
+// Returns:
+//
+//
+// State:
+//
+//---------------------------------------------------------------------------
+static int __init EplLinInit(void)
+{
+	tEplKernel EplRet;
+	int iRet;
+	static tEplApiInitParam EplApiInitParam = { 0 };
+	char *sHostname = HOSTNAME;
+	char *argv[4], *envp[3];
+	char sBuffer[16];
+	unsigned int uiVarEntries;
+	tEplObdSize ObdSize;
+
+	atomic_set(&AtomicShutdown_g, TRUE);
+
+	// get node ID from insmod command line
+	EplApiInitParam.m_uiNodeId = uiNodeId_g;
+
+	if (EplApiInitParam.m_uiNodeId == EPL_C_ADR_INVALID) {	// invalid node ID set
+		// set default node ID
+		EplApiInitParam.m_uiNodeId = NODEID;
+	}
+
+	uiNodeId_g = EplApiInitParam.m_uiNodeId;
+
+	// calculate IP address
+	EplApiInitParam.m_dwIpAddress =
+	    (0xFFFFFF00 & IP_ADDR) | EplApiInitParam.m_uiNodeId;
+
+	EplApiInitParam.m_fAsyncOnly = FALSE;
+
+	EplApiInitParam.m_uiSizeOfStruct = sizeof(EplApiInitParam);
+	EPL_MEMCPY(EplApiInitParam.m_abMacAddress, abMacAddr,
+		   sizeof(EplApiInitParam.m_abMacAddress));
+//    EplApiInitParam.m_abMacAddress[5] = (BYTE) EplApiInitParam.m_uiNodeId;
+	EplApiInitParam.m_dwFeatureFlags = -1;
+	EplApiInitParam.m_dwCycleLen = uiCycleLen_g;	// required for error detection
+	EplApiInitParam.m_uiIsochrTxMaxPayload = 100;	// const
+	EplApiInitParam.m_uiIsochrRxMaxPayload = 100;	// const
+	EplApiInitParam.m_dwPresMaxLatency = 50000;	// const; only required for IdentRes
+	EplApiInitParam.m_uiPreqActPayloadLimit = 36;	// required for initialisation (+28 bytes)
+	EplApiInitParam.m_uiPresActPayloadLimit = 36;	// required for initialisation of Pres frame (+28 bytes)
+	EplApiInitParam.m_dwAsndMaxLatency = 150000;	// const; only required for IdentRes
+	EplApiInitParam.m_uiMultiplCycleCnt = 0;	// required for error detection
+	EplApiInitParam.m_uiAsyncMtu = 1500;	// required to set up max frame size
+	EplApiInitParam.m_uiPrescaler = 2;	// required for sync
+	EplApiInitParam.m_dwLossOfFrameTolerance = 500000;
+	EplApiInitParam.m_dwAsyncSlotTimeout = 3000000;
+	EplApiInitParam.m_dwWaitSocPreq = 150000;
+	EplApiInitParam.m_dwDeviceType = -1;	// NMT_DeviceType_U32
+	EplApiInitParam.m_dwVendorId = -1;	// NMT_IdentityObject_REC.VendorId_U32
+	EplApiInitParam.m_dwProductCode = -1;	// NMT_IdentityObject_REC.ProductCode_U32
+	EplApiInitParam.m_dwRevisionNumber = -1;	// NMT_IdentityObject_REC.RevisionNo_U32
+	EplApiInitParam.m_dwSerialNumber = -1;	// NMT_IdentityObject_REC.SerialNo_U32
+	EplApiInitParam.m_dwSubnetMask = SUBNET_MASK;
+	EplApiInitParam.m_dwDefaultGateway = 0;
+	EPL_MEMCPY(EplApiInitParam.m_sHostname, sHostname,
+		   sizeof(EplApiInitParam.m_sHostname));
+
+	// currently unset parameters left at default value 0
+	//EplApiInitParam.m_qwVendorSpecificExt1;
+	//EplApiInitParam.m_dwVerifyConfigurationDate; // CFM_VerifyConfiguration_REC.ConfDate_U32
+	//EplApiInitParam.m_dwVerifyConfigurationTime; // CFM_VerifyConfiguration_REC.ConfTime_U32
+	//EplApiInitParam.m_dwApplicationSwDate;       // PDL_LocVerApplSw_REC.ApplSwDate_U32 on programmable device or date portion of NMT_ManufactSwVers_VS on non-programmable device
+	//EplApiInitParam.m_dwApplicationSwTime;       // PDL_LocVerApplSw_REC.ApplSwTime_U32 on programmable device or time portion of NMT_ManufactSwVers_VS on non-programmable device
+	//EplApiInitParam.m_abVendorSpecificExt2[48];
+
+	// set callback functions
+	EplApiInitParam.m_pfnCbEvent = AppCbEvent;
+	EplApiInitParam.m_pfnCbSync = AppCbSync;
+
+	printk
+	    ("\n\n Hello, I'm a simple POWERLINK node running as %s!\n  (build: %s / %s)\n\n",
+	     (uiNodeId_g ==
+	      EPL_C_ADR_MN_DEF_NODE_ID ? "Managing Node" : "Controlled Node"),
+	     __DATE__, __TIME__);
+
+	// initialize the Linux a wait queue for shutdown of this module
+	init_waitqueue_head(&WaitQueueShutdown_g);
+
+	// initialize the procfs device
+	EplRet = EplLinProcInit();
+	if (EplRet != kEplSuccessful) {
+		goto Exit;
+	}
+	// initialize POWERLINK stack
+	EplRet = EplApiInitialize(&EplApiInitParam);
+	if (EplRet != kEplSuccessful) {
+		goto Exit;
+	}
+	// link process variables used by CN to object dictionary
+	ObdSize = sizeof(bVarIn1_l);
+	uiVarEntries = 1;
+	EplRet =
+	    EplApiLinkObject(0x6000, &bVarIn1_l, &uiVarEntries, &ObdSize, 0x01);
+	if (EplRet != kEplSuccessful) {
+		goto Exit;
+	}
+
+	ObdSize = sizeof(bVarOut1_l);
+	uiVarEntries = 1;
+	EplRet =
+	    EplApiLinkObject(0x6200, &bVarOut1_l, &uiVarEntries, &ObdSize,
+			     0x01);
+	if (EplRet != kEplSuccessful) {
+		goto Exit;
+	}
+	// link process variables used by MN to object dictionary
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+	ObdSize = sizeof(bLedsRow1_l);
+	uiVarEntries = 1;
+	EplRet =
+	    EplApiLinkObject(0x2000, &bLedsRow1_l, &uiVarEntries, &ObdSize,
+			     0x01);
+	if (EplRet != kEplSuccessful) {
+		goto Exit;
+	}
+
+	ObdSize = sizeof(bLedsRow2_l);
+	uiVarEntries = 1;
+	EplRet =
+	    EplApiLinkObject(0x2000, &bLedsRow2_l, &uiVarEntries, &ObdSize,
+			     0x02);
+	if (EplRet != kEplSuccessful) {
+		goto Exit;
+	}
+
+	ObdSize = sizeof(bSpeedSelect_l);
+	uiVarEntries = 1;
+	EplRet =
+	    EplApiLinkObject(0x2000, &bSpeedSelect_l, &uiVarEntries, &ObdSize,
+			     0x03);
+	if (EplRet != kEplSuccessful) {
+		goto Exit;
+	}
+
+	ObdSize = sizeof(bSpeedSelectOld_l);
+	uiVarEntries = 1;
+	EplRet =
+	    EplApiLinkObject(0x2000, &bSpeedSelectOld_l, &uiVarEntries,
+			     &ObdSize, 0x04);
+	if (EplRet != kEplSuccessful) {
+		goto Exit;
+	}
+
+	ObdSize = sizeof(abSelect_l[0]);
+	uiVarEntries = sizeof(abSelect_l);
+	EplRet =
+	    EplApiLinkObject(0x2200, &abSelect_l[0], &uiVarEntries, &ObdSize,
+			     0x01);
+	if (EplRet != kEplSuccessful) {
+		goto Exit;
+	}
+#endif
+
+	// link a DOMAIN to object 0x6100, but do not exit, if it is missing
+	ObdSize = sizeof(abDomain_l);
+	uiVarEntries = 1;
+	EplRet =
+	    EplApiLinkObject(0x6100, &abDomain_l, &uiVarEntries, &ObdSize,
+			     0x00);
+	if (EplRet != kEplSuccessful) {
+		printk("EplApiLinkObject(0x6100): returns 0x%X\n", EplRet);
+	}
+	// reset old process variables
+	bVarOut1Old_l = 0;
+	bSpeedSelectOld_l = 0;
+	dwMode_l = APP_DEFAULT_MODE;
+	iMaxCycleCount_l = DEFAULT_MAX_CYCLE_COUNT;
+
+	// configure IP address of virtual network interface
+	// for TCP/IP communication over the POWERLINK network
+	sprintf(sBuffer, "%lu.%lu.%lu.%lu",
+		(EplApiInitParam.m_dwIpAddress >> 24),
+		((EplApiInitParam.m_dwIpAddress >> 16) & 0xFF),
+		((EplApiInitParam.m_dwIpAddress >> 8) & 0xFF),
+		(EplApiInitParam.m_dwIpAddress & 0xFF));
+	/* set up a minimal environment */
+	iRet = 0;
+	envp[iRet++] = "HOME=/";
+	envp[iRet++] = "PATH=/sbin:/bin:/usr/sbin:/usr/bin";
+	envp[iRet] = NULL;
+
+	/* set up the argument list */
+	iRet = 0;
+	argv[iRet++] = "/sbin/ifconfig";
+	argv[iRet++] = IF_ETH;
+	argv[iRet++] = sBuffer;
+	argv[iRet] = NULL;
+
+	/* call ifconfig to configure the virtual network interface */
+	iRet = call_usermodehelper(argv[0], argv, envp, 1);
+	printk("ifconfig %s %s returned %d\n", argv[1], argv[2], iRet);
+
+	// start the NMT state machine
+	EplRet = EplApiExecNmtCommand(kEplNmtEventSwReset);
+	atomic_set(&AtomicShutdown_g, FALSE);
+
+      Exit:
+	printk("EplLinInit(): returns 0x%X\n", EplRet);
+	return EplRet;
+}
+
+static void __exit EplLinExit(void)
+{
+	tEplKernel EplRet;
+
+	// halt the NMT state machine
+	// so the processing of POWERLINK frames stops
+	EplRet = EplApiExecNmtCommand(kEplNmtEventSwitchOff);
+
+	// wait until NMT state machine is shut down
+	wait_event_interruptible(WaitQueueShutdown_g,
+				 (atomic_read(&AtomicShutdown_g) == TRUE));
+/*    if ((iErr != 0) || (atomic_read(&AtomicShutdown_g) == EVENT_STATE_IOCTL))
+    {   // waiting was interrupted by signal or application called wrong function
+        EplRet = kEplShutdown;
+    }*/
+	// delete instance for all modules
+	EplRet = EplApiShutdown();
+	printk("EplApiShutdown():  0x%X\n", EplRet);
+
+	// deinitialize proc fs
+	EplRet = EplLinProcFree();
+	printk("EplLinProcFree():        0x%X\n", EplRet);
+
+}
+
+//=========================================================================//
+//                                                                         //
+//          P R I V A T E   F U N C T I O N S                              //
+//                                                                         //
+//=========================================================================//
+
+//---------------------------------------------------------------------------
+//
+// Function:    AppCbEvent
+//
+// Description: event callback function called by EPL API layer within
+//              user part (low priority).
+//
+// Parameters:  EventType_p     = event type
+//              pEventArg_p     = pointer to union, which describes
+//                                the event in detail
+//              pUserArg_p      = user specific argument
+//
+// Returns:     tEplKernel      = error code,
+//                                kEplSuccessful = no error
+//                                kEplReject = reject further processing
+//                                otherwise = post error event to API layer
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC AppCbEvent(tEplApiEventType EventType_p,	// IN: event type (enum)
+			     tEplApiEventArg * pEventArg_p,	// IN: event argument (union)
+			     void GENERIC * pUserArg_p)
+{
+	tEplKernel EplRet = kEplSuccessful;
+
+	// check if NMT_GS_OFF is reached
+	switch (EventType_p) {
+	case kEplApiEventNmtStateChange:
+		{
+			switch (pEventArg_p->m_NmtStateChange.m_NewNmtState) {
+			case kEplNmtGsOff:
+				{	// NMT state machine was shut down,
+					// because of user signal (CTRL-C) or critical EPL stack error
+					// -> also shut down EplApiProcess() and main()
+					EplRet = kEplShutdown;
+
+					printk
+					    ("AppCbEvent(kEplNmtGsOff) originating event = 0x%X\n",
+					     pEventArg_p->m_NmtStateChange.
+					     m_NmtEvent);
+
+					// wake up EplLinExit()
+					atomic_set(&AtomicShutdown_g, TRUE);
+					wake_up_interruptible
+					    (&WaitQueueShutdown_g);
+					break;
+				}
+
+			case kEplNmtGsResetCommunication:
+				{
+					DWORD dwBuffer;
+
+					// configure OD for MN in state ResetComm after reseting the OD
+					// TODO: setup your own network configuration here
+					dwBuffer = (EPL_NODEASSIGN_NODE_IS_CN | EPL_NODEASSIGN_NODE_EXISTS);	// 0x00000003L
+					EplRet =
+					    EplApiWriteLocalObject(0x1F81, 0x01,
+								   &dwBuffer,
+								   4);
+					EplRet =
+					    EplApiWriteLocalObject(0x1F81, 0x02,
+								   &dwBuffer,
+								   4);
+					EplRet =
+					    EplApiWriteLocalObject(0x1F81, 0x03,
+								   &dwBuffer,
+								   4);
+					EplRet =
+					    EplApiWriteLocalObject(0x1F81, 0x04,
+								   &dwBuffer,
+								   4);
+					EplRet =
+					    EplApiWriteLocalObject(0x1F81, 0x05,
+								   &dwBuffer,
+								   4);
+					EplRet =
+					    EplApiWriteLocalObject(0x1F81, 0x06,
+								   &dwBuffer,
+								   4);
+					EplRet =
+					    EplApiWriteLocalObject(0x1F81, 0x07,
+								   &dwBuffer,
+								   4);
+					EplRet =
+					    EplApiWriteLocalObject(0x1F81, 0x08,
+								   &dwBuffer,
+								   4);
+					EplRet =
+					    EplApiWriteLocalObject(0x1F81, 0x20,
+								   &dwBuffer,
+								   4);
+					EplRet =
+					    EplApiWriteLocalObject(0x1F81, 0xFE,
+								   &dwBuffer,
+								   4);
+					EplRet =
+					    EplApiWriteLocalObject(0x1F81, 0x6E,
+								   &dwBuffer,
+								   4);
+
+//                    dwBuffer |= EPL_NODEASSIGN_MANDATORY_CN;    // 0x0000000BL
+//                    EplRet = EplApiWriteLocalObject(0x1F81, 0x6E, &dwBuffer, 4);
+					dwBuffer = (EPL_NODEASSIGN_MN_PRES | EPL_NODEASSIGN_NODE_EXISTS);	// 0x00010001L
+					EplRet =
+					    EplApiWriteLocalObject(0x1F81, 0xF0,
+								   &dwBuffer,
+								   4);
+
+					// continue
+				}
+
+			case kEplNmtGsResetConfiguration:
+				{
+					unsigned int uiSize;
+
+					// fetch object 0x1006 NMT_CycleLen_U32 from local OD (in little endian byte order)
+					// for configuration of remote CN
+					uiSize = 4;
+					EplRet =
+					    EplApiReadObject(NULL, 0, 0x1006,
+							     0x00,
+							     &dw_le_CycleLen_g,
+							     &uiSize,
+							     kEplSdoTypeAsnd,
+							     NULL);
+					if (EplRet != kEplSuccessful) {	// local OD access failed
+						break;
+					}
+					// continue
+				}
+
+			case kEplNmtMsPreOperational1:
+				{
+					printk
+					    ("AppCbEvent(0x%X) originating event = 0x%X\n",
+					     pEventArg_p->m_NmtStateChange.
+					     m_NewNmtState,
+					     pEventArg_p->m_NmtStateChange.
+					     m_NmtEvent);
+
+					// continue
+				}
+
+			case kEplNmtGsInitialising:
+			case kEplNmtGsResetApplication:
+			case kEplNmtMsNotActive:
+			case kEplNmtCsNotActive:
+			case kEplNmtCsPreOperational1:
+				{
+					break;
+				}
+
+			case kEplNmtCsOperational:
+			case kEplNmtMsOperational:
+				{
+					break;
+				}
+
+			default:
+				{
+					break;
+				}
+			}
+
+/*
+            switch (pEventArg_p->m_NmtStateChange.m_NmtEvent)
+            {
+                case kEplNmtEventSwReset:
+                case kEplNmtEventResetNode:
+                case kEplNmtEventResetCom:
+                case kEplNmtEventResetConfig:
+                case kEplNmtEventInternComError:
+                case kEplNmtEventNmtCycleError:
+                {
+                    printk("AppCbEvent(0x%X) originating event = 0x%X\n",
+                           pEventArg_p->m_NmtStateChange.m_NewNmtState,
+                           pEventArg_p->m_NmtStateChange.m_NmtEvent);
+                    break;
+                }
+
+                default:
+                {
+                    break;
+                }
+            }
+*/
+			break;
+		}
+
+	case kEplApiEventCriticalError:
+	case kEplApiEventWarning:
+		{		// error or warning occured within the stack or the application
+			// on error the API layer stops the NMT state machine
+
+			printk
+			    ("AppCbEvent(Err/Warn): Source=%02X EplError=0x%03X",
+			     pEventArg_p->m_InternalError.m_EventSource,
+			     pEventArg_p->m_InternalError.m_EplError);
+			// check additional argument
+			switch (pEventArg_p->m_InternalError.m_EventSource) {
+			case kEplEventSourceEventk:
+			case kEplEventSourceEventu:
+				{	// error occured within event processing
+					// either in kernel or in user part
+					printk(" OrgSource=%02X\n",
+					       pEventArg_p->m_InternalError.
+					       m_Arg.m_EventSource);
+					break;
+				}
+
+			case kEplEventSourceDllk:
+				{	// error occured within the data link layer (e.g. interrupt processing)
+					// the DWORD argument contains the DLL state and the NMT event
+					printk(" val=%lX\n",
+					       pEventArg_p->m_InternalError.
+					       m_Arg.m_dwArg);
+					break;
+				}
+
+			default:
+				{
+					printk("\n");
+					break;
+				}
+			}
+			break;
+		}
+
+	case kEplApiEventNode:
+		{
+//            printk("AppCbEvent(Node): Source=%02X EplError=0x%03X", pEventArg_p->m_InternalError.m_EventSource, pEventArg_p->m_InternalError.m_EplError);
+			// check additional argument
+			switch (pEventArg_p->m_Node.m_NodeEvent) {
+			case kEplNmtNodeEventCheckConf:
+				{
+					tEplSdoComConHdl SdoComConHdl;
+					// update object 0x1006 on CN
+					EplRet =
+					    EplApiWriteObject(&SdoComConHdl,
+							      pEventArg_p->
+							      m_Node.m_uiNodeId,
+							      0x1006, 0x00,
+							      &dw_le_CycleLen_g,
+							      4,
+							      kEplSdoTypeAsnd,
+							      NULL);
+					if (EplRet == kEplApiTaskDeferred) {	// SDO transfer started
+						EplRet = kEplReject;
+					} else if (EplRet == kEplSuccessful) {	// local OD access (should not occur)
+						printk
+						    ("AppCbEvent(Node) write to local OD\n");
+					} else {	// error occured
+						TGT_DBG_SIGNAL_TRACE_POINT(1);
+
+						EplRet =
+						    EplApiFreeSdoChannel
+						    (SdoComConHdl);
+						SdoComConHdl = 0;
+
+						EplRet =
+						    EplApiWriteObject
+						    (&SdoComConHdl,
+						     pEventArg_p->m_Node.
+						     m_uiNodeId, 0x1006, 0x00,
+						     &dw_le_CycleLen_g, 4,
+						     kEplSdoTypeAsnd, NULL);
+						if (EplRet == kEplApiTaskDeferred) {	// SDO transfer started
+							EplRet = kEplReject;
+						} else {
+							printk
+							    ("AppCbEvent(Node): EplApiWriteObject() returned 0x%02X\n",
+							     EplRet);
+						}
+					}
+
+					break;
+				}
+
+			default:
+				{
+					break;
+				}
+			}
+			break;
+		}
+
+	case kEplApiEventSdo:
+		{		// SDO transfer finished
+			EplRet =
+			    EplApiFreeSdoChannel(pEventArg_p->m_Sdo.
+						 m_SdoComConHdl);
+			if (EplRet != kEplSuccessful) {
+				break;
+			}
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+			if (pEventArg_p->m_Sdo.m_SdoComConState == kEplSdoComTransferFinished) {	// continue boot-up of CN with NMT command Reset Configuration
+				EplRet =
+				    EplApiMnTriggerStateChange(pEventArg_p->
+							       m_Sdo.m_uiNodeId,
+							       kEplNmtNodeCommandConfReset);
+			} else {	// indicate configuration error CN
+				EplRet =
+				    EplApiMnTriggerStateChange(pEventArg_p->
+							       m_Sdo.m_uiNodeId,
+							       kEplNmtNodeCommandConfErr);
+			}
+#endif
+
+			break;
+		}
+
+	default:
+		break;
+	}
+
+	return EplRet;
+}
+
+//---------------------------------------------------------------------------
+//
+// Function:    AppCbSync
+//
+// Description: sync event callback function called by event module within
+//              kernel part (high priority).
+//              This function sets the outputs, reads the inputs and runs
+//              the control loop.
+//
+// Parameters:  void
+//
+// Returns:     tEplKernel      = error code,
+//                                kEplSuccessful = no error
+//                                otherwise = post error event to API layer
+//
+// State:
+//
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC AppCbSync(void)
+{
+	tEplKernel EplRet = kEplSuccessful;
+
+	if (bVarOut1Old_l != bVarOut1_l) {	// output variable has changed
+		bVarOut1Old_l = bVarOut1_l;
+		// set LEDs
+
+//        printk("bVarIn = 0x%02X bVarOut = 0x%02X\n", (WORD) bVarIn_l, (WORD) bVarOut_l);
+	}
+	if (uiNodeId_g != EPL_C_ADR_MN_DEF_NODE_ID) {
+		bVarIn1_l++;
+	}
+
+	if (uiNodeId_g == EPL_C_ADR_MN_DEF_NODE_ID) {	// we are the master and must run the control loop
+
+		// collect inputs from CNs and own input
+		bSpeedSelect_l = (bVarIn1_l | abSelect_l[0]) & 0x07;
+
+		bModeSelect_l = abSelect_l[1] | abSelect_l[2];
+
+		if ((bModeSelect_l & APP_MODE_MASK) != 0) {
+			dwMode_l = bModeSelect_l & APP_MODE_MASK;
+		}
+
+		iCurCycleCount_l--;
+
+		if (iCurCycleCount_l <= 0) {
+			if ((dwMode_l & 0x01) != 0) {	// fill-up
+				if (iToggle) {
+					if ((dwLeds_l & APP_DOUBLE_LED_MASK) ==
+					    0x00) {
+						dwLeds_l = 0x01;
+					} else {
+						dwLeds_l <<= 1;
+						dwLeds_l++;
+						if (dwLeds_l >=
+						    APP_DOUBLE_LED_MASK) {
+							iToggle = 0;
+						}
+					}
+				} else {
+					dwLeds_l <<= 1;
+					if ((dwLeds_l & APP_DOUBLE_LED_MASK) ==
+					    0x00) {
+						iToggle = 1;
+					}
+				}
+				bLedsRow1_l =
+				    (unsigned char)(dwLeds_l & APP_LED_MASK);
+				bLedsRow2_l =
+				    (unsigned char)((dwLeds_l >> APP_LED_COUNT)
+						    & APP_LED_MASK);
+			}
+
+			else if ((dwMode_l & 0x02) != 0) {	// running light forward
+				dwLeds_l <<= 1;
+				if ((dwLeds_l > APP_DOUBLE_LED_MASK)
+				    || (dwLeds_l == 0x00000000L)) {
+					dwLeds_l = 0x01;
+				}
+				bLedsRow1_l =
+				    (unsigned char)(dwLeds_l & APP_LED_MASK);
+				bLedsRow2_l =
+				    (unsigned char)((dwLeds_l >> APP_LED_COUNT)
+						    & APP_LED_MASK);
+			}
+
+			else if ((dwMode_l & 0x04) != 0) {	// running light backward
+				dwLeds_l >>= 1;
+				if ((dwLeds_l > APP_DOUBLE_LED_MASK)
+				    || (dwLeds_l == 0x00000000L)) {
+					dwLeds_l = 1 << (APP_LED_COUNT * 2);
+				}
+				bLedsRow1_l =
+				    (unsigned char)(dwLeds_l & APP_LED_MASK);
+				bLedsRow2_l =
+				    (unsigned char)((dwLeds_l >> APP_LED_COUNT)
+						    & APP_LED_MASK);
+			}
+
+			else if ((dwMode_l & 0x08) != 0) {	// Knightrider
+				if (bLedsRow1_l == 0x00) {
+					bLedsRow1_l = 0x01;
+					iToggle = 1;
+				} else if (iToggle) {
+					bLedsRow1_l <<= 1;
+					if (bLedsRow1_l >=
+					    (1 << (APP_LED_COUNT - 1))) {
+						iToggle = 0;
+					}
+				} else {
+					bLedsRow1_l >>= 1;
+					if (bLedsRow1_l <= 0x01) {
+						iToggle = 1;
+					}
+				}
+				bLedsRow2_l = bLedsRow1_l;
+			}
+
+			else if ((dwMode_l & 0x10) != 0) {	// Knightrider
+				if ((bLedsRow1_l == 0x00)
+				    || (bLedsRow2_l == 0x00)
+				    || ((bLedsRow2_l & ~APP_LED_MASK) != 0)) {
+					bLedsRow1_l = 0x01;
+					bLedsRow2_l =
+					    (1 << (APP_LED_COUNT - 1));
+					iToggle = 1;
+				} else if (iToggle) {
+					bLedsRow1_l <<= 1;
+					bLedsRow2_l >>= 1;
+					if (bLedsRow1_l >=
+					    (1 << (APP_LED_COUNT - 1))) {
+						iToggle = 0;
+					}
+				} else {
+					bLedsRow1_l >>= 1;
+					bLedsRow2_l <<= 1;
+					if (bLedsRow1_l <= 0x01) {
+						iToggle = 1;
+					}
+				}
+			}
+			// set own output
+			bVarOut1_l = bLedsRow1_l;
+//            bVarOut1_l = (bLedsRow1_l & 0x03) | (bLedsRow2_l << 2);
+
+			// restart cycle counter
+			iCurCycleCount_l = iMaxCycleCount_l;
+		}
+
+		if (bSpeedSelectOld_l == 0) {
+			if ((bSpeedSelect_l & 0x01) != 0) {
+				if (iMaxCycleCount_l < 200) {
+					iMaxCycleCount_l++;
+				}
+				bSpeedSelectOld_l = bSpeedSelect_l;
+			} else if ((bSpeedSelect_l & 0x02) != 0) {
+				if (iMaxCycleCount_l > 1) {
+					iMaxCycleCount_l--;
+				}
+				bSpeedSelectOld_l = bSpeedSelect_l;
+			} else if ((bSpeedSelect_l & 0x04) != 0) {
+				iMaxCycleCount_l = DEFAULT_MAX_CYCLE_COUNT;
+				bSpeedSelectOld_l = bSpeedSelect_l;
+			}
+		} else if (bSpeedSelect_l == 0) {
+			bSpeedSelectOld_l = 0;
+		}
+	}
+
+	TGT_DBG_SIGNAL_TRACE_POINT(1);
+
+	return EplRet;
+}
+
+// EOF
diff --git a/drivers/staging/epl/edrv.h b/drivers/staging/epl/edrv.h
new file mode 100644
index 00000000000..a45984dfb09
--- /dev/null
+++ b/drivers/staging/epl/edrv.h
@@ -0,0 +1,167 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  interface for ethernetdriver
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: edrv.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.6 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                Dev C++ and GNU-Compiler for m68k
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2005/08/01 m.b.:   start of implementation
+
+****************************************************************************/
+
+#ifndef _EDRV_H_
+#define _EDRV_H_
+
+#include "EplInc.h"
+#include "EplFrame.h"
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+// --------------------------------------------------------------------------
+#define MAX_ETH_DATA_SIZE       1500
+#define MIN_ETH_DATA_SIZE         46
+
+#define ETH_HDR_OFFSET 	 0	// Ethernet header at the top of the frame
+#define ETH_HDR_SIZE	14	// size of Ethernet header
+#define MIN_ETH_SIZE     (MIN_ETH_DATA_SIZE + ETH_HDR_SIZE)	// without CRC
+
+#define ETH_CRC_SIZE	 4	// size of Ethernet CRC, i.e. FCS
+
+//---------------------------------------------------------------------------
+// types
+//---------------------------------------------------------------------------
+
+// position of a buffer in an ethernet-frame
+typedef enum {
+	kEdrvBufferFirstInFrame = 0x01,	// first data buffer in an ethernet frame
+	kEdrvBufferMiddleInFrame = 0x02,	// a middle data buffer in an ethernet frame
+	kEdrvBufferLastInFrame = 0x04	// last data buffer in an ethernet frame
+} tEdrvBufferInFrame;
+
+// format of a tx-buffer
+typedef struct _tEdrvTxBuffer {
+	tEplMsgType m_EplMsgType;	// IN: type of EPL message, set by calling function
+	unsigned int m_uiTxMsgLen;	// IN: length of message to be send (set for each transmit call)
+	// ----------------------
+	unsigned int m_uiBufferNumber;	// OUT: number of the buffer, set by ethernetdriver
+	BYTE *m_pbBuffer;	// OUT: pointer to the buffer, set by ethernetdriver
+	tEplNetTime m_NetTime;	// OUT: Timestamp of end of transmission, set by ethernetdriver
+	// ----------------------
+	unsigned int m_uiMaxBufferLen;	// IN/OUT: maximum length of the buffer
+} tEdrvTxBuffer;
+
+// format of a rx-buffer
+typedef struct _tEdrvRxBuffer {
+	tEdrvBufferInFrame m_BufferInFrame;	// OUT position of received buffer in an ethernet-frame
+	unsigned int m_uiRxMsgLen;	// OUT: length of received buffer (without CRC)
+	BYTE *m_pbBuffer;	// OUT: pointer to the buffer, set by ethernetdriver
+	tEplNetTime m_NetTime;	// OUT: Timestamp of end of receiption
+
+} tEdrvRxBuffer;
+
+//typedef void (*tEdrvRxHandler) (BYTE bBufferInFrame_p, tBufferDescr * pbBuffer_p);
+//typedef void (*tEdrvRxHandler) (BYTE bBufferInFrame_p, BYTE * pbEthernetData_p, WORD wDataLen_p);
+typedef void (*tEdrvRxHandler) (tEdrvRxBuffer * pRxBuffer_p);
+typedef void (*tEdrvTxHandler) (tEdrvTxBuffer * pTxBuffer_p);
+
+// format of init structure
+typedef struct {
+	BYTE m_abMyMacAddr[6];	// the own MAC address
+
+//    BYTE            m_bNoOfRxBuffDescr;     // number of entries in rx bufferdescriptor table
+//    tBufferDescr *  m_pRxBuffDescrTable;    // rx bufferdescriptor table
+//    WORD            m_wRxBufferSize;        // size of the whole rx buffer
+
+	tEdrvRxHandler m_pfnRxHandler;
+	tEdrvTxHandler m_pfnTxHandler;
+
+} tEdrvInitParam;
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+tEplKernel EdrvInit(tEdrvInitParam * pEdrvInitParam_p);
+
+tEplKernel EdrvShutdown(void);
+
+tEplKernel EdrvDefineRxMacAddrEntry(BYTE * pbMacAddr_p);
+tEplKernel EdrvUndefineRxMacAddrEntry(BYTE * pbMacAddr_p);
+
+//tEplKernel EdrvDefineUnicastEntry     (BYTE * pbUCEntry_p);
+//tEplKernel EdrvUndfineUnicastEntry    (BYTE * pbUCEntry_p);
+
+tEplKernel EdrvAllocTxMsgBuffer(tEdrvTxBuffer * pBuffer_p);
+tEplKernel EdrvReleaseTxMsgBuffer(tEdrvTxBuffer * pBuffer_p);
+
+//tEplKernel EdrvWriteMsg               (tBufferDescr * pbBuffer_p);
+tEplKernel EdrvSendTxMsg(tEdrvTxBuffer * pBuffer_p);
+tEplKernel EdrvTxMsgReady(tEdrvTxBuffer * pBuffer_p);
+tEplKernel EdrvTxMsgStart(tEdrvTxBuffer * pBuffer_p);
+
+//tEplKernel EdrvReadMsg                (void);
+
+// interrupt handler called by target specific interrupt handler
+void EdrvInterruptHandler(void);
+
+#endif // #ifndef _EDRV_H_
diff --git a/drivers/staging/epl/global.h b/drivers/staging/epl/global.h
new file mode 100644
index 00000000000..fe167165a83
--- /dev/null
+++ b/drivers/staging/epl/global.h
@@ -0,0 +1,1391 @@
+/****************************************************************************
+
+    global project definition file
+
+    12.06.1998   -rs
+    11.02.2002   r.d. Erweiterungen, Ergaenzungen
+    20.08.2002   SYS TEC electronic -as
+                 Definition Schluesselwort 'GENERIC'
+                 fuer das Erzeugen von Generic Pointer
+    28.08.2002   r.d. erweiterter SYS TEC Debug Code
+    16.09.2002   r.d. komplette Uebersetzung in Englisch
+    11.04.2003   f.j. Ergaenzung fuer Mitsubishi NC30 Compiler
+    17.06.2003   -rs  Definition von Basistypen in <#ifndef _WINDEF_> gesetzt
+    16.04.2004   r.d. Ergaenzung fuer Borland C++ Builder
+    30.08.2004   -rs  TRACE5 eingefügt
+    23.12.2005   d.k. Definitions for IAR compiler
+
+    $Id: global.h,v 1.6 2008/11/07 13:55:56 D.Krueger Exp $
+
+****************************************************************************/
+
+#ifndef _GLOBAL_H_
+#define _GLOBAL_H_
+
+//---------------------------------------------------------------------------
+//  elements of defines for development system
+//---------------------------------------------------------------------------
+
+// these defines are necessary to check some of characteristics of the development system
+#define _DEV_BIGEND_            0x80000000L	// big endian (motorolla format)
+#define _DEV_ALIGNMENT_4_       0x00400000L	//                  the CPU needs alignment of 4 bytes
+#define _DEV_ONLY_INT_MAIN_     0x00004000L	//                  the compiler needs "int main(int)" instead of "void main(void)"
+#define _DEV_COMMA_EXT_         0x00002000L	//                  support of last comma in struct predefinition
+#define _DEV_64BIT_SUPPORT_     0x00001000L	//                  support of 64 bit operations
+#define _DEV_BIT64_             0x00000400L	// count of bits:   64 bit
+#define _DEV_BIT32_             0x00000300L	//                  32 bit
+#define _DEV_BIT16_             0x00000200L	//                  16 bit
+#define _DEV_BIT8_              0x00000100L	//                  8 bit
+#define _DEV_RVCT_ARM_          0x0000001CL	//                  RealView ARM
+#define _DEV_RENESASM32C        0x0000001BL	// compiler from:   Renesas
+#define _DEV_GNUC_MIPS2_        0x0000001AL	//                  GNU for MIPS2
+#define _DEV_MPLAB_C30_         0x00000019L	//                  MPLAB C30 for Microchip dsPIC33F series
+#define _DEV_GNUC_TC_           0x00000018L	//                  GNU for Infineon TriCore
+#define _DEV_GNUC_X86_          0x00000017L	//                  GNU for I386
+#define _DEV_IAR_ARM_           0x00000016L	//                  ARM IAR C/C++ Compiler
+#define _DEV_PARADGM_X86        0x00000015L	//                  Paradigm C/C++ for Beck 1x3
+#define _DEV_GNUC_CF_           0x00000014L	//                  GNU for Coldfire
+#define _DEV_KEIL_ARM_          0x00000013L	//                  Keil ARM
+#define _DEV_MSEVC_             0x00000012L	//                  Microsoft embedded Visual C/C++
+#define _DEV_HIGHTEC_GNUC_X86_  0x00000011L	//                  Hightec elf386 gcc
+#define _DEV_MSVC_RTX_          0x00000010L	//                  VC600 + RTX
+#define _DEV_MSVC_V1_5_         0x0000000FL	//                  Microsoft Visual C/C++ V1.5
+#define _DEV_GNUC_ARM7_         0x0000000EL	//                  GNU Compiler gcc for ARM7
+#define _DEV_METROWERKS_CW_     0x0000000DL	//                  Metrowerks Code Warrior
+#define _DEV_MITSUBISHIM16C_    0x0000000CL	//compiler from:    Mitsubishi
+#define _DEV_GNUC_C16X_         0x0000000BL	//                  GNU Compiler gcc166 for Infineon C16x
+#define _DEV_LINUX_GCC_         0x0000000AL	//                  Linux GNU Compiler gcc
+#define _DEV_GNUC_MPC5X5        0x00000009L	//                  GNU for Motorola PPC5x5
+#define _DEV_TASKINGM16C_       0x00000008L	//                  Tasking for Mitsubishi M16C
+#define _DEV_FUJITSU_           0x00000007L	//                  Fujitsu
+#define _DEV_TASKING8_          0x00000006L	//                  Tasking 8051
+#define _DEV_TASKING16_         0x00000005L	//                  Tasking 166
+#define _DEV_KEIL8_             0x00000004L	//                  Keil C51
+#define _DEV_KEIL16_            0x00000003L	//                  Keil C166
+#define _DEV_BORLANDC_          0x00000002L	//                  Borland C/C++
+#define _DEV_MSVC16_            0x00000001L	//                  Microsoft Visual C/C++
+#define _DEV_MSVC32_            0x00000000L	//                  Microsoft Visual C/C++
+
+// these defines can be used to mask previous elements
+#define _DEV_MASK_COMPILER      0x000000FFL
+#define _DEV_MASK_BITCOUNT      0x00000F00L
+#define _DEV_MASK_ADDSUPPORT    0x0000F000L
+#define _DEV_MASK_ALIGNMENT     0x00F00000L
+
+//---------------------------------------------------------------------------
+//  defines for development system (DEV_SYSTEM) including previous elements
+//---------------------------------------------------------------------------
+
+#define _DEV_WIN16_             (_DEV_BIT16_ | _DEV_MSVC16_                  )
+#define _DEV_WIN32_             (_DEV_BIT32_ | _DEV_MSVC32_                   | _DEV_64BIT_SUPPORT_ | _DEV_COMMA_EXT_)
+#define _DEV_MSVC_DOS_          (_DEV_BIT32_ | _DEV_MSVC_V1_5_               )
+#define _DEV_BORLAND_DOS_       (_DEV_BIT32_ | _DEV_BORLANDC_                )	//| _DEV_64BIT_SUPPORT_ | _DEV_COMMA_EXT_)
+#define _DEV_KEIL_C51X_         (_DEV_BIT8_  | _DEV_KEIL8_     | _DEV_BIGEND_ | _DEV_COMMA_EXT_)	// at least C51 version 7.05 supports comma extension
+#define _DEV_KEIL_C16X_         (_DEV_BIT16_ | _DEV_KEIL16_                   | _DEV_COMMA_EXT_)	// at least C166 version 5.03 supports comma extension
+#define _DEV_TASKING_C51X_      (_DEV_BIT8_  | _DEV_TASKING8_  | _DEV_BIGEND_)
+#define _DEV_TASKING_C16X_      (_DEV_BIT16_ | _DEV_TASKING16_               )
+#define _DEV_FUJITSU_F590_      (_DEV_BIT8_  | _DEV_FUJITSU_                  | _DEV_COMMA_EXT_)	// softune is not able to support 64 bit variables QWORD !!!
+//f.j.29.04.03 M16C kann effektiv mit Bytes umgehen
+//#define _DEV_TASKING_M16C_      (_DEV_BIT16_ | _DEV_TASKINGM16C_             )
+#define _DEV_TASKING_M16C_      (_DEV_BIT8_  | _DEV_TASKINGM16C_             )
+#define _DEV_MITSUBISHI_M16C_   (_DEV_BIT8_  | _DEV_MITSUBISHIM16C_          )
+#define _DEV_GNU_MPC5X5_        (_DEV_BIT32_ | _DEV_GNUC_MPC5X5| _DEV_BIGEND_ | _DEV_64BIT_SUPPORT_ | _DEV_COMMA_EXT_)
+#define _DEV_LINUX_             (_DEV_BIT32_ | _DEV_LINUX_GCC_                | _DEV_64BIT_SUPPORT_ | _DEV_COMMA_EXT_)
+#define _DEV_GNU_C16X_          (_DEV_BIT16_ | _DEV_GNUC_C16X_               )	//| _DEV_COMMA_EXT_)
+#define _DEV_MCW_MPC5X5_        (_DEV_BIT32_ | _DEV_METROWERKS_CW_           )	//| _DEV_64BIT_SUPPORT_ | _DEV_COMMA_EXT_)
+#define _DEV_GNU_ARM7_          (_DEV_BIT32_ | _DEV_GNUC_ARM7_                | _DEV_64BIT_SUPPORT_ | _DEV_COMMA_EXT_ | _DEV_ONLY_INT_MAIN_)
+#define _DEV_WIN32_RTX_         (_DEV_BIT32_ | _DEV_MSVC_RTX_                )	//| _DEV_64BIT_SUPPORT_ | _DEV_COMMA_EXT_)
+#define _DEV_HIGHTEC_X86_       (_DEV_BIT32_ | _DEV_HIGHTEC_GNUC_X86_        )	//| _DEV_64BIT_SUPPORT_ | _DEV_COMMA_EXT_)
+#define _DEV_WIN_CE_            (_DEV_BIT32_ | _DEV_MSEVC_                   )	//| _DEV_64BIT_SUPPORT_ | _DEV_COMMA_EXT_)
+#define _DEV_KEIL_CARM_         (_DEV_BIT32_ | _DEV_KEIL_ARM_                 | _DEV_64BIT_SUPPORT_ | _DEV_COMMA_EXT_)
+#define _DEV_IAR_CARM_          (_DEV_BIT32_ | _DEV_IAR_ARM_                  | _DEV_64BIT_SUPPORT_ | _DEV_COMMA_EXT_)
+#define _DEV_RVCT_CARM_         (_DEV_BIT32_ | _DEV_RVCT_ARM_                 | _DEV_64BIT_SUPPORT_ | _DEV_COMMA_EXT_ | _DEV_ONLY_INT_MAIN_)
+#define _DEV_MCW_MCF5XXX_       (_DEV_BIT32_ | _DEV_METROWERKS_CW_           )	//| _DEV_64BIT_SUPPORT_ | _DEV_COMMA_EXT_)
+#define _DEV_GNU_CF5282_        (_DEV_BIT32_ | _DEV_GNUC_CF_   | _DEV_BIGEND_)
+#define _DEV_PAR_BECK1X3_       (_DEV_BIT16_ | _DEV_PARADGM_X86)
+#define _DEV_GNU_CF548X_        (_DEV_BIT32_ | _DEV_GNUC_CF_   | _DEV_BIGEND_ | _DEV_64BIT_SUPPORT_ | _DEV_COMMA_EXT_)
+#define _DEV_GNU_I386_          (_DEV_BIT32_ | _DEV_GNUC_X86_                 | _DEV_64BIT_SUPPORT_ | _DEV_COMMA_EXT_ | _DEV_ONLY_INT_MAIN_)
+#define _DEV_GNU_TRICORE_       (_DEV_BIT32_ | _DEV_GNUC_TC_                  | _DEV_64BIT_SUPPORT_ | _DEV_COMMA_EXT_ | _DEV_ONLY_INT_MAIN_ | _DEV_ALIGNMENT_4_)
+#define _DEV_MPLAB_DSPIC33F_    (_DEV_BIT16_ | _DEV_MPLAB_C30_               )	//| _DEV_COMMA_EXT_)
+#define _DEV_GNU_MIPSEL_        (_DEV_BIT32_ | _DEV_GNUC_MIPS2_     | _DEV_BIGEND_ | _DEV_64BIT_SUPPORT_ | _DEV_COMMA_EXT_ | _DEV_ONLY_INT_MAIN_)
+
+#define _DEV_RENESAS_M32C_      (_DEV_BIT32_ | _DEV_RENESASM32C)
+
+//---------------------------------------------------------------------------
+//  usefull macros
+//---------------------------------------------------------------------------
+
+#define CHECK_IF_ONLY_INT_MAIN()    (DEV_SYSTEM & _DEV_ONLY_INT_MAIN_)
+#define CHECK_MEMORY_ALINMENT()     (DEV_SYSTEM & _DEV_MASK_ALIGNMENT)
+
+//---------------------------------------------------------------------------
+//  defines for target system (TARGET_SYSTEM)
+//---------------------------------------------------------------------------
+
+#define _DOS_              (16 + 0x10000)
+#define _WIN16_             16
+#define _WIN32_             32
+#define _WINCE_            (32 + 0x20000)
+#define _NO_OS_              0
+#define _LINUX_              1
+#define _PXROS_              2
+#define _ECOSPRO_            3
+
+//---------------------------------------------------------------------------
+//  definitions for function inlining
+//---------------------------------------------------------------------------
+
+#define INLINE_FUNCTION		// empty define
+#undef  INLINE_ENABLED		// disable actual inlining of functions
+#undef  INLINE_FUNCTION_DEF	// disable inlining for all compilers per default
+
+//---------------------------------------------------------------------------
+//  definitions for Keil C51
+//---------------------------------------------------------------------------
+
+#ifdef  __C51__
+
+#define TARGET_SYSTEM   _NO_OS_
+#define DEV_SYSTEM      _DEV_KEIL_C51X_
+
+#pragma DEBUG OBJECTEXTEND
+#pragma WARNINGLEVEL(2)		// maximum warning level
+
+#define NEAR            idata	// variables mapped to internal data storage location
+#define FAR             xdata	// variables mapped to external data storage location
+#define CONST           const	// variables mapped to ROM (i.e. flash)
+#define ROM             code	// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+#define HWACC           xdata	// hardware access through external memory (i.e. CAN)
+#define LARGE           large	// functions set parameters to external data storage location
+
+    // These types can be adjusted by users to match application requirements. The goal is to
+    // minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM             xdata	// Memory attribute to optimize speed and code of pointer access.
+
+#define REENTRANT       reentrant
+#define PUBLIC
+
+#ifndef NDEBUG
+#include <stdio.h>		// prototype printf() (for TRACE)
+#define TRACE  printf
+#endif
+
+//---------------------------------------------------------------------------
+//  definitions for GNU Compiler for Infineon C16x
+//  - it have to be befor Keil (it has __C166__ too)
+//---------------------------------------------------------------------------
+#elif  defined (__GNUC__) && defined (__C166__)
+
+#define TARGET_SYSTEM   _NO_OS_
+#define DEV_SYSTEM      _DEV_GNU_C16X_
+
+//    #define NEAR            idata       // variables mapped to internal data storage location
+#define NEAR            near	// variables mapped to internal data storage location
+//    #define FAR             xhuge       // variables mapped to external data storage location
+#define FAR             huge	// variables mapped to external data storage location
+#define CONST           const	// variables mapped to ROM (i.e. flash)
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+//    #define HWACC           sdata       // hardware access through external memory (i.e. CAN)
+#define HWACC           huge	// hardware access through external memory (i.e. CAN)
+#define LARGE			// functions set parameters to external data storage location
+
+    // These types can be adjusted by users to match application requirements. The goal is to
+    // minimize code memory and maximize speed.
+//    #define GENERIC         xhuge       // generic pointer to point to application data
+#define GENERIC         huge	// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+
+#define REENTRANT
+#define PUBLIC
+
+#ifndef NDEBUG
+#include <stdio.h>		// prototype printf() (for TRACE)
+#define TRACE  printf
+
+#define ASSERT(p)  \
+            if (p)         \
+            {              \
+                ;          \
+            }              \
+            else           \
+            {              \
+                PRINTF0("Assert failed: " #p " (file %s line %d)\n", __FILE__, (int) __LINE__ ); \
+                while (1); \
+            }
+#else
+#define ASSERT(p)
+#endif
+
+//---------------------------------------------------------------------------
+//  definitions for Keil C166
+//---------------------------------------------------------------------------
+#elif  defined (__C166__)	// 24.01.2005 r.d.: Keil ARM7 needs directive 'defined'
+
+#define TARGET_SYSTEM   _NO_OS_
+#define DEV_SYSTEM      _DEV_KEIL_C16X_
+
+#pragma CODE
+#pragma MOD167
+#pragma NOINIT
+#pragma DEBUG
+#pragma WARNINGLEVEL(3)		// maximum warning level
+#pragma WARNING DISABLE = 47	// warning <unreferenced parameter> = OFF
+#pragma WARNING DISABLE = 38	// warning <empty translation unit> = OFF
+//  #pragma WARNING DISABLE = 102       // warning <different const/volatile qualifiers> = OFF
+#pragma WARNING DISABLE = 174	// warning <unreferenced 'static' function> = OFF
+#pragma WARNING DISABLE = 183	// warning <dead assignement eliminated> = OFF
+
+#define NEAR            idata	// variables mapped to internal data storage location
+#define FAR             xhuge	// variables mapped to external data storage location
+#define CONST           const	// variables mapped to ROM (i.e. flash)
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+//    #define HWACC           sdata       // hardware access through external memory (i.e. CAN)
+#define HWACC           huge	// hardware access through external memory (i.e. CAN)
+#define LARGE			// functions set parameters to external data storage location
+
+    // These types can be adjusted by users to match application requirements. The goal is to
+    // minimize code memory and maximize speed.
+#define GENERIC         xhuge	// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+
+#define REENTRANT
+#define PUBLIC
+
+#ifndef NDEBUG
+#include <stdio.h>		// prototype printf() (for TRACE)
+#define TRACE  printf
+#endif
+
+//---------------------------------------------------------------------------
+//  definitions for MPLAB C30 for dsPIC33F series
+//---------------------------------------------------------------------------
+#elif  defined (__C30__)
+
+#define TARGET_SYSTEM   _NO_OS_
+#define DEV_SYSTEM      _DEV_MPLAB_DSPIC33F_
+
+#define NEAR			// variables mapped to internal data storage location
+#define FAR			// variables mapped to external data storage location
+#define CONST        const	// variables mapped to ROM (i.e. flash)
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+#define HWACC			// hardware access through external memory (i.e. CAN)
+#define LARGE			// functions set parameters to external data storage location
+
+    // These types can be adjusted by users to match application requirements. The goal is to
+    // minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+
+#define REENTRANT
+#define PUBLIC
+
+//    #ifndef QWORD
+//        #define QWORD long long
+//    #endif
+
+#ifndef NDEBUG
+#include <stdio.h>		// prototype printf() (for TRACE)
+#define TRACE  printf
+#endif
+
+//---------------------------------------------------------------------------
+//  definitions for Keil ARM
+//---------------------------------------------------------------------------
+#elif  defined (__CA__)
+
+#define TARGET_SYSTEM   _NO_OS_
+#define DEV_SYSTEM      _DEV_KEIL_CARM_
+
+#define NEAR			// variables mapped to internal data storage location
+#define FAR			// variables mapped to external data storage location
+#define CONST        const	// variables mapped to ROM (i.e. flash)
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+#define HWACC			// hardware access through external memory (i.e. CAN)
+#define LARGE			// functions set parameters to external data storage location
+
+    // These types can be adjusted by users to match application requirements. The goal is to
+    // minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+
+#define REENTRANT
+#define PUBLIC
+
+#ifndef QWORD
+#define QWORD long long
+#endif
+
+#ifndef NDEBUG
+#include <stdio.h>		// prototype printf() (for TRACE)
+#define TRACE  printf
+#endif
+
+//---------------------------------------------------------------------------
+//  definitions for RealView ARM compilation tools (provided by recent Keil Microcontroller Development Kits)
+//---------------------------------------------------------------------------
+#elif  defined (__ARMCC_VERSION)
+
+#define TARGET_SYSTEM   _NO_OS_
+#define DEV_SYSTEM      _DEV_RVCT_CARM_
+
+#define NEAR			// variables mapped to internal data storage location
+#define FAR			// variables mapped to external data storage location
+#define CONST        const	// variables mapped to ROM (i.e. flash)
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+#define HWACC			// hardware access through external memory (i.e. CAN)
+#define LARGE			// functions set parameters to external data storage location
+
+    // These types can be adjusted by users to match application requirements. The goal is to
+    // minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+
+#define REENTRANT
+#define PUBLIC
+
+#ifndef QWORD
+#define QWORD long long
+#endif
+
+#ifndef NDEBUG
+#define ASSERT(expr)    if (!(expr)) {\
+                                   TRACE0 ("Assertion failed: " #expr );\
+                                   while (1);}
+#else
+#define ASSERT(expr)
+#endif
+
+#ifndef NDEBUG
+#include <stdio.h>		// prototype printf() (for TRACE)
+#define TRACE  printf
+#endif
+
+//---------------------------------------------------------------------------
+//  definitions for ARM IAR C Compiler
+//---------------------------------------------------------------------------
+#elif  defined (__ICCARM__)
+
+#define TARGET_SYSTEM   _NO_OS_
+#define DEV_SYSTEM      _DEV_IAR_CARM_
+
+#define NEAR			// variables mapped to internal data storage location
+#define FAR			// variables mapped to external data storage location
+#define CONST        const	// variables mapped to ROM (i.e. flash)
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+#define HWACC			// hardware access through external memory (i.e. CAN)
+#define LARGE			// functions set parameters to external data storage location
+
+    // These types can be adjusted by users to match application requirements. The goal is to
+    // minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+
+#define REENTRANT
+#define PUBLIC
+
+#ifndef QWORD
+#define QWORD long long
+#endif
+
+    // Workaround:
+    // If we use IAR and want to debug but don't want to use C-Spy Debugger
+    // assert() doesn't work in debug mode because it needs support for FILE descriptors
+    // (_DLIB_FILE_DESCRIPTOR == 1).
+#ifndef NDEBUG
+#define ASSERT(expr)    if (!(expr)) {\
+                                   TRACE0 ("Assertion failed: " #expr );\
+                                   while (1);}
+#else
+#define ASSERT(expr)
+#endif
+
+#ifndef NDEBUG
+#include <stdio.h>		// prototype printf() (for TRACE)
+#define TRACE  printf
+//        #define TRACE  PRINTF4
+#endif
+
+//---------------------------------------------------------------------------
+//  definitions for Tasking 8051
+//---------------------------------------------------------------------------
+
+#elif defined (_CC51)
+
+#include <cc51.h>
+
+#define TARGET_SYSTEM   _NO_OS_
+#define DEV_SYSTEM      _DEV_TASKING_C51X_
+
+#define NEAR            _data	// variables mapped to internal data storage location
+#define FAR             _xdat	// variables mapped to external data storage location
+#define CONST           const	// variables mapped to ROM (i.e. flash)
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+#define HWACC           _xdat	// hardware access through external memory (i.e. CAN)
+#define LARGE			// functions set parameters to external data storage location
+
+    // These types can be adjusted by users to match application requirements. The goal is to
+    // minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM             _xdat	// Memory attribute to optimize speed and code of pointer access.
+
+#define REENTRANT       _reentrant
+#define PUBLIC
+
+#ifndef NDEBUG
+#include <stdio.h>		// prototype printf() (for TRACE)
+#define TRACE  printf
+#endif
+
+//---------------------------------------------------------------------------
+//  definitions for Tasking C167CR and C164CI
+//---------------------------------------------------------------------------
+
+#elif defined (_C166)
+
+#define TARGET_SYSTEM   _NO_OS_
+#define DEV_SYSTEM      _DEV_TASKING_C16X_
+
+#define NEAR            near	// variables mapped to internal data storage location
+#define FAR             far	// variables mapped to external data storage location
+#define CONST           const	// variables mapped to ROM (i.e. flash)
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+#define HWACC   /* to be defined */	// hardware access through external memory (i.e. CAN)
+#define LARGE			// functions set parameters to external data storage location
+
+    // These types can be adjusted by users to match application requirements. The goal is to
+    // minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+
+#define REENTRANT
+#define PUBLIC
+
+    // Stdio.h has to be alway included here. If printf() is used stdio.h defines NULL
+    // without checking if it is already included. So an error occurs while compiling.
+    // (r.d.)
+#include <stdio.h>		// prototype printf() (for TRACE)
+#ifndef NDEBUG
+#define TRACE  printf
+#endif
+
+//---------------------------------------------------------------------------
+//  definitions for FUJITSU FFMC-16LX MB90590
+//---------------------------------------------------------------------------
+
+//#elif (defined (F590) || defined (F543) || defined (F598) || defined (F495) || defined (F350))
+#elif defined(__COMPILER_FCC907__)
+
+#define TARGET_SYSTEM   _NO_OS_
+#define DEV_SYSTEM      _DEV_FUJITSU_F590_
+
+#define NEAR    /* to be defined */	// variables mapped to internal data storage location
+#define FAR     /* to be defined */	// variables mapped to external data storage location
+#define CONST           const	// variables mapped to ROM (i.e. flash)
+#define ROM     /* to be defined */	// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+#define HWACC   /* to be defined */	// hardware access through external memory (i.e. CAN)
+#define LARGE			// functions set parameters to external data storage location
+
+    // These types can be adjusted by users to match application requirements. The goal is to
+    // minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+
+    // softune is not able to support 64 bit variables QWORD !!!
+
+#define REENTRANT
+#define PUBLIC
+
+#ifndef NDEBUG
+#include <stdio.h>		// prototype printf() (for TRACE)
+#define TRACE  printf
+#endif
+
+//---------------------------------------------------------------------------
+//  definitions for Mitsubishi M16C family for TASKING Compiler CM16
+//---------------------------------------------------------------------------
+
+#elif defined (_CM16C)
+
+#define TARGET_SYSTEM   _NO_OS_
+#define DEV_SYSTEM      _DEV_TASKING_M16C_
+
+#define NEAR            _near	// variables mapped to internal data storage location
+#define FAR             _far	// variables mapped to external data storage location
+#define CONST           _farrom	// variables mapped to ROM (i.e. flash)
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+#define HWACC           _near	// hardware access through external memory (i.e. CAN)
+#define LARGE			// functions set parameters to external data storage location
+
+    // These types can be adjusted by users to match application requirements. The goal is to
+    // minimize code memory and maximize speed.
+#define GENERIC         _far	// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+					// do you use memory model SMALL, than you have to set _far
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+
+#define REENTRANT
+#define PUBLIC
+
+    // Stdio.h has to be alway included here. If printf() is used stdio.h defines NULL
+    // without checking if it is already included. So an error occurs while compiling.
+    // (r.d.)
+#include <stdio.h>		// prototype printf() (for TRACE)
+#ifndef NDEBUG
+#define TRACE  printf
+#endif
+
+//---------------------------------------------------------------------------
+//  definitions for Mitsubishi M16C family for Mitsubishi Compiler NC30
+//---------------------------------------------------------------------------
+// name NC30, andere Form will der Compiler nicht !!
+#elif defined (NC30)
+
+#define TARGET_SYSTEM   _NO_OS_
+#define DEV_SYSTEM      _DEV_MITSUBISHI_M16C_
+
+#define NEAR            near	// variables mapped to internal data storage location
+#define FAR             far	// variables mapped to external data storage location
+#define CONST           const	// variables mapped to ROM (i.e. flash)
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+#define HWACC           near	// hardware access through external memory (i.e. CAN)
+#define LARGE			// functions set parameters to external data storage location
+
+    // These types can be adjusted by users to match application requirements. The goal is to
+    // minimize code memory and maximize speed.
+#define GENERIC         far	// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+
+#define REENTRANT
+#define PUBLIC
+
+#ifndef NDEBUG
+#include <stdio.h>		// prototype printf() (for TRACE)
+#define TRACE  printf
+#endif
+
+//---------------------------------------------------------------------------
+//  definitions for Renesas M32C family for Renesas Compiler
+//---------------------------------------------------------------------------
+#elif defined (NC308)
+
+#define TARGET_SYSTEM   _NO_OS_
+#define DEV_SYSTEM      _DEV_RENESAS_M32C_
+
+#define NEAR             near	// variables mapped to internal data storage location
+#define FAR              far	// variables mapped to external data storage location
+#define CONST            const	// variables mapped to ROM (i.e. flash)
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+#define HWACC			// hardware access through external memory (i.e. CAN)
+#define LARGE			// functions set parameters to external data storage location
+
+    // These types can be adjusted by users to match application requirements. The goal is to
+    // minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM              far	// Memory attribute to optimize speed and code of pointer access.
+
+#define REENTRANT
+#define PUBLIC
+
+#ifndef NDEBUG
+#include <stdio.h>		// prototype printf() (for TRACE)
+#define TRACE  printf
+#endif
+
+//    #error ("RENESAS o.k.")
+
+//---------------------------------------------------------------------------
+//  definitions for ARM7 family with GNU compiler
+//---------------------------------------------------------------------------
+
+#elif defined(__GNUC__) && defined(__arm__) && !defined(__LINUX_ARM_ARCH__)
+
+#define TARGET_SYSTEM   _NO_OS_
+#define DEV_SYSTEM      _DEV_GNU_ARM7_
+
+#define NEAR			// variables mapped to internal data storage location
+#define FAR			// variables mapped to external data storage location
+#define CONST           const	// variables mapped to ROM (i.e. flash)
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+#define HWACC			// hardware access through external memory (i.e. CAN)
+#define LARGE			// functions set parameters to external data storage location
+
+    // These types can be adjusted by users to match application requirements. The goal is to
+    // minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+#define HWACC			// hardware access through external memory (i.e. CAN)
+
+#define REENTRANT
+#define PUBLIC
+
+#ifndef QWORD
+#define QWORD long long		// i.A. durch Herr Kuschel
+#endif
+
+#ifndef NDEBUG
+#include <stdio.h>		// prototype printf() (for TRACE)
+#define TRACE  printf
+#endif
+
+//---------------------------------------------------------------------------
+//  definitions for Motorola PowerPC family 5x5 (555/565)
+//  definitions Linux-PC
+//---------------------------------------------------------------------------
+
+#elif defined (__GNUC__)
+
+#if defined (LINUX) || defined (linux) || defined (__linux__)
+#define LINUX_SYSTEM		// define 'LINUX_SYSTEM' uniform for all Linux based systems
+	// r.d.: We will need an other solution here! There are two sections here which do check the preproc-definitions:
+	//     LINUX and __linux__ . The first one was Linux for PC, the second one is this section for embedded Linux (MCF5xxx).
+	//     But Linux for PC does not need the definitions for embedded Linux.
+#endif
+
+    // GNU C compiler supports function inlining
+#define INLINE_FUNCTION_DEF extern inline
+
+    // to actually enable inlining just include the following three lines
+    // #undef INLINE_FUNCTION
+    // #define INLINE_FUNCTION     INLINE_FUNCTION_DEF
+    // #define INLINE_ENABLED      TRUE
+
+#ifdef PXROS
+#define TARGET_SYSTEM       _PXROS_
+#ifdef __i386__
+#undef LINUX			// this define seems to be set from compiler
+#define DEV_SYSTEM      _DEV_HIGHTEC_X86_
+#elif defined (__tricore__)
+#define DEV_SYSTEM      _DEV_GNU_TRICORE_
+#else // MPC5x5
+#define DEV_SYSTEM      _DEV_GNU_MPC5X5_
+#endif
+
+#elif defined (LINUX) || defined (__linux__)
+#define TARGET_SYSTEM       _LINUX_	// Linux definition
+#define DEV_SYSTEM          _DEV_LINUX_
+
+#elif defined (GNU_CF5282)
+#define TARGET_SYSTEM       _NO_OS_
+#define DEV_SYSTEM          _DEV_GNU_CF5282_
+
+#elif defined (ECOSPRO_I386_PEAK_PCI)
+#define TARGET_SYSTEM       _ECOSPRO_
+#define DEV_SYSTEM          _DEV_GNU_I386_
+
+#elif defined (GNU_CF548X)
+#define TARGET_SYSTEM       _NO_OS_
+#define DEV_SYSTEM          _DEV_GNU_CF548X_
+#else
+#error 'ERROR: DEV_SYSTEM not found!'
+#endif
+
+#ifndef QWORD
+#define QWORD long long int
+#endif
+
+#if (TARGET_SYSTEM == _PXROS_)
+
+#ifndef __KERNEL__
+#include <string.h>
+#endif
+
+#define NEAR			// variables mapped to internal data storage location
+#define FAR			// variables mapped to external data storage location
+#define CONST           const	// variables mapped to ROM (i.e. flash)
+#define ROM     /* to be defined */	// code or variables mapped to ROM (i.e. flash)
+					    // usage: CONST BYTE ROM foo = 0x00;
+#define LARGE			// functions set parameters to external data storage location
+
+	// These types can be adjusted by users to match application requirements. The goal is to
+	// minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+					    // Variables with this attribute can be located in external
+					    // or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+
+#define HWACC			// hardware access through external memory (i.e. CAN)
+
+#define REENTRANT
+#define PUBLIC
+
+#ifndef QWORD
+#define QWORD long long int
+#endif
+
+#ifndef NDEBUG
+#include <stdio.h>		// prototype printf() (for TRACE)
+#define TRACE  printf
+#endif
+
+#endif
+
+    // ------------------ GNUC for I386 ---------------------------------------------
+
+#if (TARGET_SYSTEM == _LINUX_) || (TARGET_SYSTEM == _ECOSPRO_)
+
+#ifndef __KERNEL__
+#include <string.h>
+#endif
+
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+#define HWACC			// hardware access through external memory (i.e. CAN)
+
+	// These types can be adjusted by users to match application requirements. The goal is to
+	// minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+
+#ifndef NEAR
+#define NEAR			// variables mapped to internal data storage location
+#endif
+
+#ifndef FAR
+#define FAR			// variables mapped to external data storage location
+#endif
+
+#ifndef CONST
+#define CONST const		// variables mapped to ROM (i.e. flash)
+#endif
+
+#define LARGE
+
+#define REENTRANT
+#define PUBLIC
+
+#ifndef NDEBUG
+#ifndef __KERNEL__
+#include <stdio.h>		// prototype printf() (for TRACE)
+#define TRACE  printf
+#else
+#define TRACE  printk
+#endif
+#endif
+#endif
+
+    // ------------------ GNU without OS ---------------------------------------------
+
+#if (TARGET_SYSTEM == _NO_OS_)
+
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+#define HWACC			// hardware access through external memory (i.e. CAN)
+
+	// These types can be adjusted by users to match application requirements. The goal is to
+	// minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+
+#ifndef NEAR
+#define NEAR			// variables mapped to internal data storage location
+#endif
+
+#ifndef FAR
+#define FAR			// variables mapped to external data storage location
+#endif
+
+#ifndef CONST
+#define CONST const		// variables mapped to ROM (i.e. flash)
+#endif
+
+#define LARGE
+
+#define REENTRANT
+#define PUBLIC
+
+#ifndef NDEBUG
+//            #include "xuartdrv.h"
+//            #include <stdio.h>              // prototype printf() (for TRACE)
+#define TRACE  printf
+//            #define TRACE  mprintf
+//            #ifndef TRACE
+//                #define TRACE trace
+//                void trace (char *fmt, ...);
+//            #endif
+#endif
+
+#endif
+
+//---------------------------------------------------------------------------
+//  definitions for MPC565
+//---------------------------------------------------------------------------
+#elif __MWERKS__
+
+#ifdef __MC68K__
+
+#define TARGET_SYSTEM = _MCF548X_
+#define DEV_SYSTEM      _DEV_MCW_MCF5XXX_
+
+#else
+#define TARGET_SYSTEM = _MPC565_
+#define DEV_SYSTEM      _DEV_MCW_MPC5X5_
+#endif
+
+#define NEAR			// variables mapped to internal data storage location
+#define FAR			// variables mapped to external data storage location
+#define CONST           const	// variables mapped to ROM (i.e. flash)
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+#define LARGE			// functions set parameters to external data storage location
+
+    // These types can be adjusted by users to match application requirements. The goal is to
+    // minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+
+#define HWACC			// hardware access through external memory (i.e. CAN)
+
+#define REENTRANT
+#define PUBLIC
+
+#ifndef NDEBUG
+#include <stdio.h>		// prototype printf() (for TRACE)
+#define TRACE  printf
+#endif
+
+//---------------------------------------------------------------------------
+//  definitions for BECK 1x3
+//---------------------------------------------------------------------------
+#elif defined (__BORLANDC__) && defined (__PARADIGM__)
+
+#define TARGET_SYSTEM      _NO_OS_
+#define DEV_SYSTEM         _DEV_PAR_BECK1X3_
+
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+				     // usage: CONST BYTE ROM foo = 0x00;
+#define HWACC			// hardware access through external memory (i.e. CAN)
+
+     // These types can be adjusted by users to match application requirements. The goal is to
+     // minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+				     // Variables with this attribute can be located in external
+				     // or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+#define NEAR __near		// variables mapped to internal data storage location
+#define FAR  __far		// variables mapped to external data storage location
+#define CONST const		// variables mapped to ROM (i.e. flash)
+#define LARGE
+
+#define REENTRANT
+#define PUBLIC
+
+#ifndef NDEBUG
+#ifndef TRACE
+#include <stdio.h>
+#define TRACE printf
+#endif
+#endif
+
+//---------------------------------------------------------------------------
+//  definitions for PC
+//---------------------------------------------------------------------------
+
+#elif defined (__BORLANDC__)
+
+    // ------------------ definition target system --------------------------
+
+#ifdef _WIN32
+#define TARGET_SYSTEM   _WIN32_	// WIN32 definition
+#define DEV_SYSTEM      _DEV_WIN32_
+#else
+#define TARGET_SYSTEM   _DOS_
+#define DEV_SYSTEM      _DEV_BORLAND_DOS_
+#endif
+
+    // ------------------ WIN32 ---------------------------------------------
+
+#if (TARGET_SYSTEM == _WIN32_)
+
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+#define HWACC			// hardware access through external memory (i.e. CAN)
+
+	// These types can be adjusted by users to match application requirements. The goal is to
+	// minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+
+#ifndef NEAR
+#define NEAR			// variables mapped to internal data storage location
+#endif
+
+#ifndef FAR
+#define FAR			// variables mapped to external data storage location
+#endif
+
+#ifndef CONST
+#define CONST const		// variables mapped to ROM (i.e. flash)
+#endif
+
+#define LARGE
+
+#define REENTRANT
+#define PUBLIC __stdcall
+
+#ifndef NDEBUG
+#ifndef TRACE
+#include <stdio.h>
+#define TRACE printf
+#endif
+#endif
+
+#elif (TARGET_SYSTEM == _DOS_)
+
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+#define HWACC			// hardware access through external memory (i.e. CAN)
+
+	// These types can be adjusted by users to match application requirements. The goal is to
+	// minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+#define NEAR near		// variables mapped to internal data storage location
+#define FAR  far		// variables mapped to external data storage location
+#define CONST const		// variables mapped to ROM (i.e. flash)
+#define LARGE
+
+#define REENTRANT
+#define PUBLIC
+
+#ifndef NDEBUG
+#ifndef TRACE
+#include <stdio.h>
+#define TRACE printf
+#endif
+#endif
+
+#endif
+
+#elif (_MSC_VER == 800)		// PC MS Visual C/C++ for DOS applications
+
+#define TARGET_SYSTEM   _DOS_
+#define DEV_SYSTEM      _DEV_MSVC_DOS_
+
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+				    // usage: CONST BYTE ROM foo = 0x00;
+#define HWACC near		// hardware access through external memory (i.e. CAN)
+
+    // These types can be adjusted by users to match application requirements. The goal is to
+    // minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+				    // Variables with this attribute can be located in external
+				    // or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+#define NEAR near		// variables mapped to internal data storage location
+#define FAR  far		// variables mapped to external data storage location
+#define CONST const		// variables mapped to ROM (i.e. flash)
+#define LARGE
+
+#define REENTRANT
+#define PUBLIC
+
+#ifndef NDEBUG
+#ifndef TRACE
+#include <stdio.h>
+#define TRACE printf
+#endif
+#endif
+
+//---------------------------------------------------------------------------
+// definitions for RTX under WIN32
+//---------------------------------------------------------------------------
+#elif (defined (UNDER_RTSS) && defined (WIN32))
+
+    // ------------------ definition target system --------------------------
+#define TARGET_SYSTEM   _WIN32_RTX_
+#define DEV_SYSTEM      _DEV_WIN32_RTX_
+
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+				    // usage: CONST BYTE ROM foo = 0x00;
+#define HWACC			// hardware access through external memory (i.e. CAN)
+
+    // These types can be adjusted by users to match application requirements. The goal is to
+    // minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+				    // Variables with this attribute can be located in external
+				    // or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+
+#ifndef NEAR
+#define NEAR			// variables mapped to internal data storage location
+#endif
+
+#ifndef FAR
+#define FAR			// variables mapped to external data storage location
+#endif
+
+#ifndef CONST
+#define CONST const		// variables mapped to ROM (i.e. flash)
+#endif
+
+#define LARGE
+
+#define REENTRANT
+#define PUBLIC __stdcall
+
+#ifndef NDEBUG
+#ifndef TRACE
+#define TRACE RtPrintf
+#endif
+#endif
+
+//---------------------------------------------------------------------------
+// definitions for WinCE
+//---------------------------------------------------------------------------
+#elif defined (_WIN32_WCE)
+
+    // ------------------ definition target system --------------------------
+#define TARGET_SYSTEM           _WINCE_
+#define DEV_SYSTEM              _DEV_WIN_CE_
+
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+				    // usage: CONST BYTE ROM foo = 0x00;
+#define HWACC			// hardware access through external memory (i.e. CAN)
+
+    // These types can be adjusted by users to match application requirements. The goal is to
+    // minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+				    // Variables with this attribute can be located in external
+				    // or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+
+#ifndef NEAR
+#define NEAR			// variables mapped to internal data storage location
+#endif
+
+#ifndef FAR
+#define FAR			// variables mapped to external data storage location
+#endif
+
+#ifndef CONST
+#define CONST const		// variables mapped to ROM (i.e. flash)
+#endif
+
+#define LARGE
+
+#ifndef QWORD
+      //#define QWORD long long int // MSVC .NET can use "long long int" too (like GNU)
+#define QWORD __int64
+#endif
+
+#define REENTRANT
+#define PUBLIC __cdecl
+
+#ifdef ASSERTMSG
+#undef ASSERTMSG
+#endif
+
+#ifndef NDEBUG
+#ifndef TRACE
+#define TRACE printf
+//            void trace (char *fmt, ...);
+#endif
+#endif
+
+#else // ===> PC MS Visual C/C++
+
+    // ------------------ definition target system --------------------------
+
+#ifdef _WIN32
+#define TARGET_SYSTEM   _WIN32_	// WIN32 definition
+#define DEV_SYSTEM      _DEV_WIN32_
+#else
+#define TARGET_SYSTEM   _WIN16_	// WIN16 definition
+#define DEV_SYSTEM      _DEV_WIN16_
+#endif
+
+    // ------------------ WIN16 ---------------------------------------------
+
+#if (TARGET_SYSTEM == _WIN16_)
+
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+#define HWACC			// hardware access through external memory (i.e. CAN)
+
+	// These types can be adjusted by users to match application requirements. The goal is to
+	// minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+					// Variables with this attribute can be located in external
+					// or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+
+#ifndef NEAR
+#define NEAR			// variables mapped to internal data storage location
+#endif
+
+#ifndef FAR
+#define FAR far			// variables mapped to external data storage location
+#endif
+
+#ifndef CONST
+#define CONST const		// variables mapped to ROM (i.e. flash)
+#endif
+
+#define LARGE
+
+#define REENTRANT
+#define PUBLIC _far _pascal _export
+
+#ifndef NDEBUG
+#ifndef TRACE
+#define TRACE trace
+#ifdef __cplusplus
+extern "C" {
+#endif
+	void trace(const char *fmt, ...);
+#ifdef __cplusplus
+}
+#endif
+#endif
+#endif
+#endif
+    // ------------------ WIN32 ---------------------------------------------
+#if (TARGET_SYSTEM == _WIN32_)
+#define ROM			// code or variables mapped to ROM (i.e. flash)
+					// usage: CONST BYTE ROM foo = 0x00;
+#define HWACC			// hardware access through external memory (i.e. CAN)
+	// These types can be adjusted by users to match application requirements. The goal is to// minimize code memory and maximize speed.
+#define GENERIC			// generic pointer to point to application data
+					// Variables with this attribute can be located in external// or internal data memory.
+#define MEM			// Memory attribute to optimize speed and code of pointer access.
+#ifndef NEAR
+#define NEAR			// variables mapped to internal data storage location
+#endif
+#ifndef FAR
+#define FAR			// variables mapped to external data storage location
+#endif
+#ifndef CONST
+#define CONST const		// variables mapped to ROM (i.e. flash)
+#endif
+#define LARGE
+#define REENTRANT
+#define PUBLIC __stdcall
+#ifndef QWORD
+	  //#define QWORD long long int // MSVC .NET can use "long long int" too (like GNU)
+#define QWORD __int64
+#endif
+#ifndef NDEBUG
+#ifndef TRACE
+#define TRACE trace
+#ifdef __cplusplus
+extern "C" {
+#endif
+	void trace(const char *fmt, ...);
+#ifdef __cplusplus
+}
+#endif
+#endif
+#endif
+	// MS Visual C++ compiler supports function inlining
+#define INLINE_FUNCTION_DEF __forceinline
+	// to actually enable inlining just include the following two lines// #define INLINE_FUNCTION     INLINE_FUNCTION_DEF// #define INLINE_ENABLED      TRUE
+#endif
+#endif				// ===> PC
+//---------------------------------------------------------------------------//  definitions of basic types//---------------------------------------------------------------------------
+#ifndef _WINDEF_		// defined in WINDEF.H, included by <windows.h>
+    // --- arithmetic types ---
+#ifndef SHORT
+#define SHORT short int
+#endif
+#ifndef USHORT
+#define USHORT unsigned short int
+#endif
+#ifndef INT
+#define INT int
+#endif
+#ifndef UINT
+#define UINT unsigned int
+#endif
+#ifndef LONG
+#define LONG long int
+#endif
+#ifndef ULONG
+#define ULONG unsigned long int
+#endif
+    // --- logic types ---
+#ifndef BYTE
+#define BYTE unsigned char
+#endif
+#ifndef WORD
+#define WORD unsigned short int
+#endif
+#ifndef DWORD
+#define DWORD unsigned long int
+#endif
+#ifndef BOOL
+#define BOOL unsigned char
+#endif
+    // --- alias types ---
+#ifndef TRUE
+#define TRUE  0xFF
+#endif
+#ifndef FALSE
+#define FALSE 0x00
+#endif
+#ifndef NULL
+#define NULL ((void *) 0)
+#endif
+#endif
+#ifndef _TIME_OF_DAY_DEFINED_
+typedef struct {
+	unsigned long int m_dwMs;
+	unsigned short int m_wDays;
+
+} tTimeOfDay;
+
+#define _TIME_OF_DAY_DEFINED_
+
+#endif
+
+//---------------------------------------------------------------------------
+//  Definition von TRACE
+//---------------------------------------------------------------------------
+
+#ifndef NDEBUG
+
+#ifndef TRACE0
+#define TRACE0(p0)                      TRACE(p0)
+#endif
+
+#ifndef TRACE1
+#define TRACE1(p0, p1)                  TRACE(p0, p1)
+#endif
+
+#ifndef TRACE2
+#define TRACE2(p0, p1, p2)              TRACE(p0, p1, p2)
+#endif
+
+#ifndef TRACE3
+#define TRACE3(p0, p1, p2, p3)          TRACE(p0, p1, p2, p3)
+#endif
+
+#ifndef TRACE4
+#define TRACE4(p0, p1, p2, p3, p4)      TRACE(p0, p1, p2, p3, p4)
+#endif
+
+#ifndef TRACE5
+#define TRACE5(p0, p1, p2, p3, p4, p5)  TRACE(p0, p1, p2, p3, p4, p5)
+#endif
+
+#ifndef TRACE6
+#define TRACE6(p0, p1, p2, p3, p4, p5, p6)  TRACE(p0, p1, p2, p3, p4, p5, p6)
+#endif
+
+#else
+
+#ifndef TRACE0
+#define TRACE0(p0)
+#endif
+
+#ifndef TRACE1
+#define TRACE1(p0, p1)
+#endif
+
+#ifndef TRACE2
+#define TRACE2(p0, p1, p2)
+#endif
+
+#ifndef TRACE3
+#define TRACE3(p0, p1, p2, p3)
+#endif
+
+#ifndef TRACE4
+#define TRACE4(p0, p1, p2, p3, p4)
+#endif
+
+#ifndef TRACE5
+#define TRACE5(p0, p1, p2, p3, p4, p5)
+#endif
+
+#ifndef TRACE6
+#define TRACE6(p0, p1, p2, p3, p4, p5, p6)
+#endif
+
+#endif
+
+//---------------------------------------------------------------------------
+//  definition of ASSERT
+//---------------------------------------------------------------------------
+
+#ifndef ASSERT
+#if !defined (__linux__) && !defined (__KERNEL__)
+#include <assert.h>
+#ifndef ASSERT
+#define ASSERT(p)    assert(p)
+#endif
+#else
+#define ASSERT(p)
+#endif
+#endif
+
+//---------------------------------------------------------------------------
+//  SYS TEC extensions
+//---------------------------------------------------------------------------
+
+// This macro doesn't print out C-file and line number of the failed assertion
+// but a string, which exactly names the mistake.
+#ifndef NDEBUG
+
+#define ASSERTMSG(expr,string)  if (!(expr)) {\
+                                        PRINTF0 ("Assertion failed: " string );\
+                                        while (1);}
+#else
+#define ASSERTMSG(expr,string)
+#endif
+
+//---------------------------------------------------------------------------
+
+#endif // #ifndef _GLOBAL_H_
+
+// Please keep an empty line at the end of this file.
diff --git a/drivers/staging/epl/kernel/EplDllk.h b/drivers/staging/epl/kernel/EplDllk.h
new file mode 100644
index 00000000000..588e871a922
--- /dev/null
+++ b/drivers/staging/epl/kernel/EplDllk.h
@@ -0,0 +1,165 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for kernelspace DLL module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplDllk.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.6 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/08 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_DLLK_H_
+#define _EPL_DLLK_H_
+
+#include "../EplDll.h"
+#include "../EplEvent.h"
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+typedef tEplKernel(*tEplDllkCbAsync) (tEplFrameInfo * pFrameInfo_p);
+
+typedef struct {
+	BYTE m_be_abSrcMac[6];
+
+} tEplDllkInitParam;
+
+// forward declaration
+struct _tEdrvTxBuffer;
+
+struct _tEplDllkNodeInfo {
+	struct _tEplDllkNodeInfo *m_pNextNodeInfo;
+	struct _tEdrvTxBuffer *m_pPreqTxBuffer;
+	unsigned int m_uiNodeId;
+	DWORD m_dwPresTimeout;
+	unsigned long m_ulDllErrorEvents;
+	tEplNmtState m_NmtState;
+	WORD m_wPresPayloadLimit;
+	BYTE m_be_abMacAddr[6];
+	BYTE m_bSoaFlag1;
+	BOOL m_fSoftDelete;	// delete node after error and ignore error
+
+};
+
+typedef struct _tEplDllkNodeInfo tEplDllkNodeInfo;
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+
+tEplKernel EplDllkAddInstance(tEplDllkInitParam * pInitParam_p);
+
+tEplKernel EplDllkDelInstance(void);
+
+// called before NMT_GS_COMMUNICATING will be entered to configure fixed parameters
+tEplKernel EplDllkConfig(tEplDllConfigParam * pDllConfigParam_p);
+
+// set identity of local node (may be at any time, e.g. in case of hostname change)
+tEplKernel EplDllkSetIdentity(tEplDllIdentParam * pDllIdentParam_p);
+
+// process internal events and do work that cannot be done in interrupt-context
+tEplKernel EplDllkProcess(tEplEvent * pEvent_p);
+
+// registers handler for non-EPL frames
+tEplKernel EplDllkRegAsyncHandler(tEplDllkCbAsync pfnDllkCbAsync_p);
+
+// deregisters handler for non-EPL frames
+tEplKernel EplDllkDeregAsyncHandler(tEplDllkCbAsync pfnDllkCbAsync_p);
+
+// register C_DLL_MULTICAST_ASND in ethernet driver if any AsndServiceId is registered
+tEplKernel EplDllkSetAsndServiceIdFilter(tEplDllAsndServiceId ServiceId_p,
+					 tEplDllAsndFilter Filter_p);
+
+// creates the buffer for a Tx frame and registers it to the ethernet driver
+tEplKernel EplDllkCreateTxFrame(unsigned int *puiHandle_p,
+				tEplFrame ** ppFrame_p,
+				unsigned int *puiFrameSize_p,
+				tEplMsgType MsgType_p,
+				tEplDllAsndServiceId ServiceId_p);
+
+tEplKernel EplDllkDeleteTxFrame(unsigned int uiHandle_p);
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+tEplKernel EplDllkAddNode(tEplDllNodeInfo * pNodeInfo_p);
+
+tEplKernel EplDllkDeleteNode(unsigned int uiNodeId_p);
+
+tEplKernel EplDllkSoftDeleteNode(unsigned int uiNodeId_p);
+
+tEplKernel EplDllkSetFlag1OfNode(unsigned int uiNodeId_p, BYTE bSoaFlag1_p);
+
+tEplKernel EplDllkGetFirstNodeInfo(tEplDllkNodeInfo ** ppNodeInfo_p);
+
+#endif //(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+#endif // #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+
+#endif // #ifndef _EPL_DLLK_H_
diff --git a/drivers/staging/epl/kernel/EplDllkCal.h b/drivers/staging/epl/kernel/EplDllkCal.h
new file mode 100644
index 00000000000..6c4dc7e4a80
--- /dev/null
+++ b/drivers/staging/epl/kernel/EplDllkCal.h
@@ -0,0 +1,141 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for kernelspace DLL Communication Abstraction Layer module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplDllkCal.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.6 $  $Date: 2008/11/13 17:13:09 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/13 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_DLLKCAL_H_
+#define _EPL_DLLKCAL_H_
+
+#include "../EplDll.h"
+#include "../EplEvent.h"
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+typedef struct {
+	unsigned long m_ulCurTxFrameCountGen;
+	unsigned long m_ulCurTxFrameCountNmt;
+	unsigned long m_ulCurRxFrameCount;
+	unsigned long m_ulMaxTxFrameCountGen;
+	unsigned long m_ulMaxTxFrameCountNmt;
+	unsigned long m_ulMaxRxFrameCount;
+
+} tEplDllkCalStatistics;
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+
+tEplKernel EplDllkCalAddInstance(void);
+
+tEplKernel EplDllkCalDelInstance(void);
+
+tEplKernel EplDllkCalAsyncGetTxCount(tEplDllAsyncReqPriority * pPriority_p,
+				     unsigned int *puiCount_p);
+tEplKernel EplDllkCalAsyncGetTxFrame(void *pFrame_p,
+				     unsigned int *puiFrameSize_p,
+				     tEplDllAsyncReqPriority Priority_p);
+// only frames with registered AsndServiceIds are passed to CAL
+tEplKernel EplDllkCalAsyncFrameReceived(tEplFrameInfo * pFrameInfo_p);
+
+tEplKernel EplDllkCalAsyncSend(tEplFrameInfo * pFrameInfo_p,
+			       tEplDllAsyncReqPriority Priority_p);
+
+tEplKernel EplDllkCalAsyncClearBuffer(void);
+
+tEplKernel EplDllkCalGetStatistics(tEplDllkCalStatistics ** ppStatistics);
+
+tEplKernel EplDllkCalProcess(tEplEvent * pEvent_p);
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+tEplKernel EplDllkCalAsyncClearQueues(void);
+
+tEplKernel EplDllkCalIssueRequest(tEplDllReqServiceId Service_p,
+				  unsigned int uiNodeId_p, BYTE bSoaFlag1_p);
+
+tEplKernel EplDllkCalAsyncGetSoaRequest(tEplDllReqServiceId * pReqServiceId_p,
+					unsigned int *puiNodeId_p);
+
+tEplKernel EplDllkCalAsyncSetPendingRequests(unsigned int uiNodeId_p,
+					     tEplDllAsyncReqPriority
+					     AsyncReqPrio_p,
+					     unsigned int uiCount_p);
+
+#endif //(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+#endif // #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLK)) != 0)
+
+#endif // #ifndef _EPL_DLLKCAL_H_
diff --git a/drivers/staging/epl/kernel/EplErrorHandlerk.h b/drivers/staging/epl/kernel/EplErrorHandlerk.h
new file mode 100644
index 00000000000..4a67ef88b97
--- /dev/null
+++ b/drivers/staging/epl/kernel/EplErrorHandlerk.h
@@ -0,0 +1,100 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for kernel error handler module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplErrorHandlerk.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/10/02 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_ERRORHANDLERK_H_
+#define _EPL_ERRORHANDLERK_H_
+
+#include "../EplEvent.h"
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+// init function
+tEplKernel PUBLIC EplErrorHandlerkInit(void);
+
+// add instance
+tEplKernel PUBLIC EplErrorHandlerkAddInstance(void);
+
+// delete instance
+tEplKernel PUBLIC EplErrorHandlerkDelInstance(void);
+
+// processes error events
+tEplKernel PUBLIC EplErrorHandlerkProcess(tEplEvent * pEvent_p);
+
+#endif // #ifndef _EPL_ERRORHANDLERK_H_
diff --git a/drivers/staging/epl/kernel/EplEventk.h b/drivers/staging/epl/kernel/EplEventk.h
new file mode 100644
index 00000000000..1d25aaa2ed4
--- /dev/null
+++ b/drivers/staging/epl/kernel/EplEventk.h
@@ -0,0 +1,108 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for kernel event module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplEventk.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/12 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_EVENTK_H_
+#define _EPL_EVENTK_H_
+
+#include "../EplEvent.h"
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+// init function
+tEplKernel PUBLIC EplEventkInit(tEplSyncCb fpSyncCb);
+
+// add instance
+tEplKernel PUBLIC EplEventkAddInstance(tEplSyncCb fpSyncCb);
+
+// delete instance
+tEplKernel PUBLIC EplEventkDelInstance(void);
+
+// Kernelthread that dispatches events in kernelspace
+tEplKernel PUBLIC EplEventkProcess(tEplEvent * pEvent_p);
+
+// post events from kernelspace
+tEplKernel PUBLIC EplEventkPost(tEplEvent * pEvent_p);
+
+// post errorevents from kernelspace
+tEplKernel PUBLIC EplEventkPostError(tEplEventSource EventSource_p,
+				     tEplKernel EplError_p,
+				     unsigned int uiArgSize_p, void *pArg_p);
+
+#endif // #ifndef _EPL_EVENTK_H_
diff --git a/drivers/staging/epl/kernel/EplNmtk.h b/drivers/staging/epl/kernel/EplNmtk.h
new file mode 100644
index 00000000000..53409cc8992
--- /dev/null
+++ b/drivers/staging/epl/kernel/EplNmtk.h
@@ -0,0 +1,105 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for NMT-Kernelspace-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplNmtk.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/09 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#ifndef _EPLNMTK_H_
+#define _EPLNMTK_H_
+
+#include "../EplNmt.h"
+#include "EplEventk.h"
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTK)) != 0)
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtkInit(EPL_MCO_DECL_PTR_INSTANCE_PTR);
+
+EPLDLLEXPORT tEplKernel PUBLIC
+EplNmtkAddInstance(EPL_MCO_DECL_PTR_INSTANCE_PTR);
+
+EPLDLLEXPORT tEplKernel PUBLIC
+EplNmtkDelInstance(EPL_MCO_DECL_PTR_INSTANCE_PTR);
+
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtkProcess(EPL_MCO_DECL_PTR_INSTANCE_PTR_
+					      tEplEvent * pEvent_p);
+
+EPLDLLEXPORT tEplNmtState PUBLIC
+EplNmtkGetNmtState(EPL_MCO_DECL_PTR_INSTANCE_PTR);
+
+#endif // #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTK)) != 0)
+
+#endif // #ifndef _EPLNMTK_H_
diff --git a/drivers/staging/epl/kernel/EplNmtkCal.h b/drivers/staging/epl/kernel/EplNmtkCal.h
new file mode 100644
index 00000000000..9edeafca492
--- /dev/null
+++ b/drivers/staging/epl/kernel/EplNmtkCal.h
@@ -0,0 +1,89 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for communication abstraction layer of the
+                NMT-Kernel-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplNmtkCal.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.3 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                KEIL uVision 2
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/16 -k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "EplNmtk.h"
+
+#ifndef _EPLNMTKCAL_H_
+#define _EPLNMTKCAL_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#endif // #ifndef _EPLNMTKCAL_H_
diff --git a/drivers/staging/epl/kernel/EplObdk.h b/drivers/staging/epl/kernel/EplObdk.h
new file mode 100644
index 00000000000..cf9f5837dd3
--- /dev/null
+++ b/drivers/staging/epl/kernel/EplObdk.h
@@ -0,0 +1,196 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for Epl-Obd-Kernel-Modul
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplObdk.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.8 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/19 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "../EplObd.h"
+
+#ifndef _EPLOBDK_H_
+#define _EPLOBDK_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// global variables
+//---------------------------------------------------------------------------
+
+extern BYTE MEM abEplObdTrashObject_g[8];
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdInit(EPL_MCO_DECL_PTR_INSTANCE_PTR_
+					  tEplObdInitParam MEM * pInitParam_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdAddInstance(EPL_MCO_DECL_PTR_INSTANCE_PTR_
+						 tEplObdInitParam MEM *
+						 pInitParam_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdDeleteInstance(EPL_MCO_DECL_INSTANCE_PTR);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdWriteEntry(EPL_MCO_DECL_INSTANCE_PTR_
+						unsigned int uiIndex_p,
+						unsigned int uiSubIndex_p,
+						void *pSrcData_p,
+						tEplObdSize Size_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdReadEntry(EPL_MCO_DECL_INSTANCE_PTR_
+					       unsigned int uiIndex_p,
+					       unsigned int uiSubIndex_p,
+					       void *pDstData_p,
+					       tEplObdSize * pSize_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC
+EplObdSetStoreLoadObjCallback(EPL_MCO_DECL_INSTANCE_PTR_
+			      tEplObdStoreLoadObjCallback fpCallback_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdAccessOdPart(EPL_MCO_DECL_INSTANCE_PTR_
+						  tEplObdPart ObdPart_p,
+						  tEplObdDir Direction_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdDefineVar(EPL_MCO_DECL_INSTANCE_PTR_
+					       tEplVarParam MEM * pVarParam_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT void *PUBLIC EplObdGetObjectDataPtr(EPL_MCO_DECL_INSTANCE_PTR_
+						 unsigned int uiIndex_p,
+						 unsigned int uiSubIndex_p);
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdRegisterUserOd(EPL_MCO_DECL_INSTANCE_PTR_
+						    tEplObdEntryPtr pUserOd_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT void PUBLIC EplObdInitVarEntry(EPL_MCO_DECL_INSTANCE_PTR_
+					    tEplObdVarEntry MEM * pVarEntry_p,
+					    tEplObdType Type_p,
+					    tEplObdSize ObdSize_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplObdSize PUBLIC EplObdGetDataSize(EPL_MCO_DECL_INSTANCE_PTR_
+						  unsigned int uiIndex_p,
+						  unsigned int uiSubIndex_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT unsigned int PUBLIC EplObdGetNodeId(EPL_MCO_DECL_INSTANCE_PTR);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdSetNodeId(EPL_MCO_DECL_INSTANCE_PTR_
+					       unsigned int uiNodeId_p,
+					       tEplObdNodeIdType NodeIdType_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdIsNumerical(EPL_MCO_DECL_INSTANCE_PTR_
+						 unsigned int uiIndex_p,
+						 unsigned int uiSubIndex_p,
+						 BOOL * pfEntryNumerical);
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdWriteEntryFromLe(EPL_MCO_DECL_INSTANCE_PTR_
+						      unsigned int uiIndex_p,
+						      unsigned int uiSubIndex_p,
+						      void *pSrcData_p,
+						      tEplObdSize Size_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdReadEntryToLe(EPL_MCO_DECL_INSTANCE_PTR_
+						   unsigned int uiIndex_p,
+						   unsigned int uiSubIndex_p,
+						   void *pDstData_p,
+						   tEplObdSize * pSize_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdGetAccessType(EPL_MCO_DECL_INSTANCE_PTR_
+						   unsigned int uiIndex_p,
+						   unsigned int uiSubIndex_p,
+						   tEplObdAccess *
+						   pAccessTyp_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObdSearchVarEntry(EPL_MCO_DECL_INSTANCE_PTR_
+						    unsigned int uiIndex_p,
+						    unsigned int uiSubindex_p,
+						    tEplObdVarEntry MEM **
+						    ppVarEntry_p);
+
+#endif // end of #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDK)) != 0)
+
+#endif // #ifndef _EPLOBDK_H_
diff --git a/drivers/staging/epl/kernel/EplObdkCal.h b/drivers/staging/epl/kernel/EplObdkCal.h
new file mode 100644
index 00000000000..c173a950054
--- /dev/null
+++ b/drivers/staging/epl/kernel/EplObdkCal.h
@@ -0,0 +1,89 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for communication abstraction layer
+                for the Epl-Obd-Kernelspace-Modul
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplObdkCal.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/19 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "../EplObd.h"
+
+#ifndef _EPLOBDKCAL_H_
+#define _EPLOBDKCAL_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#endif // #ifndef _EPLOBDKCAL_H_
diff --git a/drivers/staging/epl/kernel/EplPdok.h b/drivers/staging/epl/kernel/EplPdok.h
new file mode 100644
index 00000000000..b5b18f4cf68
--- /dev/null
+++ b/drivers/staging/epl/kernel/EplPdok.h
@@ -0,0 +1,110 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for kernel PDO module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplPdok.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/06/23 14:56:33 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/05/22 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_PDOK_H_
+#define _EPL_PDOK_H_
+
+#include "../EplPdo.h"
+#include "../EplEvent.h"
+#include "../EplDll.h"
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+// process events from queue (PDOs/frames and SoA for synchronization)
+tEplKernel EplPdokProcess(tEplEvent * pEvent_p);
+
+// copies RPDO to event queue for processing
+// is called by DLL in NMT_CS_READY_TO_OPERATE and NMT_CS_OPERATIONAL
+// PDO needs not to be valid
+tEplKernel EplPdokCbPdoReceived(tEplFrameInfo * pFrameInfo_p);
+
+// posts pointer and size of TPDO to event queue
+// is called by DLL in NMT_CS_PRE_OPERATIONAL_2,
+//     NMT_CS_READY_TO_OPERATE and NMT_CS_OPERATIONAL
+tEplKernel EplPdokCbPdoTransmitted(tEplFrameInfo * pFrameInfo_p);
+
+// posts SoA event to queue
+tEplKernel EplPdokCbSoa(tEplFrameInfo * pFrameInfo_p);
+
+tEplKernel EplPdokAddInstance(void);
+
+tEplKernel EplPdokDelInstance(void);
+
+#endif // #ifndef _EPL_PDOK_H_
diff --git a/drivers/staging/epl/kernel/EplPdokCal.h b/drivers/staging/epl/kernel/EplPdokCal.h
new file mode 100644
index 00000000000..6a183ebe81e
--- /dev/null
+++ b/drivers/staging/epl/kernel/EplPdokCal.h
@@ -0,0 +1,99 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for kernel PDO Communication Abstraction Layer module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplPdokCal.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/26 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_PDOKCAL_H_
+#define _EPL_PDOKCAL_H_
+
+#include "../EplInc.h"
+//#include "EplPdo.h"
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+tEplKernel EplPdokCalAddInstance(void);
+
+tEplKernel EplPdokCalDelInstance(void);
+
+// sets flag for validity of TPDOs in shared memory
+tEplKernel EplPdokCalSetTpdosValid(BOOL fValid_p);
+
+// gets flag for validity of TPDOs from shared memory
+tEplKernel EplPdokCalAreTpdosValid(BOOL * pfValid_p);
+
+#endif // #ifndef _EPL_PDOKCAL_H_
diff --git a/drivers/staging/epl/kernel/EplTimerHighResk.h b/drivers/staging/epl/kernel/EplTimerHighResk.h
new file mode 100644
index 00000000000..d5d046d4d37
--- /dev/null
+++ b/drivers/staging/epl/kernel/EplTimerHighResk.h
@@ -0,0 +1,109 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for EPL high resolution Timermodule
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplTimerHighResk.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.3 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/09/29 d.k.:   start of the implementation
+
+****************************************************************************/
+
+#include "../EplTimer.h"
+
+#ifndef _EPLTIMERHIGHRESK_H_
+#define _EPLTIMERHIGHRESK_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplTimerHighReskInit(void);
+
+tEplKernel PUBLIC EplTimerHighReskAddInstance(void);
+
+tEplKernel PUBLIC EplTimerHighReskDelInstance(void);
+
+tEplKernel PUBLIC EplTimerHighReskSetTimerNs(tEplTimerHdl * pTimerHdl_p,
+					     unsigned long long ullTimeNs_p,
+					     tEplTimerkCallback pfnCallback_p,
+					     unsigned long ulArgument_p,
+					     BOOL fContinuously_p);
+
+tEplKernel PUBLIC EplTimerHighReskModifyTimerNs(tEplTimerHdl * pTimerHdl_p,
+						unsigned long long ullTimeNs_p,
+						tEplTimerkCallback
+						pfnCallback_p,
+						unsigned long ulArgument_p,
+						BOOL fContinuously_p);
+
+tEplKernel PUBLIC EplTimerHighReskDeleteTimer(tEplTimerHdl * pTimerHdl_p);
+
+#endif // #ifndef _EPLTIMERHIGHRESK_H_
diff --git a/drivers/staging/epl/kernel/EplTimerk.h b/drivers/staging/epl/kernel/EplTimerk.h
new file mode 100644
index 00000000000..9160e7260de
--- /dev/null
+++ b/drivers/staging/epl/kernel/EplTimerk.h
@@ -0,0 +1,118 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for EPL Kernel-Timermodule
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplTimerk.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/07/06 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "../EplTimer.h"
+#include "../user/EplEventu.h"
+
+#ifndef _EPLTIMERK_H_
+#define _EPLTIMERK_H_
+
+#if EPL_TIMER_USE_USER != FALSE
+#include "../user/EplTimeru.h"
+#endif
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#if EPL_TIMER_USE_USER != FALSE
+#define EplTimerkInit           EplTimeruInit
+#define EplTimerkAddInstance    EplTimeruAddInstance
+#define EplTimerkDelInstance    EplTimeruDelInstance
+#define EplTimerkSetTimerMs     EplTimeruSetTimerMs
+#define EplTimerkModifyTimerMs  EplTimeruModifyTimerMs
+#define EplTimerkDeleteTimer    EplTimeruDeleteTimer
+#endif
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+#if EPL_TIMER_USE_USER == FALSE
+tEplKernel PUBLIC EplTimerkInit(void);
+
+tEplKernel PUBLIC EplTimerkAddInstance(void);
+
+tEplKernel PUBLIC EplTimerkDelInstance(void);
+
+tEplKernel PUBLIC EplTimerkSetTimerMs(tEplTimerHdl * pTimerHdl_p,
+				      unsigned long ulTime_p,
+				      tEplTimerArg Argument_p);
+
+tEplKernel PUBLIC EplTimerkModifyTimerMs(tEplTimerHdl * pTimerHdl_p,
+					 unsigned long ulTime_p,
+					 tEplTimerArg Argument_p);
+
+tEplKernel PUBLIC EplTimerkDeleteTimer(tEplTimerHdl * pTimerHdl_p);
+#endif
+#endif // #ifndef _EPLTIMERK_H_
diff --git a/drivers/staging/epl/kernel/VirtualEthernet.h b/drivers/staging/epl/kernel/VirtualEthernet.h
new file mode 100644
index 00000000000..deff8720e37
--- /dev/null
+++ b/drivers/staging/epl/kernel/VirtualEthernet.h
@@ -0,0 +1,96 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for virtual ethernet driver module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: VirtualEthernet.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                KEIL uVision 2
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/09/19 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_VETH_H_
+#define _EPL_VETH_H_
+
+#include "EplDllk.h"
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_VETH)) != 0)
+
+tEplKernel PUBLIC VEthAddInstance(tEplDllkInitParam * pInitParam_p);
+
+tEplKernel PUBLIC VEthDelInstance(void);
+
+#endif // #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_VETH)) != 0)
+
+#endif // #ifndef _EPL_VETH_H_
diff --git a/drivers/staging/epl/proc_fs.c b/drivers/staging/epl/proc_fs.c
new file mode 100644
index 00000000000..f4910332d3c
--- /dev/null
+++ b/drivers/staging/epl/proc_fs.c
@@ -0,0 +1,409 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  proc fs entry with diagnostic information under Linux
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: proc_fs.c,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.13 $  $Date: 2008/11/07 13:55:56 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GNU
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/07/31 d.k.:   start of implementation
+
+****************************************************************************/
+
+#include "kernel/EplNmtk.h"
+#include "user/EplNmtu.h"
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+#include "user/EplNmtMnu.h"
+#endif
+
+#include "kernel/EplDllkCal.h"
+
+//#include <linux/config.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/major.h>
+#include <linux/version.h>
+#include <asm/io.h>
+#include <asm/uaccess.h>
+#include <asm/atomic.h>
+#include <linux/proc_fs.h>
+#include <linux/spinlock.h>
+
+#ifdef CONFIG_COLDFIRE
+#include <asm/coldfire.h>
+#include "fec.h"
+#endif
+
+/***************************************************************************/
+/*                                                                         */
+/*                                                                         */
+/*          G L O B A L   D E F I N I T I O N S                            */
+/*                                                                         */
+/*                                                                         */
+/***************************************************************************/
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+#ifndef EPL_PROC_DEV_NAME
+#define EPL_PROC_DEV_NAME    "epl"
+#endif
+
+#ifndef DBG_TRACE_POINTS
+#define DBG_TRACE_POINTS    23	// # of supported debug trace points
+#endif
+
+#ifndef DBG_TRACE_VALUES
+#define DBG_TRACE_VALUES    24	// # of supported debug trace values (size of circular buffer)
+#endif
+
+//---------------------------------------------------------------------------
+// modul global types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// local vars
+//---------------------------------------------------------------------------
+
+#ifdef _DBG_TRACE_POINTS_
+atomic_t aatmDbgTracePoint_l[DBG_TRACE_POINTS];
+DWORD adwDbgTraceValue_l[DBG_TRACE_VALUES];
+DWORD dwDbgTraceValueOld_l;
+unsigned int uiDbgTraceValuePos_l;
+spinlock_t spinlockDbgTraceValue_l;
+unsigned long ulDbTraceValueFlags_l;
+#endif
+
+//---------------------------------------------------------------------------
+// local function prototypes
+//---------------------------------------------------------------------------
+
+static int EplLinProcRead(char *pcBuffer_p, char **ppcStart_p, off_t Offset_p,
+			  int nBufferSize_p, int *pEof_p, void *pData_p);
+static int EplLinProcWrite(struct file *file, const char __user * buffer,
+			   unsigned long count, void *data);
+
+void PUBLIC TgtDbgSignalTracePoint(BYTE bTracePointNumber_p);
+void PUBLIC TgtDbgPostTraceValue(DWORD dwTraceValue_p);
+
+EPLDLLEXPORT DWORD PUBLIC EplIdentuGetRunningRequests(void);
+
+//=========================================================================//
+//                                                                         //
+//          P U B L I C   F U N C T I O N S                                //
+//                                                                         //
+//=========================================================================//
+
+tEplKernel EplLinProcInit(void)
+{
+	struct proc_dir_entry *pProcDirEntry;
+	pProcDirEntry = create_proc_entry(EPL_PROC_DEV_NAME, S_IRUGO, NULL);
+	if (pProcDirEntry != NULL) {
+		pProcDirEntry->read_proc = EplLinProcRead;
+		pProcDirEntry->write_proc = EplLinProcWrite;
+		pProcDirEntry->data = NULL;	// device number or something else
+
+	} else {
+		return kEplNoResource;
+	}
+
+#ifdef _DBG_TRACE_POINTS_
+	// initialize spinlock and circular buffer position
+	spin_lock_init(&spinlockDbgTraceValue_l);
+	uiDbgTraceValuePos_l = 0;
+	dwDbgTraceValueOld_l = 0;
+#endif
+
+	return kEplSuccessful;
+}
+
+tEplKernel EplLinProcFree(void)
+{
+	remove_proc_entry(EPL_PROC_DEV_NAME, NULL);
+
+	return kEplSuccessful;
+}
+
+//---------------------------------------------------------------------------
+//  Target specific event signaling (FEC Tx-/Rx-Interrupt, used by Edrv)
+//---------------------------------------------------------------------------
+
+#ifdef _DBG_TRACE_POINTS_
+void PUBLIC TgtDbgSignalTracePoint(BYTE bTracePointNumber_p)
+{
+
+	if (bTracePointNumber_p >=
+	    (sizeof(aatmDbgTracePoint_l) / sizeof(aatmDbgTracePoint_l[0]))) {
+		goto Exit;
+	}
+
+	atomic_inc(&aatmDbgTracePoint_l[bTracePointNumber_p]);
+
+      Exit:
+
+	return;
+
+}
+
+void PUBLIC TgtDbgPostTraceValue(DWORD dwTraceValue_p)
+{
+
+	spin_lock_irqsave(&spinlockDbgTraceValue_l, ulDbTraceValueFlags_l);
+	if (dwDbgTraceValueOld_l != dwTraceValue_p) {
+		adwDbgTraceValue_l[uiDbgTraceValuePos_l] = dwTraceValue_p;
+		uiDbgTraceValuePos_l =
+		    (uiDbgTraceValuePos_l + 1) % DBG_TRACE_VALUES;
+		dwDbgTraceValueOld_l = dwTraceValue_p;
+	}
+	spin_unlock_irqrestore(&spinlockDbgTraceValue_l, ulDbTraceValueFlags_l);
+
+	return;
+
+}
+#endif
+
+//---------------------------------------------------------------------------
+//  Read function for PROC-FS read access
+//---------------------------------------------------------------------------
+
+static int EplLinProcRead(char *pcBuffer_p,
+			  char **ppcStart_p,
+			  off_t Offset_p,
+			  int nBufferSize_p, int *pEof_p, void *pData_p)
+{
+
+	int nSize;
+	int Eof;
+	tEplDllkCalStatistics *pDllkCalStats;
+
+	nSize = 0;
+	Eof = 0;
+
+	// count calls of this function
+#ifdef _DBG_TRACE_POINTS_
+	TgtDbgSignalTracePoint(0);
+#endif
+
+	//---------------------------------------------------------------
+	// generate static information
+	//---------------------------------------------------------------
+
+	// ---- Driver information ----
+	nSize += snprintf(pcBuffer_p + nSize, nBufferSize_p - nSize,
+			  "%s    %s    (c) 2006 %s\n",
+			  EPL_PRODUCT_NAME, EPL_PRODUCT_VERSION,
+			  EPL_PRODUCT_MANUFACTURER);
+
+	//---------------------------------------------------------------
+	// generate process information
+	//---------------------------------------------------------------
+
+	// ---- EPL state ----
+	nSize += snprintf(pcBuffer_p + nSize, nBufferSize_p - nSize,
+			  "NMT state:                  0x%04X\n",
+			  (WORD) EplNmtkGetNmtState());
+
+	EplDllkCalGetStatistics(&pDllkCalStats);
+
+	nSize += snprintf(pcBuffer_p + nSize, nBufferSize_p - nSize,
+			  "CurAsyncTxGen=%lu CurAsyncTxNmt=%lu CurAsyncRx=%lu\nMaxAsyncTxGen=%lu MaxAsyncTxNmt=%lu MaxAsyncRx=%lu\n",
+			  pDllkCalStats->m_ulCurTxFrameCountGen,
+			  pDllkCalStats->m_ulCurTxFrameCountNmt,
+			  pDllkCalStats->m_ulCurRxFrameCount,
+			  pDllkCalStats->m_ulMaxTxFrameCountGen,
+			  pDllkCalStats->m_ulMaxTxFrameCountNmt,
+			  pDllkCalStats->m_ulMaxRxFrameCount);
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+	// fetch running IdentRequests
+	nSize += snprintf(pcBuffer_p + nSize, nBufferSize_p - nSize,
+			  "running IdentRequests:      0x%08lX\n",
+			  EplIdentuGetRunningRequests());
+
+	// fetch state of NmtMnu module
+	{
+		unsigned int uiMandatorySlaveCount;
+		unsigned int uiSignalSlaveCount;
+		WORD wFlags;
+
+		EplNmtMnuGetDiagnosticInfo(&uiMandatorySlaveCount,
+					   &uiSignalSlaveCount, &wFlags);
+
+		nSize += snprintf(pcBuffer_p + nSize, nBufferSize_p - nSize,
+				  "MN  MandSlaveCount: %u  SigSlaveCount: %u  Flags: 0x%X\n",
+				  uiMandatorySlaveCount, uiSignalSlaveCount,
+				  wFlags);
+
+	}
+#endif
+
+	// ---- FEC state ----
+#ifdef CONFIG_COLDFIRE
+	{
+		// Receive the base address
+		unsigned long base_addr;
+#if (EDRV_USED_ETH_CTRL == 0)
+		// Set the base address of FEC0
+		base_addr = FEC_BASE_ADDR_FEC0;
+#else
+		// Set the base address of FEC1
+		base_addr = FEC_BASE_ADDR_FEC1;
+#endif
+
+		nSize += snprintf(pcBuffer_p + nSize, nBufferSize_p - nSize,
+				  "FEC_ECR = 0x%08X FEC_EIR = 0x%08X FEC_EIMR = 0x%08X\nFEC_TCR = 0x%08X FECTFSR = 0x%08X FECRFSR = 0x%08X\n",
+				  FEC_ECR(base_addr), FEC_EIR(base_addr),
+				  FEC_EIMR(base_addr), FEC_TCR(base_addr),
+				  FEC_FECTFSR(base_addr),
+				  FEC_FECRFSR(base_addr));
+	}
+#endif
+
+	// ---- DBG: TracePoints ----
+#ifdef _DBG_TRACE_POINTS_
+	{
+		int nNum;
+
+		nSize += snprintf(pcBuffer_p + nSize, nBufferSize_p - nSize,
+				  "DbgTracePoints:\n");
+		for (nNum = 0;
+		     nNum < (sizeof(aatmDbgTracePoint_l) / sizeof(atomic_t));
+		     nNum++) {
+			nSize +=
+			    snprintf(pcBuffer_p + nSize, nBufferSize_p - nSize,
+				     " TracePoint[%2d]: %d\n", (int)nNum,
+				     atomic_read(&aatmDbgTracePoint_l[nNum]));
+		}
+
+		nSize += snprintf(pcBuffer_p + nSize, nBufferSize_p - nSize,
+				  "DbgTraceValues:\n");
+		for (nNum = 0; nNum < DBG_TRACE_VALUES; nNum++) {
+			if (nNum == uiDbgTraceValuePos_l) {	// next value will be stored at that position
+				nSize +=
+				    snprintf(pcBuffer_p + nSize,
+					     nBufferSize_p - nSize, "*%08lX",
+					     adwDbgTraceValue_l[nNum]);
+			} else {
+				nSize +=
+				    snprintf(pcBuffer_p + nSize,
+					     nBufferSize_p - nSize, " %08lX",
+					     adwDbgTraceValue_l[nNum]);
+			}
+			if ((nNum & 0x00000007) == 0x00000007) {	// 8 values printed -> end of line reached
+				nSize +=
+				    snprintf(pcBuffer_p + nSize,
+					     nBufferSize_p - nSize, "\n");
+			}
+		}
+		if ((nNum & 0x00000007) != 0x00000007) {	// number of values printed is not a multiple of 8 -> print new line
+			nSize +=
+			    snprintf(pcBuffer_p + nSize, nBufferSize_p - nSize,
+				     "\n");
+		}
+	}
+#endif
+
+	Eof = 1;
+	goto Exit;
+
+      Exit:
+
+	*pEof_p = Eof;
+
+	return (nSize);
+
+}
+
+//---------------------------------------------------------------------------
+//  Write function for PROC-FS write access
+//---------------------------------------------------------------------------
+
+static int EplLinProcWrite(struct file *file, const char __user * buffer,
+			   unsigned long count, void *data)
+{
+	char abBuffer[count + 1];
+	int iErr;
+	int iVal = 0;
+	tEplNmtEvent NmtEvent;
+
+	if (count > 0) {
+		iErr = copy_from_user(abBuffer, buffer, count);
+		if (iErr != 0) {
+			return count;
+		}
+		abBuffer[count] = '\0';
+
+		iErr = sscanf(abBuffer, "%i", &iVal);
+	}
+	if ((iVal <= 0) || (iVal > 0x2F)) {
+		NmtEvent = kEplNmtEventSwReset;
+	} else {
+		NmtEvent = (tEplNmtEvent) iVal;
+	}
+	// execute specified NMT command on write access of /proc/epl
+	EplNmtuNmtEvent(NmtEvent);
+
+	return count;
+}
diff --git a/drivers/staging/epl/proc_fs.h b/drivers/staging/epl/proc_fs.h
new file mode 100644
index 00000000000..0586f499553
--- /dev/null
+++ b/drivers/staging/epl/proc_fs.h
@@ -0,0 +1,89 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  interface for proc fs entry under Linux
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: proc_fs.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.3 $  $Date: 2008/04/17 21:36:33 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GNU
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/07/31 d.k.:   start of implementation
+
+****************************************************************************/
+
+#ifndef _EPLPROCFS_H_
+#define _EPLPROCFS_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// types
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+tEplKernel EplLinProcInit(void);
+tEplKernel EplLinProcFree(void);
+
+#endif // #ifndef _EPLPROCFS_H_
diff --git a/drivers/staging/epl/user/EplCfgMau.h b/drivers/staging/epl/user/EplCfgMau.h
new file mode 100644
index 00000000000..d25a1e9d36c
--- /dev/null
+++ b/drivers/staging/epl/user/EplCfgMau.h
@@ -0,0 +1,284 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for Epl Configuration Manager Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplCfgMau.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                VC7
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/07/14 k.t.:   start of the implementation
+                     -> based on CANopen CfgMa-Modul (CANopen version 5.34)
+
+****************************************************************************/
+
+#include "../EplInc.h"
+
+#ifndef _EPLCFGMA_H_
+#define _EPLCFGMA_H_
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_CFGMA)) != 0)
+
+#include "EplObdu.h"
+#include "EplSdoComu.h"
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+//define max number of timeouts for configuration of 1 device
+#define EPL_CFGMA_MAX_TIMEOUT   3
+
+//callbackfunction, called if configuration is finished
+typedef void (PUBLIC * tfpEplCfgMaCb) (unsigned int uiNodeId_p,
+				       tEplKernel Errorstate_p);
+
+//State for configuartion manager Statemachine
+typedef enum {
+	// general states
+	kEplCfgMaIdle = 0x0000,	// Configurationsprocess
+	// is idle
+	kEplCfgMaWaitForSdocEvent = 0x0001,	// wait until the last
+	// SDOC is finisched
+	kEplCfgMaSkipMappingSub0 = 0x0002,	// write Sub0 of mapping
+	// parameter with 0
+
+	kEplCfgMaFinished = 0x0004	// configuartion is finished
+} tEplCfgState;
+
+typedef enum {
+	kEplCfgMaDcfTypSystecSeg = 0x00,
+	kEplCfgMaDcfTypConDcf = 0x01,
+	kEplCfgMaDcfTypDcf = 0x02,	// not supported
+	kEplCfgMaDcfTypXdc = 0x03	// not supported
+} tEplCfgMaDcfTyp;
+
+typedef enum {
+	kEplCfgMaCommon = 0,	// all other index
+	kEplCfgMaPdoComm = 1,	// communication index
+	kEplCfgMaPdoMapp = 2,	// mapping index
+	kEplCfgMaPdoCommAfterMapp = 3,	// write PDO Cob-Id after mapping subindex 0(set PDO valid)
+
+} tEplCfgMaIndexType;
+
+//bitcoded answer about the last sdo transfer saved in m_SdocState
+// also used to singal start of the State Maschine
+typedef enum {
+	kEplCfgMaSdocBusy = 0x00,	// SDOC activ
+	kEplCfgMaSdocReady = 0x01,	// SDOC finished
+	kEplCfgMaSdocTimeout = 0x02,	// SDOC Timeout
+	kEplCfgMaSdocAbortReceived = 0x04,	// SDOC Abort, see Abortcode
+	kEplCfgMaSdocStart = 0x08	// start State Mschine
+} tEplSdocState;
+
+//internal structure (instancetable for modul configuration manager)
+typedef struct {
+	tEplCfgState m_CfgState;	// state of the configuration state maschine
+	tEplSdoComConHdl m_SdoComConHdl;	// handle for sdo connection
+	DWORD m_dwLastAbortCode;
+	unsigned int m_uiLastIndex;	// last index of configuration, to compair with actual index
+	BYTE *m_pbConcise;	// Ptr to concise DCF
+	BYTE *m_pbActualIndex;	// Ptr to actual index in the DCF segment
+	tfpEplCfgMaCb m_pfnCfgMaCb;	// Ptr to CfgMa Callback, is call if configuration finished
+	tEplKernel m_EplKernelError;	// errorcode
+	DWORD m_dwNumValueCopy;	// numeric values are copied in this variable
+	unsigned int m_uiPdoNodeId;	// buffer for PDO node id
+	BYTE m_bNrOfMappedObject;	// number of mapped objects
+	unsigned int m_uiNodeId;	// Epl node addresse
+	tEplSdocState m_SdocState;	// bitcoded state of the SDO transfer
+	unsigned int m_uiLastSubIndex;	// last subindex of configuration
+	BOOL m_fOneTranferOk;	// atleased one transfer was successful
+	BYTE m_bEventFlag;	// for Eventsignaling to the State Maschine
+	DWORD m_dwCntObjectInDcf;	// number of Objects in DCF
+	tEplCfgMaIndexType m_SkipCfg;	// TRUE if a adsitional Configurationprocess
+	// have to insert e.g. PDO-mapping
+	WORD m_wTimeOutCnt;	// Timeout Counter, break configuration is
+	// m_wTimeOutCnt == CFGMA_MAX_TIMEOUT
+
+} tEplCfgMaNode;
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// Function:    EplCfgMaInit()
+//
+// Description: Function creates first instance of Configuration Manager
+//
+// Parameters:
+//
+// Returns:     tEplKernel              = error code
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplCfgMaInit();
+
+//---------------------------------------------------------------------------
+// Function:    EplCfgMaAddInstance()
+//
+// Description: Function creates additional instance of Configuration Manager
+//
+// Parameters:
+//
+// Returns:     tEplKernel              = error code
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplCfgMaAddInstance();
+
+//---------------------------------------------------------------------------
+// Function:    EplCfgMaDelInstance()
+//
+// Description: Function delete instance of Configuration Manager
+//
+// Parameters:
+//
+// Returns:     tEplKernel              = error code
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplCfgMaDelInstance();
+
+//---------------------------------------------------------------------------
+// Function:    EplCfgMaStartConfig()
+//
+// Description: Function starts the configuration process
+//              initialization the statemachine for CfgMa- process
+//
+// Parameters:  uiNodeId_p              = NodeId of the node to configure
+//              pbConcise_p             = pointer to DCF
+//              fpCfgMaCb_p             = pointer to callback function (should not be NULL)
+//              SizeOfConcise_p         = size of DCF in BYTE -> for future use
+//              DcfType_p               = type of the DCF
+//
+// Returns:     tCopKernel              = error code
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplCfgMaStartConfig(unsigned int uiNodeId_p,
+				      BYTE * pbConcise_p,
+				      tfpEplCfgMaCb fpCfgMaCb_p,
+				      tEplObdSize SizeOfConcise_p,
+				      tEplCfgMaDcfTyp DcfType_p);
+
+//---------------------------------------------------------------------------
+// Function:    CfgMaStartConfigurationNode()
+//
+// Description: Function started the configuration process
+//              with the DCF from according OD-entry Subindex == bNodeId_p
+//
+// Parameters:  uiNodeId_p              = NodeId of the node to configure
+//              fpCfgMaCb_p             = pointer to callback function (should not be NULL)
+//              DcfType_p               = type of the DCF
+//
+// Returns:     tCopKernel              = error code
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplCfgMaStartConfigNode(unsigned int uiNodeId_p,
+					  tfpEplCfgMaCb fpCfgMaCb_p,
+					  tEplCfgMaDcfTyp DcfType_p);
+
+//---------------------------------------------------------------------------
+// Function:    EplCfgMaStartConfigNodeDcf()
+//
+// Description: Function starts the configuration process
+//              and links the configuration data to the OD
+//
+// Parameters:  uiNodeId_p              = NodeId of the node to configure
+//              pbConcise_p             = pointer to DCF
+//              fpCfgMaCb_p             = pointer to callback function (should not be NULL)
+//              SizeOfConcise_p         = size of DCF in BYTE -> for future use
+//              DcfType_p               = type of the DCF
+//
+// Returns:     tCopKernel              = error code
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplCfgMaStartConfigNodeDcf(unsigned int uiNodeId_p,
+					     BYTE * pbConcise_p,
+					     tfpEplCfgMaCb fpCfgMaCb_p,
+					     tEplObdSize SizeOfConcise_p,
+					     tEplCfgMaDcfTyp DcfType_p);
+
+//---------------------------------------------------------------------------
+// Function:    EplCfgMaLinkDcf()
+//
+// Description: Function links the configuration data to the OD
+//
+// Parameters:  uiNodeId_p              = NodeId of the node to configure
+//              pbConcise_p             = pointer to DCF
+//              SizeOfConcise_p        = size of DCF in BYTE -> for future use
+//              DcfType_p               = type of the DCF
+//
+// Returns:     tCopKernel              = error code
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplCfgMaLinkDcf(unsigned int uiNodeId_p,
+				  BYTE * pbConcise_p,
+				  tEplObdSize SizeOfConcise_p,
+				  tEplCfgMaDcfTyp DcfType_p);
+
+//---------------------------------------------------------------------------
+// Function:    EplCfgMaCheckDcf()
+//
+// Description: Function check if there is allready a configuration file linked
+//              to the OD (type is given by DcfType_p)
+//
+// Parameters:  uiNodeId_p              = NodeId
+//              DcfType_p               = type of the DCF
+//
+// Returns:     tCopKernel              = error code
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplCfgMaCheckDcf(unsigned int uiNodeId_p,
+				   tEplCfgMaDcfTyp DcfType_p);
+
+#endif // #if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_CFGMA)) != 0)
+
+#endif // _EPLCFGMA_H_
+
+// EOF
diff --git a/drivers/staging/epl/user/EplDllu.h b/drivers/staging/epl/user/EplDllu.h
new file mode 100644
index 00000000000..36f8bb76f7c
--- /dev/null
+++ b/drivers/staging/epl/user/EplDllu.h
@@ -0,0 +1,108 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for userspace DLL module for asynchronous communication
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplDllu.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/20 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_DLLU_H_
+#define _EPL_DLLU_H_
+
+#include "../EplDll.h"
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+typedef tEplKernel(PUBLIC * tEplDlluCbAsnd) (tEplFrameInfo * pFrameInfo_p);
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLU)) != 0)
+
+tEplKernel EplDlluAddInstance(void);
+
+tEplKernel EplDlluDelInstance(void);
+
+tEplKernel EplDlluRegAsndService(tEplDllAsndServiceId ServiceId_p,
+				 tEplDlluCbAsnd pfnDlluCbAsnd_p,
+				 tEplDllAsndFilter Filter_p);
+
+tEplKernel EplDlluAsyncSend(tEplFrameInfo * pFrameInfo_p,
+			    tEplDllAsyncReqPriority Priority_p);
+
+// processes asynch frames
+tEplKernel EplDlluProcess(tEplFrameInfo * pFrameInfo_p);
+
+#endif // #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_DLLU)) != 0)
+
+#endif // #ifndef _EPL_DLLU_H_
diff --git a/drivers/staging/epl/user/EplDlluCal.h b/drivers/staging/epl/user/EplDlluCal.h
new file mode 100644
index 00000000000..b1dcd060957
--- /dev/null
+++ b/drivers/staging/epl/user/EplDlluCal.h
@@ -0,0 +1,117 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for DLL Communication Abstraction Layer module in EPL user part
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplDlluCal.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/20 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_DLLUCAL_H_
+#define _EPL_DLLUCAL_H_
+
+#include "../EplDll.h"
+#include "../EplEvent.h"
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+typedef tEplKernel(PUBLIC * tEplDlluCbAsnd) (tEplFrameInfo * pFrameInfo_p);
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+tEplKernel EplDlluCalAddInstance(void);
+
+tEplKernel EplDlluCalDelInstance(void);
+
+tEplKernel EplDlluCalRegAsndService(tEplDllAsndServiceId ServiceId_p,
+				    tEplDlluCbAsnd pfnDlluCbAsnd_p,
+				    tEplDllAsndFilter Filter_p);
+
+tEplKernel EplDlluCalAsyncSend(tEplFrameInfo * pFrameInfo,
+			       tEplDllAsyncReqPriority Priority_p);
+
+tEplKernel EplDlluCalProcess(tEplEvent * pEvent_p);
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+tEplKernel EplDlluCalAddNode(tEplDllNodeInfo * pNodeInfo_p);
+
+tEplKernel EplDlluCalDeleteNode(unsigned int uiNodeId_p);
+
+tEplKernel EplDlluCalSoftDeleteNode(unsigned int uiNodeId_p);
+
+tEplKernel EplDlluCalIssueRequest(tEplDllReqServiceId Service_p,
+				  unsigned int uiNodeId_p, BYTE bSoaFlag1_p);
+
+#endif
+
+#endif // #ifndef _EPL_DLLUCAL_H_
diff --git a/drivers/staging/epl/user/EplEventu.h b/drivers/staging/epl/user/EplEventu.h
new file mode 100644
index 00000000000..322cffd1187
--- /dev/null
+++ b/drivers/staging/epl/user/EplEventu.h
@@ -0,0 +1,108 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for kernel event module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplEventu.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/12 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_EVENTU_H_
+#define _EPL_EVENTU_H_
+
+#include "../EplEvent.h"
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+// init function
+tEplKernel PUBLIC EplEventuInit(tEplProcessEventCb pfnApiProcessEventCb_p);
+
+// add instance
+tEplKernel PUBLIC EplEventuAddInstance(tEplProcessEventCb
+				       pfnApiProcessEventCb_p);
+
+// delete instance
+tEplKernel PUBLIC EplEventuDelInstance(void);
+
+// Task that dispatches events in userspace
+tEplKernel PUBLIC EplEventuProcess(tEplEvent * pEvent_p);
+
+// post events from userspace
+tEplKernel PUBLIC EplEventuPost(tEplEvent * pEvent_p);
+
+// post errorevents from userspace
+tEplKernel PUBLIC EplEventuPostError(tEplEventSource EventSource_p,
+				     tEplKernel EplError_p,
+				     unsigned int uiArgSize_p, void *pArg_p);
+
+#endif // #ifndef _EPL_EVENTU_H_
diff --git a/drivers/staging/epl/user/EplIdentu.h b/drivers/staging/epl/user/EplIdentu.h
new file mode 100644
index 00000000000..e7302106c74
--- /dev/null
+++ b/drivers/staging/epl/user/EplIdentu.h
@@ -0,0 +1,108 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for Identu-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplIdentu.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.3 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/11/15 d.k.:   start of the implementation
+
+****************************************************************************/
+
+#include "../EplDll.h"
+
+#ifndef _EPLIDENTU_H_
+#define _EPLIDENTU_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+typedef tEplKernel(PUBLIC * tEplIdentuCbResponse) (unsigned int uiNodeId_p,
+						   tEplIdentResponse *
+						   pIdentResponse_p);
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplIdentuInit(void);
+
+tEplKernel PUBLIC EplIdentuAddInstance(void);
+
+tEplKernel PUBLIC EplIdentuDelInstance(void);
+
+tEplKernel PUBLIC EplIdentuReset(void);
+
+tEplKernel PUBLIC EplIdentuGetIdentResponse(unsigned int uiNodeId_p,
+					    tEplIdentResponse **
+					    ppIdentResponse_p);
+
+tEplKernel PUBLIC EplIdentuRequestIdentResponse(unsigned int uiNodeId_p,
+						tEplIdentuCbResponse
+						pfnCbResponse_p);
+
+#endif // #ifndef _EPLIDENTU_H_
diff --git a/drivers/staging/epl/user/EplLedu.h b/drivers/staging/epl/user/EplLedu.h
new file mode 100644
index 00000000000..78e70d06445
--- /dev/null
+++ b/drivers/staging/epl/user/EplLedu.h
@@ -0,0 +1,109 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for status and error LED user part module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplLedu.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.1 $  $Date: 2008/11/17 16:40:39 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2008/11/17 d.k.:   start of the implementation
+
+****************************************************************************/
+
+#include "../EplLed.h"
+#include "../EplNmt.h"
+#include "EplEventu.h"
+
+#ifndef _EPLLEDU_H_
+#define _EPLLEDU_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+typedef tEplKernel(PUBLIC * tEplLeduStateChangeCallback) (tEplLedType LedType_p,
+							  BOOL fOn_p);
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_LEDU)) != 0)
+
+tEplKernel PUBLIC EplLeduInit(tEplLeduStateChangeCallback pfnCbStateChange_p);
+
+tEplKernel PUBLIC EplLeduAddInstance(tEplLeduStateChangeCallback
+				     pfnCbStateChange_p);
+
+tEplKernel PUBLIC EplLeduDelInstance(void);
+
+tEplKernel PUBLIC EplLeduCbNmtStateChange(tEplEventNmtStateChange
+					  NmtStateChange_p);
+
+tEplKernel PUBLIC EplLeduProcessEvent(tEplEvent * pEplEvent_p);
+
+#endif // #if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_LEDU)) != 0)
+
+#endif // #ifndef _EPLLEDU_H_
diff --git a/drivers/staging/epl/user/EplNmtCnu.h b/drivers/staging/epl/user/EplNmtCnu.h
new file mode 100644
index 00000000000..e508055b5b8
--- /dev/null
+++ b/drivers/staging/epl/user/EplNmtCnu.h
@@ -0,0 +1,108 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for NMT-CN-Userspace-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplNmtCnu.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/09 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "EplNmtu.h"
+#include "../EplDll.h"
+#include "../EplFrame.h"
+
+#ifndef _EPLNMTCNU_H_
+#define _EPLNMTCNU_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_CN)) != 0)
+
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtCnuInit(unsigned int uiNodeId_p);
+
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtCnuAddInstance(unsigned int uiNodeId_p);
+
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtCnuDelInstance(void);
+
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtCnuSendNmtRequest(unsigned int uiNodeId_p,
+						       tEplNmtCommand
+						       NmtCommand_p);
+
+EPLDLLEXPORT tEplKernel PUBLIC
+EplNmtCnuRegisterCheckEventCb(tEplNmtuCheckEventCallback
+			      pfnEplNmtCheckEventCb_p);
+
+#endif // #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_CN)) != 0)
+
+#endif // #ifndef _EPLNMTCNU_H_
diff --git a/drivers/staging/epl/user/EplNmtMnu.h b/drivers/staging/epl/user/EplNmtMnu.h
new file mode 100644
index 00000000000..c54efeba303
--- /dev/null
+++ b/drivers/staging/epl/user/EplNmtMnu.h
@@ -0,0 +1,131 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for NMT-MN-Userspace-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplNmtMnu.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.6 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/09 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "EplNmtu.h"
+
+#ifndef _EPLNMTMNU_H_
+#define _EPLNMTMNU_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+typedef tEplKernel(PUBLIC * tEplNmtMnuCbNodeEvent) (unsigned int uiNodeId_p,
+						    tEplNmtNodeEvent
+						    NodeEvent_p,
+						    tEplNmtState NmtState_p,
+						    WORD wErrorCode_p,
+						    BOOL fMandatory_p);
+
+typedef tEplKernel(PUBLIC *
+		   tEplNmtMnuCbBootEvent) (tEplNmtBootEvent BootEvent_p,
+					   tEplNmtState NmtState_p,
+					   WORD wErrorCode_p);
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMT_MN)) != 0)
+
+tEplKernel EplNmtMnuInit(tEplNmtMnuCbNodeEvent pfnCbNodeEvent_p,
+			 tEplNmtMnuCbBootEvent pfnCbBootEvent_p);
+
+tEplKernel EplNmtMnuAddInstance(tEplNmtMnuCbNodeEvent pfnCbNodeEvent_p,
+				tEplNmtMnuCbBootEvent pfnCbBootEvent_p);
+
+tEplKernel EplNmtMnuDelInstance(void);
+
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtMnuProcessEvent(tEplEvent * pEvent_p);
+
+tEplKernel EplNmtMnuSendNmtCommand(unsigned int uiNodeId_p,
+				   tEplNmtCommand NmtCommand_p);
+
+tEplKernel EplNmtMnuTriggerStateChange(unsigned int uiNodeId_p,
+				       tEplNmtNodeCommand NodeCommand_p);
+
+tEplKernel PUBLIC EplNmtMnuCbNmtStateChange(tEplEventNmtStateChange
+					    NmtStateChange_p);
+
+tEplKernel PUBLIC EplNmtMnuCbCheckEvent(tEplNmtEvent NmtEvent_p);
+
+tEplKernel PUBLIC EplNmtMnuGetDiagnosticInfo(unsigned int
+					     *puiMandatorySlaveCount_p,
+					     unsigned int
+					     *puiSignalSlaveCount_p,
+					     WORD * pwFlags_p);
+
+#endif
+
+#endif // #ifndef _EPLNMTMNU_H_
diff --git a/drivers/staging/epl/user/EplNmtu.h b/drivers/staging/epl/user/EplNmtu.h
new file mode 100644
index 00000000000..5a56c583060
--- /dev/null
+++ b/drivers/staging/epl/user/EplNmtu.h
@@ -0,0 +1,155 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for NMT-Userspace-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplNmtu.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/09 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "../EplNmt.h"
+#include "EplEventu.h"
+
+#ifndef _EPLNMTU_H_
+#define _EPLNMTU_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+// nmt commands
+typedef enum {
+	// requestable ASnd ServiceIds    0x01..0x1F
+	kEplNmtCmdIdentResponse = 0x01,
+	kEplNmtCmdStatusResponse = 0x02,
+	// plain NMT state commands       0x20..0x3F
+	kEplNmtCmdStartNode = 0x21,
+	kEplNmtCmdStopNode = 0x22,
+	kEplNmtCmdEnterPreOperational2 = 0x23,
+	kEplNmtCmdEnableReadyToOperate = 0x24,
+	kEplNmtCmdResetNode = 0x28,
+	kEplNmtCmdResetCommunication = 0x29,
+	kEplNmtCmdResetConfiguration = 0x2A,
+	kEplNmtCmdSwReset = 0x2B,
+	// extended NMT state commands    0x40..0x5F
+	kEplNmtCmdStartNodeEx = 0x41,
+	kEplNmtCmdStopNodeEx = 0x42,
+	kEplNmtCmdEnterPreOperational2Ex = 0x43,
+	kEplNmtCmdEnableReadyToOperateEx = 0x44,
+	kEplNmtCmdResetNodeEx = 0x48,
+	kEplNmtCmdResetCommunicationEx = 0x49,
+	kEplNmtCmdResetConfigurationEx = 0x4A,
+	kEplNmtCmdSwResetEx = 0x4B,
+	// NMT managing commands          0x60..0x7F
+	kEplNmtCmdNetHostNameSet = 0x62,
+	kEplNmtCmdFlushArpEntry = 0x63,
+	// NMT info services              0x80..0xBF
+	kEplNmtCmdPublishConfiguredCN = 0x80,
+	kEplNmtCmdPublishActiveCN = 0x90,
+	kEplNmtCmdPublishPreOperational1 = 0x91,
+	kEplNmtCmdPublishPreOperational2 = 0x92,
+	kEplNmtCmdPublishReadyToOperate = 0x93,
+	kEplNmtCmdPublishOperational = 0x94,
+	kEplNmtCmdPublishStopped = 0x95,
+	kEplNmtCmdPublishEmergencyNew = 0xA0,
+	kEplNmtCmdPublishTime = 0xB0,
+
+	kEplNmtCmdInvalidService = 0xFF
+} tEplNmtCommand;
+
+typedef tEplKernel(PUBLIC *
+		   tEplNmtuStateChangeCallback) (tEplEventNmtStateChange
+						 NmtStateChange_p);
+
+typedef tEplKernel(PUBLIC *
+		   tEplNmtuCheckEventCallback) (tEplNmtEvent NmtEvent_p);
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTU)) != 0)
+
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtuInit(void);
+
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtuAddInstance(void);
+
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtuDelInstance(void);
+
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtuNmtEvent(tEplNmtEvent NmtEvent_p);
+
+EPLDLLEXPORT tEplNmtState PUBLIC EplNmtuGetNmtState(void);
+
+EPLDLLEXPORT tEplKernel PUBLIC EplNmtuProcessEvent(tEplEvent * pEplEvent_p);
+
+EPLDLLEXPORT tEplKernel PUBLIC
+EplNmtuRegisterStateChangeCb(tEplNmtuStateChangeCallback
+			     pfnEplNmtStateChangeCb_p);
+
+#endif // #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_NMTU)) != 0)
+
+#endif // #ifndef _EPLNMTU_H_
diff --git a/drivers/staging/epl/user/EplNmtuCal.h b/drivers/staging/epl/user/EplNmtuCal.h
new file mode 100644
index 00000000000..c881582702b
--- /dev/null
+++ b/drivers/staging/epl/user/EplNmtuCal.h
@@ -0,0 +1,91 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for communication abstraction layer of the
+                NMT-Userspace-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplNmtuCal.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/16 -k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "EplNmtu.h"
+#include "../kernel/EplNmtk.h"
+
+#ifndef _EPLNMTUCAL_H_
+#define _EPLNMTUCAL_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplNmtState PUBLIC EplNmtkCalGetNmtState(void);
+
+#endif // #ifndef _EPLNMTUCAL_H_
diff --git a/drivers/staging/epl/user/EplObdu.h b/drivers/staging/epl/user/EplObdu.h
new file mode 100644
index 00000000000..bc1e1730301
--- /dev/null
+++ b/drivers/staging/epl/user/EplObdu.h
@@ -0,0 +1,192 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for Epl-Obd-Userspace-module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplObdu.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.6 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/19 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "../EplObd.h"
+
+#ifndef _EPLOBDU_H_
+#define _EPLOBDU_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0)
+
+#if EPL_OBD_USE_KERNEL != FALSE
+#error "EPL OBDu module enabled, but OBD_USE_KERNEL == TRUE"
+#endif
+
+EPLDLLEXPORT tEplKernel PUBLIC EplObduWriteEntry(unsigned int uiIndex_p,
+						 unsigned int uiSubIndex_p,
+						 void *pSrcData_p,
+						 tEplObdSize Size_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduReadEntry(unsigned int uiIndex_p,
+						unsigned int uiSubIndex_p,
+						void *pDstData_p,
+						tEplObdSize * pSize_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduAccessOdPart(tEplObdPart ObdPart_p,
+						   tEplObdDir Direction_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduDefineVar(tEplVarParam MEM * pVarParam_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT void *PUBLIC EplObduGetObjectDataPtr(unsigned int uiIndex_p,
+						  unsigned int uiSubIndex_p);
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduRegisterUserOd(tEplObdEntryPtr pUserOd_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT void PUBLIC EplObduInitVarEntry(tEplObdVarEntry MEM * pVarEntry_p,
+					     BYTE bType_p,
+					     tEplObdSize ObdSize_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplObdSize PUBLIC EplObduGetDataSize(unsigned int uiIndex_p,
+						   unsigned int uiSubIndex_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT unsigned int PUBLIC EplObduGetNodeId(void);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduSetNodeId(unsigned int uiNodeId_p,
+						tEplObdNodeIdType NodeIdType_p);
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduGetAccessType(unsigned int uiIndex_p,
+						    unsigned int uiSubIndex_p,
+						    tEplObdAccess *
+						    pAccessTyp_p);
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduReadEntryToLe(unsigned int uiIndex_p,
+						    unsigned int uiSubIndex_p,
+						    void *pDstData_p,
+						    tEplObdSize * pSize_p);
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduWriteEntryFromLe(unsigned int uiIndex_p,
+						       unsigned int
+						       uiSubIndex_p,
+						       void *pSrcData_p,
+						       tEplObdSize Size_p);
+
+// ---------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduSearchVarEntry(EPL_MCO_DECL_INSTANCE_PTR_
+						     unsigned int uiIndex_p,
+						     unsigned int uiSubindex_p,
+						     tEplObdVarEntry MEM **
+						     ppVarEntry_p);
+
+#elif EPL_OBD_USE_KERNEL != FALSE
+#include "../kernel/EplObdk.h"
+
+#define EplObduWriteEntry       EplObdWriteEntry
+
+#define EplObduReadEntry        EplObdReadEntry
+
+#define EplObduAccessOdPart     EplObdAccessOdPart
+
+#define EplObduDefineVar        EplObdDefineVar
+
+#define EplObduGetObjectDataPtr EplObdGetObjectDataPtr
+
+#define EplObduRegisterUserOd   EplObdRegisterUserOd
+
+#define EplObduInitVarEntry     EplObdInitVarEntry
+
+#define EplObduGetDataSize      EplObdGetDataSize
+
+#define EplObduGetNodeId        EplObdGetNodeId
+
+#define EplObduSetNodeId        EplObdSetNodeId
+
+#define EplObduGetAccessType    EplObdGetAccessType
+
+#define EplObduReadEntryToLe    EplObdReadEntryToLe
+
+#define EplObduWriteEntryFromLe EplObdWriteEntryFromLe
+
+#define EplObduSearchVarEntry   EplObdSearchVarEntry
+
+#define EplObduIsNumerical      EplObdIsNumerical
+
+#endif // #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_OBDU)) != 0)
+
+#endif // #ifndef _EPLOBDU_H_
diff --git a/drivers/staging/epl/user/EplObduCal.h b/drivers/staging/epl/user/EplObduCal.h
new file mode 100644
index 00000000000..498e0112fac
--- /dev/null
+++ b/drivers/staging/epl/user/EplObduCal.h
@@ -0,0 +1,148 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for communication abstraction layer
+                for the Epl-Obd-Userspace-Modul
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplObduCal.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/19 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "../EplObd.h"
+
+#ifndef _EPLOBDUCAL_H_
+#define _EPLOBDUCAL_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduCalWriteEntry(unsigned int uiIndex_p,
+						    unsigned int uiSubIndex_p,
+						    void *pSrcData_p,
+						    tEplObdSize Size_p);
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduCalReadEntry(unsigned int uiIndex_p,
+						   unsigned int uiSubIndex_p,
+						   void *pDstData_p,
+						   tEplObdSize * pSize_p);
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduCalAccessOdPart(tEplObdPart ObdPart_p,
+						      tEplObdDir Direction_p);
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduCalDefineVar(tEplVarParam MEM *
+						   pVarParam_p);
+//---------------------------------------------------------------------------
+EPLDLLEXPORT void *PUBLIC EplObduCalGetObjectDataPtr(unsigned int uiIndex_p,
+						     unsigned int uiSubIndex_p);
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduCalRegisterUserOd(tEplObdEntryPtr
+							pUserOd_p);
+//---------------------------------------------------------------------------
+EPLDLLEXPORT void PUBLIC EplObduCalInitVarEntry(tEplObdVarEntry MEM *
+						pVarEntry_p, BYTE bType_p,
+						tEplObdSize ObdSize_p);
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplObdSize PUBLIC EplObduCalGetDataSize(unsigned int uiIndex_p,
+						      unsigned int
+						      uiSubIndex_p);
+//---------------------------------------------------------------------------
+EPLDLLEXPORT unsigned int PUBLIC EplObduCalGetNodeId(void);
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduCalSetNodeId(unsigned int uiNodeId_p,
+						   tEplObdNodeIdType
+						   NodeIdType_p);
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduCalGetAccessType(unsigned int uiIndex_p,
+						       unsigned int
+						       uiSubIndex_p,
+						       tEplObdAccess *
+						       pAccessTyp_p);
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduCalReadEntryToLe(unsigned int uiIndex_p,
+						       unsigned int
+						       uiSubIndex_p,
+						       void *pDstData_p,
+						       tEplObdSize * pSize_p);
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC EplObduCalWriteEntryFromLe(unsigned int
+							  uiIndex_p,
+							  unsigned int
+							  uiSubIndex_p,
+							  void *pSrcData_p,
+							  tEplObdSize Size_p);
+//---------------------------------------------------------------------------
+EPLDLLEXPORT tEplKernel PUBLIC
+EplObduCalSearchVarEntry(EPL_MCO_DECL_INSTANCE_PTR_ unsigned int uiIndex_p,
+			 unsigned int uiSubindex_p,
+			 tEplObdVarEntry MEM ** ppVarEntry_p);
+
+#endif // #ifndef _EPLOBDUCAL_H_
diff --git a/drivers/staging/epl/user/EplPdou.h b/drivers/staging/epl/user/EplPdou.h
new file mode 100644
index 00000000000..11de4862e8b
--- /dev/null
+++ b/drivers/staging/epl/user/EplPdou.h
@@ -0,0 +1,108 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for userspace PDO module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplPdou.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/11/19 17:14:38 $
+
+                $State: Exp $
+
+                Build Environment:
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/05/22 d.k.:   start of the implementation, version 1.00
+
+****************************************************************************/
+
+#ifndef _EPL_PDOU_H_
+#define _EPL_PDOU_H_
+
+#include "../EplPdo.h"
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+tEplKernel EplPdouAddInstance(void);
+
+tEplKernel EplPdouDelInstance(void);
+
+#if (((EPL_MODULE_INTEGRATION) & (EPL_MODULE_PDOU)) != 0)
+tEplKernel PUBLIC EplPdouCbObdAccess(tEplObdCbParam MEM * pParam_p);
+#else
+#define EplPdouCbObdAccess		NULL
+#endif
+
+// returns error if bPdoId_p is already valid
+/*
+tEplKernel EplPdouSetMapping(
+    BYTE bPdoId_p, BOOL fTxRx_p, BYTE bNodeId, BYTE bMappingVersion,
+    tEplPdoMapping * pMapping_p, BYTE bMaxEntries_p);
+
+tEplKernel EplPdouGetMapping(
+    BYTE bPdoId_p, BOOL fTxRx_p, BYTE * pbNodeId, BYTE * pbMappingVersion,
+    tEplPdoMapping * pMapping_p, BYTE * pbMaxEntries_p);
+*/
+
+#endif // #ifndef _EPL_PDOU_H_
diff --git a/drivers/staging/epl/user/EplSdoAsndu.h b/drivers/staging/epl/user/EplSdoAsndu.h
new file mode 100644
index 00000000000..e34959f4279
--- /dev/null
+++ b/drivers/staging/epl/user/EplSdoAsndu.h
@@ -0,0 +1,107 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for SDO/Asnd-Protocolabstractionlayer module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplSdoAsndu.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.6 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/07/07 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "../EplSdo.h"
+#include "../EplDll.h"
+
+#ifndef _EPLSDOASNDU_H_
+#define _EPLSDOASNDU_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_ASND)) != 0)
+
+tEplKernel PUBLIC EplSdoAsnduInit(tEplSequLayerReceiveCb fpReceiveCb_p);
+
+tEplKernel PUBLIC EplSdoAsnduAddInstance(tEplSequLayerReceiveCb fpReceiveCb_p);
+
+tEplKernel PUBLIC EplSdoAsnduDelInstance(void);
+
+tEplKernel PUBLIC EplSdoAsnduInitCon(tEplSdoConHdl * pSdoConHandle_p,
+				     unsigned int uiTargetNodeId_p);
+
+tEplKernel PUBLIC EplSdoAsnduSendData(tEplSdoConHdl SdoConHandle_p,
+				      tEplFrame * pSrcData_p,
+				      DWORD dwDataSize_p);
+
+tEplKernel PUBLIC EplSdoAsnduDelCon(tEplSdoConHdl SdoConHandle_p);
+
+#endif // end of #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_ASND)) != 0)
+
+#endif // #ifndef _EPLSDOASNDU_H_
diff --git a/drivers/staging/epl/user/EplSdoAsySequ.h b/drivers/staging/epl/user/EplSdoAsySequ.h
new file mode 100644
index 00000000000..4658b5f8c53
--- /dev/null
+++ b/drivers/staging/epl/user/EplSdoAsySequ.h
@@ -0,0 +1,111 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for asychrionus SDO Sequence Layer module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplSdoAsySequ.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.4 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/26 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "../EplSdo.h"
+#include "EplSdoUdpu.h"
+#include "EplSdoAsndu.h"
+#include "../EplEvent.h"
+#include "EplTimeru.h"
+
+#ifndef _EPLSDOASYSEQU_H_
+#define _EPLSDOASYSEQU_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoAsySeqInit(tEplSdoComReceiveCb fpSdoComCb_p,
+				   tEplSdoComConCb fpSdoComConCb_p);
+
+tEplKernel PUBLIC EplSdoAsySeqAddInstance(tEplSdoComReceiveCb fpSdoComCb_p,
+					  tEplSdoComConCb fpSdoComConCb_p);
+
+tEplKernel PUBLIC EplSdoAsySeqDelInstance(void);
+
+tEplKernel PUBLIC EplSdoAsySeqInitCon(tEplSdoSeqConHdl * pSdoSeqConHdl_p,
+				      unsigned int uiNodeId_p,
+				      tEplSdoType SdoType);
+
+tEplKernel PUBLIC EplSdoAsySeqSendData(tEplSdoSeqConHdl SdoSeqConHdl_p,
+				       unsigned int uiDataSize_p,
+				       tEplFrame * pData_p);
+
+tEplKernel PUBLIC EplSdoAsySeqProcessEvent(tEplEvent * pEvent_p);
+
+tEplKernel PUBLIC EplSdoAsySeqDelCon(tEplSdoSeqConHdl SdoSeqConHdl_p);
+
+#endif // #ifndef _EPLSDOASYSEQU_H_
diff --git a/drivers/staging/epl/user/EplSdoComu.h b/drivers/staging/epl/user/EplSdoComu.h
new file mode 100644
index 00000000000..3e454c7a355
--- /dev/null
+++ b/drivers/staging/epl/user/EplSdoComu.h
@@ -0,0 +1,126 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for SDO Command Layer module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplSdoComu.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/26 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "../EplSdo.h"
+#include "../EplObd.h"
+#include "../EplSdoAc.h"
+#include "EplObdu.h"
+#include "EplSdoAsySequ.h"
+
+#ifndef _EPLSDOCOMU_H_
+#define _EPLSDOCOMU_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+tEplKernel PUBLIC EplSdoComInit(void);
+
+tEplKernel PUBLIC EplSdoComAddInstance(void);
+
+tEplKernel PUBLIC EplSdoComDelInstance(void);
+
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDOC)) != 0)
+
+tEplKernel PUBLIC EplSdoComDefineCon(tEplSdoComConHdl * pSdoComConHdl_p,
+				     unsigned int uiTargetNodeId_p,
+				     tEplSdoType ProtType_p);
+
+tEplKernel PUBLIC EplSdoComInitTransferByIndex(tEplSdoComTransParamByIndex *
+					       pSdoComTransParam_p);
+
+tEplKernel PUBLIC EplSdoComUndefineCon(tEplSdoComConHdl SdoComConHdl_p);
+
+tEplKernel PUBLIC EplSdoComGetState(tEplSdoComConHdl SdoComConHdl_p,
+				    tEplSdoComFinished * pSdoComFinished_p);
+
+tEplKernel PUBLIC EplSdoComSdoAbort(tEplSdoComConHdl SdoComConHdl_p,
+				    DWORD dwAbortCode_p);
+
+#endif
+
+// for future extention
+/*
+tEplKernel PUBLIC EplSdoComInitTransferAllByIndex(tEplSdoComTransParamAllByIndex* pSdoComTransParam_p);
+
+tEplKernel PUBLIC EplSdoComInitTransferByName(tEplSdoComTransParamByName* pSdoComTransParam_p);
+
+tEplKernel PUBLIC EplSdoComInitTransferFile(tEplSdoComTransParamFile* pSdoComTransParam_p);
+
+*/
+
+#endif // #ifndef _EPLSDOCOMU_H_
diff --git a/drivers/staging/epl/user/EplSdoUdpu.h b/drivers/staging/epl/user/EplSdoUdpu.h
new file mode 100644
index 00000000000..2d77b6fff19
--- /dev/null
+++ b/drivers/staging/epl/user/EplSdoUdpu.h
@@ -0,0 +1,109 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for SDO/UDP-Protocollabstractionlayer module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplSdoUdpu.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/10/17 15:32:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/06/26 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "../EplSdo.h"
+
+#ifndef _EPLSDOUDPU_H_
+#define _EPLSDOUDPU_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+#if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) != 0)
+
+tEplKernel PUBLIC EplSdoUdpuInit(tEplSequLayerReceiveCb fpReceiveCb_p);
+
+tEplKernel PUBLIC EplSdoUdpuAddInstance(tEplSequLayerReceiveCb fpReceiveCb_p);
+
+tEplKernel PUBLIC EplSdoUdpuDelInstance(void);
+
+tEplKernel PUBLIC EplSdoUdpuConfig(unsigned long ulIpAddr_p,
+				   unsigned int uiPort_p);
+
+tEplKernel PUBLIC EplSdoUdpuInitCon(tEplSdoConHdl * pSdoConHandle_p,
+				    unsigned int uiTargetNodeId_p);
+
+tEplKernel PUBLIC EplSdoUdpuSendData(tEplSdoConHdl SdoConHandle_p,
+				     tEplFrame * pSrcData_p,
+				     DWORD dwDataSize_p);
+
+tEplKernel PUBLIC EplSdoUdpuDelCon(tEplSdoConHdl SdoConHandle_p);
+
+#endif // end of #if(((EPL_MODULE_INTEGRATION) & (EPL_MODULE_SDO_UDP)) != 0)
+
+#endif // #ifndef _EPLSDOUDPU_H_
diff --git a/drivers/staging/epl/user/EplStatusu.h b/drivers/staging/epl/user/EplStatusu.h
new file mode 100644
index 00000000000..d211935f0e8
--- /dev/null
+++ b/drivers/staging/epl/user/EplStatusu.h
@@ -0,0 +1,104 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for Statusu-Module
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplStatusu.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.3 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/11/15 d.k.:   start of the implementation
+
+****************************************************************************/
+
+#include "../EplDll.h"
+
+#ifndef _EPLSTATUSU_H_
+#define _EPLSTATUSU_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+typedef tEplKernel(PUBLIC * tEplStatusuCbResponse) (unsigned int uiNodeId_p,
+						    tEplStatusResponse *
+						    pStatusResponse_p);
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplStatusuInit(void);
+
+tEplKernel PUBLIC EplStatusuAddInstance(void);
+
+tEplKernel PUBLIC EplStatusuDelInstance(void);
+
+tEplKernel PUBLIC EplStatusuReset(void);
+
+tEplKernel PUBLIC EplStatusuRequestStatusResponse(unsigned int uiNodeId_p,
+						  tEplStatusuCbResponse
+						  pfnCbResponse_p);
+
+#endif // #ifndef _EPLSTATUSU_H_
diff --git a/drivers/staging/epl/user/EplTimeru.h b/drivers/staging/epl/user/EplTimeru.h
new file mode 100644
index 00000000000..404495501b8
--- /dev/null
+++ b/drivers/staging/epl/user/EplTimeru.h
@@ -0,0 +1,107 @@
+/****************************************************************************
+
+  (c) SYSTEC electronic GmbH, D-07973 Greiz, August-Bebel-Str. 29
+      www.systec-electronic.com
+
+  Project:      openPOWERLINK
+
+  Description:  include file for Epl Userspace-Timermodule
+
+  License:
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions
+    are met:
+
+    1. Redistributions of source code must retain the above copyright
+       notice, this list of conditions and the following disclaimer.
+
+    2. Redistributions in binary form must reproduce the above copyright
+       notice, this list of conditions and the following disclaimer in the
+       documentation and/or other materials provided with the distribution.
+
+    3. Neither the name of SYSTEC electronic GmbH nor the names of its
+       contributors may be used to endorse or promote products derived
+       from this software without prior written permission. For written
+       permission, please contact info@systec-electronic.com.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+    COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+    POSSIBILITY OF SUCH DAMAGE.
+
+    Severability Clause:
+
+        If a provision of this License is or becomes illegal, invalid or
+        unenforceable in any jurisdiction, that shall not affect:
+        1. the validity or enforceability in that jurisdiction of any other
+           provision of this License; or
+        2. the validity or enforceability in other jurisdictions of that or
+           any other provision of this License.
+
+  -------------------------------------------------------------------------
+
+                $RCSfile: EplTimeru.h,v $
+
+                $Author: D.Krueger $
+
+                $Revision: 1.5 $  $Date: 2008/04/17 21:36:32 $
+
+                $State: Exp $
+
+                Build Environment:
+                    GCC V3.4
+
+  -------------------------------------------------------------------------
+
+  Revision History:
+
+  2006/07/06 k.t.:   start of the implementation
+
+****************************************************************************/
+
+#include "../EplTimer.h"
+#include "EplEventu.h"
+
+#ifndef _EPLTIMERU_H_
+#define _EPLTIMERU_H_
+
+//---------------------------------------------------------------------------
+// const defines
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// typedef
+//---------------------------------------------------------------------------
+
+//---------------------------------------------------------------------------
+// function prototypes
+//---------------------------------------------------------------------------
+
+tEplKernel PUBLIC EplTimeruInit(void);
+
+tEplKernel PUBLIC EplTimeruAddInstance(void);
+
+tEplKernel PUBLIC EplTimeruDelInstance(void);
+
+tEplKernel PUBLIC EplTimeruSetTimerMs(tEplTimerHdl * pTimerHdl_p,
+				      unsigned long ulTime_p,
+				      tEplTimerArg Argument_p);
+
+tEplKernel PUBLIC EplTimeruModifyTimerMs(tEplTimerHdl * pTimerHdl_p,
+					 unsigned long ulTime_p,
+					 tEplTimerArg Argument_p);
+
+tEplKernel PUBLIC EplTimeruDeleteTimer(tEplTimerHdl * pTimerHdl_p);
+
+BOOL PUBLIC EplTimeruIsTimerActive(tEplTimerHdl TimerHdl_p);
+
+#endif // #ifndef _EPLTIMERU_H_
diff --git a/drivers/staging/et131x/et1310_tx.c b/drivers/staging/et131x/et1310_tx.c
index a95c2608a0c..30eaac4c870 100644
--- a/drivers/staging/et131x/et1310_tx.c
+++ b/drivers/staging/et131x/et1310_tx.c
@@ -1345,7 +1345,6 @@ void et131x_free_busy_send_packets(struct et131x_adapter *pAdapter)
 {
 	PMP_TCB pMpTcb;
 	struct list_head *pEntry;
-	struct sk_buff *pPacket = NULL;
 	unsigned long lockflags;
 	uint32_t FreeCounter = 0;
 
@@ -1358,8 +1357,6 @@ void et131x_free_busy_send_packets(struct et131x_adapter *pAdapter)
 		spin_unlock_irqrestore(&pAdapter->SendWaitLock, lockflags);
 
 		pEntry = pAdapter->TxRing.SendWaitQueue.next;
-
-		pPacket = NULL;
 	}
 
 	pAdapter->TxRing.nWaitSend = 0;
diff --git a/drivers/staging/et131x/et131x_debug.h b/drivers/staging/et131x/et131x_debug.h
index dab608031d0..994108eca66 100644
--- a/drivers/staging/et131x/et131x_debug.h
+++ b/drivers/staging/et131x/et131x_debug.h
@@ -82,11 +82,11 @@
 #define DBG_LVL	3
 #endif /* DBG_LVL */
 
-#define DBG_DEFAULTS		(DBG_ERROR_ON | DBG_WARNING_ON | DBG_BREAK_ON )
+#define DBG_DEFAULTS		(DBG_ERROR_ON | DBG_WARNING_ON | DBG_BREAK_ON)
 
-#define DBG_FLAGS(A)		(A)->dbgFlags
-#define DBG_NAME(A)		(A)->dbgName
-#define DBG_LEVEL(A)		(A)->dbgLevel
+#define DBG_FLAGS(A)		((A)->dbgFlags)
+#define DBG_NAME(A)		((A)->dbgName)
+#define DBG_LEVEL(A)		((A)->dbgLevel)
 
 #ifndef DBG_PRINT
 #define DBG_PRINT(S...)		printk(KERN_DEBUG S)
@@ -108,56 +108,110 @@
 #define _DBG_LEAVE(A)	printk(KERN_DEBUG "%s:%.*s:%s\n", DBG_NAME(A),	\
 				DBG_LEVEL(A)--, _LEAVE_STR, __func__)
 
-#define DBG_ENTER(A)        {if (DBG_FLAGS(A) & DBG_TRACE_ON) \
-                                _DBG_ENTER(A);}
+#define DBG_ENTER(A)							\
+	do {								\
+		if (DBG_FLAGS(A) & DBG_TRACE_ON)			\
+			_DBG_ENTER(A);					\
+	} while (0)
+
+#define DBG_LEAVE(A)							\
+	do {								\
+		if (DBG_FLAGS(A) & DBG_TRACE_ON)			\
+			_DBG_LEAVE(A);					\
+	} while (0)
+
+#define DBG_PARAM(A, N, F, S...)					\
+	do {								\
+		if (DBG_FLAGS(A) & DBG_PARAM_ON)			\
+			DBG_PRINT("  %s -- "F" ", N, S);		\
+	} while (0)
+
+#define DBG_ERROR(A, S...)						 \
+	do {								 \
+		if (DBG_FLAGS(A) & DBG_ERROR_ON) {			 \
+			DBG_PRINT("%s:ERROR:%s ", DBG_NAME(A), __func__);\
+			DBG_PRINTC(S);					 \
+			DBG_TRAP;					 \
+		}							 \
+	} while (0)
+
+#define DBG_WARNING(A, S...)						    \
+	do {								    \
+		if (DBG_FLAGS(A) & DBG_WARNING_ON) {			    \
+			DBG_PRINT("%s:WARNING:%s ", DBG_NAME(A), __func__); \
+			DBG_PRINTC(S);					    \
+		}							    \
+	} while (0)
+
+#define DBG_NOTICE(A, S...)						   \
+	do {								   \
+		if (DBG_FLAGS(A) & DBG_NOTICE_ON) {			   \
+			DBG_PRINT("%s:NOTICE:%s ", DBG_NAME(A), __func__); \
+			DBG_PRINTC(S);					   \
+		}							   \
+	} while (0)
+
+#define DBG_TRACE(A, S...)						  \
+	do {								  \
+		if (DBG_FLAGS(A) & DBG_TRACE_ON) {			  \
+			DBG_PRINT("%s:TRACE:%s ", DBG_NAME(A), __func__); \
+			DBG_PRINTC(S);					  \
+		}							  \
+	} while (0)
+
+#define DBG_VERBOSE(A, S...)						    \
+	do {								    \
+		if (DBG_FLAGS(A) & DBG_VERBOSE_ON) {			    \
+			DBG_PRINT("%s:VERBOSE:%s ", DBG_NAME(A), __func__); \
+			DBG_PRINTC(S);					    \
+		}							    \
+	} while (0)
+
+#define DBG_RX(A, S...)				\
+	do {					\
+		if (DBG_FLAGS(A) & DBG_RX_ON)	\
+			DBG_PRINT(S);		\
+	} while (0)
+
+#define DBG_RX_ENTER(A)				\
+	do {					\
+		if (DBG_FLAGS(A) & DBG_RX_ON)	\
+			_DBG_ENTER(A);		\
+	} while (0)
+
+#define DBG_RX_LEAVE(A)				\
+	do {					\
+		if (DBG_FLAGS(A) & DBG_RX_ON)	\
+			_DBG_LEAVE(A);		\
+	} while (0)
+
+#define DBG_TX(A, S...)				\
+	do {					\
+		if (DBG_FLAGS(A) & DBG_TX_ON)	\
+			DBG_PRINT(S);		\
+	} while (0)
+
+#define DBG_TX_ENTER(A)				\
+	do {					\
+		if (DBG_FLAGS(A) & DBG_TX_ON)	\
+			_DBG_ENTER(A);		\
+	} while (0)
+
+#define DBG_TX_LEAVE(A)				\
+	do {					\
+		if (DBG_FLAGS(A) & DBG_TX_ON)	\
+			_DBG_LEAVE(A);		\
+	} while (0)
+
+#define DBG_ASSERT(C)						\
+	do {							\
+		if (!(C)) {					\
+			DBG_PRINT("ASSERT(%s) -- %s#%d (%s) ",  \
+			     #C, __FILE__, __LINE__, __func__); \
+			DBG_TRAP;				\
+		}						\
+	} while (0)
 
-#define DBG_LEAVE(A)        {if (DBG_FLAGS(A) & DBG_TRACE_ON) \
-                                _DBG_LEAVE(A);}
-
-#define DBG_PARAM(A,N,F,S...)   {if (DBG_FLAGS(A) & DBG_PARAM_ON) \
-                                    DBG_PRINT("  %s -- "F"\n",N,S);}
-
-#define DBG_ERROR(A,S...)	\
-	if (DBG_FLAGS(A) & DBG_ERROR_ON) {				\
-		DBG_PRINT("%s:ERROR:%s ",DBG_NAME(A), __func__);	\
-		DBG_PRINTC(S);						\
-		DBG_TRAP;						\
-	}
-
-#define DBG_WARNING(A,S...) {if (DBG_FLAGS(A) & DBG_WARNING_ON) \
-                                {DBG_PRINT("%s:WARNING:%s ",DBG_NAME(A),__func__);DBG_PRINTC(S);}}
-
-#define DBG_NOTICE(A,S...)  {if (DBG_FLAGS(A) & DBG_NOTICE_ON) \
-                                {DBG_PRINT("%s:NOTICE:%s ",DBG_NAME(A),__func__);DBG_PRINTC(S);}}
-
-#define DBG_TRACE(A,S...)   {if (DBG_FLAGS(A) & DBG_TRACE_ON) \
-                                {DBG_PRINT("%s:TRACE:%s ",DBG_NAME(A), __func__);DBG_PRINTC(S);}}
-
-#define DBG_VERBOSE(A,S...) {if (DBG_FLAGS(A) & DBG_VERBOSE_ON) \
-                                {DBG_PRINT("%s:VERBOSE:%s ",DBG_NAME(A), __func__);DBG_PRINTC(S);}}
-
-#define DBG_RX(A,S...)      {if (DBG_FLAGS(A) & DBG_RX_ON) \
-                                {DBG_PRINT(S);}}
-
-#define DBG_RX_ENTER(A)     {if (DBG_FLAGS(A) & DBG_RX_ON) \
-                                _DBG_ENTER(A);}
-
-#define DBG_RX_LEAVE(A)     {if (DBG_FLAGS(A) & DBG_RX_ON) \
-                                _DBG_LEAVE(A);}
-
-#define DBG_TX(A,S...)      {if (DBG_FLAGS(A) & DBG_TX_ON) \
-                                {DBG_PRINT(S);}}
-
-#define DBG_TX_ENTER(A)     {if (DBG_FLAGS(A) & DBG_TX_ON) \
-                                _DBG_ENTER(A);}
-
-#define DBG_TX_LEAVE(A)     {if (DBG_FLAGS(A) & DBG_TX_ON) \
-                                _DBG_LEAVE(A);}
-
-#define DBG_ASSERT(C)       {if (!(C)) \
-                                {DBG_PRINT("ASSERT(%s) -- %s#%d (%s)\n", \
-                                    #C,__FILE__,__LINE__,__func__); \
-                                DBG_TRAP;}}
 #define STATIC
 
 typedef struct {
diff --git a/drivers/staging/frontier/Kconfig b/drivers/staging/frontier/Kconfig
new file mode 100644
index 00000000000..7121853bd39
--- /dev/null
+++ b/drivers/staging/frontier/Kconfig
@@ -0,0 +1,6 @@
+config TRANZPORT
+	tristate "Frontier Tranzport and Alphatrack support"
+	depends on USB
+	default N
+	---help---
+	  Enable support for the Frontier Tranzport and Alphatrack devices.
diff --git a/drivers/staging/frontier/Makefile b/drivers/staging/frontier/Makefile
new file mode 100644
index 00000000000..2d2ac97492d
--- /dev/null
+++ b/drivers/staging/frontier/Makefile
@@ -0,0 +1,2 @@
+obj-$(CONFIG_TRANZPORT)		+= tranzport.o
+obj-$(CONFIG_TRANZPORT)		+= alphatrack.o
diff --git a/drivers/staging/frontier/README b/drivers/staging/frontier/README
new file mode 100644
index 00000000000..07c9ef9b8fc
--- /dev/null
+++ b/drivers/staging/frontier/README
@@ -0,0 +1,28 @@
+This directory contains the USB Tranzport and Alphatrack Kernel drivers for Linux.
+
+At present the tranzport does reads/writes of 8 byte cmds to /dev/tranzport0 to control
+the lights and screen and wheel
+
+At present the alphatrack accepts reads/writes of 12 byte cmds to /dev/tranzport0 to control
+the lights and screen and fader.
+
+Both drivers also have some sysfs hooks that are non-functional at the moment.
+
+The API is currently closely tied to the ardour revision and WILL change.
+
+A sysfs interface is PERFECT for simple userspace apps to do fun things with the
+lights and screen. It's fairly lousy for handling input events and very lousy
+for watching the state of the shuttle wheel.
+
+A linux input events interface is great for the input events and shuttle wheel. It's
+theoretically OK on LEDs. A Fader can be mapped to an absolute mouse device.
+But there is no LCD support at all.
+
+In the end this is going to be driven by a midi layer, which handles all those
+cases via a defined API, but - among other things - is slow, doesn't do
+flow control, and is a LOT of extra work. Frankly, I'd like to keep the
+core driver simple because the only realtime work really required is
+the bottom half interrupt handler and the output overlapping.
+
+Exposing some sort of clean aio api to userspace would be perfect. What that
+API looks like? Gah. beats me.
diff --git a/drivers/staging/frontier/TODO b/drivers/staging/frontier/TODO
new file mode 100644
index 00000000000..3620ad2df3e
--- /dev/null
+++ b/drivers/staging/frontier/TODO
@@ -0,0 +1,9 @@
+TODO:
+	- checkpatch.pl clean
+	- sparse clean
+	- fix userspace interface to be sane
+	- possibly just port to userspace with libusb
+	- review by the USB developer community
+
+Please send any patches for this driver to Greg Kroah-Hartman <greg@kroah.com>
+and David Taht <d@teklibre.com>.
diff --git a/drivers/staging/frontier/alphatrack.c b/drivers/staging/frontier/alphatrack.c
new file mode 100644
index 00000000000..61d7c5df87a
--- /dev/null
+++ b/drivers/staging/frontier/alphatrack.c
@@ -0,0 +1,853 @@
+/*
+ * Frontier Designs Alphatrack driver
+ *
+ * Copyright (C) 2007 Michael Taht (m@taht.net)
+ *
+ * Based on the usbled driver and ldusb drivers by
+ *
+ * Copyright (C) 2004 Greg Kroah-Hartman (greg@kroah.com)
+ * Copyright (C) 2005 Michael Hund <mhund@ld-didactic.de>
+ *
+ * The ldusb driver was, in turn, derived from Lego USB Tower driver
+ * Copyright (C) 2003 David Glance <advidgsf@sourceforge.net>
+ *		 2001-2004 Juergen Stuber <starblue@users.sourceforge.net>
+ *
+ *	This program is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU General Public License as
+ *	published by the Free Software Foundation, version 2.
+ *
+ */
+
+/**
+ * This driver uses a ring buffer for time critical reading of
+ * interrupt in reports and provides read and write methods for
+ * raw interrupt reports.
+ */
+
+/* Note: this currently uses a dumb ringbuffer for reads and writes.
+ * A more optimal driver would cache and kill off outstanding urbs that are
+ * now invalid, and ignore ones that already were in the queue but valid
+ * as we only have 30 commands for the alphatrack. In particular this is
+ * key for getting lights to flash in time as otherwise many commands
+ * can be buffered up before the light change makes it to the interface.
+*/
+
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/kobject.h>
+#include <linux/mutex.h>
+#include <linux/version.h>
+
+#include <asm/uaccess.h>
+#include <linux/input.h>
+#include <linux/usb.h>
+#include <linux/poll.h>
+
+#include "surface_sysfs.h"
+
+/* make this work on older kernel versions */
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 19)
+#include "frontier_compat.h"
+#endif /* older kernel versions */
+
+#include "alphatrack.h"
+
+#define VENDOR_ID	0x165b
+#define PRODUCT_ID	0xfad1
+
+#ifdef CONFIG_USB_DYNAMIC_MINORS
+#define USB_ALPHATRACK_MINOR_BASE	0
+#else
+// FIXME 176 - is another driver's minor - apply for that
+// #define USB_ALPHATRACK_MINOR_BASE	177
+#define USB_ALPHATRACK_MINOR_BASE	176
+#endif
+
+/* table of devices that work with this driver */
+static struct usb_device_id usb_alphatrack_table [] = {
+	{ USB_DEVICE(VENDOR_ID, PRODUCT_ID) },
+	{ }					/* Terminating entry */
+};
+
+MODULE_DEVICE_TABLE(usb, usb_alphatrack_table);
+MODULE_VERSION("0.40");
+MODULE_AUTHOR("Mike Taht <m@taht.net>");
+MODULE_DESCRIPTION("Alphatrack USB Driver");
+MODULE_LICENSE("GPL");
+MODULE_SUPPORTED_DEVICE("Frontier Designs Alphatrack Control Surface");
+
+/* These aren't done yet */
+
+#define SUPPRESS_EXTRA_ONLINE_EVENTS 0
+#define BUFFERED_WRITES 0
+#define SUPPRESS_EXTRA_OFFLINE_EVENTS 0
+#define COMPRESS_FADER_EVENTS 0
+
+#define BUFFERED_READS 1
+#define RING_BUFFER_SIZE 512
+#define WRITE_BUFFER_SIZE 34
+#define ALPHATRACK_USB_TIMEOUT 10
+#define OUTPUT_CMD_SIZE 8
+#define INPUT_CMD_SIZE 12
+
+
+static int debug = 0;
+
+/* Use our own dbg macro */
+#define dbg_info(dev, format, arg...) do { if (debug) dev_info(dev , format , ## arg); } while (0)
+
+#define alphatrack_ocmd_info(dev, cmd, format, arg...)
+
+#define alphatrack_icmd_info(dev, cmd, format, arg...)
+
+
+/* Module parameters */
+
+module_param(debug, int, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(debug, "Debug enabled or not");
+
+/* All interrupt in transfers are collected in a ring buffer to
+ * avoid racing conditions and get better performance of the driver.
+ */
+
+static int ring_buffer_size = RING_BUFFER_SIZE;
+
+module_param(ring_buffer_size, int,  S_IRUGO);
+MODULE_PARM_DESC(ring_buffer_size, "Read ring buffer size");
+
+/* The write_buffer can one day contain more than one interrupt out transfer.
+ */
+
+static int write_buffer_size = WRITE_BUFFER_SIZE;
+module_param(write_buffer_size, int,  S_IRUGO);
+MODULE_PARM_DESC(write_buffer_size, "Write buffer size");
+
+/*
+ * Increase the interval for debugging purposes.
+ * or set to 1 to use the standard interval from the endpoint descriptors.
+ */
+
+static int min_interrupt_in_interval = ALPHATRACK_USB_TIMEOUT;
+module_param(min_interrupt_in_interval, int, 0);
+MODULE_PARM_DESC(min_interrupt_in_interval, "Minimum interrupt in interval in ms");
+
+static int min_interrupt_out_interval = ALPHATRACK_USB_TIMEOUT;
+module_param(min_interrupt_out_interval, int, 0);
+MODULE_PARM_DESC(min_interrupt_out_interval, "Minimum interrupt out interval in ms");
+
+
+
+/* Structure to hold all of our device specific stuff */
+
+struct usb_alphatrack {
+	struct semaphore	sem;		/* locks this structure */
+	struct usb_interface*	intf;		/* save off the usb interface pointer */
+	int			open_count;	/* number of times this port has been opened */
+
+	struct alphatrack_icmd	(*ring_buffer)[RING_BUFFER_SIZE]; /* just make c happy */
+	struct alphatrack_ocmd	(*write_buffer)[WRITE_BUFFER_SIZE]; /* just make c happy */
+	unsigned int		ring_head;
+	unsigned int		ring_tail;
+
+	wait_queue_head_t	read_wait;
+	wait_queue_head_t	write_wait;
+
+	unsigned char*		interrupt_in_buffer;
+	unsigned char*		oldi_buffer;
+	struct usb_endpoint_descriptor* interrupt_in_endpoint;
+	struct urb*		interrupt_in_urb;
+	int			interrupt_in_interval;
+	size_t			interrupt_in_endpoint_size;
+	int			interrupt_in_running;
+	int			interrupt_in_done;
+
+	char*			interrupt_out_buffer;
+	struct usb_endpoint_descriptor* interrupt_out_endpoint;
+	struct urb*		interrupt_out_urb;
+	int			interrupt_out_interval;
+	size_t			interrupt_out_endpoint_size;
+	int			interrupt_out_busy;
+
+	atomic_t writes_pending;
+	int event; /* alternate interface to events */
+	int fader; /* 10 bits */
+	int lights; /* 23 bits */
+	unsigned char dump_state; /* 0 if disabled 1 if enabled */
+	unsigned char enable; /* 0 if disabled 1 if enabled */
+	unsigned char offline; /* if the device is out of range or asleep */
+	unsigned char verbose; /* be verbose in error reporting */
+	unsigned char  last_cmd[OUTPUT_CMD_SIZE];
+	unsigned char  screen[32];
+};
+
+/* prevent races between open() and disconnect() */
+static DEFINE_MUTEX(disconnect_mutex);
+
+/* forward declaration */
+
+static struct usb_driver usb_alphatrack_driver;
+
+/**
+ *	usb_alphatrack_abort_transfers
+ *      aborts transfers and frees associated data structures
+ */
+static void usb_alphatrack_abort_transfers(struct usb_alphatrack *dev)
+{
+	/* shutdown transfer */
+	if (dev->interrupt_in_running) {
+		dev->interrupt_in_running = 0;
+		if (dev->intf)
+			usb_kill_urb(dev->interrupt_in_urb);
+	}
+	if (dev->interrupt_out_busy)
+		if (dev->intf)
+			usb_kill_urb(dev->interrupt_out_urb);
+}
+
+/**
+ *	usb_alphatrack_delete
+ */
+static void usb_alphatrack_delete(struct usb_alphatrack *dev)
+{
+	usb_alphatrack_abort_transfers(dev);
+	usb_free_urb(dev->interrupt_in_urb);
+	usb_free_urb(dev->interrupt_out_urb);
+	kfree(dev->ring_buffer);
+	kfree(dev->interrupt_in_buffer);
+	kfree(dev->interrupt_out_buffer);
+	kfree(dev); // fixme oldi_buffer
+}
+
+/**
+ *	usb_alphatrack_interrupt_in_callback
+ */
+
+static void usb_alphatrack_interrupt_in_callback(struct urb *urb)
+{
+	struct usb_alphatrack *dev = urb->context;
+	unsigned int next_ring_head;
+	int retval = -1;
+
+	if (urb->status) {
+		if (urb->status == -ENOENT ||
+		    urb->status == -ECONNRESET ||
+		    urb->status == -ESHUTDOWN) {
+			goto exit;
+		} else {
+			dbg_info(&dev->intf->dev, "%s: nonzero status received: %d\n",
+				 __FUNCTION__, urb->status);
+			goto resubmit; /* maybe we can recover */
+		}
+	}
+
+	if (urb->actual_length != INPUT_CMD_SIZE) {
+		dev_warn(&dev->intf->dev,
+			 "Urb length was %d bytes!! Do something intelligent \n", urb->actual_length);
+	} else {
+		 alphatrack_ocmd_info(&dev->intf->dev,&(*dev->ring_buffer)[dev->ring_tail].cmd,"%s", "bla");
+		 if(memcmp(dev->interrupt_in_buffer,dev->oldi_buffer,INPUT_CMD_SIZE)==0) {
+						goto resubmit;
+		}
+		memcpy(dev->oldi_buffer,dev->interrupt_in_buffer,INPUT_CMD_SIZE);
+
+#if SUPPRESS_EXTRA_OFFLINE_EVENTS
+	if(dev->offline == 2 && dev->interrupt_in_buffer[1] == 0xff) { goto resubmit; }
+		if(dev->offline == 1 && dev->interrupt_in_buffer[1] == 0xff) { dev->offline = 2; goto resubmit; }
+/* Always pass one offline event up the stack */
+		if(dev->offline > 0 && dev->interrupt_in_buffer[1] != 0xff) { dev->offline = 0; }
+		if(dev->offline == 0 && dev->interrupt_in_buffer[1] == 0xff) { dev->offline = 1; }
+#endif
+		dbg_info(&dev->intf->dev, "%s: head, tail are %x, %x\n", __FUNCTION__,dev->ring_head,dev->ring_tail);
+		next_ring_head = (dev->ring_head+1) % ring_buffer_size;
+
+		if (next_ring_head != dev->ring_tail) {
+			memcpy(&((*dev->ring_buffer)[dev->ring_head]),
+						 dev->interrupt_in_buffer, urb->actual_length);
+			dev->ring_head = next_ring_head;
+			retval = 0;
+			memset(dev->interrupt_in_buffer, 0, urb->actual_length);
+		} else {
+			dev_warn(&dev->intf->dev,
+				 "Ring buffer overflow, %d bytes dropped\n",
+				 urb->actual_length);
+			memset(dev->interrupt_in_buffer, 0, urb->actual_length);
+		}
+	}
+
+resubmit:
+	/* resubmit if we're still running */
+	if (dev->interrupt_in_running && dev->intf) {
+		retval = usb_submit_urb(dev->interrupt_in_urb, GFP_ATOMIC);
+		if (retval)
+			dev_err(&dev->intf->dev,
+				"usb_submit_urb failed (%d)\n", retval);
+	}
+
+exit:
+	dev->interrupt_in_done = 1;
+	wake_up_interruptible(&dev->read_wait);
+}
+
+/**
+ *	usb_alphatrack_interrupt_out_callback
+ */
+static void usb_alphatrack_interrupt_out_callback(struct urb *urb)
+{
+	struct usb_alphatrack *dev = urb->context;
+
+	/* sync/async unlink faults aren't errors */
+	if (urb->status && !(urb->status == -ENOENT ||
+			     urb->status == -ECONNRESET ||
+			     urb->status == -ESHUTDOWN))
+		dbg_info(&dev->intf->dev,
+			 "%s - nonzero write interrupt status received: %d\n",
+			 __FUNCTION__, urb->status);
+	atomic_dec(&dev->writes_pending);
+	dev->interrupt_out_busy = 0;
+	wake_up_interruptible(&dev->write_wait);
+}
+
+/**
+ *	usb_alphatrack_open
+ */
+static int usb_alphatrack_open(struct inode *inode, struct file *file)
+{
+	struct usb_alphatrack *dev;
+	int subminor;
+	int retval = 0;
+	struct usb_interface *interface;
+
+	nonseekable_open(inode, file);
+	subminor = iminor(inode);
+
+	mutex_lock(&disconnect_mutex);
+
+	interface = usb_find_interface(&usb_alphatrack_driver, subminor);
+
+	if (!interface) {
+		err("%s - error, can't find device for minor %d\n",
+		     __FUNCTION__, subminor);
+		retval = -ENODEV;
+		goto unlock_disconnect_exit;
+	}
+
+	dev = usb_get_intfdata(interface);
+
+	if (!dev) {
+		retval = -ENODEV;
+		goto unlock_disconnect_exit;
+	}
+
+	/* lock this device */
+	if (down_interruptible(&dev->sem)) {
+		retval = -ERESTARTSYS;
+		goto unlock_disconnect_exit;
+	}
+
+	/* allow opening only once */
+	if (dev->open_count) {
+		retval = -EBUSY;
+		goto unlock_exit;
+	}
+	dev->open_count = 1;
+
+	/* initialize in direction */
+	dev->ring_head = 0;
+	dev->ring_tail = 0;
+	usb_fill_int_urb(dev->interrupt_in_urb,
+			 interface_to_usbdev(interface),
+			 usb_rcvintpipe(interface_to_usbdev(interface),
+					dev->interrupt_in_endpoint->bEndpointAddress),
+			 dev->interrupt_in_buffer,
+			 dev->interrupt_in_endpoint_size,
+			 usb_alphatrack_interrupt_in_callback,
+			 dev,
+			 dev->interrupt_in_interval);
+
+	dev->interrupt_in_running = 1;
+	dev->interrupt_in_done = 0;
+	dev->enable = 1;
+	dev->offline = 0;
+
+	retval = usb_submit_urb(dev->interrupt_in_urb, GFP_KERNEL);
+	if (retval) {
+		dev_err(&interface->dev, "Couldn't submit interrupt_in_urb %d\n", retval);
+		dev->interrupt_in_running = 0;
+		dev->open_count = 0;
+		goto unlock_exit;
+	}
+
+	/* save device in the file's private structure */
+	file->private_data = dev;
+
+
+unlock_exit:
+	up(&dev->sem);
+
+unlock_disconnect_exit:
+	mutex_unlock(&disconnect_mutex);
+
+	return retval;
+}
+
+/**
+ *	usb_alphatrack_release
+ */
+static int usb_alphatrack_release(struct inode *inode, struct file *file)
+{
+	struct usb_alphatrack *dev;
+	int retval = 0;
+
+	dev = file->private_data;
+
+	if (dev == NULL) {
+		retval = -ENODEV;
+		goto exit;
+	}
+
+	if (down_interruptible(&dev->sem)) {
+		retval = -ERESTARTSYS;
+		goto exit;
+	}
+
+	if (dev->open_count != 1) {
+		retval = -ENODEV;
+		goto unlock_exit;
+	}
+
+	if (dev->intf == NULL) {
+		/* the device was unplugged before the file was released */
+		up(&dev->sem);
+		/* unlock here as usb_alphatrack_delete frees dev */
+		usb_alphatrack_delete(dev);
+		retval = -ENODEV;
+		goto exit;
+	}
+
+	/* wait until write transfer is finished */
+	if (dev->interrupt_out_busy)
+		wait_event_interruptible_timeout(dev->write_wait, !dev->interrupt_out_busy, 2 * HZ);
+	usb_alphatrack_abort_transfers(dev);
+	dev->open_count = 0;
+
+unlock_exit:
+	up(&dev->sem);
+
+exit:
+	return retval;
+}
+
+/**
+ *	usb_alphatrack_poll
+ */
+static unsigned int usb_alphatrack_poll(struct file *file, poll_table *wait)
+{
+	struct usb_alphatrack *dev;
+	unsigned int mask = 0;
+
+	dev = file->private_data;
+
+	poll_wait(file, &dev->read_wait, wait);
+	poll_wait(file, &dev->write_wait, wait);
+
+	if (dev->ring_head != dev->ring_tail)
+		mask |= POLLIN | POLLRDNORM;
+	if (!dev->interrupt_out_busy)
+		mask |= POLLOUT | POLLWRNORM;
+
+	return mask;
+}
+
+/**
+ *	usb_alphatrack_read
+ */
+static ssize_t usb_alphatrack_read(struct file *file, char __user *buffer, size_t count,
+			   loff_t *ppos)
+{
+	struct usb_alphatrack *dev;
+	int retval = 0;
+
+	int c = 0;
+
+	dev = file->private_data;
+
+	/* verify that we actually have some data to read */
+	if (count == 0)
+		goto exit;
+
+	/* lock this object */
+	if (down_interruptible(&dev->sem)) {
+		retval = -ERESTARTSYS;
+		goto exit;
+	}
+
+	/* verify that the device wasn't unplugged */
+	if (dev->intf == NULL) {
+		retval = -ENODEV;
+		err("No device or device unplugged %d\n", retval);
+		goto unlock_exit;
+	}
+
+	while (dev->ring_head == dev->ring_tail) {
+					if (file->f_flags & O_NONBLOCK) {
+									retval = -EAGAIN;
+									goto unlock_exit;
+					}
+					dev->interrupt_in_done = 0 ;
+					retval = wait_event_interruptible(dev->read_wait, dev->interrupt_in_done);
+					if (retval < 0) {
+									goto unlock_exit;
+					}
+	}
+
+	alphatrack_ocmd_info(&dev->intf->dev, &(*dev->ring_buffer)[dev->ring_tail].cmd, "%s", ": copying to userspace");
+
+	   c = 0;
+	   while((c < count) && (dev->ring_tail != dev->ring_head)) {
+						 if (copy_to_user(&buffer[c], &(*dev->ring_buffer)[dev->ring_tail], INPUT_CMD_SIZE)) {
+										 retval = -EFAULT;
+										 goto unlock_exit;
+						 }
+						 dev->ring_tail = (dev->ring_tail+1) % ring_buffer_size;
+						 c+=INPUT_CMD_SIZE;
+						 dbg_info(&dev->intf->dev, "%s: head, tail are %x, %x\n", __FUNCTION__,dev->ring_head,dev->ring_tail);
+	   }
+	   retval = c;
+
+unlock_exit:
+	/* unlock the device */
+	up(&dev->sem);
+
+exit:
+	return retval;
+}
+
+/**
+ *	usb_alphatrack_write
+ */
+static ssize_t usb_alphatrack_write(struct file *file, const char __user *buffer,
+			    size_t count, loff_t *ppos)
+{
+	struct usb_alphatrack *dev;
+	size_t bytes_to_write;
+	int retval = 0;
+
+	dev = file->private_data;
+
+	/* verify that we actually have some data to write */
+	if (count == 0)
+		goto exit;
+
+	/* lock this object */
+	if (down_interruptible(&dev->sem)) {
+		retval = -ERESTARTSYS;
+		goto exit;
+	}
+
+	/* verify that the device wasn't unplugged */
+	if (dev->intf == NULL) {
+		retval = -ENODEV;
+		err("No device or device unplugged %d\n", retval);
+		goto unlock_exit;
+	}
+
+	/* wait until previous transfer is finished */
+	if (dev->interrupt_out_busy) {
+		if (file->f_flags & O_NONBLOCK) {
+			retval = -EAGAIN;
+			goto unlock_exit;
+		}
+		retval = wait_event_interruptible(dev->write_wait, !dev->interrupt_out_busy);
+		if (retval < 0) {
+			goto unlock_exit;
+		}
+	}
+
+	/* write the data into interrupt_out_buffer from userspace */
+  /* FIXME - if you write more than 12 bytes this breaks */
+	bytes_to_write = min(count, write_buffer_size*dev->interrupt_out_endpoint_size);
+	if (bytes_to_write < count)
+		dev_warn(&dev->intf->dev, "Write buffer overflow, %zd bytes dropped\n",count-bytes_to_write);
+
+	dbg_info(&dev->intf->dev, "%s: count = %zd, bytes_to_write = %zd\n", __FUNCTION__, count, bytes_to_write);
+
+	if (copy_from_user(dev->interrupt_out_buffer, buffer, bytes_to_write)) {
+		retval = -EFAULT;
+		goto unlock_exit;
+	}
+
+	if (dev->interrupt_out_endpoint == NULL) {
+		err("Endpoint should not be be null! \n");
+		goto unlock_exit;
+	}
+
+	/* send off the urb */
+	usb_fill_int_urb(dev->interrupt_out_urb,
+			 interface_to_usbdev(dev->intf),
+			 usb_sndintpipe(interface_to_usbdev(dev->intf),
+					dev->interrupt_out_endpoint->bEndpointAddress),
+			 dev->interrupt_out_buffer,
+			 bytes_to_write,
+			 usb_alphatrack_interrupt_out_callback,
+			 dev,
+			 dev->interrupt_out_interval);
+	dev->interrupt_out_busy = 1;
+	atomic_inc(&dev->writes_pending);
+	wmb();
+
+	retval = usb_submit_urb(dev->interrupt_out_urb, GFP_KERNEL);
+	if (retval) {
+		dev->interrupt_out_busy = 0;
+		err("Couldn't submit interrupt_out_urb %d\n", retval);
+		atomic_dec(&dev->writes_pending);
+		goto unlock_exit;
+	}
+	retval = bytes_to_write;
+
+unlock_exit:
+	/* unlock the device */
+	up(&dev->sem);
+
+exit:
+	return retval;
+}
+
+/* file operations needed when we register this driver */
+static const struct file_operations usb_alphatrack_fops = {
+	.owner =	THIS_MODULE,
+	.read  =	usb_alphatrack_read,
+	.write =	usb_alphatrack_write,
+	.open =		usb_alphatrack_open,
+	.release =	usb_alphatrack_release,
+	.poll =		usb_alphatrack_poll,
+};
+
+/*
+ * usb class driver info in order to get a minor number from the usb core,
+ * and to have the device registered with the driver core
+ */
+
+static struct usb_class_driver usb_alphatrack_class = {
+	.name =		"alphatrack%d",
+	.fops =		&usb_alphatrack_fops,
+	.minor_base =	USB_ALPHATRACK_MINOR_BASE,
+};
+
+
+/**
+ *	usb_alphatrack_probe
+ *
+ *	Called by the usb core when a new device is connected that it thinks
+ *	this driver might be interested in.
+ */
+static int usb_alphatrack_probe(struct usb_interface *intf, const struct usb_device_id *id)
+{
+	struct usb_device *udev = interface_to_usbdev(intf);
+	struct usb_alphatrack *dev = NULL;
+	struct usb_host_interface *iface_desc;
+	struct usb_endpoint_descriptor *endpoint;
+	int i;
+	int true_size;
+	int retval = -ENOMEM;
+
+	/* allocate memory for our device state and intialize it */
+
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (dev == NULL) {
+		dev_err(&intf->dev, "Out of memory\n");
+		goto exit;
+	}
+	init_MUTEX(&dev->sem);
+	dev->intf = intf;
+	init_waitqueue_head(&dev->read_wait);
+	init_waitqueue_head(&dev->write_wait);
+
+	iface_desc = intf->cur_altsetting;
+
+	/* set up the endpoint information */
+	for (i = 0; i < iface_desc->desc.bNumEndpoints; ++i) {
+		endpoint = &iface_desc->endpoint[i].desc;
+
+		if (usb_endpoint_is_int_in(endpoint))
+			dev->interrupt_in_endpoint = endpoint;
+
+		if (usb_endpoint_is_int_out(endpoint))
+			dev->interrupt_out_endpoint = endpoint;
+	}
+	if (dev->interrupt_in_endpoint == NULL) {
+		dev_err(&intf->dev, "Interrupt in endpoint not found\n");
+		goto error;
+	}
+	if (dev->interrupt_out_endpoint == NULL)
+		dev_warn(&intf->dev, "Interrupt out endpoint not found (using control endpoint instead)\n");
+
+	dev->interrupt_in_endpoint_size = le16_to_cpu(dev->interrupt_in_endpoint->wMaxPacketSize);
+
+	if (dev->interrupt_in_endpoint_size != 64)
+	    dev_warn(&intf->dev, "Interrupt in endpoint size is not 64!\n");
+
+	if(ring_buffer_size == 0) { ring_buffer_size = RING_BUFFER_SIZE; }
+
+	true_size = min(ring_buffer_size,RING_BUFFER_SIZE);
+
+	/* FIXME - there are more usb_alloc routines for dma correctness. Needed? */
+
+//	dev->ring_buffer = kmalloc((true_size*sizeof(struct alphatrack_icmd))+12, GFP_KERNEL);
+	dev->ring_buffer = kmalloc((true_size*sizeof(struct alphatrack_icmd)), GFP_KERNEL);
+
+	if (!dev->ring_buffer) {
+		dev_err(&intf->dev, "Couldn't allocate input ring_buffer of size %d\n",true_size);
+		goto error;
+	}
+
+	dev->interrupt_in_buffer = kmalloc(dev->interrupt_in_endpoint_size, GFP_KERNEL);
+
+	if (!dev->interrupt_in_buffer) {
+		dev_err(&intf->dev, "Couldn't allocate interrupt_in_buffer\n");
+		goto error;
+	}
+	dev->oldi_buffer = kmalloc(dev->interrupt_in_endpoint_size, GFP_KERNEL);
+	if (!dev->oldi_buffer) {
+		dev_err(&intf->dev, "Couldn't allocate old buffer\n");
+		goto error;
+	}
+	dev->interrupt_in_urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!dev->interrupt_in_urb) {
+		dev_err(&intf->dev, "Couldn't allocate interrupt_in_urb\n");
+		goto error;
+	}
+
+	dev->interrupt_out_endpoint_size = dev->interrupt_out_endpoint ? le16_to_cpu(dev->interrupt_out_endpoint->wMaxPacketSize) :
+									 udev->descriptor.bMaxPacketSize0;
+
+	if (dev->interrupt_out_endpoint_size !=64)
+		dev_warn(&intf->dev, "Interrupt out endpoint size is not 64!)\n");
+
+	if(write_buffer_size == 0) { write_buffer_size = WRITE_BUFFER_SIZE; }
+	true_size = min(write_buffer_size,WRITE_BUFFER_SIZE);
+
+	dev->interrupt_out_buffer = kmalloc(true_size*dev->interrupt_out_endpoint_size, GFP_KERNEL);
+
+	if (!dev->interrupt_out_buffer) {
+		dev_err(&intf->dev, "Couldn't allocate interrupt_out_buffer\n");
+		goto error;
+	}
+
+	dev->write_buffer = kmalloc(sizeof(struct alphatrack_ocmd)*true_size, GFP_KERNEL);
+
+	if (!dev->write_buffer) {
+		dev_err(&intf->dev, "Couldn't allocate write_buffer \n");
+		goto error;
+	}
+
+	dev->interrupt_out_urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!dev->interrupt_out_urb) {
+		dev_err(&intf->dev, "Couldn't allocate interrupt_out_urb\n");
+		goto error;
+	}
+	dev->interrupt_in_interval = min_interrupt_in_interval > dev->interrupt_in_endpoint->bInterval ? min_interrupt_in_interval : dev->interrupt_in_endpoint->bInterval;
+	if (dev->interrupt_out_endpoint)
+		dev->interrupt_out_interval = min_interrupt_out_interval > dev->interrupt_out_endpoint->bInterval ? min_interrupt_out_interval : dev->interrupt_out_endpoint->bInterval;
+
+	/* we can register the device now, as it is ready */
+	usb_set_intfdata(intf, dev);
+
+	atomic_set(&dev->writes_pending,0);
+	retval = usb_register_dev(intf, &usb_alphatrack_class);
+	if (retval) {
+		/* something prevented us from registering this driver */
+		dev_err(&intf->dev, "Not able to get a minor for this device.\n");
+		usb_set_intfdata(intf, NULL);
+		goto error;
+	}
+
+	/* let the user know what node this device is now attached to */
+	dev_info(&intf->dev, "Alphatrack Device #%d now attached to major %d minor %d\n",
+		(intf->minor - USB_ALPHATRACK_MINOR_BASE), USB_MAJOR, intf->minor);
+
+exit:
+	return retval;
+
+error:
+	usb_alphatrack_delete(dev);
+
+	return retval;
+}
+
+/**
+ *	usb_alphatrack_disconnect
+ *
+ *	Called by the usb core when the device is removed from the system.
+ */
+static void usb_alphatrack_disconnect(struct usb_interface *intf)
+{
+	struct usb_alphatrack *dev;
+	int minor;
+
+	mutex_lock(&disconnect_mutex);
+
+	dev = usb_get_intfdata(intf);
+	usb_set_intfdata(intf, NULL);
+
+	down(&dev->sem);
+
+	minor = intf->minor;
+
+	/* give back our minor */
+	usb_deregister_dev(intf, &usb_alphatrack_class);
+
+	/* if the device is not opened, then we clean up right now */
+	if (!dev->open_count) {
+		up(&dev->sem);
+		usb_alphatrack_delete(dev);
+	} else {
+		dev->intf = NULL;
+		up(&dev->sem);
+	}
+
+	atomic_set(&dev->writes_pending,0);
+	mutex_unlock(&disconnect_mutex);
+
+	dev_info(&intf->dev, "Alphatrack Surface #%d now disconnected\n",
+		 (minor - USB_ALPHATRACK_MINOR_BASE));
+}
+
+/* usb specific object needed to register this driver with the usb subsystem */
+static struct usb_driver usb_alphatrack_driver = {
+	.name =		"alphatrack",
+	.probe =	usb_alphatrack_probe,
+	.disconnect =	usb_alphatrack_disconnect,
+	.id_table =	usb_alphatrack_table,
+};
+
+/**
+ *	usb_alphatrack_init
+ */
+static int __init usb_alphatrack_init(void)
+{
+	int retval;
+
+	/* register this driver with the USB subsystem */
+	retval = usb_register(&usb_alphatrack_driver);
+	if (retval)
+		err("usb_register failed for the "__FILE__" driver. Error number %d\n", retval);
+
+	return retval;
+}
+
+/**
+ *	usb_alphatrack_exit
+ */
+static void __exit usb_alphatrack_exit(void)
+{
+	/* deregister this driver with the USB subsystem */
+	usb_deregister(&usb_alphatrack_driver);
+}
+
+module_init(usb_alphatrack_init);
+module_exit(usb_alphatrack_exit);
+
diff --git a/drivers/staging/frontier/alphatrack.h b/drivers/staging/frontier/alphatrack.h
new file mode 100644
index 00000000000..35c90a90eb0
--- /dev/null
+++ b/drivers/staging/frontier/alphatrack.h
@@ -0,0 +1,92 @@
+#define show_set_bit(a) show_set_mbit(alphatrack,a)
+#define show_set_cmd(a) show_set_mcmd(alphatrack,a)
+#define show_set_int(a) show_set_mint(alphatrack,a)
+#define show_set_char(a) show_set_mchar(alphatrack,a)
+#define show_set_light(a) show_set_ebit(alphatrack,LightID,lights,a)
+#define show_set_button(a) show_set_ebit(alphatrack,ButtonID,button,a)
+
+struct alphatrack_icmd {
+    unsigned char cmd[12];
+};
+
+struct alphatrack_ocmd {
+    unsigned char cmd[8];
+};
+
+enum LightID {
+        LIGHT_EQ = 0,
+        LIGHT_OUT,
+        LIGHT_F2,
+        LIGHT_SEND,
+        LIGHT_IN,
+        LIGHT_F1,
+        LIGHT_PAN,
+        LIGHT_UNDEF1,
+        LIGHT_UNDEF2,
+        LIGHT_SHIFT,
+        LIGHT_TRACKMUTE,
+        LIGHT_TRACKSOLO,
+        LIGHT_TRACKREC,
+        LIGHT_READ,
+        LIGHT_WRITE,
+        LIGHT_ANYSOLO,
+        LIGHT_AUTO,
+        LIGHT_F4,
+        LIGHT_RECORD,
+        LIGHT_WINDOW,
+        LIGHT_PLUGIN,
+        LIGHT_F3,
+        LIGHT_LOOP
+};
+
+#define BUTTONMASK_BATTERY     0x00004000
+#define BUTTONMASK_BACKLIGHT   0x00008000
+#define BUTTONMASK_FASTFORWARD 0x04000000
+#define BUTTONMASK_TRACKMUTE   0x00040000
+#define BUTTONMASK_TRACKSOLO   0x00800000
+#define BUTTONMASK_TRACKLEFT   0x80000000
+#define BUTTONMASK_RECORD      0x02000000
+#define BUTTONMASK_SHIFT       0x20000000
+#define BUTTONMASK_PUNCH       0x00800000
+#define BUTTONMASK_TRACKRIGHT  0x00020000
+#define BUTTONMASK_REWIND      0x01000000
+#define BUTTONMASK_STOP        0x10000000
+#define BUTTONMASK_LOOP        0x00010000
+#define BUTTONMASK_TRACKREC    0x00001000
+#define BUTTONMASK_PLAY        0x08000000
+#define BUTTONMASK_TOUCH1      0x00000008
+#define BUTTONMASK_TOUCH2      0x00000010
+#define BUTTONMASK_TOUCH3      0x00000020
+
+#define BUTTONMASK_PRESS1      0x00000009
+#define BUTTONMASK_PRESS2      0x00008010
+#define BUTTONMASK_PRESS3      0x00002020
+
+// last 3 bytes are the slider position
+// 40 is the actual slider moving, the most sig bits, and 3 lsb
+
+#define BUTTONMASK_FLIP         0x40000000
+#define BUTTONMASK_F1           0x00100000
+#define BUTTONMASK_F2           0x00400000
+#define BUTTONMASK_F3           0x00200000
+#define BUTTONMASK_F4           0x00080000
+#define BUTTONMASK_PAN          0x00000200
+#define BUTTONMASK_SEND         0x00000800
+#define BUTTONMASK_EQ           0x00004000
+#define BUTTONMASK_PLUGIN       0x00000400
+#define BUTTONMASK_AUTO         0x00000100
+
+
+// #define BUTTONMASK_FOOTSWITCH FIXME
+
+// Lookup. name. midi out. midi in.
+
+struct buttonmap_t {
+	u32 mask;
+	short midi_in;
+	short midi_out;
+	char *name;
+//  	void (*function) (buttonmap_t *);
+  	void (*function) (void);
+};
+
diff --git a/drivers/staging/frontier/frontier_compat.h b/drivers/staging/frontier/frontier_compat.h
new file mode 100644
index 00000000000..00450e637ac
--- /dev/null
+++ b/drivers/staging/frontier/frontier_compat.h
@@ -0,0 +1,63 @@
+/* USB defines for older kernels */
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 19)
+
+/**
+ * usb_endpoint_dir_out - check if the endpoint has OUT direction
+ * @epd: endpoint to be checked
+ *
+ * Returns true if the endpoint is of type OUT, otherwise it returns false.
+ */
+
+static inline int usb_endpoint_dir_out(const struct usb_endpoint_descriptor *epd)
+{
+       return ((epd->bEndpointAddress & USB_ENDPOINT_DIR_MASK) == USB_DIR_OUT);
+}
+
+static inline int usb_endpoint_dir_in(const struct usb_endpoint_descriptor *epd)
+{
+       return ((epd->bEndpointAddress & USB_ENDPOINT_DIR_MASK) == USB_DIR_IN);
+}
+
+
+/**
+ * usb_endpoint_xfer_int - check if the endpoint has interrupt transfer type
+ * @epd: endpoint to be checked
+ *
+ * Returns true if the endpoint is of type interrupt, otherwise it returns
+ * false.
+ */
+static inline int usb_endpoint_xfer_int(const struct usb_endpoint_descriptor *epd)
+{
+       return ((epd->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK) ==
+               USB_ENDPOINT_XFER_INT);
+}
+
+
+/**
+ * usb_endpoint_is_int_in - check if the endpoint is interrupt IN
+ * @epd: endpoint to be checked
+ *
+ * Returns true if the endpoint has interrupt transfer type and IN direction,
+ * otherwise it returns false.
+ */
+
+static inline int usb_endpoint_is_int_in(const struct usb_endpoint_descriptor *epd)
+{
+       return (usb_endpoint_xfer_int(epd) && usb_endpoint_dir_in(epd));
+}
+
+/**
+ * usb_endpoint_is_int_out - check if the endpoint is interrupt OUT
+ * @epd: endpoint to be checked
+ *
+ * Returns true if the endpoint has interrupt transfer type and OUT direction,
+ * otherwise it returns false.
+ */
+
+static inline int usb_endpoint_is_int_out(const struct usb_endpoint_descriptor *epd)
+{
+       return (usb_endpoint_xfer_int(epd) && usb_endpoint_dir_out(epd));
+}
+
+#endif /* older kernel versions */
diff --git a/drivers/staging/frontier/surface_sysfs.h b/drivers/staging/frontier/surface_sysfs.h
new file mode 100644
index 00000000000..d50a562d658
--- /dev/null
+++ b/drivers/staging/frontier/surface_sysfs.h
@@ -0,0 +1,100 @@
+/* If you are going to abuse the preprocessor, why not ABUSE the preprocessor?
+   I stuck this header in a separate file so I don't have to look at it */
+
+// FIXME Need locking or atomic ops
+
+#define show_set_mbit(dname,value,bit)																			\
+static ssize_t show_##value(struct device *dev, struct device_attribute *attr, char *buf)		\
+{									\
+	struct usb_interface *intf = to_usb_interface(dev);		\
+	struct usb_##dname *t = usb_get_intfdata(intf);			\
+	int temp = (1 && (t->value & (1 << bit)));					\
+	return sprintf(buf, "%d\n", temp);			\
+}									\
+static ssize_t set_##value(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)	\
+{									\
+	struct usb_interface *intf = to_usb_interface(dev);		\
+	struct usb_##dname *t = usb_get_intfdata(intf);			\
+	int temp = simple_strtoul(buf, NULL, 10);			\
+	if(temp > 0) { long b = 1 << bit; t->value |= b; } \
+	else { long b = ~(1 << bit); t->value &= b ;				 \
+	return count;							\
+}									\
+static DEVICE_ATTR(value, S_IWUGO | S_IRUGO, show_##value, set_##value);
+
+#define show_set_ebit(dname,enumname,value,bit)																			\
+static ssize_t show_##bit(struct device *dev, struct device_attribute *attr, char *buf)		\
+{									\
+	struct usb_interface *intf = to_usb_interface(dev);		\
+	struct usb_##dname *t = usb_get_intfdata(intf);			\
+  enum enumname l = bit; \
+  int temp = t->value & (1 << l);						\
+	return sprintf(buf, "%d\n", temp);			\
+}									\
+static ssize_t set_##bit(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)	\
+{									\
+	struct usb_interface *intf = to_usb_interface(dev);		\
+	struct usb_##dname *t = usb_get_intfdata(intf);			\
+	int temp = simple_strtoul(buf, NULL, 10);			\
+  enum enumname l = bit;\
+	long b  = 1 << l; \
+	if(temp > 0) { t->value |= b; }	\
+	else { t->value &= ~b ;				\
+	return count;							\
+}									\
+static DEVICE_ATTR(value, S_IWUGO | S_IRUGO, show_##value, set_##value);
+
+// FIXME FOR CORRECTLY SETTING HEX from a string
+#define show_set_mcmd(dname,value)																			\
+static ssize_t show_##value(struct device *dev, struct device_attribute *attr, char *buf)		\
+{									\
+	struct usb_interface *intf = to_usb_interface(dev);		\
+	struct usb_##dname *t = usb_get_intfdata(intf);			\
+  int count = 0;\
+	int i; \
+	for (i = 0,i<sizeof(dname); i++) count += snprintf(buf, "%02x",t->dname[i]); \
+  return(count);\
+}									\
+static ssize_t set_##value(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)	\
+{									\
+	struct usb_interface *intf = to_usb_interface(dev);		\
+	struct usb_##dname *t = usb_get_intfdata(intf);			\
+	int temp = simple_strtoul(buf, NULL, 10);			\
+	t->value = temp;						\
+	return count;							\
+}									\
+static DEVICE_ATTR(value, S_IWUGO | S_IRUGO, show_##value, set_##value);
+
+#define show_set_mint(dname,value)				\
+static ssize_t show_##value(struct device *dev, struct device_attribute *attr, char *buf)		\
+{									\
+	struct usb_interface *intf = to_usb_interface(dev);		\
+	struct usb_##dname *t = usb_get_intfdata(intf);			\
+	return sprintf(buf, "%d\n", t->value);			\
+}									\
+static ssize_t set_##value(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)	\
+{									\
+	struct usb_interface *intf = to_usb_interface(dev);		\
+	struct usb_##dname *t = usb_get_intfdata(intf);			\
+	int temp = simple_strtoul(buf, NULL, 10);			\
+	t->value = temp;						\
+	return count;							\
+}									\
+static DEVICE_ATTR(value, S_IWUGO | S_IRUGO, show_##value, set_##value);
+
+#define show_set_mchar(dname,value)																				\
+static ssize_t show_##value(struct device *dev, struct device_attribute *attr, char *buf)		\
+{									\
+	struct usb_interface *intf = to_usb_interface(dev);		\
+	struct usb_##dname *t = usb_get_intfdata(intf);			\
+	return sprintf(buf, "%c\n", t->value);			\
+}									\
+static ssize_t set_##value(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)	\
+{									\
+	struct usb_interface *intf = to_usb_interface(dev);		\
+	struct usb_##dname *t = usb_get_intfdata(intf);			\
+	int temp = simple_strtoul(buf, NULL, 10);			\
+	t->value = temp;						\
+	return count;							\
+}									\
+static DEVICE_ATTR(value, S_IWUGO | S_IRUGO, show_##value, set_##value);
diff --git a/drivers/staging/frontier/tranzport.c b/drivers/staging/frontier/tranzport.c
new file mode 100644
index 00000000000..275faa76238
--- /dev/null
+++ b/drivers/staging/frontier/tranzport.c
@@ -0,0 +1,1006 @@
+/*
+ * Frontier Designs Tranzport driver
+ *
+ * Copyright (C) 2007 Michael Taht (m@taht.net)
+ *
+ * Based on the usbled driver and ldusb drivers by
+ *
+ * Copyright (C) 2004 Greg Kroah-Hartman (greg@kroah.com)
+ * Copyright (C) 2005 Michael Hund <mhund@ld-didactic.de>
+ *
+ * The ldusb driver was, in turn, derived from Lego USB Tower driver
+ * Copyright (C) 2003 David Glance <advidgsf@sourceforge.net>
+ *		 2001-2004 Juergen Stuber <starblue@users.sourceforge.net>
+ *
+ *	This program is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU General Public License as
+ *	published by the Free Software Foundation, version 2.
+ *
+ */
+
+/**
+ * This driver uses a ring buffer for time critical reading of
+ * interrupt in reports and provides read and write methods for
+ * raw interrupt reports.
+ */
+
+/* Note: this currently uses a dumb ringbuffer for reads and writes.
+ * A more optimal driver would cache and kill off outstanding urbs that are
+ * now invalid, and ignore ones that already were in the queue but valid
+ * as we only have 17 commands for the tranzport. In particular this is
+ * key for getting lights to flash in time as otherwise many commands
+ * can be buffered up before the light change makes it to the interface.
+*/
+
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/version.h>
+
+#include <asm/uaccess.h>
+#include <linux/input.h>
+#include <linux/usb.h>
+#include <linux/poll.h>
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 19)
+#include frontier_compat.h
+#endif
+
+/* Define these values to match your devices */
+#define VENDOR_ID   0x165b
+#define PRODUCT_ID	0x8101
+
+#ifdef CONFIG_USB_DYNAMIC_MINORS
+#define USB_TRANZPORT_MINOR_BASE	0
+#else
+// FIXME 176 - is the ldusb driver's minor - apply for a minor soon
+#define USB_TRANZPORT_MINOR_BASE	177
+#endif
+
+/* table of devices that work with this driver */
+static struct usb_device_id usb_tranzport_table [] = {
+	{ USB_DEVICE(VENDOR_ID, PRODUCT_ID) },
+	{ }					/* Terminating entry */
+};
+
+MODULE_DEVICE_TABLE(usb, usb_tranzport_table);
+MODULE_VERSION("0.33");
+MODULE_AUTHOR("Mike Taht <m@taht.net>");
+MODULE_DESCRIPTION("Tranzport USB Driver");
+MODULE_LICENSE("GPL");
+MODULE_SUPPORTED_DEVICE("Frontier Designs Tranzport Control Surface");
+
+/* These two aren't done yet */
+
+#define SUPPRESS_EXTRA_ONLINE_EVENTS 0
+#define BUFFERED_WRITES 0
+
+#define SUPPRESS_EXTRA_OFFLINE_EVENTS 1
+#define COMPRESS_WHEEL_EVENTS 1
+#define BUFFERED_READS 1
+#define RING_BUFFER_SIZE 1000
+#define WRITE_BUFFER_SIZE 34
+#define TRANZPORT_USB_TIMEOUT 10
+
+
+static int debug = 0;
+
+/* Use our own dbg macro */
+#define dbg_info(dev, format, arg...) do { if (debug) dev_info(dev , format , ## arg); } while (0)
+
+/* Module parameters */
+
+module_param(debug, int, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(debug, "Debug enabled or not");
+
+/* All interrupt in transfers are collected in a ring buffer to
+ * avoid racing conditions and get better performance of the driver.
+ */
+
+static int ring_buffer_size = RING_BUFFER_SIZE;
+
+module_param(ring_buffer_size, int,  S_IRUGO);
+MODULE_PARM_DESC(ring_buffer_size, "Read ring buffer size in reports");
+
+/* The write_buffer can one day contain more than one interrupt out transfer.
+ */
+static int write_buffer_size = WRITE_BUFFER_SIZE;
+module_param(write_buffer_size, int,  S_IRUGO);
+MODULE_PARM_DESC(write_buffer_size, "Write buffer size");
+
+/*
+ * Increase the interval for debugging purposes.
+ * or set to 1 to use the standard interval from the endpoint descriptors.
+ */
+
+static int min_interrupt_in_interval = TRANZPORT_USB_TIMEOUT;
+module_param(min_interrupt_in_interval, int, 0);
+MODULE_PARM_DESC(min_interrupt_in_interval, "Minimum interrupt in interval in ms");
+
+static int min_interrupt_out_interval = TRANZPORT_USB_TIMEOUT;
+module_param(min_interrupt_out_interval, int, 0);
+MODULE_PARM_DESC(min_interrupt_out_interval, "Minimum interrupt out interval in ms");
+
+struct tranzport_cmd {
+    unsigned char cmd[8];
+};
+
+enum LightID {
+	LightRecord = 0,
+	LightTrackrec,
+	LightTrackmute,
+	LightTracksolo,
+	LightAnysolo,
+	LightLoop,
+	LightPunch
+	};
+
+/* Structure to hold all of our device specific stuff */
+
+struct usb_tranzport {
+	struct semaphore	sem;		/* locks this structure */
+	struct usb_interface*	intf;		/* save off the usb interface pointer */
+
+	int			open_count;	/* number of times this port has been opened */
+
+	struct tranzport_cmd	(*ring_buffer)[RING_BUFFER_SIZE]; /* just make c happy */
+	unsigned int		ring_head;
+	unsigned int		ring_tail;
+
+	wait_queue_head_t	read_wait;
+	wait_queue_head_t	write_wait;
+
+	unsigned char*		interrupt_in_buffer;
+	struct usb_endpoint_descriptor* interrupt_in_endpoint;
+	struct urb*		interrupt_in_urb;
+	int			interrupt_in_interval;
+	size_t	interrupt_in_endpoint_size;
+	int			interrupt_in_running;
+	int			interrupt_in_done;
+
+	char*			interrupt_out_buffer;
+	struct usb_endpoint_descriptor* interrupt_out_endpoint;
+	struct urb*		interrupt_out_urb;
+	int			interrupt_out_interval;
+	size_t	interrupt_out_endpoint_size;
+	int			interrupt_out_busy;
+
+	/* Sysfs and translation support */
+
+	int event; /* alternate interface to events */
+	int wheel; /* - for negative, 0 for none, + for positive */
+	unsigned char dump_state; /* 0 if disabled 1 if enabled */
+	unsigned char enable; /* 0 if disabled 1 if enabled */
+	unsigned char offline; /* if the device is out of range or asleep */
+	unsigned char compress_wheel; /* flag to compress wheel events */
+	unsigned char light; /* 7 bits used */
+	unsigned char last_cmd[8];
+	unsigned char last_input[8];
+	unsigned char screen[40]; // We'll also have cells
+
+};
+
+/* prevent races between open() and disconnect() */
+static DEFINE_MUTEX(disconnect_mutex);
+
+static struct usb_driver usb_tranzport_driver;
+
+/**
+ *	usb_tranzport_abort_transfers
+ *      aborts transfers and frees associated data structures
+ */
+static void usb_tranzport_abort_transfers(struct usb_tranzport *dev)
+{
+	/* shutdown transfer */
+	if (dev->interrupt_in_running) {
+		dev->interrupt_in_running = 0;
+		if (dev->intf)
+			usb_kill_urb(dev->interrupt_in_urb);
+	}
+	if (dev->interrupt_out_busy)
+		if (dev->intf)
+			usb_kill_urb(dev->interrupt_out_urb);
+}
+
+// FIXME ~light not good enough or correct - need atomic set_bit
+
+#define show_set_light(value)	\
+static ssize_t show_##value(struct device *dev, struct device_attribute *attr, char *buf)		\
+{									\
+	struct usb_interface *intf = to_usb_interface(dev);		\
+	struct usb_tranzport *t = usb_get_intfdata(intf);		\
+	enum LightID light = value;					\
+	int temp = (1 && (t->light & (1 << light)));			\
+	return sprintf(buf, "%d\n", temp );				\
+}									\
+static ssize_t set_##value(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)	\
+{									\
+	struct usb_interface *intf = to_usb_interface(dev);		\
+	struct usb_tranzport *t = usb_get_intfdata(intf);		\
+	int temp = simple_strtoul(buf, NULL, 10);			\
+	enum LightID light = (temp << value) & (t->light << value);	\
+	t->light = (t->light & ~light) ;				\
+	return count;							\
+}									\
+static DEVICE_ATTR(value, S_IWUGO | S_IRUGO, show_##value, set_##value);
+
+show_set_light(LightRecord);
+show_set_light(LightTrackrec);
+show_set_light(LightTrackmute);
+show_set_light(LightTracksolo);
+show_set_light(LightAnysolo);
+show_set_light(LightLoop);
+show_set_light(LightPunch);
+
+
+#define show_set_int(value)	\
+static ssize_t show_##value(struct device *dev, struct device_attribute *attr, char *buf)		\
+{									\
+	struct usb_interface *intf = to_usb_interface(dev);		\
+	struct usb_tranzport *t = usb_get_intfdata(intf);			\
+									\
+	return sprintf(buf, "%d\n", t->value);			\
+}									\
+static ssize_t set_##value(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)	\
+{									\
+	struct usb_interface *intf = to_usb_interface(dev);		\
+	struct usb_tranzport *t = usb_get_intfdata(intf);			\
+	int temp = simple_strtoul(buf, NULL, 10);			\
+									\
+	t->value = temp;						\
+	return count;							\
+}									\
+static DEVICE_ATTR(value, S_IWUGO | S_IRUGO, show_##value, set_##value);
+
+show_set_int(enable);
+show_set_int(offline);
+show_set_int(compress_wheel);
+show_set_int(dump_state);
+show_set_int(wheel);
+show_set_int(event);
+
+#define show_set_cmd(value)	\
+static ssize_t show_##value(struct device *dev, struct device_attribute *attr, char *buf)		\
+{									\
+	struct usb_interface *intf = to_usb_interface(dev);		\
+	struct usb_tranzport *t = usb_get_intfdata(intf);			\
+									\
+	return sprintf(buf, "%d\n", t->value);			\
+}									\
+static ssize_t set_##value(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)	\
+{									\
+	struct usb_interface *intf = to_usb_interface(dev);		\
+	struct usb_tranzport *t = usb_get_intfdata(intf);			\
+	int temp = simple_strtoul(buf, NULL, 10);			\
+									\
+	t->value = temp;						\
+	return count;							\
+}									\
+static DEVICE_ATTR(value, S_IWUGO | S_IRUGO, show_##value, set_##value);
+
+
+
+
+/**
+ *	usb_tranzport_delete
+ */
+static void usb_tranzport_delete(struct usb_tranzport *dev)
+{
+	usb_tranzport_abort_transfers(dev);
+	/*  This is just too twisted to be correct */
+	if(dev->intf != NULL) {
+	device_remove_file(&dev->intf->dev, &dev_attr_LightRecord);
+	device_remove_file(&dev->intf->dev, &dev_attr_LightTrackrec);
+	device_remove_file(&dev->intf->dev, &dev_attr_LightTrackmute);
+	device_remove_file(&dev->intf->dev, &dev_attr_LightTracksolo);
+	device_remove_file(&dev->intf->dev, &dev_attr_LightTrackmute);
+	device_remove_file(&dev->intf->dev, &dev_attr_LightAnysolo);
+	device_remove_file(&dev->intf->dev, &dev_attr_LightLoop);
+	device_remove_file(&dev->intf->dev, &dev_attr_LightPunch);
+	device_remove_file(&dev->intf->dev, &dev_attr_wheel);
+	device_remove_file(&dev->intf->dev, &dev_attr_enable);
+	device_remove_file(&dev->intf->dev, &dev_attr_event);
+	device_remove_file(&dev->intf->dev, &dev_attr_offline);
+	device_remove_file(&dev->intf->dev, &dev_attr_compress_wheel);
+
+	device_remove_file(&dev->intf->dev, &dev_attr_dump_state);
+	}
+
+	/* free data structures */
+	usb_free_urb(dev->interrupt_in_urb);
+	usb_free_urb(dev->interrupt_out_urb);
+	kfree(dev->ring_buffer);
+	kfree(dev->interrupt_in_buffer);
+	kfree(dev->interrupt_out_buffer);
+	kfree(dev);
+}
+
+/**
+ *	usb_tranzport_interrupt_in_callback
+ */
+
+static void usb_tranzport_interrupt_in_callback(struct urb *urb)
+{
+	struct usb_tranzport *dev = urb->context;
+	unsigned int next_ring_head;
+	int retval = -1;
+
+	if (urb->status) {
+		if (urb->status == -ENOENT ||
+		    urb->status == -ECONNRESET ||
+		    urb->status == -ESHUTDOWN) {
+			goto exit;
+		} else {
+			dbg_info(&dev->intf->dev, "%s: nonzero status received: %d\n",
+				 __FUNCTION__, urb->status);
+			goto resubmit; /* maybe we can recover */
+		}
+	}
+
+	if (urb->actual_length != 8) {
+		dev_warn(&dev->intf->dev,
+			 "Urb length was %d bytes!! Do something intelligent \n", urb->actual_length);
+	} else {
+		dbg_info(&dev->intf->dev, "%s: received: %02x%02x%02x%02x%02x%02x%02x%02x\n",
+			 __FUNCTION__, dev->interrupt_in_buffer[0],dev->interrupt_in_buffer[1],dev->interrupt_in_buffer[2],dev->interrupt_in_buffer[3],dev->interrupt_in_buffer[4],dev->interrupt_in_buffer[5],dev->interrupt_in_buffer[6],dev->interrupt_in_buffer[7]);
+#if SUPPRESS_EXTRA_OFFLINE_EVENTS
+		if(dev->offline == 2 && dev->interrupt_in_buffer[1] == 0xff) { goto resubmit; }
+		if(dev->offline == 1 && dev->interrupt_in_buffer[1] == 0xff) { dev->offline = 2; goto resubmit; }
+
+/* Always pass one offline event up the stack */
+		if(dev->offline > 0 && dev->interrupt_in_buffer[1] != 0xff) { dev->offline = 0; }
+		if(dev->offline == 0 && dev->interrupt_in_buffer[1] == 0xff) { dev->offline = 1; }
+
+#endif
+		dbg_info(&dev->intf->dev, "%s: head, tail are %x, %x\n", __FUNCTION__,dev->ring_head,dev->ring_tail);
+
+		next_ring_head = (dev->ring_head+1) % ring_buffer_size;
+
+		if (next_ring_head != dev->ring_tail) {
+			memcpy(&((*dev->ring_buffer)[dev->ring_head]), dev->interrupt_in_buffer, urb->actual_length);
+			dev->ring_head = next_ring_head;
+			retval = 0;
+			memset(dev->interrupt_in_buffer, 0, urb->actual_length);
+		} else {
+			dev_warn(&dev->intf->dev,
+				 "Ring buffer overflow, %d bytes dropped\n",
+				 urb->actual_length);
+			memset(dev->interrupt_in_buffer, 0, urb->actual_length);
+		}
+	}
+
+resubmit:
+	/* resubmit if we're still running */
+	if (dev->interrupt_in_running && dev->intf) {
+		retval = usb_submit_urb(dev->interrupt_in_urb, GFP_ATOMIC);
+		if (retval)
+			dev_err(&dev->intf->dev,
+				"usb_submit_urb failed (%d)\n", retval);
+	}
+
+exit:
+	dev->interrupt_in_done = 1;
+	wake_up_interruptible(&dev->read_wait);
+}
+
+/**
+ *	usb_tranzport_interrupt_out_callback
+ */
+static void usb_tranzport_interrupt_out_callback(struct urb *urb)
+{
+	struct usb_tranzport *dev = urb->context;
+
+	/* sync/async unlink faults aren't errors */
+	if (urb->status && !(urb->status == -ENOENT ||
+			     urb->status == -ECONNRESET ||
+			     urb->status == -ESHUTDOWN))
+		dbg_info(&dev->intf->dev,
+			 "%s - nonzero write interrupt status received: %d\n",
+			 __FUNCTION__, urb->status);
+
+	dev->interrupt_out_busy = 0;
+	wake_up_interruptible(&dev->write_wait);
+}
+
+/**
+ *	usb_tranzport_open
+ */
+static int usb_tranzport_open(struct inode *inode, struct file *file)
+{
+	struct usb_tranzport *dev;
+	int subminor;
+	int retval = 0;
+	struct usb_interface *interface;
+
+	nonseekable_open(inode, file);
+	subminor = iminor(inode);
+
+	mutex_lock(&disconnect_mutex);
+
+	interface = usb_find_interface(&usb_tranzport_driver, subminor);
+
+	if (!interface) {
+		err("%s - error, can't find device for minor %d\n",
+		     __FUNCTION__, subminor);
+		retval = -ENODEV;
+		goto unlock_disconnect_exit;
+	}
+
+	dev = usb_get_intfdata(interface);
+
+	if (!dev) {
+		retval = -ENODEV;
+		goto unlock_disconnect_exit;
+	}
+
+	/* lock this device */
+	if (down_interruptible(&dev->sem)) {
+		retval = -ERESTARTSYS;
+		goto unlock_disconnect_exit;
+	}
+
+	/* allow opening only once */
+	if (dev->open_count) {
+		retval = -EBUSY;
+		goto unlock_exit;
+	}
+	dev->open_count = 1;
+
+	/* initialize in direction */
+	dev->ring_head = 0;
+	dev->ring_tail = 0;
+	usb_fill_int_urb(dev->interrupt_in_urb,
+			 interface_to_usbdev(interface),
+			 usb_rcvintpipe(interface_to_usbdev(interface),
+					dev->interrupt_in_endpoint->bEndpointAddress),
+			 dev->interrupt_in_buffer,
+			 dev->interrupt_in_endpoint_size,
+			 usb_tranzport_interrupt_in_callback,
+			 dev,
+			 dev->interrupt_in_interval);
+
+	dev->interrupt_in_running = 1;
+	dev->interrupt_in_done = 0;
+	dev->enable = 1;
+	dev->offline = 0;
+	dev->compress_wheel = 1;
+
+	retval = usb_submit_urb(dev->interrupt_in_urb, GFP_KERNEL);
+	if (retval) {
+		dev_err(&interface->dev, "Couldn't submit interrupt_in_urb %d\n", retval);
+		dev->interrupt_in_running = 0;
+		dev->open_count = 0;
+		goto unlock_exit;
+	}
+
+	/* save device in the file's private structure */
+	file->private_data = dev;
+
+
+unlock_exit:
+	up(&dev->sem);
+
+unlock_disconnect_exit:
+	mutex_unlock(&disconnect_mutex);
+
+	return retval;
+}
+
+/**
+ *	usb_tranzport_release
+ */
+static int usb_tranzport_release(struct inode *inode, struct file *file)
+{
+	struct usb_tranzport *dev;
+	int retval = 0;
+
+	dev = file->private_data;
+
+	if (dev == NULL) {
+		retval = -ENODEV;
+		goto exit;
+	}
+
+	if (down_interruptible(&dev->sem)) {
+		retval = -ERESTARTSYS;
+		goto exit;
+	}
+
+	if (dev->open_count != 1) {
+		retval = -ENODEV;
+		goto unlock_exit;
+	}
+
+	if (dev->intf == NULL) {
+		/* the device was unplugged before the file was released */
+		up(&dev->sem);
+		/* unlock here as usb_tranzport_delete frees dev */
+		usb_tranzport_delete(dev);
+		retval = -ENODEV;
+		goto exit;
+	}
+
+	/* wait until write transfer is finished */
+	if (dev->interrupt_out_busy)
+		wait_event_interruptible_timeout(dev->write_wait, !dev->interrupt_out_busy, 2 * HZ);
+	usb_tranzport_abort_transfers(dev);
+	dev->open_count = 0;
+
+unlock_exit:
+	up(&dev->sem);
+
+exit:
+	return retval;
+}
+
+/**
+ *	usb_tranzport_poll
+ */
+static unsigned int usb_tranzport_poll(struct file *file, poll_table *wait)
+{
+	struct usb_tranzport *dev;
+	unsigned int mask = 0;
+
+	dev = file->private_data;
+
+	poll_wait(file, &dev->read_wait, wait);
+	poll_wait(file, &dev->write_wait, wait);
+
+	if (dev->ring_head != dev->ring_tail)
+		mask |= POLLIN | POLLRDNORM;
+	if (!dev->interrupt_out_busy)
+		mask |= POLLOUT | POLLWRNORM;
+
+	return mask;
+}
+
+/**
+ *	usb_tranzport_read
+ */
+static ssize_t usb_tranzport_read(struct file *file, char __user *buffer, size_t count,
+			   loff_t *ppos)
+{
+	struct usb_tranzport *dev;
+	int retval = 0;
+
+#if BUFFERED_READS
+	int c = 0;
+#endif
+
+#if COMPRESS_WHEEL_EVENTS
+	signed char oldwheel;
+	signed char newwheel;
+	int cancompress = 1;
+	int next_tail;
+#endif
+
+/* do I have such a thing as a null event? */
+
+	dev = file->private_data;
+
+	/* verify that we actually have some data to read */
+	if (count == 0)
+		goto exit;
+
+	/* lock this object */
+	if (down_interruptible(&dev->sem)) {
+		retval = -ERESTARTSYS;
+		goto exit;
+	}
+
+	/* verify that the device wasn't unplugged */
+	if (dev->intf == NULL) {
+		retval = -ENODEV;
+		err("No device or device unplugged %d\n", retval);
+		goto unlock_exit;
+	}
+
+	while (dev->ring_head == dev->ring_tail) {
+
+		if (file->f_flags & O_NONBLOCK) {
+			retval = -EAGAIN;
+			goto unlock_exit;
+		}
+		// atomic_cmp_exchange(&dev->interrupt_in_done,0,0);
+		dev->interrupt_in_done = 0 ; /* tiny race - FIXME: make atomic? */
+		retval = wait_event_interruptible(dev->read_wait, dev->interrupt_in_done);
+		if (retval < 0) {
+			goto unlock_exit;
+		}
+	}
+
+	dbg_info(&dev->intf->dev, "%s: copying to userspace: %02x%02x%02x%02x%02x%02x%02x%02x\n",
+			 __FUNCTION__, (*dev->ring_buffer)[dev->ring_tail].cmd[0],(*dev->ring_buffer)[dev->ring_tail].cmd[1],(*dev->ring_buffer)[dev->ring_tail].cmd[2],(*dev->ring_buffer)[dev->ring_tail].cmd[3],(*dev->ring_buffer)[dev->ring_tail].cmd[4],(*dev->ring_buffer)[dev->ring_tail].cmd[5],(*dev->ring_buffer)[dev->ring_tail].cmd[6],(*dev->ring_buffer)[dev->ring_tail].cmd[7]);
+
+#if BUFFERED_READS
+	   c = 0;
+	   while((c < count) && (dev->ring_tail != dev->ring_head)) {
+
+/* This started off in the lower level service routine, and I moved it here. Then my brain died. Not done yet. */
+#if COMPRESS_WHEEL_EVENTS
+		next_tail = (dev->ring_tail+1) % ring_buffer_size;
+		if(dev->compress_wheel) cancompress = 1;
+		while(dev->ring_head != next_tail && cancompress == 1 ) {
+			newwheel = (*dev->ring_buffer)[next_tail].cmd[6];
+			oldwheel = (*dev->ring_buffer)[dev->ring_tail].cmd[6];
+			// if both are wheel events, and no buttons have changes (FIXME, do I have to check?),
+			// and we are the same sign, we can compress +- 7F
+			// FIXME: saner check for overflow! - max of +- 7F
+			// FIXME the math is wrong for going in reverse, actually, as the midi spec doesn't allow signed chars
+
+	dbg_info(&dev->intf->dev, "%s: trying to compress: %02x%02x%02x%02x%02x %02x %02x %02x\n",
+			 __FUNCTION__, (*dev->ring_buffer)[dev->ring_tail].cmd[0],(*dev->ring_buffer)[dev->ring_tail].cmd[1],(*dev->ring_buffer)[dev->ring_tail].cmd[2],(*dev->ring_buffer)[dev->ring_tail].cmd[3],(*dev->ring_buffer)[dev->ring_tail].cmd[4],(*dev->ring_buffer)[dev->ring_tail].cmd[5],(*dev->ring_buffer)[dev->ring_tail].cmd[6],(*dev->ring_buffer)[dev->ring_tail].cmd[7]);
+
+
+			if(((*dev->ring_buffer)[dev->ring_tail].cmd[6] != 0 &&
+			    (*dev->ring_buffer)[next_tail].cmd[6] != 0 ) &&
+				((newwheel > 0 && oldwheel > 0) ||
+				(newwheel < 0 && oldwheel < 0)) &&
+				((*dev->ring_buffer)[dev->ring_tail].cmd[2] == (*dev->ring_buffer)[next_tail].cmd[2]) &&
+				((*dev->ring_buffer)[dev->ring_tail].cmd[3] == (*dev->ring_buffer)[next_tail].cmd[3]) &&
+				((*dev->ring_buffer)[dev->ring_tail].cmd[4] == (*dev->ring_buffer)[next_tail].cmd[4]) &&
+				((*dev->ring_buffer)[dev->ring_tail].cmd[5] == (*dev->ring_buffer)[next_tail].cmd[5]))
+ {
+	dbg_info(&dev->intf->dev, "%s: should compress: %02x%02x%02x%02x%02x%02x%02x%02x\n",
+			 __FUNCTION__, (*dev->ring_buffer)[dev->ring_tail].cmd[0],(*dev->ring_buffer)[dev->ring_tail].cmd[1],(*dev->ring_buffer)[dev->ring_tail].cmd[2],(*dev->ring_buffer)[dev->ring_tail].cmd[3],(*dev->ring_buffer)[dev->ring_tail].cmd[4],(*dev->ring_buffer)[dev->ring_tail].cmd[5],(*dev->ring_buffer)[dev->ring_tail].cmd[6],(*dev->ring_buffer)[dev->ring_tail].cmd[7]);
+
+				newwheel += oldwheel;
+				if(oldwheel > 0 && !(newwheel > 0)) {
+					newwheel = 0x7f;
+					cancompress = 0;
+				}
+				if(oldwheel < 0 && !(newwheel < 0)) {
+					newwheel = 0x80;
+					cancompress = 0;
+				}
+
+				(*dev->ring_buffer)[next_tail].cmd[6] = newwheel;
+				dev->ring_tail = next_tail;
+				next_tail = (dev->ring_tail+1) % ring_buffer_size;
+			} else {
+				cancompress = 0;
+			}
+		}
+#endif /* COMPRESS_WHEEL_EVENTS */
+
+		if (copy_to_user(&buffer[c], &(*dev->ring_buffer)[dev->ring_tail], 8)) {
+			retval = -EFAULT;
+			goto unlock_exit;
+		}
+
+		dev->ring_tail = (dev->ring_tail+1) % ring_buffer_size;
+		c+=8;
+		dbg_info(&dev->intf->dev, "%s: head, tail are %x, %x\n", __FUNCTION__,dev->ring_head,dev->ring_tail);
+	   }
+	   retval = c;
+
+#else
+	if (copy_to_user(buffer, &(*dev->ring_buffer)[dev->ring_tail], 8)) {
+		retval = -EFAULT;
+		goto unlock_exit;
+	}
+
+	dev->ring_tail = (dev->ring_tail+1) % ring_buffer_size;
+	dbg_info(&dev->intf->dev, "%s: head, tail are %x, %x\n", __FUNCTION__,dev->ring_head,dev->ring_tail);
+
+	retval = 8;
+#endif /* BUFFERED_READS */
+
+unlock_exit:
+	/* unlock the device */
+	up(&dev->sem);
+
+exit:
+	return retval;
+}
+
+/**
+ *	usb_tranzport_write
+ */
+static ssize_t usb_tranzport_write(struct file *file, const char __user *buffer,
+			    size_t count, loff_t *ppos)
+{
+	struct usb_tranzport *dev;
+	size_t bytes_to_write;
+	int retval = 0;
+
+	dev = file->private_data;
+
+	/* verify that we actually have some data to write */
+	if (count == 0)
+		goto exit;
+
+	/* lock this object */
+	if (down_interruptible(&dev->sem)) {
+		retval = -ERESTARTSYS;
+		goto exit;
+	}
+
+	/* verify that the device wasn't unplugged */
+	if (dev->intf == NULL) {
+		retval = -ENODEV;
+		err("No device or device unplugged %d\n", retval);
+		goto unlock_exit;
+	}
+
+	/* wait until previous transfer is finished */
+	if (dev->interrupt_out_busy) {
+		if (file->f_flags & O_NONBLOCK) {
+			retval = -EAGAIN;
+			goto unlock_exit;
+		}
+		retval = wait_event_interruptible(dev->write_wait, !dev->interrupt_out_busy);
+		if (retval < 0) {
+			goto unlock_exit;
+		}
+	}
+
+	/* write the data into interrupt_out_buffer from userspace */
+	bytes_to_write = min(count, write_buffer_size*dev->interrupt_out_endpoint_size);
+	if (bytes_to_write < count)
+		dev_warn(&dev->intf->dev, "Write buffer overflow, %zd bytes dropped\n",count-bytes_to_write);
+
+	dbg_info(&dev->intf->dev, "%s: count = %zd, bytes_to_write = %zd\n", __FUNCTION__, count, bytes_to_write);
+
+	if (copy_from_user(dev->interrupt_out_buffer, buffer, bytes_to_write)) {
+		retval = -EFAULT;
+		goto unlock_exit;
+	}
+
+	if (dev->interrupt_out_endpoint == NULL) {
+		err("Endpoint should not be be null! \n");
+		goto unlock_exit;
+	}
+
+	/* send off the urb */
+	usb_fill_int_urb(dev->interrupt_out_urb,
+			 interface_to_usbdev(dev->intf),
+			 usb_sndintpipe(interface_to_usbdev(dev->intf),
+					dev->interrupt_out_endpoint->bEndpointAddress),
+			 dev->interrupt_out_buffer,
+			 bytes_to_write,
+			 usb_tranzport_interrupt_out_callback,
+			 dev,
+			 dev->interrupt_out_interval);
+
+	dev->interrupt_out_busy = 1;
+	wmb();
+
+	retval = usb_submit_urb(dev->interrupt_out_urb, GFP_KERNEL);
+	if (retval) {
+		dev->interrupt_out_busy = 0;
+		err("Couldn't submit interrupt_out_urb %d\n", retval);
+		goto unlock_exit;
+	}
+	retval = bytes_to_write;
+
+unlock_exit:
+	/* unlock the device */
+	up(&dev->sem);
+
+exit:
+	return retval;
+}
+
+/* file operations needed when we register this driver */
+static const struct file_operations usb_tranzport_fops = {
+	.owner =	THIS_MODULE,
+	.read  =	usb_tranzport_read,
+	.write =	usb_tranzport_write,
+	.open =		usb_tranzport_open,
+	.release =	usb_tranzport_release,
+	.poll =		usb_tranzport_poll,
+};
+
+/*
+ * usb class driver info in order to get a minor number from the usb core,
+ * and to have the device registered with the driver core
+ */
+static struct usb_class_driver usb_tranzport_class = {
+	.name =		"tranzport%d",
+	.fops =		&usb_tranzport_fops,
+	.minor_base =	USB_TRANZPORT_MINOR_BASE,
+};
+
+
+/**
+ *	usb_tranzport_probe
+ *
+ *	Called by the usb core when a new device is connected that it thinks
+ *	this driver might be interested in.
+ */
+static int usb_tranzport_probe(struct usb_interface *intf, const struct usb_device_id *id)
+{
+	struct usb_device *udev = interface_to_usbdev(intf);
+	struct usb_tranzport *dev = NULL;
+	struct usb_host_interface *iface_desc;
+	struct usb_endpoint_descriptor *endpoint;
+	int i;
+	int true_size;
+	int retval = -ENOMEM;
+
+	/* allocate memory for our device state and intialize it */
+
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (dev == NULL) {
+		dev_err(&intf->dev, "Out of memory\n");
+		goto exit;
+	}
+	init_MUTEX(&dev->sem);
+	dev->intf = intf;
+	init_waitqueue_head(&dev->read_wait);
+	init_waitqueue_head(&dev->write_wait);
+
+	iface_desc = intf->cur_altsetting;
+
+	/* set up the endpoint information */
+	for (i = 0; i < iface_desc->desc.bNumEndpoints; ++i) {
+		endpoint = &iface_desc->endpoint[i].desc;
+
+		if (usb_endpoint_is_int_in(endpoint))
+			dev->interrupt_in_endpoint = endpoint;
+
+		if (usb_endpoint_is_int_out(endpoint))
+			dev->interrupt_out_endpoint = endpoint;
+	}
+	if (dev->interrupt_in_endpoint == NULL) {
+		dev_err(&intf->dev, "Interrupt in endpoint not found\n");
+		goto error;
+	}
+	if (dev->interrupt_out_endpoint == NULL)
+		dev_warn(&intf->dev, "Interrupt out endpoint not found (using control endpoint instead)\n");
+
+
+	dev->interrupt_in_endpoint_size = le16_to_cpu(dev->interrupt_in_endpoint->wMaxPacketSize);
+
+	if (dev->interrupt_in_endpoint_size != 8)
+	    dev_warn(&intf->dev, "Interrupt in endpoint size is not 8!\n");
+
+	if(ring_buffer_size == 0) { ring_buffer_size = RING_BUFFER_SIZE; }
+	true_size = min(ring_buffer_size,RING_BUFFER_SIZE);
+	/* FIXME - there are more usb_alloc routines for dma correctness. Needed? */
+
+	dev->ring_buffer = kmalloc((true_size*sizeof(struct tranzport_cmd))+8, GFP_KERNEL);
+
+	if (!dev->ring_buffer) {
+		dev_err(&intf->dev, "Couldn't allocate ring_buffer of size %d\n",true_size);
+		goto error;
+	}
+	dev->interrupt_in_buffer = kmalloc(dev->interrupt_in_endpoint_size, GFP_KERNEL);
+	if (!dev->interrupt_in_buffer) {
+		dev_err(&intf->dev, "Couldn't allocate interrupt_in_buffer\n");
+		goto error;
+	}
+	dev->interrupt_in_urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!dev->interrupt_in_urb) {
+		dev_err(&intf->dev, "Couldn't allocate interrupt_in_urb\n");
+		goto error;
+	}
+	dev->interrupt_out_endpoint_size = dev->interrupt_out_endpoint ? le16_to_cpu(dev->interrupt_out_endpoint->wMaxPacketSize) :
+									 udev->descriptor.bMaxPacketSize0;
+
+	if (dev->interrupt_out_endpoint_size !=8)
+		dev_warn(&intf->dev, "Interrupt out endpoint size is not 8!)\n");
+
+	dev->interrupt_out_buffer = kmalloc(write_buffer_size*dev->interrupt_out_endpoint_size, GFP_KERNEL);
+	if (!dev->interrupt_out_buffer) {
+		dev_err(&intf->dev, "Couldn't allocate interrupt_out_buffer\n");
+		goto error;
+	}
+	dev->interrupt_out_urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!dev->interrupt_out_urb) {
+		dev_err(&intf->dev, "Couldn't allocate interrupt_out_urb\n");
+		goto error;
+	}
+	dev->interrupt_in_interval = min_interrupt_in_interval > dev->interrupt_in_endpoint->bInterval ? min_interrupt_in_interval : dev->interrupt_in_endpoint->bInterval;
+	if (dev->interrupt_out_endpoint)
+		dev->interrupt_out_interval = min_interrupt_out_interval > dev->interrupt_out_endpoint->bInterval ? min_interrupt_out_interval : dev->interrupt_out_endpoint->bInterval;
+
+	/* we can register the device now, as it is ready */
+	usb_set_intfdata(intf, dev);
+
+	retval = usb_register_dev(intf, &usb_tranzport_class);
+	if (retval) {
+		/* something prevented us from registering this driver */
+		dev_err(&intf->dev, "Not able to get a minor for this device.\n");
+		usb_set_intfdata(intf, NULL);
+		goto error;
+	}
+
+	if((retval = device_create_file(&intf->dev, &dev_attr_LightRecord))) goto error;
+	if((retval = device_create_file(&intf->dev, &dev_attr_LightTrackrec))) goto error;
+	if((retval = device_create_file(&intf->dev, &dev_attr_LightTrackmute))) goto error;
+	if((retval = device_create_file(&intf->dev, &dev_attr_LightTracksolo))) goto error;
+	if((retval = device_create_file(&intf->dev, &dev_attr_LightAnysolo))) goto error;
+	if((retval = device_create_file(&intf->dev, &dev_attr_LightLoop))) goto error;
+	if((retval = device_create_file(&intf->dev, &dev_attr_LightPunch))) goto error;
+	if((retval = device_create_file(&intf->dev, &dev_attr_wheel))) goto error;
+	if((retval = device_create_file(&intf->dev, &dev_attr_event))) goto error;
+	if((retval = device_create_file(&intf->dev, &dev_attr_dump_state))) goto error;
+	if((retval = device_create_file(&intf->dev, &dev_attr_compress_wheel))) goto error;
+	if((retval = device_create_file(&intf->dev, &dev_attr_enable))) goto error;
+	if((retval = device_create_file(&intf->dev, &dev_attr_offline))) goto error;
+
+	/* let the user know what node this device is now attached to */
+	dev_info(&intf->dev, "Tranzport Device #%d now attached to major %d minor %d\n",
+		(intf->minor - USB_TRANZPORT_MINOR_BASE), USB_MAJOR, intf->minor);
+
+exit:
+	return retval;
+
+error:
+	usb_tranzport_delete(dev);
+
+	return retval;
+}
+
+/**
+ *	usb_tranzport_disconnect
+ *
+ *	Called by the usb core when the device is removed from the system.
+ */
+static void usb_tranzport_disconnect(struct usb_interface *intf)
+{
+	struct usb_tranzport *dev;
+	int minor;
+	mutex_lock(&disconnect_mutex);
+	dev = usb_get_intfdata(intf);
+	usb_set_intfdata(intf, NULL);
+	down(&dev->sem);
+	minor = intf->minor;
+	/* give back our minor */
+	usb_deregister_dev(intf, &usb_tranzport_class);
+
+	/* if the device is not opened, then we clean up right now */
+	if (!dev->open_count) {
+		up(&dev->sem);
+		usb_tranzport_delete(dev);
+	} else {
+		dev->intf = NULL;
+		up(&dev->sem);
+	}
+
+	mutex_unlock(&disconnect_mutex);
+
+	dev_info(&intf->dev, "Tranzport Surface #%d now disconnected\n",
+		 (minor - USB_TRANZPORT_MINOR_BASE));
+}
+
+/* usb specific object needed to register this driver with the usb subsystem */
+static struct usb_driver usb_tranzport_driver = {
+	.name =		"tranzport",
+	.probe =	usb_tranzport_probe,
+	.disconnect =	usb_tranzport_disconnect,
+	.id_table =	usb_tranzport_table,
+};
+
+/**
+ *	usb_tranzport_init
+ */
+static int __init usb_tranzport_init(void)
+{
+	int retval;
+
+	/* register this driver with the USB subsystem */
+	retval = usb_register(&usb_tranzport_driver);
+	if (retval)
+		err("usb_register failed for the "__FILE__" driver. Error number %d\n", retval);
+
+	return retval;
+}
+
+/**
+ *	usb_tranzport_exit
+ */
+static void __exit usb_tranzport_exit(void)
+{
+	/* deregister this driver with the USB subsystem */
+	usb_deregister(&usb_tranzport_driver);
+}
+
+module_init(usb_tranzport_init);
+module_exit(usb_tranzport_exit);
+
diff --git a/drivers/staging/go7007/Kconfig b/drivers/staging/go7007/Kconfig
index 593fdb767aa..f2cf7f66ae0 100644
--- a/drivers/staging/go7007/Kconfig
+++ b/drivers/staging/go7007/Kconfig
@@ -25,3 +25,13 @@ config VIDEO_GO7007_USB
 	  To compile this driver as a module, choose M here: the
 	  module will be called go7007-usb
 
+config VIDEO_GO7007_USB_S2250_BOARD
+	tristate "Sensoray 2250/2251 support"
+	depends on VIDEO_GO7007_USB && DVB_USB
+	default N
+	---help---
+	  This is a video4linux driver for the Sensoray 2250/2251 device
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called s2250-board
+
diff --git a/drivers/staging/go7007/Makefile b/drivers/staging/go7007/Makefile
index 9b9310cae1c..e514b4af6d0 100644
--- a/drivers/staging/go7007/Makefile
+++ b/drivers/staging/go7007/Makefile
@@ -5,14 +5,23 @@
 
 obj-$(CONFIG_VIDEO_GO7007) += go7007.o
 obj-$(CONFIG_VIDEO_GO7007_USB) += go7007-usb.o
+obj-$(CONFIG_VIDEO_GO7007_USB_S2250_BOARD) += s2250.o
 
-go7007-objs += go7007-v4l2.o go7007-driver.o go7007-i2c.o go7007-fw.o snd-go7007.o
+go7007-objs += go7007-v4l2.o go7007-driver.o go7007-i2c.o go7007-fw.o \
+		snd-go7007.o wis-saa7113.o
 
+s2250-objs += s2250-board.o s2250-loader.o
 
-#ifneq ($(SAA7134_BUILD),)
-#obj-m += saa7134-go7007.o
+# Uncompile when the saa7134 patches get into upstream
+#ifneq ($(CONFIG_VIDEO_SAA7134),)
+#obj-$(CONFIG_VIDEO_SAA7134) += saa7134-go7007.o
+#EXTRA_CFLAGS += -Idrivers/media/video/saa7134
 #endif
 
+ifneq ($(CONFIG_VIDEO_GO7007_USB_S2250_BOARD),)
+EXTRA_CFLAGS += -Idrivers/media/dvb/dvb-usb
+endif
+
 EXTRA_CFLAGS += -Idrivers/staging/saa7134
 EXTRA_CFLAGS += -Idrivers/media/dvb/frontends
 EXTRA_CFLAGS += -Idrivers/media/dvb/dvb-core
diff --git a/drivers/staging/go7007/go7007-driver.c b/drivers/staging/go7007/go7007-driver.c
index e4ead96679c..58bfc8d81b3 100644
--- a/drivers/staging/go7007/go7007-driver.c
+++ b/drivers/staging/go7007/go7007-driver.c
@@ -217,6 +217,9 @@ static int init_i2c_module(struct i2c_adapter *adapter, int id, int addr)
 	case I2C_DRIVERID_WIS_OV7640:
 		modname = "wis-ov7640";
 		break;
+	case I2C_DRIVERID_S2250:
+		modname = "s2250-board";
+		break;
 	default:
 		modname = NULL;
 		break;
@@ -227,7 +230,7 @@ static int init_i2c_module(struct i2c_adapter *adapter, int id, int addr)
 		return 0;
 	if (modname != NULL)
 		printk(KERN_INFO
-			"go7007: probing for module %s failed", modname);
+			"go7007: probing for module %s failed\n", modname);
 	else
 		printk(KERN_INFO
 			"go7007: sensor %u seems to be unsupported!\n", id);
diff --git a/drivers/staging/go7007/go7007-fw.c b/drivers/staging/go7007/go7007-fw.c
index a0e17b0e0ce..871ed43e4e0 100644
--- a/drivers/staging/go7007/go7007-fw.c
+++ b/drivers/staging/go7007/go7007-fw.c
@@ -284,7 +284,7 @@ static const int zz[64] = {
 	58, 59, 52, 45, 38, 31, 39, 46, 53, 60, 61, 54, 47, 55, 62, 63
 };
 
-static int copy_packages(u16 *dest, u16 *src, int pkg_cnt, int space)
+static int copy_packages(__le16 *dest, u16 *src, int pkg_cnt, int space)
 {
 	int i, cnt = pkg_cnt * 32;
 
@@ -292,7 +292,7 @@ static int copy_packages(u16 *dest, u16 *src, int pkg_cnt, int space)
 		return -1;
 
 	for (i = 0; i < cnt; ++i)
-		dest[i] = __cpu_to_le16(src[i]);
+		dest[i] = cpu_to_le16p(src + i);
 
 	return cnt;
 }
@@ -372,7 +372,7 @@ static int mjpeg_frame_header(struct go7007 *go, unsigned char *buf, int q)
 	return p;
 }
 
-static int gen_mjpeghdr_to_package(struct go7007 *go, u16 *code, int space)
+static int gen_mjpeghdr_to_package(struct go7007 *go, __le16 *code, int space)
 {
 	u8 *buf;
 	u16 mem = 0x3e00;
@@ -643,7 +643,7 @@ static int mpeg1_sequence_header(struct go7007 *go, unsigned char *buf, int ext)
 }
 
 static int gen_mpeg1hdr_to_package(struct go7007 *go,
-					u16 *code, int space, int *framelen)
+					__le16 *code, int space, int *framelen)
 {
 	u8 *buf;
 	u16 mem = 0x3e00;
@@ -831,7 +831,7 @@ static int mpeg4_sequence_header(struct go7007 *go, unsigned char *buf, int ext)
 }
 
 static int gen_mpeg4hdr_to_package(struct go7007 *go,
-					u16 *code, int space, int *framelen)
+					__le16 *code, int space, int *framelen)
 {
 	u8 *buf;
 	u16 mem = 0x3e00;
@@ -936,7 +936,7 @@ done:
 }
 
 static int brctrl_to_package(struct go7007 *go,
-					u16 *code, int space, int *framelen)
+					__le16 *code, int space, int *framelen)
 {
 	int converge_speed = 0;
 	int lambda = (go->format == GO7007_FORMAT_MJPEG || go->dvd_mode) ?
@@ -1091,7 +1091,7 @@ static int brctrl_to_package(struct go7007 *go,
 	return copy_packages(code, pack, 6, space);
 }
 
-static int config_package(struct go7007 *go, u16 *code, int space)
+static int config_package(struct go7007 *go, __le16 *code, int space)
 {
 	int fps = go->sensor_framerate / go->fps_scale / 1000;
 	int rows = go->interlace_coding ? go->height / 32 : go->height / 16;
@@ -1213,7 +1213,7 @@ static int config_package(struct go7007 *go, u16 *code, int space)
 	return copy_packages(code, pack, 5, space);
 }
 
-static int seqhead_to_package(struct go7007 *go, u16 *code, int space,
+static int seqhead_to_package(struct go7007 *go, __le16 *code, int space,
 	int (*sequence_header_func)(struct go7007 *go,
 		unsigned char *buf, int ext))
 {
@@ -1292,7 +1292,7 @@ static int relative_prime(int big, int little)
 	return big;
 }
 
-static int avsync_to_package(struct go7007 *go, u16 *code, int space)
+static int avsync_to_package(struct go7007 *go, __le16 *code, int space)
 {
 	int arate = go->board_info->audio_rate * 1001 * go->fps_scale;
 	int ratio = arate / go->sensor_framerate;
@@ -1323,7 +1323,7 @@ static int avsync_to_package(struct go7007 *go, u16 *code, int space)
 	return copy_packages(code, pack, 1, space);
 }
 
-static int final_package(struct go7007 *go, u16 *code, int space)
+static int final_package(struct go7007 *go, __le16 *code, int space)
 {
 	int rows = go->interlace_coding ? go->height / 32 : go->height / 16;
 	u16 pack[] = {
@@ -1386,7 +1386,7 @@ static int final_package(struct go7007 *go, u16 *code, int space)
 	return copy_packages(code, pack, 1, space);
 }
 
-static int audio_to_package(struct go7007 *go, u16 *code, int space)
+static int audio_to_package(struct go7007 *go, __le16 *code, int space)
 {
 	int clock_config = ((go->board_info->audio_flags &
 				GO7007_AUDIO_I2S_MASTER ? 1 : 0) << 11) |
@@ -1436,7 +1436,7 @@ static int audio_to_package(struct go7007 *go, u16 *code, int space)
 	return copy_packages(code, pack, 2, space);
 }
 
-static int modet_to_package(struct go7007 *go, u16 *code, int space)
+static int modet_to_package(struct go7007 *go, __le16 *code, int space)
 {
 	int ret, mb, i, addr, cnt = 0;
 	u16 pack[32];
@@ -1505,7 +1505,7 @@ static int modet_to_package(struct go7007 *go, u16 *code, int space)
 	return cnt;
 }
 
-static int do_special(struct go7007 *go, u16 type, u16 *code, int space,
+static int do_special(struct go7007 *go, u16 type, __le16 *code, int space,
 			int *framelen)
 {
 	switch (type) {
@@ -1555,7 +1555,7 @@ static int do_special(struct go7007 *go, u16 type, u16 *code, int space,
 int go7007_construct_fw_image(struct go7007 *go, u8 **fw, int *fwlen)
 {
 	const struct firmware *fw_entry;
-	u16 *code, *src;
+	__le16 *code, *src;
 	int framelen[8] = { }; /* holds the lengths of empty frame templates */
 	int codespace = 64 * 1024, i = 0, srclen, chunk_len, chunk_flags;
 	int mode_flag;
@@ -1590,7 +1590,7 @@ int go7007_construct_fw_image(struct go7007 *go, u8 **fw, int *fwlen)
 		goto fw_failed;
 	}
 	memset(code, 0, codespace * 2);
-	src = (u16 *)fw_entry->data;
+	src = (__le16 *)fw_entry->data;
 	srclen = fw_entry->size / 2;
 	while (srclen >= 2) {
 		chunk_flags = __le16_to_cpu(src[0]);
diff --git a/drivers/staging/go7007/go7007-priv.h b/drivers/staging/go7007/go7007-priv.h
index 005542d16a5..372f1f1c09b 100644
--- a/drivers/staging/go7007/go7007-priv.h
+++ b/drivers/staging/go7007/go7007-priv.h
@@ -40,6 +40,7 @@ struct go7007;
 #define GO7007_BOARDID_LIFEVIEW_LR192	21 /* TV Walker Ultra */
 #define GO7007_BOARDID_ENDURA		22
 #define GO7007_BOARDID_ADLINK_MPG24	23
+#define GO7007_BOARDID_SENSORAY_2250	24 /* Sensoray 2250/2251 */
 
 /* Various characteristics of each board */
 #define GO7007_BOARD_HAS_AUDIO		(1<<0)
@@ -104,6 +105,7 @@ struct go7007_hpi_ops {
 	int (*stream_start)(struct go7007 *go);
 	int (*stream_stop)(struct go7007 *go);
 	int (*send_firmware)(struct go7007 *go, u8 *data, int len);
+	int (*send_command)(struct go7007 *go, unsigned int cmd, void *arg);
 };
 
 /* The video buffer size must be a multiple of PAGE_SIZE */
diff --git a/drivers/staging/go7007/go7007-usb.c b/drivers/staging/go7007/go7007-usb.c
index 3f5ee3424e7..83eec920c7d 100644
--- a/drivers/staging/go7007/go7007-usb.c
+++ b/drivers/staging/go7007/go7007-usb.c
@@ -225,7 +225,7 @@ static struct go7007_usb_board board_px_tv402u = {
 		.inputs 	 = {
 			{
 				.video_input	= 1,
-		.audio_input	 = TVAUDIO_INPUT_EXTERN,
+				.audio_input	= TVAUDIO_INPUT_EXTERN,
 				.name		= "Composite",
 			},
 			{
@@ -398,6 +398,41 @@ static struct go7007_usb_board board_adlink_mpg24 = {
 	},
 };
 
+static struct go7007_usb_board board_sensoray_2250 = {
+	.flags		= GO7007_USB_EZUSB | GO7007_USB_EZUSB_I2C,
+	.main_info	= {
+		.firmware	 = "go7007tv.bin",
+		.audio_flags	 = GO7007_AUDIO_I2S_MODE_1 |
+					GO7007_AUDIO_I2S_MASTER |
+					GO7007_AUDIO_WORD_16,
+		.flags		 = GO7007_BOARD_HAS_AUDIO,
+		.audio_rate	 = 48000,
+		.audio_bclk_div	 = 8,
+		.audio_main_div	 = 2,
+		.hpi_buffer_cap  = 7,
+		.sensor_flags	 = GO7007_SENSOR_656 |
+					GO7007_SENSOR_TV,
+		.num_i2c_devs	 = 1,
+		.i2c_devs	 = {
+			{
+				.id	= I2C_DRIVERID_S2250,
+				.addr	= 0x34,
+			},
+		},
+		.num_inputs	 = 2,
+		.inputs 	 = {
+			{
+				.video_input	= 0,
+				.name		= "Composite",
+			},
+			{
+				.video_input	= 1,
+				.name		= "S-Video",
+			},
+		},
+	},
+};
+
 static struct usb_device_id go7007_usb_id_table[] = {
 	{
 		.match_flags	= USB_DEVICE_ID_MATCH_DEVICE_AND_VERSION |
@@ -491,6 +526,14 @@ static struct usb_device_id go7007_usb_id_table[] = {
 		.bcdDevice_hi	= 0x1,
 		.driver_info	= (kernel_ulong_t)GO7007_BOARDID_LIFEVIEW_LR192,
 	},
+	{
+		.match_flags	= USB_DEVICE_ID_MATCH_DEVICE_AND_VERSION,
+		.idVendor	= 0x1943,  /* Vendor ID Sensoray */
+		.idProduct	= 0x2250,  /* Product ID of 2250/2251 */
+		.bcdDevice_lo	= 0x1,
+		.bcdDevice_hi	= 0x1,
+		.driver_info	= (kernel_ulong_t)GO7007_BOARDID_SENSORAY_2250,
+	},
 	{ }					/* Terminating entry */
 };
 
@@ -637,9 +680,10 @@ static void go7007_usb_readinterrupt_complete(struct urb *urb)
 {
 	struct go7007 *go = (struct go7007 *)urb->context;
 	u16 *regs = (u16 *)urb->transfer_buffer;
+	int status = urb->status;
 
-	if (urb->status != 0) {
-		if (urb->status != -ESHUTDOWN &&
+	if (status) {
+		if (status != -ESHUTDOWN &&
 				go->status != STATUS_SHUTDOWN) {
 			printk(KERN_ERR
 				"go7007-usb: error in read interrupt: %d\n",
@@ -680,15 +724,14 @@ static int go7007_usb_read_interrupt(struct go7007 *go)
 static void go7007_usb_read_video_pipe_complete(struct urb *urb)
 {
 	struct go7007 *go = (struct go7007 *)urb->context;
-	int r;
+	int r, status = urb-> status;
 
 	if (!go->streaming) {
 		wake_up_interruptible(&go->frame_waitq);
 		return;
 	}
-	if (urb->status != 0) {
-		printk(KERN_ERR "go7007-usb: error in video pipe: %d\n",
-				urb->status);
+	if (status) {
+		printk(KERN_ERR "go7007-usb: error in video pipe: %d\n", status);
 		return;
 	}
 	if (urb->actual_length != urb->transfer_buffer_length) {
@@ -704,13 +747,12 @@ static void go7007_usb_read_video_pipe_complete(struct urb *urb)
 static void go7007_usb_read_audio_pipe_complete(struct urb *urb)
 {
 	struct go7007 *go = (struct go7007 *)urb->context;
-	int r;
+	int r, status = urb->status;
 
 	if (!go->streaming)
 		return;
-	if (urb->status != 0) {
-		printk(KERN_ERR "go7007-usb: error in audio pipe: %d\n",
-				urb->status);
+	if (status) {
+		printk(KERN_ERR "go7007-usb: error in audio pipe: %d\n", status);
 		return;
 	}
 	if (urb->actual_length != urb->transfer_buffer_length) {
@@ -751,7 +793,7 @@ static int go7007_usb_stream_start(struct go7007 *go)
 	return 0;
 
 audio_submit_failed:
-	for (i = 0; i < 8; ++i)
+	for (i = 0; i < 7; ++i)
 		usb_kill_urb(usb->audio_urbs[i]);
 video_submit_failed:
 	for (i = 0; i < 8; ++i)
@@ -965,16 +1007,20 @@ static int go7007_usb_probe(struct usb_interface *intf,
 		name = "Lifeview TV Walker Ultra";
 		board = &board_lifeview_lr192;
 		break;
+	case GO7007_BOARDID_SENSORAY_2250:
+		printk(KERN_INFO "Sensoray 2250 found\n");
+		name = "Sensoray 2250/2251\n";
+		board = &board_sensoray_2250;
+		break;
 	default:
 		printk(KERN_ERR "go7007-usb: unknown board ID %d!\n",
 				(unsigned int)id->driver_info);
 		return 0;
 	}
 
-	usb = kmalloc(sizeof(struct go7007_usb), GFP_KERNEL);
+	usb = kzalloc(sizeof(struct go7007_usb), GFP_KERNEL);
 	if (usb == NULL)
 		return -ENOMEM;
-	memset(usb, 0, sizeof(struct go7007_usb));
 
 	/* Allocate the URB and buffer for receiving incoming interrupts */
 	usb->intr_urb = usb_alloc_urb(0, GFP_KERNEL);
@@ -1179,6 +1225,7 @@ static void go7007_usb_disconnect(struct usb_interface *intf)
 {
 	struct go7007 *go = usb_get_intfdata(intf);
 	struct go7007_usb *usb = go->hpi_context;
+	struct urb *vurb, *aurb;
 	int i;
 
 	go->status = STATUS_SHUTDOWN;
@@ -1186,15 +1233,19 @@ static void go7007_usb_disconnect(struct usb_interface *intf)
 
 	/* Free USB-related structs */
 	for (i = 0; i < 8; ++i) {
-		if (usb->video_urbs[i] != NULL) {
-			if (usb->video_urbs[i]->transfer_buffer != NULL)
-				kfree(usb->video_urbs[i]->transfer_buffer);
-			usb_free_urb(usb->video_urbs[i]);
+		vurb = usb->video_urbs[i];
+		if (vurb) {
+			usb_kill_urb(vurb);
+			if (vurb->transfer_buffer)
+				kfree(vurb->transfer_buffer);
+			usb_free_urb(vurb);
 		}
-		if (usb->audio_urbs[i] != NULL) {
-			if (usb->audio_urbs[i]->transfer_buffer != NULL)
-				kfree(usb->audio_urbs[i]->transfer_buffer);
-			usb_free_urb(usb->audio_urbs[i]);
+		aurb = usb->audio_urbs[i];
+		if (aurb) {
+			usb_kill_urb(aurb);
+			if (aurb->transfer_buffer)
+				kfree(aurb->transfer_buffer);
+			usb_free_urb(aurb);
 		}
 	}
 	kfree(usb->intr_urb->transfer_buffer);
diff --git a/drivers/staging/go7007/go7007-v4l2.c b/drivers/staging/go7007/go7007-v4l2.c
index 94e1141a1fc..4f7237a03ad 100644
--- a/drivers/staging/go7007/go7007-v4l2.c
+++ b/drivers/staging/go7007/go7007-v4l2.c
@@ -38,6 +38,14 @@
 #include "go7007-priv.h"
 #include "wis-i2c.h"
 
+/* Temporary defines until accepted in v4l-dvb */
+#ifndef V4L2_MPEG_STREAM_TYPE_MPEG_ELEM
+#define	V4L2_MPEG_STREAM_TYPE_MPEG_ELEM   6 /* MPEG elementary stream */
+#endif
+#ifndef V4L2_MPEG_VIDEO_ENCODING_MPEG_4
+#define	V4L2_MPEG_VIDEO_ENCODING_MPEG_4   3
+#endif
+
 static void deactivate_buffer(struct go7007_buffer *gobuf)
 {
 	int i;
@@ -81,7 +89,7 @@ static int go7007_streamoff(struct go7007 *go)
 	return 0;
 }
 
-static int go7007_open(struct inode *inode, struct file *file)
+static int go7007_open(struct file *file)
 {
 	struct go7007 *go = video_get_drvdata(video_devdata(file));
 	struct go7007_file *gofh;
@@ -99,7 +107,7 @@ static int go7007_open(struct inode *inode, struct file *file)
 	return 0;
 }
 
-static int go7007_release(struct inode *inode, struct file *file)
+static int go7007_release(struct file *file)
 {
 	struct go7007_file *gofh = file->private_data;
 	struct go7007 *go = gofh->go;
@@ -319,6 +327,7 @@ static int set_capture_size(struct go7007 *go, struct v4l2_format *fmt, int try)
 	return 0;
 }
 
+#if 0
 static int clip_to_modet_map(struct go7007 *go, int region,
 		struct v4l2_clip *clip_list)
 {
@@ -375,499 +384,801 @@ static int clip_to_modet_map(struct go7007 *go, int region,
 	return 0;
 }
 
-static int go7007_do_ioctl(struct inode *inode, struct file *file,
-		unsigned int cmd, void *arg)
+static int mpeg_queryctrl(u32 id, struct v4l2_queryctrl *ctrl)
 {
-	struct go7007_file *gofh = file->private_data;
-	struct go7007 *go = gofh->go;
-	unsigned long flags;
-	int retval = 0;
-
-	switch (cmd) {
-	case VIDIOC_QUERYCAP:
-	{
-		struct v4l2_capability *cap = arg;
-
-		memset(cap, 0, sizeof(*cap));
-		strcpy(cap->driver, "go7007");
-		strncpy(cap->card, go->name, sizeof(cap->card));
-		cap->version = KERNEL_VERSION(0, 9, 8);
-		cap->capabilities = V4L2_CAP_VIDEO_CAPTURE |
-				V4L2_CAP_STREAMING; /* | V4L2_CAP_AUDIO; */
-		if (go->board_info->flags & GO7007_BOARD_HAS_TUNER)
-			cap->capabilities |= V4L2_CAP_TUNER;
+	static const u32 user_ctrls[] = {
+		V4L2_CID_USER_CLASS,
+		0
+	};
+	static const u32 mpeg_ctrls[] = {
+		V4L2_CID_MPEG_CLASS,
+		V4L2_CID_MPEG_STREAM_TYPE,
+		V4L2_CID_MPEG_VIDEO_ENCODING,
+		V4L2_CID_MPEG_VIDEO_ASPECT,
+		V4L2_CID_MPEG_VIDEO_GOP_SIZE,
+		V4L2_CID_MPEG_VIDEO_GOP_CLOSURE,
+		V4L2_CID_MPEG_VIDEO_BITRATE,
+		0
+	};
+	static const u32 *ctrl_classes[] = {
+		user_ctrls,
+		mpeg_ctrls,
+		NULL
+	};
+
+	/* The ctrl may already contain the queried i2c controls,
+	 * query the mpeg controls if the existing ctrl id is
+	 * greater than the next mpeg ctrl id.
+	 */
+	id = v4l2_ctrl_next(ctrl_classes, id);
+	if (id >= ctrl->id && ctrl->name[0])
 		return 0;
+
+	memset(ctrl, 0, sizeof(*ctrl));
+	ctrl->id = id;
+
+	switch (ctrl->id) {
+	case V4L2_CID_USER_CLASS:
+	case V4L2_CID_MPEG_CLASS:
+		return v4l2_ctrl_query_fill_std(ctrl);
+	case V4L2_CID_MPEG_STREAM_TYPE:
+		return v4l2_ctrl_query_fill(ctrl,
+				V4L2_MPEG_STREAM_TYPE_MPEG2_DVD,
+				V4L2_MPEG_STREAM_TYPE_MPEG_ELEM, 1,
+				V4L2_MPEG_STREAM_TYPE_MPEG_ELEM);
+	case V4L2_CID_MPEG_VIDEO_ENCODING:
+		return v4l2_ctrl_query_fill(ctrl,
+				V4L2_MPEG_VIDEO_ENCODING_MPEG_1,
+				V4L2_MPEG_VIDEO_ENCODING_MPEG_4, 1,
+				V4L2_MPEG_VIDEO_ENCODING_MPEG_2);
+	case V4L2_CID_MPEG_VIDEO_ASPECT:
+		return v4l2_ctrl_query_fill(ctrl,
+				V4L2_MPEG_VIDEO_ASPECT_1x1,
+				V4L2_MPEG_VIDEO_ASPECT_16x9, 1,
+				V4L2_MPEG_VIDEO_ASPECT_1x1);
+	case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
+	case V4L2_CID_MPEG_VIDEO_GOP_CLOSURE:
+		return v4l2_ctrl_query_fill_std(ctrl);
+	case V4L2_CID_MPEG_VIDEO_BITRATE:
+		return v4l2_ctrl_query_fill(ctrl,
+				64000,
+				10000000, 1,
+				9800000);
+	default:
+		break;
 	}
-	case VIDIOC_ENUM_FMT:
-	{
-		struct v4l2_fmtdesc *fmt = arg;
-		unsigned int index;
-		char *desc;
-		u32 pixelformat;
+	return -EINVAL;
+}
+
+static int mpeg_s_control(struct v4l2_control *ctrl, struct go7007 *go)
+{
+	/* pretty sure we can't change any of these while streaming */
+	if (go->streaming)
+		return -EBUSY;
 
-		if (fmt->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+	switch (ctrl->id) {
+	case V4L2_CID_MPEG_STREAM_TYPE:
+		switch (ctrl->value) {
+		case V4L2_MPEG_STREAM_TYPE_MPEG2_DVD:
+			go->format = GO7007_FORMAT_MPEG2;
+			go->bitrate = 9800000;
+			go->gop_size = 15;
+			go->pali = 0x48;
+			go->closed_gop = 1;
+			go->repeat_seqhead = 0;
+			go->seq_header_enable = 1;
+			go->gop_header_enable = 1;
+			go->dvd_mode = 1;
+			break;
+		case V4L2_MPEG_STREAM_TYPE_MPEG_ELEM:
+			/* todo: */
+			break;
+		default:
 			return -EINVAL;
-		switch (fmt->index) {
-		case 0:
-			pixelformat = V4L2_PIX_FMT_MJPEG;
-			desc = "Motion-JPEG";
+		}
+		break;
+	case V4L2_CID_MPEG_VIDEO_ENCODING:
+		switch (ctrl->value) {
+		case V4L2_MPEG_VIDEO_ENCODING_MPEG_1:
+			go->format = GO7007_FORMAT_MPEG1;
+			go->pali = 0;
 			break;
-		case 1:
-			pixelformat = V4L2_PIX_FMT_MPEG;
-			desc = "MPEG1/MPEG2/MPEG4";
+		case V4L2_MPEG_VIDEO_ENCODING_MPEG_2:
+			go->format = GO7007_FORMAT_MPEG2;
+			/*if (mpeg->pali >> 24 == 2)
+				go->pali = mpeg->pali & 0xff;
+			else*/
+				go->pali = 0x48;
+			break;
+		case V4L2_MPEG_VIDEO_ENCODING_MPEG_4:
+			go->format = GO7007_FORMAT_MPEG4;
+			/*if (mpeg->pali >> 24 == 4)
+				go->pali = mpeg->pali & 0xff;
+			else*/
+				go->pali = 0xf5;
 			break;
 		default:
 			return -EINVAL;
 		}
-		index = fmt->index;
-		memset(fmt, 0, sizeof(*fmt));
-		fmt->index = index;
-		fmt->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-		fmt->flags = V4L2_FMT_FLAG_COMPRESSED;
-		strncpy(fmt->description, desc, sizeof(fmt->description));
-		fmt->pixelformat = pixelformat;
-
-		return 0;
-	}
-	case VIDIOC_TRY_FMT:
-	{
-		struct v4l2_format *fmt = arg;
-
-		if (fmt->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		go->gop_header_enable =
+			/*mpeg->flags & GO7007_MPEG_OMIT_GOP_HEADER
+			? 0 :*/ 1;
+		/*if (mpeg->flags & GO7007_MPEG_REPEAT_SEQHEADER)
+			go->repeat_seqhead = 1;
+		else*/
+			go->repeat_seqhead = 0;
+		go->dvd_mode = 0;
+		break;
+	case V4L2_CID_MPEG_VIDEO_ASPECT:
+		if (go->format == GO7007_FORMAT_MJPEG)
 			return -EINVAL;
-		return set_capture_size(go, fmt, 1);
-	}
-	case VIDIOC_G_FMT:
-	{
-		struct v4l2_format *fmt = arg;
-
-		if (fmt->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		switch (ctrl->value) {
+		case V4L2_MPEG_VIDEO_ASPECT_1x1:
+			go->aspect_ratio = GO7007_RATIO_1_1;
+			break;
+		case V4L2_MPEG_VIDEO_ASPECT_4x3:
+			go->aspect_ratio = GO7007_RATIO_4_3;
+			break;
+		case V4L2_MPEG_VIDEO_ASPECT_16x9:
+			go->aspect_ratio = GO7007_RATIO_16_9;
+			break;
+		case V4L2_MPEG_VIDEO_ASPECT_221x100:
+		default:
 			return -EINVAL;
-		memset(fmt, 0, sizeof(*fmt));
-		fmt->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-		fmt->fmt.pix.width = go->width;
-		fmt->fmt.pix.height = go->height;
-		fmt->fmt.pix.pixelformat = go->format == GO7007_FORMAT_MJPEG ?
-			V4L2_PIX_FMT_MJPEG : V4L2_PIX_FMT_MPEG;
-		fmt->fmt.pix.field = V4L2_FIELD_NONE;
-		fmt->fmt.pix.bytesperline = 0;
-		fmt->fmt.pix.sizeimage = GO7007_BUF_SIZE;
-		fmt->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M; /* ?? */
-		return 0;
-	}
-	case VIDIOC_S_FMT:
-	{
-		struct v4l2_format *fmt = arg;
-
-		if (fmt->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		}
+		break;
+	case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
+		go->gop_size = ctrl->value;
+		break;
+	case V4L2_CID_MPEG_VIDEO_GOP_CLOSURE:
+		if (ctrl->value != 0 && ctrl->value != 1)
 			return -EINVAL;
-		if (go->streaming)
-			return -EBUSY;
-		return set_capture_size(go, fmt, 0);
-	}
-	case VIDIOC_G_FBUF:
-	case VIDIOC_S_FBUF:
-		return -EINVAL;
-	case VIDIOC_REQBUFS:
-	{
-		struct v4l2_requestbuffers *req = arg;
-		unsigned int count, i;
-
-		if (go->streaming)
-			return -EBUSY;
-		if (req->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
-				req->memory != V4L2_MEMORY_MMAP)
+		go->closed_gop = ctrl->value;
+		break;
+	case V4L2_CID_MPEG_VIDEO_BITRATE:
+		/* Upper bound is kind of arbitrary here */
+		if (ctrl->value < 64000 || ctrl->value > 10000000)
 			return -EINVAL;
-
-		down(&gofh->lock);
-		retval = -EBUSY;
-		for (i = 0; i < gofh->buf_count; ++i)
-			if (gofh->bufs[i].mapped > 0)
-				goto unlock_and_return;
-		down(&go->hw_lock);
-		if (go->in_use > 0 && gofh->buf_count == 0) {
-			up(&go->hw_lock);
-			goto unlock_and_return;
-		}
-		if (gofh->buf_count > 0)
-			kfree(gofh->bufs);
-		retval = -ENOMEM;
-		count = req->count;
-		if (count > 0) {
-			if (count < 2)
-				count = 2;
-			if (count > 32)
-				count = 32;
-			gofh->bufs = kmalloc(count *
-						sizeof(struct go7007_buffer),
-					GFP_KERNEL);
-			if (gofh->bufs == NULL) {
-				up(&go->hw_lock);
-				goto unlock_and_return;
-			}
-			memset(gofh->bufs, 0,
-					count * sizeof(struct go7007_buffer));
-			for (i = 0; i < count; ++i) {
-				gofh->bufs[i].go = go;
-				gofh->bufs[i].index = i;
-				gofh->bufs[i].state = BUF_STATE_IDLE;
-				gofh->bufs[i].mapped = 0;
-			}
-			go->in_use = 1;
-		} else {
-			go->in_use = 0;
-		}
-		gofh->buf_count = count;
-		up(&go->hw_lock);
-		up(&gofh->lock);
-		memset(req, 0, sizeof(*req));
-		req->count = count;
-		req->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-		req->memory = V4L2_MEMORY_MMAP;
-		return 0;
+		go->bitrate = ctrl->value;
+		break;
+	default:
+		return -EINVAL;
 	}
-	case VIDIOC_QUERYBUF:
-	{
-		struct v4l2_buffer *buf = arg;
-		unsigned int index;
+	return 0;
+}
 
-		retval = -EINVAL;
-		if (buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+static int mpeg_g_control(struct v4l2_control *ctrl, struct go7007 *go)
+{
+	switch (ctrl->id) {
+	case V4L2_CID_MPEG_STREAM_TYPE:
+		if (go->dvd_mode)
+			ctrl->value = V4L2_MPEG_STREAM_TYPE_MPEG2_DVD;
+		else
+			ctrl->value = V4L2_MPEG_STREAM_TYPE_MPEG_ELEM;
+		break;
+	case V4L2_CID_MPEG_VIDEO_ENCODING:
+		switch (go->format) {
+		case GO7007_FORMAT_MPEG1:
+			ctrl->value = V4L2_MPEG_VIDEO_ENCODING_MPEG_1;
+			break;
+		case GO7007_FORMAT_MPEG2:
+			ctrl->value = V4L2_MPEG_VIDEO_ENCODING_MPEG_2;
+			break;
+		case GO7007_FORMAT_MPEG4:
+			ctrl->value = V4L2_MPEG_VIDEO_ENCODING_MPEG_4;
+			break;
+		default:
 			return -EINVAL;
-		index = buf->index;
-		down(&gofh->lock);
-		if (index >= gofh->buf_count)
-			goto unlock_and_return;
-		memset(buf, 0, sizeof(*buf));
-		buf->index = index;
-		buf->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-		switch (gofh->bufs[index].state) {
-		case BUF_STATE_QUEUED:
-			buf->flags = V4L2_BUF_FLAG_QUEUED;
+		}
+		break;
+	case V4L2_CID_MPEG_VIDEO_ASPECT:
+		switch (go->aspect_ratio) {
+		case GO7007_RATIO_1_1:
+			ctrl->value = V4L2_MPEG_VIDEO_ASPECT_1x1;
 			break;
-		case BUF_STATE_DONE:
-			buf->flags = V4L2_BUF_FLAG_DONE;
+		case GO7007_RATIO_4_3:
+			ctrl->value = V4L2_MPEG_VIDEO_ASPECT_4x3;
+			break;
+		case GO7007_RATIO_16_9:
+			ctrl->value = V4L2_MPEG_VIDEO_ASPECT_16x9;
 			break;
 		default:
-			buf->flags = 0;
+			return -EINVAL;
 		}
-		if (gofh->bufs[index].mapped)
-			buf->flags |= V4L2_BUF_FLAG_MAPPED;
-		buf->memory = V4L2_MEMORY_MMAP;
-		buf->m.offset = index * GO7007_BUF_SIZE;
-		buf->length = GO7007_BUF_SIZE;
-		up(&gofh->lock);
+		break;
+	case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
+		ctrl->value = go->gop_size;
+		break;
+	case V4L2_CID_MPEG_VIDEO_GOP_CLOSURE:
+		ctrl->value = go->closed_gop;
+		break;
+	case V4L2_CID_MPEG_VIDEO_BITRATE:
+		ctrl->value = go->bitrate;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+#endif
 
-		return 0;
+static int vidioc_querycap(struct file *file, void  *priv,
+					struct v4l2_capability *cap)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+
+	strlcpy(cap->driver, "go7007", sizeof(cap->driver));
+	strlcpy(cap->card, go->name, sizeof(cap->card));
+#if 0
+	strlcpy(cap->bus_info, dev_name(&dev->udev->dev), sizeof(cap->bus_info));
+#endif
+
+	cap->version = KERNEL_VERSION(0, 9, 8);
+
+	cap->capabilities = V4L2_CAP_VIDEO_CAPTURE |
+			    V4L2_CAP_STREAMING; /* | V4L2_CAP_AUDIO; */
+
+	if (go->board_info->flags & GO7007_BOARD_HAS_TUNER)
+		cap->capabilities |= V4L2_CAP_TUNER;
+
+	return 0;
+}
+
+static int vidioc_enum_fmt_vid_cap(struct file *file, void  *priv,
+					struct v4l2_fmtdesc *fmt)
+{
+	char *desc = NULL;
+
+	switch (fmt->index) {
+	case 0:
+		fmt->pixelformat = V4L2_PIX_FMT_MJPEG;
+		desc = "Motion-JPEG";
+		break;
+	case 1:
+		fmt->pixelformat = V4L2_PIX_FMT_MPEG;
+		desc = "MPEG1/MPEG2/MPEG4";
+		break;
+	default:
+		return -EINVAL;
 	}
-	case VIDIOC_QBUF:
-	{
-		struct v4l2_buffer *buf = arg;
-		struct go7007_buffer *gobuf;
-		int ret;
+	fmt->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	fmt->flags = V4L2_FMT_FLAG_COMPRESSED;
 
-		retval = -EINVAL;
-		if (buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
-				buf->memory != V4L2_MEMORY_MMAP)
-			return -EINVAL;
-		down(&gofh->lock);
-		if (buf->index < 0 || buf->index >= gofh->buf_count)
-			goto unlock_and_return;
-		gobuf = &gofh->bufs[buf->index];
-		if (gobuf->mapped == 0)
-			goto unlock_and_return;
-		retval = -EBUSY;
-		if (gobuf->state != BUF_STATE_IDLE)
-			goto unlock_and_return;
-		/* offset will be 0 until we really support USERPTR streaming */
-		gobuf->offset = gobuf->user_addr & ~PAGE_MASK;
-		gobuf->bytesused = 0;
-		gobuf->frame_offset = 0;
-		gobuf->modet_active = 0;
-		if (gobuf->offset > 0)
-			gobuf->page_count = GO7007_BUF_PAGES + 1;
-		else
-			gobuf->page_count = GO7007_BUF_PAGES;
-		retval = -ENOMEM;
-		down_read(&current->mm->mmap_sem);
-		ret = get_user_pages(current, current->mm,
-				gobuf->user_addr & PAGE_MASK, gobuf->page_count,
-				1, 1, gobuf->pages, NULL);
-		up_read(&current->mm->mmap_sem);
-		if (ret != gobuf->page_count) {
-			int i;
-			for (i = 0; i < ret; ++i)
-				page_cache_release(gobuf->pages[i]);
-			gobuf->page_count = 0;
+	strncpy(fmt->description, desc, sizeof(fmt->description));
+
+	return 0;
+}
+
+static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
+					struct v4l2_format *fmt)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+
+	fmt->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	fmt->fmt.pix.width = go->width;
+	fmt->fmt.pix.height = go->height;
+	fmt->fmt.pix.pixelformat = (go->format == GO7007_FORMAT_MJPEG) ?
+				   V4L2_PIX_FMT_MJPEG : V4L2_PIX_FMT_MPEG;
+	fmt->fmt.pix.field = V4L2_FIELD_NONE;
+	fmt->fmt.pix.bytesperline = 0;
+	fmt->fmt.pix.sizeimage = GO7007_BUF_SIZE;
+	fmt->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
+
+	return 0;
+}
+
+static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
+			struct v4l2_format *fmt)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+
+	return set_capture_size(go, fmt, 1);
+}
+
+static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
+			struct v4l2_format *fmt)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+
+	if (go->streaming)
+		return -EBUSY;
+
+	return set_capture_size(go, fmt, 0);
+}
+
+static int vidioc_reqbufs(struct file *file, void *priv,
+			  struct v4l2_requestbuffers *req)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+	int retval = -EBUSY;
+	unsigned int count, i;
+
+	if (go->streaming)
+		return retval;
+
+	if (req->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
+			req->memory != V4L2_MEMORY_MMAP)
+		return -EINVAL;
+
+	down(&gofh->lock);
+	for (i = 0; i < gofh->buf_count; ++i)
+		if (gofh->bufs[i].mapped > 0)
 			goto unlock_and_return;
-		}
-		gobuf->state = BUF_STATE_QUEUED;
-		spin_lock_irqsave(&go->spinlock, flags);
-		list_add_tail(&gobuf->stream, &go->stream);
-		spin_unlock_irqrestore(&go->spinlock, flags);
-		up(&gofh->lock);
-		return 0;
+
+	down(&go->hw_lock);
+	if (go->in_use > 0 && gofh->buf_count == 0) {
+		up(&go->hw_lock);
+		goto unlock_and_return;
 	}
-	case VIDIOC_DQBUF:
-	{
-		struct v4l2_buffer *buf = arg;
-		struct go7007_buffer *gobuf;
-		u32 frame_type_flag;
-		DEFINE_WAIT(wait);
 
-		if (buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-			return -EINVAL;
-		if (buf->memory != V4L2_MEMORY_MMAP)
-			return -EINVAL;
-		down(&gofh->lock);
-		retval = -EINVAL;
-		if (list_empty(&go->stream))
+	if (gofh->buf_count > 0)
+		kfree(gofh->bufs);
+
+	retval = -ENOMEM;
+	count = req->count;
+	if (count > 0) {
+		if (count < 2)
+			count = 2;
+		if (count > 32)
+			count = 32;
+
+		gofh->bufs = kmalloc(count * sizeof(struct go7007_buffer),
+				     GFP_KERNEL);
+
+		if (!gofh->bufs) {
+			up(&go->hw_lock);
 			goto unlock_and_return;
-		gobuf = list_entry(go->stream.next,
-				struct go7007_buffer, stream);
-		retval = -EAGAIN;
-		if (gobuf->state != BUF_STATE_DONE &&
-				!(file->f_flags & O_NONBLOCK)) {
-			for (;;) {
-				prepare_to_wait(&go->frame_waitq, &wait,
-						TASK_INTERRUPTIBLE);
-				if (gobuf->state == BUF_STATE_DONE)
-					break;
-				if (signal_pending(current)) {
-					retval = -ERESTARTSYS;
-					break;
-				}
-				schedule();
-			}
-			finish_wait(&go->frame_waitq, &wait);
 		}
-		if (gobuf->state != BUF_STATE_DONE)
-			goto unlock_and_return;
-		spin_lock_irqsave(&go->spinlock, flags);
-		deactivate_buffer(gobuf);
-		spin_unlock_irqrestore(&go->spinlock, flags);
-		frame_type_flag = get_frame_type_flag(gobuf, go->format);
-		gobuf->state = BUF_STATE_IDLE;
-		memset(buf, 0, sizeof(*buf));
-		buf->index = gobuf->index;
-		buf->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-		buf->bytesused = gobuf->bytesused;
-		buf->flags = V4L2_BUF_FLAG_MAPPED | frame_type_flag;
-		buf->field = V4L2_FIELD_NONE;
-		buf->timestamp = gobuf->timestamp;
-		buf->sequence = gobuf->seq;
-		buf->memory = V4L2_MEMORY_MMAP;
-		buf->m.offset = gobuf->index * GO7007_BUF_SIZE;
-		buf->length = GO7007_BUF_SIZE;
-		buf->reserved = gobuf->modet_active;
-		up(&gofh->lock);
-		return 0;
-	}
-	case VIDIOC_STREAMON:
-	{
-		unsigned int *type = arg;
 
-		if (*type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-			return -EINVAL;
-		down(&gofh->lock);
-		down(&go->hw_lock);
-		if (!go->streaming) {
-			go->streaming = 1;
-			go->next_seq = 0;
-			go->active_buf = NULL;
-			if (go7007_start_encoder(go) < 0)
-				retval = -EIO;
-			else
-				retval = 0;
+		memset(gofh->bufs, 0, count * sizeof(struct go7007_buffer));
+
+		for (i = 0; i < count; ++i) {
+			gofh->bufs[i].go = go;
+			gofh->bufs[i].index = i;
+			gofh->bufs[i].state = BUF_STATE_IDLE;
+			gofh->bufs[i].mapped = 0;
 		}
-		up(&go->hw_lock);
-		up(&gofh->lock);
-		return retval;
-	}
-	case VIDIOC_STREAMOFF:
-	{
-		unsigned int *type = arg;
 
-		if (*type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-			return -EINVAL;
-		down(&gofh->lock);
-		go7007_streamoff(go);
-		up(&gofh->lock);
-		return 0;
+		go->in_use = 1;
+	} else {
+		go->in_use = 0;
 	}
-	case VIDIOC_QUERYCTRL:
-	{
-		struct v4l2_queryctrl *ctrl = arg;
-		u32 id;
 
-		if (!go->i2c_adapter_online)
-			return -EIO;
-		id = ctrl->id;
-		memset(ctrl, 0, sizeof(*ctrl));
-		ctrl->id = id;
-		i2c_clients_command(&go->i2c_adapter, VIDIOC_QUERYCTRL, arg);
-		return ctrl->name[0] == 0 ? -EINVAL : 0;
-	}
-	case VIDIOC_G_CTRL:
-	{
-		struct v4l2_control *ctrl = arg;
-		struct v4l2_queryctrl query;
+	gofh->buf_count = count;
+	up(&go->hw_lock);
+	up(&gofh->lock);
 
-		if (!go->i2c_adapter_online)
-			return -EIO;
-		memset(&query, 0, sizeof(query));
-		query.id = ctrl->id;
-		i2c_clients_command(&go->i2c_adapter, VIDIOC_QUERYCTRL, &query);
-		if (query.name[0] == 0)
-			return -EINVAL;
-		i2c_clients_command(&go->i2c_adapter, VIDIOC_G_CTRL, arg);
-		return 0;
-	}
-	case VIDIOC_S_CTRL:
-	{
-		struct v4l2_control *ctrl = arg;
-		struct v4l2_queryctrl query;
+	memset(req, 0, sizeof(*req));
 
-		if (!go->i2c_adapter_online)
-			return -EIO;
-		memset(&query, 0, sizeof(query));
-		query.id = ctrl->id;
-		i2c_clients_command(&go->i2c_adapter, VIDIOC_QUERYCTRL, &query);
-		if (query.name[0] == 0)
-			return -EINVAL;
-		i2c_clients_command(&go->i2c_adapter, VIDIOC_S_CTRL, arg);
-		return 0;
-	}
-	case VIDIOC_G_PARM:
-	{
-		struct v4l2_streamparm *parm = arg;
-		struct v4l2_fract timeperframe = {
-			.numerator = 1001 *  go->fps_scale,
-			.denominator = go->sensor_framerate,
-		};
+	req->count = count;
+	req->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	req->memory = V4L2_MEMORY_MMAP;
 
-		if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-			return -EINVAL;
-		memset(parm, 0, sizeof(*parm));
-		parm->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-		parm->parm.capture.capability |= V4L2_CAP_TIMEPERFRAME;
-		parm->parm.capture.timeperframe = timeperframe;
-		return 0;
+	return 0;
+
+unlock_and_return:
+	up(&gofh->lock);
+	return retval;
+}
+
+static int vidioc_querybuf(struct file *file, void *priv,
+			   struct v4l2_buffer *buf)
+{
+	struct go7007_file *gofh = priv;
+	int retval = -EINVAL;
+	unsigned int index;
+
+	if (buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return retval;
+
+	index = buf->index;
+
+	down(&gofh->lock);
+	if (index >= gofh->buf_count)
+		goto unlock_and_return;
+
+	memset(buf, 0, sizeof(*buf));
+	buf->index = index;
+	buf->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	switch (gofh->bufs[index].state) {
+	case BUF_STATE_QUEUED:
+		buf->flags = V4L2_BUF_FLAG_QUEUED;
+		break;
+	case BUF_STATE_DONE:
+		buf->flags = V4L2_BUF_FLAG_DONE;
+		break;
+	default:
+		buf->flags = 0;
 	}
-	case VIDIOC_S_PARM:
-	{
-		struct v4l2_streamparm *parm = arg;
-		unsigned int n, d;
 
-		if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-			return -EINVAL;
-		if (parm->parm.capture.capturemode != 0)
-			return -EINVAL;
-		n = go->sensor_framerate *
-			parm->parm.capture.timeperframe.numerator;
-		d = 1001 * parm->parm.capture.timeperframe.denominator;
-		if (n != 0 && d != 0 && n > d)
-			go->fps_scale = (n + d/2) / d;
-		else
-			go->fps_scale = 1;
-		return 0;
+	if (gofh->bufs[index].mapped)
+		buf->flags |= V4L2_BUF_FLAG_MAPPED;
+	buf->memory = V4L2_MEMORY_MMAP;
+	buf->m.offset = index * GO7007_BUF_SIZE;
+	buf->length = GO7007_BUF_SIZE;
+	up(&gofh->lock);
+
+	return 0;
+
+unlock_and_return:
+	up(&gofh->lock);
+	return retval;
+}
+
+static int vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+	struct go7007_buffer *gobuf;
+	unsigned long flags;
+	int retval = -EINVAL;
+	int ret;
+
+	if (buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
+			buf->memory != V4L2_MEMORY_MMAP)
+		return retval;
+
+	down(&gofh->lock);
+	if (buf->index < 0 || buf->index >= gofh->buf_count)
+		goto unlock_and_return;
+
+	gobuf = &gofh->bufs[buf->index];
+	if (!gobuf->mapped)
+		goto unlock_and_return;
+
+	retval = -EBUSY;
+	if (gobuf->state != BUF_STATE_IDLE)
+		goto unlock_and_return;
+
+	/* offset will be 0 until we really support USERPTR streaming */
+	gobuf->offset = gobuf->user_addr & ~PAGE_MASK;
+	gobuf->bytesused = 0;
+	gobuf->frame_offset = 0;
+	gobuf->modet_active = 0;
+	if (gobuf->offset > 0)
+		gobuf->page_count = GO7007_BUF_PAGES + 1;
+	else
+		gobuf->page_count = GO7007_BUF_PAGES;
+
+	retval = -ENOMEM;
+	down_read(&current->mm->mmap_sem);
+	ret = get_user_pages(current, current->mm,
+			gobuf->user_addr & PAGE_MASK, gobuf->page_count,
+			1, 1, gobuf->pages, NULL);
+	up_read(&current->mm->mmap_sem);
+
+	if (ret != gobuf->page_count) {
+		int i;
+		for (i = 0; i < ret; ++i)
+			page_cache_release(gobuf->pages[i]);
+		gobuf->page_count = 0;
+		goto unlock_and_return;
 	}
-	case VIDIOC_ENUMSTD:
-	{
-		struct v4l2_standard *std = arg;
 
-		if ((go->board_info->flags & GO7007_BOARD_HAS_TUNER) &&
-				go->input == go->board_info->num_inputs - 1) {
-			if (!go->i2c_adapter_online)
-				return -EIO;
-			i2c_clients_command(&go->i2c_adapter,
-						VIDIOC_ENUMSTD, arg);
-			if (!std->id) /* hack to indicate EINVAL from tuner */
-				return -EINVAL;
-		} else if (go->board_info->sensor_flags & GO7007_SENSOR_TV) {
-			switch (std->index) {
-			case 0:
-				v4l2_video_std_construct(std,
-						V4L2_STD_NTSC, "NTSC");
+	gobuf->state = BUF_STATE_QUEUED;
+	spin_lock_irqsave(&go->spinlock, flags);
+	list_add_tail(&gobuf->stream, &go->stream);
+	spin_unlock_irqrestore(&go->spinlock, flags);
+	up(&gofh->lock);
+
+	return 0;
+
+unlock_and_return:
+	up(&gofh->lock);
+	return retval;
+}
+
+
+static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+	struct go7007_buffer *gobuf;
+	int retval = -EINVAL;
+	unsigned long flags;
+	u32 frame_type_flag;
+	DEFINE_WAIT(wait);
+
+	if (buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return retval;
+	if (buf->memory != V4L2_MEMORY_MMAP)
+		return retval;
+
+	down(&gofh->lock);
+	if (list_empty(&go->stream))
+		goto unlock_and_return;
+	gobuf = list_entry(go->stream.next,
+			struct go7007_buffer, stream);
+
+	retval = -EAGAIN;
+	if (gobuf->state != BUF_STATE_DONE &&
+			!(file->f_flags & O_NONBLOCK)) {
+		for (;;) {
+			prepare_to_wait(&go->frame_waitq, &wait,
+					TASK_INTERRUPTIBLE);
+			if (gobuf->state == BUF_STATE_DONE)
 				break;
-			case 1:
-				v4l2_video_std_construct(std,
-						V4L2_STD_PAL | V4L2_STD_SECAM,
-						"PAL/SECAM");
+			if (signal_pending(current)) {
+				retval = -ERESTARTSYS;
 				break;
-			default:
-				return -EINVAL;
 			}
-		} else {
-			if (std->index != 0)
-				return -EINVAL;
-			memset(std, 0, sizeof(*std));
-			snprintf(std->name, sizeof(std->name), "%dx%d, %dfps",
-				go->board_info->sensor_width,
-				go->board_info->sensor_height,
-				go->board_info->sensor_framerate / 1000);
-			std->frameperiod.numerator = 1001;
-			std->frameperiod.denominator =
-					go->board_info->sensor_framerate;
+			schedule();
 		}
-		return 0;
+		finish_wait(&go->frame_waitq, &wait);
 	}
-	case VIDIOC_G_STD:
-	{
-		v4l2_std_id *std = arg;
+	if (gobuf->state != BUF_STATE_DONE)
+		goto unlock_and_return;
+
+	spin_lock_irqsave(&go->spinlock, flags);
+	deactivate_buffer(gobuf);
+	spin_unlock_irqrestore(&go->spinlock, flags);
+	frame_type_flag = get_frame_type_flag(gobuf, go->format);
+	gobuf->state = BUF_STATE_IDLE;
+
+	memset(buf, 0, sizeof(*buf));
+	buf->index = gobuf->index;
+	buf->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	buf->bytesused = gobuf->bytesused;
+	buf->flags = V4L2_BUF_FLAG_MAPPED | frame_type_flag;
+	buf->field = V4L2_FIELD_NONE;
+	buf->timestamp = gobuf->timestamp;
+	buf->sequence = gobuf->seq;
+	buf->memory = V4L2_MEMORY_MMAP;
+	buf->m.offset = gobuf->index * GO7007_BUF_SIZE;
+	buf->length = GO7007_BUF_SIZE;
+	buf->reserved = gobuf->modet_active;
 
-		if ((go->board_info->flags & GO7007_BOARD_HAS_TUNER) &&
-				go->input == go->board_info->num_inputs - 1) {
-			if (!go->i2c_adapter_online)
-				return -EIO;
-			i2c_clients_command(&go->i2c_adapter,
-						VIDIOC_G_STD, arg);
-		} else if (go->board_info->sensor_flags & GO7007_SENSOR_TV) {
-			if (go->standard == GO7007_STD_NTSC)
-				*std = V4L2_STD_NTSC;
-			else
-				*std = V4L2_STD_PAL | V4L2_STD_SECAM;
-		} else
-			*std = 0;
-		return 0;
+	up(&gofh->lock);
+	return 0;
+
+unlock_and_return:
+	up(&gofh->lock);
+	return retval;
+}
+
+static int vidioc_streamon(struct file *file, void *priv,
+					enum v4l2_buf_type type)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+	int retval = 0;
+
+	if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	down(&gofh->lock);
+	down(&go->hw_lock);
+
+	if (!go->streaming) {
+		go->streaming = 1;
+		go->next_seq = 0;
+		go->active_buf = NULL;
+		if (go7007_start_encoder(go) < 0)
+			retval = -EIO;
+		else
+			retval = 0;
 	}
-	case VIDIOC_S_STD:
-	{
-		v4l2_std_id *std = arg;
+	up(&go->hw_lock);
+	up(&gofh->lock);
 
-		if (go->streaming)
-			return -EBUSY;
-		if (!(go->board_info->sensor_flags & GO7007_SENSOR_TV) &&
-				*std != 0)
-			return -EINVAL;
-		if (*std == 0)
-			return -EINVAL;
-		if ((go->board_info->flags & GO7007_BOARD_HAS_TUNER) &&
-				go->input == go->board_info->num_inputs - 1) {
-			if (!go->i2c_adapter_online)
-				return -EIO;
-			i2c_clients_command(&go->i2c_adapter,
-						VIDIOC_S_STD, arg);
-			if (!*std) /* hack to indicate EINVAL from tuner */
-				return -EINVAL;
-		}
-		if (*std & V4L2_STD_NTSC) {
-			go->standard = GO7007_STD_NTSC;
-			go->sensor_framerate = 30000;
-		} else if (*std & V4L2_STD_PAL) {
-			go->standard = GO7007_STD_PAL;
-			go->sensor_framerate = 25025;
-		} else if (*std & V4L2_STD_SECAM) {
-			go->standard = GO7007_STD_PAL;
-			go->sensor_framerate = 25025;
-		} else
+	return retval;
+}
+
+static int vidioc_streamoff(struct file *file, void *priv,
+					enum v4l2_buf_type type)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+
+	if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+	down(&gofh->lock);
+	go7007_streamoff(go);
+	up(&gofh->lock);
+
+	return 0;
+}
+
+static int vidioc_queryctrl(struct file *file, void *priv,
+			   struct v4l2_queryctrl *query)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+
+	if (!go->i2c_adapter_online)
+		return -EIO;
+
+	i2c_clients_command(&go->i2c_adapter, VIDIOC_QUERYCTRL, query);
+
+	return (!query->name[0]) ? -EINVAL : 0;
+}
+
+static int vidioc_g_ctrl(struct file *file, void *priv,
+				struct v4l2_control *ctrl)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+	struct v4l2_queryctrl query;
+
+	if (!go->i2c_adapter_online)
+		return -EIO;
+
+	memset(&query, 0, sizeof(query));
+	query.id = ctrl->id;
+	i2c_clients_command(&go->i2c_adapter, VIDIOC_QUERYCTRL, &query);
+	if (query.name[0] == 0)
+		return -EINVAL;
+	i2c_clients_command(&go->i2c_adapter, VIDIOC_G_CTRL, ctrl);
+
+	return 0;
+}
+
+static int vidioc_s_ctrl(struct file *file, void *priv,
+				struct v4l2_control *ctrl)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+	struct v4l2_queryctrl query;
+
+	if (!go->i2c_adapter_online)
+		return -EIO;
+
+	memset(&query, 0, sizeof(query));
+	query.id = ctrl->id;
+	i2c_clients_command(&go->i2c_adapter, VIDIOC_QUERYCTRL, &query);
+	if (query.name[0] == 0)
+		return -EINVAL;
+	i2c_clients_command(&go->i2c_adapter, VIDIOC_S_CTRL, ctrl);
+
+	return 0;
+}
+
+static int vidioc_g_parm(struct file *filp, void *priv,
+		struct v4l2_streamparm *parm)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+	struct v4l2_fract timeperframe = {
+		.numerator = 1001 *  go->fps_scale,
+		.denominator = go->sensor_framerate,
+	};
+
+	if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	parm->parm.capture.capability |= V4L2_CAP_TIMEPERFRAME;
+	parm->parm.capture.timeperframe = timeperframe;
+
+	return 0;
+}
+
+static int vidioc_s_parm(struct file *filp, void *priv,
+		struct v4l2_streamparm *parm)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+	unsigned int n, d;
+
+	if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+	if (parm->parm.capture.capturemode != 0)
+		return -EINVAL;
+
+	n = go->sensor_framerate *
+		parm->parm.capture.timeperframe.numerator;
+	d = 1001 * parm->parm.capture.timeperframe.denominator;
+	if (n != 0 && d != 0 && n > d)
+		go->fps_scale = (n + d/2) / d;
+	else
+		go->fps_scale = 1;
+
+	return 0;
+}
+
+/* VIDIOC_ENUMSTD on go7007 were used for enumberating the supported fps and
+   its resolution, when the device is not connected to TV.
+   This were an API abuse, probably used by the lack of specific IOCTL's to
+   enumberate it, by the time the driver were written.
+
+   However, since kernel 2.6.19, two new ioctls (VIDIOC_ENUM_FRAMEINTERVALS
+   and VIDIOC_ENUM_FRAMESIZES) were added for this purpose.
+
+   The two functions bellow implements the newer ioctls
+*/
+static int vidioc_enum_framesizes(struct file *filp, void *priv,
+				  struct v4l2_frmsizeenum *fsize)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+
+	/* Return -EINVAL, if it is a TV board */
+	if ((go->board_info->flags & GO7007_BOARD_HAS_TUNER) ||
+	    (go->board_info->sensor_flags & GO7007_SENSOR_TV))
+		return -EINVAL;
+
+	if (fsize->index > 0)
+		return -EINVAL;
+
+	fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+	fsize->discrete.width = go->board_info->sensor_width;
+	fsize->discrete.height = go->board_info->sensor_height;
+
+	return 0;
+}
+
+static int vidioc_enum_frameintervals(struct file *filp, void *priv,
+				      struct v4l2_frmivalenum *fival)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+
+	/* Return -EINVAL, if it is a TV board */
+	if ((go->board_info->flags & GO7007_BOARD_HAS_TUNER) ||
+	    (go->board_info->sensor_flags & GO7007_SENSOR_TV))
+		return -EINVAL;
+
+	if (fival->index > 0)
+		return -EINVAL;
+
+	fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
+	fival->discrete.numerator = 1001;
+	fival->discrete.denominator = go->board_info->sensor_framerate;
+
+	return 0;
+}
+
+static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id *std)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+
+	if (go->streaming)
+		return -EBUSY;
+
+	if (!(go->board_info->sensor_flags & GO7007_SENSOR_TV) &&
+			*std != 0)
+		return -EINVAL;
+
+	if (*std == 0)
+		return -EINVAL;
+
+	if ((go->board_info->flags & GO7007_BOARD_HAS_TUNER) &&
+			go->input == go->board_info->num_inputs - 1) {
+		if (!go->i2c_adapter_online)
+			return -EIO;
+		i2c_clients_command(&go->i2c_adapter,
+					VIDIOC_S_STD, std);
+		if (!*std) /* hack to indicate EINVAL from tuner */
 			return -EINVAL;
-		if (go->i2c_adapter_online)
-			i2c_clients_command(&go->i2c_adapter,
-					    VIDIOC_S_STD, std);
-		set_capture_size(go, NULL, 0);
-		return 0;
 	}
+
+	if (*std & V4L2_STD_NTSC) {
+		go->standard = GO7007_STD_NTSC;
+		go->sensor_framerate = 30000;
+	} else if (*std & V4L2_STD_PAL) {
+		go->standard = GO7007_STD_PAL;
+		go->sensor_framerate = 25025;
+	} else if (*std & V4L2_STD_SECAM) {
+		go->standard = GO7007_STD_PAL;
+		go->sensor_framerate = 25025;
+	} else
+		return -EINVAL;
+
+	if (go->i2c_adapter_online)
+		i2c_clients_command(&go->i2c_adapter,
+					VIDIOC_S_STD, std);
+	set_capture_size(go, NULL, 0);
+
+	return 0;
+}
+
+#if 0
 	case VIDIOC_QUERYSTD:
 	{
 		v4l2_std_id *std = arg;
@@ -884,219 +1195,269 @@ static int go7007_do_ioctl(struct inode *inode, struct file *file,
 			*std = 0;
 		return 0;
 	}
-	case VIDIOC_ENUMINPUT:
-	{
-		struct v4l2_input *inp = arg;
-		int index;
+#endif
 
-		if (inp->index >= go->board_info->num_inputs)
-			return -EINVAL;
-		index = inp->index;
-		memset(inp, 0, sizeof(*inp));
-		inp->index = index;
-		strncpy(inp->name, go->board_info->inputs[index].name,
-				sizeof(inp->name));
-		/* If this board has a tuner, it will be the last input */
-		if ((go->board_info->flags & GO7007_BOARD_HAS_TUNER) &&
-				index == go->board_info->num_inputs - 1)
-			inp->type = V4L2_INPUT_TYPE_TUNER;
-		else
-			inp->type = V4L2_INPUT_TYPE_CAMERA;
-		inp->audioset = 0;
-		inp->tuner = 0;
-		if (go->board_info->sensor_flags & GO7007_SENSOR_TV)
-			inp->std = V4L2_STD_NTSC | V4L2_STD_PAL |
-							V4L2_STD_SECAM;
-		else
-			inp->std = 0;
-		return 0;
-	}
-	case VIDIOC_G_INPUT:
-	{
-		int *input = arg;
+static int vidioc_enum_input(struct file *file, void *priv,
+				struct v4l2_input *inp)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
 
-		*input = go->input;
-		return 0;
-	}
-	case VIDIOC_S_INPUT:
-	{
-		int *input = arg;
+	if (inp->index >= go->board_info->num_inputs)
+		return -EINVAL;
 
-		if (*input >= go->board_info->num_inputs)
-			return -EINVAL;
-		if (go->streaming)
-			return -EBUSY;
-		go->input = *input;
-		if (go->i2c_adapter_online) {
-			i2c_clients_command(&go->i2c_adapter, VIDIOC_S_INPUT,
-				&go->board_info->inputs[*input].video_input);
-			i2c_clients_command(&go->i2c_adapter, VIDIOC_S_AUDIO,
-				&go->board_info->inputs[*input].audio_input);
-		}
-		return 0;
-	}
-	case VIDIOC_G_TUNER:
-	{
-		struct v4l2_tuner *t = arg;
+	strncpy(inp->name, go->board_info->inputs[inp->index].name,
+			sizeof(inp->name));
 
-		if (!(go->board_info->flags & GO7007_BOARD_HAS_TUNER))
-			return -EINVAL;
-		if (t->index != 0)
-			return -EINVAL;
-		if (!go->i2c_adapter_online)
-			return -EIO;
-		i2c_clients_command(&go->i2c_adapter, VIDIOC_G_TUNER, arg);
-		t->index = 0;
-		return 0;
-	}
-	case VIDIOC_S_TUNER:
-	{
-		struct v4l2_tuner *t = arg;
+	/* If this board has a tuner, it will be the last input */
+	if ((go->board_info->flags & GO7007_BOARD_HAS_TUNER) &&
+			inp->index == go->board_info->num_inputs - 1)
+		inp->type = V4L2_INPUT_TYPE_TUNER;
+	else
+		inp->type = V4L2_INPUT_TYPE_CAMERA;
 
-		if (!(go->board_info->flags & GO7007_BOARD_HAS_TUNER))
-			return -EINVAL;
-		if (t->index != 0)
-			return -EINVAL;
-		if (!go->i2c_adapter_online)
-			return -EIO;
-		switch (go->board_id) {
-		case GO7007_BOARDID_PX_TV402U_NA:
-		case GO7007_BOARDID_PX_TV402U_JP:
-			/* No selectable options currently */
-			if (t->audmode != V4L2_TUNER_MODE_STEREO)
-				return -EINVAL;
-			break;
-		}
-		i2c_clients_command(&go->i2c_adapter, VIDIOC_S_TUNER, arg);
-		return 0;
-	}
-	case VIDIOC_G_FREQUENCY:
-	{
-		struct v4l2_frequency *f = arg;
+	inp->audioset = 0;
+	inp->tuner = 0;
+	if (go->board_info->sensor_flags & GO7007_SENSOR_TV)
+		inp->std = V4L2_STD_NTSC | V4L2_STD_PAL |
+						V4L2_STD_SECAM;
+	else
+		inp->std = 0;
 
-		if (!(go->board_info->flags & GO7007_BOARD_HAS_TUNER))
-			return -EINVAL;
-		if (!go->i2c_adapter_online)
-			return -EIO;
-		memset(f, 0, sizeof(*f));
-		f->type = V4L2_TUNER_ANALOG_TV;
-		i2c_clients_command(&go->i2c_adapter, VIDIOC_G_FREQUENCY, arg);
-		return 0;
-	}
-	case VIDIOC_S_FREQUENCY:
-	{
-		if (!(go->board_info->flags & GO7007_BOARD_HAS_TUNER))
-			return -EINVAL;
-		if (!go->i2c_adapter_online)
-			return -EIO;
-		i2c_clients_command(&go->i2c_adapter, VIDIOC_S_FREQUENCY, arg);
-		return 0;
-	}
-	case VIDIOC_CROPCAP:
-	{
-		struct v4l2_cropcap *cropcap = arg;
+	return 0;
+}
 
-		if (cropcap->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-			return -EINVAL;
-		memset(cropcap, 0, sizeof(*cropcap));
-		cropcap->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-		/* These specify the raw input of the sensor */
-		switch (go->standard) {
-		case GO7007_STD_NTSC:
-			cropcap->bounds.top = 0;
-			cropcap->bounds.left = 0;
-			cropcap->bounds.width = 720;
-			cropcap->bounds.height = 480;
-			cropcap->defrect.top = 0;
-			cropcap->defrect.left = 0;
-			cropcap->defrect.width = 720;
-			cropcap->defrect.height = 480;
-			break;
-		case GO7007_STD_PAL:
-			cropcap->bounds.top = 0;
-			cropcap->bounds.left = 0;
-			cropcap->bounds.width = 720;
-			cropcap->bounds.height = 576;
-			cropcap->defrect.top = 0;
-			cropcap->defrect.left = 0;
-			cropcap->defrect.width = 720;
-			cropcap->defrect.height = 576;
-			break;
-		case GO7007_STD_OTHER:
-			cropcap->bounds.top = 0;
-			cropcap->bounds.left = 0;
-			cropcap->bounds.width = go->board_info->sensor_width;
-			cropcap->bounds.height = go->board_info->sensor_height;
-			cropcap->defrect.top = 0;
-			cropcap->defrect.left = 0;
-			cropcap->defrect.width = go->board_info->sensor_width;
-			cropcap->defrect.height = go->board_info->sensor_height;
-			break;
-		}
 
-		return 0;
-	}
-	case VIDIOC_G_CROP:
-	{
-		struct v4l2_crop *crop = arg;
+static int vidioc_g_input(struct file *file, void *priv, unsigned int *input)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
 
-		if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-			return -EINVAL;
-		memset(crop, 0, sizeof(*crop));
-		crop->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-		/* These specify the raw input of the sensor */
-		switch (go->standard) {
-		case GO7007_STD_NTSC:
-			crop->c.top = 0;
-			crop->c.left = 0;
-			crop->c.width = 720;
-			crop->c.height = 480;
-			break;
-		case GO7007_STD_PAL:
-			crop->c.top = 0;
-			crop->c.left = 0;
-			crop->c.width = 720;
-			crop->c.height = 576;
-			break;
-		case GO7007_STD_OTHER:
-			crop->c.top = 0;
-			crop->c.left = 0;
-			crop->c.width = go->board_info->sensor_width;
-			crop->c.height = go->board_info->sensor_height;
-			break;
-		}
+	*input = go->input;
 
-		return 0;
+	return 0;
+}
+
+static int vidioc_s_input(struct file *file, void *priv, unsigned int input)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+
+	if (input >= go->board_info->num_inputs)
+		return -EINVAL;
+	if (go->streaming)
+		return -EBUSY;
+
+	go->input = input;
+	if (go->i2c_adapter_online) {
+		i2c_clients_command(&go->i2c_adapter, VIDIOC_S_INPUT,
+			&go->board_info->inputs[input].video_input);
+		i2c_clients_command(&go->i2c_adapter, VIDIOC_S_AUDIO,
+			&go->board_info->inputs[input].audio_input);
 	}
-	case VIDIOC_S_CROP:
-	{
-		struct v4l2_crop *crop = arg;
 
-		if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+	return 0;
+}
+
+static int vidioc_g_tuner(struct file *file, void *priv,
+				struct v4l2_tuner *t)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+
+	if (!(go->board_info->flags & GO7007_BOARD_HAS_TUNER))
+		return -EINVAL;
+	if (t->index != 0)
+		return -EINVAL;
+	if (!go->i2c_adapter_online)
+		return -EIO;
+
+	i2c_clients_command(&go->i2c_adapter, VIDIOC_G_TUNER, t);
+
+	t->index = 0;
+	return 0;
+}
+
+static int vidioc_s_tuner(struct file *file, void *priv,
+				struct v4l2_tuner *t)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+
+	if (!(go->board_info->flags & GO7007_BOARD_HAS_TUNER))
+		return -EINVAL;
+	if (t->index != 0)
+		return -EINVAL;
+	if (!go->i2c_adapter_online)
+		return -EIO;
+
+	switch (go->board_id) {
+	case GO7007_BOARDID_PX_TV402U_NA:
+	case GO7007_BOARDID_PX_TV402U_JP:
+		/* No selectable options currently */
+		if (t->audmode != V4L2_TUNER_MODE_STEREO)
 			return -EINVAL;
-		return 0;
+		break;
 	}
-	case VIDIOC_G_JPEGCOMP:
-	{
-		struct v4l2_jpegcompression *params = arg;
 
-		memset(params, 0, sizeof(*params));
-		params->quality = 50; /* ?? */
-		params->jpeg_markers = V4L2_JPEG_MARKER_DHT |
-					V4L2_JPEG_MARKER_DQT;
+	i2c_clients_command(&go->i2c_adapter, VIDIOC_S_TUNER, t);
 
-		return 0;
+	return 0;
+}
+
+static int vidioc_g_frequency(struct file *file, void *priv,
+				struct v4l2_frequency *f)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+
+	if (!(go->board_info->flags & GO7007_BOARD_HAS_TUNER))
+		return -EINVAL;
+	if (!go->i2c_adapter_online)
+		return -EIO;
+
+	f->type = V4L2_TUNER_ANALOG_TV;
+	i2c_clients_command(&go->i2c_adapter, VIDIOC_G_FREQUENCY, f);
+	return 0;
+}
+
+static int vidioc_s_frequency(struct file *file, void *priv,
+				struct v4l2_frequency *f)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+
+	if (!(go->board_info->flags & GO7007_BOARD_HAS_TUNER))
+		return -EINVAL;
+	if (!go->i2c_adapter_online)
+		return -EIO;
+
+	i2c_clients_command(&go->i2c_adapter, VIDIOC_S_FREQUENCY, f);
+
+	return 0;
+}
+
+static int vidioc_cropcap(struct file *file, void *priv,
+					struct v4l2_cropcap *cropcap)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+
+	if (cropcap->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	/* These specify the raw input of the sensor */
+	switch (go->standard) {
+	case GO7007_STD_NTSC:
+		cropcap->bounds.top = 0;
+		cropcap->bounds.left = 0;
+		cropcap->bounds.width = 720;
+		cropcap->bounds.height = 480;
+		cropcap->defrect.top = 0;
+		cropcap->defrect.left = 0;
+		cropcap->defrect.width = 720;
+		cropcap->defrect.height = 480;
+		break;
+	case GO7007_STD_PAL:
+		cropcap->bounds.top = 0;
+		cropcap->bounds.left = 0;
+		cropcap->bounds.width = 720;
+		cropcap->bounds.height = 576;
+		cropcap->defrect.top = 0;
+		cropcap->defrect.left = 0;
+		cropcap->defrect.width = 720;
+		cropcap->defrect.height = 576;
+		break;
+	case GO7007_STD_OTHER:
+		cropcap->bounds.top = 0;
+		cropcap->bounds.left = 0;
+		cropcap->bounds.width = go->board_info->sensor_width;
+		cropcap->bounds.height = go->board_info->sensor_height;
+		cropcap->defrect.top = 0;
+		cropcap->defrect.left = 0;
+		cropcap->defrect.width = go->board_info->sensor_width;
+		cropcap->defrect.height = go->board_info->sensor_height;
+		break;
 	}
-	case VIDIOC_S_JPEGCOMP:
-	{
-		struct v4l2_jpegcompression *params = arg;
 
-		if (params->quality != 50 ||
-				params->jpeg_markers != (V4L2_JPEG_MARKER_DHT |
-							V4L2_JPEG_MARKER_DQT))
-			return -EINVAL;
-		return 0;
+	return 0;
+}
+
+static int vidioc_g_crop(struct file *file, void *priv, struct v4l2_crop *crop)
+{
+	struct go7007_file *gofh = priv;
+	struct go7007 *go = gofh->go;
+
+	if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	crop->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	/* These specify the raw input of the sensor */
+	switch (go->standard) {
+	case GO7007_STD_NTSC:
+		crop->c.top = 0;
+		crop->c.left = 0;
+		crop->c.width = 720;
+		crop->c.height = 480;
+		break;
+	case GO7007_STD_PAL:
+		crop->c.top = 0;
+		crop->c.left = 0;
+		crop->c.width = 720;
+		crop->c.height = 576;
+		break;
+	case GO7007_STD_OTHER:
+		crop->c.top = 0;
+		crop->c.left = 0;
+		crop->c.width = go->board_info->sensor_width;
+		crop->c.height = go->board_info->sensor_height;
+		break;
 	}
+
+	return 0;
+}
+
+/* FIXME: vidioc_s_crop is not really implemented!!!
+ */
+static int vidioc_s_crop(struct file *file, void *priv, struct v4l2_crop *crop)
+{
+	if (crop->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int vidioc_g_jpegcomp(struct file *file, void *priv,
+			 struct v4l2_jpegcompression *params)
+{
+	memset(params, 0, sizeof(*params));
+	params->quality = 50; /* ?? */
+	params->jpeg_markers = V4L2_JPEG_MARKER_DHT |
+				V4L2_JPEG_MARKER_DQT;
+
+	return 0;
+}
+
+static int vidioc_s_jpegcomp(struct file *file, void *priv,
+			 struct v4l2_jpegcompression *params)
+{
+	if (params->quality != 50 ||
+			params->jpeg_markers != (V4L2_JPEG_MARKER_DHT |
+						V4L2_JPEG_MARKER_DQT))
+		return -EINVAL;
+
+	return 0;
+}
+
+/* FIXME:
+	Those ioctls are private, and not needed, since several standard
+	extended controls already provide streaming control.
+	So, those ioctls should be converted into vidioc_g_ext_ctrls()
+	and vidioc_s_ext_ctrls()
+ */
+
+#if 0
 	/* Temporary ioctls for controlling compression characteristics */
 	case GO7007IOC_S_BITRATE:
 	{
@@ -1316,27 +1677,7 @@ static int go7007_do_ioctl(struct inode *inode, struct file *file,
 			return -EINVAL;
 		return clip_to_modet_map(go, region->region, region->clips);
 	}
-	default:
-		printk(KERN_DEBUG "go7007: unsupported ioctl %d\n", cmd);
-		return -ENOIOCTLCMD;
-	}
-	return 0;
-
-unlock_and_return:
-	up(&gofh->lock);
-	return retval;
-}
-
-static int go7007_ioctl(struct inode *inode, struct file *file,
-		unsigned int cmd, unsigned long arg)
-{
-	struct go7007_file *gofh = file->private_data;
-
-	if (gofh->go->status != STATUS_ONLINE)
-		return -EIO;
-
-	return video_usercopy(inode, file, cmd, arg, go7007_do_ioctl);
-}
+#endif
 
 static ssize_t go7007_read(struct file *file, char __user *data,
 		size_t count, loff_t *ppos)
@@ -1441,23 +1782,59 @@ static void go7007_vfl_release(struct video_device *vfd)
 		kfree(go);
 }
 
-static struct file_operations go7007_fops = {
+static struct v4l2_file_operations go7007_fops = {
 	.owner		= THIS_MODULE,
 	.open		= go7007_open,
 	.release	= go7007_release,
-	.ioctl		= go7007_ioctl,
-	.llseek		= no_llseek,
+	.ioctl		= video_ioctl2,
 	.read		= go7007_read,
 	.mmap		= go7007_mmap,
 	.poll		= go7007_poll,
 };
 
+static const struct v4l2_ioctl_ops video_ioctl_ops = {
+	.vidioc_querycap          = vidioc_querycap,
+	.vidioc_enum_fmt_vid_cap  = vidioc_enum_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap     = vidioc_g_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap   = vidioc_try_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap     = vidioc_s_fmt_vid_cap,
+	.vidioc_reqbufs           = vidioc_reqbufs,
+	.vidioc_querybuf          = vidioc_querybuf,
+	.vidioc_qbuf              = vidioc_qbuf,
+	.vidioc_dqbuf             = vidioc_dqbuf,
+	.vidioc_s_std             = vidioc_s_std,
+	.vidioc_enum_input        = vidioc_enum_input,
+	.vidioc_g_input           = vidioc_g_input,
+	.vidioc_s_input           = vidioc_s_input,
+	.vidioc_queryctrl         = vidioc_queryctrl,
+	.vidioc_g_ctrl            = vidioc_g_ctrl,
+	.vidioc_s_ctrl            = vidioc_s_ctrl,
+	.vidioc_streamon          = vidioc_streamon,
+	.vidioc_streamoff         = vidioc_streamoff,
+	.vidioc_g_tuner           = vidioc_g_tuner,
+	.vidioc_s_tuner           = vidioc_s_tuner,
+	.vidioc_g_frequency       = vidioc_g_frequency,
+	.vidioc_s_frequency       = vidioc_s_frequency,
+	.vidioc_g_parm            = vidioc_g_parm,
+	.vidioc_s_parm            = vidioc_s_parm,
+	.vidioc_enum_framesizes   = vidioc_enum_framesizes,
+	.vidioc_enum_frameintervals = vidioc_enum_frameintervals,
+	.vidioc_cropcap           = vidioc_cropcap,
+	.vidioc_g_crop            = vidioc_g_crop,
+	.vidioc_s_crop            = vidioc_s_crop,
+	.vidioc_g_jpegcomp        = vidioc_g_jpegcomp,
+	.vidioc_s_jpegcomp        = vidioc_s_jpegcomp,
+};
+
 static struct video_device go7007_template = {
 	.name		= "go7007",
 	.vfl_type	= VID_TYPE_CAPTURE,
 	.fops		= &go7007_fops,
 	.minor		= -1,
 	.release	= go7007_vfl_release,
+	.ioctl_ops	= &video_ioctl_ops,
+	.tvnorms	= V4L2_STD_ALL,
+	.current_norm	= V4L2_STD_NTSC,
 };
 
 int go7007_v4l2_init(struct go7007 *go)
@@ -1477,6 +1854,8 @@ int go7007_v4l2_init(struct go7007 *go)
 	}
 	video_set_drvdata(go->video_dev, go);
 	++go->ref_count;
+	printk(KERN_INFO "%s: registered device video%d [v4l2]\n",
+	       go->video_dev->name, go->video_dev->num);
 
 	return 0;
 }
diff --git a/drivers/staging/go7007/go7007.txt b/drivers/staging/go7007/go7007.txt
new file mode 100644
index 00000000000..9f6772bc68c
--- /dev/null
+++ b/drivers/staging/go7007/go7007.txt
@@ -0,0 +1,481 @@
+This is a driver for the WIS GO7007SB multi-format video encoder.
+
+Pete Eberlein <pete@sensoray.com>
+
+The driver was orignally released under the GPL and is currently hosted at:
+http://nikosapi.org/wiki/index.php/WIS_Go7007_Linux_driver
+The go7007 firmware can be acquired from the package on the site above.
+
+I've modified the driver to support the following Video4Linux2 MPEG
+controls, with acceptable values:
+
+V4L2_CID_MPEG_STREAM_TYPE	V4L2_MPEG_STREAM_TYPE_MPEG2_DVD
+				V4L2_MPEG_STREAM_TYPE_MPEG_ELEM
+V4L2_CID_MPEG_VIDEO_ENCODING	V4L2_MPEG_VIDEO_ENCODING_MPEG_1
+				V4L2_MPEG_VIDEO_ENCODING_MPEG_2
+				V4L2_MPEG_VIDEO_ENCODING_MPEG_4
+V4L2_CID_MPEG_VIDEO_ASPECT	V4L2_MPEG_VIDEO_ASPECT_1x1
+				V4L2_MPEG_VIDEO_ASPECT_4x3
+				V4L2_MPEG_VIDEO_ASPECT_16x9
+V4L2_CID_MPEG_VIDEO_GOP_SIZE	integer
+V4L2_CID_MPEG_VIDEO_BITRATE	64000 .. 10000000
+
+These should be used instead of the non-standard GO7007 ioctls described
+below.
+
+
+The README files from the orignal package appear below:
+
+---------------------------------------------------------------------------
+                     WIS GO7007SB Public Linux Driver
+---------------------------------------------------------------------------
+
+
+*** Please see the file RELEASE-NOTES for important last-minute updates ***
+
+
+  0. OVERVIEW AND LICENSING/DISCLAIMER
+
+
+This driver kit contains Linux drivers for the WIS GO7007SB multi-format
+video encoder.  Only kernel version 2.6.x is supported.  The video stream
+is available through the Video4Linux2 API and the audio stream is available
+through the ALSA API (or the OSS emulation layer of the ALSA system).
+
+The files in kernel/ and hotplug/ are licensed under the GNU General Public
+License Version 2 from the Free Software Foundation.  A copy of the license
+is included in the file COPYING.
+
+The example applications in apps/ and C header files in include/ are
+licensed under a permissive license included in the source files which
+allows copying, modification and redistribution for any purpose without
+attribution.
+
+The firmware files included in the firmware/ directory may be freely
+redistributed only in conjunction with this document; but modification,
+tampering and reverse engineering are prohibited.
+
+MICRONAS USA, INC., MAKES NO WARRANTIES TO ANY PERSON OR ENTITY WITH
+RESPECT TO THE SOFTWARE OR ANY DERIVATIVES THEREOF OR ANY SERVICES OR
+LICENSES AND DISCLAIMS ALL IMPLIED WARRANTIES, INCLUDING WITHOUT LIMITATION
+WARRANTIES OF MERCHANTABILITY, SUPPORT, AND FITNESS FOR A PARTICULAR
+PURPOSE AND NON-INFRINGEMENT.
+
+
+  1. SYSTEM REQUIREMENTS
+
+
+This driver requires Linux kernel 2.6.  Kernel 2.4 is not supported.  Using
+kernel 2.6.10 or later is recommended, as earlier kernels are known to have
+unstable USB 2.0 support.
+
+A fully built kernel source tree must be available.  Typically this will be
+linked from "/lib/modules/<KERNEL VERSION>/build" for convenience.  If this
+link does not exist, an extra parameter will need to be passed to the
+`make` command.
+
+All vendor-built kernels should already be configured properly.  However,
+for custom-built kernels, the following options need to be enabled in the
+kernel as built-in or modules:
+
+        CONFIG_HOTPLUG           - Support for hot-pluggable devices
+        CONFIG_MODULES           - Enable loadable module support
+        CONFIG_KMOD              - Automatic kernel module loading
+        CONFIG_FW_LOADER         - Hotplug firmware loading support
+        CONFIG_I2C               - I2C support
+        CONFIG_VIDEO_DEV         - Video For Linux
+        CONFIG_SOUND             - Sound card support
+        CONFIG_SND               - Advanced Linux Sound Architecture
+        CONFIG_USB               - Support for Host-side USB
+        CONFIG_USB_DEVICEFS      - USB device filesystem
+        CONFIG_USB_EHCI_HCD      - EHCI HCD (USB 2.0) support
+
+Additionally, to use the example application, the following options need to
+be enabled in the ALSA section:
+
+        CONFIG_SND_MIXER_OSS     - OSS Mixer API
+        CONFIG_SND_PCM_OSS       - OSS PCM (digital audio) API
+
+The hotplug scripts, along with the fxload utility, must also be installed.
+These scripts can be obtained from <http://linux-hotplug.sourceforge.net/>.
+Hotplugging is used for loading firmware into the Cypruss EZ-USB chip using
+fxload and for loading firmware into the driver using the firmware agent.
+
+
+  2. COMPILING AND INSTALLING THE DRIVER
+
+
+Most users should be able to compile the driver by simply running:
+
+        $ make
+
+in the top-level directory of the driver kit.  First the kernel modules
+will be built, followed by the example applications.
+
+If the build system is unable to locate the kernel source tree for the
+currently-running kernel, or if the module should be built for a kernel
+other than the currently-running kernel, an additional parameter will need
+to be passed to make to specify the appropriate kernel source directory:
+
+        $ make KERNELSRC=/usr/src/linux-2.6.10-custom3
+
+Once the compile completes, the driver and firmware files should be
+installed by running:
+
+        $ make install
+
+The kernel modules will be placed in "/lib/modules/<KERNEL VERSION>/extra"
+and the firmware files will be placed in the appropriate hotplug firmware
+directory, usually /lib/firmware.  In addition, USB maps and scripts will
+be placed in /etc/hotplug/usb to enable fxload to initialize the EZ-USB
+control chip when the device is connected.
+
+
+  3. PAL/SECAM TUNER CONFIGURATION (TV402U-EU only)
+
+
+The PAL model of the Plextor ConvertX TV402U may require additional
+configuration to correctly select the appropriate TV frequency band and
+audio subchannel.
+
+Users with a device other than the Plextor ConvertX TV402U-EU should skip
+this section.
+
+The wide variety of PAL TV systems used in Europe requires that additional
+information about the local TV standards be passed to the driver in order
+to properly tune TV channels.  The two necessary parameters are (a) the PAL
+TV band, and (b) the audio subchannel format in use.
+
+In many cases, the appropriate TV band selection is passed to the driver
+from applications.  However, in some cases, the application only specifies
+that the driver should use PAL but not the specific information about the
+appropriate TV band.  To work around this issue, the correct TV band may be
+specified in the "force_band" parameter to the wis-sony-tuner module:
+
+     TV band           force_band
+     -------           ----------
+     PAL B/G                B
+     PAL I                  I
+     PAL D/K                D
+     SECAM L                L
+
+If the "force_band" parameter is specified, the driver will ignore any TV
+band specified by applications and will always use the band provided in the
+module parameter.
+
+The other parameter that can be specified is the audio subchannel format.
+There are several stereo audio carrier systems in use, including NICAM and
+three varieties of A2.  To receive audio broadcast on one of these stereo
+carriers, the "force_mpx_mode" parameter must be specified to the
+wis-sony-tuner module.
+
+     TV band           Audio subcarrier       force_mpx_mode
+     -------           ----------------       --------------
+     PAL B/G            Mono (default)               1
+     PAL B/G                  A2                     2
+     PAL B/G                 NICAM                   3
+     PAL I              Mono (default)               4
+     PAL I                   NICAM                   5
+     PAL D/K            Mono (default)               6
+     PAL D/K                 A2 (1)                  7
+     PAL D/K                 A2 (2)                  8
+     PAL D/K                 A2 (3)                  9
+     PAL D/K                 NICAM                  10
+     SECAM L            Mono (default)              11
+     SECAM L                 NICAM                  12
+
+If the "force_mpx_mode" parameter is not specified, the correct mono-only
+mode will be chosen based on the TV band.  However, the tuner will not
+receive stereo audio or bilingual broadcasts correctly.
+
+To pass the "force_band" or "force_mpx_mode" parameters to the
+wis-sony-tuner module, the following line must be added to the modprobe
+configuration file, which varies from one Linux distribution to another.
+
+     options wis-sony-tuner force_band=B force_mpx_mode=2
+
+The above example would force the tuner to the PAL B/G TV band and receive
+stereo audio broadcasts on the A2 carrier.
+
+To verify that the configuration has been placed in the correct location,
+execute:
+
+        $ modprobe -c | grep wis-sony-tuner
+
+If the configuration line appears, then modprobe will pass the parameters
+correctly the next time the wis-sony-tuner module is loaded into the
+kernel.
+
+
+  4. TESTING THE DRIVER
+
+
+Because few Linux applications are able to correctly capture from
+Video4Linux2 devices with only compressed formats supported, the new driver
+should be tested with the "gorecord" application in the apps/ directory.
+
+First connect a video source to the device, such as a DVD player or VCR.
+This will be captured to a file for testing the driver.  If an input source
+is unavailable, a test file can still be captured, but the video will be
+black and the audio will be silent.
+
+This application will auto-detect the V4L2 and ALSA/OSS device names of the
+hardware and will record video and audio to an AVI file for a specified
+number of seconds.  For example:
+
+        $ apps/gorecord -duration 60 capture.avi
+
+If this application does not successfully record an AVI file, the error
+messages produced by gorecord and recorded in the system log (usually in
+/var/log/messages) should provide information to help resolve the problem.
+
+Supplying no parameters to gorecord will cause it to probe the available
+devices and exit.  Use the -help flag for usage information.
+
+
+  5. USING THE DRIVER
+
+
+The V4L2 device implemented by the driver provides a standard compressed
+format API, within the following criteria:
+
+  * Applications that only support the original Video4Linux1 API will not
+    be able to communicate with this driver at all.
+
+  * No raw video modes are supported, so applications like xawtv that
+    expect only uncompressed video will not function.
+
+  * Supported compression formats are: Motion-JPEG, MPEG1, MPEG2 and MPEG4.
+
+  * MPEG video formats are delivered as Video Elementary Streams only.
+    Program Stream (PS), Transport Stream (TS) and Packetized Elementary
+    Stream (PES) formats are not supported.
+
+  * Video parameters such as format and input port may not be changed while
+    the encoder is active.
+
+  * The audio capture device only functions when the video encoder is
+    actively capturing video.  Attempts to read from the audio device when
+    the encoder is inactive will result in an I/O error.
+
+  * The native format of the audio device is 48Khz 2-channel 16-bit
+    little-endian PCM, delivered through the ALSA system.  No audio
+    compression is implemented in the hardware.  ALSA may convert to other
+    uncompressed formats on the fly.
+
+The include/ directory contains a C header file describing non-standard
+features of the GO7007SB encoder, which are described below:
+
+
+  GO7007IOC_S_COMP_PARAMS, GO7007IOC_G_COMP_PARAMS
+
+    These ioctls are used to negotiate general compression parameters.
+
+    To query the current parameters, call the GO7007IOC_G_COMP_PARAMS ioctl
+    with a pointer to a struct go7007_comp_params.  If the driver is not
+    set to MPEG format, the EINVAL error code will be returned.
+
+    To change the current parameters, initialize all fields of a struct
+    go7007_comp_params and call the GO7007_IOC_S_COMP_PARAMS ioctl with a
+    pointer to this structure.  The driver will return the current
+    parameters with any necessary changes to conform to the limitations of
+    the hardware or current compression mode.  Any or all fields can be set
+    to zero to request a reasonable default value.  If the driver is not
+    set to MPEG format, the EINVAL error code will be returned.  When I/O
+    is in progress, the EBUSY error code will be returned.
+
+    Fields in struct go7007_comp_params:
+
+        __u32                        The maximum number of frames in each
+          gop_size                   Group Of Pictures; i.e. the maximum
+                                     number of frames minus one between
+                                     each key frame.
+
+        __u32                        The maximum number of sequential
+          max_b_frames               bidirectionally-predicted frames.
+                                     (B-frames are not yet supported.)
+
+        enum go7007_aspect_ratio     The aspect ratio to be encoded in the
+          aspect_ratio               meta-data of the compressed format.
+
+                                     Choices are:
+                                        GO7007_ASPECT_RATIO_1_1
+                                        GO7007_ASPECT_RATIO_4_3_NTSC
+                                        GO7007_ASPECT_RATIO_4_3_PAL
+                                        GO7007_ASPECT_RATIO_16_9_NTSC
+                                        GO7007_ASPECT_RATIO_16_9_PAL
+
+        __u32                        Bit-wise OR of control flags (below)
+          flags
+
+    Flags in struct go7007_comp_params:
+
+        GO7007_COMP_CLOSED_GOP       Only produce self-contained GOPs, used
+                                     to produce streams appropriate for
+                                     random seeking.
+
+        GO7007_COMP_OMIT_SEQ_HEADER  Omit the stream sequence header.
+
+
+  GO7007IOC_S_MPEG_PARAMS, GO7007IOC_G_MPEG_PARAMS
+
+    These ioctls are used to negotiate MPEG-specific stream parameters when
+    the pixelformat has been set to V4L2_PIX_FMT_MPEG.
+
+    To query the current parameters, call the GO7007IOC_G_MPEG_PARAMS ioctl
+    with a pointer to a struct go7007_mpeg_params.  If the driver is not
+    set to MPEG format, the EINVAL error code will be returned.
+
+    To change the current parameters, initialize all fields of a struct
+    go7007_mpeg_params and call the GO7007_IOC_S_MPEG_PARAMS ioctl with a
+    pointer to this structure.  The driver will return the current
+    parameters with any necessary changes to conform to the limitations of
+    the hardware or selected MPEG mode.  Any or all fields can be set to
+    zero to request a reasonable default value.  If the driver is not set
+    to MPEG format, the EINVAL error code will be returned.  When I/O is in
+    progress, the EBUSY error code will be returned.
+
+    Fields in struct go7007_mpeg_params:
+
+        enum go7007_mpeg_video_standard
+          mpeg_video_standard        The MPEG video standard in which to
+                                     compress the video.
+
+                                     Choices are:
+                                        GO7007_MPEG_VIDEO_MPEG1
+                                        GO7007_MPEG_VIDEO_MPEG2
+                                        GO7007_MPEG_VIDEO_MPEG4
+
+        __u32                        Bit-wise OR of control flags (below)
+          flags
+
+        __u32                        The profile and level indication to be
+          pali                       stored in the sequence header.  This
+                                     is only used as an indicator to the
+                                     decoder, and does not affect the MPEG
+                                     features used in the video stream.
+                                     Not valid for MPEG1.
+
+                                     Choices for MPEG2 are:
+                                        GO7007_MPEG2_PROFILE_MAIN_MAIN
+
+                                     Choices for MPEG4 are:
+                                        GO7007_MPEG4_PROFILE_S_L0
+                                        GO7007_MPEG4_PROFILE_S_L1
+                                        GO7007_MPEG4_PROFILE_S_L2
+                                        GO7007_MPEG4_PROFILE_S_L3
+                                        GO7007_MPEG4_PROFILE_ARTS_L1
+                                        GO7007_MPEG4_PROFILE_ARTS_L2
+                                        GO7007_MPEG4_PROFILE_ARTS_L3
+                                        GO7007_MPEG4_PROFILE_ARTS_L4
+                                        GO7007_MPEG4_PROFILE_AS_L0
+                                        GO7007_MPEG4_PROFILE_AS_L1
+                                        GO7007_MPEG4_PROFILE_AS_L2
+                                        GO7007_MPEG4_PROFILE_AS_L3
+                                        GO7007_MPEG4_PROFILE_AS_L4
+                                        GO7007_MPEG4_PROFILE_AS_L5
+
+    Flags in struct go7007_mpeg_params:
+
+        GO7007_MPEG_FORCE_DVD_MODE   Force all compression parameters and
+                                     bitrate control settings to comply
+                                     with DVD MPEG2 stream requirements.
+                                     This overrides most compression and
+                                     bitrate settings!
+
+        GO7007_MPEG_OMIT_GOP_HEADER  Omit the GOP header.
+
+        GO7007_MPEG_REPEAT_SEQHEADER Repeat the MPEG sequence header at
+                                     the start of each GOP.
+
+
+  GO7007IOC_S_BITRATE, GO7007IOC_G_BITRATE
+
+    These ioctls are used to set and query the target bitrate value for the
+    compressed video stream.  The bitrate may be selected by storing the
+    target bits per second in an int and calling GO7007IOC_S_BITRATE with a
+    pointer to the int.  The bitrate may be queried by calling
+    GO7007IOC_G_BITRATE with a pointer to an int where the current bitrate
+    will be stored.
+
+    Note that this is the primary means of controlling the video quality
+    for all compression modes, including V4L2_PIX_FMT_MJPEG.  The
+    VIDIOC_S_JPEGCOMP ioctl is not supported.
+
+
+----------------------------------------------------------------------------
+                   Installing the WIS PCI Voyager Driver
+---------------------------------------------------------------------------
+
+The WIS PCI Voyager driver requires several patches to the Linux 2.6.11.x
+kernel source tree before compiling the driver.  These patches update the
+in-kernel SAA7134 driver to the newest development version and patch bugs
+in the TDA8290/TDA8275 tuner driver.
+
+The following patches must be downloaded from Gerd Knorr's website and
+applied in the order listed:
+
+	http://dl.bytesex.org/patches/2.6.11-2/i2c-tuner
+	http://dl.bytesex.org/patches/2.6.11-2/i2c-tuner2
+	http://dl.bytesex.org/patches/2.6.11-2/v4l2-api-mpeg
+	http://dl.bytesex.org/patches/2.6.11-2/saa7134-update
+
+The following patches are included with this SDK and can be applied in any
+order:
+
+	patches/2.6.11/saa7134-voyager.diff
+	patches/2.6.11/tda8275-newaddr.diff
+	patches/2.6.11/tda8290-ntsc.diff
+
+Check to make sure the CONFIG_VIDEO_SAA7134 option is enabled in the kernel
+configuration, and build and install the kernel.
+
+After rebooting into the new kernel, the GO7007 driver can be compiled and
+installed:
+
+	$ make SAA7134_BUILD=y
+	$ make install
+	$ modprobe saa7134-go7007
+
+There will be two V4L video devices associated with the PCI Voyager.  The
+first device (most likely /dev/video0) provides access to the raw video
+capture mode of the SAA7133 device and is used to configure the source
+video parameters and tune the TV tuner.  This device can be used with xawtv
+or other V4L(2) video software as a standard uncompressed device.
+
+The second device (most likely /dev/video1) provides access to the
+compression functions of the GO7007.  It can be tested using the gorecord
+application in the apps/ directory of this SDK:
+
+	$ apps/gorecord -vdevice /dev/video1 -noaudio test.avi
+
+Currently the frame resolution is fixed at 720x480 (NTSC) or 720x576 (PAL),
+and the video standard must be specified to both the raw and the compressed
+video devices (xawtv and gorecord, for example).
+
+
+--------------------------------------------------------------------------
+RELEASE NOTES FOR WIS GO7007SB LINUX DRIVER
+---------------------------------------------------------------------------
+
+Last updated: 5 November 2005
+
+ - Release 0.9.7 includes new support for using udev to run fxload.  The
+   install script should automatically detect whether the old hotplug
+   scripts or the new udev rules should be used.  To force the use of
+   hotplug, run "make install USE_UDEV=n".  To force the use of udev, run
+   "make install USE_UDEV=y".
+
+ - Motion detection is supported but undocumented.  Try the `modet` app
+   for a demonstration of how to use the facility.
+
+ - Using USB2.0 devices such as the TV402U with USB1.1 HCDs or hubs can
+   cause buffer overruns and frame drops, even at low framerates, due to
+   inconsistency in the bitrate control mechanism.
+
+ - On devices with an SAA7115, including the Plextor ConvertX, video height
+   values of 96, 128, 160, 192, 256, 320, and 384 do not work in NTSC mode.
+   All valid heights up to 512 work correctly in PAL mode.
+
+ - The WIS Star Trek and PCI Voyager boards have no support yet for audio
+   or the TV tuner.
diff --git a/drivers/staging/go7007/s2250-board.c b/drivers/staging/go7007/s2250-board.c
new file mode 100644
index 00000000000..fb6845e3788
--- /dev/null
+++ b/drivers/staging/go7007/s2250-board.c
@@ -0,0 +1,630 @@
+/*
+ * Copyright (C) 2008 Sensoray Company Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License (Version 2) as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 59 Temple Place - Suite 330, Boston MA 02111-1307, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/usb.h>
+#include <linux/i2c.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-common.h>
+#include "go7007-priv.h"
+#include "wis-i2c.h"
+
+extern int s2250loader_init(void);
+extern void s2250loader_cleanup(void);
+
+#define TLV320_ADDRESS      0x34
+#define S2250_VIDDEC        0x86
+#define VPX322_ADDR_ANALOGCONTROL1	0x02
+#define VPX322_ADDR_BRIGHTNESS0		0x0127
+#define VPX322_ADDR_BRIGHTNESS1		0x0131
+#define VPX322_ADDR_CONTRAST0		0x0128
+#define VPX322_ADDR_CONTRAST1		0x0132
+#define VPX322_ADDR_HUE			0x00dc
+#define VPX322_ADDR_SAT	 	        0x0030
+
+struct go7007_usb_board {
+	unsigned int flags;
+	struct go7007_board_info main_info;
+};
+
+struct go7007_usb {
+	struct go7007_usb_board *board;
+	struct semaphore i2c_lock;
+	struct usb_device *usbdev;
+	struct urb *video_urbs[8];
+	struct urb *audio_urbs[8];
+	struct urb *intr_urb;
+};
+
+static unsigned char aud_regs[] = {
+	0x1e, 0x00,
+	0x00, 0x17,
+	0x02, 0x17,
+	0x04, 0xf9,
+	0x06, 0xf9,
+	0x08, 0x02,
+	0x0a, 0x00,
+	0x0c, 0x00,
+	0x0a, 0x00,
+	0x0c, 0x00,
+	0x0e, 0x02,
+	0x10, 0x00,
+	0x12, 0x01,
+	0x00, 0x00,
+};
+
+
+static unsigned char vid_regs[] = {
+	0xF2, 0x0f,
+	0xAA, 0x00,
+	0xF8, 0xff,
+	0x00, 0x00,
+};
+
+static u16 vid_regs_fp[] = {
+	0x028, 0x067,
+	0x120, 0x016,
+	0x121, 0xcF2,
+	0x122, 0x0F2,
+	0x123, 0x00c,
+	0x124, 0x2d0,
+	0x125, 0x2e0,
+	0x126, 0x004,
+	0x128, 0x1E0,
+	0x12A, 0x016,
+	0x12B, 0x0F2,
+	0x12C, 0x0F2,
+	0x12D, 0x00c,
+	0x12E, 0x2d0,
+	0x12F, 0x2e0,
+	0x130, 0x004,
+	0x132, 0x1E0,
+	0x140, 0x060,
+	0x153, 0x00C,
+	0x154, 0x200,
+	0x150, 0x801,
+	0x000, 0x000
+};
+
+/* PAL specific values */
+static u16 vid_regs_fp_pal[] =
+{
+	0x120, 0x017,
+	0x121, 0xd22,
+	0x122, 0x122,
+	0x12A, 0x017,
+	0x12B, 0x122,
+	0x12C, 0x122,
+	0x140, 0x060,
+	0x000, 0x000,
+};
+
+struct s2250 {
+	int std;
+	int input;
+	int brightness;
+	int contrast;
+	int saturation;
+	int hue;
+	int reg12b_val;
+	int audio_input;
+};
+
+/* from go7007-usb.c which is Copyright (C) 2005-2006 Micronas USA Inc.*/
+static int go7007_usb_vendor_request(struct go7007 *go, u16 request,
+	u16 value, u16 index, void *transfer_buffer, int length, int in)
+{
+	struct go7007_usb *usb = go->hpi_context;
+	int timeout = 5000;
+
+	if (in) {
+		return usb_control_msg(usb->usbdev,
+				usb_rcvctrlpipe(usb->usbdev, 0), request,
+				USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_IN,
+				value, index, transfer_buffer, length, timeout);
+	} else {
+		return usb_control_msg(usb->usbdev,
+				usb_sndctrlpipe(usb->usbdev, 0), request,
+				USB_TYPE_VENDOR | USB_RECIP_DEVICE,
+				value, index, transfer_buffer, length, timeout);
+	}
+}
+/* end from go7007-usb.c which is Copyright (C) 2005-2006 Micronas USA Inc.*/
+
+static int write_reg(struct i2c_client *client, u8 reg, u8 value)
+{
+	struct go7007 *go = i2c_get_adapdata(client->adapter);
+	struct go7007_usb *usb = go->hpi_context;
+	int rc;
+	int dev_addr = client->addr;
+	u8 *buf;
+
+	if (go == NULL)
+		return -ENODEV;
+
+	if (go->status == STATUS_SHUTDOWN)
+		return -EBUSY;
+
+	buf = kzalloc(16, GFP_KERNEL);
+	if (buf == NULL)
+		return -ENOMEM;
+
+	if (down_interruptible(&usb->i2c_lock) != 0) {
+		printk(KERN_INFO "i2c lock failed\n");
+		return -EINTR;
+	}
+	rc = go7007_usb_vendor_request(go, 0x55, dev_addr,
+				       (reg<<8 | value),
+				       buf,
+				       16, 1);
+
+	up(&usb->i2c_lock);
+	kfree(buf);
+	return rc;
+}
+
+static int write_reg_fp(struct i2c_client *client, u16 addr, u16 val)
+{
+	struct go7007 *go = i2c_get_adapdata(client->adapter);
+	struct go7007_usb *usb = go->hpi_context;
+	u8 *buf;
+	struct s2250 *dec = i2c_get_clientdata(client);
+
+	if (go == NULL)
+		return -ENODEV;
+
+	if (go->status == STATUS_SHUTDOWN)
+		return -EBUSY;
+
+	buf = kzalloc(16, GFP_KERNEL);
+
+	if (buf == NULL)
+		return -ENOMEM;
+
+
+
+	memset(buf, 0xcd, 6);
+
+	if (down_interruptible(&usb->i2c_lock) != 0) {
+		printk(KERN_INFO "i2c lock failed\n");
+		return -EINTR;
+	}
+	if (go7007_usb_vendor_request(go, 0x57, addr, val, buf, 16, 1) < 0)
+		return -EFAULT;
+
+	up(&usb->i2c_lock);
+	if (buf[0] == 0) {
+		unsigned int subaddr, val_read;
+
+		subaddr = (buf[4] << 8) + buf[5];
+		val_read = (buf[2] << 8) + buf[3];
+		if (val_read != val) {
+			printk(KERN_INFO "invalid fp write %x %x\n",
+			       val_read, val);
+			return -EFAULT;
+		}
+		if (subaddr != addr) {
+			printk(KERN_INFO "invalid fp write addr %x %x\n",
+			       subaddr, addr);
+			return -EFAULT;
+		}
+	} else
+		return -EFAULT;
+
+	/* save last 12b value */
+	if (addr == 0x12b)
+		dec->reg12b_val = val;
+
+	return 0;
+}
+
+static int write_regs(struct i2c_client *client, u8 *regs)
+{
+	int i;
+
+	for (i = 0; !((regs[i] == 0x00) && (regs[i+1] == 0x00)); i += 2) {
+		if (write_reg(client, regs[i], regs[i+1]) < 0) {
+			printk(KERN_INFO "s2250: failed\n");
+			return -1;
+		}
+	}
+	return 0;
+}
+
+static int write_regs_fp(struct i2c_client *client, u16 *regs)
+{
+	int i;
+
+	for (i = 0; !((regs[i] == 0x00) && (regs[i+1] == 0x00)); i += 2) {
+		if (write_reg_fp(client, regs[i], regs[i+1]) < 0) {
+			printk(KERN_INFO "s2250: failed fp\n");
+			return -1;
+		}
+	}
+	return 0;
+}
+
+
+static int s2250_command(struct i2c_client *client,
+			 unsigned int cmd, void *arg)
+{
+	struct s2250 *dec = i2c_get_clientdata(client);
+
+	switch (cmd) {
+	case VIDIOC_S_INPUT:
+	{
+		int vidsys;
+		int *input = arg;
+
+		vidsys = (dec->std == V4L2_STD_NTSC) ? 0x01 : 0x00;
+		if (*input == 0) {
+			/* composite */
+			write_reg_fp(client, 0x20, 0x020 | vidsys);
+			write_reg_fp(client, 0x21, 0x662);
+			write_reg_fp(client, 0x140, 0x060);
+		} else {
+			/* S-Video */
+			write_reg_fp(client, 0x20, 0x040 | vidsys);
+			write_reg_fp(client, 0x21, 0x666);
+			write_reg_fp(client, 0x140, 0x060);
+		}
+		dec->input = *input;
+		break;
+	}
+	case VIDIOC_S_STD:
+	{
+		v4l2_std_id *std = arg;
+		u16 vidsource;
+
+		vidsource = (dec->input == 1) ? 0x040 : 0x020;
+		dec->std = *std;
+		switch (dec->std) {
+		case V4L2_STD_NTSC:
+			write_regs_fp(client, vid_regs_fp);
+			write_reg_fp(client, 0x20, vidsource | 1);
+			break;
+		case V4L2_STD_PAL:
+			write_regs_fp(client, vid_regs_fp);
+			write_regs_fp(client, vid_regs_fp_pal);
+			write_reg_fp(client, 0x20, vidsource);
+			break;
+		default:
+			return -EINVAL;
+		}
+		break;
+	}
+	case VIDIOC_QUERYCTRL:
+	{
+		struct v4l2_queryctrl *ctrl = arg;
+		static const u32 user_ctrls[] = {
+			V4L2_CID_BRIGHTNESS,
+			V4L2_CID_CONTRAST,
+			V4L2_CID_SATURATION,
+			V4L2_CID_HUE,
+			0
+		};
+		static const u32 *ctrl_classes[] = {
+			user_ctrls,
+			NULL
+		};
+
+		ctrl->id = v4l2_ctrl_next(ctrl_classes, ctrl->id);
+		switch (ctrl->id) {
+		case V4L2_CID_BRIGHTNESS:
+			v4l2_ctrl_query_fill(ctrl, 0, 100, 1, 50);
+			break;
+		case V4L2_CID_CONTRAST:
+			v4l2_ctrl_query_fill(ctrl, 0, 100, 1, 50);
+			break;
+		case V4L2_CID_SATURATION:
+			v4l2_ctrl_query_fill(ctrl, 0, 100, 1, 50);
+			break;
+		case V4L2_CID_HUE:
+			v4l2_ctrl_query_fill(ctrl, -50, 50, 1, 0);
+			break;
+		default:
+			ctrl->name[0] = '\0';
+			return -EINVAL;
+		}
+		break;
+	}
+	case VIDIOC_S_CTRL:
+	{
+		struct v4l2_control *ctrl = arg;
+		int value1;
+
+		switch (ctrl->id) {
+		case V4L2_CID_BRIGHTNESS:
+			printk(KERN_INFO "s2250: future setting\n");
+			return -EINVAL;
+		case V4L2_CID_CONTRAST:
+			printk(KERN_INFO "s2250: future setting\n");
+			return -EINVAL;
+			break;
+		case V4L2_CID_SATURATION:
+			if (ctrl->value > 127)
+				dec->saturation = 127;
+			else if (ctrl->value < 0)
+				dec->saturation = 0;
+			else
+				dec->saturation = ctrl->value;
+
+			value1 = dec->saturation * 4140 / 100;
+			if (value1 > 4094)
+				value1 = 4094;
+			write_reg_fp(client, VPX322_ADDR_SAT, value1);
+			break;
+		case V4L2_CID_HUE:
+			if (ctrl->value > 50)
+				dec->hue = 50;
+			else if (ctrl->value < -50)
+				dec->hue = -50;
+			else
+				dec->hue = ctrl->value;
+			/* clamp the hue range */
+			value1 = dec->hue * 280 / 50;
+			write_reg_fp(client, VPX322_ADDR_HUE, value1);
+			break;
+		}
+		break;
+	}
+	case VIDIOC_G_CTRL:
+	{
+		struct v4l2_control *ctrl = arg;
+
+		switch (ctrl->id) {
+		case V4L2_CID_BRIGHTNESS:
+			ctrl->value = dec->brightness;
+			break;
+		case V4L2_CID_CONTRAST:
+			ctrl->value = dec->contrast;
+			break;
+		case V4L2_CID_SATURATION:
+			ctrl->value = dec->saturation;
+			break;
+		case V4L2_CID_HUE:
+			ctrl->value = dec->hue;
+			break;
+		}
+		break;
+	}
+	case VIDIOC_S_FMT:
+	{
+		struct v4l2_format *fmt = arg;
+		if (fmt->fmt.pix.height < 640) {
+			write_reg_fp(client, 0x12b, dec->reg12b_val | 0x400);
+			write_reg_fp(client, 0x140, 0x060);
+		} else {
+			write_reg_fp(client, 0x12b, dec->reg12b_val & ~0x400);
+			write_reg_fp(client, 0x140, 0x060);
+		}
+		return 0;
+	}
+	case VIDIOC_G_AUDIO:
+	{
+		struct v4l2_audio *audio = arg;
+
+		memset(audio, 0, sizeof(*audio));
+		audio->index = dec->audio_input;
+		/* fall through */
+	}
+	case VIDIOC_ENUMAUDIO:
+	{
+		struct v4l2_audio *audio = arg;
+
+		switch (audio->index) {
+		case 0:
+			strcpy(audio->name, "Line In");
+			break;
+		case 1:
+			strcpy(audio->name, "Mic");
+			break;
+		case 2:
+			strcpy(audio->name, "Mic Boost");
+			break;
+		default:
+			audio->name[0] = '\0';
+			return 0;
+		}
+		audio->capability = V4L2_AUDCAP_STEREO;
+		audio->mode = 0;
+		return 0;
+	}
+	case VIDIOC_S_AUDIO:
+	{
+		struct v4l2_audio *audio = arg;
+
+		client->addr = TLV320_ADDRESS;
+		switch (audio->index) {
+		case 0:
+			write_reg(client, 0x08, 0x02); /* Line In */
+			break;
+		case 1:
+			write_reg(client, 0x08, 0x04); /* Mic */
+			break;
+		case 2:
+			write_reg(client, 0x08, 0x05); /* Mic Boost */
+			break;
+		default:
+			return -EINVAL;
+		}
+		dec->audio_input = audio->index;
+		return 0;
+	}
+
+	default:
+		printk(KERN_INFO "s2250: unknown command 0x%x\n", cmd);
+		break;
+	}
+	return 0;
+}
+
+static struct i2c_driver s2250_driver;
+
+static struct i2c_client s2250_client_templ = {
+	.name		= "Sensoray 2250",
+	.driver		= &s2250_driver,
+};
+
+static int s2250_detect(struct i2c_adapter *adapter, int addr, int kind)
+{
+	struct i2c_client *client;
+	struct s2250 *dec;
+	u8 *data;
+	struct go7007 *go = i2c_get_adapdata(adapter);
+	struct go7007_usb *usb = go->hpi_context;
+
+	client = kmalloc(sizeof(struct i2c_client), GFP_KERNEL);
+	if (client == NULL)
+		return -ENOMEM;
+	memcpy(client, &s2250_client_templ,
+	       sizeof(s2250_client_templ));
+	client->adapter = adapter;
+
+	dec = kmalloc(sizeof(struct s2250), GFP_KERNEL);
+	if (dec == NULL) {
+		kfree(client);
+		return -ENOMEM;
+	}
+
+	dec->std = V4L2_STD_NTSC;
+	dec->brightness = 50;
+	dec->contrast = 50;
+	dec->saturation = 50;
+	dec->hue = 0;
+	client->addr = TLV320_ADDRESS;
+	i2c_set_clientdata(client, dec);
+
+	printk(KERN_DEBUG
+	       "s2250: initializing video decoder on %s\n",
+	       adapter->name);
+
+	/* initialize the audio */
+	client->addr = TLV320_ADDRESS;
+	if (write_regs(client, aud_regs) < 0) {
+		printk(KERN_ERR
+		       "s2250: error initializing audio\n");
+		kfree(client);
+		kfree(dec);
+		return 0;
+	}
+	client->addr = S2250_VIDDEC;
+	i2c_set_clientdata(client, dec);
+
+	if (write_regs(client, vid_regs) < 0) {
+		printk(KERN_ERR
+		       "s2250: error initializing decoder\n");
+		kfree(client);
+		kfree(dec);
+		return 0;
+	}
+	if (write_regs_fp(client, vid_regs_fp) < 0) {
+		printk(KERN_ERR
+		       "s2250: error initializing decoder\n");
+		kfree(client);
+		kfree(dec);
+		return 0;
+	}
+	/* set default channel */
+	/* composite */
+	write_reg_fp(client, 0x20, 0x020 | 1);
+	write_reg_fp(client, 0x21, 0x662);
+	write_reg_fp(client, 0x140, 0x060);
+
+	/* set default audio input */
+	dec->audio_input = 0;
+	write_reg(client, 0x08, 0x02); /* Line In */
+
+	if (down_interruptible(&usb->i2c_lock) == 0) {
+		data = kzalloc(16, GFP_KERNEL);
+		if (data != NULL) {
+			int rc;
+			rc = go7007_usb_vendor_request(go, 0x41, 0, 0,
+						       data, 16, 1);
+			if (rc > 0) {
+				u8 mask;
+				data[0] = 0;
+				mask = 1<<5;
+				data[0] &= ~mask;
+				data[1] |= mask;
+				go7007_usb_vendor_request(go, 0x40, 0,
+							  (data[1]<<8)
+							  + data[1],
+							  data, 16, 0);
+			}
+			kfree(data);
+		}
+		up(&usb->i2c_lock);
+	}
+
+	i2c_attach_client(client);
+	printk("s2250: initialized successfully\n");
+	return 0;
+}
+
+static int s2250_detach(struct i2c_client *client)
+{
+	struct s2250 *dec = i2c_get_clientdata(client);
+	int r;
+
+	r = i2c_detach_client(client);
+	if (r < 0)
+		return r;
+
+	kfree(client);
+	kfree(dec);
+	return 0;
+}
+
+static struct i2c_driver s2250_driver = {
+	.driver = {
+		.name	= "Sensoray 2250 board driver",
+	},
+	.id		= I2C_DRIVERID_S2250,
+	.detach_client	= s2250_detach,
+	.command	= s2250_command,
+};
+
+static int __init s2250_init(void)
+{
+	int r;
+
+	r = s2250loader_init();
+	if (r < 0)
+		return r;
+
+	r = i2c_add_driver(&s2250_driver);
+	if (r < 0)
+		return r;
+	return wis_i2c_add_driver(s2250_driver.id, s2250_detect);
+}
+
+static void __exit s2250_cleanup(void)
+{
+	wis_i2c_del_driver(s2250_detect);
+	i2c_del_driver(&s2250_driver);
+
+	s2250loader_cleanup();
+}
+
+module_init(s2250_init);
+module_exit(s2250_cleanup);
+
+MODULE_AUTHOR("");
+MODULE_DESCRIPTION("Board driver for Sensoryray 2250");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/go7007/s2250-loader.c b/drivers/staging/go7007/s2250-loader.c
new file mode 100644
index 00000000000..a5e4acab089
--- /dev/null
+++ b/drivers/staging/go7007/s2250-loader.c
@@ -0,0 +1,188 @@
+/*
+ * Copyright (C) 2008 Sensoray Company Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License (Version 2) as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 59 Temple Place - Suite 330, Boston MA 02111-1307, USA.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/usb.h>
+#include <dvb-usb.h>
+
+#define S2250_LOADER_FIRMWARE	"s2250_loader.fw"
+#define S2250_FIRMWARE		"s2250.fw"
+
+typedef struct device_extension_s {
+    struct kref     kref;
+    int minor;
+    struct usb_device *usbdev;
+} device_extension_t, *pdevice_extension_t;
+
+#define USB_s2250loader_MAJOR 240
+#define USB_s2250loader_MINOR_BASE 0
+#define MAX_DEVICES 256
+
+static pdevice_extension_t s2250_dev_table[MAX_DEVICES];
+static DECLARE_MUTEX(s2250_dev_table_mutex);
+
+#define to_s2250loader_dev_common(d) container_of(d, device_extension_t, kref)
+static void s2250loader_delete(struct kref *kref)
+{
+	pdevice_extension_t s = to_s2250loader_dev_common(kref);
+	s2250_dev_table[s->minor] = NULL;
+	kfree(s);
+}
+
+static int s2250loader_probe(struct usb_interface *interface,
+				const struct usb_device_id *id)
+{
+	struct usb_device *usbdev;
+	int minor, ret;
+	pdevice_extension_t s = NULL;
+	const struct firmware *fw;
+
+	usbdev = usb_get_dev(interface_to_usbdev(interface));
+	if (!usbdev) {
+		printk(KERN_ERR "Enter s2250loader_probe failed\n");
+		return -1;
+	}
+	printk(KERN_INFO "Enter s2250loader_probe 2.6 kernel\n");
+	printk(KERN_INFO "vendor id 0x%x, device id 0x%x devnum:%d\n",
+	   usbdev->descriptor.idVendor, usbdev->descriptor.idProduct,
+	   usbdev->devnum);
+
+	if (usbdev->descriptor.bNumConfigurations != 1) {
+		printk(KERN_ERR "can't handle multiple config\n");
+		return -1;
+	}
+	down(&s2250_dev_table_mutex);
+
+	for (minor = 0; minor < MAX_DEVICES; minor++) {
+		if (s2250_dev_table[minor] == NULL)
+			break;
+	}
+
+	if (minor < 0 || minor >= MAX_DEVICES) {
+		printk(KERN_ERR "Invalid minor: %d\n", minor);
+		goto failed;
+	}
+
+	/* Allocate dev data structure */
+	s = kmalloc(sizeof(device_extension_t), GFP_KERNEL);
+	if (s == NULL) {
+		printk(KERN_ERR "Out of memory\n");
+		goto failed;
+	}
+	s2250_dev_table[minor] = s;
+
+	printk(KERN_INFO "s2250loader_probe: Device %d on Bus %d Minor %d\n",
+		usbdev->devnum, usbdev->bus->busnum, minor);
+
+	memset(s, 0, sizeof(device_extension_t));
+	s->usbdev = usbdev;
+	printk(KERN_INFO "loading 2250 loader\n");
+
+	kref_init(&(s->kref));
+
+	up(&s2250_dev_table_mutex);
+
+	if (request_firmware(&fw, S2250_LOADER_FIRMWARE, &usbdev->dev)) {
+		printk(KERN_ERR
+			"s2250: unable to load firmware from file \"%s\"\n",
+			S2250_LOADER_FIRMWARE);
+		goto failed2;
+	}
+	ret = usb_cypress_load_firmware(usbdev, fw, CYPRESS_FX2);
+	release_firmware(fw);
+	if (0 != ret) {
+		printk(KERN_ERR "loader download failed\n");
+		goto failed2;
+	}
+
+	if (request_firmware(&fw, S2250_FIRMWARE, &usbdev->dev)) {
+		printk(KERN_ERR
+			"s2250: unable to load firmware from file \"%s\"\n",
+			S2250_FIRMWARE);
+		goto failed2;
+	}
+	ret = usb_cypress_load_firmware(usbdev, fw, CYPRESS_FX2);
+	release_firmware(fw);
+	if (0 != ret) {
+		printk(KERN_ERR "firmware_s2250 download failed\n");
+		goto failed2;
+	}
+
+	usb_set_intfdata(interface, s);
+	return 0;
+
+failed:
+	up(&s2250_dev_table_mutex);
+failed2:
+	if (s)
+		kref_put(&(s->kref), s2250loader_delete);
+
+	printk(KERN_ERR "probe failed\n");
+	return -1;
+}
+
+static void s2250loader_disconnect(struct usb_interface *interface)
+{
+	pdevice_extension_t s = usb_get_intfdata(interface);
+	printk(KERN_INFO "s2250: disconnect\n");
+	lock_kernel();
+	s = usb_get_intfdata(interface);
+	usb_set_intfdata(interface, NULL);
+	kref_put(&(s->kref), s2250loader_delete);
+	unlock_kernel();
+}
+
+static struct usb_device_id s2250loader_ids[] = {
+	{USB_DEVICE(0x1943, 0xa250)},
+	{}                          /* Terminating entry */
+};
+
+MODULE_DEVICE_TABLE(usb, s2250loader_ids);
+
+static struct usb_driver s2250loader_driver = {
+	.name		= "s2250-loader",
+	.probe		= s2250loader_probe,
+	.disconnect	= s2250loader_disconnect,
+	.id_table	= s2250loader_ids,
+};
+
+int s2250loader_init(void)
+{
+	int r;
+	unsigned i = 0;
+
+	for (i = 0; i < MAX_DEVICES; i++)
+		s2250_dev_table[i] = NULL;
+
+	r = usb_register(&s2250loader_driver);
+	if (r) {
+		printk(KERN_ERR "usb_register failed. Error number %d\n", r);
+		return -1;
+	}
+
+	printk(KERN_INFO "s2250loader_init: driver registered\n");
+	return 0;
+}
+EXPORT_SYMBOL(s2250loader_init);
+
+void s2250loader_cleanup(void)
+{
+	printk(KERN_INFO "s2250loader_cleanup\n");
+	usb_deregister(&s2250loader_driver);
+}
+EXPORT_SYMBOL(s2250loader_cleanup);
diff --git a/drivers/staging/go7007/saa7134-go7007.c b/drivers/staging/go7007/saa7134-go7007.c
index c4a6d8ef907..665bbf59d02 100644
--- a/drivers/staging/go7007/saa7134-go7007.c
+++ b/drivers/staging/go7007/saa7134-go7007.c
@@ -27,7 +27,7 @@
 #include <linux/usb.h>
 #include <linux/i2c.h>
 #include <asm/byteorder.h>
-#include <media/audiochip.h>
+#include <media/v4l2-common.h>
 
 #include "saa7134-reg.h"
 #include "saa7134.h"
@@ -314,7 +314,13 @@ static int saa7134_go7007_stream_start(struct go7007 *go)
 static int saa7134_go7007_stream_stop(struct go7007 *go)
 {
 	struct saa7134_go7007 *saa = go->hpi_context;
-	struct saa7134_dev *dev = saa->dev;
+	struct saa7134_dev *dev;
+
+	if (!saa)
+		return -EINVAL;
+	dev = saa->dev;
+	if (!dev)
+		return -EINVAL;
 
 	/* Shut down TS FIFO */
 	saa_clearl(SAA7134_MAIN_CTRL, SAA7134_MAIN_CTRL_TE5);
@@ -373,6 +379,47 @@ static int saa7134_go7007_send_firmware(struct go7007 *go, u8 *data, int len)
 	return 0;
 }
 
+static int saa7134_go7007_send_command(struct go7007 *go, unsigned int cmd,
+					void *arg)
+{
+	struct saa7134_go7007 *saa = go->hpi_context;
+	struct saa7134_dev *dev = saa->dev;
+
+	switch (cmd) {
+	case VIDIOC_S_STD:
+	{
+		v4l2_std_id *std = arg;
+		return saa7134_s_std_internal(dev, NULL, std);
+	}
+	case VIDIOC_G_STD:
+	{
+		v4l2_std_id *std = arg;
+		*std = dev->tvnorm->id;
+		return 0;
+	}
+	case VIDIOC_QUERYCTRL:
+	{
+		struct v4l2_queryctrl *ctrl = arg;
+		if (V4L2_CTRL_ID2CLASS(ctrl->id) == V4L2_CTRL_CLASS_USER)
+			return saa7134_queryctrl(NULL, NULL, ctrl);
+	}
+	case VIDIOC_G_CTRL:
+	{
+		struct v4l2_control *ctrl = arg;
+		if (V4L2_CTRL_ID2CLASS(ctrl->id) == V4L2_CTRL_CLASS_USER)
+			return saa7134_g_ctrl_internal(dev, NULL, ctrl);
+	}
+	case VIDIOC_S_CTRL:
+	{
+		struct v4l2_control *ctrl = arg;
+		if (V4L2_CTRL_ID2CLASS(ctrl->id) == V4L2_CTRL_CLASS_USER)
+			return saa7134_s_ctrl_internal(dev, NULL, ctrl);
+	}
+	}
+	return -EINVAL;
+
+}
+
 static struct go7007_hpi_ops saa7134_go7007_hpi_ops = {
 	.interface_reset	= saa7134_go7007_interface_reset,
 	.write_interrupt	= saa7134_go7007_write_interrupt,
@@ -380,6 +427,7 @@ static struct go7007_hpi_ops saa7134_go7007_hpi_ops = {
 	.stream_start		= saa7134_go7007_stream_start,
 	.stream_stop		= saa7134_go7007_stream_stop,
 	.send_firmware		= saa7134_go7007_send_firmware,
+	.send_command		= saa7134_go7007_send_command,
 };
 
 /********************* Add/remove functions *********************/
diff --git a/drivers/staging/go7007/wis-i2c.h b/drivers/staging/go7007/wis-i2c.h
index 993f658ad73..431f41dd396 100644
--- a/drivers/staging/go7007/wis-i2c.h
+++ b/drivers/staging/go7007/wis-i2c.h
@@ -23,6 +23,7 @@
 #define	I2C_DRIVERID_WIS_SAA7113	0xf0f4
 #define	I2C_DRIVERID_WIS_OV7640		0xf0f5
 #define	I2C_DRIVERID_WIS_TW2804		0xf0f6
+#define	I2C_DRIVERID_S2250		0xf0f7
 #define	I2C_ALGO_GO7007			0xf00000
 #define	I2C_ALGO_GO7007_USB		0xf10000
 
diff --git a/drivers/staging/go7007/wis-sony-tuner.c b/drivers/staging/go7007/wis-sony-tuner.c
index 5a91ee409a7..58fddb12237 100644
--- a/drivers/staging/go7007/wis-sony-tuner.c
+++ b/drivers/staging/go7007/wis-sony-tuner.c
@@ -604,7 +604,7 @@ static int tuner_command(struct i2c_client *client, unsigned int cmd, void *arg)
 	{
 		struct v4l2_tuner *tun = arg;
 
-		memset(t, 0, sizeof(*tun));
+		memset(tun, 0, sizeof(*tun));
 		strcpy(tun->name, "Television");
 		tun->type = V4L2_TUNER_ANALOG_TV;
 		tun->rangelow = 0UL; /* does anything use these? */
diff --git a/drivers/staging/me4000/me4000.c b/drivers/staging/me4000/me4000.c
index 0394e270927..e1c4a807890 100644
--- a/drivers/staging/me4000/me4000.c
+++ b/drivers/staging/me4000/me4000.c
@@ -536,25 +536,19 @@ module_init(me4000_init_module);
 
 static void clear_board_info_list(void)
 {
-	struct list_head *board_p;
-	struct list_head *dac_p;
-	struct me4000_info *board_info;
-	struct me4000_ao_context *ao_context;
+	struct me4000_info *board_info, *board_info_safe;
+	struct me4000_ao_context *ao_context, *ao_context_safe;
 
 	/* Clear context lists */
-	for (board_p = me4000_board_info_list.next;
-	     board_p != &me4000_board_info_list; board_p = board_p->next) {
-		board_info = list_entry(board_p, struct me4000_info, list);
+	list_for_each_entry(board_info, &me4000_board_info_list, list) {
 		/* Clear analog output context list */
-		while (!list_empty(&board_info->ao_context_list)) {
-			dac_p = board_info->ao_context_list.next;
-			ao_context =
-			    list_entry(dac_p, struct me4000_ao_context, list);
+		list_for_each_entry_safe(ao_context, ao_context_safe,
+				&board_info->ao_context_list, list) {
 			me4000_ao_reset(ao_context);
 			free_irq(ao_context->irq, ao_context);
 			if (ao_context->circ_buf.buf)
 				kfree(ao_context->circ_buf.buf);
-			list_del(dac_p);
+			list_del(&ao_context->list);
 			kfree(ao_context);
 		}
 
@@ -574,11 +568,10 @@ static void clear_board_info_list(void)
 	}
 
 	/* Clear the board info list */
-	while (!list_empty(&me4000_board_info_list)) {
-		board_p = me4000_board_info_list.next;
-		board_info = list_entry(board_p, struct me4000_info, list);
+	list_for_each_entry_safe(board_info, board_info_safe,
+			&me4000_board_info_list, list) {
 		pci_release_regions(board_info->pci_dev_p);
-		list_del(board_p);
+		list_del(&board_info->list);
 		kfree(board_info);
 	}
 }
@@ -663,16 +656,17 @@ static int init_board_info(struct pci_dev *pci_dev_p,
 	}
 
 	/* Get the index of the board in the global list */
-	for (board_p = me4000_board_info_list.next, i = 0;
-	     board_p != &me4000_board_info_list; board_p = board_p->next, i++) {
+	i = 0;
+	list_for_each(board_p, &me4000_board_info_list) {
 		if (board_p == &board_info->list) {
 			board_info->board_count = i;
 			break;
 		}
+		i++;
 	}
 	if (board_p == &me4000_board_info_list) {
 		printk(KERN_ERR
-		       "ME4000:init_board_info():Cannot get index of baord\n");
+		       "ME4000:init_board_info():Cannot get index of board\n");
 		return -ENODEV;
 	}
 
@@ -863,16 +857,14 @@ static int alloc_ao_contexts(struct me4000_info *info)
 
 static void release_ao_contexts(struct me4000_info *board_info)
 {
-	struct list_head *dac_p;
-	struct me4000_ao_context *ao_context;
+	struct me4000_ao_context *ao_context, *ao_context_safe;
 
 	/* Clear analog output context list */
-	while (!list_empty(&board_info->ao_context_list)) {
-		dac_p = board_info->ao_context_list.next;
-		ao_context = list_entry(dac_p, struct me4000_ao_context, list);
+	list_for_each_entry_safe(ao_context, ao_context_safe,
+			&board_info->ao_context_list, list) {
 		free_irq(ao_context->irq, ao_context);
 		kfree(ao_context->circ_buf.buf);
-		list_del(dac_p);
+		list_del(&ao_context->list);
 		kfree(ao_context);
 	}
 }
@@ -1180,7 +1172,7 @@ static int me4000_xilinx_download(struct me4000_info *info)
 
 	/* Wait until /INIT pin is set */
 	udelay(20);
-	if (!inl(info->plx_regbase + PLX_INTCSR) & 0x20) {
+	if (!(inl(info->plx_regbase + PLX_INTCSR) & 0x20)) {
 		printk(KERN_ERR "%s:Can't init Xilinx\n", __func__);
 		return -EIO;
 	}
@@ -1303,12 +1295,13 @@ static int me4000_open(struct inode *inode_p, struct file *file_p)
 		       dev, mode);
 
 		/* Search for the board context */
-		for (ptr = me4000_board_info_list.next, i = 0;
-		     ptr != &me4000_board_info_list; ptr = ptr->next, i++) {
-			board_info = list_entry(ptr, struct me4000_info, list);
+		i = 0;
+		list_for_each(ptr, &me4000_board_info_list) {
 			if (i == board)
 				break;
+			i++;
 		}
+		board_info = list_entry(ptr, struct me4000_info, list);
 
 		if (ptr == &me4000_board_info_list) {
 			printk(KERN_ERR
@@ -1318,14 +1311,13 @@ static int me4000_open(struct inode *inode_p, struct file *file_p)
 		}
 
 		/* Search for the dac context */
-		for (ptr = board_info->ao_context_list.next, i = 0;
-		     ptr != &board_info->ao_context_list;
-		     ptr = ptr->next, i++) {
-			ao_context = list_entry(ptr, struct me4000_ao_context,
-									list);
+		i = 0;
+		list_for_each(ptr, &board_info->ao_context_list) {
 			if (i == dev)
 				break;
+			i++;
 		}
+		ao_context = list_entry(ptr, struct me4000_ao_context, list);
 
 		if (ptr == &board_info->ao_context_list) {
 			printk(KERN_ERR
@@ -1384,12 +1376,13 @@ static int me4000_open(struct inode *inode_p, struct file *file_p)
 		PDEBUG("me4000_open():ai board = %d mode = %d\n", board, mode);
 
 		/* Search for the board context */
-		for (ptr = me4000_board_info_list.next, i = 0;
-		     ptr != &me4000_board_info_list; ptr = ptr->next, i++) {
-			board_info = list_entry(ptr, struct me4000_info, list);
+		i = 0;
+		list_for_each(ptr, &me4000_board_info_list) {
 			if (i == board)
 				break;
+			i++;
 		}
+		board_info = list_entry(ptr, struct me4000_info, list);
 
 		if (ptr == &me4000_board_info_list) {
 			printk(KERN_ERR
@@ -1438,14 +1431,12 @@ static int me4000_open(struct inode *inode_p, struct file *file_p)
 		PDEBUG("me4000_open():board = %d\n", board);
 
 		/* Search for the board context */
-		for (ptr = me4000_board_info_list.next;
-		     ptr != &me4000_board_info_list; ptr = ptr->next) {
-			board_info = list_entry(ptr, struct me4000_info, list);
+		list_for_each_entry(board_info, &me4000_board_info_list, list) {
 			if (board_info->board_count == board)
 				break;
 		}
 
-		if (ptr == &me4000_board_info_list) {
+		if (&board_info->list == &me4000_board_info_list) {
 			printk(KERN_ERR
 			       "ME4000:me4000_open():Board %d not in device list\n",
 			       board);
@@ -1483,14 +1474,12 @@ static int me4000_open(struct inode *inode_p, struct file *file_p)
 		PDEBUG("me4000_open():board = %d\n", board);
 
 		/* Search for the board context */
-		for (ptr = me4000_board_info_list.next;
-		     ptr != &me4000_board_info_list; ptr = ptr->next) {
-			board_info = list_entry(ptr, struct me4000_info, list);
+		list_for_each_entry(board_info, &me4000_board_info_list, list) {
 			if (board_info->board_count == board)
 				break;
 		}
 
-		if (ptr == &me4000_board_info_list) {
+		if (&board_info->list == &me4000_board_info_list) {
 			printk(KERN_ERR
 			       "ME4000:me4000_open():Board %d not in device list\n",
 			       board);
@@ -1526,14 +1515,12 @@ static int me4000_open(struct inode *inode_p, struct file *file_p)
 		PDEBUG("me4000_open():board = %d\n", board);
 
 		/* Search for the board context */
-		for (ptr = me4000_board_info_list.next;
-		     ptr != &me4000_board_info_list; ptr = ptr->next) {
-			board_info = list_entry(ptr, struct me4000_info, list);
+		list_for_each_entry(board_info, &me4000_board_info_list, list) {
 			if (board_info->board_count == board)
 				break;
 		}
 
-		if (ptr == &me4000_board_info_list) {
+		if (&board_info->list == &me4000_board_info_list) {
 			printk(KERN_ERR
 			       "ME4000:me4000_open():Board %d not in device list\n",
 			       board);
@@ -5955,7 +5942,6 @@ static irqreturn_t me4000_ext_int_isr(int irq, void *dev_id)
 
 static void __exit me4000_module_exit(void)
 {
-	struct list_head *board_p;
 	struct me4000_info *board_info;
 
 	CALL_PDEBUG("cleanup_module() is executed\n");
@@ -5975,9 +5961,7 @@ static void __exit me4000_module_exit(void)
 	pci_unregister_driver(&me4000_driver);
 
 	/* Reset the boards */
-	for (board_p = me4000_board_info_list.next;
-	     board_p != &me4000_board_info_list; board_p = board_p->next) {
-		board_info = list_entry(board_p, struct me4000_info, list);
+	list_for_each_entry(board_info, &me4000_board_info_list, list) {
 		me4000_reset_board(board_info);
 	}
 
@@ -5992,7 +5976,6 @@ static int me4000_read_procmem(char *buf, char **start, off_t offset, int count,
 	int len = 0;
 	int limit = count - 1000;
 	struct me4000_info *board_info;
-	struct list_head *ptr;
 
 	len += sprintf(buf + len, "\nME4000 DRIVER VERSION %X.%X.%X\n\n",
 		       (ME4000_DRIVER_VERSION & 0xFF0000) >> 16,
@@ -6000,11 +5983,7 @@ static int me4000_read_procmem(char *buf, char **start, off_t offset, int count,
 		       (ME4000_DRIVER_VERSION & 0xFF));
 
 	/* Search for the board context */
-	for (ptr = me4000_board_info_list.next;
-	     (ptr != &me4000_board_info_list) && (len < limit);
-	     ptr = ptr->next) {
-		board_info = list_entry(ptr, struct me4000_info, list);
-
+	list_for_each_entry(board_info, &me4000_board_info_list, list) {
 		len +=
 		    sprintf(buf + len, "Board number %d:\n",
 			    board_info->board_count);
@@ -6110,6 +6089,8 @@ static int me4000_read_procmem(char *buf, char **start, off_t offset, int count,
 		    sprintf(buf + len, "AO 3 status register = 0x%08X\n",
 			    inl(board_info->me4000_regbase +
 				ME4000_AO_03_STATUS_REG));
+		if (len >= limit)
+			break;
 	}
 
 	*eof = 1;
diff --git a/drivers/staging/meilhaus/Kconfig b/drivers/staging/meilhaus/Kconfig
new file mode 100644
index 00000000000..6def83fa2c9
--- /dev/null
+++ b/drivers/staging/meilhaus/Kconfig
@@ -0,0 +1,127 @@
+#
+# Meilhaus configuration
+#
+
+menuconfig MEILHAUS
+	tristate "Meilhaus support"
+	---help---
+	  If you have a Meilhaus card, say Y (or M) here.
+
+	  You need both this driver, and the driver for the particular
+	  data collection card.
+
+	  To compile this driver as a module, choose M here. The module will
+	  be called memain.
+
+if MEILHAUS
+
+config ME0600
+	tristate "Meilhaus ME-600 support"
+	default n
+	depends on PCI
+	help
+	  This driver supports the Meilhaus ME-600 family of boards
+	  that do data collection and multipurpose I/O.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called me0600.
+
+config ME0900
+	tristate "Meilhaus ME-900 support"
+	default n
+	depends on PCI
+	help
+	  This driver supports the Meilhaus ME-900 family of boards
+	  that do data collection and multipurpose I/O.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called me0900.
+
+config ME1000
+	tristate "Meilhaus ME-1000 support"
+	default n
+	depends on PCI
+	help
+	  This driver supports the Meilhaus ME-1000 family of boards
+	  that do data collection and multipurpose I/O.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called me1000.
+
+config ME1400
+	tristate "Meilhaus ME-1400 support"
+	default n
+	depends on PCI
+	help
+	  This driver supports the Meilhaus ME-1400 family of boards
+	  that do data collection and multipurpose I/O.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called me1400.
+
+config ME1600
+	tristate "Meilhaus ME-1600 support"
+	default n
+	depends on PCI
+	help
+	  This driver supports the Meilhaus ME-1600 family of boards
+	  that do data collection and multipurpose I/O.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called me1600.
+
+config ME4600
+	tristate "Meilhaus ME-4600 support"
+	default n
+	depends on PCI
+	help
+	  This driver supports the Meilhaus ME-4600 family of boards
+	  that do data collection and multipurpose I/O.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called me4600.
+
+config ME6000
+	tristate "Meilhaus ME-6000 support"
+	default n
+	depends on PCI
+	help
+	  This driver supports the Meilhaus ME-6000 family of boards
+	  that do data collection and multipurpose I/O.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called me6000.
+
+config ME8100
+	tristate "Meilhaus ME-8100 support"
+	default n
+	depends on PCI
+	help
+	  This driver supports the Meilhaus ME-8100 family of boards
+	  that do data collection and multipurpose I/O.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called me8100.
+
+config ME8200
+	tristate "Meilhaus ME-8200 support"
+	default n
+	depends on PCI
+	help
+	  This driver supports the Meilhaus ME-8200 family of boards
+	  that do data collection and multipurpose I/O.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called me8200.
+
+config MEDUMMY
+	tristate "Meilhaus dummy driver"
+	default n
+	depends on PCI
+	help
+	  This provides a dummy driver for the Meilhaus driver package
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called medummy.
+
+endif # MEILHAUS
diff --git a/drivers/staging/meilhaus/Makefile b/drivers/staging/meilhaus/Makefile
new file mode 100644
index 00000000000..5ab2c1c9c86
--- /dev/null
+++ b/drivers/staging/meilhaus/Makefile
@@ -0,0 +1,43 @@
+#
+# Makefile for Meilhaus linux driver system
+#
+
+obj-$(CONFIG_MEILHAUS) += memain.o
+obj-$(CONFIG_ME1600) += me1600.o
+obj-$(CONFIG_ME1000) += me1000.o
+obj-$(CONFIG_ME1400) += me1400.o
+obj-$(CONFIG_ME4600) += me4600.o
+obj-$(CONFIG_ME6000) += me6000.o
+obj-$(CONFIG_ME0600) += me0600.o
+obj-$(CONFIG_ME8100) += me8100.o
+obj-$(CONFIG_ME8200) += me8200.o
+obj-$(CONFIG_ME0900) += me0900.o
+obj-$(CONFIG_MEDUMMY) += medummy.o
+
+
+me1600-objs := medevice.o medlist.o medlock.o me1600_device.o
+me1600-objs += mesubdevice.o meslist.o meslock.o me1600_ao.o
+
+me1000-objs := medevice.o medlist.o medlock.o me1000_device.o
+me1000-objs += mesubdevice.o meslist.o meslock.o me1000_dio.o
+
+me1400-objs := medevice.o medlist.o medlock.o me1400_device.o
+me1400-objs += mesubdevice.o meslist.o meslock.o me8254.o me8255.o me1400_ext_irq.o
+
+me4600-objs := medevice.o medlist.o medlock.o mefirmware.o me4600_device.o
+me4600-objs += mesubdevice.o meslist.o meslock.o me4600_do.o me4600_di.o me4600_dio.o me8254.o me4600_ai.o me4600_ao.o me4600_ext_irq.o
+
+me6000-objs := medevice.o medlist.o medlock.o mefirmware.o me6000_device.o
+me6000-objs += mesubdevice.o meslist.o meslock.o me6000_dio.o me6000_ao.o
+
+me0600-objs := medevice.o medlist.o medlock.o me0600_device.o
+me0600-objs += mesubdevice.o meslist.o meslock.o me0600_relay.o me0600_ttli.o me0600_optoi.o me0600_dio.o me0600_ext_irq.o
+
+me8100-objs := medevice.o medlist.o medlock.o me8100_device.o
+me8100-objs += mesubdevice.o meslist.o meslock.o me8100_di.o me8100_do.o me8254.o
+
+me8200-objs := medevice.o medlist.o medlock.o me8200_device.o
+me8200-objs += mesubdevice.o meslist.o meslock.o me8200_di.o me8200_do.o me8200_dio.o
+
+me0900-objs := medevice.o medlist.o medlock.o me0900_device.o
+me0900-objs += mesubdevice.o meslist.o meslock.o me0900_do.o me0900_di.o
diff --git a/drivers/staging/meilhaus/TODO b/drivers/staging/meilhaus/TODO
new file mode 100644
index 00000000000..6ec25203089
--- /dev/null
+++ b/drivers/staging/meilhaus/TODO
@@ -0,0 +1,10 @@
+TODO:
+	- checkpatch.pl cleanups
+	- sparse issues
+	- Lindent
+	- audit userspace interface
+	- handle firmware properly
+	- possible comedi merge
+
+Please send cleanup patches to Greg Kroah-Hartman <greg@kroah.com>
+and CC: David Kiliani <mail@davidkiliani.de>
diff --git a/drivers/staging/meilhaus/me0600_device.c b/drivers/staging/meilhaus/me0600_device.c
new file mode 100644
index 00000000000..8950e47e0e8
--- /dev/null
+++ b/drivers/staging/meilhaus/me0600_device.c
@@ -0,0 +1,215 @@
+/**
+ * @file me0600_device.c
+ *
+ * @brief ME-630 device class implementation.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+#ifndef MODULE
+#  define MODULE
+#endif
+
+#include <linux/module.h>
+
+#include <linux/pci.h>
+#include <linux/slab.h>
+
+#include "meids.h"
+#include "meerror.h"
+#include "mecommon.h"
+#include "meinternal.h"
+
+#include "medebug.h"
+#include "medevice.h"
+#include "me0600_device.h"
+#include "mesubdevice.h"
+#include "me0600_relay.h"
+#include "me0600_ttli.h"
+#include "me0600_optoi.h"
+#include "me0600_dio.h"
+#include "me0600_ext_irq.h"
+
+me_device_t *me0600_pci_constructor(struct pci_dev *pci_device)
+{
+	me0600_device_t *me0600_device;
+	me_subdevice_t *subdevice;
+	unsigned int version_idx;
+	int err;
+	int i;
+
+	PDEBUG("executed.\n");
+
+	// Allocate structure for device instance.
+	me0600_device = kmalloc(sizeof(me0600_device_t), GFP_KERNEL);
+
+	if (!me0600_device) {
+		PERROR("Cannot get memory for device instance.\n");
+		return NULL;
+	}
+
+	memset(me0600_device, 0, sizeof(me0600_device_t));
+
+	// Initialize base class structure.
+	err = me_device_pci_init((me_device_t *) me0600_device, pci_device);
+
+	if (err) {
+		kfree(me0600_device);
+		PERROR("Cannot initialize device base class.\n");
+		return NULL;
+	}
+
+	/* Get the index in the device version information table. */
+	version_idx =
+	    me0600_versions_get_device_index(me0600_device->base.info.pci.
+					     device_id);
+
+	// Initialize spin lock .
+	spin_lock_init(&me0600_device->dio_ctrl_reg_lock);
+	spin_lock_init(&me0600_device->intcsr_lock);
+
+	// Create subdevice instances.
+
+	for (i = 0; i < me0600_versions[version_idx].optoi_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) me0600_optoi_constructor(me0600_device->
+								base.info.pci.
+								reg_bases[2]);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me0600_device);
+			kfree(me0600_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me0600_device->base.slist,
+					    subdevice);
+	}
+
+	for (i = 0; i < me0600_versions[version_idx].relay_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) me0600_relay_constructor(me0600_device->
+								base.info.pci.
+								reg_bases[2]);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me0600_device);
+			kfree(me0600_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me0600_device->base.slist,
+					    subdevice);
+	}
+
+	for (i = 0; i < me0600_versions[version_idx].ttli_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) me0600_ttli_constructor(me0600_device->
+							       base.info.pci.
+							       reg_bases[2]);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me0600_device);
+			kfree(me0600_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me0600_device->base.slist,
+					    subdevice);
+	}
+
+	for (i = 0; i < me0600_versions[version_idx].dio_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) me0600_dio_constructor(me0600_device->
+							      base.info.pci.
+							      reg_bases[2], i,
+							      &me0600_device->
+							      dio_ctrl_reg_lock);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me0600_device);
+			kfree(me0600_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me0600_device->base.slist,
+					    subdevice);
+	}
+
+	for (i = 0; i < me0600_versions[version_idx].ext_irq_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *)
+		    me0600_ext_irq_constructor(me0600_device->base.info.pci.
+					       reg_bases[1],
+					       me0600_device->base.info.pci.
+					       reg_bases[2],
+					       &me0600_device->intcsr_lock, i,
+					       me0600_device->base.irq);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me0600_device);
+			kfree(me0600_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me0600_device->base.slist,
+					    subdevice);
+	}
+
+	return (me_device_t *) me0600_device;
+}
+
+// Init and exit of module.
+
+static int __init me0600_init(void)
+{
+	PDEBUG("executed.\n");
+	return 0;
+}
+
+static void __exit me0600_exit(void)
+{
+	PDEBUG("executed.\n");
+}
+
+module_init(me0600_init);
+
+module_exit(me0600_exit);
+
+// Administrative stuff for modinfo.
+MODULE_AUTHOR
+    ("Guenter Gebhardt <g.gebhardt@meilhaus.de> & Krzysztof Gantzke <k.gantzke@meilhaus.de>");
+MODULE_DESCRIPTION("Device Driver Module for ME-6xx Device");
+MODULE_SUPPORTED_DEVICE("Meilhaus ME-6xx Devices");
+MODULE_LICENSE("GPL");
+
+// Export the constructor.
+EXPORT_SYMBOL(me0600_pci_constructor);
diff --git a/drivers/staging/meilhaus/me0600_device.h b/drivers/staging/meilhaus/me0600_device.h
new file mode 100644
index 00000000000..d93a8aee581
--- /dev/null
+++ b/drivers/staging/meilhaus/me0600_device.h
@@ -0,0 +1,97 @@
+/**
+ * @file me0600_device.h
+ *
+ * @brief ME-630 device class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME0600_DEVICE_H
+#define _ME0600_DEVICE_H
+
+#include <linux/pci.h>
+#include <linux/spinlock.h>
+
+#include "medevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief Structure holding ME-630 device capabilities.
+ */
+typedef struct me0600_version {
+	uint16_t device_id;
+	unsigned int relay_subdevices;
+	unsigned int ttli_subdevices;
+	unsigned int optoi_subdevices;
+	unsigned int dio_subdevices;
+	unsigned int ext_irq_subdevices;
+} me0600_version_t;
+
+/**
+ * @brief Device capabilities.
+ */
+static me0600_version_t me0600_versions[] = {
+	{PCI_DEVICE_ID_MEILHAUS_ME0630, 1, 1, 1, 2, 2},
+	{0},
+};
+
+#define ME0600_DEVICE_VERSIONS (sizeof(me0600_versions) / sizeof(me0600_version_t) - 1)	/**< Returns the number of entries in #me0600_versions. */
+
+/**
+ * @brief Returns the index of the device entry in #me0600_versions.
+ *
+ * @param device_id The PCI device id of the device to query.
+ * @return The index of the device in #me0600_versions.
+ */
+static inline unsigned int me0600_versions_get_device_index(uint16_t device_id)
+{
+	unsigned int i;
+	for (i = 0; i < ME0600_DEVICE_VERSIONS; i++)
+		if (me0600_versions[i].device_id == device_id)
+			break;
+	return i;
+}
+
+/**
+ * @brief The ME-630 device class structure.
+ */
+typedef struct me0600_device {
+	me_device_t base;			/**< The Meilhaus device base class. */
+
+	/* Child class attributes. */
+	spinlock_t dio_ctrl_reg_lock;
+	spinlock_t intcsr_lock;
+} me0600_device_t;
+
+/**
+ * @brief The ME-630 device class constructor.
+ *
+ * @param pci_device The pci device structure given by the PCI subsystem.
+ *
+ * @return On succes a new ME-630 device instance. \n
+ *         NULL on error.
+ */
+me_device_t *me0600_pci_constructor(struct pci_dev *pci_device)
+    __attribute__ ((weak));
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me0600_dio.c b/drivers/staging/meilhaus/me0600_dio.c
new file mode 100644
index 00000000000..3a2775749a2
--- /dev/null
+++ b/drivers/staging/meilhaus/me0600_dio.c
@@ -0,0 +1,415 @@
+/**
+ * @file me0600_dio.c
+ *
+ * @brief ME-630 digital input/output subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "me0600_dio_reg.h"
+#include "me0600_dio.h"
+
+/*
+ * Defines
+ */
+
+/*
+ * Functions
+ */
+
+static int me0600_dio_io_reset_subdevice(struct me_subdevice *subdevice,
+					 struct file *filep, int flags)
+{
+	me0600_dio_subdevice_t *instance;
+	uint8_t mode;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0600_dio_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	mode = inb(instance->ctrl_reg);
+	mode &= ~(0x3 << (instance->dio_idx * 2));
+	outb(mode, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, mode);
+	spin_unlock(instance->ctrl_reg_lock);
+
+	outb(0x00, instance->port_reg);
+	PDEBUG_REG("port_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->port_reg - instance->reg_base, 0x00);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0600_dio_io_single_config(me_subdevice_t * subdevice,
+				       struct file *filep,
+				       int channel,
+				       int single_config,
+				       int ref,
+				       int trig_chan,
+				       int trig_type, int trig_edge, int flags)
+{
+	me0600_dio_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint8_t mode;
+	int size =
+	    flags & (ME_IO_SINGLE_CONFIG_DIO_BIT | ME_IO_SINGLE_CONFIG_DIO_BYTE
+		     | ME_IO_SINGLE_CONFIG_DIO_WORD |
+		     ME_IO_SINGLE_CONFIG_DIO_DWORD);
+
+	PDEBUG("executed.\n");
+
+	instance = (me0600_dio_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	mode = inb(instance->ctrl_reg);
+	switch (size) {
+	case ME_IO_SINGLE_CONFIG_NO_FLAGS:
+	case ME_IO_SINGLE_CONFIG_DIO_BYTE:
+		if (channel == 0) {
+			if (single_config == ME_SINGLE_CONFIG_DIO_INPUT) {
+				mode &=
+				    ~((ME0600_DIO_CONFIG_BIT_OUT_0) <<
+				      (instance->dio_idx * 2));
+			} else if (single_config == ME_SINGLE_CONFIG_DIO_OUTPUT) {
+				mode &=
+				    ~((ME0600_DIO_CONFIG_BIT_OUT_0) <<
+				      (instance->dio_idx * 2));
+				mode |=
+				    ME0600_DIO_CONFIG_BIT_OUT_0 << (instance->
+								    dio_idx *
+								    2);
+			} else {
+				PERROR
+				    ("Invalid port configuration specified.\n");
+				err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		} else {
+			PERROR("Invalid channel number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (!err) {
+		outb(mode, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, mode);
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me0600_dio_io_single_read(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int channel,
+				     int *value, int time_out, int flags)
+{
+	me0600_dio_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint8_t mode;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0600_dio_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			mode =
+			    inb(instance->
+				ctrl_reg) & ((ME0600_DIO_CONFIG_BIT_OUT_0) <<
+					     (instance->dio_idx * 2));
+
+			if ((mode ==
+			     (ME0600_DIO_CONFIG_BIT_OUT_0 <<
+			      (instance->dio_idx * 2))) || !mode) {
+				*value =
+				    inb(instance->
+					port_reg) & (0x0001 << channel);
+			} else {
+				PERROR("Port not in output or input mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			mode =
+			    inb(instance->
+				ctrl_reg) & ((ME0600_DIO_CONFIG_BIT_OUT_0) <<
+					     (instance->dio_idx * 2));
+
+			if ((mode ==
+			     (ME0600_DIO_CONFIG_BIT_OUT_0 <<
+			      (instance->dio_idx * 2))) || !mode) {
+				*value = inb(instance->port_reg) & 0x00FF;
+			} else {
+				PERROR("Port not in output or input mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+
+		err = ME_ERRNO_INVALID_FLAGS;
+
+		break;
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me0600_dio_io_single_write(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int value, int time_out, int flags)
+{
+	me0600_dio_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint8_t mode;
+	uint8_t byte;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0600_dio_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	switch (flags) {
+
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			mode =
+			    inb(instance->
+				ctrl_reg) & ((ME0600_DIO_CONFIG_BIT_OUT_0) <<
+					     (instance->dio_idx * 2));
+
+			if (mode ==
+			    (ME0600_DIO_CONFIG_BIT_OUT_0 <<
+			     (instance->dio_idx * 2))) {
+				byte = inb(instance->port_reg);
+
+				if (value)
+					byte |= 0x1 << channel;
+				else
+					byte &= ~(0x1 << channel);
+
+				outb(byte, instance->port_reg);
+			} else {
+				PERROR("Port not in output or input mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			mode =
+			    inb(instance->
+				ctrl_reg) & ((ME0600_DIO_CONFIG_BIT_OUT_0) <<
+					     (instance->dio_idx * 2));
+
+			if (mode ==
+			    (ME0600_DIO_CONFIG_BIT_OUT_0 <<
+			     (instance->dio_idx * 2))) {
+				outb(value, instance->port_reg);
+			} else {
+				PERROR("Port not in output or input mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+
+		err = ME_ERRNO_INVALID_FLAGS;
+
+		break;
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me0600_dio_query_number_channels(me_subdevice_t * subdevice,
+					    int *number)
+{
+	PDEBUG("executed.\n");
+	*number = 8;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0600_dio_query_subdevice_type(me_subdevice_t * subdevice,
+					   int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_DIO;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0600_dio_query_subdevice_caps(me_subdevice_t * subdevice,
+					   int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = ME_CAPS_DIO_DIR_BYTE;
+	return ME_ERRNO_SUCCESS;
+}
+
+me0600_dio_subdevice_t *me0600_dio_constructor(uint32_t reg_base,
+					       unsigned int dio_idx,
+					       spinlock_t * ctrl_reg_lock)
+{
+	me0600_dio_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me0600_dio_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me0600_dio_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	subdevice->ctrl_reg_lock = ctrl_reg_lock;
+
+	/* Save digital i/o index */
+	subdevice->dio_idx = dio_idx;
+
+	/* Save the subdevice index */
+	subdevice->ctrl_reg = reg_base + ME0600_DIO_CONFIG_REG;
+	subdevice->port_reg = reg_base + ME0600_DIO_PORT_REG + dio_idx;
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = reg_base;
+#endif
+
+	/* Overload base class methods. */
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me0600_dio_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me0600_dio_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me0600_dio_io_single_read;
+	subdevice->base.me_subdevice_io_single_write =
+	    me0600_dio_io_single_write;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me0600_dio_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me0600_dio_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me0600_dio_query_subdevice_caps;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me0600_dio.h b/drivers/staging/meilhaus/me0600_dio.h
new file mode 100644
index 00000000000..5d075c7d688
--- /dev/null
+++ b/drivers/staging/meilhaus/me0600_dio.h
@@ -0,0 +1,68 @@
+/**
+ * @file me0600_dio.h
+ *
+ * @brief ME-630 digital input/output subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME0600_DIO_H_
+#define _ME0600_DIO_H_
+
+#include "mesubdevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The template subdevice class.
+ */
+typedef struct me0600_dio_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *ctrl_reg_lock;		/**< Spin lock to protect #ctrl_reg from concurrent access. */
+	unsigned int dio_idx;			/**< The index of the digital i/o on the device. */
+
+	unsigned long port_reg;			/**< Register holding the port status. */
+	unsigned long ctrl_reg;			/**< Register to configure the port direction. */
+#ifdef MEDEBUG_DEBUG_REG
+	unsigned long reg_base;
+#endif
+} me0600_dio_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-630 digital input/ouput subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ * @param dio_idx The index of the digital i/o port on the device.
+ * @param ctrl_reg_lock Spin lock protecting the control register.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me0600_dio_subdevice_t *me0600_dio_constructor(uint32_t reg_base,
+					       unsigned int dio_idx,
+					       spinlock_t * ctrl_reg_lock);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me0600_dio_reg.h b/drivers/staging/meilhaus/me0600_dio_reg.h
new file mode 100644
index 00000000000..f116ea3b79d
--- /dev/null
+++ b/drivers/staging/meilhaus/me0600_dio_reg.h
@@ -0,0 +1,41 @@
+/**
+ * @file me0600_dio_reg.h
+ *
+ * @brief ME-630 digital input/output subdevice register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME0600_DIO_REG_H_
+#define _ME0600_DIO_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME0600_DIO_CONFIG_REG		0x0007
+#define ME0600_DIO_PORT_0_REG		0x0008
+#define ME0600_DIO_PORT_1_REG		0x0009
+#define ME0600_DIO_PORT_REG			ME0600_DIO_PORT_0_REG
+
+#define ME0600_DIO_CONFIG_BIT_OUT_0	0x0001
+#define ME0600_DIO_CONFIG_BIT_OUT_1	0x0004
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me0600_ext_irq.c b/drivers/staging/meilhaus/me0600_ext_irq.c
new file mode 100644
index 00000000000..a449ab20094
--- /dev/null
+++ b/drivers/staging/meilhaus/me0600_ext_irq.c
@@ -0,0 +1,478 @@
+/**
+ * @file me0600_ext_irq.c
+ *
+ * @brief ME-630 external interrupt subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/version.h>
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+#include <linux/interrupt.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+#include "meids.h"
+#include "medebug.h"
+
+#include "meplx_reg.h"
+#include "me0600_ext_irq_reg.h"
+#include "me0600_ext_irq.h"
+
+/*
+ * Functions
+ */
+
+static int me0600_ext_irq_io_irq_start(struct me_subdevice *subdevice,
+				       struct file *filep,
+				       int channel,
+				       int irq_source,
+				       int irq_edge, int irq_arg, int flags)
+{
+	me0600_ext_irq_subdevice_t *instance;
+	uint32_t tmp;
+	unsigned long cpu_flags;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0600_ext_irq_subdevice_t *) subdevice;
+
+	if (flags & ~ME_IO_IRQ_START_DIO_BIT) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (instance->lintno > 1) {
+		PERROR("Wrong idx=%d.\n", instance->lintno);
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	if (irq_source != ME_IRQ_SOURCE_DIO_LINE) {
+		PERROR("Invalid irq source specified.\n");
+		return ME_ERRNO_INVALID_IRQ_SOURCE;
+	}
+
+	if (irq_edge != ME_IRQ_EDGE_RISING) {
+		PERROR("Invalid irq edge specified.\n");
+		return ME_ERRNO_INVALID_IRQ_EDGE;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	spin_lock(instance->intcsr_lock);
+	tmp = inl(instance->intcsr);
+	switch (instance->lintno) {
+	case 0:
+		tmp |=
+		    PLX_INTCSR_LOCAL_INT1_EN | PLX_INTCSR_LOCAL_INT1_POL |
+		    PLX_INTCSR_PCI_INT_EN;
+		break;
+	case 1:
+		tmp |=
+		    PLX_INTCSR_LOCAL_INT2_EN | PLX_INTCSR_LOCAL_INT2_POL |
+		    PLX_INTCSR_PCI_INT_EN;
+		break;
+	}
+	outl(tmp, instance->intcsr);
+	PDEBUG_REG("intcsr outl(plx:0x%X)=0x%x\n", instance->intcsr, tmp);
+	spin_unlock(instance->intcsr_lock);
+	instance->rised = 0;
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0600_ext_irq_io_irq_wait(struct me_subdevice *subdevice,
+				      struct file *filep,
+				      int channel,
+				      int *irq_count,
+				      int *value, int time_out, int flags)
+{
+	me0600_ext_irq_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	long t = 0;
+	unsigned long cpu_flags;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0600_ext_irq_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid time_out specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	if (time_out) {
+		t = (time_out * HZ) / 1000;
+
+		if (t == 0)
+			t = 1;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	if (instance->rised <= 0) {
+		instance->rised = 0;
+
+		if (time_out) {
+			t = wait_event_interruptible_timeout(instance->
+							     wait_queue,
+							     (instance->rised !=
+							      0), t);
+
+			if (t == 0) {
+				PERROR("Wait on interrupt timed out.\n");
+				err = ME_ERRNO_TIMEOUT;
+			}
+		} else {
+			wait_event_interruptible(instance->wait_queue,
+						 (instance->rised != 0));
+		}
+
+		if (instance->rised < 0) {
+			PERROR("Wait on interrupt aborted by user.\n");
+			err = ME_ERRNO_CANCELLED;
+		}
+	}
+
+	if (signal_pending(current)) {
+		PERROR("Wait on interrupt aborted by signal.\n");
+		err = ME_ERRNO_SIGNAL;
+	}
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	instance->rised = 0;
+	*irq_count = instance->n;
+	*value = 1;
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me0600_ext_irq_io_irq_stop(struct me_subdevice *subdevice,
+				      struct file *filep,
+				      int channel, int flags)
+{
+	me0600_ext_irq_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint32_t tmp;
+	unsigned long cpu_flags;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0600_ext_irq_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (instance->lintno > 1) {
+		PERROR("Wrong idx=%d.\n", instance->lintno);
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	spin_lock(instance->intcsr_lock);
+	tmp = inl(instance->intcsr);
+	switch (instance->lintno) {
+	case 0:
+		tmp &= ~PLX_INTCSR_LOCAL_INT1_EN;
+		break;
+	case 1:
+		tmp &= ~PLX_INTCSR_LOCAL_INT2_EN;
+		break;
+	}
+	outl(tmp, instance->intcsr);
+	PDEBUG_REG("intcsr outl(plx:0x%X)=0x%x\n", instance->intcsr, tmp);
+	spin_unlock(instance->intcsr_lock);
+	instance->rised = -1;
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me0600_ext_irq_io_reset_subdevice(struct me_subdevice *subdevice,
+					     struct file *filep, int flags)
+{
+	me0600_ext_irq_subdevice_t *instance;
+	uint32_t tmp;
+	unsigned long cpu_flags;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0600_ext_irq_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	spin_lock(instance->intcsr_lock);
+	tmp = inl(instance->intcsr);
+	switch (instance->lintno) {
+	case 0:
+		tmp |= PLX_INTCSR_LOCAL_INT1_POL | PLX_INTCSR_PCI_INT_EN;
+		tmp &= ~PLX_INTCSR_LOCAL_INT1_EN;
+		break;
+	case 1:
+		tmp |= PLX_INTCSR_LOCAL_INT2_POL | PLX_INTCSR_PCI_INT_EN;
+		tmp &= ~PLX_INTCSR_LOCAL_INT2_EN;
+		break;
+	}
+	outl(tmp, instance->intcsr);
+	PDEBUG_REG("intcsr outl(plx:0x%X)=0x%x\n", instance->intcsr, tmp);
+	spin_unlock(instance->intcsr_lock);
+
+	instance->rised = -1;
+	instance->n = 0;
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0600_ext_irq_query_number_channels(struct me_subdevice *subdevice,
+						int *number)
+{
+	PDEBUG("executed.\n");
+	*number = 1;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0600_ext_irq_query_subdevice_type(struct me_subdevice *subdevice,
+					       int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_EXT_IRQ;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0600_ext_irq_query_subdevice_caps(struct me_subdevice *subdevice,
+					       int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = ME_CAPS_EXT_IRQ_EDGE_RISING;
+	return ME_ERRNO_SUCCESS;
+}
+
+static void me0600_ext_irq_destructor(struct me_subdevice *subdevice)
+{
+	me0600_ext_irq_subdevice_t *instance;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0600_ext_irq_subdevice_t *) subdevice;
+
+	free_irq(instance->irq, (void *)instance);
+	me_subdevice_deinit(&instance->base);
+	kfree(instance);
+}
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 19)
+static irqreturn_t me0600_isr(int irq, void *dev_id)
+#else
+static irqreturn_t me0600_isr(int irq, void *dev_id, struct pt_regs *regs)
+#endif
+{
+	me0600_ext_irq_subdevice_t *instance;
+	uint32_t status;
+	uint32_t mask = PLX_INTCSR_PCI_INT_EN;
+	irqreturn_t ret = IRQ_HANDLED;
+
+	instance = (me0600_ext_irq_subdevice_t *) dev_id;
+
+	if (irq != instance->irq) {
+		PERROR("Incorrect interrupt num: %d.\n", irq);
+		return IRQ_NONE;
+	}
+
+	PDEBUG("executed.\n");
+
+	if (instance->lintno > 1) {
+		PERROR_CRITICAL
+		    ("%s():Wrong subdevice index=%d plx:irq_status_reg=0x%04X.\n",
+		     __FUNCTION__, instance->lintno, inl(instance->intcsr));
+		return IRQ_NONE;
+	}
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->intcsr_lock);
+	status = inl(instance->intcsr);
+	switch (instance->lintno) {
+	case 0:
+		mask |= PLX_INTCSR_LOCAL_INT1_STATE | PLX_INTCSR_LOCAL_INT1_EN;
+		break;
+	case 1:
+		mask |= PLX_INTCSR_LOCAL_INT2_STATE | PLX_INTCSR_LOCAL_INT2_EN;
+		break;
+	}
+
+	if ((status & mask) == mask) {
+		instance->rised = 1;
+		instance->n++;
+		inb(instance->reset_reg);
+		PDEBUG("Interrupt detected.\n");
+	} else {
+		PINFO
+		    ("%ld Shared interrupt. %s(): idx=0 plx:irq_status_reg=0x%04X\n",
+		     jiffies, __FUNCTION__, status);
+		ret = IRQ_NONE;
+	}
+	spin_unlock(instance->intcsr_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	return ret;
+}
+
+me0600_ext_irq_subdevice_t *me0600_ext_irq_constructor(uint32_t plx_reg_base,
+						       uint32_t me0600_reg_base,
+						       spinlock_t * intcsr_lock,
+						       unsigned ext_irq_idx,
+						       int irq)
+{
+	me0600_ext_irq_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me0600_ext_irq_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for 630_ext_irq instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me0600_ext_irq_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	subdevice->intcsr_lock = intcsr_lock;
+
+	/* Initialize wait queue */
+	init_waitqueue_head(&subdevice->wait_queue);
+
+	subdevice->lintno = ext_irq_idx;
+
+	/* Request interrupt line */
+	subdevice->irq = irq;
+
+	err = request_irq(subdevice->irq, me0600_isr,
+#ifdef IRQF_DISABLED
+			  IRQF_DISABLED | IRQF_SHARED,
+#else
+			  SA_INTERRUPT | SA_SHIRQ,
+#endif
+			  ME0600_NAME, (void *)subdevice);
+
+	if (err) {
+		PERROR("Cannot get interrupt line.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	PINFO("Registered irq=%d.\n", subdevice->irq);
+
+	/* Initialize registers */
+	subdevice->intcsr = plx_reg_base + PLX_INTCSR;
+	subdevice->reset_reg =
+	    me0600_reg_base + ME0600_INT_0_RESET_REG + ext_irq_idx;
+
+	/* Initialize the subdevice methods */
+	subdevice->base.me_subdevice_io_irq_start = me0600_ext_irq_io_irq_start;
+	subdevice->base.me_subdevice_io_irq_wait = me0600_ext_irq_io_irq_wait;
+	subdevice->base.me_subdevice_io_irq_stop = me0600_ext_irq_io_irq_stop;
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me0600_ext_irq_io_reset_subdevice;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me0600_ext_irq_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me0600_ext_irq_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me0600_ext_irq_query_subdevice_caps;
+	subdevice->base.me_subdevice_destructor = me0600_ext_irq_destructor;
+
+	subdevice->rised = 0;
+	subdevice->n = 0;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me0600_ext_irq.h b/drivers/staging/meilhaus/me0600_ext_irq.h
new file mode 100644
index 00000000000..f5f2204b49a
--- /dev/null
+++ b/drivers/staging/meilhaus/me0600_ext_irq.h
@@ -0,0 +1,58 @@
+/**
+ * @file me0600_ext_irq.h
+ *
+ * @brief ME-630 external interrupt implementation.
+ * @note Copyright (C) 2006 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+#ifndef _ME0600_EXT_IRQ_H_
+#define _ME0600_EXT_IRQ_H_
+
+#include <linux/sched.h>
+
+#include "mesubdevice.h"
+#include "meslock.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The ME-630 external interrupt subdevice class.
+ */
+typedef struct me0600_ext_irq_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *intcsr_lock;		/**< Spin lock to protect #intcsr. */
+
+	wait_queue_head_t wait_queue;		/**< Queue to put on threads waiting for an interrupt. */
+
+	int irq;				/**< The irq number assigned by PCI BIOS. */
+	int rised;				/**< If true an interrupt has occured. */
+	unsigned int n;				/**< The number of interrupt since the driver was loaded. */
+	unsigned int lintno;			/**< The number of the local PCI interrupt. */
+
+	uint32_t intcsr;			/**< The PLX interrupt control and status register. */
+	uint32_t reset_reg;			/**< The control register. */
+} me0600_ext_irq_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-630 external interrupt instance.
+ *
+ * @param plx_reg_base The register base address of the PLX chip as returned by the PCI BIOS.
+ * @param me0600_reg_base The register base address of the ME-630 device as returned by the PCI BIOS.
+ * @param irq The irq assigned by the PCI BIOS.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me0600_ext_irq_subdevice_t *me0600_ext_irq_constructor(uint32_t plx_reg_base,
+						       uint32_t me0600_reg_base,
+						       spinlock_t * intcsr_lock,
+						       unsigned int ext_irq_idx,
+						       int irq);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me0600_ext_irq_reg.h b/drivers/staging/meilhaus/me0600_ext_irq_reg.h
new file mode 100644
index 00000000000..f6198fa6d2b
--- /dev/null
+++ b/drivers/staging/meilhaus/me0600_ext_irq_reg.h
@@ -0,0 +1,18 @@
+/**
+ * @file me0600_ext_irq_reg.h
+ *
+ * @brief ME-630 external interrupt register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+#ifndef _ME0600_EXT_IRQ_REG_H_
+#define _ME0600_EXT_IRQ_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME0600_INT_0_RESET_REG     0x0005
+#define ME0600_INT_1_RESET_REG     0x0006
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me0600_optoi.c b/drivers/staging/meilhaus/me0600_optoi.c
new file mode 100644
index 00000000000..b6d977f228c
--- /dev/null
+++ b/drivers/staging/meilhaus/me0600_optoi.c
@@ -0,0 +1,243 @@
+/**
+ * @file me0600_optoi.c
+ *
+ * @brief ME-630 Optoisolated input subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "me0600_optoi_reg.h"
+#include "me0600_optoi.h"
+
+/*
+ * Defines
+ */
+
+/*
+ * Functions
+ */
+
+static int me0600_optoi_io_reset_subdevice(struct me_subdevice *subdevice,
+					   struct file *filep, int flags)
+{
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	PDEBUG("executed.\n");
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0600_optoi_io_single_config(me_subdevice_t * subdevice,
+					 struct file *filep,
+					 int channel,
+					 int single_config,
+					 int ref,
+					 int trig_chan,
+					 int trig_type,
+					 int trig_edge, int flags)
+{
+	me0600_optoi_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0600_optoi_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+
+	switch (flags) {
+	case ME_IO_SINGLE_CONFIG_NO_FLAGS:
+	case ME_IO_SINGLE_CONFIG_DIO_BYTE:
+		if (channel == 0) {
+			if (single_config != ME_SINGLE_CONFIG_DIO_INPUT) {
+				PERROR("Invalid port direction specified.\n");
+				err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		} else {
+			PERROR("Invalid channel specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+
+		err = ME_ERRNO_INVALID_FLAGS;
+
+		break;
+	}
+
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me0600_optoi_io_single_read(me_subdevice_t * subdevice,
+				       struct file *filep,
+				       int channel,
+				       int *value, int time_out, int flags)
+{
+	me0600_optoi_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0600_optoi_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			*value = inb(instance->port_reg) & (0x1 << channel);
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			*value = inb(instance->port_reg);
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me0600_optoi_query_number_channels(me_subdevice_t * subdevice,
+					      int *number)
+{
+	PDEBUG("executed.\n");
+	*number = 8;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0600_optoi_query_subdevice_type(me_subdevice_t * subdevice,
+					     int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_DI;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0600_optoi_query_subdevice_caps(me_subdevice_t * subdevice,
+					     int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = 0;
+	return ME_ERRNO_SUCCESS;
+}
+
+me0600_optoi_subdevice_t *me0600_optoi_constructor(uint32_t reg_base)
+{
+	me0600_optoi_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me0600_optoi_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me0600_optoi_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	/* Save the subdevice index */
+	subdevice->port_reg = reg_base + ME0600_OPTO_INPUT_REG;
+
+	/* Overload base class methods. */
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me0600_optoi_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me0600_optoi_io_single_config;
+	subdevice->base.me_subdevice_io_single_read =
+	    me0600_optoi_io_single_read;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me0600_optoi_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me0600_optoi_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me0600_optoi_query_subdevice_caps;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me0600_optoi.h b/drivers/staging/meilhaus/me0600_optoi.h
new file mode 100644
index 00000000000..e7e69bcde9c
--- /dev/null
+++ b/drivers/staging/meilhaus/me0600_optoi.h
@@ -0,0 +1,58 @@
+/**
+ * @file me0600_optoi.h
+ *
+ * @brief ME-630 Optoisolated input subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME0600_OPTOI_H_
+#define _ME0600_OPTOI_H_
+
+#include "mesubdevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The template subdevice class.
+ */
+typedef struct me0600_optoi_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+
+	uint32_t port_reg;			/**< Register holding the port status. */
+} me0600_optoi_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-630 Optoisolated input subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me0600_optoi_subdevice_t *me0600_optoi_constructor(uint32_t reg_base);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me0600_optoi_reg.h b/drivers/staging/meilhaus/me0600_optoi_reg.h
new file mode 100644
index 00000000000..e0bc4505400
--- /dev/null
+++ b/drivers/staging/meilhaus/me0600_optoi_reg.h
@@ -0,0 +1,35 @@
+/**
+ * @file me0600_optoi_reg.h
+ *
+ * @brief ME-630 Optoisolated input subdevice register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME0600_OPTOI_REG_H_
+#define _ME0600_OPTOI_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME0600_OPTO_INPUT_REG      0x0004
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me0600_relay.c b/drivers/staging/meilhaus/me0600_relay.c
new file mode 100644
index 00000000000..2665c69addd
--- /dev/null
+++ b/drivers/staging/meilhaus/me0600_relay.c
@@ -0,0 +1,359 @@
+/**
+ * @file me0600_relay.c
+ *
+ * @brief ME-630 relay subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "me0600_relay_reg.h"
+#include "me0600_relay.h"
+
+/*
+ * Defines
+ */
+
+/*
+ * Functions
+ */
+
+static int me0600_relay_io_reset_subdevice(struct me_subdevice *subdevice,
+					   struct file *filep, int flags)
+{
+	me0600_relay_subdevice_t *instance;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0600_relay_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	outb(0x0, instance->port_0_reg);
+	PDEBUG_REG("port_0_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->port_0_reg - instance->reg_base, 0);
+	outb(0x0, instance->port_1_reg);
+	PDEBUG_REG("port_1_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->port_1_reg - instance->reg_base, 0);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0600_relay_io_single_config(me_subdevice_t * subdevice,
+					 struct file *filep,
+					 int channel,
+					 int single_config,
+					 int ref,
+					 int trig_chan,
+					 int trig_type,
+					 int trig_edge, int flags)
+{
+	me0600_relay_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0600_relay_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+
+	switch (flags) {
+	case ME_IO_SINGLE_CONFIG_NO_FLAGS:
+	case ME_IO_SINGLE_CONFIG_DIO_WORD:
+		if (channel == 0) {
+			if (single_config != ME_SINGLE_CONFIG_DIO_OUTPUT) {
+				PERROR("Invalid word direction specified.\n");
+				err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		} else {
+			PERROR("Invalid channel specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+
+		err = ME_ERRNO_INVALID_FLAGS;
+
+		break;
+	}
+
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me0600_relay_io_single_read(me_subdevice_t * subdevice,
+				       struct file *filep,
+				       int channel,
+				       int *value, int time_out, int flags)
+{
+	me0600_relay_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0600_relay_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+
+	switch (flags) {
+
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			*value = inb(instance->port_0_reg) & (0x1 << channel);
+		} else if ((channel >= 8) && (channel < 16)) {
+			*value =
+			    inb(instance->port_1_reg) & (0x1 << (channel - 8));
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+
+		break;
+
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			*value = inb(instance->port_0_reg);
+		} else if (channel == 1) {
+			*value = inb(instance->port_1_reg);
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_WORD:
+		if (channel == 0) {
+			*value = (uint32_t) inb(instance->port_1_reg) << 8;
+			*value |= inb(instance->port_0_reg);
+		} else {
+			PERROR("Invalid word number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me0600_relay_io_single_write(me_subdevice_t * subdevice,
+					struct file *filep,
+					int channel,
+					int value, int time_out, int flags)
+{
+	me0600_relay_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint8_t state;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0600_relay_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			state = inb(instance->port_0_reg);
+			state =
+			    value ? (state | (0x1 << channel)) : (state &
+								  ~(0x1 <<
+								    channel));
+			outb(state, instance->port_0_reg);
+		} else if ((channel >= 8) && (channel < 16)) {
+			state = inb(instance->port_1_reg);
+			state =
+			    value ? (state | (0x1 << (channel - 8))) : (state &
+									~(0x1 <<
+									  (channel
+									   -
+									   8)));
+			outb(state, instance->port_1_reg);
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			outb(value, instance->port_0_reg);
+		} else if (channel == 1) {
+			outb(value, instance->port_1_reg);
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_WORD:
+		if (channel == 0) {
+			outb(value, instance->port_0_reg);
+			outb(value >> 8, instance->port_1_reg);
+		} else {
+			PERROR("Invalid word number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+		break;
+	}
+
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me0600_relay_query_number_channels(me_subdevice_t * subdevice,
+					      int *number)
+{
+	PDEBUG("executed.\n");
+	*number = 16;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0600_relay_query_subdevice_type(me_subdevice_t * subdevice,
+					     int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_DO;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0600_relay_query_subdevice_caps(me_subdevice_t * subdevice,
+					     int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = 0;
+	return ME_ERRNO_SUCCESS;
+}
+
+me0600_relay_subdevice_t *me0600_relay_constructor(uint32_t reg_base)
+{
+	me0600_relay_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me0600_relay_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me0600_relay_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	/* Save the subdevice index */
+	subdevice->port_0_reg = reg_base + ME0600_RELAIS_0_REG;
+	subdevice->port_1_reg = reg_base + ME0600_RELAIS_1_REG;
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = reg_base;
+#endif
+
+	/* Overload base class methods. */
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me0600_relay_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me0600_relay_io_single_config;
+	subdevice->base.me_subdevice_io_single_read =
+	    me0600_relay_io_single_read;
+	subdevice->base.me_subdevice_io_single_write =
+	    me0600_relay_io_single_write;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me0600_relay_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me0600_relay_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me0600_relay_query_subdevice_caps;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me0600_relay.h b/drivers/staging/meilhaus/me0600_relay.h
new file mode 100644
index 00000000000..2ce7dcab8b3
--- /dev/null
+++ b/drivers/staging/meilhaus/me0600_relay.h
@@ -0,0 +1,63 @@
+/**
+ * @file me0600_relay.h
+ *
+ * @brief ME-630 relay subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME0600_RELAY_H_
+#define _ME0600_RELAY_H_
+
+#include "mesubdevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The template subdevice class.
+ */
+typedef struct me0600_relay_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+
+	unsigned long port_0_reg;			/**< Register holding the port status. */
+	unsigned long port_1_reg;			/**< Register holding the port status. */
+#ifdef MEDEBUG_DEBUG_REG
+	unsigned long reg_base;
+#endif
+} me0600_relay_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-630 relay subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ * @param ctrl_reg_lock Spin lock protecting the control register.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me0600_relay_subdevice_t *me0600_relay_constructor(uint32_t reg_base);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me0600_relay_reg.h b/drivers/staging/meilhaus/me0600_relay_reg.h
new file mode 100644
index 00000000000..ba4db2e223c
--- /dev/null
+++ b/drivers/staging/meilhaus/me0600_relay_reg.h
@@ -0,0 +1,36 @@
+/**
+ * @file me0600_relay_reg.h
+ *
+ * @brief ME-630 relay subdevice register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME0600_RELAY_REG_H_
+#define _ME0600_RELAY_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME0600_RELAIS_0_REG        0x0001
+#define ME0600_RELAIS_1_REG        0x0002
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me0600_ttli.c b/drivers/staging/meilhaus/me0600_ttli.c
new file mode 100644
index 00000000000..ab8e13b6f32
--- /dev/null
+++ b/drivers/staging/meilhaus/me0600_ttli.c
@@ -0,0 +1,238 @@
+/**
+ * @file me0600_ttli.c
+ *
+ * @brief ME-630 TTL input subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "me0600_ttli_reg.h"
+#include "me0600_ttli.h"
+
+/*
+ * Defines
+ */
+
+/*
+ * Functions
+ */
+
+static int me0600_ttli_io_reset_subdevice(struct me_subdevice *subdevice,
+					  struct file *filep, int flags)
+{
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	PDEBUG("executed.\n");
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0600_ttli_io_single_config(me_subdevice_t * subdevice,
+					struct file *filep,
+					int channel,
+					int single_config,
+					int ref,
+					int trig_chan,
+					int trig_type, int trig_edge, int flags)
+{
+	me0600_ttli_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0600_ttli_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+
+	switch (flags) {
+	case ME_IO_SINGLE_CONFIG_NO_FLAGS:
+	case ME_IO_SINGLE_CONFIG_DIO_BYTE:
+		if (channel == 0) {
+			if (single_config != ME_SINGLE_CONFIG_DIO_INPUT) {
+				PERROR("Invalid port direction specified.\n");
+				err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		} else {
+			PERROR("Invalid channel specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+
+		err = ME_ERRNO_INVALID_FLAGS;
+
+		break;
+	}
+
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me0600_ttli_io_single_read(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int *value, int time_out, int flags)
+{
+	me0600_ttli_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0600_ttli_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			*value = inb(instance->port_reg) & (0x1 << channel);
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			*value = inb(instance->port_reg);
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me0600_ttli_query_number_channels(me_subdevice_t * subdevice,
+					     int *number)
+{
+	PDEBUG("executed.\n");
+	*number = 8;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0600_ttli_query_subdevice_type(me_subdevice_t * subdevice,
+					    int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_DI;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0600_ttli_query_subdevice_caps(me_subdevice_t * subdevice,
+					    int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = 0;
+	return ME_ERRNO_SUCCESS;
+}
+
+me0600_ttli_subdevice_t *me0600_ttli_constructor(uint32_t reg_base)
+{
+	me0600_ttli_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me0600_ttli_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me0600_ttli_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	/* Save the subdevice index */
+	subdevice->port_reg = reg_base + ME0600_TTL_INPUT_REG;
+
+	/* Overload base class methods. */
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me0600_ttli_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me0600_ttli_io_single_config;
+	subdevice->base.me_subdevice_io_single_read =
+	    me0600_ttli_io_single_read;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me0600_ttli_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me0600_ttli_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me0600_ttli_query_subdevice_caps;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me0600_ttli.h b/drivers/staging/meilhaus/me0600_ttli.h
new file mode 100644
index 00000000000..6c903961486
--- /dev/null
+++ b/drivers/staging/meilhaus/me0600_ttli.h
@@ -0,0 +1,58 @@
+/**
+ * @file me0600_ttli.h
+ *
+ * @brief ME-630 TTL input subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME0600_TTLI_H_
+#define _ME0600_TTLI_H_
+
+#include "mesubdevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The template subdevice class.
+ */
+typedef struct me0600_ttli_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+
+	uint32_t port_reg;			/**< Register holding the port status. */
+} me0600_ttli_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-630 TTL input subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me0600_ttli_subdevice_t *me0600_ttli_constructor(uint32_t reg_base);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me0600_ttli_reg.h b/drivers/staging/meilhaus/me0600_ttli_reg.h
new file mode 100644
index 00000000000..4f986d16093
--- /dev/null
+++ b/drivers/staging/meilhaus/me0600_ttli_reg.h
@@ -0,0 +1,35 @@
+/**
+ * @file me0600_ttli_reg.h
+ *
+ * @brief ME-630 TTL input subdevice register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME0600_TTLI_REG_H_
+#define _ME0600_TTLI_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME0600_TTL_INPUT_REG       0x0003
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me0900_device.c b/drivers/staging/meilhaus/me0900_device.c
new file mode 100644
index 00000000000..764d5d307c4
--- /dev/null
+++ b/drivers/staging/meilhaus/me0900_device.c
@@ -0,0 +1,180 @@
+/**
+ * @file me0900_device.c
+ *
+ * @brief ME-9x device class implementation.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+*/
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+#ifndef MODULE
+#  define MODULE
+#endif
+
+#include <linux/module.h>
+
+#include <linux/pci.h>
+#include <linux/slab.h>
+
+#include "meids.h"
+#include "meerror.h"
+#include "mecommon.h"
+#include "meinternal.h"
+
+#include "medebug.h"
+#include "medevice.h"
+#include "me0900_device.h"
+#include "me0900_reg.h"
+#include "mesubdevice.h"
+#include "me0900_do.h"
+#include "me0900_di.h"
+
+me_device_t *me0900_pci_constructor(struct pci_dev *pci_device)
+{
+	me0900_device_t *me0900_device;
+	me_subdevice_t *subdevice;
+	unsigned int version_idx;
+	int err;
+	int i;
+	int port_shift;
+
+	PDEBUG("executed.\n");
+
+	// Allocate structure for device instance.
+	me0900_device = kmalloc(sizeof(me0900_device_t), GFP_KERNEL);
+
+	if (!me0900_device) {
+		PERROR("Cannot get memory for device instance.\n");
+		return NULL;
+	}
+
+	memset(me0900_device, 0, sizeof(me0900_device_t));
+
+	// Initialize base class structure.
+	err = me_device_pci_init((me_device_t *) me0900_device, pci_device);
+
+	if (err) {
+		kfree(me0900_device);
+		PERROR("Cannot initialize device base class.\n");
+		return NULL;
+	}
+
+	/* Get the index in the device version information table. */
+	version_idx =
+	    me0900_versions_get_device_index(me0900_device->base.info.pci.
+					     device_id);
+
+	/* Initialize 8255 chip to desired mode */
+	if (me0900_device->base.info.pci.device_id ==
+	    PCI_DEVICE_ID_MEILHAUS_ME0940) {
+		outb(0x9B,
+		     me0900_device->base.info.pci.reg_bases[2] +
+		     ME0900_CTRL_REG);
+	} else if (me0900_device->base.info.pci.device_id ==
+		   PCI_DEVICE_ID_MEILHAUS_ME0950) {
+		outb(0x89,
+		     me0900_device->base.info.pci.reg_bases[2] +
+		     ME0900_CTRL_REG);
+		outb(0x00,
+		     me0900_device->base.info.pci.reg_bases[2] +
+		     ME0900_WRITE_ENABLE_REG);
+	} else if (me0900_device->base.info.pci.device_id ==
+		   PCI_DEVICE_ID_MEILHAUS_ME0960) {
+		outb(0x8B,
+		     me0900_device->base.info.pci.reg_bases[2] +
+		     ME0900_CTRL_REG);
+		outb(0x00,
+		     me0900_device->base.info.pci.reg_bases[2] +
+		     ME0900_WRITE_ENABLE_REG);
+	}
+
+	port_shift =
+	    (me0900_device->base.info.pci.device_id ==
+	     PCI_DEVICE_ID_MEILHAUS_ME0960) ? 1 : 0;
+	// Create subdevice instances.
+
+	for (i = 0; i < me0900_versions[version_idx].di_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) me0900_di_constructor(me0900_device->
+							     base.info.pci.
+							     reg_bases[2],
+							     i + port_shift);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me0900_device);
+			kfree(me0900_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me0900_device->base.slist,
+					    subdevice);
+	}
+
+	for (i = 0; i < me0900_versions[version_idx].do_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) me0900_do_constructor(me0900_device->
+							     base.info.pci.
+							     reg_bases[2], i);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me0900_device);
+			kfree(me0900_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me0900_device->base.slist,
+					    subdevice);
+	}
+
+	return (me_device_t *) me0900_device;
+}
+
+// Init and exit of module.
+
+static int __init me0900_init(void)
+{
+	PDEBUG("executed.\n.");
+	return 0;
+}
+
+static void __exit me0900_exit(void)
+{
+	PDEBUG("executed.\n.");
+}
+
+module_init(me0900_init);
+module_exit(me0900_exit);
+
+// Administrative stuff for modinfo.
+MODULE_AUTHOR
+    ("Guenter Gebhardt <g.gebhardt@meilhaus.de> & Krzysztof Gantzke <k.gantzke@meilhaus.de>");
+MODULE_DESCRIPTION("Device Driver Module for ME-9x Device");
+MODULE_SUPPORTED_DEVICE("Meilhaus ME-9x Devices");
+MODULE_LICENSE("GPL");
+
+// Export the constructor.
+EXPORT_SYMBOL(me0900_pci_constructor);
diff --git a/drivers/staging/meilhaus/me0900_device.h b/drivers/staging/meilhaus/me0900_device.h
new file mode 100644
index 00000000000..bd17f252151
--- /dev/null
+++ b/drivers/staging/meilhaus/me0900_device.h
@@ -0,0 +1,92 @@
+/**
+ * @file me0900_device.h
+ *
+ * @brief ME-0900 (ME-9x) device class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME0900_DEVICE_H
+#define _ME0900_DEVICE_H
+
+#include <linux/pci.h>
+#include <linux/spinlock.h>
+
+#include "medevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief Structure holding ME-0900 (ME-9x) device capabilities.
+ */
+typedef struct me0900_version {
+	uint16_t device_id;
+	unsigned int di_subdevices;
+	unsigned int do_subdevices;
+} me0900_version_t;
+
+/**
+ * @brief Device capabilities.
+ */
+static me0900_version_t me0900_versions[] = {
+	{PCI_DEVICE_ID_MEILHAUS_ME0940, 2, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME0950, 0, 2},
+	{PCI_DEVICE_ID_MEILHAUS_ME0960, 1, 1},
+	{0},
+};
+
+#define ME0900_DEVICE_VERSIONS (sizeof(me0900_versions) / sizeof(me0900_version_t) - 1)	/**< Returns the number of entries in #me0900_versions. */
+
+/**
+ * @brief Returns the index of the device entry in #me0900_versions.
+ *
+ * @param device_id The PCI device id of the device to query.
+ * @return The index of the device in #me0900_versions.
+ */
+static inline unsigned int me0900_versions_get_device_index(uint16_t device_id)
+{
+	unsigned int i;
+	for (i = 0; i < ME0900_DEVICE_VERSIONS; i++)
+		if (me0900_versions[i].device_id == device_id)
+			break;
+	return i;
+}
+
+/**
+ * @brief The ME-0900 (ME-9x) device class structure.
+ */
+typedef struct me0900_device {
+	me_device_t base;			/**< The Meilhaus device base class. */
+} me0900_device_t;
+
+/**
+ * @brief The ME-9x device class constructor.
+ *
+ * @param pci_device The pci device structure given by the PCI subsystem.
+ *
+ * @return On succes a new ME-0900 (ME-9x) device instance. \n
+ *         NULL on error.
+ */
+me_device_t *me0900_pci_constructor(struct pci_dev *pci_device)
+    __attribute__ ((weak));
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me0900_di.c b/drivers/staging/meilhaus/me0900_di.c
new file mode 100644
index 00000000000..d7d7394f800
--- /dev/null
+++ b/drivers/staging/meilhaus/me0900_di.c
@@ -0,0 +1,246 @@
+/**
+ * @file me0900_di.c
+ *
+ * @brief ME-9x digital input subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+#include <linux/interrupt.h>
+#include <linux/version.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+
+#include "meids.h"
+#include "medebug.h"
+#include "meplx_reg.h"
+#include "me0900_reg.h"
+#include "me0900_di.h"
+
+/*
+ * Defines
+ */
+
+/*
+ * Functions
+ */
+
+static int me0900_di_io_reset_subdevice(struct me_subdevice *subdevice,
+					struct file *filep, int flags)
+{
+	PDEBUG("executed.\n");
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0900_di_io_single_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int single_config,
+				      int ref,
+				      int trig_chan,
+				      int trig_type, int trig_edge, int flags)
+{
+	me0900_di_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0900_di_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	switch (flags) {
+	case ME_IO_SINGLE_CONFIG_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			if (single_config == ME_SINGLE_CONFIG_DIO_INPUT) {
+			} else {
+				PERROR("Invalid byte direction specified.\n");
+				err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me0900_di_io_single_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int channel,
+				    int *value, int time_out, int flags)
+{
+	me0900_di_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0900_di_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			*value = (~inb(instance->port_reg)) & (0x1 << channel);
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			*value = ~inb(instance->port_reg);
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me0900_di_query_number_channels(me_subdevice_t * subdevice,
+					   int *number)
+{
+	PDEBUG("executed.\n");
+	*number = 8;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0900_di_query_subdevice_type(me_subdevice_t * subdevice,
+					  int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_DI;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0900_di_query_subdevice_caps(me_subdevice_t * subdevice, int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = 0;
+	return ME_ERRNO_SUCCESS;
+}
+
+me0900_di_subdevice_t *me0900_di_constructor(uint32_t reg_base,
+					     unsigned int di_idx)
+{
+	me0900_di_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me0900_di_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me0900_di_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	/* Save the subdevice index. */
+	subdevice->di_idx = di_idx;
+
+	/* Initialize registers */
+	if (di_idx == 0) {
+		subdevice->ctrl_reg = reg_base + ME0900_CTRL_REG;
+		subdevice->port_reg = reg_base + ME0900_PORT_A_REG;
+	} else {
+		subdevice->ctrl_reg = reg_base + ME0900_CTRL_REG;
+		subdevice->port_reg = reg_base + ME0900_PORT_B_REG;
+	}
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = reg_base;
+#endif
+
+	/* Overload base class methods. */
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me0900_di_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me0900_di_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me0900_di_io_single_read;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me0900_di_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me0900_di_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me0900_di_query_subdevice_caps;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me0900_di.h b/drivers/staging/meilhaus/me0900_di.h
new file mode 100644
index 00000000000..014f1348fc9
--- /dev/null
+++ b/drivers/staging/meilhaus/me0900_di.h
@@ -0,0 +1,65 @@
+/**
+ * @file me0900_di.h
+ *
+ * @brief ME-9x digital input subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME0900_DI_H_
+#define _ME0900_DI_H_
+
+#include "mesubdevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The template subdevice class.
+ */
+typedef struct me0900_di_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+
+	unsigned int di_idx;
+
+	unsigned long ctrl_reg;
+	unsigned long port_reg;
+#ifdef MEDEBUG_DEBUG_REG
+	unsigned long reg_base;
+#endif
+} me0900_di_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-9x digital input subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me0900_di_subdevice_t *me0900_di_constructor(uint32_t me0900_reg_base,
+					     unsigned int di_idx);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me0900_do.c b/drivers/staging/meilhaus/me0900_do.c
new file mode 100644
index 00000000000..b5b9c3a98c9
--- /dev/null
+++ b/drivers/staging/meilhaus/me0900_do.c
@@ -0,0 +1,314 @@
+/**
+ * @file me0900_do.c
+ *
+ * @brief ME-9x digital output subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "me0900_reg.h"
+#include "me0900_do.h"
+
+/*
+ * Defines
+ */
+
+/*
+ * Functions
+ */
+
+static int me0900_do_io_reset_subdevice(struct me_subdevice *subdevice,
+					struct file *filep, int flags)
+{
+	me0900_do_subdevice_t *instance;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0900_do_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	outb(0xFF, instance->port_reg);
+	PDEBUG_REG("port_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->port_reg - instance->reg_base, 0xff);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0900_do_io_single_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int single_config,
+				      int ref,
+				      int trig_chan,
+				      int trig_type, int trig_edge, int flags)
+{
+	me0900_do_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0900_do_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	switch (flags) {
+	case ME_IO_SINGLE_CONFIG_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			if (single_config == ME_SINGLE_CONFIG_DIO_OUTPUT) {
+			} else {
+				PERROR("Invalid byte direction specified.\n");
+				err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me0900_do_io_single_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int channel,
+				    int *value, int time_out, int flags)
+{
+	me0900_do_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0900_do_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			*value = (~inb(instance->port_reg)) & (0x1 << channel);
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			*value = ~inb(instance->port_reg);
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me0900_do_io_single_write(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int channel,
+				     int value, int time_out, int flags)
+{
+	me0900_do_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long state;
+
+	PDEBUG("executed.\n");
+
+	instance = (me0900_do_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			state = inb(instance->port_reg);
+			state =
+			    (!value) ? (state | (0x1 << channel)) : (state &
+								     ~(0x1 <<
+								       channel));
+			outb(state, instance->port_reg);
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			outb(~(value), instance->port_reg);
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me0900_do_query_number_channels(me_subdevice_t * subdevice,
+					   int *number)
+{
+	PDEBUG("executed.\n");
+	*number = 8;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0900_do_query_subdevice_type(me_subdevice_t * subdevice,
+					  int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_DO;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me0900_do_query_subdevice_caps(me_subdevice_t * subdevice, int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = 0;
+	return ME_ERRNO_SUCCESS;
+}
+
+me0900_do_subdevice_t *me0900_do_constructor(uint32_t reg_base,
+					     unsigned int do_idx)
+{
+	me0900_do_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me0900_do_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me0900_do_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	/* Save the subdevice index */
+	subdevice->do_idx = do_idx;
+
+	/* Initialize registers */
+	if (do_idx == 0) {
+		subdevice->ctrl_reg = reg_base + ME0900_CTRL_REG;
+		subdevice->port_reg = reg_base + ME0900_PORT_A_REG;
+		subdevice->enable_reg = reg_base + ME0900_WRITE_ENABLE_REG;
+		subdevice->disable_reg = reg_base + ME0900_WRITE_DISABLE_REG;
+	} else {
+		subdevice->ctrl_reg = reg_base + ME0900_CTRL_REG;
+		subdevice->port_reg = reg_base + ME0900_PORT_B_REG;
+		subdevice->enable_reg = reg_base + ME0900_WRITE_ENABLE_REG;
+		subdevice->disable_reg = reg_base + ME0900_WRITE_DISABLE_REG;
+	}
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = reg_base;
+#endif
+
+	/* Overload base class methods. */
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me0900_do_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me0900_do_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me0900_do_io_single_read;
+	subdevice->base.me_subdevice_io_single_write =
+	    me0900_do_io_single_write;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me0900_do_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me0900_do_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me0900_do_query_subdevice_caps;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me0900_do.h b/drivers/staging/meilhaus/me0900_do.h
new file mode 100644
index 00000000000..13e8a8b94cf
--- /dev/null
+++ b/drivers/staging/meilhaus/me0900_do.h
@@ -0,0 +1,68 @@
+/**
+ * @file me0900_do.h
+ *
+ * @brief ME-9x digital output subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME0900_DO_H_
+#define _ME0900_DO_H_
+
+#include "mesubdevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The template subdevice class.
+ */
+typedef struct me0900_do_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+
+	unsigned int do_idx;
+
+	unsigned long ctrl_reg;
+	unsigned long port_reg;
+	unsigned long enable_reg;
+	unsigned long disable_reg;
+#ifdef MEDEBUG_DEBUG_REG
+	unsigned long reg_base;
+#endif
+} me0900_do_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-9x digital output subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ * @param do_idx The index of the digital output subdevice on this device.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me0900_do_subdevice_t *me0900_do_constructor(uint32_t reg_base,
+					     unsigned int do_idx);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me0900_reg.h b/drivers/staging/meilhaus/me0900_reg.h
new file mode 100644
index 00000000000..3bf163b6ac4
--- /dev/null
+++ b/drivers/staging/meilhaus/me0900_reg.h
@@ -0,0 +1,40 @@
+/**
+ * @file me0900_reg.h
+ *
+ * @brief ME-9x register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME0900_REG_H_
+#define _ME0900_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME0900_PORT_A_REG          0x00
+#define ME0900_PORT_B_REG          0x01
+#define ME0900_PORT_C_REG          0x02
+#define ME0900_CTRL_REG            0x03	// ( ,w)
+#define ME0900_WRITE_ENABLE_REG    0x04	// (r,w)
+#define ME0900_WRITE_DISABLE_REG   0x08	// (r,w)
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me1000_device.c b/drivers/staging/meilhaus/me1000_device.c
new file mode 100644
index 00000000000..c44e214af26
--- /dev/null
+++ b/drivers/staging/meilhaus/me1000_device.c
@@ -0,0 +1,208 @@
+/**
+ * @file me1000_device.c
+ *
+ * @brief ME-1000 device class implementation.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+#ifndef MODULE
+#  define MODULE
+#endif
+
+#include <linux/module.h>
+
+#include <linux/pci.h>
+#include <linux/slab.h>
+
+#include "meids.h"
+#include "meerror.h"
+#include "mecommon.h"
+#include "meinternal.h"
+
+#include "medebug.h"
+#include "medevice.h"
+#include "me1000_device.h"
+#include "mesubdevice.h"
+#include "me1000_dio.h"
+
+static int me1000_config_load(me_device_t * me_device, struct file *filep,
+			      me_cfg_device_entry_t * config)
+{
+	me1000_device_t *me1000_device;
+	me1000_dio_subdevice_t *dio;
+
+	PDEBUG("executed.\n");
+
+	me1000_device = (me1000_device_t *) me_device;
+
+	if (config->count == 2) {
+		if (me_slist_get_number_subdevices(&me1000_device->base.slist)
+		    == 2) {
+			// Nothing to do.
+		} else {
+			// Remove 2 extra subdevices
+			dio =
+			    (me1000_dio_subdevice_t *)
+			    me_slist_del_subdevice_tail(&me1000_device->base.
+							slist);
+			if (dio)
+				dio->base.
+				    me_subdevice_destructor((me_subdevice_t *)
+							    dio);
+
+			dio =
+			    (me1000_dio_subdevice_t *)
+			    me_slist_del_subdevice_tail(&me1000_device->base.
+							slist);
+			if (dio)
+				dio->base.
+				    me_subdevice_destructor((me_subdevice_t *)
+							    dio);
+		}
+	} else if (config->count == 4) {
+		//Add 2 subdevices
+		if (me_slist_get_number_subdevices(&me1000_device->base.slist)
+		    == 2) {
+			dio =
+			    me1000_dio_constructor(me1000_device->base.info.pci.
+						   reg_bases[2], 2,
+						   &me1000_device->ctrl_lock);
+			if (!dio) {
+				PERROR("Cannot create dio subdevice.\n");
+				return ME_ERRNO_INTERNAL;
+			}
+			me_slist_add_subdevice_tail(&me1000_device->base.slist,
+						    (me_subdevice_t *) dio);
+
+			dio =
+			    me1000_dio_constructor(me1000_device->base.info.pci.
+						   reg_bases[2], 3,
+						   &me1000_device->ctrl_lock);
+			if (!dio) {
+				dio =
+				    (me1000_dio_subdevice_t *)
+				    me_slist_del_subdevice_tail(&me1000_device->
+								base.slist);
+				if (dio)
+					dio->base.
+					    me_subdevice_destructor((me_subdevice_t *) dio);
+
+				PERROR("Cannot create dio subdevice.\n");
+				return ME_ERRNO_INTERNAL;
+			}
+			me_slist_add_subdevice_tail(&me1000_device->base.slist,
+						    (me_subdevice_t *) dio);
+		} else {
+			// Nothing to do.
+		}
+	} else {
+		PERROR("Invalid configuration.\n");
+		return ME_ERRNO_INTERNAL;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+me_device_t *me1000_pci_constructor(struct pci_dev * pci_device)
+{
+	me1000_device_t *me1000_device;
+	me_subdevice_t *subdevice;
+	int err;
+	int i;
+
+	PDEBUG("executed.\n");
+
+	// Allocate structure for device instance.
+	me1000_device = kmalloc(sizeof(me1000_device_t), GFP_KERNEL);
+
+	if (!me1000_device) {
+		PERROR("Cannot get memory for ME-1000 device instance.\n");
+		return NULL;
+	}
+
+	memset(me1000_device, 0, sizeof(me1000_device_t));
+
+	// Initialize base class structure.
+	err = me_device_pci_init((me_device_t *) me1000_device, pci_device);
+
+	if (err) {
+		kfree(me1000_device);
+		PERROR("Cannot initialize device base class.\n");
+		return NULL;
+	}
+	// Initialize spin lock .
+	spin_lock_init(&me1000_device->ctrl_lock);
+
+	for (i = 0; i < 4; i++) {
+		subdevice =
+		    (me_subdevice_t *) me1000_dio_constructor(me1000_device->
+							      base.info.pci.
+							      reg_bases[2], i,
+							      &me1000_device->
+							      ctrl_lock);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me1000_device);
+			kfree(me1000_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me1000_device->base.slist,
+					    subdevice);
+	}
+
+	// Overwrite base class methods.
+	me1000_device->base.me_device_config_load = me1000_config_load;
+
+	return (me_device_t *) me1000_device;
+}
+
+// Init and exit of module.
+static int __init me1000_init(void)
+{
+	PDEBUG("executed.\n");
+	return 0;
+}
+
+static void __exit me1000_exit(void)
+{
+	PDEBUG("executed.\n");
+}
+
+module_init(me1000_init);
+module_exit(me1000_exit);
+
+// Administrative stuff for modinfo.
+MODULE_AUTHOR
+    ("Guenter Gebhardt <g.gebhardt@meilhaus.de> & Krzysztof Gantzke <k.gantzke@meilhaus.de>");
+MODULE_DESCRIPTION("Device Driver Module for Meilhaus ME-1000 Devices");
+MODULE_SUPPORTED_DEVICE("Meilhaus ME-1000 Digital I/O Devices");
+MODULE_LICENSE("GPL");
+
+// Export the constructor.
+EXPORT_SYMBOL(me1000_pci_constructor);
diff --git a/drivers/staging/meilhaus/me1000_device.h b/drivers/staging/meilhaus/me1000_device.h
new file mode 100644
index 00000000000..cbbe1263017
--- /dev/null
+++ b/drivers/staging/meilhaus/me1000_device.h
@@ -0,0 +1,59 @@
+/**
+ * @file me1000_device.h
+ *
+ * @brief ME-1000 device class instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME1000_H_
+#define _ME1000_H_
+
+#include <linux/pci.h>
+#include <linux/spinlock.h>
+
+#include "medevice.h"
+
+#ifdef __KERNEL__
+
+#define ME1000_MAGIC_NUMBER	1000
+
+/**
+ * @brief The ME-1000 device class structure.
+ */
+typedef struct me1000_device {
+	me_device_t base;		/**< The Meilhaus device base class. */
+	spinlock_t ctrl_lock;		/**< Guards the DIO mode register. */
+} me1000_device_t;
+
+/**
+ * @brief The ME-1000 device class constructor.
+ *
+ * @param pci_device The pci device structure given by the PCI subsystem.
+ *
+ * @return On succes a new ME-1000 device instance. \n
+ *         NULL on error.
+ */
+me_device_t *me1000_pci_constructor(struct pci_dev *pci_device)
+    __attribute__ ((weak));
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me1000_dio.c b/drivers/staging/meilhaus/me1000_dio.c
new file mode 100644
index 00000000000..87605a9108a
--- /dev/null
+++ b/drivers/staging/meilhaus/me1000_dio.c
@@ -0,0 +1,438 @@
+/**
+ * @file me1000_dio.c
+ *
+ * @brief ME-1000 DIO subdevice instance.
+ * @note Copyright (C) 2006 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2006 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+#include "medebug.h"
+
+#include "me1000_dio_reg.h"
+#include "me1000_dio.h"
+
+/*
+ * Defines
+ */
+#define ME1000_DIO_MAGIC_NUMBER	0x1000	/**< The magic number of the class structure. */
+
+/*
+ * Functions
+ */
+
+static int me1000_dio_io_reset_subdevice(struct me_subdevice *subdevice,
+					 struct file *filep, int flags)
+{
+	me1000_dio_subdevice_t *instance;
+	uint32_t tmp;
+
+	PDEBUG("executed.\n");
+
+	instance = (me1000_dio_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	tmp = inl(instance->ctrl_reg);
+	tmp &= ~(0x1 << instance->dio_idx);
+	outl(tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp);
+	spin_unlock(instance->ctrl_reg_lock);
+
+	outl(0x00000000, instance->port_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, 0);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me1000_dio_io_single_config(struct me_subdevice *subdevice,
+				       struct file *filep,
+				       int channel,
+				       int single_config,
+				       int ref,
+				       int trig_chan,
+				       int trig_type, int trig_edge, int flags)
+{
+	me1000_dio_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	int ctrl;
+	int size =
+	    flags & (ME_IO_SINGLE_CONFIG_DIO_BIT | ME_IO_SINGLE_CONFIG_DIO_BYTE
+		     | ME_IO_SINGLE_CONFIG_DIO_WORD |
+		     ME_IO_SINGLE_CONFIG_DIO_DWORD);
+
+	PDEBUG("executed.\n");
+
+	instance = (me1000_dio_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	ctrl = inl(instance->ctrl_reg);
+
+	switch (size) {
+	case ME_IO_SINGLE_CONFIG_NO_FLAGS:
+	case ME_IO_SINGLE_CONFIG_DIO_DWORD:
+		if (channel == 0) {
+			if (single_config == ME_SINGLE_CONFIG_DIO_INPUT) {
+				ctrl &= ~(0x1 << instance->dio_idx);
+			} else if (single_config == ME_SINGLE_CONFIG_DIO_OUTPUT) {
+				ctrl |= 0x1 << instance->dio_idx;
+			} else {
+				PERROR("Invalid port direction.\n");
+				err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		} else {
+			PERROR("Invalid channel number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (!err) {
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me1000_dio_io_single_read(struct me_subdevice *subdevice,
+				     struct file *filep,
+				     int channel,
+				     int *value, int time_out, int flags)
+{
+	me1000_dio_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me1000_dio_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 32)) {
+			*value = inl(instance->port_reg) & (0x1 << channel);
+		} else {
+			PERROR("Invalid bit number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if ((channel >= 0) && (channel < 4)) {
+			*value =
+			    (inl(instance->port_reg) >> (channel * 8)) & 0xFF;
+		} else {
+			PERROR("Invalid byte number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_TYPE_DIO_WORD:
+		if ((channel >= 0) && (channel < 2)) {
+			*value =
+			    (inl(instance->port_reg) >> (channel * 16)) &
+			    0xFFFF;
+		} else {
+			PERROR("Invalid word number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_DWORD:
+		if (channel == 0) {
+			*value = inl(instance->port_reg);
+		} else {
+			PERROR("Invalid dword number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me1000_dio_io_single_write(struct me_subdevice *subdevice,
+				      struct file *filep,
+				      int channel,
+				      int value, int time_out, int flags)
+{
+	me1000_dio_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint32_t config;
+	uint32_t state;
+
+	PDEBUG("executed.\n");
+
+	instance = (me1000_dio_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	config = inl(instance->ctrl_reg) & (0x1 << instance->dio_idx);
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 32)) {
+			if (config) {
+				state = inl(instance->port_reg);
+				state =
+				    value ? (state | (0x1 << channel)) : (state
+									  &
+									  ~(0x1
+									    <<
+									    channel));
+				outl(state, instance->port_reg);
+				PDEBUG_REG("port_reg outl(0x%lX+0x%lX)=0x%x\n",
+					   instance->reg_base,
+					   instance->port_reg -
+					   instance->reg_base, state);
+			} else {
+				PERROR("Port is not in output mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid bit number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if ((channel >= 0) && (channel < 4)) {
+			if (config) {
+				state = inl(instance->port_reg);
+				state &= ~(0xFF << (channel * 8));
+				state |= (value & 0xFF) << (channel * 8);
+				outl(state, instance->port_reg);
+				PDEBUG_REG("port_reg outl(0x%lX+0x%lX)=0x%x\n",
+					   instance->reg_base,
+					   instance->port_reg -
+					   instance->reg_base, state);
+			} else {
+				PERROR("Port is not in output mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid byte number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_TYPE_DIO_WORD:
+		if ((channel >= 0) && (channel < 2)) {
+			if (config) {
+				state = inl(instance->port_reg);
+				state &= ~(0xFFFF << (channel * 16));
+				state |= (value & 0xFFFF) << (channel * 16);
+				outl(state, instance->port_reg);
+				PDEBUG_REG("port_reg outl(0x%lX+0x%lX)=0x%x\n",
+					   instance->reg_base,
+					   instance->port_reg -
+					   instance->reg_base, state);
+			} else {
+				PERROR("Port is not in output mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid word number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_DWORD:
+		if (channel == 0) {
+			if (config) {
+				outl(value, instance->port_reg);
+				PDEBUG_REG("port_reg outl(0x%lX+0x%lX)=0x%x\n",
+					   instance->reg_base,
+					   instance->port_reg -
+					   instance->reg_base, value);
+			} else {
+				PERROR("Port is not in output mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid dword number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me1000_dio_query_number_channels(struct me_subdevice *subdevice,
+					    int *number)
+{
+	PDEBUG("executed.\n");
+	*number = ME1000_DIO_NUMBER_CHANNELS;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me1000_dio_query_subdevice_type(struct me_subdevice *subdevice,
+					   int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_DIO;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me1000_dio_query_subdevice_caps(struct me_subdevice *subdevice,
+					   int *caps)
+{
+	me1000_dio_subdevice_t *instance;
+
+	PDEBUG("executed.\n");
+
+	instance = (me1000_dio_subdevice_t *) subdevice;
+
+	*caps = ME_CAPS_DIO_DIR_DWORD;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+me1000_dio_subdevice_t *me1000_dio_constructor(uint32_t reg_base,
+					       unsigned int dio_idx,
+					       spinlock_t * ctrl_reg_lock)
+{
+	me1000_dio_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me1000_dio_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for ME-1000 DIO instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me1000_dio_subdevice_t));
+
+	/* Check if counter index is out of range */
+
+	if (dio_idx >= ME1000_DIO_NUMBER_PORTS) {
+		PERROR("DIO index is out of range.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+	subdevice->ctrl_reg_lock = ctrl_reg_lock;
+
+	/* Save the DIO index */
+	subdevice->dio_idx = dio_idx;
+
+	/* Initialize registers. */
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = reg_base;
+#endif
+	subdevice->ctrl_reg = reg_base + ME1000_PORT_MODE;
+	subdevice->port_reg =
+	    reg_base + ME1000_PORT + (dio_idx * ME1000_PORT_STEP);
+
+	/* Override base class methods. */
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me1000_dio_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me1000_dio_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me1000_dio_io_single_read;
+	subdevice->base.me_subdevice_io_single_write =
+	    me1000_dio_io_single_write;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me1000_dio_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me1000_dio_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me1000_dio_query_subdevice_caps;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me1000_dio.h b/drivers/staging/meilhaus/me1000_dio.h
new file mode 100644
index 00000000000..d26e93f531a
--- /dev/null
+++ b/drivers/staging/meilhaus/me1000_dio.h
@@ -0,0 +1,71 @@
+/**
+ * @file me1000_dio.h
+ *
+ * @brief Meilhaus ME-1000 digital i/o implementation.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME1000_DIO_H_
+#define _ME1000_DIO_H_
+
+#include "mesubdevice.h"
+#include "meslock.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The ME-1000 DIO subdevice class.
+ */
+typedef struct me1000_dio_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+//      uint32_t magic;                                 /**< The magic number unique for this structure. */
+
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *ctrl_reg_lock;		/**< Spin lock to protect #ctrl_reg and #ctrl_reg_mirror from concurrent access. */
+	int dio_idx;					/**< The index of the DIO port on the device. */
+
+	unsigned long port_reg;			/**< Register to read or write a value from or to the port respectively. */
+	unsigned long ctrl_reg;			/**< Register to configure the DIO modes. */
+#ifdef MEDEBUG_DEBUG_REG
+	unsigned long reg_base;
+#endif
+} me1000_dio_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-1000 DIO instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ * @param dio_idx The index of the DIO on the device.
+ * @param ctrl_reg_lock Pointer to spin lock protecting the control register and from concurrent access.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me1000_dio_subdevice_t *me1000_dio_constructor(uint32_t reg_base,
+					       unsigned int dio_idx,
+					       spinlock_t * ctrl_reg_lock);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me1000_dio_reg.h b/drivers/staging/meilhaus/me1000_dio_reg.h
new file mode 100644
index 00000000000..4d5b38df437
--- /dev/null
+++ b/drivers/staging/meilhaus/me1000_dio_reg.h
@@ -0,0 +1,50 @@
+/**
+ * @file me1000_dio_reg.h
+ *
+ * @brief ME-1000 digital i/o register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME1000_DIO_REG_H_
+# define _ME1000_DIO_REG_H_
+
+# ifdef __KERNEL__
+
+# define ME1000_DIO_NUMBER_CHANNELS	32				/**< The number of channels per DIO port. */
+# define ME1000_DIO_NUMBER_PORTS	4				/**< The number of ports per ME-1000. */
+
+// # define ME1000_PORT_A                               0x0000                  /**< Port A base register offset. */
+// # define ME1000_PORT_B                               0x0004                  /**< Port B base register offset. */
+// # define ME1000_PORT_C                               0x0008                  /**< Port C base register offset. */
+// # define ME1000_PORT_D                               0x000C                  /**< Port D base register offset. */
+# define ME1000_PORT				0x0000			/**< Base for port's register. */
+# define ME1000_PORT_STEP			4				/**< Distance between port's register. */
+
+# define ME1000_PORT_MODE			0x0010			/**< Configuration register to switch the port direction. */
+// # define ME1000_PORT_MODE_OUTPUT_A   (1 << 0)                /**< If set, port A is in output, otherwise in input mode. */
+// # define ME1000_PORT_MODE_OUTPUT_B   (1 << 1)                /**< If set, port B is in output, otherwise in input mode. */
+// # define ME1000_PORT_MODE_OUTPUT_C   (1 << 2)                /**< If set, port C is in output, otherwise in input mode. */
+// # define ME1000_PORT_MODE_OUTPUT_D   (1 << 3)                /**< If set, port D is in output, otherwise in input mode. */
+
+# endif	//__KERNEL__
+#endif //_ME1000_DIO_REG_H_
diff --git a/drivers/staging/meilhaus/me1400_device.c b/drivers/staging/meilhaus/me1400_device.c
new file mode 100644
index 00000000000..b95bb4fce6a
--- /dev/null
+++ b/drivers/staging/meilhaus/me1400_device.c
@@ -0,0 +1,256 @@
+/**
+ * @file me1400_device.c
+ *
+ * @brief ME-1400 device instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+/*
+ * User application could also include the kernel header files. But the
+ * real kernel functions are protected by #ifdef __KERNEL__.
+ */
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * This must be defined before module.h is included. Not needed, when
+ * it is a built in driver.
+ */
+#ifndef MODULE
+#  define MODULE
+#endif
+
+#include <linux/module.h>
+
+#include <linux/pci.h>
+#include <linux/slab.h>
+#include <linux/sched.h>
+#include <linux/interrupt.h>
+#include <linux/version.h>
+
+#include "meids.h"
+#include "meerror.h"
+#include "mecommon.h"
+#include "meinternal.h"
+
+#include "medebug.h"
+
+#include "me1400_device.h"
+#include "me8254.h"
+#include "me8254_reg.h"
+#include "me8255.h"
+#include "me1400_ext_irq.h"
+
+me_device_t *me1400_pci_constructor(struct pci_dev *pci_device)
+{
+	int err;
+	me1400_device_t *me1400_device;
+	me_subdevice_t *subdevice;
+	unsigned int version_idx;
+	unsigned int me8255_idx;
+	unsigned int dio_idx;
+	unsigned int me8254_idx;
+	unsigned int ctr_idx;
+	unsigned int ext_irq_idx;
+
+	PDEBUG("executed.\n");
+
+	// Allocate structure for device instance.
+	me1400_device = kmalloc(sizeof(me1400_device_t), GFP_KERNEL);
+
+	if (!me1400_device) {
+		PERROR("Cannot get memory for 1400ate device instance.\n");
+		return NULL;
+	}
+
+	memset(me1400_device, 0, sizeof(me1400_device_t));
+
+	// Initialize base class structure.
+	err = me_device_pci_init((me_device_t *) me1400_device, pci_device);
+
+	if (err) {
+		kfree(me1400_device);
+		PERROR("Cannot initialize device base class.\n");
+		return NULL;
+	}
+
+	/* Check for ME1400 extension device. If detected we fake a ME-1400 D device id. */
+	if (me1400_device->base.info.pci.device_id ==
+	    PCI_DEVICE_ID_MEILHAUS_ME140C) {
+		uint8_t ctrl;
+		ctrl =
+		    inb(me1400_device->base.info.pci.reg_bases[2] +
+			ME1400D_CLK_SRC_2_REG);
+		PDEBUG_REG("xxx_reg inb(0x%X+0x%X)=0x%x\n",
+			   me1400_device->base.info.pci.reg_bases[2],
+			   ME1400D_CLK_SRC_2_REG, ctrl);
+		outb(ctrl | 0xF0,
+		     me1400_device->base.info.pci.reg_bases[2] +
+		     ME1400D_CLK_SRC_2_REG);
+		PDEBUG_REG("xxx_reg outb(0x%X+0x%X)=0x%x\n",
+			   me1400_device->base.info.pci.reg_bases[2],
+			   ME1400D_CLK_SRC_2_REG, ctrl | 0xF0);
+		ctrl =
+		    inb(me1400_device->base.info.pci.reg_bases[2] +
+			ME1400D_CLK_SRC_2_REG);
+		PDEBUG_REG("xxx_reg inb(0x%X+0x%X)=0x%x\n",
+			   me1400_device->base.info.pci.reg_bases[2],
+			   ME1400D_CLK_SRC_2_REG, ctrl);
+
+		if ((ctrl & 0xF0) == 0xF0) {
+			PINFO("ME1400 D detected.\n");
+			me1400_device->base.info.pci.device_id =
+			    PCI_DEVICE_ID_MEILHAUS_ME140D;
+		}
+	}
+
+	/* Initialize global stuff of digital i/o subdevices. */
+	for (me8255_idx = 0; me8255_idx < ME1400_MAX_8255; me8255_idx++) {
+		me1400_device->dio_current_mode[me8255_idx] = 0;
+		spin_lock_init(&me1400_device->dio_ctrl_reg_lock[me8255_idx]);
+	}
+
+	/* Initialize global stuff of counter subdevices. */
+	spin_lock_init(&me1400_device->clk_src_reg_lock);
+
+	for (me8254_idx = 0; me8254_idx < ME1400_MAX_8254; me8254_idx++)
+		spin_lock_init(&me1400_device->ctr_ctrl_reg_lock[me8254_idx]);
+
+	/* Get the index in the device version information table. */
+	version_idx =
+	    me1400_versions_get_device_index(me1400_device->base.info.pci.
+					     device_id);
+
+	/* Generate DIO subdevice instances. */
+	for (me8255_idx = 0;
+	     me8255_idx < me1400_versions[version_idx].dio_chips;
+	     me8255_idx++) {
+		for (dio_idx = 0; dio_idx < 3; dio_idx++) {
+			subdevice =
+			    (me_subdevice_t *)
+			    me8255_constructor(me1400_versions[version_idx].
+					       device_id,
+					       me1400_device->base.info.pci.
+					       reg_bases[2], me8255_idx,
+					       dio_idx,
+					       &me1400_device->
+					       dio_current_mode[me8255_idx],
+					       &me1400_device->
+					       dio_ctrl_reg_lock[me8255_idx]);
+
+			if (!subdevice) {
+				me_device_deinit((me_device_t *) me1400_device);
+				kfree(me1400_device);
+				PERROR("Cannot get memory for subdevice.\n");
+				return NULL;
+			}
+
+			me_slist_add_subdevice_tail(&me1400_device->base.slist,
+						    subdevice);
+		}
+	}
+
+	/* Generate counter subdevice instances. */
+	for (me8254_idx = 0;
+	     me8254_idx < me1400_versions[version_idx].ctr_chips;
+	     me8254_idx++) {
+		for (ctr_idx = 0; ctr_idx < 3; ctr_idx++) {
+			subdevice =
+			    (me_subdevice_t *)
+			    me8254_constructor(me1400_device->base.info.pci.
+					       device_id,
+					       me1400_device->base.info.pci.
+					       reg_bases[2], me8254_idx,
+					       ctr_idx,
+					       &me1400_device->
+					       ctr_ctrl_reg_lock[me8254_idx],
+					       &me1400_device->
+					       clk_src_reg_lock);
+
+			if (!subdevice) {
+				me_device_deinit((me_device_t *) me1400_device);
+				kfree(me1400_device);
+				PERROR("Cannot get memory for subdevice.\n");
+				return NULL;
+			}
+
+			me_slist_add_subdevice_tail(&me1400_device->base.slist,
+						    subdevice);
+		}
+	}
+
+	/* Generate external interrupt subdevice instances. */
+	for (ext_irq_idx = 0;
+	     ext_irq_idx < me1400_versions[version_idx].ext_irq_subdevices;
+	     ext_irq_idx++) {
+		subdevice =
+		    (me_subdevice_t *)
+		    me1400_ext_irq_constructor(me1400_device->base.info.pci.
+					       device_id,
+					       me1400_device->base.info.pci.
+					       reg_bases[1],
+					       me1400_device->base.info.pci.
+					       reg_bases[2],
+					       &me1400_device->clk_src_reg_lock,
+					       me1400_device->base.irq);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me1400_device);
+			kfree(me1400_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me1400_device->base.slist,
+					    subdevice);
+	}
+
+	return (me_device_t *) me1400_device;
+}
+
+// Init and exit of module.
+
+static int __init me1400_init(void)
+{
+	PDEBUG("executed.\n");
+	return 0;
+}
+
+static void __exit me1400_exit(void)
+{
+	PDEBUG("executed.\n");
+}
+
+module_init(me1400_init);
+module_exit(me1400_exit);
+
+// Administrative stuff for modinfo.
+MODULE_AUTHOR
+    ("Guenter Gebhardt <g.gebhardt@meilhaus.de> & Krzysztof Gantzke <k.gantzke@meilhaus.de>");
+MODULE_DESCRIPTION("Device Driver Module for Meilhaus ME-14xx devices");
+MODULE_SUPPORTED_DEVICE("Meilhaus ME-14xx MIO devices");
+MODULE_LICENSE("GPL");
+
+// Export the constructor.
+EXPORT_SYMBOL(me1400_pci_constructor);
diff --git a/drivers/staging/meilhaus/me1400_device.h b/drivers/staging/meilhaus/me1400_device.h
new file mode 100644
index 00000000000..6215b250047
--- /dev/null
+++ b/drivers/staging/meilhaus/me1400_device.h
@@ -0,0 +1,108 @@
+/**
+ * @file me1400_device.c
+ *
+ * @brief ME-1400 device family instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME1400_DEVICE_H_
+#define _ME1400_DEVICE_H_
+
+#include "metypes.h"
+#include "medefines.h"
+#include "meinternal.h"
+
+#include "medevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief Structure to store device capabilities.
+ */
+typedef struct me1400_version {
+	uint16_t device_id;					/**< The PCI device id of the device. */
+	unsigned int dio_chips;				/**< The number of 8255 chips on the device. */
+	unsigned int ctr_chips;				/**< The number of 8254 chips on the device. */
+	unsigned int ext_irq_subdevices;	/**< The number of external interrupt inputs on the device. */
+} me1400_version_t;
+
+/**
+  * @brief Defines for each ME-1400 device version its capabilities.
+ */
+static me1400_version_t me1400_versions[] = {
+	{PCI_DEVICE_ID_MEILHAUS_ME1400, 1, 0, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME140A, 1, 1, 1},
+	{PCI_DEVICE_ID_MEILHAUS_ME140B, 2, 2, 1},
+	{PCI_DEVICE_ID_MEILHAUS_ME14E0, 1, 0, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME14EA, 1, 1, 1},
+	{PCI_DEVICE_ID_MEILHAUS_ME14EB, 2, 2, 1},
+	{PCI_DEVICE_ID_MEILHAUS_ME140C, 1, 5, 1},
+	{PCI_DEVICE_ID_MEILHAUS_ME140D, 2, 10, 1},
+	{0}
+};
+
+#define ME1400_DEVICE_VERSIONS (sizeof(me1400_versions) / sizeof(me1400_version_t) - 1)	/**< Returns the number of entries in #me1400_versions. */
+
+/**
+ * @brief Returns the index of the device entry in #me1400_versions.
+ *
+ * @param device_id The PCI device id of the device to query.
+ * @return The index of the device in #me1400_versions.
+ */
+static inline unsigned int me1400_versions_get_device_index(uint16_t device_id)
+{
+	unsigned int i;
+	for (i = 0; i < ME1400_DEVICE_VERSIONS; i++)
+		if (me1400_versions[i].device_id == device_id)
+			break;
+	return i;
+}
+
+#define ME1400_MAX_8254		10	/**< The maximum number of 8254 counter subdevices available on any ME-1400 device. */
+#define ME1400_MAX_8255		2	/**< The maximum number of 8255 digital i/o subdevices available on any ME-1400 device. */
+
+/**
+ * @brief The ME-1400 device class.
+ */
+typedef struct me1400_device {
+	me_device_t base;									/**< The Meilhaus device base class. */
+
+	spinlock_t clk_src_reg_lock;						/**< Guards the 8254 clock source registers. */
+	spinlock_t ctr_ctrl_reg_lock[ME1400_MAX_8254];		/**< Guards the 8254 ctrl registers. */
+
+	int dio_current_mode[ME1400_MAX_8255];				/**< Saves the current mode setting of a single 8255 DIO chip. */
+	spinlock_t dio_ctrl_reg_lock[ME1400_MAX_8255];		/**< Guards the 8255 ctrl register and #dio_current_mode. */
+} me1400_device_t;
+
+/**
+ * @brief The ME-1400 device class constructor.
+ *
+ * @param pci_device The pci device structure given by the PCI subsystem.
+ *
+ * @return On succes a new ME-1400 device instance. \n
+ *         NULL on error.
+ */
+me_device_t *me1400_pci_constructor(struct pci_dev *pci_device)
+    __attribute__ ((weak));
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me1400_ext_irq.c b/drivers/staging/meilhaus/me1400_ext_irq.c
new file mode 100644
index 00000000000..b8c2696bc15
--- /dev/null
+++ b/drivers/staging/meilhaus/me1400_ext_irq.c
@@ -0,0 +1,517 @@
+/**
+ * @file me1400_ext_irq.c
+ *
+ * @brief ME-1400 external interrupt subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/version.h>
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+#include <linux/interrupt.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+#include "medebug.h"
+#include "meids.h"
+
+#include "me1400_ext_irq.h"
+#include "me1400_ext_irq_reg.h"
+
+/*
+ * Defines
+ */
+#define ME1400_EXT_IRQ_MAGIC_NUMBER	0x1401	/**< The magic number of the class structure. */
+#define ME1400_EXT_IRQ_NUMBER_CHANNELS 1	/**< One channel per counter. */
+
+/*
+ * Functions
+ */
+
+static int me1400_ext_irq_io_irq_start(struct me_subdevice *subdevice,
+				       struct file *filep,
+				       int channel,
+				       int irq_source,
+				       int irq_edge, int irq_arg, int flags)
+{
+	me1400_ext_irq_subdevice_t *instance;
+	unsigned long cpu_flags;
+	uint8_t tmp;
+
+	PDEBUG("executed.\n");
+
+	instance = (me1400_ext_irq_subdevice_t *) subdevice;
+
+	if (flags & ~ME_IO_IRQ_START_DIO_BIT) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	if (irq_source != ME_IRQ_SOURCE_DIO_LINE) {
+		PERROR("Invalid irq source.\n");
+		return ME_ERRNO_INVALID_IRQ_SOURCE;
+	}
+
+	if (irq_edge != ME_IRQ_EDGE_RISING) {
+		PERROR("Invalid irq edge.\n");
+		return ME_ERRNO_INVALID_IRQ_EDGE;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+
+	spin_lock(instance->clk_src_reg_lock);
+//                      // Enable IRQ on PLX
+//                      tmp = inb(instance->plx_intcs_reg) | (PLX_LOCAL_INT1_EN | PLX_LOCAL_INT1_POL | PLX_PCI_INT_EN);
+//                      outb(tmp, instance->plx_intcs_reg);
+//                      PDEBUG_REG("ctrl_reg outb(PLX:0x%lX)=0x%x\n", instance->plx_intcs_reg, tmp);
+
+	// Enable IRQ
+	switch (instance->device_id) {
+	case PCI_DEVICE_ID_MEILHAUS_ME140C:
+	case PCI_DEVICE_ID_MEILHAUS_ME140D:
+		tmp = inb(instance->ctrl_reg);
+		tmp |= ME1400CD_EXT_IRQ_CLK_EN;
+		outb(tmp, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, tmp);
+		break;
+
+	default:
+		outb(ME1400AB_EXT_IRQ_IRQ_EN, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base,
+			   ME1400AB_EXT_IRQ_IRQ_EN);
+		break;
+	}
+	spin_unlock(instance->clk_src_reg_lock);
+	instance->rised = 0;
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me1400_ext_irq_io_irq_wait(struct me_subdevice *subdevice,
+				      struct file *filep,
+				      int channel,
+				      int *irq_count,
+				      int *value, int time_out, int flags)
+{
+	me1400_ext_irq_subdevice_t *instance;
+	unsigned long cpu_flags;
+	long t = 0;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me1400_ext_irq_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid time out.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	if (time_out) {
+		/* Convert to ticks */
+		t = (time_out * HZ) / 1000;
+
+		if (t == 0)
+			t = 1;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	if (instance->rised <= 0) {
+		instance->rised = 0;
+		if (time_out) {
+			t = wait_event_interruptible_timeout(instance->
+							     wait_queue,
+							     (instance->rised !=
+							      0), t);
+
+			if (t == 0) {
+				PERROR("Wait on interrupt timed out.\n");
+				err = ME_ERRNO_TIMEOUT;
+			}
+		} else {
+			wait_event_interruptible(instance->wait_queue,
+						 (instance->rised != 0));
+		}
+
+		if (instance->rised < 0) {
+			PERROR("Wait on interrupt aborted by user.\n");
+			err = ME_ERRNO_CANCELLED;
+		}
+	}
+
+	if (signal_pending(current)) {
+		PERROR("Wait on interrupt aborted by signal.\n");
+		err = ME_ERRNO_SIGNAL;
+	}
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	instance->rised = 0;
+	*irq_count = instance->n;
+	*value = 1;
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me1400_ext_irq_io_irq_stop(struct me_subdevice *subdevice,
+				      struct file *filep,
+				      int channel, int flags)
+{
+	me1400_ext_irq_subdevice_t *instance;
+	unsigned long cpu_flags;
+	uint8_t tmp;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me1400_ext_irq_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	spin_lock(instance->clk_src_reg_lock);
+//                      // Disable IRQ on PLX
+//                      tmp = inb(instance->plx_intcs_reg) & ( ~(PLX_LOCAL_INT1_EN | PLX_LOCAL_INT1_POL | PLX_PCI_INT_EN));
+//                      outb(tmp, instance->plx_intcs_reg);
+//                      PDEBUG_REG("ctrl_reg outb(PLX:0x%lX)=0x%x\n", instance->plx_intcs_reg, tmp);
+
+	switch (instance->device_id) {
+	case PCI_DEVICE_ID_MEILHAUS_ME140C:
+	case PCI_DEVICE_ID_MEILHAUS_ME140D:
+		tmp = inb(instance->ctrl_reg);
+		tmp &= ~ME1400CD_EXT_IRQ_CLK_EN;
+		outb(tmp, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, tmp);
+
+		break;
+
+	default:
+		outb(0x00, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, 0x00);
+		break;
+	}
+	spin_unlock(instance->clk_src_reg_lock);
+	instance->rised = -1;
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me1400_ext_irq_io_reset_subdevice(struct me_subdevice *subdevice,
+					     struct file *filep, int flags)
+{
+	me1400_ext_irq_subdevice_t *instance =
+	    (me1400_ext_irq_subdevice_t *) subdevice;
+
+	PDEBUG("executed.\n");
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	instance->n = 0;
+	return me1400_ext_irq_io_irq_stop(subdevice, filep, 0, flags);
+}
+
+static int me1400_ext_irq_query_number_channels(struct me_subdevice *subdevice,
+						int *number)
+{
+	PDEBUG("executed.\n");
+	*number = ME1400_EXT_IRQ_NUMBER_CHANNELS;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me1400_ext_irq_query_subdevice_type(struct me_subdevice *subdevice,
+					       int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_EXT_IRQ;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me1400_ext_irq_query_subdevice_caps(struct me_subdevice *subdevice,
+					       int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = ME_CAPS_EXT_IRQ_EDGE_RISING;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me1400_ext_irq_query_subdevice_caps_args(struct me_subdevice
+						    *subdevice, int cap,
+						    int *args, int count)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 19)
+static irqreturn_t me1400_ext_irq_isr(int irq, void *dev_id)
+#else
+static irqreturn_t me1400_ext_irq_isr(int irq, void *dev_id,
+				      struct pt_regs *regs)
+#endif
+{
+	me1400_ext_irq_subdevice_t *instance;
+	uint32_t status;
+	uint8_t tmp;
+
+	instance = (me1400_ext_irq_subdevice_t *) dev_id;
+
+	if (irq != instance->irq) {
+		PERROR("Incorrect interrupt num: %d.\n", irq);
+		return IRQ_NONE;
+	}
+
+	spin_lock(&instance->subdevice_lock);
+	status = inl(instance->plx_intcs_reg);
+//              if (!((status & PLX_LOCAL_INT1_STATE) && (status & PLX_LOCAL_INT1_EN) && (status & PLX_PCI_INT_EN)))
+	if ((status &
+	     (PLX_LOCAL_INT1_STATE | PLX_LOCAL_INT1_EN | PLX_PCI_INT_EN)) !=
+	    (PLX_LOCAL_INT1_STATE | PLX_LOCAL_INT1_EN | PLX_PCI_INT_EN)) {
+		spin_unlock(&instance->subdevice_lock);
+		PINFO("%ld Shared interrupt. %s(): irq_status_reg=0x%04X\n",
+		      jiffies, __FUNCTION__, status);
+		return IRQ_NONE;
+	}
+
+	inl(instance->ctrl_reg);
+
+	PDEBUG("executed.\n");
+
+	instance->n++;
+	instance->rised = 1;
+
+	switch (instance->device_id) {
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140C:
+	case PCI_DEVICE_ID_MEILHAUS_ME140D:
+		spin_lock(instance->clk_src_reg_lock);
+		tmp = inb(instance->ctrl_reg);
+		tmp &= ~ME1400CD_EXT_IRQ_CLK_EN;
+		outb(tmp, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outb(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, tmp);
+		tmp |= ME1400CD_EXT_IRQ_CLK_EN;
+		outb(tmp, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outb(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, tmp);
+		spin_unlock(instance->clk_src_reg_lock);
+
+		break;
+
+	default:
+		outb(0, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outb(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, 0);
+		outb(ME1400AB_EXT_IRQ_IRQ_EN, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outb(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base,
+			   ME1400AB_EXT_IRQ_IRQ_EN);
+		break;
+	}
+
+	spin_unlock(&instance->subdevice_lock);
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	return IRQ_HANDLED;
+}
+
+static void me1400_ext_irq_destructor(struct me_subdevice *subdevice)
+{
+	me1400_ext_irq_subdevice_t *instance;
+	uint8_t tmp;
+
+	PDEBUG("executed.\n");
+
+	instance = (me1400_ext_irq_subdevice_t *) subdevice;
+
+	// Disable IRQ on PLX
+	tmp =
+	    inb(instance->
+		plx_intcs_reg) & (~(PLX_LOCAL_INT1_EN | PLX_LOCAL_INT1_POL |
+				    PLX_PCI_INT_EN));
+	outb(tmp, instance->plx_intcs_reg);
+	PDEBUG_REG("ctrl_reg outb(plx:0x%lX)=0x%x\n", instance->plx_intcs_reg,
+		   tmp);
+
+	free_irq(instance->irq, (void *)instance);
+	me_subdevice_deinit(&instance->base);
+	kfree(instance);
+}
+
+me1400_ext_irq_subdevice_t *me1400_ext_irq_constructor(uint32_t device_id,
+						       uint32_t plx_reg_base,
+						       uint32_t me1400_reg_base,
+						       spinlock_t *
+						       clk_src_reg_lock,
+						       int irq)
+{
+	me1400_ext_irq_subdevice_t *subdevice;
+	int err;
+	uint8_t tmp;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me1400_ext_irq_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for 1400_ext_irq instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me1400_ext_irq_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+	subdevice->clk_src_reg_lock = clk_src_reg_lock;
+
+	/* Initialize wait queue */
+	init_waitqueue_head(&subdevice->wait_queue);
+
+	subdevice->irq = irq;
+
+	err = request_irq(irq, me1400_ext_irq_isr,
+#ifdef IRQF_DISABLED
+			  IRQF_DISABLED | IRQF_SHARED,
+#else
+			  SA_INTERRUPT | SA_SHIRQ,
+#endif
+			  ME1400_NAME, (void *)subdevice);
+
+	if (err) {
+		PERROR("Can't get irq.\n");
+		me_subdevice_deinit(&subdevice->base);
+		kfree(subdevice);
+		return NULL;
+	}
+	PINFO("Registered irq=%d.\n", subdevice->irq);
+
+	/* Initialize registers */
+	subdevice->plx_intcs_reg = plx_reg_base + PLX_INTCSR_REG;
+	subdevice->ctrl_reg = me1400_reg_base + ME1400AB_EXT_IRQ_CTRL_REG;
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = me1400_reg_base;
+#endif
+
+	// Enable IRQ on PLX
+	tmp =
+	    inb(subdevice->
+		plx_intcs_reg) | (PLX_LOCAL_INT1_EN | PLX_LOCAL_INT1_POL |
+				  PLX_PCI_INT_EN);
+	outb(tmp, subdevice->plx_intcs_reg);
+	PDEBUG_REG("ctrl_reg outb(Pplx:0x%lX)=0x%x\n", subdevice->plx_intcs_reg,
+		   tmp);
+
+	/* Initialize the subdevice methods */
+	subdevice->base.me_subdevice_io_irq_start = me1400_ext_irq_io_irq_start;
+	subdevice->base.me_subdevice_io_irq_wait = me1400_ext_irq_io_irq_wait;
+	subdevice->base.me_subdevice_io_irq_stop = me1400_ext_irq_io_irq_stop;
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me1400_ext_irq_io_reset_subdevice;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me1400_ext_irq_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me1400_ext_irq_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me1400_ext_irq_query_subdevice_caps;
+	subdevice->base.me_subdevice_query_subdevice_caps_args =
+	    me1400_ext_irq_query_subdevice_caps_args;
+	subdevice->base.me_subdevice_destructor = me1400_ext_irq_destructor;
+
+	subdevice->rised = 0;
+	subdevice->n = 0;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me1400_ext_irq.h b/drivers/staging/meilhaus/me1400_ext_irq.h
new file mode 100644
index 00000000000..9b72a04701c
--- /dev/null
+++ b/drivers/staging/meilhaus/me1400_ext_irq.h
@@ -0,0 +1,62 @@
+/**
+ * @file me1400_ext_irq.h
+ *
+ * @brief ME-1400 external interrupt implementation.
+ * @note Copyright (C) 2006 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+#ifndef _ME1400_EXT_IRQ_H_
+#define _ME1400_EXT_IRQ_H_
+
+#include <linux/sched.h>
+
+#include "mesubdevice.h"
+#include "meslock.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The ME-1400 external interrupt subdevice class.
+ */
+typedef struct me1400_ext_irq_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *clk_src_reg_lock;	/**< Lock protecting the clock control register. */
+
+	wait_queue_head_t wait_queue;	/**< Queue to put on threads waiting for an interrupt. */
+
+	uint32_t device_id;				/**< The device id of the device holding the subdevice. */
+	int irq;						/**< The irq number assigned by PCI BIOS. */
+	int rised;						/**< If true an interrupt has occured. */
+	unsigned int n;					/**< The number of interrupt since the driver was loaded. */
+
+	unsigned long plx_intcs_reg;	/**< The PLX interrupt control and status register. */
+	unsigned long ctrl_reg;			/**< The control register. */
+#ifdef MEDEBUG_DEBUG_REG
+	unsigned long reg_base;
+#endif
+} me1400_ext_irq_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-1400 external interrupt instance.
+ *
+ * @param plx_reg_base The register base address of the PLX chip as returned by the PCI BIOS.
+ * @param me1400_reg_base The register base address of the ME-1400 device as returned by the PCI BIOS.
+ * @param irq The irq assigned by the PCI BIOS.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me1400_ext_irq_subdevice_t *me1400_ext_irq_constructor(uint32_t device_id,
+						       uint32_t plx_reg_base,
+						       uint32_t me1400_reg_base,
+						       spinlock_t *
+						       clk_src_reg_lock,
+						       int irq);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me1400_ext_irq_reg.h b/drivers/staging/meilhaus/me1400_ext_irq_reg.h
new file mode 100644
index 00000000000..c9740f2dd3a
--- /dev/null
+++ b/drivers/staging/meilhaus/me1400_ext_irq_reg.h
@@ -0,0 +1,56 @@
+/**
+ * @file me1400_ext_irq_reg.h
+ *
+ * @brief ME-1400 external interrupt register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME1400_EXT_IRQ_REG_H_
+# define _ME1400_EXT_IRQ_REG_H_
+
+# ifdef __KERNEL__
+
+#  define PLX_INTCSR_REG				0x4C	/**< The PLX interrupt control and status register offset. */
+#  define PLX_ICR_REG					0x50	/**< The PLX initialization control register offset. */
+
+#  define PLX_LOCAL_INT1_EN				0x01	/**< If set the local interrupt 1 is enabled. */
+#  define PLX_LOCAL_INT1_POL			0x02	/**< If set the local interrupt 1 polarity is high active. */
+#  define PLX_LOCAL_INT1_STATE			0x04	/**< If set the local interrupt 1 is activ. */
+#  define PLX_LOCAL_INT2_EN				0x08	/**< If set the local interrupt 2 is enabled. */
+#  define PLX_LOCAL_INT2_POL			0x10	/**< If set the local interrupt 2 polarity is high active. */
+#  define PLX_LOCAL_INT2_STATE			0x20	/**< If set the local interrupt 2 is activ. */
+#  define PLX_PCI_INT_EN				0x40	/**< If set the PCI interrupt is enabled. */
+#  define PLX_SOFT_INT					0x80	/**< If set an interrupt is generated. */
+
+#  define ME1400AB_EXT_IRQ_CTRL_REG		0x11	/**< The external interrupt control register offset. */
+
+#  define ME1400AB_EXT_IRQ_CLK_EN		0x01	/**< If this bit is set, the clock output is enabled. */
+#  define ME1400AB_EXT_IRQ_IRQ_EN		0x02	/**< If set the external interrupt is enabled. Clearing this bit clears a pending interrupt. */
+
+#  define ME1400CD_EXT_IRQ_CTRL_REG		0x11	/**< The external interrupt control register offset. */
+
+#  define ME1400CD_EXT_IRQ_CLK_EN		0x10	/**< If set the external interrupt is enabled. Clearing this bit clears a pending interrupt.*/
+
+# endif	//__KERNEL__
+
+#endif //_ME1400_EXT_IRQ_REG_H_
diff --git a/drivers/staging/meilhaus/me1600_ao.c b/drivers/staging/meilhaus/me1600_ao.c
new file mode 100644
index 00000000000..6f26665b30b
--- /dev/null
+++ b/drivers/staging/meilhaus/me1600_ao.c
@@ -0,0 +1,1033 @@
+/**
+ * @file me1600_ao.c
+ *
+ * @brief ME-1600 analog output subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/* Includes
+ */
+
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+#include <linux/sched.h>
+
+#include <linux/workqueue.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+#include "medebug.h"
+
+#include "me1600_ao_reg.h"
+#include "me1600_ao.h"
+
+/* Defines
+ */
+
+static void me1600_ao_destructor(struct me_subdevice *subdevice);
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+static void me1600_ao_work_control_task(void *subdevice);
+#else
+static void me1600_ao_work_control_task(struct work_struct *work);
+#endif
+
+static int me1600_ao_io_reset_subdevice(me_subdevice_t * subdevice,
+					struct file *filep, int flags);
+static int me1600_ao_io_single_config(me_subdevice_t * subdevice,
+				      struct file *filep, int channel,
+				      int single_config, int ref, int trig_chan,
+				      int trig_type, int trig_edge, int flags);
+static int me1600_ao_io_single_read(me_subdevice_t * subdevice,
+				    struct file *filep, int channel, int *value,
+				    int time_out, int flags);
+static int me1600_ao_io_single_write(me_subdevice_t * subdevice,
+				     struct file *filep, int channel, int value,
+				     int time_out, int flags);
+static int me1600_ao_query_number_channels(me_subdevice_t * subdevice,
+					   int *number);
+static int me1600_ao_query_subdevice_type(me_subdevice_t * subdevice, int *type,
+					  int *subtype);
+static int me1600_ao_query_subdevice_caps(me_subdevice_t * subdevice,
+					  int *caps);
+static int me1600_ao_query_range_by_min_max(me_subdevice_t * subdevice,
+					    int unit, int *min, int *max,
+					    int *maxdata, int *range);
+static int me1600_ao_query_number_ranges(me_subdevice_t * subdevice, int unit,
+					 int *count);
+static int me1600_ao_query_range_info(me_subdevice_t * subdevice, int range,
+				      int *unit, int *min, int *max,
+				      int *maxdata);
+
+/* Functions
+ */
+
+me1600_ao_subdevice_t *me1600_ao_constructor(uint32_t reg_base,
+					     unsigned int ao_idx,
+					     int curr,
+					     spinlock_t * config_regs_lock,
+					     spinlock_t * ao_shadows_lock,
+					     me1600_ao_shadow_t *
+					     ao_regs_shadows,
+					     struct workqueue_struct *me1600_wq)
+{
+	me1600_ao_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed. idx=%d\n", ao_idx);
+
+	// Allocate memory for subdevice instance.
+	subdevice = kmalloc(sizeof(me1600_ao_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR
+		    ("Cannot get memory for analog output subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me1600_ao_subdevice_t));
+
+	// Initialize subdevice base class.
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+	subdevice->config_regs_lock = config_regs_lock;
+	subdevice->ao_shadows_lock = ao_shadows_lock;
+
+	// Save the subdevice index.
+	subdevice->ao_idx = ao_idx;
+
+	// Initialize range lists.
+	subdevice->u_ranges_count = 2;
+
+	subdevice->u_ranges[0].min = 0;	//0V
+	subdevice->u_ranges[0].max = 9997558;	//10V
+
+	subdevice->u_ranges[1].min = -10E6;	//-10V
+	subdevice->u_ranges[1].max = 9995117;	//10V
+
+	if (curr) {		// This is version with current outputs.
+		subdevice->i_ranges_count = 2;
+
+		subdevice->i_ranges[0].min = 0;	//0mA
+		subdevice->i_ranges[0].max = 19995117;	//20mA
+
+		subdevice->i_ranges[1].min = 4E3;	//4mA
+		subdevice->i_ranges[1].max = 19995118;	//20mA
+	} else {		// This is version without current outputs.
+		subdevice->i_ranges_count = 0;
+
+		subdevice->i_ranges[0].min = 0;	//0mA
+		subdevice->i_ranges[0].max = 0;	//0mA
+
+		subdevice->i_ranges[1].min = 0;	//0mA
+		subdevice->i_ranges[1].max = 0;	//0mA
+	}
+
+	// Initialize registers.
+	subdevice->uni_bi_reg = reg_base + ME1600_UNI_BI_REG;
+	subdevice->i_range_reg = reg_base + ME1600_020_420_REG;
+	subdevice->sim_output_reg = reg_base + ME1600_SIM_OUTPUT_REG;
+	subdevice->current_on_reg = reg_base + ME1600_CURRENT_ON_REG;
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = reg_base;
+#endif
+
+	// Initialize shadow structure.
+	subdevice->ao_regs_shadows = ao_regs_shadows;
+
+	// Override base class methods.
+	subdevice->base.me_subdevice_destructor = me1600_ao_destructor;
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me1600_ao_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me1600_ao_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me1600_ao_io_single_read;
+	subdevice->base.me_subdevice_io_single_write =
+	    me1600_ao_io_single_write;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me1600_ao_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me1600_ao_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me1600_ao_query_subdevice_caps;
+	subdevice->base.me_subdevice_query_range_by_min_max =
+	    me1600_ao_query_range_by_min_max;
+	subdevice->base.me_subdevice_query_number_ranges =
+	    me1600_ao_query_number_ranges;
+	subdevice->base.me_subdevice_query_range_info =
+	    me1600_ao_query_range_info;
+
+	// Initialize wait queue.
+	init_waitqueue_head(&subdevice->wait_queue);
+
+	// Prepare work queue.
+	subdevice->me1600_workqueue = me1600_wq;
+
+/* workqueue API changed in kernel 2.6.20 */
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20) )
+	INIT_WORK(&subdevice->ao_control_task, me1600_ao_work_control_task,
+		  (void *)subdevice);
+#else
+	INIT_DELAYED_WORK(&subdevice->ao_control_task,
+			  me1600_ao_work_control_task);
+#endif
+	return subdevice;
+}
+
+static void me1600_ao_destructor(struct me_subdevice *subdevice)
+{
+	me1600_ao_subdevice_t *instance;
+
+	instance = (me1600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	instance->ao_control_task_flag = 0;
+
+	// Reset subdevice to asure clean exit.
+	me1600_ao_io_reset_subdevice(subdevice, NULL,
+				     ME_IO_RESET_SUBDEVICE_NO_FLAGS);
+
+	// Remove any tasks from work queue. This is paranoic because it was done allready in reset().
+	if (!cancel_delayed_work(&instance->ao_control_task)) {	//Wait 2 ticks to be sure that control task is removed from queue.
+		set_current_state(TASK_INTERRUPTIBLE);
+		schedule_timeout(2);
+	}
+}
+
+static int me1600_ao_io_reset_subdevice(me_subdevice_t * subdevice,
+					struct file *filep, int flags)
+{
+	me1600_ao_subdevice_t *instance;
+	uint16_t tmp;
+
+	instance = (me1600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	//Cancel control task
+	PDEBUG("Cancel control task. idx=%d\n", instance->ao_idx);
+	instance->ao_control_task_flag = 0;
+	cancel_delayed_work(&instance->ao_control_task);
+	(instance->ao_regs_shadows)->trigger &= ~(0x1 << instance->ao_idx);	//Cancell waiting for trigger.
+
+	// Reset all settings.
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ao_shadows_lock);
+	(instance->ao_regs_shadows)->shadow[instance->ao_idx] = 0;
+	(instance->ao_regs_shadows)->mirror[instance->ao_idx] = 0;
+	(instance->ao_regs_shadows)->trigger &= ~(0x1 << instance->ao_idx);	//Not waiting for triggering.
+	(instance->ao_regs_shadows)->synchronous &= ~(0x1 << instance->ao_idx);	//Individual triggering.
+
+	// Set output to default (safe) state.
+	spin_lock(instance->config_regs_lock);
+	tmp = inw(instance->uni_bi_reg);	// unipolar
+	tmp |= (0x1 << instance->ao_idx);
+	outw(tmp, instance->uni_bi_reg);
+	PDEBUG_REG("uni_bi_reg outw(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->uni_bi_reg - instance->reg_base, tmp);
+
+	tmp = inw(instance->current_on_reg);	// Volts only!
+	tmp &= ~(0x1 << instance->ao_idx);
+	tmp &= 0x00FF;
+	outw(tmp, instance->current_on_reg);
+	PDEBUG_REG("current_on_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->current_on_reg - instance->reg_base, tmp);
+
+	tmp = inw(instance->i_range_reg);	// 0..20mA <= If exists.
+	tmp &= ~(0x1 << instance->ao_idx);
+	outw(tmp, instance->i_range_reg);
+	PDEBUG_REG("i_range_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->i_range_reg - instance->reg_base, tmp);
+
+	outw(0, (instance->ao_regs_shadows)->registry[instance->ao_idx]);
+	PDEBUG_REG("channel_reg outw(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   (instance->ao_regs_shadows)->registry[instance->ao_idx] -
+		   instance->reg_base, 0);
+
+	// Trigger output.
+	outw(0x0000, instance->sim_output_reg);
+	PDEBUG_REG("sim_output_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->sim_output_reg - instance->reg_base, 0x0000);
+	outw(0xFFFF, instance->sim_output_reg);
+	PDEBUG_REG("sim_output_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->sim_output_reg - instance->reg_base, 0xFFFF);
+	spin_unlock(instance->config_regs_lock);
+	spin_unlock(instance->ao_shadows_lock);
+
+	// Set status to 'none'
+	instance->status = ao_status_none;
+	spin_unlock(&instance->subdevice_lock);
+
+	//Signal reset if user is on wait.
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me1600_ao_io_single_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int single_config,
+				      int ref,
+				      int trig_chan,
+				      int trig_type, int trig_edge, int flags)
+{
+	me1600_ao_subdevice_t *instance;
+	uint16_t tmp;
+
+	instance = (me1600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	// Checking parameters.
+	if (flags) {
+		PERROR
+		    ("Invalid flag specified. Must be ME_IO_SINGLE_CONFIG_NO_FLAGS.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (trig_edge != ME_TRIG_EDGE_NONE) {
+		PERROR
+		    ("Invalid trigger edge. Software trigger has not edge. Must be ME_TRIG_EDGE_NONE\n");
+		return ME_ERRNO_INVALID_TRIG_EDGE;
+	}
+
+	if (trig_type != ME_TRIG_TYPE_SW) {
+		PERROR("Invalid trigger edge. Must be ME_TRIG_TYPE_SW.\n");
+		return ME_ERRNO_INVALID_TRIG_TYPE;
+	}
+
+	if ((trig_chan != ME_TRIG_CHAN_DEFAULT)
+	    && (trig_chan != ME_TRIG_CHAN_SYNCHRONOUS)) {
+		PERROR("Invalid trigger channel specified.\n");
+		return ME_ERRNO_INVALID_TRIG_CHAN;
+	}
+
+	if (ref != ME_REF_AO_GROUND) {
+		PERROR
+		    ("Invalid reference. Analog outputs have to have got REF_AO_GROUND.\n");
+		return ME_ERRNO_INVALID_REF;
+	}
+
+	if (((single_config + 1) >
+	     (instance->u_ranges_count + instance->i_ranges_count))
+	    || (single_config < 0)) {
+		PERROR("Invalid range specified.\n");
+		return ME_ERRNO_INVALID_SINGLE_CONFIG;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+	// Checking parameters - done. All is fine. Do config.
+
+	ME_SUBDEVICE_ENTER;
+
+	//Cancel control task
+	PDEBUG("Cancel control task. idx=%d\n", instance->ao_idx);
+	instance->ao_control_task_flag = 0;
+	cancel_delayed_work(&instance->ao_control_task);
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ao_shadows_lock);
+	(instance->ao_regs_shadows)->trigger &= ~(0x1 << instance->ao_idx);	//Cancell waiting for trigger.
+	(instance->ao_regs_shadows)->shadow[instance->ao_idx] = 0;
+	(instance->ao_regs_shadows)->mirror[instance->ao_idx] = 0;
+
+	spin_lock(instance->config_regs_lock);
+	switch (single_config) {
+	case 0:		// 0V 10V
+		tmp = inw(instance->current_on_reg);	// Volts
+		tmp &= ~(0x1 << instance->ao_idx);
+		outw(tmp, instance->current_on_reg);
+		PDEBUG_REG("current_on_reg outw(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->current_on_reg - instance->reg_base, tmp);
+
+		// 0V
+		outw(0,
+		     (instance->ao_regs_shadows)->registry[instance->ao_idx]);
+		PDEBUG_REG("channel_reg outw(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   (instance->ao_regs_shadows)->registry[instance->
+								 ao_idx] -
+			   instance->reg_base, 0);
+
+		tmp = inw(instance->uni_bi_reg);	// unipolar
+		tmp |= (0x1 << instance->ao_idx);
+		outw(tmp, instance->uni_bi_reg);
+		PDEBUG_REG("uni_bi_reg outw(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->uni_bi_reg - instance->reg_base, tmp);
+
+		tmp = inw(instance->i_range_reg);	// 0..20mA <= If exists.
+		tmp &= ~(0x1 << instance->ao_idx);
+		outw(tmp, instance->i_range_reg);
+		PDEBUG_REG("i_range_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->i_range_reg - instance->reg_base, tmp);
+		break;
+
+	case 1:		// -10V 10V
+		tmp = inw(instance->current_on_reg);	// Volts
+		tmp &= ~(0x1 << instance->ao_idx);
+		outw(tmp, instance->current_on_reg);
+		PDEBUG_REG("current_on_reg outw(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->current_on_reg - instance->reg_base, tmp);
+
+		// 0V
+		outw(0x0800,
+		     (instance->ao_regs_shadows)->registry[instance->ao_idx]);
+		PDEBUG_REG("channel_reg outw(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   (instance->ao_regs_shadows)->registry[instance->
+								 ao_idx] -
+			   instance->reg_base, 0x0800);
+
+		tmp = inw(instance->uni_bi_reg);	// bipolar
+		tmp &= ~(0x1 << instance->ao_idx);
+		outw(tmp, instance->uni_bi_reg);
+		PDEBUG_REG("uni_bi_reg outw(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->uni_bi_reg - instance->reg_base, tmp);
+
+		tmp = inw(instance->i_range_reg);	// 0..20mA <= If exists.
+		tmp &= ~(0x1 << instance->ao_idx);
+		outw(tmp, instance->i_range_reg);
+		PDEBUG_REG("i_range_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->i_range_reg - instance->reg_base, tmp);
+		break;
+
+	case 2:		// 0mA 20mA
+		tmp = inw(instance->current_on_reg);	// mAmpers
+		tmp |= (0x1 << instance->ao_idx);
+		outw(tmp, instance->current_on_reg);
+		PDEBUG_REG("current_on_reg outw(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->current_on_reg - instance->reg_base, tmp);
+
+		tmp = inw(instance->i_range_reg);	// 0..20mA
+		tmp &= ~(0x1 << instance->ao_idx);
+		outw(tmp, instance->i_range_reg);
+		PDEBUG_REG("i_range_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->i_range_reg - instance->reg_base, tmp);
+
+		// 0mA
+		outw(0,
+		     (instance->ao_regs_shadows)->registry[instance->ao_idx]);
+		PDEBUG_REG("channel_reg outw(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   (instance->ao_regs_shadows)->registry[instance->
+								 ao_idx] -
+			   instance->reg_base, 0);
+
+		tmp = inw(instance->uni_bi_reg);	// unipolar
+		tmp |= (0x1 << instance->ao_idx);
+		outw(tmp, instance->uni_bi_reg);
+		PDEBUG_REG("uni_bi_reg outw(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->uni_bi_reg - instance->reg_base, tmp);
+		break;
+
+	case 3:		// 4mA 20mA
+		tmp = inw(instance->current_on_reg);	// mAmpers
+		tmp |= (0x1 << instance->ao_idx);
+		outw(tmp, instance->current_on_reg);
+		PDEBUG_REG("current_on_reg outw(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->current_on_reg - instance->reg_base, tmp);
+
+		tmp = inw(instance->i_range_reg);	// 4..20mA
+		tmp |= (0x1 << instance->ao_idx);
+		outw(tmp, instance->i_range_reg);
+		PDEBUG_REG("i_range_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->i_range_reg - instance->reg_base, tmp);
+
+		// 4mA
+		outw(0,
+		     (instance->ao_regs_shadows)->registry[instance->ao_idx]);
+		PDEBUG_REG("channel_reg outw(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   (instance->ao_regs_shadows)->registry[instance->
+								 ao_idx] -
+			   instance->reg_base, 0);
+
+		tmp = inw(instance->uni_bi_reg);	// unipolar
+		tmp |= (0x1 << instance->ao_idx);
+		outw(tmp, instance->uni_bi_reg);
+		PDEBUG_REG("uni_bi_reg outw(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->uni_bi_reg - instance->reg_base, tmp);
+		break;
+	}
+
+	// Trigger output.
+	outw(0x0000, instance->sim_output_reg);
+	PDEBUG_REG("sim_output_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->sim_output_reg - instance->reg_base, 0x0000);
+	outw(0xFFFF, instance->sim_output_reg);
+	PDEBUG_REG("sim_output_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->sim_output_reg - instance->reg_base, 0xFFFF);
+
+	if (trig_chan == ME_TRIG_CHAN_DEFAULT) {	// Individual triggering.
+		(instance->ao_regs_shadows)->synchronous &=
+		    ~(0x1 << instance->ao_idx);
+		PDEBUG("Individual triggering.\n");
+	} else if (trig_chan == ME_TRIG_CHAN_SYNCHRONOUS) {	// Synchronous triggering.
+		(instance->ao_regs_shadows)->synchronous |=
+		    (0x1 << instance->ao_idx);
+		PDEBUG("Synchronous triggering.\n");
+	}
+	spin_unlock(instance->config_regs_lock);
+	spin_unlock(instance->ao_shadows_lock);
+
+	instance->status = ao_status_single_configured;
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me1600_ao_io_single_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int channel,
+				    int *value, int time_out, int flags)
+{
+	me1600_ao_subdevice_t *instance;
+	unsigned long delay = 0;
+	unsigned long j = 0;
+	int err = ME_ERRNO_SUCCESS;
+
+	instance = (me1600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (flags & ~ME_IO_SINGLE_NONBLOCKING) {
+		PERROR("Invalid flag specified. %d\n", flags);
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid timeout specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	if ((!flags) && ((instance->ao_regs_shadows)->trigger & instance->ao_idx)) {	//Blocking mode. Wait for software trigger.
+		if (time_out) {
+			delay = (time_out * HZ) / 1000;
+			if (delay == 0)
+				delay = 1;
+		}
+
+		j = jiffies;
+
+		//Only runing process will interrupt this call. Events are signaled when status change. This procedure has own timeout.
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 (!((instance->
+						     ao_regs_shadows)->
+						    trigger & instance->
+						    ao_idx)),
+						 (delay) ? delay : LONG_MAX);
+
+		if (instance == ao_status_none) {	// Reset was called.
+			PDEBUG("Single canceled.\n");
+			err = ME_ERRNO_CANCELLED;
+		}
+
+		if (signal_pending(current)) {
+			PERROR("Wait on start of state machine interrupted.\n");
+			err = ME_ERRNO_SIGNAL;
+		}
+
+		if ((delay) && ((jiffies - j) >= delay)) {
+			PDEBUG("Timeout reached.\n");
+			err = ME_ERRNO_TIMEOUT;
+		}
+	}
+
+	*value = (instance->ao_regs_shadows)->mirror[instance->ao_idx];
+
+	return err;
+}
+
+static int me1600_ao_io_single_write(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int channel,
+				     int value, int time_out, int flags)
+{
+	me1600_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long delay = 0;
+	int i;
+	unsigned long j = 0;
+
+	instance = (me1600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (flags &
+	    ~(ME_IO_SINGLE_TYPE_TRIG_SYNCHRONOUS |
+	      ME_IO_SINGLE_TYPE_WRITE_NONBLOCKING)) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid timeout specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	if (value & ~ME1600_AO_MAX_DATA) {
+		PERROR("Invalid value provided.\n");
+		return ME_ERRNO_VALUE_OUT_OF_RANGE;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	//Cancel control task
+	PDEBUG("Cancel control task. idx=%d\n", instance->ao_idx);
+	instance->ao_control_task_flag = 0;
+	cancel_delayed_work(&instance->ao_control_task);
+	(instance->ao_regs_shadows)->trigger &= ~(0x1 << instance->ao_idx);	//Cancell waiting for trigger.
+
+	if (time_out) {
+		delay = (time_out * HZ) / 1000;
+
+		if (delay == 0)
+			delay = 1;
+	}
+	//Write value.
+	spin_lock(instance->ao_shadows_lock);
+	(instance->ao_regs_shadows)->shadow[instance->ao_idx] =
+	    (uint16_t) value;
+
+	if (flags & ME_IO_SINGLE_TYPE_TRIG_SYNCHRONOUS) {	// Trigger all outputs from synchronous list.
+		for (i = 0; i < (instance->ao_regs_shadows)->count; i++) {
+			if (((instance->ao_regs_shadows)->synchronous & (0x1 << i)) || (i == instance->ao_idx)) {	// Set all from synchronous list to correct state.
+				PDEBUG
+				    ("Synchronous triggering: output %d. idx=%d\n",
+				     i, instance->ao_idx);
+				(instance->ao_regs_shadows)->mirror[i] =
+				    (instance->ao_regs_shadows)->shadow[i];
+
+				outw((instance->ao_regs_shadows)->shadow[i],
+				     (instance->ao_regs_shadows)->registry[i]);
+				PDEBUG_REG
+				    ("channel_reg outw(0x%lX+0x%lX)=0x%x\n",
+				     instance->reg_base,
+				     (instance->ao_regs_shadows)->registry[i] -
+				     instance->reg_base,
+				     (instance->ao_regs_shadows)->shadow[i]);
+
+				(instance->ao_regs_shadows)->trigger &=
+				    ~(0x1 << i);
+			}
+		}
+
+		// Trigger output.
+		outw(0x0000, instance->sim_output_reg);
+		PDEBUG_REG("sim_output_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->sim_output_reg - instance->reg_base, 0);
+		outw(0xFFFF, instance->sim_output_reg);
+		PDEBUG_REG("sim_output_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->sim_output_reg - instance->reg_base,
+			   0xFFFF);
+		instance->status = ao_status_single_end;
+	} else {		// Individual mode.
+		if ((instance->ao_regs_shadows)->synchronous & (0x1 << instance->ao_idx)) {	// Put on synchronous start list. Set output as waiting for trigger.
+			PDEBUG("Add to synchronous list. idx=%d\n",
+			       instance->ao_idx);
+			(instance->ao_regs_shadows)->trigger |=
+			    (0x1 << instance->ao_idx);
+			instance->status = ao_status_single_run;
+			PDEBUG("Synchronous list: 0x%x.\n",
+			       (instance->ao_regs_shadows)->synchronous);
+		} else {	// Fired this one.
+			PDEBUG("Triggering. idx=%d\n", instance->ao_idx);
+			(instance->ao_regs_shadows)->mirror[instance->ao_idx] =
+			    (instance->ao_regs_shadows)->shadow[instance->
+								ao_idx];
+
+			outw((instance->ao_regs_shadows)->
+			     shadow[instance->ao_idx],
+			     (instance->ao_regs_shadows)->registry[instance->
+								   ao_idx]);
+			PDEBUG_REG("channel_reg outw(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   (instance->ao_regs_shadows)->
+				   registry[instance->ao_idx] -
+				   instance->reg_base,
+				   (instance->ao_regs_shadows)->
+				   shadow[instance->ao_idx]);
+
+			// Set output as triggered.
+			(instance->ao_regs_shadows)->trigger &=
+			    ~(0x1 << instance->ao_idx);
+
+			// Trigger output.
+			outw(0x0000, instance->sim_output_reg);
+			PDEBUG_REG("sim_output_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->sim_output_reg -
+				   instance->reg_base, 0);
+			outw(0xFFFF, instance->sim_output_reg);
+			PDEBUG_REG("sim_output_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->sim_output_reg -
+				   instance->reg_base, 0xFFFF);
+			instance->status = ao_status_single_end;
+		}
+	}
+	spin_unlock(instance->ao_shadows_lock);
+
+	//Init control task
+	instance->timeout.delay = delay;
+	instance->timeout.start_time = jiffies;
+	instance->ao_control_task_flag = 1;
+	queue_delayed_work(instance->me1600_workqueue,
+			   &instance->ao_control_task, 1);
+
+	if ((!flags & ME_IO_SINGLE_TYPE_WRITE_NONBLOCKING) && ((instance->ao_regs_shadows)->trigger & instance->ao_idx)) {	//Blocking mode. Wait for software trigger.
+		if (time_out) {
+			delay = (time_out * HZ) / 1000;
+			if (delay == 0)
+				delay = 1;
+		}
+
+		j = jiffies;
+
+		//Only runing process will interrupt this call. Events are signaled when status change. This procedure has own timeout.
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 (!((instance->
+						     ao_regs_shadows)->
+						    trigger & instance->
+						    ao_idx)),
+						 (delay) ? delay : LONG_MAX);
+
+		if (instance == ao_status_none) {
+			PDEBUG("Single canceled.\n");
+			err = ME_ERRNO_CANCELLED;
+		}
+		if (signal_pending(current)) {
+			PERROR("Wait on start of state machine interrupted.\n");
+			err = ME_ERRNO_SIGNAL;
+		}
+
+		if ((delay) && ((jiffies - j) >= delay)) {
+			PDEBUG("Timeout reached.\n");
+			err = ME_ERRNO_TIMEOUT;
+		}
+	}
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me1600_ao_query_number_channels(me_subdevice_t * subdevice,
+					   int *number)
+{
+	me1600_ao_subdevice_t *instance;
+	instance = (me1600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	*number = 1;		//Every subdevice has only 1 channel.
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me1600_ao_query_subdevice_type(me_subdevice_t * subdevice, int *type,
+					  int *subtype)
+{
+	me1600_ao_subdevice_t *instance;
+	instance = (me1600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	*type = ME_TYPE_AO;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me1600_ao_query_subdevice_caps(me_subdevice_t * subdevice, int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = ME_CAPS_AO_TRIG_SYNCHRONOUS;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me1600_ao_query_range_by_min_max(me_subdevice_t * subdevice,
+					    int unit,
+					    int *min,
+					    int *max, int *maxdata, int *range)
+{
+	me1600_ao_subdevice_t *instance;
+	int i;
+	int r = -1;
+	int diff = 21E6;
+
+	instance = (me1600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if ((*max - *min) < 0) {
+		PERROR("Invalid minimum and maximum values specified.\n");
+		return ME_ERRNO_INVALID_MIN_MAX;
+	}
+	// Maximum ranges are slightly less then 10V or 20mA. For convenient we accepted this value as valid one.
+	if (unit == ME_UNIT_VOLT) {
+		for (i = 0; i < instance->u_ranges_count; i++) {
+			if ((instance->u_ranges[i].min <= *min)
+			    && ((instance->u_ranges[i].max + 5000) >= *max)) {
+				if ((instance->u_ranges[i].max -
+				     instance->u_ranges[i].min) - (*max -
+								   *min) <
+				    diff) {
+					r = i;
+					diff =
+					    (instance->u_ranges[i].max -
+					     instance->u_ranges[i].min) -
+					    (*max - *min);
+				}
+			}
+		}
+
+		if (r < 0) {
+			PERROR("No matching range found.\n");
+			return ME_ERRNO_NO_RANGE;
+		} else {
+			*min = instance->u_ranges[r].min;
+			*max = instance->u_ranges[r].max;
+			*range = r;
+		}
+	} else if (unit == ME_UNIT_AMPERE) {
+		for (i = 0; i < instance->i_ranges_count; i++) {
+			if ((instance->i_ranges[i].min <= *min)
+			    && (instance->i_ranges[i].max + 5000 >= *max)) {
+				if ((instance->i_ranges[i].max -
+				     instance->i_ranges[i].min) - (*max -
+								   *min) <
+				    diff) {
+					r = i;
+					diff =
+					    (instance->i_ranges[i].max -
+					     instance->i_ranges[i].min) -
+					    (*max - *min);
+				}
+			}
+		}
+
+		if (r < 0) {
+			PERROR("No matching range found.\n");
+			return ME_ERRNO_NO_RANGE;
+		} else {
+			*min = instance->i_ranges[r].min;
+			*max = instance->i_ranges[r].max;
+			*range = r + instance->u_ranges_count;
+		}
+	} else {
+		PERROR("Invalid physical unit specified.\n");
+		return ME_ERRNO_INVALID_UNIT;
+	}
+	*maxdata = ME1600_AO_MAX_DATA;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me1600_ao_query_number_ranges(me_subdevice_t * subdevice,
+					 int unit, int *count)
+{
+	me1600_ao_subdevice_t *instance;
+
+	PDEBUG("executed.\n");
+
+	instance = (me1600_ao_subdevice_t *) subdevice;
+	switch (unit) {
+	case ME_UNIT_VOLT:
+		*count = instance->u_ranges_count;
+		break;
+	case ME_UNIT_AMPERE:
+		*count = instance->i_ranges_count;
+		break;
+	case ME_UNIT_ANY:
+		*count = instance->u_ranges_count + instance->i_ranges_count;
+		break;
+	default:
+		*count = 0;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me1600_ao_query_range_info(me_subdevice_t * subdevice,
+				      int range,
+				      int *unit,
+				      int *min, int *max, int *maxdata)
+{
+	me1600_ao_subdevice_t *instance;
+
+	PDEBUG("executed.\n");
+
+	instance = (me1600_ao_subdevice_t *) subdevice;
+
+	if (((range + 1) >
+	     (instance->u_ranges_count + instance->i_ranges_count))
+	    || (range < 0)) {
+		PERROR("Invalid range number specified.\n");
+		return ME_ERRNO_INVALID_RANGE;
+	}
+
+	if (range < instance->u_ranges_count) {
+		*unit = ME_UNIT_VOLT;
+		*min = instance->u_ranges[range].min;
+		*max = instance->u_ranges[range].max;
+	} else if (range < instance->u_ranges_count + instance->i_ranges_count) {
+		*unit = ME_UNIT_AMPERE;
+		*min = instance->i_ranges[range - instance->u_ranges_count].min;
+		*max = instance->i_ranges[range - instance->u_ranges_count].max;
+	}
+	*maxdata = ME1600_AO_MAX_DATA;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+static void me1600_ao_work_control_task(void *subdevice)
+#else
+static void me1600_ao_work_control_task(struct work_struct *work)
+#endif
+{
+	me1600_ao_subdevice_t *instance;
+	int reschedule = 1;
+	int signaling = 0;
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+	instance = (me1600_ao_subdevice_t *) subdevice;
+#else
+	instance =
+	    container_of((void *)work, me1600_ao_subdevice_t, ao_control_task);
+#endif
+
+	PINFO("<%s: %ld> executed. idx=%d\n", __FUNCTION__, jiffies,
+	      instance->ao_idx);
+
+	if (!((instance->ao_regs_shadows)->trigger & instance->ao_idx)) {	// Output was triggerd.
+		// Signal the end.
+		signaling = 1;
+		reschedule = 0;
+		if (instance->status == ao_status_single_run) {
+			instance->status = ao_status_single_end;
+		}
+
+	} else if ((instance->timeout.delay) && ((jiffies - instance->timeout.start_time) >= instance->timeout.delay)) {	// Timeout
+		PDEBUG("Timeout reached.\n");
+		spin_lock(instance->ao_shadows_lock);
+		// Restore old settings.
+		PDEBUG("Write old value back to register.\n");
+		(instance->ao_regs_shadows)->shadow[instance->ao_idx] =
+		    (instance->ao_regs_shadows)->mirror[instance->ao_idx];
+
+		outw((instance->ao_regs_shadows)->mirror[instance->ao_idx],
+		     (instance->ao_regs_shadows)->registry[instance->ao_idx]);
+		PDEBUG_REG("channel_reg outw(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   (instance->ao_regs_shadows)->registry[instance->
+								 ao_idx] -
+			   instance->reg_base,
+			   (instance->ao_regs_shadows)->mirror[instance->
+							       ao_idx]);
+
+		//Remove from synchronous strt list.
+		(instance->ao_regs_shadows)->trigger &=
+		    ~(0x1 << instance->ao_idx);
+		if (instance->status == ao_status_none) {
+			instance->status = ao_status_single_end;
+		}
+		spin_unlock(instance->ao_shadows_lock);
+
+		// Signal the end.
+		signaling = 1;
+		reschedule = 0;
+	}
+
+	if (signaling) {	//Signal it.
+		wake_up_interruptible_all(&instance->wait_queue);
+	}
+
+	if (instance->ao_control_task_flag && reschedule) {	// Reschedule task
+		queue_delayed_work(instance->me1600_workqueue,
+				   &instance->ao_control_task, 1);
+	} else {
+		PINFO("<%s> Ending control task.\n", __FUNCTION__);
+	}
+
+}
diff --git a/drivers/staging/meilhaus/me1600_ao.h b/drivers/staging/meilhaus/me1600_ao.h
new file mode 100644
index 00000000000..b82bf5a1676
--- /dev/null
+++ b/drivers/staging/meilhaus/me1600_ao.h
@@ -0,0 +1,132 @@
+/**
+ * @file me1600_ao.h
+ *
+ * @brief Meilhaus ME-1600 analog output subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME1600_AO_H_
+#define _ME1600_AO_H_
+
+# include <linux/version.h>
+# include "mesubdevice.h"
+
+# ifdef __KERNEL__
+
+#  define ME1600_MAX_RANGES	2	/**< Specifies the maximum number of ranges in me1600_ao_subdevice_t::u_ranges und me1600_ao_subdevice_t::i_ranges. */
+
+/**
+ * @brief Defines a entry in the range table.
+ */
+typedef struct me1600_ao_range_entry {
+	int32_t min;
+	int32_t max;
+} me1600_ao_range_entry_t;
+
+typedef struct me1600_ao_timeout {
+	unsigned long start_time;
+	unsigned long delay;
+} me1600_ao_timeout_t;
+
+typedef struct me1600_ao_shadow {
+	int count;
+	unsigned long *registry;
+	uint16_t *shadow;
+	uint16_t *mirror;
+	uint16_t synchronous;									/**< Synchronization list. */
+	uint16_t trigger;										/**< Synchronization flag. */
+} me1600_ao_shadow_t;
+
+typedef enum ME1600_AO_STATUS {
+	ao_status_none = 0,
+	ao_status_single_configured,
+	ao_status_single_run,
+	ao_status_single_end,
+	ao_status_last
+} ME1600_AO_STATUS;
+
+/**
+ * @brief The ME-1600 analog output subdevice class.
+ */
+typedef struct me1600_ao_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;									/**< The subdevice base class. */
+
+	/* Attributes */
+	int ao_idx;												/**< The index of the analog output subdevice on the device. */
+
+	spinlock_t subdevice_lock;								/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *config_regs_lock;							/**< Spin lock to protect configuration registers from concurrent access. */
+
+	int u_ranges_count;										/**< The number of voltage ranges available on this subdevice. */
+	me1600_ao_range_entry_t u_ranges[ME1600_MAX_RANGES];	/**< Array holding the voltage ranges on this subdevice. */
+	int i_ranges_count;										/**< The number of current ranges available on this subdevice. */
+	me1600_ao_range_entry_t i_ranges[ME1600_MAX_RANGES];	/**< Array holding the current ranges on this subdevice. */
+
+	/* Registers */
+	unsigned long uni_bi_reg;								/**< Register for switching between unipoar and bipolar output mode. */
+	unsigned long i_range_reg;								/**< Register for switching between ranges. */
+	unsigned long sim_output_reg;							/**< Register used in order to update all channels simultaneously. */
+	unsigned long current_on_reg;							/**< Register enabling current output on the fourth subdevice. */
+#   ifdef PDEBUG_REG
+	unsigned long reg_base;
+#   endif
+
+	ME1600_AO_STATUS status;
+	me1600_ao_shadow_t *ao_regs_shadows;					/**< Addresses and shadows of output's registers. */
+	spinlock_t *ao_shadows_lock;							/**< Protects the shadow's struct. */
+	int mode;												/**< Mode in witch output should works. */
+	wait_queue_head_t wait_queue;							/**< Wait queue to put on tasks waiting for data to arrive. */
+	me1600_ao_timeout_t timeout;							/**< The timeout for start in blocking and non-blocking mode. */
+	struct workqueue_struct *me1600_workqueue;
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+	struct work_struct ao_control_task;
+#else
+	struct delayed_work ao_control_task;
+#endif
+
+	volatile int ao_control_task_flag;						/**< Flag controling reexecuting of control task */
+} me1600_ao_subdevice_t;
+
+/**
+ * @brief The constructor to generate a subdevice template instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ * @param ao_idx The index of the analog output subdevice on the device.
+ * @param current Flag indicating that analog output with #ao_idx of 3 is capable of current output.
+ * @param config_regs_lock Pointer to spin lock protecting the configuration registers and from concurrent access.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me1600_ao_subdevice_t *me1600_ao_constructor(uint32_t reg_base,
+					     unsigned int ao_idx,
+					     int curr,
+					     spinlock_t * config_regs_lock,
+					     spinlock_t * ao_shadows_lock,
+					     me1600_ao_shadow_t *
+					     ao_regs_shadows,
+					     struct workqueue_struct
+					     *me1600_wq);
+
+# endif	//__KERNEL__
+#endif //_ME1600_AO_H_
diff --git a/drivers/staging/meilhaus/me1600_ao_reg.h b/drivers/staging/meilhaus/me1600_ao_reg.h
new file mode 100644
index 00000000000..31e7800e807
--- /dev/null
+++ b/drivers/staging/meilhaus/me1600_ao_reg.h
@@ -0,0 +1,66 @@
+/**
+ * @file me1600_ao_reg.h
+ *
+ * @brief ME-1600 analog output subdevice register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME1600_AO_REG_H_
+#define _ME1600_AO_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME1600_CHANNEL_0_REG    0x00	/**< Register to set a digital value on channel 0. */
+#define ME1600_CHANNEL_1_REG    0x02	/**< Register to set a digital value on channel 1. */
+#define ME1600_CHANNEL_2_REG    0x04	/**< Register to set a digital value on channel 2. */
+#define ME1600_CHANNEL_3_REG    0x06	/**< Register to set a digital value on channel 3. */
+#define ME1600_CHANNEL_4_REG    0x08	/**< Register to set a digital value on channel 4. */
+#define ME1600_CHANNEL_5_REG    0x0A	/**< Register to set a digital value on channel 5. */
+#define ME1600_CHANNEL_6_REG    0x0C	/**< Register to set a digital value on channel 6. */
+#define ME1600_CHANNEL_7_REG    0x0E	/**< Register to set a digital value on channel 7. */
+#define ME1600_CHANNEL_8_REG    0x10	/**< Register to set a digital value on channel 8. */
+#define ME1600_CHANNEL_9_REG    0x12	/**< Register to set a digital value on channel 9. */
+#define ME1600_CHANNEL_10_REG   0x14	/**< Register to set a digital value on channel 10. */
+#define ME1600_CHANNEL_11_REG   0x16	/**< Register to set a digital value on channel 11. */
+#define ME1600_CHANNEL_12_REG   0x18	/**< Register to set a digital value on channel 12. */
+#define ME1600_CHANNEL_13_REG   0x1A	/**< Register to set a digital value on channel 13. */
+#define ME1600_CHANNEL_14_REG   0x1C	/**< Register to set a digital value on channel 14. */
+#define ME1600_CHANNEL_15_REG   0x1E	/**< Register to set a digital value on channel 15. */
+
+/* Every channel one bit: bipolar = 0, unipolar = 1 */
+#define ME1600_UNI_BI_REG		0x20	/**< Register to switch between unipolar and bipolar. */
+
+/* Every channel one bit (only lower 8 Bits): 0..20mA = 0, 4..20mA = 1 */
+#define ME1600_020_420_REG		0x22	/**< Register to switch between the two current ranges. */
+
+/* If a bit is set, the corresponding DAC (4 ports each) is
+   not set at the moment you write to an output of it.
+   Clearing the bit updates the port. */
+#define ME1600_SIM_OUTPUT_REG	0x24	/**< Register to update all channels of a subdevice simultaneously. */
+
+/* Current on/off (only lower 8 bits): off = 0, on  = 1 */
+#define ME1600_CURRENT_ON_REG	0x26	/**< Register to swicht between voltage and current output. */
+
+#define ME1600_AO_MAX_DATA		0x0FFF	/**< The maximum digital data accepted by an analog output channel. */
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me1600_device.c b/drivers/staging/meilhaus/me1600_device.c
new file mode 100644
index 00000000000..3bc2cb1dc86
--- /dev/null
+++ b/drivers/staging/meilhaus/me1600_device.c
@@ -0,0 +1,261 @@
+/**
+ * @file me1600_device.c
+ *
+ * @brief ME-1600 device class implementation.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+#ifndef MODULE
+#  define MODULE
+#endif
+
+#include <linux/module.h>
+
+#include <linux/pci.h>
+#include <linux/slab.h>
+
+#include "meids.h"
+#include "meerror.h"
+#include "mecommon.h"
+#include "meinternal.h"
+
+#include "medebug.h"
+#include "medevice.h"
+#include "mesubdevice.h"
+#include "me1600_device.h"
+
+static void me1600_set_registry(me1600_device_t * subdevice, uint32_t reg_base);
+static void me1600_destructor(struct me_device *device);
+
+/**
+ * @brief Global variable.
+ * This is working queue for runing a separate atask that will be responsible for work status (start, stop, timeouts).
+ */
+static struct workqueue_struct *me1600_workqueue;
+
+me_device_t *me1600_pci_constructor(struct pci_dev *pci_device)
+{
+	int err;
+	me1600_device_t *me1600_device;
+	me_subdevice_t *subdevice;
+	unsigned int chip_idx;
+	int i;
+
+	PDEBUG("executed.\n");
+
+	// Allocate structure for device instance.
+	me1600_device = kmalloc(sizeof(me1600_device_t), GFP_KERNEL);
+
+	if (!me1600_device) {
+		PERROR("Cannot get memory for device instance.\n");
+		return NULL;
+	}
+
+	memset(me1600_device, 0, sizeof(me1600_device_t));
+
+	// Initialize base class structure.
+	err = me_device_pci_init((me_device_t *) me1600_device, pci_device);
+
+	if (err) {
+		kfree(me1600_device);
+		PERROR("Cannot initialize device base class.\n");
+		return NULL;
+	}
+	// Initialize spin lock .
+	spin_lock_init(&me1600_device->config_regs_lock);
+	spin_lock_init(&me1600_device->ao_shadows_lock);
+
+	// Get the number of analog output subdevices.
+	chip_idx =
+	    me1600_versions_get_device_index(me1600_device->base.info.pci.
+					     device_id);
+
+	// Create shadow instance.
+	me1600_device->ao_regs_shadows.count =
+	    me1600_versions[chip_idx].ao_chips;
+	me1600_device->ao_regs_shadows.registry =
+	    kmalloc(me1600_versions[chip_idx].ao_chips * sizeof(unsigned long),
+		    GFP_KERNEL);
+	me1600_set_registry(me1600_device,
+			    me1600_device->base.info.pci.reg_bases[2]);
+	me1600_device->ao_regs_shadows.shadow =
+	    kmalloc(me1600_versions[chip_idx].ao_chips * sizeof(uint16_t),
+		    GFP_KERNEL);
+	me1600_device->ao_regs_shadows.mirror =
+	    kmalloc(me1600_versions[chip_idx].ao_chips * sizeof(uint16_t),
+		    GFP_KERNEL);
+
+	// Create subdevice instances.
+	for (i = 0; i < me1600_versions[chip_idx].ao_chips; i++) {
+		subdevice =
+		    (me_subdevice_t *) me1600_ao_constructor(me1600_device->
+							     base.info.pci.
+							     reg_bases[2], i,
+							     ((me1600_versions
+							       [chip_idx].curr >
+							       i) ? 1 : 0),
+							     &me1600_device->
+							     config_regs_lock,
+							     &me1600_device->
+							     ao_shadows_lock,
+							     &me1600_device->
+							     ao_regs_shadows,
+							     me1600_workqueue);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me1600_device);
+			kfree(me1600_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me1600_device->base.slist,
+					    subdevice);
+	}
+
+	// Overwrite base class methods.
+	me1600_device->base.me_device_destructor = me1600_destructor;
+
+	return (me_device_t *) me1600_device;
+}
+
+static void me1600_destructor(struct me_device *device)
+{
+	me1600_device_t *me1600_device = (me1600_device_t *) device;
+	PDEBUG("executed.\n");
+
+	// Destroy shadow instance.
+	kfree(me1600_device->ao_regs_shadows.registry);
+	kfree(me1600_device->ao_regs_shadows.shadow);
+	kfree(me1600_device->ao_regs_shadows.mirror);
+
+	me_device_deinit((me_device_t *) me1600_device);
+	kfree(me1600_device);
+}
+
+static void me1600_set_registry(me1600_device_t * subdevice, uint32_t reg_base)
+{				// Create shadow structure.
+	if (subdevice->ao_regs_shadows.count >= 1) {
+		subdevice->ao_regs_shadows.registry[0] =
+		    (unsigned long)(reg_base + ME1600_CHANNEL_0_REG);
+	}
+	if (subdevice->ao_regs_shadows.count >= 2) {
+		subdevice->ao_regs_shadows.registry[1] =
+		    (unsigned long)(reg_base + ME1600_CHANNEL_1_REG);
+	}
+	if (subdevice->ao_regs_shadows.count >= 3) {
+		subdevice->ao_regs_shadows.registry[2] =
+		    (unsigned long)(reg_base + ME1600_CHANNEL_2_REG);
+	}
+	if (subdevice->ao_regs_shadows.count >= 4) {
+		subdevice->ao_regs_shadows.registry[3] =
+		    (unsigned long)(reg_base + ME1600_CHANNEL_3_REG);
+	}
+	if (subdevice->ao_regs_shadows.count >= 5) {
+		subdevice->ao_regs_shadows.registry[4] =
+		    (unsigned long)(reg_base + ME1600_CHANNEL_4_REG);
+	}
+	if (subdevice->ao_regs_shadows.count >= 6) {
+		subdevice->ao_regs_shadows.registry[5] =
+		    (unsigned long)(reg_base + ME1600_CHANNEL_5_REG);
+	}
+	if (subdevice->ao_regs_shadows.count >= 7) {
+		subdevice->ao_regs_shadows.registry[6] =
+		    (unsigned long)(reg_base + ME1600_CHANNEL_6_REG);
+	}
+	if (subdevice->ao_regs_shadows.count >= 8) {
+		subdevice->ao_regs_shadows.registry[7] =
+		    (unsigned long)(reg_base + ME1600_CHANNEL_7_REG);
+	}
+	if (subdevice->ao_regs_shadows.count >= 9) {
+		subdevice->ao_regs_shadows.registry[8] =
+		    (unsigned long)(reg_base + ME1600_CHANNEL_8_REG);
+	}
+	if (subdevice->ao_regs_shadows.count >= 10) {
+		subdevice->ao_regs_shadows.registry[9] =
+		    (unsigned long)(reg_base + ME1600_CHANNEL_9_REG);
+	}
+	if (subdevice->ao_regs_shadows.count >= 11) {
+		subdevice->ao_regs_shadows.registry[10] =
+		    (unsigned long)(reg_base + ME1600_CHANNEL_10_REG);
+	}
+	if (subdevice->ao_regs_shadows.count >= 12) {
+		subdevice->ao_regs_shadows.registry[11] =
+		    (unsigned long)(reg_base + ME1600_CHANNEL_11_REG);
+	}
+	if (subdevice->ao_regs_shadows.count >= 13) {
+		subdevice->ao_regs_shadows.registry[12] =
+		    (unsigned long)(reg_base + ME1600_CHANNEL_12_REG);
+	}
+	if (subdevice->ao_regs_shadows.count >= 14) {
+		subdevice->ao_regs_shadows.registry[13] =
+		    (unsigned long)(reg_base + ME1600_CHANNEL_13_REG);
+	}
+	if (subdevice->ao_regs_shadows.count >= 15) {
+		subdevice->ao_regs_shadows.registry[14] =
+		    (unsigned long)(reg_base + ME1600_CHANNEL_14_REG);
+	}
+	if (subdevice->ao_regs_shadows.count >= 16) {
+		subdevice->ao_regs_shadows.registry[15] =
+		    (unsigned long)(reg_base + ME1600_CHANNEL_15_REG);
+	}
+	if (subdevice->ao_regs_shadows.count > 16) {
+		PERROR("More than 16 outputs! (%d)\n",
+		       subdevice->ao_regs_shadows.count);
+	}
+}
+
+// Init and exit of module.
+
+static int __init me1600_init(void)
+{
+	PDEBUG("executed\n.");
+
+	me1600_workqueue = create_singlethread_workqueue("me1600");
+	return 0;
+}
+
+static void __exit me1600_exit(void)
+{
+	PDEBUG("executed\n.");
+
+	flush_workqueue(me1600_workqueue);
+	destroy_workqueue(me1600_workqueue);
+}
+
+module_init(me1600_init);
+module_exit(me1600_exit);
+
+// Administrative stuff for modinfo.
+MODULE_AUTHOR
+    ("Guenter Gebhardt <g.gebhardt@meilhaus.de> & Krzysztof Gantzke <k.gantzke@meilhaus.de>");
+MODULE_DESCRIPTION("Device Driver Module for ME-1600 Device");
+MODULE_SUPPORTED_DEVICE("Meilhaus ME-1600 Devices");
+MODULE_LICENSE("GPL");
+
+// Export the constructor.
+EXPORT_SYMBOL(me1600_pci_constructor);
diff --git a/drivers/staging/meilhaus/me1600_device.h b/drivers/staging/meilhaus/me1600_device.h
new file mode 100644
index 00000000000..f7b231f73ac
--- /dev/null
+++ b/drivers/staging/meilhaus/me1600_device.h
@@ -0,0 +1,101 @@
+/**
+ * @file me1600_device.h
+ *
+ * @brief ME-1600 device class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME1600_H
+#define _ME1600_H
+
+#include <linux/pci.h>
+#include <linux/spinlock.h>
+
+#include "medevice.h"
+#include "me1600_ao.h"
+#include "me1600_ao_reg.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief Structure to store device capabilities.
+ */
+typedef struct me1600_version {
+	uint16_t device_id;				/**< The PCI device id of the device. */
+	unsigned int ao_chips;			/**< The number of analog outputs on the device. */
+	int curr;						/**< Flag to identify amounts of current output. */
+} me1600_version_t;
+
+/**
+  * @brief Defines for each ME-1600 device version its capabilities.
+ */
+static me1600_version_t me1600_versions[] = {
+	{PCI_DEVICE_ID_MEILHAUS_ME1600_4U, 4, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME1600_8U, 8, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME1600_12U, 12, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME1600_16U, 16, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME1600_16U_8I, 16, 8},
+	{0}
+};
+
+/**< Returns the number of entries in #me1600_versions. */
+#define ME1600_DEVICE_VERSIONS (sizeof(me1600_versions) / sizeof(me1600_version_t) - 1)
+
+/**
+ * @brief Returns the index of the device entry in #me1600_versions.
+ *
+ * @param device_id The PCI device id of the device to query.
+ * @return The index of the device in #me1600_versions.
+ */
+static inline unsigned int me1600_versions_get_device_index(uint16_t device_id)
+{
+	unsigned int i;
+	for (i = 0; i < ME1600_DEVICE_VERSIONS; i++)
+		if (me1600_versions[i].device_id == device_id)
+			break;
+	return i;
+}
+
+/**
+ * @brief The ME-1600 device class structure.
+ */
+typedef struct me1600_device {
+	me_device_t base;						/**< The Meilhaus device base class. */
+	spinlock_t config_regs_lock;			/**< Protects the configuration registers. */
+
+	me1600_ao_shadow_t ao_regs_shadows;		/**< Addresses and shadows of output's registers. */
+	spinlock_t ao_shadows_lock;				/**< Protects the shadow's struct. */
+} me1600_device_t;
+
+/**
+ * @brief The ME-1600 device class constructor.
+ *
+ * @param pci_device The pci device structure given by the PCI subsystem.
+ *
+ * @return On succes a new ME-1600 device instance. \n
+ *         NULL on error.
+ */
+me_device_t *me1600_pci_constructor(struct pci_dev *pci_device)
+    __attribute__ ((weak));
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me4600_ai.c b/drivers/staging/meilhaus/me4600_ai.c
new file mode 100644
index 00000000000..1a0de5dea27
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_ai.c
@@ -0,0 +1,3434 @@
+/**
+ * @file me4600_ai.c
+ *
+ * @brief ME-4000 analog input subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+# define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <asm/uaccess.h>
+#include <linux/types.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+#include "medebug.h"
+#include "meids.h"
+
+#include "me4600_reg.h"
+#include "me4600_ai_reg.h"
+#include "me4600_ai.h"
+
+/*
+ * Declarations (local)
+ */
+
+static void me4600_ai_destructor(struct me_subdevice *subdevice);
+static int me4600_ai_io_reset_subdevice(me_subdevice_t * subdevice,
+					struct file *filep, int flags);
+
+static int me4600_ai_io_single_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int single_config,
+				      int ref,
+				      int trig_chan,
+				      int trig_type, int trig_edge, int flags);
+
+static int me4600_ai_io_single_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int channel,
+				    int *value, int time_out, int flags);
+
+static int me4600_ai_io_stream_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      meIOStreamConfig_t * config_list,
+				      int count,
+				      meIOStreamTrigger_t * trigger,
+				      int fifo_irq_threshold, int flags);
+static int me4600_ai_io_stream_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int read_mode,
+				    int *values, int *count, int flags);
+static int me4600_ai_io_stream_new_values(me_subdevice_t * subdevice,
+					  struct file *filep,
+					  int time_out, int *count, int flags);
+static int inline me4600_ai_io_stream_read_get_value(me4600_ai_subdevice_t *
+						     instance, int *values,
+						     const int count,
+						     const int flags);
+
+static int me4600_ai_io_stream_start(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int start_mode, int time_out, int flags);
+static int me4600_ai_io_stream_stop(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int stop_mode, int flags);
+static int me4600_ai_io_stream_status(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int wait,
+				      int *status, int *values, int flags);
+
+static int me4600_ai_query_range_by_min_max(me_subdevice_t * subdevice,
+					    int unit,
+					    int *min,
+					    int *max, int *maxdata, int *range);
+static int me4600_ai_query_number_ranges(me_subdevice_t * subdevice,
+					 int unit, int *count);
+static int me4600_ai_query_range_info(me_subdevice_t * subdevice,
+				      int range,
+				      int *unit,
+				      int *min, int *max, int *maxdata);
+static int me4600_ai_query_timer(me_subdevice_t * subdevice,
+				 int timer,
+				 int *base_frequency,
+				 long long *min_ticks, long long *max_ticks);
+static int me4600_ai_query_number_channels(me_subdevice_t * subdevice,
+					   int *number);
+static int me4600_ai_query_subdevice_type(me_subdevice_t * subdevice,
+					  int *type, int *subtype);
+static int me4600_ai_query_subdevice_caps(me_subdevice_t * subdevice,
+					  int *caps);
+static int me4600_ai_query_subdevice_caps_args(struct me_subdevice *subdevice,
+					       int cap, int *args, int count);
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 19)
+static irqreturn_t me4600_ai_isr(int irq, void *dev_id);
+#else
+static irqreturn_t me4600_ai_isr(int irq, void *dev_id, struct pt_regs *regs);
+#endif
+
+static int ai_mux_toggler(me4600_ai_subdevice_t * subdevice);
+
+/** Immidiate stop.
+* Reset all IRQ's sources. (block laches)
+* Preserve FIFO
+*/
+static int ai_stop_immediately(me4600_ai_subdevice_t * instance);
+
+/** Immidiate stop.
+* Reset all IRQ's sources. (block laches)
+* Reset data FIFO
+*/
+void inline ai_stop_isr(me4600_ai_subdevice_t * instance);
+
+/** Interrupt logics.
+* Read datas
+* Reset latches
+*/
+void ai_limited_isr(me4600_ai_subdevice_t * instance, const uint32_t irq_status,
+		    const uint32_t ctrl_status);
+void ai_infinite_isr(me4600_ai_subdevice_t * instance,
+		     const uint32_t irq_status, const uint32_t ctrl_status);
+
+/** Last chunck of datas. We must reschedule sample counter.
+* Leaving SC_RELOAD doesn't do any harm, but in some bad case can make extra interrupts.
+* When threshold is wrongly set some IRQ are lost.(!!!)
+*/
+void inline ai_reschedule_SC(me4600_ai_subdevice_t * instance);
+
+/** Read datas from FIFO and copy them to buffer */
+static int inline ai_read_data(me4600_ai_subdevice_t * instance,
+			       const int count);
+
+/** Copy rest of data from fifo to circular buffer.*/
+static int inline ai_read_data_pooling(me4600_ai_subdevice_t * instance);
+
+/** Set ISM to next state for infinite data aqusation mode*/
+void inline ai_infinite_ISM(me4600_ai_subdevice_t * instance);
+
+/** Set ISM to next state for define amount of data aqusation mode*/
+void inline ai_limited_ISM(me4600_ai_subdevice_t * instance,
+			   uint32_t irq_status);
+
+/** Set ISM to next stage for limited mode */
+void inline ai_data_acquisition_logic(me4600_ai_subdevice_t * instance);
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+static void me4600_ai_work_control_task(void *subdevice);
+#else
+static void me4600_ai_work_control_task(struct work_struct *work);
+#endif
+
+/* Definitions
+ */
+
+me4600_ai_subdevice_t *me4600_ai_constructor(uint32_t reg_base,
+					     unsigned int channels,
+					     unsigned int ranges,
+					     int isolated,
+					     int sh,
+					     int irq,
+					     spinlock_t * ctrl_reg_lock,
+					     struct workqueue_struct *me4600_wq)
+{
+	me4600_ai_subdevice_t *subdevice;
+	int err;
+	unsigned int i;
+
+	PDEBUG("executed. idx=0\n");
+
+	// Allocate memory for subdevice instance.
+	subdevice = kmalloc(sizeof(me4600_ai_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me4600_ai_subdevice_t));
+
+	// Initialize subdevice base class.
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	subdevice->ctrl_reg_lock = ctrl_reg_lock;
+
+	// Initialize circular buffer.
+	subdevice->circ_buf.mask = ME4600_AI_CIRC_BUF_COUNT - 1;
+
+	subdevice->circ_buf.buf =
+	    (void *)__get_free_pages(GFP_KERNEL, ME4600_AI_CIRC_BUF_SIZE_ORDER);
+	PDEBUG("circ_buf = %p size=%ld\n", subdevice->circ_buf.buf,
+	       ME4600_AI_CIRC_BUF_SIZE);
+
+	if (!subdevice->circ_buf.buf) {
+		PERROR("Cannot get circular buffer.\n");
+		me_subdevice_deinit((me_subdevice_t *) subdevice);
+		kfree(subdevice);
+		return NULL;
+	}
+
+	memset(subdevice->circ_buf.buf, 0, ME4600_AI_CIRC_BUF_SIZE);
+	subdevice->circ_buf.head = 0;
+	subdevice->circ_buf.tail = 0;
+	subdevice->status = ai_status_none;
+
+	// Initialize wait queue.
+	init_waitqueue_head(&subdevice->wait_queue);
+
+	// Save the number of channels.
+	subdevice->channels = channels;
+
+	/* Initialize the single config entries to reset values */
+	for (i = 0; i < channels; i++) {
+		subdevice->single_config[i].status = ME_SINGLE_CHANNEL_NOT_CONFIGURED;	//not configured
+	}
+
+	// Save if isolated device.
+	subdevice->isolated = isolated;
+
+	// Save if sample and hold is available.
+	subdevice->sh = sh;
+
+	// Set stream config to not configured state.
+	subdevice->fifo_irq_threshold = 0;
+	subdevice->data_required = 0;
+	subdevice->chan_list_len = 0;
+
+	// Initialize registers addresses.
+	subdevice->ctrl_reg = reg_base + ME4600_AI_CTRL_REG;
+	subdevice->status_reg = reg_base + ME4600_AI_STATUS_REG;
+	subdevice->channel_list_reg = reg_base + ME4600_AI_CHANNEL_LIST_REG;
+	subdevice->data_reg = reg_base + ME4600_AI_DATA_REG;
+	subdevice->chan_timer_reg = reg_base + ME4600_AI_CHAN_TIMER_REG;
+	subdevice->chan_pre_timer_reg = reg_base + ME4600_AI_CHAN_PRE_TIMER_REG;
+	subdevice->scan_timer_low_reg = reg_base + ME4600_AI_SCAN_TIMER_LOW_REG;
+	subdevice->scan_timer_high_reg =
+	    reg_base + ME4600_AI_SCAN_TIMER_HIGH_REG;
+	subdevice->scan_pre_timer_low_reg =
+	    reg_base + ME4600_AI_SCAN_PRE_TIMER_LOW_REG;
+	subdevice->scan_pre_timer_high_reg =
+	    reg_base + ME4600_AI_SCAN_PRE_TIMER_HIGH_REG;
+	subdevice->start_reg = reg_base + ME4600_AI_START_REG;
+	subdevice->irq_status_reg = reg_base + ME4600_IRQ_STATUS_REG;
+	subdevice->sample_counter_reg = reg_base + ME4600_AI_SAMPLE_COUNTER_REG;
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = reg_base;
+#endif
+
+	// Initialize ranges.
+	subdevice->ranges_len = ranges;
+	subdevice->ranges[0].min = -10E6;
+	subdevice->ranges[0].max = 9999694;
+
+	subdevice->ranges[1].min = 0;
+	subdevice->ranges[1].max = 9999847;
+
+	subdevice->ranges[2].min = -25E5;
+	subdevice->ranges[2].max = 2499923;
+
+	subdevice->ranges[3].min = 0;
+	subdevice->ranges[3].max = 2499961;
+
+	// We have to switch the mux in order to get it work correctly.
+	ai_mux_toggler(subdevice);
+
+	// Register interrupt service routine.
+	subdevice->irq = irq;
+	if (request_irq(subdevice->irq, me4600_ai_isr,
+#ifdef IRQF_DISABLED
+			IRQF_DISABLED | IRQF_SHARED,
+#else
+			SA_INTERRUPT | SA_SHIRQ,
+#endif
+			ME4600_NAME, subdevice)) {
+		PERROR("Cannot register interrupt service routine.\n");
+		me_subdevice_deinit((me_subdevice_t *) subdevice);
+		free_pages((unsigned long)subdevice->circ_buf.buf,
+			   ME4600_AI_CIRC_BUF_SIZE_ORDER);
+		subdevice->circ_buf.buf = NULL;
+		kfree(subdevice);
+		return NULL;
+	}
+	PINFO("Registered irq=%d.\n", subdevice->irq);
+
+	// Override base class methods.
+	subdevice->base.me_subdevice_destructor = me4600_ai_destructor;
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me4600_ai_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me4600_ai_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me4600_ai_io_single_read;
+	subdevice->base.me_subdevice_io_stream_config =
+	    me4600_ai_io_stream_config;
+	subdevice->base.me_subdevice_io_stream_new_values =
+	    me4600_ai_io_stream_new_values;
+	subdevice->base.me_subdevice_io_stream_read = me4600_ai_io_stream_read;
+	subdevice->base.me_subdevice_io_stream_start =
+	    me4600_ai_io_stream_start;
+	subdevice->base.me_subdevice_io_stream_status =
+	    me4600_ai_io_stream_status;
+	subdevice->base.me_subdevice_io_stream_stop = me4600_ai_io_stream_stop;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me4600_ai_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me4600_ai_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me4600_ai_query_subdevice_caps;
+	subdevice->base.me_subdevice_query_subdevice_caps_args =
+	    me4600_ai_query_subdevice_caps_args;
+	subdevice->base.me_subdevice_query_range_by_min_max =
+	    me4600_ai_query_range_by_min_max;
+	subdevice->base.me_subdevice_query_number_ranges =
+	    me4600_ai_query_number_ranges;
+	subdevice->base.me_subdevice_query_range_info =
+	    me4600_ai_query_range_info;
+	subdevice->base.me_subdevice_query_timer = me4600_ai_query_timer;
+
+	// Prepare work queue.
+	subdevice->me4600_workqueue = me4600_wq;
+
+/* workqueue API changed in kernel 2.6.20 */
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20) )
+	INIT_WORK(&subdevice->ai_control_task, me4600_ai_work_control_task,
+		  (void *)subdevice);
+#else
+	INIT_DELAYED_WORK(&subdevice->ai_control_task,
+			  me4600_ai_work_control_task);
+#endif
+
+	return subdevice;
+}
+
+static void me4600_ai_destructor(struct me_subdevice *subdevice)
+{
+	me4600_ai_subdevice_t *instance;
+
+	instance = (me4600_ai_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=0\n");
+
+	instance->ai_control_task_flag = 0;
+	// Reset subdevice to asure clean exit.
+	me4600_ai_io_reset_subdevice(subdevice, NULL,
+				     ME_IO_RESET_SUBDEVICE_NO_FLAGS);
+
+	// Remove any tasks from work queue. This is paranoic because it was done allready in reset().
+	if (!cancel_delayed_work(&instance->ai_control_task)) {	//Wait 2 ticks to be sure that control task is removed from queue.
+		set_current_state(TASK_INTERRUPTIBLE);
+		schedule_timeout(2);
+	}
+
+	free_irq(instance->irq, instance);
+	free_pages((unsigned long)instance->circ_buf.buf,
+		   ME4600_AI_CIRC_BUF_SIZE_ORDER);
+	me_subdevice_deinit(&instance->base);
+	kfree(instance);
+}
+
+static int me4600_ai_io_reset_subdevice(me_subdevice_t * subdevice,
+					struct file *filep, int flags)
+{
+	me4600_ai_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	volatile uint32_t ctrl;
+	unsigned long status;
+	const int timeout = HZ / 10;	//100ms
+	int i;
+
+	PDEBUG("executed. idx=0\n");
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	instance = (me4600_ai_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	instance->ai_control_task_flag = 0;
+	instance->status = ai_status_none;
+
+	for (i = 0; i <= timeout; i++) {
+		spin_lock_irqsave(instance->ctrl_reg_lock, status);
+		ctrl = inl(instance->ctrl_reg);
+		//Stop DMA
+		ctrl &= ~ME4600_AI_CTRL_RPCI_FIFO;
+		// Stop all actions. No conditions!
+		ctrl &= ~ME4600_AI_CTRL_BIT_STOP;
+		ctrl |= ME4600_AI_CTRL_BIT_IMMEDIATE_STOP;
+
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+		spin_unlock_irqrestore(instance->ctrl_reg_lock, status);
+
+		if (!(inl(instance->status_reg) & ME4600_AI_STATUS_BIT_FSM))
+			break;
+
+		set_current_state(TASK_INTERRUPTIBLE);
+		schedule_timeout(1);
+	}
+
+	if (i > timeout) {
+		PERROR("FSM is still busy.\n");
+		ME_SUBDEVICE_EXIT;
+		return ME_ERRNO_INTERNAL;
+	}
+
+	spin_lock_irqsave(instance->ctrl_reg_lock, status);
+	ctrl = inl(instance->ctrl_reg);
+	// Clear all features. Dissable interrupts.
+	ctrl &= ~(ME4600_AI_CTRL_BIT_STOP
+		  | ME4600_AI_CTRL_BIT_LE_IRQ
+		  | ME4600_AI_CTRL_BIT_HF_IRQ | ME4600_AI_CTRL_BIT_SC_IRQ);
+	ctrl |= (ME4600_AI_CTRL_BIT_IMMEDIATE_STOP
+		 | ME4600_AI_CTRL_BIT_LE_IRQ_RESET
+		 | ME4600_AI_CTRL_BIT_HF_IRQ_RESET
+		 | ME4600_AI_CTRL_BIT_SC_IRQ_RESET);
+
+	outl(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl);
+	spin_unlock_irqrestore(instance->ctrl_reg_lock, status);
+
+	outl(ME4600_AI_MIN_CHAN_TICKS - 1, instance->chan_timer_reg);
+	PDEBUG_REG("chan_timer_reg outl(0x%lX+0x%lX)=0x%llx\n",
+		   instance->reg_base,
+		   instance->chan_timer_reg - instance->reg_base,
+		   ME4600_AI_MIN_CHAN_TICKS);
+	outl(ME4600_AI_MIN_ACQ_TICKS - 1, instance->chan_pre_timer_reg);
+	PDEBUG_REG("chan_pre_timer_reg outl(0x%lX+0x%lX)=0x%llx\n",
+		   instance->reg_base,
+		   instance->chan_pre_timer_reg - instance->reg_base,
+		   ME4600_AI_MIN_ACQ_TICKS);
+	outl(0, instance->scan_timer_low_reg);
+	PDEBUG_REG("scan_timer_low_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->scan_timer_low_reg - instance->reg_base, 0);
+	outl(0, instance->scan_timer_high_reg);
+	PDEBUG_REG("scan_timer_high_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->scan_timer_high_reg - instance->reg_base, 0);
+	outl(0, instance->scan_pre_timer_low_reg);
+	PDEBUG_REG("scan_pre_timer_low_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->scan_pre_timer_low_reg - instance->reg_base, 0);
+	outl(0, instance->scan_pre_timer_high_reg);
+	PDEBUG_REG("scan_pre_timer_high_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->scan_pre_timer_high_reg - instance->reg_base, 0);
+	outl(0xEFFFFFFF, instance->sample_counter_reg);
+	PDEBUG_REG("sample_counter_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->sample_counter_reg - instance->reg_base,
+		   0xEFFFFFFF);
+
+	instance->circ_buf.head = 0;
+	instance->circ_buf.tail = 0;
+
+	instance->fifo_irq_threshold = 0;
+	instance->data_required = 0;
+	instance->chan_list_len = 0;
+
+	// Initialize the single config entries to reset values.
+	for (i = 0; i < instance->channels; i++) {
+		instance->single_config[i].status =
+		    ME_SINGLE_CHANNEL_NOT_CONFIGURED;
+	}
+	instance->status = ai_status_none;
+
+	//Signal reset if user is on wait.
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ai_io_single_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int single_config,
+				      int ref,
+				      int trig_chan,
+				      int trig_type, int trig_edge, int flags)
+{
+	me4600_ai_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long cpu_flags;
+	int i;
+
+	instance = (me4600_ai_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=0\n");
+
+	if (flags & ~ME_IO_SINGLE_CONFIG_CONTINUE) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	switch (trig_type) {
+	case ME_TRIG_TYPE_SW:
+		if (trig_edge != ME_TRIG_EDGE_NONE) {
+			PERROR
+			    ("Invalid trigger edge. Software trigger has not edge.\n");
+			return ME_ERRNO_INVALID_TRIG_EDGE;
+		}
+		break;
+
+	case ME_TRIG_TYPE_EXT_ANALOG:
+		if (instance->channels <= 16)	//Only versions with 32 channels have analog trigger (4670 and 4680)
+		{
+			PERROR("Invalid trigger type specified.\n");
+			return ME_ERRNO_INVALID_TRIG_TYPE;
+		}
+
+	case ME_TRIG_TYPE_EXT_DIGITAL:
+		if ((trig_edge != ME_TRIG_EDGE_ANY)
+		    && (trig_edge != ME_TRIG_EDGE_RISING)
+		    && (trig_edge != ME_TRIG_EDGE_FALLING)) {
+			PERROR("Invalid trigger edge specified.\n");
+			return ME_ERRNO_INVALID_TRIG_EDGE;
+		}
+		break;
+
+	default:
+		PERROR("Invalid trigger type specified.\n");
+		return ME_ERRNO_INVALID_TRIG_TYPE;
+	}
+
+	if (trig_chan != ME_TRIG_CHAN_DEFAULT) {
+		PERROR("Invalid trigger channel specified.\n");
+		return ME_ERRNO_INVALID_TRIG_CHAN;
+	}
+
+	if ((single_config < 0) || (single_config >= instance->ranges_len)) {
+		PERROR("Invalid single config specified.\n");
+		return ME_ERRNO_INVALID_SINGLE_CONFIG;
+	}
+
+	if ((ref != ME_REF_AI_GROUND) && (ref != ME_REF_AI_DIFFERENTIAL)) {
+		PERROR("Invalid analog reference specified.\n");
+		return ME_ERRNO_INVALID_REF;
+	}
+
+	if ((single_config % 2) && (ref != ME_REF_AI_GROUND)) {
+		PERROR("Invalid analog reference specified.\n");
+		return ME_ERRNO_INVALID_REF;
+	}
+
+	if ((ref == ME_REF_AI_DIFFERENTIAL)
+	    && ((instance->channels == 16) || (channel >= 16))) {
+		PERROR("Invalid analog reference specified.\n");
+		return ME_ERRNO_INVALID_REF;
+	}
+
+	if (channel < 0) {
+		PERROR("Invalid channel number specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	if (channel >= instance->channels) {
+		PERROR("Invalid channel number specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	//Prepare data entry.
+	// Common for all modes.
+	instance->single_config[channel].entry =
+	    channel | ME4600_AI_LIST_LAST_ENTRY;
+
+	if (ref == ME_REF_AI_DIFFERENTIAL) {	// ME_REF_AI_DIFFERENTIAL
+		instance->single_config[channel].entry |=
+		    ME4600_AI_LIST_INPUT_DIFFERENTIAL;
+	}
+/*
+		// ME4600_AI_LIST_INPUT_SINGLE_ENDED = 0x0000
+		// 'entry |= ME4600_AI_LIST_INPUT_SINGLE_ENDED' <== Do nothing. Removed.
+		else
+		{// ME_REF_AI_GROUND
+			instance->single_config[channel].entry |= ME4600_AI_LIST_INPUT_SINGLE_ENDED;
+		}
+*/
+	switch (single_config) {
+	case 0:		//-10V..10V
+/*
+					// ME4600_AI_LIST_RANGE_BIPOLAR_10 = 0x0000
+					// 'entry |= ME4600_AI_LIST_RANGE_BIPOLAR_10' <== Do nothing. Removed.
+					instance->single_config[channel].entry |= ME4600_AI_LIST_RANGE_BIPOLAR_10;
+*/ break;
+
+	case 1:		//0V..10V
+		instance->single_config[channel].entry |=
+		    ME4600_AI_LIST_RANGE_UNIPOLAR_10;
+		break;
+
+	case 2:		//-2.5V..2.5V
+		instance->single_config[channel].entry |=
+		    ME4600_AI_LIST_RANGE_BIPOLAR_2_5;
+		break;
+
+	case 3:		//0V..2.5V
+		instance->single_config[channel].entry |=
+		    ME4600_AI_LIST_RANGE_UNIPOLAR_2_5;
+		break;
+	}
+
+	// Prepare control register.
+	// Common for all modes.
+	instance->single_config[channel].ctrl =
+	    ME4600_AI_CTRL_BIT_CHANNEL_FIFO | ME4600_AI_CTRL_BIT_DATA_FIFO;
+
+	switch (trig_type) {
+	case ME_TRIG_TYPE_SW:
+		// Nothing to set.
+		break;
+
+	case ME_TRIG_TYPE_EXT_ANALOG:
+		instance->single_config[channel].ctrl |=
+		    ME4600_AI_CTRL_BIT_EX_TRIG_ANALOG;
+
+	case ME_TRIG_TYPE_EXT_DIGITAL:
+		instance->single_config[channel].ctrl |=
+		    ME4600_AI_CTRL_BIT_EX_TRIG;
+		break;
+	}
+
+	switch (trig_edge) {
+	case ME_TRIG_EDGE_RISING:
+		// Nothing to set.
+		break;
+
+	case ME_TRIG_EDGE_ANY:
+		instance->single_config[channel].ctrl |=
+		    ME4600_AI_CTRL_BIT_EX_TRIG_BOTH;
+
+	case ME_TRIG_EDGE_FALLING:
+		instance->single_config[channel].ctrl |=
+		    ME4600_AI_CTRL_BIT_EX_TRIG_FALLING;
+		break;
+	}
+
+	// Enable this channel
+	instance->single_config[channel].status = ME_SINGLE_CHANNEL_CONFIGURED;
+
+	// Copy this settings to other outputs.
+	if (flags == ME_IO_SINGLE_CONFIG_CONTINUE) {
+		for (i = channel + 1; i < instance->channels; i++) {
+			instance->single_config[i].ctrl =
+			    instance->single_config[channel].ctrl;
+			instance->single_config[i].entry =
+			    instance->single_config[channel].entry;
+			instance->single_config[i].status =
+			    ME_SINGLE_CHANNEL_CONFIGURED;
+		}
+	}
+
+	instance->status = ai_status_single_configured;
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ai_io_single_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int channel,
+				    int *value, int time_out, int flags)
+{
+	me4600_ai_subdevice_t *instance;
+	volatile uint32_t tmp;
+	volatile uint32_t val;
+	unsigned long cpu_flags;
+	int err = ME_ERRNO_SUCCESS;
+
+	unsigned long j;
+	unsigned long delay = 0;
+
+	PDEBUG("executed. idx=0\n");
+
+	instance = (me4600_ai_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (instance->status != ai_status_single_configured) {
+		PERROR("Subdevice not configured to work in single mode!\n");
+		return ME_ERRNO_PREVIOUS_CONFIG;
+	}
+
+	if ((channel > instance->channels) || (channel < 0)) {
+		PERROR("Invalid channel specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid timeout specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	if (instance->single_config[channel].status !=
+	    ME_SINGLE_CHANNEL_CONFIGURED) {
+		PERROR("Channel is not configured to work in single mode!\n");
+		return ME_ERRNO_PREVIOUS_CONFIG;
+	}
+
+	if (inl(instance->status_reg) & ME4600_AI_STATUS_BIT_FSM) {
+		PERROR("Subdevice is busy.\n");
+		return ME_ERRNO_SUBDEVICE_BUSY;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	// Cancel control task
+	PDEBUG("Cancel control task.\n");
+	instance->ai_control_task_flag = 0;
+	cancel_delayed_work(&instance->ai_control_task);
+
+	if (time_out) {
+		delay = (time_out * HZ) / 1000;
+
+		if (delay == 0)
+			delay = 1;
+	}
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+
+	// Mark that StreamConfig is removed.
+	instance->chan_list_len = 0;
+
+	spin_lock_irqsave(instance->ctrl_reg_lock, cpu_flags);
+	/// @note Imprtant: Preserve EXT IRQ settings.
+	tmp = inl(instance->ctrl_reg);
+	// Clear FIFOs and dissable interrupts
+	tmp &=
+	    ~(ME4600_AI_CTRL_BIT_CHANNEL_FIFO | ME4600_AI_CTRL_BIT_DATA_FIFO);
+
+	tmp &=
+	    ~(ME4600_AI_CTRL_BIT_SC_IRQ | ME4600_AI_CTRL_BIT_HF_IRQ |
+	      ME4600_AI_CTRL_BIT_LE_IRQ);
+	tmp |=
+	    ME4600_AI_CTRL_BIT_SC_IRQ_RESET | ME4600_AI_CTRL_BIT_HF_IRQ_RESET |
+	    ME4600_AI_CTRL_BIT_LE_IRQ_RESET;
+
+	tmp |= ME4600_AI_CTRL_BIT_IMMEDIATE_STOP;
+	outl(tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp);
+
+	outl(0, instance->scan_pre_timer_low_reg);
+	PDEBUG_REG("scan_pre_timer_low_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->scan_pre_timer_low_reg - instance->reg_base, 0);
+	outl(0, instance->scan_pre_timer_high_reg);
+	PDEBUG_REG("scan_pre_timer_high_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->scan_pre_timer_high_reg - instance->reg_base, 0);
+	outl(0, instance->scan_timer_low_reg);
+	PDEBUG_REG("scan_timer_low_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->scan_timer_low_reg - instance->reg_base, 0);
+	outl(0, instance->scan_timer_high_reg);
+	PDEBUG_REG("scan_timer_high_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->scan_timer_high_reg - instance->reg_base, 0);
+	outl(65, instance->chan_timer_reg);
+	PDEBUG_REG("chan_timer_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->chan_timer_reg - instance->reg_base, 65);
+	outl(65, instance->chan_pre_timer_reg);
+	PDEBUG_REG("chan_pre_timer_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->chan_pre_timer_reg - instance->reg_base, 65);
+
+	//Reactive FIFOs. Enable work.
+	tmp |= ME4600_AI_CTRL_BIT_CHANNEL_FIFO | ME4600_AI_CTRL_BIT_DATA_FIFO;
+	outl(tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp);
+
+	outl(instance->single_config[channel].entry,
+	     instance->channel_list_reg);
+	PDEBUG_REG("channel_list_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->channel_list_reg - instance->reg_base,
+		   instance->single_config[channel].entry);
+
+	// Preserve EXT IRQ settings.
+	tmp &= (ME4600_AI_CTRL_BIT_EX_IRQ | ME4600_AI_CTRL_BIT_EX_IRQ_RESET);
+	outl(instance->single_config[channel].ctrl | tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base,
+		   instance->single_config[channel].ctrl | tmp);
+
+	spin_unlock_irqrestore(instance->ctrl_reg_lock, cpu_flags);
+
+	if (!(instance->single_config[channel].ctrl & ME4600_AI_CTRL_BIT_EX_TRIG)) {	// Software start
+		inl(instance->start_reg);
+		PDEBUG_REG("start_reg inl(0x%lX+0x%lX)\n", instance->reg_base,
+			   instance->start_reg - instance->reg_base);
+
+		delay = 2;
+	}
+
+	j = jiffies;
+
+	while (!(inl(instance->status_reg) & ME4600_AI_STATUS_BIT_EF_DATA)) {
+		if (delay && ((jiffies - j) >= delay)) {
+			if (!(instance->single_config[channel].ctrl & ME4600_AI_CTRL_BIT_EX_TRIG)) {	// Software start.
+				PERROR("Value not available after wait.\n");
+				err = ME_ERRNO_INTERNAL;
+			} else {	// External start.
+				PERROR("Timeout reached.\n");
+				err = ME_ERRNO_TIMEOUT;
+			}
+			break;
+		}
+		// Wait
+		set_current_state(TASK_INTERRUPTIBLE);
+		schedule_timeout(1);
+
+		if (signal_pending(current)) {
+			PERROR
+			    ("Wait on external trigger interrupted by signal.\n");
+			err = ME_ERRNO_SIGNAL;
+			break;
+		}
+
+		if (instance->status != ai_status_single_configured) {
+			PERROR("Wait interrupted by reset.\n");
+			err = ME_ERRNO_CANCELLED;
+			break;
+		}
+	}
+
+	// Read value.
+	if (!err) {
+		val = inl(instance->data_reg) ^ 0x8000;
+		PDEBUG_REG("data_reg inl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->data_reg - instance->reg_base, val);
+		*value = val & ME4600_AI_MAX_DATA;
+	} else {
+		*value = 0xFFFFFFFF;
+	}
+
+	// Restore settings.
+	spin_lock_irqsave(instance->ctrl_reg_lock, cpu_flags);
+	tmp = inl(instance->ctrl_reg);
+	// Clear FIFOs and dissable interrupts.
+	tmp &=
+	    ~(ME4600_AI_CTRL_BIT_CHANNEL_FIFO | ME4600_AI_CTRL_BIT_DATA_FIFO);
+	tmp |= ME4600_AI_CTRL_BIT_SC_IRQ | ME4600_AI_CTRL_BIT_HF_IRQ;
+	tmp |=
+	    ME4600_AI_CTRL_BIT_SC_IRQ_RESET | ME4600_AI_CTRL_BIT_HF_IRQ_RESET |
+	    ME4600_AI_CTRL_BIT_LE_IRQ_RESET | ME4600_AI_CTRL_BIT_IMMEDIATE_STOP;
+	outl(tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp);
+	spin_unlock_irqrestore(instance->ctrl_reg_lock, cpu_flags);
+
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ai_io_stream_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      meIOStreamConfig_t * config_list,
+				      int count,
+				      meIOStreamTrigger_t * trigger,
+				      int fifo_irq_threshold, int flags)
+{
+	me4600_ai_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	int i;			// internal multipurpose variable
+	unsigned long long data_required;
+
+	volatile uint32_t entry;
+	volatile uint32_t ctrl = ME4600_AI_CTRL_BIT_IMMEDIATE_STOP;
+	volatile uint32_t tmp;	// use when current copy of register's value needed
+	unsigned long cpu_flags;
+
+	uint64_t acq_ticks;
+	uint64_t scan_ticks;
+	uint64_t conv_ticks;
+	unsigned int acq_start_ticks_low = trigger->iAcqStartTicksLow;
+	unsigned int acq_start_ticks_high = trigger->iAcqStartTicksHigh;
+	unsigned int scan_start_ticks_low = trigger->iScanStartTicksLow;
+	unsigned int scan_start_ticks_high = trigger->iScanStartTicksHigh;
+	unsigned int conv_start_ticks_low = trigger->iConvStartTicksLow;
+	unsigned int conv_start_ticks_high = trigger->iConvStartTicksHigh;
+
+	PDEBUG("executed. idx=0\n");
+
+	instance = (me4600_ai_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER
+	    // Convert ticks to 64 bit long values
+	    acq_ticks =
+	    (uint64_t) acq_start_ticks_low +
+	    ((uint64_t) acq_start_ticks_high << 32);
+	scan_ticks =
+	    (uint64_t) scan_start_ticks_low +
+	    ((uint64_t) scan_start_ticks_high << 32);
+	conv_ticks =
+	    (uint64_t) conv_start_ticks_low +
+	    ((uint64_t) conv_start_ticks_high << 32);
+
+	// Check settings - begin
+	switch (trigger->iAcqStartTrigType) {
+	case ME_TRIG_TYPE_SW:
+	case ME_TRIG_TYPE_EXT_DIGITAL:
+	case ME_TRIG_TYPE_EXT_ANALOG:
+		break;
+
+	default:
+		PERROR("Invalid acquisition start trigger type specified.\n");
+		err = ME_ERRNO_INVALID_ACQ_START_TRIG_TYPE;
+		goto ERROR;
+		break;
+	}
+
+	if ((trigger->iAcqStartTrigType == ME_TRIG_TYPE_SW)
+	    && (trigger->iAcqStartTrigEdge != ME_TRIG_EDGE_NONE)) {
+		PERROR("Invalid acquisition start trigger edge specified.\n");
+		err = ME_ERRNO_INVALID_ACQ_START_TRIG_EDGE;
+		goto ERROR;
+	}
+
+	if (trigger->iAcqStartTrigType != ME_TRIG_TYPE_SW) {
+		switch (trigger->iAcqStartTrigEdge) {
+		case ME_TRIG_EDGE_RISING:
+		case ME_TRIG_EDGE_FALLING:
+		case ME_TRIG_EDGE_ANY:
+			break;
+
+		default:
+			PERROR
+			    ("Invalid acquisition start trigger edge specified.\n");
+			err = ME_ERRNO_INVALID_ACQ_START_TRIG_EDGE;
+			goto ERROR;
+			break;
+		}
+	}
+
+	if (trigger->iAcqStartTrigChan != ME_TRIG_CHAN_DEFAULT) {
+		PERROR
+		    ("Invalid acquisition start trigger channel specified.\n");
+		err = ME_ERRNO_INVALID_ACQ_START_TRIG_CHAN;
+		goto ERROR;
+	}
+
+	if ((acq_ticks < ME4600_AI_MIN_ACQ_TICKS)
+	    || (acq_ticks > ME4600_AI_MAX_ACQ_TICKS)) {
+		PERROR
+		    ("Invalid acquisition start trigger argument specified.\n");
+		err = ME_ERRNO_INVALID_ACQ_START_ARG;
+		goto ERROR;
+	}
+
+	switch (trigger->iScanStartTrigType) {
+
+	case ME_TRIG_TYPE_TIMER:
+		if ((scan_ticks < ME4600_AI_MIN_SCAN_TICKS)
+		    || (scan_ticks > ME4600_AI_MAX_SCAN_TICKS)
+		    || (scan_ticks < count * conv_ticks)
+		    ) {
+			PERROR("Invalid scan start argument specified.\n");
+			err = ME_ERRNO_INVALID_SCAN_START_ARG;
+			goto ERROR;
+		}
+		break;
+
+	case ME_TRIG_TYPE_EXT_DIGITAL:
+		if (trigger->iAcqStartTrigType != ME_TRIG_TYPE_EXT_DIGITAL) {
+			PERROR
+			    ("Invalid scan start trigger type specified (Acq is HW digital)\n");
+			err = ME_ERRNO_INVALID_SCAN_START_TRIG_TYPE;
+			goto ERROR;
+		}
+		break;
+
+	case ME_TRIG_TYPE_EXT_ANALOG:
+		if (trigger->iAcqStartTrigType != ME_TRIG_TYPE_EXT_ANALOG) {
+			PERROR
+			    ("Invalid scan start trigger type specified (Acq is HW analog)\n");
+			err = ME_ERRNO_INVALID_SCAN_START_TRIG_TYPE;
+			goto ERROR;
+		}
+		break;
+
+	case ME_TRIG_TYPE_FOLLOW:
+		break;
+
+	default:
+		PERROR("Invalid scan start trigger type specified.\n");
+		err = ME_ERRNO_INVALID_SCAN_START_TRIG_TYPE;
+		goto ERROR;
+		break;
+	}
+
+	switch (trigger->iConvStartTrigType) {
+
+	case ME_TRIG_TYPE_TIMER:
+		if ((conv_ticks < ME4600_AI_MIN_CHAN_TICKS)
+		    || (conv_ticks > ME4600_AI_MAX_CHAN_TICKS)) {
+			PERROR
+			    ("Invalid conv start trigger argument specified.\n");
+			err = ME_ERRNO_INVALID_CONV_START_ARG;
+			goto ERROR;
+		}
+		break;
+
+	case ME_TRIG_TYPE_EXT_DIGITAL:
+		if ((trigger->iScanStartTrigType != ME_TRIG_TYPE_FOLLOW)
+		    || (trigger->iAcqStartTrigType !=
+			ME_TRIG_TYPE_EXT_DIGITAL)) {
+			PERROR("Invalid conv start trigger type specified.\n");
+			err = ME_ERRNO_INVALID_CONV_START_TRIG_TYPE;
+			goto ERROR;
+		}
+		break;
+
+	case ME_TRIG_TYPE_EXT_ANALOG:
+		if ((trigger->iScanStartTrigType != ME_TRIG_TYPE_FOLLOW)
+		    || (trigger->iAcqStartTrigType !=
+			ME_TRIG_TYPE_EXT_ANALOG)) {
+			PERROR("Invalid conv start trigger type specified.\n");
+			err = ME_ERRNO_INVALID_CONV_START_TRIG_TYPE;
+			goto ERROR;
+		}
+		break;
+
+	default:
+		PERROR("Invalid conv start trigger type specified.\n");
+		err = ME_ERRNO_INVALID_CONV_START_TRIG_TYPE;
+		goto ERROR;
+
+		break;
+	}
+/**
+* Aceptable settings:
+* iScanStopTrigType		:	iAcqStopTrigType
+*
+* ME_TRIG_TYPE_NONE		:	ME_TRIG_TYPE_NONE	-> infinite count with manual stop
+* ME_TRIG_TYPE_NONE		:	ME_TRIG_TYPE_COUNT	-> stop after getting iScanStopCount list of values (iScanStopCount * count)
+* ME_TRIG_TYPE_COUNT	:	ME_TRIG_TYPE_FOLLOW	-> stop after getting iAcqStopCount values (it can stops in midle of the list)
+*/
+	switch (trigger->iScanStopTrigType) {
+
+	case ME_TRIG_TYPE_NONE:
+		break;
+
+	case ME_TRIG_TYPE_COUNT:
+		if (trigger->iScanStopCount <= 0) {
+			PERROR("Invalid scan stop argument specified.\n");
+			err = ME_ERRNO_INVALID_SCAN_STOP_ARG;
+			goto ERROR;
+		}
+		break;
+
+	default:
+		PERROR("Invalid scan stop trigger type specified.\n");
+		err = ME_ERRNO_INVALID_SCAN_STOP_TRIG_TYPE;
+		goto ERROR;
+		break;
+	}
+
+	switch (trigger->iAcqStopTrigType) {
+
+	case ME_TRIG_TYPE_NONE:
+		if (trigger->iScanStopTrigType != ME_TRIG_TYPE_NONE) {
+			PERROR("Invalid acq stop trigger type specified.\n");
+			err = ME_ERRNO_INVALID_ACQ_STOP_TRIG_TYPE;
+			goto ERROR;
+		}
+		break;
+
+	case ME_TRIG_TYPE_FOLLOW:
+		if (trigger->iScanStopTrigType != ME_TRIG_TYPE_COUNT) {
+			PERROR("Invalid acq stop trigger type specified.\n");
+			err = ME_ERRNO_INVALID_ACQ_STOP_TRIG_TYPE;
+			goto ERROR;
+		}
+		break;
+
+	case ME_TRIG_TYPE_COUNT:
+		if (trigger->iScanStopTrigType != ME_TRIG_TYPE_NONE) {
+			PERROR("Invalid acq stop trigger type specified.\n");
+			err = ME_ERRNO_INVALID_ACQ_STOP_TRIG_TYPE;
+			goto ERROR;
+		}
+
+		if (trigger->iAcqStopCount <= 0) {
+			PERROR
+			    ("Invalid acquisition or scan stop argument specified.\n");
+			err = ME_ERRNO_INVALID_ACQ_STOP_ARG;
+			goto ERROR;
+		}
+		break;
+
+	default:
+		PERROR("Invalid acq stop trigger type specified.\n");
+		err = ME_ERRNO_INVALID_ACQ_STOP_TRIG_TYPE;
+		goto ERROR;
+		break;
+	}
+
+	if ((count <= 0) || (count > ME4600_AI_LIST_COUNT)) {
+		PERROR("Invalid channel list count specified.\n");
+		err = ME_ERRNO_INVALID_CONFIG_LIST_COUNT;
+		goto ERROR;
+	}
+///This is general limitation
+//      if (fifo_irq_threshold < 0 || fifo_irq_threshold >= ME4600_AI_CIRC_BUF_COUNT)
+///This is limitation from Windows. I use it for compatibility.
+	if (fifo_irq_threshold < 0
+	    || fifo_irq_threshold >= ME4600_AI_FIFO_COUNT) {
+		PERROR("Invalid fifo irq threshold specified.\n");
+		err = ME_ERRNO_INVALID_FIFO_IRQ_THRESHOLD;
+		goto ERROR;
+	}
+
+	if ((config_list[0].iRef == ME_REF_AI_DIFFERENTIAL)
+	    && (instance->channels == 16)) {
+		PERROR
+		    ("Differential reference is not available on this subdevice.\n");
+		err = ME_ERRNO_INVALID_REF;
+		goto ERROR;
+	}
+
+	if (flags & ME_IO_STREAM_CONFIG_SAMPLE_AND_HOLD) {
+		if (!instance->sh) {
+			PERROR
+			    ("Sample and hold is not available for this board.\n");
+			err = ME_ERRNO_INVALID_FLAGS;
+			goto ERROR;
+		}
+		if (config_list[0].iRef == ME_REF_AI_DIFFERENTIAL) {
+			PERROR
+			    ("Sample and hold is not available in differential mode.\n");
+			err = ME_ERRNO_INVALID_FLAGS;
+			goto ERROR;
+		}
+	}
+
+	for (i = 0; i < count; i++) {
+		if ((config_list[i].iStreamConfig < 0)
+		    || (config_list[i].iStreamConfig >= instance->ranges_len)) {
+			PERROR("Invalid stream config specified.\n");
+			err = ME_ERRNO_INVALID_STREAM_CONFIG;
+			goto ERROR;
+		}
+
+		if ((config_list[i].iRef != ME_REF_AI_GROUND)
+		    && (config_list[i].iRef != ME_REF_AI_DIFFERENTIAL)) {
+			PERROR("Invalid references in the list. Ref=0x%x\n",
+			       config_list[i].iRef);
+			err = ME_ERRNO_INVALID_REF;
+			goto ERROR;
+		}
+
+		if (config_list[i].iStreamConfig % 2) {	// StreamConfig: 1 or 3
+			if (config_list[i].iRef == ME_REF_AI_DIFFERENTIAL) {
+				PERROR
+				    ("Only bipolar modes support differential measurement.\n");
+				err = ME_ERRNO_INVALID_REF;
+				goto ERROR;
+			}
+		}
+
+		if (config_list[i].iRef != config_list[0].iRef) {
+			PERROR
+			    ("Not all references in the configuration list are equal. Ref[0]=0x%x Ref[%d]=0x%x\n",
+			     config_list[0].iRef, i, config_list[i].iRef);
+			err = ME_ERRNO_INVALID_REF;
+			goto ERROR;
+		}
+
+		if ((config_list[i].iRef == ME_REF_AI_DIFFERENTIAL)
+		    && (config_list[i].iChannel >= 16)) {
+			PERROR("Channel not available in differential mode.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+			goto ERROR;
+		}
+
+		if ((config_list[i].iChannel < 0)
+		    || (config_list[i].iChannel >= instance->channels)) {
+			PERROR("Invalid channel number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+			goto ERROR;
+		}
+	}
+
+	// Check settings - end
+
+	//Cancel control task
+	PDEBUG("Cancel control task.\n");
+	instance->ai_control_task_flag = 0;
+	cancel_delayed_work(&instance->ai_control_task);
+
+	// Work around from Keith Hartley - begin
+	if (trigger->iScanStartTrigType == ME_TRIG_TYPE_TIMER) {
+		if (count == 1) {
+			// The hardware does not work properly with a non-zero scan time
+			// if there is only ONE channel in the channel list. In this case
+			// we must set the scan time to zero and use the channel time.
+
+			conv_ticks = scan_ticks;
+			trigger->iScanStartTrigType = ME_TRIG_TYPE_FOLLOW;
+		} else if (scan_ticks == count * conv_ticks) {
+			// Another hardware problem. If the number of scan ticks is
+			// exactly equal to the number of channel ticks multiplied by
+			// the number of channels then the sampling rate is reduced
+			// by half.
+			trigger->iScanStartTrigType = ME_TRIG_TYPE_FOLLOW;
+		}
+	}
+	// Work around from Keith Hartley - end
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+
+	if (inl(instance->status_reg) & ME4600_AI_STATUS_BIT_FSM) {
+		PERROR("Subdevice is busy.\n");
+		spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+		ME_SUBDEVICE_EXIT;
+		return ME_ERRNO_SUBDEVICE_BUSY;
+	}
+
+	instance->status = ai_status_none;
+	spin_lock_irqsave(instance->ctrl_reg_lock, cpu_flags);
+	// Stop all actions. Block all interrupts. Clear (disable) FIFOs.
+	ctrl =
+	    ME4600_AI_CTRL_BIT_LE_IRQ_RESET | ME4600_AI_CTRL_BIT_HF_IRQ_RESET |
+	    ME4600_AI_CTRL_BIT_SC_IRQ_RESET;
+
+	tmp = inl(instance->ctrl_reg);
+	// Preserve EXT IRQ and OFFSET settings. Clean other bits.
+	tmp &=
+	    (ME4600_AI_CTRL_BIT_EX_IRQ | ME4600_AI_CTRL_BIT_EX_IRQ_RESET |
+	     ME4600_AI_CTRL_BIT_FULLSCALE | ME4600_AI_CTRL_BIT_OFFSET);
+
+	// Send it to register.
+	outl(tmp | ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp | ctrl);
+
+	// Enable channel fifo -> data fifo in stream_start().
+	ctrl |= ME4600_AI_CTRL_BIT_CHANNEL_FIFO;
+	outl(tmp | ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp | ctrl);
+	spin_unlock_irqrestore(instance->ctrl_reg_lock, cpu_flags);
+
+	// Write the channel list
+	for (i = 0; i < count; i++) {
+		entry = config_list[i].iChannel;
+
+		switch (config_list[i].iStreamConfig) {
+		case 0:	//BIPOLAR 10V
+/*
+				// ME4600_AI_LIST_RANGE_BIPOLAR_10 = 0x0000
+				// 'entry |= ME4600_AI_LIST_RANGE_BIPOLAR_10' <== Do nothing. Removed.
+				entry |= ME4600_AI_LIST_RANGE_BIPOLAR_10;
+*/
+			break;
+		case 1:	//UNIPOLAR 10V
+			entry |= ME4600_AI_LIST_RANGE_UNIPOLAR_10;
+			break;
+		case 2:	//BIPOLAR 2.5V
+			entry |= ME4600_AI_LIST_RANGE_BIPOLAR_2_5;
+			break;
+		case 3:	//UNIPOLAR 2.5V
+			entry |= ME4600_AI_LIST_RANGE_UNIPOLAR_2_5;
+			break;
+		default:
+			PERROR_CRITICAL("UNCHECK ERROR in config_list!\n");
+			PERROR_CRITICAL
+			    ("WRONG range\nPosition:%d Range:0x%04X\n", i,
+			     config_list[i].iStreamConfig);
+			goto VERIFY_ERROR;
+			break;
+		}
+
+		switch (config_list[i].iRef) {
+		case ME_REF_AI_GROUND:	//SINGLE ENDED
+/*
+				// ME4600_AI_LIST_INPUT_SINGLE_ENDED = 0x0000
+				// 'entry |= ME4600_AI_LIST_INPUT_SINGLE_ENDED' ==> Do nothing. Removed.
+				entry |= ME4600_AI_LIST_INPUT_SINGLE_ENDED;
+*/ break;
+		case ME_REF_AI_DIFFERENTIAL:	//DIFFERENTIAL
+			entry |= ME4600_AI_LIST_INPUT_DIFFERENTIAL;
+			break;
+		default:
+			PERROR_CRITICAL("UNCHECK ERROR in config_list!\n");
+			PERROR_CRITICAL
+			    ("WRONG reference\nPosition:%d Reference:0x%04X\n",
+			     i, config_list[i].iRef);
+			goto VERIFY_ERROR;
+			break;
+		}
+
+		//Add last entry flag
+		if (i == (count - 1)) {
+			entry |= ME4600_AI_LIST_LAST_ENTRY;
+		}
+
+		outl(entry, instance->channel_list_reg);
+		PDEBUG_REG("channel_list_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->channel_list_reg - instance->reg_base,
+			   entry);
+	}
+
+	// Set triggering registers
+	--acq_ticks;
+	outl(acq_ticks, instance->chan_pre_timer_reg);
+	PDEBUG_REG("chan_pre_timer_reg outl(0x%lX+0x%lX)=0x%llX\n",
+		   instance->reg_base,
+		   instance->chan_pre_timer_reg - instance->reg_base,
+		   acq_ticks);
+	outl(acq_ticks, instance->scan_pre_timer_low_reg);
+	PDEBUG_REG("scan_pre_timer_low_reg outl(0x%lX+0x%lX)=0x%llX\n",
+		   instance->reg_base,
+		   instance->scan_pre_timer_low_reg - instance->reg_base,
+		   acq_ticks & 0xFFFFFFFF);
+	outl((acq_ticks >> 32), instance->scan_pre_timer_high_reg);
+	PDEBUG_REG("scan_pre_timer_high_reg outl(0x%lX+0x%lX)=0x%llX\n",
+		   instance->reg_base,
+		   instance->scan_pre_timer_high_reg - instance->reg_base,
+		   (acq_ticks >> 32) & 0xFFFFFFFF);
+
+	// Set triggers
+	switch (trigger->iAcqStartTrigType) {
+		// Internal
+	case ME_TRIG_TYPE_SW:
+		// Nothing to set.
+		break;
+
+		// External
+	case ME_TRIG_TYPE_EXT_ANALOG:
+		ctrl |= ME4600_AI_CTRL_BIT_EX_TRIG_ANALOG;
+	case ME_TRIG_TYPE_EXT_DIGITAL:
+		ctrl |= ME4600_AI_CTRL_BIT_EX_TRIG;
+
+		// External trigger needs edge's definition
+		switch (trigger->iAcqStartTrigEdge) {
+		case ME_TRIG_EDGE_RISING:
+			// Nothing to set.
+			break;
+
+		case ME_TRIG_EDGE_FALLING:
+			ctrl |= ME4600_AI_CTRL_BIT_EX_TRIG_FALLING;
+			break;
+
+		case ME_TRIG_EDGE_ANY:
+			ctrl |=
+			    ME4600_AI_CTRL_BIT_EX_TRIG_FALLING |
+			    ME4600_AI_CTRL_BIT_EX_TRIG_BOTH;
+			break;
+
+		default:
+			PERROR_CRITICAL
+			    ("UNCHECK TRIGGER EDGE in triggers structure!\n");
+			PERROR_CRITICAL
+			    ("WRONG acquisition start trigger:0x%04X.\n",
+			     trigger->iAcqStartTrigEdge);
+			err = ME_ERRNO_INVALID_ACQ_START_TRIG_EDGE;
+			goto VERIFY_ERROR;
+			break;
+		}
+		break;
+
+	default:
+		PERROR_CRITICAL("UNCHECK TRIGGER in triggers structure!\n");
+		PERROR_CRITICAL("WRONG acquisition start trigger:0x%04X.\n",
+				trigger->iAcqStartTrigType);
+		err = ME_ERRNO_INVALID_ACQ_START_TRIG_TYPE;
+		goto VERIFY_ERROR;
+		break;
+	}
+
+	switch (trigger->iScanStartTrigType) {
+	case ME_TRIG_TYPE_TIMER:
+		--scan_ticks;
+		outl(scan_ticks, instance->scan_timer_low_reg);
+		PDEBUG_REG("scan_timer_low_reg outl(0x%lX+0x%lX)=0x%llX\n",
+			   instance->reg_base,
+			   instance->scan_timer_low_reg - instance->reg_base,
+			   scan_ticks & 0xFFFFFFFF);
+		outl((scan_ticks >> 32), instance->scan_timer_high_reg);
+		PDEBUG_REG("scan_timer_high_reg outl(0x%lX+0x%lX)=0x%llX\n",
+			   instance->reg_base,
+			   instance->scan_timer_high_reg - instance->reg_base,
+			   (scan_ticks >> 32) & 0xFFFFFFFF);
+
+		if (trigger->iAcqStartTrigType == ME_TRIG_TYPE_SW) {
+			ctrl |= ME4600_AI_CTRL_BIT_MODE_0;
+		} else {
+			ctrl |= ME4600_AI_CTRL_BIT_MODE_1;
+		}
+		break;
+
+	case ME_TRIG_TYPE_EXT_DIGITAL:
+	case ME_TRIG_TYPE_EXT_ANALOG:
+		outl(0, instance->scan_timer_low_reg);
+		PDEBUG_REG("scan_timer_low_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->scan_timer_low_reg - instance->reg_base,
+			   0);
+		outl(0, instance->scan_timer_high_reg);
+		PDEBUG_REG("scan_timer_high_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->scan_timer_high_reg - instance->reg_base,
+			   0);
+		ctrl |= ME4600_AI_CTRL_BIT_MODE_2;
+		break;
+
+	case ME_TRIG_TYPE_FOLLOW:
+		outl(0, instance->scan_timer_low_reg);
+		PDEBUG_REG("scan_timer_low_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->scan_timer_low_reg - instance->reg_base,
+			   0);
+		outl(0, instance->scan_timer_high_reg);
+		PDEBUG_REG("scan_timer_high_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->scan_timer_high_reg - instance->reg_base,
+			   0);
+
+		if (trigger->iAcqStartTrigType == ME_TRIG_TYPE_SW) {
+			ctrl |= ME4600_AI_CTRL_BIT_MODE_0;
+		} else {
+			ctrl |= ME4600_AI_CTRL_BIT_MODE_1;
+		}
+		break;
+
+	default:
+		PERROR_CRITICAL("UNCHECK TRIGGER in triggers structure!\n");
+		PERROR_CRITICAL("WRONG scan start trigger:0x%04X.\n",
+				trigger->iScanStartTrigType);
+		err = ME_ERRNO_INVALID_SCAN_START_TRIG_TYPE;
+		goto VERIFY_ERROR;
+		break;
+	}
+
+	switch (trigger->iConvStartTrigType) {
+
+	case ME_TRIG_TYPE_TIMER:
+		--conv_ticks;
+		outl(conv_ticks, instance->chan_timer_reg);
+		PDEBUG_REG("chan_timer_reg outl(0x%lX+0x%lX)=0x%llX\n",
+			   instance->reg_base,
+			   instance->chan_timer_reg - instance->reg_base,
+			   conv_ticks);
+		break;
+
+	case ME_TRIG_TYPE_EXT_DIGITAL:
+	case ME_TRIG_TYPE_EXT_ANALOG:
+		outl(0, instance->chan_timer_reg);
+		PDEBUG_REG("chan_timer_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->chan_timer_reg - instance->reg_base, 0);
+		ctrl |= ME4600_AI_CTRL_BIT_MODE_0 | ME4600_AI_CTRL_BIT_MODE_1;
+		break;
+
+	default:
+		PERROR_CRITICAL("UNCHECK TRIGGER in triggers structure!\n");
+		PERROR_CRITICAL("WRONG conv start trigger:0x%04X.\n",
+				trigger->iConvStartTrigType);
+		err = ME_ERRNO_INVALID_CONV_START_TRIG_TYPE;
+		goto VERIFY_ERROR;
+
+		break;
+	}
+
+	//Sample & Hold feature
+	if (flags & ME_IO_STREAM_CONFIG_SAMPLE_AND_HOLD) {
+		if (instance->sh) {
+			ctrl |= ME4600_AI_CTRL_BIT_SAMPLE_HOLD;
+		} else {
+			PERROR_CRITICAL("UNCHECK S&H feature!\n");
+			err = ME_ERRNO_INVALID_FLAGS;
+			goto VERIFY_ERROR;
+		}
+	}
+	//Enable IRQs sources but leave latches blocked.
+	ctrl |= (ME4600_AI_CTRL_BIT_HF_IRQ | ME4600_AI_CTRL_BIT_SC_IRQ | ME4600_AI_CTRL_BIT_LE_IRQ);	//The last IRQ source (ME4600_AI_CTRL_BIT_LE_IRQ) is unused!
+
+	//Everything is good. Finalize
+	spin_lock_irqsave(instance->ctrl_reg_lock, cpu_flags);
+	tmp = inl(instance->ctrl_reg);
+
+	//Preserve EXT IRQ and OFFSET settings. Clean other bits.
+	tmp &=
+	    (ME4600_AI_CTRL_BIT_EX_IRQ | ME4600_AI_CTRL_BIT_EX_IRQ_RESET |
+	     ME4600_AI_CTRL_BIT_FULLSCALE | ME4600_AI_CTRL_BIT_OFFSET);
+
+	// write the control word
+	outl(ctrl | tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl | tmp);
+	spin_unlock_irqrestore(instance->ctrl_reg_lock, cpu_flags);
+
+	//Set the global parameters end exit.
+	instance->chan_list_len = count;
+	instance->fifo_irq_threshold = fifo_irq_threshold;
+
+	if (trigger->iAcqStopTrigType == ME_TRIG_TYPE_COUNT) {
+		data_required =
+		    (unsigned long long)trigger->iAcqStopCount *
+		    (unsigned long long)count;
+		if (data_required > UINT_MAX)
+			data_required = UINT_MAX;
+		instance->data_required = (unsigned int)data_required;
+	} else if (trigger->iScanStopTrigType == ME_TRIG_TYPE_COUNT)
+		instance->data_required =
+		    (unsigned long long)trigger->iScanStopCount;
+	else
+		instance->data_required = 0;
+
+	// Mark subdevice as configured to work in stream mode.
+	instance->status = ai_status_stream_configured;
+
+	// Deinit single config. Set all entries to NOT_CONFIGURED.
+	for (i = 0; i < instance->channels; i++) {
+		instance->single_config[i].status =
+		    ME_SINGLE_CHANNEL_NOT_CONFIGURED;
+	}
+
+      VERIFY_ERROR:		// Error in code. Wrong setting check. This should never ever happend!
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+      ERROR:			// Error in settings.
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ai_io_stream_new_values(me_subdevice_t * subdevice,
+					  struct file *filep,
+					  int time_out, int *count, int flags)
+{
+	me4600_ai_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long t;
+	unsigned long j;
+	int volatile head;
+
+	PDEBUG("executed. idx=0\n");
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid time_out specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	if (time_out) {
+		t = (time_out * HZ) / 1000;
+
+		if (t == 0)
+			t = 1;
+	} else {		// Max time.
+		t = LONG_MAX;
+	}
+
+	instance = (me4600_ai_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	j = jiffies;
+
+	while (1) {
+		// Only runing device can generate break.
+		head = instance->circ_buf.head;
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 ((head !=
+						   instance->circ_buf.head)
+						  ||
+						  ((instance->status <=
+						    ai_status_stream_run_wait)
+						   && (instance->status >=
+						       ai_status_stream_end_wait))),
+						 t);
+
+		if (head != instance->circ_buf.head) {	// New data in buffer.
+			break;
+		} else if (instance->status == ai_status_stream_end) {	// End of work.
+			break;
+		} else if (instance->status == ai_status_stream_fifo_error) {
+			err = ME_ERRNO_FIFO_BUFFER_OVERFLOW;
+			break;
+		} else if (instance->status == ai_status_stream_buffer_error) {
+			err = ME_ERRNO_RING_BUFFER_OVERFLOW;
+			break;
+		} else if (instance->status == ai_status_stream_error) {
+			err = ME_ERRNO_INTERNAL;
+			break;
+		} else if ((jiffies - j) >= t) {
+			PERROR("Wait on values timed out.\n");
+			err = ME_ERRNO_TIMEOUT;
+			break;
+		} else if (signal_pending(current)) {
+			PERROR("Wait on values interrupted from signal.\n");
+			err = ME_ERRNO_SIGNAL;
+			break;
+		}
+		// Correct timeout.
+		t -= jiffies - j;
+	}
+
+	*count = me_circ_buf_values(&instance->circ_buf);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int inline me4600_ai_io_stream_read_get_value(me4600_ai_subdevice_t *
+						     instance, int *values,
+						     const int count,
+						     const int flags)
+{
+	int n;
+	int i;
+	uint32_t value;
+
+	///Checking how many datas can be copied.
+	n = me_circ_buf_values(&instance->circ_buf);
+	if (n <= 0)
+		return 0;
+
+	if (n > count)
+		n = count;
+
+	if (flags & ME_IO_STREAM_READ_FRAMES) {
+		if (n < instance->chan_list_len)	//Not enough data!
+			return 0;
+		n -= n % instance->chan_list_len;
+	}
+
+	for (i = 0; i < n; i++) {
+		value = *(instance->circ_buf.buf + instance->circ_buf.tail);
+		if (put_user(value, values + i)) {
+			PERROR("Cannot copy new values to user.\n");
+			return -ME_ERRNO_INTERNAL;
+		}
+		instance->circ_buf.tail++;
+		instance->circ_buf.tail &= instance->circ_buf.mask;
+	}
+	return n;
+}
+
+static int me4600_ai_io_stream_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int read_mode,
+				    int *values, int *count, int flags)
+{
+	me4600_ai_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	int ret;
+
+	int c = *count;
+	int min = c;
+
+	PDEBUG("executed. idx=0\n");
+
+	if (flags & ~ME_IO_STREAM_READ_FRAMES) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (!values || !count) {
+		PERROR("Request has invalid pointer.\n");
+		return ME_ERRNO_INVALID_POINTER;
+	}
+
+	if (c < 0) {
+		PERROR("Request has invalid value's counter.\n");
+		return ME_ERRNO_INVALID_VALUE_COUNT;
+	}
+
+	if ((read_mode != ME_READ_MODE_BLOCKING)
+	    && (read_mode != ME_READ_MODE_NONBLOCKING)) {
+		PERROR("Invalid read mode specified.\n");
+		return ME_ERRNO_INVALID_READ_MODE;
+	}
+
+	if (c == 0) {		//You get what you want! Nothing more or less.
+		return ME_ERRNO_SUCCESS;
+	}
+
+	instance = (me4600_ai_subdevice_t *) subdevice;
+	ME_SUBDEVICE_ENTER;
+
+	//Check if subdevice is configured.
+	if (instance->chan_list_len <= 0) {
+		PERROR("Subdevice wasn't configured.\n");
+		ME_SUBDEVICE_EXIT;
+		return ME_ERRNO_PREVIOUS_CONFIG;
+	}
+
+	if (flags & ME_IO_STREAM_READ_FRAMES) {
+		if (c < instance->chan_list_len) {	//Not enough data requested.
+			PERROR
+			    ("When using FRAME_READ mode minimal size is defined by channel list.\n");
+			ME_SUBDEVICE_EXIT;
+			return ME_ERRNO_INVALID_VALUE_COUNT;
+		}
+	}
+
+	if (c > (ME4600_AI_CIRC_BUF_COUNT - instance->chan_list_len)) {	// To return acceptable amount of data when user pass too big value.
+		min = ME4600_AI_CIRC_BUF_COUNT - instance->chan_list_len;
+	}
+
+	if (flags & ME_IO_STREAM_READ_FRAMES) {
+		//Wait for whole list.
+		if (read_mode == ME_READ_MODE_BLOCKING) {
+			min = c - (c % instance->chan_list_len);
+		}
+
+		if (read_mode == ME_READ_MODE_NONBLOCKING) {
+			min = instance->chan_list_len;
+		}
+	}
+
+	if ((inl(instance->status_reg) & ME4600_AI_STATUS_BIT_FSM)) {	//Working
+		//If blocking mode -> wait for data.
+		if ((me_circ_buf_values(&instance->circ_buf) < min)
+		    && (read_mode == ME_READ_MODE_BLOCKING)) {
+			wait_event_interruptible(instance->wait_queue,
+						 ((me_circ_buf_values
+						   (&instance->circ_buf) >= min)
+						  || !(inl(instance->status_reg)
+						       &
+						       ME4600_AI_STATUS_BIT_FSM)));
+
+			if (signal_pending(current)) {
+				PERROR
+				    ("Wait on values interrupted from signal.\n");
+				err = ME_ERRNO_SIGNAL;
+			}
+		}
+	}
+
+	ret = me4600_ai_io_stream_read_get_value(instance, values, c, flags);
+	if (ret < 0) {
+		err = -ret;
+		*count = 0;
+	} else if (ret == 0) {
+		*count = 0;
+		if (instance->status == ai_status_stream_fifo_error) {
+			err = ME_ERRNO_FIFO_BUFFER_OVERFLOW;
+			instance->status = ai_status_stream_end;
+		} else if (instance->status == ai_status_stream_buffer_error) {
+			err = ME_ERRNO_RING_BUFFER_OVERFLOW;
+			instance->status = ai_status_stream_end;
+		} else if (instance->status == ai_status_stream_end) {
+			err = ME_ERRNO_SUBDEVICE_NOT_RUNNING;
+		} else if (instance->status == ai_status_stream_error) {
+			err = ME_ERRNO_INTERNAL;
+		} else if (instance->status == ai_status_none) {
+			PDEBUG("Stream canceled.\n");
+			err = ME_ERRNO_INTERNAL;
+		}
+	} else {
+		*count = ret;
+	}
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+/** @brief Stop aqusation. Preserve FIFOs.
+*
+* @param instance The subdevice instance (pointer).
+*/
+
+static int ai_stop_immediately(me4600_ai_subdevice_t * instance)
+{
+	unsigned long cpu_flags = 0;
+	volatile uint32_t ctrl;
+	const int timeout = HZ / 10;	//100ms
+	int i;
+
+	for (i = 0; i <= timeout; i++) {
+		spin_lock_irqsave(instance->ctrl_reg_lock, cpu_flags);
+		ctrl = inl(instance->ctrl_reg);
+		ctrl &= ~ME4600_AI_CTRL_BIT_STOP;
+		ctrl |=
+		    (ME4600_AI_CTRL_BIT_IMMEDIATE_STOP |
+		     ME4600_AI_CTRL_BIT_HF_IRQ_RESET |
+		     ME4600_AI_CTRL_BIT_SC_IRQ_RESET);
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+		spin_unlock_irqrestore(instance->ctrl_reg_lock, cpu_flags);
+
+		if (!(inl(instance->status_reg) & ME4600_AI_STATUS_BIT_FSM)) {	// Exit.
+			break;
+		}
+
+		PINFO("Wait for stop: %d\n", i + 1);
+		//Still working!
+		set_current_state(TASK_INTERRUPTIBLE);
+		schedule_timeout(1);
+	}
+
+	if (i > timeout) {
+		PERROR_CRITICAL("FSM IS BUSY!\n");
+		return ME_ERRNO_INTERNAL;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_ai_io_stream_start(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int start_mode, int time_out, int flags)
+{
+	me4600_ai_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long cpu_flags = 0;
+	unsigned long ref;
+	unsigned long delay = 0;
+
+	volatile uint32_t tmp;
+
+	PDEBUG("executed. idx=0\n");
+
+	instance = (me4600_ai_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if ((start_mode != ME_START_MODE_BLOCKING)
+	    && (start_mode != ME_START_MODE_NONBLOCKING)) {
+		PERROR("Invalid start mode specified.\n");
+		return ME_ERRNO_INVALID_START_MODE;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid timeout specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	if (time_out) {
+		delay = (time_out * HZ) / 1000;
+
+		if (delay == 0)
+			delay = 1;
+	}
+
+	ME_SUBDEVICE_ENTER
+	    spin_lock_irqsave(instance->ctrl_reg_lock, cpu_flags);
+
+	tmp = inl(instance->ctrl_reg);
+
+	if ((tmp & ME4600_AI_STATUS_BIT_FSM)) {
+		PERROR("Conversion is already running.\n");
+		spin_unlock_irqrestore(instance->ctrl_reg_lock, cpu_flags);
+		err = ME_ERRNO_SUBDEVICE_BUSY;
+		goto ERROR;
+	}
+
+	if (instance->chan_list_len == 0) {	//Not configured!
+		PERROR("Subdevice is not configured to work in stream mode!\n");
+		spin_unlock_irqrestore(instance->ctrl_reg_lock, cpu_flags);
+		err = ME_ERRNO_PREVIOUS_CONFIG;
+		goto ERROR;
+	}
+
+	if (!(tmp & (ME4600_AI_CTRL_BIT_MODE_0 | ME4600_AI_CTRL_BIT_MODE_1 | ME4600_AI_CTRL_BIT_MODE_2))) {	//Mode 0 = single work => no stream config
+		PERROR("Subdevice is configured to work in single mode.\n");
+		spin_unlock_irqrestore(instance->ctrl_reg_lock, cpu_flags);
+		err = ME_ERRNO_PREVIOUS_CONFIG;
+		goto ERROR;
+	}
+	//Reset stop bits.
+	tmp |= ME4600_AI_CTRL_BIT_IMMEDIATE_STOP | ME4600_AI_CTRL_BIT_STOP;
+	outl(tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp);
+
+	//Start datas' FIFO.
+	tmp |= ME4600_AI_CTRL_BIT_DATA_FIFO;
+	//Free stop bits.
+	tmp &= ~(ME4600_AI_CTRL_BIT_IMMEDIATE_STOP | ME4600_AI_CTRL_BIT_STOP);
+	outl(tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp);
+	spin_unlock_irqrestore(instance->ctrl_reg_lock, cpu_flags);
+
+	//Cancel control task
+	PDEBUG("Cancel control task.\n");
+	instance->ai_control_task_flag = 0;
+	cancel_delayed_work(&instance->ai_control_task);
+
+	//Set the starting values.
+	instance->ISM.global_read = 0;
+	instance->ISM.read = 0;
+	//Clear circular buffer
+	instance->circ_buf.head = 0;
+	instance->circ_buf.tail = 0;
+
+	//Set everything.
+	ai_data_acquisition_logic(instance);
+
+	//Set status to 'wait for start'
+	instance->status = ai_status_stream_run_wait;
+
+	// Set control task's timeout
+	instance->timeout.delay = delay;
+	instance->timeout.start_time = jiffies;
+
+	//Lets go! Start work
+	inl(instance->start_reg);
+	PDEBUG_REG("start_reg inl(0x%lX+0x%lX)\n", instance->reg_base,
+		   instance->start_reg - instance->reg_base);
+
+	// Schedule control task
+	instance->ai_control_task_flag = 1;
+	queue_delayed_work(instance->me4600_workqueue,
+			   &instance->ai_control_task, 1);
+
+	PDEVELOP("Delay:%ld\n", delay);
+
+	if (start_mode == ME_START_MODE_BLOCKING) {	//Wait for start.
+		ref = jiffies;
+		//Only runing process will interrupt this call. Events are signaled when status change. Extra timeout add for safe reason.
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 (instance->status !=
+						  ai_status_stream_run_wait),
+						 (delay) ? delay +
+						 1 : LONG_MAX);
+
+		if ((instance->status != ai_status_stream_run)
+		    && (instance->status != ai_status_stream_end)) {
+			PDEBUG("Starting stream canceled. %d\n",
+			       instance->status);
+			err = ME_ERRNO_CANCELLED;
+		}
+
+		if (signal_pending(current)) {
+			PERROR("Wait on start of state machine interrupted.\n");
+			instance->status = ai_status_none;
+			ai_stop_isr(instance);
+			err = ME_ERRNO_SIGNAL;
+		} else if ((delay) && ((jiffies - ref) > delay)) {
+			if (instance->status != ai_status_stream_run) {
+				if (instance->status == ai_status_stream_end) {
+					PDEBUG("Timeout reached.\n");
+				} else if ((jiffies - ref) > delay + 1) {
+					PERROR
+					    ("Timeout reached. Not handled by control task!\n");
+					ai_stop_isr(instance);
+					instance->status =
+					    ai_status_stream_error;
+				} else {
+					PERROR
+					    ("Timeout reached. Signal come but status is strange: %d\n",
+					     instance->status);
+					ai_stop_isr(instance);
+					instance->status =
+					    ai_status_stream_error;
+				}
+
+				instance->ai_control_task_flag = 0;
+				cancel_delayed_work(&instance->ai_control_task);
+				err = ME_ERRNO_TIMEOUT;
+			}
+		}
+	}
+#ifdef MEDEBUG_INFO
+	tmp = inl(instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg inl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp);
+
+	PINFO("STATUS_BIT_FSM=%s.\n",
+	      (tmp & ME4600_AI_STATUS_BIT_FSM) ? "on" : "off");
+	PINFO("CTRL_BIT_HF_IRQ=%s.\n",
+	      (tmp & ME4600_AI_CTRL_BIT_HF_IRQ) ? "enable" : "disable");
+	PINFO("CTRL_BIT_HF_IRQ_RESET=%s.\n",
+	      (tmp & ME4600_AI_CTRL_BIT_HF_IRQ_RESET) ? "reset" : "work");
+	PINFO("CTRL_BIT_SC_IRQ=%s.\n",
+	      (tmp & ME4600_AI_CTRL_BIT_SC_IRQ) ? "enable" : "disable");
+	PINFO("CTRL_BIT_SC_RELOAD=%s.\n",
+	      (tmp & ME4600_AI_CTRL_BIT_SC_RELOAD) ? "on" : "off");
+	PINFO("CTRL_BIT_SC_IRQ_RESET=%s.\n",
+	      (tmp & ME4600_AI_CTRL_BIT_SC_IRQ_RESET) ? "reset" : "work");
+#endif
+
+      ERROR:
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ai_io_stream_status(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int wait,
+				      int *status, int *values, int flags)
+{
+	me4600_ai_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed. idx=0\n");
+
+	instance = (me4600_ai_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	switch (instance->status) {
+	case ai_status_single_configured:
+	case ai_status_stream_configured:
+	case ai_status_stream_end:
+	case ai_status_stream_fifo_error:
+	case ai_status_stream_buffer_error:
+	case ai_status_stream_error:
+		*status = ME_STATUS_IDLE;
+		break;
+
+	case ai_status_stream_run_wait:
+	case ai_status_stream_run:
+	case ai_status_stream_end_wait:
+		*status = ME_STATUS_BUSY;
+		break;
+
+	case ai_status_none:
+	default:
+		*status =
+		    (inl(instance->status_reg) & ME4600_AI_STATUS_BIT_FSM) ?
+		    ME_STATUS_BUSY : ME_STATUS_IDLE;
+		break;
+	}
+
+	if ((wait == ME_WAIT_IDLE) && (*status == ME_STATUS_BUSY)) {
+		// Only runing process will interrupt this call. Events are signaled when status change. Extra timeout add for safe reason.
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 ((instance->status !=
+						   ai_status_stream_run_wait)
+						  && (instance->status !=
+						      ai_status_stream_run)
+						  && (instance->status !=
+						      ai_status_stream_end_wait)),
+						 LONG_MAX);
+
+		if (instance->status != ai_status_stream_end) {
+			PDEBUG("Wait for IDLE canceled. %d\n",
+			       instance->status);
+			err = ME_ERRNO_CANCELLED;
+		}
+
+		if (signal_pending(current)) {
+			PERROR("Wait for IDLE interrupted.\n");
+			instance->status = ai_status_none;
+			ai_stop_isr(instance);
+			err = ME_ERRNO_SIGNAL;
+		}
+
+		*status = ME_STATUS_IDLE;
+	}
+
+	*values = me_circ_buf_values(&instance->circ_buf);
+	PDEBUG("me_circ_buf_values(&instance->circ_buf)=%d.\n", *values);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ai_io_stream_stop(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int stop_mode, int flags)
+{
+/**
+ @note Stop is implemented only in blocking mode.
+ @note Function return when state machine is stoped.
+*/
+	me4600_ai_subdevice_t *instance;
+	unsigned long cpu_flags;
+	uint32_t ctrl;
+	int ret;
+
+	PDEBUG("executed. idx=0\n");
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if ((stop_mode != ME_STOP_MODE_IMMEDIATE)
+	    && (stop_mode != ME_STOP_MODE_LAST_VALUE)) {
+		PERROR("Invalid stop mode specified.\n");
+		return ME_ERRNO_INVALID_STOP_MODE;
+	}
+
+	instance = (me4600_ai_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	// Mark as stopping. => Software stop.
+	instance->status = ai_status_stream_end_wait;
+
+	if (stop_mode == ME_STOP_MODE_IMMEDIATE) {
+		ret = ai_stop_immediately(instance);
+
+		if (ret) {
+			PERROR("FSM is still busy.\n");
+			ME_SUBDEVICE_EXIT;
+			return ME_ERRNO_SUBDEVICE_BUSY;
+		}
+		instance->ai_control_task_flag = 0;
+
+	} else if (stop_mode == ME_STOP_MODE_LAST_VALUE) {
+		// Set stop bit in registry.
+		spin_lock_irqsave(instance->ctrl_reg_lock, cpu_flags);
+		ctrl = inl(instance->ctrl_reg);
+		ctrl |= ME4600_AI_CTRL_BIT_STOP;
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+		spin_unlock_irqrestore(instance->ctrl_reg_lock, cpu_flags);
+
+		// Only runing process will interrupt this call. Events are signaled when status change.
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 (instance->status !=
+						  ai_status_stream_end_wait),
+						 LONG_MAX);
+
+		if (instance->status != ai_status_stream_end) {
+			PDEBUG("Stopping stream canceled.\n");
+			ret = ME_ERRNO_CANCELLED;
+		}
+
+		if (signal_pending(current)) {
+			PERROR("Stopping stream interrupted.\n");
+			instance->status = ai_status_none;
+			ret = ME_ERRNO_SIGNAL;
+		}
+		// End of work.
+		ai_stop_immediately(instance);
+
+	}
+
+	ret = ai_read_data_pooling(instance);
+	if (ret > 0) {		// Everything fine. More datas put to software buffer.
+		instance->status = ai_status_stream_end;
+		ret = ME_ERRNO_SUCCESS;
+		// Signal that we put last data to software buffer.
+		wake_up_interruptible_all(&instance->wait_queue);
+	} else if (ret == 0) {	// Everything fine. No more datas in FIFO.
+		instance->status = ai_status_stream_end;
+		ret = ME_ERRNO_SUCCESS;
+	} else if (ret == -ME_ERRNO_RING_BUFFER_OVERFLOW) {	// Stop is unsuccessful, buffer is overflow.
+		instance->status = ai_status_stream_buffer_error;
+		ret = ME_ERRNO_SUCCESS;
+	} else {		// Stop is unsuccessful
+		instance->status = ai_status_stream_end;
+		ret = -ret;
+	}
+
+	ME_SUBDEVICE_EXIT;
+
+	return ret;
+}
+
+static int me4600_ai_query_range_by_min_max(me_subdevice_t * subdevice,
+					    int unit,
+					    int *min,
+					    int *max, int *maxdata, int *range)
+{
+	me4600_ai_subdevice_t *instance;
+	int i;
+	int r = -1;
+	int diff = 21E6;
+
+	PDEBUG("executed. idx=0\n");
+
+	instance = (me4600_ai_subdevice_t *) subdevice;
+
+	if ((*max - *min) < 0) {
+		PERROR("Invalid minimum and maximum values specified.\n");
+		return ME_ERRNO_INVALID_MIN_MAX;
+	}
+
+	if ((unit == ME_UNIT_VOLT) || (unit == ME_UNIT_ANY)) {
+		for (i = 0; i < instance->ranges_len; i++) {
+			if ((instance->ranges[i].min <= *min)
+			    && ((instance->ranges[i].max + 1000) >= *max)) {
+				if ((instance->ranges[i].max -
+				     instance->ranges[i].min) - (*max - *min) <
+				    diff) {
+					r = i;
+					diff =
+					    (instance->ranges[i].max -
+					     instance->ranges[i].min) - (*max -
+									 *min);
+				}
+			}
+		}
+
+		if (r < 0) {
+			PERROR("No matching range found.\n");
+			return ME_ERRNO_NO_RANGE;
+		} else {
+			*min = instance->ranges[r].min;
+			*max = instance->ranges[r].max;
+			*maxdata = ME4600_AI_MAX_DATA;
+			*range = r;
+		}
+	} else {
+		PERROR("Invalid physical unit specified.\n");
+		return ME_ERRNO_INVALID_UNIT;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_ai_query_number_ranges(me_subdevice_t * subdevice,
+					 int unit, int *count)
+{
+	me4600_ai_subdevice_t *instance;
+
+	PDEBUG("executed. idx=0\n");
+
+	instance = (me4600_ai_subdevice_t *) subdevice;
+
+	if ((unit == ME_UNIT_VOLT) || (unit == ME_UNIT_ANY)) {
+		*count = instance->ranges_len;
+	} else {
+		*count = 0;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_ai_query_range_info(me_subdevice_t * subdevice,
+				      int range,
+				      int *unit,
+				      int *min, int *max, int *maxdata)
+{
+	me4600_ai_subdevice_t *instance;
+
+	PDEBUG("executed. idx=0\n");
+
+	instance = (me4600_ai_subdevice_t *) subdevice;
+
+	if ((range < instance->ranges_len) && (range >= 0)) {
+		*unit = ME_UNIT_VOLT;
+		*min = instance->ranges[range].min;
+		*max = instance->ranges[range].max;
+		*maxdata = ME4600_AI_MAX_DATA;
+	} else {
+		PERROR("Invalid range number specified.\n");
+		return ME_ERRNO_INVALID_RANGE;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_ai_query_timer(me_subdevice_t * subdevice,
+				 int timer,
+				 int *base_frequency,
+				 long long *min_ticks, long long *max_ticks)
+{
+	me4600_ai_subdevice_t *instance;
+
+	PDEBUG("executed. idx=0\n");
+
+	instance = (me4600_ai_subdevice_t *) subdevice;
+
+	switch (timer) {
+
+	case ME_TIMER_ACQ_START:
+		*base_frequency = ME4600_AI_BASE_FREQUENCY;
+		*min_ticks = ME4600_AI_MIN_ACQ_TICKS;
+		*max_ticks = ME4600_AI_MAX_ACQ_TICKS;
+		break;
+
+	case ME_TIMER_SCAN_START:
+		*base_frequency = ME4600_AI_BASE_FREQUENCY;
+		*min_ticks = ME4600_AI_MIN_SCAN_TICKS;
+		*max_ticks = ME4600_AI_MAX_SCAN_TICKS;
+		break;
+
+	case ME_TIMER_CONV_START:
+		*base_frequency = ME4600_AI_BASE_FREQUENCY;
+		*min_ticks = ME4600_AI_MIN_CHAN_TICKS;
+		*max_ticks = ME4600_AI_MAX_CHAN_TICKS;
+		break;
+
+	default:
+		PERROR("Invalid timer specified.(0x%04x)\n", timer);
+
+		return ME_ERRNO_INVALID_TIMER;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_ai_query_number_channels(me_subdevice_t * subdevice,
+					   int *number)
+{
+	me4600_ai_subdevice_t *instance;
+
+	PDEBUG("executed. idx=0\n");
+
+	instance = (me4600_ai_subdevice_t *) subdevice;
+	*number = instance->channels;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_ai_query_subdevice_type(me_subdevice_t * subdevice,
+					  int *type, int *subtype)
+{
+	PDEBUG("executed. idx=0\n");
+
+	*type = ME_TYPE_AI;
+	*subtype = ME_SUBTYPE_STREAMING;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_ai_query_subdevice_caps(me_subdevice_t * subdevice, int *caps)
+{
+	PDEBUG("executed. idx=0\n");
+
+	*caps =
+	    ME_CAPS_AI_TRIG_SYNCHRONOUS | ME_CAPS_AI_FIFO |
+	    ME_CAPS_AI_FIFO_THRESHOLD;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_ai_query_subdevice_caps_args(struct me_subdevice *subdevice,
+					       int cap, int *args, int count)
+{
+	me4600_ai_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	instance = (me4600_ai_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=0\n");
+
+	if (count != 1) {
+		PERROR("Invalid capability argument count.\n");
+		return ME_ERRNO_INVALID_CAP_ARG_COUNT;
+	}
+
+	switch (cap) {
+	case ME_CAP_AI_FIFO_SIZE:
+		args[0] = ME4600_AI_FIFO_COUNT;
+		break;
+
+	case ME_CAP_AI_BUFFER_SIZE:
+		args[0] =
+		    (instance->circ_buf.buf) ? ME4600_AI_CIRC_BUF_COUNT : 0;
+		break;
+
+	default:
+		PERROR("Invalid capability.\n");
+		err = ME_ERRNO_INVALID_CAP;
+		args[0] = 0;
+	}
+
+	return err;
+}
+
+void ai_limited_isr(me4600_ai_subdevice_t * instance, const uint32_t irq_status,
+		    const uint32_t ctrl_status)
+{
+	int to_read;
+
+	if (!instance->fifo_irq_threshold) {	//No threshold provided. SC ends work. HF need reseting.
+		if (irq_status & ME4600_IRQ_STATUS_BIT_SC) {
+			if (ai_read_data(instance, instance->ISM.next) != instance->ISM.next) {	//ERROR!
+				PERROR
+				    ("Limited amounts aqusition with TH=0: Circular buffer full!\n");
+				instance->status =
+				    ai_status_stream_buffer_error;
+			} else {
+				instance->status = ai_status_stream_end;
+			}
+			//End of work.
+			ai_stop_isr(instance);
+		} else if (irq_status & ME4600_IRQ_STATUS_BIT_AI_HF) {
+			instance->ISM.global_read += ME4600_AI_FIFO_HALF;
+
+			if (ai_read_data(instance, ME4600_AI_FIFO_HALF) != ME4600_AI_FIFO_HALF) {	//ERROR!
+				PERROR
+				    ("Limited amounts aqusition with TH = 0: Circular buffer full!\n");
+				//End of work.
+				ai_stop_isr(instance);
+				instance->status =
+				    ai_status_stream_buffer_error;
+			} else {
+				//Continue.
+				ai_limited_ISM(instance, irq_status);
+			}
+		}
+		//Signal user.
+		wake_up_interruptible_all(&instance->wait_queue);
+	} else			//if(instance->fifo_irq_threshold)
+	{
+		if (irq_status & ME4600_IRQ_STATUS_BIT_SC) {
+			instance->ISM.read = 0;
+			if ((instance->fifo_irq_threshold < ME4600_AI_FIFO_HALF)
+			    && (!(ctrl_status & ME4600_AI_STATUS_BIT_HF_DATA)))
+			{
+				to_read =
+				    ME4600_AI_FIFO_HALF -
+				    (ME4600_AI_FIFO_HALF %
+				     instance->fifo_irq_threshold);
+				PDEBUG
+				    ("Limited amounts aqusition with TH != 0: Not fast enough data aqusition! correction=%d\n",
+				     to_read);
+			} else {
+				to_read = instance->ISM.next;
+			}
+			instance->ISM.global_read += to_read;
+
+			ai_reschedule_SC(instance);
+
+			if (ai_read_data(instance, to_read) != to_read) {	//ERROR!
+				PERROR
+				    ("Limited amounts aqusition with TH != 0: Circular buffer full!\n");
+				//End of work.
+				ai_stop_isr(instance);
+				instance->status =
+				    ai_status_stream_buffer_error;
+			} else {
+				//Continue.
+				ai_limited_ISM(instance, irq_status);
+			}
+
+			//Signal user.
+			wake_up_interruptible_all(&instance->wait_queue);
+		} else if (irq_status & ME4600_IRQ_STATUS_BIT_AI_HF) {
+			instance->ISM.read += ME4600_AI_FIFO_HALF;
+			instance->ISM.global_read += ME4600_AI_FIFO_HALF;
+
+			if (ai_read_data(instance, ME4600_AI_FIFO_HALF) != ME4600_AI_FIFO_HALF) {	//ERROR!
+				PERROR
+				    ("Limited amounts aqusition with TH != 0: Circular buffer full!\n");
+				ai_stop_isr(instance);
+
+				instance->status =
+				    ai_status_stream_buffer_error;
+				//Signal user.
+				wake_up_interruptible_all(&instance->
+							  wait_queue);
+			} else {
+				//Countinue.
+				ai_limited_ISM(instance, irq_status);
+			}
+		}
+
+		if (instance->ISM.global_read >= instance->data_required) {	//End of work. Next paranoid pice of code: '>=' instead od '==' only to be sure.
+			ai_stop_isr(instance);
+			if (instance->status < ai_status_stream_end) {
+				instance->status = ai_status_stream_end;
+			}
+#ifdef MEDEBUG_ERROR
+			if (instance->ISM.global_read > instance->data_required) {	//This is security check case. This should never ever happend!
+				PERROR
+				    ("Limited amounts aqusition: Read more data than necessary! data_required=%d < read=%d\n",
+				     instance->data_required,
+				     instance->ISM.global_read);
+				//Signal error (warning??).
+				instance->status = ai_status_stream_error;
+			}
+#endif
+		}
+	}
+}
+
+void ai_infinite_isr(me4600_ai_subdevice_t * instance,
+		     const uint32_t irq_status, const uint32_t ctrl_status)
+{
+	int to_read;
+
+	if (irq_status & ME4600_IRQ_STATUS_BIT_SC) {	//next chunck of data -> read fifo
+		//Set new state in ISM.
+		if ((instance->fifo_irq_threshold < ME4600_AI_FIFO_HALF) && (!(ctrl_status & ME4600_AI_STATUS_BIT_HF_DATA))) {	//There is more data than we ecpected. Propably we aren't fast enough. Read as many as possible.
+			if (instance->fifo_irq_threshold) {
+				to_read =
+				    ME4600_AI_FIFO_HALF -
+				    (ME4600_AI_FIFO_HALF %
+				     instance->fifo_irq_threshold);
+				if (to_read > instance->fifo_irq_threshold) {
+					PDEBUG
+					    ("Infinite aqusition: Not fast enough data aqusition! TH != 0: correction=%d\n",
+					     to_read);
+				}
+			} else {	//No threshold specified.
+				to_read = ME4600_AI_FIFO_HALF;
+			}
+		} else {
+			to_read = instance->ISM.next;
+		}
+
+		instance->ISM.read += to_read;
+
+		//Get data
+		if (ai_read_data(instance, to_read) != to_read) {	//ERROR!
+			PERROR("Infinite aqusition: Circular buffer full!\n");
+			ai_stop_isr(instance);
+			instance->status = ai_status_stream_buffer_error;
+		} else {
+			ai_infinite_ISM(instance);
+			instance->ISM.global_read += instance->ISM.read;
+			instance->ISM.read = 0;
+		}
+
+		//Signal data to user
+		wake_up_interruptible_all(&instance->wait_queue);
+	} else if (irq_status & ME4600_IRQ_STATUS_BIT_AI_HF) {	//fifo is half full -> read fifo       Large blocks only!
+		instance->ISM.read += ME4600_AI_FIFO_HALF;
+
+		if (ai_read_data(instance, ME4600_AI_FIFO_HALF) != ME4600_AI_FIFO_HALF) {	//ERROR!
+			PERROR("Infinite aqusition: Circular buffer full!\n");
+			ai_stop_isr(instance);
+			instance->status = ai_status_stream_buffer_error;
+
+			//Signal it.
+			wake_up_interruptible_all(&instance->wait_queue);
+		} else {
+			ai_infinite_ISM(instance);
+		}
+	}
+}
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 19)
+static irqreturn_t me4600_ai_isr(int irq, void *dev_id)
+#else
+static irqreturn_t me4600_ai_isr(int irq, void *dev_id, struct pt_regs *regs)
+#endif
+{				/// @note This is time critical function!
+	uint32_t irq_status;
+	uint32_t ctrl_status;
+	me4600_ai_subdevice_t *instance = dev_id;
+	//int to_read;
+
+	PDEBUG("executed. idx=0\n");
+
+	if (irq != instance->irq) {
+		PERROR("Incorrect interrupt num: %d.\n", irq);
+		return IRQ_NONE;
+	}
+
+	irq_status = inl(instance->irq_status_reg);
+	if (!
+	    (irq_status &
+	     (ME4600_IRQ_STATUS_BIT_AI_HF | ME4600_IRQ_STATUS_BIT_SC))) {
+#ifdef MEDEBUG_INFO
+		if ((irq_status & (ME4600_IRQ_STATUS_BIT_AI_HF | ME4600_IRQ_STATUS_BIT_SC | ME4600_IRQ_STATUS_BIT_LE)) == ME4600_IRQ_STATUS_BIT_LE) {	//This is security check case. LE is unused. This should never ever happend.
+			PINFO
+			    ("%ld Shared interrupt. %s(): irq_status_reg=LE_IRQ\n",
+			     jiffies, __FUNCTION__);
+		} else {
+			PINFO
+			    ("%ld Shared interrupt. %s(): irq_status_reg=0x%04X\n",
+			     jiffies, __FUNCTION__, irq_status);
+		}
+#endif
+		return IRQ_NONE;
+	}
+
+	if (!instance->circ_buf.buf) {	//Security check.
+		PERROR_CRITICAL("CIRCULAR BUFFER NOT EXISTS!\n");
+		ai_stop_isr(instance);
+		return IRQ_HANDLED;
+	}
+	//Get the status register.
+	ctrl_status = inl(instance->status_reg);
+
+#ifdef MEDEBUG_INFO
+	if (irq_status & ME4600_IRQ_STATUS_BIT_AI_HF)
+		PINFO("HF interrupt active\n");
+	if (irq_status & ME4600_IRQ_STATUS_BIT_SC)
+		PINFO("SC interrupt active\n");
+	if (irq_status & ME4600_IRQ_STATUS_BIT_LE)
+		PINFO("LE interrupt active\n");
+#endif
+
+	//This is safety check!
+	if ((irq_status & ME4600_IRQ_STATUS_BIT_AI_HF)
+	    && (ctrl_status & ME4600_AI_STATUS_BIT_HF_DATA)) {
+		PDEBUG("HF interrupt active but FIFO under half\n");
+		//Reset HF interrupt latch.
+		spin_lock(instance->ctrl_reg_lock);
+		outl(ctrl_status | ME4600_AI_CTRL_BIT_HF_IRQ_RESET,
+		     instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base,
+			   ctrl_status);
+		outl(ctrl_status, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base,
+			   ctrl_status);
+		spin_unlock(instance->ctrl_reg_lock);
+		return IRQ_HANDLED;
+	}
+#ifdef MEDEBUG_INFO
+	PINFO("STATUS_BIT_FSM=%s.\n",
+	      (ctrl_status & ME4600_AI_STATUS_BIT_FSM) ? "on" : "off");
+
+	PINFO("STATUS_BIT_EF_CHANNEL=%s.\n",
+	      (ctrl_status & ME4600_AI_STATUS_BIT_EF_CHANNEL) ? "not empty" :
+	      "empty");
+	PINFO("STATUS_BIT_HF_CHANNEL=%s.\n",
+	      (ctrl_status & ME4600_AI_STATUS_BIT_HF_CHANNEL) ? " < HF" :
+	      " > HF");
+	PINFO("STATUS_BIT_FF_CHANNEL=%s.\n",
+	      (ctrl_status & ME4600_AI_STATUS_BIT_FF_CHANNEL) ? "not full" :
+	      "full");
+
+	PINFO("STATUS_BIT_EF_DATA=%s.\n",
+	      (ctrl_status & ME4600_AI_STATUS_BIT_EF_DATA) ? "not empty" :
+	      "empty");
+	PINFO("STATUS_BIT_HF_DATA=%s.\n",
+	      (ctrl_status & ME4600_AI_STATUS_BIT_HF_DATA) ? " < HF" : " > HF");
+	PINFO("STATUS_BIT_FF_DATA=%s.\n",
+	      (ctrl_status & ME4600_AI_STATUS_BIT_FF_DATA) ? "not full" :
+	      "full");
+
+	PINFO("CTRL_BIT_HF_IRQ=%s.\n",
+	      (ctrl_status & ME4600_AI_CTRL_BIT_HF_IRQ) ? "enable" : "disable");
+	PINFO("CTRL_BIT_HF_IRQ_RESET=%s.\n",
+	      (ctrl_status & ME4600_AI_CTRL_BIT_HF_IRQ_RESET) ? "reset" :
+	      "work");
+	PINFO("CTRL_BIT_SC_IRQ=%s.\n",
+	      (ctrl_status & ME4600_AI_CTRL_BIT_SC_IRQ) ? "enable" : "disable");
+	PINFO("CTRL_BIT_SC_RELOAD=%s.\n",
+	      (ctrl_status & ME4600_AI_CTRL_BIT_SC_RELOAD) ? "on" : "off");
+	PINFO("CTRL_BIT_SC_IRQ_RESET=%s.\n",
+	      (ctrl_status & ME4600_AI_CTRL_BIT_SC_IRQ_RESET) ? "reset" :
+	      "work");
+#endif
+
+	//Look for overflow error.
+	if (!(ctrl_status & ME4600_AI_STATUS_BIT_FF_DATA)) {
+		//FIFO is full. Read datas and reset all settings.
+		PERROR("FIFO overflow.\n");
+		ai_read_data(instance, ME4600_AI_FIFO_COUNT);
+		ai_stop_isr(instance);
+
+		instance->status = ai_status_stream_fifo_error;
+		//Signal it.
+		wake_up_interruptible_all(&instance->wait_queue);
+
+		return IRQ_HANDLED;
+	}
+
+	if (!instance->data_required) {	//This is infinite aqusition.
+#ifdef MEDEBUG_ERROR
+		if ((irq_status &
+		     (ME4600_IRQ_STATUS_BIT_AI_HF | ME4600_IRQ_STATUS_BIT_SC))
+		    ==
+		    (ME4600_IRQ_STATUS_BIT_AI_HF | ME4600_IRQ_STATUS_BIT_SC)) {
+			///In infinite mode only one interrupt source should be reported!
+			PERROR
+			    ("Error in ISM! Infinite aqusition: HF and SC interrupts active! threshold=%d next=%d ctrl=0x%04X irq_status_reg=0x%04X",
+			     instance->fifo_irq_threshold, instance->ISM.next,
+			     ctrl_status, irq_status);
+		}
+#endif
+
+		ai_infinite_isr(instance, irq_status, ctrl_status);
+
+#ifdef MEDEBUG_INFO
+		ctrl_status = inl(instance->ctrl_reg);
+#endif
+	} else {
+
+		ai_limited_isr(instance, irq_status, ctrl_status);
+		ctrl_status = inl(instance->status_reg);
+		if (!(ctrl_status & (ME4600_AI_STATUS_BIT_HF_DATA | ME4600_AI_CTRL_BIT_HF_IRQ_RESET))) {	//HF active, but we have more than half already => HF will never come
+			PDEBUG
+			    ("MISSED HF. data_required=%d ISM.read=%d ISM.global=%d ISM.next=%d\n",
+			     instance->data_required, instance->ISM.read,
+			     instance->ISM.global_read, instance->ISM.next);
+			ai_limited_isr(instance, ME4600_IRQ_STATUS_BIT_AI_HF,
+				       ctrl_status);
+		}
+	}
+
+#ifdef MEDEBUG_INFO
+	PINFO("STATUS_BIT_FSM=%s.\n",
+	      (ctrl_status & ME4600_AI_STATUS_BIT_FSM) ? "on" : "off");
+
+	PINFO("STATUS_BIT_EF_CHANNEL=%s.\n",
+	      (ctrl_status & ME4600_AI_STATUS_BIT_EF_CHANNEL) ? "not empty" :
+	      "empty");
+	PINFO("STATUS_BIT_HF_CHANNEL=%s.\n",
+	      (ctrl_status & ME4600_AI_STATUS_BIT_HF_CHANNEL) ? " < HF" :
+	      " > HF");
+	PINFO("STATUS_BIT_FF_CHANNEL=%s.\n",
+	      (ctrl_status & ME4600_AI_STATUS_BIT_FF_CHANNEL) ? "not full" :
+	      "full");
+
+	PINFO("STATUS_BIT_EF_DATA=%s.\n",
+	      (ctrl_status & ME4600_AI_STATUS_BIT_EF_DATA) ? "not empty" :
+	      "empty");
+	PINFO("STATUS_BIT_HF_DATA=%s.\n",
+	      (ctrl_status & ME4600_AI_STATUS_BIT_HF_DATA) ? " < HF" : " > HF");
+	PINFO("STATUS_BIT_FF_DATA=%s.\n",
+	      (ctrl_status & ME4600_AI_STATUS_BIT_FF_DATA) ? "not full" :
+	      "full");
+
+	PINFO("CTRL_BIT_HF_IRQ_RESET=%s.\n",
+	      (ctrl_status & ME4600_AI_CTRL_BIT_HF_IRQ_RESET) ? "reset" :
+	      "work");
+	PINFO("CTRL_BIT_SC_IRQ=%s.\n",
+	      (ctrl_status & ME4600_AI_CTRL_BIT_SC_IRQ) ? "enable" : "disable");
+	PINFO("CTRL_BIT_SC_RELOAD=%s.\n",
+	      (ctrl_status & ME4600_AI_CTRL_BIT_SC_RELOAD) ? "on" : "off");
+	PINFO("CTRL_BIT_SC_IRQ_RESET=%s.\n",
+	      (ctrl_status & ME4600_AI_CTRL_BIT_SC_IRQ_RESET) ? "reset" :
+	      "work");
+	PINFO("%ld END\n", jiffies);
+#endif
+
+	return IRQ_HANDLED;
+}
+
+/** @brief Stop aqusation of data. Reset interrupts' laches. Clear data's FIFO.
+*
+* @param instance The subdevice instance (pointer).
+*/
+void inline ai_stop_isr(me4600_ai_subdevice_t * instance)
+{				/// @note This is soft time critical function!
+	register uint32_t tmp;
+
+	spin_lock(instance->ctrl_reg_lock);
+	//Stop all. Reset interrupt laches. Reset data FIFO.
+	tmp = inl(instance->ctrl_reg);
+	tmp |=
+	    (ME4600_AI_CTRL_BIT_IMMEDIATE_STOP | ME4600_AI_CTRL_BIT_HF_IRQ_RESET
+	     | ME4600_AI_CTRL_BIT_LE_IRQ_RESET |
+	     ME4600_AI_CTRL_BIT_SC_IRQ_RESET);
+	tmp &= ~ME4600_AI_CTRL_BIT_DATA_FIFO;
+	outl(tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp);
+	spin_unlock(instance->ctrl_reg_lock);
+}
+
+/** @brief Copy data from fifo to circular buffer.
+*
+* @param instance The subdevice instance (pointer).
+* @param count The number of requested data.
+*
+* @return On success: Number of copied values.
+* @return On error: -ME_ERRNO_RING_BUFFER_OVERFLOW.
+*/
+static int inline ai_read_data(me4600_ai_subdevice_t * instance,
+			       const int count)
+{				/// @note This is time critical function!
+	int c = count;
+	int empty_space;
+	int copied = 0;
+	int i, j;
+
+	empty_space = me_circ_buf_space_to_end(&instance->circ_buf);
+	if (empty_space <= 0) {
+		PDEBUG("Circular buffer full.\n");
+		return -ME_ERRNO_RING_BUFFER_OVERFLOW;
+	}
+
+	if (empty_space < c) {	//Copy first part. Max to end of buffer.
+		PDEBUG
+		    ("Try to copy %d values from FIFO to circular buffer (pass 1).\n",
+		     empty_space);
+		for (i = 0; i < empty_space; i++) {
+			*(instance->circ_buf.buf + instance->circ_buf.head) =
+			    (inw(instance->data_reg) ^ 0x8000);
+			instance->circ_buf.head++;
+		}
+		instance->circ_buf.head &= instance->circ_buf.mask;
+		c -= empty_space;
+		copied = empty_space;
+
+		empty_space = me_circ_buf_space_to_end(&instance->circ_buf);
+	}
+
+	if (empty_space > 0) {
+		j = (empty_space < c) ? empty_space : c;
+		PDEBUG
+		    ("Try to copy %d values from FIFO to circular buffer (pass 2).\n",
+		     c);
+		for (i = 0; i < j; i++) {
+			*(instance->circ_buf.buf + instance->circ_buf.head) =
+			    (inw(instance->data_reg) ^ 0x8000);
+			instance->circ_buf.head++;
+		}
+		instance->circ_buf.head &= instance->circ_buf.mask;
+		copied += j;
+	}
+	return copied;
+}
+
+void inline ai_infinite_ISM(me4600_ai_subdevice_t * instance)
+{				/// @note This is time critical function!
+	register volatile uint32_t ctrl_set, ctrl_reset, tmp;
+
+	if (instance->fifo_irq_threshold < ME4600_AI_FIFO_MAX_SC) {	// Only sample counter with reloadnig is working. Reset it.
+		PINFO
+		    ("Only sample counter with reloadnig is working. Reset it.\n");
+		ctrl_set = ME4600_AI_CTRL_BIT_SC_IRQ_RESET;
+		ctrl_reset = ~ME4600_AI_CTRL_BIT_SC_IRQ_RESET;
+	} else if (instance->fifo_irq_threshold == instance->ISM.read) {	//This is SC interrupt for large block. The whole section is done. Reset SC_IRQ an HF_IRQ and start everything again from beginning.
+		PINFO
+		    ("This is SC interrupt for large block. The whole section is done. Reset SC_IRQ an HF_IRQ and start everything again from beginning.\n");
+		ctrl_set =
+		    ME4600_AI_CTRL_BIT_SC_IRQ_RESET |
+		    ME4600_AI_CTRL_BIT_HF_IRQ_RESET;
+		ctrl_reset =
+		    ~(ME4600_AI_CTRL_BIT_SC_IRQ_RESET |
+		      ME4600_AI_CTRL_BIT_HF_IRQ_RESET);
+	} else if (instance->fifo_irq_threshold >= (ME4600_AI_FIFO_MAX_SC + instance->ISM.read)) {	//This is HF interrupt for large block.The next interrupt should be from HF, also. Reset HF.
+		PINFO
+		    ("This is HF interrupt for large block.The next interrupt should be from HF, also. Reset HF.\n");
+		ctrl_set = ME4600_AI_CTRL_BIT_HF_IRQ_RESET;
+		ctrl_reset = ~ME4600_AI_CTRL_BIT_HF_IRQ_RESET;
+	} else {		//This is HF interrupt for large block.The next interrupt should be from SC. Don't reset HF!
+		PINFO
+		    ("This is HF interrupt for large block.The next interrupt should be from SC. Don't reset HF!\n");
+		ctrl_set = ME4600_AI_CTRL_BIT_HF_IRQ_RESET;
+		ctrl_reset = 0xFFFFFFFF;
+	}
+
+	//Reset interrupt latch.
+	spin_lock(instance->ctrl_reg_lock);
+	tmp = inl(instance->ctrl_reg);
+	PINFO("ctrl=0x%x ctrl_set=0x%x ctrl_reset=0x%x\n", tmp, ctrl_set,
+	      ctrl_reset);
+	tmp |= ctrl_set;
+	outl(tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp);
+	if (ctrl_reset != 0xFFFFFFFF) {
+		outl(tmp & ctrl_reset, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reset outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base,
+			   tmp & ctrl_reset);
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+
+}
+
+void inline ai_limited_ISM(me4600_ai_subdevice_t * instance,
+			   uint32_t irq_status)
+{				/// @note This is time critical function!
+	register volatile uint32_t ctrl_set, ctrl_reset = 0xFFFFFFFF, tmp;
+
+	if (!instance->fifo_irq_threshold) {	//No threshold provided. SC ends work.
+		PINFO("No threshold provided. SC ends work.\n");
+		ctrl_set = ME4600_AI_CTRL_BIT_HF_IRQ_RESET;
+		if (instance->data_required > (ME4600_AI_FIFO_COUNT - 1 + instance->ISM.global_read)) {	//HF need reseting.
+			ctrl_reset &= ~ME4600_AI_CTRL_BIT_HF_IRQ_RESET;
+		}
+	} else			//if(instance->fifo_irq_threshold)
+	{
+		if (irq_status & ME4600_IRQ_STATUS_BIT_AI_HF) {
+			PINFO("Threshold provided. Clear HF latch.\n");
+			ctrl_set = ME4600_AI_CTRL_BIT_HF_IRQ_RESET;
+
+			if (instance->fifo_irq_threshold >= (ME4600_AI_FIFO_MAX_SC + instance->ISM.read)) {	//This is not the last one. HF need reseting.
+				PINFO
+				    ("The next interrupt is HF. HF need be activating.\n");
+				ctrl_reset = ~ME4600_AI_CTRL_BIT_HF_IRQ_RESET;
+			}
+		}
+
+		if (irq_status & ME4600_IRQ_STATUS_BIT_SC) {
+			PINFO("Threshold provided. Restart SC.\n");
+			ctrl_set = ME4600_AI_CTRL_BIT_SC_IRQ_RESET;
+			ctrl_reset &= ~ME4600_AI_CTRL_BIT_SC_IRQ_RESET;
+
+			if (instance->fifo_irq_threshold >= ME4600_AI_FIFO_MAX_SC) {	//This is not the last one. HF need to be activating.
+				PINFO
+				    ("The next interrupt is HF. HF need to be activating.\n");
+				ctrl_reset &= ~ME4600_AI_CTRL_BIT_HF_IRQ_RESET;
+			}
+		}
+	}
+
+	//Reset interrupt latch.
+	spin_lock(instance->ctrl_reg_lock);
+	tmp = inl(instance->ctrl_reg);
+	tmp |= ctrl_set;
+	outl(tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp);
+
+	if (ctrl_reset != 0xFFFFFFFF) {
+		outl(tmp & ctrl_reset, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base,
+			   tmp & ctrl_reset);
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+
+}
+
+/** @brief Last chunck of datas. We must reschedule sample counter.
+*	@note Last chunck.
+*	Leaving SC_RELOAD doesn't do any harm, but in some bad case can make extra interrupts.
+*	@warning When threshold is wrongly set some IRQ are lost.(!!!)
+*/
+void inline ai_reschedule_SC(me4600_ai_subdevice_t * instance)
+{
+	register uint32_t rest;
+
+	if (instance->data_required <= instance->ISM.global_read)
+		return;
+
+	rest = instance->data_required - instance->ISM.global_read;
+	if (rest < instance->fifo_irq_threshold) {	//End of work soon ....
+		PDEBUG("Rescheduling SC from %d to %d.\n",
+		       instance->fifo_irq_threshold, rest);
+		/// @note Write new value to SC <==  DANGER! This is not safe solution! We can miss some inputs.
+		outl(rest, instance->sample_counter_reg);
+		PDEBUG_REG("sample_counter_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->sample_counter_reg - instance->reg_base,
+			   rest);
+		instance->fifo_irq_threshold = rest;
+
+		if (rest < ME4600_AI_FIFO_MAX_SC) {
+			instance->ISM.next = rest;
+		} else {
+			instance->ISM.next = rest % ME4600_AI_FIFO_HALF;
+			if (instance->ISM.next + ME4600_AI_FIFO_HALF <
+			    ME4600_AI_FIFO_MAX_SC) {
+				instance->ISM.next += ME4600_AI_FIFO_HALF;
+			}
+		}
+	}
+}
+
+/** Start the ISM. All must be reseted before enter to this function. */
+void inline ai_data_acquisition_logic(me4600_ai_subdevice_t * instance)
+{
+	register uint32_t tmp;
+
+	if (!instance->data_required) {	//This is infinite aqusition.
+		if (!instance->fifo_irq_threshold) {	//No threshold provided. Set SC to 0.5*FIFO. Clear the SC's latch.
+			//Set the sample counter
+			outl(ME4600_AI_FIFO_HALF, instance->sample_counter_reg);
+			PDEBUG_REG
+			    ("sample_counter_reg outl(0x%lX+0x%lX)=0x%x\n",
+			     instance->reg_base,
+			     instance->sample_counter_reg - instance->reg_base,
+			     ME4600_AI_FIFO_HALF);
+		} else {	//Threshold provided. Set SC to treshold. Clear the SC's latch.
+			//Set the sample counter
+			outl(instance->fifo_irq_threshold,
+			     instance->sample_counter_reg);
+			PDEBUG_REG
+			    ("sample_counter_reg outl(0x%lX+0x%lX)=0x%x\n",
+			     instance->reg_base,
+			     instance->sample_counter_reg - instance->reg_base,
+			     instance->fifo_irq_threshold);
+		}
+
+		if (instance->fifo_irq_threshold < ME4600_AI_FIFO_MAX_SC) {	//Enable only sample counter's interrupt. Set reload bit. Clear the SC's latch.
+			spin_lock(instance->ctrl_reg_lock);
+			tmp = inl(instance->ctrl_reg);
+			tmp |= ME4600_AI_CTRL_BIT_SC_RELOAD;
+			tmp &= ~ME4600_AI_CTRL_BIT_SC_IRQ_RESET;
+			outl(tmp, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   tmp);
+			spin_unlock(instance->ctrl_reg_lock);
+			if (!instance->fifo_irq_threshold) {	//No threshold provided. Set ISM.next to 0.5*FIFO.
+				instance->ISM.next = ME4600_AI_FIFO_HALF;
+			} else {	//Threshold provided. Set ISM.next to treshold.
+				instance->ISM.next =
+				    instance->fifo_irq_threshold;
+			}
+		} else {	//Enable sample counter's and HF's interrupts.
+			spin_lock(instance->ctrl_reg_lock);
+			tmp = inl(instance->ctrl_reg);
+			tmp |= ME4600_AI_CTRL_BIT_SC_RELOAD;
+			tmp &=
+			    ~(ME4600_AI_CTRL_BIT_SC_IRQ_RESET |
+			      ME4600_AI_CTRL_BIT_HF_IRQ_RESET);
+			outl(tmp, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   tmp);
+			spin_unlock(instance->ctrl_reg_lock);
+
+			instance->ISM.next =
+			    instance->fifo_irq_threshold % ME4600_AI_FIFO_HALF;
+			if (instance->ISM.next + ME4600_AI_FIFO_HALF <
+			    ME4600_AI_FIFO_MAX_SC) {
+				instance->ISM.next += ME4600_AI_FIFO_HALF;
+			}
+		}
+	} else {		//This aqusition is limited to set number of data.
+		if (instance->fifo_irq_threshold >= instance->data_required) {	//Stupid situation.
+			instance->fifo_irq_threshold = 0;
+			PDEBUG
+			    ("Stupid situation: data_required(%d) < threshold(%d).\n",
+			     instance->fifo_irq_threshold,
+			     instance->data_required);
+		}
+
+		if (!instance->fifo_irq_threshold) {	//No threshold provided. Easy case: HF=read and SC=end.
+			//Set the sample counter to data_required.
+			outl(instance->data_required,
+			     instance->sample_counter_reg);
+			PDEBUG_REG
+			    ("sample_counter_reg outl(0x%lX+0x%lX)=0x%x\n",
+			     instance->reg_base,
+			     instance->sample_counter_reg - instance->reg_base,
+			     instance->data_required);
+
+			//Reset the latches of sample counter and HF (if SC>FIFO).
+			//No SC reload!
+			spin_lock(instance->ctrl_reg_lock);
+			tmp = inl(instance->ctrl_reg);
+			tmp &=
+			    ~(ME4600_AI_CTRL_BIT_SC_IRQ_RESET |
+			      ME4600_AI_CTRL_BIT_SC_RELOAD);
+			if (instance->data_required >
+			    (ME4600_AI_FIFO_COUNT - 1)) {
+				tmp &= ~ME4600_AI_CTRL_BIT_HF_IRQ_RESET;
+				instance->ISM.next =
+				    instance->data_required %
+				    ME4600_AI_FIFO_HALF;
+				instance->ISM.next += ME4600_AI_FIFO_HALF;
+
+			} else {
+				instance->ISM.next = instance->data_required;
+			}
+			outl(tmp, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   tmp);
+			spin_unlock(instance->ctrl_reg_lock);
+
+		} else {	//The most general case. We have concret numbe of required data and threshold. SC=TH
+			//Set the sample counter to threshold.
+			outl(instance->fifo_irq_threshold,
+			     instance->sample_counter_reg);
+			PDEBUG_REG
+			    ("sample_counter_reg outl(0x%lX+0x%lX)=0x%x\n",
+			     instance->reg_base,
+			     instance->sample_counter_reg - instance->reg_base,
+			     instance->fifo_irq_threshold);
+
+			spin_lock(instance->ctrl_reg_lock);
+			tmp = inl(instance->ctrl_reg);
+			//In this moment we are sure that SC will come more than once.
+			tmp |= ME4600_AI_CTRL_BIT_SC_RELOAD;
+
+			if (instance->fifo_irq_threshold < ME4600_AI_FIFO_MAX_SC) {	//The threshold is so small that we do need HF.
+				tmp &= ~ME4600_AI_CTRL_BIT_SC_IRQ_RESET;
+				instance->ISM.next =
+				    instance->fifo_irq_threshold;
+			} else {	//The threshold is large. The HF must be use.
+				tmp &=
+				    ~(ME4600_AI_CTRL_BIT_SC_IRQ_RESET |
+				      ME4600_AI_CTRL_BIT_HF_IRQ_RESET);
+				instance->ISM.next =
+				    instance->fifo_irq_threshold %
+				    ME4600_AI_FIFO_HALF;
+				if (instance->ISM.next + ME4600_AI_FIFO_HALF <
+				    ME4600_AI_FIFO_MAX_SC) {
+					instance->ISM.next +=
+					    ME4600_AI_FIFO_HALF;
+				}
+			}
+			outl(tmp, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   tmp);
+			spin_unlock(instance->ctrl_reg_lock);
+		}
+	}
+}
+
+static int ai_mux_toggler(me4600_ai_subdevice_t * instance)
+{
+	uint32_t tmp;
+
+	PDEBUG("executed. idx=0\n");
+
+	outl(0, instance->scan_pre_timer_low_reg);
+	PDEBUG_REG("scan_pre_timer_low_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->scan_pre_timer_low_reg - instance->reg_base, 0);
+	outl(0, instance->scan_pre_timer_high_reg);
+	PDEBUG_REG("scan_pre_timer_high_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->scan_pre_timer_high_reg - instance->reg_base, 0);
+	outl(0, instance->scan_timer_low_reg);
+	PDEBUG_REG("scan_timer_low_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->scan_timer_low_reg - instance->reg_base, 0);
+	outl(0, instance->scan_timer_high_reg);
+	PDEBUG_REG("scan_timer_high_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->scan_timer_high_reg - instance->reg_base, 0);
+	outl(65, instance->chan_timer_reg);
+	PDEBUG_REG("chan_timer_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->chan_timer_reg - instance->reg_base, 65);
+	outl(65, instance->chan_pre_timer_reg);
+	PDEBUG_REG("chan_pre_timer_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->chan_pre_timer_reg - instance->reg_base, 65);
+
+	// Turn on internal reference.
+	tmp = inl(instance->ctrl_reg);
+	tmp |= ME4600_AI_CTRL_BIT_FULLSCALE;
+	outl(tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp);
+
+	// Clear data and channel fifo.
+	tmp &=
+	    ~(ME4600_AI_CTRL_BIT_CHANNEL_FIFO | ME4600_AI_CTRL_BIT_DATA_FIFO);
+	outl(tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp);
+	tmp |= ME4600_AI_CTRL_BIT_CHANNEL_FIFO | ME4600_AI_CTRL_BIT_DATA_FIFO;
+	outl(tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp);
+
+	// Write channel entry.
+	outl(ME4600_AI_LIST_INPUT_DIFFERENTIAL |
+	     ME4600_AI_LIST_RANGE_UNIPOLAR_2_5 | 31,
+	     instance->channel_list_reg);
+	PDEBUG_REG("channel_list_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->channel_list_reg - instance->reg_base,
+		   ME4600_AI_LIST_INPUT_DIFFERENTIAL |
+		   ME4600_AI_LIST_RANGE_UNIPOLAR_2_5 | 31);
+
+	// Start conversion.
+	inl(instance->start_reg);
+	PDEBUG_REG("start_reg inl(0x%lX+0x%lX)\n", instance->reg_base,
+		   instance->start_reg - instance->reg_base);
+	udelay(10);
+
+	// Clear data and channel fifo.
+	tmp &=
+	    ~(ME4600_AI_CTRL_BIT_CHANNEL_FIFO | ME4600_AI_CTRL_BIT_DATA_FIFO);
+	outl(tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp);
+	tmp |= ME4600_AI_CTRL_BIT_CHANNEL_FIFO | ME4600_AI_CTRL_BIT_DATA_FIFO;
+	outl(tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp);
+
+	// Write channel entry.
+	// ME4600_AI_LIST_INPUT_SINGLE_ENDED | ME4600_AI_LIST_RANGE_BIPOLAR_10 <= 0x0000
+	outl(ME4600_AI_LIST_INPUT_SINGLE_ENDED |
+	     ME4600_AI_LIST_RANGE_BIPOLAR_10, instance->channel_list_reg);
+	PDEBUG_REG("channel_list_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->channel_list_reg - instance->reg_base,
+		   ME4600_AI_LIST_INPUT_SINGLE_ENDED |
+		   ME4600_AI_LIST_RANGE_BIPOLAR_10);
+
+	// Start conversion.
+	inl(instance->start_reg);
+	PDEBUG_REG("start_reg inl(0x%lX+0x%lX)\n", instance->reg_base,
+		   instance->start_reg - instance->reg_base);
+	udelay(10);
+
+	// Clear control register.
+	tmp &= (ME4600_AI_CTRL_BIT_EX_IRQ | ME4600_AI_CTRL_BIT_EX_IRQ_RESET);
+	outl(tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp);
+
+	return ME_ERRNO_SUCCESS;
+}
+
+/** @brief Copy rest of data from fifo to circular buffer.
+* @note Helper for STOP command. After FSM is stopped.
+* @note This is slow function that copy all remainig data from FIFO to buffer.
+*
+* @param instance The subdevice instance (pointer).
+*
+* @return On success: Number of copied values.
+* @return On error: Negative error code -ME_ERRNO_RING_BUFFER_OVERFLOW.
+*/
+static int inline ai_read_data_pooling(me4600_ai_subdevice_t * instance)
+{				/// @note This is time critical function!
+	int empty_space;
+	int copied = 0;
+	int status = ME_ERRNO_SUCCESS;
+
+	PDEBUG("Space left in circular buffer = %d.\n",
+	       me_circ_buf_space(&instance->circ_buf));
+
+	while ((empty_space = me_circ_buf_space(&instance->circ_buf))) {
+		if (!(status = inl(instance->status_reg) & ME4600_AI_STATUS_BIT_EF_DATA)) {	//No more data. status = ME_ERRNO_SUCCESS = 0
+			break;
+		}
+		*(instance->circ_buf.buf + instance->circ_buf.head) =
+		    (inw(instance->data_reg) ^ 0x8000);
+		instance->circ_buf.head++;
+		instance->circ_buf.head &= instance->circ_buf.mask;
+	}
+
+#ifdef MEDEBUG_ERROR
+	if (!status)
+		PDEBUG
+		    ("Copied all remaining datas (%d) from FIFO to circular buffer.\n",
+		     copied);
+	else {
+		PDEBUG("No more empty space in buffer.\n");
+		PDEBUG("Copied %d datas from FIFO to circular buffer.\n",
+		       copied);
+		PDEBUG("FIFO still not empty.\n");
+	}
+#endif
+	return (!status) ? copied : -ME_ERRNO_RING_BUFFER_OVERFLOW;
+}
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+static void me4600_ai_work_control_task(void *subdevice)
+#else
+static void me4600_ai_work_control_task(struct work_struct *work)
+#endif
+{
+	me4600_ai_subdevice_t *instance;
+	uint32_t status;
+	uint32_t ctrl;
+	unsigned long cpu_flags = 0;
+	int reschedule = 0;
+	int signaling = 0;
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+	instance = (me4600_ai_subdevice_t *) subdevice;
+#else
+	instance =
+	    container_of((void *)work, me4600_ai_subdevice_t, ai_control_task);
+#endif
+	PINFO("<%s: %ld> executed.\n", __FUNCTION__, jiffies);
+
+	status = inl(instance->status_reg);
+	PDEBUG_REG("status_reg inl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->status_reg - instance->reg_base, status);
+
+	switch (instance->status) {	// Checking actual mode.
+		// Not configured for work.
+	case ai_status_none:
+		break;
+
+		//This are stable modes. No need to do anything. (?)
+	case ai_status_single_configured:
+	case ai_status_stream_configured:
+	case ai_status_stream_fifo_error:
+	case ai_status_stream_buffer_error:
+	case ai_status_stream_error:
+		PERROR("Shouldn't be running!.\n");
+		break;
+
+		// Stream modes
+	case ai_status_stream_run_wait:
+		if (status & ME4600_AI_STATUS_BIT_FSM) {	// ISM started..
+			instance->status = ai_status_stream_run;
+			// Signal the end of wait for start.
+			signaling = 1;
+			// Wait now for stop.
+			reschedule = 1;
+			break;
+
+			// Check timeout.
+			if ((instance->timeout.delay) && ((jiffies - instance->timeout.start_time) >= instance->timeout.delay)) {	// Timeout
+				PDEBUG("Timeout reached.\n");
+				// Stop all actions. No conditions! Block interrupts. Reset FIFO => Too late!
+				ai_stop_isr(instance);
+
+				instance->status = ai_status_stream_end;
+
+				// Signal the end.
+				signaling = 1;
+			}
+		}
+		break;
+
+	case ai_status_stream_run:
+		// Wait for stop ISM.
+		reschedule = 1;
+		break;
+
+	case ai_status_stream_end_wait:
+		if (!(status & ME4600_AI_STATUS_BIT_FSM)) {	// ISM stoped. Overwrite ISR.
+			instance->status = ai_status_stream_end;
+			// Signal the end of wait for stop.
+			signaling = 1;
+		} else {
+			// Wait for stop ISM.
+			reschedule = 1;
+		}
+		break;
+
+	case ai_status_stream_end:
+		//End work.
+		if (status & ME4600_AI_STATUS_BIT_FSM) {	// Still working? Stop it!
+			PERROR
+			    ("Status is 'ai_status_stream_end' but hardware is still working!\n");
+			spin_lock_irqsave(instance->ctrl_reg_lock, cpu_flags);
+			ctrl = inl(instance->ctrl_reg);
+			ctrl |=
+			    (ME4600_AI_CTRL_BIT_IMMEDIATE_STOP |
+			     ME4600_AI_CTRL_BIT_HF_IRQ_RESET |
+			     ME4600_AI_CTRL_BIT_SC_IRQ_RESET);
+			outl(ctrl, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   ctrl);
+			spin_unlock_irqrestore(instance->ctrl_reg_lock,
+					       cpu_flags);
+		}
+		break;
+
+	default:
+		PERROR_CRITICAL("Status is in wrong state (%d)!\n",
+				instance->status);
+		instance->status = ai_status_stream_error;
+		// Signal the end.
+		signaling = 1;
+		break;
+
+	}
+
+	if (signaling) {	//Signal it.
+		wake_up_interruptible_all(&instance->wait_queue);
+	}
+
+	if (instance->ai_control_task_flag && reschedule) {	// Reschedule task
+		queue_delayed_work(instance->me4600_workqueue,
+				   &instance->ai_control_task, 1);
+	} else {
+		PINFO("<%s> Ending control task.\n", __FUNCTION__);
+	}
+
+}
diff --git a/drivers/staging/meilhaus/me4600_ai.h b/drivers/staging/meilhaus/me4600_ai.h
new file mode 100644
index 00000000000..1d5a1b9c6f9
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_ai.h
@@ -0,0 +1,180 @@
+/**
+ * @file me4600_ai.h
+ *
+ * @brief Meilhaus ME-4000 analog input subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke  (k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME4600_AI_H_
+#define _ME4600_AI_H_
+
+#include <linux/version.h>
+#include "mesubdevice.h"
+#include "meioctl.h"
+#include "mecirc_buf.h"
+
+#ifdef __KERNEL__
+
+#define ME4600_AI_MAX_DATA				0xFFFF
+
+#ifdef ME_SYNAPSE
+# define ME4600_AI_CIRC_BUF_SIZE_ORDER	8	// 2^n PAGES =>> Maximum value of 1MB for Synapse
+#else
+# define ME4600_AI_CIRC_BUF_SIZE_ORDER	5	// 2^n PAGES =>> 128KB
+#endif
+#define ME4600_AI_CIRC_BUF_SIZE 		PAGE_SIZE<<ME4600_AI_CIRC_BUF_SIZE_ORDER	// Buffer size in bytes.
+
+#ifdef _CBUFF_32b_t
+# define ME4600_AI_CIRC_BUF_COUNT		((ME4600_AI_CIRC_BUF_SIZE) / sizeof(uint32_t))	// Size in values
+#else
+# define ME4600_AI_CIRC_BUF_COUNT		((ME4600_AI_CIRC_BUF_SIZE) / sizeof(uint16_t))	// Size in values
+#endif
+
+#define ME4600_AI_FIFO_HALF				1024	//ME4600_AI_FIFO_COUNT/2                //1024
+#define ME4600_AI_FIFO_MAX_SC			1352	//0.66*ME4600_AI_FIFO_COUNT             //1352
+
+typedef enum ME4600_AI_STATUS {
+	ai_status_none = 0,
+	ai_status_single_configured,
+	ai_status_stream_configured,
+	ai_status_stream_run_wait,
+	ai_status_stream_run,
+	ai_status_stream_end_wait,
+	ai_status_stream_end,
+	ai_status_stream_fifo_error,
+	ai_status_stream_buffer_error,
+	ai_status_stream_error,
+	ai_status_last
+} ME4600_AI_STATUS;
+
+typedef struct me4600_single_config_entry {
+	unsigned short status;
+	uint32_t entry;
+	uint32_t ctrl;
+} me4600_single_config_entry_t;
+
+typedef struct me4600_range_entry {
+	int min;
+	int max;
+} me4600_range_entry_t;
+
+typedef struct me4600_ai_ISM {
+	volatile unsigned int global_read;				/**< The number of data read in total. */
+	volatile unsigned int read;						/**< The number of data read for this chunck. */
+	volatile unsigned int next;						/**< The number of data request by user. */
+} me4600_ai_ISM_t;
+
+typedef struct me4600_ai_timeout {
+	unsigned long start_time;
+	unsigned long delay;
+} me4600_ai_timeout_t;
+
+/**
+ * @brief The ME-4000 analog input subdevice class.
+ */
+typedef struct me4600_ai_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;							/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;						/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *ctrl_reg_lock;						/**< Spin lock to protect #ctrl_reg from concurrent access. */
+
+	/* Hardware feautres */
+	unsigned int irq;								/**< The interrupt request number assigned by the PCI BIOS. */
+	int isolated;									/**< Marks if this subdevice is on an optoisolated device. */
+	int sh;											/**< Marks if this subdevice has sample and hold devices. */
+
+	unsigned int channels;							/**< The number of channels available on this subdevice. */
+	me4600_single_config_entry_t single_config[32];	/**< The configuration set for single acquisition. */
+
+	unsigned int data_required;						/**< The number of data request by user. */
+	unsigned int fifo_irq_threshold;				/**< The user adjusted FIFO high water interrupt level. */
+	unsigned int chan_list_len;						/**< The length of the user defined channel list. */
+
+	me4600_ai_ISM_t ISM;							/**< The information request by Interrupt-State-Machine. */
+	volatile enum ME4600_AI_STATUS status;			/**< The current stream status flag. */
+	me4600_ai_timeout_t timeout;					/**< The timeout for start in blocking and non-blocking mode. */
+
+											/* Registers *//**< All registers are 32 bits long. */
+	unsigned long ctrl_reg;
+	unsigned long status_reg;
+	unsigned long channel_list_reg;
+	unsigned long data_reg;
+	unsigned long chan_timer_reg;
+	unsigned long chan_pre_timer_reg;
+	unsigned long scan_timer_low_reg;
+	unsigned long scan_timer_high_reg;
+	unsigned long scan_pre_timer_low_reg;
+	unsigned long scan_pre_timer_high_reg;
+	unsigned long start_reg;
+	unsigned long irq_status_reg;
+	unsigned long sample_counter_reg;
+
+	unsigned int ranges_len;
+	me4600_range_entry_t ranges[4];					/**< The ranges available on this subdevice. */
+
+	/* Software buffer */
+	me_circ_buf_t circ_buf;							/**< Circular buffer holding measurment data. */
+	wait_queue_head_t wait_queue;					/**< Wait queue to put on tasks waiting for data to arrive. */
+
+	struct workqueue_struct *me4600_workqueue;
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+	struct work_struct ai_control_task;
+#else
+	struct delayed_work ai_control_task;
+#endif
+
+	volatile int ai_control_task_flag;				/**< Flag controling reexecuting of control task */
+
+#ifdef MEDEBUG_DEBUG_REG
+	unsigned long reg_base;
+#endif
+} me4600_ai_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-4000 analog input subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ * @param channels The number of analog input channels available on this subdevice.
+ * @param channels The number of analog input ranges available on this subdevice.
+ * @param isolated Flag indicating if this device is opto isolated.
+ * @param sh Flag indicating if sample and hold devices are available.
+ * @param irq The irq number assigned by PCI BIOS.
+ * @param ctrl_reg_lock Pointer to spin lock protecting the control register from concurrent access.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me4600_ai_subdevice_t *me4600_ai_constructor(uint32_t reg_base,
+					     unsigned int channels,
+					     unsigned int ranges,
+					     int isolated,
+					     int sh,
+					     int irq,
+					     spinlock_t * ctrl_reg_lock,
+					     struct workqueue_struct
+					     *me4600_wq);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me4600_ai_reg.h b/drivers/staging/meilhaus/me4600_ai_reg.h
new file mode 100644
index 00000000000..083fac7685f
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_ai_reg.h
@@ -0,0 +1,107 @@
+/**
+ * @file me4600_ai_reg.h
+ *
+ * @brief ME-4000 analog input subdevice register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME4600_AI_REG_H_
+#define _ME4600_AI_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME4600_AI_CTRL_REG					0x74	// _/W
+#define ME4600_AI_STATUS_REG				0x74	// R/_
+#define ME4600_AI_CHANNEL_LIST_REG			0x78	// _/W
+#define ME4600_AI_DATA_REG					0x7C	// R/_
+#define ME4600_AI_CHAN_TIMER_REG			0x80	// _/W
+#define ME4600_AI_CHAN_PRE_TIMER_REG		0x84	// _/W
+#define ME4600_AI_SCAN_TIMER_LOW_REG		0x88	// _/W
+#define ME4600_AI_SCAN_TIMER_HIGH_REG		0x8C	// _/W
+#define ME4600_AI_SCAN_PRE_TIMER_LOW_REG	0x90	// _/W
+#define ME4600_AI_SCAN_PRE_TIMER_HIGH_REG	0x94	// _/W
+#define ME4600_AI_START_REG					0x98	// R/_
+
+#define ME4600_AI_SAMPLE_COUNTER_REG		0xC0	// _/W
+
+#define ME4600_AI_CTRL_BIT_MODE_0			0x00000001
+#define ME4600_AI_CTRL_BIT_MODE_1			0x00000002
+#define ME4600_AI_CTRL_BIT_MODE_2			0x00000004
+#define ME4600_AI_CTRL_BIT_SAMPLE_HOLD		0x00000008
+#define ME4600_AI_CTRL_BIT_IMMEDIATE_STOP	0x00000010
+#define ME4600_AI_CTRL_BIT_STOP				0x00000020
+#define ME4600_AI_CTRL_BIT_CHANNEL_FIFO		0x00000040
+#define ME4600_AI_CTRL_BIT_DATA_FIFO		0x00000080
+#define ME4600_AI_CTRL_BIT_FULLSCALE		0x00000100
+#define ME4600_AI_CTRL_BIT_OFFSET			0x00000200
+#define ME4600_AI_CTRL_BIT_EX_TRIG_ANALOG	0x00000400
+#define ME4600_AI_CTRL_BIT_EX_TRIG			0x00000800
+#define ME4600_AI_CTRL_BIT_EX_TRIG_FALLING	0x00001000
+#define ME4600_AI_CTRL_BIT_EX_IRQ			0x00002000
+#define ME4600_AI_CTRL_BIT_EX_IRQ_RESET		0x00004000
+#define ME4600_AI_CTRL_BIT_LE_IRQ			0x00008000
+#define ME4600_AI_CTRL_BIT_LE_IRQ_RESET		0x00010000
+#define ME4600_AI_CTRL_BIT_HF_IRQ			0x00020000
+#define ME4600_AI_CTRL_BIT_HF_IRQ_RESET		0x00040000
+#define ME4600_AI_CTRL_BIT_SC_IRQ			0x00080000
+#define ME4600_AI_CTRL_BIT_SC_IRQ_RESET		0x00100000
+#define ME4600_AI_CTRL_BIT_SC_RELOAD		0x00200000
+#define ME4600_AI_CTRL_BIT_EX_TRIG_BOTH		0x80000000
+
+#define ME4600_AI_STATUS_BIT_EF_CHANNEL		0x00400000
+#define ME4600_AI_STATUS_BIT_HF_CHANNEL		0x00800000
+#define ME4600_AI_STATUS_BIT_FF_CHANNEL		0x01000000
+#define ME4600_AI_STATUS_BIT_EF_DATA		0x02000000
+#define ME4600_AI_STATUS_BIT_HF_DATA		0x04000000
+#define ME4600_AI_STATUS_BIT_FF_DATA		0x08000000
+#define ME4600_AI_STATUS_BIT_LE				0x10000000
+#define ME4600_AI_STATUS_BIT_FSM			0x20000000
+
+#define ME4600_AI_CTRL_RPCI_FIFO			0x40000000	//Always set to zero!
+
+#define ME4600_AI_BASE_FREQUENCY			33E6
+
+#define ME4600_AI_MIN_ACQ_TICKS				66LL
+#define ME4600_AI_MAX_ACQ_TICKS				0xFFFFFFFFLL
+
+#define ME4600_AI_MIN_SCAN_TICKS			66LL
+#define ME4600_AI_MAX_SCAN_TICKS			0xFFFFFFFFFLL
+
+#define ME4600_AI_MIN_CHAN_TICKS			66LL
+#define ME4600_AI_MAX_CHAN_TICKS			0xFFFFFFFFLL
+
+#define ME4600_AI_FIFO_COUNT				2048
+
+#define ME4600_AI_LIST_COUNT				1024
+
+#define ME4600_AI_LIST_INPUT_SINGLE_ENDED	0x000
+#define ME4600_AI_LIST_INPUT_DIFFERENTIAL	0x020
+
+#define ME4600_AI_LIST_RANGE_BIPOLAR_10		0x000
+#define ME4600_AI_LIST_RANGE_BIPOLAR_2_5	0x040
+#define ME4600_AI_LIST_RANGE_UNIPOLAR_10	0x080
+#define ME4600_AI_LIST_RANGE_UNIPOLAR_2_5	0x0C0
+
+#define ME4600_AI_LIST_LAST_ENTRY		0x100
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me4600_ao.c b/drivers/staging/meilhaus/me4600_ao.c
new file mode 100644
index 00000000000..2c92e655a81
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_ao.c
@@ -0,0 +1,6011 @@
+/**
+ * @file me4600_ao.c
+ *
+ * @brief ME-4000 analog output subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+///Common part. (For normal and Bosch builds.)
+
+/* Includes
+ */
+
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <asm/uaccess.h>
+#include <linux/types.h>
+#include <linux/version.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "meids.h"
+#include "me4600_reg.h"
+#include "me4600_ao_reg.h"
+#include "me4600_ao.h"
+
+/* Defines
+ */
+
+static int me4600_ao_query_range_by_min_max(me_subdevice_t * subdevice,
+					    int unit,
+					    int *min,
+					    int *max, int *maxdata, int *range);
+
+static int me4600_ao_query_number_ranges(me_subdevice_t * subdevice,
+					 int unit, int *count);
+
+static int me4600_ao_query_range_info(me_subdevice_t * subdevice,
+				      int range,
+				      int *unit,
+				      int *min, int *max, int *maxdata);
+
+static int me4600_ao_query_timer(me_subdevice_t * subdevice,
+				 int timer,
+				 int *base_frequency,
+				 long long *min_ticks, long long *max_ticks);
+
+static int me4600_ao_query_number_channels(me_subdevice_t * subdevice,
+					   int *number);
+
+static int me4600_ao_query_subdevice_type(me_subdevice_t * subdevice,
+					  int *type, int *subtype);
+
+static int me4600_ao_query_subdevice_caps(me_subdevice_t * subdevice,
+					  int *caps);
+
+static int me4600_ao_query_subdevice_caps_args(struct me_subdevice *subdevice,
+					       int cap, int *args, int count);
+
+#ifndef BOSCH
+/// @note NORMAL BUILD
+/// @author Krzysztof Gantzke   (k.gantzke@meilhaus.de)
+/* Includes
+ */
+
+# include <linux/workqueue.h>
+
+/* Defines
+ */
+
+/** Remove subdevice.
+*/
+static void me4600_ao_destructor(struct me_subdevice *subdevice);
+
+/** Reset subdevice. Stop all actions. Reset registry. Disable FIFO. Set output to 0V and status to 'none'.
+*/
+static int me4600_ao_io_reset_subdevice(me_subdevice_t * subdevice,
+					struct file *filep, int flags);
+
+/** Set output as single
+*/
+static int me4600_ao_io_single_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int single_config,
+				      int ref,
+				      int trig_chan,
+				      int trig_type, int trig_edge, int flags);
+
+/** Pass to user actual value of output.
+*/
+static int me4600_ao_io_single_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int channel,
+				    int *value, int time_out, int flags);
+
+/** Write to output requed value.
+*/
+static int me4600_ao_io_single_write(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int channel,
+				     int value, int time_out, int flags);
+
+/** Set output as streamed device.
+*/
+static int me4600_ao_io_stream_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      meIOStreamConfig_t * config_list,
+				      int count,
+				      meIOStreamTrigger_t * trigger,
+				      int fifo_irq_threshold, int flags);
+
+/** Wait for / Check empty space in buffer.
+*/
+static int me4600_ao_io_stream_new_values(me_subdevice_t * subdevice,
+					  struct file *filep,
+					  int time_out, int *count, int flags);
+
+/** Start streaming.
+*/
+static int me4600_ao_io_stream_start(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int start_mode, int time_out, int flags);
+
+/** Check actual state. / Wait for end.
+*/
+static int me4600_ao_io_stream_status(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int wait,
+				      int *status, int *values, int flags);
+
+/** Stop streaming.
+*/
+static int me4600_ao_io_stream_stop(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int stop_mode, int flags);
+
+/** Write datas to buffor.
+*/
+static int me4600_ao_io_stream_write(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int write_mode,
+				     int *values, int *count, int flags);
+
+/** Interrupt handler. Copy from buffer to FIFO.
+*/
+static irqreturn_t me4600_ao_isr(int irq, void *dev_id
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 19)
+				 , struct pt_regs *regs
+#endif
+    );
+/** Copy data from circular buffer to fifo (fast) in wraparound mode.
+*/
+int inline ao_write_data_wraparound(me4600_ao_subdevice_t * instance, int count,
+				    int start_pos);
+
+/** Copy data from circular buffer to fifo (fast).
+*/
+int inline ao_write_data(me4600_ao_subdevice_t * instance, int count,
+			 int start_pos);
+
+/** Copy data from circular buffer to fifo (slow).
+*/
+int inline ao_write_data_pooling(me4600_ao_subdevice_t * instance, int count,
+				 int start_pos);
+
+/** Copy data from user space to circular buffer.
+*/
+int inline ao_get_data_from_user(me4600_ao_subdevice_t * instance, int count,
+				 int *user_values);
+
+/** Stop presentation. Preserve FIFOs.
+*/
+int inline ao_stop_immediately(me4600_ao_subdevice_t * instance);
+
+/** Task for asynchronical state verifying.
+*/
+static void me4600_ao_work_control_task(
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+					       void *subdevice
+#else
+					       struct work_struct *work
+#endif
+    );
+/* Functions
+ */
+
+static int me4600_ao_io_reset_subdevice(me_subdevice_t * subdevice,
+					struct file *filep, int flags)
+{
+	me4600_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint32_t tmp;
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	instance->status = ao_status_none;
+	instance->ao_control_task_flag = 0;
+	cancel_delayed_work(&instance->ao_control_task);
+	instance->timeout.delay = 0;
+	instance->timeout.start_time = jiffies;
+
+	//Stop state machine.
+	err = ao_stop_immediately(instance);
+
+	//Remove from synchronous start.
+	spin_lock(instance->preload_reg_lock);
+	tmp = inl(instance->preload_reg);
+	tmp &=
+	    ~((ME4600_AO_SYNC_HOLD | ME4600_AO_SYNC_EXT_TRIG) << instance->
+	      ao_idx);
+	outl(tmp, instance->preload_reg);
+	PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->preload_reg - instance->reg_base, tmp);
+	*instance->preload_flags &=
+	    ~((ME4600_AO_SYNC_HOLD | ME4600_AO_SYNC_EXT_TRIG) << instance->
+	      ao_idx);
+	spin_unlock(instance->preload_reg_lock);
+
+	//Set single mode, dissable FIFO, dissable external trigger, set output to analog, block interrupt.
+	outl(ME4600_AO_MODE_SINGLE | ME4600_AO_CTRL_BIT_STOP |
+	     ME4600_AO_CTRL_BIT_IMMEDIATE_STOP | ME4600_AO_CTRL_BIT_RESET_IRQ,
+	     instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base,
+		   ME4600_AO_MODE_SINGLE | ME4600_AO_CTRL_BIT_STOP |
+		   ME4600_AO_CTRL_BIT_IMMEDIATE_STOP |
+		   ME4600_AO_CTRL_BIT_RESET_IRQ);
+
+	//Set output to 0V
+	outl(0x8000, instance->single_reg);
+	PDEBUG_REG("single_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->single_reg - instance->reg_base, 0x8000);
+
+	instance->circ_buf.head = 0;
+	instance->circ_buf.tail = 0;
+	instance->preloaded_count = 0;
+	instance->data_count = 0;
+	instance->single_value = 0x8000;
+	instance->single_value_in_fifo = 0x8000;
+
+	//Set status to signal that device is unconfigured.
+	instance->status = ao_status_none;
+
+	//Signal reset if user is on wait.
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ao_io_single_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int single_config,
+				      int ref,
+				      int trig_chan,
+				      int trig_type, int trig_edge, int flags)
+{
+	me4600_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint32_t ctrl;
+	uint32_t sync;
+	unsigned long cpu_flags;
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	// Checking parameters
+	if (flags) {
+		PERROR
+		    ("Invalid flag specified. Must be ME_IO_SINGLE_CONFIG_NO_FLAGS.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	switch (trig_type) {
+	case ME_TRIG_TYPE_SW:
+		if (trig_edge != ME_TRIG_EDGE_NONE) {
+			PERROR
+			    ("Invalid trigger edge. Software trigger has not edge.\n");
+			return ME_ERRNO_INVALID_TRIG_EDGE;
+		}
+		break;
+
+	case ME_TRIG_TYPE_EXT_DIGITAL:
+		switch (trig_edge) {
+		case ME_TRIG_EDGE_ANY:
+		case ME_TRIG_EDGE_RISING:
+		case ME_TRIG_EDGE_FALLING:
+			break;
+
+		default:
+			PERROR("Invalid trigger edge.\n");
+			return ME_ERRNO_INVALID_TRIG_EDGE;
+		}
+		break;
+
+	default:
+		PERROR
+		    ("Invalid trigger type. Trigger must be software or digital.\n");
+		return ME_ERRNO_INVALID_TRIG_TYPE;
+	}
+
+	if ((trig_chan != ME_TRIG_CHAN_DEFAULT)
+	    && (trig_chan != ME_TRIG_CHAN_SYNCHRONOUS)) {
+		PERROR("Invalid trigger channel specified.\n");
+		return ME_ERRNO_INVALID_TRIG_CHAN;
+	}
+
+	if (ref != ME_REF_AO_GROUND) {
+		PERROR
+		    ("Invalid reference. Analog outputs have to have got REF_AO_GROUND.\n");
+		return ME_ERRNO_INVALID_REF;
+	}
+
+	if (single_config != 0) {
+		PERROR
+		    ("Invalid single config specified. Only one range for anlog outputs is available.\n");
+		return ME_ERRNO_INVALID_SINGLE_CONFIG;
+	}
+
+	if (channel != 0) {
+		PERROR
+		    ("Invalid channel number specified. Analog output have only one channel.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	//Subdevice running in stream mode!
+	if ((instance->status >= ao_status_stream_run_wait)
+	    && (instance->status < ao_status_stream_end)) {
+		PERROR("Subdevice is busy.\n");
+		ME_SUBDEVICE_EXIT;
+
+		return ME_ERRNO_SUBDEVICE_BUSY;
+	}
+/// @note For single all calls (config and write) are erasing previous state!
+
+	instance->status = ao_status_none;
+
+	// Correct single mirrors
+	instance->single_value_in_fifo = instance->single_value;
+
+	//Stop device
+	err = ao_stop_immediately(instance);
+	if (err) {
+		PERROR_CRITICAL("FSM IS BUSY!\n");
+		ME_SUBDEVICE_EXIT;
+
+		return ME_ERRNO_SUBDEVICE_BUSY;
+	}
+	// Set control register.
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	// Set stop bit. Stop streaming mode.
+	ctrl = inl(instance->ctrl_reg);
+	//Reset all bits.
+	ctrl = ME4600_AO_CTRL_BIT_IMMEDIATE_STOP | ME4600_AO_CTRL_BIT_STOP;
+
+	if (trig_type == ME_TRIG_TYPE_EXT_DIGITAL) {
+		PINFO("External digital trigger.\n");
+
+		if (trig_edge == ME_TRIG_EDGE_ANY) {
+//                              ctrl |= ME4600_AO_CTRL_BIT_EX_TRIG_EDGE | ME4600_AO_CTRL_BIT_EX_TRIG_EDGE_BOTH;
+			instance->ctrl_trg =
+			    ME4600_AO_CTRL_BIT_EX_TRIG_EDGE |
+			    ME4600_AO_CTRL_BIT_EX_TRIG_EDGE_BOTH;
+		} else if (trig_edge == ME_TRIG_EDGE_FALLING) {
+//                              ctrl |= ME4600_AO_CTRL_BIT_EX_TRIG_EDGE;
+			instance->ctrl_trg = ME4600_AO_CTRL_BIT_EX_TRIG_EDGE;
+		} else if (trig_edge == ME_TRIG_EDGE_RISING) {
+			instance->ctrl_trg = 0x0;
+		}
+	} else if (trig_type == ME_TRIG_TYPE_SW) {
+		PDEBUG("Software trigger\n");
+		instance->ctrl_trg = 0x0;
+	}
+
+	outl(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl);
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	// Set preload/synchronization register.
+	spin_lock(instance->preload_reg_lock);
+	if (trig_type == ME_TRIG_TYPE_SW) {
+		*instance->preload_flags &=
+		    ~(ME4600_AO_SYNC_EXT_TRIG << instance->ao_idx);
+	} else			//if (trig_type == ME_TRIG_TYPE_EXT_DIGITAL)
+	{
+		*instance->preload_flags |=
+		    ME4600_AO_SYNC_EXT_TRIG << instance->ao_idx;
+	}
+
+	if (trig_chan == ME_TRIG_CHAN_DEFAULT) {
+		*instance->preload_flags &=
+		    ~(ME4600_AO_SYNC_HOLD << instance->ao_idx);
+	} else			//if (trig_chan == ME_TRIG_CHAN_SYNCHRONOUS)
+	{
+		*instance->preload_flags |=
+		    ME4600_AO_SYNC_HOLD << instance->ao_idx;
+	}
+
+	//Reset hardware register
+	sync = inl(instance->preload_reg);
+	PDEBUG_REG("preload_reg inl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->preload_reg - instance->reg_base, sync);
+	sync &= ~(ME4600_AO_SYNC_EXT_TRIG << instance->ao_idx);
+	sync |= ME4600_AO_SYNC_HOLD << instance->ao_idx;
+
+	//Output configured in default (safe) mode.
+	outl(sync, instance->preload_reg);
+	PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->preload_reg - instance->reg_base, sync);
+	spin_unlock(instance->preload_reg_lock);
+
+	instance->status = ao_status_single_configured;
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ao_io_single_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int channel,
+				    int *value, int time_out, int flags)
+{
+	me4600_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	unsigned long j;
+	unsigned long delay = 0;
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (flags & ~ME_IO_SINGLE_NONBLOCKING) {
+		PERROR("Invalid flag specified. %d\n", flags);
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid timeout specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	if (channel != 0) {
+		PERROR("Invalid channel number specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	if ((instance->status >= ao_status_stream_configured)
+	    && (instance->status <= ao_status_stream_end)) {
+		PERROR("Subdevice not configured to work in single mode!\n");
+		return ME_ERRNO_PREVIOUS_CONFIG;
+	}
+
+	ME_SUBDEVICE_ENTER;
+	if ((!flags) && (instance->status == ao_status_single_run_wait)) {	//Blocking mode. Wait for trigger.
+		if (time_out) {
+			delay = (time_out * HZ) / 1000;
+			if (delay == 0)
+				delay = 1;
+		}
+
+		j = jiffies;
+
+		//Only runing process will interrupt this call. Events are signaled when status change. This procedure has own timeout.
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 (instance->status !=
+						  ao_status_single_run_wait),
+						 (delay) ? delay +
+						 1 : LONG_MAX);
+
+		if (instance->status == ao_status_none) {
+			PDEBUG("Single canceled.\n");
+			err = ME_ERRNO_CANCELLED;
+		}
+
+		if (signal_pending(current)) {
+			PERROR("Wait on start of state machine interrupted.\n");
+			instance->status = ao_status_none;
+			ao_stop_immediately(instance);
+			err = ME_ERRNO_SIGNAL;
+		}
+
+		if ((delay) && ((jiffies - j) >= delay)) {
+
+			PDEBUG("Timeout reached.\n");
+			err = ME_ERRNO_TIMEOUT;
+		}
+
+		*value =
+		    (!err) ? instance->single_value_in_fifo : instance->
+		    single_value;
+	} else {		//Non-blocking mode
+		//Read value
+		*value = instance->single_value;
+	}
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ao_io_single_write(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int channel,
+				     int value, int time_out, int flags)
+{
+	me4600_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long cpu_flags;
+	unsigned long j;
+	unsigned long delay = 0x0;
+
+	//Registry handling variables.
+	uint32_t sync_mask;
+	uint32_t mode;
+	uint32_t tmp;
+	uint32_t ctrl;
+	uint32_t status;
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (flags &
+	    ~(ME_IO_SINGLE_TYPE_TRIG_SYNCHRONOUS |
+	      ME_IO_SINGLE_TYPE_WRITE_NONBLOCKING)) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid timeout specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	if (value & ~ME4600_AO_MAX_DATA) {
+		PERROR("Invalid value provided.\n");
+		return ME_ERRNO_VALUE_OUT_OF_RANGE;
+	}
+
+	if (channel != 0) {
+		PERROR("Invalid channel number specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	if ((instance->status == ao_status_none)
+	    || (instance->status > ao_status_single_end)) {
+		PERROR("Subdevice not configured to work in single mode!\n");
+		return ME_ERRNO_PREVIOUS_CONFIG;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+/// @note For single all calls (config and write) are erasing previous state!
+
+	//Cancel control task
+	PDEBUG("Cancel control task. idx=%d\n", instance->ao_idx);
+	instance->ao_control_task_flag = 0;
+	cancel_delayed_work(&instance->ao_control_task);
+
+	// Correct single mirrors
+	instance->single_value_in_fifo = instance->single_value;
+
+	//Stop device
+	err = ao_stop_immediately(instance);
+	if (err) {
+		PERROR_CRITICAL("FSM IS BUSY!\n");
+		ME_SUBDEVICE_EXIT;
+
+		return ME_ERRNO_SUBDEVICE_BUSY;
+	}
+
+	if (time_out) {
+		delay = (time_out * HZ) / 1000;
+
+		if (delay == 0)
+			delay = 1;
+	}
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+
+	instance->single_value_in_fifo = value;
+
+	ctrl = inl(instance->ctrl_reg);
+
+	if (!instance->fifo) {	//No FIFO
+		//Set the single mode.
+		ctrl &= ~ME4600_AO_CTRL_MODE_MASK;
+
+		//Write value
+		PDEBUG("Write value\n");
+		outl(value, instance->single_reg);
+		PDEBUG_REG("single_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->single_reg - instance->reg_base, value);
+	} else {		// mix-mode
+		//Set speed
+		outl(ME4600_AO_MIN_CHAN_TICKS - 1, instance->timer_reg);
+		PDEBUG_REG("timer_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->timer_reg - instance->reg_base,
+			   (int)ME4600_AO_MIN_CHAN_TICKS);
+		instance->hardware_stop_delay = HZ / 10;	//100ms
+
+		status = inl(instance->status_reg);
+
+		//Set the continous mode.
+		ctrl &= ~ME4600_AO_CTRL_MODE_MASK;
+		ctrl |= ME4600_AO_MODE_CONTINUOUS;
+
+		//Prepare FIFO
+		if (!(ctrl & ME4600_AO_CTRL_BIT_ENABLE_FIFO)) {	//FIFO wasn't enabeled. Do it.
+			PINFO("Enableing FIFO.\n");
+			ctrl &= ~ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+			ctrl |=
+			    ME4600_AO_CTRL_BIT_ENABLE_FIFO |
+			    ME4600_AO_CTRL_BIT_RESET_IRQ;
+		} else {	//Check if FIFO is empty
+			if (status & ME4600_AO_STATUS_BIT_EF) {	//FIFO not empty
+				PINFO("Reseting FIFO.\n");
+				ctrl &=
+				    ~(ME4600_AO_CTRL_BIT_ENABLE_FIFO |
+				      ME4600_AO_CTRL_BIT_ENABLE_IRQ);
+				ctrl |= ME4600_AO_CTRL_BIT_RESET_IRQ;
+				outl(ctrl, instance->ctrl_reg);
+				PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+					   instance->reg_base,
+					   instance->ctrl_reg -
+					   instance->reg_base, ctrl);
+
+				ctrl |=
+				    ME4600_AO_CTRL_BIT_ENABLE_FIFO |
+				    ME4600_AO_CTRL_BIT_RESET_IRQ;
+			} else {	//FIFO empty, only interrupt needs to be disabled!
+				ctrl &= ~ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+				ctrl |= ME4600_AO_CTRL_BIT_RESET_IRQ;
+			}
+		}
+
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+
+		//Write output - 1 value to FIFO
+		if (instance->ao_idx & 0x1) {
+			outl(value <<= 16, instance->fifo_reg);
+			PDEBUG_REG("fifo_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->fifo_reg - instance->reg_base,
+				   value <<= 16);
+		} else {
+			outl(value, instance->fifo_reg);
+			PDEBUG_REG("fifo_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->fifo_reg - instance->reg_base,
+				   value);
+		}
+	}
+
+	mode = *instance->preload_flags >> instance->ao_idx;
+	mode &= (ME4600_AO_SYNC_HOLD | ME4600_AO_SYNC_EXT_TRIG);
+
+	PINFO("Triggering mode: 0x%x\n", mode);
+
+	spin_lock(instance->preload_reg_lock);
+	sync_mask = inl(instance->preload_reg);
+	PDEBUG_REG("preload_reg inl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->preload_reg - instance->reg_base, sync_mask);
+	switch (mode) {
+	case 0:		//Individual software
+		ctrl &= ~ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG;
+
+		if (!instance->fifo) {	// No FIFO - In this case resetting 'ME4600_AO_SYNC_HOLD' will trigger output.
+			if ((sync_mask & ((ME4600_AO_SYNC_HOLD | ME4600_AO_SYNC_EXT_TRIG) << instance->ao_idx)) != ME4600_AO_SYNC_HOLD) {	//Now we can set correct mode. This is exception. It is set to synchronous and triggered later.
+				sync_mask &=
+				    ~(ME4600_AO_SYNC_EXT_TRIG << instance->
+				      ao_idx);
+				sync_mask |=
+				    ME4600_AO_SYNC_HOLD << instance->ao_idx;
+
+				outl(sync_mask, instance->preload_reg);
+				PDEBUG_REG
+				    ("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				     instance->reg_base,
+				     instance->preload_reg - instance->reg_base,
+				     sync_mask);
+			}
+		} else {	// FIFO
+			if ((sync_mask & ((ME4600_AO_SYNC_HOLD | ME4600_AO_SYNC_EXT_TRIG) << instance->ao_idx)) != 0x0) {	//Now we can set correct mode.
+				sync_mask &=
+				    ~((ME4600_AO_SYNC_EXT_TRIG |
+				       ME4600_AO_SYNC_HOLD) << instance->
+				      ao_idx);
+
+				outl(sync_mask, instance->preload_reg);
+				PDEBUG_REG
+				    ("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				     instance->reg_base,
+				     instance->preload_reg - instance->reg_base,
+				     sync_mask);
+			}
+		}
+		instance->single_value = value;
+		break;
+
+	case ME4600_AO_SYNC_EXT_TRIG:	//Individual hardware
+		ctrl |= ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG;
+
+		if (!instance->fifo) {	// No FIFO - In this case resetting 'ME4600_AO_SYNC_HOLD' will trigger output.
+			if ((sync_mask & ((ME4600_AO_SYNC_HOLD | ME4600_AO_SYNC_EXT_TRIG) << instance->ao_idx)) != ME4600_AO_SYNC_HOLD) {	//Now we can set correct mode
+				sync_mask &=
+				    ~(ME4600_AO_SYNC_EXT_TRIG << instance->
+				      ao_idx);
+				sync_mask |=
+				    ME4600_AO_SYNC_HOLD << instance->ao_idx;
+
+				outl(sync_mask, instance->preload_reg);
+				PDEBUG_REG
+				    ("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				     instance->reg_base,
+				     instance->preload_reg - instance->reg_base,
+				     sync_mask);
+			}
+		} else {	// FIFO
+			if ((sync_mask & ((ME4600_AO_SYNC_HOLD | ME4600_AO_SYNC_EXT_TRIG) << instance->ao_idx)) != 0x0) {	//Now we can set correct mode.
+				sync_mask &=
+				    ~((ME4600_AO_SYNC_EXT_TRIG |
+				       ME4600_AO_SYNC_HOLD) << instance->
+				      ao_idx);
+
+				outl(sync_mask, instance->preload_reg);
+				PDEBUG_REG
+				    ("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				     instance->reg_base,
+				     instance->preload_reg - instance->reg_base,
+				     sync_mask);
+			}
+		}
+		break;
+
+	case ME4600_AO_SYNC_HOLD:	//Synchronous software
+		ctrl &= ~ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG;
+
+//                                      if((sync_mask & ((ME4600_AO_SYNC_HOLD | ME4600_AO_SYNC_EXT_TRIG) << instance->ao_idx)) != ME4600_AO_SYNC_HOLD)
+		if ((sync_mask & ((ME4600_AO_SYNC_HOLD | ME4600_AO_SYNC_EXT_TRIG) << instance->ao_idx)) != (ME4600_AO_SYNC_HOLD | ME4600_AO_SYNC_EXT_TRIG)) {	//Now we can set correct mode
+			sync_mask |=
+			    ME4600_AO_SYNC_EXT_TRIG << instance->ao_idx;
+//                                              sync_mask &= ~(ME4600_AO_SYNC_EXT_TRIG << instance->ao_idx);
+			sync_mask |= ME4600_AO_SYNC_HOLD << instance->ao_idx;
+
+			outl(sync_mask, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   sync_mask);
+		}
+		break;
+
+	case (ME4600_AO_SYNC_HOLD | ME4600_AO_SYNC_EXT_TRIG):	//Synchronous hardware
+		ctrl |= ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG;
+		if ((sync_mask & ((ME4600_AO_SYNC_HOLD | ME4600_AO_SYNC_EXT_TRIG) << instance->ao_idx)) != (ME4600_AO_SYNC_HOLD | ME4600_AO_SYNC_EXT_TRIG)) {	//Now we can set correct mode
+			sync_mask |=
+			    (ME4600_AO_SYNC_HOLD | ME4600_AO_SYNC_EXT_TRIG) <<
+			    instance->ao_idx;
+
+			outl(sync_mask, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   sync_mask);
+		}
+		break;
+	}
+//              spin_unlock(instance->preload_reg_lock);        // Moved down.
+
+	//Activate ISM (remove 'stop' bits)
+	ctrl &=
+	    ~(ME4600_AO_CTRL_BIT_EX_TRIG_EDGE |
+	      ME4600_AO_CTRL_BIT_EX_TRIG_EDGE_BOTH);
+	ctrl |= instance->ctrl_trg;
+	ctrl &= ~(ME4600_AO_CTRL_BIT_STOP | ME4600_AO_CTRL_BIT_IMMEDIATE_STOP);
+	outl(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl);
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+/// @note When flag 'ME_IO_SINGLE_TYPE_TRIG_SYNCHRONOUS' is set than output is triggered. ALWAYS!
+
+	if (!instance->fifo) {	//No FIFO
+		if (flags & ME_IO_SINGLE_TYPE_TRIG_SYNCHRONOUS) {	//Fired all software synchronous outputs.
+			tmp = ~(*instance->preload_flags | 0xFFFF0000);
+			PINFO
+			    ("Fired all software synchronous outputs. mask:0x%08x\n",
+			     tmp);
+			tmp |= sync_mask & 0xFFFF0000;
+			// Add this channel to list
+			tmp &= ~(ME4600_AO_SYNC_HOLD << instance->ao_idx);
+
+			//Fire
+			PINFO("Software trigger.\n");
+			outl(tmp, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   tmp);
+
+			//Restore save settings
+			outl(sync_mask, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   sync_mask);
+		} else if (!mode) {	// Add this channel to list
+			outl(sync_mask &
+			     ~(ME4600_AO_SYNC_HOLD << instance->ao_idx),
+			     instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   sync_mask & ~(ME4600_AO_SYNC_HOLD <<
+						 instance->ao_idx));
+
+			//Fire
+			PINFO("Software trigger.\n");
+
+			//Restore save settings
+			outl(sync_mask, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   sync_mask);
+		}
+
+	} else {		// mix-mode - begin
+		if (flags & ME_IO_SINGLE_TYPE_TRIG_SYNCHRONOUS) {	//Trigger outputs
+			//Add channel to start list
+			outl(sync_mask |
+			     (ME4600_AO_SYNC_HOLD << instance->ao_idx),
+			     instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   sync_mask | (ME4600_AO_SYNC_HOLD <<
+						instance->ao_idx));
+
+			//Fire
+			PINFO
+			    ("Fired all software synchronous outputs by software trigger.\n");
+			outl(0x8000, instance->single_reg);
+			PDEBUG_REG("single_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->single_reg - instance->reg_base,
+				   0x8000);
+
+			//Restore save settings
+			outl(sync_mask, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   sync_mask);
+		} else if (!mode) {	//Trigger outputs
+/*			//Remove channel from start list //<== Unnecessary. Removed.
+			outl(sync_mask & ~(ME4600_AO_SYNC_HOLD << instance->ao_idx), instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base, instance->preload_reg - instance->reg_base, tmp);
+*/
+			//Fire
+			PINFO("Software trigger.\n");
+			outl(0x8000, instance->single_reg);
+			PDEBUG_REG("single_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->single_reg - instance->reg_base,
+				   0x8000);
+
+/*			//Restore save settings //<== Unnecessary. Removed.
+			outl(sync_mask, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base, instance->preload_reg - instance->reg_base, sync_mask);
+*/
+		}
+	}
+	spin_unlock(instance->preload_reg_lock);
+
+	j = jiffies;
+	instance->status = ao_status_single_run_wait;
+
+	instance->timeout.delay = delay;
+	instance->timeout.start_time = j;
+	instance->ao_control_task_flag = 1;
+	queue_delayed_work(instance->me4600_workqueue,
+			   &instance->ao_control_task, 1);
+
+	if (!(flags & ME_IO_SINGLE_TYPE_WRITE_NONBLOCKING)) {
+
+		//Only runing process will interrupt this call. Events are signaled when status change. Extra timeout add for safe reason.
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 (instance->status !=
+						  ao_status_single_run_wait),
+						 (delay) ? delay +
+						 1 : LONG_MAX);
+
+		if (((!delay) || ((jiffies - j) <= delay))
+		    && (instance->status != ao_status_single_end)) {
+			PDEBUG("Single canceled.\n");
+			err = ME_ERRNO_CANCELLED;
+		}
+
+		if (signal_pending(current)) {
+			PERROR("Wait on start of state machine interrupted.\n");
+			instance->ao_control_task_flag = 0;
+			cancel_delayed_work(&instance->ao_control_task);
+			ao_stop_immediately(instance);
+			instance->status = ao_status_none;
+			err = ME_ERRNO_SIGNAL;
+		}
+
+		if ((delay) && ((jiffies - j) >= delay)) {
+			if (instance->status == ao_status_single_end) {
+				PDEBUG("Timeout reached.\n");
+			} else {
+				if ((jiffies - j) > delay) {
+					PERROR
+					    ("Timeout reached. Not handled by control task!\n");
+				} else {
+					PERROR
+					    ("Timeout reached. Signal come but status is strange: %d\n",
+					     instance->status);
+				}
+
+				ao_stop_immediately(instance);
+			}
+
+			instance->ao_control_task_flag = 0;
+			cancel_delayed_work(&instance->ao_control_task);
+			instance->status = ao_status_single_end;
+			err = ME_ERRNO_TIMEOUT;
+		}
+	}
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ao_io_stream_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      meIOStreamConfig_t * config_list,
+				      int count,
+				      meIOStreamTrigger_t * trigger,
+				      int fifo_irq_threshold, int flags)
+{
+	me4600_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint32_t ctrl;
+	unsigned long cpu_flags;
+	uint64_t conv_ticks;
+	unsigned int conv_start_ticks_low = trigger->iConvStartTicksLow;
+	unsigned int conv_start_ticks_high = trigger->iConvStartTicksHigh;
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (!instance->fifo) {
+		PERROR("Not a streaming ao.\n");
+		return ME_ERRNO_NOT_SUPPORTED;
+	}
+
+	conv_ticks =
+	    (uint64_t) conv_start_ticks_low +
+	    ((uint64_t) conv_start_ticks_high << 32);
+
+	if (flags &
+	    ~(ME_IO_STREAM_CONFIG_HARDWARE_ONLY | ME_IO_STREAM_CONFIG_WRAPAROUND
+	      | ME_IO_STREAM_CONFIG_BIT_PATTERN)) {
+		PERROR("Invalid flags.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (flags & ME_IO_STREAM_CONFIG_HARDWARE_ONLY) {
+		if (!flags & ME_IO_STREAM_CONFIG_WRAPAROUND) {
+			PERROR
+			    ("Hardware ME_IO_STREAM_CONFIG_HARDWARE_ONLY has to be with ME_IO_STREAM_CONFIG_WRAPAROUND.\n");
+			return ME_ERRNO_INVALID_FLAGS;
+		}
+
+		if ((trigger->iAcqStopTrigType != ME_TRIG_TYPE_NONE)
+		    || (trigger->iScanStopTrigType != ME_TRIG_TYPE_NONE)) {
+			PERROR
+			    ("Hardware wraparound mode must be in infinite mode.\n");
+			return ME_ERRNO_INVALID_FLAGS;
+		}
+	}
+
+	if (count != 1) {
+		PERROR("Only 1 entry in config list acceptable.\n");
+		return ME_ERRNO_INVALID_CONFIG_LIST_COUNT;
+	}
+
+	if (config_list[0].iChannel != 0) {
+		PERROR("Invalid channel number specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	if (config_list[0].iStreamConfig != 0) {
+		PERROR("Only one range available.\n");
+		return ME_ERRNO_INVALID_STREAM_CONFIG;
+	}
+
+	if (config_list[0].iRef != ME_REF_AO_GROUND) {
+		PERROR("Output is referenced to ground.\n");
+		return ME_ERRNO_INVALID_REF;
+	}
+
+	if ((trigger->iAcqStartTicksLow != 0)
+	    || (trigger->iAcqStartTicksHigh != 0)) {
+		PERROR
+		    ("Invalid acquisition start trigger argument specified.\n");
+		return ME_ERRNO_INVALID_ACQ_START_ARG;
+	}
+
+	if (config_list[0].iFlags) {
+		PERROR("Invalid config list flag.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	switch (trigger->iAcqStartTrigType) {
+	case ME_TRIG_TYPE_SW:
+		if (trigger->iAcqStartTrigEdge != ME_TRIG_EDGE_NONE) {
+			PERROR
+			    ("Invalid acquisition start trigger edge specified.\n");
+			return ME_ERRNO_INVALID_ACQ_START_TRIG_EDGE;
+		}
+		break;
+
+	case ME_TRIG_TYPE_EXT_DIGITAL:
+		switch (trigger->iAcqStartTrigEdge) {
+		case ME_TRIG_EDGE_ANY:
+		case ME_TRIG_EDGE_RISING:
+		case ME_TRIG_EDGE_FALLING:
+			break;
+
+		default:
+			PERROR
+			    ("Invalid acquisition start trigger edge specified.\n");
+			return ME_ERRNO_INVALID_ACQ_START_TRIG_EDGE;
+		}
+		break;
+
+	default:
+		PERROR("Invalid acquisition start trigger type specified.\n");
+		return ME_ERRNO_INVALID_ACQ_START_TRIG_TYPE;
+	}
+
+	if (trigger->iScanStartTrigType != ME_TRIG_TYPE_FOLLOW) {
+		PERROR("Invalid scan start trigger type specified.\n");
+		return ME_ERRNO_INVALID_SCAN_START_TRIG_TYPE;
+	}
+
+	if (trigger->iConvStartTrigType != ME_TRIG_TYPE_TIMER) {
+		PERROR("Invalid conv start trigger type specified.\n");
+		return ME_ERRNO_INVALID_CONV_START_TRIG_TYPE;
+	}
+
+	if ((conv_ticks < ME4600_AO_MIN_CHAN_TICKS)
+	    || (conv_ticks > ME4600_AO_MAX_CHAN_TICKS)) {
+		PERROR("Invalid conv start trigger argument specified.\n");
+		return ME_ERRNO_INVALID_CONV_START_ARG;
+	}
+
+	if (trigger->iAcqStartTicksLow || trigger->iAcqStartTicksHigh) {
+		PERROR("Invalid acq start trigger argument specified.\n");
+		return ME_ERRNO_INVALID_ACQ_START_ARG;
+	}
+
+	if (trigger->iScanStartTicksLow || trigger->iScanStartTicksHigh) {
+		PERROR("Invalid scan start trigger argument specified.\n");
+		return ME_ERRNO_INVALID_SCAN_START_ARG;
+	}
+
+	switch (trigger->iScanStopTrigType) {
+	case ME_TRIG_TYPE_NONE:
+		if (trigger->iScanStopCount != 0) {
+			PERROR("Invalid scan stop count specified.\n");
+			return ME_ERRNO_INVALID_SCAN_STOP_ARG;
+		}
+		break;
+
+	case ME_TRIG_TYPE_COUNT:
+		if (flags & ME_IO_STREAM_CONFIG_WRAPAROUND) {
+			if (trigger->iScanStopCount <= 0) {
+				PERROR("Invalid scan stop count specified.\n");
+				return ME_ERRNO_INVALID_SCAN_STOP_ARG;
+			}
+		} else {
+			PERROR("The continous mode has not 'scan' contects.\n");
+			return ME_ERRNO_INVALID_ACQ_STOP_TRIG_TYPE;
+		}
+		break;
+
+	default:
+		PERROR("Invalid scan stop trigger type specified.\n");
+		return ME_ERRNO_INVALID_SCAN_STOP_TRIG_TYPE;
+	}
+
+	switch (trigger->iAcqStopTrigType) {
+	case ME_TRIG_TYPE_NONE:
+		if (trigger->iAcqStopCount != 0) {
+			PERROR("Invalid acq stop count specified.\n");
+			return ME_ERRNO_INVALID_ACQ_STOP_ARG;
+		}
+		break;
+
+	case ME_TRIG_TYPE_COUNT:
+		if (trigger->iScanStopTrigType != ME_TRIG_TYPE_NONE) {
+			PERROR("Invalid acq stop trigger type specified.\n");
+			return ME_ERRNO_INVALID_ACQ_STOP_TRIG_TYPE;
+		}
+
+		if (flags & ME_IO_STREAM_CONFIG_WRAPAROUND) {
+			if (trigger->iAcqStopCount <= 0) {
+				PERROR
+				    ("The continous mode has not 'scan' contects.\n");
+				return ME_ERRNO_INVALID_ACQ_STOP_ARG;
+			}
+		}
+		break;
+
+	default:
+		PERROR("Invalid acq stop trigger type specified.\n");
+		return ME_ERRNO_INVALID_ACQ_STOP_TRIG_TYPE;
+	}
+
+	switch (trigger->iAcqStartTrigChan) {
+	case ME_TRIG_CHAN_DEFAULT:
+	case ME_TRIG_CHAN_SYNCHRONOUS:
+		break;
+
+	default:
+		PERROR("Invalid acq start trigger channel specified.\n");
+		return ME_ERRNO_INVALID_ACQ_START_TRIG_CHAN;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	if ((flags & ME_IO_STREAM_CONFIG_BIT_PATTERN) && !instance->bitpattern) {
+		PERROR("This subdevice not support output redirection.\n");
+		ME_SUBDEVICE_EXIT;
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+	//Stop device
+
+	//Cancel control task
+	PDEBUG("Cancel control task. idx=%d\n", instance->ao_idx);
+	instance->ao_control_task_flag = 0;
+	cancel_delayed_work(&instance->ao_control_task);
+
+	//Check if state machine is stopped.
+	err = ao_stop_immediately(instance);
+	if (err) {
+		PERROR_CRITICAL("FSM IS BUSY!\n");
+		ME_SUBDEVICE_EXIT;
+
+		return ME_ERRNO_SUBDEVICE_BUSY;
+	}
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	//Reset control register. Block all actions. Disable IRQ. Disable FIFO.
+	ctrl =
+	    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP | ME4600_AO_CTRL_BIT_STOP |
+	    ME4600_AO_CTRL_BIT_RESET_IRQ;
+	outl(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl);
+
+	//This is paranoic, but to be sure.
+	instance->preloaded_count = 0;
+	instance->data_count = 0;
+	instance->circ_buf.head = 0;
+	instance->circ_buf.tail = 0;
+
+	/* Set mode. */
+	if (flags & ME_IO_STREAM_CONFIG_WRAPAROUND) {	//Wraparound
+		if (flags & ME_IO_STREAM_CONFIG_HARDWARE_ONLY) {	//Hardware wraparound
+			PINFO("Hardware wraparound.\n");
+			ctrl |= ME4600_AO_MODE_WRAPAROUND;
+			instance->mode = ME4600_AO_HW_WRAP_MODE;
+		} else {	//Software wraparound
+			PINFO("Software wraparound.\n");
+			ctrl |= ME4600_AO_MODE_CONTINUOUS;
+			instance->mode = ME4600_AO_SW_WRAP_MODE;
+		}
+	} else {		//Continous
+		PINFO("Continous.\n");
+		ctrl |= ME4600_AO_MODE_CONTINUOUS;
+		instance->mode = ME4600_AO_CONTINOUS;
+	}
+
+	//Set the trigger edge.
+	if (trigger->iAcqStartTrigType == ME_TRIG_TYPE_EXT_DIGITAL) {	//Set the trigger type and edge for external trigger.
+		PINFO("External digital trigger.\n");
+		instance->start_mode = ME4600_AO_EXT_TRIG;
+/*
+			ctrl |= ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG;
+*/
+		switch (trigger->iAcqStartTrigEdge) {
+		case ME_TRIG_EDGE_RISING:
+			PINFO("Set the trigger edge: rising.\n");
+			instance->ctrl_trg = 0x0;
+			break;
+
+		case ME_TRIG_EDGE_FALLING:
+			PINFO("Set the trigger edge: falling.\n");
+//                                      ctrl |= ME4600_AO_CTRL_BIT_EX_TRIG_EDGE;
+			instance->ctrl_trg = ME4600_AO_CTRL_BIT_EX_TRIG_EDGE;
+			break;
+
+		case ME_TRIG_EDGE_ANY:
+			PINFO("Set the trigger edge: both edges.\n");
+//                                      ctrl |= ME4600_AO_CTRL_BIT_EX_TRIG_EDGE | ME4600_AO_CTRL_BIT_EX_TRIG_EDGE_BOTH;
+			instance->ctrl_trg =
+			    ME4600_AO_CTRL_BIT_EX_TRIG_EDGE |
+			    ME4600_AO_CTRL_BIT_EX_TRIG_EDGE_BOTH;
+			break;
+		}
+	} else {
+		PINFO("Internal software trigger.\n");
+		instance->start_mode = 0;
+	}
+
+	//Set the stop mode and value.
+	if (trigger->iAcqStopTrigType == ME_TRIG_TYPE_COUNT) {	//Amount of data
+		instance->stop_mode = ME4600_AO_ACQ_STOP_MODE;
+		instance->stop_count = trigger->iAcqStopCount;
+	} else if (trigger->iScanStopTrigType == ME_TRIG_TYPE_COUNT) {	//Amount of 'scans'
+		instance->stop_mode = ME4600_AO_SCAN_STOP_MODE;
+		instance->stop_count = trigger->iScanStopCount;
+	} else {		//Infinite
+		instance->stop_mode = ME4600_AO_INF_STOP_MODE;
+		instance->stop_count = 0;
+	}
+
+	PINFO("Stop count: %d.\n", instance->stop_count);
+
+	if (trigger->iAcqStartTrigChan == ME_TRIG_CHAN_SYNCHRONOUS) {	//Synchronous start
+		instance->start_mode |= ME4600_AO_SYNC_HOLD;
+		if (trigger->iAcqStartTrigType == ME_TRIG_TYPE_EXT_DIGITAL) {	//Externaly triggered
+			PINFO("Synchronous start. Externaly trigger active.\n");
+			instance->start_mode |= ME4600_AO_SYNC_EXT_TRIG;
+		}
+#ifdef MEDEBUG_INFO
+		else {
+			PINFO
+			    ("Synchronous start. Externaly trigger dissabled.\n");
+		}
+#endif
+
+	}
+	//Set speed
+	outl(conv_ticks - 2, instance->timer_reg);
+	PDEBUG_REG("timer_reg outl(0x%lX+0x%lX)=0x%llx\n", instance->reg_base,
+		   instance->timer_reg - instance->reg_base, conv_ticks - 2);
+	instance->hardware_stop_delay = (int)(conv_ticks * HZ) / ME4600_AO_BASE_FREQUENCY;	//<== MUST be with cast!
+
+	//Conect outputs to analog or digital port.
+	if (flags & ME_IO_STREAM_CONFIG_BIT_PATTERN) {
+		ctrl |= ME4600_AO_CTRL_BIT_ENABLE_DO;
+	}
+	// Write the control word
+	outl(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl);
+
+	//Set status.
+	instance->status = ao_status_stream_configured;
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ao_io_stream_new_values(me_subdevice_t * subdevice,
+					  struct file *filep,
+					  int time_out, int *count, int flags)
+{
+	me4600_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	long t = 0;
+	long j;
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (!instance->fifo) {
+		PERROR("Not a streaming ao.\n");
+		return ME_ERRNO_NOT_SUPPORTED;
+	}
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (!instance->circ_buf.buf) {
+		PERROR("Circular buffer not exists.\n");
+		return ME_ERRNO_INTERNAL;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid time_out specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	if (me_circ_buf_space(&instance->circ_buf)) {	//The buffer is NOT full.
+		*count = me_circ_buf_space(&instance->circ_buf);
+	} else {		//The buffer is full.
+		if (time_out) {
+			t = (time_out * HZ) / 1000;
+
+			if (t == 0)
+				t = 1;
+		} else {	//Max time.
+			t = LONG_MAX;
+		}
+
+		*count = 0;
+
+		j = jiffies;
+
+		//Only runing process will interrupt this call. Interrupts are when FIFO HF is signaled.
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 ((me_circ_buf_space
+						   (&instance->circ_buf))
+						  || !(inl(instance->status_reg)
+						       &
+						       ME4600_AO_STATUS_BIT_FSM)),
+						 t);
+
+		if (!(inl(instance->status_reg) & ME4600_AO_STATUS_BIT_FSM)) {
+			PERROR("AO subdevice is not running.\n");
+			err = ME_ERRNO_SUBDEVICE_NOT_RUNNING;
+		} else if (signal_pending(current)) {
+			PERROR("Wait on values interrupted from signal.\n");
+			instance->status = ao_status_none;
+			ao_stop_immediately(instance);
+			err = ME_ERRNO_SIGNAL;
+		} else if ((jiffies - j) >= t) {
+			PERROR("Wait on values timed out.\n");
+			err = ME_ERRNO_TIMEOUT;
+		} else {	//Uff... all is good. Inform user about empty space.
+			*count = me_circ_buf_space(&instance->circ_buf);
+		}
+	}
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ao_io_stream_start(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int start_mode, int time_out, int flags)
+{
+	me4600_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long cpu_flags = 0;
+	uint32_t status;
+	uint32_t ctrl;
+	uint32_t synch;
+	int count = 0;
+	int circ_buffer_count;
+
+	unsigned long ref;
+	unsigned long delay = 0;
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (!instance->fifo) {
+		PERROR("Not a streaming ao.\n");
+		return ME_ERRNO_NOT_SUPPORTED;
+	}
+
+	if (flags & ~ME_IO_STREAM_START_TYPE_TRIG_SYNCHRONOUS) {
+		PERROR("Invalid flags.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid timeout specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	if ((start_mode != ME_START_MODE_BLOCKING)
+	    && (start_mode != ME_START_MODE_NONBLOCKING)) {
+		PERROR("Invalid start mode specified.\n");
+		return ME_ERRNO_INVALID_START_MODE;
+	}
+
+	if (time_out) {
+		delay = (time_out * HZ) / 1000;
+		if (delay == 0)
+			delay = 1;
+	}
+
+	switch (instance->status) {	//Checking actual mode.
+	case ao_status_stream_configured:
+	case ao_status_stream_end:
+		//Correct modes!
+		break;
+
+		//The device is in wrong mode.
+	case ao_status_none:
+	case ao_status_single_configured:
+	case ao_status_single_run_wait:
+	case ao_status_single_run:
+	case ao_status_single_end_wait:
+		PERROR
+		    ("Subdevice must be preinitialize correctly for streaming.\n");
+		return ME_ERRNO_PREVIOUS_CONFIG;
+
+	case ao_status_stream_fifo_error:
+	case ao_status_stream_buffer_error:
+	case ao_status_stream_error:
+		PDEBUG("Before restart broke stream 'STOP' must be caled.\n");
+		return ME_STATUS_ERROR;
+
+	case ao_status_stream_run_wait:
+	case ao_status_stream_run:
+	case ao_status_stream_end_wait:
+		PDEBUG("Stream is already working.\n");
+		return ME_ERRNO_SUBDEVICE_BUSY;
+
+	default:
+		instance->status = ao_status_stream_error;
+		PERROR_CRITICAL("Status is in wrong state!\n");
+		return ME_ERRNO_INTERNAL;
+
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	if (instance->mode == ME4600_AO_CONTINOUS) {	//Continous
+		instance->circ_buf.tail += instance->preloaded_count;
+		instance->circ_buf.tail &= instance->circ_buf.mask;
+	}
+	circ_buffer_count = me_circ_buf_values(&instance->circ_buf);
+
+	if (!circ_buffer_count && !instance->preloaded_count) {	//No values in buffer
+		ME_SUBDEVICE_EXIT;
+		PERROR("No values in buffer!\n");
+		return ME_ERRNO_LACK_OF_RESOURCES;
+	}
+
+	//Cancel control task
+	PDEBUG("Cancel control task. idx=%d\n", instance->ao_idx);
+	instance->ao_control_task_flag = 0;
+	cancel_delayed_work(&instance->ao_control_task);
+
+	//Stop device
+	err = ao_stop_immediately(instance);
+	if (err) {
+		PERROR_CRITICAL("FSM IS BUSY!\n");
+		ME_SUBDEVICE_EXIT;
+
+		return ME_ERRNO_SUBDEVICE_BUSY;
+	}
+	//Set values for single_read()
+	instance->single_value = ME4600_AO_MAX_DATA + 1;
+	instance->single_value_in_fifo = ME4600_AO_MAX_DATA + 1;
+
+	//Setting stop points
+	if (instance->stop_mode == ME4600_AO_SCAN_STOP_MODE) {
+		instance->stop_data_count =
+		    instance->stop_count * circ_buffer_count;
+	} else {
+		instance->stop_data_count = instance->stop_count;
+	}
+
+	if ((instance->stop_data_count != 0)
+	    && (instance->stop_data_count < circ_buffer_count)) {
+		PERROR("More data in buffer than previously set limit!\n");
+	}
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	ctrl = inl(instance->ctrl_reg);
+	//Check FIFO
+	if (!(ctrl & ME4600_AO_CTRL_BIT_ENABLE_FIFO)) {	//FIFO wasn't enabeled. Do it. <= This should be done by user call with ME_WRITE_MODE_PRELOAD
+		PINFO("Enableing FIFO.\n");
+		ctrl |=
+		    ME4600_AO_CTRL_BIT_ENABLE_FIFO |
+		    ME4600_AO_CTRL_BIT_RESET_IRQ;
+
+		instance->preloaded_count = 0;
+		instance->data_count = 0;
+	} else {		//Block IRQ
+		ctrl |= ME4600_AO_CTRL_BIT_RESET_IRQ;
+	}
+	outl(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base,
+		   ctrl | ME4600_AO_CTRL_BIT_RESET_IRQ);
+
+	//Fill FIFO <= Generaly this should be done by user pre-load call but this is second place to do it.
+	status = inl(instance->status_reg);
+	if (!(status & ME4600_AO_STATUS_BIT_EF)) {	//FIFO empty
+		if (instance->stop_data_count == 0) {
+			count = ME4600_AO_FIFO_COUNT;
+		} else {
+			count =
+			    (ME4600_AO_FIFO_COUNT <
+			     instance->
+			     stop_data_count) ? ME4600_AO_FIFO_COUNT :
+			    instance->stop_data_count;
+		}
+
+		//Copy data
+		count =
+		    ao_write_data(instance, count, instance->preloaded_count);
+
+		if (count < 0) {	//This should never happend!
+			PERROR_CRITICAL("COPY FINISH WITH ERROR!\n");
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+			ME_SUBDEVICE_EXIT;
+			return ME_ERRNO_INTERNAL;
+		}
+	}
+	//Set pre-load features.
+	spin_lock(instance->preload_reg_lock);
+	synch = inl(instance->preload_reg);
+	synch &=
+	    ~((ME4600_AO_SYNC_HOLD | ME4600_AO_SYNC_EXT_TRIG) << instance->
+	      ao_idx);
+	synch |=
+	    (instance->start_mode & ~ME4600_AO_EXT_TRIG) << instance->ao_idx;
+	outl(synch, instance->preload_reg);
+	PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->preload_reg - instance->reg_base, synch);
+	spin_unlock(instance->preload_reg_lock);
+
+	//Default count is '0'
+	if (instance->mode == ME4600_AO_CONTINOUS) {	//Continous
+		instance->preloaded_count = 0;
+		instance->circ_buf.tail += count;
+		instance->circ_buf.tail &= instance->circ_buf.mask;
+	} else {		//Wraparound
+		instance->preloaded_count += count;
+		instance->data_count += count;
+
+		//Special case: Infinite wraparound with less than FIFO datas always should runs in hardware mode.
+		if ((instance->stop_mode == ME4600_AO_INF_STOP_MODE)
+		    && (circ_buffer_count <= ME4600_AO_FIFO_COUNT)) {	//Change to hardware wraparound
+			PDEBUG
+			    ("Changeing mode from software wraparound to hardware wraparound.\n");
+			//Copy all data
+			count =
+			    ao_write_data(instance, circ_buffer_count,
+					  instance->preloaded_count);
+			ctrl &= ~ME4600_AO_CTRL_MODE_MASK;
+			ctrl |= ME4600_AO_MODE_WRAPAROUND;
+		}
+
+		if (instance->preloaded_count == me_circ_buf_values(&instance->circ_buf)) {	//Reset position indicator.
+			instance->preloaded_count = 0;
+		} else if (instance->preloaded_count > me_circ_buf_values(&instance->circ_buf)) {	//This should never happend!
+			PERROR_CRITICAL
+			    ("PRELOADED MORE VALUES THAN ARE IN BUFFER!\n");
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+			ME_SUBDEVICE_EXIT;
+			return ME_ERRNO_INTERNAL;
+		}
+	}
+
+	//Set status to 'wait for start'
+	instance->status = ao_status_stream_run_wait;
+
+	status = inl(instance->status_reg);
+	//Start state machine and interrupts
+	ctrl &= ~(ME4600_AO_CTRL_BIT_STOP | ME4600_AO_CTRL_BIT_IMMEDIATE_STOP);
+	if (instance->start_mode == ME4600_AO_EXT_TRIG) {	// External trigger.
+		PINFO("External trigger.\n");
+		ctrl |= ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG;
+	}
+	if (!(status & ME4600_AO_STATUS_BIT_HF)) {	//More than half!
+		if ((ctrl & ME4600_AO_CTRL_MODE_MASK) == ME4600_AO_MODE_CONTINUOUS) {	//Enable IRQ only when hardware_continous is set and FIFO is more than half
+			ctrl &= ~ME4600_AO_CTRL_BIT_RESET_IRQ;
+			ctrl |= ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+		}
+	}
+	outl(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl);
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	//Trigger output
+	if (flags & ME_IO_SINGLE_TYPE_TRIG_SYNCHRONOUS) {	//Trigger outputs
+		spin_lock(instance->preload_reg_lock);
+		synch = inl(instance->preload_reg);
+		//Add channel to start list
+		outl(synch | (ME4600_AO_SYNC_HOLD << instance->ao_idx),
+		     instance->preload_reg);
+		PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->preload_reg - instance->reg_base,
+			   synch | (ME4600_AO_SYNC_HOLD << instance->ao_idx));
+
+		//Fire
+		PINFO
+		    ("Fired all software synchronous outputs by software trigger.\n");
+		outl(0x8000, instance->single_reg);
+		PDEBUG_REG("single_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->single_reg - instance->reg_base, 0x8000);
+
+		//Restore save settings
+		outl(synch, instance->preload_reg);
+		PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->preload_reg - instance->reg_base, synch);
+		spin_unlock(instance->preload_reg_lock);
+	} else if (!instance->start_mode) {	//Trigger outputs
+/*
+		//Remove channel from start list.	// <== Unnecessary. Removed.
+		spin_lock(instance->preload_reg_lock);
+			synch = inl(instance->preload_reg);
+			outl(synch & ~(ME4600_AO_SYNC_HOLD << instance->ao_idx), instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base, instance->preload_reg - instance->reg_base, synch & ~(ME4600_AO_SYNC_HOLD << instance->ao_idx));
+*/
+		//Fire
+		PINFO("Software trigger.\n");
+		outl(0x8000, instance->single_reg);
+		PDEBUG_REG("single_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->single_reg - instance->reg_base, 0x8000);
+
+/*
+			//Restore save settings.	// <== Unnecessary. Removed.
+			outl(synch, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base, instance->preload_reg - instance->reg_base, synch);
+		spin_unlock(instance->preload_reg_lock);
+*/
+	}
+	// Set control task's timeout
+	ref = jiffies;
+	instance->timeout.delay = delay;
+	instance->timeout.start_time = ref;
+
+	if (status & ME4600_AO_STATUS_BIT_HF) {	//Less than half but not empty!
+		PINFO("Less than half.\n");
+		if (instance->stop_data_count != 0) {
+			count = ME4600_AO_FIFO_COUNT / 2;
+		} else {
+			count =
+			    ((ME4600_AO_FIFO_COUNT / 2) <
+			     instance->stop_data_count) ? ME4600_AO_FIFO_COUNT /
+			    2 : instance->stop_data_count;
+		}
+
+		//Copy data
+		count =
+		    ao_write_data(instance, count, instance->preloaded_count);
+
+		if (count < 0) {	//This should never happend!
+			PERROR_CRITICAL("COPY FINISH WITH ERROR!\n");
+			ME_SUBDEVICE_EXIT;
+			return ME_ERRNO_INTERNAL;
+		}
+
+		if (instance->mode == ME4600_AO_CONTINOUS) {	//Continous
+			instance->circ_buf.tail += count;
+			instance->circ_buf.tail &= instance->circ_buf.mask;
+		} else {	//Wraparound
+			instance->data_count += count;
+			instance->preloaded_count += count;
+
+			if (instance->preloaded_count == me_circ_buf_values(&instance->circ_buf)) {	//Reset position indicator.
+				instance->preloaded_count = 0;
+			} else if (instance->preloaded_count > me_circ_buf_values(&instance->circ_buf)) {	//This should never happend!
+				PERROR_CRITICAL
+				    ("PRELOADED MORE VALUES THAN ARE IN BUFFER!\n");
+				ME_SUBDEVICE_EXIT;
+				return ME_ERRNO_INTERNAL;
+			}
+		}
+
+		status = inl(instance->status_reg);
+		if (!(status & ME4600_AO_STATUS_BIT_HF)) {	//More than half!
+			spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+			ctrl = inl(instance->ctrl_reg);
+			ctrl &= ~ME4600_AO_CTRL_BIT_RESET_IRQ;
+			ctrl |= ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+			outl(ctrl, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   ctrl);
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+		}
+	}
+	//Special case: Limited wraparound with less than HALF FIFO datas need work around to generate first interrupt.
+	if ((instance->stop_mode != ME4600_AO_INF_STOP_MODE)
+	    && (instance->mode == ME4600_AO_SW_WRAP_MODE)
+	    && (circ_buffer_count <= (ME4600_AO_FIFO_COUNT / 2))) {	//Put more data to FIFO
+		PINFO("Limited wraparound with less than HALF FIFO datas.\n");
+		if (instance->preloaded_count) {	//This should never happend!
+			PERROR_CRITICAL
+			    ("ERROR WHEN LOADING VALUES FOR WRAPAROUND!\n");
+			ME_SUBDEVICE_EXIT;
+			return ME_ERRNO_INTERNAL;
+		}
+
+		while (instance->stop_data_count > instance->data_count) {	//Maximum data not set jet.
+			//Copy to buffer
+			if (circ_buffer_count != ao_write_data(instance, circ_buffer_count, 0)) {	//This should never happend!
+				PERROR_CRITICAL
+				    ("ERROR WHEN LOADING VALUES FOR WRAPAROUND!\n");
+				ME_SUBDEVICE_EXIT;
+				return ME_ERRNO_INTERNAL;
+			}
+			instance->data_count += circ_buffer_count;
+
+			if (!((status = inl(instance->status_reg)) & ME4600_AO_STATUS_BIT_HF)) {	//FIFO is more than half. Enable IRQ and end copy.
+				spin_lock_irqsave(&instance->subdevice_lock,
+						  cpu_flags);
+				ctrl = inl(instance->ctrl_reg);
+				ctrl &= ~ME4600_AO_CTRL_BIT_RESET_IRQ;
+				ctrl |= ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+				outl(ctrl, instance->ctrl_reg);
+				PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+					   instance->reg_base,
+					   instance->ctrl_reg -
+					   instance->reg_base, ctrl);
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+				break;
+			}
+		}
+	}
+	// Schedule control task.
+	instance->ao_control_task_flag = 1;
+	queue_delayed_work(instance->me4600_workqueue,
+			   &instance->ao_control_task, 1);
+
+	if (start_mode == ME_START_MODE_BLOCKING) {	//Wait for start.
+		//Only runing process will interrupt this call. Events are signaled when status change. Extra timeout add for safe reason.
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 (instance->status !=
+						  ao_status_stream_run_wait),
+						 (delay) ? delay +
+						 1 : LONG_MAX);
+
+		if ((instance->status != ao_status_stream_run)
+		    && (instance->status != ao_status_stream_end)) {
+			PDEBUG("Starting stream canceled. %d\n",
+			       instance->status);
+			err = ME_ERRNO_CANCELLED;
+		}
+
+		if (signal_pending(current)) {
+			PERROR("Wait on start of state machine interrupted.\n");
+			instance->status = ao_status_none;
+			ao_stop_immediately(instance);
+			err = ME_ERRNO_SIGNAL;
+		} else if ((delay) && ((jiffies - ref) >= delay)) {
+			if (instance->status != ao_status_stream_run) {
+				if (instance->status == ao_status_stream_end) {
+					PDEBUG("Timeout reached.\n");
+				} else {
+					if ((jiffies - ref) > delay) {
+						PERROR
+						    ("Timeout reached. Not handled by control task!\n");
+					} else {
+						PERROR
+						    ("Timeout reached. Signal come but status is strange: %d\n",
+						     instance->status);
+					}
+					ao_stop_immediately(instance);
+				}
+
+				instance->ao_control_task_flag = 0;
+				cancel_delayed_work(&instance->ao_control_task);
+				instance->status = ao_status_stream_end;
+				err = ME_ERRNO_TIMEOUT;
+			}
+		}
+	}
+
+	ME_SUBDEVICE_EXIT;
+	return err;
+}
+
+static int me4600_ao_io_stream_status(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int wait,
+				      int *status, int *values, int flags)
+{
+	me4600_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (!instance->fifo) {
+		PERROR("Not a streaming ao.\n");
+		return ME_ERRNO_NOT_SUPPORTED;
+	}
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if ((wait != ME_WAIT_NONE) && (wait != ME_WAIT_IDLE)) {
+		PERROR("Invalid wait argument specified.\n");
+		*status = ME_STATUS_INVALID;
+		return ME_ERRNO_INVALID_WAIT;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	switch (instance->status) {
+	case ao_status_single_configured:
+	case ao_status_single_end:
+	case ao_status_stream_configured:
+	case ao_status_stream_end:
+	case ao_status_stream_fifo_error:
+	case ao_status_stream_buffer_error:
+	case ao_status_stream_error:
+		*status = ME_STATUS_IDLE;
+		break;
+
+	case ao_status_single_run_wait:
+	case ao_status_single_run:
+	case ao_status_single_end_wait:
+	case ao_status_stream_run_wait:
+	case ao_status_stream_run:
+	case ao_status_stream_end_wait:
+		*status = ME_STATUS_BUSY;
+		break;
+
+	case ao_status_none:
+	default:
+		*status =
+		    (inl(instance->status_reg) & ME4600_AO_STATUS_BIT_FSM) ?
+		    ME_STATUS_BUSY : ME_STATUS_IDLE;
+		break;
+	}
+
+	if ((wait == ME_WAIT_IDLE) && (*status == ME_STATUS_BUSY)) {
+		//Only runing process will interrupt this call. Events are signaled when status change. Extra timeout add for safe reason.
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 ((instance->status !=
+						   ao_status_single_run_wait)
+						  && (instance->status !=
+						      ao_status_single_run)
+						  && (instance->status !=
+						      ao_status_single_end_wait)
+						  && (instance->status !=
+						      ao_status_stream_run_wait)
+						  && (instance->status !=
+						      ao_status_stream_run)
+						  && (instance->status !=
+						      ao_status_stream_end_wait)),
+						 LONG_MAX);
+
+		if (instance->status != ao_status_stream_end) {
+			PDEBUG("Wait for IDLE canceled. %d\n",
+			       instance->status);
+			err = ME_ERRNO_CANCELLED;
+		}
+
+		if (signal_pending(current)) {
+			PERROR("Wait for IDLE interrupted.\n");
+			instance->status = ao_status_none;
+			ao_stop_immediately(instance);
+			err = ME_ERRNO_SIGNAL;
+		}
+
+		*status = ME_STATUS_IDLE;
+	}
+
+	*values = me_circ_buf_space(&instance->circ_buf);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ao_io_stream_stop(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int stop_mode, int flags)
+{				// Stop work and empty buffer and FIFO
+	int err = ME_ERRNO_SUCCESS;
+	me4600_ao_subdevice_t *instance;
+	unsigned long cpu_flags;
+	volatile uint32_t ctrl;
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (flags & ~ME_IO_STREAM_STOP_PRESERVE_BUFFERS) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if ((stop_mode != ME_STOP_MODE_IMMEDIATE)
+	    && (stop_mode != ME_STOP_MODE_LAST_VALUE)) {
+		PERROR("Invalid stop mode specified.\n");
+		return ME_ERRNO_INVALID_STOP_MODE;
+	}
+
+	if (!instance->fifo) {
+		PERROR("Not a streaming ao.\n");
+		return ME_ERRNO_NOT_SUPPORTED;
+	}
+
+	if (instance->status < ao_status_stream_configured) {
+		//There is nothing to stop!
+		PERROR("Subdevice not in streaming mode. %d\n",
+		       instance->status);
+		return ME_ERRNO_PREVIOUS_CONFIG;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	//Mark as stopping. => Software stop.
+	instance->status = ao_status_stream_end_wait;
+
+	if (stop_mode == ME_STOP_MODE_IMMEDIATE) {	//Stopped now!
+		err = ao_stop_immediately(instance);
+	} else if (stop_mode == ME_STOP_MODE_LAST_VALUE) {
+		ctrl = inl(instance->ctrl_reg) & ME4600_AO_CTRL_MODE_MASK;
+		if (ctrl == ME4600_AO_MODE_WRAPAROUND) {	//Hardware wraparound => Hardware stop.
+			spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+			ctrl = inl(instance->ctrl_reg);
+			ctrl |=
+			    ME4600_AO_CTRL_BIT_STOP |
+			    ME4600_AO_CTRL_BIT_RESET_IRQ;
+			ctrl &= ~ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+			outl(ctrl, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   ctrl);
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+		}
+		//Only runing process will interrupt this call. Events are signaled when status change.
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 (instance->status !=
+						  ao_status_stream_end_wait),
+						 LONG_MAX);
+
+		if (instance->status != ao_status_stream_end) {
+			PDEBUG("Stopping stream canceled.\n");
+			err = ME_ERRNO_CANCELLED;
+		}
+
+		if (signal_pending(current)) {
+			PERROR("Stopping stream interrupted.\n");
+			instance->status = ao_status_none;
+			ao_stop_immediately(instance);
+			err = ME_ERRNO_SIGNAL;
+		}
+	}
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	ctrl = inl(instance->ctrl_reg);
+	ctrl |=
+	    ME4600_AO_CTRL_BIT_STOP | ME4600_AO_CTRL_BIT_IMMEDIATE_STOP |
+	    ME4600_AO_CTRL_BIT_RESET_IRQ;
+	ctrl &= ~ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+	if (!flags) {		//Reset FIFO
+		ctrl &= ~ME4600_AO_CTRL_BIT_ENABLE_FIFO;
+	}
+	outl(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl);
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	if (!flags) {		//Reset software buffer
+		instance->circ_buf.head = 0;
+		instance->circ_buf.tail = 0;
+		instance->preloaded_count = 0;
+		instance->data_count = 0;
+	}
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ao_io_stream_write(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int write_mode,
+				     int *values, int *count, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me4600_ao_subdevice_t *instance;
+	unsigned long cpu_flags = 0;
+	uint32_t reg_copy;
+
+	int copied_from_user = 0;
+	int left_to_copy_from_user = *count;
+
+	int copied_values;
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	//Checking arguments
+	if (!instance->fifo) {
+		PERROR("Not a streaming ao.\n");
+		return ME_ERRNO_NOT_SUPPORTED;
+	}
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (*count <= 0) {
+		PERROR("Invalid count of values specified.\n");
+		return ME_ERRNO_INVALID_VALUE_COUNT;
+	}
+
+	if (values == NULL) {
+		PERROR("Invalid address of values specified.\n");
+		return ME_ERRNO_INVALID_POINTER;
+	}
+
+	if ((instance->status == ao_status_none) || (instance->status == ao_status_single_configured)) {	//The device is in single mode.
+		PERROR
+		    ("Subdevice must be preinitialize correctly for streaming.\n");
+		return ME_ERRNO_PREVIOUS_CONFIG;
+	}
+/// @note If no 'pre-load' is used. stream_start() will move data to FIFO.
+	switch (write_mode) {
+	case ME_WRITE_MODE_PRELOAD:
+
+		//Device must be stopped.
+		if ((instance->status != ao_status_stream_configured)
+		    && (instance->status != ao_status_stream_end)) {
+			PERROR
+			    ("Subdevice mustn't be runing when 'pre-load' mode is used.\n");
+			return ME_ERRNO_PREVIOUS_CONFIG;
+		}
+		break;
+	case ME_WRITE_MODE_NONBLOCKING:
+	case ME_WRITE_MODE_BLOCKING:
+		/// @note In blocking mode: When device is not runing and there is not enought space call will blocked up!
+		/// @note Some other thread must empty buffer by starting engine.
+		break;
+
+	default:
+		PERROR("Invalid write mode specified.\n");
+		return ME_ERRNO_INVALID_WRITE_MODE;
+	}
+
+	if (instance->mode & ME4600_AO_WRAP_MODE) {	//Wraparound mode. Device must be stopped.
+		if ((instance->status != ao_status_stream_configured)
+		    && (instance->status != ao_status_stream_end)) {
+			PERROR
+			    ("Subdevice mustn't be runing when 'pre-load' mode is used.\n");
+			return ME_ERRNO_INVALID_WRITE_MODE;
+		}
+	}
+
+	if ((instance->mode == ME4600_AO_HW_WRAP_MODE) && (write_mode != ME_WRITE_MODE_PRELOAD)) {	// hardware wrap_around mode.
+		//This is transparent for user.
+		PDEBUG("Changing write_mode to ME_WRITE_MODE_PRELOAD.\n");
+		write_mode = ME_WRITE_MODE_PRELOAD;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	if (write_mode == ME_WRITE_MODE_PRELOAD) {	//Init enviroment - preload
+		spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+		reg_copy = inl(instance->ctrl_reg);
+		//Check FIFO
+		if (!(reg_copy & ME4600_AO_CTRL_BIT_ENABLE_FIFO)) {	//FIFO not active. Enable it.
+			reg_copy |= ME4600_AO_CTRL_BIT_ENABLE_FIFO;
+			outl(reg_copy, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   reg_copy);
+			instance->preloaded_count = 0;
+		}
+		spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+	}
+
+	while (1) {
+		//Copy to buffer. This step is common for all modes.
+		copied_from_user =
+		    ao_get_data_from_user(instance, left_to_copy_from_user,
+					  values + (*count -
+						    left_to_copy_from_user));
+		left_to_copy_from_user -= copied_from_user;
+
+		reg_copy = inl(instance->status_reg);
+		if ((instance->status == ao_status_stream_run) && !(reg_copy & ME4600_AO_STATUS_BIT_FSM)) {	//BROKEN PIPE! The state machine is stoped but logical status show that should be working.
+			PERROR("Broken pipe in write.\n");
+			err = ME_ERRNO_SUBDEVICE_NOT_RUNNING;
+			break;
+		}
+
+		if ((instance->status == ao_status_stream_run) && (instance->mode == ME4600_AO_CONTINOUS) && (reg_copy & ME4600_AO_STATUS_BIT_HF)) {	//Continous mode runing and data are below half!
+
+			// Block interrupts.
+			spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+			reg_copy = inl(instance->ctrl_reg);
+			//reg_copy &= ~ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+			reg_copy |= ME4600_AO_CTRL_BIT_RESET_IRQ;
+			outl(reg_copy, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   reg_copy);
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+
+			//Fast copy
+			copied_values =
+			    ao_write_data(instance, ME4600_AO_FIFO_COUNT / 2,
+					  0);
+			if (copied_values > 0) {
+				instance->circ_buf.tail += copied_values;
+				instance->circ_buf.tail &=
+				    instance->circ_buf.mask;
+				continue;
+			}
+			// Activate interrupts.
+			spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+			reg_copy = inl(instance->ctrl_reg);
+			//reg_copy |= ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+			reg_copy &= ~ME4600_AO_CTRL_BIT_RESET_IRQ;
+			outl(reg_copy, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   reg_copy);
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+
+			if (copied_values == 0) {	//This was checked and never should happend!
+				PERROR_CRITICAL("COPING FINISH WITH 0!\n");
+			}
+
+			if (copied_values < 0) {	//This was checked and never should happend!
+				PERROR_CRITICAL
+				    ("COPING FINISH WITH AN ERROR!\n");
+				instance->status = ao_status_stream_fifo_error;
+				err = ME_ERRNO_FIFO_BUFFER_OVERFLOW;
+				break;
+			}
+		}
+
+		if (!left_to_copy_from_user) {	//All datas were copied.
+			break;
+		} else {	//Not all datas were copied.
+			if (instance->mode & ME4600_AO_WRAP_MODE) {	//Error too much datas! Wraparound is limited in size!
+				PERROR
+				    ("Too much data for wraparound mode!  Exceeded size of %ld.\n",
+				     ME4600_AO_CIRC_BUF_COUNT - 1);
+				err = ME_ERRNO_RING_BUFFER_OVERFLOW;
+				break;
+			}
+
+			if (write_mode != ME_WRITE_MODE_BLOCKING) {	//Non blocking calls
+				break;
+			}
+
+			wait_event_interruptible(instance->wait_queue,
+						 me_circ_buf_space(&instance->
+								   circ_buf));
+
+			if (signal_pending(current)) {
+				PERROR("Writing interrupted by signal.\n");
+				instance->status = ao_status_none;
+				ao_stop_immediately(instance);
+				err = ME_ERRNO_SIGNAL;
+				break;
+			}
+
+			if (instance->status == ao_status_none) {	//Reset
+				PERROR("Writing interrupted by reset.\n");
+				err = ME_ERRNO_CANCELLED;
+				break;
+			}
+		}
+	}
+
+	if (write_mode == ME_WRITE_MODE_PRELOAD) {	//Copy data to FIFO - preload
+		copied_values =
+		    ao_write_data_pooling(instance, ME4600_AO_FIFO_COUNT,
+					  instance->preloaded_count);
+		instance->preloaded_count += copied_values;
+		instance->data_count += copied_values;
+
+		if ((instance->mode == ME4600_AO_HW_WRAP_MODE)
+		    && (me_circ_buf_values(&instance->circ_buf) >
+			ME4600_AO_FIFO_COUNT)) {
+			PERROR
+			    ("Too much data for hardware wraparound mode! Exceeded size of %d.\n",
+			     ME4600_AO_FIFO_COUNT);
+			err = ME_ERRNO_FIFO_BUFFER_OVERFLOW;
+		}
+	}
+
+	*count = *count - left_to_copy_from_user;
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+static irqreturn_t me4600_ao_isr(int irq, void *dev_id
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2, 6, 19)
+				 , struct pt_regs *regs
+#endif
+    )
+{
+	me4600_ao_subdevice_t *instance = dev_id;
+	uint32_t irq_status;
+	uint32_t ctrl;
+	uint32_t status;
+	int count = 0;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (irq != instance->irq) {
+		PERROR("Incorrect interrupt num: %d.\n", irq);
+		return IRQ_NONE;
+	}
+
+	irq_status = inl(instance->irq_status_reg);
+	if (!(irq_status & (ME4600_IRQ_STATUS_BIT_AO_HF << instance->ao_idx))) {
+		PINFO("%ld Shared interrupt. %s(): ID=%d: status_reg=0x%04X\n",
+		      jiffies, __FUNCTION__, instance->ao_idx, irq_status);
+		return IRQ_NONE;
+	}
+
+	if (!instance->circ_buf.buf) {
+		instance->status = ao_status_stream_error;
+		PERROR_CRITICAL("CIRCULAR BUFFER NOT EXISTS!\n");
+		//Block interrupts. Stop machine.
+		ctrl = inl(instance->ctrl_reg);
+		ctrl &= ~ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+		ctrl |=
+		    ME4600_AO_CTRL_BIT_RESET_IRQ |
+		    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP | ME4600_AO_CTRL_BIT_STOP;
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+
+		//Inform user
+		wake_up_interruptible_all(&instance->wait_queue);
+		return IRQ_HANDLED;
+	}
+
+	status = inl(instance->status_reg);
+	if (!(status & ME4600_AO_STATUS_BIT_FSM)) {	//Too late. Not working! END? BROKEN PIPE?
+		PDEBUG("Interrupt come but ISM is not working!\n");
+		//Block interrupts. Stop machine.
+		ctrl = inl(instance->ctrl_reg);
+		ctrl &= ~ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+		ctrl |=
+		    ME4600_AO_CTRL_BIT_RESET_IRQ | ME4600_AO_CTRL_BIT_STOP |
+		    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+
+		return IRQ_HANDLED;
+	}
+	//General procedure. Process more datas.
+
+#ifdef MEDEBUG_DEBUG
+	if (!me_circ_buf_values(&instance->circ_buf)) {	//Buffer is empty!
+		PDEBUG("Circular buffer empty!\n");
+	}
+#endif
+
+	//Check FIFO
+	if (status & ME4600_AO_STATUS_BIT_HF) {	//OK less than half
+
+		//Block interrupts
+		ctrl = inl(instance->ctrl_reg);
+		ctrl &= ~ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+		ctrl |= ME4600_AO_CTRL_BIT_RESET_IRQ;
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+
+		do {
+			//Calculate how many should be copied.
+			count =
+			    (instance->stop_data_count) ? instance->
+			    stop_data_count -
+			    instance->data_count : ME4600_AO_FIFO_COUNT / 2;
+			if (ME4600_AO_FIFO_COUNT / 2 < count) {
+				count = ME4600_AO_FIFO_COUNT / 2;
+			}
+			//Copy data
+			if (instance->mode == ME4600_AO_CONTINOUS) {	//Continous
+				count = ao_write_data(instance, count, 0);
+				if (count > 0) {
+					instance->circ_buf.tail += count;
+					instance->circ_buf.tail &=
+					    instance->circ_buf.mask;
+					instance->data_count += count;
+
+					if ((instance->status == ao_status_stream_end_wait) && !me_circ_buf_values(&instance->circ_buf)) {	//Stoping. Whole buffer was copied.
+						break;
+					}
+				}
+			} else if ((instance->mode == ME4600_AO_SW_WRAP_MODE) && ((ctrl & ME4600_AO_CTRL_MODE_MASK) == ME4600_AO_MODE_CONTINUOUS)) {	//Wraparound (software)
+				if (instance->status == ao_status_stream_end_wait) {	//We stoping => Copy to the end of the buffer.
+					count =
+					    ao_write_data(instance, count, 0);
+				} else {	//Copy in wraparound mode.
+					count =
+					    ao_write_data_wraparound(instance,
+								     count,
+								     instance->
+								     preloaded_count);
+				}
+
+				if (count > 0) {
+					instance->data_count += count;
+					instance->preloaded_count += count;
+					instance->preloaded_count %=
+					    me_circ_buf_values(&instance->
+							       circ_buf);
+
+					if ((instance->status == ao_status_stream_end_wait) && !instance->preloaded_count) {	//Stoping. Whole buffer was copied.
+						break;
+					}
+				}
+			}
+
+			if ((count <= 0) || (instance->stop_data_count && (instance->stop_data_count <= instance->data_count))) {	//End of work.
+				break;
+			}
+		}		//Repeat if still is under half fifo
+		while ((status =
+			inl(instance->status_reg)) & ME4600_AO_STATUS_BIT_HF);
+
+		//Unblock interrupts
+		ctrl = inl(instance->ctrl_reg);
+		if (count >= 0) {	//Copy was successful.
+			if (instance->stop_data_count && (instance->stop_data_count <= instance->data_count)) {	//Finishing work. No more interrupts.
+				PDEBUG("Finishing work. Interrupt disabled.\n");
+				instance->status = ao_status_stream_end_wait;
+			} else if (count > 0) {	//Normal work. Enable interrupt.
+				PDEBUG("Normal work. Enable interrupt.\n");
+				ctrl &= ~ME4600_AO_CTRL_BIT_RESET_IRQ;
+				ctrl |= ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+			} else {	//Normal work but there are no more data in buffer. Interrupt active but blocked. stream_write() will unblock it.
+				PDEBUG
+				    ("No data in software buffer. Interrupt blocked.\n");
+				ctrl |= ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+			}
+		} else {	//Error during copy.
+			instance->status = ao_status_stream_fifo_error;
+		}
+
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+	} else {		//?? more than half
+		PDEBUG
+		    ("Interrupt come but FIFO more than half full! Reset interrupt.\n");
+		//Reset pending interrupt
+		ctrl = inl(instance->ctrl_reg);
+		ctrl |= ME4600_AO_CTRL_BIT_RESET_IRQ;
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+		ctrl &= ~ME4600_AO_CTRL_BIT_RESET_IRQ;
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+	}
+
+	PINFO("ISR: Buffer count: %d.(T:%d H:%d)\n",
+	      me_circ_buf_values(&instance->circ_buf), instance->circ_buf.tail,
+	      instance->circ_buf.head);
+	PINFO("ISR: Stop count: %d.\n", instance->stop_count);
+	PINFO("ISR: Stop data count: %d.\n", instance->stop_data_count);
+	PINFO("ISR: Data count: %d.\n", instance->data_count);
+
+	//Inform user
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	return IRQ_HANDLED;
+}
+
+static void me4600_ao_destructor(struct me_subdevice *subdevice)
+{
+	me4600_ao_subdevice_t *instance;
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	instance->ao_control_task_flag = 0;
+
+	// Reset subdevice to asure clean exit.
+	me4600_ao_io_reset_subdevice(subdevice, NULL,
+				     ME_IO_RESET_SUBDEVICE_NO_FLAGS);
+
+	// Remove any tasks from work queue. This is paranoic because it was done allready in reset().
+	if (!cancel_delayed_work(&instance->ao_control_task)) {	//Wait 2 ticks to be sure that control task is removed from queue.
+		set_current_state(TASK_INTERRUPTIBLE);
+		schedule_timeout(2);
+	}
+
+	if (instance->fifo) {
+		if (instance->irq) {
+			free_irq(instance->irq, instance);
+			instance->irq = 0;
+		}
+
+		if (instance->circ_buf.buf) {
+			free_pages((unsigned long)instance->circ_buf.buf,
+				   ME4600_AO_CIRC_BUF_SIZE_ORDER);
+		}
+		instance->circ_buf.buf = NULL;
+	}
+
+	me_subdevice_deinit(&instance->base);
+	kfree(instance);
+}
+
+me4600_ao_subdevice_t *me4600_ao_constructor(uint32_t reg_base,
+					     spinlock_t * preload_reg_lock,
+					     uint32_t * preload_flags,
+					     int ao_idx,
+					     int fifo,
+					     int irq,
+					     struct workqueue_struct *me4600_wq)
+{
+	me4600_ao_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed. idx=%d\n", ao_idx);
+
+	// Allocate memory for subdevice instance.
+	subdevice = kmalloc(sizeof(me4600_ao_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me4600_ao_subdevice_t));
+
+	// Initialize subdevice base class.
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	subdevice->preload_reg_lock = preload_reg_lock;
+	subdevice->preload_flags = preload_flags;
+
+	// Store analog output index.
+	subdevice->ao_idx = ao_idx;
+
+	// Store if analog output has fifo.
+	subdevice->fifo = (ao_idx < fifo) ? 1 : 0;
+
+	if (subdevice->fifo) {	// Allocate and initialize circular buffer.
+		subdevice->circ_buf.mask = ME4600_AO_CIRC_BUF_COUNT - 1;
+
+		subdevice->circ_buf.buf =
+		    (void *)__get_free_pages(GFP_KERNEL,
+					     ME4600_AO_CIRC_BUF_SIZE_ORDER);
+		PDEBUG("circ_buf = %p size=%ld\n", subdevice->circ_buf.buf,
+		       ME4600_AO_CIRC_BUF_SIZE);
+
+		if (!subdevice->circ_buf.buf) {
+			PERROR
+			    ("Cannot initialize subdevice base class instance.\n");
+			kfree(subdevice);
+			return NULL;
+		}
+
+		memset(subdevice->circ_buf.buf, 0, ME4600_AO_CIRC_BUF_SIZE);
+	} else {		// No FIFO.
+		subdevice->circ_buf.mask = 0;
+		subdevice->circ_buf.buf = NULL;
+	}
+
+	subdevice->circ_buf.head = 0;
+	subdevice->circ_buf.tail = 0;
+
+	subdevice->status = ao_status_none;
+	subdevice->ao_control_task_flag = 0;
+	subdevice->timeout.delay = 0;
+	subdevice->timeout.start_time = jiffies;
+
+	// Initialize wait queue.
+	init_waitqueue_head(&subdevice->wait_queue);
+
+	// Initialize single value to 0V.
+	subdevice->single_value = 0x8000;
+	subdevice->single_value_in_fifo = 0x8000;
+
+	// Register interrupt service routine.
+	if (subdevice->fifo) {
+		subdevice->irq = irq;
+		if (request_irq(subdevice->irq, me4600_ao_isr,
+#ifdef IRQF_DISABLED
+				IRQF_DISABLED | IRQF_SHARED,
+#else
+				SA_INTERRUPT | SA_SHIRQ,
+#endif
+				ME4600_NAME, subdevice)) {
+			PERROR("Cannot get interrupt line.\n");
+			PDEBUG("free circ_buf = %p size=%d",
+			       subdevice->circ_buf.buf,
+			       PAGE_SHIFT << ME4600_AO_CIRC_BUF_SIZE_ORDER);
+			free_pages((unsigned long)subdevice->circ_buf.buf,
+				   ME4600_AO_CIRC_BUF_SIZE_ORDER);
+			me_subdevice_deinit((me_subdevice_t *) subdevice);
+			kfree(subdevice);
+			return NULL;
+		}
+		PINFO("Registered irq=%d.\n", subdevice->irq);
+	} else {
+		subdevice->irq = 0;
+	}
+
+	// Initialize registers.
+	subdevice->irq_status_reg = reg_base + ME4600_IRQ_STATUS_REG;
+	subdevice->preload_reg = reg_base + ME4600_AO_SYNC_REG;
+	if (ao_idx == 0) {
+		subdevice->ctrl_reg = reg_base + ME4600_AO_00_CTRL_REG;
+		subdevice->status_reg = reg_base + ME4600_AO_00_STATUS_REG;
+		subdevice->fifo_reg = reg_base + ME4600_AO_00_FIFO_REG;
+		subdevice->single_reg = reg_base + ME4600_AO_00_SINGLE_REG;
+		subdevice->timer_reg = reg_base + ME4600_AO_00_TIMER_REG;
+		subdevice->reg_base = reg_base;
+		subdevice->bitpattern = 0;
+	} else if (ao_idx == 1) {
+		subdevice->ctrl_reg = reg_base + ME4600_AO_01_CTRL_REG;
+		subdevice->status_reg = reg_base + ME4600_AO_01_STATUS_REG;
+		subdevice->fifo_reg = reg_base + ME4600_AO_01_FIFO_REG;
+		subdevice->single_reg = reg_base + ME4600_AO_01_SINGLE_REG;
+		subdevice->timer_reg = reg_base + ME4600_AO_01_TIMER_REG;
+		subdevice->reg_base = reg_base;
+		subdevice->bitpattern = 0;
+	} else if (ao_idx == 2) {
+		subdevice->ctrl_reg = reg_base + ME4600_AO_02_CTRL_REG;
+		subdevice->status_reg = reg_base + ME4600_AO_02_STATUS_REG;
+		subdevice->fifo_reg = reg_base + ME4600_AO_02_FIFO_REG;
+		subdevice->single_reg = reg_base + ME4600_AO_02_SINGLE_REG;
+		subdevice->timer_reg = reg_base + ME4600_AO_02_TIMER_REG;
+		subdevice->reg_base = reg_base;
+		subdevice->bitpattern = 0;
+	} else if (ao_idx == 3) {
+		subdevice->ctrl_reg = reg_base + ME4600_AO_03_CTRL_REG;
+		subdevice->status_reg = reg_base + ME4600_AO_03_STATUS_REG;
+		subdevice->fifo_reg = reg_base + ME4600_AO_03_FIFO_REG;
+		subdevice->single_reg = reg_base + ME4600_AO_03_SINGLE_REG;
+		subdevice->timer_reg = reg_base + ME4600_AO_03_TIMER_REG;
+		subdevice->reg_base = reg_base;
+		subdevice->bitpattern = 1;
+	} else {
+		PERROR_CRITICAL("WRONG SUBDEVICE idx=%d!", ao_idx);
+		me_subdevice_deinit((me_subdevice_t *) subdevice);
+		if (subdevice->fifo) {
+			free_pages((unsigned long)subdevice->circ_buf.buf,
+				   ME4600_AO_CIRC_BUF_SIZE_ORDER);
+		}
+		subdevice->circ_buf.buf = NULL;
+		kfree(subdevice);
+		return NULL;
+	}
+
+	// Override base class methods.
+	subdevice->base.me_subdevice_destructor = me4600_ao_destructor;
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me4600_ao_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me4600_ao_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me4600_ao_io_single_read;
+	subdevice->base.me_subdevice_io_single_write =
+	    me4600_ao_io_single_write;
+	subdevice->base.me_subdevice_io_stream_config =
+	    me4600_ao_io_stream_config;
+	subdevice->base.me_subdevice_io_stream_new_values =
+	    me4600_ao_io_stream_new_values;
+	subdevice->base.me_subdevice_io_stream_write =
+	    me4600_ao_io_stream_write;
+	subdevice->base.me_subdevice_io_stream_start =
+	    me4600_ao_io_stream_start;
+	subdevice->base.me_subdevice_io_stream_status =
+	    me4600_ao_io_stream_status;
+	subdevice->base.me_subdevice_io_stream_stop = me4600_ao_io_stream_stop;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me4600_ao_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me4600_ao_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me4600_ao_query_subdevice_caps;
+	subdevice->base.me_subdevice_query_subdevice_caps_args =
+	    me4600_ao_query_subdevice_caps_args;
+	subdevice->base.me_subdevice_query_range_by_min_max =
+	    me4600_ao_query_range_by_min_max;
+	subdevice->base.me_subdevice_query_number_ranges =
+	    me4600_ao_query_number_ranges;
+	subdevice->base.me_subdevice_query_range_info =
+	    me4600_ao_query_range_info;
+	subdevice->base.me_subdevice_query_timer = me4600_ao_query_timer;
+
+	// Prepare work queue
+	subdevice->me4600_workqueue = me4600_wq;
+
+/* workqueue API changed in kernel 2.6.20 */
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20) )
+	INIT_WORK(&subdevice->ao_control_task, me4600_ao_work_control_task,
+		  (void *)subdevice);
+#else
+	INIT_DELAYED_WORK(&subdevice->ao_control_task,
+			  me4600_ao_work_control_task);
+#endif
+
+	if (subdevice->fifo) {	// Set speed for single operations.
+		outl(ME4600_AO_MIN_CHAN_TICKS - 1, subdevice->timer_reg);
+		subdevice->hardware_stop_delay = HZ / 10;	//100ms
+	}
+
+	return subdevice;
+}
+
+/** @brief Stop presentation. Preserve FIFOs.
+*
+* @param instance The subdevice instance (pointer).
+*/
+int inline ao_stop_immediately(me4600_ao_subdevice_t * instance)
+{
+	unsigned long cpu_flags;
+	uint32_t ctrl;
+	int timeout;
+	int i;
+
+	timeout =
+	    (instance->hardware_stop_delay >
+	     (HZ / 10)) ? instance->hardware_stop_delay : HZ / 10;
+	for (i = 0; i <= timeout; i++) {
+		spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+		// Stop all actions. No conditions! Block interrupts. Leave FIFO untouched!
+		ctrl = inl(instance->ctrl_reg);
+		ctrl |=
+		    ME4600_AO_CTRL_BIT_STOP | ME4600_AO_CTRL_BIT_IMMEDIATE_STOP
+		    | ME4600_AO_CTRL_BIT_RESET_IRQ;
+		ctrl &=
+		    ~(ME4600_AO_CTRL_BIT_ENABLE_IRQ |
+		      ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG);
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+		spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+		if (!(inl(instance->status_reg) & ME4600_AO_STATUS_BIT_FSM)) {	// Exit.
+			break;
+		}
+		//Still working!
+		set_current_state(TASK_INTERRUPTIBLE);
+		schedule_timeout(1);
+	}
+
+	if (i > timeout) {
+		PERROR_CRITICAL("FSM IS BUSY!\n");
+		return ME_ERRNO_INTERNAL;
+	}
+	return ME_ERRNO_SUCCESS;
+}
+
+/** @brief Copy data from circular buffer to fifo (fast) in wraparound.
+* @note This is time critical function. Checking is done at begining and end only.
+* @note The is not reasonable way to check how many walues was in FIFO at begining. The count must be managed externaly.
+*
+* @param instance The subdevice instance (pointer).
+* @param count Maximum number of copied data.
+* @param start_pos Position of the firs value in buffer.
+*
+* @return On success: Number of copied data.
+* @return On error/success: 0.	No datas were copied => no data in buffer.
+* @return On error: -ME_ERRNO_FIFO_BUFFER_OVERFLOW.
+*/
+int inline ao_write_data_wraparound(me4600_ao_subdevice_t * instance, int count,
+				    int start_pos)
+{				/// @note This is time critical function!
+	uint32_t status;
+	uint32_t value;
+	int pos =
+	    (instance->circ_buf.tail + start_pos) & instance->circ_buf.mask;
+	int local_count = count;
+	int i = 1;
+
+	if (count <= 0) {	//Wrong count!
+		return 0;
+	}
+
+	while (i < local_count) {
+		//Get value from buffer
+		value = *(instance->circ_buf.buf + pos);
+		//Prepare it
+		if (instance->ao_idx & 0x1) {
+			value <<= 16;
+		}
+		//Put value to FIFO
+		outl(value, instance->fifo_reg);
+		//PDEBUG_REG("idx=%d fifo_reg outl(0x%lX+0x%lX)=0x%x\n", instance->ao_idx, instance->reg_base, instance->fifo_reg - instance->reg_base, value);
+
+		pos++;
+		pos &= instance->circ_buf.mask;
+		if (pos == instance->circ_buf.head) {
+			pos = instance->circ_buf.tail;
+		}
+		i++;
+	}
+
+	status = inl(instance->status_reg);
+	if (!(status & ME4600_AO_STATUS_BIT_FF)) {	//FIFO is full before all datas were copied!
+		PERROR("FIFO was full before all datas were copied! idx=%d\n",
+		       instance->ao_idx);
+		return -ME_ERRNO_FIFO_BUFFER_OVERFLOW;
+	} else {		//Add last value
+		value = *(instance->circ_buf.buf + pos);
+		if (instance->ao_idx & 0x1) {
+			value <<= 16;
+		}
+		//Put value to FIFO
+		outl(value, instance->fifo_reg);
+		//PDEBUG_REG("idx=%d fifo_reg outl(0x%lX+0x%lX)=0x%x\n", instance->ao_idx, instance->reg_base, instance->fifo_reg - instance->reg_base, value);
+	}
+
+	PINFO("WRAPAROUND LOADED %d values. idx=%d\n", local_count,
+	      instance->ao_idx);
+	return local_count;
+}
+
+/** @brief Copy data from software buffer to fifo (fast).
+* @note This is time critical function. Checking is done at begining and end only.
+* @note The is not reasonable way to check how many walues was in FIFO at begining. The count must be managed externaly.
+*
+* @param instance The subdevice instance (pointer).
+* @param count Maximum number of copied data.
+* @param start_pos Position of the firs value in buffer.
+*
+* @return On success: Number of copied data.
+* @return On error/success: 0.	No datas were copied => no data in buffer.
+* @return On error: -ME_ERRNO_FIFO_BUFFER_OVERFLOW.
+*/
+int inline ao_write_data(me4600_ao_subdevice_t * instance, int count,
+			 int start_pos)
+{				/// @note This is time critical function!
+	uint32_t status;
+	uint32_t value;
+	int pos =
+	    (instance->circ_buf.tail + start_pos) & instance->circ_buf.mask;
+	int local_count = count;
+	int max_count;
+	int i = 1;
+
+	if (count <= 0) {	//Wrong count!
+		return 0;
+	}
+
+	max_count = me_circ_buf_values(&instance->circ_buf) - start_pos;
+	if (max_count <= 0) {	//No data to copy!
+		return 0;
+	}
+
+	if (max_count < count) {
+		local_count = max_count;
+	}
+
+	while (i < local_count) {
+		//Get value from buffer
+		value = *(instance->circ_buf.buf + pos);
+		//Prepare it
+		if (instance->ao_idx & 0x1) {
+			value <<= 16;
+		}
+		//Put value to FIFO
+		outl(value, instance->fifo_reg);
+		//PDEBUG_REG("idx=%d fifo_reg outl(0x%lX+0x%lX)=0x%x\n", instance->ao_idx, instance->reg_base, instance->fifo_reg - instance->reg_base, value);
+
+		pos++;
+		pos &= instance->circ_buf.mask;
+		i++;
+	}
+
+	status = inl(instance->status_reg);
+	if (!(status & ME4600_AO_STATUS_BIT_FF)) {	//FIFO is full before all datas were copied!
+		PERROR("FIFO was full before all datas were copied! idx=%d\n",
+		       instance->ao_idx);
+		return -ME_ERRNO_FIFO_BUFFER_OVERFLOW;
+	} else {		//Add last value
+		value = *(instance->circ_buf.buf + pos);
+		if (instance->ao_idx & 0x1) {
+			value <<= 16;
+		}
+		//Put value to FIFO
+		outl(value, instance->fifo_reg);
+		//PDEBUG_REG("idx=%d fifo_reg outl(0x%lX+0x%lX)=0x%x\n", instance->ao_idx, instance->reg_base, instance->fifo_reg - instance->reg_base, value);
+	}
+
+	PINFO("FAST LOADED %d values. idx=%d\n", local_count, instance->ao_idx);
+	return local_count;
+}
+
+/** @brief Copy data from software buffer to fifo (slow).
+* @note This is slow function that copy all data from buffer to FIFO with full control.
+*
+* @param instance The subdevice instance (pointer).
+* @param count Maximum number of copied data.
+* @param start_pos Position of the firs value in buffer.
+*
+* @return On success: Number of copied values.
+* @return On error/success: 0.	FIFO was full at begining.
+* @return On error: -ME_ERRNO_RING_BUFFER_UNDEFFLOW.
+*/
+int inline ao_write_data_pooling(me4600_ao_subdevice_t * instance, int count,
+				 int start_pos)
+{				/// @note This is slow function!
+	uint32_t status;
+	uint32_t value;
+	int pos =
+	    (instance->circ_buf.tail + start_pos) & instance->circ_buf.mask;
+	int local_count = count;
+	int i;
+	int max_count;
+
+	if (count <= 0) {	//Wrong count!
+		PERROR("SLOW LOADED: Wrong count! idx=%d\n", instance->ao_idx);
+		return 0;
+	}
+
+	max_count = me_circ_buf_values(&instance->circ_buf) - start_pos;
+	if (max_count <= 0) {	//No data to copy!
+		PERROR("SLOW LOADED: No data to copy! idx=%d\n",
+		       instance->ao_idx);
+		return 0;
+	}
+
+	if (max_count < count) {
+		local_count = max_count;
+	}
+
+	for (i = 0; i < local_count; i++) {
+		status = inl(instance->status_reg);
+		if (!(status & ME4600_AO_STATUS_BIT_FF)) {	//FIFO is full!
+			return i;
+		}
+		//Get value from buffer
+		value = *(instance->circ_buf.buf + pos);
+		//Prepare it
+		if (instance->ao_idx & 0x1) {
+			value <<= 16;
+		}
+		//Put value to FIFO
+		outl(value, instance->fifo_reg);
+		//PDEBUG_REG("idx=%d fifo_reg outl(0x%lX+0x%lX)=0x%x\n", instance->ao_idx, instance->reg_base, instance->fifo_reg - instance->reg_base, value);
+
+		pos++;
+		pos &= instance->circ_buf.mask;
+	}
+
+	PINFO("SLOW LOADED %d values. idx=%d\n", local_count, instance->ao_idx);
+	return local_count;
+}
+
+/** @brief Copy data from user space to circular buffer.
+* @param instance The subdevice instance (pointer).
+* @param count Number of datas in user space.
+* @param user_values Buffer's pointer.
+*
+* @return On success: Number of copied values.
+* @return On error: -ME_ERRNO_INTERNAL.
+*/
+int inline ao_get_data_from_user(me4600_ao_subdevice_t * instance, int count,
+				 int *user_values)
+{
+	int i, err;
+	int empty_space;
+	int copied;
+	int value;
+
+	empty_space = me_circ_buf_space(&instance->circ_buf);
+	//We have only this space free.
+	copied = (count < empty_space) ? count : empty_space;
+	for (i = 0; i < copied; i++) {	//Copy from user to buffer
+		if ((err = get_user(value, (int *)(user_values + i)))) {
+			PERROR
+			    ("BUFFER LOADED: get_user(0x%p) return an error: %d. idx=%d\n",
+			     user_values + i, err, instance->ao_idx);
+			return -ME_ERRNO_INTERNAL;
+		}
+		/// @note The analog output in me4600 series has size of 16 bits.
+		*(instance->circ_buf.buf + instance->circ_buf.head) =
+		    (uint16_t) value;
+		instance->circ_buf.head++;
+		instance->circ_buf.head &= instance->circ_buf.mask;
+	}
+
+	PINFO("BUFFER LOADED %d values. idx=%d\n", copied, instance->ao_idx);
+	return copied;
+}
+
+/** @brief Checking actual hardware and logical state.
+* @param instance The subdevice instance (pointer).
+*/
+static void me4600_ao_work_control_task(
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+					       void *subdevice
+#else
+					       struct work_struct *work
+#endif
+    )
+{
+	me4600_ao_subdevice_t *instance;
+	unsigned long cpu_flags = 0;
+	uint32_t status;
+	uint32_t ctrl;
+	uint32_t synch;
+	int reschedule = 0;
+	int signaling = 0;
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+	instance = (me4600_ao_subdevice_t *) subdevice;
+#else
+	instance =
+	    container_of((void *)work, me4600_ao_subdevice_t, ao_control_task);
+#endif
+	PINFO("<%s: %ld> executed. idx=%d\n", __FUNCTION__, jiffies,
+	      instance->ao_idx);
+
+	status = inl(instance->status_reg);
+	PDEBUG_REG("status_reg inl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->status_reg - instance->reg_base, status);
+
+	switch (instance->status) {	// Checking actual mode.
+
+		// Not configured for work.
+	case ao_status_none:
+		break;
+
+		//This are stable modes. No need to do anything. (?)
+	case ao_status_single_configured:
+	case ao_status_stream_configured:
+	case ao_status_stream_fifo_error:
+	case ao_status_stream_buffer_error:
+	case ao_status_stream_error:
+		PERROR("Shouldn't be running!.\n");
+		break;
+
+	case ao_status_stream_end:
+		if (!instance->fifo) {
+			PERROR_CRITICAL
+			    ("Streaming on single device! This feature is not implemented in this version!\n");
+			instance->status = ao_status_stream_error;
+			// Signal the end.
+			signaling = 1;
+			break;
+		}
+	case ao_status_single_end:
+		if (status & ME4600_AO_STATUS_BIT_FSM) {	// State machine is working but the status is set to end. Force stop.
+
+			// Wait for stop.
+			reschedule = 1;
+		}
+
+		spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+		// Stop all actions. No conditions! Block interrupts and trigger. Leave FIFO untouched!
+		ctrl = inl(instance->ctrl_reg);
+		ctrl |=
+		    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP | ME4600_AO_CTRL_BIT_STOP
+		    | ME4600_AO_CTRL_BIT_RESET_IRQ;
+		ctrl &=
+		    ~(ME4600_AO_CTRL_BIT_ENABLE_IRQ |
+		      ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG);
+		ctrl &=
+		    ~(ME4600_AO_CTRL_BIT_EX_TRIG_EDGE |
+		      ME4600_AO_CTRL_BIT_EX_TRIG_EDGE_BOTH);
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+		spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+		break;
+
+		// Single modes
+	case ao_status_single_run_wait:
+	case ao_status_single_run:
+	case ao_status_single_end_wait:
+
+		if (!(status & ME4600_AO_STATUS_BIT_FSM)) {	// State machine is not working.
+			if (((instance->fifo)
+			     && (!(status & ME4600_AO_STATUS_BIT_EF)))
+			    || (!(instance->fifo))) {	// Single is in end state.
+				PDEBUG("Single call has been complited.\n");
+
+				// Set correct value for single_read();
+				instance->single_value =
+				    instance->single_value_in_fifo;
+
+				// Set status as 'ao_status_single_end'
+				instance->status = ao_status_single_end;
+
+				// Signal the end.
+				signaling = 1;
+				// Wait for stop ISM.
+				reschedule = 1;
+
+				break;
+			}
+		}
+		// Check timeout.
+		if ((instance->timeout.delay) && ((jiffies - instance->timeout.start_time) >= instance->timeout.delay)) {	// Timeout
+			PDEBUG("Timeout reached.\n");
+			// Stop all actions. No conditions! Block interrupts and trigger. Leave FIFO untouched!
+			spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+			ctrl = inl(instance->ctrl_reg);
+			ctrl |=
+			    ME4600_AO_CTRL_BIT_STOP |
+			    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP |
+			    ME4600_AO_CTRL_BIT_RESET_IRQ;
+			ctrl &=
+			    ~(ME4600_AO_CTRL_BIT_ENABLE_IRQ |
+			      ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG);
+			/// Fix for timeout error.
+			ctrl &=
+			    ~(ME4600_AO_CTRL_BIT_EX_TRIG_EDGE |
+			      ME4600_AO_CTRL_BIT_EX_TRIG_EDGE_BOTH);
+			if (instance->fifo) {	//Disabling FIFO
+				ctrl &= ~ME4600_AO_CTRL_BIT_ENABLE_FIFO;
+			}
+			outl(ctrl, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   ctrl);
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+
+			spin_lock(instance->preload_reg_lock);
+			//Remove from synchronous start. Block triggering from this output.
+			synch = inl(instance->preload_reg);
+			synch &=
+			    ~((ME4600_AO_SYNC_HOLD | ME4600_AO_SYNC_EXT_TRIG) <<
+			      instance->ao_idx);
+			if (!(instance->fifo)) {	// No FIFO - set to single safe mode
+				synch |=
+				    ME4600_AO_SYNC_HOLD << instance->ao_idx;
+			}
+			outl(synch, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   synch);
+			spin_unlock(instance->preload_reg_lock);
+
+			if (!(instance->fifo)) {	// No FIFO
+				// Restore old settings.
+				PDEBUG("Write old value back to register.\n");
+				outl(instance->single_value,
+				     instance->single_reg);
+				PDEBUG_REG
+				    ("single_reg outl(0x%lX+0x%lX)=0x%x\n",
+				     instance->reg_base,
+				     instance->single_reg - instance->reg_base,
+				     instance->single_value);
+			}
+			// Set correct value for single_read();
+			instance->single_value_in_fifo = instance->single_value;
+
+			instance->status = ao_status_single_end;
+
+			// Signal the end.
+			signaling = 1;
+		}
+		// Wait for stop.
+		reschedule = 1;
+		break;
+
+		// Stream modes
+	case ao_status_stream_run_wait:
+		if (!instance->fifo) {
+			PERROR_CRITICAL
+			    ("Streaming on single device! This feature is not implemented in this version!\n");
+			instance->status = ao_status_stream_error;
+			// Signal the end.
+			signaling = 1;
+			break;
+		}
+
+		if (status & ME4600_AO_STATUS_BIT_FSM) {	// State machine is working. Waiting for start finish.
+			instance->status = ao_status_stream_run;
+
+			// Signal end of this step
+			signaling = 1;
+		} else {	// State machine is not working.
+			if (!(status & ME4600_AO_STATUS_BIT_EF)) {	// FIFO is empty. Procedure has started and finish already!
+				instance->status = ao_status_stream_end;
+
+				// Signal the end.
+				signaling = 1;
+				// Wait for stop.
+				reschedule = 1;
+				break;
+			}
+		}
+
+		// Check timeout.
+		if ((instance->timeout.delay) && ((jiffies - instance->timeout.start_time) >= instance->timeout.delay)) {	// Timeout
+			PDEBUG("Timeout reached.\n");
+			// Stop all actions. No conditions! Block interrupts. Leave FIFO untouched!
+			spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+			ctrl = inl(instance->ctrl_reg);
+			ctrl |=
+			    ME4600_AO_CTRL_BIT_STOP |
+			    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP |
+			    ME4600_AO_CTRL_BIT_RESET_IRQ;
+			ctrl &=
+			    ~(ME4600_AO_CTRL_BIT_ENABLE_IRQ |
+			      ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG);
+			outl(ctrl, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   ctrl);
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+			spin_lock(instance->preload_reg_lock);
+			//Remove from synchronous start. Block triggering from this output.
+			synch = inl(instance->preload_reg);
+			synch &=
+			    ~((ME4600_AO_SYNC_HOLD | ME4600_AO_SYNC_EXT_TRIG) <<
+			      instance->ao_idx);
+			outl(synch, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   synch);
+			spin_unlock(instance->preload_reg_lock);
+
+			instance->status = ao_status_stream_end;
+
+			// Signal the end.
+			signaling = 1;
+		}
+		// Wait for stop.
+		reschedule = 1;
+		break;
+
+	case ao_status_stream_run:
+		if (!instance->fifo) {
+			PERROR_CRITICAL
+			    ("Streaming on single device! This feature is not implemented in this version!\n");
+			instance->status = ao_status_stream_error;
+			// Signal the end.
+			signaling = 1;
+			break;
+		}
+
+		if (!(status & ME4600_AO_STATUS_BIT_FSM)) {	// State machine is not working. This is an error.
+			// BROKEN PIPE!
+			if (!(status & ME4600_AO_STATUS_BIT_EF)) {	// FIFO is empty.
+				if (me_circ_buf_values(&instance->circ_buf)) {	// Software buffer is not empty.
+					if (instance->stop_data_count && (instance->stop_data_count <= instance->data_count)) {	//Finishing work. Requed data shown.
+						PDEBUG
+						    ("ISM stoped. No data in FIFO. Buffer is not empty.\n");
+						instance->status =
+						    ao_status_stream_end;
+					} else {
+						PERROR
+						    ("Output stream has been broken. ISM stoped. No data in FIFO. Buffer is not empty.\n");
+						instance->status =
+						    ao_status_stream_buffer_error;
+					}
+				} else {	// Software buffer is empty.
+					PDEBUG
+					    ("ISM stoped. No data in FIFO. Buffer is empty.\n");
+					instance->status = ao_status_stream_end;
+				}
+			} else {	// There are still datas in FIFO.
+				if (me_circ_buf_values(&instance->circ_buf)) {	// Software buffer is not empty.
+					PERROR
+					    ("Output stream has been broken. ISM stoped but some data in FIFO and buffer.\n");
+				} else {	// Software buffer is empty.
+					PERROR
+					    ("Output stream has been broken. ISM stoped but some data in FIFO. Buffer is empty.\n");
+				}
+				instance->status = ao_status_stream_fifo_error;
+
+			}
+
+			// Signal the failure.
+			signaling = 1;
+			break;
+		}
+		// Wait for stop.
+		reschedule = 1;
+		break;
+
+	case ao_status_stream_end_wait:
+		if (!instance->fifo) {
+			PERROR_CRITICAL
+			    ("Streaming on single device! This feature is not implemented in this version!\n");
+			instance->status = ao_status_stream_error;
+			// Signal the end.
+			signaling = 1;
+			break;
+		}
+
+		if (!(status & ME4600_AO_STATUS_BIT_FSM)) {	// State machine is not working. Waiting for stop finish.
+			instance->status = ao_status_stream_end;
+			signaling = 1;
+		}
+		// State machine is working.
+		reschedule = 1;
+		break;
+
+	default:
+		PERROR_CRITICAL("Status is in wrong state (%d)!\n",
+				instance->status);
+		instance->status = ao_status_stream_error;
+		// Signal the end.
+		signaling = 1;
+		break;
+
+	}
+
+	if (signaling) {	//Signal it.
+		wake_up_interruptible_all(&instance->wait_queue);
+	}
+
+	if (instance->ao_control_task_flag && reschedule) {	// Reschedule task
+		queue_delayed_work(instance->me4600_workqueue,
+				   &instance->ao_control_task, 1);
+	} else {
+		PINFO("<%s> Ending control task.\n", __FUNCTION__);
+	}
+
+}
+#else
+/// @note SPECIAL BUILD FOR BOSCH
+/// @author Guenter Gebhardt
+static int me4600_ao_io_reset_subdevice(me_subdevice_t * subdevice,
+					struct file *filep, int flags)
+{
+	me4600_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint32_t tmp;
+	unsigned long status;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER spin_lock_irqsave(&instance->subdevice_lock, status);
+	spin_lock(instance->preload_reg_lock);
+	tmp = inl(instance->preload_reg);
+	tmp &= ~(0x10001 << instance->ao_idx);
+	outl(tmp, instance->preload_reg);
+	*instance->preload_flags &= ~(0x1 << instance->ao_idx);
+	spin_unlock(instance->preload_reg_lock);
+
+	tmp = inl(instance->ctrl_reg);
+	tmp |= ME4600_AO_CTRL_BIT_STOP | ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+	outl(tmp, instance->ctrl_reg);
+
+	while (inl(instance->status_reg) & ME4600_AO_STATUS_BIT_FSM) ;
+
+	outl(ME4600_AO_CTRL_BIT_STOP | ME4600_AO_CTRL_BIT_IMMEDIATE_STOP,
+	     instance->ctrl_reg);
+
+	outl(0x8000, instance->single_reg);
+
+	instance->single_value = 0x8000;
+	instance->circ_buf.head = 0;
+	instance->circ_buf.tail = 0;
+
+	spin_unlock_irqrestore(&instance->subdevice_lock, status);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ao_io_single_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int single_config,
+				      int ref,
+				      int trig_chan,
+				      int trig_type, int trig_edge, int flags)
+{
+	me4600_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint32_t tmp;
+	unsigned long cpu_flags;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER
+	    spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+
+	if (inl(instance->status_reg) & ME4600_AO_STATUS_BIT_FSM) {
+		PERROR("Subdevice is busy.\n");
+		err = ME_ERRNO_SUBDEVICE_BUSY;
+		goto ERROR;
+	}
+
+	if (channel == 0) {
+		if (single_config == 0) {
+			if (ref == ME_REF_AO_GROUND) {
+				if (trig_chan == ME_TRIG_CHAN_DEFAULT) {
+					if (trig_type == ME_TRIG_TYPE_SW) {
+						tmp = inl(instance->ctrl_reg);
+						tmp |=
+						    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+						outl(tmp, instance->ctrl_reg);
+						tmp =
+						    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+						outl(tmp, instance->ctrl_reg);
+
+						spin_lock(instance->
+							  preload_reg_lock);
+						tmp =
+						    inl(instance->preload_reg);
+						tmp &=
+						    ~(0x10001 << instance->
+						      ao_idx);
+						outl(tmp,
+						     instance->preload_reg);
+						*instance->preload_flags &=
+						    ~(0x1 << instance->ao_idx);
+						spin_unlock(instance->
+							    preload_reg_lock);
+					} else if (trig_type ==
+						   ME_TRIG_TYPE_EXT_DIGITAL) {
+						if (trig_edge ==
+						    ME_TRIG_EDGE_RISING) {
+							tmp =
+							    inl(instance->
+								ctrl_reg);
+							tmp |=
+							    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+							outl(tmp,
+							     instance->
+							     ctrl_reg);
+							tmp =
+							    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP
+							    |
+							    ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG;
+							outl(tmp,
+							     instance->
+							     ctrl_reg);
+						} else if (trig_edge ==
+							   ME_TRIG_EDGE_FALLING)
+						{
+							tmp =
+							    inl(instance->
+								ctrl_reg);
+							tmp |=
+							    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+							outl(tmp,
+							     instance->
+							     ctrl_reg);
+							tmp =
+							    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP
+							    |
+							    ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG
+							    |
+							    ME4600_AO_CTRL_BIT_EX_TRIG_EDGE;
+							outl(tmp,
+							     instance->
+							     ctrl_reg);
+						} else if (trig_edge ==
+							   ME_TRIG_EDGE_ANY) {
+							tmp =
+							    inl(instance->
+								ctrl_reg);
+							tmp |=
+							    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+							outl(tmp,
+							     instance->
+							     ctrl_reg);
+							tmp =
+							    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP
+							    |
+							    ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG
+							    |
+							    ME4600_AO_CTRL_BIT_EX_TRIG_EDGE
+							    |
+							    ME4600_AO_CTRL_BIT_EX_TRIG_EDGE_BOTH;
+							outl(tmp,
+							     instance->
+							     ctrl_reg);
+						} else {
+							PERROR
+							    ("Invalid trigger edge.\n");
+							err =
+							    ME_ERRNO_INVALID_TRIG_EDGE;
+							goto ERROR;
+						}
+
+						spin_lock(instance->
+							  preload_reg_lock);
+
+						tmp =
+						    inl(instance->preload_reg);
+						tmp &=
+						    ~(0x10001 << instance->
+						      ao_idx);
+						tmp |= 0x1 << instance->ao_idx;
+						outl(tmp,
+						     instance->preload_reg);
+						*instance->preload_flags &=
+						    ~(0x1 << instance->ao_idx);
+						spin_unlock(instance->
+							    preload_reg_lock);
+					} else {
+						PERROR
+						    ("Invalid trigger type.\n");
+						err =
+						    ME_ERRNO_INVALID_TRIG_TYPE;
+						goto ERROR;
+					}
+				} else if (trig_chan ==
+					   ME_TRIG_CHAN_SYNCHRONOUS) {
+					if (trig_type == ME_TRIG_TYPE_SW) {
+						tmp = inl(instance->ctrl_reg);
+						tmp |=
+						    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+						outl(tmp, instance->ctrl_reg);
+						tmp =
+						    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+						outl(tmp, instance->ctrl_reg);
+
+						spin_lock(instance->
+							  preload_reg_lock);
+						tmp =
+						    inl(instance->preload_reg);
+						tmp &=
+						    ~(0x10001 << instance->
+						      ao_idx);
+						tmp |= 0x1 << instance->ao_idx;
+						outl(tmp,
+						     instance->preload_reg);
+						*instance->preload_flags |=
+						    0x1 << instance->ao_idx;
+						spin_unlock(instance->
+							    preload_reg_lock);
+					} else if (trig_type ==
+						   ME_TRIG_TYPE_EXT_DIGITAL) {
+						if (trig_edge ==
+						    ME_TRIG_EDGE_RISING) {
+							tmp =
+							    inl(instance->
+								ctrl_reg);
+							tmp |=
+							    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+							outl(tmp,
+							     instance->
+							     ctrl_reg);
+							tmp =
+							    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+							outl(tmp,
+							     instance->
+							     ctrl_reg);
+						} else if (trig_edge ==
+							   ME_TRIG_EDGE_FALLING)
+						{
+							tmp =
+							    inl(instance->
+								ctrl_reg);
+							tmp |=
+							    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+							outl(tmp,
+							     instance->
+							     ctrl_reg);
+							tmp =
+							    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP
+							    |
+							    ME4600_AO_CTRL_BIT_EX_TRIG_EDGE;
+							outl(tmp,
+							     instance->
+							     ctrl_reg);
+						} else if (trig_edge ==
+							   ME_TRIG_EDGE_ANY) {
+							tmp =
+							    inl(instance->
+								ctrl_reg);
+							tmp |=
+							    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+							outl(tmp,
+							     instance->
+							     ctrl_reg);
+							tmp =
+							    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP
+							    |
+							    ME4600_AO_CTRL_BIT_EX_TRIG_EDGE
+							    |
+							    ME4600_AO_CTRL_BIT_EX_TRIG_EDGE_BOTH;
+							outl(tmp,
+							     instance->
+							     ctrl_reg);
+						} else {
+							PERROR
+							    ("Invalid trigger edge.\n");
+							err =
+							    ME_ERRNO_INVALID_TRIG_EDGE;
+							goto ERROR;
+						}
+
+						spin_lock(instance->
+							  preload_reg_lock);
+
+						tmp =
+						    inl(instance->preload_reg);
+						tmp |=
+						    0x10001 << instance->ao_idx;
+						outl(tmp,
+						     instance->preload_reg);
+						*instance->preload_flags &=
+						    ~(0x1 << instance->ao_idx);
+						spin_unlock(instance->
+							    preload_reg_lock);
+					} else {
+						PERROR
+						    ("Invalid trigger type.\n");
+						err =
+						    ME_ERRNO_INVALID_TRIG_TYPE;
+						goto ERROR;
+					}
+				} else {
+					PERROR
+					    ("Invalid trigger channel specified.\n");
+					err = ME_ERRNO_INVALID_REF;
+					goto ERROR;
+				}
+			} else {
+				PERROR("Invalid analog reference specified.\n");
+				err = ME_ERRNO_INVALID_REF;
+				goto ERROR;
+			}
+		} else {
+			PERROR("Invalid single config specified.\n");
+			err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+			goto ERROR;
+		}
+	} else {
+		PERROR("Invalid channel number specified.\n");
+		err = ME_ERRNO_INVALID_CHANNEL;
+		goto ERROR;
+	}
+
+      ERROR:
+
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ao_io_single_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int channel,
+				    int *value, int time_out, int flags)
+{
+	me4600_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long tmp;
+	unsigned long cpu_flags;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	if (channel != 0) {
+		PERROR("Invalid channel number specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	ME_SUBDEVICE_ENTER
+	    spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	tmp = inl(instance->ctrl_reg);
+
+	if (tmp & 0x3) {
+		PERROR("Not in single mode.\n");
+		err = ME_ERRNO_PREVIOUS_CONFIG;
+	} else {
+		*value = instance->single_value;
+	}
+
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ao_io_single_write(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int channel,
+				     int value, int time_out, int flags)
+{
+	me4600_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long mask = 0;
+	unsigned long tmp;
+	unsigned long cpu_flags;
+	int i;
+	wait_queue_head_t queue;
+	unsigned long j;
+	unsigned long delay = 0;
+
+	PDEBUG("executed.\n");
+
+	init_waitqueue_head(&queue);
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	if (channel != 0) {
+		PERROR("Invalid channel number specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid timeout specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	if (time_out) {
+		delay = (time_out * HZ) / 1000;
+
+		if (delay == 0)
+			delay = 1;
+	}
+
+	ME_SUBDEVICE_ENTER
+	    spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+
+	tmp = inl(instance->ctrl_reg);
+
+	if (tmp & 0x3) {
+		spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+		PERROR("Not in single mode.\n");
+		err = ME_ERRNO_PREVIOUS_CONFIG;
+		goto ERROR;
+	}
+
+	if (tmp & ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG) {
+		outl(value, instance->single_reg);
+		instance->single_value = value;
+		spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+		if (!(flags & ME_IO_SINGLE_TYPE_WRITE_NONBLOCKING)) {
+			j = jiffies;
+
+			while (inl(instance->status_reg) &
+			       ME4600_AO_STATUS_BIT_FSM) {
+				interruptible_sleep_on_timeout(&queue, 1);
+
+				if (signal_pending(current)) {
+					PERROR
+					    ("Wait on external trigger interrupted by signal.\n");
+					err = ME_ERRNO_SIGNAL;
+					goto ERROR;
+				}
+
+				if (delay && ((jiffies - j) > delay)) {
+					PERROR("Timeout reached.\n");
+					err = ME_ERRNO_TIMEOUT;
+					goto ERROR;
+				}
+			}
+		}
+	} else if ((inl(instance->preload_reg) & (0x10001 << instance->ao_idx))
+		   == (0x10001 << instance->ao_idx)) {
+		if (flags & ME_IO_SINGLE_TYPE_TRIG_SYNCHRONOUS) {
+			tmp |= ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG;
+			outl(tmp, instance->ctrl_reg);
+			outl(value, instance->single_reg);
+			instance->single_value = value;
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+
+			if (!(flags & ME_IO_SINGLE_TYPE_WRITE_NONBLOCKING)) {
+				j = jiffies;
+
+				while (inl(instance->status_reg) &
+				       ME4600_AO_STATUS_BIT_FSM) {
+					interruptible_sleep_on_timeout(&queue,
+								       1);
+
+					if (signal_pending(current)) {
+						PERROR
+						    ("Wait on external trigger interrupted by signal.\n");
+						err = ME_ERRNO_SIGNAL;
+						goto ERROR;
+					}
+
+					if (delay && ((jiffies - j) > delay)) {
+						PERROR("Timeout reached.\n");
+						err = ME_ERRNO_TIMEOUT;
+						goto ERROR;
+					}
+				}
+			}
+		} else {
+			outl(value, instance->single_reg);
+			instance->single_value = value;
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+		}
+	} else if ((inl(instance->preload_reg) & (0x10001 << instance->ao_idx))
+		   == (0x1 << instance->ao_idx)) {
+		outl(value, instance->single_reg);
+		instance->single_value = value;
+
+		PDEBUG("Synchronous SW, flags = 0x%X.\n", flags);
+
+		if (flags & ME_IO_SINGLE_TYPE_TRIG_SYNCHRONOUS) {
+			PDEBUG("Trigger synchronous SW.\n");
+			spin_lock(instance->preload_reg_lock);
+			tmp = inl(instance->preload_reg);
+
+			for (i = 0; i < ME4600_AO_MAX_SUBDEVICES; i++) {
+				if ((*instance->preload_flags & (0x1 << i))) {
+					if ((tmp & (0x10001 << i)) ==
+					    (0x1 << i)) {
+						mask |= 0x1 << i;
+					}
+				}
+			}
+
+			tmp &= ~(mask);
+
+			outl(tmp, instance->preload_reg);
+			spin_unlock(instance->preload_reg_lock);
+		}
+
+		spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+	} else {
+		outl(value, instance->single_reg);
+		instance->single_value = value;
+		spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+	}
+
+      ERROR:
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ao_io_stream_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      meIOStreamConfig_t * config_list,
+				      int count,
+				      meIOStreamTrigger_t * trigger,
+				      int fifo_irq_threshold, int flags)
+{
+	me4600_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long ctrl;
+	unsigned long tmp;
+	unsigned long cpu_flags;
+	uint64_t conv_ticks;
+	unsigned int conv_start_ticks_low = trigger->iConvStartTicksLow;
+	unsigned int conv_start_ticks_high = trigger->iConvStartTicksHigh;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	conv_ticks =
+	    (uint64_t) conv_start_ticks_low +
+	    ((uint64_t) conv_start_ticks_high << 32);
+
+	if (!instance->fifo) {
+		PERROR("Not a streaming ao.\n");
+		return ME_ERRNO_NOT_SUPPORTED;
+	}
+
+	ME_SUBDEVICE_ENTER
+	    spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+
+	if ((inl(instance->status_reg)) & ME4600_AO_STATUS_BIT_FSM) {
+		PERROR("Subdevice is busy.\n");
+		err = ME_ERRNO_SUBDEVICE_BUSY;
+		goto ERROR;
+	}
+
+	ctrl = inl(instance->ctrl_reg);
+	ctrl |= ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+	outl(ctrl, instance->ctrl_reg);
+	ctrl = ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+	outl(ctrl, instance->ctrl_reg);
+
+	if (count != 1) {
+		PERROR("Invalid stream configuration list count specified.\n");
+		err = ME_ERRNO_INVALID_CONFIG_LIST_COUNT;
+		goto ERROR;
+	}
+
+	if (config_list[0].iChannel != 0) {
+		PERROR("Invalid channel number specified.\n");
+		err = ME_ERRNO_INVALID_CHANNEL;
+		goto ERROR;
+	}
+
+	if (config_list[0].iStreamConfig != 0) {
+		PERROR("Invalid stream config specified.\n");
+		err = ME_ERRNO_INVALID_STREAM_CONFIG;
+		goto ERROR;
+	}
+
+	if (config_list[0].iRef != ME_REF_AO_GROUND) {
+		PERROR("Invalid analog reference.\n");
+		err = ME_ERRNO_INVALID_REF;
+		goto ERROR;
+	}
+
+	if ((trigger->iAcqStartTicksLow != 0)
+	    || (trigger->iAcqStartTicksHigh != 0)) {
+		PERROR
+		    ("Invalid acquisition start trigger argument specified.\n");
+		err = ME_ERRNO_INVALID_ACQ_START_ARG;
+		goto ERROR;
+	}
+
+	switch (trigger->iAcqStartTrigType) {
+
+	case ME_TRIG_TYPE_SW:
+		break;
+
+	case ME_TRIG_TYPE_EXT_DIGITAL:
+		ctrl |= ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG;
+
+		switch (trigger->iAcqStartTrigEdge) {
+
+		case ME_TRIG_EDGE_RISING:
+			break;
+
+		case ME_TRIG_EDGE_FALLING:
+			ctrl |= ME4600_AO_CTRL_BIT_EX_TRIG_EDGE;
+
+			break;
+
+		case ME_TRIG_EDGE_ANY:
+			ctrl |=
+			    ME4600_AO_CTRL_BIT_EX_TRIG_EDGE |
+			    ME4600_AO_CTRL_BIT_EX_TRIG_EDGE_BOTH;
+
+			break;
+
+		default:
+			PERROR
+			    ("Invalid acquisition start trigger edge specified.\n");
+
+			err = ME_ERRNO_INVALID_ACQ_START_TRIG_EDGE;
+
+			goto ERROR;
+
+			break;
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid acquisition start trigger type specified.\n");
+
+		err = ME_ERRNO_INVALID_ACQ_START_TRIG_TYPE;
+
+		goto ERROR;
+
+		break;
+	}
+
+	switch (trigger->iScanStartTrigType) {
+
+	case ME_TRIG_TYPE_FOLLOW:
+		break;
+
+	default:
+		PERROR("Invalid scan start trigger type specified.\n");
+
+		err = ME_ERRNO_INVALID_SCAN_START_TRIG_TYPE;
+
+		goto ERROR;
+
+		break;
+	}
+
+	switch (trigger->iConvStartTrigType) {
+
+	case ME_TRIG_TYPE_TIMER:
+		if ((conv_ticks < ME4600_AO_MIN_CHAN_TICKS)
+		    || (conv_ticks > ME4600_AO_MAX_CHAN_TICKS)) {
+			PERROR
+			    ("Invalid conv start trigger argument specified.\n");
+			err = ME_ERRNO_INVALID_CONV_START_ARG;
+			goto ERROR;
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid conv start trigger type specified.\n");
+
+		err = ME_ERRNO_INVALID_CONV_START_TRIG_TYPE;
+
+		goto ERROR;
+
+		break;
+	}
+
+	/* Preset to hardware wraparound mode */
+	instance->flags &= ~(ME4600_AO_FLAGS_SW_WRAP_MODE_MASK);
+
+	switch (trigger->iScanStopTrigType) {
+
+	case ME_TRIG_TYPE_NONE:
+		if (flags & ME_IO_STREAM_CONFIG_WRAPAROUND) {
+			/* Set flags to indicate usage of software mode. */
+			instance->flags |= ME4600_AO_FLAGS_SW_WRAP_MODE_INF;
+			instance->wrap_count = 0;
+			instance->wrap_remaining = 0;
+		}
+
+		break;
+
+	case ME_TRIG_TYPE_COUNT:
+		if (flags & ME_IO_STREAM_CONFIG_WRAPAROUND) {
+			if (trigger->iScanStopCount <= 0) {
+				PERROR("Invalid scan stop count specified.\n");
+				err = ME_ERRNO_INVALID_SCAN_STOP_ARG;
+				goto ERROR;
+			}
+
+			/* Set flags to indicate usage of software mode. */
+			instance->flags |= ME4600_AO_FLAGS_SW_WRAP_MODE_FIN;
+			instance->wrap_count = trigger->iScanStopCount;
+			instance->wrap_remaining = trigger->iScanStopCount;
+		} else {
+			PERROR("Invalid scan stop trigger type specified.\n");
+			err = ME_ERRNO_INVALID_ACQ_STOP_TRIG_TYPE;
+			goto ERROR;
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid scan stop trigger type specified.\n");
+
+		err = ME_ERRNO_INVALID_SCAN_STOP_TRIG_TYPE;
+
+		goto ERROR;
+
+		break;
+	}
+
+	switch (trigger->iAcqStopTrigType) {
+
+	case ME_TRIG_TYPE_NONE:
+		break;
+
+	case ME_TRIG_TYPE_COUNT:
+		if (trigger->iScanStopTrigType != ME_TRIG_TYPE_NONE) {
+			PERROR("Invalid acq stop trigger type specified.\n");
+			err = ME_ERRNO_INVALID_ACQ_STOP_TRIG_TYPE;
+			goto ERROR;
+		}
+
+		if (flags & ME_IO_STREAM_CONFIG_WRAPAROUND) {
+			if (trigger->iAcqStopCount <= 0) {
+				PERROR("Invalid acq stop count specified.\n");
+				err = ME_ERRNO_INVALID_ACQ_STOP_ARG;
+				goto ERROR;
+			}
+
+			/* Set flags to indicate usage of software mode. */
+			instance->flags |= ME4600_AO_FLAGS_SW_WRAP_MODE_FIN;
+			instance->wrap_count = trigger->iAcqStopCount;
+			instance->wrap_remaining = trigger->iAcqStopCount;
+		} else {
+			PERROR("Invalid acp stop trigger type specified.\n");
+			err = ME_ERRNO_INVALID_ACQ_STOP_TRIG_TYPE;
+			goto ERROR;
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid acq stop trigger type specified.\n");
+		err = ME_ERRNO_INVALID_ACQ_STOP_TRIG_TYPE;
+		goto ERROR;
+		break;
+	}
+
+	switch (trigger->iAcqStartTrigChan) {
+
+	case ME_TRIG_CHAN_DEFAULT:
+		spin_lock(instance->preload_reg_lock);
+		tmp = inl(instance->preload_reg);
+		tmp &= ~(0x10001 << instance->ao_idx);
+		outl(tmp, instance->preload_reg);
+		spin_unlock(instance->preload_reg_lock);
+
+		break;
+
+	case ME_TRIG_CHAN_SYNCHRONOUS:
+		if (trigger->iAcqStartTrigType == ME_TRIG_TYPE_SW) {
+			spin_lock(instance->preload_reg_lock);
+			tmp = inl(instance->preload_reg);
+			tmp &= ~(0x10001 << instance->ao_idx);
+			outl(tmp, instance->preload_reg);
+			tmp |= 0x1 << instance->ao_idx;
+			outl(tmp, instance->preload_reg);
+			spin_unlock(instance->preload_reg_lock);
+		} else {
+			ctrl &= ~(ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG);
+			spin_lock(instance->preload_reg_lock);
+			tmp = inl(instance->preload_reg);
+			tmp &= ~(0x10001 << instance->ao_idx);
+			outl(tmp, instance->preload_reg);
+			tmp |= 0x10000 << instance->ao_idx;
+			outl(tmp, instance->preload_reg);
+			spin_unlock(instance->preload_reg_lock);
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid acq start trigger channel specified.\n");
+		err = ME_ERRNO_INVALID_ACQ_START_TRIG_CHAN;
+		goto ERROR;
+
+		break;
+	}
+
+	outl(conv_ticks - 2, instance->timer_reg);
+
+	if (flags & ME_IO_STREAM_CONFIG_BIT_PATTERN) {
+		if (instance->ao_idx == 3) {
+			ctrl |= ME4600_AO_CTRL_BIT_ENABLE_DO;
+		} else {
+			err = ME_ERRNO_INVALID_FLAGS;
+			goto ERROR;
+		}
+	} else {
+		if (instance->ao_idx == 3) {
+			ctrl &= ~ME4600_AO_CTRL_BIT_ENABLE_DO;
+		}
+	}
+
+	/* Set hardware mode. */
+	if (flags & ME_IO_STREAM_CONFIG_WRAPAROUND) {
+		ctrl |= ME4600_AO_CTRL_BIT_MODE_0;
+	} else {
+		ctrl |= ME4600_AO_CTRL_BIT_MODE_1;
+	}
+
+	PDEBUG("Preload word = 0x%X.\n", inl(instance->preload_reg));
+
+	PDEBUG("Ctrl word = 0x%lX.\n", ctrl);
+	outl(ctrl, instance->ctrl_reg);	// Write the control word
+
+      ERROR:
+
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ao_io_stream_new_values(me_subdevice_t * subdevice,
+					  struct file *filep,
+					  int time_out, int *count, int flags)
+{
+	me4600_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	long t = 0;
+	long j;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	if (!instance->fifo) {
+		PERROR("Not a streaming ao.\n");
+		return ME_ERRNO_NOT_SUPPORTED;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid time_out specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	if (time_out) {
+		t = (time_out * HZ) / 1000;
+
+		if (t == 0)
+			t = 1;
+	}
+
+	*count = 0;
+
+	ME_SUBDEVICE_ENTER;
+
+	if (t) {
+		j = jiffies;
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 ((me_circ_buf_space
+						   (&instance->circ_buf))
+						  || !(inl(instance->status_reg)
+						       &
+						       ME4600_AO_STATUS_BIT_FSM)),
+						 t);
+
+		if (!(inl(instance->status_reg) & ME4600_AO_STATUS_BIT_FSM)) {
+			PERROR("AO subdevice is not running.\n");
+			err = ME_ERRNO_SUBDEVICE_NOT_RUNNING;
+		} else if (signal_pending(current)) {
+			PERROR("Wait on values interrupted from signal.\n");
+			err = ME_ERRNO_SIGNAL;
+		} else if ((jiffies - j) >= t) {
+			PERROR("Wait on values timed out.\n");
+			err = ME_ERRNO_TIMEOUT;
+		} else {
+			*count = me_circ_buf_space(&instance->circ_buf);
+		}
+	} else {
+		wait_event_interruptible(instance->wait_queue,
+					 ((me_circ_buf_space
+					   (&instance->circ_buf))
+					  || !(inl(instance->status_reg) &
+					       ME4600_AO_STATUS_BIT_FSM)));
+
+		if (!(inl(instance->status_reg) & ME4600_AO_STATUS_BIT_FSM)) {
+			PERROR("AO subdevice is not running.\n");
+			err = ME_ERRNO_SUBDEVICE_NOT_RUNNING;
+		} else if (signal_pending(current)) {
+			PERROR("Wait on values interrupted from signal.\n");
+			err = ME_ERRNO_SIGNAL;
+		} else {
+			*count = me_circ_buf_space(&instance->circ_buf);
+		}
+	}
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static void stop_immediately(me4600_ao_subdevice_t * instance)
+{
+	unsigned long cpu_flags;
+	uint32_t tmp;
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	tmp = inl(instance->ctrl_reg);
+	tmp |= ME4600_AO_CTRL_BIT_STOP | ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+	outl(tmp, instance->ctrl_reg);
+
+	while (inl(instance->status_reg) & ME4600_AO_STATUS_BIT_FSM) ;
+
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+}
+
+static int me4600_ao_io_stream_start(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int start_mode, int time_out, int flags)
+{
+	me4600_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long cpu_flags = 0;
+	unsigned long ref;
+	unsigned long tmp;
+	unsigned long delay = 0;
+	wait_queue_head_t queue;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	init_waitqueue_head(&queue);
+
+	if (time_out < 0) {
+		PERROR("Invalid timeout specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	if (time_out) {
+		delay = (time_out * HZ) / 1000;
+
+		if (delay == 0)
+			delay = 1;
+	}
+
+	if (!instance->fifo) {
+		PERROR("Not a streaming ao.\n");
+		return ME_ERRNO_NOT_SUPPORTED;
+	}
+
+	ME_SUBDEVICE_ENTER
+	    spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+
+	tmp = inl(instance->ctrl_reg);
+
+	switch (tmp & (ME4600_AO_CTRL_MASK_MODE)) {
+
+	case 0:		// Single mode
+		spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+		PERROR("Subdevice is configured in single mode.\n");
+		err = ME_ERRNO_PREVIOUS_CONFIG;
+		goto ERROR;
+
+	case 1:		// Wraparound mode
+		if (tmp & ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG) {	// Normal wraparound with external trigger
+
+			if ((inl(instance->status_reg) &
+			     ME4600_AO_STATUS_BIT_FSM)) {
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+				PERROR("Conversion is already running.\n");
+				err = ME_ERRNO_SUBDEVICE_BUSY;
+				goto ERROR;
+			}
+
+			tmp &=
+			    ~(ME4600_AO_CTRL_BIT_ENABLE_IRQ |
+			      ME4600_AO_CTRL_BIT_STOP |
+			      ME4600_AO_CTRL_BIT_IMMEDIATE_STOP);
+
+			outl(tmp, instance->ctrl_reg);
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+
+			if (start_mode == ME_START_MODE_BLOCKING) {
+				init_waitqueue_head(&queue);
+
+				if (delay) {
+					ref = jiffies;
+
+					while (!
+					       (inl(instance->status_reg) &
+						ME4600_AO_STATUS_BIT_FSM)) {
+						interruptible_sleep_on_timeout
+						    (&queue, 1);
+
+						if (signal_pending(current)) {
+							PERROR
+							    ("Wait on start of state machine interrupted.\n");
+							stop_immediately
+							    (instance);
+							err = ME_ERRNO_SIGNAL;
+							goto ERROR;
+						}
+
+						if (((jiffies - ref) >= delay)) {
+							PERROR
+							    ("Timeout reached.\n");
+							stop_immediately
+							    (instance);
+							err = ME_ERRNO_TIMEOUT;
+							goto ERROR;
+						}
+					}
+				} else {
+					while (!
+					       (inl(instance->status_reg) &
+						ME4600_AO_STATUS_BIT_FSM)) {
+						interruptible_sleep_on_timeout
+						    (&queue, 1);
+
+						if (signal_pending(current)) {
+							PERROR
+							    ("Wait on start of state machine interrupted.\n");
+							stop_immediately
+							    (instance);
+							err = ME_ERRNO_SIGNAL;
+							goto ERROR;
+						}
+					}
+				}
+			} else if (start_mode == ME_START_MODE_NONBLOCKING) {
+			} else {
+				PERROR("Invalid start mode specified.\n");
+				err = ME_ERRNO_INVALID_START_MODE;
+				goto ERROR;
+			}
+		} else if ((inl(instance->preload_reg) & (0x10001 << instance->ao_idx)) == (0x10000 << instance->ao_idx)) {	// Synchronous with external trigger
+
+			if ((inl(instance->status_reg) &
+			     ME4600_AO_STATUS_BIT_FSM)) {
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+				PERROR("Conversion is already running.\n");
+				err = ME_ERRNO_SUBDEVICE_BUSY;
+				goto ERROR;
+			}
+
+			if (flags & ME_IO_STREAM_START_TYPE_TRIG_SYNCHRONOUS) {
+				tmp |= ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG;
+				tmp &=
+				    ~(ME4600_AO_CTRL_BIT_ENABLE_IRQ |
+				      ME4600_AO_CTRL_BIT_STOP |
+				      ME4600_AO_CTRL_BIT_IMMEDIATE_STOP);
+				outl(tmp, instance->ctrl_reg);
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+
+				if (start_mode == ME_START_MODE_BLOCKING) {
+					init_waitqueue_head(&queue);
+
+					if (delay) {
+						ref = jiffies;
+
+						while (!
+						       (inl
+							(instance->
+							 status_reg) &
+							ME4600_AO_STATUS_BIT_FSM))
+						{
+							interruptible_sleep_on_timeout
+							    (&queue, 1);
+
+							if (signal_pending
+							    (current)) {
+								PERROR
+								    ("Wait on start of state machine interrupted.\n");
+								stop_immediately
+								    (instance);
+								err =
+								    ME_ERRNO_SIGNAL;
+								goto ERROR;
+							}
+
+							if (((jiffies - ref) >=
+							     delay)) {
+								PERROR
+								    ("Timeout reached.\n");
+								stop_immediately
+								    (instance);
+								err =
+								    ME_ERRNO_TIMEOUT;
+								goto ERROR;
+							}
+						}
+					} else {
+						while (!
+						       (inl
+							(instance->
+							 status_reg) &
+							ME4600_AO_STATUS_BIT_FSM))
+						{
+							interruptible_sleep_on_timeout
+							    (&queue, 1);
+
+							if (signal_pending
+							    (current)) {
+								PERROR
+								    ("Wait on start of state machine interrupted.\n");
+								stop_immediately
+								    (instance);
+								err =
+								    ME_ERRNO_SIGNAL;
+								goto ERROR;
+							}
+						}
+					}
+				} else if (start_mode ==
+					   ME_START_MODE_NONBLOCKING) {
+				} else {
+					PERROR
+					    ("Invalid start mode specified.\n");
+					err = ME_ERRNO_INVALID_START_MODE;
+					goto ERROR;
+				}
+			} else {
+				tmp &=
+				    ~(ME4600_AO_CTRL_BIT_ENABLE_IRQ |
+				      ME4600_AO_CTRL_BIT_STOP |
+				      ME4600_AO_CTRL_BIT_IMMEDIATE_STOP);
+				outl(tmp, instance->ctrl_reg);
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+			}
+		} else if ((inl(instance->preload_reg) & (0x10001 << instance->ao_idx)) == (0x1 << instance->ao_idx)) {	// Synchronous wraparound with sw trigger
+
+			if ((inl(instance->status_reg) &
+			     ME4600_AO_STATUS_BIT_FSM)) {
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+				PERROR("Conversion is already running.\n");
+				err = ME_ERRNO_SUBDEVICE_BUSY;
+				goto ERROR;
+			}
+
+			tmp &=
+			    ~(ME4600_AO_CTRL_BIT_ENABLE_IRQ |
+			      ME4600_AO_CTRL_BIT_STOP |
+			      ME4600_AO_CTRL_BIT_IMMEDIATE_STOP);
+
+			outl(tmp, instance->ctrl_reg);
+
+			if (flags & ME_IO_STREAM_START_TYPE_TRIG_SYNCHRONOUS) {
+				outl(0x8000, instance->single_reg);
+				instance->single_value = 0x8000;
+			}
+
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+		} else {	// Software start
+
+			if ((inl(instance->status_reg) &
+			     ME4600_AO_STATUS_BIT_FSM)) {
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+				PERROR("Conversion is already running.\n");
+				err = ME_ERRNO_SUBDEVICE_BUSY;
+				goto ERROR;
+			}
+
+			tmp &=
+			    ~(ME4600_AO_CTRL_BIT_ENABLE_IRQ |
+			      ME4600_AO_CTRL_BIT_STOP |
+			      ME4600_AO_CTRL_BIT_IMMEDIATE_STOP);
+
+			outl(tmp, instance->ctrl_reg);
+
+			outl(0x8000, instance->single_reg);
+			instance->single_value = 0x8000;
+
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+		}
+
+		break;
+
+	case 2:		// Continuous mode
+		if (tmp & ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG) {	// Externally triggered
+
+			if ((inl(instance->status_reg) &
+			     ME4600_AO_STATUS_BIT_FSM)) {
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+				PERROR("Conversion is already running.\n");
+				err = ME_ERRNO_SUBDEVICE_BUSY;
+				goto ERROR;
+			}
+
+			tmp &=
+			    ~(ME4600_AO_CTRL_BIT_STOP |
+			      ME4600_AO_CTRL_BIT_IMMEDIATE_STOP);
+			tmp |= ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+			outl(tmp, instance->ctrl_reg);
+			instance->wrap_remaining = instance->wrap_count;
+			instance->circ_buf.tail = 0;
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+
+			if (start_mode == ME_START_MODE_BLOCKING) {
+				init_waitqueue_head(&queue);
+
+				if (delay) {
+					ref = jiffies;
+
+					while (!
+					       (inl(instance->status_reg) &
+						ME4600_AO_STATUS_BIT_FSM)) {
+						interruptible_sleep_on_timeout
+						    (&queue, 1);
+
+						if (signal_pending(current)) {
+							PERROR
+							    ("Wait on start of state machine interrupted.\n");
+							stop_immediately
+							    (instance);
+							err = ME_ERRNO_SIGNAL;
+							goto ERROR;
+						}
+
+						if (((jiffies - ref) >= delay)) {
+							PERROR
+							    ("Timeout reached.\n");
+							stop_immediately
+							    (instance);
+							err = ME_ERRNO_TIMEOUT;
+							goto ERROR;
+						}
+					}
+				} else {
+					while (!
+					       (inl(instance->status_reg) &
+						ME4600_AO_STATUS_BIT_FSM)) {
+						interruptible_sleep_on_timeout
+						    (&queue, 1);
+
+						if (signal_pending(current)) {
+							PERROR
+							    ("Wait on start of state machine interrupted.\n");
+							stop_immediately
+							    (instance);
+							err = ME_ERRNO_SIGNAL;
+							goto ERROR;
+						}
+					}
+				}
+			} else if (start_mode == ME_START_MODE_NONBLOCKING) {
+				/* Do nothing */
+			} else {
+				PERROR("Invalid start mode specified.\n");
+				stop_immediately(instance);
+				err = ME_ERRNO_INVALID_START_MODE;
+				goto ERROR;
+			}
+		} else if ((inl(instance->preload_reg) & (0x10001 << instance->ao_idx)) == (0x10000 << instance->ao_idx)) {	// Synchronous with external trigger
+
+			if ((inl(instance->status_reg) &
+			     ME4600_AO_STATUS_BIT_FSM)) {
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+				PERROR("Conversion is already running.\n");
+				err = ME_ERRNO_SUBDEVICE_BUSY;
+				goto ERROR;
+			}
+
+			if (flags & ME_IO_STREAM_START_TYPE_TRIG_SYNCHRONOUS) {
+				tmp |=
+				    ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG |
+				    ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+				tmp &=
+				    ~(ME4600_AO_CTRL_BIT_STOP |
+				      ME4600_AO_CTRL_BIT_IMMEDIATE_STOP);
+				outl(tmp, instance->ctrl_reg);
+				instance->wrap_remaining = instance->wrap_count;
+				instance->circ_buf.tail = 0;
+
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+
+				if (start_mode == ME_START_MODE_BLOCKING) {
+					init_waitqueue_head(&queue);
+
+					if (delay) {
+						ref = jiffies;
+
+						while (!
+						       (inl
+							(instance->
+							 status_reg) &
+							ME4600_AO_STATUS_BIT_FSM))
+						{
+							interruptible_sleep_on_timeout
+							    (&queue, 1);
+
+							if (signal_pending
+							    (current)) {
+								PERROR
+								    ("Wait on start of state machine interrupted.\n");
+								stop_immediately
+								    (instance);
+								err =
+								    ME_ERRNO_SIGNAL;
+								goto ERROR;
+							}
+
+							if (((jiffies - ref) >=
+							     delay)) {
+								PERROR
+								    ("Timeout reached.\n");
+								stop_immediately
+								    (instance);
+								err =
+								    ME_ERRNO_TIMEOUT;
+								goto ERROR;
+							}
+						}
+					} else {
+						while (!
+						       (inl
+							(instance->
+							 status_reg) &
+							ME4600_AO_STATUS_BIT_FSM))
+						{
+							interruptible_sleep_on_timeout
+							    (&queue, 1);
+
+							if (signal_pending
+							    (current)) {
+								PERROR
+								    ("Wait on start of state machine interrupted.\n");
+								stop_immediately
+								    (instance);
+								err =
+								    ME_ERRNO_SIGNAL;
+								goto ERROR;
+							}
+						}
+					}
+				} else if (start_mode ==
+					   ME_START_MODE_NONBLOCKING) {
+				} else {
+					PERROR
+					    ("Invalid start mode specified.\n");
+					stop_immediately(instance);
+					err = ME_ERRNO_INVALID_START_MODE;
+					goto ERROR;
+				}
+			} else {
+				tmp |= ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+				tmp &=
+				    ~(ME4600_AO_CTRL_BIT_STOP |
+				      ME4600_AO_CTRL_BIT_IMMEDIATE_STOP);
+				outl(tmp, instance->ctrl_reg);
+				instance->wrap_remaining = instance->wrap_count;
+				instance->circ_buf.tail = 0;
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+			}
+		} else if ((inl(instance->preload_reg) & (0x10001 << instance->ao_idx)) == (0x1 << instance->ao_idx)) {	// Synchronous wraparound with sw trigger
+
+			if ((inl(instance->status_reg) &
+			     ME4600_AO_STATUS_BIT_FSM)) {
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+				PERROR("Conversion is already running.\n");
+				err = ME_ERRNO_SUBDEVICE_BUSY;
+				goto ERROR;
+			}
+
+			tmp &=
+			    ~(ME4600_AO_CTRL_BIT_STOP |
+			      ME4600_AO_CTRL_BIT_IMMEDIATE_STOP);
+			tmp |= ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+			instance->wrap_remaining = instance->wrap_count;
+			instance->circ_buf.tail = 0;
+			PDEBUG("CTRL Reg = 0x%X.\n", inl(instance->ctrl_reg));
+			outl(tmp, instance->ctrl_reg);
+
+			if (flags & ME_IO_STREAM_START_TYPE_TRIG_SYNCHRONOUS) {
+				outl(0x8000, instance->single_reg);
+				instance->single_value = 0x8000;
+			}
+
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+		} else {	// Software start
+
+			if ((inl(instance->status_reg) &
+			     ME4600_AO_STATUS_BIT_FSM)) {
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+				PERROR("Conversion is already running.\n");
+				err = ME_ERRNO_SUBDEVICE_BUSY;
+				goto ERROR;
+			}
+
+			tmp &=
+			    ~(ME4600_AO_CTRL_BIT_STOP |
+			      ME4600_AO_CTRL_BIT_IMMEDIATE_STOP);
+
+			tmp |= ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+			outl(tmp, instance->ctrl_reg);
+			outl(0x8000, instance->single_reg);
+			instance->single_value = 0x8000;
+			instance->wrap_remaining = instance->wrap_count;
+			instance->circ_buf.tail = 0;
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+		}
+
+		break;
+
+	default:
+		spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+		PERROR("Invalid mode configured.\n");
+		err = ME_ERRNO_INTERNAL;
+		goto ERROR;
+	}
+
+      ERROR:
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ao_io_stream_status(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int wait,
+				      int *status, int *values, int flags)
+{
+	me4600_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	wait_queue_head_t queue;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	init_waitqueue_head(&queue);
+
+	if (!instance->fifo) {
+		PERROR("Not a streaming ao.\n");
+		return ME_ERRNO_NOT_SUPPORTED;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	if (wait == ME_WAIT_NONE) {
+		*status =
+		    (inl(instance->status_reg) & ME4600_AO_STATUS_BIT_FSM) ?
+		    ME_STATUS_BUSY : ME_STATUS_IDLE;
+		*values = me_circ_buf_space(&instance->circ_buf);
+	} else if (wait == ME_WAIT_IDLE) {
+		while (inl(instance->status_reg) & ME4600_AO_STATUS_BIT_FSM) {
+			interruptible_sleep_on_timeout(&queue, 1);
+
+			if (instance->flags & ME4600_AO_FLAGS_BROKEN_PIPE) {
+				PERROR("Output stream was interrupted.\n");
+				*status = ME_STATUS_ERROR;
+				err = ME_ERRNO_SUCCESS;
+				goto ERROR;
+			}
+
+			if (signal_pending(current)) {
+				PERROR
+				    ("Wait on state machine interrupted by signal.\n");
+				*status = ME_STATUS_INVALID;
+				err = ME_ERRNO_SIGNAL;
+				goto ERROR;
+			}
+		}
+
+		*status = ME_STATUS_IDLE;
+
+		*values = me_circ_buf_space(&instance->circ_buf);
+	} else {
+		PERROR("Invalid wait argument specified.\n");
+		*status = ME_STATUS_INVALID;
+		err = ME_ERRNO_INVALID_WAIT;
+		goto ERROR;
+	}
+
+      ERROR:
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ao_io_stream_stop(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int stop_mode, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me4600_ao_subdevice_t *instance;
+	unsigned long cpu_flags;
+	unsigned long tmp;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	if (!instance->fifo) {
+		PERROR("Not a streaming ao.\n");
+		return ME_ERRNO_NOT_SUPPORTED;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	if (stop_mode == ME_STOP_MODE_IMMEDIATE) {
+		spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+		tmp = inl(instance->ctrl_reg);
+		tmp |=
+		    ME4600_AO_CTRL_BIT_STOP | ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+		outl(tmp, instance->ctrl_reg);
+
+		while (inl(instance->status_reg) & ME4600_AO_STATUS_BIT_FSM) ;
+
+		spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+	} else if (stop_mode == ME_STOP_MODE_LAST_VALUE) {
+		spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+		tmp = inl(instance->ctrl_reg);
+		tmp |= ME4600_AO_CTRL_BIT_STOP;
+		outl(tmp, instance->ctrl_reg);
+		spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+	} else {
+		PERROR("Invalid stop mode specified.\n");
+		err = ME_ERRNO_INVALID_STOP_MODE;
+		goto ERROR;
+	}
+
+      ERROR:
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ao_io_stream_write(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int write_mode,
+				     int *values, int *count, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me4600_ao_subdevice_t *instance;
+	unsigned long tmp;
+	int i;
+	int value;
+	int cnt = *count;
+	int c;
+	int k;
+	int ret = 0;
+	unsigned long cpu_flags = 0;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	if (!instance->fifo) {
+		PERROR("Not a streaming ao.\n");
+		return ME_ERRNO_NOT_SUPPORTED;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	if (*count <= 0) {
+		PERROR("Invalid count of values specified.\n");
+		err = ME_ERRNO_INVALID_VALUE_COUNT;
+		goto ERROR;
+	}
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+
+	tmp = inl(instance->ctrl_reg);
+
+	switch (tmp & 0x3) {
+
+	case 1:		// Wraparound mode
+		if (instance->bosch_fw) {	// Bosch firmware
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+
+			if (cnt != 7) {
+				PERROR
+				    ("Invalid count of values specified. 7 expected.\n");
+				err = ME_ERRNO_INVALID_VALUE_COUNT;
+				goto ERROR;
+			}
+
+			for (i = 0; i < 7; i++) {
+				if (get_user(value, values)) {
+					PERROR
+					    ("Can't copy value from user space.\n");
+					err = ME_ERRNO_INTERNAL;
+					goto ERROR;
+				}
+
+				if (i == 0) {
+					/* Maximum voltage */
+					value <<= 16;
+					value |=
+					    inl(instance->reg_base +
+						0xD4) & 0xFFFF;
+					outl(value, instance->reg_base + 0xD4);
+				} else if (i == 1) {
+					/* Minimum voltage */
+					value &= 0xFFFF;
+					value |=
+					    inl(instance->reg_base +
+						0xD4) & 0xFFFF0000;
+					outl(value, instance->reg_base + 0xD4);
+				} else if (i == 2) {
+					/* Delta up */
+					value <<= 16;
+					value |=
+					    inl(instance->reg_base +
+						0xD8) & 0xFFFF;
+					outl(value, instance->reg_base + 0xD8);
+				} else if (i == 3) {
+					/* Delta down */
+					value &= 0xFFFF;
+					value |=
+					    inl(instance->reg_base +
+						0xD8) & 0xFFFF0000;
+					outl(value, instance->reg_base + 0xD8);
+				} else if (i == 4) {
+					/* Start value */
+					outl(value, instance->reg_base + 0xDC);
+				} else if (i == 5) {
+					/* Invert */
+					if (value) {
+						value = inl(instance->ctrl_reg);
+						value |= 0x100;
+						outl(value, instance->ctrl_reg);
+					} else {
+						value = inl(instance->ctrl_reg);
+						value &= ~0x100;
+						outl(value, instance->ctrl_reg);
+					}
+				} else if (i == 6) {
+					/* Timer for positive ramp */
+					outl(value, instance->reg_base + 0xE0);
+				}
+
+				values++;
+			}
+		} else {	// Normal firmware
+			PDEBUG("Write for wraparound mode.\n");
+
+			if (inl(instance->status_reg) &
+			    ME4600_AO_STATUS_BIT_FSM) {
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+				PERROR
+				    ("There is already a conversion running.\n");
+				err = ME_ERRNO_SUBDEVICE_BUSY;
+				goto ERROR;
+			}
+
+			tmp |= ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+			tmp &= ~ME4600_AO_CTRL_BIT_ENABLE_FIFO;
+			outl(tmp, instance->ctrl_reg);
+			tmp |= ME4600_AO_CTRL_BIT_ENABLE_FIFO;
+
+			if ((*count > ME4600_AO_FIFO_COUNT) ||
+			    ((instance->
+			      flags & ME4600_AO_FLAGS_SW_WRAP_MODE_MASK) ==
+			     ME4600_AO_FLAGS_SW_WRAP_MODE_FIN)) {
+				tmp &=
+				    ~(ME4600_AO_CTRL_BIT_MODE_0 |
+				      ME4600_AO_CTRL_BIT_MODE_1);
+				tmp |= ME4600_AO_CTRL_BIT_MODE_1;
+			}
+
+			outl(tmp, instance->ctrl_reg);
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+
+			if ((*count <= ME4600_AO_FIFO_COUNT) &&
+			    ((instance->
+			      flags & ME4600_AO_FLAGS_SW_WRAP_MODE_MASK) ==
+			     ME4600_AO_FLAGS_SW_WRAP_MODE_INF)) {
+				for (i = 0; i < *count; i++) {
+					if (get_user(value, values + i)) {
+						PERROR
+						    ("Cannot copy value from user space.\n");
+						err = ME_ERRNO_INTERNAL;
+						goto ERROR;
+					}
+
+					if (instance->ao_idx & 0x1)
+						value <<= 16;
+
+					outl(value, instance->fifo_reg);
+				}
+			} else if ((*count <= ME4600_AO_CIRC_BUF_COUNT) &&
+				   ((instance->
+				     flags & ME4600_AO_FLAGS_SW_WRAP_MODE_MASK)
+				    == ME4600_AO_FLAGS_SW_WRAP_MODE_INF)) {
+				for (i = 0; i < *count; i++) {
+					if (get_user(value, values + i)) {
+						PERROR
+						    ("Cannot copy value from user space.\n");
+						err = ME_ERRNO_INTERNAL;
+						goto ERROR;
+					}
+
+					instance->circ_buf.buf[i] = value;	/* Used to hold the values. */
+				}
+
+				instance->circ_buf.tail = 0;	/* Used as the current read position. */
+				instance->circ_buf.head = *count;	/* Used as the buffer size. */
+
+				/* Preload the FIFO. */
+
+				for (i = 0; i < ME4600_AO_FIFO_COUNT;
+				     i++, instance->circ_buf.tail++) {
+					if (instance->circ_buf.tail >=
+					    instance->circ_buf.head)
+						instance->circ_buf.tail = 0;
+
+					if (instance->ao_idx & 0x1)
+						outl(instance->circ_buf.
+						     buf[instance->circ_buf.
+							 tail] << 16,
+						     instance->fifo_reg);
+					else
+						outl(instance->circ_buf.
+						     buf[instance->circ_buf.
+							 tail],
+						     instance->fifo_reg);
+				}
+			} else if ((*count <= ME4600_AO_CIRC_BUF_COUNT) &&
+				   ((instance->
+				     flags & ME4600_AO_FLAGS_SW_WRAP_MODE_MASK)
+				    == ME4600_AO_FLAGS_SW_WRAP_MODE_FIN)) {
+				unsigned int preload_count;
+
+				for (i = 0; i < *count; i++) {
+					if (get_user(value, values + i)) {
+						PERROR
+						    ("Cannot copy value from user space.\n");
+						err = ME_ERRNO_INTERNAL;
+						goto ERROR;
+					}
+
+					instance->circ_buf.buf[i] = value;	/* Used to hold the values. */
+				}
+
+				instance->circ_buf.tail = 0;	/* Used as the current read position. */
+				instance->circ_buf.head = *count;	/* Used as the buffer size. */
+
+				/* Try to preload the whole FIFO. */
+				preload_count = ME4600_AO_FIFO_COUNT;
+
+				if (preload_count > instance->wrap_count)
+					preload_count = instance->wrap_count;
+
+				/* Preload the FIFO. */
+				for (i = 0; i < preload_count;
+				     i++, instance->circ_buf.tail++) {
+					if (instance->circ_buf.tail >=
+					    instance->circ_buf.head)
+						instance->circ_buf.tail = 0;
+
+					if (instance->ao_idx & 0x1)
+						outl(instance->circ_buf.
+						     buf[instance->circ_buf.
+							 tail] << 16,
+						     instance->fifo_reg);
+					else
+						outl(instance->circ_buf.
+						     buf[instance->circ_buf.
+							 tail],
+						     instance->fifo_reg);
+				}
+
+				instance->wrap_remaining =
+				    instance->wrap_count - preload_count;
+			} else {
+				PERROR("To many values written.\n");
+				err = ME_ERRNO_INVALID_VALUE_COUNT;
+				goto ERROR;
+			}
+		}
+
+		break;
+
+	case 2:		// Continuous mode
+		/* Check if in SW wrapround mode */
+		if (instance->flags & ME4600_AO_FLAGS_SW_WRAP_MODE_MASK) {
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+			PERROR("Subdevice is configured SW wrapround mode.\n");
+			err = ME_ERRNO_PREVIOUS_CONFIG;
+			goto ERROR;
+		}
+
+		switch (write_mode) {
+
+		case ME_WRITE_MODE_BLOCKING:
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+
+			PDEBUG("Write for blocking continuous mode.\n");
+
+			while (cnt > 0) {
+				wait_event_interruptible(instance->wait_queue,
+							 (c =
+							  me_circ_buf_space_to_end
+							  (&instance->
+							   circ_buf)));
+
+				if (instance->
+				    flags & ME4600_AO_FLAGS_BROKEN_PIPE) {
+					PERROR
+					    ("Broken pipe in blocking write.\n");
+					err = ME_ERRNO_SUBDEVICE_NOT_RUNNING;
+					goto ERROR;
+				} else if (signal_pending(current)) {
+					PERROR
+					    ("Wait for free buffer interrupted from signal.\n");
+					err = ME_ERRNO_SIGNAL;
+					goto ERROR;
+				}
+
+				PDEBUG("Space to end = %d.\n", c);
+
+				/* Only able to write size of free buffer or size of count */
+
+				if (cnt < c)
+					c = cnt;
+				k = sizeof(int) * c;
+				k -= copy_from_user(instance->circ_buf.buf +
+						    instance->circ_buf.head,
+						    values, k);
+				c = k / sizeof(int);
+
+				PDEBUG("Copy %d values from user space.\n", c);
+
+				if (!c) {
+					PERROR
+					    ("Cannot copy values from user space.\n");
+					err = ME_ERRNO_INTERNAL;
+					goto ERROR;
+				}
+
+				instance->circ_buf.head =
+				    (instance->circ_buf.head +
+				     c) & (instance->circ_buf.mask);
+
+				values += c;
+				cnt -= c;
+				ret += c;
+
+				/* Values are now available so enable interrupts */
+				spin_lock_irqsave(&instance->subdevice_lock,
+						  cpu_flags);
+
+				if (me_circ_buf_space(&instance->circ_buf)) {
+					tmp = inl(instance->ctrl_reg);
+					tmp |= ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+					outl(tmp, instance->ctrl_reg);
+				}
+
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+			}
+
+			*count = ret;
+
+			break;
+
+		case ME_WRITE_MODE_NONBLOCKING:
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+
+			PDEBUG("Write for non blocking continuous mode.\n");
+
+			while (cnt > 0) {
+				if (instance->
+				    flags & ME4600_AO_FLAGS_BROKEN_PIPE) {
+					PERROR
+					    ("ME4600:Broken pipe in nonblocking write.\n");
+					err = ME_ERRNO_SUBDEVICE_NOT_RUNNING;
+					goto ERROR;
+				}
+
+				c = me_circ_buf_space_to_end(&instance->
+							     circ_buf);
+
+				if (!c) {
+					PDEBUG
+					    ("Returning from nonblocking write.\n");
+					break;
+				}
+
+				PDEBUG("Space to end = %d.\n", c);
+
+				/* Only able to write size of free buffer or size of count */
+
+				if (cnt < c)
+					c = cnt;
+				k = sizeof(int) * c;
+				k -= copy_from_user(instance->circ_buf.buf +
+						    instance->circ_buf.head,
+						    values, k);
+				c = k / sizeof(int);
+
+				PDEBUG("Copy %d values from user space.\n", c);
+
+				if (!c) {
+					PERROR
+					    ("Cannot copy values from user space.\n");
+					err = ME_ERRNO_INTERNAL;
+					goto ERROR;
+				}
+
+				instance->circ_buf.head =
+				    (instance->circ_buf.head +
+				     c) & (instance->circ_buf.mask);
+
+				values += c;
+				cnt -= c;
+				ret += c;
+
+				/* Values are now available so enable interrupts */
+				spin_lock_irqsave(&instance->subdevice_lock,
+						  cpu_flags);
+
+				if (me_circ_buf_space(&instance->circ_buf)) {
+					tmp = inl(instance->ctrl_reg);
+					tmp |= ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+					outl(tmp, instance->ctrl_reg);
+				}
+
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+			}
+
+			*count = ret;
+
+			break;
+
+		case ME_WRITE_MODE_PRELOAD:
+			PDEBUG("Write for preload continuous mode.\n");
+
+			if ((inl(instance->status_reg) &
+			     ME4600_AO_STATUS_BIT_FSM)) {
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+				PERROR
+				    ("Can't Preload DAC FIFO while conversion is running.\n");
+				err = ME_ERRNO_SUBDEVICE_BUSY;
+				goto ERROR;
+			}
+
+			tmp = inl(instance->ctrl_reg);
+
+			tmp |=
+			    ME4600_AO_CTRL_BIT_STOP |
+			    ME4600_AO_CTRL_BIT_IMMEDIATE_STOP;
+			outl(tmp, instance->ctrl_reg);
+			tmp &=
+			    ~(ME4600_AO_CTRL_BIT_ENABLE_FIFO |
+			      ME4600_AO_CTRL_BIT_ENABLE_IRQ);
+			outl(tmp, instance->ctrl_reg);
+			tmp |= ME4600_AO_CTRL_BIT_ENABLE_FIFO;
+			outl(tmp, instance->ctrl_reg);
+
+			instance->circ_buf.head = 0;
+			instance->circ_buf.tail = 0;
+			instance->flags &= ~ME4600_AO_FLAGS_BROKEN_PIPE;
+
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+
+			c = ME4600_AO_FIFO_COUNT;
+
+			if (cnt < c)
+				c = cnt;
+
+			for (i = 0; i < c; i++) {
+				if (get_user(value, values)) {
+					PERROR
+					    ("Can't copy value from user space.\n");
+					err = ME_ERRNO_INTERNAL;
+					goto ERROR;
+				}
+
+				if (instance->ao_idx & 0x1)
+					value <<= 16;
+
+				outl(value, instance->fifo_reg);
+
+				values++;
+			}
+
+			cnt -= c;
+
+			ret += c;
+
+			PDEBUG("Wrote %d values to fifo.\n", c);
+
+			while (1) {
+				c = me_circ_buf_space_to_end(&instance->
+							     circ_buf);
+
+				if (c == 0)
+					break;
+
+				if (cnt < c)
+					c = cnt;
+
+				if (c <= 0)
+					break;
+
+				k = sizeof(int) * c;
+
+				k -= copy_from_user(instance->circ_buf.buf +
+						    instance->circ_buf.head,
+						    values, k);
+
+				c = k / sizeof(int);
+
+				PDEBUG("Wrote %d values to circular buffer.\n",
+				       c);
+
+				if (!c) {
+					PERROR
+					    ("Can't copy values from user space.\n");
+					err = ME_ERRNO_INTERNAL;
+					goto ERROR;
+				}
+
+				instance->circ_buf.head =
+				    (instance->circ_buf.head +
+				     c) & (instance->circ_buf.mask);
+
+				values += c;
+				cnt -= c;
+				ret += c;
+			}
+
+			*count = ret;
+
+			break;
+
+		default:
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+
+			PERROR("Invalid write mode specified.\n");
+
+			err = ME_ERRNO_INVALID_WRITE_MODE;
+
+			goto ERROR;
+		}
+
+		break;
+
+	default:		// Single mode of invalid
+		spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+		PERROR("Subdevice is configured in single mode.\n");
+		err = ME_ERRNO_PREVIOUS_CONFIG;
+		goto ERROR;
+	}
+
+      ERROR:
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 19)
+static irqreturn_t me4600_ao_isr(int irq, void *dev_id)
+#else
+static irqreturn_t me4600_ao_isr(int irq, void *dev_id, struct pt_regs *regs)
+#endif
+{
+	unsigned long tmp;
+	int value;
+	me4600_ao_subdevice_t *instance = dev_id;
+	int i;
+	int c = 0;
+	int c1 = 0;
+
+	if (irq != instance->irq) {
+		PDEBUG("Incorrect interrupt num: %d.\n", irq);
+		return IRQ_NONE;
+	}
+
+	if (!((0x1 << (instance->ao_idx + 3)) & inl(instance->irq_status_reg))) {
+		return IRQ_NONE;
+	}
+
+	PDEBUG("executed.\n");
+
+	tmp = inl(instance->status_reg);
+
+	if (!(tmp & ME4600_AO_STATUS_BIT_EF) &&
+	    (tmp & ME4600_AO_STATUS_BIT_HF) &&
+	    (tmp & ME4600_AO_STATUS_BIT_HF)) {
+		c = ME4600_AO_FIFO_COUNT;
+		PDEBUG("Fifo empty.\n");
+	} else if ((tmp & ME4600_AO_STATUS_BIT_EF) &&
+		   (tmp & ME4600_AO_STATUS_BIT_HF) &&
+		   (tmp & ME4600_AO_STATUS_BIT_HF)) {
+		c = ME4600_AO_FIFO_COUNT / 2;
+		PDEBUG("Fifo under half full.\n");
+	} else {
+		c = 0;
+		PDEBUG("Fifo full.\n");
+	}
+
+	PDEBUG("Try to write 0x%04X values.\n", c);
+
+	if ((instance->flags & ME4600_AO_FLAGS_SW_WRAP_MODE_MASK) ==
+	    ME4600_AO_FLAGS_SW_WRAP_MODE_INF) {
+		while (c) {
+			c1 = c;
+
+			if (c1 > (instance->circ_buf.head - instance->circ_buf.tail))	/* Only up to the end of the buffer */
+				c1 = (instance->circ_buf.head -
+				      instance->circ_buf.tail);
+
+			/* Write the values to the FIFO */
+			for (i = 0; i < c1; i++, instance->circ_buf.tail++, c--) {
+				if (instance->ao_idx & 0x1)
+					outl(instance->circ_buf.
+					     buf[instance->circ_buf.tail] << 16,
+					     instance->fifo_reg);
+				else
+					outl(instance->circ_buf.
+					     buf[instance->circ_buf.tail],
+					     instance->fifo_reg);
+			}
+
+			if (instance->circ_buf.tail >= instance->circ_buf.head)	/* Start from beginning */
+				instance->circ_buf.tail = 0;
+		}
+
+		spin_lock(&instance->subdevice_lock);
+
+		tmp = inl(instance->ctrl_reg);
+		tmp |= ME4600_AO_CTRL_BIT_RESET_IRQ;
+		outl(tmp, instance->ctrl_reg);
+		tmp &= ~ME4600_AO_CTRL_BIT_RESET_IRQ;
+		outl(tmp, instance->ctrl_reg);
+
+		if (!(inl(instance->status_reg) & ME4600_AO_STATUS_BIT_FSM)) {
+			PERROR("Broken pipe.\n");
+			instance->flags |= ME4600_AO_FLAGS_BROKEN_PIPE;
+			tmp &= ~ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+			outl(tmp, instance->ctrl_reg);
+		}
+
+		spin_unlock(&instance->subdevice_lock);
+	} else if ((instance->flags & ME4600_AO_FLAGS_SW_WRAP_MODE_MASK) ==
+		   ME4600_AO_FLAGS_SW_WRAP_MODE_FIN) {
+		while (c && instance->wrap_remaining) {
+			c1 = c;
+
+			if (c1 > (instance->circ_buf.head - instance->circ_buf.tail))	/* Only up to the end of the buffer */
+				c1 = (instance->circ_buf.head -
+				      instance->circ_buf.tail);
+
+			if (c1 > instance->wrap_remaining)	/* Only up to count of user defined number of values */
+				c1 = instance->wrap_remaining;
+
+			/* Write the values to the FIFO */
+			for (i = 0; i < c1;
+			     i++, instance->circ_buf.tail++, c--,
+			     instance->wrap_remaining--) {
+				if (instance->ao_idx & 0x1)
+					outl(instance->circ_buf.
+					     buf[instance->circ_buf.tail] << 16,
+					     instance->fifo_reg);
+				else
+					outl(instance->circ_buf.
+					     buf[instance->circ_buf.tail],
+					     instance->fifo_reg);
+			}
+
+			if (instance->circ_buf.tail >= instance->circ_buf.head)	/* Start from beginning */
+				instance->circ_buf.tail = 0;
+		}
+
+		spin_lock(&instance->subdevice_lock);
+
+		tmp = inl(instance->ctrl_reg);
+
+		if (!instance->wrap_remaining) {
+			PDEBUG("Finite SW wraparound done.\n");
+			tmp &= ~ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+		}
+
+		tmp |= ME4600_AO_CTRL_BIT_RESET_IRQ;
+
+		outl(tmp, instance->ctrl_reg);
+		tmp &= ~ME4600_AO_CTRL_BIT_RESET_IRQ;
+		outl(tmp, instance->ctrl_reg);
+
+		if (!(inl(instance->status_reg) & ME4600_AO_STATUS_BIT_FSM)) {
+			PERROR("Broken pipe.\n");
+			instance->flags |= ME4600_AO_FLAGS_BROKEN_PIPE;
+			tmp &= ~ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+			outl(tmp, instance->ctrl_reg);
+		}
+
+		spin_unlock(&instance->subdevice_lock);
+
+	} else {		/* Regular continuous mode */
+
+		while (1) {
+			c1 = me_circ_buf_values_to_end(&instance->circ_buf);
+			PDEBUG("Values to end = %d.\n", c1);
+
+			if (c1 > c)
+				c1 = c;
+
+			if (c1 <= 0) {
+				PDEBUG("Work done or buffer empty.\n");
+				break;
+			}
+
+			if (instance->ao_idx & 0x1) {
+				for (i = 0; i < c1; i++) {
+					value =
+					    *(instance->circ_buf.buf +
+					      instance->circ_buf.tail +
+					      i) << 16;
+					outl(value, instance->fifo_reg);
+				}
+			} else
+				outsl(instance->fifo_reg,
+				      instance->circ_buf.buf +
+				      instance->circ_buf.tail, c1);
+
+			instance->circ_buf.tail =
+			    (instance->circ_buf.tail +
+			     c1) & (instance->circ_buf.mask);
+
+			PDEBUG("%d values wrote to port 0x%04X.\n", c1,
+			       instance->fifo_reg);
+
+			c -= c1;
+		}
+
+		spin_lock(&instance->subdevice_lock);
+
+		tmp = inl(instance->ctrl_reg);
+
+		if (!me_circ_buf_values(&instance->circ_buf)) {
+			PDEBUG
+			    ("Disable Interrupt because no values left in buffer.\n");
+			tmp &= ~ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+		}
+
+		tmp |= ME4600_AO_CTRL_BIT_RESET_IRQ;
+
+		outl(tmp, instance->ctrl_reg);
+		tmp &= ~ME4600_AO_CTRL_BIT_RESET_IRQ;
+		outl(tmp, instance->ctrl_reg);
+
+		if (!(inl(instance->status_reg) & ME4600_AO_STATUS_BIT_FSM)) {
+			PDEBUG("Broken pipe in me4600_ao_isr.\n");
+			instance->flags |= ME4600_AO_FLAGS_BROKEN_PIPE;
+			tmp &= ~ME4600_AO_CTRL_BIT_ENABLE_IRQ;
+			outl(tmp, instance->ctrl_reg);
+		}
+
+		spin_unlock(&instance->subdevice_lock);
+
+		wake_up_interruptible(&instance->wait_queue);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static void me4600_ao_destructor(struct me_subdevice *subdevice)
+{
+	me4600_ao_subdevice_t *instance;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	free_irq(instance->irq, instance);
+	kfree(instance->circ_buf.buf);
+	me_subdevice_deinit(&instance->base);
+	kfree(instance);
+}
+
+me4600_ao_subdevice_t *me4600_ao_constructor(uint32_t reg_base,
+					     spinlock_t * preload_reg_lock,
+					     uint32_t * preload_flags,
+					     int ao_idx, int fifo, int irq)
+{
+	me4600_ao_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me4600_ao_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me4600_ao_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	subdevice->preload_reg_lock = preload_reg_lock;
+	subdevice->preload_flags = preload_flags;
+
+	/* Allocate and initialize circular buffer */
+	subdevice->circ_buf.mask = ME4600_AO_CIRC_BUF_COUNT - 1;
+	subdevice->circ_buf.buf = kmalloc(ME4600_AO_CIRC_BUF_SIZE, GFP_KERNEL);
+
+	if (!subdevice->circ_buf.buf) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		me_subdevice_deinit((me_subdevice_t *) subdevice);
+		kfree(subdevice);
+		return NULL;
+	}
+
+	memset(subdevice->circ_buf.buf, 0, ME4600_AO_CIRC_BUF_SIZE);
+
+	subdevice->circ_buf.head = 0;
+	subdevice->circ_buf.tail = 0;
+
+	/* Initialize wait queue */
+	init_waitqueue_head(&subdevice->wait_queue);
+
+	/* Initialize single value to 0V */
+	subdevice->single_value = 0x8000;
+
+	/* Store analog output index */
+	subdevice->ao_idx = ao_idx;
+
+	/* Store if analog output has fifo */
+	subdevice->fifo = fifo;
+
+	/* Initialize registers */
+
+	if (ao_idx == 0) {
+		subdevice->ctrl_reg = reg_base + ME4600_AO_00_CTRL_REG;
+		subdevice->status_reg = reg_base + ME4600_AO_00_STATUS_REG;
+		subdevice->fifo_reg = reg_base + ME4600_AO_00_FIFO_REG;
+		subdevice->single_reg = reg_base + ME4600_AO_00_SINGLE_REG;
+		subdevice->timer_reg = reg_base + ME4600_AO_00_TIMER_REG;
+		subdevice->reg_base = reg_base;
+
+		if (inl(subdevice->reg_base + ME4600_AO_BOSCH_REG) == 0x20000) {
+			PINFO("Bosch firmware in use for channel 0.\n");
+			subdevice->bosch_fw = 1;
+		} else {
+			subdevice->bosch_fw = 0;
+		}
+	} else if (ao_idx == 1) {
+		subdevice->ctrl_reg = reg_base + ME4600_AO_01_CTRL_REG;
+		subdevice->status_reg = reg_base + ME4600_AO_01_STATUS_REG;
+		subdevice->fifo_reg = reg_base + ME4600_AO_01_FIFO_REG;
+		subdevice->single_reg = reg_base + ME4600_AO_01_SINGLE_REG;
+		subdevice->timer_reg = reg_base + ME4600_AO_01_TIMER_REG;
+		subdevice->reg_base = reg_base;
+		subdevice->bosch_fw = 0;
+	} else if (ao_idx == 2) {
+		subdevice->ctrl_reg = reg_base + ME4600_AO_02_CTRL_REG;
+		subdevice->status_reg = reg_base + ME4600_AO_02_STATUS_REG;
+		subdevice->fifo_reg = reg_base + ME4600_AO_02_FIFO_REG;
+		subdevice->single_reg = reg_base + ME4600_AO_02_SINGLE_REG;
+		subdevice->timer_reg = reg_base + ME4600_AO_02_TIMER_REG;
+		subdevice->reg_base = reg_base;
+		subdevice->bosch_fw = 0;
+	} else {
+		subdevice->ctrl_reg = reg_base + ME4600_AO_03_CTRL_REG;
+		subdevice->status_reg = reg_base + ME4600_AO_03_STATUS_REG;
+		subdevice->fifo_reg = reg_base + ME4600_AO_03_FIFO_REG;
+		subdevice->single_reg = reg_base + ME4600_AO_03_SINGLE_REG;
+		subdevice->timer_reg = reg_base + ME4600_AO_03_TIMER_REG;
+		subdevice->reg_base = reg_base;
+		subdevice->bosch_fw = 0;
+	}
+
+	subdevice->irq_status_reg = reg_base + ME4600_IRQ_STATUS_REG;
+	subdevice->preload_reg = reg_base + ME4600_AO_LOADSETREG_XX;
+
+	/* Register interrupt service routine */
+	subdevice->irq = irq;
+
+	if (request_irq
+	    (subdevice->irq, me4600_ao_isr, SA_INTERRUPT | SA_SHIRQ,
+	     ME4600_NAME, subdevice)) {
+		PERROR("Cannot get interrupt line.\n");
+		me_subdevice_deinit((me_subdevice_t *) subdevice);
+		kfree(subdevice->circ_buf.buf);
+		kfree(subdevice);
+		return NULL;
+	}
+
+	/* Override base class methods. */
+	subdevice->base.me_subdevice_destructor = me4600_ao_destructor;
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me4600_ao_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me4600_ao_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me4600_ao_io_single_read;
+	subdevice->base.me_subdevice_io_single_write =
+	    me4600_ao_io_single_write;
+	subdevice->base.me_subdevice_io_stream_config =
+	    me4600_ao_io_stream_config;
+	subdevice->base.me_subdevice_io_stream_new_values =
+	    me4600_ao_io_stream_new_values;
+	subdevice->base.me_subdevice_io_stream_write =
+	    me4600_ao_io_stream_write;
+	subdevice->base.me_subdevice_io_stream_start =
+	    me4600_ao_io_stream_start;
+	subdevice->base.me_subdevice_io_stream_status =
+	    me4600_ao_io_stream_status;
+	subdevice->base.me_subdevice_io_stream_stop = me4600_ao_io_stream_stop;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me4600_ao_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me4600_ao_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me4600_ao_query_subdevice_caps;
+	subdevice->base.me_subdevice_query_subdevice_caps_args =
+	    me4600_ao_query_subdevice_caps_args;
+	subdevice->base.me_subdevice_query_range_by_min_max =
+	    me4600_ao_query_range_by_min_max;
+	subdevice->base.me_subdevice_query_number_ranges =
+	    me4600_ao_query_number_ranges;
+	subdevice->base.me_subdevice_query_range_info =
+	    me4600_ao_query_range_info;
+	subdevice->base.me_subdevice_query_timer = me4600_ao_query_timer;
+
+	return subdevice;
+}
+
+#endif // BOSCH
+
+/* Common functions
+*/
+
+static int me4600_ao_query_range_by_min_max(me_subdevice_t * subdevice,
+					    int unit,
+					    int *min,
+					    int *max, int *maxdata, int *range)
+{
+	me4600_ao_subdevice_t *instance;
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if ((*max - *min) < 0) {
+		PERROR("Invalid minimum and maximum values specified.\n");
+		return ME_ERRNO_INVALID_MIN_MAX;
+	}
+
+	if ((unit == ME_UNIT_VOLT) || (unit == ME_UNIT_ANY)) {
+		if ((*max <= (ME4600_AO_MAX_RANGE + 1000))
+		    && (*min >= ME4600_AO_MIN_RANGE)) {
+			*min = ME4600_AO_MIN_RANGE;
+			*max = ME4600_AO_MAX_RANGE;
+			*maxdata = ME4600_AO_MAX_DATA;
+			*range = 0;
+		} else {
+			PERROR("No matching range available.\n");
+			return ME_ERRNO_NO_RANGE;
+		}
+	} else {
+		PERROR("Invalid physical unit specified.\n");
+		return ME_ERRNO_INVALID_UNIT;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_ao_query_number_ranges(me_subdevice_t * subdevice,
+					 int unit, int *count)
+{
+	me4600_ao_subdevice_t *instance;
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if ((unit == ME_UNIT_VOLT) || (unit == ME_UNIT_ANY)) {
+		*count = 1;
+	} else {
+		*count = 0;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_ao_query_range_info(me_subdevice_t * subdevice,
+				      int range,
+				      int *unit,
+				      int *min, int *max, int *maxdata)
+{
+	me4600_ao_subdevice_t *instance;
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (range == 0) {
+		*unit = ME_UNIT_VOLT;
+		*min = ME4600_AO_MIN_RANGE;
+		*max = ME4600_AO_MAX_RANGE;
+		*maxdata = ME4600_AO_MAX_DATA;
+	} else {
+		PERROR("Invalid range number specified.\n");
+		return ME_ERRNO_INVALID_RANGE;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_ao_query_timer(me_subdevice_t * subdevice,
+				 int timer,
+				 int *base_frequency,
+				 long long *min_ticks, long long *max_ticks)
+{
+	me4600_ao_subdevice_t *instance;
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if ((timer != ME_TIMER_ACQ_START) && (timer != ME_TIMER_CONV_START)) {
+		PERROR("Invalid timer specified.\n");
+		return ME_ERRNO_INVALID_TIMER;
+	}
+
+	if (instance->fifo) {	//Streaming device.
+		*base_frequency = ME4600_AO_BASE_FREQUENCY;
+		if (timer == ME_TIMER_ACQ_START) {
+			*min_ticks = ME4600_AO_MIN_ACQ_TICKS;
+			*max_ticks = ME4600_AO_MAX_ACQ_TICKS;
+		} else if (timer == ME_TIMER_CONV_START) {
+			*min_ticks = ME4600_AO_MIN_CHAN_TICKS;
+			*max_ticks = ME4600_AO_MAX_CHAN_TICKS;
+		}
+	} else {		//Not streaming device!
+		*base_frequency = 0;
+		*min_ticks = 0;
+		*max_ticks = 0;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_ao_query_number_channels(me_subdevice_t * subdevice,
+					   int *number)
+{
+	me4600_ao_subdevice_t *instance;
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	*number = 1;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_ao_query_subdevice_type(me_subdevice_t * subdevice,
+					  int *type, int *subtype)
+{
+	me4600_ao_subdevice_t *instance;
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	*type = ME_TYPE_AO;
+	*subtype = (instance->fifo) ? ME_SUBTYPE_STREAMING : ME_SUBTYPE_SINGLE;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_ao_query_subdevice_caps(me_subdevice_t * subdevice, int *caps)
+{
+	me4600_ao_subdevice_t *instance;
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	*caps =
+	    ME_CAPS_AO_TRIG_SYNCHRONOUS | ((instance->fifo) ? ME_CAPS_AO_FIFO :
+					   ME_CAPS_NONE);
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_ao_query_subdevice_caps_args(struct me_subdevice *subdevice,
+					       int cap, int *args, int count)
+{
+	me4600_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	instance = (me4600_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (count != 1) {
+		PERROR("Invalid capability argument count.\n");
+		return ME_ERRNO_INVALID_CAP_ARG_COUNT;
+	}
+
+	switch (cap) {
+	case ME_CAP_AI_FIFO_SIZE:
+		args[0] = (instance->fifo) ? ME4600_AO_FIFO_COUNT : 0;
+		break;
+
+	case ME_CAP_AI_BUFFER_SIZE:
+		args[0] =
+		    (instance->circ_buf.buf) ? ME4600_AO_CIRC_BUF_COUNT : 0;
+		break;
+
+	default:
+		PERROR("Invalid capability.\n");
+		err = ME_ERRNO_INVALID_CAP;
+		args[0] = 0;
+	}
+
+	return err;
+}
diff --git a/drivers/staging/meilhaus/me4600_ao.h b/drivers/staging/meilhaus/me4600_ao.h
new file mode 100644
index 00000000000..6fbc4a2dd9d
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_ao.h
@@ -0,0 +1,263 @@
+/**
+ * @file me4600_ao.h
+ *
+ * @brief Meilhaus ME-4000 analog output subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME4600_AO_H_
+# define _ME4600_AO_H_
+
+# include <linux/version.h>
+# include "mesubdevice.h"
+# include "mecirc_buf.h"
+# include "meioctl.h"
+
+# ifdef __KERNEL__
+
+#  ifdef BOSCH
+#   undef ME_SYNAPSE
+#   ifndef _CBUFF_32b_t
+# 	 define _CBUFF_32b_t
+#   endif //_CBUFF_32b_t
+#  endif //BOSCH
+
+#  define ME4600_AO_MAX_SUBDEVICES		4
+#  define ME4600_AO_FIFO_COUNT			4096
+
+#  define ME4600_AO_BASE_FREQUENCY		33000000LL
+
+#  define ME4600_AO_MIN_ACQ_TICKS		0LL
+#  define ME4600_AO_MAX_ACQ_TICKS		0LL
+
+#  define ME4600_AO_MIN_CHAN_TICKS		66LL
+#  define ME4600_AO_MAX_CHAN_TICKS		0xFFFFFFFFLL
+
+#  define ME4600_AO_MIN_RANGE			-10000000
+#  define ME4600_AO_MAX_RANGE			9999694
+
+#  define ME4600_AO_MAX_DATA			0xFFFF
+
+#  ifdef ME_SYNAPSE
+#   define ME4600_AO_CIRC_BUF_SIZE_ORDER 		8	// 2^n PAGES =>> Maximum value of 1MB for Synapse
+#  else
+#   define ME4600_AO_CIRC_BUF_SIZE_ORDER 		5	// 2^n PAGES =>> 128KB
+#  endif
+#  define ME4600_AO_CIRC_BUF_SIZE 		PAGE_SIZE<<ME4600_AO_CIRC_BUF_SIZE_ORDER	// Buffer size in bytes.
+
+#  ifdef _CBUFF_32b_t
+#   define ME4600_AO_CIRC_BUF_COUNT	((ME4600_AO_CIRC_BUF_SIZE) / sizeof(uint32_t))	// Size in values
+#  else
+#   define ME4600_AO_CIRC_BUF_COUNT	((ME4600_AO_CIRC_BUF_SIZE) / sizeof(uint16_t))	// Size in values
+#  endif
+
+#  define ME4600_AO_CONTINOUS					0x0
+#  define ME4600_AO_WRAP_MODE					0x1
+#  define ME4600_AO_HW_MODE						0x2
+
+#  define ME4600_AO_HW_WRAP_MODE				(ME4600_AO_WRAP_MODE | ME4600_AO_HW_MODE)
+#  define ME4600_AO_SW_WRAP_MODE				ME4600_AO_WRAP_MODE
+
+#  define ME4600_AO_INF_STOP_MODE				0x0
+#  define ME4600_AO_ACQ_STOP_MODE				0x1
+#  define ME4600_AO_SCAN_STOP_MODE				0x2
+
+#  ifdef BOSCH			//SPECIAL BUILD FOR BOSCH
+
+/* Bits for flags attribute. */
+#   define ME4600_AO_FLAGS_BROKEN_PIPE			0x1
+#   define ME4600_AO_FLAGS_SW_WRAP_MODE_0		0x2
+#   define ME4600_AO_FLAGS_SW_WRAP_MODE_1		0x4
+#   define ME4600_AO_FLAGS_SW_WRAP_MODE_MASK	(ME4600_AO_FLAGS_SW_WRAP_MODE_0 | ME4600_AO_FLAGS_SW_WRAP_MODE_1)
+
+#   define ME4600_AO_FLAGS_SW_WRAP_MODE_NONE	0x0
+#   define ME4600_AO_FLAGS_SW_WRAP_MODE_INF		0x2
+#   define ME4600_AO_FLAGS_SW_WRAP_MODE_FIN		0x4
+
+	/**
+	* @brief The ME-4000 analog output subdevice class.
+	*/
+typedef struct me4600_ao_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;				/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;			/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *preload_reg_lock;		/**< Spin lock to protect #preload_reg from concurrent access. */
+	uint32_t *preload_flags;
+
+	unsigned int irq;					/**< The interrupt request number assigned by the PCI BIOS. */
+	me_circ_buf_t circ_buf;				/**< Circular buffer holding measurment data. */
+	wait_queue_head_t wait_queue;		/**< Wait queue to put on tasks waiting for data to arrive. */
+
+	int single_value;					/**< Mirror of the value written in single mode. */
+
+	int volatile flags;					/**< Flags used for storing SW wraparound setup and error signalling from ISR. */
+	unsigned int wrap_count;			/**< The user defined wraparound cycle count. */
+	unsigned int wrap_remaining;		/**< The wraparound cycle down counter used by a running conversion. */
+	unsigned int ao_idx;				/**< The index of this analog output on this device. */
+	int fifo;							/**< If set this device has a FIFO. */
+
+	int bosch_fw;						/**< If set the bosch firmware is in PROM. */
+
+	/* Registers */
+	uint32_t ctrl_reg;
+	uint32_t status_reg;
+	uint32_t fifo_reg;
+	uint32_t single_reg;
+	uint32_t timer_reg;
+	uint32_t irq_status_reg;
+	uint32_t preload_reg;
+	uint32_t reg_base;
+} me4600_ao_subdevice_t;
+
+	/**
+	* @brief The constructor to generate a ME-4000 analog output subdevice instance for BOSCH project.
+	*
+	* @param reg_base The register base address of the device as returned by the PCI BIOS.
+	* @param ctrl_reg_lock Pointer to spin lock protecting the control register from concurrent access.
+	* @param preload_flags Pointer to spin lock protecting the hold&trigger register from concurrent access.
+	* @param ao_idx Subdevice number.
+	* @param fifo Flag set if subdevice has hardware FIFO.
+	* @param irq IRQ number.
+	*
+	* @return Pointer to new instance on success.\n
+	* NULL on error.
+	*/
+me4600_ao_subdevice_t *me4600_ao_constructor(uint32_t reg_base,
+					     spinlock_t * preload_reg_lock,
+					     uint32_t * preload_flags,
+					     int ao_idx, int fifo, int irq);
+
+#  else	//~BOSCH
+
+//ME4600_AO_FLAGS_BROKEN_PIPE is OBSOLETE => Now problems are reported in status.
+
+typedef enum ME4600_AO_STATUS {
+	ao_status_none = 0,
+	ao_status_single_configured,
+	ao_status_single_run_wait,
+	ao_status_single_run,
+	ao_status_single_end_wait,
+	ao_status_single_end,
+	ao_status_stream_configured,
+	ao_status_stream_run_wait,
+	ao_status_stream_run,
+	ao_status_stream_end_wait,
+	ao_status_stream_end,
+	ao_status_stream_fifo_error,
+	ao_status_stream_buffer_error,
+	ao_status_stream_error,
+	ao_status_last
+} ME4600_AO_STATUS;
+
+typedef struct me4600_ao_timeout {
+	unsigned long start_time;
+	unsigned long delay;
+} me4600_ao_timeout_t;
+
+	/**
+	* @brief The ME-4600 analog output subdevice class.
+	*/
+typedef struct me4600_ao_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;						/**< The subdevice base class. */
+	unsigned int ao_idx;						/**< The index of this analog output on this device. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;					/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *preload_reg_lock;				/**< Spin lock to protect preload_reg from concurrent access. */
+
+	uint32_t *preload_flags;
+
+	/* Hardware feautres */
+	unsigned int irq;							/**< The interrupt request number assigned by the PCI BIOS. */
+	int fifo;									/**< If set this device has a FIFO. */
+	int bitpattern;								/**< If set this device use bitpattern. */
+
+	int single_value;							/**< Mirror of the output value in single mode. */
+	int single_value_in_fifo;					/**< Mirror of the value written in single mode. */
+	uint32_t ctrl_trg;							/**< Mirror of the trigger settings. */
+
+	volatile int mode;							/**< Flags used for storing SW wraparound setup*/
+	int stop_mode;								/**< The user defined stop condition flag. */
+	unsigned int start_mode;
+	unsigned int stop_count;					/**< The user defined dates presentation end count. */
+	unsigned int stop_data_count;				/**< The stop presentation count. */
+	unsigned int data_count;					/**< The real presentation count. */
+	unsigned int preloaded_count;				/**< The next data addres in buffer. <= for wraparound mode. */
+	int hardware_stop_delay;					/**< The time that stop can take. This is only to not show hardware bug to user. */
+
+	volatile enum ME4600_AO_STATUS status;		/**< The current stream status flag. */
+	me4600_ao_timeout_t timeout;				/**< The timeout for start in blocking and non-blocking mode. */
+
+										/* Registers *//**< All registers are 32 bits long. */
+	unsigned long ctrl_reg;
+	unsigned long status_reg;
+	unsigned long fifo_reg;
+	unsigned long single_reg;
+	unsigned long timer_reg;
+	unsigned long irq_status_reg;
+	unsigned long preload_reg;
+	unsigned long reg_base;
+
+	/* Software buffer */
+	me_circ_buf_t circ_buf;						/**< Circular buffer holding measurment data. 32 bit long */
+	wait_queue_head_t wait_queue;				/**< Wait queue to put on tasks waiting for data to arrive. */
+
+	struct workqueue_struct *me4600_workqueue;
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+	struct work_struct ao_control_task;
+#else
+	struct delayed_work ao_control_task;
+#endif
+
+	volatile int ao_control_task_flag;			/**< Flag controling reexecuting of control task */
+
+} me4600_ao_subdevice_t;
+
+	/**
+	* @brief The constructor to generate a ME-4600 analog output subdevice instance.
+	*
+	* @param reg_base The register base address of the device as returned by the PCI BIOS.
+	* @param ctrl_reg_lock Pointer to spin lock protecting the control register from concurrent access.
+	* @param preload_flags Pointer to spin lock protecting the hold&trigger register from concurrent access.
+	* @param ao_idx Subdevice number.
+	* @param fifo Flag set if subdevice has hardware FIFO.
+	* @param irq IRQ number.
+	* @param me4600_wq Queue for asynchronous task (1 queue for all subdevice on 1 board).
+	*
+	* @return Pointer to new instance on success.\n
+	* NULL on error.
+	*/
+me4600_ao_subdevice_t *me4600_ao_constructor(uint32_t reg_base,
+					     spinlock_t * preload_reg_lock,
+					     uint32_t * preload_flags,
+					     int ao_idx,
+					     int fifo,
+					     int irq,
+					     struct workqueue_struct
+					     *me4600_wq);
+
+#  endif //BOSCH
+# endif	//__KERNEL__
+#endif // ~_ME4600_AO_H_
diff --git a/drivers/staging/meilhaus/me4600_ao_reg.h b/drivers/staging/meilhaus/me4600_ao_reg.h
new file mode 100644
index 00000000000..f83d82ecd4b
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_ao_reg.h
@@ -0,0 +1,113 @@
+/**
+ * @file me4600_ao_reg.h
+ *
+ * @brief ME-4000 analog output subdevice register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME4600_AO_REG_H_
+#define _ME4600_AO_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME4600_AO_00_CTRL_REG					0x00	// R/W
+#define ME4600_AO_00_STATUS_REG					0x04	// R/_
+#define ME4600_AO_00_FIFO_REG					0x08	// _/W
+#define ME4600_AO_00_SINGLE_REG					0x0C	// R/W
+#define ME4600_AO_00_TIMER_REG					0x10	// _/W
+
+#define ME4600_AO_01_CTRL_REG					0x18	// R/W
+#define ME4600_AO_01_STATUS_REG					0x1C	// R/_
+#define ME4600_AO_01_FIFO_REG					0x20	// _/W
+#define ME4600_AO_01_SINGLE_REG					0x24	// R/W
+#define ME4600_AO_01_TIMER_REG					0x28	// _/W
+
+#define ME4600_AO_02_CTRL_REG					0x30	// R/W
+#define ME4600_AO_02_STATUS_REG					0x34	// R/_
+#define ME4600_AO_02_FIFO_REG					0x38	// _/W
+#define ME4600_AO_02_SINGLE_REG					0x3C	// R/W
+#define ME4600_AO_02_TIMER_REG					0x40	// _/W
+
+#define ME4600_AO_03_CTRL_REG					0x48	// R/W
+#define ME4600_AO_03_STATUS_REG					0x4C	// R/_
+#define ME4600_AO_03_FIFO_REG					0x50	// _/W
+#define ME4600_AO_03_SINGLE_REG					0x54	// R/W
+#define ME4600_AO_03_TIMER_REG					0x58	// _/W
+
+#define ME4600_AO_DEMUX_ADJUST_REG				0xBC	// -/W
+#define ME4600_AO_DEMUX_ADJUST_VALUE			0x4C
+
+#ifdef BOSCH
+# define ME4600_AO_BOSCH_REG					0xC4
+
+# define ME4600_AO_LOADSETREG_XX				0xB4	// R/W
+
+# define ME4600_AO_CTRL_BIT_MODE_0				0x001
+# define ME4600_AO_CTRL_BIT_MODE_1				0x002
+# define ME4600_AO_CTRL_MASK_MODE				0x003
+
+#else //~BOSCH
+
+#define ME4600_AO_SYNC_REG						0xB4	// R/W    ///ME4600_AO_SYNC_REG <==> ME4600_AO_PRELOAD_REG <==> ME4600_AO_LOADSETREG_XX
+
+# define ME4600_AO_MODE_SINGLE					0x00000000
+# define ME4600_AO_MODE_WRAPAROUND				0x00000001
+# define ME4600_AO_MODE_CONTINUOUS				0x00000002
+# define ME4600_AO_CTRL_MODE_MASK				(ME4600_AO_MODE_WRAPAROUND | ME4600_AO_MODE_CONTINUOUS)
+#endif //BOSCH
+
+#define ME4600_AO_CTRL_BIT_MODE_WRAPAROUND		ME4600_AO_MODE_WRAPAROUND
+#define ME4600_AO_CTRL_BIT_MODE_CONTINOUS		ME4600_AO_MODE_CONTINUOUS
+#define ME4600_AO_CTRL_BIT_STOP					0x00000004
+#define ME4600_AO_CTRL_BIT_ENABLE_FIFO			0x00000008
+#define ME4600_AO_CTRL_BIT_ENABLE_EX_TRIG		0x00000010
+#define ME4600_AO_CTRL_BIT_EX_TRIG_EDGE			0x00000020
+#define ME4600_AO_CTRL_BIT_IMMEDIATE_STOP		0x00000080
+#define ME4600_AO_CTRL_BIT_ENABLE_DO			0x00000100
+#define ME4600_AO_CTRL_BIT_ENABLE_IRQ			0x00000200
+#define ME4600_AO_CTRL_BIT_RESET_IRQ			0x00000400
+#define ME4600_AO_CTRL_BIT_EX_TRIG_EDGE_BOTH	0x00000800
+/*
+#define ME4600_AO_SYNC_HOLD_0					0x00000001
+#define ME4600_AO_SYNC_HOLD_1					0x00000002
+#define ME4600_AO_SYNC_HOLD_2					0x00000004
+#define ME4600_AO_SYNC_HOLD_3					0x00000008
+*/
+#define ME4600_AO_SYNC_HOLD						0x00000001
+
+/*
+#define ME4600_AO_SYNC_EXT_TRIG_0				0x00010000
+#define ME4600_AO_SYNC_EXT_TRIG_1				0x00020000
+#define ME4600_AO_SYNC_EXT_TRIG_2				0x00040000
+#define ME4600_AO_SYNC_EXT_TRIG_3				0x00080000
+*/
+#define ME4600_AO_SYNC_EXT_TRIG					0x00010000
+
+#define ME4600_AO_EXT_TRIG						0x80000000
+
+#define ME4600_AO_STATUS_BIT_FSM				0x00000001
+#define ME4600_AO_STATUS_BIT_FF					0x00000002
+#define ME4600_AO_STATUS_BIT_HF					0x00000004
+#define ME4600_AO_STATUS_BIT_EF					0x00000008
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me4600_device.c b/drivers/staging/meilhaus/me4600_device.c
new file mode 100644
index 00000000000..fa455844f4e
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_device.c
@@ -0,0 +1,373 @@
+/**
+ * @file me4600_device.c
+ *
+ * @brief ME-4600 device class implementation.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+#ifndef MODULE
+#  define MODULE
+#endif
+
+#include <linux/module.h>
+
+#include <linux/pci.h>
+#include <linux/slab.h>
+
+#include "meids.h"
+#include "meerror.h"
+#include "mecommon.h"
+#include "meinternal.h"
+
+#include "medebug.h"
+#include "medevice.h"
+#include "me4600_device.h"
+#include "meplx_reg.h"
+
+#include "mefirmware.h"
+
+#include "mesubdevice.h"
+#include "me4600_do.h"
+#include "me4600_di.h"
+#include "me4600_dio.h"
+#include "me8254.h"
+#include "me4600_ai.h"
+#include "me4600_ao.h"
+#include "me4600_ext_irq.h"
+
+/**
+ * @brief Global variable.
+ * This is working queue for runing a separate atask that will be responsible for work status (start, stop, timeouts).
+ */
+static struct workqueue_struct *me4600_workqueue;
+
+#ifdef BOSCH
+me_device_t *me4600_pci_constructor(struct pci_dev *pci_device, int me_bosch_fw)
+#else //~BOSCH
+me_device_t *me4600_pci_constructor(struct pci_dev *pci_device)
+#endif				//BOSCH
+{
+	me4600_device_t *me4600_device;
+	me_subdevice_t *subdevice;
+	unsigned int version_idx;
+	int err;
+	int i;
+
+	PDEBUG("executed.\n");
+
+	// Allocate structure for device instance.
+	me4600_device = kmalloc(sizeof(me4600_device_t), GFP_KERNEL);
+
+	if (!me4600_device) {
+		PERROR("Cannot get memory for ME-4600 device instance.\n");
+		return NULL;
+	}
+
+	memset(me4600_device, 0, sizeof(me4600_device_t));
+
+	// Initialize base class structure.
+	err = me_device_pci_init((me_device_t *) me4600_device, pci_device);
+
+	if (err) {
+		kfree(me4600_device);
+		PERROR("Cannot initialize device base class.\n");
+		return NULL;
+	}
+	// Download the xilinx firmware.
+	if (me4600_device->base.info.pci.device_id == PCI_DEVICE_ID_MEILHAUS_ME4610) {	//Jekyll <=> me4610
+		err =
+		    me_xilinx_download(me4600_device->base.info.pci.
+				       reg_bases[1],
+				       me4600_device->base.info.pci.
+				       reg_bases[5], &pci_device->dev,
+				       "me4610.bin");
+	} else {		// General me4600 firmware
+#ifdef BOSCH
+		err =
+		    me_xilinx_download(me4600_device->base.info.pci.
+				       reg_bases[1],
+				       me4600_device->base.info.pci.
+				       reg_bases[5], &pci_device->dev,
+				       (me_bosch_fw) ? "me4600_bosch.bin" :
+				       "me4600.bin");
+#else //~BOSCH
+		err =
+		    me_xilinx_download(me4600_device->base.info.pci.
+				       reg_bases[1],
+				       me4600_device->base.info.pci.
+				       reg_bases[5], &pci_device->dev,
+				       "me4600.bin");
+#endif
+	}
+
+	if (err) {
+		me_device_deinit((me_device_t *) me4600_device);
+		kfree(me4600_device);
+		PERROR("Cannot download firmware.\n");
+		return NULL;
+	}
+	// Get the index in the device version information table.
+	version_idx =
+	    me4600_versions_get_device_index(me4600_device->base.info.pci.
+					     device_id);
+
+	// Initialize spin locks.
+	spin_lock_init(&me4600_device->preload_reg_lock);
+
+	me4600_device->preload_flags = 0;
+
+	spin_lock_init(&me4600_device->dio_lock);
+	spin_lock_init(&me4600_device->ai_ctrl_lock);
+	spin_lock_init(&me4600_device->ctr_ctrl_reg_lock);
+	spin_lock_init(&me4600_device->ctr_clk_src_reg_lock);
+
+	// Create digital input instances.
+	for (i = 0; i < me4600_versions[version_idx].di_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) me4600_di_constructor(me4600_device->
+							     base.info.pci.
+							     reg_bases[2],
+							     &me4600_device->
+							     dio_lock);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me4600_device);
+			kfree(me4600_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me4600_device->base.slist,
+					    subdevice);
+	}
+
+	// Create digital output instances.
+	for (i = 0; i < me4600_versions[version_idx].do_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) me4600_do_constructor(me4600_device->
+							     base.info.pci.
+							     reg_bases[2],
+							     &me4600_device->
+							     dio_lock);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me4600_device);
+			kfree(me4600_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me4600_device->base.slist,
+					    subdevice);
+	}
+
+	// Create digital input/output instances.
+	for (i = 0; i < me4600_versions[version_idx].dio_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) me4600_dio_constructor(me4600_device->
+							      base.info.pci.
+							      reg_bases[2],
+							      me4600_versions
+							      [version_idx].
+							      do_subdevices +
+							      me4600_versions
+							      [version_idx].
+							      di_subdevices + i,
+							      &me4600_device->
+							      dio_lock);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me4600_device);
+			kfree(me4600_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me4600_device->base.slist,
+					    subdevice);
+	}
+
+	// Create analog input instances.
+	for (i = 0; i < me4600_versions[version_idx].ai_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) me4600_ai_constructor(me4600_device->
+							     base.info.pci.
+							     reg_bases[2],
+							     me4600_versions
+							     [version_idx].
+							     ai_channels,
+							     me4600_versions
+							     [version_idx].
+							     ai_ranges,
+							     me4600_versions
+							     [version_idx].
+							     ai_isolated,
+							     me4600_versions
+							     [version_idx].
+							     ai_sh,
+							     me4600_device->
+							     base.irq,
+							     &me4600_device->
+							     ai_ctrl_lock,
+							     me4600_workqueue);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me4600_device);
+			kfree(me4600_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me4600_device->base.slist,
+					    subdevice);
+	}
+
+	// Create analog output instances.
+	for (i = 0; i < me4600_versions[version_idx].ao_subdevices; i++) {
+#ifdef BOSCH
+		subdevice =
+		    (me_subdevice_t *) me4600_ao_constructor(me4600_device->
+							     base.info.pci.
+							     reg_bases[2],
+							     &me4600_device->
+							     preload_reg_lock,
+							     &me4600_device->
+							     preload_flags, i,
+							     me4600_versions
+							     [version_idx].
+							     ao_fifo,
+							     me4600_device->
+							     base.irq);
+#else //~BOSCH
+		subdevice =
+		    (me_subdevice_t *) me4600_ao_constructor(me4600_device->
+							     base.info.pci.
+							     reg_bases[2],
+							     &me4600_device->
+							     preload_reg_lock,
+							     &me4600_device->
+							     preload_flags, i,
+							     me4600_versions
+							     [version_idx].
+							     ao_fifo,
+							     me4600_device->
+							     base.irq,
+							     me4600_workqueue);
+#endif
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me4600_device);
+			kfree(me4600_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me4600_device->base.slist,
+					    subdevice);
+	}
+
+	// Create counter instances.
+	for (i = 0; i < me4600_versions[version_idx].ctr_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) me8254_constructor(me4600_device->base.
+							  info.pci.device_id,
+							  me4600_device->base.
+							  info.pci.reg_bases[3],
+							  0, i,
+							  &me4600_device->
+							  ctr_ctrl_reg_lock,
+							  &me4600_device->
+							  ctr_clk_src_reg_lock);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me4600_device);
+			kfree(me4600_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me4600_device->base.slist,
+					    subdevice);
+	}
+
+	// Create external interrupt instances.
+	for (i = 0; i < me4600_versions[version_idx].ext_irq_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *)
+		    me4600_ext_irq_constructor(me4600_device->base.info.pci.
+					       reg_bases[2],
+					       me4600_device->base.irq,
+					       &me4600_device->ai_ctrl_lock);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me4600_device);
+			kfree(me4600_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me4600_device->base.slist,
+					    subdevice);
+	}
+
+	return (me_device_t *) me4600_device;
+}
+
+// Init and exit of module.
+
+static int __init me4600_init(void)
+{
+	PDEBUG("executed.\n");
+
+#ifndef BOSCH
+	me4600_workqueue = create_singlethread_workqueue("me4600");
+#endif
+	return 0;
+}
+
+static void __exit me4600_exit(void)
+{
+	PDEBUG("executed.\n");
+
+#ifndef BOSCH
+	flush_workqueue(me4600_workqueue);
+	destroy_workqueue(me4600_workqueue);
+#endif
+}
+
+module_init(me4600_init);
+module_exit(me4600_exit);
+
+// Administrative stuff for modinfo.
+MODULE_AUTHOR
+    ("Guenter Gebhardt <g.gebhardt@meilhaus.de> & Krzysztof Gantzke <k.gantzke@meilhaus.de>");
+MODULE_DESCRIPTION("Device Driver Module for ME-46xx Devices");
+MODULE_SUPPORTED_DEVICE("Meilhaus ME-46xx Devices");
+MODULE_LICENSE("GPL");
+
+// Export the constructor.
+EXPORT_SYMBOL(me4600_pci_constructor);
diff --git a/drivers/staging/meilhaus/me4600_device.h b/drivers/staging/meilhaus/me4600_device.h
new file mode 100644
index 00000000000..fa812d4cc6d
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_device.h
@@ -0,0 +1,151 @@
+/**
+ * @file me4600_device.h
+ *
+ * @brief ME-4600 device class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME4600_DEVICE_H
+#define _ME4600_DEVICE_H
+
+#include <linux/pci.h>
+#include <linux/spinlock.h>
+
+#include "medevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief Structure holding ME-4600 device capabilities.
+ */
+typedef struct me4600_version {
+	uint16_t device_id;
+	unsigned int do_subdevices;
+	unsigned int di_subdevices;
+	unsigned int dio_subdevices;
+	unsigned int ctr_subdevices;
+	unsigned int ai_subdevices;
+	unsigned int ai_channels;
+	unsigned int ai_ranges;
+	unsigned int ai_isolated;
+	unsigned int ai_sh;
+	unsigned int ao_subdevices;
+	unsigned int ao_fifo;	//How many devices have FIFO
+	unsigned int ext_irq_subdevices;
+} me4600_version_t;
+
+/**
+ * @brief ME-4600 device capabilities.
+ */
+static me4600_version_t me4600_versions[] = {
+	{PCI_DEVICE_ID_MEILHAUS_ME4610, 0, 0, 4, 3, 1, 16, 1, 0, 0, 0, 0, 1},
+
+	{PCI_DEVICE_ID_MEILHAUS_ME4650, 0, 0, 4, 0, 1, 16, 4, 0, 0, 0, 0, 1},
+
+	{PCI_DEVICE_ID_MEILHAUS_ME4660, 0, 0, 4, 3, 1, 16, 4, 0, 0, 2, 0, 1},
+	{PCI_DEVICE_ID_MEILHAUS_ME4660I, 1, 1, 2, 3, 1, 16, 4, 1, 0, 2, 0, 1},
+	{PCI_DEVICE_ID_MEILHAUS_ME4660S, 0, 0, 4, 3, 1, 16, 4, 0, 1, 2, 0, 1},
+	{PCI_DEVICE_ID_MEILHAUS_ME4660IS, 1, 1, 2, 3, 1, 16, 4, 1, 1, 2, 0, 1},
+
+	{PCI_DEVICE_ID_MEILHAUS_ME4670, 0, 0, 4, 3, 1, 32, 4, 0, 0, 4, 0, 1},
+	{PCI_DEVICE_ID_MEILHAUS_ME4670I, 1, 1, 2, 3, 1, 32, 4, 1, 0, 4, 0, 1},
+	{PCI_DEVICE_ID_MEILHAUS_ME4670S, 0, 0, 4, 3, 1, 32, 4, 0, 1, 4, 0, 1},
+	{PCI_DEVICE_ID_MEILHAUS_ME4670IS, 1, 1, 2, 3, 1, 32, 4, 1, 1, 4, 0, 1},
+
+	{PCI_DEVICE_ID_MEILHAUS_ME4680, 0, 0, 4, 3, 1, 32, 4, 0, 0, 4, 4, 1},
+	{PCI_DEVICE_ID_MEILHAUS_ME4680I, 1, 1, 2, 3, 1, 32, 4, 1, 0, 4, 4, 1},
+	{PCI_DEVICE_ID_MEILHAUS_ME4680S, 0, 0, 4, 3, 1, 32, 4, 0, 1, 4, 4, 1},
+	{PCI_DEVICE_ID_MEILHAUS_ME4680IS, 1, 1, 2, 3, 1, 32, 4, 1, 1, 4, 4, 1},
+
+	{0},
+};
+
+#define ME4600_DEVICE_VERSIONS (sizeof(me4600_versions) / sizeof(me4600_version_t) - 1)	/**< Returns the number of entries in #me4600_versions. */
+
+/**
+ * @brief Returns the index of the device entry in #me4600_versions.
+ *
+ * @param device_id The PCI device id of the device to query.
+ * @return The index of the device in #me4600_versions.
+ */
+static inline unsigned int me4600_versions_get_device_index(uint16_t device_id)
+{
+	unsigned int i;
+	for (i = 0; i < ME4600_DEVICE_VERSIONS; i++)
+		if (me4600_versions[i].device_id == device_id)
+			break;
+	return i;
+}
+
+/**
+ * @brief The ME-4600 device class structure.
+ */
+typedef struct me4600_device {
+	me_device_t base;					/**< The Meilhaus device base class. */
+
+	/* Child class attributes. */
+	spinlock_t preload_reg_lock;		/**< Guards the preload register of the anaolog output devices. */
+	unsigned int preload_flags;			/**< Used in conjunction with #preload_reg_lock. */
+	spinlock_t dio_lock;				/**< Locks the control register of the digital input/output subdevices. */
+	spinlock_t ai_ctrl_lock;			/**< Locks the control register of the analog input subdevice. */
+	spinlock_t ctr_ctrl_reg_lock;		/**< Locks the counter control register. */
+	spinlock_t ctr_clk_src_reg_lock;	/**< Not used on this device but needed for the me8254 subdevice constructor call. */
+} me4600_device_t;
+
+/**
+ * @brief The ME-4600 device class constructor.
+ *
+ * @param pci_device The pci device structure given by the PCI subsystem.
+ * @param me_bosch_fw If set the device shall use the bosch firmware. (Only for special BOSCH build)
+ *
+ * @return On succes a new ME-4600 device instance. \n
+ *         NULL on error.
+ */
+
+#ifdef BOSCH
+/**
+ * @brief The ME-4600 device class constructor.
+ *
+ * @param pci_device The pci device structure given by the PCI subsystem.
+ * @param me_bosch_fw If set the device shall use the bosch firmware.
+ *
+ * @return On succes a new ME-4600 device instance. \n
+ *         NULL on error.
+ */
+me_device_t *me4600_pci_constructor(struct pci_dev *pci_device, int me_bosch_fw)
+    __attribute__ ((weak));
+#else //~BOSCH
+/**
+ * @brief The ME-4600 device class constructor.
+ *
+ * @param pci_device The pci device structure given by the PCI subsystem.
+ *
+ * @return On succes a new ME-4600 device instance. \n
+ *         NULL on error.
+ */
+me_device_t *me4600_pci_constructor(struct pci_dev *pci_device)
+    __attribute__ ((weak));
+#endif
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me4600_di.c b/drivers/staging/meilhaus/me4600_di.c
new file mode 100644
index 00000000000..7e3c9f4d2df
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_di.c
@@ -0,0 +1,256 @@
+/**
+ * @file me4600_di.c
+ *
+ * @brief ME-4000 digital input subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "me4600_dio_reg.h"
+#include "me4600_di.h"
+
+/*
+ * Defines
+ */
+
+/*
+ * Functions
+ */
+
+static int me4600_di_io_reset_subdevice(struct me_subdevice *subdevice,
+					struct file *filep, int flags)
+{
+	me4600_di_subdevice_t *instance;
+	uint32_t mode;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_di_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	mode = inl(instance->ctrl_reg);
+	mode &= ~(ME4600_DIO_CTRL_BIT_MODE_2 | ME4600_DIO_CTRL_BIT_MODE_3);	//0xFFF3
+	outl(mode, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, mode);
+	spin_unlock(instance->ctrl_reg_lock);
+
+	outl(0, instance->port_reg);
+	PDEBUG_REG("port_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->port_reg - instance->reg_base, 0);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_di_io_single_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int single_config,
+				      int ref,
+				      int trig_chan,
+				      int trig_type, int trig_edge, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me4600_di_subdevice_t *instance;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_di_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	switch (flags) {
+	case ME_IO_SINGLE_CONFIG_NO_FLAGS:
+	case ME_IO_SINGLE_CONFIG_DIO_BYTE:
+		if (channel == 0) {
+			if (single_config == ME_SINGLE_CONFIG_DIO_INPUT) {
+			} else {
+				PERROR("Invalid port direction specified.\n");
+				err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		} else {
+			PERROR("Invalid channel number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_di_io_single_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int channel,
+				    int *value, int time_out, int flags)
+{
+	me4600_di_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_di_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			*value = inl(instance->port_reg) & (0x1 << channel);
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			*value = inl(instance->port_reg) & 0xFF;
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_di_query_number_channels(me_subdevice_t * subdevice,
+					   int *number)
+{
+	PDEBUG("executed.\n");
+	*number = 8;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_di_query_subdevice_type(me_subdevice_t * subdevice,
+					  int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_DI;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_di_query_subdevice_caps(me_subdevice_t * subdevice, int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = 0;
+	return ME_ERRNO_SUCCESS;
+}
+
+me4600_di_subdevice_t *me4600_di_constructor(uint32_t reg_base,
+					     spinlock_t * ctrl_reg_lock)
+{
+	me4600_di_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me4600_di_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me4600_di_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	subdevice->ctrl_reg_lock = ctrl_reg_lock;
+
+	/* Save the subdevice index */
+	subdevice->port_reg = reg_base + ME4600_DIO_PORT_1_REG;
+	subdevice->ctrl_reg = reg_base + ME4600_DIO_CTRL_REG;
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = reg_base;
+#endif
+
+	/* Overload base class methods. */
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me4600_di_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me4600_di_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me4600_di_io_single_read;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me4600_di_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me4600_di_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me4600_di_query_subdevice_caps;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me4600_di.h b/drivers/staging/meilhaus/me4600_di.h
new file mode 100644
index 00000000000..ec8b175755b
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_di.h
@@ -0,0 +1,64 @@
+/**
+ * @file me4600_di.h
+ *
+ * @brief ME-4000 digital input subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME4600_DI_H_
+#define _ME4600_DI_H_
+
+#include "mesubdevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The template subdevice class.
+ */
+typedef struct me4600_di_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *ctrl_reg_lock;		/**< Spin lock to protect #ctrl_reg from concurrent access. */
+
+	unsigned long port_reg;			/**< Register holding the port status. */
+	unsigned long ctrl_reg;			/**< Register to configure the port direction. */
+#ifdef MEDEBUG_DEBUG_REG
+	unsigned long reg_base;
+#endif
+} me4600_di_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-4000 digital input subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me4600_di_subdevice_t *me4600_di_constructor(uint32_t reg_base,
+					     spinlock_t * ctrl_reg_lock);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me4600_dio.c b/drivers/staging/meilhaus/me4600_dio.c
new file mode 100644
index 00000000000..0af95d1a8f5
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_dio.c
@@ -0,0 +1,510 @@
+/**
+ * @file me4600_dio.c
+ *
+ * @brief ME-4000 digital input/output subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "me4600_dio_reg.h"
+#include "me4600_dio.h"
+
+/*
+ * Defines
+ */
+
+/*
+ * Functions
+ */
+
+static int me4600_dio_io_reset_subdevice(struct me_subdevice *subdevice,
+					 struct file *filep, int flags)
+{
+	me4600_dio_subdevice_t *instance;
+	uint32_t mode;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_dio_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	/* Set port to input mode */
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	mode = inl(instance->ctrl_reg);
+	mode &=
+	    ~((ME4600_DIO_CTRL_BIT_MODE_0 | ME4600_DIO_CTRL_BIT_MODE_1) <<
+	      (instance->dio_idx * 2));
+	outl(mode, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, mode);
+	spin_unlock(instance->ctrl_reg_lock);
+
+	outl(0, instance->port_reg);
+	PDEBUG_REG("port_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->port_reg - instance->reg_base, 0);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_dio_io_single_config(me_subdevice_t * subdevice,
+				       struct file *filep,
+				       int channel,
+				       int single_config,
+				       int ref,
+				       int trig_chan,
+				       int trig_type, int trig_edge, int flags)
+{
+	me4600_dio_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint32_t mode;
+	uint32_t size =
+	    flags & (ME_IO_SINGLE_CONFIG_DIO_BIT | ME_IO_SINGLE_CONFIG_DIO_BYTE
+		     | ME_IO_SINGLE_CONFIG_DIO_WORD |
+		     ME_IO_SINGLE_CONFIG_DIO_DWORD);
+	uint32_t mask;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_dio_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	mode = inl(instance->ctrl_reg);
+	switch (size) {
+	case ME_IO_SINGLE_CONFIG_NO_FLAGS:
+	case ME_IO_SINGLE_CONFIG_DIO_BYTE:
+		if (channel == 0) {
+			if (single_config == ME_SINGLE_CONFIG_DIO_INPUT) {
+				mode &=
+				    ~((ME4600_DIO_CTRL_BIT_MODE_0 |
+				       ME4600_DIO_CTRL_BIT_MODE_1) <<
+				      (instance->dio_idx * 2));
+			} else if (single_config == ME_SINGLE_CONFIG_DIO_OUTPUT) {
+				mode &=
+				    ~((ME4600_DIO_CTRL_BIT_MODE_0 |
+				       ME4600_DIO_CTRL_BIT_MODE_1) <<
+				      (instance->dio_idx * 2));
+				mode |=
+				    ME4600_DIO_CTRL_BIT_MODE_0 << (instance->
+								   dio_idx * 2);
+			} else if (single_config == ME_SINGLE_CONFIG_DIO_MUX32M) {
+				mask =
+				    (ME4600_DIO_CTRL_BIT_MODE_0 |
+				     ME4600_DIO_CTRL_BIT_MODE_1) << (instance->
+								     dio_idx *
+								     2);
+				mask |=
+				    ME4600_DIO_CTRL_BIT_FUNCTION_0 |
+				    ME4600_DIO_CTRL_BIT_FUNCTION_1;
+				mask |=
+				    ME4600_DIO_CTRL_BIT_FIFO_HIGH_0 <<
+				    instance->dio_idx;
+				mode &= ~mask;
+
+				if (ref == ME_REF_DIO_FIFO_LOW) {
+					mode |=
+					    (ME4600_DIO_CTRL_BIT_MODE_0 |
+					     ME4600_DIO_CTRL_BIT_MODE_1) <<
+					    (instance->dio_idx * 2);
+					mode |= ME4600_DIO_CTRL_BIT_FUNCTION_1;
+				} else if (ref == ME_REF_DIO_FIFO_HIGH) {
+					mode |=
+					    (ME4600_DIO_CTRL_BIT_MODE_0 |
+					     ME4600_DIO_CTRL_BIT_MODE_1) <<
+					    (instance->dio_idx * 2);
+					mode |= ME4600_DIO_CTRL_BIT_FUNCTION_1;
+					mode |=
+					    ME4600_DIO_CTRL_BIT_FIFO_HIGH_0 <<
+					    instance->dio_idx;
+				} else {
+					PERROR
+					    ("Invalid port reference specified.\n");
+					err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+				}
+			} else if (single_config ==
+				   ME_SINGLE_CONFIG_DIO_DEMUX32) {
+				mask =
+				    (ME4600_DIO_CTRL_BIT_MODE_0 |
+				     ME4600_DIO_CTRL_BIT_MODE_1) << (instance->
+								     dio_idx *
+								     2);
+				mask |=
+				    ME4600_DIO_CTRL_BIT_FUNCTION_0 |
+				    ME4600_DIO_CTRL_BIT_FUNCTION_1;
+				mask |=
+				    ME4600_DIO_CTRL_BIT_FIFO_HIGH_0 <<
+				    instance->dio_idx;
+				mode &= ~mask;
+
+				if (ref == ME_REF_DIO_FIFO_LOW) {
+					mode |=
+					    (ME4600_DIO_CTRL_BIT_MODE_0 |
+					     ME4600_DIO_CTRL_BIT_MODE_1) <<
+					    (instance->dio_idx * 2);
+					mode |= ME4600_DIO_CTRL_BIT_FUNCTION_0;
+				} else if (ref == ME_REF_DIO_FIFO_HIGH) {
+					mode |=
+					    (ME4600_DIO_CTRL_BIT_MODE_0 |
+					     ME4600_DIO_CTRL_BIT_MODE_1) <<
+					    (instance->dio_idx * 2);
+					mode |= ME4600_DIO_CTRL_BIT_FUNCTION_0;
+					mode |=
+					    ME4600_DIO_CTRL_BIT_FIFO_HIGH_0 <<
+					    instance->dio_idx;
+				} else {
+					PERROR
+					    ("Invalid port reference specified.\n");
+					err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+				}
+			} else if (single_config ==
+				   ME_SINGLE_CONFIG_DIO_BIT_PATTERN) {
+				mask =
+				    (ME4600_DIO_CTRL_BIT_MODE_0 |
+				     ME4600_DIO_CTRL_BIT_MODE_1) << (instance->
+								     dio_idx *
+								     2);
+				mask |=
+				    ME4600_DIO_CTRL_BIT_FUNCTION_0 |
+				    ME4600_DIO_CTRL_BIT_FUNCTION_1;
+				mask |=
+				    ME4600_DIO_CTRL_BIT_FIFO_HIGH_0 <<
+				    instance->dio_idx;
+				mode &= ~mask;
+
+				if (ref == ME_REF_DIO_FIFO_LOW) {
+					mode |=
+					    (ME4600_DIO_CTRL_BIT_MODE_0 |
+					     ME4600_DIO_CTRL_BIT_MODE_1) <<
+					    (instance->dio_idx * 2);
+				} else if (ref == ME_REF_DIO_FIFO_HIGH) {
+					mode |=
+					    (ME4600_DIO_CTRL_BIT_MODE_0 |
+					     ME4600_DIO_CTRL_BIT_MODE_1) <<
+					    (instance->dio_idx * 2);
+					mode |=
+					    ME4600_DIO_CTRL_BIT_FIFO_HIGH_0 <<
+					    instance->dio_idx;
+				} else {
+					PERROR
+					    ("Invalid port reference specified.\n");
+					err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+				}
+			} else {
+				PERROR
+				    ("Invalid port configuration specified.\n");
+				err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		} else {
+			PERROR("Invalid channel number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid flags.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (!err) {
+		outl(mode, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, mode);
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_dio_io_single_read(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int channel,
+				     int *value, int time_out, int flags)
+{
+	me4600_dio_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint32_t mode;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_dio_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			mode =
+			    inl(instance->
+				ctrl_reg) & ((ME4600_DIO_CTRL_BIT_MODE_0 |
+					      ME4600_DIO_CTRL_BIT_MODE_1) <<
+					     (instance->dio_idx * 2));
+			if ((mode ==
+			     (ME4600_DIO_CTRL_BIT_MODE_0 <<
+			      (instance->dio_idx * 2))) || !mode) {
+				*value =
+				    inl(instance->port_reg) & (0x1 << channel);
+			} else {
+				PERROR("Port not in output or input mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			mode =
+			    inl(instance->
+				ctrl_reg) & ((ME4600_DIO_CTRL_BIT_MODE_0 |
+					      ME4600_DIO_CTRL_BIT_MODE_1) <<
+					     (instance->dio_idx * 2));
+			if ((mode ==
+			     (ME4600_DIO_CTRL_BIT_MODE_0 <<
+			      (instance->dio_idx * 2))) || !mode) {
+				*value = inl(instance->port_reg) & 0xFF;
+			} else {
+				PERROR("Port not in output or input mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_dio_io_single_write(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int value, int time_out, int flags)
+{
+	me4600_dio_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint32_t mode;
+	uint32_t byte;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_dio_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			mode =
+			    inl(instance->
+				ctrl_reg) & ((ME4600_DIO_CTRL_BIT_MODE_0 |
+					      ME4600_DIO_CTRL_BIT_MODE_1) <<
+					     (instance->dio_idx * 2));
+
+			if (mode ==
+			    (ME4600_DIO_CTRL_BIT_MODE_0 <<
+			     (instance->dio_idx * 2))) {
+				byte = inl(instance->port_reg) & 0xFF;
+
+				if (value)
+					byte |= 0x1 << channel;
+				else
+					byte &= ~(0x1 << channel);
+
+				outl(byte, instance->port_reg);
+			} else {
+				PERROR("Port not in output or input mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			mode =
+			    inl(instance->
+				ctrl_reg) & ((ME4600_DIO_CTRL_BIT_MODE_0 |
+					      ME4600_DIO_CTRL_BIT_MODE_1) <<
+					     (instance->dio_idx * 2));
+
+			if (mode ==
+			    (ME4600_DIO_CTRL_BIT_MODE_0 <<
+			     (instance->dio_idx * 2))) {
+				outl(value, instance->port_reg);
+			} else {
+				PERROR("Port not in output or input mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_dio_query_number_channels(me_subdevice_t * subdevice,
+					    int *number)
+{
+	PDEBUG("executed.\n");
+	*number = 8;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_dio_query_subdevice_type(me_subdevice_t * subdevice,
+					   int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_DIO;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_dio_query_subdevice_caps(me_subdevice_t * subdevice,
+					   int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = ME_CAPS_DIO_DIR_BYTE;
+	return ME_ERRNO_SUCCESS;
+}
+
+me4600_dio_subdevice_t *me4600_dio_constructor(uint32_t reg_base,
+					       unsigned int dio_idx,
+					       spinlock_t * ctrl_reg_lock)
+{
+	me4600_dio_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me4600_dio_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me4600_dio_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+	subdevice->ctrl_reg_lock = ctrl_reg_lock;
+
+	/* Save digital i/o index */
+	subdevice->dio_idx = dio_idx;
+
+	/* Save the subdevice index */
+	subdevice->ctrl_reg = reg_base + ME4600_DIO_CTRL_REG;
+	subdevice->port_reg = reg_base + ME4600_DIO_PORT_REG + (dio_idx * 4);
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = reg_base;
+#endif
+
+	/* Overload base class methods. */
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me4600_dio_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me4600_dio_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me4600_dio_io_single_read;
+	subdevice->base.me_subdevice_io_single_write =
+	    me4600_dio_io_single_write;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me4600_dio_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me4600_dio_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me4600_dio_query_subdevice_caps;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me4600_dio.h b/drivers/staging/meilhaus/me4600_dio.h
new file mode 100644
index 00000000000..4625ba91f60
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_dio.h
@@ -0,0 +1,69 @@
+/**
+ * @file me4600_dio.h
+ *
+ * @brief ME-4000 digital input/output subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME4600_DIO_H_
+#define _ME4600_DIO_H_
+
+#include "mesubdevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The template subdevice class.
+ */
+typedef struct me4600_dio_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *ctrl_reg_lock;		/**< Spin lock to protect #ctrl_reg from concurrent access. */
+	unsigned int dio_idx;			/**< The index of the digital i/o on the device. */
+
+	/* Registers */
+	unsigned long port_reg;			/**< Register holding the port status. */
+	unsigned long ctrl_reg;			/**< Register to configure the port direction. */
+#ifdef MEDEBUG_DEBUG_REG
+	unsigned long reg_base;
+#endif
+} me4600_dio_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-4000 digital input/ouput subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ * @param dio_idx The index of the digital i/o port on the device.
+ * @param ctrl_reg_lock Spin lock protecting the control register.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me4600_dio_subdevice_t *me4600_dio_constructor(uint32_t reg_base,
+					       unsigned int dio_idx,
+					       spinlock_t * ctrl_reg_lock);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me4600_dio_reg.h b/drivers/staging/meilhaus/me4600_dio_reg.h
new file mode 100644
index 00000000000..7a4016a80fd
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_dio_reg.h
@@ -0,0 +1,63 @@
+/**
+ * @file me4600_dio_reg.h
+ *
+ * @brief ME-4000 digital input/output subdevice register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME4600_DIO_REG_H_
+#define _ME4600_DIO_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME4600_DIO_PORT_0_REG				0xA0					/**< Port 0 register. */
+#define ME4600_DIO_PORT_1_REG				0xA4					/**< Port 1 register. */
+#define ME4600_DIO_PORT_2_REG				0xA8					/**< Port 2 register. */
+#define ME4600_DIO_PORT_3_REG				0xAC					/**< Port 3 register. */
+
+#define ME4600_DIO_DIR_REG					0xB0					/**< Direction register. */
+#define ME4600_DIO_PORT_REG					ME4600_DIO_PORT_0_REG	/**< Base for port's register. */
+
+#define ME4600_DIO_CTRL_REG					0xB8					/**< Control register. */
+/** Port A - DO */
+#define ME4600_DIO_CTRL_BIT_MODE_0			0x0001
+#define ME4600_DIO_CTRL_BIT_MODE_1			0x0002
+/** Port B - DI */
+#define ME4600_DIO_CTRL_BIT_MODE_2			0x0004
+#define ME4600_DIO_CTRL_BIT_MODE_3			0x0008
+/** Port C - DIO */
+#define ME4600_DIO_CTRL_BIT_MODE_4			0x0010
+#define ME4600_DIO_CTRL_BIT_MODE_5			0x0020
+/** Port D - DIO */
+#define ME4600_DIO_CTRL_BIT_MODE_6			0x0040
+#define ME4600_DIO_CTRL_BIT_MODE_7			0x0080
+
+#define ME4600_DIO_CTRL_BIT_FUNCTION_0		0x0100
+#define ME4600_DIO_CTRL_BIT_FUNCTION_1		0x0200
+
+#define ME4600_DIO_CTRL_BIT_FIFO_HIGH_0		0x0400
+#define ME4600_DIO_CTRL_BIT_FIFO_HIGH_1		0x0800
+#define ME4600_DIO_CTRL_BIT_FIFO_HIGH_2		0x1000
+#define ME4600_DIO_CTRL_BIT_FIFO_HIGH_3		0x2000
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me4600_do.c b/drivers/staging/meilhaus/me4600_do.c
new file mode 100644
index 00000000000..ee591bc1185
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_do.c
@@ -0,0 +1,433 @@
+/**
+ * @file me4600_do.c
+ *
+ * @brief ME-4000 digital output subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "me4600_dio_reg.h"
+#include "me4600_do.h"
+
+/*
+ * Defines
+ */
+
+/*
+ * Functions
+ */
+
+static int me4600_do_io_reset_subdevice(struct me_subdevice *subdevice,
+					struct file *filep, int flags)
+{
+	me4600_do_subdevice_t *instance;
+	uint32_t mode;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_do_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	/* Set port to output mode */
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	mode = inl(instance->ctrl_reg);
+	mode &= ~ME4600_DIO_CTRL_BIT_MODE_1;	//0xFFFD
+	mode |= ME4600_DIO_CTRL_BIT_MODE_0;	//0x1
+	outl(mode, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, mode);
+	spin_unlock(instance->ctrl_reg_lock);
+
+	outl(0, instance->port_reg);
+	PDEBUG_REG("port_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->port_reg - instance->reg_base, 0);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_do_io_single_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int single_config,
+				      int ref,
+				      int trig_chan,
+				      int trig_type, int trig_edge, int flags)
+{
+	me4600_do_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint32_t mode;
+	int size =
+	    flags & (ME_IO_SINGLE_CONFIG_DIO_BIT | ME_IO_SINGLE_CONFIG_DIO_BYTE
+		     | ME_IO_SINGLE_CONFIG_DIO_WORD |
+		     ME_IO_SINGLE_CONFIG_DIO_DWORD);
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_do_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	mode = inl(instance->ctrl_reg);
+
+	switch (size) {
+	case ME_IO_SINGLE_CONFIG_NO_FLAGS:
+	case ME_IO_SINGLE_CONFIG_DIO_BYTE:
+		if (channel == 0) {
+			if (single_config == ME_SINGLE_CONFIG_DIO_OUTPUT) {
+				mode &= ~(ME4600_DIO_CTRL_BIT_MODE_0 |
+					  ME4600_DIO_CTRL_BIT_MODE_1);
+				mode |= (ME4600_DIO_CTRL_BIT_MODE_0);
+			} else if (single_config == ME_SINGLE_CONFIG_DIO_MUX32M) {
+				mode &= ~(ME4600_DIO_CTRL_BIT_MODE_0 |
+					  ME4600_DIO_CTRL_BIT_MODE_1 |
+					  ME4600_DIO_CTRL_BIT_FUNCTION_0 |
+					  ME4600_DIO_CTRL_BIT_FUNCTION_1 |
+					  ME4600_DIO_CTRL_BIT_FIFO_HIGH_0);
+
+				if (ref == ME_REF_DIO_FIFO_LOW) {
+					mode |= (ME4600_DIO_CTRL_BIT_MODE_0 |
+						 ME4600_DIO_CTRL_BIT_MODE_1 |
+						 ME4600_DIO_CTRL_BIT_FUNCTION_1);
+				} else if (ref == ME_REF_DIO_FIFO_HIGH) {
+					mode |= (ME4600_DIO_CTRL_BIT_MODE_0 |
+						 ME4600_DIO_CTRL_BIT_MODE_1 |
+						 ME4600_DIO_CTRL_BIT_FUNCTION_1
+						 |
+						 ME4600_DIO_CTRL_BIT_FIFO_HIGH_0);
+				} else {
+					PERROR
+					    ("Invalid port reference specified.\n");
+					err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+				}
+			} else if (single_config ==
+				   ME_SINGLE_CONFIG_DIO_DEMUX32) {
+				mode &=
+				    ~(ME4600_DIO_CTRL_BIT_MODE_0 |
+				      ME4600_DIO_CTRL_BIT_MODE_1 |
+				      ME4600_DIO_CTRL_BIT_FUNCTION_0 |
+				      ME4600_DIO_CTRL_BIT_FUNCTION_1 |
+				      ME4600_DIO_CTRL_BIT_FIFO_HIGH_0);
+
+				if (ref == ME_REF_DIO_FIFO_LOW) {
+					mode |= (ME4600_DIO_CTRL_BIT_MODE_0 |
+						 ME4600_DIO_CTRL_BIT_MODE_1 |
+						 ME4600_DIO_CTRL_BIT_FUNCTION_0);
+				} else if (ref == ME_REF_DIO_FIFO_HIGH) {
+					mode |= (ME4600_DIO_CTRL_BIT_MODE_0 |
+						 ME4600_DIO_CTRL_BIT_MODE_1 |
+						 ME4600_DIO_CTRL_BIT_FUNCTION_0
+						 |
+						 ME4600_DIO_CTRL_BIT_FIFO_HIGH_0);
+				} else {
+					PERROR
+					    ("Invalid port reference specified.\n");
+					err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+				}
+			} else if (single_config ==
+				   ME_SINGLE_CONFIG_DIO_BIT_PATTERN) {
+				mode &=
+				    ~(ME4600_DIO_CTRL_BIT_MODE_0 |
+				      ME4600_DIO_CTRL_BIT_MODE_1 |
+				      ME4600_DIO_CTRL_BIT_FUNCTION_0 |
+				      ME4600_DIO_CTRL_BIT_FUNCTION_1 |
+				      ME4600_DIO_CTRL_BIT_FIFO_HIGH_0);
+
+				if (ref == ME_REF_DIO_FIFO_LOW) {
+					mode |= (ME4600_DIO_CTRL_BIT_MODE_0 |
+						 ME4600_DIO_CTRL_BIT_MODE_1);
+				} else if (ref == ME_REF_DIO_FIFO_HIGH) {
+					mode |= (ME4600_DIO_CTRL_BIT_MODE_0 |
+						 ME4600_DIO_CTRL_BIT_MODE_1 |
+						 ME4600_DIO_CTRL_BIT_FIFO_HIGH_0);
+				} else {
+					PERROR
+					    ("Invalid port reference specified.\n");
+					err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+				}
+			} else {
+				PERROR("Invalid port direction specified.\n");
+				err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		} else {
+			PERROR("Invalid channel number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (!err) {
+		outl(mode, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, mode);
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_do_io_single_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int channel,
+				    int *value, int time_out, int flags)
+{
+	me4600_do_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint32_t mode;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_do_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	mode =
+	    inl(instance->
+		ctrl_reg) & (ME4600_DIO_CTRL_BIT_MODE_0 |
+			     ME4600_DIO_CTRL_BIT_MODE_1);
+
+	if (mode == ME4600_DIO_CTRL_BIT_MODE_0) {
+		switch (flags) {
+		case ME_IO_SINGLE_TYPE_DIO_BIT:
+			if ((channel >= 0) && (channel < 8)) {
+				*value =
+				    inl(instance->port_reg) & (0x1 << channel);
+			} else {
+				PERROR("Invalid bit number specified.\n");
+				err = ME_ERRNO_INVALID_CHANNEL;
+			}
+			break;
+
+		case ME_IO_SINGLE_NO_FLAGS:
+		case ME_IO_SINGLE_TYPE_DIO_BYTE:
+			if (channel == 0) {
+				*value = inl(instance->port_reg) & 0xFF;
+			} else {
+				PERROR("Invalid byte number specified.\n");
+				err = ME_ERRNO_INVALID_CHANNEL;
+			}
+			break;
+
+		default:
+			PERROR("Invalid flags specified.\n");
+			err = ME_ERRNO_INVALID_FLAGS;
+		}
+	} else {
+		PERROR("Port not in output mode.\n");
+		err = ME_ERRNO_PREVIOUS_CONFIG;
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_do_io_single_write(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int channel,
+				     int value, int time_out, int flags)
+{
+	me4600_do_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint32_t byte;
+	uint32_t mode;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_do_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	mode =
+	    inl(instance->
+		ctrl_reg) & (ME4600_DIO_CTRL_BIT_MODE_0 |
+			     ME4600_DIO_CTRL_BIT_MODE_1);
+
+	if (mode == ME4600_DIO_CTRL_BIT_MODE_0) {
+		switch (flags) {
+
+		case ME_IO_SINGLE_TYPE_DIO_BIT:
+			if ((channel >= 0) && (channel < 8)) {
+				byte = inl(instance->port_reg) & 0xFF;
+
+				if (value)
+					byte |= 0x1 << channel;
+				else
+					byte &= ~(0x1 << channel);
+
+				outl(byte, instance->port_reg);
+			} else {
+				PERROR("Invalid bit number specified.\n");
+				err = ME_ERRNO_INVALID_CHANNEL;
+			}
+			break;
+
+		case ME_IO_SINGLE_NO_FLAGS:
+		case ME_IO_SINGLE_TYPE_DIO_BYTE:
+			if (channel == 0) {
+				outl(value, instance->port_reg);
+			} else {
+				PERROR("Invalid byte number specified.\n");
+				err = ME_ERRNO_INVALID_CHANNEL;
+			}
+			break;
+
+		default:
+			PERROR("Invalid flags specified.\n");
+			err = ME_ERRNO_INVALID_FLAGS;
+		}
+	} else {
+		PERROR("Port not in output mode.\n");
+		err = ME_ERRNO_PREVIOUS_CONFIG;
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_do_query_number_channels(me_subdevice_t * subdevice,
+					   int *number)
+{
+	PDEBUG("executed.\n");
+	*number = 8;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_do_query_subdevice_type(me_subdevice_t * subdevice,
+					  int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_DO;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_do_query_subdevice_caps(me_subdevice_t * subdevice, int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = 0;
+	return ME_ERRNO_SUCCESS;
+}
+
+me4600_do_subdevice_t *me4600_do_constructor(uint32_t reg_base,
+					     spinlock_t * ctrl_reg_lock)
+{
+	me4600_do_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me4600_do_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me4600_do_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	subdevice->ctrl_reg_lock = ctrl_reg_lock;
+
+	/* Save the subdevice index */
+	subdevice->ctrl_reg = reg_base + ME4600_DIO_CTRL_REG;
+	subdevice->port_reg = reg_base + ME4600_DIO_PORT_0_REG;
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = reg_base;
+#endif
+
+	/* Overload base class methods. */
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me4600_do_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me4600_do_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me4600_do_io_single_read;
+	subdevice->base.me_subdevice_io_single_write =
+	    me4600_do_io_single_write;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me4600_do_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me4600_do_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me4600_do_query_subdevice_caps;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me4600_do.h b/drivers/staging/meilhaus/me4600_do.h
new file mode 100644
index 00000000000..e8385648e92
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_do.h
@@ -0,0 +1,65 @@
+/**
+ * @file me4600_do.h
+ *
+ * @brief ME-4000 digital output subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME4600_DO_H_
+#define _ME4600_DO_H_
+
+#include "mesubdevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The template subdevice class.
+ */
+typedef struct me4600_do_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *ctrl_reg_lock;		/**< Spin lock to protect #ctrl_reg from concurrent access. */
+
+	unsigned long port_reg;			/**< Register holding the port status. */
+	unsigned long ctrl_reg;			/**< Register to configure the port direction. */
+#ifdef MEDEBUG_DEBUG_REG
+	unsigned long reg_base;
+#endif
+} me4600_do_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-4000 digital output subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ * @param ctrl_reg_lock Spin lock protecting the control register.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me4600_do_subdevice_t *me4600_do_constructor(uint32_t reg_base,
+					     spinlock_t * ctrl_reg_lock);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me4600_ext_irq.c b/drivers/staging/meilhaus/me4600_ext_irq.c
new file mode 100644
index 00000000000..8a10dceae32
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_ext_irq.c
@@ -0,0 +1,467 @@
+/**
+ * @file me4600_ext_irq.c
+ *
+ * @brief ME-4000 external interrupt subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/interrupt.h>
+#include <asm/io.h>
+#include <linux/types.h>
+#include <linux/version.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "meids.h"
+#include "me4600_reg.h"
+#include "me4600_ai_reg.h"
+#include "me4600_ext_irq_reg.h"
+#include "me4600_ext_irq.h"
+
+/*
+ * Defines
+ */
+
+/*
+ * Functions
+ */
+
+static int me4600_ext_irq_io_irq_start(me_subdevice_t * subdevice,
+				       struct file *filep,
+				       int channel,
+				       int irq_source,
+				       int irq_edge, int irq_arg, int flags)
+{
+	me4600_ext_irq_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long cpu_flags;
+	uint32_t tmp;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_ext_irq_subdevice_t *) subdevice;
+
+	if (flags & ~ME_IO_IRQ_START_DIO_BIT) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if ((irq_edge != ME_IRQ_EDGE_RISING)
+	    && (irq_edge != ME_IRQ_EDGE_FALLING)
+	    && (irq_edge != ME_IRQ_EDGE_ANY)
+	    ) {
+		PERROR("Invalid irq edge specified.\n");
+		return ME_ERRNO_INVALID_IRQ_EDGE;
+	}
+
+	if (irq_source != ME_IRQ_SOURCE_DIO_LINE) {
+		PERROR("Invalid irq source specified.\n");
+		return ME_ERRNO_INVALID_IRQ_SOURCE;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	tmp = 0x0;		//inl(instance->ext_irq_config_reg);
+
+	if (irq_edge == ME_IRQ_EDGE_RISING) {
+		//tmp &= ~ME4600_EXT_IRQ_CONFIG_MASK;
+		//tmp |= ME4600_EXT_IRQ_CONFIG_MASK_RISING;
+	} else if (irq_edge == ME_IRQ_EDGE_FALLING) {
+		//tmp &= ~ME4600_EXT_IRQ_CONFIG_MASK;
+		//tmp |= ME4600_EXT_IRQ_CONFIG_MASK_FALLING;
+		tmp = ME4600_EXT_IRQ_CONFIG_MASK_FALLING;
+	} else if (irq_edge == ME_IRQ_EDGE_ANY) {
+		//tmp &= ~ME4600_EXT_IRQ_CONFIG_MASK;
+		//tmp |= ME4600_EXT_IRQ_CONFIG_MASK_ANY;
+		tmp = ME4600_EXT_IRQ_CONFIG_MASK_ANY;
+	}
+
+	outl(tmp, instance->ext_irq_config_reg);
+	PDEBUG_REG("ext_irq_config_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->ext_irq_config_reg - instance->reg_base, tmp);
+
+	spin_lock_irqsave(instance->ctrl_reg_lock, cpu_flags);
+	tmp = inl(instance->ctrl_reg);
+	tmp &= ~(ME4600_AI_CTRL_BIT_EX_IRQ | ME4600_AI_CTRL_BIT_EX_IRQ_RESET);
+	tmp |= ME4600_AI_CTRL_BIT_EX_IRQ;
+	outl(tmp, instance->ctrl_reg);
+	spin_unlock_irqrestore(instance->ctrl_reg_lock, cpu_flags);
+	instance->rised = 0;
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ext_irq_io_irq_wait(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int *irq_count,
+				      int *value, int time_out, int flags)
+{
+	me4600_ext_irq_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	long t = 0;
+	unsigned long cpu_flags;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_ext_irq_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid time_out specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	if (time_out) {
+		t = (time_out * HZ) / 1000;
+
+		if (t == 0)
+			t = 1;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	if (instance->rised <= 0) {
+		instance->rised = 0;
+		if (time_out) {
+			t = wait_event_interruptible_timeout(instance->
+							     wait_queue,
+							     (instance->rised !=
+							      0), t);
+
+			if (t == 0) {
+				PERROR
+				    ("Wait on external interrupt timed out.\n");
+				err = ME_ERRNO_TIMEOUT;
+			}
+		} else {
+			wait_event_interruptible(instance->wait_queue,
+						 (instance->rised != 0));
+		}
+
+		if (instance->rised < 0) {
+			PERROR("Wait on interrupt aborted by user.\n");
+			err = ME_ERRNO_CANCELLED;
+		}
+	}
+
+	if (signal_pending(current)) {
+		PERROR("Wait on external interrupt aborted by signal.\n");
+		err = ME_ERRNO_SIGNAL;
+	}
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	instance->rised = 0;
+	*irq_count = instance->count;
+	*value = instance->value;
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ext_irq_io_irq_stop(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel, int flags)
+{
+	me4600_ext_irq_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long cpu_flags;
+	uint32_t tmp;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_ext_irq_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	spin_lock(instance->ctrl_reg_lock);
+	tmp = inl(instance->ctrl_reg);
+	tmp &= ~(ME4600_AI_CTRL_BIT_EX_IRQ | ME4600_AI_CTRL_BIT_EX_IRQ_RESET);
+	outl(tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_regv outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp);
+	spin_unlock(instance->ctrl_reg_lock);
+	instance->rised = -1;
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me4600_ext_irq_io_reset_subdevice(me_subdevice_t * subdevice,
+					     struct file *filep, int flags)
+{
+	me4600_ext_irq_subdevice_t *instance;
+	unsigned long cpu_flags;
+	uint32_t tmp;
+
+	PDEBUG("executed.\n");
+
+	instance = (me4600_ext_irq_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	spin_lock(instance->ctrl_reg_lock);
+	tmp = inl(instance->ctrl_reg);
+	tmp &= ~(ME4600_AI_CTRL_BIT_EX_IRQ | ME4600_AI_CTRL_BIT_EX_IRQ_RESET);
+	outl(tmp, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_regv outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, tmp);
+	spin_unlock(instance->ctrl_reg_lock);
+	instance->rised = -1;
+	instance->count = 0;
+	outl(ME4600_EXT_IRQ_CONFIG_MASK_ANY, instance->ext_irq_config_reg);
+	PDEBUG_REG("ext_irq_config_reg outl(0x%lX+0x%lX)=0x%x\n",
+		   instance->reg_base,
+		   instance->ext_irq_config_reg - instance->reg_base,
+		   ME4600_EXT_IRQ_CONFIG_MASK_ANY);
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static void me4600_ext_irq_destructor(struct me_subdevice *subdevice)
+{
+	me4600_ext_irq_subdevice_t *instance;
+
+	PDEBUG("executed.\n");
+	instance = (me4600_ext_irq_subdevice_t *) subdevice;
+	me_subdevice_deinit(&instance->base);
+	free_irq(instance->irq, instance);
+	kfree(instance);
+}
+
+static int me4600_ext_irq_query_number_channels(me_subdevice_t * subdevice,
+						int *number)
+{
+	PDEBUG("executed.\n");
+	*number = 1;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_ext_irq_query_subdevice_type(me_subdevice_t * subdevice,
+					       int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_EXT_IRQ;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_ext_irq_query_subdevice_caps(me_subdevice_t * subdevice,
+					       int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps =
+	    ME_CAPS_EXT_IRQ_EDGE_RISING | ME_CAPS_EXT_IRQ_EDGE_FALLING |
+	    ME_CAPS_EXT_IRQ_EDGE_ANY;
+	return ME_ERRNO_SUCCESS;
+}
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 19)
+static irqreturn_t me4600_ext_irq_isr(int irq, void *dev_id)
+#else
+static irqreturn_t me4600_ext_irq_isr(int irq, void *dev_id,
+				      struct pt_regs *regs)
+#endif
+{
+	me4600_ext_irq_subdevice_t *instance;
+	uint32_t ctrl;
+	uint32_t irq_status;
+
+	instance = (me4600_ext_irq_subdevice_t *) dev_id;
+
+	if (irq != instance->irq) {
+		PERROR("Incorrect interrupt num: %d.\n", irq);
+		return IRQ_NONE;
+	}
+
+	irq_status = inl(instance->irq_status_reg);
+	if (!(irq_status & ME4600_IRQ_STATUS_BIT_EX)) {
+		PINFO("%ld Shared interrupt. %s(): irq_status_reg=0x%04X\n",
+		      jiffies, __FUNCTION__, irq_status);
+		return IRQ_NONE;
+	}
+
+	PDEBUG("executed.\n");
+
+	spin_lock(&instance->subdevice_lock);
+	instance->rised = 1;
+	instance->value = inl(instance->ext_irq_value_reg);
+	instance->count++;
+
+	spin_lock(instance->ctrl_reg_lock);
+	ctrl = inl(instance->ctrl_reg);
+	ctrl |= ME4600_AI_CTRL_BIT_EX_IRQ_RESET;
+	outl(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl);
+	ctrl &= ~ME4600_AI_CTRL_BIT_EX_IRQ_RESET;
+	outl(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl);
+	spin_unlock(instance->ctrl_reg_lock);
+
+	spin_unlock(&instance->subdevice_lock);
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	return IRQ_HANDLED;
+}
+
+me4600_ext_irq_subdevice_t *me4600_ext_irq_constructor(uint32_t reg_base,
+						       int irq,
+						       spinlock_t *
+						       ctrl_reg_lock)
+{
+	me4600_ext_irq_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me4600_ext_irq_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me4600_ext_irq_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	subdevice->ctrl_reg_lock = ctrl_reg_lock;
+
+	/* Initialize wait queue */
+	init_waitqueue_head(&subdevice->wait_queue);
+
+	/* Register interrupt */
+	subdevice->irq = irq;
+
+	if (request_irq(subdevice->irq, me4600_ext_irq_isr,
+#ifdef IRQF_DISABLED
+			IRQF_DISABLED | IRQF_SHARED,
+#else
+			SA_INTERRUPT | SA_SHIRQ,
+#endif
+			ME4600_NAME, subdevice)) {
+		PERROR("Cannot register interrupt.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	PINFO("Registered irq=%d.\n", subdevice->irq);
+
+	/* Initialize registers */
+	subdevice->irq_status_reg = reg_base + ME4600_IRQ_STATUS_REG;
+	subdevice->ctrl_reg = reg_base + ME4600_AI_CTRL_REG;
+	subdevice->ext_irq_config_reg = reg_base + ME4600_EXT_IRQ_CONFIG_REG;
+	subdevice->ext_irq_value_reg = reg_base + ME4600_EXT_IRQ_VALUE_REG;
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = reg_base;
+#endif
+
+	/* Override base class methods. */
+	subdevice->base.me_subdevice_destructor = me4600_ext_irq_destructor;
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me4600_ext_irq_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_irq_start = me4600_ext_irq_io_irq_start;
+	subdevice->base.me_subdevice_io_irq_wait = me4600_ext_irq_io_irq_wait;
+	subdevice->base.me_subdevice_io_irq_stop = me4600_ext_irq_io_irq_stop;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me4600_ext_irq_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me4600_ext_irq_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me4600_ext_irq_query_subdevice_caps;
+
+	subdevice->rised = 0;
+	subdevice->count = 0;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me4600_ext_irq.h b/drivers/staging/meilhaus/me4600_ext_irq.h
new file mode 100644
index 00000000000..3c7b27f9e5d
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_ext_irq.h
@@ -0,0 +1,78 @@
+/**
+ * @file me4600_ext_irq.h
+ *
+ * @brief Meilhaus ME-4000 external interrupt subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME4600_EXT_IRQ_H_
+#define _ME4600_EXT_IRQ_H_
+
+#include "mesubdevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The subdevice class.
+ */
+typedef struct me4600_ext_irq_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *ctrl_reg_lock;		/**< Spin lock to protect #ctrl_reg from concurrent access. */
+
+	wait_queue_head_t wait_queue;
+
+	int irq;
+
+	int rised;
+	int value;
+	int count;
+
+	unsigned long ctrl_reg;
+	unsigned long irq_status_reg;
+	unsigned long ext_irq_config_reg;
+	unsigned long ext_irq_value_reg;
+#ifdef MEDEBUG_DEBUG_REG
+	unsigned long reg_base;
+#endif
+} me4600_ext_irq_subdevice_t;
+
+/**
+ * @brief The constructor to generate a external interrupt subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ * @param irq The interrupt number assigned by the PCI BIOS.
+ * @param ctrl_reg_lock Pointer to spin lock protecting the control register from concurrent access.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me4600_ext_irq_subdevice_t *me4600_ext_irq_constructor(uint32_t reg_base,
+						       int irq,
+						       spinlock_t *
+						       ctrl_reg_lock);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me4600_ext_irq_reg.h b/drivers/staging/meilhaus/me4600_ext_irq_reg.h
new file mode 100644
index 00000000000..898e1e74d9e
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_ext_irq_reg.h
@@ -0,0 +1,41 @@
+/**
+ * @file me4600_ext_irq_reg.h
+ *
+ * @brief ME-4000 external interrupt subdevice register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME4600_EXT_IRQ_REG_H_
+#define _ME4600_EXT_IRQ_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME4600_EXT_IRQ_CONFIG_REG		0xCC	// R/_
+#define ME4600_EXT_IRQ_VALUE_REG		0xD0	// R/_
+
+#define ME4600_EXT_IRQ_CONFIG_MASK_RISING	0x0
+#define ME4600_EXT_IRQ_CONFIG_MASK_FALLING	0x1
+#define ME4600_EXT_IRQ_CONFIG_MASK_ANY		0x3
+#define ME4600_EXT_IRQ_CONFIG_MASK		0x3
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me4600_reg.h b/drivers/staging/meilhaus/me4600_reg.h
new file mode 100644
index 00000000000..ae152bbc6a3
--- /dev/null
+++ b/drivers/staging/meilhaus/me4600_reg.h
@@ -0,0 +1,46 @@
+/**
+ * @file me4600_reg.h
+ *
+ * @brief ME-4000 register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME4600_REG_H_
+#define _ME4600_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME4600_IRQ_STATUS_REG			0x9C	// R/_
+
+#define ME4600_IRQ_STATUS_BIT_EX		0x01
+#define ME4600_IRQ_STATUS_BIT_LE		0x02
+#define ME4600_IRQ_STATUS_BIT_AI_HF		0x04
+#define ME4600_IRQ_STATUS_BIT_AO_0_HF	0x08
+#define ME4600_IRQ_STATUS_BIT_AO_1_HF	0x10
+#define ME4600_IRQ_STATUS_BIT_AO_2_HF	0x20
+#define ME4600_IRQ_STATUS_BIT_AO_3_HF	0x40
+#define ME4600_IRQ_STATUS_BIT_SC		0x80
+
+#define ME4600_IRQ_STATUS_BIT_AO_HF		ME4600_IRQ_STATUS_BIT_AO_0_HF
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me6000_ao.c b/drivers/staging/meilhaus/me6000_ao.c
new file mode 100644
index 00000000000..3f5ff6d1b99
--- /dev/null
+++ b/drivers/staging/meilhaus/me6000_ao.c
@@ -0,0 +1,3739 @@
+/**
+ * @file me6000_ao.c
+ *
+ * @brief ME-6000 analog output subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/* Includes
+ */
+#include <linux/version.h>
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <asm/uaccess.h>
+#include <linux/types.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+
+#include <linux/workqueue.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "meids.h"
+#include "me6000_reg.h"
+#include "me6000_ao_reg.h"
+#include "me6000_ao.h"
+
+/* Defines
+ */
+
+static int me6000_ao_query_range_by_min_max(me_subdevice_t * subdevice,
+					    int unit,
+					    int *min,
+					    int *max, int *maxdata, int *range);
+
+static int me6000_ao_query_number_ranges(me_subdevice_t * subdevice,
+					 int unit, int *count);
+
+static int me6000_ao_query_range_info(me_subdevice_t * subdevice,
+				      int range,
+				      int *unit,
+				      int *min, int *max, int *maxdata);
+
+static int me6000_ao_query_timer(me_subdevice_t * subdevice,
+				 int timer,
+				 int *base_frequency,
+				 long long *min_ticks, long long *max_ticks);
+
+static int me6000_ao_query_number_channels(me_subdevice_t * subdevice,
+					   int *number);
+
+static int me6000_ao_query_subdevice_type(me_subdevice_t * subdevice,
+					  int *type, int *subtype);
+
+static int me6000_ao_query_subdevice_caps(me_subdevice_t * subdevice,
+					  int *caps);
+
+static int me6000_ao_query_subdevice_caps_args(struct me_subdevice *subdevice,
+					       int cap, int *args, int count);
+
+/** Remove subdevice. */
+static void me6000_ao_destructor(struct me_subdevice *subdevice);
+
+/** Reset subdevice. Stop all actions. Reset registry. Disable FIFO. Set output to 0V and status to 'none'. */
+static int me6000_ao_io_reset_subdevice(me_subdevice_t * subdevice,
+					struct file *filep, int flags);
+
+/** Set output as single */
+static int me6000_ao_io_single_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int single_config,
+				      int ref,
+				      int trig_chan,
+				      int trig_type, int trig_edge, int flags);
+
+/** Pass to user actual value of output. */
+static int me6000_ao_io_single_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int channel,
+				    int *value, int time_out, int flags);
+
+/** Write to output requed value. */
+static int me6000_ao_io_single_write(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int channel,
+				     int value, int time_out, int flags);
+
+/** Set output as streamed device. */
+static int me6000_ao_io_stream_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      meIOStreamConfig_t * config_list,
+				      int count,
+				      meIOStreamTrigger_t * trigger,
+				      int fifo_irq_threshold, int flags);
+
+/** Wait for / Check empty space in buffer. */
+static int me6000_ao_io_stream_new_values(me_subdevice_t * subdevice,
+					  struct file *filep,
+					  int time_out, int *count, int flags);
+
+/** Start streaming. */
+static int me6000_ao_io_stream_start(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int start_mode, int time_out, int flags);
+
+/** Check actual state. / Wait for end. */
+static int me6000_ao_io_stream_status(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int wait,
+				      int *status, int *values, int flags);
+
+/** Stop streaming. */
+static int me6000_ao_io_stream_stop(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int stop_mode, int flags);
+
+/** Write datas to buffor. */
+static int me6000_ao_io_stream_write(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int write_mode,
+				     int *values, int *count, int flags);
+
+/** Interrupt handler. Copy from buffer to FIFO. */
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 19)
+static irqreturn_t me6000_ao_isr(int irq, void *dev_id);
+#else
+static irqreturn_t me6000_ao_isr(int irq, void *dev_id, struct pt_regs *regs);
+#endif
+
+/** Copy data from circular buffer to fifo (fast) in wraparound mode. */
+int inline ao_write_data_wraparound(me6000_ao_subdevice_t * instance, int count,
+				    int start_pos);
+
+/** Copy data from circular buffer to fifo (fast).*/
+int inline ao_write_data(me6000_ao_subdevice_t * instance, int count,
+			 int start_pos);
+
+/** Copy data from circular buffer to fifo (slow).*/
+int inline ao_write_data_pooling(me6000_ao_subdevice_t * instance, int count,
+				 int start_pos);
+
+/** Copy data from user space to circular buffer. */
+int inline ao_get_data_from_user(me6000_ao_subdevice_t * instance, int count,
+				 int *user_values);
+
+/** Stop presentation. Preserve FIFOs. */
+int inline ao_stop_immediately(me6000_ao_subdevice_t * instance);
+
+/** Function for checking timeout in non-blocking mode. */
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+static void me6000_ao_work_control_task(void *subdevice);
+#else
+static void me6000_ao_work_control_task(struct work_struct *work);
+#endif
+
+/* Functions
+ */
+
+static int me6000_ao_io_reset_subdevice(me_subdevice_t * subdevice,
+					struct file *filep, int flags)
+{
+	me6000_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint32_t tmp;
+	uint32_t ctrl;
+
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	instance->status = ao_status_none;
+	instance->ao_control_task_flag = 0;
+	cancel_delayed_work(&instance->ao_control_task);
+	instance->timeout.delay = 0;
+	instance->timeout.start_time = jiffies;
+
+	//Stop state machine.
+	err = ao_stop_immediately(instance);
+
+	//Remove from synchronous start.
+	spin_lock(instance->preload_reg_lock);
+	tmp = inl(instance->preload_reg);
+	tmp &=
+	    ~((ME6000_AO_SYNC_HOLD | ME6000_AO_SYNC_EXT_TRIG) << instance->
+	      ao_idx);
+	outl(tmp, instance->preload_reg);
+	PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->preload_reg - instance->reg_base, tmp);
+	*instance->preload_flags &=
+	    ~((ME6000_AO_SYNC_HOLD | ME6000_AO_SYNC_EXT_TRIG) << instance->
+	      ao_idx);
+
+	//Reset triggering flag
+	*instance->triggering_flags &= ~(0x1 << instance->ao_idx);
+	spin_unlock(instance->preload_reg_lock);
+
+	if (instance->fifo) {
+		//Set single mode, dissable FIFO, dissable external trigger, block interrupt.
+		ctrl = ME6000_AO_MODE_SINGLE;
+
+		//Block ISM.
+		ctrl |=
+		    (ME6000_AO_CTRL_BIT_STOP |
+		     ME6000_AO_CTRL_BIT_IMMEDIATE_STOP);
+
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+		//Set speed
+		outl(ME6000_AO_MIN_CHAN_TICKS - 1, instance->timer_reg);
+		//Reset interrupt latch
+		inl(instance->irq_reset_reg);
+	}
+
+	instance->hardware_stop_delay = HZ / 10;	//100ms
+
+	//Set output to 0V
+	outl(0x8000, instance->single_reg);
+	PDEBUG_REG("single_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->single_reg - instance->reg_base, 0x8000);
+
+	instance->circ_buf.head = 0;
+	instance->circ_buf.tail = 0;
+	instance->preloaded_count = 0;
+	instance->data_count = 0;
+	instance->single_value = 0x8000;
+	instance->single_value_in_fifo = 0x8000;
+
+	//Set status to signal that device is unconfigured.
+	instance->status = ao_status_none;
+	//Signal reset if user is on wait.
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me6000_ao_io_single_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int single_config,
+				      int ref,
+				      int trig_chan,
+				      int trig_type, int trig_edge, int flags)
+{
+	me6000_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint32_t ctrl;
+	uint32_t sync;
+	unsigned long cpu_flags;
+
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. ID=%d\n", instance->ao_idx);
+
+	// Checking parameters
+	if (flags) {
+		PERROR
+		    ("Invalid flag specified. Must be ME_IO_SINGLE_CONFIG_NO_FLAGS.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (instance->fifo) {	//Stream hardware (with or without fifo)
+		if ((trig_edge == ME_TRIG_TYPE_SW)
+		    && (trig_edge != ME_TRIG_EDGE_NONE)) {
+			PERROR
+			    ("Invalid trigger edge. Software trigger has not edge.\n");
+			return ME_ERRNO_INVALID_TRIG_EDGE;
+		}
+
+		if (trig_type == ME_TRIG_TYPE_EXT_DIGITAL) {
+			switch (trig_edge) {
+			case ME_TRIG_EDGE_ANY:
+			case ME_TRIG_EDGE_RISING:
+			case ME_TRIG_EDGE_FALLING:
+				break;
+
+			default:
+				PERROR("Invalid trigger edge.\n");
+				return ME_ERRNO_INVALID_TRIG_EDGE;
+			}
+		}
+
+		if ((trig_type != ME_TRIG_TYPE_SW)
+		    && (trig_type != ME_TRIG_TYPE_EXT_DIGITAL)) {
+			PERROR
+			    ("Invalid trigger type. Trigger must be software or digital.\n");
+			return ME_ERRNO_INVALID_TRIG_TYPE;
+		}
+	} else {		//Single
+		if (trig_edge != ME_TRIG_EDGE_NONE) {
+			PERROR
+			    ("Invalid trigger edge. Single output trigger hasn't own edge.\n");
+			return ME_ERRNO_INVALID_TRIG_EDGE;
+		}
+
+		if (trig_type != ME_TRIG_TYPE_SW) {
+			PERROR
+			    ("Invalid trigger type. Trigger must be software.\n");
+			return ME_ERRNO_INVALID_TRIG_TYPE;
+		}
+
+	}
+
+	if ((trig_chan != ME_TRIG_CHAN_DEFAULT)
+	    && (trig_chan != ME_TRIG_CHAN_SYNCHRONOUS)) {
+		PERROR("Invalid trigger channel specified.\n");
+		return ME_ERRNO_INVALID_TRIG_CHAN;
+	}
+/*
+	if ((trig_type == ME_TRIG_TYPE_EXT_DIGITAL) && (trig_chan != ME_TRIG_CHAN_SYNCHRONOUS))
+	{
+		PERROR("Invalid trigger channel specified. Must be synchronous when digital is choose.\n");
+		return ME_ERRNO_INVALID_TRIG_CHAN;
+	}
+*/
+	if (ref != ME_REF_AO_GROUND) {
+		PERROR
+		    ("Invalid reference. Analog outputs have to have got REF_AO_GROUND.\n");
+		return ME_ERRNO_INVALID_REF;
+	}
+
+	if (single_config != 0) {
+		PERROR
+		    ("Invalid single config specified. Only one range for anlog outputs is available.\n");
+		return ME_ERRNO_INVALID_SINGLE_CONFIG;
+	}
+
+	if (channel != 0) {
+		PERROR
+		    ("Invalid channel number specified. Analog output have only one channel.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	//Subdevice running in stream mode!
+	if ((instance->status >= ao_status_stream_run_wait)
+	    && (instance->status < ao_status_stream_end)) {
+		PERROR("Subdevice is busy.\n");
+		ME_SUBDEVICE_EXIT;
+
+		return ME_ERRNO_SUBDEVICE_BUSY;
+	}
+/// @note For single all calls (config and write) are erasing previous state!
+
+	instance->status = ao_status_none;
+
+	// Correct single mirrors
+	instance->single_value_in_fifo = instance->single_value;
+
+	//Stop device
+	err = ao_stop_immediately(instance);
+	if (err) {
+		PERROR_CRITICAL("FSM IS BUSY!\n");
+		ME_SUBDEVICE_EXIT;
+
+		return ME_ERRNO_SUBDEVICE_BUSY;
+	}
+
+	if (instance->fifo) {	// Set control register.
+		spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+		// Set stop bit. Stop streaming mode (If running.).
+		ctrl = inl(instance->ctrl_reg);
+		//Reset all bits.
+		ctrl =
+		    ME6000_AO_CTRL_BIT_IMMEDIATE_STOP | ME6000_AO_CTRL_BIT_STOP;
+		if (trig_type == ME_TRIG_TYPE_EXT_DIGITAL) {
+			PINFO("External digital trigger.\n");
+
+			if (trig_edge == ME_TRIG_EDGE_ANY) {
+//                                      ctrl |= ME6000_AO_CTRL_BIT_EX_TRIG_EDGE | ME6000_AO_CTRL_BIT_EX_TRIG_EDGE_BOTH;
+				instance->ctrl_trg =
+				    ME6000_AO_CTRL_BIT_EX_TRIG_EDGE |
+				    ME6000_AO_CTRL_BIT_EX_TRIG_EDGE_BOTH;
+			} else if (trig_edge == ME_TRIG_EDGE_FALLING) {
+//                                      ctrl |= ME6000_AO_CTRL_BIT_EX_TRIG_EDGE;
+				instance->ctrl_trg =
+				    ME6000_AO_CTRL_BIT_EX_TRIG_EDGE;
+			} else if (trig_edge == ME_TRIG_EDGE_RISING) {
+				instance->ctrl_trg = 0x0;
+			}
+		} else if (trig_type == ME_TRIG_TYPE_SW) {
+			PDEBUG("SOFTWARE TRIGGER\n");
+			instance->ctrl_trg = 0x0;
+		}
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+		spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+	} else {
+		PDEBUG("SOFTWARE TRIGGER\n");
+	}
+
+	// Set preload/synchronization register.
+	spin_lock(instance->preload_reg_lock);
+
+	if (trig_type == ME_TRIG_TYPE_SW) {
+		*instance->preload_flags &=
+		    ~(ME6000_AO_SYNC_EXT_TRIG << instance->ao_idx);
+	} else			//if (trig_type == ME_TRIG_TYPE_EXT_DIGITAL)
+	{
+		*instance->preload_flags |=
+		    ME6000_AO_SYNC_EXT_TRIG << instance->ao_idx;
+	}
+
+	if (trig_chan == ME_TRIG_CHAN_DEFAULT) {
+		*instance->preload_flags &=
+		    ~(ME6000_AO_SYNC_HOLD << instance->ao_idx);
+	} else			//if (trig_chan == ME_TRIG_CHAN_SYNCHRONOUS)
+	{
+		*instance->preload_flags |=
+		    ME6000_AO_SYNC_HOLD << instance->ao_idx;
+	}
+
+	//Reset hardware register
+	sync = inl(instance->preload_reg);
+	PDEBUG_REG("preload_reg inl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->preload_reg - instance->reg_base, sync);
+	sync &= ~(ME6000_AO_SYNC_EXT_TRIG << instance->ao_idx);
+	sync |= ME6000_AO_SYNC_HOLD << instance->ao_idx;
+
+	//Output configured in default mode (safe one)
+	outl(sync, instance->preload_reg);
+	PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->preload_reg - instance->reg_base, sync);
+	spin_unlock(instance->preload_reg_lock);
+
+	instance->status = ao_status_single_configured;
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me6000_ao_io_single_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int channel,
+				    int *value, int time_out, int flags)
+{
+	me6000_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	unsigned long j;
+	unsigned long delay = 0;
+
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (flags & ~ME_IO_SINGLE_NONBLOCKING) {
+		PERROR("Invalid flag specified. %d\n", flags);
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if ((instance->status >= ao_status_stream_configured)
+	    && (instance->status <= ao_status_stream_end)) {
+		PERROR("Subdevice not configured to work in single mode!\n");
+		return ME_ERRNO_PREVIOUS_CONFIG;
+	}
+
+	if (channel != 0) {
+		PERROR("Invalid channel number specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid timeout specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	ME_SUBDEVICE_ENTER;
+	if ((!flags) && (instance->status == ao_status_single_run_wait)) {	//Blocking mode. Wait for trigger.
+		if (time_out) {
+			delay = (time_out * HZ) / 1000;
+			if (delay == 0)
+				delay = 1;
+		}
+
+		j = jiffies;
+
+		//Only runing process will interrupt this call. Events are signaled when status change. This procedure has own timeout.
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 (instance->status !=
+						  ao_status_single_run_wait),
+						 (delay) ? delay : LONG_MAX);
+
+		if (instance->status == ao_status_none) {
+			PDEBUG("Single canceled.\n");
+			err = ME_ERRNO_CANCELLED;
+		}
+
+		if (signal_pending(current)) {
+			PERROR("Wait on start of state machine interrupted.\n");
+			instance->status = ao_status_none;
+			ao_stop_immediately(instance);
+			err = ME_ERRNO_SIGNAL;
+		}
+
+		if ((delay) && ((jiffies - j) >= delay)) {
+			PDEBUG("Timeout reached.\n");
+			err = ME_ERRNO_TIMEOUT;
+		}
+
+		*value =
+		    (!err) ? instance->single_value_in_fifo : instance->
+		    single_value;
+	} else {		//Non-blocking mode
+		//Read value
+		*value = instance->single_value;
+	}
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me6000_ao_io_single_write(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int channel,
+				     int value, int time_out, int flags)
+{
+	me6000_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long cpu_flags;
+	unsigned long j;
+	unsigned long delay = 0;
+
+	uint32_t sync_mask;
+	uint32_t mode;
+
+	uint32_t tmp;
+
+/// Workaround for mix-mode - begin
+	uint32_t ctrl = 0x0;
+	uint32_t status;
+/// Workaround for mix-mode - end
+
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (flags &
+	    ~(ME_IO_SINGLE_TYPE_TRIG_SYNCHRONOUS |
+	      ME_IO_SINGLE_TYPE_WRITE_NONBLOCKING)) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if ((instance->status == ao_status_none)
+	    || (instance->status > ao_status_single_end)) {
+		PERROR("Subdevice not configured to work in single mode!\n");
+		return ME_ERRNO_PREVIOUS_CONFIG;
+	}
+
+	if (channel != 0) {
+		PERROR("Invalid channel number specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	if (value & ~ME6000_AO_MAX_DATA) {
+		PERROR("Invalid value provided.\n");
+		return ME_ERRNO_VALUE_OUT_OF_RANGE;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid timeout specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+/// @note For single all calls (config and write) are erasing previous state!
+
+	//Cancel control task
+	PDEBUG("Cancel control task. idx=%d\n", instance->ao_idx);
+	instance->ao_control_task_flag = 0;
+	cancel_delayed_work(&instance->ao_control_task);
+
+	// Correct single mirrors
+	instance->single_value_in_fifo = instance->single_value;
+
+	//Stop device
+	err = ao_stop_immediately(instance);
+	if (err) {
+		PERROR_CRITICAL("FSM IS BUSY!\n");
+		ME_SUBDEVICE_EXIT;
+
+		return ME_ERRNO_SUBDEVICE_BUSY;
+	}
+
+	if (time_out) {
+		delay = (time_out * HZ) / 1000;
+
+		if (delay == 0)
+			delay = 1;
+	}
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+
+	instance->single_value_in_fifo = value;
+
+	if (instance->fifo) {
+		ctrl = inl(instance->ctrl_reg);
+	}
+
+	if (instance->fifo & ME6000_AO_HAS_FIFO) {	/// Workaround for mix-mode - begin
+		//Set speed
+		outl(ME6000_AO_MIN_CHAN_TICKS - 1, instance->timer_reg);
+		PDEBUG_REG("timer_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->timer_reg - instance->reg_base,
+			   (int)ME6000_AO_MIN_CHAN_TICKS);
+		instance->hardware_stop_delay = HZ / 10;	//100ms
+
+		status = inl(instance->status_reg);
+
+		//Set the continous mode.
+		ctrl &= ~ME6000_AO_CTRL_MODE_MASK;
+		ctrl |= ME6000_AO_MODE_CONTINUOUS;
+
+		//Prepare FIFO
+		if (!(ctrl & ME6000_AO_CTRL_BIT_ENABLE_FIFO)) {	//FIFO wasn't enabeled. Do it.
+			PINFO("Enableing FIFO.\n");
+			ctrl &= ~ME6000_AO_CTRL_BIT_ENABLE_IRQ;
+			ctrl |= ME6000_AO_CTRL_BIT_ENABLE_FIFO;
+		} else {	//Check if FIFO is empty
+			if (status & ME6000_AO_STATUS_BIT_EF) {	//FIFO not empty
+				PINFO("Reseting FIFO.\n");
+				ctrl &=
+				    ~(ME6000_AO_CTRL_BIT_ENABLE_FIFO |
+				      ME6000_AO_CTRL_BIT_ENABLE_IRQ);
+				outl(ctrl, instance->ctrl_reg);
+				PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+					   instance->reg_base,
+					   instance->ctrl_reg -
+					   instance->reg_base, ctrl);
+
+				ctrl |= ME6000_AO_CTRL_BIT_ENABLE_FIFO;
+			} else {	//FIFO empty, only interrupt needs to be disabled!
+				ctrl &= ~ME6000_AO_CTRL_BIT_ENABLE_IRQ;
+			}
+		}
+
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+
+		//Reset interrupt latch
+		inl(instance->irq_reset_reg);
+
+		//Write output - 1 value to FIFO
+		if (instance->ao_idx & 0x1) {
+			outl(value <<= 16, instance->fifo_reg);
+			PDEBUG_REG("fifo_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->fifo_reg - instance->reg_base,
+				   value <<= 16);
+		} else {
+			outl(value, instance->fifo_reg);
+			PDEBUG_REG("fifo_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->fifo_reg - instance->reg_base,
+				   value);
+		}
+		/// Workaround for mix-mode - end
+	} else {		//No FIFO - always in single mode
+		//Write value
+		PDEBUG("Write value\n");
+		outl(value, instance->single_reg);
+		PDEBUG_REG("single_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->single_reg - instance->reg_base, value);
+	}
+
+	mode = *instance->preload_flags >> instance->ao_idx;
+	mode &= (ME6000_AO_SYNC_HOLD | ME6000_AO_SYNC_EXT_TRIG);
+
+	PINFO("Triggering mode: 0x%08x\n", mode);
+
+	spin_lock(instance->preload_reg_lock);
+	sync_mask = inl(instance->preload_reg);
+	PDEBUG_REG("preload_reg inl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->preload_reg - instance->reg_base, sync_mask);
+	switch (mode) {
+	case 0:		//0x00000000: Individual software
+		ctrl &= ~ME6000_AO_CTRL_BIT_ENABLE_EX_TRIG;
+
+		if (instance->fifo & ME6000_AO_HAS_FIFO) {	// FIFO - Continous mode
+			ctrl &= ~ME6000_AO_CTRL_BIT_ENABLE_EX_TRIG;
+			if ((sync_mask & ((ME6000_AO_SYNC_HOLD | ME6000_AO_SYNC_EXT_TRIG) << instance->ao_idx)) != 0x0) {	//Now we can set correct mode.
+				sync_mask &=
+				    ~((ME6000_AO_SYNC_EXT_TRIG |
+				       ME6000_AO_SYNC_HOLD) << instance->
+				      ao_idx);
+
+				outl(sync_mask, instance->preload_reg);
+				PDEBUG_REG
+				    ("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				     instance->reg_base,
+				     instance->preload_reg - instance->reg_base,
+				     sync_mask);
+			}
+		} else {	// No FIFO - Single mode: In this case resetting 'ME6000_AO_SYNC_HOLD' will trigger output.
+			if ((sync_mask & ((ME6000_AO_SYNC_HOLD | ME6000_AO_SYNC_EXT_TRIG) << instance->ao_idx)) != ME6000_AO_SYNC_HOLD) {	//Now we can set correct mode. This is exception. It is set to synchronous and triggered later.
+				sync_mask &=
+				    ~(ME6000_AO_SYNC_EXT_TRIG << instance->
+				      ao_idx);
+				sync_mask |=
+				    ME6000_AO_SYNC_HOLD << instance->ao_idx;
+
+				outl(sync_mask, instance->preload_reg);
+				PDEBUG_REG
+				    ("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				     instance->reg_base,
+				     instance->preload_reg - instance->reg_base,
+				     sync_mask);
+			}
+		}
+		instance->single_value = value;
+		break;
+
+	case ME6000_AO_SYNC_EXT_TRIG:	//0x00010000: Individual hardware
+		PDEBUG("DIGITAL TRIGGER\n");
+		ctrl |= ME6000_AO_CTRL_BIT_ENABLE_EX_TRIG;
+
+		if (instance->fifo & ME6000_AO_HAS_FIFO) {	// FIFO - Continous mode
+			if ((sync_mask & ((ME6000_AO_SYNC_HOLD | ME6000_AO_SYNC_EXT_TRIG) << instance->ao_idx)) != 0x0) {	//Now we can set correct mode.
+				sync_mask &=
+				    ~((ME6000_AO_SYNC_EXT_TRIG |
+				       ME6000_AO_SYNC_HOLD) << instance->
+				      ao_idx);
+
+				outl(sync_mask, instance->preload_reg);
+				PDEBUG_REG
+				    ("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				     instance->reg_base,
+				     instance->preload_reg - instance->reg_base,
+				     sync_mask);
+			}
+		} else {	// No FIFO - Single mode
+			if ((sync_mask &
+			     ((ME6000_AO_SYNC_HOLD | ME6000_AO_SYNC_EXT_TRIG) <<
+			      instance->ao_idx)) != ME6000_AO_SYNC_HOLD) {
+				//Now we can set correct mode
+				sync_mask &=
+				    ~(ME6000_AO_SYNC_EXT_TRIG << instance->
+				      ao_idx);
+				sync_mask |=
+				    ME6000_AO_SYNC_HOLD << instance->ao_idx;
+
+				outl(sync_mask, instance->preload_reg);
+				PDEBUG_REG
+				    ("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				     instance->reg_base,
+				     instance->preload_reg - instance->reg_base,
+				     sync_mask);
+			}
+		}
+		break;
+
+	case ME6000_AO_SYNC_HOLD:	//0x00000001: Synchronous software
+		ctrl &= ~ME6000_AO_CTRL_BIT_ENABLE_EX_TRIG;
+
+		if ((sync_mask &
+		     ((ME6000_AO_SYNC_HOLD | ME6000_AO_SYNC_EXT_TRIG) <<
+		      instance->ao_idx)) !=
+		    (ME6000_AO_SYNC_HOLD | ME6000_AO_SYNC_EXT_TRIG)) {
+			//Now we can set correct mode
+			sync_mask |=
+			    ME6000_AO_SYNC_EXT_TRIG << instance->ao_idx;
+			sync_mask |= ME6000_AO_SYNC_HOLD << instance->ao_idx;
+			outl(sync_mask, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   sync_mask);
+		}
+		//Set triggering flag
+		*instance->triggering_flags |= 0x1 << instance->ao_idx;
+		break;
+
+	case (ME6000_AO_SYNC_HOLD | ME6000_AO_SYNC_EXT_TRIG):	//0x00010001: Synchronous hardware
+		PDEBUG("DIGITAL TRIGGER\n");
+		ctrl |= ME6000_AO_CTRL_BIT_ENABLE_EX_TRIG;
+
+		if ((sync_mask &
+		     ((ME6000_AO_SYNC_HOLD | ME6000_AO_SYNC_EXT_TRIG) <<
+		      instance->ao_idx)) !=
+		    (ME6000_AO_SYNC_HOLD | ME6000_AO_SYNC_EXT_TRIG)) {
+			//Now we can set correct mode
+			sync_mask |=
+			    (ME6000_AO_SYNC_HOLD | ME6000_AO_SYNC_EXT_TRIG) <<
+			    instance->ao_idx;
+			outl(sync_mask, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   sync_mask);
+		}
+		//Set triggering flag
+		*instance->triggering_flags |= 0x1 << instance->ao_idx;
+		break;
+	}
+//              spin_unlock(instance->preload_reg_lock);        // Moved down.
+
+	if (instance->fifo) {	//Activate ISM (remove 'stop' bits)
+		ctrl &=
+		    ~(ME6000_AO_CTRL_BIT_EX_TRIG_EDGE |
+		      ME6000_AO_CTRL_BIT_EX_TRIG_EDGE_BOTH);
+		ctrl |= instance->ctrl_trg;
+		ctrl &=
+		    ~(ME6000_AO_CTRL_BIT_STOP |
+		      ME6000_AO_CTRL_BIT_IMMEDIATE_STOP);
+
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+	}
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+/// @note When flag 'ME_IO_SINGLE_TYPE_TRIG_SYNCHRONOUS' is set than output is triggered. ALWAYS!
+
+	PINFO("<%s> start mode= 0x%08x %s\n", __FUNCTION__, mode,
+	      (flags & ME_IO_SINGLE_TYPE_TRIG_SYNCHRONOUS) ? "SYNCHRONOUS" :
+	      "");
+	if (instance->fifo & ME6000_AO_HAS_FIFO) {	// FIFO - Continous mode
+		if (flags & ME_IO_SINGLE_TYPE_TRIG_SYNCHRONOUS) {	//Trigger outputs
+			//Add channel to start list
+			outl(sync_mask |
+			     (ME6000_AO_SYNC_HOLD << instance->ao_idx),
+			     instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   sync_mask | (ME6000_AO_SYNC_HOLD <<
+						instance->ao_idx));
+
+			//Fire
+			PINFO
+			    ("Fired all software synchronous outputs by software trigger.\n");
+			outl(0x8000, instance->single_reg);
+			PDEBUG_REG("single_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->single_reg - instance->reg_base,
+				   0x8000);
+
+			//Restore save settings
+			outl(sync_mask, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   sync_mask);
+
+		} else if (!mode) {	//Trigger outputs
+/*			//Remove channel from start list
+			outl(sync_mask & ~(ME6000_AO_SYNC_HOLD << instance->ao_idx), instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base, instance->preload_reg - instance->reg_base, sync_mask & ~(ME6000_AO_SYNC_HOLD << instance->ao_idx));
+*/
+			//Fire
+			PINFO("Software trigger.\n");
+			outl(0x8000, instance->single_reg);
+			PDEBUG_REG("single_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->single_reg - instance->reg_base,
+				   0x8000);
+
+/*			//Restore save settings
+			outl(sync_mask, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base, instance->preload_reg - instance->reg_base, sync_mask);
+*/
+		}
+/// @note This is mix-mode case. For now I do not have possibility to trigger first 4 channels (continous mode) and other (single) ones at once.
+/// @note Because triggering is not working it can not be add to synchronous list. First 4 channels don't need this information, anyway.
+		*instance->triggering_flags &= 0xFFFFFFF0;
+	} else {		// No FIFO - Single mode
+		if (flags & ME_IO_SINGLE_TYPE_TRIG_SYNCHRONOUS) {	//Fired all software synchronous outputs.
+			tmp = ~(*instance->preload_flags | 0xFFFF0000);
+			PINFO
+			    ("Fired all software synchronous outputs. mask:0x%08x\n",
+			     tmp);
+			tmp |= sync_mask & 0xFFFF0000;
+			// Add this channel to list
+			tmp &= ~(ME6000_AO_SYNC_HOLD << instance->ao_idx);
+
+			//Fire
+			PINFO("Software trigger.\n");
+			outl(tmp, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   tmp);
+
+			//Restore save settings
+			outl(sync_mask, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   sync_mask);
+
+			//Set all as triggered.
+			*instance->triggering_flags = 0x0;
+		} else if (!mode) {	// Add this channel to list
+			outl(sync_mask &
+			     ~(ME6000_AO_SYNC_HOLD << instance->ao_idx),
+			     instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   sync_mask & ~(ME6000_AO_SYNC_HOLD <<
+						 instance->ao_idx));
+
+			//Fire
+			PINFO("Software trigger.\n");
+
+			//Restore save settings
+			outl(sync_mask, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   sync_mask);
+
+			//Set all as triggered.
+			*instance->triggering_flags = 0x0;
+		}
+
+	}
+	spin_unlock(instance->preload_reg_lock);
+
+	instance->status = ao_status_single_run_wait;
+
+	instance->timeout.delay = delay;
+	instance->timeout.start_time = jiffies;
+	instance->ao_control_task_flag = 1;
+	queue_delayed_work(instance->me6000_workqueue,
+			   &instance->ao_control_task, 1);
+
+	if (!(flags & ME_IO_SINGLE_TYPE_WRITE_NONBLOCKING)) {
+		j = jiffies;
+
+		//Only runing process will interrupt this call. Events are signaled when status change. Extra timeout add for safe reason.
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 (instance->status !=
+						  ao_status_single_run_wait),
+						 (delay) ? delay +
+						 1 : LONG_MAX);
+
+		if (instance->status != ao_status_single_end) {
+			PDEBUG("Single canceled.\n");
+			err = ME_ERRNO_CANCELLED;
+		}
+
+		if (signal_pending(current)) {
+			PERROR("Wait on start of state machine interrupted.\n");
+			instance->ao_control_task_flag = 0;
+			cancel_delayed_work(&instance->ao_control_task);
+			ao_stop_immediately(instance);
+			instance->status = ao_status_none;
+			err = ME_ERRNO_SIGNAL;
+		}
+
+		if ((delay) && ((jiffies - j) >= delay)) {
+			if (instance->status == ao_status_single_end) {
+				PDEBUG("Timeout reached.\n");
+			} else if ((jiffies - j) > delay) {
+				PERROR
+				    ("Timeout reached. Not handled by control task!\n");
+				ao_stop_immediately(instance);
+			} else {
+				PERROR
+				    ("Timeout reached. Signal come but status is strange: %d\n",
+				     instance->status);
+				ao_stop_immediately(instance);
+			}
+
+			instance->ao_control_task_flag = 0;
+			cancel_delayed_work(&instance->ao_control_task);
+			instance->status = ao_status_single_end;
+			err = ME_ERRNO_TIMEOUT;
+		}
+	}
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me6000_ao_io_stream_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      meIOStreamConfig_t * config_list,
+				      int count,
+				      meIOStreamTrigger_t * trigger,
+				      int fifo_irq_threshold, int flags)
+{
+	me6000_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint32_t ctrl;
+	unsigned long cpu_flags;
+	uint64_t conv_ticks;
+	unsigned int conv_start_ticks_low = trigger->iConvStartTicksLow;
+	unsigned int conv_start_ticks_high = trigger->iConvStartTicksHigh;
+
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (!(instance->fifo & ME6000_AO_HAS_FIFO)) {
+		PERROR("Not a streaming ao.\n");
+		return ME_ERRNO_NOT_SUPPORTED;
+	}
+
+	conv_ticks =
+	    (uint64_t) conv_start_ticks_low +
+	    ((uint64_t) conv_start_ticks_high << 32);
+
+	if (flags &
+	    ~(ME_IO_STREAM_CONFIG_HARDWARE_ONLY |
+	      ME_IO_STREAM_CONFIG_WRAPAROUND)) {
+		PERROR("Invalid flags.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (flags & ME_IO_STREAM_CONFIG_HARDWARE_ONLY) {
+		if (!flags & ME_IO_STREAM_CONFIG_WRAPAROUND) {
+			PERROR
+			    ("Hardware ME_IO_STREAM_CONFIG_HARDWARE_ONLY has to be with ME_IO_STREAM_CONFIG_WRAPAROUND.\n");
+			return ME_ERRNO_INVALID_FLAGS;
+		}
+
+		if ((trigger->iAcqStopTrigType != ME_TRIG_TYPE_NONE)
+		    || (trigger->iScanStopTrigType != ME_TRIG_TYPE_NONE)) {
+			PERROR
+			    ("Hardware wraparound mode must be in infinite mode.\n");
+			return ME_ERRNO_INVALID_FLAGS;
+		}
+	}
+
+	if (count != 1) {
+		PERROR("Only 1 entry in config list acceptable.\n");
+		return ME_ERRNO_INVALID_CONFIG_LIST_COUNT;
+	}
+
+	if (config_list[0].iChannel != 0) {
+		PERROR("Invalid channel number specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	if (config_list[0].iStreamConfig != 0) {
+		PERROR("Only one range available.\n");
+		return ME_ERRNO_INVALID_STREAM_CONFIG;
+	}
+
+	if (config_list[0].iRef != ME_REF_AO_GROUND) {
+		PERROR("Output is referenced to ground.\n");
+		return ME_ERRNO_INVALID_REF;
+	}
+
+	if ((trigger->iAcqStartTicksLow != 0)
+	    || (trigger->iAcqStartTicksHigh != 0)) {
+		PERROR
+		    ("Invalid acquisition start trigger argument specified.\n");
+		return ME_ERRNO_INVALID_ACQ_START_ARG;
+	}
+
+	if (config_list[0].iFlags) {
+		PERROR("Invalid config list flag.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if ((trigger->iAcqStartTrigType != ME_TRIG_TYPE_SW)
+	    && (trigger->iAcqStartTrigType != ME_TRIG_TYPE_EXT_DIGITAL)) {
+		PERROR("Invalid acquisition start trigger type specified.\n");
+		return ME_ERRNO_INVALID_ACQ_START_TRIG_TYPE;
+	}
+
+	if (trigger->iAcqStartTrigType == ME_TRIG_TYPE_EXT_DIGITAL) {
+		switch (trigger->iAcqStartTrigEdge) {
+		case ME_TRIG_EDGE_RISING:
+		case ME_TRIG_EDGE_FALLING:
+		case ME_TRIG_EDGE_ANY:
+			break;
+
+		default:
+			PERROR
+			    ("Invalid acquisition start trigger edge specified.\n");
+			return ME_ERRNO_INVALID_ACQ_START_TRIG_EDGE;
+		}
+	}
+
+	if ((trigger->iAcqStartTrigType == ME_TRIG_TYPE_SW)
+	    && (trigger->iAcqStartTrigEdge != ME_TRIG_TYPE_NONE)) {
+		PERROR("Invalid acquisition start trigger edge specified.\n");
+		return ME_ERRNO_INVALID_ACQ_START_TRIG_EDGE;
+	}
+
+	if (trigger->iScanStartTrigType != ME_TRIG_TYPE_FOLLOW) {
+		PERROR("Invalid scan start trigger type specified.\n");
+		return ME_ERRNO_INVALID_SCAN_START_TRIG_TYPE;
+	}
+
+	if (trigger->iConvStartTrigType != ME_TRIG_TYPE_TIMER) {
+		PERROR("Invalid conv start trigger type specified.\n");
+		return ME_ERRNO_INVALID_CONV_START_TRIG_TYPE;
+	}
+
+	if ((conv_ticks < ME6000_AO_MIN_CHAN_TICKS)
+	    || (conv_ticks > ME6000_AO_MAX_CHAN_TICKS)) {
+		PERROR("Invalid conv start trigger argument specified.\n");
+		return ME_ERRNO_INVALID_CONV_START_ARG;
+	}
+
+	if (trigger->iAcqStartTicksLow || trigger->iAcqStartTicksHigh) {
+		PERROR("Invalid acq start trigger argument specified.\n");
+		return ME_ERRNO_INVALID_ACQ_START_ARG;
+	}
+
+	if (trigger->iScanStartTicksLow || trigger->iScanStartTicksHigh) {
+		PERROR("Invalid scan start trigger argument specified.\n");
+		return ME_ERRNO_INVALID_SCAN_START_ARG;
+	}
+
+	switch (trigger->iScanStopTrigType) {
+	case ME_TRIG_TYPE_NONE:
+		if (trigger->iScanStopCount != 0) {
+			PERROR("Invalid scan stop count specified.\n");
+			return ME_ERRNO_INVALID_SCAN_STOP_ARG;
+		}
+		break;
+
+	case ME_TRIG_TYPE_COUNT:
+		if (flags & ME_IO_STREAM_CONFIG_WRAPAROUND) {
+			if (trigger->iScanStopCount <= 0) {
+				PERROR("Invalid scan stop count specified.\n");
+				return ME_ERRNO_INVALID_SCAN_STOP_ARG;
+			}
+		} else {
+			PERROR("The continous mode has not 'scan' contects.\n");
+			return ME_ERRNO_INVALID_ACQ_STOP_TRIG_TYPE;
+		}
+		break;
+
+	default:
+		PERROR("Invalid scan stop trigger type specified.\n");
+		return ME_ERRNO_INVALID_SCAN_STOP_TRIG_TYPE;
+	}
+
+	switch (trigger->iAcqStopTrigType) {
+	case ME_TRIG_TYPE_NONE:
+		if (trigger->iAcqStopCount != 0) {
+			PERROR("Invalid acq stop count specified.\n");
+			return ME_ERRNO_INVALID_ACQ_STOP_ARG;
+		}
+		break;
+
+	case ME_TRIG_TYPE_COUNT:
+		if (trigger->iScanStopTrigType != ME_TRIG_TYPE_NONE) {
+			PERROR("Invalid acq stop trigger type specified.\n");
+			return ME_ERRNO_INVALID_ACQ_STOP_TRIG_TYPE;
+		}
+
+		if (flags & ME_IO_STREAM_CONFIG_WRAPAROUND) {
+			if (trigger->iAcqStopCount <= 0) {
+				PERROR
+				    ("The continous mode has not 'scan' contects.\n");
+				return ME_ERRNO_INVALID_ACQ_STOP_ARG;
+			}
+		}
+//                      else
+//                      {
+//                              PERROR("Invalid acq stop trigger type specified.\n");
+//                              return ME_ERRNO_INVALID_ACQ_STOP_TRIG_TYPE;
+//                      }
+
+		break;
+
+	default:
+		PERROR("Invalid acq stop trigger type specified.\n");
+		return ME_ERRNO_INVALID_ACQ_STOP_TRIG_TYPE;
+	}
+
+	switch (trigger->iAcqStartTrigChan) {
+	case ME_TRIG_CHAN_DEFAULT:
+	case ME_TRIG_CHAN_SYNCHRONOUS:
+		break;
+
+	default:
+		PERROR("Invalid acq start trigger channel specified.\n");
+		return ME_ERRNO_INVALID_ACQ_START_TRIG_CHAN;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	//Stop device
+
+	//Cancel control task
+	PDEBUG("Cancel control task. idx=%d\n", instance->ao_idx);
+	instance->ao_control_task_flag = 0;
+	cancel_delayed_work(&instance->ao_control_task);
+
+	//Check if state machine is stopped.
+	err = ao_stop_immediately(instance);
+	if (err) {
+		PERROR_CRITICAL("FSM IS BUSY!\n");
+		ME_SUBDEVICE_EXIT;
+
+		return ME_ERRNO_SUBDEVICE_BUSY;
+	}
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	//Reset control register. Block all actions. Disable IRQ. Disable FIFO.
+	ctrl = ME6000_AO_CTRL_BIT_IMMEDIATE_STOP | ME6000_AO_CTRL_BIT_STOP;
+	outl(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl);
+
+	//Reset interrupt latch
+	inl(instance->irq_reset_reg);
+
+	//This is paranoic, but to be sure.
+	instance->preloaded_count = 0;
+	instance->data_count = 0;
+	instance->circ_buf.head = 0;
+	instance->circ_buf.tail = 0;
+
+	/* Set mode. */
+	if (flags & ME_IO_STREAM_CONFIG_WRAPAROUND) {	//Wraparound
+		if (flags & ME_IO_STREAM_CONFIG_HARDWARE_ONLY) {	//Hardware wraparound
+			PINFO("Hardware wraparound.\n");
+			ctrl |= ME6000_AO_MODE_WRAPAROUND;
+			instance->mode = ME6000_AO_HW_WRAP_MODE;
+		} else {	//Software wraparound
+			PINFO("Software wraparound.\n");
+			ctrl |= ME6000_AO_MODE_CONTINUOUS;
+			instance->mode = ME6000_AO_SW_WRAP_MODE;
+		}
+	} else {		//Continous
+		PINFO("Continous.\n");
+		ctrl |= ME6000_AO_MODE_CONTINUOUS;
+		instance->mode = ME6000_AO_CONTINOUS;
+	}
+
+	//Set the trigger edge.
+	if (trigger->iAcqStartTrigType == ME_TRIG_TYPE_EXT_DIGITAL) {	//Set the trigger type and edge for external trigger.
+		PINFO("External digital trigger.\n");
+		instance->start_mode = ME6000_AO_EXT_TRIG;
+
+		switch (trigger->iAcqStartTrigEdge) {
+		case ME_TRIG_EDGE_RISING:
+			PINFO("Set the trigger edge: rising.\n");
+			instance->ctrl_trg = 0x0;
+			break;
+
+		case ME_TRIG_EDGE_FALLING:
+			PINFO("Set the trigger edge: falling.\n");
+//                                      ctrl |= ME6000_AO_CTRL_BIT_EX_TRIG_EDGE;
+			instance->ctrl_trg = ME6000_AO_CTRL_BIT_EX_TRIG_EDGE;
+			break;
+
+		case ME_TRIG_EDGE_ANY:
+			PINFO("Set the trigger edge: both edges.\n");
+//                                      ctrl |= ME6000_AO_CTRL_BIT_EX_TRIG_EDGE | ME6000_AO_CTRL_BIT_EX_TRIG_EDGE_BOTH;
+			instance->ctrl_trg =
+			    ME6000_AO_CTRL_BIT_EX_TRIG_EDGE |
+			    ME6000_AO_CTRL_BIT_EX_TRIG_EDGE_BOTH;
+			break;
+		}
+	} else {
+		PINFO("Internal software trigger.\n");
+		instance->start_mode = 0;
+	}
+
+	//Set the stop mode and value.
+	if (trigger->iAcqStopTrigType == ME_TRIG_TYPE_COUNT) {	//Amount of data
+		instance->stop_mode = ME6000_AO_ACQ_STOP_MODE;
+		instance->stop_count = trigger->iAcqStopCount;
+	} else if (trigger->iScanStopTrigType == ME_TRIG_TYPE_COUNT) {	//Amount of 'scans'
+		instance->stop_mode = ME6000_AO_SCAN_STOP_MODE;
+		instance->stop_count = trigger->iScanStopCount;
+	} else {		//Infinite
+		instance->stop_mode = ME6000_AO_INF_STOP_MODE;
+		instance->stop_count = 0;
+	}
+
+	PINFO("Stop count: %d.\n", instance->stop_count);
+
+	if (trigger->iAcqStartTrigChan == ME_TRIG_CHAN_SYNCHRONOUS) {	//Synchronous start
+		instance->start_mode |= ME6000_AO_SYNC_HOLD;
+		if (trigger->iAcqStartTrigType == ME_TRIG_TYPE_EXT_DIGITAL) {	//Externaly triggered
+			PINFO("Synchronous start. Externaly trigger active.\n");
+			instance->start_mode |= ME6000_AO_SYNC_EXT_TRIG;
+		}
+#ifdef MEDEBUG_INFO
+		else {
+			PINFO
+			    ("Synchronous start. Externaly trigger dissabled.\n");
+		}
+#endif
+
+	}
+	//Set speed
+	outl(conv_ticks - 2, instance->timer_reg);
+	PDEBUG_REG("timer_reg outl(0x%lX+0x%lX)=0x%llx\n", instance->reg_base,
+		   instance->timer_reg - instance->reg_base, conv_ticks - 2);
+	instance->hardware_stop_delay = (int)(conv_ticks * HZ) / ME6000_AO_BASE_FREQUENCY;	//<== MUST be with cast!
+
+	// Write the control word
+	outl(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl);
+
+	//Set status.
+	instance->status = ao_status_stream_configured;
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me6000_ao_io_stream_new_values(me_subdevice_t * subdevice,
+					  struct file *filep,
+					  int time_out, int *count, int flags)
+{
+	me6000_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	long t = 0;
+	long j;
+
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (!(instance->fifo & ME6000_AO_HAS_FIFO)) {
+		PERROR("Not a streaming ao.\n");
+		return ME_ERRNO_NOT_SUPPORTED;
+	}
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (!instance->circ_buf.buf) {
+		PERROR("Circular buffer not exists.\n");
+		return ME_ERRNO_INTERNAL;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid time_out specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	if (me_circ_buf_space(&instance->circ_buf)) {	//The buffer is NOT full.
+		*count = me_circ_buf_space(&instance->circ_buf);
+	} else {		//The buffer is full.
+		if (time_out) {
+			t = (time_out * HZ) / 1000;
+
+			if (t == 0)
+				t = 1;
+		} else {	//Max time.
+			t = LONG_MAX;
+		}
+
+		*count = 0;
+
+		j = jiffies;
+
+		//Only runing process will interrupt this call. Interrupts are when FIFO HF is signaled.
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 ((me_circ_buf_space
+						   (&instance->circ_buf))
+						  || !(inl(instance->status_reg)
+						       &
+						       ME6000_AO_STATUS_BIT_FSM)),
+						 t);
+
+		if (!(inl(instance->status_reg) & ME6000_AO_STATUS_BIT_FSM)) {
+			PERROR("AO subdevice is not running.\n");
+			err = ME_ERRNO_SUBDEVICE_NOT_RUNNING;
+		} else if (signal_pending(current)) {
+			PERROR("Wait on values interrupted from signal.\n");
+			instance->status = ao_status_none;
+			ao_stop_immediately(instance);
+			err = ME_ERRNO_SIGNAL;
+		} else if ((jiffies - j) >= t) {
+			PERROR("Wait on values timed out.\n");
+			err = ME_ERRNO_TIMEOUT;
+		} else {	//Uff... all is good. Inform user about empty space.
+			*count = me_circ_buf_space(&instance->circ_buf);
+		}
+	}
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me6000_ao_io_stream_start(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int start_mode, int time_out, int flags)
+{
+	me6000_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long cpu_flags = 0;
+	uint32_t status;
+	uint32_t ctrl;
+	uint32_t synch;
+	int count = 0;
+	int circ_buffer_count;
+
+	unsigned long ref;
+	unsigned long delay = 0;
+
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (!(instance->fifo & ME6000_AO_HAS_FIFO)) {
+		PERROR("Not a streaming ao.\n");
+		return ME_ERRNO_NOT_SUPPORTED;
+	}
+
+	if (flags & ~ME_IO_STREAM_START_TYPE_TRIG_SYNCHRONOUS) {
+		PERROR("Invalid flags.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid timeout specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	if ((start_mode != ME_START_MODE_BLOCKING)
+	    && (start_mode != ME_START_MODE_NONBLOCKING)) {
+		PERROR("Invalid start mode specified.\n");
+		return ME_ERRNO_INVALID_START_MODE;
+	}
+
+	if (time_out) {
+		delay = (time_out * HZ) / 1000;
+		if (delay == 0)
+			delay = 1;
+	}
+
+	switch (instance->status) {	//Checking actual mode.
+	case ao_status_stream_configured:
+	case ao_status_stream_end:
+		//Correct modes!
+		break;
+
+		//The device is in wrong mode.
+	case ao_status_none:
+	case ao_status_single_configured:
+	case ao_status_single_run_wait:
+	case ao_status_single_run:
+	case ao_status_single_end_wait:
+		PERROR
+		    ("Subdevice must be preinitialize correctly for streaming.\n");
+		return ME_ERRNO_PREVIOUS_CONFIG;
+
+	case ao_status_stream_fifo_error:
+	case ao_status_stream_buffer_error:
+	case ao_status_stream_error:
+		PDEBUG("Before restart broke stream 'STOP' must be caled.\n");
+		return ME_STATUS_ERROR;
+
+	case ao_status_stream_run_wait:
+	case ao_status_stream_run:
+	case ao_status_stream_end_wait:
+		PDEBUG("Stream is already working.\n");
+		return ME_ERRNO_SUBDEVICE_BUSY;
+
+	default:
+		instance->status = ao_status_stream_error;
+		PERROR_CRITICAL("Status is in wrong state!\n");
+		return ME_ERRNO_INTERNAL;
+
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	if (instance->mode == ME6000_AO_CONTINOUS) {	//Continous
+		instance->circ_buf.tail += instance->preloaded_count;
+		instance->circ_buf.tail &= instance->circ_buf.mask;
+	}
+	circ_buffer_count = me_circ_buf_values(&instance->circ_buf);
+
+	if (!circ_buffer_count && !instance->preloaded_count) {	//No values in buffer
+		ME_SUBDEVICE_EXIT;
+		PERROR("No values in buffer!\n");
+		return ME_ERRNO_LACK_OF_RESOURCES;
+	}
+
+	//Cancel control task
+	PDEBUG("Cancel control task. idx=%d\n", instance->ao_idx);
+	instance->ao_control_task_flag = 0;
+	cancel_delayed_work(&instance->ao_control_task);
+
+	//Stop device
+	err = ao_stop_immediately(instance);
+	if (err) {
+		PERROR_CRITICAL("FSM IS BUSY!\n");
+		ME_SUBDEVICE_EXIT;
+
+		return ME_ERRNO_SUBDEVICE_BUSY;
+	}
+	//Set values for single_read()
+	instance->single_value = ME6000_AO_MAX_DATA + 1;
+	instance->single_value_in_fifo = ME6000_AO_MAX_DATA + 1;
+
+	//Setting stop points
+	if (instance->stop_mode == ME6000_AO_SCAN_STOP_MODE) {
+		instance->stop_data_count =
+		    instance->stop_count * circ_buffer_count;
+	} else {
+		instance->stop_data_count = instance->stop_count;
+	}
+
+	if ((instance->stop_data_count != 0)
+	    && (instance->stop_data_count < circ_buffer_count)) {
+		PERROR("More data in buffer than previously set limit!\n");
+	}
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	ctrl = inl(instance->ctrl_reg);
+	//Check FIFO
+	if (!(ctrl & ME6000_AO_CTRL_BIT_ENABLE_FIFO)) {	//FIFO wasn't enabeled. Do it. <= This should be done by user call with ME_WRITE_MODE_PRELOAD
+		PINFO("Enableing FIFO.\n");
+		ctrl |= ME6000_AO_CTRL_BIT_ENABLE_FIFO;
+		ctrl &= ~ME6000_AO_CTRL_BIT_ENABLE_IRQ;
+
+		instance->preloaded_count = 0;
+		instance->data_count = 0;
+	} else {		//Block IRQ
+		ctrl &= ~ME6000_AO_CTRL_BIT_ENABLE_IRQ;
+	}
+	outl(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl);
+
+	//Reset interrupt latch
+	inl(instance->irq_reset_reg);
+
+	//Fill FIFO <= Generaly this should be done by user pre-load call but this is second place to do it.
+	status = inl(instance->status_reg);
+	if (!(status & ME6000_AO_STATUS_BIT_EF)) {	//FIFO empty
+		if (instance->stop_data_count != 0) {
+			count = ME6000_AO_FIFO_COUNT;
+		} else {
+			count =
+			    (ME6000_AO_FIFO_COUNT <
+			     instance->
+			     stop_data_count) ? ME6000_AO_FIFO_COUNT :
+			    instance->stop_data_count;
+		}
+
+		//Copy data
+		count =
+		    ao_write_data(instance, count, instance->preloaded_count);
+
+		if (count < 0) {	//This should never happend!
+			PERROR_CRITICAL("COPY FINISH WITH ERROR!\n");
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+			ME_SUBDEVICE_EXIT;
+			return ME_ERRNO_INTERNAL;
+		}
+	}
+	//Set pre-load features.
+	spin_lock(instance->preload_reg_lock);
+	synch = inl(instance->preload_reg);
+	synch &=
+	    ~((ME6000_AO_SYNC_HOLD | ME6000_AO_SYNC_EXT_TRIG) << instance->
+	      ao_idx);
+	synch |=
+	    (instance->start_mode & ~ME6000_AO_EXT_TRIG) << instance->ao_idx;
+	outl(synch, instance->preload_reg);
+	PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->preload_reg - instance->reg_base, synch);
+	spin_unlock(instance->preload_reg_lock);
+
+	//Default count is '0'
+	if (instance->mode == ME6000_AO_CONTINOUS) {	//Continous
+		instance->preloaded_count = 0;
+		instance->circ_buf.tail += count;
+		instance->circ_buf.tail &= instance->circ_buf.mask;
+	} else {		//Wraparound
+		instance->preloaded_count += count;
+		instance->data_count += count;
+
+		//Special case: Infinite wraparound with less than FIFO datas always should runs in hardware mode.
+		if ((instance->stop_mode == ME6000_AO_INF_STOP_MODE)
+		    && (circ_buffer_count <= ME6000_AO_FIFO_COUNT)) {	//Change to hardware wraparound
+			PDEBUG
+			    ("Changeing mode from software wraparound to hardware wraparound.\n");
+			//Copy all data
+			count =
+			    ao_write_data(instance, circ_buffer_count,
+					  instance->preloaded_count);
+			ctrl &= ~ME6000_AO_CTRL_MODE_MASK;
+			ctrl |= ME6000_AO_MODE_WRAPAROUND;
+		}
+
+		if (instance->preloaded_count == me_circ_buf_values(&instance->circ_buf)) {	//Reset position indicator.
+			instance->preloaded_count = 0;
+		} else if (instance->preloaded_count > me_circ_buf_values(&instance->circ_buf)) {	//This should never happend!
+			PERROR_CRITICAL
+			    ("PRELOADED MORE VALUES THAN ARE IN BUFFER!\n");
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+			ME_SUBDEVICE_EXIT;
+			return ME_ERRNO_INTERNAL;
+		}
+	}
+
+	//Set status to 'wait for start'
+	instance->status = ao_status_stream_run_wait;
+
+	status = inl(instance->status_reg);
+	//Start state machine and interrupts
+	PINFO("<%s:%d> Start state machine.\n", __FUNCTION__, __LINE__);
+	ctrl &= ~(ME6000_AO_CTRL_BIT_STOP | ME6000_AO_CTRL_BIT_IMMEDIATE_STOP);
+	if (instance->start_mode == ME6000_AO_EXT_TRIG) {
+		PDEBUG("DIGITAL TRIGGER\n");
+		ctrl |= ME6000_AO_CTRL_BIT_ENABLE_EX_TRIG;
+	}
+	if (!(status & ME6000_AO_STATUS_BIT_HF)) {	//More than half!
+		if ((ctrl & ME6000_AO_CTRL_MODE_MASK) == ME6000_AO_MODE_CONTINUOUS) {	//Enable IRQ only when hardware_continous is set and FIFO is more than half
+			PINFO("<%s:%d> Start interrupts.\n", __FUNCTION__,
+			      __LINE__);
+			ctrl |= ME6000_AO_CTRL_BIT_ENABLE_IRQ;
+		}
+	}
+	outl(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl);
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	//Trigger output
+	PINFO("<%s> start mode= 0x%x %s\n", __FUNCTION__, instance->start_mode,
+	      (flags & ME_IO_SINGLE_TYPE_TRIG_SYNCHRONOUS) ? "SYNCHRONOUS" :
+	      "");
+	if (flags & ME_IO_SINGLE_TYPE_TRIG_SYNCHRONOUS) {	//Trigger outputs
+		spin_lock(instance->preload_reg_lock);
+		synch = inl(instance->preload_reg);
+		//Add channel to start list
+		outl(synch | (ME6000_AO_SYNC_HOLD << instance->ao_idx),
+		     instance->preload_reg);
+		PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->preload_reg - instance->reg_base,
+			   synch | (ME6000_AO_SYNC_HOLD << instance->ao_idx));
+
+		//Fire
+		PINFO
+		    ("Fired all software synchronous outputs by software trigger.\n");
+		outl(0x8000, instance->single_reg);
+		PDEBUG_REG("single_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->single_reg - instance->reg_base, 0x8000);
+
+		//Restore save settings
+		outl(synch, instance->preload_reg);
+		PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->preload_reg - instance->reg_base, synch);
+		spin_unlock(instance->preload_reg_lock);
+	} else if (!instance->start_mode) {	//Trigger outputs
+/*
+		spin_lock(instance->preload_reg_lock);
+			synch = inl(instance->preload_reg);
+			//Remove channel from start list
+			outl(synch & ~(ME6000_AO_SYNC_HOLD << instance->ao_idx), instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base, instance->preload_reg - instance->reg_base, synch & ~(ME6000_AO_SYNC_HOLD << instance->ao_idx));
+*/
+		//Fire
+		PINFO("Software trigger.\n");
+		outl(0x8000, instance->single_reg);
+		PDEBUG_REG("single_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->single_reg - instance->reg_base, 0x8000);
+
+/*
+			//Restore save settings
+			outl(synch, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base, instance->preload_reg - instance->reg_base, synch);
+		spin_unlock(instance->preload_reg_lock);
+*/
+	}
+	// Set control task's timeout
+	instance->timeout.delay = delay;
+	instance->timeout.start_time = jiffies;
+
+	if (status & ME6000_AO_STATUS_BIT_HF) {	//Less than half but not empty!
+		PINFO("Less than half.\n");
+		if (instance->stop_data_count == 0) {
+			count = ME6000_AO_FIFO_COUNT / 2;
+		} else {
+			count =
+			    ((ME6000_AO_FIFO_COUNT / 2) <
+			     instance->stop_data_count) ? ME6000_AO_FIFO_COUNT /
+			    2 : instance->stop_data_count;
+		}
+
+		//Copy data
+		count =
+		    ao_write_data(instance, count, instance->preloaded_count);
+
+		if (count < 0) {	//This should never happend!
+			PERROR_CRITICAL("COPY FINISH WITH ERROR!\n");
+			ME_SUBDEVICE_EXIT;
+			return ME_ERRNO_INTERNAL;
+		}
+
+		if (instance->mode == ME6000_AO_CONTINOUS) {	//Continous
+			instance->circ_buf.tail += count;
+			instance->circ_buf.tail &= instance->circ_buf.mask;
+		} else {	//Wraparound
+			instance->data_count += count;
+			instance->preloaded_count += count;
+
+			if (instance->preloaded_count == me_circ_buf_values(&instance->circ_buf)) {	//Reset position indicator.
+				instance->preloaded_count = 0;
+			} else if (instance->preloaded_count > me_circ_buf_values(&instance->circ_buf)) {	//This should never happend!
+				PERROR_CRITICAL
+				    ("PRELOADED MORE VALUES THAN ARE IN BUFFER!\n");
+				ME_SUBDEVICE_EXIT;
+				return ME_ERRNO_INTERNAL;
+			}
+		}
+
+		status = inl(instance->status_reg);
+		if (!(status & ME6000_AO_STATUS_BIT_HF)) {	//More than half!
+			spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+			PINFO("<%s:%d> Start interrupts.\n", __FUNCTION__,
+			      __LINE__);
+			ctrl = inl(instance->ctrl_reg);
+			ctrl |= ME6000_AO_CTRL_BIT_ENABLE_IRQ;
+			outl(ctrl, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   ctrl);
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+		}
+	}
+	//Special case: Limited wraparound with less than HALF FIFO datas need work around to generate first interrupt.
+	if ((instance->stop_mode != ME6000_AO_INF_STOP_MODE)
+	    && (instance->mode == ME6000_AO_SW_WRAP_MODE)
+	    && (circ_buffer_count <= (ME6000_AO_FIFO_COUNT / 2))) {	//Put more data to FIFO
+		PINFO("Limited wraparound with less than HALF FIFO datas.\n");
+		if (instance->preloaded_count) {	//This should never happend!
+			PERROR_CRITICAL
+			    ("ERROR WHEN LOADING VALUES FOR WRAPAROUND!\n");
+			ME_SUBDEVICE_EXIT;
+			return ME_ERRNO_INTERNAL;
+		}
+
+		while (instance->stop_data_count > instance->data_count) {	//Maximum data not set jet.
+			//Copy to buffer
+			if (circ_buffer_count != ao_write_data(instance, circ_buffer_count, 0)) {	//This should never happend!
+				PERROR_CRITICAL
+				    ("ERROR WHEN LOADING VALUES FOR WRAPAROUND!\n");
+				ME_SUBDEVICE_EXIT;
+				return ME_ERRNO_INTERNAL;
+			}
+			instance->data_count += circ_buffer_count;
+
+			if (!((status = inl(instance->status_reg)) & ME6000_AO_STATUS_BIT_HF)) {	//FIFO is more than half. Enable IRQ and end copy.
+				//Reset interrupt latch
+				inl(instance->irq_reset_reg);
+
+				spin_lock_irqsave(&instance->subdevice_lock,
+						  cpu_flags);
+				PINFO("<%s:%d> Start interrupts.\n",
+				      __FUNCTION__, __LINE__);
+				ctrl = inl(instance->ctrl_reg);
+				ctrl |= ME6000_AO_CTRL_BIT_ENABLE_IRQ;
+				outl(ctrl, instance->ctrl_reg);
+				PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+					   instance->reg_base,
+					   instance->ctrl_reg -
+					   instance->reg_base, ctrl);
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+				break;
+			}
+		}
+	}
+	// Schedule control task
+	instance->ao_control_task_flag = 1;
+	queue_delayed_work(instance->me6000_workqueue,
+			   &instance->ao_control_task, 1);
+
+	if (start_mode == ME_START_MODE_BLOCKING) {	//Wait for start.
+		ref = jiffies;
+		//Only runing process will interrupt this call. Events are signaled when status change. Extra timeout add for safe reason.
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 (instance->status !=
+						  ao_status_stream_run_wait),
+						 (delay) ? delay +
+						 1 : LONG_MAX);
+
+		if ((instance->status != ao_status_stream_run)
+		    && (instance->status != ao_status_stream_end)) {
+			PDEBUG("Starting stream canceled. %d\n",
+			       instance->status);
+			err = ME_ERRNO_CANCELLED;
+		}
+
+		if (signal_pending(current)) {
+			PERROR("Wait on start of state machine interrupted.\n");
+			instance->status = ao_status_none;
+			ao_stop_immediately(instance);
+			err = ME_ERRNO_SIGNAL;
+		}
+
+		if ((delay) && ((jiffies - ref) >= delay)) {
+			if (instance->status != ao_status_stream_run) {
+				if (instance->status == ao_status_stream_end) {
+					PDEBUG("Timeout reached.\n");
+				} else if ((jiffies - ref) > delay) {
+					PERROR
+					    ("Timeout reached. Not handled by control task!\n");
+					ao_stop_immediately(instance);
+				} else {
+					PERROR
+					    ("Timeout reached. Signal come but status is strange: %d\n",
+					     instance->status);
+					ao_stop_immediately(instance);
+				}
+
+				instance->ao_control_task_flag = 0;
+				cancel_delayed_work(&instance->ao_control_task);
+				instance->status = ao_status_stream_end;
+				err = ME_ERRNO_TIMEOUT;
+			}
+		}
+	}
+
+	ME_SUBDEVICE_EXIT;
+	return err;
+}
+
+static int me6000_ao_io_stream_status(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int wait,
+				      int *status, int *values, int flags)
+{
+	me6000_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (!(instance->fifo & ME6000_AO_HAS_FIFO)) {
+		PERROR("Not a streaming ao.\n");
+		return ME_ERRNO_NOT_SUPPORTED;
+	}
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if ((wait != ME_WAIT_NONE) && (wait != ME_WAIT_IDLE)) {
+		PERROR("Invalid wait argument specified.\n");
+		*status = ME_STATUS_INVALID;
+		return ME_ERRNO_INVALID_WAIT;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	switch (instance->status) {
+	case ao_status_single_configured:
+	case ao_status_single_end:
+	case ao_status_stream_configured:
+	case ao_status_stream_end:
+	case ao_status_stream_fifo_error:
+	case ao_status_stream_buffer_error:
+	case ao_status_stream_error:
+		*status = ME_STATUS_IDLE;
+		break;
+
+	case ao_status_single_run_wait:
+	case ao_status_single_run:
+	case ao_status_single_end_wait:
+	case ao_status_stream_run_wait:
+	case ao_status_stream_run:
+	case ao_status_stream_end_wait:
+		*status = ME_STATUS_BUSY;
+		break;
+
+	case ao_status_none:
+	default:
+		*status =
+		    (inl(instance->status_reg) & ME6000_AO_STATUS_BIT_FSM) ?
+		    ME_STATUS_BUSY : ME_STATUS_IDLE;
+		break;
+	}
+
+	if ((wait == ME_WAIT_IDLE) && (*status == ME_STATUS_BUSY)) {
+		//Only runing process will interrupt this call. Events are signaled when status change. Extra timeout add for safe reason.
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 ((instance->status !=
+						   ao_status_single_run_wait)
+						  && (instance->status !=
+						      ao_status_single_run)
+						  && (instance->status !=
+						      ao_status_single_end_wait)
+						  && (instance->status !=
+						      ao_status_stream_run_wait)
+						  && (instance->status !=
+						      ao_status_stream_run)
+						  && (instance->status !=
+						      ao_status_stream_end_wait)),
+						 LONG_MAX);
+
+		if (instance->status != ao_status_stream_end) {
+			PDEBUG("Wait for IDLE canceled. %d\n",
+			       instance->status);
+			err = ME_ERRNO_CANCELLED;
+		}
+
+		if (signal_pending(current)) {
+			PERROR("Wait for IDLE interrupted.\n");
+			instance->status = ao_status_none;
+			ao_stop_immediately(instance);
+			err = ME_ERRNO_SIGNAL;
+		}
+
+		*status = ME_STATUS_IDLE;
+	}
+
+	*values = me_circ_buf_space(&instance->circ_buf);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me6000_ao_io_stream_stop(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int stop_mode, int flags)
+{				/// @note Stop work and empty buffer and FIFO
+	int err = ME_ERRNO_SUCCESS;
+	me6000_ao_subdevice_t *instance;
+	unsigned long cpu_flags;
+	volatile uint32_t ctrl;
+
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (flags & ~ME_IO_STREAM_STOP_PRESERVE_BUFFERS) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if ((stop_mode != ME_STOP_MODE_IMMEDIATE)
+	    && (stop_mode != ME_STOP_MODE_LAST_VALUE)) {
+		PERROR("Invalid stop mode specified.\n");
+		return ME_ERRNO_INVALID_STOP_MODE;
+	}
+
+	if (!(instance->fifo & ME6000_AO_HAS_FIFO)) {
+		PERROR("Not a streaming ao.\n");
+		return ME_ERRNO_NOT_SUPPORTED;
+	}
+
+	if (instance->status < ao_status_stream_configured) {
+		//There is nothing to stop!
+		PERROR("Subdevice not in streaming mode. %d\n",
+		       instance->status);
+		return ME_ERRNO_PREVIOUS_CONFIG;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	//Mark as stopping. => Software stop.
+	instance->status = ao_status_stream_end_wait;
+
+	if (stop_mode == ME_STOP_MODE_IMMEDIATE) {	//Stopped now!
+		err = ao_stop_immediately(instance);
+	} else if (stop_mode == ME_STOP_MODE_LAST_VALUE) {
+		ctrl = inl(instance->ctrl_reg) & ME6000_AO_CTRL_MODE_MASK;
+		if (ctrl == ME6000_AO_MODE_WRAPAROUND) {	//Hardware wraparound => Hardware stop.
+			spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+			ctrl = inl(instance->ctrl_reg);
+			ctrl |= ME6000_AO_CTRL_BIT_STOP;
+			ctrl &= ~ME6000_AO_CTRL_BIT_ENABLE_IRQ;
+			outl(ctrl, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   ctrl);
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+
+			//Reset interrupt latch
+			inl(instance->irq_reset_reg);
+		}
+		//Only runing process will interrupt this call. Events are signaled when status change. Extra timeout add for safe reason.
+		wait_event_interruptible_timeout(instance->wait_queue,
+						 (instance->status !=
+						  ao_status_stream_end_wait),
+						 LONG_MAX);
+
+		if (instance->status != ao_status_stream_end) {
+			PDEBUG("Stopping stream canceled.\n");
+			err = ME_ERRNO_CANCELLED;
+		}
+
+		if (signal_pending(current)) {
+			PERROR("Stopping stream interrupted.\n");
+			instance->status = ao_status_none;
+			ao_stop_immediately(instance);
+			err = ME_ERRNO_SIGNAL;
+		}
+	}
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	ctrl = inl(instance->ctrl_reg);
+	ctrl |= ME6000_AO_CTRL_BIT_STOP | ME6000_AO_CTRL_BIT_IMMEDIATE_STOP;
+	ctrl &= ~ME6000_AO_CTRL_BIT_ENABLE_IRQ;
+	if (!flags) {		//Reset FIFO
+		ctrl &= ~ME6000_AO_CTRL_BIT_ENABLE_FIFO;
+	}
+	outl(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl);
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	//Reset interrupt latch
+	inl(instance->irq_reset_reg);
+
+	if (!flags) {		//Reset software buffer
+		instance->circ_buf.head = 0;
+		instance->circ_buf.tail = 0;
+		instance->preloaded_count = 0;
+		instance->data_count = 0;
+	}
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me6000_ao_io_stream_write(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int write_mode,
+				     int *values, int *count, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me6000_ao_subdevice_t *instance;
+	unsigned long cpu_flags = 0;
+	uint32_t reg_copy;
+
+	int copied_from_user = 0;
+	int left_to_copy_from_user = *count;
+
+	int copied_values;
+
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	//Checking arguments
+	if (!(instance->fifo & ME6000_AO_HAS_FIFO)) {
+		PERROR("Not a streaming ao.\n");
+		return ME_ERRNO_NOT_SUPPORTED;
+	}
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (*count <= 0) {
+		PERROR("Invalid count of values specified.\n");
+		return ME_ERRNO_INVALID_VALUE_COUNT;
+	}
+
+	if (values == NULL) {
+		PERROR("Invalid address of values specified.\n");
+		return ME_ERRNO_INVALID_POINTER;
+	}
+
+	if ((instance->status == ao_status_none) || (instance->status == ao_status_single_configured)) {	//The device is in single mode.
+		PERROR
+		    ("Subdevice must be preinitialize correctly for streaming.\n");
+		return ME_ERRNO_PREVIOUS_CONFIG;
+	}
+
+	switch (write_mode) {
+	case ME_WRITE_MODE_PRELOAD:
+
+		//Device must be stopped.
+		if ((instance->status != ao_status_stream_configured)
+		    && (instance->status != ao_status_stream_end)) {
+			PERROR
+			    ("Subdevice mustn't be runing when 'pre-load' mode is used.\n");
+			return ME_ERRNO_PREVIOUS_CONFIG;
+		}
+		break;
+	case ME_WRITE_MODE_NONBLOCKING:
+	case ME_WRITE_MODE_BLOCKING:
+		/// @note In blocking mode: When device is not runing and there is not enought space call will blocked up!
+		/// @note Some other thread must empty buffer by strating engine.
+		break;
+
+	default:
+		PERROR("Invalid write mode specified.\n");
+		return ME_ERRNO_INVALID_WRITE_MODE;
+	}
+
+	if (instance->mode & ME6000_AO_WRAP_MODE) {	//Wraparound mode. Device must be stopped.
+		if ((instance->status != ao_status_stream_configured)
+		    && (instance->status != ao_status_stream_end)) {
+			PERROR
+			    ("Subdevice mustn't be runing when 'pre-load' mode is used.\n");
+			return ME_ERRNO_INVALID_WRITE_MODE;
+		}
+	}
+
+	if ((instance->mode == ME6000_AO_HW_WRAP_MODE)
+	    && (write_mode != ME_WRITE_MODE_PRELOAD)) {
+/*
+		PERROR("Only 'pre-load' write is acceptable in hardware wraparound mode.\n");
+		return ME_ERRNO_PREVIOUS_CONFIG;
+*/
+		//This is transparent for user.
+		PDEBUG("Changing write_mode to ME_WRITE_MODE_PRELOAD.\n");
+		write_mode = ME_WRITE_MODE_PRELOAD;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	if (write_mode == ME_WRITE_MODE_PRELOAD) {	//Init enviroment - preload
+		spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+		reg_copy = inl(instance->ctrl_reg);
+		//Check FIFO
+		if (!(reg_copy & ME6000_AO_CTRL_BIT_ENABLE_FIFO)) {	//FIFO not active. Enable it.
+			reg_copy |= ME6000_AO_CTRL_BIT_ENABLE_FIFO;
+			outl(reg_copy, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   reg_copy);
+			instance->preloaded_count = 0;
+		}
+		spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+	}
+
+	while (1) {
+		//Copy to buffer. This step is common for all modes.
+		copied_from_user =
+		    ao_get_data_from_user(instance, left_to_copy_from_user,
+					  values + (*count -
+						    left_to_copy_from_user));
+		left_to_copy_from_user -= copied_from_user;
+
+		reg_copy = inl(instance->status_reg);
+		if ((instance->status == ao_status_stream_run) && !(reg_copy & ME6000_AO_STATUS_BIT_FSM)) {	//BROKEN PIPE! The state machine is stoped but logical status show that should be working.
+			PERROR("Broken pipe in write.\n");
+			err = ME_ERRNO_SUBDEVICE_NOT_RUNNING;
+			break;
+		}
+
+		if ((instance->status == ao_status_stream_run) && (instance->mode == ME6000_AO_CONTINOUS) && (reg_copy & ME6000_AO_STATUS_BIT_HF)) {	//Continous mode runing and data are below half!
+
+			// Block interrupts.
+			spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+			reg_copy = inl(instance->ctrl_reg);
+			reg_copy &= ~ME6000_AO_CTRL_BIT_ENABLE_IRQ;
+			outl(reg_copy, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   reg_copy);
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+
+			//Fast copy
+			copied_values =
+			    ao_write_data(instance, ME6000_AO_FIFO_COUNT / 2,
+					  0);
+			if (copied_values > 0) {
+				instance->circ_buf.tail += copied_values;
+				instance->circ_buf.tail &=
+				    instance->circ_buf.mask;
+				continue;
+			}
+			//Reset interrupt latch
+			inl(instance->irq_reset_reg);
+
+			// Activate interrupts.
+			spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+			reg_copy = inl(instance->ctrl_reg);
+			reg_copy |= ME6000_AO_CTRL_BIT_ENABLE_IRQ;
+			outl(reg_copy, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   reg_copy);
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+
+			if (copied_values == 0) {	//This was checked and never should happend!
+				PERROR_CRITICAL("COPY FINISH WITH 0!\n");
+			}
+
+			if (copied_values < 0) {	//This was checked and never should happend!
+				PERROR_CRITICAL("COPY FINISH WITH ERROR!\n");
+				instance->status = ao_status_stream_fifo_error;
+				err = ME_ERRNO_FIFO_BUFFER_OVERFLOW;
+				break;
+			}
+		}
+
+		if (!left_to_copy_from_user) {	//All datas were copied.
+			break;
+		} else {	//Not all datas were copied.
+			if (instance->mode & ME6000_AO_WRAP_MODE) {	//Error too much datas! Wraparound is limited in size!
+				PERROR
+				    ("Too much data for wraparound mode!  Exceeded size of %ld.\n",
+				     ME6000_AO_CIRC_BUF_COUNT - 1);
+				err = ME_ERRNO_RING_BUFFER_OVERFLOW;
+				break;
+			}
+
+			if (write_mode != ME_WRITE_MODE_BLOCKING) {	//Non blocking calls
+				break;
+			}
+
+			wait_event_interruptible(instance->wait_queue,
+						 me_circ_buf_space(&instance->
+								   circ_buf));
+
+			if (signal_pending(current)) {
+				PERROR("Writing interrupted by signal.\n");
+				instance->status = ao_status_none;
+				ao_stop_immediately(instance);
+				err = ME_ERRNO_SIGNAL;
+				break;
+			}
+
+			if (instance->status == ao_status_none) {	//Reset
+				PERROR("Writing interrupted by reset.\n");
+				err = ME_ERRNO_CANCELLED;
+				break;
+			}
+		}
+	}
+
+	if (write_mode == ME_WRITE_MODE_PRELOAD) {	//Copy data to FIFO - preload
+		copied_values =
+		    ao_write_data_pooling(instance, ME6000_AO_FIFO_COUNT,
+					  instance->preloaded_count);
+		instance->preloaded_count += copied_values;
+		instance->data_count += copied_values;
+
+		if ((instance->mode == ME6000_AO_HW_WRAP_MODE)
+		    && (me_circ_buf_values(&instance->circ_buf) >
+			ME6000_AO_FIFO_COUNT)) {
+			PERROR
+			    ("Too much data for hardware wraparound mode! Exceeded size of %d.\n",
+			     ME6000_AO_FIFO_COUNT);
+			err = ME_ERRNO_FIFO_BUFFER_OVERFLOW;
+		}
+	}
+
+	*count = *count - left_to_copy_from_user;
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 19)
+static irqreturn_t me6000_ao_isr(int irq, void *dev_id)
+#else
+static irqreturn_t me6000_ao_isr(int irq, void *dev_id, struct pt_regs *regs)
+#endif
+{
+	me6000_ao_subdevice_t *instance = dev_id;
+	uint32_t irq_status;
+	uint32_t ctrl;
+	uint32_t status;
+	int count = 0;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (irq != instance->irq) {
+		PERROR("Incorrect interrupt num: %d.\n", irq);
+		return IRQ_NONE;
+	}
+
+	irq_status = inl(instance->irq_status_reg);
+	if (!(irq_status & (ME6000_IRQ_STATUS_BIT_AO_HF << instance->ao_idx))) {
+		PINFO("%ld Shared interrupt. %s(): ID=%d: status_reg=0x%04X\n",
+		      jiffies, __FUNCTION__, instance->ao_idx, irq_status);
+		return IRQ_NONE;
+	}
+
+	if (!instance->circ_buf.buf) {
+		instance->status = ao_status_stream_error;
+		PERROR_CRITICAL("CIRCULAR BUFFER NOT EXISTS!\n");
+		//Block interrupts. Stop machine.
+		ctrl = inl(instance->ctrl_reg);
+		ctrl &= ~ME6000_AO_CTRL_BIT_ENABLE_IRQ;
+		ctrl |=
+		    ME6000_AO_CTRL_BIT_IMMEDIATE_STOP | ME6000_AO_CTRL_BIT_STOP;
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+
+		//Inform user
+		wake_up_interruptible_all(&instance->wait_queue);
+		return IRQ_HANDLED;
+	}
+
+	status = inl(instance->status_reg);
+	if (!(status & ME6000_AO_STATUS_BIT_FSM)) {	//Too late. Not working! END? BROKEN PIPE?
+		/// @note Error checking was moved to separate task.
+		PDEBUG("Interrupt come but ISM is not working!\n");
+		//Block interrupts. Stop machine.
+		ctrl = inl(instance->ctrl_reg);
+		ctrl &= ~ME6000_AO_CTRL_BIT_ENABLE_IRQ;
+		ctrl |=
+		    ME6000_AO_CTRL_BIT_STOP | ME6000_AO_CTRL_BIT_IMMEDIATE_STOP;
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+
+		//Reset interrupt latch
+		inl(instance->irq_reset_reg);
+
+		/// @note User notification was also moved to separate task.
+		return IRQ_HANDLED;
+	}
+	//General procedure. Process more datas.
+
+#ifdef MEDEBUG_DEBUG
+	if (!me_circ_buf_values(&instance->circ_buf)) {	//Buffer is empty!
+		PDEBUG("Circular buffer empty!\n");
+	}
+#endif
+
+	//Check FIFO
+	if (status & ME6000_AO_STATUS_BIT_HF) {	//OK less than half
+
+		//Block interrupts
+		ctrl = inl(instance->ctrl_reg);
+		ctrl &= ~ME6000_AO_CTRL_BIT_ENABLE_IRQ;
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+
+		do {
+			//Calculate how many should be copied.
+			count =
+			    (instance->stop_data_count) ? instance->
+			    stop_data_count -
+			    instance->data_count : ME6000_AO_FIFO_COUNT / 2;
+			if (ME6000_AO_FIFO_COUNT / 2 < count) {
+				count = ME6000_AO_FIFO_COUNT / 2;
+			}
+			//Copy data
+			if (instance->mode == ME6000_AO_CONTINOUS) {	//Continous
+				count = ao_write_data(instance, count, 0);
+				if (count > 0) {
+					instance->circ_buf.tail += count;
+					instance->circ_buf.tail &=
+					    instance->circ_buf.mask;
+					instance->data_count += count;
+
+					if ((instance->status == ao_status_stream_end_wait) && !me_circ_buf_values(&instance->circ_buf)) {	//Stoping. Whole buffer was copied.
+						break;
+					}
+				}
+			} else if ((instance->mode == ME6000_AO_SW_WRAP_MODE) && ((ctrl & ME6000_AO_CTRL_MODE_MASK) == ME6000_AO_MODE_CONTINUOUS)) {	//Wraparound (software)
+				if (instance->status == ao_status_stream_end_wait) {	//We stoping => Copy to the end of the buffer.
+					count =
+					    ao_write_data(instance, count, 0);
+				} else {	//Copy in wraparound mode.
+					count =
+					    ao_write_data_wraparound(instance,
+								     count,
+								     instance->
+								     preloaded_count);
+				}
+
+				if (count > 0) {
+					instance->data_count += count;
+					instance->preloaded_count += count;
+					instance->preloaded_count %=
+					    me_circ_buf_values(&instance->
+							       circ_buf);
+
+					if ((instance->status == ao_status_stream_end_wait) && !instance->preloaded_count) {	//Stoping. Whole buffer was copied.
+						break;
+					}
+				}
+			}
+
+			if ((count <= 0) || (instance->stop_data_count && (instance->stop_data_count <= instance->data_count))) {	//End of work.
+				break;
+			}
+		}		//Repeat if still is under half fifo
+		while ((status =
+			inl(instance->status_reg)) & ME6000_AO_STATUS_BIT_HF);
+
+		//Unblock interrupts
+		ctrl = inl(instance->ctrl_reg);
+		if (count >= 0) {	//Copy was successful.
+			if (instance->stop_data_count && (instance->stop_data_count <= instance->data_count)) {	//Finishing work. No more interrupts.
+				PDEBUG("Finishing work. Interrupt disabled.\n");
+				instance->status = ao_status_stream_end_wait;
+			} else if (count > 0) {	//Normal work. Enable interrupt.
+				PDEBUG("Normal work. Enable interrupt.\n");
+				ctrl |= ME6000_AO_CTRL_BIT_ENABLE_IRQ;
+			} else {	//Normal work but there are no more data in buffer. Interrupt blocked. stream_write() will unblock it.
+				PDEBUG
+				    ("No data in software buffer. Interrupt blocked.\n");
+			}
+		} else {	//Error during copy.
+			instance->status = ao_status_stream_fifo_error;
+		}
+
+		outl(ctrl, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, ctrl);
+	} else {		//?? more than half
+		PDEBUG
+		    ("Interrupt come but FIFO more than half full! Reset interrupt.\n");
+	}
+
+	PINFO("ISR: Buffer count: %d.(T:%d H:%d)\n",
+	      me_circ_buf_values(&instance->circ_buf), instance->circ_buf.tail,
+	      instance->circ_buf.head);
+	PINFO("ISR: Stop count: %d.\n", instance->stop_count);
+	PINFO("ISR: Stop data count: %d.\n", instance->stop_data_count);
+	PINFO("ISR: Data count: %d.\n", instance->data_count);
+
+	//Reset interrupt latch
+	inl(instance->irq_reset_reg);
+
+	//Inform user
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	return IRQ_HANDLED;
+}
+
+static void me6000_ao_destructor(struct me_subdevice *subdevice)
+{
+	me6000_ao_subdevice_t *instance;
+
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	instance->ao_control_task_flag = 0;
+
+	// Reset subdevice to asure clean exit.
+	me6000_ao_io_reset_subdevice(subdevice, NULL,
+				     ME_IO_RESET_SUBDEVICE_NO_FLAGS);
+
+	// Remove any tasks from work queue. This is paranoic because it was done allready in reset().
+	if (!cancel_delayed_work(&instance->ao_control_task)) {	//Wait 2 ticks to be sure that control task is removed from queue.
+		set_current_state(TASK_INTERRUPTIBLE);
+		schedule_timeout(2);
+	}
+
+	if (instance->fifo & ME6000_AO_HAS_FIFO) {
+		if (instance->irq) {
+			free_irq(instance->irq, instance);
+			instance->irq = 0;
+		}
+
+		if (instance->circ_buf.buf) {
+			PDEBUG("free circ_buf = %p size=%d",
+			       instance->circ_buf.buf,
+			       PAGE_SHIFT << ME6000_AO_CIRC_BUF_SIZE_ORDER);
+			free_pages((unsigned long)instance->circ_buf.buf,
+				   ME6000_AO_CIRC_BUF_SIZE_ORDER);
+		}
+		instance->circ_buf.buf = NULL;
+	}
+
+	me_subdevice_deinit(&instance->base);
+	kfree(instance);
+}
+
+me6000_ao_subdevice_t *me6000_ao_constructor(uint32_t reg_base,
+					     spinlock_t * preload_reg_lock,
+					     uint32_t * preload_flags,
+					     uint32_t * triggering_flags,
+					     int ao_idx,
+					     int fifo,
+					     int irq,
+					     int high_range,
+					     struct workqueue_struct *me6000_wq)
+{
+	me6000_ao_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed ID=%d.\n", ao_idx);
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me6000_ao_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me6000_ao_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	subdevice->preload_reg_lock = preload_reg_lock;
+	subdevice->preload_flags = preload_flags;
+	subdevice->triggering_flags = triggering_flags;
+
+	/* Store analog output index */
+	subdevice->ao_idx = ao_idx;
+
+	/* Store if analog output has fifo */
+	subdevice->fifo = fifo;
+
+	if (subdevice->fifo & ME6000_AO_HAS_FIFO) {
+		/* Allocate and initialize circular buffer */
+		subdevice->circ_buf.mask = ME6000_AO_CIRC_BUF_COUNT - 1;
+		subdevice->circ_buf.buf =
+		    (void *)__get_free_pages(GFP_KERNEL,
+					     ME6000_AO_CIRC_BUF_SIZE_ORDER);
+		PDEBUG("circ_buf = %p size=%ld\n", subdevice->circ_buf.buf,
+		       ME6000_AO_CIRC_BUF_SIZE);
+
+		if (!subdevice->circ_buf.buf) {
+			PERROR
+			    ("Cannot initialize subdevice base class instance.\n");
+			kfree(subdevice);
+			return NULL;
+		}
+
+		memset(subdevice->circ_buf.buf, 0, ME6000_AO_CIRC_BUF_SIZE);
+	} else {
+		subdevice->circ_buf.mask = 0;
+		subdevice->circ_buf.buf = NULL;
+	}
+	subdevice->circ_buf.head = 0;
+	subdevice->circ_buf.tail = 0;
+
+	subdevice->status = ao_status_none;
+	subdevice->ao_control_task_flag = 0;
+	subdevice->timeout.delay = 0;
+	subdevice->timeout.start_time = jiffies;
+
+	/* Initialize wait queue */
+	init_waitqueue_head(&subdevice->wait_queue);
+
+	/* Initialize single value to 0V */
+	subdevice->single_value = 0x8000;
+	subdevice->single_value_in_fifo = 0x8000;
+
+	/* Initialize range boarders */
+	if (high_range) {
+		subdevice->min = ME6000_AO_MIN_RANGE_HIGH;
+		subdevice->max = ME6000_AO_MAX_RANGE_HIGH;
+	} else {
+		subdevice->min = ME6000_AO_MIN_RANGE;
+		subdevice->max = ME6000_AO_MAX_RANGE;
+	}
+
+	/* Register interrupt service routine */
+
+	if (subdevice->fifo & ME6000_AO_HAS_FIFO) {
+		subdevice->irq = irq;
+		if (request_irq(subdevice->irq, me6000_ao_isr,
+#ifdef IRQF_DISABLED
+				IRQF_DISABLED | IRQF_SHARED,
+#else
+				SA_INTERRUPT | SA_SHIRQ,
+#endif
+				ME6000_NAME, subdevice)) {
+			PERROR("Cannot get interrupt line.\n");
+			PDEBUG("free circ_buf = %p size=%d",
+			       subdevice->circ_buf.buf,
+			       PAGE_SHIFT << ME6000_AO_CIRC_BUF_SIZE_ORDER);
+			free_pages((unsigned long)subdevice->circ_buf.buf,
+				   ME6000_AO_CIRC_BUF_SIZE_ORDER);
+			subdevice->circ_buf.buf = NULL;
+			kfree(subdevice);
+			return NULL;
+		}
+		PINFO("Registered irq=%d.\n", subdevice->irq);
+	} else {
+		subdevice->irq = 0;
+	}
+
+	/* Initialize registers */
+	// Only streamed subdevices support interrupts. For the rest this register has no meaning.
+	subdevice->irq_status_reg = reg_base + ME6000_AO_IRQ_STATUS_REG;
+	subdevice->preload_reg = reg_base + ME6000_AO_PRELOAD_REG;
+
+	if (ao_idx == 0) {
+		subdevice->ctrl_reg = reg_base + ME6000_AO_00_CTRL_REG;
+		subdevice->status_reg = reg_base + ME6000_AO_00_STATUS_REG;
+		subdevice->fifo_reg = reg_base + ME6000_AO_00_FIFO_REG;
+		subdevice->timer_reg = reg_base + ME6000_AO_00_TIMER_REG;
+		subdevice->irq_reset_reg =
+		    reg_base + ME6000_AO_00_IRQ_RESET_REG;
+		subdevice->single_reg = reg_base + ME6000_AO_00_SINGLE_REG;
+	} else if (ao_idx == 1) {
+		subdevice->ctrl_reg = reg_base + ME6000_AO_01_CTRL_REG;
+		subdevice->status_reg = reg_base + ME6000_AO_01_STATUS_REG;
+		subdevice->fifo_reg = reg_base + ME6000_AO_01_FIFO_REG;
+		subdevice->timer_reg = reg_base + ME6000_AO_01_TIMER_REG;
+		subdevice->irq_reset_reg =
+		    reg_base + ME6000_AO_01_IRQ_RESET_REG;
+		subdevice->single_reg = reg_base + ME6000_AO_01_SINGLE_REG;
+	} else if (ao_idx == 2) {
+		subdevice->ctrl_reg = reg_base + ME6000_AO_02_CTRL_REG;
+		subdevice->status_reg = reg_base + ME6000_AO_02_STATUS_REG;
+		subdevice->fifo_reg = reg_base + ME6000_AO_02_FIFO_REG;
+		subdevice->timer_reg = reg_base + ME6000_AO_02_TIMER_REG;
+		subdevice->irq_reset_reg =
+		    reg_base + ME6000_AO_02_IRQ_RESET_REG;
+		subdevice->single_reg = reg_base + ME6000_AO_02_SINGLE_REG;
+	} else if (ao_idx == 3) {
+		subdevice->ctrl_reg = reg_base + ME6000_AO_03_CTRL_REG;
+		subdevice->status_reg = reg_base + ME6000_AO_03_STATUS_REG;
+		subdevice->fifo_reg = reg_base + ME6000_AO_03_FIFO_REG;
+		subdevice->timer_reg = reg_base + ME6000_AO_03_TIMER_REG;
+		subdevice->irq_reset_reg =
+		    reg_base + ME6000_AO_03_IRQ_RESET_REG;
+		subdevice->single_reg = reg_base + ME6000_AO_03_SINGLE_REG;
+	} else {
+		subdevice->ctrl_reg = reg_base + ME6000_AO_DUMY;
+		subdevice->fifo_reg = reg_base + ME6000_AO_DUMY;
+		subdevice->timer_reg = reg_base + ME6000_AO_DUMY;
+		subdevice->irq_reset_reg = reg_base + ME6000_AO_DUMY;
+		subdevice->single_reg = reg_base + ME6000_AO_DUMY;
+
+		subdevice->status_reg = reg_base + ME6000_AO_SINGLE_STATUS_REG;
+		if (ao_idx == 4) {
+			subdevice->single_reg =
+			    reg_base + ME6000_AO_04_SINGLE_REG;
+		} else if (ao_idx == 5) {
+			subdevice->single_reg =
+			    reg_base + ME6000_AO_05_SINGLE_REG;
+		} else if (ao_idx == 6) {
+			subdevice->single_reg =
+			    reg_base + ME6000_AO_06_SINGLE_REG;
+		} else if (ao_idx == 7) {
+			subdevice->single_reg =
+			    reg_base + ME6000_AO_07_SINGLE_REG;
+		} else if (ao_idx == 8) {
+			subdevice->single_reg =
+			    reg_base + ME6000_AO_08_SINGLE_REG;
+		} else if (ao_idx == 9) {
+			subdevice->single_reg =
+			    reg_base + ME6000_AO_09_SINGLE_REG;
+		} else if (ao_idx == 10) {
+			subdevice->single_reg =
+			    reg_base + ME6000_AO_10_SINGLE_REG;
+		} else if (ao_idx == 11) {
+			subdevice->single_reg =
+			    reg_base + ME6000_AO_11_SINGLE_REG;
+		} else if (ao_idx == 12) {
+			subdevice->single_reg =
+			    reg_base + ME6000_AO_12_SINGLE_REG;
+		} else if (ao_idx == 13) {
+			subdevice->single_reg =
+			    reg_base + ME6000_AO_13_SINGLE_REG;
+		} else if (ao_idx == 14) {
+			subdevice->single_reg =
+			    reg_base + ME6000_AO_14_SINGLE_REG;
+		} else if (ao_idx == 15) {
+			subdevice->single_reg =
+			    reg_base + ME6000_AO_15_SINGLE_REG;
+		} else {
+			PERROR_CRITICAL("WRONG SUBDEVICE ID=%d!", ao_idx);
+			me_subdevice_deinit((me_subdevice_t *) subdevice);
+			if (subdevice->fifo) {
+				free_pages((unsigned long)subdevice->circ_buf.
+					   buf, ME6000_AO_CIRC_BUF_SIZE_ORDER);
+			}
+			subdevice->circ_buf.buf = NULL;
+			kfree(subdevice);
+			return NULL;
+		}
+	}
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = reg_base;
+#endif
+
+	/* Override base class methods. */
+	subdevice->base.me_subdevice_destructor = me6000_ao_destructor;
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me6000_ao_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me6000_ao_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me6000_ao_io_single_read;
+	subdevice->base.me_subdevice_io_single_write =
+	    me6000_ao_io_single_write;
+	subdevice->base.me_subdevice_io_stream_config =
+	    me6000_ao_io_stream_config;
+	subdevice->base.me_subdevice_io_stream_new_values =
+	    me6000_ao_io_stream_new_values;
+	subdevice->base.me_subdevice_io_stream_write =
+	    me6000_ao_io_stream_write;
+	subdevice->base.me_subdevice_io_stream_start =
+	    me6000_ao_io_stream_start;
+	subdevice->base.me_subdevice_io_stream_status =
+	    me6000_ao_io_stream_status;
+	subdevice->base.me_subdevice_io_stream_stop = me6000_ao_io_stream_stop;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me6000_ao_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me6000_ao_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me6000_ao_query_subdevice_caps;
+	subdevice->base.me_subdevice_query_subdevice_caps_args =
+	    me6000_ao_query_subdevice_caps_args;
+	subdevice->base.me_subdevice_query_range_by_min_max =
+	    me6000_ao_query_range_by_min_max;
+	subdevice->base.me_subdevice_query_number_ranges =
+	    me6000_ao_query_number_ranges;
+	subdevice->base.me_subdevice_query_range_info =
+	    me6000_ao_query_range_info;
+	subdevice->base.me_subdevice_query_timer = me6000_ao_query_timer;
+
+	//prepare work queue and work function
+	subdevice->me6000_workqueue = me6000_wq;
+
+/* workqueue API changed in kernel 2.6.20 */
+#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20) )
+	INIT_WORK(&subdevice->ao_control_task, me6000_ao_work_control_task,
+		  (void *)subdevice);
+#else
+	INIT_DELAYED_WORK(&subdevice->ao_control_task,
+			  me6000_ao_work_control_task);
+#endif
+
+	if (subdevice->fifo) {	//Set speed
+		outl(ME6000_AO_MIN_CHAN_TICKS - 1, subdevice->timer_reg);
+		subdevice->hardware_stop_delay = HZ / 10;	//100ms
+	}
+
+	return subdevice;
+}
+
+/** @brief Stop presentation. Preserve FIFOs.
+*
+* @param instance The subdevice instance (pointer).
+*/
+int inline ao_stop_immediately(me6000_ao_subdevice_t * instance)
+{
+	unsigned long cpu_flags;
+	uint32_t ctrl;
+	int timeout;
+	int i;
+	uint32_t single_mask;
+
+	single_mask =
+	    (instance->ao_idx - ME6000_AO_SINGLE_STATUS_OFFSET <
+	     0) ? 0x0000 : (0x0001 << (instance->ao_idx -
+				       ME6000_AO_SINGLE_STATUS_OFFSET));
+
+	timeout =
+	    (instance->hardware_stop_delay >
+	     (HZ / 10)) ? instance->hardware_stop_delay : HZ / 10;
+	for (i = 0; i <= timeout; i++) {
+		if (instance->fifo) {
+			spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+			// Stop all actions. No conditions! Block interrupts. Leave FIFO untouched!
+			ctrl = inl(instance->ctrl_reg);
+			ctrl |=
+			    ME6000_AO_CTRL_BIT_STOP |
+			    ME6000_AO_CTRL_BIT_IMMEDIATE_STOP;
+			ctrl &=
+			    ~(ME6000_AO_CTRL_BIT_ENABLE_IRQ |
+			      ME6000_AO_CTRL_BIT_ENABLE_EX_TRIG);
+			outl(ctrl, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   ctrl);
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+
+			if (!(inl(instance->status_reg) & ME6000_AO_STATUS_BIT_FSM)) {	// Exit.
+				break;
+			}
+		} else {
+			if (!(inl(instance->status_reg) & single_mask)) {	// Exit.
+				break;
+			}
+		}
+
+		PINFO("<%s> Wait for stop: %d\n", __FUNCTION__, i);
+
+		//Still working!
+		set_current_state(TASK_INTERRUPTIBLE);
+		schedule_timeout(1);
+	}
+
+	if (i > timeout) {
+		PERROR_CRITICAL("FSM IS BUSY!\n");
+		return ME_ERRNO_INTERNAL;
+	}
+	return ME_ERRNO_SUCCESS;
+}
+
+/** @brief Copy data from circular buffer to fifo (fast) in wraparound.
+* @note This is time critical function. Checking is done at begining and end only.
+* @note The is not reasonable way to check how many walues was in FIFO at begining. The count must be managed externaly.
+*
+* @param instance The subdevice instance (pointer).
+* @param count Maximum number of copied data.
+* @param start_pos Position of the firs value in buffer.
+*
+* @return On success: Number of copied data.
+* @return On error/success: 0.	No datas were copied => no data in buffer.
+* @return On error: -ME_ERRNO_FIFO_BUFFER_OVERFLOW.
+*/
+int inline ao_write_data_wraparound(me6000_ao_subdevice_t * instance, int count,
+				    int start_pos)
+{				/// @note This is time critical function!
+	uint32_t status;
+	uint32_t value;
+	int pos =
+	    (instance->circ_buf.tail + start_pos) & instance->circ_buf.mask;
+	int local_count = count;
+	int i = 1;
+
+	if (count <= 0) {	//Wrong count!
+		return 0;
+	}
+
+	while (i < local_count) {
+		//Get value from buffer
+		value = *(instance->circ_buf.buf + pos);
+		//Prepare it
+		if (instance->ao_idx & 0x1) {
+			value <<= 16;
+		}
+		//Put value to FIFO
+		outl(value, instance->fifo_reg);
+		//PDEBUG_REG("idx=%d fifo_reg outl(0x%lX+0x%lX)=0x%x\n", instance->ao_idx, instance->reg_base, instance->fifo_reg - instance->reg_base, value);
+
+		pos++;
+		pos &= instance->circ_buf.mask;
+		if (pos == instance->circ_buf.head) {
+			pos = instance->circ_buf.tail;
+		}
+		i++;
+	}
+
+	status = inl(instance->status_reg);
+	if (!(status & ME6000_AO_STATUS_BIT_FF)) {	//FIFO is full before all datas were copied!
+		PERROR("idx=%d FIFO is full before all datas were copied!\n",
+		       instance->ao_idx);
+		return -ME_ERRNO_FIFO_BUFFER_OVERFLOW;
+	} else {		//Add last value
+		value = *(instance->circ_buf.buf + pos);
+		if (instance->ao_idx & 0x1) {
+			value <<= 16;
+		}
+		//Put value to FIFO
+		outl(value, instance->fifo_reg);
+		//PDEBUG_REG("idx=%d fifo_reg outl(0x%lX+0x%lX)=0x%x\n", instance->ao_idx, instance->reg_base, instance->fifo_reg - instance->reg_base, value);
+	}
+
+	PINFO("idx=%d WRAPAROUND LOADED %d values\n", instance->ao_idx,
+	      local_count);
+	return local_count;
+}
+
+/** @brief Copy data from software buffer to fifo (fast).
+* @note This is time critical function. Checking is done at begining and end only.
+* @note The is not reasonable way to check how many walues was in FIFO at begining. The count must be managed externaly.
+*
+* @param instance The subdevice instance (pointer).
+* @param count Maximum number of copied data.
+* @param start_pos Position of the firs value in buffer.
+*
+* @return On success: Number of copied data.
+* @return On error/success: 0.	No datas were copied => no data in buffer.
+* @return On error: -ME_ERRNO_FIFO_BUFFER_OVERFLOW.
+*/
+int inline ao_write_data(me6000_ao_subdevice_t * instance, int count,
+			 int start_pos)
+{				/// @note This is time critical function!
+	uint32_t status;
+	uint32_t value;
+	int pos =
+	    (instance->circ_buf.tail + start_pos) & instance->circ_buf.mask;
+	int local_count = count;
+	int max_count;
+	int i = 1;
+
+	if (count <= 0) {	//Wrong count!
+		return 0;
+	}
+
+	max_count = me_circ_buf_values(&instance->circ_buf) - start_pos;
+	if (max_count <= 0) {	//No data to copy!
+		return 0;
+	}
+
+	if (max_count < count) {
+		local_count = max_count;
+	}
+
+	while (i < local_count) {
+		//Get value from buffer
+		value = *(instance->circ_buf.buf + pos);
+		//Prepare it
+		if (instance->ao_idx & 0x1) {
+			value <<= 16;
+		}
+		//Put value to FIFO
+		outl(value, instance->fifo_reg);
+		//PDEBUG_REG("idx=%d fifo_reg outl(0x%lX+0x%lX)=0x%x\n", instance->ao_idx, instance->reg_base, instance->fifo_reg - instance->reg_base, value);
+
+		pos++;
+		pos &= instance->circ_buf.mask;
+		i++;
+	}
+
+	status = inl(instance->status_reg);
+	if (!(status & ME6000_AO_STATUS_BIT_FF)) {	//FIFO is full before all datas were copied!
+		PERROR("idx=%d FIFO is full before all datas were copied!\n",
+		       instance->ao_idx);
+		return -ME_ERRNO_FIFO_BUFFER_OVERFLOW;
+	} else {		//Add last value
+		value = *(instance->circ_buf.buf + pos);
+		if (instance->ao_idx & 0x1) {
+			value <<= 16;
+		}
+		//Put value to FIFO
+		outl(value, instance->fifo_reg);
+		//PDEBUG_REG("idx=%d fifo_reg outl(0x%lX+0x%lX)=0x%x\n", instance->ao_idx, instance->reg_base, instance->fifo_reg - instance->reg_base, value);
+	}
+
+	PINFO("idx=%d FAST LOADED %d values\n", instance->ao_idx, local_count);
+	return local_count;
+}
+
+/** @brief Copy data from software buffer to fifo (slow).
+* @note This is slow function that copy all data from buffer to FIFO with full control.
+*
+* @param instance The subdevice instance (pointer).
+* @param count Maximum number of copied data.
+* @param start_pos Position of the firs value in buffer.
+*
+* @return On success: Number of copied values.
+* @return On error/success: 0.	FIFO was full at begining.
+* @return On error: -ME_ERRNO_RING_BUFFER_UNDEFFLOW.
+*/
+int inline ao_write_data_pooling(me6000_ao_subdevice_t * instance, int count,
+				 int start_pos)
+{				/// @note This is slow function!
+	uint32_t status;
+	uint32_t value;
+	int pos =
+	    (instance->circ_buf.tail + start_pos) & instance->circ_buf.mask;
+	int local_count = count;
+	int i;
+	int max_count;
+
+	if (count <= 0) {	//Wrong count!
+		PERROR("idx=%d SLOW LOADED: Wrong count!\n", instance->ao_idx);
+		return 0;
+	}
+
+	max_count = me_circ_buf_values(&instance->circ_buf) - start_pos;
+	if (max_count <= 0) {	//No data to copy!
+		PERROR("idx=%d SLOW LOADED: No data to copy!\n",
+		       instance->ao_idx);
+		return 0;
+	}
+
+	if (max_count < count) {
+		local_count = max_count;
+	}
+
+	for (i = 0; i < local_count; i++) {
+		status = inl(instance->status_reg);
+		if (!(status & ME6000_AO_STATUS_BIT_FF)) {	//FIFO is full!
+			return i;
+		}
+		//Get value from buffer
+		value = *(instance->circ_buf.buf + pos);
+		//Prepare it
+		if (instance->ao_idx & 0x1) {
+			value <<= 16;
+		}
+		//Put value to FIFO
+		outl(value, instance->fifo_reg);
+		//PDEBUG_REG("idx=%d fifo_reg outl(0x%lX+0x%lX)=0x%x\n", instance->ao_idx, instance->reg_base, instance->fifo_reg - instance->reg_base, value);
+
+		pos++;
+		pos &= instance->circ_buf.mask;
+	}
+
+	PINFO("idx=%d SLOW LOADED %d values\n", instance->ao_idx, local_count);
+	return local_count;
+}
+
+/** @brief Copy data from user space to circular buffer.
+* @param instance The subdevice instance (pointer).
+* @param count Number of datas in user space.
+* @param user_values Buffer's pointer.
+*
+* @return On success: Number of copied values.
+* @return On error: -ME_ERRNO_INTERNAL.
+*/
+int inline ao_get_data_from_user(me6000_ao_subdevice_t * instance, int count,
+				 int *user_values)
+{
+	int i, err;
+	int empty_space;
+	int copied;
+	int value;
+
+	empty_space = me_circ_buf_space(&instance->circ_buf);
+	//We have only this space free.
+	copied = (count < empty_space) ? count : empty_space;
+	for (i = 0; i < copied; i++) {	//Copy from user to buffer
+		if ((err = get_user(value, (int *)(user_values + i)))) {
+			PERROR
+			    ("idx=%d BUFFER LOADED: get_user(0x%p) return an error: %d\n",
+			     instance->ao_idx, user_values + i, err);
+			return -ME_ERRNO_INTERNAL;
+		}
+		/// @note The analog output in me6000 series has size of 16 bits.
+		*(instance->circ_buf.buf + instance->circ_buf.head) =
+		    (uint16_t) value;
+		instance->circ_buf.head++;
+		instance->circ_buf.head &= instance->circ_buf.mask;
+	}
+
+	PINFO("idx=%d BUFFER LOADED %d values\n", instance->ao_idx, copied);
+	return copied;
+}
+
+static void me6000_ao_work_control_task(
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+					       void *subdevice
+#else
+					       struct work_struct *work
+#endif
+    )
+{
+	me6000_ao_subdevice_t *instance;
+	unsigned long cpu_flags = 0;
+	uint32_t status;
+	uint32_t ctrl;
+	uint32_t synch;
+	int reschedule = 0;
+	int signaling = 0;
+	uint32_t single_mask;
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+	instance = (me6000_ao_subdevice_t *) subdevice;
+#else
+	instance =
+	    container_of((void *)work, me6000_ao_subdevice_t, ao_control_task);
+#endif
+	PINFO("<%s: %ld> executed. idx=%d\n", __FUNCTION__, jiffies,
+	      instance->ao_idx);
+
+	status = inl(instance->status_reg);
+	PDEBUG_REG("status_reg inl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->status_reg - instance->reg_base, status);
+
+/// @note AO_STATUS_BIT_FSM doesn't work as should be for pure single channels (idx>=4)
+//      single_mask = (instance->ao_idx-ME6000_AO_SINGLE_STATUS_OFFSET < 0) ? 0x0000 : (0x0001 << (instance->ao_idx-ME6000_AO_SINGLE_STATUS_OFFSET));
+	single_mask = *instance->triggering_flags & (0x1 << instance->ao_idx);
+
+	switch (instance->status) {	// Checking actual mode.
+
+		// Not configured for work.
+	case ao_status_none:
+		break;
+
+		//This are stable modes. No need to do anything. (?)
+	case ao_status_single_configured:
+	case ao_status_stream_configured:
+	case ao_status_stream_fifo_error:
+	case ao_status_stream_buffer_error:
+	case ao_status_stream_error:
+		PERROR("Shouldn't be running!.\n");
+		break;
+
+		// Single modes
+	case ao_status_single_run_wait:
+	case ao_status_single_run:
+	case ao_status_single_end_wait:
+		if (instance->fifo) {	// Extra registers.
+			if (!(status & ME6000_AO_STATUS_BIT_FSM)) {	// State machine is not working.
+				if (((instance->fifo & ME6000_AO_HAS_FIFO)
+				     && (!(status & ME6000_AO_STATUS_BIT_EF)))
+				    || (!(instance->fifo & ME6000_AO_HAS_FIFO))) {	// Single is in end state.
+					PDEBUG
+					    ("Single call has been complited.\n");
+
+					// Set correct value for single_read();
+					instance->single_value =
+					    instance->single_value_in_fifo;
+
+					// Set status as 'ao_status_single_end'
+					instance->status = ao_status_single_end;
+
+					spin_lock(instance->preload_reg_lock);
+					if ((single_mask) && (*instance->preload_flags & (ME6000_AO_SYNC_HOLD << instance->ao_idx))) {	// This is one of synchronous start channels. Set all as triggered.
+						*instance->triggering_flags =
+						    0x00000000;
+					} else {
+						//Set this channel as triggered (none active).
+						*instance->triggering_flags &=
+						    ~(0x1 << instance->ao_idx);
+					}
+					spin_unlock(instance->preload_reg_lock);
+
+					// Signal the end.
+					signaling = 1;
+					// Wait for stop ISM.
+					reschedule = 1;
+
+					break;
+				}
+			}
+			// Check timeout.
+			if ((instance->timeout.delay) && ((jiffies - instance->timeout.start_time) >= instance->timeout.delay)) {	// Timeout
+				PDEBUG("Timeout reached.\n");
+				// Stop all actions. No conditions! Block interrupts and trigger. Leave FIFO untouched!
+				spin_lock_irqsave(&instance->subdevice_lock,
+						  cpu_flags);
+				ctrl = inl(instance->ctrl_reg);
+				ctrl |=
+				    ME6000_AO_CTRL_BIT_STOP |
+				    ME6000_AO_CTRL_BIT_IMMEDIATE_STOP;
+				ctrl &=
+				    ~(ME6000_AO_CTRL_BIT_ENABLE_IRQ |
+				      ME6000_AO_CTRL_BIT_ENABLE_EX_TRIG);
+				ctrl &=
+				    ~(ME6000_AO_CTRL_BIT_EX_TRIG_EDGE |
+				      ME6000_AO_CTRL_BIT_EX_TRIG_EDGE_BOTH);
+				//Disabling FIFO
+				ctrl &= ~ME6000_AO_CTRL_BIT_ENABLE_FIFO;
+
+				outl(ctrl, instance->ctrl_reg);
+				PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+					   instance->reg_base,
+					   instance->ctrl_reg -
+					   instance->reg_base, ctrl);
+				spin_unlock_irqrestore(&instance->
+						       subdevice_lock,
+						       cpu_flags);
+
+				//Reset interrupt latch
+				inl(instance->irq_reset_reg);
+
+				spin_lock(instance->preload_reg_lock);
+				//Remove from synchronous start. Block triggering from this output.
+				synch = inl(instance->preload_reg);
+				synch &=
+				    ~((ME6000_AO_SYNC_HOLD |
+				       ME6000_AO_SYNC_EXT_TRIG) << instance->
+				      ao_idx);
+				if (!(instance->fifo & ME6000_AO_HAS_FIFO)) {	// No FIFO - set to single safe mode
+					synch |=
+					    ME6000_AO_SYNC_HOLD << instance->
+					    ao_idx;
+				}
+				outl(synch, instance->preload_reg);
+				PDEBUG_REG
+				    ("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				     instance->reg_base,
+				     instance->preload_reg - instance->reg_base,
+				     synch);
+				//Set this channel as triggered (none active).
+				*instance->triggering_flags &=
+				    ~(0x1 << instance->ao_idx);
+				spin_unlock(instance->preload_reg_lock);
+
+				// Set correct value for single_read();
+				instance->single_value_in_fifo =
+				    instance->single_value;
+
+				instance->status = ao_status_single_end;
+
+				// Signal the end.
+				signaling = 1;
+			}
+		} else {	// No extra registers.
+/*
+				if (!(status & single_mask))
+				{// State machine is not working.
+					PDEBUG("Single call has been complited.\n");
+
+					// Set correct value for single_read();
+					instance->single_value = instance->single_value_in_fifo;
+
+					// Set status as 'ao_status_single_end'
+					instance->status = ao_status_single_end;
+
+					// Signal the end.
+					signaling = 1;
+					// Wait for stop ISM.
+					reschedule = 1;
+
+					break;
+				}
+*/
+			if (!single_mask) {	// Was triggered.
+				PDEBUG("Single call has been complited.\n");
+
+				// Set correct value for single_read();
+				instance->single_value =
+				    instance->single_value_in_fifo;
+
+				// Set status as 'ao_status_single_end'
+				instance->status = ao_status_single_end;
+
+				// Signal the end.
+				signaling = 1;
+
+				break;
+			}
+			// Check timeout.
+			if ((instance->timeout.delay) && ((jiffies - instance->timeout.start_time) >= instance->timeout.delay)) {	// Timeout
+				PDEBUG("Timeout reached.\n");
+
+				spin_lock(instance->preload_reg_lock);
+				//Remove from synchronous start. Block triggering from this output.
+				synch = inl(instance->preload_reg);
+				synch &=
+				    ~(ME6000_AO_SYNC_EXT_TRIG << instance->
+				      ao_idx);
+				synch |=
+				    ME6000_AO_SYNC_HOLD << instance->ao_idx;
+
+				outl(synch, instance->preload_reg);
+				PDEBUG_REG
+				    ("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				     instance->reg_base,
+				     instance->preload_reg - instance->reg_base,
+				     synch);
+				//Set this channel as triggered (none active).
+				*instance->triggering_flags &=
+				    ~(0x1 << instance->ao_idx);
+				spin_unlock(instance->preload_reg_lock);
+
+				// Restore old settings.
+				PDEBUG("Write old value back to register.\n");
+				outl(instance->single_value,
+				     instance->single_reg);
+				PDEBUG_REG
+				    ("single_reg outl(0x%lX+0x%lX)=0x%x\n",
+				     instance->reg_base,
+				     instance->single_reg - instance->reg_base,
+				     instance->single_value);
+
+				// Set correct value for single_read();
+				instance->single_value_in_fifo =
+				    instance->single_value;
+
+				instance->status = ao_status_single_end;
+
+				// Signal the end.
+				signaling = 1;
+			}
+		}
+
+		// Wait for stop.
+		reschedule = 1;
+		break;
+
+	case ao_status_stream_end:
+		if (!(instance->fifo & ME6000_AO_HAS_FIFO)) {	// No FIFO
+			PERROR_CRITICAL
+			    ("Streaming on single device! This feature is not implemented in this version!\n");
+			instance->status = ao_status_stream_error;
+			// Signal the end.
+			signaling = 1;
+			break;
+		}
+	case ao_status_single_end:
+		if (instance->fifo) {	// Extra registers.
+			if (status & ME6000_AO_STATUS_BIT_FSM) {	// State machine is working but the status is set to end. Force stop.
+
+				// Wait for stop.
+				reschedule = 1;
+			}
+
+			spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+			// Stop all actions. No conditions! Block interrupts and trigger. Leave FIFO untouched!
+			ctrl = inl(instance->ctrl_reg);
+			ctrl |=
+			    ME6000_AO_CTRL_BIT_IMMEDIATE_STOP |
+			    ME6000_AO_CTRL_BIT_STOP;
+			ctrl &=
+			    ~(ME6000_AO_CTRL_BIT_ENABLE_IRQ |
+			      ME6000_AO_CTRL_BIT_ENABLE_EX_TRIG);
+			outl(ctrl, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   ctrl);
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+
+			//Reset interrupt latch
+			inl(instance->irq_reset_reg);
+		} else {	// No extra registers.
+/*
+				if (status & single_mask)
+				{// State machine is working but the status is set to end. Force stop.
+
+					// Wait for stop.
+					reschedule = 1;
+				}
+*/
+		}
+		break;
+
+		// Stream modes
+	case ao_status_stream_run_wait:
+		if (!(instance->fifo & ME6000_AO_HAS_FIFO)) {	// No FIFO
+			PERROR_CRITICAL
+			    ("Streaming on single device! This feature is not implemented in this version!\n");
+			instance->status = ao_status_stream_error;
+			// Signal the end.
+			signaling = 1;
+			break;
+		}
+
+		if (status & ME6000_AO_STATUS_BIT_FSM) {	// State machine is working. Waiting for start finish.
+			instance->status = ao_status_stream_run;
+
+			// Signal end of this step
+			signaling = 1;
+		} else {	// State machine is not working.
+			if (!(status & ME6000_AO_STATUS_BIT_EF)) {	// FIFO is empty. Procedure has started and finish already!
+				instance->status = ao_status_stream_end;
+
+				// Signal the end.
+				signaling = 1;
+				// Wait for stop.
+				reschedule = 1;
+				break;
+			}
+		}
+
+		// Check timeout.
+		if ((instance->timeout.delay) && ((jiffies - instance->timeout.start_time) >= instance->timeout.delay)) {	// Timeout
+			PDEBUG("Timeout reached.\n");
+			// Stop all actions. No conditions! Block interrupts. Leave FIFO untouched!
+			spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+			ctrl = inl(instance->ctrl_reg);
+			ctrl |=
+			    ME6000_AO_CTRL_BIT_STOP |
+			    ME6000_AO_CTRL_BIT_IMMEDIATE_STOP;
+			ctrl &=
+			    ~(ME6000_AO_CTRL_BIT_ENABLE_IRQ |
+			      ME6000_AO_CTRL_BIT_ENABLE_EX_TRIG);
+			outl(ctrl, instance->ctrl_reg);
+			PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->ctrl_reg - instance->reg_base,
+				   ctrl);
+			spin_unlock_irqrestore(&instance->subdevice_lock,
+					       cpu_flags);
+
+			//Reset interrupt latch
+			inl(instance->irq_reset_reg);
+
+			spin_lock(instance->preload_reg_lock);
+			//Remove from synchronous start. Block triggering from this output.
+			synch = inl(instance->preload_reg);
+			synch &=
+			    ~((ME6000_AO_SYNC_HOLD | ME6000_AO_SYNC_EXT_TRIG) <<
+			      instance->ao_idx);
+			outl(synch, instance->preload_reg);
+			PDEBUG_REG("preload_reg outl(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->preload_reg - instance->reg_base,
+				   synch);
+			spin_unlock(instance->preload_reg_lock);
+
+			instance->status = ao_status_stream_end;
+
+			// Signal the end.
+			signaling = 1;
+		}
+		// Wait for stop.
+		reschedule = 1;
+		break;
+
+	case ao_status_stream_run:
+		if (!(instance->fifo & ME6000_AO_HAS_FIFO)) {	// No FIFO
+			PERROR_CRITICAL
+			    ("Streaming on single device! This feature is not implemented in this version!\n");
+			instance->status = ao_status_stream_error;
+			// Signal the end.
+			signaling = 1;
+			break;
+		}
+
+		if (!(status & ME6000_AO_STATUS_BIT_FSM)) {	// State machine is not working. This is an error.
+			// BROKEN PIPE!
+			if (!(status & ME6000_AO_STATUS_BIT_EF)) {	// FIFO is empty.
+				if (me_circ_buf_values(&instance->circ_buf)) {	// Software buffer is not empty.
+					if (instance->stop_data_count && (instance->stop_data_count <= instance->data_count)) {	//Finishing work. Requed data shown.
+						PDEBUG
+						    ("ISM stoped. No data in FIFO. Buffer is not empty.\n");
+						instance->status =
+						    ao_status_stream_end;
+					} else {
+						PERROR
+						    ("Output stream has been broken. ISM stoped. No data in FIFO. Buffer is not empty.\n");
+						instance->status =
+						    ao_status_stream_buffer_error;
+					}
+				} else {	// Software buffer is empty.
+					PDEBUG
+					    ("ISM stoped. No data in FIFO. Buffer is empty.\n");
+					instance->status = ao_status_stream_end;
+				}
+			} else {	// There are still datas in FIFO.
+				if (me_circ_buf_values(&instance->circ_buf)) {	// Software buffer is not empty.
+					PERROR
+					    ("Output stream has been broken. ISM stoped but some data in FIFO and buffer.\n");
+				} else {	// Software buffer is empty.
+					PERROR
+					    ("Output stream has been broken. ISM stoped but some data in FIFO. Buffer is empty.\n");
+				}
+				instance->status = ao_status_stream_fifo_error;
+
+			}
+
+			// Signal the failure.
+			signaling = 1;
+			break;
+		}
+		// Wait for stop.
+		reschedule = 1;
+		break;
+
+	case ao_status_stream_end_wait:
+		if (!(instance->fifo & ME6000_AO_HAS_FIFO)) {	// No FIFO
+			PERROR_CRITICAL
+			    ("Streaming on single device! This feature is not implemented in this version!\n");
+			instance->status = ao_status_stream_error;
+			// Signal the end.
+			signaling = 1;
+			break;
+		}
+
+		if (!(status & ME6000_AO_STATUS_BIT_FSM)) {	// State machine is not working. Waiting for stop finish.
+			instance->status = ao_status_stream_end;
+			signaling = 1;
+		}
+		// State machine is working.
+		reschedule = 1;
+		break;
+
+	default:
+		PERROR_CRITICAL("Status is in wrong state (%d)!\n",
+				instance->status);
+		instance->status = ao_status_stream_error;
+		// Signal the end.
+		signaling = 1;
+		break;
+
+	}
+
+	if (signaling) {	//Signal it.
+		wake_up_interruptible_all(&instance->wait_queue);
+	}
+
+	if (instance->ao_control_task_flag && reschedule) {	// Reschedule task
+		queue_delayed_work(instance->me6000_workqueue,
+				   &instance->ao_control_task, 1);
+	} else {
+		PINFO("<%s> Ending control task.\n", __FUNCTION__);
+	}
+
+}
+
+static int me6000_ao_query_range_by_min_max(me_subdevice_t * subdevice,
+					    int unit,
+					    int *min,
+					    int *max, int *maxdata, int *range)
+{
+	me6000_ao_subdevice_t *instance;
+
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if ((*max - *min) < 0) {
+		PERROR("Invalid minimum and maximum values specified.\n");
+		return ME_ERRNO_INVALID_MIN_MAX;
+	}
+
+	if ((unit == ME_UNIT_VOLT) || (unit == ME_UNIT_ANY)) {
+		if ((*max <= (instance->max + 1000)) && (*min >= instance->min)) {
+			*min = instance->min;
+			*max = instance->max;
+			*maxdata = ME6000_AO_MAX_DATA;
+			*range = 0;
+		} else {
+			PERROR("No matching range available.\n");
+			return ME_ERRNO_NO_RANGE;
+		}
+	} else {
+		PERROR("Invalid physical unit specified.\n");
+		return ME_ERRNO_INVALID_UNIT;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me6000_ao_query_number_ranges(me_subdevice_t * subdevice,
+					 int unit, int *count)
+{
+	me6000_ao_subdevice_t *instance;
+
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if ((unit == ME_UNIT_VOLT) || (unit == ME_UNIT_ANY)) {
+		*count = 1;
+	} else {
+		*count = 0;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me6000_ao_query_range_info(me_subdevice_t * subdevice,
+				      int range,
+				      int *unit,
+				      int *min, int *max, int *maxdata)
+{
+	me6000_ao_subdevice_t *instance;
+
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (range == 0) {
+		*unit = ME_UNIT_VOLT;
+		*min = instance->min;
+		*max = instance->max;
+		*maxdata = ME6000_AO_MAX_DATA;
+	} else {
+		PERROR("Invalid range number specified.\n");
+		return ME_ERRNO_INVALID_RANGE;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me6000_ao_query_timer(me_subdevice_t * subdevice,
+				 int timer,
+				 int *base_frequency,
+				 long long *min_ticks, long long *max_ticks)
+{
+	me6000_ao_subdevice_t *instance;
+
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (instance->fifo) {	//Streaming device.
+		*base_frequency = ME6000_AO_BASE_FREQUENCY;
+		if (timer == ME_TIMER_ACQ_START) {
+			*min_ticks = ME6000_AO_MIN_ACQ_TICKS;
+			*max_ticks = ME6000_AO_MAX_ACQ_TICKS;
+		} else if (timer == ME_TIMER_CONV_START) {
+			*min_ticks = ME6000_AO_MIN_CHAN_TICKS;
+			*max_ticks = ME6000_AO_MAX_CHAN_TICKS;
+		}
+	} else {		//Not streaming device!
+		*base_frequency = 0;
+		*min_ticks = 0;
+		*max_ticks = 0;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me6000_ao_query_number_channels(me_subdevice_t * subdevice,
+					   int *number)
+{
+	me6000_ao_subdevice_t *instance;
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	*number = 1;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me6000_ao_query_subdevice_type(me_subdevice_t * subdevice,
+					  int *type, int *subtype)
+{
+	me6000_ao_subdevice_t *instance;
+
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	*type = ME_TYPE_AO;
+	*subtype =
+	    (instance->
+	     fifo & ME6000_AO_HAS_FIFO) ? ME_SUBTYPE_STREAMING :
+	    ME_SUBTYPE_SINGLE;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me6000_ao_query_subdevice_caps(me_subdevice_t * subdevice, int *caps)
+{
+	me6000_ao_subdevice_t *instance;
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	*caps =
+	    ME_CAPS_AO_TRIG_SYNCHRONOUS | ((instance->fifo) ? ME_CAPS_AO_FIFO :
+					   ME_CAPS_NONE);
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me6000_ao_query_subdevice_caps_args(struct me_subdevice *subdevice,
+					       int cap, int *args, int count)
+{
+	me6000_ao_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	instance = (me6000_ao_subdevice_t *) subdevice;
+
+	PDEBUG("executed. idx=%d\n", instance->ao_idx);
+
+	if (count != 1) {
+		PERROR("Invalid capability argument count.\n");
+		return ME_ERRNO_INVALID_CAP_ARG_COUNT;
+	}
+
+	switch (cap) {
+	case ME_CAP_AI_FIFO_SIZE:
+		args[0] = (instance->fifo) ? ME6000_AO_FIFO_COUNT : 0;
+		break;
+
+	case ME_CAP_AI_BUFFER_SIZE:
+		args[0] =
+		    (instance->circ_buf.buf) ? ME6000_AO_CIRC_BUF_COUNT : 0;
+		break;
+
+	default:
+		PERROR("Invalid capability.\n");
+		err = ME_ERRNO_INVALID_CAP;
+		args[0] = 0;
+	}
+
+	return err;
+}
diff --git a/drivers/staging/meilhaus/me6000_ao.h b/drivers/staging/meilhaus/me6000_ao.h
new file mode 100644
index 00000000000..9629649cd41
--- /dev/null
+++ b/drivers/staging/meilhaus/me6000_ao.h
@@ -0,0 +1,200 @@
+/**
+ * @file me6000_ao.h
+ *
+ * @brief Meilhaus ME-6000 analog output subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME6000_AO_H_
+#define _ME6000_AO_H_
+
+#include <linux/version.h>
+#include "mesubdevice.h"
+#include "mecirc_buf.h"
+#include "meioctl.h"
+
+#ifdef __KERNEL__
+
+#define ME6000_AO_MAX_SUBDEVICES	16
+#define ME6000_AO_FIFO_COUNT		8192
+
+#define ME6000_AO_BASE_FREQUENCY	33000000L
+
+#define ME6000_AO_MIN_ACQ_TICKS		0LL
+#define ME6000_AO_MAX_ACQ_TICKS		0LL
+
+#define ME6000_AO_MIN_CHAN_TICKS	66LL
+#define ME6000_AO_MAX_CHAN_TICKS	0xFFFFFFFFLL
+
+#define ME6000_AO_MIN_RANGE			-10000000
+#define ME6000_AO_MAX_RANGE			9999694
+
+#define ME6000_AO_MIN_RANGE_HIGH	0
+#define ME6000_AO_MAX_RANGE_HIGH	49999237
+
+#define ME6000_AO_MAX_DATA			0xFFFF
+
+#ifdef ME_SYNAPSE
+# define ME6000_AO_CIRC_BUF_SIZE_ORDER 		8	// 2^n PAGES =>> Maximum value of 1MB for Synapse
+#else
+# define ME6000_AO_CIRC_BUF_SIZE_ORDER 		5	// 2^n PAGES =>> 128KB
+#endif
+#define ME6000_AO_CIRC_BUF_SIZE 		PAGE_SIZE<<ME6000_AO_CIRC_BUF_SIZE_ORDER	// Buffer size in bytes.
+
+#  ifdef _CBUFF_32b_t
+#   define ME6000_AO_CIRC_BUF_COUNT	((ME6000_AO_CIRC_BUF_SIZE) / sizeof(uint32_t))	// Size in values
+#  else
+#   define ME6000_AO_CIRC_BUF_COUNT	((ME6000_AO_CIRC_BUF_SIZE) / sizeof(uint16_t))	// Size in values
+#  endif
+
+#  define ME6000_AO_CONTINOUS					0x0
+#  define ME6000_AO_WRAP_MODE					0x1
+#  define ME6000_AO_HW_MODE						0x2
+
+#  define ME6000_AO_HW_WRAP_MODE				(ME6000_AO_WRAP_MODE | ME6000_AO_HW_MODE)
+#  define ME6000_AO_SW_WRAP_MODE				ME6000_AO_WRAP_MODE
+
+#  define ME6000_AO_INF_STOP_MODE				0x0
+#  define ME6000_AO_ACQ_STOP_MODE				0x1
+#  define ME6000_AO_SCAN_STOP_MODE				0x2
+
+#  define ME6000_AO_EXTRA_HARDWARE				0x1
+#  define ME6000_AO_HAS_FIFO					0x2
+
+typedef enum ME6000_AO_STATUS {
+	ao_status_none = 0,
+	ao_status_single_configured,
+	ao_status_single_run_wait,
+	ao_status_single_run,
+	ao_status_single_end_wait,
+	ao_status_single_end,
+	ao_status_stream_configured,
+	ao_status_stream_run_wait,
+	ao_status_stream_run,
+	ao_status_stream_end_wait,
+	ao_status_stream_end,
+	ao_status_stream_fifo_error,
+	ao_status_stream_buffer_error,
+	ao_status_stream_error,
+	ao_status_last
+} ME6000_AO_STATUS;
+
+typedef struct me6000_ao_timeout {
+	unsigned long start_time;
+	unsigned long delay;
+} me6000_ao_timeout_t;
+
+/**
+ * @brief The ME-6000 analog output subdevice class.
+ */
+typedef struct me6000_ao_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;					/**< The subdevice base class. */
+	unsigned int ao_idx;
+
+	/* Attributes */
+	spinlock_t subdevice_lock;				/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *preload_reg_lock;			/**< Spin lock to protect preload_reg from concurrent access. */
+
+	uint32_t *preload_flags;
+	uint32_t *triggering_flags;
+
+	/* Hardware feautres */
+	unsigned int irq;						/**< The interrupt request number assigned by the PCI BIOS. */
+	int fifo;								/**< If set this device has a FIFO. */
+
+	//Range
+	int min;
+	int max;
+
+	int single_value;						/**< Mirror of the output value in single mode. */
+	int single_value_in_fifo;				/**< Mirror of the value written in single mode. */
+	uint32_t ctrl_trg;						/**< Mirror of the trigger settings. */
+
+	volatile int mode;						/**< Flags used for storing SW wraparound setup*/
+	int stop_mode;							/**< The user defined stop condition flag. */
+	unsigned int start_mode;
+	unsigned int stop_count;				/**< The user defined dates presentation end count. */
+	unsigned int stop_data_count;			/**< The stop presentation count. */
+	unsigned int data_count;				/**< The real presentation count. */
+	unsigned int preloaded_count;			/**< The next data addres in buffer. <= for wraparound mode. */
+	int hardware_stop_delay;				/**< The time that stop can take. This is only to not show hardware bug to user. */
+
+	volatile enum ME6000_AO_STATUS status;	/**< The current stream status flag. */
+	me6000_ao_timeout_t timeout;			/**< The timeout for start in blocking and non-blocking mode. */
+
+									/* Registers *//**< All registers are 32 bits long. */
+	unsigned long ctrl_reg;
+	unsigned long status_reg;
+	unsigned long fifo_reg;
+	unsigned long single_reg;
+	unsigned long timer_reg;
+	unsigned long irq_status_reg;
+	unsigned long preload_reg;
+	unsigned long irq_reset_reg;
+#ifdef MEDEBUG_DEBUG_REG
+	unsigned long reg_base;
+#endif
+
+	/* Software buffer */
+	me_circ_buf_t circ_buf;					/**< Circular buffer holding measurment data. */
+	wait_queue_head_t wait_queue;			/**< Wait queue to put on tasks waiting for data to arrive. */
+
+	struct workqueue_struct *me6000_workqueue;
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+	struct work_struct ao_control_task;
+#else
+	struct delayed_work ao_control_task;
+#endif
+
+	volatile int ao_control_task_flag;		/**< Flag controling reexecuting of control task */
+
+} me6000_ao_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-6000 analog output subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ * @param ctrl_reg_lock Pointer to spin lock protecting the control register from concurrent access.
+ * @param preload_flags Pointer to spin lock protecting the hold&trigger register from concurrent access.
+ * @param ao_idx Subdevice number.
+ * @param fifo Flag set if subdevice has hardware FIFO.
+ * @param irq IRQ number.
+ * @param high_range Flag set if subdevice has high curren output.
+ * @param me6000_wq Queue for asynchronous task (1 queue for all subdevice on 1 board).
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me6000_ao_subdevice_t *me6000_ao_constructor(uint32_t reg_base,
+					     spinlock_t * preload_reg_lock,
+					     uint32_t * preload_flags,
+					     uint32_t * triggering_flags,
+					     int ao_idx,
+					     int fifo,
+					     int irq,
+					     int high_range,
+					     struct workqueue_struct
+					     *me6000_wq);
+
+#endif //__KERNEL__
+#endif //_ME6000_AO_H_
diff --git a/drivers/staging/meilhaus/me6000_ao_reg.h b/drivers/staging/meilhaus/me6000_ao_reg.h
new file mode 100644
index 00000000000..eb8f46e1b75
--- /dev/null
+++ b/drivers/staging/meilhaus/me6000_ao_reg.h
@@ -0,0 +1,177 @@
+/**
+ * @file me6000_ao_reg.h
+ *
+ * @brief ME-6000 analog output subdevice register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME6000_AO_REG_H_
+#define _ME6000_AO_REG_H_
+
+#ifdef __KERNEL__
+
+// AO
+#define ME6000_AO_00_CTRL_REG					0x00	// R/W
+#define ME6000_AO_00_STATUS_REG					0x04	// R/_
+#define ME6000_AO_00_FIFO_REG					0x08	// _/W
+#define ME6000_AO_00_SINGLE_REG					0x0C	// R/W
+#define ME6000_AO_00_TIMER_REG					0x10	// _/W
+
+#define ME6000_AO_01_CTRL_REG					0x18	// R/W
+#define ME6000_AO_01_STATUS_REG					0x1C	// R/_
+#define ME6000_AO_01_FIFO_REG					0x20	// _/W
+#define ME6000_AO_01_SINGLE_REG					0x24	// R/W
+#define ME6000_AO_01_TIMER_REG					0x28	// _/W
+
+#define ME6000_AO_02_CTRL_REG					0x30	// R/W
+#define ME6000_AO_02_STATUS_REG					0x34	// R/_
+#define ME6000_AO_02_FIFO_REG					0x38	// _/W
+#define ME6000_AO_02_SINGLE_REG					0x3C	// R/W
+#define ME6000_AO_02_TIMER_REG					0x40	// _/W
+
+#define ME6000_AO_03_CTRL_REG					0x48	// R/W
+#define ME6000_AO_03_STATUS_REG					0x4C	// R/_
+#define ME6000_AO_03_FIFO_REG					0x50	// _/W
+#define ME6000_AO_03_SINGLE_REG					0x54	// R/W
+#define ME6000_AO_03_TIMER_REG					0x58	// _/W
+
+#define ME6000_AO_SINGLE_STATUS_REG				0xA4	// R/_
+#define ME6000_AO_SINGLE_STATUS_OFFSET			4	//The first single subdevice => bit 0 in ME6000_AO_SINGLE_STATUS_REG.
+
+#define ME6000_AO_04_STATUS_REG					ME6000_AO_SINGLE_STATUS_REG
+#define ME6000_AO_04_SINGLE_REG					0x74	// _/W
+
+#define ME6000_AO_05_STATUS_REG					ME6000_AO_SINGLE_STATUS_REG
+#define ME6000_AO_05_SINGLE_REG					0x78	// _/W
+
+#define ME6000_AO_06_STATUS_REG					ME6000_AO_SINGLE_STATUS_REG
+#define ME6000_AO_06_SINGLE_REG					0x7C	// _/W
+
+#define ME6000_AO_07_STATUS_REG					ME6000_AO_SINGLE_STATUS_REG
+#define ME6000_AO_07_SINGLE_REG					0x80	// _/W
+
+#define ME6000_AO_08_STATUS_REG					ME6000_AO_SINGLE_STATUS_REG
+#define ME6000_AO_08_SINGLE_REG					0x84	// _/W
+
+#define ME6000_AO_09_STATUS_REG					ME6000_AO_SINGLE_STATUS_REG
+#define ME6000_AO_09_SINGLE_REG					0x88	// _/W
+
+#define ME6000_AO_10_STATUS_REG					ME6000_AO_SINGLE_STATUS_REG
+#define ME6000_AO_10_SINGLE_REG					0x8C	// _/W
+
+#define ME6000_AO_11_STATUS_REG					ME6000_AO_SINGLE_STATUS_REG
+#define ME6000_AO_11_SINGLE_REG					0x90	// _/W
+
+#define ME6000_AO_12_STATUS_REG					ME6000_AO_SINGLE_STATUS_REG
+#define ME6000_AO_12_SINGLE_REG					0x94	// _/W
+
+#define ME6000_AO_13_STATUS_REG					ME6000_AO_SINGLE_STATUS_REG
+#define ME6000_AO_13_SINGLE_REG					0x98	// _/W
+
+#define ME6000_AO_14_STATUS_REG					ME6000_AO_SINGLE_STATUS_REG
+#define ME6000_AO_14_SINGLE_REG					0x9C	// _/W
+
+#define ME6000_AO_15_STATUS_REG					ME6000_AO_SINGLE_STATUS_REG
+#define ME6000_AO_15_SINGLE_REG					0xA0	// _/W
+
+//ME6000_AO_CTRL_REG
+#define ME6000_AO_MODE_SINGLE					0x00
+#define ME6000_AO_MODE_WRAPAROUND				0x01
+#define ME6000_AO_MODE_CONTINUOUS				0x02
+#define ME6000_AO_CTRL_MODE_MASK				(ME6000_AO_MODE_WRAPAROUND | ME6000_AO_MODE_CONTINUOUS)
+
+#define ME6000_AO_CTRL_BIT_MODE_WRAPAROUND		0x001
+#define ME6000_AO_CTRL_BIT_MODE_CONTINUOUS		0x002
+#define ME6000_AO_CTRL_BIT_STOP					0x004
+#define ME6000_AO_CTRL_BIT_ENABLE_FIFO			0x008
+#define ME6000_AO_CTRL_BIT_ENABLE_EX_TRIG		0x010
+#define ME6000_AO_CTRL_BIT_EX_TRIG_EDGE			0x020
+#define ME6000_AO_CTRL_BIT_ENABLE_IRQ			0x040
+#define ME6000_AO_CTRL_BIT_IMMEDIATE_STOP		0x080
+#define ME6000_AO_CTRL_BIT_EX_TRIG_EDGE_BOTH 	0x800
+
+//ME6000_AO_STATUS_REG
+#define ME6000_AO_STATUS_BIT_FSM				0x01
+#define ME6000_AO_STATUS_BIT_FF					0x02
+#define ME6000_AO_STATUS_BIT_HF					0x04
+#define ME6000_AO_STATUS_BIT_EF					0x08
+
+#define ME6000_AO_PRELOAD_REG					0xA8	// R/W    ///ME6000_AO_SYNC_REG <==> ME6000_AO_PRELOAD_REG
+/*
+#define ME6000_AO_SYNC_HOLD_0					0x00000001
+#define ME6000_AO_SYNC_HOLD_1					0x00000002
+#define ME6000_AO_SYNC_HOLD_2					0x00000004
+#define ME6000_AO_SYNC_HOLD_3					0x00000008
+#define ME6000_AO_SYNC_HOLD_4					0x00000010
+#define ME6000_AO_SYNC_HOLD_5					0x00000020
+#define ME6000_AO_SYNC_HOLD_6					0x00000040
+#define ME6000_AO_SYNC_HOLD_7					0x00000080
+#define ME6000_AO_SYNC_HOLD_8					0x00000100
+#define ME6000_AO_SYNC_HOLD_9					0x00000200
+#define ME6000_AO_SYNC_HOLD_10					0x00000400
+#define ME6000_AO_SYNC_HOLD_11					0x00000800
+#define ME6000_AO_SYNC_HOLD_12					0x00001000
+#define ME6000_AO_SYNC_HOLD_13					0x00002000
+#define ME6000_AO_SYNC_HOLD_14					0x00004000
+#define ME6000_AO_SYNC_HOLD_15					0x00008000
+*/
+#define ME6000_AO_SYNC_HOLD						0x00000001
+/*
+#define ME6000_AO_SYNC_EXT_TRIG_0				0x00010000
+#define ME6000_AO_SYNC_EXT_TRIG_1				0x00020000
+#define ME6000_AO_SYNC_EXT_TRIG_2				0x00040000
+#define ME6000_AO_SYNC_EXT_TRIG_3				0x00080000
+#define ME6000_AO_SYNC_EXT_TRIG_4				0x00100000
+#define ME6000_AO_SYNC_EXT_TRIG_5				0x00200000
+#define ME6000_AO_SYNC_EXT_TRIG_6				0x00400000
+#define ME6000_AO_SYNC_EXT_TRIG_7				0x00800000
+#define ME6000_AO_SYNC_EXT_TRIG_8				0x01000000
+#define ME6000_AO_SYNC_EXT_TRIG_9				0x02000000
+#define ME6000_AO_SYNC_EXT_TRIG_10				0x04000000
+#define ME6000_AO_SYNC_EXT_TRIG_11				0x08000000
+#define ME6000_AO_SYNC_EXT_TRIG_12				0x10000000
+#define ME6000_AO_SYNC_EXT_TRIG_13				0x20000000
+#define ME6000_AO_SYNC_EXT_TRIG_14				0x40000000
+#define ME6000_AO_SYNC_EXT_TRIG_15				0x80000000
+*/
+#define ME6000_AO_SYNC_EXT_TRIG					0x00010000
+
+#define ME6000_AO_EXT_TRIG						0x80000000
+
+// AO-IRQ
+#define ME6000_AO_IRQ_STATUS_REG				0x60	// R/_
+#define ME6000_AO_00_IRQ_RESET_REG				0x64	// R/_
+#define ME6000_AO_01_IRQ_RESET_REG				0x68	// R/_
+#define ME6000_AO_02_IRQ_RESET_REG				0x6C	// R/_
+#define ME6000_AO_03_IRQ_RESET_REG				0x70	// R/_
+
+#define ME6000_IRQ_STATUS_BIT_0					0x01
+#define ME6000_IRQ_STATUS_BIT_1					0x02
+#define ME6000_IRQ_STATUS_BIT_2					0x04
+#define ME6000_IRQ_STATUS_BIT_3					0x08
+
+#define ME6000_IRQ_STATUS_BIT_AO_HF				ME6000_IRQ_STATUS_BIT_0
+
+//DUMY register
+#define ME6000_AO_DUMY									0xFC
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me6000_device.c b/drivers/staging/meilhaus/me6000_device.c
new file mode 100644
index 00000000000..fee4c58b846
--- /dev/null
+++ b/drivers/staging/meilhaus/me6000_device.c
@@ -0,0 +1,211 @@
+/**
+ * @file me6000_device.c
+ *
+ * @brief Device class template implementation.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+#ifndef MODULE
+#  define MODULE
+#endif
+
+#include <linux/module.h>
+
+#include <linux/pci.h>
+#include <linux/slab.h>
+
+#include "meids.h"
+#include "meerror.h"
+#include "mecommon.h"
+#include "meinternal.h"
+
+#include "mefirmware.h"
+
+#include "mesubdevice.h"
+#include "medebug.h"
+#include "medevice.h"
+#include "me6000_reg.h"
+#include "me6000_device.h"
+#include "meplx_reg.h"
+#include "me6000_dio.h"
+#include "me6000_ao.h"
+
+/**
+ * @brief Global variable.
+ * This is working queue for runing a separate atask that will be responsible for work status (start, stop, timeouts).
+ */
+static struct workqueue_struct *me6000_workqueue;
+
+me_device_t *me6000_pci_constructor(struct pci_dev *pci_device)
+{
+	me6000_device_t *me6000_device;
+	me_subdevice_t *subdevice;
+	unsigned int version_idx;
+	int err;
+	int i;
+	int high_range = 0;
+	int fifo;
+
+	PDEBUG("executed.\n");
+
+	// Allocate structure for device instance.
+	me6000_device = kmalloc(sizeof(me6000_device_t), GFP_KERNEL);
+
+	if (!me6000_device) {
+		PERROR("Cannot get memory for device instance.\n");
+		return NULL;
+	}
+
+	memset(me6000_device, 0, sizeof(me6000_device_t));
+
+	// Initialize base class structure.
+	err = me_device_pci_init((me_device_t *) me6000_device, pci_device);
+
+	if (err) {
+		kfree(me6000_device);
+		PERROR("Cannot initialize device base class.\n");
+		return NULL;
+	}
+
+	/* Download the xilinx firmware */
+	err = me_xilinx_download(me6000_device->base.info.pci.reg_bases[1],
+				 me6000_device->base.info.pci.reg_bases[2],
+				 &pci_device->dev, "me6000.bin");
+
+	if (err) {
+		me_device_deinit((me_device_t *) me6000_device);
+		kfree(me6000_device);
+		PERROR("Can't download firmware.\n");
+		return NULL;
+	}
+
+	/* Get the index in the device version information table. */
+	version_idx =
+	    me6000_versions_get_device_index(me6000_device->base.info.pci.
+					     device_id);
+
+	// Initialize spin lock .
+	spin_lock_init(&me6000_device->preload_reg_lock);
+	spin_lock_init(&me6000_device->dio_ctrl_reg_lock);
+
+	/* Create digital input/output instances. */
+	for (i = 0; i < me6000_versions[version_idx].dio_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) me6000_dio_constructor(me6000_device->
+							      base.info.pci.
+							      reg_bases[3], i,
+							      &me6000_device->
+							      dio_ctrl_reg_lock);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me6000_device);
+			kfree(me6000_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me6000_device->base.slist,
+					    subdevice);
+	}
+
+	/* Create analog output instances. */
+	for (i = 0; i < me6000_versions[version_idx].ao_subdevices; i++) {
+		high_range = ((i == 8)
+			      &&
+			      ((me6000_device->base.info.pci.device_id ==
+				PCI_DEVICE_ID_MEILHAUS_ME6359)
+			       || (me6000_device->base.info.pci.device_id ==
+				   PCI_DEVICE_ID_MEILHAUS_ME6259)
+			      )
+		    )? 1 : 0;
+
+		fifo =
+		    (i <
+		     me6000_versions[version_idx].
+		     ao_fifo) ? ME6000_AO_HAS_FIFO : 0x0;
+		fifo |= (i < 4) ? ME6000_AO_EXTRA_HARDWARE : 0x0;
+
+		subdevice =
+		    (me_subdevice_t *) me6000_ao_constructor(me6000_device->
+							     base.info.pci.
+							     reg_bases[2],
+							     &me6000_device->
+							     preload_reg_lock,
+							     &me6000_device->
+							     preload_flags,
+							     &me6000_device->
+							     triggering_flags,
+							     i, fifo,
+							     me6000_device->
+							     base.irq,
+							     high_range,
+							     me6000_workqueue);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me6000_device);
+			kfree(me6000_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me6000_device->base.slist,
+					    subdevice);
+	}
+
+	return (me_device_t *) me6000_device;
+}
+
+// Init and exit of module.
+
+static int __init me6000_init(void)
+{
+	PDEBUG("executed.\n");
+
+	me6000_workqueue = create_singlethread_workqueue("me6000");
+	return 0;
+}
+
+static void __exit me6000_exit(void)
+{
+	PDEBUG("executed.\n");
+
+	flush_workqueue(me6000_workqueue);
+	destroy_workqueue(me6000_workqueue);
+}
+
+module_init(me6000_init);
+module_exit(me6000_exit);
+
+// Administrative stuff for modinfo.
+MODULE_AUTHOR
+    ("Guenter Gebhardt <g.gebhardt@meilhaus.de> & Krzysztof Gantzke <k.gantzke@meilhaus.de>");
+MODULE_DESCRIPTION("Device Driver Module for ME-6000 Device");
+MODULE_SUPPORTED_DEVICE("Meilhaus ME-6000 Devices");
+MODULE_LICENSE("GPL");
+
+// Export the constructor.
+EXPORT_SYMBOL(me6000_pci_constructor);
diff --git a/drivers/staging/meilhaus/me6000_device.h b/drivers/staging/meilhaus/me6000_device.h
new file mode 100644
index 00000000000..18cc7d1e14f
--- /dev/null
+++ b/drivers/staging/meilhaus/me6000_device.h
@@ -0,0 +1,149 @@
+/**
+ * @file me6000_device.h
+ *
+ * @brief ME-6000 device class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME6000_DEVICE_H
+#define _ME6000_DEVICE_H
+
+#include <linux/pci.h>
+#include <linux/spinlock.h>
+
+#include "medevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief Structure holding ME-6000 device capabilities.
+ */
+typedef struct me6000_version {
+	uint16_t device_id;
+	unsigned int dio_subdevices;
+	unsigned int ao_subdevices;
+	unsigned int ao_fifo;	//How many devices have FIFO
+} me6000_version_t;
+
+/**
+ * @brief ME-6000 device capabilities.
+ */
+static me6000_version_t me6000_versions[] = {
+	{PCI_DEVICE_ID_MEILHAUS_ME6004, 0, 4, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME6008, 0, 8, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME600F, 0, 16, 0},
+
+	{PCI_DEVICE_ID_MEILHAUS_ME6014, 0, 4, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME6018, 0, 8, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME601F, 0, 16, 0},
+
+	{PCI_DEVICE_ID_MEILHAUS_ME6034, 0, 4, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME6038, 0, 8, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME603F, 0, 16, 0},
+
+	{PCI_DEVICE_ID_MEILHAUS_ME6104, 0, 4, 4},
+	{PCI_DEVICE_ID_MEILHAUS_ME6108, 0, 8, 4},
+	{PCI_DEVICE_ID_MEILHAUS_ME610F, 0, 16, 4},
+
+	{PCI_DEVICE_ID_MEILHAUS_ME6114, 0, 4, 4},
+	{PCI_DEVICE_ID_MEILHAUS_ME6118, 0, 8, 4},
+	{PCI_DEVICE_ID_MEILHAUS_ME611F, 0, 16, 4},
+
+	{PCI_DEVICE_ID_MEILHAUS_ME6134, 0, 4, 4},
+	{PCI_DEVICE_ID_MEILHAUS_ME6138, 0, 8, 4},
+	{PCI_DEVICE_ID_MEILHAUS_ME613F, 0, 16, 4},
+
+	{PCI_DEVICE_ID_MEILHAUS_ME6044, 2, 4, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME6048, 2, 8, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME604F, 2, 16, 0},
+
+	{PCI_DEVICE_ID_MEILHAUS_ME6054, 2, 4, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME6058, 2, 8, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME605F, 2, 16, 0},
+
+	{PCI_DEVICE_ID_MEILHAUS_ME6074, 2, 4, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME6078, 2, 8, 0},
+	{PCI_DEVICE_ID_MEILHAUS_ME607F, 2, 16, 0},
+
+	{PCI_DEVICE_ID_MEILHAUS_ME6144, 2, 4, 4},
+	{PCI_DEVICE_ID_MEILHAUS_ME6148, 2, 8, 4},
+	{PCI_DEVICE_ID_MEILHAUS_ME614F, 2, 16, 4},
+
+	{PCI_DEVICE_ID_MEILHAUS_ME6154, 2, 4, 4},
+	{PCI_DEVICE_ID_MEILHAUS_ME6158, 2, 8, 4},
+	{PCI_DEVICE_ID_MEILHAUS_ME615F, 2, 16, 4},
+
+	{PCI_DEVICE_ID_MEILHAUS_ME6174, 2, 4, 4},
+	{PCI_DEVICE_ID_MEILHAUS_ME6178, 2, 8, 4},
+	{PCI_DEVICE_ID_MEILHAUS_ME617F, 2, 16, 4},
+
+	{PCI_DEVICE_ID_MEILHAUS_ME6259, 2, 9, 0},
+
+	{PCI_DEVICE_ID_MEILHAUS_ME6359, 2, 9, 4},
+
+	{0},
+};
+
+#define ME6000_DEVICE_VERSIONS (sizeof(me6000_versions) / sizeof(me6000_version_t) - 1)	/**< Returns the number of entries in #me6000_versions. */
+
+/**
+ * @brief Returns the index of the device entry in #me6000_versions.
+ *
+ * @param device_id The PCI device id of the device to query.
+ * @return The index of the device in #me6000_versions.
+ */
+static inline unsigned int me6000_versions_get_device_index(uint16_t device_id)
+{
+	unsigned int i;
+	for (i = 0; i < ME6000_DEVICE_VERSIONS; i++)
+		if (me6000_versions[i].device_id == device_id)
+			break;
+	return i;
+}
+
+/**
+ * @brief The ME-6000 device class structure.
+ */
+typedef struct me6000_device {
+	me_device_t base;			/**< The Meilhaus device base class. */
+
+	/* Child class attributes. */
+	spinlock_t preload_reg_lock;		/**< Guards the preload register. */
+	uint32_t preload_flags;
+	uint32_t triggering_flags;
+
+	spinlock_t dio_ctrl_reg_lock;
+} me6000_device_t;
+
+/**
+ * @brief The ME-6000 device class constructor.
+ *
+ * @param pci_device The pci device structure given by the PCI subsystem.
+ *
+ * @return On succes a new ME-6000 device instance. \n
+ *         NULL on error.
+ */
+me_device_t *me6000_pci_constructor(struct pci_dev *pci_device)
+    __attribute__ ((weak));
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me6000_dio.c b/drivers/staging/meilhaus/me6000_dio.c
new file mode 100644
index 00000000000..07f1069f9ac
--- /dev/null
+++ b/drivers/staging/meilhaus/me6000_dio.c
@@ -0,0 +1,415 @@
+/**
+ * @file me6000_dio.c
+ *
+ * @brief ME-6000 digital input/output subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "me6000_dio_reg.h"
+#include "me6000_dio.h"
+
+/*
+ * Defines
+ */
+
+/*
+ * Functions
+ */
+
+static int me6000_dio_io_reset_subdevice(struct me_subdevice *subdevice,
+					 struct file *filep, int flags)
+{
+	me6000_dio_subdevice_t *instance;
+	uint8_t mode;
+
+	PDEBUG("executed.\n");
+
+	instance = (me6000_dio_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	mode = inb(instance->ctrl_reg);
+	mode &= ~(0x3 << (instance->dio_idx * 2));
+	outb(mode, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, mode);
+	spin_unlock(instance->ctrl_reg_lock);
+
+	outb(0x00, instance->port_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, 0x00);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me6000_dio_io_single_config(me_subdevice_t * subdevice,
+				       struct file *filep,
+				       int channel,
+				       int single_config,
+				       int ref,
+				       int trig_chan,
+				       int trig_type, int trig_edge, int flags)
+{
+	me6000_dio_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint8_t mode;
+	int size =
+	    flags & (ME_IO_SINGLE_CONFIG_DIO_BIT | ME_IO_SINGLE_CONFIG_DIO_BYTE
+		     | ME_IO_SINGLE_CONFIG_DIO_WORD |
+		     ME_IO_SINGLE_CONFIG_DIO_DWORD);
+
+	PDEBUG("executed.\n");
+
+	instance = (me6000_dio_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	mode = inb(instance->ctrl_reg);
+	switch (size) {
+	case ME_IO_SINGLE_CONFIG_NO_FLAGS:
+	case ME_IO_SINGLE_CONFIG_DIO_BYTE:
+		if (channel == 0) {
+			if (single_config == ME_SINGLE_CONFIG_DIO_INPUT) {
+				mode &=
+				    ~((ME6000_DIO_CTRL_BIT_MODE_0 |
+				       ME6000_DIO_CTRL_BIT_MODE_1) <<
+				      (instance->dio_idx * 2));
+			} else if (single_config == ME_SINGLE_CONFIG_DIO_OUTPUT) {
+				mode &=
+				    ~((ME6000_DIO_CTRL_BIT_MODE_0 |
+				       ME6000_DIO_CTRL_BIT_MODE_1) <<
+				      (instance->dio_idx * 2));
+				mode |=
+				    ME6000_DIO_CTRL_BIT_MODE_0 << (instance->
+								   dio_idx * 2);
+			} else {
+				PERROR
+				    ("Invalid port configuration specified.\n");
+				err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		} else {
+			PERROR("Invalid channel number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (!err) {
+		outb(mode, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, mode);
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me6000_dio_io_single_read(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int channel,
+				     int *value, int time_out, int flags)
+{
+	me6000_dio_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint8_t mode;
+
+	PDEBUG("executed.\n");
+
+	instance = (me6000_dio_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			mode =
+			    inb(instance->
+				ctrl_reg) & ((ME6000_DIO_CTRL_BIT_MODE_0 |
+					      ME6000_DIO_CTRL_BIT_MODE_1) <<
+					     (instance->dio_idx * 2));
+			if ((mode ==
+			     (ME6000_DIO_CTRL_BIT_MODE_0 <<
+			      (instance->dio_idx * 2))) || !mode) {
+				*value =
+				    inb(instance->port_reg) & (0x1 << channel);
+			} else {
+				PERROR("Port not in output or input mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			mode =
+			    inb(instance->
+				ctrl_reg) & ((ME6000_DIO_CTRL_BIT_MODE_0 |
+					      ME6000_DIO_CTRL_BIT_MODE_1) <<
+					     (instance->dio_idx * 2));
+			if ((mode ==
+			     (ME6000_DIO_CTRL_BIT_MODE_0 <<
+			      (instance->dio_idx * 2))) || !mode) {
+				*value = inb(instance->port_reg) & 0x00FF;
+			} else {
+				PERROR("Port not in output or input mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me6000_dio_io_single_write(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int value, int time_out, int flags)
+{
+	me6000_dio_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint8_t mode;
+	uint8_t byte;
+
+	PDEBUG("executed.\n");
+
+	instance = (me6000_dio_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			mode =
+			    inb(instance->
+				ctrl_reg) & ((ME6000_DIO_CTRL_BIT_MODE_0 |
+					      ME6000_DIO_CTRL_BIT_MODE_1) <<
+					     (instance->dio_idx * 2));
+
+			if (mode ==
+			    (ME6000_DIO_CTRL_BIT_MODE_0 <<
+			     (instance->dio_idx * 2))) {
+				byte = inb(instance->port_reg) & 0x00FF;
+
+				if (value)
+					byte |= 0x1 << channel;
+				else
+					byte &= ~(0x1 << channel);
+
+				outb(byte, instance->port_reg);
+			} else {
+				PERROR("Port not in output or input mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			mode =
+			    inb(instance->
+				ctrl_reg) & ((ME6000_DIO_CTRL_BIT_MODE_0 |
+					      ME6000_DIO_CTRL_BIT_MODE_1) <<
+					     (instance->dio_idx * 2));
+
+			if (mode ==
+			    (ME6000_DIO_CTRL_BIT_MODE_0 <<
+			     (instance->dio_idx * 2))) {
+				outb(value, instance->port_reg);
+			} else {
+				PERROR("Port not in output or input mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me6000_dio_query_number_channels(me_subdevice_t * subdevice,
+					    int *number)
+{
+	PDEBUG("executed.\n");
+	*number = 8;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me6000_dio_query_subdevice_type(me_subdevice_t * subdevice,
+					   int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_DIO;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me6000_dio_query_subdevice_caps(me_subdevice_t * subdevice,
+					   int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = ME_CAPS_DIO_DIR_BYTE;
+	return ME_ERRNO_SUCCESS;
+}
+
+me6000_dio_subdevice_t *me6000_dio_constructor(uint32_t reg_base,
+					       unsigned int dio_idx,
+					       spinlock_t * ctrl_reg_lock)
+{
+	me6000_dio_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me6000_dio_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me6000_dio_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+
+	/* Set the subdevice ports */
+	subdevice->ctrl_reg = reg_base + ME6000_DIO_CTRL_REG;
+	switch (dio_idx) {
+	case 0:
+		subdevice->port_reg = reg_base + ME6000_DIO_PORT_0_REG;
+		break;
+	case 1:
+		subdevice->port_reg = reg_base + ME6000_DIO_PORT_1_REG;
+		break;
+	default:
+		err = ME_ERRNO_INVALID_SUBDEVICE;
+	}
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	subdevice->ctrl_reg_lock = ctrl_reg_lock;
+
+	/* Save digital i/o index */
+	subdevice->dio_idx = dio_idx;
+
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = reg_base;
+#endif
+
+	/* Overload base class methods. */
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me6000_dio_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me6000_dio_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me6000_dio_io_single_read;
+	subdevice->base.me_subdevice_io_single_write =
+	    me6000_dio_io_single_write;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me6000_dio_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me6000_dio_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me6000_dio_query_subdevice_caps;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me6000_dio.h b/drivers/staging/meilhaus/me6000_dio.h
new file mode 100644
index 00000000000..858bec1c459
--- /dev/null
+++ b/drivers/staging/meilhaus/me6000_dio.h
@@ -0,0 +1,68 @@
+/**
+ * @file me6000_dio.h
+ *
+ * @brief ME-6000 digital input/output subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME6000_DIO_H_
+#define _ME6000_DIO_H_
+
+#include "mesubdevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The template subdevice class.
+ */
+typedef struct me6000_dio_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *ctrl_reg_lock;		/**< Spin lock to protect #ctrl_reg from concurrent access. */
+	unsigned int dio_idx;			/**< The index of the digital i/o on the device. */
+
+	unsigned long port_reg;			/**< Register holding the port status. */
+	unsigned long ctrl_reg;			/**< Register to configure the port direction. */
+#ifdef MEDEBUG_DEBUG_REG
+	unsigned long reg_base;
+#endif
+} me6000_dio_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-6000 digital input/ouput subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ * @param dio_idx The index of the digital i/o port on the device.
+ * @param ctrl_reg_lock Spin lock protecting the control register.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me6000_dio_subdevice_t *me6000_dio_constructor(uint32_t reg_base,
+					       unsigned int dio_idx,
+					       spinlock_t * ctrl_reg_lock);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me6000_dio_reg.h b/drivers/staging/meilhaus/me6000_dio_reg.h
new file mode 100644
index 00000000000..e67a791a1e6
--- /dev/null
+++ b/drivers/staging/meilhaus/me6000_dio_reg.h
@@ -0,0 +1,43 @@
+/**
+ * @file me6000_dio_reg.h
+ *
+ * @brief ME-6000 digital input/output subdevice register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME6000_DIO_REG_H_
+#define _ME6000_DIO_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME6000_DIO_CTRL_REG				0x00	// R/W
+#define ME6000_DIO_PORT_0_REG			0x01	// R/W
+#define ME6000_DIO_PORT_1_REG			0x02	// R/W
+#define ME6000_DIO_PORT_REG				ME6000_DIO_PORT_0_REG	// R/W
+
+#define ME6000_DIO_CTRL_BIT_MODE_0		0x01
+#define ME6000_DIO_CTRL_BIT_MODE_1		0x02
+#define ME6000_DIO_CTRL_BIT_MODE_2		0x04
+#define ME6000_DIO_CTRL_BIT_MODE_3		0x08
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me6000_reg.h b/drivers/staging/meilhaus/me6000_reg.h
new file mode 100644
index 00000000000..d3527300341
--- /dev/null
+++ b/drivers/staging/meilhaus/me6000_reg.h
@@ -0,0 +1,35 @@
+/**
+ * @file me6000_reg.h
+ *
+ * @brief ME-6000 device register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME6000_REG_H_
+#define _ME6000_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME6000_INIT_XILINX_REG		0xAC	// R/-
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me8100_device.c b/drivers/staging/meilhaus/me8100_device.c
new file mode 100644
index 00000000000..1fb79e49026
--- /dev/null
+++ b/drivers/staging/meilhaus/me8100_device.c
@@ -0,0 +1,187 @@
+/**
+ * @file me8100_device.c
+ *
+ * @brief ME-8100 device class implementation.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+#ifndef MODULE
+#  define MODULE
+#endif
+
+#include <linux/module.h>
+
+#include <linux/pci.h>
+#include <linux/slab.h>
+
+#include "meids.h"
+#include "meerror.h"
+#include "mecommon.h"
+#include "meinternal.h"
+
+#include "medebug.h"
+#include "medevice.h"
+#include "me8100_device.h"
+#include "mesubdevice.h"
+#include "me8100_di.h"
+#include "me8100_do.h"
+#include "me8254.h"
+
+me_device_t *me8100_pci_constructor(struct pci_dev *pci_device)
+{
+	me8100_device_t *me8100_device;
+	me_subdevice_t *subdevice;
+	unsigned int version_idx;
+	int err;
+	int i;
+
+	PDEBUG("executed.\n");
+
+	// Allocate structure for device instance.
+	me8100_device = kmalloc(sizeof(me8100_device_t), GFP_KERNEL);
+
+	if (!me8100_device) {
+		PERROR("Cannot get memory for device instance.\n");
+		return NULL;
+	}
+
+	memset(me8100_device, 0, sizeof(me8100_device_t));
+
+	// Initialize base class structure.
+	err = me_device_pci_init((me_device_t *) me8100_device, pci_device);
+
+	if (err) {
+		kfree(me8100_device);
+		PERROR("Cannot initialize device base class.\n");
+		return NULL;
+	}
+
+	/* Get the index in the device version information table. */
+	version_idx =
+	    me8100_versions_get_device_index(me8100_device->base.info.pci.
+					     device_id);
+
+	// Initialize spin lock .
+	spin_lock_init(&me8100_device->dio_ctrl_reg_lock);
+	spin_lock_init(&me8100_device->ctr_ctrl_reg_lock);
+	spin_lock_init(&me8100_device->clk_src_reg_lock);
+
+	// Create subdevice instances.
+
+	for (i = 0; i < me8100_versions[version_idx].di_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) me8100_di_constructor(me8100_device->
+							     base.info.pci.
+							     reg_bases[2],
+							     me8100_device->
+							     base.info.pci.
+							     reg_bases[1], i,
+							     me8100_device->
+							     base.irq,
+							     &me8100_device->
+							     dio_ctrl_reg_lock);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me8100_device);
+			kfree(me8100_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me8100_device->base.slist,
+					    subdevice);
+	}
+
+	for (i = 0; i < me8100_versions[version_idx].do_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) me8100_do_constructor(me8100_device->
+							     base.info.pci.
+							     reg_bases[2], i,
+							     &me8100_device->
+							     dio_ctrl_reg_lock);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me8100_device);
+			kfree(me8100_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me8100_device->base.slist,
+					    subdevice);
+	}
+
+	for (i = 0; i < me8100_versions[version_idx].ctr_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) me8254_constructor(me8100_device->base.
+							  info.pci.device_id,
+							  me8100_device->base.
+							  info.pci.reg_bases[2],
+							  0, i,
+							  &me8100_device->
+							  ctr_ctrl_reg_lock,
+							  &me8100_device->
+							  clk_src_reg_lock);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me8100_device);
+			kfree(me8100_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me8100_device->base.slist,
+					    subdevice);
+	}
+
+	return (me_device_t *) me8100_device;
+}
+
+// Init and exit of module.
+
+static int __init me8100_init(void)
+{
+	PDEBUG("executed.\n.");
+	return ME_ERRNO_SUCCESS;
+}
+
+static void __exit me8100_exit(void)
+{
+	PDEBUG("executed.\n.");
+}
+
+module_init(me8100_init);
+
+module_exit(me8100_exit);
+
+// Administrative stuff for modinfo.
+MODULE_AUTHOR("Guenter Gebhardt <g.gebhardt@meilhaus.de>");
+MODULE_DESCRIPTION("Device Driver Module for Template Device");
+MODULE_SUPPORTED_DEVICE("Meilhaus Template Devices");
+MODULE_LICENSE("GPL");
+
+// Export the constructor.
+EXPORT_SYMBOL(me8100_pci_constructor);
diff --git a/drivers/staging/meilhaus/me8100_device.h b/drivers/staging/meilhaus/me8100_device.h
new file mode 100644
index 00000000000..44c42efb04e
--- /dev/null
+++ b/drivers/staging/meilhaus/me8100_device.h
@@ -0,0 +1,97 @@
+/**
+ * @file me8100_device.h
+ *
+ * @brief ME-8100 device class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME8100_DEVICE_H
+#define _ME8100_DEVICE_H
+
+#include <linux/pci.h>
+#include <linux/spinlock.h>
+
+#include "medevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief Structure holding ME-8100 device capabilities.
+ */
+typedef struct me8100_version {
+	uint16_t device_id;
+	unsigned int di_subdevices;
+	unsigned int do_subdevices;
+	unsigned int ctr_subdevices;
+} me8100_version_t;
+
+/**
+ * @brief Device capabilities.
+ */
+static me8100_version_t me8100_versions[] = {
+	{PCI_DEVICE_ID_MEILHAUS_ME8100_A, 1, 1, 3},
+	{PCI_DEVICE_ID_MEILHAUS_ME8100_B, 2, 2, 3},
+	{0},
+};
+
+#define ME8100_DEVICE_VERSIONS (sizeof(me8100_versions) / sizeof(me8100_version_t) - 1)	/**< Returns the number of entries in #me8100_versions. */
+
+/**
+ * @brief Returns the index of the device entry in #me8100_versions.
+ *
+ * @param device_id The PCI device id of the device to query.
+ * @return The index of the device in #me8100_versions.
+ */
+static inline unsigned int me8100_versions_get_device_index(uint16_t device_id)
+{
+	unsigned int i;
+	for (i = 0; i < ME8100_DEVICE_VERSIONS; i++)
+		if (me8100_versions[i].device_id == device_id)
+			break;
+	return i;
+}
+
+/**
+ * @brief The ME-8100 device class structure.
+ */
+typedef struct me8100_device {
+	me_device_t base;			/**< The Meilhaus device base class. */
+
+	/* Child class attributes. */
+	spinlock_t dio_ctrl_reg_lock;
+	spinlock_t ctr_ctrl_reg_lock;
+	spinlock_t clk_src_reg_lock;
+} me8100_device_t;
+
+/**
+ * @brief The ME-8100 device class constructor.
+ *
+ * @param pci_device The pci device structure given by the PCI subsystem.
+ *
+ * @return On succes a new ME-8100 device instance. \n
+ *         NULL on error.
+ */
+me_device_t *me8100_pci_constructor(struct pci_dev *pci_device)
+    __attribute__ ((weak));
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me8100_di.c b/drivers/staging/meilhaus/me8100_di.c
new file mode 100644
index 00000000000..0f146371b9a
--- /dev/null
+++ b/drivers/staging/meilhaus/me8100_di.c
@@ -0,0 +1,693 @@
+/**
+ * @file me8100_di.c
+ *
+ * @brief ME-8100 digital input subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+#include <linux/interrupt.h>
+#include <linux/version.h>
+
+#include "medefines.h"
+#include "meerror.h"
+
+#include "meids.h"
+#include "medebug.h"
+#include "meplx_reg.h"
+#include "me8100_reg.h"
+#include "me8100_di_reg.h"
+#include "me8100_di.h"
+
+/*
+ * Defines
+ */
+
+/*
+ * Functions
+ */
+
+static int me8100_di_io_reset_subdevice(struct me_subdevice *subdevice,
+					struct file *filep, int flags)
+{
+	me8100_di_subdevice_t *instance;
+	unsigned short ctrl;
+	unsigned long cpu_flags;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8100_di_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	spin_lock(instance->ctrl_reg_lock);
+	ctrl = inw(instance->ctrl_reg);
+	ctrl &= ~(ME8100_DIO_CTRL_BIT_INTB_1 | ME8100_DIO_CTRL_BIT_INTB_0);
+	outw(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl);
+	spin_unlock(instance->ctrl_reg_lock);
+
+	outw(0, instance->mask_reg);
+	PDEBUG_REG("mask_reg outw(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->mask_reg - instance->reg_base, 0);
+	outw(0, instance->pattern_reg);
+	PDEBUG_REG("pattern_reg outw(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->pattern_reg - instance->reg_base, 0);
+	instance->rised = -1;
+	instance->irq_count = 0;
+	instance->filtering_flag = 0;
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	outl(PLX_INTCSR_LOCAL_INT1_EN |
+	     PLX_INTCSR_LOCAL_INT1_POL |
+	     PLX_INTCSR_LOCAL_INT2_EN |
+	     PLX_INTCSR_LOCAL_INT2_POL |
+	     PLX_INTCSR_PCI_INT_EN, instance->irq_status_reg);
+	PDEBUG_REG("plx:irq_status_reg outl(0x%lX)=0x%x\n",
+		   instance->irq_status_reg,
+		   PLX_INTCSR_LOCAL_INT1_EN | PLX_INTCSR_LOCAL_INT1_POL |
+		   PLX_INTCSR_LOCAL_INT2_EN | PLX_INTCSR_LOCAL_INT2_POL |
+		   PLX_INTCSR_PCI_INT_EN);
+
+	wake_up_interruptible_all(&instance->wait_queue);
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8100_di_io_irq_start(me_subdevice_t * subdevice,
+				  struct file *filep,
+				  int channel,
+				  int irq_source,
+				  int irq_edge, int irq_arg, int flags)
+{
+	me8100_di_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint16_t ctrl;
+	unsigned long cpu_flags;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8100_di_subdevice_t *) subdevice;
+
+	if (irq_source == ME_IRQ_SOURCE_DIO_PATTERN) {
+		if (flags &
+		    ~(ME_IO_IRQ_START_PATTERN_FILTERING |
+		      ME_IO_IRQ_START_DIO_WORD)) {
+			PERROR("Invalid flag specified.\n");
+			return ME_ERRNO_INVALID_FLAGS;
+		}
+
+		if (irq_edge != ME_IRQ_EDGE_NOT_USED) {
+			PERROR("Invalid irq edge specified.\n");
+			return ME_ERRNO_INVALID_IRQ_EDGE;
+		}
+	} else if (irq_source == ME_IRQ_SOURCE_DIO_MASK) {
+		if (flags &
+		    ~(ME_IO_IRQ_START_EXTENDED_STATUS |
+		      ME_IO_IRQ_START_DIO_WORD)) {
+			PERROR("Invalid flag specified.\n");
+			return ME_ERRNO_INVALID_FLAGS;
+		}
+
+		if (irq_edge != ME_IRQ_EDGE_ANY) {
+			PERROR("Invalid irq edge specified.\n");
+			return ME_ERRNO_INVALID_IRQ_EDGE;
+		}
+
+		if (!(irq_arg & 0xFFFF)) {
+			PERROR("No mask specified.\n");
+			return ME_ERRNO_INVALID_IRQ_ARG;
+		}
+	} else {
+		PERROR("Invalid irq source specified.\n");
+		return ME_ERRNO_INVALID_IRQ_SOURCE;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	if (irq_source == ME_IRQ_SOURCE_DIO_PATTERN) {
+		outw(irq_arg, instance->pattern_reg);
+		instance->compare_value = irq_arg;
+		instance->filtering_flag =
+		    (flags & ME_IO_IRQ_START_PATTERN_FILTERING) ? 1 : 0;
+	}
+	if (irq_source == ME_IRQ_SOURCE_DIO_MASK) {
+		outw(irq_arg, instance->mask_reg);
+	}
+
+	spin_lock(instance->ctrl_reg_lock);
+	ctrl = inw(instance->ctrl_reg);
+	ctrl |= ME8100_DIO_CTRL_BIT_INTB_0;
+	if (irq_source == ME_IRQ_SOURCE_DIO_PATTERN) {
+		ctrl &= ~ME8100_DIO_CTRL_BIT_INTB_1;
+	}
+
+	if (irq_source == ME_IRQ_SOURCE_DIO_MASK) {
+		ctrl |= ME8100_DIO_CTRL_BIT_INTB_1;
+	}
+	outw(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outw(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl);
+	spin_unlock(instance->ctrl_reg_lock);
+
+	instance->rised = 0;
+	instance->status_value = 0;
+	instance->status_value_edges = 0;
+	instance->line_value = inw(instance->port_reg);
+	instance->status_flag = flags & ME_IO_IRQ_START_EXTENDED_STATUS;
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8100_di_io_irq_wait(me_subdevice_t * subdevice,
+				 struct file *filep,
+				 int channel,
+				 int *irq_count,
+				 int *value, int time_out, int flags)
+{
+	me8100_di_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	long t = 0;
+	unsigned long cpu_flags;
+	int count;
+
+	PDEBUG("executed.\n");
+	PDEVELOP("PID: %d.\n", current->pid);
+
+	instance = (me8100_di_subdevice_t *) subdevice;
+
+	if (flags &
+	    ~(ME_IO_IRQ_WAIT_NORMAL_STATUS | ME_IO_IRQ_WAIT_EXTENDED_STATUS)) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid time_out specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	if (time_out) {
+		t = (time_out * HZ) / 1000;
+
+		if (t == 0)
+			t = 1;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	if (instance->rised <= 0) {
+		instance->rised = 0;
+		count = instance->irq_count;
+
+		if (time_out) {
+			t = wait_event_interruptible_timeout(instance->
+							     wait_queue,
+							     ((count !=
+							       instance->
+							       irq_count)
+							      || (instance->
+								  rised < 0)),
+							     t);
+//                      t = wait_event_interruptible_timeout(instance->wait_queue, (instance->rised != 0), t);
+			if (t == 0) {
+				PERROR("Wait on interrupt timed out.\n");
+				err = ME_ERRNO_TIMEOUT;
+			}
+		} else {
+			wait_event_interruptible(instance->wait_queue,
+						 ((count != instance->irq_count)
+						  || (instance->rised < 0)));
+//                      wait_event_interruptible(instance->wait_queue, (instance->rised != 0));
+		}
+
+		if (instance->rised < 0) {
+			PERROR("Wait on interrupt aborted by user.\n");
+			err = ME_ERRNO_CANCELLED;
+		}
+	}
+
+	if (signal_pending(current)) {
+		PERROR("Wait on interrupt aborted by signal.\n");
+		err = ME_ERRNO_SIGNAL;
+	}
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	*irq_count = instance->irq_count;
+	if (!err) {
+		if (flags & ME_IO_IRQ_WAIT_NORMAL_STATUS) {
+			*value = instance->status_value;
+		} else if (flags & ME_IO_IRQ_WAIT_EXTENDED_STATUS) {
+			*value = instance->status_value_edges;
+		} else {	// Use default
+			if (!instance->status_flag) {
+				*value = instance->status_value;
+			} else {
+				*value = instance->status_value_edges;
+			}
+		}
+		instance->rised = 0;
+/*
+			instance->status_value = 0;
+			instance->status_value_edges = 0;
+*/
+	} else {
+		*value = 0;
+	}
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8100_di_io_irq_stop(me_subdevice_t * subdevice,
+				 struct file *filep, int channel, int flags)
+{
+	me8100_di_subdevice_t *instance;
+	uint16_t ctrl;
+	unsigned long cpu_flags;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8100_di_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	spin_lock(instance->ctrl_reg_lock);
+	ctrl = inw(instance->ctrl_reg);
+	ctrl &= ~(ME8100_DIO_CTRL_BIT_INTB_1 | ME8100_DIO_CTRL_BIT_INTB_0);
+	outw(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outw(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl);
+	spin_unlock(instance->ctrl_reg_lock);
+	instance->rised = -1;
+	instance->status_value = 0;
+	instance->status_value_edges = 0;
+	instance->filtering_flag = 0;
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8100_di_io_single_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int single_config,
+				      int ref,
+				      int trig_chan,
+				      int trig_type, int trig_edge, int flags)
+{
+	me8100_di_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8100_di_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+
+	switch (flags) {
+	case ME_IO_SINGLE_CONFIG_NO_FLAGS:
+	case ME_IO_SINGLE_CONFIG_DIO_WORD:
+		if (channel == 0) {
+			if (single_config == ME_SINGLE_CONFIG_DIO_INPUT) {
+			} else {
+				PERROR
+				    ("Invalid port configuration specified.\n");
+				err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		} else {
+			PERROR("Invalid channel number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8100_di_io_single_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int channel,
+				    int *value, int time_out, int flags)
+{
+	me8100_di_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8100_di_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+
+	switch (flags) {
+
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 16)) {
+			*value = inw(instance->port_reg) & (0x1 << channel);
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			*value = inw(instance->port_reg) & 0xFF;
+		} else if (channel == 1) {
+			*value = (inw(instance->port_reg) >> 8) & 0xFF;
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_WORD:
+		if (channel == 0) {
+			*value = inw(instance->port_reg);
+		} else {
+			PERROR("Invalid word number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8100_di_query_number_channels(me_subdevice_t * subdevice,
+					   int *number)
+{
+	PDEBUG("executed.\n");
+	*number = 16;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8100_di_query_subdevice_type(me_subdevice_t * subdevice,
+					  int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_DI;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8100_di_query_subdevice_caps(me_subdevice_t * subdevice, int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = ME_CAPS_DIO_BIT_PATTERN_IRQ | ME_CAPS_DIO_BIT_MASK_IRQ_EDGE_ANY;
+	return ME_ERRNO_SUCCESS;
+}
+
+static void me8100_di_destructor(struct me_subdevice *subdevice)
+{
+	me8100_di_subdevice_t *instance;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8100_di_subdevice_t *) subdevice;
+
+	free_irq(instance->irq, (void *)instance);
+	me_subdevice_deinit(&instance->base);
+	kfree(instance);
+}
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 19)
+static irqreturn_t me8100_isr(int irq, void *dev_id)
+#else
+static irqreturn_t me8100_isr(int irq, void *dev_id, struct pt_regs *regs)
+#endif
+{
+	me8100_di_subdevice_t *instance;
+	uint32_t icsr;
+
+	uint16_t irq_status;
+	uint16_t line_value = 0;
+
+	uint32_t status_val = 0;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8100_di_subdevice_t *) dev_id;
+
+	if (irq != instance->irq) {
+		PERROR("Incorrect interrupt num: %d.\n", irq);
+		return IRQ_NONE;
+	}
+
+	icsr = inl(instance->irq_status_reg);
+	if (instance->di_idx == 0) {
+
+		if ((icsr &
+		     (PLX_INTCSR_LOCAL_INT1_STATE | PLX_INTCSR_PCI_INT_EN |
+		      PLX_INTCSR_LOCAL_INT1_EN)) !=
+		    (PLX_INTCSR_LOCAL_INT1_STATE | PLX_INTCSR_PCI_INT_EN |
+		     PLX_INTCSR_LOCAL_INT1_EN)) {
+			PINFO
+			    ("%ld Shared interrupt. %s(): idx=0 plx:irq_status_reg=0x%04X\n",
+			     jiffies, __FUNCTION__, icsr);
+			return IRQ_NONE;
+		}
+	} else if (instance->di_idx == 1) {
+		if ((icsr &
+		     (PLX_INTCSR_LOCAL_INT2_STATE | PLX_INTCSR_PCI_INT_EN |
+		      PLX_INTCSR_LOCAL_INT2_EN)) !=
+		    (PLX_INTCSR_LOCAL_INT2_STATE | PLX_INTCSR_PCI_INT_EN |
+		     PLX_INTCSR_LOCAL_INT2_EN)) {
+			PINFO
+			    ("%ld Shared interrupt. %s(): idx=1 plx:irq_status_reg=0x%04X\n",
+			     jiffies, __FUNCTION__, icsr);
+			return IRQ_NONE;
+		}
+	} else {
+		PERROR("%s():Wrong interrupt idx=%d csr=0x%X.\n", __FUNCTION__,
+		       instance->di_idx, icsr);
+		return IRQ_NONE;
+	}
+
+	PDEBUG("me8100_isr():Interrupt from idx=%d occured.\n",
+	       instance->di_idx);
+	spin_lock(&instance->subdevice_lock);
+	inw(instance->irq_reset_reg);
+	line_value = inw(instance->port_reg);
+
+	irq_status = instance->line_value ^ line_value;
+
+	// Make extended information.
+	status_val |= (0x00FF & (~(uint16_t) instance->line_value & line_value)) << 16;	//Raise
+	status_val |= (0x00FF & ((uint16_t) instance->line_value & ~line_value));	//Fall
+
+	instance->line_value = line_value;
+
+	if (instance->rised == 0) {
+		instance->status_value = irq_status;
+		instance->status_value_edges = status_val;
+	} else {
+		instance->status_value |= irq_status;
+		instance->status_value_edges |= status_val;
+	}
+
+	if (instance->filtering_flag) {	// For compare mode only.
+		if (instance->compare_value == instance->line_value) {
+			instance->rised = 1;
+			instance->irq_count++;
+		}
+	} else {
+		instance->rised = 1;
+		instance->irq_count++;
+	}
+
+	spin_unlock(&instance->subdevice_lock);
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	return IRQ_HANDLED;
+}
+
+me8100_di_subdevice_t *me8100_di_constructor(uint32_t me8100_reg_base,
+					     uint32_t plx_reg_base,
+					     unsigned int di_idx,
+					     int irq,
+					     spinlock_t * ctrl_reg_lock)
+{
+	me8100_di_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me8100_di_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me8100_di_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	subdevice->ctrl_reg_lock = ctrl_reg_lock;
+
+	/* Save the subdevice index. */
+	subdevice->di_idx = di_idx;
+
+	/* Initialize wait queue */
+	init_waitqueue_head(&subdevice->wait_queue);
+
+	/* Register interrupt service routine. */
+	subdevice->irq = irq;
+	err = request_irq(subdevice->irq, me8100_isr,
+#ifdef IRQF_DISABLED
+			  IRQF_DISABLED | IRQF_SHARED,
+#else
+			  SA_INTERRUPT | SA_SHIRQ,
+#endif
+			  ME8100_NAME, (void *)subdevice);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	PINFO("Registered irq=%d.\n", subdevice->irq);
+
+	/* Initialize the registers */
+	subdevice->ctrl_reg =
+	    me8100_reg_base + ME8100_CTRL_REG_A + di_idx * ME8100_REG_OFFSET;
+	subdevice->port_reg =
+	    me8100_reg_base + ME8100_DI_REG_A + di_idx * ME8100_REG_OFFSET;
+	subdevice->mask_reg =
+	    me8100_reg_base + ME8100_MASK_REG_A + di_idx * ME8100_REG_OFFSET;
+	subdevice->pattern_reg =
+	    me8100_reg_base + ME8100_PATTERN_REG_A + di_idx * ME8100_REG_OFFSET;
+	subdevice->din_int_reg =
+	    me8100_reg_base + ME8100_INT_DI_REG_A + di_idx * ME8100_REG_OFFSET;
+	subdevice->irq_reset_reg =
+	    me8100_reg_base + ME8100_RES_INT_REG_A + di_idx * ME8100_REG_OFFSET;
+	subdevice->irq_status_reg = plx_reg_base + PLX_INTCSR;
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = me8100_reg_base;
+#endif
+
+	/* Overload base class methods. */
+	subdevice->base.me_subdevice_io_irq_start = me8100_di_io_irq_start;
+	subdevice->base.me_subdevice_io_irq_wait = me8100_di_io_irq_wait;
+	subdevice->base.me_subdevice_io_irq_stop = me8100_di_io_irq_stop;
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me8100_di_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me8100_di_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me8100_di_io_single_read;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me8100_di_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me8100_di_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me8100_di_query_subdevice_caps;
+	subdevice->base.me_subdevice_destructor = me8100_di_destructor;
+
+	subdevice->rised = 0;
+	subdevice->irq_count = 0;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me8100_di.h b/drivers/staging/meilhaus/me8100_di.h
new file mode 100644
index 00000000000..e1db7912917
--- /dev/null
+++ b/drivers/staging/meilhaus/me8100_di.h
@@ -0,0 +1,89 @@
+/**
+ * @file me8100_di.h
+ *
+ * @brief ME-8100 digital input subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME8100_DI_H_
+#define _ME8100_DI_H_
+
+#include "mesubdevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The template subdevice class.
+ */
+typedef struct me8100_di_subdevice {
+	// Inheritance
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *ctrl_reg_lock;
+
+	unsigned di_idx;
+
+	int irq;
+	volatile int rised;
+	unsigned int irq_count;
+
+	uint status_flag;				/**< Default interupt status flag */
+	uint status_value;				/**< Interupt status */
+	uint status_value_edges;		/**< Extended interupt status */
+	uint line_value;
+
+	uint16_t compare_value;
+	uint8_t filtering_flag;
+
+	wait_queue_head_t wait_queue;
+
+	unsigned long ctrl_reg;
+	unsigned long port_reg;
+	unsigned long mask_reg;
+	unsigned long pattern_reg;
+	unsigned long long din_int_reg;
+	unsigned long irq_reset_reg;
+	unsigned long irq_status_reg;
+#ifdef MEDEBUG_DEBUG_REG
+	unsigned long reg_base;
+#endif
+
+} me8100_di_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-8100 digital input subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me8100_di_subdevice_t *me8100_di_constructor(uint32_t me8100_reg_base,
+					     uint32_t plx_reg_base,
+					     unsigned int di_idx,
+					     int irq,
+					     spinlock_t * ctrl_leg_lock);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me8100_di_reg.h b/drivers/staging/meilhaus/me8100_di_reg.h
new file mode 100644
index 00000000000..063bd193709
--- /dev/null
+++ b/drivers/staging/meilhaus/me8100_di_reg.h
@@ -0,0 +1,47 @@
+/**
+ * @file me8100_di_reg.h
+ *
+ * @brief ME-8100 digital input subdevice register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME8100_DI_REG_H_
+#define _ME8100_DI_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME8100_RES_INT_REG_A		0x02	//(r, )
+#define ME8100_DI_REG_A			0x04	//(r, )
+#define ME8100_PATTERN_REG_A		0x08	//( ,w)
+#define ME8100_MASK_REG_A		0x0A	//( ,w)
+#define ME8100_INT_DI_REG_A		0x0A	//(r, )
+
+#define ME8100_RES_INT_REG_B		0x0E	//(r, )
+#define ME8100_DI_REG_B			0x10	//(r, )
+#define ME8100_PATTERN_REG_B		0x14	//( ,w)
+#define ME8100_MASK_REG_B		0x16	//( ,w)
+#define ME8100_INT_DI_REG_B		0x16	//(r, )
+
+#define ME8100_REG_OFFSET		0x0C
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me8100_do.c b/drivers/staging/meilhaus/me8100_do.c
new file mode 100644
index 00000000000..957b9f92f76
--- /dev/null
+++ b/drivers/staging/meilhaus/me8100_do.c
@@ -0,0 +1,391 @@
+/**
+ * @file me8100_do.c
+ *
+ * @brief ME-8100 digital output subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "me8100_reg.h"
+#include "me8100_do_reg.h"
+#include "me8100_do.h"
+
+/*
+ * Defines
+ */
+
+/*
+ * Functions
+ */
+
+static int me8100_do_io_reset_subdevice(struct me_subdevice *subdevice,
+					struct file *filep, int flags)
+{
+	me8100_do_subdevice_t *instance;
+	uint16_t ctrl;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8100_do_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	ctrl = inw(instance->ctrl_reg);
+	ctrl &= ME8100_DIO_CTRL_BIT_INTB_1 | ME8100_DIO_CTRL_BIT_INTB_0;
+	outw(ctrl, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outw(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, ctrl);
+	spin_unlock(instance->ctrl_reg_lock);
+	outw(0, instance->port_reg);
+	instance->port_reg_mirror = 0;
+	PDEBUG_REG("port_reg outw(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->port_reg - instance->reg_base, 0);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8100_do_io_single_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int single_config,
+				      int ref,
+				      int trig_chan,
+				      int trig_type, int trig_edge, int flags)
+{
+	me8100_do_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	int config;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8100_do_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	config = inw(instance->ctrl_reg);
+	switch (flags) {
+	case ME_IO_SINGLE_CONFIG_NO_FLAGS:
+	case ME_IO_SINGLE_CONFIG_DIO_WORD:
+		if (channel == 0) {
+			if (single_config ==
+			    ME_SINGLE_CONFIG_DIO_HIGH_IMPEDANCE) {
+				config &= ~(ME8100_DIO_CTRL_BIT_ENABLE_DIO);
+			} else if (single_config == ME_SINGLE_CONFIG_DIO_SINK) {
+				config |= ME8100_DIO_CTRL_BIT_ENABLE_DIO;
+				config &= ~ME8100_DIO_CTRL_BIT_SOURCE;
+			} else if (single_config == ME_SINGLE_CONFIG_DIO_SOURCE) {
+				config |=
+				    ME8100_DIO_CTRL_BIT_ENABLE_DIO |
+				    ME8100_DIO_CTRL_BIT_SOURCE;
+			} else {
+				PERROR
+				    ("Invalid port configuration specified.\n");
+				err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		} else {
+			PERROR("Invalid word number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (!err) {
+		outw(config, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outw(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, config);
+	}
+
+	spin_unlock(instance->ctrl_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8100_do_io_single_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int channel,
+				    int *value, int time_out, int flags)
+{
+	me8100_do_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8100_do_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 16)) {
+			*value = instance->port_reg_mirror & (0x1 << channel);
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			*value = instance->port_reg_mirror & 0xFF;
+		} else if (channel == 1) {
+			*value = (instance->port_reg_mirror >> 8) & 0xFF;
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_WORD:
+		if (channel == 0) {
+			*value = instance->port_reg_mirror;
+		} else {
+			PERROR("Invalid word number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8100_do_io_single_write(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int channel,
+				     int value, int time_out, int flags)
+{
+	me8100_do_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8100_do_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 16)) {
+			instance->port_reg_mirror =
+			    value ? (instance->
+				     port_reg_mirror | (0x1 << channel))
+			    : (instance->port_reg_mirror & ~(0x1 << channel));
+			outw(instance->port_reg_mirror, instance->port_reg);
+			PDEBUG_REG("port_reg outw(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->port_reg - instance->reg_base,
+				   instance->port_reg_mirror);
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			instance->port_reg_mirror &= ~0xFF;
+			instance->port_reg_mirror |= value & 0xFF;
+			outw(instance->port_reg_mirror, instance->port_reg);
+			PDEBUG_REG("port_reg outw(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->port_reg - instance->reg_base,
+				   instance->port_reg_mirror);
+		} else if (channel == 1) {
+			instance->port_reg_mirror &= ~0xFF00;
+			instance->port_reg_mirror |= (value << 8) & 0xFF00;
+			outw(instance->port_reg_mirror, instance->port_reg);
+			PDEBUG_REG("port_reg outw(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->port_reg - instance->reg_base,
+				   instance->port_reg_mirror);
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_WORD:
+		if (channel == 0) {
+			instance->port_reg_mirror = value;
+			outw(value, instance->port_reg);
+			PDEBUG_REG("port_reg outw(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->port_reg - instance->reg_base,
+				   value);
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8100_do_query_number_channels(me_subdevice_t * subdevice,
+					   int *number)
+{
+	PDEBUG("executed.\n");
+	*number = 16;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8100_do_query_subdevice_type(me_subdevice_t * subdevice,
+					  int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_DO;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8100_do_query_subdevice_caps(me_subdevice_t * subdevice, int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = ME_CAPS_DIO_SINK_SOURCE;
+	return ME_ERRNO_SUCCESS;
+}
+
+me8100_do_subdevice_t *me8100_do_constructor(uint32_t reg_base,
+					     unsigned int do_idx,
+					     spinlock_t * ctrl_reg_lock)
+{
+	me8100_do_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me8100_do_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me8100_do_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+
+	/* Initialize registers */
+	if (do_idx == 0) {
+		subdevice->port_reg = reg_base + ME8100_DO_REG_A;
+		subdevice->ctrl_reg = reg_base + ME8100_CTRL_REG_A;
+	} else if (do_idx == 1) {
+		subdevice->port_reg = reg_base + ME8100_DO_REG_B;
+		subdevice->ctrl_reg = reg_base + ME8100_CTRL_REG_B;
+	} else {
+		PERROR("Wrong subdevice idx=%d.\n", do_idx);
+		kfree(subdevice);
+		return NULL;
+	}
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = reg_base;
+#endif
+
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+	subdevice->ctrl_reg_lock = ctrl_reg_lock;
+
+	/* Save the subdevice index */
+	subdevice->do_idx = do_idx;
+
+	/* Overload base class methods. */
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me8100_do_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me8100_do_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me8100_do_io_single_read;
+	subdevice->base.me_subdevice_io_single_write =
+	    me8100_do_io_single_write;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me8100_do_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me8100_do_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me8100_do_query_subdevice_caps;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me8100_do.h b/drivers/staging/meilhaus/me8100_do.h
new file mode 100644
index 00000000000..acf88013666
--- /dev/null
+++ b/drivers/staging/meilhaus/me8100_do.h
@@ -0,0 +1,70 @@
+/**
+ * @file me8100_do.h
+ *
+ * @brief ME-8100 digital output subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME8100_DO_H_
+#define _ME8100_DO_H_
+
+#include "mesubdevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The template subdevice class.
+ */
+typedef struct me8100_do_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *ctrl_reg_lock;		/**< Spin lock to protect the #ctrl_reg. */
+
+	unsigned int do_idx;
+
+	uint16_t port_reg_mirror;		/**< Mirror used to store current port register setting which is write only. */
+
+	unsigned long port_reg;			/**< Register holding the port status. */
+	unsigned long ctrl_reg;			/**< Control register. */
+#ifdef MEDEBUG_DEBUG_REG
+	unsigned long reg_base;
+#endif
+} me8100_do_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-8100 digital output subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ * @param do_idx The index of the digital output subdevice on this device.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me8100_do_subdevice_t *me8100_do_constructor(uint32_t reg_base,
+					     unsigned int do_idx,
+					     spinlock_t * ctrl_reg_lock);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me8100_do_reg.h b/drivers/staging/meilhaus/me8100_do_reg.h
new file mode 100644
index 00000000000..13a23802b31
--- /dev/null
+++ b/drivers/staging/meilhaus/me8100_do_reg.h
@@ -0,0 +1,36 @@
+/**
+ * @file me8100_ao_reg.h
+ *
+ * @brief ME-8100 analog output subdevice register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME8100_DO_REG_H_
+#define _ME8100_DO_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME8100_DO_REG_A			0x06	//( ,w)
+#define ME8100_DO_REG_B			0x12	//( ,w)
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me8100_reg.h b/drivers/staging/meilhaus/me8100_reg.h
new file mode 100644
index 00000000000..d8c4b1c6b15
--- /dev/null
+++ b/drivers/staging/meilhaus/me8100_reg.h
@@ -0,0 +1,41 @@
+/**
+ * @file me8100_reg.h
+ *
+ * @brief ME-8100 register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME8100_REG_H_
+#define _ME8100_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME8100_CTRL_REG_A			0x00	//( ,w)
+#define ME8100_CTRL_REG_B			0x0C	//( ,w)
+
+#define ME8100_DIO_CTRL_BIT_SOURCE		0x10
+#define ME8100_DIO_CTRL_BIT_INTB_1		0x20
+#define ME8100_DIO_CTRL_BIT_INTB_0		0x40
+#define ME8100_DIO_CTRL_BIT_ENABLE_DIO		0x80
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me8200_device.c b/drivers/staging/meilhaus/me8200_device.c
new file mode 100644
index 00000000000..261c0cbd9d0
--- /dev/null
+++ b/drivers/staging/meilhaus/me8200_device.c
@@ -0,0 +1,194 @@
+/**
+ * @file me8200_device.c
+ *
+ * @brief ME-8200 device class implementation.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+#ifndef MODULE
+#  define MODULE
+#endif
+
+#include <linux/module.h>
+
+#include <linux/pci.h>
+#include <linux/slab.h>
+
+#include "meids.h"
+#include "meerror.h"
+#include "mecommon.h"
+#include "meinternal.h"
+
+#include "medebug.h"
+#include "meplx_reg.h"
+#include "medevice.h"
+#include "me8200_device.h"
+#include "mesubdevice.h"
+#include "me8200_di.h"
+#include "me8200_do.h"
+#include "me8200_dio.h"
+
+me_device_t *me8200_pci_constructor(struct pci_dev *pci_device)
+{
+	me8200_device_t *me8200_device;
+	me_subdevice_t *subdevice;
+	unsigned int version_idx;
+	int err;
+	int i;
+
+	PDEBUG("executed.\n");
+
+	// Allocate structure for device instance.
+	me8200_device = kmalloc(sizeof(me8200_device_t), GFP_KERNEL);
+
+	if (!me8200_device) {
+		PERROR("Cannot get memory for device instance.\n");
+		return NULL;
+	}
+
+	memset(me8200_device, 0, sizeof(me8200_device_t));
+
+	// Initialize base class structure.
+	err = me_device_pci_init((me_device_t *) me8200_device, pci_device);
+
+	if (err) {
+		kfree(me8200_device);
+		PERROR("Cannot initialize device base class.\n");
+		return NULL;
+	}
+
+	/* Get the index in the device version information table. */
+	version_idx =
+	    me8200_versions_get_device_index(me8200_device->base.info.pci.
+					     device_id);
+
+	// Initialize spin lock .
+	spin_lock_init(&me8200_device->irq_ctrl_lock);
+	spin_lock_init(&me8200_device->irq_mode_lock);
+	spin_lock_init(&me8200_device->dio_ctrl_lock);
+
+	/* Setup the PLX interrupt configuration */
+	outl(PLX_INTCSR_LOCAL_INT1_EN |
+	     PLX_INTCSR_LOCAL_INT1_POL |
+	     PLX_INTCSR_LOCAL_INT2_EN |
+	     PLX_INTCSR_LOCAL_INT2_POL |
+	     PLX_INTCSR_PCI_INT_EN,
+	     me8200_device->base.info.pci.reg_bases[1] + PLX_INTCSR);
+
+	// Create subdevice instances.
+
+	for (i = 0; i < me8200_versions[version_idx].di_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) me8200_di_constructor(me8200_device->
+							     base.info.pci.
+							     reg_bases[2], i,
+							     me8200_device->
+							     base.irq,
+							     &me8200_device->
+							     irq_ctrl_lock,
+							     &me8200_device->
+							     irq_mode_lock);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me8200_device);
+			kfree(me8200_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me8200_device->base.slist,
+					    subdevice);
+	}
+
+	for (i = 0; i < me8200_versions[version_idx].do_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) me8200_do_constructor(me8200_device->
+							     base.info.pci.
+							     reg_bases[2], i,
+							     me8200_device->
+							     base.irq,
+							     &me8200_device->
+							     irq_mode_lock);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me8200_device);
+			kfree(me8200_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me8200_device->base.slist,
+					    subdevice);
+	}
+
+	for (i = 0; i < me8200_versions[version_idx].dio_subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) me8200_dio_constructor(me8200_device->
+							      base.info.pci.
+							      reg_bases[2], i,
+							      &me8200_device->
+							      dio_ctrl_lock);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) me8200_device);
+			kfree(me8200_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&me8200_device->base.slist,
+					    subdevice);
+	}
+
+	return (me_device_t *) me8200_device;
+}
+
+// Init and exit of module.
+
+static int __init me8200_init(void)
+{
+	PDEBUG("executed.\n.");
+	return 0;
+}
+
+static void __exit me8200_exit(void)
+{
+	PDEBUG("executed.\n.");
+}
+
+module_init(me8200_init);
+
+module_exit(me8200_exit);
+
+// Administrative stuff for modinfo.
+MODULE_AUTHOR("Guenter Gebhardt <g.gebhardt@meilhaus.de>");
+MODULE_DESCRIPTION("Device Driver Module for Template Device");
+MODULE_SUPPORTED_DEVICE("Meilhaus Template Devices");
+MODULE_LICENSE("GPL");
+
+// Export the constructor.
+EXPORT_SYMBOL(me8200_pci_constructor);
diff --git a/drivers/staging/meilhaus/me8200_device.h b/drivers/staging/meilhaus/me8200_device.h
new file mode 100644
index 00000000000..cbd2a01ddb4
--- /dev/null
+++ b/drivers/staging/meilhaus/me8200_device.h
@@ -0,0 +1,97 @@
+/**
+ * @file me8200_device.h
+ *
+ * @brief ME-8200 device class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME8200_DEVICE_H
+#define _ME8200_DEVICE_H
+
+#include <linux/pci.h>
+#include <linux/spinlock.h>
+
+#include "medevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief Structure holding ME-8200 device capabilities.
+ */
+typedef struct me8200_version {
+	uint16_t device_id;
+	unsigned int di_subdevices;
+	unsigned int do_subdevices;
+	unsigned int dio_subdevices;
+} me8200_version_t;
+
+/**
+ * @brief Device capabilities.
+ */
+static me8200_version_t me8200_versions[] = {
+	{PCI_DEVICE_ID_MEILHAUS_ME8200_A, 1, 1, 2},
+	{PCI_DEVICE_ID_MEILHAUS_ME8200_B, 2, 2, 2},
+	{0},
+};
+
+#define ME8200_DEVICE_VERSIONS (sizeof(me8200_versions) / sizeof(me8200_version_t) - 1)	/**< Returns the number of entries in #me8200_versions. */
+
+/**
+ * @brief Returns the index of the device entry in #me8200_versions.
+ *
+ * @param device_id The PCI device id of the device to query.
+ * @return The index of the device in #me8200_versions.
+ */
+static inline unsigned int me8200_versions_get_device_index(uint16_t device_id)
+{
+	unsigned int i;
+	for (i = 0; i < ME8200_DEVICE_VERSIONS; i++)
+		if (me8200_versions[i].device_id == device_id)
+			break;
+	return i;
+}
+
+/**
+ * @brief The ME-8200 device class structure.
+ */
+typedef struct me8200_device {
+	me_device_t base;		/**< The Meilhaus device base class. */
+
+	/* Child class attributes. */
+	spinlock_t irq_ctrl_lock;	/**< Lock for the interrupt control register. */
+	spinlock_t irq_mode_lock;	/**< Lock for the interrupt mode register. */
+	spinlock_t dio_ctrl_lock;	/**< Lock for the digital i/o control register. */
+} me8200_device_t;
+
+/**
+ * @brief The ME-8200 device class constructor.
+ *
+ * @param pci_device The pci device structure given by the PCI subsystem.
+ *
+ * @return On succes a new ME-8200 device instance. \n
+ *         NULL on error.
+ */
+me_device_t *me8200_pci_constructor(struct pci_dev *pci_device)
+    __attribute__ ((weak));
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me8200_di.c b/drivers/staging/meilhaus/me8200_di.c
new file mode 100644
index 00000000000..0bb4567091c
--- /dev/null
+++ b/drivers/staging/meilhaus/me8200_di.c
@@ -0,0 +1,857 @@
+/**
+ * @file me8200_di.c
+ *
+ * @brief ME-8200 digital input subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+///Includes
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+#include <linux/interrupt.h>
+#include <linux/version.h>
+
+#include "medefines.h"
+#include "meerror.h"
+
+#include "meids.h"
+#include "medebug.h"
+#include "me8200_reg.h"
+#include "me8200_di_reg.h"
+#include "me8200_di.h"
+
+/// Defines
+static void me8200_di_destructor(struct me_subdevice *subdevice);
+static int me8200_di_io_irq_start(me_subdevice_t * subdevice,
+				  struct file *filep,
+				  int channel,
+				  int irq_source,
+				  int irq_edge, int irq_arg, int flags);
+static int me8200_di_io_irq_wait(me_subdevice_t * subdevice,
+				 struct file *filep,
+				 int channel,
+				 int *irq_count,
+				 int *value, int time_out, int flags);
+static int me8200_di_io_irq_stop(me_subdevice_t * subdevice,
+				 struct file *filep, int channel, int flags);
+static int me8200_di_io_single_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int single_config,
+				      int ref,
+				      int trig_chan,
+				      int trig_type, int trig_edge, int flags);
+static int me8200_di_io_single_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int channel,
+				    int *value, int time_out, int flags);
+static int me8200_di_io_reset_subdevice(struct me_subdevice *subdevice,
+					struct file *filep, int flags);
+static int me8200_di_query_number_channels(me_subdevice_t * subdevice,
+					   int *number);
+static int me8200_di_query_subdevice_type(me_subdevice_t * subdevice,
+					  int *type, int *subtype);
+static int me8200_di_query_subdevice_caps(me_subdevice_t * subdevice,
+					  int *caps);
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 19)
+static irqreturn_t me8200_isr(int irq, void *dev_id);
+#else
+static irqreturn_t me8200_isr(int irq, void *dev_id, struct pt_regs *regs);
+#endif
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 19)
+static irqreturn_t me8200_isr_EX(int irq, void *dev_id);
+#else
+static irqreturn_t me8200_isr_EX(int irq, void *dev_id, struct pt_regs *regs);
+#endif
+static void me8200_di_check_version(me8200_di_subdevice_t * instance,
+				    unsigned long addr);
+
+///Functions
+static int me8200_di_io_irq_start(me_subdevice_t * subdevice,
+				  struct file *filep,
+				  int channel,
+				  int irq_source,
+				  int irq_edge, int irq_arg, int flags)
+{
+	me8200_di_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	volatile uint8_t tmp;
+	unsigned long status;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8200_di_subdevice_t *) subdevice;
+
+	if (irq_source == ME_IRQ_SOURCE_DIO_PATTERN) {
+		if (flags &
+		    ~(ME_IO_IRQ_START_PATTERN_FILTERING |
+		      ME_IO_IRQ_START_DIO_BYTE)) {
+			PERROR("Invalid flag specified.\n");
+			return ME_ERRNO_INVALID_FLAGS;
+		}
+
+		if (irq_edge != ME_IRQ_EDGE_NOT_USED) {
+			PERROR("Invalid irq edge specified.\n");
+			return ME_ERRNO_INVALID_IRQ_EDGE;
+		}
+	} else if (irq_source == ME_IRQ_SOURCE_DIO_MASK) {
+		if (flags &
+		    ~(ME_IO_IRQ_START_EXTENDED_STATUS |
+		      ME_IO_IRQ_START_DIO_BYTE)) {
+			PERROR("Invalid flag specified.\n");
+			return ME_ERRNO_INVALID_FLAGS;
+		}
+
+		if ((irq_edge != ME_IRQ_EDGE_RISING)
+		    && (irq_edge != ME_IRQ_EDGE_FALLING)
+		    && (irq_edge != ME_IRQ_EDGE_ANY)) {
+			PERROR("Invalid irq edge specified.\n");
+			return ME_ERRNO_INVALID_IRQ_EDGE;
+		}
+
+		if (!(irq_arg & 0xFF)) {
+			PERROR("No mask specified.\n");
+			return ME_ERRNO_INVALID_IRQ_ARG;
+		}
+	} else {
+		PERROR("Invalid irq source specified.\n");
+		return ME_ERRNO_INVALID_IRQ_SOURCE;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, status);
+	if (irq_source == ME_IRQ_SOURCE_DIO_PATTERN) {
+		outb(irq_arg, instance->compare_reg);
+		PDEBUG_REG("compare_reg outb(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->compare_reg - instance->reg_base, irq_arg);
+		outb(0xFF, instance->mask_reg);
+		PDEBUG_REG("mask_reg outb(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->mask_reg - instance->reg_base, 0xff);
+		instance->compare_value = irq_arg;
+		instance->filtering_flag =
+		    (flags & ME_IO_IRQ_START_PATTERN_FILTERING) ? 1 : 0;
+	}
+	if (irq_source == ME_IRQ_SOURCE_DIO_MASK) {
+		outb(irq_arg, instance->mask_reg);
+		PDEBUG_REG("mask_reg outb(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->mask_reg - instance->reg_base, irq_arg);
+		instance->filtering_flag = 0;
+	}
+
+	spin_lock(instance->irq_mode_lock);
+	tmp = inb(instance->irq_mode_reg);
+	tmp &=
+	    ~(ME8200_IRQ_MODE_MASK <<
+	      (ME8200_IRQ_MODE_DI_SHIFT * instance->di_idx));
+	if (irq_source == ME_IRQ_SOURCE_DIO_PATTERN) {
+		tmp |=
+		    ME8200_IRQ_MODE_MASK_COMPARE << (ME8200_IRQ_MODE_DI_SHIFT *
+						     instance->di_idx);
+	}
+
+	if (irq_source == ME_IRQ_SOURCE_DIO_MASK) {
+		tmp |=
+		    ME8200_IRQ_MODE_MASK_MASK << (ME8200_IRQ_MODE_DI_SHIFT *
+						  instance->di_idx);
+	}
+	outb(tmp, instance->irq_mode_reg);
+	PDEBUG_REG("irq_mode_reg outb(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->irq_mode_reg - instance->reg_base, tmp);
+	spin_unlock(instance->irq_mode_lock);
+
+	spin_lock(instance->irq_ctrl_lock);
+	tmp = inb(instance->irq_ctrl_reg);
+	tmp |=
+	    (ME8200_DI_IRQ_CTRL_BIT_CLEAR <<
+	     (ME8200_DI_IRQ_CTRL_SHIFT * instance->di_idx));
+	tmp |=
+	    ME8200_DI_IRQ_CTRL_BIT_ENABLE << (ME8200_DI_IRQ_CTRL_SHIFT *
+					      instance->di_idx);
+
+	if (irq_source == ME_IRQ_SOURCE_DIO_MASK) {
+		tmp &=
+		    ~(ME8200_DI_IRQ_CTRL_MASK_EDGE <<
+		      (ME8200_DI_IRQ_CTRL_SHIFT * instance->di_idx));
+		if (irq_edge == ME_IRQ_EDGE_RISING) {
+			tmp |=
+			    ME8200_DI_IRQ_CTRL_MASK_EDGE_RISING <<
+			    (ME8200_DI_IRQ_CTRL_SHIFT * instance->di_idx);
+		} else if (irq_edge == ME_IRQ_EDGE_FALLING) {
+			tmp |=
+			    ME8200_DI_IRQ_CTRL_MASK_EDGE_FALLING <<
+			    (ME8200_DI_IRQ_CTRL_SHIFT * instance->di_idx);
+		} else if (irq_edge == ME_IRQ_EDGE_ANY) {
+			tmp |=
+			    ME8200_DI_IRQ_CTRL_MASK_EDGE_ANY <<
+			    (ME8200_DI_IRQ_CTRL_SHIFT * instance->di_idx);
+		}
+	}
+	outb(tmp, instance->irq_ctrl_reg);
+	PDEBUG_REG("irq_ctrl_reg outb(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->irq_ctrl_reg - instance->reg_base, tmp);
+	tmp &=
+	    ~(ME8200_DI_IRQ_CTRL_BIT_CLEAR <<
+	      (ME8200_DI_IRQ_CTRL_SHIFT * instance->di_idx));
+	outb(tmp, instance->irq_ctrl_reg);
+	PDEBUG_REG("irq_ctrl_reg outb(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->irq_ctrl_reg - instance->reg_base, tmp);
+
+	instance->line_value = inb(instance->port_reg);
+	spin_unlock(instance->irq_ctrl_lock);
+
+	instance->rised = 0;
+	instance->status_value = 0;
+	instance->status_value_edges = 0;
+	instance->status_flag = flags & ME_IO_IRQ_START_EXTENDED_STATUS;
+	spin_unlock_irqrestore(&instance->subdevice_lock, status);
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8200_di_io_irq_wait(me_subdevice_t * subdevice,
+				 struct file *filep,
+				 int channel,
+				 int *irq_count,
+				 int *value, int time_out, int flags)
+{
+	me8200_di_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	long t = 0;
+	unsigned long cpu_flags;
+	int count;
+
+	PDEBUG("executed.\n");
+	PDEVELOP("PID: %d.\n", current->pid);
+
+	instance = (me8200_di_subdevice_t *) subdevice;
+
+	if (flags &
+	    ~(ME_IO_IRQ_WAIT_NORMAL_STATUS | ME_IO_IRQ_WAIT_EXTENDED_STATUS)) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid time_out specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	if (time_out) {
+		t = (time_out * HZ) / 1000;
+
+		if (t == 0)
+			t = 1;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	if (instance->rised <= 0) {
+		instance->rised = 0;
+		count = instance->count;
+
+		if (time_out) {
+			t = wait_event_interruptible_timeout(instance->
+							     wait_queue,
+							     ((count !=
+							       instance->count)
+							      || (instance->
+								  rised < 0)),
+							     t);
+//                      t = wait_event_interruptible_timeout(instance->wait_queue, (instance->rised != 0), t);
+			if (t == 0) {
+				PERROR("Wait on interrupt timed out.\n");
+				err = ME_ERRNO_TIMEOUT;
+			}
+		} else {
+			wait_event_interruptible(instance->wait_queue,
+						 ((count != instance->count)
+						  || (instance->rised < 0)));
+//                      wait_event_interruptible(instance->wait_queue, (instance->rised != 0));
+		}
+
+		if (instance->rised < 0) {
+			PERROR("Wait on interrupt aborted by user.\n");
+			err = ME_ERRNO_CANCELLED;
+		}
+	}
+
+	if (signal_pending(current)) {
+		PERROR("Wait on interrupt aborted by signal.\n");
+		err = ME_ERRNO_SIGNAL;
+	}
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	*irq_count = instance->count;
+	if (!err) {
+		if (flags & ME_IO_IRQ_WAIT_NORMAL_STATUS) {
+			*value = instance->status_value;
+		} else if (flags & ME_IO_IRQ_WAIT_EXTENDED_STATUS) {
+			*value = instance->status_value_edges;
+		} else {	// Use default
+			if (!instance->status_flag) {
+				*value = instance->status_value;
+			} else {
+				*value = instance->status_value_edges;
+			}
+		}
+		instance->rised = 0;
+/*
+			instance->status_value = 0;
+			instance->status_value_edges = 0;
+*/
+	} else {
+		*value = 0;
+	}
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8200_di_io_irq_stop(me_subdevice_t * subdevice,
+				 struct file *filep, int channel, int flags)
+{
+	me8200_di_subdevice_t *instance;
+	uint8_t tmp;
+	unsigned long status;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8200_di_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	ME_SUBDEVICE_ENTER spin_lock_irqsave(&instance->subdevice_lock, status);
+	spin_lock(instance->irq_ctrl_lock);
+	tmp = inb(instance->irq_ctrl_reg);
+	tmp |=
+	    (ME8200_DI_IRQ_CTRL_BIT_ENABLE <<
+	     (ME8200_DI_IRQ_CTRL_SHIFT * instance->di_idx));
+	outb(tmp, instance->irq_ctrl_reg);
+	PDEBUG_REG("irq_ctrl_reg outb(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->irq_ctrl_reg - instance->reg_base, tmp);
+	tmp &=
+	    ~(ME8200_DI_IRQ_CTRL_BIT_ENABLE <<
+	      (ME8200_DI_IRQ_CTRL_SHIFT * instance->di_idx));
+	tmp |=
+	    (ME8200_DI_IRQ_CTRL_BIT_CLEAR <<
+	     (ME8200_DI_IRQ_CTRL_SHIFT * instance->di_idx));
+//                      tmp &= ~(ME8200_DI_IRQ_CTRL_BIT_CLEAR << (ME8200_DI_IRQ_CTRL_SHIFT * instance->di_idx));
+	outb(tmp, instance->irq_ctrl_reg);
+	PDEBUG_REG("irq_ctrl_reg outb(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->irq_ctrl_reg - instance->reg_base, tmp);
+	spin_unlock(instance->irq_ctrl_lock);
+
+	instance->rised = -1;
+	instance->status_value = 0;
+	instance->status_value_edges = 0;
+	instance->filtering_flag = 0;
+	spin_unlock_irqrestore(&instance->subdevice_lock, status);
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8200_di_io_single_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int single_config,
+				      int ref,
+				      int trig_chan,
+				      int trig_type, int trig_edge, int flags)
+{
+	me8200_di_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long status;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8200_di_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, status);
+
+	switch (flags) {
+	case ME_IO_SINGLE_CONFIG_NO_FLAGS:
+	case ME_IO_SINGLE_CONFIG_DIO_BYTE:
+		if (channel == 0) {
+			if (single_config == ME_SINGLE_CONFIG_DIO_INPUT) {
+			} else {
+				PERROR("Invalid port direction specified.\n");
+				err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		} else {
+			PERROR("Invalid channel number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+
+	spin_unlock_irqrestore(&instance->subdevice_lock, status);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8200_di_io_single_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int channel,
+				    int *value, int time_out, int flags)
+{
+	me8200_di_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long status;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8200_di_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, status);
+
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			*value = inb(instance->port_reg) & (0x1 << channel);
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			*value = inb(instance->port_reg);
+		} else {
+			PERROR("Invalid channel number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+
+	spin_unlock_irqrestore(&instance->subdevice_lock, status);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8200_di_io_reset_subdevice(struct me_subdevice *subdevice,
+					struct file *filep, int flags)
+{
+	me8200_di_subdevice_t *instance = (me8200_di_subdevice_t *) subdevice;
+
+	PDEBUG("executed.\n");
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	instance->count = 0;
+	return me8200_di_io_irq_stop(subdevice, filep, 0, 0);
+}
+
+static int me8200_di_query_number_channels(me_subdevice_t * subdevice,
+					   int *number)
+{
+	PDEBUG("executed.\n");
+	*number = 8;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8200_di_query_subdevice_type(me_subdevice_t * subdevice,
+					  int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_DI;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8200_di_query_subdevice_caps(me_subdevice_t * subdevice, int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps =
+	    ME_CAPS_DIO_BIT_PATTERN_IRQ |
+	    ME_CAPS_DIO_BIT_MASK_IRQ_EDGE_RISING |
+	    ME_CAPS_DIO_BIT_MASK_IRQ_EDGE_FALLING |
+	    ME_CAPS_DIO_BIT_MASK_IRQ_EDGE_ANY;
+	return ME_ERRNO_SUCCESS;
+}
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 19)
+static irqreturn_t me8200_isr(int irq, void *dev_id)
+#else
+static irqreturn_t me8200_isr(int irq, void *dev_id, struct pt_regs *regs)
+#endif
+{
+	me8200_di_subdevice_t *instance;
+	uint8_t ctrl;
+	uint8_t irq_status;
+	uint8_t line_value = 0;
+	uint8_t line_status = 0;
+	uint32_t status_val = 0;
+
+	instance = (me8200_di_subdevice_t *) dev_id;
+
+	if (irq != instance->irq) {
+		PERROR("Incorrect interrupt num: %d.\n", irq);
+		return IRQ_NONE;
+	}
+
+	irq_status = inb(instance->irq_status_reg);
+	if (!irq_status) {
+		PINFO
+		    ("%ld Shared interrupt. %s(): idx=%d irq_status_reg=0x%04X\n",
+		     jiffies, __FUNCTION__, instance->di_idx, irq_status);
+		return IRQ_NONE;
+	}
+
+	PDEBUG("executed.\n");
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->irq_ctrl_lock);
+	ctrl = inb(instance->irq_ctrl_reg);
+	ctrl |=
+	    ME8200_DI_IRQ_CTRL_BIT_CLEAR << (ME8200_DI_IRQ_CTRL_SHIFT *
+					     instance->di_idx);
+	outb(ctrl, instance->irq_ctrl_reg);
+	PDEBUG_REG("irq_ctrl_reg outb(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->irq_ctrl_reg - instance->reg_base, ctrl);
+	ctrl &=
+	    ~(ME8200_DI_IRQ_CTRL_BIT_CLEAR <<
+	      (ME8200_DI_IRQ_CTRL_SHIFT * instance->di_idx));
+	outb(ctrl, instance->irq_ctrl_reg);
+	PDEBUG_REG("irq_ctrl_reg outb(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->irq_ctrl_reg - instance->reg_base, ctrl);
+
+	line_value = inb(instance->port_reg);
+	spin_unlock(instance->irq_ctrl_lock);
+
+	line_status = ((uint8_t) instance->line_value ^ line_value);
+
+	// Make extended information.
+	status_val |= (0x00FF & (~(uint8_t) instance->line_value & line_value)) << 16;	//Raise
+	status_val |= (0x00FF & ((uint8_t) instance->line_value & ~line_value));	//Fall
+
+	instance->line_value = (int)line_value;
+
+	if (instance->rised == 0) {
+		instance->status_value = irq_status | line_status;
+		instance->status_value_edges = status_val;
+	} else {
+		instance->status_value |= irq_status | line_status;
+		instance->status_value_edges |= status_val;
+	}
+
+	if (instance->filtering_flag) {	// For compare mode only.
+		if (instance->compare_value == instance->line_value) {
+			instance->rised = 1;
+			instance->count++;
+		}
+	} else {
+		instance->rised = 1;
+		instance->count++;
+	}
+	spin_unlock(&instance->subdevice_lock);
+
+	spin_unlock(&instance->subdevice_lock);
+
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	return IRQ_HANDLED;
+}
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 19)
+static irqreturn_t me8200_isr_EX(int irq, void *dev_id)
+#else
+static irqreturn_t me8200_isr_EX(int irq, void *dev_id, struct pt_regs *regs)
+#endif
+{
+	me8200_di_subdevice_t *instance;
+	uint8_t irq_status = 0;
+	uint16_t irq_status_EX = 0;
+	uint32_t status_val = 0;
+	int i, j;
+
+	instance = (me8200_di_subdevice_t *) dev_id;
+
+	if (irq != instance->irq) {
+		PERROR("Incorrect interrupt num: %d.\n", irq);
+		return IRQ_NONE;
+	}
+
+	PDEBUG("executed.\n");
+
+	//Reset latches. Copy status to extended registers.
+	irq_status = inb(instance->irq_status_reg);
+	PDEBUG_REG("idx=%d irq_status_reg=0x%02X\n", instance->di_idx,
+		   irq_status);
+
+	if (!irq_status) {
+		PINFO
+		    ("%ld Shared interrupt. %s(): idx=%d irq_status_reg=0x%04X\n",
+		     jiffies, __FUNCTION__, instance->di_idx, irq_status);
+		return IRQ_NONE;
+	}
+
+	irq_status_EX = inb(instance->irq_status_low_reg);
+	irq_status_EX |= (inb(instance->irq_status_high_reg) << 8);
+
+	PDEVELOP("EXTENDED REG: 0x%04x\n", irq_status_EX);
+	instance->line_value = inb(instance->port_reg);
+
+	// Format extended information.
+	for (i = 0, j = 0; i < 8; i++, j += 2) {
+		status_val |= ((0x01 << j) & irq_status_EX) >> (j - i);	//Fall
+		status_val |= ((0x01 << (j + 1)) & irq_status_EX) << (15 - j + i);	//Raise
+	}
+
+	spin_lock(&instance->subdevice_lock);
+	if (instance->rised == 0) {
+		instance->status_value = irq_status;
+		instance->status_value_edges = status_val;
+	} else {
+		instance->status_value |= irq_status;
+		instance->status_value_edges |= status_val;
+	}
+
+	if (instance->filtering_flag) {	// For compare mode only.
+		if (instance->compare_value == instance->line_value) {
+			instance->rised = 1;
+			instance->count++;
+		}
+	} else {
+		instance->rised = 1;
+		instance->count++;
+	}
+	spin_unlock(&instance->subdevice_lock);
+
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	return IRQ_HANDLED;
+}
+
+static void me8200_di_destructor(struct me_subdevice *subdevice)
+{
+	me8200_di_subdevice_t *instance;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8200_di_subdevice_t *) subdevice;
+
+	free_irq(instance->irq, (void *)instance);
+	me_subdevice_deinit(&instance->base);
+	kfree(instance);
+}
+
+me8200_di_subdevice_t *me8200_di_constructor(uint32_t me8200_regbase,
+					     unsigned int di_idx,
+					     int irq,
+					     spinlock_t * irq_ctrl_lock,
+					     spinlock_t * irq_mode_lock)
+{
+	me8200_di_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me8200_di_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me8200_di_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Check firmware version.
+	me8200_di_check_version(subdevice,
+				me8200_regbase + ME8200_FIRMWARE_VERSION_REG);
+
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	subdevice->irq_ctrl_lock = irq_ctrl_lock;
+	subdevice->irq_mode_lock = irq_mode_lock;
+
+	/* Save the subdevice index. */
+	subdevice->di_idx = di_idx;
+
+	/* Initialize registers */
+	if (di_idx == 0) {
+		subdevice->port_reg = me8200_regbase + ME8200_DI_PORT_0_REG;
+		subdevice->mask_reg = me8200_regbase + ME8200_DI_MASK_0_REG;
+		subdevice->compare_reg =
+		    me8200_regbase + ME8200_DI_COMPARE_0_REG;
+		subdevice->irq_status_reg =
+		    me8200_regbase + ME8200_DI_CHANGE_0_REG;
+
+		subdevice->irq_status_low_reg =
+		    me8200_regbase + ME8200_DI_EXTEND_CHANGE_0_LOW_REG;
+		subdevice->irq_status_high_reg =
+		    me8200_regbase + ME8200_DI_EXTEND_CHANGE_0_HIGH_REG;
+	} else if (di_idx == 1) {
+		subdevice->port_reg = me8200_regbase + ME8200_DI_PORT_1_REG;
+		subdevice->mask_reg = me8200_regbase + ME8200_DI_MASK_1_REG;
+		subdevice->compare_reg =
+		    me8200_regbase + ME8200_DI_COMPARE_1_REG;
+		subdevice->irq_status_reg =
+		    me8200_regbase + ME8200_DI_CHANGE_1_REG;
+
+		subdevice->irq_status_low_reg =
+		    me8200_regbase + ME8200_DI_EXTEND_CHANGE_1_LOW_REG;
+		subdevice->irq_status_high_reg =
+		    me8200_regbase + ME8200_DI_EXTEND_CHANGE_1_HIGH_REG;
+	} else {
+		PERROR("Wrong subdevice idx=%d.\n", di_idx);
+		kfree(subdevice);
+		return NULL;
+	}
+	subdevice->irq_ctrl_reg = me8200_regbase + ME8200_DI_IRQ_CTRL_REG;
+	subdevice->irq_mode_reg = me8200_regbase + ME8200_IRQ_MODE_REG;
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = me8200_regbase;
+#endif
+
+	/* Initialize wait queue */
+	init_waitqueue_head(&subdevice->wait_queue);
+
+	/* Overload base class methods. */
+	subdevice->base.me_subdevice_io_irq_start = me8200_di_io_irq_start;
+	subdevice->base.me_subdevice_io_irq_wait = me8200_di_io_irq_wait;
+	subdevice->base.me_subdevice_io_irq_stop = me8200_di_io_irq_stop;
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me8200_di_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me8200_di_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me8200_di_io_single_read;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me8200_di_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me8200_di_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me8200_di_query_subdevice_caps;
+	subdevice->base.me_subdevice_destructor = me8200_di_destructor;
+
+	subdevice->rised = 0;
+	subdevice->count = 0;
+
+	/* Register interrupt service routine. */
+	subdevice->irq = irq;
+	if (subdevice->version > 0) {	// NEW
+		err = request_irq(subdevice->irq, me8200_isr_EX,
+#ifdef IRQF_DISABLED
+				  IRQF_DISABLED | IRQF_SHARED,
+#else
+				  SA_INTERRUPT | SA_SHIRQ,
+#endif
+				  ME8200_NAME, (void *)subdevice);
+	} else {		//OLD
+		err = request_irq(subdevice->irq, me8200_isr,
+#ifdef IRQF_DISABLED
+				  IRQF_DISABLED | IRQF_SHARED,
+#else
+				  SA_INTERRUPT | SA_SHIRQ,
+#endif
+				  ME8200_NAME, (void *)subdevice);
+	}
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	PDEBUG("Registred irq=%d.\n", subdevice->irq);
+
+	return subdevice;
+}
+
+static void me8200_di_check_version(me8200_di_subdevice_t * instance,
+				    unsigned long addr)
+{
+
+	PDEBUG("executed.\n");
+	instance->version = 0x000000FF & inb(addr);
+	PDEVELOP("me8200 firmware version: %d\n", instance->version);
+
+	/// @note Fix for wrong values in this registry.
+	if ((instance->version < 0x7) || (instance->version > 0x1F))
+		instance->version = 0x0;
+}
diff --git a/drivers/staging/meilhaus/me8200_di.h b/drivers/staging/meilhaus/me8200_di.h
new file mode 100644
index 00000000000..2a3b005b67d
--- /dev/null
+++ b/drivers/staging/meilhaus/me8200_di.h
@@ -0,0 +1,92 @@
+/**
+ * @file me8200_di.h
+ *
+ * @brief ME-8200 digital input subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME8200_DI_H_
+#define _ME8200_DI_H_
+
+#include "mesubdevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The template subdevice class.
+ */
+typedef struct me8200_di_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *ctrl_reg_lock;
+	spinlock_t *irq_ctrl_lock;
+	spinlock_t *irq_mode_lock;
+
+	unsigned int di_idx;
+	unsigned int version;
+
+	int irq;						/**< The number of the interrupt request. */
+	volatile int rised;				/**< Flag to indicate if an interrupt occured. */
+	uint status_flag;				/**< Default interupt status flag */
+	uint status_value;				/**< Interupt status */
+	uint status_value_edges;			/**< Extended interupt status */
+	uint line_value;
+	int count;						/**< Counts the number of interrupts occured. */
+	uint8_t compare_value;
+	uint8_t filtering_flag;
+
+	wait_queue_head_t wait_queue;	/**< To wait on interrupts. */
+
+	unsigned long port_reg;			/**< The digital input port. */
+	unsigned long compare_reg;		/**< The register to hold the value to compare with. */
+	unsigned long mask_reg;			/**< The register to hold the mask. */
+	unsigned long irq_mode_reg;		/**< The interrupt mode register. */
+	unsigned long irq_ctrl_reg;		/**< The interrupt control register. */
+	unsigned long irq_status_reg;	/**< The interrupt status register. Also interrupt reseting register (firmware version 7 and later).*/
+#ifdef MEDEBUG_DEBUG_REG
+	unsigned long reg_base;
+#endif
+	unsigned long firmware_version_reg;	/**< The interrupt reseting register. */
+
+	unsigned long irq_status_low_reg;	/**< The interrupt extended status register (low part). */
+	unsigned long irq_status_high_reg;	/**< The interrupt extended status register (high part). */
+} me8200_di_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-8200 digital input subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me8200_di_subdevice_t *me8200_di_constructor(uint32_t me8200_reg_base,
+					     unsigned int di_idx,
+					     int irq,
+					     spinlock_t * irq_ctrl_lock,
+					     spinlock_t * irq_mode_lock);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me8200_di_reg.h b/drivers/staging/meilhaus/me8200_di_reg.h
new file mode 100644
index 00000000000..b9a619d31c2
--- /dev/null
+++ b/drivers/staging/meilhaus/me8200_di_reg.h
@@ -0,0 +1,75 @@
+/**
+ * @file me8200_di_reg.h
+ *
+ * @brief ME-8200 digital input subdevice register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME8200_DI_REG_H_
+#define _ME8200_DI_REG_H_
+
+#ifdef __KERNEL__
+
+// Common registry for whole family.
+#define ME8200_DI_PORT_0_REG					0x3	// R
+#define ME8200_DI_PORT_1_REG					0x4	// R
+
+#define ME8200_DI_MASK_0_REG					0x5	// R/W
+#define ME8200_DI_MASK_1_REG					0x6	// R/W
+
+#define ME8200_DI_COMPARE_0_REG					0xA	// R/W
+#define ME8200_DI_COMPARE_1_REG					0xB	// R/W
+
+#define ME8200_DI_IRQ_CTRL_REG					0xC	// R/W
+
+#ifndef ME8200_IRQ_MODE_REG
+# define ME8200_IRQ_MODE_REG					0xD	// R/W
+#endif
+
+// This registry are for all versions
+#define ME8200_DI_CHANGE_0_REG					0xE	// R
+#define ME8200_DI_CHANGE_1_REG					0xF	// R
+
+#define ME8200_DI_IRQ_CTRL_BIT_CLEAR			0x4
+#define ME8200_DI_IRQ_CTRL_BIT_ENABLE			0x8
+
+// This registry are for firmware versions 7 and later
+#define ME8200_DI_EXTEND_CHANGE_0_LOW_REG		0x10	// R
+#define ME8200_DI_EXTEND_CHANGE_0_HIGH_REG		0x11	// R
+#define ME8200_DI_EXTEND_CHANGE_1_LOW_REG		0x12	// R
+#define ME8200_DI_EXTEND_CHANGE_1_HIGH_REG		0x13	// R
+
+#ifndef ME8200_FIRMWARE_VERSION_REG
+# define ME8200_FIRMWARE_VERSION_REG			0x14	// R
+#endif
+
+// Bit definitions
+#define ME8200_DI_IRQ_CTRL_MASK_EDGE			0x3
+#define ME8200_DI_IRQ_CTRL_MASK_EDGE_RISING		0x0
+#define ME8200_DI_IRQ_CTRL_MASK_EDGE_FALLING	0x1
+#define ME8200_DI_IRQ_CTRL_MASK_EDGE_ANY		0x3
+
+// Others
+#define ME8200_DI_IRQ_CTRL_SHIFT				4
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me8200_dio.c b/drivers/staging/meilhaus/me8200_dio.c
new file mode 100644
index 00000000000..ff8ca1b8b7f
--- /dev/null
+++ b/drivers/staging/meilhaus/me8200_dio.c
@@ -0,0 +1,418 @@
+/**
+ * @file me8200_dio.c
+ *
+ * @brief ME-8200 digital input/output subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "me8200_dio_reg.h"
+#include "me8200_dio.h"
+
+/*
+ * Defines
+ */
+
+/*
+ * Functions
+ */
+
+static int me8200_dio_io_reset_subdevice(struct me_subdevice *subdevice,
+					 struct file *filep, int flags)
+{
+	me8200_dio_subdevice_t *instance;
+	uint8_t mode;
+
+	PDEBUG("executed.\n");
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	instance = (me8200_dio_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	mode = inb(instance->ctrl_reg);
+	mode &= ~(0x3 << (instance->dio_idx * 2));
+	outb(mode, instance->ctrl_reg);
+	PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->ctrl_reg - instance->reg_base, mode);
+	spin_unlock(instance->ctrl_reg_lock);
+	outb(0x00, instance->port_reg);
+	PDEBUG_REG("port_reg outl(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->port_reg - instance->reg_base, 0x00);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8200_dio_io_single_config(me_subdevice_t * subdevice,
+				       struct file *filep,
+				       int channel,
+				       int single_config,
+				       int ref,
+				       int trig_chan,
+				       int trig_type, int trig_edge, int flags)
+{
+	me8200_dio_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint32_t mode;
+	uint32_t size =
+	    flags & (ME_IO_SINGLE_CONFIG_DIO_BIT | ME_IO_SINGLE_CONFIG_DIO_BYTE
+		     | ME_IO_SINGLE_CONFIG_DIO_WORD |
+		     ME_IO_SINGLE_CONFIG_DIO_DWORD);
+
+	PDEBUG("executed.\n");
+
+	instance = (me8200_dio_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	mode = inb(instance->ctrl_reg);
+	switch (size) {
+	case ME_IO_SINGLE_CONFIG_NO_FLAGS:
+	case ME_IO_SINGLE_CONFIG_DIO_BYTE:
+		if (channel == 0) {
+			if (single_config == ME_SINGLE_CONFIG_DIO_INPUT) {
+				mode &=
+				    ~((ME8200_DIO_CTRL_BIT_MODE_0 |
+				       ME8200_DIO_CTRL_BIT_MODE_1) <<
+				      (instance->dio_idx * 2));
+			} else if (single_config == ME_SINGLE_CONFIG_DIO_OUTPUT) {
+				mode &=
+				    ~((ME8200_DIO_CTRL_BIT_MODE_0 |
+				       ME8200_DIO_CTRL_BIT_MODE_1) <<
+				      (instance->dio_idx * 2));
+				mode |=
+				    ME8200_DIO_CTRL_BIT_MODE_0 << (instance->
+								   dio_idx * 2);
+			} else {
+				PERROR
+				    ("Invalid port configuration specified.\n");
+				err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		} else {
+			PERROR("Invalid channel number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid flags.\n");
+
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (!err) {
+		outb(mode, instance->ctrl_reg);
+		PDEBUG_REG("ctrl_reg outl(0x%lX+0x%lX)=0x%x\n",
+			   instance->reg_base,
+			   instance->ctrl_reg - instance->reg_base, mode);
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8200_dio_io_single_read(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int channel,
+				     int *value, int time_out, int flags)
+{
+	me8200_dio_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint8_t mode;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8200_dio_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			mode =
+			    inb(instance->
+				ctrl_reg) & ((ME8200_DIO_CTRL_BIT_MODE_0 |
+					      ME8200_DIO_CTRL_BIT_MODE_1) <<
+					     (instance->dio_idx * 2));
+
+			if ((mode ==
+			     (ME8200_DIO_CTRL_BIT_MODE_0 <<
+			      (instance->dio_idx * 2))) || !mode) {
+				*value =
+				    inb(instance->
+					port_reg) & (0x0001 << channel);
+			} else {
+				PERROR("Port not in output or input mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			mode =
+			    inb(instance->
+				ctrl_reg) & ((ME8200_DIO_CTRL_BIT_MODE_0 |
+					      ME8200_DIO_CTRL_BIT_MODE_1) <<
+					     (instance->dio_idx * 2));
+
+			if ((mode ==
+			     (ME8200_DIO_CTRL_BIT_MODE_0 <<
+			      (instance->dio_idx * 2))) || !mode) {
+				*value = inb(instance->port_reg) & 0x00FF;
+			} else {
+				PERROR("Port not in output or input mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8200_dio_io_single_write(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int value, int time_out, int flags)
+{
+	me8200_dio_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint8_t mode;
+	uint8_t byte;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8200_dio_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			mode =
+			    inb(instance->
+				ctrl_reg) & ((ME8200_DIO_CTRL_BIT_MODE_0 |
+					      ME8200_DIO_CTRL_BIT_MODE_1) <<
+					     (instance->dio_idx * 2));
+
+			if (mode ==
+			    (ME8200_DIO_CTRL_BIT_MODE_0 <<
+			     (instance->dio_idx * 2))) {
+				byte = inb(instance->port_reg);
+
+				if (value)
+					byte |= 0x1 << channel;
+				else
+					byte &= ~(0x1 << channel);
+
+				outb(byte, instance->port_reg);
+				PDEBUG_REG("port_reg outl(0x%lX+0x%lX)=0x%x\n",
+					   instance->reg_base,
+					   instance->port_reg -
+					   instance->reg_base, byte);
+			} else {
+				PERROR("Port not in output or input mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			mode =
+			    inb(instance->
+				ctrl_reg) & ((ME8200_DIO_CTRL_BIT_MODE_0 |
+					      ME8200_DIO_CTRL_BIT_MODE_1) <<
+					     (instance->dio_idx * 2));
+
+			if (mode ==
+			    (ME8200_DIO_CTRL_BIT_MODE_0 <<
+			     (instance->dio_idx * 2))) {
+				outb(value, instance->port_reg);
+				PDEBUG_REG("port_reg outl(0x%lX+0x%lX)=0x%x\n",
+					   instance->reg_base,
+					   instance->port_reg -
+					   instance->reg_base, value);
+			} else {
+				PERROR("Port not in output or input mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock(instance->ctrl_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8200_dio_query_number_channels(me_subdevice_t * subdevice,
+					    int *number)
+{
+	PDEBUG("executed.\n");
+	*number = 8;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8200_dio_query_subdevice_type(me_subdevice_t * subdevice,
+					   int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_DIO;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8200_dio_query_subdevice_caps(me_subdevice_t * subdevice,
+					   int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = ME_CAPS_DIO_DIR_BYTE;
+	return ME_ERRNO_SUCCESS;
+}
+
+me8200_dio_subdevice_t *me8200_dio_constructor(uint32_t reg_base,
+					       unsigned int dio_idx,
+					       spinlock_t * ctrl_reg_lock)
+{
+	me8200_dio_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me8200_dio_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me8200_dio_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	subdevice->ctrl_reg_lock = ctrl_reg_lock;
+
+	/* Save digital i/o index */
+	subdevice->dio_idx = dio_idx;
+
+	/* Save the subdevice index */
+	subdevice->ctrl_reg = reg_base + ME8200_DIO_CTRL_REG;
+	subdevice->port_reg = reg_base + ME8200_DIO_PORT_REG + dio_idx;
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = reg_base;
+#endif
+
+	/* Overload base class methods. */
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me8200_dio_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me8200_dio_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me8200_dio_io_single_read;
+	subdevice->base.me_subdevice_io_single_write =
+	    me8200_dio_io_single_write;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me8200_dio_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me8200_dio_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me8200_dio_query_subdevice_caps;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me8200_dio.h b/drivers/staging/meilhaus/me8200_dio.h
new file mode 100644
index 00000000000..9ddd93d26f1
--- /dev/null
+++ b/drivers/staging/meilhaus/me8200_dio.h
@@ -0,0 +1,68 @@
+/**
+ * @file me8200_dio.h
+ *
+ * @brief ME-8200 digital input/output subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME8200_DIO_H_
+#define _ME8200_DIO_H_
+
+#include "mesubdevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The template subdevice class.
+ */
+typedef struct me8200_dio_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *ctrl_reg_lock;		/**< Spin lock to protect #ctrl_reg from concurrent access. */
+	unsigned int dio_idx;			/**< The index of the digital i/o on the device. */
+
+	unsigned long port_reg;			/**< Register holding the port status. */
+	unsigned long ctrl_reg;			/**< Register to configure the port direction. */
+#ifdef MEDEBUG_DEBUG_REG
+	unsigned long reg_base;
+#endif
+} me8200_dio_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-8200 digital input/ouput subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ * @param dio_idx The index of the digital i/o port on the device.
+ * @param ctrl_reg_lock Spin lock protecting the control register.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me8200_dio_subdevice_t *me8200_dio_constructor(uint32_t reg_base,
+					       unsigned int dio_idx,
+					       spinlock_t * ctrl_reg_lock);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me8200_dio_reg.h b/drivers/staging/meilhaus/me8200_dio_reg.h
new file mode 100644
index 00000000000..ac94a133aba
--- /dev/null
+++ b/drivers/staging/meilhaus/me8200_dio_reg.h
@@ -0,0 +1,43 @@
+/**
+ * @file me8200_dio_reg.h
+ *
+ * @brief ME-8200 digital input/output subdevice register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME8200_DIO_REG_H_
+#define _ME8200_DIO_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME8200_DIO_CTRL_REG					0x7	// R/W
+#define ME8200_DIO_PORT_0_REG				0x8	// R/W
+#define ME8200_DIO_PORT_1_REG				0x9	// R/W
+#define ME8200_DIO_PORT_REG					ME8200_DIO_PORT_0_REG	// R/W
+
+#define ME8200_DIO_CTRL_BIT_MODE_0			0x01
+#define ME8200_DIO_CTRL_BIT_MODE_1			0x02
+#define ME8200_DIO_CTRL_BIT_MODE_2			0x04
+#define ME8200_DIO_CTRL_BIT_MODE_3			0x08
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me8200_do.c b/drivers/staging/meilhaus/me8200_do.c
new file mode 100644
index 00000000000..5f4ba5dfa86
--- /dev/null
+++ b/drivers/staging/meilhaus/me8200_do.c
@@ -0,0 +1,600 @@
+/**
+ * @file me8200_do.c
+ *
+ * @brief ME-8200 digital output subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/interrupt.h>
+#include <asm/io.h>
+#include <linux/types.h>
+#include <linux/version.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+
+#include "meids.h"
+#include "medebug.h"
+#include "me8200_reg.h"
+#include "me8200_do_reg.h"
+#include "me8200_do.h"
+
+/*
+ * Defines
+ */
+
+/*
+ * Functions
+ */
+
+static int me8200_do_io_irq_start(me_subdevice_t * subdevice,
+				  struct file *filep,
+				  int channel,
+				  int irq_source,
+				  int irq_edge, int irq_arg, int flags)
+{
+	me8200_do_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint8_t tmp;
+	unsigned long status;
+
+	if (flags & ~ME_IO_IRQ_START_DIO_BYTE) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (channel != 0) {
+		PERROR("Invalid channel specified.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	if (irq_source != ME_IRQ_SOURCE_DIO_OVER_TEMP) {
+		PERROR("Invalid interrupt source specified.\n");
+		return ME_ERRNO_INVALID_IRQ_SOURCE;
+	}
+
+	PDEBUG("executed.\n");
+
+	instance = (me8200_do_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, status);
+	spin_lock(instance->irq_mode_lock);
+	tmp = inb(instance->irq_ctrl_reg);
+	tmp |=
+	    ME8200_IRQ_MODE_BIT_ENABLE_POWER << (ME8200_IRQ_MODE_POWER_SHIFT *
+						 instance->do_idx);
+	outb(tmp, instance->irq_ctrl_reg);
+	PDEBUG_REG("irq_ctrl_reg outb(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->irq_ctrl_reg - instance->reg_base, tmp);
+	spin_unlock(instance->irq_mode_lock);
+	instance->rised = 0;
+	spin_unlock_irqrestore(&instance->subdevice_lock, status);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8200_do_io_irq_wait(me_subdevice_t * subdevice,
+				 struct file *filep,
+				 int channel,
+				 int *irq_count,
+				 int *value, int time_out, int flags)
+{
+	me8200_do_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	long t = 0;
+	unsigned long cpu_flags;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8200_do_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (time_out < 0) {
+		PERROR("Invalid time_out specified.\n");
+		return ME_ERRNO_INVALID_TIMEOUT;
+	}
+
+	if (time_out) {
+		t = (time_out * HZ) / 1000;
+
+		if (t == 0)
+			t = 1;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	if (instance->rised <= 0) {
+		instance->rised = 0;
+
+		if (time_out) {
+			t = wait_event_interruptible_timeout(instance->
+							     wait_queue,
+							     (instance->rised !=
+							      0), t);
+
+			if (t == 0) {
+				PERROR
+				    ("Wait on external interrupt timed out.\n");
+				err = ME_ERRNO_TIMEOUT;
+			}
+		} else {
+			wait_event_interruptible(instance->wait_queue,
+						 (instance->rised != 0));
+		}
+
+		if (instance->rised < 0) {
+			PERROR("Wait on interrupt aborted by user.\n");
+			err = ME_ERRNO_CANCELLED;
+		}
+	}
+
+	if (signal_pending(current)) {
+		PERROR("Wait on external interrupt aborted by signal.\n");
+		err = ME_ERRNO_SIGNAL;
+	}
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	instance->rised = 0;
+	*irq_count = instance->count;
+	*value = 0;
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8200_do_io_irq_stop(me_subdevice_t * subdevice,
+				 struct file *filep, int channel, int flags)
+{
+	me8200_do_subdevice_t *instance;
+	uint8_t tmp;
+	unsigned long cpu_flags;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8200_do_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	spin_lock(instance->irq_mode_lock);
+	tmp = inb(instance->irq_ctrl_reg);
+	tmp &=
+	    ~(ME8200_IRQ_MODE_BIT_ENABLE_POWER <<
+	      (ME8200_IRQ_MODE_POWER_SHIFT * instance->do_idx));
+	outb(tmp, instance->irq_ctrl_reg);
+	PDEBUG_REG("irq_ctrl_reg outb(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->irq_ctrl_reg - instance->reg_base, tmp);
+	spin_unlock(instance->irq_mode_lock);
+	instance->rised = -1;
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8200_do_io_reset_subdevice(struct me_subdevice *subdevice,
+					struct file *filep, int flags)
+{
+	me8200_do_subdevice_t *instance;
+	unsigned long cpu_flags;
+	uint8_t tmp;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8200_do_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, cpu_flags);
+	outb(0x00, instance->port_reg);
+	PDEBUG_REG("port_reg outb(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->port_reg - instance->reg_base, 0x00);
+	spin_lock(instance->irq_mode_lock);
+	tmp = inb(instance->irq_ctrl_reg);
+	tmp &=
+	    ~(ME8200_IRQ_MODE_BIT_ENABLE_POWER <<
+	      (ME8200_IRQ_MODE_POWER_SHIFT * instance->do_idx));
+	outb(tmp, instance->irq_ctrl_reg);
+	PDEBUG_REG("irq_ctrl_reg outb(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->irq_ctrl_reg - instance->reg_base, tmp);
+	spin_unlock(instance->irq_mode_lock);
+	instance->rised = -1;
+	instance->count = 0;
+	spin_unlock_irqrestore(&instance->subdevice_lock, cpu_flags);
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8200_do_io_single_config(me_subdevice_t * subdevice,
+				      struct file *filep,
+				      int channel,
+				      int single_config,
+				      int ref,
+				      int trig_chan,
+				      int trig_type, int trig_edge, int flags)
+{
+	me8200_do_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long status;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8200_do_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, status);
+	switch (flags) {
+	case ME_IO_SINGLE_CONFIG_NO_FLAGS:
+	case ME_IO_SINGLE_CONFIG_DIO_BYTE:
+		if (channel == 0) {
+			if (single_config == ME_SINGLE_CONFIG_DIO_OUTPUT) {
+			} else {
+				PERROR("Invalid byte direction specified.\n");
+				err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		} else {
+			PERROR("Invalid byte specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock_irqrestore(&instance->subdevice_lock, status);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8200_do_io_single_read(me_subdevice_t * subdevice,
+				    struct file *filep,
+				    int channel,
+				    int *value, int time_out, int flags)
+{
+	me8200_do_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	unsigned long status;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8200_do_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, status);
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			*value = inb(instance->port_reg) & (0x1 << channel);
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			*value = inb(instance->port_reg);
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock_irqrestore(&instance->subdevice_lock, status);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8200_do_io_single_write(me_subdevice_t * subdevice,
+				     struct file *filep,
+				     int channel,
+				     int value, int time_out, int flags)
+{
+	me8200_do_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+	uint8_t state;
+	unsigned long status;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8200_do_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock_irqsave(&instance->subdevice_lock, status);
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			state = inb(instance->port_reg);
+			state =
+			    value ? (state | (0x1 << channel)) : (state &
+								  ~(0x1 <<
+								    channel));
+			outb(state, instance->port_reg);
+			PDEBUG_REG("port_reg outb(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->port_reg - instance->reg_base,
+				   state);
+		} else {
+			PERROR("Invalid bit number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			outb(value, instance->port_reg);
+			PDEBUG_REG("port_reg outb(0x%lX+0x%lX)=0x%x\n",
+				   instance->reg_base,
+				   instance->port_reg - instance->reg_base,
+				   value);
+		} else {
+			PERROR("Invalid byte number specified.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock_irqrestore(&instance->subdevice_lock, status);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8200_do_query_number_channels(me_subdevice_t * subdevice,
+					   int *number)
+{
+	PDEBUG("executed.\n");
+	*number = 8;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8200_do_query_subdevice_type(me_subdevice_t * subdevice,
+					  int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_DO;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8200_do_query_subdevice_caps(me_subdevice_t * subdevice, int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = ME_CAPS_DIO_OVER_TEMP_IRQ;
+	return ME_ERRNO_SUCCESS;
+}
+
+static void me8200_do_destructor(struct me_subdevice *subdevice)
+{
+	me8200_do_subdevice_t *instance;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8200_do_subdevice_t *) subdevice;
+
+	free_irq(instance->irq, (void *)instance);
+	me_subdevice_deinit(&instance->base);
+	kfree(instance);
+}
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 19)
+static irqreturn_t me8200_do_isr(int irq, void *dev_id)
+#else
+static irqreturn_t me8200_do_isr(int irq, void *dev_id, struct pt_regs *regs)
+#endif
+{
+	me8200_do_subdevice_t *instance;
+	uint16_t ctrl;
+	uint8_t irq_status;
+
+	instance = (me8200_do_subdevice_t *) dev_id;
+
+	if (irq != instance->irq) {
+		PERROR("Incorrect interrupt num: %d.\n", irq);
+		return IRQ_NONE;
+	}
+
+	irq_status = inb(instance->irq_status_reg);
+	if (!
+	    (irq_status &
+	     (ME8200_DO_IRQ_STATUS_BIT_ACTIVE << instance->do_idx))) {
+		PINFO
+		    ("%ld Shared interrupt. %s(): idx=%d irq_status_reg=0x%04X\n",
+		     jiffies, __FUNCTION__, instance->do_idx, irq_status);
+		return IRQ_NONE;
+	}
+
+	PDEBUG("executed.\n");
+
+	spin_lock(&instance->subdevice_lock);
+	instance->rised = 1;
+	instance->count++;
+
+	spin_lock(instance->irq_mode_lock);
+	ctrl = inw(instance->irq_ctrl_reg);
+	ctrl |= ME8200_IRQ_MODE_BIT_CLEAR_POWER << instance->do_idx;
+	outw(ctrl, instance->irq_ctrl_reg);
+	PDEBUG_REG("irq_ctrl_reg outw(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->irq_ctrl_reg - instance->reg_base, ctrl);
+	ctrl &= ~(ME8200_IRQ_MODE_BIT_CLEAR_POWER << instance->do_idx);
+	outw(ctrl, instance->irq_ctrl_reg);
+	PDEBUG_REG("irq_ctrl_reg outw(0x%lX+0x%lX)=0x%x\n", instance->reg_base,
+		   instance->irq_ctrl_reg - instance->reg_base, ctrl);
+	spin_unlock(instance->irq_mode_lock);
+	spin_unlock(&instance->subdevice_lock);
+	wake_up_interruptible_all(&instance->wait_queue);
+
+	return IRQ_HANDLED;
+}
+
+me8200_do_subdevice_t *me8200_do_constructor(uint32_t reg_base,
+					     unsigned int do_idx,
+					     int irq,
+					     spinlock_t * irq_mode_lock)
+{
+	me8200_do_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me8200_do_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me8200_do_subdevice_t));
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	subdevice->irq_mode_lock = irq_mode_lock;
+
+	/* Save the index of the digital output */
+	subdevice->do_idx = do_idx;
+	subdevice->irq = irq;
+
+	/* Initialize the registers */
+	if (do_idx == 0) {
+		subdevice->port_reg = reg_base + ME8200_DO_PORT_0_REG;
+	} else if (do_idx == 1) {
+		subdevice->port_reg = reg_base + ME8200_DO_PORT_1_REG;
+	} else {
+		PERROR("Wrong subdevice idx=%d.\n", do_idx);
+		kfree(subdevice);
+		return NULL;
+	}
+	subdevice->irq_ctrl_reg = reg_base + ME8200_IRQ_MODE_REG;
+	subdevice->irq_status_reg = reg_base + ME8200_DO_IRQ_STATUS_REG;
+#ifdef MEDEBUG_DEBUG_REG
+	subdevice->reg_base = reg_base;
+#endif
+
+	/* Initialize the wait queue */
+	init_waitqueue_head(&subdevice->wait_queue);
+
+	/* Request the interrupt line */
+	err = request_irq(irq, me8200_do_isr,
+#ifdef IRQF_DISABLED
+			  IRQF_DISABLED | IRQF_SHARED,
+#else
+			  SA_INTERRUPT | SA_SHIRQ,
+#endif
+			  ME8200_NAME, (void *)subdevice);
+
+	if (err) {
+		PERROR("Cannot get interrupt line.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	PINFO("Registered irq=%d.\n", irq);
+
+	/* Overload base class methods. */
+	subdevice->base.me_subdevice_io_irq_start = me8200_do_io_irq_start;
+	subdevice->base.me_subdevice_io_irq_wait = me8200_do_io_irq_wait;
+	subdevice->base.me_subdevice_io_irq_stop = me8200_do_io_irq_stop;
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me8200_do_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config =
+	    me8200_do_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me8200_do_io_single_read;
+	subdevice->base.me_subdevice_io_single_write =
+	    me8200_do_io_single_write;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me8200_do_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me8200_do_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me8200_do_query_subdevice_caps;
+	subdevice->base.me_subdevice_destructor = me8200_do_destructor;
+
+	subdevice->rised = 0;
+	subdevice->count = 0;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me8200_do.h b/drivers/staging/meilhaus/me8200_do.h
new file mode 100644
index 00000000000..27581251c84
--- /dev/null
+++ b/drivers/staging/meilhaus/me8200_do.h
@@ -0,0 +1,75 @@
+/**
+ * @file me8200_do.h
+ *
+ * @brief ME-8200 digital output subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME8200_DO_H_
+#define _ME8200_DO_H_
+
+#include "mesubdevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The template subdevice class.
+ */
+typedef struct me8200_do_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *irq_mode_lock;
+
+	int irq;				/**< The number of the interrupt request */
+	int rised;				/**< Flag to indicate if an interrupt occured */
+	int count;				/**< Counts the number of interrupts occured */
+	wait_queue_head_t wait_queue;		/**< To wait on interrupts */
+
+	unsigned int do_idx;			/**< The number of the digital output */
+
+	unsigned long port_reg;			/**< The digital output port */
+	unsigned long irq_ctrl_reg;		/**< The interrupt control register */
+	unsigned long irq_status_reg;		/**< The interrupt status register */
+#ifdef MEDEBUG_DEBUG_REG
+	unsigned long reg_base;
+#endif
+} me8200_do_subdevice_t;
+
+/**
+ * @brief The constructor to generate a ME-8200 digital output subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ * @param do_idx The index of the digital output subdevice on this device.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me8200_do_subdevice_t *me8200_do_constructor(uint32_t reg_base,
+					     unsigned int do_idx,
+					     int irq,
+					     spinlock_t * irq_mode_lock);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me8200_do_reg.h b/drivers/staging/meilhaus/me8200_do_reg.h
new file mode 100644
index 00000000000..41095046037
--- /dev/null
+++ b/drivers/staging/meilhaus/me8200_do_reg.h
@@ -0,0 +1,40 @@
+/**
+ * @file me8200_ao_reg.h
+ *
+ * @brief ME-8200 analog output subdevice register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME8200_DO_REG_H_
+#define _ME8200_DO_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME8200_DO_IRQ_STATUS_REG			0x0	// R
+#define ME8200_DO_PORT_0_REG				0x1	// R/W
+#define ME8200_DO_PORT_1_REG				0x2	// R/W
+
+#define ME8200_DO_IRQ_STATUS_BIT_ACTIVE		0x1
+#define ME8200_DO_IRQ_STATUS_SHIFT			1
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me8200_reg.h b/drivers/staging/meilhaus/me8200_reg.h
new file mode 100644
index 00000000000..a73fe4d5b0f
--- /dev/null
+++ b/drivers/staging/meilhaus/me8200_reg.h
@@ -0,0 +1,46 @@
+/**
+ * @file me8200_reg.h
+ *
+ * @brief ME-8200 register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME8200_REG_H_
+#define _ME8200_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME8200_IRQ_MODE_REG				0xD	// R/W
+
+#define ME8200_IRQ_MODE_MASK     			0x3
+
+#define ME8200_IRQ_MODE_MASK_MASK			0x0
+#define ME8200_IRQ_MODE_MASK_COMPARE			0x1
+
+#define ME8200_IRQ_MODE_BIT_ENABLE_POWER		0x10
+#define ME8200_IRQ_MODE_BIT_CLEAR_POWER			0x40
+
+#define ME8200_IRQ_MODE_DI_SHIFT			2
+#define ME8200_IRQ_MODE_POWER_SHIFT			1
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me8254.c b/drivers/staging/meilhaus/me8254.c
new file mode 100644
index 00000000000..6e44c3d7a0c
--- /dev/null
+++ b/drivers/staging/meilhaus/me8254.c
@@ -0,0 +1,1176 @@
+/**
+ * @file me8254.c
+ *
+ * @brief 8254 subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "me8254_reg.h"
+#include "me8254.h"
+
+/*
+ * Defines
+ */
+#define ME8254_NUMBER_CHANNELS 1	/**< One channel per counter. */
+#define ME8254_CTR_WIDTH 16			/**< One counter has 16 bits. */
+
+/*
+ * Functions
+ */
+
+static int me8254_io_reset_subdevice(struct me_subdevice *subdevice,
+				     struct file *filep, int flags)
+{
+	me8254_subdevice_t *instance;
+	uint8_t clk_src;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8254_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	if (instance->ctr_idx == 0)
+		outb(ME8254_CTRL_SC0 | ME8254_CTRL_LM | ME8254_CTRL_M0 |
+		     ME8254_CTRL_BIN, instance->ctrl_reg);
+	else if (instance->ctr_idx == 1)
+		outb(ME8254_CTRL_SC1 | ME8254_CTRL_LM | ME8254_CTRL_M0 |
+		     ME8254_CTRL_BIN, instance->ctrl_reg);
+	else
+		outb(ME8254_CTRL_SC2 | ME8254_CTRL_LM | ME8254_CTRL_M0 |
+		     ME8254_CTRL_BIN, instance->ctrl_reg);
+	spin_unlock(instance->ctrl_reg_lock);
+
+	outb(0x00, instance->val_reg);
+	outb(0x00, instance->val_reg);
+
+	spin_lock(instance->clk_src_reg_lock);
+	clk_src = inb(instance->clk_src_reg);
+
+	switch (instance->device_id) {
+	case PCI_DEVICE_ID_MEILHAUS_ME1400:
+	case PCI_DEVICE_ID_MEILHAUS_ME140A:
+	case PCI_DEVICE_ID_MEILHAUS_ME140B:
+	case PCI_DEVICE_ID_MEILHAUS_ME14E0:
+	case PCI_DEVICE_ID_MEILHAUS_ME14EA:
+	case PCI_DEVICE_ID_MEILHAUS_ME14EB:
+		if (instance->me8254_idx == 0) {
+			if (instance->ctr_idx == 0)
+				clk_src &=
+				    ~(ME1400AB_8254_A_0_CLK_SRC_10MHZ |
+				      ME1400AB_8254_A_0_CLK_SRC_QUARZ);
+			else if (instance->ctr_idx == 1)
+				clk_src &= ~(ME1400AB_8254_A_1_CLK_SRC_PREV);
+			else
+				clk_src &= ~(ME1400AB_8254_A_2_CLK_SRC_PREV);
+		} else {
+			if (instance->ctr_idx == 0)
+				clk_src &=
+				    ~(ME1400AB_8254_B_0_CLK_SRC_10MHZ |
+				      ME1400AB_8254_B_0_CLK_SRC_QUARZ);
+			else if (instance->ctr_idx == 1)
+				clk_src &= ~(ME1400AB_8254_B_1_CLK_SRC_PREV);
+			else
+				clk_src &= ~(ME1400AB_8254_B_2_CLK_SRC_PREV);
+		}
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140C:
+	case PCI_DEVICE_ID_MEILHAUS_ME140D:
+		switch (instance->me8254_idx) {
+		case 0:
+		case 2:
+		case 4:
+		case 6:
+		case 8:
+			if (instance->ctr_idx == 0)
+				clk_src &= ~(ME1400CD_8254_ACE_0_CLK_SRC_MASK);
+			else if (instance->ctr_idx == 1)
+				clk_src &= ~(ME1400CD_8254_ACE_1_CLK_SRC_MASK);
+			else
+				clk_src &= ~(ME1400CD_8254_ACE_2_CLK_SRC_MASK);
+			break;
+
+		default:
+			if (instance->ctr_idx == 0)
+				clk_src &= ~(ME1400CD_8254_BD_0_CLK_SRC_MASK);
+			else if (instance->ctr_idx == 1)
+				clk_src &= ~(ME1400CD_8254_BD_1_CLK_SRC_MASK);
+			else
+				clk_src &= ~(ME1400CD_8254_BD_2_CLK_SRC_MASK);
+			break;
+		}
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4610:
+	case PCI_DEVICE_ID_MEILHAUS_ME4660:
+	case PCI_DEVICE_ID_MEILHAUS_ME4660I:
+	case PCI_DEVICE_ID_MEILHAUS_ME4660S:
+	case PCI_DEVICE_ID_MEILHAUS_ME4660IS:
+	case PCI_DEVICE_ID_MEILHAUS_ME4670:
+	case PCI_DEVICE_ID_MEILHAUS_ME4670I:
+	case PCI_DEVICE_ID_MEILHAUS_ME4670S:
+	case PCI_DEVICE_ID_MEILHAUS_ME4670IS:
+	case PCI_DEVICE_ID_MEILHAUS_ME4680:
+	case PCI_DEVICE_ID_MEILHAUS_ME4680I:
+	case PCI_DEVICE_ID_MEILHAUS_ME4680S:
+	case PCI_DEVICE_ID_MEILHAUS_ME4680IS:
+	case PCI_DEVICE_ID_MEILHAUS_ME8100_A:
+	case PCI_DEVICE_ID_MEILHAUS_ME8100_B:
+
+		/* No clock source register available */
+		break;
+
+	default:
+		PERROR("Invalid device type.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	if (!err)
+		outb(clk_src, instance->clk_src_reg);
+
+	spin_unlock(instance->clk_src_reg_lock);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me1400_ab_ref_config(me8254_subdevice_t * instance, int ref)
+{
+	uint8_t clk_src;
+
+	spin_lock(instance->clk_src_reg_lock);
+	clk_src = inb(instance->clk_src_reg);
+
+	switch (ref) {
+	case ME_REF_CTR_EXTERNAL:
+		if (instance->me8254_idx == 0) {
+			if (instance->ctr_idx == 0)
+				clk_src &= ~(ME1400AB_8254_A_0_CLK_SRC_QUARZ);
+			else if (instance->ctr_idx == 1)
+				clk_src &= ~(ME1400AB_8254_A_1_CLK_SRC_PREV);
+			else
+				clk_src &= ~(ME1400AB_8254_A_2_CLK_SRC_PREV);
+		} else {
+			if (instance->ctr_idx == 0)
+				clk_src &= ~(ME1400AB_8254_B_0_CLK_SRC_QUARZ);
+			else if (instance->ctr_idx == 1)
+				clk_src &= ~(ME1400AB_8254_B_1_CLK_SRC_PREV);
+			else
+				clk_src &= ~(ME1400AB_8254_B_2_CLK_SRC_PREV);
+		}
+
+		break;
+
+	case ME_REF_CTR_PREVIOUS:
+		if (instance->me8254_idx == 0) {
+			if (instance->ctr_idx == 0) {
+				PERROR("Invalid reference.\n");
+				spin_unlock(instance->clk_src_reg_lock);
+				return ME_ERRNO_INVALID_SINGLE_CONFIG;
+			} else if (instance->ctr_idx == 1)
+				clk_src |= (ME1400AB_8254_A_1_CLK_SRC_PREV);
+			else
+				clk_src |= (ME1400AB_8254_A_2_CLK_SRC_PREV);
+		} else {
+			if (instance->ctr_idx == 0) {
+				PERROR("Invalid reference.\n");
+				spin_unlock(instance->clk_src_reg_lock);
+				return ME_ERRNO_INVALID_SINGLE_CONFIG;
+			} else if (instance->ctr_idx == 1)
+				clk_src |= (ME1400AB_8254_B_1_CLK_SRC_PREV);
+			else
+				clk_src |= (ME1400AB_8254_B_2_CLK_SRC_PREV);
+		}
+
+		break;
+
+	case ME_REF_CTR_INTERNAL_1MHZ:
+		if (instance->me8254_idx == 0) {
+			if (instance->ctr_idx == 0) {
+				clk_src |= (ME1400AB_8254_A_0_CLK_SRC_QUARZ);
+				clk_src &= ~(ME1400AB_8254_A_0_CLK_SRC_10MHZ);
+			} else {
+				PERROR("Invalid reference.\n");
+				spin_unlock(instance->clk_src_reg_lock);
+				return ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		} else {
+			if (instance->ctr_idx == 0) {
+				clk_src |= (ME1400AB_8254_B_0_CLK_SRC_QUARZ);
+				clk_src &= ~(ME1400AB_8254_B_0_CLK_SRC_10MHZ);
+			} else {
+				PERROR("Invalid reference.\n");
+				spin_unlock(instance->clk_src_reg_lock);
+				return ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		}
+
+		break;
+
+	case ME_REF_CTR_INTERNAL_10MHZ:
+		if (instance->me8254_idx == 0) {
+			if (instance->ctr_idx == 0) {
+				clk_src |= (ME1400AB_8254_A_0_CLK_SRC_QUARZ);
+				clk_src |= (ME1400AB_8254_A_0_CLK_SRC_10MHZ);
+			} else {
+				PERROR("Invalid reference.\n");
+				spin_unlock(instance->clk_src_reg_lock);
+				return ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		} else {
+			if (instance->ctr_idx == 0) {
+				clk_src |= (ME1400AB_8254_A_0_CLK_SRC_QUARZ);
+				clk_src |= (ME1400AB_8254_A_0_CLK_SRC_10MHZ);
+			} else {
+				PERROR("Invalid reference.\n");
+				spin_unlock(instance->clk_src_reg_lock);
+				return ME_ERRNO_INVALID_SINGLE_CONFIG;
+			}
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid reference.\n");
+		spin_unlock(instance->clk_src_reg_lock);
+		return ME_ERRNO_INVALID_REF;
+	}
+
+	outb(clk_src, instance->clk_src_reg);
+	spin_unlock(instance->clk_src_reg_lock);
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me1400_cd_ref_config(me8254_subdevice_t * instance, int ref)
+{
+	uint8_t clk_src;
+
+	spin_lock(instance->clk_src_reg_lock);
+	clk_src = inb(instance->clk_src_reg);
+
+	switch (ref) {
+	case ME_REF_CTR_EXTERNAL:
+		switch (instance->me8254_idx) {
+		case 0:
+		case 2:
+		case 4:
+		case 6:
+		case 8:
+			if (instance->ctr_idx == 0)
+				clk_src &= ~(ME1400CD_8254_ACE_0_CLK_SRC_MASK);
+			else if (instance->ctr_idx == 1)
+				clk_src &= ~(ME1400CD_8254_ACE_1_CLK_SRC_MASK);
+			else
+				clk_src &= ~(ME1400CD_8254_ACE_2_CLK_SRC_MASK);
+			break;
+
+		default:
+			if (instance->ctr_idx == 0)
+				clk_src &= ~(ME1400CD_8254_BD_0_CLK_SRC_MASK);
+			else if (instance->ctr_idx == 1)
+				clk_src &= ~(ME1400CD_8254_BD_1_CLK_SRC_MASK);
+			else
+				clk_src &= ~(ME1400CD_8254_BD_2_CLK_SRC_MASK);
+			break;
+		}
+		break;
+
+	case ME_REF_CTR_PREVIOUS:
+		switch (instance->me8254_idx) {
+		case 0:
+		case 2:
+		case 4:
+		case 6:
+		case 8:
+			if (instance->ctr_idx == 0) {
+				clk_src &= ~(ME1400CD_8254_ACE_0_CLK_SRC_MASK);
+				clk_src |= (ME1400CD_8254_ACE_0_CLK_SRC_PREV);
+			} else if (instance->ctr_idx == 1) {
+				clk_src &= ~(ME1400CD_8254_ACE_1_CLK_SRC_MASK);
+				clk_src |= (ME1400CD_8254_ACE_1_CLK_SRC_PREV);
+			} else {
+				clk_src &= ~(ME1400CD_8254_ACE_2_CLK_SRC_MASK);
+				clk_src |= (ME1400CD_8254_ACE_2_CLK_SRC_PREV);
+			}
+			break;
+
+		default:
+			if (instance->ctr_idx == 0) {
+				clk_src &= ~(ME1400CD_8254_BD_0_CLK_SRC_MASK);
+				clk_src |= (ME1400CD_8254_BD_0_CLK_SRC_PREV);
+			} else if (instance->ctr_idx == 1) {
+				clk_src &= ~(ME1400CD_8254_BD_1_CLK_SRC_MASK);
+				clk_src |= (ME1400CD_8254_BD_1_CLK_SRC_PREV);
+			} else {
+				clk_src &= ~(ME1400CD_8254_BD_2_CLK_SRC_MASK);
+				clk_src |= (ME1400CD_8254_BD_2_CLK_SRC_PREV);
+			}
+			break;
+		}
+
+		break;
+
+	case ME_REF_CTR_INTERNAL_1MHZ:
+		switch (instance->me8254_idx) {
+		case 0:
+		case 2:
+		case 4:
+		case 6:
+		case 8:
+			if (instance->ctr_idx == 0) {
+				clk_src &= ~(ME1400CD_8254_ACE_0_CLK_SRC_MASK);
+				clk_src |= (ME1400CD_8254_ACE_0_CLK_SRC_1MHZ);
+			} else {
+				PERROR("Invalid reference.\n");
+				spin_unlock(instance->clk_src_reg_lock);
+				return ME_ERRNO_INVALID_REF;
+			}
+
+			break;
+
+		default:
+			if (instance->ctr_idx == 0) {
+				clk_src &= ~(ME1400CD_8254_BD_0_CLK_SRC_MASK);
+				clk_src |= (ME1400CD_8254_BD_0_CLK_SRC_1MHZ);
+			} else {
+				PERROR("Invalid reference.\n");
+				spin_unlock(instance->clk_src_reg_lock);
+				return ME_ERRNO_INVALID_REF;
+			}
+			break;
+		}
+
+		break;
+
+	case ME_REF_CTR_INTERNAL_10MHZ:
+		switch (instance->me8254_idx) {
+		case 0:
+		case 2:
+		case 4:
+		case 6:
+		case 8:
+			if (instance->ctr_idx == 0) {
+				clk_src &= ~(ME1400CD_8254_ACE_0_CLK_SRC_MASK);
+				clk_src |= (ME1400CD_8254_ACE_0_CLK_SRC_10MHZ);
+			} else {
+				PERROR("Invalid reference.\n");
+				spin_unlock(instance->clk_src_reg_lock);
+				return ME_ERRNO_INVALID_REF;
+			}
+			break;
+
+		default:
+			if (instance->ctr_idx == 0) {
+				clk_src &= ~(ME1400CD_8254_BD_0_CLK_SRC_MASK);
+				clk_src |= (ME1400CD_8254_BD_0_CLK_SRC_10MHZ);
+			} else {
+				PERROR("Invalid reference.\n");
+				spin_unlock(instance->clk_src_reg_lock);
+				return ME_ERRNO_INVALID_REF;
+			}
+
+			break;
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid reference.\n");
+		spin_unlock(instance->clk_src_reg_lock);
+		return ME_ERRNO_INVALID_REF;
+	}
+
+	outb(clk_src, instance->clk_src_reg);
+	spin_unlock(instance->clk_src_reg_lock);
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me4600_ref_config(me8254_subdevice_t * instance, int ref)
+{
+	switch (ref) {
+
+	case ME_REF_CTR_EXTERNAL:
+		// Nothing to do
+		break;
+
+	default:
+		PERROR("Invalid reference.\n");
+//                      spin_unlock(instance->clk_src_reg_lock);
+		return ME_ERRNO_INVALID_REF;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8100_ref_config(me8254_subdevice_t * instance, int ref)
+{
+	switch (ref) {
+
+	case ME_REF_CTR_EXTERNAL:
+		// Nothing to do
+		break;
+
+	default:
+		PERROR("Invalid reference.\n");
+//                      spin_unlock(instance->clk_src_reg_lock);
+		return ME_ERRNO_INVALID_REF;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8254_io_single_config(struct me_subdevice *subdevice,
+				   struct file *filep,
+				   int channel,
+				   int single_config,
+				   int ref,
+				   int trig_chan,
+				   int trig_type, int trig_edge, int flags)
+{
+	me8254_subdevice_t *instance;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	if (channel) {
+		PERROR("Invalid channel.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	instance = (me8254_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	// Configure the counter modes
+	if (instance->ctr_idx == 0) {
+		if (single_config == ME_SINGLE_CONFIG_CTR_8254_MODE_0) {
+			outb(ME8254_CTRL_SC0 | ME8254_CTRL_LM | ME8254_CTRL_M0 |
+			     ME8254_CTRL_BIN, instance->ctrl_reg);
+		} else if (single_config == ME_SINGLE_CONFIG_CTR_8254_MODE_1) {
+			outb(ME8254_CTRL_SC0 | ME8254_CTRL_LM | ME8254_CTRL_M1 |
+			     ME8254_CTRL_BIN, instance->ctrl_reg);
+		} else if (single_config == ME_SINGLE_CONFIG_CTR_8254_MODE_2) {
+			outb(ME8254_CTRL_SC0 | ME8254_CTRL_LM | ME8254_CTRL_M2 |
+			     ME8254_CTRL_BIN, instance->ctrl_reg);
+		} else if (single_config == ME_SINGLE_CONFIG_CTR_8254_MODE_3) {
+			outb(ME8254_CTRL_SC0 | ME8254_CTRL_LM | ME8254_CTRL_M3 |
+			     ME8254_CTRL_BIN, instance->ctrl_reg);
+		} else if (single_config == ME_SINGLE_CONFIG_CTR_8254_MODE_4) {
+			outb(ME8254_CTRL_SC0 | ME8254_CTRL_LM | ME8254_CTRL_M4 |
+			     ME8254_CTRL_BIN, instance->ctrl_reg);
+		} else if (single_config == ME_SINGLE_CONFIG_CTR_8254_MODE_5) {
+			outb(ME8254_CTRL_SC0 | ME8254_CTRL_LM | ME8254_CTRL_M5 |
+			     ME8254_CTRL_BIN, instance->ctrl_reg);
+		} else {
+			PERROR("Invalid single configuration.\n");
+			spin_unlock(&instance->subdevice_lock);
+			return ME_ERRNO_INVALID_SINGLE_CONFIG;
+		}
+	} else if (instance->ctr_idx == 1) {
+		if (single_config == ME_SINGLE_CONFIG_CTR_8254_MODE_0) {
+			outb(ME8254_CTRL_SC1 | ME8254_CTRL_LM | ME8254_CTRL_M0 |
+			     ME8254_CTRL_BIN, instance->ctrl_reg);
+		} else if (single_config == ME_SINGLE_CONFIG_CTR_8254_MODE_1) {
+			outb(ME8254_CTRL_SC1 | ME8254_CTRL_LM | ME8254_CTRL_M1 |
+			     ME8254_CTRL_BIN, instance->ctrl_reg);
+		} else if (single_config == ME_SINGLE_CONFIG_CTR_8254_MODE_2) {
+			outb(ME8254_CTRL_SC1 | ME8254_CTRL_LM | ME8254_CTRL_M2 |
+			     ME8254_CTRL_BIN, instance->ctrl_reg);
+		} else if (single_config == ME_SINGLE_CONFIG_CTR_8254_MODE_3) {
+			outb(ME8254_CTRL_SC1 | ME8254_CTRL_LM | ME8254_CTRL_M3 |
+			     ME8254_CTRL_BIN, instance->ctrl_reg);
+		} else if (single_config == ME_SINGLE_CONFIG_CTR_8254_MODE_4) {
+			outb(ME8254_CTRL_SC1 | ME8254_CTRL_LM | ME8254_CTRL_M4 |
+			     ME8254_CTRL_BIN, instance->ctrl_reg);
+		} else if (single_config == ME_SINGLE_CONFIG_CTR_8254_MODE_5) {
+			outb(ME8254_CTRL_SC1 | ME8254_CTRL_LM | ME8254_CTRL_M5 |
+			     ME8254_CTRL_BIN, instance->ctrl_reg);
+		} else {
+			PERROR("Invalid single configuration.\n");
+			spin_unlock(&instance->subdevice_lock);
+			return ME_ERRNO_INVALID_SINGLE_CONFIG;
+		}
+	} else {
+		if (single_config == ME_SINGLE_CONFIG_CTR_8254_MODE_0) {
+			outb(ME8254_CTRL_SC2 | ME8254_CTRL_LM | ME8254_CTRL_M0 |
+			     ME8254_CTRL_BIN, instance->ctrl_reg);
+		} else if (single_config == ME_SINGLE_CONFIG_CTR_8254_MODE_1) {
+			outb(ME8254_CTRL_SC2 | ME8254_CTRL_LM | ME8254_CTRL_M1 |
+			     ME8254_CTRL_BIN, instance->ctrl_reg);
+		} else if (single_config == ME_SINGLE_CONFIG_CTR_8254_MODE_2) {
+			outb(ME8254_CTRL_SC2 | ME8254_CTRL_LM | ME8254_CTRL_M2 |
+			     ME8254_CTRL_BIN, instance->ctrl_reg);
+		} else if (single_config == ME_SINGLE_CONFIG_CTR_8254_MODE_3) {
+			outb(ME8254_CTRL_SC2 | ME8254_CTRL_LM | ME8254_CTRL_M3 |
+			     ME8254_CTRL_BIN, instance->ctrl_reg);
+		} else if (single_config == ME_SINGLE_CONFIG_CTR_8254_MODE_4) {
+			outb(ME8254_CTRL_SC2 | ME8254_CTRL_LM | ME8254_CTRL_M4 |
+			     ME8254_CTRL_BIN, instance->ctrl_reg);
+		} else if (single_config == ME_SINGLE_CONFIG_CTR_8254_MODE_5) {
+			outb(ME8254_CTRL_SC2 | ME8254_CTRL_LM | ME8254_CTRL_M5 |
+			     ME8254_CTRL_BIN, instance->ctrl_reg);
+		} else {
+			PERROR("Invalid single configuration.\n");
+			spin_unlock(&instance->subdevice_lock);
+			return ME_ERRNO_INVALID_SINGLE_CONFIG;
+		}
+	}
+
+	switch (instance->device_id) {
+	case PCI_DEVICE_ID_MEILHAUS_ME1400:
+	case PCI_DEVICE_ID_MEILHAUS_ME14E0:
+	case PCI_DEVICE_ID_MEILHAUS_ME140A:
+	case PCI_DEVICE_ID_MEILHAUS_ME14EA:
+	case PCI_DEVICE_ID_MEILHAUS_ME140B:
+	case PCI_DEVICE_ID_MEILHAUS_ME14EB:
+		err = me1400_ab_ref_config(instance, ref);
+
+		if (err) {
+			spin_unlock(&instance->subdevice_lock);
+			return err;
+		}
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140C:
+	case PCI_DEVICE_ID_MEILHAUS_ME140D:
+		err = me1400_cd_ref_config(instance, ref);
+
+		if (err) {
+			spin_unlock(&instance->subdevice_lock);
+			return err;
+		}
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4610:
+	case PCI_DEVICE_ID_MEILHAUS_ME4660:
+	case PCI_DEVICE_ID_MEILHAUS_ME4660I:
+	case PCI_DEVICE_ID_MEILHAUS_ME4660S:
+	case PCI_DEVICE_ID_MEILHAUS_ME4660IS:
+	case PCI_DEVICE_ID_MEILHAUS_ME4670:
+	case PCI_DEVICE_ID_MEILHAUS_ME4670I:
+	case PCI_DEVICE_ID_MEILHAUS_ME4670S:
+	case PCI_DEVICE_ID_MEILHAUS_ME4670IS:
+	case PCI_DEVICE_ID_MEILHAUS_ME4680:
+	case PCI_DEVICE_ID_MEILHAUS_ME4680I:
+	case PCI_DEVICE_ID_MEILHAUS_ME4680S:
+	case PCI_DEVICE_ID_MEILHAUS_ME4680IS:
+		err = me4600_ref_config(instance, ref);
+
+		if (err) {
+			spin_unlock(&instance->subdevice_lock);
+			return err;
+		}
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME8100_A:
+	case PCI_DEVICE_ID_MEILHAUS_ME8100_B:
+		err = me8100_ref_config(instance, ref);
+
+		if (err) {
+			spin_unlock(&instance->subdevice_lock);
+			return err;
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid device type.\n");
+
+		spin_unlock(&instance->subdevice_lock);
+//                              spin_unlock(instance->clk_src_reg_lock);
+		return ME_ERRNO_INVALID_SINGLE_CONFIG;
+	}
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8254_io_single_read(struct me_subdevice *subdevice,
+				 struct file *filep,
+				 int channel,
+				 int *value, int time_out, int flags)
+{
+	me8254_subdevice_t *instance;
+	uint16_t lo_byte;
+	uint16_t hi_byte;
+
+	PDEBUG("executed.\n");
+
+	if (channel) {
+		PERROR("Invalid channel.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	instance = (me8254_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	if (instance->ctr_idx == 0)
+		outb(ME8254_CTRL_SC0 | ME8254_CTRL_TLO, instance->ctrl_reg);
+	else if (instance->ctr_idx == 1)
+		outb(ME8254_CTRL_SC1 | ME8254_CTRL_TLO, instance->ctrl_reg);
+	else
+		outb(ME8254_CTRL_SC2 | ME8254_CTRL_TLO, instance->ctrl_reg);
+
+	lo_byte = inb(instance->val_reg);
+	hi_byte = inb(instance->val_reg);
+	spin_unlock(instance->ctrl_reg_lock);
+
+	*value = lo_byte | (hi_byte << 8);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8254_io_single_write(struct me_subdevice *subdevice,
+				  struct file *filep,
+				  int channel,
+				  int value, int time_out, int flags)
+{
+	me8254_subdevice_t *instance;
+
+	PDEBUG("executed.\n");
+
+	if (channel) {
+		PERROR("Invalid channel.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	instance = (me8254_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	outb(value, instance->val_reg);
+	outb((value >> 8), instance->val_reg);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8254_query_number_channels(struct me_subdevice *subdevice,
+					int *number)
+{
+	PDEBUG("executed.\n");
+	*number = ME8254_NUMBER_CHANNELS;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8254_query_subdevice_type(struct me_subdevice *subdevice,
+				       int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_CTR;
+	*subtype = ME_SUBTYPE_CTR_8254;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8254_query_subdevice_caps(struct me_subdevice *subdevice,
+				       int *caps)
+{
+	me8254_subdevice_t *instance;
+	PDEBUG("executed.\n");
+	instance = (me8254_subdevice_t *) subdevice;
+	*caps = instance->caps;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8254_query_subdevice_caps_args(struct me_subdevice *subdevice,
+					    int cap, int *args, int count)
+{
+	PDEBUG("executed.\n");
+
+	if (count != 1) {
+		PERROR("Invalid capability argument count.\n");
+		return ME_ERRNO_INVALID_CAP_ARG_COUNT;
+	}
+
+	if (cap == ME_CAP_CTR_WIDTH) {
+		args[0] = ME8254_CTR_WIDTH;
+	} else {
+		PERROR("Invalid capability.\n");
+		return ME_ERRNO_INVALID_CAP;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static uint32_t me1400AB_get_val_reg(uint32_t reg_base, unsigned int me8254_idx,
+				     unsigned int ctr_idx)
+{
+	switch (me8254_idx) {
+
+	case 0:
+		return (reg_base + ME1400AB_8254_A_0_VAL_REG + ctr_idx);
+
+	default:
+		return (reg_base + ME1400AB_8254_B_0_VAL_REG + ctr_idx);
+	}
+
+	return 0;
+}
+
+static uint32_t me1400AB_get_ctrl_reg(uint32_t reg_base,
+				      unsigned int me8254_idx,
+				      unsigned int ctr_idx)
+{
+	switch (me8254_idx) {
+	case 0:
+		return (reg_base + ME1400AB_8254_A_CTRL_REG);
+
+	default:
+		return (reg_base + ME1400AB_8254_B_CTRL_REG);
+	}
+
+	return 0;
+}
+
+static uint32_t me1400AB_get_clk_src_reg(uint32_t reg_base,
+					 unsigned int me8254_idx,
+					 unsigned int ctr_idx)
+{
+	switch (me8254_idx) {
+	case 0:
+		return (reg_base + ME1400AB_CLK_SRC_REG);
+
+	default:
+		return (reg_base + ME1400AB_CLK_SRC_REG);
+	}
+
+	return 0;
+}
+
+static uint32_t me1400CD_get_val_reg(uint32_t reg_base, unsigned int me8254_idx,
+				     unsigned int ctr_idx)
+{
+	switch (me8254_idx) {
+	case 0:
+		return (reg_base + ME1400C_8254_A_0_VAL_REG + ctr_idx);
+
+	case 1:
+		return (reg_base + ME1400C_8254_B_0_VAL_REG + ctr_idx);
+
+	case 2:
+		return (reg_base + ME1400C_8254_C_0_VAL_REG + ctr_idx);
+
+	case 3:
+		return (reg_base + ME1400C_8254_D_0_VAL_REG + ctr_idx);
+
+	case 4:
+		return (reg_base + ME1400C_8254_E_0_VAL_REG + ctr_idx);
+
+	case 5:
+		return (reg_base + ME1400D_8254_A_0_VAL_REG + ctr_idx);
+
+	case 6:
+		return (reg_base + ME1400D_8254_B_0_VAL_REG + ctr_idx);
+
+	case 7:
+		return (reg_base + ME1400D_8254_C_0_VAL_REG + ctr_idx);
+
+	case 8:
+		return (reg_base + ME1400D_8254_D_0_VAL_REG + ctr_idx);
+
+	default:
+		return (reg_base + ME1400D_8254_E_0_VAL_REG + ctr_idx);
+	}
+
+	return 0;
+}
+
+static uint32_t me1400CD_get_ctrl_reg(uint32_t reg_base,
+				      unsigned int me8254_idx,
+				      unsigned int ctr_idx)
+{
+	switch (me8254_idx) {
+	case 0:
+		return (reg_base + ME1400C_8254_A_CTRL_REG);
+
+	case 1:
+		return (reg_base + ME1400C_8254_B_CTRL_REG);
+
+	case 2:
+		return (reg_base + ME1400C_8254_C_CTRL_REG);
+
+	case 3:
+		return (reg_base + ME1400C_8254_D_CTRL_REG);
+
+	case 4:
+		return (reg_base + ME1400C_8254_E_CTRL_REG);
+
+	case 5:
+		return (reg_base + ME1400D_8254_A_CTRL_REG);
+
+	case 6:
+		return (reg_base + ME1400D_8254_B_CTRL_REG);
+
+	case 7:
+		return (reg_base + ME1400D_8254_C_CTRL_REG);
+
+	case 8:
+		return (reg_base + ME1400D_8254_D_CTRL_REG);
+
+	default:
+		return (reg_base + ME1400D_8254_E_CTRL_REG);
+	}
+
+	return 0;
+}
+
+static uint32_t me1400CD_get_clk_src_reg(uint32_t reg_base,
+					 unsigned int me8254_idx,
+					 unsigned int ctr_idx)
+{
+	switch (me8254_idx) {
+	case 0:
+		return (reg_base + ME1400C_CLK_SRC_0_REG);
+
+	case 1:
+		return (reg_base + ME1400C_CLK_SRC_0_REG);
+
+	case 2:
+		return (reg_base + ME1400C_CLK_SRC_1_REG);
+
+	case 3:
+		return (reg_base + ME1400C_CLK_SRC_1_REG);
+
+	case 4:
+		return (reg_base + ME1400C_CLK_SRC_2_REG);
+
+	case 5:
+		return (reg_base + ME1400D_CLK_SRC_0_REG);
+
+	case 6:
+		return (reg_base + ME1400D_CLK_SRC_0_REG);
+
+	case 7:
+		return (reg_base + ME1400D_CLK_SRC_1_REG);
+
+	case 8:
+		return (reg_base + ME1400D_CLK_SRC_1_REG);
+
+	default:
+		return (reg_base + ME1400D_CLK_SRC_2_REG);
+	}
+
+	return 0;
+}
+
+static uint32_t me4600_get_val_reg(uint32_t reg_base, unsigned int me8254_idx,
+				   unsigned int ctr_idx)
+{
+	return (reg_base + ME4600_8254_0_VAL_REG + ctr_idx);
+}
+
+static uint32_t me4600_get_ctrl_reg(uint32_t reg_base, unsigned int me8254_idx,
+				    unsigned int ctr_idx)
+{
+	return (reg_base + ME4600_8254_CTRL_REG);
+}
+
+static uint32_t me8100_get_val_reg(uint32_t reg_base, unsigned int me8254_idx,
+				   unsigned int ctr_idx)
+{
+	return (reg_base + ME8100_COUNTER_REG_0 + ctr_idx * 2);
+}
+
+static uint32_t me8100_get_ctrl_reg(uint32_t reg_base, unsigned int me8254_idx,
+				    unsigned int ctr_idx)
+{
+	return (reg_base + ME8100_COUNTER_CTRL_REG);
+}
+
+me8254_subdevice_t *me8254_constructor(uint32_t device_id,
+				       uint32_t reg_base,
+				       unsigned int me8254_idx,
+				       unsigned int ctr_idx,
+				       spinlock_t * ctrl_reg_lock,
+				       spinlock_t * clk_src_reg_lock)
+{
+	me8254_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	// Allocate memory for subdevice instance
+	subdevice = kmalloc(sizeof(me8254_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for 8254 instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me8254_subdevice_t));
+
+	// Check if counter index is out of range
+
+	if (ctr_idx > 2) {
+		PERROR("Counter index is out of range.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize subdevice base class
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+	subdevice->ctrl_reg_lock = ctrl_reg_lock;
+	subdevice->clk_src_reg_lock = clk_src_reg_lock;
+
+	// Save type of Meilhaus device
+	subdevice->device_id = device_id;
+
+	// Save the indices
+	subdevice->me8254_idx = me8254_idx;
+	subdevice->ctr_idx = ctr_idx;
+
+	// Do device specific initialization
+	switch (device_id) {
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140A:
+	case PCI_DEVICE_ID_MEILHAUS_ME14EA:
+		// Check if 8254 index is out of range
+		if (me8254_idx > 0) {
+			PERROR("8254 index is out of range.\n");
+			me_subdevice_deinit(&subdevice->base);
+			kfree(subdevice);
+			return NULL;
+		}
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140B:	// Fall through
+	case PCI_DEVICE_ID_MEILHAUS_ME14EB:
+		// Check if 8254 index is out of range
+		if (me8254_idx > 1) {
+			PERROR("8254 index is out of range.\n");
+			me_subdevice_deinit(&subdevice->base);
+			kfree(subdevice);
+			return NULL;
+		}
+		// Initialize the counters capabilities
+		if (ctr_idx == 0)
+			subdevice->caps =
+			    ME_CAPS_CTR_CLK_INTERNAL_1MHZ |
+			    ME_CAPS_CTR_CLK_INTERNAL_10MHZ |
+			    ME_CAPS_CTR_CLK_EXTERNAL;
+		else
+			subdevice->caps =
+			    ME_CAPS_CTR_CLK_PREVIOUS | ME_CAPS_CTR_CLK_EXTERNAL;
+
+		// Get the counters registers
+		subdevice->val_reg =
+		    me1400AB_get_val_reg(reg_base, me8254_idx, ctr_idx);
+		subdevice->ctrl_reg =
+		    me1400AB_get_ctrl_reg(reg_base, me8254_idx, ctr_idx);
+		subdevice->clk_src_reg =
+		    me1400AB_get_clk_src_reg(reg_base, me8254_idx, ctr_idx);
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140C:
+		// Check if 8254 index is out of range
+		if (me8254_idx > 4) {
+			PERROR("8254 index is out of range.\n");
+			me_subdevice_deinit(&subdevice->base);
+			kfree(subdevice);
+			return NULL;
+		}
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140D:
+		// Check if 8254 index is out of range
+		if (me8254_idx > 9) {
+			PERROR("8254 index is out of range.\n");
+			me_subdevice_deinit(&subdevice->base);
+			kfree(subdevice);
+			return NULL;
+		}
+		// Initialize the counters capabilities
+		if (ctr_idx == 0) {
+			if (me8254_idx == 0)
+				subdevice->caps =
+				    ME_CAPS_CTR_CLK_PREVIOUS |
+				    ME_CAPS_CTR_CLK_INTERNAL_1MHZ |
+				    ME_CAPS_CTR_CLK_INTERNAL_10MHZ |
+				    ME_CAPS_CTR_CLK_EXTERNAL;
+			else
+				subdevice->caps =
+				    ME_CAPS_CTR_CLK_INTERNAL_1MHZ |
+				    ME_CAPS_CTR_CLK_INTERNAL_10MHZ |
+				    ME_CAPS_CTR_CLK_EXTERNAL;
+		} else
+			subdevice->caps =
+			    ME_CAPS_CTR_CLK_PREVIOUS | ME_CAPS_CTR_CLK_EXTERNAL;
+
+		// Get the counters registers
+		subdevice->val_reg =
+		    me1400CD_get_val_reg(reg_base, me8254_idx, ctr_idx);
+		subdevice->ctrl_reg =
+		    me1400CD_get_ctrl_reg(reg_base, me8254_idx, ctr_idx);
+		subdevice->clk_src_reg =
+		    me1400CD_get_clk_src_reg(reg_base, me8254_idx, ctr_idx);
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4610:
+	case PCI_DEVICE_ID_MEILHAUS_ME4660:
+	case PCI_DEVICE_ID_MEILHAUS_ME4660I:
+	case PCI_DEVICE_ID_MEILHAUS_ME4660S:
+	case PCI_DEVICE_ID_MEILHAUS_ME4660IS:
+	case PCI_DEVICE_ID_MEILHAUS_ME4670:
+	case PCI_DEVICE_ID_MEILHAUS_ME4670I:
+	case PCI_DEVICE_ID_MEILHAUS_ME4670S:
+	case PCI_DEVICE_ID_MEILHAUS_ME4670IS:
+	case PCI_DEVICE_ID_MEILHAUS_ME4680:
+	case PCI_DEVICE_ID_MEILHAUS_ME4680I:
+	case PCI_DEVICE_ID_MEILHAUS_ME4680S:
+	case PCI_DEVICE_ID_MEILHAUS_ME4680IS:
+		// Check if 8254 index is out of range
+		if (me8254_idx > 0) {
+			PERROR("8254 index is out of range.\n");
+			me_subdevice_deinit(&subdevice->base);
+			kfree(subdevice);
+			return NULL;
+		}
+		// Initialize the counters capabilities
+		subdevice->caps = ME_CAPS_CTR_CLK_EXTERNAL;
+
+		// Get the counters registers
+		subdevice->val_reg =
+		    me4600_get_val_reg(reg_base, me8254_idx, ctr_idx);
+		subdevice->ctrl_reg =
+		    me4600_get_ctrl_reg(reg_base, me8254_idx, ctr_idx);
+		subdevice->clk_src_reg = 0;	// Not used
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME8100_A:
+	case PCI_DEVICE_ID_MEILHAUS_ME8100_B:
+		// Check if 8254 index is out of range
+		if (me8254_idx > 0) {
+			PERROR("8254 index is out of range.\n");
+			me_subdevice_deinit(&subdevice->base);
+			kfree(subdevice);
+			return NULL;
+		}
+		// Initialize the counters capabilities
+		subdevice->caps = ME_CAPS_CTR_CLK_EXTERNAL;
+
+		// Get the counters registers
+		subdevice->val_reg =
+		    me8100_get_val_reg(reg_base, me8254_idx, ctr_idx);
+		subdevice->ctrl_reg =
+		    me8100_get_ctrl_reg(reg_base, me8254_idx, ctr_idx);
+		subdevice->clk_src_reg = 0;	// Not used
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4650:
+	case PCI_DEVICE_ID_MEILHAUS_ME1400:
+	case PCI_DEVICE_ID_MEILHAUS_ME14E0:
+		PERROR("No 8254 subdevices available for subdevice device.\n");
+		me_subdevice_deinit(&subdevice->base);
+		kfree(subdevice);
+		return NULL;
+
+	default:
+		PERROR("Unknown device type.\n");
+		me_subdevice_deinit(&subdevice->base);
+		kfree(subdevice);
+		return NULL;
+	}
+
+	// Overload subdevice base class methods.
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me8254_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config = me8254_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me8254_io_single_read;
+	subdevice->base.me_subdevice_io_single_write = me8254_io_single_write;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me8254_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me8254_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me8254_query_subdevice_caps;
+	subdevice->base.me_subdevice_query_subdevice_caps_args =
+	    me8254_query_subdevice_caps_args;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me8254.h b/drivers/staging/meilhaus/me8254.h
new file mode 100644
index 00000000000..572b7196d5a
--- /dev/null
+++ b/drivers/staging/meilhaus/me8254.h
@@ -0,0 +1,80 @@
+/**
+ * @file me8254.h
+ *
+ * @brief 8254 counter implementation.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _ME8254_H_
+#define _ME8254_H_
+
+#include "mesubdevice.h"
+#include "meslock.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The 8254 subdevice class.
+ */
+typedef struct me8254_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+
+	spinlock_t *ctrl_reg_lock;		/**< Spin lock to protect the control register from concurrent access. */
+	spinlock_t *clk_src_reg_lock;		/**< Spin lock to protect the clock source register from concurrent access. */
+
+	uint32_t device_id;			/**< The Meilhaus device type carrying the 8254 chip. */
+	int me8254_idx;				/**< The index of the 8254 chip on the device. */
+	int ctr_idx;				/**< The index of the counter on the 8254 chip. */
+
+	int caps;				/**< Holds the device capabilities. */
+
+	unsigned long val_reg;			/**< Holds the actual counter value. */
+	unsigned long ctrl_reg;			/**< Register to configure the 8254 modes. */
+	unsigned long clk_src_reg;		/**< Register to configure the counter connections. */
+} me8254_subdevice_t;
+
+/**
+ * @brief The constructor to generate a 8254 instance.
+ *
+ * @param device_id The kind of Meilhaus device holding the 8254.
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ * @param me8254_idx The index of the 8254 chip on the Meilhaus device.
+ * @param ctr_idx The index of the counter inside a 8254 chip.
+ * @param ctrl_reg_lock Pointer to spin lock protecting the 8254 control register from concurrent access.
+ * @param clk_src_reg_lock Pointer to spin lock protecting the clock source register from concurrent access.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me8254_subdevice_t *me8254_constructor(uint32_t device_id,
+				       uint32_t reg_base,
+				       unsigned int me8254_idx,
+				       unsigned int ctr_idx,
+				       spinlock_t * ctrl_reg_lock,
+				       spinlock_t * clk_src_reg_lock);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me8254_reg.h b/drivers/staging/meilhaus/me8254_reg.h
new file mode 100644
index 00000000000..7e2c36b46f5
--- /dev/null
+++ b/drivers/staging/meilhaus/me8254_reg.h
@@ -0,0 +1,172 @@
+/**
+ * @file me8254_reg.h
+ *
+ * @brief 8254 counter register definitions.
+ * @note Copyright (C) 2006 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+#ifndef _ME8254_REG_H_
+#define _ME8254_REG_H_
+
+#ifdef __KERNEL__
+
+/* ME1400 A/B register offsets */
+#define ME1400AB_8254_A_0_VAL_REG		0x0004 /**< Offset of 8254 A counter 0 value register. */
+#define ME1400AB_8254_A_1_VAL_REG		0x0005 /**< Offset of 8254 A counter 1 value register. */
+#define ME1400AB_8254_A_2_VAL_REG		0x0006 /**< Offset of 8254 A counter 2 value register. */
+#define ME1400AB_8254_A_CTRL_REG		0x0007 /**< Offset of 8254 A control register. */
+
+#define ME1400AB_8254_B_0_VAL_REG		0x000C /**< Offset of 8254 B counter 0 value register. */
+#define ME1400AB_8254_B_1_VAL_REG		0x000D /**< Offset of 8254 B counter 1 value register. */
+#define ME1400AB_8254_B_2_VAL_REG		0x000E /**< Offset of 8254 B counter 2 value register. */
+#define ME1400AB_8254_B_CTRL_REG		0x000F /**< Offset of 8254 B control register. */
+
+#define ME1400AB_CLK_SRC_REG			0x0010 /**< Offset of clock source register. */
+
+/* ME1400 C register offsets */
+#define ME1400C_8254_A_0_VAL_REG		0x0004 /**< Offset of 8254 A counter 0 value register. */
+#define ME1400C_8254_A_1_VAL_REG		0x0005 /**< Offset of 8254 A counter 0 value register. */
+#define ME1400C_8254_A_2_VAL_REG		0x0006 /**< Offset of 8254 A counter 0 value register. */
+#define ME1400C_8254_A_CTRL_REG			0x0007 /**< Offset of 8254 A control register. */
+
+#define ME1400C_8254_B_0_VAL_REG		0x000C /**< Offset of 8254 B counter 0 value register. */
+#define ME1400C_8254_B_1_VAL_REG		0x000D /**< Offset of 8254 B counter 0 value register. */
+#define ME1400C_8254_B_2_VAL_REG		0x000E /**< Offset of 8254 B counter 0 value register. */
+#define ME1400C_8254_B_CTRL_REG			0x000F /**< Offset of 8254 B control register. */
+
+#define ME1400C_8254_C_0_VAL_REG		0x0010 /**< Offset of 8254 C counter 0 value register. */
+#define ME1400C_8254_C_1_VAL_REG		0x0011 /**< Offset of 8254 C counter 0 value register. */
+#define ME1400C_8254_C_2_VAL_REG		0x0012 /**< Offset of 8254 C counter 0 value register. */
+#define ME1400C_8254_C_CTRL_REG			0x0013 /**< Offset of 8254 C control register. */
+
+#define ME1400C_8254_D_0_VAL_REG		0x0014 /**< Offset of 8254 D counter 0 value register. */
+#define ME1400C_8254_D_1_VAL_REG		0x0015 /**< Offset of 8254 D counter 0 value register. */
+#define ME1400C_8254_D_2_VAL_REG		0x0016 /**< Offset of 8254 D counter 0 value register. */
+#define ME1400C_8254_D_CTRL_REG			0x0017 /**< Offset of 8254 D control register. */
+
+#define ME1400C_8254_E_0_VAL_REG		0x0018 /**< Offset of 8254 E counter 0 value register. */
+#define ME1400C_8254_E_1_VAL_REG		0x0019 /**< Offset of 8254 E counter 0 value register. */
+#define ME1400C_8254_E_2_VAL_REG		0x001A /**< Offset of 8254 E counter 0 value register. */
+#define ME1400C_8254_E_CTRL_REG			0x001B /**< Offset of 8254 E control register. */
+
+#define ME1400C_CLK_SRC_0_REG  			0x001C /**< Offset of clock source register 0. */
+#define ME1400C_CLK_SRC_1_REG  			0x001D /**< Offset of clock source register 1. */
+#define ME1400C_CLK_SRC_2_REG  			0x001E /**< Offset of clock source register 2. */
+
+/* ME1400 D register offsets */
+#define ME1400D_8254_A_0_VAL_REG		0x0044 /**< Offset of 8254 A counter 0 value register. */
+#define ME1400D_8254_A_1_VAL_REG		0x0045 /**< Offset of 8254 A counter 0 value register. */
+#define ME1400D_8254_A_2_VAL_REG		0x0046 /**< Offset of 8254 A counter 0 value register. */
+#define ME1400D_8254_A_CTRL_REG			0x0047 /**< Offset of 8254 A control register. */
+
+#define ME1400D_8254_B_0_VAL_REG		0x004C /**< Offset of 8254 B counter 0 value register. */
+#define ME1400D_8254_B_1_VAL_REG		0x004D /**< Offset of 8254 B counter 0 value register. */
+#define ME1400D_8254_B_2_VAL_REG		0x004E /**< Offset of 8254 B counter 0 value register. */
+#define ME1400D_8254_B_CTRL_REG			0x004F /**< Offset of 8254 B control register. */
+
+#define ME1400D_8254_C_0_VAL_REG		0x0050 /**< Offset of 8254 C counter 0 value register. */
+#define ME1400D_8254_C_1_VAL_REG		0x0051 /**< Offset of 8254 C counter 0 value register. */
+#define ME1400D_8254_C_2_VAL_REG		0x0052 /**< Offset of 8254 C counter 0 value register. */
+#define ME1400D_8254_C_CTRL_REG			0x0053 /**< Offset of 8254 C control register. */
+
+#define ME1400D_8254_D_0_VAL_REG		0x0054 /**< Offset of 8254 D counter 0 value register. */
+#define ME1400D_8254_D_1_VAL_REG		0x0055 /**< Offset of 8254 D counter 0 value register. */
+#define ME1400D_8254_D_2_VAL_REG		0x0056 /**< Offset of 8254 D counter 0 value register. */
+#define ME1400D_8254_D_CTRL_REG			0x0057 /**< Offset of 8254 D control register. */
+
+#define ME1400D_8254_E_0_VAL_REG		0x0058 /**< Offset of 8254 E counter 0 value register. */
+#define ME1400D_8254_E_1_VAL_REG		0x0059 /**< Offset of 8254 E counter 0 value register. */
+#define ME1400D_8254_E_2_VAL_REG		0x005A /**< Offset of 8254 E counter 0 value register. */
+#define ME1400D_8254_E_CTRL_REG			0x005B /**< Offset of 8254 E control register. */
+
+#define ME1400D_CLK_SRC_0_REG  			0x005C /**< Offset of clock source register 0. */
+#define ME1400D_CLK_SRC_1_REG  			0x005D /**< Offset of clock source register 1. */
+#define ME1400D_CLK_SRC_2_REG  			0x005E /**< Offset of clock source register 2. */
+
+/* ME4600 register offsets */
+#define ME4600_8254_0_VAL_REG			0x0000 /**< Offset of 8254 A counter 0 value register. */
+#define ME4600_8254_1_VAL_REG			0x0001 /**< Offset of 8254 A counter 0 value register. */
+#define ME4600_8254_2_VAL_REG			0x0002 /**< Offset of 8254 A counter 0 value register. */
+#define ME4600_8254_CTRL_REG			0x0003 /**< Offset of 8254 A control register. */
+
+/* Command words for 8254 control register */
+#define ME8254_CTRL_SC0   0x00	 /**< Counter 0 selection. */
+#define ME8254_CTRL_SC1   0x40	 /**< Counter 1 selection. */
+#define ME8254_CTRL_SC2   0x80	 /**< Counter 2 selection. */
+
+#define ME8254_CTRL_TLO   0x00	 /**< Counter latching operation. */
+#define ME8254_CTRL_LSB   0x10	 /**< Only read LSB. */
+#define ME8254_CTRL_MSB   0x20	 /**< Only read MSB. */
+#define ME8254_CTRL_LM    0x30	 /**< First read LSB, then MSB.  */
+
+#define ME8254_CTRL_M0    0x00	 /**< Mode 0 selection. */
+#define ME8254_CTRL_M1    0x02	 /**< Mode 1 selection. */
+#define ME8254_CTRL_M2    0x04	 /**< Mode 2 selection. */
+#define ME8254_CTRL_M3    0x06	 /**< Mode 3 selection. */
+#define ME8254_CTRL_M4    0x08	 /**< Mode 4 selection. */
+#define ME8254_CTRL_M5    0x0A	 /**< Mode 5 selection. */
+
+#define ME8254_CTRL_BIN   0x00	 /**< Binary counter. */
+#define ME8254_CTRL_BCD   0x01	 /**< BCD counter. */
+
+/* ME-1400 A/B clock source register bits */
+#define ME1400AB_8254_A_0_CLK_SRC_1MHZ		(0 << 7)	/**< 1MHz clock. */
+#define ME1400AB_8254_A_0_CLK_SRC_10MHZ		(1 << 7)	/**< 10MHz clock. */
+#define ME1400AB_8254_A_0_CLK_SRC_PIN		(0 << 6)	/**< CLK 0 to SUB-D. */
+#define ME1400AB_8254_A_0_CLK_SRC_QUARZ		(1 << 6)	/**< Connect CLK 0 with quarz. */
+
+#define ME1400AB_8254_A_1_CLK_SRC_PIN		(0 << 5)	/**< CLK 1 to SUB-D. */
+#define ME1400AB_8254_A_1_CLK_SRC_PREV		(1 << 5)	/**< Connect OUT 0 with CLK 1. */
+
+#define ME1400AB_8254_A_2_CLK_SRC_PIN		(0 << 4)	/**< CLK 2 to SUB-D. */
+#define ME1400AB_8254_A_2_CLK_SRC_PREV		(1 << 4)	/**< Connect OUT 1 with CLK 2. */
+
+#define ME1400AB_8254_B_0_CLK_SRC_1MHZ		(0 << 3)	/**< 1MHz clock. */
+#define ME1400AB_8254_B_0_CLK_SRC_10MHZ		(1 << 3)	/**< 10MHz clock. */
+#define ME1400AB_8254_B_0_CLK_SRC_PIN		(0 << 2)	/**< CLK 0 to SUB-D. */
+#define ME1400AB_8254_B_0_CLK_SRC_QUARZ		(1 << 2)	/**< Connect CLK 0 with quarz. */
+
+#define ME1400AB_8254_B_1_CLK_SRC_PIN		(0 << 1)	/**< CLK 1 to SUB-D. */
+#define ME1400AB_8254_B_1_CLK_SRC_PREV		(1 << 1)	/**< Connect OUT 0 with CLK 1. */
+
+#define ME1400AB_8254_B_2_CLK_SRC_PIN		(0 << 0)	/**< CLK 2 to SUB-D. */
+#define ME1400AB_8254_B_2_CLK_SRC_PREV		(1 << 0)	/**< Connect OUT 1 with CLK 2. */
+
+/* ME-1400 C/D clock source registers bits */
+#define ME1400CD_8254_ACE_0_CLK_SRC_MASK	0x03	/**< Masks all CLK source bits. */
+#define ME1400CD_8254_ACE_0_CLK_SRC_PIN		0x00	/**< Connect CLK to SUB-D. */
+#define ME1400CD_8254_ACE_0_CLK_SRC_1MHZ	0x01	/**< Connect CLK to 1MHz. */
+#define ME1400CD_8254_ACE_0_CLK_SRC_10MHZ	0x02	/**< Connect CLK to 10MHz. */
+#define ME1400CD_8254_ACE_0_CLK_SRC_PREV	0x03	/**< Connect CLK to previous counter output on ME-1400 D extension. */
+
+#define ME1400CD_8254_ACE_1_CLK_SRC_MASK	0x04	/**< Masks all CLK source bits. */
+#define ME1400CD_8254_ACE_1_CLK_SRC_PIN		0x00	/**< Connect CLK to SUB-D. */
+#define ME1400CD_8254_ACE_1_CLK_SRC_PREV	0x04	/**< Connect CLK to previous counter output. */
+
+#define ME1400CD_8254_ACE_2_CLK_SRC_MASK	0x08	/**< Masks all CLK source bits. */
+#define ME1400CD_8254_ACE_2_CLK_SRC_PIN		0x00	/**< Connect to SUB-D. */
+#define ME1400CD_8254_ACE_2_CLK_SRC_PREV	0x08	/**< Connect CLK to previous counter output. */
+
+#define ME1400CD_8254_BD_0_CLK_SRC_MASK		0x30	/**< Masks all CLK source bits. */
+#define ME1400CD_8254_BD_0_CLK_SRC_PIN		0x00	/**< Connect CLK to SUB-D. */
+#define ME1400CD_8254_BD_0_CLK_SRC_1MHZ		0x10	/**< Connect CLK to 1MHz. */
+#define ME1400CD_8254_BD_0_CLK_SRC_10MHZ	0x20	/**< Connect CLK to 10MHz. */
+#define ME1400CD_8254_BD_0_CLK_SRC_PREV		0x30	/**< Connect CLK to previous counter output. */
+
+#define ME1400CD_8254_BD_1_CLK_SRC_MASK		0x40	/**< Masks all CLK source bits. */
+#define ME1400CD_8254_BD_1_CLK_SRC_PIN		0x00	/**< Connect CLK to SUB-D. */
+#define ME1400CD_8254_BD_1_CLK_SRC_PREV		0x40	/**< Connect CLK to previous counter output. */
+
+#define ME1400CD_8254_BD_2_CLK_SRC_MASK		0x80	/**< Masks all CLK source bits. */
+#define ME1400CD_8254_BD_2_CLK_SRC_PIN		0x00	/**< Connect CLK to SUB-D. */
+#define ME1400CD_8254_BD_2_CLK_SRC_PREV		0x80	/**< Connect CLK to previous counter output. */
+
+/* ME-8100 counter registers */
+#define ME8100_COUNTER_REG_0				0x18	//(r,w)
+#define ME8100_COUNTER_REG_1				0x1A	//(r,w)
+#define ME8100_COUNTER_REG_2				0x1C	//(r,w)
+#define ME8100_COUNTER_CTRL_REG				0x1E	//(r,w)
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me8255.c b/drivers/staging/meilhaus/me8255.c
new file mode 100644
index 00000000000..180e7f8d214
--- /dev/null
+++ b/drivers/staging/meilhaus/me8255.c
@@ -0,0 +1,462 @@
+/**
+ * @file me8255.c
+ *
+ * @brief 8255 subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+#include "medebug.h"
+
+#include "me8255_reg.h"
+#include "me8255.h"
+
+/*
+ * Defines
+ */
+
+/*
+ * Functions
+ */
+
+static uint8_t get_mode_from_mirror(uint32_t mirror)
+{
+	PDEBUG("executed.\n");
+
+	if (mirror & ME8255_PORT_0_OUTPUT) {
+		if (mirror & ME8255_PORT_1_OUTPUT) {
+			if (mirror & ME8255_PORT_2_OUTPUT) {
+				return ME8255_MODE_OOO;
+			} else {
+				return ME8255_MODE_IOO;
+			}
+		} else {
+			if (mirror & ME8255_PORT_2_OUTPUT) {
+				return ME8255_MODE_OIO;
+			} else {
+				return ME8255_MODE_IIO;
+			}
+		}
+	} else {
+		if (mirror & ME8255_PORT_1_OUTPUT) {
+			if (mirror & ME8255_PORT_2_OUTPUT) {
+				return ME8255_MODE_OOI;
+			} else {
+				return ME8255_MODE_IOI;
+			}
+		} else {
+			if (mirror & ME8255_PORT_2_OUTPUT) {
+				return ME8255_MODE_OII;
+			} else {
+				return ME8255_MODE_III;
+			}
+		}
+	}
+}
+
+static int me8255_io_reset_subdevice(struct me_subdevice *subdevice,
+				     struct file *filep, int flags)
+{
+	me8255_subdevice_t *instance;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8255_subdevice_t *) subdevice;
+
+	if (flags) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	spin_lock(instance->ctrl_reg_lock);
+	*instance->ctrl_reg_mirror &=
+	    ~(ME8255_PORT_0_OUTPUT << instance->dio_idx);
+	outb(get_mode_from_mirror(*instance->ctrl_reg_mirror),
+	     instance->ctrl_reg);
+	spin_unlock(instance->ctrl_reg_lock);
+
+	outb(0, instance->port_reg);
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8255_io_single_config(struct me_subdevice *subdevice,
+				   struct file *filep,
+				   int channel,
+				   int single_config,
+				   int ref,
+				   int trig_chan,
+				   int trig_type, int trig_edge, int flags)
+{
+	me8255_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8255_subdevice_t *) subdevice;
+
+	if (flags & ~ME_IO_SINGLE_CONFIG_DIO_BYTE) {
+		PERROR("Invalid flag specified.\n");
+		return ME_ERRNO_INVALID_FLAGS;
+	}
+
+	if (channel) {
+		PERROR("Invalid channel.\n");
+		return ME_ERRNO_INVALID_CHANNEL;
+	}
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	if (single_config == ME_SINGLE_CONFIG_DIO_INPUT) {
+		spin_lock(instance->ctrl_reg_lock);
+		*instance->ctrl_reg_mirror &=
+		    ~(ME8255_PORT_0_OUTPUT << instance->dio_idx);
+		outb(get_mode_from_mirror(*instance->ctrl_reg_mirror),
+		     instance->ctrl_reg);
+		spin_unlock(instance->ctrl_reg_lock);
+	} else if (single_config == ME_SINGLE_CONFIG_DIO_OUTPUT) {
+		spin_lock(instance->ctrl_reg_lock);
+		*instance->ctrl_reg_mirror |=
+		    (ME8255_PORT_0_OUTPUT << instance->dio_idx);
+		outb(get_mode_from_mirror(*instance->ctrl_reg_mirror),
+		     instance->ctrl_reg);
+		spin_unlock(instance->ctrl_reg_lock);
+	} else {
+		PERROR("Invalid port direction.\n");
+		err = ME_ERRNO_INVALID_SINGLE_CONFIG;
+	}
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8255_io_single_read(struct me_subdevice *subdevice,
+				 struct file *filep,
+				 int channel,
+				 int *value, int time_out, int flags)
+{
+	me8255_subdevice_t *instance;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8255_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			*value = inb(instance->port_reg) & (0x1 << channel);
+		} else {
+			PERROR("Invalid bit number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			*value = inb(instance->port_reg);
+		} else {
+			PERROR("Invalid byte number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8255_io_single_write(struct me_subdevice *subdevice,
+				  struct file *filep,
+				  int channel,
+				  int value, int time_out, int flags)
+{
+	me8255_subdevice_t *instance;
+	uint8_t byte;
+	int err = ME_ERRNO_SUCCESS;
+
+	PDEBUG("executed.\n");
+
+	instance = (me8255_subdevice_t *) subdevice;
+
+	ME_SUBDEVICE_ENTER;
+
+	spin_lock(&instance->subdevice_lock);
+	switch (flags) {
+	case ME_IO_SINGLE_TYPE_DIO_BIT:
+		if ((channel >= 0) && (channel < 8)) {
+			if (*instance->
+			    ctrl_reg_mirror & (ME8255_PORT_0_OUTPUT <<
+					       instance->dio_idx)) {
+				byte = inb(instance->port_reg);
+
+				if (value)
+					byte |= 0x1 << channel;
+				else
+					byte &= ~(0x1 << channel);
+
+				outb(byte, instance->port_reg);
+			} else {
+				PERROR("Port not in output mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid bit number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	case ME_IO_SINGLE_NO_FLAGS:
+	case ME_IO_SINGLE_TYPE_DIO_BYTE:
+		if (channel == 0) {
+			if (*instance->
+			    ctrl_reg_mirror & (ME8255_PORT_0_OUTPUT <<
+					       instance->dio_idx)) {
+				outb(value, instance->port_reg);
+			} else {
+				PERROR("Port not in output mode.\n");
+				err = ME_ERRNO_PREVIOUS_CONFIG;
+			}
+		} else {
+			PERROR("Invalid byte number.\n");
+			err = ME_ERRNO_INVALID_CHANNEL;
+		}
+		break;
+
+	default:
+		PERROR("Invalid flags specified.\n");
+		err = ME_ERRNO_INVALID_FLAGS;
+	}
+	spin_unlock(&instance->subdevice_lock);
+
+	ME_SUBDEVICE_EXIT;
+
+	return err;
+}
+
+static int me8255_query_number_channels(struct me_subdevice *subdevice,
+					int *number)
+{
+	PDEBUG("executed.\n");
+	*number = ME8255_NUMBER_CHANNELS;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8255_query_subdevice_type(struct me_subdevice *subdevice,
+				       int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = ME_TYPE_DIO;
+	*subtype = ME_SUBTYPE_SINGLE;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me8255_query_subdevice_caps(struct me_subdevice *subdevice,
+				       int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = ME_CAPS_DIO_DIR_BYTE;
+	return ME_ERRNO_SUCCESS;
+}
+
+me8255_subdevice_t *me8255_constructor(uint32_t device_id,
+				       uint32_t reg_base,
+				       unsigned int me8255_idx,
+				       unsigned int dio_idx,
+				       int *ctrl_reg_mirror,
+				       spinlock_t * ctrl_reg_lock)
+{
+	me8255_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(me8255_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for 8255 instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(me8255_subdevice_t));
+
+	/* Check if counter index is out of range */
+
+	if (dio_idx > 2) {
+		PERROR("DIO index is out of range.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	subdevice->ctrl_reg_lock = ctrl_reg_lock;
+
+	/* Save the pointer to global port settings */
+	subdevice->ctrl_reg_mirror = ctrl_reg_mirror;
+
+	/* Save type of Meilhaus device */
+	subdevice->device_id = device_id;
+
+	/* Save the indices */
+	subdevice->me8255_idx = me8255_idx;
+	subdevice->dio_idx = dio_idx;
+
+	/* Do device specific initialization */
+	switch (device_id) {
+	case PCI_DEVICE_ID_MEILHAUS_ME1400:
+	case PCI_DEVICE_ID_MEILHAUS_ME14E0:
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140A:
+	case PCI_DEVICE_ID_MEILHAUS_ME14EA:
+		/* Check if 8255 index is out of range */
+		if (me8255_idx > 0) {
+			PERROR("8255 index is out of range.\n");
+			me_subdevice_deinit(&subdevice->base);
+			kfree(subdevice);
+			return NULL;
+		}
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140B:	/* Fall through */
+	case PCI_DEVICE_ID_MEILHAUS_ME14EB:
+		/* Check if 8255 index is out of range */
+		if (me8255_idx > 1) {
+			PERROR("8255 index is out of range.\n");
+			me_subdevice_deinit(&subdevice->base);
+			kfree(subdevice);
+			return NULL;
+		}
+
+		/* Get the registers */
+		if (me8255_idx == 0) {
+			subdevice->ctrl_reg = reg_base + ME1400AB_PORT_A_CTRL;
+			subdevice->port_reg =
+			    reg_base + ME1400AB_PORT_A_0 + dio_idx;
+		} else if (me8255_idx == 1) {
+			subdevice->ctrl_reg = reg_base + ME1400AB_PORT_B_CTRL;
+			subdevice->port_reg =
+			    reg_base + ME1400AB_PORT_B_0 + dio_idx;
+		}
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140C:
+		/* Check if 8255 index is out of range */
+		if (me8255_idx > 0) {
+			PERROR("8255 index is out of range.\n");
+			me_subdevice_deinit(&subdevice->base);
+			kfree(subdevice);
+			return NULL;
+		}
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140D:	/* Fall through */
+		/* Check if 8255 index is out of range */
+		if (me8255_idx > 1) {
+			PERROR("8255 index is out of range.\n");
+			me_subdevice_deinit(&subdevice->base);
+			kfree(subdevice);
+			return NULL;
+		}
+
+		/* Get the registers */
+		if (me8255_idx == 0) {
+			subdevice->ctrl_reg = reg_base + ME1400CD_PORT_A_CTRL;
+			subdevice->port_reg =
+			    reg_base + ME1400CD_PORT_A_0 + dio_idx;
+		} else if (me8255_idx == 1) {
+			subdevice->ctrl_reg = reg_base + ME1400CD_PORT_B_CTRL;
+			subdevice->port_reg =
+			    reg_base + ME1400CD_PORT_B_0 + dio_idx;
+		}
+
+		break;
+
+	default:
+		PERROR("Unknown device type. dev ID: 0x%04x\n", device_id);
+
+		me_subdevice_deinit(&subdevice->base);
+
+		kfree(subdevice);
+
+		return NULL;
+	}
+
+	/* Overload subdevice base class methods. */
+	subdevice->base.me_subdevice_io_reset_subdevice =
+	    me8255_io_reset_subdevice;
+	subdevice->base.me_subdevice_io_single_config = me8255_io_single_config;
+	subdevice->base.me_subdevice_io_single_read = me8255_io_single_read;
+	subdevice->base.me_subdevice_io_single_write = me8255_io_single_write;
+	subdevice->base.me_subdevice_query_number_channels =
+	    me8255_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    me8255_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    me8255_query_subdevice_caps;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/me8255.h b/drivers/staging/meilhaus/me8255.h
new file mode 100644
index 00000000000..338230052b3
--- /dev/null
+++ b/drivers/staging/meilhaus/me8255.h
@@ -0,0 +1,59 @@
+/**
+ * @file me8255.h
+ *
+ * @brief Meilhaus PIO 8255 implementation.
+ * @note Copyright (C) 2006 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+#ifndef _ME8255_H_
+#define _ME8255_H_
+
+#include "mesubdevice.h"
+#include "meslock.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The 8255 subdevice class.
+ */
+typedef struct me8255_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+
+	int *ctrl_reg_mirror;			/**< Pointer to mirror of the control register. */
+	spinlock_t *ctrl_reg_lock;		/**< Spin lock to protect #ctrl_reg and #ctrl_reg_mirror from concurrent access. */
+
+	uint32_t device_id;				/**< The PCI device id of the device holding the 8255 chip. */
+	int me8255_idx;					/**< The index of the 8255 chip on the device. */
+	int dio_idx;					/**< The index of the DIO port on the 8255 chip. */
+
+	unsigned long port_reg;			/**< Register to read or write a value from or to the port respectively. */
+	unsigned long ctrl_reg;			/**< Register to configure the 8255 modes. */
+} me8255_subdevice_t;
+
+/**
+ * @brief The constructor to generate a 8255 instance.
+ *
+ * @param device_id The kind of Meilhaus device holding the 8255.
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ * @param me8255_idx The index of the 8255 chip on the Meilhaus device.
+ * @param dio_idx The index of the counter inside a 8255 chip.
+ * @param ctr_reg_mirror Pointer to mirror of control register.
+ * @param ctrl_reg_lock Pointer to spin lock protecting the 8255 control register and #ctrl_reg_mirror from concurrent access.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+me8255_subdevice_t *me8255_constructor(uint32_t device_id,
+				       uint32_t reg_base,
+				       unsigned int me8255_idx,
+				       unsigned int dio_idx,
+				       int *ctrl_reg_mirror,
+				       spinlock_t * ctrl_reg_lock);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/me8255_reg.h b/drivers/staging/meilhaus/me8255_reg.h
new file mode 100644
index 00000000000..d1dea1a447f
--- /dev/null
+++ b/drivers/staging/meilhaus/me8255_reg.h
@@ -0,0 +1,50 @@
+/**
+ * @file me8255_reg.h
+ *
+ * @brief 8255 counter register definitions.
+ * @note Copyright (C) 2006 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+#ifndef _ME8255_REG_H_
+#define _ME8255_REG_H_
+
+#ifdef __KERNEL__
+
+#define ME8255_NUMBER_CHANNELS		8		/**< The number of channels per 8255 port. */
+
+#define ME1400AB_PORT_A_0			0x0000	/**< Port 0 offset. */
+#define ME1400AB_PORT_A_1			0x0001	/**< Port 1 offset. */
+#define ME1400AB_PORT_A_2			0x0002	/**< Port 2 offset. */
+#define ME1400AB_PORT_A_CTRL		0x0003	/**< Control register for 8255 A. */
+
+#define ME1400AB_PORT_B_0			0x0008	/**< Port 0 offset. */
+#define ME1400AB_PORT_B_1			0x0009	/**< Port 1 offset. */
+#define ME1400AB_PORT_B_2			0x000A	/**< Port 2 offset. */
+#define ME1400AB_PORT_B_CTRL		0x000B	/**< Control register for 8255 B. */
+
+#define ME1400CD_PORT_A_0			0x0000	/**< Port 0 offset. */
+#define ME1400CD_PORT_A_1			0x0001	/**< Port 1 offset. */
+#define ME1400CD_PORT_A_2			0x0002	/**< Port 2 offset. */
+#define ME1400CD_PORT_A_CTRL		0x0003	/**< Control register for 8255 A. */
+
+#define ME1400CD_PORT_B_0			0x0040	/**< Port 0 offset. */
+#define ME1400CD_PORT_B_1			0x0041	/**< Port 1 offset. */
+#define ME1400CD_PORT_B_2			0x0042	/**< Port 2 offset. */
+#define ME1400CD_PORT_B_CTRL		0x0043	/**< Control register for 8255 B. */
+
+#define ME8255_MODE_OOO				0x80	/**< Port 2 = Output, Port 1 = Output, Port 0 = Output */
+#define ME8255_MODE_IOO				0x89	/**< Port 2 = Input,  Port 1 = Output, Port 0 = Output */
+#define ME8255_MODE_OIO				0x82	/**< Port 2 = Output, Port 1 = Input,  Port 0 = Output */
+#define ME8255_MODE_IIO				0x8B	/**< Port 2 = Input,  Port 1 = Input,  Port 0 = Output */
+#define ME8255_MODE_OOI				0x90	/**< Port 2 = Output, Port 1 = Output, Port 0 = Input */
+#define ME8255_MODE_IOI				0x99	/**< Port 2 = Input,  Port 1 = Output, Port 0 = Input */
+#define ME8255_MODE_OII				0x92	/**< Port 2 = Output, Port 1 = Input,  Port 0 = Input */
+#define ME8255_MODE_III				0x9B	/**< Port 2 = Input,  Port 1 = Input,  Port 0 = Input */
+
+#define ME8255_PORT_0_OUTPUT		0x1		/**< If set in mirror then port 0 is in output mode. */
+#define ME8255_PORT_1_OUTPUT		0x2		/**< If set in mirror then port 1 is in output mode. */
+#define ME8255_PORT_2_OUTPUT		0x4		/**< If set in mirror then port 2 is in output mode. */
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/mecirc_buf.h b/drivers/staging/meilhaus/mecirc_buf.h
new file mode 100644
index 00000000000..e9b591eaa34
--- /dev/null
+++ b/drivers/staging/meilhaus/mecirc_buf.h
@@ -0,0 +1,131 @@
+/**
+ * @file mecirc_buf.h
+ *
+ * @brief Meilhaus circular buffer implementation.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke  (k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _MECIRC_BUF_H_
+#define _MECIRC_BUF_H_
+
+# ifdef __KERNEL__
+
+#  ifdef BOSCH
+
+typedef struct me_circ_buf {
+	unsigned int mask;
+//      unsigned int count;
+	uint32_t *buf;
+	int volatile head;
+	int volatile tail;
+} me_circ_buf_t;
+
+static int inline me_circ_buf_values(me_circ_buf_t * buf)
+{
+//      return ((buf->head - buf->tail) & (buf->count - 1));
+	return ((buf->head - buf->tail) & (buf->mask));
+}
+
+static int inline me_circ_buf_space(me_circ_buf_t * buf)
+{
+//      return ((buf->tail - (buf->head + 1)) & (buf->count - 1));
+	return ((buf->tail - (buf->head + 1)) & (buf->mask));
+}
+
+static int inline me_circ_buf_values_to_end(me_circ_buf_t * buf)
+{
+	int end;
+	int n;
+//      end = buf->count - buf->tail;
+//      n = (buf->head + end) & (buf->count - 1);
+	end = buf->mask + 1 - buf->tail;
+	n = (buf->head + end) & (buf->mask);
+	return (n < end) ? n : end;
+}
+
+static int inline me_circ_buf_space_to_end(me_circ_buf_t * buf)
+{
+	int end;
+	int n;
+
+//      end = buf->count - 1 - buf->head;
+//      n = (end + buf->tail) & (buf->count - 1);
+	end = buf->mask - buf->head;
+	n = (end + buf->tail) & (buf->mask);
+	return (n <= end) ? n : (end + 1);
+}
+
+#define _CBUFF_32b_t
+
+#  else	//~BOSCH
+/// @note buf->mask = buf->count-1 = ME4600_AI_CIRC_BUF_COUNT-1
+
+#   ifdef _CBUFF_32b_t
+	//32 bit
+typedef struct me_circ_buf_32b {
+	int volatile head;
+	int volatile tail;
+	unsigned int mask;	//buffor size-1 must be 2^n-1 to work
+	uint32_t *buf;
+} me_circ_buf_t;
+#   else
+	//16 bit
+typedef struct me_circ_buf_16b {
+	int volatile head;
+	int volatile tail;
+	unsigned int mask;	//buffor size-1 must be 2^n-1 to work
+	uint16_t *buf;
+} me_circ_buf_t;
+#   endif //_CBUFF_32b_t
+
+/** How many values is in buffer */
+static int inline me_circ_buf_values(me_circ_buf_t * buf)
+{
+	return ((buf->head - buf->tail) & (buf->mask));
+}
+
+/** How many space left */
+static int inline me_circ_buf_space(me_circ_buf_t * buf)
+{
+	return ((buf->tail - (buf->head + 1)) & (buf->mask));
+}
+
+/** How many values can be read from buffor in one chunck. */
+static int inline me_circ_buf_values_to_end(me_circ_buf_t * buf)
+{
+	return (buf->tail <=
+		buf->head) ? (buf->head - buf->tail) : (buf->mask - buf->tail +
+							1);
+}
+
+/** How many values can be write to buffer in one chunck. */
+static int inline me_circ_buf_space_to_end(me_circ_buf_t * buf)
+{
+	return (buf->tail <=
+		buf->head) ? (buf->mask - buf->head + 1) : (buf->tail -
+							    buf->head - 1);
+}
+
+#  endif //BOSCH
+# endif	//__KERNEL__
+#endif //_MECIRC_BUF_H_
diff --git a/drivers/staging/meilhaus/mecommon.h b/drivers/staging/meilhaus/mecommon.h
new file mode 100644
index 00000000000..ef47c384e01
--- /dev/null
+++ b/drivers/staging/meilhaus/mecommon.h
@@ -0,0 +1,26 @@
+/*
+ * Copyright (C) 2005 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * Source File	:mecommon.h
+ * Author	:GG (Guenter Gebhardt)	<g.gebhardt@meilhaus.de>
+ * Author	:KG (Krzysztof Gantzke)	<k.gantzke@meilhaus.de>
+ */
+
+#ifndef _MECOMMON_H_
+#define _MECOMMON_H_
+
+/*==================================================================
+  The version of this release
+  ================================================================*/
+
+#ifndef ME_VERSION_DRIVER
+/* Unknown version */
+# define ME_VERSION_DRIVER	0xFFFFFFFF
+#endif
+
+#ifndef LIBMEDRIVER_VERSION
+/* Unknown version */
+# define LIBMEDRIVER_VERSION	0xFFFFFFFF
+#endif
+
+#endif
diff --git a/drivers/staging/meilhaus/medebug.h b/drivers/staging/meilhaus/medebug.h
new file mode 100644
index 00000000000..382d00fe311
--- /dev/null
+++ b/drivers/staging/meilhaus/medebug.h
@@ -0,0 +1,125 @@
+/**
+ * @file medebug.h
+ *
+ * @brief Debugging defines.
+ * @note Copyright (C) 2006 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+#ifndef _MEDEBUG_H_
+#define _MEDEBUG_H_
+
+#ifdef __KERNEL__
+
+#include <linux/kernel.h>
+
+//Messages control.
+
+#ifdef MEDEBUG_TEST_ALL		/* Switch to enable all info messages. */
+# ifndef MEDEBUG_TEST
+#  define MEDEBUG_TEST
+# endif
+# ifndef MEDEBUG_TEST_INFO
+#  define MEDEBUG_TEST_INFO
+# endif
+# ifndef MEDEBUG_DEBUG_REG
+#  define MEDEBUG_DEBUG_REG	/* Switch to enable registry access debuging messages. */
+# endif
+# ifndef MEDEBUG_DEBUG_LOCKS
+#  define MEDEBUG_DEBUG_LOCKS	/* Switch to enable locking messages. */
+# endif
+#endif
+
+#ifdef MEDEBUG_TEST_INFO	/* Switch to enable info  and test messages. */
+# ifndef MEDEBUG_INFO
+#  define MEDEBUG_INFO		/* Switch to enable info messages. */
+# endif
+# ifndef MEDEBUG_TEST
+#  define MEDEBUG_TEST
+# endif
+#endif
+
+#ifdef MEDEBUG_TEST		/* Switch to enable debug test messages. */
+# ifndef MEDEBUG_DEBUG
+#  define MEDEBUG_DEBUG		/* Switch to enable debug messages. */
+# endif
+# ifndef MEDEBUG_ERROR
+#  define MEDEBUG_ERROR		/* Switch to enable error messages. */
+# endif
+#endif
+
+#ifdef MEDEBUG_ERROR		/* Switch to enable error messages. */
+# ifndef MEDEBUG_ERROR_CRITICAL	/* Also critical error messages. */
+#  define MEDEBUG_ERROR_CRITICAL	/* Switch to enable high importance error messages. */
+# endif
+#endif
+
+#undef PDEBUG			/* Only to be sure. */
+#undef PINFO			/* Only to be sure. */
+#undef PERROR			/* Only to be sure. */
+#undef PERROR_CRITICAL		/* Only to be sure. */
+#undef PDEBUG_REG		/* Only to be sure. */
+#undef PDEBUG_LOCKS		/* Only to be sure. */
+#undef PSECURITY		/* Only to be sure. */
+#undef PLOG			/* Only to be sure. */
+
+#ifdef MEDEBUG_DEBUG
+# define PDEBUG(fmt, args...) \
+	printk(KERN_DEBUG"ME_DRV D: <%s> " fmt, __FUNCTION__, ##args)
+#else
+# define PDEBUG(fmt, args...)
+#endif
+
+#ifdef MEDEBUG_DEBUG_LOCKS
+# define PDEBUG_LOCKS(fmt, args...) \
+	printk(KERN_DEBUG"ME_DRV L: <%s> " fmt, __FUNCTION__, ##args)
+#else
+# define PDEBUG_LOCKS(fmt, args...)
+#endif
+
+#ifdef MEDEBUG_DEBUG_REG
+# define PDEBUG_REG(fmt, args...) \
+	printk(KERN_DEBUG"ME_DRV R: <%s:%d> REG:" fmt, __FUNCTION__, __LINE__, ##args)
+#else
+# define PDEBUG_REG(fmt, args...)
+#endif
+
+#ifdef MEDEBUG_INFO
+# define PINFO(fmt, args...) \
+	printk(KERN_INFO"ME_DRV I: " fmt, ##args)
+#else
+# define PINFO(fmt, args...)
+#endif
+
+#ifdef MEDEBUG_ERROR
+# define PERROR(fmt, args...) \
+	printk(KERN_ERR"ME_DRV E: <%s:%i> " fmt, __FILE__, __LINE__, ##args)
+#else
+# define PERROR(fmt, args...)
+#endif
+
+#ifdef MEDEBUG_ERROR_CRITICAL
+# define PERROR_CRITICAL(fmt, args...) \
+	printk(KERN_CRIT"ME_DRV C: <%s:%i> " fmt, __FILE__, __LINE__, ##args)
+#else
+# define PERROR_CRITICAL(fmt, args...)
+#endif
+
+//This debug is only to detect logical errors!
+# define PSECURITY(fmt, args...) \
+	printk(KERN_CRIT"ME_DRV SECURITY: <%s:%s:%i> " fmt, __FILE__, __FUNCTION__, __LINE__, ##args)
+//This debug is to keep track in customers' system
+# define PLOG(fmt, args...) \
+	printk(KERN_INFO"ME_DRV: " fmt, ##args)
+
+//This debug is to check new parts during development
+#ifdef MEDEBUG_DEVELOP
+# define PDEVELOP(fmt, args...) \
+	printk(KERN_CRIT"ME_DRV: <%s:%s:%i> " fmt, __FILE__, __FUNCTION__, __LINE__, ##args)
+#else
+# define PDEVELOP(fmt, args...)
+#endif
+
+#endif //__KERNEL__
+#endif //_MEDEBUG_H_
diff --git a/drivers/staging/meilhaus/medefines.h b/drivers/staging/meilhaus/medefines.h
new file mode 100644
index 00000000000..6158ef5b80e
--- /dev/null
+++ b/drivers/staging/meilhaus/medefines.h
@@ -0,0 +1,449 @@
+/*
+ * Copyright (C) 2005 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * Source File : medefines.h
+ * Author      : GG (Guenter Gebhardt)  <g.gebhardt@meilhaus.de>
+ * Author      : KG (Krzysztof Gantzke)  <k.gantzke@meilhaus.de>
+ */
+
+#ifndef _MEDEFINES_H_
+#define _MEDEFINES_H_
+
+/*==================================================================
+  General
+  ================================================================*/
+
+#define ME_VALUE_NOT_USED							0x0
+#define ME_VALUE_INVALID							~0x0
+
+/*==================================================================
+  Defines common to access functions
+  ================================================================*/
+
+#define ME_LOCK_RELEASE								0x00010001
+#define ME_LOCK_SET									0x00010002
+#define ME_LOCK_CHECK								0x00010003
+
+/*==================================================================
+  Defines meOpen function
+  ================================================================*/
+
+#define ME_OPEN_NO_FLAGS							0x0
+
+/*==================================================================
+  Defines meClose function
+  ================================================================*/
+
+#define ME_CLOSE_NO_FLAGS							0x0
+
+/*==================================================================
+  Defines meLockDriver function
+  ================================================================*/
+
+#define ME_LOCK_DRIVER_NO_FLAGS						0x0
+
+/*==================================================================
+  Defines meLockDevice function
+  ================================================================*/
+
+#define ME_LOCK_DEVICE_NO_FLAGS						0x0
+
+/*==================================================================
+  Defines meLockSubdevice function
+  ================================================================*/
+
+#define ME_LOCK_SUBDEVICE_NO_FLAGS					0x0
+
+
+/*==================================================================
+  Defines common to error functions
+  ================================================================*/
+
+#define ME_ERROR_MSG_MAX_COUNT						256
+
+#define ME_SWITCH_DISABLE							0x00020001
+#define ME_SWITCH_ENABLE							0x00020002
+
+/*==================================================================
+  Defines common to io functions
+  ================================================================*/
+
+#define ME_REF_DIO_FIFO_LOW							0x00030001
+#define ME_REF_DIO_FIFO_HIGH						0x00030002
+
+#define ME_REF_CTR_PREVIOUS							0x00040001
+#define ME_REF_CTR_INTERNAL_1MHZ					0x00040002
+#define ME_REF_CTR_INTERNAL_10MHZ					0x00040003
+#define ME_REF_CTR_EXTERNAL							0x00040004
+
+#define ME_REF_AI_GROUND							0x00050001
+#define ME_REF_AI_DIFFERENTIAL						0x00050002
+
+#define ME_REF_AO_GROUND							0x00060001
+
+#define ME_TRIG_CHAN_DEFAULT						0x00070001
+#define ME_TRIG_CHAN_SYNCHRONOUS					0x00070002
+
+#define ME_TRIG_TYPE_NONE							0x00000000
+#define ME_TRIG_TYPE_SW								0x00080001
+#define ME_TRIG_TYPE_THRESHOLD						0x00080002
+#define ME_TRIG_TYPE_WINDOW							0x00080003
+#define ME_TRIG_TYPE_EDGE							0x00080004
+#define ME_TRIG_TYPE_SLOPE							0x00080005
+#define ME_TRIG_TYPE_EXT_DIGITAL					0x00080006
+#define ME_TRIG_TYPE_EXT_ANALOG						0x00080007
+#define ME_TRIG_TYPE_PATTERN						0x00080008
+#define ME_TRIG_TYPE_TIMER							0x00080009
+#define ME_TRIG_TYPE_COUNT							0x0008000A
+#define ME_TRIG_TYPE_FOLLOW							0x0008000B
+
+#define ME_TRIG_EDGE_NONE							0x00000000
+#define ME_TRIG_EDGE_ABOVE							0x00090001
+#define ME_TRIG_EDGE_BELOW							0x00090002
+#define ME_TRIG_EDGE_ENTRY							0x00090003
+#define ME_TRIG_EDGE_EXIT							0x00090004
+#define ME_TRIG_EDGE_RISING							0x00090005
+#define ME_TRIG_EDGE_FALLING						0x00090006
+#define ME_TRIG_EDGE_ANY							0x00090007
+
+#define ME_TIMER_ACQ_START							0x000A0001
+#define ME_TIMER_SCAN_START							0x000A0002
+#define ME_TIMER_CONV_START							0x000A0003
+
+/*==================================================================
+  Defines for meIOFrequencyToTicks function
+  ================================================================*/
+
+#define ME_IO_FREQUENCY_TO_TICKS_NO_FLAGS			0x0
+
+/*==================================================================
+  Defines for meIOIrqStart function
+  ================================================================*/
+
+#define ME_IRQ_SOURCE_DIO_PATTERN					0x000B0001
+#define ME_IRQ_SOURCE_DIO_MASK						0x000B0002
+#define ME_IRQ_SOURCE_DIO_LINE						0x000B0003
+#define ME_IRQ_SOURCE_DIO_OVER_TEMP					0x000B0004
+
+#define ME_IRQ_EDGE_NOT_USED						0x00000000
+#define ME_IRQ_EDGE_RISING							0x000C0001
+#define ME_IRQ_EDGE_FALLING							0x000C0002
+#define ME_IRQ_EDGE_ANY								0x000C0003
+
+/*==================================================================
+  Defines for meIOIrqStart function
+  ================================================================*/
+
+#define ME_IO_IRQ_START_NO_FLAGS					0x000000
+#define ME_IO_IRQ_START_DIO_BIT						0x000001
+#define ME_IO_IRQ_START_DIO_BYTE					0x000002
+#define ME_IO_IRQ_START_DIO_WORD					0x000004
+#define ME_IO_IRQ_START_DIO_DWORD					0x000008
+#define ME_IO_IRQ_START_PATTERN_FILTERING			0x000010
+#define ME_IO_IRQ_START_EXTENDED_STATUS				0x000020
+
+/*==================================================================
+  Defines for meIOIrqWait function
+  ================================================================*/
+
+#define ME_IO_IRQ_WAIT_NO_FLAGS						0x000000
+#define ME_IO_IRQ_WAIT_NORMAL_STATUS				0x000001
+#define ME_IO_IRQ_WAIT_EXTENDED_STATUS				0x000002
+
+/*==================================================================
+  Defines for meIOIrqStop function
+  ================================================================*/
+
+#define ME_IO_IRQ_STOP_NO_FLAGS						0x000000
+
+/*==================================================================
+  Defines for meIOIrqSetCallback function
+  ================================================================*/
+
+#define ME_IO_IRQ_SET_CALLBACK_NO_FLAGS				0x0
+
+/*==================================================================
+  Defines for meIOResetDevice function
+  ================================================================*/
+
+#define ME_IO_RESET_DEVICE_NO_FLAGS					0x0
+
+/*==================================================================
+  Defines for meIOResetSubdevice function
+  ================================================================*/
+
+#define ME_IO_RESET_SUBDEVICE_NO_FLAGS				0x0
+
+/*==================================================================
+  Defines for meIOSingleConfig function
+  ================================================================*/
+
+#define ME_SINGLE_CONFIG_DIO_INPUT					0x000D0001
+#define ME_SINGLE_CONFIG_DIO_OUTPUT					0x000D0002
+#define ME_SINGLE_CONFIG_DIO_HIGH_IMPEDANCE			0x000D0003
+#define ME_SINGLE_CONFIG_DIO_SINK					0x000D0004
+#define ME_SINGLE_CONFIG_DIO_SOURCE					0x000D0005
+#define ME_SINGLE_CONFIG_DIO_MUX32M					0x000D0006
+#define ME_SINGLE_CONFIG_DIO_DEMUX32				0x000D0007
+#define ME_SINGLE_CONFIG_DIO_BIT_PATTERN			0x000D0008
+
+#define ME_SINGLE_CONFIG_CTR_8254_MODE_0			0x000E0001
+#define ME_SINGLE_CONFIG_CTR_8254_MODE_1			0x000E0002
+#define ME_SINGLE_CONFIG_CTR_8254_MODE_2			0x000E0003
+#define ME_SINGLE_CONFIG_CTR_8254_MODE_3			0x000E0004
+#define ME_SINGLE_CONFIG_CTR_8254_MODE_4			0x000E0005
+#define ME_SINGLE_CONFIG_CTR_8254_MODE_5			0x000E0006
+
+#define ME_IO_SINGLE_CONFIG_NO_FLAGS				0x00
+#define ME_IO_SINGLE_CONFIG_DIO_BIT					0x01
+#define ME_IO_SINGLE_CONFIG_DIO_BYTE				0x02
+#define ME_IO_SINGLE_CONFIG_DIO_WORD				0x04
+#define ME_IO_SINGLE_CONFIG_DIO_DWORD				0x08
+#define ME_IO_SINGLE_CONFIG_MULTISIG_LED_ON			0x10
+#define ME_IO_SINGLE_CONFIG_MULTISIG_LED_OFF		0x20
+#define ME_IO_SINGLE_CONFIG_AI_RMS					0x40
+#define ME_IO_SINGLE_CONFIG_CONTINUE				0x80
+
+/*==================================================================
+  Defines for meIOSingle function
+  ================================================================*/
+
+#define ME_IO_SINGLE_NO_FLAGS						0x0
+#define ME_IO_SINGLE_NONBLOCKING					0x20
+
+#define ME_DIR_INPUT								0x000F0001
+#define ME_DIR_OUTPUT								0x000F0002
+
+#define ME_IO_SINGLE_TYPE_NO_FLAGS					0x00
+#define ME_IO_SINGLE_TYPE_DIO_BIT					0x01
+#define ME_IO_SINGLE_TYPE_DIO_BYTE					0x02
+#define ME_IO_SINGLE_TYPE_DIO_WORD					0x04
+#define ME_IO_SINGLE_TYPE_DIO_DWORD					0x08
+#define ME_IO_SINGLE_TYPE_TRIG_SYNCHRONOUS			0x10
+#define ME_IO_SINGLE_TYPE_WRITE_NONBLOCKING			0x20
+
+/*==================================================================
+  Defines for meIOStreamConfig function
+  ================================================================*/
+
+#define ME_IO_STREAM_CONFIG_NO_FLAGS				0x0
+#define ME_IO_STREAM_CONFIG_BIT_PATTERN				0x1
+#define ME_IO_STREAM_CONFIG_WRAPAROUND				0x2
+#define ME_IO_STREAM_CONFIG_SAMPLE_AND_HOLD			0x4
+#define ME_IO_STREAM_CONFIG_HARDWARE_ONLY			0x8
+
+#define ME_IO_STREAM_CONFIG_TYPE_NO_FLAGS			0x0
+
+#define ME_IO_STREAM_TRIGGER_TYPE_NO_FLAGS			0x0
+
+/*==================================================================
+  Defines for meIOStreamRead function
+  ================================================================*/
+
+#define ME_READ_MODE_BLOCKING						0x00100001
+#define ME_READ_MODE_NONBLOCKING					0x00100002
+
+#define ME_IO_STREAM_READ_NO_FLAGS					0x0
+#define ME_IO_STREAM_READ_FRAMES					0x1
+
+/*==================================================================
+  Defines for meIOStreamWrite function
+  ================================================================*/
+
+#define ME_WRITE_MODE_BLOCKING						0x00110001
+#define ME_WRITE_MODE_NONBLOCKING					0x00110002
+#define ME_WRITE_MODE_PRELOAD						0x00110003
+
+#define ME_IO_STREAM_WRITE_NO_FLAGS					0x00000000
+
+/*==================================================================
+  Defines for meIOStreamStart function
+  ================================================================*/
+
+#define ME_IO_STREAM_START_NO_FLAGS					0x00000000
+
+#define ME_START_MODE_BLOCKING						0x00120001
+#define ME_START_MODE_NONBLOCKING					0x00120002
+
+#define ME_IO_STREAM_START_TYPE_NO_FLAGS			0x0
+#define ME_IO_STREAM_START_TYPE_TRIG_SYNCHRONOUS	0x1
+
+/*==================================================================
+  Defines for meIOStreamStop function
+  ================================================================*/
+
+#define ME_IO_STREAM_STOP_NO_FLAGS					0x00000000
+#define ME_IO_STREAM_STOP_PRESERVE_BUFFERS			0x00000001
+
+#define ME_STOP_MODE_IMMEDIATE						0x00130001
+#define ME_STOP_MODE_LAST_VALUE						0x00130002
+
+#define ME_IO_STREAM_STOP_TYPE_NO_FLAGS				0x00000000
+
+/*==================================================================
+  Defines for meIOStreamStatus function
+  ================================================================*/
+
+#define ME_WAIT_NONE								0x00140001
+#define ME_WAIT_IDLE								0x00140002
+
+#define ME_STATUS_INVALID							0x00000000
+#define ME_STATUS_IDLE								0x00150001
+#define ME_STATUS_BUSY								0x00150002
+#define ME_STATUS_ERROR								0x00150003
+
+#define ME_IO_STREAM_STATUS_NO_FLAGS				0x00000000
+
+/*==================================================================
+  Defines for meIOStreamSetCallbacks function
+  ================================================================*/
+
+#define ME_IO_STREAM_SET_CALLBACKS_NO_FLAGS			0x00000000
+
+/*==================================================================
+  Defines for meIOStreamNewValues function
+  ================================================================*/
+
+#define ME_IO_STREAM_NEW_VALUES_NO_FLAGS			0x00000000
+
+/*==================================================================
+  Defines for meIOTimeToTicks function
+  ================================================================*/
+
+#define ME_IO_STREAM_TIME_TO_TICKS_NO_FLAGS			0x00000000
+
+/*==================================================================
+  Defines for module types
+  ================================================================*/
+
+#define ME_MODULE_TYPE_MULTISIG_NONE				0x00000000
+#define ME_MODULE_TYPE_MULTISIG_DIFF16_10V			0x00160001
+#define ME_MODULE_TYPE_MULTISIG_DIFF16_20V			0x00160002
+#define ME_MODULE_TYPE_MULTISIG_DIFF16_50V			0x00160003
+#define ME_MODULE_TYPE_MULTISIG_CURRENT16_0_20MA	0x00160004
+#define ME_MODULE_TYPE_MULTISIG_RTD8_PT100			0x00160005
+#define ME_MODULE_TYPE_MULTISIG_RTD8_PT500			0x00160006
+#define ME_MODULE_TYPE_MULTISIG_RTD8_PT1000			0x00160007
+#define ME_MODULE_TYPE_MULTISIG_TE8_TYPE_B			0x00160008
+#define ME_MODULE_TYPE_MULTISIG_TE8_TYPE_E			0x00160009
+#define ME_MODULE_TYPE_MULTISIG_TE8_TYPE_J			0x0016000A
+#define ME_MODULE_TYPE_MULTISIG_TE8_TYPE_K			0x0016000B
+#define ME_MODULE_TYPE_MULTISIG_TE8_TYPE_N			0x0016000C
+#define ME_MODULE_TYPE_MULTISIG_TE8_TYPE_R			0x0016000D
+#define ME_MODULE_TYPE_MULTISIG_TE8_TYPE_S			0x0016000E
+#define ME_MODULE_TYPE_MULTISIG_TE8_TYPE_T			0x0016000F
+#define ME_MODULE_TYPE_MULTISIG_TE8_TEMP_SENSOR		0x00160010
+
+/*==================================================================
+  Defines for meQuerySubdeviceCaps function
+  ================================================================*/
+
+#define ME_CAPS_NONE								0x00000000
+
+#define ME_CAPS_DIO_DIR_BIT							0x00000001
+#define ME_CAPS_DIO_DIR_BYTE						0x00000002
+#define ME_CAPS_DIO_DIR_WORD						0x00000004
+#define ME_CAPS_DIO_DIR_DWORD						0x00000008
+#define ME_CAPS_DIO_SINK_SOURCE						0x00000010
+#define ME_CAPS_DIO_BIT_PATTERN_IRQ					0x00000020
+#define ME_CAPS_DIO_BIT_MASK_IRQ_EDGE_RISING		0x00000040
+#define ME_CAPS_DIO_BIT_MASK_IRQ_EDGE_FALLING		0x00000080
+#define ME_CAPS_DIO_BIT_MASK_IRQ_EDGE_ANY			0x00000100
+#define ME_CAPS_DIO_OVER_TEMP_IRQ					0x00000200
+
+#define ME_CAPS_CTR_CLK_PREVIOUS					0x00000001
+#define ME_CAPS_CTR_CLK_INTERNAL_1MHZ				0x00000002
+#define ME_CAPS_CTR_CLK_INTERNAL_10MHZ				0x00000004
+#define ME_CAPS_CTR_CLK_EXTERNAL					0x00000008
+
+#define ME_CAPS_AI_TRIG_SYNCHRONOUS					0x00000001
+/// @note Backward compatibility for me1600 in old style.
+#define ME_CAPS_AI_TRIG_SIMULTANEOUS				ME_CAPS_AI_TRIG_SYNCHRONOUS
+#define ME_CAPS_AI_FIFO								0x00000002
+#define ME_CAPS_AI_FIFO_THRESHOLD					0x00000004
+
+#define ME_CAPS_AO_TRIG_SYNCHRONOUS					0x00000001
+/// @note Backward compatibility for me1600 in old style.
+#define ME_CAPS_AO_TRIG_SIMULTANEOUS				ME_CAPS_AO_TRIG_SYNCHRONOUS
+#define ME_CAPS_AO_FIFO								0x00000002
+#define ME_CAPS_AO_FIFO_THRESHOLD					0x00000004
+
+#define ME_CAPS_EXT_IRQ_EDGE_RISING					0x00000001
+#define ME_CAPS_EXT_IRQ_EDGE_FALLING				0x00000002
+#define ME_CAPS_EXT_IRQ_EDGE_ANY					0x00000004
+
+/*==================================================================
+  Defines for meQuerySubdeviceCapsArgs function
+  ================================================================*/
+
+#define ME_CAP_AI_FIFO_SIZE							0x001D0000
+#define ME_CAP_AI_BUFFER_SIZE						0x001D0001
+
+#define ME_CAP_AO_FIFO_SIZE							0x001F0000
+#define ME_CAP_AO_BUFFER_SIZE						0x001F0001
+
+#define ME_CAP_CTR_WIDTH							0x00200000
+
+/*==================================================================
+  Defines common to query functions
+  ================================================================*/
+
+#define ME_UNIT_INVALID								0x00000000
+#define ME_UNIT_VOLT								0x00170001
+#define ME_UNIT_AMPERE								0x00170002
+#define ME_UNIT_ANY									0x00170003
+
+#define ME_TYPE_INVALID								0x00000000
+#define ME_TYPE_AO									0x00180001
+#define ME_TYPE_AI									0x00180002
+#define ME_TYPE_DIO									0x00180003
+#define ME_TYPE_DO									0x00180004
+#define ME_TYPE_DI									0x00180005
+#define ME_TYPE_CTR									0x00180006
+#define ME_TYPE_EXT_IRQ								0x00180007
+
+#define ME_SUBTYPE_INVALID							0x00000000
+#define ME_SUBTYPE_SINGLE							0x00190001
+#define ME_SUBTYPE_STREAMING						0x00190002
+#define ME_SUBTYPE_CTR_8254							0x00190003
+#define ME_SUBTYPE_ANY								0x00190004
+
+#define ME_DEVICE_DRIVER_NAME_MAX_COUNT				64
+#define ME_DEVICE_NAME_MAX_COUNT					64
+
+#define ME_DEVICE_DESCRIPTION_MAX_COUNT				256
+
+#define ME_BUS_TYPE_INVALID							0x00000000
+#define ME_BUS_TYPE_PCI								0x001A0001
+#define ME_BUS_TYPE_USB								0x001A0002
+
+#define ME_PLUGGED_INVALID							0x00000000
+#define ME_PLUGGED_IN								0x001B0001
+#define ME_PLUGGED_OUT								0x001B0002
+
+#define ME_EXTENSION_TYPE_INVALID					0x00000000
+#define ME_EXTENSION_TYPE_NONE						0x001C0001
+#define ME_EXTENSION_TYPE_MUX32M					0x001C0002
+#define ME_EXTENSION_TYPE_DEMUX32					0x001C0003
+#define ME_EXTENSION_TYPE_MUX32S					0x001C0004
+
+#define ME_ACCESS_TYPE_INVALID						0x00000000
+#define ME_ACCESS_TYPE_LOCAL						0x001D0001
+#define ME_ACCESS_TYPE_REMOTE						0x001D0002
+
+/// @note Add by KG
+
+/*==================================================================
+  Defines for meUtilityPWM
+  ================================================================*/
+#define ME_PWM_START_CONNECT_INTERNAL				0x00200001
+
+/* Flags for SingleConfig channels configure */
+#define ME_SINGLE_CHANNEL_NOT_CONFIGURED			0x00
+#define ME_SINGLE_CHANNEL_CONFIGURED				0x01
+
+/* Define if configuration should be downloaded to driver */
+#define ME_CONFIG_LOAD_NO_FLAGS						0x0
+#define ME_CONFIG_LOAD_TO_DRIVER					0x1
+
+#endif
diff --git a/drivers/staging/meilhaus/medevice.c b/drivers/staging/meilhaus/medevice.c
new file mode 100644
index 00000000000..8f62e16c7a3
--- /dev/null
+++ b/drivers/staging/meilhaus/medevice.c
@@ -0,0 +1,1740 @@
+/**
+ * @file medevice.c
+ *
+ * @brief Meilhaus device base class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include "mecommon.h"
+#include "meinternal.h"
+#include "medefines.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "medevice.h"
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+static int me_device_io_irq_start(struct me_device *device,
+				  struct file *filep,
+				  int subdevice,
+				  int channel,
+				  int irq_source,
+				  int irq_edge, int irq_arg, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Enter device.
+	err = me_dlock_enter(&device->dlock, filep);
+
+	if (err) {
+		PERROR("Cannot enter device.\n");
+		return err;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_io_irq_start(s,
+						   filep,
+						   channel,
+						   irq_source,
+						   irq_edge, irq_arg, flags);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	// Exit device.
+	me_dlock_exit(&device->dlock, filep);
+
+	return err;
+}
+
+static int me_device_io_irq_wait(struct me_device *device,
+				 struct file *filep,
+				 int subdevice,
+				 int channel,
+				 int *irq_count,
+				 int *value, int time_out, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Enter device.
+	err = me_dlock_enter(&device->dlock, filep);
+
+	if (err) {
+		PERROR("Cannot enter device.\n");
+		return err;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_io_irq_wait(s,
+						  filep,
+						  channel,
+						  irq_count,
+						  value, time_out, flags);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	// Exit device.
+	me_dlock_exit(&device->dlock, filep);
+
+	return err;
+}
+
+static int me_device_io_irq_stop(struct me_device *device,
+				 struct file *filep,
+				 int subdevice, int channel, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Enter device.
+	err = me_dlock_enter(&device->dlock, filep);
+
+	if (err) {
+		PERROR("Cannot enter device.\n");
+		return err;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_io_irq_stop(s, filep, channel, flags);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	// Exit device.
+	me_dlock_exit(&device->dlock, filep);
+
+	return err;
+}
+
+static int me_device_io_reset_device(struct me_device *device,
+				     struct file *filep, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+	int i, n;
+
+	PDEBUG("executed.\n");
+
+	/* Get the number of subdevices. */
+	n = me_slist_get_number_subdevices(&device->slist);
+
+	// Enter device.
+	err = me_dlock_enter(&device->dlock, filep);
+
+	if (err) {
+		PERROR("Cannot enter device.\n");
+		return err;
+	}
+
+	/* Reset every subdevice in list. */
+	for (i = 0; i < n; i++) {
+		s = me_slist_get_subdevice(&device->slist, i);
+		err = s->me_subdevice_io_reset_subdevice(s, filep, flags);
+
+		if (err) {
+			PERROR("Cannot reset subdevice.\n");
+			break;
+		}
+	}
+
+	// Exit device.
+	me_dlock_exit(&device->dlock, filep);
+
+	return err;
+}
+
+static int me_device_io_reset_subdevice(struct me_device *device,
+					struct file *filep,
+					int subdevice, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Enter device.
+	err = me_dlock_enter(&device->dlock, filep);
+
+	if (err) {
+		PERROR("Cannot enter device.\n");
+		return err;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_io_reset_subdevice(s, filep, flags);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	// Exit device.
+	me_dlock_exit(&device->dlock, filep);
+
+	return err;
+}
+
+static int me_device_io_single_config(struct me_device *device,
+				      struct file *filep,
+				      int subdevice,
+				      int channel,
+				      int single_config,
+				      int ref,
+				      int trig_chan,
+				      int trig_type, int trig_edge, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Enter device.
+	err = me_dlock_enter(&device->dlock, filep);
+
+	if (err) {
+		PERROR("Cannot enter device.\n");
+		return err;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_io_single_config(s,
+						       filep,
+						       channel,
+						       single_config,
+						       ref,
+						       trig_chan,
+						       trig_type,
+						       trig_edge, flags);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	// Exit device.
+	me_dlock_exit(&device->dlock, filep);
+
+	return err;
+}
+
+static int me_device_io_single_read(struct me_device *device,
+				    struct file *filep,
+				    int subdevice,
+				    int channel,
+				    int *value, int time_out, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Enter device.
+	err = me_dlock_enter(&device->dlock, filep);
+
+	if (err) {
+		PERROR("Cannot enter device.\n");
+		return err;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_io_single_read(s,
+						     filep,
+						     channel,
+						     value, time_out, flags);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	// Exit device.
+	me_dlock_exit(&device->dlock, filep);
+
+	return err;
+}
+
+static int me_device_io_single_write(struct me_device *device,
+				     struct file *filep,
+				     int subdevice,
+				     int channel,
+				     int value, int time_out, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Enter device.
+	err = me_dlock_enter(&device->dlock, filep);
+
+	if (err) {
+		PERROR("Cannot enter device.\n");
+		return err;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_io_single_write(s,
+						      filep,
+						      channel,
+						      value, time_out, flags);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	// Exit device.
+	me_dlock_exit(&device->dlock, filep);
+
+	return err;
+}
+
+static int me_device_io_stream_config(struct me_device *device,
+				      struct file *filep,
+				      int subdevice,
+				      meIOStreamConfig_t * config_list,
+				      int count,
+				      meIOStreamTrigger_t * trigger,
+				      int fifo_irq_threshold, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Enter device.
+	err = me_dlock_enter(&device->dlock, filep);
+
+	if (err) {
+		PERROR("Cannot enter device.\n");
+		return err;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_io_stream_config(s,
+						       filep,
+						       config_list,
+						       count,
+						       trigger,
+						       fifo_irq_threshold,
+						       flags);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	// Exit device.
+	me_dlock_exit(&device->dlock, filep);
+
+	return err;
+}
+
+static int me_device_io_stream_new_values(struct me_device *device,
+					  struct file *filep,
+					  int subdevice,
+					  int time_out, int *count, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Enter device.
+	err = me_dlock_enter(&device->dlock, filep);
+
+	if (err) {
+		PERROR("Cannot enter device.\n");
+		return err;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_io_stream_new_values(s,
+							   filep,
+							   time_out,
+							   count, flags);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	// Exit device.
+	me_dlock_exit(&device->dlock, filep);
+
+	return err;
+}
+
+static int me_device_io_stream_read(struct me_device *device,
+				    struct file *filep,
+				    int subdevice,
+				    int read_mode,
+				    int *values, int *count, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Enter device.
+	err = me_dlock_enter(&device->dlock, filep);
+
+	if (err) {
+		PERROR("Cannot enter device.\n");
+		return err;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_io_stream_read(s,
+						     filep,
+						     read_mode,
+						     values, count, flags);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	// Exit device.
+	me_dlock_exit(&device->dlock, filep);
+
+	return err;
+}
+
+static int me_device_io_stream_start(struct me_device *device,
+				     struct file *filep,
+				     int subdevice,
+				     int start_mode, int time_out, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Enter device.
+	err = me_dlock_enter(&device->dlock, filep);
+
+	if (err) {
+		PERROR("Cannot enter device.\n");
+		return err;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_io_stream_start(s,
+						      filep,
+						      start_mode,
+						      time_out, flags);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	// Exit device.
+	me_dlock_exit(&device->dlock, filep);
+
+	return err;
+}
+
+static int me_device_io_stream_status(struct me_device *device,
+				      struct file *filep,
+				      int subdevice,
+				      int wait,
+				      int *status, int *count, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Enter device.
+	err = me_dlock_enter(&device->dlock, filep);
+
+	if (err) {
+		PERROR("Cannot enter device.\n");
+		return err;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_io_stream_status(s,
+						       filep,
+						       wait,
+						       status, count, flags);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	// Exit device.
+	me_dlock_exit(&device->dlock, filep);
+
+	return err;
+}
+
+static int me_device_io_stream_stop(struct me_device *device,
+				    struct file *filep,
+				    int subdevice, int stop_mode, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Enter device.
+	err = me_dlock_enter(&device->dlock, filep);
+
+	if (err) {
+		PERROR("Cannot enter device.\n");
+		return err;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_io_stream_stop(s,
+						     filep, stop_mode, flags);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	// Exit device.
+	me_dlock_exit(&device->dlock, filep);
+
+	return err;
+}
+
+static int me_device_io_stream_write(struct me_device *device,
+				     struct file *filep,
+				     int subdevice,
+				     int write_mode,
+				     int *values, int *count, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Enter device.
+	err = me_dlock_enter(&device->dlock, filep);
+
+	if (err) {
+		PERROR("Cannot enter device.\n");
+		return err;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_io_stream_write(s,
+						      filep,
+						      write_mode,
+						      values, count, flags);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	// Exit device.
+	me_dlock_exit(&device->dlock, filep);
+
+	return err;
+}
+
+static int me_device_lock_device(struct me_device *device,
+				 struct file *filep, int lock, int flags)
+{
+	PDEBUG("executed.\n");
+
+	return me_dlock_lock(&device->dlock,
+			     filep, lock, flags, &device->slist);
+}
+
+static int me_device_lock_subdevice(struct me_device *device,
+				    struct file *filep,
+				    int subdevice, int lock, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Enter device.
+	err = me_dlock_enter(&device->dlock, filep);
+
+	if (err) {
+		PERROR("Cannot enter device.\n");
+		return err;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_lock_subdevice(s, filep, lock, flags);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	// Exit device.
+	me_dlock_exit(&device->dlock, filep);
+
+	return err;
+}
+
+static int me_device_query_description_device(struct me_device *device,
+					      char **description)
+{
+	PDEBUG("executed.\n");
+	*description = device->device_description;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me_device_query_info_device(struct me_device *device,
+				       int *vendor_id,
+				       int *device_id,
+				       int *serial_no,
+				       int *bus_type,
+				       int *bus_no,
+				       int *dev_no, int *func_no, int *plugged)
+{
+	PDEBUG("executed.\n");
+
+	if (device->bus_type == ME_BUS_TYPE_PCI) {
+		*vendor_id = device->info.pci.vendor_id;
+		*device_id = device->info.pci.device_id;
+		*serial_no = device->info.pci.serial_no;
+		*bus_type = ME_BUS_TYPE_PCI;
+		*bus_no = device->info.pci.pci_bus_no;
+		*dev_no = device->info.pci.pci_dev_no;
+		*func_no = device->info.pci.pci_func_no;
+		*plugged = ME_PLUGGED_IN;
+	} else {
+		*plugged = ME_PLUGGED_OUT;
+	}
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me_device_query_name_device(struct me_device *device, char **name)
+{
+	PDEBUG("executed.\n");
+	*name = device->device_name;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me_device_query_name_device_driver(struct me_device *device,
+					      char **name)
+{
+	PDEBUG("executed.\n");
+	*name = device->driver_name;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me_device_query_number_subdevices(struct me_device *device,
+					     int *number)
+{
+	PDEBUG("executed.\n");
+	return me_slist_query_number_subdevices(&device->slist, number);
+}
+
+static int me_device_query_number_channels(struct me_device *device,
+					   int subdevice, int *number)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_query_number_channels(s, number);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	return err;
+}
+
+static int me_device_query_number_ranges(struct me_device *device,
+					 int subdevice, int unit, int *count)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_query_number_ranges(s, unit, count);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	return err;
+}
+
+static int me_device_query_range_by_min_max(struct me_device *device,
+					    int subdevice,
+					    int unit,
+					    int *min,
+					    int *max, int *maxdata, int *range)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_query_range_by_min_max(s,
+							     unit,
+							     min,
+							     max,
+							     maxdata, range);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	return err;
+}
+
+static int me_device_query_range_info(struct me_device *device,
+				      int subdevice,
+				      int range,
+				      int *unit,
+				      int *min, int *max, int *maxdata)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_query_range_info(s,
+						       range,
+						       unit, min, max, maxdata);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	return err;
+}
+
+static int me_device_query_subdevice_by_type(struct me_device *device,
+					     int start_subdevice,
+					     int type,
+					     int subtype, int *subdevice)
+{
+	PDEBUG("executed.\n");
+
+	return me_slist_get_subdevice_by_type(&device->slist,
+					      start_subdevice,
+					      type, subtype, subdevice);
+}
+
+static int me_device_query_subdevice_type(struct me_device *device,
+					  int subdevice,
+					  int *type, int *subtype)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_query_subdevice_type(s, type, subtype);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	return err;
+}
+
+static int me_device_query_subdevice_caps(struct me_device *device,
+					  int subdevice, int *caps)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_query_subdevice_caps(s, caps);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	return err;
+}
+
+static int me_device_query_subdevice_caps_args(struct me_device *device,
+					       int subdevice,
+					       int cap, int *args, int count)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_query_subdevice_caps_args(s,
+								cap,
+								args, count);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	return err;
+}
+
+static int me_device_query_timer(struct me_device *device,
+				 int subdevice,
+				 int timer,
+				 int *base_frequency,
+				 uint64_t * min_ticks, uint64_t * max_ticks)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_subdevice_t *s;
+
+	PDEBUG("executed.\n");
+
+	// Check subdevice index.
+
+	if (subdevice >= me_slist_get_number_subdevices(&device->slist)) {
+		PERROR("Invalid subdevice.\n");
+		return ME_ERRNO_INVALID_SUBDEVICE;
+	}
+	// Get subdevice instance.
+	s = me_slist_get_subdevice(&device->slist, subdevice);
+
+	if (s) {
+		// Call subdevice method.
+		err = s->me_subdevice_query_timer(s,
+						  timer,
+						  base_frequency,
+						  min_ticks, max_ticks);
+	} else {
+		// Something really bad happened.
+		PERROR("Cannot get subdevice instance.\n");
+		err = ME_ERRNO_INTERNAL;
+	}
+
+	return err;
+}
+
+static int me_device_query_version_device_driver(struct me_device *device,
+						 int *version)
+/** @todo Versions shold be read from driver. I must overwrite this function in each module. Here should be returned an error!
+*/
+{
+	PDEBUG("executed.\n");
+	*version = ME_VERSION_DRIVER;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me_device_config_load(struct me_device *device, struct file *filep,
+				 me_cfg_device_entry_t * config)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_SUCCESS;	//If no need for config return success.
+//      return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static void me_device_destructor(me_device_t * me_device)
+{
+	PDEBUG("executed.\n");
+	me_device_deinit(me_device);
+	kfree(me_device);
+}
+
+/* //me_device_usb_init
+int me_device_usb_init(me_device_t *me_device, struct usb_interface *interface)
+{
+	PDEBUG("executed.\n");
+	return -1;
+}
+*/
+
+static int get_device_descriptions(uint16_t device_id,
+				   char **device_name,
+				   char **device_description,
+				   char **driver_name)
+/** @todo This is wrong concept! Static table has too strong limitations!
+* 'device_name' and 'driver_name' should be calculated from 'device_id'
+* 'device_description' should be read from device or moved to user space and handled by library!
+*/
+{
+	PDEBUG("executed.\n");
+
+	switch (device_id) {
+	case PCI_DEVICE_ID_MEILHAUS_ME1000:
+	case PCI_DEVICE_ID_MEILHAUS_ME1000_A:
+	case PCI_DEVICE_ID_MEILHAUS_ME1000_B:
+		*device_name = ME1000_NAME_DEVICE_ME1000;
+		*device_description = ME1000_DESCRIPTION_DEVICE_ME1000;
+		*driver_name = ME1000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1400:
+		*device_name = ME1400_NAME_DEVICE_ME1400;
+		*device_description = ME1400_DESCRIPTION_DEVICE_ME1400;
+		*driver_name = ME1400_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140A:
+		*device_name = ME1400_NAME_DEVICE_ME1400A;
+		*device_description = ME1400_DESCRIPTION_DEVICE_ME1400A;
+		*driver_name = ME1400_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140B:
+		*device_name = ME1400_NAME_DEVICE_ME1400B;
+		*device_description = ME1400_DESCRIPTION_DEVICE_ME1400B;
+		*driver_name = ME1400_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME14E0:
+		*device_name = ME1400_NAME_DEVICE_ME1400E;
+		*device_description = ME1400_DESCRIPTION_DEVICE_ME1400E;
+		*driver_name = ME1400_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME14EA:
+		*device_name = ME1400_NAME_DEVICE_ME1400EA;
+		*device_description = ME1400_DESCRIPTION_DEVICE_ME1400EA;
+		*driver_name = ME1400_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME14EB:
+		*device_name = ME1400_NAME_DEVICE_ME1400EB;
+		*device_description = ME1400_DESCRIPTION_DEVICE_ME1400EB;
+		*driver_name = ME1400_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140C:
+		*device_name = ME1400_NAME_DEVICE_ME1400C;
+		*device_description = ME1400_DESCRIPTION_DEVICE_ME1400C;
+		*driver_name = ME1400_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140D:
+		*device_name = ME1400_NAME_DEVICE_ME1400D;
+		*device_description = ME1400_DESCRIPTION_DEVICE_ME1400D;
+		*driver_name = ME1400_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1600_4U:
+		*device_name = ME1600_NAME_DEVICE_ME16004U;
+		*device_description = ME1600_DESCRIPTION_DEVICE_ME16004U;
+		*driver_name = ME1600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1600_8U:
+		*device_name = ME1600_NAME_DEVICE_ME16008U;
+		*device_description = ME1600_DESCRIPTION_DEVICE_ME16008U;
+		*driver_name = ME1600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1600_12U:
+		*device_name = ME1600_NAME_DEVICE_ME160012U;
+		*device_description = ME1600_DESCRIPTION_DEVICE_ME160012U;
+		*driver_name = ME1600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1600_16U:
+		*device_name = ME1600_NAME_DEVICE_ME160016U;
+		*device_description = ME1600_DESCRIPTION_DEVICE_ME160016U;
+		*driver_name = ME1600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1600_16U_8I:
+		*device_name = ME1600_NAME_DEVICE_ME160016U8I;
+		*device_description = ME1600_DESCRIPTION_DEVICE_ME160016U8I;
+		*driver_name = ME1600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4610:
+		*device_name = ME4600_NAME_DEVICE_ME4610;
+		*device_description = ME4600_DESCRIPTION_DEVICE_ME4610;
+		*driver_name = ME4600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4650:
+		*device_name = ME4600_NAME_DEVICE_ME4650;
+		*device_description = ME4600_DESCRIPTION_DEVICE_ME4650;
+		*driver_name = ME4600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4660:
+		*device_name = ME4600_NAME_DEVICE_ME4660;
+		*device_description = ME4600_DESCRIPTION_DEVICE_ME4660;
+		*driver_name = ME4600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4660I:
+		*device_name = ME4600_NAME_DEVICE_ME4660I;
+		*device_description = ME4600_DESCRIPTION_DEVICE_ME4660I;
+		*driver_name = ME4600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4660S:
+		*device_name = ME4600_NAME_DEVICE_ME4660S;
+		*device_description = ME4600_DESCRIPTION_DEVICE_ME4660S;
+		*driver_name = ME4600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4660IS:
+		*device_name = ME4600_NAME_DEVICE_ME4660IS;
+		*device_description = ME4600_DESCRIPTION_DEVICE_ME4660IS;
+		*driver_name = ME4600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4670:
+		*device_name = ME4600_NAME_DEVICE_ME4670;
+		*device_description = ME4600_DESCRIPTION_DEVICE_ME4670;
+		*driver_name = ME4600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4670I:
+		*device_name = ME4600_NAME_DEVICE_ME4670I;
+		*device_description = ME4600_DESCRIPTION_DEVICE_ME4670I;
+		*driver_name = ME4600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4670S:
+		*device_name = ME4600_NAME_DEVICE_ME4670S;
+		*device_description = ME4600_DESCRIPTION_DEVICE_ME4670S;
+		*driver_name = ME4600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4670IS:
+		*device_name = ME4600_NAME_DEVICE_ME4670IS;
+		*device_description = ME4600_DESCRIPTION_DEVICE_ME4670IS;
+		*driver_name = ME4600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4680:
+		*device_name = ME4600_NAME_DEVICE_ME4680;
+		*device_description = ME4600_DESCRIPTION_DEVICE_ME4680;
+		*driver_name = ME4600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4680I:
+		*device_name = ME4600_NAME_DEVICE_ME4680I;
+		*device_description = ME4600_DESCRIPTION_DEVICE_ME4680I;
+		*driver_name = ME4600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4680S:
+		*device_name = ME4600_NAME_DEVICE_ME4680S;
+		*device_description = ME4600_DESCRIPTION_DEVICE_ME4680S;
+		*driver_name = ME4600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4680IS:
+		*device_name = ME4600_NAME_DEVICE_ME4680IS;
+		*device_description = ME4600_DESCRIPTION_DEVICE_ME4680IS;
+		*driver_name = ME4600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6004:
+		*device_name = ME6000_NAME_DEVICE_ME60004;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME60004;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6008:
+		*device_name = ME6000_NAME_DEVICE_ME60008;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME60008;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME600F:
+		*device_name = ME6000_NAME_DEVICE_ME600016;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME600016;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6014:
+		*device_name = ME6000_NAME_DEVICE_ME6000I4;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6000I4;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6018:
+		*device_name = ME6000_NAME_DEVICE_ME6000I8;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6000I8;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME601F:
+		*device_name = ME6000_NAME_DEVICE_ME6000I16;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6000I16;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6034:
+		*device_name = ME6000_NAME_DEVICE_ME6000ISLE4;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6000ISLE4;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6038:
+		*device_name = ME6000_NAME_DEVICE_ME6000ISLE8;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6000ISLE8;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME603F:
+		*device_name = ME6000_NAME_DEVICE_ME6000ISLE16;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6000ISLE16;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6104:
+		*device_name = ME6000_NAME_DEVICE_ME61004;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME61004;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6108:
+		*device_name = ME6000_NAME_DEVICE_ME61008;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME61008;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME610F:
+		*device_name = ME6000_NAME_DEVICE_ME610016;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME610016;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6114:
+		*device_name = ME6000_NAME_DEVICE_ME6100I4;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6100I4;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6118:
+		*device_name = ME6000_NAME_DEVICE_ME6100I8;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6100I8;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME611F:
+		*device_name = ME6000_NAME_DEVICE_ME6100I16;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6100I16;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6134:
+		*device_name = ME6000_NAME_DEVICE_ME6100ISLE4;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6100ISLE4;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6138:
+		*device_name = ME6000_NAME_DEVICE_ME6100ISLE8;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6100ISLE8;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME613F:
+		*device_name = ME6000_NAME_DEVICE_ME6100ISLE16;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6100ISLE16;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6044:
+		*device_name = ME6000_NAME_DEVICE_ME60004DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME60004DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6048:
+		*device_name = ME6000_NAME_DEVICE_ME60008DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME60008DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME604F:
+		*device_name = ME6000_NAME_DEVICE_ME600016DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME600016DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6054:
+		*device_name = ME6000_NAME_DEVICE_ME6000I4DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6000I4DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6058:
+		*device_name = ME6000_NAME_DEVICE_ME6000I8DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6000I8DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME605F:
+		*device_name = ME6000_NAME_DEVICE_ME6000I16DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6000I16DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6074:
+		*device_name = ME6000_NAME_DEVICE_ME6000ISLE4DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6000ISLE4DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6078:
+		*device_name = ME6000_NAME_DEVICE_ME6000ISLE8DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6000ISLE8DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME607F:
+		*device_name = ME6000_NAME_DEVICE_ME6000ISLE16DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6000ISLE16DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6144:
+		*device_name = ME6000_NAME_DEVICE_ME61004DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME61004DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6148:
+		*device_name = ME6000_NAME_DEVICE_ME61008DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME61008DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME614F:
+		*device_name = ME6000_NAME_DEVICE_ME610016DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME610016DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6154:
+		*device_name = ME6000_NAME_DEVICE_ME6100I4DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6100I4DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6158:
+		*device_name = ME6000_NAME_DEVICE_ME6100I8DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6100I8DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME615F:
+		*device_name = ME6000_NAME_DEVICE_ME6100I16DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6100I16DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6174:
+		*device_name = ME6000_NAME_DEVICE_ME6100ISLE4DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6100ISLE4DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6178:
+		*device_name = ME6000_NAME_DEVICE_ME6100ISLE8DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6100ISLE8DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME617F:
+		*device_name = ME6000_NAME_DEVICE_ME6100ISLE16DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6100ISLE16DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6259:
+		*device_name = ME6000_NAME_DEVICE_ME6200I9DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6200I9DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6359:
+		*device_name = ME6000_NAME_DEVICE_ME6300I9DIO;
+		*device_description = ME6000_DESCRIPTION_DEVICE_ME6300I9DIO;
+		*driver_name = ME6000_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME0630:
+		*device_name = ME0600_NAME_DEVICE_ME0630;
+		*device_description = ME0600_DESCRIPTION_DEVICE_ME0630;
+		*driver_name = ME0600_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME8100_A:
+		*device_name = ME8100_NAME_DEVICE_ME8100A;
+		*device_description = ME8100_DESCRIPTION_DEVICE_ME8100A;
+		*driver_name = ME8100_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME8100_B:
+		*device_name = ME8100_NAME_DEVICE_ME8100B;
+		*device_description = ME8100_DESCRIPTION_DEVICE_ME8100B;
+		*driver_name = ME8100_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME8200_A:
+		*device_name = ME8200_NAME_DEVICE_ME8200A;
+		*device_description = ME8200_DESCRIPTION_DEVICE_ME8200A;
+		*driver_name = ME8200_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME8200_B:
+		*device_name = ME8200_NAME_DEVICE_ME8200B;
+		*device_description = ME8200_DESCRIPTION_DEVICE_ME8200B;
+		*driver_name = ME8200_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME0940:
+		*device_name = ME0900_NAME_DEVICE_ME0940;
+		*device_description = ME0900_DESCRIPTION_DEVICE_ME0940;
+		*driver_name = ME0900_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME0950:
+		*device_name = ME0900_NAME_DEVICE_ME0950;
+		*device_description = ME0900_DESCRIPTION_DEVICE_ME0950;
+		*driver_name = ME0900_NAME_DRIVER;
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME0960:
+		*device_name = ME0900_NAME_DEVICE_ME0960;
+		*device_description = ME0900_DESCRIPTION_DEVICE_ME0960;
+		*driver_name = ME0900_NAME_DRIVER;
+		break;
+/*
+		case USB_DEVICE_ID_MEPHISTO_S1:
+			*device_name = MEPHISTO_S1_NAME_DEVICE;
+			*device_description = MEPHISTO_S1_DESCRIPTION_DEVICE;
+			*driver_name = MEPHISTO_S1_NAME_DRIVER;
+			break;
+*/
+	default:
+		*device_name = EMPTY_NAME_DEVICE;
+		*device_description = EMPTY_DESCRIPTION_DEVICE;
+		*driver_name = EMPTY_NAME_DRIVER;
+
+		PERROR("Invalid device id.\n");
+
+		return 1;
+	}
+
+	return 0;
+}
+
+int me_device_pci_init(me_device_t * me_device, struct pci_dev *pci_device)
+{
+	int err;
+	int i;
+
+	PDEBUG("executed.\n");
+
+	// Initialize device list head.
+	INIT_LIST_HEAD(&me_device->list);
+
+	// Initialize device description strings.
+	err = get_device_descriptions(pci_device->device,
+				      &me_device->device_name,
+				      &me_device->device_description,
+				      &me_device->driver_name);
+
+	if (err) {
+		PERROR("Cannot initialize device description strings.\n");
+		return 1;
+	}
+	// Enable the pci device.
+	err = pci_enable_device(pci_device);
+
+	if (err < 0) {
+		PERROR("Cannot enable PCI device.\n");
+		return 1;
+	}
+	// Request the PCI register regions.
+	err = pci_request_regions(pci_device, me_device->device_name);
+
+	if (err < 0) {
+		PERROR("Cannot request PCI regions.\n");
+		goto ERROR_0;
+	}
+	// The bus carrying the device is a PCI bus.
+	me_device->bus_type = ME_BUS_TYPE_PCI;
+
+	// Store the PCI information for later usage.
+	me_device->info.pci.pci_device = pci_device;
+
+	// Get PCI register bases and sizes.
+	for (i = 0; i < 6; i++) {
+		me_device->info.pci.reg_bases[i] =
+		    pci_resource_start(pci_device, i);
+		me_device->info.pci.reg_sizes[i] =
+		    pci_resource_len(pci_device, i);
+	}
+
+	// Get the PCI location.
+	me_device->info.pci.pci_bus_no = pci_device->bus->number;
+	me_device->info.pci.pci_dev_no = PCI_SLOT(pci_device->devfn);
+	me_device->info.pci.pci_func_no = PCI_FUNC(pci_device->devfn);
+
+	// Get Meilhaus specific device information.
+	me_device->info.pci.vendor_id = pci_device->vendor;
+	me_device->info.pci.device_id = pci_device->device;
+	pci_read_config_byte(pci_device, 0x08,
+			     &me_device->info.pci.hw_revision);
+	pci_read_config_dword(pci_device, 0x2C, &me_device->info.pci.serial_no);
+
+	// Get the interrupt request number.
+	me_device->irq = pci_device->irq;
+
+	// Initialize device lock instance.
+	err = me_dlock_init(&me_device->dlock);
+
+	if (err) {
+		PERROR("Cannot initialize device lock instance.\n");
+		goto ERROR_1;
+	}
+	// Initialize subdevice list instance.
+	me_slist_init(&me_device->slist);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice list instance.\n");
+		goto ERROR_2;
+	}
+	// Initialize method pointers.
+	me_device->me_device_io_irq_start = me_device_io_irq_start;
+	me_device->me_device_io_irq_wait = me_device_io_irq_wait;
+	me_device->me_device_io_irq_stop = me_device_io_irq_stop;
+	me_device->me_device_io_reset_device = me_device_io_reset_device;
+	me_device->me_device_io_reset_subdevice = me_device_io_reset_subdevice;
+	me_device->me_device_io_single_config = me_device_io_single_config;
+	me_device->me_device_io_single_read = me_device_io_single_read;
+	me_device->me_device_io_single_write = me_device_io_single_write;
+	me_device->me_device_io_stream_config = me_device_io_stream_config;
+	me_device->me_device_io_stream_new_values =
+	    me_device_io_stream_new_values;
+	me_device->me_device_io_stream_read = me_device_io_stream_read;
+	me_device->me_device_io_stream_start = me_device_io_stream_start;
+	me_device->me_device_io_stream_status = me_device_io_stream_status;
+	me_device->me_device_io_stream_stop = me_device_io_stream_stop;
+	me_device->me_device_io_stream_write = me_device_io_stream_write;
+	me_device->me_device_lock_device = me_device_lock_device;
+	me_device->me_device_lock_subdevice = me_device_lock_subdevice;
+	me_device->me_device_query_description_device =
+	    me_device_query_description_device;
+	me_device->me_device_query_info_device = me_device_query_info_device;
+	me_device->me_device_query_name_device = me_device_query_name_device;
+	me_device->me_device_query_name_device_driver =
+	    me_device_query_name_device_driver;
+	me_device->me_device_query_number_subdevices =
+	    me_device_query_number_subdevices;
+	me_device->me_device_query_number_channels =
+	    me_device_query_number_channels;
+	me_device->me_device_query_number_ranges =
+	    me_device_query_number_ranges;
+	me_device->me_device_query_range_by_min_max =
+	    me_device_query_range_by_min_max;
+	me_device->me_device_query_range_info = me_device_query_range_info;
+	me_device->me_device_query_subdevice_by_type =
+	    me_device_query_subdevice_by_type;
+	me_device->me_device_query_subdevice_type =
+	    me_device_query_subdevice_type;
+	me_device->me_device_query_subdevice_caps =
+	    me_device_query_subdevice_caps;
+	me_device->me_device_query_subdevice_caps_args =
+	    me_device_query_subdevice_caps_args;
+	me_device->me_device_query_timer = me_device_query_timer;
+	me_device->me_device_query_version_device_driver =
+	    me_device_query_version_device_driver;
+	me_device->me_device_config_load = me_device_config_load;
+	me_device->me_device_destructor = me_device_destructor;
+
+	return 0;
+
+      ERROR_0:
+	me_dlock_deinit(&me_device->dlock);
+
+      ERROR_1:
+	pci_release_regions(pci_device);
+
+      ERROR_2:
+	pci_disable_device(pci_device);
+
+	return 1;
+}
+
+void me_device_deinit(me_device_t * me_device)
+{
+	PDEBUG("executed.\n");
+
+	me_slist_deinit(&me_device->slist);
+	me_dlock_deinit(&me_device->dlock);
+
+	if (me_device->bus_type == ME_BUS_TYPE_PCI) {
+		pci_release_regions(me_device->info.pci.pci_device);
+		pci_disable_device(me_device->info.pci.pci_device);
+	}
+/*
+	else
+	{
+		// Must be an USB device.
+	}
+*/
+}
diff --git a/drivers/staging/meilhaus/medevice.h b/drivers/staging/meilhaus/medevice.h
new file mode 100644
index 00000000000..25da82883e1
--- /dev/null
+++ b/drivers/staging/meilhaus/medevice.h
@@ -0,0 +1,304 @@
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * Source File : medevice.h
+ * Author      : GG (Guenter Gebhardt)  <support@meilhaus.de>
+ */
+
+#ifndef _MEDEVICE_H_
+#define _MEDEVICE_H_
+
+#ifndef KBUILD_MODNAME
+#  define KBUILD_MODNAME KBUILD_STR(memain)
+#endif
+
+#include <linux/pci.h>
+//#include <linux/usb.h>
+#include <linux/fs.h>
+#include <linux/spinlock.h>
+
+#include "metypes.h"
+#include "meslist.h"
+#include "medlock.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief Defines a pointer type to a PCI constructor function.
+ */
+typedef struct me_device *(*me_pci_constructor_t) (struct pci_dev *);
+
+/**
+ * @brief Defines a pointer type to a ME-4000 PCI constructor function.
+ */
+#ifdef BOSCH
+typedef struct me_device *(*me_bosch_constructor_t) (struct pci_dev *,
+						     int me_bosch_fw);
+#endif
+
+/**
+ * @brief Defines a pointer type to a USB constructor function.
+ */
+//typedef struct me_device *(*me_usb_constructor_t)(struct usb_interface *);
+
+/**
+ * @brief Defines a pointer type to the dummy constructor function.
+ */
+typedef struct me_device *(*me_dummy_constructor_t) (unsigned short vendor_id,
+						     unsigned short device_id,
+						     unsigned int serial_no,
+						     int bus_type,
+						     int bus_no,
+						     int dev_no, int func_no);
+
+//extern me_usb_constructor_t mephisto_s1_constructor __attribute__ ((weak));
+
+/**
+ * @brief Holds the PCI device information.
+ */
+typedef struct me_pci_info {
+	struct pci_dev *pci_device;			/**< Kernel PCI device structure. */
+	uint32_t reg_bases[6];				/**< The base adresses of the PCI bars. */
+	uint32_t reg_sizes[6];				/**< The sizes of the PCI bars. */
+
+	uint32_t pci_bus_no;				/**< PCI bus number. */
+	uint32_t pci_dev_no;				/**< PCI device number. */
+	uint32_t pci_func_no;				/**< PCI function number. */
+
+	uint16_t vendor_id;					/**< Meilhaus PCI vendor id. */
+	uint16_t device_id;					/**< Meilhaus device id. */
+	uint8_t hw_revision;				/**< Hardware revision of the device. */
+	uint32_t serial_no;					/**< Serial number of the device. */
+} me_pci_info_t;
+
+/**
+ * @brief Holds the USB device information.
+ */
+//typedef struct me_usb_info {
+//} me_usb_info_t;
+
+/**
+ * @brief The Meilhaus device base class structure.
+ */
+typedef struct me_device {
+	/* Attributes */
+	struct list_head list;				/**< Enables the device to be added to a dynamic list. */
+//      int magic;                                                      /**< The magic number of the structure. */
+
+	int bus_type;						/**< The descriminator for the union. */
+	union {
+		me_pci_info_t pci;				/**< PCI specific device information. */
+//              me_usb_info_t usb;                              /**< USB specific device information. */
+	} info;								/**< Holds the device information. */
+
+	int irq;							/**< The irq assigned to this device. */
+
+	me_dlock_t dlock;					/**< The device locking structure. */
+	me_slist_t slist;					/**< The container holding all subdevices belonging to this device. */
+
+	char *device_name;					/**< The name of the Meilhaus device. */
+	char *device_description;			/**< The description of the Meilhaus device. */
+	char *driver_name;					/**< The name of the device driver module supporting the device family. */
+
+	/* Methods */
+	int (*me_device_io_irq_start) (struct me_device * device,
+				       struct file * filep,
+				       int subdevice,
+				       int channel,
+				       int irq_source,
+				       int irq_edge, int irq_arg, int flags);
+
+	int (*me_device_io_irq_wait) (struct me_device * device,
+				      struct file * filep,
+				      int subdevice,
+				      int channel,
+				      int *irq_count,
+				      int *value, int time_out, int flags);
+
+	int (*me_device_io_irq_stop) (struct me_device * device,
+				      struct file * filep,
+				      int subdevice, int channel, int flags);
+
+	int (*me_device_io_reset_device) (struct me_device * device,
+					  struct file * filep, int flags);
+
+	int (*me_device_io_reset_subdevice) (struct me_device * device,
+					     struct file * filep,
+					     int subdevice, int flags);
+
+	int (*me_device_io_single_config) (struct me_device * device,
+					   struct file * filep,
+					   int subdevice,
+					   int channel,
+					   int single_config,
+					   int ref,
+					   int trig_chan,
+					   int trig_type,
+					   int trig_edge, int flags);
+
+	int (*me_device_io_single_read) (struct me_device * device,
+					 struct file * filep,
+					 int subdevice,
+					 int channel,
+					 int *value, int time_out, int flags);
+
+	int (*me_device_io_single_write) (struct me_device * device,
+					  struct file * filep,
+					  int subdevice,
+					  int channel,
+					  int value, int time_out, int flags);
+
+	int (*me_device_io_stream_config) (struct me_device * device,
+					   struct file * filep,
+					   int subdevice,
+					   meIOStreamConfig_t * config_list,
+					   int count,
+					   meIOStreamTrigger_t * trigger,
+					   int fifo_irq_threshold, int flags);
+
+	int (*me_device_io_stream_new_values) (struct me_device * device,
+					       struct file * filep,
+					       int subdevice,
+					       int time_out,
+					       int *count, int flags);
+
+	int (*me_device_io_stream_read) (struct me_device * device,
+					 struct file * filep,
+					 int subdevice,
+					 int read_mode,
+					 int *values, int *count, int flags);
+
+	int (*me_device_io_stream_start) (struct me_device * device,
+					  struct file * filep,
+					  int subdevice,
+					  int start_mode,
+					  int time_out, int flags);
+
+	int (*me_device_io_stream_status) (struct me_device * device,
+					   struct file * filep,
+					   int subdevice,
+					   int wait,
+					   int *status, int *count, int flags);
+
+	int (*me_device_io_stream_stop) (struct me_device * device,
+					 struct file * filep,
+					 int subdevice,
+					 int stop_mode, int flags);
+
+	int (*me_device_io_stream_write) (struct me_device * device,
+					  struct file * filep,
+					  int subdevice,
+					  int write_mode,
+					  int *values, int *count, int flags);
+
+	int (*me_device_lock_device) (struct me_device * device,
+				      struct file * filep, int lock, int flags);
+
+	int (*me_device_lock_subdevice) (struct me_device * device,
+					 struct file * filep,
+					 int subdevice, int lock, int flags);
+
+	int (*me_device_query_description_device) (struct me_device * device,
+						   char **description);
+
+	int (*me_device_query_info_device) (struct me_device * device,
+					    int *vendor_id,
+					    int *device_id,
+					    int *serial_no,
+					    int *bus_type,
+					    int *bus_no,
+					    int *dev_no,
+					    int *func_no, int *plugged);
+
+	int (*me_device_query_name_device) (struct me_device * device,
+					    char **name);
+
+	int (*me_device_query_name_device_driver) (struct me_device * device,
+						   char **name);
+
+	int (*me_device_query_number_subdevices) (struct me_device * device,
+						  int *number);
+
+	int (*me_device_query_number_channels) (struct me_device * device,
+						int subdevice, int *number);
+
+	int (*me_device_query_number_ranges) (struct me_device * device,
+					      int subdevice,
+					      int unit, int *count);
+
+	int (*me_device_query_range_by_min_max) (struct me_device * device,
+						 int subdevice,
+						 int unit,
+						 int *min,
+						 int *max,
+						 int *maxdata, int *range);
+
+	int (*me_device_query_range_info) (struct me_device * device,
+					   int subdevice,
+					   int range,
+					   int *unit,
+					   int *min, int *max, int *maxdata);
+
+	int (*me_device_query_subdevice_by_type) (struct me_device * device,
+						  int start_subdevice,
+						  int type,
+						  int subtype, int *subdevice);
+
+	int (*me_device_query_subdevice_type) (struct me_device * device,
+					       int subdevice,
+					       int *type, int *subtype);
+
+	int (*me_device_query_subdevice_caps) (struct me_device * device,
+					       int subdevice, int *caps);
+
+	int (*me_device_query_subdevice_caps_args) (struct me_device * device,
+						    int subdevice,
+						    int cap,
+						    int *args, int count);
+
+	int (*me_device_query_timer) (struct me_device * device,
+				      int subdevice,
+				      int timer,
+				      int *base_frequency,
+				      uint64_t * min_ticks,
+				      uint64_t * max_ticks);
+
+	int (*me_device_query_version_device_driver) (struct me_device * device,
+						      int *version);
+
+	int (*me_device_config_load) (struct me_device * device,
+				      struct file * filep,
+				      me_cfg_device_entry_t * config);
+
+	void (*me_device_destructor) (struct me_device * device);
+} me_device_t;
+
+/**
+ * @brief Initializes a PCI device base class structure.
+ *
+ * @param pci_device The PCI device context as handed over by kernel.
+ *
+ * @return 0 on success.
+ */
+int me_device_pci_init(me_device_t * me_device, struct pci_dev *pci_device);
+
+/**
+ * @brief Initializes a USB device base class structure.
+ *
+ * @param usb_interface The USB device interface as handed over by kernel.
+ *
+ * @return 0 on success.
+ */
+//int me_device_usb_init(me_device_t *me_device, struct usb_interface *interface);
+
+/**
+  * @brief Deinitializes a device base class structure and frees any previously
+  * requested resources related with this structure. It also frees any subdevice
+  * instance hold by the subdevice list.
+  *
+  * @param me_device The device class to deinitialize.
+  */
+void me_device_deinit(me_device_t * me_device);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/medlist.c b/drivers/staging/meilhaus/medlist.c
new file mode 100644
index 00000000000..ef4e36955dc
--- /dev/null
+++ b/drivers/staging/meilhaus/medlist.c
@@ -0,0 +1,127 @@
+/**
+ * @file me_dlist.c
+ *
+ * @brief Implements the device list class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include "meerror.h"
+#include "medefines.h"
+
+#include "medlist.h"
+#include "medebug.h"
+
+int me_dlist_query_number_devices(struct me_dlist *dlist, int *number)
+{
+	PDEBUG_LOCKS("called.\n");
+	*number = dlist->n;
+	return ME_ERRNO_SUCCESS;
+}
+
+unsigned int me_dlist_get_number_devices(struct me_dlist *dlist)
+{
+	PDEBUG_LOCKS("called.\n");
+	return dlist->n;
+}
+
+me_device_t *me_dlist_get_device(struct me_dlist * dlist, unsigned int index)
+{
+
+	struct list_head *pos;
+	me_device_t *device = NULL;
+	unsigned int i = 0;
+
+	PDEBUG_LOCKS("called.\n");
+
+	if (index >= dlist->n) {
+		PERROR("Index out of range.\n");
+		return NULL;
+	}
+
+	list_for_each(pos, &dlist->head) {
+		if (i == index) {
+			device = list_entry(pos, me_device_t, list);
+			break;
+		}
+
+		++i;
+	}
+
+	return device;
+}
+
+void me_dlist_add_device_tail(struct me_dlist *dlist, me_device_t * device)
+{
+	PDEBUG_LOCKS("called.\n");
+
+	list_add_tail(&device->list, &dlist->head);
+	++dlist->n;
+}
+
+me_device_t *me_dlist_del_device_tail(struct me_dlist *dlist)
+{
+
+	struct list_head *last;
+	me_device_t *device;
+
+	PDEBUG_LOCKS("called.\n");
+
+	if (list_empty(&dlist->head))
+		return NULL;
+
+	last = dlist->head.prev;
+
+	device = list_entry(last, me_device_t, list);
+
+	list_del(last);
+
+	--dlist->n;
+
+	return device;
+}
+
+int me_dlist_init(me_dlist_t * dlist)
+{
+	PDEBUG_LOCKS("called.\n");
+
+	INIT_LIST_HEAD(&dlist->head);
+	dlist->n = 0;
+	return 0;
+}
+
+void me_dlist_deinit(me_dlist_t * dlist)
+{
+
+	struct list_head *s;
+	me_device_t *device;
+
+	PDEBUG_LOCKS("called.\n");
+
+	while (!list_empty(&dlist->head)) {
+		s = dlist->head.next;
+		list_del(s);
+		device = list_entry(s, me_device_t, list);
+		device->me_device_destructor(device);
+	}
+
+	dlist->n = 0;
+}
diff --git a/drivers/staging/meilhaus/medlist.h b/drivers/staging/meilhaus/medlist.h
new file mode 100644
index 00000000000..091c11e48ed
--- /dev/null
+++ b/drivers/staging/meilhaus/medlist.h
@@ -0,0 +1,91 @@
+/**
+ * @file me_dlist.h
+ *
+ * @brief Provides the device list class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+#ifndef _ME_DLIST_H_
+#define _ME_DLIST_H_
+
+#include <linux/list.h>
+
+#include "medevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The device list container.
+ */
+typedef struct me_dlist {
+	struct list_head head;		/**< The head of the internal list. */
+	unsigned int n;			/**< The number of devices in the list. */
+} me_dlist_t;
+
+/**
+ * @brief Queries the number of devices currently inside the list.
+ *
+ * @param dlist The device list to query.
+ * @param[out] number The number of devices.
+ *
+ * @return ME-iDS error code.
+ */
+int me_dlist_query_number_devices(struct me_dlist *dlist, int *number);
+
+/**
+ * @brief Returns the number of devices currently inside the list.
+ *
+ * @param dlist The device list to query.
+ *
+ * @return The number of devices in the list.
+ */
+unsigned int me_dlist_get_number_devices(struct me_dlist *dlist);
+
+/**
+ * @brief Get a device by index.
+ *
+ * @param dlist The device list to query.
+ * @param index The index of the device to get in the list.
+ *
+ * @return The device at index if available.\n
+ *         NULL if the index is out of range.
+ */
+me_device_t *me_dlist_get_device(struct me_dlist *dlist, unsigned int index);
+
+/**
+ * @brief Adds a device to the tail of the list.
+ *
+ * @param dlist The device list to add a device to.
+ * @param device The device to add to the list.
+ */
+void me_dlist_add_device_tail(struct me_dlist *dlist, me_device_t * device);
+
+/**
+ * @brief Removes a device from the tail of the list.
+ *
+ * @param dlist The device list.
+ *
+ * @return Pointer to the removed subdeivce.\n
+ *         NULL in cases where the list was empty.
+ */
+me_device_t *me_dlist_del_device_tail(struct me_dlist *dlist);
+
+/**
+ * @brief Initializes a device list structure.
+ *
+ * @param lock The device list structure to initialize.
+ * @return 0 on success.
+ */
+int me_dlist_init(me_dlist_t * dlist);
+
+/**
+ * @brief Deinitializes a device list structure and destructs every device in it.
+ *
+ * @param dlist The device list structure to deinitialize.
+ * @return 0 on success.
+ */
+void me_dlist_deinit(me_dlist_t * dlist);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/medlock.c b/drivers/staging/meilhaus/medlock.c
new file mode 100644
index 00000000000..f649e3da4f0
--- /dev/null
+++ b/drivers/staging/meilhaus/medlock.c
@@ -0,0 +1,195 @@
+/**
+ * @file medlock.c
+ *
+ * @brief Implements the device lock class.
+ * @note Copyright (C) 2006 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/spinlock.h>
+
+#include "medefines.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "meslist.h"
+#include "mesubdevice.h"
+#include "medlock.h"
+
+int me_dlock_enter(struct me_dlock *dlock, struct file *filep)
+{
+	PDEBUG_LOCKS("executed.\n");
+
+	spin_lock(&dlock->spin_lock);
+
+	if ((dlock->filep) != NULL && (dlock->filep != filep)) {
+		PERROR("Device is locked by another process.\n");
+		spin_unlock(&dlock->spin_lock);
+		return ME_ERRNO_LOCKED;
+	}
+
+	dlock->count++;
+
+	spin_unlock(&dlock->spin_lock);
+
+	return ME_ERRNO_SUCCESS;
+}
+
+int me_dlock_exit(struct me_dlock *dlock, struct file *filep)
+{
+	PDEBUG_LOCKS("executed.\n");
+
+	spin_lock(&dlock->spin_lock);
+	dlock->count--;
+	spin_unlock(&dlock->spin_lock);
+
+	return ME_ERRNO_SUCCESS;
+}
+
+int me_dlock_lock(struct me_dlock *dlock,
+		  struct file *filep, int lock, int flags, me_slist_t * slist)
+{
+	int err = ME_ERRNO_SUCCESS;
+	int i;
+	me_subdevice_t *subdevice;
+
+	PDEBUG_LOCKS("executed.\n");
+
+	spin_lock(&dlock->spin_lock);
+
+	switch (lock) {
+
+	case ME_LOCK_RELEASE:
+		if ((dlock->filep == filep) || (dlock->filep == NULL)) {
+			dlock->filep = NULL;
+
+			/* Unlock all possibly locked subdevices. */
+
+			for (i = 0; i < me_slist_get_number_subdevices(slist);
+			     i++) {
+				subdevice = me_slist_get_subdevice(slist, i);
+
+				if (subdevice)
+					err =
+					    subdevice->
+					    me_subdevice_lock_subdevice
+					    (subdevice, filep, ME_LOCK_RELEASE,
+					     flags);
+				else
+					err = ME_ERRNO_INTERNAL;
+			}
+		}
+
+		break;
+
+	case ME_LOCK_SET:
+		if (dlock->count) {
+			PERROR("Device is used by another process.\n");
+			err = ME_ERRNO_USED;
+		} else if ((dlock->filep != NULL) && (dlock->filep != filep)) {
+			PERROR("Device is locked by another process.\n");
+			err = ME_ERRNO_LOCKED;
+		} else if (dlock->filep == NULL) {
+			/* Check any subdevice is locked by another process. */
+
+			for (i = 0; i < me_slist_get_number_subdevices(slist);
+			     i++) {
+				subdevice = me_slist_get_subdevice(slist, i);
+
+				if (subdevice) {
+					if ((err =
+					     subdevice->
+					     me_subdevice_lock_subdevice
+					     (subdevice, filep, ME_LOCK_CHECK,
+					      flags))) {
+						PERROR
+						    ("A subdevice is locked by another process.\n");
+						break;
+					}
+				} else {
+					err = ME_ERRNO_INTERNAL;
+				}
+			}
+
+			/* If no subdevices are locked by other processes,
+			   we can take ownership of the device. Otherwise we jump ahead. */
+			if (!err)
+				dlock->filep = filep;
+		}
+
+		break;
+
+	case ME_LOCK_CHECK:
+		if (dlock->count) {
+			err = ME_ERRNO_USED;
+		} else if ((dlock->filep != NULL) && (dlock->filep != filep)) {
+			err = ME_ERRNO_LOCKED;
+		} else if (dlock->filep == NULL) {
+			for (i = 0; i < me_slist_get_number_subdevices(slist);
+			     i++) {
+				subdevice = me_slist_get_subdevice(slist, i);
+
+				if (subdevice) {
+					if ((err =
+					     subdevice->
+					     me_subdevice_lock_subdevice
+					     (subdevice, filep, ME_LOCK_CHECK,
+					      flags))) {
+						PERROR
+						    ("A subdevice is locked by another process.\n");
+						break;
+					}
+				} else {
+					err = ME_ERRNO_INTERNAL;
+				}
+			}
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid lock.\n");
+
+		err = ME_ERRNO_INVALID_LOCK;
+
+		break;
+	}
+
+	spin_unlock(&dlock->spin_lock);
+
+	return err;
+}
+
+void me_dlock_deinit(struct me_dlock *dlock)
+{
+	PDEBUG_LOCKS("executed.\n");
+}
+
+int me_dlock_init(me_dlock_t * dlock)
+{
+	PDEBUG_LOCKS("executed.\n");
+
+	dlock->filep = NULL;
+	dlock->count = 0;
+	spin_lock_init(&dlock->spin_lock);
+
+	return 0;
+}
diff --git a/drivers/staging/meilhaus/medlock.h b/drivers/staging/meilhaus/medlock.h
new file mode 100644
index 00000000000..4d6ddc8e58a
--- /dev/null
+++ b/drivers/staging/meilhaus/medlock.h
@@ -0,0 +1,76 @@
+/**
+ * @file medlock.h
+ *
+ * @brief Provides the device lock class.
+ * @note Copyright (C) 2006 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+#ifndef _MEDLOCK_H_
+#define _MEDLOCK_H_
+
+#include <linux/spinlock.h>
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The device lock class.
+ */
+typedef struct me_dlock {
+	struct file *filep;		/**< Pointer to file structure holding the device. */
+	int count;				/**< Number of tasks which are inside the device. */
+	spinlock_t spin_lock;	/**< Spin lock protecting the attributes from concurrent access. */
+} me_dlock_t;
+
+/**
+ * @brief Tries to enter a device.
+ *
+ * @param dlock The device lock instance.
+ * @param filep The file structure identifying the calling process.
+ *
+ * @return 0 on success.
+ */
+int me_dlock_enter(struct me_dlock *dlock, struct file *filep);
+
+/**
+ * @brief Exits a device.
+ *
+ * @param dlock The device lock instance.
+ * @param filep The file structure identifying the calling process.
+ *
+ * @return 0 on success.
+ */
+int me_dlock_exit(struct me_dlock *dlock, struct file *filep);
+
+/**
+ * @brief Tries to perform a locking action on a device.
+ *
+ * @param dlock The device lock instance.
+ * @param filep The file structure identifying the calling process.
+ * @param The action to be done.
+ * @param flags Flags from user space.
+ * @param slist The subdevice list of the device.
+ *
+ * @return 0 on success.
+ */
+int me_dlock_lock(struct me_dlock *dlock,
+		  struct file *filep, int lock, int flags, me_slist_t * slist);
+
+/**
+ * @brief Initializes a lock structure.
+ *
+ * @param dlock The lock structure to initialize.
+ * @return 0 on success.
+ */
+int me_dlock_init(me_dlock_t * dlock);
+
+/**
+ * @brief Deinitializes a lock structure.
+ *
+ * @param dlock The lock structure to deinitialize.
+ * @return 0 on success.
+ */
+void me_dlock_deinit(me_dlock_t * dlock);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/medriver.h b/drivers/staging/meilhaus/medriver.h
new file mode 100644
index 00000000000..02e2408ce5f
--- /dev/null
+++ b/drivers/staging/meilhaus/medriver.h
@@ -0,0 +1,350 @@
+/*
+ * Copyright (C) 2005 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * Source File : medriver.h
+ * Author      : GG (Guenter Gebhardt)  <g.gebhardt@meilhaus.de>
+ *  Author:	Krzysztof Gantzke	<k.gantzke@meilhaus.de>
+ */
+
+#ifndef _MEDRIVER_H_
+#define _MEDRIVER_H_
+
+#include "metypes.h"
+#include "meerror.h"
+#include "medefines.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+	/*===========================================================================
+	  Functions to access the driver system
+	  =========================================================================*/
+
+	int meOpen(int iFlags);
+	int meClose(int iFlags);
+
+	int meLockDriver(int iLock, int iFlags);
+	int meLockDevice(int iDevice, int iLock, int iFlags);
+	int meLockSubdevice(int iDevice, int iSubdevice, int iLock, int iFlags);
+
+	/*===========================================================================
+	  Error handling functions
+	  =========================================================================*/
+
+	int meErrorGetLastMessage(char *pcErrorMsg, int iCount);
+	int meErrorGetMessage(int iErrorCode, char *pcErrorMsg, int iCount);
+	int meErrorSetDefaultProc(int iSwitch);
+	int meErrorSetUserProc(meErrorCB_t pErrorProc);
+
+
+	/*===========================================================================
+	  Functions to perform I/O on a device
+	  =========================================================================*/
+
+	int meIOIrqSetCallback(
+			int iDevice,
+			int iSubdevice,
+			meIOIrqCB_t pCallback,
+			void *pCallbackContext,
+			int iFlags);
+	int meIOIrqStart(
+			int iDevice,
+			int iSubdevice,
+			int iChannel,
+			int iIrqSource,
+			int iIrqEdge,
+			int iIrqArg,
+			int iFlags);
+	int meIOIrqStop(
+			int iDevice,
+			int iSubdevice,
+			int iChannel,
+			int iFlags);
+	int meIOIrqWait(
+			int iDevice,
+			int iSubdevice,
+			int iChannel,
+			int *piIrqCount,
+			int *piValue,
+			int iTimeOut,
+			int iFlags);
+
+	int meIOResetDevice(int iDevice, int iFlags);
+	int meIOResetSubdevice(int iDevice, int iSubdevice, int iFlags);
+
+	int meIOStreamFrequencyToTicks(
+			int iDevice,
+			int iSubdevice,
+			int iTimer,
+			double *pdFrequency,
+			int *piTicksLow,
+			int *piTicksHigh,
+			int iFlags);
+
+	int meIOSingleConfig(
+			int iDevice,
+			int iSubdevice,
+			int iChannel,
+			int iSingleConfig,
+			int iRef,
+			int iTrigChan,
+			int iTrigType,
+			int iTrigEdge,
+			int iFlags);
+	int meIOSingle(meIOSingle_t *pSingleList, int iCount, int iFlags);
+
+	int meIOStreamConfig(
+			int iDevice,
+			int iSubdevice,
+			meIOStreamConfig_t *pConfigList,
+			int iCount,
+			meIOStreamTrigger_t *pTrigger,
+			int iFifoIrqThreshold,
+			int iFlags);
+	int meIOStreamNewValues(
+			int iDevice,
+			int iSubdevice,
+			int iTimeOut,
+			int *piCount,
+			int iFlags);
+	int meIOStreamRead(
+			int iDevice,
+			int iSubdevice,
+			int iReadMode,
+			int *piValues,
+			int *piCount,
+			int iFlags);
+	int meIOStreamWrite(
+			int iDevice,
+			int iSubdevice,
+			int iWriteMode,
+			int *piValues,
+			int *piCount,
+			int iFlags);
+	int meIOStreamStart(meIOStreamStart_t *pStartList, int iCount, int iFlags);
+	int meIOStreamStop(meIOStreamStop_t *pStopList, int iCount, int iFlags);
+	int meIOStreamStatus(
+			int iDevice,
+			int iSubdevice,
+			int iWait,
+			int *piStatus,
+			int *piCount,
+			int iFlags);
+	int meIOStreamSetCallbacks(
+			int iDevice,
+			int iSubdevice,
+			meIOStreamCB_t pStartCB,
+			void *pStartCBContext,
+			meIOStreamCB_t pNewValuesCB,
+			void *pNewValuesCBContext,
+			meIOStreamCB_t pEndCB,
+			void *pEndCBContext,
+			int iFlags);
+	int meIOStreamTimeToTicks(
+			int iDevice,
+			int iSubdevice,
+			int iTimer,
+			double *pdTime,
+			int *piTicksLow,
+			int *piTicksHigh,
+			int iFlags);
+
+
+	/*===========================================================================
+	  Functions to query the driver system
+	  =========================================================================*/
+
+	int meQueryDescriptionDevice(int iDevice, char *pcDescription, int iCount);
+
+	int meQueryInfoDevice(
+			int iDevice,
+			int *piVendorId,
+			int *piDeviceId,
+			int *piSerialNo,
+			int *piBusType,
+			int *piBusNo,
+			int *piDevNo,
+			int *piFuncNo,
+			int *piPlugged);
+
+	int meQueryNameDevice(int iDevice, char *pcName, int iCount);
+	int meQueryNameDeviceDriver(int iDevice, char *pcName, int iCount);
+
+	int meQueryNumberDevices(int *piNumber);
+	int meQueryNumberSubdevices(int iDevice, int *piNumber);
+	int meQueryNumberChannels(int iDevice, int iSubdevice, int *piNumber);
+	int meQueryNumberRanges(
+			int iDevice,
+			int iSubdevice,
+			int iUnit,
+			int *piNumber);
+
+	int meQueryRangeByMinMax(
+			int iDevice,
+			int iSubdevice,
+			int iUnit,
+			double *pdMin,
+			double *pdMax,
+			int *piMaxData,
+			int *piRange);
+	int meQueryRangeInfo(
+			int iDevice,
+			int iSubdevice,
+			int iRange,
+			int *piUnit,
+			double *pdMin,
+			double *pdMax,
+			int *piMaxData);
+
+	int meQuerySubdeviceByType(
+			int iDevice,
+			int iStartSubdevice,
+			int iType,
+			int iSubtype,
+			int *piSubdevice);
+	int meQuerySubdeviceType(
+			int iDevice,
+			int iSubdevice,
+			int *piType,
+			int *piSubtype);
+	int meQuerySubdeviceCaps(
+			int iDevice,
+			int iSubdevice,
+			int *piCaps);
+	int meQuerySubdeviceCapsArgs(
+			int iDevice,
+			int iSubdevice,
+			int iCap,
+			int *piArgs,
+			int iCount);
+
+	int meQueryVersionLibrary(int *piVersion);
+	int meQueryVersionMainDriver(int *piVersion);
+	int meQueryVersionDeviceDriver(int iDevice, int *piVersion);
+
+
+	/*===========================================================================
+	  Common utility functions
+	  =========================================================================*/
+
+	int meUtilityExtractValues(
+			int iChannel,
+			int *piAIBuffer,
+			int iAIBufferCount,
+			meIOStreamConfig_t *pConfigList,
+			int iConfigListCount,
+			int *piChanBuffer,
+			int *piChanBufferCount);
+	int meUtilityDigitalToPhysical(
+			double dMin,
+			double dMax,
+			int iMaxData,
+			int iData,
+			int iModuleType,
+			double dRefValue,
+			double *pdPhysical);
+	int meUtilityDigitalToPhysicalV(
+			double dMin,
+			double dMax,
+			int iMaxData,
+			int *piDataBuffer,
+			int iCount,
+			int iModuleType,
+			double dRefValue,
+			double *pdPhysicalBuffer);
+	int meUtilityPhysicalToDigital(
+			double dMin,
+			double dMax,
+			int iMaxData,
+			double dPhysical,
+			int *piData);
+	int meUtilityPWMStart(
+			int iDevice,
+			int iSubdevice1,
+			int iSubdevice2,
+			int iSubdevice3,
+			int iRef,
+			int iPrescaler,
+			int iDutyCycle,
+			int iFlag);
+	int meUtilityPWMStop(int iDevice,
+			int iSubdevice1);
+	int meUtilityPWMRestart(
+			int iDevice,
+			int iSubdevice1,
+			int iRef,
+			int iPrescaler);
+
+
+	/*===========================================================================
+	  Load configuration from file into driver system
+	  =========================================================================*/
+
+	int meConfigLoad(char *pcConfigFile);
+
+
+	/*===========================================================================
+	  Functions to query a remote driver system
+	  =========================================================================*/
+
+	int meRQueryDescriptionDevice(
+			char *location,
+			int iDevice,
+			char *pcDescription,
+			int iCount);
+
+	int meRQueryInfoDevice(
+			char *location,
+			int iDevice,
+			int *piVendorId,
+			int *piDeviceId,
+			int *piSerialNo,
+			int *piBusType,
+			int *piBusNo,
+			int *piDevNo,
+			int *piFuncNo,
+			int *piPlugged);
+
+	int meRQueryNameDevice(
+			char *location,
+			int iDevice,
+			char *pcName,
+			int iCount);
+
+	int meRQueryNumberDevices(char *location, int *piNumber);
+	int meRQueryNumberSubdevices(char *location, int iDevice, int *piNumber);
+	int meRQueryNumberChannels(
+			char *location,
+			int iDevice,
+			int iSubdevice,
+			int *piNumber);
+	int meRQueryNumberRanges(
+			char *location,
+			int iDevice,
+			int iSubdevice,
+			int iUnit,
+			int *piNumber);
+
+	int meRQueryRangeInfo(
+			char *location,
+			int iDevice,
+			int iSubdevice,
+			int iRange,
+			int *piUnit,
+			double *pdMin,
+			double *pdMax,
+			int *piMaxData);
+
+	int meRQuerySubdeviceType(
+			char *location,
+			int iDevice,
+			int iSubdevice,
+			int *piType,
+			int *piSubtype);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/drivers/staging/meilhaus/medummy.c b/drivers/staging/meilhaus/medummy.c
new file mode 100644
index 00000000000..6a9f08d50bb
--- /dev/null
+++ b/drivers/staging/meilhaus/medummy.c
@@ -0,0 +1,1266 @@
+/* Device driver for Meilhaus ME-DUMMY devices.
+ * ===========================================
+ *
+ *    Copyright (C) 2005 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ *    This file is free software; you can redistribute it and/or modify
+ *    it under the terms of the GNU General Public License as published by
+ *    the Free Software Foundation; either version 2 of the License, or
+ *    (at your option) any later version.
+ *
+ *    This program is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *    GNU General Public License for more details.
+ *
+ *    You should have received a copy of the GNU General Public License
+ *    along with this program; if not, write to the Free Software
+ *    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+/*
+ * User application could also include the kernel header files. But the
+ * real kernel functions are protected by #ifdef __KERNEL__.
+ */
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * This must be defined before module.h is included. Not needed, when
+ * it is a built in driver.
+ */
+#ifndef MODULE
+#  define MODULE
+#endif
+
+#include <linux/module.h>
+#include <linux/slab.h>
+
+#include "meerror.h"
+#include "meinternal.h"
+
+#include "meids.h"
+#include "mecommon.h"
+#include "medevice.h"
+#include "medebug.h"
+
+#include "medummy.h"
+
+static int medummy_io_irq_start(me_device_t * device,
+				struct file *filep,
+				int subdevice,
+				int channel,
+				int irq_source,
+				int irq_edge, int irq_arg, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_io_irq_wait(me_device_t * device,
+			       struct file *filep,
+			       int subdevice,
+			       int channel,
+			       int *irq_count,
+			       int *value, int timeout, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_io_irq_stop(me_device_t * device,
+			       struct file *filep,
+			       int subdevice, int channel, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_io_reset_device(me_device_t * device,
+				   struct file *filep, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_io_reset_subdevice(me_device_t * device,
+				      struct file *filep,
+				      int subdevice, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_io_single_config(me_device_t * device,
+				    struct file *filep,
+				    int subdevice,
+				    int channel,
+				    int single_config,
+				    int ref,
+				    int trig_chan,
+				    int trig_type, int trig_edge, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_io_single_read(me_device_t * device,
+				  struct file *filep,
+				  int subdevice,
+				  int channel,
+				  int *value, int time_out, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_io_single_write(me_device_t * device,
+				   struct file *filep,
+				   int subdevice,
+				   int channel,
+				   int value, int time_out, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_io_stream_config(me_device_t * device,
+				    struct file *filep,
+				    int subdevice,
+				    meIOStreamConfig_t * config_list,
+				    int count,
+				    meIOStreamTrigger_t * trigger,
+				    int fifo_irq_threshold, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_io_stream_new_values(me_device_t * device,
+					struct file *filep,
+					int subdevice,
+					int timeout, int *count, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_io_stream_read(me_device_t * device,
+				  struct file *filep,
+				  int subdevice,
+				  int read_mode,
+				  int *values, int *count, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_io_stream_start(me_device_t * device,
+				   struct file *filep,
+				   int subdevice,
+				   int start_mode, int time_out, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_io_stream_status(me_device_t * device,
+				    struct file *filep,
+				    int subdevice,
+				    int wait,
+				    int *status, int *values, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_io_stream_stop(me_device_t * device,
+				  struct file *filep,
+				  int subdevice, int stop_mode, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_io_stream_write(me_device_t * device,
+				   struct file *filep,
+				   int subdevice,
+				   int write_mode,
+				   int *values, int *count, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_lock_device(me_device_t * device,
+			       struct file *filep, int lock, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_lock_subdevice(me_device_t * device,
+				  struct file *filep,
+				  int subdevice, int lock, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_query_description_device(me_device_t * device,
+					    char **description)
+{
+	medummy_device_t *instance = (medummy_device_t *) device;
+
+	PDEBUG("executed.\n");
+
+//      if (instance->magic != MEDUMMY_MAGIC_NUMBER)
+//      {
+//              PERROR("Wrong magic number.\n");
+//              return ME_ERRNO_INTERNAL;
+//      }
+
+	switch (instance->device_id) {
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1000:
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1000_A:
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1000_B:
+		*description = ME1000_DESCRIPTION_DEVICE_ME1000;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1400:
+		*description = ME1400_DESCRIPTION_DEVICE_ME1400;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140A:
+		*description = ME1400_DESCRIPTION_DEVICE_ME1400A;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140B:
+		*description = ME1400_DESCRIPTION_DEVICE_ME1400B;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME14E0:
+		*description = ME1400_DESCRIPTION_DEVICE_ME1400E;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME14EA:
+		*description = ME1400_DESCRIPTION_DEVICE_ME1400EA;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME14EB:
+		*description = ME1400_DESCRIPTION_DEVICE_ME1400EB;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140C:
+		*description = ME1400_DESCRIPTION_DEVICE_ME1400C;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140D:
+		*description = ME1400_DESCRIPTION_DEVICE_ME1400D;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1600_4U:
+		*description = ME1600_DESCRIPTION_DEVICE_ME16004U;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1600_8U:
+		*description = ME1600_DESCRIPTION_DEVICE_ME16008U;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1600_12U:
+		*description = ME1600_DESCRIPTION_DEVICE_ME160012U;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1600_16U:
+		*description = ME1600_DESCRIPTION_DEVICE_ME160016U;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1600_16U_8I:
+		*description = ME1600_DESCRIPTION_DEVICE_ME160016U8I;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4610:
+		*description = ME4600_DESCRIPTION_DEVICE_ME4610;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4650:
+		*description = ME4600_DESCRIPTION_DEVICE_ME4650;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4660:
+		*description = ME4600_DESCRIPTION_DEVICE_ME4660;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4660I:
+		*description = ME4600_DESCRIPTION_DEVICE_ME4660I;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4660S:
+		*description = ME4600_DESCRIPTION_DEVICE_ME4660S;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4660IS:
+		*description = ME4600_DESCRIPTION_DEVICE_ME4660IS;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4670:
+		*description = ME4600_DESCRIPTION_DEVICE_ME4670;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4670I:
+		*description = ME4600_DESCRIPTION_DEVICE_ME4670I;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4670S:
+		*description = ME4600_DESCRIPTION_DEVICE_ME4670S;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4670IS:
+		*description = ME4600_DESCRIPTION_DEVICE_ME4670IS;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4680:
+		*description = ME4600_DESCRIPTION_DEVICE_ME4680;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4680I:
+		*description = ME4600_DESCRIPTION_DEVICE_ME4680I;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4680S:
+		*description = ME4600_DESCRIPTION_DEVICE_ME4680S;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4680IS:
+		*description = ME4600_DESCRIPTION_DEVICE_ME4680IS;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6004:
+		*description = ME6000_DESCRIPTION_DEVICE_ME60004;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6008:
+		*description = ME6000_DESCRIPTION_DEVICE_ME60008;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME600F:
+		*description = ME6000_DESCRIPTION_DEVICE_ME600016;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6014:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6000I4;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6018:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6000I8;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME601F:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6000I16;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6034:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6000ISLE4;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6038:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6000ISLE8;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME603F:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6000ISLE16;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6104:
+		*description = ME6000_DESCRIPTION_DEVICE_ME61004;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6108:
+		*description = ME6000_DESCRIPTION_DEVICE_ME61008;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME610F:
+		*description = ME6000_DESCRIPTION_DEVICE_ME610016;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6114:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6100I4;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6118:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6100I8;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME611F:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6100I16;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6134:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6100ISLE4;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6138:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6100ISLE8;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME613F:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6100ISLE16;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6044:
+		*description = ME6000_DESCRIPTION_DEVICE_ME60004DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6048:
+		*description = ME6000_DESCRIPTION_DEVICE_ME60008DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME604F:
+		*description = ME6000_DESCRIPTION_DEVICE_ME600016DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6054:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6000I4DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6058:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6000I8DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME605F:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6000I16DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6074:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6000ISLE4DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6078:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6000ISLE8DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME607F:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6000ISLE16DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6144:
+		*description = ME6000_DESCRIPTION_DEVICE_ME61004DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6148:
+		*description = ME6000_DESCRIPTION_DEVICE_ME61008DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME614F:
+		*description = ME6000_DESCRIPTION_DEVICE_ME610016DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6154:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6100I4DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6158:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6100I8DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME615F:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6100I16DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6174:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6100ISLE4DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6178:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6100ISLE8DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME617F:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6100ISLE16DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6259:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6200I9DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6359:
+		*description = ME6000_DESCRIPTION_DEVICE_ME6300I9DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME0630:
+		*description = ME0600_DESCRIPTION_DEVICE_ME0630;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME8100_A:
+		*description = ME8100_DESCRIPTION_DEVICE_ME8100A;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME8100_B:
+		*description = ME8100_DESCRIPTION_DEVICE_ME8100B;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME0940:
+		*description = ME0900_DESCRIPTION_DEVICE_ME0940;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME0950:
+		*description = ME0900_DESCRIPTION_DEVICE_ME0950;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME0960:
+		*description = ME0900_DESCRIPTION_DEVICE_ME0960;
+
+		break;
+/*
+		case USB_DEVICE_ID_MEPHISTO_S1:
+			*description = MEPHISTO_S1_DESCRIPTION_DEVICE;
+
+			break;
+*/
+	default:
+		*description = EMPTY_DESCRIPTION_DEVICE;
+		PERROR("Invalid device id in device info.\n");
+
+		return ME_ERRNO_INTERNAL;
+	}
+
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_query_info_device(me_device_t * device,
+				     int *vendor_id,
+				     int *device_id,
+				     int *serial_no,
+				     int *bus_type,
+				     int *bus_no,
+				     int *dev_no, int *func_no, int *plugged)
+{
+	medummy_device_t *instance = (medummy_device_t *) device;
+
+	PDEBUG("executed.\n");
+
+//      if (instance->magic != MEDUMMY_MAGIC_NUMBER)
+//      {
+//              PERROR("Wrong magic number.\n");
+//              return ME_ERRNO_INTERNAL;
+//      }
+
+	*vendor_id = instance->vendor_id;
+	*device_id = instance->device_id;
+	*serial_no = instance->serial_no;
+	*bus_type = instance->bus_type;
+	*bus_no = instance->bus_no;
+	*dev_no = instance->dev_no;
+	*func_no = instance->func_no;
+	*plugged = ME_PLUGGED_OUT;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int medummy_query_name_device_driver(me_device_t * device, char **name)
+{
+	PDEBUG("executed.\n");
+	*name = MEDUMMY_NAME_DRIVER;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int medummy_query_name_device(me_device_t * device, char **name)
+{
+	medummy_device_t *instance = (medummy_device_t *) device;
+
+	PDEBUG("executed.\n");
+
+// // //        if (instance->magic != MEDUMMY_MAGIC_NUMBER)
+// // //        {
+// // //                PERROR("Wrong magic number.\n");
+// // //                return ME_ERRNO_INTERNAL;
+// // //        }
+
+	switch (instance->device_id) {
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1000:
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1000_A:
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1000_B:
+		*name = ME1000_NAME_DEVICE_ME1000;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1400:
+		*name = ME1400_NAME_DEVICE_ME1400;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140A:
+		*name = ME1400_NAME_DEVICE_ME1400A;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140B:
+		*name = ME1400_NAME_DEVICE_ME1400B;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME14E0:
+		*name = ME1400_NAME_DEVICE_ME1400E;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME14EA:
+		*name = ME1400_NAME_DEVICE_ME1400EA;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME14EB:
+		*name = ME1400_NAME_DEVICE_ME1400EB;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140C:
+		*name = ME1400_NAME_DEVICE_ME1400C;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME140D:
+		*name = ME1400_NAME_DEVICE_ME1400D;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1600_4U:
+		*name = ME1600_NAME_DEVICE_ME16004U;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1600_8U:
+		*name = ME1600_NAME_DEVICE_ME16008U;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1600_12U:
+		*name = ME1600_NAME_DEVICE_ME160012U;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1600_16U:
+		*name = ME1600_NAME_DEVICE_ME160016U;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME1600_16U_8I:
+		*name = ME1600_NAME_DEVICE_ME160016U8I;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4610:
+		*name = ME4600_NAME_DEVICE_ME4610;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4650:
+		*name = ME4600_NAME_DEVICE_ME4650;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4660:
+		*name = ME4600_NAME_DEVICE_ME4660;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4660I:
+		*name = ME4600_NAME_DEVICE_ME4660I;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4670:
+		*name = ME4600_NAME_DEVICE_ME4670;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4670I:
+		*name = ME4600_NAME_DEVICE_ME4670I;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4670S:
+		*name = ME4600_NAME_DEVICE_ME4670S;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4670IS:
+		*name = ME4600_NAME_DEVICE_ME4670IS;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4680:
+		*name = ME4600_NAME_DEVICE_ME4680;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4680I:
+		*name = ME4600_NAME_DEVICE_ME4680I;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4680S:
+		*name = ME4600_NAME_DEVICE_ME4680S;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME4680IS:
+		*name = ME4600_NAME_DEVICE_ME4680IS;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6004:
+		*name = ME6000_NAME_DEVICE_ME60004;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6008:
+		*name = ME6000_NAME_DEVICE_ME60008;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME600F:
+		*name = ME6000_NAME_DEVICE_ME600016;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6014:
+		*name = ME6000_NAME_DEVICE_ME6000I4;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6018:
+		*name = ME6000_NAME_DEVICE_ME6000I8;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME601F:
+		*name = ME6000_NAME_DEVICE_ME6000I16;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6034:
+		*name = ME6000_NAME_DEVICE_ME6000ISLE4;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6038:
+		*name = ME6000_NAME_DEVICE_ME6000ISLE8;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME603F:
+		*name = ME6000_NAME_DEVICE_ME6000ISLE16;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6104:
+		*name = ME6000_NAME_DEVICE_ME61004;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6108:
+		*name = ME6000_NAME_DEVICE_ME61008;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME610F:
+		*name = ME6000_NAME_DEVICE_ME610016;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6114:
+		*name = ME6000_NAME_DEVICE_ME6100I4;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6118:
+		*name = ME6000_NAME_DEVICE_ME6100I8;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME611F:
+		*name = ME6000_NAME_DEVICE_ME6100I16;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6134:
+		*name = ME6000_NAME_DEVICE_ME6100ISLE4;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6138:
+		*name = ME6000_NAME_DEVICE_ME6100ISLE8;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME613F:
+		*name = ME6000_NAME_DEVICE_ME6100ISLE16;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6044:
+		*name = ME6000_NAME_DEVICE_ME60004DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6048:
+		*name = ME6000_NAME_DEVICE_ME60008DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME604F:
+		*name = ME6000_NAME_DEVICE_ME600016DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6054:
+		*name = ME6000_NAME_DEVICE_ME6000I4DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6058:
+		*name = ME6000_NAME_DEVICE_ME6000I8DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME605F:
+		*name = ME6000_NAME_DEVICE_ME6000I16DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6074:
+		*name = ME6000_NAME_DEVICE_ME6000ISLE4DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6078:
+		*name = ME6000_NAME_DEVICE_ME6000ISLE8DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME607F:
+		*name = ME6000_NAME_DEVICE_ME6000ISLE16DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6144:
+		*name = ME6000_NAME_DEVICE_ME61004DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6148:
+		*name = ME6000_NAME_DEVICE_ME61008DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME614F:
+		*name = ME6000_NAME_DEVICE_ME610016DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6154:
+		*name = ME6000_NAME_DEVICE_ME6100I4DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6158:
+		*name = ME6000_NAME_DEVICE_ME6100I8DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME615F:
+		*name = ME6000_NAME_DEVICE_ME6100I16DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6174:
+		*name = ME6000_NAME_DEVICE_ME6100ISLE4DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME6178:
+		*name = ME6000_NAME_DEVICE_ME6100ISLE8DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME617F:
+		*name = ME6000_NAME_DEVICE_ME6100ISLE16DIO;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME0630:
+		*name = ME0600_NAME_DEVICE_ME0630;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME8100_A:
+		*name = ME8100_NAME_DEVICE_ME8100A;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME8100_B:
+		*name = ME8100_NAME_DEVICE_ME8100B;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME0940:
+		*name = ME0900_NAME_DEVICE_ME0940;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME0950:
+		*name = ME0900_NAME_DEVICE_ME0950;
+
+		break;
+
+	case PCI_DEVICE_ID_MEILHAUS_ME0960:
+		*name = ME0900_NAME_DEVICE_ME0960;
+
+		break;
+/*
+		case USB_DEVICE_ID_MEPHISTO_S1:
+			*name = MEPHISTO_S1_NAME_DEVICE;
+
+			break;
+*/
+	default:
+		*name = EMPTY_NAME_DEVICE;
+		PERROR("Invalid PCI device id.\n");
+
+		return ME_ERRNO_INTERNAL;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int medummy_query_number_subdevices(me_device_t * device, int *number)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_query_number_channels(me_device_t * device,
+					 int subdevice, int *number)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_query_number_ranges(me_device_t * device,
+				       int subdevice, int unit, int *count)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_query_subdevice_type(me_device_t * device,
+					int subdevice, int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_query_subdevice_caps(me_device_t * device,
+					int subdevice, int *caps)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_query_subdevice_caps_args(me_device_t * device,
+					     int subdevice,
+					     int cap, int *args, int count)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int medummy_query_subdevice_by_type(me_device_t * device,
+					   int start_subdevice,
+					   int type,
+					   int subtype, int *subdevice)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_query_range_by_min_max(me_device_t * device,
+					  int subdevice,
+					  int unit,
+					  int *min,
+					  int *max, int *maxdata, int *range)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_query_range_info(me_device_t * device,
+				    int subdevice,
+				    int range,
+				    int *unit, int *min, int *max, int *maxdata)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+int medummy_query_timer(me_device_t * device,
+			int subdevice,
+			int timer,
+			int *base_frequency,
+			uint64_t * min_ticks, uint64_t * max_ticks)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_DEVICE_UNPLUGGED;
+}
+
+static int medummy_query_version_device_driver(me_device_t * device,
+					       int *version)
+{
+	PDEBUG("executed.\n");
+
+	*version = ME_VERSION_DRIVER;
+	return ME_ERRNO_SUCCESS;
+}
+
+static void medummy_destructor(me_device_t * device)
+{
+	PDEBUG("executed.\n");
+	kfree(device);
+}
+
+static int init_device_info(unsigned short vendor_id,
+			    unsigned short device_id,
+			    unsigned int serial_no,
+			    int bus_type,
+			    int bus_no,
+			    int dev_no,
+			    int func_no, medummy_device_t * instance)
+{
+	PDEBUG("executed.\n");
+
+//      instance->magic = MEDUMMY_MAGIC_NUMBER;
+	instance->vendor_id = vendor_id;
+	instance->device_id = device_id;
+	instance->serial_no = serial_no;
+	instance->bus_type = bus_type;
+	instance->bus_no = bus_no;
+	instance->dev_no = dev_no;
+	instance->func_no = func_no;
+
+	return 0;
+}
+
+static int medummy_config_load(me_device_t * device, struct file *filep,
+			       me_cfg_device_entry_t * config)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_SUCCESS;
+}
+
+static int init_device_instance(me_device_t * device)
+{
+	PDEBUG("executed.\n");
+
+	INIT_LIST_HEAD(&device->list);
+
+	device->me_device_io_irq_start = medummy_io_irq_start;
+	device->me_device_io_irq_wait = medummy_io_irq_wait;
+	device->me_device_io_irq_stop = medummy_io_irq_stop;
+	device->me_device_io_reset_device = medummy_io_reset_device;
+	device->me_device_io_reset_subdevice = medummy_io_reset_subdevice;
+	device->me_device_io_single_config = medummy_io_single_config;
+	device->me_device_io_single_read = medummy_io_single_read;
+	device->me_device_io_single_write = medummy_io_single_write;
+	device->me_device_io_stream_config = medummy_io_stream_config;
+	device->me_device_io_stream_new_values = medummy_io_stream_new_values;
+	device->me_device_io_stream_read = medummy_io_stream_read;
+	device->me_device_io_stream_start = medummy_io_stream_start;
+	device->me_device_io_stream_status = medummy_io_stream_status;
+	device->me_device_io_stream_stop = medummy_io_stream_stop;
+	device->me_device_io_stream_write = medummy_io_stream_write;
+
+	device->me_device_lock_device = medummy_lock_device;
+	device->me_device_lock_subdevice = medummy_lock_subdevice;
+
+	device->me_device_query_description_device =
+	    medummy_query_description_device;
+	device->me_device_query_info_device = medummy_query_info_device;
+	device->me_device_query_name_device_driver =
+	    medummy_query_name_device_driver;
+	device->me_device_query_name_device = medummy_query_name_device;
+
+	device->me_device_query_number_subdevices =
+	    medummy_query_number_subdevices;
+	device->me_device_query_number_channels = medummy_query_number_channels;
+	device->me_device_query_number_ranges = medummy_query_number_ranges;
+
+	device->me_device_query_range_by_min_max =
+	    medummy_query_range_by_min_max;
+	device->me_device_query_range_info = medummy_query_range_info;
+
+	device->me_device_query_subdevice_type = medummy_query_subdevice_type;
+	device->me_device_query_subdevice_by_type =
+	    medummy_query_subdevice_by_type;
+	device->me_device_query_subdevice_caps = medummy_query_subdevice_caps;
+	device->me_device_query_subdevice_caps_args =
+	    medummy_query_subdevice_caps_args;
+
+	device->me_device_query_timer = medummy_query_timer;
+
+	device->me_device_query_version_device_driver =
+	    medummy_query_version_device_driver;
+
+	device->me_device_destructor = medummy_destructor;
+	device->me_device_config_load = medummy_config_load;
+	return 0;
+}
+
+me_device_t *medummy_constructor(unsigned short vendor_id,
+				 unsigned short device_id,
+				 unsigned int serial_no,
+				 int bus_type,
+				 int bus_no, int dev_no, int func_no)
+{
+	int result = 0;
+	medummy_device_t *instance;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate structure for device attributes */
+	instance = kmalloc(sizeof(medummy_device_t), GFP_KERNEL);
+
+	if (!instance) {
+		PERROR("Can't get memory for device instance.\n");
+		return NULL;
+	}
+
+	memset(instance, 0, sizeof(medummy_device_t));
+
+	/* Initialize device info */
+	result = init_device_info(vendor_id,
+				  device_id,
+				  serial_no,
+				  bus_type, bus_no, dev_no, func_no, instance);
+
+	if (result) {
+		PERROR("Cannot init baord info.\n");
+		kfree(instance);
+		return NULL;
+	}
+
+	/* Initialize device instance */
+	result = init_device_instance((me_device_t *) instance);
+
+	if (result) {
+		PERROR("Cannot init baord info.\n");
+		kfree(instance);
+		return NULL;
+	}
+
+	return (me_device_t *) instance;
+}
+
+// Init and exit of module.
+
+static int __init dummy_init(void)
+{
+	PDEBUG("executed.\n");
+	return 0;
+}
+
+static void __exit dummy_exit(void)
+{
+	PDEBUG("executed.\n");
+}
+
+module_init(dummy_init);
+
+module_exit(dummy_exit);
+
+// Administrative stuff for modinfo.
+MODULE_AUTHOR("Guenter Gebhardt <g.gebhardt@meilhaus.de>");
+MODULE_DESCRIPTION("Device Driver Module for Meilhaus ME-DUMMY Devices");
+MODULE_SUPPORTED_DEVICE("Meilhaus ME-DUMMY Devices");
+MODULE_LICENSE("GPL");
+
+// Export the constructor.
+EXPORT_SYMBOL(medummy_constructor);
diff --git a/drivers/staging/meilhaus/medummy.h b/drivers/staging/meilhaus/medummy.h
new file mode 100644
index 00000000000..717000ff6c1
--- /dev/null
+++ b/drivers/staging/meilhaus/medummy.h
@@ -0,0 +1,40 @@
+/*
+ * Copyright (C) 2005 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * Source File : medummy.h
+ * Author      : GG (Guenter Gebhardt)  <g.gebhardt@meilhaus.de>
+ */
+
+#ifndef _MEDUMMY_H_
+#define _MEDUMMY_H_
+
+#include "metypes.h"
+#include "medefines.h"
+#include "medevice.h"
+
+#ifdef __KERNEL__
+
+#define MEDUMMY_MAGIC_NUMBER	0xDDDD
+
+typedef struct medummy_device {
+	me_device_t base;			/**< The Meilhaus device base class. */
+//      int magic;                                      /**< The magic number of the structure */
+	unsigned short vendor_id;	/**< Vendor ID */
+	unsigned short device_id;	/**< Device ID */
+	unsigned int serial_no;		/**< Serial number of the device */
+	int bus_type;				/**< Bus type */
+	int bus_no;					/**< Bus number */
+	int dev_no;					/**< Device number */
+	int func_no;				/**< Function number */
+} medummy_device_t;
+
+me_device_t *medummy_constructor(unsigned short vendor_id,
+				 unsigned short device_id,
+				 unsigned int serial_no,
+				 int bus_type,
+				 int bus_no,
+				 int dev_no,
+				 int func_no) __attribute__ ((weak));
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/meerror.h b/drivers/staging/meilhaus/meerror.h
new file mode 100644
index 00000000000..9eda4bf907b
--- /dev/null
+++ b/drivers/staging/meilhaus/meerror.h
@@ -0,0 +1,100 @@
+/*
+ * Copyright (C) 2005 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * Source File : meerror.h
+ * Author      : GG (Guenter Gebhardt)  <g.gebhardt@meilhaus.de>
+ * Author      : KG (Krzysztof Gantzke)	<k.gantzke@meilhaus.de>
+ */
+
+#ifndef _MEERROR_H_
+#define _MEERROR_H_
+
+extern char *meErrorMsgTable[];
+
+#define ME_ERRNO_SUCCESS						0
+#define ME_ERRNO_INVALID_DEVICE					1
+#define ME_ERRNO_INVALID_SUBDEVICE				2
+#define ME_ERRNO_INVALID_CHANNEL				3
+#define ME_ERRNO_INVALID_SINGLE_CONFIG			4
+#define ME_ERRNO_INVALID_REF					5
+#define ME_ERRNO_INVALID_TRIG_CHAN				6
+#define ME_ERRNO_INVALID_TRIG_TYPE				7
+#define ME_ERRNO_INVALID_TRIG_EDGE				8
+#define ME_ERRNO_INVALID_TIMEOUT				9
+#define ME_ERRNO_INVALID_FLAGS					10
+#define ME_ERRNO_OPEN							11
+#define ME_ERRNO_CLOSE							12
+#define ME_ERRNO_NOT_OPEN						13
+#define ME_ERRNO_INVALID_DIR					14
+#define ME_ERRNO_PREVIOUS_CONFIG				15
+#define ME_ERRNO_NOT_SUPPORTED					16
+#define ME_ERRNO_SUBDEVICE_TYPE					17
+#define ME_ERRNO_USER_BUFFER_SIZE				18
+#define ME_ERRNO_LOCKED							19
+#define ME_ERRNO_NOMORE_SUBDEVICE_TYPE			20
+#define ME_ERRNO_TIMEOUT						21
+#define ME_ERRNO_SIGNAL							22
+#define ME_ERRNO_INVALID_IRQ_SOURCE				23
+#define ME_ERRNO_THREAD_RUNNING					24
+#define ME_ERRNO_START_THREAD					25
+#define ME_ERRNO_CANCEL_THREAD					26
+#define ME_ERRNO_NO_CALLBACK					27
+#define ME_ERRNO_USED							28
+#define ME_ERRNO_INVALID_UNIT					29
+#define ME_ERRNO_INVALID_MIN_MAX				30
+#define ME_ERRNO_NO_RANGE						31
+#define ME_ERRNO_INVALID_RANGE					32
+#define ME_ERRNO_SUBDEVICE_BUSY					33
+#define ME_ERRNO_INVALID_LOCK					34
+#define ME_ERRNO_INVALID_SWITCH					35
+#define ME_ERRNO_INVALID_ERROR_MSG_COUNT		36
+#define ME_ERRNO_INVALID_STREAM_CONFIG			37
+#define ME_ERRNO_INVALID_CONFIG_LIST_COUNT		38
+#define ME_ERRNO_INVALID_ACQ_START_TRIG_TYPE	39
+#define ME_ERRNO_INVALID_ACQ_START_TRIG_EDGE	40
+#define ME_ERRNO_INVALID_ACQ_START_TRIG_CHAN	41
+#define ME_ERRNO_INVALID_ACQ_START_TIMEOUT		42
+#define ME_ERRNO_INVALID_ACQ_START_ARG			43
+#define ME_ERRNO_INVALID_SCAN_START_TRIG_TYPE	44
+#define ME_ERRNO_INVALID_SCAN_START_ARG			45
+#define ME_ERRNO_INVALID_CONV_START_TRIG_TYPE	46
+#define ME_ERRNO_INVALID_CONV_START_ARG			47
+#define ME_ERRNO_INVALID_SCAN_STOP_TRIG_TYPE	48
+#define ME_ERRNO_INVALID_SCAN_STOP_ARG			49
+#define ME_ERRNO_INVALID_ACQ_STOP_TRIG_TYPE		50
+#define ME_ERRNO_INVALID_ACQ_STOP_ARG			51
+#define ME_ERRNO_SUBDEVICE_NOT_RUNNING			52
+#define ME_ERRNO_INVALID_READ_MODE				53
+#define ME_ERRNO_INVALID_VALUE_COUNT			54
+#define ME_ERRNO_INVALID_WRITE_MODE				55
+#define ME_ERRNO_INVALID_TIMER					56
+#define ME_ERRNO_DEVICE_UNPLUGGED				57
+#define ME_ERRNO_USED_INTERNAL					58
+#define ME_ERRNO_INVALID_DUTY_CYCLE				59
+#define ME_ERRNO_INVALID_WAIT					60
+#define ME_ERRNO_CONNECT_REMOTE					61
+#define ME_ERRNO_COMMUNICATION					62
+#define ME_ERRNO_INVALID_SINGLE_LIST			63
+#define ME_ERRNO_INVALID_MODULE_TYPE			64
+#define ME_ERRNO_INVALID_START_MODE				65
+#define ME_ERRNO_INVALID_STOP_MODE				66
+#define ME_ERRNO_INVALID_FIFO_IRQ_THRESHOLD		67
+#define ME_ERRNO_INVALID_POINTER				68
+#define ME_ERRNO_CREATE_EVENT					69
+#define ME_ERRNO_LACK_OF_RESOURCES				70
+#define ME_ERRNO_CANCELLED						71
+#define ME_ERRNO_RING_BUFFER_OVERFLOW			72
+#define ME_ERRNO_RING_BUFFER_UNDEFFLOW			73
+#define ME_ERRNO_INVALID_IRQ_EDGE				74
+#define ME_ERRNO_INVALID_IRQ_ARG				75
+#define ME_ERRNO_INVALID_CAP					76
+#define ME_ERRNO_INVALID_CAP_ARG_COUNT			77
+#define ME_ERRNO_INTERNAL						78
+
+/** New error for range check */
+#define ME_ERRNO_VALUE_OUT_OF_RANGE				79
+#define ME_ERRNO_FIFO_BUFFER_OVERFLOW			80
+#define ME_ERRNO_FIFO_BUFFER_UNDEFFLOW			81
+
+#define ME_ERRNO_INVALID_ERROR_NUMBER			82
+#endif
diff --git a/drivers/staging/meilhaus/mefirmware.c b/drivers/staging/meilhaus/mefirmware.c
new file mode 100644
index 00000000000..c07d202e8cb
--- /dev/null
+++ b/drivers/staging/meilhaus/mefirmware.c
@@ -0,0 +1,137 @@
+/**
+ * @file mefirmware.c
+ *
+ * @brief Implements the firmware handling.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/***************************************************************************
+ *   Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)     *
+ *   Copyright (C) 2007 by Krzysztof Gantzke k.gantzke@meilhaus.de         *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+
+#ifndef __KERNEL__
+# define __KERNEL__
+#endif
+
+#ifndef KBUILD_MODNAME
+#  define KBUILD_MODNAME KBUILD_STR(mefirmware)
+#endif
+
+#include <linux/pci.h>
+#include <linux/delay.h>
+
+#include <linux/firmware.h>
+
+#include "meplx_reg.h"
+#include "medebug.h"
+
+#include "mefirmware.h"
+
+int me_xilinx_download(unsigned long register_base_control,
+		       unsigned long register_base_data,
+		       struct device *dev, const char *firmware_name)
+{
+	int err = ME_ERRNO_FIRMWARE;
+	uint32_t value = 0;
+	int idx = 0;
+
+	const struct firmware *fw;
+
+	PDEBUG("executed.\n");
+
+	if (!firmware_name) {
+		PERROR("Request for firmware failed. No name provided. \n");
+		return err;
+	}
+
+	PINFO("Request '%s' firmware.\n", firmware_name);
+	err = request_firmware(&fw, firmware_name, dev);
+
+	if (err) {
+		PERROR("Request for firmware failed.\n");
+		return err;
+	}
+	// Set PLX local interrupt 2 polarity to high.
+	// Interrupt is thrown by init pin of xilinx.
+	outl(PLX_INTCSR_LOCAL_INT2_POL, register_base_control + PLX_INTCSR);
+
+	// Set /CS and /WRITE of the Xilinx
+	value = inl(register_base_control + PLX_ICR);
+	value |= ME_FIRMWARE_CS_WRITE;
+	outl(value, register_base_control + PLX_ICR);
+
+	// Init Xilinx with CS1
+	inl(register_base_data + ME_XILINX_CS1_REG);
+
+	// Wait for init to complete
+	udelay(20);
+
+	// Checkl /INIT pin
+	if (!
+	    (inl(register_base_control + PLX_INTCSR) &
+	     PLX_INTCSR_LOCAL_INT2_STATE)) {
+		PERROR("Can't init Xilinx.\n");
+		release_firmware(fw);
+		return -EIO;
+	}
+	// Reset /CS and /WRITE of the Xilinx
+	value = inl(register_base_control + PLX_ICR);
+	value &= ~ME_FIRMWARE_CS_WRITE;
+	outl(value, register_base_control + PLX_ICR);
+
+	// Download Xilinx firmware
+	udelay(10);
+
+	for (idx = 0; idx < fw->size; idx++) {
+		outl(fw->data[idx], register_base_data);
+#ifdef ME6000_v2_4
+///     This checking only for board's version 2.4
+		// Check if BUSY flag is set (low = ready, high = busy)
+		if (inl(register_base_control + PLX_ICR) &
+		    ME_FIRMWARE_BUSY_FLAG) {
+			PERROR("Xilinx is still busy (idx = %d)\n", idx);
+			release_firmware(fw);
+			return -EIO;
+		}
+#endif //ME6000_v2_4
+	}
+	PDEBUG("Download finished. %d bytes written to PLX.\n", idx);
+
+	// If done flag is high download was successful
+	if (inl(register_base_control + PLX_ICR) & ME_FIRMWARE_DONE_FLAG) {
+		PDEBUG("SUCCESS. Done flag is set.\n");
+	} else {
+		PERROR("FAILURE. DONE flag is not set.\n");
+		release_firmware(fw);
+		return -EIO;
+	}
+
+	// Set /CS and /WRITE
+	value = inl(register_base_control + PLX_ICR);
+	value |= ME_FIRMWARE_CS_WRITE;
+	outl(value, register_base_control + PLX_ICR);
+
+	PDEBUG("Enable interrupts on the PCI interface.\n");
+	outl(ME_PLX_PCI_ACTIVATE, register_base_control + PLX_INTCSR);
+	release_firmware(fw);
+
+	return 0;
+}
diff --git a/drivers/staging/meilhaus/mefirmware.h b/drivers/staging/meilhaus/mefirmware.h
new file mode 100644
index 00000000000..a2685080c97
--- /dev/null
+++ b/drivers/staging/meilhaus/mefirmware.h
@@ -0,0 +1,57 @@
+/**
+ * @file mefirmware.h
+ *
+ * @brief Definitions of the firmware handling functions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/***************************************************************************
+ *   Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)     *
+ *   Copyright (C) 2007 by Krzysztof Gantzke k.gantzke@meilhaus.de         *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ ***************************************************************************/
+
+#ifndef _MEFIRMWARE_H
+# define _MEFIRMWARE_H
+
+# ifdef __KERNEL__
+
+#define ME_ERRNO_FIRMWARE		-1
+
+/**
+* Registry
+*/
+#define ME_XILINX_CS1_REG		0x00C8
+
+/**
+* Flags (bits)
+*/
+
+#define ME_FIRMWARE_BUSY_FLAG	0x00000020
+#define ME_FIRMWARE_DONE_FLAG	0x00000004
+#define ME_FIRMWARE_CS_WRITE	0x00000100
+
+#define ME_PLX_PCI_ACTIVATE		0x43
+
+int me_xilinx_download(unsigned long register_base_control,
+		       unsigned long register_base_data,
+		       struct device *dev, const char *firmware_name);
+
+# endif	//__KERNEL__
+
+#endif //_MEFIRMWARE_H
diff --git a/drivers/staging/meilhaus/meids.h b/drivers/staging/meilhaus/meids.h
new file mode 100644
index 00000000000..b3e757cbdda
--- /dev/null
+++ b/drivers/staging/meilhaus/meids.h
@@ -0,0 +1,31 @@
+/*
+ * Copyright (C) 2005 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * Source File : meids.h
+ * Author      : GG (Guenter Gebhardt)  <g.gebhardt@meilhaus.de>
+ */
+
+#ifndef _MEIDS_H_
+#define _MEIDS_H_
+
+#ifdef __KERNEL__
+
+/*=============================================================================
+  Driver names
+  ===========================================================================*/
+
+#define MEMAIN_NAME				"memain"
+#define ME1000_NAME				"me1000"
+#define ME1400_NAME				"me1400"
+#define ME1600_NAME				"me1600"
+#define ME4600_NAME				"me4600"
+#define ME6000_NAME				"me6000"
+#define ME0600_NAME				"me0600"	//"me630"
+#define ME8100_NAME				"me8100"
+#define ME8200_NAME				"me8200"
+#define ME0900_NAME				"me0900"	//"me9x"
+//#define MEPHISTO_S1_NAME                      "mephisto_s1"
+#define MEDUMMY_NAME			"medummy"
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/meinternal.h b/drivers/staging/meilhaus/meinternal.h
new file mode 100644
index 00000000000..8d126b4905a
--- /dev/null
+++ b/drivers/staging/meilhaus/meinternal.h
@@ -0,0 +1,363 @@
+/*
+ * Copyright (C) 2005 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * Source File : meinternal.h
+ * Author      : GG (Guenter Gebhardt)  <g.gebhardt@meilhaus.de>
+ */
+
+#ifndef _MEINTERNAL_H_
+#define _MEINTERNAL_H_
+
+/*=============================================================================
+  PCI Vendor IDs
+  ===========================================================================*/
+
+#define PCI_VENDOR_ID_MEILHAUS						0x1402
+
+/*=============================================================================
+  PCI Device IDs
+  ===========================================================================*/
+
+#define PCI_DEVICE_ID_MEILHAUS_ME1000				0x1000
+#define PCI_DEVICE_ID_MEILHAUS_ME1000_A				0x100A
+#define PCI_DEVICE_ID_MEILHAUS_ME1000_B				0x100B
+
+#define PCI_DEVICE_ID_MEILHAUS_ME1400				0x1400
+#define PCI_DEVICE_ID_MEILHAUS_ME140A				0x140A
+#define PCI_DEVICE_ID_MEILHAUS_ME140B				0x140B
+#define PCI_DEVICE_ID_MEILHAUS_ME14E0				0x14E0
+#define PCI_DEVICE_ID_MEILHAUS_ME14EA				0x14EA
+#define PCI_DEVICE_ID_MEILHAUS_ME14EB				0x14EB
+#define PCI_DEVICE_ID_MEILHAUS_ME140C				0X140C
+#define PCI_DEVICE_ID_MEILHAUS_ME140D				0X140D
+
+#define PCI_DEVICE_ID_MEILHAUS_ME1600_4U			0x1604 // 4 voltage outputs
+#define PCI_DEVICE_ID_MEILHAUS_ME1600_8U			0x1608 // 8 voltage outputs
+#define PCI_DEVICE_ID_MEILHAUS_ME1600_12U			0x160C // 12 voltage outputs
+#define PCI_DEVICE_ID_MEILHAUS_ME1600_16U			0x160F // 16 voltage outputs
+#define PCI_DEVICE_ID_MEILHAUS_ME1600_16U_8I		0x168F // 16 voltage/8 current o.
+
+#define PCI_DEVICE_ID_MEILHAUS_ME4610				0x4610 // Jekyll
+
+#define PCI_DEVICE_ID_MEILHAUS_ME4650				0x4650 // Low Cost version
+
+#define PCI_DEVICE_ID_MEILHAUS_ME4660				0x4660 // Standard version
+#define PCI_DEVICE_ID_MEILHAUS_ME4660I				0x4661 // Isolated version
+#define PCI_DEVICE_ID_MEILHAUS_ME4660S				0x4662 // Standard version with Sample and Hold
+#define PCI_DEVICE_ID_MEILHAUS_ME4660IS				0x4663 // Isolated version with Sample and Hold
+
+#define PCI_DEVICE_ID_MEILHAUS_ME4670				0x4670 // Standard version
+#define PCI_DEVICE_ID_MEILHAUS_ME4670I				0x4671 // Isolated version
+#define PCI_DEVICE_ID_MEILHAUS_ME4670S				0x4672 // Standard version with Sample and Hold
+#define PCI_DEVICE_ID_MEILHAUS_ME4670IS				0x4673 // Isolated version with Sample and Hold
+
+#define PCI_DEVICE_ID_MEILHAUS_ME4680				0x4680 // Standard version
+#define PCI_DEVICE_ID_MEILHAUS_ME4680I				0x4681 // Isolated version
+#define PCI_DEVICE_ID_MEILHAUS_ME4680S				0x4682 // Standard version with Sample and Hold
+#define PCI_DEVICE_ID_MEILHAUS_ME4680IS				0x4683 // Isolated version with Sample and Hold
+
+/* ME6000 standard version */
+#define PCI_DEVICE_ID_MEILHAUS_ME6004   			0x6004
+#define PCI_DEVICE_ID_MEILHAUS_ME6008   			0x6008
+#define PCI_DEVICE_ID_MEILHAUS_ME600F   			0x600F
+
+/* ME6000 isolated version */
+#define PCI_DEVICE_ID_MEILHAUS_ME6014   			0x6014
+#define PCI_DEVICE_ID_MEILHAUS_ME6018   			0x6018
+#define PCI_DEVICE_ID_MEILHAUS_ME601F   			0x601F
+
+/* ME6000 isle version */
+#define PCI_DEVICE_ID_MEILHAUS_ME6034   			0x6034
+#define PCI_DEVICE_ID_MEILHAUS_ME6038   			0x6038
+#define PCI_DEVICE_ID_MEILHAUS_ME603F   			0x603F
+
+/* ME6000 standard version with DIO */
+#define PCI_DEVICE_ID_MEILHAUS_ME6044   			0x6044
+#define PCI_DEVICE_ID_MEILHAUS_ME6048   			0x6048
+#define PCI_DEVICE_ID_MEILHAUS_ME604F   			0x604F
+
+/* ME6000 isolated version with DIO */
+#define PCI_DEVICE_ID_MEILHAUS_ME6054   			0x6054
+#define PCI_DEVICE_ID_MEILHAUS_ME6058   			0x6058
+#define PCI_DEVICE_ID_MEILHAUS_ME605F   			0x605F
+
+/* ME6000 isle version with DIO */
+#define PCI_DEVICE_ID_MEILHAUS_ME6074   			0x6074
+#define PCI_DEVICE_ID_MEILHAUS_ME6078   			0x6078
+#define PCI_DEVICE_ID_MEILHAUS_ME607F   			0x607F
+
+/* ME6100 standard version */
+#define PCI_DEVICE_ID_MEILHAUS_ME6104   			0x6104
+#define PCI_DEVICE_ID_MEILHAUS_ME6108   			0x6108
+#define PCI_DEVICE_ID_MEILHAUS_ME610F   			0x610F
+
+/* ME6100 isolated version */
+#define PCI_DEVICE_ID_MEILHAUS_ME6114   			0x6114
+#define PCI_DEVICE_ID_MEILHAUS_ME6118   			0x6118
+#define PCI_DEVICE_ID_MEILHAUS_ME611F   			0x611F
+
+/* ME6100 isle version */
+#define PCI_DEVICE_ID_MEILHAUS_ME6134   			0x6134
+#define PCI_DEVICE_ID_MEILHAUS_ME6138   			0x6138
+#define PCI_DEVICE_ID_MEILHAUS_ME613F   			0x613F
+
+/* ME6100 standard version with DIO */
+#define PCI_DEVICE_ID_MEILHAUS_ME6144				0x6144
+#define PCI_DEVICE_ID_MEILHAUS_ME6148   			0x6148
+#define PCI_DEVICE_ID_MEILHAUS_ME614F   			0x614F
+
+/* ME6100 isolated version with DIO */
+#define PCI_DEVICE_ID_MEILHAUS_ME6154   			0x6154
+#define PCI_DEVICE_ID_MEILHAUS_ME6158   			0x6158
+#define PCI_DEVICE_ID_MEILHAUS_ME615F   			0x615F
+
+/* ME6100 isle version with DIO */
+#define PCI_DEVICE_ID_MEILHAUS_ME6174   			0x6174
+#define PCI_DEVICE_ID_MEILHAUS_ME6178   			0x6178
+#define PCI_DEVICE_ID_MEILHAUS_ME617F   			0x617F
+
+/* ME6200 isolated version with DIO */
+#define PCI_DEVICE_ID_MEILHAUS_ME6259				0x6259
+
+/* ME6300 isolated version with DIO */
+#define PCI_DEVICE_ID_MEILHAUS_ME6359				0x6359
+
+/* ME0630 */
+#define PCI_DEVICE_ID_MEILHAUS_ME0630				0x0630
+
+/* ME8100 */
+#define PCI_DEVICE_ID_MEILHAUS_ME8100_A				0x810A
+#define PCI_DEVICE_ID_MEILHAUS_ME8100_B  			0x810B
+
+/* ME8200 */
+#define PCI_DEVICE_ID_MEILHAUS_ME8200_A				0x820A
+#define PCI_DEVICE_ID_MEILHAUS_ME8200_B  			0x820B
+
+/* ME0900 */
+#define PCI_DEVICE_ID_MEILHAUS_ME0940				0x0940
+#define PCI_DEVICE_ID_MEILHAUS_ME0950				0x0950
+#define PCI_DEVICE_ID_MEILHAUS_ME0960				0x0960
+
+
+/*=============================================================================
+  USB Vendor IDs
+  ===========================================================================*/
+
+//#define USB_VENDOR_ID_MEPHISTO_S1					0x0403
+
+
+/*=============================================================================
+  USB Device IDs
+  ===========================================================================*/
+
+//#define USB_DEVICE_ID_MEPHISTO_S1					0xDCD0
+
+
+/* ME-1000 defines */
+#define ME1000_NAME_DRIVER							"ME-1000"
+
+#define ME1000_NAME_DEVICE_ME1000					"ME-1000"
+
+#define ME1000_DESCRIPTION_DEVICE_ME1000			"ME-1000 device, 128 digital i/o lines."
+
+/* ME-1400 defines */
+#define ME1400_NAME_DRIVER							"ME-1400"
+
+#define ME1400_NAME_DEVICE_ME1400					"ME-1400"
+#define ME1400_NAME_DEVICE_ME1400E					"ME-1400E"
+#define ME1400_NAME_DEVICE_ME1400A					"ME-1400A"
+#define ME1400_NAME_DEVICE_ME1400EA					"ME-1400EA"
+#define ME1400_NAME_DEVICE_ME1400B					"ME-1400B"
+#define ME1400_NAME_DEVICE_ME1400EB					"ME-1400EB"
+#define ME1400_NAME_DEVICE_ME1400C					"ME-1400C"
+#define ME1400_NAME_DEVICE_ME1400D					"ME-1400D"
+
+#define ME1400_DESCRIPTION_DEVICE_ME1400			"ME-1400 device, 24 digital i/o lines."
+#define ME1400_DESCRIPTION_DEVICE_ME1400E			"ME-1400E device, 24 digital i/o lines."
+#define ME1400_DESCRIPTION_DEVICE_ME1400A			"ME-1400A device, 24 digital i/o lines, 3 counters."
+#define ME1400_DESCRIPTION_DEVICE_ME1400EA			"ME-1400EA device, 24 digital i/o lines, 3 counters."
+#define ME1400_DESCRIPTION_DEVICE_ME1400B			"ME-1400B device, 48 digital i/o lines, 6 counters."
+#define ME1400_DESCRIPTION_DEVICE_ME1400EB			"ME-1400EB device, 48 digital i/o lines, 6 counters."
+#define ME1400_DESCRIPTION_DEVICE_ME1400C			"ME-1400C device, 24 digital i/o lines, 15 counters."
+#define ME1400_DESCRIPTION_DEVICE_ME1400D			"ME-1400D device, 48 digital i/o lines, 30 counters."
+
+/* ME-1600 defines */
+#define ME1600_NAME_DRIVER							"ME-1600"
+
+#define ME1600_NAME_DEVICE_ME16004U					"ME-1600/4U"
+#define ME1600_NAME_DEVICE_ME16008U					"ME-1600/8U"
+#define ME1600_NAME_DEVICE_ME160012U				"ME-1600/12U"
+#define ME1600_NAME_DEVICE_ME160016U				"ME-1600/16U"
+#define ME1600_NAME_DEVICE_ME160016U8I				"ME-1600/16U8I"
+
+#define ME1600_DESCRIPTION_DEVICE_ME16004U			"ME-1600/4U device, 4 voltage outputs."
+#define ME1600_DESCRIPTION_DEVICE_ME16008U			"ME-1600/8U device, 8 voltage outputs."
+#define ME1600_DESCRIPTION_DEVICE_ME160012U			"ME-1600/12U device, 12 voltage outputs."
+#define ME1600_DESCRIPTION_DEVICE_ME160016U			"ME-1600/16U device, 16 voltage outputs."
+#define ME1600_DESCRIPTION_DEVICE_ME160016U8I		"ME-1600/16U8I device, 16 voltage, 8 current outputs."
+
+/* ME-4000 defines */
+#define ME4600_NAME_DRIVER							"ME-4600"
+
+#define ME4600_NAME_DEVICE_ME4610					"ME-4610"
+#define ME4600_NAME_DEVICE_ME4650					"ME-4650"
+#define ME4600_NAME_DEVICE_ME4660					"ME-4660"
+#define ME4600_NAME_DEVICE_ME4660I					"ME-4660I"
+#define ME4600_NAME_DEVICE_ME4660S					"ME-4660S"
+#define ME4600_NAME_DEVICE_ME4660IS					"ME-4660IS"
+#define ME4600_NAME_DEVICE_ME4670					"ME-4670"
+#define ME4600_NAME_DEVICE_ME4670I					"ME-4670I"
+#define ME4600_NAME_DEVICE_ME4670S					"ME-4670S"
+#define ME4600_NAME_DEVICE_ME4670IS					"ME-4670IS"
+#define ME4600_NAME_DEVICE_ME4680					"ME-4680"
+#define ME4600_NAME_DEVICE_ME4680I					"ME-4680I"
+#define ME4600_NAME_DEVICE_ME4680S					"ME-4680S"
+#define ME4600_NAME_DEVICE_ME4680IS					"ME-4680IS"
+
+#define ME4600_DESCRIPTION_DEVICE_ME4610			"ME-4610 device, 16 streaming analog inputs, 32 digital i/o lines, 3 counters, 1 external interrupt."
+#define ME4600_DESCRIPTION_DEVICE_ME4650			"ME-4650 device, 16 streaming analog inputs, 32 digital i/o lines, 1 external interrupt."
+#define ME4600_DESCRIPTION_DEVICE_ME4660			"ME-4660 device, 16 streaming analog inputs, 2 single analog outputs, 32 digital i/o lines, 3 counters, 1 external interrupt."
+#define ME4600_DESCRIPTION_DEVICE_ME4660I			"ME-4660I opto isolated device, 16 streaming analog inputs, 2 single analog outputs, 32 digital i/o lines, 3 counters, 1 external interrupt."
+#define ME4600_DESCRIPTION_DEVICE_ME4660S			"ME-4660 device, 16 streaming analog inputs (8 S&H), 2 single analog outputs, 32 digital i/o lines, 3 counters, 1 external interrupt."
+#define ME4600_DESCRIPTION_DEVICE_ME4660IS			"ME-4660I opto isolated device, 16 streaming analog inputs (8 S&H), 2 single analog outputs, 32 digital i/o lines, 3 counters, 1 external interrupt."
+#define ME4600_DESCRIPTION_DEVICE_ME4670			"ME-4670 device, 32 streaming analog inputs, 4 single analog outputs, 32 digital i/o lines, 3 counters, 1 external interrupt."
+#define ME4600_DESCRIPTION_DEVICE_ME4670I			"ME-4670I opto isolated device, 32 streaming analog inputs, 4 single analog outputs, 32 digital i/o lines, 3 counters, 1 external interrupt."
+#define ME4600_DESCRIPTION_DEVICE_ME4670S			"ME-4670S device, 32 streaming analog inputs (8 S&H), 4 single analog outputs, 32 digital i/o lines, 3 counters, 1 external interrupt."
+#define ME4600_DESCRIPTION_DEVICE_ME4670IS			"ME-4670IS opto isolated device, 32 streaming analog inputs (8 S&H), 4 single analog outputs, 32 digital i/o lines, 3 counters, 1 external interrupt."
+#define ME4600_DESCRIPTION_DEVICE_ME4680			"ME-4680 device, 32 streaming analog inputs, 4 streaming analog outputs, 32 digital i/o lines, 3 counters, 1 external interrupt."
+#define ME4600_DESCRIPTION_DEVICE_ME4680I			"ME-4680I opto isolated device, 32 streaming analog inputs, 4 streaming analog outputs, 32 digital i/o lines, 3 counters, 1 external interrupt."
+#define ME4600_DESCRIPTION_DEVICE_ME4680S			"ME-4680S device, 32 streaming analog inputs, 4 streaming analog outputs, 32 digital i/o lines, 3 counters, 1 external interrupt."
+#define ME4600_DESCRIPTION_DEVICE_ME4680IS			"ME-4680IS opto isolated device, 32 streaming analog inputs (8 S&H), 4 streaming analog outputs, 32 digital i/o lines, 3 counters, 1 external interrupt."
+
+/* ME-6000 defines */
+#define ME6000_NAME_DRIVER							"ME-6000"
+
+#define ME6000_NAME_DEVICE_ME60004					"ME-6000/4"
+#define ME6000_NAME_DEVICE_ME60008					"ME-6000/8"
+#define ME6000_NAME_DEVICE_ME600016					"ME-6000/16"
+#define ME6000_NAME_DEVICE_ME6000I4					"ME-6000I/4"
+#define ME6000_NAME_DEVICE_ME6000I8					"ME-6000I/8"
+#define ME6000_NAME_DEVICE_ME6000I16				"ME-6000I/16"
+#define ME6000_NAME_DEVICE_ME6000ISLE4				"ME-6000ISLE/4"
+#define ME6000_NAME_DEVICE_ME6000ISLE8				"ME-6000ISLE/8"
+#define ME6000_NAME_DEVICE_ME6000ISLE16				"ME-6000ISLE/16"
+#define ME6000_NAME_DEVICE_ME61004					"ME-6100/4"
+#define ME6000_NAME_DEVICE_ME61008					"ME-6100/8"
+#define ME6000_NAME_DEVICE_ME610016					"ME-6100/16"
+#define ME6000_NAME_DEVICE_ME6100I4					"ME-6100I/4"
+#define ME6000_NAME_DEVICE_ME6100I8					"ME-6100I/8"
+#define ME6000_NAME_DEVICE_ME6100I16				"ME-6100I/16"
+#define ME6000_NAME_DEVICE_ME6100ISLE4				"ME-6100ISLE/4"
+#define ME6000_NAME_DEVICE_ME6100ISLE8				"ME-6100ISLE/8"
+#define ME6000_NAME_DEVICE_ME6100ISLE16				"ME-6100ISLE/16"
+#define ME6000_NAME_DEVICE_ME60004DIO				"ME-6000/4/DIO"
+#define ME6000_NAME_DEVICE_ME60008DIO				"ME-6000/8/DIO"
+#define ME6000_NAME_DEVICE_ME600016DIO				"ME-6000/16/DIO"
+#define ME6000_NAME_DEVICE_ME6000I4DIO				"ME-6000I/4/DIO"
+#define ME6000_NAME_DEVICE_ME6000I8DIO				"ME-6000I/8/DIO"
+#define ME6000_NAME_DEVICE_ME6000I16DIO				"ME-6000I/16/DIO"
+#define ME6000_NAME_DEVICE_ME6000ISLE4DIO			"ME-6000ISLE/4/DIO"
+#define ME6000_NAME_DEVICE_ME6000ISLE8DIO			"ME-6000ISLE/8/DIO"
+#define ME6000_NAME_DEVICE_ME6000ISLE16DIO			"ME-6000ISLE/16/DIO"
+#define ME6000_NAME_DEVICE_ME61004DIO				"ME-6100/4/DIO"
+#define ME6000_NAME_DEVICE_ME61008DIO				"ME-6100/8/DIO"
+#define ME6000_NAME_DEVICE_ME610016DIO				"ME-6100/16/DIO"
+#define ME6000_NAME_DEVICE_ME6100I4DIO				"ME-6100I/4/DIO"
+#define ME6000_NAME_DEVICE_ME6100I8DIO				"ME-6100I/8/DIO"
+#define ME6000_NAME_DEVICE_ME6100I16DIO				"ME-6100I/16/DIO"
+#define ME6000_NAME_DEVICE_ME6100ISLE4DIO			"ME-6100ISLE/4/DIO"
+#define ME6000_NAME_DEVICE_ME6100ISLE8DIO			"ME-6100ISLE/8/DIO"
+#define ME6000_NAME_DEVICE_ME6100ISLE16DIO			"ME-6100ISLE/16/DIO"
+#define ME6000_NAME_DEVICE_ME6200I9DIO				"ME-6200I/9/DIO"
+#define ME6000_NAME_DEVICE_ME6300I9DIO				"ME-6300I/9/DIO"
+
+#define ME6000_DESCRIPTION_DEVICE_ME60004			"ME-6000/4 device, 4 single analog outputs."
+#define ME6000_DESCRIPTION_DEVICE_ME60008			"ME-6000/8 device, 8 single analog outputs"
+#define ME6000_DESCRIPTION_DEVICE_ME600016			"ME-6000/16 device, 16 single analog outputs"
+#define ME6000_DESCRIPTION_DEVICE_ME6000I4			"ME-6000I/4 isolated device, 4 single analog outputs"
+#define ME6000_DESCRIPTION_DEVICE_ME6000I8			"ME-6000I/8 isolated device, 8 single analog outputs"
+#define ME6000_DESCRIPTION_DEVICE_ME6000I16			"ME-6000I/16 isolated device, 16 single analog outputs"
+#define ME6000_DESCRIPTION_DEVICE_ME6000ISLE4		"ME-6000ISLE/4 isle device, 4 single analog outputs"
+#define ME6000_DESCRIPTION_DEVICE_ME6000ISLE8		"ME-6000ISLE/8 isle device, 8 single analog outputs"
+#define ME6000_DESCRIPTION_DEVICE_ME6000ISLE16		"ME-6000ISLE/16 isle device, 16 single analog outputs"
+#define ME6000_DESCRIPTION_DEVICE_ME61004			"ME-6100/4 device, 4 streaming analog outputs."
+#define ME6000_DESCRIPTION_DEVICE_ME61008			"ME-6100/8 device, 4 streaming, 4 single analog outputs."
+#define ME6000_DESCRIPTION_DEVICE_ME610016			"ME-6100/16 device, 4 streaming, 12 single analog outputs."
+#define ME6000_DESCRIPTION_DEVICE_ME6100I4			"ME-6100I/4 isolated device, 4 streaming analog outputs."
+#define ME6000_DESCRIPTION_DEVICE_ME6100I8			"ME-6100I/8 isolated device, 4 streaming, 4 single analog outputs."
+#define ME6000_DESCRIPTION_DEVICE_ME6100I16			"ME-6100I/16 isolated device, 4 streaming, 12 single analog outputs."
+#define ME6000_DESCRIPTION_DEVICE_ME6100ISLE4		"ME-6100ISLE/4 isle device, 4 streaming analog outputs."
+#define ME6000_DESCRIPTION_DEVICE_ME6100ISLE8		"ME-6100ISLE/8 isle device, 4 streaming, 4 single analog outputs."
+#define ME6000_DESCRIPTION_DEVICE_ME6100ISLE16		"ME-6100ISLE/16 isle device, 4 streaming, 12 single analog outputs."
+#define ME6000_DESCRIPTION_DEVICE_ME60004DIO		"ME-6000/4/DIO device, 4 single analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME60008DIO		"ME-6000/8/DIO device, 8 single analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME600016DIO		"ME-6000/16/DIO device, 8 single analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME6000I4DIO		"ME-6000I/4/DIO isolated device, 4 single analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME6000I8DIO		"ME-6000I/8/DIO isolated device, 8 single analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME6000I16DIO		"ME-6000I/16/DIO isolated device, 16 single analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME6000ISLE4DIO	"ME-6000ISLE/4/DIO isle device, 4 single analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME6000ISLE8DIO	"ME-6000ISLE/8/DIO isle device, 8 single analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME6000ISLE16DIO	"ME-6000ISLE/16/DIO isle device, 16 single analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME61004DIO		"ME-6100/4/DIO device, 4 streaming analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME61008DIO		"ME-6100/8/DIO device, 4 streaming, 4 single analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME610016DIO		"ME-6100/16/DIO device, 4 streaming, 12 single analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME6100I4DIO		"ME-6100I/4/DIO isolated device, 4 streaming analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME6100I8DIO		"ME-6100I/8/DIO isolated device, 4 streaming, 4 single analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME6100I16DIO		"ME-6100I/16/DIO isolated device, 4 streaming, 12 single analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME6100ISLE4DIO	"ME-6100ISLE/4/DIO isle device, 4 streaming analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME6100ISLE8DIO	"ME-6100ISLE/8/DIO isle device, 4 streaming, 4 single analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME6100ISLE16DIO	"ME-6100ISLE/16/DIO isle device, 4 streaming, 12 single analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME6200I9DIO		"ME-6200I/9/DIO isolated device, 9 single analog outputs, 16 digital i/o lines."
+#define ME6000_DESCRIPTION_DEVICE_ME6300I9DIO		"ME-6300I/9/DIO isolated device, 4 streaming, 5 single analog outputs, 16 digital i/o lines."
+
+/* ME-630 defines */
+#define ME0600_NAME_DRIVER							"ME-0600"
+
+#define ME0600_NAME_DEVICE_ME0630					"ME-630"
+
+#define ME0600_DESCRIPTION_DEVICE_ME0630			"ME-630 device, up to 16 relay, 8 digital ttl input lines, 8 isolated digital input lines, 16 digital i/o lines, 2 external interrupts."
+
+/* ME-8100 defines */
+#define ME8100_NAME_DRIVER							"ME-8100"
+
+#define ME8100_NAME_DEVICE_ME8100A					"ME-8100A"
+#define ME8100_NAME_DEVICE_ME8100B					"ME-8100B"
+
+#define ME8100_DESCRIPTION_DEVICE_ME8100A			"ME-8100A opto isolated device, 16 digital input lines, 16 digital output lines."
+#define ME8100_DESCRIPTION_DEVICE_ME8100B			"ME-8100B opto isolated device, 32 digital input lines, 32 digital output lines, 3 counters."
+
+/* ME-8200 defines */
+#define ME8200_NAME_DRIVER							"ME-8200"
+
+#define ME8200_NAME_DEVICE_ME8200A					"ME-8200A"
+#define ME8200_NAME_DEVICE_ME8200B					"ME-8200B"
+
+#define ME8200_DESCRIPTION_DEVICE_ME8200A			"ME-8200A opto isolated device, 8 digital output lines, 8 digital input lines, 16 digital i/o lines."
+#define ME8200_DESCRIPTION_DEVICE_ME8200B			"ME-8200B opto isolated device, 16 digital output lines, 16 digital input lines, 16 digital i/o lines."
+
+/* ME-0900 defines */
+#define ME0900_NAME_DRIVER							"ME-0900"
+
+#define ME0900_NAME_DEVICE_ME0940					"ME-94"
+#define ME0900_NAME_DEVICE_ME0950					"ME-95"
+#define ME0900_NAME_DEVICE_ME0960					"ME-96"
+
+#define ME0900_DESCRIPTION_DEVICE_ME0940			"ME-94 device, 16 digital input lines, 2 external interrupt lines."
+#define ME0900_DESCRIPTION_DEVICE_ME0950			"ME-95 device, 16 digital output lines."
+#define ME0900_DESCRIPTION_DEVICE_ME0960			"ME-96 device, 8 digital input lines, 8 digital output lines, 2 external interrupt lines."
+
+/* ME-DUMMY defines */
+#define MEDUMMY_NAME_DRIVER							"ME-Dummy"
+
+/* MEPHISTO_S1 defines */
+/*
+#define MEPHISTO_S1_NAME_DRIVER						"MEphisto Scope 1"
+#define MEPHISTO_S1_NAME_DEVICE						"MEphisto Scope 1"
+#define MEPHISTO_S1_DESCRIPTION_DEVICE				"MEphisto Scope 1 device, 2 analog inputs, 24 digital i/o."
+*/
+/* Error defines */
+#define EMPTY_NAME_DRIVER							"ME-???"
+#define EMPTY_NAME_DEVICE							"ME-???"
+#define EMPTY_DESCRIPTION_DEVICE					"ME-??? unknown device"
+
+#endif
diff --git a/drivers/staging/meilhaus/meioctl.h b/drivers/staging/meilhaus/meioctl.h
new file mode 100644
index 00000000000..6dc719fba57
--- /dev/null
+++ b/drivers/staging/meilhaus/meioctl.h
@@ -0,0 +1,515 @@
+/*
+ * Copyright (C) 2005 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * Source File : meioctl.h
+ * Author      : GG (Guenter Gebhardt)  <g.gebhardt@meilhaus.de>
+ */
+
+#ifndef _MEIOCTL_H_
+#define _MEIOCTL_H_
+
+
+/*=============================================================================
+  Types for the input/output ioctls
+  ===========================================================================*/
+
+typedef struct me_io_irq_start {
+	int device;
+	int subdevice;
+	int channel;
+	int irq_source;
+	int irq_edge;
+	int irq_arg;
+	int flags;
+	int errno;
+} me_io_irq_start_t;
+
+
+typedef struct me_io_irq_wait {
+	int device;
+	int subdevice;
+	int channel;
+	int irq_count;
+	int value;
+	int time_out;
+	int flags;
+	int errno;
+} me_io_irq_wait_t;
+
+
+typedef struct me_io_irq_stop {
+	int device;
+	int subdevice;
+	int channel;
+	int flags;
+	int errno;
+} me_io_irq_stop_t;
+
+
+typedef struct me_io_reset_device {
+	int device;
+	int flags;
+	int errno;
+} me_io_reset_device_t;
+
+
+typedef struct me_io_reset_subdevice {
+	int device;
+	int subdevice;
+	int flags;
+	int errno;
+} me_io_reset_subdevice_t;
+
+
+typedef struct me_io_single_config {
+	int device;
+	int subdevice;
+	int channel;
+	int single_config;
+	int ref;
+	int trig_chan;
+	int trig_type;
+	int trig_edge;
+	int flags;
+	int errno;
+} me_io_single_config_t;
+
+
+typedef struct me_io_single {
+	meIOSingle_t *single_list;
+	int count;
+	int flags;
+	int errno;
+} me_io_single_t;
+
+
+typedef struct me_io_stream_config {
+	int device;
+	int subdevice;
+	meIOStreamConfig_t *config_list;
+	int count;
+	meIOStreamTrigger_t trigger;
+	int fifo_irq_threshold;
+	int flags;
+	int errno;
+} me_io_stream_config_t;
+
+
+typedef struct me_io_stream_new_values {
+	int device;
+	int subdevice;
+	int time_out;
+	int count;
+	int flags;
+	int errno;
+} me_io_stream_new_values_t;
+
+
+typedef struct me_io_stream_read {
+	int device;
+	int subdevice;
+	int read_mode;
+	int *values;
+	int count;
+	int flags;
+	int errno;
+} me_io_stream_read_t;
+
+
+typedef struct me_io_stream_start {
+	meIOStreamStart_t *start_list;
+	int count;
+	int flags;
+	int errno;
+} me_io_stream_start_t;
+
+
+typedef struct me_io_stream_status {
+	int device;
+	int subdevice;
+	int wait;
+	int status;
+	int count;
+	int flags;
+	int errno;
+} me_io_stream_status_t;
+
+
+typedef struct me_io_stream_stop {
+	meIOStreamStop_t *stop_list;
+	int count;
+	int flags;
+	int errno;
+} me_io_stream_stop_t;
+
+
+typedef struct me_io_stream_write {
+	int device;
+	int subdevice;
+	int write_mode;
+	int *values;
+	int count;
+	int flags;
+	int errno;
+} me_io_stream_write_t;
+
+
+/*=============================================================================
+  Types for the lock ioctls
+  ===========================================================================*/
+
+typedef struct me_lock_device {
+	int device;
+	int lock;
+	int flags;
+	int errno;
+} me_lock_device_t;
+
+
+typedef struct me_lock_driver {
+	int flags;
+	int lock;
+	int errno;
+} me_lock_driver_t;
+
+
+typedef struct me_lock_subdevice {
+	int device;
+	int subdevice;
+	int lock;
+	int flags;
+	int errno;
+} me_lock_subdevice_t;
+
+
+/*=============================================================================
+  Types for the query ioctls
+  ===========================================================================*/
+
+typedef struct me_query_info_device {
+	int device;
+	int vendor_id;
+	int device_id;
+	int serial_no;
+	int bus_type;
+	int bus_no;
+	int dev_no;
+	int func_no;
+	int plugged;
+	int errno;
+} me_query_info_device_t;
+
+
+typedef struct me_query_description_device {
+	int device;
+	char *name;
+	int count;
+	int errno;
+} me_query_description_device_t;
+
+
+typedef struct me_query_name_device {
+	int device;
+	char *name;
+	int count;
+	int errno;
+} me_query_name_device_t;
+
+
+typedef struct me_query_name_device_driver {
+	int device;
+	char *name;
+	int count;
+	int errno;
+} me_query_name_device_driver_t;
+
+
+typedef struct me_query_version_main_driver {
+	int version;
+	int errno;
+} me_query_version_main_driver_t;
+
+
+typedef struct me_query_version_device_driver {
+	int device;
+	int version;
+	int errno;
+} me_query_version_device_driver_t;
+
+
+typedef struct me_query_number_devices {
+	int number;
+	int errno;
+} me_query_number_devices_t;
+
+
+typedef struct me_query_number_subdevices {
+	int device;
+	int number;
+	int errno;
+} me_query_number_subdevices_t;
+
+
+typedef struct me_query_number_channels {
+	int device;
+	int subdevice;
+	int number;
+	int errno;
+} me_query_number_channels_t;
+
+
+typedef struct me_query_number_ranges {
+	int device;
+	int subdevice;
+	int channel;
+	int unit;
+	int number;
+	int errno;
+} me_query_number_ranges_t;
+
+
+typedef struct me_query_subdevice_by_type {
+	int device;
+	int start_subdevice;
+	int type;
+	int subtype;
+	int subdevice;
+	int errno;
+} me_query_subdevice_by_type_t;
+
+
+typedef struct me_query_subdevice_type {
+	int device;
+	int subdevice;
+	int type;
+	int subtype;
+	int errno;
+} me_query_subdevice_type_t;
+
+
+typedef struct me_query_subdevice_caps {
+	int device;
+	int subdevice;
+	int caps;
+	int errno;
+} me_query_subdevice_caps_t;
+
+
+typedef struct me_query_subdevice_caps_args {
+	int device;
+	int subdevice;
+	int cap;
+	int args[8];
+	int count;
+	int errno;
+} me_query_subdevice_caps_args_t;
+
+
+typedef struct me_query_timer {
+	int device;
+	int subdevice;
+	int timer;
+	int base_frequency;
+	long long min_ticks;
+	long long max_ticks;
+	int errno;
+} me_query_timer_t;
+
+
+typedef struct me_query_range_by_min_max {
+	int device;
+	int subdevice;
+	int channel;
+	int unit;
+	int min;
+	int max;
+	int max_data;
+	int range;
+	int errno;
+} me_query_range_by_min_max_t;
+
+
+typedef struct me_query_range_info {
+	int device;
+	int subdevice;
+	int channel;
+	int unit;
+	int range;
+	int min;
+	int max;
+	int max_data;
+	int errno;
+} me_query_range_info_t;
+
+
+/*=============================================================================
+  Types for the configuration ioctls
+  ===========================================================================*/
+
+typedef struct me_cfg_tcpip_location {
+	int access_type;
+	char *remote_host;
+	int remote_device_number;
+} me_cfg_tcpip_location_t;
+
+
+typedef union me_cfg_tcpip {
+	int access_type;
+	me_cfg_tcpip_location_t location;
+} me_cfg_tcpip_t;
+
+
+typedef struct me_cfg_pci_hw_location {
+	unsigned int bus_type;
+	unsigned int bus_no;
+	unsigned int device_no;
+	unsigned int function_no;
+} me_cfg_pci_hw_location_t;
+
+/*
+typedef struct me_cfg_usb_hw_location {
+	unsigned int bus_type;
+	unsigned int root_hub_no;
+} me_cfg_usb_hw_location_t;
+*/
+
+typedef union me_cfg_hw_location {
+	unsigned int bus_type;
+	me_cfg_pci_hw_location_t pci;
+//	me_cfg_usb_hw_location_t usb;
+} me_cfg_hw_location_t;
+
+
+typedef struct me_cfg_device_info {
+	unsigned int vendor_id;
+	unsigned int device_id;
+	unsigned int serial_no;
+	me_cfg_hw_location_t hw_location;
+} me_cfg_device_info_t;
+
+
+typedef struct me_cfg_subdevice_info {
+	int type;
+	int sub_type;
+	unsigned int number_channels;
+} me_cfg_subdevice_info_t;
+
+
+typedef struct me_cfg_range_entry {
+	int unit;
+	double min;
+	double max;
+	unsigned int max_data;
+} me_cfg_range_entry_t;
+
+
+typedef struct me_cfg_mux32m_device {
+	int type;
+	int timed;
+	unsigned int ai_channel;
+	unsigned int dio_device;
+	unsigned int dio_subdevice;
+	unsigned int timer_device;
+	unsigned int timer_subdevice;
+	unsigned int mux32s_count;
+} me_cfg_mux32m_device_t;
+
+
+typedef struct me_cfg_demux32_device {
+	int type;
+	int timed;
+	unsigned int ao_channel;
+	unsigned int dio_device;
+	unsigned int dio_subdevice;
+	unsigned int timer_device;
+	unsigned int timer_subdevice;
+} me_cfg_demux32_device_t;
+
+
+typedef union me_cfg_external_device {
+	int type;
+	me_cfg_mux32m_device_t mux32m;
+	me_cfg_demux32_device_t demux32;
+} me_cfg_external_device_t;
+
+
+typedef struct me_cfg_subdevice_entry {
+	me_cfg_subdevice_info_t info;
+	me_cfg_range_entry_t *range_list;
+	unsigned int count;
+	int locked;
+	me_cfg_external_device_t external_device;
+} me_cfg_subdevice_entry_t;
+
+
+typedef struct me_cfg_device_entry {
+	me_cfg_tcpip_t tcpip;
+	me_cfg_device_info_t info;
+	me_cfg_subdevice_entry_t *subdevice_list;
+	unsigned int count;
+} me_cfg_device_entry_t;
+
+
+typedef struct me_config_load {
+	me_cfg_device_entry_t *device_list;
+	unsigned int count;
+	int errno;
+} me_config_load_t;
+
+
+/*=============================================================================
+  The ioctls of the board
+  ===========================================================================*/
+
+#define MEMAIN_MAGIC 'y'
+
+#define ME_IO_IRQ_ENABLE				_IOR (MEMAIN_MAGIC, 1, me_io_irq_start_t)
+#define ME_IO_IRQ_WAIT					_IOR (MEMAIN_MAGIC, 2, me_io_irq_wait_t)
+#define ME_IO_IRQ_DISABLE				_IOR (MEMAIN_MAGIC, 3, me_io_irq_stop_t)
+
+#define ME_IO_RESET_DEVICE				_IOW (MEMAIN_MAGIC, 4, me_io_reset_device_t)
+#define ME_IO_RESET_SUBDEVICE			_IOW (MEMAIN_MAGIC, 5, me_io_reset_subdevice_t)
+
+#define ME_IO_SINGLE					_IOWR(MEMAIN_MAGIC, 6, me_io_single_t)
+#define ME_IO_SINGLE_CONFIG				_IOW (MEMAIN_MAGIC, 7, me_io_single_config_t)
+
+#define ME_IO_STREAM_CONFIG				_IOW (MEMAIN_MAGIC, 8, me_io_stream_config_t)
+#define ME_IO_STREAM_NEW_VALUES			_IOR (MEMAIN_MAGIC, 9, me_io_stream_new_values_t)
+#define ME_IO_STREAM_READ				_IOR (MEMAIN_MAGIC, 10, me_io_stream_read_t)
+#define ME_IO_STREAM_START				_IOW (MEMAIN_MAGIC, 11, me_io_stream_start_t)
+#define ME_IO_STREAM_STATUS				_IOR (MEMAIN_MAGIC, 12, me_io_stream_status_t)
+#define ME_IO_STREAM_STOP				_IOW (MEMAIN_MAGIC, 13, me_io_stream_stop_t)
+#define ME_IO_STREAM_WRITE				_IOW (MEMAIN_MAGIC, 14, me_io_stream_write_t)
+
+#define ME_LOCK_DRIVER					_IOW (MEMAIN_MAGIC, 15, me_lock_driver_t)
+#define ME_LOCK_DEVICE					_IOW (MEMAIN_MAGIC, 16, me_lock_device_t)
+#define ME_LOCK_SUBDEVICE				_IOW (MEMAIN_MAGIC, 17, me_lock_subdevice_t)
+
+#define ME_QUERY_DESCRIPTION_DEVICE		_IOR (MEMAIN_MAGIC, 18, me_query_description_device_t)
+
+#define ME_QUERY_INFO_DEVICE			_IOR (MEMAIN_MAGIC, 19, me_query_info_device_t)
+
+#define ME_QUERY_NAME_DEVICE			_IOR (MEMAIN_MAGIC, 20, me_query_name_device_t)
+#define ME_QUERY_NAME_DEVICE_DRIVER		_IOR (MEMAIN_MAGIC, 21, me_query_name_device_driver_t)
+
+#define ME_QUERY_NUMBER_DEVICES			_IOR (MEMAIN_MAGIC, 22, me_query_number_devices_t)
+#define ME_QUERY_NUMBER_SUBDEVICES		_IOR (MEMAIN_MAGIC, 23, me_query_number_subdevices_t)
+#define ME_QUERY_NUMBER_CHANNELS		_IOR (MEMAIN_MAGIC, 24, me_query_number_channels_t)
+#define ME_QUERY_NUMBER_RANGES			_IOR (MEMAIN_MAGIC, 25, me_query_number_ranges_t)
+
+#define ME_QUERY_RANGE_BY_MIN_MAX		_IOR (MEMAIN_MAGIC, 26, me_query_range_by_min_max_t)
+#define ME_QUERY_RANGE_INFO				_IOR (MEMAIN_MAGIC, 27, me_query_range_info_t)
+
+#define ME_QUERY_SUBDEVICE_BY_TYPE		_IOR (MEMAIN_MAGIC, 28, me_query_subdevice_by_type_t)
+#define ME_QUERY_SUBDEVICE_TYPE			_IOR (MEMAIN_MAGIC, 29, me_query_subdevice_type_t)
+#define ME_QUERY_SUBDEVICE_CAPS			_IOR (MEMAIN_MAGIC, 29, me_query_subdevice_caps_t)
+#define ME_QUERY_SUBDEVICE_CAPS_ARGS	_IOR (MEMAIN_MAGIC, 30, me_query_subdevice_caps_args_t)
+
+#define ME_QUERY_TIMER					_IOR (MEMAIN_MAGIC, 31, me_query_timer_t)
+
+#define ME_QUERY_VERSION_DEVICE_DRIVER	_IOR (MEMAIN_MAGIC, 32, me_query_version_device_driver_t)
+#define ME_QUERY_VERSION_MAIN_DRIVER	_IOR (MEMAIN_MAGIC, 33, me_query_version_main_driver_t)
+
+#define ME_CONFIG_LOAD					_IOWR(MEMAIN_MAGIC, 34, me_config_load_t)
+
+#endif
diff --git a/drivers/staging/meilhaus/memain.c b/drivers/staging/meilhaus/memain.c
new file mode 100644
index 00000000000..b09d1a6c766
--- /dev/null
+++ b/drivers/staging/meilhaus/memain.c
@@ -0,0 +1,2022 @@
+/**
+ * @file memain.c
+ *
+ * @brief Main Meilhaus device driver.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ * @author Krzysztof Gantzke	(k.gantzke@meilhaus.de)
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+#ifndef MODULE
+#  define MODULE
+#endif
+
+#include <linux/module.h>
+#include <linux/pci.h>
+//#include <linux/usb.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+#include <linux/cdev.h>
+#include <linux/rwsem.h>
+
+#include "medefines.h"
+#include "metypes.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "memain.h"
+#include "medevice.h"
+#include "meioctl.h"
+#include "mecommon.h"
+
+/* Module parameters
+*/
+
+#ifdef BOSCH
+static unsigned int me_bosch_fw = 0;
+
+# ifdef module_param
+module_param(me_bosch_fw, int, S_IRUGO);
+# else
+MODULE_PARM(me_bosch_fw, "i");
+# endif
+
+MODULE_PARM_DESC(me_bosch_fw,
+		 "Flags which signals the ME-4600 driver to load the bosch firmware (default = 0).");
+#endif //BOSCH
+
+static unsigned int major = 0;
+#ifdef module_param
+module_param(major, int, S_IRUGO);
+#else
+MODULE_PARM(major, "i");
+#endif
+
+/* Global Driver Lock
+*/
+
+static struct file *me_filep = NULL;
+static int me_count = 0;
+static spinlock_t me_lock = SPIN_LOCK_UNLOCKED;
+static DECLARE_RWSEM(me_rwsem);
+
+/* Board instances are kept in a global list */
+LIST_HEAD(me_device_list);
+
+/* Prototypes
+*/
+
+static int me_probe_pci(struct pci_dev *dev, const struct pci_device_id *id);
+static void me_remove_pci(struct pci_dev *dev);
+static int insert_to_device_list(me_device_t * n_device);
+static int replace_with_dummy(int vendor_id, int device_id, int serial_no);
+static void clear_device_list(void);
+static int me_open(struct inode *inode_ptr, struct file *filep);
+static int me_release(struct inode *, struct file *);
+static int me_ioctl(struct inode *, struct file *, unsigned int, unsigned long);
+//static int me_probe_usb(struct usb_interface *interface, const struct usb_device_id *id);
+//static void me_disconnect_usb(struct usb_interface *interface);
+
+/* Character device structure
+*/
+
+static struct cdev *cdevp;
+
+/* File operations provided by the module
+*/
+
+static struct file_operations me_file_operations = {
+	.owner = THIS_MODULE,
+	.ioctl = me_ioctl,
+	.open = me_open,
+	.release = me_release,
+};
+
+struct pci_driver me_pci_driver = {
+	.name = MEMAIN_NAME,
+	.id_table = me_pci_table,
+	.probe = me_probe_pci,
+	.remove = me_remove_pci
+};
+
+/* //me_usb_driver
+static struct usb_driver me_usb_driver =
+{
+	.name = MEMAIN_NAME,
+	.id_table = me_usb_table,
+	.probe = me_probe_usb,
+	.disconnect = me_disconnect_usb
+};
+*/
+
+#ifdef ME_LOCK_MULTIPLEX_TEMPLATE
+ME_LOCK_MULTIPLEX_TEMPLATE("me_lock_device",
+			   me_lock_device_t,
+			   me_lock_device,
+			   me_device_lock_device,
+			   (device, filep, karg.lock, karg.flags))
+
+    ME_LOCK_MULTIPLEX_TEMPLATE("me_lock_subdevice",
+			   me_lock_subdevice_t,
+			   me_lock_subdevice,
+			   me_device_lock_subdevice,
+			   (device, filep, karg.subdevice, karg.lock,
+			karg.flags))
+#else
+#error macro ME_LOCK_MULTIPLEX_TEMPLATE not defined
+#endif
+
+#ifdef ME_IO_MULTIPLEX_TEMPLATE
+ME_IO_MULTIPLEX_TEMPLATE("me_io_irq_start",
+			 me_io_irq_start_t,
+			 me_io_irq_start,
+			 me_device_io_irq_start,
+			 (device,
+			  filep,
+			  karg.subdevice,
+			  karg.channel,
+			  karg.irq_source,
+			  karg.irq_edge, karg.irq_arg, karg.flags))
+
+    ME_IO_MULTIPLEX_TEMPLATE("me_io_irq_wait",
+			 me_io_irq_wait_t,
+			 me_io_irq_wait,
+			 me_device_io_irq_wait,
+			 (device,
+		      filep,
+		      karg.subdevice,
+		      karg.channel,
+		      &karg.irq_count, &karg.value, karg.time_out, karg.flags))
+
+    ME_IO_MULTIPLEX_TEMPLATE("me_io_irq_stop",
+			 me_io_irq_stop_t,
+			 me_io_irq_stop,
+			 me_device_io_irq_stop,
+			 (device,
+		      filep, karg.subdevice, karg.channel, karg.flags))
+
+    ME_IO_MULTIPLEX_TEMPLATE("me_io_reset_device",
+			 me_io_reset_device_t,
+			 me_io_reset_device,
+			 me_device_io_reset_device, (device, filep, karg.flags))
+
+    ME_IO_MULTIPLEX_TEMPLATE("me_io_reset_subdevice",
+			 me_io_reset_subdevice_t,
+			 me_io_reset_subdevice,
+			 me_device_io_reset_subdevice,
+			 (device, filep, karg.subdevice, karg.flags))
+
+    ME_IO_MULTIPLEX_TEMPLATE("me_io_single_config",
+			 me_io_single_config_t,
+			 me_io_single_config,
+			 me_device_io_single_config,
+			 (device,
+		      filep,
+		      karg.subdevice,
+		      karg.channel,
+		      karg.single_config,
+		      karg.ref,
+		      karg.trig_chan,
+		      karg.trig_type, karg.trig_edge, karg.flags))
+
+    ME_IO_MULTIPLEX_TEMPLATE("me_io_stream_new_values",
+			 me_io_stream_new_values_t,
+			 me_io_stream_new_values,
+			 me_device_io_stream_new_values,
+			 (device,
+		      filep,
+		      karg.subdevice, karg.time_out, &karg.count, karg.flags))
+
+    ME_IO_MULTIPLEX_TEMPLATE("me_io_stream_read",
+			 me_io_stream_read_t,
+			 me_io_stream_read,
+			 me_device_io_stream_read,
+			 (device,
+		      filep,
+		      karg.subdevice,
+		      karg.read_mode, karg.values, &karg.count, karg.flags))
+
+    ME_IO_MULTIPLEX_TEMPLATE("me_io_stream_status",
+			 me_io_stream_status_t,
+			 me_io_stream_status,
+			 me_device_io_stream_status,
+			 (device,
+		      filep,
+		      karg.subdevice,
+		      karg.wait, &karg.status, &karg.count, karg.flags))
+
+    ME_IO_MULTIPLEX_TEMPLATE("me_io_stream_write",
+			 me_io_stream_write_t,
+			 me_io_stream_write,
+			 me_device_io_stream_write,
+			 (device,
+		      filep,
+		      karg.subdevice,
+		      karg.write_mode, karg.values, &karg.count, karg.flags))
+#else
+#error macro ME_IO_MULTIPLEX_TEMPLATE not defined
+#endif
+
+#ifdef ME_QUERY_MULTIPLEX_STR_TEMPLATE
+ME_QUERY_MULTIPLEX_STR_TEMPLATE("me_query_name_device",
+				me_query_name_device_t,
+				me_query_name_device,
+				me_device_query_name_device, (device, &msg))
+
+    ME_QUERY_MULTIPLEX_STR_TEMPLATE("me_query_name_device_driver",
+				me_query_name_device_driver_t,
+				me_query_name_device_driver,
+				me_device_query_name_device_driver,
+				(device, &msg))
+
+    ME_QUERY_MULTIPLEX_STR_TEMPLATE("me_query_description_device",
+				me_query_description_device_t,
+				me_query_description_device,
+				me_device_query_description_device,
+				(device, &msg))
+#else
+#error macro ME_QUERY_MULTIPLEX_STR_TEMPLATE not defined
+#endif
+
+#ifdef ME_QUERY_MULTIPLEX_TEMPLATE
+ME_QUERY_MULTIPLEX_TEMPLATE("me_query_info_device",
+			    me_query_info_device_t,
+			    me_query_info_device,
+			    me_device_query_info_device,
+			    (device,
+			     &karg.vendor_id,
+			     &karg.device_id,
+			     &karg.serial_no,
+			     &karg.bus_type,
+			     &karg.bus_no,
+			     &karg.dev_no, &karg.func_no, &karg.plugged))
+
+    ME_QUERY_MULTIPLEX_TEMPLATE("me_query_number_subdevices",
+			    me_query_number_subdevices_t,
+			    me_query_number_subdevices,
+			    me_device_query_number_subdevices,
+			    (device, &karg.number))
+
+    ME_QUERY_MULTIPLEX_TEMPLATE("me_query_number_channels",
+			    me_query_number_channels_t,
+			    me_query_number_channels,
+			    me_device_query_number_channels,
+			    (device, karg.subdevice, &karg.number))
+
+    ME_QUERY_MULTIPLEX_TEMPLATE("me_query_subdevice_by_type",
+			    me_query_subdevice_by_type_t,
+			    me_query_subdevice_by_type,
+			    me_device_query_subdevice_by_type,
+			    (device,
+			 karg.start_subdevice,
+			 karg.type, karg.subtype, &karg.subdevice))
+
+    ME_QUERY_MULTIPLEX_TEMPLATE("me_query_subdevice_type",
+			    me_query_subdevice_type_t,
+			    me_query_subdevice_type,
+			    me_device_query_subdevice_type,
+			    (device, karg.subdevice, &karg.type, &karg.subtype))
+
+    ME_QUERY_MULTIPLEX_TEMPLATE("me_query_subdevice_caps",
+			    me_query_subdevice_caps_t,
+			    me_query_subdevice_caps,
+			    me_device_query_subdevice_caps,
+			    (device, karg.subdevice, &karg.caps))
+
+    ME_QUERY_MULTIPLEX_TEMPLATE("me_query_subdevice_caps_args",
+			    me_query_subdevice_caps_args_t,
+			    me_query_subdevice_caps_args,
+			    me_device_query_subdevice_caps_args,
+			    (device, karg.subdevice, karg.cap, karg.args,
+			 karg.count))
+
+    ME_QUERY_MULTIPLEX_TEMPLATE("me_query_number_ranges",
+			    me_query_number_ranges_t,
+			    me_query_number_ranges,
+			    me_device_query_number_ranges,
+			    (device, karg.subdevice, karg.unit, &karg.number))
+
+    ME_QUERY_MULTIPLEX_TEMPLATE("me_query_range_by_min_max",
+			    me_query_range_by_min_max_t,
+			    me_query_range_by_min_max,
+			    me_device_query_range_by_min_max,
+			    (device,
+			 karg.subdevice,
+			 karg.unit,
+			 &karg.min, &karg.max, &karg.max_data, &karg.range))
+
+    ME_QUERY_MULTIPLEX_TEMPLATE("me_query_range_info",
+			    me_query_range_info_t,
+			    me_query_range_info,
+			    me_device_query_range_info,
+			    (device,
+			 karg.subdevice,
+			 karg.range,
+			 &karg.unit, &karg.min, &karg.max, &karg.max_data))
+
+    ME_QUERY_MULTIPLEX_TEMPLATE("me_query_timer",
+			    me_query_timer_t,
+			    me_query_timer,
+			    me_device_query_timer,
+			    (device,
+			 karg.subdevice,
+			 karg.timer,
+			 &karg.base_frequency,
+			 &karg.min_ticks, &karg.max_ticks))
+
+    ME_QUERY_MULTIPLEX_TEMPLATE("me_query_version_device_driver",
+			    me_query_version_device_driver_t,
+			    me_query_version_device_driver,
+			    me_device_query_version_device_driver,
+			    (device, &karg.version))
+#else
+#error macro ME_QUERY_MULTIPLEX_TEMPLATE not defined
+#endif
+
+/** ******************************************************************************** **/
+
+static me_device_t *get_dummy_instance(unsigned short vendor_id,
+				       unsigned short device_id,
+				       unsigned int serial_no,
+				       int bus_type,
+				       int bus_no, int dev_no, int func_no)
+{
+	int err;
+	me_dummy_constructor_t constructor = NULL;
+	me_device_t *instance;
+
+	PDEBUG("executed.\n");
+
+	if ((constructor = symbol_get(medummy_constructor)) == NULL) {
+		err = request_module(MEDUMMY_NAME);
+
+		if (err) {
+			PERROR("Error while request for module %s.\n",
+			       MEDUMMY_NAME);
+			return NULL;
+		}
+
+		if ((constructor = symbol_get(medummy_constructor)) == NULL) {
+			PERROR("Can't get %s driver module constructor.\n",
+			       MEDUMMY_NAME);
+			return NULL;
+		}
+	}
+
+	if ((instance = (*constructor) (vendor_id,
+					device_id,
+					serial_no,
+					bus_type,
+					bus_no, dev_no, func_no)) == NULL)
+		symbol_put(medummy_constructor);
+
+	return instance;
+}
+
+static int me_probe_pci(struct pci_dev *dev, const struct pci_device_id *id)
+{
+	int err;
+	me_pci_constructor_t constructor = NULL;
+#ifdef BOSCH
+	me_bosch_constructor_t constructor_bosch = NULL;
+#endif
+	me_device_t *n_device = NULL;
+	uint32_t device;
+
+	char constructor_name[24] = "me0000_pci_constructor";
+	char module_name[7] = "me0000";
+
+	PDEBUG("executed.\n");
+	device = dev->device;
+	if ((device & 0xF000) == 0x6000) {	// Exceptions: me61xx, me62xx, me63xx are handled by one driver.
+		device &= 0xF0FF;
+	}
+
+	constructor_name[2] += (char)((device >> 12) & 0x000F);
+	constructor_name[3] += (char)((device >> 8) & 0x000F);
+	PDEBUG("constructor_name: %s\n", constructor_name);
+	module_name[2] += (char)((device >> 12) & 0x000F);
+	module_name[3] += (char)((device >> 8) & 0x000F);
+	PDEBUG("module_name: %s\n", module_name);
+
+	if ((constructor =
+	     (me_pci_constructor_t) symbol_get(constructor_name)) == NULL) {
+		if (request_module(module_name)) {
+			PERROR("Error while request for module %s.\n",
+			       module_name);
+			return -ENODEV;
+		}
+
+		if ((constructor =
+		     (me_pci_constructor_t) symbol_get(constructor_name)) ==
+		    NULL) {
+			PERROR("Can't get %s driver module constructor.\n",
+			       module_name);
+			return -ENODEV;
+		}
+	}
+#ifdef BOSCH
+	if ((device & 0xF000) == 0x4000) {	// Bosch build has differnt constructor for me4600.
+		if ((n_device =
+		     (*constructor_bosch) (dev, me_bosch_fw)) == NULL) {
+			symbol_put(constructor_name);
+			PERROR
+			    ("Can't get device instance of %s driver module.\n",
+			     module_name);
+			return -ENODEV;
+		}
+	} else {
+#endif
+		if ((n_device = (*constructor) (dev)) == NULL) {
+			symbol_put(constructor_name);
+			PERROR
+			    ("Can't get device instance of %s driver module.\n",
+			     module_name);
+			return -ENODEV;
+		}
+#ifdef BOSCH
+	}
+#endif
+
+	insert_to_device_list(n_device);
+	err =
+	    n_device->me_device_io_reset_device(n_device, NULL,
+						ME_IO_RESET_DEVICE_NO_FLAGS);
+	if (err) {
+		PERROR("Error while reseting device.\n");
+	} else {
+		PDEBUG("Reseting device was sucessful.\n");
+	}
+	return ME_ERRNO_SUCCESS;
+}
+
+static void release_instance(me_device_t * device)
+{
+	int vendor_id;
+	int device_id;
+	int serial_no;
+	int bus_type;
+	int bus_no;
+	int dev_no;
+	int func_no;
+	int plugged;
+
+	uint32_t dev_id;
+
+	char constructor_name[24] = "me0000_pci_constructor";
+
+	PDEBUG("executed.\n");
+
+	device->me_device_query_info_device(device,
+					    &vendor_id,
+					    &device_id,
+					    &serial_no,
+					    &bus_type,
+					    &bus_no,
+					    &dev_no, &func_no, &plugged);
+
+	dev_id = device_id;
+	device->me_device_destructor(device);
+
+	if (plugged != ME_PLUGGED_IN) {
+		PDEBUG("release: medummy_constructor\n");
+
+		symbol_put("medummy_constructor");
+	} else {
+		if ((dev_id & 0xF000) == 0x6000) {	// Exceptions: me61xx, me62xx, me63xx are handled by one driver.
+			dev_id &= 0xF0FF;
+		}
+
+		constructor_name[2] += (char)((dev_id >> 12) & 0x000F);
+		constructor_name[3] += (char)((dev_id >> 8) & 0x000F);
+		PDEBUG("release: %s\n", constructor_name);
+
+		symbol_put(constructor_name);
+	}
+}
+
+static int insert_to_device_list(me_device_t * n_device)
+{
+	me_device_t *o_device = NULL;
+
+	struct list_head *pos;
+	int n_vendor_id;
+	int n_device_id;
+	int n_serial_no;
+	int n_bus_type;
+	int n_bus_no;
+	int n_dev_no;
+	int n_func_no;
+	int n_plugged;
+	int o_vendor_id;
+	int o_device_id;
+	int o_serial_no;
+	int o_bus_type;
+	int o_bus_no;
+	int o_dev_no;
+	int o_func_no;
+	int o_plugged;
+
+	PDEBUG("executed.\n");
+
+	n_device->me_device_query_info_device(n_device,
+					      &n_vendor_id,
+					      &n_device_id,
+					      &n_serial_no,
+					      &n_bus_type,
+					      &n_bus_no,
+					      &n_dev_no,
+					      &n_func_no, &n_plugged);
+
+	down_write(&me_rwsem);
+
+	list_for_each(pos, &me_device_list) {
+		o_device = list_entry(pos, me_device_t, list);
+		o_device->me_device_query_info_device(o_device,
+						      &o_vendor_id,
+						      &o_device_id,
+						      &o_serial_no,
+						      &o_bus_type,
+						      &o_bus_no,
+						      &o_dev_no,
+						      &o_func_no, &o_plugged);
+
+		if (o_plugged == ME_PLUGGED_OUT) {
+			if (((o_vendor_id == n_vendor_id) &&
+			     (o_device_id == n_device_id) &&
+			     (o_serial_no == n_serial_no) &&
+			     (o_bus_type == n_bus_type)) ||
+			    ((o_vendor_id == n_vendor_id) &&
+			     (o_device_id == n_device_id) &&
+			     (o_bus_type == n_bus_type) &&
+			     (o_bus_no == n_bus_no) &&
+			     (o_dev_no == n_dev_no) &&
+			     (o_func_no == n_func_no))) {
+				n_device->list.prev = pos->prev;
+				n_device->list.next = pos->next;
+				pos->prev->next = &n_device->list;
+				pos->next->prev = &n_device->list;
+				release_instance(o_device);
+				break;
+			}
+		}
+	}
+
+	if (pos == &me_device_list) {
+		list_add_tail(&n_device->list, &me_device_list);
+	}
+
+	up_write(&me_rwsem);
+
+	return 0;
+}
+
+static void me_remove_pci(struct pci_dev *dev)
+{
+	int vendor_id = dev->vendor;
+	int device_id = dev->device;
+	int subsystem_vendor = dev->subsystem_vendor;
+	int subsystem_device = dev->subsystem_device;
+	int serial_no = (subsystem_device << 16) | subsystem_vendor;
+
+	PDEBUG("executed.\n");
+
+	PINFO("Vendor id = 0x%08X\n", vendor_id);
+	PINFO("Device id = 0x%08X\n", device_id);
+	PINFO("Serial Number = 0x%08X\n", serial_no);
+
+	replace_with_dummy(vendor_id, device_id, serial_no);
+}
+
+static int replace_with_dummy(int vendor_id, int device_id, int serial_no)
+{
+
+	struct list_head *pos;
+	me_device_t *n_device = NULL;
+	me_device_t *o_device = NULL;
+	int o_vendor_id;
+	int o_device_id;
+	int o_serial_no;
+	int o_bus_type;
+	int o_bus_no;
+	int o_dev_no;
+	int o_func_no;
+	int o_plugged;
+
+	PDEBUG("executed.\n");
+
+	down_write(&me_rwsem);
+
+	list_for_each(pos, &me_device_list) {
+		o_device = list_entry(pos, me_device_t, list);
+		o_device->me_device_query_info_device(o_device,
+						      &o_vendor_id,
+						      &o_device_id,
+						      &o_serial_no,
+						      &o_bus_type,
+						      &o_bus_no,
+						      &o_dev_no,
+						      &o_func_no, &o_plugged);
+
+		if (o_plugged == ME_PLUGGED_IN) {
+			if (((o_vendor_id == vendor_id) &&
+			     (o_device_id == device_id) &&
+			     (o_serial_no == serial_no))) {
+				n_device = get_dummy_instance(o_vendor_id,
+							      o_device_id,
+							      o_serial_no,
+							      o_bus_type,
+							      o_bus_no,
+							      o_dev_no,
+							      o_func_no);
+
+				if (!n_device) {
+					up_write(&me_rwsem);
+					PERROR("Cannot get dummy instance.\n");
+					return 1;
+				}
+
+				n_device->list.prev = pos->prev;
+
+				n_device->list.next = pos->next;
+				pos->prev->next = &n_device->list;
+				pos->next->prev = &n_device->list;
+				release_instance(o_device);
+				break;
+			}
+		}
+	}
+
+	up_write(&me_rwsem);
+
+	return 0;
+}
+
+static void clear_device_list(void)
+{
+
+	struct list_head *entry;
+	me_device_t *device;
+
+	// Clear the device info list .
+	down_write(&me_rwsem);
+
+	while (!list_empty(&me_device_list)) {
+		entry = me_device_list.next;
+		device = list_entry(entry, me_device_t, list);
+		list_del(entry);
+		release_instance(device);
+	}
+
+	up_write(&me_rwsem);
+}
+
+static int lock_driver(struct file *filep, int lock, int flags)
+{
+	int err = ME_ERRNO_SUCCESS;
+	me_device_t *device;
+
+	PDEBUG("executed.\n");
+
+	down_read(&me_rwsem);
+
+	spin_lock(&me_lock);
+
+	switch (lock) {
+
+	case ME_LOCK_SET:
+		if (me_count) {
+			PERROR
+			    ("Driver System is currently used by another process.\n");
+			err = ME_ERRNO_USED;
+		} else if ((me_filep != NULL) && (me_filep != filep)) {
+			PERROR
+			    ("Driver System is already logged by another process.\n");
+			err = ME_ERRNO_LOCKED;
+		} else {
+			list_for_each_entry(device, &me_device_list, list) {
+				err =
+				    device->me_device_lock_device(device, filep,
+								  ME_LOCK_CHECK,
+								  flags);
+
+				if (err)
+					break;
+			}
+
+			if (!err)
+				me_filep = filep;
+		}
+
+		break;
+
+	case ME_LOCK_RELEASE:
+		if ((me_filep != NULL) && (me_filep != filep)) {
+			err = ME_ERRNO_SUCCESS;
+		} else {
+			list_for_each_entry(device, &me_device_list, list) {
+				device->me_device_lock_device(device, filep,
+							      ME_LOCK_RELEASE,
+							      flags);
+			}
+
+			me_filep = NULL;
+		}
+
+		break;
+
+	default:
+		PERROR("Invalid lock specified.\n");
+
+		err = ME_ERRNO_INVALID_LOCK;
+
+		break;
+	}
+
+	spin_unlock(&me_lock);
+
+	up_read(&me_rwsem);
+
+	return err;
+}
+
+static int me_lock_driver(struct file *filep, me_lock_driver_t * arg)
+{
+	int err = 0;
+
+	me_lock_driver_t lock;
+
+	PDEBUG("executed.\n");
+
+	err = copy_from_user(&lock, arg, sizeof(me_lock_driver_t));
+
+	if (err) {
+		PERROR("Can't copy arguments to kernel space.\n");
+		return -EFAULT;
+	}
+
+	lock.errno = lock_driver(filep, lock.lock, lock.flags);
+
+	err = copy_to_user(arg, &lock, sizeof(me_lock_driver_t));
+
+	if (err) {
+		PERROR("Can't copy query back to user space.\n");
+		return -EFAULT;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me_open(struct inode *inode_ptr, struct file *filep)
+{
+
+	PDEBUG("executed.\n");
+	// Nothing to do here.
+	return 0;
+}
+
+static int me_release(struct inode *inode_ptr, struct file *filep)
+{
+
+	PDEBUG("executed.\n");
+	lock_driver(filep, ME_LOCK_RELEASE, ME_LOCK_DRIVER_NO_FLAGS);
+
+	return 0;
+}
+
+static int me_query_version_main_driver(struct file *filep,
+					me_query_version_main_driver_t * arg)
+{
+	int err;
+	me_query_version_main_driver_t karg;
+
+	PDEBUG("executed.\n");
+
+	karg.version = ME_VERSION_DRIVER;
+	karg.errno = ME_ERRNO_SUCCESS;
+
+	err = copy_to_user(arg, &karg, sizeof(me_query_version_main_driver_t));
+
+	if (err) {
+		PERROR("Can't copy query back to user space.\n");
+		return -EFAULT;
+	}
+
+	return 0;
+}
+
+static int me_config_load_device(struct file *filep,
+				 me_cfg_device_entry_t * karg, int device_no)
+{
+
+	int err = ME_ERRNO_SUCCESS;
+	int k = 0;
+
+	struct list_head *pos = NULL;
+	me_device_t *device = NULL;
+
+	PDEBUG("executed.\n");
+
+	list_for_each(pos, &me_device_list) {
+		if (k == device_no) {
+			device = list_entry(pos, me_device_t, list);
+			break;
+		}
+
+		k++;
+	}
+
+	if (pos == &me_device_list) {
+		PERROR("Invalid device number specified.\n");
+		return ME_ERRNO_INVALID_DEVICE;
+	} else {
+		spin_lock(&me_lock);
+
+		if ((me_filep != NULL) && (me_filep != filep)) {
+			spin_unlock(&me_lock);
+			PERROR("Resource is locked by another process.\n");
+			return ME_ERRNO_LOCKED;
+		} else {
+			me_count++;
+			spin_unlock(&me_lock);
+
+			err =
+			    device->me_device_config_load(device, filep, karg);
+
+			spin_lock(&me_lock);
+			me_count--;
+			spin_unlock(&me_lock);
+		}
+	}
+
+	return err;
+}
+
+static int me_config_load(struct file *filep, me_config_load_t * arg)
+{
+	int err;
+	int i;
+	me_config_load_t cfg_setup;
+	me_config_load_t karg_cfg_setup;
+
+	struct list_head *pos = NULL;
+
+	struct list_head new_list;
+	me_device_t *o_device;
+	me_device_t *n_device;
+	int o_vendor_id;
+	int o_device_id;
+	int o_serial_no;
+	int o_bus_type;
+	int o_bus_no;
+	int o_dev_no;
+	int o_func_no;
+	int o_plugged;
+
+	PDEBUG("executed.\n");
+
+	// Copy argument to kernel space.
+	err = copy_from_user(&karg_cfg_setup, arg, sizeof(me_config_load_t));
+
+	if (err) {
+		PERROR("Can't copy arguments to kernel space.\n");
+		return -EFAULT;
+	}
+	// Allocate kernel buffer for device list.
+	cfg_setup.device_list =
+	    kmalloc(sizeof(me_cfg_device_entry_t) * karg_cfg_setup.count,
+		    GFP_KERNEL);
+
+	if (!cfg_setup.device_list) {
+		PERROR("Can't get buffer %li for device list.\n",
+		       sizeof(me_cfg_device_entry_t) * karg_cfg_setup.count);
+		return -ENOMEM;
+	}
+	// Copy device list to kernel space.
+	err =
+	    copy_from_user(cfg_setup.device_list, karg_cfg_setup.device_list,
+			   sizeof(me_cfg_device_entry_t) *
+			   karg_cfg_setup.count);
+
+	if (err) {
+		PERROR("Can't copy device list to kernel space.\n");
+		kfree(cfg_setup.device_list);
+		return -EFAULT;
+	}
+
+	cfg_setup.count = karg_cfg_setup.count;
+
+	INIT_LIST_HEAD(&new_list);
+
+	down_write(&me_rwsem);
+
+	spin_lock(&me_lock);
+
+	if ((me_filep != NULL) && (me_filep != filep)) {
+		spin_unlock(&me_lock);
+		PERROR("Driver System is logged by another process.\n");
+		karg_cfg_setup.errno = ME_ERRNO_LOCKED;
+	} else {
+		me_count++;
+		spin_unlock(&me_lock);
+
+		for (i = 0; i < karg_cfg_setup.count; i++) {
+			PDEBUG("me_config_load() device=%d.\n", i);
+			if (cfg_setup.device_list[i].tcpip.access_type ==
+			    ME_ACCESS_TYPE_LOCAL) {
+				list_for_each(pos, &me_device_list) {
+					o_device =
+					    list_entry(pos, me_device_t, list);
+					o_device->
+					    me_device_query_info_device
+					    (o_device, &o_vendor_id,
+					     &o_device_id, &o_serial_no,
+					     &o_bus_type, &o_bus_no, &o_dev_no,
+					     &o_func_no, &o_plugged);
+
+					if (cfg_setup.device_list[i].info.
+					    hw_location.bus_type ==
+					    ME_BUS_TYPE_PCI) {
+						if (((o_vendor_id ==
+						      cfg_setup.device_list[i].
+						      info.vendor_id)
+						     && (o_device_id ==
+							 cfg_setup.
+							 device_list[i].info.
+							 device_id)
+						     && (o_serial_no ==
+							 cfg_setup.
+							 device_list[i].info.
+							 serial_no)
+						     && (o_bus_type ==
+							 cfg_setup.
+							 device_list[i].info.
+							 hw_location.bus_type))
+						    ||
+						    ((o_vendor_id ==
+						      cfg_setup.device_list[i].
+						      info.vendor_id)
+						     && (o_device_id ==
+							 cfg_setup.
+							 device_list[i].info.
+							 device_id)
+						     && (o_bus_type ==
+							 cfg_setup.
+							 device_list[i].info.
+							 hw_location.bus_type)
+						     && (o_bus_no ==
+							 cfg_setup.
+							 device_list[i].info.
+							 hw_location.pci.bus_no)
+						     && (o_dev_no ==
+							 cfg_setup.
+							 device_list[i].info.
+							 hw_location.pci.
+							 device_no)
+						     && (o_func_no ==
+							 cfg_setup.
+							 device_list[i].info.
+							 hw_location.pci.
+							 function_no))) {
+							list_move_tail(pos,
+								       &new_list);
+							break;
+						}
+					}
+/*
+					else if (cfg_setup.device_list[i].info.hw_location.bus_type == ME_BUS_TYPE_USB)
+					{
+						if (((o_vendor_id == cfg_setup.device_list[i].info.vendor_id) &&
+						        (o_device_id == cfg_setup.device_list[i].info.device_id) &&
+						        (o_serial_no == cfg_setup.device_list[i].info.serial_no) &&
+						        (o_bus_type == cfg_setup.device_list[i].info.hw_location.bus_type)) ||
+						        ((o_vendor_id == cfg_setup.device_list[i].info.vendor_id) &&
+						         (o_device_id == cfg_setup.device_list[i].info.device_id) &&
+						         (o_bus_type == cfg_setup.device_list[i].info.hw_location.bus_type) &&
+						         (o_bus_no == cfg_setup.device_list[i].info.hw_location.usb.root_hub_no)))
+						{
+							list_move_tail(pos, &new_list);
+							break;
+						}
+					}
+*/
+					else {
+						PERROR("Wrong bus type: %d.\n",
+						       cfg_setup.device_list[i].
+						       info.hw_location.
+						       bus_type);
+					}
+				}
+
+				if (pos == &me_device_list) {	// Device is not already in the list
+					if (cfg_setup.device_list[i].info.
+					    hw_location.bus_type ==
+					    ME_BUS_TYPE_PCI) {
+						n_device =
+						    get_dummy_instance
+						    (cfg_setup.device_list[i].
+						     info.vendor_id,
+						     cfg_setup.device_list[i].
+						     info.device_id,
+						     cfg_setup.device_list[i].
+						     info.serial_no,
+						     cfg_setup.device_list[i].
+						     info.hw_location.bus_type,
+						     cfg_setup.device_list[i].
+						     info.hw_location.pci.
+						     bus_no,
+						     cfg_setup.device_list[i].
+						     info.hw_location.pci.
+						     device_no,
+						     cfg_setup.device_list[i].
+						     info.hw_location.pci.
+						     function_no);
+
+						if (!n_device) {
+							PERROR
+							    ("Can't get dummy instance.\n");
+							kfree(cfg_setup.
+							      device_list);
+							spin_lock(&me_lock);
+							me_count--;
+							spin_unlock(&me_lock);
+							up_write(&me_rwsem);
+							return -EFAULT;
+						}
+
+						list_add_tail(&n_device->list,
+							      &new_list);
+					}
+/*
+					else if (cfg_setup.device_list[i].info.hw_location.bus_type == ME_BUS_TYPE_USB)
+					{
+						n_device = get_dummy_instance(
+						               cfg_setup.device_list[i].info.vendor_id,
+						               cfg_setup.device_list[i].info.device_id,
+						               cfg_setup.device_list[i].info.serial_no,
+						               cfg_setup.device_list[i].info.hw_location.bus_type,
+						               cfg_setup.device_list[i].info.hw_location.usb.root_hub_no,
+						               0,
+						               0);
+
+						if (!n_device)
+						{
+							PERROR("Can't get dummy instance.\n");
+							kfree(cfg_setup.device_list);
+							spin_lock(&me_lock);
+							me_count--;
+							spin_unlock(&me_lock);
+							up_write(&me_rwsem);
+							return -EFAULT;
+						}
+
+						list_add_tail(&n_device->list, &new_list);
+					}
+*/
+				}
+			} else {
+				n_device = get_dummy_instance(0,
+							      0, 0, 0, 0, 0, 0);
+
+				if (!n_device) {
+					PERROR("Can't get dummy instance.\n");
+					kfree(cfg_setup.device_list);
+					spin_lock(&me_lock);
+					me_count--;
+					spin_unlock(&me_lock);
+					up_write(&me_rwsem);
+					return -EFAULT;
+				}
+
+				list_add_tail(&n_device->list, &new_list);
+			}
+		}
+
+		while (!list_empty(&me_device_list)) {
+			o_device =
+			    list_entry(me_device_list.next, me_device_t, list);
+			o_device->me_device_query_info_device(o_device,
+							      &o_vendor_id,
+							      &o_device_id,
+							      &o_serial_no,
+							      &o_bus_type,
+							      &o_bus_no,
+							      &o_dev_no,
+							      &o_func_no,
+							      &o_plugged);
+
+			if (o_plugged == ME_PLUGGED_IN) {
+				list_move_tail(me_device_list.next, &new_list);
+			} else {
+				list_del(me_device_list.next);
+				release_instance(o_device);
+			}
+		}
+
+		// Move temporary new list to global driver list.
+		list_splice(&new_list, &me_device_list);
+
+		karg_cfg_setup.errno = ME_ERRNO_SUCCESS;
+	}
+
+	for (i = 0; i < cfg_setup.count; i++) {
+
+		karg_cfg_setup.errno =
+		    me_config_load_device(filep, &cfg_setup.device_list[i], i);
+		if (karg_cfg_setup.errno) {
+			PERROR("me_config_load_device(%d)=%d\n", i,
+			       karg_cfg_setup.errno);
+			break;
+		}
+	}
+
+	spin_lock(&me_lock);
+
+	me_count--;
+	spin_unlock(&me_lock);
+	up_write(&me_rwsem);
+
+	err = copy_to_user(arg, &karg_cfg_setup, sizeof(me_config_load_t));
+
+	if (err) {
+		PERROR("Can't copy config list to user space.\n");
+		kfree(cfg_setup.device_list);
+		return -EFAULT;
+	}
+
+	kfree(cfg_setup.device_list);
+	return 0;
+}
+
+static int me_io_stream_start(struct file *filep, me_io_stream_start_t * arg)
+{
+	int err;
+	int i, k;
+
+	struct list_head *pos;
+	me_device_t *device;
+	me_io_stream_start_t karg;
+	meIOStreamStart_t *list;
+
+	PDEBUG("executed.\n");
+
+	err = copy_from_user(&karg, arg, sizeof(me_io_stream_start_t));
+
+	if (err) {
+		PERROR("Can't copy arguments to kernel space.\n");
+		return -EFAULT;
+	}
+
+	karg.errno = ME_ERRNO_SUCCESS;
+
+	list = kmalloc(sizeof(meIOStreamStart_t) * karg.count, GFP_KERNEL);
+
+	if (!list) {
+		PERROR("Can't get buffer for start list.\n");
+		return -ENOMEM;
+	}
+
+	err =
+	    copy_from_user(list, karg.start_list,
+			   sizeof(meIOStreamStart_t) * karg.count);
+
+	if (err) {
+		PERROR("Can't copy start list to kernel space.\n");
+		kfree(list);
+		return -EFAULT;
+	}
+
+	spin_lock(&me_lock);
+
+	if ((me_filep != NULL) && (me_filep != filep)) {
+		spin_unlock(&me_lock);
+		PERROR("Driver System is logged by another process.\n");
+
+		for (i = 0; i < karg.count; i++) {
+			list[i].iErrno = ME_ERRNO_LOCKED;
+		}
+	} else {
+		me_count++;
+		spin_unlock(&me_lock);
+
+		for (i = 0; i < karg.count; i++) {
+			down_read(&me_rwsem);
+			k = 0;
+			list_for_each(pos, &me_device_list) {
+				if (k == list[i].iDevice) {
+					device =
+					    list_entry(pos, me_device_t, list);
+					break;
+				}
+
+				k++;
+			}
+
+			if (pos == &me_device_list) {
+				up_read(&me_rwsem);
+				PERROR("Invalid device number specified.\n");
+				list[i].iErrno = ME_ERRNO_INVALID_DEVICE;
+				karg.errno = ME_ERRNO_INVALID_DEVICE;
+				break;
+			} else {
+				list[i].iErrno =
+				    device->me_device_io_stream_start(device,
+								      filep,
+								      list[i].
+								      iSubdevice,
+								      list[i].
+								      iStartMode,
+								      list[i].
+								      iTimeOut,
+								      list[i].
+								      iFlags);
+
+				if (list[i].iErrno) {
+					up_read(&me_rwsem);
+					karg.errno = list[i].iErrno;
+					break;
+				}
+			}
+
+			up_read(&me_rwsem);
+		}
+
+		spin_lock(&me_lock);
+
+		me_count--;
+		spin_unlock(&me_lock);
+	}
+
+	err = copy_to_user(arg, &karg, sizeof(me_io_stream_start_t));
+
+	if (err) {
+		PERROR("Can't copy arguments to user space.\n");
+		kfree(list);
+		return -EFAULT;
+	}
+
+	err =
+	    copy_to_user(karg.start_list, list,
+			 sizeof(meIOStreamStart_t) * karg.count);
+
+	if (err) {
+		PERROR("Can't copy start list to user space.\n");
+		kfree(list);
+		return -EFAULT;
+	}
+
+	kfree(list);
+
+	return err;
+}
+
+static int me_io_single(struct file *filep, me_io_single_t * arg)
+{
+	int err;
+	int i, k;
+
+	struct list_head *pos;
+	me_device_t *device;
+	me_io_single_t karg;
+	meIOSingle_t *list;
+
+	PDEBUG("executed.\n");
+
+	err = copy_from_user(&karg, arg, sizeof(me_io_single_t));
+
+	if (err) {
+		PERROR("Can't copy arguments to kernel space.\n");
+		return -EFAULT;
+	}
+
+	karg.errno = ME_ERRNO_SUCCESS;
+
+	list = kmalloc(sizeof(meIOSingle_t) * karg.count, GFP_KERNEL);
+
+	if (!list) {
+		PERROR("Can't get buffer for single list.\n");
+		return -ENOMEM;
+	}
+
+	err =
+	    copy_from_user(list, karg.single_list,
+			   sizeof(meIOSingle_t) * karg.count);
+
+	if (err) {
+		PERROR("Can't copy single list to kernel space.\n");
+		kfree(list);
+		return -EFAULT;
+	}
+
+	spin_lock(&me_lock);
+
+	if ((me_filep != NULL) && (me_filep != filep)) {
+		spin_unlock(&me_lock);
+		PERROR("Driver System is logged by another process.\n");
+
+		for (i = 0; i < karg.count; i++) {
+			list[i].iErrno = ME_ERRNO_LOCKED;
+		}
+	} else {
+		me_count++;
+		spin_unlock(&me_lock);
+
+		for (i = 0; i < karg.count; i++) {
+			k = 0;
+
+			down_read(&me_rwsem);
+
+			list_for_each(pos, &me_device_list) {
+				if (k == list[i].iDevice) {
+					device =
+					    list_entry(pos, me_device_t, list);
+					break;
+				}
+
+				k++;
+			}
+
+			if (pos == &me_device_list) {
+				up_read(&me_rwsem);
+				PERROR("Invalid device number specified.\n");
+				list[i].iErrno = ME_ERRNO_INVALID_DEVICE;
+				karg.errno = ME_ERRNO_INVALID_DEVICE;
+				break;
+			} else {
+				if (list[i].iDir == ME_DIR_OUTPUT) {
+					list[i].iErrno =
+					    device->
+					    me_device_io_single_write(device,
+								      filep,
+								      list[i].
+								      iSubdevice,
+								      list[i].
+								      iChannel,
+								      list[i].
+								      iValue,
+								      list[i].
+								      iTimeOut,
+								      list[i].
+								      iFlags);
+
+					if (list[i].iErrno) {
+						up_read(&me_rwsem);
+						karg.errno = list[i].iErrno;
+						break;
+					}
+				} else if (list[i].iDir == ME_DIR_INPUT) {
+					list[i].iErrno =
+					    device->
+					    me_device_io_single_read(device,
+								     filep,
+								     list[i].
+								     iSubdevice,
+								     list[i].
+								     iChannel,
+								     &list[i].
+								     iValue,
+								     list[i].
+								     iTimeOut,
+								     list[i].
+								     iFlags);
+
+					if (list[i].iErrno) {
+						up_read(&me_rwsem);
+						karg.errno = list[i].iErrno;
+						break;
+					}
+				} else {
+					up_read(&me_rwsem);
+					PERROR
+					    ("Invalid single direction specified.\n");
+					list[i].iErrno = ME_ERRNO_INVALID_DIR;
+					karg.errno = ME_ERRNO_INVALID_DIR;
+					break;
+				}
+			}
+
+			up_read(&me_rwsem);
+		}
+
+		spin_lock(&me_lock);
+
+		me_count--;
+		spin_unlock(&me_lock);
+	}
+
+	err = copy_to_user(arg, &karg, sizeof(me_io_single_t));
+
+	if (err) {
+		PERROR("Can't copy arguments to user space.\n");
+		return -EFAULT;
+	}
+
+	err =
+	    copy_to_user(karg.single_list, list,
+			 sizeof(meIOSingle_t) * karg.count);
+
+	if (err) {
+		PERROR("Can't copy single list to user space.\n");
+		kfree(list);
+		return -EFAULT;
+	}
+
+	kfree(list);
+
+	return err;
+}
+
+static int me_io_stream_config(struct file *filep, me_io_stream_config_t * arg)
+{
+	int err;
+	int k = 0;
+
+	struct list_head *pos;
+	me_device_t *device;
+	me_io_stream_config_t karg;
+	meIOStreamConfig_t *list;
+
+	PDEBUG("executed.\n");
+
+	err = copy_from_user(&karg, arg, sizeof(me_io_stream_config_t));
+
+	if (err) {
+		PERROR("Can't copy arguments to kernel space.\n");
+		return -EFAULT;
+	}
+
+	list = kmalloc(sizeof(meIOStreamConfig_t) * karg.count, GFP_KERNEL);
+
+	if (!list) {
+		PERROR("Can't get buffer for config list.\n");
+		return -ENOMEM;
+	}
+
+	err =
+	    copy_from_user(list, karg.config_list,
+			   sizeof(meIOStreamConfig_t) * karg.count);
+
+	if (err) {
+		PERROR("Can't copy config list to kernel space.\n");
+		kfree(list);
+		return -EFAULT;
+	}
+
+	spin_lock(&me_lock);
+
+	if ((me_filep != NULL) && (me_filep != filep)) {
+		spin_unlock(&me_lock);
+		PERROR("Driver System is logged by another process.\n");
+		karg.errno = ME_ERRNO_LOCKED;
+	} else {
+		me_count++;
+		spin_unlock(&me_lock);
+
+		down_read(&me_rwsem);
+
+		list_for_each(pos, &me_device_list) {
+			if (k == karg.device) {
+				device = list_entry(pos, me_device_t, list);
+				break;
+			}
+
+			k++;
+		}
+
+		if (pos == &me_device_list) {
+			PERROR("Invalid device number specified.\n");
+			karg.errno = ME_ERRNO_INVALID_DEVICE;
+		} else {
+			karg.errno =
+			    device->me_device_io_stream_config(device, filep,
+							       karg.subdevice,
+							       list, karg.count,
+							       &karg.trigger,
+							       karg.
+							       fifo_irq_threshold,
+							       karg.flags);
+		}
+
+		up_read(&me_rwsem);
+
+		spin_lock(&me_lock);
+		me_count--;
+		spin_unlock(&me_lock);
+	}
+
+	err = copy_to_user(arg, &karg, sizeof(me_io_stream_config_t));
+
+	if (err) {
+		PERROR("Can't copy back to user space.\n");
+		kfree(list);
+		return -EFAULT;
+	}
+
+	kfree(list);
+
+	return err;
+}
+
+static int me_query_number_devices(struct file *filep,
+				   me_query_number_devices_t * arg)
+{
+	int err;
+	me_query_number_devices_t karg;
+
+	struct list_head *pos;
+
+	PDEBUG("executed.\n");
+
+	karg.number = 0;
+	down_read(&me_rwsem);
+	list_for_each(pos, &me_device_list) {
+		karg.number++;
+	}
+
+	up_read(&me_rwsem);
+
+	karg.errno = ME_ERRNO_SUCCESS;
+
+	err = copy_to_user(arg, &karg, sizeof(me_query_number_devices_t));
+
+	if (err) {
+		PERROR("Can't copy query back to user space.\n");
+		return -EFAULT;
+	}
+
+	return 0;
+}
+
+static int me_io_stream_stop(struct file *filep, me_io_stream_stop_t * arg)
+{
+	int err;
+	int i, k;
+
+	struct list_head *pos;
+	me_device_t *device;
+	me_io_stream_stop_t karg;
+	meIOStreamStop_t *list;
+
+	PDEBUG("executed.\n");
+
+	err = copy_from_user(&karg, arg, sizeof(me_io_stream_stop_t));
+
+	if (err) {
+		PERROR("Can't copy arguments to kernel space.\n");
+		return -EFAULT;
+	}
+
+	karg.errno = ME_ERRNO_SUCCESS;
+
+	list = kmalloc(sizeof(meIOStreamStop_t) * karg.count, GFP_KERNEL);
+
+	if (!list) {
+		PERROR("Can't get buffer for stop list.\n");
+		return -ENOMEM;
+	}
+
+	err =
+	    copy_from_user(list, karg.stop_list,
+			   sizeof(meIOStreamStop_t) * karg.count);
+
+	if (err) {
+		PERROR("Can't copy stop list to kernel space.\n");
+		kfree(list);
+		return -EFAULT;
+	}
+
+	spin_lock(&me_lock);
+
+	if ((me_filep != NULL) && (me_filep != filep)) {
+		spin_unlock(&me_lock);
+		PERROR("Driver System is logged by another process.\n");
+
+		for (i = 0; i < karg.count; i++) {
+			list[i].iErrno = ME_ERRNO_LOCKED;
+		}
+	} else {
+		me_count++;
+		spin_unlock(&me_lock);
+
+		for (i = 0; i < karg.count; i++) {
+			k = 0;
+			down_read(&me_rwsem);
+			list_for_each(pos, &me_device_list) {
+				if (k == list[i].iDevice) {
+					device =
+					    list_entry(pos, me_device_t, list);
+					break;
+				}
+
+				k++;
+			}
+
+			if (pos == &me_device_list) {
+				up_read(&me_rwsem);
+				PERROR("Invalid device number specified.\n");
+				list[i].iErrno = ME_ERRNO_INVALID_DEVICE;
+				karg.errno = ME_ERRNO_INVALID_DEVICE;
+				break;
+			} else {
+				list[i].iErrno =
+				    device->me_device_io_stream_stop(device,
+								     filep,
+								     list[i].
+								     iSubdevice,
+								     list[i].
+								     iStopMode,
+								     list[i].
+								     iFlags);
+
+				if (list[i].iErrno) {
+					up_read(&me_rwsem);
+					karg.errno = list[i].iErrno;
+					break;
+				}
+			}
+
+			up_read(&me_rwsem);
+		}
+
+		spin_lock(&me_lock);
+
+		me_count--;
+		spin_unlock(&me_lock);
+	}
+
+	err = copy_to_user(arg, &karg, sizeof(me_io_stream_stop_t));
+
+	if (err) {
+		PERROR("Can't copy arguments to user space.\n");
+		return -EFAULT;
+	}
+
+	err =
+	    copy_to_user(karg.stop_list, list,
+			 sizeof(meIOStreamStop_t) * karg.count);
+
+	if (err) {
+		PERROR("Can't copy stop list to user space.\n");
+		kfree(list);
+		return -EFAULT;
+	}
+
+	kfree(list);
+
+	return err;
+}
+
+/*  //me_probe_usb
+static int me_probe_usb(struct usb_interface *interface, const struct usb_device_id *id)
+{
+	//int err;
+	//me_usb_constructor_t *constructor = NULL;
+	me_device_t *n_device = NULL;
+
+	PDEBUG("executed.\n");
+
+	switch (id->idProduct)
+	{
+			case USB_DEVICE_ID_MEPHISTO_S1:
+				if((constructor = symbol_get(mephisto_s1_constructor)) == NULL){
+					err = request_module(MEPHISTO_S1_NAME);
+					if(err){
+						PERROR("Error while request for module %s.\n", MEPHISTO_S1_NAME);
+						return -ENODEV;
+					}
+					if((constructor = symbol_get(mephisto_s1_constructor)) == NULL){
+						PERROR("Can't get %s driver module constructor.\n", MEPHISTO_S1_NAME);
+						return -ENODEV;
+					}
+				}
+
+				if((n_device = (*constructor)(interface)) == NULL){
+					symbol_put(mephisto_s1_constructor);
+					PERROR("Can't get device instance of %s driver module.\n", MEPHISTO_S1_NAME);
+					return -ENODEV;
+				}
+
+				break;
+
+		default:
+			PERROR("Invalid product id.\n");
+
+			return -EINVAL;
+	}
+
+	return insert_to_device_list(n_device);
+}
+*/
+
+/*  //me_disconnect_usb
+static void me_disconnect_usb(struct usb_interface *interface)
+{
+
+	struct usb_device *device = interface_to_usbdev(interface);
+	int vendor_id = device->descriptor.idVendor;
+	int device_id = device->descriptor.idProduct;
+	int serial_no;
+
+	sscanf(&device->serial[2], "%x", &serial_no);
+
+	PDEBUG("executed.\n");
+
+	PINFO("Vendor id = 0x%08X\n", vendor_id);
+	PINFO("Device id = 0x%08X\n", device_id);
+	PINFO("Serial Number = 0x%08X\n", serial_no);
+
+	replace_with_dummy(vendor_id, device_id, serial_no);
+}
+*/
+
+static int me_ioctl(struct inode *inodep,
+		    struct file *filep, unsigned int service, unsigned long arg)
+{
+
+	PDEBUG("executed.\n");
+
+	if (_IOC_TYPE(service) != MEMAIN_MAGIC) {
+		PERROR("Invalid magic number.\n");
+		return -ENOTTY;
+	}
+
+	PDEBUG("service number: 0x%x.\n", service);
+
+	switch (service) {
+	case ME_IO_IRQ_ENABLE:
+		return me_io_irq_start(filep, (me_io_irq_start_t *) arg);
+
+	case ME_IO_IRQ_WAIT:
+		return me_io_irq_wait(filep, (me_io_irq_wait_t *) arg);
+
+	case ME_IO_IRQ_DISABLE:
+		return me_io_irq_stop(filep, (me_io_irq_stop_t *) arg);
+
+	case ME_IO_RESET_DEVICE:
+		return me_io_reset_device(filep, (me_io_reset_device_t *) arg);
+
+	case ME_IO_RESET_SUBDEVICE:
+		return me_io_reset_subdevice(filep,
+					     (me_io_reset_subdevice_t *) arg);
+
+	case ME_IO_SINGLE_CONFIG:
+		return me_io_single_config(filep,
+					   (me_io_single_config_t *) arg);
+
+	case ME_IO_SINGLE:
+		return me_io_single(filep, (me_io_single_t *) arg);
+
+	case ME_IO_STREAM_CONFIG:
+		return me_io_stream_config(filep,
+					   (me_io_stream_config_t *) arg);
+
+	case ME_IO_STREAM_NEW_VALUES:
+		return me_io_stream_new_values(filep,
+					       (me_io_stream_new_values_t *)
+					       arg);
+
+	case ME_IO_STREAM_READ:
+		return me_io_stream_read(filep, (me_io_stream_read_t *) arg);
+
+	case ME_IO_STREAM_START:
+		return me_io_stream_start(filep, (me_io_stream_start_t *) arg);
+
+	case ME_IO_STREAM_STATUS:
+		return me_io_stream_status(filep,
+					   (me_io_stream_status_t *) arg);
+
+	case ME_IO_STREAM_STOP:
+		return me_io_stream_stop(filep, (me_io_stream_stop_t *) arg);
+
+	case ME_IO_STREAM_WRITE:
+		return me_io_stream_write(filep, (me_io_stream_write_t *) arg);
+
+	case ME_LOCK_DRIVER:
+		return me_lock_driver(filep, (me_lock_driver_t *) arg);
+
+	case ME_LOCK_DEVICE:
+		return me_lock_device(filep, (me_lock_device_t *) arg);
+
+	case ME_LOCK_SUBDEVICE:
+		return me_lock_subdevice(filep, (me_lock_subdevice_t *) arg);
+
+	case ME_QUERY_INFO_DEVICE:
+		return me_query_info_device(filep,
+					    (me_query_info_device_t *) arg);
+
+	case ME_QUERY_DESCRIPTION_DEVICE:
+		return me_query_description_device(filep,
+						   (me_query_description_device_t
+						    *) arg);
+
+	case ME_QUERY_NAME_DEVICE:
+		return me_query_name_device(filep,
+					    (me_query_name_device_t *) arg);
+
+	case ME_QUERY_NAME_DEVICE_DRIVER:
+		return me_query_name_device_driver(filep,
+						   (me_query_name_device_driver_t
+						    *) arg);
+
+	case ME_QUERY_NUMBER_DEVICES:
+		return me_query_number_devices(filep,
+					       (me_query_number_devices_t *)
+					       arg);
+
+	case ME_QUERY_NUMBER_SUBDEVICES:
+		return me_query_number_subdevices(filep,
+						  (me_query_number_subdevices_t
+						   *) arg);
+
+	case ME_QUERY_NUMBER_CHANNELS:
+		return me_query_number_channels(filep,
+						(me_query_number_channels_t *)
+						arg);
+
+	case ME_QUERY_NUMBER_RANGES:
+		return me_query_number_ranges(filep,
+					      (me_query_number_ranges_t *) arg);
+
+	case ME_QUERY_RANGE_BY_MIN_MAX:
+		return me_query_range_by_min_max(filep,
+						 (me_query_range_by_min_max_t *)
+						 arg);
+
+	case ME_QUERY_RANGE_INFO:
+		return me_query_range_info(filep,
+					   (me_query_range_info_t *) arg);
+
+	case ME_QUERY_SUBDEVICE_BY_TYPE:
+		return me_query_subdevice_by_type(filep,
+						  (me_query_subdevice_by_type_t
+						   *) arg);
+
+	case ME_QUERY_SUBDEVICE_TYPE:
+		return me_query_subdevice_type(filep,
+					       (me_query_subdevice_type_t *)
+					       arg);
+
+	case ME_QUERY_SUBDEVICE_CAPS:
+		return me_query_subdevice_caps(filep,
+					       (me_query_subdevice_caps_t *)
+					       arg);
+
+	case ME_QUERY_SUBDEVICE_CAPS_ARGS:
+		return me_query_subdevice_caps_args(filep,
+						    (me_query_subdevice_caps_args_t
+						     *) arg);
+
+	case ME_QUERY_TIMER:
+		return me_query_timer(filep, (me_query_timer_t *) arg);
+
+	case ME_QUERY_VERSION_MAIN_DRIVER:
+		return me_query_version_main_driver(filep,
+						    (me_query_version_main_driver_t
+						     *) arg);
+
+	case ME_QUERY_VERSION_DEVICE_DRIVER:
+		return me_query_version_device_driver(filep,
+						      (me_query_version_device_driver_t
+						       *) arg);
+
+	case ME_CONFIG_LOAD:
+		return me_config_load(filep, (me_config_load_t *) arg);
+	}
+
+	PERROR("Invalid ioctl number.\n");
+	return -ENOTTY;
+}
+
+// Init and exit of module.
+static int memain_init(void)
+{
+	int result = 0;
+	dev_t dev = MKDEV(major, 0);
+
+	PDEBUG("executed.\n");
+
+	// Register pci driver. This will return 0 if the PCI subsystem is not available.
+	result = pci_register_driver(&me_pci_driver);
+
+	if (result < 0) {
+		PERROR("Can't register pci driver.\n");
+		goto INIT_ERROR_1;
+	}
+
+/*
+	// Register usb driver. This will return -ENODEV if no USB subsystem is available.
+	result = usb_register(&me_usb_driver);
+
+	if (result)
+	{
+		if (result == -ENODEV)
+		{
+			PERROR("No USB subsystem available.\n");
+		}
+		else
+		{
+			PERROR("Can't register usb driver.\n");
+			goto INIT_ERROR_2;
+		}
+	}
+*/
+	// Register the character device.
+	if (major) {
+		result = register_chrdev_region(dev, 1, MEMAIN_NAME);
+	} else {
+		result = alloc_chrdev_region(&dev, 0, 1, MEMAIN_NAME);
+		major = MAJOR(dev);
+	}
+
+	if (result < 0) {
+		PERROR("Can't get major driver no.\n");
+		goto INIT_ERROR_3;
+	}
+
+	cdevp = cdev_alloc();
+
+	if (!cdevp) {
+		PERROR("Can't get character device structure.\n");
+		result = -ENOMEM;
+		goto INIT_ERROR_4;
+	}
+
+	cdevp->ops = &me_file_operations;
+
+	cdevp->owner = THIS_MODULE;
+
+	result = cdev_add(cdevp, dev, 1);
+
+	if (result < 0) {
+		PERROR("Cannot add character device structure.\n");
+		goto INIT_ERROR_5;
+	}
+
+	return 0;
+
+      INIT_ERROR_5:
+	cdev_del(cdevp);
+
+      INIT_ERROR_4:
+	unregister_chrdev_region(dev, 1);
+
+      INIT_ERROR_3:
+//      usb_deregister(&me_usb_driver);
+
+//INIT_ERROR_2:
+	pci_unregister_driver(&me_pci_driver);
+	clear_device_list();
+
+      INIT_ERROR_1:
+	return result;
+}
+
+static void __exit memain_exit(void)
+{
+	dev_t dev = MKDEV(major, 0);
+
+	PDEBUG("executed.\n");
+
+	cdev_del(cdevp);
+	unregister_chrdev_region(dev, 1);
+	pci_unregister_driver(&me_pci_driver);
+//      usb_deregister(&me_usb_driver);
+	clear_device_list();
+}
+
+module_init(memain_init);
+module_exit(memain_exit);
+
+// Administrative stuff for modinfo.
+MODULE_AUTHOR
+    ("Guenter Gebhardt <g.gebhardt@meilhaus.de> & Krzysztof Gantzke <k.gantzke@meilhaus.de>");
+MODULE_DESCRIPTION("Central module for Meilhaus Driver System.");
+MODULE_SUPPORTED_DEVICE("Meilhaus PCI/cPCI boards.");
+MODULE_LICENSE("GPL");
+
+#ifdef BOSCH
+// Export the flag for the BOSCH firmware.
+EXPORT_SYMBOL(me_bosch_fw);
+#endif // BOSCH
diff --git a/drivers/staging/meilhaus/memain.h b/drivers/staging/meilhaus/memain.h
new file mode 100644
index 00000000000..7616ff7f65c
--- /dev/null
+++ b/drivers/staging/meilhaus/memain.h
@@ -0,0 +1,460 @@
+/*
+ * Copyright (C) 2005 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * Source File : memain.h
+ * Author      : GG (Guenter Gebhardt)  <g.gebhardt@meilhaus.de>
+ */
+
+#ifndef _MEMAIN_H_
+#define _MEMAIN_H_
+
+#include "meinternal.h"
+
+#include "meids.h"
+#include "medebug.h"
+
+#include "medevice.h"
+/*#include "me1000_device.h"
+#include "me1400_device.h"
+#include "me1600_device.h"*/
+#include "me4600_device.h"
+/*#include "me6000_device.h"
+#include "me0600_device.h"
+#include "me8100_device.h"
+#include "me8200_device.h"
+#include "me0900_device.h"*/
+#include "medummy.h"
+
+#ifdef __KERNEL__
+
+/*=============================================================================
+  PCI device table.
+  This is used by modprobe to translate PCI IDs to drivers.
+  ===========================================================================*/
+
+static struct pci_device_id me_pci_table[] __devinitdata = {
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME1000, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME1000_A, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME1000_B, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME1400, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME140A, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME140B, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME14E0, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME14EA, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME14EB, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME140C, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME140D, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME1600_4U, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME1600_8U, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME1600_12U, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME1600_16U, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME1600_16U_8I,
+	 PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4610, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4650, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4660, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4660I, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4670, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4670I, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4670S, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4670IS, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4680, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4680I, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4680S, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME4680IS, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6004, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6008, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME600F, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6014, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6018, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME601F, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6034, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6038, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME603F, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6104, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6108, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME610F, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6114, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6118, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME611F, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6134, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6138, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME613F, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6044, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6048, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME604F, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6054, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6058, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME605F, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6074, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6078, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME607F, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6144, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6148, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME614F, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6154, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6158, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME615F, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6174, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6178, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME617F, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6259, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME6359, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME0630, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME8100_A, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME8100_B, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME8200_A, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME8200_B, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME0940, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME0950, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+	{PCI_VENDOR_ID_MEILHAUS, PCI_DEVICE_ID_MEILHAUS_ME0960, PCI_ANY_ID,
+	 PCI_ANY_ID, 0, 0, 0},
+
+	{0}
+};
+
+MODULE_DEVICE_TABLE(pci, me_pci_table);
+
+/*=============================================================================
+  USB device table.
+  This is used by modprobe to translate USB IDs to drivers.
+  ===========================================================================*/
+/*
+static struct usb_device_id me_usb_table[] __devinitdata = {
+	{ USB_DEVICE(USB_VENDOR_ID_MEPHISTO_S1, USB_DEVICE_ID_MEPHISTO_S1) },
+	{ 0 }
+};
+
+MODULE_DEVICE_TABLE (usb, me_usb_table);
+*/
+
+/*=============================================================================
+  Templates
+  ===========================================================================*/
+
+#define ME_LOCK_MULTIPLEX_TEMPLATE(NAME, TYPE, CALL, DEV_CALL, ARGS)	\
+static int CALL(struct file *filep, TYPE *arg){	\
+	int err = 0; \
+	int k = 0; \
+	struct list_head *pos; \
+	me_device_t *device; \
+	TYPE karg; \
+	\
+	PDEBUG("executed.\n"); \
+	\
+	err = copy_from_user(&karg, arg, sizeof(TYPE)); \
+	if(err){ \
+		PERROR("Can't copy arguments to kernel space\n"); \
+		return -EFAULT; \
+	} \
+	\
+	down_read(&me_rwsem);	\
+	\
+	list_for_each(pos, &me_device_list){	\
+		if(k == karg.device){	\
+			device = list_entry(pos, me_device_t, list);	\
+				break;	\
+		}	\
+		k++;	\
+	}	\
+	\
+	if(pos == &me_device_list){ \
+		PERROR("Invalid device number specified\n"); \
+		karg.errno = ME_ERRNO_INVALID_DEVICE; \
+	} \
+	else{ \
+		spin_lock(&me_lock);	\
+		if((me_filep != NULL) && (me_filep != filep)){	\
+			spin_unlock(&me_lock);	\
+			PERROR("Resource is locked by another process\n");	\
+			if(karg.lock == ME_LOCK_SET)	\
+				karg.errno = ME_ERRNO_LOCKED;	\
+			else if(karg.lock == ME_LOCK_RELEASE)	\
+				karg.errno = ME_ERRNO_SUCCESS;	\
+			else{	\
+				PERROR("Invalid lock specified\n");	\
+				karg.errno = ME_ERRNO_INVALID_LOCK;	\
+			}\
+		}	\
+		else {	\
+			me_count++;	\
+			spin_unlock(&me_lock);	\
+			\
+			karg.errno = device->DEV_CALL ARGS;	\
+			\
+			spin_lock(&me_lock);	\
+			me_count--;	\
+			spin_unlock(&me_lock);	\
+		}	\
+	} \
+	\
+	up_read(&me_rwsem);	\
+	\
+	err = copy_to_user(arg, &karg, sizeof(TYPE)); \
+	if(err){ \
+		PERROR("Can't copy arguments back to user space\n"); \
+		return -EFAULT; \
+	} \
+	\
+	return ME_ERRNO_SUCCESS; \
+}
+
+#define ME_IO_MULTIPLEX_TEMPLATE(NAME, TYPE, CALL, DEV_CALL, ARGS)	\
+static int CALL(struct file *filep, TYPE *arg){	\
+	int err = 0; \
+	int k = 0; \
+	struct list_head *pos; \
+	me_device_t *device; \
+	TYPE karg; \
+	\
+	PDEBUG("executed.\n"); \
+	\
+	err = copy_from_user(&karg, arg, sizeof(TYPE)); \
+	if(err){ \
+		PERROR("Can't copy arguments to kernel space\n"); \
+		return -EFAULT; \
+	} \
+	\
+	down_read(&me_rwsem);	\
+	\
+	list_for_each(pos, &me_device_list){	\
+		if(k == karg.device){	\
+			device = list_entry(pos, me_device_t, list);	\
+				break;	\
+		}	\
+		k++;	\
+	}	\
+	\
+	if(pos == &me_device_list){ \
+		PERROR("Invalid device number specified\n"); \
+		karg.errno = ME_ERRNO_INVALID_DEVICE; \
+	} \
+	else{ \
+		spin_lock(&me_lock);	\
+		if((me_filep != NULL) && (me_filep != filep)){	\
+			spin_unlock(&me_lock);	\
+			PERROR("Resource is locked by another process\n");	\
+			karg.errno = ME_ERRNO_LOCKED;	\
+		}	\
+		else {	\
+			me_count++;	\
+			spin_unlock(&me_lock);	\
+			\
+			karg.errno = device->DEV_CALL ARGS;	\
+			\
+			spin_lock(&me_lock);	\
+			me_count--;	\
+			spin_unlock(&me_lock);	\
+		}	\
+	} \
+	\
+	up_read(&me_rwsem);	\
+	\
+	err = copy_to_user(arg, &karg, sizeof(TYPE)); \
+	if(err){ \
+		PERROR("Can't copy arguments back to user space\n"); \
+		return -EFAULT; \
+	} \
+ \
+	return ME_ERRNO_SUCCESS; \
+}
+
+#define ME_QUERY_MULTIPLEX_STR_TEMPLATE(NAME, TYPE, CALL, DEV_CALL, ARGS)	\
+static int CALL(struct file *filep, TYPE *arg){	\
+	int err = 0;	\
+	int k = 0;	\
+	struct list_head *pos;	\
+	me_device_t *device;	\
+	char *msg = NULL;	\
+	TYPE karg;	\
+	\
+	PDEBUG("executed.\n"); \
+	\
+	err = copy_from_user(&karg, arg, sizeof(TYPE));	\
+	if(err){	\
+		PERROR("Can't copy arguments to kernel space\n");	\
+		return -EFAULT;	\
+	}	\
+	\
+	down_read(&me_rwsem);	\
+	\
+	list_for_each(pos, &me_device_list){	\
+		if(k == karg.device){	\
+			device = list_entry(pos, me_device_t, list);	\
+				break;	\
+		}	\
+		k++;	\
+	}	\
+	\
+	if(pos == &me_device_list){	\
+		PERROR("Invalid device number specified\n");	\
+		karg.errno = ME_ERRNO_INVALID_DEVICE;	\
+	}	\
+	else{	\
+		karg.errno = device->DEV_CALL ARGS;	\
+		if(!karg.errno){	\
+			if((strlen(msg) + 1) > karg.count){	\
+				PERROR("User buffer for device name is to little\n");	\
+				karg.errno = ME_ERRNO_USER_BUFFER_SIZE;	\
+			}	\
+			else{	\
+				err = copy_to_user(karg.name, msg, strlen(msg) + 1);	\
+				if(err){	\
+					PERROR("Can't copy device name to user space\n");	\
+					return -EFAULT;	\
+				}	\
+			}	\
+		}	\
+	}	\
+	\
+	up_read(&me_rwsem);	\
+	\
+	err = copy_to_user(arg, &karg, sizeof(TYPE));	\
+	if(err){	\
+		PERROR("Can't copy query back to user space\n");	\
+		return -EFAULT;	\
+	}	\
+	\
+	return ME_ERRNO_SUCCESS;	\
+}
+
+#define ME_QUERY_MULTIPLEX_TEMPLATE(NAME, TYPE, CALL, DEV_CALL, ARGS)	\
+static int CALL(struct file *filep, TYPE *arg){	\
+	int err = 0;	\
+	int k = 0;	\
+	struct list_head *pos;	\
+	me_device_t *device;	\
+	TYPE karg;	\
+		\
+	PDEBUG("executed.\n"); \
+	 \
+	err = copy_from_user(&karg, arg, sizeof(TYPE));	\
+	if(err){	\
+		PERROR("Can't copy arguments from user space\n");	\
+		return -EFAULT;	\
+	}	\
+	\
+	down_read(&me_rwsem);	\
+	\
+	list_for_each(pos, &me_device_list){	\
+		if(k == karg.device){	\
+			device = list_entry(pos, me_device_t, list);	\
+			break;	\
+		}	\
+		k++;	\
+	}	\
+		\
+	if(pos == &me_device_list){	\
+		PERROR("Invalid device number specified\n");	\
+		karg.errno = ME_ERRNO_INVALID_DEVICE;	\
+	}	\
+	else{	\
+		karg.errno = device->DEV_CALL ARGS;	\
+	}	\
+	\
+	up_read(&me_rwsem);	\
+	\
+	err = copy_to_user(arg, &karg, sizeof(TYPE));	\
+	if(err){	\
+		PERROR("Can't copy arguments to user space\n");	\
+		return -EFAULT;	\
+	}	\
+		\
+	return ME_ERRNO_SUCCESS;	\
+}
+
+#endif //__KERNEL__
+#endif
diff --git a/drivers/staging/meilhaus/meplx_reg.h b/drivers/staging/meilhaus/meplx_reg.h
new file mode 100644
index 00000000000..1868614dc23
--- /dev/null
+++ b/drivers/staging/meilhaus/meplx_reg.h
@@ -0,0 +1,53 @@
+/**
+ * @file meplx_reg.h
+ *
+ * @brief PLX 9052 PCI bridge register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _MEPLX_REG_H_
+#define _MEPLX_REG_H_
+
+#ifdef __KERNEL__
+
+#define PLX_INTCSR				0x4C		/**< Interrupt control and status register. */
+#define PLX_INTCSR_LOCAL_INT1_EN		0x01		/**< If set, local interrupt 1 is enabled (r/w). */
+#define PLX_INTCSR_LOCAL_INT1_POL		0x02		/**< If set, local interrupt 1 polarity is active high (r/w). */
+#define PLX_INTCSR_LOCAL_INT1_STATE		0x04		/**< If set, local interrupt 1 is active (r/_). */
+#define PLX_INTCSR_LOCAL_INT2_EN		0x08		/**< If set, local interrupt 2 is enabled (r/w). */
+#define PLX_INTCSR_LOCAL_INT2_POL		0x10		/**< If set, local interrupt 2 polarity is active high (r/w). */
+#define PLX_INTCSR_LOCAL_INT2_STATE		0x20		/**< If set, local interrupt 2 is active  (r/_). */
+#define PLX_INTCSR_PCI_INT_EN			0x40		/**< If set, PCI interrupt is enabled (r/w). */
+#define PLX_INTCSR_SOFT_INT			0x80		/**< If set, a software interrupt is generated (r/w). */
+
+#define PLX_ICR					0x50		/**< Initialization control register. */
+#define PLX_ICR_BIT_EEPROM_CLOCK_SET		0x01000000
+#define PLX_ICR_BIT_EEPROM_CHIP_SELECT		0x02000000
+#define PLX_ICR_BIT_EEPROM_WRITE		0x04000000
+#define PLX_ICR_BIT_EEPROM_READ			0x08000000
+#define PLX_ICR_BIT_EEPROM_VALID		0x10000000
+
+#define PLX_ICR_MASK_EEPROM			0x1F000000
+#define EEPROM_DELAY				1
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/meslist.c b/drivers/staging/meilhaus/meslist.c
new file mode 100644
index 00000000000..7e8b66c05f7
--- /dev/null
+++ b/drivers/staging/meilhaus/meslist.c
@@ -0,0 +1,173 @@
+/**
+ * @file me_slist.c
+ *
+ * @brief Implements the subdevice list class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include "meerror.h"
+#include "medefines.h"
+
+#include "meslist.h"
+#include "medebug.h"
+
+int me_slist_query_number_subdevices(struct me_slist *slist, int *number)
+{
+	PDEBUG_LOCKS("called.\n");
+	*number = slist->n;
+	return ME_ERRNO_SUCCESS;
+}
+
+unsigned int me_slist_get_number_subdevices(struct me_slist *slist)
+{
+	PDEBUG_LOCKS("called.\n");
+	return slist->n;
+}
+
+me_subdevice_t *me_slist_get_subdevice(struct me_slist * slist,
+				       unsigned int index)
+{
+
+	struct list_head *pos;
+	me_subdevice_t *subdevice = NULL;
+	unsigned int i = 0;
+
+	PDEBUG_LOCKS("called.\n");
+
+	if (index >= slist->n) {
+		PERROR("Index out of range.\n");
+		return NULL;
+	}
+
+	list_for_each(pos, &slist->head) {
+		if (i == index) {
+			subdevice = list_entry(pos, me_subdevice_t, list);
+			break;
+		}
+
+		++i;
+	}
+
+	return subdevice;
+}
+
+int me_slist_get_subdevice_by_type(struct me_slist *slist,
+				   unsigned int start_subdevice,
+				   int type, int subtype, int *subdevice)
+{
+	me_subdevice_t *pos;
+	int s_type, s_subtype;
+	unsigned int index = 0;
+
+	PDEBUG_LOCKS("called.\n");
+
+	if (start_subdevice >= slist->n) {
+		PERROR("Start index out of range.\n");
+		return ME_ERRNO_NOMORE_SUBDEVICE_TYPE;
+	}
+
+	list_for_each_entry(pos, &slist->head, list) {
+		if (index < start_subdevice) {	// Go forward to start subdevice.
+			++index;
+			continue;
+		}
+
+		pos->me_subdevice_query_subdevice_type(pos,
+						       &s_type, &s_subtype);
+
+		if (subtype == ME_SUBTYPE_ANY) {
+			if (s_type == type)
+				break;
+		} else {
+			if ((s_type == type) && (s_subtype == subtype))
+				break;
+		}
+
+		++index;
+	}
+
+	if (index >= slist->n) {
+		return ME_ERRNO_NOMORE_SUBDEVICE_TYPE;
+	}
+
+	*subdevice = index;
+
+	return ME_ERRNO_SUCCESS;
+}
+
+void me_slist_add_subdevice_tail(struct me_slist *slist,
+				 me_subdevice_t * subdevice)
+{
+	PDEBUG_LOCKS("called.\n");
+
+	list_add_tail(&subdevice->list, &slist->head);
+	++slist->n;
+}
+
+me_subdevice_t *me_slist_del_subdevice_tail(struct me_slist *slist)
+{
+
+	struct list_head *last;
+	me_subdevice_t *subdevice;
+
+	PDEBUG_LOCKS("called.\n");
+
+	if (list_empty(&slist->head))
+		return NULL;
+
+	last = slist->head.prev;
+
+	subdevice = list_entry(last, me_subdevice_t, list);
+
+	list_del(last);
+
+	--slist->n;
+
+	return subdevice;
+}
+
+int me_slist_init(me_slist_t * slist)
+{
+	PDEBUG_LOCKS("called.\n");
+
+	INIT_LIST_HEAD(&slist->head);
+	slist->n = 0;
+	return 0;
+}
+
+void me_slist_deinit(me_slist_t * slist)
+{
+
+	struct list_head *s;
+	me_subdevice_t *subdevice;
+
+	PDEBUG_LOCKS("called.\n");
+
+	while (!list_empty(&slist->head)) {
+		s = slist->head.next;
+		list_del(s);
+		subdevice = list_entry(s, me_subdevice_t, list);
+		subdevice->me_subdevice_destructor(subdevice);
+	}
+
+	slist->n = 0;
+}
diff --git a/drivers/staging/meilhaus/meslist.h b/drivers/staging/meilhaus/meslist.h
new file mode 100644
index 00000000000..d26c89693d2
--- /dev/null
+++ b/drivers/staging/meilhaus/meslist.h
@@ -0,0 +1,108 @@
+/**
+ * @file me_slist.h
+ *
+ * @brief Provides the subdevice list class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+#ifndef _ME_SLIST_H_
+#define _ME_SLIST_H_
+
+#include <linux/list.h>
+
+#include "mesubdevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The subdevice list container.
+ */
+typedef struct me_slist {
+	struct list_head head;		/**< The head of the internal list. */
+	unsigned int n;			/**< The number of subdevices in the list. */
+} me_slist_t;
+
+/**
+ * @brief Queries the number of subdevices currently inside the list.
+ *
+ * @param slist The subdevice list to query.
+ * @param[out] number The number of subdevices of the device.
+ *
+ * @return ME-iDS error code.
+ */
+int me_slist_query_number_subdevices(struct me_slist *slist, int *number);
+
+/**
+ * @brief Returns the number of subdevices currently inside the list.
+ *
+ * @param slist The subdevice list to query.
+ *
+ * @return The number of subdevices in the list.
+ */
+unsigned int me_slist_get_number_subdevices(struct me_slist *slist);
+
+/**
+ * @brief Get a subdevice by index.
+ *
+ * @param slist The subdevice list to query.
+ * @param index The index of the subdevice to get in the list.
+ *
+ * @return The subdevice at index if available.\n
+ *         NULL if the index is out of range.
+ */
+me_subdevice_t *me_slist_get_subdevice(struct me_slist *slist,
+				       unsigned int index);
+
+/**
+ * @brief Get a subdevice index by type and subtype.
+ *
+ * @param slist The subdevice list to query.
+ * @param start_subdevice The subdevice index at which the start shall begin.
+ * @param type The type of the subdevice to query.
+ * @param subtype The subtype of the subdevice to query.
+ * @param[out] subdevice On success this parameter returns the index of the subdevice matching the requested type.
+ *
+ * @return ME_ERRNO_SUCCESS on success.
+ */
+int me_slist_get_subdevice_by_type(struct me_slist *slist,
+				   unsigned int start_subdevice,
+				   int type, int subtype, int *subdevice);
+
+/**
+ * @brief Adds a subdevice to the tail of the list.
+ *
+ * @param slist The subdevice list to add a subdevice to.
+ * @param subdevice The subdevice to add to the list.
+ */
+void me_slist_add_subdevice_tail(struct me_slist *slist,
+				 me_subdevice_t * subdevice);
+
+/**
+ * @brief Removes a subdevice from the tail of the list.
+ *
+ * @param slist The subdevice list.
+ *
+ * @return Pointer to the removed subdeivce.\n
+ *         NULL in cases where the list was empty.
+ */
+me_subdevice_t *me_slist_del_subdevice_tail(struct me_slist *slist);
+
+/**
+ * @brief Initializes a subdevice list structure.
+ *
+ * @param lock The subdevice list structure to initialize.
+ * @return 0 on success.
+ */
+int me_slist_init(me_slist_t * slist);
+
+/**
+ * @brief Deinitializes a subdevice list structure and destructs every subdevice in it.
+ *
+ * @param slist The subdevice list structure to deinitialize.
+ * @return 0 on success.
+ */
+void me_slist_deinit(me_slist_t * slist);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/meslock.c b/drivers/staging/meilhaus/meslock.c
new file mode 100644
index 00000000000..5230b89b45b
--- /dev/null
+++ b/drivers/staging/meilhaus/meslock.c
@@ -0,0 +1,136 @@
+/**
+ * @file meslock.c
+ *
+ * @brief Implements the subdevice lock class.
+ * @note Copyright (C) 2006 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2006 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/spinlock.h>
+
+#include "medefines.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "meslock.h"
+
+int me_slock_enter(struct me_slock *slock, struct file *filep)
+{
+	PDEBUG_LOCKS("executed.\n");
+
+	spin_lock(&slock->spin_lock);
+
+	if ((slock->filep) != NULL && (slock->filep != filep)) {
+		PERROR("Subdevice is locked by another process.\n");
+		spin_unlock(&slock->spin_lock);
+		return ME_ERRNO_LOCKED;
+	}
+
+	slock->count++;
+
+	spin_unlock(&slock->spin_lock);
+
+	return ME_ERRNO_SUCCESS;
+}
+
+int me_slock_exit(struct me_slock *slock, struct file *filep)
+{
+	PDEBUG_LOCKS("executed.\n");
+
+	spin_lock(&slock->spin_lock);
+	slock->count--;
+	spin_unlock(&slock->spin_lock);
+
+	return ME_ERRNO_SUCCESS;
+}
+
+int me_slock_lock(struct me_slock *slock, struct file *filep, int lock)
+{
+	PDEBUG_LOCKS("executed.\n");
+
+	switch (lock) {
+
+	case ME_LOCK_RELEASE:
+		spin_lock(&slock->spin_lock);
+
+		if (slock->filep == filep)
+			slock->filep = NULL;
+
+		spin_unlock(&slock->spin_lock);
+
+		break;
+
+	case ME_LOCK_SET:
+		spin_lock(&slock->spin_lock);
+
+		if (slock->count) {
+			spin_unlock(&slock->spin_lock);
+			PERROR("Subdevice is used by another process.\n");
+			return ME_ERRNO_USED;
+		} else if (slock->filep == NULL)
+			slock->filep = filep;
+		else if (slock->filep != filep) {
+			spin_unlock(&slock->spin_lock);
+			PERROR("Subdevice is locked by another process.\n");
+			return ME_ERRNO_LOCKED;
+		}
+
+		spin_unlock(&slock->spin_lock);
+
+		break;
+
+	case ME_LOCK_CHECK:
+		spin_lock(&slock->spin_lock);
+
+		if (slock->count) {
+			spin_unlock(&slock->spin_lock);
+			return ME_ERRNO_USED;
+		} else if ((slock->filep != NULL) && (slock->filep != filep)) {
+			spin_unlock(&slock->spin_lock);
+			return ME_ERRNO_LOCKED;
+		}
+
+		spin_unlock(&slock->spin_lock);
+
+		break;
+
+	default:
+		break;
+	}
+
+	return ME_ERRNO_SUCCESS;
+}
+
+void me_slock_deinit(struct me_slock *slock)
+{
+	PDEBUG_LOCKS("executed.\n");
+}
+
+int me_slock_init(me_slock_t * slock)
+{
+	PDEBUG_LOCKS("executed.\n");
+
+	slock->filep = NULL;
+	slock->count = 0;
+	spin_lock_init(&slock->spin_lock);
+
+	return 0;
+}
diff --git a/drivers/staging/meilhaus/meslock.h b/drivers/staging/meilhaus/meslock.h
new file mode 100644
index 00000000000..f42b25c3f62
--- /dev/null
+++ b/drivers/staging/meilhaus/meslock.h
@@ -0,0 +1,73 @@
+/**
+ * @file meslock.h
+ *
+ * @brief Provides the subdevice lock class.
+ * @note Copyright (C) 2006 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+#ifndef _MESLOCK_H_
+#define _MESLOCK_H_
+
+#include <linux/spinlock.h>
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The subdevice lock class.
+ */
+typedef struct me_slock {
+	struct file *filep;		/**< Pointer to file structure holding the subdevice. */
+	int count;			/**< Number of tasks which are inside the subdevice. */
+	spinlock_t spin_lock;		/**< Spin lock protecting the attributes from concurrent access. */
+} me_slock_t;
+
+/**
+ * @brief Tries to enter a subdevice.
+ *
+ * @param slock The subdevice lock instance.
+ * @param filep The file structure identifying the calling process.
+ *
+ * @return 0 on success.
+ */
+int me_slock_enter(struct me_slock *slock, struct file *filep);
+
+/**
+ * @brief Exits a subdevice.
+ *
+ * @param slock The subdevice lock instance.
+ * @param filep The file structure identifying the calling process.
+ *
+ * @return 0 on success.
+ */
+int me_slock_exit(struct me_slock *slock, struct file *filep);
+
+/**
+ * @brief Tries to perform a locking action on a subdevice.
+ *
+ * @param slock The subdevice lock instance.
+ * @param filep The file structure identifying the calling process.
+ * @param The action to be done.
+ *
+ * @return 0 on success.
+ */
+int me_slock_lock(struct me_slock *slock, struct file *filep, int lock);
+
+/**
+ * @brief Initializes a lock structure.
+ *
+ * @param slock The lock structure to initialize.
+ * @return 0 on success.
+ */
+int me_slock_init(me_slock_t * slock);
+
+/**
+ * @brief Deinitializes a lock structure.
+ *
+ * @param slock The lock structure to deinitialize.
+ * @return 0 on success.
+ */
+void me_slock_deinit(me_slock_t * slock);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/mesubdevice.c b/drivers/staging/meilhaus/mesubdevice.c
new file mode 100644
index 00000000000..98d4f1f7a82
--- /dev/null
+++ b/drivers/staging/meilhaus/mesubdevice.c
@@ -0,0 +1,317 @@
+/**
+ * @file mesubdevice.c
+ *
+ * @brief Subdevice base class implemention.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+#include <linux/slab.h>
+
+#include "medefines.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "mesubdevice.h"
+
+static int me_subdevice_io_irq_start(struct me_subdevice *subdevice,
+				     struct file *filep,
+				     int channel,
+				     int irq_source,
+				     int irq_edge, int irq_arg, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_io_irq_wait(struct me_subdevice *subdevice,
+				    struct file *filep,
+				    int channel,
+				    int *irq_count,
+				    int *value, int time_out, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_io_irq_stop(struct me_subdevice *subdevice,
+				    struct file *filep, int channel, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_io_reset_subdevice(struct me_subdevice *subdevice,
+					   struct file *filep, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_io_single_config(struct me_subdevice *subdevice,
+					 struct file *filep,
+					 int channel,
+					 int single_config,
+					 int ref,
+					 int trig_chan,
+					 int trig_type,
+					 int trig_edge, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_io_single_read(struct me_subdevice *subdevice,
+				       struct file *filep,
+				       int channel,
+				       int *value, int time_out, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_io_single_write(struct me_subdevice *subdevice,
+					struct file *filep,
+					int channel,
+					int value, int time_out, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_io_stream_config(struct me_subdevice *subdevice,
+					 struct file *filep,
+					 meIOStreamConfig_t * config_list,
+					 int count,
+					 meIOStreamTrigger_t * trigger,
+					 int fifo_irq_threshold, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_io_stream_new_values(struct me_subdevice *subdevice,
+					     struct file *filep,
+					     int time_out,
+					     int *count, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_io_stream_read(struct me_subdevice *subdevice,
+				       struct file *filep,
+				       int read_mode,
+				       int *values, int *count, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_io_stream_start(struct me_subdevice *subdevice,
+					struct file *filep,
+					int start_mode, int time_out, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_io_stream_status(struct me_subdevice *subdevice,
+					 struct file *filep,
+					 int wait,
+					 int *status, int *count, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_io_stream_stop(struct me_subdevice *subdevice,
+				       struct file *filep,
+				       int stop_mode, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_io_stream_write(struct me_subdevice *subdevice,
+					struct file *filep,
+					int write_mode,
+					int *values, int *count, int flags)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_lock_subdevice(me_subdevice_t * subdevice,
+				       struct file *filep, int lock, int flags)
+{
+	PDEBUG("executed.\n");
+	return me_slock_lock(&subdevice->lock, filep, lock);
+}
+
+static int me_subdevice_query_number_channels(struct me_subdevice *subdevice,
+					      int *number)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_query_number_ranges(struct me_subdevice *subdevice,
+					    int unit, int *count)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_query_range_by_min_max(struct me_subdevice *subdevice,
+					       int unit,
+					       int *min,
+					       int *max,
+					       int *maxdata, int *range)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_query_range_info(struct me_subdevice *subdevice,
+					 int range,
+					 int *unit,
+					 int *min, int *max, int *maxdata)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_query_subdevice_type(struct me_subdevice *subdevice,
+					     int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_query_subdevice_caps(struct me_subdevice *subdevice,
+					     int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = 0;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int me_subdevice_query_subdevice_caps_args(struct me_subdevice
+						  *subdevice, int cap,
+						  int *args, int count)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_query_timer(struct me_subdevice *subdevice,
+				    int timer,
+				    int *base_frequency,
+				    long long *min_ticks, long long *max_ticks)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_NOT_SUPPORTED;
+}
+
+static int me_subdevice_config_load(struct me_subdevice *subdevice,
+				    me_cfg_device_entry_t * config)
+{
+	PDEBUG("executed.\n");
+	return ME_ERRNO_SUCCESS;
+}
+
+static void me_subdevice_destructor(struct me_subdevice *subdevice)
+{
+	PDEBUG("executed.\n");
+	me_subdevice_deinit(subdevice);
+	kfree(subdevice);
+}
+
+int me_subdevice_init(me_subdevice_t * subdevice)
+{
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Init list head */
+	INIT_LIST_HEAD(&subdevice->list);
+
+	/* Initialize the subdevice lock instance */
+
+	err = me_slock_init(&subdevice->lock);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice lock instance.\n");
+		return 1;
+	}
+
+	/* Subdevice base class methods */
+	subdevice->me_subdevice_io_irq_start = me_subdevice_io_irq_start;
+	subdevice->me_subdevice_io_irq_wait = me_subdevice_io_irq_wait;
+	subdevice->me_subdevice_io_irq_stop = me_subdevice_io_irq_stop;
+	subdevice->me_subdevice_io_reset_subdevice =
+	    me_subdevice_io_reset_subdevice;
+	subdevice->me_subdevice_io_single_config =
+	    me_subdevice_io_single_config;
+	subdevice->me_subdevice_io_single_read = me_subdevice_io_single_read;
+	subdevice->me_subdevice_io_single_write = me_subdevice_io_single_write;
+	subdevice->me_subdevice_io_stream_config =
+	    me_subdevice_io_stream_config;
+	subdevice->me_subdevice_io_stream_new_values =
+	    me_subdevice_io_stream_new_values;
+	subdevice->me_subdevice_io_stream_read = me_subdevice_io_stream_read;
+	subdevice->me_subdevice_io_stream_start = me_subdevice_io_stream_start;
+	subdevice->me_subdevice_io_stream_status =
+	    me_subdevice_io_stream_status;
+	subdevice->me_subdevice_io_stream_stop = me_subdevice_io_stream_stop;
+	subdevice->me_subdevice_io_stream_write = me_subdevice_io_stream_write;
+	subdevice->me_subdevice_lock_subdevice = me_subdevice_lock_subdevice;
+	subdevice->me_subdevice_query_number_channels =
+	    me_subdevice_query_number_channels;
+	subdevice->me_subdevice_query_number_ranges =
+	    me_subdevice_query_number_ranges;
+	subdevice->me_subdevice_query_range_by_min_max =
+	    me_subdevice_query_range_by_min_max;
+	subdevice->me_subdevice_query_range_info =
+	    me_subdevice_query_range_info;
+	subdevice->me_subdevice_query_subdevice_type =
+	    me_subdevice_query_subdevice_type;
+	subdevice->me_subdevice_query_subdevice_caps =
+	    me_subdevice_query_subdevice_caps;
+	subdevice->me_subdevice_query_subdevice_caps_args =
+	    me_subdevice_query_subdevice_caps_args;
+	subdevice->me_subdevice_query_timer = me_subdevice_query_timer;
+	subdevice->me_subdevice_config_load = me_subdevice_config_load;
+	subdevice->me_subdevice_destructor = me_subdevice_destructor;
+
+	return 0;
+}
+
+void me_subdevice_deinit(me_subdevice_t * subdevice)
+{
+	PDEBUG("executed.\n");
+	me_subdevice_io_reset_subdevice(subdevice, NULL,
+					ME_IO_RESET_SUBDEVICE_NO_FLAGS);
+	me_slock_deinit(&subdevice->lock);
+}
diff --git a/drivers/staging/meilhaus/mesubdevice.h b/drivers/staging/meilhaus/mesubdevice.h
new file mode 100644
index 00000000000..19ec2b5d96f
--- /dev/null
+++ b/drivers/staging/meilhaus/mesubdevice.h
@@ -0,0 +1,197 @@
+/**
+ * @file mesubdevice.h
+ *
+ * @brief Provides the subdevice base class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+#ifndef _MESUBDEVICE_H_
+#define _MESUBDEVICE_H_
+
+#include <linux/list.h>
+
+#include "metypes.h"
+#include "meioctl.h"
+#include "meslock.h"
+
+# include <linux/workqueue.h>
+
+#ifdef __KERNEL__
+
+/**
+ * @brief Macro used to enter a subdevice.
+ */
+#define ME_SUBDEVICE_ENTER	\
+{ \
+	int err; \
+	err = me_slock_enter(&instance->base.lock, filep); \
+	if(err){ \
+		PERROR("Cannot enter subdevice.\n"); \
+		return err; \
+	} \
+}
+
+/**
+ * @brief Macro used to exit a subdevice.
+ */
+#define ME_SUBDEVICE_EXIT	\
+{\
+	int err; \
+	err = me_slock_exit(&instance->base.lock, filep); \
+	if(err){ \
+		PERROR("Cannot exit subdevice.\n"); \
+		return err; \
+	} \
+}
+
+/**
+ * @brief The subdevice base class.
+ */
+typedef struct me_subdevice {
+	/* Attributes */
+	struct list_head list;		/**< Enables the subdevice to be added to a dynamic list. */
+	me_slock_t lock;		/**< Used by user application in order to lock the subdevice for exclusive usage. */
+
+	/* Methods */
+	int (*me_subdevice_io_irq_start) (struct me_subdevice * subdevice,
+					  struct file * filep,
+					  int channel,
+					  int irq_source,
+					  int irq_edge, int irq_arg, int flags);
+
+	int (*me_subdevice_io_irq_wait) (struct me_subdevice * subdevice,
+					 struct file * filep,
+					 int channel,
+					 int *irq_count,
+					 int *value, int time_out, int flags);
+
+	int (*me_subdevice_io_irq_stop) (struct me_subdevice * subdevice,
+					 struct file * filep,
+					 int channel, int flags);
+
+	int (*me_subdevice_io_reset_subdevice) (struct me_subdevice * subdevice,
+						struct file * filep, int flags);
+
+	int (*me_subdevice_io_single_config) (struct me_subdevice * subdevice,
+					      struct file * filep,
+					      int channel,
+					      int single_config,
+					      int ref,
+					      int trig_chan,
+					      int trig_type,
+					      int trig_edge, int flags);
+
+	int (*me_subdevice_io_single_read) (struct me_subdevice * subdevice,
+					    struct file * filep,
+					    int channel,
+					    int *value,
+					    int time_out, int flags);
+
+	int (*me_subdevice_io_single_write) (struct me_subdevice * subdevice,
+					     struct file * filep,
+					     int channel,
+					     int value,
+					     int time_out, int flags);
+
+	int (*me_subdevice_io_stream_config) (struct me_subdevice * subdevice,
+					      struct file * filep,
+					      meIOStreamConfig_t * config_list,
+					      int count,
+					      meIOStreamTrigger_t * trigger,
+					      int fifo_irq_threshold,
+					      int flags);
+
+	int (*me_subdevice_io_stream_new_values) (struct me_subdevice *
+						  subdevice,
+						  struct file * filep,
+						  int time_out, int *count,
+						  int flags);
+
+	int (*me_subdevice_io_stream_read) (struct me_subdevice * subdevice,
+					    struct file * filep,
+					    int read_mode,
+					    int *values, int *count, int flags);
+
+	int (*me_subdevice_io_stream_start) (struct me_subdevice * subdevice,
+					     struct file * filep,
+					     int start_mode,
+					     int time_out, int flags);
+
+	int (*me_subdevice_io_stream_status) (struct me_subdevice * subdevice,
+					      struct file * filep,
+					      int wait,
+					      int *status,
+					      int *count, int flags);
+
+	int (*me_subdevice_io_stream_stop) (struct me_subdevice * subdevice,
+					    struct file * filep,
+					    int stop_mode, int flags);
+
+	int (*me_subdevice_io_stream_write) (struct me_subdevice * subdevice,
+					     struct file * filep,
+					     int write_mode,
+					     int *values,
+					     int *count, int flags);
+
+	int (*me_subdevice_lock_subdevice) (struct me_subdevice * subdevice,
+					    struct file * filep,
+					    int lock, int flags);
+
+	int (*me_subdevice_query_number_channels) (struct me_subdevice *
+						   subdevice, int *number);
+
+	int (*me_subdevice_query_number_ranges) (struct me_subdevice *
+						 subdevice, int unit,
+						 int *count);
+
+	int (*me_subdevice_query_range_by_min_max) (struct me_subdevice *
+						    subdevice, int unit,
+						    int *min, int *max,
+						    int *maxdata, int *range);
+
+	int (*me_subdevice_query_range_info) (struct me_subdevice * subdevice,
+					      int range,
+					      int *unit,
+					      int *min, int *max, int *maxdata);
+
+	int (*me_subdevice_query_subdevice_type) (struct me_subdevice *
+						  subdevice, int *type,
+						  int *subtype);
+
+	int (*me_subdevice_query_subdevice_caps) (struct me_subdevice *
+						  subdevice, int *caps);
+
+	int (*me_subdevice_query_subdevice_caps_args) (struct me_subdevice *
+						       subdevice, int cap,
+						       int *args, int count);
+
+	int (*me_subdevice_query_timer) (struct me_subdevice * subdevice,
+					 int timer,
+					 int *base_frequency,
+					 long long *min_ticks,
+					 long long *max_ticks);
+
+	int (*me_subdevice_config_load) (struct me_subdevice * subdevice,
+					 me_cfg_device_entry_t * config);
+
+	void (*me_subdevice_destructor) (struct me_subdevice * subdevice);
+} me_subdevice_t;
+
+/**
+ * @brief Initializes a subdevice structure.
+ *
+ * @param subdevice The subdevice structure to initialize.
+ * @return 0 on success.
+ */
+int me_subdevice_init(me_subdevice_t * subdevice);
+
+/**
+ * @brief Deinitializes a subdevice structure.
+ *
+ * @param subdevice The subdevice structure to initialize.
+ */
+void me_subdevice_deinit(me_subdevice_t * subdevice);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/metempl_device.c b/drivers/staging/meilhaus/metempl_device.c
new file mode 100644
index 00000000000..e48632ddc1a
--- /dev/null
+++ b/drivers/staging/meilhaus/metempl_device.c
@@ -0,0 +1,137 @@
+/**
+ * @file metempl_device.c
+ *
+ * @brief template device class implementation.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+#ifndef MODULE
+#  define MODULE
+#endif
+
+#include <linux/module.h>
+
+#include <linux/pci.h>
+#include <linux/slab.h>
+
+#include <meids.h>
+#include "meerror.h"
+#include "mecommon.h"
+#include "meinternal.h"
+
+#include "medebug.h"
+#include "medevice.h"
+#include "metempl_device.h"
+#include "mesubdevice.h"
+#include "metempl_sub.h"
+
+me_device_t *metempl_pci_constructor(struct pci_dev *pci_device)
+{
+	metempl_device_t *metempl_device;
+	me_subdevice_t *subdevice;
+	unsigned int version_idx;
+	int err;
+	int i;
+
+	PDEBUG("executed.\n");
+
+	// Allocate structure for device instance.
+	metempl_device = kmalloc(sizeof(metempl_device_t), GFP_KERNEL);
+
+	if (!metempl_device) {
+		PERROR("Cannot get memory for device instance.\n");
+		return NULL;
+	}
+
+	memset(metempl_device, 0, sizeof(metempl_device_t));
+
+	// Initialize base class structure.
+	err = me_device_pci_init((me_device_t *) metempl_device, pci_device);
+
+	if (err) {
+		kfree(metempl_device);
+		PERROR("Cannot initialize device base class.\n");
+		return NULL;
+	}
+
+	/* Get the index in the device version information table. */
+	version_idx =
+	    metempl_versions_get_device_index(metempl_device->base.info.pci.
+					      device_id);
+
+	// Initialize spin lock .
+	spin_lock_init(&metempl_device->ctrl_reg_lock);
+
+	// Create subdevice instances.
+	for (i = 0; i < metempl_versions[version_idx].subdevices; i++) {
+		subdevice =
+		    (me_subdevice_t *) metempl_sub_constructor(metempl_device->
+							       base.info.pci.
+							       reg_bases[2], i,
+							       &metempl_device->
+							       ctrl_reg_lock);
+
+		if (!subdevice) {
+			me_device_deinit((me_device_t *) metempl_device);
+			kfree(metempl_device);
+			PERROR("Cannot get memory for subdevice.\n");
+			return NULL;
+		}
+
+		me_slist_add_subdevice_tail(&metempl_device->base.slist,
+					    subdevice);
+	}
+
+	/* Overwrite base class methods if applicable. */
+
+	return (me_device_t *) metempl_device;
+}
+
+// Init and exit of module.
+
+static int __init metempl_init(void)
+{
+	PDEBUG("executed.\n.");
+	return 0;
+}
+
+static void __exit metempl_exit(void)
+{
+	PDEBUG("executed.\n.");
+}
+
+module_init(metempl_init);
+
+module_exit(metempl_exit);
+
+// Administrative stuff for modinfo.
+MODULE_AUTHOR("Guenter Gebhardt <g.gebhardt@meilhaus.de>");
+MODULE_DESCRIPTION("Device Driver Module for Template Device");
+MODULE_SUPPORTED_DEVICE("Meilhaus Template Devices");
+MODULE_LICENSE("GPL");
+
+// Export the constructor.
+EXPORT_SYMBOL(metempl_pci_constructor);
diff --git a/drivers/staging/meilhaus/metempl_device.h b/drivers/staging/meilhaus/metempl_device.h
new file mode 100644
index 00000000000..3c3702cc72e
--- /dev/null
+++ b/drivers/staging/meilhaus/metempl_device.h
@@ -0,0 +1,92 @@
+/**
+ * @file metempl_device.h
+ *
+ * @brief template device class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _METEMPL_DEVICE_H
+#define _METEMPL_DEVICE_H
+
+#include <linux/pci.h>
+#include <linux/spinlock.h>
+
+#include "medevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief Structure holding template device capabilities.
+ */
+typedef struct metempl_version {
+	uint16_t device_id;
+	unsigned int subdevices;
+} metempl_version_t;
+
+/**
+ * @brief Device capabilities.
+ */
+static metempl_version_t metempl_versions[] = {
+	{0xDEAD, 1},
+	{0},
+};
+
+#define METEMPL_DEVICE_VERSIONS (sizeof(metempl_versions) / sizeof(metempl_version_t) - 1) /**< Returns the number of entries in #metempl_versions. */
+
+/**
+ * @brief Returns the index of the device entry in #metempl_versions.
+ *
+ * @param device_id The PCI device id of the device to query.
+ * @return The index of the device in #metempl_versions.
+ */
+static inline unsigned int metempl_versions_get_device_index(uint16_t device_id)
+{
+	unsigned int i;
+	for (i = 0; i < METEMPL_DEVICE_VERSIONS; i++)
+		if (metempl_versions[i].device_id == device_id)
+			break;
+	return i;
+}
+
+/**
+ * @brief The template device class structure.
+ */
+typedef struct metempl_device {
+	me_device_t base;			/**< The Meilhaus device base class. */
+
+	/* Child class attributes. */
+	spinlock_t ctrl_reg_lock;
+} metempl_device_t;
+
+/**
+ * @brief The template device class constructor.
+ *
+ * @param pci_device The pci device structure given by the PCI subsystem.
+ *
+ * @return On succes a new template device instance. \n
+ *         NULL on error.
+ */
+me_device_t *metempl_pci_constructor(struct pci_dev *pci_device)
+    __attribute__ ((weak));
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/metempl_sub.c b/drivers/staging/meilhaus/metempl_sub.c
new file mode 100644
index 00000000000..f1d65d889e2
--- /dev/null
+++ b/drivers/staging/meilhaus/metempl_sub.c
@@ -0,0 +1,149 @@
+/**
+ * @file metempl_sub.c
+ *
+ * @brief Subdevice instance.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef __KERNEL__
+#  define __KERNEL__
+#endif
+
+/*
+ * Includes
+ */
+#include <linux/module.h>
+
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <asm/io.h>
+#include <linux/types.h>
+
+#include "medefines.h"
+#include "meinternal.h"
+#include "meerror.h"
+
+#include "medebug.h"
+#include "metempl_sub_reg.h"
+#include "metempl_sub.h"
+
+/*
+ * Defines
+ */
+
+/*
+ * Functions
+ */
+
+static void metempl_sub_destructor(struct me_subdevice *subdevice)
+{
+	metempl_sub_subdevice_t *instance;
+
+	PDEBUG("executed.\n");
+	instance = (metempl_sub_subdevice_t *) subdevice;
+
+	/* Until there this was the things the default constructor does.
+	   If you do not have any additional things to do you can wipe it out. */
+
+	me_subdevice_deinit(&instance->base);
+	kfree(instance);
+}
+
+static int metempl_sub_query_number_channels(me_subdevice_t * subdevice,
+					     int *number)
+{
+	PDEBUG("executed.\n");
+	*number = 0;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int metempl_sub_query_subdevice_type(me_subdevice_t * subdevice,
+					    int *type, int *subtype)
+{
+	PDEBUG("executed.\n");
+	*type = 0;
+	*subtype = 0;
+	return ME_ERRNO_SUCCESS;
+}
+
+static int metempl_sub_query_subdevice_caps(me_subdevice_t * subdevice,
+					    int *caps)
+{
+	PDEBUG("executed.\n");
+	*caps = 0;
+	return ME_ERRNO_SUCCESS;
+}
+
+metempl_sub_subdevice_t *metempl_sub_constructor(uint32_t reg_base,
+						 unsigned int sub_idx,
+						 spinlock_t * ctrl_reg_lock)
+{
+	metempl_sub_subdevice_t *subdevice;
+	int err;
+
+	PDEBUG("executed.\n");
+
+	/* Allocate memory for subdevice instance */
+	subdevice = kmalloc(sizeof(metempl_sub_subdevice_t), GFP_KERNEL);
+
+	if (!subdevice) {
+		PERROR("Cannot get memory for subdevice instance.\n");
+		return NULL;
+	}
+
+	memset(subdevice, 0, sizeof(metempl_sub_subdevice_t));
+
+	/* Check if subdevice index is out of range */
+
+	if (sub_idx >= 2) {
+		PERROR("Template subdevice index is out of range.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+
+	/* Initialize subdevice base class */
+	err = me_subdevice_init(&subdevice->base);
+
+	if (err) {
+		PERROR("Cannot initialize subdevice base class instance.\n");
+		kfree(subdevice);
+		return NULL;
+	}
+	// Initialize spin locks.
+	spin_lock_init(&subdevice->subdevice_lock);
+
+	subdevice->ctrl_reg_lock = ctrl_reg_lock;
+
+	/* Save the subdevice index */
+	subdevice->sub_idx = sub_idx;
+
+	/* Override base class methods. */
+	subdevice->base.me_subdevice_destructor = metempl_sub_destructor;
+	subdevice->base.me_subdevice_query_number_channels =
+	    metempl_sub_query_number_channels;
+	subdevice->base.me_subdevice_query_subdevice_type =
+	    metempl_sub_query_subdevice_type;
+	subdevice->base.me_subdevice_query_subdevice_caps =
+	    metempl_sub_query_subdevice_caps;
+
+	return subdevice;
+}
diff --git a/drivers/staging/meilhaus/metempl_sub.h b/drivers/staging/meilhaus/metempl_sub.h
new file mode 100644
index 00000000000..80c8af9a8c5
--- /dev/null
+++ b/drivers/staging/meilhaus/metempl_sub.h
@@ -0,0 +1,64 @@
+/**
+ * @file metempl_sub.h
+ *
+ * @brief Meilhaus subdevice class.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _METEMPL_SUB_H_
+#define _METEMPL_SUB_H_
+
+#include "mesubdevice.h"
+
+#ifdef __KERNEL__
+
+/**
+ * @brief The subdevice class.
+ */
+typedef struct metempl_sub_subdevice {
+	/* Inheritance */
+	me_subdevice_t base;			/**< The subdevice base class. */
+
+	/* Attributes */
+	spinlock_t subdevice_lock;		/**< Spin lock to protect the subdevice from concurrent access. */
+	spinlock_t *ctrl_reg_lock;		/**< Spin lock to protect #ctrl_reg from concurrent access. */
+	int sub_idx;				/**< The index of the subdevice on the device. */
+
+	unsigned long ctrl_reg;			/**< Register to configure the modes. */
+} metempl_sub_subdevice_t;
+
+/**
+ * @brief The constructor to generate a subdevice instance.
+ *
+ * @param reg_base The register base address of the device as returned by the PCI BIOS.
+ * @param sub_idx The index of the subdevice on the device.
+ * @param ctrl_reg_lock Pointer to spin lock protecting the control register from concurrent access.
+ *
+ * @return Pointer to new instance on success.\n
+ * NULL on error.
+ */
+metempl_sub_subdevice_t *metempl_sub_constructor(uint32_t reg_base,
+						 unsigned int sub_idx,
+						 spinlock_t * ctrl_reg_lock);
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/metempl_sub_reg.h b/drivers/staging/meilhaus/metempl_sub_reg.h
new file mode 100644
index 00000000000..1a2cab778a1
--- /dev/null
+++ b/drivers/staging/meilhaus/metempl_sub_reg.h
@@ -0,0 +1,35 @@
+/**
+ * @file metempl_sub_reg.h
+ *
+ * @brief Subdevice register definitions.
+ * @note Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ * @author Guenter Gebhardt
+ */
+
+/*
+ * Copyright (C) 2007 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#ifndef _METEMPL_SUB_REG_H_
+#define _METEMPL_SUB_REG_H_
+
+#ifdef __KERNEL__
+
+#define METEMPL_PORT_MODE			0x0010		/**< Configuration register. */
+
+#endif
+#endif
diff --git a/drivers/staging/meilhaus/metypes.h b/drivers/staging/meilhaus/metypes.h
new file mode 100644
index 00000000000..228ea15753e
--- /dev/null
+++ b/drivers/staging/meilhaus/metypes.h
@@ -0,0 +1,95 @@
+/*
+ * Copyright (C) 2005 Meilhaus Electronic GmbH (support@meilhaus.de)
+ *
+ * Source File : metypes.h
+ * Author      : GG (Guenter Gebhardt)  <g.gebhardt@meilhaus.de>
+ */
+
+#ifndef _METYPES_H_
+#define _METYPES_H_
+
+
+typedef int (*meErrorCB_t)(char *pcFunctionName, int iErrorCode);
+
+typedef int (*meIOStreamCB_t)(
+		int iDevice,
+		int iSubdevice,
+		int iCount,
+	   	void *pvContext,
+	   	int iErrorCode);
+
+typedef int (*meIOIrqCB_t)(
+		int iDevice,
+	   	int iSubdevice,
+	   	int iChannel,
+		int iIrqCount,
+	   	int iValue,
+	   	void *pvContext,
+	   	int iErrorCode);
+
+
+typedef struct meIOSingle {
+	int iDevice;
+	int iSubdevice;
+	int iChannel;
+	int iDir;
+	int iValue;
+	int iTimeOut;
+	int iFlags;
+	int iErrno;
+} meIOSingle_t;
+
+
+typedef struct meIOStreamConfig {
+	int iChannel;
+	int iStreamConfig;
+	int iRef;
+	int iFlags;
+} meIOStreamConfig_t;
+
+
+typedef struct meIOStreamTrigger {
+	int iAcqStartTrigType;
+	int iAcqStartTrigEdge;
+	int iAcqStartTrigChan;
+	int iAcqStartTicksLow;
+	int iAcqStartTicksHigh;
+	int iAcqStartArgs[10];
+	int iScanStartTrigType;
+	int iScanStartTicksLow;
+	int iScanStartTicksHigh;
+	int iScanStartArgs[10];
+	int iConvStartTrigType;
+	int iConvStartTicksLow;
+	int iConvStartTicksHigh;
+	int iConvStartArgs[10];
+	int iScanStopTrigType;
+	int iScanStopCount;
+	int iScanStopArgs[10];
+	int iAcqStopTrigType;
+	int iAcqStopCount;
+	int iAcqStopArgs[10];
+	int iFlags;
+} meIOStreamTrigger_t;
+
+
+typedef struct meIOStreamStart {
+	int iDevice;
+	int iSubdevice;
+	int iStartMode;
+	int iTimeOut;
+	int iFlags;
+	int iErrno;
+} meIOStreamStart_t;
+
+
+typedef struct meIOStreamStop {
+	int iDevice;
+	int iSubdevice;
+	int iStopMode;
+	int iFlags;
+	int iErrno;
+} meIOStreamStop_t;
+
+
+#endif
diff --git a/drivers/staging/mimio/Kconfig b/drivers/staging/mimio/Kconfig
new file mode 100644
index 00000000000..c0ba4c800df
--- /dev/null
+++ b/drivers/staging/mimio/Kconfig
@@ -0,0 +1,10 @@
+config INPUT_MIMIO
+	tristate "Mimio Xi interactive whiteboard support"
+	depends on USB
+	default N
+	help
+	  Say Y here if you want to use a Mimio Xi interactive
+	  whiteboard device.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called mimio.
diff --git a/drivers/staging/mimio/Makefile b/drivers/staging/mimio/Makefile
new file mode 100644
index 00000000000..77807ee0450
--- /dev/null
+++ b/drivers/staging/mimio/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_INPUT_MIMIO)	+= mimio.o
diff --git a/drivers/staging/mimio/mimio.c b/drivers/staging/mimio/mimio.c
new file mode 100644
index 00000000000..1ba8103f500
--- /dev/null
+++ b/drivers/staging/mimio/mimio.c
@@ -0,0 +1,914 @@
+/*
+ * Hardware event => input event mapping:
+ *
+ *
+ *
+ input.h:#define BTN_TOOL_PEN            0x140 black
+ input.h:#define BTN_TOOL_RUBBER         0x141 blue
+ input.h:#define BTN_TOOL_BRUSH          0x142 green
+ input.h:#define BTN_TOOL_PENCIL         0x143 red
+ input.h:#define BTN_TOOL_AIRBRUSH       0x144 eraser
+ input.h:#define BTN_TOOL_FINGER         0x145 small eraser
+ input.h:#define BTN_TOOL_MOUSE          0x146 mimio interactive
+ input.h:#define BTN_TOOL_LENS           0x147 mimio interactive but1
+ input.h:#define LOCALBTN_TOOL_EXTRA1    0x14a mimio interactive but2 == BTN_TOUCH
+ input.h:#define LOCALBTN_TOOL_EXTRA2    0x14b mimio extra pens (orange, brown, yellow, purple) == BTN_STYLUS
+ input.h:#define LOCALBTN_TOOL_EXTRA3    0x14c unused == BTN_STYLUS2
+ input.h:#define BTN_TOOL_DOUBLETAP      0x14d unused
+ input.h:#define BTN_TOOL_TRIPLETAP      0x14e unused
+ *
+ * MIMIO_EV_PENDOWN(MIMIO_PEN_K)     => EV_KEY BIT(BTN_TOOL_PEN)
+ * MIMIO_EV_PENDOWN(MIMIO_PEN_B)     => EV_KEY BIT(BTN_TOOL_RUBBER)
+ * MIMIO_EV_PENDOWN(MIMIO_PEN_G)     => EV_KEY BIT(BTN_TOOL_BRUSH)
+ * MIMIO_EV_PENDOWN(MIMIO_PEN_R)     => EV_KEY BIT(BTN_TOOL_PENCIL)
+ * MIMIO_EV_PENDOWN(MIMIO_PEN_E)     => EV_KEY BIT(BTN_TOOL_AIRBRUSH)
+ * MIMIO_EV_PENDOWN(MIMIO_PEN_ES)    => EV_KEY BIT(BTN_TOOL_FINGER)
+ * MIMIO_EV_PENDOWN(MIMIO_PEN_I)     => EV_KEY BIT(BTN_TOOL_MOUSE)
+ * MIMIO_EV_PENDOWN(MIMIO_PEN_IL)    => EV_KEY BIT(BTN_TOOL_LENS)
+ * MIMIO_EV_PENDOWN(MIMIO_PEN_IR)    => EV_KEY BIT(BTN_TOOL_DOUBLETAP)
+ * MIMIO_EV_PENDOWN(MIMIO_PEN_EX)    => EV_KEY BIT(BTN_TOOL_TRIPLETAP)
+ * MIMIO_EV_PENDATA                 => EV_ABS BIT(ABS_X), BIT(ABS_Y)
+ * MIMIO_EV_MEMRESET              => EV_KEY BIT(BTN_0)
+ * MIMIO_EV_ACC(ACC_NEWPAGE)       => EV_KEY BIT(BTN_1)
+ * MIMIO_EV_ACC(ACC_TAGPAGE)      => EV_KEY BIT(BTN_2)
+ * MIMIO_EV_ACC(ACC_PRINTPAGE)      => EV_KEY BIT(BTN_3)
+ * MIMIO_EV_ACC(ACC_MAXIMIZE)      => EV_KEY BIT(BTN_4)
+ * MIMIO_EV_ACC(ACC_FINDCTLPNL)      => EV_KEY BIT(BTN_5)
+ *
+ *
+ * open issues:
+ *      - cold-load of data captured when mimio in standalone mode not yet
+ *         supported; need to snoop Win32 box to see datastream for this.
+ *       - mimio mouse not yet supported; need to snoop Win32 box to see the
+ *         datastream for this.
+ */
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/input.h>
+#include <linux/usb.h>
+
+#define DRIVER_VERSION		"v0.031"
+#define DRIVER_AUTHOR		"mwilder@cs.nmsu.edu"
+#define DRIVER_DESC		"USB mimio-xi driver"
+
+enum {UPVALUE, DOWNVALUE, MOVEVALUE};
+
+#define MIMIO_XRANGE_MAX	9600
+#define MIMIO_YRANGE_MAX	4800
+
+#define LOCALBTN_TOOL_EXTRA1	BTN_TOUCH
+#define LOCALBTN_TOOL_EXTRA2	BTN_STYLUS
+#define LOCALBTN_TOOL_EXTRA3	BTN_STYLUS2
+
+#define MIMIO_VENDOR_ID		0x08d3
+#define MIMIO_PRODUCT_ID	0x0001
+#define MIMIO_MAXPAYLOAD	(8)
+#define MIMIO_MAXNAMELEN	(64)
+#define MIMIO_TXWAIT		(1)
+#define MIMIO_TXDONE		(2)
+
+#define MIMIO_EV_PENDOWN	(0x22)
+#define MIMIO_EV_PENDATA	(0x24)
+#define MIMIO_EV_PENUP		(0x51)
+#define MIMIO_EV_MEMRESET	(0x45)
+#define MIMIO_EV_ACC		(0xb2)
+
+#define MIMIO_PEN_K		(1)	/* black pen */
+#define MIMIO_PEN_B		(2)	/* blue pen */
+#define MIMIO_PEN_G		(3)	/* green pen */
+#define MIMIO_PEN_R		(4)	/* red pen */
+/* 5, 6, 7, 8 are extra pens */
+#define MIMIO_PEN_E		(9)	/* big eraser */
+#define MIMIO_PEN_ES		(10)	/* lil eraser */
+#define MIMIO_PENJUMP_START	(10)
+#define MIMIO_PENJUMP		(6)
+#define MIMIO_PEN_I		(17)	/* mimio interactive */
+#define MIMIO_PEN_IL		(18)	/* mimio interactive button 1 */
+#define MIMIO_PEN_IR		(19)	/* mimio interactive button 2 */
+
+#define MIMIO_PEN_MAX		(MIMIO_PEN_IR)
+
+#define ACC_DONE		(0)
+#define ACC_NEWPAGE		(1)
+#define ACC_TAGPAGE		(2)
+#define ACC_PRINTPAGE		(4)
+#define ACC_MAXIMIZE		(8)
+#define ACC_FINDCTLPNL		(16)
+
+#define isvalidtxsize(n)	((n) > 0 && (n) <= MIMIO_MAXPAYLOAD)
+
+
+struct pktbuf {
+	unsigned char instr;
+	unsigned char buf[16];
+	unsigned char *p;
+	unsigned char *q;
+};
+
+struct usbintendpt {
+	dma_addr_t dma;
+	struct urb *urb;
+	unsigned char *buf;
+	struct usb_endpoint_descriptor *desc;
+};
+
+struct mimio {
+	struct input_dev *idev;
+	struct usb_device *udev;
+	struct usb_interface *uifc;
+	int open;
+	int present;
+	int greeted;
+	int txflags;
+	char phys[MIMIO_MAXNAMELEN];
+	struct usbintendpt in;
+	struct usbintendpt out;
+	struct pktbuf pktbuf;
+	unsigned char minor;
+	wait_queue_head_t waitq;
+	spinlock_t txlock;
+	void (*rxhandler)(struct mimio *, unsigned char *, unsigned int);
+	int last_pen_down;
+};
+
+static void mimio_close(struct input_dev *);
+static void mimio_dealloc(struct mimio *);
+static void mimio_disconnect(struct usb_interface *);
+static int mimio_greet(struct mimio *);
+static void mimio_irq_in(struct urb *);
+static void mimio_irq_out(struct urb *);
+static int mimio_open(struct input_dev *);
+static int mimio_probe(struct usb_interface *, const struct usb_device_id *);
+static void mimio_rx_handler(struct mimio *, unsigned char *, unsigned int);
+static int mimio_tx(struct mimio *, const char *, int);
+
+static char mimio_name[] = "VirtualInk mimio-Xi";
+static struct usb_device_id mimio_table [] = {
+	{ USB_DEVICE(MIMIO_VENDOR_ID, MIMIO_PRODUCT_ID) },
+	{ USB_DEVICE(0x0525, 0xa4a0) }, /* gadget zero firmware */
+	{ }
+};
+
+MODULE_DEVICE_TABLE(usb, mimio_table);
+
+static struct usb_driver mimio_driver = {
+	.name = "mimio",
+	.probe = mimio_probe,
+	.disconnect = mimio_disconnect,
+	.id_table = mimio_table,
+};
+
+static DECLARE_MUTEX(disconnect_sem);
+
+static void mimio_close(struct input_dev *idev)
+{
+	struct mimio *mimio;
+
+	mimio = input_get_drvdata(idev);
+	if (!mimio) {
+		dev_err(&idev->dev, "null mimio attached to input device\n");
+		return;
+	}
+
+	if (mimio->open <= 0)
+		dev_err(&idev->dev, "mimio not open.\n");
+	else
+		mimio->open--;
+
+	if (mimio->present == 0 && mimio->open == 0)
+		mimio_dealloc(mimio);
+}
+
+static void mimio_dealloc(struct mimio *mimio)
+{
+	if (mimio == NULL)
+		return;
+
+	usb_kill_urb(mimio->in.urb);
+
+	usb_kill_urb(mimio->out.urb);
+
+	if (mimio->idev) {
+		input_unregister_device(mimio->idev);
+		if (mimio->idev->grab)
+			input_close_device(mimio->idev->grab);
+		else
+			dev_dbg(&mimio->idev->dev, "mimio->idev->grab == NULL"
+				" -- didn't call input_close_device\n");
+	}
+
+	usb_free_urb(mimio->in.urb);
+
+	usb_free_urb(mimio->out.urb);
+
+	if (mimio->in.buf) {
+		usb_buffer_free(mimio->udev, MIMIO_MAXPAYLOAD, mimio->in.buf,
+				mimio->in.dma);
+	}
+
+	if (mimio->out.buf)
+		usb_buffer_free(mimio->udev, MIMIO_MAXPAYLOAD, mimio->out.buf,
+				mimio->out.dma);
+
+	if (mimio->idev)
+		input_free_device(mimio->idev);
+
+	kfree(mimio);
+}
+
+static void mimio_disconnect(struct usb_interface *ifc)
+{
+	struct mimio *mimio;
+
+	down(&disconnect_sem);
+
+	mimio = usb_get_intfdata(ifc);
+	usb_set_intfdata(ifc, NULL);
+	dev_dbg(&mimio->idev->dev, "disconnect\n");
+
+	if (mimio) {
+		mimio->present = 0;
+
+		if (mimio->open <= 0)
+			mimio_dealloc(mimio);
+	}
+
+	up(&disconnect_sem);
+}
+
+static int mimio_greet(struct mimio *mimio)
+{
+	const struct grtpkt {
+		int nbytes;
+		unsigned delay;
+		char data[8];
+	} grtpkts[] = {
+		{ 3, 0, { 0x11, 0x55, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00 } },
+		{ 5, 0, { 0x53, 0x55, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00 } },
+		{ 5, 0, { 0x43, 0x55, 0x00, 0x00, 0x16, 0x00, 0x00, 0x00 } },
+		{ 5, 0, { 0x33, 0x55, 0x00, 0x00, 0x66, 0x00, 0x00, 0x00 } },
+		{ 5, 0, { 0x13, 0x00, 0x5e, 0x02, 0x4f, 0x00, 0x00, 0x00 } },
+		{ 5, 0, { 0x13, 0x00, 0x04, 0x03, 0x14, 0x00, 0x00, 0x00 } },
+		{ 5, 2, { 0x13, 0x00, 0x00, 0x04, 0x17, 0x00, 0x00, 0x00 } },
+		{ 5, 0, { 0x13, 0x00, 0x0d, 0x08, 0x16, 0x00, 0x00, 0x00 } },
+		{ 5, 0, { 0x13, 0x00, 0x4d, 0x01, 0x5f, 0x00, 0x00, 0x00 } },
+		{ 3, 0, { 0xf1, 0x55, 0xa4, 0x00, 0x00, 0x00, 0x00, 0x00 } },
+		{ 7, 2, { 0x52, 0x55, 0x00, 0x07, 0x31, 0x55, 0x64, 0x00 } },
+		{ 0, 0, { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 } },
+	};
+	int rslt;
+	const struct grtpkt *pkt;
+
+	for (pkt = grtpkts; pkt->nbytes; pkt++) {
+		rslt = mimio_tx(mimio, pkt->data, pkt->nbytes);
+		if (rslt)
+			return rslt;
+		if (pkt->delay)
+			msleep(pkt->delay);
+	}
+
+	return 0;
+}
+
+static void mimio_irq_in(struct urb *urb)
+{
+	int rslt;
+	char *data;
+	const char *reason = "going down";
+	struct mimio *mimio;
+
+	mimio = urb->context;
+
+	if (mimio == NULL)
+		/* paranoia */
+		return;
+
+	switch (urb->status) {
+	case 0:
+		/* success */
+		break;
+	case -ETIMEDOUT:
+		reason = "timeout -- unplugged?";
+	case -ECONNRESET:
+	case -ENOENT:
+	case -ESHUTDOWN:
+		dev_dbg(&mimio->idev->dev, "%s.\n", reason);
+		return;
+	default:
+		dev_dbg(&mimio->idev->dev, "unknown urb-status: %d.\n",
+			urb->status);
+		goto exit;
+	}
+	data = mimio->in.buf;
+
+	if (mimio->rxhandler)
+		mimio->rxhandler(mimio, data, urb->actual_length);
+exit:
+	/*
+	 * Keep listening to device on same urb.
+	 */
+	rslt = usb_submit_urb(urb, GFP_ATOMIC);
+	if (rslt)
+		dev_err(&mimio->idev->dev, "usb_submit_urb failure: %d.\n",
+			rslt);
+}
+
+static void mimio_irq_out(struct urb *urb)
+{
+	unsigned long flags;
+	struct mimio *mimio;
+
+	mimio = urb->context;
+
+	if (urb->status)
+		dev_dbg(&mimio->idev->dev, "urb-status: %d.\n", urb->status);
+
+	spin_lock_irqsave(&mimio->txlock, flags);
+	mimio->txflags |= MIMIO_TXDONE;
+	spin_unlock_irqrestore(&mimio->txlock, flags);
+	wmb();
+	wake_up(&mimio->waitq);
+}
+
+static int mimio_open(struct input_dev *idev)
+{
+	int rslt;
+	struct mimio *mimio;
+
+	rslt = 0;
+	down(&disconnect_sem);
+	mimio = input_get_drvdata(idev);
+	dev_dbg(&idev->dev, "mimio_open\n");
+
+	if (mimio == NULL) {
+		dev_err(&idev->dev, "null mimio.\n");
+		rslt = -ENODEV;
+		goto exit;
+	}
+
+	if (mimio->open++)
+		goto exit;
+
+	if (mimio->present && !mimio->greeted) {
+		struct urb *urb = mimio->in.urb;
+		mimio->in.urb->dev = mimio->udev;
+		rslt = usb_submit_urb(mimio->in.urb, GFP_KERNEL);
+		if (rslt) {
+			dev_err(&idev->dev, "usb_submit_urb failure "
+				"(res = %d: %s). Not greeting.\n",
+				rslt,
+				(!urb ? "urb is NULL" :
+				 (urb->hcpriv ? "urb->hcpriv is non-NULL" :
+				  (!urb->complete ? "urb is not complete" :
+				   (urb->number_of_packets <= 0 ? "urb has no packets" :
+				    (urb->interval <= 0 ? "urb interval too small" :
+				     "urb interval too large or some other error"))))));
+			rslt = -EIO;
+			goto exit;
+		}
+		rslt = mimio_greet(mimio);
+		if (rslt == 0) {
+			dev_dbg(&idev->dev, "Mimio greeted OK.\n");
+			mimio->greeted = 1;
+		} else {
+			dev_dbg(&idev->dev, "Mimio greet Failure (%d)\n",
+				rslt);
+		}
+	}
+
+exit:
+	up(&disconnect_sem);
+	return rslt;
+}
+
+static int mimio_probe(struct usb_interface *ifc,
+		       const struct usb_device_id *id)
+{
+	char path[64];
+	int pipe, maxp;
+	struct mimio *mimio;
+	struct usb_device *udev;
+	struct usb_host_interface *hostifc;
+	struct input_dev *input_dev;
+	int res = 0;
+	int i;
+
+	udev = interface_to_usbdev(ifc);
+
+	mimio = kzalloc(sizeof(struct mimio), GFP_KERNEL);
+	if (!mimio)
+		return -ENOMEM;
+
+	input_dev = input_allocate_device();
+	if (!input_dev) {
+		mimio_dealloc(mimio);
+		return -ENOMEM;
+	}
+
+	mimio->uifc = ifc;
+	mimio->udev = udev;
+	mimio->pktbuf.p = mimio->pktbuf.buf;
+	mimio->pktbuf.q = mimio->pktbuf.buf;
+	/* init_input_dev(mimio->idev); */
+	mimio->idev = input_dev;
+	init_waitqueue_head(&mimio->waitq);
+	spin_lock_init(&mimio->txlock);
+	hostifc = ifc->cur_altsetting;
+
+	if (hostifc->desc.bNumEndpoints != 2) {
+		dev_err(&udev->dev, "Unexpected endpoint count: %d.\n",
+			hostifc->desc.bNumEndpoints);
+		mimio_dealloc(mimio);
+		return -ENODEV;
+	}
+
+	mimio->in.desc = &(hostifc->endpoint[0].desc);
+	mimio->out.desc = &(hostifc->endpoint[1].desc);
+
+	mimio->in.buf = usb_buffer_alloc(udev, MIMIO_MAXPAYLOAD, GFP_KERNEL,
+					 &mimio->in.dma);
+	mimio->out.buf = usb_buffer_alloc(udev, MIMIO_MAXPAYLOAD, GFP_KERNEL,
+					  &mimio->out.dma);
+
+	if (mimio->in.buf == NULL || mimio->out.buf == NULL) {
+		dev_err(&udev->dev, "usb_buffer_alloc failure.\n");
+		mimio_dealloc(mimio);
+		return -ENOMEM;
+	}
+
+	mimio->in.urb = usb_alloc_urb(0, GFP_KERNEL);
+	mimio->out.urb = usb_alloc_urb(0, GFP_KERNEL);
+
+	if (mimio->in.urb == NULL || mimio->out.urb == NULL) {
+		dev_err(&udev->dev, "usb_alloc_urb failure.\n");
+		mimio_dealloc(mimio);
+		return -ENOMEM;
+	}
+
+	/*
+	 * Build the input urb.
+	 */
+	pipe = usb_rcvintpipe(udev, mimio->in.desc->bEndpointAddress);
+	maxp = usb_maxpacket(udev, pipe, usb_pipeout(pipe));
+	if (maxp > MIMIO_MAXPAYLOAD)
+		maxp = MIMIO_MAXPAYLOAD;
+	usb_fill_int_urb(mimio->in.urb, udev, pipe, mimio->in.buf, maxp,
+			 mimio_irq_in, mimio, mimio->in.desc->bInterval);
+	mimio->in.urb->transfer_dma = mimio->in.dma;
+	mimio->in.urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
+
+	/*
+	 * Build the output urb.
+	 */
+	pipe = usb_sndintpipe(udev, mimio->out.desc->bEndpointAddress);
+	maxp = usb_maxpacket(udev, pipe, usb_pipeout(pipe));
+	if (maxp > MIMIO_MAXPAYLOAD)
+		maxp = MIMIO_MAXPAYLOAD;
+	usb_fill_int_urb(mimio->out.urb, udev, pipe, mimio->out.buf, maxp,
+			 mimio_irq_out, mimio, mimio->out.desc->bInterval);
+	mimio->out.urb->transfer_dma = mimio->out.dma;
+	mimio->out.urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
+
+	/*
+	 * Build input device info
+	 */
+	usb_make_path(udev, path, 64);
+	snprintf(mimio->phys, MIMIO_MAXNAMELEN, "%s/input0", path);
+	input_set_drvdata(input_dev, mimio);
+	/* input_dev->dev = &ifc->dev; */
+	input_dev->open = mimio_open;
+	input_dev->close = mimio_close;
+	input_dev->name = mimio_name;
+	input_dev->phys = mimio->phys;
+	input_dev->dev.parent = &ifc->dev;
+
+	input_dev->id.bustype = BUS_USB;
+	input_dev->id.vendor = le16_to_cpu(udev->descriptor.idVendor);
+	input_dev->id.product = le16_to_cpu(udev->descriptor.idProduct);
+	input_dev->id.version = le16_to_cpu(udev->descriptor.bcdDevice);
+
+	input_dev->evbit[0] |= BIT(EV_KEY) | BIT(EV_ABS);
+	for (i = BTN_TOOL_PEN; i <= LOCALBTN_TOOL_EXTRA2; ++i)
+		set_bit(i, input_dev->keybit);
+
+	input_dev->keybit[BIT_WORD(BTN_MISC)] |= BIT_MASK(BTN_0) |
+						 BIT_MASK(BTN_1) |
+						 BIT_MASK(BTN_2) |
+						 BIT_MASK(BTN_3) |
+						 BIT_MASK(BTN_4) |
+						 BIT_MASK(BTN_5);
+	/*   input_dev->keybit[BTN_MOUSE] |= BIT(BTN_LEFT); */
+	input_dev->absbit[0] |= BIT_MASK(ABS_X) | BIT_MASK(ABS_Y);
+	input_set_abs_params(input_dev, ABS_X, 0, MIMIO_XRANGE_MAX, 0, 0);
+	input_set_abs_params(input_dev, ABS_Y, 0, MIMIO_YRANGE_MAX, 0, 0);
+	input_dev->absbit[BIT_WORD(ABS_MISC)] |= BIT_MASK(ABS_MISC);
+
+#if 0
+	input_dev->absmin[ABS_X] = 0;
+	input_dev->absmin[ABS_Y] = 0;
+	input_dev->absmax[ABS_X] = 9600;
+	input_dev->absmax[ABS_Y] = 4800;
+	input_dev->absfuzz[ABS_X] = 0;
+	input_dev->absfuzz[ABS_Y] = 0;
+	input_dev->absflat[ABS_X] = 0;
+	input_dev->absflat[ABS_Y] = 0;
+#endif
+
+#if 0
+	/* this will just reduce the precision */
+	input_dev->absfuzz[ABS_X] = 8; /* experimental; may need to change */
+	input_dev->absfuzz[ABS_Y] = 8; /* experimental; may need to change */
+#endif
+
+	/*
+	 * Register the input device.
+	 */
+	res = input_register_device(mimio->idev);
+	if (res) {
+		dev_err(&udev->dev, "input_register_device failure (%d)\n",
+			res);
+		mimio_dealloc(mimio);
+		return -EIO;
+	}
+	dev_dbg(&mimio->idev->dev, "input: %s on %s (res = %d).\n",
+		input_dev->name, input_dev->phys, res);
+
+	usb_set_intfdata(ifc, mimio);
+	mimio->present = 1;
+
+	/*
+	 * Submit the input urb to the usb subsystem.
+	 */
+	mimio->in.urb->dev = mimio->udev;
+	res = usb_submit_urb(mimio->in.urb, GFP_KERNEL);
+	if (res) {
+		dev_err(&mimio->idev->dev, "usb_submit_urb failure (%d)\n",
+			res);
+		mimio_dealloc(mimio);
+		return -EIO;
+	}
+
+	/*
+	 * Attempt to greet the mimio after giving
+	 * it some post-init settling time.
+	 *
+	 * note: sometimes this sleep interval isn't
+	 * long enough to permit the device to re-init
+	 * after a hot-swap; maybe need to bump it up.
+	 *
+	 * As it is, this probably breaks module unloading support!
+	 */
+	msleep(1024);
+
+	res = mimio_greet(mimio);
+	if (res == 0) {
+		dev_dbg(&mimio->idev->dev, "Mimio greeted OK.\n");
+		mimio->greeted = 1;
+		mimio->rxhandler = mimio_rx_handler;
+	} else {
+		dev_dbg(&mimio->idev->dev, "Mimio greet Failure (%d)\n", res);
+	}
+
+	return 0;
+}
+
+static int handle_mimio_rx_penupdown(struct mimio *mimio,
+				     int down,
+				     const char *const instr[],
+				     const int instr_ofst[])
+{
+	int penid, x;
+	if (mimio->pktbuf.q - mimio->pktbuf.p < (down ? 4 : 3))
+		return 1; 		/* partial pkt */
+
+	if (down) {
+		x = *mimio->pktbuf.p ^ *(mimio->pktbuf.p + 1) ^
+			*(mimio->pktbuf.p + 2);
+		if (x != *(mimio->pktbuf.p + 3)) {
+			dev_dbg(&mimio->idev->dev, "EV_PEN%s: bad xsum.\n",
+				down ? "DOWN":"UP");
+			/* skip this event data */
+			mimio->pktbuf.p += 4;
+			/* decode any remaining events */
+			return 0;
+		}
+		penid = mimio->pktbuf.instr = *(mimio->pktbuf.p + 2);
+		if (penid > MIMIO_PEN_MAX) {
+			dev_dbg(&mimio->idev->dev,
+				"Unmapped penID (not in [0, %d]): %d\n",
+				MIMIO_PEN_MAX, (int)mimio->pktbuf.instr);
+			penid = mimio->pktbuf.instr = 0;
+		}
+		mimio->last_pen_down = penid;
+	} else {
+		penid = mimio->last_pen_down;
+	}
+	dev_dbg(&mimio->idev->dev, "%s (id %d, code %d) %s.\n", instr[penid],
+		instr_ofst[penid], penid, down ? "down" : "up");
+
+	if (instr_ofst[penid] >= 0) {
+		int code = BTN_TOOL_PEN + instr_ofst[penid];
+		int value = down ? DOWNVALUE : UPVALUE;
+		if (code > KEY_MAX)
+			dev_dbg(&mimio->idev->dev, "input_event will ignore "
+				"-- code (%d) > KEY_MAX\n", code);
+		if (!test_bit(code, mimio->idev->keybit))
+			dev_dbg(&mimio->idev->dev, "input_event will ignore "
+				"-- bit for code (%d) not enabled\n", code);
+		if (!!test_bit(code, mimio->idev->key) == value)
+			dev_dbg(&mimio->idev->dev, "input_event will ignore "
+				"-- bit for code (%d) already set to %d\n",
+				code, value);
+		if (value != DOWNVALUE) {
+			/* input_regs(mimio->idev, regs); */
+			input_report_key(mimio->idev, code, value);
+			input_sync(mimio->idev);
+		} else {
+			/* wait until we get some coordinates */
+		}
+	} else {
+		dev_dbg(&mimio->idev->dev, "penID offset[%d] == %d is < 0 "
+			"- not sending\n", penid, instr_ofst[penid]);
+	}
+	mimio->pktbuf.p += down ? 4 : 3; /* 3 for up, 4 for down */
+	return 0;
+}
+
+/*
+ * Stay tuned for partial-packet excitement.
+ *
+ * This routine buffers data packets received from the mimio device
+ * in the mimio's data space.  This buffering is necessary because
+ * the mimio's in endpoint can serve us partial packets of data, and
+ * we want the driver to support the servicing of multiple mimios.
+ * Empirical evidence gathered so far suggests that the method of
+ * buffering packet data in the mimio's data space works.  Previous
+ * versions of this driver did not buffer packet data in each mimio's
+ * data-space, and were therefore not able to service multiple mimios.
+ * Note that since the caller of this routine is running in interrupt
+ * context, care needs to be taken to ensure that this routine does not
+ * become bloated, and it may be that another spinlock is needed in each
+ * mimio to guard the buffered packet data properly.
+ */
+static void mimio_rx_handler(struct mimio *mimio,
+			     unsigned char *data,
+			     unsigned int nbytes)
+{
+	struct device *dev = &mimio->idev->dev;
+	unsigned int x;
+	unsigned int y;
+	static const char * const instr[] = {
+		"?0",
+		"black pen", "blue pen", "green pen", "red pen",
+		"brown pen", "orange pen", "purple pen", "yellow pen",
+		"big eraser", "lil eraser",
+		"?11", "?12", "?13", "?14", "?15", "?16",
+		"mimio interactive", "interactive button1",
+		"interactive button2"
+	};
+
+	/* Mimio Interactive gives:
+	 * down: [0x22 0x01 0x11 0x32 0x24]
+	 * b1  : [0x22 0x01 0x12 0x31 0x24]
+	 * b2  : [0x22 0x01 0x13 0x30 0x24]
+	 */
+	static const int instr_ofst[] = {
+		-1,
+		0, 1, 2, 3,
+		9, 9, 9, 9,
+		4, 5,
+		-1, -1, -1, -1, -1, -1,
+		6, 7, 8,
+	};
+
+	memcpy(mimio->pktbuf.q, data, nbytes);
+	mimio->pktbuf.q += nbytes;
+
+	while (mimio->pktbuf.p < mimio->pktbuf.q) {
+		int t = *mimio->pktbuf.p;
+		switch (t) {
+		case MIMIO_EV_PENUP:
+		case MIMIO_EV_PENDOWN:
+			if (handle_mimio_rx_penupdown(mimio,
+						      t == MIMIO_EV_PENDOWN,
+						      instr, instr_ofst))
+				return; /* partial packet */
+			break;
+
+		case MIMIO_EV_PENDATA:
+			if (mimio->pktbuf.q - mimio->pktbuf.p < 6)
+				/* partial pkt */
+				return;
+			x = *mimio->pktbuf.p ^ *(mimio->pktbuf.p + 1) ^
+				*(mimio->pktbuf.p + 2) ^
+				*(mimio->pktbuf.p + 3) ^
+				*(mimio->pktbuf.p + 4);
+			if (x != *(mimio->pktbuf.p + 5)) {
+				dev_dbg(dev, "EV_PENDATA: bad xsum.\n");
+				mimio->pktbuf.p += 6; /* skip this event data */
+				break; /* decode any remaining events */
+			}
+			x = *(mimio->pktbuf.p + 1);
+			x <<= 8;
+			x |= *(mimio->pktbuf.p + 2);
+			y = *(mimio->pktbuf.p + 3);
+			y <<= 8;
+			y |= *(mimio->pktbuf.p + 4);
+			dev_dbg(dev, "coord: (%d, %d)\n", x, y);
+			if (instr_ofst[mimio->pktbuf.instr] >= 0) {
+				int code = BTN_TOOL_PEN +
+					   instr_ofst[mimio->last_pen_down];
+#if 0
+				/* Utter hack to ensure we get forwarded _AND_
+				 * so we can identify when a complete signal is
+				 * received */
+				mimio->idev->abs[ABS_Y] = -1;
+				mimio->idev->abs[ABS_X] = -1;
+#endif
+				/* input_regs(mimio->idev, regs); */
+				input_report_abs(mimio->idev, ABS_X, x);
+				input_report_abs(mimio->idev, ABS_Y, y);
+				/* fake a penup */
+				change_bit(code, mimio->idev->key);
+				input_report_key(mimio->idev,
+						 code,
+						 DOWNVALUE);
+				/* always sync here */
+				mimio->idev->sync = 0;
+				input_sync(mimio->idev);
+			}
+			mimio->pktbuf.p += 6;
+			break;
+		case MIMIO_EV_MEMRESET:
+			if (mimio->pktbuf.q - mimio->pktbuf.p < 7)
+				/* partial pkt */
+				return;
+			dev_dbg(dev, "mem-reset.\n");
+			/* input_regs(mimio->idev, regs); */
+			input_event(mimio->idev, EV_KEY, BTN_0, 1);
+			input_event(mimio->idev, EV_KEY, BTN_0, 0);
+			input_sync(mimio->idev);
+			mimio->pktbuf.p += 7;
+			break;
+		case MIMIO_EV_ACC:
+			if (mimio->pktbuf.q - mimio->pktbuf.p < 4)
+				/* partial pkt */
+				return;
+			x = *mimio->pktbuf.p ^ *(mimio->pktbuf.p + 1) ^
+				*(mimio->pktbuf.p + 2);
+			if (x != *(mimio->pktbuf.p + 3)) {
+				dev_dbg(dev, "EV_ACC: bad xsum.\n");
+				mimio->pktbuf.p += 4; /* skip this event data */
+				break; /* decode any remaining events */
+			}
+			switch (*(mimio->pktbuf.p + 2)) {
+			case ACC_NEWPAGE:
+				dev_dbg(&mimio->idev->dev, "new-page.\n");
+				/* input_regs(mimio->idev, regs); */
+				input_event(mimio->idev, EV_KEY, BTN_1, 1);
+				input_event(mimio->idev, EV_KEY, BTN_1, 0);
+				input_sync(mimio->idev);
+				break;
+			case ACC_TAGPAGE:
+				dev_dbg(&mimio->idev->dev, "tag-page.\n");
+				/* input_regs(mimio->idev, regs); */
+				input_event(mimio->idev, EV_KEY, BTN_2, 1);
+				input_event(mimio->idev, EV_KEY, BTN_2, 0);
+				input_sync(mimio->idev);
+				break;
+			case ACC_PRINTPAGE:
+				dev_dbg(&mimio->idev->dev, "print-page.\n");
+				/* input_regs(mimio->idev, regs);*/
+				input_event(mimio->idev, EV_KEY, BTN_3, 1);
+				input_event(mimio->idev, EV_KEY, BTN_3, 0);
+				input_sync(mimio->idev);
+				break;
+			case ACC_MAXIMIZE:
+				dev_dbg(&mimio->idev->dev,
+					"maximize-window.\n");
+				/* input_regs(mimio->idev, regs); */
+				input_event(mimio->idev, EV_KEY, BTN_4, 1);
+				input_event(mimio->idev, EV_KEY, BTN_4, 0);
+				input_sync(mimio->idev);
+				break;
+			case ACC_FINDCTLPNL:
+				dev_dbg(&mimio->idev->dev, "find-ctl-panel.\n");
+				/* input_regs(mimio->idev, regs); */
+				input_event(mimio->idev, EV_KEY, BTN_5, 1);
+				input_event(mimio->idev, EV_KEY, BTN_5, 0);
+				input_sync(mimio->idev);
+				break;
+			case ACC_DONE:
+				dev_dbg(&mimio->idev->dev, "acc-done.\n");
+				/* no event is dispatched to the input
+				 * subsystem for this device event.
+				 */
+				break;
+			default:
+				dev_dbg(dev, "unknown acc event.\n");
+				break;
+			}
+			mimio->pktbuf.p += 4;
+			break;
+		default:
+			mimio->pktbuf.p++;
+			break;
+		}
+	}
+
+	/*
+	 * No partial event was received, so reset mimio's pktbuf ptrs.
+	 */
+	mimio->pktbuf.p = mimio->pktbuf.q = mimio->pktbuf.buf;
+}
+
+static int mimio_tx(struct mimio *mimio, const char *buf, int nbytes)
+{
+	int rslt;
+	int timeout;
+	unsigned long flags;
+	DECLARE_WAITQUEUE(wait, current);
+
+	if (!(isvalidtxsize(nbytes))) {
+		dev_err(&mimio->idev->dev, "invalid arg: nbytes: %d.\n",
+			nbytes);
+		return -EINVAL;
+	}
+
+	/*
+	 * Init the out urb and copy the data to send.
+	 */
+	mimio->out.urb->dev = mimio->udev;
+	mimio->out.urb->transfer_buffer_length = nbytes;
+	memcpy(mimio->out.urb->transfer_buffer, buf, nbytes);
+
+	/*
+	 * Send the data.
+	 */
+	spin_lock_irqsave(&mimio->txlock, flags);
+	mimio->txflags = MIMIO_TXWAIT;
+	rslt = usb_submit_urb(mimio->out.urb, GFP_ATOMIC);
+	spin_unlock_irqrestore(&mimio->txlock, flags);
+	dev_dbg(&mimio->idev->dev, "rslt: %d.\n", rslt);
+
+	if (rslt) {
+		dev_err(&mimio->idev->dev, "usb_submit_urb failure: %d.\n",
+			rslt);
+		return rslt;
+	}
+
+	/*
+	 * Wait for completion to be signalled (the mimio_irq_out
+	 * completion routine will or MIMIO_TXDONE in with txflags).
+	 */
+	timeout = HZ;
+	set_current_state(TASK_INTERRUPTIBLE);
+	add_wait_queue(&mimio->waitq, &wait);
+
+	while (timeout && ((mimio->txflags & MIMIO_TXDONE) == 0)) {
+		timeout = schedule_timeout(timeout);
+		rmb();
+	}
+
+	if ((mimio->txflags & MIMIO_TXDONE) == 0)
+		dev_dbg(&mimio->idev->dev, "tx timed out.\n");
+
+	/*
+	 * Now that completion has been signalled,
+	 * unlink the urb so that it can be recycled.
+	 */
+	set_current_state(TASK_RUNNING);
+	remove_wait_queue(&mimio->waitq, &wait);
+	usb_unlink_urb(mimio->out.urb);
+
+	return rslt;
+}
+
+static int __init mimio_init(void)
+{
+	int rslt;
+
+	rslt = usb_register(&mimio_driver);
+	if (rslt != 0) {
+		err("%s: usb_register failure: %d", __func__, rslt);
+		return rslt;
+	}
+
+	printk(KERN_INFO KBUILD_MODNAME ":"
+	       DRIVER_DESC " " DRIVER_VERSION "\n");
+	return rslt;
+}
+
+static void __exit mimio_exit(void)
+{
+	usb_deregister(&mimio_driver);
+}
+
+module_init(mimio_init);
+module_exit(mimio_exit);
+
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/otus/80211core/amsdu.c b/drivers/staging/otus/80211core/amsdu.c
new file mode 100644
index 00000000000..c9123d58b82
--- /dev/null
+++ b/drivers/staging/otus/80211core/amsdu.c
@@ -0,0 +1,134 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include "cprecomp.h"
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfGetAmsduSubFrame          */
+/*      Get a subframe from a-MSDU.                                     */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : A-MSDU frame buffer                                       */
+/*      offset : offset of subframe in the A-MSDU                       */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      NULL or subframe                                                */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.2      */
+/*                                                                      */
+/************************************************************************/
+zbuf_t* zfGetAmsduSubFrame(zdev_t* dev, zbuf_t* buf, u16_t* offset)
+{
+    u16_t subframeLen;
+    u16_t amsduLen = zfwBufGetSize(dev, buf);
+    zbuf_t* newBuf;
+
+    ZM_PERFORMANCE_RX_AMSDU(dev, buf, amsduLen);
+
+    /* Verify A-MSDU length */
+    if (amsduLen < (*offset + 14))
+    {
+        return NULL;
+    }
+
+    /* Locate A-MSDU subframe by offset and verify subframe length */
+    subframeLen = (zmw_buf_readb(dev, buf, *offset + 12) << 8) +
+                  zmw_buf_readb(dev, buf, *offset + 13);
+    if (subframeLen == 0)
+    {
+        return NULL;
+    }
+
+    /* Verify A-MSDU subframe length */
+    if ((*offset+14+subframeLen) <= amsduLen)
+    {
+        /* Allocate a new buffer */
+        if ((newBuf = zfwBufAllocate(dev, 24+2+subframeLen)) != NULL)
+        {
+#ifdef ZM_ENABLE_NATIVE_WIFI
+            /* Copy and convert subframe to wlan frame format */
+            /* SHALL NOT INCLUDE QOS and AMSDU header. Ray 20070807 For Vista */
+            zfRxBufferCopy(dev, newBuf, buf, 0, 0, 24);
+            zfRxBufferCopy(dev, newBuf, buf, 24, *offset+14, subframeLen);
+            zfwBufSetSize(dev, newBuf, 24+subframeLen);
+#else
+            /* Copy subframe to new buffer */
+            zfRxBufferCopy(dev, newBuf, buf, 0, *offset, 14+subframeLen);
+            zfwBufSetSize(dev, newBuf, 14+subframeLen);
+#endif
+            /* Update offset */
+            *offset += (((14+subframeLen)+3) & 0xfffc);
+
+            /* Return buffer pointer */
+            return newBuf;
+        }
+    }
+    return NULL;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfDeAmsdu                   */
+/*      De-AMSDU.                                                       */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : A-MSDU frame buffer                                       */
+/*      vap : VAP port                                                  */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.2      */
+/*                                                                      */
+/************************************************************************/
+void zfDeAmsdu(zdev_t* dev, zbuf_t* buf, u16_t vap, u8_t encryMode)
+{
+    u16_t offset = ZM_SIZE_OF_WLAN_DATA_HEADER+ZM_SIZE_OF_QOS_CTRL;
+    zbuf_t* subframeBuf;
+    zmw_get_wlan_dev(dev);
+
+    ZM_BUFFER_TRACE(dev, buf)
+
+    if (encryMode == ZM_AES || encryMode == ZM_TKIP)
+    {
+        offset += (ZM_SIZE_OF_IV + ZM_SIZE_OF_EXT_IV);
+    }
+    else if (encryMode == ZM_WEP64 || encryMode == ZM_WEP128)
+    {
+        offset += ZM_SIZE_OF_IV;
+    }
+
+    /* Repeatly calling zfGetAmsduSubFrame() until NULL returned */
+    while ((subframeBuf = zfGetAmsduSubFrame(dev, buf, &offset)) != NULL)
+    {
+        wd->commTally.NotifyNDISRxFrmCnt++;
+        if (wd->zfcbRecvEth != NULL)
+    	{
+            wd->zfcbRecvEth(dev, subframeBuf, (u8_t)vap);
+            ZM_PERFORMANCE_RX_MSDU(dev, wd->tick);
+        }
+    }
+    zfwBufFree(dev, buf, 0);
+
+    return;
+}
diff --git a/drivers/staging/otus/80211core/cagg.c b/drivers/staging/otus/80211core/cagg.c
new file mode 100644
index 00000000000..fcfd01a6b36
--- /dev/null
+++ b/drivers/staging/otus/80211core/cagg.c
@@ -0,0 +1,3611 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : cagg.c                                                */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains A-MPDU aggregation related functions.      */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+
+#include "cprecomp.h"
+
+extern u8_t zcUpToAc[8];
+const u8_t pri[] = {3,3,2,3,2,1,3,2,1,0};
+
+
+u16_t aggr_count;
+u32_t success_mpdu;
+u32_t total_mpdu;
+
+void zfAggInit(zdev_t* dev)
+{
+    u16_t i,j;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+    /*
+     * reset sta information
+     */
+
+    zmw_enter_critical_section(dev);
+    wd->aggInitiated = 0;
+    wd->addbaComplete = 0;
+    wd->addbaCount = 0;
+    wd->reorder = 1;
+    for (i=0; i<ZM_MAX_STA_SUPPORT; i++)
+    {
+        for (j=0; j<ZM_AC; j++)
+        {
+            //wd->aggSta[i].aggQNumber[j] = ZM_AGG_POOL_SIZE;
+            wd->aggSta[i].aggFlag[j] = wd->aggSta[i].count[j] = 0;
+            wd->aggSta[i].tid_tx[j] = NULL;
+            wd->aggSta[i].tid_tx[j+1] = NULL;
+
+        }
+    }
+
+    /*
+     * reset Tx/Rx aggregation queue information
+     */
+    wd->aggState = 0;
+    for (i=0; i<ZM_AGG_POOL_SIZE; i++)
+    {
+        /*
+         * reset tx aggregation queue
+         */
+        wd->aggQPool[i] = zfwMemAllocate(dev, sizeof(struct aggQueue));
+        if(!wd->aggQPool[i])
+        {
+            zmw_leave_critical_section(dev);
+            return;
+        }
+        wd->aggQPool[i]->aggHead = wd->aggQPool[i]->aggTail =
+        wd->aggQPool[i]->aggQEnabled = wd->aggQPool[i]->aggReady =
+        wd->aggQPool[i]->clearFlag = wd->aggQPool[i]->deleteFlag = 0;
+        //wd->aggQPool[i]->aggSize = 16;
+
+        /*
+         * reset rx aggregation queue
+         */
+        wd->tid_rx[i] = zfwMemAllocate(dev, sizeof(struct agg_tid_rx));
+        if (!wd->tid_rx[i])
+        {
+            zmw_leave_critical_section(dev);
+            return;
+        }
+        wd->tid_rx[i]->aid = ZM_MAX_STA_SUPPORT;
+        wd->tid_rx[i]->seq_start = wd->tid_rx[i]->baw_head = \
+        wd->tid_rx[i]->baw_tail = 0;
+        wd->tid_rx[i]->sq_exceed_count = wd->tid_rx[i]->sq_behind_count = 0;
+        for (j=0; j<=ZM_AGG_BAW_SIZE; j++)
+            wd->tid_rx[i]->frame[j].buf = 0;
+        /*
+         * reset ADDBA exchange status code
+         * 0: NULL
+         * 1: ADDBA Request sent/received
+         * 2: ACK for ADDBA Request sent/received
+         * 3: ADDBA Response sent/received
+         * 4: ACK for ADDBA Response sent/received
+         */
+        wd->tid_rx[i]->addBaExchangeStatusCode = 0;
+
+    }
+    zmw_leave_critical_section(dev);
+    zfAggTallyReset(dev);
+    DESTQ.init = zfAggDestInit;
+    DESTQ.init(dev);
+    wd->aggInitiated = 1;
+    aggr_count = 0;
+    success_mpdu = 0;
+    total_mpdu = 0;
+#ifdef ZM_ENABLE_AGGREGATION
+#ifndef ZM_ENABLE_FW_BA_RETRANSMISSION //disable BAW
+    BAW = zfwMemAllocate(dev, sizeof(struct baw_enabler));
+    if(!BAW)
+    {
+        return;
+    }
+    BAW->init = zfBawInit;
+    BAW->init(dev);
+#endif //disable BAW
+#endif
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggGetSta                 */
+/*      return STA AID.                                                 */
+/*      take buf as input, use the dest address of buf as index to      */
+/*      search STA AID.                                                 */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer for one particular packet                          */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      AID                                                             */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               ZyDAS Technology Corporation    2006.11     */
+/*                                                                      */
+/************************************************************************/
+
+
+
+u16_t zfAggGetSta(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t id;
+    u16_t dst[3];
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    dst[0] = zmw_rx_buf_readh(dev, buf, 0);
+    dst[1] = zmw_rx_buf_readh(dev, buf, 2);
+    dst[2] = zmw_rx_buf_readh(dev, buf, 4);
+
+    zmw_enter_critical_section(dev);
+
+    if(wd->wlanMode == ZM_MODE_AP) {
+        id = zfApFindSta(dev, dst);
+    }
+    else {
+        id = 0;
+    }
+    zmw_leave_critical_section(dev);
+
+#if ZM_AGG_FPGA_DEBUG
+    id = 0;
+#endif
+
+    return id;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggTxGetQueue             */
+/*      return Queue Pool index.                                        */
+/*      take aid as input, look for the queue index associated          */
+/*      with this aid.                                                  */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      aid : associated id                                             */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      Queue number                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               ZyDAS Technology Corporation    2006.11     */
+/*                                                                      */
+/************************************************************************/
+TID_TX zfAggTxGetQueue(zdev_t* dev, u16_t aid, u16_t tid)
+{
+    //u16_t   i;
+    TID_TX  tid_tx;
+    zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+    /*
+     * not a STA aid
+     */
+    if (0xffff == aid)
+        return NULL;
+
+    //zmw_enter_critical_section(dev);
+
+    tid_tx = wd->aggSta[aid].tid_tx[tid];
+    if (!tid_tx) return NULL;
+    if (0 == tid_tx->aggQEnabled)
+        return NULL;
+
+    //zmw_leave_critical_section(dev);
+
+    return tid_tx;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggTxNewQueue             */
+/*      return Queue Pool index.                                        */
+/*      take aid as input, find a new queue for this aid.               */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      aid : associated id                                             */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      Queue number                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               ZyDAS Technology Corporation    2006.12     */
+/*                                                                      */
+/************************************************************************/
+TID_TX zfAggTxNewQueue(zdev_t* dev, u16_t aid, u16_t tid, zbuf_t* buf)
+{
+    u16_t   i;
+    TID_TX  tid_tx=NULL;
+    u16_t   ac = zcUpToAc[tid&0x7] & 0x3;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    /*
+     * not a STA aid
+     */
+    if (0xffff == aid)
+        return NULL;
+
+    zmw_enter_critical_section(dev);
+
+    /*
+     * find one new queue for sta
+     */
+    for (i=0; i<ZM_AGG_POOL_SIZE; i++)
+    {
+        if (wd->aggQPool[i]->aggQEnabled)
+        {
+                /*
+                 * this q is enabled
+                 */
+        }
+        else
+        {
+            tid_tx = wd->aggQPool[i];
+            tid_tx->aggQEnabled = 1;
+            tid_tx->aggQSTA = aid;
+            tid_tx->ac = ac;
+            tid_tx->tid = tid;
+            tid_tx->aggHead = tid_tx->aggTail = tid_tx->size = 0;
+            tid_tx->aggReady = 0;
+            wd->aggSta[aid].tid_tx[tid] = tid_tx;
+            tid_tx->dst[0] = zmw_rx_buf_readh(dev, buf, 0);
+            tid_tx->dst[1] = zmw_rx_buf_readh(dev, buf, 2);
+            tid_tx->dst[2] = zmw_rx_buf_readh(dev, buf, 4);
+            break;
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return tid_tx;
+}
+
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggTxEnqueue              */
+/*      return Status code ZM_SUCCESS or error code                     */
+/*      take (aid,ac,qnum,buf) as input                                 */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      aid : associated id                                             */
+/*      ac  : access category                                           */
+/*      qnum: the queue number to which will be enqueued                */
+/*      buf : the packet to be queued                                   */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      status code                                                     */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfAggTxEnqueue(zdev_t* dev, zbuf_t* buf, u16_t aid, TID_TX tid_tx)
+{
+    //u16_t   qlen, frameLen;
+    u32_t   time;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    tid_tx->size = zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail);
+
+    if (tid_tx->size < (ZM_AGGQ_SIZE - 2))
+    {
+        /* Queue not full */
+
+
+        /*
+         * buffer copy
+         * in zfwBufFree will return a ndismsendcomplete
+         * to resolve the synchronize problem in aggregate
+         */
+
+        u8_t    sendComplete = 0;
+
+        tid_tx->aggvtxq[tid_tx->aggHead].buf = buf;
+        time = zm_agg_GetTime();
+        tid_tx->aggvtxq[tid_tx->aggHead].arrivalTime = time;
+        tid_tx->aggvtxq[tid_tx->aggHead].baw_retransmit = 0;
+
+        tid_tx->aggHead = ((tid_tx->aggHead + 1) & ZM_AGGQ_SIZE_MASK);
+        tid_tx->lastArrival = time;
+        tid_tx->size++;
+        tid_tx->size = zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail);
+        if (buf && (tid_tx->size < (ZM_AGGQ_SIZE - 10))) {
+            tid_tx->complete = tid_tx->aggHead;
+            sendComplete = 1;
+        }
+        zmw_leave_critical_section(dev);
+
+        if (!DESTQ.exist(dev, 0, tid_tx->ac, tid_tx, NULL)) {
+            DESTQ.insert(dev, 0, tid_tx->ac, tid_tx, NULL);
+        }
+
+        zm_msg1_agg(ZM_LV_0, "tid_tx->size=", tid_tx->size);
+        //zm_debug_msg1("tid_tx->size=", tid_tx->size);
+
+        if (buf && sendComplete && wd->zfcbSendCompleteIndication) {
+            //zmw_leave_critical_section(dev);
+            wd->zfcbSendCompleteIndication(dev, buf);
+        }
+
+        /*if (tid_tx->size >= 16 && zfHpGetFreeTxdCount(dev) > 20)
+            zfAggTxSend(dev, zfHpGetFreeTxdCount(dev), tid_tx);
+        */
+        return ZM_SUCCESS;
+    }
+    else
+    {
+        zm_msg1_agg(ZM_LV_0, "can't enqueue, tid_tx->size=", tid_tx->size);
+        /*
+         * Queue Full
+         */
+
+        /*
+         * zm_msg1_agg(ZM_LV_0, "Queue full, qnum = ", qnum);
+         * wd->commTally.txQosDropCount[ac]++;
+         * zfwBufFree(dev, buf, ZM_SUCCESS);
+         * zm_msg1_agg(ZM_LV_1, "Packet discarded, VTXQ full, ac=", ac);
+         *
+         * return ZM_ERR_EXCEED_PRIORITY_THRESHOLD;
+         */
+    }
+
+    zmw_leave_critical_section(dev);
+
+    if (!DESTQ.exist(dev, 0, tid_tx->ac, tid_tx, NULL)) {
+            DESTQ.insert(dev, 0, tid_tx->ac, tid_tx, NULL);
+    }
+
+    return ZM_ERR_EXCEED_PRIORITY_THRESHOLD;
+}
+
+u16_t    zfAggDestExist(zdev_t* dev, u16_t Qtype, u16_t ac, TID_TX tid_tx, void* vtxq) {
+    struct dest* dest;
+    u16_t   exist = 0;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    if (!DESTQ.Head[ac]) {
+        exist = 0;
+    }
+    else {
+        dest = DESTQ.Head[ac];
+        if (dest->tid_tx == tid_tx) {
+            exist = 1;
+        }
+        else {
+            while (dest->next != DESTQ.Head[ac]) {
+                dest = dest->next;
+                if (dest->tid_tx == tid_tx){
+                    exist = 1;
+                    break;
+                }
+            }
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return exist;
+}
+
+void    zfAggDestInsert(zdev_t* dev, u16_t Qtype, u16_t ac, TID_TX tid_tx, void* vtxq)
+{
+    struct dest* new_dest;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    new_dest = zfwMemAllocate(dev, sizeof(struct dest));
+    if(!new_dest)
+    {
+        return;
+    }
+    new_dest->Qtype = Qtype;
+    new_dest->tid_tx = tid_tx;
+    if (0 == Qtype)
+        new_dest->tid_tx = tid_tx;
+    else
+        new_dest->vtxq = vtxq;
+    if (!DESTQ.Head[ac]) {
+
+        zmw_enter_critical_section(dev);
+        new_dest->next = new_dest;
+        DESTQ.Head[ac] = DESTQ.dest[ac] = new_dest;
+        zmw_leave_critical_section(dev);
+    }
+    else {
+
+        zmw_enter_critical_section(dev);
+        new_dest->next = DESTQ.dest[ac]->next;
+        DESTQ.dest[ac]->next = new_dest;
+        zmw_leave_critical_section(dev);
+    }
+
+
+    //DESTQ.size[ac]++;
+    return;
+}
+
+void    zfAggDestDelete(zdev_t* dev, u16_t Qtype, TID_TX tid_tx, void* vtxq)
+{
+    struct dest* dest, *temp;
+    u16_t   i;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    if (wd->destLock) {
+        zmw_leave_critical_section(dev);
+        return;
+    }
+
+
+    //zmw_declare_for_critical_section();
+    for (i=0; i<4; i++) {
+        if (!DESTQ.Head[i]) continue;
+        dest = DESTQ.Head[i];
+        if (!dest) continue;
+
+
+        while (dest && (dest->next != DESTQ.Head[i])) {
+            if (Qtype == 0 && dest->next->tid_tx == tid_tx){
+                break;
+            }
+            if (Qtype == 1 && dest->next->vtxq == vtxq) {
+                break;
+            }
+            dest = dest->next;
+        }
+
+        if ((Qtype == 0 && dest->next->tid_tx == tid_tx) || (Qtype == 1 && dest->next->vtxq == vtxq)) {
+
+            tid_tx->size = zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail);
+            if (tid_tx->size) {
+                zmw_leave_critical_section(dev);
+                return;
+            }
+            if (!DESTQ.Head[i]) {
+                temp = NULL;
+            }
+            else {
+                temp = dest->next;
+                if (temp == dest) {
+                    DESTQ.Head[i] = DESTQ.dest[i] = NULL;
+                    //DESTQ.size[i] = 0;
+                }
+                else {
+                    dest->next = dest->next->next;
+                }
+            }
+
+            if (temp == NULL)
+                {/* do nothing */} //zfwMemFree(dev, temp, sizeof(struct dest));
+            else
+                zfwMemFree(dev, temp, sizeof(struct dest));
+
+            /*zmw_enter_critical_section(dev);
+            if (DESTQ.size[i] > 0)
+                DESTQ.size[i]--;
+            zmw_leave_critical_section(dev);
+            */
+        }
+
+    }
+    zmw_leave_critical_section(dev);
+    return;
+}
+
+void    zfAggDestInit(zdev_t* dev)
+{
+    u16_t i;
+    zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+    for (i=0; i<4; i++) {
+        //wd->destQ.Head[i].next = wd->destQ.Head[i];
+        //wd->destQ.dest[i] = wd->destQ.Head[i];
+        //DESTQ.size[i] = 0;
+        DESTQ.Head[i] = NULL;
+    }
+    DESTQ.insert  = zfAggDestInsert;
+    DESTQ.delete  = zfAggDestDelete;
+    DESTQ.init    = zfAggDestInit;
+    DESTQ.getNext = zfAggDestGetNext;
+    DESTQ.exist   = zfAggDestExist;
+    DESTQ.ppri = 0;
+    return;
+}
+
+struct dest* zfAggDestGetNext(zdev_t* dev, u16_t ac)
+{
+    struct dest *dest = NULL;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    if (DESTQ.dest[ac]) {
+        dest = DESTQ.dest[ac];
+        DESTQ.dest[ac] = DESTQ.dest[ac]->next;
+    }
+    else {
+        dest = NULL;
+    }
+    zmw_leave_critical_section(dev);
+
+    return dest;
+}
+
+#ifdef ZM_ENABLE_AGGREGATION
+#ifndef ZM_ENABLE_FW_BA_RETRANSMISSION //disable BAW
+u16_t   zfAggTidTxInsertHead(zdev_t* dev, struct bufInfo *buf_info,TID_TX tid_tx)
+{
+    zbuf_t* buf;
+    u32_t time;
+    struct baw_header *baw_header;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+
+    buf = buf_info->buf;
+
+    zmw_enter_critical_section(dev);
+    tid_tx->size = zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail);
+    zmw_leave_critical_section(dev);
+
+    if (tid_tx->size >= (ZM_AGGQ_SIZE - 2)) {
+        zfwBufFree(dev, buf, ZM_SUCCESS);
+        return 0;
+    }
+
+    zmw_enter_critical_section(dev);
+    tid_tx->aggTail = (tid_tx->aggTail == 0)? ZM_AGGQ_SIZE_MASK: tid_tx->aggTail - 1;
+    tid_tx->aggvtxq[tid_tx->aggTail].buf = buf;
+    //time = zm_agg_GetTime();
+    tid_tx->aggvtxq[tid_tx->aggTail].arrivalTime = buf_info->timestamp;
+    tid_tx->aggvtxq[tid_tx->aggTail].baw_retransmit = buf_info->baw_retransmit;
+
+    baw_header = &tid_tx->aggvtxq[tid_tx->aggTail].baw_header;
+    baw_header->headerLen   = buf_info->baw_header->headerLen;
+    baw_header->micLen      = buf_info->baw_header->micLen;
+    baw_header->snapLen     = buf_info->baw_header->snapLen;
+    baw_header->removeLen   = buf_info->baw_header->removeLen;
+    baw_header->keyIdx      = buf_info->baw_header->keyIdx;
+    zfwMemoryCopy((u8_t *)baw_header->header, (u8_t *)buf_info->baw_header->header, 58);
+    zfwMemoryCopy((u8_t *)baw_header->mic   , (u8_t *)buf_info->baw_header->mic   , 8);
+    zfwMemoryCopy((u8_t *)baw_header->snap  , (u8_t *)buf_info->baw_header->snap  , 8);
+
+    tid_tx->size++;
+    tid_tx->size = zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail);
+    zmw_leave_critical_section(dev);
+
+    //tid_tx->lastArrival = time;
+    if (1 == tid_tx->size) {
+        DESTQ.insert(dev, 0, tid_tx->ac, tid_tx, NULL);
+    }
+
+
+    zm_msg1_agg(ZM_LV_0, "0xC2:insertHead, tid_tx->size=", tid_tx->size);
+
+    return TRUE;
+}
+#endif //disable BAW
+#endif
+
+void    zfiTxComplete(zdev_t* dev)
+{
+
+    zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+    if( (wd->wlanMode == ZM_MODE_AP) ||
+        (wd->wlanMode == ZM_MODE_INFRASTRUCTURE && wd->sta.EnableHT) ||
+        (wd->wlanMode == ZM_MODE_PSEUDO) ) {
+        zfAggTxScheduler(dev, 0);
+    }
+
+    return;
+}
+
+TID_TX  zfAggTxReady(zdev_t* dev) {
+    //struct dest* dest;
+    u16_t   i;
+    TID_TX  tid_tx = NULL;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    for (i=0; i<ZM_AGG_POOL_SIZE; i++)
+    {
+        if (wd->aggQPool[i]->aggQEnabled)
+        {
+            if (wd->aggQPool[i]->size >= 16) {
+                tid_tx = wd->aggQPool[i];
+                break;
+            }
+        }
+        else {
+        }
+    }
+    zmw_leave_critical_section(dev);
+    return tid_tx;
+}
+
+u16_t   zfAggValidTidTx(zdev_t* dev, TID_TX tid_tx) {
+    u16_t   i, valid = 0;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    for (i=0; i<ZM_AGG_POOL_SIZE; i++)
+    {
+        if (wd->aggQPool[i] == tid_tx)
+        {
+            valid = 1;
+            break;
+        }
+        else {
+        }
+    }
+    zmw_leave_critical_section(dev);
+
+    return valid;
+}
+
+void    zfAggTxScheduler(zdev_t* dev, u8_t ScanAndClear)
+{
+    TID_TX  tid_tx = NULL;
+    void*   vtxq;
+    struct dest* dest;
+    zbuf_t*  buf;
+    u32_t txql, min_txql;
+    //u16_t aggr_size = 1;
+    u16_t txq_threshold;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if (!wd->aggInitiated)
+    {
+        return;
+    }
+
+    /* debug */
+    txql = TXQL;
+    min_txql = AGG_MIN_TXQL;
+
+    if(wd->txq_threshold)
+        txq_threshold = wd->txq_threshold;
+    else
+        txq_threshold = AGG_MIN_TXQL;
+
+    tid_tx = zfAggTxReady(dev);
+    if (tid_tx) ScanAndClear = 0;
+    while (zfHpGetFreeTxdCount(dev) > 20 && (TXQL < txq_threshold || tid_tx)) {
+    //while (zfHpGetFreeTxdCount(dev) > 20 && (ScanAndClear || tid_tx)) {
+    //while (TXQL < txq_threshold) {
+        u16_t i;
+        u8_t ac;
+        s8_t destQ_count = 0;
+    //while ((zfHpGetFreeTxdCount(dev)) > 32) {
+
+        //DbgPrint("zfAggTxScheduler: in while loop");
+        for (i=0; i<4; i++) {
+            if (DESTQ.Head[i]) destQ_count++;
+        }
+        if (0 >= destQ_count) break;
+
+        zmw_enter_critical_section(dev);
+        ac = pri[DESTQ.ppri]; DESTQ.ppri = (DESTQ.ppri + 1) % 10;
+        zmw_leave_critical_section(dev);
+
+        for (i=0; i<10; i++){
+            if(DESTQ.Head[ac]) break;
+
+            zmw_enter_critical_section(dev);
+            ac = pri[DESTQ.ppri]; DESTQ.ppri = (DESTQ.ppri + 1) % 10;
+            zmw_leave_critical_section(dev);
+        }
+        if (i == 10) break;
+        //DbgPrint("zfAggTxScheduler: have dest Q");
+        zmw_enter_critical_section(dev);
+        wd->destLock = 1;
+        zmw_leave_critical_section(dev);
+
+        dest = DESTQ.getNext(dev, ac);
+        if (!dest) {
+            zmw_enter_critical_section(dev);
+            wd->destLock = 0;
+            zmw_leave_critical_section(dev);
+
+            DbgPrint("bug report! DESTQ.getNext got nothing!");
+            break;
+        }
+        if (dest->Qtype == 0) {
+            tid_tx = dest->tid_tx;
+
+            //DbgPrint("zfAggTxScheduler: have tid_tx Q");
+
+            if(tid_tx && zfAggValidTidTx(dev, tid_tx))
+                tid_tx->size = zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail);
+            else {
+                zmw_enter_critical_section(dev);
+                wd->destLock = 0;
+                zmw_leave_critical_section(dev);
+
+                tid_tx = zfAggTxReady(dev);
+                continue;
+            }
+
+            zmw_enter_critical_section(dev);
+            wd->destLock = 0;
+            zmw_leave_critical_section(dev);
+            //zmw_enter_critical_section(dev);
+            if (tid_tx && !tid_tx->size) {
+
+                //zmw_leave_critical_section(dev);
+                //DESTQ.delete(dev, 0, tid_tx, NULL);
+            }
+            else if(wd->aggState == 0){
+                //wd->aggState = 1;
+                //zmw_leave_critical_section(dev);
+                zfAggTxSend(dev, zfHpGetFreeTxdCount(dev), tid_tx);
+                //wd->aggState = 0;
+            }
+            else {
+                //zmw_leave_critical_section(dev);
+                break;
+            }
+        }
+        else {
+            vtxq = dest->vtxq;
+            buf = zfGetVtxq(dev, ac);
+            zm_assert( buf != 0 );
+
+            zfTxSendEth(dev, buf, 0, ZM_EXTERNAL_ALLOC_BUF, 0);
+
+        }
+        /*flush all but < 16 frames in tid_tx to TXQ*/
+        tid_tx = zfAggTxReady(dev);
+    }
+
+    /*while ((zfHpGetFreeTxdCount(dev)) > 32) {
+    //while ((zfHpGetFreeTxdCount(dev)) > 32) {
+
+        destQ_count = 0;
+        for (i=0; i<4; i++) destQ_count += wd->destQ.size[i];
+        if (0 >= destQ_count) break;
+
+        ac = pri[wd->destQ.ppri]; wd->destQ.ppri = (wd->destQ.ppri + 1) % 10;
+        for (i=0; i<10; i++){
+            if(wd->destQ.size[ac]!=0) break;
+            ac = pri[wd->destQ.ppri]; wd->destQ.ppri = (wd->destQ.ppri + 1) % 10;
+        }
+        if (i == 10) break;
+        dest = wd->destQ.getNext(dev, ac);
+        if (dest->Qtype == 0) {
+            tid_tx = dest->tid_tx;
+            tid_tx->size = zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail);
+            if (!tid_tx->size) {
+                wd->destQ.delete(dev, 0, tid_tx, NULL);
+                break;
+            }
+            else if((wd->aggState == 0) && (tid_tx->size >= 16)){
+                zfAggTxSend(dev, zfHpGetFreeTxdCount(dev), tid_tx);
+            }
+            else {
+                break;
+            }
+        }
+
+    }
+    */
+    return;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggTx                     */
+/*      return Status code ZM_SUCCESS or error code                     */
+/*      management A-MPDU aggregation function,                         */
+/*      management aggregation queue, calculate arrivalrate,            */
+/*      add/delete an aggregation queue of a stream,                    */
+/*      enqueue packets into responsible aggregate queue.               */
+/*      take (dev, buf, ac) as input                                    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : packet buff                                               */
+/*      ac  : access category                                           */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      status code                                                     */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfAggTx(zdev_t* dev, zbuf_t* buf, u16_t tid)
+{
+    u16_t aid;
+    //u16_t qnum;
+    //u16_t aggflag = 0;
+    //u16_t arrivalrate = 0;
+    TID_TX tid_tx;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if(!wd->aggInitiated)
+    {
+        return ZM_ERR_TX_BUFFER_UNAVAILABLE;
+    }
+
+    aid = zfAggGetSta(dev, buf);
+
+    //arrivalrate = zfAggTxArrivalRate(dev, aid, tid);
+
+    if (0xffff == aid)
+    {
+        /*
+         * STA not associated, this is a BC/MC or STA->AP packet
+         */
+
+        return ZM_ERR_TX_BUFFER_UNAVAILABLE;
+    }
+
+    /*
+     * STA associated, a unicast packet
+     */
+
+    tid_tx = zfAggTxGetQueue(dev, aid, tid);
+
+    /*tid_q.tid_tx = tid_tx;
+    wd->destQ.insert = zfAggDestInsert;
+    wd->destQ.insert(dev, 0, tid_q);
+    */
+    if (tid_tx != NULL)
+    {
+        /*
+         * this (aid, ac) is aggregated
+         */
+
+        //if (arrivalrate < ZM_AGG_LOW_THRESHOLD)
+        if (0)
+        {
+            /*
+             * arrival rate too low
+             * delete this aggregate queue
+             */
+
+            zmw_enter_critical_section(dev);
+
+            //wd->aggQPool[qnum]->clearFlag = wd->aggQPool[qnum]->deleteFlag =1;
+
+            zmw_leave_critical_section(dev);
+
+        }
+
+        return zfAggTxEnqueue(dev, buf, aid, tid_tx);
+
+    }
+    else
+    {
+        /*
+         * this (aid, ac) not yet aggregated
+         * queue not found
+         */
+
+        //if (arrivalrate > ZM_AGG_HIGH_THRESHOLD)
+        if (1)
+        {
+            /*
+             * arrivalrate high enough to get a new agg queue
+             */
+
+            tid_tx = zfAggTxNewQueue(dev, aid, tid, buf);
+
+            //zm_msg1_agg(ZM_LV_0, "get new AggQueue qnum = ", tid_tx->);
+
+            if (tid_tx)
+            {
+                /*
+                 * got a new aggregate queue
+                 */
+
+                //zmw_enter_critical_section(dev);
+
+                //wd->aggSta[aid].aggFlag[ac] = 1;
+
+                //zmw_leave_critical_section(dev);
+
+                /*
+                 * add ADDBA functions here
+                 * return ZM_ERR_TX_BUFFER_UNAVAILABLE;
+                 */
+
+
+                //zfAggSendAddbaRequest(dev, tid_tx->dst, tid_tx->ac, tid_tx->tid);
+                //zmw_enter_critical_section(dev);
+
+                //wd->aggSta[aid].aggFlag[ac] = 0;
+
+                //zmw_leave_critical_section(dev);
+
+                return zfAggTxEnqueue(dev, buf, aid, tid_tx);
+
+            }
+            else
+            {
+                /*
+                 * just can't get a new aggregate queue
+                 */
+
+                return ZM_ERR_TX_BUFFER_UNAVAILABLE;
+            }
+        }
+        else
+        {
+            /*
+             * arrival rate is not high enough to get a new agg queue
+             */
+
+            return ZM_ERR_TX_BUFFER_UNAVAILABLE;
+        }
+    }
+
+
+
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggTxReadyCount           */
+/*      return counter of ready to aggregate queues.                    */
+/*      take (dev, ac) as input, only calculate the ready to aggregate  */
+/*      queues of one particular ac.                                    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      ac  : access category                                           */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      counter of ready to aggregate queues                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfAggTxReadyCount(zdev_t* dev, u16_t ac)
+{
+    u16_t i;
+    u16_t readycount = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    for (i=0 ; i<ZM_AGG_POOL_SIZE; i++)
+    {
+        if (wd->aggQPool[i]->aggQEnabled && (wd->aggQPool[i]->aggReady || \
+                wd->aggQPool[i]->clearFlag) && ac == wd->aggQPool[i]->ac)
+            readycount++;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return readycount;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggTxPartial              */
+/*      return the number that Vtxq has to send.                        */
+/*      take (dev, ac, readycount) as input, calculate the ratio of     */
+/*      Vtxq length to (Vtxq length + readycount) of a particular ac,   */
+/*      and returns the Vtxq length * the ratio                         */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      ac  : access category                                           */
+/*      readycount: the number of ready to aggregate queues of this ac  */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      Vtxq length * ratio                                             */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfAggTxPartial(zdev_t* dev, u16_t ac, u16_t readycount)
+{
+    u16_t qlen;
+    u16_t partial;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    qlen = zm_agg_qlen(dev, wd->vtxqHead[ac], wd->vtxqTail[ac]);
+
+    if ((qlen + readycount) > 0)
+    {
+        partial = (u16_t)( zm_agg_weight(ac) * ((u16_t)qlen/(qlen + \
+                        readycount)) );
+    }
+    else
+    {
+        partial = 0;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    if (partial > qlen)
+        partial = qlen;
+
+    return partial;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggTxSend                 */
+/*      return sentcount                                                */
+/*      take (dev, ac, n) as input, n is the number of scheduled agg    */
+/*      queues to be sent of the particular ac.                         */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      ac  : access category                                           */
+/*      n   : the number of scheduled aggregation queues to be sent     */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      sentcount                                                       */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfAggTxSend(zdev_t* dev, u32_t freeTxd, TID_TX tid_tx)
+{
+    //u16_t   qnum;
+    //u16_t   qlen;
+    u16_t   j;
+    //u16_t   sentcount = 0;
+    zbuf_t* buf;
+    struct  aggControl aggControl;
+    u16_t   aggLen;
+    //zbuf_t*  newBuf;
+    //u16_t   bufLen;
+    //TID_BAW tid_baw = NULL;
+    //struct bufInfo *buf_info;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    //while (tid_tx->size > 0)
+
+    zmw_enter_critical_section(dev);
+    tid_tx->size = zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail);
+    aggLen = zm_agg_min(16, zm_agg_min(tid_tx->size, (u16_t)(freeTxd - 2)));
+    zmw_leave_critical_section(dev);
+
+            /*
+             * why there have to be 2 free Txd?
+             */
+    if (aggLen <=0 )
+        return 0;
+
+
+    if (aggLen == 1) {
+        buf = zfAggTxGetVtxq(dev, tid_tx);
+        if (buf)
+            zfTxSendEth(dev, buf, 0, ZM_EXTERNAL_ALLOC_BUF, 0);
+        if (tid_tx->size == 0) {
+            //DESTQ.delete(dev, 0, tid_tx, NULL);
+        }
+
+        return 1;
+    }
+                /*
+                 * Free Txd queue is big enough to put aggregation
+                 */
+    zmw_enter_critical_section(dev);
+    if (wd->aggState == 1) {
+        zmw_leave_critical_section(dev);
+        return 0;
+    }
+    wd->aggState = 1;
+    zmw_leave_critical_section(dev);
+
+
+    zm_msg1_agg(ZM_LV_0, "aggLen=", aggLen);
+    tid_tx->aggFrameSize = 0;
+    for (j=0; j < aggLen; j++) {
+        buf = zfAggTxGetVtxq(dev, tid_tx);
+
+        zmw_enter_critical_section(dev);
+        tid_tx->size = zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail);
+        zmw_leave_critical_section(dev);
+
+        if ( buf ) {
+            //struct aggTally *agg_tal;
+            u16_t completeIndex;
+
+            if (0 == j) {
+                aggControl.ampduIndication = ZM_AGG_FIRST_MPDU;
+
+            }
+            else if ((j == (aggLen - 1)) || tid_tx->size == 0)
+            {
+                aggControl.ampduIndication = ZM_AGG_LAST_MPDU;
+                //wd->aggState = 0;
+
+            }
+            else
+            {
+                aggControl.ampduIndication = ZM_AGG_MIDDLE_MPDU;
+                /* the packet is delayed more than 500 ms, drop it */
+
+            }
+            tid_tx->aggFrameSize += zfwBufGetSize(dev, buf);
+            aggControl.addbaIndication = 0;
+            aggControl.aggEnabled = 1;
+
+#ifdef ZM_AGG_TALLY
+            agg_tal = &wd->agg_tal;
+            agg_tal->sent_packets_sum++;
+
+#endif
+
+            zfAggTxSendEth(dev, buf, 0, ZM_EXTERNAL_ALLOC_BUF, 0, &aggControl, tid_tx);
+
+            zmw_enter_critical_section(dev);
+            completeIndex = tid_tx->complete;
+            if(zm_agg_inQ(tid_tx, tid_tx->complete))
+                zm_agg_plus(tid_tx->complete);
+            zmw_leave_critical_section(dev);
+
+            if(zm_agg_inQ(tid_tx, completeIndex) && wd->zfcbSendCompleteIndication
+                    && tid_tx->aggvtxq[completeIndex].buf) {
+                wd->zfcbSendCompleteIndication(dev, tid_tx->aggvtxq[completeIndex].buf);
+                zm_debug_msg0("in queue complete worked!");
+            }
+
+        }
+        else {
+            /*
+             * this aggregation queue is empty
+             */
+            zm_msg1_agg(ZM_LV_0, "aggLen not reached, but no more frame, j=", j);
+
+            break;
+        }
+    }
+    zmw_enter_critical_section(dev);
+    wd->aggState = 0;
+    zmw_leave_critical_section(dev);
+
+    //zm_acquire_agg_spin_lock(Adapter);
+    tid_tx->size = zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail);
+    //zm_release_agg_spin_lock(Adapter);
+
+    if (tid_tx->size == 0) {
+        //DESTQ.delete(dev, 0, tid_tx, NULL);
+    }
+
+
+
+    //zfAggInvokeBar(dev, tid_tx);
+    if(j>0) {
+        aggr_count++;
+        zm_msg1_agg(ZM_LV_0, "0xC2:sent 1 aggr, aggr_count=", aggr_count);
+        zm_msg1_agg(ZM_LV_0, "0xC2:sent 1 aggr, aggr_size=", j);
+    }
+    return j;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggTxGetReadyQueue        */
+/*      return the number of the aggregation queue                      */
+/*      take (dev, ac) as input, find the agg queue with smallest       */
+/*      arrival time (waited longest) among those ready or clearFlag    */
+/*      set queues.                                                     */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      ac  : access category                                           */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      aggregation queue number                                        */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+TID_TX zfAggTxGetReadyQueue(zdev_t* dev, u16_t ac)
+{
+    //u16_t       qnum = ZM_AGG_POOL_SIZE;
+    u16_t       i;
+    u32_t       time = 0;
+    TID_TX      tid_tx = NULL;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    for (i=0 ;i<ZM_AGG_POOL_SIZE; i++)
+    {
+        if (1 == wd->aggQPool[i]->aggQEnabled && ac == wd->aggQPool[i]->ac &&
+                (wd->aggQPool[i]->size > 0))
+        {
+            if (0 == time || time > wd->aggQPool[i]->aggvtxq[ \
+                            wd->aggQPool[i]->aggHead ].arrivalTime)
+            {
+                tid_tx = wd->aggQPool[i];
+                time = tid_tx->aggvtxq[ tid_tx->aggHead ].arrivalTime;
+            }
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return tid_tx;
+}
+
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggTxGetVtxq              */
+/*      return an MSDU                                                  */
+/*      take (dev, qnum) as input, return an MSDU out of the agg queue. */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      qnum: queue number                                              */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      a MSDU                                                          */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+zbuf_t* zfAggTxGetVtxq(zdev_t* dev, TID_TX tid_tx)
+{
+    zbuf_t* buf = NULL;
+
+    zmw_declare_for_critical_section();
+
+    if (tid_tx->aggHead != tid_tx->aggTail)
+    {
+        buf = tid_tx->aggvtxq[ tid_tx->aggTail ].buf;
+
+        tid_tx->aggvtxq[tid_tx->aggTail].buf = NULL;
+
+        zmw_enter_critical_section(dev);
+        tid_tx->aggTail = ((tid_tx->aggTail + 1) & ZM_AGGQ_SIZE_MASK);
+        if(tid_tx->size > 0) tid_tx->size--;
+        tid_tx->size = zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail);
+        if (NULL == buf) {
+            //tid_tx->aggTail = tid_tx->aggHead = tid_tx->size = 0;
+            //zm_msg1_agg(ZM_LV_0, "GetVtxq buf == NULL, tid_tx->size=", tid_tx->size);
+        }
+        zmw_leave_critical_section(dev);
+    }
+    else
+    {
+        /*
+         * queue is empty
+         */
+        zm_msg1_agg(ZM_LV_0, "tid_tx->aggHead == tid_tx->aggTail, tid_tx->size=", tid_tx->size);
+
+    }
+
+    if (zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail) != tid_tx->size)
+        zm_msg1_agg(ZM_LV_0, "qlen!=tid_tx->size! tid_tx->size=", tid_tx->size);
+    return buf;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggTxDeleteQueue          */
+/*      return ZM_SUCCESS (can't fail)                                  */
+/*      take (dev, qnum) as input, reset (delete) this aggregate queue, */
+/*      this queue is virtually returned to the aggregate queue pool.   */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      qnum: queue number                                              */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      ZM_SUCCESS                                                      */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfAggTxDeleteQueue(zdev_t* dev, u16_t qnum)
+{
+    u16_t ac, tid;
+    struct aggQueue *tx_tid;
+    struct aggSta   *agg_sta;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    tx_tid = wd->aggQPool[qnum];
+    agg_sta = &wd->aggSta[tx_tid->aggQSTA];
+    ac = tx_tid->ac;
+    tid = tx_tid->tid;
+
+    zmw_enter_critical_section(dev);
+
+    tx_tid->aggQEnabled = 0;
+    tx_tid->aggHead = tx_tid->aggTail = 0;
+    tx_tid->aggReady = 0;
+    tx_tid->clearFlag = tx_tid->deleteFlag = 0;
+    tx_tid->size = 0;
+    agg_sta->count[ac] = 0;
+
+    agg_sta->tid_tx[tid] = NULL;
+    agg_sta->aggFlag[ac] = 0;
+
+    zmw_leave_critical_section(dev);
+
+    zm_msg1_agg(ZM_LV_0, "queue deleted! qnum=", qnum);
+
+    return ZM_SUCCESS;
+}
+
+#ifdef ZM_ENABLE_AGGREGATION
+#ifndef ZM_ENABLE_FW_BA_RETRANSMISSION //disable BAW
+void zfBawCore(zdev_t* dev, u16_t baw_seq, u32_t bitmap, u16_t aggLen) {
+    TID_BAW tid_baw;
+    s16_t i;
+    zbuf_t* buf;
+    struct bufInfo *buf_info;
+
+    zmw_get_wlan_dev(dev);
+    //zmw_declare_for_critical_section();
+    tid_baw = BAW->getQ(dev, baw_seq);
+    //tid_baw = NULL;
+    if (NULL == tid_baw)
+        return;
+
+    total_mpdu += aggLen;
+    for (i = aggLen - 1; i>=0; i--) {
+        if (((bitmap >> i) & 0x1) == 0) {
+            buf_info = BAW->pop(dev, i, tid_baw);
+            buf = buf_info->buf;
+            if (buf) {
+                //wd->zfcbSetBawQ(dev, buf, 0);
+                zfAggTidTxInsertHead(dev, buf_info, tid_baw->tid_tx);
+            }
+        }
+        else {
+            success_mpdu++;
+        }
+    }
+    BAW->disable(dev, tid_baw);
+    zfAggTxScheduler(dev);
+    zm_debug_msg1("success_mpdu = ", success_mpdu);
+    zm_debug_msg1("  total_mpdu = ", total_mpdu);
+}
+
+void    zfBawInit(zdev_t* dev) {
+    TID_BAW tid_baw;
+    u16_t i,j;
+    zmw_get_wlan_dev(dev);
+    //zmw_declare_for_critical_section();
+
+    for (i=0; i<ZM_BAW_POOL_SIZE; i++){
+        tid_baw = &BAW->tid_baw[i];
+        for (j=0; j<ZM_VTXQ_SIZE; j++) {
+            tid_baw->frame[j].buf = NULL;
+        }
+        tid_baw->enabled = tid_baw->head = tid_baw->tail = tid_baw->size = 0;
+        tid_baw->start_seq = 0;
+    }
+    BAW->delPoint = 0;
+    BAW->core = zfBawCore;
+    BAW->getNewQ = zfBawGetNewQ;
+    BAW->insert = zfBawInsert;
+    BAW->pop = zfBawPop;
+    BAW->enable = zfBawEnable;
+    BAW->disable = zfBawDisable;
+    BAW->getQ = zfBawGetQ;
+}
+
+
+
+TID_BAW zfBawGetNewQ(zdev_t* dev, u16_t start_seq, TID_TX tid_tx) {
+    TID_BAW tid_baw=NULL;
+    TID_BAW next_baw=NULL;
+    u16_t i;
+    zmw_get_wlan_dev(dev);
+    //zmw_declare_for_critical_section();
+
+    /*
+    for (i=0; i<ZM_BAW_POOL_SIZE; i++){
+        tid_baw = &BAW->tid_baw[i];
+        if (FALSE == tid_baw->enabled)
+            break;
+    }
+    */
+
+    tid_baw = &BAW->tid_baw[BAW->delPoint];
+    i = BAW->delPoint;
+    //if (ZM_BAW_POOL_SIZE == i) {
+        //return NULL;
+    //    u8_t temp = BAW->delPoint;
+    //    tid_baw = &BAW->tid_baw[BAW->delPoint];
+    //    BAW->disable(dev, tid_baw);
+    //    BAW->delPoint = (BAW->delPoint < (ZM_BAW_POOL_SIZE - 1))? (BAW->delPoint + 1): 0;
+    //    temp = BAW->delPoint;
+    //}
+
+    zm_msg1_agg(ZM_LV_0, "get new tid_baw, index=", i);
+    BAW->delPoint = (i < (ZM_BAW_POOL_SIZE -1))? (i + 1): 0;
+    next_baw = &BAW->tid_baw[BAW->delPoint];
+    if (1 == next_baw->enabled) BAW->disable(dev, next_baw);
+
+    BAW->enable(dev, tid_baw, start_seq);
+    tid_baw->tid_tx = tid_tx;
+
+    return tid_baw;
+}
+
+u16_t   zfBawInsert(zdev_t* dev, zbuf_t* buf, u16_t baw_seq, TID_BAW tid_baw, u8_t baw_retransmit, struct baw_header_r *header_r) {
+    //TID_BAW tid_baw;
+    //u16_t   bufLen;
+
+    //zmw_get_wlan_dev(dev);
+    //zmw_declare_for_critical_section();
+
+    if(tid_baw->size < (ZM_VTXQ_SIZE - 1)) {
+        struct baw_header *baw_header = &tid_baw->frame[tid_baw->head].baw_header;
+
+        baw_header->headerLen   = header_r->headerLen;
+        baw_header->micLen      = header_r->micLen;
+        baw_header->snapLen     = header_r->snapLen;
+        baw_header->removeLen   = header_r->removeLen;
+        baw_header->keyIdx      = header_r->keyIdx;
+        zfwMemoryCopy((u8_t *)baw_header->header, (u8_t *)header_r->header, 58);
+        zfwMemoryCopy((u8_t *)baw_header->mic   , (u8_t *)header_r->mic   , 8);
+        zfwMemoryCopy((u8_t *)baw_header->snap  , (u8_t *)header_r->snap  , 8);
+        //wd->zfcbSetBawQ(dev, buf, 1);
+        tid_baw->frame[tid_baw->head].buf = buf;
+        tid_baw->frame[tid_baw->head].baw_seq = baw_seq;
+        tid_baw->frame[tid_baw->head].baw_retransmit = baw_retransmit + 1;
+
+        //tid_baw->frame[tid_baw->head].data = pBuf->data;
+        tid_baw->head++;
+        tid_baw->size++;
+    }
+    else {
+        //wd->zfcbSetBawQ(dev, buf, 0);
+        zfwBufFree(dev, buf, ZM_SUCCESS);
+        return FALSE;
+    }
+    return TRUE;
+}
+
+struct bufInfo* zfBawPop(zdev_t* dev, u16_t index, TID_BAW tid_baw) {
+    //TID_BAW tid_baw;
+    //zbuf_t* buf;
+    struct bufInfo *buf_info;
+    zmw_get_wlan_dev(dev);
+
+    buf_info = &wd->buf_info;
+    buf_info->baw_header = NULL;
+
+    if (NULL == (buf_info->buf = tid_baw->frame[index].buf))
+        return buf_info;
+
+    buf_info->baw_retransmit = tid_baw->frame[index].baw_retransmit;
+    buf_info->baw_header = &tid_baw->frame[index].baw_header;
+    buf_info->timestamp = tid_baw->frame[index].timestamp;
+    //pBuf->data = pBuf->buffer;
+    //wd->zfcbRestoreBufData(dev, buf);
+    tid_baw->frame[index].buf = NULL;
+
+    return buf_info;
+}
+
+void    zfBawEnable(zdev_t* dev, TID_BAW tid_baw, u16_t start_seq) {
+    //TID_BAW tid_baw;
+
+    //zmw_get_wlan_dev(dev);
+    //zmw_declare_for_critical_section();
+
+    tid_baw->enabled = TRUE;
+    tid_baw->head = tid_baw->tail = tid_baw->size = 0;
+    tid_baw->start_seq = start_seq;
+}
+
+void    zfBawDisable(zdev_t* dev, TID_BAW tid_baw) {
+    //TID_BAW tid_baw;
+    u16_t i;
+
+    //zmw_get_wlan_dev(dev);
+    //zmw_declare_for_critical_section();
+    for (i=0; i<ZM_VTXQ_SIZE; i++) {
+        if (tid_baw->frame[i].buf) {
+
+            //wd->zfcbSetBawQ(dev, tid_baw->frame[i].buf, 0);
+            zfwBufFree(dev, tid_baw->frame[i].buf, ZM_SUCCESS);
+            tid_baw->frame[i].buf = NULL;
+        }
+    }
+
+    tid_baw->enabled = FALSE;
+}
+
+TID_BAW zfBawGetQ(zdev_t* dev, u16_t baw_seq) {
+    TID_BAW tid_baw=NULL;
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+    //zmw_declare_for_critical_section();
+    for (i=0; i<ZM_BAW_POOL_SIZE; i++){
+        tid_baw = &BAW->tid_baw[i];
+        if (TRUE == tid_baw->enabled)
+        {
+            zm_msg1_agg(ZM_LV_0, "get an old tid_baw, baw_seq=", baw_seq);
+            zm_msg1_agg(ZM_LV_0, "check a  tid_baw->start_seq=", tid_baw->start_seq);
+            if(baw_seq == tid_baw->start_seq)
+                break;
+        }
+
+    }
+    if (ZM_BAW_POOL_SIZE == i)
+        return NULL;
+    return tid_baw;
+}
+#endif //disable BAW
+#endif
+
+u16_t zfAggTallyReset(zdev_t* dev)
+{
+    struct aggTally* agg_tal;
+
+    zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+    agg_tal = &wd->agg_tal;
+    agg_tal->got_packets_sum = 0;
+    agg_tal->got_bytes_sum = 0;
+    agg_tal->sent_bytes_sum = 0;
+    agg_tal->sent_packets_sum = 0;
+    agg_tal->avg_got_packets = 0;
+    agg_tal->avg_got_bytes = 0;
+    agg_tal->avg_sent_packets = 0;
+    agg_tal->avg_sent_bytes = 0;
+    agg_tal->time = 0;
+    return 0;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggScanAndClear           */
+/*      If the packets in a queue have waited for too long, clear and   */
+/*      delete this aggregation queue.                                  */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev     : device pointer                                        */
+/*      time    : current time                                          */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      ZM_SUCCESS                                                      */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t   zfAggScanAndClear(zdev_t* dev, u32_t time)
+{
+    u16_t i;
+    u16_t head;
+    u16_t tail;
+    u32_t tick;
+    u32_t arrivalTime;
+    //u16_t aid, ac;
+    TID_TX tid_tx;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if(!(wd->state == ZM_WLAN_STATE_ENABLED)) return 0;
+    zfAggTxScheduler(dev, 1);
+    tick = zm_agg_GetTime();
+    for (i=0; i<ZM_AGG_POOL_SIZE; i++)
+    {
+        if (!wd->aggQPool[i]) return 0;
+        if (1 == wd->aggQPool[i]->aggQEnabled)
+        {
+            tid_tx = wd->aggQPool[i];
+            zmw_enter_critical_section(dev);
+
+            head = tid_tx->aggHead;
+            tail = tid_tx->aggTail;
+
+            arrivalTime = (u32_t)tid_tx->aggvtxq[tid_tx->aggTail].arrivalTime;
+
+
+            if((tick - arrivalTime) <= ZM_AGG_CLEAR_TIME)
+            {
+
+            }
+            else if((tid_tx->size = zm_agg_qlen(dev, tid_tx->aggHead, tid_tx->aggTail)) > 0)
+            {
+
+                tid_tx->clearFlag = 1;
+
+                //zm_msg1_agg(ZM_LV_0, "clear queue    tick =", tick);
+                //zm_msg1_agg(ZM_LV_0, "clear queue arrival =", arrivalTime);
+
+
+                //zmw_leave_critical_section(dev);
+                //zfAggTxScheduler(dev);
+                //zmw_enter_critical_section(dev);
+
+            }
+
+            if (tid_tx->size == 0)
+            {
+                /*
+                 * queue empty
+                 */
+                if (tick - tid_tx->lastArrival > ZM_AGG_DELETE_TIME)
+                {
+                    zm_msg1_agg(ZM_LV_0, "delete queue, idle for n sec. n = ", \
+                            ZM_AGG_DELETE_TIME/10);
+
+                    zmw_leave_critical_section(dev);
+                    zfAggTxDeleteQueue(dev, i);
+                    zmw_enter_critical_section(dev);
+                }
+            }
+
+            zmw_leave_critical_section(dev);
+        }
+    }
+
+        zfAggRxClear(dev, time);
+
+#ifdef ZM_AGG_TALLY
+    if((wd->tick % 100) == 0) {
+        zfAggPrintTally(dev);
+    }
+#endif
+
+    return ZM_SUCCESS;
+}
+
+u16_t   zfAggPrintTally(zdev_t* dev)
+{
+    struct aggTally* agg_tal;
+
+    zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+    agg_tal = &wd->agg_tal;
+
+    if(agg_tal->got_packets_sum < 10)
+    {
+        zfAggTallyReset(dev);
+        return 0;
+    }
+
+    agg_tal->time++;
+    agg_tal->avg_got_packets = (agg_tal->avg_got_packets * (agg_tal->time - 1) +
+            agg_tal->got_packets_sum) / agg_tal->time;
+    agg_tal->avg_got_bytes = (agg_tal->avg_got_bytes * (agg_tal->time - 1) +
+            agg_tal->got_bytes_sum) / agg_tal->time;
+    agg_tal->avg_sent_packets = (agg_tal->avg_sent_packets * (agg_tal->time - 1)
+            + agg_tal->sent_packets_sum) / agg_tal->time;
+    agg_tal->avg_sent_bytes = (agg_tal->avg_sent_bytes * (agg_tal->time - 1) +
+            agg_tal->sent_bytes_sum) / agg_tal->time;
+    zm_msg1_agg(ZM_LV_0, "got_packets_sum =", agg_tal->got_packets_sum);
+    zm_msg1_agg(ZM_LV_0, "  got_bytes_sum =", agg_tal->got_bytes_sum);
+    zm_msg1_agg(ZM_LV_0, "sent_packets_sum=", agg_tal->sent_packets_sum);
+    zm_msg1_agg(ZM_LV_0, " sent_bytes_sum =", agg_tal->sent_bytes_sum);
+    agg_tal->got_packets_sum = agg_tal->got_bytes_sum =agg_tal->sent_packets_sum
+                = agg_tal->sent_bytes_sum = 0;
+    zm_msg1_agg(ZM_LV_0, "avg_got_packets =", agg_tal->avg_got_packets);
+    zm_msg1_agg(ZM_LV_0, "  avg_got_bytes =", agg_tal->avg_got_bytes);
+    zm_msg1_agg(ZM_LV_0, "avg_sent_packets=", agg_tal->avg_sent_packets);
+    zm_msg1_agg(ZM_LV_0, " avg_sent_bytes =", agg_tal->avg_sent_bytes);
+    if ((wd->commTally.BA_Fail == 0) || (wd->commTally.Hw_Tx_MPDU == 0))
+    {
+        zm_msg1_agg(ZM_LV_0, "Hardware Tx MPDU=", wd->commTally.Hw_Tx_MPDU);
+        zm_msg1_agg(ZM_LV_0, "  BA Fail number=", wd->commTally.BA_Fail);
+    }
+    else
+        zm_msg1_agg(ZM_LV_0, "1/(BA fail rate)=", wd->commTally.Hw_Tx_MPDU/wd->commTally.BA_Fail);
+
+    return 0;
+}
+
+u16_t zfAggRxClear(zdev_t* dev, u32_t time)
+{
+    u16_t   i;
+    struct agg_tid_rx *tid_rx;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    for (i=0; i<ZM_AGG_POOL_SIZE; i++)
+    {
+        zmw_enter_critical_section(dev);
+        tid_rx = wd->tid_rx[i];
+        if (tid_rx->baw_head != tid_rx->baw_tail)
+        {
+            u16_t j = tid_rx->baw_tail;
+            while ((j != tid_rx->baw_head) && !tid_rx->frame[j].buf) {
+            	j = (j + 1) & ZM_AGG_BAW_MASK;
+            }
+            if ((j != tid_rx->baw_head) && (time - tid_rx->frame[j].arrivalTime) >
+                    (ZM_AGG_CLEAR_TIME - 5))
+            {
+                zmw_leave_critical_section(dev);
+                zm_msg0_agg(ZM_LV_1, "queue RxFlush by RxClear");
+                zfAggRxFlush(dev, 0, tid_rx);
+                zmw_enter_critical_section(dev);
+            }
+        }
+        zmw_leave_critical_section(dev);
+    }
+
+    return ZM_SUCCESS;
+}
+
+struct agg_tid_rx* zfAggRxEnabled(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t   dst0, src[3], ac, aid, fragOff;
+    u8_t    up;
+    u16_t   offset = 0;
+    u16_t   seq_no;
+    u16_t frameType;
+    u16_t frameCtrl;
+    u16_t frameSubtype;
+    u32_t tcp_seq;
+    //struct aggSta *agg_sta;
+#if ZM_AGG_FPGA_REORDERING
+    struct agg_tid_rx *tid_rx;
+#endif
+    zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+    seq_no = zmw_rx_buf_readh(dev, buf, 22) >> 4;
+    //DbgPrint("Rx seq=%d\n", seq_no);
+    if (wd->sta.EnableHT == 0)
+    {
+        return NULL;
+    }
+
+    frameCtrl = zmw_rx_buf_readb(dev, buf, 0);
+    frameType = frameCtrl & 0xf;
+    frameSubtype = frameCtrl & 0xf0;
+
+
+    if (frameType != ZM_WLAN_DATA_FRAME) //non-Qos Data? (frameSubtype&0x80)
+    {
+        return NULL;
+    }
+#ifdef ZM_ENABLE_PERFORMANCE_EVALUATION
+    tcp_seq = zmw_rx_buf_readb(dev, buf, 22+36) << 24;
+    tcp_seq += zmw_rx_buf_readb(dev, buf, 22+37) << 16;
+    tcp_seq += zmw_rx_buf_readb(dev, buf, 22+38) << 8;
+    tcp_seq += zmw_rx_buf_readb(dev, buf, 22+39);
+#endif
+
+    ZM_SEQ_DEBUG("In                   %5d, %12u\n", seq_no, tcp_seq);
+    dst0 = zmw_rx_buf_readh(dev, buf, offset+4);
+
+    src[0] = zmw_rx_buf_readh(dev, buf, offset+10);
+    src[1] = zmw_rx_buf_readh(dev, buf, offset+12);
+    src[2] = zmw_rx_buf_readh(dev, buf, offset+14);
+
+#if ZM_AGG_FPGA_DEBUG
+    aid = 0;
+#else
+    aid = zfApFindSta(dev, src);
+#endif
+
+    //agg_sta = &wd->aggSta[aid];
+    //zfTxGetIpTosAndFrag(dev, buf, &up, &fragOff);
+    //ac = zcUpToAc[up&0x7] & 0x3;
+
+    /*
+     * Filter unicast frame only, aid == 0 is for debug only
+     */
+    if ((dst0 & 0x1) == 0 && aid == 0)
+    {
+#if ZM_AGG_FPGA_REORDERING
+        tid_rx = zfAggRxGetQueue(dev, buf) ;
+        if(!tid_rx)
+            return NULL;
+        else
+        {
+            //if (tid_rx->addBaExchangeStatusCode == ZM_AGG_ADDBA_RESPONSE)
+            return tid_rx;
+        }
+#else
+        return NULL;
+#endif
+    }
+
+    return NULL;
+}
+
+u16_t zfAggRx(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo *addInfo, struct agg_tid_rx *tid_rx)
+{
+    u16_t   seq_no;
+    s16_t   index;
+    u16_t   offset = 0;
+    zbuf_t* pbuf;
+    u8_t    frameSubType;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    ZM_BUFFER_TRACE(dev, buf)
+
+    ZM_PERFORMANCE_RX_REORDER(dev);
+
+    seq_no = zmw_rx_buf_readh(dev, buf, offset+22) >> 4;
+
+    index = seq_no - tid_rx->seq_start;
+    /*
+     * for debug
+     */
+
+    /* zm_msg2_agg(ZM_LV_0, "queue seq = ", seq_no);
+     * DbgPrint("%s:%s%lxh %s%lxh\n", __FUNCTION__, "queue seq=", seq_no,
+     *   "; seq_start=", tid_rx->seq_start);
+     */
+
+    //DbgPrint("seq_no=%d, seq_start=%d\n", seq_no, tid_rx->seq_start);
+
+    /* In some APs, we found that it might transmit NULL data whose sequence number
+       is out or order. In order to avoid this problem, we ignore these NULL data.
+     */
+
+    frameSubType = (zmw_rx_buf_readh(dev, buf, 0) & 0xF0) >> 4;
+
+    /* If this is a NULL data instead of Qos NULL data */
+    if ((frameSubType & 0x0C) == 0x04)
+    {
+        s16_t seq_diff;
+
+        seq_diff = (seq_no > tid_rx->seq_start) ?
+                       seq_no - tid_rx->seq_start : tid_rx->seq_start - seq_no;
+
+        if (seq_diff > ZM_AGG_BAW_SIZE)
+        {
+            zm_debug_msg0("Free Rx NULL data in zfAggRx");
+
+            /* Free Rx buffer */
+            zfwBufFree(dev, buf, 0);
+            return ZM_ERR_OUT_OF_ORDER_NULL_DATA;
+        }
+    }
+
+    /*
+     * sequence number wrap at 4k
+     */
+    if (tid_rx->seq_start > seq_no)
+    {
+        //index += 4096;
+
+        zmw_enter_critical_section(dev);
+        if (tid_rx->seq_start >= 4096) {
+            tid_rx->seq_start = 0;
+        }
+        zmw_leave_critical_section(dev);
+
+    }
+
+    if (tid_rx->seq_start == seq_no) {
+    	zmw_enter_critical_section(dev);
+    	if (((tid_rx->baw_head - tid_rx->baw_tail) & ZM_AGG_BAW_MASK) > 0) {
+    	    //DbgPrint("head=%d, tail=%d", tid_rx->baw_head, tid_rx->baw_tail);
+            tid_rx->baw_tail = (tid_rx->baw_tail + 1) & ZM_AGG_BAW_MASK;
+        }
+        tid_rx->seq_start = (tid_rx->seq_start + 1) & (4096 - 1);
+    	zmw_leave_critical_section(dev);
+
+        ZM_PERFORMANCE_RX_SEQ(dev, buf);
+
+    	if (wd->zfcbRecv80211 != NULL) {
+            //seq_no = zmw_rx_buf_readh(dev, buf, offset+22) >> 4;
+            //DbgPrint("Recv indicate seq=%d\n", seq_no);
+            //DbgPrint("1. seq=%d\n", seq_no);
+
+            wd->zfcbRecv80211(dev, buf, addInfo);
+        }
+        else {
+            zfiRecv80211(dev, buf, addInfo);
+        }
+    }
+    else if (!zfAggRxEnqueue(dev, buf, tid_rx, addInfo))
+    {
+        /*
+         * duplicated packet
+         */
+        return 1;
+    }
+
+    while (tid_rx->baw_head != tid_rx->baw_tail) {// && tid_rx->frame[tid_rx->baw_tail].buf)
+        u16_t tailIndex;
+
+        zmw_enter_critical_section(dev);
+
+        tailIndex = tid_rx->baw_tail;
+        pbuf = tid_rx->frame[tailIndex].buf;
+        tid_rx->frame[tailIndex].buf = 0;
+        if (!pbuf)
+        {
+            zmw_leave_critical_section(dev);
+            break;
+        }
+
+        tid_rx->baw_tail = (tid_rx->baw_tail + 1) & ZM_AGG_BAW_MASK;
+        tid_rx->seq_start = (tid_rx->seq_start + 1) & (4096 - 1);
+
+
+        //if(pbuf && tid_rx->baw_size > 0)
+        //    tid_rx->baw_size--;
+
+        zmw_leave_critical_section(dev);
+
+        ZM_PERFORMANCE_RX_SEQ(dev, pbuf);
+
+        if (wd->zfcbRecv80211 != NULL)
+        {
+            //seq_no = zmw_rx_buf_readh(dev, pbuf, offset+22) >> 4;
+            //DbgPrint("Recv indicate seq=%d\n", seq_no);
+            //DbgPrint("1. seq=%d\n", seq_no);
+            wd->zfcbRecv80211(dev, pbuf, addInfo);
+        }
+        else
+        {
+            //seq_no = zmw_rx_buf_readh(dev, pbuf, offset+22) >> 4;
+            //DbgPrint("Recv indicate seq=%d\n", seq_no);
+            zfiRecv80211(dev, pbuf, addInfo);
+        }
+    }
+
+    return 1;
+}
+
+struct agg_tid_rx *zfAggRxGetQueue(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t   src[3];
+    u16_t   aid, ac, i;
+    u16_t   offset = 0;
+    struct agg_tid_rx *tid_rx = NULL;
+
+    zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+    src[0] = zmw_rx_buf_readh(dev, buf, offset+10);
+    src[1] = zmw_rx_buf_readh(dev, buf, offset+12);
+    src[2] = zmw_rx_buf_readh(dev, buf, offset+14);
+    aid = zfApFindSta(dev, src);
+
+    ac = (zmw_rx_buf_readh(dev, buf, 24) & 0xF);
+
+    // mark by spin lock debug
+    //zmw_enter_critical_section(dev);
+
+    for (i=0; i<ZM_AGG_POOL_SIZE ; i++)
+    {
+        if((wd->tid_rx[i]->aid == aid) && (wd->tid_rx[i]->ac == ac))
+        {
+            tid_rx = wd->tid_rx[i];
+            break;
+        }
+    }
+
+    // mark by spin lock debug
+    //zmw_leave_critical_section(dev);
+    return tid_rx;
+}
+
+
+u16_t   zfAggRxEnqueue(zdev_t* dev, zbuf_t* buf, struct agg_tid_rx *tid_rx, struct zsAdditionInfo *addInfo)
+{
+    u16_t seq_no, offset = 0;
+    u16_t q_index;
+    s16_t index;
+    u8_t  bdropframe = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    ZM_BUFFER_TRACE(dev, buf)
+
+    seq_no = zmw_rx_buf_readh(dev, buf, offset+22) >> 4;
+    index  = seq_no - tid_rx->seq_start;
+
+    /*
+     * sequence number wrap at 4k
+     * -1000: check for duplicate past packet
+     */
+    bdropframe = 0;
+    if (tid_rx->seq_start > seq_no) {
+        if ((tid_rx->seq_start > 3967) && (seq_no < 128)) {
+            index += 4096;
+        } else if (tid_rx->seq_start - seq_no > 70) {
+            zmw_enter_critical_section(dev);
+            tid_rx->sq_behind_count++;
+            if (tid_rx->sq_behind_count > 3) {
+                tid_rx->sq_behind_count = 0;
+            } else {
+                bdropframe = 1;
+            }
+            zmw_leave_critical_section(dev);
+        } else {
+            bdropframe = 1;
+        }
+    } else {
+        if (seq_no - tid_rx->seq_start > 70) {
+            zmw_enter_critical_section(dev);
+            tid_rx->sq_exceed_count++;
+            if (tid_rx->sq_exceed_count > 3) {
+                tid_rx->sq_exceed_count = 0;
+            } else {
+                bdropframe = 1;
+            }
+            zmw_leave_critical_section(dev);
+        }
+    }
+
+    if (bdropframe == 1) {
+        /*if (wd->zfcbRecv80211 != NULL) {
+            wd->zfcbRecv80211(dev, buf, addInfo);
+        }
+        else {
+            zfiRecv80211(dev, buf, addInfo);
+        }*/
+
+        ZM_PERFORMANCE_FREE(dev, buf);
+
+        zfwBufFree(dev, buf, 0);
+        /*zfAggRxFlush(dev, seq_no, tid_rx);
+        tid_rx->seq_start = seq_no;
+        index = seq_no - tid_rx->seq_start;
+        */
+
+        //DbgPrint("Free an old packet, seq_start=%d, seq_no=%d\n", tid_rx->seq_start, seq_no);
+
+        /*
+         * duplicate past packet
+         * happens only in simulated aggregation environment
+         */
+        return 0;
+    } else {
+        zmw_enter_critical_section(dev);
+        if (tid_rx->sq_exceed_count > 0){
+            tid_rx->sq_exceed_count--;
+        }
+
+        if (tid_rx->sq_behind_count > 0) {
+            tid_rx->sq_behind_count--;
+        }
+        zmw_leave_critical_section(dev);
+    }
+
+    if (index < 0) {
+        zfAggRxFlush(dev, seq_no, tid_rx);
+        tid_rx->seq_start = seq_no;
+        index = 0;
+    }
+
+    //if (index >= (ZM_AGG_BAW_SIZE - 1))
+    if (index >= (ZM_AGG_BAW_MASK))
+    {
+        /*
+         * queue full
+         */
+        //DbgPrint("index >= 64, seq_start=%d, seq_no=%d\n", tid_rx->seq_start, seq_no);
+        zfAggRxFlush(dev, seq_no, tid_rx);
+        //tid_rx->seq_start = seq_no;
+        index = seq_no - tid_rx->seq_start;
+        if ((tid_rx->seq_start > seq_no) && (tid_rx->seq_start > 1000) && (tid_rx->seq_start - 1000) > seq_no)
+        {
+        //index = seq_no - tid_rx->seq_start;
+            index += 4096;
+        }
+        //index = seq_no - tid_rx->seq_start;
+        while (index >= (ZM_AGG_BAW_MASK)) {
+            //DbgPrint("index >= 64, seq_start=%d, seq_no=%d\n", tid_rx->seq_start, seq_no);
+            tid_rx->seq_start = (tid_rx->seq_start + ZM_AGG_BAW_MASK) & (4096 - 1);
+            index = seq_no - tid_rx->seq_start;
+            if ((tid_rx->seq_start > seq_no) && (tid_rx->seq_start > 1000) && (tid_rx->seq_start - 1000) > seq_no)
+            {
+                index += 4096;
+            }
+        }
+    }
+
+
+    q_index = (tid_rx->baw_tail + index) & ZM_AGG_BAW_MASK;
+    if (tid_rx->frame[q_index].buf && (((tid_rx->baw_head - tid_rx->baw_tail) & ZM_AGG_BAW_MASK) >
+                (((q_index) - tid_rx->baw_tail) & ZM_AGG_BAW_MASK)))
+    {
+
+        ZM_PERFORMANCE_DUP(dev, tid_rx->frame[q_index].buf, buf);
+        zfwBufFree(dev, buf, 0);
+        //DbgPrint("Free a duplicate packet, seq_start=%d, seq_no=%d\n", tid_rx->seq_start, seq_no);
+        //DbgPrint("head=%d, tail=%d", tid_rx->baw_head, tid_rx->baw_tail);
+        /*
+         * duplicate packet
+         */
+        return 0;
+    }
+
+    zmw_enter_critical_section(dev);
+    if(tid_rx->frame[q_index].buf) {
+        zfwBufFree(dev, tid_rx->frame[q_index].buf, 0);
+        tid_rx->frame[q_index].buf = 0;
+    }
+
+    tid_rx->frame[q_index].buf = buf;
+    tid_rx->frame[q_index].arrivalTime = zm_agg_GetTime();
+    zfwMemoryCopy((void*)&tid_rx->frame[q_index].addInfo, (void*)addInfo, sizeof(struct zsAdditionInfo));
+
+    /*
+     * for debug simulated aggregation only,
+     * should be done in rx of ADDBA Request
+     */
+    //tid_rx->addInfo = addInfo;
+
+
+    if (((tid_rx->baw_head - tid_rx->baw_tail) & ZM_AGG_BAW_MASK) <= index)
+    {
+        //tid_rx->baw_size = index + 1;
+        if (((tid_rx->baw_head - tid_rx->baw_tail) & ZM_AGG_BAW_MASK) <=
+                //((q_index + 1) & ZM_AGG_BAW_MASK))
+                (((q_index) - tid_rx->baw_tail) & ZM_AGG_BAW_MASK))//tid_rx->baw_size )
+            tid_rx->baw_head = (q_index + 1) & ZM_AGG_BAW_MASK;
+    }
+    zmw_leave_critical_section(dev);
+
+    /*
+     * success
+     */
+    //DbgPrint("head=%d, tail=%d, start=%d", tid_rx->baw_head, tid_rx->baw_tail, tid_rx->seq_start);
+    return 1;
+}
+
+u16_t zfAggRxFlush(zdev_t* dev, u16_t seq_no, struct agg_tid_rx *tid_rx)
+{
+    zbuf_t* pbuf;
+    u16_t   seq;
+    struct zsAdditionInfo addInfo;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    ZM_PERFORMANCE_RX_FLUSH(dev);
+
+    while (1)
+    {
+        zmw_enter_critical_section(dev);
+        if (tid_rx->baw_tail == tid_rx->baw_head) {
+            zmw_leave_critical_section(dev);
+            break;
+        }
+
+        pbuf = tid_rx->frame[tid_rx->baw_tail].buf;
+        zfwMemoryCopy((void*)&addInfo, (void*)&tid_rx->frame[tid_rx->baw_tail].addInfo, sizeof(struct zsAdditionInfo));
+        tid_rx->frame[tid_rx->baw_tail].buf = 0;
+        //if(pbuf && tid_rx->baw_size > 0) tid_rx->baw_size--;
+        tid_rx->baw_tail = (tid_rx->baw_tail + 1) & ZM_AGG_BAW_MASK;
+        tid_rx->seq_start = (tid_rx->seq_start + 1) & (4096 - 1);
+	    zmw_leave_critical_section(dev);
+
+        if (pbuf)
+        {
+
+            ZM_PERFORMANCE_RX_SEQ(dev, pbuf);
+
+            if (wd->zfcbRecv80211 != NULL)
+            {
+                seq = zmw_rx_buf_readh(dev, pbuf, 22) >> 4;
+                //DbgPrint("Recv indicate seq=%d\n", seq);
+                //DbgPrint("2. seq=%d\n", seq);
+                wd->zfcbRecv80211(dev, pbuf, &addInfo);
+            }
+            else
+            {
+                seq = zmw_rx_buf_readh(dev, pbuf, 22) >> 4;
+                //DbgPrint("Recv indicate seq=%d\n", seq);
+                zfiRecv80211(dev, pbuf, &addInfo);
+            }
+        }
+    }
+
+    zmw_enter_critical_section(dev);
+    tid_rx->baw_head = tid_rx->baw_tail = 0;
+    zmw_leave_critical_section(dev);
+    return 1;
+}
+
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggRxFreeBuf              */
+/*      Frees all queued packets in buffer when the driver is down.     */
+/*      The zfFreeResource() will check if the buffer is all freed.     */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev     : device pointer                                        */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      ZM_SUCCESS                                                      */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Honda               Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t   zfAggRxFreeBuf(zdev_t* dev, u16_t destroy)
+{
+    u16_t   i;
+    zbuf_t* buf;
+    struct agg_tid_rx *tid_rx;
+
+    TID_TX  tid_tx;
+    //struct bufInfo *buf_info;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    for (i=0; i<ZM_AGG_POOL_SIZE; i++)
+    {
+        u16_t j;
+
+        tid_rx = wd->tid_rx[i];
+
+        for(j=0; j <= ZM_AGG_BAW_SIZE; j++)
+        {
+            zmw_enter_critical_section(dev);
+            buf = tid_rx->frame[j].buf;
+            tid_rx->frame[j].buf = 0;
+            zmw_leave_critical_section(dev);
+
+            if (buf)
+            {
+                zfwBufFree(dev, buf, 0);
+            }
+        }
+
+        #if 0
+        if ( tid_rx->baw_head != tid_rx->baw_tail )
+        {
+            while (tid_rx->baw_head != tid_rx->baw_tail)
+            {
+                buf = tid_rx->frame[tid_rx->baw_tail].buf;
+                tid_rx->frame[tid_rx->baw_tail].buf = 0;
+                if (buf)
+                {
+                    zfwBufFree(dev, buf, 0);
+
+                    zmw_enter_critical_section(dev);
+                    tid_rx->frame[tid_rx->baw_tail].buf = 0;
+                    zmw_leave_critical_section(dev);
+                }
+                zmw_enter_critical_section(dev);
+                //if (tid_rx->baw_size > 0)tid_rx->baw_size--;
+                tid_rx->baw_tail = (tid_rx->baw_tail + 1) & ZM_AGG_BAW_MASK;
+                tid_rx->seq_start++;
+                zmw_leave_critical_section(dev);
+            }
+        }
+        #endif
+
+        zmw_enter_critical_section(dev);
+        tid_rx->seq_start = 0;
+        tid_rx->baw_head = tid_rx->baw_tail = 0;
+        tid_rx->aid = ZM_MAX_STA_SUPPORT;
+        zmw_leave_critical_section(dev);
+
+        #ifdef ZM_ENABLE_AGGREGATION
+        #ifndef ZM_ENABLE_FW_BA_RETRANSMISSION //disable BAW
+        if (tid_baw->enabled) {
+            zm_msg1_agg(ZM_LV_0, "Device down, clear BAW queue:", i);
+            BAW->disable(dev, tid_baw);
+        }
+        #endif
+        #endif
+        if (1 == wd->aggQPool[i]->aggQEnabled) {
+            tid_tx = wd->aggQPool[i];
+            buf = zfAggTxGetVtxq(dev, tid_tx);
+            while (buf) {
+                zfwBufFree(dev, buf, 0);
+                buf = zfAggTxGetVtxq(dev, tid_tx);
+            }
+        }
+
+        if(destroy) {
+            zfwMemFree(dev, wd->aggQPool[i], sizeof(struct aggQueue));
+            zfwMemFree(dev, wd->tid_rx[i], sizeof(struct agg_tid_rx));
+        }
+    }
+    #ifdef ZM_ENABLE_AGGREGATION
+    #ifndef ZM_ENABLE_FW_BA_RETRANSMISSION //disable BAW
+    if(destroy) zfwMemFree(dev, BAW, sizeof(struct baw_enabler));
+    #endif
+    #endif
+    return ZM_SUCCESS;
+}
+
+
+void zfAggRecvBAR(zdev_t* dev, zbuf_t *buf) {
+    u16_t start_seq, len;
+    u8_t i, bitmap[8];
+    len = zfwBufGetSize(dev, buf);
+    start_seq = zmw_rx_buf_readh(dev, buf, len-2);
+    DbgPrint("Received a BAR Control frame, start_seq=%d", start_seq>>4);
+    /* todo: set the bitmap by reordering buffer! */
+    for (i=0; i<8; i++) bitmap[i]=0;
+    zfSendBA(dev, start_seq, bitmap);
+}
+
+#ifdef ZM_ENABLE_AGGREGATION
+#ifndef ZM_ENABLE_FW_BA_RETRANSMISSION //disable BAW
+void zfAggTxRetransmit(zdev_t* dev, struct bufInfo *buf_info, struct aggControl *aggControl, TID_TX tid_tx) {
+    u16_t removeLen;
+    u16_t err;
+
+    zmw_get_wlan_dev(dev);
+    if (aggControl && (ZM_AGG_FIRST_MPDU == aggControl->ampduIndication) ) {
+        tid_tx->bar_ssn = buf_info->baw_header->header[15];
+        aggControl->tid_baw->start_seq = tid_tx->bar_ssn >> 4;
+        zm_msg1_agg(ZM_LV_0, "start seq=", tid_tx->bar_ssn >> 4);
+    }
+    buf_info->baw_header->header[4] |= (1 << 11);
+    if (aggControl && aggControl->aggEnabled) {
+        //if (wd->enableAggregation==0 && !(buf_info->baw_header->header[6]&0x1))
+        //{
+            //if (((buf_info->baw_header->header[2] & 0x3) == 2))
+            //{
+                /* Enable aggregation */
+                buf_info->baw_header->header[1] |= 0x20;
+                if (ZM_AGG_LAST_MPDU == aggControl->ampduIndication) {
+                    buf_info->baw_header->header[1] |= 0x4000;
+                }
+                else {
+                    buf_info->baw_header->header[1] &= ~0x4000;
+                    //zm_debug_msg0("ZM_AGG_LAST_MPDU");
+                }
+            //}
+            //else {
+            //    zm_debug_msg1("no aggr, header[2]&0x3 = ",buf_info->baw_header->header[2] & 0x3)
+            //    aggControl->aggEnabled = 0;
+            //}
+        //}
+        //else {
+        //    zm_debug_msg1("no aggr, wd->enableAggregation = ", wd->enableAggregation);
+        //    zm_debug_msg1("no aggr, !header[6]&0x1 = ",!(buf_info->baw_header->header[6]&0x1));
+        //    aggControl->aggEnabled = 0;
+        //}
+    }
+
+    /*if (aggControl->tid_baw) {
+        struct baw_header_r header_r;
+
+        header_r.header      = buf_info->baw_header->header;
+        header_r.mic         = buf_info->baw_header->mic;
+        header_r.snap        = buf_info->baw_header->snap;
+        header_r.headerLen   = buf_info->baw_header->headerLen;
+        header_r.micLen      = buf_info->baw_header->micLen;
+        header_r.snapLen     = buf_info->baw_header->snapLen;
+        header_r.removeLen   = buf_info->baw_header->removeLen;
+        header_r.keyIdx      = buf_info->baw_header->keyIdx;
+
+        BAW->insert(dev, buf_info->buf, tid_tx->bar_ssn >> 4, aggControl->tid_baw, buf_info->baw_retransmit, &header_r);
+    }*/
+
+    if ((err = zfHpSend(dev,
+                    buf_info->baw_header->header,
+                    buf_info->baw_header->headerLen,
+                    buf_info->baw_header->snap,
+                    buf_info->baw_header->snapLen,
+                    buf_info->baw_header->mic,
+                    buf_info->baw_header->micLen,
+                    buf_info->buf,
+                    buf_info->baw_header->removeLen,
+                    ZM_EXTERNAL_ALLOC_BUF,
+                    (u8_t)tid_tx->ac,
+                    buf_info->baw_header->keyIdx)) != ZM_SUCCESS)
+    {
+        goto zlError;
+    }
+
+    return;
+
+zlError:
+    zfwBufFree(dev, buf_info->buf, 0);
+    return;
+
+}
+#endif //disable BAW
+#endif
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAggTxSendEth              */
+/*      Called to transmit Ethernet frame from upper elayer.            */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer pointer                                            */
+/*      port : WLAN port, 0=>standard, 0x10-0x17=>VAP, 0x20-0x25=>WDS   */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      error code                                                      */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen, Honda      Atheros Communications, Inc.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfAggTxSendEth(zdev_t* dev, zbuf_t* buf, u16_t port, u16_t bufType, u8_t flag, struct aggControl *aggControl, TID_TX tid_tx)
+{
+    u16_t err;
+    //u16_t addrTblSize;
+    //struct zsAddrTbl addrTbl;
+    u16_t removeLen;
+    u16_t header[(8+30+2+18)/2];    /* ctr+(4+a1+a2+a3+2+a4)+qos+iv */
+    u16_t headerLen;
+    u16_t mic[8/2];
+    u16_t micLen;
+    u16_t snap[8/2];
+    u16_t snapLen;
+    u16_t fragLen;
+    u16_t frameLen;
+    u16_t fragNum;
+    struct zsFrag frag;
+    u16_t i, id;
+    u16_t da[3];
+    u16_t sa[3];
+    u8_t up;
+    u8_t qosType, keyIdx = 0;
+    u16_t fragOff;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zm_msg1_tx(ZM_LV_2, "zfTxSendEth(), port=", port);
+
+    /* Get IP TOS for QoS AC and IP frag offset */
+    zfTxGetIpTosAndFrag(dev, buf, &up, &fragOff);
+
+#ifdef ZM_ENABLE_NATIVE_WIFI
+    if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+    {
+        /* DA */
+        da[0] = zmw_tx_buf_readh(dev, buf, 16);
+        da[1] = zmw_tx_buf_readh(dev, buf, 18);
+        da[2] = zmw_tx_buf_readh(dev, buf, 20);
+        /* SA */
+        sa[0] = zmw_tx_buf_readh(dev, buf, 10);
+        sa[1] = zmw_tx_buf_readh(dev, buf, 12);
+        sa[2] = zmw_tx_buf_readh(dev, buf, 14);
+    }
+    else if ( wd->wlanMode == ZM_MODE_IBSS )
+    {
+        /* DA */
+        da[0] = zmw_tx_buf_readh(dev, buf, 4);
+        da[1] = zmw_tx_buf_readh(dev, buf, 6);
+        da[2] = zmw_tx_buf_readh(dev, buf, 8);
+        /* SA */
+        sa[0] = zmw_tx_buf_readh(dev, buf, 10);
+        sa[1] = zmw_tx_buf_readh(dev, buf, 12);
+        sa[2] = zmw_tx_buf_readh(dev, buf, 14);
+    }
+    else if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        /* DA */
+        da[0] = zmw_tx_buf_readh(dev, buf, 4);
+        da[1] = zmw_tx_buf_readh(dev, buf, 6);
+        da[2] = zmw_tx_buf_readh(dev, buf, 8);
+        /* SA */
+        sa[0] = zmw_tx_buf_readh(dev, buf, 16);
+        sa[1] = zmw_tx_buf_readh(dev, buf, 18);
+        sa[2] = zmw_tx_buf_readh(dev, buf, 20);
+    }
+    else
+    {
+        //
+    }
+#else
+    /* DA */
+    da[0] = zmw_tx_buf_readh(dev, buf, 0);
+    da[1] = zmw_tx_buf_readh(dev, buf, 2);
+    da[2] = zmw_tx_buf_readh(dev, buf, 4);
+    /* SA */
+    sa[0] = zmw_tx_buf_readh(dev, buf, 6);
+    sa[1] = zmw_tx_buf_readh(dev, buf, 8);
+    sa[2] = zmw_tx_buf_readh(dev, buf, 10);
+#endif
+    //Decide Key Index in ATOM, No meaning in OTUS--CWYang(m)
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        keyIdx = wd->ap.bcHalKeyIdx[port];
+        id = zfApFindSta(dev, da);
+        if (id != 0xffff)
+        {
+            switch (wd->ap.staTable[id].encryMode)
+            {
+            case ZM_AES:
+            case ZM_TKIP:
+#ifdef ZM_ENABLE_CENC
+            case ZM_CENC:
+#endif //ZM_ENABLE_CENC
+                keyIdx = wd->ap.staTable[id].keyIdx;
+                break;
+            }
+        }
+    }
+    else
+    {
+        switch (wd->sta.encryMode)
+        {
+        case ZM_WEP64:
+        case ZM_WEP128:
+        case ZM_WEP256:
+            keyIdx = wd->sta.keyId;
+            break;
+        case ZM_AES:
+        case ZM_TKIP:
+            if ((da[0]& 0x1))
+                keyIdx = 5;
+            else
+                keyIdx = 4;
+            break;
+#ifdef ZM_ENABLE_CENC
+        case ZM_CENC:
+            keyIdx = wd->sta.cencKeyId;
+            break;
+#endif //ZM_ENABLE_CENC
+        }
+    }
+
+    /* Create SNAP */
+    removeLen = zfTxGenWlanSnap(dev, buf, snap, &snapLen);
+    //zm_msg1_tx(ZM_LV_0, "fragOff=", fragOff);
+
+    fragLen = wd->fragThreshold;
+    frameLen = zfwBufGetSize(dev, buf);
+    frameLen -= removeLen;
+
+#if 0
+    /* Create MIC */
+    if ( (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)&&
+         (wd->sta.encryMode == ZM_TKIP) )
+    {
+        if ( frameLen > fragLen )
+        {
+            micLen = zfTxGenWlanTail(dev, buf, snap, snapLen, mic);
+        }
+        else
+        {
+            /* append MIC by HMAC */
+            micLen = 8;
+        }
+    }
+    else
+    {
+        micLen = 0;
+    }
+#else
+    if ( frameLen > fragLen )
+    {
+        micLen = zfTxGenWlanTail(dev, buf, snap, snapLen, mic);
+    }
+    else
+    {
+        /* append MIC by HMAC */
+        micLen = 0;
+    }
+#endif
+
+    /* Access Category */
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        zfApGetStaQosType(dev, da, &qosType);
+        if (qosType == 0)
+        {
+            up = 0;
+        }
+    }
+    else if (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)
+    {
+        if (wd->sta.wmeConnected == 0)
+        {
+            up = 0;
+        }
+    }
+    else
+    {
+        /* TODO : STA QoS control field */
+        up = 0;
+    }
+
+    /* Assign sequence number */
+    zmw_enter_critical_section(dev);
+    frag.seq[0] = ((wd->seq[zcUpToAc[up&0x7]]++) << 4);
+    if (aggControl && (ZM_AGG_FIRST_MPDU == aggControl->ampduIndication) ) {
+        tid_tx->bar_ssn = frag.seq[0];
+
+        zm_msg1_agg(ZM_LV_0, "start seq=", tid_tx->bar_ssn >> 4);
+    }
+    //tid_tx->baw_buf[tid_tx->baw_head-1].baw_seq=frag.seq[0];
+    zmw_leave_critical_section(dev);
+
+
+        frag.buf[0] = buf;
+        frag.bufType[0] = bufType;
+        frag.flag[0] = flag;
+        fragNum = 1;
+
+    for (i=0; i<fragNum; i++)
+    {
+        /* Create WLAN header(Control Setting + 802.11 header + IV) */
+        if (up !=0 ) zm_debug_msg1("up not 0, up=",up);
+        headerLen = zfTxGenWlanHeader(dev, frag.buf[i], header, frag.seq[i],
+                                      frag.flag[i], snapLen+micLen, removeLen,
+                                      port, da, sa, up, &micLen, snap, snapLen,
+                                      aggControl);
+
+        /* Get buffer DMA address */
+        //if ((addrTblSize = zfwBufMapDma(dev, frag.buf[i], &addrTbl)) == 0)
+        //if ((addrTblSize = zfwMapTxDma(dev, frag.buf[i], &addrTbl)) == 0)
+        //{
+        //    err = ZM_ERR_BUFFER_DMA_ADDR;
+        //    goto zlError;
+        //}
+
+        /* Flush buffer on cache */
+        //zfwBufFlush(dev, frag.buf[i]);
+
+#if 0
+        zm_msg1_tx(ZM_LV_0, "headerLen=", headerLen);
+        zm_msg1_tx(ZM_LV_0, "snapLen=", snapLen);
+        zm_msg1_tx(ZM_LV_0, "micLen=", micLen);
+        zm_msg1_tx(ZM_LV_0, "removeLen=", removeLen);
+        zm_msg1_tx(ZM_LV_0, "addrTblSize=", addrTblSize);
+        zm_msg1_tx(ZM_LV_0, "frag.bufType[0]=", frag.bufType[0]);
+#endif
+
+        fragLen = zfwBufGetSize(dev, frag.buf[i]);
+        if ((da[0]&0x1) == 0)
+        {
+            wd->commTally.txUnicastFrm++;
+            wd->commTally.txUnicastOctets += (fragLen+snapLen);
+        }
+        else if ((da[0]& 0x1))
+        {
+            wd->commTally.txBroadcastFrm++;
+            wd->commTally.txBroadcastOctets += (fragLen+snapLen);
+        }
+        else
+        {
+            wd->commTally.txMulticastFrm++;
+            wd->commTally.txMulticastOctets += (fragLen+snapLen);
+        }
+        wd->ledStruct.txTraffic++;
+
+#if 0 //Who care this?
+        if ( (i)&&(i == (fragNum-1)) )
+        {
+            wd->trafTally.txDataByteCount -= micLen;
+        }
+#endif
+
+        /*if (aggControl->tid_baw && aggControl->aggEnabled) {
+            struct baw_header_r header_r;
+
+            header_r.header      = header;
+            header_r.mic         = mic;
+            header_r.snap        = snap;
+            header_r.headerLen   = headerLen;
+            header_r.micLen      = micLen;
+            header_r.snapLen     = snapLen;
+            header_r.removeLen   = removeLen;
+            header_r.keyIdx      = keyIdx;
+
+            BAW->insert(dev, buf, tid_tx->bar_ssn >> 4, aggControl->tid_baw, 0, &header_r);
+        }*/
+
+        if ((err = zfHpSend(dev, header, headerLen, snap, snapLen,
+                             mic, micLen, frag.buf[i], removeLen,
+                             frag.bufType[i], zcUpToAc[up&0x7], keyIdx)) != ZM_SUCCESS)
+        {
+            goto zlError;
+        }
+
+
+        continue;
+
+zlError:
+        if (frag.bufType[i] == ZM_EXTERNAL_ALLOC_BUF)
+        {
+            zfwBufFree(dev, frag.buf[i], err);
+        }
+        else if (frag.bufType[i] == ZM_INTERNAL_ALLOC_BUF)
+        {
+            zfwBufFree(dev, frag.buf[i], 0);
+        }
+        else
+        {
+            zm_assert(0);
+        }
+    } /* for (i=0; i<fragNum; i++) */
+
+    return ZM_SUCCESS;
+}
+
+/*
+ * zfAggSendADDBA() refers zfSendMmFrame() in cmm.c
+ */
+u16_t   zfAggSendAddbaRequest(zdev_t* dev, u16_t *dst, u16_t ac, u16_t up)
+{
+    zbuf_t* buf;
+    //u16_t addrTblSize;
+    //struct zsAddrTbl addrTbl;
+    //u16_t err;
+    u16_t offset = 0;
+    u16_t hlen = 32;
+    u16_t header[(24+25+1)/2];
+    u16_t vap = 0;
+    u16_t i;
+    u8_t encrypt = 0;
+
+    //zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+
+    /*
+     * TBD : Maximum size of managment frame
+     */
+    if ((buf = zfwBufAllocate(dev, 1024)) == NULL)
+    {
+        zm_msg0_mm(ZM_LV_0, "Alloc mm buf Fail!");
+        return ZM_SUCCESS;
+    }
+
+    /*
+     * Reserve room for wlan header
+     */
+    offset = hlen;
+
+    /*
+     * add addba frame body
+     */
+    offset = zfAggSetAddbaFrameBody(dev, buf, offset, ac, up);
+
+
+    zfwBufSetSize(dev, buf, offset);
+
+    /*
+     * Copy wlan header
+     */
+    zfAggGenAddbaHeader(dev, dst, header, offset-hlen, buf, vap, encrypt);
+    for (i=0; i<(hlen>>1); i++)
+    {
+        zmw_tx_buf_writeh(dev, buf, i*2, header[i]);
+    }
+
+    /* Get buffer DMA address */
+    //if ((addrTblSize = zfwBufMapDma(dev, buf, &addrTbl)) == 0)
+    //if ((addrTblSize = zfwMapTxDma(dev, buf, &addrTbl)) == 0)
+    //{
+    //    goto zlError;
+    //}
+
+    //zm_msg2_mm(ZM_LV_2, "offset=", offset);
+    //zm_msg2_mm(ZM_LV_2, "hlen=", hlen);
+    //zm_msg2_mm(ZM_LV_2, "addrTblSize=", addrTblSize);
+    //zm_msg2_mm(ZM_LV_2, "addrTbl.len[0]=", addrTbl.len[0]);
+    //zm_msg2_mm(ZM_LV_2, "addrTbl.physAddrl[0]=", addrTbl.physAddrl[0]);
+    //zm_msg2_mm(ZM_LV_2, "buf->data=", buf->data);
+
+    #if 0
+    if ((err = zfHpSend(dev, NULL, 0, NULL, 0, NULL, 0, buf, 0,
+            ZM_INTERNAL_ALLOC_BUF, 0, 0xff)) != ZM_SUCCESS)
+    {
+        goto zlError;
+    }
+    #else
+    zfPutVmmq(dev, buf);
+    zfPushVtxq(dev);
+    #endif
+
+    return ZM_SUCCESS;
+
+}
+
+u16_t   zfAggSetAddbaFrameBody(zdev_t* dev, zbuf_t* buf, u16_t offset, u16_t ac, u16_t up)
+{
+    u16_t ba_parameter, start_seq;
+
+    zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+    /*
+     * ADDBA Request frame body
+     */
+
+    /*
+     * Category
+     */
+    zmw_tx_buf_writeb(dev, buf, offset++, 3);
+    /*
+     * Action details = 0
+     */
+    zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_ADDBA_REQUEST_FRAME);
+    /*
+     * Dialog Token = nonzero
+     * TBD: define how to get dialog token?
+     */
+    zmw_tx_buf_writeb(dev, buf, offset++, 2);
+    /*
+     * Block Ack parameter set
+     * BA policy = 1 for immediate BA, 0 for delayed BA
+     * TID(4bits) & buffer size(4bits) (TID=up & buffer size=0x80)
+     * TBD: how to get buffer size?
+     * ¢z¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢{
+     * ¢x    B0    ¢x    B1     ¢x B2  B5 ¢x B6      B15 ¢x
+     * ¢u¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢t
+     * ¢x Reserved ¢x BA policy ¢x  TID   ¢x Buffer size ¢x
+     * ¢|¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢}
+     */
+    ba_parameter = 1 << 12;     // buffer size = 0x40(64)
+    ba_parameter |= up << 2;    // tid = up
+    ba_parameter |= 2;          // ba policy = 1
+    zmw_tx_buf_writeh(dev, buf, offset, ba_parameter);
+    offset+=2;
+    /*
+     * BA timeout value
+     */
+    zmw_tx_buf_writeh(dev, buf, offset, 0);
+    offset+=2;
+    /*
+     * BA starting sequence number
+     * ¢z¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢{
+     * ¢x B0       B3 ¢x B4              B15 ¢x
+     * ¢u¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢t
+     * ¢x Frag num(0) ¢x BA starting seq num ¢x
+     * ¢|¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢}
+     */
+    start_seq = ((wd->seq[ac]) << 4) & 0xFFF0;
+    zmw_tx_buf_writeh(dev, buf, offset, start_seq);
+    offset+=2;
+
+    return offset;
+}
+
+u16_t zfAggGenAddbaHeader(zdev_t* dev, u16_t* dst,
+        u16_t* header, u16_t len, zbuf_t* buf, u16_t vap, u8_t encrypt)
+{
+    u8_t  hlen = 32;        // MAC ctrl + PHY ctrl + 802.11 MM header
+    //u8_t frameType = ZM_WLAN_FRAME_TYPE_ACTION;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    /*
+     * Generate control setting
+     */
+    //bodyLen = zfwBufGetSize(dev, buf);
+    header[0] = 24+len+4;   //Length
+    header[1] = 0x8;        //MAC control, backoff + (ack)
+
+#if 0
+    /* CCK 1M */
+    header[2] = 0x0f00;          //PHY control L
+    header[3] = 0x0000;          //PHY control H
+#else
+    /* OFDM 6M */
+    header[2] = 0x0f01;          //PHY control L
+    header[3] = 0x000B;          //PHY control H
+#endif
+
+    /*
+     * Generate WLAN header
+     * Frame control frame type and subtype
+     */
+    header[4+0] = ZM_WLAN_FRAME_TYPE_ACTION;
+    /*
+     * Duration
+     */
+    header[4+1] = 0;
+
+    if (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)
+    {
+        header[4+8] = wd->sta.bssid[0];
+        header[4+9] = wd->sta.bssid[1];
+        header[4+10] = wd->sta.bssid[2];
+    }
+    else if (wd->wlanMode == ZM_MODE_PSEUDO)
+    {
+        /* Address 3 = 00:00:00:00:00:00 */
+        header[4+8] = 0;
+        header[4+9] = 0;
+        header[4+10] = 0;
+    }
+    else if (wd->wlanMode == ZM_MODE_IBSS)
+    {
+        header[4+8] = wd->sta.bssid[0];
+        header[4+9] = wd->sta.bssid[1];
+        header[4+10] = wd->sta.bssid[2];
+    }
+    else if (wd->wlanMode == ZM_MODE_AP)
+    {
+        /* Address 3 = BSSID */
+        header[4+8] = wd->macAddr[0];
+        header[4+9] = wd->macAddr[1];
+        header[4+10] = wd->macAddr[2] + (vap<<8);
+    }
+
+    /* Address 1 = DA */
+    header[4+2] = dst[0];
+    header[4+3] = dst[1];
+    header[4+4] = dst[2];
+
+    /* Address 2 = SA */
+    header[4+5] = wd->macAddr[0];
+    header[4+6] = wd->macAddr[1];
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        header[4+7] = wd->macAddr[2] + (vap<<8);
+    }
+    else
+    {
+        header[4+7] = wd->macAddr[2];
+    }
+
+    /* Sequence Control */
+    zmw_enter_critical_section(dev);
+    header[4+11] = ((wd->mmseq++)<<4);
+    zmw_leave_critical_section(dev);
+
+
+    return hlen;
+}
+
+
+u16_t   zfAggProcessAction(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t category;
+
+    //zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+    category = zmw_rx_buf_readb(dev, buf, 24);
+
+    switch (category)
+    {
+    case ZM_WLAN_BLOCK_ACK_ACTION_FRAME:
+        zfAggBlockAckActionFrame(dev, buf);
+        break;
+
+    }
+
+    return ZM_SUCCESS;
+}
+
+
+u16_t   zfAggBlockAckActionFrame(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t action;
+
+    //zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+    action = zmw_rx_buf_readb(dev, buf, 25);
+#ifdef ZM_ENABLE_AGGREGATION
+    switch (action)
+    {
+    case ZM_WLAN_ADDBA_REQUEST_FRAME:
+        zm_msg0_agg(ZM_LV_0, "Received BA Action frame is ADDBA request");
+        zfAggRecvAddbaRequest(dev, buf);
+        break;
+    case ZM_WLAN_ADDBA_RESPONSE_FRAME:
+        zm_msg0_agg(ZM_LV_0, "Received BA Action frame is ADDBA response");
+        zfAggRecvAddbaResponse(dev, buf);
+        break;
+    case ZM_WLAN_DELBA_FRAME:
+        zfAggRecvDelba(dev, buf);
+        break;
+    }
+#endif
+    return ZM_SUCCESS;
+}
+
+u16_t   zfAggRecvAddbaRequest(zdev_t* dev, zbuf_t* buf)
+{
+    //u16_t dialog;
+    struct aggBaFrameParameter bf;
+    u16_t i;
+    //zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+    bf.buf = buf;
+    bf.dialog = zmw_rx_buf_readb(dev, buf, 26);
+    /*
+     * ba parameter set
+     */
+    bf.ba_parameter = zmw_rx_buf_readh(dev, buf, 27);
+    bf.ba_policy   = (bf.ba_parameter >> 1) & 1;
+    bf.tid         = (bf.ba_parameter >> 2) & 0xF;
+    bf.buffer_size = (bf.ba_parameter >> 6);
+    /*
+     * BA timeout value
+     */
+    bf.ba_timeout = zmw_rx_buf_readh(dev, buf, 29);
+    /*
+     * BA starting sequence number
+     */
+    bf.ba_start_seq = zmw_rx_buf_readh(dev, buf, 31) >> 4;
+
+    i=26;
+    while(i < 32) {
+        zm_debug_msg2("Recv ADDBA Req:", zmw_rx_buf_readb(dev,buf,i));
+        i++;
+    }
+
+    zfAggSendAddbaResponse(dev, &bf);
+
+    zfAggAddbaSetTidRx(dev, buf, &bf);
+
+    return ZM_SUCCESS;
+}
+
+u16_t   zfAggAddbaSetTidRx(zdev_t* dev, zbuf_t* buf, struct aggBaFrameParameter *bf)
+{
+    u16_t i, ac, aid, fragOff;
+    u16_t src[3];
+    u16_t offset = 0;
+    u8_t  up;
+    struct agg_tid_rx *tid_rx = NULL;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    src[0] = zmw_rx_buf_readh(dev, buf, offset+10);
+    src[1] = zmw_rx_buf_readh(dev, buf, offset+12);
+    src[2] = zmw_rx_buf_readh(dev, buf, offset+14);
+    aid = zfApFindSta(dev, src);
+
+    zfTxGetIpTosAndFrag(dev, buf, &up, &fragOff);
+    ac = zcUpToAc[up&0x7] & 0x3;
+
+    ac = bf->tid;
+
+    for (i=0; i<ZM_AGG_POOL_SIZE ; i++)
+    {
+        if((wd->tid_rx[i]->aid == aid) && (wd->tid_rx[i]->ac == ac))
+        {
+            tid_rx = wd->tid_rx[i];
+            break;
+        }
+    }
+
+    if (!tid_rx)
+    {
+        for (i=0; i<ZM_AGG_POOL_SIZE; i++)
+        {
+            if (wd->tid_rx[i]->aid == ZM_MAX_STA_SUPPORT)
+            {
+                tid_rx = wd->tid_rx[i];
+                break;
+            }
+        }
+        if (!tid_rx)
+            return 0;
+    }
+
+    zmw_enter_critical_section(dev);
+
+    tid_rx->aid = aid;
+    tid_rx->ac = ac;
+    tid_rx->addBaExchangeStatusCode = ZM_AGG_ADDBA_RESPONSE;
+    tid_rx->seq_start = bf->ba_start_seq;
+    tid_rx->baw_head = tid_rx->baw_tail = 0;
+    tid_rx->sq_exceed_count = tid_rx->sq_behind_count = 0;
+    zmw_leave_critical_section(dev);
+
+    return 0;
+}
+
+u16_t   zfAggRecvAddbaResponse(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t i,ac, aid=0;
+    u16_t src[3];
+    struct aggBaFrameParameter bf;
+
+    zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+    src[0] = zmw_rx_buf_readh(dev, buf, 10);
+    src[1] = zmw_rx_buf_readh(dev, buf, 12);
+    src[2] = zmw_rx_buf_readh(dev, buf, 14);
+
+    if (wd->wlanMode == ZM_MODE_AP)
+        aid = zfApFindSta(dev, src);
+
+
+    bf.buf = buf;
+    bf.dialog = zmw_rx_buf_readb(dev, buf, 26);
+    bf.status_code = zmw_rx_buf_readh(dev, buf, 27);
+    if (!bf.status_code)
+    {
+        wd->addbaComplete=1;
+    }
+
+    /*
+     * ba parameter set
+     */
+    bf.ba_parameter = zmw_rx_buf_readh(dev, buf, 29);
+    bf.ba_policy   = (bf.ba_parameter >> 1) & 1;
+    bf.tid         = (bf.ba_parameter >> 2) & 0xF;
+    bf.buffer_size = (bf.ba_parameter >> 6);
+    /*
+     * BA timeout value
+     */
+    bf.ba_timeout = zmw_rx_buf_readh(dev, buf, 31);
+
+    i=26;
+    while(i < 32) {
+        zm_debug_msg2("Recv ADDBA Rsp:", zmw_rx_buf_readb(dev,buf,i));
+        i++;
+    }
+
+    ac = zcUpToAc[bf.tid&0x7] & 0x3;
+
+    //zmw_enter_critical_section(dev);
+
+    //wd->aggSta[aid].aggFlag[ac] = 0;
+
+    //zmw_leave_critical_section(dev);
+
+    return ZM_SUCCESS;
+}
+
+u16_t   zfAggRecvDelba(zdev_t* dev, zbuf_t* buf)
+{
+    //zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+    return ZM_SUCCESS;
+}
+
+u16_t   zfAggSendAddbaResponse(zdev_t* dev, struct aggBaFrameParameter *bf)
+{
+    zbuf_t* buf;
+    //u16_t addrTblSize;
+    //struct zsAddrTbl addrTbl;
+    //u16_t err;
+    u16_t offset = 0;
+    u16_t hlen = 32;
+    u16_t header[(24+25+1)/2];
+    u16_t vap = 0;
+    u16_t i;
+    u8_t encrypt = 0;
+    u16_t dst[3];
+
+    //zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+
+    /*
+     * TBD : Maximum size of managment frame
+     */
+    if ((buf = zfwBufAllocate(dev, 1024)) == NULL)
+    {
+        zm_msg0_mm(ZM_LV_0, "Alloc mm buf Fail!");
+        return ZM_SUCCESS;
+    }
+
+    /*
+     * Reserve room for wlan header
+     */
+    offset = hlen;
+
+    /*
+     * add addba frame body
+     */
+    offset = zfAggSetAddbaResponseFrameBody(dev, buf, bf, offset);
+
+
+    zfwBufSetSize(dev, buf, offset);
+
+    /*
+     * Copy wlan header
+     */
+
+    dst[0] = zmw_rx_buf_readh(dev, bf->buf, 10);
+    dst[1] = zmw_rx_buf_readh(dev, bf->buf, 12);
+    dst[2] = zmw_rx_buf_readh(dev, bf->buf, 14);
+    zfAggGenAddbaHeader(dev, dst, header, offset-hlen, buf, vap, encrypt);
+    for (i=0; i<(hlen>>1); i++)
+    {
+        zmw_tx_buf_writeh(dev, buf, i*2, header[i]);
+    }
+
+    /* Get buffer DMA address */
+    //if ((addrTblSize = zfwBufMapDma(dev, buf, &addrTbl)) == 0)
+    //if ((addrTblSize = zfwMapTxDma(dev, buf, &addrTbl)) == 0)
+    //{
+    //    goto zlError;
+    //}
+
+    //zm_msg2_mm(ZM_LV_2, "offset=", offset);
+    //zm_msg2_mm(ZM_LV_2, "hlen=", hlen);
+    //zm_msg2_mm(ZM_LV_2, "addrTblSize=", addrTblSize);
+    //zm_msg2_mm(ZM_LV_2, "addrTbl.len[0]=", addrTbl.len[0]);
+    //zm_msg2_mm(ZM_LV_2, "addrTbl.physAddrl[0]=", addrTbl.physAddrl[0]);
+    //zm_msg2_mm(ZM_LV_2, "buf->data=", buf->data);
+
+    #if 0
+    if ((err = zfHpSend(dev, NULL, 0, NULL, 0, NULL, 0, buf, 0,
+            ZM_INTERNAL_ALLOC_BUF, 0, 0xff)) != ZM_SUCCESS)
+    {
+        goto zlError;
+    }
+    #else
+    zfPutVmmq(dev, buf);
+    zfPushVtxq(dev);
+    #endif
+
+    //zfAggSendAddbaRequest(dev, dst, zcUpToAc[bf->tid&0x7] & 0x3, bf->tid);
+    return ZM_SUCCESS;
+
+}
+
+u16_t   zfAggSetAddbaResponseFrameBody(zdev_t* dev, zbuf_t* buf,
+                struct aggBaFrameParameter *bf, u16_t offset)
+{
+
+    //zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+    /*
+     * ADDBA Request frame body
+     */
+
+    /*
+     * Category
+     */
+    zmw_tx_buf_writeb(dev, buf, offset++, 3);
+    /*
+     * Action details = 0
+     */
+    zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_ADDBA_RESPONSE_FRAME);
+    /*
+     * Dialog Token = nonzero
+     */
+    zmw_tx_buf_writeb(dev, buf, offset++, bf->dialog);
+    /*
+     * Status code
+     */
+    zmw_tx_buf_writeh(dev, buf, offset, 0);
+    offset+=2;
+    /*
+     * Block Ack parameter set
+     * BA policy = 1 for immediate BA, 0 for delayed BA
+     * TID(4bits) & buffer size(4bits) (TID=0x1 & buffer size=0x80)
+     * TBD: how to get TID number and buffer size?
+     * ¢z¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢{
+     * ¢x    B0    ¢x    B1     ¢x B2  B5 ¢x B6      B15 ¢x
+     * ¢u¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢t
+     * ¢x Reserved ¢x BA policy ¢x  TID   ¢x Buffer size ¢x
+     * ¢|¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢}
+     */
+    zmw_tx_buf_writeh(dev, buf, offset, bf->ba_parameter);
+    offset+=2;
+    /*
+     * BA timeout value
+     */
+    zmw_tx_buf_writeh(dev, buf, offset, bf->ba_timeout);
+    offset+=2;
+
+    return offset;
+}
+
+void   zfAggInvokeBar(zdev_t* dev, TID_TX tid_tx)
+{
+    struct aggBarControl aggBarControl;
+    //zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+    //bar_control = aggBarControl->tid_info << 12 | aggBarControl->compressed_bitmap << 2
+    //        | aggBarControl->multi_tid << 1 | aggBarControl->bar_ack_policy;
+    aggBarControl.bar_ack_policy = 0;
+    aggBarControl.multi_tid = 0;
+    aggBarControl.compressed_bitmap = 0;
+    aggBarControl.tid_info = tid_tx->tid;
+    zfAggSendBar(dev, tid_tx, &aggBarControl);
+
+    return;
+
+}
+/*
+ * zfAggSendBar() refers zfAggSendAddbaRequest()
+ */
+u16_t   zfAggSendBar(zdev_t* dev, TID_TX tid_tx, struct aggBarControl *aggBarControl)
+{
+    zbuf_t* buf;
+    //u16_t addrTblSize;
+    //struct zsAddrTbl addrTbl;
+    //u16_t err;
+    u16_t offset = 0;
+    u16_t hlen = 16+8;  /* mac header + control headers*/
+    u16_t header[(8+24+1)/2];
+    u16_t vap = 0;
+    u16_t i;
+    u8_t encrypt = 0;
+
+    //zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+
+    /*
+     * TBD : Maximum size of managment frame
+     */
+    if ((buf = zfwBufAllocate(dev, 1024)) == NULL)
+    {
+        zm_msg0_mm(ZM_LV_0, "Alloc mm buf Fail!");
+        return ZM_SUCCESS;
+    }
+
+    /*
+     * Reserve room for wlan header
+     */
+    offset = hlen;
+
+    /*
+     * add addba frame body
+     */
+    offset = zfAggSetBarBody(dev, buf, offset, tid_tx, aggBarControl);
+
+
+    zfwBufSetSize(dev, buf, offset);
+
+    /*
+     * Copy wlan header
+     */
+    zfAggGenBarHeader(dev, tid_tx->dst, header, offset-hlen, buf, vap, encrypt);
+    for (i=0; i<(hlen>>1); i++)
+    {
+        zmw_tx_buf_writeh(dev, buf, i*2, header[i]);
+    }
+
+    /* Get buffer DMA address */
+    //if ((addrTblSize = zfwBufMapDma(dev, buf, &addrTbl)) == 0)
+    //if ((addrTblSize = zfwMapTxDma(dev, buf, &addrTbl)) == 0)
+    //{
+    //    goto zlError;
+    //}
+
+    //zm_msg2_mm(ZM_LV_2, "offset=", offset);
+    //zm_msg2_mm(ZM_LV_2, "hlen=", hlen);
+    //zm_msg2_mm(ZM_LV_2, "addrTblSize=", addrTblSize);
+    //zm_msg2_mm(ZM_LV_2, "addrTbl.len[0]=", addrTbl.len[0]);
+    //zm_msg2_mm(ZM_LV_2, "addrTbl.physAddrl[0]=", addrTbl.physAddrl[0]);
+    //zm_msg2_mm(ZM_LV_2, "buf->data=", buf->data);
+
+    #if 0
+    if ((err = zfHpSend(dev, NULL, 0, NULL, 0, NULL, 0, buf, 0,
+            ZM_INTERNAL_ALLOC_BUF, 0, 0xff)) != ZM_SUCCESS)
+    {
+        goto zlError;
+    }
+    #else
+    zfPutVmmq(dev, buf);
+    zfPushVtxq(dev);
+    #endif
+
+    return ZM_SUCCESS;
+
+}
+
+u16_t   zfAggSetBarBody(zdev_t* dev, zbuf_t* buf, u16_t offset, TID_TX tid_tx, struct aggBarControl *aggBarControl)
+{
+    u16_t bar_control, start_seq;
+
+    //zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+    /*
+     * BAR Control frame body
+     */
+
+    /*
+     * BAR Control Field
+     * ¢z¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢{
+     * ¢x    B0   ¢x    B1     ¢x     B2     ¢x B3   B11 ¢x B12  B15 ¢x
+     * ¢u¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢t
+     * ¢x BAR Ack ¢x Multi-TID ¢x Compressed ¢x Reserved ¢x TID_INFO ¢x
+     * ¢x  Policy ¢x           ¢x   Bitmap   ¢x          ¢x          ¢x
+     * ¢|¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢}
+     */
+    bar_control = aggBarControl->tid_info << 12 | aggBarControl->compressed_bitmap << 2
+            | aggBarControl->multi_tid << 1 | aggBarControl->bar_ack_policy;
+
+    zmw_tx_buf_writeh(dev, buf, offset, bar_control);
+    offset+=2;
+    if (0 == aggBarControl->multi_tid) {
+        /*
+         * BA starting sequence number
+         * ¢z¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢{
+         * ¢x B0       B3 ¢x B4              B15 ¢x
+         * ¢u¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢t
+         * ¢x Frag num(0) ¢x BA starting seq num ¢x
+         * ¢|¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢}
+         */
+        start_seq = (tid_tx->bar_ssn << 4) & 0xFFF0;
+        zmw_tx_buf_writeh(dev, buf, offset, start_seq);
+        offset+=2;
+    }
+    if (1 == aggBarControl->multi_tid && 1 == aggBarControl->compressed_bitmap) {
+        /* multi-tid BlockAckReq variant, not implemented*/
+    }
+
+    return offset;
+}
+
+u16_t zfAggGenBarHeader(zdev_t* dev, u16_t* dst,
+        u16_t* header, u16_t len, zbuf_t* buf, u16_t vap, u8_t encrypt)
+{
+    u8_t  hlen = 16+8;        // MAC ctrl + PHY ctrl + 802.11 MM header
+    //u8_t frameType = ZM_WLAN_FRAME_TYPE_ACTION;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    /*
+     * Generate control setting
+     */
+    //bodyLen = zfwBufGetSize(dev, buf);
+    header[0] = 16+len+4;   //Length
+    header[1] = 0x8;        //MAC control, backoff + (ack)
+
+#if 1
+    /* CCK 1M */
+    header[2] = 0x0f00;          //PHY control L
+    header[3] = 0x0000;          //PHY control H
+#else
+    /* CCK 6M */
+    header[2] = 0x0f01;          //PHY control L
+    header[3] = 0x000B;          //PHY control H
+
+#endif
+    /*
+     * Generate WLAN header
+     * Frame control frame type and subtype
+     */
+    header[4+0] = ZM_WLAN_FRAME_TYPE_BAR;
+    /*
+     * Duration
+     */
+    header[4+1] = 0;
+
+    /* Address 1 = DA */
+    header[4+2] = dst[0];
+    header[4+3] = dst[1];
+    header[4+4] = dst[2];
+
+    /* Address 2 = SA */
+    header[4+5] = wd->macAddr[0];
+    header[4+6] = wd->macAddr[1];
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+#ifdef ZM_VAPMODE_MULTILE_SSID
+        header[4+7] = wd->macAddr[2]; //Multiple SSID
+#else
+        header[4+7] = wd->macAddr[2] + (vap<<8); //VAP
+#endif
+    }
+    else
+    {
+        header[4+7] = wd->macAddr[2];
+    }
+
+    /* Sequence Control */
+    zmw_enter_critical_section(dev);
+    header[4+11] = ((wd->mmseq++)<<4);
+    zmw_leave_critical_section(dev);
+
+
+    return hlen;
+}
diff --git a/drivers/staging/otus/80211core/cagg.h b/drivers/staging/otus/80211core/cagg.h
new file mode 100644
index 00000000000..1d87a564162
--- /dev/null
+++ b/drivers/staging/otus/80211core/cagg.h
@@ -0,0 +1,435 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                          */
+/*  Module Name : cagg.h                                                    */
+/*                                                                          */
+/*  Abstract                                                                */
+/*      This module contains A-MPDU aggregation relatived functions.        */
+/*                                                                          */
+/*  NOTES                                                                   */
+/*      None                                                                */
+/*                                                                          */
+/****************************************************************************/
+/*Revision History:                                                         */
+/*    Who         When        What                                          */
+/*    --------    --------    ----------------------------------------------*/
+/*                                                                          */
+/*    Honda       12-4-06     created                                       */
+/*                                                                          */
+/****************************************************************************/
+
+#ifndef _CAGG_H
+#define _CAGG_H
+
+
+/*
+ * the aggregation functions flag, 0 if don't do aggregate
+ */
+
+#define ZM_AGG_FPGA_DEBUG                   1
+#define ZM_AGG_FPGA_REORDERING              1
+
+#ifndef ZM_AGG_TALLY
+//#define ZM_AGG_TALLY
+#endif
+/*
+ * Aggregate control
+ */
+
+
+#define ZM_AGG_POOL_SIZE                    20
+#define ZM_BAW_POOL_SIZE                    32
+#define ZM_AGGQ_SIZE                        64
+#define ZM_AGGQ_SIZE_MASK                   (ZM_AGGQ_SIZE-1)
+#define ZM_AGG_LOW_THRESHOLD                1
+#define ZM_AGG_HIGH_THRESHOLD               5
+
+/*
+ * number of access categories (ac)
+ */
+#define ZM_AC                               4
+/*
+ * the timer to clear aggregation queue, unit: 1 tick
+ * if the packet is too old (current time - arrival time)
+ * the packet and the aggregate queue will be cleared
+ */
+#define ZM_AGG_CLEAR_TIME                   10
+/*
+ * delete the queue if idle for ZM_DELETE_TIME
+ * unit: 10ms
+ */
+#define ZM_AGG_DELETE_TIME                  10000
+
+/*
+ * block ack window size
+ */
+#define ZM_AGG_BAW_SIZE                     64
+#define ZM_AGG_BAW_MASK                     (ZM_AGG_BAW_SIZE-1)
+/*
+ * originator     ADDBA Resquest    receiver
+ *      |----------------------------->|
+ *     1|          ACK                 |1
+ *      |<-----------------------------|
+ *     2|         ADDBA Response       |2
+ *      |<-----------------------------|
+ *     3|          ACK                 |3
+ *      |----------------------------->|
+ *     4                                4
+ */
+#define ZM_AGG_ADDBA_REQUEST                1
+#define ZM_AGG_ADDBA_REQUEST_ACK            2
+#define ZM_AGG_ADDBA_RESPONSE               3
+#define ZM_AGG_ADDBA_RESPONSE_ACK           4
+
+#define ZM_AGG_SINGLE_MPDU                  00
+#define ZM_AGG_FIRST_MPDU                   01
+#define ZM_AGG_MIDDLE_MPDU                  11
+#define ZM_AGG_LAST_MPDU                    10
+/*
+ * end of Aggregate control
+ */
+
+#define TID_TX  struct aggQueue*
+#define TID_BAW struct baw_q*
+#define BAW wd->baw_enabler
+#define DESTQ wd->destQ
+
+/*
+ * Queue access
+ */
+#define zm_agg_qlen(dev, head, tail) ((head - tail) & ZM_AGGQ_SIZE_MASK)
+#define zm_agg_inQ(tid_tx, pt) ((((pt - tid_tx->aggTail) & ZM_AGGQ_SIZE_MASK) < \
+        ((tid_tx->aggHead - tid_tx->aggTail) & ZM_AGGQ_SIZE_MASK))? TRUE:FALSE)
+#define zm_agg_plus(pt) pt = (pt + 1) & ZM_AGGQ_SIZE_MASK
+#define zm_agg_min(A, B) ((A>B)? B:A)
+#define zm_agg_GetTime() wd->tick
+#define TXQL (zfHpGetMaxTxdCount(dev) - zfHpGetFreeTxdCount(dev))
+
+/* don't change AGG_MIN_TXQL easily, this might cause BAW BSOD */
+#define AGG_MIN_TXQL                        2
+/*
+ * consider tcp,udp,ac(1234)
+ */
+#define zm_agg_dynamic_threshold(dev, ar)   ((ar > 16)? 11: \
+                                             (ar > 12)? 8: \
+                                             (ar > 8)? 5: \
+                                             (ar > 4)? 2:1)
+#define zm_agg_weight(ac)   ((3 == ac)? 4: \
+                             (2 == ac)? 3: \
+                             (0 == ac)? 2:1)
+/*
+ * the required free queue ratio per ac
+ */
+
+#define zm_agg_ratio(ac)    ((3 == ac)? 3: \
+                             (2 == ac)? (zfHpGetMaxTxdCount(dev)*1/4): \
+                             (0 == ac)? (zfHpGetMaxTxdCount(dev)*2/4): \
+                                        (zfHpGetMaxTxdCount(dev)*3/4))
+
+//#define zm_agg_ratio(ac)    3
+/*
+ * end of Queue access
+ */
+
+#define ZM_AGGMSG_LEV    ZM_LV_3
+#define zm_msg0_agg(lv, msg) if (ZM_AGGMSG_LEV >= lv) \
+        {zm_debug_msg0(msg);}
+#define zm_msg1_agg(lv, msg, val) if (ZM_AGGMSG_LEV >= lv) \
+        {zm_debug_msg1(msg, val);}
+#define zm_msg2_agg(lv, msg, val) if (ZM_AGGMSG_LEV >= lv) \
+        {zm_debug_msg2(msg, val);}
+
+#ifndef ZM_ENABLE_FW_BA_RETRANSMISSION //disable BAW
+struct baw_header_r {
+    u16_t       *header;
+    u16_t       *mic;
+    u16_t       *snap;
+    u16_t       headerLen;
+    u16_t       micLen;
+    u16_t       snapLen;
+    u16_t       removeLen;
+    u8_t        keyIdx;
+};
+
+struct baw_header {
+    u16_t       header[29];//[(8+30+2+18)/2];  58 bytes  /* ctr+(4+a1+a2+a3+2+a4)+qos+iv */
+    u16_t       headerLen;
+    u16_t       mic[4]; //[8/2]; 8 bytes
+    u16_t       micLen;
+    u16_t       snap[4]; //[8/2]; 8 bytes
+    u16_t       snapLen;
+    u16_t       removeLen;
+    u8_t        keyIdx;
+};
+
+struct bufInfo {
+    zbuf_t*     buf;
+    u8_t        baw_retransmit;
+    u32_t       timestamp;
+    struct baw_header   *baw_header;
+};
+#endif
+struct aggElement
+{
+    zbuf_t*     buf;
+    u32_t       arrivalTime;
+    u8_t        baw_retransmit;
+    struct zsAdditionInfo addInfo;
+    //struct baw_header  baw_header;
+};
+
+
+#ifndef ZM_ENABLE_FW_BA_RETRANSMISSION //disable BAW
+struct baw_buf
+{
+    zbuf_t*     buf;
+    u16_t       baw_seq;
+    u32_t       timestamp;
+    u8_t        baw_retransmit;
+    struct baw_header baw_header;
+};
+
+struct baw_q {
+    struct baw_buf  frame[ZM_VTXQ_SIZE];
+    u16_t       enabled;
+    u16_t       start_seq;
+    u16_t       head;
+    u16_t       tail;
+    u16_t       size;
+    TID_TX      tid_tx;
+
+    //struct baw_header *baw_header;
+};
+
+struct baw_enabler
+{
+    struct baw_q    tid_baw[ZM_BAW_POOL_SIZE];
+    u8_t    delPoint;
+    void    (*core)(zdev_t* dev, u16_t baw_seq, u32_t bitmap, u16_t aggLen);
+    //void    (*core);
+    void    (*init)(zdev_t* dev);
+    TID_BAW (*getNewQ)(zdev_t* dev, u16_t start_seq, TID_TX tid_tx);
+    TID_BAW (*getQ)(zdev_t* dev, u16_t baw_seq);
+    u16_t   (*insert)(zdev_t* dev, zbuf_t* buf, u16_t baw_seq, TID_BAW tid_baw, u8_t baw_retransmit, struct baw_header_r *header_r);
+    struct bufInfo* (*pop)(zdev_t* dev, u16_t index, TID_BAW tid_baw);
+    void    (*enable)(zdev_t* dev, TID_BAW tid_baw, u16_t start_seq);
+    void    (*disable)(zdev_t* dev, TID_BAW tid_baw);
+
+};
+#endif
+struct aggQueue
+{
+    struct      aggElement  aggvtxq[ZM_AGGQ_SIZE];
+    u16_t       aggHead;
+    u16_t       aggTail;
+    s16_t       size;
+    u16_t       aggQSTA;
+    u16_t       aggQEnabled;
+    u16_t       ac;
+    u16_t       tid;
+    u16_t       aggReady;
+    u16_t       clearFlag;
+    u16_t       deleteFlag;
+    u32_t       lastArrival;
+    u16_t       aggFrameSize;
+    u16_t       bar_ssn;    /* starting sequence number in BAR */
+    u16_t       dst[3];
+    u16_t       complete;     /* complete indication pointer */
+};
+
+struct aggSta
+{
+    u16_t       count[ZM_AC];
+    TID_TX      tid_tx[8];
+    u16_t       aggFlag[ZM_AC];
+};
+
+struct agg_tid_rx
+{
+    u16_t       aid;
+    u16_t       ac;
+    u16_t       addBaExchangeStatusCode;
+    //struct zsAdditionInfo *addInfo;
+    u16_t       seq_start;		/* first seq expected next */
+    u16_t       baw_head;		/* head of valid block ack window */
+    u16_t       baw_tail;		/* tail of valid block ack window */
+    //u16_t       free_count;		/* block ack window size	*/
+    u8_t        sq_exceed_count;
+    u8_t        sq_behind_count;
+    struct aggElement frame[ZM_AGG_BAW_SIZE + 1]; /* out-of-order rx frames */
+};
+
+struct aggControl
+{
+    u16_t       aggEnabled;
+    u16_t       ampduIndication;
+    u16_t       addbaIndication;
+    //TID_BAW     tid_baw;
+    u32_t       timestamp;
+};
+
+struct aggBaFrameParameter
+{
+    zbuf_t*     buf;
+    u16_t       ba_parameter;
+    u8_t        dialog;
+    u16_t       ba_policy;
+    u16_t       tid;
+    u16_t       buffer_size;
+    u16_t       ba_timeout;
+    u16_t       ba_start_seq;
+    u16_t       status_code;
+};
+
+struct aggBarControl
+{
+    u16_t       bar_ack_policy      ;
+    u16_t       multi_tid           ;
+    u16_t       compressed_bitmap   ;
+    u16_t       tid_info            ;
+};
+
+struct aggTally
+{
+    u32_t       got_packets_sum;
+    u32_t       got_bytes_sum;
+    u32_t       sent_packets_sum;
+    u32_t       sent_bytes_sum;
+    u32_t       avg_got_packets;
+    u32_t       avg_got_bytes;
+    u32_t       avg_sent_packets;
+    u32_t       avg_sent_bytes;
+    u16_t       time;
+};
+
+
+struct destQ {
+    struct dest{
+        u16_t   Qtype : 1; /* 0 aggr, 1 vtxq */
+        TID_TX  tid_tx;
+        void*   vtxq;
+
+        struct dest* next;
+    } *dest[4];
+    struct dest* Head[4];
+    //s16_t   size[4];
+    u16_t   ppri;
+    void    (*insert)(zdev_t* dev, u16_t Qtype, u16_t ac, TID_TX tid_tx, void* vtxq);
+    void    (*delete)(zdev_t* dev, u16_t Qtype, TID_TX tid_tx, void* vtxq);
+    void    (*init)(zdev_t* dev);
+    struct dest* (*getNext)(zdev_t* dev, u16_t ac);
+    u16_t   (*exist)(zdev_t* dev, u16_t Qtype, u16_t ac, TID_TX tid_tx, void* vtxq);
+    //void    (*scan)(zdev_t* dev);
+};
+/*
+ * aggregation tx
+ */
+void    zfAggInit(zdev_t* dev);
+u16_t   zfApFindSta(zdev_t* dev, u16_t* addr);
+u16_t   zfAggGetSta(zdev_t* dev, zbuf_t* buf);
+TID_TX  zfAggTxGetQueue(zdev_t* dev, u16_t aid, u16_t tid);
+TID_TX  zfAggTxNewQueue(zdev_t* dev, u16_t aid, u16_t tid, zbuf_t* buf);
+u16_t   zfAggTxEnqueue(zdev_t* dev, zbuf_t* buf, u16_t aid, TID_TX tid_tx);
+u16_t   zfAggTx(zdev_t* dev, zbuf_t* buf, u16_t tid);
+u16_t   zfAggTxReadyCount(zdev_t* dev, u16_t ac);
+u16_t   zfAggTxPartial(zdev_t* dev, u16_t ac, u16_t readycount);
+u16_t   zfAggTxSend(zdev_t* dev, u32_t freeTxd, TID_TX tid_tx);
+TID_TX  zfAggTxGetReadyQueue(zdev_t* dev, u16_t ac);
+zbuf_t* zfAggTxGetVtxq(zdev_t* dev, TID_TX tid_tx);
+u16_t   zfAggTxDeleteQueue(zdev_t* dev, u16_t qnum);
+u16_t   zfAggScanAndClear(zdev_t* dev, u32_t time);
+u16_t   zfAggClearQueue(zdev_t* dev);
+void    zfAggTxScheduler(zdev_t* dev, u8_t ScanAndClear);
+
+/* tid_tx manipulation */
+#ifndef ZM_ENABLE_FW_BA_RETRANSMISSION //disable BAW
+u16_t   zfAggTidTxInsertHead(zdev_t* dev, struct bufInfo* buf_info, TID_TX tid_tx);
+#endif
+void    zfAggDestInsert(zdev_t* dev, u16_t Qtype, u16_t ac, TID_TX tid_tx, void* vtxq);
+void    zfAggDestDelete(zdev_t* dev, u16_t Qtype, TID_TX tid_tx, void* vtxq);
+void    zfAggDestInit(zdev_t* dev);
+struct dest* zfAggDestGetNext(zdev_t* dev, u16_t ac);
+u16_t   zfAggDestExist(zdev_t* dev, u16_t Qtype, u16_t ac, TID_TX tid_tx, void* vtxq);
+/*
+ * aggregation rx
+ */
+struct agg_tid_rx *zfAggRxEnabled(zdev_t* dev, zbuf_t* buf);
+u16_t   zfAggRx(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo *addInfo, struct agg_tid_rx *tid_rx);
+struct agg_tid_rx *zfAggRxGetQueue(zdev_t* dev, zbuf_t* buf);
+u16_t   zfAggRxEnqueue(zdev_t* dev, zbuf_t* buf, struct agg_tid_rx *tid_rx, struct zsAdditionInfo *addInfo);
+u16_t   zfAggRxFlush(zdev_t* dev, u16_t seq_no, struct agg_tid_rx *tid_rx);
+u16_t   zfAggRxFreeBuf(zdev_t* dev, u16_t destroy);
+u16_t   zfAggRxClear(zdev_t* dev, u32_t time);
+void    zfAggRecvBAR(zdev_t* dev, zbuf_t* buf);
+/*
+ * end of aggregation rx
+ */
+
+/*
+ * ADDBA
+ */
+u16_t   zfAggSendAddbaRequest(zdev_t* dev, u16_t *dst, u16_t ac, u16_t up);
+u16_t   zfAggSetAddbaFrameBody(zdev_t* dev,zbuf_t* buf, u16_t offset, u16_t ac, u16_t up);
+u16_t   zfAggGenAddbaHeader(zdev_t* dev, u16_t* dst,
+                u16_t* header, u16_t len, zbuf_t* buf, u16_t vap, u8_t encrypt);
+u16_t   zfAggProcessAction(zdev_t* dev, zbuf_t* buf);
+u16_t   zfAggBlockAckActionFrame(zdev_t* dev, zbuf_t* buf);
+u16_t   zfAggRecvAddbaRequest(zdev_t* dev, zbuf_t* buf);
+u16_t   zfAggRecvAddbaResponse(zdev_t* dev, zbuf_t* buf);
+u16_t   zfAggRecvDelba(zdev_t* dev, zbuf_t* buf);
+u16_t   zfAggSendAddbaResponse(zdev_t* dev, struct aggBaFrameParameter *bf);
+u16_t   zfAggSetAddbaResponseFrameBody(zdev_t* dev, zbuf_t* buf,
+                struct aggBaFrameParameter *bf, u16_t offset);
+u16_t   zfAggAddbaSetTidRx(zdev_t* dev, zbuf_t* buf,
+                struct aggBaFrameParameter *bf);
+/*
+ * zfAggTxSendEth
+ */
+u16_t zfAggTxSendEth(zdev_t* dev, zbuf_t* buf, u16_t port, u16_t bufType, u8_t flag, struct aggControl *aggControl, TID_TX tid_tx);
+
+/*
+ * statistics functions
+ */
+u16_t zfAggTallyReset(zdev_t* dev);
+
+u16_t   zfAggPrintTally(zdev_t* dev);
+
+/*
+ * BAR
+ */
+void    zfAggInvokeBar(zdev_t* dev, TID_TX tid_tx);
+u16_t   zfAggSendBar(zdev_t* dev, TID_TX tid_tx, struct aggBarControl *aggBarControl);
+u16_t   zfAggSetBarBody(zdev_t* dev, zbuf_t* buf, u16_t offset, TID_TX tid_tx, struct aggBarControl *aggBarControl);
+u16_t   zfAggGenBarHeader(zdev_t* dev, u16_t* dst,
+                u16_t* header, u16_t len, zbuf_t* buf, u16_t vap, u8_t encrypt);
+
+#ifndef ZM_ENABLE_FW_BA_RETRANSMISSION //disable BAW
+/* BAW BA retransmission */
+void    zfBawCore(zdev_t* dev, u16_t baw_seq, u32_t bitmap, u16_t aggLen);
+void    zfBawInit(zdev_t* dev);
+TID_BAW zfBawGetNewQ(zdev_t* dev, u16_t start_seq, TID_TX tid_tx);
+u16_t   zfBawInsert(zdev_t* dev, zbuf_t* buf, u16_t baw_seq, TID_BAW tid_baw, u8_t baw_retransmit, struct baw_header_r *header_r);
+struct bufInfo* zfBawPop(zdev_t* dev, u16_t index, TID_BAW tid_baw);
+void    zfBawEnable(zdev_t* dev, TID_BAW tid_baw, u16_t start_seq);
+void    zfBawDisable(zdev_t* dev, TID_BAW tid_baw);
+TID_BAW zfBawGetQ(zdev_t* dev, u16_t baw_seq);
+void zfAggTxRetransmit(zdev_t* dev, struct bufInfo *buf_info, struct aggControl *aggControl, TID_TX tid_tx);
+#endif
+/* extern functions */
+extern zbuf_t* zfGetVtxq(zdev_t* dev, u8_t ac);
+
+#endif /* #ifndef _CAGG_H */
+
diff --git a/drivers/staging/otus/80211core/ccmd.c b/drivers/staging/otus/80211core/ccmd.c
new file mode 100644
index 00000000000..47997797367
--- /dev/null
+++ b/drivers/staging/otus/80211core/ccmd.c
@@ -0,0 +1,1861 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : cmd.c                                                 */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains command interface functions.               */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+#include "cprecomp.h"
+#include "../hal/hpreg.h"
+
+
+u16_t zfWlanReset(zdev_t* dev);
+u32_t zfUpdateRxRate(zdev_t* dev);
+
+
+extern void zfiUsbRecv(zdev_t *dev, zbuf_t *buf);
+extern void zfiUsbRegIn(zdev_t* dev, u32_t* rsp, u16_t rspLen);
+extern void zfiUsbOutComplete(zdev_t* dev, zbuf_t *buf, u8_t status, u8_t *hdr);
+extern void zfiUsbRegOutComplete(zdev_t* dev);
+extern u16_t zfHpReinit(zdev_t* dev, u32_t frequency);
+
+/* Get size (byte) of driver core global data structure.    */
+/* This size will be used by driver wrapper to allocate     */
+/* a memory space for driver core to store global variables */
+u16_t zfiGlobalDataSize(zdev_t* dev)
+{
+    u32_t ret;
+    ret = (sizeof(struct zsWlanDev));
+    zm_assert((ret>>16) == 0);
+    return (u16_t)ret;
+}
+
+
+/* Initialize WLAN hardware and software, resource will be allocated */
+/* for WLAN operation, must be called first before other function.   */
+extern u16_t zfiWlanOpen(zdev_t* dev, struct zsCbFuncTbl* cbFuncTbl)
+{
+    //u16_t ret;
+    //u32_t i;
+    //u8_t* ch;
+    //u8_t  bPassive;
+    u32_t devSize;
+    struct zfCbUsbFuncTbl cbUsbFuncTbl;
+    zmw_get_wlan_dev(dev);
+
+    zm_debug_msg0("start");
+
+    devSize = sizeof(struct zsWlanDev);
+    /* Zeroize zsWlanDev struct */
+    zfZeroMemory((u8_t*)wd, (u16_t)devSize);
+
+#ifdef ZM_ENABLE_AGGREGATION
+    zfAggInit(dev);
+#endif
+
+    zfCwmInit(dev);
+
+    wd->commTally.RateCtrlTxMPDU = 0;
+    wd->commTally.RateCtrlBAFail = 0;
+    wd->preambleTypeInUsed = ZM_PREAMBLE_TYPE_SHORT;
+
+    if (cbFuncTbl == NULL)
+    {
+        /* zfcbRecvEth() is mandatory */
+        zm_assert(0);
+    }
+    else
+    {
+        if (cbFuncTbl->zfcbRecvEth == NULL)
+        {
+            /* zfcbRecvEth() is mandatory */
+            zm_assert(0);
+        }
+        wd->zfcbAuthNotify = cbFuncTbl->zfcbAuthNotify;
+        wd->zfcbAuthNotify = cbFuncTbl->zfcbAuthNotify;
+        wd->zfcbAsocNotify = cbFuncTbl->zfcbAsocNotify;
+        wd->zfcbDisAsocNotify = cbFuncTbl->zfcbDisAsocNotify;
+        wd->zfcbApConnectNotify = cbFuncTbl->zfcbApConnectNotify;
+        wd->zfcbConnectNotify = cbFuncTbl->zfcbConnectNotify;
+        wd->zfcbScanNotify = cbFuncTbl->zfcbScanNotify;
+        wd->zfcbMicFailureNotify = cbFuncTbl->zfcbMicFailureNotify;
+        wd->zfcbApMicFailureNotify = cbFuncTbl->zfcbApMicFailureNotify;
+        wd->zfcbIbssPartnerNotify = cbFuncTbl->zfcbIbssPartnerNotify;
+        wd->zfcbMacAddressNotify = cbFuncTbl->zfcbMacAddressNotify;
+        wd->zfcbSendCompleteIndication = cbFuncTbl->zfcbSendCompleteIndication;
+        wd->zfcbRecvEth = cbFuncTbl->zfcbRecvEth;
+        wd->zfcbRestoreBufData = cbFuncTbl->zfcbRestoreBufData;
+        wd->zfcbRecv80211 = cbFuncTbl->zfcbRecv80211;
+#ifdef ZM_ENABLE_CENC
+        wd->zfcbCencAsocNotify = cbFuncTbl->zfcbCencAsocNotify;
+#endif //ZM_ENABLE_CENC
+        wd->zfcbClassifyTxPacket = cbFuncTbl->zfcbClassifyTxPacket;
+        wd->zfcbHwWatchDogNotify = cbFuncTbl->zfcbHwWatchDogNotify;
+    }
+
+    //add by honda 0330
+    cbUsbFuncTbl.zfcbUsbRecv = zfiUsbRecv;
+    cbUsbFuncTbl.zfcbUsbRegIn = zfiUsbRegIn;
+    cbUsbFuncTbl.zfcbUsbOutComplete = zfiUsbOutComplete;
+    cbUsbFuncTbl.zfcbUsbRegOutComplete = zfiUsbRegOutComplete;
+    zfwUsbRegisterCallBack(dev, &cbUsbFuncTbl);
+    /* Init OWN MAC address */
+    wd->macAddr[0] = 0x8000;
+    wd->macAddr[1] = 0x0000;
+    wd->macAddr[2] = 0x0000;
+
+    wd->regulationTable.regionCode = 0xffff;
+
+    zfHpInit(dev, wd->frequency);
+
+    /* init region code */
+    //wd->regulationTable.regionCode = NULL1_WORLD; //Only 2.4g RegCode
+    //zfHpGetRegulationTablefromRegionCode(dev, NULL1_WORLD);
+    //zfiWlanSetDot11DMode(dev , 1); // Enable 802.11d
+    /* Get the first channel */
+    //wd->frequency = zfChGetFirstChannel(dev, &bPassive);
+#ifdef ZM_AP_DEBUG
+    //wd->frequency = 2437;
+#endif
+
+    //STA mode
+    wd->sta.mTxRate = 0x0;
+    wd->sta.uTxRate = 0x3;
+    wd->sta.mmTxRate = 0x0;
+    wd->sta.adapterState = ZM_STA_STATE_DISCONNECT;
+    wd->sta.capability[0] = 0x01;
+    wd->sta.capability[1] = 0x00;
+
+    wd->sta.preambleTypeHT = 0;
+    wd->sta.htCtrlBandwidth = 0;
+    wd->sta.htCtrlSTBC = 0;
+    wd->sta.htCtrlSG = 0;
+    wd->sta.defaultTA = 0;
+    //wd->sta.activescanTickPerChannel = ZM_TIME_ACTIVE_SCAN/ZM_MS_PER_TICK;
+	{
+		u8_t Dur = ZM_TIME_ACTIVE_SCAN;
+		zfwGetActiveScanDur(dev, &Dur);
+		wd->sta.activescanTickPerChannel = Dur/ZM_MS_PER_TICK;
+
+	}
+    wd->sta.passiveScanTickPerChannel = ZM_TIME_PASSIVE_SCAN/ZM_MS_PER_TICK;
+    wd->sta.bAutoReconnect = TRUE;
+    wd->sta.dropUnencryptedPkts = FALSE;
+
+    /* set default to bypass all multicast packet for linux, window XP would set 0 by wrapper initialization */
+	wd->sta.bAllMulticast = 1;
+
+    /* Initial the RIFS Status / RIFS-like frame count / RIFS count */
+    wd->sta.rifsState = ZM_RIFS_STATE_DETECTING;
+    wd->sta.rifsLikeFrameCnt = 0;
+    wd->sta.rifsCount = 0;
+
+    wd->sta.osRxFilter = 0;
+    wd->sta.bSafeMode = 0;
+
+    //Common
+    zfResetSupportRate(dev, ZM_DEFAULT_SUPPORT_RATE_DISCONNECT);
+    wd->beaconInterval = 100;
+    wd->rtsThreshold = 2346;
+    wd->fragThreshold = 32767;
+    wd->wlanMode = ZM_MODE_INFRASTRUCTURE;
+    wd->txMCS = 0xff;    //AUTO
+    wd->dtim = 1;
+    //wd->txMT = 1;       //OFDM
+    wd->tick = 1;
+    wd->maxTxPower2 = 0xff;
+    wd->maxTxPower5 = 0xff;
+    wd->supportMode = 0xffffffff;
+    wd->ws.adhocMode = ZM_ADHOCBAND_G;
+    wd->ws.autoSetFrequency = 0xff;
+
+    //AP mode
+    //wd->bgMode = wd->ws.bgMode;
+    wd->ap.ssidLen[0] = 6;
+    wd->ap.ssid[0][0] = 'Z';
+    wd->ap.ssid[0][1] = 'D';
+    wd->ap.ssid[0][2] = '1';
+    wd->ap.ssid[0][3] = '2';
+    wd->ap.ssid[0][4] = '2';
+    wd->ap.ssid[0][5] = '1';
+
+    // Init the country iso name as NA
+    wd->ws.countryIsoName[0] = 0;
+    wd->ws.countryIsoName[1] = 0;
+    wd->ws.countryIsoName[2] = '\0';
+
+	/* init fragmentation is disabled */
+	//zfiWlanSetFragThreshold(dev, 0);
+
+	/* airopeek : swSniffer 1=>on  0=>off */
+	wd->swSniffer = 0;
+    wd->XLinkMode = 0;
+
+// jhlee HT 0
+#if 1
+    /* AP Mode*/
+    /* Init HT Capability Info */
+    wd->ap.HTCap.Data.ElementID = ZM_WLAN_EID_HT_CAPABILITY;
+    wd->ap.HTCap.Data.Length = 26;
+    //wd->ap.HTCap.Data.SupChannelWidthSet = 0;
+    //wd->ap.HTCap.Data.MIMOPowerSave = 3;
+    //wd->ap.HTCap.Data.ShortGIfor40MHz = 0;
+    //wd->ap.HTCap.Data.ShortGIfor20MHz = 0;
+    //wd->ap.HTCap.Data.DSSSandCCKin40MHz = 0;
+    wd->ap.HTCap.Data.AMPDUParam |= HTCAP_MaxRxAMPDU3;
+    wd->ap.HTCap.Data.MCSSet[0] = 0xFF; // MCS 0 ~  7
+    wd->ap.HTCap.Data.MCSSet[1] = 0xFF; // MCS 8 ~ 15
+
+    /* Init Extended HT Capability Info */
+    wd->ap.ExtHTCap.Data.ElementID = ZM_WLAN_EID_EXTENDED_HT_CAPABILITY;
+    wd->ap.ExtHTCap.Data.Length = 22;
+    wd->ap.ExtHTCap.Data.ControlChannel = 6;
+    //wd->ap.ExtHTCap.Data.ExtChannelOffset = 3;
+    wd->ap.ExtHTCap.Data.ChannelInfo |= ExtHtCap_RecomTxWidthSet;
+    //wd->ap.ExtHTCap.Data.RIFSMode = 1;
+    wd->ap.ExtHTCap.Data.OperatingInfo |= 1;
+
+    /* STA Mode*/
+    /* Init HT Capability Info */
+    wd->sta.HTCap.Data.ElementID = ZM_WLAN_EID_HT_CAPABILITY;
+    wd->sta.HTCap.Data.Length = 26;
+
+    /* Test with 5G-AP : 7603 */
+    //wd->sta.HTCap.Data.SupChannelWidthSet = 1;
+    wd->sta.HTCap.Data.HtCapInfo |= HTCAP_SMEnabled;
+    wd->sta.HTCap.Data.HtCapInfo |= HTCAP_SupChannelWidthSet;
+    wd->sta.HTCap.Data.HtCapInfo |= HTCAP_ShortGIfor40MHz;
+    wd->sta.HTCap.Data.HtCapInfo |= HTCAP_DSSSandCCKin40MHz;
+#ifndef ZM_DISABLE_AMSDU8K_SUPPORT
+    wd->sta.HTCap.Data.HtCapInfo |= HTCAP_MaxAMSDULength;
+#endif
+    //wd->sta.HTCap.Data.MIMOPowerSave = 0;
+    //wd->sta.HTCap.Data.ShortGIfor40MHz = 0;
+    //wd->sta.HTCap.Data.ShortGIfor20MHz = 0;
+    //wd->sta.HTCap.Data.DSSSandCCKin40MHz = 0;
+    wd->sta.HTCap.Data.AMPDUParam |= HTCAP_MaxRxAMPDU3;
+    wd->sta.HTCap.Data.MCSSet[0] = 0xFF; // MCS 0 ~  7
+    wd->sta.HTCap.Data.MCSSet[1] = 0xFF; // MCS 8 ~ 15
+    wd->sta.HTCap.Data.PCO |= HTCAP_TransmissionTime3;
+    //wd->sta.HTCap.Data.TransmissionTime = 0;
+    /* Init Extended HT Capability Info */
+    wd->sta.ExtHTCap.Data.ElementID = ZM_WLAN_EID_EXTENDED_HT_CAPABILITY;
+    wd->sta.ExtHTCap.Data.Length = 22;
+    wd->sta.ExtHTCap.Data.ControlChannel = 6;
+
+    //wd->sta.ExtHTCap.Data.ExtChannelOffset |= 3;
+    wd->sta.ExtHTCap.Data.ChannelInfo |= ExtHtCap_ExtChannelOffsetBelow;
+
+    //wd->sta.ExtHTCap.Data.RecomTxWidthSet = 1;
+    //wd->sta.ExtHTCap.Data.RIFSMode = 1;
+    wd->sta.ExtHTCap.Data.OperatingInfo |= 1;
+#endif
+
+#if 0
+    /* WME test code */
+    wd->ap.qosMode[0] = 1;
+#endif
+
+    wd->ledStruct.ledMode[0] = 0x2221;
+    wd->ledStruct.ledMode[1] = 0x2221;
+
+    zfTimerInit(dev);
+
+    ZM_PERFORMANCE_INIT(dev);
+
+    zfBssInfoCreate(dev);
+    zfScanMgrInit(dev);
+    zfPowerSavingMgrInit(dev);
+
+#if 0
+    /* Test code */
+    {
+        u32_t key[4] = {0xffffffff, 0xff, 0, 0};
+        u16_t addr[3] = {0x8000, 0x01ab, 0x0000};
+        //zfSetKey(dev, 0, 0, ZM_WEP64, addr, key);
+        //zfSetKey(dev, 0, 0, ZM_AES, addr, key);
+        //zfSetKey(dev, 64, 0, 1, wd->macAddr, key);
+    }
+#endif
+
+    // WME settings
+    wd->ws.staWmeEnabled = 1;           // Enable WME by default
+    #define ZM_UAPSD_Q_SIZE 32 //2^N
+    wd->ap.uapsdQ = zfQueueCreate(dev, ZM_UAPSD_Q_SIZE);
+    zm_assert(wd->ap.uapsdQ != NULL);
+    wd->sta.uapsdQ = zfQueueCreate(dev, ZM_UAPSD_Q_SIZE);
+    zm_assert(wd->sta.uapsdQ != NULL);
+
+    //zfHpInit(dev, wd->frequency);
+
+    /* MAC address */
+    //zfHpSetMacAddress(dev, wd->macAddr, 0);
+    zfHpGetMacAddress(dev);
+
+    zfCoreSetFrequency(dev, wd->frequency);
+
+#if ZM_PCI_LOOP_BACK == 1
+    zfwWriteReg(dev, ZM_REG_PCI_CONTROL, 6);
+#endif /* #if ZM_PCI_LOOP_BACK == 1 */
+
+    //zfiWlanSetDot11DMode(dev , 1); // Enable 802.11d
+    //zfiWlanSetDot11HDFSMode(dev , 1); // Enable 802.11h DFS
+    wd->sta.DFSEnable = 1;
+    wd->sta.capability[1] |= ZM_BIT_0;
+
+    //zfiWlanSetFrequency(dev, 5260000, TRUE);
+    //zfiWlanSetAniMode(dev , 1); // Enable ANI
+
+    /* Trgger Rx DMA */
+    zfHpStartRecv(dev);
+
+    zm_debug_msg0("end");
+
+    return 0;
+}
+
+/* WLAN hardware will be shutdown and all resource will be release */
+u16_t zfiWlanClose(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    zm_msg0_init(ZM_LV_0, "enter");
+
+    wd->state = ZM_WLAN_STATE_CLOSEDED;
+
+    //zfiWlanDisable(dev, 1);
+    zfWlanReset(dev);
+
+    zfHpStopRecv(dev);
+
+    /* Disable MAC */
+    /* Disable PHY */
+    /* Disable RF */
+
+    zfHpRelease(dev);
+
+    zfQueueDestroy(dev, wd->ap.uapsdQ);
+    zfQueueDestroy(dev, wd->sta.uapsdQ);
+
+    zfBssInfoDestroy(dev);
+
+#ifdef ZM_ENABLE_AGGREGATION
+    /* add by honda */
+    zfAggRxFreeBuf(dev, 1);  //1 for release structure memory
+    /* end of add by honda */
+#endif
+
+    zm_msg0_init(ZM_LV_0, "exit");
+
+    return 0;
+}
+
+void zfGetWrapperSetting(zdev_t* dev)
+{
+    u8_t   bPassive;
+    u16_t vapId = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+#if 0
+    if ( (wd->ws.countryIsoName[0] != 0)
+         || (wd->ws.countryIsoName[1] != 0)
+         || (wd->ws.countryIsoName[2] != '\0') )
+    {
+        zfHpGetRegulationTablefromRegionCode(
+            dev,
+            zfHpGetRegionCodeFromIsoName(dev, wd->ws.countryIsoName) );
+    }
+#endif
+    zmw_enter_critical_section(dev);
+
+    wd->wlanMode = wd->ws.wlanMode;
+
+    /* set channel */
+    if ( wd->ws.frequency )
+    {
+        wd->frequency = wd->ws.frequency;
+        wd->ws.frequency = 0;
+    }
+    else
+    {
+        wd->frequency = zfChGetFirstChannel(dev, &bPassive);
+
+        if ( wd->wlanMode == ZM_MODE_IBSS )
+        {
+            if (wd->ws.adhocMode == ZM_ADHOCBAND_A)
+            {
+                wd->frequency = ZM_CH_A_36;
+            }
+            else
+            {
+            wd->frequency = ZM_CH_G_6;
+            }
+        }
+    }
+#ifdef ZM_AP_DEBUG
+    /* honda add for debug, 2437 channel 6, 2452 channel 9 */
+    wd->frequency = 2437;
+    /* end of add by honda */
+#endif
+
+    /* set preamble type */
+    switch (wd->ws.preambleType)
+    {
+    case ZM_PREAMBLE_TYPE_AUTO:
+    case ZM_PREAMBLE_TYPE_SHORT:
+    case ZM_PREAMBLE_TYPE_LONG:
+        wd->preambleType = wd->ws.preambleType;
+        break;
+    default:
+        wd->preambleType = ZM_PREAMBLE_TYPE_SHORT;
+        break;
+    }
+    wd->ws.preambleType = 0;
+
+    if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        vapId = zfwGetVapId(dev);
+
+        if (vapId == 0xffff)
+        {
+            wd->ap.authAlgo[0] = wd->ws.authMode;
+            wd->ap.encryMode[0] = wd->ws.encryMode;
+        }
+        else
+        {
+            wd->ap.authAlgo[vapId + 1] = wd->ws.authMode;
+            wd->ap.encryMode[vapId + 1] = wd->ws.encryMode;
+        }
+        wd->ws.authMode = 0;
+        wd->ws.encryMode = ZM_NO_WEP;
+
+        /* Get beaconInterval from WrapperSetting */
+        if ((wd->ws.beaconInterval >= 20) && (wd->ws.beaconInterval <= 1000))
+        {
+            wd->beaconInterval = wd->ws.beaconInterval;
+        }
+        else
+        {
+            wd->beaconInterval = 100; //100ms
+        }
+
+        if (wd->ws.dtim > 0)
+        {
+            wd->dtim = wd->ws.dtim;
+        }
+        else
+        {
+            wd->dtim = 1;
+        }
+
+        wd->ap.qosMode = wd->ws.apWmeEnabled & 0x1;
+        wd->ap.uapsdEnabled = (wd->ws.apWmeEnabled & 0x2) >> 1;
+    }
+    else
+    {
+        wd->sta.authMode = wd->ws.authMode;
+        wd->sta.currentAuthMode = wd->ws.authMode;
+        wd->sta.wepStatus = wd->ws.wepStatus;
+
+        if ( wd->ws.beaconInterval )
+        {
+            wd->beaconInterval = wd->ws.beaconInterval;
+        }
+        else
+        {
+            wd->beaconInterval = 0x64;
+        }
+
+        if ( wd->wlanMode == ZM_MODE_IBSS )
+        {
+            /* 1. Set default channel 6 (2437MHz) */
+//            wd->frequency = 2437;
+
+            /* 2. Otus support 802.11g Mode */
+            if ((wd->ws.adhocMode == ZM_ADHOCBAND_G) ||
+                (wd->ws.adhocMode == ZM_ADHOCBAND_BG) ||
+                (wd->ws.adhocMode == ZM_ADHOCBAND_ABG) ) {
+                wd->wfc.bIbssGMode = 1;
+            } else {
+                wd->wfc.bIbssGMode = 0;
+            }
+
+            /* 3. set short preamble  */
+            //wd->sta.preambleType = ZM_PREAMBLE_TYPE_SHORT ;
+        }
+
+        /* set ATIM window */
+        if ( wd->ws.atimWindow )
+        {
+            wd->sta.atimWindow = wd->ws.atimWindow;
+        }
+        else
+        {
+            //wd->sta.atimWindow = 0x0a;
+            wd->sta.atimWindow = 0;
+        }
+
+        //wd->sta.connectingHiddenAP = 1;//wd->ws.connectingHiddenAP;
+        wd->sta.dropUnencryptedPkts = wd->ws.dropUnencryptedPkts;
+        wd->sta.ibssJoinOnly = wd->ws.ibssJoinOnly;
+
+        if ( wd->ws.bDesiredBssid )
+        {
+            zfMemoryCopy(wd->sta.desiredBssid, wd->ws.desiredBssid, 6);
+            wd->sta.bDesiredBssid = TRUE;
+            wd->ws.bDesiredBssid = FALSE;
+        }
+        else
+        {
+            wd->sta.bDesiredBssid = FALSE;
+        }
+
+        /* check ssid */
+        if ( wd->ws.ssidLen != 0 )
+        {
+            if ( (!zfMemoryIsEqual(wd->ws.ssid, wd->sta.ssid,
+                                   wd->sta.ssidLen))||
+                 (wd->ws.ssidLen != wd->sta.ssidLen)||
+                 (wd->sta.authMode == ZM_AUTH_MODE_WPA)||
+                 (wd->sta.authMode == ZM_AUTH_MODE_WPAPSK) ||
+                 (wd->ws.staWmeQosInfo!= 0) )
+            {
+                /*if u-APSD test(set QosInfo), clear connectByReasso to do association (not reassociation)*/
+                wd->sta.connectByReasso = FALSE;
+                wd->sta.failCntOfReasso = 0;
+                wd->sta.pmkidInfo.bssidCount = 0;
+
+                wd->sta.ssidLen = wd->ws.ssidLen;
+                zfMemoryCopy(wd->sta.ssid, wd->ws.ssid, wd->sta.ssidLen);
+
+                if ( wd->sta.ssidLen < 32 )
+                {
+                    wd->sta.ssid[wd->sta.ssidLen] = 0;
+                }
+            }
+        }
+        else
+        {   /* ANY BSS */
+            wd->sta.ssid[0] = 0;
+            wd->sta.ssidLen = 0;
+        }
+
+        wd->sta.wmeEnabled = wd->ws.staWmeEnabled;
+        wd->sta.wmeQosInfo = wd->ws.staWmeQosInfo;
+
+    }
+
+    zmw_leave_critical_section(dev);
+}
+
+u16_t zfWlanEnable(zdev_t* dev)
+{
+    u8_t     bssid[6] = {0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if ( wd->wlanMode == ZM_MODE_UNKNOWN )
+    {
+        zm_debug_msg0("Unknown Mode...Skip...");
+        return 0;
+    }
+
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        u16_t vapId;
+
+        vapId = zfwGetVapId(dev);
+
+        if (vapId == 0xffff)
+        {
+            /* AP mode */
+            zfApInitStaTbl(dev);
+
+            /* AP default parameters */
+            wd->bRate = 0xf;
+            wd->gRate = 0xff;
+            wd->bRateBasic = 0xf;
+            wd->gRateBasic = 0x0;
+            //wd->beaconInterval = 100;
+            wd->ap.apBitmap = 1;
+            wd->ap.beaconCounter = 0;
+            //wd->ap.vapNumber = 1;    //mark by ygwei for Vap
+
+            wd->ap.hideSsid[0] = 0;
+            wd->ap.staAgingTimeSec = 10*60;
+            wd->ap.staProbingTimeSec = 60;
+
+            for (i=0; i<ZM_MAX_AP_SUPPORT; i++)
+            {
+                wd->ap.bcmcHead[i] = wd->ap.bcmcTail[i] = 0;
+            }
+
+            //wd->ap.uniHead = wd->ap.uniTail = 0;
+
+            /* load AP parameters */
+            wd->bRateBasic = wd->ws.bRateBasic;
+            wd->gRateBasic = wd->ws.gRateBasic;
+            wd->bgMode = wd->ws.bgMode;
+            if ((wd->ws.ssidLen <= 32) && (wd->ws.ssidLen != 0))
+            {
+                wd->ap.ssidLen[0] = wd->ws.ssidLen;
+                for(i=0; i<wd->ws.ssidLen; i++)
+                {
+                    wd->ap.ssid[0][i] = wd->ws.ssid[i];
+                }
+                wd->ws.ssidLen = 0; // Reset Wrapper Variable
+            }
+
+            if (wd->ap.encryMode[0] == 0)
+            {
+                wd->ap.capab[0] = 0x001;
+            }
+            else
+            {
+                wd->ap.capab[0] = 0x011;
+            }
+            /* set Short Slot Time bit if not 11b */
+            if (wd->ap.wlanType[0] != ZM_WLAN_TYPE_PURE_B)
+            {
+              wd->ap.capab[0] |= 0x400;
+            }
+
+            // wd->ap.vapNumber = 1;    // mark by ygwei for Vap Test
+        }
+        else
+        {
+#if 0
+            /* VAP Test Code */
+            wd->ap.apBitmap = 0x3;
+            wd->ap.capab[1] = 0x401;
+            wd->ap.ssidLen[1] = 4;
+            wd->ap.ssid[1][0] = 'v';
+            wd->ap.ssid[1][1] = 'a';
+            wd->ap.ssid[1][2] = 'p';
+            wd->ap.ssid[1][3] = '1';
+            wd->ap.authAlgo[1] = wd->ws.authMode;
+            wd->ap.encryMode[1] = wd->ws.encryMode;
+            wd->ap.vapNumber = 2;
+#else
+            /* VAP Test Code */
+            wd->ap.apBitmap = 0x1 | (0x01 << (vapId+1));
+
+            if ((wd->ws.ssidLen <= 32) && (wd->ws.ssidLen != 0))
+            {
+                wd->ap.ssidLen[vapId+1] = wd->ws.ssidLen;
+                for(i=0; i<wd->ws.ssidLen; i++)
+                {
+                    wd->ap.ssid[vapId+1][i] = wd->ws.ssid[i];
+                }
+                wd->ws.ssidLen = 0; // Reset Wrapper Variable
+            }
+
+            if (wd->ap.encryMode[vapId+1] == 0)
+            {
+                wd->ap.capab[vapId+1] = 0x401;
+            }
+            else
+            {
+                wd->ap.capab[vapId+1] = 0x411;
+            }
+
+            wd->ap.authAlgo[vapId+1] = wd->ws.authMode;
+            wd->ap.encryMode[vapId+1] = wd->ws.encryMode;
+
+            /* Need to be modified when VAP is used */
+            //wd->ap.vapNumber = 2;
+#endif
+        }
+
+        wd->ap.vapNumber++;
+
+        zfCoreSetFrequency(dev, wd->frequency);
+
+        zfInitMacApMode(dev);
+
+        /* Disable protection mode */
+        zfApSetProtectionMode(dev, 0);
+
+        zfApSendBeacon(dev);
+    } /*if (wd->wlanMode == ZM_MODE_AP) */
+    else
+    {
+        zfScanMgrScanStop(dev, ZM_SCAN_MGR_SCAN_INTERNAL);
+        zfScanMgrScanStop(dev, ZM_SCAN_MGR_SCAN_EXTERNAL);
+
+        zmw_enter_critical_section(dev);
+        wd->sta.oppositeCount = 0;    /* reset opposite count */
+        //wd->sta.bAutoReconnect = wd->sta.bAutoReconnectEnabled;
+        //wd->sta.scanWithSSID = 0;
+        zfStaInitOppositeInfo(dev);
+        zmw_leave_critical_section(dev);
+
+        zfStaResetStatus(dev, 0);
+
+        if ( (wd->sta.cmDisallowSsidLength != 0)&&
+             (wd->sta.ssidLen == wd->sta.cmDisallowSsidLength)&&
+             (zfMemoryIsEqual(wd->sta.ssid, wd->sta.cmDisallowSsid,
+                              wd->sta.ssidLen)) &&
+             (wd->sta.wepStatus == ZM_ENCRYPTION_TKIP))
+        {   /* countermeasures */
+            zm_debug_msg0("countermeasures disallow association");
+
+        }
+        else
+        {
+            switch( wd->wlanMode )
+            {
+                case ZM_MODE_IBSS:
+                    /* some registers may be set here */
+                    if ( wd->sta.authMode == ZM_AUTH_MODE_WPA2PSK )
+                    {
+                        zfHpSetApStaMode(dev, ZM_HAL_80211_MODE_IBSS_WPA2PSK);
+                    }
+                    else
+                    {
+                        zfHpSetApStaMode(dev, ZM_HAL_80211_MODE_IBSS_GENERAL);
+                    }
+
+                    zm_msg0_mm(ZM_LV_0, "ZM_MODE_IBSS");
+                    zfIbssConnectNetwork(dev);
+                    break;
+
+                case ZM_MODE_INFRASTRUCTURE:
+                    /* some registers may be set here */
+                    zfHpSetApStaMode(dev, ZM_HAL_80211_MODE_STA);
+
+                    zfInfraConnectNetwork(dev);
+                    break;
+
+                case ZM_MODE_PSEUDO:
+                    /* some registers may be set here */
+                    zfHpSetApStaMode(dev, ZM_HAL_80211_MODE_STA);
+
+                    zfUpdateBssid(dev, bssid);
+                    zfCoreSetFrequency(dev, wd->frequency);
+                    break;
+
+                default:
+                    break;
+            }
+        }
+
+    }
+
+
+    //if ( (wd->wlanMode != ZM_MODE_INFRASTRUCTURE)&&
+    //     (wd->wlanMode != ZM_MODE_AP) )
+    if ( wd->wlanMode == ZM_MODE_PSEUDO )
+    {
+        /* Reset Wlan status */
+        zfWlanReset(dev);
+
+        if (wd->zfcbConnectNotify != NULL)
+        {
+            wd->zfcbConnectNotify(dev, ZM_STATUS_MEDIA_CONNECT, wd->sta.bssid);
+        }
+        zfChangeAdapterState(dev, ZM_STA_STATE_CONNECTED);
+    }
+
+
+    if(wd->wlanMode == ZM_MODE_AP)
+    {
+        if (wd->zfcbConnectNotify != NULL)
+        {
+            wd->zfcbConnectNotify(dev, ZM_STATUS_MEDIA_CONNECT, wd->sta.bssid);
+        }
+        //zfChangeAdapterState(dev, ZM_STA_STATE_CONNECTED);
+    }
+
+    // Assign default Tx Rate
+    if ( wd->sta.EnableHT )
+    {
+		u32_t oneTxStreamCap;
+		oneTxStreamCap = (zfHpCapability(dev) & ZM_HP_CAP_11N_ONE_TX_STREAM);
+		if(oneTxStreamCap)
+			wd->CurrentTxRateKbps = 135000;
+		else
+			wd->CurrentTxRateKbps = 270000;
+        wd->CurrentRxRateKbps = 270000;
+    }
+    else
+    {
+        wd->CurrentTxRateKbps = 54000;
+        wd->CurrentRxRateKbps = 54000;
+    }
+
+    wd->state = ZM_WLAN_STATE_ENABLED;
+
+    return 0;
+}
+
+/* Enable/disable Wlan operation */
+u16_t zfiWlanEnable(zdev_t* dev)
+{
+    u16_t ret;
+
+    zmw_get_wlan_dev(dev);
+
+    zm_msg0_mm(ZM_LV_1, "Enable Wlan");
+
+    zfGetWrapperSetting(dev);
+
+    zfZeroMemory((u8_t*) &wd->trafTally, sizeof(struct zsTrafTally));
+
+    // Reset cmMicFailureCount to 0 for new association request
+    if ( wd->sta.cmMicFailureCount == 1 )
+    {
+        zfTimerCancel(dev, ZM_EVENT_CM_TIMER);
+        wd->sta.cmMicFailureCount = 0;
+    }
+
+    zfFlushVtxq(dev);
+    if ((wd->queueFlushed & 0x10) != 0)
+    {
+        zfHpUsbReset(dev);
+    }
+    ret = zfWlanEnable(dev);
+
+    return ret;
+}
+/* Add a flag named ResetKeyCache to show if KeyCache should be cleared.
+   for hostapd in AP mode, if driver receives iwconfig ioctl
+   after setting group key, it shouldn't clear KeyCache.                 */
+u16_t zfiWlanDisable(zdev_t* dev, u8_t ResetKeyCache)
+{
+    u16_t  i;
+    u8_t isConnected;
+
+    zmw_get_wlan_dev(dev);
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+    zmw_declare_for_critical_section();
+#endif
+    wd->state = ZM_WLAN_STATE_DISABLED;
+
+    zm_msg0_mm(ZM_LV_1, "Disable Wlan");
+
+    if ( wd->wlanMode != ZM_MODE_AP )
+    {
+        isConnected = zfStaIsConnected(dev);
+
+        if ( (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)&&
+             (wd->sta.currentAuthMode != ZM_AUTH_MODE_WPA2) )
+        {
+            /* send deauthentication frame */
+            if (isConnected)
+            {
+                //zfiWlanDeauth(dev, NULL, 0);
+                zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH, wd->sta.bssid, 3, 0, 0);
+                //zmw_debug_msg0("send a Deauth frame!");
+            }
+        }
+
+        // Remove all the connected peer stations
+        if ( wd->wlanMode == ZM_MODE_IBSS )
+        {
+            wd->sta.ibssBssIsCreator = 0;
+            zfTimerCancel(dev, ZM_EVENT_IBSS_MONITOR);
+            zfStaIbssMonitoring(dev, 1);
+        }
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+        zmw_enter_critical_section(dev);
+        wd->sta.ibssWpa2Psk = 0;
+        zmw_leave_critical_section(dev);
+#endif
+
+        wd->sta.wpaState = ZM_STA_WPA_STATE_INIT;
+
+        /* reset connect timeout counter */
+        wd->sta.connectTimeoutCount = 0;
+
+        /* reset connectState to None */
+        wd->sta.connectState = ZM_STA_CONN_STATE_NONE;
+
+        /* reset leap enable variable */
+        wd->sta.leapEnabled = 0;
+
+        /* Disable the RIFS Status / RIFS-like frame count / RIFS count */
+        if( wd->sta.rifsState == ZM_RIFS_STATE_DETECTED )
+            zfHpDisableRifs(dev);
+        wd->sta.rifsState = ZM_RIFS_STATE_DETECTING;
+        wd->sta.rifsLikeFrameCnt = 0;
+        wd->sta.rifsCount = 0;
+
+        wd->sta.osRxFilter = 0;
+        wd->sta.bSafeMode = 0;
+
+        zfChangeAdapterState(dev, ZM_STA_STATE_DISCONNECT);
+        if (ResetKeyCache)
+            zfHpResetKeyCache(dev);
+
+        if (isConnected)
+        {
+            if (wd->zfcbConnectNotify != NULL)
+            {
+                wd->zfcbConnectNotify(dev, ZM_STATUS_MEDIA_CONNECTION_DISABLED, wd->sta.bssid);
+            }
+        }
+        else
+        {
+            if (wd->zfcbConnectNotify != NULL)
+            {
+                wd->zfcbConnectNotify(dev, ZM_STATUS_MEDIA_DISABLED, wd->sta.bssid);
+            }
+        }
+    }
+    else //if (wd->wlanMode == ZM_MODE_AP)
+    {
+        for (i=0; i<ZM_MAX_STA_SUPPORT; i++)
+        {
+            /* send deauthentication frame */
+            if (wd->ap.staTable[i].valid == 1)
+            {
+                /* Reason : Sending station is leaving */
+                zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH,
+                        wd->ap.staTable[i].addr, 3, 0, 0);
+            }
+        }
+
+        if (ResetKeyCache)
+            zfHpResetKeyCache(dev);
+
+        wd->ap.vapNumber--;
+    }
+
+    /* stop beacon */
+    zfHpDisableBeacon(dev);
+
+    /* Flush VTxQ and MmQ */
+    zfFlushVtxq(dev);
+    /* Flush AP PS queues */
+    zfApFlushBufferedPsFrame(dev);
+    /* Free buffer in defragment list*/
+    zfAgingDefragList(dev, 1);
+
+    #ifdef ZM_ENABLE_AGGREGATION
+    /* add by honda */
+    zfAggRxFreeBuf(dev, 0);  //1 for release structure memory
+    /* end of add by honda */
+    #endif
+
+    // Clear the information for the peer stations of IBSS or AP of Station mode
+    zfZeroMemory((u8_t*)wd->sta.oppositeInfo, sizeof(struct zsOppositeInfo) * ZM_MAX_OPPOSITE_COUNT);
+
+    /* Turn off Software WEP/TKIP */
+    if (wd->sta.SWEncryptEnable != 0)
+    {
+        zm_debug_msg0("Disable software encryption");
+        zfStaDisableSWEncryption(dev);
+    }
+
+    /* Improve WEP/TKIP performace with HT AP, detail information please look bug#32495 */
+    //zfHpSetTTSIFSTime(dev, 0x8);
+
+    return 0;
+}
+
+u16_t zfiWlanSuspend(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    // Change the HAL state to init so that any packet can't be transmitted between
+    // resume & HAL reinit. This would cause the chip hang issue in OTUS.
+    zmw_enter_critical_section(dev);
+    wd->halState = ZM_HAL_STATE_INIT;
+    zmw_leave_critical_section(dev);
+
+    return 0;
+}
+
+u16_t zfiWlanResume(zdev_t* dev, u8_t doReconn)
+{
+    u16_t ret;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    /* Redownload firmware, Reinit MAC,PHY,RF */
+    zfHpReinit(dev, wd->frequency);
+
+    //Set channel according to AP's configuration
+    zfCoreSetFrequencyExV2(dev, wd->frequency, wd->BandWidth40,
+            wd->ExtOffset, NULL, 1);
+
+    zfHpSetMacAddress(dev, wd->macAddr, 0);
+
+    /* Start Rx */
+    zfHpStartRecv(dev);
+
+    zfFlushVtxq(dev);
+
+    if ( wd->wlanMode != ZM_MODE_INFRASTRUCTURE &&
+         wd->wlanMode != ZM_MODE_IBSS )
+    {
+        return 1;
+    }
+
+    zm_msg0_mm(ZM_LV_1, "Resume Wlan");
+    if ( (zfStaIsConnected(dev)) || (zfStaIsConnecting(dev)) )
+    {
+        if (doReconn == 1)
+        {
+            zm_msg0_mm(ZM_LV_1, "Re-connect...");
+            zmw_enter_critical_section(dev);
+            wd->sta.connectByReasso = FALSE;
+            zmw_leave_critical_section(dev);
+
+            zfWlanEnable(dev);
+        }
+        else if (doReconn == 0)
+        {
+            zfHpSetRollCallTable(dev);
+        }
+    }
+
+    ret = 0;
+
+    return ret;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                 zfiWlanFlushAllQueuedBuffers */
+/*      Flush Virtual TxQ, MmQ, PS frames and defragment list           */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.1      */
+/*                                                                      */
+/************************************************************************/
+void zfiWlanFlushAllQueuedBuffers(zdev_t* dev)
+{
+    /* Flush VTxQ and MmQ */
+    zfFlushVtxq(dev);
+    /* Flush AP PS queues */
+    zfApFlushBufferedPsFrame(dev);
+    /* Free buffer in defragment list*/
+    zfAgingDefragList(dev, 1);
+}
+
+/* Do WLAN site survey */
+u16_t zfiWlanScan(zdev_t* dev)
+{
+    u16_t ret = 1;
+    zmw_get_wlan_dev(dev);
+
+    zm_debug_msg0("");
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        wd->heartBeatNotification |= ZM_BSSID_LIST_SCAN;
+        wd->sta.scanFrequency = 0;
+        //wd->sta.pUpdateBssList->bssCount = 0;
+        ret = 0;
+    }
+    else
+    {
+        #if 0
+        if ( !zfStaBlockWlanScan(dev) )
+        {
+            zm_debug_msg0("scan request");
+            //zfTimerSchedule(dev, ZM_EVENT_SCAN, ZM_TICK_ZERO);
+            ret = 0;
+            goto start_scan;
+        }
+        #else
+            goto start_scan;
+        #endif
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return ret;
+
+start_scan:
+    zmw_leave_critical_section(dev);
+
+    if(wd->ledStruct.LEDCtrlFlagFromReg & ZM_LED_CTRL_FLAG_ALPHA)   // flag for Alpha
+        wd->ledStruct.LEDCtrlFlag |= ZM_LED_CTRL_FLAG_ALPHA;
+
+    ret = zfScanMgrScanStart(dev, ZM_SCAN_MGR_SCAN_EXTERNAL);
+
+    zm_debug_msg1("ret = ", ret);
+
+    return ret;
+}
+
+
+/* rate         */
+/*    0 : AUTO  */
+/*    1 : CCK 1M */
+/*    2 : CCK 2M */
+/*    3 : CCK 5.5M */
+/*    4 : CCK 11M */
+/*    5 : OFDM 6M */
+/*    6 : OFDM 9M */
+/*    7 : OFDM 12M */
+/*    8 : OFDM 18M */
+/*    9 : OFDM 24M */
+/*    10 : OFDM 36M */
+/*    11 : OFDM 48M */
+/*    12 : OFDM 54M */
+/*    13 : MCS 0 */
+/*    28 : MCS 15 */
+u16_t zcRateToMCS[] =
+    {0xff, 0, 1, 2, 3, 0xb, 0xf, 0xa, 0xe, 0x9, 0xd, 0x8, 0xc};
+u16_t zcRateToMT[] = {0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1};
+
+u16_t zfiWlanSetTxRate(zdev_t* dev, u16_t rate)
+{   // jhlee HT 0
+    zmw_get_wlan_dev(dev);
+
+    if (rate <=12)
+    {
+        wd->txMCS = zcRateToMCS[rate];
+        wd->txMT = zcRateToMT[rate];
+        return ZM_SUCCESS;
+    }
+    else if ((rate<=28)||(rate==13+32))
+    {
+        wd->txMCS = rate - 12 - 1;
+        wd->txMT = 2;
+        return ZM_SUCCESS;
+    }
+
+    return ZM_ERR_INVALID_TX_RATE;
+}
+
+const u32_t zcRateIdToKbps40M[] =
+    {
+        1000, 2000, 5500, 11000,                  /* 1M, 2M, 5M, 11M          ,  0  1  2  3*/
+        6000, 9000, 12000, 18000,                 /* 6M  9M  12M  18M         ,  4  5  6  7*/
+        24000, 36000, 48000, 54000,               /* 24M  36M  48M  54M       ,  8  9 10 11*/
+        13500, 27000, 40500, 54000,               /* MCS0 MCS1 MCS2 MCS3      , 12 13 14 15*/
+        81000, 108000, 121500, 135000,            /* MCS4 MCS5 MCS6 MCS7      , 16 17 18 19*/
+        27000, 54000, 81000, 108000,              /* MCS8 MCS9 MCS10 MCS11    , 20 21 22 23*/
+        162000, 216000, 243000, 270000,           /* MCS12 MCS13 MCS14 MCS15  , 24 25 26 27*/
+        270000, 300000, 150000                    /* MCS14SG, MCS15SG, MCS7SG , 28 29 30*/
+    };
+
+const u32_t zcRateIdToKbps20M[] =
+    {
+        1000, 2000, 5500, 11000,                  /* 1M, 2M, 5M, 11M          ,  0  1  2  3*/
+        6000, 9000, 12000, 18000,                 /* 6M  9M  12M  18M         ,  4  5  6  7*/
+        24000, 36000, 48000, 54000,               /* 24M  36M  48M  54M       ,  8  9 10 11*/
+        6500, 13000, 19500, 26000,                /* MCS0 MCS1 MCS2 MCS3      , 12 13 14 15*/
+        39000, 52000, 58500, 65000,               /* MCS4 MCS5 MCS6 MCS7      , 16 17 18 19*/
+        13000, 26000, 39000, 52000,               /* MCS8 MCS9 MCS10 MCS11    , 20 21 22 23*/
+        78000, 104000, 117000, 130000,            /* MCS12 MCS13 MCS14 MCS15  , 24 25 26 27*/
+        130000, 144400, 72200                     /* MCS14SG, MCS15SG, MSG7SG , 28 29 30*/
+    };
+
+u32_t zfiWlanQueryTxRate(zdev_t* dev)
+{
+    u8_t rateId = 0xff;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    /* If Tx rate had not been trained, return maximum Tx rate instead */
+    if ((wd->wlanMode == ZM_MODE_INFRASTRUCTURE) && (zfStaIsConnected(dev)))
+    {
+        zmw_enter_critical_section(dev);
+        //Not in fixed rate mode
+        if (wd->txMCS == 0xff)
+        {
+            if ((wd->sta.oppositeInfo[0].rcCell.flag & ZM_RC_TRAINED_BIT) == 0)
+            {
+                rateId = wd->sta.oppositeInfo[0].rcCell.operationRateSet[wd->sta.oppositeInfo[0].rcCell.operationRateCount-1];
+            }
+            else
+            {
+                rateId = wd->sta.oppositeInfo[0].rcCell.operationRateSet[wd->sta.oppositeInfo[0].rcCell.currentRateIndex];
+            }
+        }
+        zmw_leave_critical_section(dev);
+    }
+    if (rateId != 0xff)
+    {
+        if (wd->sta.htCtrlBandwidth)
+        {
+            return zcRateIdToKbps40M[rateId];
+        }
+        else
+        {
+            return zcRateIdToKbps20M[rateId];
+        }
+    }
+    else
+    {
+        return wd->CurrentTxRateKbps;
+    }
+}
+
+void zfWlanUpdateRxRate(zdev_t* dev, struct zsAdditionInfo* addInfo)
+{
+    u32_t rxRateKbps;
+    zmw_get_wlan_dev(dev);
+    //zm_msg1_mm(ZM_LV_0, "addInfo->Tail.Data.RxMacStatus =", addInfo->Tail.Data.RxMacStatus & 0x03);
+
+    /* b5~b4: MPDU indication.                */
+    /*        00: Single MPDU.                */
+    /*        10: First MPDU of A-MPDU.       */
+    /*        11: Middle MPDU of A-MPDU.      */
+    /*        01: Last MPDU of A-MPDU.        */
+    /* Only First MPDU and Single MPDU have PLCP header */
+    /* First MPDU  : (mpduInd & 0x30) == 0x00 */
+    /* Single MPDU : (mpduInd & 0x30) == 0x20 */
+    if ((addInfo->Tail.Data.RxMacStatus & 0x10) == 0)
+    {
+        /* Modulation type */
+        wd->modulationType = addInfo->Tail.Data.RxMacStatus & 0x03;
+        switch(wd->modulationType)
+        {
+    		case 0x0: wd->rateField = addInfo->PlcpHeader[0] & 0xff; //CCK mode
+                      wd->rxInfo = 0;
+                      break;
+    		case 0x1: wd->rateField = addInfo->PlcpHeader[0] & 0x0f; //Legacy-OFDM mode
+                      wd->rxInfo = 0;
+                      break;
+    		case 0x2: wd->rateField = addInfo->PlcpHeader[3]; //HT-OFDM mode
+    		          wd->rxInfo = addInfo->PlcpHeader[6];
+                      break;
+            default: break;
+         }
+
+        rxRateKbps = zfUpdateRxRate(dev);
+        if (wd->CurrentRxRateUpdated == 1)
+        {
+            if (rxRateKbps > wd->CurrentRxRateKbps)
+            {
+                wd->CurrentRxRateKbps = rxRateKbps;
+            }
+        }
+        else
+        {
+            wd->CurrentRxRateKbps = rxRateKbps;
+            wd->CurrentRxRateUpdated = 1;
+        }
+    }
+}
+#if 0
+u16_t zcIndextoRateBG[16] = {1000, 2000, 5500, 11000, 0, 0, 0, 0, 48000,
+                               24000, 12000, 6000, 54000, 36000, 18000, 9000};
+u32_t zcIndextoRateN20L[16] = {6500, 13000, 19500, 26000, 39000, 52000, 58500,
+                              65000, 13000, 26000, 39000, 52000, 78000, 104000,
+                              117000, 130000};
+u32_t zcIndextoRateN20S[16] = {7200, 14400, 21700, 28900, 43300, 57800, 65000,
+                              72200, 14400, 28900, 43300, 57800, 86700, 115600,
+                              130000, 144400};
+u32_t zcIndextoRateN40L[16] = {13500, 27000, 40500, 54000, 81000, 108000, 121500,
+                              135000, 27000, 54000, 81000, 108000, 162000, 216000,
+                              243000, 270000};
+u32_t zcIndextoRateN40S[16] = {15000, 30000, 45000, 60000, 90000, 120000, 135000,
+                              150000, 30000, 60000, 90000, 120000, 180000, 240000,
+                              270000, 300000};
+#endif
+
+extern u16_t zcIndextoRateBG[16];
+extern u32_t zcIndextoRateN20L[16];
+extern u32_t zcIndextoRateN20S[16];
+extern u32_t zcIndextoRateN40L[16];
+extern u32_t zcIndextoRateN40S[16];
+
+u32_t zfiWlanQueryRxRate(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->CurrentRxRateUpdated = 0;
+    return wd->CurrentRxRateKbps;
+}
+
+u32_t zfUpdateRxRate(zdev_t* dev)
+{
+    u8_t mcs, bandwidth;
+    u32_t rxRateKbps = 130000;
+    zmw_get_wlan_dev(dev);
+
+    switch (wd->modulationType)
+    {
+		case 0x0: //CCK mode
+                  switch (wd->rateField)
+                  {
+                      case 0x0a: rxRateKbps = 1000;
+                            break;
+                      case 0x14: rxRateKbps = 2000;
+
+                      case 0x37: rxRateKbps = 5500;
+                            break;
+                      case 0x6e: rxRateKbps = 11000;
+                            break;
+                      default:
+                            break;
+                  }
+                  break;
+		case 0x1: //Legacy-OFDM mode
+                  if (wd->rateField <= 15)
+                  {
+                      rxRateKbps = zcIndextoRateBG[wd->rateField];
+                  }
+                  break;
+		case 0x2: //HT-OFDM mode
+		          mcs = wd->rateField & 0x7F;
+		          bandwidth = wd->rateField & 0x80;
+                  if (mcs <= 15)
+                  {
+                      if (bandwidth != 0)
+                      {
+                          if((wd->rxInfo & 0x80) != 0)
+                          {
+                              /* Short GI 40 MHz MIMO Rate */
+                              rxRateKbps = zcIndextoRateN40S[mcs];
+                          }
+                          else
+                          {
+                              /* Long GI 40 MHz MIMO Rate */
+                              rxRateKbps = zcIndextoRateN40L[mcs];
+                          }
+                      }
+                      else
+                      {
+                          if((wd->rxInfo & 0x80) != 0)
+                          {
+                              /* Short GI 20 MHz MIMO Rate */
+                              rxRateKbps = zcIndextoRateN20S[mcs];
+                          }
+                          else
+                          {
+                              /* Long GI 20 MHz MIMO Rate */
+                              rxRateKbps = zcIndextoRateN20L[mcs];
+                          }
+                      }
+                  }
+                  break;
+        default:
+                  break;
+    }
+    //zm_msg1_mm(ZM_LV_0, "wd->CurrentRxRateKbps=", wd->CurrentRxRateKbps);
+
+     // ToDo: use bandwith field to define 40MB
+    return rxRateKbps;
+}
+
+/* Get WLAN stastics */
+u16_t zfiWlanGetStatistics(zdev_t* dev)
+{
+    /* Return link statistics */
+    return 0;
+}
+
+u16_t zfiWlanReset(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->state = ZM_WLAN_STATE_DISABLED;
+
+    return zfWlanReset(dev);
+}
+
+/* Reset WLAN */
+u16_t zfWlanReset(zdev_t* dev)
+{
+    u8_t isConnected;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zm_debug_msg0("zfWlanReset");
+
+    isConnected = zfStaIsConnected(dev);
+
+    //if ( wd->wlanMode != ZM_MODE_AP )
+    {
+        if ( (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)&&
+             (wd->sta.currentAuthMode != ZM_AUTH_MODE_WPA2) )
+        {
+            /* send deauthentication frame */
+            if (isConnected)
+            {
+                //zfiWlanDeauth(dev, NULL, 0);
+                zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH, wd->sta.bssid, 3, 0, 0);
+                //zmw_debug_msg0("send a Deauth frame!");
+            }
+        }
+    }
+
+    zfChangeAdapterState(dev, ZM_STA_STATE_DISCONNECT);
+    zfHpResetKeyCache(dev);
+
+    if (isConnected)
+    {
+        //zfiWlanDisable(dev);
+        if (wd->zfcbConnectNotify != NULL)
+        {
+            wd->zfcbConnectNotify(dev, ZM_STATUS_MEDIA_CONNECTION_RESET, wd->sta.bssid);
+        }
+    }
+    else
+    {
+        if (wd->zfcbConnectNotify != NULL)
+        {
+            wd->zfcbConnectNotify(dev, ZM_STATUS_MEDIA_RESET, wd->sta.bssid);
+        }
+    }
+
+    /* stop beacon */
+    zfHpDisableBeacon(dev);
+
+    /* Free buffer in defragment list*/
+    zfAgingDefragList(dev, 1);
+
+    /* Flush VTxQ and MmQ */
+    zfFlushVtxq(dev);
+
+    #ifdef ZM_ENABLE_AGGREGATION
+    /* add by honda */
+    zfAggRxFreeBuf(dev, 0);  //1 for release structure memory
+    /* end of add by honda */
+    #endif
+
+    zfStaRefreshBlockList(dev, 1);
+
+    zmw_enter_critical_section(dev);
+
+    zfTimerCancel(dev, ZM_EVENT_IBSS_MONITOR);
+    zfTimerCancel(dev, ZM_EVENT_CM_BLOCK_TIMER);
+    zfTimerCancel(dev, ZM_EVENT_CM_DISCONNECT);
+
+    wd->sta.connectState = ZM_STA_CONN_STATE_NONE;
+    wd->sta.connectByReasso = FALSE;
+    wd->sta.cmDisallowSsidLength = 0;
+    wd->sta.bAutoReconnect = 0;
+    wd->sta.InternalScanReq = 0;
+    wd->sta.encryMode = ZM_NO_WEP;
+    wd->sta.wepStatus = ZM_ENCRYPTION_WEP_DISABLED;
+    wd->sta.wpaState = ZM_STA_WPA_STATE_INIT;
+    wd->sta.cmMicFailureCount = 0;
+    wd->sta.ibssBssIsCreator = 0;
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+    wd->sta.ibssWpa2Psk = 0;
+#endif
+    /* reset connect timeout counter */
+    wd->sta.connectTimeoutCount = 0;
+
+    /* reset leap enable variable */
+    wd->sta.leapEnabled = 0;
+
+    /* Reset the RIFS Status / RIFS-like frame count / RIFS count */
+    if( wd->sta.rifsState == ZM_RIFS_STATE_DETECTED )
+        zfHpDisableRifs(dev);
+    wd->sta.rifsState = ZM_RIFS_STATE_DETECTING;
+    wd->sta.rifsLikeFrameCnt = 0;
+    wd->sta.rifsCount = 0;
+
+    wd->sta.osRxFilter = 0;
+    wd->sta.bSafeMode = 0;
+
+    // Clear the information for the peer stations of IBSS or AP of Station mode
+    zfZeroMemory((u8_t*)wd->sta.oppositeInfo, sizeof(struct zsOppositeInfo) * ZM_MAX_OPPOSITE_COUNT);
+
+    zmw_leave_critical_section(dev);
+
+    zfScanMgrScanStop(dev, ZM_SCAN_MGR_SCAN_INTERNAL);
+    zfScanMgrScanStop(dev, ZM_SCAN_MGR_SCAN_EXTERNAL);
+
+    /* Turn off Software WEP/TKIP */
+    if (wd->sta.SWEncryptEnable != 0)
+    {
+        zm_debug_msg0("Disable software encryption");
+        zfStaDisableSWEncryption(dev);
+    }
+
+    /* Improve WEP/TKIP performace with HT AP, detail information please look bug#32495 */
+    //zfHpSetTTSIFSTime(dev, 0x8);
+
+    /* Keep Pseudo mode */
+    if ( wd->wlanMode != ZM_MODE_PSEUDO )
+    {
+        wd->wlanMode = ZM_MODE_INFRASTRUCTURE;
+    }
+    return 0;
+}
+
+/* Deauthenticate a STA */
+u16_t zfiWlanDeauth(zdev_t* dev, u16_t* macAddr, u16_t reason)
+{
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        //u16_t id;
+
+        /*
+         * we will reset all key in zfHpResetKeyCache() when call
+         * zfiWlanDisable(), if we want to reset PairwiseKey for each sta,
+         * need to use a nullAddr to let keyindex not match.
+         * otherwise hardware will still find PairwiseKey when AP change
+         * encryption mode from WPA to WEP
+         */
+
+        /*
+        if ((id = zfApFindSta(dev, macAddr)) != 0xffff)
+        {
+            u32_t key[8];
+            u16_t nullAddr[3] = { 0x0, 0x0, 0x0 };
+
+            if (wd->ap.staTable[i].encryMode != ZM_NO_WEP)
+            {
+                zfHpSetApPairwiseKey(dev, nullAddr,
+                        ZM_NO_WEP, &key[0], &key[4], i+1);
+            }
+            //zfHpSetApPairwiseKey(dev, (u16_t *)macAddr,
+            //        ZM_NO_WEP, &key[0], &key[4], id+1);
+            wd->ap.staTable[id].encryMode = ZM_NO_WEP;
+            wd->ap.staTable[id].keyIdx = 0xff;
+        }
+        */
+
+        zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH, macAddr, reason, 0, 0);
+    }
+    else
+    {
+        zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH, wd->sta.bssid, 3, 0, 0);
+    }
+
+    /* Issue DEAUTH command to FW */
+    return 0;
+}
+
+
+/* XP packet filter feature : */
+/* 1=>enable: All multicast address packets, not just the ones enumerated in the multicast address list. */
+/* 0=>disable */
+void zfiWlanSetAllMulticast(zdev_t* dev, u32_t setting)
+{
+    zmw_get_wlan_dev(dev);
+    zm_msg1_mm(ZM_LV_0, "sta.bAllMulticast = ", setting);
+    wd->sta.bAllMulticast = (u8_t)setting;
+}
+
+
+/* HT configure API */
+void zfiWlanSetHTCtrl(zdev_t* dev, u32_t *setting, u32_t forceTxTPC)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->preambleType        = (u8_t)setting[0];
+    wd->sta.preambleTypeHT  = (u8_t)setting[1];
+    wd->sta.htCtrlBandwidth = (u8_t)setting[2];
+    wd->sta.htCtrlSTBC      = (u8_t)setting[3];
+    wd->sta.htCtrlSG        = (u8_t)setting[4];
+    wd->sta.defaultTA       = (u8_t)setting[5];
+    wd->enableAggregation   = (u8_t)setting[6];
+    wd->enableWDS           = (u8_t)setting[7];
+
+    wd->forceTxTPC          = forceTxTPC;
+}
+
+/* FB50 in OS XP, RD private test code */
+void zfiWlanQueryHTCtrl(zdev_t* dev, u32_t *setting, u32_t *forceTxTPC)
+{
+    zmw_get_wlan_dev(dev);
+
+    setting[0] = wd->preambleType;
+    setting[1] = wd->sta.preambleTypeHT;
+    setting[2] = wd->sta.htCtrlBandwidth;
+    setting[3] = wd->sta.htCtrlSTBC;
+    setting[4] = wd->sta.htCtrlSG;
+    setting[5] = wd->sta.defaultTA;
+    setting[6] = wd->enableAggregation;
+    setting[7] = wd->enableWDS;
+
+    *forceTxTPC = wd->forceTxTPC;
+}
+
+void zfiWlanDbg(zdev_t* dev, u8_t setting)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->enableHALDbgInfo = setting;
+}
+
+/* FB50 in OS XP, RD private test code */
+void zfiWlanSetRxPacketDump(zdev_t* dev, u32_t setting)
+{
+    zmw_get_wlan_dev(dev);
+    if (setting)
+    {
+        wd->rxPacketDump = 1;   /* enable */
+    }
+    else
+    {
+        wd->rxPacketDump = 0;   /* disable */
+    }
+}
+
+
+/* FB50 in OS XP, RD private test code */
+/* Tally */
+void zfiWlanResetTally(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+	   wd->commTally.txUnicastFrm = 0;		//txUnicastFrames
+	   wd->commTally.txMulticastFrm = 0;		//txMulticastFrames
+	   wd->commTally.txUnicastOctets = 0;	//txUniOctets  byte size
+	   wd->commTally.txMulticastOctets = 0;	//txMultiOctets  byte size
+	   wd->commTally.txFrmUpperNDIS = 0;
+	   wd->commTally.txFrmDrvMgt = 0;
+	   wd->commTally.RetryFailCnt = 0;
+	   wd->commTally.Hw_TotalTxFrm = 0;		//Hardware total Tx Frame
+	   wd->commTally.Hw_RetryCnt = 0;		//txMultipleRetriesFrames
+	   wd->commTally.Hw_UnderrunCnt = 0;//
+	   wd->commTally.DriverRxFrmCnt = 0;//
+	   wd->commTally.rxUnicastFrm = 0;		//rxUnicastFrames
+	   wd->commTally.rxMulticastFrm = 0;	//rxMulticastFrames
+	   wd->commTally.NotifyNDISRxFrmCnt = 0;//
+	   wd->commTally.rxUnicastOctets = 0;	//rxUniOctets  byte size
+	   wd->commTally.rxMulticastOctets = 0;	//rxMultiOctets  byte size
+	   wd->commTally.DriverDiscardedFrm = 0;// Discard by ValidateFrame
+	   wd->commTally.LessThanDataMinLen = 0;//
+	   wd->commTally.GreaterThanMaxLen = 0;//
+	   wd->commTally.DriverDiscardedFrmCauseByMulticastList = 0;
+	   wd->commTally.DriverDiscardedFrmCauseByFrmCtrl = 0;
+	   wd->commTally.rxNeedFrgFrm = 0;		// need more frg frm
+	   wd->commTally.DriverRxMgtFrmCnt = 0;
+	   wd->commTally.rxBroadcastFrm = 0;	//Receive broadcast frame count
+	   wd->commTally.rxBroadcastOctets = 0;	//Receive broadcast frame byte size
+	   wd->commTally.Hw_TotalRxFrm = 0;//
+	   wd->commTally.Hw_CRC16Cnt = 0;		//rxPLCPCRCErrCnt
+	   wd->commTally.Hw_CRC32Cnt = 0;		//rxCRC32ErrCnt
+	   wd->commTally.Hw_DecrypErr_UNI = 0;//
+	   wd->commTally.Hw_DecrypErr_Mul = 0;//
+	   wd->commTally.Hw_RxFIFOOverrun = 0;//
+	   wd->commTally.Hw_RxTimeOut = 0;
+	   wd->commTally.LossAP = 0;//
+
+	   wd->commTally.Tx_MPDU = 0;
+	   wd->commTally.BA_Fail = 0;
+	   wd->commTally.Hw_Tx_AMPDU = 0;
+	   wd->commTally.Hw_Tx_MPDU = 0;
+
+    wd->commTally.txQosDropCount[0] = 0;
+    wd->commTally.txQosDropCount[1] = 0;
+    wd->commTally.txQosDropCount[2] = 0;
+    wd->commTally.txQosDropCount[3] = 0;
+    wd->commTally.txQosDropCount[4] = 0;
+
+    wd->commTally.Hw_RxMPDU = 0;
+    wd->commTally.Hw_RxDropMPDU = 0;
+    wd->commTally.Hw_RxDelMPDU = 0;
+
+    wd->commTally.Hw_RxPhyMiscError = 0;
+    wd->commTally.Hw_RxPhyXRError = 0;
+    wd->commTally.Hw_RxPhyOFDMError = 0;
+    wd->commTally.Hw_RxPhyCCKError = 0;
+    wd->commTally.Hw_RxPhyHTError = 0;
+    wd->commTally.Hw_RxPhyTotalCount = 0;
+
+#if	(defined(GCCK) && defined(OFDM))
+	   wd->commTally.rx11bDataFrame = 0;
+	   wd->commTally.rxOFDMDataFrame = 0;
+#endif
+
+    zmw_leave_critical_section(dev);
+}
+
+/* FB50 in OS XP, RD private test code */
+void zfiWlanQueryTally(zdev_t* dev, struct zsCommTally *tally)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    zfMemoryCopy((u8_t*)tally, (u8_t*)&wd->commTally, sizeof(struct zsCommTally));
+    zmw_leave_critical_section(dev);
+}
+void zfiWlanQueryTrafTally(zdev_t* dev, struct zsTrafTally *tally)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    zfMemoryCopy((u8_t*)tally, (u8_t*)&wd->trafTally, sizeof(struct zsTrafTally));
+    zmw_leave_critical_section(dev);
+}
+
+void zfiWlanQueryMonHalRxInfo(zdev_t* dev, struct zsMonHalRxInfo *monHalRxInfo)
+{
+    zfHpQueryMonHalRxInfo(dev, (u8_t *)monHalRxInfo);
+}
+
+/* parse the modeMDKEnable to DrvCore */
+void zfiDKEnable(zdev_t* dev, u32_t enable)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->modeMDKEnable = enable;
+    zm_debug_msg1("modeMDKEnable = ", wd->modeMDKEnable);
+}
+
+/* airoPeek */
+u32_t zfiWlanQueryPacketTypePromiscuous(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->swSniffer;
+}
+
+/* airoPeek */
+void zfiWlanSetPacketTypePromiscuous(zdev_t* dev, u32_t setValue)
+{
+    zmw_get_wlan_dev(dev);
+
+	   wd->swSniffer = setValue;
+	   zm_msg1_mm(ZM_LV_0, "wd->swSniffer ", wd->swSniffer);
+	   if (setValue)
+	   {
+	       	/* write register for sniffer mode */
+            zfHpSetSnifferMode(dev, 1);
+            zm_msg0_mm(ZM_LV_1, "enalbe sniffer mode");
+	   }
+	   else
+	   {
+            zfHpSetSnifferMode(dev, 0);
+            zm_msg0_mm(ZM_LV_0, "disalbe sniffer mode");
+	   }
+}
+
+void zfiWlanSetXLinkMode(zdev_t* dev, u32_t setValue)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->XLinkMode = setValue;
+    if (setValue)
+    {
+        /* write register for sniffer mode */
+        zfHpSetSnifferMode(dev, 1);
+    }
+    else
+    {
+        zfHpSetSnifferMode(dev, 0);
+    }
+}
+
+extern void zfStaChannelManagement(zdev_t* dev, u8_t scan);
+void zfiSetChannelManagement(zdev_t* dev, u32_t setting)
+{
+    zmw_get_wlan_dev(dev);
+
+    switch (setting)
+    {
+        case 1:
+            wd->sta.EnableHT = 1;
+            wd->BandWidth40 = 1;
+            wd->ExtOffset   = 1;
+            break;
+        case 3:
+            wd->sta.EnableHT = 1;
+            wd->BandWidth40 = 1;
+            wd->ExtOffset   = 3;
+            break;
+        case 0:
+            wd->sta.EnableHT = 1;
+            wd->BandWidth40 = 0;
+            wd->ExtOffset   = 0;
+            break;
+        default:
+            wd->BandWidth40 = 0;
+            wd->ExtOffset   = 0;
+            break;
+
+    }
+    zfCoreSetFrequencyEx(dev, wd->frequency, wd->BandWidth40,
+                    wd->ExtOffset, NULL);
+}
+
+void zfiSetRifs(zdev_t* dev, u16_t setting)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.ie.HtInfo.ChannelInfo |= ExtHtCap_RIFSMode;
+    wd->sta.EnableHT = 1;
+    switch (setting)
+    {
+        case 0:
+            wd->sta.HT2040 = 0;
+//            zfHpSetRifs(dev, 1, 0, (wd->sta.currentFrequency < 3000)? 1:0);
+            break;
+        case 1:
+            wd->sta.HT2040 = 1;
+//            zfHpSetRifs(dev, 1, 1, (wd->sta.currentFrequency < 3000)? 1:0);
+            break;
+        default:
+            wd->sta.HT2040 = 0;
+//            zfHpSetRifs(dev, 1, 0, (wd->sta.currentFrequency < 3000)? 1:0);
+            break;
+    }
+}
+
+void zfiCheckRifs(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if(wd->sta.ie.HtInfo.ChannelInfo & ExtHtCap_RIFSMode)
+    {
+//        zfHpSetRifs(dev, wd->sta.EnableHT, wd->sta.HT2040, (wd->sta.currentFrequency < 3000)? 1:0);
+    }
+}
+
+void zfiSetReorder(zdev_t* dev, u16_t value)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->reorder = value;
+}
+
+void zfiSetSeqDebug(zdev_t* dev, u16_t value)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->seq_debug = value;
+}
diff --git a/drivers/staging/otus/80211core/cfunc.c b/drivers/staging/otus/80211core/cfunc.c
new file mode 100644
index 00000000000..d7c49d7523d
--- /dev/null
+++ b/drivers/staging/otus/80211core/cfunc.c
@@ -0,0 +1,1227 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include "cprecomp.h"
+
+u8_t zfQueryOppositeRate(zdev_t* dev, u8_t dst_mac[6], u8_t frameType)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* For AP's rate adaption */
+    if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        return 0;
+    }
+
+    /* For STA's rate adaption */
+    if ( (frameType & 0x0c) == ZM_WLAN_DATA_FRAME )
+    {
+        if ( ZM_IS_MULTICAST(dst_mac) )
+        {
+            return wd->sta.mTxRate;
+        }
+        else
+        {
+            return wd->sta.uTxRate;
+        }
+    }
+
+    return wd->sta.mmTxRate;
+}
+
+void zfCopyToIntTxBuffer(zdev_t* dev, zbuf_t* buf, u8_t* src,
+                         u16_t offset, u16_t length)
+{
+    u16_t i;
+
+    for(i=0; i<length;i++)
+    {
+        zmw_tx_buf_writeb(dev, buf, offset+i, src[i]);
+    }
+}
+
+void zfCopyToRxBuffer(zdev_t* dev, zbuf_t* buf, u8_t* src,
+                      u16_t offset, u16_t length)
+{
+    u16_t i;
+
+    for(i=0; i<length;i++)
+    {
+        zmw_rx_buf_writeb(dev, buf, offset+i, src[i]);
+    }
+}
+
+void zfCopyFromIntTxBuffer(zdev_t* dev, zbuf_t* buf, u8_t* dst,
+                           u16_t offset, u16_t length)
+{
+    u16_t i;
+
+    for(i=0; i<length; i++)
+    {
+        dst[i] = zmw_tx_buf_readb(dev, buf, offset+i);
+    }
+}
+
+void zfCopyFromRxBuffer(zdev_t* dev, zbuf_t* buf, u8_t* dst,
+                        u16_t offset, u16_t length)
+{
+    u16_t i;
+
+    for(i=0; i<length; i++)
+    {
+        dst[i] = zmw_rx_buf_readb(dev, buf, offset+i);
+    }
+}
+
+#if 1
+void zfMemoryCopy(u8_t* dst, u8_t* src, u16_t length)
+{
+    zfwMemoryCopy(dst, src, length);
+}
+
+void zfMemoryMove(u8_t* dst, u8_t* src, u16_t length)
+{
+    zfwMemoryMove(dst, src, length);
+}
+
+void zfZeroMemory(u8_t* va, u16_t length)
+{
+    zfwZeroMemory(va, length);
+}
+
+u8_t zfMemoryIsEqual(u8_t* m1, u8_t* m2, u16_t length)
+{
+    return zfwMemoryIsEqual(m1, m2, length);
+}
+#endif
+
+u8_t zfRxBufferEqualToStr(zdev_t* dev, zbuf_t* buf,
+                          const u8_t* str, u16_t offset, u16_t length)
+{
+    u16_t i;
+    u8_t ch;
+
+    for(i=0; i<length; i++)
+    {
+        ch = zmw_rx_buf_readb(dev, buf, offset+i);
+        if ( ch != str[i] )
+        {
+            return FALSE;
+        }
+    }
+
+    return TRUE;
+}
+
+void zfTxBufferCopy(zdev_t*dev, zbuf_t* dst, zbuf_t* src,
+                    u16_t dstOffset, u16_t srcOffset, u16_t length)
+{
+    u16_t i;
+
+    for(i=0; i<length; i++)
+    {
+        zmw_tx_buf_writeb(dev, dst, dstOffset+i,
+                          zmw_tx_buf_readb(dev, src, srcOffset+i));
+    }
+}
+
+void zfRxBufferCopy(zdev_t*dev, zbuf_t* dst, zbuf_t* src,
+                    u16_t dstOffset, u16_t srcOffset, u16_t length)
+{
+    u16_t i;
+
+    for(i=0; i<length; i++)
+    {
+        zmw_rx_buf_writeb(dev, dst, dstOffset+i,
+                             zmw_rx_buf_readb(dev, src, srcOffset+i));
+    }
+}
+
+
+void zfCollectHWTally(zdev_t*dev, u32_t* rsp, u8_t id)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if (id == 0)
+    {
+        wd->commTally.Hw_UnderrunCnt += (0xFFFF & rsp[1]);
+        wd->commTally.Hw_TotalRxFrm += rsp[2];
+        wd->commTally.Hw_CRC32Cnt += rsp[3];
+        wd->commTally.Hw_CRC16Cnt += rsp[4];
+        #ifdef ZM_ENABLE_NATIVE_WIFI
+        /* These code are here to satisfy Vista DTM */
+        wd->commTally.Hw_DecrypErr_UNI += ((rsp[5]>50) && (rsp[5]<60))?50:rsp[5];
+        #else
+        wd->commTally.Hw_DecrypErr_UNI += rsp[5];
+        #endif
+        wd->commTally.Hw_RxFIFOOverrun += rsp[6];
+        wd->commTally.Hw_DecrypErr_Mul += rsp[7];
+        wd->commTally.Hw_RetryCnt += rsp[8];
+        wd->commTally.Hw_TotalTxFrm += rsp[9];
+        wd->commTally.Hw_RxTimeOut +=rsp[10];
+
+        wd->commTally.Tx_MPDU += rsp[11];
+        wd->commTally.BA_Fail += rsp[12];
+        wd->commTally.Hw_Tx_AMPDU += rsp[13];
+        wd->commTally.Hw_Tx_MPDU += rsp[14];
+        wd->commTally.RateCtrlTxMPDU += rsp[11];
+        wd->commTally.RateCtrlBAFail += rsp[12];
+    }
+    else
+    {
+        wd->commTally.Hw_RxMPDU += rsp[1];
+        wd->commTally.Hw_RxDropMPDU += rsp[2];
+        wd->commTally.Hw_RxDelMPDU += rsp[3];
+
+        wd->commTally.Hw_RxPhyMiscError += rsp[4];
+        wd->commTally.Hw_RxPhyXRError += rsp[5];
+        wd->commTally.Hw_RxPhyOFDMError += rsp[6];
+        wd->commTally.Hw_RxPhyCCKError += rsp[7];
+        wd->commTally.Hw_RxPhyHTError += rsp[8];
+        wd->commTally.Hw_RxPhyTotalCount += rsp[9];
+    }
+
+    zmw_leave_critical_section(dev);
+
+    if (id == 0)
+    {
+        zm_msg1_mm(ZM_LV_1, "rsplen =", rsp[0]);
+        zm_msg1_mm(ZM_LV_1, "Hw_UnderrunCnt    = ", (0xFFFF & rsp[1]));
+        zm_msg1_mm(ZM_LV_1, "Hw_TotalRxFrm     = ", rsp[2]);
+        zm_msg1_mm(ZM_LV_1, "Hw_CRC32Cnt       = ", rsp[3]);
+        zm_msg1_mm(ZM_LV_1, "Hw_CRC16Cnt       = ", rsp[4]);
+        zm_msg1_mm(ZM_LV_1, "Hw_DecrypErr_UNI  = ", rsp[5]);
+        zm_msg1_mm(ZM_LV_1, "Hw_RxFIFOOverrun  = ", rsp[6]);
+        zm_msg1_mm(ZM_LV_1, "Hw_DecrypErr_Mul  = ", rsp[7]);
+        zm_msg1_mm(ZM_LV_1, "Hw_RetryCnt       = ", rsp[8]);
+        zm_msg1_mm(ZM_LV_1, "Hw_TotalTxFrm     = ", rsp[9]);
+        zm_msg1_mm(ZM_LV_1, "Hw_RxTimeOut      = ", rsp[10]);
+        zm_msg1_mm(ZM_LV_1, "Tx_MPDU           = ", rsp[11]);
+        zm_msg1_mm(ZM_LV_1, "BA_Fail           = ", rsp[12]);
+        zm_msg1_mm(ZM_LV_1, "Hw_Tx_AMPDU       = ", rsp[13]);
+        zm_msg1_mm(ZM_LV_1, "Hw_Tx_MPDU        = ", rsp[14]);
+    }
+    else
+    {
+        zm_msg1_mm(ZM_LV_1, "rsplen             = ", rsp[0]);
+        zm_msg1_mm(ZM_LV_1, "Hw_RxMPDU          = ", (0xFFFF & rsp[1]));
+        zm_msg1_mm(ZM_LV_1, "Hw_RxDropMPDU      = ", rsp[2]);
+        zm_msg1_mm(ZM_LV_1, "Hw_RxDelMPDU       = ", rsp[3]);
+        zm_msg1_mm(ZM_LV_1, "Hw_RxPhyMiscError  = ", rsp[4]);
+        zm_msg1_mm(ZM_LV_1, "Hw_RxPhyXRError    = ", rsp[5]);
+        zm_msg1_mm(ZM_LV_1, "Hw_RxPhyOFDMError  = ", rsp[6]);
+        zm_msg1_mm(ZM_LV_1, "Hw_RxPhyCCKError   = ", rsp[7]);
+        zm_msg1_mm(ZM_LV_1, "Hw_RxPhyHTError    = ", rsp[8]);
+        zm_msg1_mm(ZM_LV_1, "Hw_RxPhyTotalCount = ", rsp[9]);
+    }
+
+}
+
+/* Timer related functions */
+void zfTimerInit(zdev_t* dev)
+{
+    u8_t   i;
+
+    zmw_get_wlan_dev(dev);
+
+    zm_debug_msg0("");
+
+    wd->timerList.freeCount = ZM_MAX_TIMER_COUNT;
+    wd->timerList.head = &(wd->timerList.list[0]);
+    wd->timerList.tail = &(wd->timerList.list[ZM_MAX_TIMER_COUNT-1]);
+    wd->timerList.head->pre = NULL;
+    wd->timerList.head->next = &(wd->timerList.list[1]);
+    wd->timerList.tail->pre = &(wd->timerList.list[ZM_MAX_TIMER_COUNT-2]);
+    wd->timerList.tail->next = NULL;
+
+    for( i=1; i<(ZM_MAX_TIMER_COUNT-1); i++ )
+    {
+        wd->timerList.list[i].pre = &(wd->timerList.list[i-1]);
+        wd->timerList.list[i].next = &(wd->timerList.list[i+1]);
+    }
+
+    wd->bTimerReady = TRUE;
+}
+
+
+u16_t zfTimerSchedule(zdev_t* dev, u16_t event, u32_t tick)
+{
+    struct zsTimerEntry *pFreeEntry;
+    struct zsTimerEntry *pEntry;
+    u8_t   i, count;
+
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->timerList.freeCount == 0 )
+    {
+        zm_debug_msg0("no more timer");
+        return 1;
+    }
+
+    //zm_debug_msg2("event = ", event);
+    //zm_debug_msg1("target tick = ", wd->tick + tick);
+
+    count = ZM_MAX_TIMER_COUNT - wd->timerList.freeCount;
+
+    if ( count == 0 )
+    {
+        wd->timerList.freeCount--;
+        wd->timerList.head->event = event;
+        wd->timerList.head->timer = wd->tick + tick;
+        //zm_debug_msg1("free timer count = ", wd->timerList.freeCount);
+
+        return 0;
+    }
+
+    pFreeEntry = wd->timerList.tail;
+    pFreeEntry->timer = wd->tick + tick;
+    pFreeEntry->event = event;
+    wd->timerList.tail = pFreeEntry->pre;
+    pEntry = wd->timerList.head;
+
+    for( i=0; i<count; i++ )
+    {
+        // prevent from the case of tick overflow
+        if ( ( pEntry->timer > pFreeEntry->timer )&&
+             ((pEntry->timer - pFreeEntry->timer) < 1000000000) )
+        {
+            if ( i != 0 )
+            {
+                pFreeEntry->pre = pEntry->pre;
+                pFreeEntry->pre->next = pFreeEntry;
+            }
+            else
+            {
+                pFreeEntry->pre = NULL;
+            }
+
+            pEntry->pre = pFreeEntry;
+            pFreeEntry->next = pEntry;
+            break;
+        }
+
+        pEntry = pEntry->next;
+    }
+
+    if ( i == 0 )
+    {
+        wd->timerList.head = pFreeEntry;
+    }
+
+    if ( i == count )
+    {
+        pFreeEntry->pre = pEntry->pre;
+        pFreeEntry->pre->next = pFreeEntry;
+        pEntry->pre = pFreeEntry;
+        pFreeEntry->next = pEntry;
+    }
+
+    wd->timerList.freeCount--;
+    //zm_debug_msg1("free timer count = ", wd->timerList.freeCount);
+
+    return 0;
+}
+
+u16_t zfTimerCancel(zdev_t* dev, u16_t event)
+{
+    struct zsTimerEntry *pEntry;
+    u8_t   i, count;
+
+    zmw_get_wlan_dev(dev);
+
+    //zm_debug_msg2("event = ", event);
+    //zm_debug_msg1("free timer count(b) = ", wd->timerList.freeCount);
+
+    pEntry = wd->timerList.head;
+    count = ZM_MAX_TIMER_COUNT - wd->timerList.freeCount;
+
+    for( i=0; i<count; i++ )
+    {
+        if ( pEntry->event == event )
+        {
+            if ( pEntry == wd->timerList.head )
+            {   /* remove head entry */
+                wd->timerList.head = pEntry->next;
+                wd->timerList.tail->next = pEntry;
+                pEntry->pre = wd->timerList.tail;
+                wd->timerList.tail = pEntry;
+                pEntry = wd->timerList.head;
+            }
+            else
+            {   /* remove non-head entry */
+                pEntry->pre->next = pEntry->next;
+                pEntry->next->pre = pEntry->pre;
+                wd->timerList.tail->next = pEntry;
+                pEntry->pre = wd->timerList.tail;
+                wd->timerList.tail = pEntry;
+                pEntry = pEntry->next;
+            }
+
+            wd->timerList.freeCount++;
+        }
+        else
+        {
+            pEntry = pEntry->next;
+        }
+    }
+
+    //zm_debug_msg1("free timer count(a) = ", wd->timerList.freeCount);
+
+    return 0;
+}
+
+void zfTimerClear(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->timerList.freeCount = ZM_MAX_TIMER_COUNT;
+}
+
+u16_t zfTimerCheckAndHandle(zdev_t* dev)
+{
+    struct zsTimerEntry *pEntry;
+    struct zsTimerEntry *pTheLastEntry = NULL;
+    u16_t  event[ZM_MAX_TIMER_COUNT];
+    u8_t   i, j=0, count;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if ( !wd->bTimerReady )
+    {
+        return 0;
+    }
+
+    zmw_enter_critical_section(dev);
+
+    pEntry = wd->timerList.head;
+    count = ZM_MAX_TIMER_COUNT - wd->timerList.freeCount;
+
+    for( i=0; i<count; i++ )
+    {
+        // prevent from the case of tick overflow
+        if ( ( pEntry->timer > wd->tick )&&
+             ((pEntry->timer - wd->tick) < 1000000000) )
+        {
+            break;
+        }
+
+        event[j++] = pEntry->event;
+        pTheLastEntry = pEntry;
+        pEntry = pEntry->next;
+    }
+
+    if ( j > 0 )
+    {
+        wd->timerList.tail->next = wd->timerList.head;
+        wd->timerList.head->pre = wd->timerList.tail;
+        wd->timerList.head = pEntry;
+        wd->timerList.tail = pTheLastEntry;
+        wd->timerList.freeCount += j;
+        //zm_debug_msg1("free timer count = ", wd->timerList.freeCount);
+    }
+
+    zmw_leave_critical_section(dev);
+
+    zfProcessEvent(dev, event, j);
+
+    return 0;
+}
+
+u32_t zfCoreSetKey(zdev_t* dev, u8_t user, u8_t keyId, u8_t type,
+        u16_t* mac, u32_t* key)
+{
+    u32_t ret;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    wd->sta.flagKeyChanging++;
+    zm_debug_msg1("   zfCoreSetKey++++ ", wd->sta.flagKeyChanging);
+    zmw_leave_critical_section(dev);
+
+    ret = zfHpSetKey(dev, user, keyId, type, mac, key);
+    return ret;
+}
+
+void zfCoreSetKeyComplete(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+#if 0
+    wd->sta.flagKeyChanging = 0;
+#else
+    if(wd->sta.flagKeyChanging)
+    {
+        zmw_enter_critical_section(dev);
+        wd->sta.flagKeyChanging--;
+        zmw_leave_critical_section(dev);
+    }
+#endif
+    zm_debug_msg1("  zfCoreSetKeyComplete--- ", wd->sta.flagKeyChanging);
+
+    zfPushVtxq(dev);
+}
+
+void zfCoreHalInitComplete(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    wd->halState = ZM_HAL_STATE_RUNNING;
+    zmw_leave_critical_section(dev);
+
+    zfPushVtxq(dev);
+}
+
+void zfCoreMacAddressNotify(zdev_t* dev, u8_t* addr)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->macAddr[0] = addr[0] | ((u16_t)addr[1]<<8);
+    wd->macAddr[1] = addr[2] | ((u16_t)addr[3]<<8);
+    wd->macAddr[2] = addr[4] | ((u16_t)addr[5]<<8);
+
+
+    //zfHpSetMacAddress(dev, wd->macAddr, 0);
+    if (wd->zfcbMacAddressNotify != NULL)
+    {
+        wd->zfcbMacAddressNotify(dev, addr);
+    }
+}
+
+void zfCoreSetIsoName(zdev_t* dev, u8_t* isoName)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->ws.countryIsoName[0] = isoName[0];
+    wd->ws.countryIsoName[1] = isoName[1];
+    wd->ws.countryIsoName[2] = '\0';
+ }
+
+
+extern void zfScanMgrScanEventStart(zdev_t* dev);
+extern u8_t zfScanMgrScanEventTimeout(zdev_t* dev);
+extern void zfScanMgrScanEventRetry(zdev_t* dev);
+
+void zfProcessEvent(zdev_t* dev, u16_t* eventArray, u8_t eventCount)
+{
+    u8_t i, j, bypass = FALSE;
+    u16_t eventBypass[32];
+    u8_t eventBypassCount = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zfZeroMemory((u8_t*) eventBypass, 64);
+
+    for( i=0; i<eventCount; i++ )
+    {
+        for( j=0; j<eventBypassCount; j++ )
+        {
+            if ( eventBypass[j] == eventArray[i] )
+            {
+                bypass = TRUE;
+                break;
+            }
+        }
+
+        if ( bypass )
+        {
+            continue;
+        }
+
+        switch( eventArray[i] )
+        {
+            case ZM_EVENT_SCAN:
+                {
+                    zfScanMgrScanEventStart(dev);
+                    eventBypass[eventBypassCount++] = ZM_EVENT_IN_SCAN;
+                    eventBypass[eventBypassCount++] = ZM_EVENT_TIMEOUT_SCAN;
+                }
+                break;
+
+            case ZM_EVENT_TIMEOUT_SCAN:
+                {
+                    u8_t res;
+
+                    res = zfScanMgrScanEventTimeout(dev);
+                    if ( res == 0 )
+                    {
+                        eventBypass[eventBypassCount++] = ZM_EVENT_TIMEOUT_SCAN;
+                    }
+                    else if ( res == 1 )
+                    {
+                        eventBypass[eventBypassCount++] = ZM_EVENT_IN_SCAN;
+                    }
+                }
+                break;
+
+            case ZM_EVENT_IBSS_MONITOR:
+                {
+                    zfStaIbssMonitoring(dev, 0);
+                }
+                break;
+
+            case ZM_EVENT_IN_SCAN:
+                {
+                    zfScanMgrScanEventRetry(dev);
+                }
+                break;
+
+            case ZM_EVENT_CM_TIMER:
+                {
+                    zm_msg0_mm(ZM_LV_0, "ZM_EVENT_CM_TIMER");
+
+                    wd->sta.cmMicFailureCount = 0;
+                }
+                break;
+
+            case ZM_EVENT_CM_DISCONNECT:
+                {
+                    zm_msg0_mm(ZM_LV_0, "ZM_EVENT_CM_DISCONNECT");
+
+                    zfChangeAdapterState(dev, ZM_STA_STATE_DISCONNECT);
+
+                    zmw_enter_critical_section(dev);
+                    //zfTimerSchedule(dev, ZM_EVENT_CM_BLOCK_TIMER,
+                    //                ZM_TICK_CM_BLOCK_TIMEOUT);
+
+                    /* Timer Resolution on WinXP is 15/16 ms  */
+                    /* Decrease Time offset for <XP> Counter Measure */
+                    zfTimerSchedule(dev, ZM_EVENT_CM_BLOCK_TIMER,
+                                         ZM_TICK_CM_BLOCK_TIMEOUT - ZM_TICK_CM_BLOCK_TIMEOUT_OFFSET);
+
+                    zmw_leave_critical_section(dev);
+                    wd->sta.cmMicFailureCount = 0;
+                    //zfiWlanDisable(dev);
+                    zfHpResetKeyCache(dev);
+                    if (wd->zfcbConnectNotify != NULL)
+                    {
+                        wd->zfcbConnectNotify(dev, ZM_STATUS_MEDIA_DISCONNECT_MIC_FAIL,
+                             wd->sta.bssid);
+                    }
+                }
+                break;
+
+            case ZM_EVENT_CM_BLOCK_TIMER:
+                {
+                    zm_msg0_mm(ZM_LV_0, "ZM_EVENT_CM_BLOCK_TIMER");
+
+                    //zmw_enter_critical_section(dev);
+                    wd->sta.cmDisallowSsidLength = 0;
+                    if ( wd->sta.bAutoReconnect )
+                    {
+                        zm_msg0_mm(ZM_LV_0, "ZM_EVENT_CM_BLOCK_TIMER:bAutoReconnect!=0");
+                        zfScanMgrScanStop(dev, ZM_SCAN_MGR_SCAN_INTERNAL);
+                        zfScanMgrScanStart(dev, ZM_SCAN_MGR_SCAN_INTERNAL);
+                    }
+                    //zmw_leave_critical_section(dev);
+                }
+                break;
+
+            case ZM_EVENT_TIMEOUT_ADDBA:
+                {
+                    if (!wd->addbaComplete && (wd->addbaCount < 5))
+                    {
+                        zfAggSendAddbaRequest(dev, wd->sta.bssid, 0, 0);
+                        wd->addbaCount++;
+                        zfTimerSchedule(dev, ZM_EVENT_TIMEOUT_ADDBA, 100);
+                    }
+                    else
+                    {
+                        zfTimerCancel(dev, ZM_EVENT_TIMEOUT_ADDBA);
+                    }
+                }
+                break;
+
+            #ifdef ZM_ENABLE_PERFORMANCE_EVALUATION
+            case ZM_EVENT_TIMEOUT_PERFORMANCE:
+                {
+                    zfiPerformanceRefresh(dev);
+                }
+                break;
+            #endif
+            case ZM_EVENT_SKIP_COUNTERMEASURE:
+				//enable the Countermeasure
+				{
+					zm_debug_msg0("Countermeasure : Enable MIC Check ");
+					wd->TKIP_Group_KeyChanging = 0x0;
+				}
+				break;
+
+            default:
+                break;
+        }
+    }
+}
+
+void zfBssInfoCreate(zdev_t* dev)
+{
+    u8_t   i;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    wd->sta.bssList.bssCount = 0;
+    wd->sta.bssList.head = NULL;
+    wd->sta.bssList.tail = NULL;
+    wd->sta.bssInfoArrayHead = 0;
+    wd->sta.bssInfoArrayTail = 0;
+    wd->sta.bssInfoFreeCount = ZM_MAX_BSS;
+
+    for( i=0; i< ZM_MAX_BSS; i++ )
+    {
+        //wd->sta.bssInfoArray[i] = &(wd->sta.bssInfoPool[i]);
+        wd->sta.bssInfoArray[i] = zfwMemAllocate(dev, sizeof(struct zsBssInfo));
+
+    }
+
+    zmw_leave_critical_section(dev);
+}
+
+void zfBssInfoDestroy(zdev_t* dev)
+{
+    u8_t   i;
+    zmw_get_wlan_dev(dev);
+
+    zfBssInfoRefresh(dev, 1);
+
+    for( i=0; i< ZM_MAX_BSS; i++ )
+    {
+        if (wd->sta.bssInfoArray[i] != NULL)
+        {
+            zfwMemFree(dev, wd->sta.bssInfoArray[i], sizeof(struct zsBssInfo));
+        }
+        else
+        {
+            zm_assert(0);
+        }
+    }
+    return;
+}
+
+struct zsBssInfo* zfBssInfoAllocate(zdev_t* dev)
+{
+    struct zsBssInfo* pBssInfo;
+
+    zmw_get_wlan_dev(dev);
+
+    if (wd->sta.bssInfoFreeCount == 0)
+        return NULL;
+
+    pBssInfo = wd->sta.bssInfoArray[wd->sta.bssInfoArrayHead];
+    wd->sta.bssInfoArray[wd->sta.bssInfoArrayHead] = NULL;
+    wd->sta.bssInfoArrayHead = (wd->sta.bssInfoArrayHead + 1) & (ZM_MAX_BSS - 1);
+    wd->sta.bssInfoFreeCount--;
+
+    zfZeroMemory((u8_t*)pBssInfo, sizeof(struct zsBssInfo));
+
+    return pBssInfo;
+}
+
+void zfBssInfoFree(zdev_t* dev, struct zsBssInfo* pBssInfo)
+{
+    zmw_get_wlan_dev(dev);
+
+    zm_assert(wd->sta.bssInfoArray[wd->sta.bssInfoArrayTail] == NULL);
+
+    pBssInfo->signalStrength = pBssInfo->signalQuality = 0;
+    pBssInfo->sortValue = 0;
+
+    wd->sta.bssInfoArray[wd->sta.bssInfoArrayTail] = pBssInfo;
+    wd->sta.bssInfoArrayTail = (wd->sta.bssInfoArrayTail + 1) & (ZM_MAX_BSS - 1);
+    wd->sta.bssInfoFreeCount++;
+}
+
+void zfBssInfoReorderList(zdev_t* dev)
+{
+    struct zsBssInfo* pBssInfo = NULL;
+    struct zsBssInfo* pInsBssInfo = NULL;
+    struct zsBssInfo* pNextBssInfo = NULL;
+    struct zsBssInfo* pPreBssInfo = NULL;
+    u8_t i = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if (wd->sta.bssList.bssCount > 1)
+    {
+        pInsBssInfo = wd->sta.bssList.head;
+        wd->sta.bssList.tail = pInsBssInfo;
+        pBssInfo = pInsBssInfo->next;
+        pInsBssInfo->next = NULL;
+        while (pBssInfo != NULL)
+        {
+            i = 0;
+            while (1)
+            {
+//                if (pBssInfo->signalStrength >= pInsBssInfo->signalStrength)
+                if( pBssInfo->sortValue >= pInsBssInfo->sortValue)
+                {
+                    if (i==0)
+                    {
+                        //Insert BssInfo to head
+                        wd->sta.bssList.head = pBssInfo;
+                        pNextBssInfo = pBssInfo->next;
+                        pBssInfo->next = pInsBssInfo;
+                        break;
+                    }
+                    else
+                    {
+                        //Insert BssInfo to neither head nor tail
+                        pPreBssInfo->next = pBssInfo;
+                        pNextBssInfo = pBssInfo->next;
+                        pBssInfo->next = pInsBssInfo;
+                        break;
+                    }
+                }
+                else
+                {
+                    if (pInsBssInfo->next != NULL)
+                    {
+                        //Signal strength smaller than current BssInfo, check next
+                        pPreBssInfo = pInsBssInfo;
+                        pInsBssInfo = pInsBssInfo->next;
+                    }
+                    else
+                    {
+                        //Insert BssInfo to tail
+                        pInsBssInfo->next = pBssInfo;
+                        pNextBssInfo = pBssInfo->next;
+                        wd->sta.bssList.tail = pBssInfo;
+                        pBssInfo->next = NULL;
+                        break;
+                    }
+                }
+                i++;
+            }
+            pBssInfo = pNextBssInfo;
+            pInsBssInfo = wd->sta.bssList.head;
+        }
+    } //if (wd->sta.bssList.bssCount > 1)
+
+    zmw_leave_critical_section(dev);
+}
+
+void zfBssInfoInsertToList(zdev_t* dev, struct zsBssInfo* pBssInfo)
+{
+    zmw_get_wlan_dev(dev);
+
+    zm_assert(pBssInfo);
+
+    //zm_debug_msg2("pBssInfo = ", pBssInfo);
+
+    if ( wd->sta.bssList.bssCount == 0 )
+    {
+        wd->sta.bssList.head = pBssInfo;
+        wd->sta.bssList.tail = pBssInfo;
+    }
+    else
+    {
+        wd->sta.bssList.tail->next = pBssInfo;
+        wd->sta.bssList.tail = pBssInfo;
+    }
+
+    pBssInfo->next = NULL;
+    wd->sta.bssList.bssCount++;
+
+    //zm_debug_msg2("bss count = ", wd->sta.bssList.bssCount);
+}
+
+void zfBssInfoRemoveFromList(zdev_t* dev, struct zsBssInfo* pBssInfo)
+{
+    struct zsBssInfo* pNowBssInfo;
+    struct zsBssInfo* pPreBssInfo = NULL;
+    u8_t   i;
+
+    zmw_get_wlan_dev(dev);
+
+    zm_assert(pBssInfo);
+    zm_assert(wd->sta.bssList.bssCount);
+
+    //zm_debug_msg2("pBssInfo = ", pBssInfo);
+
+    pNowBssInfo = wd->sta.bssList.head;
+
+    for( i=0; i<wd->sta.bssList.bssCount; i++ )
+    {
+        if ( pNowBssInfo == pBssInfo )
+        {
+            if ( i == 0 )
+            {   /* remove head */
+                wd->sta.bssList.head = pBssInfo->next;
+            }
+            else
+            {
+                pPreBssInfo->next = pBssInfo->next;
+            }
+
+            if ( i == (wd->sta.bssList.bssCount - 1) )
+            {   /* remove tail */
+                wd->sta.bssList.tail = pPreBssInfo;
+            }
+
+            break;
+        }
+
+        pPreBssInfo = pNowBssInfo;
+        pNowBssInfo = pNowBssInfo->next;
+    }
+
+    zm_assert(i != wd->sta.bssList.bssCount);
+    wd->sta.bssList.bssCount--;
+
+    //zm_debug_msg2("bss count = ", wd->sta.bssList.bssCount);
+}
+
+void zfBssInfoRefresh(zdev_t* dev, u16_t mode)
+{
+    struct zsBssInfo*   pBssInfo;
+    struct zsBssInfo*   pNextBssInfo;
+    u8_t   i, bssCount;
+
+    zmw_get_wlan_dev(dev);
+
+    pBssInfo = wd->sta.bssList.head;
+    bssCount = wd->sta.bssList.bssCount;
+
+    for( i=0; i<bssCount; i++ )
+    {
+        if (mode == 1)
+        {
+            pNextBssInfo = pBssInfo->next;
+            zfBssInfoRemoveFromList(dev, pBssInfo);
+            zfBssInfoFree(dev, pBssInfo);
+            pBssInfo = pNextBssInfo;
+        }
+        else
+        {
+            if ( pBssInfo->flag & ZM_BSS_INFO_VALID_BIT )
+            {   /* this one must be kept */
+                pBssInfo->flag &= ~ZM_BSS_INFO_VALID_BIT;
+                pBssInfo = pBssInfo->next;
+            }
+            else
+            {
+                #define ZM_BSS_CACHE_TIME_IN_MS   20000
+                if ((wd->tick - pBssInfo->tick) > (ZM_BSS_CACHE_TIME_IN_MS/ZM_MS_PER_TICK))
+                {
+                    pNextBssInfo = pBssInfo->next;
+                    zfBssInfoRemoveFromList(dev, pBssInfo);
+                    zfBssInfoFree(dev, pBssInfo);
+                    pBssInfo = pNextBssInfo;
+                }
+                else
+                {
+                    pBssInfo = pBssInfo->next;
+                }
+            }
+        }
+    } //for( i=0; i<bssCount; i++ )
+    return;
+}
+
+void zfDumpSSID(u8_t length, u8_t *value)
+{
+    u8_t buf[50];
+    u8_t tmpLength = length;
+
+    if ( tmpLength > 49 )
+    {
+        tmpLength = 49;
+    }
+
+    zfMemoryCopy(buf, value, tmpLength);
+    buf[tmpLength] = '\0';
+    //printk("SSID: %s\n", buf);
+    //zm_debug_msg_s("ssid = ", value);
+}
+
+void zfCoreReinit(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.flagKeyChanging = 0;
+    wd->sta.flagFreqChanging = 0;
+}
+
+void zfGenerateRandomBSSID(zdev_t* dev, u8_t *MACAddr, u8_t *BSSID)
+{
+    //ULONGLONG   time;
+    u32_t time;
+
+    zmw_get_wlan_dev(dev);
+
+    time = wd->tick;
+
+    //
+    // Initialize the random BSSID to be the same as MAC address.
+    //
+
+    // RtlCopyMemory(BSSID, MACAddr, sizeof(DOT11_MAC_ADDRESS));
+    zfMemoryCopy(BSSID, MACAddr, 6);
+
+    //
+    // Get the system time in 10 millisecond.
+    //
+
+    // NdisGetCurrentSystemTime((PLARGE_INTEGER)&time);
+    // time /= 100000;
+
+    //
+    // Randomize the first 4 bytes of BSSID.
+    //
+
+    BSSID[0] ^= (u8_t)(time & 0xff);
+    BSSID[0] &= ~0x01;              // Turn off multicast bit
+    BSSID[0] |= 0x02;               // Turn on local bit
+
+    time >>= 8;
+    BSSID[1] ^= (u8_t)(time & 0xff);
+
+    time >>= 8;
+    BSSID[2] ^= (u8_t)(time & 0xff);
+
+    time >>= 8;
+    BSSID[3] ^= (u8_t)(time & 0xff);
+}
+
+u8_t zfiWlanGetDestAddrFromBuf(zdev_t *dev, zbuf_t *buf, u16_t *macAddr)
+{
+#ifdef ZM_ENABLE_NATIVE_WIFI
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+    {
+        /* DA */
+        macAddr[0] = zmw_tx_buf_readh(dev, buf, 16);
+        macAddr[1] = zmw_tx_buf_readh(dev, buf, 18);
+        macAddr[2] = zmw_tx_buf_readh(dev, buf, 20);
+    }
+    else if ( wd->wlanMode == ZM_MODE_IBSS )
+    {
+        /* DA */
+        macAddr[0] = zmw_tx_buf_readh(dev, buf, 4);
+        macAddr[1] = zmw_tx_buf_readh(dev, buf, 6);
+        macAddr[2] = zmw_tx_buf_readh(dev, buf, 8);
+    }
+    else if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        /* DA */
+        macAddr[0] = zmw_tx_buf_readh(dev, buf, 4);
+        macAddr[1] = zmw_tx_buf_readh(dev, buf, 6);
+        macAddr[2] = zmw_tx_buf_readh(dev, buf, 8);
+    }
+    else
+    {
+        return 1;
+    }
+#else
+    /* DA */
+    macAddr[0] = zmw_tx_buf_readh(dev, buf, 0);
+    macAddr[1] = zmw_tx_buf_readh(dev, buf, 2);
+    macAddr[2] = zmw_tx_buf_readh(dev, buf, 4);
+#endif
+
+    return 0;
+}
+
+/* Leave an empty line below to remove warning message on some compiler */
+
+u16_t zfFindCleanFrequency(zdev_t* dev, u32_t adhocMode)
+{
+    u8_t   i, j;
+    u16_t  returnChannel;
+    u16_t  count_24G = 0, min24GIndex = 0;
+    u16_t  count_5G = 0,  min5GIndex = 0;
+    u16_t  CombinationBssNumberIn24G[15] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
+    u16_t  BssNumberIn24G[17]  = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
+    u16_t  Array_24G[15]       = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
+    u16_t  BssNumberIn5G[31]   = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
+    u16_t  Array_5G[31]        = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
+    struct zsBssInfo* pBssInfo;
+
+    zmw_get_wlan_dev(dev);
+
+    if ((pBssInfo = wd->sta.bssList.head) == NULL)
+    {
+        if( adhocMode == ZM_ADHOCBAND_B || adhocMode == ZM_ADHOCBAND_G ||
+            adhocMode == ZM_ADHOCBAND_BG || adhocMode == ZM_ADHOCBAND_ABG )
+        {
+            returnChannel = zfChGetFirst2GhzChannel(dev);
+        }
+        else
+        {
+            returnChannel = zfChGetFirst5GhzChannel(dev);
+        }
+
+        return returnChannel;
+    }
+
+    /* #1 Get Allowed Channel following Country Code ! */
+    zmw_declare_for_critical_section();
+    zmw_enter_critical_section(dev);
+    for (i = 0; i < wd->regulationTable.allowChannelCnt; i++)
+    {
+        if (wd->regulationTable.allowChannel[i].channel < 3000)
+        { // 2.4GHz
+            Array_24G[count_24G] = wd->regulationTable.allowChannel[i].channel;
+            count_24G++;
+        }
+        else
+        { // 5GHz
+            count_5G++;
+            Array_5G[i] = wd->regulationTable.allowChannel[i].channel;
+        }
+    }
+    zmw_leave_critical_section(dev);
+
+    while( pBssInfo != NULL )
+    {
+        /* #2_1 Count BSS number in some specificed frequency in 2.4GHz band ! */
+        if( adhocMode == ZM_ADHOCBAND_B || adhocMode == ZM_ADHOCBAND_G ||
+            adhocMode == ZM_ADHOCBAND_BG || adhocMode == ZM_ADHOCBAND_ABG )
+        {
+            for( i=0; i<=(count_24G+3); i++ )
+            {
+                if( pBssInfo->frequency == Array_24G[i] )
+                { // Array_24G[0] correspond to BssNumberIn24G[2]
+                    BssNumberIn24G[pBssInfo->channel+1]++;
+                }
+            }
+        }
+
+        /* #2_2 Count BSS number in some specificed frequency in 5GHz band ! */
+        if( adhocMode == ZM_ADHOCBAND_A || adhocMode == ZM_ADHOCBAND_ABG )
+        {
+            for( i=0; i<count_5G; i++ )
+            { // 5GHz channel is not equal to array index
+                if( pBssInfo->frequency == Array_5G[i] )
+                { // Array_5G[0] correspond to BssNumberIn5G[0]
+                    BssNumberIn5G[i]++;
+                }
+            }
+        }
+
+        pBssInfo = pBssInfo->next;
+    }
+
+#if 0
+    for(i=0; i<=(count_24G+3); i++)
+    {
+        printk("2.4GHz Before combin, %d BSS network : %d", i, BssNumberIn24G[i]);
+    }
+
+    for(i=0; i<count_5G; i++)
+    {
+        printk("5GHz Before combin, %d BSS network : %d", i, BssNumberIn5G[i]);
+    }
+#endif
+
+    if( adhocMode == ZM_ADHOCBAND_B || adhocMode == ZM_ADHOCBAND_G ||
+        adhocMode == ZM_ADHOCBAND_BG || adhocMode == ZM_ADHOCBAND_ABG )
+    {
+        /* #3_1 Count BSS number that influence the specificed frequency in 2.4GHz ! */
+        for( j=0; j<count_24G; j++ )
+        {
+            CombinationBssNumberIn24G[j] = BssNumberIn24G[j]   + BssNumberIn24G[j+1] +
+                                           BssNumberIn24G[j+2] + BssNumberIn24G[j+3] +
+                                           BssNumberIn24G[j+4];
+            //printk("After combine, the number of BSS network channel %d is %d",
+            //                                   j , CombinationBssNumberIn24G[j]);
+        }
+
+        /* #4_1 Find the less utilized frequency in 2.4GHz band ! */
+        min24GIndex = zfFindMinimumUtilizationChannelIndex(dev, CombinationBssNumberIn24G, count_24G);
+    }
+
+    /* #4_2 Find the less utilized frequency in 5GHz band ! */
+    if( adhocMode == ZM_ADHOCBAND_A || adhocMode == ZM_ADHOCBAND_ABG )
+    {
+        min5GIndex = zfFindMinimumUtilizationChannelIndex(dev, BssNumberIn5G, count_5G);
+    }
+
+    if( adhocMode == ZM_ADHOCBAND_B || adhocMode == ZM_ADHOCBAND_G || adhocMode == ZM_ADHOCBAND_BG )
+    {
+        return Array_24G[min24GIndex];
+    }
+    else if( adhocMode == ZM_ADHOCBAND_A )
+    {
+        return Array_5G[min5GIndex];
+    }
+    else if( adhocMode == ZM_ADHOCBAND_ABG )
+    {
+        if ( CombinationBssNumberIn24G[min24GIndex] <= BssNumberIn5G[min5GIndex] )
+            return Array_24G[min24GIndex];
+        else
+            return Array_5G[min5GIndex];
+    }
+    else
+        return 2412;
+}
+
+u16_t zfFindMinimumUtilizationChannelIndex(zdev_t* dev, u16_t* array, u16_t count)
+{
+    u8_t   i;
+    u16_t  tempMinIndex, tempMinValue;
+
+    zmw_get_wlan_dev(dev);
+
+    i = 1;
+    tempMinIndex = 0;
+    tempMinValue = array[tempMinIndex];
+    while( i< count )
+    {
+        if( array[i] < tempMinValue )
+        {
+            tempMinValue = array[i];
+            tempMinIndex = i;
+        }
+        i++;
+    }
+
+    return tempMinIndex;
+}
+
+u8_t zfCompareWithBssid(zdev_t* dev, u16_t* bssid)
+{
+    zmw_get_wlan_dev(dev);
+
+    if ( zfMemoryIsEqual((u8_t*)bssid, (u8_t*)wd->sta.bssid, 6) )
+    {
+        return 1;
+    }
+    else
+    {
+        return 0;
+    }
+}
diff --git a/drivers/staging/otus/80211core/cfunc.h b/drivers/staging/otus/80211core/cfunc.h
new file mode 100644
index 00000000000..fc7548c39d1
--- /dev/null
+++ b/drivers/staging/otus/80211core/cfunc.h
@@ -0,0 +1,449 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : func_extr.c                                           */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains function prototype.                        */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+
+#ifndef _CFUNC_H
+#define _CFUNC_H
+
+#include "queue.h"
+
+/* amsdu.c */
+void zfDeAmsdu(zdev_t* dev, zbuf_t* buf, u16_t vap, u8_t encryMode);
+
+/* cscanmgr.c */
+void zfScanMgrInit(zdev_t* dev);
+u8_t zfScanMgrScanStart(zdev_t* dev, u8_t scanType);
+void zfScanMgrScanStop(zdev_t* dev, u8_t scanType);
+void zfScanMgrScanAck(zdev_t* dev);
+
+/* cpsmgr.c */
+void zfPowerSavingMgrInit(zdev_t* dev);
+void zfPowerSavingMgrSetMode(zdev_t* dev, u8_t mode);
+void zfPowerSavingMgrMain(zdev_t* dev);
+void zfPowerSavingMgrWakeup(zdev_t* dev);
+u8_t zfPowerSavingMgrIsSleeping(zdev_t *dev);
+void zfPowerSavingMgrProcessBeacon(zdev_t* dev, zbuf_t* buf);
+void zfPowerSavingMgrAtimWinExpired(zdev_t* dev);
+void zfPowerSavingMgrConnectNotify(zdev_t *dev);
+void zfPowerSavingMgrPreTBTTInterrupt(zdev_t *dev);
+
+/* ccmd.c */
+u16_t zfWlanEnable(zdev_t* dev);
+
+/* cfunc.c */
+u8_t zfQueryOppositeRate(zdev_t* dev, u8_t dst_mac[6], u8_t frameType);
+void zfCopyToIntTxBuffer(zdev_t* dev, zbuf_t* buf, u8_t* src,
+                         u16_t offset, u16_t length);
+void zfCopyToRxBuffer(zdev_t* dev, zbuf_t* buf, u8_t* src,
+                      u16_t offset, u16_t length);
+void zfCopyFromIntTxBuffer(zdev_t* dev, zbuf_t* buf, u8_t* dst,
+                           u16_t offset, u16_t length);
+void zfCopyFromRxBuffer(zdev_t* dev, zbuf_t* buf, u8_t* dst,
+                        u16_t offset, u16_t length);
+void zfMemoryCopy(u8_t* dst, u8_t* src, u16_t length);
+void zfMemoryMove(u8_t* dst, u8_t* src, u16_t length);
+void zfZeroMemory(u8_t* va, u16_t length);
+u8_t zfMemoryIsEqual(u8_t* m1, u8_t* m2, u16_t length);
+u8_t zfRxBufferEqualToStr(zdev_t* dev, zbuf_t* buf, const u8_t* str,
+                          u16_t offset, u16_t length);
+void zfTxBufferCopy(zdev_t*dev, zbuf_t* dst, zbuf_t* src,
+                    u16_t dstOffset, u16_t srcOffset, u16_t length);
+void zfRxBufferCopy(zdev_t*dev, zbuf_t* dst, zbuf_t* src,
+                    u16_t dstOffset, u16_t srcOffset, u16_t length);
+
+void zfCollectHWTally(zdev_t*dev, u32_t* rsp, u8_t id);
+void zfTimerInit(zdev_t* dev);
+u16_t zfTimerSchedule(zdev_t* dev, u16_t event, u32_t tick);
+u16_t zfTimerCancel(zdev_t* dev, u16_t event);
+void zfTimerClear(zdev_t* dev);
+u16_t zfTimerCheckAndHandle(zdev_t* dev);
+void zfProcessEvent(zdev_t* dev, u16_t* eventArray, u8_t eventCount);
+
+void zfBssInfoCreate(zdev_t* dev);
+void zfBssInfoDestroy(zdev_t* dev);
+
+struct zsBssInfo* zfBssInfoAllocate(zdev_t* dev);
+void zfBssInfoFree(zdev_t* dev, struct zsBssInfo* pBssInfo);
+void zfBssInfoReorderList(zdev_t* dev);
+void zfBssInfoInsertToList(zdev_t* dev, struct zsBssInfo* pBssInfo);
+void zfBssInfoRemoveFromList(zdev_t* dev, struct zsBssInfo* pBssInfo);
+void zfBssInfoRefresh(zdev_t* dev, u16_t mode);
+void zfCoreSetFrequencyComplete(zdev_t* dev);
+void zfCoreSetFrequency(zdev_t* dev, u16_t frequency);
+void zfCoreSetFrequencyV2(zdev_t* dev, u16_t frequency,
+        zfpFreqChangeCompleteCb cb);
+void zfCoreSetFrequencyEx(zdev_t* dev, u16_t frequency, u8_t bw40,
+        u8_t extOffset, zfpFreqChangeCompleteCb cb);
+void zfCoreSetFrequencyExV2(zdev_t* dev, u16_t frequency, u8_t bw40,
+        u8_t extOffset, zfpFreqChangeCompleteCb cb, u8_t forceSetFreq);
+void zfReSetCurrentFrequency(zdev_t* dev);
+u32_t zfCoreSetKey(zdev_t* dev, u8_t user, u8_t keyId, u8_t type,
+        u16_t* mac, u32_t* key);
+void zfCoreSetKeyComplete(zdev_t* dev);
+void zfCoreReinit(zdev_t* dev);
+void zfCoreMacAddressNotify(zdev_t* dev, u8_t *addr);
+void zfCoreSetIsoName(zdev_t* dev, u8_t* isoName);
+void zfGenerateRandomBSSID(zdev_t* dev, u8_t *MACAddr, u8_t *BSSID);
+void zfCoreHalInitComplete(zdev_t* dev);
+
+u16_t zfFindCleanFrequency(zdev_t* dev, u32_t adhocMode);
+u16_t zfFindMinimumUtilizationChannelIndex(zdev_t* dev, u16_t* array, u16_t count);
+u8_t zfCompareWithBssid(zdev_t* dev, u16_t* bssid);
+
+/* chb.c */
+void zfDumpBssList(zdev_t* dev);
+
+
+u16_t zfIssueCmd(zdev_t* dev, u32_t* cmd, u16_t cmdLen, u16_t src, u8_t* buf);
+
+
+/* cic.c */
+void zfUpdateBssid(zdev_t* dev, u8_t* bssid);
+void zfResetSupportRate(zdev_t* dev, u8_t type);
+void zfUpdateSupportRate(zdev_t* dev, u8_t* rateArray);
+u8_t zfIsGOnlyMode(zdev_t* dev, u16_t  frequency, u8_t* rateArray);
+void zfGatherBMode(zdev_t* dev, u8_t* rateArray, u8_t* extrateArray);
+u8_t zfPSDeviceSleep(zdev_t* dev);
+u16_t zfGetRandomNumber(zdev_t* dev, u16_t initValue);
+void zfCoreEvent(zdev_t* dev, u16_t event, u8_t* rsp);
+void zfBeaconCfgInterrupt(zdev_t* dev, u8_t* rsp);
+void zfEndOfAtimWindowInterrupt(zdev_t* dev);
+
+/* cinit.c */
+u16_t zfTxGenWlanHeader(zdev_t* dev, zbuf_t* buf, u16_t* header, u16_t seq,
+                        u8_t flag, u16_t plusLen, u16_t minusLen, u16_t port,
+                        u16_t* da, u16_t* sa, u8_t up, u16_t *micLen,
+                        u16_t* snap, u16_t snapLen, struct aggControl *aggControl);
+u16_t zfTxGenMmHeader(zdev_t* dev, u8_t frameType, u16_t* dst,
+        u16_t* header, u16_t len, zbuf_t* buf, u16_t vap, u8_t encrypt);
+void zfInitMacApMode(zdev_t* dev);
+u16_t zfChGetNextChannel(zdev_t* dev, u16_t frequency, u8_t* pbPassive);
+u16_t zfChGetFirstChannel(zdev_t* dev, u8_t* pbPassive);
+u16_t zfChGetFirst2GhzChannel(zdev_t* dev);
+u16_t zfChGetFirst5GhzChannel(zdev_t* dev);
+u16_t zfChGetLastChannel(zdev_t* dev, u8_t* pbPassive);
+u16_t zfChGetLast5GhzChannel(zdev_t* dev);
+u16_t zfChNumToFreq(zdev_t* dev, u8_t ch, u8_t freqBand);
+u8_t zfChFreqToNum(u16_t freq, u8_t* bIs5GBand);
+
+/* cmm.c */
+void zfProcessManagement(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* AddInfo); //CWYang(m)
+void zfSendMmFrame(zdev_t* dev, u8_t frameType, u16_t* dst,
+                   u32_t p1, u32_t p2, u32_t p3);
+u16_t zfFindElement(zdev_t* dev, zbuf_t* buf, u8_t eid);
+u16_t zfFindWifiElement(zdev_t* dev, zbuf_t* buf, u8_t type, u8_t subtype);
+u16_t zfFindSuperGElement(zdev_t* dev, zbuf_t* buf, u8_t type);
+u16_t zfFindXRElement(zdev_t* dev, zbuf_t* buf, u8_t type);
+u16_t zfRemoveElement(zdev_t* dev, u8_t* buf, u16_t size, u8_t eid);
+u16_t zfUpdateElement(zdev_t* dev, u8_t* buf, u16_t size, u8_t* updateeid);
+void zfProcessProbeReq(zdev_t* dev, zbuf_t* buf, u16_t* src);
+void zfProcessProbeRsp(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* AddInfo);
+u16_t zfSendProbeReq(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t bWithSSID);
+u16_t zfMmAddIeSupportRate(zdev_t* dev, zbuf_t* buf,
+                           u16_t offset, u8_t eid, u8_t rateSet);
+u16_t zfMmAddIeDs(zdev_t* dev, zbuf_t* buf, u16_t offset);
+u16_t zfMmAddIeErp(zdev_t* dev, zbuf_t* buf, u16_t offset);
+void zfUpdateDefaultQosParameter(zdev_t* dev, u8_t mode);
+u16_t zfMmAddIeWpa(zdev_t* dev, zbuf_t* buf, u16_t offset, u16_t apId);
+u16_t zfMmAddHTCapability(zdev_t* dev, zbuf_t* buf, u16_t offset); //CWYang(+)
+u16_t zfMmAddPreNHTCapability(zdev_t* dev, zbuf_t* buf, u16_t offset);
+u16_t zfMmAddExtendedHTCapability(zdev_t* dev, zbuf_t* buf, u16_t offset); //CWYang(+)
+u16_t zfFindATHExtCap(zdev_t* dev, zbuf_t* buf, u8_t type, u8_t subtype);
+u16_t zfFindBrdcmMrvlRlnkExtCap(zdev_t* dev, zbuf_t* buf);
+u16_t zfFindMarvelExtCap(zdev_t* dev, zbuf_t* buf);
+u16_t zfFindBroadcomExtCap(zdev_t* dev, zbuf_t* buf);
+u16_t zfFindRlnkExtCap(zdev_t* dev, zbuf_t* buf);
+
+/* cmmap.c */
+void zfMmApTimeTick(zdev_t* dev);
+void zfApAgingSta(zdev_t* dev);
+u16_t zfApAddSta(zdev_t* dev, u16_t* addr, u16_t state, u16_t apId, u8_t type,
+                 u8_t qosType, u8_t qosInfo);
+void zfApProtctionMonitor(zdev_t* dev);
+void zfApProcessBeacon(zdev_t* dev, zbuf_t* buf);
+void zfApProcessAuth(zdev_t* dev, zbuf_t* buf, u16_t* src, u16_t apId);
+void zfApProcessAsocReq(zdev_t* dev, zbuf_t* buf, u16_t* src, u16_t apId);
+void zfApProcessAsocRsp(zdev_t* dev, zbuf_t* buf);
+void zfApProcessDeauth(zdev_t* dev, zbuf_t* buf, u16_t* src, u16_t apId);
+void zfApProcessDisasoc(zdev_t* dev, zbuf_t* buf, u16_t* src, u16_t apId);
+void zfApProcessProbeRsp(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* AddInfo);
+void zfApStoreAsocReqIe(zdev_t* dev, zbuf_t* buf, u16_t aid);
+u16_t zfApAddIeSsid(zdev_t* dev, zbuf_t* buf, u16_t offset, u16_t vap);
+void zfApSendBeacon(zdev_t* dev);
+u16_t zfApGetSTAInfo(zdev_t* dev, u16_t* addr, u16_t* state, u8_t* vap);
+u16_t zfIntrabssForward(zdev_t* dev, zbuf_t* buf, u8_t srcVap);
+u16_t zfApBufferPsFrame(zdev_t* dev, zbuf_t* buf, u16_t port);
+void zfApInitStaTbl(zdev_t* dev);
+void zfApGetStaTxRateAndQosType(zdev_t* dev, u16_t* addr, u32_t* phyCtrl,
+                                u8_t* qosType, u16_t* rcProbingFlag);
+void zfApGetStaQosType(zdev_t* dev, u16_t* addr, u8_t* qosType);
+void zfApSetStaTxRate(zdev_t* dev, u16_t* addr, u32_t phyCtrl);
+struct zsMicVar* zfApGetRxMicKey(zdev_t* dev, zbuf_t* buf);
+struct zsMicVar* zfApGetTxMicKey(zdev_t* dev, zbuf_t* buf, u8_t* qosType);
+u16_t zfApAddIeWmePara(zdev_t* dev, zbuf_t* buf, u16_t offset, u16_t vap);
+u16_t zfApUpdatePsBit(zdev_t* dev, zbuf_t* buf, u8_t* vap, u8_t* uapsdTrig);
+void zfApProcessPsPoll(zdev_t* dev, zbuf_t* buf);
+u16_t zfApFindSta(zdev_t* dev, u16_t* addr);
+void zfApGetStaEncryType(zdev_t* dev, u16_t* addr, u8_t* encryType);
+void zfApGetStaWpaIv(zdev_t* dev, u16_t* addr, u16_t* iv16, u32_t* iv32);
+void zfApSetStaWpaIv(zdev_t* dev, u16_t* addr, u16_t iv16, u32_t iv32);
+void zfApClearStaKey(zdev_t* dev, u16_t* addr);
+#ifdef ZM_ENABLE_CENC
+void zfApGetStaCencIvAndKeyIdx(zdev_t* dev, u16_t* addr, u32_t *iv,
+        u8_t *keyIdx);
+void zfApSetStaCencIv(zdev_t* dev, u16_t* addr, u32_t *iv);
+#endif //ZM_ENABLE_CENC
+void zfApSetProtectionMode(zdev_t* dev, u16_t mode);
+void zfApFlushBufferedPsFrame(zdev_t* dev);
+void zfApSendFailure(zdev_t* dev, u8_t* addr);
+u8_t zfApRemoveFromPsQueue(zdev_t* dev, u16_t id, u16_t* src);
+void zfApProcessAction(zdev_t* dev, zbuf_t* buf);
+/* cmmsta.c */
+void zfMmStaTimeTick(zdev_t* dev);
+void zfReWriteBeaconStartAddress(zdev_t* dev);  // Mxzeng
+void zfStaProcessBeacon(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* AddInfo); //CWYang(m)
+void zfStaProcessAuth(zdev_t* dev, zbuf_t* buf, u16_t* src, u16_t apId);
+void zfStaProcessAsocReq(zdev_t* dev, zbuf_t* buf, u16_t* src, u16_t apId);
+void zfStaProcessAsocRsp(zdev_t* dev, zbuf_t* buf);
+void zfStaProcessDeauth(zdev_t* dev, zbuf_t* buf);
+void zfStaProcessDisasoc(zdev_t* dev, zbuf_t* buf);
+void zfStaProcessProbeRsp(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* AddInfo);
+void zfStaProcessAtim(zdev_t* dev, zbuf_t* buf);
+void zfStaStoreAsocRspIe(zdev_t* dev, zbuf_t* buf);
+void zfStaChannelManagement(zdev_t* dev, u8_t scan);
+void zfIbssConnectNetwork(zdev_t* dev);
+void zfInfraConnectNetwork(zdev_t* dev);
+u8_t zfCheckAuthentication(zdev_t* dev, struct zsBssInfo* pBssInfo);
+u8_t zfChangeAdapterState(zdev_t* dev, u8_t newState);
+u16_t zfStaAddIeSsid(zdev_t* dev, zbuf_t* buf, u16_t offset);
+u16_t zfStaAddIeWpaRsn(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t frameType);
+u16_t zfStaAddIeIbss(zdev_t* dev, zbuf_t* buf, u16_t offset);
+void zfStaStartConnect(zdev_t* dev, u8_t bIsSharedKey);
+u8_t zfStaIsConnected(zdev_t* dev);
+u8_t zfStaIsConnecting(zdev_t* dev);
+u8_t zfStaIsDisconnect(zdev_t* dev);
+void zfStaSendBeacon(zdev_t* dev);
+void zfSendNullData(zdev_t* dev, u8_t type);
+void zfSendPSPoll(zdev_t* dev);
+void zfSendBA(zdev_t* dev, u16_t start_seq, u8_t *bitmap);
+void zdRateInfoCountTx(zdev_t* dev, u16_t* macAddr);
+struct zsMicVar* zfStaGetRxMicKey(zdev_t* dev, zbuf_t* buf);
+struct zsMicVar* zfStaGetTxMicKey(zdev_t* dev, zbuf_t* buf);
+u16_t zfStaRxValidateFrame(zdev_t* dev, zbuf_t* buf);
+void zfStaMicFailureHandling(zdev_t* dev, zbuf_t* buf);
+u8_t zfStaBlockWlanScan(zdev_t* dev);
+void zfStaIbssPSCheckState(zdev_t* dev, zbuf_t* buf);
+u8_t zfStaIbssPSQueueData(zdev_t* dev, zbuf_t* buf);
+void zfStaIbssPSSend(zdev_t* dev);
+void zfStaResetStatus(zdev_t* dev, u8_t bInit);
+u16_t zfStaAddIeWmeInfo(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t qosInfo);
+void zfInitPartnerNotifyEvent(zdev_t* dev, zbuf_t* buf, struct zsPartnerNotifyEvent *event);
+void zfStaInitOppositeInfo(zdev_t* dev);
+void zfStaIbssMonitoring(zdev_t* dev, u8_t reset);
+struct zsBssInfo* zfStaFindBssInfo(zdev_t* dev, zbuf_t* buf, struct zsWlanProbeRspFrameHeader *pProbeRspHeader);
+u8_t zfStaInitBssInfo(zdev_t* dev, zbuf_t* buf,
+        struct zsWlanProbeRspFrameHeader *pProbeRspHeader,
+        struct zsBssInfo* pBssInfo, struct zsAdditionInfo* AddInfo, u8_t type);
+s8_t zfStaFindFreeOpposite(zdev_t* dev, u16_t *sa, int *pFoundIdx);
+s8_t zfStaFindOppositeByMACAddr(zdev_t* dev, u16_t *sa, u8_t *pFoundIdx);
+void zfStaRefreshBlockList(zdev_t* dev, u16_t flushFlag);
+void zfStaConnectFail(zdev_t* dev, u16_t reason, u16_t* bssid, u8_t weight);
+void zfStaGetTxRate(zdev_t* dev, u16_t* macAddr, u32_t* phyCtrl,
+        u16_t* rcProbingFlag);
+u16_t zfStaProcessAction(zdev_t* dev, zbuf_t* buf);
+struct zsTkipSeed* zfStaGetRxSeed(zdev_t* dev, zbuf_t* buf);
+#ifdef ZM_ENABLE_CENC
+/* CENC */
+u16_t zfStaAddIeCenc(zdev_t* dev, zbuf_t* buf, u16_t offset);
+#endif //ZM_ENABLE_CENC
+void zfStaEnableSWEncryption(zdev_t *dev, u8_t value);
+void zfStaDisableSWEncryption(zdev_t *dev);
+u16_t zfComputeBssInfoWeightValue(zdev_t *dev, u8_t isBMode, u8_t isHT, u8_t isHT40, u8_t signalStrength);
+u16_t zfStaAddIbssAdditionalIE(zdev_t* dev, zbuf_t* buf, u16_t offset);
+
+/* ctkip.c */
+void zfTkipInit(u8_t* key, u8_t* ta, struct zsTkipSeed* pSeed, u8_t* initIv);
+void zfMicSetKey(u8_t* key, struct zsMicVar* pMic);
+void zfMicAppendByte(u8_t b, struct zsMicVar* pMic);
+void zfMicClear(struct zsMicVar* pMic);
+void zfMicAppendTxBuf(zdev_t* dev, zbuf_t* buf, u8_t* da, u8_t* sa,
+                      u16_t removeLen, u8_t* mic);
+u8_t zfMicRxVerify(zdev_t* dev, zbuf_t* buf);
+void zfMicGetMic(u8_t* dst, struct zsMicVar* pMic);
+void zfCalTxMic(zdev_t *dev, zbuf_t *buf, u8_t *snap, u16_t snapLen, u16_t offset, u16_t *da, u16_t *sa, u8_t up, u8_t *mic);
+void zfTKIPEncrypt(zdev_t *dev, zbuf_t *buf, u8_t *snap, u16_t snapLen, u16_t offset, u8_t keyLen, u8_t* key, u32_t* icv);
+u16_t zfTKIPDecrypt(zdev_t *dev, zbuf_t *buf, u16_t offset, u8_t keyLen, u8_t* key);
+void zfTkipGetseeds(u16_t iv16, u8_t *RC4Key, struct zsTkipSeed *Seed);
+u8_t zfTkipPhase1KeyMix(u32_t iv32, struct zsTkipSeed* pSeed);
+u8_t zfTkipPhase2KeyMix(u16_t iv16, struct zsTkipSeed* pSeed);
+void zfWEPEncrypt(zdev_t *dev, zbuf_t *buf, u8_t *snap, u16_t snapLen, u16_t offset, u8_t keyLen, u8_t* WepKey, u8_t *iv);
+u16_t zfWEPDecrypt(zdev_t *dev, zbuf_t *buf, u16_t offset, u8_t keyLen, u8_t* WepKey, u8_t *iv);
+
+/* ctxrx.c */
+u16_t zfSend80211Frame(zdev_t* dev, zbuf_t* buf);
+void zfIsrPciTxComp(zdev_t* dev);
+void zfTxPciDmaStart(zdev_t* dev);
+u16_t zfTxPortControl(zdev_t* dev, zbuf_t* buf, u16_t port);
+u16_t zfTxSendEth(zdev_t* dev, zbuf_t* buf, u16_t port,
+                  u16_t bufType, u16_t flag);
+u16_t zfTxGenWlanTail(zdev_t* dev, zbuf_t* buf, u16_t* snap, u16_t snaplen,
+                      u16_t* mic);
+u16_t zfTxGenWlanSnap(zdev_t* dev, zbuf_t* buf, u16_t* snap, u16_t* snaplen);
+void zfTxGetIpTosAndFrag(zdev_t* dev, zbuf_t* buf, u8_t* up, u16_t* fragOff);
+u16_t zfPutVtxq(zdev_t* dev, zbuf_t* buf);
+void zfPushVtxq(zdev_t* dev);
+u8_t zfIsVtxqEmpty(zdev_t* dev);
+u16_t zfGetSeqCtrl(zdev_t* dev, zbuf_t* buf, u16_t offset);
+u8_t zfGetFragNo(zdev_t* dev, zbuf_t* buf);
+void zfShowRxEAPOL(zdev_t* dev, zbuf_t* buf, u16_t offset);
+void zfShowTxEAPOL(zdev_t* dev, zbuf_t* buf, u16_t offset);
+void zfCoreRecv(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* addInfo);
+u16_t zfPutVmmq(zdev_t* dev, zbuf_t* buf);
+void zfFlushVtxq(zdev_t* dev);
+void zfAgingDefragList(zdev_t* dev, u16_t flushFlag);
+
+void zfLed100msCtrl(zdev_t* dev);
+void zf80211FrameSend(zdev_t* dev, zbuf_t* buf, u16_t* header, u16_t snapLen,
+                           u16_t* da, u16_t* sa, u8_t up, u16_t headerLen, u16_t* snap,
+                           u16_t* tail, u16_t tailLen, u16_t offset, u16_t bufType,
+                           u8_t ac, u8_t keyIdx);
+void zfCheckIsRIFSFrame(zdev_t* dev, zbuf_t* buf, u16_t frameSubType);
+
+/* queue.c */
+struct zsQueue* zfQueueCreate(zdev_t* dev, u16_t size);
+void zfQueueDestroy(zdev_t* dev, struct zsQueue* q);
+u16_t zfQueuePutNcs(zdev_t* dev, struct zsQueue* q, zbuf_t* buf, u32_t tick);
+u16_t zfQueuePut(zdev_t* dev, struct zsQueue* q, zbuf_t* buf, u32_t tick);
+zbuf_t* zfQueueGet(zdev_t* dev, struct zsQueue* q);
+zbuf_t* zfQueueGetWithMac(zdev_t* dev, struct zsQueue* q, u8_t* addr, u8_t* mb);
+void zfQueueFlush(zdev_t* dev, struct zsQueue* q);
+void zfQueueAge(zdev_t* dev, struct zsQueue* q, u32_t tick, u32_t msAge);
+void zfQueueGenerateUapsdTim(zdev_t* dev, struct zsQueue* q,
+        u8_t* uniBitMap, u16_t* highestByte);
+
+/* hpmain.c */
+u16_t zfHpInit(zdev_t* dev, u32_t frequency);
+u16_t zfHpRelease(zdev_t* dev);
+void zfHpSetFrequencyEx(zdev_t* dev, u32_t frequency, u8_t bw40,
+        u8_t extOffset, u8_t initRF);
+u16_t zfHpStartRecv(zdev_t* dev);
+u16_t zfHpStopRecv(zdev_t* dev);
+u16_t zfHpResetKeyCache(zdev_t* dev);
+u16_t zfHpSetApStaMode(zdev_t* dev, u8_t mode);
+u16_t zfHpSetBssid(zdev_t* dev, u8_t* bssid);
+u16_t zfHpSetSnifferMode(zdev_t* dev, u16_t on);
+u8_t zfHpUpdateQosParameter(zdev_t* dev, u16_t* cwminTbl, u16_t* cwmaxTbl,
+        u16_t* aifsTbl, u16_t* txopTbl);
+void zfHpSetAtimWindow(zdev_t* dev, u16_t atimWin);
+void zfHpEnableBeacon(zdev_t* dev, u16_t mode, u16_t bcnInterval, u16_t dtim, u8_t enableAtim);
+void zfHpDisableBeacon(zdev_t* dev);
+void zfHpSetBasicRateSet(zdev_t* dev, u16_t bRateBasic, u16_t gRateBasic);
+void zfHpSetRTSCTSRate(zdev_t* dev, u32_t rate);
+void zfHpSetMacAddress(zdev_t* dev, u16_t* macAddr, u16_t macAddrId);
+u32_t zfHpGetMacAddress(zdev_t* dev);
+u32_t zfHpGetTransmitPower(zdev_t* dev);
+void zfHpSetMulticastList(zdev_t* dev, u8_t size, u8_t* pList, u8_t bAllMulticast);
+
+u16_t zfHpRemoveKey(zdev_t* dev, u16_t user);
+u32_t zfHpSetKey(zdev_t* dev, u8_t user, u8_t keyId, u8_t type,
+        u16_t* mac, u32_t* key);
+//u32_t zfHpSetStaPairwiseKey(zdev_t* dev, u16_t* apMacAddr, u8_t type,
+//        u32_t* key, u32_t* micKey);
+//u32_t zfHpSetStaGroupKey(zdev_t* dev, u16_t* apMacAddr, u8_t type,
+//        u32_t* key, u32_t* micKey);
+u32_t zfHpSetApPairwiseKey(zdev_t* dev, u16_t* staMacAddr, u8_t type,
+        u32_t* key, u32_t* micKey, u16_t staAid);
+u32_t zfHpSetApGroupKey(zdev_t* dev, u16_t* apMacAddr, u8_t type,
+        u32_t* key, u32_t* micKey, u16_t vapId);
+u32_t zfHpSetDefaultKey(zdev_t* dev, u8_t keyId, u8_t type, u32_t* key, u32_t* micKey);
+u32_t zfHpSetPerUserKey(zdev_t* dev, u8_t user, u8_t keyId, u8_t* mac, u8_t type, u32_t* key, u32_t* micKey);
+
+void zfHpSendBeacon(zdev_t* dev, zbuf_t* buf, u16_t len);
+u16_t zfHpGetPayloadLen(zdev_t* dev,
+                        zbuf_t* buf,
+                        u16_t len,
+                        u16_t plcpHdrLen,
+                        u32_t *rxMT,
+                        u32_t *rxMCS,
+                        u32_t *rxBW,
+                        u32_t *rxSG
+                        );
+u32_t zfHpGetFreeTxdCount(zdev_t* dev);
+u32_t zfHpGetMaxTxdCount(zdev_t* dev);
+u16_t zfHpSend(zdev_t* dev, u16_t* header, u16_t headerLen,
+        u16_t* snap, u16_t snapLen, u16_t* tail, u16_t tailLen, zbuf_t* buf,
+        u16_t offset, u16_t bufType, u8_t ac, u8_t keyIdx);
+void zfHpGetRegulationTablefromRegionCode(zdev_t* dev, u16_t regionCode);
+void zfHpGetRegulationTablefromCountry(zdev_t* dev, u16_t CountryCode);
+u8_t zfHpGetRegulationTablefromISO(zdev_t* dev, u8_t *countryInfo, u8_t length);
+const char* zfHpGetisoNamefromregionCode(zdev_t* dev, u16_t regionCode);
+u16_t zfHpGetRegionCodeFromIsoName(zdev_t* dev, u8_t *countryIsoName);
+u8_t zfHpGetRegulatoryDomain(zdev_t* dev);
+void zfHpLedCtrl(zdev_t* dev, u16_t ledId, u8_t mode);
+u16_t zfHpResetTxRx(zdev_t* dev);
+u16_t zfHpDeleteAllowChannel(zdev_t* dev, u16_t freq);
+u16_t zfHpAddAllowChannel(zdev_t* dev, u16_t freq);
+u32_t zfHpCwmUpdate(zdev_t* dev);
+u32_t zfHpAniUpdate(zdev_t* dev);
+u32_t zfHpAniUpdateRssi(zdev_t* dev, u8_t rssi);
+void zfHpAniAttach(zdev_t* dev);
+void zfHpAniArPoll(zdev_t* dev, u32_t listenTime, u32_t phyCnt1, u32_t phyCnt2);
+void zfHpHeartBeat(zdev_t* dev);
+void zfHpPowerSaveSetState(zdev_t* dev, u8_t psState);
+void zfHpPowerSaveSetMode(zdev_t* dev, u8_t staMode, u8_t psMode, u16_t bcnInterval);
+u16_t zfHpIsDfsChannel(zdev_t* dev, u16_t freq);
+u16_t zfHpIsDfsChannelNCS(zdev_t* dev, u16_t freq);
+u16_t zfHpFindFirstNonDfsChannel(zdev_t* dev, u16_t aBand);
+u16_t zfHpIsAllowedChannel(zdev_t* dev, u16_t freq);
+void zfHpDisableDfsChannel(zdev_t* dev, u8_t disableFlag);
+void zfHpSetTTSIFSTime(zdev_t* dev, u8_t sifs_time);
+
+void zfHpQueryMonHalRxInfo(zdev_t* dev, u8_t *monHalRxInfo);
+
+void zfDumpSSID(u8_t length, u8_t *value);
+void zfHpSetAggPktNum(zdev_t* dev, u32_t num);
+void zfHpSetMPDUDensity(zdev_t* dev, u8_t density);
+void zfHpSetSlotTime(zdev_t* dev, u8_t type);
+void zfHpSetSlotTimeRegister(zdev_t* dev, u8_t type);
+void zfHpSetRifs(zdev_t* dev, u8_t ht_enable, u8_t ht2040, u8_t g_mode);
+void zfHpBeginSiteSurvey(zdev_t* dev, u8_t status);
+void zfHpFinishSiteSurvey(zdev_t* dev, u8_t status);
+u16_t zfHpEnableHwRetry(zdev_t* dev);
+u16_t zfHpDisableHwRetry(zdev_t* dev);
+void zfHpSWDecrypt(zdev_t* dev, u8_t enable);
+void zfHpSWEncrypt(zdev_t* dev, u8_t enable);
+u32_t zfHpCapability(zdev_t* dev);
+void zfHpSetRollCallTable(zdev_t* dev);
+u8_t zfHpregulatoryDomain(zdev_t* dev);
+u16_t zfStaAddIePowerCap(zdev_t* dev, zbuf_t* buf, u16_t offset);
+u8_t zfHpGetMaxTxPower(zdev_t* dev);
+u8_t zfHpGetMinTxPower(zdev_t* dev);
+u16_t zfStaAddIeSupportCh(zdev_t* dev, zbuf_t* buf, u16_t offset);
+void zfHpEnableRifs(zdev_t* dev, u8_t mode24g, u8_t modeHt, u8_t modeHt2040);
+void zfHpDisableRifs(zdev_t* dev);
+u16_t zfHpUsbReset(zdev_t* dev);
+
+
+#endif /* #ifndef _CFUNC_H */
diff --git a/drivers/staging/otus/80211core/chb.c b/drivers/staging/otus/80211core/chb.c
new file mode 100644
index 00000000000..7fac1501125
--- /dev/null
+++ b/drivers/staging/otus/80211core/chb.c
@@ -0,0 +1,200 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : hb.c                                                  */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains house keeping and timer functions.         */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+#include "cprecomp.h"
+
+/* Called by wrapper every 10 msec */
+void zfiHeartBeat(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->tick++;
+
+#if 0
+    /* => every 1.28 seconds */
+    if (wd->cwm.cw_enable && ((wd->tick & 0x7f) == 0x3f))
+    {
+        zfHpCwmUpdate(dev);
+    }
+#endif
+    /* => every 2.56 seconds */
+    if ((wd->tick & 0xff) == 0)
+    {
+        zfAgingDefragList(dev, 1);
+    }
+
+    /* Watch Dog */
+    //zfWatchDog();
+
+    /* LED Control (per 100ms) */
+    if ((wd->tick % 10) == 9)
+    {
+        zfLed100msCtrl(dev);
+#ifdef ZM_ENABLE_BA_RATECTRL
+        if (!wd->modeMDKEnable)
+        {
+            zfiDbgReadTally(dev);
+        }
+#endif
+    }
+
+#ifdef ZM_ENABLE_REWRITE_BEACON_START_ADDRESS
+    if ( wd->wlanMode == ZM_MODE_IBSS )
+    {
+        if ( zfStaIsConnected(dev) )
+        {
+            zfReWriteBeaconStartAddress(dev);
+        }
+    }
+#endif
+
+    if ( wd->wlanMode == ZM_MODE_IBSS )
+    {
+        if ( zfStaIsConnected(dev) )
+        {
+            wd->tickIbssReceiveBeacon++;  // add 10ms
+
+            if ( (wd->sta.ibssSiteSurveyStatus == 2) &&
+                 (wd->tickIbssReceiveBeacon == 300) &&
+                 (wd->sta.ibssReceiveBeaconCount < 3) )
+            {
+                zm_debug_msg0("It is happen!!! No error message");
+                zfReSetCurrentFrequency(dev);
+            }
+        }
+    }
+
+    if(wd->sta.ReceivedPacketRateCounter <= 0)
+    {
+        wd->sta.ReceivedPktRatePerSecond = wd->sta.TotalNumberOfReceivePackets;
+	//zm_debug_msg1("Receive Packet Per Second  = ", wd->sta.ReceivedPktRatePerSecond);
+	    if (wd->sta.TotalNumberOfReceivePackets != 0)
+	    {
+	        wd->sta.avgSizeOfReceivePackets = wd->sta.TotalNumberOfReceiveBytes/wd->sta.TotalNumberOfReceivePackets;
+	    }
+	    else
+	    {
+	        wd->sta.avgSizeOfReceivePackets = 640;
+	    }
+        wd->sta.TotalNumberOfReceivePackets = 0;
+        wd->sta.TotalNumberOfReceiveBytes = 0;
+        wd->sta.ReceivedPacketRateCounter = 100; /*for another 1s*/
+    }
+    else
+    {
+        wd->sta.ReceivedPacketRateCounter--;
+    }
+
+	/* => every 1.28 seconds */
+	if((wd->tick & 0x7f) == 0x3f)
+	{
+		if( wd->sta.NonNAPcount > 0)
+		{
+			wd->sta.RTSInAGGMode = TRUE;
+			wd->sta.NonNAPcount = 0;
+		}
+		else
+		{
+			wd->sta.RTSInAGGMode = FALSE;
+		}
+	}
+
+
+
+    /* Maintain management time tick */
+    zfMmApTimeTick(dev);
+    zfMmStaTimeTick(dev);
+
+    //zfPhyCrTuning(dev);
+
+    //zfTxPowerControl(dev);
+    zfHpHeartBeat(dev);
+
+}
+
+
+void zfDumpBssList(zdev_t* dev)
+{
+    struct zsBssInfo* pBssInfo;
+    u8_t   str[33];
+    u8_t   i, j;
+    u32_t  addr1, addr2;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zm_debug_msg0("***** Bss scan result *****");
+    zmw_enter_critical_section(dev);
+
+    pBssInfo = wd->sta.bssList.head;
+
+    for( i=0; i<wd->sta.bssList.bssCount; i++ )
+    {
+        if ( i )
+        {
+            zm_debug_msg0("---------------------------");
+        }
+
+        zm_debug_msg1("BSS #", i);
+        for(j=0; j<pBssInfo->ssid[1]; j++)
+        {
+            str[j] = pBssInfo->ssid[2+j];
+        }
+        str[pBssInfo->ssid[1]] = 0;
+        zm_debug_msg0("SSID = ");
+        zm_debug_msg0(str);
+
+        addr1 = (pBssInfo->bssid[0] << 16) + (pBssInfo->bssid[1] << 8 )
+                + pBssInfo->bssid[2];
+        addr2 = (pBssInfo->bssid[3] << 16) + (pBssInfo->bssid[4] << 8 )
+                + pBssInfo->bssid[5];
+        zm_debug_msg2("Bssid = ", addr1);
+        zm_debug_msg2("        ", addr2);
+        zm_debug_msg1("frequency = ", pBssInfo->frequency);
+        zm_debug_msg1("security type = ", pBssInfo->securityType);
+        zm_debug_msg1("WME = ", pBssInfo->wmeSupport);
+        zm_debug_msg1("beacon interval = ", pBssInfo->beaconInterval[0]
+                      + (pBssInfo->beaconInterval[1] << 8));
+        zm_debug_msg1("capability = ", pBssInfo->capability[0]
+                      + (pBssInfo->capability[1] << 8));
+        if ( pBssInfo->supportedRates[1] > 0 )
+        {
+            for( j=0; j<pBssInfo->supportedRates[1]; j++ )
+            {
+                zm_debug_msg2("supported rates = ", pBssInfo->supportedRates[2+j]);
+            }
+        }
+
+        for( j=0; j<pBssInfo->extSupportedRates[1]; j++ )
+        {
+            zm_debug_msg2("ext supported rates = ", pBssInfo->extSupportedRates[2+j]);
+        }
+
+        pBssInfo = pBssInfo->next;
+    }
+    zmw_leave_critical_section(dev);
+
+    zm_debug_msg0("***************************");
+}
+
diff --git a/drivers/staging/otus/80211core/cic.c b/drivers/staging/otus/80211core/cic.c
new file mode 100644
index 00000000000..c84f079e3d8
--- /dev/null
+++ b/drivers/staging/otus/80211core/cic.c
@@ -0,0 +1,496 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include "cprecomp.h"
+#include "ratectrl.h"
+
+
+void zfUpdateBssid(zdev_t* dev, u8_t* bssid)
+{
+
+    zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+    //zmw_enter_critical_section(dev);
+    wd->sta.bssid[0] = bssid[0] + (((u16_t) bssid[1]) << 8);
+    wd->sta.bssid[1] = bssid[2] + (((u16_t) bssid[3]) << 8);
+    wd->sta.bssid[2] = bssid[4] + (((u16_t) bssid[5]) << 8);
+    //zmw_leave_critical_section(dev);
+
+    zfHpSetBssid(dev, bssid);
+
+}
+
+/************************************************************************************/
+/*                                                                                  */
+/*    FUNCTION DESCRIPTION                  zfResetSupportRate                      */
+/*      Reset support rate to default value.                                        */
+/*                                                                                  */
+/*    INPUTS                                                                        */
+/*      dev : device pointer                                                        */
+/*      type: ZM_DEFAULT_SUPPORT_RATE_ZERO       => reset to zero                   */
+/*            ZM_DEFAULT_SUPPORT_RATE_DISCONNECT => reset to disconnect status      */
+/*            ZM_DEFAULT_SUPPORT_RATE_IBSS_B     => reset to IBSS creator(b mode)   */
+/*            ZM_DEFAULT_SUPPORT_RATE_IBSS_AG    => reset to IBSS creator(a/g mode) */
+/*                                                                                  */
+/************************************************************************************/
+void zfResetSupportRate(zdev_t* dev, u8_t type)
+{
+    zmw_get_wlan_dev(dev);
+
+    switch(type)
+    {
+    case ZM_DEFAULT_SUPPORT_RATE_ZERO:
+        wd->bRate = 0;
+        wd->bRateBasic = 0;
+        wd->gRate = 0;
+        wd->gRateBasic = 0;
+        break;
+    case ZM_DEFAULT_SUPPORT_RATE_DISCONNECT:
+        wd->bRate = 0xf;
+        wd->bRateBasic = 0xf;
+        wd->gRate = 0xff;
+        wd->gRateBasic = 0x15;
+        break;
+    case ZM_DEFAULT_SUPPORT_RATE_IBSS_B:
+        wd->bRate = 0xf;
+        wd->bRateBasic = 0xf;
+        wd->gRate = 0;
+        wd->gRateBasic = 0;
+        break;
+    case ZM_DEFAULT_SUPPORT_RATE_IBSS_AG:
+        wd->bRate = 0xf;
+        wd->bRateBasic = 0xf;
+        wd->gRate = 0xff;
+        wd->gRateBasic = 0;
+        break;
+    }
+}
+
+void zfUpdateSupportRate(zdev_t* dev, u8_t* rateArray)
+{
+    u8_t bRate=0, bRateBasic=0, gRate=0, gRateBasic=0;
+    u8_t length = rateArray[1];
+    u8_t i, j;
+
+    zmw_get_wlan_dev(dev);
+
+    for(i=2; i<length+2; i++)
+    {
+        for(j=0; j<4; j++)
+        {
+            if ( (rateArray[i] & 0x7f) == zg11bRateTbl[j] )
+            {
+                bRate |= (1 << j);
+                if ( rateArray[i] & 0x80 )
+                {
+                    bRateBasic |= (1 << j);
+                }
+            }
+        }
+
+        if ( j == 4 )
+        {
+            for(j=0; j<8; j++)
+            {
+                if ( (rateArray[i] & 0x7f) == zg11gRateTbl[j] )
+                {
+                    gRate |= (1 << j);
+                    if ( rateArray[i] & 0x80 )
+                    {
+                        gRateBasic |= (1 << j);
+                    }
+                }
+            }
+        }
+    }
+
+
+    wd->bRate |= bRate;
+    wd->bRateBasic |= bRateBasic;
+    wd->gRate |= gRate;
+    wd->gRateBasic |= gRateBasic;
+}
+
+u8_t zfIsGOnlyMode(zdev_t* dev, u16_t  frequency, u8_t* rateArray)
+{
+    u8_t length = rateArray[1];
+    u8_t i, j;
+
+    if (frequency < 3000) {
+        for (i = 2; i < length+2; i++) {
+            for (j = 0; j < 8; j++) {
+                if ( ((rateArray[i] & 0x7f) == zg11gRateTbl[j])
+                     && (rateArray[i] & 0x80) ) {
+                    return 1;
+                }
+            }
+        }
+    }
+
+    return 0;
+}
+
+void zfGatherBMode(zdev_t* dev, u8_t* rateArray, u8_t* extrateArray)
+{
+    u8_t gatherBMode[ZM_MAX_SUPP_RATES_IE_SIZE + 2];
+    u8_t i, j, k = 0;
+    u8_t length;
+
+    gatherBMode[0] = ZM_WLAN_EID_SUPPORT_RATE;
+    gatherBMode[1] = 0;
+
+    length = rateArray[1];
+    for (i = 2; i < length+2; i++) {
+        for (j = 0; j < 4; j++) {
+            if ( (rateArray[i] & 0x7f) == zg11bRateTbl[j] ) {
+                gatherBMode[2+k] = rateArray[i];
+
+                gatherBMode[1]++;
+                k++;
+            }
+        }
+    }
+
+    length = extrateArray[1];
+    for (i = 2; i < length+2; i++) {
+        for (j = 0; j < 4; j++) {
+            if ( (extrateArray[i] & 0x7f) == zg11bRateTbl[j] ) {
+                gatherBMode[2+k] = extrateArray[i];
+
+                gatherBMode[1]++;
+                k++;
+            }
+        }
+    }
+
+    extrateArray[0] = extrateArray[1] = 0;
+    zfMemoryCopy(rateArray, gatherBMode, gatherBMode[1]+2);
+}
+
+u16_t zfGetRandomNumber(zdev_t* dev, u16_t initValue)
+{
+#if 0
+    /* Compiler/Linker error on Linux */
+    if ( initValue )
+    {
+        srand(initValue);
+    }
+
+    return ((u16_t)rand());
+#endif
+    return 0;
+}
+
+u8_t zfPSDeviceSleep(zdev_t* dev)
+{
+    //zmw_get_wlan_dev(dev);
+
+    /* enter PS mode */
+
+    return 0;
+}
+
+u8_t zcOfdmPhyCrtlToRate[] =
+{
+    /* 0x8=48M, 0x9=24M, 0xa=12M, 0xb=6M, 0xc=54M, 0xd=36M, 0xe=18M, 0xf=9M */
+            10,       8,       6,      4,      11,       9,       7,      5
+};
+
+u8_t zfPhyCtrlToRate(u32_t phyCtrl)
+{
+    u32_t mt, mcs, sg;
+    u8_t rate = 0;
+
+    mt = phyCtrl & 0x3;
+    mcs = (phyCtrl>>18) & 0x3f;
+    sg = (phyCtrl>>31) & 0x1;
+
+    if ((mt == 0) && (mcs <=3))
+    {
+        rate = (u8_t)mcs;
+    }
+    else if ((mt == 1) && (mcs >= 0x8) && (mcs <= 0xf))
+    {
+        rate = zcOfdmPhyCrtlToRate[mcs-8];
+    }
+    else if ((mt == 2) && (mcs <= 15))
+    {
+        rate = (u8_t)mcs + 12;
+        if(sg) {
+            if (mcs != 7)
+            {
+                rate = (u8_t)mcs + 12 + 2;
+            }
+            else //MCS7-SG
+            {
+                rate = (u8_t)30;
+            }
+        }
+    }
+
+    return rate;
+}
+
+
+void zfCoreEvent(zdev_t* dev, u16_t event, u8_t* rsp)
+{
+    u16_t i;
+    zbuf_t* psBuf;
+    u8_t moreData;
+    u8_t vap = 0;
+    u8_t peerIdx;
+    s8_t res;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+
+    if (event == 0) //Beacon Event
+    {
+        if ( wd->wlanMode == ZM_MODE_AP )
+        {
+            zfApSendBeacon(dev);
+
+            if (wd->CurrentDtimCount == 0)
+            {
+                /* TODO : Send queued broadcast frames at BC/MC event */
+                do
+                {
+                    psBuf = NULL;
+                    moreData = 0;
+                    zmw_enter_critical_section(dev);
+                    if (wd->ap.bcmcTail[vap] != wd->ap.bcmcHead[vap])
+                    {
+                        //zm_msg0_mm(ZM_LV_0, "Send BCMC frames");
+                        psBuf = wd->ap.bcmcArray[vap][wd->ap.bcmcHead[vap]];
+                        wd->ap.bcmcHead[vap] = (wd->ap.bcmcHead[vap] + 1)
+                                & (ZM_BCMC_ARRAY_SIZE - 1);
+                        if (wd->ap.bcmcTail[vap] != wd->ap.bcmcHead[vap])
+                        {
+                            moreData = 0x20;
+                        }
+                    }
+                    zmw_leave_critical_section(dev);
+                    if (psBuf != NULL)
+                    {
+                        /* TODO : config moreData bit */
+                        zfTxSendEth(dev, psBuf, 0, ZM_EXTERNAL_ALLOC_BUF,
+                                moreData);
+                    }
+                } while(psBuf != NULL);
+
+            }
+        }
+        else
+        {
+            /* STA mode */
+            if ( wd->sta.powerSaveMode > ZM_STA_PS_NONE )
+            {
+                /* send queued packets */
+                for(i=0; i<wd->sta.staPSDataCount; i++)
+                {
+                    zfTxSendEth(dev, wd->sta.staPSDataQueue[i], 0,
+                                ZM_EXTERNAL_ALLOC_BUF, 0);
+                }
+
+                wd->sta.staPSDataCount = 0;
+            }
+
+            if ( wd->wlanMode == ZM_MODE_IBSS )
+            {
+                zfStaSendBeacon(dev);
+                wd->sta.ibssAtimTimer = ZM_BIT_15 | wd->sta.atimWindow;
+            }
+
+            zfPowerSavingMgrPreTBTTInterrupt(dev);
+        }
+    } //if (event == 0) //Beacon Event
+    else if (event == 1) //Retry completed event
+    {
+        u32_t retryRate;
+
+        retryRate = (u32_t)(rsp[6]) + (((u32_t)(rsp[7]))<<8)
+                + (((u32_t)(rsp[8]))<<16) + (((u32_t)(rsp[9]))<<24);
+        /* Degrade Tx Rate */
+        if (wd->wlanMode == ZM_MODE_AP)
+        {
+            zmw_enter_critical_section(dev);
+            if ((i=zfApFindSta(dev, (u16_t*)rsp)) != 0xffff)
+            {
+                zfRateCtrlTxFailEvent(dev, &wd->ap.staTable[i].rcCell, 0,(u32_t)zfPhyCtrlToRate(retryRate));
+            }
+            zmw_leave_critical_section(dev);
+        }
+        else
+        {
+            zmw_enter_critical_section(dev);
+            res = zfStaFindOppositeByMACAddr(dev, (u16_t*)rsp, &peerIdx);
+            if ( res == 0 )
+            {
+                zfRateCtrlTxFailEvent(dev, &wd->sta.oppositeInfo[peerIdx].rcCell, 0,(u32_t)zfPhyCtrlToRate(retryRate));
+            }
+            zmw_leave_critical_section(dev);
+        }
+    } //else if (event == 1) //Retry completed event
+    else if (event == 2) //Tx Fail event
+    {
+        u32_t retryRate;
+
+        retryRate = (u32_t)(rsp[6]) + (((u32_t)(rsp[7]))<<8)
+                + (((u32_t)(rsp[8]))<<16) + (((u32_t)(rsp[9]))<<24);
+
+        /* Degrade Tx Rate */
+        if (wd->wlanMode == ZM_MODE_AP)
+        {
+            zmw_enter_critical_section(dev);
+            if ((i=zfApFindSta(dev, (u16_t*)rsp)) != 0xffff)
+            {
+                zfRateCtrlTxFailEvent(dev, &wd->ap.staTable[i].rcCell, 0,(u32_t)zfPhyCtrlToRate(retryRate));
+            }
+            zmw_leave_critical_section(dev);
+
+            zfApSendFailure(dev, rsp);
+        }
+        else
+        {
+            zmw_enter_critical_section(dev);
+            res = zfStaFindOppositeByMACAddr(dev, (u16_t*)rsp, &peerIdx);
+            if ( res == 0 )
+            {
+                zfRateCtrlTxFailEvent(dev, &wd->sta.oppositeInfo[peerIdx].rcCell, 0,(u32_t)zfPhyCtrlToRate(retryRate));
+            }
+            zmw_leave_critical_section(dev);
+        }
+    } //else if (event == 2) //Tx Fail event
+    else if (event == 3) //Tx Comp event
+    {
+        u32_t retryRate;
+
+        retryRate = (u32_t)(rsp[6]) + (((u32_t)(rsp[7]))<<8)
+                + (((u32_t)(rsp[8]))<<16) + (((u32_t)(rsp[9]))<<24);
+
+        /* TODO : Tx completed, used for rate control probing */
+        if (wd->wlanMode == ZM_MODE_AP)
+        {
+            zmw_enter_critical_section(dev);
+            if ((i=zfApFindSta(dev, (u16_t*)rsp)) != 0xffff)
+            {
+                zfRateCtrlTxSuccessEvent(dev, &wd->ap.staTable[i].rcCell, zfPhyCtrlToRate(retryRate));
+            }
+            zmw_leave_critical_section(dev);
+        }
+        else
+        {
+            zmw_enter_critical_section(dev);
+            res = zfStaFindOppositeByMACAddr(dev, (u16_t*)rsp, &peerIdx);
+            if ( res == 0 )
+            {
+                zfRateCtrlTxSuccessEvent(dev, &wd->sta.oppositeInfo[peerIdx].rcCell, zfPhyCtrlToRate(retryRate));
+            }
+            zmw_leave_critical_section(dev);
+        }
+    } //else if (event == 3) //Tx Comp event
+    else if (event == 4) //BA failed count
+    {
+        u32_t fail;
+        u32_t rate;
+        peerIdx = 0;
+
+        fail=((u32_t*)rsp)[0] & 0xFFFF;
+        rate=((u32_t*)rsp)[0] >> 16;
+
+        if (rate > 15) {
+            rate = (rate & 0xF) + 12 + 2;
+        }
+        else {
+            rate = rate + 12;
+        }
+
+        zmw_enter_critical_section(dev);
+        zfRateCtrlTxFailEvent(dev, &wd->sta.oppositeInfo[peerIdx].rcCell, (u8_t)rate, fail);
+        zmw_leave_critical_section(dev);
+    }
+}
+
+void zfBeaconCfgInterrupt(zdev_t* dev, u8_t* rsp)
+{
+    u32_t txBeaconCounter;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if ( wd->wlanMode == ZM_MODE_IBSS )
+    {
+        txBeaconCounter = *((u32_t *)rsp);
+        if ( wd->sta.beaconTxCnt != txBeaconCounter )
+        {
+            wd->sta.txBeaconInd = 1;
+
+            zmw_enter_critical_section(dev);
+            wd->tickIbssSendBeacon = 0;
+            zmw_leave_critical_section(dev);
+        }
+        else
+        {
+            wd->sta.txBeaconInd = 0;
+        }
+
+#ifdef ZM_ENABLE_IBSS_DELAYED_JOIN_INDICATION
+        if ( wd->sta.txBeaconInd && wd->sta.ibssDelayedInd )
+        {
+            if (wd->zfcbIbssPartnerNotify != NULL)
+            {
+                wd->zfcbIbssPartnerNotify(dev, 1, &wd->sta.ibssDelayedIndEvent);
+            }
+
+            wd->sta.ibssDelayedInd = 0;
+        }
+#endif
+
+        wd->sta.beaconTxCnt = txBeaconCounter;
+
+        // Need to check if the time is expired after ATIM window??
+
+        // Check if we have buffered any data for those stations that are sleeping
+        // If it's true, then transmitting ATIM pkt to notify them
+
+#ifdef ZM_ENABLE_IBSS_PS
+        // TODO: Need to check if the station receive our ATIM pkt???
+        zfStaIbssPSSend(dev);
+
+        if ( wd->sta.atimWindow == 0 )
+        {
+            // We won't receive the end of ATIM isr so we fake it
+            zfPowerSavingMgrAtimWinExpired(dev);
+        }
+#endif
+    }
+}
+
+void zfEndOfAtimWindowInterrupt(zdev_t* dev)
+{
+#ifdef ZM_ENABLE_IBSS_PS
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->wlanMode == ZM_MODE_IBSS )
+    {
+        // Transmit any queued pkt for the stations!!
+        zfPowerSavingMgrAtimWinExpired(dev);
+    }
+#endif
+}
diff --git a/drivers/staging/otus/80211core/cinit.c b/drivers/staging/otus/80211core/cinit.c
new file mode 100644
index 00000000000..5f853ce7930
--- /dev/null
+++ b/drivers/staging/otus/80211core/cinit.c
@@ -0,0 +1,1911 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : init.c                                                */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains init functions.                            */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+#include "cprecomp.h"
+#include "../hal/hpreg.h"
+
+extern const u8_t zcUpToAc[8];
+
+u16_t zcIndextoRateBG[16] = {1000, 2000, 5500, 11000, 0, 0, 0, 0, 48000,
+                               24000, 12000, 6000, 54000, 36000, 18000, 9000};
+u32_t zcIndextoRateN20L[16] = {6500, 13000, 19500, 26000, 39000, 52000, 58500,
+                              65000, 13000, 26000, 39000, 52000, 78000, 104000,
+                              117000, 130000};
+u32_t zcIndextoRateN20S[16] = {7200, 14400, 21700, 28900, 43300, 57800, 65000,
+                              72200, 14400, 28900, 43300, 57800, 86700, 115600,
+                              130000, 144400};
+u32_t zcIndextoRateN40L[16] = {13500, 27000, 40500, 54000, 81000, 108000, 121500,
+                              135000, 27000, 54000, 81000, 108000, 162000, 216000,
+                              243000, 270000};
+u32_t zcIndextoRateN40S[16] = {15000, 30000, 45000, 60000, 90000, 120000, 135000,
+                              150000, 30000, 60000, 90000, 120000, 180000, 240000,
+                              270000, 300000};
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfTxGenWlanHeader           */
+/*      Generate WLAN MAC header and LLC header.                        */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer pointer                                            */
+/*      id : Index of TxD                                               */
+/*      port : WLAN port                                                */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      length of removed Ethernet header                               */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen             ZyDAS Technology Corporation    2005.5      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfTxGenWlanHeader(zdev_t* dev, zbuf_t* buf, u16_t* header, u16_t seq,
+                        u8_t flag, u16_t plusLen, u16_t minusLen, u16_t port,
+                        u16_t* da, u16_t* sa, u8_t up, u16_t *micLen,
+                        u16_t* snap, u16_t snapLen, struct aggControl *aggControl)
+{
+
+    u16_t len;
+    u16_t macCtrl;
+    u32_t phyCtrl;
+    u16_t hlen = 16;
+    u16_t icvLen = 0;
+    u16_t wdsPortId;
+    u16_t vap = 0;
+    u16_t mcs = 0;
+    u16_t mt = 0;
+    u8_t  qosType;
+    u8_t  b1, b2;
+    u16_t wdsPort;
+    u8_t  encExemptionActionType;
+    u16_t rateProbingFlag = 0;
+    u8_t  tkipFrameOffset = 0;
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+    u8_t    res, peerIdx;
+    u8_t    userIdx=0;
+    u16_t   *iv16;
+    u32_t   *iv32;
+#endif
+
+    zmw_get_wlan_dev(dev);
+
+   /* Generate WLAN header */
+    /* Frame control */
+    header[4] = 0x0008 | (flag<<8);
+    /* Duration */
+    header[5] = 0x0000;
+
+    if (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)
+    {
+        /* ToDS bit */
+        header[4] |= 0x0100;
+
+        /*Sometimes we wake up to tx/rx but AP still think we are sleeping, so still need to set this bit*/
+        if ( zfPowerSavingMgrIsSleeping(dev) || wd->sta.psMgr.tempWakeUp == 1 )
+        {
+            header[4] |= 0x1000;
+        }
+
+        /* Address 1 = BSSID */
+        header[6] = wd->sta.bssid[0];
+        header[7] = wd->sta.bssid[1];
+        header[8] = wd->sta.bssid[2];
+        /* Address 3 = DA */
+        header[12] = da[0];
+        header[13] = da[1];
+        header[14] = da[2];
+    }
+    else if (wd->wlanMode == ZM_MODE_PSEUDO)
+    {
+        /* Address 1 = DA */
+        header[6] = da[0];
+        header[7] = da[1];
+        header[8] = da[2];
+        /* Address 3 = 00:00:00:00:00:00 */
+        header[12] = 0;
+        header[13] = 0;
+        header[14] = 0;
+
+        /* PSEUDO test : WDS */
+        if (wd->enableWDS)
+        {
+            /* ToDS and FromDS bit */
+            header[4] |= 0x0300;
+
+            /* Address 4 = SA */
+            header[16] = 0;
+            header[17] = 0;
+            header[18] = 0;
+
+            hlen = 19;
+        }
+    }
+    else if (wd->wlanMode == ZM_MODE_IBSS)
+    {
+        /* Address 1 = DA */
+        header[6] = da[0];
+        header[7] = da[1];
+        header[8] = da[2];
+        /* Address 3 = BSSID */
+        header[12] = wd->sta.bssid[0];
+        header[13] = wd->sta.bssid[1];
+        header[14] = wd->sta.bssid[2];
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+        zmw_enter_critical_section(dev);
+        res = zfStaFindOppositeByMACAddr(dev, da, &peerIdx);
+        if(res == 0) // Find opposite in our OppositeInfo Structure !
+        {
+            userIdx = peerIdx;
+        }
+        zmw_leave_critical_section(dev);
+#endif
+    }
+    else if (wd->wlanMode == ZM_MODE_AP)
+    {
+        if (port < 0x20)
+        /* AP mode */
+        {
+            /* FromDS bit */
+            header[4] |= 0x0200;
+
+            /* Address 1 = DA */
+            header[6] = da[0];
+            header[7] = da[1];
+            header[8] = da[2];
+            /* Address 3 = SA */
+            header[12] = sa[0];
+            header[13] = sa[1];
+            header[14] = sa[2];
+
+            if (port < ZM_MAX_AP_SUPPORT)
+            {
+                vap = port;
+                header[14] += (vap<<8);
+            }
+        }
+        else
+        /* WDS port */
+        {
+            /* ToDS and FromDS bit */
+            header[4] |= 0x0300;
+
+            wdsPortId = port - 0x20;
+
+            /* Address 1 = RA */
+            header[6] = wd->ap.wds.macAddr[wdsPortId][0];
+            header[7] = wd->ap.wds.macAddr[wdsPortId][1];
+            header[8] = wd->ap.wds.macAddr[wdsPortId][2];
+            /* Address 3 = DA */
+            header[12] = da[0];
+            header[13] = da[1];
+            header[14] = da[2];
+            /* Address 4 = SA */
+            header[16] = sa[0];
+            header[17] = sa[1];
+            header[18] = sa[2];
+
+            hlen = 19;
+        }
+    } /* else if (wd->wlanMode == ZM_MODE_AP) */
+
+    /* Address 2 = TA */
+    header[9] = wd->macAddr[0];
+    header[10] = wd->macAddr[1];
+#ifdef ZM_VAPMODE_MULTILE_SSID
+    header[11] = wd->macAddr[2]; //Multiple SSID
+#else
+    header[11] = wd->macAddr[2] + (vap<<8); //VAP
+#endif
+
+    if ( (wd->wlanMode == ZM_MODE_IBSS) && (wd->XLinkMode) )
+    {
+        header[9]  = sa[0];
+        header[10] = sa[1];
+        header[11] = sa[2];
+    }
+
+    /* Sequence Control */
+    header[15] = seq;
+
+
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        zfApGetStaTxRateAndQosType(dev, da, &phyCtrl, &qosType, &rateProbingFlag);
+        mt = (u16_t)(phyCtrl & 0x3);
+        mcs = (u16_t)((phyCtrl >> 16) & 0x3f);
+#if 1
+        //zfApGetStaQosType(dev, da, &qosType);
+
+        /* if DA == WME STA */
+        if (qosType == 1)
+        {
+            /* QoS data */
+            header[4] |= 0x0080;
+
+            /* QoS Control */
+            header[hlen] = up;
+            hlen += 1;
+        }
+#endif
+    }
+
+#if 0
+    //AGG Test Code
+    if (header[6] == 0x8000)
+    {
+        /* QoS data */
+        header[4] |= 0x0080;
+
+        /* QoS Control */
+        header[hlen] = 0;
+        hlen += 1;
+    }
+#endif
+
+    if (wd->wlanMode == ZM_MODE_AP) {
+        /* Todo: rate control here for qos field */
+    }
+    else {
+        /* Rate control */
+        zfStaGetTxRate(dev, da, &phyCtrl, &rateProbingFlag);
+        mt = (u16_t)(phyCtrl & 0x3);
+        mcs = (u16_t)((phyCtrl >> 16) & 0x3f);
+    }
+
+    if (wd->txMCS != 0xff)
+    {
+        /* fixed rate */
+	    phyCtrl = ((u32_t)wd->txMCS<<16) + wd->txMT;
+        mcs = wd->txMCS;
+        mt = wd->txMT;
+    }
+
+    if (wd->enableAggregation)
+    {
+        /* force enable aggregation */
+        if (wd->enableAggregation==2 && !(header[6]&0x1))
+        {
+            /* QoS data */
+            header[4] |= 0x0080;
+
+            /* QoS Control */
+            header[hlen] = 0;
+            hlen += 1;
+        }
+        /* if wd->enableAggregation=1 => force disable */
+        /* if wd->enableAggregation=0 => auto */
+    }
+
+#ifdef ZM_ENABLE_AGGREGATION
+    /*
+     * aggregation control
+     */
+
+    /*
+     * QoS data
+     */
+    if (wd->wlanMode == ZM_MODE_AP) {
+        if (aggControl && mt == 2) {
+            if (wd->enableAggregation==0 && !(header[6]&0x1))
+            {
+                header[4] |= 0x0080;
+
+                /*
+                 * QoS Control
+                 */
+                header[hlen] = 0;
+                hlen += 1;
+            }
+        }
+    }
+#endif
+
+    // MSDU Length
+    len = zfwBufGetSize(dev, buf);
+
+    /* Generate control setting */
+    /* Backoff, Non-Burst and hardware duration */
+    macCtrl = 0x208;
+
+    /* ACK */
+    if ((header[6] & 0x1) == 0x1)
+    {
+        /* multicast frame : Set NO-ACK bit */
+        macCtrl |= 0x4;
+    }
+    else
+    {
+        /* unicast frame */
+    #if 0
+        // Enable RTS according to MPDU Lengths ( not MSDU Lengths )
+        if (len >= wd->rtsThreshold)
+        {
+            /* Enable RTS */
+            macCtrl |= 1;
+        }
+    #endif
+    }
+    /* VAP test code */
+    //macCtrl |= 0x4;
+
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        u8_t encryType;
+        u16_t iv16;
+        u32_t iv32;
+
+        /* Check whether this is a multicast frame */
+        if ((header[6] & 0x1) == 0x1)
+        {
+            /* multicast frame */
+            if (wd->ap.encryMode[vap] == ZM_TKIP)
+            {
+                wd->ap.iv16[vap]++;
+
+                if(wd->ap.iv16[vap] == 0)
+                {
+                    wd->ap.iv32[vap]++;
+                }
+
+                b1 = (u8_t) (wd->ap.iv16[vap] >> 8);
+                b2 = (b1 | 0x20) & 0x7f;
+                header[hlen] = ((u16_t)b2 << 8) + b1;
+                b1 = (u8_t) wd->ap.iv16[vap];
+                b2 = 0x20 | (wd->ap.bcKeyIndex[vap] << 6);
+                header[hlen+1] = ((u16_t)b2 << 8) + b1;
+                header[hlen+2] = (u16_t) wd->ap.iv32[vap];
+                header[hlen+3] = (u16_t) (wd->ap.iv32[vap] >> 16);
+
+                //macCtrl |= 0x80;
+                macCtrl |= 0x40;
+                icvLen = 4;
+
+                /* set hardware MIC */
+                if ( (!(seq & 0xf))&&(!(flag & 0x4)) )
+                {
+                    macCtrl |= 0x100;
+                    plusLen += 8;
+                    *micLen = 8;
+                }
+
+                header[4] |= 0x4000;
+                hlen += 4;
+            }
+            else if (wd->ap.encryMode[vap] == ZM_AES)
+            {
+                wd->ap.iv16[vap]++;
+
+                if(wd->ap.iv16[vap] == 0)
+                {
+                    wd->ap.iv32[vap]++;
+                }
+
+                b1 = (u8_t) wd->ap.iv16[vap];
+                b2 = (u8_t) (wd->ap.iv16[vap] >> 8);
+                header[hlen] = ((u16_t)b2 << 8) + b1;
+                header[hlen+1] = 0x2000 | (wd->ap.bcKeyIndex[vap] << 14);
+                header[hlen+2] = (u16_t) (wd->ap.iv32[vap]);
+                header[hlen+3] = (u16_t) (wd->ap.iv32[vap] >> 16);
+
+                macCtrl |= 0xc0;
+                icvLen = 8;  /* MIC */
+
+                header[4] |= 0x4000;
+                hlen += 4;
+            }
+            #ifdef ZM_ENABLE_CENC
+            else if (wd->ap.encryMode[vap] == ZM_CENC)
+            {
+                //u32_t txiv[4];
+
+                wd->ap.txiv[vap][0]++;
+
+                if (wd->ap.txiv[vap][0] == 0)
+                {
+                    wd->ap.txiv[vap][1]++;
+                }
+
+                if (wd->ap.txiv[vap][1] == 0)
+                {
+                    wd->ap.txiv[vap][2]++;
+                }
+
+                if (wd->ap.txiv[vap][2] == 0)
+                {
+                    wd->ap.txiv[vap][3]++;
+                }
+
+                if (wd->ap.txiv[vap][3] == 0)
+                {
+                    wd->ap.txiv[vap][0] = 0;
+                    wd->ap.txiv[vap][1] = 0;
+                    wd->ap.txiv[vap][2] = 0;
+                }
+
+                header[hlen] = (wd->ap.bcKeyIndex[vap] & 0x0001);    /* For Key Id and reserved field */
+                header[hlen+1] = (u16_t)wd->ap.txiv[vap][0];
+                header[hlen+2] = (u16_t)(wd->ap.txiv[vap][0] >> 16);
+                header[hlen+3] = (u16_t)wd->ap.txiv[vap][1];
+                header[hlen+4] = (u16_t)(wd->ap.txiv[vap][1] >> 16);
+                header[hlen+5] = (u16_t)wd->ap.txiv[vap][2];
+                header[hlen+6] = (u16_t)(wd->ap.txiv[vap][2] >> 16);
+                header[hlen+7] = (u16_t)wd->ap.txiv[vap][3];
+                header[hlen+8] = (u16_t)(wd->ap.txiv[vap][3] >> 16);
+
+                macCtrl |= 0x80;
+                icvLen = 16;  /* MIC */
+
+                header[4] |= 0x4000;
+                hlen += 9;
+            }
+            #endif //ZM_ENABLE_CENC
+        }
+        else
+        {
+            /* Get STA's encryption type */
+            zfApGetStaEncryType(dev, da, &encryType);
+
+            if (encryType == ZM_TKIP)
+            {
+                /* Get iv16 and iv32 */
+                zfApGetStaWpaIv(dev, da, &iv16, &iv32);
+
+                iv16++;
+                if (iv16 == 0)
+                {
+                    iv32++;
+                }
+
+                b1 = (u8_t) (iv16 >> 8);
+                b2 = (b1 | 0x20) & 0x7f;
+                header[hlen] = ((u16_t)b2 << 8) + b1;
+                b1 = (u8_t) iv16;
+                b2 = 0x20;
+                header[hlen+1] = ((u16_t)b2 << 8) + b1;
+                header[hlen+2] = (u16_t) iv32;
+                header[hlen+3] = (u16_t) (iv32 >> 16);
+
+                //macCtrl |= 0x80;
+                macCtrl |= 0x40;
+                icvLen = 4;
+
+                /* set hardware MIC */
+                if ( (!(seq & 0xf))&&(!(flag & 0x4)) )
+                {
+                    macCtrl |= 0x100;
+                    plusLen += 8;
+                    *micLen = 8;
+                }
+
+                header[4] |= 0x4000;
+                hlen += 4;
+
+                /* Set iv16 and iv32 */
+                zfApSetStaWpaIv(dev, da, iv16, iv32);
+            }
+            else if (encryType == ZM_AES)
+            {
+                /* Get iv16 and iv32 */
+                zfApGetStaWpaIv(dev, da, &iv16, &iv32);
+
+                iv16++;
+                if (iv16 == 0)
+                {
+                    iv32++;
+                }
+
+                b1 = (u8_t) iv16;
+                b2 = (u8_t) (iv16 >> 8);
+                header[hlen] = ((u16_t)b2 << 8) + b1;
+                header[hlen+1] = 0x2000;
+                header[hlen+2] = (u16_t) (iv32);
+                header[hlen+3] = (u16_t) (iv32 >> 16);
+
+                macCtrl |= 0xc0;
+                icvLen = 8;  /* MIC */
+
+                header[4] |= 0x4000;
+                hlen += 4;
+
+                /* Set iv16 and iv32 */
+                zfApSetStaWpaIv(dev, da, iv16, iv32);
+            }
+            #ifdef ZM_ENABLE_CENC
+            else if (encryType == ZM_CENC)
+            {
+                u32_t txiv[4];
+                u8_t keyIdx;
+
+                /* Get CENC TxIV */
+                zfApGetStaCencIvAndKeyIdx(dev, da, txiv, &keyIdx);
+
+                txiv[0] += 2;
+
+                if (txiv[0] == 0 || txiv[0] == 1)
+                {
+                    txiv[1]++;
+                }
+
+                if (txiv[1] == 0)
+                {
+                    txiv[2]++;
+                }
+
+                if (txiv[2] == 0)
+                {
+                    txiv[3]++;
+                }
+
+                if (txiv[3] == 0)
+                {
+                    txiv[0] = 0;
+                    txiv[1] = 0;
+                    txiv[2] = 0;
+                }
+
+                header[hlen] = (keyIdx & 0x0001);    /* For Key Id and reserved field */
+                header[hlen+1] = (u16_t)txiv[0];
+                header[hlen+2] = (u16_t)(txiv[0] >> 16);
+                header[hlen+3] = (u16_t)txiv[1];
+                header[hlen+4] = (u16_t)(txiv[1] >> 16);
+                header[hlen+5] = (u16_t)txiv[2];
+                header[hlen+6] = (u16_t)(txiv[2] >> 16);
+                header[hlen+7] = (u16_t)txiv[3];
+                header[hlen+8] = (u16_t)(txiv[3] >> 16);
+
+                macCtrl |= 0x80;
+                icvLen = 16;  /* MIC */
+
+                header[4] |= 0x4000;
+                hlen += 9;
+
+                /* Set CENC IV */
+                zfApSetStaCencIv(dev, da, txiv);
+            }
+            #endif //ZM_ENABLE_CENC
+        }
+
+        /* protection mode */
+        if (wd->ap.protectionMode == 1)
+        {
+            /* Enable Self-CTS */
+            macCtrl &= 0xFFFC;
+            macCtrl |= 2;
+        }
+
+        /* Rate Control */
+        if (port < 0x20)
+        {
+            /* AP */
+            /* IV */
+            if ((wd->ap.encryMode[vap] == ZM_WEP64) ||
+                    (wd->ap.encryMode[vap] == ZM_WEP128) ||
+                    (wd->ap.encryMode[vap] == ZM_WEP256))
+            {
+                header[4] |= 0x4000;
+                header[hlen] = 0x0;   //IV
+                header[hlen+1] = wd->ap.bcKeyIndex[vap] << 14; //IV with Keyid--CWYang(m)
+                hlen += 2;
+                icvLen = 4;
+                macCtrl |= 0x40;
+            }
+        }
+        else
+        {
+            /* WDS */
+
+            /* TODO : Fixed rate to 54M */
+            phyCtrl = 0xc0001;   //PHY control L
+
+            /* WDS port checking */
+            if ((wdsPort = (port - 0x20)) >= ZM_MAX_WDS_SUPPORT)
+            {
+                wdsPort = 0;
+            }
+
+            #if 1
+            /* IV */
+            switch (wd->ap.wds.encryMode[wdsPort])
+            {
+            case ZM_WEP64:
+            case ZM_WEP128:
+            case ZM_WEP256:
+                    header[4] |= 0x4000;
+                    header[hlen] = 0x0;   //IV
+                    header[hlen+1] = wd->ap.bcKeyIndex[vap] << 14; //IV with Keyid
+                    hlen += 2;
+                    icvLen = 4;
+                    macCtrl |= 0x40;
+                    break;
+
+            case ZM_TKIP:
+                    wd->sta.iv16++;
+
+                    if ( wd->sta.iv16 == 0 )
+                    {
+                        wd->sta.iv32++;
+                    }
+
+                    b1 = (u8_t) (wd->sta.iv16 >> 8);
+                    b2 = (b1 | 0x20) & 0x7f;
+                    header[hlen] = ((u16_t)b2 << 8) + b1;
+                    b1 = (u8_t) wd->sta.iv16;
+                    b2 = 0x20;
+                    header[hlen+1] = ((u16_t)b2 << 8) + b1;
+                    header[hlen+2] = (u16_t) wd->sta.iv32;
+                    header[hlen+3] = (u16_t) (wd->sta.iv32 >> 16);
+
+                    //macCtrl |= 0x80;
+                    macCtrl |= 0x40;
+                    icvLen = 4;
+
+                    /* set hardware MIC */
+                    if ( (!(seq & 0xf))&&(!(flag & 0x4)) )
+                    {
+                        macCtrl |= 0x100;
+                        plusLen += 8;
+                        *micLen = 8;
+                    }
+
+                    header[4] |= 0x4000;
+                    hlen += 4;
+                    break;
+
+            case ZM_AES:
+                    wd->sta.iv16++;
+                    if ( wd->sta.iv16 == 0 )
+                    {
+                        wd->sta.iv32++;
+                    }
+
+                    b1 = (u8_t) wd->sta.iv16;
+                    b2 = (u8_t) (wd->sta.iv16 >> 8);
+                    header[hlen] = ((u16_t)b2 << 8) + b1;
+                    header[hlen+1] = 0x2000;
+                    header[hlen+2] = (u16_t) (wd->sta.iv32);
+                    header[hlen+3] = (u16_t) (wd->sta.iv32 >> 16);
+
+                    macCtrl |= 0xc0; /* Set to AES in control setting */
+                    icvLen = 8;  /* MIC */
+
+                    header[4] |= 0x4000; /* Set WEP bit in wlan header */
+                    hlen += 4; /* plus IV length */
+                    break;
+            }/* end of switch */
+            #endif
+        }
+    }
+    else   /* wd->wlanMode != ZM_MODE_AP */
+    {
+        encExemptionActionType = zfwGetPktEncExemptionActionType(dev, buf);
+
+        if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+        {
+            #if 1
+            /* if WME AP */
+            if (wd->sta.wmeConnected != 0)
+            {
+                /* QoS data */
+                header[4] |= 0x0080;
+
+                /* QoS Control */
+                header[hlen] = up;
+                hlen += 1;
+            }
+            #endif
+
+            if ( encExemptionActionType == ZM_ENCRYPTION_EXEMPT_NO_EXEMPTION )
+            {
+                if ( wd->sta.authMode < ZM_AUTH_MODE_WPA )
+                {   /* non-WPA */
+                    if ( wd->sta.wepStatus == ZM_ENCRYPTION_WEP_ENABLED )
+                    {
+                        if ( (wd->sta.encryMode == ZM_WEP64)||
+                             (wd->sta.encryMode == ZM_WEP128)||
+                             (wd->sta.encryMode == ZM_WEP256) )
+                        {
+                            header[4] |= 0x4000;
+                            header[hlen] = 0x0;   //IV
+                            header[hlen+1] = 0x0; //IV
+                            header[hlen+1] |= (((u16_t) wd->sta.keyId) << 14);
+                            hlen += 2;
+                            icvLen = 4;
+
+                            /* For Software WEP */
+                            if ((wd->sta.SWEncryptEnable & ZM_SW_WEP_ENCRY_EN) != 0)
+                            {
+                                u8_t keyLen = 5;
+                                u8_t iv[3];
+
+                                iv[0] = 0x0;
+                                iv[1] = 0x0;
+                                iv[2] = 0x0;
+
+                                if (wd->sta.SWEncryMode[wd->sta.keyId] == ZM_WEP64)
+                                {
+                                    keyLen = 5;
+                                }
+                                else if (wd->sta.SWEncryMode[wd->sta.keyId] == ZM_WEP128)
+                                {
+                                    keyLen = 13;
+                                }
+                                else if (wd->sta.SWEncryMode[wd->sta.keyId] == ZM_WEP256)
+                                {
+                                    keyLen = 29;
+                                }
+
+                                zfWEPEncrypt(dev, buf, (u8_t*) snap, snapLen, minusLen, keyLen,
+                                        wd->sta.wepKey[wd->sta.keyId], iv);
+                            }
+                            else
+                            {
+                                macCtrl |= 0x40;
+                            }
+                        }
+                    }
+                }
+                else
+                {   /* WPA */
+                    if ( wd->sta.wpaState >= ZM_STA_WPA_STATE_PK_OK )
+                    {
+                        wd->sta.iv16++;
+                        if ( wd->sta.iv16 == 0 )
+                        {
+                            wd->sta.iv32++;
+                        }
+
+                        /* set encryption mode */
+                        if ( wd->sta.encryMode == ZM_TKIP )
+                        {
+                            b1 = (u8_t) (wd->sta.iv16 >> 8);
+                            b2 = (b1 | 0x20) & 0x7f;
+                            header[hlen] = ((u16_t)b2 << 8) + b1;
+                            b1 = (u8_t) wd->sta.iv16;
+                            b2 = 0x20;
+
+                            // header[hlen+1] = (((u16_t) wd->sta.keyId) << 14) | (((u16_t)b2 << 8) + b1);
+                            // STA in infrastructure mode should use keyId = 0 to transmit unicast !
+                            header[hlen+1] = (((u16_t)b2 << 8) + b1);
+                            header[hlen+2] = (u16_t) wd->sta.iv32;
+                            header[hlen+3] = (u16_t) (wd->sta.iv32 >> 16);
+
+                            /* If software encryption enable */
+                            if ((wd->sta.SWEncryptEnable & ZM_SW_TKIP_ENCRY_EN) == 0)
+                            {
+                                //macCtrl |= 0x80;
+                                /* TKIP same to WEP */
+                                macCtrl |= 0x40;
+                                icvLen = 4;
+
+                                /* set hardware MIC */
+                                if ( (!(seq & 0xf))&&(!(flag & 0x4)) )
+                                {
+                                    macCtrl |= 0x100;
+                                    plusLen += 8;
+                                    *micLen = 8;
+                                }
+                            }
+                            else
+                            {
+                                u8_t mic[8];
+                                u16_t offset;
+                                u32_t icv;
+                                u8_t RC4Key[16];
+
+                                /* TODO: Remove the criticial section here. */
+                                zmw_declare_for_critical_section();
+
+                                zmw_enter_critical_section(dev);
+                                /* Calculate MIC */
+                                zfCalTxMic(dev, buf, (u8_t *)snap, snapLen, minusLen, da, sa, up, mic);
+
+                                offset = zfwBufGetSize(dev, buf);
+
+                                /* Append MIC to the buffer */
+                                zfCopyToIntTxBuffer(dev, buf, mic, offset, 8);
+                                zfwBufSetSize(dev, buf, offset+8);
+                                zmw_leave_critical_section(dev);
+
+                                /* TKIP Key Mixing */
+                                zfTkipPhase1KeyMix(wd->sta.iv32, &wd->sta.txSeed);
+                                zfTkipPhase2KeyMix(wd->sta.iv16, &wd->sta.txSeed);
+                                zfTkipGetseeds(wd->sta.iv16, RC4Key, &wd->sta.txSeed);
+
+                                /* Encrypt Data */
+                                zfTKIPEncrypt(dev, buf, (u8_t *)snap, snapLen, minusLen, 16, RC4Key, &icv);
+
+                                icvLen = 4;
+                                len += 8;
+                            }
+
+                            header[4] |= 0x4000;
+                            hlen += 4;
+                        }
+                        else if ( wd->sta.encryMode == ZM_AES )
+                        {
+                            b1 = (u8_t) wd->sta.iv16;
+                            b2 = (u8_t) (wd->sta.iv16 >> 8);
+                            header[hlen] = ((u16_t)b2 << 8) + b1;
+                            // header[hlen+1] = (((u16_t) wd->sta.keyId) << 14) | (0x2000);
+                            // STA in infrastructure mode should use keyId = 0 to transmit unicast !
+                            header[hlen+1] = 0x2000;
+                            header[hlen+2] = (u16_t) (wd->sta.iv32);
+                            header[hlen+3] = (u16_t) (wd->sta.iv32 >> 16);
+
+                            macCtrl |= 0xc0;
+                            icvLen = 8;  /* MIC */
+
+                            header[4] |= 0x4000;
+                            hlen += 4;
+                        }
+                        #ifdef ZM_ENABLE_CENC
+                        else if ( wd->sta.encryMode == ZM_CENC )
+                        {
+                            /* Accumlate the PN sequence */
+                            wd->sta.txiv[0] += 2;
+
+                            if (wd->sta.txiv[0] == 0 || wd->sta.txiv[0] == 1)
+                            {
+                                wd->sta.txiv[1]++;
+                            }
+
+                            if (wd->sta.txiv[1] == 0)
+                            {
+                                wd->sta.txiv[2]++;
+                            }
+
+                            if (wd->sta.txiv[2] == 0)
+                            {
+                                wd->sta.txiv[3]++;
+                            }
+
+                            if (wd->sta.txiv[3] == 0)
+                            {
+                                wd->sta.txiv[0] = 0;
+                                wd->sta.txiv[1] = 0;
+                                wd->sta.txiv[2] = 0;
+                            }
+
+                            header[hlen] = (wd->sta.cencKeyId & 0x0001);    /* For Key Id and reserved field */
+                            header[hlen+1] = (u16_t) wd->sta.txiv[0];
+                            header[hlen+2] = (u16_t) (wd->sta.txiv[0] >> 16);
+                            header[hlen+3] = (u16_t) wd->sta.txiv[1];
+                            header[hlen+4] = (u16_t) (wd->sta.txiv[1] >> 16);
+                            header[hlen+5] = (u16_t) wd->sta.txiv[2];
+                            header[hlen+6] = (u16_t) (wd->sta.txiv[2] >> 16);
+                            header[hlen+7] = (u16_t) wd->sta.txiv[3];
+                            header[hlen+8] = (u16_t) (wd->sta.txiv[3] >> 16);
+
+                            macCtrl |= 0x80;
+                            icvLen = 16;  /* MIC */
+
+                            header[4] |= 0x4000;
+                            hlen += 9;
+                        }
+                        #endif //ZM_ENABLE_CENC
+                    }
+                }
+            } // if ( encExemptionActionType == ZM_ENCRYPTION_EXEMPT_NO_EXEMPTION )
+        } /* if ( wd->wlanMode != ZM_MODE_INFRASTRUCTURE ) */
+
+        if ( wd->wlanMode == ZM_MODE_IBSS )
+        {
+            if ( encExemptionActionType == ZM_ENCRYPTION_EXEMPT_NO_EXEMPTION )
+            {
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+                if( wd->sta.oppositeInfo[userIdx].wpaState >= ZM_STA_WPA_STATE_PK_OK || wd->sta.wpaState >= ZM_STA_WPA_STATE_PK_OK)
+                {
+                    int isUnicast = 1 ;
+
+                    if((da[0]& 0x1))
+                    {
+                        isUnicast = 0 ; // Not unicast , is broadcast
+                    }
+
+                    if( wd->sta.ibssWpa2Psk == 1 )
+                    { /* The IV order is not the same between unicast and broadcast ! */
+                        if ( isUnicast )
+                        {
+                            iv16 = &wd->sta.oppositeInfo[userIdx].iv16;
+                            iv32 = &wd->sta.oppositeInfo[userIdx].iv32;
+                        }
+                        else
+                        {
+                            iv16 = &wd->sta.iv16;
+                            iv32 = &wd->sta.iv32;
+                        }
+                    }
+                    else
+                    {
+                        iv16 = &wd->sta.iv16;
+                        iv32 = &wd->sta.iv32;
+                    }
+
+                    (*iv16)++;
+                    if ( *iv16 == 0 )
+                    {
+                        *iv32++;
+                    }
+
+                    if ( wd->sta.oppositeInfo[userIdx].encryMode == ZM_AES || wd->sta.encryMode == ZM_AES)
+                    {
+                        //printk("Station encryption mode is AES-CCMP\n") ;
+                        b1 = (u8_t) (*iv16);
+                        b2 = (u8_t) ((*iv16) >> 8);
+                        header[hlen] = ((u16_t)b2 << 8) + b1;
+
+                        if ( isUnicast )
+                        {
+                            header[hlen+1] = 0x2000;
+                        }
+                        else
+                        {
+                            header[hlen+1] = 0x2000 | (((u16_t) wd->sta.keyId) << 14);
+                        }
+
+                        header[hlen+2] = (u16_t) (*iv32);
+                        header[hlen+3] = (u16_t) ((*iv32) >> 16);
+                        macCtrl |= 0xc0;
+                        icvLen = 8;  /* MIC */
+                    }
+
+                    header[4] |= 0x4000;
+                    hlen += 4;
+                }
+                else if ( wd->sta.wepStatus == ZM_ENCRYPTION_WEP_ENABLED)
+                {
+                    if ( (wd->sta.encryMode == ZM_WEP64)||
+                        (wd->sta.encryMode == ZM_WEP128)||
+                        (wd->sta.encryMode == ZM_WEP256) )
+                    {
+                        header[4] |= 0x4000;
+                        header[hlen] = 0x0;   //IV
+                        header[hlen+1] = 0x0; //IV
+                        header[hlen+1] |= (((u16_t) wd->sta.keyId) << 14);
+                        hlen += 2;
+                        icvLen = 4;
+                        macCtrl |= 0x40;
+                    }
+                }
+#else
+                /* ----- 20070405 add by Mxzeng ----- */
+                if( wd->sta.wpaState >= ZM_STA_WPA_STATE_PK_OK )
+                {
+                    int isUnicast = 1 ;
+
+                    if((da[0]& 0x1))
+                    {
+                        isUnicast = 0 ; // Not unicast , is broadcast
+                    }
+
+                    wd->sta.iv16++;
+                    if ( wd->sta.iv16 == 0 )
+                    {
+                        wd->sta.iv32++;
+                    }
+
+                    if ( wd->sta.encryMode == ZM_AES )
+                    {
+                        //printk("Station encryption mode is AES-CCMP\n") ;
+                        b1 = (u8_t) wd->sta.iv16;
+                        b2 = (u8_t) (wd->sta.iv16 >> 8);
+                        header[hlen] = ((u16_t)b2 << 8) + b1;
+
+                        if ( isUnicast )
+                        {
+                            header[hlen+1] = 0x2000;
+                        }
+                        else
+                        {
+                            header[hlen+1] = 0x2000 | (((u16_t) wd->sta.keyId) << 14);
+                        }
+
+                            header[hlen+2] = (u16_t) (wd->sta.iv32);
+                            header[hlen+3] = (u16_t) (wd->sta.iv32 >> 16);
+                            macCtrl |= 0xc0;
+                            icvLen = 8;  /* MIC */
+                    }
+
+                    header[4] |= 0x4000;
+                    hlen += 4;
+                }
+                else if ( wd->sta.wepStatus == ZM_ENCRYPTION_WEP_ENABLED)
+                {
+                    if ( (wd->sta.encryMode == ZM_WEP64)||
+                         (wd->sta.encryMode == ZM_WEP128)||
+                         (wd->sta.encryMode == ZM_WEP256) )
+                    {
+                        header[4] |= 0x4000;
+                        header[hlen] = 0x0;   //IV
+                        header[hlen+1] = 0x0; //IV
+                        header[hlen+1] |= (((u16_t) wd->sta.keyId) << 14);
+                        hlen += 2;
+                        icvLen = 4;
+                        macCtrl |= 0x40;
+                    }
+                }
+#endif
+            }   // End if ( encExemptionActionType == ZM_ENCRYPTION_EXEMPT_NO_EXEMPTION )
+        }   // End if ( wd->wlanMode == ZM_MODE_IBSS )
+        else if ( wd->wlanMode == ZM_MODE_PSEUDO )
+        {
+            switch (wd->sta.encryMode)
+	        {
+            case ZM_WEP64:
+            case ZM_WEP128:
+            case ZM_WEP256:
+                header[4] |= 0x4000;
+                header[hlen] = 0x0;   //IV
+                header[hlen+1] = 0x0; //IV
+                hlen += 2;
+                icvLen = 4;
+                macCtrl |= 0x40;
+                break;
+
+            case ZM_TKIP:
+            {
+                wd->sta.iv16++;
+                if ( wd->sta.iv16 == 0 )
+                {
+                    wd->sta.iv32++;
+                }
+
+                b1 = (u8_t) (wd->sta.iv16 >> 8);
+                b2 = (b1 | 0x20) & 0x7f;
+                header[hlen] = ((u16_t)b2 << 8) + b1;
+                b1 = (u8_t) wd->sta.iv16;
+                b2 = 0x20;
+                header[hlen+1] = ((u16_t)b2 << 8) + b1;
+                header[hlen+2] = (u16_t) wd->sta.iv32;
+                header[hlen+3] = (u16_t) (wd->sta.iv32 >> 16);
+
+                //macCtrl |= 0x80;
+                macCtrl |= 0x40;
+                icvLen = 4;
+
+                /* set hardware MIC */
+                if ( (!(seq & 0xf))&&(!(flag & 0x4)) )
+                {
+                    macCtrl |= 0x100;
+                    plusLen += 8;
+                    *micLen = 8;
+                }
+
+                header[4] |= 0x4000;
+                hlen += 4;
+            }/* end of PSEUDO TKIP */
+                break;
+
+            case ZM_AES:
+            {
+                wd->sta.iv16++;
+                if ( wd->sta.iv16 == 0 )
+                {
+                    wd->sta.iv32++;
+                }
+
+                b1 = (u8_t) wd->sta.iv16;
+                b2 = (u8_t) (wd->sta.iv16 >> 8);
+                header[hlen] = ((u16_t)b2 << 8) + b1;
+                header[hlen+1] = 0x2000;
+                header[hlen+2] = (u16_t) (wd->sta.iv32);
+                header[hlen+3] = (u16_t) (wd->sta.iv32 >> 16);
+                macCtrl |= 0xc0;
+                icvLen = 8;  /* MIC */
+                header[4] |= 0x4000;
+                hlen += 4;
+            }/* end of PSEUDO AES */
+                    break;
+
+              #ifdef ZM_ENABLE_CENC
+              case ZM_CENC:
+                    /* Accumlate the PN sequence */
+                    wd->sta.txiv[0] += 2;
+
+                    if (wd->sta.txiv[0] == 0 || wd->sta.txiv[0] == 1)
+                    {
+                        wd->sta.txiv[1]++;
+                    }
+
+                    if (wd->sta.txiv[1] == 0)
+                    {
+                        wd->sta.txiv[2]++;
+                    }
+
+                    if (wd->sta.txiv[2] == 0)
+                    {
+                        wd->sta.txiv[3]++;
+                    }
+
+                    if (wd->sta.txiv[3] == 0)
+                    {
+                        wd->sta.txiv[0] = 0;
+                        wd->sta.txiv[1] = 0;
+                        wd->sta.txiv[2] = 0;
+                    }
+
+                    header[hlen] = 0;
+                    header[hlen+1] = (u16_t) wd->sta.txiv[0];
+                    header[hlen+2] = (u16_t) (wd->sta.txiv[0] >> 16);
+                    header[hlen+3] = (u16_t) wd->sta.txiv[1];
+                    header[hlen+4] = (u16_t) (wd->sta.txiv[1] >> 16);
+                    header[hlen+5] = (u16_t) wd->sta.txiv[2];
+                    header[hlen+6] = (u16_t) (wd->sta.txiv[2] >> 16);
+                    header[hlen+7] = (u16_t) wd->sta.txiv[3];
+                    header[hlen+8] = (u16_t) (wd->sta.txiv[3] >> 16);
+
+                    macCtrl |= 0x80;
+                    icvLen = 16;  /* MIC */
+
+                    header[4] |= 0x4000;
+                    hlen += 9;
+				break;
+		    #endif //ZM_ENABLE_CENC
+			}/* end of switch */
+		}
+
+        /* Generate control setting */
+
+        /* protection mode */
+        if (wd->enableProtectionMode)
+        {
+            if (wd->enableProtectionMode==2)
+            {
+                /* Force enable protection: self cts  */
+                macCtrl &= 0xFFFC;
+                macCtrl |= 2;
+            }
+            /* if wd->enableProtectionMode=1 => force disable */
+            /* if wd->enableProtectionMode=0 => auto */
+        }
+        else
+        {
+
+            /* protection mode */
+            if (wd->sta.bProtectionMode == TRUE)
+            {
+                /* Enable Self-CTS */
+                macCtrl &= 0xFFFC;
+                macCtrl |= 2;
+            }
+        }
+
+    }
+
+    if (wd->txMCS != 0xff)
+    {
+        /* fixed rate */
+	    phyCtrl = ((u32_t)wd->txMCS<<16) + wd->txMT;
+        mcs = wd->txMCS;
+        mt = wd->txMT;
+    }
+
+    if (mt == 2)
+    {
+#if 0
+        /* HT PT: 0 Mixed mode    1 Green field */
+	    if (wd->sta.preambleTypeHT == ZM_PREAMBLE_TYPE_GREEN_FIELD)
+	    {
+            phyCtrl |= 0x4;     /* Bit 2 */
+        }
+#endif
+        /* Bandwidth */
+        if (wd->sta.htCtrlBandwidth == ZM_BANDWIDTH_40MHZ)
+        {
+            phyCtrl |= (0x80<<16);  /* BIT 23 */
+        }
+#if 0
+        /* STBC */
+        if (wd->sta.htCtrlSTBC<=0x3)
+        {
+            phyCtrl |= (wd->sta.htCtrlSTBC<<28);   /* BIT 23 */
+        }
+#endif
+        /* Short GI */
+        if(wd->sta.htCtrlSG)
+        {
+            phyCtrl |= (0x8000<<16);         /* BIT 31 */
+        }
+
+        /* TA */
+        if ( ((mcs >=0x8) && (mcs<=0xf))  || (wd->sta.htCtrlSTBC) )
+        {
+       	    phyCtrl |= 0x1800;               /* BIT 11 12 */
+    	}
+    }
+    else if(mt == 1)
+    {
+        #if 0
+        //bug that cause OFDM rate become duplicate legacy rate
+        /* Bandwidth */
+        if (wd->sta.htCtrlBandwidth == ZM_BANDWIDTH_40MHZ)
+        {
+            phyCtrl |= (0x80<<16);  /* BIT 23 */
+            mt = 3;                 /* duplicate legacy */
+            phyCtrl |= mt;
+        }
+        #endif
+    }
+    else if(mt == 0)
+    {
+	/* CCK PT: Legcy Preamble: 1 long preamble    2 short preamble */
+        if (wd->preambleTypeInUsed == ZM_PREAMBLE_TYPE_SHORT)
+        {
+    	       //phyCtrl |= 0x4;    /* BIT 2 */
+    	}
+    }
+
+    /* TA */
+    if (wd->sta.defaultTA)
+    {
+        phyCtrl |= 0x1000;
+    }
+    else
+    {
+        phyCtrl |= 0x0800;
+    }
+
+    //Get CurrentTxRate -- CWYang(+)
+    if ((mt == 0) || (mt == 1)) //B,G Rate
+    {
+        if (mcs < 16)
+        {
+            wd->CurrentTxRateKbps = zcIndextoRateBG[mcs];
+        }
+    }
+    else if (mt == 2)
+    {
+        if (mcs < 16)
+        {
+            if (wd->sta.htCtrlBandwidth == ZM_BANDWIDTH_40MHZ)
+            {
+                if((phyCtrl & 0x80000000) != 0)
+                {
+                    /* Short GI 40 MHz MIMO Rate */
+                    wd->CurrentTxRateKbps = zcIndextoRateN40S[mcs];
+                }
+                else
+                {
+                    /* Long GI 40 MHz MIMO Rate */
+                    wd->CurrentTxRateKbps = zcIndextoRateN40L[mcs];
+                }
+            }
+            else
+            {
+                if((phyCtrl & 0x80000000) != 0)
+                {
+                    /* Short GI 20 MHz MIMO Rate */
+                    wd->CurrentTxRateKbps = zcIndextoRateN20S[mcs];
+                }
+                else
+                {
+                    /* Long GI 20 MHz MIMO Rate */
+                    wd->CurrentTxRateKbps = zcIndextoRateN20L[mcs];
+                }
+            }
+        }
+    }
+
+    //802.11 header(include IV) = (hlen<<1)-8
+    //ethernet frame = len
+    //snap + mic = plusLen
+    //ethernet header = minusLen
+    //icv = icvLen
+    //crc32 = 4
+    //length=802.11 header+snap+(ethernet frame-ethernet header)+mic+icv+crc32
+    header[0] = ((hlen<<1)-8)+plusLen+(len-minusLen)+icvLen+4;  //Length
+
+    // header[0] : MPDU Lengths
+    if ((header[6] & 0x1) != 0x1) // Unicast Frame
+    {
+        if (header[0] >= wd->rtsThreshold)
+        {
+            /* Enable RTS */
+            macCtrl |= 1;
+        }
+    }
+
+    if ( wd->sta.encryMode == ZM_TKIP )
+        tkipFrameOffset = 8;
+
+    if( wd->sta.EnableHT != 1 )
+    { // Aggregation should not be fragmented !
+        if ( header[0] > ( wd->fragThreshold + tkipFrameOffset ) )
+        {
+            return 0; // Need to be fragmented ! !
+        }
+    }
+
+    //if ( wd->sta.encryMode == ZM_TKIP )
+    //{
+    //    zm_debug_msg1("ctrl length = ", header[0]);
+    //}
+
+    //MAC control
+    if (rateProbingFlag != 0)
+    {
+        macCtrl |= 0x8000;
+    }
+    header[1] = macCtrl;
+    //PHY control L
+    header[2] = (u16_t) ((phyCtrl&0xffff) | 0x700 | (zcUpToAc[up&0x7]<<13));
+    //PHY control H
+    header[3] = (u16_t) ((phyCtrl>>16) | 0x700);
+
+    if (wd->enableAggregation)
+    {
+        /* force enable aggregation */
+        if (wd->enableAggregation==2 && !(header[6]&0x1))
+        {
+            if (((header[2] & 0x3) == 2))
+            {
+                /* Enable aggregation */
+                header[1] |= 0x20;
+            }
+        }
+        /* if wd->enableAggregation=1 => force disable */
+        /* if wd->enableAggregation=0 => auto */
+    }
+
+#ifdef ZM_ENABLE_AGGREGATION
+    if (wd->addbaComplete) {
+        #ifdef ZM_BYPASS_AGGR_SCHEDULING
+        if (!(header[6]&0x1) && !rateProbingFlag && (wd->enableAggregation != 1))
+        {
+            if (((header[2] & 0x3) == 2))
+            {
+                /* Unicast frame with HT rate => Enable aggregation */
+                /* We only support software encryption in single packet mode */
+                if ((wd->sta.SWEncryptEnable & ZM_SW_TKIP_ENCRY_EN) == 0 &&
+                    (wd->sta.SWEncryptEnable & ZM_SW_WEP_ENCRY_EN) == 0)
+                {
+                    /* Set aggregation group bits per AC */
+                    header[1] |= (0x20 | (zcUpToAc[up&0x7]<<10));
+
+                    //if (wd->sta.currentFrequency < 3000)
+                    {
+                        /* issue: -PB42 Enable RTS/CTS to prevent OWL Tx hang up */
+                        /* If this is Owl Ap, enable RTS/CTS protect */
+                        if ( (wd->sta.athOwlAp == 1) || (wd->sta.RTSInAGGMode == TRUE) )
+                        {
+                            header[1] &= 0xfffc;
+                            header[1] |= 0x1;
+                        }
+
+                        /* Enable RIFS : workaround 854T RTS/CTS */
+                        /* Bit13 : TI enable RIFS */
+                        //header[1] |= 0x2000;
+                    }
+                }
+            }
+        }
+        #else
+        /*
+         * aggregation ampduIndication control
+         */
+        if (aggControl && aggControl->aggEnabled) {
+            if (wd->enableAggregation==0 && !(header[6]&0x1))
+            {
+                if (((header[2] & 0x3) == 2))
+                {
+                    /* Enable aggregation */
+                    header[1] |= 0x20;
+                    if (ZM_AGG_LAST_MPDU == aggControl->ampduIndication)
+                        header[1] |= 0x4000;
+                }
+                else {
+                    zm_debug_msg1("no aggr, header[2]&0x3 = ",header[2] & 0x3)
+                    aggControl->aggEnabled = 0;
+                }
+            }
+            else {
+                zm_debug_msg1("no aggr, wd->enableAggregation = ", wd->enableAggregation);
+                zm_debug_msg1("no aggr, !header[6]&0x1 = ",!(header[6]&0x1));
+                aggControl->aggEnabled = 0;
+            }
+        }
+        #endif
+
+        #ifdef ZM_AGGR_BIT_ON
+        if (!(header[6]&0x1) && !rateProbingFlag)
+        {
+            if (((header[2] & 0x3) == 2))
+            {
+                /* Unicast frame with HT rate => Enable aggregation */
+                /* Set aggregation group bits per AC */
+                header[1] |= (0x20 | (zcUpToAc[up&0x7]<<10));
+
+                //if (wd->sta.currentFrequency < 3000)
+                {
+                    /* Enable RTS/CTS to prevent OWL Tx hang up */
+                    header[1] &= 0xfffc;
+                    header[1] |= 0x1;
+                }
+            }
+        }
+        #endif
+    }
+#endif
+
+    return (hlen<<1);
+}
+
+
+u16_t zfTxGenMmHeader(zdev_t* dev, u8_t frameType, u16_t* dst,
+        u16_t* header, u16_t len, zbuf_t* buf, u16_t vap, u8_t encrypt)
+{
+    //u16_t bodyLen;
+    u8_t  hlen = 32;        // MAC ctrl + PHY ctrl + 802.11 MM header
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    /* Generate control setting */
+    //bodyLen = zfwBufGetSize(dev, buf);
+    header[0] = 24+len+4;   //Length
+    if ((dst[0] & 0x1) != 0) //Broadcast, multicast frames
+    {
+        header[1] = 0xc;            //MAC control, backoff + noack
+    }
+    else
+    {
+        header[1] = 0x8;            //MAC control, backoff + (ack)
+    }
+    /* Dualband Management frame tx Rate */
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        if (wd->frequency < 3000)
+        {
+            /* CCK 1M */
+            header[2] = 0x0f00;          //PHY control L
+            header[3] = 0x0000;          //PHY control H
+        }
+        else
+        {
+            /* CCK 6M */
+            header[2] = 0x0f01;          //PHY control L
+            header[3] = 0x000B;          //PHY control H
+        }
+    }
+    else
+    {
+        if (wd->sta.currentFrequency < 3000)
+        {
+            /* CCK 2M */
+            header[2] = 0x0f00;          //PHY control L
+            header[3] = 0x0001;          //PHY control H
+        }
+        else
+        {
+            /* CCK 6M */
+            header[2] = 0x0f01;          //PHY control L
+            header[3] = 0x000B;          //PHY control H
+        }
+    }
+    /* Generate WLAN header */
+    /* Frame control */
+    header[4+0] = frameType;
+    /* Duration */
+    header[4+1] = 0;
+
+    if (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)
+    {
+        if ( frameType == ZM_WLAN_FRAME_TYPE_PROBEREQ )
+        {
+            header[4+8] = 0xFFFF;
+            header[4+9] = 0xFFFF;
+            header[4+10] = 0xFFFF;
+        }
+        else if ( frameType == ZM_WLAN_FRAME_TYPE_BA ) {
+            /* do nothing */
+        }
+        else
+        {
+            header[4+8] = wd->sta.bssid[0];
+            header[4+9] = wd->sta.bssid[1];
+            header[4+10] = wd->sta.bssid[2];
+        }
+    }
+    else if (wd->wlanMode == ZM_MODE_PSEUDO)
+    {
+        /* Address 3 = 00:00:00:00:00:00 */
+        header[4+8] = 0;
+        header[4+9] = 0;
+        header[4+10] = 0;
+    }
+    else if (wd->wlanMode == ZM_MODE_IBSS)
+    {
+        header[4+8] = wd->sta.bssid[0];
+        header[4+9] = wd->sta.bssid[1];
+        header[4+10] = wd->sta.bssid[2];
+
+        if ( frameType == ZM_WLAN_FRAME_TYPE_ATIM )
+        {
+            /* put ATIM to queue 5th */
+            //header[2] |= (ZM_BIT_13|ZM_BIT_14);
+            header[2] |= ZM_BIT_15;
+        }
+    }
+    else if (wd->wlanMode == ZM_MODE_AP)
+    {
+        /* Address 3 = BSSID */
+        header[4+8] = wd->macAddr[0];
+        header[4+9] = wd->macAddr[1];
+#ifdef ZM_VAPMODE_MULTILE_SSID
+        header[4+10] = wd->macAddr[2]; //Multiple SSID
+#else
+        header[4+10] = wd->macAddr[2] + (vap<<8); //VAP
+#endif
+        //if in scan, must set address 3 to broadcast because of some ap would care this
+        //if ((wd->heartBeatNotification & ZM_BSSID_LIST_SCAN)
+        //        == ZM_BSSID_LIST_SCAN)
+        //if FrameType is Probe Request, Address3 should be boradcast
+        if (frameType == ZM_WLAN_FRAME_TYPE_PROBEREQ)
+        {
+            header[4+8] = 0xFFFF;
+            header[4+9] = 0xFFFF;
+            header[4+10] = 0xFFFF;
+        }
+    }
+
+    /* Address 1 = DA */
+    header[4+2] = dst[0];
+    header[4+3] = dst[1];
+    header[4+4] = dst[2];
+
+    /* Address 2 = SA */
+    header[4+5] = wd->macAddr[0];
+    header[4+6] = wd->macAddr[1];
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+#ifdef ZM_VAPMODE_MULTILE_SSID
+        header[4+7] = wd->macAddr[2]; //Multiple SSID
+#else
+        header[4+7] = wd->macAddr[2] + (vap<<8); //VAP
+#endif
+    }
+    else
+    {
+        header[4+7] = wd->macAddr[2];
+    }
+
+    /* Sequence Control */
+    zmw_enter_critical_section(dev);
+    header[4+11] = ((wd->mmseq++)<<4);
+    zmw_leave_critical_section(dev);
+
+    if( frameType == ZM_WLAN_FRAME_TYPE_QOS_NULL )
+    {
+        /*Qos Control*/
+        header[4+12] = 0x0;
+        hlen+=2;
+        header[0]+=2;
+    }
+
+    if ( encrypt )
+    {
+        if ( wd->sta.wepStatus == ZM_ENCRYPTION_WEP_ENABLED )
+        {
+            if ( (wd->sta.encryMode == ZM_WEP64)||
+                 (wd->sta.encryMode == ZM_WEP128)||
+                 (wd->sta.encryMode == ZM_WEP256) )
+            {
+                header[4] |= 0x4000;
+                header[16] = 0x0;   //IV
+                header[17] = 0x0; //IV
+                header[17] |= (((u16_t) wd->sta.keyId) << 14);
+                hlen += 4;
+
+                header[0] += 8;         // icvLen = 4;
+                header[1] |= 0x40;      // enable encryption on macCtrl
+             }
+        }
+    }
+
+    // Enable HW duration
+    if ( frameType != ZM_WLAN_FRAME_TYPE_PSPOLL )
+    {
+        header[1] |= 0x200;
+    }
+
+    return hlen;
+}
+
+void zfInitMacApMode(zdev_t* dev)
+{
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    zfHpEnableBeacon(dev, ZM_MODE_AP, (wd->beaconInterval/wd->ap.vapNumber), 1, 0);
+
+    /* AP mode */
+    zfHpSetApStaMode(dev, ZM_HAL_80211_MODE_AP);
+
+    /* VAP test code */
+    /* AP + VAP mode */
+    if (wd->ap.vapNumber >= 2)
+    {
+        for (i=1; i<ZM_MAX_AP_SUPPORT; i++)
+        {
+            if (((wd->ap.apBitmap >> i) & 0x1) != 0)
+            {
+                u16_t mac[3];
+                mac[0] = wd->macAddr[0];
+                mac[1] = wd->macAddr[1];
+#ifdef ZM_VAPMODE_MULTILE_SSID
+                mac[2] = wd->macAddr[2]; //Multiple SSID
+#else
+                mac[2] = wd->macAddr[2] + (i<<8); //VAP
+#endif
+                zfHpSetMacAddress(dev, mac, i);
+
+            }
+        }
+    }
+
+    /* basic rate setting */
+    zfHpSetBasicRateSet(dev, wd->bRateBasic, wd->gRateBasic);
+
+    /* Set TxQs CWMIN, CWMAX, AIFS and TXO to WME AP default. */
+    zfUpdateDefaultQosParameter(dev, 1);
+
+    return;
+}
+
+u16_t zfChGetNextChannel(zdev_t* dev, u16_t frequency, u8_t* pbPassive)
+{
+    u8_t   i;
+    u8_t   bPassive;
+
+    zmw_get_wlan_dev(dev);
+
+    /* Avoid NULL value */
+    if ( pbPassive == NULL )
+    {
+        pbPassive = &bPassive;
+    }
+
+    for( i=0; i<wd->regulationTable.allowChannelCnt; i++ )
+    {
+        if ( wd->regulationTable.allowChannel[i].channel == frequency )
+        {
+            if ( i == (wd->regulationTable.allowChannelCnt-1) )
+            {
+                i = 0;
+            }
+            else
+            {
+                i++;
+            }
+
+            if ( wd->regulationTable.allowChannel[i].channelFlags
+                    & ZM_REG_FLAG_CHANNEL_PASSIVE )
+            {
+                *pbPassive = TRUE;
+            }
+            else
+            {
+                *pbPassive = FALSE;
+            }
+
+            return wd->regulationTable.allowChannel[i].channel;
+        }
+    }
+
+    return 0xffff;
+}
+
+u16_t zfChGetFirstChannel(zdev_t* dev, u8_t* pbPassive)
+{
+    u8_t   bPassive;
+
+    zmw_get_wlan_dev(dev);
+
+    /* Avoid NULL value */
+    if ( pbPassive == NULL )
+    {
+        pbPassive = &bPassive;
+    }
+
+   if ( wd->regulationTable.allowChannel[0].channelFlags & ZM_REG_FLAG_CHANNEL_PASSIVE )
+    {
+        *pbPassive = TRUE;
+    }
+    else
+    {
+        *pbPassive = FALSE;
+    }
+
+    return wd->regulationTable.allowChannel[0].channel;
+}
+
+u16_t zfChGetFirst2GhzChannel(zdev_t* dev)
+{
+    u8_t    i;
+
+    zmw_get_wlan_dev(dev);
+
+    for( i=0; i<wd->regulationTable.allowChannelCnt; i++ )
+    {
+        if ( wd->regulationTable.allowChannel[i].channel < 3000 )
+        {
+            /* find the first 2Ghz channel */
+            return wd->regulationTable.allowChannel[i].channel;
+        }
+    }
+
+    /* Can not find any 2Ghz channel */
+    return 0;
+}
+
+u16_t zfChGetFirst5GhzChannel(zdev_t* dev)
+{
+    u8_t    i;
+
+    zmw_get_wlan_dev(dev);
+
+    for( i=0; i<wd->regulationTable.allowChannelCnt; i++ )
+    {
+        if ( wd->regulationTable.allowChannel[i].channel > 3000 )
+        {
+            /* find the first 5Ghz channel */
+            return wd->regulationTable.allowChannel[i].channel;
+        }
+    }
+
+    /* Can not find any 5Ghz channel */
+    return 0;
+}
+
+u16_t zfChGetLastChannel(zdev_t* dev, u8_t* pbPassive)
+{
+    u8_t   bPassive;
+    u8_t   ChannelIndex;
+
+    zmw_get_wlan_dev(dev);
+
+    ChannelIndex = wd->regulationTable.allowChannelCnt-1;
+
+    /* Avoid NULL value */
+    if ( pbPassive == NULL )
+    {
+        pbPassive = &bPassive;
+    }
+
+    if ( wd->regulationTable.allowChannel[ChannelIndex].channelFlags
+            & ZM_REG_FLAG_CHANNEL_PASSIVE )
+    {
+        *pbPassive = TRUE;
+    }
+    else
+    {
+        *pbPassive = FALSE;
+    }
+
+    return wd->regulationTable.allowChannel[ChannelIndex].channel;
+}
+
+u16_t zfChGetLast5GhzChannel(zdev_t* dev)
+{
+    u8_t    i;
+    u16_t   last5Ghzfrequency;
+
+    zmw_get_wlan_dev(dev);
+
+    last5Ghzfrequency = 0;
+    for( i=0; i<wd->regulationTable.allowChannelCnt; i++ )
+    {
+        if ( wd->regulationTable.allowChannel[i].channel > 3000 )
+        {
+            last5Ghzfrequency = wd->regulationTable.allowChannel[i].channel;
+        }
+    }
+
+    return last5Ghzfrequency;
+}
+
+/* freqBand = 0 => auto check   */
+/*          = 1 => 2.4 GHz band */
+/*          = 2 => 5 GHz band   */
+u16_t zfChNumToFreq(zdev_t* dev, u8_t ch, u8_t freqBand)
+{
+    u16_t freq = 0xffff;
+
+    if ( freqBand == 0 )
+    {
+        if (ch > 14)
+        {   /* adapter is at 5 GHz band */
+            freqBand = 2;
+        }
+        else
+        {
+            freqBand = 1;
+        }
+    }
+
+    if ( freqBand == 2 )
+    {   /* the channel belongs to 5 GHz band */
+        if ( (ch >= 184)&&(ch <= 196) )
+        {
+            freq = 4000 + ch*5;
+        }
+        else
+        {
+            freq = 5000 + ch*5;
+        }
+    }
+    else
+    {   /* the channel belongs to 2.4 GHz band */
+        if ( ch == 14 )
+        {
+            freq = ZM_CH_G_14;
+        }
+        else
+        {
+            freq = ZM_CH_G_1 + (ch-1)*5;
+        }
+    }
+
+    return freq;
+}
+
+u8_t zfChFreqToNum(u16_t freq, u8_t* pbIs5GBand)
+{
+    u8_t   ch;
+    u8_t   Is5GBand;
+
+    /* to avoid NULL value */
+    if ( pbIs5GBand == NULL )
+    {
+        pbIs5GBand = &Is5GBand;
+    }
+
+    *pbIs5GBand = FALSE;
+
+    if ( freq == ZM_CH_G_14 )
+    {
+        ch = 14;
+    }
+    else if ( freq < 4000 )
+    {
+        ch = (freq - ZM_CH_G_1) / 5 + 1;
+    }
+    else if ( freq < 5000 )
+    {
+        ch = (freq - 4000) / 5;
+        *pbIs5GBand = TRUE;
+    }
+    else
+    {
+        ch = (freq - 5000) / 5;
+        *pbIs5GBand = TRUE;
+    }
+
+    return ch;
+}
diff --git a/drivers/staging/otus/80211core/cmm.c b/drivers/staging/otus/80211core/cmm.c
new file mode 100644
index 00000000000..b74379d928f
--- /dev/null
+++ b/drivers/staging/otus/80211core/cmm.c
@@ -0,0 +1,2141 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : mm.c                                                  */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains common functions for handle management     */
+/*      frame.                                                          */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+#include "cprecomp.h"
+#include "../hal/hpreg.h"
+
+/* TODO : put all constant tables to a file */
+const u8_t zg11bRateTbl[4] = {2, 4, 11, 22};
+const u8_t zg11gRateTbl[8] = {12, 18, 24, 36, 48, 72, 96, 108};
+
+/* 0xff => element does not exist */
+const u8_t zgElementOffsetTable[] =
+{
+    4,      /*  0 : asoc req */
+    6,      /*  1 : asoc rsp */
+    10,     /*  2 : reasoc req*/
+    6,      /*  3 : reasoc rsp */
+    0,      /*  4 : probe req */
+    12,     /*  5 : probe rsp */
+    0xff,   /*  6 : reserved */
+    0xff,   /*  7 : reserved */
+    12,     /*  8 : beacon */
+    4,      /*  9 : ATIM */
+    0xff,   /* 10 : disasoc */
+    6,      /* 11 : auth */
+    0xff,   /* 12 : deauth */
+    4,      /* 13 : action */
+    0xff,   /* 14 : reserved */
+    0xff,   /* 15 : reserved */
+};
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfFindElement               */
+/*      Find a specific element in management frame                     */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : management frame buffer                                   */
+/*      eid : target element id                                         */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      byte offset of target element                                   */
+/*      or 0xffff if not found                                          */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfFindElement(zdev_t* dev, zbuf_t* buf, u8_t eid)
+{
+    u8_t subType;
+    u16_t offset;
+    u16_t bufLen;
+    u16_t elen;
+    u8_t id, HTEid=0;
+    u8_t oui[4] = {0x00, 0x50, 0xf2, 0x01};
+    u8_t oui11n[3] = {0x00,0x90,0x4C};
+    u8_t HTType = 0;
+
+    /* Get offset of first element */
+    subType = (zmw_rx_buf_readb(dev, buf, 0) >> 4);
+    if ((offset = zgElementOffsetTable[subType]) == 0xff)
+    {
+        zm_assert(0);
+    }
+
+    /* Plus wlan header */
+    offset += 24;
+
+    // jhlee HT 0
+
+    if ((eid == ZM_WLAN_EID_HT_CAPABILITY) ||
+        (eid == ZM_WLAN_EID_EXTENDED_HT_CAPABILITY))
+    {
+        HTEid = eid;
+        eid = ZM_WLAN_EID_WPA_IE;
+        HTType = 1;
+    }
+
+
+    bufLen = zfwBufGetSize(dev, buf);
+    /* Search loop */
+    while ((offset+2)<bufLen)                   // including element ID and length (2bytes)
+    {
+        /* Search target element */
+        if ((id = zmw_rx_buf_readb(dev, buf, offset)) == eid)
+        {
+            /* Bingo */
+            if ((elen = zmw_rx_buf_readb(dev, buf, offset+1))>(bufLen - offset))
+            {
+                /* Element length error */
+                return 0xffff;
+            }
+
+            if ( elen == 0 && eid != ZM_WLAN_EID_SSID)
+            {
+                /* Element length error */
+                return 0xffff;
+            }
+
+            if ( eid == ZM_WLAN_EID_WPA_IE )
+            {
+                /* avoid sta to be thought use 11n when find a WPA_IE */
+                if ( (HTType == 0) && zfRxBufferEqualToStr(dev, buf, oui, offset+2, 4) )
+                {
+                    return offset;
+                }
+
+                // jhlee HT 0
+                // CWYang(+)
+
+                if ((HTType == 1) && ( zfRxBufferEqualToStr(dev, buf, oui11n, offset+2, 3) ))
+                {
+                    if ( zmw_rx_buf_readb(dev, buf, offset+5) == HTEid )
+                    {
+                        return offset + 5;
+                    }
+                }
+
+            }
+            else
+            {
+                return offset;
+            }
+        }
+        /* Advance to next element */
+        #if 1
+        elen = zmw_rx_buf_readb(dev, buf, offset+1);
+        #else
+        if ((elen = zmw_rx_buf_readb(dev, buf, offset+1)) == 0)
+        {
+            return 0xffff;
+        }
+        #endif
+
+        offset += (elen+2);
+    }
+    return 0xffff;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfFindWifiElement           */
+/*      Find a specific Wifi element in management frame                */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : management frame buffer                                   */
+/*      type : OUI type                                                 */
+/*      subType : OUI subtype                                           */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      byte offset of target element                                   */
+/*      or 0xffff if not found                                          */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2006.1      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfFindWifiElement(zdev_t* dev, zbuf_t* buf, u8_t type, u8_t subtype)
+{
+    u8_t subType;
+    u16_t offset;
+    u16_t bufLen;
+    u16_t elen;
+    u8_t id;
+    u8_t tmp;
+
+    /* Get offset of first element */
+    subType = (zmw_rx_buf_readb(dev, buf, 0) >> 4);
+
+    if ((offset = zgElementOffsetTable[subType]) == 0xff)
+    {
+        zm_assert(0);
+    }
+
+    /* Plus wlan header */
+    offset += 24;
+
+    bufLen = zfwBufGetSize(dev, buf);
+    /* Search loop */
+    while ((offset+2)<bufLen)                   // including element ID and length (2bytes)
+    {
+        /* Search target element */
+        if ((id = zmw_rx_buf_readb(dev, buf, offset)) == ZM_WLAN_EID_WIFI_IE)
+        {
+            /* Bingo */
+            if ((elen = zmw_rx_buf_readb(dev, buf, offset+1))>(bufLen - offset))
+            {
+                /* Element length error */
+                return 0xffff;
+            }
+
+            if ( elen == 0 )
+            {
+                return 0xffff;
+            }
+
+            if (((tmp = zmw_rx_buf_readb(dev, buf, offset+2)) == 0x00)
+                    && ((tmp = zmw_rx_buf_readb(dev, buf, offset+3)) == 0x50)
+                    && ((tmp = zmw_rx_buf_readb(dev, buf, offset+4)) == 0xF2)
+                    && ((tmp = zmw_rx_buf_readb(dev, buf, offset+5)) == type))
+
+            {
+                if ( subtype != 0xff )
+                {
+                    if ( (tmp = zmw_rx_buf_readb(dev, buf, offset+6)) == subtype  )
+                    {
+                        return offset;
+                    }
+                }
+                else
+                {
+                    return offset;
+                }
+            }
+        }
+        /* Advance to next element */
+        if ((elen = zmw_rx_buf_readb(dev, buf, offset+1)) == 0)
+        {
+            return 0xffff;
+        }
+        offset += (elen+2);
+    }
+    return 0xffff;
+}
+
+u16_t zfRemoveElement(zdev_t* dev, u8_t* buf, u16_t size, u8_t eid)
+{
+    u16_t offset = 0;
+    u16_t elen;
+    u8_t  HTEid = 0;
+    u8_t  oui[4] = {0x00, 0x50, 0xf2, 0x01};
+    u8_t  oui11n[3] = {0x00,0x90,0x4C};
+    u8_t  HTType = 0;
+
+    if ((eid == ZM_WLAN_EID_HT_CAPABILITY) ||
+        (eid == ZM_WLAN_EID_EXTENDED_HT_CAPABILITY))
+    {
+        HTEid = eid;
+        eid = ZM_WLAN_EID_WPA_IE;
+        HTType = 1;
+    }
+
+    while (offset < size)
+    {
+        elen = *(buf+offset+1);
+
+        if (*(buf+offset) == eid)
+        {
+            if ( eid == ZM_WLAN_EID_WPA_IE )
+            {
+                if ( (HTType == 0)
+                     && (*(buf+offset+2) == oui[0])
+                     && (*(buf+offset+3) == oui[1])
+                     && (*(buf+offset+4) == oui[2])
+                     && (*(buf+offset+5) == oui[3]) )
+                {
+                    zfMemoryMove(buf+offset, buf+offset+elen+2, size-offset-elen-2);
+                    return (size-elen-2);
+                }
+
+                if ( (HTType == 1)
+                    && (*(buf+offset+2) == oui11n[0])
+                    && (*(buf+offset+3) == oui11n[1])
+                    && (*(buf+offset+4) == oui11n[2])
+                    && (*(buf+offset+5) == HTEid) )
+                {
+                    zfMemoryMove(buf+offset, buf+offset+elen+2, size-offset-elen-2);
+                    return (size-elen-2);
+                }
+            }
+            else
+            {
+                zfMemoryMove(buf+offset, buf+offset+elen+2, size-offset-elen-2);
+                return (size-elen-2);
+            }
+        }
+
+        offset += (elen+2);
+    }
+
+    return size;
+}
+
+u16_t zfUpdateElement(zdev_t* dev, u8_t* buf, u16_t size, u8_t* updateeid)
+{
+    u16_t offset = 0;
+    u16_t elen;
+
+    while (offset < size) {
+        elen = *(buf+offset+1);
+
+        if (*(buf+offset) == updateeid[0]) {
+            if (updateeid[1] <= elen) {
+                zfMemoryMove(buf+offset, updateeid, updateeid[1]+2);
+                zfMemoryMove(buf+offset+updateeid[1]+2, buf+offset+elen+2, size-offset-elen-2);
+
+                return size-(elen-updateeid[1]);
+            } else {
+                zfMemoryMove(buf+offset+updateeid[1]+2, buf+offset+elen+2, size-offset-elen-2);
+                zfMemoryMove(buf+offset, updateeid, updateeid[1]+2);
+
+                return size+(updateeid[1]-elen);
+            }
+        }
+
+        offset += (elen+2);
+    }
+
+    return size;
+}
+
+u16_t zfFindSuperGElement(zdev_t* dev, zbuf_t* buf, u8_t type)
+{
+    u8_t subType;
+    u16_t offset;
+    u16_t bufLen;
+    u16_t elen;
+    u8_t id;
+    u8_t super_feature;
+    u8_t ouiSuperG[6] = {0x00,0x03,0x7f,0x01, 0x01, 0x00};
+
+    zmw_get_wlan_dev(dev);
+
+    /* Get offset of first element */
+    subType = (zmw_rx_buf_readb(dev, buf, 0) >> 4);
+    if ((offset = zgElementOffsetTable[subType]) == 0xff)
+    {
+        zm_assert(0);
+    }
+
+    /* Plus wlan header */
+    offset += 24;
+
+    bufLen = zfwBufGetSize(dev, buf);
+    /* Search loop */
+    while ((offset+2)<bufLen)                   // including element ID and length (2bytes)
+    {
+        /* Search target element */
+        if ((id = zmw_rx_buf_readb(dev, buf, offset)) == ZM_WLAN_EID_VENDOR_PRIVATE)
+        {
+            /* Bingo */
+            if ((elen = zmw_rx_buf_readb(dev, buf, offset+1))>(bufLen - offset))
+            {
+                /* Element length error */
+                return 0xffff;
+            }
+
+            if ( elen == 0 )
+            {
+                return 0xffff;
+            }
+
+            if (zfRxBufferEqualToStr(dev, buf, ouiSuperG, offset+2, 6) && ( zmw_rx_buf_readb(dev, buf, offset+1) >= 6))
+            {
+                /* super_feature 0:useFastFrame, 1:useCompression, 2:useTurboPrime */
+                super_feature= zmw_rx_buf_readb(dev, buf, offset+8);
+                if ((super_feature & 0x01) || (super_feature & 0x02) || (super_feature & 0x04))
+                {
+                    return offset;
+                }
+            }
+        }
+        /* Advance to next element */
+        #if 1
+        elen = zmw_rx_buf_readb(dev, buf, offset+1);
+        #else
+        if ((elen = zmw_rx_buf_readb(dev, buf, offset+1)) == 0)
+        {
+            return 0xffff;
+        }
+        #endif
+
+        offset += (elen+2);
+    }
+    return 0xffff;
+}
+
+u16_t zfFindXRElement(zdev_t* dev, zbuf_t* buf, u8_t type)
+{
+    u8_t subType;
+    u16_t offset;
+    u16_t bufLen;
+    u16_t elen;
+    u8_t id;
+    u8_t ouixr[6] = {0x00,0x03,0x7f,0x03, 0x01, 0x00};
+
+    zmw_get_wlan_dev(dev);
+
+    /* Get offset of first element */
+    subType = (zmw_rx_buf_readb(dev, buf, 0) >> 4);
+    if ((offset = zgElementOffsetTable[subType]) == 0xff)
+    {
+        zm_assert(0);
+    }
+
+    /* Plus wlan header */
+    offset += 24;
+
+    bufLen = zfwBufGetSize(dev, buf);
+    /* Search loop */
+    while ((offset+2)<bufLen)                   // including element ID and length (2bytes)
+    {
+        /* Search target element */
+        if ((id = zmw_rx_buf_readb(dev, buf, offset)) == ZM_WLAN_EID_VENDOR_PRIVATE)
+        {
+            /* Bingo */
+            if ((elen = zmw_rx_buf_readb(dev, buf, offset+1))>(bufLen - offset))
+            {
+                /* Element length error */
+                return 0xffff;
+            }
+
+            if ( elen == 0 )
+            {
+                return 0xffff;
+            }
+
+            if (zfRxBufferEqualToStr(dev, buf, ouixr, offset+2, 6) && ( zmw_rx_buf_readb(dev, buf, offset+1) >= 6))
+            {
+                return offset;
+            }
+        }
+        /* Advance to next element */
+        #if 1
+        elen = zmw_rx_buf_readb(dev, buf, offset+1);
+        #else
+        if ((elen = zmw_rx_buf_readb(dev, buf, offset+1)) == 0)
+        {
+            return 0xffff;
+        }
+        #endif
+
+        offset += (elen+2);
+    }
+    return 0xffff;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfMmAddIeSupportRate        */
+/*      Add information element Support Rate to buffer.                 */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*      eid : element ID                                                */
+/*      rateSet :  CCK or OFDM                                          */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfMmAddIeSupportRate(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t eid, u8_t rateSet)
+{
+    u8_t len = 0;
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    //if ( (rateSet == ZM_RATE_SET_OFDM)&&((wd->gRate & 0xff) == 0) )
+    //{
+    //    return offset;
+    //}
+
+    /* Information : Support Rate */
+    if ( rateSet == ZM_RATE_SET_CCK )
+    {
+        for (i=0; i<4; i++)
+        {
+            if ((wd->bRate & (0x1<<i)) == (0x1<<i))
+            //if ((0xf & (0x1<<i)) == (0x1<<i))
+            {
+                zmw_tx_buf_writeb(dev, buf, offset+len+2,
+                                     zg11bRateTbl[i]+((wd->bRateBasic & (0x1<<i))<<(7-i)));
+                len++;
+            }
+        }
+    }
+    else if ( rateSet == ZM_RATE_SET_OFDM )
+    {
+        for (i=0; i<8; i++)
+        {
+            if ((wd->gRate & (0x1<<i)) == (0x1<<i))
+            //if ((0xff & (0x1<<i)) == (0x1<<i))
+            {
+                zmw_tx_buf_writeb(dev, buf, offset+len+2,
+                                     zg11gRateTbl[i]+((wd->gRateBasic & (0x1<<i))<<(7-i)));
+                len++;
+            }
+        }
+    }
+
+    if (len > 0)
+    {
+        /* Element ID */
+        zmw_tx_buf_writeb(dev, buf, offset, eid);
+
+        /* Element Length */
+        zmw_tx_buf_writeb(dev, buf, offset+1, len);
+
+        /* Return value */
+        offset += (2+len);
+    }
+
+    return offset;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfMmAddIeDs                 */
+/*      Add information element DS to buffer.                           */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfMmAddIeDs(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* Element ID */
+    zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_DS);
+
+    /* Element Length */
+    zmw_tx_buf_writeb(dev, buf, offset++, 1);
+
+    /* Information : DS */
+    zmw_tx_buf_writeb(dev, buf, offset++,
+                         zfChFreqToNum(wd->frequency, NULL));
+
+    return offset;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfMmAddIeErp                */
+/*      Add information element ERP to buffer.                          */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfMmAddIeErp(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* Element ID */
+    zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_ERP);
+
+    /* Element Length */
+    zmw_tx_buf_writeb(dev, buf, offset++, 1);
+
+    /* Information : ERP */
+    zmw_tx_buf_writeb(dev, buf, offset++, wd->erpElement);
+
+    return offset;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfMmAddIeWpa                */
+/*      Add information element WPA to buffer.                          */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Yuan-Gu Wei         ZyDAS Technology Corporation    2006.2      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfMmAddIeWpa(zdev_t* dev, zbuf_t* buf, u16_t offset, u16_t apId)
+{
+    //struct zsWlanDev* wd = (struct zsWlanDev*) zmw_wlan_dev(dev);
+    int i;
+
+    zmw_get_wlan_dev(dev);
+
+    /* Element ID */
+    //zmw_inttx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_WPA_IE);
+
+    /* Element Length */
+    //zmw_inttx_buf_writeb(dev, buf, offset++, wd->ap.wpaLen);
+    for(i = 0; i < wd->ap.wpaLen[apId]; i++)
+    {
+        /* Information : WPA */
+        zmw_tx_buf_writeb(dev, buf, offset++, wd->ap.wpaIe[apId][i]);
+    }
+
+    return offset;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfMmAddHTCapability         */
+/*      Add HT Capability Infomation to buffer.                         */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Chao-Wen Yang     ZyDAS Technology Corporation       2006.06    */
+/*                                                                      */
+/************************************************************************/
+u16_t zfMmAddHTCapability(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+    u8_t OUI[3] = {0x0,0x90,0x4C};
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    /* Prob ID */
+    zmw_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_WPA_IE);
+
+    if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        /* Element Length */
+        zmw_buf_writeb(dev, buf, offset++, wd->ap.HTCap.Data.Length + 4);
+
+        /* OUI Data */
+        for (i = 0; i < 3; i++)
+        {
+            zmw_buf_writeb(dev, buf, offset++, OUI[i]);
+        }
+
+        /* Element Type ID */
+        zmw_buf_writeb(dev, buf, offset++, wd->ap.HTCap.Data.ElementID);
+
+        /* HT Capability Data */
+        for (i = 0; i < 26; i++)
+        {
+            zmw_buf_writeb(dev, buf, offset++, wd->ap.HTCap.Byte[i+2]);
+        }
+    }
+    else
+    {
+        /* Element Length */
+        zmw_buf_writeb(dev, buf, offset++, wd->sta.HTCap.Data.Length + 4);
+
+        /* OUI Data */
+        for (i = 0; i < 3; i++)
+        {
+            zmw_buf_writeb(dev, buf, offset++, OUI[i]);
+        }
+
+        /* Element Type ID */
+        zmw_buf_writeb(dev, buf, offset++, wd->sta.HTCap.Data.ElementID);
+
+        /* HT Capability Data */
+        for (i = 0; i < 26; i++)
+        {
+            zmw_buf_writeb(dev, buf, offset++, wd->sta.HTCap.Byte[i+2]);
+        }
+    }
+
+    return offset;
+}
+
+
+u16_t zfMmAddPreNHTCapability(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+    //u8_t OUI[3] = {0x0,0x90,0x4C};
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    /* Prob ID */
+    zmw_buf_writeb(dev, buf, offset++, ZM_WLAN_PREN2_EID_HTCAPABILITY);
+
+    if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        /* Element Length */
+        zmw_buf_writeb(dev, buf, offset++, wd->ap.HTCap.Data.Length);
+
+        /* HT Capability Data */
+        for (i = 0; i < 26; i++)
+        {
+            zmw_buf_writeb(dev, buf, offset++, wd->ap.HTCap.Byte[i+2]);
+        }
+    }
+    else
+    {
+        /* Element Length */
+        zmw_buf_writeb(dev, buf, offset++, wd->sta.HTCap.Data.Length);
+
+        /* HT Capability Data */
+        for (i = 0; i < 26; i++)
+        {
+            zmw_buf_writeb(dev, buf, offset++, wd->sta.HTCap.Byte[i+2]);
+        }
+    }
+
+    return offset;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                zfMmAddExtendedHTCapability   */
+/*      Add Extended HT Capability Infomation to buffer.                */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Chao-Wen Yang     ZyDAS Technology Corporation       2006.06    */
+/*                                                                      */
+/************************************************************************/
+u16_t zfMmAddExtendedHTCapability(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+    u8_t OUI[3] = {0x0,0x90,0x4C};
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    /* Prob ID */
+    zmw_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_WPA_IE);
+
+    if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        /* Element Length */
+        zmw_buf_writeb(dev, buf, offset++, wd->ap.ExtHTCap.Data.Length + 4);
+
+        /* OUI Data */
+        for (i = 0; i < 3; i++)
+        {
+            zmw_buf_writeb(dev, buf, offset++, OUI[i]);
+        }
+
+        /* Element Type ID */
+        zmw_buf_writeb(dev, buf, offset++, wd->ap.ExtHTCap.Data.ElementID);
+
+        /* HT Capability Data */
+        for (i = 0; i < 22; i++)
+        {
+            zmw_buf_writeb(dev, buf, offset++, wd->ap.ExtHTCap.Byte[i+2]);
+        }
+    }
+    else
+    {
+        /* Element Length */
+        zmw_buf_writeb(dev, buf, offset++, wd->sta.ExtHTCap.Data.Length + 4);
+
+        /* OUI Data */
+        for (i = 0; i < 3; i++)
+        {
+            zmw_buf_writeb(dev, buf, offset++, OUI[i]);
+        }
+
+        /* Element Type ID */
+        zmw_buf_writeb(dev, buf, offset++, wd->sta.ExtHTCap.Data.ElementID);
+
+        /* HT Capability Data */
+        for (i = 0; i < 22; i++)
+        {
+            zmw_buf_writeb(dev, buf, offset++, wd->sta.ExtHTCap.Byte[i+2]);
+        }
+    }
+
+    return offset;
+}
+
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfSendMmFrame               */
+/*      Send management frame.                                          */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      frameType : management frame type                               */
+/*      dst : destination MAC address                                   */
+/*      p1 : parameter 1                                                */
+/*      p2 : parameter 2                                                */
+/*      p3 : parameter 3                                                */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+/* probe req : p1=> bWithSSID, p2=>R, p3=>R                                      */
+/* probe rsp : p1=>R, p2=>R, p3=>VAP ID(AP)                             */
+/* deauth : p1=>Reason Code, p2=>R, p3=>VAP ID(AP)                      */
+/* Disasoc : p1=>Reason Code, p2=>R, p3=>VAP ID(AP)                     */
+/* ATIM : p1=>R, p2=>R, p3=>R                                           */
+/* (re)asoc rsp : p1=>Status Code, p2=>AID, p3=>VAP ID(AP)              */
+/* asoc req : p1=>R, p2=>R, p3=>R                                       */
+/* reasoc req : p1=>AP MAC[0], p2=>AP MAC[1], p3=>AP MAC[2]             */
+/* auth : p1=>low=Algorithm, high=Transaction, p2=>Status, p3=>VAP ID   */
+void zfSendMmFrame(zdev_t* dev, u8_t frameType, u16_t* dst,
+                   u32_t p1, u32_t p2, u32_t p3)
+{
+    zbuf_t* buf;
+    //u16_t addrTblSize;
+    //struct zsAddrTbl addrTbl;
+    u16_t offset = 0;
+    u16_t hlen = 32;
+    u16_t header[(24+25+1)/2];
+    u16_t vap = 0;
+    u16_t i;
+    u8_t encrypt = 0;
+    u16_t aid;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zm_msg2_mm(ZM_LV_2, "Send mm frame, type=", frameType);
+    /* TBD : Maximum size of managment frame */
+    if ((buf = zfwBufAllocate(dev, 1024)) == NULL)
+    {
+        zm_msg0_mm(ZM_LV_0, "Alloc mm buf Fail!");
+        return;
+    }
+
+    //Reserve room for wlan header
+    offset = hlen;
+
+    switch (frameType)
+    {
+        case ZM_WLAN_FRAME_TYPE_PROBEREQ :
+            offset = zfSendProbeReq(dev, buf, offset, (u8_t) p1);
+            break;
+
+        case ZM_WLAN_FRAME_TYPE_PROBERSP :
+            zm_msg0_mm(ZM_LV_3, "probe rsp");
+            /* 24-31 Time Stamp : hardware WON'T fill this field */
+            zmw_tx_buf_writeh(dev, buf, offset, 0);
+            zmw_tx_buf_writeh(dev, buf, offset+2, 0);
+            zmw_tx_buf_writeh(dev, buf, offset+4, 0);
+            zmw_tx_buf_writeh(dev, buf, offset+6, 0);
+            offset+=8;
+
+            /* Beacon Interval */
+            zmw_tx_buf_writeh(dev, buf, offset, wd->beaconInterval);
+            offset+=2;
+
+            if (wd->wlanMode == ZM_MODE_AP)
+            {
+                vap = (u16_t) p3;
+                /* Capability */
+                zmw_tx_buf_writeh(dev, buf, offset, wd->ap.capab[vap]);
+                offset+=2;
+                /* SSID */
+                offset = zfApAddIeSsid(dev, buf, offset, vap);
+            }
+            else
+            {
+                /* Capability */
+                zmw_tx_buf_writeb(dev, buf, offset++, wd->sta.capability[0]);
+                zmw_tx_buf_writeb(dev, buf, offset++, wd->sta.capability[1]);
+                /* SSID */
+                offset = zfStaAddIeSsid(dev, buf, offset);
+            }
+
+            /* Support Rate */
+            if ( wd->frequency < 3000 )
+            {
+            offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                          ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_CCK);
+            }
+            else
+            {
+                offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                          ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_OFDM);
+            }
+
+            /* DS parameter set */
+            offset = zfMmAddIeDs(dev, buf, offset);
+
+            /* TODO ¡G IBSS */
+            if ( wd->wlanMode == ZM_MODE_IBSS )
+            {
+                offset = zfStaAddIeIbss(dev, buf, offset);
+
+                if (wd->frequency < 3000)
+                {
+                    if( wd->wfc.bIbssGMode
+                        && (wd->supportMode & (ZM_WIRELESS_MODE_24_54|ZM_WIRELESS_MODE_24_N)) )    // Only accompany with enabling a mode .
+                    {
+                        /* ERP Information */
+                        wd->erpElement = 0;
+                        offset = zfMmAddIeErp(dev, buf, offset);
+
+                        /* Enable G Mode */
+                        /* Extended Supported Rates */
+                   	    offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                ZM_WLAN_EID_EXTENDED_RATE, ZM_RATE_SET_OFDM);
+            	    }
+        	    }
+            }
+
+
+            if ((wd->wlanMode == ZM_MODE_AP)
+                 && (wd->ap.wlanType[vap] != ZM_WLAN_TYPE_PURE_B))
+            {
+                /* ERP Information */
+                offset = zfMmAddIeErp(dev, buf, offset);
+
+                /* Extended Supported Rates */
+		if ( wd->frequency < 3000 )
+                {
+                offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                   ZM_WLAN_EID_EXTENDED_RATE, ZM_RATE_SET_OFDM);
+            }
+            }
+
+            /* ERP Information */
+            //offset = zfMmAddIeErp(dev, buf, offset);
+
+            /* Extended Supported Rates */
+            //offset = zfMmAddIeSupportRate(dev, buf, offset,
+            //                              ZM_WLAN_EID_EXTENDED_RATE, ZM_RATE_SET_OFDM);
+
+            /* TODO : RSN */
+            if (wd->wlanMode == ZM_MODE_AP && wd->ap.wpaSupport[vap] == 1)
+            {
+                offset = zfMmAddIeWpa(dev, buf, offset, vap);
+            }
+            else if ( wd->wlanMode == ZM_MODE_IBSS && wd->sta.authMode == ZM_AUTH_MODE_WPA2PSK)
+            {
+                offset = zfwStaAddIeWpaRsn(dev, buf, offset, ZM_WLAN_FRAME_TYPE_AUTH);
+            }
+
+            /* WME Parameters */
+            if (wd->wlanMode == ZM_MODE_AP)
+            {
+                if (wd->ap.qosMode == 1)
+                {
+                    offset = zfApAddIeWmePara(dev, buf, offset, vap);
+                }
+            }
+
+            if ( wd->wlanMode != ZM_MODE_IBSS )
+            {
+            // jhlee HT 0
+            //CWYang(+)
+                /* TODO : Need to check if it is ok */
+            /* HT Capabilities Info */
+            offset = zfMmAddHTCapability(dev, buf, offset);
+            //CWYang(+)
+            /* Extended HT Capabilities Info */
+            offset = zfMmAddExtendedHTCapability(dev, buf, offset);
+            }
+
+            if ( wd->sta.ibssAdditionalIESize )
+                offset = zfStaAddIbssAdditionalIE(dev, buf, offset);
+            break;
+
+        case ZM_WLAN_FRAME_TYPE_AUTH :
+            if (p1 == 0x30001)
+            {
+                hlen += 4;
+                offset += 4;        // for reserving wep header
+                encrypt = 1;
+            }
+
+            /* Algotrithm Number */
+            zmw_tx_buf_writeh(dev, buf, offset, (u16_t)(p1&0xffff));
+            offset+=2;
+
+            /* Transaction Number */
+            zmw_tx_buf_writeh(dev, buf, offset, (u16_t)(p1>>16));
+            offset+=2;
+
+            /* Status Code */
+            zmw_tx_buf_writeh(dev, buf, offset, (u16_t)p2);
+            offset+=2;
+
+            if (wd->wlanMode == ZM_MODE_AP)
+            {
+                vap = (u16_t) p3;
+            }
+
+            /* Challenge Text => share-2 or share-3 */
+            if (p1 == 0x20001)
+            {
+                if (p2 == 0) //Status == success
+                {
+                    zmw_buf_writeh(dev, buf, offset, 0x8010);
+                    offset+=2;
+                    /* share-2 : AP generate challenge text */
+                    for (i=0; i<128; i++)
+                    {
+                        wd->ap.challengeText[i] = (u8_t)zfGetRandomNumber(dev, 0);
+                    }
+                    zfCopyToIntTxBuffer(dev, buf, wd->ap.challengeText, offset, 128);
+                    offset += 128;
+                }
+            }
+            else if (p1 == 0x30001)
+            {
+                /* share-3 : STA return challenge Text */
+                zfCopyToIntTxBuffer(dev, buf, wd->sta.challengeText, offset, wd->sta.challengeText[1]+2);
+                offset += (wd->sta.challengeText[1]+2);
+            }
+
+            break;
+
+        case ZM_WLAN_FRAME_TYPE_ASOCREQ :
+        case ZM_WLAN_FRAME_TYPE_REASOCREQ :
+            /* Capability */
+            zmw_tx_buf_writeb(dev, buf, offset++, wd->sta.capability[0]);
+            zmw_tx_buf_writeb(dev, buf, offset++, wd->sta.capability[1]);
+
+            /* Listen Interval */
+            zmw_tx_buf_writeh(dev, buf, offset, 0x0005);
+            offset+=2;
+
+            /* Reassocaited Request : Current AP address */
+            if (frameType == ZM_WLAN_FRAME_TYPE_REASOCREQ)
+            {
+            zmw_tx_buf_writeh(dev, buf, offset, wd->sta.bssid[0]);
+                offset+=2;
+            zmw_tx_buf_writeh(dev, buf, offset, wd->sta.bssid[1]);
+                offset+=2;
+            zmw_tx_buf_writeh(dev, buf, offset, wd->sta.bssid[2]);
+                offset+=2;
+            }
+
+            /* SSID */
+            offset = zfStaAddIeSsid(dev, buf, offset);
+
+
+            if ( wd->sta.currentFrequency < 3000 )
+            {
+                /* Support Rate */
+                offset = zfMmAddIeSupportRate(dev, buf, offset, ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_CCK);
+            }
+            else
+            {
+                /* Support Rate */
+                offset = zfMmAddIeSupportRate(dev, buf, offset, ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_OFDM);
+            }
+
+            if ((wd->sta.capability[1] & ZM_BIT_0) == 1)
+            {   //spectrum managment flag enable
+                offset = zfStaAddIePowerCap(dev, buf, offset);
+                offset = zfStaAddIeSupportCh(dev, buf, offset);
+            }
+
+            if (wd->sta.currentFrequency < 3000)
+            {
+                /* Extended Supported Rates */
+                if (wd->supportMode & (ZM_WIRELESS_MODE_24_54|ZM_WIRELESS_MODE_24_N))
+                {
+                    offset = zfMmAddIeSupportRate(dev, buf, offset, ZM_WLAN_EID_EXTENDED_RATE, ZM_RATE_SET_OFDM);
+                }
+            }
+
+
+            //offset = zfStaAddIeWpaRsn(dev, buf, offset, frameType);
+            //Move to wrapper function, for OS difference--CWYang(m)
+            //for windows wrapper, zfwStaAddIeWpaRsn() should be below:
+            //u16_t zfwStaAddIeWpaRsn(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t frameType)
+            //{
+            //    return zfStaAddIeWpaRsn(dev, buf, offset, frameType);
+            //}
+            offset = zfwStaAddIeWpaRsn(dev, buf, offset, frameType);
+
+#ifdef ZM_ENABLE_CENC
+            /* CENC */
+            //if (wd->sta.encryMode == ZM_CENC)
+            offset = zfStaAddIeCenc(dev, buf, offset);
+#endif //ZM_ENABLE_CENC
+            if (((wd->sta.wmeEnabled & ZM_STA_WME_ENABLE_BIT) != 0) //WME enabled
+              && ((wd->sta.apWmeCapability & 0x1) != 0)) //WME AP
+            {
+                if (((wd->sta.apWmeCapability & 0x80) != 0) //UAPSD AP
+                 && ((wd->sta.wmeEnabled & ZM_STA_UAPSD_ENABLE_BIT) != 0)) //UAPSD enabled
+                {
+                    offset = zfStaAddIeWmeInfo(dev, buf, offset, wd->sta.wmeQosInfo);
+                }
+                else
+                {
+                    offset = zfStaAddIeWmeInfo(dev, buf, offset, 0);
+                }
+            }
+            // jhlee HT 0
+            //CWYang(+)
+            if (wd->sta.EnableHT != 0)
+            {
+                #ifndef ZM_DISABLE_AMSDU8K_SUPPORT
+                    //Support 8K A-MSDU
+                    if (wd->sta.wepStatus == ZM_ENCRYPTION_WEP_DISABLED)
+                    {
+                        wd->sta.HTCap.Data.HtCapInfo |= HTCAP_MaxAMSDULength;
+                    }
+                    else
+                    {
+                        wd->sta.HTCap.Data.HtCapInfo &= (~HTCAP_MaxAMSDULength);
+                    }
+                #else
+                    //Support 4K A-MSDU
+                    wd->sta.HTCap.Data.HtCapInfo &= (~HTCAP_MaxAMSDULength);
+                #endif
+
+                /* HT Capabilities Info */
+                if (wd->BandWidth40 == 1) {
+                    wd->sta.HTCap.Data.HtCapInfo |= HTCAP_SupChannelWidthSet;
+                }
+                else {
+                    wd->sta.HTCap.Data.HtCapInfo &= ~HTCAP_SupChannelWidthSet;
+                    //wd->sta.HTCap.Data.HtCapInfo |= HTCAP_SupChannelWidthSet;
+                }
+
+                wd->sta.HTCap.Data.AMPDUParam &= ~HTCAP_MaxRxAMPDU3;
+                wd->sta.HTCap.Data.AMPDUParam |= HTCAP_MaxRxAMPDU3;
+                wd->sta.HTCap.Data.MCSSet[1] = 0xFF; // MCS 8 ~ 15
+                offset = zfMmAddHTCapability(dev, buf, offset);
+                offset = zfMmAddPreNHTCapability(dev, buf, offset);
+                //CWYang(+)
+                /* Extended HT Capabilities Info */
+                //offset = zfMmAddExtendedHTCapability(dev, buf, offset);
+            }
+
+
+            //Store asoc request frame body, for VISTA only
+            wd->sta.asocReqFrameBodySize = ((offset - hlen) >
+                    ZM_CACHED_FRAMEBODY_SIZE)?
+                    ZM_CACHED_FRAMEBODY_SIZE:(offset - hlen);
+            for (i=0; i<wd->sta.asocReqFrameBodySize; i++)
+            {
+                wd->sta.asocReqFrameBody[i] = zmw_tx_buf_readb(dev, buf, i + hlen);
+            }
+            break;
+
+        case ZM_WLAN_FRAME_TYPE_ASOCRSP :
+        case ZM_WLAN_FRAME_TYPE_REASOCRSP :
+            vap = (u16_t) p3;
+
+            /* Capability */
+            zmw_tx_buf_writeh(dev, buf, offset, wd->ap.capab[vap]);
+            offset+=2;
+
+            /* Status Code */
+            zmw_tx_buf_writeh(dev, buf, offset, (u16_t)p1);
+            offset+=2;
+
+            /* AID */
+            zmw_tx_buf_writeh(dev, buf, offset, (u16_t)(p2|0xc000));
+            offset+=2;
+
+
+            if ( wd->frequency < 3000 )
+            {
+            /* Support Rate */
+            offset = zfMmAddIeSupportRate(dev, buf, offset, ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_CCK);
+
+            /* Extended Supported Rates */
+            offset = zfMmAddIeSupportRate(dev, buf, offset, ZM_WLAN_EID_EXTENDED_RATE, ZM_RATE_SET_OFDM);
+            }
+            else
+            {
+                /* Support Rate */
+                offset = zfMmAddIeSupportRate(dev, buf, offset, ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_OFDM);
+            }
+
+
+
+            /* WME Parameters */
+            if (wd->wlanMode == ZM_MODE_AP)
+            {
+                /* TODO : if WME STA then send WME parameter element */
+                if (wd->ap.qosMode == 1)
+                {
+                    offset = zfApAddIeWmePara(dev, buf, offset, vap);
+                }
+            }
+            // jhlee HT 0
+            //CWYang(+)
+            /* HT Capabilities Info */
+            offset = zfMmAddHTCapability(dev, buf, offset);
+            //CWYang(+)
+            /* Extended HT Capabilities Info */
+            offset = zfMmAddExtendedHTCapability(dev, buf, offset);
+            break;
+
+        case ZM_WLAN_FRAME_TYPE_ATIM :
+            /* NULL frame */
+            /* TODO : add two dumb bytes temporarily */
+            offset += 2;
+            break;
+
+        case ZM_WLAN_FRAME_TYPE_QOS_NULL :
+            zmw_buf_writeh(dev, buf, offset, 0x0010);
+            offset += 2;
+            break;
+
+        case ZM_WLAN_DATA_FRAME :
+            break;
+
+        case ZM_WLAN_FRAME_TYPE_DISASOC :
+        case ZM_WLAN_FRAME_TYPE_DEAUTH :
+            if (wd->wlanMode == ZM_MODE_AP)
+            {
+              vap = (u16_t) p3;
+
+              if ((aid = zfApFindSta(dev, dst)) != 0xffff)
+              {
+                  zmw_enter_critical_section(dev);
+                  /* Clear STA table */
+                  wd->ap.staTable[aid].valid = 0;
+
+                  zmw_leave_critical_section(dev);
+
+                  if (wd->zfcbDisAsocNotify != NULL)
+                  {
+                      wd->zfcbDisAsocNotify(dev, (u8_t*)dst, vap);
+                  }
+              }
+            }
+            /* Reason Code */
+            zmw_tx_buf_writeh(dev, buf, offset, (u16_t)p1);
+            offset+=2;
+            break;
+    }
+
+    zfwBufSetSize(dev, buf, offset);
+
+    zm_msg2_mm(ZM_LV_2, "management frame body size=", offset-hlen);
+
+    //Copy wlan header
+    zfTxGenMmHeader(dev, frameType, dst, header, offset-hlen, buf, vap, encrypt);
+    for (i=0; i<(hlen>>1); i++)
+    {
+        zmw_tx_buf_writeh(dev, buf, i*2, header[i]);
+    }
+
+    /* Get buffer DMA address */
+    //if ((addrTblSize = zfwBufMapDma(dev, buf, &addrTbl)) == 0)
+    //if ((addrTblSize = zfwMapTxDma(dev, buf, &addrTbl)) == 0)
+    //{
+    //    goto zlError;
+    //}
+
+    zm_msg2_mm(ZM_LV_2, "offset=", offset);
+    zm_msg2_mm(ZM_LV_2, "hlen=", hlen);
+    //zm_msg2_mm(ZM_LV_2, "addrTblSize=", addrTblSize);
+    //zm_msg2_mm(ZM_LV_2, "addrTbl.len[0]=", addrTbl.len[0]);
+    //zm_msg2_mm(ZM_LV_2, "addrTbl.physAddrl[0]=", addrTbl.physAddrl[0]);
+    //zm_msg2_mm(ZM_LV_2, "buf->data=", buf->data);
+
+    #if 0
+    if ((err = zfHpSend(dev, NULL, 0, NULL, 0, NULL, 0, buf, 0,
+            ZM_INTERNAL_ALLOC_BUF, 0, 0xff)) != ZM_SUCCESS)
+    {
+        goto zlError;
+    }
+    #else
+    zfPutVmmq(dev, buf);
+    zfPushVtxq(dev);
+    #endif
+
+    return;
+#if 0
+zlError:
+
+    zfwBufFree(dev, buf, 0);
+    return;
+#endif
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfProcessManagement         */
+/*      Process received management frame.                              */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : received management frame buffer                           */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+void zfProcessManagement(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* AddInfo) //CWYang(m)
+{
+    u8_t frameType;
+    u16_t ta[3];
+    u16_t ra[3];
+    u16_t vap = 0, index = 0;
+    //u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    ra[0] = zmw_rx_buf_readh(dev, buf, 4);
+    ra[1] = zmw_rx_buf_readh(dev, buf, 6);
+    ra[2] = zmw_rx_buf_readh(dev, buf, 8);
+
+    ta[0] = zmw_rx_buf_readh(dev, buf, 10);
+    ta[1] = zmw_rx_buf_readh(dev, buf, 12);
+    ta[2] = zmw_rx_buf_readh(dev, buf, 14);
+
+    frameType = zmw_rx_buf_readb(dev, buf, 0);
+
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+#if 1
+        vap = 0;
+        if ((ra[0] & 0x1) != 1)
+        {
+            /* AP : Find virtual AP */
+            if ((index = zfApFindSta(dev, ta)) != 0xffff)
+            {
+                vap = wd->ap.staTable[index].vap;
+            }
+        }
+        zm_msg2_mm(ZM_LV_2, "vap=", vap);
+#endif
+
+        /* Dispatch by frame type */
+        switch (frameType)
+        {
+                /* Beacon */
+            case ZM_WLAN_FRAME_TYPE_BEACON :
+                zfApProcessBeacon(dev, buf);
+                break;
+                /* Authentication */
+            case ZM_WLAN_FRAME_TYPE_AUTH :
+                zfApProcessAuth(dev, buf, ta, vap);
+                break;
+                /* Association request */
+            case ZM_WLAN_FRAME_TYPE_ASOCREQ :
+                /* Reassociation request */
+            case ZM_WLAN_FRAME_TYPE_REASOCREQ :
+                zfApProcessAsocReq(dev, buf, ta, vap);
+                break;
+                /* Association response */
+            case ZM_WLAN_FRAME_TYPE_ASOCRSP :
+                //zfApProcessAsocRsp(dev, buf);
+                break;
+                /* Deauthentication */
+            case ZM_WLAN_FRAME_TYPE_DEAUTH :
+                zfApProcessDeauth(dev, buf, ta, vap);
+                break;
+                /* Disassociation */
+            case ZM_WLAN_FRAME_TYPE_DISASOC :
+                zfApProcessDisasoc(dev, buf, ta, vap);
+                break;
+                /* Probe request */
+            case ZM_WLAN_FRAME_TYPE_PROBEREQ :
+                zfProcessProbeReq(dev, buf, ta);
+                break;
+                /* Probe response */
+            case ZM_WLAN_FRAME_TYPE_PROBERSP :
+                zfApProcessProbeRsp(dev, buf, AddInfo);
+                break;
+                /* Action */
+            case ZM_WLAN_FRAME_TYPE_ACTION :
+                zfApProcessAction(dev, buf);
+                break;
+        }
+    }
+    else //if ((wd->wlanMode == ZM_MODE_INFRASTRUCTURE) || (wd->wlanMode == ZM_MODE_IBSS))
+    {
+        /* Dispatch by frame type */
+        switch (frameType)
+        {
+                /* Beacon */
+            case ZM_WLAN_FRAME_TYPE_BEACON :
+                /* if enable 802.11h and current chanel is silent but receive beacon from other AP */
+                if (((wd->regulationTable.allowChannel[wd->regulationTable.CurChIndex].channelFlags
+                        & ZM_REG_FLAG_CHANNEL_CSA) != 0) && wd->sta.DFSEnable)
+                {
+                    wd->regulationTable.allowChannel[wd->regulationTable.CurChIndex].channelFlags
+                            &= ~(ZM_REG_FLAG_CHANNEL_CSA & ZM_REG_FLAG_CHANNEL_PASSIVE);
+                }
+                zfStaProcessBeacon(dev, buf, AddInfo); //CWYang(m)
+                break;
+                /* Authentication */
+            case ZM_WLAN_FRAME_TYPE_AUTH :
+                /* TODO : vap parameter is useless in STA mode, get rid of it */
+                zfStaProcessAuth(dev, buf, ta, 0);
+                break;
+                /* Association request */
+            case ZM_WLAN_FRAME_TYPE_ASOCREQ :
+                /* TODO : vap parameter is useless in STA mode, get rid of it */
+                zfStaProcessAsocReq(dev, buf, ta, 0);
+                break;
+                /* Association response */
+            case ZM_WLAN_FRAME_TYPE_ASOCRSP :
+                /* Reassociation request */
+            case ZM_WLAN_FRAME_TYPE_REASOCRSP :
+                zfStaProcessAsocRsp(dev, buf);
+                break;
+                /* Deauthentication */
+            case ZM_WLAN_FRAME_TYPE_DEAUTH :
+                zm_debug_msg0("Deauthentication received");
+                zfStaProcessDeauth(dev, buf);
+                break;
+                /* Disassociation */
+            case ZM_WLAN_FRAME_TYPE_DISASOC :
+                zm_debug_msg0("Disassociation received");
+                zfStaProcessDisasoc(dev, buf);
+                break;
+                /* Probe request */
+            case ZM_WLAN_FRAME_TYPE_PROBEREQ :
+                zfProcessProbeReq(dev, buf, ta);
+                break;
+                /* Probe response */
+            case ZM_WLAN_FRAME_TYPE_PROBERSP :
+                /* if enable 802.11h and current chanel is silent but receive probe response from other AP */
+                if (((wd->regulationTable.allowChannel[wd->regulationTable.CurChIndex].channelFlags
+                        & ZM_REG_FLAG_CHANNEL_CSA) != 0) && wd->sta.DFSEnable)
+                {
+                    wd->regulationTable.allowChannel[wd->regulationTable.CurChIndex].channelFlags
+                            &= ~(ZM_REG_FLAG_CHANNEL_CSA & ZM_REG_FLAG_CHANNEL_PASSIVE);
+                }
+                zfStaProcessProbeRsp(dev, buf, AddInfo);
+                break;
+
+            case ZM_WLAN_FRAME_TYPE_ATIM:
+                zfStaProcessAtim(dev, buf);
+                break;
+                /* Action */
+            case ZM_WLAN_FRAME_TYPE_ACTION :
+                zm_msg0_mm(ZM_LV_2, "ProcessActionMgtFrame");
+                zfStaProcessAction(dev, buf);
+                break;
+        }
+    }
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfProcessProbeReq           */
+/*      Process probe request management frame.                         */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : auth frame buffer                                         */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+void zfProcessProbeReq(zdev_t* dev, zbuf_t* buf, u16_t* src)
+{
+    u16_t offset;
+    u8_t len;
+    u16_t i, j;
+    u8_t ch;
+    u16_t sendFlag;
+
+    zmw_get_wlan_dev(dev);
+
+    /* check mode : AP/IBSS */
+    if ((wd->wlanMode != ZM_MODE_AP) && (wd->wlanMode != ZM_MODE_IBSS))
+    {
+        zm_msg0_mm(ZM_LV_3, "Ignore probe req");
+        return;
+    }
+
+    if ((wd->wlanMode != ZM_MODE_AP) && (wd->sta.adapterState == ZM_STA_STATE_DISCONNECT))
+    {
+        zm_msg0_mm(ZM_LV_3, "Packets dropped due to disconnect state");
+        return;
+    }
+
+    if ( wd->wlanMode == ZM_MODE_IBSS )
+    {
+        zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_PROBERSP, src, 0, 0, 0);
+
+        return;
+    }
+
+    /* check SSID */
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_SSID)) == 0xffff)
+    {
+        zm_msg0_mm(ZM_LV_3, "probe req SSID not found");
+        return;
+    }
+
+    len = zmw_rx_buf_readb(dev, buf, offset+1);
+
+    for (i=0; i<ZM_MAX_AP_SUPPORT; i++)
+    {
+        if ((wd->ap.apBitmap & (1<<i)) != 0)
+        {
+            zm_msg1_mm(ZM_LV_3, "len=", len);
+            sendFlag = 0;
+            /* boardcast SSID */
+            if (len == 0)
+            {
+                if (wd->ap.hideSsid[i] == 0)
+                {
+                    sendFlag = 1;
+                }
+            }
+            /* Not broadcast SSID */
+            else if (wd->ap.ssidLen[i] == len)
+            {
+                for (j=0; j<len; j++)
+                {
+                    if ((ch = zmw_rx_buf_readb(dev, buf, offset+2+j))
+                            != wd->ap.ssid[i][j])
+                    {
+                        break;
+                    }
+                }
+                if (j == len)
+                {
+                    sendFlag = 1;
+                }
+            }
+            if (sendFlag == 1)
+            {
+                /* Send probe response */
+                zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_PROBERSP, src, i, 0, i);
+            }
+        }
+    }
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfProcessProbeRsp           */
+/*      Process probe response management frame.                        */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : auth frame buffer                                         */
+/*  AddInfo : Rx Header and Rx Mac Status                               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Aress Yang          ZyDAS Technology Corporation    2006.11     */
+/*                                                                      */
+/************************************************************************/
+void zfProcessProbeRsp(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* AddInfo)
+{
+    /* Gather scan result */
+    /* Parse TIM and send PS-POLL in power saving mode */
+    struct zsWlanProbeRspFrameHeader*  pProbeRspHeader;
+    struct zsBssInfo* pBssInfo;
+    u8_t   pBuf[sizeof(struct zsWlanProbeRspFrameHeader)];
+    int    res;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zfCopyFromRxBuffer(dev, buf, pBuf, 0,
+                       sizeof(struct zsWlanProbeRspFrameHeader));
+    pProbeRspHeader = (struct zsWlanProbeRspFrameHeader*) pBuf;
+
+    zmw_enter_critical_section(dev);
+
+    //zm_debug_msg1("bss count = ", wd->sta.bssList.bssCount);
+
+    pBssInfo = zfStaFindBssInfo(dev, buf, pProbeRspHeader);
+
+    //if ( i == wd->sta.bssList.bssCount )
+    if ( pBssInfo == NULL )
+    {
+        /* Allocate a new entry if BSS not in the scan list */
+        pBssInfo = zfBssInfoAllocate(dev);
+        if (pBssInfo != NULL)
+        {
+            res = zfStaInitBssInfo(dev, buf, pProbeRspHeader, pBssInfo, AddInfo, 0);
+            //zfDumpSSID(pBssInfo->ssid[1], &(pBssInfo->ssid[2]));
+            if ( res != 0 )
+            {
+                zfBssInfoFree(dev, pBssInfo);
+            }
+            else
+            {
+                zfBssInfoInsertToList(dev, pBssInfo);
+            }
+        }
+    }
+    else
+    {
+        res = zfStaInitBssInfo(dev, buf, pProbeRspHeader, pBssInfo, AddInfo, 1);
+        if (res == 2)
+        {
+            zfBssInfoRemoveFromList(dev, pBssInfo);
+            zfBssInfoFree(dev, pBssInfo);
+        }
+        else if ( wd->wlanMode == ZM_MODE_IBSS )
+        {
+            int idx;
+
+            // It would reset the alive counter if the peer station is found!
+            zfStaFindFreeOpposite(dev, (u16_t *)pBssInfo->macaddr, &idx);
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfSendProbeReq              */
+/*      Send probe request management frame.                            */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Ji-Huang Lee        ZyDAS Technology Corporation    2005.11     */
+/*                                                                      */
+/************************************************************************/
+
+u16_t zfSendProbeReq(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t bWithSSID)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+
+    /* SSID */
+    if (bWithSSID == 0)  /* broadcast ssid */
+    {
+        //zmw_leave_critical_section(dev);
+        zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_SSID);
+        zmw_tx_buf_writeb(dev, buf, offset++, 0);   /* length = 0 */
+    }
+    else
+    {
+        zmw_enter_critical_section(dev);
+        if (wd->ws.probingSsidList[bWithSSID-1].ssidLen == 0)
+        {
+            zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_SSID);
+            zmw_tx_buf_writeb(dev, buf, offset++, 0);   /* length = 0 */
+        }
+        else
+        {
+            zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_SSID);
+            zmw_tx_buf_writeb(dev, buf, offset++,
+                    wd->ws.probingSsidList[bWithSSID-1].ssidLen);
+            zfCopyToIntTxBuffer(dev, buf,
+                    wd->ws.probingSsidList[bWithSSID-1].ssid,
+                    offset,
+                    wd->ws.probingSsidList[bWithSSID-1].ssidLen); /* ssid */
+            offset += wd->ws.probingSsidList[bWithSSID-1].ssidLen;
+        }
+        zmw_leave_critical_section(dev);
+    }
+
+    /* Supported rates */
+    if ( wd->sta.currentFrequency < 3000 )
+    {   /* 802.11b+g */
+        offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                      ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_CCK);
+
+        if (wd->supportMode & (ZM_WIRELESS_MODE_24_54|ZM_WIRELESS_MODE_24_N)) {
+            if (wd->wlanMode == ZM_MODE_IBSS) {
+                if (wd->wfc.bIbssGMode) {
+                    offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                      ZM_WLAN_EID_EXTENDED_RATE, ZM_RATE_SET_OFDM);
+                }
+            } else {
+                offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                      ZM_WLAN_EID_EXTENDED_RATE, ZM_RATE_SET_OFDM);
+            }
+        }
+    }
+    else
+    {  /* 802.11a */
+        offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                      ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_OFDM);
+    }
+
+    return offset;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfUpdateDefaultQosParameter */
+/*      Update TxQs CWMIN, CWMAX, AIFS and TXO to WME default value.    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      mode : 0=>STA, 1=>AP                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen             ZyDAS Technology Corporation    2006.6      */
+/*                                                                      */
+/************************************************************************/
+void zfUpdateDefaultQosParameter(zdev_t* dev, u8_t mode)
+{
+    u16_t cwmin[5];
+    u16_t cwmax[5];
+    u16_t aifs[5];
+    u16_t txop[5];
+
+    /* WMM parameter for STA */
+    /* Best Effor */
+    cwmin[0] = 15;
+    cwmax[0] = 1023;
+    aifs[0] = 3 * 9 + 10;
+    txop[0] = 0;
+    /* Back Ground */
+    cwmin[1] = 15;
+    cwmax[1] = 1023;
+    aifs[1] = 7 * 9 + 10;
+    txop[1] = 0;
+    /* VIDEO */
+    cwmin[2] = 7;
+    cwmax[2] = 15;
+    aifs[2] = 2 * 9 + 10;
+    txop[2] = 94;
+    /* VOICE */
+    cwmin[3] = 3;
+    cwmax[3] = 7;
+    aifs[3] = 2 * 9 + 10;
+    txop[3] = 47;
+    /* Special TxQ */
+    cwmin[4] = 3;
+    cwmax[4] = 7;
+    aifs[4] = 2 * 9 + 10;
+    txop[4] = 0;
+
+    /* WMM parameter for AP */
+    if (mode == 1)
+    {
+        cwmax[0] = 63;
+        aifs[3] = 1 * 9 + 10;
+        aifs[4] = 1 * 9 + 10;
+    }
+    zfHpUpdateQosParameter(dev, cwmin, cwmax, aifs, txop);
+}
+
+u16_t zfFindATHExtCap(zdev_t* dev, zbuf_t* buf, u8_t type, u8_t subtype)
+{
+    u8_t subType;
+    u16_t offset;
+    u16_t bufLen;
+    u16_t elen;
+    u8_t id;
+    u8_t tmp;
+
+    /* Get offset of first element */
+    subType = (zmw_rx_buf_readb(dev, buf, 0) >> 4);
+
+    if ((offset = zgElementOffsetTable[subType]) == 0xff)
+    {
+        zm_assert(0);
+    }
+
+    /* Plus wlan header */
+    offset += 24;
+
+    bufLen = zfwBufGetSize(dev, buf);
+
+    /* Search loop */
+    while ((offset+2)<bufLen)                   // including element ID and length (2bytes)
+    {
+        /* Search target element */
+        if ((id = zmw_rx_buf_readb(dev, buf, offset)) == ZM_WLAN_EID_WIFI_IE)
+        {
+            /* Bingo */
+            if ((elen = zmw_rx_buf_readb(dev, buf, offset+1))>(bufLen - offset))
+            {
+                /* Element length error */
+                return 0xffff;
+            }
+
+            if ( elen == 0 )
+            {
+                return 0xffff;
+            }
+
+            if (((tmp = zmw_rx_buf_readb(dev, buf, offset+2)) == 0x00)
+                    && ((tmp = zmw_rx_buf_readb(dev, buf, offset+3)) == 0x03)
+                    && ((tmp = zmw_rx_buf_readb(dev, buf, offset+4)) == 0x7f)
+                    && ((tmp = zmw_rx_buf_readb(dev, buf, offset+5)) == type))
+
+            {
+                if ( subtype != 0xff )
+                {
+                    if ( (tmp = zmw_rx_buf_readb(dev, buf, offset+6)) == subtype  )
+                    {
+                        return offset;
+                    }
+                }
+                else
+                {
+                    return offset;
+                }
+            }
+        }
+
+        /* Advance to next element */
+        if ((elen = zmw_rx_buf_readb(dev, buf, offset+1)) == 0)
+        {
+            return 0xffff;
+        }
+        offset += (elen+2);
+    }
+    return 0xffff;
+}
+
+u16_t zfFindBrdcmMrvlRlnkExtCap(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t subType;
+    u16_t offset;
+    u16_t bufLen;
+    u16_t elen;
+    u8_t id;
+    u8_t tmp;
+
+    /* Get offset of first element */
+    subType = (zmw_rx_buf_readb(dev, buf, 0) >> 4);
+
+    if ((offset = zgElementOffsetTable[subType]) == 0xff)
+    {
+        zm_assert(0);
+    }
+
+    /* Plus wlan header */
+    offset += 24;
+
+    bufLen = zfwBufGetSize(dev, buf);
+
+    /* Search loop */
+    while ((offset+2)<bufLen)                   // including element ID and length (2bytes)
+    {
+        /* Search target element */
+        if ((id = zmw_rx_buf_readb(dev, buf, offset)) == ZM_WLAN_EID_WIFI_IE)
+        {
+            /* Bingo */
+            if ((elen = zmw_rx_buf_readb(dev, buf, offset+1))>(bufLen - offset))
+            {
+                /* Element length error */
+                return 0xffff;
+            }
+
+            if ( elen == 0 )
+            {
+                return 0xffff;
+            }
+
+            if (((tmp = zmw_rx_buf_readb(dev, buf, offset+2)) == 0x00)
+                    && ((tmp = zmw_rx_buf_readb(dev, buf, offset+3)) == 0x10)
+                    && ((tmp = zmw_rx_buf_readb(dev, buf, offset+4)) == 0x18))
+
+            {
+                return offset;
+            }
+            else if (((tmp = zmw_rx_buf_readb(dev, buf, offset+2)) == 0x00)
+                    && ((tmp = zmw_rx_buf_readb(dev, buf, offset+3)) == 0x50)
+                    && ((tmp = zmw_rx_buf_readb(dev, buf, offset+4)) == 0x43))
+
+            {
+                return offset;
+            }
+        }
+        else if ((id = zmw_rx_buf_readb(dev, buf, offset)) == 0x7F)
+        {
+            /* Bingo */
+            if ((elen = zmw_rx_buf_readb(dev, buf, offset+1))>(bufLen - offset))
+            {
+                /* Element length error */
+                return 0xffff;
+            }
+
+            if ( elen == 0 )
+            {
+                return 0xffff;
+            }
+
+            if ((tmp = zmw_rx_buf_readb(dev, buf, offset+2)) == 0x01)
+
+            {
+                return offset;
+            }
+        }
+
+        /* Advance to next element */
+        if ((elen = zmw_rx_buf_readb(dev, buf, offset+1)) == 0)
+        {
+            return 0xffff;
+        }
+        offset += (elen+2);
+    }
+    return 0xffff;
+}
+
+u16_t zfFindMarvelExtCap(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t subType;
+    u16_t offset;
+    u16_t bufLen;
+    u16_t elen;
+    u8_t id;
+    u8_t tmp;
+
+    /* Get offset of first element */
+    subType = (zmw_rx_buf_readb(dev, buf, 0) >> 4);
+
+    if ((offset = zgElementOffsetTable[subType]) == 0xff)
+    {
+        zm_assert(0);
+    }
+
+    /* Plus wlan header */
+    offset += 24;
+
+    bufLen = zfwBufGetSize(dev, buf);
+
+    /* Search loop */
+    while ((offset+2)<bufLen)                   // including element ID and length (2bytes)
+    {
+        /* Search target element */
+        if ((id = zmw_rx_buf_readb(dev, buf, offset)) == ZM_WLAN_EID_WIFI_IE)
+        {
+            /* Bingo */
+            if ((elen = zmw_rx_buf_readb(dev, buf, offset+1))>(bufLen - offset))
+            {
+                /* Element length error */
+                return 0xffff;
+            }
+
+            if ( elen == 0 )
+            {
+                return 0xffff;
+            }
+
+            if (((tmp = zmw_rx_buf_readb(dev, buf, offset+2)) == 0x00)
+                  && ((tmp = zmw_rx_buf_readb(dev, buf, offset+3)) == 0x50)
+                  && ((tmp = zmw_rx_buf_readb(dev, buf, offset+4)) == 0x43))
+
+            {
+                return offset;
+            }
+        }
+
+        /* Advance to next element */
+        if ((elen = zmw_rx_buf_readb(dev, buf, offset+1)) == 0)
+        {
+            return 0xffff;
+        }
+        offset += (elen+2);
+    }
+    return 0xffff;
+}
+
+u16_t zfFindBroadcomExtCap(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t subType;
+    u16_t offset;
+    u16_t bufLen;
+    u16_t elen;
+    u8_t id;
+    u8_t tmp;
+
+    /* Get offset of first element */
+    subType = (zmw_rx_buf_readb(dev, buf, 0) >> 4);
+
+    if ((offset = zgElementOffsetTable[subType]) == 0xff)
+    {
+        zm_assert(0);
+    }
+
+    /* Plus wlan header */
+    offset += 24;
+
+    bufLen = zfwBufGetSize(dev, buf);
+
+    /* Search loop */
+    while((offset+2) < bufLen)                   // including element ID and length (2bytes)
+    {
+        /* Search target element */
+        if ((id = zmw_rx_buf_readb(dev, buf, offset)) == ZM_WLAN_EID_WIFI_IE)
+        {
+            /* Bingo */
+            if ((elen = zmw_rx_buf_readb(dev, buf, offset+1)) > (bufLen - offset))
+            {
+                /* Element length error */
+                return 0xffff;
+            }
+
+            if (elen == 0)
+            {
+                return 0xffff;
+            }
+
+            if ( ((tmp = zmw_rx_buf_readb(dev, buf, offset+2)) == 0x00)
+                 && ((tmp = zmw_rx_buf_readb(dev, buf, offset+3)) == 0x10)
+                 && ((tmp = zmw_rx_buf_readb(dev, buf, offset+4)) == 0x18) )
+            {
+                return offset;
+            }
+        }
+
+        /* Advance to next element */
+        if ((elen = zmw_rx_buf_readb(dev, buf, offset+1)) == 0)
+        {
+            return 0xffff;
+        }
+
+        offset += (elen+2);
+    }
+
+    return 0xffff;
+}
+
+u16_t zfFindRlnkExtCap(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t subType;
+    u16_t offset;
+    u16_t bufLen;
+    u16_t elen;
+    u8_t id;
+    u8_t tmp;
+
+    /* Get offset of first element */
+    subType = (zmw_rx_buf_readb(dev, buf, 0) >> 4);
+
+    if ((offset = zgElementOffsetTable[subType]) == 0xff)
+    {
+        zm_assert(0);
+    }
+
+    /* Plus wlan header */
+    offset += 24;
+
+    bufLen = zfwBufGetSize(dev, buf);
+
+    /* Search loop */
+    while((offset+2) < bufLen)                   // including element ID and length (2bytes)
+    {
+        /* Search target element */
+        if ((id = zmw_rx_buf_readb(dev, buf, offset)) == 0x7F)
+        {
+            /* Bingo */
+            if ((elen = zmw_rx_buf_readb(dev, buf, offset+1)) > (bufLen - offset))
+            {
+                /* Element length error */
+                return 0xffff;
+            }
+
+            if ( elen == 0 )
+            {
+                return 0xffff;
+            }
+
+            if ((tmp = zmw_rx_buf_readb(dev, buf, offset+2)) == 0x01)
+
+            {
+                return offset;
+            }
+        }
+
+        /* Advance to next element */
+        if ((elen = zmw_rx_buf_readb(dev, buf, offset+1)) == 0)
+        {
+            return 0xffff;
+        }
+
+        offset += (elen+2);
+    }
+
+    return 0xffff;
+}
diff --git a/drivers/staging/otus/80211core/cmmap.c b/drivers/staging/otus/80211core/cmmap.c
new file mode 100644
index 00000000000..7f09fded459
--- /dev/null
+++ b/drivers/staging/otus/80211core/cmmap.c
@@ -0,0 +1,2402 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : mm.c                                                  */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains common functions for handle AP             */
+/*      management frame.                                               */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+#include "cprecomp.h"
+#include "ratectrl.h"
+
+extern const u8_t zcUpToAc[];
+
+void zfMmApTimeTick(zdev_t* dev)
+{
+    u32_t now;
+    zmw_get_wlan_dev(dev);
+
+    //zm_debug_msg1("wd->wlanMode : ", wd->wlanMode);
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        /* => every 1.28 seconds */
+        /* AP : aging STA that does not active for wd->ap.staAgingTime    */
+        now = wd->tick & 0x7f;
+        if (now == 0x0)
+        {
+            zfApAgingSta(dev);
+        }
+        else if (now == 0x1f)
+        {
+            zfQueueAge(dev, wd->ap.uapsdQ, wd->tick, 10000);
+        }
+        /* AP : check (wd->ap.protectedObss) and (wd->ap.bStaAssociated)  */
+        /*      to enable NonErp and Protection mode                      */
+        else if (now == 0x3f)
+        {
+            //zfApProtctionMonitor(dev);
+        }
+    }
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfApInitStaTbl              */
+/*      Init AP's station table.                                        */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+void zfApInitStaTbl(zdev_t* dev)
+{
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    for (i=0; i<ZM_MAX_STA_SUPPORT; i++)
+    {
+        wd->ap.staTable[i].valid = 0;
+        wd->ap.staTable[i].state = 0;
+        wd->ap.staTable[i].addr[0] = 0;
+        wd->ap.staTable[i].addr[1] = 0;
+        wd->ap.staTable[i].addr[2] = 0;
+        wd->ap.staTable[i].time = 0;
+        wd->ap.staTable[i].vap = 0;
+        wd->ap.staTable[i].encryMode = ZM_NO_WEP;
+    }
+    return;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfApFindSta                 */
+/*      Find a STA in station table.                                    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      addr : Target STA address                                       */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0xffff : fail                                                   */
+/*      other : STA table index                                         */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfApFindSta(zdev_t* dev, u16_t* addr)
+{
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    for (i=0; i<ZM_MAX_STA_SUPPORT; i++)
+    {
+        if (wd->ap.staTable[i].valid == 1)
+        {
+            if ((wd->ap.staTable[i].addr[0] == addr[0])
+                    && (wd->ap.staTable[i].addr[1] == addr[1])
+                    && (wd->ap.staTable[i].addr[2] == addr[2]))
+            {
+                return i;
+            }
+        }
+    }
+    return 0xffff;
+}
+
+u16_t zfApGetSTAInfo(zdev_t* dev, u16_t* addr, u16_t* state, u8_t* vap)
+{
+    u16_t id;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if ((id = zfApFindSta(dev, addr)) != 0xffff)
+    {
+        *vap = wd->ap.staTable[id].vap;
+        *state = wd->ap.staTable[id++].state;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return id;
+}
+
+
+void zfApGetStaQosType(zdev_t* dev, u16_t* addr, u8_t* qosType)
+{
+    u16_t id;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if ((id = zfApFindSta(dev, addr)) != 0xffff)
+    {
+        *qosType = wd->ap.staTable[id].qosType;
+    }
+    else
+    {
+        *qosType = 0;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return;
+}
+
+void zfApGetStaTxRateAndQosType(zdev_t* dev, u16_t* addr, u32_t* phyCtrl,
+                                u8_t* qosType, u16_t* rcProbingFlag)
+{
+    u16_t id;
+    u8_t rate;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if ((id = zfApFindSta(dev, addr)) != 0xffff)
+    {
+        rate = (u8_t)zfRateCtrlGetTxRate(dev, &wd->ap.staTable[id].rcCell, rcProbingFlag);
+#ifdef ZM_AP_DEBUG
+        //rate = 15;
+#endif
+        *phyCtrl = zcRateToPhyCtrl[rate];
+        *qosType = wd->ap.staTable[id].qosType;
+    }
+    else
+    {
+        if (wd->frequency < 3000)
+        {
+            /* CCK 1M */
+            //header[2] = 0x0f00;          //PHY control L
+            //header[3] = 0x0000;          //PHY control H
+            *phyCtrl = 0x00000F00;
+        }
+        else
+        {
+            /* CCK 6M */
+            //header[2] = 0x0f01;          //PHY control L
+            //header[3] = 0x000B;          //PHY control H
+            *phyCtrl = 0x000B0F01;
+        }
+        *qosType = 0;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    zm_msg2_mm(ZM_LV_3, "PhyCtrl=", *phyCtrl);
+    return;
+}
+
+void zfApGetStaEncryType(zdev_t* dev, u16_t* addr, u8_t* encryType)
+{
+    //struct zsWlanDev* wd = (struct zsWlanDev*) zmw_wlan_dev(dev);
+    u16_t id;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if ((id = zfApFindSta(dev, addr)) != 0xffff)
+    {
+        *encryType = wd->ap.staTable[id].encryMode;
+    }
+    else
+    {
+        *encryType = ZM_NO_WEP;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    zm_msg2_mm(ZM_LV_3, "encyrType=", *encryType);
+    return;
+}
+
+void zfApGetStaWpaIv(zdev_t* dev, u16_t* addr, u16_t* iv16, u32_t* iv32)
+{
+    //struct zsWlanDev* wd = (struct zsWlanDev*) zmw_wlan_dev(dev);
+    u16_t id;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if ((id = zfApFindSta(dev, addr)) != 0xffff)
+    {
+        *iv16 = wd->ap.staTable[id].iv16;
+        *iv32 = wd->ap.staTable[id].iv32;
+    }
+    else
+    {
+        *iv16 = 0;
+        *iv32 = 0;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    zm_msg2_mm(ZM_LV_3, "iv16=", *iv16);
+    zm_msg2_mm(ZM_LV_3, "iv32=", *iv32);
+    return;
+}
+
+void zfApSetStaWpaIv(zdev_t* dev, u16_t* addr, u16_t iv16, u32_t iv32)
+{
+    //struct zsWlanDev* wd = (struct zsWlanDev*) zmw_wlan_dev(dev);
+    u16_t id;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if ((id = zfApFindSta(dev, addr)) != 0xffff)
+    {
+        wd->ap.staTable[id].iv16 = iv16;
+        wd->ap.staTable[id].iv32 = iv32;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    zm_msg2_mm(ZM_LV_3, "iv16=", iv16);
+    zm_msg2_mm(ZM_LV_3, "iv32=", iv32);
+    return;
+}
+
+void zfApClearStaKey(zdev_t* dev, u16_t* addr)
+{
+    //struct zsWlanDev* wd = (struct zsWlanDev*) zmw_wlan_dev(dev);
+    u16_t bcAddr[3] = { 0xffff, 0xffff, 0xffff };
+    u16_t id;
+
+    zmw_get_wlan_dev(dev);
+
+    if (zfMemoryIsEqual((u8_t*)bcAddr, (u8_t*)addr, sizeof(bcAddr)) == TRUE)
+    {
+        /* Turn off group key information */
+    //    zfClearKey(dev, 0);
+    }
+    else
+    {
+        zmw_declare_for_critical_section();
+
+        zmw_enter_critical_section(dev);
+
+        if ((id = zfApFindSta(dev, addr)) != 0xffff)
+        {
+            /* Turn off STA's key information */
+            zfHpRemoveKey(dev, id+1);
+
+            /* Update STA's Encryption Type */
+            wd->ap.staTable[id].encryMode = ZM_NO_WEP;
+        }
+        else
+        {
+            zm_msg0_mm(ZM_LV_3, "Can't find STA address\n");
+        }
+        zmw_leave_critical_section(dev);
+    }
+}
+
+#ifdef ZM_ENABLE_CENC
+void zfApGetStaCencIvAndKeyIdx(zdev_t* dev, u16_t* addr, u32_t *iv, u8_t *keyIdx)
+{
+    //struct zsWlanDev* wd = (struct zsWlanDev*) zmw_wlan_dev(dev);
+    u16_t id;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+
+    zmw_enter_critical_section(dev);
+
+    if ((id = zfApFindSta(dev, addr)) != 0xffff)
+    {
+        *iv++ = wd->ap.staTable[id].txiv[0];
+        *iv++ = wd->ap.staTable[id].txiv[1];
+        *iv++ = wd->ap.staTable[id].txiv[2];
+        *iv = wd->ap.staTable[id].txiv[3];
+        *keyIdx = wd->ap.staTable[id].cencKeyIdx;
+    }
+    else
+    {
+        *iv++ = 0x5c365c37;
+        *iv++ = 0x5c365c36;
+        *iv++ = 0x5c365c36;
+        *iv = 0x5c365c36;
+        *keyIdx = 0;
+    }
+
+    zmw_leave_critical_section(dev);
+    return;
+}
+
+void zfApSetStaCencIv(zdev_t* dev, u16_t* addr, u32_t *iv)
+{
+    //struct zsWlanDev* wd = (struct zsWlanDev*) zmw_wlan_dev(dev);
+    u16_t id;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+
+    zmw_enter_critical_section(dev);
+
+    if ((id = zfApFindSta(dev, addr)) != 0xffff)
+    {
+        wd->ap.staTable[id].txiv[0] = *iv++;
+        wd->ap.staTable[id].txiv[1] = *iv++;
+        wd->ap.staTable[id].txiv[2] = *iv++;
+        wd->ap.staTable[id].txiv[3] = *iv;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return;
+}
+#endif //ZM_ENABLE_CENC
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfApFlushBufferedPsFrame    */
+/*      Free buffered PS frames.                                        */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.1      */
+/*                                                                      */
+/************************************************************************/
+void zfApFlushBufferedPsFrame(zdev_t* dev)
+{
+    u16_t emptyFlag;
+    u16_t freeCount;
+    u16_t vap;
+    zbuf_t* psBuf = NULL;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    freeCount = 0;
+    emptyFlag = 0;
+    while (1)
+    {
+        psBuf = NULL;
+        zmw_enter_critical_section(dev);
+        if (wd->ap.uniHead != wd->ap.uniTail)
+        {
+            psBuf = wd->ap.uniArray[wd->ap.uniHead];
+            wd->ap.uniHead = (wd->ap.uniHead + 1) & (ZM_UNI_ARRAY_SIZE - 1);
+        }
+        else
+        {
+            emptyFlag = 1;
+        }
+        zmw_leave_critical_section(dev);
+
+        if (psBuf != NULL)
+        {
+            zfwBufFree(dev, psBuf, ZM_ERR_FLUSH_PS_QUEUE);
+        }
+        zm_assert(freeCount++ < (ZM_UNI_ARRAY_SIZE*2));
+
+        if (emptyFlag != 0)
+        {
+            break;
+        }
+    }
+
+    for (vap=0; vap<ZM_MAX_AP_SUPPORT; vap++)
+    {
+        freeCount = 0;
+        emptyFlag = 0;
+        while (1)
+        {
+            psBuf = NULL;
+            zmw_enter_critical_section(dev);
+            if (wd->ap.bcmcHead[vap] != wd->ap.bcmcTail[vap])
+            {
+                psBuf = wd->ap.bcmcArray[vap][wd->ap.bcmcHead[vap]];
+                wd->ap.bcmcHead[vap] = (wd->ap.bcmcHead[vap] + 1)
+                        & (ZM_BCMC_ARRAY_SIZE - 1);
+            }
+            else
+            {
+                emptyFlag = 1;
+            }
+            zmw_leave_critical_section(dev);
+
+            if (psBuf != NULL)
+            {
+                zfwBufFree(dev, psBuf, ZM_ERR_FLUSH_PS_QUEUE);
+            }
+            zm_assert(freeCount++ < (ZM_BCMC_ARRAY_SIZE*2));
+
+            if (emptyFlag != 0)
+            {
+                break;
+            }
+        }
+    }
+    return;
+}
+
+
+u16_t zfApBufferPsFrame(zdev_t* dev, zbuf_t* buf, u16_t port)
+{
+    u16_t id;
+    u16_t addr[3];
+    u16_t vap = 0;
+    u8_t up;
+    u16_t fragOff;
+    u8_t ac;
+    u16_t ret;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if (port < ZM_MAX_AP_SUPPORT)
+    {
+        vap = port;
+    }
+
+    addr[0] = zmw_rx_buf_readh(dev, buf, 0);
+    addr[1] = zmw_rx_buf_readh(dev, buf, 2);
+    addr[2] = zmw_rx_buf_readh(dev, buf, 4);
+
+    if ((addr[0] & 0x1) == 0x1)
+    {
+        if (wd->ap.staPowerSaving > 0)
+        {
+            zmw_enter_critical_section(dev);
+
+            /* Buffer this BC or MC frame */
+            if (((wd->ap.bcmcTail[vap]+1)&(ZM_BCMC_ARRAY_SIZE-1))
+                    != wd->ap.bcmcHead[vap])
+            {
+                wd->ap.bcmcArray[vap][wd->ap.bcmcTail[vap]++] = buf;
+                wd->ap.bcmcTail[vap] &= (ZM_BCMC_ARRAY_SIZE-1);
+                zmw_leave_critical_section(dev);
+
+                zm_msg0_tx(ZM_LV_0, "Buffer BCMC");
+            }
+            else
+            {
+                /* bcmcArray full */
+                zmw_leave_critical_section(dev);
+
+                zm_msg0_tx(ZM_LV_0, "BCMC buffer full");
+
+                /* free buffer according to buffer type */
+                zfwBufFree(dev, buf, ZM_ERR_BCMC_PS_BUFFER_UNAVAILABLE);
+            }
+            return 1;
+        }
+    }
+    else
+    {
+        zmw_enter_critical_section(dev);
+
+        if ((id = zfApFindSta(dev, addr)) != 0xffff)
+        {
+            if (wd->ap.staTable[id].psMode == 1)
+            {
+
+                zfTxGetIpTosAndFrag(dev, buf, &up, &fragOff);
+                ac = zcUpToAc[up&0x7] & 0x3;
+
+                if ((wd->ap.staTable[id].qosType == 1) &&
+                        ((wd->ap.staTable[id].qosInfo & (0x8>>ac)) != 0))
+                {
+                    ret = zfQueuePutNcs(dev, wd->ap.uapsdQ, buf, wd->tick);
+                    zmw_leave_critical_section(dev);
+                    if (ret != ZM_SUCCESS)
+                    {
+                        zfwBufFree(dev, buf, ZM_ERR_AP_UAPSD_QUEUE_FULL);
+                    }
+                }
+                else
+                {
+                /* Buffer this unicast frame */
+                if (((wd->ap.uniTail+1)&(ZM_UNI_ARRAY_SIZE-1))
+                        != wd->ap.uniHead)
+                {
+                    wd->ap.uniArray[wd->ap.uniTail++] = buf;
+                    wd->ap.uniTail &= (ZM_UNI_ARRAY_SIZE-1);
+                    zmw_leave_critical_section(dev);
+                    zm_msg0_tx(ZM_LV_0, "Buffer UNI");
+
+                }
+                else
+                {
+                    /* uniArray full */
+                    zmw_leave_critical_section(dev);
+                    zm_msg0_tx(ZM_LV_0, "UNI buffer full");
+                    /* free buffer according to buffer type */
+                    zfwBufFree(dev, buf, ZM_ERR_UNI_PS_BUFFER_UNAVAILABLE);
+                }
+                }
+                return 1;
+            } /* if (wd->ap.staTable[id++].psMode == 1) */
+        } /* if ((id = zfApFindSta(dev, addr)) != 0xffff) */
+        zmw_leave_critical_section(dev);
+    }
+
+    return 0;
+}
+
+u16_t zfApGetSTAInfoAndUpdatePs(zdev_t* dev, u16_t* addr, u16_t* state,
+                                u8_t* vap, u16_t psMode, u8_t* uapsdTrig)
+{
+    u16_t id;
+    u8_t uapsdStaAwake = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+#ifdef ZM_AP_DEBUG
+    //psMode=0;
+#endif
+
+    if ((id = zfApFindSta(dev, addr)) != 0xffff)
+    {
+        if (psMode != 0)
+        {
+            zm_msg0_mm(ZM_LV_0, "psMode = 1");
+            if (wd->ap.staTable[id].psMode == 0)
+            {
+                wd->ap.staPowerSaving++;
+            }
+            else
+            {
+                if (wd->ap.staTable[id].qosType == 1)
+                {
+                    zm_msg0_mm(ZM_LV_0, "UAPSD trigger");
+                    *uapsdTrig = wd->ap.staTable[id].qosInfo;
+                }
+            }
+        }
+        else
+        {
+            if (wd->ap.staTable[id].psMode != 0)
+            {
+                wd->ap.staPowerSaving--;
+                if ((wd->ap.staTable[id].qosType == 1) && ((wd->ap.staTable[id].qosInfo&0xf)!=0))
+                {
+                    uapsdStaAwake = 1;
+                }
+            }
+        }
+
+        wd->ap.staTable[id].psMode = (u8_t) psMode;
+        wd->ap.staTable[id].time = wd->tick;
+        *vap = wd->ap.staTable[id].vap;
+        *state = wd->ap.staTable[id++].state;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    if (uapsdStaAwake == 1)
+    {
+        zbuf_t* psBuf;
+        u8_t mb;
+
+        while (1)
+        {
+            if ((psBuf = zfQueueGetWithMac(dev, wd->ap.uapsdQ, (u8_t*)addr, &mb)) != NULL)
+            {
+                zfTxSendEth(dev, psBuf, 0, ZM_EXTERNAL_ALLOC_BUF, 0);
+            }
+            else
+            {
+                break;
+            }
+        }
+    }
+
+    return id;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfApGetNewSta               */
+/*      Get a new STA from station table.                               */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0xffff : fail                                                   */
+/*      other : STA table index                                         */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfApGetNewSta(zdev_t* dev)
+{
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    for (i=0; i<ZM_MAX_STA_SUPPORT; i++)
+    {
+        if (wd->ap.staTable[i].valid == 0)
+        {
+            zm_msg2_mm(ZM_LV_0, "zfApGetNewSta=", i);
+            return i;
+        }
+    }
+    return 0xffff;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfApAddSta                  */
+/*      Add a STA to station table.                                     */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      addr : STA MAC address                                          */
+/*      state : STA state                                               */
+/*      apId : Virtual AP ID                                            */
+/*      type : 0=>11b, 1=>11g                                           */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0xffff : fail                                                   */
+/*      Other : index                                                   */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfApAddSta(zdev_t* dev, u16_t* addr, u16_t state, u16_t apId, u8_t type,
+                 u8_t qosType, u8_t qosInfo)
+{
+    u16_t index;
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zm_msg1_mm(ZM_LV_0, "STA type=", type);
+
+    zmw_enter_critical_section(dev);
+
+    if ((index = zfApFindSta(dev, addr)) != 0xffff)
+    {
+        zm_msg0_mm(ZM_LV_2, "found");
+        /* Update STA state */
+        if ((state == ZM_STATE_AUTH) || (state == ZM_STATE_PREAUTH))
+        {
+            wd->ap.staTable[index].state = state;
+            wd->ap.staTable[index].time = wd->tick;
+            wd->ap.staTable[index].vap = (u8_t)apId;
+        }
+        else if (state == ZM_STATE_ASOC)
+        {
+            if ((wd->ap.staTable[index].state == ZM_STATE_AUTH))
+                    //&& (wd->ap.staTable[index].vap == apId))
+            {
+                wd->ap.staTable[index].state = state;
+                wd->ap.staTable[index].time = wd->tick;
+                wd->ap.staTable[index].qosType = qosType;
+                wd->ap.staTable[index].vap = (u8_t)apId;
+                wd->ap.staTable[index].staType = type;
+                wd->ap.staTable[index].qosInfo = qosInfo;
+
+                if (wd->frequency < 3000)
+                {
+                    /* Init 11b/g */
+                    zfRateCtrlInitCell(dev, &wd->ap.staTable[index].rcCell, type, 1, 1);
+                }
+                else
+                {
+                    /* Init 11a */
+                    zfRateCtrlInitCell(dev, &wd->ap.staTable[index].rcCell, type, 0, 1);
+                }
+
+                if (wd->zfcbApConnectNotify != NULL)
+                {
+                    wd->zfcbApConnectNotify(dev, (u8_t*)addr, apId);
+                }
+            }
+            else
+            {
+                index = 0xffff;
+            }
+        }
+    }
+    else
+    {
+        zm_msg0_mm(ZM_LV_2, "Not found");
+        if ((state == ZM_STATE_AUTH) || (state == ZM_STATE_PREAUTH))
+        {
+            /* Get a new STA and update state */
+            index = zfApGetNewSta(dev);
+            zm_msg2_mm(ZM_LV_1, "new STA index=", index);
+
+            if (index != 0xffff)
+            {
+                for (i=0; i<3; i++)
+                {
+                    wd->ap.staTable[index].addr[i] = addr[i];
+                }
+                wd->ap.staTable[index].state = state;
+                wd->ap.staTable[index].valid = 1;
+                wd->ap.staTable[index].time = wd->tick;
+                wd->ap.staTable[index].vap = (u8_t)apId;
+                wd->ap.staTable[index].encryMode = ZM_NO_WEP;
+            }
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return index;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfApAgingSta                */
+/*      Aging STA in station table.                                     */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      number of 11b STA in STA table                                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+void zfApAgingSta(zdev_t* dev)
+{
+    u16_t i;
+    u32_t deltaMs;
+    u16_t addr[3];
+    u16_t txFlag;
+    u16_t psStaCount = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    wd->ap.gStaAssociated = wd->ap.bStaAssociated = 0;
+
+    for (i=0; i<ZM_MAX_STA_SUPPORT; i++)
+    {
+        txFlag = 0;
+        zmw_enter_critical_section(dev);
+        if (wd->ap.staTable[i].valid == 1)
+        {
+            addr[0] = wd->ap.staTable[i].addr[0];
+            addr[1] = wd->ap.staTable[i].addr[1];
+            addr[2] = wd->ap.staTable[i].addr[2];
+            /* millisecond */
+            deltaMs = (u32_t)((u32_t)wd->tick-(u32_t)wd->ap.staTable[i].time)
+                      * ZM_MS_PER_TICK;
+
+            /* preauth */
+            if ((wd->ap.staTable[i].state == ZM_STATE_PREAUTH)
+                    && (deltaMs > ZM_PREAUTH_TIMEOUT_MS))
+            {
+                /* Aging STA */
+                wd->ap.staTable[i].valid = 0;
+                wd->ap.authSharing = 0;
+                txFlag = 1;
+            }
+
+            /* auth */
+            if ((wd->ap.staTable[i].state == ZM_STATE_AUTH)
+                    && (deltaMs > ZM_AUTH_TIMEOUT_MS))
+            {
+                /* Aging STA */
+                wd->ap.staTable[i].valid = 0;
+                txFlag = 1;
+            }
+
+            /* asoc */
+            if (wd->ap.staTable[i].state == ZM_STATE_ASOC)
+            {
+                if (wd->ap.staTable[i].psMode != 0)
+                {
+                    psStaCount++;
+                }
+
+                if (deltaMs > ((u32_t)wd->ap.staAgingTimeSec<<10))
+                {
+                    /* Aging STA */
+                    zm_msg1_mm(ZM_LV_0, "Age STA index=", i);
+                    wd->ap.staTable[i].valid = 0;
+                    txFlag = 1;
+                }
+                else if (deltaMs > ((u32_t)wd->ap.staProbingTimeSec<<10))
+                {
+                    if (wd->ap.staTable[i].psMode == 0)
+                    {
+                        /* Probing non-PS STA */
+                        zm_msg1_mm(ZM_LV_0, "Probing STA index=", i);
+                        wd->ap.staTable[i].time +=
+                                (wd->ap.staProbingTimeSec * ZM_TICK_PER_SECOND);
+                        txFlag = 2;
+                    }
+                }
+            }
+
+
+        }
+        zmw_leave_critical_section(dev);
+
+        if (txFlag == 1)
+        {
+            /* Send deauthentication management frame */
+            zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH, addr, 4, 0, 0);
+        }
+        else if (txFlag == 2)
+        {
+            zfSendMmFrame(dev, ZM_WLAN_DATA_FRAME, addr, 0, 0, 0);
+        }
+
+    }
+
+    wd->ap.staPowerSaving = psStaCount;
+
+    return;
+}
+
+void zfApProtctionMonitor(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* 11b STA associated => nonErp, Protect */
+    if (wd->ap.bStaAssociated > 0)
+    {
+        /* Enable NonErp bit in information element */
+        wd->erpElement = ZM_WLAN_NON_ERP_PRESENT_BIT
+                         | ZM_WLAN_USE_PROTECTION_BIT;
+
+        /* Enable protection mode */
+        zfApSetProtectionMode(dev, 1);
+
+    }
+    /* 11b STA not associated, protection OBSS present => Protect */
+    else if (wd->ap.protectedObss > 2) //Threshold
+    {
+        if (wd->disableSelfCts == 0)
+        {
+            /* Disable NonErp bit in information element */
+            wd->erpElement = ZM_WLAN_USE_PROTECTION_BIT;
+
+            /* Enable protection mode */
+            zfApSetProtectionMode(dev, 1);
+        }
+    }
+    else
+    {
+        /* Disable NonErp bit in information element */
+        wd->erpElement = 0;
+
+        /* Disable protection mode */
+        zfApSetProtectionMode(dev, 0);
+    }
+    wd->ap.protectedObss = 0;
+}
+
+
+void zfApProcessBeacon(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t offset;
+    u8_t ch;
+
+    zmw_get_wlan_dev(dev);
+
+    zm_msg0_mm(ZM_LV_3, "Rx beacon");
+
+    /* update Non-ERP flag(wd->ap.nonErpObss) */
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_ERP)) == 0xffff)
+    {
+        /* 11b OBSS */
+        wd->ap.protectedObss++;
+        return;
+    }
+
+    ch = zmw_rx_buf_readb(dev, buf, offset+2);
+    if ((ch & ZM_WLAN_USE_PROTECTION_BIT) == ZM_WLAN_USE_PROTECTION_BIT)
+    {
+        /* Protected OBSS */
+        wd->ap.protectedObss = 1;
+    }
+
+    return;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfProcessAuth               */
+/*      Process authenticate management frame.                          */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : auth frame buffer                                         */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+/* Note : AP allows one authenticating STA at a time, does not          */
+/*        support multiple authentication process. Make sure            */
+/*        authentication state machine will not be blocked due          */
+/*        to incompleted authentication handshake.                      */
+void zfApProcessAuth(zdev_t* dev, zbuf_t* buf, u16_t* src, u16_t apId)
+{
+    u16_t algo, seq, status;
+    u8_t authSharing;
+    u16_t ret;
+    u16_t i;
+    u8_t challengePassed = 0;
+    u8_t frameCtrl;
+    u32_t retAlgoSeq;
+    u32_t retStatus;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+
+    frameCtrl = zmw_rx_buf_readb(dev, buf, 1);
+    /* AP : Auth share 3 */
+    /* shift for WEP IV */
+    if ((frameCtrl & 0x40) != 0)
+    {
+        algo = zmw_rx_buf_readh(dev, buf, 28);
+        seq = zmw_rx_buf_readh(dev, buf, 30);
+        status = zmw_rx_buf_readh(dev, buf, 32);
+    }
+    else
+    {
+        algo = zmw_rx_buf_readh(dev, buf, 24);
+        seq = zmw_rx_buf_readh(dev, buf, 26);
+        status = zmw_rx_buf_readh(dev, buf, 28);
+    }
+
+    zm_msg2_mm(ZM_LV_0, "Rx Auth, seq=", seq);
+
+    /* Set default to authentication algorithm not support */
+    retAlgoSeq = 0x20000 | algo;
+    retStatus = 13; /* authentication algorithm not support */
+
+    /* AP : Auth open 1 */
+    if (algo == 0)
+    {
+        if (wd->ap.authAlgo[apId] == 0)
+        {
+            retAlgoSeq = 0x20000;
+            if (seq == 1)
+            {
+                /* AP : update STA to auth */
+                if ((ret = zfApAddSta(dev, src, ZM_STATE_AUTH, apId, 0, 0, 0)) != 0xffff)
+                {
+                    /* AP : call zfwAuthNotify() for host to judge */
+                    //zfwAuthNotify(dev, src);
+
+                    /* AP : response Auth seq=2, success */
+                    retStatus = 0;
+
+                }
+                else
+                {
+                    /* AP : response Auth seq=2, unspecific error */
+                    retStatus = 1;
+                }
+            }
+            else
+            {
+                /* AP : response Auth seq=2, sequence number out of expected */
+                retStatus = 14;
+            }
+        }
+    }
+    /* AP : Auth share 1 */
+    else if (algo == 1)
+    {
+        if (wd->ap.authAlgo[apId] == 1)
+        {
+            if (seq == 1)
+            {
+                retAlgoSeq = 0x20001;
+
+                /* critical section */
+                zmw_enter_critical_section(dev);
+                if (wd->ap.authSharing == 1)
+                {
+                    authSharing = 1;
+                }
+                else
+                {
+                    authSharing = 0;
+                    wd->ap.authSharing = 1;
+                }
+                /* end of critical section */
+                zmw_leave_critical_section(dev);
+
+                if (authSharing == 1)
+                {
+                    /* AP : response Auth seq=2, status = fail */
+                    retStatus = 1;
+                }
+                else
+                {
+                    /* AP : update STA to preauth */
+                    zfApAddSta(dev, src, ZM_STATE_PREAUTH, apId, 0, 0, 0);
+
+                    /* AP : call zfwAuthNotify() for host to judge */
+                    //zfwAuthNotify(dev, src);
+
+                    /* AP : response Auth seq=2 */
+                    retStatus = 0;
+                }
+            }
+            else if (seq == 3)
+            {
+                retAlgoSeq = 0x40001;
+
+                if (wd->ap.authSharing == 1)
+                {
+                    /* check challenge text */
+                    if (zmw_buf_readh(dev, buf, 30+4) == 0x8010)
+                    {
+                        for (i=0; i<128; i++)
+                        {
+                            if (wd->ap.challengeText[i]
+                                        != zmw_buf_readb(dev, buf, 32+i+4))
+                            {
+                                break;
+                            }
+                        }
+                        if (i == 128)
+                        {
+                            challengePassed = 1;
+                        }
+                    }
+
+                    if (challengePassed == 1)
+                    {
+                        /* AP : update STA to auth */
+                        zfApAddSta(dev, src, ZM_STATE_AUTH, apId, 0, 0, 0);
+
+                        /* AP : response Auth seq=2 */
+                        retStatus = 0;
+                    }
+                    else
+                    {
+                        /* AP : response Auth seq=2, challenge failure */
+                        retStatus = 15;
+
+                        /* TODO : delete STA */
+                    }
+
+                    wd->ap.authSharing = 0;
+                }
+            }
+            else
+            {
+                retAlgoSeq = 0x40001;
+                retStatus = 14;
+            }
+        }
+    }
+
+    zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_AUTH, src, retAlgoSeq,
+            retStatus, apId);
+    return;
+}
+
+void zfApProcessAsocReq(zdev_t* dev, zbuf_t* buf, u16_t* src, u16_t apId)
+{
+    u16_t aid = 0xffff;
+    u8_t frameType;
+    u16_t offset;
+    u8_t staType = 0;
+    u8_t qosType = 0;
+    u8_t qosInfo = 0;
+    u8_t tmp;
+    u16_t i, j, k;
+    u16_t encMode = 0;
+
+    zmw_get_wlan_dev(dev);
+    /* AP : check SSID */
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_SSID)) != 0xffff)
+    {
+        k = 0;
+        for (j = 0; j < wd->ap.vapNumber; j++)
+        {
+            if ((tmp = zmw_buf_readb(dev, buf, offset+1))
+                        != wd->ap.ssidLen[j])
+            {
+                k++;
+            }
+        }
+        if (k == wd->ap.vapNumber)
+        {
+            goto zlDeauth;
+        }
+
+        k = 0;
+        for (j = 0; j < wd->ap.vapNumber; j++)
+        {
+            for (i=0; i<wd->ap.ssidLen[j]; i++)
+            {
+                if ((tmp = zmw_buf_readb(dev, buf, offset+2+i))
+                        != wd->ap.ssid[j][i])
+                {
+                    break;
+                }
+            }
+            if (i == wd->ap.ssidLen[j])
+            {
+                apId = j;
+            }
+            else
+            {
+                k++;
+            }
+        }
+        if (k == wd->ap.vapNumber)
+        {
+            goto zlDeauth;
+        }
+    }
+
+    /* TODO : check capability */
+
+    /* AP : check support rate */
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_EXTENDED_RATE)) != 0xffff)
+    {
+        /* 11g STA */
+        staType = 1;
+    }
+    //CWYang(+)
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_HT_CAPABILITY)) != 0xffff)
+    {
+        /* 11n STA */
+        staType = 2;
+    }
+
+    /* TODO : do not allow 11b STA to associated in Pure G mode */
+    if (wd->ap.wlanType[apId] == ZM_WLAN_TYPE_PURE_G && staType == 0)
+    {
+        zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH, src, 3, 0, 0);
+        return;
+    }
+
+    /* In pure B mode, we set G STA into B mode */
+    if (wd->ap.wlanType[apId] == ZM_WLAN_TYPE_PURE_B && staType == 1)
+    {
+        staType = 0;
+    }
+
+    /* AP : check 11i and WPA */
+    /* AP : check 11h */
+
+    /* AP : check WME */
+    if ((offset = zfFindWifiElement(dev, buf, 2, 0)) != 0xffff)
+    {
+        /* WME STA */
+        qosType = 1;
+        zm_msg0_mm(ZM_LV_0, "WME STA");
+
+        if (wd->ap.uapsdEnabled != 0)
+        {
+            qosInfo = zmw_rx_buf_readb(dev, buf, offset+8);
+        }
+    }
+
+    if (wd->ap.wpaSupport[apId] == 1)
+    {
+        if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_WPA_IE)) != 0xffff )
+        {
+            /* get WPA IE */
+            u8_t length = zmw_rx_buf_readb(dev, buf, offset+1);
+            if (length+2 < ZM_MAX_WPAIE_SIZE)
+            {
+                zfCopyFromRxBuffer(dev, buf, wd->ap.stawpaIe[apId], offset, length+2);
+                wd->ap.stawpaLen[apId] = length+2;
+                encMode = 1;
+
+
+                zm_msg1_mm(ZM_LV_0, "WPA Mode zfwAsocNotify, apId=", apId);
+
+                /* AP : Call zfwAsocNotify() */
+                if (wd->zfcbAsocNotify != NULL)
+                {
+                    wd->zfcbAsocNotify(dev, src, wd->ap.stawpaIe[apId], wd->ap.stawpaLen[apId], apId);
+                }
+            }
+            else
+            {
+                goto zlDeauth;
+            }
+        }
+        else if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_RSN_IE)) != 0xffff )
+        {
+            /* get RSN IE */
+            u8_t length = zmw_rx_buf_readb(dev, buf, offset+1);
+            if (length+2 < ZM_MAX_WPAIE_SIZE)
+            {
+                zfCopyFromRxBuffer(dev, buf, wd->ap.stawpaIe[apId], offset, length+2);
+                wd->ap.stawpaLen[apId] = length+2;
+                encMode = 1;
+
+                zm_msg1_mm(ZM_LV_0, "RSN Mode zfwAsocNotify, apId=", apId);
+
+                /* AP : Call zfwAsocNotify() */
+                if (wd->zfcbAsocNotify != NULL)
+                {
+                    wd->zfcbAsocNotify(dev, src, wd->ap.stawpaIe[apId], wd->ap.stawpaLen[apId], apId);
+                }
+            }
+            else
+            {
+                goto zlDeauth;
+            }
+        }
+#ifdef ZM_ENABLE_CENC
+        else if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_CENC_IE)) != 0xffff )
+        {
+            /* get CENC IE */
+            u8_t length = zmw_rx_buf_readb(dev, buf, offset+1);
+
+            if (length+2 < ZM_MAX_WPAIE_SIZE)
+            {
+                zfCopyFromRxBuffer(dev, buf, wd->ap.stawpaIe[apId], offset, length+2);
+                wd->ap.stawpaLen[apId] = length+2;
+                encMode = 1;
+
+                zm_msg1_mm(ZM_LV_0, "CENC Mode zfwAsocNotify, apId=", apId);
+
+                /* AP : Call zfwAsocNotify() */
+                if (wd->zfcbCencAsocNotify != NULL)
+                {
+                    wd->zfcbCencAsocNotify(dev, src, wd->ap.stawpaIe[apId],
+                            wd->ap.stawpaLen[apId], apId);
+                }
+            }
+            else
+            {
+                goto zlDeauth;
+            }
+        }
+#endif //ZM_ENABLE_CENC
+        else
+        {   /* ap is encryption but sta has no wpa/rsn ie */
+            zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH, src, 6, 0, 0);
+            return;
+        }
+    }
+    /* sta has wpa/rsn ie but ap is no encryption */
+    if ((wd->ap.wpaSupport[apId] == 0) && (encMode == 1))
+    {
+        zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH, src, 6, 0, 0);
+        return;
+    }
+
+    /* AP : update STA to asoc */
+    aid = zfApAddSta(dev, src, ZM_STATE_ASOC, apId, staType, qosType, qosInfo);
+
+    zfApStoreAsocReqIe(dev, buf, aid);
+
+zlDeauth:
+    /* AP : send asoc rsp2 */
+    if (aid != 0xffff)
+    {
+        frameType = zmw_rx_buf_readb(dev, buf, 0);
+
+        if (frameType == ZM_WLAN_FRAME_TYPE_ASOCREQ)
+        {
+            zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_ASOCRSP, src, 0, aid+1, apId);
+        }
+        else
+        {
+            zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_REASOCRSP, src, 0, aid+1, apId);
+        }
+    }
+    else
+    {
+        /* TODO : send deauthentication */
+        zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH, src, 6, 0, 0);
+    }
+
+    return;
+}
+
+void zfApStoreAsocReqIe(zdev_t* dev, zbuf_t* buf, u16_t aid)
+{
+    //struct zsWlanAssoFrameHeader* pAssoFrame;
+    //u8_t  pBuf[sizeof(struct zsWlanAssoFrameHeader)];
+    u16_t offset;
+    u32_t i;
+    u16_t length;
+    u8_t  *htcap;
+
+    zmw_get_wlan_dev(dev);
+
+    for (i=0; i<wd->sta.asocRspFrameBodySize; i++)
+    {
+        wd->sta.asocRspFrameBody[i] = zmw_rx_buf_readb(dev, buf, i+24);
+    }
+    /* capability: 2 octets */
+    offset = 24;
+
+    /* Listen interval: 2 octets */
+    offset = 26;
+
+    /* SSID */
+    offset = 28;
+
+    /* supported rates */
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_SUPPORT_RATE)) == 0xffff)
+        return;
+    length = zmw_rx_buf_readb(dev, buf, offset + 1);
+
+    /* extended supported rates */
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_EXTENDED_RATE)) == 0xffff)
+        return;
+    length = zmw_rx_buf_readb(dev, buf, offset + 1);
+
+    /* power capability:4 octets */
+    offset = offset + 2 + length;
+
+    /* supported channels: 4 octets */
+    offset = offset + 2 + 4;
+
+    /* RSN */
+
+    /* QoS */
+
+    /* HT capabilities: 28 octets */
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_HT_CAPABILITY)) != 0xffff) {
+        /* atheros pre n */
+        htcap = (u8_t *)&wd->ap.ie[aid].HtCap;
+        htcap[0] = zmw_rx_buf_readb(dev, buf, offset);
+        htcap[1] = 26;
+        for (i=1; i<=26; i++)
+        {
+            htcap[i+1] = zmw_rx_buf_readb(dev, buf, offset + i);
+            zm_debug_msg2("ASOC:  HT Capabilities, htcap=", htcap[i+1]);
+        }
+        return;
+    }
+    else if ((offset = zfFindElement(dev, buf, ZM_WLAN_PREN2_EID_HTCAPABILITY)) != 0xffff) {
+        /* pre n 2.0 standard */
+        htcap = (u8_t *)&wd->ap.ie[aid].HtCap;
+        for (i=0; i<28; i++)
+        {
+            htcap[i] = zmw_rx_buf_readb(dev, buf, offset + i);
+            zm_debug_msg2("ASOC:  HT Capabilities, htcap=", htcap[i]);
+        }
+    }
+    else {
+        /* not 11n AP */
+        return;
+    }
+
+
+    /* supported regulatory classes */
+    offset = offset + length;
+    //length = zmw_rx_buf_readb(dev, buf, offset + 1);
+    {
+    u8_t *htcap;
+    htcap = (u8_t *)&wd->sta.ie.HtInfo;
+    //zm_debug_msg2("ASOC:  HT Capabilities info=", ((u16_t *)htcap)[1]);
+    //zm_debug_msg2("ASOC:  A-MPDU parameters=", htcap[4]);
+    //zm_debug_msg2("ASOC:  Supported MCS set=", ((u32_t *)htcap)[1]>>8);
+    }
+
+}
+
+void zfApProcessAsocRsp(zdev_t* dev, zbuf_t* buf)
+{
+
+}
+
+void zfApProcessDeauth(zdev_t* dev, zbuf_t* buf, u16_t* src, u16_t apId)
+{
+    u16_t aid;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    /* AP : if SA=associated STA then deauthenticate STA */
+    if ((aid = zfApFindSta(dev, src)) != 0xffff)
+    {
+        /* Clear STA table */
+        wd->ap.staTable[aid].valid = 0;
+        if (wd->zfcbDisAsocNotify != NULL)
+        {
+            wd->zfcbDisAsocNotify(dev, (u8_t*)src, apId);
+        }
+    }
+    zmw_leave_critical_section(dev);
+
+}
+
+void zfApProcessDisasoc(zdev_t* dev, zbuf_t* buf, u16_t* src, u16_t apId)
+{
+    u16_t aid;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    /* AP : if SA=associated STA then deauthenticate STA */
+    if ((aid = zfApFindSta(dev, src)) != 0xffff)
+    {
+        /* Clear STA table */
+        wd->ap.staTable[aid].valid = 0;
+        zmw_leave_critical_section(dev);
+        if (wd->zfcbDisAsocNotify != NULL)
+        {
+            wd->zfcbDisAsocNotify(dev, (u8_t*)src, apId);
+        }
+    }
+    zmw_leave_critical_section(dev);
+
+}
+
+
+void zfApProcessProbeRsp(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* AddInfo)
+{
+#if 0
+    zmw_get_wlan_dev(dev);
+
+    zm_msg0_mm(ZM_LV_0, "Rx probersp");
+
+    /* Gather scan result */
+
+    //zm_debug_msg1("bssList Count = ", wd->sta.bssList.bssCount);
+    /* return if not in scanning */
+    if ((wd->heartBeatNotification & ZM_BSSID_LIST_SCAN)
+            != ZM_BSSID_LIST_SCAN)
+    {
+        return;
+    }
+
+    //if ( wd->sta.pUpdateBssList->bssCount == ZM_MAX_BSS )
+    if ( wd->sta.bssList.bssCount == ZM_MAX_BSS )
+    {
+        return;
+    }
+
+    zfProcessProbeRsp(dev, buf, AddInfo);
+
+#endif
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfApAddIeSsid               */
+/*      Add AP information element SSID to buffer.                      */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*      vap : virtual AP ID                                             */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.11     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfApAddIeSsid(zdev_t* dev, zbuf_t* buf, u16_t offset, u16_t vap)
+{
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    /* Element ID */
+    zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_SSID);
+
+    /* Element Length */
+    zmw_tx_buf_writeb(dev, buf, offset++, wd->ap.ssidLen[vap]);
+
+    /* Information : SSID */
+    for (i=0; i<wd->ap.ssidLen[vap]; i++)
+    {
+        zmw_tx_buf_writeb(dev, buf, offset++, wd->ap.ssid[vap][i]);
+    }
+
+    return offset;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfApAddIeTim                */
+/*      Add AP information element TIM to buffer.                       */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*      vap : virtual AP ID                                             */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.11     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfApAddIeTim(zdev_t* dev, zbuf_t* buf, u16_t offset, u16_t vap)
+{
+    u8_t uniBitMap[9];
+    u16_t highestByte;
+    u16_t i;
+    u16_t lenOffset;
+    u16_t id;
+    u16_t dst[3];
+    u16_t aid;
+    u16_t bitPosition;
+    u16_t bytePosition;
+    zbuf_t* psBuf;
+    zbuf_t* tmpBufArray[ZM_UNI_ARRAY_SIZE];
+    u16_t tmpBufArraySize = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    /* Element ID */
+    zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_TIM);
+
+    /* offset of Element Length */
+    lenOffset = offset++;
+
+    /* Information : TIM */
+    /* DTIM count */
+    /* TODO : Doesn't work for Virtual AP's case */
+    wd->CurrentDtimCount++;
+    if (wd->CurrentDtimCount >= wd->dtim)
+    {
+        wd->CurrentDtimCount = 0;
+    }
+    zmw_tx_buf_writeb(dev, buf, offset++, wd->CurrentDtimCount);
+    /* DTIM period */
+    zmw_tx_buf_writeb(dev, buf, offset++, wd->dtim);
+    /* bitmap offset */
+    zmw_tx_buf_writeb(dev, buf, offset++, 0);
+
+    /* Update BCMC bit */
+    if (wd->CurrentDtimCount == 0)
+    {
+        zmw_enter_critical_section(dev);
+        wd->ap.timBcmcBit[vap] = (wd->ap.bcmcTail[vap]!=wd->ap.bcmcHead[vap])?1:0;
+        zmw_leave_critical_section(dev);
+    }
+    else
+    {
+        wd->ap.timBcmcBit[vap] = 0;
+    }
+
+    /* Update Unicast bitmap */
+    /* reset bit map */
+    for (i=0; i<9; i++)
+    {
+        uniBitMap[i] = 0;
+    }
+    highestByte = 0;
+#if 1
+
+    zmw_enter_critical_section(dev);
+
+    id = wd->ap.uniHead;
+    while (id != wd->ap.uniTail)
+    {
+        psBuf = wd->ap.uniArray[id];
+
+        /* TODO : Aging PS frame after queuing for more than 10 seconds */
+
+        /* get destination STA's aid */
+        dst[0] = zmw_tx_buf_readh(dev, psBuf, 0);
+        dst[1] = zmw_tx_buf_readh(dev, psBuf, 2);
+        dst[2] = zmw_tx_buf_readh(dev, psBuf, 4);
+        if ((aid = zfApFindSta(dev, dst)) != 0xffff)
+        {
+            if (wd->ap.staTable[aid].psMode != 0)
+            {
+                zm_msg1_mm(ZM_LV_0, "aid=",aid);
+                aid++;
+                zm_assert(aid<=64);
+                bitPosition = (1 << (aid & 0x7));
+                bytePosition = (aid >> 3);
+                uniBitMap[bytePosition] |= bitPosition;
+
+                if (bytePosition>highestByte)
+                {
+                    highestByte = bytePosition;
+                }
+                id = (id+1) & (ZM_UNI_ARRAY_SIZE-1);
+            }
+            else
+            {
+                zm_msg0_mm(ZM_LV_0, "Send PS frame which STA no longer in PS mode");
+                /* Send PS frame which STA no longer in PS mode */
+                zfApRemoveFromPsQueue(dev, id, dst);
+                tmpBufArray[tmpBufArraySize++] = psBuf;
+            }
+        }
+        else
+        {
+            zm_msg0_mm(ZM_LV_0, "Free garbage PS frame");
+            /* Free garbage PS frame */
+            zfApRemoveFromPsQueue(dev, id, dst);
+            zfwBufFree(dev, psBuf, 0);
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+#endif
+
+    zfQueueGenerateUapsdTim(dev, wd->ap.uapsdQ, uniBitMap, &highestByte);
+
+    zm_msg1_mm(ZM_LV_3, "bm=",uniBitMap[0]);
+    zm_msg1_mm(ZM_LV_3, "highestByte=",highestByte);
+    zm_msg1_mm(ZM_LV_3, "timBcmcBit[]=",wd->ap.timBcmcBit[vap]);
+
+    /* bitmap */
+    zmw_tx_buf_writeb(dev, buf, offset++,
+                         uniBitMap[0] | wd->ap.timBcmcBit[vap]);
+    for (i=0; i<highestByte; i++)
+    {
+        zmw_tx_buf_writeb(dev, buf, offset++, uniBitMap[i+1]);
+    }
+
+    /* Element Length */
+    zmw_tx_buf_writeb(dev, buf, lenOffset, highestByte+4);
+
+    for (i=0; i<tmpBufArraySize; i++)
+    {
+        /* Put to VTXQ[ac] */
+        zfPutVtxq(dev, tmpBufArray[i]);
+    }
+    /* Push VTXQ[ac] */
+    zfPushVtxq(dev);
+
+    return offset;
+}
+
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfApRemoveFromPsQueue       */
+/*      Remove zbuf from PS queue.                                      */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      id : index in ps queue                                          */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      more data bit                                                   */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.1      */
+/*                                                                      */
+/************************************************************************/
+u8_t zfApRemoveFromPsQueue(zdev_t* dev, u16_t id, u16_t* addr)
+{
+    u16_t dst[3];
+    u16_t nid;
+    u8_t moreData = 0;
+    zmw_get_wlan_dev(dev);
+
+    wd->ap.uniTail = (wd->ap.uniTail-1) & (ZM_UNI_ARRAY_SIZE-1);
+    while (id != wd->ap.uniTail)
+    {
+        nid = (id + 1) & (ZM_UNI_ARRAY_SIZE - 1);
+        wd->ap.uniArray[id] = wd->ap.uniArray[nid];
+
+        /* Search until tail to config more data bit */
+        dst[0] = zmw_buf_readh(dev, wd->ap.uniArray[id], 0);
+        dst[1] = zmw_buf_readh(dev, wd->ap.uniArray[id], 2);
+        dst[2] = zmw_buf_readh(dev, wd->ap.uniArray[id], 4);
+        if ((addr[0] == dst[0]) && (addr[1] == dst[1])
+                && (addr[2] == dst[2]))
+        {
+            moreData = 0x20;
+        }
+
+        id = nid;
+    }
+    return moreData;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfApAddIeWmePara            */
+/*      Add WME Parameter Element to buffer.                            */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*      vap : virtual AP ID                                             */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2006.1      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfApAddIeWmePara(zdev_t* dev, zbuf_t* buf, u16_t offset, u16_t vap)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* Element ID */
+    zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_WIFI_IE);
+
+    /* Element Length */
+    zmw_tx_buf_writeb(dev, buf, offset++, 24);
+
+    /* OUI */
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x00);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x50);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0xF2);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x02);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x01);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x01);
+
+    /* QoS Info */
+    if (wd->ap.uapsdEnabled)
+    {
+        zmw_tx_buf_writeb(dev, buf, offset++, 0x81);
+    }
+    else
+    {
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x01);
+    }
+
+    /* Reserved */
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x00);
+
+    /* Best Effort AC parameters */
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x03);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0xA4);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x00);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x00);
+    /* Backfround AC parameters */
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x27);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0xA4);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x00);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x00);
+    /* Video AC parameters */
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x42);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x43);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x5E);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x00);
+    /* Voice AC parameters */
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x62);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x32);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x2F);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x00);
+
+    return offset;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfApSendBeacon              */
+/*      Sned AP mode beacon.                                            */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.11     */
+/*                                                                      */
+/************************************************************************/
+void zfApSendBeacon(zdev_t* dev)
+{
+    zbuf_t* buf;
+    u16_t offset;
+    u16_t vap;
+    u16_t seq;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    wd->ap.beaconCounter++;
+    if (wd->ap.beaconCounter >= wd->ap.vapNumber)
+    {
+        wd->ap.beaconCounter = 0;
+    }
+    vap = wd->ap.beaconCounter;
+
+
+    zm_msg1_mm(ZM_LV_2, "Send beacon, vap=", vap);
+
+    /* TBD : Maximum size of beacon */
+    if ((buf = zfwBufAllocate(dev, 1024)) == NULL)
+    {
+        zm_msg0_mm(ZM_LV_0, "Alloc beacon buf Fail!");
+        return;
+    }
+
+    offset = 0;
+
+    /* wlan header */
+    /* Frame control */
+    zmw_tx_buf_writeh(dev, buf, offset, 0x0080);
+    offset+=2;
+    /* Duration */
+    zmw_tx_buf_writeh(dev, buf, offset, 0x0000);
+    offset+=2;
+    /* Address 1 */
+    zmw_tx_buf_writeh(dev, buf, offset, 0xffff);
+    offset+=2;
+    zmw_tx_buf_writeh(dev, buf, offset, 0xffff);
+    offset+=2;
+    zmw_tx_buf_writeh(dev, buf, offset, 0xffff);
+    offset+=2;
+    /* Address 2 */
+    zmw_tx_buf_writeh(dev, buf, offset, wd->macAddr[0]);
+    offset+=2;
+    zmw_tx_buf_writeh(dev, buf, offset, wd->macAddr[1]);
+    offset+=2;
+#ifdef ZM_VAPMODE_MULTILE_SSID
+    zmw_tx_buf_writeh(dev, buf, offset, wd->macAddr[2]); //Multiple SSID
+#else
+    zmw_tx_buf_writeh(dev, buf, offset, (wd->macAddr[2]+(vap<<8))); //VAP
+#endif
+    offset+=2;
+    /* Address 3 */
+    zmw_tx_buf_writeh(dev, buf, offset, wd->macAddr[0]);
+    offset+=2;
+    zmw_tx_buf_writeh(dev, buf, offset, wd->macAddr[1]);
+    offset+=2;
+#ifdef ZM_VAPMODE_MULTILE_SSID
+    zmw_tx_buf_writeh(dev, buf, offset, wd->macAddr[2]); //Multiple SSID
+#else
+    zmw_tx_buf_writeh(dev, buf, offset, (wd->macAddr[2]+(vap<<8))); //VAP
+#endif
+    offset+=2;
+
+    /* Sequence number */
+    zmw_enter_critical_section(dev);
+    seq = ((wd->mmseq++)<<4);
+    zmw_leave_critical_section(dev);
+    zmw_tx_buf_writeh(dev, buf, offset, seq);
+    offset+=2;
+
+    /* 24-31 Time Stamp : hardware will fill this field */
+    zmw_tx_buf_writeh(dev, buf, offset, 0);
+    zmw_tx_buf_writeh(dev, buf, offset+2, 0);
+    zmw_tx_buf_writeh(dev, buf, offset+4, 0);
+    zmw_tx_buf_writeh(dev, buf, offset+6, 0);
+    offset+=8;
+
+    /* Beacon Interval */
+    zmw_tx_buf_writeh(dev, buf, offset, wd->beaconInterval);
+    offset+=2;
+
+    /* Capability */
+    zmw_tx_buf_writeh(dev, buf, offset, wd->ap.capab[vap]);
+    offset+=2;
+
+    /* SSID */
+    if (wd->ap.hideSsid[vap] == 0)
+    {
+        offset = zfApAddIeSsid(dev, buf, offset, vap);
+    }
+    else
+    {
+        zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_SSID);
+        zmw_tx_buf_writeb(dev, buf, offset++, 0);
+
+    }
+
+    /* Support Rate */
+    if ( wd->frequency < 3000 )
+    {
+    offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                  ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_CCK);
+    }
+    else
+    {
+        offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                  ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_OFDM);
+    }
+
+    /* DS parameter set */
+    offset = zfMmAddIeDs(dev, buf, offset);
+
+    /* TIM */
+    offset = zfApAddIeTim(dev, buf, offset, vap);
+
+    /* If WLAN Type is not PURE B */
+    if (wd->ap.wlanType[vap] != ZM_WLAN_TYPE_PURE_B)
+    {
+        if ( wd->frequency < 3000 )
+        {
+        /* ERP Information */
+        offset = zfMmAddIeErp(dev, buf, offset);
+
+        /* Extended Supported Rates */
+        offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                      ZM_WLAN_EID_EXTENDED_RATE, ZM_RATE_SET_OFDM);
+    }
+    }
+
+    /* TODO : country information */
+    /* TODO : RSN */
+    if (wd->ap.wpaSupport[vap] == 1)
+    {
+        offset = zfMmAddIeWpa(dev, buf, offset, vap);
+    }
+
+    /* WME Parameters */
+    if (wd->ap.qosMode == 1)
+    {
+        offset = zfApAddIeWmePara(dev, buf, offset, vap);
+    }
+
+    /* HT Capabilities Info */
+    offset = zfMmAddHTCapability(dev, buf, offset);
+
+    /* Extended HT Capabilities Info */
+    offset = zfMmAddExtendedHTCapability(dev, buf, offset);
+
+    /* 1212 : write to beacon fifo */
+    /* 1221 : write to share memory */
+    zfHpSendBeacon(dev, buf, offset);
+
+    /* Free beacon buffer */
+    /* TODO: In order to fit the madwifi beacon architecture, we need to
+       free beacon buffer in the HAL layer.
+     */
+
+    //zfwBufFree(dev, buf, 0);
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfIntrabssForward           */
+/*      Called to transmit intra-BSS frame from upper layer.            */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer pointer                                            */
+/*      vap : virtual AP                                                */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      1 : unicast intras-BSS frame                                    */
+/*      0 : other frames                                                */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen             ZyDAS Technology Corporation    2005.11     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfIntrabssForward(zdev_t* dev, zbuf_t* buf, u8_t srcVap)
+{
+    u16_t err;
+    u16_t asocFlag = 0;
+    u16_t dst[3];
+    u16_t aid;
+    u16_t staState;
+    zbuf_t* txBuf;
+    u16_t len;
+    u16_t i;
+    u16_t temp;
+    u16_t ret;
+    u8_t vap = 0;
+#ifdef ZM_ENABLE_NATIVE_WIFI
+    dst[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET);
+    dst[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET+2);
+    dst[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET+4);
+#else
+    dst[0] = zmw_rx_buf_readh(dev, buf, 0);
+    dst[1] = zmw_rx_buf_readh(dev, buf, 2);
+    dst[2] = zmw_rx_buf_readh(dev, buf, 4);
+#endif  // ZM_ENABLE_NATIVE_WIFI
+
+    /* Do Intra-BSS forward(data copy) if necessary*/
+    if ((dst[0]&0x1) != 0x1)
+    {
+        aid = zfApGetSTAInfo(dev, dst, &staState, &vap);
+        if ((aid != 0xffff) && (staState == ZM_STATE_ASOC) && (srcVap == vap))
+        {
+            asocFlag = 1;
+            zm_msg0_rx(ZM_LV_2, "Intra-BSS forward : asoc STA");
+        }
+
+    }
+    else
+    {
+        vap = srcVap;
+        zm_msg0_rx(ZM_LV_2, "Intra-BSS forward : BCorMC");
+    }
+
+    /* destination address = associated STA or BC/MC */
+    if ((asocFlag == 1) || ((dst[0]&0x1) == 0x1))
+    {
+        /* Allocate frame */
+        if ((txBuf = zfwBufAllocate(dev, ZM_RX_FRAME_SIZE))
+                == NULL)
+        {
+            zm_msg0_rx(ZM_LV_1, "Alloc intra-bss buf Fail!");
+            goto zlAllocError;
+        }
+
+        /* Copy frame */
+        len = zfwBufGetSize(dev, buf);
+        for (i=0; i<len; i+=2)
+        {
+            temp = zmw_rx_buf_readh(dev, buf, i);
+            zmw_tx_buf_writeh(dev, txBuf, i, temp);
+        }
+        zfwBufSetSize(dev, txBuf, len);
+
+#ifdef ZM_ENABLE_NATIVE_WIFI
+        /* Tx-A2 = Rx-A1, Tx-A3 = Rx-A2, Tx-A1 = Rx-A3 */
+        for (i=0; i<6; i+=2)
+        {
+            temp = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET+i);
+            zmw_tx_buf_writeh(dev, txBuf, ZM_WLAN_HEADER_A2_OFFSET+i, temp);
+            temp = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+i);
+            zmw_tx_buf_writeh(dev, txBuf, ZM_WLAN_HEADER_A3_OFFSET+i, temp);
+            temp = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET+i);
+            zmw_tx_buf_writeh(dev, txBuf, ZM_WLAN_HEADER_A1_OFFSET+i, temp);
+        }
+
+        #endif
+
+        /* Transmit frame */
+        /* Return error if port is disabled */
+        if ((err = zfTxPortControl(dev, txBuf, vap)) == ZM_PORT_DISABLED)
+        {
+            err = ZM_ERR_TX_PORT_DISABLED;
+            goto zlTxError;
+        }
+
+#if 1
+        /* AP : Buffer frame for power saving STA */
+        if ((ret = zfApBufferPsFrame(dev, txBuf, vap)) == 0)
+        {
+            /* forward frame if not been buffered */
+            #if 1
+            /* Put to VTXQ[ac] */
+            ret = zfPutVtxq(dev, txBuf);
+            /* Push VTXQ[ac] */
+            zfPushVtxq(dev);
+            #else
+            zfTxSendEth(dev, txBuf, vap, ZM_INTERNAL_ALLOC_BUF, 0);
+            #endif
+
+        }
+#endif
+    }
+    return asocFlag;
+
+zlTxError:
+    zfwBufFree(dev, txBuf, 0);
+zlAllocError:
+    return asocFlag;
+}
+
+struct zsMicVar* zfApGetRxMicKey(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t sa[6];
+    u16_t id = 0, macAddr[3];
+
+    zmw_get_wlan_dev(dev);
+
+    zfCopyFromRxBuffer(dev, buf, sa, ZM_WLAN_HEADER_A2_OFFSET, 6);
+
+    macAddr[0] = sa[0] + (sa[1] << 8);
+    macAddr[1] = sa[2] + (sa[3] << 8);
+    macAddr[2] = sa[4] + (sa[5] << 8);
+
+    if ((id = zfApFindSta(dev, macAddr)) != 0xffff)
+        return (&wd->ap.staTable[id].rxMicKey);
+
+    return NULL;
+}
+
+struct zsMicVar* zfApGetTxMicKey(zdev_t* dev, zbuf_t* buf, u8_t* qosType)
+{
+    u8_t da[6];
+    u16_t id = 0, macAddr[3];
+
+    zmw_get_wlan_dev(dev);
+
+    zfCopyFromIntTxBuffer(dev, buf, da, 0, 6);
+
+    macAddr[0] = da[0] + (da[1] << 8);
+    macAddr[1] = da[2] + (da[3] << 8);
+    macAddr[2] = da[4] + (da[5] << 8);
+
+    if ((macAddr[0] & 0x1))
+    {
+        return (&wd->ap.bcMicKey[0]);
+    }
+    else if ((id = zfApFindSta(dev, macAddr)) != 0xffff)
+    {
+        *qosType = wd->ap.staTable[id].qosType;
+        return (&wd->ap.staTable[id].txMicKey);
+    }
+
+    return NULL;
+}
+
+u16_t zfApUpdatePsBit(zdev_t* dev, zbuf_t* buf, u8_t* vap, u8_t* uapsdTrig)
+{
+    u16_t staState;
+    u16_t aid;
+    u16_t psBit;
+    u16_t src[3];
+    u16_t dst[1];
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    src[0] = zmw_rx_buf_readh(dev, buf, 10);
+    src[1] = zmw_rx_buf_readh(dev, buf, 12);
+    src[2] = zmw_rx_buf_readh(dev, buf, 14);
+
+    if ((zmw_rx_buf_readb(dev, buf, 1) & 0x3) != 3)
+    {
+        /* AP */
+        dst[0] = zmw_rx_buf_readh(dev, buf, 4);
+
+        psBit = (zmw_rx_buf_readb(dev, buf, 1) & 0x10) >> 4;
+        /* Get AID and update STA PS mode */
+        aid = zfApGetSTAInfoAndUpdatePs(dev, src, &staState, vap, psBit, uapsdTrig);
+
+        /* if STA not associated, send deauth */
+        if ((aid == 0xffff) || (staState != ZM_STATE_ASOC))
+        {
+            if ((dst[0]&0x1)==0)
+            {
+                zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH, src, 0x7,
+                        0, 0);
+            }
+
+            return ZM_ERR_STA_NOT_ASSOCIATED;
+        }
+    } /* if ((zmw_rx_buf_readb(dev, buf, 1) & 0x3) != 3) */
+    else
+    {
+        /* WDS */
+        for (i=0; i<ZM_MAX_WDS_SUPPORT; i++)
+        {
+            if ((wd->ap.wds.wdsBitmap & (1<<i)) != 0)
+            {
+                if ((src[0] == wd->ap.wds.macAddr[i][0])
+                        && (src[1] == wd->ap.wds.macAddr[i][1])
+                        && (src[2] == wd->ap.wds.macAddr[i][2]))
+                {
+                    *vap = 0x20 + i;
+                    break;
+                }
+            }
+        }
+    }
+    return ZM_SUCCESS;
+}
+
+void zfApProcessPsPoll(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t src[3];
+    u16_t dst[3];
+    zbuf_t* psBuf = NULL;
+    u16_t id;
+    u8_t moreData = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    src[0] = zmw_tx_buf_readh(dev, buf, 10);
+    src[1] = zmw_tx_buf_readh(dev, buf, 12);
+    src[2] = zmw_tx_buf_readh(dev, buf, 14);
+
+    /* Find ps buffer for PsPoll */
+    zmw_enter_critical_section(dev);
+    id = wd->ap.uniHead;
+    while (id != wd->ap.uniTail)
+    {
+        psBuf = wd->ap.uniArray[id];
+
+        dst[0] = zmw_tx_buf_readh(dev, psBuf, 0);
+        dst[1] = zmw_tx_buf_readh(dev, psBuf, 2);
+        dst[2] = zmw_tx_buf_readh(dev, psBuf, 4);
+
+        if ((src[0] == dst[0]) && (src[1] == dst[1]) && (src[2] == dst[2]))
+        {
+            moreData = zfApRemoveFromPsQueue(dev, id, src);
+            break;
+        }
+        else
+        {
+            psBuf = NULL;
+        }
+        id = (id + 1) & (ZM_UNI_ARRAY_SIZE - 1);
+    }
+    zmw_leave_critical_section(dev);
+
+    /* Send ps buffer */
+    if (psBuf != NULL)
+    {
+        /* Send with more data bit */
+        zfTxSendEth(dev, psBuf, 0, ZM_EXTERNAL_ALLOC_BUF, moreData);
+    }
+
+    return;
+}
+
+void zfApSetProtectionMode(zdev_t* dev, u16_t mode)
+{
+    zmw_get_wlan_dev(dev);
+
+    if (mode == 0)
+    {
+        if (wd->ap.protectionMode != mode)
+        {
+            /* Write MAC&PHY registers to disable protection */
+
+            wd->ap.protectionMode = mode;
+        }
+
+    }
+    else
+    {
+        if (wd->ap.protectionMode != mode)
+        {
+            /* Write MAC&PHY registers to enable protection */
+
+            wd->ap.protectionMode = mode;
+        }
+    }
+    return;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfApSendFailure             */
+/*      Send failure.                                                   */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      addr : receiver address                                         */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.1      */
+/*                                                                      */
+/************************************************************************/
+void zfApSendFailure(zdev_t* dev, u8_t* addr)
+{
+    u16_t id;
+    u16_t staAddr[3];
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    staAddr[0] = addr[0] + (((u16_t)addr[1])<<8);
+    staAddr[1] = addr[2] + (((u16_t)addr[3])<<8);
+    staAddr[2] = addr[4] + (((u16_t)addr[5])<<8);
+    zmw_enter_critical_section(dev);
+    if ((id = zfApFindSta(dev, staAddr)) != 0xffff)
+    {
+        /* Send failture : Add 3 minutes to inactive time that will */
+        /*                 will make STA been kicked out soon */
+        wd->ap.staTable[id].time -= (3*ZM_TICK_PER_MINUTE);
+    }
+    zmw_leave_critical_section(dev);
+}
+
+
+void zfApProcessAction(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t category;
+
+    //zmw_get_wlan_dev(dev);
+
+    //zmw_declare_for_critical_section();
+
+    category = zmw_rx_buf_readb(dev, buf, 24);
+
+    switch (category)
+    {
+    case ZM_WLAN_BLOCK_ACK_ACTION_FRAME:
+        zfAggBlockAckActionFrame(dev, buf);
+        break;
+    default:
+        break;
+    }
+
+    return;
+}
diff --git a/drivers/staging/otus/80211core/cmmsta.c b/drivers/staging/otus/80211core/cmmsta.c
new file mode 100644
index 00000000000..c75ba11ee43
--- /dev/null
+++ b/drivers/staging/otus/80211core/cmmsta.c
@@ -0,0 +1,5782 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include "cprecomp.h"
+#include "ratectrl.h"
+#include "../hal/hpreg.h"
+
+/* TODO : change global variable to constant */
+u8_t   zgWpaRadiusOui[] = { 0x00, 0x50, 0xf2, 0x01 };
+u8_t   zgWpaAesOui[] = { 0x00, 0x50, 0xf2, 0x04 };
+u8_t   zgWpa2RadiusOui[] = { 0x00, 0x0f, 0xac, 0x01 };
+u8_t   zgWpa2AesOui[] = { 0x00, 0x0f, 0xac, 0x04 };
+
+const u16_t zcCwTlb[16] = {   0,    1,    3,    7,   15,   31,   63,  127,
+                            255,  511, 1023, 2047, 4095, 4095, 4095, 4095};
+
+void zfStaStartConnectCb(zdev_t* dev);
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfStaPutApIntoBlockingList  */
+/*      Put AP into blocking AP list.                                   */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      bssid : AP's BSSID                                              */
+/*      weight : weight of AP                                           */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+void zfStaPutApIntoBlockingList(zdev_t* dev, u8_t* bssid, u8_t weight)
+{
+    u16_t i, j;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    if (weight > 0)
+    {
+        zmw_enter_critical_section(dev);
+        /*Find same bssid entry first*/
+        for (i=0; i<ZM_MAX_BLOCKING_AP_LIST_SIZE; i++)
+        {
+            for (j=0; j<6; j++)
+            {
+                if(wd->sta.blockingApList[i].addr[j]!= bssid[j])
+                {
+                    break;
+                }
+            }
+
+            if(j==6)
+            {
+                break;
+            }
+        }
+        /*This bssid doesn't have old record.Find an empty entry*/
+        if (i == ZM_MAX_BLOCKING_AP_LIST_SIZE)
+        {
+            for (i=0; i<ZM_MAX_BLOCKING_AP_LIST_SIZE; i++)
+            {
+                if (wd->sta.blockingApList[i].weight == 0)
+                {
+                    break;
+                }
+            }
+        }
+
+        /* If the list is full, pick one entry for replacement */
+        if (i == ZM_MAX_BLOCKING_AP_LIST_SIZE)
+        {
+            i = bssid[5] & (ZM_MAX_BLOCKING_AP_LIST_SIZE-1);
+        }
+
+        /* Update AP address and weight */
+        for (j=0; j<6; j++)
+        {
+            wd->sta.blockingApList[i].addr[j] = bssid[j];
+        }
+
+        wd->sta.blockingApList[i].weight = weight;
+        zmw_leave_critical_section(dev);
+    }
+
+    return;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfStaIsApInBlockingList     */
+/*      Is AP in blocking list.                                         */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      bssid : AP's BSSID                                              */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      TRUE : AP in blocking list                                      */
+/*      FALSE : AP not in blocking list                                 */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfStaIsApInBlockingList(zdev_t* dev, u8_t* bssid)
+{
+    u16_t i, j;
+    zmw_get_wlan_dev(dev);
+    //zmw_declare_for_critical_section();
+
+    //zmw_enter_critical_section(dev);
+    for (i=0; i<ZM_MAX_BLOCKING_AP_LIST_SIZE; i++)
+    {
+        if (wd->sta.blockingApList[i].weight != 0)
+        {
+            for (j=0; j<6; j++)
+            {
+                if (wd->sta.blockingApList[i].addr[j] != bssid[j])
+                {
+                    break;
+                }
+            }
+            if (j == 6)
+            {
+                //zmw_leave_critical_section(dev);
+                return TRUE;
+            }
+        }
+    }
+    //zmw_leave_critical_section(dev);
+    return FALSE;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfStaRefreshBlockList       */
+/*      Is AP in blocking list.                                         */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      flushFlag : flush whole blocking list                           */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+void zfStaRefreshBlockList(zdev_t* dev, u16_t flushFlag)
+{
+    u16_t i;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    for (i=0; i<ZM_MAX_BLOCKING_AP_LIST_SIZE; i++)
+    {
+        if (wd->sta.blockingApList[i].weight != 0)
+        {
+            if (flushFlag != 0)
+            {
+                wd->sta.blockingApList[i].weight = 0;
+            }
+            else
+            {
+                wd->sta.blockingApList[i].weight--;
+            }
+        }
+    }
+    zmw_leave_critical_section(dev);
+    return;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfStaConnectFail            */
+/*      Handle Connect failure.                                         */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      bssid : BSSID                                                   */
+/*      reason : reason of failure                                      */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+void zfStaConnectFail(zdev_t* dev, u16_t reason, u16_t* bssid, u8_t weight)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* Change internal state */
+    zfChangeAdapterState(dev, ZM_STA_STATE_DISCONNECT);
+
+    /* Improve WEP/TKIP performace with HT AP, detail information please look bug#32495 */
+    //zfHpSetTTSIFSTime(dev, 0x8);
+
+    /* Notify wrapper of connection status changes */
+    if (wd->zfcbConnectNotify != NULL)
+    {
+        wd->zfcbConnectNotify(dev, reason, bssid);
+    }
+
+    /* Put AP into internal blocking list */
+    zfStaPutApIntoBlockingList(dev, (u8_t *)bssid, weight);
+
+    /* Issue another SCAN */
+    if ( wd->sta.bAutoReconnect )
+    {
+        zm_debug_msg0("Start internal scan...");
+        zfScanMgrScanStop(dev, ZM_SCAN_MGR_SCAN_INTERNAL);
+        zfScanMgrScanStart(dev, ZM_SCAN_MGR_SCAN_INTERNAL);
+    }
+}
+
+u8_t zfiWlanIBSSGetPeerStationsCount(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->sta.oppositeCount;
+}
+
+u8_t zfiWlanIBSSIteratePeerStations(zdev_t* dev, u8_t numToIterate, zfpIBSSIteratePeerStationCb callback, void *ctx)
+{
+    u8_t oppositeCount;
+    u8_t i;
+    u8_t index = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    oppositeCount = wd->sta.oppositeCount;
+    if ( oppositeCount > numToIterate )
+    {
+        oppositeCount = numToIterate;
+    }
+
+    for(i=0; i < ZM_MAX_OPPOSITE_COUNT; i++)
+    {
+        if ( oppositeCount == 0 )
+        {
+            break;
+        }
+
+        if ( wd->sta.oppositeInfo[i].valid == 0 )
+        {
+            continue;
+        }
+
+        callback(dev, &wd->sta.oppositeInfo[i], ctx, index++);
+        oppositeCount--;
+
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return index;
+}
+
+
+s8_t zfStaFindFreeOpposite(zdev_t* dev, u16_t *sa, int *pFoundIdx)
+{
+    int oppositeCount;
+    int i;
+
+    zmw_get_wlan_dev(dev);
+
+    oppositeCount = wd->sta.oppositeCount;
+
+    for(i=0; i < ZM_MAX_OPPOSITE_COUNT; i++)
+    {
+        if ( oppositeCount == 0 )
+        {
+            break;
+        }
+
+        if ( wd->sta.oppositeInfo[i].valid == 0 )
+        {
+            continue;
+        }
+
+        oppositeCount--;
+        if ( zfMemoryIsEqual((u8_t*) sa, wd->sta.oppositeInfo[i].macAddr, 6) )
+        {
+            //wd->sta.oppositeInfo[i].aliveCounter++;
+            wd->sta.oppositeInfo[i].aliveCounter = ZM_IBSS_PEER_ALIVE_COUNTER;
+
+            /* it is already stored */
+            return 1;
+        }
+    }
+
+    // Check if there's still space for new comer
+    if ( wd->sta.oppositeCount == ZM_MAX_OPPOSITE_COUNT )
+    {
+        return -1;
+    }
+
+    // Find an unused slot for new peer station
+    for(i=0; i < ZM_MAX_OPPOSITE_COUNT; i++)
+    {
+        if ( wd->sta.oppositeInfo[i].valid == 0 )
+        {
+            break;
+        }
+    }
+
+    *pFoundIdx = i;
+    return 0;
+}
+
+s8_t zfStaFindOppositeByMACAddr(zdev_t* dev, u16_t *sa, u8_t *pFoundIdx)
+{
+    u32_t oppositeCount;
+    u32_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    oppositeCount = wd->sta.oppositeCount;
+
+    for(i=0; i < ZM_MAX_OPPOSITE_COUNT; i++)
+    {
+        if ( oppositeCount == 0 )
+        {
+            break;
+        }
+
+        if ( wd->sta.oppositeInfo[i].valid == 0 )
+        {
+            continue;
+        }
+
+        oppositeCount--;
+        if ( zfMemoryIsEqual((u8_t*) sa, wd->sta.oppositeInfo[i].macAddr, 6) )
+        {
+            *pFoundIdx = (u8_t)i;
+
+            return 0;
+        }
+    }
+
+    *pFoundIdx = 0;
+    return 1;
+}
+
+static void zfStaInitCommonOppositeInfo(zdev_t* dev, int i)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* set the default rate to the highest rate */
+    wd->sta.oppositeInfo[i].valid = 1;
+    wd->sta.oppositeInfo[i].aliveCounter = ZM_IBSS_PEER_ALIVE_COUNTER;
+    wd->sta.oppositeCount++;
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+    /* Set parameters for new opposite peer station !!! */
+    wd->sta.oppositeInfo[i].camIdx = 0xff;  // Not set key in this location
+    wd->sta.oppositeInfo[i].pkInstalled = 0;
+    wd->sta.oppositeInfo[i].wpaState = ZM_STA_WPA_STATE_INIT ;  // No encryption
+#endif
+}
+
+int zfStaSetOppositeInfoFromBSSInfo(zdev_t* dev, struct zsBssInfo* pBssInfo)
+{
+    int i;
+    u8_t*  dst;
+    u16_t  sa[3];
+    int res;
+    u32_t oneTxStreamCap;
+
+    zmw_get_wlan_dev(dev);
+
+    zfMemoryCopy((u8_t*) sa, pBssInfo->macaddr, 6);
+
+    res = zfStaFindFreeOpposite(dev, sa, &i);
+    if ( res != 0 )
+    {
+        goto zlReturn;
+    }
+
+    dst = wd->sta.oppositeInfo[i].macAddr;
+    zfMemoryCopy(dst, (u8_t *)sa, 6);
+
+    oneTxStreamCap = (zfHpCapability(dev) & ZM_HP_CAP_11N_ONE_TX_STREAM);
+
+    if (pBssInfo->extSupportedRates[1] != 0)
+    {
+        /* TODO : Handle 11n */
+        if (pBssInfo->frequency < 3000)
+        {
+            /* 2.4GHz */
+            if (pBssInfo->EnableHT == 1)
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, (oneTxStreamCap!=0)?3:2, 1, pBssInfo->SG40);
+            else
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, 1, 1, pBssInfo->SG40);
+        }
+        else
+        {
+            /* 5GHz */
+            if (pBssInfo->EnableHT == 1)
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, (oneTxStreamCap!=0)?3:2, 0, pBssInfo->SG40);
+            else
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, 1, 0, pBssInfo->SG40);
+        }
+    }
+    else
+    {
+        /* TODO : Handle 11n */
+        if (pBssInfo->frequency < 3000)
+        {
+            /* 2.4GHz */
+            if (pBssInfo->EnableHT == 1)
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, (oneTxStreamCap!=0)?3:2, 1, pBssInfo->SG40);
+            else
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, 0, 1, pBssInfo->SG40);
+        }
+        else
+        {
+            /* 5GHz */
+            if (pBssInfo->EnableHT == 1)
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, (oneTxStreamCap!=0)?3:2, 0, pBssInfo->SG40);
+            else
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, 1, 0, pBssInfo->SG40);
+        }
+    }
+
+
+    zfStaInitCommonOppositeInfo(dev, i);
+zlReturn:
+    return 0;
+}
+
+int zfStaSetOppositeInfoFromRxBuf(zdev_t* dev, zbuf_t* buf)
+{
+    int   i;
+    u8_t*  dst;
+    u16_t  sa[3];
+    int res = 0;
+    u16_t  offset;
+    u8_t   bSupportExtRate;
+    u32_t rtsctsRate = 0xffffffff; /* CTS:OFDM 6M, RTS:OFDM 6M */
+    u32_t oneTxStreamCap;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    sa[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET);
+    sa[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+2);
+    sa[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+4);
+
+    zmw_enter_critical_section(dev);
+
+    res = zfStaFindFreeOpposite(dev, sa, &i);
+    if ( res != 0 )
+    {
+        goto zlReturn;
+    }
+
+    dst = wd->sta.oppositeInfo[i].macAddr;
+    zfCopyFromRxBuffer(dev, buf, dst, ZM_WLAN_HEADER_A2_OFFSET, 6);
+
+    if ( (wd->sta.currentFrequency < 3000) && !(wd->supportMode & (ZM_WIRELESS_MODE_24_54|ZM_WIRELESS_MODE_24_N)) )
+    {
+        bSupportExtRate = 0;
+    } else {
+        bSupportExtRate = 1;
+    }
+
+    if ( (bSupportExtRate == 1)
+         && (wd->sta.currentFrequency < 3000)
+         && (wd->wlanMode == ZM_MODE_IBSS)
+         && (wd->wfc.bIbssGMode == 0) )
+    {
+        bSupportExtRate = 0;
+    }
+
+    wd->sta.connection_11b = 0;
+    oneTxStreamCap = (zfHpCapability(dev) & ZM_HP_CAP_11N_ONE_TX_STREAM);
+
+    if ( ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_EXTENDED_RATE)) != 0xffff)
+         && (bSupportExtRate == 1) )
+    {
+        /* TODO : Handle 11n */
+        if (wd->sta.currentFrequency < 3000)
+        {
+            /* 2.4GHz */
+            if (wd->sta.EnableHT == 1)
+            {
+                //11ng
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, (oneTxStreamCap!=0)?3:2, 1, wd->sta.SG40);
+            }
+            else
+            {
+                //11g
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, 1, 1, wd->sta.SG40);
+            }
+            rtsctsRate = 0x00001bb; /* CTS:CCK 1M, RTS:OFDM 6M */
+        }
+        else
+        {
+            /* 5GHz */
+            if (wd->sta.EnableHT == 1)
+            {
+                //11na
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, (oneTxStreamCap!=0)?3:2, 0, wd->sta.SG40);
+            }
+            else
+            {
+                //11a
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, 1, 0, wd->sta.SG40);
+            }
+            rtsctsRate = 0x10b01bb; /* CTS:OFDM 6M, RTS:OFDM 6M */
+        }
+    }
+    else
+    {
+        /* TODO : Handle 11n */
+        if (wd->sta.currentFrequency < 3000)
+        {
+            /* 2.4GHz */
+            if (wd->sta.EnableHT == 1)
+            {
+                //11ng
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, (oneTxStreamCap!=0)?3:2, 1, wd->sta.SG40);
+                rtsctsRate = 0x00001bb; /* CTS:CCK 1M, RTS:OFDM 6M */
+            }
+            else
+            {
+                //11b
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, 0, 1, wd->sta.SG40);
+                rtsctsRate = 0x0; /* CTS:CCK 1M, RTS:CCK 1M */
+                wd->sta.connection_11b = 1;
+            }
+        }
+        else
+        {
+            /* 5GHz */
+            if (wd->sta.EnableHT == 1)
+            {
+                //11na
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, (oneTxStreamCap!=0)?3:2, 0, wd->sta.SG40);
+            }
+            else
+            {
+                //11a
+                zfRateCtrlInitCell(dev, &wd->sta.oppositeInfo[i].rcCell, 1, 0, wd->sta.SG40);
+            }
+            rtsctsRate = 0x10b01bb; /* CTS:OFDM 6M, RTS:OFDM 6M */
+        }
+    }
+
+    zfStaInitCommonOppositeInfo(dev, i);
+
+zlReturn:
+    zmw_leave_critical_section(dev);
+
+    if (rtsctsRate != 0xffffffff)
+    {
+        zfHpSetRTSCTSRate(dev, rtsctsRate);
+    }
+    return res;
+}
+
+void zfStaProtErpMonitor(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t   offset;
+    u8_t    erp;
+    u8_t    bssid[6];
+
+    zmw_get_wlan_dev(dev);
+
+    if ( (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)&&(zfStaIsConnected(dev)) )
+    {
+        ZM_MAC_WORD_TO_BYTE(wd->sta.bssid, bssid);
+
+        if (zfRxBufferEqualToStr(dev, buf, bssid, ZM_WLAN_HEADER_A2_OFFSET, 6))
+        {
+            if ( (offset=zfFindElement(dev, buf, ZM_WLAN_EID_ERP)) != 0xffff )
+            {
+                erp = zmw_rx_buf_readb(dev, buf, offset+2);
+
+                if ( erp & ZM_BIT_1 )
+                {
+                    //zm_debug_msg0("protection mode on");
+                    if (wd->sta.bProtectionMode == FALSE)
+                    {
+                        wd->sta.bProtectionMode = TRUE;
+                        zfHpSetSlotTime(dev, 0);
+                    }
+                }
+                else
+                {
+                    //zm_debug_msg0("protection mode off");
+                    if (wd->sta.bProtectionMode == TRUE)
+                    {
+                        wd->sta.bProtectionMode = FALSE;
+                        zfHpSetSlotTime(dev, 1);
+                    }
+                }
+            }
+        }
+		//Check the existence of Non-N AP
+		//Follow the check the "pBssInfo->EnableHT"
+			if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_HT_CAPABILITY)) != 0xffff)
+			{}
+			else if ((offset = zfFindElement(dev, buf, ZM_WLAN_PREN2_EID_HTCAPABILITY)) != 0xffff)
+			{}
+			else
+			{wd->sta.NonNAPcount++;}
+    }
+}
+
+void zfStaUpdateWmeParameter(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t   tmp;
+    u16_t   aifs[5];
+    u16_t   cwmin[5];
+    u16_t   cwmax[5];
+    u16_t   txop[5];
+    u8_t    acm;
+    u8_t    ac;
+    u16_t   len;
+    u16_t   i;
+   	u16_t   offset;
+    u8_t    rxWmeParameterSetCount;
+
+    zmw_get_wlan_dev(dev);
+
+    /* Update if WME parameter set count is changed */
+    /* If connect to WME AP */
+    if (wd->sta.wmeConnected != 0)
+    {
+        /* Find WME parameter element */
+        if ((offset = zfFindWifiElement(dev, buf, 2, 1)) != 0xffff)
+        {
+            if ((len = zmw_rx_buf_readb(dev, buf, offset+1)) >= 7)
+            {
+                rxWmeParameterSetCount=zmw_rx_buf_readb(dev, buf, offset+8);
+                if (rxWmeParameterSetCount != wd->sta.wmeParameterSetCount)
+                {
+                    zm_msg0_mm(ZM_LV_0, "wmeParameterSetCount changed!");
+                    wd->sta.wmeParameterSetCount = rxWmeParameterSetCount;
+                    /* retrieve WME parameter and update TxQ parameters */
+                    acm = 0xf;
+                    for (i=0; i<4; i++)
+                    {
+                        if (len >= (8+(i*4)+4))
+                        {
+                            tmp=zmw_rx_buf_readb(dev, buf, offset+10+i*4);
+                            ac = (tmp >> 5) & 0x3;
+                            if ((tmp & 0x10) == 0)
+                            {
+                                acm &= (~(1<<ac));
+                            }
+                            aifs[ac] = ((tmp & 0xf) * 9) + 10;
+                            tmp=zmw_rx_buf_readb(dev, buf, offset+11+i*4);
+                            /* Convert to 2^n */
+                            cwmin[ac] = zcCwTlb[(tmp & 0xf)];
+                            cwmax[ac] = zcCwTlb[(tmp >> 4)];
+                            txop[ac]=zmw_rx_buf_readh(dev, buf,
+                                    offset+12+i*4);
+                        }
+                    }
+
+                    if ((acm & 0x4) != 0)
+                    {
+                        cwmin[2] = cwmin[0];
+                        cwmax[2] = cwmax[0];
+                        aifs[2] = aifs[0];
+                        txop[2] = txop[0];
+                    }
+                    if ((acm & 0x8) != 0)
+                    {
+                        cwmin[3] = cwmin[2];
+                        cwmax[3] = cwmax[2];
+                        aifs[3] = aifs[2];
+                        txop[3] = txop[2];
+                    }
+                    cwmin[4] = 3;
+                    cwmax[4] = 7;
+                    aifs[4] = 28;
+
+                    if ((cwmin[2]+aifs[2]) > ((cwmin[0]+aifs[0])+1))
+                    {
+                        wd->sta.ac0PriorityHigherThanAc2 = 1;
+                    }
+                    else
+                    {
+                        wd->sta.ac0PriorityHigherThanAc2 = 0;
+                    }
+                    zfHpUpdateQosParameter(dev, cwmin, cwmax, aifs, txop);
+                }
+            }
+        }
+    } //if (wd->sta.wmeConnected != 0)
+}
+/* process 802.11h Dynamic Frequency Selection */
+void zfStaUpdateDot11HDFS(zdev_t* dev, zbuf_t* buf)
+{
+    zmw_get_wlan_dev(dev);
+
+    /*
+    Channel Switch Announcement Element Format
+    +------+----------+------+-------------------+------------------+--------------------+
+    |Format|Element ID|Length|Channel Switch Mode|New Channel Number|Channel Switch Count|
+    +------+----------+------+-------------------+------------------+--------------------+
+    |Bytes |   1      |  1   |	     1           |       1          |          1         |
+    +------+----------+------+-------------------+------------------+--------------------+
+    |Value |   37     |  3   |       0 or 1      |unsigned integer  |unsigned integer    |
+    +------+----------+------+-------------------+------------------+--------------------+
+    */
+    //u8_t    length, channel, is5G;
+    u16_t   offset;
+
+    /* get EID(Channel Switch Announcement) */
+    if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_CHANNEL_SWITCH_ANNOUNCE)) == 0xffff )
+    {
+        //zm_debug_msg0("EID(Channel Switch Announcement) not found");
+        return;
+    }
+    else if ( zmw_rx_buf_readb(dev, buf, offset+1) == 0x3 )
+    {
+        zm_debug_msg0("EID(Channel Switch Announcement) found");
+
+        //length = zmw_rx_buf_readb(dev, buf, offset+1);
+        //zfCopyFromRxBuffer(dev, buf, pBssInfo->supportedRates, offset, length+2);
+
+        //Chanell Switch Mode set to 1, driver should disable transmit immediate
+        //we do this by poll CCA high
+        if (zmw_rx_buf_readb(dev, buf, offset+2) == 0x1 )
+        {
+        	//use ZM_OID_INTERNAL_WRITE,ZM_CMD_RESET to notice firmware flush quene and stop dma,
+        	//then restart rx dma but not tx dma
+        	if (wd->sta.DFSDisableTx != TRUE)
+        	{
+                /* TODO : zfHpResetTxRx would cause Rx hang */
+                //zfHpResetTxRx(dev);
+                wd->sta.DFSDisableTx = TRUE;
+                /* Trgger Rx DMA */
+                zfHpStartRecv(dev);
+            }
+        	//Adapter->ZD80211HSetting.DisableTxBy80211H=TRUE;
+        	//AcquireCtrOfPhyReg(Adapter);
+        	//ZD1205_WRITE_REGISTER(Adapter,CR24, 0x0);
+        	//ReleaseDoNotSleep(Adapter);
+        }
+
+        if (zmw_rx_buf_readb(dev, buf, offset+4) <= 0x2 )
+        {
+        	//Channel Switch
+        	//if Channel Switch Count = 0 , STA should change channel immediately.
+        	//if Channel Switch Count > 0 , STA should change channel after TBTT*count
+        	//But it won't be accurate to let driver calculate TBTT*count, and the value of
+        	//Channel Switch Count will decrease by one each when continue receving beacon
+        	//So we change channel here when we receive count <=2.
+
+            zfHpDeleteAllowChannel(dev, wd->sta.currentFrequency);
+        	wd->frequency = zfChNumToFreq(dev, zmw_rx_buf_readb(dev, buf, offset+3), 0);
+        	//zfHpAddAllowChannel(dev, wd->frequency);
+        	zm_debug_msg1("CWY - jump to frequency = ", wd->frequency);
+        	zfCoreSetFrequency(dev, wd->frequency);
+        	wd->sta.DFSDisableTx = FALSE;
+            /* Increase rxBeaconCount to prevent beacon lost */
+            if (zfStaIsConnected(dev))
+            {
+                wd->sta.rxBeaconCount = 1 << 6; // 2 times of check would pass
+            }
+        	//start tx dma to transmit packet
+
+        	//if (zmw_rx_buf_readb(dev, buf, offset+3) != wd->frequency)
+        	//{
+        	//	//ZDDbgPrint(("Radar Detect by AP\n"));
+        	//	zfCoreSetFrequency();
+        	//	ProcessRadarDetectEvent(Adapter);
+        	//	Set_RF_Channel(Adapter, SwRfd->Rfd->RxBuffer[index+3], (UCHAR)Adapter->RF_Mode, 1);
+        	//	Adapter->CardSetting.Channel = SwRfd->Rfd->RxBuffer[index+3];
+        	//	Adapter->SaveChannel = Adapter->CardSetting.Channel;
+        	//	Adapter->UtilityChannel = Adapter->CardSetting.Channel;
+        	//}
+        }
+    }
+
+}
+/* TODO : process 802.11h Transmission Power Control */
+void zfStaUpdateDot11HTPC(zdev_t* dev, zbuf_t* buf)
+{
+}
+
+/* IBSS power-saving mode */
+void zfStaIbssPSCheckState(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t   i, frameCtrl;
+
+    zmw_get_wlan_dev(dev);
+
+    if ( !zfStaIsConnected(dev) )
+    {
+        return;
+    }
+
+    if ( wd->wlanMode != ZM_MODE_IBSS )
+    {
+        return ;
+    }
+
+    /* check BSSID */
+    if ( !zfRxBufferEqualToStr(dev, buf, (u8_t*) wd->sta.bssid,
+                               ZM_WLAN_HEADER_A3_OFFSET, 6) )
+    {
+        return;
+    }
+
+    frameCtrl = zmw_rx_buf_readb(dev, buf, 1);
+
+    /* check power management bit */
+    if ( frameCtrl & ZM_BIT_4 )
+    {
+        for(i=1; i<ZM_MAX_PS_STA; i++)
+        {
+            if ( !wd->sta.staPSList.entity[i].bUsed )
+            {
+                continue;
+            }
+
+            /* check source address */
+            if ( zfRxBufferEqualToStr(dev, buf,
+                                      wd->sta.staPSList.entity[i].macAddr,
+                                      ZM_WLAN_HEADER_A2_OFFSET, 6) )
+            {
+                return;
+            }
+        }
+
+        for(i=1; i<ZM_MAX_PS_STA; i++)
+        {
+            if ( !wd->sta.staPSList.entity[i].bUsed )
+            {
+                wd->sta.staPSList.entity[i].bUsed = TRUE;
+                wd->sta.staPSList.entity[i].bDataQueued = FALSE;
+                break;
+            }
+        }
+
+        if ( i == ZM_MAX_PS_STA )
+        {
+            /* STA list is full */
+            return;
+        }
+
+        zfCopyFromRxBuffer(dev, buf, wd->sta.staPSList.entity[i].macAddr,
+                           ZM_WLAN_HEADER_A2_OFFSET, 6);
+
+        if ( wd->sta.staPSList.count == 0 )
+        {
+            // enable ATIM window
+            //zfEnableAtimWindow(dev);
+        }
+
+        wd->sta.staPSList.count++;
+    }
+    else if ( wd->sta.staPSList.count )
+    {
+        for(i=1; i<ZM_MAX_PS_STA; i++)
+        {
+            if ( wd->sta.staPSList.entity[i].bUsed )
+            {
+                if ( zfRxBufferEqualToStr(dev, buf,
+                                          wd->sta.staPSList.entity[i].macAddr,
+                                          ZM_WLAN_HEADER_A2_OFFSET, 6) )
+                {
+                    wd->sta.staPSList.entity[i].bUsed = FALSE;
+                    wd->sta.staPSList.count--;
+
+                    if ( wd->sta.staPSList.entity[i].bDataQueued )
+                    {
+                        /* send queued data */
+                    }
+                }
+            }
+        }
+
+        if ( wd->sta.staPSList.count == 0 )
+        {
+            /* disable ATIM window */
+            //zfDisableAtimWindow(dev);
+        }
+
+    }
+}
+
+/* IBSS power-saving mode */
+u8_t zfStaIbssPSQueueData(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t   i;
+    u16_t  da[3];
+
+    zmw_get_wlan_dev(dev);
+
+    if ( !zfStaIsConnected(dev) )
+    {
+        return 0;
+    }
+
+    if ( wd->wlanMode != ZM_MODE_IBSS )
+    {
+        return 0;
+    }
+
+    if ( wd->sta.staPSList.count == 0 && wd->sta.powerSaveMode <= ZM_STA_PS_NONE )
+    {
+        return 0;
+    }
+
+    /* DA */
+#ifdef ZM_ENABLE_NATIVE_WIFI
+    da[0] = zmw_tx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET);
+    da[1] = zmw_tx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET + 2);
+    da[2] = zmw_tx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET + 4);
+#else
+    da[0] = zmw_tx_buf_readh(dev, buf, 0);
+    da[1] = zmw_tx_buf_readh(dev, buf, 2);
+    da[2] = zmw_tx_buf_readh(dev, buf, 4);
+#endif
+
+    if ( ZM_IS_MULTICAST_OR_BROADCAST(da) )
+    {
+        wd->sta.staPSList.entity[0].bDataQueued = TRUE;
+        wd->sta.ibssPSDataQueue[wd->sta.ibssPSDataCount++] = buf;
+        return 1;
+    }
+
+    // Unicast packet...
+
+    for(i=1; i<ZM_MAX_PS_STA; i++)
+    {
+        if ( zfMemoryIsEqual(wd->sta.staPSList.entity[i].macAddr,
+                             (u8_t*) da, 6) )
+        {
+            wd->sta.staPSList.entity[i].bDataQueued = TRUE;
+            wd->sta.ibssPSDataQueue[wd->sta.ibssPSDataCount++] = buf;
+
+            return 1;
+        }
+    }
+
+#if 0
+    if ( wd->sta.powerSaveMode > ZM_STA_PS_NONE )
+    {
+        wd->sta.staPSDataQueue[wd->sta.staPSDataCount++] = buf;
+
+        return 1;
+    }
+#endif
+
+    return 0;
+}
+
+/* IBSS power-saving mode */
+void zfStaIbssPSSend(zdev_t* dev)
+{
+    u8_t   i;
+    u16_t  bcastAddr[3] = {0xffff, 0xffff, 0xffff};
+
+    zmw_get_wlan_dev(dev);
+
+    if ( !zfStaIsConnected(dev) )
+    {
+        return ;
+    }
+
+    if ( wd->wlanMode != ZM_MODE_IBSS )
+    {
+        return ;
+    }
+
+    for(i=0; i<ZM_MAX_PS_STA; i++)
+    {
+        if ( wd->sta.staPSList.entity[i].bDataQueued )
+        {
+            if ( i == 0 )
+            {
+                zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_ATIM,
+                              bcastAddr,
+                              0, 0, 0);
+            }
+            else if ( wd->sta.staPSList.entity[i].bUsed )
+            {
+                // Send ATIM to prevent the peer to go to sleep
+                zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_ATIM,
+                              (u16_t*) wd->sta.staPSList.entity[i].macAddr,
+                              0, 0, 0);
+            }
+
+            wd->sta.staPSList.entity[i].bDataQueued = FALSE;
+        }
+    }
+
+    for(i=0; i<wd->sta.ibssPSDataCount; i++)
+    {
+        zfTxSendEth(dev, wd->sta.ibssPSDataQueue[i], 0,
+                    ZM_EXTERNAL_ALLOC_BUF, 0);
+    }
+
+    wd->sta.ibssPrevPSDataCount = wd->sta.ibssPSDataCount;
+    wd->sta.ibssPSDataCount = 0;
+}
+
+
+void zfStaReconnect(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    if ( wd->wlanMode != ZM_MODE_INFRASTRUCTURE &&
+         wd->wlanMode != ZM_MODE_IBSS )
+    {
+        return;
+    }
+
+    if ( (zfStaIsConnected(dev))||(zfStaIsConnecting(dev)) )
+    {
+        return;
+    }
+
+    if ( wd->sta.bChannelScan )
+    {
+        return;
+    }
+
+    /* Recover zero SSID length  */
+    if ( (wd->wlanMode == ZM_MODE_INFRASTRUCTURE) && (wd->ws.ssidLen == 0))
+    {
+        zm_debug_msg0("zfStaReconnect: NOT Support!! Set SSID to any BSS");
+        /* ANY BSS */
+        zmw_enter_critical_section(dev);
+        wd->sta.ssid[0] = 0;
+        wd->sta.ssidLen = 0;
+        zmw_leave_critical_section(dev);
+    }
+
+    // RAY: To ensure no TX pending before re-connecting
+    zfFlushVtxq(dev);
+    zfWlanEnable(dev);
+    zfScanMgrScanAck(dev);
+}
+
+void zfStaTimer100ms(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if ( (wd->tick % 10) == 0 )
+    {
+        zfPushVtxq(dev);
+//        zfPowerSavingMgrMain(dev);
+    }
+}
+
+
+void zfStaCheckRxBeacon(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if (( wd->wlanMode == ZM_MODE_INFRASTRUCTURE ) && (zfStaIsConnected(dev)))
+    {
+        if (wd->beaconInterval == 0)
+        {
+            wd->beaconInterval = 100;
+        }
+        if ( (wd->tick % ((wd->beaconInterval * 10) / ZM_MS_PER_TICK)) == 0 )
+        {
+            /* Check rxBeaconCount */
+            if (wd->sta.rxBeaconCount == 0)
+            {
+                if (wd->sta.beaconMissState == 1)
+                {
+            	/*notify AP that we left*/
+            	zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH, wd->sta.bssid, 3, 0, 0);
+                /* Beacon Lost */
+                zfStaConnectFail(dev, ZM_STATUS_MEDIA_DISCONNECT_BEACON_MISS,
+                        wd->sta.bssid, 0);
+                }
+                else
+                {
+                    wd->sta.beaconMissState = 1;
+                    /* Reset channel */
+                    zfCoreSetFrequencyExV2(dev, wd->frequency, wd->BandWidth40,
+                            wd->ExtOffset, NULL, 1);
+                }
+            }
+            else
+            {
+                wd->sta.beaconMissState = 0;
+            }
+            wd->sta.rxBeaconCount = 0;
+        }
+    }
+}
+
+
+
+void zfStaCheckConnectTimeout(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    if ( wd->wlanMode != ZM_MODE_INFRASTRUCTURE )
+    {
+        return;
+    }
+
+    if ( !zfStaIsConnecting(dev) )
+    {
+        return;
+    }
+
+    zmw_enter_critical_section(dev);
+    if ( (wd->sta.connectState == ZM_STA_CONN_STATE_AUTH_OPEN)||
+         (wd->sta.connectState == ZM_STA_CONN_STATE_AUTH_SHARE_1)||
+         (wd->sta.connectState == ZM_STA_CONN_STATE_AUTH_SHARE_2)||
+         (wd->sta.connectState == ZM_STA_CONN_STATE_ASSOCIATE) )
+    {
+        if ( (wd->tick - wd->sta.connectTimer) > ZM_INTERVAL_CONNECT_TIMEOUT )
+        {
+            if ( wd->sta.connectByReasso )
+            {
+                wd->sta.failCntOfReasso++;
+                if ( wd->sta.failCntOfReasso > 2 )
+                {
+                    wd->sta.connectByReasso = FALSE;
+                }
+            }
+
+            wd->sta.connectState = ZM_STA_CONN_STATE_NONE;
+            zm_debug_msg1("connect timeout, state = ", wd->sta.connectState);
+            //zfiWlanDisable(dev);
+            goto failed;
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+    return;
+
+failed:
+    zmw_leave_critical_section(dev);
+    if(wd->sta.authMode == ZM_AUTH_MODE_AUTO)
+	{ // Fix some AP not send authentication failed message to sta and lead to connect timeout !
+            wd->sta.connectTimeoutCount++;
+	}
+    zfStaConnectFail(dev, ZM_STATUS_MEDIA_DISCONNECT_TIMEOUT, wd->sta.bssid, 2);
+    return;
+}
+
+void zfMmStaTimeTick(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* airopeek */
+    if (wd->wlanMode != ZM_MODE_AP && !wd->swSniffer)
+    {
+        if ( wd->tick & 1 )
+        {
+            zfTimerCheckAndHandle(dev);
+        }
+
+        zfStaCheckRxBeacon(dev);
+        zfStaTimer100ms(dev);
+        zfStaCheckConnectTimeout(dev);
+        zfPowerSavingMgrMain(dev);
+    }
+
+#ifdef ZM_ENABLE_AGGREGATION
+    /*
+     * add by honda
+     */
+    zfAggScanAndClear(dev, wd->tick);
+#endif
+}
+
+void zfStaSendBeacon(zdev_t* dev)
+{
+    zbuf_t* buf;
+    u16_t offset, seq;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    //zm_debug_msg0("\n");
+
+    /* TBD : Maximum size of beacon */
+    if ((buf = zfwBufAllocate(dev, 1024)) == NULL)
+    {
+        zm_debug_msg0("Allocate beacon buffer failed");
+        return;
+    }
+
+    offset = 0;
+    /* wlan header */
+    /* Frame control */
+    zmw_tx_buf_writeh(dev, buf, offset, 0x0080);
+    offset+=2;
+    /* Duration */
+    zmw_tx_buf_writeh(dev, buf, offset, 0x0000);
+    offset+=2;
+    /* Address 1 */
+    zmw_tx_buf_writeh(dev, buf, offset, 0xffff);
+    offset+=2;
+    zmw_tx_buf_writeh(dev, buf, offset, 0xffff);
+    offset+=2;
+    zmw_tx_buf_writeh(dev, buf, offset, 0xffff);
+    offset+=2;
+    /* Address 2 */
+    zmw_tx_buf_writeh(dev, buf, offset, wd->macAddr[0]);
+    offset+=2;
+    zmw_tx_buf_writeh(dev, buf, offset, wd->macAddr[1]);
+    offset+=2;
+    zmw_tx_buf_writeh(dev, buf, offset, wd->macAddr[2]);
+    offset+=2;
+    /* Address 3 */
+    zmw_tx_buf_writeh(dev, buf, offset, wd->sta.bssid[0]);
+    offset+=2;
+    zmw_tx_buf_writeh(dev, buf, offset, wd->sta.bssid[1]);
+    offset+=2;
+    zmw_tx_buf_writeh(dev, buf, offset, wd->sta.bssid[2]);
+    offset+=2;
+
+    /* Sequence number */
+    zmw_enter_critical_section(dev);
+    seq = ((wd->mmseq++)<<4);
+    zmw_leave_critical_section(dev);
+    zmw_tx_buf_writeh(dev, buf, offset, seq);
+    offset+=2;
+
+    /* 24-31 Time Stamp : hardware will fill this field */
+    offset+=8;
+
+    /* Beacon Interval */
+    zmw_tx_buf_writeh(dev, buf, offset, wd->beaconInterval);
+    offset+=2;
+
+    /* Capability */
+    zmw_tx_buf_writeb(dev, buf, offset++, wd->sta.capability[0]);
+    zmw_tx_buf_writeb(dev, buf, offset++, wd->sta.capability[1]);
+
+    /* SSID */
+    offset = zfStaAddIeSsid(dev, buf, offset);
+
+    if(wd->frequency <= ZM_CH_G_14)  // 2.4 GHz  b+g
+    {
+
+    	/* Support Rate */
+    	offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                  		ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_CCK);
+
+    	/* DS parameter set */
+    	offset = zfMmAddIeDs(dev, buf, offset);
+
+    	offset = zfStaAddIeIbss(dev, buf, offset);
+
+        if( wd->wfc.bIbssGMode
+            && (wd->supportMode & (ZM_WIRELESS_MODE_24_54|ZM_WIRELESS_MODE_24_N)) )    // Only accompany with enabling a mode .
+        {
+      	    /* ERP Information */
+       	    wd->erpElement = 0;
+       	    offset = zfMmAddIeErp(dev, buf, offset);
+       	}
+
+       	/* TODO : country information */
+        /* RSN */
+        if ( wd->sta.authMode == ZM_AUTH_MODE_WPA2PSK )
+        {
+            offset = zfwStaAddIeWpaRsn(dev, buf, offset, ZM_WLAN_FRAME_TYPE_AUTH);
+        }
+
+        if( wd->wfc.bIbssGMode
+            && (wd->supportMode & (ZM_WIRELESS_MODE_24_54|ZM_WIRELESS_MODE_24_N)) )    // Only accompany with enabling a mode .
+        {
+            /* Enable G Mode */
+            /* Extended Supported Rates */
+       	    offset = zfMmAddIeSupportRate(dev, buf, offset,
+                                   		    ZM_WLAN_EID_EXTENDED_RATE, ZM_RATE_SET_OFDM);
+	    }
+    }
+    else    // 5GHz a
+    {
+        /* Support Rate a Mode */
+    	offset = zfMmAddIeSupportRate(dev, buf, offset,
+        	                            ZM_WLAN_EID_SUPPORT_RATE, ZM_RATE_SET_OFDM);
+
+        /* DS parameter set */
+    	offset = zfMmAddIeDs(dev, buf, offset);
+
+    	offset = zfStaAddIeIbss(dev, buf, offset);
+
+        /* TODO : country information */
+        /* RSN */
+        if ( wd->sta.authMode == ZM_AUTH_MODE_WPA2PSK )
+        {
+            offset = zfwStaAddIeWpaRsn(dev, buf, offset, ZM_WLAN_FRAME_TYPE_AUTH);
+        }
+    }
+
+    if ( wd->wlanMode != ZM_MODE_IBSS )
+    {
+        /* TODO : Need to check if it is ok */
+        /* HT Capabilities Info */
+        offset = zfMmAddHTCapability(dev, buf, offset);
+
+        /* Extended HT Capabilities Info */
+        offset = zfMmAddExtendedHTCapability(dev, buf, offset);
+    }
+
+    if ( wd->sta.ibssAdditionalIESize )
+        offset = zfStaAddIbssAdditionalIE(dev, buf, offset);
+
+    /* 1212 : write to beacon fifo */
+    /* 1221 : write to share memory */
+    zfHpSendBeacon(dev, buf, offset);
+
+    /* Free beacon buffer */
+    //zfwBufFree(dev, buf, 0);
+}
+
+void zfStaSignalStatistic(zdev_t* dev, u8_t SignalStrength, u8_t SignalQuality) //CWYang(+)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* Add Your Code to Do Works Like Moving Average Here */
+    wd->SignalStrength = (wd->SignalStrength * 7 + SignalStrength * 3)/10;
+    wd->SignalQuality = (wd->SignalQuality * 7 + SignalQuality * 3)/10;
+
+}
+
+struct zsBssInfo* zfStaFindBssInfo(zdev_t* dev, zbuf_t* buf, struct zsWlanProbeRspFrameHeader *pProbeRspHeader)
+{
+    u8_t    i;
+    u8_t    j;
+    u8_t    k;
+    u8_t    isMatched, length, channel;
+    u16_t   offset, frequency;
+    struct zsBssInfo* pBssInfo;
+
+    zmw_get_wlan_dev(dev);
+
+    if ((pBssInfo = wd->sta.bssList.head) == NULL)
+    {
+        return NULL;
+    }
+
+    for( i=0; i<wd->sta.bssList.bssCount; i++ )
+    {
+        //zm_debug_msg2("check pBssInfo = ", pBssInfo);
+
+        /* Check BSSID */
+        for( j=0; j<6; j++ )
+        {
+            if ( pBssInfo->bssid[j] != pProbeRspHeader->bssid[j] )
+            {
+                break;
+            }
+        }
+
+		/* Check SSID */
+        if (j == 6)
+        {
+            if (pProbeRspHeader->ssid[1] <= 32)
+            {
+                /* compare length and ssid */
+                isMatched = 1;
+				if((pProbeRspHeader->ssid[1] != 0) && (pBssInfo->ssid[1] != 0))
+				{
+                for( k=1; k<pProbeRspHeader->ssid[1] + 1; k++ )
+                {
+                    if ( pBssInfo->ssid[k] != pProbeRspHeader->ssid[k] )
+                    {
+                        isMatched = 0;
+                        break;
+                    }
+                }
+            }
+            }
+            else
+            {
+                isMatched = 0;
+            }
+        }
+        else
+        {
+            isMatched = 0;
+        }
+
+        /* Check channel */
+        /* Add check channel to solve the bug #31222 */
+        if (isMatched) {
+            if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_DS)) != 0xffff) {
+                if ((length = zmw_rx_buf_readb(dev, buf, offset+1)) == 1) {
+                    channel = zmw_rx_buf_readb(dev, buf, offset+2);
+                    if (zfHpIsAllowedChannel(dev, zfChNumToFreq(dev, channel, 0)) == 0) {
+                        frequency = 0;
+                    } else {
+                        frequency = zfChNumToFreq(dev, channel, 0);;
+                    }
+                } else {
+                    frequency = 0;
+                }
+            } else {
+                frequency = wd->sta.currentFrequency;
+            }
+
+            if (frequency != 0) {
+                if ( ((frequency > 3000) && (pBssInfo->frequency > 3000))
+                     || ((frequency < 3000) && (pBssInfo->frequency < 3000)) ) {
+                    /* redundant */
+                    break;
+                }
+            }
+        }
+
+        pBssInfo = pBssInfo->next;
+    }
+
+    if ( i == wd->sta.bssList.bssCount )
+    {
+        pBssInfo = NULL;
+    }
+
+    return pBssInfo;
+}
+
+u8_t zfStaInitBssInfo(zdev_t* dev, zbuf_t* buf,
+        struct zsWlanProbeRspFrameHeader *pProbeRspHeader,
+        struct zsBssInfo* pBssInfo, struct zsAdditionInfo* AddInfo, u8_t type)
+{
+    u8_t    length, channel, is5G;
+    u16_t   i, offset;
+    u8_t    apQosInfo;
+    u16_t    eachIElength = 0;
+    u16_t   accumulateLen = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    if ((type == 1) && ((pBssInfo->flag & ZM_BSS_INFO_VALID_BIT) != 0))
+    {
+        goto zlUpdateRssi;
+    }
+
+    /* get SSID */
+    if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_SSID)) == 0xffff )
+    {
+        zm_debug_msg0("EID(SSID) not found");
+        goto zlError;
+    }
+
+    length = zmw_rx_buf_readb(dev, buf, offset+1);
+
+	{
+		u8_t Show_Flag = 0;
+		zfwGetShowZeroLengthSSID(dev, &Show_Flag);
+
+		if(Show_Flag)
+		{
+			if (length > ZM_MAX_SSID_LENGTH )
+			{
+				zm_debug_msg0("EID(SSID) is invalid");
+				goto zlError;
+			}
+		}
+		else
+		{
+    if ( length == 0 || length > ZM_MAX_SSID_LENGTH )
+    {
+        zm_debug_msg0("EID(SSID) is invalid");
+        goto zlError;
+    }
+
+		}
+	}
+    zfCopyFromRxBuffer(dev, buf, pBssInfo->ssid, offset, length+2);
+
+    /* get DS parameter */
+    if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_DS)) != 0xffff )
+    {
+        length = zmw_rx_buf_readb(dev, buf, offset+1);
+        if ( length != 1 )
+        {
+            zm_msg0_mm(ZM_LV_0, "Abnormal DS Param Set IE");
+            goto zlError;
+        }
+        channel = zmw_rx_buf_readb(dev, buf, offset+2);
+
+        if (zfHpIsAllowedChannel(dev, zfChNumToFreq(dev, channel, 0)) == 0)
+        {
+            goto zlError2;
+        }
+
+        pBssInfo->frequency = zfChNumToFreq(dev, channel, 0); // auto check
+        pBssInfo->channel = channel;
+
+
+    }
+    else
+    {
+        /* DS parameter not found */
+        pBssInfo->frequency = wd->sta.currentFrequency;
+        pBssInfo->channel = zfChFreqToNum(wd->sta.currentFrequency, &is5G);
+    }
+
+    /* initialize security type */
+    pBssInfo->securityType = ZM_SECURITY_TYPE_NONE;
+
+    /* get macaddr */
+    for( i=0; i<6; i++ )
+    {
+        pBssInfo->macaddr[i] = pProbeRspHeader->sa[i];
+    }
+
+    /* get bssid */
+    for( i=0; i<6; i++ )
+    {
+        pBssInfo->bssid[i] = pProbeRspHeader->bssid[i];
+    }
+
+    /* get timestamp */
+    for( i=0; i<8; i++ )
+    {
+        pBssInfo->timeStamp[i] = pProbeRspHeader->timeStamp[i];
+    }
+
+    /* get beacon interval */
+    pBssInfo->beaconInterval[0] = pProbeRspHeader->beaconInterval[0];
+    pBssInfo->beaconInterval[1] = pProbeRspHeader->beaconInterval[1];
+
+    /* get capability */
+    pBssInfo->capability[0] = pProbeRspHeader->capability[0];
+    pBssInfo->capability[1] = pProbeRspHeader->capability[1];
+
+    /* Copy frame body */
+    offset = 36;            // Copy from the start of variable IE
+    pBssInfo->frameBodysize = zfwBufGetSize(dev, buf)-offset;
+    if (pBssInfo->frameBodysize > (ZM_MAX_PROBE_FRAME_BODY_SIZE-1))
+    {
+        pBssInfo->frameBodysize = ZM_MAX_PROBE_FRAME_BODY_SIZE-1;
+    }
+    accumulateLen = 0;
+    do
+    {
+        eachIElength = zmw_rx_buf_readb(dev, buf, offset + accumulateLen+1) + 2;  //Len+(EID+Data)
+
+        if ( (eachIElength >= 2)
+             && ((accumulateLen + eachIElength) <= pBssInfo->frameBodysize) )
+        {
+            zfCopyFromRxBuffer(dev, buf, pBssInfo->frameBody+accumulateLen, offset+accumulateLen, eachIElength);
+            accumulateLen+=(u16_t)eachIElength;
+        }
+        else
+        {
+            zm_msg0_mm(ZM_LV_1, "probersp frameBodysize abnormal");
+            break;
+        }
+    }
+    while(accumulateLen < pBssInfo->frameBodysize);
+    pBssInfo->frameBodysize = accumulateLen;
+
+    /* get supported rates */
+    if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_SUPPORT_RATE)) == 0xffff )
+    {
+        zm_debug_msg0("EID(supported rates) not found");
+        goto zlError;
+    }
+
+    length = zmw_rx_buf_readb(dev, buf, offset+1);
+    if ( length == 0 || length > ZM_MAX_SUPP_RATES_IE_SIZE)
+    {
+        zm_msg0_mm(ZM_LV_0, "Supported rates IE length abnormal");
+        goto zlError;
+    }
+    zfCopyFromRxBuffer(dev, buf, pBssInfo->supportedRates, offset, length+2);
+
+
+
+    /* get Country information */
+    if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_COUNTRY)) != 0xffff )
+    {
+        length = zmw_rx_buf_readb(dev, buf, offset+1);
+        if (length > ZM_MAX_COUNTRY_INFO_SIZE)
+        {
+            length = ZM_MAX_COUNTRY_INFO_SIZE;
+        }
+        zfCopyFromRxBuffer(dev, buf, pBssInfo->countryInfo, offset, length+2);
+        /* check 802.11d support data */
+        if (wd->sta.b802_11D)
+        {
+            zfHpGetRegulationTablefromISO(dev, (u8_t *)&pBssInfo->countryInfo, 3);
+            /* only set regulatory one time */
+            wd->sta.b802_11D = 0;
+        }
+    }
+
+    /* get ERP information */
+    if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_ERP)) != 0xffff )
+    {
+        pBssInfo->erp = zmw_rx_buf_readb(dev, buf, offset+2);
+    }
+
+    /* get extended supported rates */
+    if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_EXTENDED_RATE)) != 0xffff )
+    {
+        length = zmw_rx_buf_readb(dev, buf, offset+1);
+        if (length > ZM_MAX_SUPP_RATES_IE_SIZE)
+        {
+            zm_msg0_mm(ZM_LV_0, "Extended rates IE length abnormal");
+            goto zlError;
+        }
+        zfCopyFromRxBuffer(dev, buf, pBssInfo->extSupportedRates, offset, length+2);
+    }
+    else
+    {
+        pBssInfo->extSupportedRates[0] = 0;
+        pBssInfo->extSupportedRates[1] = 0;
+    }
+
+    /* get WPA IE */
+    if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_WPA_IE)) != 0xffff )
+    {
+        length = zmw_rx_buf_readb(dev, buf, offset+1);
+        if (length > ZM_MAX_IE_SIZE)
+        {
+            length = ZM_MAX_IE_SIZE;
+        }
+        zfCopyFromRxBuffer(dev, buf, pBssInfo->wpaIe, offset, length+2);
+        pBssInfo->securityType = ZM_SECURITY_TYPE_WPA;
+    }
+    else
+    {
+        pBssInfo->wpaIe[1] = 0;
+    }
+
+    /* get WPS IE */
+    if ((offset = zfFindWifiElement(dev, buf, 4, 0xff)) != 0xffff)
+    {
+        length = zmw_rx_buf_readb(dev, buf, offset+1);
+        if (length > ZM_MAX_WPS_IE_SIZE )
+        {
+            length = ZM_MAX_WPS_IE_SIZE;
+        }
+        zfCopyFromRxBuffer(dev, buf, pBssInfo->wscIe, offset, length+2);
+    }
+    else
+    {
+        pBssInfo->wscIe[1] = 0;
+    }
+
+    /* get SuperG IE */
+    if ((offset = zfFindSuperGElement(dev, buf, ZM_WLAN_EID_VENDOR_PRIVATE)) != 0xffff)
+    {
+        pBssInfo->apCap |= ZM_SuperG_AP;
+    }
+
+    /* get XR IE */
+    if ((offset = zfFindXRElement(dev, buf, ZM_WLAN_EID_VENDOR_PRIVATE)) != 0xffff)
+    {
+        pBssInfo->apCap |= ZM_XR_AP;
+    }
+
+    /* get RSN IE */
+    if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_RSN_IE)) != 0xffff )
+    {
+        length = zmw_rx_buf_readb(dev, buf, offset+1);
+        if (length > ZM_MAX_IE_SIZE)
+        {
+            length = ZM_MAX_IE_SIZE;
+        }
+        zfCopyFromRxBuffer(dev, buf, pBssInfo->rsnIe, offset, length+2);
+        pBssInfo->securityType = ZM_SECURITY_TYPE_WPA;
+    }
+    else
+    {
+        pBssInfo->rsnIe[1] = 0;
+    }
+#ifdef ZM_ENABLE_CENC
+    /* get CENC IE */
+    if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_CENC_IE)) != 0xffff )
+    {
+        length = zmw_rx_buf_readb(dev, buf, offset+1);
+        if (length > ZM_MAX_IE_SIZE )
+        {
+            length = ZM_MAX_IE_SIZE;
+        }
+        zfCopyFromRxBuffer(dev, buf, pBssInfo->cencIe, offset, length+2);
+        pBssInfo->securityType = ZM_SECURITY_TYPE_CENC;
+        pBssInfo->capability[0] &= 0xffef;
+    }
+    else
+    {
+        pBssInfo->cencIe[1] = 0;
+    }
+#endif //ZM_ENABLE_CENC
+    /* get WME Parameter IE, probe rsp may contain WME parameter element */
+    //if ( wd->bQoSEnable )
+    {
+        if ((offset = zfFindWifiElement(dev, buf, 2, 1)) != 0xffff)
+        {
+            apQosInfo = zmw_rx_buf_readb(dev, buf, offset+8) & 0x80;
+            pBssInfo->wmeSupport = 1 | apQosInfo;
+        }
+        else if ((offset = zfFindWifiElement(dev, buf, 2, 0)) != 0xffff)
+        {
+            apQosInfo = zmw_rx_buf_readb(dev, buf, offset+8) & 0x80;
+            pBssInfo->wmeSupport = 1  | apQosInfo;
+        }
+        else
+        {
+            pBssInfo->wmeSupport = 0;
+        }
+    }
+    //CWYang(+)
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_HT_CAPABILITY)) != 0xffff)
+    {
+        /* 11n AP */
+        pBssInfo->EnableHT = 1;
+        if (zmw_rx_buf_readb(dev, buf, offset+1) & 0x02)
+        {
+            pBssInfo->enableHT40 = 1;
+        }
+        else
+        {
+            pBssInfo->enableHT40 = 0;
+        }
+
+        if (zmw_rx_buf_readb(dev, buf, offset+1) & 0x40)
+        {
+            pBssInfo->SG40 = 1;
+        }
+        else
+        {
+            pBssInfo->SG40 = 0;
+        }
+    }
+    else if ((offset = zfFindElement(dev, buf, ZM_WLAN_PREN2_EID_HTCAPABILITY)) != 0xffff)
+    {
+        /* 11n AP */
+        pBssInfo->EnableHT = 1;
+        pBssInfo->apCap |= ZM_All11N_AP;
+        if (zmw_rx_buf_readb(dev, buf, offset+2) & 0x02)
+        {
+            pBssInfo->enableHT40 = 1;
+        }
+        else
+        {
+            pBssInfo->enableHT40 = 0;
+        }
+
+        if (zmw_rx_buf_readb(dev, buf, offset+2) & 0x40)
+        {
+            pBssInfo->SG40 = 1;
+        }
+        else
+        {
+            pBssInfo->SG40 = 0;
+        }
+    }
+    else
+    {
+        pBssInfo->EnableHT = 0;
+    }
+    /* HT information */
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_EXTENDED_HT_CAPABILITY)) != 0xffff)
+    {
+        /* atheros pre n */
+        pBssInfo->extChOffset = zmw_rx_buf_readb(dev, buf, offset+2) & 0x03;
+    }
+    else if ((offset = zfFindElement(dev, buf, ZM_WLAN_PREN2_EID_HTINFORMATION)) != 0xffff)
+    {
+        /* pre n 2.0 standard */
+        pBssInfo->extChOffset = zmw_rx_buf_readb(dev, buf, offset+3) & 0x03;
+    }
+    else
+    {
+        pBssInfo->extChOffset = 0;
+    }
+
+    if ( (pBssInfo->enableHT40 == 1)
+         && ((pBssInfo->extChOffset != 1) && (pBssInfo->extChOffset != 3)) )
+    {
+        pBssInfo->enableHT40 = 0;
+    }
+
+    if (pBssInfo->enableHT40 == 1)
+    {
+        if (zfHpIsAllowedChannel(dev, pBssInfo->frequency+((pBssInfo->extChOffset==1)?20:-20)) == 0)
+        {
+            /* if extension channel is not an allowed channel, treat AP as non-HT mode */
+            pBssInfo->EnableHT = 0;
+            pBssInfo->enableHT40 = 0;
+            pBssInfo->extChOffset = 0;
+        }
+    }
+
+    /* get ATH Extended Capability */
+    if ( ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_EXTENDED_HT_CAPABILITY)) != 0xffff)&&
+        ((offset = zfFindBrdcmMrvlRlnkExtCap(dev, buf)) == 0xffff))
+
+    {
+        pBssInfo->athOwlAp = 1;
+    }
+    else
+    {
+        pBssInfo->athOwlAp = 0;
+    }
+
+    /* get Broadcom Extended Capability */
+    if ( (pBssInfo->EnableHT == 1) //((offset = zfFindElement(dev, buf, ZM_WLAN_EID_EXTENDED_HT_CAPABILITY)) != 0xffff)
+         && ((offset = zfFindBroadcomExtCap(dev, buf)) != 0xffff) )
+    {
+        pBssInfo->broadcomHTAp = 1;
+    }
+    else
+    {
+        pBssInfo->broadcomHTAp = 0;
+    }
+
+    /* get Marvel Extended Capability */
+    if ((offset = zfFindMarvelExtCap(dev, buf)) != 0xffff)
+    {
+        pBssInfo->marvelAp = 1;
+    }
+    else
+    {
+        pBssInfo->marvelAp = 0;
+    }
+
+    /* get ATIM window */
+    if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_IBSS)) != 0xffff )
+    {
+        pBssInfo->atimWindow = zmw_rx_buf_readh(dev, buf,offset+2);
+    }
+
+    /* Fit for support mode */
+    if (pBssInfo->frequency > 3000) {
+        if (wd->supportMode & ZM_WIRELESS_MODE_5_N) {
+#if 0
+            if (wd->supportMode & ZM_WIRELESS_MODE_5_54) {
+                /* support mode: a, n */
+                /* do nothing */
+            } else {
+                /* support mode: n */
+                /* reject non-n bss info */
+                if (!pBssInfo->EnableHT) {
+                    goto zlError2;
+                }
+            }
+#endif
+        } else {
+            if (wd->supportMode & ZM_WIRELESS_MODE_5_54) {
+                /* support mode: a */
+                /* delete n mode information */
+                pBssInfo->EnableHT = 0;
+                pBssInfo->enableHT40 = 0;
+                pBssInfo->apCap &= (~ZM_All11N_AP);
+                pBssInfo->extChOffset = 0;
+                pBssInfo->frameBodysize = zfRemoveElement(dev, pBssInfo->frameBody,
+                            pBssInfo->frameBodysize, ZM_WLAN_EID_HT_CAPABILITY);
+                pBssInfo->frameBodysize = zfRemoveElement(dev, pBssInfo->frameBody,
+                            pBssInfo->frameBodysize, ZM_WLAN_PREN2_EID_HTCAPABILITY);
+                pBssInfo->frameBodysize = zfRemoveElement(dev, pBssInfo->frameBody,
+                            pBssInfo->frameBodysize, ZM_WLAN_EID_EXTENDED_HT_CAPABILITY);
+                pBssInfo->frameBodysize = zfRemoveElement(dev, pBssInfo->frameBody,
+                            pBssInfo->frameBodysize, ZM_WLAN_PREN2_EID_HTINFORMATION);
+            } else {
+                /* support mode: none */
+                goto zlError2;
+            }
+        }
+    } else {
+        if (wd->supportMode & ZM_WIRELESS_MODE_24_N) {
+#if 0
+            if (wd->supportMode & ZM_WIRELESS_MODE_24_54) {
+                if (wd->supportMode & ZM_WIRELESS_MODE_24_11) {
+                    /* support mode: b, g, n */
+                    /* do nothing */
+                } else {
+                    /* support mode: g, n */
+                    /* reject b-only bss info */
+                    if ( (!pBssInfo->EnableHT)
+                         && (pBssInfo->extSupportedRates[1] == 0) ) {
+                         goto zlError2;
+                    }
+                }
+            } else {
+                if (wd->supportMode & ZM_WIRELESS_MODE_24_11) {
+                    /* support mode: b, n */
+                    /* 1. reject g-only bss info
+                     * 2. if non g-only, delete g mode information
+                     */
+                    if ( !pBssInfo->EnableHT ) {
+                        if ( zfIsGOnlyMode(dev, pBssInfo->frequency, pBssInfo->supportedRates)
+                             || zfIsGOnlyMode(dev, pBssInfo->frequency, pBssInfo->extSupportedRates) ) {
+                            goto zlError2;
+                        } else {
+                            zfGatherBMode(dev, pBssInfo->supportedRates,
+                                          pBssInfo->extSupportedRates);
+                            pBssInfo->erp = 0;
+
+                            pBssInfo->frameBodysize = zfRemoveElement(dev,
+                                pBssInfo->frameBody, pBssInfo->frameBodysize,
+                                ZM_WLAN_EID_ERP);
+                            pBssInfo->frameBodysize = zfRemoveElement(dev,
+                                pBssInfo->frameBody, pBssInfo->frameBodysize,
+                                ZM_WLAN_EID_EXTENDED_RATE);
+
+                            pBssInfo->frameBodysize = zfUpdateElement(dev,
+                                pBssInfo->frameBody, pBssInfo->frameBodysize,
+                                pBssInfo->supportedRates);
+                        }
+                    }
+                } else {
+                    /* support mode: n */
+                    /* reject non-n bss info */
+                    if (!pBssInfo->EnableHT) {
+                        goto zlError2;
+                    }
+                }
+            }
+#endif
+        } else {
+            /* delete n mode information */
+            pBssInfo->EnableHT = 0;
+            pBssInfo->enableHT40 = 0;
+            pBssInfo->apCap &= (~ZM_All11N_AP);
+            pBssInfo->extChOffset = 0;
+            pBssInfo->frameBodysize = zfRemoveElement(dev, pBssInfo->frameBody,
+                        pBssInfo->frameBodysize, ZM_WLAN_EID_HT_CAPABILITY);
+            pBssInfo->frameBodysize = zfRemoveElement(dev, pBssInfo->frameBody,
+                        pBssInfo->frameBodysize, ZM_WLAN_PREN2_EID_HTCAPABILITY);
+            pBssInfo->frameBodysize = zfRemoveElement(dev, pBssInfo->frameBody,
+                        pBssInfo->frameBodysize, ZM_WLAN_EID_EXTENDED_HT_CAPABILITY);
+            pBssInfo->frameBodysize = zfRemoveElement(dev, pBssInfo->frameBody,
+                        pBssInfo->frameBodysize, ZM_WLAN_PREN2_EID_HTINFORMATION);
+
+            if (wd->supportMode & ZM_WIRELESS_MODE_24_54) {
+#if 0
+                if (wd->supportMode & ZM_WIRELESS_MODE_24_11) {
+                    /* support mode: b, g */
+                    /* delete n mode information */
+                } else {
+                    /* support mode: g */
+                    /* delete n mode information */
+                    /* reject b-only bss info */
+                    if (pBssInfo->extSupportedRates[1] == 0) {
+                         goto zlError2;
+                    }
+                }
+#endif
+            } else {
+                if (wd->supportMode & ZM_WIRELESS_MODE_24_11) {
+                    /* support mode: b */
+                    /* delete n mode information */
+                    if ( zfIsGOnlyMode(dev, pBssInfo->frequency, pBssInfo->supportedRates)
+                         || zfIsGOnlyMode(dev, pBssInfo->frequency, pBssInfo->extSupportedRates) ) {
+                        goto zlError2;
+                    } else {
+                        zfGatherBMode(dev, pBssInfo->supportedRates,
+                                          pBssInfo->extSupportedRates);
+                        pBssInfo->erp = 0;
+
+                        pBssInfo->frameBodysize = zfRemoveElement(dev,
+                            pBssInfo->frameBody, pBssInfo->frameBodysize,
+                            ZM_WLAN_EID_ERP);
+                        pBssInfo->frameBodysize = zfRemoveElement(dev,
+                            pBssInfo->frameBody, pBssInfo->frameBodysize,
+                            ZM_WLAN_EID_EXTENDED_RATE);
+
+                        pBssInfo->frameBodysize = zfUpdateElement(dev,
+                            pBssInfo->frameBody, pBssInfo->frameBodysize,
+                            pBssInfo->supportedRates);
+                    }
+                } else {
+                    /* support mode: none */
+                    goto zlError2;
+                }
+            }
+        }
+    }
+
+    pBssInfo->flag |= ZM_BSS_INFO_VALID_BIT;
+
+zlUpdateRssi:
+    /* Update Timer information */
+    pBssInfo->tick = wd->tick;
+
+    /* Update ERP information */
+    if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_ERP)) != 0xffff )
+    {
+        pBssInfo->erp = zmw_rx_buf_readb(dev, buf, offset+2);
+    }
+
+    if( (s8_t)pBssInfo->signalStrength < (s8_t)AddInfo->Tail.Data.SignalStrength1 )
+    {
+        /* Update signal strength */
+        pBssInfo->signalStrength = (u8_t)AddInfo->Tail.Data.SignalStrength1;
+        /* Update signal quality */
+        pBssInfo->signalQuality = (u8_t)(AddInfo->Tail.Data.SignalStrength1 * 2);
+
+        /* Update the sorting value  */
+        pBssInfo->sortValue = zfComputeBssInfoWeightValue(dev,
+                                               (pBssInfo->supportedRates[6] + pBssInfo->extSupportedRates[0]),
+                                               pBssInfo->EnableHT,
+                                               pBssInfo->enableHT40,
+                                               pBssInfo->signalStrength);
+    }
+
+    return 0;
+
+zlError:
+
+    return 1;
+
+zlError2:
+
+    return 2;
+}
+
+void zfStaProcessBeacon(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* AddInfo) //CWYang(m)
+{
+    /* Parse TIM and send PS-POLL in power saving mode */
+    struct zsWlanBeaconFrameHeader*  pBeaconHeader;
+    struct zsBssInfo* pBssInfo;
+    u8_t   pBuf[sizeof(struct zsWlanBeaconFrameHeader)];
+    u8_t   bssid[6];
+    int    res;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    /* sta routine jobs */
+    zfStaProtErpMonitor(dev, buf);  /* check protection mode */
+
+    if (zfStaIsConnected(dev))
+    {
+        ZM_MAC_WORD_TO_BYTE(wd->sta.bssid, bssid);
+
+        if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+        {
+            if ( zfRxBufferEqualToStr(dev, buf, bssid, ZM_WLAN_HEADER_A2_OFFSET, 6) )
+            {
+                zfPowerSavingMgrProcessBeacon(dev, buf);
+                zfStaUpdateWmeParameter(dev, buf);
+                if (wd->sta.DFSEnable)
+                    zfStaUpdateDot11HDFS(dev, buf);
+                if (wd->sta.TPCEnable)
+                    zfStaUpdateDot11HTPC(dev, buf);
+                /* update signal strength and signal quality */
+                zfStaSignalStatistic(dev, AddInfo->Tail.Data.SignalStrength1,
+                        AddInfo->Tail.Data.SignalQuality); //CWYang(+)
+                wd->sta.rxBeaconCount++;
+            }
+        }
+        else if ( wd->wlanMode == ZM_MODE_IBSS )
+        {
+            if ( zfRxBufferEqualToStr(dev, buf, bssid, ZM_WLAN_HEADER_A3_OFFSET, 6) )
+            {
+                int res;
+                struct zsPartnerNotifyEvent event;
+
+                zm_debug_msg0("20070916 Receive opposite Beacon!");
+                zmw_enter_critical_section(dev);
+                wd->sta.ibssReceiveBeaconCount++;
+                zmw_leave_critical_section(dev);
+
+                res = zfStaSetOppositeInfoFromRxBuf(dev, buf);
+                if ( res == 0 )
+                {
+                    // New peer station found. Notify the wrapper now
+                    zfInitPartnerNotifyEvent(dev, buf, &event);
+                    if (wd->zfcbIbssPartnerNotify != NULL)
+                    {
+                        wd->zfcbIbssPartnerNotify(dev, 1, &event);
+                    }
+                }
+                /* update signal strength and signal quality */
+                zfStaSignalStatistic(dev, AddInfo->Tail.Data.SignalStrength1,
+                        AddInfo->Tail.Data.SignalQuality); //CWYang(+)
+            }
+            //else if ( wd->sta.ibssPartnerStatus == ZM_IBSS_PARTNER_LOST )
+            // Why does this happen in IBSS?? The impact of Vista since
+            // we need to tell it the BSSID
+#if 0
+            else if ( wd->sta.oppositeCount == 0 )
+            {   /* IBSS merge if SSID matched */
+                if ( (offset = zfFindElement(dev, buf, ZM_WLAN_EID_SSID)) != 0xffff )
+                {
+                    if ( (wd->sta.ssidLen == zmw_buf_readb(dev, buf, offset+1))&&
+                         (zfRxBufferEqualToStr(dev, buf, wd->sta.ssid,
+                                               offset+2, wd->sta.ssidLen)) )
+                    {
+                        capabilityInfo = zmw_buf_readh(dev, buf, 34);
+
+                        if ( capabilityInfo & ZM_BIT_1 )
+                        {
+                            if ( (wd->sta.capability[0] & ZM_BIT_4) ==
+                                 (capabilityInfo & ZM_BIT_4) )
+                            {
+                                zm_debug_msg0("IBSS merge");
+                                zfCopyFromRxBuffer(dev, buf, bssid,
+                                                   ZM_WLAN_HEADER_A3_OFFSET, 6);
+                                zfUpdateBssid(dev, bssid);
+                            }
+                        }
+                    }
+                }
+            }
+#endif
+        }
+    }
+
+    /* return if not channel scan */
+    if ( !wd->sta.bChannelScan )
+    {
+        goto zlReturn;
+    }
+
+    zfCopyFromRxBuffer(dev, buf, pBuf, 0, sizeof(struct zsWlanBeaconFrameHeader));
+    pBeaconHeader = (struct zsWlanBeaconFrameHeader*) pBuf;
+
+    zmw_enter_critical_section(dev);
+
+    //zm_debug_msg1("bss count = ", wd->sta.bssList.bssCount);
+
+    pBssInfo = zfStaFindBssInfo(dev, buf, pBeaconHeader);
+
+    if ( pBssInfo == NULL )
+    {
+        /* Allocate a new entry if BSS not in the scan list */
+        pBssInfo = zfBssInfoAllocate(dev);
+        if (pBssInfo != NULL)
+        {
+            res = zfStaInitBssInfo(dev, buf, pBeaconHeader, pBssInfo, AddInfo, 0);
+            //zfDumpSSID(pBssInfo->ssid[1], &(pBssInfo->ssid[2]));
+            if ( res != 0 )
+            {
+                zfBssInfoFree(dev, pBssInfo);
+            }
+            else
+            {
+                zfBssInfoInsertToList(dev, pBssInfo);
+            }
+        }
+    }
+    else
+    {
+        res = zfStaInitBssInfo(dev, buf, pBeaconHeader, pBssInfo, AddInfo, 1);
+        if (res == 2)
+        {
+            zfBssInfoRemoveFromList(dev, pBssInfo);
+            zfBssInfoFree(dev, pBssInfo);
+        }
+        else if ( wd->wlanMode == ZM_MODE_IBSS )
+        {
+            int idx;
+
+            // It would reset the alive counter if the peer station is found!
+            zfStaFindFreeOpposite(dev, (u16_t *)pBssInfo->macaddr, &idx);
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+
+zlReturn:
+
+    return;
+}
+
+
+void zfAuthFreqCompleteCb(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if (wd->sta.connectState == ZM_STA_CONN_STATE_AUTH_COMPLETED)
+    {
+        zm_debug_msg0("ZM_STA_CONN_STATE_ASSOCIATE");
+        wd->sta.connectTimer = wd->tick;
+        wd->sta.connectState = ZM_STA_CONN_STATE_ASSOCIATE;
+    }
+
+    zmw_leave_critical_section(dev);
+    return;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfProcessAuth               */
+/*      Process authenticate management frame.                          */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : auth frame buffer                                         */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+/* Note : AP allows one authenticating STA at a time, does not          */
+/*        support multiple authentication process. Make sure            */
+/*        authentication state machine will not be blocked due          */
+/*        to incompleted authentication handshake.                      */
+void zfStaProcessAuth(zdev_t* dev, zbuf_t* buf, u16_t* src, u16_t apId)
+{
+    struct zsWlanAuthFrameHeader* pAuthFrame;
+    u8_t  pBuf[sizeof(struct zsWlanAuthFrameHeader)];
+    u32_t p1, p2;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    if ( !zfStaIsConnecting(dev) )
+    {
+        return;
+    }
+
+    pAuthFrame = (struct zsWlanAuthFrameHeader*) pBuf;
+    zfCopyFromRxBuffer(dev, buf, pBuf, 0, sizeof(struct zsWlanAuthFrameHeader));
+
+    if ( wd->sta.connectState == ZM_STA_CONN_STATE_AUTH_OPEN )
+    {
+        if ( (zmw_le16_to_cpu(pAuthFrame->seq) == 2)&&
+             (zmw_le16_to_cpu(pAuthFrame->algo) == 0)&&
+             (zmw_le16_to_cpu(pAuthFrame->status) == 0) )
+        {
+
+            zmw_enter_critical_section(dev);
+            wd->sta.connectTimer = wd->tick;
+            zm_debug_msg0("ZM_STA_CONN_STATE_AUTH_COMPLETED");
+            wd->sta.connectState = ZM_STA_CONN_STATE_AUTH_COMPLETED;
+            zmw_leave_critical_section(dev);
+
+            //Set channel according to AP's configuration
+            //Move to here because of Cisco 11n AP feature
+            zfCoreSetFrequencyEx(dev, wd->frequency, wd->BandWidth40,
+                    wd->ExtOffset, zfAuthFreqCompleteCb);
+
+            /* send association frame */
+            if ( wd->sta.connectByReasso )
+            {
+                zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_REASOCREQ,
+                              wd->sta.bssid, 0, 0, 0);
+            }
+            else
+            {
+                zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_ASOCREQ,
+                              wd->sta.bssid, 0, 0, 0);
+            }
+
+
+        }
+        else
+        {
+            zm_debug_msg1("authentication failed, status = ",
+                          pAuthFrame->status);
+
+            if (wd->sta.authMode == ZM_AUTH_MODE_AUTO)
+            {
+                wd->sta.bIsSharedKey = 1;
+                zfStaStartConnect(dev, wd->sta.bIsSharedKey);
+            }
+            else
+            {
+                zm_debug_msg0("ZM_STA_STATE_DISCONNECT");
+                zfStaConnectFail(dev, ZM_STATUS_MEDIA_DISCONNECT_AUTH_FAILED, wd->sta.bssid, 3);
+            }
+        }
+    }
+    else if ( wd->sta.connectState == ZM_STA_CONN_STATE_AUTH_SHARE_1 )
+    {
+        if ( (zmw_le16_to_cpu(pAuthFrame->algo) == 1) &&
+             (zmw_le16_to_cpu(pAuthFrame->seq) == 2) &&
+             (zmw_le16_to_cpu(pAuthFrame->status) == 0))
+              //&& (pAuthFrame->challengeText[1] <= 255) )
+        {
+            zfMemoryCopy(wd->sta.challengeText, pAuthFrame->challengeText,
+                         pAuthFrame->challengeText[1]+2);
+
+            /* send the 3rd authentication frame */
+            p1 = 0x30001;
+            p2 = 0;
+            zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_AUTH,
+                          wd->sta.bssid, p1, p2, 0);
+
+            zmw_enter_critical_section(dev);
+            wd->sta.connectTimer = wd->tick;
+
+            zm_debug_msg0("ZM_STA_SUB_STATE_AUTH_SHARE_2");
+            wd->sta.connectState = ZM_STA_CONN_STATE_AUTH_SHARE_2;
+            zmw_leave_critical_section(dev);
+        }
+        else
+        {
+            zm_debug_msg1("authentication failed, status = ",
+                          pAuthFrame->status);
+
+            zm_debug_msg0("ZM_STA_STATE_DISCONNECT");
+            zfStaConnectFail(dev, ZM_STATUS_MEDIA_DISCONNECT_AUTH_FAILED, wd->sta.bssid, 3);
+        }
+    }
+    else if ( wd->sta.connectState == ZM_STA_CONN_STATE_AUTH_SHARE_2 )
+    {
+        if ( (zmw_le16_to_cpu(pAuthFrame->algo) == 1)&&
+             (zmw_le16_to_cpu(pAuthFrame->seq) == 4)&&
+             (zmw_le16_to_cpu(pAuthFrame->status) == 0) )
+        {
+            //Set channel according to AP's configuration
+            //Move to here because of Cisco 11n AP feature
+            zfCoreSetFrequencyEx(dev, wd->frequency, wd->BandWidth40,
+                    wd->ExtOffset, NULL);
+
+            /* send association frame */
+            zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_ASOCREQ,
+                          wd->sta.bssid, 0, 0, 0);
+
+            zmw_enter_critical_section(dev);
+            wd->sta.connectTimer = wd->tick;
+
+            zm_debug_msg0("ZM_STA_SUB_STATE_ASSOCIATE");
+            wd->sta.connectState = ZM_STA_CONN_STATE_ASSOCIATE;
+            zmw_leave_critical_section(dev);
+        }
+        else
+        {
+            zm_debug_msg1("authentication failed, status = ",
+                          pAuthFrame->status);
+
+            zm_debug_msg0("ZM_STA_STATE_DISCONNECT");
+            zfStaConnectFail(dev, ZM_STATUS_MEDIA_DISCONNECT_AUTH_FAILED, wd->sta.bssid, 3);
+        }
+    }
+    else
+    {
+        zm_debug_msg0("unknown case");
+    }
+}
+
+void zfStaProcessAsocReq(zdev_t* dev, zbuf_t* buf, u16_t* src, u16_t apId)
+{
+
+    return;
+}
+
+void zfStaProcessAsocRsp(zdev_t* dev, zbuf_t* buf)
+{
+    struct zsWlanAssoFrameHeader* pAssoFrame;
+    u8_t  pBuf[sizeof(struct zsWlanAssoFrameHeader)];
+    u16_t offset;
+    u32_t i;
+    u32_t oneTxStreamCap;
+
+    zmw_get_wlan_dev(dev);
+
+    if ( !zfStaIsConnecting(dev) )
+    {
+        return;
+    }
+
+    pAssoFrame = (struct zsWlanAssoFrameHeader*) pBuf;
+    zfCopyFromRxBuffer(dev, buf, pBuf, 0, sizeof(struct zsWlanAssoFrameHeader));
+
+    if ( wd->sta.connectState == ZM_STA_CONN_STATE_ASSOCIATE )
+    {
+        if ( pAssoFrame->status == 0 )
+        {
+            zm_debug_msg0("ZM_STA_STATE_CONNECTED");
+
+            if (wd->sta.EnableHT == 1)
+            {
+                wd->sta.wmeConnected = 1;
+            }
+            if ((wd->sta.wmeEnabled & ZM_STA_WME_ENABLE_BIT) != 0) //WME enabled
+            {
+                /* Asoc rsp may contain WME parameter element */
+                if ((offset = zfFindWifiElement(dev, buf, 2, 1)) != 0xffff)
+                {
+                    zm_debug_msg0("WME enable");
+                    wd->sta.wmeConnected = 1;
+                    if ((wd->sta.wmeEnabled & ZM_STA_UAPSD_ENABLE_BIT) != 0)
+                    {
+                        if ((zmw_rx_buf_readb(dev, buf, offset+8) & 0x80) != 0)
+                        {
+                            zm_debug_msg0("UAPSD enable");
+                            wd->sta.qosInfo = wd->sta.wmeQosInfo;
+                        }
+                    }
+
+                    zfStaUpdateWmeParameter(dev, buf);
+                }
+            }
+
+
+            //Store asoc response frame body, for VISTA only
+            wd->sta.asocRspFrameBodySize = zfwBufGetSize(dev, buf)-24;
+            if (wd->sta.asocRspFrameBodySize > ZM_CACHED_FRAMEBODY_SIZE)
+            {
+                wd->sta.asocRspFrameBodySize = ZM_CACHED_FRAMEBODY_SIZE;
+            }
+            for (i=0; i<wd->sta.asocRspFrameBodySize; i++)
+            {
+                wd->sta.asocRspFrameBody[i] = zmw_rx_buf_readb(dev, buf, i+24);
+            }
+
+            zfStaStoreAsocRspIe(dev, buf);
+            if (wd->sta.EnableHT &&
+                ((wd->sta.ie.HtCap.HtCapInfo & HTCAP_SupChannelWidthSet) != 0) &&
+                (wd->ExtOffset != 0))
+            {
+                wd->sta.htCtrlBandwidth = 1;
+            }
+            else
+            {
+                wd->sta.htCtrlBandwidth = 0;
+            }
+
+            //Set channel according to AP's configuration
+            //zfCoreSetFrequencyEx(dev, wd->frequency, wd->BandWidth40,
+            //        wd->ExtOffset, NULL);
+
+            if (wd->sta.EnableHT == 1)
+            {
+                wd->addbaComplete = 0;
+
+                if ((wd->sta.SWEncryptEnable & ZM_SW_TKIP_ENCRY_EN) == 0 &&
+                    (wd->sta.SWEncryptEnable & ZM_SW_WEP_ENCRY_EN) == 0)
+                {
+                    wd->addbaCount = 1;
+                    zfAggSendAddbaRequest(dev, wd->sta.bssid, 0, 0);
+                    zfTimerSchedule(dev, ZM_EVENT_TIMEOUT_ADDBA, 100);
+                }
+            }
+
+            /* set RIFS support */
+            if(wd->sta.ie.HtInfo.ChannelInfo & ExtHtCap_RIFSMode)
+            {
+                wd->sta.HT2040 = 1;
+//                zfHpSetRifs(dev, wd->sta.EnableHT, 1, (wd->sta.currentFrequency < 3000)? 1:0);
+            }
+
+            wd->sta.aid = pAssoFrame->aid & 0x3fff;
+            wd->sta.oppositeCount = 0;    /* reset opposite count */
+            zfStaSetOppositeInfoFromRxBuf(dev, buf);
+
+            wd->sta.rxBeaconCount = 16;
+
+            zfChangeAdapterState(dev, ZM_STA_STATE_CONNECTED);
+            wd->sta.connPowerInHalfDbm = zfHpGetTransmitPower(dev);
+            if (wd->zfcbConnectNotify != NULL)
+            {
+                if (wd->sta.EnableHT != 0) /* 11n */
+            	{
+    		        oneTxStreamCap = (zfHpCapability(dev) & ZM_HP_CAP_11N_ONE_TX_STREAM);
+    		        if (wd->sta.htCtrlBandwidth == 1) /* HT40*/
+    		        {
+    					if(oneTxStreamCap) /* one Tx stream */
+    				    {
+    				        if (wd->sta.SG40)
+    				        {
+    				            wd->CurrentTxRateKbps = 150000;
+    						    wd->CurrentRxRateKbps = 300000;
+    				        }
+    				        else
+    				        {
+    				            wd->CurrentTxRateKbps = 135000;
+    						    wd->CurrentRxRateKbps = 270000;
+    				        }
+    				    }
+    				    else /* Two Tx streams */
+    				    {
+    				        if (wd->sta.SG40)
+    				        {
+    				            wd->CurrentTxRateKbps = 300000;
+    						    wd->CurrentRxRateKbps = 300000;
+    				        }
+    				        else
+    				        {
+    				            wd->CurrentTxRateKbps = 270000;
+    						    wd->CurrentRxRateKbps = 270000;
+    				        }
+    				    }
+    		        }
+    		        else /* HT20 */
+    		        {
+    		            if(oneTxStreamCap) /* one Tx stream */
+    				    {
+    				        wd->CurrentTxRateKbps = 650000;
+    						wd->CurrentRxRateKbps = 130000;
+    				    }
+    				    else /* Two Tx streams */
+    				    {
+    				        wd->CurrentTxRateKbps = 130000;
+    					    wd->CurrentRxRateKbps = 130000;
+    				    }
+    		        }
+                }
+                else /* 11abg */
+                {
+                    if (wd->sta.connection_11b != 0)
+                    {
+                        wd->CurrentTxRateKbps = 11000;
+    			        wd->CurrentRxRateKbps = 11000;
+                    }
+                    else
+                    {
+                        wd->CurrentTxRateKbps = 54000;
+    			        wd->CurrentRxRateKbps = 54000;
+    			    }
+                }
+
+
+                wd->zfcbConnectNotify(dev, ZM_STATUS_MEDIA_CONNECT, wd->sta.bssid);
+            }
+            wd->sta.connectByReasso = TRUE;
+            wd->sta.failCntOfReasso = 0;
+
+            zfPowerSavingMgrConnectNotify(dev);
+
+            /* Disable here because fixed rate is only for test, TBD. */
+            //if (wd->sta.EnableHT)
+            //{
+            //    wd->txMCS = 7; //Rate = 65Mbps
+            //    wd->txMT = 2; // Ht rate
+            //    wd->enableAggregation = 2; // Enable Aggregation
+            //}
+        }
+        else
+        {
+            zm_debug_msg1("association failed, status = ",
+                          pAssoFrame->status);
+
+            zm_debug_msg0("ZM_STA_STATE_DISCONNECT");
+            wd->sta.connectByReasso = FALSE;
+            zfStaConnectFail(dev, ZM_STATUS_MEDIA_DISCONNECT_ASOC_FAILED, wd->sta.bssid, 3);
+        }
+    }
+
+}
+
+void zfStaStoreAsocRspIe(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t offset;
+    u32_t i;
+    u16_t length;
+    u8_t  *htcap;
+    u8_t  asocBw40 = 0;
+    u8_t  asocExtOffset = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    for (i=0; i<wd->sta.asocRspFrameBodySize; i++)
+    {
+        wd->sta.asocRspFrameBody[i] = zmw_rx_buf_readb(dev, buf, i+24);
+    }
+
+    /* HT capabilities: 28 octets */
+    if (    ((wd->sta.currentFrequency > 3000) && !(wd->supportMode & ZM_WIRELESS_MODE_5_N))
+         || ((wd->sta.currentFrequency < 3000) && !(wd->supportMode & ZM_WIRELESS_MODE_24_N)) )
+    {
+        /* not 11n AP */
+        htcap = (u8_t *)&wd->sta.ie.HtCap;
+        for (i=0; i<28; i++)
+        {
+            htcap[i] = 0;
+        }
+        wd->BandWidth40 = 0;
+        wd->ExtOffset = 0;
+        return;
+    }
+
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_HT_CAPABILITY)) != 0xffff)
+    {
+        /* atheros pre n */
+        zm_debug_msg0("atheros pre n");
+        htcap = (u8_t *)&wd->sta.ie.HtCap;
+        htcap[0] = zmw_rx_buf_readb(dev, buf, offset);
+        htcap[1] = 26;
+        for (i=1; i<=26; i++)
+        {
+            htcap[i+1] = zmw_rx_buf_readb(dev, buf, offset + i);
+            zm_msg2_mm(ZM_LV_1, "ASOC:  HT Capabilities, htcap=", htcap[i+1]);
+        }
+    }
+    else if ((offset = zfFindElement(dev, buf, ZM_WLAN_PREN2_EID_HTCAPABILITY)) != 0xffff)
+    {
+        /* pre n 2.0 standard */
+        zm_debug_msg0("pre n 2.0 standard");
+        htcap = (u8_t *)&wd->sta.ie.HtCap;
+        for (i=0; i<28; i++)
+        {
+            htcap[i] = zmw_rx_buf_readb(dev, buf, offset + i);
+            zm_msg2_mm(ZM_LV_1, "ASOC:  HT Capabilities, htcap=", htcap[i]);
+        }
+    }
+    else
+    {
+        /* not 11n AP */
+        htcap = (u8_t *)&wd->sta.ie.HtCap;
+        for (i=0; i<28; i++)
+        {
+            htcap[i] = 0;
+        }
+        wd->BandWidth40 = 0;
+        wd->ExtOffset = 0;
+        return;
+    }
+
+    asocBw40 = (u8_t)((wd->sta.ie.HtCap.HtCapInfo & HTCAP_SupChannelWidthSet) >> 1);
+
+    /* HT information */
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_EXTENDED_HT_CAPABILITY)) != 0xffff)
+    {
+        /* atheros pre n */
+        zm_debug_msg0("atheros pre n HTINFO");
+        length = 22;
+        htcap = (u8_t *)&wd->sta.ie.HtInfo;
+        htcap[0] = zmw_rx_buf_readb(dev, buf, offset);
+        htcap[1] = 22;
+        for (i=1; i<=22; i++)
+        {
+            htcap[i+1] = zmw_rx_buf_readb(dev, buf, offset + i);
+            zm_msg2_mm(ZM_LV_1, "ASOC:  HT Info, htinfo=", htcap[i+1]);
+        }
+    }
+    else if ((offset = zfFindElement(dev, buf, ZM_WLAN_PREN2_EID_HTINFORMATION)) != 0xffff)
+    {
+        /* pre n 2.0 standard */
+        zm_debug_msg0("pre n 2.0 standard HTINFO");
+        length = zmw_rx_buf_readb(dev, buf, offset + 1);
+        htcap = (u8_t *)&wd->sta.ie.HtInfo;
+        for (i=0; i<24; i++)
+        {
+            htcap[i] = zmw_rx_buf_readb(dev, buf, offset + i);
+            zm_msg2_mm(ZM_LV_1, "ASOC:  HT Info, htinfo=", htcap[i]);
+        }
+    }
+    else
+    {
+        zm_debug_msg0("no HTINFO");
+        htcap = (u8_t *)&wd->sta.ie.HtInfo;
+        for (i=0; i<24; i++)
+        {
+            htcap[i] = 0;
+        }
+    }
+    asocExtOffset = wd->sta.ie.HtInfo.ChannelInfo & ExtHtCap_ExtChannelOffsetBelow;
+
+    if ((wd->sta.EnableHT == 1) && (asocBw40 == 1) && ((asocExtOffset == 1) || (asocExtOffset == 3)))
+    {
+        wd->BandWidth40 = asocBw40;
+        wd->ExtOffset = asocExtOffset;
+    }
+    else
+    {
+        wd->BandWidth40 = 0;
+        wd->ExtOffset = 0;
+    }
+
+    return;
+}
+
+void zfStaProcessDeauth(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t apMacAddr[3];
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    /* STA : if SA=connected AP then disconnect with AP */
+    if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+    {
+        apMacAddr[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET);
+        apMacAddr[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET+2);
+        apMacAddr[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET+4);
+  	if ((apMacAddr[0] == wd->sta.bssid[0]) && (apMacAddr[1] == wd->sta.bssid[1]) && (apMacAddr[2] == wd->sta.bssid[2]))
+        {
+            if (zfwBufGetSize(dev, buf) >= 24+2) //not a malformed frame
+            {
+                if ( zfStaIsConnected(dev) )
+                {
+                    zfStaConnectFail(dev, ZM_STATUS_MEDIA_DISCONNECT_DEAUTH, wd->sta.bssid, 2);
+                }
+                else if (zfStaIsConnecting(dev))
+                {
+                    zfStaConnectFail(dev, ZM_STATUS_MEDIA_DISCONNECT_AUTH_FAILED, wd->sta.bssid, 3);
+                }
+                else
+                {
+                }
+            }
+        }
+    }
+    else if ( wd->wlanMode == ZM_MODE_IBSS )
+    {
+        u16_t peerMacAddr[3];
+        u8_t  peerIdx;
+        s8_t  res;
+
+        if ( zfStaIsConnected(dev) )
+        {
+            peerMacAddr[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET);
+            peerMacAddr[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+2);
+            peerMacAddr[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+4);
+
+            zmw_enter_critical_section(dev);
+            res = zfStaFindOppositeByMACAddr(dev, peerMacAddr, &peerIdx);
+            if ( res == 0 )
+            {
+                wd->sta.oppositeInfo[peerIdx].aliveCounter = 0;
+            }
+            zmw_leave_critical_section(dev);
+        }
+    }
+}
+
+void zfStaProcessDisasoc(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t apMacAddr[3];
+
+    zmw_get_wlan_dev(dev);
+
+    /* STA : if SA=connected AP then disconnect with AP */
+    if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+    {
+        apMacAddr[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET);
+        apMacAddr[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET+2);
+        apMacAddr[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET+4);
+
+        if ((apMacAddr[0] == wd->sta.bssid[0]) && (apMacAddr[1] == wd->sta.bssid[1]) && (apMacAddr[2] == wd->sta.bssid[2]))
+        {
+            if (zfwBufGetSize(dev, buf) >= 24+2) //not a malformed frame
+            {
+                if ( zfStaIsConnected(dev) )
+                {
+                    zfStaConnectFail(dev, ZM_STATUS_MEDIA_DISCONNECT_DISASOC, wd->sta.bssid, 2);
+                }
+                else
+                {
+                    zfStaConnectFail(dev, ZM_STATUS_MEDIA_DISCONNECT_ASOC_FAILED, wd->sta.bssid, 3);
+                }
+            }
+        }
+    }
+}
+
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfProcessProbeReq           */
+/*      Process probe request management frame.                         */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : auth frame buffer                                         */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+void zfStaProcessProbeReq(zdev_t* dev, zbuf_t* buf, u16_t* src)
+{
+    u16_t offset;
+    u8_t len;
+    u16_t i, j;
+    u16_t sendFlag;
+
+    zmw_get_wlan_dev(dev);
+
+    /* check mode : AP/IBSS */
+    if ((wd->wlanMode != ZM_MODE_AP) || (wd->wlanMode != ZM_MODE_IBSS))
+    {
+        zm_msg0_mm(ZM_LV_3, "Ignore probe req");
+        return;
+    }
+
+    /* check SSID */
+    if ((offset = zfFindElement(dev, buf, ZM_WLAN_EID_SSID)) == 0xffff)
+    {
+        zm_msg0_mm(ZM_LV_3, "probe req SSID not found");
+        return;
+    }
+
+    len = zmw_rx_buf_readb(dev, buf, offset+1);
+
+    for (i=0; i<ZM_MAX_AP_SUPPORT; i++)
+    {
+        if ((wd->ap.apBitmap & (i<<i)) != 0)
+        {
+            sendFlag = 0;
+            /* boardcast SSID */
+            if ((len == 0) && (wd->ap.hideSsid[i] == 0))
+            {
+                sendFlag = 1;
+            }
+            /* Not broadcast SSID */
+            else if (wd->ap.ssidLen[i] == len)
+            {
+                for (j=0; j<len; j++)
+                {
+                    if (zmw_rx_buf_readb(dev, buf, offset+1+j)
+                            != wd->ap.ssid[i][j])
+                    {
+                        break;
+                    }
+                }
+                if (j == len)
+                {
+                    sendFlag = 1;
+                }
+            }
+            if (sendFlag == 1)
+            {
+                /* Send probe response */
+                zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_PROBERSP, src, i, 0, 0);
+            }
+        }
+    }
+}
+
+void zfStaProcessProbeRsp(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* AddInfo)
+{
+    /* return if not channel scan */
+    // Probe response is sent with unicast. Is this required?
+    // IBSS would send probe request and the code below would prevent
+    // the probe response from handling.
+    #if 0
+    zmw_get_wlan_dev(dev);
+
+    if ( !wd->sta.bChannelScan )
+    {
+        return;
+    }
+    #endif
+
+    zfProcessProbeRsp(dev, buf, AddInfo);
+}
+
+void zfIBSSSetupBssDesc(zdev_t *dev)
+{
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+    u8_t i;
+#endif
+    struct zsBssInfo *pBssInfo;
+    u16_t offset = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    pBssInfo = &wd->sta.ibssBssDesc;
+    zfZeroMemory((u8_t *)pBssInfo, sizeof(struct zsBssInfo));
+
+    pBssInfo->signalStrength = 100;
+
+    zfMemoryCopy((u8_t *)pBssInfo->macaddr, (u8_t *)wd->macAddr,6);
+    zfMemoryCopy((u8_t *)pBssInfo->bssid, (u8_t *)wd->sta.bssid, 6);
+
+    pBssInfo->beaconInterval[0] = (u8_t)(wd->beaconInterval) ;
+    pBssInfo->beaconInterval[1] = (u8_t)((wd->beaconInterval) >> 8) ;
+
+    pBssInfo->capability[0] = wd->sta.capability[0];
+    pBssInfo->capability[1] = wd->sta.capability[1];
+
+    pBssInfo->ssid[0] = ZM_WLAN_EID_SSID;
+    pBssInfo->ssid[1] = wd->sta.ssidLen;
+    zfMemoryCopy((u8_t *)&pBssInfo->ssid[2], (u8_t *)wd->sta.ssid, wd->sta.ssidLen);
+    zfMemoryCopy((u8_t *)&pBssInfo->frameBody[offset], (u8_t *)pBssInfo->ssid,
+                 wd->sta.ssidLen + 2);
+    offset += wd->sta.ssidLen + 2;
+
+    /* support rate */
+
+    /* DS parameter set */
+    pBssInfo->channel = zfChFreqToNum(wd->frequency, NULL);
+    pBssInfo->frequency = wd->frequency;
+    pBssInfo->atimWindow = wd->sta.atimWindow;
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+    if ( wd->sta.authMode == ZM_AUTH_MODE_WPA2PSK )
+    {
+        u8_t rsn[64]=
+        {
+                    /* Element ID */
+                    0x30,
+                    /* Length */
+                    0x14,
+                    /* Version */
+                    0x01, 0x00,
+                    /* Group Cipher Suite, default=TKIP */
+                    0x00, 0x0f, 0xac, 0x04,
+                    /* Pairwise Cipher Suite Count */
+                    0x01, 0x00,
+                    /* Pairwise Cipher Suite, default=TKIP */
+                    0x00, 0x0f, 0xac, 0x02,
+                    /* Authentication and Key Management Suite Count */
+                    0x01, 0x00,
+                    /* Authentication type, default=PSK */
+                    0x00, 0x0f, 0xac, 0x02,
+                    /* RSN capability */
+                    0x00, 0x00
+        };
+
+        /* Overwrite Group Cipher Suite by AP's setting */
+        zfMemoryCopy(rsn+4, zgWpa2AesOui, 4);
+
+        if ( wd->sta.wepStatus == ZM_ENCRYPTION_AES )
+        {
+            /* Overwrite Pairwise Cipher Suite by AES */
+            zfMemoryCopy(rsn+10, zgWpa2AesOui, 4);
+        }
+
+        // RSN element id
+        pBssInfo->frameBody[offset++] = ZM_WLAN_EID_RSN_IE ;
+
+        // RSN length
+        pBssInfo->frameBody[offset++] = rsn[1] ;
+
+        // RSN information
+        for(i=0; i<rsn[1]; i++)
+        {
+            pBssInfo->frameBody[offset++] = rsn[i+2] ;
+        }
+
+        zfMemoryCopy(pBssInfo->rsnIe, rsn, rsn[1]+2);
+    }
+#endif
+}
+
+void zfIbssConnectNetwork(zdev_t* dev)
+{
+    struct zsBssInfo* pBssInfo;
+    struct zsBssInfo tmpBssInfo;
+    u8_t   macAddr[6], bssid[6], bssNotFound = TRUE;
+    u16_t  i, j=100;
+    u16_t  k;
+    struct zsPartnerNotifyEvent event;
+    u32_t  channelFlags;
+    u16_t  oppositeWepStatus;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    /* change state to CONNECTING and stop the channel scanning */
+    zfChangeAdapterState(dev, ZM_STA_STATE_CONNECTING);
+    zfPowerSavingMgrWakeup(dev);
+
+    /* Set TxQs CWMIN, CWMAX, AIFS and TXO to WME STA default. */
+    zfUpdateDefaultQosParameter(dev, 0);
+
+    wd->sta.bProtectionMode = FALSE;
+    zfHpSetSlotTime(dev, 1);
+
+    /* ESS bit off */
+    wd->sta.capability[0] &= ~ZM_BIT_0;
+    /* IBSS bit on */
+    wd->sta.capability[0] |= ZM_BIT_1;
+    /* not not use short slot time */
+    wd->sta.capability[1] &= ~ZM_BIT_2;
+
+    wd->sta.wmeConnected = 0;
+    wd->sta.psMgr.tempWakeUp = 0;
+    wd->sta.qosInfo = 0;
+    wd->sta.EnableHT = 0;
+    wd->BandWidth40 = 0;
+    wd->ExtOffset = 0;
+
+    if ( wd->sta.bssList.bssCount )
+    {
+        //Reorder BssList by RSSI--CWYang(+)
+        zfBssInfoReorderList(dev);
+
+        zmw_enter_critical_section(dev);
+
+        pBssInfo = wd->sta.bssList.head;
+
+        for(i=0; i<wd->sta.bssList.bssCount; i++)
+        {
+            // 20070806 #1 Privacy bit
+            if ( pBssInfo->capability[0] & ZM_BIT_4 )
+            { // Privacy Ibss network
+//                zm_debug_msg0("Privacy bit on");
+                oppositeWepStatus = ZM_ENCRYPTION_WEP_ENABLED;
+
+                if ( pBssInfo->rsnIe[1] != 0 )
+                {
+                    if ( (pBssInfo->rsnIe[7] == 0x01) || (pBssInfo->rsnIe[7] == 0x05) )
+                    { // WEP-40 & WEP-104
+//                        zm_debug_msg0("WEP40 or WEP104");
+                        oppositeWepStatus = ZM_ENCRYPTION_WEP_ENABLED;
+                    }
+                    else if ( pBssInfo->rsnIe[7] == 0x02 )
+                    { // TKIP
+//                        zm_debug_msg0("TKIP");
+                        oppositeWepStatus = ZM_ENCRYPTION_TKIP;
+                    }
+                    else if ( pBssInfo->rsnIe[7] == 0x04 )
+                    { // AES
+//                        zm_debug_msg0("CCMP-AES");
+                        oppositeWepStatus = ZM_ENCRYPTION_AES;
+                    }
+                }
+            }
+            else
+            {
+//                zm_debug_msg0("Privacy bit off");
+                oppositeWepStatus = ZM_ENCRYPTION_WEP_DISABLED;
+            }
+
+            if ( (zfMemoryIsEqual(&(pBssInfo->ssid[2]), wd->sta.ssid,
+                                  wd->sta.ssidLen))&&
+                 (wd->sta.ssidLen == pBssInfo->ssid[1])&&
+                 (oppositeWepStatus == wd->sta.wepStatus) )
+            {
+                /* Check support mode */
+                if (pBssInfo->frequency > 3000) {
+                    if (   (pBssInfo->EnableHT == 1)
+                        || (pBssInfo->apCap & ZM_All11N_AP) ) //11n AP
+                    {
+                        channelFlags = CHANNEL_A_HT;
+                        if (pBssInfo->enableHT40 == 1) {
+                            channelFlags |= CHANNEL_HT40;
+                        }
+                    } else {
+                        channelFlags = CHANNEL_A;
+                    }
+                } else {
+                    if (   (pBssInfo->EnableHT == 1)
+                        || (pBssInfo->apCap & ZM_All11N_AP) ) //11n AP
+                    {
+                        channelFlags = CHANNEL_G_HT;
+                        if(pBssInfo->enableHT40 == 1) {
+                            channelFlags |= CHANNEL_HT40;
+                        }
+                    } else {
+                        if (pBssInfo->extSupportedRates[1] == 0) {
+                            channelFlags = CHANNEL_B;
+                        } else {
+                            channelFlags = CHANNEL_G;
+                        }
+                    }
+                }
+
+                if (   ((channelFlags == CHANNEL_B) && (wd->connectMode & ZM_BIT_0))
+                    || ((channelFlags == CHANNEL_G) && (wd->connectMode & ZM_BIT_1))
+                    || ((channelFlags == CHANNEL_A) && (wd->connectMode & ZM_BIT_2))
+                    || ((channelFlags & CHANNEL_HT20) && (wd->connectMode & ZM_BIT_3)) )
+                {
+                    pBssInfo = pBssInfo->next;
+                    continue;
+                }
+
+                /* Bypass DFS channel */
+                if (zfHpIsDfsChannelNCS(dev, pBssInfo->frequency))
+                {
+                    zm_debug_msg0("Bypass DFS channel");
+                    continue;
+                }
+
+                /* check IBSS bit */
+                if ( pBssInfo->capability[0] & ZM_BIT_1 )
+                {
+                    /* may check timestamp here */
+                    j = i;
+                    break;
+                }
+            }
+
+            pBssInfo = pBssInfo->next;
+        }
+
+        if ((j < wd->sta.bssList.bssCount) && (pBssInfo != NULL))
+        {
+            zfwMemoryCopy((u8_t*)&tmpBssInfo, (u8_t*)(pBssInfo), sizeof(struct zsBssInfo));
+            pBssInfo = &tmpBssInfo;
+        }
+        else
+        {
+            pBssInfo = NULL;
+        }
+
+        zmw_leave_critical_section(dev);
+
+        //if ( j < wd->sta.bssList.bssCount )
+        if (pBssInfo != NULL)
+        {
+            int res;
+
+            zm_debug_msg0("IBSS found");
+
+            /* Found IBSS, reset bssNotFoundCount */
+            zmw_enter_critical_section(dev);
+            wd->sta.bssNotFoundCount = 0;
+            zmw_leave_critical_section(dev);
+
+            bssNotFound = FALSE;
+            wd->sta.atimWindow = pBssInfo->atimWindow;
+            wd->frequency = pBssInfo->frequency;
+            //wd->sta.flagFreqChanging = 1;
+            zfCoreSetFrequency(dev, wd->frequency);
+            zfUpdateBssid(dev, pBssInfo->bssid);
+            zfResetSupportRate(dev, ZM_DEFAULT_SUPPORT_RATE_ZERO);
+            zfUpdateSupportRate(dev, pBssInfo->supportedRates);
+            zfUpdateSupportRate(dev, pBssInfo->extSupportedRates);
+            wd->beaconInterval = pBssInfo->beaconInterval[0] +
+                                 (((u16_t) pBssInfo->beaconInterval[1]) << 8);
+
+            if (wd->beaconInterval == 0)
+            {
+                wd->beaconInterval = 100;
+            }
+
+            /* rsn information element */
+            if ( pBssInfo->rsnIe[1] != 0 )
+            {
+                zfMemoryCopy(wd->sta.rsnIe, pBssInfo->rsnIe,
+                             pBssInfo->rsnIe[1]+2);
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+                /* If not use RSNA , run traditional */
+                zmw_enter_critical_section(dev);
+                wd->sta.ibssWpa2Psk = 1;
+                zmw_leave_critical_section(dev);
+#endif
+            }
+            else
+            {
+                wd->sta.rsnIe[1] = 0;
+            }
+
+            /* privacy bit */
+            if ( pBssInfo->capability[0] & ZM_BIT_4 )
+            {
+                wd->sta.capability[0] |= ZM_BIT_4;
+            }
+            else
+            {
+                wd->sta.capability[0] &= ~ZM_BIT_4;
+            }
+
+            /* preamble type */
+            wd->preambleTypeInUsed = wd->preambleType;
+            if ( wd->preambleTypeInUsed == ZM_PREAMBLE_TYPE_AUTO )
+            {
+                if (pBssInfo->capability[0] & ZM_BIT_5)
+                {
+                    wd->preambleTypeInUsed = ZM_PREAMBLE_TYPE_SHORT;
+                }
+                else
+                {
+                    wd->preambleTypeInUsed = ZM_PREAMBLE_TYPE_LONG;
+                }
+            }
+
+            if (wd->preambleTypeInUsed == ZM_PREAMBLE_TYPE_LONG)
+            {
+                wd->sta.capability[0] &= ~ZM_BIT_5;
+            }
+            else
+            {
+                wd->sta.capability[0] |= ZM_BIT_5;
+            }
+
+            wd->sta.beaconFrameBodySize = pBssInfo->frameBodysize + 12;
+
+            if (wd->sta.beaconFrameBodySize > ZM_CACHED_FRAMEBODY_SIZE)
+            {
+                wd->sta.beaconFrameBodySize = ZM_CACHED_FRAMEBODY_SIZE;
+            }
+
+            for (k=0; k<8; k++)
+            {
+                wd->sta.beaconFrameBody[k] = pBssInfo->timeStamp[k];
+            }
+            wd->sta.beaconFrameBody[8] = pBssInfo->beaconInterval[0];
+            wd->sta.beaconFrameBody[9] = pBssInfo->beaconInterval[1];
+            wd->sta.beaconFrameBody[10] = pBssInfo->capability[0];
+            wd->sta.beaconFrameBody[11] = pBssInfo->capability[1];
+            //for (k=12; k<wd->sta.beaconFrameBodySize; k++)
+            for (k=0; k<pBssInfo->frameBodysize; k++)
+            {
+                wd->sta.beaconFrameBody[k+12] = pBssInfo->frameBody[k];
+            }
+
+            zmw_enter_critical_section(dev);
+            res = zfStaSetOppositeInfoFromBSSInfo(dev, pBssInfo);
+            if ( res == 0 )
+            {
+                zfMemoryCopy(event.bssid, (u8_t *)(pBssInfo->bssid), 6);
+                zfMemoryCopy(event.peerMacAddr, (u8_t *)(pBssInfo->macaddr), 6);
+            }
+            zmw_leave_critical_section(dev);
+
+            //zfwIbssPartnerNotify(dev, 1, &event);
+            goto connect_done;
+        }
+    }
+
+    /* IBSS not found */
+    if ( bssNotFound )
+    {
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+        u16_t offset ;
+#endif
+        if ( wd->sta.ibssJoinOnly )
+        {
+            zm_debug_msg0("IBSS join only...retry...");
+            goto retry_ibss;
+        }
+
+        if(wd->sta.bssNotFoundCount<2)
+        {
+            zmw_enter_critical_section(dev);
+            zm_debug_msg1("IBSS not found, do sitesurvey!!  bssNotFoundCount=", wd->sta.bssNotFoundCount);
+            wd->sta.bssNotFoundCount++;
+            zmw_leave_critical_section(dev);
+            goto retry_ibss;
+        }
+        else
+        {
+            zmw_enter_critical_section(dev);
+            /* Fail IBSS found, TODO create IBSS */
+            wd->sta.bssNotFoundCount = 0;
+            zmw_leave_critical_section(dev);
+        }
+
+
+        if (zfHpIsDfsChannel(dev, wd->frequency))
+        {
+            wd->frequency = zfHpFindFirstNonDfsChannel(dev, wd->frequency > 3000);
+        }
+
+        if( wd->ws.autoSetFrequency == 0 )
+        { /* Auto set frequency */
+            zm_debug_msg1("Create Ad Hoc Network Band ", wd->ws.adhocMode);
+            wd->frequency = zfFindCleanFrequency(dev, wd->ws.adhocMode);
+            wd->ws.autoSetFrequency = 0xff;
+        }
+        zm_debug_msg1("IBSS not found, created one in channel ", wd->frequency);
+
+        wd->sta.ibssBssIsCreator = 1;
+
+        //wd->sta.flagFreqChanging = 1;
+        zfCoreSetFrequency(dev, wd->frequency);
+        if (wd->sta.bDesiredBssid == TRUE)
+        {
+            for (k=0; k<6; k++)
+            {
+                bssid[k] = wd->sta.desiredBssid[k];
+            }
+        }
+        else
+        {
+            #if 1
+            macAddr[0] = (wd->macAddr[0] & 0xff);
+            macAddr[1] = (wd->macAddr[0] >> 8);
+            macAddr[2] = (wd->macAddr[1] & 0xff);
+            macAddr[3] = (wd->macAddr[1] >> 8);
+            macAddr[4] = (wd->macAddr[2] & 0xff);
+            macAddr[5] = (wd->macAddr[2] >> 8);
+            zfGenerateRandomBSSID(dev, (u8_t *)wd->macAddr, (u8_t *)bssid);
+            #else
+            for (k=0; k<6; k++)
+            {
+                bssid[k] = (u8_t) zfGetRandomNumber(dev, 0);
+            }
+            bssid[0] &= ~ZM_BIT_0;
+            bssid[0] |= ZM_BIT_1;
+            #endif
+        }
+
+        zfUpdateBssid(dev, bssid);
+        //wd->sta.atimWindow = 0x0a;
+
+        /* rate information */
+        if(wd->frequency <= ZM_CH_G_14)  // 2.4 GHz  b+g
+        {
+            if ( wd->wfc.bIbssGMode
+                 && (wd->supportMode & (ZM_WIRELESS_MODE_24_54|ZM_WIRELESS_MODE_24_N)) )
+            {
+                zfResetSupportRate(dev, ZM_DEFAULT_SUPPORT_RATE_IBSS_AG);
+            }
+            else
+            {
+                zfResetSupportRate(dev, ZM_DEFAULT_SUPPORT_RATE_IBSS_B);
+            }
+        } else {
+            zfResetSupportRate(dev, ZM_DEFAULT_SUPPORT_RATE_IBSS_AG);
+        }
+
+        if ( wd->sta.wepStatus == ZM_ENCRYPTION_WEP_DISABLED )
+        {
+            wd->sta.capability[0] &= ~ZM_BIT_4;
+        }
+        else
+        {
+            wd->sta.capability[0] |= ZM_BIT_4;
+        }
+
+        wd->preambleTypeInUsed = wd->preambleType;
+        if (wd->preambleTypeInUsed == ZM_PREAMBLE_TYPE_LONG)
+        {
+            wd->sta.capability[0] &= ~ZM_BIT_5;
+        }
+        else
+        {
+            wd->preambleTypeInUsed = ZM_PREAMBLE_TYPE_SHORT;
+            wd->sta.capability[0] |= ZM_BIT_5;
+        }
+
+        zfIBSSSetupBssDesc(dev);
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+
+        // 20070411 Add WPA2PSK information to its IBSS network !!!
+        offset = 0 ;
+
+        /* timestamp */
+        offset += 8 ;
+
+        /* beacon interval */
+        wd->sta.beaconFrameBody[offset++] = (u8_t)(wd->beaconInterval) ;
+        wd->sta.beaconFrameBody[offset++] = (u8_t)((wd->beaconInterval) >> 8) ;
+
+        /* capability information */
+        wd->sta.beaconFrameBody[offset++] = wd->sta.capability[0] ;
+        wd->sta.beaconFrameBody[offset++] = wd->sta.capability[1] ;
+        #if 0
+        /* ssid */
+        // ssid element id
+        wd->sta.beaconFrameBody[offset++] = ZM_WLAN_EID_SSID ;
+        // ssid length
+        wd->sta.beaconFrameBody[offset++] = wd->sta.ssidLen ;
+        // ssid information
+        for(i=0; i<wd->sta.ssidLen; i++)
+        {
+            wd->sta.beaconFrameBody[offset++] = wd->sta.ssid[i] ;
+        }
+
+        /* support rate */
+        rateSet = ZM_RATE_SET_CCK ;
+        if ( (rateSet == ZM_RATE_SET_OFDM)&&((wd->gRate & 0xff) == 0) )
+        {
+            offset += 0 ;
+        }
+        else
+        {
+            // support rate element id
+            wd->sta.beaconFrameBody[offset++] = ZM_WLAN_EID_SUPPORT_RATE ;
+
+            // support rate length
+            lenOffset = offset++;
+
+            // support rate information
+            for (i=0; i<4; i++)
+            {
+                if ((wd->bRate & (0x1<<i)) == (0x1<<i))
+                {
+                    wd->sta.beaconFrameBody[offset++] =
+                	    zg11bRateTbl[i]+((wd->bRateBasic & (0x1<<i))<<(7-i)) ;
+                    len++;
+                }
+            }
+
+            // support rate length
+            wd->sta.beaconFrameBody[lenOffset] = len ;
+        }
+
+        /* DS parameter set */
+        // DS parameter set elemet id
+        wd->sta.beaconFrameBody[offset++] = ZM_WLAN_EID_DS ;
+
+        // DS parameter set length
+        wd->sta.beaconFrameBody[offset++] = 1 ;
+
+        // DS parameter set information
+        wd->sta.beaconFrameBody[offset++] =
+         	zfChFreqToNum(wd->frequency, NULL) ;
+
+        /* IBSS parameter set */
+        // IBSS parameter set element id
+        wd->sta.beaconFrameBody[offset++] = ZM_WLAN_EID_IBSS ;
+
+        // IBSS parameter set length
+        wd->sta.beaconFrameBody[offset++] = 2 ;
+
+        // IBSS parameter set information
+        wd->sta.beaconFrameBody[offset] = wd->sta.atimWindow ;
+        offset += 2 ;
+
+        /* ERP Information and Extended Supported Rates */
+        if ( wd->wfc.bIbssGMode
+             && (wd->supportMode & (ZM_WIRELESS_MODE_24_54|ZM_WIRELESS_MODE_24_N)) )
+        {
+            /* ERP Information */
+            wd->erpElement = 0;
+            // ERP element id
+            wd->sta.beaconFrameBody[offset++] = ZM_WLAN_EID_ERP ;
+
+            // ERP length
+            wd->sta.beaconFrameBody[offset++] = 1 ;
+
+            // ERP information
+            wd->sta.beaconFrameBody[offset++] = wd->erpElement ;
+
+            /* Extended Supported Rates */
+            if ( (rateSet == ZM_RATE_SET_OFDM)&&((wd->gRate & 0xff) == 0) )
+            {
+                offset += 0 ;
+            }
+            else
+            {
+                len = 0 ;
+
+                // Extended Supported Rates element id
+                wd->sta.beaconFrameBody[offset++] = ZM_WLAN_EID_EXTENDED_RATE ;
+
+                // Extended Supported Rates length
+                lenOffset = offset++ ;
+
+                // Extended Supported Rates information
+                for (i=0; i<8; i++)
+                {
+                    if ((wd->gRate & (0x1<<i)) == (0x1<<i))
+                    {
+                        wd->sta.beaconFrameBody[offset++] =
+                                     zg11gRateTbl[i]+((wd->gRateBasic & (0x1<<i))<<(7-i));
+                        len++;
+                    }
+                }
+
+                // extended support rate length
+            	  wd->sta.beaconFrameBody[lenOffset] = len ;
+            }
+        }
+        #endif
+
+        /* RSN : important information influence the result of creating an IBSS network */
+        if ( wd->sta.authMode == ZM_AUTH_MODE_WPA2PSK )
+        {
+            u8_t frameType = ZM_WLAN_FRAME_TYPE_AUTH ;
+            u8_t    rsn[64]=
+            {
+                        /* Element ID */
+                        0x30,
+                        /* Length */
+                        0x14,
+                        /* Version */
+                        0x01, 0x00,
+                        /* Group Cipher Suite, default=TKIP */
+                        0x00, 0x0f, 0xac, 0x04,
+                        /* Pairwise Cipher Suite Count */
+                        0x01, 0x00,
+                        /* Pairwise Cipher Suite, default=TKIP */
+                        0x00, 0x0f, 0xac, 0x02,
+                        /* Authentication and Key Management Suite Count */
+                        0x01, 0x00,
+                        /* Authentication type, default=PSK */
+                        0x00, 0x0f, 0xac, 0x02,
+                        /* RSN capability */
+                        0x00, 0x00
+            };
+
+            /* Overwrite Group Cipher Suite by AP's setting */
+            zfMemoryCopy(rsn+4, zgWpa2AesOui, 4);
+
+            if ( wd->sta.wepStatus == ZM_ENCRYPTION_AES )
+            {
+                /* Overwrite Pairwise Cipher Suite by AES */
+                zfMemoryCopy(rsn+10, zgWpa2AesOui, 4);
+            }
+
+            // RSN element id
+            wd->sta.beaconFrameBody[offset++] = ZM_WLAN_EID_RSN_IE ;
+
+            // RSN length
+            wd->sta.beaconFrameBody[offset++] = rsn[1] ;
+
+            // RSN information
+            for(i=0; i<rsn[1]; i++)
+                wd->sta.beaconFrameBody[offset++] = rsn[i+2] ;
+
+            zfMemoryCopy(wd->sta.rsnIe, rsn, rsn[1]+2);
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+            /* If not use RSNA , run traditional */
+            zmw_enter_critical_section(dev);
+            wd->sta.ibssWpa2Psk = 1;
+            zmw_leave_critical_section(dev);
+#endif
+        }
+
+        #if 0
+        /* HT Capabilities Info */
+        {
+            u8_t OUI[3] = { 0x0 , 0x90 , 0x4C } ;
+
+            wd->sta.beaconFrameBody[offset++] = ZM_WLAN_EID_WPA_IE ;
+
+            wd->sta.beaconFrameBody[offset++] = wd->sta.HTCap.Data.Length + 4 ;
+
+            for (i = 0; i < 3; i++)
+            {
+                wd->sta.beaconFrameBody[offset++] = OUI[i] ;
+            }
+
+            wd->sta.beaconFrameBody[offset++] = wd->sta.HTCap.Data.ElementID ;
+
+            for (i = 0; i < 26; i++)
+            {
+                wd->sta.beaconFrameBody[offset++] = wd->sta.HTCap.Byte[i+2] ;
+            }
+        }
+
+        /* Extended HT Capabilities Info */
+        {
+            u8_t OUI[3] = { 0x0 , 0x90 , 0x4C } ;
+
+            wd->sta.beaconFrameBody[offset++] = ZM_WLAN_EID_WPA_IE ;
+
+            wd->sta.beaconFrameBody[offset++] = wd->sta.ExtHTCap.Data.Length + 4 ;
+
+            for (i = 0; i < 3; i++)
+            {
+                wd->sta.beaconFrameBody[offset++] = OUI[i] ;
+            }
+
+            wd->sta.beaconFrameBody[offset++] = wd->sta.ExtHTCap.Data.ElementID ;
+
+            for (i = 0; i < 22; i++)
+            {
+                wd->sta.beaconFrameBody[offset++] = wd->sta.ExtHTCap.Byte[i+2] ;
+            }
+        }
+        #endif
+
+        wd->sta.beaconFrameBodySize = offset ;
+
+        if (wd->sta.beaconFrameBodySize > ZM_CACHED_FRAMEBODY_SIZE)
+        {
+            wd->sta.beaconFrameBodySize = ZM_CACHED_FRAMEBODY_SIZE;
+        }
+
+        // 20070416 Let Create IBSS network could enter the zfwIbssPartnerNotify function
+        // bssNotFound = FALSE ;
+
+        printk("The capability info 1 = %02x\n", wd->sta.capability[0]) ;
+        printk("The capability info 2 = %02x\n", wd->sta.capability[1]) ;
+        for(k=0; k<wd->sta.beaconFrameBodySize; k++)
+        {
+	     printk("%02x ", wd->sta.beaconFrameBody[k]) ;
+        }
+        #if 0
+        zmw_enter_critical_section(dev);
+        zfMemoryCopy(event.bssid, (u8_t *)bssid, 6);
+        zfMemoryCopy(event.peerMacAddr, (u8_t *)wd->macAddr, 6);
+        zmw_leave_critical_section(dev);
+        #endif
+#endif
+
+        //zmw_enter_critical_section(dev);
+        //wd->sta.ibssPartnerStatus = ZM_IBSS_PARTNER_LOST;
+        //zmw_leave_critical_section(dev);
+    }
+    else
+    {
+        wd->sta.ibssBssIsCreator = 0;
+    }
+
+connect_done:
+    zfHpEnableBeacon(dev, ZM_MODE_IBSS, wd->beaconInterval, wd->dtim, (u8_t)wd->sta.atimWindow);
+    zfStaSendBeacon(dev); // Refresh Beacon content for ZD1211B HalPlus
+    zfHpSetAtimWindow(dev, wd->sta.atimWindow);
+
+    // Start the IBSS timer to monitor for new stations
+    zmw_enter_critical_section(dev);
+    zfTimerSchedule(dev, ZM_EVENT_IBSS_MONITOR, ZM_TICK_IBSS_MONITOR);
+    zmw_leave_critical_section(dev);
+
+
+    if (wd->zfcbConnectNotify != NULL)
+    {
+        wd->zfcbConnectNotify(dev, ZM_STATUS_MEDIA_CONNECT, wd->sta.bssid);
+    }
+    zfChangeAdapterState(dev, ZM_STA_STATE_CONNECTED);
+    wd->sta.connPowerInHalfDbm = zfHpGetTransmitPower(dev);
+
+#ifdef ZM_ENABLE_IBSS_DELAYED_JOIN_INDICATION
+    if ( !bssNotFound )
+    {
+        wd->sta.ibssDelayedInd = 1;
+        zfMemoryCopy((u8_t *)&wd->sta.ibssDelayedIndEvent, (u8_t *)&event, sizeof(struct zsPartnerNotifyEvent));
+    }
+#else
+    if ( !bssNotFound )
+    {
+        if (wd->zfcbIbssPartnerNotify != NULL)
+        {
+            wd->zfcbIbssPartnerNotify(dev, 1, &event);
+        }
+    }
+#endif
+
+    return;
+
+retry_ibss:
+    zfChangeAdapterState(dev, ZM_STA_STATE_CONNECTING);
+    zfStaConnectFail(dev, ZM_STATUS_MEDIA_DISCONNECT_NOT_FOUND, wd->sta.bssid, 0);
+    return;
+}
+
+void zfStaProcessAtim(zdev_t* dev, zbuf_t* buf)
+{
+    zmw_get_wlan_dev(dev);
+
+    zm_debug_msg0("Receiving Atim window notification");
+
+    wd->sta.recvAtim = 1;
+}
+
+static struct zsBssInfo* zfInfraFindAPToConnect(zdev_t* dev,
+        struct zsBssInfo* candidateBss)
+{
+    struct zsBssInfo* pBssInfo;
+    struct zsBssInfo* pNowBssInfo=NULL;
+    u16_t i;
+    u16_t ret, apWepStatus;
+    u32_t k;
+    u32_t channelFlags;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    pBssInfo = wd->sta.bssList.head;
+
+    for(i=0; i<wd->sta.bssList.bssCount; i++)
+    {
+        if ( pBssInfo->capability[0] & ZM_BIT_4 )
+        {
+            apWepStatus = ZM_ENCRYPTION_WEP_ENABLED;
+        }
+        else
+        {
+            apWepStatus = ZM_ENCRYPTION_WEP_DISABLED;
+        }
+
+        if ( ((zfMemoryIsEqual(&(pBssInfo->ssid[2]), wd->sta.ssid,
+                               wd->sta.ssidLen))&&
+              (wd->sta.ssidLen == pBssInfo->ssid[1]))||
+             ((wd->sta.ssidLen == 0)&&
+               /* connect to any BSS: AP's ans STA's WEP status must match */
+              (wd->sta.wepStatus == apWepStatus )&&
+              (pBssInfo->securityType != ZM_SECURITY_TYPE_WPA) ))
+        {
+            if ( wd->sta.ssidLen == 0 )
+            {
+                zm_debug_msg0("ANY BSS found");
+            }
+
+            if ( ((wd->sta.wepStatus == ZM_ENCRYPTION_WEP_DISABLED && apWepStatus == ZM_ENCRYPTION_WEP_ENABLED) ||
+                 (wd->sta.wepStatus == ZM_ENCRYPTION_WEP_ENABLED &&
+                 (apWepStatus == ZM_ENCRYPTION_WEP_DISABLED && wd->sta.dropUnencryptedPkts == 1))) &&
+                 (wd->sta.authMode >= ZM_AUTH_MODE_OPEN && wd->sta.authMode <= ZM_AUTH_MODE_AUTO) )
+            {
+                zm_debug_msg0("Privacy policy is inconsistent");
+                pBssInfo = pBssInfo->next;
+                continue;
+            }
+
+            /* for WPA negative test */
+            if ( !zfCheckAuthentication(dev, pBssInfo) )
+            {
+                pBssInfo = pBssInfo->next;
+                continue;
+            }
+
+            /* Check bssid */
+            if (wd->sta.bDesiredBssid == TRUE)
+            {
+                for (k=0; k<6; k++)
+                {
+                    if (wd->sta.desiredBssid[k] != pBssInfo->bssid[k])
+                    {
+                        zm_msg0_mm(ZM_LV_1, "desired bssid not matched 1");
+                        break;
+                    }
+                }
+
+                if (k != 6)
+                {
+                    zm_msg0_mm(ZM_LV_1, "desired bssid not matched 2");
+                    pBssInfo = pBssInfo->next;
+                    continue;
+                }
+            }
+
+            /* Check support mode */
+            if (pBssInfo->frequency > 3000) {
+                if (   (pBssInfo->EnableHT == 1)
+                    || (pBssInfo->apCap & ZM_All11N_AP) ) //11n AP
+                {
+                    channelFlags = CHANNEL_A_HT;
+                    if (pBssInfo->enableHT40 == 1) {
+                        channelFlags |= CHANNEL_HT40;
+                    }
+                } else {
+                    channelFlags = CHANNEL_A;
+                }
+            } else {
+                if (   (pBssInfo->EnableHT == 1)
+                    || (pBssInfo->apCap & ZM_All11N_AP) ) //11n AP
+                {
+                    channelFlags = CHANNEL_G_HT;
+                    if(pBssInfo->enableHT40 == 1) {
+                        channelFlags |= CHANNEL_HT40;
+                    }
+                } else {
+                    if (pBssInfo->extSupportedRates[1] == 0) {
+                        channelFlags = CHANNEL_B;
+                    } else {
+                        channelFlags = CHANNEL_G;
+                    }
+                }
+            }
+
+            if (   ((channelFlags == CHANNEL_B) && (wd->connectMode & ZM_BIT_0))
+                || ((channelFlags == CHANNEL_G) && (wd->connectMode & ZM_BIT_1))
+                || ((channelFlags == CHANNEL_A) && (wd->connectMode & ZM_BIT_2))
+                || ((channelFlags & CHANNEL_HT20) && (wd->connectMode & ZM_BIT_3)) )
+            {
+                pBssInfo = pBssInfo->next;
+                continue;
+            }
+
+            /* Skip if AP in blocking list */
+            if ((ret = zfStaIsApInBlockingList(dev, pBssInfo->bssid)) == TRUE)
+            {
+                zm_msg0_mm(ZM_LV_0, "Candidate AP in blocking List, skip if there's stilla choice!");
+                pNowBssInfo = pBssInfo;
+                pBssInfo = pBssInfo->next;
+                continue;
+            }
+
+            if ( pBssInfo->capability[0] & ZM_BIT_0 )  // check if infra-BSS
+            {
+                    pNowBssInfo = pBssInfo;
+                    wd->sta.apWmeCapability = pBssInfo->wmeSupport;
+
+
+                    goto done;
+            }
+        }
+
+        pBssInfo = pBssInfo->next;
+    }
+
+done:
+    if (pNowBssInfo != NULL)
+    {
+        zfwMemoryCopy((void*)candidateBss, (void*)pNowBssInfo, sizeof(struct zsBssInfo));
+        pNowBssInfo = candidateBss;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return pNowBssInfo;
+}
+
+
+void zfInfraConnectNetwork(zdev_t* dev)
+{
+    struct zsBssInfo* pBssInfo;
+    struct zsBssInfo* pNowBssInfo=NULL;
+    struct zsBssInfo candidateBss;
+    //u16_t i, j=100, quality=10000;
+    //u8_t ret=FALSE, apWepStatus;
+    u8_t ret=FALSE;
+    u16_t k;
+    u8_t density = ZM_MPDU_DENSITY_NONE;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    /* Reset bssNotFoundCount for Ad-Hoc:IBSS */
+    /* Need review : IbssConn -> InfraConn -> IbssConn etc, flag/counter reset? */
+    zmw_enter_critical_section(dev);
+    wd->sta.bssNotFoundCount = 0;
+    zmw_leave_critical_section(dev);
+
+    /* Set TxQs CWMIN, CWMAX, AIFS and TXO to WME STA default. */
+    zfUpdateDefaultQosParameter(dev, 0);
+
+    zfStaRefreshBlockList(dev, 0);
+
+    /* change state to CONNECTING and stop the channel scanning */
+    zfChangeAdapterState(dev, ZM_STA_STATE_CONNECTING);
+    zfPowerSavingMgrWakeup(dev);
+
+    wd->sta.wmeConnected = 0;
+    wd->sta.psMgr.tempWakeUp = 0;
+    wd->sta.qosInfo = 0;
+    zfQueueFlush(dev, wd->sta.uapsdQ);
+
+    wd->sta.connectState = ZM_STA_CONN_STATE_NONE;
+
+    //Reorder BssList by RSSI--CWYang(+)
+    zfBssInfoReorderList(dev);
+
+    pNowBssInfo = zfInfraFindAPToConnect(dev, &candidateBss);
+
+	if (wd->sta.SWEncryptEnable != 0)
+	{
+	    if (wd->sta.bSafeMode == 0)
+	    {
+		    zfStaDisableSWEncryption(dev);//Quickly reboot
+	    }
+	}
+    if ( pNowBssInfo != NULL )
+    {
+        //zm_assert(pNowBssInfo != NULL);
+
+        pBssInfo = pNowBssInfo;
+        wd->sta.ssidLen = pBssInfo->ssid[1];
+        zfMemoryCopy(wd->sta.ssid, &(pBssInfo->ssid[2]), pBssInfo->ssid[1]);
+        wd->frequency = pBssInfo->frequency;
+        //wd->sta.flagFreqChanging = 1;
+
+        //zfCoreSetFrequency(dev, wd->frequency);
+        zfUpdateBssid(dev, pBssInfo->bssid);
+        zfResetSupportRate(dev, ZM_DEFAULT_SUPPORT_RATE_ZERO);
+        zfUpdateSupportRate(dev, pBssInfo->supportedRates);
+        zfUpdateSupportRate(dev, pBssInfo->extSupportedRates);
+
+        wd->beaconInterval = pBssInfo->beaconInterval[0] +
+                             (((u16_t) pBssInfo->beaconInterval[1]) << 8);
+        if (wd->beaconInterval == 0)
+        {
+            wd->beaconInterval = 100;
+        }
+
+        /* ESS bit on */
+        wd->sta.capability[0] |= ZM_BIT_0;
+        /* IBSS bit off */
+        wd->sta.capability[0] &= ~ZM_BIT_1;
+
+        /* 11n AP flag */
+        wd->sta.EnableHT = pBssInfo->EnableHT;
+        wd->sta.SG40 = pBssInfo->SG40;
+#ifdef ZM_ENABLE_CENC
+        if ( pBssInfo->securityType == ZM_SECURITY_TYPE_CENC )
+        {
+            wd->sta.wmeEnabled = 0; //Disable WMM in CENC
+            cencInit(dev);
+            cencSetCENCMode(dev, NdisCENC_PSK);
+            wd->sta.wpaState = ZM_STA_WPA_STATE_INIT;
+            /* CENC */
+            if ( pBssInfo->cencIe[1] != 0 )
+            {
+                //wd->sta.wepStatus = ZM_ENCRYPTION_CENC;
+                //wd->sta.encryMode = ZM_CENC;
+                zfwCencHandleBeaconProbrespon(dev, (u8_t *)&pBssInfo->cencIe,
+                        (u8_t *)&pBssInfo->ssid, (u8_t *)&pBssInfo->macaddr);
+                zfMemoryCopy(wd->sta.cencIe, pBssInfo->cencIe,
+                        pBssInfo->cencIe[1]+2);
+            }
+            else
+            {
+                wd->sta.cencIe[1] = 0;
+            }
+        }
+#endif //ZM_ENABLE_CENC
+        if ( pBssInfo->securityType == ZM_SECURITY_TYPE_WPA )
+        {
+            wd->sta.wpaState = ZM_STA_WPA_STATE_INIT;
+
+            if ( wd->sta.wepStatus == ZM_ENCRYPTION_TKIP )
+            {
+                wd->sta.encryMode = ZM_TKIP;
+
+                /* Turn on software encryption/decryption for TKIP */
+                if (wd->sta.EnableHT == 1)
+                {
+                    zfStaEnableSWEncryption(dev, (ZM_SW_TKIP_ENCRY_EN|ZM_SW_TKIP_DECRY_EN));
+                }
+
+                /* Do not support TKIP in 11n mode */
+                //wd->sta.EnableHT = 0;
+                //pBssInfo->enableHT40 = 0;
+            }
+            else if ( wd->sta.wepStatus == ZM_ENCRYPTION_AES )
+            {
+                wd->sta.encryMode = ZM_AES;
+
+                /* If AP supports HT mode */
+                if (wd->sta.EnableHT)
+                {
+                    /* Set MPDU density to 8 us*/
+                    density = ZM_MPDU_DENSITY_8US;
+                }
+            }
+
+            if ( pBssInfo->wpaIe[1] != 0 )
+            {
+                zfMemoryCopy(wd->sta.wpaIe, pBssInfo->wpaIe,
+                             pBssInfo->wpaIe[1]+2);
+            }
+            else
+            {
+                wd->sta.wpaIe[1] = 0;
+            }
+
+            if ( pBssInfo->rsnIe[1] != 0 )
+            {
+                zfMemoryCopy(wd->sta.rsnIe, pBssInfo->rsnIe,
+                             pBssInfo->rsnIe[1]+2);
+            }
+            else
+            {
+                wd->sta.rsnIe[1] = 0;
+            }
+        }
+
+
+
+        /* check preamble bit */
+        wd->preambleTypeInUsed = wd->preambleType;
+        if ( wd->preambleTypeInUsed == ZM_PREAMBLE_TYPE_AUTO )
+        {
+            if (pBssInfo->capability[0] & ZM_BIT_5)
+            {
+                wd->preambleTypeInUsed = ZM_PREAMBLE_TYPE_SHORT;
+            }
+            else
+            {
+                wd->preambleTypeInUsed = ZM_PREAMBLE_TYPE_LONG;
+            }
+        }
+
+        if (wd->preambleTypeInUsed == ZM_PREAMBLE_TYPE_LONG)
+        {
+            wd->sta.capability[0] &= ~ZM_BIT_5;
+        }
+        else
+        {
+            wd->sta.capability[0] |= ZM_BIT_5;
+        }
+
+        /* check 802.11n 40MHz Setting */
+        if ((pBssInfo->enableHT40 == 1) &&
+            ((pBssInfo->extChOffset == 1) || (pBssInfo->extChOffset == 3)))
+        {
+            wd->BandWidth40 = pBssInfo->enableHT40;
+            wd->ExtOffset = pBssInfo->extChOffset;
+        }
+        else
+        {
+            wd->BandWidth40 = 0;
+            wd->ExtOffset = 0;
+        }
+
+        /* check 802.11H support bit */
+
+        /* check Owl Ap */
+        if ( pBssInfo->athOwlAp & ZM_BIT_0 )
+        {
+            /* In this function, FW retry will be enable, ZM_MAC_REG_RETRY_MAX
+               will be set to 0.
+             */
+            zfHpDisableHwRetry(dev);
+            wd->sta.athOwlAp = 1;
+            /* Set MPDU density to 8 us*/
+            density = ZM_MPDU_DENSITY_8US;
+        }
+        else
+        {
+            /* In this function, FW retry will be disable, ZM_MAC_REG_RETRY_MAX
+               will be set to 3.
+             */
+            zfHpEnableHwRetry(dev);
+            wd->sta.athOwlAp = 0;
+        }
+        wd->reorder = 1;
+
+        /* Set MPDU density */
+        zfHpSetMPDUDensity(dev, density);
+
+        /* check short slot time bit */
+        if ( pBssInfo->capability[1] & ZM_BIT_2 )
+        {
+            wd->sta.capability[1] |= ZM_BIT_2;
+        }
+
+        if ( pBssInfo->erp & ZM_BIT_1 )
+        {
+            //zm_debug_msg0("protection mode on");
+            wd->sta.bProtectionMode = TRUE;
+            zfHpSetSlotTime(dev, 0);
+        }
+        else
+        {
+            //zm_debug_msg0("protection mode off");
+            wd->sta.bProtectionMode = FALSE;
+            zfHpSetSlotTime(dev, 1);
+        }
+
+        if (pBssInfo->marvelAp == 1)
+        {
+            wd->sta.enableDrvBA = 0;
+            /*
+             * 8701 : NetGear 3500 (MARVELL)
+             * Downlink issue : set slottime to 20.
+             */
+            zfHpSetSlotTimeRegister(dev, 0);
+        }
+        else
+        {
+            wd->sta.enableDrvBA = 1;
+
+            /*
+             * This is not good for here do reset slot time.
+             * I think it should reset when leave MARVELL ap
+             * or enter disconnect state etc.
+             */
+            zfHpSetSlotTimeRegister(dev, 1);
+        }
+
+        //Store probe response frame body, for VISTA only
+        wd->sta.beaconFrameBodySize = pBssInfo->frameBodysize + 12;
+        if (wd->sta.beaconFrameBodySize > ZM_CACHED_FRAMEBODY_SIZE)
+        {
+            wd->sta.beaconFrameBodySize = ZM_CACHED_FRAMEBODY_SIZE;
+        }
+        for (k=0; k<8; k++)
+        {
+            wd->sta.beaconFrameBody[k] = pBssInfo->timeStamp[k];
+        }
+        wd->sta.beaconFrameBody[8] = pBssInfo->beaconInterval[0];
+        wd->sta.beaconFrameBody[9] = pBssInfo->beaconInterval[1];
+        wd->sta.beaconFrameBody[10] = pBssInfo->capability[0];
+        wd->sta.beaconFrameBody[11] = pBssInfo->capability[1];
+        for (k=0; k<(wd->sta.beaconFrameBodySize - 12); k++)
+        {
+            wd->sta.beaconFrameBody[k+12] = pBssInfo->frameBody[k];
+        }
+
+        if ( ( pBssInfo->capability[0] & ZM_BIT_4 )&&
+             (( wd->sta.authMode == ZM_AUTH_MODE_OPEN )||
+              ( wd->sta.authMode == ZM_AUTH_MODE_SHARED_KEY)||
+              (wd->sta.authMode == ZM_AUTH_MODE_AUTO)) )
+        {   /* privacy enabled */
+
+            if ( wd->sta.wepStatus == ZM_ENCRYPTION_WEP_DISABLED )
+            {
+                zm_debug_msg0("Adapter is no WEP, try to connect to WEP AP");
+                ret = FALSE;
+            }
+
+            /* Do not support WEP in 11n mode */
+            if ( wd->sta.wepStatus == ZM_ENCRYPTION_WEP_ENABLED )
+            {
+                /* Turn on software encryption/decryption for WEP */
+                if (wd->sta.EnableHT == 1)
+                {
+                    zfStaEnableSWEncryption(dev, (ZM_SW_WEP_ENCRY_EN|ZM_SW_WEP_DECRY_EN));
+                }
+
+                //wd->sta.EnableHT = 0;
+                //wd->BandWidth40 = 0;
+                //wd->ExtOffset = 0;
+            }
+
+            wd->sta.capability[0] |= ZM_BIT_4;
+
+            if ( wd->sta.authMode == ZM_AUTH_MODE_AUTO )
+            { /* Try to use open and shared-key authehtication alternatively */
+                if ( (wd->sta.connectTimeoutCount % 2) == 0 )
+                    wd->sta.bIsSharedKey = 0;
+                else
+                    wd->sta.bIsSharedKey = 1;
+            }
+            else if ( wd->sta.authMode != ZM_AUTH_MODE_SHARED_KEY )
+            {   /* open  or auto */
+                //zfStaStartConnect(dev, 0);
+                wd->sta.bIsSharedKey = 0;
+            }
+            else if ( wd->sta.authMode != ZM_AUTH_MODE_OPEN )
+            {   /* shared key */
+                //zfStaStartConnect(dev, 1) ;
+                wd->sta.bIsSharedKey = 1;
+            }
+        }
+        else
+        {
+            if ( (pBssInfo->securityType == ZM_SECURITY_TYPE_WPA)||
+                 (pBssInfo->capability[0] & ZM_BIT_4) )
+            {
+                wd->sta.capability[0] |= ZM_BIT_4;
+                /* initialize WPA related parameters */
+            }
+            else
+            {
+                wd->sta.capability[0] &= (~ZM_BIT_4);
+            }
+
+            /* authentication with open system */
+            //zfStaStartConnect(dev, 0);
+            wd->sta.bIsSharedKey = 0;
+        }
+
+        /* Improve WEP/TKIP performace with HT AP, detail information please look bug#32495 */
+        /*
+        if ( (pBssInfo->broadcomHTAp == 1)
+             && (wd->sta.SWEncryptEnable != 0) )
+        {
+            zfHpSetTTSIFSTime(dev, 0xa);
+        }
+        else
+        {
+            zfHpSetTTSIFSTime(dev, 0x8);
+        }
+        */
+    }
+    else
+    {
+        zm_debug_msg0("Desired SSID not found");
+        goto zlConnectFailed;
+    }
+
+
+    zfCoreSetFrequencyV2(dev, wd->frequency, zfStaStartConnectCb);
+    return;
+
+zlConnectFailed:
+    zfStaConnectFail(dev, ZM_STATUS_MEDIA_DISCONNECT_NOT_FOUND, wd->sta.bssid, 0);
+    return;
+}
+
+u8_t zfCheckWPAAuth(zdev_t* dev, struct zsBssInfo* pBssInfo)
+{
+    u8_t   ret=TRUE;
+    u8_t   pmkCount;
+    u8_t   i;
+    u16_t   encAlgoType = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->sta.wepStatus == ZM_ENCRYPTION_TKIP )
+    {
+        encAlgoType = ZM_TKIP;
+    }
+    else if ( wd->sta.wepStatus == ZM_ENCRYPTION_AES )
+    {
+        encAlgoType = ZM_AES;
+    }
+
+    switch(wd->sta.authMode)
+    {
+        case ZM_AUTH_MODE_WPA:
+        case ZM_AUTH_MODE_WPAPSK:
+            if ( pBssInfo->wpaIe[1] == 0 )
+            {
+                ret = FALSE;
+                break;
+            }
+
+            pmkCount = pBssInfo->wpaIe[12];
+            for(i=0; i < pmkCount; i++)
+            {
+                if ( pBssInfo->wpaIe[17 + 4*i] == encAlgoType )
+                {
+                    ret = TRUE;
+                    goto done;
+                }
+            }
+
+            ret = FALSE;
+            break;
+
+        case ZM_AUTH_MODE_WPA2:
+        case ZM_AUTH_MODE_WPA2PSK:
+            if ( pBssInfo->rsnIe[1] == 0 )
+            {
+                ret = FALSE;
+                break;
+            }
+
+            pmkCount = pBssInfo->rsnIe[8];
+            for(i=0; i < pmkCount; i++)
+            {
+                if ( pBssInfo->rsnIe[13 + 4*i] == encAlgoType )
+                {
+                    ret = TRUE;
+                    goto done;
+                }
+            }
+
+            ret = FALSE;
+            break;
+    }
+
+done:
+    return ret;
+}
+
+u8_t zfCheckAuthentication(zdev_t* dev, struct zsBssInfo* pBssInfo)
+{
+    u8_t   ret=TRUE;
+    u16_t  encAlgoType;
+    u16_t UnicastCipherNum;
+
+    zmw_get_wlan_dev(dev);
+
+    /* Connecting to ANY has been checked */
+    if ( wd->sta.ssidLen == 0 )
+    {
+        return ret;
+    }
+
+
+	switch(wd->sta.authMode)
+	//switch(wd->ws.authMode)//Quickly reboot
+    {
+        case ZM_AUTH_MODE_WPA_AUTO:
+        case ZM_AUTH_MODE_WPAPSK_AUTO:
+            encAlgoType = 0;
+            if(pBssInfo->rsnIe[1] != 0)
+            {
+                UnicastCipherNum = (pBssInfo->rsnIe[8]) +
+                                   (pBssInfo->rsnIe[9] << 8);
+
+                /* If there is only one unicast cipher */
+                if (UnicastCipherNum == 1)
+                {
+                    encAlgoType = pBssInfo->rsnIe[13];
+                    //encAlgoType = pBssInfo->rsnIe[7];
+                }
+                else
+                {
+                    u16_t ii;
+                    u16_t desiredCipher = 0;
+                    u16_t IEOffSet = 13;
+
+                    /* Enumerate all the supported unicast cipher */
+                    for (ii = 0; ii < UnicastCipherNum; ii++)
+                    {
+                        if (pBssInfo->rsnIe[IEOffSet+ii*4] > desiredCipher)
+                        {
+                            desiredCipher = pBssInfo->rsnIe[IEOffSet+ii*4];
+                        }
+                    }
+
+                    encAlgoType = desiredCipher;
+                }
+
+                if ( encAlgoType == 0x02 )
+                {
+    			    wd->sta.wepStatus = ZM_ENCRYPTION_TKIP;
+
+    			    if ( wd->sta.authMode == ZM_AUTH_MODE_WPA_AUTO )
+                    {
+                        wd->sta.currentAuthMode = ZM_AUTH_MODE_WPA2;
+                    }
+                    else //ZM_AUTH_MODE_WPAPSK_AUTO
+                    {
+                        wd->sta.currentAuthMode = ZM_AUTH_MODE_WPA2PSK;
+                    }
+                }
+                else if ( encAlgoType == 0x04 )
+                {
+                    wd->sta.wepStatus = ZM_ENCRYPTION_AES;
+
+                    if ( wd->sta.authMode == ZM_AUTH_MODE_WPA_AUTO )
+                    {
+                        wd->sta.currentAuthMode = ZM_AUTH_MODE_WPA2;
+                    }
+                    else //ZM_AUTH_MODE_WPAPSK_AUTO
+                    {
+                        wd->sta.currentAuthMode = ZM_AUTH_MODE_WPA2PSK;
+                    }
+                }
+                else
+                {
+                    ret = FALSE;
+                }
+            }
+            else if(pBssInfo->wpaIe[1] != 0)
+            {
+                UnicastCipherNum = (pBssInfo->wpaIe[12]) +
+                                   (pBssInfo->wpaIe[13] << 8);
+
+                /* If there is only one unicast cipher */
+                if (UnicastCipherNum == 1)
+                {
+                    encAlgoType = pBssInfo->wpaIe[17];
+                    //encAlgoType = pBssInfo->wpaIe[11];
+                }
+                else
+                {
+                    u16_t ii;
+                    u16_t desiredCipher = 0;
+                    u16_t IEOffSet = 17;
+
+                    /* Enumerate all the supported unicast cipher */
+                    for (ii = 0; ii < UnicastCipherNum; ii++)
+                    {
+                        if (pBssInfo->wpaIe[IEOffSet+ii*4] > desiredCipher)
+                        {
+                            desiredCipher = pBssInfo->wpaIe[IEOffSet+ii*4];
+                        }
+                    }
+
+                    encAlgoType = desiredCipher;
+                }
+
+                if ( encAlgoType == 0x02 )
+                {
+    			    wd->sta.wepStatus = ZM_ENCRYPTION_TKIP;
+
+    			    if ( wd->sta.authMode == ZM_AUTH_MODE_WPA_AUTO )
+                    {
+                        wd->sta.currentAuthMode = ZM_AUTH_MODE_WPA;
+                    }
+                    else //ZM_AUTH_MODE_WPAPSK_AUTO
+                    {
+                        wd->sta.currentAuthMode = ZM_AUTH_MODE_WPAPSK;
+                    }
+                }
+                else if ( encAlgoType == 0x04 )
+                {
+                    wd->sta.wepStatus = ZM_ENCRYPTION_AES;
+
+                    if ( wd->sta.authMode == ZM_AUTH_MODE_WPA_AUTO )
+                    {
+                        wd->sta.currentAuthMode = ZM_AUTH_MODE_WPA;
+                    }
+                    else //ZM_AUTH_MODE_WPAPSK_AUTO
+                    {
+                        wd->sta.currentAuthMode = ZM_AUTH_MODE_WPAPSK;
+                    }
+                }
+                else
+                {
+                    ret = FALSE;
+                }
+
+
+            }
+            else
+            {
+                ret = FALSE;
+            }
+
+            break;
+
+        case ZM_AUTH_MODE_WPA:
+        case ZM_AUTH_MODE_WPAPSK:
+        case ZM_AUTH_MODE_WPA_NONE:
+        case ZM_AUTH_MODE_WPA2:
+        case ZM_AUTH_MODE_WPA2PSK:
+            {
+                if ( pBssInfo->securityType != ZM_SECURITY_TYPE_WPA )
+                {
+                    ret = FALSE;
+                }
+
+                ret = zfCheckWPAAuth(dev, pBssInfo);
+            }
+            break;
+
+        case ZM_AUTH_MODE_OPEN:
+        case ZM_AUTH_MODE_SHARED_KEY:
+        case ZM_AUTH_MODE_AUTO:
+            {
+                if ( pBssInfo->wscIe[1] )
+                {
+                    // If the AP is a Jumpstart AP, it's ok!! Ray
+                    break;
+                }
+                else if ( pBssInfo->securityType == ZM_SECURITY_TYPE_WPA )
+                {
+                    ret = FALSE;
+                }
+            }
+            break;
+
+        default:
+            break;
+    }
+
+    return ret;
+}
+
+u8_t zfStaIsConnected(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->sta.adapterState == ZM_STA_STATE_CONNECTED )
+    {
+        return TRUE;
+    }
+
+    return FALSE;
+}
+
+u8_t zfStaIsConnecting(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->sta.adapterState == ZM_STA_STATE_CONNECTING )
+    {
+        return TRUE;
+    }
+
+    return FALSE;
+}
+
+u8_t zfStaIsDisconnect(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->sta.adapterState == ZM_STA_STATE_DISCONNECT )
+    {
+        return TRUE;
+    }
+
+    return FALSE;
+}
+
+u8_t zfChangeAdapterState(zdev_t* dev, u8_t newState)
+{
+    u8_t ret = TRUE;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    //if ( newState == wd->sta.adapterState )
+    //{
+    //    return FALSE;
+    //}
+
+    switch(newState)
+    {
+    case ZM_STA_STATE_DISCONNECT:
+        zfResetSupportRate(dev, ZM_DEFAULT_SUPPORT_RATE_DISCONNECT);
+
+        #if 1
+            zfScanMgrScanStop(dev, ZM_SCAN_MGR_SCAN_INTERNAL);
+        #else
+            if ( wd->sta.bChannelScan )
+            {
+                /* stop the action of channel scanning */
+                wd->sta.bChannelScan = FALSE;
+                ret =  TRUE;
+                break;
+            }
+        #endif
+
+        break;
+    case ZM_STA_STATE_CONNECTING:
+        #if 1
+            zfScanMgrScanStop(dev, ZM_SCAN_MGR_SCAN_INTERNAL);
+        #else
+            if ( wd->sta.bChannelScan )
+            {
+                /* stop the action of channel scanning */
+                wd->sta.bChannelScan = FALSE;
+                ret =  TRUE;
+                break;
+            }
+        #endif
+
+        break;
+    case ZM_STA_STATE_CONNECTED:
+        break;
+    default:
+        break;
+    }
+
+    //if ( ret )
+    //{
+        zmw_enter_critical_section(dev);
+        wd->sta.adapterState = newState;
+        zmw_leave_critical_section(dev);
+
+        zm_debug_msg1("change adapter state = ", newState);
+    //}
+
+    return ret;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfStaMmAddIeSsid            */
+/*      Add information element SSID to buffer.                         */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Ji-Huang Lee        ZyDAS Technology Corporation    2005.11     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfStaAddIeSsid(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    /* Element ID */
+    zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_SSID);
+
+    /* Element Length */
+    zmw_tx_buf_writeb(dev, buf, offset++, wd->sta.ssidLen);
+
+    /* Information : SSID */
+    for (i=0; i<wd->sta.ssidLen; i++)
+    {
+        zmw_tx_buf_writeb(dev, buf, offset++, wd->sta.ssid[i]);
+    }
+
+    return offset;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfStaMmAddIeWpa             */
+/*      Add information element SSID to buffer.                         */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Ji-Huang Lee        ZyDAS Technology Corporation    2006.01     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfStaAddIeWpaRsn(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t frameType)
+{
+    u32_t  i;
+    u8_t    ssn[64]={
+                        /* Element ID */
+                        0xdd,
+                        /* Length */
+                        0x18,
+                        /* OUI type */
+                        0x00, 0x50, 0xf2, 0x01,
+                        /* Version */
+                        0x01, 0x00,
+                        /* Group Cipher Suite, default=TKIP */
+                        0x00, 0x50, 0xf2, 0x02,
+                        /* Pairwise Cipher Suite Count */
+                        0x01, 0x00,
+                        /* Pairwise Cipher Suite, default=TKIP */
+                        0x00, 0x50, 0xf2, 0x02,
+                        /* Authentication and Key Management Suite Count */
+                        0x01, 0x00,
+                        /* Authentication type, default=PSK */
+                        0x00, 0x50, 0xf2, 0x02,
+                        /* WPA capability */
+                        0x00, 0x00
+                    };
+
+    u8_t    rsn[64]={
+                        /* Element ID */
+                        0x30,
+                        /* Length */
+                        0x14,
+                        /* Version */
+                        0x01, 0x00,
+                        /* Group Cipher Suite, default=TKIP */
+                        0x00, 0x0f, 0xac, 0x02,
+                        /* Pairwise Cipher Suite Count */
+                        0x01, 0x00,
+                        /* Pairwise Cipher Suite, default=TKIP */
+                        0x00, 0x0f, 0xac, 0x02,
+                        /* Authentication and Key Management Suite Count */
+                        0x01, 0x00,
+                        /* Authentication type, default=PSK */
+                        0x00, 0x0f, 0xac, 0x02,
+                        /* RSN capability */
+                        0x00, 0x00
+                    };
+
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->sta.currentAuthMode == ZM_AUTH_MODE_WPAPSK )
+    {
+        /* Overwrite Group Cipher Suite by AP's setting */
+        zfMemoryCopy(ssn+8, wd->sta.wpaIe+8, 4);
+
+        if ( wd->sta.wepStatus == ZM_ENCRYPTION_AES )
+        {
+            /* Overwrite Pairwise Cipher Suite by AES */
+            zfMemoryCopy(ssn+14, zgWpaAesOui, 4);
+        }
+
+        zfCopyToIntTxBuffer(dev, buf, ssn, offset, ssn[1]+2);
+        zfMemoryCopy(wd->sta.wpaIe, ssn, ssn[1]+2);
+        offset += (ssn[1]+2);
+    }
+    else if ( wd->sta.currentAuthMode == ZM_AUTH_MODE_WPA )
+    {
+        /* Overwrite Group Cipher Suite by AP's setting */
+        zfMemoryCopy(ssn+8, wd->sta.wpaIe+8, 4);
+        /* Overwrite Key Management Suite by WPA-Radius */
+        zfMemoryCopy(ssn+20, zgWpaRadiusOui, 4);
+
+        if ( wd->sta.wepStatus == ZM_ENCRYPTION_AES )
+        {
+            /* Overwrite Pairwise Cipher Suite by AES */
+            zfMemoryCopy(ssn+14, zgWpaAesOui, 4);
+        }
+
+        zfCopyToIntTxBuffer(dev, buf, ssn, offset, ssn[1]+2);
+        zfMemoryCopy(wd->sta.wpaIe, ssn, ssn[1]+2);
+        offset += (ssn[1]+2);
+    }
+    else if ( wd->sta.currentAuthMode == ZM_AUTH_MODE_WPA2PSK )
+    {
+        /* Overwrite Group Cipher Suite by AP's setting */
+        zfMemoryCopy(rsn+4, wd->sta.rsnIe+4, 4);
+
+        if ( wd->sta.wepStatus == ZM_ENCRYPTION_AES )
+        {
+            /* Overwrite Pairwise Cipher Suite by AES */
+            zfMemoryCopy(rsn+10, zgWpa2AesOui, 4);
+        }
+
+        if ( frameType == ZM_WLAN_FRAME_TYPE_REASOCREQ )
+        {
+            for(i=0; i<wd->sta.pmkidInfo.bssidCount; i++)
+            {
+                if ( zfMemoryIsEqual((u8_t*) wd->sta.pmkidInfo.bssidInfo[i].bssid,
+                                     (u8_t*) wd->sta.bssid, 6) )
+                {
+                    /* matched */
+                    break;
+                }
+
+                if ( i < wd->sta.pmkidInfo.bssidCount )
+                {
+                    // Fill PMKID Count in RSN information element
+                    rsn[22] = 0x01;
+                    rsn[23] = 0x00;
+
+                    // Fill PMKID in RSN information element
+                    zfMemoryCopy(rsn+24,
+                                 wd->sta.pmkidInfo.bssidInfo[i].pmkid, 16);
+			                 rsn[1] += 18;
+                }
+            }
+        }
+
+        zfCopyToIntTxBuffer(dev, buf, rsn, offset, rsn[1]+2);
+        zfMemoryCopy(wd->sta.rsnIe, rsn, rsn[1]+2);
+        offset += (rsn[1]+2);
+    }
+    else if ( wd->sta.currentAuthMode == ZM_AUTH_MODE_WPA2 )
+    {
+        /* Overwrite Group Cipher Suite by AP's setting */
+        zfMemoryCopy(rsn+4, wd->sta.rsnIe+4, 4);
+        /* Overwrite Key Management Suite by WPA2-Radius */
+        zfMemoryCopy(rsn+16, zgWpa2RadiusOui, 4);
+
+        if ( wd->sta.wepStatus == ZM_ENCRYPTION_AES )
+        {
+            /* Overwrite Pairwise Cipher Suite by AES */
+            zfMemoryCopy(rsn+10, zgWpa2AesOui, 4);
+        }
+
+        if (( frameType == ZM_WLAN_FRAME_TYPE_REASOCREQ || ( frameType == ZM_WLAN_FRAME_TYPE_ASOCREQ )))
+        {
+
+            if (wd->sta.pmkidInfo.bssidCount != 0) {
+                // Fill PMKID Count in RSN information element
+                rsn[22] = 1;
+                rsn[23] = 0;
+                /*
+                 *  The caller is respnsible to give us the relevant PMKID.
+                 *  We'll only accept 1 PMKID for now.
+                 */
+                for(i=0; i<wd->sta.pmkidInfo.bssidCount; i++)
+                {
+                    if ( zfMemoryIsEqual((u8_t*) wd->sta.pmkidInfo.bssidInfo[i].bssid, (u8_t*) wd->sta.bssid, 6) )
+                    {
+                        zfMemoryCopy(rsn+24, wd->sta.pmkidInfo.bssidInfo[i].pmkid, 16);
+                        break;
+                    }
+                }
+                rsn[1] += 18;
+            }
+
+        }
+
+        zfCopyToIntTxBuffer(dev, buf, rsn, offset, rsn[1]+2);
+        zfMemoryCopy(wd->sta.rsnIe, rsn, rsn[1]+2);
+        offset += (rsn[1]+2);
+    }
+
+    return offset;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfStaAddIeIbss              */
+/*      Add information element IBSS parameter to buffer.               */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Ji-Huang Lee        ZyDAS Technology Corporation    2005.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfStaAddIeIbss(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* Element ID */
+    zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_IBSS);
+
+    /* Element Length */
+    zmw_tx_buf_writeb(dev, buf, offset++, 2);
+
+    /* ATIM window */
+    zmw_tx_buf_writeh(dev, buf, offset, wd->sta.atimWindow);
+    offset += 2;
+
+    return offset;
+}
+
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfStaAddIeWmeInfo           */
+/*      Add WME Information Element to buffer.                          */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2006.6      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfStaAddIeWmeInfo(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t qosInfo)
+{
+    /* Element ID */
+    zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_WIFI_IE);
+
+    /* Element Length */
+    zmw_tx_buf_writeb(dev, buf, offset++, 7);
+
+    /* OUI */
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x00);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x50);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0xF2);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x02);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x00);
+    zmw_tx_buf_writeb(dev, buf, offset++, 0x01);
+
+    /* QoS Info */
+    zmw_tx_buf_writeb(dev, buf, offset++, qosInfo);
+
+    return offset;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfStaAddIePowerCap          */
+/*      Add information element Power capability to buffer.             */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Sharon                                            2007.12       */
+/*                                                                      */
+/************************************************************************/
+u16_t zfStaAddIePowerCap(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+    u8_t MaxTxPower;
+    u8_t MinTxPower;
+
+    zmw_get_wlan_dev(dev);
+
+    /* Element ID */
+    zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_POWER_CAPABILITY);
+
+    /* Element Length */
+    zmw_tx_buf_writeb(dev, buf, offset++, 2);
+
+    MinTxPower = (u8_t)(zfHpGetMinTxPower(dev)/2);
+    MaxTxPower = (u8_t)(zfHpGetMaxTxPower(dev)/2);
+
+    /* Min Transmit Power Cap */
+    zmw_tx_buf_writeh(dev, buf, offset++, MinTxPower);
+
+    /* Max Transmit Power Cap */
+    zmw_tx_buf_writeh(dev, buf, offset++, MaxTxPower);
+
+    return offset;
+}
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfStaAddIeSupportCh              */
+/*      Add information element supported channels to buffer.               */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer to add information element                         */
+/*      offset : add information element from this offset               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      buffer offset after adding information element                  */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Sharon            2007.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfStaAddIeSupportCh(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+
+    u8_t   i;
+    u16_t  count_24G = 0;
+    u16_t  count_5G = 0;
+    u16_t  channelNum;
+    u8_t   length;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+    zmw_enter_critical_section(dev);
+
+    for (i = 0; i < wd->regulationTable.allowChannelCnt; i++)
+    {
+        if (wd->regulationTable.allowChannel[i].channel < 3000)
+        { // 2.4Hz
+            count_24G++;
+        }
+        else
+        { // 5GHz
+            count_5G++;
+        }
+    }
+
+    length = (u8_t)(count_5G * 2 + 2); //5G fill by pair, 2,4G (continuous channels) fill 2 bytes
+
+    /* Element ID */
+    zmw_tx_buf_writeb(dev, buf, offset++, ZM_WLAN_EID_SUPPORTED_CHANNELS );
+
+    /* Element Length */
+    zmw_tx_buf_writeb(dev, buf, offset++, length);
+
+    // 2.4GHz (continuous channels)
+    /* First channel number */
+    zmw_tx_buf_writeh(dev, buf, offset++, 1); //Start from channle 1
+    /* Number of channels */
+    zmw_tx_buf_writeh(dev, buf, offset++, count_24G);
+
+    for (i = 0; i < wd->regulationTable.allowChannelCnt ; i++)
+    {
+        if (wd->regulationTable.allowChannel[i].channel > 4000 && wd->regulationTable.allowChannel[i].channel < 5000)
+        { // 5GHz 4000 -5000Mhz
+            channelNum = (wd->regulationTable.allowChannel[i].channel-4000)/5;
+            /* First channel number */
+            zmw_tx_buf_writeh(dev, buf, offset++, channelNum);
+            /* Number of channels */
+            zmw_tx_buf_writeh(dev, buf, offset++, 1);
+        }
+        else if (wd->regulationTable.allowChannel[i].channel >= 5000)
+        { // 5GHz >5000Mhz
+            channelNum = (wd->regulationTable.allowChannel[i].channel-5000)/5;
+            /* First channel number */
+            zmw_tx_buf_writeh(dev, buf, offset++, channelNum);
+            /* Number of channels */
+            zmw_tx_buf_writeh(dev, buf, offset++, 1);
+        }
+    }
+   zmw_leave_critical_section(dev);
+
+    return offset;
+}
+
+void zfStaStartConnectCb(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    zfStaStartConnect(dev, wd->sta.bIsSharedKey);
+}
+
+void zfStaStartConnect(zdev_t* dev, u8_t bIsSharedKey)
+{
+    u32_t p1, p2;
+    u8_t newConnState;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    /* p1_low = algorithm number, p1_high = transaction sequence number */
+    if ( bIsSharedKey )
+    {
+        //wd->sta.connectState = ZM_STA_CONN_STATE_AUTH_SHARE_1;
+        newConnState = ZM_STA_CONN_STATE_AUTH_SHARE_1;
+        zm_debug_msg0("ZM_STA_CONN_STATE_AUTH_SHARE_1");
+        p1 = ZM_AUTH_ALGO_SHARED_KEY;
+    }
+    else
+    {
+        //wd->sta.connectState = ZM_STA_CONN_STATE_AUTH_OPEN;
+        newConnState = ZM_STA_CONN_STATE_AUTH_OPEN;
+        zm_debug_msg0("ZM_STA_CONN_STATE_AUTH_OPEN");
+        if( wd->sta.leapEnabled )
+            p1 = ZM_AUTH_ALGO_LEAP;
+        else
+            p1 = ZM_AUTH_ALGO_OPEN_SYSTEM;
+    }
+
+    /* status code */
+    p2 = 0x0;
+
+    zmw_enter_critical_section(dev);
+    wd->sta.connectTimer = wd->tick;
+    wd->sta.connectState = newConnState;
+    zmw_leave_critical_section(dev);
+
+    /* send the 1st authentication frame */
+    zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_AUTH, wd->sta.bssid, p1, p2, 0);
+
+    return;
+}
+
+void zfSendNullData(zdev_t* dev, u8_t type)
+{
+    zbuf_t* buf;
+    //u16_t addrTblSize;
+    //struct zsAddrTbl addrTbl;
+    u16_t err;
+    u16_t hlen;
+    u16_t header[(34+8+1)/2];
+    u16_t bcastAddr[3] = {0xffff,0xffff,0xffff};
+    u16_t *dstAddr;
+
+    zmw_get_wlan_dev(dev);
+
+    if ((buf = zfwBufAllocate(dev, 1024)) == NULL)
+    {
+        zm_msg0_mm(ZM_LV_0, "Alloc mm buf Fail!");
+        return;
+    }
+
+    zfwBufSetSize(dev, buf, 0);
+
+    //zm_msg2_mm(ZM_LV_2, "buf->len=", buf->len);
+
+    if ( wd->wlanMode == ZM_MODE_IBSS)
+    {
+        dstAddr = bcastAddr;
+    }
+    else
+    {
+        dstAddr = wd->sta.bssid;
+    }
+
+    if (wd->sta.wmeConnected != 0)
+    {
+        /* If connect to a WMM AP, Send QoS Null data */
+        hlen = zfTxGenMmHeader(dev, ZM_WLAN_FRAME_TYPE_QOS_NULL, dstAddr, header, 0, buf, 0, 0);
+    }
+    else
+    {
+        hlen = zfTxGenMmHeader(dev, ZM_WLAN_FRAME_TYPE_NULL, dstAddr, header, 0, buf, 0, 0);
+    }
+
+    if (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)
+    {
+        header[4] |= 0x0100; //TODS bit
+    }
+
+    if ( type == 1 )
+    {
+        header[4] |= 0x1000;
+    }
+
+    /* Get buffer DMA address */
+    //if ((addrTblSize = zfwBufMapDma(dev, buf, &addrTbl)) == 0)
+    //if ((addrTblSize = zfwMapTxDma(dev, buf, &addrTbl)) == 0)
+    //{
+    //    goto zlError;
+    //}
+
+    /*increase unicast frame counter*/
+    wd->commTally.txUnicastFrm++;
+
+    if ((err = zfHpSend(dev, header, hlen, NULL, 0, NULL, 0, buf, 0,
+            ZM_INTERNAL_ALLOC_BUF, 0, 0xff)) != ZM_SUCCESS)
+    {
+        goto zlError;
+    }
+
+
+    return;
+
+zlError:
+
+    zfwBufFree(dev, buf, 0);
+    return;
+
+}
+
+void zfSendPSPoll(zdev_t* dev)
+{
+    zbuf_t* buf;
+    //u16_t addrTblSize;
+    //struct zsAddrTbl addrTbl;
+    u16_t err;
+    u16_t hlen;
+    u16_t header[(8+24+1)/2];
+
+    zmw_get_wlan_dev(dev);
+
+    if ((buf = zfwBufAllocate(dev, 1024)) == NULL)
+    {
+        zm_msg0_mm(ZM_LV_0, "Alloc mm buf Fail!");
+        return;
+    }
+
+    zfwBufSetSize(dev, buf, 0);
+
+    //zm_msg2_mm(ZM_LV_2, "buf->len=", buf->len);
+
+    zfTxGenMmHeader(dev, ZM_WLAN_FRAME_TYPE_PSPOLL, wd->sta.bssid, header, 0, buf, 0, 0);
+
+    header[0] = 20;
+    header[4] |= 0x1000;
+    header[5] = wd->sta.aid | 0xc000; //Both bit-14 and bit-15 are 1
+    hlen = 16 + 8;
+
+    /* Get buffer DMA address */
+    //if ((addrTblSize = zfwBufMapDma(dev, buf, &addrTbl)) == 0)
+    //if ((addrTblSize = zfwMapTxDma(dev, buf, &addrTbl)) == 0)
+    //{
+    //    goto zlError;
+    //}
+
+    if ((err = zfHpSend(dev, header, hlen, NULL, 0, NULL, 0, buf, 0,
+            ZM_INTERNAL_ALLOC_BUF, 0, 0xff)) != ZM_SUCCESS)
+    {
+        goto zlError;
+    }
+
+    return;
+
+zlError:
+
+    zfwBufFree(dev, buf, 0);
+    return;
+
+}
+
+void zfSendBA(zdev_t* dev, u16_t start_seq, u8_t *bitmap)
+{
+    zbuf_t* buf;
+    //u16_t addrTblSize;
+    //struct zsAddrTbl addrTbl;
+    u16_t err;
+    u16_t hlen;
+    u16_t header[(8+24+1)/2];
+    u16_t i, offset = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    if ((buf = zfwBufAllocate(dev, 1024)) == NULL)
+    {
+        zm_msg0_mm(ZM_LV_0, "Alloc mm buf Fail!");
+        return;
+    }
+
+    zfwBufSetSize(dev, buf, 12); // 28 = FC 2 + DU 2 + RA 6 + TA 6 + BAC 2 + SEQ 2 + BitMap 8
+                                 // 12 = BAC 2 + SEQ 2 + BitMap 8
+
+    //zm_msg2_mm(ZM_LV_2, "buf->len=", buf->len);
+
+    zfTxGenMmHeader(dev, ZM_WLAN_FRAME_TYPE_BA, wd->sta.bssid, header, 0, buf, 0, 0);
+
+    header[0] = 32; /* MAC header 16 + BA control 2 + BA info 10 + FCS 4*/
+    header[1] = 0x4;  /* No ACK */
+
+    /* send by OFDM 6M */
+    header[2] = (u16_t)(zcRateToPhyCtrl[4] & 0xffff);
+    header[3] = (u16_t)(zcRateToPhyCtrl[4]>>16) & 0xffff;
+
+    hlen = 16 + 8;  /* MAC header 16 + control 8*/
+    offset = 0;
+    zmw_tx_buf_writeh(dev, buf, offset, 0x05); /*compressed bitmap on*/
+    offset+=2;
+    zmw_tx_buf_writeh(dev, buf, offset, start_seq);
+    offset+=2;
+
+    for (i=0; i<8; i++) {
+        zmw_tx_buf_writeb(dev, buf, offset, bitmap[i]);
+        offset++;
+    }
+
+    if ((err = zfHpSend(dev, header, hlen, NULL, 0, NULL, 0, buf, 0,
+            ZM_INTERNAL_ALLOC_BUF, 0, 0xff)) != ZM_SUCCESS)
+    {
+        goto zlError;
+    }
+
+    return;
+
+zlError:
+
+    zfwBufFree(dev, buf, 0);
+    return;
+
+}
+
+void zfStaGetTxRate(zdev_t* dev, u16_t* macAddr, u32_t* phyCtrl,
+        u16_t* rcProbingFlag)
+{
+    u8_t   addr[6], i;
+    u8_t   rate;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    ZM_MAC_WORD_TO_BYTE(macAddr, addr);
+    *phyCtrl = 0;
+
+    if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+    {
+        zmw_enter_critical_section(dev);
+        rate = (u8_t)zfRateCtrlGetTxRate(dev, &wd->sta.oppositeInfo[0].rcCell, rcProbingFlag);
+//#ifdef ZM_FB50
+        //rate = 27;
+//#endif
+        *phyCtrl = zcRateToPhyCtrl[rate];
+        zmw_leave_critical_section(dev);
+    }
+    else
+    {
+        zmw_enter_critical_section(dev);
+        for(i=0; i<wd->sta.oppositeCount; i++)
+        {
+            if ( addr[0] && 0x01 == 1 ) // The default beacon transmitted rate is CCK and 1 Mbps , but the a mode should use
+                                        // OFDM modulation and 6Mbps to transmit beacon.
+            {
+                //rate = (u8_t)zfRateCtrlGetTxRate(dev, &wd->sta.oppositeInfo[i].rcCell, rcProbingFlag);
+                rate = wd->sta.oppositeInfo[i].rcCell.operationRateSet[0];
+                *phyCtrl = zcRateToPhyCtrl[rate];
+                break;
+            }
+            else if ( zfMemoryIsEqual(addr, wd->sta.oppositeInfo[i].macAddr, 6) )
+            {
+                rate = (u8_t)zfRateCtrlGetTxRate(dev, &wd->sta.oppositeInfo[i].rcCell, rcProbingFlag);
+                *phyCtrl = zcRateToPhyCtrl[rate];
+                break;
+            }
+        }
+        zmw_leave_critical_section(dev);
+    }
+
+    return;
+}
+
+struct zsMicVar* zfStaGetRxMicKey(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t keyIndex;
+    u8_t da0;
+
+    zmw_get_wlan_dev(dev);
+
+    /* if need not check MIC, return NULL */
+    if ( ((wd->sta.encryMode != ZM_TKIP)&&(wd->sta.encryMode != ZM_AES))||
+         (wd->sta.wpaState < ZM_STA_WPA_STATE_PK_OK) )
+    {
+        return NULL;
+    }
+
+    da0 = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_A1_OFFSET);
+
+    if ((zmw_rx_buf_readb(dev, buf, 0) & 0x80) == 0x80)
+        keyIndex = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_IV_OFFSET+5); /* Qos Packet*/
+    else
+        keyIndex = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_IV_OFFSET+3); /* normal Packet*/
+    keyIndex = (keyIndex & 0xc0) >> 6;
+
+    return (&wd->sta.rxMicKey[keyIndex]);
+}
+
+struct zsMicVar* zfStaGetTxMicKey(zdev_t* dev, zbuf_t* buf)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* if need not check MIC, return NULL */
+    //if ( ((wd->sta.encryMode != ZM_TKIP)&&(wd->sta.encryMode != ZM_AES))||
+    //     (wd->sta.wpaState < ZM_STA_WPA_STATE_PK_OK) )
+    if ( (wd->sta.encryMode != ZM_TKIP) || (wd->sta.wpaState < ZM_STA_WPA_STATE_PK_OK) )
+    {
+        return NULL;
+    }
+
+    return (&wd->sta.txMicKey);
+}
+
+u16_t zfStaRxValidateFrame(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t   frameType, frameCtrl;
+    u8_t   da0;
+    //u16_t  sa[3];
+    u16_t  ret;
+    u16_t  i;
+    //u8_t    sa0;
+
+    zmw_get_wlan_dev(dev);
+
+    frameType = zmw_rx_buf_readb(dev, buf, 0);
+    da0 = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_A1_OFFSET);
+    //sa0 = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_A2_OFFSET);
+
+    if ( (!zfStaIsConnected(dev))&&((frameType & 0xf) == ZM_WLAN_DATA_FRAME) )
+    {
+        return ZM_ERR_DATA_BEFORE_CONNECTED;
+    }
+
+
+    if ( (zfStaIsConnected(dev))&&((frameType & 0xf) == ZM_WLAN_DATA_FRAME) )
+    {
+        /* check BSSID */
+        if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+        {
+            /* Big Endian and Little Endian Compatibility */
+            u16_t mac[3];
+            mac[0] = zmw_cpu_to_le16(wd->sta.bssid[0]);
+            mac[1] = zmw_cpu_to_le16(wd->sta.bssid[1]);
+            mac[2] = zmw_cpu_to_le16(wd->sta.bssid[2]);
+            if ( !zfRxBufferEqualToStr(dev, buf, (u8_t *)mac,
+                                       ZM_WLAN_HEADER_A2_OFFSET, 6) )
+            {
+/*We will get lots of garbage data, especially in AES mode.*/
+/*To avoid sending too many deauthentication frames in STA mode, mark it.*/
+#if 0
+                /* If unicast frame, send deauth to the transmitter */
+                if (( da0 & 0x01 ) == 0)
+                {
+                    for (i=0; i<3; i++)
+                    {
+                        sa[i] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+(i*2));
+                    }
+					/* If mutilcast address, don't send deauthentication*/
+					if (( sa0 & 0x01 ) == 0)
+	                  	zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_DEAUTH, sa, 7, 0, 0);
+                }
+#endif
+                return ZM_ERR_DATA_BSSID_NOT_MATCHED;
+            }
+        }
+        else if ( wd->wlanMode == ZM_MODE_IBSS )
+        {
+            /* Big Endian and Little Endian Compatibility */
+            u16_t mac[3];
+            mac[0] = zmw_cpu_to_le16(wd->sta.bssid[0]);
+            mac[1] = zmw_cpu_to_le16(wd->sta.bssid[1]);
+            mac[2] = zmw_cpu_to_le16(wd->sta.bssid[2]);
+            if ( !zfRxBufferEqualToStr(dev, buf, (u8_t *)mac,
+                                       ZM_WLAN_HEADER_A3_OFFSET, 6) )
+            {
+                return ZM_ERR_DATA_BSSID_NOT_MATCHED;
+            }
+        }
+
+        frameCtrl = zmw_rx_buf_readb(dev, buf, 1);
+
+        /* check security bit */
+        if ( wd->sta.dropUnencryptedPkts &&
+             (wd->sta.wepStatus != ZM_ENCRYPTION_WEP_DISABLED )&&
+             ( !(frameCtrl & ZM_BIT_6) ) )
+        {   /* security on, but got data without encryption */
+
+            #if 1
+            ret = ZM_ERR_DATA_NOT_ENCRYPTED;
+            if ( wd->sta.pStaRxSecurityCheckCb != NULL )
+            {
+                ret = wd->sta.pStaRxSecurityCheckCb(dev, buf);
+            }
+            else
+            {
+                ret = ZM_ERR_DATA_NOT_ENCRYPTED;
+            }
+            if (ret == ZM_ERR_DATA_NOT_ENCRYPTED)
+            {
+                wd->commTally.swRxDropUnencryptedCount++;
+            }
+            return ret;
+            #else
+            if ( (wd->sta.wepStatus != ZM_ENCRYPTION_TKIP)&&
+                 (wd->sta.wepStatus != ZM_ENCRYPTION_AES) )
+            {
+                return ZM_ERR_DATA_NOT_ENCRYPTED;
+            }
+            #endif
+        }
+    }
+
+    return ZM_SUCCESS;
+}
+
+void zfStaMicFailureHandling(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t   da0;
+    u8_t   micNotify = 1;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if ( wd->sta.wpaState <  ZM_STA_WPA_STATE_PK_OK )
+    {
+        return;
+    }
+
+    zmw_enter_critical_section(dev);
+
+    wd->sta.cmMicFailureCount++;
+
+    if ( wd->sta.cmMicFailureCount == 1 )
+    {
+        zm_debug_msg0("get the first MIC failure");
+        //zfTimerSchedule(dev, ZM_EVENT_CM_TIMER, ZM_TICK_CM_TIMEOUT);
+
+        /* Timer Resolution on WinXP is 15/16 ms  */
+        /* Decrease Time offset for <XP> Counter Measure */
+        zfTimerSchedule(dev, ZM_EVENT_CM_TIMER, ZM_TICK_CM_TIMEOUT - ZM_TICK_CM_TIMEOUT_OFFSET);
+    }
+    else if ( wd->sta.cmMicFailureCount == 2 )
+    {
+        zm_debug_msg0("get the second MIC failure");
+        /* reserve 2 second for OS to send MIC failure report to AP */
+        wd->sta.cmDisallowSsidLength = wd->sta.ssidLen;
+        zfMemoryCopy(wd->sta.cmDisallowSsid, wd->sta.ssid, wd->sta.ssidLen);
+        //wd->sta.cmMicFailureCount = 0;
+        zfTimerCancel(dev, ZM_EVENT_CM_TIMER);
+        //zfTimerSchedule(dev, ZM_EVENT_CM_DISCONNECT, ZM_TICK_CM_DISCONNECT);
+
+        /* Timer Resolution on WinXP is 15/16 ms  */
+        /* Decrease Time offset for <XP> Counter Measure */
+        zfTimerSchedule(dev, ZM_EVENT_CM_DISCONNECT, ZM_TICK_CM_DISCONNECT - ZM_TICK_CM_DISCONNECT_OFFSET);
+    }
+    else
+    {
+        micNotify = 0;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    if (micNotify == 1)
+    {
+        da0 = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_A1_OFFSET);
+        if ( da0 & 0x01 )
+        {
+            if (wd->zfcbMicFailureNotify != NULL)
+            {
+                wd->zfcbMicFailureNotify(dev, wd->sta.bssid, ZM_MIC_GROUP_ERROR);
+            }
+        }
+        else
+        {
+            if (wd->zfcbMicFailureNotify != NULL)
+            {
+                wd->zfcbMicFailureNotify(dev, wd->sta.bssid, ZM_MIC_PAIRWISE_ERROR);
+            }
+        }
+    }
+}
+
+
+u8_t zfStaBlockWlanScan(zdev_t* dev)
+{
+    u8_t   ret=FALSE;
+
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->sta.bChannelScan )
+    {
+        return TRUE;
+    }
+
+    return ret;
+}
+
+void zfStaResetStatus(zdev_t* dev, u8_t bInit)
+{
+    u8_t   i;
+
+    zmw_get_wlan_dev(dev);
+
+    zfHpDisableBeacon(dev);
+
+    wd->dtim = 1;
+    wd->sta.capability[0] = 0x01;
+    wd->sta.capability[1] = 0x00;
+    /* 802.11h */
+    if (wd->sta.DFSEnable || wd->sta.TPCEnable)
+        wd->sta.capability[1] |= ZM_BIT_0;
+
+    /* release queued packets */
+    for(i=0; i<wd->sta.ibssPSDataCount; i++)
+    {
+        zfwBufFree(dev, wd->sta.ibssPSDataQueue[i], 0);
+    }
+
+    for(i=0; i<wd->sta.staPSDataCount; i++)
+    {
+        zfwBufFree(dev, wd->sta.staPSDataQueue[i], 0);
+    }
+
+    wd->sta.ibssPSDataCount = 0;
+    wd->sta.staPSDataCount = 0;
+    zfZeroMemory((u8_t*) &wd->sta.staPSList, sizeof(struct zsStaPSList));
+
+    wd->sta.wmeConnected = 0;
+    wd->sta.psMgr.tempWakeUp = 0;
+    wd->sta.qosInfo = 0;
+    zfQueueFlush(dev, wd->sta.uapsdQ);
+
+    return;
+
+}
+
+void zfStaIbssMonitoring(zdev_t* dev, u8_t reset)
+{
+    u16_t i;
+    u16_t oppositeCount;
+    struct zsPartnerNotifyEvent event;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    //zm_debug_msg1("zfStaIbssMonitoring %d", wd->sta.oppositeCount);
+
+    zmw_enter_critical_section(dev);
+
+    if ( wd->sta.oppositeCount == 0 )
+    {
+        goto done;
+    }
+
+    if ( wd->sta.bChannelScan )
+    {
+        goto done;
+    }
+
+    oppositeCount = wd->sta.oppositeCount;
+
+    for(i=0; i < ZM_MAX_OPPOSITE_COUNT; i++)
+    {
+        if ( oppositeCount == 0 )
+        {
+            break;
+        }
+
+        if ( reset )
+        {
+            wd->sta.oppositeInfo[i].valid = 0;
+        }
+
+        if ( wd->sta.oppositeInfo[i].valid == 0 )
+        {
+            continue;
+        }
+
+        oppositeCount--;
+
+        if ( wd->sta.oppositeInfo[i].aliveCounter )
+        {
+            zm_debug_msg1("Setting alive to ", wd->sta.oppositeInfo[i].aliveCounter);
+
+            zmw_leave_critical_section(dev);
+
+            if ( wd->sta.oppositeInfo[i].aliveCounter != ZM_IBSS_PEER_ALIVE_COUNTER )
+            {
+                zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_PROBEREQ,
+                              (u16_t*)wd->sta.oppositeInfo[i].macAddr, 1, 0, 0);
+            }
+
+            zmw_enter_critical_section(dev);
+            wd->sta.oppositeInfo[i].aliveCounter--;
+        }
+        else
+        {
+            zm_debug_msg0("zfStaIbssMonitoring remove the peer station");
+            zfMemoryCopy(event.bssid, (u8_t *)(wd->sta.bssid), 6);
+            zfMemoryCopy(event.peerMacAddr, wd->sta.oppositeInfo[i].macAddr, 6);
+
+            wd->sta.oppositeInfo[i].valid = 0;
+            wd->sta.oppositeCount--;
+            if (wd->zfcbIbssPartnerNotify != NULL)
+            {
+                zmw_leave_critical_section(dev);
+                wd->zfcbIbssPartnerNotify(dev, 0, &event);
+                zmw_enter_critical_section(dev);
+            }
+        }
+    }
+
+done:
+    if ( reset == 0 )
+    {
+        zfTimerSchedule(dev, ZM_EVENT_IBSS_MONITOR, ZM_TICK_IBSS_MONITOR);
+    }
+
+    zmw_leave_critical_section(dev);
+}
+
+void zfInitPartnerNotifyEvent(zdev_t* dev, zbuf_t* buf, struct zsPartnerNotifyEvent *event)
+{
+    u16_t  *peerMacAddr;
+
+    zmw_get_wlan_dev(dev);
+
+    peerMacAddr = (u16_t *)event->peerMacAddr;
+
+    zfMemoryCopy(event->bssid, (u8_t *)(wd->sta.bssid), 6);
+    peerMacAddr[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET);
+    peerMacAddr[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET + 2);
+    peerMacAddr[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET + 4);
+}
+
+void zfStaInitOppositeInfo(zdev_t* dev)
+{
+    int i;
+
+    zmw_get_wlan_dev(dev);
+
+    for(i=0; i<ZM_MAX_OPPOSITE_COUNT; i++)
+    {
+        wd->sta.oppositeInfo[i].valid = 0;
+        wd->sta.oppositeInfo[i].aliveCounter = ZM_IBSS_PEER_ALIVE_COUNTER;
+    }
+}
+#ifdef ZM_ENABLE_CENC
+u16_t zfStaAddIeCenc(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+    zmw_get_wlan_dev(dev);
+
+    if (wd->sta.cencIe[1] != 0)
+    {
+        zfCopyToIntTxBuffer(dev, buf, wd->sta.cencIe, offset, wd->sta.cencIe[1]+2);
+        offset += (wd->sta.cencIe[1]+2);
+    }
+    return offset;
+}
+#endif //ZM_ENABLE_CENC
+u16_t zfStaProcessAction(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t category, actionDetails;
+    zmw_get_wlan_dev(dev);
+
+    category = zmw_rx_buf_readb(dev, buf, 24);
+    actionDetails = zmw_rx_buf_readb(dev, buf, 25);
+    switch (category)
+    {
+        case 0:		//Spectrum Management
+	        switch(actionDetails)
+	        {
+	        	case 0:			//Measurement Request
+	        		break;
+	        	case 1:			//Measurement Report
+	        		//ProcessActionSpectrumFrame_MeasurementReport(Adapter,pActionBody+3);
+	        		break;
+	        	case 2:			//TPC request
+                    //if (wd->sta.TPCEnable)
+                    //    zfStaUpdateDot11HTPC(dev, buf);
+	        		break;
+	        	case 3:			//TPC report
+                    //if (wd->sta.TPCEnable)
+                    //    zfStaUpdateDot11HTPC(dev, buf);
+	        		break;
+	        	case 4:			//Channel Switch Announcement
+                    if (wd->sta.DFSEnable)
+                        zfStaUpdateDot11HDFS(dev, buf);
+	        		break;
+	        	default:
+	        		zm_debug_msg1("Action Frame contain not support action field ", actionDetails);
+	        		break;
+	        }
+        	break;
+        case ZM_WLAN_BLOCK_ACK_ACTION_FRAME:
+            zfAggBlockAckActionFrame(dev, buf);
+            break;
+        case 17:	//Qos Management
+        	break;
+    }
+
+    return 0;
+}
+
+/* Determine the time not send beacon , if more than some value ,
+   re-write the beacon start address */
+void zfReWriteBeaconStartAddress(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    wd->tickIbssSendBeacon++;    // Increase 1 per 10ms .
+    zmw_leave_critical_section(dev);
+
+    if ( wd->tickIbssSendBeacon == 40 )
+    {
+//        DbgPrint("20070727");
+        zfHpEnableBeacon(dev, ZM_MODE_IBSS, wd->beaconInterval, wd->dtim, (u8_t)wd->sta.atimWindow);
+        zmw_enter_critical_section(dev);
+        wd->tickIbssSendBeacon = 0;
+        zmw_leave_critical_section(dev);
+    }
+}
+
+struct zsTkipSeed* zfStaGetRxSeed(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t keyIndex;
+    u8_t da0;
+
+    zmw_get_wlan_dev(dev);
+
+    /* if need not check MIC, return NULL */
+    if ( ((wd->sta.encryMode != ZM_TKIP)&&(wd->sta.encryMode != ZM_AES))||
+         (wd->sta.wpaState < ZM_STA_WPA_STATE_PK_OK) )
+    {
+        return NULL;
+    }
+
+    da0 = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_A1_OFFSET);
+
+    if ((zmw_rx_buf_readb(dev, buf, 0) & 0x80) == 0x80)
+        keyIndex = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_IV_OFFSET+5); /* Qos Packet*/
+    else
+        keyIndex = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_IV_OFFSET+3); /* normal Packet*/
+    keyIndex = (keyIndex & 0xc0) >> 6;
+
+    return (&wd->sta.rxSeed[keyIndex]);
+}
+
+void zfStaEnableSWEncryption(zdev_t *dev, u8_t value)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.SWEncryptEnable = value;
+    zfHpSWDecrypt(dev, 1);
+    zfHpSWEncrypt(dev, 1);
+}
+
+void zfStaDisableSWEncryption(zdev_t *dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.SWEncryptEnable = 0;
+    zfHpSWDecrypt(dev, 0);
+    zfHpSWEncrypt(dev, 0);
+}
+
+u16_t zfComputeBssInfoWeightValue(zdev_t *dev, u8_t isBMode, u8_t isHT, u8_t isHT40, u8_t signalStrength)
+{
+	u8_t  weightOfB           = 0;
+	u8_t  weightOfAGBelowThr  = 0;
+	u8_t  weightOfAGUpThr     = 15;
+	u8_t  weightOfN20BelowThr = 15;
+	u8_t  weightOfN20UpThr    = 30;
+	u8_t  weightOfN40BelowThr = 16;
+	u8_t  weightOfN40UpThr    = 32;
+
+    zmw_get_wlan_dev(dev);
+
+    if( isBMode == 0 )
+        return (signalStrength + weightOfB);    // pure b mode , do not add the weight value for this AP !
+    else
+    {
+        if( isHT == 0 && isHT40 == 0 )
+        { // a , g , b/g mode ! add the weight value 15 for this AP if it's signal strength is more than some value !
+            if( signalStrength < 18 ) // -77 dBm
+				return signalStrength + weightOfAGBelowThr;
+			else
+				return (signalStrength + weightOfAGUpThr);
+        }
+        else if( isHT == 1 && isHT40 == 0 )
+        { // 80211n mode use 20MHz
+            if( signalStrength < 23 ) // -72 dBm
+                return (signalStrength + weightOfN20BelowThr);
+            else
+                return (signalStrength + weightOfN20UpThr);
+        }
+        else // isHT == 1 && isHT40 == 1
+        { // 80211n mode use 40MHz
+            if( signalStrength < 16 ) // -79 dBm
+                return (signalStrength + weightOfN40BelowThr);
+            else
+                return (signalStrength + weightOfN40UpThr);
+        }
+    }
+}
+
+u16_t zfStaAddIbssAdditionalIE(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+	u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    for (i=0; i<wd->sta.ibssAdditionalIESize; i++)
+    {
+        zmw_tx_buf_writeb(dev, buf, offset++, wd->sta.ibssAdditionalIE[i]);
+    }
+
+    return offset;
+}
diff --git a/drivers/staging/otus/80211core/coid.c b/drivers/staging/otus/80211core/coid.c
new file mode 100644
index 00000000000..6007f3131f8
--- /dev/null
+++ b/drivers/staging/otus/80211core/coid.c
@@ -0,0 +1,2695 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : iod.c                                                 */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains OID functions.                             */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+#include "cprecomp.h"
+#include "../hal/hpreg.h"
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiWlanQueryMacAddress      */
+/*      Query OWN MAC address.                                          */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      addr : for return MAC address                                   */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+void zfiWlanQueryMacAddress(zdev_t* dev, u8_t* addr)
+{
+    u16_t vapId = 0;
+    zmw_get_wlan_dev(dev);
+
+    vapId = zfwGetVapId(dev);
+
+    addr[0] = (u8_t)(wd->macAddr[0] & 0xff);
+    addr[1] = (u8_t)(wd->macAddr[0] >> 8);
+    addr[2] = (u8_t)(wd->macAddr[1] & 0xff);
+    addr[3] = (u8_t)(wd->macAddr[1] >> 8);
+    addr[4] = (u8_t)(wd->macAddr[2] & 0xff);
+    if (vapId == 0xffff)
+        addr[5] = (u8_t)(wd->macAddr[2] >> 8);
+    else
+    {
+#ifdef ZM_VAPMODE_MULTILE_SSID
+        addr[5] = (u8_t)(wd->macAddr[2] >> 8); // Multiple SSID
+#else
+        addr[5] = vapId + 1 + (u8_t)(wd->macAddr[2] >> 8); //VAP
+#endif
+    }
+
+    return;
+}
+
+void zfiWlanQueryBssList(zdev_t* dev, struct zsBssList* pBssList)
+{
+    struct zsBssInfo*   pBssInfo;
+    struct zsBssInfo*   pDstBssInfo;
+    u8_t   i;
+    u8_t*  pMemList;
+    u8_t*  pMemInfo;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    pMemList = (u8_t*) pBssList;
+    pMemInfo = pMemList + sizeof(struct zsBssList);
+    pBssList->head = (struct zsBssInfo*) pMemInfo;
+
+    zmw_enter_critical_section(dev);
+
+    pBssInfo = wd->sta.bssList.head;
+    pDstBssInfo = (struct zsBssInfo*) pMemInfo;
+    pBssList->bssCount = wd->sta.bssList.bssCount;
+
+    for( i=0; i<wd->sta.bssList.bssCount; i++ )
+    {
+        zfMemoryCopy((u8_t*)pDstBssInfo, (u8_t*)pBssInfo,
+                sizeof(struct zsBssInfo));
+
+        if ( pBssInfo->next != NULL )
+        {
+            pBssInfo = pBssInfo->next;
+            pDstBssInfo->next = pDstBssInfo + 1;
+            pDstBssInfo++;
+        }
+        else
+        {
+            zm_assert(i==(wd->sta.bssList.bssCount-1));
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+
+    zfScanMgrScanAck(dev);
+}
+
+void zfiWlanQueryBssListV1(zdev_t* dev, struct zsBssListV1* bssListV1)
+{
+    struct zsBssInfo*   pBssInfo;
+    //struct zsBssInfo*   pDstBssInfo;
+    u8_t   i, j, bdrop = 0, k = 0, Same_Count = 0;
+    u8_t   bssid[6];
+    //u8_t*  pMemList;
+    //u8_t*  pMemInfo;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    bssListV1->bssCount = wd->sta.bssList.bssCount;
+
+    pBssInfo = wd->sta.bssList.head;
+    ZM_MAC_WORD_TO_BYTE(wd->sta.bssid, bssid);
+
+    for( i=0; i<wd->sta.bssList.bssCount; i++ )
+    {
+        bdrop = 0;
+        if ( zfStaIsConnected(dev)
+             && (wd->wlanMode == ZM_MODE_INFRASTRUCTURE ) )
+        {
+			for (j = 0; j < 6; j++)
+            {
+                if ( pBssInfo->bssid[j] != bssid[j] )
+                {
+                    break;
+                }
+            }
+
+            if  ( (j == 6)
+                  &&((pBssInfo->ssid[1] == wd->sta.ssidLen) || (pBssInfo->ssid[1] == 0) )&& (pBssInfo->frequency == wd->frequency) )
+            {
+				if(pBssInfo->ssid[1] == 0)
+					pBssInfo->ssid[1] = wd->sta.ssidLen;
+
+				if(Same_Count == 0)
+				{//First meet
+					Same_Count++;
+				}
+				else
+				{//same one
+					bdrop = 1;
+					bssListV1->bssCount--;
+				}
+
+            }
+        }
+
+        if (bdrop == 0)
+        {
+            zfMemoryCopy((u8_t*)(&bssListV1->bssInfo[k]), (u8_t*)pBssInfo,
+                sizeof(struct zsBssInfo));
+
+			if(Same_Count == 1)
+			{
+				zfMemoryCopy(&(bssListV1->bssInfo[k].ssid[2]), wd->sta.ssid, wd->sta.ssidLen);
+				Same_Count++;
+			}
+
+			k++;
+        }
+
+        if ( pBssInfo->next != NULL )
+        {
+            pBssInfo = pBssInfo->next;
+        }
+        else
+        {
+            zm_assert(i==(wd->sta.bssList.bssCount-1));
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+
+    zfScanMgrScanAck(dev);
+}
+
+void zfiWlanQueryAdHocCreatedBssDesc(zdev_t* dev, struct zsBssInfo *pBssInfo)
+{
+    zmw_get_wlan_dev(dev);
+
+    zfMemoryCopy((u8_t *)pBssInfo, (u8_t *)&wd->sta.ibssBssDesc, sizeof(struct zsBssInfo));
+}
+
+u8_t zfiWlanQueryAdHocIsCreator(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->sta.ibssBssIsCreator;
+}
+
+u32_t zfiWlanQuerySupportMode(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->supportMode;
+}
+
+u32_t zfiWlanQueryTransmitPower(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    u32_t ret = 0;
+
+    if (zfStaIsConnected(dev)) {
+        ret = wd->sta.connPowerInHalfDbm;
+    } else {
+        ret = zfHpGetTransmitPower(dev);
+    }
+
+    return ret;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiWlanFlushBssList         */
+/*      Flush BSSID List.                                               */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2006.12     */
+/*                                                                      */
+/************************************************************************/
+void zfiWlanFlushBssList(zdev_t* dev)
+{
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    /* Call zfBssInfoRefresh() twice to remove all entry */
+    zfBssInfoRefresh(dev, 1);
+    zmw_leave_critical_section(dev);
+}
+
+void zfiWlanSetWlanMode(zdev_t* dev, u8_t wlanMode)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    wd->ws.wlanMode = wlanMode;
+    zmw_leave_critical_section(dev);
+}
+
+void zfiWlanSetAuthenticationMode(zdev_t* dev, u8_t authMode)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    wd->ws.authMode = authMode;
+    zmw_leave_critical_section(dev);
+}
+
+void zfiWlanSetWepStatus(zdev_t* dev, u8_t wepStatus)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    wd->ws.wepStatus = wepStatus;
+    zmw_leave_critical_section(dev);
+
+}
+
+void zfiWlanSetSSID(zdev_t* dev, u8_t* ssid, u8_t ssidLength)
+{
+    u16_t i;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if ( ssidLength <= 32 )
+    {
+        zmw_enter_critical_section(dev);
+
+        wd->ws.ssidLen = ssidLength;
+        zfMemoryCopy(wd->ws.ssid, ssid, ssidLength);
+
+        if ( ssidLength < 32 )
+        {
+            wd->ws.ssid[ssidLength] = 0;
+        }
+
+        wd->ws.probingSsidList[0].ssidLen = ssidLength;
+        zfMemoryCopy(wd->ws.probingSsidList[0].ssid, ssid, ssidLength);
+        for (i=1; i<ZM_MAX_PROBE_HIDDEN_SSID_SIZE; i++)
+        {
+            wd->ws.probingSsidList[i].ssidLen = 0;
+        }
+
+        zmw_leave_critical_section(dev);
+    }
+}
+
+void zfiWlanSetFragThreshold(zdev_t* dev, u16_t fragThreshold)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if (fragThreshold == 0)
+    {   /* fragmentation is disabled */
+        wd->fragThreshold = 32767;
+    }
+    else if (fragThreshold < 256)
+    {
+        /* Minimum fragment threshold */
+        wd->fragThreshold = 256;
+    }
+    else if (fragThreshold > 2346)
+    {
+        wd->fragThreshold = 2346;
+    }
+    else
+    {
+        wd->fragThreshold = fragThreshold & 0xfffe;
+    }
+
+    zmw_leave_critical_section(dev);
+}
+
+void zfiWlanSetRtsThreshold(zdev_t* dev, u16_t rtsThreshold)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    wd->rtsThreshold = rtsThreshold;
+    zmw_leave_critical_section(dev);
+}
+
+void zfiWlanSetFrequency(zdev_t* dev, u32_t frequency, u8_t bImmediate)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if ( bImmediate )
+    {
+        zmw_enter_critical_section(dev);
+        wd->frequency = (u16_t) (frequency/1000);
+        zmw_leave_critical_section(dev);
+        zfCoreSetFrequency(dev, wd->frequency);
+    }
+    else
+    {
+        zmw_enter_critical_section(dev);
+        if( frequency == 0 )
+        { // Auto select clean channel depend on wireless environment !
+            wd->ws.autoSetFrequency = 0;
+        }
+        wd->ws.frequency = (u16_t) (frequency/1000);
+        zmw_leave_critical_section(dev);
+    }
+}
+
+void zfiWlanSetBssid(zdev_t* dev, u8_t* bssid)
+{
+    u16_t i;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    for (i=0; i<6; i++)
+    {
+        wd->ws.desiredBssid[i] = bssid[i];
+    }
+    wd->ws.bDesiredBssid = TRUE;
+    zmw_leave_critical_section(dev);
+
+}
+
+void zfiWlanSetBeaconInterval(zdev_t* dev,
+                              u16_t  beaconInterval,
+                              u8_t   bImmediate)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if ( bImmediate )
+    {
+        zmw_enter_critical_section(dev);
+        wd->beaconInterval = beaconInterval;
+        zmw_leave_critical_section(dev);
+
+        /* update beacon interval here */
+    }
+    else
+    {
+        zmw_enter_critical_section(dev);
+        wd->ws.beaconInterval = beaconInterval;
+        zmw_leave_critical_section(dev);
+    }
+}
+
+
+void zfiWlanSetDtimCount(zdev_t* dev, u8_t  dtim)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    if (dtim > 0)
+    {
+        wd->ws.dtim = dtim;
+    }
+    zmw_leave_critical_section(dev);
+}
+
+
+void zfiWlanSetAtimWindow(zdev_t* dev, u16_t atimWindow, u8_t bImmediate)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if ( bImmediate )
+    {
+        zmw_enter_critical_section(dev);
+        wd->sta.atimWindow = atimWindow;
+        zmw_leave_critical_section(dev);
+
+        /* atim window here */
+    }
+    else
+    {
+        zmw_enter_critical_section(dev);
+        wd->ws.atimWindow = atimWindow;
+        zmw_leave_critical_section(dev);
+    }
+}
+
+
+void zfiWlanSetEncryMode(zdev_t* dev, u8_t encryMode)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        /* Hostapd Issue */
+        if ((wd->ws.encryMode != ZM_AES) && (wd->ws.encryMode != ZM_TKIP))
+            wd->ws.encryMode = encryMode;
+    }
+    else
+        wd->ws.encryMode = encryMode;
+    zmw_leave_critical_section(dev);
+}
+
+void zfiWlanSetDefaultKeyId(zdev_t* dev, u8_t keyId)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.keyId = keyId;
+}
+
+u8_t zfiWlanQueryIsPKInstalled(zdev_t *dev, u8_t *staMacAddr)
+{
+    u8_t isInstalled = 0;
+
+#if 1
+//#ifdef ZM_ENABLE_IBSS_WPA2PSK
+    u8_t   res, peerIdx;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    res = zfStaFindOppositeByMACAddr(dev, (u16_t *)staMacAddr, &peerIdx);
+    if( res == 0 )
+    {
+        isInstalled = wd->sta.oppositeInfo[peerIdx].pkInstalled;
+    }
+    zmw_leave_critical_section(dev);
+//#endif
+#endif
+
+    return isInstalled;
+}
+
+u8_t zfiWlanSetKey(zdev_t* dev, struct zsKeyInfo keyInfo)
+{
+    u16_t  broadcast[3] = {0xffff, 0xffff, 0xffff};
+    u32_t* key;
+    u8_t   encryMode = ZM_NO_WEP;
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+    u8_t   encryType = ZM_NO_WEP;
+#endif
+    u8_t   micKey[16];
+    u16_t  id = 0;
+    u8_t   vapId, i, addr[6];
+    u8_t   userIdx=0;
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+    /* Determine opposite exist or not */
+    u8_t   res, peerIdx;
+//    u8_t   userIdx=0;
+
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->sta.ibssWpa2Psk == 1 )
+    {
+        zmw_enter_critical_section(dev);
+        res = zfStaFindOppositeByMACAddr(dev, (u16_t*)keyInfo.macAddr, &peerIdx);
+        if( res == 0 )
+        {
+            userIdx = peerIdx;
+            if ( wd->sta.oppositeInfo[userIdx].camIdx == 0xff )
+                wd->sta.oppositeInfo[userIdx].camIdx = userIdx;
+        }
+        zmw_leave_critical_section(dev);
+    }
+#else
+    zmw_get_wlan_dev(dev);
+#endif
+
+    if ( keyInfo.flag & ZM_KEY_FLAG_AUTHENTICATOR )
+    {   /* set key by authenticator */
+        /* set pairwise key */
+        if (keyInfo.flag & ZM_KEY_FLAG_PK)
+        {
+            /* Find STA's information */
+            if ((id = zfApFindSta(dev, keyInfo.macAddr)) == 0xffff)
+            {
+                /* Can't STA in the staTable */
+                return ZM_STATUS_FAILURE;
+            }
+
+            wd->ap.staTable[id].iv16 = 0;
+            wd->ap.staTable[id].iv32 = 0;
+
+            if (keyInfo.keyLength == 32)
+            {   /* TKIP */
+                //u8_t KeyRsc[6] = {0, 0, 0, 0, 0, 0};
+
+                /* In the current AP mode, we set KeyRsc to zero */
+                //zfTkipInit(keyInfo.key, (u8_t*) wd->macAddr,
+                //           &(wd->ap.staTable[id].txSeed), KeyRsc);
+                //zfTkipInit(keyInfo.key, (u8_t*) keyInfo.macAddr,
+                //           &(wd->ap.staTable[id].rxSeed), KeyRsc);
+#ifdef ZM_ENABLE_CENC
+                if (keyInfo.flag & ZM_KEY_FLAG_CENC)
+                {
+                    zm_debug_msg0("Set CENC pairwise Key");
+
+                    wd->ap.staTable[id].encryMode = ZM_CENC;
+
+                    /* Reset txiv and rxiv */
+                    wd->ap.staTable[id].txiv[0] = 0x5c365c37;
+                    wd->ap.staTable[id].txiv[1] = 0x5c365c36;
+                    wd->ap.staTable[id].txiv[2] = 0x5c365c36;
+                    wd->ap.staTable[id].txiv[3] = 0x5c365c36;
+
+                    wd->ap.staTable[id].rxiv[0] = 0x5c365c36;
+                    wd->ap.staTable[id].rxiv[1] = 0x5c365c36;
+                    wd->ap.staTable[id].rxiv[2] = 0x5c365c36;
+                    wd->ap.staTable[id].rxiv[3] = 0x5c365c36;
+
+                    /* Set Key Index */
+                    wd->ap.staTable[id].cencKeyIdx = keyInfo.keyIndex;
+
+                    //zfCoreSetKey(dev, id+1, 1, ZM_CENC, (u16_t *)keyInfo.macAddr,
+                    //          (u32_t*) &keyInfo.key[16]);
+                }
+                else
+#endif //ZM_ENABLE_CENC
+                {
+                    wd->ap.staTable[id].encryMode = ZM_TKIP;
+
+                    zfMemoryCopy(micKey, &keyInfo.key[16], 8);
+                    zfMemoryCopy(&micKey[8], &keyInfo.key[24], 8);
+
+                    //zfCoreSetKey(dev, id+1, 1, ZM_TKIP, (u16_t *)keyInfo.macAddr,
+                    //           (u32_t*) micKey);
+
+                    /* For fragmentation, we use software MIC */
+                    zfMemoryCopy((u8_t *)&(wd->ap.staTable[id].txMicKey), &(keyInfo.key[16]), 8);
+                    zfMemoryCopy((u8_t *)&(wd->ap.staTable[id].rxMicKey), &(keyInfo.key[24]), 8);
+
+                }
+            }
+            else if (keyInfo.keyLength == 16)
+            {   /* AES */
+                wd->ap.staTable[id].encryMode = ZM_AES;
+            }
+            else if (keyInfo.keyLength == 0)
+            {
+                /* Clear Key Info */
+                zfApClearStaKey(dev, (u16_t *)keyInfo.macAddr);
+
+                return ZM_STATUS_SUCCESS;
+            }
+            else
+            {
+                return ZM_STATUS_FAILURE;
+            }
+
+            //zfCoreSetKey(dev, id+1, 0, wd->ap.staTable[id].encryMode,
+            //      (u16_t *)keyInfo.macAddr, (u32_t*) keyInfo.key);
+            zfHpSetApPairwiseKey(dev, (u16_t *)keyInfo.macAddr,
+                    wd->ap.staTable[id].encryMode, (u32_t*) keyInfo.key,
+                    (u32_t*) &keyInfo.key[16], id+1);
+            wd->ap.staTable[id].keyIdx = id + 1 + 4;
+        }
+        else if (keyInfo.flag & ZM_KEY_FLAG_GK)
+        {
+            vapId = keyInfo.vapId;
+
+            wd->ap.iv16[vapId] = 0;
+            wd->ap.iv32[vapId] = 0;
+
+            if (keyInfo.keyLength == 32)
+            {   /* TKIP */
+                //u8_t KeyRsc[6] = {0, 0, 0, 0, 0, 0};
+
+                //zfTkipInit(keyInfo.key, (u8_t*) wd->macAddr,
+                //           &(wd->ap.bcSeed), KeyRsc);
+#ifdef ZM_ENABLE_CENC
+                if (keyInfo.flag & ZM_KEY_FLAG_CENC)
+                {
+                    encryMode = ZM_CENC;
+                    zm_debug_msg0("Set CENC group Key");
+
+                    /* Reset txiv and rxiv */
+                    wd->ap.txiv[vapId][0] = 0x5c365c36;
+                    wd->ap.txiv[vapId][1] = 0x5c365c36;
+                    wd->ap.txiv[vapId][2] = 0x5c365c36;
+                    wd->ap.txiv[vapId][3] = 0x5c365c36;
+
+                    //zfCoreSetKey(dev, 0, 1, ZM_CENC, keyInfo.vapAddr,
+                    //          (u32_t*) &keyInfo.key[16]);
+                    key = (u32_t*) keyInfo.key;
+                }
+                else
+#endif //ZM_ENABLE_CENC
+                {
+                    encryMode = ZM_TKIP;
+                    key = (u32_t *)keyInfo.key;
+
+                    /* set MIC key to HMAC */
+                    //zfCoreSetKey(dev, 0, 1, ZM_TKIP, broadcast,
+                    //         (u32_t*) (&keyInfo.key[16]));
+                    //zfCoreSetKey(dev, 0, 1, ZM_TKIP, keyInfo.vapAddr,
+                    //           (u32_t*) (&keyInfo.key[16]));
+
+                    zfMicSetKey(&(keyInfo.key[16]), &(wd->ap.bcMicKey[0]));
+                    key = (u32_t*) keyInfo.key;
+                }
+            }
+            else if (keyInfo.keyLength == 16)
+            {   /* AES */
+                encryMode = ZM_AES;
+                key = (u32_t *)keyInfo.key;
+                zm_debug_msg0("CWY - Set AES Group Key");
+            }
+            else if (keyInfo.keyLength == 0)
+            {
+                /* Clear Key Info */
+                zfApClearStaKey(dev, broadcast);
+
+                /* Turn off WEP bit in the capability field */
+                wd->ap.capab[vapId] &= 0xffef;
+
+                return ZM_STATUS_SUCCESS;
+            }
+            else
+            {   /* WEP */
+                if (keyInfo.keyLength == 5)
+                {
+                    encryMode = ZM_WEP64;
+                }
+                else if (keyInfo.keyLength == 13)
+                {
+                    encryMode = ZM_WEP128;
+                }
+                else if (keyInfo.keyLength == 29)
+                {
+                    encryMode = ZM_WEP256;
+                }
+
+                key = (u32_t*) keyInfo.key;
+            }
+
+            // Modification for CAM not support VAP search
+            //zfCoreSetKey(dev, 0, 0, encryMode, broadcast, key);
+            //zfCoreSetKey(dev, 0, 0, encryMode, wd->macAddr, key);
+            //zfCoreSetKey(dev, 0, 0, encryMode, keyInfo.vapAddr, key);
+            zfHpSetApGroupKey(dev, wd->macAddr, encryMode,
+                    key, (u32_t*) &keyInfo.key[16], vapId);
+
+            //zfiWlanSetEncryMode(dev, encryMode);
+            wd->ws.encryMode = encryMode;
+
+            /* set the multicast address encryption type */
+            wd->ap.encryMode[vapId] = encryMode;
+
+            /* set the multicast key index */
+            wd->ap.bcKeyIndex[vapId] = keyInfo.keyIndex;
+            wd->ap.bcHalKeyIdx[vapId] = vapId + 60;
+
+            /* Turn on WEP bit in the capability field */
+            wd->ap.capab[vapId] |= 0x10;
+        }
+    }
+    else
+    {   /* set by supplicant */
+
+        if ( keyInfo.flag & ZM_KEY_FLAG_PK )
+        {   /* set pairwise key */
+
+            //zfTkipInit(keyInfo.key, (u8_t*) wd->macAddr,
+            //           &wd->sta.txSeed, keyInfo.initIv);
+            //zfTkipInit(keyInfo.key, (u8_t*) wd->sta.bssid,
+            //           &wd->sta.rxSeed[keyInfo.keyIndex], keyInfo.initIv);
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+            if ( wd->sta.ibssWpa2Psk == 1 )
+            {
+                /* unicast -- > pairwise key */
+                wd->sta.oppositeInfo[userIdx].iv16 = 0;
+                wd->sta.oppositeInfo[userIdx].iv32 = 0;
+            }
+            else
+            {
+                wd->sta.iv16 = 0;
+                wd->sta.iv32 = 0;
+            }
+
+            wd->sta.oppositeInfo[userIdx].pkInstalled = 1;
+#else
+            wd->sta.iv16 = 0;
+            wd->sta.iv32 = 0;
+
+            wd->sta.oppositeInfo[userIdx].pkInstalled = 1;
+#endif
+
+            if ( keyInfo.keyLength == 32 )
+            {   /* TKIP */
+                zfTkipInit(keyInfo.key, (u8_t*) wd->macAddr,
+                        &wd->sta.txSeed, keyInfo.initIv);
+                zfTkipInit(keyInfo.key, (u8_t*) wd->sta.bssid,
+                        &wd->sta.rxSeed[keyInfo.keyIndex], keyInfo.initIv);
+
+#ifdef ZM_ENABLE_CENC
+                if (keyInfo.flag & ZM_KEY_FLAG_CENC)
+                {
+                    zm_debug_msg0("Set CENC pairwise Key");
+
+                    wd->sta.encryMode = ZM_CENC;
+
+                    /* Reset txiv and rxiv */
+                    wd->sta.txiv[0] = 0x5c365c36;
+                    wd->sta.txiv[1] = 0x5c365c36;
+                    wd->sta.txiv[2] = 0x5c365c36;
+                    wd->sta.txiv[3] = 0x5c365c36;
+
+                    wd->sta.rxiv[0] = 0x5c365c37;
+                    wd->sta.rxiv[1] = 0x5c365c36;
+                    wd->sta.rxiv[2] = 0x5c365c36;
+                    wd->sta.rxiv[3] = 0x5c365c36;
+
+                    /* Set Key Index */
+                    wd->sta.cencKeyId = keyInfo.keyIndex;
+
+                    //zfCoreSetKey(dev, id+1, 1, ZM_CENC, (u16_t *)keyInfo.macAddr,
+                    //         (u32_t*) &keyInfo.key[16]);
+                }
+                else
+#endif //ZM_ENABLE_CENC
+                {
+                    wd->sta.encryMode = ZM_TKIP;
+
+                    //zfCoreSetKey(dev, 0, 1, ZM_TKIP, wd->sta.bssid,
+                    //         (u32_t*) &keyInfo.key[16]);
+
+                    zfMicSetKey(&keyInfo.key[16], &wd->sta.txMicKey);
+                    zfMicSetKey(&keyInfo.key[24],
+                                &wd->sta.rxMicKey[keyInfo.keyIndex]);
+                }
+            }
+            else if ( keyInfo.keyLength == 16 )
+            {   /* AES */
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+                if ( wd->sta.ibssWpa2Psk == 1 )
+                {
+                    wd->sta.oppositeInfo[userIdx].encryMode = ZM_AES;
+                    encryType = wd->sta.oppositeInfo[userIdx].encryMode;
+                }
+                else
+                {
+                    wd->sta.encryMode = ZM_AES;
+                    encryType = wd->sta.encryMode;
+                }
+#else
+                wd->sta.encryMode = ZM_AES;
+#endif
+            }
+            else
+            {
+                return ZM_STATUS_FAILURE;
+            }
+
+            /* user 0 */
+            //zfCoreSetKey(dev, 0, 0, wd->sta.encryMode,
+            //         wd->sta.bssid, (u32_t*) keyInfo.key);
+            //zfHpSetStaPairwiseKey(dev, wd->sta.bssid, wd->sta.encryMode,
+            //    (u32_t*) keyInfo.key, (u32_t*) &keyInfo.key[16]);
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+            if ( (keyInfo.keyLength==16) && (wd->sta.ibssWpa2Psk==1) )
+            { /* If not AES-CCMP and ibss network , use traditional */
+                zfHpSetPerUserKey(dev,
+                                userIdx,
+                                keyInfo.keyIndex,  // key id == 0 ( Pairwise key = 0 )
+                                (u8_t*)keyInfo.macAddr,   // RX need Source Address ( Address 2 )
+                                encryType,
+//                              wd->sta.encryMode,
+                                (u32_t*) keyInfo.key, (u32_t*) &keyInfo.key[16]);
+
+                wd->sta.oppositeInfo[userIdx].wpaState = ZM_STA_WPA_STATE_PK_OK ;
+            }
+            else
+            {/* Big Endian and Little Endian Compatibility */
+                for (i = 0; i < 3; i++)
+                {
+                    addr[2 * i] = wd->sta.bssid[i] & 0xff;
+                    addr[2 * i + 1] = wd->sta.bssid[i] >> 8;
+                }
+                zfHpSetPerUserKey(dev,
+                                    ZM_USER_KEY_PK,   // user id
+                                    0,                // key id
+                                    addr,//(u8_t *)wd->sta.bssid,
+                              wd->sta.encryMode,
+                              (u32_t*) keyInfo.key, (u32_t*) &keyInfo.key[16]);
+
+                wd->sta.keyId = 4;
+            }
+#else
+            /* Big Endian and Little Endian Compatibility */
+            for (i = 0; i < 3; i++)
+            {
+                addr[2 * i] = wd->sta.bssid[i] & 0xff;
+                addr[2 * i + 1] = wd->sta.bssid[i] >> 8;
+            }
+            zfHpSetPerUserKey(dev,
+                              ZM_USER_KEY_PK,   // user id
+                              0,                // key id
+                              addr,//(u8_t *)wd->sta.bssid,
+                              wd->sta.encryMode,
+                              (u32_t*) keyInfo.key, (u32_t*) &keyInfo.key[16]);
+
+            wd->sta.keyId = 4;
+#endif
+
+            wd->sta.wpaState = ZM_STA_WPA_STATE_PK_OK;
+        }
+        else if ( keyInfo.flag & ZM_KEY_FLAG_GK )
+        {   /* set group key */
+
+            zfTkipInit(keyInfo.key, (u8_t*) wd->sta.bssid,
+                       &wd->sta.rxSeed[keyInfo.keyIndex], keyInfo.initIv);
+
+            if ( keyInfo.keyLength == 32 )
+            {   /* TKIP */
+#ifdef ZM_ENABLE_CENC
+                if (keyInfo.flag & ZM_KEY_FLAG_CENC)
+                {
+                    encryMode = ZM_CENC;
+                    zm_debug_msg0("Set CENC group Key");
+
+                    /* Reset txiv and rxiv */
+                    wd->sta.rxivGK[0] = 0x5c365c36;
+                    wd->sta.rxivGK[1] = 0x5c365c36;
+                    wd->sta.rxivGK[2] = 0x5c365c36;
+                    wd->sta.rxivGK[3] = 0x5c365c36;
+
+                    //zfCoreSetKey(dev, 0, 1, ZM_CENC, keyInfo.vapAddr,
+                    //         (u32_t*) &keyInfo.key[16]);
+                    key = (u32_t*) keyInfo.key;
+                }
+                else
+#endif //ZM_ENABLE_CENC
+                {
+                    encryMode = ZM_TKIP;
+                    key = (u32_t*) wd->sta.rxSeed[keyInfo.keyIndex].tk;
+
+                    if ( !(keyInfo.flag & ZM_KEY_FLAG_INIT_IV) )
+                    {
+                        wd->sta.rxSeed[keyInfo.keyIndex].iv16 = 0;
+                        wd->sta.rxSeed[keyInfo.keyIndex].iv32 = 0;
+                    }
+
+                    /* set MIC key to HMAC */
+                    //zfCoreSetKey(dev, 8, 1, ZM_TKIP, broadcast,
+                    //         (u32_t*) (&keyInfo.key[16]));
+
+                    zfMicSetKey(&keyInfo.key[24],
+                                &wd->sta.rxMicKey[keyInfo.keyIndex]);
+                }
+            }
+            else if ( keyInfo.keyLength == 16 )
+            {   /* AES */
+                encryMode = ZM_AES;
+                //key = (u32_t*) wd->sta.rxSeed[keyInfo.keyIndex].tk;
+            }
+            else
+            {   /* WEP */
+                if ( keyInfo.keyLength == 5 )
+                {
+                    encryMode = ZM_WEP64;
+                }
+                else if ( keyInfo.keyLength == 13 )
+                {
+                    encryMode = ZM_WEP128;
+                }
+                else if ( keyInfo.keyLength == 29 )
+                {
+                    encryMode = ZM_WEP256;
+                }
+
+                key = (u32_t*) keyInfo.key;
+            }
+
+            /* user 8 */
+            //zfCoreSetKey(dev, 8, 0, encryMode, broadcast, key);
+            //zfHpSetStaGroupKey(dev, broadcast, encryMode,
+            //        (u32_t*) keyInfo.key, (u32_t*) (&keyInfo.key[16]));
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+            if ( (keyInfo.keyLength==16) && (wd->sta.ibssWpa2Psk==1) )
+            {/* If not AES-CCMP and ibss network , use traditional */
+                zfHpSetPerUserKey(dev,
+                              userIdx,
+                              keyInfo.keyIndex,                // key id
+                              // (u8_t *)broadcast,                  // for only 2 stations IBSS netwrl ( A2 )
+                              (u8_t*)keyInfo.macAddr,   // for multiple ( > 2 ) stations IBSS network ( A2 )
+                              encryMode,
+                              (u32_t*) keyInfo.key, (u32_t*) &keyInfo.key[16]);
+            }
+            else
+            {
+                zfHpSetPerUserKey(dev,
+                                ZM_USER_KEY_GK,   // user id
+                                0,                // key id
+                                (u8_t *)broadcast,
+                                encryMode,
+                                (u32_t*) keyInfo.key, (u32_t*) &keyInfo.key[16]);
+
+                wd->sta.wpaState = ZM_STA_WPA_STATE_GK_OK;
+            }
+#else
+            zfHpSetPerUserKey(dev,
+                              ZM_USER_KEY_GK,   // user id
+                              0,                // key id
+                              (u8_t *)broadcast,
+                              encryMode,
+                              (u32_t*) keyInfo.key, (u32_t*) &keyInfo.key[16]);
+
+            wd->sta.wpaState = ZM_STA_WPA_STATE_GK_OK;
+#endif
+        }
+        else
+        {   /* legacy WEP */
+            zm_debug_msg0("legacy WEP");
+
+            if ( keyInfo.keyIndex >= 4 )
+            {
+                return ZM_STATUS_FAILURE;
+            }
+
+            if ( keyInfo.keyLength == 5 )
+            {
+                zm_debug_msg0("WEP 64");
+
+                encryMode = ZM_WEP64;
+            }
+            else if ( keyInfo.keyLength == 13 )
+            {
+                zm_debug_msg0("WEP 128");
+
+                encryMode = ZM_WEP128;
+            }
+            else if ( keyInfo.keyLength == 32 )
+            {
+                /* TKIP */
+                #if 0
+                // Don't reset the IV since some AP would fail in IV check and drop our connection
+                if ( wd->sta.wpaState != ZM_STA_WPA_STATE_PK_OK )
+                {
+                    wd->sta.iv16 = 0;
+                    wd->sta.iv32 = 0;
+                }
+                #endif
+
+                encryMode = ZM_TKIP;
+
+                zfTkipInit(keyInfo.key, (u8_t*) wd->sta.bssid,
+                           &wd->sta.rxSeed[keyInfo.keyIndex], keyInfo.initIv);
+                zfMicSetKey(&keyInfo.key[24],
+                           &wd->sta.rxMicKey[keyInfo.keyIndex]);
+            }
+            else if ( keyInfo.keyLength == 16 )
+            {
+                /* AES */
+                #if 0
+                // Don't reset the IV since some AP would fail in IV check and drop our connection
+                if ( wd->sta.wpaState != ZM_STA_WPA_STATE_PK_OK )
+                {
+                    /* broadcast -- > group key */
+                    /* Only initialize when set our default key ! */
+                    wd->sta.iv16 = 0;
+                    wd->sta.iv32 = 0;
+                }
+                #endif
+
+                encryMode = ZM_AES;
+            }
+            else if ( keyInfo.keyLength == 29 )
+            {
+                zm_debug_msg0("WEP 256");
+
+                encryMode = ZM_WEP256;
+                //zfCoreSetKey(dev, 64, 1, wd->sta.encryMode,
+                //         wd->sta.bssid, (u32_t*) (&keyInfo.key[16]));
+            }
+            else
+            {
+                return ZM_STATUS_FAILURE;
+            }
+
+            {
+                u8_t i;
+
+                zm_debug_msg0("key = ");
+                for(i = 0; i < keyInfo.keyLength; i++)
+                {
+                    zm_debug_msg2("", keyInfo.key[i]);
+                }
+            }
+
+            if ( keyInfo.flag & ZM_KEY_FLAG_DEFAULT_KEY )
+            {
+                //for WEP default key 1~3 and ATOM platform--CWYang(+)
+                vapId = 0;
+                wd->ap.bcHalKeyIdx[vapId] = keyInfo.keyIndex;
+                wd->ap.bcKeyIndex[vapId] = keyInfo.keyIndex;
+                wd->sta.keyId = keyInfo.keyIndex;
+            }
+
+			if(encryMode == ZM_TKIP)
+			{
+				if(wd->TKIP_Group_KeyChanging == 0x1)
+				{
+					zm_debug_msg0("Countermeasure : Cancel Old Timer ");
+					zfTimerCancel(dev,	ZM_EVENT_SKIP_COUNTERMEASURE);
+				}
+				else
+				{
+					zm_debug_msg0("Countermeasure : Create New Timer ");
+				}
+
+				wd->TKIP_Group_KeyChanging = 0x1;
+				zfTimerSchedule(dev, ZM_EVENT_SKIP_COUNTERMEASURE, 150);
+			}
+
+
+
+			//------------------------------------------------------------------------
+
+            /* use default key */
+            //zfCoreSetKey(dev, ZM_USER_KEY_DEFAULT+keyInfo.keyIndex, 0,
+            //         wd->sta.encryMode, wd->sta.bssid, (u32_t*) keyInfo.key);
+
+            if ( encryMode == ZM_TKIP ||
+                 encryMode == ZM_AES )
+            {
+                zfHpSetDefaultKey(dev, keyInfo.keyIndex, encryMode,
+                                 (u32_t*) keyInfo.key, (u32_t*) &keyInfo.key[16]);
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+            if ( (keyInfo.keyLength==16) && (wd->sta.ibssWpa2Psk==1) )
+            {/* If not AES-CCMP and ibss network , use traditional */
+                wd->sta.wpaState = ZM_STA_WPA_STATE_PK_OK;
+            }
+            else
+            {
+                if (wd->sta.wpaState == ZM_STA_WPA_STATE_PK_OK)
+                    wd->sta.wpaState = ZM_STA_WPA_STATE_GK_OK;
+                else
+                {
+                    wd->sta.wpaState = ZM_STA_WPA_STATE_PK_OK;
+                    wd->sta.encryMode = encryMode;
+                    wd->ws.encryMode = encryMode;
+                }
+            }
+#else
+                if (wd->sta.wpaState == ZM_STA_WPA_STATE_PK_OK)
+                    wd->sta.wpaState = ZM_STA_WPA_STATE_GK_OK;
+                else if ( wd->sta.wpaState == ZM_STA_WPA_STATE_INIT )
+                {
+                    wd->sta.wpaState = ZM_STA_WPA_STATE_PK_OK;
+                    wd->sta.encryMode = encryMode;
+                    wd->ws.encryMode = encryMode;
+                }
+#endif
+            }
+            else
+            {
+                zfHpSetDefaultKey(dev, keyInfo.keyIndex, encryMode,
+                               (u32_t*) keyInfo.key, NULL);
+
+                /* Save key for software WEP */
+                zfMemoryCopy(wd->sta.wepKey[keyInfo.keyIndex], keyInfo.key,
+                        keyInfo.keyLength);
+
+                /* TODO: Check whether we need to save the SWEncryMode */
+                wd->sta.SWEncryMode[keyInfo.keyIndex] = encryMode;
+
+                wd->sta.encryMode = encryMode;
+                wd->ws.encryMode = encryMode;
+            }
+        }
+    }
+
+//    wd->sta.flagKeyChanging = 1;
+    return ZM_STATUS_SUCCESS;
+}
+
+/* PSEUDO test */
+u8_t zfiWlanPSEUDOSetKey(zdev_t* dev, struct zsKeyInfo keyInfo)
+{
+    //u16_t  broadcast[3] = {0xffff, 0xffff, 0xffff};
+    //u32_t* key;
+    u8_t   micKey[16];
+
+    zmw_get_wlan_dev(dev);
+
+    switch (keyInfo.keyLength)
+    {
+        case 5:
+            wd->sta.encryMode = ZM_WEP64;
+            /* use default key */
+            zfCoreSetKey(dev, 64, 0, ZM_WEP64, (u16_t *)keyInfo.macAddr, (u32_t*) keyInfo.key);
+		          break;
+
+       	case 13:
+            wd->sta.encryMode = ZM_WEP128;
+            /* use default key */
+            zfCoreSetKey(dev, 64, 0, ZM_WEP128, (u16_t *)keyInfo.macAddr, (u32_t*) keyInfo.key);
+          		break;
+
+       	case 29:
+            wd->sta.encryMode = ZM_WEP256;
+            /* use default key */
+            zfCoreSetKey(dev, 64, 1, ZM_WEP256,  (u16_t *)keyInfo.macAddr, (u32_t*) (&keyInfo.key[16]));
+            zfCoreSetKey(dev, 64, 0, ZM_WEP256, (u16_t *)keyInfo.macAddr, (u32_t*) keyInfo.key);
+		          break;
+
+       	case 16:
+            wd->sta.encryMode = ZM_AES;
+            //zfCoreSetKey(dev, 0, 0, ZM_AES, (u16_t *)keyInfo.macAddr, (u32_t*) keyInfo.key);
+            zfCoreSetKey(dev, 64, 0, ZM_AES, (u16_t *)keyInfo.macAddr, (u32_t*) keyInfo.key);
+            break;
+
+       	case 32:
+#ifdef ZM_ENABLE_CENC
+            if (keyInfo.flag & ZM_KEY_FLAG_CENC)
+            {
+                u16_t boardcastAddr[3] = {0xffff, 0xffff, 0xffff};
+                u16_t Addr_a[] = { 0x0000, 0x0080, 0x0901};
+                u16_t Addr_b[] = { 0x0000, 0x0080, 0x0902};
+                /* CENC test: user0,1 and user2 for boardcast */
+                wd->sta.encryMode = ZM_CENC;
+                zfCoreSetKey(dev, 0, 1, ZM_CENC, (u16_t *)Addr_a, (u32_t*) (&keyInfo.key[16]));
+                zfCoreSetKey(dev, 0, 0, ZM_CENC, (u16_t *)Addr_a, (u32_t*) keyInfo.key);
+
+                zfCoreSetKey(dev, 1, 1, ZM_CENC, (u16_t *)Addr_b, (u32_t*) (&keyInfo.key[16]));
+                zfCoreSetKey(dev, 1, 0, ZM_CENC, (u16_t *)Addr_b, (u32_t*) keyInfo.key);
+
+                zfCoreSetKey(dev, 2, 1, ZM_CENC, (u16_t *)boardcastAddr, (u32_t*) (&keyInfo.key[16]));
+                zfCoreSetKey(dev, 2, 0, ZM_CENC, (u16_t *)boardcastAddr, (u32_t*) keyInfo.key);
+
+                /* Initialize PN sequence */
+                wd->sta.txiv[0] = 0x5c365c36;
+                wd->sta.txiv[1] = 0x5c365c36;
+                wd->sta.txiv[2] = 0x5c365c36;
+                wd->sta.txiv[3] = 0x5c365c36;
+            }
+            else
+#endif //ZM_ENABLE_CENC
+            {
+                wd->sta.encryMode = ZM_TKIP;
+                zfCoreSetKey(dev, 64, 1, ZM_TKIP, (u16_t *)keyInfo.macAddr, (u32_t*) micKey);
+                zfCoreSetKey(dev, 64, 0, ZM_TKIP, (u16_t *)keyInfo.macAddr, (u32_t*) keyInfo.key);
+            }
+            break;
+        default:
+            wd->sta.encryMode = ZM_NO_WEP;
+    }
+
+    return ZM_STATUS_SUCCESS;
+}
+
+void zfiWlanSetPowerSaveMode(zdev_t* dev, u8_t mode)
+{
+#if 0
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.powerSaveMode = mode;
+
+    /* send null data with PwrBit to inform AP */
+    if ( mode > ZM_STA_PS_NONE )
+    {
+        if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+        {
+            zfSendNullData(dev, 1);
+        }
+
+        /* device into PS mode */
+        zfPSDeviceSleep(dev);
+    }
+#endif
+
+    zfPowerSavingMgrSetMode(dev, mode);
+}
+
+void zfiWlanSetMacAddress(zdev_t* dev, u16_t* mac)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->macAddr[0] = mac[0];
+    wd->macAddr[1] = mac[1];
+    wd->macAddr[2] = mac[2];
+
+    zfHpSetMacAddress(dev, mac, 0);
+}
+
+u8_t zfiWlanQueryWlanMode(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->wlanMode;
+}
+
+u8_t zfiWlanQueryAdapterState(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->state;
+}
+
+u8_t zfiWlanQueryAuthenticationMode(zdev_t* dev, u8_t bWrapper)
+{
+    u8_t   authMode;
+
+    zmw_get_wlan_dev(dev);
+
+    if ( bWrapper )
+    {
+        authMode = wd->ws.authMode;
+    }
+    else
+    {
+        //authMode = wd->sta.authMode;
+        authMode = wd->sta.currentAuthMode;
+    }
+
+    return authMode;
+}
+
+u8_t zfiWlanQueryWepStatus(zdev_t* dev, u8_t bWrapper)
+{
+    u8_t wepStatus;
+
+    zmw_get_wlan_dev(dev);
+
+    if ( bWrapper )
+    {
+        wepStatus = wd->ws.wepStatus;
+    }
+    else
+    {
+        wepStatus = wd->sta.wepStatus;
+    }
+
+    return wepStatus;
+}
+
+void zfiWlanQuerySSID(zdev_t* dev, u8_t* ssid, u8_t* pSsidLength)
+{
+    u16_t vapId = 0;
+    zmw_get_wlan_dev(dev);
+
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        vapId = zfwGetVapId(dev);
+
+        if (vapId == 0xffff)
+        {
+            *pSsidLength = wd->ap.ssidLen[0];
+            zfMemoryCopy(ssid, wd->ap.ssid[0], wd->ap.ssidLen[0]);
+        }
+        else
+        {
+            *pSsidLength = wd->ap.ssidLen[vapId + 1];
+            zfMemoryCopy(ssid, wd->ap.ssid[vapId + 1], wd->ap.ssidLen[vapId + 1]);
+        }
+    }
+    else
+    {
+        *pSsidLength = wd->sta.ssidLen;
+        zfMemoryCopy(ssid, wd->sta.ssid, wd->sta.ssidLen);
+    }
+}
+
+u16_t zfiWlanQueryFragThreshold(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->fragThreshold;
+}
+
+u16_t zfiWlanQueryRtsThreshold(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->rtsThreshold;
+}
+
+u32_t zfiWlanQueryFrequency(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return (wd->frequency*1000);
+}
+
+/***********************************************************
+ * Function: zfiWlanQueryCurrentFrequency
+ * Return value:
+ *   -   0 : no validate current frequency
+ *   - (>0): current frequency depend on "qmode"
+ * Input:
+ *   - qmode:
+ *      0: return value depend on the support mode, this
+           qmode is use to solve the bug #31223
+ *      1: return the actually current frequency
+ ***********************************************************/
+u32_t zfiWlanQueryCurrentFrequency(zdev_t* dev, u8_t qmode)
+{
+    u32_t frequency;
+
+    zmw_get_wlan_dev(dev);
+
+    switch (qmode)
+    {
+    case 0:
+        if (wd->sta.currentFrequency > 3000)
+        {
+            if (wd->supportMode & ZM_WIRELESS_MODE_5)
+            {
+                frequency = wd->sta.currentFrequency;
+            }
+            else if (wd->supportMode & ZM_WIRELESS_MODE_24)
+            {
+                frequency = zfChGetFirst2GhzChannel(dev);
+            }
+            else
+            {
+                frequency = 0;
+            }
+        }
+        else
+        {
+            if (wd->supportMode & ZM_WIRELESS_MODE_24)
+            {
+                frequency = wd->sta.currentFrequency;
+            }
+            else if (wd->supportMode & ZM_WIRELESS_MODE_5)
+            {
+                frequency = zfChGetLast5GhzChannel(dev);
+            }
+            else
+            {
+                frequency = 0;
+            }
+        }
+        break;
+
+    case 1:
+        frequency = wd->sta.currentFrequency;
+        break;
+
+    default:
+        frequency = 0;
+    }
+
+    return (frequency*1000);
+}
+
+u32_t zfiWlanQueryFrequencyAttribute(zdev_t* dev, u32_t freq)
+{
+    zmw_get_wlan_dev(dev);
+
+    u8_t  i;
+    u16_t frequency = (u16_t) (freq/1000);
+    u32_t ret = 0;
+
+    for (i = 0; i < wd->regulationTable.allowChannelCnt; i++)
+    {
+        if ( wd->regulationTable.allowChannel[i].channel == frequency )
+        {
+            ret = wd->regulationTable.allowChannel[i].channelFlags;
+        }
+    }
+
+    return ret;
+}
+
+/* BandWidth  0=>20  1=>40 */
+/* ExtOffset  0=>20  1=>high control 40   3=>low control 40 */
+void zfiWlanQueryFrequencyHT(zdev_t* dev, u32_t *bandWidth, u32_t *extOffset)
+{
+    zmw_get_wlan_dev(dev);
+
+    *bandWidth = wd->BandWidth40;
+    *extOffset = wd->ExtOffset;
+}
+
+u8_t zfiWlanQueryCWMode(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->cwm.cw_mode;
+}
+
+u32_t zfiWlanQueryCWEnable(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->cwm.cw_enable;
+}
+
+void zfiWlanQueryBssid(zdev_t* dev, u8_t* bssid)
+{
+    u8_t   addr[6];
+
+    zmw_get_wlan_dev(dev);
+
+    ZM_MAC_WORD_TO_BYTE(wd->sta.bssid, addr);
+    zfMemoryCopy(bssid, addr, 6);
+}
+
+u16_t zfiWlanQueryBeaconInterval(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->beaconInterval;
+}
+
+u32_t zfiWlanQueryRxBeaconTotal(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    wd->sta.rxBeaconTotal += wd->sta.rxBeaconCount;
+
+    return wd->sta.rxBeaconTotal;
+}
+
+u16_t zfiWlanQueryAtimWindow(zdev_t* dev)
+{
+    u16_t atimWindow;
+
+    zmw_get_wlan_dev(dev);
+
+    atimWindow = wd->sta.atimWindow;
+
+    return atimWindow;
+}
+
+u8_t zfiWlanQueryEncryMode(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if (wd->wlanMode == ZM_MODE_AP)
+        return wd->ap.encryMode[0];
+    else
+        return wd->sta.encryMode;
+}
+
+u16_t zfiWlanQueryCapability(zdev_t* dev)
+{
+    u16_t capability;
+
+    zmw_get_wlan_dev(dev);
+
+    capability = wd->sta.capability[0] +
+                 (((u16_t) wd->sta.capability[1]) << 8);
+
+    return capability;
+
+}
+
+u16_t zfiWlanQueryAid(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->sta.aid;
+}
+
+void zfiWlanQuerySupportRate(zdev_t* dev, u8_t* rateArray, u8_t* pLength)
+{
+    u8_t   i, j=0;
+
+    zmw_get_wlan_dev(dev);
+
+    for( i=0; i<4; i++ )
+    {
+        if ( wd->bRate & (0x1 << i) )
+        {
+            rateArray[j] = zg11bRateTbl[i] +
+                           ((wd->bRateBasic & (0x1<<i))<<(7-i));
+            j++;
+        }
+    }
+
+    *pLength = j;
+}
+
+void zfiWlanQueryExtSupportRate(zdev_t* dev, u8_t* rateArray, u8_t* pLength)
+{
+    u8_t   i, j=0;
+
+    zmw_get_wlan_dev(dev);
+
+    for( i=0; i<8; i++ )
+    {
+        if ( wd->gRate & (0x1 << i) )
+        {
+            rateArray[j] = zg11gRateTbl[i] +
+                           ((wd->gRateBasic & (0x1<<i))<<(7-i));
+            j++;
+        }
+    }
+
+    *pLength = j;
+}
+
+void zfiWlanQueryRsnIe(zdev_t* dev, u8_t* ie, u8_t* pLength)
+{
+    u8_t len;
+
+    zmw_get_wlan_dev(dev);
+
+    len = wd->sta.rsnIe[1] + 2;
+    zfMemoryCopy(ie, wd->sta.rsnIe, len);
+    *pLength = len;
+}
+
+void zfiWlanQueryWpaIe(zdev_t* dev, u8_t* ie, u8_t* pLength)
+{
+    u8_t len;
+
+    zmw_get_wlan_dev(dev);
+
+    len = wd->sta.wpaIe[1] + 2;
+    zfMemoryCopy(ie, wd->sta.wpaIe, len);
+    *pLength = len;
+
+}
+
+u8_t zfiWlanQueryMulticastCipherAlgo(zdev_t *dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    switch( wd->sta.currentAuthMode )
+    {
+        case ZM_AUTH_MODE_WPA2PSK:
+        case ZM_AUTH_MODE_WPA2:
+            if ( wd->sta.rsnIe[7] == 2 )
+            {
+                return ZM_TKIP;
+            }
+            else
+            {
+                return ZM_AES;
+            }
+            break;
+
+        case ZM_AUTH_MODE_WPAPSK:
+        case ZM_AUTH_MODE_WPA:
+            if ( wd->sta.rsnIe[11] == 2 )
+            {
+                return ZM_TKIP;
+            }
+            else
+            {
+                return ZM_AES;
+            }
+            break;
+
+        default:
+            return wd->sta.encryMode;
+    }
+}
+
+u8_t zfiWlanQueryHTMode(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    // 0:Legancy, 1:N
+    return wd->sta.EnableHT;
+}
+
+u8_t zfiWlanQueryBandWidth40(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    // 0:20M, 1:40M
+    return wd->BandWidth40;
+}
+
+u16_t zfiWlanQueryRegionCode(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->regulationTable.regionCode;
+}
+void zfiWlanSetWpaIe(zdev_t* dev, u8_t* ie, u8_t Length)
+{
+    u16_t vapId = 0;
+    zmw_get_wlan_dev(dev);
+
+    if (wd->wlanMode == ZM_MODE_AP) // AP Mode
+    {
+        vapId = zfwGetVapId(dev);
+
+        if (vapId == 0xffff)
+            vapId = 0;
+        else
+            vapId++;
+
+        zm_assert(Length < ZM_MAX_WPAIE_SIZE);
+        if (Length < ZM_MAX_WPAIE_SIZE)
+        {
+            wd->ap.wpaLen[vapId] = Length;
+            zfMemoryCopy(wd->ap.wpaIe[vapId], ie, wd->ap.wpaLen[vapId]);
+        }
+
+    }
+    else
+    {
+        wd->sta.wpaLen = Length;
+        zfMemoryCopy(wd->sta.wpaIe, ie, wd->sta.wpaLen);
+    }
+    //zfiWlanSetWpaSupport(dev, 1);
+    if (wd->wlanMode == ZM_MODE_AP) // AP Mode
+    {
+        wd->ap.wpaSupport[vapId] = 1;
+    }
+    else
+    {
+        wd->sta.wpaSupport = 1;
+    }
+
+}
+
+void zfiWlanSetWpaSupport(zdev_t* dev, u8_t WpaSupport)
+{
+    u16_t vapId = 0;
+    zmw_get_wlan_dev(dev);
+
+    if (wd->wlanMode == ZM_MODE_AP) // AP Mode
+    {
+        vapId = zfwGetVapId(dev);
+
+        if (vapId == 0xffff)
+            vapId = 0;
+        else
+            vapId++;
+
+        wd->ap.wpaSupport[vapId] = WpaSupport;
+    }
+    else
+    {
+        wd->sta.wpaSupport = WpaSupport;
+    }
+
+}
+
+void zfiWlanSetProtectionMode(zdev_t* dev, u8_t mode)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.bProtectionMode = mode;
+    if (wd->sta.bProtectionMode == TRUE)
+    {
+        zfHpSetSlotTime(dev, 0);
+    }
+    else
+    {
+        zfHpSetSlotTime(dev, 1);
+    }
+
+    zm_msg1_mm(ZM_LV_1, "wd->protectionMode=", wd->sta.bProtectionMode);
+}
+
+void zfiWlanSetBasicRate(zdev_t* dev, u8_t bRateSet, u8_t gRateSet,
+                         u32_t nRateSet)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->ws.bRateBasic = bRateSet;
+    wd->ws.gRateBasic = gRateSet;
+    wd->ws.nRateBasic = nRateSet;
+}
+
+void zfiWlanSetBGMode(zdev_t* dev, u8_t mode)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->ws.bgMode = mode;
+}
+
+void zfiWlanSetpreambleType(zdev_t* dev, u8_t type)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->ws.preambleType = type;
+}
+
+u8_t zfiWlanQuerypreambleType(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->ws.preambleType;
+}
+
+u8_t zfiWlanQueryPowerSaveMode(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->sta.powerSaveMode;
+}
+
+u8_t zfiWlanSetPmkidInfo(zdev_t* dev, u16_t* bssid, u8_t* pmkid)
+{
+    u32_t  i;
+
+    zmw_get_wlan_dev(dev);
+
+    for(i=0; i<wd->sta.pmkidInfo.bssidCount; i++)
+    {
+        if ( zfMemoryIsEqual((u8_t*) wd->sta.pmkidInfo.bssidInfo[i].bssid,
+                             (u8_t*) bssid, 6) )
+        {
+            /* matched */
+            break;
+        }
+    }
+
+    if ( i < wd->sta.pmkidInfo.bssidCount )
+    {
+        /* overwrite the original one */
+        zfMemoryCopy(wd->sta.pmkidInfo.bssidInfo[i].pmkid, pmkid, 16);
+    }
+    else
+    {
+        if ( i < ZM_PMKID_MAX_BSS_CNT )
+        {
+            wd->sta.pmkidInfo.bssidInfo[i].bssid[0] = bssid[0];
+            wd->sta.pmkidInfo.bssidInfo[i].bssid[1] = bssid[1];
+            wd->sta.pmkidInfo.bssidInfo[i].bssid[2] = bssid[2];
+
+            zfMemoryCopy(wd->sta.pmkidInfo.bssidInfo[i].pmkid, pmkid, 16);
+            wd->sta.pmkidInfo.bssidCount++;
+        }
+    }
+
+    return 0;
+}
+
+u32_t zfiWlanQueryPmkidInfo(zdev_t* dev, u8_t* buf, u32_t len)
+{
+    //struct zsPmkidInfo* pPmkidInfo = ( struct zsPmkidInfo* ) buf;
+    u32_t  size;
+
+    zmw_get_wlan_dev(dev);
+
+    size = sizeof(u32_t) +
+           wd->sta.pmkidInfo.bssidCount * sizeof(struct zsPmkidBssidInfo);
+
+    if ( len < size )
+    {
+        return wd->sta.pmkidInfo.bssidCount;
+    }
+
+    zfMemoryCopy(buf, (u8_t*) &wd->sta.pmkidInfo, (u16_t) size);
+
+    return 0;
+}
+
+void zfiWlanSetMulticastList(zdev_t* dev, u8_t size, u8_t* pList)
+{
+    struct zsMulticastAddr* pMacList = (struct zsMulticastAddr*) pList;
+    u8_t   i;
+    u8_t   bAllMulticast = 0;
+    //u32_t  value;
+
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.multicastList.size = size;
+    for(i=0; i<size; i++)
+    {
+        zfMemoryCopy(wd->sta.multicastList.macAddr[i].addr,
+                     pMacList[i].addr, 6);
+    }
+
+    if ( wd->sta.osRxFilter & ZM_PACKET_TYPE_ALL_MULTICAST )
+        bAllMulticast = 1;
+    zfHpSetMulticastList(dev, size, pList, bAllMulticast);
+
+}
+
+void zfiWlanRemoveKey(zdev_t* dev, u8_t keyType, u8_t keyId)
+{
+    u16_t  fakeMacAddr[3] = {0, 0, 0};
+    u32_t  fakeKey[4] = {0, 0, 0, 0};
+
+    zmw_get_wlan_dev(dev);
+
+    if ( keyType == 0 )
+    {
+        /* remove WEP key */
+        zm_debug_msg0("remove WEP key");
+        zfCoreSetKey(dev, ZM_USER_KEY_DEFAULT+keyId, 0,
+                 ZM_NO_WEP, fakeMacAddr, fakeKey);
+        wd->sta.encryMode = ZM_NO_WEP;
+    }
+    else if ( keyType == 1 )
+    {
+        /* remove pairwise key */
+        zm_debug_msg0("remove pairwise key");
+        zfHpRemoveKey(dev, ZM_USER_KEY_PK);
+        wd->sta.encryMode = ZM_NO_WEP;
+    }
+    else
+    {
+        /* remove group key */
+        zm_debug_msg0("remove group key");
+        zfHpRemoveKey(dev, ZM_USER_KEY_GK);
+    }
+}
+
+
+void zfiWlanQueryRegulationTable(zdev_t* dev, struct zsRegulationTable* pEntry)
+{
+    zmw_get_wlan_dev(dev);
+
+    zfMemoryCopy((u8_t*) pEntry, (u8_t*) &wd->regulationTable,
+                 sizeof(struct zsRegulationTable));
+}
+
+/* parameter "time" is specified in ms */
+void zfiWlanSetScanTimerPerChannel(zdev_t* dev, u16_t time)
+{
+    zmw_get_wlan_dev(dev);
+
+    zm_debug_msg1("scan time (ms) = ", time);
+
+    wd->sta.activescanTickPerChannel = time / ZM_MS_PER_TICK;
+}
+
+void zfiWlanSetAutoReconnect(zdev_t* dev, u8_t enable)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.bAutoReconnect = enable;
+    //wd->sta.bAutoReconnectEnabled = enable;
+}
+
+void zfiWlanSetStaWme(zdev_t* dev, u8_t enable, u8_t uapsdInfo)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->ws.staWmeEnabled = enable & 0x3;
+    if ((enable & 0x2) != 0)
+    {
+        wd->ws.staWmeQosInfo = uapsdInfo & 0x6f;
+    }
+    else
+    {
+        wd->ws.staWmeQosInfo = 0;
+    }
+}
+
+void zfiWlanSetApWme(zdev_t* dev, u8_t enable)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->ws.apWmeEnabled = enable;
+}
+
+u8_t zfiWlanQuerywmeEnable(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->ws.staWmeEnabled;
+}
+
+void zfiWlanSetProbingHiddenSsid(zdev_t* dev, u8_t* ssid, u8_t ssidLen,
+    u16_t entry)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+
+    if ((ssidLen <= 32) && (entry < ZM_MAX_PROBE_HIDDEN_SSID_SIZE))
+    {
+        zmw_enter_critical_section(dev);
+        wd->ws.probingSsidList[entry].ssidLen = ssidLen;
+        zfMemoryCopy(wd->ws.probingSsidList[entry].ssid, ssid, ssidLen);
+        zmw_leave_critical_section(dev);
+    }
+
+    return;
+}
+
+void zfiWlanSetDisableProbingWithSsid(zdev_t* dev, u8_t mode)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.disableProbingWithSsid = mode;
+
+    return;
+}
+
+void zfiWlanSetDropUnencryptedPackets(zdev_t* dev, u8_t enable)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->ws.dropUnencryptedPkts = enable;
+}
+
+void zfiWlanSetStaRxSecurityCheckCb(zdev_t* dev, zfpStaRxSecurityCheckCb pStaRxSecurityCheckCb)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.pStaRxSecurityCheckCb = pStaRxSecurityCheckCb;
+}
+
+void zfiWlanSetIBSSJoinOnly(zdev_t* dev, u8_t joinOnly)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->ws.ibssJoinOnly = joinOnly;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiConfigWdsPort            */
+/*      Configure WDS port.                                             */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      wdsPortId : WDS port ID, start from 0                           */
+/*      flag : 0=>disable WDS port, 1=>enable WDS port                  */
+/*      wdsAddr : WDS neighbor MAC address                              */
+/*      encType : encryption type for WDS port                          */
+/*      wdsKey : encryption key for WDS port                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      Error code                                                      */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2006.6      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfiConfigWdsPort(zdev_t* dev, u8_t wdsPortId, u16_t flag, u16_t* wdsAddr,
+        u16_t encType, u32_t* wdsKey)
+{
+    u16_t addr[3];
+    u32_t key[4];
+
+    zmw_get_wlan_dev(dev);
+
+    if (wdsPortId > ZM_MAX_WDS_SUPPORT)
+    {
+        return ZM_ERR_WDS_PORT_ID;
+    }
+
+    if (flag == 1)
+    {
+        /* Enable WDS port */
+        wd->ap.wds.macAddr[wdsPortId][0] = wdsAddr[0];
+        wd->ap.wds.macAddr[wdsPortId][1] = wdsAddr[1];
+        wd->ap.wds.macAddr[wdsPortId][2] = wdsAddr[2];
+
+        wd->ap.wds.wdsBitmap |= (1 << wdsPortId);
+        wd->ap.wds.encryMode[wdsPortId] = (u8_t) encType;
+
+        zfCoreSetKey(dev, 10+ZM_MAX_WDS_SUPPORT, 0, (u8_t) encType, wdsAddr, wdsKey);
+    }
+    else
+    {
+        /* Disable WDS port */
+        addr[0] = addr[1] = addr[2] = 0;
+        key[0] = key[1] = key[2] = key[3] = 0;
+        wd->ap.wds.wdsBitmap &= (~(1 << wdsPortId));
+        zfCoreSetKey(dev, 10+ZM_MAX_WDS_SUPPORT, 0, ZM_NO_WEP, addr, key);
+    }
+
+    return ZM_SUCCESS;
+}
+#ifdef ZM_ENABLE_CENC
+/* CENC */
+void zfiWlanQueryGSN(zdev_t* dev, u8_t *gsn, u16_t vapId)
+{
+    //struct zsWlanDev* wd = (struct zsWlanDev*) zmw_wlan_dev(dev);
+    u32_t txiv[4];
+    zmw_get_wlan_dev(dev);
+
+    /* convert little endian to big endian for 32 bits */
+    txiv[3] = wd->ap.txiv[vapId][0];
+    txiv[2] = wd->ap.txiv[vapId][1];
+    txiv[1] = wd->ap.txiv[vapId][2];
+    txiv[0] = wd->ap.txiv[vapId][3];
+
+    zfMemoryCopy(gsn, (u8_t*)txiv, 16);
+}
+#endif //ZM_ENABLE_CENC
+//CWYang(+)
+void zfiWlanQuerySignalInfo(zdev_t* dev, u8_t *buffer)
+{
+    zmw_get_wlan_dev(dev);
+
+    /*Change Signal Strength/Quality Value to Human Sense Here*/
+
+    buffer[0] = wd->SignalStrength;
+    buffer[1] = wd->SignalQuality;
+}
+
+/* OS-XP */
+u16_t zfiStaAddIeWpaRsn(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t frameType)
+{
+    return  zfStaAddIeWpaRsn(dev, buf, offset, frameType);
+}
+
+/* zfiDebugCmd                                                                        */
+/*     cmd       value-description                                                  */
+/*         0       schedule timer                                                     */
+/*         1       cancel timer                                                         */
+/*         2       clear timer                                                           */
+/*         3       test timer                                                            */
+/*         4                                                                                 */
+/*         5                                                                                 */
+/*         6       checksum test     0/1                                           */
+/*         7       enableProtectionMode                                          */
+/*         8       rx packet content dump    0/1                               */
+
+u32_t zfiDebugCmd(zdev_t* dev, u32_t cmd, u32_t value)
+{
+    u16_t event;
+    u32_t tick;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+
+    zmw_enter_critical_section(dev);
+
+    if ( cmd == 0 )
+    {   /* schedule timer */
+        event = (u16_t) ((value >> 16) & 0xffff);
+        tick = value & 0xffff;
+        zfTimerSchedule(dev, event, tick);
+    }
+    else if ( cmd == 1 )
+    {   /* cancel timer */
+        event = (u16_t) (value & 0xffff);
+        zfTimerCancel(dev, event);
+    }
+    else if ( cmd == 2 )
+    {   /* clear timer */
+        zfTimerClear(dev);
+    }
+    else if ( cmd == 3 )
+    {   /* test timer */
+        zfTimerSchedule(dev, 1,  500);
+        zfTimerSchedule(dev, 2, 1000);
+        zfTimerSchedule(dev, 3, 1000);
+        zfTimerSchedule(dev, 4, 1000);
+        zfTimerSchedule(dev, 5, 1500);
+        zfTimerSchedule(dev, 6, 2000);
+        zfTimerSchedule(dev, 7, 2200);
+        zfTimerSchedule(dev, 6, 2500);
+        zfTimerSchedule(dev, 8, 2800);
+    }
+    else if ( cmd == 4)
+    {
+        zfTimerSchedule(dev, 1,  500);
+        zfTimerSchedule(dev, 2, 1000);
+        zfTimerSchedule(dev, 3, 1000);
+        zfTimerSchedule(dev, 4, 1000);
+        zfTimerSchedule(dev, 5, 1500);
+        zfTimerSchedule(dev, 6, 2000);
+        zfTimerSchedule(dev, 7, 2200);
+        zfTimerSchedule(dev, 6, 2500);
+        zfTimerSchedule(dev, 8, 2800);
+        zfTimerCancel(dev, 1);
+        zfTimerCancel(dev, 3);
+        zfTimerCancel(dev, 6);
+    }
+    else if ( cmd == 5 )
+    {
+        wd->sta.keyId = (u8_t) value;
+    }
+	else if ( cmd == 6 )
+	{
+	    /* 0: normal    1: always set TCP/UDP checksum zero */
+        wd->checksumTest = value;
+	}
+	else if ( cmd == 7 )
+	{
+        wd->enableProtectionMode = value;
+   	    zm_msg1_mm(ZM_LV_1, "wd->enableProtectionMode=", wd->enableProtectionMode);
+	}
+	else if ( cmd == 8 )
+	{
+        /* rx packet content dump */
+        if (value)
+        {
+            wd->rxPacketDump = 1;
+        }
+        else
+        {
+            wd->rxPacketDump = 0;
+        }
+	}
+
+
+    zmw_leave_critical_section(dev);
+
+    return 0;
+}
+
+#ifdef ZM_ENABLE_CENC
+u8_t zfiWlanSetCencPairwiseKey(zdev_t* dev, u8_t keyid, u32_t *txiv, u32_t *rxiv,
+        u8_t *key, u8_t *mic)
+{
+    struct zsKeyInfo keyInfo;
+    u8_t cencKey[32];
+    u8_t i;
+    u16_t macAddr[3];
+
+    zmw_get_wlan_dev(dev);
+
+    for (i = 0; i < 16; i++)
+        cencKey[i] = key[i];
+    for (i = 0; i < 16; i++)
+        cencKey[i + 16] = mic[i];
+    keyInfo.key = cencKey;
+    keyInfo.keyLength = 32;
+    keyInfo.keyIndex = keyid;
+    keyInfo.flag = ZM_KEY_FLAG_CENC | ZM_KEY_FLAG_PK;
+    for (i = 0; i < 3; i++)
+        macAddr[i] = wd->sta.bssid[i];
+    keyInfo.macAddr = macAddr;
+
+    zfiWlanSetKey(dev, keyInfo);
+
+    /* Reset txiv and rxiv */
+    //wd->sta.txiv[0] = txiv[0];
+    //wd->sta.txiv[1] = txiv[1];
+    //wd->sta.txiv[2] = txiv[2];
+    //wd->sta.txiv[3] = txiv[3];
+    //
+    //wd->sta.rxiv[0] = rxiv[0];
+    //wd->sta.rxiv[1] = rxiv[1];
+    //wd->sta.rxiv[2] = rxiv[2];
+    //wd->sta.rxiv[3] = rxiv[3];
+
+    return 0;
+}
+
+u8_t zfiWlanSetCencGroupKey(zdev_t* dev, u8_t keyid, u32_t *rxiv,
+        u8_t *key, u8_t *mic)
+{
+    struct zsKeyInfo keyInfo;
+    u8_t cencKey[32];
+    u8_t i;
+    u16_t macAddr[6] = {0xffff, 0xffff, 0xffff};
+
+    zmw_get_wlan_dev(dev);
+
+    for (i = 0; i < 16; i++)
+        cencKey[i] = key[i];
+    for (i = 0; i < 16; i++)
+        cencKey[i + 16] = mic[i];
+    keyInfo.key = cencKey;
+    keyInfo.keyLength = 32;
+    keyInfo.keyIndex = keyid;
+    keyInfo.flag = ZM_KEY_FLAG_CENC | ZM_KEY_FLAG_GK;
+    keyInfo.vapId = 0;
+    for (i = 0; i < 3; i++)
+        keyInfo.vapAddr[i] = wd->macAddr[i];
+    keyInfo.macAddr = macAddr;
+
+    zfiWlanSetKey(dev, keyInfo);
+
+    /* Reset txiv and rxiv */
+    wd->sta.rxivGK[0] = ((rxiv[3] >> 24) & 0xFF)
+                      + (((rxiv[3] >> 16) & 0xFF) << 8)
+                      + (((rxiv[3] >> 8) & 0xFF) << 16)
+                      + ((rxiv[3] & 0xFF) << 24);
+    wd->sta.rxivGK[1] = ((rxiv[2] >> 24) & 0xFF)
+                      + (((rxiv[2] >> 16) & 0xFF) << 8)
+                      + (((rxiv[2] >> 8) & 0xFF) << 16)
+                      + ((rxiv[2] & 0xFF) << 24);
+    wd->sta.rxivGK[2] = ((rxiv[1] >> 24) & 0xFF)
+                      + (((rxiv[1] >> 16) & 0xFF) << 8)
+                      + (((rxiv[1] >> 8) & 0xFF) << 16)
+                      + ((rxiv[1] & 0xFF) << 24);
+    wd->sta.rxivGK[3] = ((rxiv[0] >> 24) & 0xFF)
+                      + (((rxiv[0] >> 16) & 0xFF) << 8)
+                      + (((rxiv[0] >> 8) & 0xFF) << 16)
+                      + ((rxiv[0] & 0xFF) << 24);
+
+    wd->sta.authMode = ZM_AUTH_MODE_CENC;
+    wd->sta.currentAuthMode = ZM_AUTH_MODE_CENC;
+
+    return 0;
+}
+#endif //ZM_ENABLE_CENC
+
+u8_t zfiWlanSetDot11DMode(zdev_t* dev, u8_t mode)
+{
+    u8_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.b802_11D = mode;
+    if (mode) //Enable 802.11d
+    {
+        wd->regulationTable.regionCode = NO_ENUMRD;
+        for (i = 0; i < wd->regulationTable.allowChannelCnt; i++)
+            wd->regulationTable.allowChannel[i].channelFlags |= ZM_REG_FLAG_CHANNEL_PASSIVE;
+    }
+    else //Disable
+    {
+        for (i = 0; i < wd->regulationTable.allowChannelCnt; i++)
+            wd->regulationTable.allowChannel[i].channelFlags &= ~ZM_REG_FLAG_CHANNEL_PASSIVE;
+    }
+
+    return 0;
+}
+
+u8_t zfiWlanSetDot11HDFSMode(zdev_t* dev, u8_t mode)
+{
+    zmw_get_wlan_dev(dev);
+
+    //zm_debug_msg0("CWY - Enable 802.11h DFS");
+
+    // TODO : DFS Enable in 5250 to 5350 MHz and 5470 to 5725 MHz .
+    //if ( Adapter->ZD80211HSupport &&
+    //   Adapter->CardSetting.NetworkTypeInUse == Ndis802_11OFDM5 &&
+    //   ((ChannelNo >=52 && ChannelNo <= 64)	||				//5250~5350 MHZ
+    //    (ChannelNo >=100 && ChannelNo <= 140))) 			//5470~5725 MHZ
+    //{
+    //   Adapter->ZD80211HSetting.DFSEnable=TRUE;
+    //}
+    //else
+    //{
+    //   Adapter->ZD80211HSetting.DFSEnable=FALSE;
+    //}
+
+    wd->sta.DFSEnable = mode;
+    if (mode)
+        wd->sta.capability[1] |= ZM_BIT_0;
+    else
+        wd->sta.capability[1] &= (~ZM_BIT_0);
+
+    return 0;
+}
+
+u8_t zfiWlanSetDot11HTPCMode(zdev_t* dev, u8_t mode)
+{
+    zmw_get_wlan_dev(dev);
+
+    // TODO : TPC Enable in 5150~5350 MHz and 5470~5725MHz.
+    //if ( Adapter->ZD80211HSupport &&
+    //   Adapter->CardSetting.NetworkTypeInUse == Ndis802_11OFDM5 &&
+    //   ((ChannelNo == 36 || ChannelNo == 40 || ChannelNo == 44 || ChannelNo == 48) ||	//5150~5250 MHZ , Not Japan
+    //    (ChannelNo >=52 && ChannelNo <= 64) ||				//5250~5350 MHZ
+    //    (ChannelNo >=100 && ChannelNo <= 140))) 			//5470~5725 MHZ
+    //{
+    //   Adapter->ZD80211HSetting.TPCEnable=TRUE;
+    //}
+    //else
+    //{
+    //   Adapter->ZD80211HSetting.TPCEnable=FALSE;
+    //}
+
+    wd->sta.TPCEnable = mode;
+    if (mode)
+        wd->sta.capability[1] |= ZM_BIT_0;
+    else
+        wd->sta.capability[1] &= (~ZM_BIT_0);
+
+    return 0;
+}
+
+u8_t zfiWlanSetAniMode(zdev_t* dev, u8_t mode)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->aniEnable = mode;
+    if (mode)
+        zfHpAniAttach(dev);
+
+    return 0;
+}
+
+#ifdef ZM_OS_LINUX_FUNC
+void zfiWlanShowTally(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    zm_msg1_mm(ZM_LV_0, "Hw_UnderrunCnt    = ", wd->commTally.Hw_UnderrunCnt);
+    zm_msg1_mm(ZM_LV_0, "Hw_TotalRxFrm     = ", wd->commTally.Hw_TotalRxFrm);
+    zm_msg1_mm(ZM_LV_0, "Hw_CRC32Cnt       = ", wd->commTally.Hw_CRC32Cnt);
+    zm_msg1_mm(ZM_LV_0, "Hw_CRC16Cnt       = ", wd->commTally.Hw_CRC16Cnt);
+    zm_msg1_mm(ZM_LV_1, "Hw_DecrypErr_UNI  = ", wd->commTally.Hw_DecrypErr_UNI);
+    zm_msg1_mm(ZM_LV_0, "Hw_RxFIFOOverrun  = ", wd->commTally.Hw_RxFIFOOverrun);
+    zm_msg1_mm(ZM_LV_1, "Hw_DecrypErr_Mul  = ", wd->commTally.Hw_DecrypErr_Mul);
+    zm_msg1_mm(ZM_LV_1, "Hw_RetryCnt       = ", wd->commTally.Hw_RetryCnt);
+    zm_msg1_mm(ZM_LV_0, "Hw_TotalTxFrm     = ", wd->commTally.Hw_TotalTxFrm);
+    zm_msg1_mm(ZM_LV_0, "Hw_RxTimeOut      = ", wd->commTally.Hw_RxTimeOut);
+    zm_msg1_mm(ZM_LV_0, "Tx_MPDU           = ", wd->commTally.Tx_MPDU);
+    zm_msg1_mm(ZM_LV_0, "BA_Fail           = ", wd->commTally.BA_Fail);
+    zm_msg1_mm(ZM_LV_0, "Hw_Tx_AMPDU       = ", wd->commTally.Hw_Tx_AMPDU);
+    zm_msg1_mm(ZM_LV_0, "Hw_Tx_MPDU        = ", wd->commTally.Hw_Tx_MPDU);
+
+    zm_msg1_mm(ZM_LV_1, "Hw_RxMPDU          = ", wd->commTally.Hw_RxMPDU);
+    zm_msg1_mm(ZM_LV_1, "Hw_RxDropMPDU      = ", wd->commTally.Hw_RxDropMPDU);
+    zm_msg1_mm(ZM_LV_1, "Hw_RxDelMPDU       = ", wd->commTally.Hw_RxDelMPDU);
+    zm_msg1_mm(ZM_LV_1, "Hw_RxPhyMiscError  = ", wd->commTally.Hw_RxPhyMiscError);
+    zm_msg1_mm(ZM_LV_1, "Hw_RxPhyXRError    = ", wd->commTally.Hw_RxPhyXRError);
+    zm_msg1_mm(ZM_LV_1, "Hw_RxPhyOFDMError  = ", wd->commTally.Hw_RxPhyOFDMError);
+    zm_msg1_mm(ZM_LV_1, "Hw_RxPhyCCKError   = ", wd->commTally.Hw_RxPhyCCKError);
+    zm_msg1_mm(ZM_LV_1, "Hw_RxPhyHTError    = ", wd->commTally.Hw_RxPhyHTError);
+    zm_msg1_mm(ZM_LV_1, "Hw_RxPhyTotalCount = ", wd->commTally.Hw_RxPhyTotalCount);
+
+    if (!((wd->commTally.Tx_MPDU == 0) && (wd->commTally.BA_Fail == 0)))
+    {
+        zm_debug_msg_p("BA Fail Ratio(%)  = ", wd->commTally.BA_Fail * 100,
+                (wd->commTally.BA_Fail + wd->commTally.Tx_MPDU));
+    }
+
+    if (!((wd->commTally.Hw_Tx_MPDU == 0) && (wd->commTally.Hw_Tx_AMPDU == 0)))
+    {
+        zm_debug_msg_p("Avg Agg Number    = ",
+                wd->commTally.Hw_Tx_MPDU, wd->commTally.Hw_Tx_AMPDU);
+    }
+}
+#endif
+
+void zfiWlanSetMaxTxPower(zdev_t* dev, u8_t power2, u8_t power5)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    wd->maxTxPower2 = power2;
+    wd->maxTxPower5 = power5;
+    zmw_leave_critical_section(dev);
+}
+
+void zfiWlanQueryMaxTxPower(zdev_t* dev, u8_t *power2, u8_t *power5)
+{
+    zmw_get_wlan_dev(dev);
+
+    *power2 = wd->maxTxPower2;
+    *power5 = wd->maxTxPower5;
+}
+
+void zfiWlanSetConnectMode(zdev_t* dev, u8_t mode)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    wd->connectMode = mode;
+    zmw_leave_critical_section(dev);
+}
+
+void zfiWlanSetSupportMode(zdev_t* dev, u32_t mode)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    wd->supportMode = mode;
+    zmw_leave_critical_section(dev);
+}
+
+void zfiWlanSetAdhocMode(zdev_t* dev, u32_t mode)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->ws.adhocMode = mode;
+}
+
+u32_t zfiWlanQueryAdhocMode(zdev_t* dev, u8_t bWrapper)
+{
+    u32_t adhocMode;
+
+    zmw_get_wlan_dev(dev);
+
+    if ( bWrapper )
+    {
+        adhocMode = wd->ws.adhocMode;
+    }
+    else
+    {
+        adhocMode = wd->wfc.bIbssGMode;
+    }
+
+    return adhocMode;
+}
+
+
+u8_t zfiWlanSetCountryIsoName(zdev_t* dev, u8_t *countryIsoName, u8_t length)
+{
+    u8_t buf[5];
+    zmw_get_wlan_dev(dev);
+
+    if (length == 4)
+    {
+        buf[2] = wd->ws.countryIsoName[0] = countryIsoName[2];
+        buf[3] = wd->ws.countryIsoName[1] = countryIsoName[1];
+        buf[4] = wd->ws.countryIsoName[2] = countryIsoName[0];
+    }
+    else if (length == 3)
+    {
+        buf[2] = wd->ws.countryIsoName[0] = countryIsoName[1];
+        buf[3] = wd->ws.countryIsoName[1] = countryIsoName[0];
+        buf[4] = wd->ws.countryIsoName[2] = '\0';
+    }
+    else
+    {
+        return 1;
+    }
+
+    return zfHpGetRegulationTablefromISO(dev, buf, length);
+}
+
+
+const char* zfiWlanQueryCountryIsoName(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->ws.countryIsoName;
+}
+
+
+
+void zfiWlanSetRegulatory(zdev_t* dev, u8_t CCS, u16_t Code, u8_t bfirstChannel)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if (CCS)
+    {
+        /* Reset Regulation Table by Country Code */
+        zfHpGetRegulationTablefromCountry(dev, Code);
+    }
+    else
+    {
+        /* Reset Regulation Table by Region Code */
+        zfHpGetRegulationTablefromRegionCode(dev, Code);
+    }
+
+    if (bfirstChannel) {
+        zmw_enter_critical_section(dev);
+        wd->frequency = zfChGetFirstChannel(dev, NULL);
+        zmw_leave_critical_section(dev);
+        zfCoreSetFrequency(dev, wd->frequency);
+    }
+}
+
+
+const char* zfiHpGetisoNamefromregionCode(zdev_t* dev, u16_t regionCode)
+{
+    return zfHpGetisoNamefromregionCode(dev, regionCode);
+}
+
+u16_t zfiWlanChannelToFrequency(zdev_t* dev, u8_t channel)
+{
+    return zfChNumToFreq(dev, channel, 0);
+}
+
+u8_t zfiWlanFrequencyToChannel(zdev_t* dev, u16_t freq)
+{
+    u8_t is5GBand = 0;
+
+    return zfChFreqToNum(freq, &is5GBand);
+}
+
+void zfiWlanDisableDfsChannel(zdev_t* dev, u8_t disableFlag)
+{
+    zfHpDisableDfsChannel(dev, disableFlag);
+    return;
+}
+
+void zfiWlanSetLEDCtrlParam(zdev_t* dev, u8_t type, u8_t flag)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    wd->ledStruct.LEDCtrlType = type;
+    wd->ledStruct.LEDCtrlFlagFromReg  = flag;
+    zmw_leave_critical_section(dev);
+}
+
+void zfiWlanEnableLeapConfig(zdev_t* dev, u8_t leapEnabled)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.leapEnabled = leapEnabled;
+}
+
+u32_t zfiWlanQueryHwCapability(zdev_t* dev)
+{
+    return zfHpCapability(dev);
+}
+
+u32_t zfiWlanQueryReceivedPacket(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->sta.ReceivedPktRatePerSecond;
+}
+
+void zfiWlanCheckSWEncryption(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if (wd->sta.SWEncryptEnable != 0)
+    {
+        zfHpSWDecrypt(dev, 1);
+    }
+}
+
+u16_t zfiWlanQueryAllowChannels(zdev_t* dev, u16_t *channels)
+{
+    u16_t ii;
+    zmw_get_wlan_dev(dev);
+
+    for (ii = 0; ii < wd->regulationTable.allowChannelCnt; ii++)
+    {
+        channels[ii] = wd->regulationTable.allowChannel[ii].channel;
+    }
+
+    return wd->regulationTable.allowChannelCnt;
+}
+
+void zfiWlanSetDynamicSIFSParam(zdev_t* dev, u8_t val)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->dynamicSIFSEnable = val;
+
+    zm_debug_msg1("wd->dynamicSIFSEnable = ", wd->dynamicSIFSEnable)
+}
+
+u16_t zfiWlanGetMulticastAddressCount(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    return wd->sta.multicastList.size;
+}
+
+void zfiWlanGetMulticastList(zdev_t* dev, u8_t* pMCList)
+{
+    struct zsMulticastAddr* pMacList = (struct zsMulticastAddr*) pMCList;
+    u8_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    for ( i=0; i<wd->sta.multicastList.size; i++ )
+    {
+        zfMemoryCopy(pMacList[i].addr, wd->sta.multicastList.macAddr[i].addr, 6);
+    }
+}
+
+void zfiWlanSetPacketFilter(zdev_t* dev, u32_t PacketFilter)
+{
+    u8_t  bAllMulticast = 0;
+    u32_t oldFilter;
+
+    zmw_get_wlan_dev(dev);
+
+    oldFilter = wd->sta.osRxFilter;
+
+    wd->sta.osRxFilter = PacketFilter;
+
+    if ((oldFilter & ZM_PACKET_TYPE_ALL_MULTICAST) !=
+        (wd->sta.osRxFilter & ZM_PACKET_TYPE_ALL_MULTICAST))
+    {
+        if ( wd->sta.osRxFilter & ZM_PACKET_TYPE_ALL_MULTICAST )
+            bAllMulticast = 1;
+        zfHpSetMulticastList(dev, wd->sta.multicastList.size,
+                             (u8_t*)wd->sta.multicastList.macAddr, bAllMulticast);
+    }
+}
+
+u8_t zfiCompareWithMulticastListAddress(zdev_t* dev, u16_t* dstMacAddr)
+{
+    u8_t i;
+    u8_t bIsInMCListAddr = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    for ( i=0; i<wd->sta.multicastList.size; i++ )
+    {
+    	if ( zfwMemoryIsEqual((u8_t*)dstMacAddr, (u8_t*)wd->sta.multicastList.macAddr[i].addr, 6) )
+    	{
+            bIsInMCListAddr = 1;
+            break;
+    	}
+    }
+
+    return bIsInMCListAddr;
+}
+
+void zfiWlanSetSafeModeEnabled(zdev_t* dev, u8_t safeMode)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.bSafeMode = safeMode;
+
+    if ( safeMode )
+    	zfStaEnableSWEncryption(dev, 1);
+    else
+        zfStaDisableSWEncryption(dev);
+}
+
+void zfiWlanSetIBSSAdditionalIELength(zdev_t* dev, u32_t ibssAdditionalIESize, u8_t* ibssAdditionalIE)
+{
+	zmw_get_wlan_dev(dev);
+
+	if ( ibssAdditionalIESize )
+    {
+	    wd->sta.ibssAdditionalIESize = ibssAdditionalIESize;
+        zfMemoryCopy(wd->sta.ibssAdditionalIE, ibssAdditionalIE, (u16_t)ibssAdditionalIESize);
+    }
+    else
+    	wd->sta.ibssAdditionalIESize = 0;
+}
diff --git a/drivers/staging/otus/80211core/cprecomp.h b/drivers/staging/otus/80211core/cprecomp.h
new file mode 100644
index 00000000000..1670bfc2258
--- /dev/null
+++ b/drivers/staging/otus/80211core/cprecomp.h
@@ -0,0 +1,32 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef _CPRECOMP_H
+#define _CPRECOMP_H
+
+#include "../oal_dt.h"
+#include "../oal_marc.h"
+#include "pub_zfi.h"
+#include "pub_zfw.h"
+#include "pub_usb.h"
+#include "wlan.h"
+#include "struct.h"
+#include "cfunc.h"
+#include "cagg.h"
+#include "cwm.h"
+#include "performance.h"
+#endif
+
diff --git a/drivers/staging/otus/80211core/cpsmgr.c b/drivers/staging/otus/80211core/cpsmgr.c
new file mode 100644
index 00000000000..cf73caca8e5
--- /dev/null
+++ b/drivers/staging/otus/80211core/cpsmgr.c
@@ -0,0 +1,731 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+/**
+  *  The power saving manager is to save the power as much as possible.
+  *  Generally speaking, it controls:
+  *
+  *         - when to sleep
+  *         -
+  *
+  */
+#include "cprecomp.h"
+
+void zfPowerSavingMgrInit(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.powerSaveMode = ZM_STA_PS_NONE;
+    wd->sta.psMgr.state = ZM_PS_MSG_STATE_ACTIVE;
+    wd->sta.psMgr.isSleepAllowed = 0;
+    wd->sta.psMgr.maxSleepPeriods = 1;
+    wd->sta.psMgr.ticks = 0;
+    wd->sta.psMgr.sleepAllowedtick = 0;
+}
+
+static u16_t zfPowerSavingMgrHandlePsNone(zdev_t* dev, u8_t *isWakeUpRequired)
+{
+    u16_t ret = 0;
+    zmw_get_wlan_dev(dev);
+
+    switch(wd->sta.psMgr.state)
+    {
+        case ZM_PS_MSG_STATE_ACTIVE:
+            *isWakeUpRequired = 0;
+            break;
+
+        case ZM_PS_MSG_STATE_T1:
+        case ZM_PS_MSG_STATE_T2:
+        case ZM_PS_MSG_STATE_SLEEP:
+        default:
+            *isWakeUpRequired = 1;
+zm_debug_msg0("zfPowerSavingMgrHandlePsNone: Wake up now\n");
+            if ( zfStaIsConnected(dev) )
+            {
+                zm_debug_msg0("zfPowerSavingMgrOnHandleT1 send Null data\n");
+                //zfSendNullData(dev, 0);
+                ret = 1;
+            }
+
+            wd->sta.psMgr.state = ZM_PS_MSG_STATE_ACTIVE;
+            break;
+    }
+    return ret;
+}
+
+static void zfPowerSavingMgrHandlePs(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    switch(wd->sta.psMgr.state)
+    {
+        case ZM_PS_MSG_STATE_ACTIVE:
+            //zm_debug_msg0("zfPowerSavingMgrHandlePs: Prepare to sleep...\n");
+            //wd->sta.psMgr.state = ZM_PS_MSG_STATE_T1;
+            break;
+
+        case ZM_PS_MSG_STATE_T1:
+        case ZM_PS_MSG_STATE_T2:
+        case ZM_PS_MSG_STATE_SLEEP:
+        default:
+            break;
+    }
+}
+
+void zfPowerSavingMgrSetMode(zdev_t* dev, u8_t mode)
+{
+    u16_t sendNull = 0;
+    u8_t isWakeUpRequired = 0;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zm_debug_msg1("mode = ", mode);
+
+    if (mode > ZM_STA_PS_LIGHT)
+    {
+        zm_debug_msg0("return - wrong power save mode");
+        return;
+    }
+
+    zmw_enter_critical_section(dev);
+
+    #if 1
+    switch(mode)
+    {
+        case ZM_STA_PS_NONE:
+            sendNull = zfPowerSavingMgrHandlePsNone(dev, &isWakeUpRequired);
+            break;
+
+        case ZM_STA_PS_FAST:
+        case ZM_STA_PS_LIGHT:
+            wd->sta.psMgr.maxSleepPeriods = 1;
+            zfPowerSavingMgrHandlePs(dev);
+            break;
+
+        case ZM_STA_PS_MAX:
+            wd->sta.psMgr.maxSleepPeriods = ZM_PS_MAX_SLEEP_PERIODS;
+            zfPowerSavingMgrHandlePs(dev);
+            break;
+    }
+    #else
+    switch(wd->sta.psMgr.state)
+    {
+        case ZM_PS_MSG_STATE_ACTIVE:
+            if ( mode != ZM_STA_PS_NONE )
+            {
+zm_debug_msg0("zfPowerSavingMgrSetMode: switch from ZM_PS_MSG_STATE_ACTIVE to ZM_PS_MSG_STATE_T1\n");
+                // Stall the TX & start to wait the pending TX to be completed
+                wd->sta.psMgr.state = ZM_PS_MSG_STATE_T1;
+            }
+            break;
+
+        case ZM_PS_MSG_STATE_SLEEP:
+            break;
+    }
+    #endif
+
+    wd->sta.powerSaveMode = mode;
+    zmw_leave_critical_section(dev);
+
+    if ( isWakeUpRequired )
+    {
+        zfHpPowerSaveSetState(dev, 0);
+        wd->sta.psMgr.tempWakeUp = 0;
+    }
+
+    if ( zfStaIsConnected(dev)
+         && (wd->wlanMode == ZM_MODE_INFRASTRUCTURE) )
+    {
+        switch(mode)
+        {
+            case ZM_STA_PS_NONE:
+                zfHpPowerSaveSetMode(dev, 0, 0, wd->beaconInterval);
+                break;
+
+            case ZM_STA_PS_FAST:
+            case ZM_STA_PS_MAX:
+            case ZM_STA_PS_LIGHT:
+                zfHpPowerSaveSetMode(dev, 0, 1, wd->beaconInterval);
+                break;
+
+            default:
+                zfHpPowerSaveSetMode(dev, 0, 0, wd->beaconInterval);
+                break;
+        }
+    }
+
+    if (sendNull == 1)
+    {
+        zfSendNullData(dev, 0);
+    }
+
+    return;
+}
+
+static void zfPowerSavingMgrNotifyPSToAP(zdev_t *dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    if ( (wd->sta.psMgr.tempWakeUp != 1)&&
+         (wd->sta.psMgr.lastTxUnicastFrm != wd->commTally.txUnicastFrm ||
+          wd->sta.psMgr.lastTxBroadcastFrm != wd->commTally.txBroadcastFrm ||
+          wd->sta.psMgr.lastTxMulticastFrm != wd->commTally.txMulticastFrm) )
+    {
+        zmw_enter_critical_section(dev);
+        wd->sta.psMgr.lastTxUnicastFrm = wd->commTally.txUnicastFrm;
+        wd->sta.psMgr.lastTxBroadcastFrm = wd->commTally.txBroadcastFrm;
+        wd->sta.psMgr.lastTxMulticastFrm = wd->commTally.txMulticastFrm;
+        zmw_leave_critical_section(dev);
+
+        zfSendNullData(dev, 1);
+    }
+}
+
+static void zfPowerSavingMgrOnHandleT1(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    // If the tx Q is not empty...return
+    if ( zfIsVtxqEmpty(dev) == FALSE )
+    {
+        return;
+    }
+
+zm_debug_msg0("VtxQ is empty now...Check if HAL TXQ is empty\n");
+
+    // The the HAL TX Q is not empty...return
+    if ( zfHpGetFreeTxdCount(dev) != zfHpGetMaxTxdCount(dev) )
+    {
+        return;
+    }
+
+zm_debug_msg0("HAL TXQ is empty now...Could go to sleep...\n");
+
+    zmw_enter_critical_section(dev);
+
+    if (wd->sta.powerSaveMode == ZM_STA_PS_LIGHT)
+    {
+        if (wd->sta.ReceivedPktRatePerSecond > 200)
+        {
+            zmw_leave_critical_section(dev);
+            return;
+        }
+
+        if ( zfStaIsConnected(dev)
+             && (wd->wlanMode == ZM_MODE_INFRASTRUCTURE) )
+        {
+            if (wd->sta.psMgr.sleepAllowedtick) {
+                wd->sta.psMgr.sleepAllowedtick--;
+                zmw_leave_critical_section(dev);
+                return;
+            }
+        }
+    }
+
+    wd->sta.psMgr.state = ZM_PS_MSG_STATE_T2;
+
+    zmw_leave_critical_section(dev);
+
+    // Send the Null pkt to AP to notify that I'm going to sleep
+    if ( zfStaIsConnected(dev) )
+    {
+zm_debug_msg0("zfPowerSavingMgrOnHandleT1 send Null data\n");
+        zfPowerSavingMgrNotifyPSToAP(dev);
+    }
+
+    // Stall the TX now
+    // zfTxEngineStop(dev);
+}
+
+static void zfPowerSavingMgrOnHandleT2(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    // Wait until the Null pkt is transmitted
+    if ( zfHpGetFreeTxdCount(dev) != zfHpGetMaxTxdCount(dev) )
+    {
+        return;
+    }
+
+    zmw_enter_critical_section(dev);
+    wd->sta.psMgr.state = ZM_PS_MSG_STATE_SLEEP;
+    wd->sta.psMgr.lastTxUnicastFrm = wd->commTally.txUnicastFrm;
+    wd->sta.psMgr.lastTxBroadcastFrm = wd->commTally.txBroadcastFrm;
+    wd->sta.psMgr.lastTxMulticastFrm = wd->commTally.txMulticastFrm;
+    zmw_leave_critical_section(dev);
+
+    // Let CHIP sleep now
+zm_debug_msg0("zfPowerSavingMgrOnHandleT2 zzzz....\n");
+    zfHpPowerSaveSetState(dev, 1);
+    wd->sta.psMgr.tempWakeUp = 0;
+}
+
+u8_t zfPowerSavingMgrIsSleeping(zdev_t *dev)
+{
+    u8_t isSleeping = FALSE;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    if ( wd->sta.psMgr.state == ZM_PS_MSG_STATE_SLEEP ||
+         wd->sta.psMgr.state == ZM_PS_MSG_STATE_T2)
+    {
+        isSleeping = TRUE;
+    }
+    zmw_leave_critical_section(dev);
+    return isSleeping;
+}
+
+static u8_t zfPowerSavingMgrIsIdle(zdev_t *dev)
+{
+    u8_t isIdle = 0;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if ( zfStaIsConnected(dev) && wd->sta.psMgr.isSleepAllowed == 0 )
+    {
+        goto done;
+    }
+
+    if ( wd->sta.bChannelScan )
+    {
+        goto done;
+    }
+
+    if ( zfStaIsConnecting(dev) )
+    {
+        goto done;
+    }
+
+    if (wd->sta.powerSaveMode == ZM_STA_PS_LIGHT)
+    {
+        if (wd->sta.ReceivedPktRatePerSecond > 200)
+        {
+            goto done;
+        }
+
+        if ( zfStaIsConnected(dev)
+             && (wd->wlanMode == ZM_MODE_INFRASTRUCTURE) )
+        {
+            if (wd->sta.psMgr.sleepAllowedtick) {
+                wd->sta.psMgr.sleepAllowedtick--;
+                goto done;
+            }
+        }
+    }
+
+    isIdle = 1;
+
+done:
+    zmw_leave_critical_section(dev);
+
+    if ( zfIsVtxqEmpty(dev) == FALSE )
+    {
+        isIdle = 0;
+    }
+
+    return isIdle;
+}
+
+static void zfPowerSavingMgrSleepIfIdle(zdev_t *dev)
+{
+    u8_t isIdle;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    isIdle = zfPowerSavingMgrIsIdle(dev);
+
+    if ( isIdle == 0 )
+    {
+        return;
+    }
+
+    zmw_enter_critical_section(dev);
+
+    switch(wd->sta.powerSaveMode)
+    {
+        case ZM_STA_PS_NONE:
+            break;
+
+        case ZM_STA_PS_MAX:
+        case ZM_STA_PS_FAST:
+        case ZM_STA_PS_LIGHT:
+            zm_debug_msg0("zfPowerSavingMgrSleepIfIdle: IDLE so slep now...\n");
+            wd->sta.psMgr.state = ZM_PS_MSG_STATE_T1;
+            break;
+    }
+
+    zmw_leave_critical_section(dev);
+}
+
+static void zfPowerSavingMgrDisconnectMain(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+#ifdef ZM_ENABLE_DISCONNECT_PS
+    switch(wd->sta.psMgr.state)
+    {
+        case ZM_PS_MSG_STATE_ACTIVE:
+            zfPowerSavingMgrSleepIfIdle(dev);
+            break;
+
+        case ZM_PS_MSG_STATE_SLEEP:
+            break;
+
+        case ZM_PS_MSG_STATE_T1:
+            zfPowerSavingMgrOnHandleT1(dev);
+            break;
+
+        case ZM_PS_MSG_STATE_T2:
+            zfPowerSavingMgrOnHandleT2(dev);
+            break;
+    }
+#else
+    zfPowerSavingMgrWakeup(dev);
+#endif
+}
+
+static void zfPowerSavingMgrInfraMain(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    switch(wd->sta.psMgr.state)
+    {
+        case ZM_PS_MSG_STATE_ACTIVE:
+            zfPowerSavingMgrSleepIfIdle(dev);
+            break;
+
+        case ZM_PS_MSG_STATE_SLEEP:
+            break;
+
+        case ZM_PS_MSG_STATE_T1:
+            zfPowerSavingMgrOnHandleT1(dev);
+            break;
+
+        case ZM_PS_MSG_STATE_T2:
+            zfPowerSavingMgrOnHandleT2(dev);
+            break;
+    }
+}
+
+void zfPowerSavingMgrAtimWinExpired(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+//printk("zfPowerSavingMgrAtimWinExpired #1\n");
+    if ( wd->sta.powerSaveMode == ZM_STA_PS_NONE )
+    {
+        return;
+    }
+
+//printk("zfPowerSavingMgrAtimWinExpired #2\n");
+    // if we received any ATIM window from the others to indicate we have buffered data
+    // at the other station, we can't go to sleep
+    if ( wd->sta.recvAtim )
+    {
+        wd->sta.recvAtim = 0;
+        zm_debug_msg0("Can't sleep due to receving ATIM window!");
+        return;
+    }
+
+    // if we are the one to tx beacon during last beacon interval. we can't go to sleep
+    // since we need to be alive to respond the probe request!
+    if ( wd->sta.txBeaconInd )
+    {
+        zm_debug_msg0("Can't sleep due to just transmit a beacon!");
+        return;
+    }
+
+    // If we buffer any data for the other stations. we could not go to sleep
+    if ( wd->sta.ibssPrevPSDataCount != 0 )
+    {
+        zm_debug_msg0("Can't sleep due to buffering data for the others!");
+        return;
+    }
+
+    // before sleeping, we still need to notify the others by transmitting null
+    // pkt with power mgmt bit turned on.
+    zfPowerSavingMgrOnHandleT1(dev);
+}
+
+static void zfPowerSavingMgrIBSSMain(zdev_t* dev)
+{
+    // wait for the end of
+    // if need to wait to know if we are the one to transmit the beacon
+    // during the beacon interval. If it's me, we can't go to sleep.
+
+    zmw_get_wlan_dev(dev);
+
+    switch(wd->sta.psMgr.state)
+    {
+        case ZM_PS_MSG_STATE_ACTIVE:
+        case ZM_PS_MSG_STATE_SLEEP:
+        case ZM_PS_MSG_STATE_T1:
+            break;
+
+        case ZM_PS_MSG_STATE_T2:
+            zfPowerSavingMgrOnHandleT2(dev);
+            break;
+    }
+
+    return;
+}
+
+#if 1
+void zfPowerSavingMgrMain(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    switch (wd->sta.adapterState)
+    {
+    case ZM_STA_STATE_DISCONNECT:
+        zfPowerSavingMgrDisconnectMain(dev);
+        break;
+    case ZM_STA_STATE_CONNECTED:
+        {
+            if (wd->wlanMode == ZM_MODE_INFRASTRUCTURE) {
+                zfPowerSavingMgrInfraMain(dev);
+            } else if (wd->wlanMode == ZM_MODE_IBSS) {
+                zfPowerSavingMgrIBSSMain(dev);
+            }
+        }
+        break;
+    case ZM_STA_STATE_CONNECTING:
+    default:
+        break;
+    }
+}
+#else
+void zfPowerSavingMgrMain(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->wlanMode != ZM_MODE_INFRASTRUCTURE )
+    {
+        return;
+    }
+
+    switch(wd->sta.psMgr.state)
+    {
+        case ZM_PS_MSG_STATE_ACTIVE:
+            goto check_sleep;
+            break;
+
+        case ZM_PS_MSG_STATE_SLEEP:
+            goto sleeping;
+            break;
+
+        case ZM_PS_MSG_STATE_T1:
+            zfPowerSavingMgrOnHandleT1(dev);
+            break;
+
+        case ZM_PS_MSG_STATE_T2:
+            zfPowerSavingMgrOnHandleT2(dev);
+            break;
+    }
+
+    return;
+
+sleeping:
+    return;
+
+check_sleep:
+    zfPowerSavingMgrSleepIfIdle(dev);
+    return;
+}
+#endif
+
+#ifdef ZM_ENABLE_POWER_SAVE
+void zfPowerSavingMgrWakeup(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+//zm_debug_msg0("zfPowerSavingMgrWakeup");
+
+    //if ( wd->sta.psMgr.state != ZM_PS_MSG_STATE_ACTIVE && ( zfPowerSavingMgrIsIdle(dev) == 0 ))
+    if ( wd->sta.psMgr.state != ZM_PS_MSG_STATE_ACTIVE )
+    {
+        zmw_enter_critical_section(dev);
+
+        wd->sta.psMgr.isSleepAllowed = 0;
+        wd->sta.psMgr.state = ZM_PS_MSG_STATE_ACTIVE;
+
+        if ( wd->sta.powerSaveMode > ZM_STA_PS_NONE )
+            wd->sta.psMgr.tempWakeUp = 1;
+
+        zmw_leave_critical_section(dev);
+
+        // Wake up the CHIP now!!
+        zfHpPowerSaveSetState(dev, 0);
+    }
+}
+#else
+void zfPowerSavingMgrWakeup(zdev_t* dev)
+{
+}
+#endif
+
+void zfPowerSavingMgrProcessBeacon(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t   length, bitmap;
+    u16_t  offset, n1, n2, q, r;
+    zbuf_t* psBuf;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    if ( wd->sta.powerSaveMode == ZM_STA_PS_NONE  )
+    //if ( wd->sta.psMgr.state != ZM_PS_MSG_STATE_SLEEP )
+    {
+        return;
+    }
+
+    wd->sta.psMgr.isSleepAllowed = 1;
+
+    if ( (offset=zfFindElement(dev, buf, ZM_WLAN_EID_TIM)) != 0xffff )
+    {
+        length = zmw_rx_buf_readb(dev, buf, offset+1);
+
+        if ( length > 3 )
+        {
+            n1 = zmw_rx_buf_readb(dev, buf, offset+4) & (~ZM_BIT_0);
+            n2 = length + n1 - 4;
+            q = wd->sta.aid >> 3;
+            r = wd->sta.aid & 7;
+
+            if ((q >= n1) && (q <= n2))
+            {
+                bitmap = zmw_rx_buf_readb(dev, buf, offset+5+q-n1);
+
+                if ( (bitmap >> r) &  ZM_BIT_0 )
+                {
+                    //if ( wd->sta.powerSaveMode == ZM_STA_PS_FAST )
+                    if ( 0 )
+                    {
+                        wd->sta.psMgr.state = ZM_PS_MSG_STATE_S1;
+                        //zfSendPSPoll(dev);
+                        zfSendNullData(dev, 0);
+                    }
+                    else
+                    {
+                        if ((wd->sta.qosInfo&0xf) != 0xf)
+                        {
+                            /* send ps-poll */
+                            //printk("zfSendPSPoll #1\n");
+
+                            wd->sta.psMgr.isSleepAllowed = 0;
+
+                            switch (wd->sta.powerSaveMode)
+                            {
+                            case ZM_STA_PS_MAX:
+                            case ZM_STA_PS_FAST:
+                                //zm_debug_msg0("wake up and send PS-Poll\n");
+                                zfSendPSPoll(dev);
+                                break;
+                            case ZM_STA_PS_LIGHT:
+                                zm_debug_msg0("wake up and send null data\n");
+
+                                zmw_enter_critical_section(dev);
+                                wd->sta.psMgr.sleepAllowedtick = 400;
+                                zmw_leave_critical_section(dev);
+
+                                zfSendNullData(dev, 0);
+                                break;
+                            }
+
+                            wd->sta.psMgr.tempWakeUp = 0;
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    while ((psBuf = zfQueueGet(dev, wd->sta.uapsdQ)) != NULL)
+    {
+        zfTxSendEth(dev, psBuf, 0, ZM_EXTERNAL_ALLOC_BUF, 0);
+    }
+
+    //printk("zfPowerSavingMgrProcessBeacon #1\n");
+    zfPowerSavingMgrMain(dev);
+}
+
+void zfPowerSavingMgrConnectNotify(zdev_t *dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+    {
+        switch(wd->sta.powerSaveMode)
+        {
+            case ZM_STA_PS_NONE:
+                zfHpPowerSaveSetMode(dev, 0, 0, wd->beaconInterval);
+                break;
+
+            case ZM_STA_PS_FAST:
+            case ZM_STA_PS_MAX:
+            case ZM_STA_PS_LIGHT:
+                zfHpPowerSaveSetMode(dev, 0, 1, wd->beaconInterval);
+                break;
+
+            default:
+                zfHpPowerSaveSetMode(dev, 0, 0, wd->beaconInterval);
+                break;
+        }
+    }
+}
+
+void zfPowerSavingMgrPreTBTTInterrupt(zdev_t *dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    /* disable TBTT interrupt when change from connection to disconnect */
+    if (zfStaIsDisconnect(dev)) {
+        zfHpPowerSaveSetMode(dev, 0, 0, 0);
+        zfPowerSavingMgrWakeup(dev);
+        return;
+    }
+
+    zmw_enter_critical_section(dev);
+    wd->sta.psMgr.ticks++;
+
+    if ( wd->sta.psMgr.ticks < wd->sta.psMgr.maxSleepPeriods )
+    {
+        zmw_leave_critical_section(dev);
+        return;
+    }
+    else
+    {
+        wd->sta.psMgr.ticks = 0;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    zfPowerSavingMgrWakeup(dev);
+}
+
+/* Leave an empty line below to remove warning message on some compiler */
+
diff --git a/drivers/staging/otus/80211core/cscanmgr.c b/drivers/staging/otus/80211core/cscanmgr.c
new file mode 100644
index 00000000000..b32835c8759
--- /dev/null
+++ b/drivers/staging/otus/80211core/cscanmgr.c
@@ -0,0 +1,535 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include "cprecomp.h"
+
+void zfScanMgrInit(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->sta.scanMgr.scanReqs[0] = 0;
+    wd->sta.scanMgr.scanReqs[1] = 0;
+
+    wd->sta.scanMgr.currScanType = ZM_SCAN_MGR_SCAN_NONE;
+    wd->sta.scanMgr.scanStartDelay = 3;
+    //wd->sta.scanMgr.scanStartDelay = 0;
+}
+
+u8_t zfScanMgrScanStart(zdev_t* dev, u8_t scanType)
+{
+    u8_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    zm_debug_msg1("scanType = ", scanType);
+
+    zmw_declare_for_critical_section();
+
+    if ( scanType != ZM_SCAN_MGR_SCAN_INTERNAL &&
+         scanType != ZM_SCAN_MGR_SCAN_EXTERNAL )
+    {
+        zm_debug_msg0("unknown scanType");
+        return 1;
+    }
+    else if (zfStaIsConnecting(dev))
+    {
+        zm_debug_msg0("reject scan request due to connecting");
+        return 1;
+    }
+
+    i = scanType - 1;
+
+    zmw_enter_critical_section(dev);
+
+    if ( wd->sta.scanMgr.scanReqs[i] == 1 )
+    {
+        zm_debug_msg1("scan rescheduled", scanType);
+        goto scan_done;
+    }
+
+    wd->sta.scanMgr.scanReqs[i] = 1;
+    zm_debug_msg1("scan scheduled: ", scanType);
+
+    // If there's no scan pending, we do the scan right away.
+    // If there's an internal scan and the new scan request is external one,
+    // we will restart the scan.
+    if ( wd->sta.scanMgr.currScanType == ZM_SCAN_MGR_SCAN_NONE )
+    {
+        goto schedule_scan;
+    }
+    else if ( wd->sta.scanMgr.currScanType == ZM_SCAN_MGR_SCAN_INTERNAL &&
+              scanType == ZM_SCAN_MGR_SCAN_EXTERNAL )
+    {
+        // Stop the internal scan & schedule external scan first
+        zfTimerCancel(dev, ZM_EVENT_SCAN);
+
+        /* Fix for WHQL sendrecv => we do not apply delay time in which the device
+           stop transmitting packet when we already connect to some AP  */
+        wd->sta.bScheduleScan = FALSE;
+
+        zfTimerCancel(dev, ZM_EVENT_TIMEOUT_SCAN);
+        zfTimerCancel(dev, ZM_EVENT_IN_SCAN);
+
+        wd->sta.bChannelScan = FALSE;
+        goto schedule_scan;
+    }
+    else
+    {
+        zm_debug_msg0("Scan is busy...waiting later to start\n");
+    }
+
+    zmw_leave_critical_section(dev);
+    return 0;
+
+scan_done:
+    zmw_leave_critical_section(dev);
+    return 1;
+
+schedule_scan:
+
+    wd->sta.bScheduleScan = TRUE;
+
+    zfTimerSchedule(dev, ZM_EVENT_SCAN, wd->sta.scanMgr.scanStartDelay);
+    wd->sta.scanMgr.scanStartDelay = 3;
+    //wd->sta.scanMgr.scanStartDelay = 0;
+    wd->sta.scanMgr.currScanType = scanType;
+    zmw_leave_critical_section(dev);
+
+    if ((zfStaIsConnected(dev)) && (!zfPowerSavingMgrIsSleeping(dev)))
+    {
+        zfSendNullData(dev, 1);
+    }
+    return 0;
+}
+
+void zfScanMgrScanStop(zdev_t* dev, u8_t scanType)
+{
+    u8_t scanNotifyRequired = 0;
+    u8_t theOtherScan = ZM_SCAN_MGR_SCAN_NONE;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if ( wd->sta.scanMgr.currScanType == ZM_SCAN_MGR_SCAN_NONE )
+    {
+        zm_assert(wd->sta.scanMgr.scanReqs[0] == 0);
+        zm_assert(wd->sta.scanMgr.scanReqs[1] == 0);
+        goto done;
+    }
+
+    switch(scanType)
+    {
+        case ZM_SCAN_MGR_SCAN_EXTERNAL:
+            scanNotifyRequired = 1;
+            theOtherScan = ZM_SCAN_MGR_SCAN_INTERNAL;
+            break;
+
+        case ZM_SCAN_MGR_SCAN_INTERNAL:
+            theOtherScan = ZM_SCAN_MGR_SCAN_EXTERNAL;
+            break;
+
+        default:
+            goto done;
+    }
+
+    if ( wd->sta.scanMgr.currScanType != scanType )
+    {
+        goto stop_done;
+    }
+
+    zfTimerCancel(dev, ZM_EVENT_SCAN);
+
+    /* Fix for WHQL sendrecv => we do not apply delay time in which the device
+       stop transmitting packet when we already connect to some AP  */
+    wd->sta.bScheduleScan = FALSE;
+
+    zfTimerCancel(dev, ZM_EVENT_TIMEOUT_SCAN);
+    zfTimerCancel(dev, ZM_EVENT_IN_SCAN);
+
+    wd->sta.bChannelScan = FALSE;
+    wd->sta.scanFrequency = 0;
+
+    if ( wd->sta.scanMgr.scanReqs[theOtherScan - 1] )
+    {
+        wd->sta.scanMgr.currScanType = theOtherScan;
+
+        // Schedule the other scan after 1 second later
+        zfTimerSchedule(dev, ZM_EVENT_SCAN, 100);
+    }
+    else
+    {
+        wd->sta.scanMgr.currScanType = ZM_SCAN_MGR_SCAN_NONE;
+    }
+
+stop_done:
+    wd->sta.scanMgr.scanReqs[scanType - 1] = 0;
+
+    zmw_leave_critical_section(dev);
+
+    /* avoid lose receive packet when site survey */
+    if ((zfStaIsConnected(dev)) && (!zfPowerSavingMgrIsSleeping(dev)))
+    {
+        zfSendNullData(dev, 0);
+    }
+
+    if ( scanNotifyRequired )
+    {
+        zm_debug_msg0("Scan notify after reset");
+        if (wd->zfcbScanNotify != NULL)
+        {
+            wd->zfcbScanNotify(dev, NULL);
+        }
+    }
+
+    return;
+
+done:
+    zmw_leave_critical_section(dev);
+    return;
+}
+
+void zfScanMgrScanAck(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    wd->sta.scanMgr.scanStartDelay = 3;
+    //wd->sta.scanMgr.scanStartDelay = 0;
+
+    zmw_leave_critical_section(dev);
+    return;
+}
+
+extern void zfStaReconnect(zdev_t* dev);
+
+static void zfScanSendProbeRequest(zdev_t* dev)
+{
+    u8_t k;
+    u16_t  dst[3] = { 0xffff, 0xffff, 0xffff };
+
+    zmw_get_wlan_dev(dev);
+
+    /* Increase rxBeaconCount to prevent beacon lost */
+    if (zfStaIsConnected(dev))
+    {
+        wd->sta.rxBeaconCount++;
+    }
+
+    if ( wd->sta.bPassiveScan )
+    {
+        return;
+    }
+    /* enable 802.l11h and in DFS Band , disable sending probe request */
+    if (wd->sta.DFSEnable)
+    {
+        if (zfHpIsDfsChannel(dev, wd->sta.scanFrequency))
+        {
+            return;
+        }
+    }
+
+    zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_PROBEREQ, dst, 0, 0, 0);
+
+    if ( wd->sta.disableProbingWithSsid )
+    {
+        return;
+    }
+
+    for (k=1; k<=ZM_MAX_PROBE_HIDDEN_SSID_SIZE; k++)
+    {
+        if ( wd->ws.probingSsidList[k-1].ssidLen != 0 )
+        {
+            zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_PROBEREQ, dst, k, 0, 0);
+        }
+    }
+}
+
+static void zfScanMgrEventSetFreqCompleteCb(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+//printk("zfScanMgrEventSetFreqCompleteCb #1\n");
+
+    zmw_enter_critical_section(dev);
+    zfTimerSchedule(dev, ZM_EVENT_IN_SCAN, ZM_TICK_IN_SCAN);
+    if (wd->sta.bPassiveScan)
+    {
+        zfTimerSchedule(dev, ZM_EVENT_TIMEOUT_SCAN, wd->sta.passiveScanTickPerChannel);
+    }
+    else
+    {
+        zfTimerSchedule(dev, ZM_EVENT_TIMEOUT_SCAN, wd->sta.activescanTickPerChannel);
+    }
+    zmw_leave_critical_section(dev);
+
+    zfScanSendProbeRequest(dev);
+}
+
+
+static void zfScanMgrEventScanCompleteCb(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if ((zfStaIsConnected(dev)) && (!zfPowerSavingMgrIsSleeping(dev)))
+    {
+        zfSendNullData(dev, 0);
+    }
+    return;
+}
+
+
+void zfScanMgrScanEventRetry(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if ( !wd->sta.bChannelScan )
+    {
+        return;
+    }
+
+    if ( !wd->sta.bPassiveScan )
+    {
+        zfScanSendProbeRequest(dev);
+        #if 0
+        zmw_enter_critical_section(dev);
+        zfTimerSchedule(dev, ZM_EVENT_IN_SCAN, ZM_TICK_IN_SCAN);
+        zmw_leave_critical_section(dev);
+        #endif
+    }
+}
+
+u8_t zfScanMgrScanEventTimeout(zdev_t* dev)
+{
+    u16_t   nextScanFrequency = 0;
+    u8_t    temp;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    if ( wd->sta.scanFrequency == 0 )
+    {
+        zmw_leave_critical_section(dev);
+        return -1;
+    }
+
+    nextScanFrequency = zfChGetNextChannel(dev, wd->sta.scanFrequency,
+                                           &wd->sta.bPassiveScan);
+
+    if ( (nextScanFrequency == 0xffff)
+         || (wd->sta.scanFrequency == zfChGetLastChannel(dev, &temp)) )
+    {
+        u8_t currScanType;
+        u8_t isExternalScan = 0;
+        u8_t isInternalScan = 0;
+
+        //zm_debug_msg1("end scan = ", KeQueryInterruptTime());
+        wd->sta.scanFrequency = 0;
+
+        zm_debug_msg1("scan 1 type: ", wd->sta.scanMgr.currScanType);
+        zm_debug_msg1("scan channel count = ", wd->regulationTable.allowChannelCnt);
+
+        //zfBssInfoRefresh(dev);
+        zfTimerCancel(dev, ZM_EVENT_TIMEOUT_SCAN);
+
+        if ( wd->sta.bChannelScan == FALSE )
+        {
+            zm_debug_msg0("WOW!! scan is cancelled\n");
+            zmw_leave_critical_section(dev);
+            goto report_scan_result;
+        }
+
+
+        currScanType = wd->sta.scanMgr.currScanType;
+        switch(currScanType)
+        {
+            case ZM_SCAN_MGR_SCAN_EXTERNAL:
+                isExternalScan = 1;
+
+                if ( wd->sta.scanMgr.scanReqs[ZM_SCAN_MGR_SCAN_INTERNAL - 1] )
+                {
+                    wd->sta.scanMgr.scanReqs[ZM_SCAN_MGR_SCAN_INTERNAL - 1] = 0;
+                    isInternalScan = 1;
+                }
+
+                break;
+
+            case ZM_SCAN_MGR_SCAN_INTERNAL:
+                isInternalScan = 1;
+
+                if ( wd->sta.scanMgr.scanReqs[ZM_SCAN_MGR_SCAN_EXTERNAL - 1] )
+                {
+                    // Because the external scan should pre-empts internal scan.
+                    // So this shall not be happened!!
+                    zm_assert(0);
+                }
+
+                break;
+
+            default:
+                zm_assert(0);
+                break;
+        }
+
+        wd->sta.scanMgr.scanReqs[currScanType - 1] = 0;
+        wd->sta.scanMgr.scanStartDelay = 100;
+        wd->sta.scanMgr.currScanType = ZM_SCAN_MGR_SCAN_NONE;
+        zmw_leave_critical_section(dev);
+
+        //Set channel according to AP's configuration
+        zfCoreSetFrequencyEx(dev, wd->frequency, wd->BandWidth40,
+                wd->ExtOffset, zfScanMgrEventScanCompleteCb);
+
+        wd->sta.bChannelScan = FALSE;
+
+        #if 1
+        if (zfStaIsConnected(dev))
+        { // Finish site survey, reset the variable to detect using wrong frequency !
+            zfHpFinishSiteSurvey(dev, 1);
+            zmw_enter_critical_section(dev);
+            wd->sta.ibssSiteSurveyStatus = 2;
+            wd->tickIbssReceiveBeacon = 0;
+            wd->sta.ibssReceiveBeaconCount = 0;
+            zmw_leave_critical_section(dev);
+
+            /* #5 Re-enable RIFS function after the site survey ! */
+            /* This is because switch band will reset the BB register to initial value */
+            if( wd->sta.rifsState == ZM_RIFS_STATE_DETECTED )
+            {
+                zfHpEnableRifs(dev, ((wd->sta.currentFrequency<3000)?1:0), wd->sta.EnableHT, wd->sta.HT2040);
+            }
+        }
+        else
+        {
+            zfHpFinishSiteSurvey(dev, 0);
+            zmw_enter_critical_section(dev);
+            wd->sta.ibssSiteSurveyStatus = 0;
+            zmw_leave_critical_section(dev);
+        }
+        #endif
+
+report_scan_result:
+        /* avoid lose receive packet when site survey */
+        //if ((zfStaIsConnected(dev)) && (!zfPowerSavingMgrIsSleeping(dev)))
+        //{
+        //    zfSendNullData(dev, 0);
+        //}
+
+        if ( isExternalScan )//Quickly reboot
+        {
+            if (wd->zfcbScanNotify != NULL)
+            {
+                wd->zfcbScanNotify(dev, NULL);
+            }
+        }
+
+        if ( isInternalScan )
+        {
+            //wd->sta.InternalScanReq = 0;
+            zfStaReconnect(dev);
+        }
+
+        return 0;
+    }
+    else
+    {
+        wd->sta.scanFrequency = nextScanFrequency;
+
+        //zmw_enter_critical_section(dev);
+        zfTimerCancel(dev, ZM_EVENT_IN_SCAN);
+        zmw_leave_critical_section(dev);
+
+        zm_debug_msg0("scan 2");
+        zfCoreSetFrequencyV2(dev, wd->sta.scanFrequency, zfScanMgrEventSetFreqCompleteCb);
+
+        return 1;
+    }
+}
+
+void zfScanMgrScanEventStart(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if ( wd->sta.bChannelScan )
+    {
+        return;
+    }
+
+    zfPowerSavingMgrWakeup(dev);
+
+    zmw_enter_critical_section(dev);
+
+    if ( wd->sta.scanMgr.currScanType == ZM_SCAN_MGR_SCAN_NONE )
+    {
+        goto no_scan;
+    }
+
+    //zfBssInfoRefresh(dev);
+    zfBssInfoRefresh(dev, 0);
+    wd->sta.bChannelScan = TRUE;
+    wd->sta.bScheduleScan = FALSE;
+    zfTimerCancel(dev, ZM_EVENT_IN_SCAN);
+    zfTimerCancel(dev, ZM_EVENT_TIMEOUT_SCAN);
+
+    //zm_debug_msg1("start scan = ", KeQueryInterruptTime());
+    wd->sta.scanFrequency = zfChGetFirstChannel(dev, &wd->sta.bPassiveScan);
+    zmw_leave_critical_section(dev);
+
+    /* avoid lose receive packet when site survey */
+    //if ((zfStaIsConnected(dev)) && (!zfPowerSavingMgrIsSleeping(dev)))
+    //{
+    //    zfSendNullData(dev, 1);
+    //}
+//    zm_debug_msg0("scan 0");
+//    zfCoreSetFrequencyV2(dev, wd->sta.scanFrequency, zfScanMgrEventSetFreqCompleteCb);
+
+    #if 1
+    if (zfStaIsConnected(dev))
+    {// If doing site survey !
+        zfHpBeginSiteSurvey(dev, 1);
+        zmw_enter_critical_section(dev);
+        wd->sta.ibssSiteSurveyStatus = 1;
+        zmw_leave_critical_section(dev);
+    }
+    else
+    {
+        zfHpBeginSiteSurvey(dev, 0);
+        zmw_enter_critical_section(dev);
+        wd->sta.ibssSiteSurveyStatus = 0;
+        zmw_leave_critical_section(dev);
+    }
+    #endif
+
+    zm_debug_msg0("scan 0");
+    zfCoreSetFrequencyV2(dev, wd->sta.scanFrequency, zfScanMgrEventSetFreqCompleteCb);
+
+    return;
+
+no_scan:
+    zmw_leave_critical_section(dev);
+    return;
+}
diff --git a/drivers/staging/otus/80211core/ctkip.c b/drivers/staging/otus/80211core/ctkip.c
new file mode 100644
index 00000000000..be42f7aaa37
--- /dev/null
+++ b/drivers/staging/otus/80211core/ctkip.c
@@ -0,0 +1,598 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : ctkip.c                                               */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains Tx and Rx functions.                       */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+#include "cprecomp.h"
+
+u16_t zgTkipSboxLower[256] =
+    {
+        0xA5,0x84,0x99,0x8D,0x0D,0xBD,0xB1,0x54,
+        0x50,0x03,0xA9,0x7D,0x19,0x62,0xE6,0x9A,
+        0x45,0x9D,0x40,0x87,0x15,0xEB,0xC9,0x0B,
+        0xEC,0x67,0xFD,0xEA,0xBF,0xF7,0x96,0x5B,
+        0xC2,0x1C,0xAE,0x6A,0x5A,0x41,0x02,0x4F,
+        0x5C,0xF4,0x34,0x08,0x93,0x73,0x53,0x3F,
+        0x0C,0x52,0x65,0x5E,0x28,0xA1,0x0F,0xB5,
+        0x09,0x36,0x9B,0x3D,0x26,0x69,0xCD,0x9F,
+        0x1B,0x9E,0x74,0x2E,0x2D,0xB2,0xEE,0xFB,
+        0xF6,0x4D,0x61,0xCE,0x7B,0x3E,0x71,0x97,
+        0xF5,0x68,0x00,0x2C,0x60,0x1F,0xC8,0xED,
+        0xBE,0x46,0xD9,0x4B,0xDE,0xD4,0xE8,0x4A,
+        0x6B,0x2A,0xE5,0x16,0xC5,0xD7,0x55,0x94,
+        0xCF,0x10,0x06,0x81,0xF0,0x44,0xBA,0xE3,
+        0xF3,0xFE,0xC0,0x8A,0xAD,0xBC,0x48,0x04,
+        0xDF,0xC1,0x75,0x63,0x30,0x1A,0x0E,0x6D,
+        0x4C,0x14,0x35,0x2F,0xE1,0xA2,0xCC,0x39,
+        0x57,0xF2,0x82,0x47,0xAC,0xE7,0x2B,0x95,
+        0xA0,0x98,0xD1,0x7F,0x66,0x7E,0xAB,0x83,
+        0xCA,0x29,0xD3,0x3C,0x79,0xE2,0x1D,0x76,
+        0x3B,0x56,0x4E,0x1E,0xDB,0x0A,0x6C,0xE4,
+        0x5D,0x6E,0xEF,0xA6,0xA8,0xA4,0x37,0x8B,
+        0x32,0x43,0x59,0xB7,0x8C,0x64,0xD2,0xE0,
+        0xB4,0xFA,0x07,0x25,0xAF,0x8E,0xE9,0x18,
+        0xD5,0x88,0x6F,0x72,0x24,0xF1,0xC7,0x51,
+        0x23,0x7C,0x9C,0x21,0xDD,0xDC,0x86,0x85,
+        0x90,0x42,0xC4,0xAA,0xD8,0x05,0x01,0x12,
+        0xA3,0x5F,0xF9,0xD0,0x91,0x58,0x27,0xB9,
+        0x38,0x13,0xB3,0x33,0xBB,0x70,0x89,0xA7,
+        0xB6,0x22,0x92,0x20,0x49,0xFF,0x78,0x7A,
+        0x8F,0xF8,0x80,0x17,0xDA,0x31,0xC6,0xB8,
+        0xC3,0xB0,0x77,0x11,0xCB,0xFC,0xD6,0x3A
+    };
+
+
+u16_t zgTkipSboxUpper[256] =
+    {
+        0xC6,0xF8,0xEE,0xF6,0xFF,0xD6,0xDE,0x91,
+        0x60,0x02,0xCE,0x56,0xE7,0xB5,0x4D,0xEC,
+        0x8F,0x1F,0x89,0xFA,0xEF,0xB2,0x8E,0xFB,
+        0x41,0xB3,0x5F,0x45,0x23,0x53,0xE4,0x9B,
+        0x75,0xE1,0x3D,0x4C,0x6C,0x7E,0xF5,0x83,
+        0x68,0x51,0xD1,0xF9,0xE2,0xAB,0x62,0x2A,
+        0x08,0x95,0x46,0x9D,0x30,0x37,0x0A,0x2F,
+        0x0E,0x24,0x1B,0xDF,0xCD,0x4E,0x7F,0xEA,
+        0x12,0x1D,0x58,0x34,0x36,0xDC,0xB4,0x5B,
+        0xA4,0x76,0xB7,0x7D,0x52,0xDD,0x5E,0x13,
+        0xA6,0xB9,0x00,0xC1,0x40,0xE3,0x79,0xB6,
+        0xD4,0x8D,0x67,0x72,0x94,0x98,0xB0,0x85,
+        0xBB,0xC5,0x4F,0xED,0x86,0x9A,0x66,0x11,
+        0x8A,0xE9,0x04,0xFE,0xA0,0x78,0x25,0x4B,
+        0xA2,0x5D,0x80,0x05,0x3F,0x21,0x70,0xF1,
+        0x63,0x77,0xAF,0x42,0x20,0xE5,0xFD,0xBF,
+        0x81,0x18,0x26,0xC3,0xBE,0x35,0x88,0x2E,
+        0x93,0x55,0xFC,0x7A,0xC8,0xBA,0x32,0xE6,
+        0xC0,0x19,0x9E,0xA3,0x44,0x54,0x3B,0x0B,
+        0x8C,0xC7,0x6B,0x28,0xA7,0xBC,0x16,0xAD,
+        0xDB,0x64,0x74,0x14,0x92,0x0C,0x48,0xB8,
+        0x9F,0xBD,0x43,0xC4,0x39,0x31,0xD3,0xF2,
+        0xD5,0x8B,0x6E,0xDA,0x01,0xB1,0x9C,0x49,
+        0xD8,0xAC,0xF3,0xCF,0xCA,0xF4,0x47,0x10,
+        0x6F,0xF0,0x4A,0x5C,0x38,0x57,0x73,0x97,
+        0xCB,0xA1,0xE8,0x3E,0x96,0x61,0x0D,0x0F,
+        0xE0,0x7C,0x71,0xCC,0x90,0x06,0xF7,0x1C,
+        0xC2,0x6A,0xAE,0x69,0x17,0x99,0x3A,0x27,
+        0xD9,0xEB,0x2B,0x22,0xD2,0xA9,0x07,0x33,
+        0x2D,0x3C,0x15,0xC9,0x87,0xAA,0x50,0xA5,
+        0x03,0x59,0x09,0x1A,0x65,0xD7,0x84,0xD0,
+        0x82,0x29,0x5A,0x1E,0x7B,0xA8,0x6D,0x2C
+    };
+
+u16_t zfrotr1(u16_t a)
+// rotate right by 1 bit.
+{
+    u16_t b;
+
+    if (a & 0x01)
+    {
+        b = (a >> 1) | 0x8000;
+    }
+    else
+    {
+        b = (a >> 1) & 0x7fff;
+    }
+    return b;
+}
+
+/*************************************************************/
+/* zfTkipSbox()                                              */
+/* Returns a 16 bit value from a 64K entry table. The Table  */
+/* is synthesized from two 256 entry byte wide tables.       */
+/*************************************************************/
+u16_t zfTkipSbox(u16_t index)
+{
+    u16_t   low;
+    u16_t   high;
+    u16_t   left, right;
+
+    low = (index & 0xFF);
+    high = ((index >> 8) & 0xFF);
+
+    left = zgTkipSboxLower[low] + (zgTkipSboxUpper[low] << 8 );
+    right = zgTkipSboxUpper[high] + (zgTkipSboxLower[high] << 8 );
+
+    return (left ^ right);
+}
+
+u8_t zfTkipPhase1KeyMix(u32_t iv32, struct zsTkipSeed* pSeed)
+{
+    u16_t   tsc0;
+    u16_t   tsc1;
+    u16_t   i, j;
+#if 0
+    /* Need not proceed this function with the same iv32 */
+    if ( iv32 == pSeed->iv32 )
+    {
+        return 1;
+    }
+#endif
+    tsc0 = (u16_t) ((iv32 >> 16) & 0xffff); /* msb */
+    tsc1 = (u16_t) (iv32 & 0xffff);
+
+    /* Phase 1, step 1 */
+    pSeed->ttak[0] = tsc1;
+    pSeed->ttak[1] = tsc0;
+    pSeed->ttak[2] = (u16_t) (pSeed->ta[0] + (pSeed->ta[1] <<8));
+    pSeed->ttak[3] = (u16_t) (pSeed->ta[2] + (pSeed->ta[3] <<8));
+    pSeed->ttak[4] = (u16_t) (pSeed->ta[4] + (pSeed->ta[5] <<8));
+
+    /* Phase 1, step 2 */
+    for (i=0; i<8; i++)
+    {
+        j = 2*(i & 1);
+        pSeed->ttak[0] =(pSeed->ttak[0] + zfTkipSbox(pSeed->ttak[4]
+                         ^ ZM_BYTE_TO_WORD(pSeed->tk[1+j], pSeed->tk[j])))
+                        & 0xffff;
+        pSeed->ttak[1] =(pSeed->ttak[1] + zfTkipSbox(pSeed->ttak[0]
+                         ^ ZM_BYTE_TO_WORD(pSeed->tk[5+j], pSeed->tk[4+j] )))
+                        & 0xffff;
+        pSeed->ttak[2] =(pSeed->ttak[2] + zfTkipSbox(pSeed->ttak[1]
+                         ^ ZM_BYTE_TO_WORD(pSeed->tk[9+j], pSeed->tk[8+j] )))
+                        & 0xffff;
+        pSeed->ttak[3] =(pSeed->ttak[3] + zfTkipSbox(pSeed->ttak[2]
+                         ^ ZM_BYTE_TO_WORD(pSeed->tk[13+j], pSeed->tk[12+j])))
+                        & 0xffff;
+        pSeed->ttak[4] =(pSeed->ttak[4] + zfTkipSbox(pSeed->ttak[3]
+                         ^ ZM_BYTE_TO_WORD(pSeed->tk[1+j], pSeed->tk[j]  )))
+                        & 0xffff;
+        pSeed->ttak[4] =(pSeed->ttak[4] + i) & 0xffff;
+    }
+
+    if ( iv32 == (pSeed->iv32+1) )
+    {
+        pSeed->iv32tmp = iv32;
+        return 1;
+    }
+
+    return 0;
+}
+
+u8_t zfTkipPhase2KeyMix(u16_t iv16, struct zsTkipSeed* pSeed)
+{
+    u16_t tsc2;
+
+    tsc2 = iv16;
+
+    /* Phase 2, Step 1 */
+    pSeed->ppk[0] = pSeed->ttak[0];
+    pSeed->ppk[1] = pSeed->ttak[1];
+    pSeed->ppk[2] = pSeed->ttak[2];
+    pSeed->ppk[3] = pSeed->ttak[3];
+    pSeed->ppk[4] = pSeed->ttak[4];
+    pSeed->ppk[5] = (pSeed->ttak[4] + tsc2) & 0xffff;
+
+    /* Phase2, Step 2 */
+    pSeed->ppk[0] = pSeed->ppk[0]
+                + zfTkipSbox(pSeed->ppk[5] ^ ZM_BYTE_TO_WORD(pSeed->tk[1],pSeed->tk[0]));
+    pSeed->ppk[1] = pSeed->ppk[1]
+                + zfTkipSbox(pSeed->ppk[0] ^ ZM_BYTE_TO_WORD(pSeed->tk[3],pSeed->tk[2]));
+    pSeed->ppk[2] = pSeed->ppk[2]
+                + zfTkipSbox(pSeed->ppk[1] ^ ZM_BYTE_TO_WORD(pSeed->tk[5],pSeed->tk[4]));
+    pSeed->ppk[3] = pSeed->ppk[3]
+                + zfTkipSbox(pSeed->ppk[2] ^ ZM_BYTE_TO_WORD(pSeed->tk[7],pSeed->tk[6]));
+    pSeed->ppk[4] = pSeed->ppk[4]
+                + zfTkipSbox(pSeed->ppk[3] ^ ZM_BYTE_TO_WORD(pSeed->tk[9],pSeed->tk[8]));
+    pSeed->ppk[5] = pSeed->ppk[5]
+                + zfTkipSbox(pSeed->ppk[4] ^ ZM_BYTE_TO_WORD(pSeed->tk[11],pSeed->tk[10]));
+
+    pSeed->ppk[0] = pSeed->ppk[0]
+                + zfrotr1(pSeed->ppk[5] ^ ZM_BYTE_TO_WORD(pSeed->tk[13],pSeed->tk[12]));
+    pSeed->ppk[1] = pSeed->ppk[1]
+                + zfrotr1(pSeed->ppk[0] ^ ZM_BYTE_TO_WORD(pSeed->tk[15],pSeed->tk[14]));
+    pSeed->ppk[2] = pSeed->ppk[2] + zfrotr1(pSeed->ppk[1]);
+    pSeed->ppk[3] = pSeed->ppk[3] + zfrotr1(pSeed->ppk[2]);
+    pSeed->ppk[4] = pSeed->ppk[4] + zfrotr1(pSeed->ppk[3]);
+    pSeed->ppk[5] = pSeed->ppk[5] + zfrotr1(pSeed->ppk[4]);
+
+    if (iv16 == 0)
+    {
+        if (pSeed->iv16 == 0xffff)
+        {
+            pSeed->iv16tmp=0;
+            return 1;
+        }
+        else
+            return 0;
+    }
+    else if (iv16 == (pSeed->iv16+1))
+    {
+        pSeed->iv16tmp = iv16;
+        return 1;
+    }
+    else
+        return 0;
+}
+
+void zfTkipInit(u8_t* key, u8_t* ta, struct zsTkipSeed* pSeed, u8_t* initIv)
+{
+    u16_t  iv16;
+    u32_t  iv32;
+    u16_t  i;
+
+    /* clear memory */
+    zfZeroMemory((u8_t*) pSeed, sizeof(struct zsTkipSeed));
+    /* set key to seed */
+    zfMemoryCopy(pSeed->ta, ta, 6);
+    zfMemoryCopy(pSeed->tk, key, 16);
+
+    iv16 = *initIv++;
+    iv16 += *initIv<<8;
+    initIv++;
+
+    iv32=0;
+
+    for(i=0; i<4; i++)      // initiv is little endian
+    {
+        iv32 += *initIv<<(i*8);
+        *initIv++;
+    }
+
+    pSeed->iv32 = iv32+1; // Force Recalculating on Tkip Phase1
+    zfTkipPhase1KeyMix(iv32, pSeed);
+
+    pSeed->iv16 = iv16;
+    pSeed->iv32 = iv32;
+}
+
+u32_t zfGetU32t(u8_t* p)
+{
+    u32_t res=0;
+    u16_t i;
+
+    for( i=0; i<4; i++ )
+    {
+        res |= (*p++) << (8*i);
+    }
+
+    return res;
+
+}
+
+void zfPutU32t(u8_t* p, u32_t value)
+{
+    u16_t i;
+
+    for(i=0; i<4; i++)
+    {
+        *p++ = (u8_t) (value & 0xff);
+        value >>= 8;
+    }
+}
+
+void zfMicClear(struct zsMicVar* pMic)
+{
+    pMic->left = pMic->k0;
+    pMic->right = pMic->k1;
+    pMic->nBytes = 0;
+    pMic->m = 0;
+}
+
+void zfMicSetKey(u8_t* key, struct zsMicVar* pMic)
+{
+    pMic->k0 = zfGetU32t(key);
+    pMic->k1 = zfGetU32t(key+4);
+    zfMicClear(pMic);
+}
+
+void zfMicAppendByte(u8_t b, struct zsMicVar* pMic)
+{
+    // Append the byte to our word-sized buffer
+    pMic->m |= b << (8* pMic->nBytes);
+    pMic->nBytes++;
+
+    // Process the word if it is full.
+    if ( pMic->nBytes >= 4 )
+    {
+        pMic->left ^= pMic->m;
+        pMic->right ^= ZM_ROL32(pMic->left, 17 );
+        pMic->left += pMic->right;
+        pMic->right ^= ((pMic->left & 0xff00ff00) >> 8) |
+                       ((pMic->left & 0x00ff00ff) << 8);
+        pMic->left += pMic->right;
+        pMic->right ^= ZM_ROL32( pMic->left, 3 );
+        pMic->left += pMic->right;
+        pMic->right ^= ZM_ROR32( pMic->left, 2 );
+        pMic->left += pMic->right;
+        // Clear the buffer
+        pMic->m = 0;
+        pMic->nBytes = 0;
+    }
+}
+
+void zfMicGetMic(u8_t* dst, struct zsMicVar* pMic)
+{
+    // Append the minimum padding
+    zfMicAppendByte(0x5a, pMic);
+    zfMicAppendByte(0, pMic);
+    zfMicAppendByte(0, pMic);
+    zfMicAppendByte(0, pMic);
+    zfMicAppendByte(0, pMic);
+
+    // and then zeroes until the length is a multiple of 4
+    while( pMic->nBytes != 0 )
+    {
+        zfMicAppendByte(0, pMic);
+    }
+
+    // The appendByte function has already computed the result.
+    zfPutU32t(dst, pMic->left);
+    zfPutU32t(dst+4, pMic->right);
+
+    // Reset to the empty message.
+    zfMicClear(pMic);
+
+}
+
+u8_t zfMicRxVerify(zdev_t* dev, zbuf_t* buf)
+{
+    struct zsMicVar*  pMicKey;
+    struct zsMicVar    MyMicKey;
+    u8_t   mic[8];
+    u8_t   da[6];
+    u8_t   sa[6];
+    u8_t   bValue;
+    u16_t  i, payloadOffset, tailOffset;
+
+    zmw_get_wlan_dev(dev);
+
+    /* need not check MIC if pMicKEy is equal to NULL */
+    if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        pMicKey = zfApGetRxMicKey(dev, buf);
+
+        if ( pMicKey != NULL )
+        {
+            zfCopyFromRxBuffer(dev, buf, sa, ZM_WLAN_HEADER_A2_OFFSET, 6);
+            zfCopyFromRxBuffer(dev, buf, da, ZM_WLAN_HEADER_A3_OFFSET, 6);
+        }
+        else
+        {
+            return ZM_MIC_SUCCESS;
+        }
+    }
+    else if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+    {
+        pMicKey = zfStaGetRxMicKey(dev, buf);
+
+        if ( pMicKey != NULL )
+        {
+            zfCopyFromRxBuffer(dev, buf, sa, ZM_WLAN_HEADER_A3_OFFSET, 6);
+            zfCopyFromRxBuffer(dev, buf, da, ZM_WLAN_HEADER_A1_OFFSET, 6);
+        }
+        else
+        {
+            return ZM_MIC_SUCCESS;
+        }
+    }
+    else
+    {
+        return ZM_MIC_SUCCESS;
+    }
+
+    MyMicKey.k0=pMicKey->k0;
+    MyMicKey.k1=pMicKey->k1;
+    pMicKey = &MyMicKey;
+
+    zfMicClear(pMicKey);
+    tailOffset = zfwBufGetSize(dev, buf);
+    tailOffset -= 8;
+
+    /* append DA */
+    for(i=0; i<6; i++)
+    {
+        zfMicAppendByte(da[i], pMicKey);
+    }
+    /* append SA */
+    for(i=0; i<6; i++)
+    {
+        zfMicAppendByte(sa[i], pMicKey);
+    }
+
+    /* append for alignment */
+    if ((zmw_rx_buf_readb(dev, buf, 0) & 0x80) != 0)
+        zfMicAppendByte(zmw_rx_buf_readb(dev, buf,24)&0x7, pMicKey);
+    else
+        zfMicAppendByte(0, pMicKey);
+    zfMicAppendByte(0, pMicKey);
+    zfMicAppendByte(0, pMicKey);
+    zfMicAppendByte(0, pMicKey);
+
+    /* append payload */
+    payloadOffset = ZM_SIZE_OF_WLAN_DATA_HEADER +
+                    ZM_SIZE_OF_IV +
+                    ZM_SIZE_OF_EXT_IV;
+
+    if ((zmw_rx_buf_readb(dev, buf, 0) & 0x80) != 0)
+    {
+        /* Qos Packet, Plcpheader + 2 */
+        if (wd->wlanMode == ZM_MODE_AP)
+        {
+            /* TODO : Rx Qos element offset in software MIC check */
+        }
+        else if (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)
+        {
+            if (wd->sta.wmeConnected != 0)
+            {
+                payloadOffset += 2;
+            }
+        }
+    }
+
+    for(i=payloadOffset; i<tailOffset; i++)
+    {
+        bValue = zmw_rx_buf_readb(dev, buf, i);
+        zfMicAppendByte(bValue, pMicKey);
+    }
+
+    zfMicGetMic(mic, pMicKey);
+
+    if ( !zfRxBufferEqualToStr(dev, buf, mic, tailOffset, 8) )
+    {
+        return ZM_MIC_FAILURE;
+    }
+
+    return ZM_MIC_SUCCESS;
+}
+
+void zfTkipGetseeds(u16_t iv16, u8_t *RC4Key, struct zsTkipSeed *Seed)
+{
+    RC4Key[0]  = ZM_HI8(iv16);
+    RC4Key[1]  = (ZM_HI8(iv16) | 0x20) & 0x7f;
+    RC4Key[2]  = ZM_LO8(iv16);
+    RC4Key[3]  = ((Seed->ppk[5] ^ ZM_BYTE_TO_WORD(Seed->tk[1],Seed->tk[0]))>>1) & 0xff;
+    RC4Key[4]  = Seed->ppk[0] & 0xff;
+    RC4Key[5]  = Seed->ppk[0] >> 8;
+    RC4Key[6]  = Seed->ppk[1] & 0xff;
+    RC4Key[7]  = Seed->ppk[1] >> 8;
+    RC4Key[8]  = Seed->ppk[2] & 0xff;
+    RC4Key[9]  = Seed->ppk[2] >> 8;
+    RC4Key[10] = Seed->ppk[3] & 0xff;
+    RC4Key[11] = Seed->ppk[3] >> 8;
+    RC4Key[12] = Seed->ppk[4] & 0xff;
+    RC4Key[13] = Seed->ppk[4] >> 8;
+    RC4Key[14] = Seed->ppk[5] & 0xff;
+    RC4Key[15] = Seed->ppk[5] >> 8;
+}
+
+void zfCalTxMic(zdev_t *dev, zbuf_t *buf, u8_t *snap, u16_t snapLen, u16_t offset, u16_t *da, u16_t *sa, u8_t up, u8_t *mic)
+{
+    struct zsMicVar*  pMicKey;
+    u16_t  i;
+    u16_t len;
+    u8_t bValue;
+    u8_t qosType;
+    u8_t *pDa = (u8_t *)da;
+    u8_t *pSa = (u8_t *)sa;
+
+    zmw_get_wlan_dev(dev);
+
+    /* need not check MIC if pMicKEy is equal to NULL */
+    if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        pMicKey = zfApGetTxMicKey(dev, buf, &qosType);
+
+        if ( pMicKey == NULL )
+            return;
+    }
+    else if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+    {
+        pMicKey = zfStaGetTxMicKey(dev, buf);
+
+        if ( pMicKey == NULL )
+        {
+            zm_debug_msg0("pMicKey is NULL");
+            return;
+        }
+    }
+    else
+    {
+        return;
+    }
+
+    zfMicClear(pMicKey);
+    len = zfwBufGetSize(dev, buf);
+
+    /* append DA */
+    for(i = 0; i < 6; i++)
+    {
+        zfMicAppendByte(pDa[i], pMicKey);
+    }
+
+    /* append SA */
+    for(i = 0; i < 6; i++)
+    {
+        zfMicAppendByte(pSa[i], pMicKey);
+    }
+
+    if (up != 0)
+        zfMicAppendByte((up&0x7), pMicKey);
+    else
+        zfMicAppendByte(0, pMicKey);
+
+    zfMicAppendByte(0, pMicKey);
+    zfMicAppendByte(0, pMicKey);
+    zfMicAppendByte(0, pMicKey);
+
+    /* For Snap header */
+    for(i = 0; i < snapLen; i++)
+    {
+        zfMicAppendByte(snap[i], pMicKey);
+    }
+
+    for(i = offset; i < len; i++)
+    {
+        bValue = zmw_tx_buf_readb(dev, buf, i);
+        zfMicAppendByte(bValue, pMicKey);
+    }
+
+    zfMicGetMic(mic, pMicKey);
+}
+
+void zfTKIPEncrypt(zdev_t *dev, zbuf_t *buf, u8_t *snap, u16_t snapLen, u16_t offset, u8_t keyLen, u8_t* key, u32_t* icv)
+{
+    u8_t iv[3];
+
+    iv[0] = key[0];
+    iv[1] = key[1];
+    iv[2] = key[2];
+
+    keyLen -= 3;
+
+    zfWEPEncrypt(dev, buf, snap, snapLen, offset, keyLen, &key[3], iv);
+}
+
+u16_t zfTKIPDecrypt(zdev_t *dev, zbuf_t *buf, u16_t offset, u8_t keyLen, u8_t* key)
+{
+    u16_t ret = ZM_ICV_SUCCESS;
+    u8_t iv[3];
+
+    iv[0] = key[0];
+    iv[1] = key[1];
+    iv[2] = key[2];
+
+    keyLen -= 3;
+
+    ret = zfWEPDecrypt(dev, buf, offset, keyLen, &key[3], iv);
+
+    return ret;
+}
diff --git a/drivers/staging/otus/80211core/ctxrx.c b/drivers/staging/otus/80211core/ctxrx.c
new file mode 100644
index 00000000000..e258a7df536
--- /dev/null
+++ b/drivers/staging/otus/80211core/ctxrx.c
@@ -0,0 +1,4096 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : htr.c                                                 */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains Tx and Rx functions.                       */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+#include "cprecomp.h"
+
+u16_t zfWlanRxValidate(zdev_t* dev, zbuf_t* buf);
+u16_t zfWlanRxFilter(zdev_t* dev, zbuf_t* buf);
+
+
+
+const u8_t zgSnapBridgeTunnel[6] = { 0xAA, 0xAA, 0x03, 0x00, 0x00, 0xF8 };
+const u8_t zgSnap8021h[6] = { 0xAA, 0xAA, 0x03, 0x00, 0x00, 0x00 };
+/* Table for converting IP DSCP P2-P0 bits to 802.11e Access Category */
+const u8_t zcUpToAc[8] = {0, 1, 1, 0, 2, 2, 3, 3}; //WMM default
+//const u8_t zcUpToAc[8] = {0, 1, 1, 0, 0, 0, 0, 0}; //For 2 TxQ
+//const u8_t zcUpToAc[8] = {0, 0, 0, 0, 0, 0, 0, 0}; //For single TxQ
+const u8_t zcMaxspToPktNum[4] = {8, 2, 4, 6};
+
+u8_t zfGetEncryModeFromRxStatus(struct zsAdditionInfo* addInfo)
+{
+    u8_t securityByte;
+    u8_t encryMode;
+
+    securityByte = (addInfo->Tail.Data.SAIndex & 0xc0) >> 4;  /* byte4 */
+    securityByte |= (addInfo->Tail.Data.DAIndex & 0xc0) >> 6; /* byte5 */
+
+    switch( securityByte )
+    {
+        case ZM_NO_WEP:
+        case ZM_WEP64:
+        case ZM_WEP128:
+        case ZM_WEP256:
+#ifdef ZM_ENABLE_CENC
+        case ZM_CENC:
+#endif //ZM_ENABLE_CENC
+        case ZM_TKIP:
+        case ZM_AES:
+
+            encryMode = securityByte;
+            break;
+
+        default:
+
+            if ( (securityByte & 0xf8) == 0x08 )
+            {
+                // decrypted by software
+            }
+
+            encryMode = ZM_NO_WEP;
+            break;
+    }
+
+    return encryMode;
+}
+
+void zfGetRxIvIcvLength(zdev_t* dev, zbuf_t* buf, u8_t vap, u16_t* pIvLen,
+                        u16_t* pIcvLen, struct zsAdditionInfo* addInfo)
+{
+    u16_t wdsPort;
+    u8_t  encryMode;
+
+    zmw_get_wlan_dev(dev);
+
+    *pIvLen = 0;
+    *pIcvLen = 0;
+
+    encryMode = zfGetEncryModeFromRxStatus(addInfo);
+
+    if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        if (vap < ZM_MAX_AP_SUPPORT)
+        {
+            if (( wd->ap.encryMode[vap] == ZM_WEP64 ) ||
+                    ( wd->ap.encryMode[vap] == ZM_WEP128 ) ||
+                    ( wd->ap.encryMode[vap] == ZM_WEP256 ))
+            {
+                *pIvLen = 4;
+                *pIcvLen = 4;
+            }
+            else
+            {
+                u16_t id;
+                u16_t addr[3];
+
+                addr[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET);
+                addr[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+2);
+                addr[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+4);
+
+                /* Find STA's information */
+                if ((id = zfApFindSta(dev, addr)) != 0xffff)
+                {
+                    if (wd->ap.staTable[id].encryMode == ZM_TKIP)
+                    {
+                        *pIvLen = 8;
+                        *pIcvLen = 4;
+                    }
+                    else if (wd->ap.staTable[id].encryMode == ZM_AES)
+                    {
+                        *pIvLen = 8;
+                        *pIcvLen = 8; // AES MIC
+                        //*pIcvLen = 0;
+                    }
+#ifdef ZM_ENABLE_CENC
+                    else if (wd->ap.staTable[id].encryMode == ZM_CENC)
+                    {
+                        *pIvLen = 18;
+                        *pIcvLen= 16;
+                    }
+#endif //ZM_ENABLE_CENC
+                }
+            }
+            /* WDS port checking */
+            if ((wdsPort = vap - 0x20) >= ZM_MAX_WDS_SUPPORT)
+            {
+                wdsPort = 0;
+            }
+
+            switch (wd->ap.wds.encryMode[wdsPort])
+			{
+			case ZM_WEP64:
+			case ZM_WEP128:
+			case ZM_WEP256:
+                *pIvLen = 4;
+                *pIcvLen = 4;
+				break;
+			case ZM_TKIP:
+                *pIvLen = 8;
+                *pIcvLen = 4;
+				break;
+			case ZM_AES:
+                *pIvLen = 8;
+                *pIcvLen = 0;
+				break;
+#ifdef ZM_ENABLE_CENC
+            case ZM_CENC:
+                *pIvLen = 18;
+                *pIcvLen = 16;
+				break;
+#endif //ZM_ENABLE_CENC
+			}/* end of switch */
+        }
+    }
+	else if ( wd->wlanMode == ZM_MODE_PSEUDO)
+    {
+        /* test: 6518 for QA auto test */
+        switch (encryMode)
+		{
+        case ZM_WEP64:
+        case ZM_WEP128:
+        case ZM_WEP256:
+            *pIvLen = 4;
+            *pIcvLen = 4;
+			break;
+		case ZM_TKIP:
+            *pIvLen = 8;
+            *pIcvLen = 4;
+			break;
+		case ZM_AES:
+            *pIvLen = 8;
+            *pIcvLen = 0;
+			break;
+#ifdef ZM_ENABLE_CENC
+        case ZM_CENC:
+            *pIvLen = 18;
+            *pIcvLen = 16;
+#endif //ZM_ENABLE_CENC
+		}/* end of switch */
+    }
+    else
+    {
+        if ( (encryMode == ZM_WEP64)||
+             (encryMode == ZM_WEP128)||
+             (encryMode == ZM_WEP256) )
+        {
+            *pIvLen = 4;
+            *pIcvLen = 4;
+        }
+        else if ( encryMode == ZM_TKIP )
+        {
+            *pIvLen = 8;
+            *pIcvLen = 4;
+        }
+        else if ( encryMode == ZM_AES )
+        {
+            *pIvLen = 8;
+            *pIcvLen = 8; // AES MIC
+        }
+#ifdef ZM_ENABLE_CENC
+        else if ( encryMode == ZM_CENC)
+        {
+            *pIvLen = 18;
+            *pIcvLen= 16;
+        }
+#endif //ZM_ENABLE_CENC
+    }
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAgingDefragList           */
+/*      Force flushing whole defrag list or aging the buffer            */
+/*      in the defrag list.                                             */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      flushFlag : 1=>flushing, 0=>Aging                               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.1      */
+/*                                                                      */
+/************************************************************************/
+void zfAgingDefragList(zdev_t* dev, u16_t flushFlag)
+{
+    u16_t i, j;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    for(i=0; i<ZM_MAX_DEFRAG_ENTRIES; i++)
+    {
+        if (wd->defragTable.defragEntry[i].fragCount != 0 )
+        {
+            if (((wd->tick - wd->defragTable.defragEntry[i].tick) >
+                        (ZM_DEFRAG_AGING_TIME_SEC * ZM_TICK_PER_SECOND))
+               || (flushFlag != 0))
+            {
+                zm_msg1_rx(ZM_LV_2, "Aging defrag list :", i);
+                /* Free the buffers in the defrag list */
+                for (j=0; j<wd->defragTable.defragEntry[i].fragCount; j++)
+                {
+                    zfwBufFree(dev, wd->defragTable.defragEntry[i].fragment[j], 0);
+                }
+            }
+        }
+        wd->defragTable.defragEntry[i].fragCount = 0;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAddFirstFragToDefragList  */
+/*      Add first fragment to defragment list, the first empty entry    */
+/*      will be selected. If the list is full, sequentially select      */
+/*      one entry for replacement.                                      */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : first fragment buffer                                     */
+/*      addr : address of first fragment buffer                         */
+/*      seqNum : sequence of first fragment buffer                      */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.1      */
+/*                                                                      */
+/************************************************************************/
+void zfAddFirstFragToDefragList(zdev_t* dev, zbuf_t* buf, u8_t* addr, u16_t seqNum)
+{
+    u16_t i, j;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    /* Find an empty one in defrag list */
+    for(i=0; i<ZM_MAX_DEFRAG_ENTRIES; i++)
+    {
+        if ( wd->defragTable.defragEntry[i].fragCount == 0 )
+        {
+            break;
+        }
+    }
+
+    /* If full, sequentially replace existing one */
+    if (i == ZM_MAX_DEFRAG_ENTRIES)
+    {
+        i = wd->defragTable.replaceNum++ & (ZM_MAX_DEFRAG_ENTRIES-1);
+        /* Free the buffers in the defrag list to be replaced */
+        for (j=0; j<wd->defragTable.defragEntry[i].fragCount; j++)
+        {
+            zfwBufFree(dev, wd->defragTable.defragEntry[i].fragment[j], 0);
+        }
+    }
+
+    wd->defragTable.defragEntry[i].fragCount = 1;
+    wd->defragTable.defragEntry[i].fragment[0] = buf;
+    wd->defragTable.defragEntry[i].seqNum = seqNum;
+    wd->defragTable.defragEntry[i].tick = wd->tick;
+
+    for (j=0; j<6; j++)
+    {
+        wd->defragTable.defragEntry[i].addr[j] = addr[j];
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfAddFragToDefragList       */
+/*      Add middle or last fragment to defragment list.                 */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : first fragment buffer                                     */
+/*      addr : address of fragment buffer                               */
+/*      seqNum : sequence fragment buffer                               */
+/*      fragNum : fragment number of fragment buffer                    */
+/*      moreFrag : more frag bit of fragment buffer                     */
+/*      addInfo : addition info of fragment buffer                      */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.1      */
+/*                                                                      */
+/************************************************************************/
+zbuf_t* zfAddFragToDefragList(zdev_t* dev, zbuf_t* buf, u8_t* addr,
+        u16_t seqNum, u8_t fragNum, u8_t moreFrag,
+        struct zsAdditionInfo* addInfo)
+{
+    u16_t i, j, k;
+    zbuf_t* returnBuf = NULL;
+    u16_t defragDone = 0;
+    u16_t lenErr = 0;
+    u16_t startAddr, fragHead, frameLen, ivLen, icvLen;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    /* Find frag in the defrag list */
+    for(i=0; i<ZM_MAX_DEFRAG_ENTRIES; i++)
+    {
+        if ( wd->defragTable.defragEntry[i].fragCount != 0 )
+        {
+            /* Compare address */
+            for (j=0; j<6; j++)
+            {
+                if (addr[j] != wd->defragTable.defragEntry[i].addr[j])
+                {
+                    break;
+                }
+            }
+            if (j == 6)
+            {
+                /* Compare sequence and fragment number */
+                if (seqNum == wd->defragTable.defragEntry[i].seqNum)
+                {
+                    if ((fragNum == wd->defragTable.defragEntry[i].fragCount)
+                                && (fragNum < 8))
+                    {
+                        /* Add frag frame to defrag list */
+                        wd->defragTable.defragEntry[i].fragment[fragNum] = buf;
+                        wd->defragTable.defragEntry[i].fragCount++;
+                        defragDone = 1;
+
+                        if (moreFrag == 0)
+                        {
+                            /* merge all fragment if more data bit is cleared */
+                            returnBuf = wd->defragTable.defragEntry[i].fragment[0];
+                            startAddr = zfwBufGetSize(dev, returnBuf);
+                            /* skip WLAN header 24(Data) or 26(QoS Data) */
+                            fragHead = 24 + ((zmw_rx_buf_readh(dev, returnBuf, 0) & 0x80) >> 6);
+                            zfGetRxIvIcvLength(dev, returnBuf, 0, &ivLen, &icvLen, addInfo);
+                            fragHead += ivLen; /* skip IV */
+                            for(k=1; k<wd->defragTable.defragEntry[i].fragCount; k++)
+                            {
+                                frameLen = zfwBufGetSize(dev,
+                                                         wd->defragTable.defragEntry[i].fragment[k]);
+                                if ((startAddr+frameLen-fragHead) < 1560)
+                                {
+                                    zfRxBufferCopy(dev, returnBuf, wd->defragTable.defragEntry[i].fragment[k],
+                                               startAddr, fragHead, frameLen-fragHead);
+                                    startAddr += (frameLen-fragHead);
+                                }
+                                else
+                                {
+                                    lenErr = 1;
+                                }
+                                zfwBufFree(dev, wd->defragTable.defragEntry[i].fragment[k], 0);
+                            }
+
+                            wd->defragTable.defragEntry[i].fragCount = 0;
+                            zfwBufSetSize(dev, returnBuf, startAddr);
+                        }
+                        break;
+                    }
+                }
+            }
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+
+    if (lenErr == 1)
+    {
+        zfwBufFree(dev, returnBuf, 0);
+        return NULL;
+    }
+    if (defragDone == 0)
+    {
+        zfwBufFree(dev, buf, 0);
+        return NULL;
+    }
+
+    return returnBuf;
+}
+
+
+/* return value = NULL => save or free this frame         */
+zbuf_t* zfDefragment(zdev_t* dev, zbuf_t* buf, u8_t* pbIsDefrag,
+                     struct zsAdditionInfo* addInfo)
+{
+    u8_t fragNum;
+    u16_t seqNum;
+    u8_t moreFragBit;
+    u8_t addr[6];
+    u16_t i;
+    zmw_get_wlan_dev(dev);
+
+    ZM_BUFFER_TRACE(dev, buf)
+
+    *pbIsDefrag = FALSE;
+    seqNum = zmw_buf_readh(dev, buf, 22);
+    fragNum = (u8_t)(seqNum & 0xf);
+    moreFragBit = (zmw_buf_readb(dev, buf, 1) & ZM_BIT_2) >> 2;
+
+    if ((fragNum == 0) && (moreFragBit == 0))
+    {
+        /* Not part of a fragmentation */
+
+        return buf;
+    }
+    else
+    {
+        wd->commTally.swRxFragmentCount++;
+        seqNum = seqNum >> 4;
+        for (i=0; i<6; i++)
+        {
+            addr[i] = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+i);
+        }
+
+        if (fragNum == 0)
+        {
+            /* more frag = 1 */
+            /* First part of a fragmentation */
+            zm_msg1_rx(ZM_LV_2, "First Frag, seq=", seqNum);
+            zfAddFirstFragToDefragList(dev, buf, addr, seqNum);
+            buf = NULL;
+        }
+        else
+        {
+            /* Middle or last part of a fragmentation */
+            zm_msg1_rx(ZM_LV_2, "Frag seq=", seqNum);
+            zm_msg1_rx(ZM_LV_2, "Frag moreFragBit=", moreFragBit);
+            buf = zfAddFragToDefragList(dev, buf, addr, seqNum, fragNum, moreFragBit, addInfo);
+            if (buf != NULL)
+            {
+                *pbIsDefrag = TRUE;
+            }
+        }
+    }
+
+    return buf;
+}
+
+
+#if ZM_PROTOCOL_RESPONSE_SIMULATION
+u16_t zfSwap(u16_t num)
+{
+    return ((num >> 8) + ((num & 0xff) << 8));
+}
+
+
+void zfProtRspSim(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t ethType;
+    u16_t arpOp;
+    u16_t prot;
+    u16_t temp;
+    u16_t i;
+    u16_t dip[2];
+    u16_t dstPort;
+    u16_t srcPort;
+
+    ethType = zmw_rx_buf_readh(dev, buf, 12);
+    zm_msg2_rx(ZM_LV_2, "ethType=", ethType);
+
+    /* ARP */
+    if (ethType == 0x0608)
+    {
+        arpOp = zmw_rx_buf_readh(dev, buf, 20);
+        dip[0] = zmw_rx_buf_readh(dev, buf, 38);
+        dip[1] = zmw_rx_buf_readh(dev, buf, 40);
+        zm_msg2_rx(ZM_LV_2, "arpOp=", arpOp);
+        zm_msg2_rx(ZM_LV_2, "ip0=", dip[0]);
+        zm_msg2_rx(ZM_LV_2, "ip1=", dip[1]);
+
+        //ARP request to 192.168.1.15
+        if ((arpOp == 0x0100) && (dip[0] == 0xa8c0) && (dip[1] == 0x0f01));
+        {
+            zm_msg0_rx(ZM_LV_2, "ARP");
+            /* ARP response */
+            zmw_rx_buf_writeh(dev, buf, 20, 0x0200);
+
+            /* dst hardware address */
+
+            /* src hardware address */
+            //zmw_rx_buf_writeh(dev, buf, 6, 0xa000);
+            //zmw_rx_buf_writeh(dev, buf, 8, 0x0000);
+            //zmw_rx_buf_writeh(dev, buf, 10, 0x0000);
+
+            /* dst ip address */
+            for (i=0; i<5; i++)
+            {
+                temp = zmw_rx_buf_readh(dev, buf, 22+(i*2));
+                zmw_rx_buf_writeh(dev, buf, 32+(i*2), temp);
+            }
+
+            /* src hardware address */
+            zmw_rx_buf_writeh(dev, buf, 22, 0xa000);
+            zmw_rx_buf_writeh(dev, buf, 24, 0x0000);
+            zmw_rx_buf_writeh(dev, buf, 26, 0x0000);
+
+            /* src ip address */
+            zmw_rx_buf_writeh(dev, buf, 28, 0xa8c0);
+            zmw_rx_buf_writeh(dev, buf, 30, 0x0f01);
+        }
+    }
+    /* ICMP */
+    else if (ethType == 0x0008)
+    {
+        zm_msg0_rx(ZM_LV_2, "IP");
+        prot = zmw_rx_buf_readb(dev, buf, 23);
+        dip[0] = zmw_rx_buf_readh(dev, buf, 30);
+        dip[1] = zmw_rx_buf_readh(dev, buf, 32);
+        zm_msg2_rx(ZM_LV_2, "prot=", prot);
+        zm_msg2_rx(ZM_LV_2, "ip0=", dip[0]);
+        zm_msg2_rx(ZM_LV_2, "ip1=", dip[1]);
+
+        /* PING request to 192.168.1.15 */
+        if ((prot == 0x1) && (dip[0] == 0xa8c0) && (dip[1] == 0x0f01))
+        {
+            zm_msg0_rx(ZM_LV_2, "ICMP");
+            /* change dst */
+            for (i=0; i<3; i++)
+            {
+                temp = zmw_rx_buf_readh(dev, buf, 6+(i*2));
+                zmw_rx_buf_writeh(dev, buf, i*2, temp);
+            }
+            /* change src */
+            zmw_rx_buf_writeh(dev, buf, 6, 0xa000);
+            zmw_rx_buf_writeh(dev, buf, 8, 0x0000);
+            zmw_rx_buf_writeh(dev, buf, 10, 0x0000);
+
+            /* exchange src ip and dst ip */
+            for (i=0; i<2; i++)
+            {
+                temp = zmw_rx_buf_readh(dev, buf, 26+(i*2));
+                zmw_rx_buf_writeh(dev, buf, 30+(i*2), temp);
+            }
+            zmw_rx_buf_writeh(dev, buf, 26, 0xa8c0);
+            zmw_rx_buf_writeh(dev, buf, 28, 0x0f01);
+
+            /* change icmp type to echo reply */
+            zmw_rx_buf_writeb(dev, buf, 34, 0x0);
+
+            /* update icmp checksum */
+            temp = zmw_rx_buf_readh(dev, buf, 36);
+            temp += 8;
+            zmw_rx_buf_writeh(dev, buf, 36, temp);
+        }
+        else if (prot == 0x6)
+        {
+            zm_msg0_rx(ZM_LV_2, "TCP");
+            srcPort = zmw_rx_buf_readh(dev, buf, 34);
+            dstPort = zmw_rx_buf_readh(dev, buf, 36);
+            zm_msg2_rx(ZM_LV_2, "Src Port=", srcPort);
+            zm_msg2_rx(ZM_LV_2, "Dst Port=", dstPort);
+            if ((dstPort == 0x1500) || (srcPort == 0x1500))
+            {
+                zm_msg0_rx(ZM_LV_2, "FTP");
+
+                /* change dst */
+                for (i=0; i<3; i++)
+                {
+                    temp = zmw_rx_buf_readh(dev, buf, 6+(i*2));
+                    zmw_rx_buf_writeh(dev, buf, i*2, temp);
+                }
+                /* change src */
+                zmw_rx_buf_writeh(dev, buf, 6, 0xa000);
+                zmw_rx_buf_writeh(dev, buf, 8, 0x0000);
+                zmw_rx_buf_writeh(dev, buf, 10, 0x0000);
+
+                /* exchange src ip and dst ip */
+                for (i=0; i<2; i++)
+                {
+                    temp = zmw_rx_buf_readh(dev, buf, 26+(i*2));
+                    zmw_rx_buf_writeh(dev, buf, 30+(i*2), temp);
+                }
+                zmw_rx_buf_writeh(dev, buf, 26, 0xa8c0);
+                zmw_rx_buf_writeh(dev, buf, 28, 0x0f01);
+#if 0
+                /* Patch src port */
+                temp = zmw_rx_buf_readh(dev, buf, 34);
+                temp = zfSwap(zfSwap(temp) + 1);
+                zmw_rx_buf_writeh(dev, buf, 34, temp);
+                temp = zmw_rx_buf_readh(dev, buf, 38);
+                temp = zfSwap(zfSwap(temp) + 1);
+                zmw_rx_buf_writeh(dev, buf, 38, temp);
+
+                /* Patch checksum */
+                temp = zmw_rx_buf_readh(dev, buf, 50);
+                temp = zfSwap(temp);
+                temp = ~temp;
+                temp += 2;
+                temp = ~temp;
+                temp = zfSwap(temp);
+                zmw_rx_buf_writeh(dev, buf, 50, temp);
+#endif
+            }
+
+        }
+        else if (prot == 0x11)
+        {
+            /* change dst */
+            for (i=0; i<3; i++)
+            {
+                temp = zmw_rx_buf_readh(dev, buf, 6+(i*2));
+                zmw_rx_buf_writeh(dev, buf, i*2, temp);
+            }
+            /* change src */
+            zmw_rx_buf_writeh(dev, buf, 6, 0xa000);
+            zmw_rx_buf_writeh(dev, buf, 8, 0x0000);
+            zmw_rx_buf_writeh(dev, buf, 10, 0x0000);
+
+            zm_msg0_rx(ZM_LV_2, "UDP");
+            srcPort = zmw_rx_buf_readh(dev, buf, 34);
+            dstPort = zmw_rx_buf_readh(dev, buf, 36);
+            zm_msg2_rx(ZM_LV_2, "Src Port=", srcPort);
+            zm_msg2_rx(ZM_LV_2, "Dst Port=", dstPort);
+
+            /* exchange src ip and dst ip */
+            for (i=0; i<2; i++)
+            {
+                temp = zmw_rx_buf_readh(dev, buf, 26+(i*2));
+                zmw_rx_buf_writeh(dev, buf, 30+(i*2), temp);
+            }
+            zmw_rx_buf_writeh(dev, buf, 26, 0xa8c0);
+            zmw_rx_buf_writeh(dev, buf, 28, 0x0f01);
+
+            /* exchange port */
+            zmw_rx_buf_writeh(dev, buf, 34, srcPort+1);
+            zmw_rx_buf_writeh(dev, buf, 36, dstPort);
+
+            /* checksum = 0 */
+            zmw_rx_buf_writeh(dev, buf, 40, 0);
+        }
+
+    }
+    else if (ethType == 0x0060) /* =>0x0060 is port */
+    {
+        /* change src for Evl tool loop back receive */
+        zmw_rx_buf_writeh(dev, buf, 6, 0xa000);
+        zmw_rx_buf_writeh(dev, buf, 8, 0x0000);
+        zmw_rx_buf_writeh(dev, buf, 10, 0x0000);
+    }
+
+}
+#endif
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiTxSendEth                */
+/*      Called to native 802.11 management frames                       */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer pointer                                            */
+/*      port : WLAN port, 0=>standard, 0x1-0x7=>VAP, 0x20-0x25=>WDS     */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      error code                                                      */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Ray             ZyDAS Technology Corporation    2005.5      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfiTxSend80211Mgmt(zdev_t* dev, zbuf_t* buf, u16_t port)
+{
+    u16_t err;
+    //u16_t addrTblSize = 0;
+    //struct zsAddrTbl addrTbl;
+    u16_t hlen;
+    u16_t header[(24+25+1)/2];
+    int i;
+
+    for(i=0;i<12;i++)
+    {
+        header[i] = zmw_buf_readh(dev, buf, i);
+    }
+    hlen = 24;
+
+    zfwBufRemoveHead(dev, buf, 24);
+
+    if ((err = zfHpSend(dev, header, hlen, NULL, 0, NULL, 0, buf, 0,
+            ZM_EXTERNAL_ALLOC_BUF, 0, 0)) != ZM_SUCCESS)
+    {
+        goto zlError;
+    }
+
+    return 0;
+
+zlError:
+
+    zfwBufFree(dev, buf, 0);
+    return 0;
+}
+
+u8_t zfiIsTxQueueFull(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    if ((((wd->vtxqHead[0] + 1) & ZM_VTXQ_SIZE_MASK) != wd->vtxqTail[0]) )
+    {
+        zmw_leave_critical_section(dev);
+        return 0;
+    }
+    else
+    {
+        zmw_leave_critical_section(dev);
+        return 1;
+    }
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiTxSendEth                */
+/*      Called to transmit Ethernet frame from upper layer.             */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer pointer                                            */
+/*      port : WLAN port, 0=>standard, 0x1-0x7=>VAP, 0x20-0x25=>WDS     */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      error code                                                      */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen             ZyDAS Technology Corporation    2005.5      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfiTxSendEth(zdev_t* dev, zbuf_t* buf, u16_t port)
+{
+    u16_t err, ret;
+
+    zmw_get_wlan_dev(dev);
+
+    ZM_PERFORMANCE_TX_MSDU(dev, wd->tick);
+    zm_msg1_tx(ZM_LV_2, "zfiTxSendEth(), port=", port);
+    /* Return error if port is disabled */
+    if ((err = zfTxPortControl(dev, buf, port)) == ZM_PORT_DISABLED)
+    {
+        err = ZM_ERR_TX_PORT_DISABLED;
+        goto zlError;
+    }
+
+#if 1
+    if ((wd->wlanMode == ZM_MODE_AP) && (port < 0x20))
+    {
+        /* AP : Buffer frame for power saving STA */
+        if ((ret = zfApBufferPsFrame(dev, buf, port)) == 1)
+        {
+            return ZM_SUCCESS;
+        }
+    }
+    else
+#endif
+    if (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)
+    {
+        if ( zfPowerSavingMgrIsSleeping(dev) )
+        {
+            /*check ZM_ENABLE_POWER_SAVE flag*/
+            zfPowerSavingMgrWakeup(dev);
+        }
+    }
+#ifdef ZM_ENABLE_IBSS_PS
+    /* IBSS power-saving mode */
+    else if ( wd->wlanMode == ZM_MODE_IBSS )
+    {
+        if ( zfStaIbssPSQueueData(dev, buf) )
+        {
+            return ZM_SUCCESS;
+        }
+    }
+#endif
+
+#if 1
+    //if ( wd->bQoSEnable )
+    if (1)
+    {
+        /* Put to VTXQ[ac] */
+        ret = zfPutVtxq(dev, buf);
+
+        /* Push VTXQ[ac] */
+        zfPushVtxq(dev);
+    }
+    else
+    {
+        ret = zfTxSendEth(dev, buf, port, ZM_EXTERNAL_ALLOC_BUF, 0);
+    }
+
+    return ret;
+#else
+    return zfTxSendEth(dev, buf, port, ZM_EXTERNAL_ALLOC_BUF, 0);
+#endif
+
+zlError:
+    zm_msg2_tx(ZM_LV_1, "Tx Comp err=", err);
+
+    zfwBufFree(dev, buf, err);
+    return err;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfTxSendEth                 */
+/*      Called to transmit Ethernet frame from upper layer.             */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : buffer pointer                                            */
+/*      port : WLAN port, 0=>standard, 0x10-0x17=>VAP, 0x20-0x25=>WDS   */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      error code                                                      */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen             ZyDAS Technology Corporation    2005.5      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfTxSendEth(zdev_t* dev, zbuf_t* buf, u16_t port, u16_t bufType, u16_t flag)
+{
+    u16_t err;
+    //u16_t addrTblSize;
+    //struct zsAddrTbl addrTbl;
+    u16_t removeLen;
+    u16_t header[(8+30+2+18)/2];    /* ctr+(4+a1+a2+a3+2+a4)+qos+iv */
+    u16_t headerLen;
+    u16_t mic[8/2];
+    u16_t micLen;
+    u16_t snap[8/2];
+    u16_t snapLen;
+    u16_t fragLen;
+    u16_t frameLen;
+    u16_t fragNum;
+    struct zsFrag frag;
+    u16_t i, j, id;
+    u16_t offset;
+    u16_t da[3];
+    u16_t sa[3];
+    u8_t up;
+    u8_t qosType, keyIdx = 0;
+    u16_t fragOff;
+    u16_t newFlag;
+    struct zsMicVar*  pMicKey;
+    u8_t tkipFrameOffset = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    newFlag = flag & 0xff00;
+    flag = flag & 0xff;
+
+    zm_msg1_tx(ZM_LV_2, "zfTxSendEth(), port=", port);
+
+    /* Get IP TOS for QoS AC and IP frag offset */
+    zfTxGetIpTosAndFrag(dev, buf, &up, &fragOff);
+
+    //EOSP bit
+    if (newFlag & 0x100)
+    {
+        up |= 0x10;
+    }
+
+#ifdef ZM_ENABLE_NATIVE_WIFI
+    if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+    {
+        /* DA */
+        da[0] = zmw_tx_buf_readh(dev, buf, 16);
+        da[1] = zmw_tx_buf_readh(dev, buf, 18);
+        da[2] = zmw_tx_buf_readh(dev, buf, 20);
+        /* SA */
+        sa[0] = zmw_tx_buf_readh(dev, buf, 10);
+        sa[1] = zmw_tx_buf_readh(dev, buf, 12);
+        sa[2] = zmw_tx_buf_readh(dev, buf, 14);
+    }
+    else if ( wd->wlanMode == ZM_MODE_IBSS )
+    {
+        /* DA */
+        da[0] = zmw_tx_buf_readh(dev, buf, 4);
+        da[1] = zmw_tx_buf_readh(dev, buf, 6);
+        da[2] = zmw_tx_buf_readh(dev, buf, 8);
+        /* SA */
+        sa[0] = zmw_tx_buf_readh(dev, buf, 10);
+        sa[1] = zmw_tx_buf_readh(dev, buf, 12);
+        sa[2] = zmw_tx_buf_readh(dev, buf, 14);
+    }
+    else if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        /* DA */
+        da[0] = zmw_tx_buf_readh(dev, buf, 4);
+        da[1] = zmw_tx_buf_readh(dev, buf, 6);
+        da[2] = zmw_tx_buf_readh(dev, buf, 8);
+        /* SA */
+        sa[0] = zmw_tx_buf_readh(dev, buf, 16);
+        sa[1] = zmw_tx_buf_readh(dev, buf, 18);
+        sa[2] = zmw_tx_buf_readh(dev, buf, 20);
+    }
+    else
+    {
+        //
+    }
+#else
+    /* DA */
+    da[0] = zmw_tx_buf_readh(dev, buf, 0);
+    da[1] = zmw_tx_buf_readh(dev, buf, 2);
+    da[2] = zmw_tx_buf_readh(dev, buf, 4);
+    /* SA */
+    sa[0] = zmw_tx_buf_readh(dev, buf, 6);
+    sa[1] = zmw_tx_buf_readh(dev, buf, 8);
+    sa[2] = zmw_tx_buf_readh(dev, buf, 10);
+#endif
+    //Decide Key Index in ATOM, No meaning in OTUS--CWYang(m)
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        keyIdx = wd->ap.bcHalKeyIdx[port];
+        id = zfApFindSta(dev, da);
+        if (id != 0xffff)
+        {
+            switch (wd->ap.staTable[id].encryMode)
+            {
+            case ZM_AES:
+            case ZM_TKIP:
+#ifdef ZM_ENABLE_CENC
+            case ZM_CENC:
+#endif //ZM_ENABLE_CENC
+                keyIdx = wd->ap.staTable[id].keyIdx;
+                break;
+            }
+        }
+    }
+    else
+    {
+        switch (wd->sta.encryMode)
+        {
+        case ZM_WEP64:
+        case ZM_WEP128:
+        case ZM_WEP256:
+            keyIdx = wd->sta.keyId;
+            break;
+        case ZM_AES:
+        case ZM_TKIP:
+            if ((da[0] & 0x1))
+                keyIdx = 5;
+            else
+                keyIdx = 4;
+            break;
+#ifdef ZM_ENABLE_CENC
+        case ZM_CENC:
+            keyIdx = wd->sta.cencKeyId;
+            break;
+#endif //ZM_ENABLE_CENC
+        }
+    }
+
+    /* Create SNAP */
+    removeLen = zfTxGenWlanSnap(dev, buf, snap, &snapLen);
+    //zm_msg1_tx(ZM_LV_0, "fragOff=", fragOff);
+
+
+/* ********************************************************************************************** */
+/* Add 20071025 Mxzeng                                                                            */
+/* ********************************************************************************************** */
+/* ---------------------------------------------------------------------------------------------- */
+/*  Ethernet : frameLen = zfwBufGetSize(dev, buf);                                                */
+/* ---+--6--+--6--+--2--+-----20-----+-------------------------+------ Variable -------+--------- */
+/*    |  DA |  SA | Type|  IP Header | TCP(20) UDP(12) ICMP(8) | Application Payload L |          */
+/* ---+-----+-----+-----+------------+-------------------------+-----------------------+--------- */
+/*  MSDU = 6 + 6 + 2 + ( Network Layer header ) + ( Transport Layer header ) + L                  */
+/*                                                                                                */
+/*  MSDU - DA - SA : frameLen -= removeLen;                                                       */
+/* ---+--2--+-----20-----+-------------------------+------ Variable -------+--------------------- */
+/*    | Type| IP Header  | TCP(20) UDP(12) ICMP(8) | Application Payload L |                      */
+/* ---+-----+------------+-------------------------+-----------------------+--------------------- */
+/*												  */
+/*  MPDU : frameLen + mpduLengthOffset ;                                                          */
+/* -+---2---+----2---+-6-+-6-+--6--+---2----+--1--+--1-+---1---+-------3------+-frameLen-+---4--+- */
+/*  | frame |duration| DA|SA |BSSID|sequence|SNAP |SNAP|Control|    RFC 1042  |          |  FCS |  */
+/*  |Control|        |   |   |     | number |DSAP |SSAP|       | encapsulation|          |      |  */
+/* -+-------+--------+---+---+-----+--------+-----+----+-------+--------------+----------+------+- */
+/* ----------------------------------------------------------------------------------------------- */
+
+    if ( wd->sta.encryMode == ZM_TKIP )
+        tkipFrameOffset = 8;
+
+    fragLen = wd->fragThreshold + tkipFrameOffset;   // Fragmentation threshold for MPDU Lengths
+    frameLen = zfwBufGetSize(dev, buf);    // MSDU Lengths
+    frameLen -= removeLen;                 // MSDU Lengths - DA - SA
+
+    /* #1st create MIC Length manually */
+    micLen = 0;
+
+    /* Access Category */
+    if (wd->wlanMode == ZM_MODE_AP)
+    {
+        zfApGetStaQosType(dev, da, &qosType);
+        if (qosType == 0)
+        {
+            up = 0;
+        }
+    }
+    else if (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)
+    {
+        if (wd->sta.wmeConnected == 0)
+        {
+            up = 0;
+        }
+    }
+    else
+    {
+        /* TODO : STA QoS control field */
+        up = 0;
+    }
+
+    /* #2nd Assign sequence number */
+    zmw_enter_critical_section(dev);
+    frag.seq[0] = ((wd->seq[zcUpToAc[up&0x7]]++) << 4);
+    zmw_leave_critical_section(dev);
+
+    /* #3rd Pass the total payload to generate MPDU length ! */
+    frag.buf[0] = buf;
+    frag.bufType[0] = bufType;
+    frag.flag[0] = (u8_t)flag;
+    fragNum = 1;
+
+    headerLen = zfTxGenWlanHeader(dev, frag.buf[0], header, frag.seq[0],
+                                  frag.flag[0], snapLen+micLen, removeLen, port, da, sa,
+                                  up, &micLen, snap, snapLen, NULL);
+
+    //zm_debug_msg1("#1 headerLen = ", headerLen);
+
+    /* #4th Check the HeaderLen and determine whether the MPDU Lengths bigger than Fragmentation threshold  */
+    /* If MPDU Lengths large than fragmentation threshold --> headerLen = 0 */
+    if( headerLen != 0 )
+    {
+        zf80211FrameSend(dev, frag.buf[0], header, snapLen, da, sa, up,
+                         headerLen, snap, mic, micLen, removeLen, frag.bufType[0],
+                         zcUpToAc[up&0x7], keyIdx);
+    }
+    else //if( headerLen == 0 ) // Need to be fragmented
+    {
+        u16_t mpduLengthOffset;
+        u16_t pseudSnapLen = 0;
+
+        mpduLengthOffset = header[0] - frameLen; // For fragmentation threshold !
+
+        micLen = zfTxGenWlanTail(dev, buf, snap, snapLen, mic); // Get snap and mic information
+
+        fragLen = fragLen - mpduLengthOffset;
+
+        //zm_debug_msg1("#2 frameLen = ", frameLen);
+        //zm_debug_msg1("#3 fragThreshold = ", fragLen);
+
+        /* fragmentation */
+        if (frameLen >= fragLen)
+        {
+            //copy fragLen to frag
+            i = 0;
+            while( frameLen > 0 )
+            {
+                if ((frag.buf[i] = zfwBufAllocate(dev, fragLen+32)) != NULL)
+                {
+                    frag.bufType[i] = ZM_INTERNAL_ALLOC_BUF;
+                    frag.seq[i] = frag.seq[0] + i;
+                    offset = removeLen + i*fragLen;
+
+                    /* Consider the offset if we consider snap length to the other fragmented frame */
+                    if ( i >= 1 )
+                        offset = offset + pseudSnapLen*(i-1);
+
+                    if (frameLen > fragLen + pseudSnapLen)
+                    {
+                        frag.flag[i] = flag | 0x4; /* More data */
+                        /* First fragment */
+                        if (i == 0)
+                        {
+                            /* Add SNAP */
+                            for (j=0; j<snapLen; j+=2)
+                            {
+                                zmw_tx_buf_writeh(dev, frag.buf[i], j, snap[(j>>1)]);
+                            }
+                            zfTxBufferCopy(dev, frag.buf[i], buf, snapLen, offset, fragLen);
+                            zfwBufSetSize(dev, frag.buf[i], snapLen+fragLen);
+
+                            /* Add pseud snap length to the other fragmented frame */
+                            pseudSnapLen = snapLen;
+
+                            frameLen -= fragLen;
+                        }
+                        /* Intermediate Fragment */
+                        else
+                        {
+                            //zfTxBufferCopy(dev, frag.buf[i], buf, 0, offset, fragLen);
+                            //zfwBufSetSize(dev, frag.buf[i], fragLen);
+
+                            zfTxBufferCopy(dev, frag.buf[i], buf, 0, offset, fragLen+pseudSnapLen );
+                            zfwBufSetSize(dev, frag.buf[i], fragLen+pseudSnapLen);
+
+                            frameLen -= (fragLen+pseudSnapLen);
+                        }
+                        //frameLen -= fragLen;
+                    }
+                    else
+                    {
+                        /* Last fragment  */
+                        zfTxBufferCopy(dev, frag.buf[i], buf, 0, offset, frameLen);
+                        /* Add MIC if need */
+                        if ( micLen )
+                        {
+                            zfCopyToRxBuffer(dev, frag.buf[i], (u8_t*) mic, frameLen, micLen);
+                        }
+                        zfwBufSetSize(dev, frag.buf[i], frameLen+micLen);
+                        frameLen = 0;
+                        frag.flag[i] = (u8_t)flag; /* No more data */
+                    }
+                    i++;
+                }
+                else
+                {
+                    break;
+                }
+
+                // Please pay attention to the index of the buf !!!
+                // If write to null buf , the OS will crash !!!
+                zfwCopyBufContext(dev, buf, frag.buf[i-1]);
+            }
+            fragNum = i;
+            snapLen = micLen = removeLen = 0;
+
+            zfwBufFree(dev, buf, 0);
+        }
+
+        for (i=0; i<fragNum; i++)
+        {
+            /* Create WLAN header(Control Setting + 802.11 header + IV) */
+            headerLen = zfTxGenWlanHeader(dev, frag.buf[i], header, frag.seq[i],
+                                    frag.flag[i], snapLen+micLen, removeLen, port, da, sa, up, &micLen,
+                                    snap, snapLen, NULL);
+
+            zf80211FrameSend(dev, frag.buf[i], header, snapLen, da, sa, up,
+                             headerLen, snap, mic, micLen, removeLen, frag.bufType[i],
+                             zcUpToAc[up&0x7], keyIdx);
+
+        } /* for (i=0; i<fragNum; i++) */
+    }
+
+    return ZM_SUCCESS;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfTxPortControl             */
+/*      Check port status.                                              */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      buf : buffer pointer                                            */
+/*      port : port number, 0=>standard, 10-17=>Virtual AP, 20-25=>WDS  */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      ZM_PORT_ENABLED or ZM_PORT_DISABLE                              */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Signature           ZyDAS Technology Corporation    2005.4      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfTxPortControl(zdev_t* dev, zbuf_t* buf, u16_t port)
+{
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+    {
+        if ( wd->sta.adapterState == ZM_STA_STATE_DISCONNECT )
+        {
+            zm_msg0_tx(ZM_LV_3, "Packets dropped due to disconnect state");
+            return ZM_PORT_DISABLED;
+        }
+    }
+
+    return ZM_PORT_ENABLED;
+}
+
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfIdlRecv                   */
+/*      Do frame validation and filtering then pass to zfwRecv80211().  */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : received 802.11 frame buffer.                             */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen             ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+void zfCoreRecv(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* addInfo)
+{
+    u16_t ret = 0;
+    u16_t bssid[3];
+    struct agg_tid_rx *tid_rx;
+    zmw_get_wlan_dev(dev);
+
+    ZM_BUFFER_TRACE(dev, buf)
+
+    /* tally */
+    wd->commTally.DriverRxFrmCnt++;
+
+    bssid[0] = zmw_buf_readh(dev, buf, 16);
+    bssid[1] = zmw_buf_readh(dev, buf, 18);
+    bssid[2] = zmw_buf_readh(dev, buf, 20);
+
+    /* Validate Rx frame */
+    if ((ret = zfWlanRxValidate(dev, buf)) != ZM_SUCCESS)
+    {
+        zm_msg1_rx(ZM_LV_1, "Rx invalid:", ret);
+        goto zlError;
+    }
+
+#ifdef ZM_ENABLE_AGGREGATION
+    //#ifndef ZM_ENABLE_FW_BA_RETRANSMISSION
+    /*
+     * add by honda
+     */
+    tid_rx = zfAggRxEnabled(dev, buf);
+    if (tid_rx && wd->reorder)
+    {
+        zfAggRx(dev, buf, addInfo, tid_rx);
+
+        return;
+    }
+    /*
+     * end of add by honda
+     */
+    //#endif
+#endif
+
+    /* Filter Rx frame */
+    if ((ret = zfWlanRxFilter(dev, buf)) != ZM_SUCCESS)
+    {
+        zm_msg1_rx(ZM_LV_1, "Rx duplicated:", ret);
+        goto zlError;
+    }
+
+    /* Discard error frame except mic failure */
+    if ((addInfo->Tail.Data.ErrorIndication & 0x3f) != 0)
+    {
+        if ( wd->XLinkMode && ((addInfo->Tail.Data.ErrorIndication & 0x3f)==0x10) &&
+             zfCompareWithBssid(dev, bssid) )
+        {
+            // Bypass frames !!!
+        }
+        else
+        {
+            goto zlError;
+        }
+    }
+
+
+    /* OTUS command-8212 dump rx packet */
+    if (wd->rxPacketDump)
+    {
+        zfwDumpBuf(dev, buf);
+    }
+
+    /* Call zfwRecv80211() wrapper function to deliver Rx packet */
+    /* to driver framework.                                      */
+
+    if (wd->zfcbRecv80211 != NULL)
+    {
+        wd->zfcbRecv80211(dev, buf, addInfo); //CWYang(m)
+    }
+    else
+    {
+        zfiRecv80211(dev, buf, addInfo);
+    }
+    return;
+
+zlError:
+    zm_msg1_rx(ZM_LV_1, "Free packet, error code:", ret);
+
+    wd->commTally.DriverDiscardedFrm++;
+
+    /* Free Rx buffer */
+    zfwBufFree(dev, buf, 0);
+
+    return;
+}
+
+
+void zfShowRxEAPOL(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+    u8_t   packetType, keyType, code, identifier, type, flags;
+    u16_t  packetLen, keyInfo, keyLen, keyDataLen, length, Op_Code;
+    u32_t  replayCounterH, replayCounterL, vendorId, VendorType;
+
+    /* EAPOL packet type */
+    packetType = zmw_rx_buf_readb(dev, buf, offset+1); // 0: EAP-Packet
+                                                       // 1: EAPOL-Start
+                                                       // 2: EAPOL-Logoff
+                                                       // 3: EAPOL-Key
+                                                       // 4: EAPOL-Encapsulated-ASF-Alert
+
+    /* EAPOL frame format */
+    /*  0  1  2  3  4  5  6  7  8  9 10 11 12 13 14 15   */
+    /* -----------------------------------------------   */
+    /*            PAE Ethernet Type (0x888e)             */
+    /* ----------------------------------------------- 2 */
+    /*     Protocol Version    |         Type            */
+    /* ----------------------------------------------- 4 */
+    /*                       Length                      */
+    /* ----------------------------------------------- 6 */
+    /*                    Packet Body                    */
+    /* ----------------------------------------------- N */
+
+    /* EAPOL body length */
+    packetLen = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+2)) << 8) +
+                zmw_rx_buf_readb(dev, buf, offset+3);
+
+    if( packetType == 0 )
+    { // EAP-Packet
+
+        /* EAP-Packet Code */
+        code = zmw_rx_buf_readb(dev, buf, offset+4); // 1 : Request
+                                                     // 2 : Response
+                                                     // 3 : Success
+                                                     // 4 : Failure
+        // An EAP packet of the type of Success and Failure has no Data field, and has a length of 4.
+
+        /* EAP Packet format */
+        /*  0  1  2  3  4  5  6  7  8  9 10 11 12 13 14 15   */
+        /* -----------------------------------------------   */
+        /*           Code          |        Identifier       */
+        /* ----------------------------------------------- 2 */
+        /*                       Length                      */
+        /* ----------------------------------------------- 4 */
+        /*                        Data                       */
+        /* ----------------------------------------------- N */
+
+        zm_debug_msg0("EAP-Packet");
+        zm_debug_msg1("Packet Length = ", packetLen);
+        zm_debug_msg1("EAP-Packet Code = ", code);
+
+        if( code == 1 )
+        {
+            zm_debug_msg0("EAP-Packet Request");
+
+            /* EAP-Packet Identifier */
+            identifier = zmw_rx_buf_readb(dev, buf, offset+5);
+            /* EAP-Packet Length */
+            length = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+6)) << 8) +
+                      zmw_rx_buf_readb(dev, buf, offset+7);
+            /* EAP-Packet Type */
+            type = zmw_rx_buf_readb(dev, buf, offset+8); // 1   : Identity
+                                                         // 2   : Notification
+                                                         // 3   : Nak (Response Only)
+                                                         // 4   : MD5-Challenge
+                                                         // 5   : One Time Password (OTP)
+                                                         // 6   : Generic Token Card (GTC)
+                                                         // 254 : (Expanded Types)Wi-Fi Protected Setup
+                                                         // 255 : Experimental Use
+
+            /* The data field in an EAP packet of the type of Request or Response is in the format shown bellowing */
+            /*  0  1  2  3  4  5  6  7             N             */
+            /* -----------------------------------------------   */
+            /*           Type          |        Type Data        */
+            /* -----------------------------------------------   */
+
+            zm_debug_msg1("EAP-Packet Identifier = ", identifier);
+            zm_debug_msg1("EAP-Packet Length = ", length);
+            zm_debug_msg1("EAP-Packet Type = ", type);
+
+            if( type == 1 )
+            {
+                zm_debug_msg0("EAP-Packet Request Identity");
+            }
+            else if( type == 2 )
+            {
+                zm_debug_msg0("EAP-Packet Request Notification");
+            }
+            else if( type == 4 )
+            {
+                zm_debug_msg0("EAP-Packet Request MD5-Challenge");
+            }
+            else if( type == 5 )
+            {
+                zm_debug_msg0("EAP-Packet Request One Time Password");
+            }
+            else if( type == 6 )
+            {
+                zm_debug_msg0("EAP-Packet Request Generic Token Card");
+            }
+            else if( type == 254 )
+            {
+                zm_debug_msg0("EAP-Packet Request Wi-Fi Protected Setup");
+
+                /* 0                   1                   2                   3   */
+                /* 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 */
+                /*+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+*/
+                /*|     Type      |               Vendor-Id                       |*/
+                /*+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+*/
+                /*|                          Vendor-Type                          |*/
+                /*+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+*/
+                /*|              Vendor data...                                    */
+                /*+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+                        */
+
+                /* EAP-Packet Vendor ID */
+                vendorId = (((u32_t) zmw_rx_buf_readb(dev, buf, offset+9)) << 16) +
+                           (((u32_t) zmw_rx_buf_readb(dev, buf, offset+10)) << 8) +
+                           zmw_rx_buf_readb(dev, buf, offset+11);
+                /* EAP-Packet Vendor Type */
+                VendorType = (((u32_t) zmw_rx_buf_readb(dev, buf, offset+12)) << 24) +
+                             (((u32_t) zmw_rx_buf_readb(dev, buf, offset+13)) << 16) +
+                             (((u32_t) zmw_rx_buf_readb(dev, buf, offset+14)) << 8) +
+                             zmw_rx_buf_readb(dev, buf, offset+15);
+                /* EAP-Packet Op Code */
+                Op_Code = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+16)) << 8) +
+                          zmw_rx_buf_readb(dev, buf, offset+17);
+                /* EAP-Packet Flags */
+                flags = zmw_rx_buf_readb(dev, buf, offset+18);
+
+                zm_debug_msg1("EAP-Packet Vendor ID = ", vendorId);
+                zm_debug_msg1("EAP-Packet Venodr Type = ", VendorType);
+                zm_debug_msg1("EAP-Packet Op Code = ", Op_Code);
+                zm_debug_msg1("EAP-Packet Flags = ", flags);
+            }
+        }
+        else if( code == 2 )
+        {
+            zm_debug_msg0("EAP-Packet Response");
+
+            /* EAP-Packet Identifier */
+            identifier = zmw_rx_buf_readb(dev, buf, offset+5);
+            /* EAP-Packet Length */
+            length = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+6)) << 8) +
+                      zmw_rx_buf_readb(dev, buf, offset+7);
+            /* EAP-Packet Type */
+            type = zmw_rx_buf_readb(dev, buf, offset+8);
+
+            zm_debug_msg1("EAP-Packet Identifier = ", identifier);
+            zm_debug_msg1("EAP-Packet Length = ", length);
+            zm_debug_msg1("EAP-Packet Type = ", type);
+
+            if( type == 1 )
+            {
+                zm_debug_msg0("EAP-Packet Response Identity");
+            }
+            else if( type == 2 )
+            {
+                zm_debug_msg0("EAP-Packet Request Notification");
+            }
+            else if( type == 3 )
+            {
+                zm_debug_msg0("EAP-Packet Request Nak");
+            }
+            else if( type == 4 )
+            {
+                zm_debug_msg0("EAP-Packet Request MD5-Challenge");
+            }
+            else if( type == 5 )
+            {
+                zm_debug_msg0("EAP-Packet Request One Time Password");
+            }
+            else if( type == 6 )
+            {
+                zm_debug_msg0("EAP-Packet Request Generic Token Card");
+            }
+            else if( type == 254 )
+            {
+                zm_debug_msg0("EAP-Packet Response Wi-Fi Protected Setup");
+
+                /* EAP-Packet Vendor ID */
+                vendorId = (((u32_t) zmw_rx_buf_readb(dev, buf, offset+9)) << 16) +
+                           (((u32_t) zmw_rx_buf_readb(dev, buf, offset+10)) << 8) +
+                           zmw_rx_buf_readb(dev, buf, offset+11);
+                /* EAP-Packet Vendor Type */
+                VendorType = (((u32_t) zmw_rx_buf_readb(dev, buf, offset+12)) << 24) +
+                             (((u32_t) zmw_rx_buf_readb(dev, buf, offset+13)) << 16) +
+                             (((u32_t) zmw_rx_buf_readb(dev, buf, offset+14)) << 8) +
+                             zmw_rx_buf_readb(dev, buf, offset+15);
+                /* EAP-Packet Op Code */
+                Op_Code = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+16)) << 8) +
+                          zmw_rx_buf_readb(dev, buf, offset+17);
+                /* EAP-Packet Flags */
+                flags = zmw_rx_buf_readb(dev, buf, offset+18);
+
+                zm_debug_msg1("EAP-Packet Vendor ID = ", vendorId);
+                zm_debug_msg1("EAP-Packet Venodr Type = ", VendorType);
+                zm_debug_msg1("EAP-Packet Op Code = ", Op_Code);
+                zm_debug_msg1("EAP-Packet Flags = ", flags);
+            }
+        }
+        else if( code == 3 )
+        {
+            zm_debug_msg0("EAP-Packet Success");
+
+            /* EAP-Packet Identifier */
+            identifier = zmw_rx_buf_readb(dev, buf, offset+5);
+            /* EAP-Packet Length */
+            length = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+6)) << 8) +
+                      zmw_rx_buf_readb(dev, buf, offset+7);
+
+            zm_debug_msg1("EAP-Packet Identifier = ", identifier);
+            zm_debug_msg1("EAP-Packet Length = ", length);
+        }
+        else if( code == 4 )
+        {
+            zm_debug_msg0("EAP-Packet Failure");
+
+            /* EAP-Packet Identifier */
+            identifier = zmw_rx_buf_readb(dev, buf, offset+5);
+            /* EAP-Packet Length */
+            length = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+6)) << 8) +
+                      zmw_rx_buf_readb(dev, buf, offset+7);
+
+            zm_debug_msg1("EAP-Packet Identifier = ", identifier);
+            zm_debug_msg1("EAP-Packet Length = ", length);
+        }
+    }
+    else if( packetType == 1 )
+    { // EAPOL-Start
+        zm_debug_msg0("EAPOL-Start");
+    }
+    else if( packetType == 2 )
+    { // EAPOL-Logoff
+        zm_debug_msg0("EAPOL-Logoff");
+    }
+    else if( packetType == 3 )
+    { // EAPOL-Key
+        /* EAPOL-Key type */
+        keyType = zmw_rx_buf_readb(dev, buf, offset+4);
+        /* EAPOL-Key information */
+        keyInfo = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+5)) << 8) +
+                  zmw_rx_buf_readb(dev, buf, offset+6);
+        /* EAPOL-Key length */
+        keyLen = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+7)) << 8) +
+                 zmw_rx_buf_readb(dev, buf, offset+8);
+        /* EAPOL-Key replay counter (high double word) */
+        replayCounterH = (((u32_t) zmw_rx_buf_readb(dev, buf, offset+9)) << 24) +
+                         (((u32_t) zmw_rx_buf_readb(dev, buf, offset+10)) << 16) +
+                         (((u32_t) zmw_rx_buf_readb(dev, buf, offset+11)) << 8) +
+                         zmw_rx_buf_readb(dev, buf, offset+12);
+        /* EAPOL-Key replay counter (low double word) */
+        replayCounterL = (((u32_t) zmw_rx_buf_readb(dev, buf, offset+13)) << 24) +
+                         (((u32_t) zmw_rx_buf_readb(dev, buf, offset+14)) << 16) +
+                         (((u32_t) zmw_rx_buf_readb(dev, buf, offset+15)) << 8) +
+                         zmw_rx_buf_readb(dev, buf, offset+16);
+        /* EAPOL-Key data length */
+        keyDataLen = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+97)) << 8) +
+                     zmw_rx_buf_readb(dev, buf, offset+98);
+
+        zm_debug_msg0("EAPOL-Key");
+        zm_debug_msg1("packet length = ", packetLen);
+
+        if ( keyType == 254 )
+        {
+            zm_debug_msg0("key type = 254 (SSN key descriptor)");
+        }
+        else
+        {
+            zm_debug_msg2("key type = 0x", keyType);
+        }
+
+        zm_debug_msg2("replay counter(L) = ", replayCounterL);
+
+        zm_debug_msg2("key information = ", keyInfo);
+
+        if ( keyInfo & ZM_BIT_3 )
+        {
+            zm_debug_msg0("    - pairwise key");
+        }
+        else
+        {
+            zm_debug_msg0("    - group key");
+        }
+
+        if ( keyInfo & ZM_BIT_6 )
+        {
+            zm_debug_msg0("    - Tx key installed");
+        }
+        else
+        {
+            zm_debug_msg0("    - Tx key not set");
+        }
+
+        if ( keyInfo & ZM_BIT_7 )
+        {
+            zm_debug_msg0("    - Ack needed");
+        }
+        else
+        {
+            zm_debug_msg0("    - Ack not needed");
+        }
+
+        if ( keyInfo & ZM_BIT_8 )
+        {
+            zm_debug_msg0("    - MIC set");
+        }
+        else
+        {
+            zm_debug_msg0("    - MIC not set");
+        }
+
+        if ( keyInfo & ZM_BIT_9 )
+        {
+            zm_debug_msg0("    - packet encrypted");
+        }
+        else
+        {
+            zm_debug_msg0("    - packet not encrypted");
+        }
+
+        zm_debug_msg1("keyLen = ", keyLen);
+        zm_debug_msg1("keyDataLen = ", keyDataLen);
+    }
+    else if( packetType == 4 )
+    {
+        zm_debug_msg0("EAPOL-Encapsulated-ASF-Alert");
+    }
+}
+
+void zfShowTxEAPOL(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+    u8_t   packetType, keyType, code, identifier, type, flags;
+    u16_t  packetLen, keyInfo, keyLen, keyDataLen, length, Op_Code;
+    u32_t  replayCounterH, replayCounterL, vendorId, VendorType;
+
+    zmw_get_wlan_dev(dev);
+
+    zm_debug_msg1("EAPOL Packet size = ", zfwBufGetSize(dev, buf));
+
+    /* EAPOL packet type */
+    // 0: EAP-Packet
+    // 1: EAPOL-Start
+    // 2: EAPOL-Logoff
+    // 3: EAPOL-Key
+    // 4: EAPOL-Encapsulated-ASF-Alert
+
+    /* EAPOL frame format */
+    /*  0  1  2  3  4  5  6  7  8  9 10 11 12 13 14 15   */
+    /* -----------------------------------------------   */
+    /*            PAE Ethernet Type (0x888e)             */
+    /* ----------------------------------------------- 2 */
+    /*     Protocol Version    |         Type            */
+    /* ----------------------------------------------- 4 */
+    /*                       Length                      */
+    /* ----------------------------------------------- 6 */
+    /*                    Packet Body                    */
+    /* ----------------------------------------------- N */
+
+    packetType = zmw_tx_buf_readb(dev, buf, offset+1);
+    /* EAPOL body length */
+    packetLen = (((u16_t) zmw_tx_buf_readb(dev, buf, offset+2)) << 8) +
+                zmw_tx_buf_readb(dev, buf, offset+3);
+
+    if( packetType == 0 )
+    { // EAP-Packet
+        /* EAP-Packet Code */
+        code = zmw_tx_buf_readb(dev, buf, offset+4); // 1 : Request
+                                                     // 2 : Response
+                                                     // 3 : Success
+                                                     // 4 : Failure
+
+        // An EAP packet of the type of Success and Failure has no Data field, and has a length of 4.
+
+        /* EAP Packet format */
+        /*  0  1  2  3  4  5  6  7  8  9 10 11 12 13 14 15   */
+        /* -----------------------------------------------   */
+        /*           Code          |        Identifier       */
+        /* ----------------------------------------------- 2 */
+        /*                       Length                      */
+        /* ----------------------------------------------- 4 */
+        /*                        Data                       */
+        /* ----------------------------------------------- N */
+
+        zm_debug_msg0("EAP-Packet");
+        zm_debug_msg1("Packet Length = ", packetLen);
+        zm_debug_msg1("EAP-Packet Code = ", code);
+
+        if( code == 1 )
+        {
+            zm_debug_msg0("EAP-Packet Request");
+
+            /* EAP-Packet Identifier */
+            identifier = zmw_tx_buf_readb(dev, buf, offset+5);
+            /* EAP-Packet Length */
+            length = (((u16_t) zmw_tx_buf_readb(dev, buf, offset+6)) << 8) +
+                      zmw_tx_buf_readb(dev, buf, offset+7);
+            /* EAP-Packet Type */
+            type = zmw_tx_buf_readb(dev, buf, offset+8); // 1   : Identity
+                                                         // 2   : Notification
+                                                         // 3   : Nak (Response Only)
+                                                         // 4   : MD5-Challenge
+                                                         // 5   : One Time Password (OTP)
+                                                         // 6   : Generic Token Card (GTC)
+                                                         // 254 : (Expanded Types)Wi-Fi Protected Setup
+                                                         // 255 : Experimental Use
+
+            /* The data field in an EAP packet of the type of Request or Response is in the format shown bellowing */
+            /*  0  1  2  3  4  5  6  7             N             */
+            /* -----------------------------------------------   */
+            /*           Type          |        Type Data        */
+            /* -----------------------------------------------   */
+
+            zm_debug_msg1("EAP-Packet Identifier = ", identifier);
+            zm_debug_msg1("EAP-Packet Length = ", length);
+            zm_debug_msg1("EAP-Packet Type = ", type);
+
+            if( type == 1 )
+            {
+                zm_debug_msg0("EAP-Packet Request Identity");
+            }
+            else if( type == 2 )
+            {
+                zm_debug_msg0("EAP-Packet Request Notification");
+            }
+            else if( type == 4 )
+            {
+                zm_debug_msg0("EAP-Packet Request MD5-Challenge");
+            }
+            else if( type == 5 )
+            {
+                zm_debug_msg0("EAP-Packet Request One Time Password");
+            }
+            else if( type == 6 )
+            {
+                zm_debug_msg0("EAP-Packet Request Generic Token Card");
+            }
+            else if( type == 254 )
+            {
+                zm_debug_msg0("EAP-Packet Request Wi-Fi Protected Setup");
+
+                /* 0                   1                   2                   3   */
+                /* 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 */
+                /*+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+*/
+                /*|     Type      |               Vendor-Id                       |*/
+                /*+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+*/
+                /*|                          Vendor-Type                          |*/
+                /*+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+*/
+                /*|              Vendor data...                                    */
+                /*+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+                        */
+
+                /* EAP-Packet Vendor ID */
+                vendorId = (((u32_t) zmw_tx_buf_readb(dev, buf, offset+9)) << 16) +
+                           (((u32_t) zmw_tx_buf_readb(dev, buf, offset+10)) << 8) +
+                           zmw_tx_buf_readb(dev, buf, offset+11);
+                /* EAP-Packet Vendor Type */
+                VendorType = (((u32_t) zmw_tx_buf_readb(dev, buf, offset+12)) << 24) +
+                             (((u32_t) zmw_tx_buf_readb(dev, buf, offset+13)) << 16) +
+                             (((u32_t) zmw_tx_buf_readb(dev, buf, offset+14)) << 8) +
+                             zmw_tx_buf_readb(dev, buf, offset+15);
+                /* EAP-Packet Op Code */
+                Op_Code = (((u16_t) zmw_tx_buf_readb(dev, buf, offset+16)) << 8) +
+                          zmw_tx_buf_readb(dev, buf, offset+17);
+                /* EAP-Packet Flags */
+                flags = zmw_tx_buf_readb(dev, buf, offset+18);
+
+                zm_debug_msg1("EAP-Packet Vendor ID = ", vendorId);
+                zm_debug_msg1("EAP-Packet Venodr Type = ", VendorType);
+                zm_debug_msg1("EAP-Packet Op Code = ", Op_Code);
+                zm_debug_msg1("EAP-Packet Flags = ", flags);
+            }
+        }
+        else if( code == 2 )
+        {
+            zm_debug_msg0("EAP-Packet Response");
+
+            /* EAP-Packet Identifier */
+            identifier = zmw_tx_buf_readb(dev, buf, offset+5);
+            /* EAP-Packet Length */
+            length = (((u16_t) zmw_tx_buf_readb(dev, buf, offset+6)) << 8) +
+                      zmw_tx_buf_readb(dev, buf, offset+7);
+            /* EAP-Packet Type */
+            type = zmw_tx_buf_readb(dev, buf, offset+8);
+
+            zm_debug_msg1("EAP-Packet Identifier = ", identifier);
+            zm_debug_msg1("EAP-Packet Length = ", length);
+            zm_debug_msg1("EAP-Packet Type = ", type);
+
+            if( type == 1 )
+            {
+                zm_debug_msg0("EAP-Packet Response Identity");
+            }
+            else if( type == 2 )
+            {
+                zm_debug_msg0("EAP-Packet Request Notification");
+            }
+            else if( type == 3 )
+            {
+                zm_debug_msg0("EAP-Packet Request Nak");
+            }
+            else if( type == 4 )
+            {
+                zm_debug_msg0("EAP-Packet Request MD5-Challenge");
+            }
+            else if( type == 5 )
+            {
+                zm_debug_msg0("EAP-Packet Request One Time Password");
+            }
+            else if( type == 6 )
+            {
+                zm_debug_msg0("EAP-Packet Request Generic Token Card");
+            }
+            else if( type == 254 )
+            {
+                zm_debug_msg0("EAP-Packet Response Wi-Fi Protected Setup");
+
+                /* EAP-Packet Vendor ID */
+                vendorId = (((u32_t) zmw_tx_buf_readb(dev, buf, offset+9)) << 16) +
+                           (((u32_t) zmw_tx_buf_readb(dev, buf, offset+10)) << 8) +
+                           zmw_tx_buf_readb(dev, buf, offset+11);
+                /* EAP-Packet Vendor Type */
+                VendorType = (((u32_t) zmw_tx_buf_readb(dev, buf, offset+12)) << 24) +
+                             (((u32_t) zmw_tx_buf_readb(dev, buf, offset+13)) << 16) +
+                             (((u32_t) zmw_tx_buf_readb(dev, buf, offset+14)) << 8) +
+                             zmw_tx_buf_readb(dev, buf, offset+15);
+                /* EAP-Packet Op Code */
+                Op_Code = (((u16_t) zmw_tx_buf_readb(dev, buf, offset+16)) << 8) +
+                          zmw_tx_buf_readb(dev, buf, offset+17);
+                /* EAP-Packet Flags */
+                flags = zmw_tx_buf_readb(dev, buf, offset+18);
+
+                zm_debug_msg1("EAP-Packet Vendor ID = ", vendorId);
+                zm_debug_msg1("EAP-Packet Venodr Type = ", VendorType);
+                zm_debug_msg1("EAP-Packet Op Code = ", Op_Code);
+                zm_debug_msg1("EAP-Packet Flags = ", flags);
+            }
+        }
+        else if( code == 3 )
+        {
+            zm_debug_msg0("EAP-Packet Success");
+
+            /* EAP-Packet Identifier */
+            identifier = zmw_rx_buf_readb(dev, buf, offset+5);
+            /* EAP-Packet Length */
+            length = (((u16_t) zmw_rx_buf_readb(dev, buf, offset+6)) << 8) +
+                      zmw_rx_buf_readb(dev, buf, offset+7);
+
+            zm_debug_msg1("EAP-Packet Identifier = ", identifier);
+            zm_debug_msg1("EAP-Packet Length = ", length);
+        }
+        else if( code == 4 )
+        {
+            zm_debug_msg0("EAP-Packet Failure");
+
+            /* EAP-Packet Identifier */
+            identifier = zmw_tx_buf_readb(dev, buf, offset+5);
+            /* EAP-Packet Length */
+            length = (((u16_t) zmw_tx_buf_readb(dev, buf, offset+6)) << 8) +
+                      zmw_tx_buf_readb(dev, buf, offset+7);
+
+            zm_debug_msg1("EAP-Packet Identifier = ", identifier);
+            zm_debug_msg1("EAP-Packet Length = ", length);
+        }
+    }
+    else if( packetType == 1 )
+    { // EAPOL-Start
+        zm_debug_msg0("EAPOL-Start");
+    }
+    else if( packetType == 2 )
+    { // EAPOL-Logoff
+        zm_debug_msg0("EAPOL-Logoff");
+    }
+    else if( packetType == 3 )
+    { // EAPOL-Key
+        /* EAPOL-Key type */
+        keyType = zmw_tx_buf_readb(dev, buf, offset+4);
+        /* EAPOL-Key information */
+        keyInfo = (((u16_t) zmw_tx_buf_readb(dev, buf, offset+5)) << 8) +
+                  zmw_tx_buf_readb(dev, buf, offset+6);
+        /* EAPOL-Key length */
+        keyLen = (((u16_t) zmw_tx_buf_readb(dev, buf, offset+7)) << 8) +
+                 zmw_tx_buf_readb(dev, buf, offset+8);
+        /* EAPOL-Key replay counter (high double word) */
+        replayCounterH = (((u32_t) zmw_tx_buf_readb(dev, buf, offset+9)) << 24) +
+                         (((u32_t) zmw_tx_buf_readb(dev, buf, offset+10)) << 16) +
+                         (((u32_t) zmw_tx_buf_readb(dev, buf, offset+11)) << 8) +
+                         zmw_tx_buf_readb(dev, buf, offset+12);
+        /* EAPOL-Key replay counter (low double word) */
+        replayCounterL = (((u32_t) zmw_tx_buf_readb(dev, buf, offset+13)) << 24) +
+                         (((u32_t) zmw_tx_buf_readb(dev, buf, offset+14)) << 16) +
+                         (((u32_t) zmw_tx_buf_readb(dev, buf, offset+15)) << 8) +
+                         zmw_tx_buf_readb(dev, buf, offset+16);
+        /* EAPOL-Key data length */
+        keyDataLen = (((u16_t) zmw_tx_buf_readb(dev, buf, offset+97)) << 8) +
+                     zmw_tx_buf_readb(dev, buf, offset+98);
+
+        zm_debug_msg0("EAPOL-Key");
+        zm_debug_msg1("packet length = ", packetLen);
+
+        if ( keyType == 254 )
+        {
+            zm_debug_msg0("key type = 254 (SSN key descriptor)");
+        }
+        else
+        {
+            zm_debug_msg2("key type = 0x", keyType);
+        }
+
+        zm_debug_msg2("replay counter(L) = ", replayCounterL);
+
+        zm_debug_msg2("key information = ", keyInfo);
+
+        if ( keyInfo & ZM_BIT_3 )
+        {
+            zm_debug_msg0("    - pairwise key");
+        }
+        else
+        {
+            zm_debug_msg0("    - group key");
+        }
+
+        if ( keyInfo & ZM_BIT_6 )
+        {
+            zm_debug_msg0("    - Tx key installed");
+        }
+        else
+        {
+            zm_debug_msg0("    - Tx key not set");
+        }
+
+        if ( keyInfo & ZM_BIT_7 )
+        {
+            zm_debug_msg0("    - Ack needed");
+        }
+        else
+        {
+            zm_debug_msg0("    - Ack not needed");
+        }
+
+        if ( keyInfo & ZM_BIT_8 )
+        {
+            zm_debug_msg0("    - MIC set");
+        }
+        else
+        {
+            zm_debug_msg0("    - MIC not set");
+        }
+
+        if ( keyInfo & ZM_BIT_9 )
+        {
+            zm_debug_msg0("    - packet encrypted");
+        }
+        else
+        {
+            zm_debug_msg0("    - packet not encrypted");
+        }
+
+        zm_debug_msg1("keyLen = ", keyLen);
+        zm_debug_msg1("keyDataLen = ", keyDataLen);
+    }
+    else if( packetType == 4 )
+    {
+        zm_debug_msg0("EAPOL-Encapsulated-ASF-Alert");
+    }
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiRecv80211                */
+/*      Called to receive 802.11 frame.                                 */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : received 802.11 frame buffer.                             */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen             ZyDAS Technology Corporation    2005.5      */
+/*                                                                      */
+/************************************************************************/
+void zfiRecv80211(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* addInfo)
+{
+    u8_t snapCase=0, encryMode;
+    u16_t frameType, typeLengthField;
+    u16_t frameCtrl;
+    u16_t frameSubtype;
+    u16_t ret;
+    u16_t len;
+    u8_t bIsDefrag = 0;
+    u16_t offset, tailLen;
+    u8_t vap = 0;
+    u16_t da[3], sa[3];
+    u16_t ii;
+    u8_t uapsdTrig = 0;
+    zbuf_t* psBuf;
+#ifdef ZM_ENABLE_NATIVE_WIFI
+    u8_t i;
+#endif
+
+    zmw_get_wlan_dev(dev);
+
+    ZM_BUFFER_TRACE(dev, buf)
+
+    //zm_msg2_rx(ZM_LV_2, "zfiRecv80211(), buf=", buf);
+
+    //zm_msg2_rx(ZM_LV_0, "h[0]=", zmw_rx_buf_readh(dev, buf, 0));
+    //zm_msg2_rx(ZM_LV_0, "h[2]=", zmw_rx_buf_readh(dev, buf, 2));
+    //zm_msg2_rx(ZM_LV_0, "h[4]=", zmw_rx_buf_readh(dev, buf, 4));
+
+    frameCtrl = zmw_rx_buf_readb(dev, buf, 0);
+    frameType = frameCtrl & 0xf;
+    frameSubtype = frameCtrl & 0xf0;
+
+#if 0   // Move to ProcessBeacon to judge if there's a new peer station
+    if ( (wd->wlanMode == ZM_MODE_IBSS)&&
+         (wd->sta.ibssPartnerStatus != ZM_IBSS_PARTNER_ALIVE) )
+    {
+        zfStaIbssMonitoring(dev, buf);
+    }
+#endif
+
+    /* If data frame */
+    if (frameType == ZM_WLAN_DATA_FRAME)
+    {
+        wd->sta.TotalNumberOfReceivePackets++;
+        wd->sta.TotalNumberOfReceiveBytes += zfwBufGetSize(dev, buf);
+        //zm_debug_msg1("Receive packets     = ", wd->sta.TotalNumberOfReceivePackets);
+
+        //zm_msg0_rx(ZM_LV_0, "Rx data");
+        if (wd->wlanMode == ZM_MODE_AP)
+        {
+            if ((ret = zfApUpdatePsBit(dev, buf, &vap, &uapsdTrig)) != ZM_SUCCESS)
+            {
+                zfwBufFree(dev, buf, 0);
+                return;
+            }
+
+            if (((uapsdTrig&0xf) != 0) && ((frameSubtype & 0x80) != 0))
+            {
+                u8_t ac = zcUpToAc[zmw_buf_readb(dev, buf, 24)&0x7];
+                u8_t pktNum;
+                u8_t mb;
+                u16_t flag;
+                u8_t src[6];
+
+                //printk("QoS ctrl=%d\n", zmw_buf_readb(dev, buf, 24));
+                //printk("UAPSD trigger, ac=%d\n", ac);
+
+                if (((0x8>>ac) & uapsdTrig) != 0)
+                {
+                    pktNum = zcMaxspToPktNum[(uapsdTrig>>4) & 0x3];
+
+                    for (ii=0; ii<6; ii++)
+                    {
+                        src[ii] = zmw_buf_readb(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+ii);
+                    }
+
+                    for (ii=0; ii<pktNum; ii++)
+                    {
+                        //if ((psBuf = zfQueueGet(dev, wd->ap.uapsdQ)) != NULL)
+                        if ((psBuf = zfQueueGetWithMac(dev, wd->ap.uapsdQ, src, &mb)) != NULL)
+                        {
+                            if ((ii+1) == pktNum)
+                            {
+                                //EOSP anyway
+                                flag = 0x100 | (mb<<5);
+                            }
+                            else
+                            {
+                                if (mb != 0)
+                                {
+                                    //more data, not EOSP
+                                    flag = 0x20;
+                                }
+                                else
+                                {
+                                    //no more data, EOSP
+                                    flag = 0x100;
+                                }
+                            }
+                            zfTxSendEth(dev, psBuf, 0, ZM_EXTERNAL_ALLOC_BUF, flag);
+                        }
+
+                        if ((psBuf == NULL) || (mb == 0))
+                        {
+                            if ((ii == 0) && (psBuf == NULL))
+                            {
+                                zfSendMmFrame(dev, ZM_WLAN_FRAME_TYPE_QOS_NULL, (u16_t*)src, 0, 0, 0);
+                            }
+                            break;
+                        }
+                    }
+                }
+            }
+
+        }
+        else if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+        {
+            u16_t frameCtrlMSB;
+    		u8_t   bssid[6];
+
+            /* Check Is RIFS frame and decide to enable RIFS or not */
+            if( wd->sta.EnableHT )
+                zfCheckIsRIFSFrame(dev, buf, frameSubtype);
+
+            if ( zfPowerSavingMgrIsSleeping(dev) || wd->sta.psMgr.tempWakeUp == 1)
+            {
+                frameCtrlMSB = zmw_rx_buf_readb(dev, buf, 1);
+
+                /* check more data */
+                if ( frameCtrlMSB & ZM_BIT_5 )
+                {
+                    //if rx frame's AC is not delivery-enabled
+                    if ((wd->sta.qosInfo&0xf) != 0xf)
+                    {
+                        u8_t rxAc = 0;
+                        if ((frameSubtype & 0x80) != 0)
+                        {
+                            rxAc = zcUpToAc[zmw_buf_readb(dev, buf, 24)&0x7];
+                        }
+
+                        if (((0x8>>rxAc) & wd->sta.qosInfo) == 0)
+                        {
+                            zfSendPSPoll(dev);
+                            wd->sta.psMgr.tempWakeUp = 0;
+                        }
+                    }
+                }
+            }
+			/*increase beacon count when receive vaild data frame from AP*/
+        	ZM_MAC_WORD_TO_BYTE(wd->sta.bssid, bssid);
+
+			if (zfStaIsConnected(dev)&&
+				zfRxBufferEqualToStr(dev, buf, bssid, ZM_WLAN_HEADER_A2_OFFSET, 6))
+			{
+                wd->sta.rxBeaconCount++;
+			}
+        }
+
+        zm_msg1_rx(ZM_LV_2, "Rx VAP=", vap);
+
+        /* handle IV, EXT-IV, ICV, and EXT-ICV */
+        zfGetRxIvIcvLength(dev, buf, vap, &offset, &tailLen, addInfo);
+
+        zfStaIbssPSCheckState(dev, buf);
+        //QoS data frame
+        if ((frameSubtype & 0x80) == 0x80)
+        {
+            offset += 2;
+        }
+
+        len = zfwBufGetSize(dev, buf);
+        /* remove ICV */
+        if (tailLen > 0)
+        {
+            if (len > tailLen)
+            {
+                len -= tailLen;
+                zfwBufSetSize(dev, buf, len);
+            }
+        }
+
+        /* Filter NULL data */
+        if (((frameSubtype&0x40) != 0) || ((len = zfwBufGetSize(dev, buf))<=24))
+        {
+            zm_msg1_rx(ZM_LV_1, "Free Rx NULL data, len=", len);
+            zfwBufFree(dev, buf, 0);
+            return;
+        }
+
+        /* check and handle defragmentation */
+        if ( wd->sta.bSafeMode && (wd->sta.wepStatus == ZM_ENCRYPTION_AES) && wd->sta.SWEncryptEnable )
+        {
+            zm_msg0_rx(ZM_LV_1, "Bypass defragmentation packets in safe mode");
+        }
+        else
+        {
+            if ( (buf = zfDefragment(dev, buf, &bIsDefrag, addInfo)) == NULL )
+            {
+                /* In this case, the buffer has been freed in zfDefragment */
+                return;
+            }
+        }
+
+        ret = ZM_MIC_SUCCESS;
+
+        /* If SW WEP/TKIP are not turned on */
+        if ((wd->sta.SWEncryptEnable & ZM_SW_TKIP_DECRY_EN) == 0 &&
+            (wd->sta.SWEncryptEnable & ZM_SW_WEP_DECRY_EN) == 0)
+        {
+            encryMode = zfGetEncryModeFromRxStatus(addInfo);
+
+            /* check if TKIP */
+            if ( encryMode == ZM_TKIP )
+            {
+                if ( bIsDefrag )
+                {
+                    ret = zfMicRxVerify(dev, buf);
+                }
+                else
+                {
+                    /* check MIC failure bit */
+                    if ( ZM_RX_STATUS_IS_MIC_FAIL(addInfo) )
+                    {
+                        ret = ZM_MIC_FAILURE;
+                    }
+                }
+
+                if ( ret == ZM_MIC_FAILURE )
+                {
+                    u8_t Unicast_Pkt = 0x0;
+
+                    if ((zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET) & 0x1) == 0)
+                    {
+                        wd->commTally.swRxUnicastMicFailCount++;
+                        Unicast_Pkt = 0x1;
+                    }/*
+                    else if (zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET) == 0xffff)
+                    {
+                        wd->commTally.swRxMulticastMicFailCount++;
+                    }*/
+                    else
+                    {
+                        wd->commTally.swRxMulticastMicFailCount++;
+                    }
+                    if ( wd->wlanMode == ZM_MODE_AP )
+                    {
+                        u16_t idx;
+                        u8_t addr[6];
+
+                        for (idx=0; idx<6; idx++)
+                        {
+                            addr[idx] = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+idx);
+                        }
+
+                        if (wd->zfcbApMicFailureNotify != NULL)
+                        {
+                            wd->zfcbApMicFailureNotify(dev, addr, buf);
+                        }
+                    }
+                    else
+                    {
+                        if(Unicast_Pkt)
+                        {
+                            zm_debug_msg0("Countermeasure : Unicast_Pkt ");
+                        }
+                        else
+                        {
+                            zm_debug_msg0("Countermeasure : Non-Unicast_Pkt ");
+                        }
+
+                        if((wd->TKIP_Group_KeyChanging == 0x0) || (Unicast_Pkt == 0x1))
+                        {
+                            zm_debug_msg0("Countermeasure : Do MIC Check ");
+                            zfStaMicFailureHandling(dev, buf);
+                        }
+                        else
+                        {
+                            zm_debug_msg0("Countermeasure : SKIP MIC Check due to Group Keychanging ");
+                        }
+                    }
+                    /* Discard MIC failed frame */
+                    zfwBufFree(dev, buf, 0);
+                    return;
+                }
+            }
+        }
+        else
+        {
+            u8_t IsEncryFrame;
+
+            /* TODO: Check whether WEP bit is turned on in MAC header */
+            encryMode = ZM_NO_WEP;
+
+            IsEncryFrame = (zmw_rx_buf_readb(dev, buf, 1) & 0x40);
+
+            if (IsEncryFrame)
+            {
+                /* Software decryption for TKIP */
+                if (wd->sta.SWEncryptEnable & ZM_SW_TKIP_DECRY_EN)
+                {
+                    u16_t iv16;
+                    u16_t iv32;
+                    u8_t RC4Key[16];
+                    u16_t IvOffset;
+                    struct zsTkipSeed *rxSeed;
+
+                    IvOffset = offset + ZM_SIZE_OF_WLAN_DATA_HEADER;
+
+                    rxSeed = zfStaGetRxSeed(dev, buf);
+
+                    if (rxSeed == NULL)
+                    {
+                        zm_debug_msg0("rxSeed is NULL");
+
+                        /* Discard this frame */
+                        zfwBufFree(dev, buf, 0);
+                        return;
+                    }
+
+                    iv16 = (zmw_rx_buf_readb(dev, buf, IvOffset) << 8) + zmw_rx_buf_readb(dev, buf, IvOffset+2);
+                    iv32 = zmw_rx_buf_readb(dev, buf, IvOffset+4) +
+                           (zmw_rx_buf_readb(dev, buf, IvOffset+5) << 8) +
+                           (zmw_rx_buf_readb(dev, buf, IvOffset+6) << 16) +
+                           (zmw_rx_buf_readb(dev, buf, IvOffset+7) << 24);
+
+                    /* TKIP Key Mixing */
+                    zfTkipPhase1KeyMix(iv32, rxSeed);
+                    zfTkipPhase2KeyMix(iv16, rxSeed);
+                    zfTkipGetseeds(iv16, RC4Key, rxSeed);
+
+                    /* Decrypt Data */
+                    ret = zfTKIPDecrypt(dev, buf, IvOffset+ZM_SIZE_OF_IV+ZM_SIZE_OF_EXT_IV, 16, RC4Key);
+
+                    if (ret == ZM_ICV_FAILURE)
+                    {
+                        zm_debug_msg0("TKIP ICV fail");
+
+                        /* Discard ICV failed frame */
+                        zfwBufFree(dev, buf, 0);
+                        return;
+                    }
+
+                    /* Remove ICV from buffer */
+                    zfwBufSetSize(dev, buf, len-4);
+
+                    /* Check MIC */
+                    ret = zfMicRxVerify(dev, buf);
+
+                    if (ret == ZM_MIC_FAILURE)
+                    {
+                        if ((zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET) & 0x1) == 0)
+                        {
+                            wd->commTally.swRxUnicastMicFailCount++;
+                        }
+                        else if (zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET) == 0xffff)
+                        {
+                            wd->commTally.swRxMulticastMicFailCount++;
+                        }
+                        else
+                        {
+                            wd->commTally.swRxMulticastMicFailCount++;
+                        }
+                        if ( wd->wlanMode == ZM_MODE_AP )
+                        {
+                            u16_t idx;
+                            u8_t addr[6];
+
+                            for (idx=0; idx<6; idx++)
+                            {
+                                addr[idx] = zmw_rx_buf_readb(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+idx);
+                            }
+
+                            if (wd->zfcbApMicFailureNotify != NULL)
+                            {
+                                wd->zfcbApMicFailureNotify(dev, addr, buf);
+                            }
+                        }
+                        else
+                        {
+                            zfStaMicFailureHandling(dev, buf);
+                        }
+
+                        zm_debug_msg0("MIC fail");
+                        /* Discard MIC failed frame */
+                        zfwBufFree(dev, buf, 0);
+                        return;
+                    }
+
+                    encryMode = ZM_TKIP;
+                    offset += ZM_SIZE_OF_IV + ZM_SIZE_OF_EXT_IV;
+                }
+                else if(wd->sta.SWEncryptEnable & ZM_SW_WEP_DECRY_EN)
+                {
+                    u16_t IvOffset;
+                    u8_t keyLen = 5;
+                    u8_t iv[3];
+                    u8_t *wepKey;
+                    u8_t keyIdx;
+
+                    IvOffset = offset + ZM_SIZE_OF_WLAN_DATA_HEADER;
+
+                    /* Retrieve IV */
+                    iv[0] = zmw_rx_buf_readb(dev, buf, IvOffset);
+                    iv[1] = zmw_rx_buf_readb(dev, buf, IvOffset+1);
+                    iv[2] = zmw_rx_buf_readb(dev, buf, IvOffset+2);
+
+                    keyIdx = ((zmw_rx_buf_readb(dev, buf, IvOffset+3) >> 6) & 0x03);
+
+                    IvOffset += ZM_SIZE_OF_IV;
+
+                    if (wd->sta.SWEncryMode[keyIdx] == ZM_WEP64)
+                    {
+                        keyLen = 5;
+                    }
+                    else if (wd->sta.SWEncryMode[keyIdx] == ZM_WEP128)
+                    {
+                        keyLen = 13;
+                    }
+                    else if (wd->sta.SWEncryMode[keyIdx] == ZM_WEP256)
+                    {
+                        keyLen = 29;
+                    }
+
+                    zfWEPDecrypt(dev, buf, IvOffset, keyLen, wd->sta.wepKey[keyIdx], iv);
+
+                    if (ret == ZM_ICV_FAILURE)
+                    {
+                        zm_debug_msg0("WEP ICV fail");
+
+                        /* Discard ICV failed frame */
+                        zfwBufFree(dev, buf, 0);
+                        return;
+                    }
+
+                    encryMode = wd->sta.SWEncryMode[keyIdx];
+
+                    /* Remove ICV from buffer */
+                    zfwBufSetSize(dev, buf, len-4);
+
+                    offset += ZM_SIZE_OF_IV;
+                }
+            }
+        }
+
+#ifdef ZM_ENABLE_CENC
+        //else if ( encryMode == ZM_CENC ) /* check if CENC */
+        if ( encryMode == ZM_CENC )
+        {
+            u32_t rxIV[4];
+
+            rxIV[0] = (zmw_rx_buf_readh(dev, buf, 28) << 16)
+                     + zmw_rx_buf_readh(dev, buf, 26);
+            rxIV[1] = (zmw_rx_buf_readh(dev, buf, 32) << 16)
+                     + zmw_rx_buf_readh(dev, buf, 30);
+            rxIV[2] = (zmw_rx_buf_readh(dev, buf, 36) << 16)
+                     + zmw_rx_buf_readh(dev, buf, 34);
+            rxIV[3] = (zmw_rx_buf_readh(dev, buf, 40) << 16)
+                     + zmw_rx_buf_readh(dev, buf, 38);
+
+            //zm_debug_msg2("rxIV[0] = 0x", rxIV[0]);
+            //zm_debug_msg2("rxIV[1] = 0x", rxIV[1]);
+            //zm_debug_msg2("rxIV[2] = 0x", rxIV[2]);
+            //zm_debug_msg2("rxIV[3] = 0x", rxIV[3]);
+
+            /* destination address*/
+            da[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET);
+            da[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET+2);
+            da[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET+4);
+
+            if ( wd->wlanMode == ZM_MODE_AP )
+            {
+            }
+            else
+            {
+                if ((da[0] & 0x1))
+                { //multicast frame
+                    /* Accumlate the PN sequence */
+                    wd->sta.rxivGK[0] ++;
+
+                    if (wd->sta.rxivGK[0] == 0)
+                    {
+                        wd->sta.rxivGK[1]++;
+                    }
+
+                    if (wd->sta.rxivGK[1] == 0)
+                    {
+                        wd->sta.rxivGK[2]++;
+                    }
+
+                    if (wd->sta.rxivGK[2] == 0)
+                    {
+                        wd->sta.rxivGK[3]++;
+                    }
+
+                    if (wd->sta.rxivGK[3] == 0)
+                    {
+                        wd->sta.rxivGK[0] = 0;
+                        wd->sta.rxivGK[1] = 0;
+                        wd->sta.rxivGK[2] = 0;
+                    }
+
+                    //zm_debug_msg2("wd->sta.rxivGK[0] = 0x", wd->sta.rxivGK[0]);
+                    //zm_debug_msg2("wd->sta.rxivGK[1] = 0x", wd->sta.rxivGK[1]);
+                    //zm_debug_msg2("wd->sta.rxivGK[2] = 0x", wd->sta.rxivGK[2]);
+                    //zm_debug_msg2("wd->sta.rxivGK[3] = 0x", wd->sta.rxivGK[3]);
+
+                    if ( !((wd->sta.rxivGK[0] == rxIV[0])
+                        && (wd->sta.rxivGK[1] == rxIV[1])
+                        && (wd->sta.rxivGK[2] == rxIV[2])
+                        && (wd->sta.rxivGK[3] == rxIV[3])))
+                    {
+                        u8_t PacketDiscard = 0;
+                        /* Discard PN Code Error frame */
+                        if (rxIV[0] < wd->sta.rxivGK[0])
+                        {
+                            PacketDiscard = 1;
+                        }
+                        if (wd->sta.rxivGK[0] > 0xfffffff0)
+                        { //boundary case
+                            if ((rxIV[0] < 0xfffffff0)
+                                && (((0xffffffff - wd->sta.rxivGK[0]) + rxIV[0]) > 16))
+                            {
+                                PacketDiscard = 1;
+                            }
+                        }
+                        else
+                        { //normal case
+                            if ((rxIV[0] - wd->sta.rxivGK[0]) > 16)
+                            {
+                                PacketDiscard = 1;
+                            }
+                        }
+                        // sync sta pn code with ap because of losting some packets
+                        wd->sta.rxivGK[0] = rxIV[0];
+                        wd->sta.rxivGK[1] = rxIV[1];
+                        wd->sta.rxivGK[2] = rxIV[2];
+                        wd->sta.rxivGK[3] = rxIV[3];
+                        if (PacketDiscard)
+                        {
+                            zm_debug_msg0("Discard PN Code lost too much multicast frame");
+                            zfwBufFree(dev, buf, 0);
+                            return;
+                        }
+                    }
+                }
+                else
+                { //unicast frame
+                    /* Accumlate the PN sequence */
+                    wd->sta.rxiv[0] += 2;
+
+                    if (wd->sta.rxiv[0] == 0 || wd->sta.rxiv[0] == 1)
+                    {
+                        wd->sta.rxiv[1]++;
+                    }
+
+                    if (wd->sta.rxiv[1] == 0)
+                    {
+                        wd->sta.rxiv[2]++;
+                    }
+
+                    if (wd->sta.rxiv[2] == 0)
+                    {
+                        wd->sta.rxiv[3]++;
+                    }
+
+                    if (wd->sta.rxiv[3] == 0)
+                    {
+                        wd->sta.rxiv[0] = 0;
+                        wd->sta.rxiv[1] = 0;
+                        wd->sta.rxiv[2] = 0;
+                    }
+
+                    //zm_debug_msg2("wd->sta.rxiv[0] = 0x", wd->sta.rxiv[0]);
+                    //zm_debug_msg2("wd->sta.rxiv[1] = 0x", wd->sta.rxiv[1]);
+                    //zm_debug_msg2("wd->sta.rxiv[2] = 0x", wd->sta.rxiv[2]);
+                    //zm_debug_msg2("wd->sta.rxiv[3] = 0x", wd->sta.rxiv[3]);
+
+                    if ( !((wd->sta.rxiv[0] == rxIV[0])
+                        && (wd->sta.rxiv[1] == rxIV[1])
+                        && (wd->sta.rxiv[2] == rxIV[2])
+                        && (wd->sta.rxiv[3] == rxIV[3])))
+                    {
+                        zm_debug_msg0("PN Code mismatch, lost unicast frame, sync pn code to recv packet");
+                        // sync sta pn code with ap because of losting some packets
+                        wd->sta.rxiv[0] = rxIV[0];
+                        wd->sta.rxiv[1] = rxIV[1];
+                        wd->sta.rxiv[2] = rxIV[2];
+                        wd->sta.rxiv[3] = rxIV[3];
+                        /* Discard PN Code Error frame */
+                        //zm_debug_msg0("Discard PN Code mismatch unicast frame");
+                        //zfwBufFree(dev, buf, 0);
+                        //return;
+                    }
+                }
+            }
+        }
+#endif //ZM_ENABLE_CENC
+
+        /* for tally */
+        if ((zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET) & 0x1) == 0)
+        {
+            /* for ACU to display RxRate */
+            zfWlanUpdateRxRate(dev, addInfo);
+
+            wd->commTally.rxUnicastFrm++;
+            wd->commTally.rxUnicastOctets += (len-24);
+        }
+        else if (zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET) == 0xffff)
+        {
+            wd->commTally.rxBroadcastFrm++;
+            wd->commTally.rxBroadcastOctets += (len-24);
+        }
+        else
+        {
+            wd->commTally.rxMulticastFrm++;
+            wd->commTally.rxMulticastOctets += (len-24);
+        }
+        wd->ledStruct.rxTraffic++;
+
+        if ((frameSubtype & 0x80) == 0x80)
+        {
+            /* if QoS control bit-7 is 1 => A-MSDU frame */
+            if ((zmw_rx_buf_readh(dev, buf, 24) & 0x80) != 0)
+            {
+                zfDeAmsdu(dev, buf, vap, encryMode);
+                return;
+            }
+        }
+
+        // Remove MIC of TKIP
+        if ( encryMode == ZM_TKIP )
+        {
+            zfwBufSetSize(dev, buf, zfwBufGetSize(dev, buf) - 8);
+        }
+
+        /* Convert 802.11 and SNAP header to ethernet header */
+        if ( (wd->wlanMode == ZM_MODE_INFRASTRUCTURE)||
+             (wd->wlanMode == ZM_MODE_IBSS) )
+        {
+            /* destination address*/
+            da[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET);
+            da[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET+2);
+            da[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A1_OFFSET+4);
+
+            /* check broadcast frame */
+            if ( (da[0] == 0xffff) && (da[1] == 0xffff) && (da[2] == 0xffff) )
+            {
+                // Ap send broadcast frame to the DUT !
+            }
+            /* check multicast frame */
+            /* TODO : Remove these code, hardware should be able to block */
+            /*        multicast frame on the multicast address list       */
+            /*        or bypass all multicast packet by flag bAllMulticast */
+            else if ((da[0] & 0x01) && (wd->sta.bAllMulticast == 0))
+            {
+                for(ii=0; ii<wd->sta.multicastList.size; ii++)
+                {
+                    if ( zfMemoryIsEqual(wd->sta.multicastList.macAddr[ii].addr,
+                                         (u8_t*) da, 6))
+                    {
+                        break;
+                    }
+                }
+
+                if ( ii == wd->sta.multicastList.size )
+                {   /* not found */
+                    zm_debug_msg0("discard unknown multicast frame");
+
+                    zfwBufFree(dev, buf, 0);
+                    return;
+                }
+            }
+
+#ifdef ZM_ENABLE_NATIVE_WIFI //Native Wifi : 1, Ethernet format : 0
+            //To remove IV
+            if (offset > 0)
+            {
+                for (i=12; i>0; i--)
+                {
+                    zmw_rx_buf_writeh(dev, buf, ((i-1)*2)+offset,
+                            zmw_rx_buf_readh(dev, buf, (i-1)*2));
+                }
+                zfwBufRemoveHead(dev, buf, offset);
+            }
+#else
+
+            if (zfRxBufferEqualToStr(dev, buf, zgSnapBridgeTunnel,
+                                     24+offset, 6))
+            {
+                snapCase = 1;
+            }
+            else if ( zfRxBufferEqualToStr(dev, buf, zgSnap8021h,
+                                           24+offset, 6) )
+            {
+                typeLengthField =
+                    (((u16_t) zmw_rx_buf_readb(dev, buf, 30+offset)) << 8) +
+                    zmw_rx_buf_readb(dev, buf, 31+offset);
+
+                //zm_debug_msg2("tpyeLengthField = ", typeLengthField);
+
+                //8137 : IPX, 80F3 : Appletalk
+                if ( (typeLengthField != 0x8137)&&
+                     (typeLengthField != 0x80F3) )
+                {
+                    snapCase = 2;
+                }
+
+                if ( typeLengthField == 0x888E )
+                {
+                    zfShowRxEAPOL(dev, buf, 32);
+                }
+            }
+            else
+            {
+                //zfwDumpBuf(dev, buf);
+            }
+
+            /* source address */
+            if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+            {
+                /* SA = Address 3 */
+                sa[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET);
+                sa[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET+2);
+                sa[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A3_OFFSET+4);
+            }
+            else
+            {
+                /* SA = Address 2 */
+                sa[0] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET);
+                sa[1] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+2);
+                sa[2] = zmw_rx_buf_readh(dev, buf, ZM_WLAN_HEADER_A2_OFFSET+4);
+            }
+
+            if ( snapCase )
+            {
+                /* SA */
+                zmw_rx_buf_writeh(dev, buf, 24+offset, sa[0]);
+                zmw_rx_buf_writeh(dev, buf, 26+offset, sa[1]);
+                zmw_rx_buf_writeh(dev, buf, 28+offset, sa[2]);
+
+                /* DA = Address 1 */
+                zmw_rx_buf_writeh(dev, buf, 18+offset, da[0]);
+                zmw_rx_buf_writeh(dev, buf, 20+offset, da[1]);
+                zmw_rx_buf_writeh(dev, buf, 22+offset, da[2]);
+                zfwBufRemoveHead(dev, buf, 18+offset);
+            }
+            else
+            {
+                /* SA */
+                zmw_rx_buf_writeh(dev, buf, 16+offset, sa[0]);
+                zmw_rx_buf_writeh(dev, buf, 18+offset, sa[1]);
+                zmw_rx_buf_writeh(dev, buf, 20+offset, sa[2]);
+
+                /* DA = Address 1 */
+                zmw_rx_buf_writeh(dev, buf, 10+offset, da[0]);
+                zmw_rx_buf_writeh(dev, buf, 12+offset, da[1]);
+                zmw_rx_buf_writeh(dev, buf, 14+offset, da[2]);
+                zfwBufRemoveHead(dev, buf, 10+offset);
+                /* Ethernet payload length */
+                typeLengthField = zfwBufGetSize(dev, buf) - 14;
+                zmw_rx_buf_writeh(dev, buf, 12, (typeLengthField<<8)+(typeLengthField>>8));
+            }
+#endif  // ZM_ENABLE_NATIVE_WIFI
+        }
+        else if (wd->wlanMode == ZM_MODE_AP)
+        {
+            //if ((zmw_rx_buf_readb(dev, buf, 1) & 0x3) != 3)
+            if (vap < ZM_MAX_AP_SUPPORT)
+            /* AP mode */
+            {
+#ifdef ZM_ENABLE_NATIVE_WIFI //Native Wifi : 1, Ethernet format : 0
+                //To remove IV
+                if (offset > 0)
+                {
+                    for (i=12; i>0; i--)
+                    {
+                        zmw_rx_buf_writeh(dev, buf, ((i-1)*2)+offset,
+                                zmw_rx_buf_readh(dev, buf, (i-1)*2));
+                    }
+                    zfwBufRemoveHead(dev, buf, offset);
+                }
+#else
+                /* SA = Address 2 */
+                zmw_rx_buf_writeh(dev, buf, 24+offset, zmw_rx_buf_readh(dev, buf,
+                        ZM_WLAN_HEADER_A2_OFFSET));
+                zmw_rx_buf_writeh(dev, buf, 26+offset, zmw_rx_buf_readh(dev, buf,
+                        ZM_WLAN_HEADER_A2_OFFSET+2));
+                zmw_rx_buf_writeh(dev, buf, 28+offset, zmw_rx_buf_readh(dev, buf,
+                        ZM_WLAN_HEADER_A2_OFFSET+4));
+                /* DA = Address 3 */
+                /* Seq : Read 20 write 22, read 18 write 20, read 16 write 18 */
+                /* sequence must not be inverted */
+                zmw_rx_buf_writeh(dev, buf, 22+offset, zmw_rx_buf_readh(dev, buf,
+                        ZM_WLAN_HEADER_A3_OFFSET+4));
+                zmw_rx_buf_writeh(dev, buf, 20+offset, zmw_rx_buf_readh(dev, buf,
+                        ZM_WLAN_HEADER_A3_OFFSET+2));
+                zmw_rx_buf_writeh(dev, buf, 18+offset, zmw_rx_buf_readh(dev, buf,
+                        ZM_WLAN_HEADER_A3_OFFSET));
+                zfwBufRemoveHead(dev, buf, 18+offset);
+#endif  // ZM_ENABLE_NATIVE_WIFI
+                #if 1
+                if ((ret = zfIntrabssForward(dev, buf, vap)) == 1)
+                {
+                    /* Free Rx buffer if intra-BSS unicast frame */
+                    zm_msg0_rx(ZM_LV_2, "Free intra-BSS unicast frame");
+                    zfwBufFree(dev, buf, 0);
+                    return;
+                }
+                #endif
+            }
+            else
+            /* WDS mode */
+            {
+                zm_msg0_rx(ZM_LV_2, "Rx WDS data");
+
+                /* SA = Address 4 */
+                zmw_rx_buf_writeh(dev, buf, 30+offset, zmw_rx_buf_readh(dev, buf,
+                        ZM_WLAN_HEADER_A4_OFFSET));
+                zmw_rx_buf_writeh(dev, buf, 32+offset, zmw_rx_buf_readh(dev, buf,
+                        ZM_WLAN_HEADER_A4_OFFSET+2));
+                zmw_rx_buf_writeh(dev, buf, 34+offset, zmw_rx_buf_readh(dev, buf,
+                        ZM_WLAN_HEADER_A4_OFFSET+4));
+                /* DA = Address 3 */
+                /* Seq : Read 20 write 22, read 18 write 20, read 16 write 18 */
+                /* sequence must not be inverted */
+                zmw_rx_buf_writeh(dev, buf, 28+offset, zmw_rx_buf_readh(dev, buf,
+                        ZM_WLAN_HEADER_A3_OFFSET+4));
+                zmw_rx_buf_writeh(dev, buf, 26+offset, zmw_rx_buf_readh(dev, buf,
+                        ZM_WLAN_HEADER_A3_OFFSET+2));
+                zmw_rx_buf_writeh(dev, buf, 24+offset, zmw_rx_buf_readh(dev, buf,
+                        ZM_WLAN_HEADER_A3_OFFSET));
+                zfwBufRemoveHead(dev, buf, 24+offset);
+            }
+        }
+        else if (wd->wlanMode == ZM_MODE_PSEUDO)
+        {
+			/* WDS test: remove add4 */
+            if (wd->enableWDS)
+            {
+                offset += 6;
+            }
+
+            /* SA = Address 2 */
+            zmw_rx_buf_writeh(dev, buf, 24+offset, zmw_rx_buf_readh(dev, buf,
+                              ZM_WLAN_HEADER_A2_OFFSET));
+            zmw_rx_buf_writeh(dev, buf, 26+offset, zmw_rx_buf_readh(dev, buf,
+                              ZM_WLAN_HEADER_A2_OFFSET+2));
+            zmw_rx_buf_writeh(dev, buf, 28+offset, zmw_rx_buf_readh(dev, buf,
+                              ZM_WLAN_HEADER_A2_OFFSET+4));
+            /* DA = Address 1 */
+            zmw_rx_buf_writeh(dev, buf, 18+offset, zmw_rx_buf_readh(dev, buf,
+                              ZM_WLAN_HEADER_A1_OFFSET));
+            zmw_rx_buf_writeh(dev, buf, 20+offset, zmw_rx_buf_readh(dev, buf,
+                              ZM_WLAN_HEADER_A1_OFFSET+2));
+            zmw_rx_buf_writeh(dev, buf, 22+offset, zmw_rx_buf_readh(dev, buf,
+                              ZM_WLAN_HEADER_A1_OFFSET+4));
+            zfwBufRemoveHead(dev, buf, 18+offset);
+        }
+        else
+        {
+            zm_assert(0);
+        }
+
+        /* Call zfwRecvEth() to notify upper layer */
+        //zm_msg2_rx(ZM_LV_2, "Call zfwRecvEth(), buf=", buf);
+        //zfwDumpBuf(dev, buf);
+
+        #if ZM_PROTOCOL_RESPONSE_SIMULATION == 1
+        zfProtRspSim(dev, buf);
+        #endif
+        //zfwDumpBuf(dev, buf);
+
+        /* tally */
+    	wd->commTally.NotifyNDISRxFrmCnt++;
+
+    	if (wd->zfcbRecvEth != NULL)
+    	{
+            wd->zfcbRecvEth(dev, buf, vap);
+            ZM_PERFORMANCE_RX_MSDU(dev, wd->tick)
+        }
+    }
+    /* if management frame */
+    else if (frameType == ZM_WLAN_MANAGEMENT_FRAME)
+    {
+        zm_msg2_rx(ZM_LV_2, "Rx management,FC=", frameCtrl);
+        /* Call zfProcessManagement() to handle management frame */
+        zfProcessManagement(dev, buf, addInfo); //CWYang(m)
+        zfwBufFree(dev, buf, 0);
+    }
+    /* PsPoll */
+    else if ((wd->wlanMode == ZM_MODE_AP) && (frameCtrl == 0xa4))
+    {
+        zm_msg0_rx(ZM_LV_0, "Rx PsPoll");
+        zfApProcessPsPoll(dev, buf);
+        zfwBufFree(dev, buf, 0);
+    }
+    else
+    {
+        zm_msg0_rx(ZM_LV_1, "Rx discard!!");
+        wd->commTally.DriverDiscardedFrm++;
+
+        zfwBufFree(dev, buf, 0);
+    }
+    return;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfWlanRxValidate            */
+/*      Validate Rx frame.                                              */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : received 802.11 frame buffer.                             */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      Error code                                                      */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen             ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfWlanRxValidate(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t frameType;
+    u16_t frameCtrl;
+    u16_t frameLen;
+    u16_t ret;
+    u8_t  frameSubType;
+
+    zmw_get_wlan_dev(dev);
+
+    frameCtrl = zmw_rx_buf_readh(dev, buf, 0);
+    frameType = frameCtrl & 0xC;
+    frameSubType = (frameCtrl & 0xF0) >> 4;
+
+    frameLen = zfwBufGetSize(dev, buf);
+
+    /* Accept Data/Management frame with protocol version = 0 */
+    if ((frameType == 0x8) || (frameType == 0x0))
+    {
+
+        /* TODO : check rx status => erro bit */
+
+        /* Check Minimum Length with Wep */
+        if ((frameCtrl & 0x4000) != 0)
+        {
+            /* Minimum Length =                                       */
+            /*     PLCP(5)+Header(24)+IV(4)+ICV(4)+CRC(4)+RxStatus(8) */
+            if (frameLen < 32)
+            {
+                return ZM_ERR_MIN_RX_ENCRYPT_FRAME_LENGTH;
+            }
+        }
+        else if ( frameSubType == 0x5 || frameSubType == 0x8 )
+        {
+            /* Minimum Length = PLCP(5)+MACHeader(24)+Timestamp(8)+BeaconInterval(2)+Cap(2)+CRC(4)+RxStatus(8) */
+            if (frameLen < 36)
+            {
+                return ZM_ERR_MIN_RX_FRAME_LENGTH;
+            }
+        }
+        else
+        {
+            /* Minimum Length = PLCP(5)+MACHeader(24)+CRC(4)+RxStatus(8) */
+            if (frameLen < 24)
+            {
+                return ZM_ERR_MIN_RX_FRAME_LENGTH;
+            }
+        }
+
+        /* Check if frame Length > ZM_WLAN_MAX_RX_SIZE. */
+        if (frameLen > ZM_WLAN_MAX_RX_SIZE)
+        {
+            return ZM_ERR_MAX_RX_FRAME_LENGTH;
+        }
+    }
+    else if ((frameCtrl&0xff) == 0xa4)
+    {
+        /* PsPoll */
+        //zm_msg0_rx(ZM_LV_0, "rx pspoll");
+    }
+    else if ((frameCtrl&0xff) == ZM_WLAN_FRAME_TYPE_BAR)
+    {
+        if (wd->sta.enableDrvBA == 1)
+        {
+            zfAggRecvBAR(dev, buf);
+        }
+
+        return ZM_ERR_RX_BAR_FRAME;
+    }
+    else
+    {
+        return ZM_ERR_RX_FRAME_TYPE;
+    }
+
+    if ( wd->wlanMode == ZM_MODE_AP )
+    {
+    }
+    else if ( wd->wlanMode != ZM_MODE_PSEUDO )
+    {
+        if ( (ret=zfStaRxValidateFrame(dev, buf))!=ZM_SUCCESS )
+        {
+            //zm_debug_msg1("discard frame, code = ", ret);
+            return ret;
+        }
+    }
+
+    return ZM_SUCCESS;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfWlanRxFilter              */
+/*      Filter duplicated frame.                                        */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : received 802.11 frame buffer.                             */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      Error code                                                      */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen             ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfWlanRxFilter(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t src[3];
+    u16_t dst0;
+    u16_t frameType;
+    u16_t seq;
+    u16_t offset;
+    u16_t index;
+    u16_t col;
+    u16_t i;
+    u8_t up = 0; /* User priority */
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    ZM_BUFFER_TRACE(dev, buf)
+
+    /* RX PREFIX */
+    offset = 0;
+
+    frameType = zmw_rx_buf_readh(dev, buf, offset);
+
+    // Don't divide 2^4 because we don't want the fragementation pkt to be treated as
+    // duplicated frames
+    seq = zmw_rx_buf_readh(dev, buf, offset+22);
+    dst0 = zmw_rx_buf_readh(dev, buf, offset+4);
+    src[0] = zmw_rx_buf_readh(dev, buf, offset+10);
+    src[1] = zmw_rx_buf_readh(dev, buf, offset+12);
+    src[2] = zmw_rx_buf_readh(dev, buf, offset+14);
+
+    /* QoS data frame */
+    if ((frameType & 0x88) == 0x88)
+    {
+        up = zmw_rx_buf_readb(dev, buf, offset+24);
+        up &= 0x7;
+    }
+
+    index = (src[2]+up) & (ZM_FILTER_TABLE_ROW-1);
+
+    /* TBD : filter frame with source address == own MAC adress */
+    if ((wd->macAddr[0] == src[0]) && (wd->macAddr[1] == src[1])
+            && (wd->macAddr[2] == src[2]))
+    {
+        //zm_msg0_rx(ZM_LV_0, "Rx filter=>src is own MAC");
+        wd->trafTally.rxSrcIsOwnMac++;
+#if 0
+        return ZM_ERR_RX_SRC_ADDR_IS_OWN_MAC;
+#endif
+    }
+
+    zm_msg2_rx(ZM_LV_2, "Rx seq=", seq);
+
+    /* Filter unicast frame only */
+    if ((dst0 & 0x1) == 0)
+    {
+        zmw_enter_critical_section(dev);
+
+        for(i=0; i<ZM_FILTER_TABLE_COL; i++)
+        {
+            if ((wd->rxFilterTbl[i][index].addr[0] == src[0])
+                    && (wd->rxFilterTbl[i][index].addr[1] == src[1])
+                    && (wd->rxFilterTbl[i][index].addr[2] == src[2])
+                    && (wd->rxFilterTbl[i][index].up == up))
+            {
+                if (((frameType&0x800)==0x800)
+                        &&(wd->rxFilterTbl[i][index].seq==seq))
+                {
+                    zmw_leave_critical_section(dev);
+                    /* hit : duplicated frame */
+                    zm_msg0_rx(ZM_LV_1, "Rx filter hit=>duplicated");
+                    wd->trafTally.rxDuplicate++;
+                    return ZM_ERR_RX_DUPLICATE;
+                }
+                else
+                {
+                    /* hit : not duplicated frame, update sequence number */
+                    wd->rxFilterTbl[i][index].seq = seq;
+                    zmw_leave_critical_section(dev);
+                    zm_msg0_rx(ZM_LV_2, "Rx filter hit");
+                    return ZM_SUCCESS;
+                }
+            }
+        } /* for(i=0; i<ZM_FILTER_TABLE_COL; i++) */
+
+        /* miss : add to table */
+        zm_msg0_rx(ZM_LV_1, "Rx filter miss");
+        /* TODO : Random select a column */
+        col = (u16_t)(wd->tick & (ZM_FILTER_TABLE_COL-1));
+        wd->rxFilterTbl[col][index].addr[0] = src[0];
+        wd->rxFilterTbl[col][index].addr[1] = src[1];
+        wd->rxFilterTbl[col][index].addr[2] = src[2];
+        wd->rxFilterTbl[col][index].seq = seq;
+        wd->rxFilterTbl[col][index].up = up;
+
+        zmw_leave_critical_section(dev);
+    } /* if ((dst0 & 0x1) == 0) */
+
+    return ZM_SUCCESS;
+}
+
+
+
+u16_t zfTxGenWlanTail(zdev_t* dev, zbuf_t* buf, u16_t* snap, u16_t snaplen,
+                      u16_t* mic)
+{
+    struct zsMicVar*  pMicKey;
+    u16_t  i, length, payloadOffset;
+    u8_t   bValue, qosType = 0;
+    u8_t   snapByte[12];
+
+    zmw_get_wlan_dev(dev);
+
+    if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        pMicKey = zfApGetTxMicKey(dev, buf, &qosType);
+
+        if ( pMicKey == NULL )
+        {
+            return 0;
+        }
+    }
+    else if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+    {
+        pMicKey = zfStaGetTxMicKey(dev, buf);
+
+        if ( pMicKey == NULL )
+        {
+            return 0;
+        }
+    }
+    else
+    {
+        return 0;
+    }
+
+    length = zfwBufGetSize(dev, buf);
+
+    zfMicClear(pMicKey);
+
+    /* append DA and SA */
+#ifdef ZM_ENABLE_NATIVE_WIFI
+    for(i=16; i<22; i++)
+    { // VISTA DA
+        bValue = zmw_tx_buf_readb(dev, buf, i);
+        zfMicAppendByte(bValue, pMicKey);
+    }
+    for(i=10; i<16; i++)
+    { // VISTA SA
+        bValue = zmw_tx_buf_readb(dev, buf, i);
+        zfMicAppendByte(bValue, pMicKey);
+    }
+#else
+    for(i=0; i<12; i++)
+    {
+        bValue = zmw_tx_buf_readb(dev, buf, i);
+        zfMicAppendByte(bValue, pMicKey);
+    }
+#endif
+
+    /* append for alignment */
+    if ( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+    {
+        if (wd->sta.wmeConnected != 0)
+            zfMicAppendByte(zmw_tx_buf_readb(dev, buf, ZM_80211_FRAME_IP_OFFSET + 1) >> 5, pMicKey);
+        else
+            zfMicAppendByte(0, pMicKey);
+    }
+    else if ( wd->wlanMode == ZM_MODE_AP )
+    {
+        if (qosType == 1)
+            zfMicAppendByte(zmw_tx_buf_readb(dev, buf, ZM_80211_FRAME_IP_OFFSET + 1) >> 5, pMicKey);
+        else
+            zfMicAppendByte(0, pMicKey);
+    }
+    else
+    {
+        /* TODO : Qos Software MIC in IBSS Mode */
+        zfMicAppendByte(0, pMicKey);
+    }
+    zfMicAppendByte(0, pMicKey);
+    zfMicAppendByte(0, pMicKey);
+    zfMicAppendByte(0, pMicKey);
+
+    if ( snaplen == 0 )
+    {
+        payloadOffset = ZM_80211_FRAME_IP_OFFSET;
+    }
+    else
+    {
+        payloadOffset = ZM_80211_FRAME_TYPE_OFFSET;
+
+        for(i=0; i<(snaplen>>1); i++)
+        {
+            snapByte[i*2] = (u8_t) (snap[i] & 0xff);
+            snapByte[i*2+1] = (u8_t) ((snap[i] >> 8) & 0xff);
+        }
+
+        for(i=0; i<snaplen; i++)
+        {
+            zfMicAppendByte(snapByte[i], pMicKey);
+        }
+    }
+
+    for(i=payloadOffset; i<length; i++)
+    {
+        bValue = zmw_tx_buf_readb(dev, buf, i);
+        zfMicAppendByte(bValue, pMicKey);
+    }
+
+    zfMicGetMic( (u8_t*) mic, pMicKey);
+
+    return ZM_SIZE_OF_MIC;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfTxGetIpTosAndFrag         */
+/*      Get IP TOS and frag offset from Tx buffer                       */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : Tx buffer pointer                                         */
+/*      up : pointer for returning user priority                        */
+/*      fragOff : pointer for returning ip frag offset                  */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2006.6      */
+/*                                                                      */
+/************************************************************************/
+void zfTxGetIpTosAndFrag(zdev_t* dev, zbuf_t* buf, u8_t* up, u16_t* fragOff)
+{
+    u8_t ipv;
+    u16_t len;
+	u16_t etherType;
+    u8_t tos;
+
+    *up = 0;
+    *fragOff = 0;
+
+    len = zfwBufGetSize(dev, buf);
+
+    if (len >= 34) //Minimum IPv4 packet size, 14(Ether header)+20(IPv4 header)
+    {
+        etherType = (((u16_t)zmw_tx_buf_readb(dev, buf, ZM_80211_FRAME_TYPE_OFFSET))<<8)
+                    + zmw_tx_buf_readb(dev, buf, ZM_80211_FRAME_TYPE_OFFSET + 1);
+
+        /* protocol type = IP */
+        if (etherType == 0x0800)
+        {
+            ipv = zmw_tx_buf_readb(dev, buf, ZM_80211_FRAME_IP_OFFSET) >> 4;
+            if (ipv == 0x4) //IPv4
+            {
+                tos = zmw_tx_buf_readb(dev, buf, ZM_80211_FRAME_IP_OFFSET + 1);
+                *up = (tos >> 5);
+                *fragOff = zmw_tx_buf_readh(dev, buf, ZM_80211_FRAME_IP_OFFSET + 6);
+            }
+            /* TODO : handle VLAN tag and IPv6 packet */
+        }
+    }
+    return;
+}
+
+#ifdef ZM_ENABLE_NATIVE_WIFI
+u16_t zfTxGenWlanSnap(zdev_t* dev, zbuf_t* buf, u16_t* snap, u16_t* snaplen)
+{
+    snap[0] = zmw_buf_readh(dev, buf, ZM_80211_FRAME_HEADER_LEN + 0);
+    snap[1] = zmw_buf_readh(dev, buf, ZM_80211_FRAME_HEADER_LEN + 2);
+    snap[2] = zmw_buf_readh(dev, buf, ZM_80211_FRAME_HEADER_LEN + 4);
+    *snaplen = 6;
+
+    return ZM_80211_FRAME_HEADER_LEN + *snaplen;
+}
+#else
+u16_t zfTxGenWlanSnap(zdev_t* dev, zbuf_t* buf, u16_t* snap, u16_t* snaplen)
+{
+    u16_t removed;
+	   u16_t etherType;
+   	u16_t len;
+
+	   len = zfwBufGetSize(dev, buf);
+    if (len < 14) //Minimum Ethernet packet size, 14(Ether header)
+    {
+        /* TODO : Assert? */
+        *snaplen = 0;
+        return 0;
+    }
+
+    /* Generate RFC1042 header */
+    etherType = (((u16_t)zmw_tx_buf_readb(dev, buf, 12))<<8)
+                + zmw_tx_buf_readb(dev, buf, 13);
+
+    //zm_debug_msg2("ethernet type or length = ", etherType);
+
+    if (etherType > 1500)
+    {
+        /* ETHERNET format */
+        removed = 12;
+        snap[0] = 0xaaaa;
+        snap[1] = 0x0003;
+        if ((etherType ==0x8137) || (etherType == 0x80f3))
+        {
+            /* Bridge Tunnel */
+            snap[2] = 0xF800;
+        }
+        else
+        {
+            /* RFC 1042 */
+            snap[2] = 0x0000;
+        }
+        *snaplen = 6;
+
+        if ( etherType == 0x888E )
+        {
+            zfShowTxEAPOL(dev, buf, 14);
+        }
+    }
+    else
+    {
+        /* 802.3 format */
+        removed = 14;
+        *snaplen = 0;
+    }
+
+    return removed;
+}
+#endif
+
+u8_t zfIsVtxqEmpty(zdev_t* dev)
+{
+    u8_t isEmpty = TRUE;
+    u8_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if (wd->vmmqHead != wd->vmmqTail)
+    {
+        isEmpty = FALSE;
+        goto check_done;
+    }
+
+    for(i=0; i < 4; i++)
+    {
+        if (wd->vtxqHead[i] != wd->vtxqTail[i])
+        {
+            isEmpty = FALSE;
+            goto check_done;
+        }
+    }
+
+check_done:
+    zmw_leave_critical_section(dev);
+    return isEmpty;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfPutVtxq                   */
+/*      Put Tx buffer to virtual TxQ                                    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : Tx buffer pointer                                         */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      ZM_SUCCESS or error code                                        */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2006.6      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfPutVtxq(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t ac;
+    u8_t up;
+    u16_t fragOff;
+#ifdef ZM_AGG_TALLY
+    struct aggTally *agg_tal;
+#endif
+#ifdef ZM_ENABLE_AGGREGATION
+    #ifndef ZM_BYPASS_AGGR_SCHEDULING
+	u16_t ret;
+    u16_t tid;
+    #endif
+#endif
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zfTxGetIpTosAndFrag(dev, buf, &up, &fragOff);
+
+    if ( wd->zfcbClassifyTxPacket != NULL )
+    {
+        ac = wd->zfcbClassifyTxPacket(dev, buf);
+    }
+    else
+    {
+        ac = zcUpToAc[up&0x7] & 0x3;
+    }
+
+    /*
+     * add by honda
+     * main A-MPDU aggregation function
+     */
+#ifdef ZM_AGG_TALLY
+    agg_tal = &wd->agg_tal;
+    agg_tal->got_packets_sum++;
+
+#endif
+
+#ifdef ZM_ENABLE_AGGREGATION
+    #ifndef ZM_BYPASS_AGGR_SCHEDULING
+    tid = up&0x7;
+    if(wd->enableAggregation==0)
+    {
+        if( (wd->wlanMode == ZM_MODE_AP) ||
+            (wd->wlanMode == ZM_MODE_INFRASTRUCTURE && wd->sta.EnableHT) ||
+            (wd->wlanMode == ZM_MODE_PSEUDO) ) {
+            // (infrastructure_mode && connect_to_11n_ap) || (ap_mode && is_11n_ap)
+            //ret = zfAggPutVtxq(dev, buf);
+
+
+            ret = zfAggTx(dev, buf, tid);
+            if (ZM_SUCCESS == ret)
+            {
+                //zfwBufFree(dev, buf, ZM_SUCCESS);
+
+                return ZM_SUCCESS;
+            }
+            if (ZM_ERR_EXCEED_PRIORITY_THRESHOLD == ret)
+            {
+                wd->commTally.txQosDropCount[ac]++;
+                zfwBufFree(dev, buf, ZM_SUCCESS);
+
+                zm_msg1_tx(ZM_LV_1, "Packet discarded, VTXQ full, ac=", ac);
+
+                return ZM_ERR_EXCEED_PRIORITY_THRESHOLD;
+            }
+            if (ZM_ERR_TX_BUFFER_UNAVAILABLE == ret)
+            {
+                /*
+                * do nothing
+                * continue following procession, put into VTXQ
+                * return ZM_SUCCESS;
+                */
+            }
+        }
+    }
+    #endif
+#endif
+    /*
+     * end of add by honda
+     */
+
+    /* First Ip frag */
+    if ((fragOff & 0xff3f) == 0x0020)
+    {
+        /* Don't let ip frag in if VTXQ unable to hold */
+        /* whole ip frag burst(assume 20 frag)         */
+        zmw_enter_critical_section(dev);
+        if (((wd->vtxqHead[ac] - wd->vtxqTail[ac])& ZM_VTXQ_SIZE_MASK)
+                > (ZM_VTXQ_SIZE-20))
+        {
+            wd->qosDropIpFrag[ac] = 1;
+        }
+        else
+        {
+            wd->qosDropIpFrag[ac] = 0;
+        }
+        zmw_leave_critical_section(dev);
+
+        if (wd->qosDropIpFrag[ac] == 1)
+        {
+            //zm_debug_msg2("vtQ full, drop buf = ", buf);
+            wd->commTally.txQosDropCount[ac]++;
+            zfwBufFree(dev, buf, ZM_SUCCESS);
+            zm_msg1_tx(ZM_LV_1, "Packet discarded, first ip frag, ac=", ac);
+            //VTXQ[] can not hold whold ip frag burst(assume 20 frags)
+            return ZM_ERR_EXCEED_PRIORITY_THRESHOLD;
+        }
+    }
+    else if ((fragOff & 0xff3f) == 0)
+    {
+        wd->qosDropIpFrag[ac] = 0;
+    }
+
+    if (((fragOff &= 0xff1f) != 0) && (wd->qosDropIpFrag[ac] == 1))
+    {
+        wd->commTally.txQosDropCount[ac]++;
+        zfwBufFree(dev, buf, ZM_SUCCESS);
+        zm_msg1_tx(ZM_LV_1, "Packet discarded, ip frag, ac=", ac);
+        //Discard following ip frags
+        return ZM_ERR_EXCEED_PRIORITY_THRESHOLD;
+    }
+
+    zmw_enter_critical_section(dev);
+    if (((wd->vtxqHead[ac] + 1) & ZM_VTXQ_SIZE_MASK) != wd->vtxqTail[ac])
+    {
+        wd->vtxq[ac][wd->vtxqHead[ac]] = buf;
+        wd->vtxqHead[ac] = ((wd->vtxqHead[ac] + 1) & ZM_VTXQ_SIZE_MASK);
+        zmw_leave_critical_section(dev);
+        return ZM_SUCCESS;
+    }
+    else
+    {
+        zmw_leave_critical_section(dev);
+
+        wd->commTally.txQosDropCount[ac]++;
+        zfwBufFree(dev, buf, ZM_SUCCESS);
+        zm_msg1_tx(ZM_LV_1, "Packet discarded, VTXQ full, ac=", ac);
+        return ZM_ERR_EXCEED_PRIORITY_THRESHOLD; //VTXQ[] Full
+    }
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfGetVtxq                   */
+/*      Get Tx buffer from virtual TxQ                                  */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      Tx buffer pointer                                               */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2006.6      */
+/*                                                                      */
+/************************************************************************/
+zbuf_t* zfGetVtxq(zdev_t* dev, u8_t ac)
+{
+    zbuf_t* buf;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    ac &= 0x3;
+    zmw_enter_critical_section(dev);
+    if (wd->vtxqHead[ac] != wd->vtxqTail[ac])
+    {
+        buf = wd->vtxq[ac][wd->vtxqTail[ac]];
+        wd->vtxqTail[ac] = ((wd->vtxqTail[ac] + 1) & ZM_VTXQ_SIZE_MASK);
+        zmw_leave_critical_section(dev);
+        return buf;
+    }
+    else
+    {
+        zmw_leave_critical_section(dev);
+        return 0; //VTXQ[] empty
+    }
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfPutVmmq                   */
+/*      Put Tx buffer to virtual MmQ                                    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      buf : Tx buffer pointer                                         */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      ZM_SUCCESS or error code                                        */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2006.12     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfPutVmmq(zdev_t* dev, zbuf_t* buf)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    if (((wd->vmmqHead + 1) & ZM_VMMQ_SIZE_MASK) != wd->vmmqTail)
+    {
+        wd->vmmq[wd->vmmqHead] = buf;
+        wd->vmmqHead = ((wd->vmmqHead + 1) & ZM_VMMQ_SIZE_MASK);
+        zmw_leave_critical_section(dev);
+        return ZM_SUCCESS;
+    }
+    else
+    {
+        zmw_leave_critical_section(dev);
+
+        zfwBufFree(dev, buf, ZM_SUCCESS);
+        zm_msg0_mm(ZM_LV_0, "Packet discarded, VMmQ full");
+        return ZM_ERR_VMMQ_FULL; //VTXQ[] Full
+    }
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfGetVmmq                   */
+/*      Get Tx buffer from virtual MmQ                                  */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      Tx buffer pointer                                               */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2006.12     */
+/*                                                                      */
+/************************************************************************/
+zbuf_t* zfGetVmmq(zdev_t* dev)
+{
+    zbuf_t* buf;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    if (wd->vmmqHead != wd->vmmqTail)
+    {
+        buf = wd->vmmq[wd->vmmqTail];
+        wd->vmmqTail = ((wd->vmmqTail + 1) & ZM_VMMQ_SIZE_MASK);
+        zmw_leave_critical_section(dev);
+        return buf;
+    }
+    else
+    {
+        zmw_leave_critical_section(dev);
+        return 0; //VTXQ[] empty
+    }
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfPushVtxq                  */
+/*      Service Virtual TxQ (weighted round robin)                      */
+/*      Get Tx buffer form virtual TxQ and put to hardware TxD queue    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2006.6      */
+/*                                                                      */
+/************************************************************************/
+void zfPushVtxq(zdev_t* dev)
+{
+    zbuf_t* buf;
+    u16_t i;
+    u16_t txed;
+    u32_t freeTxd;
+    u16_t err;
+    u16_t skipFlag = 0;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+
+
+    //zm_debug_msg1("zfHpGetFreeTxdCount = ", zfHpGetFreeTxdCount(dev));
+
+    if (wd->halState == ZM_HAL_STATE_INIT)
+    {
+        if (!wd->modeMDKEnable)
+        {
+            zm_debug_msg0("HAL is not ready for Tx");
+        }
+        return;
+    }
+    else if (wd->sta.DFSDisableTx)
+    {
+        zm_debug_msg0("return because 802.11h DFS Disable Tx");
+        return;
+    }
+    else if (wd->sta.flagFreqChanging != 0)
+    {
+        //Hold until RF frequency changed
+        return;
+    }
+    else if (( wd->sta.flagKeyChanging ) && ( wd->wlanMode != ZM_MODE_AP ))
+    {
+        return;
+    }
+#ifdef ZM_ENABLE_POWER_SAVE
+    else if ( zfPowerSavingMgrIsSleeping(dev) )
+    {
+        //zm_debug_msg0("Packets queued since the MAC is in power-saving mode\n");
+        return;
+    }
+#endif
+
+    zmw_enter_critical_section(dev);
+    if (wd->vtxqPushing != 0)
+    {
+        skipFlag = 1;
+    }
+    else
+    {
+        wd->vtxqPushing = 1;
+    }
+    zmw_leave_critical_section(dev);
+
+    if (skipFlag == 1)
+    {
+        return;
+    }
+
+    while (1)
+    {
+        txed = 0;
+
+        /* 2006.12.20, Serve Management queue */
+        while( zfHpGetFreeTxdCount(dev) > 0 )
+        {
+            if ((buf = zfGetVmmq(dev)) != 0)
+            {
+                txed = 1;
+                //zm_debug_msg2("send buf = ", buf);
+                if ((err = zfHpSend(dev, NULL, 0, NULL, 0, NULL, 0, buf, 0,
+                        ZM_INTERNAL_ALLOC_BUF, 0, 0xff)) != ZM_SUCCESS)
+                {
+                    zfwBufFree(dev, buf, 0);
+                }
+            }
+            else
+            {
+                break;
+            }
+        }
+        if ((wd->sta.bScheduleScan) || ((wd->sta.bChannelScan == TRUE) && (zfStaIsConnected(dev))))
+        {
+            //Hold until Scan Stop
+            wd->vtxqPushing = 0;
+            return;
+        }
+
+#ifdef ZM_ENABLE_AGGREGATION
+    #ifndef ZM_BYPASS_AGGR_SCHEDULING
+        if( (wd->wlanMode == ZM_MODE_AP) ||
+            (wd->wlanMode == ZM_MODE_INFRASTRUCTURE && wd->sta.EnableHT) ||
+            (wd->wlanMode == ZM_MODE_PSEUDO) ) {
+
+            zfAggTxScheduler(dev, 0);
+
+            if (txed == 0) {
+                wd->vtxqPushing = 0;
+                return;
+            }
+            else {
+                continue;
+            }
+        }
+    #endif
+#endif
+
+        /* Service VTxQ[3] */
+        for (i=0; i<4; i++)
+        {
+            if ((freeTxd = zfHpGetFreeTxdCount(dev)) >= 3)
+            {
+                if ((buf = zfGetVtxq(dev, 3)) != 0)
+                {
+                    txed = 1;
+                    //zm_debug_msg2("send buf = ", buf);
+                    zfTxSendEth(dev, buf, 0, ZM_EXTERNAL_ALLOC_BUF, 0);
+                    ZM_PERFORMANCE_TX_MPDU(dev, wd->tick);
+                }
+            }
+            else
+            {
+                break;
+            }
+        }
+
+        /* Service VTxQ[2] */
+        for (i=0; i<3; i++)
+        {
+            if ((freeTxd = zfHpGetFreeTxdCount(dev)) >= (zfHpGetMaxTxdCount(dev)*1/4))
+            {
+                if ((buf = zfGetVtxq(dev, 2)) != 0)
+                {
+                    txed = 1;
+                    zfTxSendEth(dev, buf, 0, ZM_EXTERNAL_ALLOC_BUF, 0);
+                    ZM_PERFORMANCE_TX_MPDU(dev, wd->tick);
+                }
+                if (wd->sta.ac0PriorityHigherThanAc2 == 1)
+                {
+                    if ((buf = zfGetVtxq(dev, 0)) != 0)
+                    {
+                        txed = 1;
+                        zfTxSendEth(dev, buf, 0, ZM_EXTERNAL_ALLOC_BUF, 0);
+                        ZM_PERFORMANCE_TX_MPDU(dev, wd->tick);
+                    }
+                }
+            }
+            else
+            {
+                break;
+            }
+        }
+
+        /* Service VTxQ[0] */
+        for (i=0; i<2; i++)
+        {
+            if ((freeTxd = zfHpGetFreeTxdCount(dev)) >= (zfHpGetMaxTxdCount(dev)*2/4))
+            {
+                if ((buf = zfGetVtxq(dev, 0)) != 0)
+                {
+                    txed = 1;
+                    zfTxSendEth(dev, buf, 0, ZM_EXTERNAL_ALLOC_BUF, 0);
+                    ZM_PERFORMANCE_TX_MPDU(dev, wd->tick);
+                }
+            }
+            else
+            {
+                break;
+            }
+
+        }
+
+        /* Service VTxQ[1] */
+        if ((freeTxd = zfHpGetFreeTxdCount(dev)) >= (zfHpGetMaxTxdCount(dev)*3/4))
+        {
+            if ((buf = zfGetVtxq(dev, 1)) != 0)
+            {
+                txed = 1;
+                zfTxSendEth(dev, buf, 0, ZM_EXTERNAL_ALLOC_BUF, 0);
+                ZM_PERFORMANCE_TX_MPDU(dev, wd->tick);
+            }
+        }
+
+        /* All VTxQs are either empty or exceed their threshold */
+        if (txed == 0)
+        {
+            wd->vtxqPushing = 0;
+            return;
+        }
+    } //while (1)
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfFlushVtxq                 */
+/*      Flush Virtual TxQ and MmQ                                       */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.1      */
+/*                                                                      */
+/************************************************************************/
+void zfFlushVtxq(zdev_t* dev)
+{
+    zbuf_t* buf;
+    u8_t i;
+    zmw_get_wlan_dev(dev);
+
+    /* Flush MmQ */
+    while ((buf = zfGetVmmq(dev)) != 0)
+    {
+        zfwBufFree(dev, buf, 0);
+        zm_debug_msg0("zfFlushVtxq: [Vmmq]");
+        wd->queueFlushed  |= 0x10;
+    }
+
+    /* Flush VTxQ */
+    for (i=0; i<4; i++)
+    {
+        while ((buf = zfGetVtxq(dev, i)) != 0)
+        {
+            zfwBufFree(dev, buf, 0);
+            zm_debug_msg1("zfFlushVtxq: [zfGetVtxq]- ", i);
+            wd->queueFlushed |= (1<<i);
+        }
+    }
+}
+
+void zf80211FrameSend(zdev_t* dev, zbuf_t* buf, u16_t* header, u16_t snapLen,
+                           u16_t* da, u16_t* sa, u8_t up, u16_t headerLen, u16_t* snap,
+                           u16_t* tail, u16_t tailLen, u16_t offset, u16_t bufType,
+                           u8_t ac, u8_t keyIdx)
+{
+    u16_t err;
+    u16_t fragLen;
+
+    zmw_get_wlan_dev(dev);
+
+    fragLen = zfwBufGetSize(dev, buf);
+    if ((da[0]&0x1) == 0)
+    {
+        wd->commTally.txUnicastFrm++;
+        wd->commTally.txUnicastOctets += (fragLen+snapLen);
+    }
+    else if (da[0] == 0xffff)
+    {
+        wd->commTally.txBroadcastFrm++;
+        wd->commTally.txBroadcastOctets += (fragLen+snapLen);
+    }
+    else
+    {
+        wd->commTally.txMulticastFrm++;
+        wd->commTally.txMulticastOctets += (fragLen+snapLen);
+    }
+    wd->ledStruct.txTraffic++;
+
+    if ((err = zfHpSend(dev, header, headerLen, snap, snapLen,
+                        tail, tailLen, buf, offset,
+                        bufType, ac, keyIdx)) != ZM_SUCCESS)
+    {
+        if (bufType == ZM_EXTERNAL_ALLOC_BUF)
+        {
+            zfwBufFree(dev, buf, err);
+        }
+        else if (bufType == ZM_INTERNAL_ALLOC_BUF)
+        {
+            zfwBufFree(dev, buf, 0);
+        }
+        else
+        {
+            zm_assert(0);
+        }
+    }
+}
+
+void zfCheckIsRIFSFrame(zdev_t* dev, zbuf_t* buf, u16_t frameSubtype)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* #2 Record the sequence number to determine whether the unicast frame is separated by RIFS or not */
+    if (frameSubtype & 0x80)
+    {   //QoS data frame
+        u16_t sequenceNum;
+        u16_t qosControlField;
+
+        sequenceNum = ( zmw_buf_readh(dev, buf, 22) >> 4 ); // Discard fragment number !
+        qosControlField = zmw_buf_readh(dev, buf, 24); // Don't consider WDS (Wireless Distribution System)
+        //DbgPrint("The QoS Control Field                              : %d", qosControlField);
+        //DbgPrint("The RIFS Count                                     : %d", wd->sta.rifsCount);
+
+        if( qosControlField & ZM_BIT_5 )
+        {// ACK policy is "No ACK"
+            /* RIFS-Like frame */
+            wd->sta.rifsLikeFrameSequence[wd->sta.rifsLikeFrameCnt]   = sequenceNum;
+
+            if( wd->sta.rifsState == ZM_RIFS_STATE_DETECTING )
+            {
+                if( wd->sta.rifsLikeFrameSequence[2] != 0 )
+                {// RIFS-like Pattern collected
+                    if( ( wd->sta.rifsLikeFrameSequence[2] - wd->sta.rifsLikeFrameSequence[1] == 2 ) &&
+                        ( wd->sta.rifsLikeFrameSequence[1] - wd->sta.rifsLikeFrameSequence[0] == 2 ) )
+                    {
+                        /* RIFS pattern matched */
+
+                        /* #3 Enable RIFS function if the RIFS pattern matched  */
+                        zfHpEnableRifs(dev, ((wd->sta.currentFrequency<3000)?1:0), wd->sta.EnableHT, wd->sta.HT2040);
+
+                        // Set RIFS timer
+                        wd->sta.rifsTimer = wd->tick;
+
+                        wd->sta.rifsCount++;
+
+                        // Set state to be Detected
+                        wd->sta.rifsState = ZM_RIFS_STATE_DETECTED;
+                    }
+                }
+            }
+            else
+            {// state = Detected
+                // Reset RIFS timer
+                if( (wd->tick - wd->sta.rifsTimer) < ZM_RIFS_TIMER_TIMEOUT )
+                    wd->sta.rifsTimer = wd->tick;
+            }
+
+            //DbgPrint("SN1 = %d, SN2 = %d, SN3 = %d\n", wd->sta.rifsLikeFrameSequence[0],
+            //                                           wd->sta.rifsLikeFrameSequence[1],
+            //                                           wd->sta.rifsLikeFrameSequence[2]);
+
+            // Update RIFS-like sequence number
+            if( wd->sta.rifsLikeFrameSequence[2] != 0 )
+            {
+                wd->sta.rifsLikeFrameSequence[0] = wd->sta.rifsLikeFrameSequence[1];
+                wd->sta.rifsLikeFrameSequence[1] = wd->sta.rifsLikeFrameSequence[2];
+                wd->sta.rifsLikeFrameSequence[2] = 0;
+            }
+
+            // Only record three adjacent frame
+            if( wd->sta.rifsLikeFrameCnt < 2 )
+                wd->sta.rifsLikeFrameCnt++;
+        }
+    }
+
+    /* #4 Disable RIFS function if the timer TIMEOUT  */
+    if( wd->sta.rifsState == ZM_RIFS_STATE_DETECTED )
+    {
+        if( ( wd->tick - wd->sta.rifsTimer ) > ZM_RIFS_TIMER_TIMEOUT )
+        {// TIMEOUT
+            // Disable RIFS
+            zfHpDisableRifs(dev);
+
+            // Reset RIFS-like sequence number FIFO
+            wd->sta.rifsLikeFrameSequence[0] = 0;
+            wd->sta.rifsLikeFrameSequence[1] = 0;
+            wd->sta.rifsLikeFrameSequence[2] = 0;
+            wd->sta.rifsLikeFrameCnt = 0;
+
+            // Set state to be Detecting
+            wd->sta.rifsState = ZM_RIFS_STATE_DETECTING;
+        }
+    }
+}
diff --git a/drivers/staging/otus/80211core/cwep.c b/drivers/staging/otus/80211core/cwep.c
new file mode 100644
index 00000000000..ec31bb1ac28
--- /dev/null
+++ b/drivers/staging/otus/80211core/cwep.c
@@ -0,0 +1,299 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : cwep.c                                                */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains Tx and Rx functions.                       */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+#include "cprecomp.h"
+
+u32_t crc32_tab[] =
+{
+       0x00000000L, 0x77073096L, 0xee0e612cL, 0x990951baL, 0x076dc419L,
+       0x706af48fL, 0xe963a535L, 0x9e6495a3L, 0x0edb8832L, 0x79dcb8a4L,
+       0xe0d5e91eL, 0x97d2d988L, 0x09b64c2bL, 0x7eb17cbdL, 0xe7b82d07L,
+       0x90bf1d91L, 0x1db71064L, 0x6ab020f2L, 0xf3b97148L, 0x84be41deL,
+       0x1adad47dL, 0x6ddde4ebL, 0xf4d4b551L, 0x83d385c7L, 0x136c9856L,
+       0x646ba8c0L, 0xfd62f97aL, 0x8a65c9ecL, 0x14015c4fL, 0x63066cd9L,
+       0xfa0f3d63L, 0x8d080df5L, 0x3b6e20c8L, 0x4c69105eL, 0xd56041e4L,
+       0xa2677172L, 0x3c03e4d1L, 0x4b04d447L, 0xd20d85fdL, 0xa50ab56bL,
+       0x35b5a8faL, 0x42b2986cL, 0xdbbbc9d6L, 0xacbcf940L, 0x32d86ce3L,
+       0x45df5c75L, 0xdcd60dcfL, 0xabd13d59L, 0x26d930acL, 0x51de003aL,
+       0xc8d75180L, 0xbfd06116L, 0x21b4f4b5L, 0x56b3c423L, 0xcfba9599L,
+       0xb8bda50fL, 0x2802b89eL, 0x5f058808L, 0xc60cd9b2L, 0xb10be924L,
+       0x2f6f7c87L, 0x58684c11L, 0xc1611dabL, 0xb6662d3dL, 0x76dc4190L,
+       0x01db7106L, 0x98d220bcL, 0xefd5102aL, 0x71b18589L, 0x06b6b51fL,
+       0x9fbfe4a5L, 0xe8b8d433L, 0x7807c9a2L, 0x0f00f934L, 0x9609a88eL,
+       0xe10e9818L, 0x7f6a0dbbL, 0x086d3d2dL, 0x91646c97L, 0xe6635c01L,
+       0x6b6b51f4L, 0x1c6c6162L, 0x856530d8L, 0xf262004eL, 0x6c0695edL,
+       0x1b01a57bL, 0x8208f4c1L, 0xf50fc457L, 0x65b0d9c6L, 0x12b7e950L,
+       0x8bbeb8eaL, 0xfcb9887cL, 0x62dd1ddfL, 0x15da2d49L, 0x8cd37cf3L,
+       0xfbd44c65L, 0x4db26158L, 0x3ab551ceL, 0xa3bc0074L, 0xd4bb30e2L,
+       0x4adfa541L, 0x3dd895d7L, 0xa4d1c46dL, 0xd3d6f4fbL, 0x4369e96aL,
+       0x346ed9fcL, 0xad678846L, 0xda60b8d0L, 0x44042d73L, 0x33031de5L,
+       0xaa0a4c5fL, 0xdd0d7cc9L, 0x5005713cL, 0x270241aaL, 0xbe0b1010L,
+       0xc90c2086L, 0x5768b525L, 0x206f85b3L, 0xb966d409L, 0xce61e49fL,
+       0x5edef90eL, 0x29d9c998L, 0xb0d09822L, 0xc7d7a8b4L, 0x59b33d17L,
+       0x2eb40d81L, 0xb7bd5c3bL, 0xc0ba6cadL, 0xedb88320L, 0x9abfb3b6L,
+       0x03b6e20cL, 0x74b1d29aL, 0xead54739L, 0x9dd277afL, 0x04db2615L,
+       0x73dc1683L, 0xe3630b12L, 0x94643b84L, 0x0d6d6a3eL, 0x7a6a5aa8L,
+       0xe40ecf0bL, 0x9309ff9dL, 0x0a00ae27L, 0x7d079eb1L, 0xf00f9344L,
+       0x8708a3d2L, 0x1e01f268L, 0x6906c2feL, 0xf762575dL, 0x806567cbL,
+       0x196c3671L, 0x6e6b06e7L, 0xfed41b76L, 0x89d32be0L, 0x10da7a5aL,
+       0x67dd4accL, 0xf9b9df6fL, 0x8ebeeff9L, 0x17b7be43L, 0x60b08ed5L,
+       0xd6d6a3e8L, 0xa1d1937eL, 0x38d8c2c4L, 0x4fdff252L, 0xd1bb67f1L,
+       0xa6bc5767L, 0x3fb506ddL, 0x48b2364bL, 0xd80d2bdaL, 0xaf0a1b4cL,
+       0x36034af6L, 0x41047a60L, 0xdf60efc3L, 0xa867df55L, 0x316e8eefL,
+       0x4669be79L, 0xcb61b38cL, 0xbc66831aL, 0x256fd2a0L, 0x5268e236L,
+       0xcc0c7795L, 0xbb0b4703L, 0x220216b9L, 0x5505262fL, 0xc5ba3bbeL,
+       0xb2bd0b28L, 0x2bb45a92L, 0x5cb36a04L, 0xc2d7ffa7L, 0xb5d0cf31L,
+       0x2cd99e8bL, 0x5bdeae1dL, 0x9b64c2b0L, 0xec63f226L, 0x756aa39cL,
+       0x026d930aL, 0x9c0906a9L, 0xeb0e363fL, 0x72076785L, 0x05005713L,
+       0x95bf4a82L, 0xe2b87a14L, 0x7bb12baeL, 0x0cb61b38L, 0x92d28e9bL,
+       0xe5d5be0dL, 0x7cdcefb7L, 0x0bdbdf21L, 0x86d3d2d4L, 0xf1d4e242L,
+       0x68ddb3f8L, 0x1fda836eL, 0x81be16cdL, 0xf6b9265bL, 0x6fb077e1L,
+       0x18b74777L, 0x88085ae6L, 0xff0f6a70L, 0x66063bcaL, 0x11010b5cL,
+       0x8f659effL, 0xf862ae69L, 0x616bffd3L, 0x166ccf45L, 0xa00ae278L,
+       0xd70dd2eeL, 0x4e048354L, 0x3903b3c2L, 0xa7672661L, 0xd06016f7L,
+       0x4969474dL, 0x3e6e77dbL, 0xaed16a4aL, 0xd9d65adcL, 0x40df0b66L,
+       0x37d83bf0L, 0xa9bcae53L, 0xdebb9ec5L, 0x47b2cf7fL, 0x30b5ffe9L,
+       0xbdbdf21cL, 0xcabac28aL, 0x53b39330L, 0x24b4a3a6L, 0xbad03605L,
+       0xcdd70693L, 0x54de5729L, 0x23d967bfL, 0xb3667a2eL, 0xc4614ab8L,
+       0x5d681b02L, 0x2a6f2b94L, 0xb40bbe37L, 0xc30c8ea1L, 0x5a05df1bL,
+       0x2d02ef8dL
+};
+
+void zfWEPEncrypt(zdev_t *dev, zbuf_t *buf, u8_t *snap, u16_t snapLen, u16_t offset, u8_t keyLen, u8_t* WepKey, u8_t *iv)
+{
+    u8_t    S[256],S2[256];
+    u16_t   ui;
+    u16_t   i;
+    u16_t   j;
+    u8_t    temp;
+    u8_t    K;
+    u32_t   ltemp;
+    u16_t   len;
+    u32_t   icv;
+    u8_t    key[32];
+
+    key[0] = iv[0];
+    key[1] = iv[1];
+    key[2] = iv[2];
+
+    /* Append Wep Key after IV */
+    zfMemoryCopy(&key[3], WepKey, keyLen);
+
+    keyLen += 3;
+
+    for(i = 0; i < 256; i++)
+    {
+        S[i] = (u8_t)i;
+        S2[i] = key[i&(keyLen-1)];
+    }
+
+    j = 0;
+    for(i = 0; i < 256; i++)
+    {
+        j = (j + S[i] + S2[i]) ;
+        j&=255 ;
+
+        // Swap S[i] and S[j]
+        temp = S[i];
+        S[i] = S[j];
+        S[j] = temp;
+    }
+
+    i = j = 0;
+    icv = -1;
+
+    /* For Snap Header */
+    for (ui = 0; ui < snapLen; ui++)
+    {
+        u8_t In;
+
+        i++;
+        i &= 255;
+        j += S[i];
+        j &= 255;
+
+        // Swap S[i] and S[j]
+        temp = S[i];
+        S[i] = S[j];
+        S[j] = temp;
+//          temp = (S[i] + temp) & 255;
+        temp += S[i];
+        temp &=255;
+        K = S[temp];        // Key used to Xor with input data
+
+        In = snap[ui];
+        icv =  (icv>>8) ^ crc32_tab[(icv^In)&0xff];
+
+        snap[ui] = In ^ K;
+        //zmw_tx_buf_writeb(dev, buf, ui, In ^ K);
+    }
+
+    len = zfwBufGetSize(dev, buf);
+
+    for (ui = offset; ui < len; ui++)
+    {
+        u8_t In;
+
+        i++;
+        i &= 255;
+        j += S[i];
+        j &= 255;
+
+        // Swap S[i] and S[j]
+        temp = S[i];
+        S[i] = S[j];
+        S[j] = temp;
+//          temp = (S[i] + temp) & 255;
+        temp += S[i];
+        temp &=255;
+        K = S[temp];        // Key used to Xor with input data
+
+        In = zmw_tx_buf_readb(dev, buf, ui);
+        icv =  (icv>>8) ^ crc32_tab[(icv^In)&0xff];
+
+        zmw_tx_buf_writeb(dev, buf, ui, In ^ K);
+    } //End of for (ui = 0; ui < Num_Bytes; ui++)
+
+    icv = ~(icv);
+    ltemp = (u32_t) icv;
+
+    for (ui = 0; ui < 4; ui++)
+    {
+        i ++;
+        i &= 255;
+        j += S[i];
+        j &= 255;
+
+        // Swap S[i] and S[j]
+        temp = S[i];
+        S[i] = S[j];
+        S[j] = temp;
+        temp += S[i];
+        temp &= 255;
+        K = S[temp];        // Key used to Xor with input data
+
+        //*Out++ = (u8_t)(ltemp ^ K)&0xff;
+        zmw_tx_buf_writeb(dev, buf, len+ui, (u8_t)(ltemp ^ K)&0xff);
+        ltemp >>= 8;
+    }
+
+    zfwBufSetSize(dev, buf, len+4);
+}
+
+u16_t zfWEPDecrypt(zdev_t *dev, zbuf_t *buf, u16_t offset, u8_t keyLen, u8_t* WepKey, u8_t *iv)
+{
+    u8_t    S[256];
+    u8_t    S2[256];
+    u16_t   ui;
+    u16_t   i;
+    u16_t   j;
+    u32_t   icv_tmp;
+    u32_t   *icv;
+    u32_t   rxbuf_icv;
+    u8_t    temp;
+    u8_t    K;
+    u16_t   len;
+    u8_t    key[32];
+
+    /* Retrieve IV */
+    key[0] = iv[0];
+    key[1] = iv[1];
+    key[2] = iv[2];
+
+    /* Append Wep Key after IV */
+    zfMemoryCopy(&key[3], WepKey, keyLen);
+
+    keyLen += 3;
+
+    for(i = 0; i < 256; i++)
+    {
+        S[i] = (u8_t)i;
+        S2[i] = key[i&(keyLen-1)];
+    }
+
+    j = 0;
+    for(i = 0; i < 256; i++)
+    {
+        j = (j + S[i] + S2[i]);
+        j&=255 ;
+
+        // Swap S[i] and S[j]
+        temp = S[i];
+        S[i] = S[j];
+        S[j] = temp;
+    }
+
+    i = j = 0;
+
+    len = zfwBufGetSize(dev, buf);
+
+    for (ui = offset; ui < len; ui++)
+    {
+        u8_t In;
+
+        i++;
+        i &= 255;
+        j += S[i];
+        j &= 255;
+
+        // Swap S[i] and S[j]
+        temp = S[i];
+        S[i] = S[j];
+        S[j] = temp;
+//          temp = (S[i] + temp) & 255;
+        temp += S[i];
+        temp &=255;
+        K = S[temp];        // Key used to Xor with input data
+
+        In = zmw_rx_buf_readb(dev, buf, ui);
+
+        zmw_rx_buf_writeb(dev, buf, ui, In ^ K);
+    } //End of for (ui = 0; ui < Num_Bytes; ui++)
+
+    icv = &icv_tmp;
+    *icv = -1;
+
+    for (ui = offset; ui < len - 4; ui++)
+    {
+        u8_t In;
+
+        In = zmw_rx_buf_readb(dev, buf, ui);
+        *icv =  (*icv>>8) ^ crc32_tab[(*icv^In)&0xff];
+    }
+
+    *icv = ~*icv;
+
+    rxbuf_icv = (zmw_rx_buf_readb(dev, buf, len-4) |
+         zmw_rx_buf_readb(dev, buf, len-3) << 8 |
+         zmw_rx_buf_readb(dev, buf, len-2) << 16 |
+         zmw_rx_buf_readb(dev, buf, len-1) << 24);
+
+    if (*icv != rxbuf_icv)
+    {
+        return ZM_ICV_FAILURE;
+    }
+
+    return ZM_ICV_SUCCESS;
+}
diff --git a/drivers/staging/otus/80211core/cwm.c b/drivers/staging/otus/80211core/cwm.c
new file mode 100644
index 00000000000..80f1141bf91
--- /dev/null
+++ b/drivers/staging/otus/80211core/cwm.c
@@ -0,0 +1,131 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : cwm.c                                                 */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains channel width related functions.           */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+
+#include "cprecomp.h"
+
+
+
+void zfCwmInit(zdev_t* dev) {
+    //u16_t i;
+    zmw_get_wlan_dev(dev);
+
+    switch (wd->wlanMode) {
+    case ZM_MODE_AP:
+        wd->cwm.cw_mode = CWM_MODE2040;
+        wd->cwm.cw_width = CWM_WIDTH40;
+        wd->cwm.cw_enable = 1;
+        break;
+    case ZM_MODE_INFRASTRUCTURE:
+    case ZM_MODE_PSEUDO:
+    case ZM_MODE_IBSS:
+    default:
+        wd->cwm.cw_mode = CWM_MODE2040;
+        wd->cwm.cw_width = CWM_WIDTH20;
+        wd->cwm.cw_enable = 1;
+        break;
+    }
+}
+
+
+void zfCoreCwmBusy(zdev_t* dev, u16_t busy)
+{
+
+    zmw_get_wlan_dev(dev);
+
+    zm_msg1_mm(ZM_LV_0, "CwmBusy=", busy);
+
+    if(wd->cwm.cw_mode == CWM_MODE20) {
+        wd->cwm.cw_width = CWM_WIDTH20;
+        return;
+    }
+
+    if(wd->cwm.cw_mode == CWM_MODE40) {
+        wd->cwm.cw_width = CWM_WIDTH40;
+        return;
+    }
+
+    if (busy) {
+        wd->cwm.cw_width = CWM_WIDTH20;
+        return;
+    }
+
+
+    if((wd->wlanMode == ZM_MODE_INFRASTRUCTURE || wd->wlanMode == ZM_MODE_PSEUDO ||
+        wd->wlanMode == ZM_MODE_IBSS)) {
+        if (wd->sta.ie.HtCap.HtCapInfo && HTCAP_SupChannelWidthSet != 0 &&
+            wd->sta.ie.HtInfo.ChannelInfo && ExtHtCap_RecomTxWidthSet != 0 &&
+            (wd->sta.ie.HtInfo.ChannelInfo && ExtHtCap_ExtChannelOffsetAbove) == 1) {
+
+            wd->cwm.cw_width = CWM_WIDTH40;
+        }
+        else {
+            wd->cwm.cw_width = CWM_WIDTH20;
+        }
+
+        return;
+    }
+
+    if(wd->wlanMode == ZM_MODE_AP) {
+        wd->cwm.cw_width = CWM_WIDTH40;
+    }
+
+}
+
+
+
+
+u16_t zfCwmIsExtChanBusy(u32_t ctlBusy, u32_t extBusy)
+{
+    u32_t busy; /* percentage */
+    u32_t cycleTime, ctlClear;
+
+    cycleTime = 1280000; //1.28 seconds
+
+    if (cycleTime > ctlBusy) {
+        ctlClear = cycleTime - ctlBusy;
+    }
+    else
+    {
+        ctlClear = 0;
+    }
+
+    /* Compute ratio of extension channel busy to control channel clear
+     * as an approximation to extension channel cleanliness.
+     *
+     * According to the hardware folks, ext rxclear is undefined
+     * if the ctrl rxclear is de-asserted (i.e. busy)
+     */
+    if (ctlClear) {
+        busy = (extBusy * 100) / ctlClear;
+    } else {
+        busy = 0;
+    }
+    if (busy > ATH_CWM_EXTCH_BUSY_THRESHOLD) {
+        return TRUE;
+    }
+
+    return FALSE;
+}
diff --git a/drivers/staging/otus/80211core/cwm.h b/drivers/staging/otus/80211core/cwm.h
new file mode 100644
index 00000000000..40c39fad5f4
--- /dev/null
+++ b/drivers/staging/otus/80211core/cwm.h
@@ -0,0 +1,45 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                          */
+/*  Module Name : cwm.h                                                     */
+/*                                                                          */
+/*  Abstract                                                                */
+/*      This module contains channel width relatived functions.             */
+/*                                                                          */
+/*  NOTES                                                                   */
+/*      None                                                                */
+/*                                                                          */
+/****************************************************************************/
+/*Revision History:                                                         */
+/*    Who         When        What                                          */
+/*    --------    --------    ----------------------------------------------*/
+/*                                                                          */
+/*    Honda       3-19-07     created                                       */
+/*                                                                          */
+/****************************************************************************/
+
+#ifndef _CWM_H
+#define _CWM_H
+
+#define ATH_CWM_EXTCH_BUSY_THRESHOLD  30  /* Extension Channel Busy Threshold (0-100%) */
+
+void zfCwmInit(zdev_t* dev);
+void zfCoreCwmBusy(zdev_t* dev, u16_t busy);
+u16_t zfCwmIsExtChanBusy(u32_t ctlBusy, u32_t extBusy);
+
+
+
+#endif /* #ifndef _CWM_H */
diff --git a/drivers/staging/otus/80211core/freqctrl.c b/drivers/staging/otus/80211core/freqctrl.c
new file mode 100644
index 00000000000..bab0df08d82
--- /dev/null
+++ b/drivers/staging/otus/80211core/freqctrl.c
@@ -0,0 +1,259 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include "cprecomp.h"
+
+/* zfAddFreqChangeReq should be called inside the critical section */
+static void zfAddFreqChangeReq(zdev_t* dev, u16_t frequency, u8_t bw40,
+        u8_t extOffset, zfpFreqChangeCompleteCb cb)
+{
+    zmw_get_wlan_dev(dev);
+
+//printk("zfAddFreqChangeReq  freqReqQueueTail%d\n", wd->freqCtrl.freqReqQueueTail);
+    wd->freqCtrl.freqReqQueue[wd->freqCtrl.freqReqQueueTail] = frequency;
+    wd->freqCtrl.freqReqBw40[wd->freqCtrl.freqReqQueueTail] = bw40;
+    wd->freqCtrl.freqReqExtOffset[wd->freqCtrl.freqReqQueueTail] = extOffset;
+    wd->freqCtrl.freqChangeCompCb[wd->freqCtrl.freqReqQueueTail] = cb;
+    wd->freqCtrl.freqReqQueueTail++;
+    if ( wd->freqCtrl.freqReqQueueTail >= ZM_MAX_FREQ_REQ_QUEUE )
+    {
+        wd->freqCtrl.freqReqQueueTail = 0;
+    }
+}
+
+void zfCoreSetFrequencyV2(zdev_t* dev, u16_t frequency, zfpFreqChangeCompleteCb cb)
+{
+    zfCoreSetFrequencyEx(dev, frequency, 0, 0, cb);
+}
+
+void zfCoreSetFrequencyExV2(zdev_t* dev, u16_t frequency, u8_t bw40,
+        u8_t extOffset, zfpFreqChangeCompleteCb cb, u8_t forceSetFreq)
+{
+    u8_t setFreqImmed = 0;
+    u8_t initRF = 0;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zm_msg1_scan(ZM_LV_1, "Freq=", frequency);
+
+    zmw_enter_critical_section(dev);
+    if ((wd->sta.currentFrequency == frequency)
+        && (wd->sta.currentBw40 == bw40)
+        && (wd->sta.currentExtOffset == extOffset))
+    {
+        if ( forceSetFreq == 0 && wd->sta.flagFreqChanging == 0 )
+        {
+            goto done;
+        }
+    }
+#ifdef ZM_FB50
+    /*if(frequency!=2437) {
+        zmw_leave_critical_section(dev);
+        return;
+    }*/
+#endif
+
+    zfAddFreqChangeReq(dev, frequency, bw40, extOffset, cb);
+
+//    zm_assert( wd->sta.flagFreqChanging == 0 );
+    //wd->sta.flagFreqChanging = 1;
+    if ( wd->sta.flagFreqChanging == 0 )
+    {
+        if ((wd->sta.currentBw40 != bw40) || (wd->sta.currentExtOffset != extOffset))
+        {
+            initRF = 1;
+        }
+        wd->sta.currentFrequency = frequency;
+        wd->sta.currentBw40 = bw40;
+        wd->sta.currentExtOffset = extOffset;
+        setFreqImmed = 1;
+    }
+    wd->sta.flagFreqChanging++;
+
+    zmw_leave_critical_section(dev);
+
+    if ( setFreqImmed )
+    {
+        //zfHpSetFrequency(dev, frequency, 0);
+        if ( forceSetFreq )
+        { // Cold reset to reset the frequency after scanning !
+            zm_debug_msg0("#6_1 20070917");
+            zm_debug_msg0("It is happen!!! No error message");
+            zfHpSetFrequencyEx(dev, frequency, bw40, extOffset, 2);
+        }
+        else
+        {
+        zfHpSetFrequencyEx(dev, frequency, bw40, extOffset, initRF);
+        }
+
+        if (    zfStaIsConnected(dev)
+             && (frequency == wd->frequency)) {
+            wd->sta.connPowerInHalfDbm = zfHpGetTransmitPower(dev);
+        }
+    }
+    return;
+
+done:
+    zmw_leave_critical_section(dev);
+
+    if ( cb != NULL )
+    {
+        cb(dev);
+    }
+    zfPushVtxq(dev);
+    return;
+}
+
+void zfCoreSetFrequencyEx(zdev_t* dev, u16_t frequency, u8_t bw40,
+        u8_t extOffset, zfpFreqChangeCompleteCb cb)
+{
+    zfCoreSetFrequencyExV2(dev, frequency, bw40, extOffset, cb, 0);
+}
+
+void zfCoreSetFrequency(zdev_t* dev, u16_t frequency)
+{
+    zfCoreSetFrequencyV2(dev, frequency, NULL);
+}
+
+/* zfRemoveFreqChangeReq SHOULD NOT be called inside the critical section */
+static void zfRemoveFreqChangeReq(zdev_t* dev)
+{
+    zfpFreqChangeCompleteCb cb = NULL;
+    u16_t frequency;
+    u8_t bw40;
+    u8_t extOffset;
+    u16_t compFreq = 0;
+    u8_t compBw40 = 0;
+    u8_t compExtOffset = 0;
+
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if (wd->freqCtrl.freqReqQueueHead != wd->freqCtrl.freqReqQueueTail)
+    {
+        zm_msg1_scan(ZM_LV_1, "Freq=",
+                wd->freqCtrl.freqReqQueue[wd->freqCtrl.freqReqQueueHead]);
+        compFreq = wd->freqCtrl.freqReqQueue[wd->freqCtrl.freqReqQueueHead];
+        compBw40 = wd->freqCtrl.freqReqBw40[wd->freqCtrl.freqReqQueueHead];
+        compExtOffset = wd->freqCtrl.freqReqExtOffset[wd->freqCtrl.freqReqQueueHead];
+
+        wd->freqCtrl.freqReqQueue[wd->freqCtrl.freqReqQueueHead] = 0;
+        cb = wd->freqCtrl.freqChangeCompCb[wd->freqCtrl.freqReqQueueHead];
+        wd->freqCtrl.freqReqQueueHead++;
+        if ( wd->freqCtrl.freqReqQueueHead >= ZM_MAX_FREQ_REQ_QUEUE )
+        {
+            wd->freqCtrl.freqReqQueueHead = 0;
+        }
+    }
+    zmw_leave_critical_section(dev);
+
+    if ( cb != NULL )
+    {
+        cb(dev);
+    }
+
+    zmw_enter_critical_section(dev);
+    while (wd->freqCtrl.freqReqQueue[wd->freqCtrl.freqReqQueueHead] != 0)
+    {
+        frequency = wd->freqCtrl.freqReqQueue[wd->freqCtrl.freqReqQueueHead];
+        bw40 = wd->freqCtrl.freqReqBw40[wd->freqCtrl.freqReqQueueHead];
+        extOffset=wd->freqCtrl.freqReqExtOffset[wd->freqCtrl.freqReqQueueHead];
+        if ((compFreq == frequency)
+            && (compBw40 == bw40)
+            && (compExtOffset == extOffset))
+        {
+            /* Duplicated frequency command */
+            zm_msg1_scan(ZM_LV_1, "Duplicated Freq=", frequency);
+
+            cb = wd->freqCtrl.freqChangeCompCb[wd->freqCtrl.freqReqQueueHead];
+            wd->freqCtrl.freqReqQueue[wd->freqCtrl.freqReqQueueHead] = 0;
+            wd->freqCtrl.freqReqQueueHead++;
+
+            if ( wd->freqCtrl.freqReqQueueHead >= ZM_MAX_FREQ_REQ_QUEUE )
+            {
+                wd->freqCtrl.freqReqQueueHead = 0;
+            }
+
+            if ( wd->sta.flagFreqChanging != 0 )
+            {
+                wd->sta.flagFreqChanging--;
+            }
+
+            zmw_leave_critical_section(dev);
+            if ( cb != NULL )
+            {
+                cb(dev);
+            }
+            zmw_enter_critical_section(dev);
+        }
+        else
+        {
+            u8_t    initRF = 0;
+            if ((wd->sta.currentBw40 != bw40) || (wd->sta.currentExtOffset != extOffset))
+            {
+                initRF = 1;
+            }
+            wd->sta.currentFrequency = frequency;
+            wd->sta.currentBw40 = bw40;
+            wd->sta.currentExtOffset = extOffset;
+            zmw_leave_critical_section(dev);
+
+            zfHpSetFrequencyEx(dev, frequency, bw40, extOffset, initRF);
+            if (    zfStaIsConnected(dev)
+                && (frequency == wd->frequency)) {
+                wd->sta.connPowerInHalfDbm = zfHpGetTransmitPower(dev);
+            }
+
+            return;
+        }
+    }
+    zmw_leave_critical_section(dev);
+
+    return;
+}
+
+void zfCoreSetFrequencyComplete(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zm_msg1_scan(ZM_LV_1, "flagFreqChanging=", wd->sta.flagFreqChanging);
+
+    zmw_enter_critical_section(dev);
+    //wd->sta.flagFreqChanging = 0;
+    if ( wd->sta.flagFreqChanging != 0 )
+    {
+        wd->sta.flagFreqChanging--;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    zfRemoveFreqChangeReq(dev);
+
+    zfPushVtxq(dev);
+    return;
+}
+
+void zfReSetCurrentFrequency(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    zm_debug_msg0("It is happen!!! No error message");
+
+    zfCoreSetFrequencyExV2(dev, wd->frequency, 0, 0, NULL, 1);
+}
diff --git a/drivers/staging/otus/80211core/ledmgr.c b/drivers/staging/otus/80211core/ledmgr.c
new file mode 100644
index 00000000000..1e104a928ca
--- /dev/null
+++ b/drivers/staging/otus/80211core/ledmgr.c
@@ -0,0 +1,557 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include "cprecomp.h"
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfLedCtrlType1              */
+/*      Traditional single-LED state                                    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.6      */
+/*                                                                      */
+/************************************************************************/
+// bit 15-12 : Toff for Scan state
+//     11-8 : Ton for Scan state
+//     7 : Reserved
+//     6 : mode
+//--------------------------------------
+//     bit 6 = 0
+//     5-4 : Connect state
+//           00 => always off
+//           01 => always on
+//           10 => Idle off, acitve on
+//           11 => Idle on, active off
+//--------------------------------------
+//     bit 6 = 1
+//     5-4 : freq
+//           00 => 1Hz
+//           01 => 0.5Hz
+//           10 => 0.25Hz
+//           11 => 0.125Hz
+//--------------------------------------
+//     3 : Power save state
+//         0 => always off in power save state
+//         1 => works as connect state
+//     2 : Disable state
+//     1 : Reserved
+//     0 : Power-on state
+void zfLedCtrlType1(zdev_t* dev)
+{
+    u16_t i;
+    u32_t ton, toff, tmp, period;
+    zmw_get_wlan_dev(dev);
+
+    for (i=0; i<ZM_MAX_LED_NUMBER; i++)
+    {
+        if (zfStaIsConnected(dev) != TRUE)
+        {
+            //Scan state
+            ton = ((wd->ledStruct.ledMode[i] & 0xf00) >> 8) * 5;
+            toff = ((wd->ledStruct.ledMode[i] & 0xf000) >> 12) * 5;
+
+            if ((ton + toff) != 0)
+            {
+                tmp = wd->ledStruct.counter / (ton+toff);
+                tmp = wd->ledStruct.counter - (tmp * (ton+toff));
+                if (tmp < ton)
+                {
+                    zfHpLedCtrl(dev, i, 1);
+                }
+                else
+                {
+                    zfHpLedCtrl(dev, i, 0);
+                }
+            }
+        }
+        else
+        {
+            if ((zfPowerSavingMgrIsSleeping(dev)) && ((wd->ledStruct.ledMode[i] & 0x8) == 0))
+            {
+                zfHpLedCtrl(dev, i, 0);
+            }
+            else
+            {
+                //Connect state
+                if ((wd->ledStruct.ledMode[i] & 0x40) == 0)
+                {
+                    if ((wd->ledStruct.counter & 1) == 0)
+                    {
+                        zfHpLedCtrl(dev, i, (wd->ledStruct.ledMode[i] & 0x10) >> 4);
+                    }
+                    else
+                    {
+                        if ((wd->ledStruct.txTraffic > 0) || (wd->ledStruct.rxTraffic > 0))
+                        {
+                            wd->ledStruct.txTraffic = wd->ledStruct.rxTraffic = 0;
+                            if ((wd->ledStruct.ledMode[i] & 0x20) != 0)
+                            {
+                                zfHpLedCtrl(dev, i, ((wd->ledStruct.ledMode[i] & 0x10) >> 4)^1);
+                            }
+                        }
+                    }
+                }// if ((wd->ledStruct.ledMode[i] & 0x40) == 0)
+                else
+                {
+                    period = 5 * (1 << ((wd->ledStruct.ledMode[i] & 0x30) >> 4));
+                    tmp = wd->ledStruct.counter / (period*2);
+                    tmp = wd->ledStruct.counter - (tmp * (period*2));
+                    if (tmp < period)
+                    {
+                        if ((wd->ledStruct.counter & 1) == 0)
+                        {
+                            zfHpLedCtrl(dev, i, 0);
+                        }
+                        else
+                        {
+                            if ((wd->ledStruct.txTraffic > 0) || (wd->ledStruct.rxTraffic > 0))
+                            {
+                                wd->ledStruct.txTraffic = wd->ledStruct.rxTraffic = 0;
+                                zfHpLedCtrl(dev, i, 1);
+                            }
+                        }
+                    }
+                    else
+                    {
+                        if ((wd->ledStruct.counter & 1) == 0)
+                        {
+                            zfHpLedCtrl(dev, i, 1);
+                        }
+                        else
+                        {
+                            if ((wd->ledStruct.txTraffic > 0) || (wd->ledStruct.rxTraffic > 0))
+                            {
+                                wd->ledStruct.txTraffic = wd->ledStruct.rxTraffic = 0;
+                                zfHpLedCtrl(dev, i, 0);
+                            }
+                        }
+                    }
+                } //else, if ((wd->ledStruct.ledMode[i] & 0x40) == 0)
+            } //else, if (zfPowerSavingMgrIsSleeping(dev))
+        } //else : if (zfStaIsConnected(dev) != TRUE)
+    } //for (i=0; i<ZM_MAX_LED_NUMBER; i++)
+}
+
+/******************************************************************************/
+/*                                                                            */
+/*    FUNCTION DESCRIPTION                  zfLedCtrlType2                    */
+/*      Customize for Netgear Dual-LED state ((bug#31292))                    */
+/*                                                                            */
+/*      1. Status:  When dongle does not connect to 2.4G or 5G but in site    */
+/*                  survey/association                                        */
+/*         LED status: Slow blinking, Amber then Blue per 500ms               */
+/*      2. Status:	Connection at 2.4G in site survey/association             */
+/*         LED status: Slow blinking, Amber/off per 500ms                     */
+/*      3. Status:	Connection at 5G in site survey/association               */
+/*         LED status: Slow blinking, Blue/off per 500ms                      */
+/*      4. Status:	When transfer the packet                                  */
+/*         LED status: Blink per packet, including TX and RX                  */
+/*      5. Status:	When linking is established but no traffic                */
+/*         LED status: Always on                                              */
+/*      6. Status:	When linking is dropped but no re-connection              */
+/*         LED status: Always off                                             */
+/*      7. Status:	From one connection(2.4G or 5G) to change to another band */
+/*         LED status: Amber/Blue =>Slow blinking, Amber then Blue per 500ms  */
+/*                                                                            */
+/*    INPUTS                                                                  */
+/*      dev : device pointer                                                  */
+/*                                                                            */
+/*    OUTPUTS                                                                 */
+/*      None                                                                  */
+/*                                                                            */
+/*    AUTHOR                                                                  */
+/*      Shang-Chun Liu        Atheros Communications, INC.    2007.11         */
+/*                                                                            */
+/******************************************************************************/
+void zfLedCtrlType2_scan(zdev_t* dev);
+
+void zfLedCtrlType2(zdev_t* dev)
+{
+    u32_t ton, toff, tmp, period;
+    u16_t OperateLED;
+    zmw_get_wlan_dev(dev);
+
+    if (zfStaIsConnected(dev) != TRUE)
+    {
+        // Disconnect state
+        if(wd->ledStruct.counter % 4 != 0)
+    	{
+      	    // Update LED each 400ms(4*100)
+      	    // Prevent this situation
+            //              _______         ___
+            // LED[0] ON   |       |       | x |
+            // ------ OFF->+-+-+-+-+-+-+-+-+-+-+-+->>>...
+            // LED[1] ON
+            //
+            return;
+        }
+
+        if (((wd->state == ZM_WLAN_STATE_DISABLED) && (wd->sta.bChannelScan))
+            || ((wd->state != ZM_WLAN_STATE_DISABLED) && (wd->sta.bAutoReconnect)))
+        {
+            // Scan/AutoReconnect state
+            zfLedCtrlType2_scan(dev);
+        }
+        else
+        {
+            // Neither Connected nor Scan
+            zfHpLedCtrl(dev, 0, 0);
+            zfHpLedCtrl(dev, 1, 0);
+        }
+    }
+    else
+    {
+        if( wd->sta.bChannelScan )
+        {
+            // Scan state
+            if(wd->ledStruct.counter % 4 != 0)
+                return;
+            zfLedCtrlType2_scan(dev);
+            return;
+        }
+
+        if(wd->frequency < 3000)
+        {
+            OperateLED = 0;     // LED[0]: work on 2.4G (b/g band)
+            zfHpLedCtrl(dev, 1, 0);
+        }
+        else
+        {
+            OperateLED = 1;     // LED[1]: work on 5G (a band)
+            zfHpLedCtrl(dev, 0, 0);
+        }
+
+        if ((zfPowerSavingMgrIsSleeping(dev)) && ((wd->ledStruct.ledMode[OperateLED] & 0x8) == 0))
+        {
+            // If Sleeping, turn OFF
+            zfHpLedCtrl(dev, OperateLED, 0);
+        }
+        else
+        {
+            //Connect state
+            if ((wd->ledStruct.counter & 1) == 0)   // even
+            {
+                // No traffic, always ON
+                zfHpLedCtrl(dev, OperateLED, 1);
+            }
+            else       // odd
+            {
+                if ((wd->ledStruct.txTraffic > 0) || (wd->ledStruct.rxTraffic > 0))
+                {
+                    // If have traffic, turn OFF
+		            //                   _____   _   _   _   _____
+		            // LED[Operate] ON        | | | | | | | |
+		            // ------------ OFF->-+-+-+-+-+-+-+-+-+-+-+-+-+->>>...
+		            //
+                    wd->ledStruct.txTraffic = wd->ledStruct.rxTraffic = 0;
+                    zfHpLedCtrl(dev, OperateLED, 0);
+                }
+            }
+        }
+    }
+}
+
+void zfLedCtrlType2_scan(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    // When doing scan, blink(Amber/Blue) and off per 500ms (about 400ms in our driver)
+    //               _______                         _______
+    // LED[0] ON    |       |       8       12      |       |
+    // ------ OFF->-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+->>>...
+    // LED[1] ON    0       4       |_______|       0       3
+    //
+
+    switch(wd->ledStruct.counter % 16)
+    {
+        case 0:   // case 0~3, LED[0] on
+            if(wd->supportMode & ZM_WIRELESS_MODE_24)
+            {
+                zfHpLedCtrl(dev, 0, 1);
+                zfHpLedCtrl(dev, 1, 0);
+            }
+            else
+            {
+                zfHpLedCtrl(dev, 1, 1);
+                zfHpLedCtrl(dev, 0, 0);
+            }
+            break;
+
+        case 8:   // case 8~11, LED[1] on
+            if(wd->supportMode & ZM_WIRELESS_MODE_5)
+            {
+                zfHpLedCtrl(dev, 1, 1);
+                zfHpLedCtrl(dev, 0, 0);
+            }
+            else
+            {
+                zfHpLedCtrl(dev, 0, 1);
+                zfHpLedCtrl(dev, 1, 0);
+            }
+            break;
+
+        default:  // others, all off
+            zfHpLedCtrl(dev, 0, 0);
+            zfHpLedCtrl(dev, 1, 0);
+            break;
+    }
+}
+
+/**********************************************************************************/
+/*                                                                                */
+/*    FUNCTION DESCRIPTION                  zfLedCtrlType3                        */
+/*      Customize for Netgear Single-LED state ((bug#32243))                      */
+/*                                                                                */
+/*  ¡EOff: when the adapter is disabled or hasn't started to associate with AP    */
+/*         yet.                                                                          */
+/*  ¡EOn: Once adpater associate with AP successfully                             */
+/*  ¡ESlow blinking: whenever adapters do site-survey or try to associate with AP */
+/*    - If there is a connection already, and adapters do site-survey or          */
+/*      re-associate action, the LED should keep LED backgraoud as ON, thus       */
+/*      the blinking behavior SHOULD be OFF (200ms) - ON (800ms) and continue this*/
+/*      cycle.                                                                    */
+/*    - If there is no connection yet, and adapters start to do site-survey or    */
+/*      associate action, the LED should keep LED background as OFF, thus the     */
+/*      blinking behavior SHOULD be ON (200ms) - OFF (800ms) and continue this    */
+/*      cycle.                                                                    */
+/*    - For the case that associate fail, adpater should keep associating, and the*/
+/*      LED should also keep slow blinking.                                       */
+/*  ¡EQuick blinking: to blink OFF-ON cycle for each time that traffic packet is  */
+/*    received or is transmitted.                                                 */
+/*                                                                                */
+/*    INPUTS                                                                      */
+/*      dev : device pointer                                                      */
+/*                                                                                */
+/*    OUTPUTS                                                                     */
+/*      None                                                                      */
+/*                                                                                */
+/*    AUTHOR                                                                      */
+/*      Shang-Chun Liu        Atheros Communications, INC.    2008.01             */
+/*                                                                                */
+/**********************************************************************************/
+void zfLedCtrlType3_scan(zdev_t* dev, u16_t isConnect);
+
+void zfLedCtrlType3(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    if (zfStaIsConnected(dev) != TRUE)
+    {
+        // Disconnect state
+        if(wd->ledStruct.counter % 2 != 0)
+    	{
+      	    // Update LED each 200ms(2*100)
+      	    // Prevent this situation
+            //              ___     _
+            // LED[0] ON   |   |   |x|
+            // ------ OFF->+-+-+-+-+-+-+->>>...
+            //
+            return;
+        }
+
+        if (((wd->state == ZM_WLAN_STATE_DISABLED) && (wd->sta.bChannelScan))
+            || ((wd->state != ZM_WLAN_STATE_DISABLED) && (wd->sta.bAutoReconnect)))
+        {
+            // Scan/AutoReconnect state
+            zfLedCtrlType3_scan(dev, 0);
+        }
+        else
+        {
+            // Neither Connected nor Scan
+            zfHpLedCtrl(dev, 0, 0);
+            zfHpLedCtrl(dev, 1, 0);
+        }
+    }
+    else
+    {
+        if( wd->sta.bChannelScan )
+        {
+            // Scan state
+            if(wd->ledStruct.counter % 2 != 0)
+                return;
+            zfLedCtrlType3_scan(dev, 1);
+            return;
+        }
+
+        if ((zfPowerSavingMgrIsSleeping(dev)) && ((wd->ledStruct.ledMode[0] & 0x8) == 0))
+        {
+            // If Sleeping, turn OFF
+            zfHpLedCtrl(dev, 0, 0);
+            zfHpLedCtrl(dev, 1, 0);
+        }
+        else
+        {
+            //Connect state
+            if ((wd->ledStruct.counter & 1) == 0)   // even
+            {
+                // No traffic, always ON
+                zfHpLedCtrl(dev, 0, 1);
+                zfHpLedCtrl(dev, 1, 1);
+            }
+            else       // odd
+            {
+                if ((wd->ledStruct.txTraffic > 0) || (wd->ledStruct.rxTraffic > 0))
+                {
+                    // If have traffic, turn OFF
+		            //                   _____   _   _   _   _____
+		            // LED[Operate] ON        | | | | | | | |
+		            // ------------ OFF->-+-+-+-+-+-+-+-+-+-+-+-+-+->>>...
+		            //
+                    wd->ledStruct.txTraffic = wd->ledStruct.rxTraffic = 0;
+                    zfHpLedCtrl(dev, 0, 0);
+                    zfHpLedCtrl(dev, 1, 0);
+                }
+            }
+        }
+    }
+}
+
+void zfLedCtrlType3_scan(zdev_t* dev, u16_t isConnect)
+{
+    u32_t ton, toff, tmp;
+    zmw_get_wlan_dev(dev);
+
+    // Doing scan when :
+    // 1. Disconnected: ON (200ms) - OFF (800ms) (200ms-600ms in our driver)
+    //               ___             ___             ___
+    // LED[0] ON    |   |           |   |           |   |
+    // ------ OFF->-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+->>>...
+    //              0   2   4   6   8  10  12  14  16
+    // 2. Connected:   ON (800ms) - OFF (200ms) (600ms-200ms in our driver)
+    //               ___________     ___________     ______
+    // LED[0] ON    |           |   |           |   |
+    // ------ OFF->-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+->>>...
+    //              0   2   4   6   8  10  12  14  16
+
+    //Scan state
+    if(!isConnect)
+        ton = 2, toff = 6;
+    else
+        ton = 6, toff = 2;
+
+    if ((ton + toff) != 0)
+    {
+        tmp = wd->ledStruct.counter % (ton+toff);
+       if (tmp < ton)
+        {
+            zfHpLedCtrl(dev, 0, 1);
+            zfHpLedCtrl(dev, 1, 1);
+        }
+        else
+        {
+            zfHpLedCtrl(dev, 0, 0);
+            zfHpLedCtrl(dev, 1, 0);
+        }
+    }
+}
+
+/******************************************************************************/
+/*                                                                            */
+/*    FUNCTION DESCRIPTION                  zfLedCtrl_BlinkWhenScan_Alpha     */
+/*      Customize for Alpha/DLink LED                                         */
+/*      - Blink LED 12 times within 3 seconds when doing Active Scan          */
+/*	                      ___   ___   ___   ___                               */
+/*	      LED[0] ON      |   | |   | |   | |   |                              */
+/*	      -------OFF->-+-+-+-+-+-+-+-+-+-+-+-+-+--+-->>>...                   */
+/*                                                                            */
+/*    INPUTS                                                                  */
+/*      dev : device pointer                                                  */
+/*                                                                            */
+/*    OUTPUTS                                                                 */
+/*      None                                                                  */
+/*                                                                            */
+/*    AUTHOR                                                                  */
+/*      Shang-Chun Liu        Atheros Communications, INC.    2007.11         */
+/*                                                                            */
+/******************************************************************************/
+void zfLedCtrl_BlinkWhenScan_Alpha(zdev_t* dev)
+{
+    static u32_t counter = 0;
+    zmw_get_wlan_dev(dev);
+
+    if(counter > 34)        // counter for 3 sec
+    {
+        wd->ledStruct.LEDCtrlFlag &= ~(u8_t)ZM_LED_CTRL_FLAG_ALPHA;
+        counter = 0;
+    }
+
+    if( (counter % 3) < 2)
+        zfHpLedCtrl(dev, 0, 1);
+    else
+        zfHpLedCtrl(dev, 0, 0);
+
+    counter++;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfLed100msCtrl              */
+/*      LED 100 milliseconds timer.                                     */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.6      */
+/*                                                                      */
+/************************************************************************/
+void zfLed100msCtrl(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    wd->ledStruct.counter++;
+
+    if(wd->ledStruct.LEDCtrlFlag)
+    {
+        switch(wd->ledStruct.LEDCtrlFlag) {
+        case ZM_LED_CTRL_FLAG_ALPHA:
+            zfLedCtrl_BlinkWhenScan_Alpha(dev);
+        break;
+        }
+    }
+    else
+    {
+        switch(wd->ledStruct.LEDCtrlType) {
+        case 1:			// Traditional 1 LED
+            zfLedCtrlType1(dev);
+        break;
+
+        case 2:			// Dual-LEDs for Netgear
+            zfLedCtrlType2(dev);
+        break;
+
+        case 3:			// Single-LED for Netgear (WN111v2)
+            zfLedCtrlType3(dev);
+        break;
+
+        default:
+            zfLedCtrlType1(dev);
+        break;
+        }
+    }
+}
+
diff --git a/drivers/staging/otus/80211core/performance.c b/drivers/staging/otus/80211core/performance.c
new file mode 100644
index 00000000000..51b42d54f65
--- /dev/null
+++ b/drivers/staging/otus/80211core/performance.c
@@ -0,0 +1,431 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : performance.c                                         */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module performance evaluation functions.                   */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+#include "cprecomp.h"
+#ifdef ZM_ENABLE_PERFORMANCE_EVALUATION
+
+#define ZM_TP_SIZE 50
+struct zsSummary zm_summary;
+struct zsVariation zm_var;
+struct zsThroughput zm_tp;
+
+void zfiPerformanceInit(zdev_t* dev)
+{
+    u16_t   i;
+
+    zmw_get_wlan_dev(dev);
+
+    zm_summary.tick_base = wd->tick;
+    zm_summary.tx_msdu_count = 0;
+    zm_summary.tx_mpdu_count = 0;
+    zm_summary.rx_msdu_count = 0;
+    zm_summary.rx_mpdu_count = 0;
+    zm_summary.rx_broken_seq = 0;
+    zm_summary.rx_broken_sum = 0;
+    zm_summary.rx_seq_base = 0;
+    zm_summary.rx_broken_seq_dis = 0;
+    zm_summary.rx_duplicate_seq = 0;
+    zm_summary.rx_old_seq = 0;
+    zm_summary.reset_count = 0;
+    zm_summary.reset_sum = 0;
+    zm_summary.rx_lost_sum = 0;
+    zm_summary.rx_duplicate_error = 0;
+    zm_summary.rx_free = 0;
+    zm_summary.rx_amsdu_len = 0;
+    zm_summary.rx_flush = 0;
+    zm_summary.rx_clear = 0;
+    zm_summary.rx_reorder = 0;
+
+    for (i=0; i<100; i++)
+    {
+        zm_var.tx_msdu_tick[i] = zm_var.tx_mpdu_tick[i] = 0;
+        zm_var.rx_msdu_tick[i] = zm_var.rx_mpdu_tick[i] = 0;
+    }
+
+    zfTimerSchedule(dev, ZM_EVENT_TIMEOUT_PERFORMANCE, 100);
+
+    zm_tp.size = ZM_TP_SIZE;
+    zm_tp.head = zm_tp.size - 1;
+    zm_tp.tail = 0;
+    for (i=0; i<zm_tp.size; i++)
+    {
+        zm_tp.tx[i]=0;
+        zm_tp.rx[i]=0;
+    }
+}
+
+void zfiPerformanceGraph(zdev_t* dev)
+{
+    s16_t   i,j;
+    u8_t    s[ZM_TP_SIZE+5];
+    zmw_get_wlan_dev(dev);
+
+    for (i=0; i<(zm_tp.size-1); i++)
+    {
+        zm_tp.tx[i] = zm_tp.tx[i+1];
+        zm_tp.rx[i] = zm_tp.rx[i+1];
+    }
+    zm_tp.tx[zm_tp.size-1] = zm_summary.tx_mpdu_count*1500*8/1000000;
+    zm_tp.rx[zm_tp.size-1] = zm_summary.rx_msdu_count*1500*8/1000000;
+
+    for (i=15; i>0; i--)
+    {
+        s[0] = (i/10) + '0';
+        s[1] = (i%10) + '0';
+        s[2] = '0';
+        s[3] = '|';
+        for (j=0; j<zm_tp.size; j++)
+        {
+            if ((zm_tp.tx[j]/10 == i) && (zm_tp.rx[j]/10 == i))
+            {
+                s[4+j] = 'X';
+            }
+            else if (zm_tp.tx[j]/10 == i)
+            {
+                s[4+j] = 'T';
+            }
+            else if (zm_tp.rx[j]/10 == i)
+            {
+                s[4+j] = 'R';
+            }
+            else
+            {
+                s[4+j] = ' ';
+            }
+        }
+        s[zm_tp.size+4] = '\0';
+        DbgPrint("%s",s);
+    }
+    DbgPrint("000|__________________________________________________");
+
+}
+
+
+void zfiPerformanceRefresh(zdev_t* dev)
+{
+    u16_t   i;
+
+    zmw_get_wlan_dev(dev);
+
+    zfiDbgReadReg(dev, 0x11772c);
+
+    zm_var.tx_msdu_mean = zm_summary.tx_msdu_count / 100;
+    zm_var.tx_mpdu_mean = zm_summary.tx_mpdu_count / 100;
+    zm_var.rx_msdu_mean = zm_summary.rx_msdu_count / 100;
+    zm_var.rx_mpdu_mean = zm_summary.rx_mpdu_count / 100;
+
+    zm_var.tx_msdu_sum = zm_var.tx_mpdu_sum = 0;
+    zm_var.rx_msdu_sum = zm_var.rx_mpdu_sum = 0;
+    zm_summary.tx_idle_count = zm_summary.rx_idle_count = 0;
+    for (i=0; i<100; i++)
+    {
+        zm_var.tx_msdu_sum += (zm_var.tx_msdu_tick[i] * zm_var.tx_msdu_tick[i]);
+        zm_var.tx_mpdu_sum += (zm_var.tx_mpdu_tick[i] * zm_var.tx_mpdu_tick[i]);
+        zm_var.rx_msdu_sum += (zm_var.rx_msdu_tick[i] * zm_var.rx_msdu_tick[i]);
+        zm_var.rx_mpdu_sum += (zm_var.rx_mpdu_tick[i] * zm_var.rx_mpdu_tick[i]);
+
+        if (!zm_var.tx_mpdu_tick[i]) zm_summary.tx_idle_count++;
+        if (!zm_var.rx_mpdu_tick[i]) zm_summary.rx_idle_count++;
+    }
+    zm_var.tx_msdu_var = (zm_var.tx_msdu_sum / 100) - (zm_var.tx_msdu_mean * zm_var.tx_msdu_mean);
+    zm_var.tx_mpdu_var = (zm_var.tx_mpdu_sum / 100) - (zm_var.tx_mpdu_mean * zm_var.tx_mpdu_mean);
+    zm_var.rx_msdu_var = (zm_var.rx_msdu_sum / 100) - (zm_var.rx_msdu_mean * zm_var.rx_msdu_mean);
+    zm_var.rx_mpdu_var = (zm_var.rx_mpdu_sum / 100) - (zm_var.rx_mpdu_mean * zm_var.rx_mpdu_mean);
+
+    zm_summary.tick_base = wd->tick;
+    zm_summary.rx_broken_sum += zm_summary.rx_broken_seq;
+    zm_summary.rx_lost_sum += (zm_summary.rx_broken_seq - zm_summary.rx_duplicate_seq - zm_summary.rx_old_seq);
+
+    zfiPerformanceGraph(dev);
+
+    DbgPrint("******************************************************\n");
+    DbgPrint("* TX: MSDU=%5d, VAR=%5d; MPDU=%5d, VAR=%5d\n", zm_summary.tx_msdu_count,
+        zm_var.tx_msdu_var, zm_summary.tx_mpdu_count, zm_var.tx_mpdu_var);
+    DbgPrint("* TX: idle=%5d,TxRate=%3d,  PER=%5d\n", zm_summary.tx_idle_count,
+        wd->CurrentTxRateKbps/1000,
+        (u16_t)wd->PER[wd->sta.oppositeInfo[0].rcCell.currentRate]);
+    DbgPrint("* RX: MSDU=%5d, VAR=%5d; MPDU=%5d, VAR=%5d\n", zm_summary.rx_msdu_count,
+        zm_var.rx_msdu_var, zm_summary.rx_mpdu_count, zm_var.rx_mpdu_var);
+    DbgPrint("* RX: idle=%5d,RxRate=%3d,AMSDU=%5d\n", zm_summary.rx_idle_count,
+        wd->CurrentRxRateKbps/1000, zm_summary.rx_amsdu_len);
+    DbgPrint("* RX broken seq=%4d, distances=%4d, duplicates=%4d\n", zm_summary.rx_broken_seq,
+        zm_summary.rx_broken_seq_dis, zm_summary.rx_duplicate_seq);
+    DbgPrint("* RX    old seq=%4d,      lost=%4d, broken sum=%4d\n", zm_summary.rx_old_seq,
+        (zm_summary.rx_broken_seq - zm_summary.rx_duplicate_seq - zm_summary.rx_old_seq),
+        zm_summary.rx_broken_sum);
+    DbgPrint("* Rx   lost sum=%4d,dup. error=%4d, free count=%4d\n", zm_summary.rx_lost_sum,
+        zm_summary.rx_duplicate_error, zm_summary.rx_free);
+    DbgPrint("* Rx  flush sum=%4d, clear sum=%4d, reorder=%7d\n", zm_summary.rx_flush,
+        zm_summary.rx_clear, zm_summary.rx_reorder);
+    DbgPrint("* Firmware reset=%3d, reset sum=%4d\n", zm_summary.reset_count,
+        zm_summary.reset_sum);
+    DbgPrint("******************************************************\n\n");
+    //reset count 11772c
+    zm_summary.tx_msdu_count = 0;
+    zm_summary.tx_mpdu_count = 0;
+    zm_summary.rx_msdu_count = 0;
+    zm_summary.rx_mpdu_count = 0;
+    zm_summary.rx_broken_seq = 0;
+    zm_summary.rx_broken_seq_dis = 0;
+    zm_summary.rx_duplicate_seq = 0;
+    zm_summary.rx_old_seq = 0;
+    zm_summary.reset_count = 0;
+    zm_summary.rx_amsdu_len = 0;
+
+    for (i=0; i<100; i++)
+    {
+        zm_var.tx_msdu_tick[i] = zm_var.tx_mpdu_tick[i] = 0;
+        zm_var.rx_msdu_tick[i] = zm_var.rx_mpdu_tick[i] = 0;
+    }
+
+    zfTimerSchedule(dev, ZM_EVENT_TIMEOUT_PERFORMANCE, 100);
+}
+
+void zfiTxPerformanceMSDU(zdev_t* dev, u32_t tick)
+{
+    u32_t   index;
+    zm_summary.tx_msdu_count++;
+
+    index = tick - zm_summary.tick_base;
+
+    if (index < 100)
+    {
+        zm_var.tx_msdu_tick[index]++;
+    }
+    else
+    {
+        //DbgPrint("wd->tick exceeded tick_base+100!\n");
+    }
+}
+
+void zfiRxPerformanceMSDU(zdev_t* dev, u32_t tick)
+{
+    u32_t   index;
+    zm_summary.rx_msdu_count++;
+
+    index = tick - zm_summary.tick_base;
+
+    if (index < 100)
+    {
+        zm_var.rx_msdu_tick[index]++;
+    }
+    else
+    {
+        //DbgPrint("wd->tick exceeded tick_base+100!\n");
+    }
+}
+
+void zfiTxPerformanceMPDU(zdev_t* dev, u32_t tick)
+{
+    u32_t   index;
+    zm_summary.tx_mpdu_count++;
+
+    index = tick - zm_summary.tick_base;
+
+    if (index < 100)
+    {
+        zm_var.tx_mpdu_tick[index]++;
+    }
+    else
+    {
+        //DbgPrint("wd->tick exceeded tick_base+100!\n");
+    }
+}
+
+#ifndef ZM_INT_USE_EP2_HEADER_SIZE
+#define ZM_INT_USE_EP2_HEADER_SIZE   12
+#endif
+void zfiRxPerformanceMPDU(zdev_t* dev, zbuf_t* buf)
+{
+    u32_t   index;
+    u16_t   frameType;
+    u16_t   frameCtrl;
+    u8_t    mpduInd;
+    u16_t   plcpHdrLen;
+    u16_t   len;
+
+    zmw_get_wlan_dev(dev);
+
+    len = zfwBufGetSize(dev, buf);
+    mpduInd = zmw_rx_buf_readb(dev, buf, len-1);
+    /* First MPDU or Single MPDU */
+    if(((mpduInd & 0x30) == 0x00) || ((mpduInd & 0x30) == 0x20))
+    //if ((mpduInd & 0x10) == 0x00)
+    {
+        plcpHdrLen = 12;        // PLCP header length
+    }
+    else
+    {
+        if (zmw_rx_buf_readh(dev, buf, 4) == wd->macAddr[0] &&
+            zmw_rx_buf_readh(dev, buf, 6) == wd->macAddr[1] &&
+            zmw_rx_buf_readh(dev, buf, 8) == wd->macAddr[2]) {
+            plcpHdrLen = 0;
+        }
+        else if (zmw_rx_buf_readh(dev, buf, 16) == wd->macAddr[0] &&
+                 zmw_rx_buf_readh(dev, buf, 18) == wd->macAddr[1] &&
+                 zmw_rx_buf_readh(dev, buf, 20) == wd->macAddr[2]){
+            plcpHdrLen = 12;
+        }
+        else {
+            plcpHdrLen = 0;
+        }
+    }
+
+    frameCtrl = zmw_rx_buf_readb(dev, buf, plcpHdrLen + 0);
+    frameType = frameCtrl & 0xf;
+
+    if (frameType != ZM_WLAN_DATA_FRAME)
+    {
+        return;
+    }
+
+    zm_summary.rx_mpdu_count++;
+
+    index = wd->tick - zm_summary.tick_base;
+
+    if (index < 100)
+    {
+        zm_var.rx_mpdu_tick[index]++;
+    }
+    else
+    {
+        //DbgPrint("wd->tick exceeded tick_base+100!\n");
+    }
+}
+
+void zfiRxPerformanceSeq(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t   seq_no;
+    u16_t   offset = 0;
+    u16_t   old_dis = zm_summary.rx_broken_seq_dis;
+    //sys_time = KeQueryPerformanceCounter(&freq);
+
+    seq_no = zmw_rx_buf_readh(dev, buf, offset+22) >> 4;
+
+    ZM_SEQ_DEBUG("Out   %5d\n", seq_no);
+
+    if (seq_no < zm_summary.rx_seq_base)
+    {
+        if (seq_no == 0)
+        {
+            if (zm_summary.rx_seq_base != 4095)
+            {
+                zm_summary.rx_broken_seq++;
+                ZM_SEQ_DEBUG("Broken seq");
+                zm_summary.rx_broken_seq_dis+=(4096 - zm_summary.rx_seq_base);
+            }
+        }
+        else if ((seq_no < 300) && (zm_summary.rx_seq_base > 3800))
+        {
+            zm_summary.rx_broken_seq++;
+            ZM_SEQ_DEBUG("Broken seq");
+            zm_summary.rx_broken_seq_dis+=(4096 - zm_summary.rx_seq_base + seq_no);
+        }
+        else
+        {
+            zm_summary.rx_broken_seq++;
+            ZM_SEQ_DEBUG("Broken seq");
+            zm_summary.rx_broken_seq_dis+=(zm_summary.rx_seq_base - seq_no);
+            zm_summary.rx_old_seq++;
+        }
+    }
+    else
+    {
+        if (seq_no != (zm_summary.rx_seq_base + 1))
+        {
+            if ((seq_no > 3800) && (zm_summary.rx_seq_base < 300))
+            {
+                zm_summary.rx_broken_seq++;
+                ZM_SEQ_DEBUG("Broken seq");
+                zm_summary.rx_broken_seq_dis+=(4096 - seq_no + zm_summary.rx_seq_base);
+                zm_summary.rx_old_seq++;
+            }
+            else
+            {
+                zm_summary.rx_broken_seq++;
+                ZM_SEQ_DEBUG("Broken seq");
+                zm_summary.rx_broken_seq_dis+=(seq_no - zm_summary.rx_seq_base);
+            }
+        }
+    }
+    if (seq_no == zm_summary.rx_seq_base)
+    {
+        zm_summary.rx_duplicate_seq++;
+    }
+
+    if ((zm_summary.rx_broken_seq_dis - old_dis) > 100)
+    {
+        DbgPrint("* seq_no=%4d, base_seq=%4d, dis_diff=%4d", seq_no,
+            zm_summary.rx_seq_base, zm_summary.rx_broken_seq_dis - old_dis);
+    }
+    zm_summary.rx_seq_base = seq_no;
+}
+
+void zfiRxPerformanceReg(zdev_t* dev, u32_t reg, u32_t rsp)
+{
+    zm_summary.reset_count = (u16_t)rsp - zm_summary.reset_sum;
+    zm_summary.reset_sum = (u16_t)rsp;
+}
+
+void zfiRxPerformanceDup(zdev_t* dev, zbuf_t* buf1, zbuf_t* buf2)
+{
+    u16_t   seq_no1, seq_no2;
+
+    seq_no1 = zmw_rx_buf_readh(dev, buf1, 22) >> 4;
+    seq_no2 = zmw_rx_buf_readh(dev, buf2, 22) >> 4;
+    if (seq_no1 != seq_no2)
+    {
+        zm_summary.rx_duplicate_error++;
+    }
+}
+
+void zfiRxPerformanceFree(zdev_t* dev, zbuf_t* buf)
+{
+    zm_summary.rx_free++;
+}
+
+void zfiRxPerformanceAMSDU(zdev_t* dev, zbuf_t* buf, u16_t len)
+{
+    if (zm_summary.rx_amsdu_len < len)
+    {
+        zm_summary.rx_amsdu_len = len;
+    }
+}
+void zfiRxPerformanceFlush(zdev_t* dev)
+{
+    zm_summary.rx_flush++;
+}
+
+void zfiRxPerformanceClear(zdev_t* dev)
+{
+    zm_summary.rx_clear++;
+    ZM_SEQ_DEBUG("RxClear");
+}
+
+void zfiRxPerformanceReorder(zdev_t* dev)
+{
+    zm_summary.rx_reorder++;
+}
+#endif /* end of ZM_ENABLE_PERFORMANCE_EVALUATION */
diff --git a/drivers/staging/otus/80211core/performance.h b/drivers/staging/otus/80211core/performance.h
new file mode 100644
index 00000000000..29f658ae477
--- /dev/null
+++ b/drivers/staging/otus/80211core/performance.h
@@ -0,0 +1,97 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#ifndef _PERFORMANCE_H
+#define _PERFORMANCE_H
+
+#ifdef ZM_ENABLE_PERFORMANCE_EVALUATION
+
+struct zsSummary
+{
+    u32_t tx_msdu_count;
+    u32_t tx_mpdu_count;
+    u32_t rx_msdu_count;
+    u32_t rx_mpdu_count;
+    u32_t tick_base;
+    u16_t rx_seq_base;
+    u16_t rx_broken_seq;
+    u16_t rx_broken_sum;
+    u16_t rx_broken_seq_dis;
+    u16_t rx_duplicate_seq;
+    u16_t rx_duplicate_error;
+    u16_t rx_old_seq;
+    u16_t rx_lost_sum;
+    u16_t tx_idle_count;
+    u16_t rx_idle_count;
+    u16_t reset_count;
+    u16_t reset_sum;
+    u16_t rx_free;
+    u16_t rx_amsdu_len;
+    u16_t rx_flush;
+    u16_t rx_clear;
+    u32_t rx_reorder;
+};
+
+struct zsVariation
+{
+    u32_t tx_msdu_tick[100];
+    u32_t tx_mpdu_tick[100];
+    u32_t rx_msdu_tick[100];
+    u32_t rx_mpdu_tick[100];
+
+    u32_t tx_msdu_mean;
+    u32_t tx_mpdu_mean;
+    u32_t rx_msdu_mean;
+    u32_t rx_mpdu_mean;
+
+    u32_t tx_msdu_sum;
+    u32_t tx_mpdu_sum;
+    u32_t rx_msdu_sum;
+    u32_t rx_mpdu_sum;
+
+    u32_t tx_msdu_var;
+    u32_t tx_mpdu_var;
+    u32_t rx_msdu_var;
+    u32_t rx_mpdu_var;
+};
+
+struct zsThroughput
+{
+    u32_t tx[50];
+    u32_t rx[50];
+    u16_t head;
+    u16_t tail;
+    u16_t size;
+    LARGE_INTEGER sys_time;
+    LARGE_INTEGER freq;
+};
+
+void zfiPerformanceInit(zdev_t* dev);
+void zfiPerformanceRefresh(zdev_t* dev);
+
+void zfiTxPerformanceMSDU(zdev_t* dev, u32_t tick);
+void zfiRxPerformanceMSDU(zdev_t* dev, u32_t tick);
+void zfiTxPerformanceMPDU(zdev_t* dev, u32_t tick);
+void zfiRxPerformanceMPDU(zdev_t* dev, zbuf_t* buf);
+void zfiRxPerformanceSeq(zdev_t* dev, zbuf_t* buf);
+void zfiRxPerformanceReg(zdev_t* dev, u32_t reg, u32_t rsp);
+void zfiRxPerformanceDup(zdev_t* dev, zbuf_t* buf1, zbuf_t* buf2);
+void zfiRxPerformanceFree(zdev_t* dev, zbuf_t* buf);
+void zfiRxPerformanceAMSDU(zdev_t* dev, zbuf_t* buf, u16_t len);
+void zfiRxPerformanceFlush(zdev_t* dev);
+void zfiRxPerformanceClear(zdev_t* dev);
+void zfiRxPerformanceReorder(zdev_t* dev);
+#endif /* end of ZM_ENABLE_PERFORMANCE_EVALUATION */
+#endif /* end of _PERFORMANCE_H */
diff --git a/drivers/staging/otus/80211core/pub_usb.h b/drivers/staging/otus/80211core/pub_usb.h
new file mode 100644
index 00000000000..c4b4bd25e82
--- /dev/null
+++ b/drivers/staging/otus/80211core/pub_usb.h
@@ -0,0 +1,102 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef _PUB_USB_H
+#define _PUB_USB_H
+
+#include "../oal_dt.h"
+
+#define ZM_HAL_80211_MODE_AP              0
+#define ZM_HAL_80211_MODE_STA             1
+#define ZM_HAL_80211_MODE_IBSS_GENERAL    2
+#define ZM_HAL_80211_MODE_IBSS_WPA2PSK    3
+
+/* USB module description                                               */
+/* Queue Management                                                     */
+/* 80211core requires OAL to implement a transmission queue in OAL's    */
+/* USB module. Because there is only limited on-chip memory, so USB     */
+/* data transfer may be pending until on-chip memory is available.      */
+/* 80211core also requires OAL's USB module to provide two functions    */
+/* zfwUsbGetFreeTxQSize() and zfwUsbGetMaxTxQSize() for 80211core to    */
+/* query the status of this transmission queue. The main purpose of     */
+/* this queue is for QoS/WMM. Though there are hardware priority        */
+/* queues on the chip, and also software priority queues in the         */
+/* 80211core. There is still one and only one USB channel. So           */
+/* 80211core will use the information that zfwUsbGetFreeTxQSize()       */
+/* returned to schedule the traffic from the software priority          */
+/* queues to the hardware priority queues. For example, if 80211core    */
+/* found that USB transmission queue is going to be full, it will       */
+/* not allow packets with lower priority to enter the USB channel.      */
+
+
+/* Structure for USB call back functions */
+struct zfCbUsbFuncTbl {
+    void (*zfcbUsbRecv)(zdev_t *dev, zbuf_t *buf);
+    void (*zfcbUsbRegIn)(zdev_t* dev, u32_t* rsp, u16_t rspLen);
+    void (*zfcbUsbOutComplete)(zdev_t* dev, zbuf_t *buf, u8_t status, u8_t *hdr);
+    void (*zfcbUsbRegOutComplete)(zdev_t* dev);
+};
+
+/* Call back functions                                                  */
+/* Below are the functions that should be called by the OAL             */
+
+/* When data is available in endpoint 3, OAL shall embed the data in */
+/* zbuf_t and supply to 80211core by calling this function           */
+/* void (*zfcbUsbRecv)(zdev_t *dev, zbuf_t *buf); */
+
+/* When data is available in endpoint 2, OAL shall call this function */
+/* void (*zfcbUsbRegIn)(zdev_t* dev, u32_t* rsp, u16_t rspLen); */
+
+/* When USB data transfer completed in endpoint 1, OAL shall call this function */
+/* void (*zfcbUsbOutComplete)(zdev_t* dev, zbuf_t *buf, u8_t status, u8_t *hdr); */
+
+
+/* Call out functions                                                   */
+/* Below are the functions that supply by the OAL for 80211core to      */
+/* manipulate the USB                                                   */
+
+/* Return OAL's USB TxQ size */
+extern u32_t zfwUsbGetMaxTxQSize(zdev_t* dev);
+
+/* Return OAL's TxQ available size */
+extern u32_t zfwUsbGetFreeTxQSize(zdev_t* dev);
+
+/* Register call back function */
+extern void zfwUsbRegisterCallBack(zdev_t* dev, struct zfCbUsbFuncTbl *zfUsbFunc);
+
+/* Enable USB interrupt endpoint */
+extern u32_t zfwUsbEnableIntEpt(zdev_t *dev, u8_t endpt);
+
+/* Enable USB Rx endpoint */
+extern int zfwUsbEnableRxEpt(zdev_t* dev, u8_t endpt);
+
+/* 80211core call this function to send a USB request over endpoint 0 */
+extern u32_t zfwUsbSubmitControl(zdev_t* dev, u8_t req, u16_t value,
+        u16_t index, void *data, u32_t size);
+extern u32_t zfwUsbSubmitControlIo(zdev_t* dev, u8_t req, u8_t reqtype,
+        u16_t value, u16_t index, void *data, u32_t size);
+
+/* 80211core call this function to transfer data out over endpoint 1 */
+extern void zfwUsbCmd(zdev_t* dev, u8_t endpt, u32_t* cmd, u16_t cmdLen);
+
+/* 80211core call this function to transfer data out over endpoint 4 */
+extern u32_t zfwUsbSend(zdev_t* dev, u8_t endpt, u8_t *hdr, u16_t hdrlen, u8_t *snap, u16_t snapLen,
+                u8_t *tail, u16_t tailLen, zbuf_t *buf, u16_t offset);
+
+/* 80211core call this function to set USB configuration */
+extern u32_t zfwUsbSetConfiguration(zdev_t *dev, u16_t value);
+
+#endif
diff --git a/drivers/staging/otus/80211core/pub_zfi.h b/drivers/staging/otus/80211core/pub_zfi.h
new file mode 100644
index 00000000000..a35bd5d41d2
--- /dev/null
+++ b/drivers/staging/otus/80211core/pub_zfi.h
@@ -0,0 +1,821 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef _PUB_DEFS_H
+#define _PUB_DEFS_H
+
+#include "../oal_dt.h"
+
+/***** Section 1 : Tunable Parameters *****/
+/* The defintions in this section are tunabel parameters */
+
+/* Maximum number of BSS that could be scaned */
+#define ZM_MAX_BSS                          128
+
+/* Maximum number of WPA2 PMKID that supported */
+#define ZM_PMKID_MAX_BSS_CNT               8
+
+/* Enable aggregation and deaggregation */
+#define ZM_ENABLE_AGGREGATION
+
+#ifdef ZM_ENABLE_AGGREGATION
+    /* Enable BA failed retransmission in firmware */
+    #define ZM_ENABLE_FW_BA_RETRANSMISSION
+    #define ZM_BYPASS_AGGR_SCHEDULING
+    //#define ZM_AGGR_BIT_ON
+#endif
+
+
+#ifndef ZM_FB50
+//#define ZM_FB50
+#endif
+
+#ifndef ZM_AP_DEBUG
+//#define ZM_AP_DEBUG
+#endif
+
+//#define ZM_ENABLE_BA_RATECTRL
+
+/***** End of section 1 *****/
+
+
+/***** Section 2 : Public Definitions, data structures and prototypes *****/
+/* function return status */
+#define ZM_STATUS_SUCCESS                   0
+#define ZM_STATUS_FAILURE                   1
+
+// media connect status
+#define ZM_STATUS_MEDIA_CONNECT             0x00
+#define ZM_STATUS_MEDIA_DISCONNECT          0x01
+#define ZM_STATUS_MEDIA_DISCONNECT_NOT_FOUND    0x02
+#define ZM_STATUS_MEDIA_DISABLED            0x03
+#define ZM_STATUS_MEDIA_CONNECTION_DISABLED 0x04
+#define ZM_STATUS_MEDIA_CONNECTION_RESET    0x05
+#define ZM_STATUS_MEDIA_RESET               0x06
+#define ZM_STATUS_MEDIA_DISCONNECT_DEAUTH   0x07
+#define ZM_STATUS_MEDIA_DISCONNECT_DISASOC  0x08
+#define ZM_STATUS_MEDIA_DISCONNECT_TIMEOUT  0x09
+#define ZM_STATUS_MEDIA_DISCONNECT_AUTH_FAILED  0x0a
+#define ZM_STATUS_MEDIA_DISCONNECT_ASOC_FAILED  0x0b
+#define ZM_STATUS_MEDIA_DISCONNECT_MIC_FAIL   0x0c
+#define ZM_STATUS_MEDIA_DISCONNECT_UNREACHABLE 0x0d
+#define ZM_STATUS_MEDIA_DISCONNECT_BEACON_MISS  0x0e
+
+// Packet Filter
+#define ZM_PACKET_TYPE_DIRECTED             0x00000001
+#define ZM_PACKET_TYPE_MULTICAST            0x00000002
+#define ZM_PACKET_TYPE_ALL_MULTICAST        0x00000004
+#define ZM_PACKET_TYPE_BROADCAST            0x00000008
+#define ZM_PACKET_TYPE_PROMISCUOUS          0x00000020
+
+/* BSS mode definition */
+/* TODO : The definitions here are coupled with XP's NDIS OID. */
+/*        We can't be changed them freely, need to disarm this mine */
+#define ZM_MODE_IBSS                        0
+#define ZM_MODE_INFRASTRUCTURE              1
+#define ZM_MODE_UNKNOWN                     2
+#define ZM_MODE_INFRASTRUCTURE_MAX          3
+#define ZM_MODE_AP                          4
+#define ZM_MODE_PSEUDO                      5
+
+
+/* Authentication mode */
+#define ZM_AUTH_MODE_OPEN                   0
+#define ZM_AUTH_MODE_SHARED_KEY             1
+#define ZM_AUTH_MODE_AUTO                   2
+#define ZM_AUTH_MODE_WPA                    3
+#define ZM_AUTH_MODE_WPAPSK                 4
+#define ZM_AUTH_MODE_WPA_NONE               5
+#define ZM_AUTH_MODE_WPA2                   6
+#define ZM_AUTH_MODE_WPA2PSK                7
+#ifdef ZM_ENABLE_CENC
+#define ZM_AUTH_MODE_CENC                   8
+#endif //ZM_ENABLE_CENC
+#define ZM_AUTH_MODE_WPA_AUTO               9
+#define ZM_AUTH_MODE_WPAPSK_AUTO            10
+
+// Encryption mode
+#define ZM_NO_WEP                           0x0
+#define ZM_AES                              0x4
+#define ZM_TKIP                             0x2
+#define ZM_WEP64                            0x1
+#define ZM_WEP128                           0x5
+#define ZM_WEP256                           0x6
+#ifdef ZM_ENABLE_CENC
+#define ZM_CENC                             0x7
+#endif //ZM_ENABLE_CENC
+
+/* Encryption type for wep status */
+#define ZM_ENCRYPTION_WEP_DISABLED          0
+#define ZM_ENCRYPTION_WEP_ENABLED           1
+#define ZM_ENCRYPTION_WEP_KEY_ABSENT        2
+#define ZM_ENCRYPTION_NOT_SUPPORTED         3
+#define ZM_ENCRYPTION_TKIP                  4
+#define ZM_ENCRYPTION_TKIP_KEY_ABSENT       5
+#define ZM_ENCRYPTION_AES                   6
+#define ZM_ENCRYPTION_AES_KEY_ABSENT        7
+
+#ifdef ZM_ENABLE_CENC
+#define ZM_ENCRYPTION_CENC                  8
+#endif //ZM_ENABLE_CENC
+
+/* security type */
+#define ZM_SECURITY_TYPE_NONE               0
+#define ZM_SECURITY_TYPE_WEP                1
+#define ZM_SECURITY_TYPE_WPA                2
+
+#ifdef ZM_ENABLE_CENC
+#define ZM_SECURITY_TYPE_CENC               3
+#endif //ZM_ENABLE_CENC
+
+/* Encryption Exemption Action Type  */
+#define ZM_ENCRYPTION_EXEMPT_NO_EXEMPTION   0
+#define ZM_ENCRYPTION_EXEMPT_ALWAYS         1
+
+/* MIC failure */
+#define ZM_MIC_PAIRWISE_ERROR               0x06
+#define ZM_MIC_GROUP_ERROR                  0x0E
+
+
+/* power save mode */
+#define ZM_STA_PS_NONE                    0
+#define ZM_STA_PS_MAX                     1
+#define ZM_STA_PS_FAST                    2
+#define ZM_STA_PS_LIGHT                   3
+
+/* WME AC Type */
+#define ZM_WME_AC_BK                        0       /* Background AC */
+#define ZM_WME_AC_BE                        1       /* Best-effort AC */
+#define ZM_WME_AC_VIDEO                     2       /* Video AC */
+#define ZM_WME_AC_VOICE                     3       /* Voice AC */
+
+/* Preamble type */
+#define ZM_PREAMBLE_TYPE_AUTO               0
+#define ZM_PREAMBLE_TYPE_LONG               1
+#define ZM_PREAMBLE_TYPE_SHORT              2
+
+/* wireless modes constants */
+#define ZM_WIRELESS_MODE_5_54        0x01   ///< 5 GHz 54 Mbps
+#define ZM_WIRELESS_MODE_5_108       0x02   ///< 5 GHz 108 Mbps
+#define ZM_WIRELESS_MODE_24_11       0x04   ///< 2.4 GHz 11 Mbps
+#define ZM_WIRELESS_MODE_24_54       0x08   ///< 2.4 GHz 54 Mbps
+#define ZM_WIRELESS_MODE_24_108      0x10   ///< 2.4 GHz 108 Mbps
+#define ZM_WIRELESS_MODE_49_13      0x100   ///< 4.9 GHz 13.5 Mbps, quarter rate chn-bandwidth = 5
+#define ZM_WIRELESS_MODE_49_27      0x200   ///< 4.9 GHz 27 Mbps, half rate chn-bandwidth = 10
+#define ZM_WIRELESS_MODE_49_54      0x400   ///< 4.9 GHz 54 Mbps, full rate chn-bandwidth = 20
+#define ZM_WIRELESS_MODE_5_300     0x1000   ///< 5 GHz 300 Mbps
+#define ZM_WIRELESS_MODE_24_300    0x2000   ///< 2.4 GHz 300 Mbps
+#define ZM_WIRELESS_MODE_5_130     0x4000   ///< 5 GHz 130 Mbps
+#define ZM_WIRELESS_MODE_24_130    0x8000   ///< 2.4 GHz 130 Mbps
+
+#define ZM_WIRELESS_MODE_24_N      (ZM_WIRELESS_MODE_24_130|ZM_WIRELESS_MODE_24_300)
+#define ZM_WIRELESS_MODE_5_N       (ZM_WIRELESS_MODE_5_130|ZM_WIRELESS_MODE_5_300)
+#define ZM_WIRELESS_MODE_24        (ZM_WIRELESS_MODE_24_11|ZM_WIRELESS_MODE_24_54|ZM_WIRELESS_MODE_24_N)
+#define ZM_WIRELESS_MODE_5         (ZM_WIRELESS_MODE_5_54|ZM_WIRELESS_MODE_5_N)
+
+/* AdHoc Mode with different band */
+#define ZM_ADHOCBAND_A         1
+#define ZM_ADHOCBAND_B         2
+#define ZM_ADHOCBAND_G         3
+#define ZM_ADHOCBAND_BG        4
+#define ZM_ADHOCBAND_ABG       5
+
+/* Authentication algorithm in the field algNo of authentication frames */
+#define ZM_AUTH_ALGO_OPEN_SYSTEM      0x10000    /* Open system */
+#define ZM_AUTH_ALGO_SHARED_KEY       0x10001    /* Shared Key */
+#define ZM_AUTH_ALGO_LEAP             0x10080    /* Leap */
+
+struct zsScanResult
+{
+    u32_t reserved;
+};
+
+
+struct zsStastics
+{
+    u32_t reserved;
+};
+
+#define ZM_MAX_SUPP_RATES_IE_SIZE       12
+#define ZM_MAX_IE_SIZE                  50 //100
+#define ZM_MAX_WPS_IE_SIZE              150
+#define ZM_MAX_PROBE_FRAME_BODY_SIZE    512//300
+#define	ZM_MAX_COUNTRY_INFO_SIZE		20
+
+#define ZM_MAX_SSID_LENGTH          32
+struct zsBssInfo
+{
+    u8_t   macaddr[6];
+    u8_t   bssid[6];
+    u8_t   beaconInterval[2];
+    u8_t   capability[2];
+    u8_t   timeStamp[8];
+    u8_t   ssid[ZM_MAX_SSID_LENGTH + 2];   // EID(1) + Length(1) + SSID(32)
+    u8_t   supportedRates[ZM_MAX_SUPP_RATES_IE_SIZE + 2]; // EID(1) + Length(1) + supported rates [12]
+    u8_t   channel;
+    u16_t  frequency;
+    u16_t  atimWindow;
+    u8_t   erp;
+    u8_t   extSupportedRates[ZM_MAX_SUPP_RATES_IE_SIZE + 2]; // EID(1) + Length(1) + extended supported rates [12]
+    u8_t   wpaIe[ZM_MAX_IE_SIZE + 2];
+    u8_t   wscIe[ZM_MAX_WPS_IE_SIZE + 2];
+    u8_t   rsnIe[ZM_MAX_IE_SIZE + 2];
+#ifdef ZM_ENABLE_CENC
+    u8_t   cencIe[ZM_MAX_IE_SIZE + 2]; /* CENC */ /* half size because of memory exceed 64k boundary */
+#endif //ZM_ENABLE_CENC
+    u8_t   securityType;
+    u8_t   signalStrength;
+    u8_t   signalQuality;
+    u16_t  sortValue;
+    u8_t   wmeSupport;
+    u8_t   flag;
+    u8_t   EnableHT;
+    u8_t   enableHT40;
+    u8_t   SG40;
+    u8_t   extChOffset;
+    u8_t   apCap; // bit0:11N AP
+    u16_t  frameBodysize;
+    u8_t   frameBody[ZM_MAX_PROBE_FRAME_BODY_SIZE];
+    u8_t   countryInfo[ZM_MAX_COUNTRY_INFO_SIZE + 2];
+    u16_t  athOwlAp;
+    u16_t  marvelAp;
+    u16_t  broadcomHTAp;
+    u32_t  tick;
+    struct zsBssInfo* next;
+};
+
+struct zsBssList
+{
+    u8_t bssCount;
+    struct zsBssInfo* head;
+    struct zsBssInfo* tail;
+};
+
+struct zsBssListV1
+{
+    u8_t bssCount;
+    struct zsBssInfo bssInfo[ZM_MAX_BSS];
+};
+
+#define ZM_KEY_FLAG_GK                 0x0001
+#define ZM_KEY_FLAG_PK                 0X0002
+#define ZM_KEY_FLAG_AUTHENTICATOR      0x0004
+#define ZM_KEY_FLAG_INIT_IV            0x0008
+#define ZM_KEY_FLAG_DEFAULT_KEY        0x0010
+
+#ifdef ZM_ENABLE_CENC
+#define ZM_KEY_FLAG_CENC               0x0020
+#endif //ZM_ENABLE_CENC
+
+// Comment: For TKIP, key[0]~key[15]  => TKIP key
+//                    key[16]~key[23] => Tx MIC key
+//                    key[24]~key[31] => Rx MIC key
+struct zsKeyInfo
+{
+    u8_t*   key;
+    u8_t    keyLength;
+    u8_t    keyIndex;
+    u8_t*   initIv;
+    u16_t   flag;
+    u8_t    vapId;
+    u16_t    vapAddr[3];
+    u16_t*   macAddr;
+};
+
+
+
+/*
+ * Channels are specified by frequency.
+ */
+typedef struct {
+	u16_t	channel;	/* setting in Mhz */
+	u32_t	channelFlags;	/* see below */
+	u8_t	privFlags;
+	s8_t	maxRegTxPower;	/* max regulatory tx power in dBm */
+	s8_t	maxTxPower;	/* max true tx power in 0.25 dBm */
+	s8_t	minTxPower;	/* min true tx power in 0.25 dBm */
+} ZM_HAL_CHANNEL;
+
+struct zsRegulationTable
+{
+    u16_t   regionCode;
+    u16_t   CurChIndex;
+    u16_t   allowChannelCnt;
+    ZM_HAL_CHANNEL   allowChannel[60];   /* 2.4GHz: 14 channels, 5 GHz: 31 channels */
+};
+
+struct zsPartnerNotifyEvent
+{
+    u8_t bssid[6];                      // The BSSID of IBSS
+    u8_t peerMacAddr[6];                // The MAC address of peer station
+};
+
+#define ZM_RC_TRAINED_BIT   0x1
+struct zsRcCell
+{
+    u32_t txCount;
+    u32_t failCount;
+    u8_t currentRate;
+    u8_t currentRateIndex;
+    u32_t probingTime;
+    u8_t operationRateSet[24];
+    u8_t operationRateCount;
+    u16_t rxRssi;
+    u8_t flag;
+    u32_t  lasttxCount;
+    u32_t  lastTime;
+};
+
+struct zsOppositeInfo
+{
+    u8_t            macAddr[6];
+    struct zsRcCell rcCell;
+    u8_t            valid;              // This indicate if this opposite is still valid
+    u8_t            aliveCounter;
+    u8_t            pkInstalled;
+
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+    /* For WPA2PSK ! */
+    u8_t 			wpaState;
+    u8_t            camIdx;
+    u8_t            encryMode;
+    u16_t           iv16;
+    u32_t           iv32;
+#endif
+};
+
+typedef void (*zfpIBSSIteratePeerStationCb)(
+    zdev_t* dev, struct zsOppositeInfo *peerInfo, void *ctx, u8_t index);
+
+typedef u16_t (*zfpStaRxSecurityCheckCb)(zdev_t* dev, zbuf_t* buf);
+
+
+/* Communication Tally data structure */
+struct zsCommTally
+{
+    u32_t txUnicastFrm;		    //  0 txUnicastFrames
+    u32_t txMulticastFrm;		//  1 txMulticastFrames
+    u32_t txUnicastOctets;	    //  2 txUniOctets  byte size
+    u32_t txMulticastOctets;	//  3 txMultiOctets  byte size
+    u32_t txFrmUpperNDIS;       //  4
+    u32_t txFrmDrvMgt;          //  5
+    u32_t RetryFailCnt;		    //  6
+    u32_t Hw_TotalTxFrm;		//  7 Hardware total Tx Frame
+    u32_t Hw_RetryCnt;		    //  8 txMultipleRetriesFrames
+    u32_t Hw_UnderrunCnt;       //  9
+
+    u32_t DriverRxFrmCnt;       // 10
+    u32_t rxUnicastFrm;		    // 11 rxUnicastFrames
+    u32_t rxMulticastFrm;	    // 12rxMulticastFrames
+
+    u32_t NotifyNDISRxFrmCnt;   // 14
+    u32_t rxUnicastOctets;	// 15 rxUniOctets  byte size
+    u32_t rxMulticastOctets;	    // 16 rxMultiOctets  byte size
+    u32_t DriverDiscardedFrm;       // 17 Discard by ValidateFrame
+    u32_t LessThanDataMinLen;       // 18
+    u32_t GreaterThanMaxLen;        // 19
+    u32_t DriverDiscardedFrmCauseByMulticastList;
+    u32_t DriverDiscardedFrmCauseByFrmCtrl;
+    u32_t rxNeedFrgFrm;		    // 22 need more frg frm
+    u32_t DriverRxMgtFrmCnt;
+    u32_t rxBroadcastFrm;	    // 24 Receive broadcast frame count
+    u32_t rxBroadcastOctets;	// 25 Receive broadcast frame byte size
+    u32_t rx11bDataFrame;		// 26 Measured quality 11b data frame count
+    u32_t rxOFDMDataFrame;	    // 27 Measured quality 11g data frame count
+
+
+    u32_t Hw_TotalRxFrm;        // 28
+    u32_t Hw_CRC16Cnt;		    // 29 rxPLCPCRCErrCnt
+    u32_t Hw_CRC32Cnt;		    // 30 rxCRC32ErrCnt
+    u32_t Hw_DecrypErr_UNI;     // 31
+    u32_t Hw_DecrypErr_Mul;     // 32
+
+    u32_t Hw_RxFIFOOverrun;     // 34
+    u32_t Hw_RxTimeOut;         // 35
+    u32_t LossAP;               // 36
+
+    u32_t Tx_MPDU;              // 37
+    u32_t BA_Fail;              // 38
+    u32_t Hw_Tx_AMPDU;          // 39
+    u32_t Hw_Tx_MPDU;           // 40
+
+    u32_t RateCtrlTxMPDU;
+    u32_t RateCtrlBAFail;
+
+    u32_t txQosDropCount[5];    //41 42 43 44 45
+
+	u32_t Hw_RxMPDU;            // 46
+	u32_t Hw_RxDropMPDU;        // 47
+	u32_t Hw_RxDelMPDU;         // 48
+
+	u32_t Hw_RxPhyMiscError;    // 49
+	u32_t Hw_RxPhyXRError;      // 50
+    u32_t Hw_RxPhyOFDMError;    // 51
+    u32_t Hw_RxPhyCCKError;     // 52
+    u32_t Hw_RxPhyHTError;      // 53
+    u32_t Hw_RxPhyTotalCount;   // 54
+
+    u32_t swRxFragmentCount;         // 55
+    u32_t swRxUnicastMicFailCount;   // 56
+    u32_t swRxMulticastMicFailCount; // 57
+    u32_t swRxDropUnencryptedCount;  // 58
+
+    u32_t txBroadcastFrm;
+    u32_t txBroadcastOctets;
+};
+
+/* Traffic Monitor Tally data structure */
+struct zsTrafTally
+{
+    u32_t rxDuplicate;
+    u32_t rxSrcIsOwnMac;
+    //u32_t rxDataFrameCount;
+    //u32_t rxDataByteCount;
+    //u32_t rxDataBytesIn1000ms;
+    //u32_t rxDataTmpFor1000ms;
+    //u32_t rxDataBytesIn2000ms;
+    //u32_t rxDataTmpFor2000ms;
+
+    //u32_t txDataFrameCount;
+    //u32_t txDataByteCount;
+    //u32_t txDataBytesIn1000ms;
+    //u32_t txDataTmpFor1000ms;
+    u32_t txDataBytesIn2000ms;
+    u32_t txDataTmpFor2000ms;
+};
+
+/* Hal rx packet moniter information */
+struct zsMonHalRxInfo
+{
+    u32_t currentRSSI[7];
+    u32_t currentRxEVM[14];
+    u32_t currentRxDataMT;
+    u32_t currentRxDataMCS;
+    u32_t currentRxDataBW;
+    u32_t currentRxDataSG;
+};
+
+struct zsTail
+{
+    u8_t SignalStrength1;
+    u8_t SignalStrength2;
+    u8_t SignalStrength3;
+    u8_t SignalQuality;
+    u8_t SAIndex;
+    u8_t DAIndex;
+    u8_t ErrorIndication;
+    u8_t RxMacStatus;
+};
+
+union zuTail
+{
+    struct zsTail Data;
+    u8_t Byte[8];
+};
+
+struct zsAdditionInfo
+{
+    u8_t PlcpHeader[12];
+    union zuTail   Tail;
+};
+
+
+struct zsPmkidBssidInfo
+{
+    u16_t      bssid[3];
+    u8_t       pmkid[16];
+};
+
+struct zsPmkidInfo
+{
+	   u32_t		bssidCount;
+	   struct zsPmkidBssidInfo	bssidInfo[ZM_PMKID_MAX_BSS_CNT];
+};
+
+
+struct zsCbFuncTbl
+{
+    u16_t (*zfcbAuthNotify)(zdev_t* dev, u16_t* macAddr);
+    u16_t (*zfcbAsocNotify)(zdev_t* dev, u16_t* macAddr, u8_t* body,
+            u16_t bodySize, u16_t port);
+    u16_t (*zfcbDisAsocNotify)(zdev_t* dev, u8_t* macAddr, u16_t port);
+    u16_t (*zfcbApConnectNotify)(zdev_t* dev, u8_t* macAddr, u16_t port);
+    void (*zfcbConnectNotify)(zdev_t* dev, u16_t status, u16_t* bssid);
+    void (*zfcbScanNotify)(zdev_t* dev, struct zsScanResult* result);
+    void (*zfcbMicFailureNotify)(zdev_t* dev, u16_t* addr, u16_t status);
+    void (*zfcbApMicFailureNotify)(zdev_t* dev, u8_t* addr, zbuf_t* buf);
+    void (*zfcbIbssPartnerNotify)(zdev_t* dev, u16_t status,
+            struct zsPartnerNotifyEvent *event);
+    void (*zfcbMacAddressNotify)(zdev_t* dev, u8_t* addr);
+    void (*zfcbSendCompleteIndication)(zdev_t* dev, zbuf_t* buf);
+    void (*zfcbRecvEth)(zdev_t* dev, zbuf_t* buf, u16_t port);
+    void (*zfcbRecv80211)(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* addInfo);
+    void (*zfcbRestoreBufData)(zdev_t* dev, zbuf_t* buf);
+#ifdef ZM_ENABLE_CENC
+    u16_t (*zfcbCencAsocNotify)(zdev_t* dev, u16_t* macAddr, u8_t* body,
+            u16_t bodySize, u16_t port);
+#endif //ZM_ENABLE_CENC
+    u8_t (*zfcbClassifyTxPacket)(zdev_t* dev, zbuf_t* buf);
+
+    void (*zfcbHwWatchDogNotify)(zdev_t* dev);
+};
+
+extern void zfZeroMemory(u8_t* va, u16_t length);
+#define ZM_INIT_CB_FUNC_TABLE(p)        zfZeroMemory((u8_t *)p, sizeof(struct zsCbFuncTbl));
+
+//extern struct zsWlanDev zgWlanDev;
+
+/* Initialize WLAN hardware and software, resource will be allocated */
+/* for WLAN operation, must be called first before other function.   */
+extern u16_t zfiWlanOpen(zdev_t* dev, struct zsCbFuncTbl* cbFuncTbl);
+
+/* WLAN hardware will be shutdown and all resource will be release */
+extern u16_t zfiWlanClose(zdev_t* dev);
+
+/* Enable/disable Wlan operation */
+extern u16_t zfiWlanEnable(zdev_t* dev);
+extern u16_t zfiWlanDisable(zdev_t* dev, u8_t ResetKeyCache);
+extern u16_t zfiWlanResume(zdev_t* dev, u8_t doReconn);
+extern u16_t zfiWlanSuspend(zdev_t* dev);
+
+/* Enable/disable ISR interrupt */
+extern u16_t zfiWlanInterruptEnable(zdev_t* dev);
+extern u16_t zfiWlanInterruptDisable(zdev_t* dev);
+
+/* Do WLAN site survey */
+extern u16_t zfiWlanScan(zdev_t* dev);
+
+/* Get WLAN stastics */
+extern u16_t zfiWlanGetStatistics(zdev_t* dev);
+
+/* Reset WLAN */
+extern u16_t zfiWlanReset(zdev_t* dev);
+
+/* Deauthenticate a STA */
+extern u16_t zfiWlanDeauth(zdev_t* dev, u16_t* macAddr, u16_t reason);
+
+extern u16_t zfiTxSendEth(zdev_t* dev, zbuf_t* buf, u16_t port);
+extern u8_t zfiIsTxQueueFull(zdev_t* dev);
+extern u16_t zfiTxSend80211Mgmt(zdev_t* dev, zbuf_t* buf, u16_t port);
+
+extern void zfiIsrPci(zdev_t* dev);
+
+extern u8_t zfiWlanIBSSGetPeerStationsCount(zdev_t* dev);
+extern u8_t zfiWlanIBSSIteratePeerStations(zdev_t* dev, u8_t numToIterate, zfpIBSSIteratePeerStationCb callback, void *ctx);
+extern void zfiWlanFlushAllQueuedBuffers(zdev_t* dev);
+
+/* coid.c */
+extern void zfiWlanQueryMacAddress(zdev_t* dev, u8_t* addr);
+
+extern u16_t zfiGlobalDataSize(zdev_t* dev);
+
+extern void zfiHeartBeat(zdev_t* dev);
+
+extern void zfiWlanSetWlanMode(zdev_t* dev, u8_t wlanMode);
+extern void zfiWlanSetAuthenticationMode(zdev_t* dev, u8_t authMode);
+extern void zfiWlanSetWepStatus(zdev_t* dev, u8_t wepStatus);
+extern void zfiWlanSetSSID(zdev_t* dev, u8_t* ssid, u8_t ssidLength);
+extern void zfiWlanSetFragThreshold(zdev_t* dev, u16_t fragThreshold);
+extern void zfiWlanSetRtsThreshold(zdev_t* dev, u16_t rtsThreshold);
+extern void zfiWlanSetFrequency(zdev_t* dev, u32_t frequency, u8_t bImmediate);
+extern void zfiWlanSetBssid(zdev_t* dev, u8_t* bssid);
+extern void zfiWlanSetBeaconInterval(zdev_t* dev, u16_t beaconInterval,
+                              u8_t bImmediate);
+extern void zfiWlanSetDtimCount(zdev_t* dev, u8_t  dtim);
+extern void zfiWlanSetAtimWindow(zdev_t* dev, u16_t atimWindow, u8_t bImmediate);
+extern void zfiWlanSetEncryMode(zdev_t* dev, u8_t encryMode);
+extern u8_t zfiWlanSetKey(zdev_t* dev, struct zsKeyInfo keyInfo);
+extern u8_t zfiWlanPSEUDOSetKey(zdev_t* dev, struct zsKeyInfo keyInfo);
+extern void zfiWlanSetPowerSaveMode(zdev_t* dev, u8_t mode);
+extern void zfiWlanQueryBssListV1(zdev_t* dev, struct zsBssListV1* bssListV1);
+extern void zfiWlanQueryBssList(zdev_t* dev, struct zsBssList* pBssList);
+extern void zfiWlanSetProtectionMode(zdev_t* dev, u8_t mode);
+extern void zfiWlanFlushBssList(zdev_t* dev);
+
+void zfiWlanDisableDfsChannel(zdev_t* dev, u8_t disableFlag);
+
+extern u8_t zfiWlanQueryWlanMode(zdev_t* dev);
+extern u16_t zfiWlanChannelToFrequency(zdev_t* dev, u8_t channel);
+extern u8_t zfiWlanFrequencyToChannel(zdev_t* dev, u16_t freq);
+
+#define ZM_WLAN_STATE_OPENED        0
+#define ZM_WLAN_STATE_ENABLED       1
+#define ZM_WLAN_STATE_DISABLED      2
+#define ZM_WLAN_STATE_CLOSEDED      3
+extern u8_t zfiWlanQueryAdapterState(zdev_t* dev);
+extern u8_t zfiWlanQueryAuthenticationMode(zdev_t* dev, u8_t bWrapper);
+extern u8_t zfiWlanQueryWepStatus(zdev_t* dev, u8_t bWrapper);
+extern void zfiWlanQuerySSID(zdev_t* dev, u8_t* ssid, u8_t* pSsidLength);
+extern u16_t zfiWlanQueryFragThreshold(zdev_t* dev);
+extern u16_t zfiWlanQueryRtsThreshold(zdev_t* dev);
+extern u32_t zfiWlanQueryFrequency(zdev_t* dev);
+extern u32_t zfiWlanQueryCurrentFrequency(zdev_t* dev, u8_t qmode);
+extern u32_t zfiWlanQueryFrequencyAttribute(zdev_t* dev, u32_t frequency);
+extern void zfiWlanQueryFrequencyHT(zdev_t* dev, u32_t *bandWidth, u32_t *extOffset);
+extern u8_t zfiWlanQueryCWMode(zdev_t* dev);
+extern u32_t zfiWlanQueryCWEnable(zdev_t* dev);
+extern void zfiWlanQueryBssid(zdev_t* dev, u8_t* bssid);
+extern u16_t zfiWlanQueryBeaconInterval(zdev_t* dev);
+extern u32_t zfiWlanQueryRxBeaconTotal(zdev_t* dev);
+extern u16_t zfiWlanQueryAtimWindow(zdev_t* dev);
+extern u8_t zfiWlanQueryEncryMode(zdev_t* dev);
+extern u16_t zfiWlanQueryCapability(zdev_t* dev);
+extern u16_t zfiWlanQueryAid(zdev_t* dev);
+extern void zfiWlanQuerySupportRate(zdev_t* dev, u8_t* rateArray, u8_t* pLength);
+extern void zfiWlanQueryExtSupportRate(zdev_t* dev, u8_t* rateArray, u8_t* pLength);
+extern void zfiWlanQueryRsnIe(zdev_t* dev, u8_t* ie, u8_t* pLength);
+extern void zfiWlanQueryWpaIe(zdev_t* dev, u8_t* ie, u8_t* pLength);
+extern u8_t zfiWlanQueryHTMode(zdev_t* dev);
+extern u8_t zfiWlanQueryBandWidth40(zdev_t* dev);
+extern u8_t zfiWlanQueryMulticastCipherAlgo(zdev_t *dev);
+extern u16_t zfiWlanQueryRegionCode(zdev_t* dev);
+extern void zfiWlanSetWpaIe(zdev_t* dev, u8_t* ie, u8_t Length);
+extern void zfiWlanSetWpaSupport(zdev_t* dev, u8_t WpaSupport);
+extern void zfiWlanCheckStaWpaIe(zdev_t* dev);
+extern void zfiWlanSetBasicRate(zdev_t* dev, u8_t bRateSet, u8_t gRateSet,
+                         u32_t nRateSet);
+extern void zfiWlanSetBGMode(zdev_t* dev, u8_t mode);
+extern void zfiWlanSetpreambleType(zdev_t* dev, u8_t type);
+extern u8_t zfiWlanQuerypreambleType(zdev_t* dev);
+extern u8_t zfiWlanQueryPowerSaveMode(zdev_t* dev);
+extern void zfiWlanSetMacAddress(zdev_t* dev, u16_t* mac);
+extern u16_t zfiWlanSetTxRate(zdev_t* dev, u16_t rate);
+extern u32_t zfiWlanQueryTxRate(zdev_t* dev);
+extern void zfWlanUpdateRxRate(zdev_t* dev, struct zsAdditionInfo* addInfo);
+extern u32_t zfiWlanQueryRxRate(zdev_t* dev);
+extern u8_t zfiWlanSetPmkidInfo(zdev_t* dev, u16_t* bssid, u8_t* pmkid);
+extern u32_t zfiWlanQueryPmkidInfo(zdev_t* dev, u8_t* buf, u32_t len);
+extern void zfiWlanSetAllMulticast(zdev_t* dev, u32_t setting);
+extern void zfiWlanSetHTCtrl(zdev_t* dev, u32_t *setting, u32_t forceTxTPC);
+extern void zfiWlanQueryHTCtrl(zdev_t* dev, u32_t *setting, u32_t *forceTxTPC);
+extern void zfiWlanDbg(zdev_t* dev, u8_t setting);
+
+extern void zfiWlanResetTally(zdev_t* dev);
+extern void zfiWlanQueryTally(zdev_t* dev, struct zsCommTally *tally);
+extern void zfiWlanQueryTrafTally(zdev_t* dev, struct zsTrafTally *tally);
+extern void zfiWlanQueryMonHalRxInfo(zdev_t* dev, struct zsMonHalRxInfo *halRxInfo);
+
+extern u32_t zfiFWConfig(zdev_t* dev, u32_t size);
+
+extern void zfiDKEnable(zdev_t* dev, u32_t enable);
+
+extern void zfiWlanSetMulticastList(zdev_t* dev, u8_t size, u8_t* pList);
+extern void zfiWlanRemoveKey(zdev_t* dev, u8_t keyType, u8_t keyId);
+extern u8_t zfiWlanQueryIsPKInstalled(zdev_t *dev, u8_t *staMacAddr);
+extern u32_t zfiWlanQueryPacketTypePromiscuous(zdev_t* dev);
+extern void zfiWlanSetPacketTypePromiscuous(zdev_t* dev, u32_t setValue);
+extern void zfiSetChannelManagement(zdev_t* dev, u32_t setting);
+extern void zfiSetRifs(zdev_t* dev, u16_t setting);
+extern void zfiCheckRifs(zdev_t* dev);
+extern void zfiSetReorder(zdev_t* dev, u16_t value);
+extern void zfiSetSeqDebug(zdev_t* dev, u16_t value);
+
+extern u16_t zfiConfigWdsPort(zdev_t* dev, u8_t wdsPortId, u16_t flag, u16_t* wdsAddr,
+        u16_t encType, u32_t* wdsKey);
+extern void zfiWlanQueryRegulationTable(zdev_t* dev, struct zsRegulationTable* pEntry);
+extern void zfiWlanSetScanTimerPerChannel(zdev_t* dev, u16_t time);
+extern void zfiWlanSetAutoReconnect(zdev_t* dev, u8_t enable);
+extern u32_t zfiDebugCmd(zdev_t* dev, u32_t cmd, u32_t value);
+extern void zfiWlanSetProbingHiddenSsid(zdev_t* dev, u8_t* ssid, u8_t ssidLen,
+    u16_t entry);
+extern void zfiWlanSetDropUnencryptedPackets(zdev_t* dev, u8_t enable);
+extern void zfiWlanSetIBSSJoinOnly(zdev_t* dev, u8_t joinOnly);
+extern void zfiWlanSetDefaultKeyId(zdev_t* dev, u8_t keyId);
+extern void zfiWlanSetDisableProbingWithSsid(zdev_t* dev, u8_t mode);
+extern void zfiWlanQueryGSN(zdev_t* dev, u8_t *gsn, u16_t vapId);
+extern u16_t zfiStaAddIeWpaRsn(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t frameType);
+extern u8_t zfiWlanSetDot11DMode(zdev_t* dev, u8_t mode);
+extern u8_t zfiWlanSetDot11HDFSMode(zdev_t* dev, u8_t mode);
+extern u8_t zfiWlanSetDot11HTPCMode(zdev_t* dev, u8_t mode);
+extern u8_t zfiWlanSetAniMode(zdev_t* dev, u8_t mode);
+extern void zfiWlanSetStaWme(zdev_t* dev, u8_t enable, u8_t uapsdInfo);
+extern void zfiWlanSetApWme(zdev_t* dev, u8_t enable);
+extern u8_t zfiWlanQuerywmeEnable(zdev_t* dev);
+#ifdef ZM_OS_LINUX_FUNC
+extern void zfiWlanShowTally(zdev_t* dev);
+#endif
+#ifdef ZM_ENABLE_CENC
+/* CENC */
+extern u8_t zfiWlanSetCencPairwiseKey(zdev_t* dev, u8_t keyid, u32_t *txiv, u32_t *rxiv,
+        u8_t *key, u8_t *mic);
+extern u8_t zfiWlanSetCencGroupKey(zdev_t* dev, u8_t keyid, u32_t *rxiv,
+        u8_t *key, u8_t *mic);
+#endif //ZM_ENABLE_CENC
+extern void zfiWlanQuerySignalInfo(zdev_t* dev, u8_t *buffer);
+extern void zfiWlanQueryAdHocCreatedBssDesc(zdev_t* dev, struct zsBssInfo *pBssInfo);
+extern u8_t zfiWlanQueryAdHocIsCreator(zdev_t* dev);
+extern u32_t zfiWlanQuerySupportMode(zdev_t* dev);
+extern u32_t zfiWlanQueryTransmitPower(zdev_t* dev);
+extern void zfiWlanEnableLeapConfig(zdev_t* dev, u8_t leapEnabled);
+
+/* returned buffer allocated by driver core */
+extern void zfiRecvEthComplete(zdev_t* dev, zbuf_t* buf);
+
+extern void zfiRecv80211(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* addInfo);
+
+extern void zfiWlanSetMaxTxPower(zdev_t* dev, u8_t power2, u8_t power5);
+extern void zfiWlanQueryMaxTxPower(zdev_t* dev, u8_t *power2, u8_t *power5);
+extern void zfiWlanSetConnectMode(zdev_t* dev, u8_t mode);
+extern void zfiWlanSetSupportMode(zdev_t* dev, u32_t mode);
+extern void zfiWlanSetAdhocMode(zdev_t* dev, u32_t mode);
+extern u32_t zfiWlanQueryAdhocMode(zdev_t* dev, u8_t bWrapper);
+extern u8_t zfiWlanSetCountryIsoName(zdev_t* dev, u8_t *countryIsoName, u8_t length);
+extern const char* zfiWlanQueryCountryIsoName(zdev_t* dev);
+extern u8_t zfiWlanQueryregulatoryDomain(zdev_t* dev);
+extern u8_t zfiWlanQueryCCS(zdev_t* dev);
+extern void zfiWlanSetCCS(zdev_t* dev, u8_t mode);
+extern void zfiWlanSetRegulatory(zdev_t* dev, u8_t CCS, u16_t Code, u8_t bfirstChannel);
+extern const char* zfiHpGetisoNamefromregionCode(zdev_t* dev, u16_t regionCode);
+extern void  zfiWlanSetLEDCtrlParam(zdev_t* dev, u8_t type, u8_t flag);
+extern u32_t zfiWlanQueryReceivedPacket(zdev_t* dev);
+extern void zfiWlanCheckSWEncryption(zdev_t* dev);
+extern u16_t zfiWlanQueryAllowChannels(zdev_t *dev, u16_t *channels);
+extern u16_t zfiWlanGetMulticastAddressCount(zdev_t* dev);
+extern void zfiWlanGetMulticastList(zdev_t* dev, u8_t* pMCList);
+extern void zfiWlanSetPacketFilter(zdev_t* dev, u32_t PacketFilter);
+extern u8_t zfiCompareWithMulticastListAddress(zdev_t* dev, u16_t* dstMacAddr);
+extern void zfiWlanSetSafeModeEnabled(zdev_t* dev, u8_t safeMode);
+extern void zfiWlanSetIBSSAdditionalIELength(zdev_t* dev, u32_t ibssAdditionalIESize,  u8_t* ibssAdditionalIE);
+extern void zfiWlanSetXLinkMode(zdev_t* dev, u32_t setValue);
+
+/* hprw.c */
+extern u32_t zfiDbgWriteFlash(zdev_t* dev, u32_t addr, u32_t val);
+extern u32_t zfiDbgWriteReg(zdev_t* dev, u32_t addr, u32_t val);
+extern u32_t zfiDbgReadReg(zdev_t* dev, u32_t addr);
+
+extern u32_t zfiDbgWriteEeprom(zdev_t* dev, u32_t addr, u32_t val);
+extern u32_t zfiDbgBlockWriteEeprom(zdev_t* dev, u32_t addr, u32_t* buf);
+extern u32_t zfiDbgBlockWriteEeprom_v2(zdev_t* dev, u32_t addr, u32_t* buf, u32_t wrlen);
+
+extern u16_t zfiDbgChipEraseFlash(zdev_t *dev);
+extern u16_t zfiDbgProgramFlash(zdev_t *dev, u32_t offset, u32_t len, u32_t *data);
+extern u32_t zfiDbgGetFlashCheckSum(zdev_t *dev, u32_t addr, u32_t len);
+extern u32_t zfiDbgReadFlash(zdev_t *dev, u32_t addr, u32_t len);
+extern u32_t zfiDownloadFwSet(zdev_t *dev);
+
+extern u32_t zfiDbgDelayWriteReg(zdev_t* dev, u32_t addr, u32_t val);
+extern u32_t zfiDbgFlushDelayWrite(zdev_t* dev);
+
+extern u32_t zfiDbgSetIFSynthesizer(zdev_t* dev, u32_t value);
+extern u32_t zfiDbgReadTally(zdev_t* dev);
+
+extern u32_t zfiDbgQueryHwTxBusy(zdev_t* dev);
+
+extern u8_t zfiWlanGetDestAddrFromBuf(zdev_t *dev, zbuf_t *buf, u16_t *macAddr);
+
+extern u32_t zfiWlanQueryHwCapability(zdev_t* dev);
+
+extern void zfiWlanSetDynamicSIFSParam(zdev_t* dev, u8_t val);
+
+/***** End of section 2 *****/
+
+/***** section 3 performace evaluation *****/
+#ifdef ZM_ENABLE_PERFORMANCE_EVALUATION
+extern void zfiTxPerformanceMSDU(zdev_t* dev, u32_t tick);
+extern void zfiRxPerformanceMPDU(zdev_t* dev, zbuf_t* buf);
+extern void zfiRxPerformanceReg(zdev_t* dev, u32_t reg, u32_t rsp);
+#define ZM_PERFORMANCE_INIT(dev)                zfiPerformanceInit(dev);
+#define ZM_PERFORMANCE_TX_MSDU(dev, tick)       zfiTxPerformanceMSDU(dev, tick);
+#define ZM_PERFORMANCE_RX_MSDU(dev, tick)       zfiRxPerformanceMSDU(dev, tick);
+#define ZM_PERFORMANCE_TX_MPDU(dev, tick)       zfiTxPerformanceMPDU(dev, tick);
+#define ZM_PERFORMANCE_RX_MPDU(dev, buf)        zfiRxPerformanceMPDU(dev, buf);
+#define ZM_PERFORMANCE_RX_SEQ(dev, buf)         zfiRxPerformanceSeq(dev, buf);
+#define ZM_PERFORMANCE_REG(dev, reg, rsp)    {if(cmd[1] == reg) zfiRxPerformanceReg(dev, reg, rsp);}
+#define ZM_PERFORMANCE_DUP(dev, buf1, buf2)     zfiRxPerformanceDup(dev, buf1, buf2);
+#define ZM_PERFORMANCE_FREE(dev, buf)           zfiRxPerformanceFree(dev, buf);
+#define ZM_PERFORMANCE_RX_AMSDU(dev, buf, len)  zfiRxPerformanceAMSDU(dev, buf, len);
+#define ZM_PERFORMANCE_RX_FLUSH(dev)            zfiRxPerformanceFlush(dev);
+#define ZM_PERFORMANCE_RX_CLEAR(dev)            zfiRxPerformanceClear(dev);
+#define ZM_SEQ_DEBUG                            if (wd->seq_debug) DbgPrint
+#define ZM_PERFORMANCE_RX_REORDER(dev)          zfiRxPerformanceReorder(dev);
+#else
+#define ZM_PERFORMANCE_INIT(dev)
+#define ZM_PERFORMANCE_TX_MSDU(dev, tick)
+#define ZM_PERFORMANCE_RX_MSDU(dev, tick)
+#define ZM_PERFORMANCE_TX_MPDU(dev, tick)
+#define ZM_PERFORMANCE_RX_MPDU(dev, buf)
+#define ZM_PERFORMANCE_RX_SEQ(dev, buf)
+#define ZM_PERFORMANCE_REG(dev, reg, rsp)
+#define ZM_PERFORMANCE_DUP(dev, buf1, buf2)
+#define ZM_PERFORMANCE_FREE(dev, buf)
+#define ZM_PERFORMANCE_RX_AMSDU(dev, buf, len)
+#define ZM_PERFORMANCE_RX_FLUSH(dev)
+#define ZM_PERFORMANCE_RX_CLEAR(dev)
+#define ZM_SEQ_DEBUG
+#define ZM_PERFORMANCE_RX_REORDER(dev)
+#endif
+/***** End of section 3 *****/
+#endif
diff --git a/drivers/staging/otus/80211core/pub_zfw.h b/drivers/staging/otus/80211core/pub_zfw.h
new file mode 100644
index 00000000000..01a22721672
--- /dev/null
+++ b/drivers/staging/otus/80211core/pub_zfw.h
@@ -0,0 +1,93 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef _PUB_ZFW_H
+#define _PUB_ZFW_H
+
+#include "../oal_dt.h"
+
+
+/* Buffer management */
+#ifdef ZM_ENABLE_BUFFER_DEBUG
+extern zbuf_t* zfwBufAllocateWithContext(zdev_t* dev, u16_t len, u8_t *functionName, ULONG line);
+#define zfwBufAllocate(dev, len)  zfwBufAllocateWithContext(dev, len, (u8_t *)__FUNCTION__, __LINE__)
+#else
+extern zbuf_t* zfwBufAllocate(zdev_t* dev, u16_t len);
+#endif
+extern void zfwBufFree(zdev_t* dev, zbuf_t* buf, u16_t errCode);
+extern u16_t zfwBufChain(zdev_t* dev, zbuf_t** head, zbuf_t* tail);
+extern u16_t zfwBufCopy(zdev_t* dev, zbuf_t* dst, zbuf_t* src);
+extern u16_t zfwBufSetSize(zdev_t* dev, zbuf_t* buf, u16_t size);
+extern u16_t zfwBufRemoveHead(zdev_t* dev, zbuf_t* buf, u16_t size);
+extern u16_t zfwBufGetSize(zdev_t* dev, zbuf_t* buf);
+extern void  zfwCopyBufContext(zdev_t* dev, zbuf_t* source, zbuf_t* dest);
+
+/* Memory management */
+extern void* zfwMemAllocate(zdev_t* dev, u32_t size);
+extern void zfwMemFree(zdev_t* dev, void* mem, u32_t size);
+extern void zfwMemoryCopy(u8_t* dst, u8_t* src, u16_t length);
+extern void zfwMemoryMove(u8_t* dst, u8_t* src, u16_t length);
+extern void zfwZeroMemory(u8_t* va, u16_t length);
+extern u8_t zfwMemoryIsEqual(u8_t* m1, u8_t* m2, u16_t length);
+
+/* Others */
+extern void zfwSleep(zdev_t* dev, u32_t ms);
+extern u16_t zfwGetVapId(zdev_t* dev);
+extern u16_t zfwStaAddIeWpaRsn(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t frameType);
+extern u32_t zfwWaitForEvent(zdev_t *dev, u32_t event, u32_t timeout);
+extern void zfwSendEvent(zdev_t* dev);
+extern void zfwGetActiveScanDur(zdev_t* dev, u8_t* Dur );
+extern void zfwGetShowZeroLengthSSID(zdev_t* dev, u8_t* Dur );
+/* For debugging */
+extern void zfwDumpBuf(zdev_t* dev, zbuf_t* buf);
+extern void zfwDbgReadRegDone(zdev_t* dev, u32_t addr, u32_t val);
+/* For Evl */
+extern void zfwDbgDownloadFwInitDone(zdev_t* dev);
+extern void zfwDbgReadFlashDone(zdev_t* dev, u32_t addr, u32_t* rspdata, u32_t datalen);
+extern void zfwDbgGetFlashChkSumDone(zdev_t* dev, u32_t* rspdata);
+extern void zfwDbgProgrameFlashDone(zdev_t* dev);
+extern void zfwDbgProgrameFlashChkDone(zdev_t* dev);
+extern void zfwDbgWriteRegDone(zdev_t* dev, u32_t addr, u32_t val);
+extern void zfwDbgWriteEepromDone(zdev_t* dev, u32_t addr, u32_t val);
+extern void zfwDbgReadTallyDone(zdev_t* dev);
+extern void zfwWlanReadRegDone(zdev_t* dev, u32_t addr, u32_t val);
+extern void zfwWlanWriteRegDone(zdev_t* dev, u32_t addr, u32_t val);
+extern void zfwWlanReadTallyDone(zdev_t* dev);
+extern void zfwDbgQueryHwTxBusyDone(zdev_t* dev, u32_t val);
+extern u32_t zfwReadReg(zdev_t* dev, u32_t offset);
+extern u32_t zfwReadEeprom(zdev_t* dev, u32_t addr);
+
+/* Reserved for Vista, please return 0 */
+extern u8_t zfwGetPktEncExemptionActionType(zdev_t* dev, zbuf_t* buf);
+
+#ifdef ZM_ENABLE_CENC
+/* Reserved for CENC, please return 0 */
+extern u8_t zfwCencHandleBeaconProbrespon(zdev_t* dev, u8_t *pWIEc,
+        u8_t *pPeerSSIDc, u8_t *pPeerAddrc);
+#endif //ZM_ENABLE_CENC
+
+#ifdef ZM_HALPLUS_LOCK
+extern asmlinkage struct zsWlanDev *zfwGetWlanDev(zdev_t* dev);
+extern asmlinkage void zfwEnterCriticalSection(zdev_t* dev);
+extern asmlinkage void zfwLeaveCriticalSection(zdev_t* dev);
+extern asmlinkage u8_t zfwBufReadByte(zdev_t* dev, zbuf_t* buf, u16_t offset);
+extern asmlinkage u16_t zfwBufReadHalfWord(zdev_t* dev, zbuf_t* buf, u16_t offset);
+extern asmlinkage void zfwBufWriteByte(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t value);
+extern asmlinkage void zfwBufWriteHalfWord(zdev_t* dev, zbuf_t* buf, u16_t offset, u16_t value);
+extern asmlinkage u8_t *zfwGetBuffer(zdev_t* dev, zbuf_t* buf);
+#endif
+
+#endif //_PUB_ZFW_H
diff --git a/drivers/staging/otus/80211core/queue.c b/drivers/staging/otus/80211core/queue.c
new file mode 100644
index 00000000000..d294831b5c3
--- /dev/null
+++ b/drivers/staging/otus/80211core/queue.c
@@ -0,0 +1,303 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : queue.c                                               */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains queue management functions.                */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+#include "cprecomp.h"
+#include "queue.h"
+
+
+struct zsQueue* zfQueueCreate(zdev_t* dev, u16_t size)
+{
+    struct zsQueue* q;
+
+    if ((q = (struct zsQueue*)zfwMemAllocate(dev, sizeof(struct zsQueue)
+            + (sizeof(struct zsQueueCell)*(size-1)))) != NULL)
+    {
+        q->size = size;
+        q->sizeMask = size-1;
+        q->head = 0;
+        q->tail = 0;
+    }
+    return q;
+}
+
+void zfQueueDestroy(zdev_t* dev, struct zsQueue* q)
+{
+    u16_t size = sizeof(struct zsQueue) + (sizeof(struct zsQueueCell)*(q->size-1));
+
+    zfQueueFlush(dev, q);
+    zfwMemFree(dev, q, size);
+
+    return;
+}
+
+u16_t zfQueuePutNcs(zdev_t* dev, struct zsQueue* q, zbuf_t* buf, u32_t tick)
+{
+    u16_t ret = ZM_ERR_QUEUE_FULL;
+
+    zm_msg0_mm(ZM_LV_1, "zfQueuePutNcs()");
+
+    if (((q->tail+1)&q->sizeMask) != q->head)
+    {
+        q->cell[q->tail].buf = buf;
+        q->cell[q->tail].tick = tick;
+        q->tail = (q->tail+1) & q->sizeMask;
+        ret = ZM_SUCCESS;
+    }
+
+    return ret;
+}
+
+u16_t zfQueuePut(zdev_t* dev, struct zsQueue* q, zbuf_t* buf, u32_t tick)
+{
+    u16_t ret;
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    ret = zfQueuePutNcs(dev, q, buf, tick);
+
+    zmw_leave_critical_section(dev);
+
+    return ret;
+}
+
+zbuf_t* zfQueueGet(zdev_t* dev, struct zsQueue* q)
+{
+    zbuf_t* buf = NULL;
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if (q->head != q->tail)
+    {
+        buf = q->cell[q->head].buf;
+        q->head = (q->head+1) & q->sizeMask;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return buf;
+}
+
+u16_t zfCompareDstwithBuf(zdev_t* dev, zbuf_t* buf, u8_t* addr)
+{
+    u16_t i;
+    u8_t dst[6];
+
+    for (i=0; i<6; i++)
+    {
+        dst[i] = zmw_buf_readb(dev, buf, i);
+        if (dst[i] != addr[i])
+        {
+            return 1+i;
+        }
+    }
+
+    return 0;
+}
+
+
+zbuf_t* zfQueueGetWithMac(zdev_t* dev, struct zsQueue* q, u8_t* addr, u8_t* mb)
+{
+    zbuf_t* buf;
+    zbuf_t* retBuf = NULL;
+    u16_t index, next;
+    zmw_declare_for_critical_section();
+
+    *mb = 0;
+
+    zmw_enter_critical_section(dev);
+
+    index = q->head;
+
+    while (1)
+    {
+        if (index != q->tail)
+        {
+            buf = q->cell[index].buf;
+
+            //if buf's detination address == input addr
+            if (zfCompareDstwithBuf(dev, buf, addr) == 0)
+            {
+                retBuf = buf;
+                //Get it, and trace the whole queue to calculate more bit
+                while ((next =((index+1)&q->sizeMask)) != q->tail)
+                {
+                    q->cell[index].buf = q->cell[next].buf;
+                    q->cell[index].tick = q->cell[next].tick;
+
+                    if ((*mb == 0) && (zfCompareDstwithBuf(dev,
+                            q->cell[next].buf, addr) == 0))
+                    {
+                        *mb = 1;
+                    }
+
+                    index = next;
+                }
+                q->tail = (q->tail-1) & q->sizeMask;
+
+                zmw_leave_critical_section(dev);
+                return retBuf;
+            }
+            index = (index + 1) & q->sizeMask;
+        } //if (index != q->tail)
+        else
+        {
+            break;
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return retBuf;
+
+}
+
+void zfQueueFlush(zdev_t* dev, struct zsQueue* q)
+{
+    zbuf_t* buf;
+
+    while ((buf = zfQueueGet(dev, q)) != NULL)
+    {
+        zfwBufFree(dev, buf, 0);
+    }
+
+    return;
+}
+
+void zfQueueAge(zdev_t* dev, struct zsQueue* q, u32_t tick, u32_t msAge)
+{
+    zbuf_t* buf;
+    u32_t   buftick;
+    zmw_declare_for_critical_section();
+
+    while (1)
+    {
+        buf = NULL;
+        zmw_enter_critical_section(dev);
+
+        if (q->head != q->tail)
+        {
+            buftick = q->cell[q->head].tick;
+            if (((tick - buftick)*ZM_MS_PER_TICK) > msAge)
+            {
+                buf = q->cell[q->head].buf;
+                q->head = (q->head+1) & q->sizeMask;
+            }
+        }
+
+        zmw_leave_critical_section(dev);
+
+        if (buf != NULL)
+        {
+            zm_msg0_mm(ZM_LV_0, "Age frame in queue!");
+            zfwBufFree(dev, buf, 0);
+        }
+        else
+        {
+            break;
+        }
+    }
+    return;
+}
+
+
+u8_t zfQueueRemovewithIndex(zdev_t* dev, struct zsQueue* q, u16_t index, u8_t* addr)
+{
+    u16_t next;
+    u8_t mb = 0;
+
+    //trace the whole queue to calculate more bit
+    while ((next =((index+1)&q->sizeMask)) != q->tail)
+    {
+        q->cell[index].buf = q->cell[next].buf;
+        q->cell[index].tick = q->cell[next].tick;
+
+        if ((mb == 0) && (zfCompareDstwithBuf(dev,
+                q->cell[next].buf, addr) == 0))
+        {
+            mb = 1;
+        }
+
+        index = next;
+    }
+    q->tail = (q->tail-1) & q->sizeMask;
+
+    return mb;
+
+}
+
+void zfQueueGenerateUapsdTim(zdev_t* dev, struct zsQueue* q,
+        u8_t* uniBitMap, u16_t* highestByte)
+{
+    zbuf_t* psBuf;
+    u8_t dst[6];
+    u16_t id, aid, index, i;
+    u16_t bitPosition;
+    u16_t bytePosition;
+    zmw_get_wlan_dev(dev);
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    index = q->head;
+
+    while (index != q->tail)
+    {
+        psBuf = q->cell[index].buf;
+        for (i=0; i<6; i++)
+        {
+            dst[i] = zmw_buf_readb(dev, psBuf, i);
+        }
+        /* TODO : use u8_t* fot MAC address */
+        if (((id = zfApFindSta(dev, (u16_t*)dst)) != 0xffff)
+                && (wd->ap.staTable[id].psMode != 0))
+        {
+            /* Calculate PVB only when all AC are delivery-enabled */
+            if ((wd->ap.staTable[id].qosInfo & 0xf) == 0xf)
+            {
+                aid = id + 1;
+                bitPosition = (1 << (aid & 0x7));
+                bytePosition = (aid >> 3);
+                uniBitMap[bytePosition] |= bitPosition;
+
+                if (bytePosition>*highestByte)
+                {
+                    *highestByte = bytePosition;
+                }
+            }
+            index = (index+1) & q->sizeMask;
+        }
+        else
+        {
+            /* Free garbage UAPSD frame */
+            zfQueueRemovewithIndex(dev, q, index, dst);
+            zfwBufFree(dev, psBuf, 0);
+        }
+    }
+    zmw_leave_critical_section(dev);
+
+    return;
+}
diff --git a/drivers/staging/otus/80211core/queue.h b/drivers/staging/otus/80211core/queue.h
new file mode 100644
index 00000000000..4526b882bd0
--- /dev/null
+++ b/drivers/staging/otus/80211core/queue.h
@@ -0,0 +1,37 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef _QUEUE_H
+#define _QUEUE_H
+
+#include "../oal_dt.h"
+
+struct zsQueueCell
+{
+    u32_t   tick;
+    zbuf_t* buf;
+};
+
+struct zsQueue
+{
+    u16_t   size;
+    u16_t   sizeMask;
+    u16_t   head;
+    u16_t   tail;
+    struct zsQueueCell cell[1];
+};
+
+#endif //#ifndef _QUEUE_H
diff --git a/drivers/staging/otus/80211core/ratectrl.c b/drivers/staging/otus/80211core/ratectrl.c
new file mode 100644
index 00000000000..a43104cd7f5
--- /dev/null
+++ b/drivers/staging/otus/80211core/ratectrl.c
@@ -0,0 +1,874 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include "cprecomp.h"
+#include "ratectrl.h"
+
+const u32_t zcRateToPhyCtrl[] =
+    {
+        /* 1M,        2M,         5M,        11M ,  0  1  2  3*/
+        0x00000,    0x10000,    0x20000,    0x30000,
+        /* 6M         9M          12M        18M ,  4  5  6  7*/
+        0xb0001,    0xf0001,    0xa0001,    0xe0001,
+        /* 24M         36M        48M        54M ,  8  9  10 11*/
+        0x90001,    0xd0001,    0x80001,    0xc0001,
+        /* MCS0      MCS1        MCS2        MCS3,  12 13 14 15*/
+        0x00002,    0x10002,    0x20002,    0x30002,
+        /* MCS4      MCS5        MCS6        MCS7,  16 17 18 19*/
+        0x40002,    0x50002,    0x60002,    0x70002,
+        /* MCS8      MCS9        MCS10       MCS11, 20 21 22 23*/
+        0x80002,    0x90002,    0xa0002,    0xb0002,
+        /* MCS12     MCS13       MCS14       MCS15, 24 25 26 27*/
+        0xc0002,    0xd0002,    0xe0002,    0xf0002,
+        /* MCS14SG,  MCS15SG     MCS7SG           , 28 29, 30*/
+        0x800e0002, 0x800f0002, 0x80070002
+    };
+
+
+const u8_t zcHtRateTable[15][4] =
+    { /*[5G 20MHz]  [5G 40MHz] [2.4G 20MHz]  [2.4G 40MHz]*/
+        {  4,          4,          0,          0},   /*OFDM6M OFDM6M  CCK1M  CCK1M  */
+        {  5,          5,          1,          1},   /*OFDM9M OFDM9M  CCK2M  CCK2M  */
+        {  13,         12,         2,          2},   /*MCS1   MCS0    CCK5M  CCK5M  */
+        {  14,         13,         3,          3},   /*MCS2   MCS1    CCK11M CCK11M */
+        {  15,         14,         13,         12},  /*MCS3   MCS2    MCS1   MCS0   */
+        {  16,         15,         14,         13},  /*MCS4   MCS3    MCS2   MCS1   */
+        {  23,         16,         15,         14},  /*MCS11  MCS4    MCS3   MCS2   */
+        {  24,         23,         16,         15},  /*MCS12  MCS11   MCS4   MCS3   */
+        {  25,         24,         23,         16},  /*MCS13  MCS12   MCS11  MCS4   */
+        {  26,         25,         24,         23},  /*MCS14  MCS13   MCS12  MCS11  */
+        {  27,         26,         25,         24},  /*MCS15  MCS14   MCS13  MCS12  */
+        {  0,          27,         26,         25},  /*0      MCS15   MCS14  MCS13  */
+        {  0,          29,         27,         26},  /*0      MCS15SG MCS15  MCS14  */
+        {  0,          0,          0,          28},  /*0      0       0      MCS14SG*/
+        {  0,          0,          0,          29}   /*0      0       0      MCS15SG*/
+    };
+
+const u8_t zcHtOneTxStreamRateTable[15][4] =
+    { /*[5G 20MHz]  [5G 40MHz] [2.4G 20MHz]  [2.4G 40MHz]*/
+        {  4,          4,          0,          0},   /*OFDM6M OFDM6M  CCK1M  CCK1M  */
+        {  5,          5,          1,          1},   /*OFDM9M OFDM9M  CCK2M  CCK2M  */
+        {  13,         12,         2,          2},   /*MCS1   MCS0    CCK5M  CCK5M  */
+        {  14,         13,         3,          3},   /*MCS2   MCS1    CCK11M CCK11M */
+        {  15,         14,         13,         12},  /*MCS3   MCS2    MCS1   MCS0   */
+        {  16,         15,         14,         13},  /*MCS4   MCS3    MCS2   MCS1   */
+        {  17,         16,         15,         14},  /*MCS5   MCS4    MCS3   MCS2   */
+        {  18,         17,         16,         15},  /*MCS6   MCS5    MCS4   MCS3   */
+        {  19,         18,         17,         16},  /*MCS7   MCS6    MCS5   MCS4   */
+        {  0,          19,         18,         17},  /*0      MCS7    MCS6   MCS5   */
+        {  0,          30,         19,         18},  /*0      MCS7SG  MCS7   MCS6   */
+        {  0,          0,          0,          19},  /*0      0       0      MCS7   */
+        {  0,          0,          0,          30},  /*0      0       0      MCS7SG */
+        {  0,          0,          0,          0 },  /*0      0       0      0      */
+        {  0,          0,          0,          0 }   /*0      0       0      0      */
+    };
+
+const u16_t zcRate[] =
+    {
+        1, 2, 5, 11,                  /* 1M, 2M, 5M, 11M          ,  0  1  2  3*/
+        6, 9, 12, 18,                 /* 6M  9M  12M  18M         ,  4  5  6  7*/
+        24, 36, 48, 54,               /* 24M  36M  48M  54M       ,  8  9 10 11*/
+        13, 27, 40, 54,               /* MCS0 MCS1 MCS2 MCS3      , 12 13 14 15*/
+        81, 108, 121, 135,            /* MCS4 MCS5 MCS6 MCS7      , 16 17 18 19*/
+        27, 54, 81, 108,              /* MCS8 MCS9 MCS10 MCS11    , 20 21 22 23*/
+        162, 216, 243, 270,           /* MCS12 MCS13 MCS14 MCS15  , 24 25 26 27*/
+        270, 300, 150                 /* MCS14SG, MCS15SG, MCS7SG , 28 29 30*/
+    };
+
+const u16_t PERThreshold[] =
+    {
+        100, 50, 50, 50,    /* 1M, 2M, 5M, 11M          ,  0  1  2  3*/
+        50, 50, 30, 30,     /* 6M  9M  12M  18M         ,  4  5  6  7*/
+        25, 25, 25, 20,     /* 24M  36M  48M  54M       ,  8  9 10 11*/
+        50, 50, 50, 40,    /* MCS0 MCS1 MCS2 MCS3      , 12 13 14 15*/
+        30, 30, 30, 30,    /* MCS4 MCS5 MCS6 MCS7      , 16 17 18 19*/
+        30, 30, 25, 25,    /* MCS8 MCS9 MCS10 MCS11    , 20 21 22 23*/
+        25, 25, 15, 15,     /* MCS12 MCS13 MCS14 MCS15  , 24 25 26 27*/
+        15, 15, 10          /* MCS14SG, MCS15SG         , 28 29*/
+    };
+
+const u16_t FailDiff[] =
+    {
+        40, 46, 40, 0,          /* 1M, 2M, 5M, 11M          ,  0  1  2  3*/
+        24, 17, 22, 16,         /* 6M  9M  12M  18M         ,  4  5  6  7*/
+        19, 13, 5, 0,           /* 24M  36M  48M  54M       ,  8  9 10 11*/
+        36, 22, 15, 19,         /* MCS0 MCS1 MCS2 MCS3      , 12 13 14 15*/
+        12, 5, 4, 7,            /* MCS4 MCS5 MCS6 MCS7      , 16 17 18 19*/
+        0, 0, 0, 0,             /* MCS8 MCS9 MCS10 MCS11    , 20 21 22 23*/
+        9, 4, 3, 3,             /* MCS12 MCS13 MCS14 MCS15  , 24 25 26 27*/
+        3, 0, 0                 /* MCS14SG, MCS15SG         , 28 29*/
+    };
+
+
+#ifdef ZM_ENABLE_BA_RATECTRL
+u32_t TxMPDU[29];
+u32_t BAFail[29];
+u32_t BAPER[29];
+const u16_t BADiff[] =
+    {
+        0, 0, 0, 0,
+        0, 0, 0, 0,
+        0, 0, 0, 0,
+        361, 220, 151, 187,
+        122, 48, 41, 65,
+        0, 0, 0, 0,
+        88, 33, 27, 25,
+        0
+    };
+#endif
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfRateCtrlInitCell          */
+/*      Initialize rate control cell.                                   */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      type : 0=>11b, 1=>11a/g, 2=>11n, 3=>11n one Tx stream           */
+/*      gBand : 1=>2.4G, 0=>5G                                          */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.2      */
+/*                                                                      */
+/************************************************************************/
+void zfRateCtrlInitCell(zdev_t* dev, struct zsRcCell* rcCell, u8_t type,
+        u8_t gBand, u8_t SG40)
+{
+    u8_t i;
+    u8_t maxrate;
+    zmw_get_wlan_dev(dev);
+
+    if (SG40) SG40 = 1;
+
+    if (gBand != 0)
+    {
+        if (type == 1) //11g
+        {
+            for (i=0; i<4; i++) //1M 2M 5M 11M
+            {
+                rcCell->operationRateSet[i] = (u8_t)i;
+            }
+            for (i=4; i<10; i++) //12M 18M 24M 36M 48M 54M
+            {
+                rcCell->operationRateSet[i] = 2+i;
+            }
+            rcCell->operationRateCount = 10;
+            rcCell->currentRateIndex = 5; //18M
+        }
+        else if (type == 2) //11ng
+        {
+            if (wd->wlanMode == ZM_MODE_AP) //AP 11ng 40M
+            {
+                for (i=0; i<15; i++)
+                {
+                    rcCell->operationRateSet[i] = zcHtRateTable[i][3];
+                }
+                if(!SG40) rcCell->operationRateSet[13] = 27;
+                rcCell->operationRateCount = 14+SG40;
+                rcCell->currentRateIndex = 10;
+            }
+            else //STA
+            {
+                if (wd->sta.htCtrlBandwidth == ZM_BANDWIDTH_40MHZ) //11ng 40M
+                {
+                    for (i=0; i<15; i++)
+                    {
+                        rcCell->operationRateSet[i] = zcHtRateTable[i][3];
+                    }
+                    if(!SG40) rcCell->operationRateSet[13] = 27;
+                    rcCell->operationRateCount = 14+SG40;
+                    rcCell->currentRateIndex = 10;
+                }
+                else    //11ng 20M
+                {
+                    for (i=0; i<13; i++)
+                    {
+                        rcCell->operationRateSet[i] = zcHtRateTable[i][2];
+                    }
+                    rcCell->operationRateCount = 13;
+                    rcCell->currentRateIndex = 9;
+                }
+            }
+        }
+        else if (type == 3) //11ng one Tx stream
+        {
+                if (wd->sta.htCtrlBandwidth == ZM_BANDWIDTH_40MHZ) //11ng 40M one Tx stream
+                {
+                    if(SG40 != 0)
+                    {
+                        maxrate = 13;
+                    }
+                    else
+                    {
+                        maxrate = 12;
+                    }
+                    for (i=0; i<maxrate; i++)
+                    {
+                        rcCell->operationRateSet[i] = zcHtOneTxStreamRateTable[i][3];
+                    }
+                    rcCell->operationRateCount = i;
+                    rcCell->currentRateIndex = ((i+1)*3)/4;
+                }
+                else    //11ng 20M
+                {
+                    for (i=0; i<11; i++)
+                    {
+                        rcCell->operationRateSet[i] = zcHtOneTxStreamRateTable[i][2];
+                    }
+                    rcCell->operationRateCount = i;
+                    rcCell->currentRateIndex = ((i+1)*3)/4;
+                }
+        }
+        else //if (type == 0) //11b
+        {
+            for (i=0; i<4; i++)
+            {
+                rcCell->operationRateSet[i] = (u8_t)i;
+            }
+            rcCell->operationRateCount = 4;
+            rcCell->currentRateIndex = rcCell->operationRateCount-1;
+        }
+    }
+    else
+    {
+        if (type == 2) //11na
+        {
+            if (wd->wlanMode == ZM_MODE_AP) //AP 11na 40M
+            {
+                for (i=0; i<(12+SG40); i++)
+                {
+                    rcCell->operationRateSet[i] = zcHtRateTable[i][1];
+                }
+                rcCell->operationRateCount = 12+SG40;
+                rcCell->currentRateIndex = 8;
+            }
+            else //STA
+            {
+                if (wd->sta.htCtrlBandwidth == ZM_BANDWIDTH_40MHZ) //11na 40M
+                {
+                    for (i=0; i<(12+SG40); i++)
+                    {
+                        rcCell->operationRateSet[i] = zcHtRateTable[i][1];
+                    }
+                    rcCell->operationRateCount = 12+SG40;
+                    rcCell->currentRateIndex = 8;
+                }
+                else    //11na 20M
+                {
+                    for (i=0; i<11; i++)
+                    {
+                        rcCell->operationRateSet[i] = zcHtRateTable[i][0];
+                    }
+                    rcCell->operationRateCount = 11;
+                    rcCell->currentRateIndex = 7;
+                }
+            }
+        }
+        else if (type == 3) //11na one Tx stream
+        {
+                if (wd->sta.htCtrlBandwidth == ZM_BANDWIDTH_40MHZ) //11na 40M one Tx stream
+                {
+                    if(SG40 != 0)
+                    {
+                        maxrate = 11;
+                    }
+                    else
+                    {
+                        maxrate = 10;
+                    }
+                    for (i=0; i<maxrate; i++)
+                    {
+                        rcCell->operationRateSet[i] = zcHtOneTxStreamRateTable[i][1];
+                    }
+                    rcCell->operationRateCount = i;
+                    rcCell->currentRateIndex = ((i+1)*3)/4;
+                }
+                else    //11ng 20M
+                {
+                    for (i=0; i<9; i++)
+                    {
+                        rcCell->operationRateSet[i] = zcHtOneTxStreamRateTable[i][0];
+                    }
+                    rcCell->operationRateCount = i;
+                    rcCell->currentRateIndex = ((i+1)*3)/4;
+                }
+        }
+        else //if (type == 1) //11a
+        {
+            for (i=0; i<8; i++) //6M 9M 12M 18M 24M 36M 48M 54M
+            {
+                rcCell->operationRateSet[i] = i+4;
+            }
+            rcCell->operationRateCount = 8;
+            rcCell->currentRateIndex = 4;  //24M
+        }
+    }
+
+    rcCell->flag = 0;
+    rcCell->txCount = 0;
+    rcCell->failCount = 0;
+    rcCell->currentRate = rcCell->operationRateSet[rcCell->currentRateIndex];
+    rcCell->lasttxCount = 0;
+    rcCell->lastTime    = wd->tick;
+    rcCell->probingTime = wd->tick;
+    for (i=0; i<ZM_RATE_TABLE_SIZE; i++) {
+        wd->PER[i] = 0;
+        wd->txMPDU[i] = wd->txFail[i] = 0;
+    }
+    wd->probeCount = 0;
+    wd->probeInterval = 0;
+#ifdef ZM_ENABLE_BA_RATECTRL
+    for (i=0; i<29; i++) {
+        TxMPDU[i]=0;
+        BAFail[i]=0;
+        BAPER[i]=0;
+    }
+#endif
+    return;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfRateCtrlGetHigherRate     */
+/*      Get a higher rate.                                              */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      rcCell : rate control cell                                      */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      rate                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.2      */
+/*                                                                      */
+/************************************************************************/
+u8_t zfRateCtrlGetHigherRate(struct zsRcCell* rcCell)
+{
+    u8_t rateIndex;
+
+    rateIndex = rcCell->currentRateIndex
+            + (((rcCell->currentRateIndex+1) < rcCell->operationRateCount)?1:0);
+    return rcCell->operationRateSet[rateIndex];
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfRateCtrlNextLowerRate     */
+/*      Get a lower rate.                                               */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      rcCell : rate control cell                                      */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      rate                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.2      */
+/*                                                                      */
+/************************************************************************/
+u8_t zfRateCtrlNextLowerRate(zdev_t* dev, struct zsRcCell* rcCell)
+{
+    zmw_get_wlan_dev(dev);
+    if (rcCell->currentRateIndex > 0)
+    {
+        rcCell->currentRateIndex--;
+        rcCell->currentRate = rcCell->operationRateSet[rcCell->currentRateIndex];
+    }
+    zm_msg1_tx(ZM_LV_0, "Lower Tx Rate=", rcCell->currentRate);
+    //DbgPrint("Lower Tx Rate=%d", rcCell->currentRate);
+    rcCell->failCount = rcCell->txCount = 0;
+    rcCell->lasttxCount = 0;
+    rcCell->lastTime  = wd->tick;
+    return rcCell->currentRate;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfRateCtrlRateDiff          */
+/*      Rate difference.                                                */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      rcCell : rate control cell                                      */
+/*      retryRate : retry rate                                          */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      rate difference                                                 */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.2      */
+/*                                                                      */
+/************************************************************************/
+u8_t zfRateCtrlRateDiff(struct zsRcCell* rcCell, u8_t retryRate)
+{
+    u16_t i;
+
+    /* Find retryRate in operationRateSet[] */
+    for (i=0; i<rcCell->operationRateCount; i++)
+    {
+        if (retryRate == rcCell->operationRateSet[i])
+        {
+            if (i < rcCell->currentRateIndex)
+            {
+                return ((rcCell->currentRateIndex - i)+1)>>1;
+            }
+            else if (i == rcCell->currentRateIndex == 0)
+            {
+                return 1;
+            }
+            else
+            {
+                return 0;
+            }
+        }
+    }
+    /* TODO : retry rate not in operation rate set */
+    zm_msg1_tx(ZM_LV_0, "Not in operation rate set:", retryRate);
+    return 1;
+
+}
+
+u32_t zfRateCtrlUDPTP(zdev_t* dev, u16_t Rate, u32_t PER) {
+    if ((PER < 100) && (Rate > 0) && PER)
+        return 1168000/(((12304/Rate)+197)*(100+100*PER/(100-PER)));
+    else
+        return 0;
+}
+
+u8_t zfRateCtrlFindMaxUDPTP(zdev_t* dev, struct zsRcCell* rcCell) {
+    u8_t i, maxIndex=0, rateIndex;
+    u32_t max=0, UDPThroughput;
+
+    zmw_get_wlan_dev(dev);
+
+    rateIndex = zm_agg_min(rcCell->currentRateIndex+3, rcCell->operationRateCount-1);
+    for (i=rcCell->currentRateIndex; i < rateIndex; i++) {
+        UDPThroughput = zfRateCtrlUDPTP(dev, zcRate[rcCell->operationRateSet[i]],
+            wd->PER[rcCell->operationRateSet[i]]);
+        if (max < UDPThroughput) {
+            max = UDPThroughput;
+            maxIndex = i;
+        }
+    }
+
+    return rcCell->operationRateSet[maxIndex];
+}
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfRateCtrlGetTxRate         */
+/*      Get transmission rate.                                          */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      rcCell : rate control cell                                      */
+/*      probing : rate probing flag                                     */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      Tx rate                                                         */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.2      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfRateCtrlGetTxRate(zdev_t* dev, struct zsRcCell* rcCell, u16_t* probing)
+{
+    u8_t newRate, highRate;
+    zmw_get_wlan_dev(dev);
+
+    zm_msg1_tx(ZM_LV_3, "txCount=", rcCell->txCount);
+    zm_msg1_tx(ZM_LV_3, "probingTime=", rcCell->probingTime);
+    zm_msg1_tx(ZM_LV_3, "tick=", wd->tick);
+    *probing = 0;
+    newRate = rcCell->currentRate;
+
+    if (wd->probeCount && (wd->probeCount < wd->success_probing))
+    {
+        if (wd->probeInterval < 50)
+        {
+            wd->probeInterval++;
+        }
+        else
+        {
+            wd->probeInterval++;
+            if (wd->probeInterval > 52) //probe 51, 52, 53 three packets every 50 packets
+            {
+                wd->probeInterval = 0;
+            }
+            newRate=zfRateCtrlGetHigherRate(rcCell);
+            *probing = 1;
+            wd->probeCount++;
+            rcCell->probingTime = wd->tick;
+        }
+    }
+    /* Accumulate at least 1000ms and 8 packets or Accumulate over 1K packets */
+    else if ((((wd->tick - rcCell->probingTime) > (ZM_RATE_CTRL_PROBING_INTERVAL_MS/ZM_MS_PER_TICK))
+                && (rcCell->txCount >= ZM_RATE_CTRL_MIN_PROBING_PACKET))
+        || (rcCell->txCount >= 1000))
+    {
+#ifndef ZM_DISABLE_RATE_CTRL
+        /* PER = fail/total */
+        wd->probeCount = 0;
+        wd->probeSuccessCount = 0;
+        if (wd->txMPDU[rcCell->currentRate] != 0) {
+            wd->PER[rcCell->currentRate] = zm_agg_min(100,
+                (wd->txFail[rcCell->currentRate]*100)/wd->txMPDU[rcCell->currentRate]);
+            if (!wd->PER[rcCell->currentRate]) wd->PER[rcCell->currentRate] ++;
+        }
+
+        /* if PER < threshold, do rate probing, return probing rate */
+        if ((wd->PER[rcCell->currentRate] <= (ZM_RATE_PROBING_THRESHOLD+15)) ||
+            ((rcCell->currentRate <= 16) &&
+            ((wd->PER[rcCell->currentRate]/2) <= ZM_RATE_PROBING_THRESHOLD)))
+        {
+            if ((newRate=zfRateCtrlGetHigherRate(rcCell)) != rcCell->currentRate)
+            {
+                *probing = 1;
+                wd->probeCount++;
+                wd->probeInterval = 0;
+                wd->success_probing =
+                    (rcCell->currentRate <= 16)? (ZM_RATE_SUCCESS_PROBING/2) : ZM_RATE_SUCCESS_PROBING;
+                //DbgPrint("Start Probing");
+                zm_msg1_tx(ZM_LV_0, "Probing Rate=", newRate);
+            }
+        }
+#endif
+
+        zm_msg0_tx(ZM_LV_1, "Diminish counter");
+        rcCell->failCount = rcCell->failCount>>1;
+        rcCell->txCount = rcCell->txCount>>1;
+        wd->txFail[rcCell->currentRate] = wd->txFail[rcCell->currentRate] >> 1;
+        wd->txMPDU[rcCell->currentRate] = wd->txMPDU[rcCell->currentRate] >> 1;
+
+
+        if (rcCell->currentRate > 15) {
+            highRate = zfRateCtrlGetHigherRate(rcCell);
+            if ((highRate != rcCell->currentRate) && wd->PER[highRate] &&
+                ((wd->PER[rcCell->currentRate] + FailDiff[rcCell->currentRate]) >
+                wd->PER[highRate])) {
+                //DbgPrint("PER compare force raise rate to %d", highRate);
+                wd->probeSuccessCount = wd->probeCount = ZM_RATE_SUCCESS_PROBING;
+                zfRateCtrlTxSuccessEvent(dev, rcCell, highRate);
+            }
+        }
+        else {
+            highRate = zfRateCtrlFindMaxUDPTP(dev, rcCell);
+            if (rcCell->currentRate < highRate) {
+                //DbgPrint("UDP Throughput compare force raise rate to %d", highRate);
+                wd->probeSuccessCount = wd->probeCount = ZM_RATE_SUCCESS_PROBING;
+                zfRateCtrlTxSuccessEvent(dev, rcCell, highRate);
+            }
+        }
+        rcCell->probingTime = wd->tick;
+    }
+
+    if( (wd->tick > 1000)
+        && ((wd->tick - rcCell->lastTime) > 3840) )
+    {
+        if (rcCell->lasttxCount < 70)
+        {
+            rcCell->failCount = rcCell->failCount>>1;
+            rcCell->txCount = rcCell->txCount>>1;
+            wd->txFail[rcCell->currentRate] = wd->txFail[rcCell->currentRate] >> 1;
+            wd->txMPDU[rcCell->currentRate] = wd->txMPDU[rcCell->currentRate] >> 1;
+
+            rcCell->failCount = (rcCell->failCount < rcCell->txCount)?
+                                rcCell->failCount : rcCell->txCount;
+            wd->txFail[rcCell->currentRate] = (wd->txFail[rcCell->currentRate] < wd->txMPDU[rcCell->currentRate])?
+                                              wd->txFail[rcCell->currentRate] : wd->txMPDU[rcCell->currentRate];
+        }
+
+        rcCell->lastTime    = wd->tick;
+        rcCell->lasttxCount = 0;
+    }
+
+    rcCell->txCount++;
+    rcCell->lasttxCount++;
+    wd->txMPDU[rcCell->currentRate]++;
+    zm_msg1_tx(ZM_LV_1, "Get Tx Rate=", newRate);
+    return newRate;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfRateCtrlTxFailEvent       */
+/*      Tx fail event. Calculate PER and lower Tx rate if under         */
+/*      PER under threshold.                                            */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      rcCell : rate control cell                                      */
+/*      retryRate : retry rate                                          */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.2      */
+/*                                                                      */
+/************************************************************************/
+void zfRateCtrlTxFailEvent(zdev_t* dev, struct zsRcCell* rcCell, u8_t aggRate, u32_t retryRate)
+{
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+#ifndef ZM_DISABLE_RATE_CTRL
+    //DbgPrint("aggRate=%d, retryRate=%d", aggRate, retryRate);
+    if (aggRate && (aggRate != rcCell->currentRate)) {
+        wd->txFail[aggRate] += retryRate;
+        return;
+    }
+
+    if (!aggRate) {
+        retryRate = (zfRateCtrlRateDiff(rcCell, (u8_t)retryRate)+1)>>1;
+        if (rcCell->currentRate <12) //legacy rate
+        {
+            retryRate*=2;
+        }
+    }
+    rcCell->failCount += retryRate;
+    wd->txFail[rcCell->currentRate] += retryRate;
+
+    //DbgPrint("failCount=%d", rcCell->failCount);
+    if (rcCell->failCount > ZM_MIN_RATE_FAIL_COUNT)
+    {
+        if (wd->txMPDU[rcCell->currentRate] != 0) {
+            wd->PER[rcCell->currentRate] = zm_agg_min(100,
+                (wd->txFail[rcCell->currentRate]*100)/wd->txMPDU[rcCell->currentRate]);
+            if (!wd->PER[rcCell->currentRate]) wd->PER[rcCell->currentRate] ++;
+        }
+        //zm_msg1_tx(ZM_LV_1, "PER=", per);
+        //DbgPrint("PER=%d, txFail=%d, txMPDU=%d", wd->PER[rcCell->currentRate], wd->txFail[rcCell->currentRate], wd->txMPDU[rcCell->currentRate]);
+        if (wd->PER[rcCell->currentRate] > PERThreshold[rcCell->currentRate])
+        {
+            /* Lower Tx Rate if PER < THRESHOLD */
+            zfRateCtrlNextLowerRate(dev, rcCell);
+            rcCell->flag |= ZM_RC_TRAINED_BIT;
+
+            // Resolve compatibility problem with Marvell
+            if(rcCell->currentRate == 15)
+            {
+                zmw_leave_critical_section(dev);
+                zfHpSetAggPktNum(dev, 8);
+                zmw_enter_critical_section(dev);
+            }
+
+            wd->txFail[rcCell->currentRate] = wd->txFail[rcCell->currentRate] >> 1;
+            wd->txMPDU[rcCell->currentRate] = wd->txMPDU[rcCell->currentRate] >> 1;
+
+            wd->probeCount = wd->probeSuccessCount = 0;
+        }
+    }
+
+#endif
+    return;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfRateCtrlTxSuccessEvent    */
+/*      Tx success event. Raise Tx rate because rate probing success.   */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      rcCell : rate control cell                                      */
+/*      successRate : success rate                                      */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.2      */
+/*                                                                      */
+/************************************************************************/
+void zfRateCtrlTxSuccessEvent(zdev_t* dev, struct zsRcCell* rcCell, u8_t successRate)
+{
+    /* Raise Tx Rate */
+    u16_t i, PERProbe;
+    u16_t pcount;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    //DbgPrint("Probing successRate=%d", successRate);
+    /* Find successRate in operationRateSet[] */
+    wd->probeSuccessCount++;
+    if (wd->probeCount < wd->success_probing)
+    {
+        return;
+    }
+
+    pcount = wd->probeCount;
+    if (pcount != 0)
+    {
+        PERProbe = wd->probeSuccessCount * 100 / pcount;
+    }
+    else
+    {
+        PERProbe = 1;
+    }
+
+    if (PERProbe < ((rcCell->currentRate < 16)? 80:100))
+    {
+        return;
+    }
+    //DbgPrint("wd->probeCount=%d, wd->probeSuccessCount=%d", wd->probeCount, wd->probeSuccessCount);
+    wd->probeCount = wd->probeSuccessCount = 0;
+    for (i=0; i<rcCell->operationRateCount; i++)
+    {
+        if (successRate == rcCell->operationRateSet[i])
+        {
+            if (i > rcCell->currentRateIndex)
+            {
+                /* Raise current Tx rate */
+                zm_msg1_tx(ZM_LV_0, "Raise Tx Rate=", successRate);
+                //DbgPrint("Raise Tx Rate=%d", successRate);
+
+                // Resolve compatibility problem with Marvell
+                if((rcCell->currentRate <= 15) && (successRate > 15))
+                {
+                    zmw_leave_critical_section(dev);
+                    zfHpSetAggPktNum(dev, 16);
+                    zmw_enter_critical_section(dev);
+                }
+
+                rcCell->currentRate = successRate;
+                rcCell->currentRateIndex = (u8_t)i;
+                rcCell->failCount = rcCell->txCount = 0;
+                rcCell->lasttxCount = 0;
+                rcCell->lastTime  = wd->tick;
+                wd->txFail[rcCell->currentRate] = wd->txFail[rcCell->currentRate] >> 1;
+                wd->txMPDU[rcCell->currentRate] = wd->txMPDU[rcCell->currentRate] >> 1;
+            }
+        }
+    }
+
+    return;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfRateCtrlRxRssiEvent       */
+/*      Rx RSSI event. Calculate RSSI moving average, accelarate        */
+/*      rate probing if RSSI variation over threshold.                  */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      rcCell : rate control cell                                      */
+/*      successRate : success rate                                      */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        Atheros Communications, INC.    2007.2      */
+/*                                                                      */
+/************************************************************************/
+void zfRateCtrlRxRssiEvent(struct zsRcCell* rcCell, u16_t rxRssi)
+{
+    /* if delta(rcCell->rxRssi, rxRssi) > ZM_RATE_CTRL_RSSI_VARIATION */
+    if ((rcCell->rxRssi - rxRssi) > ZM_RATE_CTRL_RSSI_VARIATION)
+    {
+        /* Accelerate rate probing via decreaing rcCell->probingTime */
+        rcCell->probingTime -= ZM_RATE_CTRL_PROBING_INTERVAL_MS/ZM_MS_PER_TICK;
+    }
+
+    /* Update RSSI moving average */
+    rcCell->rxRssi = (((rcCell->rxRssi*7) + rxRssi)+4) >> 3;
+    return;
+}
+
+
+#ifdef ZM_ENABLE_BA_RATECTRL
+u8_t HigherRate(u8_t Rate) {
+    if (Rate < 28) Rate++; //28=MCS15SG, 27=MCS15, 26=MCS14, 25=MCS13
+    if (Rate > 28) Rate = 28;
+    while ((Rate >= 20) && (Rate <= 23)) {
+        Rate ++;
+    }
+    return Rate;
+}
+
+u8_t LowerRate(u8_t Rate) {
+    if (Rate > 1) Rate--;
+    while ((Rate >= 20) && (Rate <= 23)) {
+        Rate --;
+    }
+    return Rate;
+}
+
+u8_t RateMapToRateIndex(u8_t Rate, struct zsRcCell* rcCell) {
+    u8_t i;
+    for (i=0; i<rcCell->operationRateCount; i++) {
+        if (Rate == rcCell->operationRateSet[i]) {
+            return i;
+        }
+    }
+    return 0;
+}
+
+void zfRateCtrlAggrSta(zdev_t* dev) {
+    u8_t RateIndex, Rate;
+    u8_t HRate;
+    u8_t LRate;
+    u32_t RateCtrlTxMPDU, RateCtrlBAFail;
+    zmw_get_wlan_dev(dev);
+
+    RateIndex = wd->sta.oppositeInfo[0].rcCell.currentRateIndex;
+    Rate = wd->sta.oppositeInfo[0].rcCell.operationRateSet[RateIndex];
+
+    TxMPDU[Rate] = (TxMPDU[Rate] / 5) + (wd->commTally.RateCtrlTxMPDU * 4 / 5);
+    BAFail[Rate] = (BAFail[Rate] / 5) + (wd->commTally.RateCtrlBAFail * 4 / 5);
+    RateCtrlTxMPDU = wd->commTally.RateCtrlTxMPDU;
+    RateCtrlBAFail = wd->commTally.RateCtrlBAFail;
+    wd->commTally.RateCtrlTxMPDU = 0;
+    wd->commTally.RateCtrlBAFail = 0;
+    if (TxMPDU[Rate] > 0) {
+        BAPER[Rate] = BAFail[Rate] * 1000 / TxMPDU[Rate]; //PER*1000
+        BAPER[Rate] = (BAPER[Rate]>0)? BAPER[Rate]:1;
+    }
+    else {
+        return;
+    }
+
+    HRate = HigherRate(Rate);
+    LRate = LowerRate(Rate);
+    if (BAPER[Rate]>200) {
+        if ((RateCtrlTxMPDU > 100) && (BAPER[Rate]<300) && (HRate != Rate) && BAPER[HRate] &&
+            (BAPER[HRate] < BAPER[Rate] + BADiff[Rate])) {
+            Rate = HRate;
+            //DbgPrint("Rate improved to %d", Rate);
+        }
+        else {
+            Rate = LRate;
+            //DbgPrint("Rate decreased to %d", Rate);
+        }
+    }
+    else if (BAPER[Rate] && BAPER[Rate]<100) {
+        if (RateCtrlTxMPDU > 100) {
+            Rate = HRate;
+            //DbgPrint("Rate improved to %d", Rate);
+        }
+    }
+    wd->sta.oppositeInfo[0].rcCell.currentRate = Rate;
+    wd->sta.oppositeInfo[0].rcCell.currentRateIndex = RateMapToRateIndex(Rate, &wd->sta.oppositeInfo[0].rcCell);
+}
+#endif
diff --git a/drivers/staging/otus/80211core/ratectrl.h b/drivers/staging/otus/80211core/ratectrl.h
new file mode 100644
index 00000000000..92411d725cd
--- /dev/null
+++ b/drivers/staging/otus/80211core/ratectrl.h
@@ -0,0 +1,37 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef _RATECTRL_H
+#define _RATECTRL_H
+
+#define ZM_RATE_CTRL_PROBING_INTERVAL_MS    1000 //1000ms
+#define ZM_RATE_CTRL_MIN_PROBING_PACKET     8
+
+#define ZM_MIN_RATE_FAIL_COUNT              20
+
+#define ZM_RATE_PROBING_THRESHOLD           15  //6%
+#define ZM_RATE_SUCCESS_PROBING             10
+
+#define ZM_RATE_CTRL_RSSI_VARIATION         5  //TBD
+
+extern const u32_t zcRateToPhyCtrl[];
+
+extern void zfRateCtrlInitCell(zdev_t* dev, struct zsRcCell* rcCell, u8_t type, u8_t gBand, u8_t SG40);
+extern u16_t zfRateCtrlGetTxRate(zdev_t* dev, struct zsRcCell* rcCell, u16_t* probing);
+extern void zfRateCtrlTxFailEvent(zdev_t* dev, struct zsRcCell* rcCell, u8_t aggRate, u32_t retryRate);
+extern void zfRateCtrlTxSuccessEvent(zdev_t* dev, struct zsRcCell* rcCell, u8_t successRate);
+extern void zfRateCtrlAggrSta(zdev_t* dev);
+#endif
diff --git a/drivers/staging/otus/80211core/struct.h b/drivers/staging/otus/80211core/struct.h
new file mode 100644
index 00000000000..43631c630a8
--- /dev/null
+++ b/drivers/staging/otus/80211core/struct.h
@@ -0,0 +1,1315 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef _STRUCT_H
+#define _STRUCT_H
+
+#include "../oal_marc.h"
+
+#define ZM_SW_LOOP_BACK                     0 /* 1=>enable, 0=>disable */
+#define ZM_PCI_LOOP_BACK                    0 /* 1=>enable, 0=>disable */
+#define ZM_PROTOCOL_RESPONSE_SIMULATION     0
+
+#define ZM_RX_FRAME_SIZE               1600
+
+extern const u8_t zg11bRateTbl[4];
+extern const u8_t zg11gRateTbl[8];
+
+#define ZM_DRIVER_CORE_MAJOR_VERSION        1
+#define ZM_DRIVER_CORE_MINOR_VERSION        1
+#define ZM_DRIVER_CORE_BRANCH_MAJOR_VERSION 3
+#define ZM_DRIVER_CORE_BRANCH_MINOR_VERSION 39
+
+#ifndef ZM_VTXQ_SIZE
+#define ZM_VTXQ_SIZE                        1024 //2^N
+#endif
+
+#define ZM_VTXQ_SIZE_MASK                   (ZM_VTXQ_SIZE-1)
+#define ZM_VMMQ_SIZE                        8 //2^N
+#define ZM_VMMQ_SIZE_MASK                   (ZM_VMMQ_SIZE-1)
+
+#include "cagg.h"
+
+#define ZM_AGG_POOL_SIZE                    20
+#define ZM_RATE_TABLE_SIZE                  32
+
+#define ZM_MAX_BUF_DISCRETE_NUMBER          5
+
+
+
+
+
+
+
+
+
+/**********************************************************************************/
+/* IBSS macros                                                     */
+/**********************************************************************************/
+#define ZM_IBSS_PEER_ALIVE_COUNTER     4
+
+/**********************************************************************************/
+/* BIT mapping related macros                                                     */
+/**********************************************************************************/
+
+#define ZM_BIT_0       0x1
+#define ZM_BIT_1       0x2
+#define ZM_BIT_2       0x4
+#define ZM_BIT_3       0x8
+#define ZM_BIT_4       0x10
+#define ZM_BIT_5       0x20
+#define ZM_BIT_6       0x40
+#define ZM_BIT_7       0x80
+#define ZM_BIT_8       0x100
+#define ZM_BIT_9       0x200
+#define ZM_BIT_10      0x400
+#define ZM_BIT_11      0x800
+#define ZM_BIT_12      0x1000
+#define ZM_BIT_13      0x2000
+#define ZM_BIT_14      0x4000
+#define ZM_BIT_15      0x8000
+#define ZM_BIT_16      0x10000
+#define ZM_BIT_17      0x20000
+#define ZM_BIT_18      0x40000
+#define ZM_BIT_19      0x80000
+#define ZM_BIT_20      0x100000
+#define ZM_BIT_21      0x200000
+#define ZM_BIT_22      0x400000
+#define ZM_BIT_23      0x800000
+#define ZM_BIT_24      0x1000000
+#define ZM_BIT_25      0x2000000
+#define ZM_BIT_26      0x4000000
+#define ZM_BIT_27      0x8000000
+#define ZM_BIT_28      0x10000000
+#define ZM_BIT_29      0x20000000   //WPA support
+#define ZM_BIT_30      0x40000000
+#define ZM_BIT_31      0x80000000
+
+
+/**********************************************************************************/
+/* MAC address related macros                                                     */
+/**********************************************************************************/
+#define ZM_MAC_BYTE_TO_WORD(macb, macw)   macw[0] = macb[0] + (macb[1] << 8); \
+                                          macw[1] = macb[2] + (macb[3] << 8); \
+                                          macw[2] = macb[4] + (macb[5] << 8);
+
+#define ZM_MAC_WORD_TO_BYTE(macw, macb)   macb[0] = (u8_t) (macw[0] & 0xff); \
+                                          macb[1] = (u8_t) (macw[0] >> 8);   \
+                                          macb[2] = (u8_t) (macw[1] & 0xff); \
+                                          macb[3] = (u8_t) (macw[1] >> 8);   \
+                                          macb[4] = (u8_t) (macw[2] & 0xff); \
+                                          macb[5] = (u8_t) (macw[2] >> 8);
+
+#define ZM_MAC_0(macw)   ((u8_t)(macw[0] & 0xff))
+#define ZM_MAC_1(macw)   ((u8_t)(macw[0] >> 8))
+#define ZM_MAC_2(macw)   ((u8_t)(macw[1] & 0xff))
+#define ZM_MAC_3(macw)   ((u8_t)(macw[1] >> 8))
+#define ZM_MAC_4(macw)   ((u8_t)(macw[2] & 0xff))
+#define ZM_MAC_5(macw)   ((u8_t)(macw[2] >> 8))
+
+#define ZM_IS_MULTICAST_OR_BROADCAST(mac) (mac[0] & 0x01)
+#define ZM_IS_MULTICAST(mac) ((mac[0] & 0x01) && (((u8_t)mac[0]) != 0xFF))
+
+#define ZM_MAC_EQUAL(mac1, mac2)   ((mac1[0]==mac2[0])&&(mac1[1]==mac2[1])&&(mac1[2]==mac2[2]))
+#define ZM_MAC_NOT_EQUAL(mac1, mac2)   ((mac1[0]!=mac2[0])||(mac1[1]!=mac2[1])||(mac1[2]!=mac2[2]))
+/**********************************************************************************/
+/* MAC address related mac'ros (end)                                               */
+/**********************************************************************************/
+#define ZM_BYTE_TO_WORD(A, B)   ((A<<8)+B)
+#define ZM_ROL32( A, n ) \
+        ( ((A) << (n)) | ( ((A)>>(32-(n)))  & ( (1UL << (n)) - 1 ) ) )
+#define ZM_ROR32( A, n ) ZM_ROL32( (A), 32-(n) )
+#define ZM_LO8(v16)  ((u8_t)((v16) & 0xFF))
+#define ZM_HI8(v16)  ((u8_t)(((v16)>>8)&0xFF))
+
+#ifdef ZM_ENABLE_BUFFER_TRACE
+extern void zfwBufTrace(zdev_t* dev, zbuf_t *buf, u8_t *functionName);
+#define ZM_BUFFER_TRACE(dev, buf)       zfwBufTrace(dev, buf, __FUNCTION__);
+#else
+#define ZM_BUFFER_TRACE(dev, buf)
+#endif
+
+/* notification events to heart beat function */
+#define ZM_BSSID_LIST_SCAN         0x01
+
+/* CAM mode */
+#define ZM_CAM_AP                       0x1
+#define ZM_CAM_STA                      0x2
+#define ZM_CAM_HOST                     0x4
+
+/* finite state machine for adapter */
+#define ZM_STA_STATE_DISCONNECT           1
+#define ZM_STA_STATE_CONNECTING           2
+#define ZM_STA_STATE_CONNECTED            3
+
+/* Event definitions for  finite state machine */
+#define ZM_EVENT_TIMEOUT_SCAN             0x0000
+#define ZM_EVENT_TIMEOUT_BG_SCAN          0x0001
+#define ZN_EVENT_TIMEOUT_RECONNECT        0x0002
+#define ZM_EVENT_TIMEOUT_INIT_SCAN        0x0003
+#define ZM_EVENT_TIMEOUT_AUTH             0x0004
+#define ZM_EVENT_TIMEOUT_ASSO             0x0005
+#define ZM_EVENT_TIMEOUT_AUTO_SCAN        0x0006
+#define ZM_EVENT_TIMEOUT_MIC_FAIL         0x0007
+#define ZM_EVENT_TIMEOUT_CHECK_AP         0x0008
+#define ZM_EVENT_CONNECT                  0x0009
+#define ZM_EVENT_INIT_SCAN                0x000a
+#define ZM_EVENT_SCAN                     0x000b
+#define ZM_EVENT_BG_SCAN                  0x000c
+#define ZM_EVENT_DISCONNECT               0x000d
+#define ZM_EVENT_WPA_MIC_FAIL             0x000e
+#define ZM_EVENT_AP_ALIVE                 0x000f
+#define ZM_EVENT_CHANGE_TO_AP             0x0010
+#define ZM_EVENT_CHANGE_TO_STA            0x0011
+#define ZM_EVENT_IDLE                     0x0012
+#define ZM_EVENT_AUTH                     0x0013
+#define ZM_EVENT_ASSO_RSP                 0x0014
+#define ZM_EVENT_WPA_PK_OK                0x0015
+#define ZM_EVENT_WPA_GK_OK                0x0016
+#define ZM_EVENT_RCV_BEACON               0x0017
+#define ZM_EVENT_RCV_PROBE_RSP            0x0018
+#define ZM_EVENT_SEND_DATA                0x0019
+#define ZM_EVENT_AUTO_SCAN                0x001a
+#define ZM_EVENT_MIC_FAIL1                0x001d
+#define ZM_EVENT_MIC_FAIL2                0x001e
+#define ZM_EVENT_IBSS_MONITOR             0x001f
+#define ZM_EVENT_IN_SCAN                  0x0020
+#define ZM_EVENT_CM_TIMER                 0x0021
+#define ZM_EVENT_CM_DISCONNECT            0x0022
+#define ZM_EVENT_CM_BLOCK_TIMER           0x0023
+#define ZM_EVENT_TIMEOUT_ADDBA            0x0024
+#define ZM_EVENT_TIMEOUT_PERFORMANCE      0x0025
+#define ZM_EVENT_SKIP_COUNTERMEASURE	  0x0026
+#define ZM_EVENT_NONE                     0xffff
+
+/* Actions after call finite state machine */
+#define ZM_ACTION_NONE                    0x0000
+#define ZM_ACTION_QUEUE_DATA              0x0001
+#define ZM_ACTION_DROP_DATA               0x0002
+
+/* Timers for finite state machine */
+#define ZM_TICK_ZERO                      0
+#define ZM_TICK_INIT_SCAN_END             8
+#define ZM_TICK_NEXT_BG_SCAN              50
+#define ZM_TICK_BG_SCAN_END               8
+#define ZM_TICK_AUTH_TIMEOUT              4
+#define ZM_TICK_ASSO_TIMEOUT              4
+#define ZM_TICK_AUTO_SCAN                 300
+#define ZM_TICK_MIC_FAIL_TIMEOUT          6000
+#define ZM_TICK_CHECK_AP1                 150
+#define ZM_TICK_CHECK_AP2                 350
+#define ZM_TICK_CHECK_AP3                 250
+#define ZM_TICK_IBSS_MONITOR              160
+#define ZM_TICK_IN_SCAN                   4
+#define ZM_TICK_CM_TIMEOUT                6000
+#define ZM_TICK_CM_DISCONNECT             200
+#define ZM_TICK_CM_BLOCK_TIMEOUT          6000
+
+/* Fix bug#33338 Counter Measure Issur */
+#ifdef NDIS_CM_FOR_XP
+#define ZM_TICK_CM_TIMEOUT_OFFSET        2160
+#define ZM_TICK_CM_DISCONNECT_OFFSET     72
+#define ZM_TICK_CM_BLOCK_TIMEOUT_OFFSET  2160
+#else
+#define ZM_TICK_CM_TIMEOUT_OFFSET        0
+#define ZM_TICK_CM_DISCONNECT_OFFSET     0
+#define ZM_TICK_CM_BLOCK_TIMEOUT_OFFSET  0
+#endif
+
+#define ZM_TIME_ACTIVE_SCAN               30 //ms
+#define ZM_TIME_PASSIVE_SCAN              110 //ms
+
+/* finite state machine for BSS connect */
+#define ZM_STA_CONN_STATE_NONE            0
+#define ZM_STA_CONN_STATE_AUTH_OPEN       1
+#define ZM_STA_CONN_STATE_AUTH_SHARE_1    2
+#define ZM_STA_CONN_STATE_AUTH_SHARE_2    3
+#define ZM_STA_CONN_STATE_ASSOCIATE       4
+#define ZM_STA_CONN_STATE_SSID_NOT_FOUND  5
+#define ZM_STA_CONN_STATE_AUTH_COMPLETED  6
+
+/* finite state machine for WPA handshaking */
+#define ZM_STA_WPA_STATE_INIT             0
+#define ZM_STA_WPA_STATE_PK_OK            1
+#define ZM_STA_WPA_STATE_GK_OK            2
+
+/* various timers */
+#define ZM_INTERVAL_CONNECT_TIMEOUT          20   /* 200 milisecond */
+
+/* IBSS definitions */
+#define ZM_IBSS_PARTNER_LOST                 0
+#define ZM_IBSS_PARTNER_ALIVE                1
+#define ZM_IBSS_PARTNER_CHECK                2
+
+#define ZM_BCMC_ARRAY_SIZE                  16 /* Must be 2^N */
+#define ZM_UNI_ARRAY_SIZE                   16 /* Must be 2^N */
+
+#define ZM_MAX_DEFRAG_ENTRIES               4  /* 2^N */
+#define ZM_DEFRAG_AGING_TIME_SEC            5  /* 5 seconds */
+
+#define ZM_MAX_WPAIE_SIZE                   128
+/* WEP related definitions */
+#define ZM_USER_KEY_DEFAULT                 64
+#define ZM_USER_KEY_PK                      0                /* Pairwise Key */
+#define ZM_USER_KEY_GK                      1                /* Group Key */
+/* AP WLAN Type */
+#define ZM_WLAN_TYPE_PURE_B                 2
+#define ZM_WLAN_TYPE_PURE_G                 1
+#define ZM_WLAN_TYPE_MIXED                  0
+
+/* HAL State */
+#define ZM_HAL_STATE_INIT                   0
+#define ZM_HAL_STATE_RUNNING                1
+
+/* AP Capability */
+#define ZM_All11N_AP                        0x01
+#define ZM_XR_AP                            0x02
+#define ZM_SuperG_AP                        0x04
+
+/* MPDU Density */
+#define ZM_MPDU_DENSITY_NONE                0
+#define ZM_MPDU_DENSITY_1_8US               1
+#define ZM_MPDU_DENSITY_1_4US               2
+#define ZM_MPDU_DENSITY_1_2US               3
+#define ZM_MPDU_DENSITY_1US                 4
+#define ZM_MPDU_DENSITY_2US                 5
+#define ZM_MPDU_DENSITY_4US                 6
+#define ZM_MPDU_DENSITY_8US                 7
+
+/* Software Encryption */
+#define ZM_SW_TKIP_ENCRY_EN                0x01
+#define ZM_SW_TKIP_DECRY_EN                0x02
+#define ZM_SW_WEP_ENCRY_EN                 0x04
+#define ZM_SW_WEP_DECRY_EN                 0x08
+
+/* Default Support Rate */
+#define ZM_DEFAULT_SUPPORT_RATE_ZERO       0x0
+#define ZM_DEFAULT_SUPPORT_RATE_DISCONNECT 0x1
+#define ZM_DEFAULT_SUPPORT_RATE_IBSS_B     0x2
+#define ZM_DEFAULT_SUPPORT_RATE_IBSS_AG    0x3
+
+/* security related definitions */
+struct zsTkipSeed
+{
+    u8_t   tk[32];     /* key */
+    u8_t   ta[6];
+    u16_t  ttak[5];
+    u16_t  ppk[6];
+    u16_t  iv16,iv16tmp;
+    u32_t  iv32,iv32tmp;
+};
+
+struct zsMicVar
+{
+    u32_t  k0, k1;        // Key
+    u32_t  left, right;   // Current state
+    u32_t  m;             // Message accumulator (single word)
+    u16_t  nBytes;        // # bytes in M
+};
+
+struct zsDefragEntry
+{
+    u8_t    fragCount;
+    u8_t    addr[6];
+    u16_t   seqNum;
+    zbuf_t* fragment[8];
+    u32_t   tick;
+};
+
+struct zsDefragList
+{
+    struct zsDefragEntry   defragEntry[ZM_MAX_DEFRAG_ENTRIES];
+    u8_t                   replaceNum;
+};
+
+#define ZM_MAX_OPPOSITE_COUNT      16
+#define ZM_MAX_TX_SAMPLES          15
+#define ZM_TX_RATE_DOWN_CRITERIA   80
+#define ZM_TX_RATE_UP_CRITERIA    200
+
+
+#define ZM_MAX_PROBE_HIDDEN_SSID_SIZE 2
+struct zsSsidList
+{
+    u8_t            ssid[32];
+    u8_t            ssidLen;
+};
+
+struct zsWrapperSetting
+{
+    u8_t            bDesiredBssid;
+    u8_t            desiredBssid[6];
+    u16_t           bssid[3];
+    u8_t            ssid[32];
+    u8_t            ssidLen;
+    u8_t            authMode;
+    u8_t            wepStatus;
+    u8_t            encryMode;
+    u8_t            wlanMode;
+    u16_t           frequency;
+    u16_t           beaconInterval;
+    u8_t            dtim;
+    u8_t            preambleType;
+    u16_t           atimWindow;
+
+    struct zsSsidList probingSsidList[ZM_MAX_PROBE_HIDDEN_SSID_SIZE];
+
+    u8_t            dropUnencryptedPkts;
+    u8_t            ibssJoinOnly;
+    u32_t           adhocMode;
+    u8_t            countryIsoName[4];
+    u16_t           autoSetFrequency;
+
+    /* AP */
+    u8_t            bRateBasic;
+    u8_t            gRateBasic;
+    u32_t           nRateBasic;
+    u8_t            bgMode;
+
+    /* Common */
+    u8_t            staWmeEnabled;
+    u8_t            staWmeQosInfo;
+    u8_t            apWmeEnabled;
+
+
+    /* rate information: added in the future */
+};
+
+struct zsWrapperFeatureCtrl
+{
+    u8_t           bIbssGMode;
+};
+
+#define  ZM_MAX_PS_STA            16
+#define  ZM_PS_QUEUE_SIZE         32
+
+struct zsStaPSEntity
+{
+    u8_t           bUsed;
+    u8_t           macAddr[6];
+    u8_t           bDataQueued;
+};
+
+struct zsStaPSList
+{
+    u8_t           count;
+    struct zsStaPSEntity    entity[ZM_MAX_PS_STA];
+};
+
+#define ZM_MAX_TIMER_COUNT   32
+
+/* double linked list */
+struct zsTimerEntry
+{
+    u16_t   event;
+    u32_t   timer;
+    struct zsTimerEntry *pre;
+    struct zsTimerEntry *next;
+};
+
+struct zsTimerList
+{
+    u8_t   freeCount;
+    struct zsTimerEntry list[ZM_MAX_TIMER_COUNT];
+    struct zsTimerEntry *head;
+    struct zsTimerEntry *tail;
+};
+
+/* Multicast list */
+#define ZM_MAX_MULTICAST_LIST_SIZE     64
+
+struct zsMulticastAddr
+{
+    u8_t addr[6];
+};
+
+struct zsMulticastList
+{
+    u8_t   size;
+    struct zsMulticastAddr macAddr[ZM_MAX_MULTICAST_LIST_SIZE];
+};
+
+enum ieee80211_cwm_mode {
+    CWM_MODE20,
+    CWM_MODE2040,
+    CWM_MODE40,
+    CWM_MODEMAX
+
+};
+
+enum ieee80211_cwm_extprotspacing {
+    CWM_EXTPROTSPACING20,
+    CWM_EXTPROTSPACING25,
+    CWM_EXTPROTSPACINGMAX
+};
+
+enum ieee80211_cwm_width {
+    CWM_WIDTH20,
+    CWM_WIDTH40
+};
+
+enum ieee80211_cwm_extprotmode {
+    CWM_EXTPROTNONE,  /* no protection */
+    CWM_EXTPROTCTSONLY,   /* CTS to self */
+    CWM_EXTPROTRTSCTS,    /* RTS-CTS */
+    CWM_EXTPROTMAX
+};
+
+struct ieee80211_cwm {
+
+    /* Configuration */
+    enum ieee80211_cwm_mode         cw_mode;            /* CWM mode */
+    u8_t                            cw_extoffset;       /* CWM Extension Channel Offset */
+    enum ieee80211_cwm_extprotmode  cw_extprotmode;     /* CWM Extension Channel Protection Mode */
+    enum ieee80211_cwm_extprotspacing cw_extprotspacing;/* CWM Extension Channel Protection Spacing */
+    u32_t                           cw_enable;          /* CWM State Machine Enabled */
+    u32_t                           cw_extbusythreshold;/* CWM Extension Channel Busy Threshold */
+
+    /* State */
+    enum ieee80211_cwm_width        cw_width;           /* CWM channel width */
+};
+
+
+/* AP : STA database structure */
+struct zsStaTable
+{
+    u32_t time;     /* tick time */
+    //u32_t phyCtrl;   /* Tx PHY CTRL */
+    u16_t addr[3];  /* STA MAC address */
+    u16_t state;    /* aut/asoc */
+    //u16_t retry;    /* Retry count */
+    struct zsRcCell rcCell;
+
+    u8_t valid;     /* Valid flag : 1=>valid */
+    u8_t psMode;    /* STA power saving mode */
+    u8_t staType;   /* 0=>11b, 1=>11g, 2=>11n */
+    u8_t qosType;   /* 0=>Legacy, 1=>WME */
+    u8_t qosInfo;   /* WME QoS info */
+    u8_t vap;       /* Virtual AP ID */
+    u8_t encryMode; /* Encryption type for this STA */
+    u8_t keyIdx;
+    struct zsMicVar     txMicKey;
+    struct zsMicVar     rxMicKey;
+    u16_t iv16;
+    u32_t iv32;
+#ifdef ZM_ENABLE_CENC
+    /* CENC */
+    u8_t cencKeyIdx;
+    u32_t txiv[4];
+    u32_t rxiv[4];
+#endif //ZM_ENABLE_CENC
+};
+
+struct zdStructWds
+{
+    u8_t    wdsBitmap;                      /* Set bit-N to 1 to enable WDS */
+    u8_t    encryMode[ZM_MAX_WDS_SUPPORT];  /* WDS encryption mode */
+    u16_t   macAddr[ZM_MAX_WDS_SUPPORT][3]; /* WDS neighbor MAC address */
+};
+
+    // htcapinfo 16bits
+#define HTCAP_AdvCodingCap          0x0001
+#define HTCAP_SupChannelWidthSet    0x0002
+#define HTCAP_DynamicSMPS           0x0004
+#define HTCAP_SMEnabled             0x000C
+#define HTCAP_GreenField            0x0010
+#define HTCAP_ShortGIfor20MHz       0x0020
+#define HTCAP_ShortGIfor40MHz       0x0040
+#define HTCAP_TxSTBC                0x0080
+#define HTCAP_RxOneStream           0x0100
+#define HTCAP_RxTwoStream           0x0200
+#define HTCAP_RxThreeStream         0x0300
+#define HTCAP_DelayedBlockACK       0x0400
+#define HTCAP_MaxAMSDULength        0x0800
+#define HTCAP_DSSSandCCKin40MHz     0x1000
+#define HTCAP_PSMPSup               0x2000
+#define HTCAP_STBCControlFrameSup   0x4000
+#define HTCAP_LSIGTXOPProtectionSUP 0x8000
+    // Ampdu HT Parameter Info 8bits
+#define HTCAP_MaxRxAMPDU0           0x00
+#define HTCAP_MaxRxAMPDU1           0x01
+#define HTCAP_MaxRxAMPDU2           0x02
+#define HTCAP_MaxRxAMPDU3           0x03
+    // PCO 8bits
+#define HTCAP_PCO                   0x01
+#define HTCAP_TransmissionTime1     0x02
+#define HTCAP_TransmissionTime2     0x04
+#define HTCAP_TransmissionTime3     0x06
+    // MCS FeedBack 8bits
+#define HTCAP_PlusHTCSupport        0x04
+#define HTCAP_RDResponder           0x08
+    // TX Beamforming 0 8bits
+#define HTCAP_TxBFCapable           0x01
+#define HTCAP_RxStaggeredSoundCap   0x02
+#define HTCAP_TxStaggeredSoundCap   0x04
+#define HTCAP_RxZLFCapable          0x08
+#define HTCAP_TxZLFCapable          0x10
+#define HTCAP_ImplicitTxBFCapable   0x20
+    // Tx Beamforming 1 8bits
+#define HTCAP_ExplicitCSITxBFCap    0x01
+#define HTCAP_ExpUncompSteerMatrCap 0x02
+    // Antenna Selection Capabilities 8bits
+#define HTCAP_AntennaSelectionCap       0x01
+#define HTCAP_ExplicitCSITxASCap        0x02
+#define HTCAP_AntennaIndFeeTxASCap      0x04
+#define HTCAP_ExplicitCSIFeedbackCap    0x08
+#define HTCAP_AntennaIndFeedbackCap     0x10
+#define HTCAP_RxASCap                   0x20
+#define HTCAP_TxSoundPPDUsCap           0x40
+
+
+
+struct zsHTCapability
+{
+    u8_t ElementID;
+    u8_t Length;
+    // HT Capability Info
+    u16_t HtCapInfo;
+    u8_t AMPDUParam;
+    u8_t MCSSet[16];    //16 bytes
+    // Extended HT Capability Info
+    u8_t PCO;
+    u8_t MCSFeedBack;
+
+    u8_t TxBFCap[4];
+    u8_t AselCap;
+};
+
+union zuHTCapability
+{
+    struct zsHTCapability Data;
+    u8_t Byte[28];
+};
+
+    //channelinfo 8bits
+#define ExtHtCap_ExtChannelOffsetAbove  0x01
+#define ExtHtCap_ExtChannelOffsetBelow  0x03
+#define ExtHtCap_RecomTxWidthSet        0x04
+#define ExtHtCap_RIFSMode               0x08
+#define ExtHtCap_ControlAccessOnly      0x10
+    //operatinginfo 16bits
+#define ExtHtCap_NonGFDevicePresent     0x0004
+    //beaconinfo 16bits
+#define ExtHtCap_DualBeacon             0x0040
+#define ExtHtCap_DualSTBCProtection     0x0080
+#define ExtHtCap_SecondaryBeacon        0x0100
+#define ExtHtCap_LSIGTXOPProtectFullSup 0x0200
+#define ExtHtCap_PCOActive              0x0400
+#define ExtHtCap_PCOPhase               0x0800
+
+
+struct zsExtHTCapability
+{
+    u8_t    ElementID;
+    u8_t    Length;
+    u8_t    ControlChannel;
+    u8_t    ChannelInfo;
+    u16_t   OperatingInfo;
+    u16_t   BeaconInfo;
+    // Supported MCS Set
+    u8_t    MCSSet[16];
+};
+
+union zuExtHTCapability
+{
+    struct zsExtHTCapability Data;
+    u8_t Byte[24];
+};
+
+struct InformationElementSta {
+    struct zsHTCapability       HtCap;
+    struct zsExtHTCapability    HtInfo;
+};
+
+struct InformationElementAp {
+    struct zsHTCapability       HtCap;
+};
+
+#define ZM_MAX_FREQ_REQ_QUEUE  32
+typedef void (*zfpFreqChangeCompleteCb)(zdev_t* dev);
+
+struct zsWlanDevFreqControl
+{
+    u16_t                     freqReqQueue[ZM_MAX_FREQ_REQ_QUEUE];
+    u8_t                     freqReqBw40[ZM_MAX_FREQ_REQ_QUEUE];
+    u8_t                     freqReqExtOffset[ZM_MAX_FREQ_REQ_QUEUE];
+    zfpFreqChangeCompleteCb   freqChangeCompCb[ZM_MAX_FREQ_REQ_QUEUE];
+    u8_t                      freqReqQueueHead;
+    u8_t                      freqReqQueueTail;
+};
+
+struct zsWlanDevAp
+{
+    u16_t   protectedObss;    /* protected overlap BSS */
+    u16_t   staAgingTimeSec;  /* in second, STA will be deathed if it does not */
+                              /* active for this long time                     */
+    u16_t   staProbingTimeSec;/* in second, STA will be probed if it does not  */
+                              /* active for this long time                     */
+    u8_t    authSharing;      /* authentication on going*/
+    u8_t    bStaAssociated;   /* 11b STA associated */
+    u8_t    gStaAssociated;   /* 11g STA associated */
+    u8_t    nStaAssociated;   /* 11n STA associated */
+    u16_t   protectionMode;   /* AP protection mode flag */
+    u16_t   staPowerSaving;   /* Set associated power saving STA count */
+
+
+
+    zbuf_t*  uniArray[ZM_UNI_ARRAY_SIZE]; /* array to store unicast frames */
+    u16_t   uniHead;
+    u16_t   uniTail;
+
+    /* HT Capability Info */
+    union zuHTCapability HTCap; //CWYang(+)
+
+    /* Extended HT Capability Info */
+    union zuExtHTCapability ExtHTCap; //CWYang(+)
+
+    /* STA table */
+    struct zsStaTable staTable[ZM_MAX_STA_SUPPORT];
+
+    /* WDS */
+    struct zdStructWds wds;
+    /* WPA */
+    u8_t wpaIe[ZM_MAX_AP_SUPPORT][ZM_MAX_WPAIE_SIZE];
+    u8_t wpaLen[ZM_MAX_AP_SUPPORT];
+    u8_t stawpaIe[ZM_MAX_AP_SUPPORT][ZM_MAX_WPAIE_SIZE];
+    u8_t stawpaLen[ZM_MAX_AP_SUPPORT];
+    u8_t wpaSupport[ZM_MAX_AP_SUPPORT];
+
+    //struct zsTkipSeed   bcSeed;
+    u8_t bcKeyIndex[ZM_MAX_AP_SUPPORT];
+    u8_t bcHalKeyIdx[ZM_MAX_AP_SUPPORT];
+    struct zsMicVar     bcMicKey[ZM_MAX_AP_SUPPORT];
+    u16_t iv16[ZM_MAX_AP_SUPPORT];
+    u32_t iv32[ZM_MAX_AP_SUPPORT];
+
+#ifdef ZM_ENABLE_CENC
+    /* CENC */
+    u32_t txiv[ZM_MAX_AP_SUPPORT][4];
+#endif //ZM_ENABLE_CENC
+
+    /* Virtual AP */
+    u8_t    beaconCounter;
+    u8_t    vapNumber;
+    u8_t    apBitmap;                         /* Set bit-N to 1 to enable VAP */
+    u8_t    hideSsid[ZM_MAX_AP_SUPPORT];
+    u8_t    authAlgo[ZM_MAX_AP_SUPPORT];
+    u8_t    ssid[ZM_MAX_AP_SUPPORT][32];      /* SSID */
+    u8_t    ssidLen[ZM_MAX_AP_SUPPORT];       /* SSID length */
+    u8_t    encryMode[ZM_MAX_AP_SUPPORT];
+    u8_t    wepStatus[ZM_MAX_AP_SUPPORT];
+    u16_t   capab[ZM_MAX_AP_SUPPORT];         /* Capability */
+    u8_t    timBcmcBit[ZM_MAX_AP_SUPPORT];    /* BMCM bit of TIM */
+    u8_t    wlanType[ZM_MAX_AP_SUPPORT];
+
+    /* Array to store BC or MC frames */
+    zbuf_t*  bcmcArray[ZM_MAX_AP_SUPPORT][ZM_BCMC_ARRAY_SIZE];
+    u16_t   bcmcHead[ZM_MAX_AP_SUPPORT];
+    u16_t   bcmcTail[ZM_MAX_AP_SUPPORT];
+
+    u8_t                    qosMode;                          /* 1=>WME */
+    u8_t                    uapsdEnabled;
+    struct zsQueue*         uapsdQ;
+
+    u8_t                    challengeText[128];
+
+    struct InformationElementAp ie[ZM_MAX_STA_SUPPORT];
+
+
+};
+
+#define ZM_MAX_BLOCKING_AP_LIST_SIZE    4 /* 2^N */
+struct zsBlockingAp
+{
+    u8_t addr[6];
+    u8_t weight;
+};
+
+#define ZM_SCAN_MGR_SCAN_NONE           0
+#define ZM_SCAN_MGR_SCAN_INTERNAL       1
+#define ZM_SCAN_MGR_SCAN_EXTERNAL       2
+
+struct zsWlanDevStaScanMgr
+{
+    u8_t                    scanReqs[2];
+    u8_t                    currScanType;
+    u8_t                    scanStartDelay;
+};
+
+#define ZM_PS_MSG_STATE_ACTIVE          0
+#define ZM_PS_MSG_STATE_SLEEP           1
+#define ZM_PS_MSG_STATE_T1              2
+#define ZM_PS_MSG_STATE_T2              3
+#define ZM_PS_MSG_STATE_S1              4
+
+#define ZM_PS_MAX_SLEEP_PERIODS         3       // The number of beacon periods
+
+struct zsWlanDevStaPSMgr
+{
+    u8_t                    state;
+    u8_t                    isSleepAllowed;
+    u8_t                    maxSleepPeriods;
+    u8_t                    ticks;
+    u32_t                   lastTxUnicastFrm;
+    u32_t                   lastTxMulticastFrm;
+    u32_t                   lastTxBroadcastFrm;
+    u8_t                    tempWakeUp; /*enable when wake up but still in ps mode */
+    u16_t                   sleepAllowedtick;
+};
+
+struct zsWlanDevSta
+{
+    u32_t                   beaconTxCnt;  /* Transmitted beacon counter (in IBSS) */
+    u8_t                    txBeaconInd;  /* In IBSS mode, true means that we just transmit a beacon during
+                                             last beacon period.
+                                           */
+    u16_t                   beaconCnt;    /* receive beacon count, will be perodically reset */
+    u16_t                   bssid[3];     /* BSSID of connected AP */
+    u8_t                    ssid[32];     /* SSID */
+    u8_t                    ssidLen;      /* SSID length */
+    u8_t                    mTxRate;      /* Tx rate for multicast */
+    u8_t                    uTxRate;      /* Tx rate for unicast */
+    u8_t                    mmTxRate;     /* Tx rate for management frame */
+    u8_t                    bChannelScan;
+    u8_t                    bScheduleScan;
+
+    u8_t                    InternalScanReq;
+    u16_t                   activescanTickPerChannel;
+    u16_t                   passiveScanTickPerChannel;
+    u16_t                   scanFrequency;
+    u32_t                   connPowerInHalfDbm;
+
+    u16_t                   currentFrequency;
+    u16_t                   currentBw40;
+    u16_t                   currentExtOffset;
+
+    u8_t                    bPassiveScan;
+
+    struct zsBlockingAp     blockingApList[ZM_MAX_BLOCKING_AP_LIST_SIZE];
+
+    //struct zsBssInfo        bssInfoPool[ZM_MAX_BSS];
+    struct zsBssInfo*       bssInfoArray[ZM_MAX_BSS];
+    struct zsBssList        bssList;
+    u8_t                    bssInfoArrayHead;
+    u8_t                    bssInfoArrayTail;
+    u8_t                    bssInfoFreeCount;
+
+    u8_t                    authMode;
+    u8_t                    currentAuthMode;
+    u8_t                    wepStatus;
+    u8_t                    encryMode;
+    u8_t                    keyId;
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+    u8_t                    ibssWpa2Psk;
+#endif
+#ifdef ZM_ENABLE_CENC
+    u8_t                    cencKeyId; //CENC
+#endif //ZM_ENABLE_CENC
+    u8_t                    dropUnencryptedPkts;
+    u8_t                    ibssJoinOnly;
+    u8_t                    adapterState;
+    u8_t                    oldAdapterState;
+    u8_t                    connectState;
+    u8_t                    connectRetry;
+    u8_t                    wpaState;
+    u8_t                    wpaIe[ZM_MAX_IE_SIZE + 2];
+    u8_t                    rsnIe[ZM_MAX_IE_SIZE + 2];
+    u8_t                    challengeText[255+2];
+    u8_t                    capability[2];
+    //u8_t                    connectingHiddenAP;
+    //u8_t                    scanWithSSID;
+    u16_t                   aid;
+    u32_t                   mgtFrameCount;
+    u8_t                    bProtectionMode;
+	u32_t					NonNAPcount;
+	u8_t					RTSInAGGMode;
+    u32_t                   connectTimer;
+    u16_t                   atimWindow;
+    u8_t                    desiredBssid[6];
+    u8_t                    bDesiredBssid;
+    struct zsTkipSeed       txSeed;
+    struct zsTkipSeed       rxSeed[4];
+    struct zsMicVar         txMicKey;
+    struct zsMicVar         rxMicKey[4];
+    u16_t                   iv16;
+    u32_t                   iv32;
+    struct zsOppositeInfo   oppositeInfo[ZM_MAX_OPPOSITE_COUNT];
+    u8_t                    oppositeCount;
+    u8_t                    bssNotFoundCount;     /* sitesurvey for search desired ISBB threshold */
+    u16_t                   rxBeaconCount;
+    u8_t                    beaconMissState;
+    u32_t                   rxBeaconTotal;
+    u8_t                    bIsSharedKey;
+    u8_t                    connectTimeoutCount;
+
+    u8_t                    recvAtim;
+
+    /* ScanMgr Control block */
+    struct zsWlanDevStaScanMgr scanMgr;
+    struct zsWlanDevStaPSMgr   psMgr;
+
+    // The callback would be called if receiving an unencrypted packets but
+    // the station is in encrypted mode. The wrapper could decide whether
+    // to drop the packet by its OS setting.
+    zfpStaRxSecurityCheckCb pStaRxSecurityCheckCb;
+
+    /* WME */
+    u8_t                    apWmeCapability; //bit-0 => a WME AP
+                                             //bit-7 => a UAPSD AP
+    u8_t                    wmeParameterSetCount;
+
+    u8_t                    wmeEnabled;
+    #define ZM_STA_WME_ENABLE_BIT       0x1
+    #define ZM_STA_UAPSD_ENABLE_BIT     0x2
+    u8_t                    wmeQosInfo;
+
+    u8_t                    wmeConnected;
+    u8_t                    qosInfo;
+    struct zsQueue*         uapsdQ;
+
+    /* countermeasures */
+    u8_t                    cmMicFailureCount;
+    u8_t                    cmDisallowSsidLength;
+    u8_t                    cmDisallowSsid[32];
+
+    /* power-saving mode */
+    u8_t                    powerSaveMode;
+    zbuf_t*                 staPSDataQueue[ZM_PS_QUEUE_SIZE];
+    u8_t                    staPSDataCount;
+
+    /* IBSS power-saving mode */
+    /* record the STA which has entered the PS mode */
+    struct zsStaPSList      staPSList;
+    /* queue the data of the PS STAs */
+    zbuf_t*                  ibssPSDataQueue[ZM_PS_QUEUE_SIZE];
+    u8_t                    ibssPSDataCount;
+    u8_t                    ibssPrevPSDataCount;
+    u8_t                    bIbssPSEnable;
+    /* BIT_15: ON/OFF, BIT_0~14: Atim Timer */
+    u16_t                   ibssAtimTimer;
+
+    /* WPA2 */
+    struct zsPmkidInfo      pmkidInfo;
+
+    /* Multicast list related objects */
+    struct zsMulticastList  multicastList;
+
+    /* XP packet filter feature : */
+    /* 1=>enable: All multicast address packets, not just the ones enumerated in the multicast address list. */
+    /* 0=>disable */
+    u8_t                    bAllMulticast;
+
+    /* reassociation flag */
+    u8_t                    connectByReasso;
+    u8_t                    failCntOfReasso;
+
+	/* for HT configure control setting */
+    u8_t                    preambleTypeHT;  /* HT: 0 Mixed mode    1 Green field */
+	u8_t                    htCtrlBandwidth;
+	u8_t                    htCtrlSTBC;
+	u8_t                    htCtrlSG;
+    u8_t                    defaultTA;
+
+    u8_t                    connection_11b;
+
+    u8_t                    EnableHT;
+    u8_t                    SG40;
+    u8_t                    HT2040;
+    /* for WPA setting */
+    u8_t                    wpaSupport;
+    u8_t                    wpaLen;
+
+    /* IBSS related objects */
+    u8_t                    ibssDelayedInd;
+    struct zsPartnerNotifyEvent ibssDelayedIndEvent;
+    u8_t                    ibssPartnerStatus;
+
+    u8_t                    bAutoReconnect;
+
+    u8_t                    flagFreqChanging;
+    u8_t                    flagKeyChanging;
+    struct zsBssInfo        ibssBssDesc;
+    u8_t                    ibssBssIsCreator;
+    u16_t                   ibssReceiveBeaconCount;
+    u8_t                    ibssSiteSurveyStatus;
+
+    u8_t                    disableProbingWithSsid;
+#ifdef ZM_ENABLE_CENC
+    /* CENC */
+    u8_t                    cencIe[ZM_MAX_IE_SIZE + 2];
+#endif //ZM_ENABLE_CENC
+    u32_t txiv[4];  //Tx PN Sequence
+    u32_t rxiv[4];  //Rx PN Sequence
+    u32_t rxivGK[4];//Broadcast Rx PN Sequence
+    u8_t  wepKey[4][32];    // For Software WEP
+	u8_t  SWEncryMode[4];
+
+    /* 802.11d */
+    u8_t b802_11D;
+
+    /* 802.11h */
+    u8_t TPCEnable;
+    u8_t DFSEnable;
+    u8_t DFSDisableTx;
+
+    /* Owl AP */
+    u8_t athOwlAp;
+
+    /* Enable BA response in driver */
+    u8_t enableDrvBA;
+
+    /* HT Capability Info */
+    union zuHTCapability HTCap; //CWYang(+)
+
+    /* Extended HT Capability Info */
+    union zuExtHTCapability ExtHTCap; //CWYang(+)
+
+    struct InformationElementSta   ie;
+
+#define ZM_CACHED_FRAMEBODY_SIZE   200
+    u8_t                    asocReqFrameBody[ZM_CACHED_FRAMEBODY_SIZE];
+    u16_t                   asocReqFrameBodySize;
+    u8_t                    asocRspFrameBody[ZM_CACHED_FRAMEBODY_SIZE];
+    u16_t                   asocRspFrameBodySize;
+    u8_t                    beaconFrameBody[ZM_CACHED_FRAMEBODY_SIZE];
+    u16_t                   beaconFrameBodySize;
+
+    u8_t                    ac0PriorityHigherThanAc2;
+    u8_t                    SWEncryptEnable;
+
+    u8_t                    leapEnabled;
+
+    u32_t                    TotalNumberOfReceivePackets;
+    u32_t                    TotalNumberOfReceiveBytes;
+    u32_t                    avgSizeOfReceivePackets;
+
+    u32_t                    ReceivedPacketRateCounter;
+    u32_t                    ReceivedPktRatePerSecond;
+
+    /* #2 Record the sequence number to determine whether the unicast frame is separated by RIFS or not */
+#define    ZM_RIFS_STATE_DETECTING    0
+#define    ZM_RIFS_STATE_DETECTED     1
+#define    ZM_RIFS_TIMER_TIMEOUT      4480          // <Driver time>4480ms <Real time>7s
+    u8_t                    rifsState;
+    u8_t                    rifsLikeFrameCnt;
+    u16_t                   rifsLikeFrameSequence[3];
+    u32_t                   rifsTimer;
+    u32_t                   rifsCount;
+
+    /* RX filter desired by upper layers.  Note this contains some bits which must be filtered
+       by sw since the hw supports only a subset of possible filter actions.= */
+    u32_t  osRxFilter;
+
+    u8_t   bSafeMode;
+
+    u32_t  ibssAdditionalIESize;
+    u8_t   ibssAdditionalIE[256];
+}; //struct zsWlanDevSta
+
+#define ZM_CMD_QUEUE_SIZE                   256  //Roger Check, test 64 when ready
+
+#define ZM_OID_READ                         1
+#define ZM_OID_WRITE                        2
+#define ZM_OID_INTERNAL_WRITE               3
+#define ZM_CMD_SET_FREQUENCY                4
+#define ZM_CMD_SET_KEY                      5
+#define ZM_CWM_READ                         6
+#define ZM_MAC_READ                         7
+#define ZM_ANI_READ                         8
+#define ZM_EEPROM_READ                      9
+#define ZM_EEPROM_WRITE                     0x0A
+#define ZM_OID_CHAN							0x30
+#define ZM_OID_SYNTH						0x32
+#define ZM_OID_TALLY						0x81
+#define ZM_OID_TALLY_APD					0x82
+
+#define ZM_OID_DKTX_STATUS                  0x92
+#define ZM_OID_FLASH_CHKSUM                 0xD0
+#define ZM_OID_FLASH_READ                   0xD1
+#define ZM_OID_FLASH_PROGRAM                0xD2
+#define ZM_OID_FW_DL_INIT                   0xD3
+
+/* Driver to Firmware OID */
+#define ZM_CMD_ECHO         0x80
+#define ZM_CMD_TALLY        0x81
+#define ZM_CMD_TALLY_APD    0x82
+#define ZM_CMD_CONFIG       0x83
+#define ZM_CMD_RREG         0x00
+#define ZM_CMD_WREG         0x01
+#define ZM_CMD_RMEM         0x02
+#define ZM_CMD_WMEM         0x03
+#define ZM_CMD_BITAND       0x04
+#define ZM_CMD_BITOR        0x05
+#define ZM_CMD_EKEY         0x28
+#define ZM_CMD_DKEY         0x29
+#define ZM_CMD_FREQUENCY    0x30
+#define ZM_CMD_RF_INIT      0x31
+#define ZM_CMD_SYNTH        0x32
+#define ZM_CMD_FREQ_STRAT   0x33
+#define ZM_CMD_RESET        0x90
+#define ZM_CMD_DKRESET      0x91
+#define ZM_CMD_DKTX_STATUS  0x92
+#define ZM_CMD_FDC          0xA0
+#define ZM_CMD_WREEPROM     0xB0
+#define ZM_CMD_WFLASH       0xB0
+#define ZM_CMD_FLASH_ERASE  0xB1
+#define ZM_CMD_FLASH_PROG   0xB2
+#define ZM_CMD_FLASH_CHKSUM 0xB3
+#define ZM_CMD_FLASH_READ   0xB4
+#define ZM_CMD_FW_DL_INIT   0xB5
+#define ZM_CMD_MEM_WREEPROM 0xBB
+
+
+/* duplicate filter table column */
+#define ZM_FILTER_TABLE_COL                 2 /* 2^n */
+/* duplicate filter table Row */
+#define ZM_FILTER_TABLE_ROW                 8 /* 2^n */
+
+/* duplicate filter table structure */
+struct zsRxFilter
+{
+    u16_t addr[3];
+    u16_t seq;
+    u8_t up;
+};
+
+struct zsWlanDev
+{
+    /* AP global variables */
+    struct zsWlanDevAp ap;
+    /* STA global variables */
+    struct zsWlanDevSta sta;
+    /* save wrapper setting */
+    struct zsWrapperSetting ws;
+    /* features determined by wrapper (vendor) */
+    struct zsWrapperFeatureCtrl wfc;
+    /* Traffic Monitor tally */
+    struct zsTrafTally trafTally;
+    /* Communication tally */
+    struct zsCommTally commTally;
+    /* Duplicate frame filter table */
+    struct zsRxFilter rxFilterTbl[ZM_FILTER_TABLE_COL][ZM_FILTER_TABLE_ROW];
+    /* Regulatory table */
+    struct zsRegulationTable  regulationTable;
+
+    /* */
+    struct zsWlanDevFreqControl freqCtrl;
+
+    enum devState state;
+
+    u8_t  halState;
+    u8_t  wlanMode;         /* AP/INFRASTRUCTURE/IBSS/PSEUDO */
+    u16_t macAddr[3];       /* MAC address */
+    u16_t beaconInterval;   /* beacon Interval */
+    u8_t dtim;              /* DTIM period */
+    u8_t            CurrentDtimCount;
+    u8_t  preambleType;
+    u8_t  preambleTypeInUsed;
+    u8_t  maxTxPower2;	    /* 2.4 GHz Max Tx power (Unit: 0.5 dBm) */
+    u8_t  maxTxPower5;	    /* 5 GHz Max Tx power (Unit: 0.5 dBm) */
+    u8_t  connectMode;
+    u32_t supportMode;
+
+    u8_t bRate;             /* 11b Support Rate bit map */
+    u8_t bRateBasic;        /* 11b Basic Rate bit map */
+    u8_t gRate;             /* 11g Support Rate bit map */
+    u8_t gRateBasic;        /* 11g Basic Rate bit map */
+    /* channel index point to the item in regulation table */
+    u8_t channelIndex;
+
+    /* channel management */
+    u8_t    BandWidth40;
+    u8_t    ExtOffset;      //1 above, 3 below, 0 not present
+    u16_t   frequency;      /* operation frequency */
+
+    u8_t erpElement;        /* ERP information element data */
+
+    u8_t disableSelfCts;    /* set to 1 to disable Self-CTS */
+    u8_t bgMode;
+
+	/* private test flag */
+    u32_t enableProtectionMode;   /* force enable/disable self cts */
+	u32_t checksumTest;     /* OTUS checksum test 1=>zero checksum 0=>normal */
+	u32_t rxPacketDump;     /* rx packet dump */
+
+    u8_t enableAggregation; /* force enable/disable A-MSPU */
+    u8_t enableWDS;         /* force enable/disable WDS testing */
+	u8_t enableTxPathMode;  /* OTUS special testing mode 1=>diable, 0=>enable: ZM_SYSTEM_TEST_MODE */
+    u8_t enableHALDbgInfo;  /*  */
+
+    u32_t forceTxTPC;       /* force tx packet send TPC */
+
+    u16_t seq[4];
+    u16_t mmseq;
+
+    /* driver core time tick */
+    u32_t tick;
+    u16_t tickIbssSendBeacon;
+    u16_t tickIbssReceiveBeacon;
+
+    /* RTS threshold */
+    u16_t rtsThreshold;
+
+    /* fragmentation threshold,  256 <= value <= 2346, 0=disabled */
+    u16_t fragThreshold;
+
+    /* Tx Rate */
+    u16_t txMCS;
+    u16_t txMT;
+    u32_t CurrentTxRateKbps; //CWYang(+)
+    /* Rx Rate */
+    u32_t CurrentRxRateKbps; //Janet(+)
+    u8_t CurrentRxRateUpdated;
+    u8_t modulationType;
+    u8_t rxInfo;
+    u16_t rateField;
+
+    /* timer related objects */
+    struct zsTimerList  timerList;
+    u8_t                bTimerReady;
+
+    /* for defragmentation */
+    struct zsDefragList defragTable;
+
+    /* Data struct for Interface Dependent Layer */
+    //struct zsIdlStruct idlStruct;
+
+    /* Signal Strength/Quality Related Parameters */
+    u8_t SignalStrength; //CWYang(+)
+    u8_t SignalQuality;  //CWYang(+)
+
+
+
+    /* QoS */
+    zbuf_t* vtxq[4][ZM_VTXQ_SIZE];
+    u16_t vtxqHead[4];
+    u16_t vtxqTail[4];
+    u16_t qosDropIpFrag[4];
+
+    /* Management Tx queue */
+    zbuf_t* vmmq[ZM_VMMQ_SIZE];
+    u16_t vmmqHead;
+    u16_t vmmqTail;
+
+    u8_t                vtxqPushing;
+
+    /*
+     * add by honda
+     * 1. Aggregate queues
+     * 2. STA's associated information and queue number
+     * 3. rx aggregation re-ordering queue
+     */
+    struct aggQueue    *aggQPool[ZM_AGG_POOL_SIZE];
+    u8_t                aggInitiated;
+    u8_t                addbaComplete;
+    u8_t                addbaCount;
+    u8_t                aggState;
+    u8_t                destLock;
+    struct aggSta       aggSta[ZM_MAX_STA_SUPPORT];
+    struct agg_tid_rx  *tid_rx[ZM_AGG_POOL_SIZE];
+    struct aggTally     agg_tal;
+    struct destQ        destQ;
+    struct baw_enabler *baw_enabler;
+    struct ieee80211_cwm    cwm;
+    u16_t               reorder;
+    u16_t               seq_debug;
+    /* rate control */
+    u32_t txMPDU[ZM_RATE_TABLE_SIZE];
+    u32_t txFail[ZM_RATE_TABLE_SIZE];
+    u32_t PER[ZM_RATE_TABLE_SIZE];
+    u16_t probeCount;
+    u16_t probeSuccessCount;
+    u16_t probeInterval;
+    u16_t success_probing;
+    /*
+     * end of add by honda
+     */
+
+    /* airopeek sniffer mode for upper sw */
+    u32_t               swSniffer;   /* window: airoPeek */
+    u32_t               XLinkMode;
+
+    /* MDK mode */
+    /* init by 0=>normal driver 1=>MDK driver */
+    u32_t               modeMDKEnable;
+
+    u32_t               heartBeatNotification;
+
+    /* pointer for HAL Plus private memory */
+    void* hpPrivate;
+
+    /* for WPA setting */
+    //u8_t                    wpaSupport[ZM_MAX_AP_SUPPORT];
+    //u8_t                    wpaLen[ZM_MAX_AP_SUPPORT];
+    //u8_t                    wpaIe[ZM_MAX_AP_SUPPORT][ZM_MAX_IE_SIZE];
+
+    struct zsLedStruct      ledStruct;
+
+    /* ani flag */
+    u8_t aniEnable;
+    u16_t txq_threshold;
+
+	//Skip Mic Error Check
+	u8_t	TKIP_Group_KeyChanging;
+
+	u8_t    dynamicSIFSEnable;
+
+	u8_t    queueFlushed;
+
+    u16_t (*zfcbAuthNotify)(zdev_t* dev, u16_t* macAddr);
+    u16_t (*zfcbAsocNotify)(zdev_t* dev, u16_t* macAddr, u8_t* body, u16_t bodySize, u16_t port);
+    u16_t (*zfcbDisAsocNotify)(zdev_t* dev, u8_t* macAddr, u16_t port);
+    u16_t (*zfcbApConnectNotify)(zdev_t* dev, u8_t* macAddr, u16_t port);
+    void (*zfcbConnectNotify)(zdev_t* dev, u16_t status, u16_t* bssid);
+    void (*zfcbScanNotify)(zdev_t* dev, struct zsScanResult* result);
+    void (*zfcbMicFailureNotify)(zdev_t* dev, u16_t* addr, u16_t status);
+    void (*zfcbApMicFailureNotify)(zdev_t* dev, u8_t* addr, zbuf_t* buf);
+    void (*zfcbIbssPartnerNotify)(zdev_t* dev, u16_t status, struct zsPartnerNotifyEvent *event);
+    void (*zfcbMacAddressNotify)(zdev_t* dev, u8_t* addr);
+    void (*zfcbSendCompleteIndication)(zdev_t* dev, zbuf_t* buf);
+    void (*zfcbRecvEth)(zdev_t* dev, zbuf_t* buf, u16_t port);
+    void (*zfcbRecv80211)(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* addInfo);
+    void (*zfcbRestoreBufData)(zdev_t* dev, zbuf_t* buf);
+#ifdef ZM_ENABLE_CENC
+    u16_t (*zfcbCencAsocNotify)(zdev_t* dev, u16_t* macAddr, u8_t* body,
+            u16_t bodySize, u16_t port);
+#endif //ZM_ENABLE_CENC
+    u8_t (*zfcbClassifyTxPacket)(zdev_t* dev, zbuf_t* buf);
+    void (*zfcbHwWatchDogNotify)(zdev_t* dev);
+};
+
+
+struct zsWlanKey
+{
+    u8_t key;
+};
+
+
+/* These macros are defined here for backward compatibility */
+/* Please leave them alone */
+/* For Tx packet allocated in upper layer layer */
+#define zmw_tx_buf_readb(dev, buf, offset) zmw_buf_readb(dev, buf, offset)
+#define zmw_tx_buf_readh(dev, buf, offset) zmw_buf_readh(dev, buf, offset)
+#define zmw_tx_buf_writeb(dev, buf, offset, value) zmw_buf_writeb(dev, buf, offset, value)
+#define zmw_tx_buf_writeh(dev, buf, offset, value) zmw_buf_writeh(dev, buf, offset, value)
+
+/* For Rx packet allocated in driver */
+#define zmw_rx_buf_readb(dev, buf, offset) zmw_buf_readb(dev, buf, offset)
+#define zmw_rx_buf_readh(dev, buf, offset) zmw_buf_readh(dev, buf, offset)
+#define zmw_rx_buf_writeb(dev, buf, offset, value) zmw_buf_writeb(dev, buf, offset, value)
+#define zmw_rx_buf_writeh(dev, buf, offset, value) zmw_buf_writeh(dev, buf, offset, value)
+
+#endif /* #ifndef _STRUCT_H */
diff --git a/drivers/staging/otus/80211core/wlan.h b/drivers/staging/otus/80211core/wlan.h
new file mode 100644
index 00000000000..26c18b837cf
--- /dev/null
+++ b/drivers/staging/otus/80211core/wlan.h
@@ -0,0 +1,595 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : wlan_defs.h                                           */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains WLAN definitions.                          */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+
+#ifndef _WLAN_H
+#define _WLAN_H
+
+
+#define ZM_EXTERNAL_ALLOC_BUF               0
+#define ZM_INTERNAL_ALLOC_BUF               1
+
+#define ZM_SIZE_OF_CTRL_SET                 8
+#define ZM_SIZE_OF_IV                       4
+#define ZM_SIZE_OF_EXT_IV                   4
+#define ZM_SIZE_OF_MIC                      8
+#define ZM_SIZE_OF_CCX_MIC                  8
+#define ZM_SIZE_OF_WLAN_DATA_HEADER         24
+#define ZM_SIZE_OF_QOS_CTRL                 2
+
+/* Header definition */
+#define ZM_SIZE_OF_WLAN_WDS_HEADER          32
+#define ZM_SIZE_OF_SNAP_HEADER              8
+
+#define ZM_WLAN_HEADER_A1_OFFSET            4
+#define ZM_WLAN_HEADER_A2_OFFSET            10
+#define ZM_WLAN_HEADER_A3_OFFSET            16
+#define ZM_WLAN_HEADER_A4_OFFSET            24
+#define ZM_WLAN_HEADER_IV_OFFSET            24
+#define ZM_SIZE_OF_WLAN_DATA_HEADER         24
+
+/* Port definition */
+#define ZM_PORT_DISABLED                    0
+#define ZM_PORT_ENABLED                     1
+
+/* Frame Type */
+#define ZM_WLAN_MANAGEMENT_FRAME            0x0
+#define ZM_WLAN_CONTROL_FRAME               0x4
+#define ZM_WLAN_DATA_FRAME                  0x8
+
+/* Frame Subtype */
+#define ZM_WLAN_FRAME_TYPE_ASOCREQ          0x00
+#define ZM_WLAN_FRAME_TYPE_ASOCRSP          0x10
+#define ZM_WLAN_FRAME_TYPE_REASOCREQ        0x20
+#define ZM_WLAN_FRAME_TYPE_REASOCRSP        0x30
+#define ZM_WLAN_FRAME_TYPE_PROBEREQ         0x40
+#define ZM_WLAN_FRAME_TYPE_PROBERSP         0x50
+/* 0x60, 0x70 => Reserved */
+#define ZM_WLAN_FRAME_TYPE_BEACON           0x80
+#define ZM_WLAN_FRAME_TYPE_ATIM             0x90
+#define ZM_WLAN_FRAME_TYPE_DISASOC          0xA0
+#define ZM_WLAN_FRAME_TYPE_AUTH             0xB0
+#define ZM_WLAN_FRAME_TYPE_DEAUTH           0xC0
+#define ZM_WLAN_FRAME_TYPE_ACTION			0xD0
+
+/* Frame type and subtype */
+#define ZM_WLAN_FRAME_TYPE_NULL             0x48
+#define ZM_WLAN_FRAME_TYPE_BAR              0x84
+#define ZM_WLAN_FRAME_TYPE_BA               0x94
+#define ZM_WLAN_FRAME_TYPE_PSPOLL           0xA4
+#define ZM_WLAN_FRAME_TYPE_RTS              0xB4
+#define ZM_WLAN_FRAME_TYPE_CTS              0xC4
+#define ZM_WLAN_FRAME_TYPE_QOS_NULL         0xC8
+
+/* action frame */
+#define ZM_WLAN_SPECTRUM_MANAGEMENT_ACTION_FRAME    0
+#define ZM_WLAN_QOS_ACTION_FRAME            1
+#define ZM_WLAN_DLS_ACTION_FRAME            2
+#define ZM_WLAN_BLOCK_ACK_ACTION_FRAME      3
+/* block ack action frame*/
+#define ZM_WLAN_ADDBA_REQUEST_FRAME         0
+#define ZM_WLAN_ADDBA_RESPONSE_FRAME        1
+#define ZM_WLAN_DELBA_FRAME                 2
+
+/* Element ID */
+#define ZM_WLAN_EID_SSID                    0
+#define ZM_WLAN_EID_SUPPORT_RATE            1
+#define ZM_WLAN_EID_FH                      2
+#define ZM_WLAN_EID_DS                      3
+#define ZM_WLAN_EID_CFS                     4
+#define ZM_WLAN_EID_TIM                     5
+#define ZM_WLAN_EID_IBSS                    6
+#define ZM_WLAN_EID_COUNTRY                 7
+/* reserved 8-15 */
+#define ZM_WLAN_EID_CHALLENGE               16
+/* reserved 17-31 */
+#define ZM_WLAN_EID_POWER_CONSTRAINT        32
+#define ZM_WLAN_EID_POWER_CAPABILITY        33
+#define ZM_WLAN_EID_TPC_REQUEST             34
+#define ZM_WLAN_EID_TPC_REPORT              35
+#define ZM_WLAN_EID_SUPPORTED_CHANNELS      36
+#define ZM_WLAN_EID_CHANNEL_SWITCH_ANNOUNCE 37
+#define ZM_WLAN_EID_MEASUREMENT_REQUEST     38
+#define ZM_WLAN_EID_MEASUREMENT_REPORT      39
+#define ZM_WLAN_EID_QUIET                   40
+#define ZM_WLAN_EID_IBSS_DFS                41
+#define ZM_WLAN_EID_ERP                     42
+#define ZM_WLAN_PREN2_EID_HTCAPABILITY      45
+#define ZM_WLAN_EID_RSN_IE                  48
+#define ZM_WLAN_EID_EXTENDED_RATE           50
+#define ZM_WLAN_EID_HT_CAPABILITY           51
+#define ZM_WLAN_EID_EXTENDED_HT_CAPABILITY  52
+#define ZM_WLAN_EID_NEW_EXT_CHANNEL_OFFSET  53
+#define ZM_WLAN_PREN2_EID_HTINFORMATION     61
+#define ZM_WLAN_PREN2_EID_SECONDCHOFFSET    62
+#ifdef ZM_ENABLE_CENC
+#define ZM_WLAN_EID_CENC_IE                 68
+#endif //ZM_ENABLE_CENC
+#define ZM_WLAN_EID_VENDOR_PRIVATE          221     /* Vendor private space; must demux OUI */
+#define ZM_WLAN_EID_WPA_IE                  221
+#define ZM_WLAN_EID_WPS_IE                  221
+#define ZM_WLAN_EID_WIFI_IE                 221
+
+/* ERP information element */
+#define ZM_WLAN_NON_ERP_PRESENT_BIT         0x1
+#define ZM_WLAN_USE_PROTECTION_BIT          0x2
+#define ZM_WLAN_BARKER_PREAMBLE_MODE_BIT    0x4
+
+/* Channel frequency, in MHz */
+#define ZM_CH_G_1                          2412
+#define ZM_CH_G_2                          2417
+#define ZM_CH_G_3                          2422
+#define ZM_CH_G_4                          2427
+#define ZM_CH_G_5                          2432
+#define ZM_CH_G_6                          2437
+#define ZM_CH_G_7                          2442
+#define ZM_CH_G_8                          2447
+#define ZM_CH_G_9                          2452
+#define ZM_CH_G_10                         2457
+#define ZM_CH_G_11                         2462
+#define ZM_CH_G_12                         2467
+#define ZM_CH_G_13                         2472
+#define ZM_CH_G_14                         2484
+#define ZM_CH_A_184                        4920
+#define ZM_CH_A_188                        4940
+#define ZM_CH_A_192                        4960
+#define ZM_CH_A_196                        4980
+#define ZM_CH_A_8                          5040
+#define ZM_CH_A_12                         5060
+#define ZM_CH_A_16                         5080
+#define ZM_CH_A_36                         5180
+#define ZM_CH_A_40                         5200
+#define ZM_CH_A_44                         5220
+#define ZM_CH_A_48                         5240
+#define ZM_CH_A_52                         5260
+#define ZM_CH_A_56                         5280
+#define ZM_CH_A_60                         5300
+#define ZM_CH_A_64                         5320
+#define ZM_CH_A_100                        5500
+#define ZM_CH_A_104                        5520
+#define ZM_CH_A_108                        5540
+#define ZM_CH_A_112                        5560
+#define ZM_CH_A_116                        5580
+#define ZM_CH_A_120                        5600
+#define ZM_CH_A_124                        5620
+#define ZM_CH_A_128                        5640
+#define ZM_CH_A_132                        5660
+#define ZM_CH_A_136                        5680
+#define ZM_CH_A_140                        5700
+#define ZM_CH_A_149                        5745
+#define ZM_CH_A_153                        5765
+#define ZM_CH_A_157                        5785
+#define ZM_CH_A_161                        5805
+#define ZM_CH_A_165                        5825
+
+
+/* AP : STA table => STA Type */
+#define ZM_11B_STA                          0x0
+#define ZM_11G_STA                          0x2
+#define ZM_11N_STA                          0x4
+
+/* AP : timeout */
+#define ZM_MS_PER_TICK                      10
+#define ZM_TICK_PER_SECOND                  (1000/ZM_MS_PER_TICK)
+#define ZM_TICK_PER_MINUTE                  (60*1000/ZM_MS_PER_TICK)
+#define ZM_PREAUTH_TIMEOUT_MS               1000 /* 1 sec */
+#define ZM_AUTH_TIMEOUT_MS                  1000 /* 1 sec */
+
+/* Error code */
+#define ZM_SUCCESS                          0
+#define ZM_ERR_TX_PORT_DISABLED             1
+#define ZM_ERR_BUFFER_DMA_ADDR              2
+#define ZM_ERR_FREE_TXD_EXHAUSTED           3
+#define ZM_ERR_TX_BUFFER_UNAVAILABLE        4
+#define ZM_ERR_BCMC_PS_BUFFER_UNAVAILABLE   5
+#define ZM_ERR_UNI_PS_BUFFER_UNAVAILABLE    6
+#define ZM_ERR_EXCEED_PRIORITY_THRESHOLD    7
+#define ZM_ERR_VMMQ_FULL                    8
+#define ZM_ERR_FLUSH_PS_QUEUE               9
+#define ZM_ERR_CMD_INT_MISSED               15 /* Polling cmd int timeout*/
+/* Rx */
+#define ZM_ERR_RX_FRAME_TYPE                20
+#define ZM_ERR_MIN_RX_ENCRYPT_FRAME_LENGTH  21
+#define ZM_ERR_MIN_RX_FRAME_LENGTH          22
+#define ZM_ERR_MAX_RX_FRAME_LENGTH          23
+#define ZM_ERR_RX_DUPLICATE                 24
+#define ZM_ERR_RX_SRC_ADDR_IS_OWN_MAC       25
+#define ZM_ERR_MIN_RX_PROTOCOL_VERSION      26
+#define ZM_ERR_WPA_GK_NOT_INSTALLED         27
+#define ZM_ERR_STA_NOT_ASSOCIATED           28
+#define ZM_ERR_DATA_BEFORE_CONNECTED        29
+#define ZM_ERR_DATA_NOT_ENCRYPTED           30
+#define ZM_ERR_DATA_BSSID_NOT_MATCHED       31
+#define ZM_ERR_RX_BAR_FRAME                 32
+#define ZM_ERR_OUT_OF_ORDER_NULL_DATA       33
+
+/* ZFI */
+#define ZM_ERR_INVALID_TX_RATE              40
+#define ZM_ERR_WDS_PORT_ID                  41
+
+/* QUEUE */
+#define ZM_ERR_QUEUE_FULL                   50
+#define ZM_ERR_STA_UAPSD_QUEUE_FULL         51
+#define ZM_ERR_AP_UAPSD_QUEUE_FULL          52
+
+/* Maximum Rx frame length */
+#if ZM_LARGEPAYLOAD_TEST == 1
+#define ZM_WLAN_MAX_RX_SIZE                 16384
+#else
+#define ZM_WLAN_MAX_RX_SIZE                 8192
+#endif
+
+/* PCI DMA test error code */
+#define ZM_ERR_INTERRUPT_MISSED             100
+#define ZM_ERR_OWN_BIT_NOT_CLEARED          101
+#define ZM_ERR_RX_SEQ_NUMBER                102
+#define ZM_ERR_RX_LENGTH                    103
+#define ZM_ERR_RX_DATA                      104
+#define ZM_ERR_RX_DESCRIPTOR_NUM            105
+/* Common register test error code */
+#define ZM_ERR_REGISTER_ACCESS              110 /* Register R/W test fail*/
+#define ZM_ERR_CLEAR_INTERRUPT_FLAG         111
+#define ZM_ERR_COMMAND_RESPONSE             112
+#define ZM_ERR_INTERRUPT_GENERATE           113
+#define ZM_ERR_INTERRUPT_ACK                114
+#define ZM_ERR_SCRATCH_ACCESS               115
+#define ZM_ERR_INTERRUPT_MASK_ACCESS        116
+#define ZM_ERR_SHARE_MEMORY_PCI_ACCESS      117
+#define ZM_ERR_SHARE_MEMORY_FW_ACCESS       118
+#define ZM_ERR_SHARE_MEMORY_DISABLE         119
+#define ZM_ERR_SHARE_MEMORY_TEST_RESPONSE   120
+
+/* Firmware Download error code */
+#define ZM_ERR_FIRMWARE_DOWNLOAD_TIMEOUT    150
+#define ZM_ERR_FIRMWARE_DOWNLOAD_INT_FLAG   151
+#define ZM_ERR_FIRMWARE_READY_TIMEOUT       152
+#define ZM_ERR_FIRMWARE_WRONG_TYPE          153
+
+/* Debug */
+#define ZM_LV_0     0//Debug level 0, Disable debug message
+#define ZM_LV_1     1//Debug level 1, Show minimum information
+#define ZM_LV_2     2//Debug level 2, Show medium message
+#define ZM_LV_3     3//Debug level 3, Show all
+
+#define ZM_SCANMSG_LEV  ZM_LV_1
+#define ZM_TXMSG_LEV    ZM_LV_0//ZM_LV_0
+#define ZM_RXMSG_LEV    ZM_LV_0
+#define ZM_MMMSG_LEV    ZM_LV_0
+#define ZM_DESMSG_LEV   ZM_LV_0//ZM_LV_0
+#define ZM_BUFMSG_LEV   ZM_LV_0//ZM_LV_1
+#define ZM_INITMSG_LEV  ZM_LV_0
+
+#define zm_msg0_scan(lv, msg) if (ZM_SCANMSG_LEV >= lv) \
+        {zm_debug_msg0(msg);}
+#define zm_msg1_scan(lv, msg, val) if (ZM_SCANMSG_LEV >= lv) \
+        {zm_debug_msg1(msg, val);}
+#define zm_msg2_scan(lv, msg, val) if (ZM_SCANMSG_LEV >= lv) \
+        {zm_debug_msg2(msg, val);}
+
+#define zm_msg0_tx(lv, msg) if (ZM_TXMSG_LEV >= lv) \
+        {zm_debug_msg0(msg);}
+#define zm_msg1_tx(lv, msg, val) if (ZM_TXMSG_LEV >= lv) \
+        {zm_debug_msg1(msg, val);}
+#define zm_msg2_tx(lv, msg, val) if (ZM_TXMSG_LEV >= lv) \
+        {zm_debug_msg2(msg, val);}
+
+#define zm_msg0_rx(lv, msg) if (ZM_RXMSG_LEV >= lv) \
+        {zm_debug_msg0(msg);}
+#define zm_msg1_rx(lv, msg, val) if (ZM_RXMSG_LEV >= lv) \
+        {zm_debug_msg1(msg, val);}
+#define zm_msg2_rx(lv, msg, val) if (ZM_RXMSG_LEV >= lv) \
+        {zm_debug_msg2(msg, val);}
+
+#define zm_msg0_mm(lv, msg) if (ZM_MMMSG_LEV >= lv) \
+        {zm_debug_msg0(msg);}
+#define zm_msg1_mm(lv, msg, val) if (ZM_MMMSG_LEV >= lv) \
+        {zm_debug_msg1(msg, val);}
+#define zm_msg2_mm(lv, msg, val) if (ZM_MMMSG_LEV >= lv) \
+        {zm_debug_msg2(msg, val);}
+
+#define zm_msg0_des(lv, msg) if (ZM_DESMSG_LEV >= lv) \
+        {zm_debug_msg0(msg);}
+#define zm_msg1_des(lv, msg, val) if (ZM_DESMSG_LEV >= lv) \
+        {zm_debug_msg1(msg, val);}
+#define zm_msg2_des(lv, msg, val) if (ZM_DESMSG_LEV >= lv) \
+        {zm_debug_msg2(msg, val);}
+
+#define zm_msg0_buf(lv, msg) if (ZM_BUFMSG_LEV >= lv) \
+        {zm_debug_msg0(msg);}
+#define zm_msg1_buf(lv, msg, val) if (ZM_BUFMSG_LEV >= lv) \
+        {zm_debug_msg1(msg, val);}
+#define zm_msg2_buf(lv, msg, val) if (ZM_BUFMSG_LEV >= lv) \
+        {zm_debug_msg2(msg, val);}
+
+#define zm_msg0_init(lv, msg) if (ZM_INITMSG_LEV >= lv) \
+        {zm_debug_msg0(msg);}
+#define zm_msg1_init(lv, msg, val) if (ZM_INITMSG_LEV >= lv) \
+        {zm_debug_msg1(msg, val);}
+#define zm_msg2_init(lv, msg, val) if (ZM_INITMSG_LEV >= lv) \
+        {zm_debug_msg2(msg, val);}
+
+#define ZM_MAX_AP_SUPPORT                   2  /* Must <= 8 */
+#define ZM_MAX_WDS_SUPPORT                  6  /* Must <= 6 */
+#define ZM_MAX_STA_SUPPORT                  16 /* Must <= 64 */
+
+/* STA table state */
+#define ZM_STATE_AUTH                       1
+#define ZM_STATE_PREAUTH                    2
+#define ZM_STATE_ASOC                       3
+
+/* Rate set */
+#define ZM_RATE_SET_CCK                     0
+#define ZM_RATE_SET_OFDM                    1
+
+/* HT PT */
+#define ZM_PREAMBLE_TYPE_MIXED_MODE         0
+#define ZM_PREAMBLE_TYPE_GREEN_FIELD        1
+
+/* HT bandwidth */
+#define ZM_BANDWIDTH_20MHZ                  0
+#define ZM_BANDWIDTH_40MHZ                  1
+
+/* MIC status */
+#define ZM_MIC_SUCCESS                      0
+#define ZM_MIC_FAILURE                      1
+
+/* ICV status */
+#define ZM_ICV_SUCCESS                      0
+#define ZM_ICV_FAILURE                      1
+
+/* definition check */
+#if (ZM_MAX_AP_SUPPORT > 8)
+definition error, ZM_MAX_AP_SUPPORT > 8
+#endif
+#if (ZM_MAX_AP_SUPPORT > 64)
+definition error, ZM_MAX_STA_SUPPORT > 64
+#endif
+
+/*  Transmission Rate information */
+
+/* WLAN frame format */
+#define ZM_PLCP_HEADER_SIZE          5
+#define ZM_ETHERNET_ADDRESS_LENGTH   6
+#define ZM_TIMESTAMP_OFFSET          0
+#define ZM_BEACON_INTERVAL_OFFSET    8
+#define ZM_CAPABILITY_OFFSET        10
+
+/* Reason Code */
+/* An unsolicited notification management frame of       */
+/* type Disassocation or Deauthentication was generated. */
+#ifdef ZM_REASON_CODE
+#define ZM_WLAN_REASON_CODE_UNSPECIFIED   1
+#define ZM_WLAN_FRAME_DISASOC_DEAUTH_REASON_CODE  24
+#endif
+
+struct zsWlanManagementFrameHeader
+{
+    //u8_t      plcpHdr[ZM_PLCP_HEADER_SIZE];
+    u8_t        frameCtrl[2];
+    u8_t        duration[2];
+    u8_t        da[ZM_ETHERNET_ADDRESS_LENGTH];
+    u8_t        sa[ZM_ETHERNET_ADDRESS_LENGTH];
+    u8_t        bssid[ZM_ETHERNET_ADDRESS_LENGTH];
+    u8_t        seqCtrl[2];
+    u8_t        body[1];
+};
+
+struct zsWlanProbeRspFrameHeader
+{
+    //u8_t      plcpHdr[ZM_PLCP_HEADER_SIZE];
+    u8_t        frameCtrl[2];
+    u8_t        duration[2];
+    u8_t        da[ZM_ETHERNET_ADDRESS_LENGTH];
+    u8_t        sa[ZM_ETHERNET_ADDRESS_LENGTH];
+    u8_t        bssid[ZM_ETHERNET_ADDRESS_LENGTH];
+    u8_t        seqCtrl[2];
+    u8_t        timeStamp[8];
+    u8_t        beaconInterval[2];
+    u8_t        capability[2];
+    u8_t        ssid[ZM_MAX_SSID_LENGTH + 2];   // EID(1) + Length(1) + SSID(32)
+} ;
+
+#define zsWlanBeaconFrameHeader zsWlanProbeRspFrameHeader
+
+struct zsWlanAuthFrameHeader
+{
+    //u8_t      plcpHdr[ZM_PLCP_HEADER_SIZE];
+    u8_t        frameCtrl[2];
+    u8_t        duration[2];
+    u8_t        address1[ZM_ETHERNET_ADDRESS_LENGTH];
+    u8_t        address2[ZM_ETHERNET_ADDRESS_LENGTH];
+    u8_t        address3[ZM_ETHERNET_ADDRESS_LENGTH];
+    u8_t        seqCtrl[2];
+    u16_t       algo;
+    u16_t       seq;
+    u16_t       status;
+    u8_t        challengeText[255]; // the first 2 bytes are information ID, length
+};
+
+struct zsWlanAssoFrameHeader
+{
+    //u8_t      plcpHdr[PLCP_HEADER_SIZE];
+    u8_t        frameCtrl[2];
+    u8_t        duration[2];
+    u8_t        address1[ZM_ETHERNET_ADDRESS_LENGTH];
+    u8_t        address2[ZM_ETHERNET_ADDRESS_LENGTH];
+    u8_t        address3[ZM_ETHERNET_ADDRESS_LENGTH];
+    u8_t        seqCtrl[2];
+    u8_t        capability[2];
+    u16_t       status;
+    u16_t       aid;
+    //u8_t        supportedRates[10];
+};
+
+struct zsFrag
+{
+    zbuf_t* buf[16];
+    u16_t bufType[16];
+    u16_t seq[16];
+    u8_t flag[16];
+
+};
+
+//================================
+// Hardware related definitions
+//================================
+#define ZM_MAC_REG_BASE                         0x1c3000
+
+#define ZM_MAC_REG_ATIM_WINDOW                  (ZM_MAC_REG_BASE + 0x51C)
+#define ZM_MAC_REG_BCN_PERIOD                   (ZM_MAC_REG_BASE + 0x520)
+#define ZM_MAC_REG_PRETBTT                      (ZM_MAC_REG_BASE + 0x524)
+
+#define ZM_MAC_REG_MAC_ADDR_L                   (ZM_MAC_REG_BASE + 0x610)
+#define ZM_MAC_REG_MAC_ADDR_H                   (ZM_MAC_REG_BASE + 0x614)
+
+#define ZM_MAC_REG_GROUP_HASH_TBL_L             (ZM_MAC_REG_BASE + 0x624)
+#define ZM_MAC_REG_GROUP_HASH_TBL_H             (ZM_MAC_REG_BASE + 0x628)
+
+#define ZM_MAC_REG_BASIC_RATE                   (ZM_MAC_REG_BASE + 0x630)
+#define ZM_MAC_REG_MANDATORY_RATE               (ZM_MAC_REG_BASE + 0x634)
+#define ZM_MAC_REG_RTS_CTS_RATE                 (ZM_MAC_REG_BASE + 0x638)
+#define ZM_MAC_REG_BACKOFF_PROTECT              (ZM_MAC_REG_BASE + 0x63c)
+#define ZM_MAC_REG_RX_THRESHOLD                 (ZM_MAC_REG_BASE + 0x640)
+#define ZM_MAC_REG_RX_PE_DELAY                  (ZM_MAC_REG_BASE + 0x64C)
+
+#define ZM_MAC_REG_DYNAMIC_SIFS_ACK             (ZM_MAC_REG_BASE + 0x658)
+#define ZM_MAC_REG_SNIFFER                      (ZM_MAC_REG_BASE + 0x674)
+#define ZM_MAC_REG_TX_UNDERRUN		            (ZM_MAC_REG_BASE + 0x688)
+#define ZM_MAC_REG_RX_TOTAL			            (ZM_MAC_REG_BASE + 0x6A0)
+#define ZM_MAC_REG_RX_CRC32			            (ZM_MAC_REG_BASE + 0x6A4)
+#define ZM_MAC_REG_RX_CRC16			            (ZM_MAC_REG_BASE + 0x6A8)
+#define ZM_MAC_REG_RX_ERR_UNI		            (ZM_MAC_REG_BASE + 0x6AC)
+#define ZM_MAC_REG_RX_OVERRUN		            (ZM_MAC_REG_BASE + 0x6B0)
+#define ZM_MAC_REG_RX_ERR_MUL		            (ZM_MAC_REG_BASE + 0x6BC)
+#define ZM_MAC_REG_TX_RETRY			            (ZM_MAC_REG_BASE + 0x6CC)
+#define ZM_MAC_REG_TX_TOTAL			            (ZM_MAC_REG_BASE + 0x6F4)
+
+
+#define ZM_MAC_REG_ACK_EXTENSION                (ZM_MAC_REG_BASE + 0x690)
+#define ZM_MAC_REG_EIFS_AND_SIFS                (ZM_MAC_REG_BASE + 0x698)
+
+#define ZM_MAC_REG_SLOT_TIME                    (ZM_MAC_REG_BASE + 0x6F0)
+
+#define ZM_MAC_REG_ROLL_CALL_TBL_L              (ZM_MAC_REG_BASE + 0x704)
+#define ZM_MAC_REG_ROLL_CALL_TBL_H              (ZM_MAC_REG_BASE + 0x708)
+
+#define ZM_MAC_REG_AC0_CW                       (ZM_MAC_REG_BASE + 0xB00)
+#define ZM_MAC_REG_AC1_CW                       (ZM_MAC_REG_BASE + 0xB04)
+#define ZM_MAC_REG_AC2_CW                       (ZM_MAC_REG_BASE + 0xB08)
+#define ZM_MAC_REG_AC3_CW                       (ZM_MAC_REG_BASE + 0xB0C)
+#define ZM_MAC_REG_AC4_CW                       (ZM_MAC_REG_BASE + 0xB10)
+#define ZM_MAC_REG_AC1_AC0_AIFS                 (ZM_MAC_REG_BASE + 0xB14)
+#define ZM_MAC_REG_AC3_AC2_AIFS                 (ZM_MAC_REG_BASE + 0xB18)
+
+#define ZM_MAC_REG_RETRY_MAX                    (ZM_MAC_REG_BASE + 0xB28)
+
+#define ZM_MAC_REG_TXOP_NOT_ENOUGH_INDICATION   (ZM_MAC_REG_BASE + 0xB30)
+
+#define ZM_MAC_REG_AC1_AC0_TXOP                 (ZM_MAC_REG_BASE + 0xB44)
+#define ZM_MAC_REG_AC3_AC2_TXOP                 (ZM_MAC_REG_BASE + 0xB48)
+
+#define ZM_MAC_REG_ACK_TABLE                    (ZM_MAC_REG_BASE + 0xC00)
+
+#define ZM_MAC_REG_BCN_ADDR                     (ZM_MAC_REG_BASE + 0xD84)
+#define ZM_MAC_REG_BCN_LENGTH                   (ZM_MAC_REG_BASE + 0xD88)
+
+#define ZM_MAC_REG_BCN_PLCP                     (ZM_MAC_REG_BASE + 0xD90)
+#define ZM_MAC_REG_BCN_CTRL                     (ZM_MAC_REG_BASE + 0xD94)
+
+#define ZM_MAC_REG_BCN_HT1                      (ZM_MAC_REG_BASE + 0xDA0)
+#define ZM_MAC_REG_BCN_HT2                      (ZM_MAC_REG_BASE + 0xDA4)
+
+
+#define ZM_RX_STATUS_IS_MIC_FAIL(rxStatus) rxStatus->Tail.Data.ErrorIndication & ZM_BIT_6
+
+//================================
+//================================
+
+#ifdef ZM_ENABLE_NATIVE_WIFI
+#define ZM_80211_FRAME_HEADER_LEN           24
+#define ZM_80211_FRAME_TYPE_OFFSET          30    // ZM_80211_FRAME_HEADER_LEN + SNAP
+#define ZM_80211_FRAME_IP_OFFSET            32    // ZM_80211_FRAME_HEADER_LEN + SNAP + TYPE
+#else
+#define ZM_80211_FRAME_HEADER_LEN           14
+#define ZM_80211_FRAME_TYPE_OFFSET          12    // ZM_80211_FRAME_HEADER_LEN + SNAP
+#define ZM_80211_FRAME_IP_OFFSET            14    // ZM_80211_FRAME_HEADER_LEN + SNAP + TYPE
+#endif
+
+#define ZM_BSS_INFO_VALID_BIT      0x01
+#define ZM_BSS_INFO_UPDATED_BIT    0x02
+
+
+
+
+
+#define ZM_ERROR_INDICATION_RX_TIMEOUT      0x01
+#define ZM_ERROR_INDICATION_OVERRUN         0x02
+#define ZM_ERROR_INDICATION_DECRYPT_ERROR   0x04
+#define ZM_ERROR_INDICATION_CRC32_ERROR     0x08
+#define ZM_ERROR_INDICATION_ADDR_NOT_MATCH  0x10
+#define ZM_ERROR_INDICATION_CRC16_ERROR     0x20
+#define ZM_ERROR_INDICATION_MIC_ERROR       0x40
+
+#define ZM_RXMAC_STATUS_MOD_TYPE_CCK        0x00
+#define ZM_RXMAC_STATUS_MOD_TYPE_OFDM       0x01
+#define ZM_RXMAC_STATUS_MOD_TYPE_HT_OFDM    0x02
+#define ZM_RXMAC_STATUS_MOD_TYPE_DL_OFDM    0x03
+#define ZM_RXMAC_STATUS_TOTAL_ERROR         0x80
+
+
+
+
+
+#define ZM_MAX_LED_NUMBER       2
+
+#define ZM_LED_DISABLE_MODE     0x0
+#define ZM_LED_LINK_MODE        0x1
+#define ZM_LED_LINK_TR_MODE     0x2
+#define ZM_LED_TR_ON_MODE       0x3
+#define ZM_LED_TR_OFF_MODE      0x4
+
+#define ZM_LED_CTRL_FLAG_ALPHA      0x1
+
+struct zsLedStruct
+{
+    u32_t   counter;
+    u32_t   counter100ms;
+    u16_t   ledLinkState;
+    u16_t   ledMode[ZM_MAX_LED_NUMBER];
+    u32_t   txTraffic;
+    u32_t   rxTraffic;
+    u8_t    LEDCtrlType;
+    u8_t    LEDCtrlFlag;         // Control Flag for vendors
+    u8_t    LEDCtrlFlagFromReg;  // Control Flag for vendors in registry
+};
+
+
+//HAL+ capability bits definition
+#define ZM_HP_CAP_11N                   0x1
+#define ZM_HP_CAP_11N_ONE_TX_STREAM     0x2
+#define ZM_HP_CAP_2G                    0x4
+#define ZM_HP_CAP_5G                    0x8
+
+#endif /* #ifndef _WLAN_H */
diff --git a/drivers/staging/otus/Kconfig b/drivers/staging/otus/Kconfig
new file mode 100644
index 00000000000..d549d08fd49
--- /dev/null
+++ b/drivers/staging/otus/Kconfig
@@ -0,0 +1,32 @@
+config OTUS
+	tristate "Atheros OTUS 802.11n USB wireless support"
+	depends on USB && WLAN_80211 && MAC80211
+	default N
+	---help---
+	  Enable support for Atheros 802.11n USB hardware:
+	    * UB81 - 2x2 2.4 GHz
+	    * UB82 - 2x2 2.4 GHz and 5 GHz
+	    * UB83 - 1x2 2.4 GHz
+
+	  This includes the following devices currently on the market:
+	  Dlink DWA-160A1, Netgear WNDA3100 and WN111v2, TP-Link
+	  TL-WN821N, and AVM FRITZ!WLAN N USB Stick.
+
+	  This driver requires its own supplicant driver for
+	  wpa_supplicant 0.4.8. For your convenience you can find the
+	  tarball here:
+
+	  http://www.kernel.org/pub/linux/kernel/people/mcgrof/otus/wpa_supplicant-0.4.8_otus.tar.bz2
+
+	  Before compiling wpa_supplicant, ensure your .config has at
+	  least the following:
+		CONFIG_WIRELESS_EXTENSION=y
+		CONFIG_EAP_WSC=y
+		CONFIG_WSC_IE=y
+		CONFIG_DRIVER_WEXT=y
+		CONFIG_DRIVER_OTUS=y
+
+	  After a successful compile, you can use the Atheros device as
+	  shown in the example:
+	  $ wpa_supplicant -Dotus -i <atheros device from ifconfig> -c /path/to/wpa_supplicant.conf -d
+
diff --git a/drivers/staging/otus/Makefile b/drivers/staging/otus/Makefile
new file mode 100644
index 00000000000..debcc5cbf87
--- /dev/null
+++ b/drivers/staging/otus/Makefile
@@ -0,0 +1,67 @@
+obj-$(CONFIG_OTUS)	+= arusb_lnx.o
+
+EXTRA_CFLAGS += -DAMAC
+EXTRA_CFLAGS += -DGCCK
+EXTRA_CFLAGS += -DOFDM
+EXTRA_CFLAGS += -DTXQ_IN_ISR
+EXTRA_CFLAGS += -DWLAN_HOSTIF=0 #0:USB, 1:PCI
+
+#Test Mode
+EXTRA_CFLAGS += -DZM_USB_STREAM_MODE=1
+EXTRA_CFLAGS += -DZM_USB_TX_STREAM_MODE=0
+EXTRA_CFLAGS += -DZM_PCI_DMA_TEST=0
+EXTRA_CFLAGS += -DZM_LARGEPAYLOAD_TEST=0
+EXTRA_CFLAGS += -DZM_FW_LOOP_BACK=0
+EXTRA_CFLAGS += -DZM_LINUX_TPC=1
+#EXTRA_CFLAGS += -DZM_DONT_COPY_RX_BUFFER
+
+EXTRA_CFLAGS += -DZM_HOSTAPD_SUPPORT
+#EXTRA_CFLAGS += -DfTX_GAIN_OFDM=0
+#EXTRA_CFLAGS += -DZM_CONFIG_BIG_ENDIAN -DBIG_ENDIAN
+EXTRA_CFLAGS += -DZM_HALPLUS_LOCK
+EXTRA_CFLAGS += -DZM_OTUS_LINUX_PHASE_2
+
+arusb_lnx-objs := \
+	usbdrv.o \
+	zdusb.o \
+	ioctl.o \
+	wrap_buf.o \
+	wrap_mem.o \
+	wrap_ev.o \
+	wrap_usb.o \
+	wrap_pkt.o \
+	wrap_dbg.o \
+	wrap_mis.o \
+	wrap_sec.o \
+	wwrap.o \
+	80211core/ccmd.o \
+	80211core/chb.o \
+	80211core/cinit.o \
+	80211core/cmm.o \
+	80211core/cmmap.o \
+	80211core/cmmsta.o \
+	80211core/cfunc.o \
+	80211core/coid.o \
+	80211core/ctkip.o \
+	80211core/ctxrx.o \
+	80211core/cic.o \
+	80211core/cpsmgr.o \
+	80211core/cscanmgr.o \
+	80211core/ratectrl.o \
+	80211core/ledmgr.o \
+	80211core/amsdu.o \
+	80211core/cwm.o \
+	80211core/cagg.o \
+	80211core/queue.o \
+	80211core/freqctrl.o \
+	80211core/cwep.o \
+	hal/hprw.o \
+	hal/hpmain.o \
+	hal/hpusb.o \
+	hal/hpreg.o \
+	hal/hpfwuinit.o \
+	hal/hpfwbu.o \
+	hal/hpfw2.o \
+	hal/hpDKfwu.o \
+	hal/hpfwspiu.o \
+	hal/hpani.o
diff --git a/drivers/staging/otus/TODO b/drivers/staging/otus/TODO
new file mode 100644
index 00000000000..e912293ed1e
--- /dev/null
+++ b/drivers/staging/otus/TODO
@@ -0,0 +1,9 @@
+TODO:
+	- checkpatch.pl cleanups
+	- sparse cleanups
+	- port to in-kernel 80211 stack
+	- proper network developer maintainer
+
+Please send any patches to Greg Kroah-Hartman <greg@kroah.com> and
+Luis Rodriguez <Luis.Rodriguez@Atheros.com> and the
+otus-devel@lists.madwifi-project.org mailing list.
diff --git a/drivers/staging/otus/apdbg.c b/drivers/staging/otus/apdbg.c
new file mode 100644
index 00000000000..d3e2f622419
--- /dev/null
+++ b/drivers/staging/otus/apdbg.c
@@ -0,0 +1,457 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : apdbg.c                                               */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      Debug tools                                                     */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <string.h>
+#include <errno.h>
+#include <ctype.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <sys/ioctl.h>
+#include <net/if.h>
+#include <netinet/in.h>
+
+#include <linux/sockios.h>
+
+#define ZM_IOCTL_REG_READ           0x01
+#define ZM_IOCTL_REG_WRITE          0x02
+#define ZM_IOCTL_MEM_DUMP           0x03
+#define ZM_IOCTL_REG_DUMP           0x05
+#define ZM_IOCTL_TXD_DUMP           0x06
+#define ZM_IOCTL_RXD_DUMP           0x07
+#define ZM_IOCTL_MEM_READ           0x0B
+#define ZM_IOCTL_MEM_WRITE          0x0C
+#define ZM_IOCTL_DMA_TEST           0x10
+#define ZM_IOCTL_REG_TEST           0x11
+#define ZM_IOCTL_TEST               0x80
+#define ZM_IOCTL_TALLY              0x81 //CWYang(+)
+#define ZM_IOCTL_RTS                0xA0
+#define ZM_IOCTL_MIX_MODE           0xA1
+#define ZM_IOCTL_FRAG               0xA2
+#define ZM_IOCTL_SCAN               0xA3
+#define ZM_IOCTL_KEY                0xA4
+#define ZM_IOCTL_RATE               0xA5
+#define ZM_IOCTL_ENCRYPTION_MODE    0xA6
+#define ZM_IOCTL_GET_TXCNT          0xA7
+#define ZM_IOCTL_GET_DEAGG_CNT      0xA8
+#define ZM_IOCTL_DURATION_MODE      0xA9
+#define ZM_IOCTL_SET_AES_KEY        0xAA
+#define ZM_IOCTL_SET_AES_MODE       0xAB
+#define ZM_IOCTL_SIGNAL_STRENGTH    0xAC //CWYang(+)
+#define ZM_IOCTL_SIGNAL_QUALITY     0xAD //CWYang(+)
+#define ZM_IOCTL_SET_PIBSS_MODE     0xAE
+#define	ZDAPIOCTL                   SIOCDEVPRIVATE
+
+struct zdap_ioctl {
+	unsigned short cmd;                /* Command to run */
+	unsigned int   addr;                /* Length of the data buffer */
+	unsigned int   value;               /* Pointer to the data buffer */
+	unsigned char data[0x100];
+};
+
+/* Declaration of macro and function for handling WEP Keys */
+
+#if 0
+
+#define SKIP_ELEM { \
+    while(isxdigit(*p)) \
+        p++; \
+}
+
+#define SKIP_DELIMETER { \
+    if(*p == ':' || *p == ' ') \
+        p++; \
+}
+
+#endif
+
+char hex(char);
+unsigned char asctohex(char *str);
+
+char *prgname;
+
+int set_ioctl(int sock, struct ifreq *req)
+{
+    if (ioctl(sock, ZDAPIOCTL, req) < 0) {
+        fprintf(stderr, "%s: ioctl(SIOCGIFMAP): %s\n",
+                prgname, strerror(errno));
+        return -1;
+    }
+
+    return 0;
+}
+
+
+int read_reg(int sock, struct ifreq *req)
+{
+    struct zdap_ioctl *zdreq = 0;
+
+    if (!set_ioctl(sock, req))
+        return -1;
+
+    //zdreq = (struct zdap_ioctl *)req->ifr_data;
+    //printf( "reg = %4x, value = %4x\n", zdreq->addr, zdreq->value);
+
+    return 0;
+}
+
+
+int read_mem(int sock, struct ifreq *req)
+{
+    struct zdap_ioctl *zdreq = 0;
+    int i;
+
+    if (!set_ioctl(sock, req))
+        return -1;
+
+    /*zdreq = (struct zdap_ioctl *)req->ifr_data;
+    printf( "dump mem from %x, length = %x\n", zdreq->addr, zdreq->value);
+
+    for (i=0; i<zdreq->value; i++) {
+        printf("%02x", zdreq->data[i]);
+        printf(" ");
+
+        if ((i>0) && ((i+1)%16 == 0))
+            printf("\n");
+    }*/
+
+    return 0;
+}
+
+
+int main(int argc, char **argv)
+{
+    int sock;
+    int addr, value;
+    struct ifreq req;
+    char *action = NULL;
+    struct zdap_ioctl zdreq;
+
+    prgname = argv[0];
+
+    if (argc < 3) {
+        fprintf(stderr,"%s: usage is \"%s <ifname> <operation> [<address>] [<value>]\"\n",
+                prgname, prgname);
+        fprintf(stderr,"valid operation: read, write, mem, reg,\n");
+        fprintf(stderr,"               : txd, rxd, rmem, wmem\n");
+        fprintf(stderr,"               : dmat, regt, test\n");
+
+        fprintf(stderr,"    scan, Channel Scan\n");
+        fprintf(stderr,"    rts <decimal>, Set RTS Threshold\n");
+        fprintf(stderr,"    frag <decimal>, Set Fragment Threshold\n");
+        fprintf(stderr,"    rate <0-28>, 0:AUTO, 1-4:CCK, 5-12:OFDM, 13-28:HT\n");
+        fprintf(stderr,"    TBD mix <0 or 1>, Set 1 to enable mixed mode\n");
+        fprintf(stderr,"    enc, <0-3>, 0=>OPEN, 1=>WEP64, 2=>WEP128, 3=>WEP256\n");
+        fprintf(stderr,"    skey <key>, Set WEP key\n");
+        fprintf(stderr,"    txcnt, Get TxQ Cnt\n");
+        fprintf(stderr,"    dagcnt, Get Deaggregate Cnt\n");
+        fprintf(stderr,"    durmode <mode>, Set Duration Mode 0=>HW, 1=>SW\n");
+        fprintf(stderr,"    aeskey <user> <key>\n");
+        fprintf(stderr,"    aesmode <mode>\n");
+        fprintf(stderr,"    wlanmode <0,1> 0:Station mode, 1:PIBSS mode\n");
+        fprintf(stderr,"    tal <0,1>, Get Current Tally Info, 0=>read, 1=>read and reset\n");
+
+        exit(1);
+    }
+
+    strcpy(req.ifr_name, argv[1]);
+    zdreq.addr = 0;
+    zdreq.value = 0;
+
+    /* a silly raw socket just for ioctl()ling it */
+    sock = socket(AF_INET, SOCK_RAW, IPPROTO_RAW);
+    if (sock < 0) {
+        fprintf(stderr, "%s: socket(): %s\n", argv[0], strerror(errno));
+        exit(1);
+    }
+
+    if (argc >= 4)
+    {
+        sscanf(argv[3], "%x", &addr);
+    }
+
+    if (argc >= 5)
+    {
+        sscanf(argv[4], "%x", &value);
+    }
+
+    zdreq.addr = addr;
+    zdreq.value = value;
+
+    if (!strcmp(argv[2], "read"))
+    {
+        zdreq.cmd = ZM_IOCTL_REG_READ;
+    }
+    else if (!strcmp(argv[2], "mem"))
+    {
+        zdreq.cmd = ZM_IOCTL_MEM_DUMP;
+    }
+    else if (!strcmp(argv[2], "write"))
+    {
+        zdreq.cmd = ZM_IOCTL_REG_WRITE;
+    }
+    else if (!strcmp(argv[2], "reg"))
+    {
+        zdreq.cmd = ZM_IOCTL_REG_DUMP;
+    }
+    else if (!strcmp(argv[2], "txd"))
+    {
+        zdreq.cmd = ZM_IOCTL_TXD_DUMP;
+    }
+    else if (!strcmp(argv[2], "rxd"))
+    {
+        zdreq.cmd = ZM_IOCTL_RXD_DUMP;
+    }
+    else if (!strcmp(argv[2], "rmem"))
+    {
+        zdreq.cmd = ZM_IOCTL_MEM_READ;
+    }
+    else if (!strcmp(argv[2], "wmem"))
+    {
+        zdreq.cmd = ZM_IOCTL_MEM_WRITE;
+    }
+    else if (!strcmp(argv[2], "dmat"))
+    {
+        zdreq.cmd = ZM_IOCTL_DMA_TEST;
+    }
+    else if (!strcmp(argv[2], "regt"))
+    {
+        zdreq.cmd = ZM_IOCTL_REG_TEST;
+    }
+    else if (!strcmp(argv[2], "test"))
+    {
+        zdreq.cmd = ZM_IOCTL_TEST;
+    }
+    else if (!strcmp(argv[2], "tal"))
+    {
+        sscanf(argv[3], "%d", &addr);
+        zdreq.addr = addr;
+        zdreq.cmd = ZM_IOCTL_TALLY;
+    }
+    else if (!strcmp(argv[2], "rts"))
+    {
+        sscanf(argv[3], "%d", &addr);
+        zdreq.addr = addr;
+        zdreq.cmd = ZM_IOCTL_RTS;
+    }
+    else if (!strcmp(argv[2], "mix"))
+    {
+        zdreq.cmd = ZM_IOCTL_MIX_MODE;
+    }
+    else if (!strcmp(argv[2], "frag"))
+    {
+        sscanf(argv[3], "%d", &addr);
+        zdreq.addr = addr;
+        zdreq.cmd = ZM_IOCTL_FRAG;
+    }
+    else if (!strcmp(argv[2], "scan"))
+    {
+        zdreq.cmd = ZM_IOCTL_SCAN;
+    }
+    else if (!strcmp(argv[2], "skey"))
+    {
+        zdreq.cmd = ZM_IOCTL_KEY;
+
+        if (argc >= 4)
+        {
+            unsigned char temp[29];
+            int i;
+            int keyLen;
+            int encType;
+
+            keyLen = strlen(argv[3]);
+
+            if (keyLen == 10)
+            {
+                sscanf(argv[3], "%02x%02x%02x%02x%02x", &temp[0], &temp[1],
+                        &temp[2], &temp[3], &temp[4]);
+            }
+            else if (keyLen == 26)
+            {
+                sscanf(argv[3], "%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x",
+                        &temp[0], &temp[1], &temp[2], &temp[3], &temp[4],
+                        &temp[5], &temp[6], &temp[7], &temp[8], &temp[9],
+                         &temp[10], &temp[11], &temp[12]);
+            }
+            else if (keyLen == 58)
+            {
+                sscanf(argv[3], "%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x",
+                        &temp[0], &temp[1], &temp[2], &temp[3], &temp[4],
+                        &temp[5], &temp[6], &temp[7], &temp[8], &temp[9],
+                        &temp[10], &temp[11], &temp[12], &temp[13], &temp[14],
+                        &temp[15], &temp[16], &temp[17], &temp[18], &temp[19],
+                        &temp[20], &temp[21], &temp[22], &temp[23], &temp[24],
+                        &temp[25], &temp[26], &temp[27], &temp[28]);
+            }
+            else
+            {
+                fprintf(stderr, "Invalid key length\n");
+                exit(1);
+            }
+            zdreq.addr = keyLen/2;
+
+            for(i=0; i<zdreq.addr; i++)
+            {
+                zdreq.data[i] = temp[i];
+            }
+        }
+        else
+        {
+            printf("Error : Key required!\n");
+        }
+    }
+    else if (!strcmp(argv[2], "rate"))
+    {
+        sscanf(argv[3], "%d", &addr);
+
+        if (addr > 28)
+        {
+            fprintf(stderr, "Invalid rate, range:0~28\n");
+            exit(1);
+        }
+        zdreq.addr = addr;
+        zdreq.cmd = ZM_IOCTL_RATE;
+    }
+    else if (!strcmp(argv[2], "enc"))
+    {
+        sscanf(argv[3], "%d", &addr);
+
+        if (addr > 3)
+        {
+            fprintf(stderr, "Invalid encryption mode, range:0~3\n");
+            exit(1);
+        }
+
+        if (addr == 2)
+        {
+            addr = 5;
+        }
+        else if (addr == 3)
+        {
+            addr = 6;
+        }
+
+        zdreq.addr = addr;
+        zdreq.cmd = ZM_IOCTL_ENCRYPTION_MODE;
+    }
+    else if (!strcmp(argv[2], "txcnt"))
+    {
+        zdreq.cmd = ZM_IOCTL_GET_TXCNT;
+    }
+    else if (!strcmp(argv[2], "dagcnt"))
+    {
+        sscanf(argv[3], "%d", &addr);
+
+        if (addr != 0 && addr != 1)
+        {
+            fprintf(stderr, "The value should be 0 or 1\n");
+            exit(0);
+        }
+
+        zdreq.addr = addr;
+        zdreq.cmd = ZM_IOCTL_GET_DEAGG_CNT;
+    }
+    else if (!strcmp(argv[2], "durmode"))
+    {
+        sscanf(argv[3], "%d", &addr);
+
+        if (addr != 0 && addr != 1)
+        {
+            fprintf(stderr, "The Duration mode should be 0 or 1\n");
+            exit(0);
+        }
+
+        zdreq.addr = addr;
+        zdreq.cmd = ZM_IOCTL_DURATION_MODE;
+    }
+    else if (!strcmp(argv[2], "aeskey"))
+    {
+        unsigned char temp[16];
+        int i;
+
+        sscanf(argv[3], "%d", &addr);
+
+        sscanf(argv[4], "%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x", &temp[0], &temp[1], &temp[2], &temp[3], &temp[4], &temp[5], &temp[6], &temp[7], &temp[8], &temp[9], &temp[10], &temp[11], &temp[12], &temp[13], &temp[14], &temp[15]);
+
+        for(i = 0; i < 16; i++)
+        {
+            zdreq.data[i] = temp[i];
+        }
+
+        zdreq.addr = addr;
+        zdreq.cmd = ZM_IOCTL_SET_AES_KEY;
+    }
+    else if (!strcmp(argv[2], "aesmode"))
+    {
+        sscanf(argv[3], "%d", &addr);
+
+        zdreq.addr = addr;
+        zdreq.cmd = ZM_IOCTL_SET_AES_MODE;
+    }
+    else if (!strcmp(argv[2], "wlanmode"))
+    {
+        sscanf(argv[3], "%d", &addr);
+
+        zdreq.addr = addr;
+        zdreq.cmd = ZM_IOCTL_SET_PIBSS_MODE;
+    }
+    else
+    {
+	    fprintf(stderr, "error action\n");
+        exit(1);
+    }
+
+    req.ifr_data = (char *)&zdreq;
+    set_ioctl(sock, &req);
+
+fail:
+    exit(0);
+}
+
+unsigned char asctohex(char *str)
+{
+    unsigned char value;
+
+    value = hex(*str) & 0x0f;
+    value = value << 4;
+    str++;
+    value |= hex(*str) & 0x0f;
+
+    return value;
+}
+
+char hex(char v)
+{
+    if(isdigit(v))
+        return v - '0';
+    else if(isxdigit(v))
+        return (tolower(v) - 'a' + 10);
+    else
+        return 0;
+}
+
diff --git a/drivers/staging/otus/athr_common.h b/drivers/staging/otus/athr_common.h
new file mode 100644
index 00000000000..620f78a41d5
--- /dev/null
+++ b/drivers/staging/otus/athr_common.h
@@ -0,0 +1,141 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*  Module Name : athr_common.h                                         */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      WPA related function and data structure definitions.            */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      Platform dependent.                                             */
+/*                                                                      */
+/************************************************************************/
+
+#ifndef _ATHR_COMMON_H
+#define _ATHR_COMMON_H
+
+#define ZD_IOCTL_WPA			(SIOCDEVPRIVATE + 1)
+#define ZD_IOCTL_PARAM			(SIOCDEVPRIVATE + 2)
+#define ZD_IOCTL_GETWPAIE		(SIOCDEVPRIVATE + 3)
+#define ZD_PARAM_ROAMING		0x0001
+#define ZD_PARAM_PRIVACY		0x0002
+#define ZD_PARAM_WPA			0x0003
+#define ZD_PARAM_COUNTERMEASURES	0x0004
+#define ZD_PARAM_DROPUNENCRYPTED	0x0005
+#define ZD_PARAM_AUTH_ALGS		0x0006
+
+#define ZD_CMD_SET_ENCRYPT_KEY		0x0001
+#define ZD_CMD_SET_MLME			0x0002
+#define ZD_CMD_SCAN_REQ			0x0003
+#define ZD_CMD_SET_GENERIC_ELEMENT	0x0004
+#define ZD_CMD_GET_TSC			0x0005
+
+#define ZD_FLAG_SET_TX_KEY              0x0001
+
+#define ZD_GENERIC_ELEMENT_HDR_LEN \
+((int) (&((struct athr_wlan_param *) 0)->u.generic_elem.data))
+
+#define ZD_CRYPT_ALG_NAME_LEN		16
+#define ZD_MAX_KEY_SIZE			32
+#define ZD_MAX_GENERIC_SIZE		64
+
+#define IEEE80211_ADDR_LEN		6
+#define IEEE80211_MAX_IE_SIZE		256
+
+#ifdef ZM_ENALBE_WAPI
+#define ZM_CMD_WAPI_SETWAPI             0x0001
+#define ZM_CMD_WAPI_GETWAPI             0x0002
+#define ZM_CMD_WAPI_SETKEY              0x0003
+#define ZM_CMD_WAPI_GETKEY              0x0004
+#define ZM_CMD_WAPI_REKEY               0x0005
+
+#define ZM_WAPI_WAI_REQUEST             0x00f1
+#define ZM_WAPI_UNICAST_REKEY           0x00f2
+#define ZM_WAPI_STA_AGING               0x00f3
+#define ZM_WAPI_MULTI_REKEY             0x00f4
+
+#define ZM_WAPI_KEY_SIZE                32
+#define ZM_WAPI_IV_LEN                  16
+#endif //ZM_ENALBE_WAPI
+/* structure definition */
+
+struct athr_wlan_param {
+	u32 cmd;
+	u8 sta_addr[ETH_ALEN];
+	union {
+		struct {
+			u8 alg[ZD_CRYPT_ALG_NAME_LEN];
+			u32 flags;
+			u32 err;
+			u8 idx;
+			u8 seq[8]; /* sequence counter (set: RX, get: TX) */
+			u16 key_len;
+			u8 key[ZD_MAX_KEY_SIZE];
+		} crypt;
+		struct {
+			u32 flags_and;
+			u32 flags_or;
+		} set_flags_sta;
+		struct {
+			u8 len;
+			u8 data[ZD_MAX_GENERIC_SIZE];
+		} generic_elem;
+		struct {
+#define MLME_STA_DEAUTH 0
+#define MLME_STA_DISASSOC 1
+			u16 cmd;
+			u16 reason_code;
+		} mlme;
+		struct {
+			u8 ssid_len;
+			u8 ssid[32];
+		} scan_req;
+	} u;
+};
+
+struct ieee80211req_wpaie {
+	u8 wpa_macaddr[IEEE80211_ADDR_LEN];
+	u8 wpa_ie[IEEE80211_MAX_IE_SIZE];
+};
+
+#ifdef ZM_ENALBE_WAPI
+struct athr_wapi_param {
+	u16 cmd;
+	u16 len;
+
+	union {
+		struct {
+			u8 sta_addr[ETH_ALEN];
+			u8 reserved;
+			u8 keyid;
+			u8 key[ZM_WAPI_KEY_SIZE];
+		} crypt;
+		struct {
+                        u8 wapi_policy;
+		} info;
+	} u;
+};
+
+struct athr_wapi_sta_info
+{
+	u16	msg_type;
+	u16	datalen;
+    	u8	sta_mac[ETH_ALEN];
+	u8	reserve_data[2];
+    	u8	gsn[ZM_WAPI_IV_LEN];
+	u8	wie[256];
+};
+#endif //ZM_ENALBE_WAPI
+#endif
diff --git a/drivers/staging/otus/hal/hpDKfwu.c b/drivers/staging/otus/hal/hpDKfwu.c
new file mode 100644
index 00000000000..144c62dc8da
--- /dev/null
+++ b/drivers/staging/otus/hal/hpDKfwu.c
@@ -0,0 +1,832 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include "../80211core/cprecomp.h"
+
+const u32_t zcDKFwImage[] = {
+0x0009000B, 0x4F222FE6, 0xDE3E7FFC, 0xE114D73E,
+0x1E13D43E, 0x1E4C470B, 0x0009B017, 0x956EE600,
+0xC84060E2, 0x2F028F03, 0x8FF93652, 0xD4387601,
+0x4E0BDE38, 0xD4380009, 0x00094E0B, 0x4E0BD437,
+0x7F040009, 0xA0364F26, 0x4F226EF6, 0x410BD134,
+0xD4340009, 0x0009440B, 0x450BD533, 0xD7330009,
+0xD233E1FF, 0x2712611D, 0xD4325029, 0xE1FFCB01,
+0x1209E501, 0x12112212, 0xD52F2452, 0xD22F9740,
+0xE7002572, 0xD42FD12E, 0x2270D62F, 0x2172E201,
+0x26202420, 0xE4FFD62D, 0xE6002641, 0xE104D52C,
+0x6063666D, 0x626D7601, 0x32124000, 0x05458FF8,
+0x000B4F26, 0xEAC80009, 0xDB266AAC, 0xDD27DC26,
+0xD828DE27, 0x4C0BE901, 0x4D0B0009, 0x4E0B0009,
+0x60B20009, 0x89078801, 0x6242D423, 0x890332A6,
+0x6050D522, 0x8BEE8801, 0x2B92D41F, 0x26686642,
+0x480B89E9, 0xD51D0009, 0xAFE4E200, 0x27102520,
+0x00000FA0, 0x001C001C, 0x00200ED4, 0x0000B38E,
+0x00202F90, 0x00201356, 0x00202F9C, 0x00202FB4,
+0x00201314, 0x00201412, 0x00200EF8, 0x001C3510,
+0x001C3624, 0x001E212C, 0x00202F00, 0x00202A9C,
+0x00202F08, 0x00202F14, 0x00202F20, 0x00202F22,
+0x00202F26, 0x001C1028, 0x00201220, 0x0020294C,
+0x00201D10, 0x00201EC8, 0x00203220, 0x00202F24,
+0x2FB62F96, 0x2FD62FC6, 0x4F222FE6, 0xDE947F80,
+0x61E0E024, 0x0F14D493, 0x710161E3, 0xD7926210,
+0x470BE028, 0xD5910F24, 0x0009450B, 0x6D032008,
+0x1F0B8F11, 0xD48FDC8E, 0xDD8F67C0, 0x657C4D0B,
+0xDD8FD18E, 0x6B9C6910, 0x420862B3, 0x32B84208,
+0x3D2C4208, 0xE0281FDB, 0xE58004FC, 0x604C66E2,
+0x3050655C, 0x2D628F13, 0x01FCE024, 0x641CE500,
+0x625DDE84, 0x8B013243, 0x0009A39E, 0x6753655D,
+0x607037EC, 0x39DC6953, 0xAFF27501, 0x20088094,
+0xE0248B13, 0xE50001FC, 0xA009DE7A, 0x655D641C,
+0x32EC6253, 0x6C536B22, 0x3CDC67B2, 0x75041C71,
+0x3243625D, 0xA37F8BF3, 0x88012D10, 0xE0248B16,
+0xE40001FC, 0x671C2D40, 0x624DDE6E, 0x8B013273,
+0x0009A372, 0x6CE3644D, 0x7C046943, 0x39EC6B43,
+0x65923BCC, 0x74086DB2, 0x25D2AFEF, 0x8B198804,
+0x01FCE024, 0x2D70E700, 0x1FD86D1C, 0x627DDE61,
+0x8B0132D3, 0x0009A358, 0x6B73677D, 0x3BEC61E3,
+0x710464B2, 0x3C1C6C73, 0x694265C2, 0x29597708,
+0x2492AFED, 0x8B188805, 0x01FCE024, 0x2D40E400,
+0xDE54671C, 0x3273624D, 0xA33D8B01, 0x644D0009,
+0x6BE36D43, 0x65D23DEC, 0x61437B04, 0x6C1231BC,
+0x74086952, 0xAFED29CB, 0x88312592, 0xDE4A8B20,
+0x65E6DB4A, 0x61E6DC4A, 0x67E2D94A, 0x62E27E04,
+0x1FEC7EE8, 0x7E0464E2, 0x6EE21FED, 0x5BFD2BE0,
+0x60B27B04, 0xC9011FBE, 0x6BB22C00, 0x29B04B09,
+0xDC412F26, 0x66134C0B, 0xE2007F04, 0x2D20A30C,
+0x8B218830, 0xD939DE38, 0xE06465E6, 0x720462E3,
+0x672666E2, 0x6E23DC36, 0x62227EE8, 0x6BE261E6,
+0x29B01FEF, 0x7E040F16, 0xC90160E2, 0x6EE22C00,
+0x4E09DC30, 0x2F262CE0, 0xD130E068, 0x04FE410B,
+0xE2007F04, 0x2D20A2E8, 0x8B058833, 0x4E0BDE2C,
+0xE1000009, 0x2D10A2E0, 0x89018828, 0x0009A106,
+0xE143DE20, 0xE04062E1, 0x3217622D, 0x0FE68F04,
+0x6023E240, 0x262106FE, 0x8B013217, 0x0009A0EF,
+0x02FEE040, 0x8521E401, 0x8B013046, 0x0009A0E7,
+0xE501E040, 0x2D5007FE, 0x6471B2C7, 0x09FEE040,
+0x6291E143, 0x652DE068, 0x8D6B3517, 0xE6400F56,
+0x8B273563, 0xE048E600, 0xE11A0F65, 0x72C0A031,
+0x00117800, 0x00202FB8, 0x00201356, 0x00202480,
+0x00202F1A, 0x00202FBC, 0x002013A2, 0x00202F19,
+0x00202B40, 0x00117804, 0x00117810, 0x00202F15,
+0x00202F16, 0x00202F17, 0x00200B84, 0x00200BD8,
+0x00200BD4, 0x41216153, 0x41214121, 0x41214121,
+0x45214521, 0x60534521, 0x6603C903, 0x0F65E048,
+0xE0077118, 0xE0442209, 0x641D0F25, 0x65F3E04C,
+0x0F46B314, 0x04FDE048, 0x0BFDE044, 0x61BD674D,
+0x41084708, 0x0F16E050, 0xD2936073, 0x420B09FE,
+0x6C07E00F, 0x607329C9, 0xE0400F96, 0x65F30EFE,
+0x6D0D85E2, 0x01FEE050, 0x60D3420B, 0x6073290B,
+0xE04C0F96, 0x04FEB2D9, 0x06FEE040, 0x6261E068,
+0x0F56652D, 0x3563E640, 0xE000894E, 0x602381F8,
+0x4008C903, 0x6B034000, 0xE0546103, 0xE0580FB6,
+0xECFFDD7D, 0x6CCC0FF6, 0x0FD6E06C, 0x4D0B60C3,
+0x42216253, 0x42214221, 0x64234221, 0x324C4200,
+0xE05C6E07, 0x45214200, 0xE0400FE6, 0x0BFE4521,
+0xC9036053, 0x30FC4008, 0x6D037B06, 0x85F81F05,
+0x6C2D1FB7, 0x1FC66E03, 0x0FC6E060, 0x05FEE058,
+0x64C3B2B4, 0x33FCE354, 0x563262D2, 0x22696132,
+0x67B42D22, 0x490B5936, 0x220B607C, 0x05FEE058,
+0x64C32D22, 0x7E01B289, 0xE70662ED, 0x8FE33273,
+0xE0407C01, 0x626106FE, 0x06FEE040, 0x85614200,
+0x302C760C, 0x6103701B, 0x64F3E500, 0x7501E704,
+0x6B5D6966, 0x24923B73, 0x74048FF9, 0xB26C65F3,
+0xE040641D, 0xB20506FE, 0xA1DD6461, 0xD44F0009,
+0xE201D74F, 0x2D20470B, 0x0009A1D6, 0x8B078829,
+0xEC00DE4C, 0x61E22DC0, 0x641DB1D7, 0x0009A1CC,
+0x622CE281, 0x8B013020, 0x0009A118, 0x06FCE028,
+0xE682626C, 0x3260666C, 0xA0EE8B01, 0xE6830009,
+0x3260666C, 0xA0DC8B01, 0xE6900009, 0x3260666C,
+0xA0D08B01, 0xE6910009, 0x3260666C, 0xA0B98B01,
+0xE6B00009, 0x3260666C, 0xA07F8B01, 0xE6BB0009,
+0x3260666C, 0xE6928920, 0x3260666C, 0xE4008B14,
+0xEB04D531, 0x52516652, 0x8B073620, 0x624D7401,
+0x8FF732B7, 0xE6007508, 0x52FBA002, 0xE60152FB,
+0xE6041261, 0x2260A188, 0xD229D428, 0xD4296542,
+0x0009420B, 0x0009A180, 0xE100E670, 0x601336FC,
+0xE0248162, 0x0BFCD21F, 0x6BBC6722, 0x26727BFC,
+0xEB0416B2, 0x06FEE078, 0x3263621D, 0xA16B8B01,
+0xDE1D0009, 0x31EC611D, 0xD41C6E12, 0x410BD114,
+0xE0700009, 0x450BD51A, 0xD41A04FE, 0x420BD210,
+0xD2170009, 0x64E3420B, 0xD60DD417, 0x0009460B,
+0x05FEE070, 0x01FDE074, 0x611DE600, 0x6253351C,
+0x326C666D, 0x22E07601, 0x32B3626D, 0x4E198FF7,
+0xE0747104, 0x0F15AFCE, 0x002029F8, 0x00202FDC,
+0x00201356, 0x00117804, 0x00202B10, 0x00117800,
+0x002013A2, 0x00203014, 0x00117808, 0x00202FF4,
+0x0020139A, 0x00203008, 0x00203010, 0x02FCE024,
+0x672CE07C, 0xEC000F76, 0xE07CEB04, 0x62CD07FE,
+0x8B013273, 0x0009A118, 0x6CCDD7B9, 0x357C65C3,
+0x62C37704, 0xD4B7327C, 0x6D52D7B7, 0x6E22470B,
+0x470BD7B6, 0xD4B664D3, 0x420BD2B3, 0xD2B30009,
+0x64E3420B, 0xD6B0D4B3, 0x0009460B, 0x67D3E600,
+0x376C666D, 0x626D7601, 0x27E032B3, 0x4E198FF7,
+0x7C08AFD3, 0x6212D1A6, 0x2228622C, 0xD2AA8B04,
+0x0009420B, 0x0009A003, 0x420BD2A8, 0x56FB0009,
+0xA0E1E200, 0xB1A62620, 0x56FB0009, 0xA0DBE200,
+0x52FB2620, 0xE500D69A, 0x65622250, 0x7604D2A0,
+0x62622252, 0xA0CFD69F, 0x56FB2620, 0x2610E124,
+0x5217D19D, 0x52181621, 0x52191622, 0x521A1623,
+0x551B1624, 0x1655E200, 0x1656551C, 0x1657551D,
+0x1658551E, 0x1659551F, 0x11281127, 0x112A1129,
+0x112C112B, 0x112E112D, 0x112FA0AE, 0xD68FD18E,
+0x6262E040, 0x76046512, 0x2152352C, 0x55116266,
+0x1151352C, 0x62626563, 0x75085612, 0x1162362C,
+0x56136252, 0x362C75EC, 0x62521163, 0x75105614,
+0x1164362C, 0x62526653, 0x76105515, 0x1155352C,
+0x56166262, 0x362CD57E, 0x62561166, 0x362C5617,
+0x66531167, 0x55186252, 0x352C7604, 0x62661158,
+0x352C5519, 0x65631159, 0x561A6262, 0x362C7504,
+0x6252116A, 0x7504561B, 0x116B362C, 0x561C6256,
+0x116C362C, 0x561D6256, 0x116D362C, 0x62526653,
+0x7604551E, 0x115E352C, 0x561F6262, 0x362CD569,
+0x6252116F, 0x7594061E, 0x0166362C, 0x6653E044,
+0x051E6252, 0x352C7644, 0xE0480156, 0x061E6262,
+0x0166362C, 0xE054D660, 0x051E6262, 0x352C4229,
+0x76040156, 0xE0586262, 0x4229061E, 0x0166362C,
+0xE23856FB, 0xE0442620, 0xE048021E, 0x62121621,
+0x55111622, 0x1653E200, 0x16545512, 0x16555515,
+0x16565513, 0x16575516, 0xE040051E, 0x051E1658,
+0x1659E050, 0x165A5514, 0xE04C051E, 0x051E165B,
+0x165CE054, 0xE058051E, 0x051E165D, 0x165EE044,
+0xE0480126, 0x11212122, 0x11251122, 0x11261123,
+0xE0400126, 0xE0500126, 0x01261124, 0x0126E04C,
+0x0126E054, 0x0126E058, 0x3F3C9358, 0x6EF64F26,
+0x6CF66DF6, 0x000B6BF6, 0x4F2269F6, 0xE240614D,
+0x89143123, 0x3127E21F, 0x8B09D734, 0xD434614D,
+0xE00171E0, 0x5671440B, 0x26596507, 0x1761A007,
+0xE001D42F, 0x6672440B, 0x26596507, 0x4F262762,
+0x0009000B, 0x614D4F22, 0x3123E240, 0xE21F8912,
+0xD7263127, 0x614D8B08, 0x5671D225, 0x420B71E0,
+0x260BE001, 0x1761A006, 0x6672D221, 0xE001420B,
+0x2762260B, 0x000B4F26, 0xE6400009, 0x46284618,
+0x6252D51C, 0x89FC2268, 0x0009000B, 0x4618E680,
+0xD5184628, 0x22686252, 0x000B89FC, 0xA0010009,
+0x7201E200, 0x8BFC3242, 0x0009000B, 0x00000080,
+0x00117804, 0x00202FF4, 0x00201356, 0x0020139A,
+0x00203008, 0x00203010, 0x00200C38, 0x00200C12,
+0x00202F00, 0x00202F14, 0x00202AA4, 0x001C36A0,
+0x001C3CA0, 0x001C36F4, 0x001C3B88, 0x001C3704,
+0x002029F8, 0x001C373C, 0x4618E680, 0xD52F4628,
+0x22686252, 0x000B8BFC, 0x2FE60009, 0x7FFC4F22,
+0xBFF16E53, 0x61E22F42, 0xE280D629, 0x54E11615,
+0x16464218, 0x422855E2, 0x57E31657, 0x16786EF2,
+0x26E22E2B, 0x4F267F04, 0x6EF6AFA8, 0x2FD62FC6,
+0x4F222FE6, 0x6C53DD1E, 0x6E43BFD6, 0x2DE2BF95,
+0x0009BFD2, 0x2C1251D5, 0x1C4154D6, 0x1C5255D7,
+0x1C6356D8, 0x6EF64F26, 0x000B6DF6, 0x61636CF6,
+0xA004E600, 0x62564109, 0x24227601, 0x36127404,
+0x000B8BF9, 0x4F220009, 0xD10FD40E, 0x0009410B,
+0xD40FD20E, 0xE5056022, 0x2202CB20, 0xD50D2452,
+0x450BE700, 0xD70C2472, 0x0009470B, 0xE601D10B,
+0x2162D20B, 0x4F264618, 0x2262000B, 0x001C3700,
+0x001C370C, 0x00203028, 0x00201356, 0x001C3500,
+0x001D4004, 0x002013CC, 0x00200EF8, 0x001E212C,
+0x001C3D30, 0x0009A1A9, 0x2FE62FD6, 0xDD8F4F22,
+0xA0049EA7, 0xD48E0009, 0x420BD28E, 0x62D265D2,
+0x8BF822E8, 0x0009A004, 0xD28AD48B, 0x55D1420B,
+0x22E852D1, 0xA0048BF8, 0xD4880009, 0x420BD285,
+0x52D255D2, 0x8BF822E8, 0x0009A004, 0xD281D484,
+0x55D3420B, 0x22E852D3, 0xA0048BF8, 0xD4810009,
+0x420BD27C, 0x52D455D4, 0x8BF822E8, 0x6EF64F26,
+0x6DF6000B, 0x2FD62FC6, 0x4F222FE6, 0x6E636D73,
+0x6C53B018, 0x64C357F4, 0xB05465E3, 0xB06A66D3,
+0xB09A0009, 0xB09E0009, 0xB0A20009, 0xB0BE0009,
+0xB0C10009, 0xB1240009, 0x4F260009, 0x6DF66EF6,
+0x6CF6A023, 0x3412D16C, 0xD66C0529, 0x2650D76C,
+0x2742000B, 0x0009A014, 0x2FD62FC6, 0x4F222FE6,
+0x6E636D73, 0x6C53BFEE, 0x64C357F4, 0xB02A65E3,
+0xB10666D3, 0x4F260009, 0x6DF66EF6, 0x6CF6A005,
+0xE603D260, 0x000B4618, 0xD25E2262, 0x000BE600,
+0x4F222262, 0xE40ABF7E, 0x0009BF7E, 0xE104D25A,
+0xE5004118, 0x2212E40A, 0x2252BF74, 0x6072D757,
+0x4F26CB20, 0x2702000B, 0xD1554F22, 0xE400410B,
+0x452BD554, 0x2FE64F26, 0x6E63D153, 0x44186612,
+0x45289210, 0x26294408, 0x44084500, 0x4400265B,
+0x4708264B, 0x47082162, 0x27EBD14C, 0x000B2172,
+0x03F06EF6, 0x2FE61FFF, 0xDE494F22, 0xE40AE101,
+0x2E12BF48, 0x726C62E3, 0xE401E100, 0x22122212,
+0x22122212, 0x22122212, 0xE7302242, 0xE40AE503,
+0x22122212, 0x22122212, 0x22122212, 0x22122212,
+0x22122212, 0x22122212, 0x22522272, 0x22122212,
+0x22122212, 0x22122212, 0x22122212, 0x121ABF22,
+0x2E62E600, 0x000B4F26, 0xD2326EF6, 0xE441E101,
+0x000B2212, 0xD1302242, 0xE605D430, 0x000B2162,
+0xD52F2462, 0x6050D22F, 0x8B0E8801, 0x6040D42E,
+0x8B078801, 0x9626D52D, 0x88016050, 0x96238B0C,
+0x0009A00A, 0xA0079621, 0xE6000009, 0x2262D426,
+0x88016040, 0xE6048B00, 0xAEF3E40A, 0xD2242262,
+0xE40AE601, 0x2262AEEE, 0x2FC62FB6, 0x2FE62FD6,
+0xDC204F22, 0x60C2ED00, 0xCB01EB64, 0x60C22C02,
+0xA041C901, 0x03C46E03, 0x034003D4, 0x001C3B88,
+0x0020302C, 0x002013A2, 0x00203034, 0x0020303C,
+0x00203044, 0x0020304C, 0x0025E720, 0x0020321C,
+0x00202F04, 0x001C5968, 0x001D4004, 0x001C3500,
+0x00201154, 0x00201180, 0x001C5814, 0x001C59D0,
+0x001C5830, 0x001C6268, 0x001C59A4, 0x001C639C,
+0x00202F16, 0x001C5804, 0x00202F15, 0x00202F17,
+0x001C581C, 0x001C5860, 0x89073DB2, 0xE40A60C2,
+0xBE9FC901, 0x7D016E03, 0x8BF52EE8, 0x8B033DB2,
+0xD23ED43D, 0x0009420B, 0x4F26E40A, 0x6DF66EF6,
+0xAE8F6CF6, 0x44116BF6, 0x604B8F01, 0x000B6043,
+0x2FB60009, 0x2FD62FC6, 0x4F222FE6, 0xDC347FFC,
+0x60C2ED00, 0xCB02EB64, 0x60C22C02, 0xC9022F02,
+0x6E03A009, 0x89083DB3, 0xE40A60C2, 0xC9022F02,
+0x6E03BE70, 0x2EE87D01, 0x3DB38BF4, 0xD4298B08,
+0x7F04D226, 0x6EF64F26, 0x6CF66DF6, 0x6BF6422B,
+0x4F267F04, 0x6DF66EF6, 0x000B6CF6, 0xD5226BF6,
+0x60525651, 0x000B4628, 0x2FB6306C, 0x2FD62FC6,
+0x4F222FE6, 0x4F024F12, 0x6E43BFF1, 0xDC1B6B03,
+0xBFECDD1B, 0x30B80009, 0x060A3C05, 0x46094609,
+0x3D654601, 0x4209020A, 0x42094209, 0x8BF032E2,
+0x4F164F06, 0x6EF64F26, 0x6CF66DF6, 0x6BF6000B,
+0x4F222FE6, 0xE102DE0F, 0xE403E500, 0xBFD42E12,
+0xE6062E52, 0xE7004618, 0x2E62E403, 0x4F262E72,
+0x6EF6AFCB, 0x0009000B, 0x00203054, 0x00201356,
+0x001C5860, 0x0020306C, 0x001C1040, 0xCCCCCCCD,
+0x10624DD3, 0x001D4004, 0x2F962F86, 0x2FB62FA6,
+0x2FD62FC6, 0x4F222FE6, 0xE5007FDC, 0x6453E110,
+0x6C534128, 0xED096E53, 0x6653655D, 0x365C4608,
+0x75014608, 0x6043361C, 0x0F66675D, 0xEB0060C3,
+0x26C137D3, 0x81628161, 0x16B28163, 0x16B416E3,
+0x74048FEA, 0xD9A668F2, 0x1981DAA6, 0x59F12982,
+0x1A91D1A5, 0x5AF22A92, 0x5DF45BF3, 0x54F65EF5,
+0x21A211A1, 0x11B211B3, 0x11D411D5, 0x11E611E7,
+0x11481149, 0x55F7EE00, 0x57F8DD9C, 0x64E3D29C,
+0xDB9DD99C, 0xE845EAB8, 0x2D521D51, 0x6AAC2272,
+0x6EED4808, 0x4D086DE3, 0x3DEC65E3, 0x4D084508,
+0x3D9C35EC, 0x450860C3, 0x81D12DC1, 0x4508E050,
+0x45084008, 0x60C381D2, 0xE60035BC, 0x81D334A2,
+0x1D531DD2, 0x8D01D489, 0xD4861D64, 0xB05C65D3,
+0x64ED7E01, 0x8BDC3482, 0xDB88D182, 0xD2806812,
+0x1B814829, 0x2FD26412, 0x2B92694D, 0xD97F6722,
+0x1B734729, 0xD77C6822, 0x1BA26A8D, 0xD2806B72,
+0x22B2D57B, 0xE0035D72, 0x5E7412D2, 0x12E44018,
+0xD67C5176, 0x54781216, 0x1248E103, 0xD4796792,
+0x6852127A, 0x28C1E7FF, 0x81916952, 0x6A52E050,
+0x81A24008, 0x60C36B52, 0x6C5281B3, 0x6E521CC2,
+0x62521E63, 0x1264E600, 0x46086563, 0x7501364C,
+0x665D2672, 0x8BF83613, 0x4F267F24, 0x6DF66EF6,
+0x6BF66CF6, 0x69F66AF6, 0x68F6000B, 0x60616642,
+0x8D04C803, 0x6061E500, 0x8802C903, 0x52628B03,
+0x51246563, 0x000B2412, 0x2FD66053, 0x4F222FE6,
+0x6E537FEC, 0xE5506253, 0xE4006D43, 0xA0014508,
+0x5224E101, 0x22116043, 0x81238121, 0x81226053,
+0x362056E2, 0xD2548BF5, 0x64F316E4, 0x420BE614,
+0x65E165E3, 0x2549E4FC, 0x61F12E51, 0x214965F3,
+0x54D12F11, 0x410BD14C, 0x57D1E614, 0xCB016071,
+0x1DE12701, 0x4F267F14, 0x000B6EF6, 0x2FD66DF6,
+0x4F222FE6, 0x6E537FEC, 0xE5FC6653, 0x60616D43,
+0xCB012059, 0x52E22601, 0x8B063260, 0x51E212E4,
+0x8B0431E0, 0xA00252D1, 0xAFF01E22, 0xD23A5664,
+0xE61464F3, 0x65E3420B, 0xE1FC67E1, 0x2E712719,
+0x54D167F1, 0xD1342719, 0xE61465F3, 0x2F71410B,
+0x602152D1, 0x2201CB01, 0x7F141DE1, 0x6EF64F26,
+0x6DF6000B, 0x4F222FE6, 0xDE23624C, 0x42004208,
+0x3E2CA005, 0xD41F5252, 0xBF8E5624, 0x65E22E62,
+0x352052E1, 0xD6228BF6, 0x4F262622, 0x6EF6000B,
+0x2FC62FB6, 0x2FE62FD6, 0xDC184F22, 0x52C1DB1F,
+0x362066C2, 0x6061891C, 0x8801C903, 0xDE138918,
+0xBF63DD1B, 0x650364E3, 0x66B28503, 0x3262620D,
+0xD40B8907, 0x0009BF9B, 0x4D0BD416, 0xAFE60009,
+0xBF620009, 0xD41464E3, 0x00094D0B, 0x0009AFDF,
+0x2262D212, 0x6EF64F26, 0x6CF66DF6, 0x6BF6000B,
+0x00202B00, 0x00202B08, 0x00202B10, 0x00202B38,
+0x00202F1C, 0x001000B4, 0x00101680, 0x001E2108,
+0x001C3D00, 0x00117880, 0x00200A9E, 0x00202F00,
+0x00201356, 0x00203088, 0x0020308C, 0x001C3D28,
+0x2FC62FB6, 0x2FE62FD6, 0x7FFC4F22, 0x6022D22B,
+0x8D41C803, 0xDE2A2F01, 0xDB2BDC2A, 0xED01A017,
+0xC9036051, 0x89168801, 0xD128D426, 0x0009410B,
+0x61035503, 0xC8208551, 0xE0508903, 0x720102BE,
+0xD2230B26, 0x420B64E3, 0xD6226513, 0x52C126D2,
+0x352065C2, 0xDE208BE4, 0xDB21DD20, 0x52D1DC21,
+0x352065D2, 0x60518918, 0x8801C903, 0xD41B8914,
+0x460BD616, 0x57030009, 0x8F0437E0, 0xE2016503,
+0xAFEC2B20, 0xD4182C52, 0x420BD218, 0xD6110009,
+0x4118E101, 0x2612AFE3, 0xC80460F1, 0xD2148907,
+0x4F267F04, 0x6DF66EF6, 0x422B6CF6, 0x7F046BF6,
+0x6EF64F26, 0x6CF66DF6, 0x6BF6000B, 0x001E2100,
+0x00202B10, 0x00202B08, 0x00202AA4, 0x0020106C,
+0x002010EE, 0x001C3D30, 0x00117880, 0x00202B00,
+0x00202F20, 0x00202F1C, 0x00202B38, 0x0020108A,
+0x00200170, 0xE601D203, 0x1265D503, 0x000B2252,
+0x00001266, 0x001C1010, 0x0000C34F, 0x0009000B,
+0x2FD62FC6, 0x4F222FE6, 0x6D436C53, 0xEE00A004,
+0x7E0164D4, 0x644CBFF2, 0x8BF93EC2, 0x6EF64F26,
+0x000B6DF6, 0xE5006CF6, 0x6643A002, 0x76017501,
+0x22286260, 0xAFE38BFA, 0x2FE60009, 0x75076253,
+0xE1086753, 0x6043EE0A, 0x4409C90F, 0x650330E2,
+0x8D014409, 0xE630E637, 0x4110365C, 0x8FF22760,
+0xE00077FF, 0x000B8028, 0x000B6EF6, 0x000BE000,
+0x2FE6E000, 0x7FEC4F22, 0x6E436253, 0xBFDC65F3,
+0xBFD06423, 0xBFCE64E3, 0xD40364F3, 0x0009BFCB,
+0x4F267F14, 0x6EF6000B, 0x00203090, 0xE4FDD59A,
+0xD69A6152, 0x25122149, 0x74016052, 0x2502CB01,
+0xD1976752, 0x25722749, 0xC8406010, 0x60628902,
+0x2602CB04, 0xE5016062, 0x2602CB08, 0xE4026062,
+0x2602C9CF, 0x45186062, 0x2602CB03, 0x000B1642,
+0xD58C1653, 0xD28DD78C, 0xE100D48D, 0x2511E600,
+0x22102711, 0x2461AFD2, 0xD28A664C, 0x362C4600,
+0xCB106060, 0x2600000B, 0xD286654C, 0x352C4500,
+0xE1EF6650, 0x000B2619, 0x664C2560, 0x4600D282,
+0x6060362C, 0x000BCB10, 0x654C2600, 0x4500D27E,
+0x6650352C, 0x2619E1EF, 0x2560000B, 0xD279664C,
+0x362C4600, 0xCB086060, 0x2600000B, 0xD275654C,
+0x352C4500, 0xE1F76650, 0x000B2619, 0x664C2560,
+0x4600D271, 0x6060362C, 0x000BCB08, 0x654C2600,
+0x4500D26D, 0x6650352C, 0x2619E1F7, 0x2560000B,
+0xD668624C, 0x326C4200, 0xC9086020, 0x40214021,
+0x000B4021, 0x624C600C, 0x4200D663, 0x6020326C,
+0x4021C908, 0x40214021, 0x600C000B, 0x644CD15F,
+0x6240341C, 0x602C000B, 0x644CD15D, 0x6240341C,
+0x602C000B, 0x4F222FE6, 0x645C6E43, 0x3467E60A,
+0xBFEB8914, 0x640C0009, 0x880160EC, 0xE00F8B02,
+0x2409A002, 0x44094409, 0xE60A624C, 0x89053263,
+0x644CBFE2, 0x6023620C, 0x8B00C880, 0x6023E200,
+0x000B4F26, 0x4F226EF6, 0x6062D64A, 0x8B038801,
+0x0009B246, 0x0009A003, 0xE640D247, 0xD6472260,
+0x4F26E200, 0x2622000B, 0xD6424F22, 0x88026062,
+0xB28F8B01, 0xD6410009, 0x4F26E200, 0x2622000B,
+0xD43DD53C, 0xE701E100, 0x000B2512, 0xD23A2470,
+0x000BE604, 0x4F222260, 0xD13AD439, 0x0009410B,
+0xE1FDD539, 0xD2396650, 0xE7002619, 0x4F262560,
+0x2270000B, 0xD4364F22, 0x410BD132, 0xD5320009,
+0x6650E7FB, 0x4F262679, 0x2560000B, 0xD4314F22,
+0x410BD12C, 0xD52C0009, 0x6650E7F7, 0x4F262679,
+0x2560000B, 0x942DD528, 0x22496250, 0x2520000B,
+0xE4BFD525, 0x22496250, 0x2520000B, 0xD2264F22,
+0x600D8522, 0x89112008, 0x89138801, 0x89158803,
+0x89438805, 0x89498806, 0x894F8808, 0x89558809,
+0x895B880A, 0x8961880B, 0x0009A068, 0x0009B06A,
+0x600CA065, 0x0009B078, 0x600CA061, 0x0009B081,
+0x600CA05D, 0x0000FF7F, 0x001E2148, 0x001E1108,
+0x001E1000, 0x00202F60, 0x00202F62, 0x00202F81,
+0x00202F44, 0x001E103F, 0x001E105F, 0x001E102F,
+0x001E1090, 0x00202F68, 0x001E100B, 0x00202F64,
+0x00203094, 0x00201356, 0x001E1028, 0x00202F80,
+0x002030A0, 0x002030B0, 0x00202F38, 0x6260D684,
+0x8B2B2228, 0x0009B061, 0x600CA029, 0x6260D680,
+0x8B232228, 0x0009B069, 0x600CA021, 0x6260D67C,
+0x8B1B2228, 0x0009B0C7, 0x600CA019, 0x6260D678,
+0x8B132228, 0x0009B0CD, 0x600CA011, 0x6260D674,
+0x8B0B2228, 0x0009B125, 0x600CA009, 0x6260D670,
+0x8B032228, 0x0009B13D, 0x600CA001, 0x4F26E000,
+0x0009000B, 0xD26CD16B, 0xD56C8412, 0x4000C90F,
+0xD76B012D, 0xE403D66B, 0xE20F611C, 0x2540E001,
+0x25202712, 0x2602000B, 0xE601D262, 0x30668523,
+0xE0008D05, 0xD663D260, 0xE0018122, 0x000B2602,
+0xD25C0009, 0x600D8523, 0x89052008, 0x8B0A8801,
+0x6060D65D, 0x2600CB01, 0xD457D65A, 0xE001E101,
+0x000B2612, 0x000B8142, 0xD152E000, 0x8513E501,
+0x640D4518, 0x66033453, 0xE0008D05, 0xD551D253,
+0x2260E001, 0x000B2502, 0x4F220009, 0x8513D149,
+0x6453650D, 0x62494419, 0x227D672E, 0x8801602C,
+0x88028909, 0x88038910, 0x8806891A, 0x88078935,
+0xA04C893B, 0xD5460009, 0x6652D746, 0x2762D446,
+0x622C6261, 0x2421A038, 0x2228625C, 0xD4438B3F,
+0x6642D540, 0x2562D440, 0x24018561, 0x6203A02C,
+0x2008605C, 0x88108907, 0x88208908, 0x88308909,
+0xA02C890A, 0xD23A0009, 0x6222A008, 0xA005D239,
+0xD2396222, 0x6222A002, 0x6262D638, 0xD432D531,
+0x66212522, 0xA00F626C, 0xD6352421, 0x6261D52D,
+0x622CD42D, 0xA0072562, 0xD6322421, 0x8561D529,
+0x2562D429, 0x62032401, 0x662D8515, 0x3617610D,
+0x65038F01, 0xB0CB2451, 0xA0010009, 0xE000E001,
+0x000B4F26, 0xD6190009, 0xD427E101, 0x65412610,
+0xD118D717, 0xE20F655D, 0x2752E001, 0x000B2620,
+0x2FE62102, 0xD20F4F22, 0x640C8523, 0x8B082448,
+0xD511D61D, 0x2621E200, 0x940F8451, 0xA0482049,
+0xDE0D8051, 0xC84060E0, 0xE2018D32, 0x89443427,
+0xD216D615, 0x2641420B, 0x0009A030, 0x0000FF7F,
+0x00202F81, 0x00202F38, 0x00202F44, 0x001E1100,
+0x001E100C, 0x00202F64, 0x001E1000, 0x001E1001,
+0x00202F6C, 0x00202F4C, 0x00202F50, 0x00202F54,
+0x00202F70, 0x00202F74, 0x00202F78, 0x00202F7C,
+0x00203280, 0x0020328A, 0x00202F5E, 0x0020225A,
+0x89123427, 0xD294D693, 0x2641420B, 0xCB8084E1,
+0x80E1B0F5, 0xD69160E0, 0x2E00CB04, 0xC93F6060,
+0xD68F2600, 0xA001E001, 0xE0002602, 0x000B4F26,
+0xD68C6EF6, 0xC8806060, 0xD2868919, 0x88016021,
+0xD2898B15, 0x8524E501, 0x89103056, 0xE203D187,
+0x2120D487, 0xE00B6541, 0x0656655D, 0xE40FD585,
+0x2140E702, 0xD77E2571, 0x000BE001, 0x000B2702,
+0x2FE6E000, 0xDE804F22, 0xC88084E1, 0xD57A892C,
+0x20088554, 0x61038F28, 0x8553D77C, 0x64036672,
+0x8566650C, 0x3520620C, 0xD6798B1E, 0x651CD774,
+0x2651644C, 0x60E02741, 0x8904C840, 0x420BD275,
+0xA0030009, 0xD2680009, 0x0009420B, 0x0009B09F,
+0xE201D167, 0x60E02122, 0xCB04D464, 0x60402E00,
+0x2400C93F, 0x6023A001, 0x4F26E000, 0x6EF6000B,
+0x2FB62FA6, 0x2FD62FC6, 0xDA622FE6, 0x66A1E240,
+0x3622DC5E, 0x62638900, 0x6ED36D2C, 0x4E2136D8,
+0x4E212A61, 0xDB61D460, 0xE700A00F, 0x770162B2,
+0x71026123, 0x66212B12, 0x71026213, 0x61212B12,
+0x651D666D, 0x356C4528, 0x627C2452, 0x8BED32E3,
+0xC90360D3, 0x8B108803, 0x617367B2, 0x2B127102,
+0x71026E13, 0x2B126571, 0x655D6DE1, 0x422862DD,
+0x325CE107, 0xA00C2C10, 0x88022422, 0xA0038B01,
+0x8801E203, 0xE2018B05, 0x66B22C20, 0x655D6561,
+0xE60F2452, 0x67A12C60, 0x8B052778, 0xDD38DC44,
+0xEB01EA00, 0x2DB22CA2, 0x6DF66EF6, 0x6BF66CF6,
+0x6AF6000B, 0x2FE62FD6, 0xE240DD36, 0x362266D1,
+0x62638900, 0x3678672C, 0x7703DE38, 0x47212D61,
+0x64E2D635, 0xA00E4721, 0x6562E100, 0x62537101,
+0x74012450, 0x24204219, 0x45297401, 0x74012450,
+0x24504519, 0x621C7401, 0x8BEE3273, 0x66E24200,
+0x420061D1, 0x2118362C, 0x2E628F06, 0xDD1CD728,
+0xE501E400, 0x2D522742, 0x000B6EF6, 0x2FD66DF6,
+0x4F222FE6, 0xED0AEE01, 0x64E3BC96, 0xBC9B64E3,
+0x62EC7E01, 0x8BF732D7, 0xBC9EEE01, 0x64E364E3,
+0x7E01BCA3, 0x32D762EC, 0x4F268BF7, 0x000B6EF6,
+0xD1186DF6, 0xD418920D, 0x72122122, 0x2422D617,
+0xD7177204, 0x72202622, 0x2722D116, 0x000B7230,
+0x137A2122, 0x00202F5E, 0x00202366, 0x001E1015,
+0x00202F64, 0x001E1001, 0x00202F38, 0x001E1100,
+0x00202F62, 0x00202F50, 0x001E1000, 0x00202F54,
+0x00202F60, 0x0020225A, 0x001E100C, 0x00202F4C,
+0x00202F68, 0x00202F6C, 0x00202F70, 0x00202F74,
+0x00202F78, 0x00202F7C, 0x4F222FE6, 0xD6507FFC,
+0x88016060, 0xE2018951, 0x2620BFBB, 0xD54ED14D,
+0xDE4E6010, 0x64E36552, 0x7402C840, 0x8D22D14C,
+0xD24C7502, 0xE601D74C, 0xE7042722, 0x76016255,
+0x626C2421, 0x8FF93273, 0xD4437402, 0x6242E601,
+0x640D8528, 0x67494419, 0x275D657E, 0x81E4607C,
+0xE417D542, 0x67557601, 0x3243626C, 0x8FF92171,
+0xA0207102, 0xD23E0009, 0xE601D73B, 0xE7042722,
+0x76016255, 0x626C2421, 0x8FF93273, 0xD4327402,
+0x6242E601, 0x640D8528, 0x67494419, 0x275D657E,
+0x81E4607C, 0xE417D533, 0x67557601, 0x3243626C,
+0x8FF92171, 0x924A7102, 0xD2262E21, 0x5E23D72E,
+0x64F22FE2, 0x604365F2, 0x2700C980, 0xC9606043,
+0x80716103, 0xC9036043, 0x80724519, 0x65F2605C,
+0x817266F2, 0x46194629, 0x606C4529, 0x4018645C,
+0x8173304C, 0x21185E23, 0x64F22FE2, 0x6E4C62F2,
+0x602C4219, 0x66F262F2, 0x46294018, 0x461930EC,
+0x42298174, 0x652C606C, 0x305C4018, 0x81758F07,
+0x0009BC9C, 0x2228620C, 0xA00A8908, 0x60130009,
+0x8B038840, 0x0009B009, 0x0009A003, 0xE202D60F,
+0x7F042622, 0x000B4F26, 0x000B6EF6, 0x060A0009,
+0x00202F80, 0x001E1000, 0x00202F6C, 0x00203280,
+0x0020328C, 0x00203224, 0x00202F54, 0x00203254,
+0x00203252, 0x00203226, 0x00202F38, 0x00202F64,
+0x4F222FE6, 0xDE937FFC, 0x200884E9, 0x2F008D06,
+0xD692D491, 0x0009460B, 0x64F0B194, 0x6620D290,
+0x89022668, 0xC9BF60E0, 0x7F042E00, 0x000B4F26,
+0x000B6EF6, 0x2FE60009, 0xDE8A4F22, 0x60E0D68A,
+0xCBC0D48A, 0x62602E00, 0xC803602C, 0x40218904,
+0x70014021, 0x6603A002, 0x66034009, 0xD684616D,
+0xE500A004, 0x75016262, 0x74042422, 0x3213625D,
+0xD2808BF8, 0x0009420B, 0xC9BF84E2, 0x4F2680E2,
+0x6EF6000B, 0x2FE62FD6, 0x7FFC4F22, 0x6260D67A,
+0x89442228, 0xD56FE100, 0x60502610, 0xCB40D477,
+0x2500440B, 0x8D052008, 0x62E06E03, 0x7104612C,
+0x2F11A006, 0xD472D66A, 0xDD726760, 0x657C4D0B,
+0xE23C6D1D, 0x8B033D27, 0xD264D46F, 0x0009420B,
+0x4D214D21, 0xA005D76D, 0x66E6E400, 0x357C4508,
+0x74012562, 0x35D3654D, 0xD7698BF7, 0x6172E003,
+0x81114018, 0x6E7260F1, 0x81E2700C, 0xD4656172,
+0xDD658113, 0x4D0BDE65, 0xE2016572, 0xD4642E22,
+0x420BD252, 0xD6530009, 0xC93F6060, 0x7F042600,
+0x6EF64F26, 0x6DF6000B, 0x2FC62FB6, 0x2FE62FD6,
+0xD25C4F22, 0x6B436E73, 0x420B6C53, 0x20086D63,
+0x61038F08, 0xD245D458, 0x6EF64F26, 0x6CF66DF6,
+0x6BF6422B, 0x21B060C3, 0x60D38011, 0xE5008111,
+0x64BCA007, 0x6053655D, 0x665300EC, 0x7501361C,
+0x625D8064, 0x8BF53243, 0x6060D636, 0x2600C9BF,
+0x6EF64F26, 0x6CF66DF6, 0x6BF6000B, 0x7FC44F22,
+0x720262F3, 0x22512F41, 0x45297202, 0x60632251,
+0xE5C4E682, 0x67F38121, 0x655C666C, 0xE408BFBC,
+0x4F267F3C, 0x0009000B, 0x2F962F86, 0x2FB62FA6,
+0x2FD62FC6, 0x4F222FE6, 0xE1007FC4, 0x6513ECFF,
+0x6B136CCD, 0xDE36D735, 0xEDFF64F3, 0xD835EA04,
+0x6053655C, 0x027D4000, 0x32C0622D, 0x66038D0D,
+0x09ED6063, 0x2491027D, 0x24217402, 0x698202ED,
+0x3928622D, 0x74022892, 0x75017104, 0x6063625C,
+0x07D532A2, 0x0EB58FE4, 0x2448641C, 0xE6808905,
+0x67F3E5C5, 0xBF7F666C, 0x7F3C655C, 0x6EF64F26,
+0x6CF66DF6, 0x6AF66BF6, 0x000B69F6, 0xD11E68F6,
+0x6012D21E, 0xCB20E405, 0x2102E500, 0x000B2242,
+0x00002252, 0x001E1017, 0x002030BC, 0x00201356,
+0x00202F1A, 0x001E1015, 0x001E10BF, 0x00117800,
+0x001E10FC, 0x00200170, 0x00202F20, 0x002024BE,
+0x002030C0, 0x002013A2, 0x002030DC, 0x0011788C,
+0x00202F1C, 0x00202B00, 0x002010EE, 0x001E2130,
+0x002030E4, 0x00202480, 0x002030E8, 0x00202F26,
+0x00202F2E, 0x00203220, 0x001C3500, 0x001D4004,
+0xD565D164, 0xE400D765, 0x2142E20F, 0x17411154,
+0xD5632722, 0x9669D763, 0x15412572, 0x96661562,
+0xE6011565, 0xD5601165, 0x666CE6F8, 0x25422542,
+0x25422542, 0x25422542, 0x25622542, 0x7601E727,
+0x67632572, 0x25627797, 0xE7042572, 0x2572E248,
+0xE2192522, 0xE2702522, 0x25422542, 0x25422542,
+0x25222542, 0x2522E20C, 0x25422542, 0x25422542,
+0x25422542, 0x25422542, 0x000B154A, 0xE2081145,
+0x0009422B, 0x2FE62FD6, 0x7FFC4F22, 0xC8206043,
+0x6E438D02, 0x0009BE6D, 0xC81060E3, 0xBE6A8901,
+0x60E30009, 0x8901C840, 0x0009BE8C, 0xC80160E3,
+0xDD3E8938, 0xC80260D0, 0x2F008D03, 0x460BD63C,
+0x60F00009, 0x8902C804, 0x460BD63A, 0x62F00009,
+0xC8806023, 0x60D08902, 0x2D00C97F, 0xC8016023,
+0xD6358906, 0x0009460B, 0x0009A007, 0x51630601,
+0x8902C808, 0x460BD631, 0x60F00009, 0x8902C810,
+0x420BD22F, 0xD52F0009, 0x88026052, 0xD22E8B03,
+0xA005E604, 0x88012260, 0xD22B8B02, 0x2260E601,
+0x2522E200, 0xC88060E3, 0xD628892E, 0x60E36E60,
+0x8902C880, 0x420BD226, 0x60E30009, 0x8902C840,
+0x420BD224, 0x60E30009, 0x8902C802, 0x420BD222,
+0x60E30009, 0x890EC804, 0x410BD120, 0xBF0E0009,
+0xBF4D0009, 0xD51E0009, 0x6050D41E, 0xC908D71E,
+0xBF842500, 0x60E32472, 0x8905C808, 0x7F04D21B,
+0x6EF64F26, 0x6DF6422B, 0x4F267F04, 0x000B6EF6,
+0x00006DF6, 0x001C581C, 0xA000A000, 0x001D0100,
+0x001D4000, 0x00040021, 0x001C589C, 0x001E1021,
+0x00201536, 0x00201558, 0x00201B98, 0x00201570,
+0x0020157E, 0x00202F64, 0x001E100B, 0x001E1028,
+0x002015D4, 0x002015E0, 0x00201586, 0x002015A4,
+0x001E1000, 0x0010F100, 0x12345678, 0x002015BC,
+0x644CD6A7, 0x000B346C, 0xD6A62450, 0x346C644C,
+0x2450000B, 0x644CD6A4, 0x000B346C, 0x625C2450,
+0x4208616D, 0x42084119, 0x42006019, 0x670E614C,
+0xD49E321C, 0x4200207D, 0x324CC90F, 0x2200000B,
+0x4208625C, 0x42004208, 0x324C644C, 0x4200D498,
+0x000B324C, 0x2FE62260, 0x614C4F12, 0x4100D493,
+0x6710314C, 0xE29F666D, 0x27294619, 0x6E536269,
+0x672E6573, 0x4221227D, 0x42214221, 0x7601662C,
+0xE4014608, 0x34E84608, 0x644C4600, 0x071A0467,
+0x2150257B, 0x000B4F16, 0x4F226EF6, 0xD2857FE8,
+0x88016021, 0xD2848B7B, 0x26686621, 0xD2838B77,
+0x26686621, 0xE50F8B73, 0xE401BFA2, 0xBFA4E501,
+0xE586E400, 0xE400655C, 0x2F50BFA4, 0xBFA1E401,
+0xE602E506, 0x60634618, 0x81F2E401, 0x6543BF9F,
+0xE40185F2, 0xBFAB6543, 0x85F26603, 0x6543E401,
+0x6603BFB1, 0xE40265F0, 0x6053756C, 0x80F8BF80,
+0xBF82E402, 0x84F8E512, 0x7090E402, 0x6503BF82,
+0x4618E602, 0x81F66063, 0xBF80E402, 0x85F6E500,
+0x6603E402, 0xE500BF8C, 0xE40285F6, 0xBF926603,
+0xE5FEE500, 0xE010655C, 0xBF61E403, 0xE5130F54,
+0xE40EBF63, 0x05FCE010, 0xBF63E40E, 0xE5007585,
+0xBF64E403, 0xE500E640, 0xBF71E403, 0xE500E640,
+0xBF78E403, 0xE5FFE640, 0xE014655C, 0xBF47E404,
+0xE40F0F54, 0xE504BF49, 0x05FCE014, 0xBF49E40F,
+0xE5017584, 0xBF4AE640, 0xE501E404, 0xBF57E640,
+0xE501E404, 0xE404E640, 0xAF5C7F18, 0x7F184F26,
+0x000B4F26, 0x4F220009, 0xD2427FF0, 0x88016021,
+0xD2418B71, 0x26686621, 0xD2408B6D, 0x26686621,
+0xE50F8B69, 0xE401BF1C, 0xBF1EE501, 0xE586E400,
+0xE400655C, 0x2F50BF1E, 0xBF1BE401, 0xE401E506,
+0xBF1C6543, 0xE401E640, 0xBF296543, 0xE401E640,
+0xBF306543, 0x65F0E640, 0x756CE402, 0xBEFF6053,
+0xE40280F4, 0xE512BF01, 0xE40284F4, 0xBF017090,
+0xE6406503, 0xBF02E402, 0xE640E500, 0xBF0FE402,
+0xE640E500, 0xBF16E402, 0xE5FEE500, 0x6053655C,
+0xBEE5E403, 0xE51380F8, 0xE40EBEE7, 0xE40E84F8,
+0xBEE77085, 0xE5006503, 0xBEE8E640, 0xE500E403,
+0xBEF5E640, 0xE500E403, 0xBEFCE640, 0xE5FFE403,
+0x6053655C, 0xBECBE404, 0xE40F80FC, 0xE504BECD,
+0xE40F84FC, 0xBECD7083, 0xE5016503, 0xBECEE640,
+0xE501E404, 0xBEDBE640, 0xE501E404, 0xE404E640,
+0xAEE07F10, 0x7F104F26, 0x000B4F26, 0x00000009,
+0x001E102F, 0x001E1080, 0x001E1090, 0x001E103F,
+0x001E103E, 0x00202F5E, 0x00202F60, 0x00202F62,
+0xD21DD11C, 0x66206010, 0x676C7001, 0x3700C90F,
+0xE5008D13, 0x67106210, 0x7701622C, 0x64232170,
+0xD6166010, 0x44084408, 0x3428C90F, 0x62602100,
+0x7201D513, 0x44082620, 0x000B354C, 0xD10F6053,
+0x25586510, 0xE6008D13, 0xD60DD40B, 0x655C6540,
+0x47086753, 0x37584708, 0x47086540, 0x24507501,
+0x367C6040, 0x2400C90F, 0x72FF6210, 0x000B2120,
+0x00006063, 0x00202F19, 0x00202F18, 0x00202F1A,
+0x00202B40, 0x7FFC4F22, 0xE680D1A8, 0x666C6212,
+0xD2A72F22, 0x67F36563, 0x420B7542, 0x7F04E404,
+0x000B4F26, 0xE6800009, 0xD2A1666C, 0xE7006563,
+0x422B7540, 0xE6806473, 0xD29D666C, 0xE7006563,
+0x422B7543, 0x2FB66473, 0x2FD62FC6, 0x4F222FE6,
+0x4D18ED01, 0xDB98DC97, 0x65C252C1, 0x89203520,
+0xC9036051, 0x891C8801, 0xD194DE92, 0x64E3410B,
+0x85036503, 0x670D66B2, 0x89073762, 0xD291D490,
+0x0009420B, 0xE701D190, 0x2172AFE6, 0xDE8F64E3,
+0x00094E0B, 0xD48FD68E, 0x410BD18F, 0xAFDB26D2,
+0x4F260009, 0x6DF66EF6, 0x000B6CF6, 0x4F226BF6,
+0x85467FF4, 0x2F01E681, 0x666C8547, 0x854881F1,
+0x81F2D27B, 0x67F38542, 0x854381F3, 0x81F4E40C,
+0x65636053, 0x420B81F5, 0x7F0C7540, 0x000B4F26,
+0x2F860009, 0x2FA62F96, 0x2FC62FB6, 0x2FE62FD6,
+0x7FEC4F22, 0xE000D176, 0xD4782F12, 0x81F26103,
+0xDC771F42, 0xD6776B13, 0xE0014B08, 0x460BDE76,
+0x3BEC4B00, 0x66C21F03, 0x362052C1, 0xA1818B01,
+0x60610009, 0x8801C903, 0xA17B8B01, 0x85610009,
+0x8B01C801, 0x0009A080, 0x85D25D63, 0xC9036603,
+0x85D36403, 0x6053650D, 0x40214021, 0x4500C93F,
+0x322A6103, 0x6053252D, 0xC901E510, 0xD95E3153,
+0x6E038D21, 0x4408D761, 0x44086870, 0x44006213,
+0x28884200, 0x342C8F0E, 0x6043D25D, 0x60E3072D,
+0x4A196A7D, 0x658E68A9, 0x285D8801, 0x6A7C8F0B,
+0x6A13A009, 0x6043D257, 0x61ED0E2D, 0x68194119,
+0x287D678E, 0xD1546AEC, 0x22286210, 0xEAFF8901,
+0xEEFF6AAC, 0x6EEC65AD, 0x8B0F35E0, 0x4D0BDD3F,
+0x540364C3, 0xBF72E502, 0xD44C6D03, 0x410BD13F,
+0xD74B65D3, 0xD44BEE01, 0x27E2A025, 0x2679E7FC,
+0x81D26063, 0x946085D3, 0x61032049, 0x4508268B,
+0x251B6063, 0x605381D2, 0x85D481D3, 0x4118E108,
+0x81D4201B, 0xEE0262C2, 0x20798521, 0x64C28121,
+0x6041678D, 0xCB0137E3, 0x24018D04, 0xEEE785D2,
+0x81D220E9, 0x490BD438, 0x60C20009, 0x52F366F2,
+0x2B02CB01, 0x2622AF6F, 0xD2208561, 0x8F02C802,
+0xA0D264C3, 0x420B0009, 0xD9300009, 0x5E036503,
+0x079EE04C, 0x7701DD2E, 0x69D20976, 0x7901D626,
+0x6D602D92, 0x89062DD8, 0xD218D424, 0xED01420B,
+0xA0B3D723, 0x625127D2, 0x4118E10F, 0x2219E402,
+0x32404418, 0x85518B46, 0x20D9EDFC, 0x60518151,
+0xCB017DE3, 0x85E12501, 0x20D9D60A, 0x460B81E1,
+0x69F264C3, 0xA09957F3, 0x7E032972, 0x001C3D9C,
+0x00201E38, 0x00202B38, 0x00202F00, 0x0020106C,
+0x00202B00, 0x002010EE, 0x001E2130, 0x0020108A,
+0x001C3D30, 0x00203200, 0x00201356, 0x0020320C,
+0x00202B10, 0x002029F8, 0x001C3D00, 0x0020321C,
+0x00203100, 0x00203180, 0x00202F14, 0x00202B08,
+0x001E212C, 0x00203204, 0x00203208, 0x00202AA4,
+0x00203220, 0x6DDD6D51, 0x6DD94D19, 0x2D6D66DE,
+0x60DC7D01, 0x41186103, 0x8F458801, 0xD65B2511,
+0x74016462, 0x85E32642, 0x6063660D, 0x40214021,
+0x4600C93F, 0x322A6D03, 0x6063262D, 0xD154C801,
+0x8901D954, 0x2D6B96A1, 0xE010E600, 0x64DD0F64,
+0x07FCE010, 0x4000607C, 0x622D021D, 0x8D123240,
+0x60636603, 0xE7FF021D, 0x8B013270, 0x01D5A00B,
+0x02FCE010, 0x7201E604, 0x622C0F24, 0x8BE73262,
+0x666C06FC, 0x60634600, 0x7101019D, 0xD1420915,
+0x697D6711, 0x89073940, 0x602D6211, 0x890388FF,
+0xDD3E21D1, 0x2D20E201, 0xEDFC8551, 0x815120D9,
+0xD23B6051, 0x64C3CB01, 0x2501420B, 0x02FCE010,
+0x612CD438, 0x440BE001, 0x270267F2, 0xD23685EF,
+0x420B54F2, 0xAE96650D, 0x420B0009, 0x54030009,
+0x85446E03, 0x4D18ED08, 0x30D020D9, 0xBE568B03,
+0xA007E501, 0x85410009, 0x620DDD2C, 0x890122D8,
+0xE500BE4D, 0xD22BD42A, 0x65E3420B, 0xED01D72A,
+0x27D2AE79, 0xEE0485F2, 0x610D7001, 0x81F231E7,
+0x7C088D02, 0x0009AE66, 0x4F267F14, 0x6DF66EF6,
+0x6BF66CF6, 0x69F66AF6, 0x68F6000B, 0x4F222FE6,
+0x6E22D21E, 0xC84060E3, 0x22E28D02, 0x0009BDD2,
+0x4218E240, 0x89012E28, 0x0009BDDD, 0xC81060E3,
+0xD4178905, 0x420BD217, 0xBDDC0009, 0x60E30009,
+0x8901C805, 0x0009BE2D, 0xC80260E3, 0x4F268902,
+0x6EF6ADD9, 0x000B4F26, 0x80006EF6, 0x00203220,
+0x00202F26, 0x00202F2E, 0x00202F22, 0x00202F24,
+0x002010EE, 0x002029F8, 0x002013A2, 0x00008000,
+0x00202B08, 0x0020108A, 0x001E212C, 0x001C3510,
+0x00203214, 0x00201356, 0x080A0C0E, 0x00020406,
+0x1A1C1E20, 0x12141618, 0x2E303234, 0x26282A2C,
+0x3A3C3E40, 0x6C625648, 0x41112F26, 0xE2208F18,
+0x890B3123, 0x321CD204, 0xD1026220, 0x412B312C,
+0x00090009, 0x00202A22, 0x002029D8, 0x000BE000,
+0x400062F6, 0x40004000, 0x40004000, 0x40004000,
+0x62F6000B, 0x40004000, 0x40004000, 0x40004000,
+0x40184000, 0x62F6000B, 0x40004000, 0x40004000,
+0x40004000, 0x40284000, 0x62F6000B, 0x40004000,
+0x40184000, 0x000B4028, 0xC90F62F6, 0x40054005,
+0x40054005, 0x62F6000B, 0x4005C907, 0x40054005,
+0x62F6000B, 0x4005C903, 0x000B4005, 0xC90162F6,
+0x000B4005, 0x000062F6, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x544F0D0A, 0x46205355, 0x00003A57, 0x20636544,
+0x32203231, 0x20373030, 0x333A3132, 0x34323A36,
+0x00000000, 0x00000D0A, 0x00000043, 0x42707372,
+0x3D206675, 0x554E203D, 0x202C4C4C, 0x6E49677A,
+0x4E497274, 0x6D754E51, 0x0000003D, 0x61766E49,
+0x2064696C, 0x72657375, 0x20726F20, 0x2079656B,
+0x00214449, 0x52504545, 0x57204D4F, 0x65746972,
+0x6461202C, 0x003D7264, 0x6C617620, 0x0000003D,
+0x00000A0D, 0x6E6B6E55, 0x206E776F, 0x6D6D6F63,
+0x3D646E61, 0x00000000, 0x000A0D52, 0x203A3051,
+0x00000020, 0x203A3151, 0x00000020, 0x203A3251,
+0x00000020, 0x203A3351, 0x00000020, 0x203A3451,
+0x00000020, 0x61437748, 0x7262696C, 0x6F697461,
+0x6620206E, 0x0A6C6961, 0x0000000D, 0x73696F4E,
+0x61432065, 0x7262696C, 0x6F697461, 0x6166206E,
+0x21216C69, 0x00000D0A, 0x00000072, 0x00205220,
+0x00000D0A, 0x62735576, 0x7473725F, 0x00000A0D,
+0x62735576, 0x7375735F, 0x646E6570, 0x00000A0D,
+0x62735576, 0x7365725F, 0x000A0D6D, 0x00000042,
+0x72746E49, 0x6D652051, 0x2C797470, 0x49677A20,
+0x4972746E, 0x754E514E, 0x00003D6D, 0x654C7245,
+0x0000006E, 0x00000049, 0x20746F4E, 0x756F6E65,
+0x49206867, 0x4220514E, 0x0A0D6675, 0x00000000,
+0x000000FF, 0x00020001, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x010E010D, 0x00020003, 0x01090108, 0x0002010A,
+0x02000003, 0x02020201, 0x02040203, 0x02060205,
+0x02020200, 0x02040203, 0x020C0207, 0x020E020D,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x010E010D, 0x00FF010F, 0x01090108, 0x010B010A,
+0x020000FF, 0x02020201, 0x02040203, 0x02060205,
+0x02020200, 0x02040203, 0x020C020B, 0x020E020D,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00205220, 0x00000046, 0x00000059, 0x73204142,
+0x003D7165, 0x49544120, 0x0000204D, 0x00000000,
+0x00000000, 0x002E0209, 0x80000101, 0x000409FA,
+0x00FF0400, 0x05070000, 0x02000201, 0x82050700,
+0x00020002, 0x03830507, 0x07010040, 0x40020405,
+0x02090000, 0x0101002E, 0x09FA8000, 0x04000004,
+0x000000FF, 0x02010507, 0x07000040, 0x40028205,
+0x05070000, 0x00400383, 0x04050701, 0x00004002,
+0x00000000, 0x00000000, 0x07090000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, };
+
+const u32_t zcDKFwImageSize=12988;
diff --git a/drivers/staging/otus/hal/hpani.c b/drivers/staging/otus/hal/hpani.c
new file mode 100644
index 00000000000..ba95b5d012a
--- /dev/null
+++ b/drivers/staging/otus/hal/hpani.c
@@ -0,0 +1,732 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include "../80211core/cprecomp.h"
+#include "hpani.h"
+#include "hpusb.h"
+
+
+extern u16_t zfDelayWriteInternalReg(zdev_t* dev, u32_t addr, u32_t val);
+extern u16_t zfFlushDelayWrite(zdev_t* dev);
+
+/*
+ * Anti noise immunity support.  We track phy errors and react
+ * to excessive errors by adjusting the noise immunity parameters.
+ */
+
+/******************************************************************************
+ *
+ * New Ani Algorithm for Station side only
+ *
+ *****************************************************************************/
+
+#define ZM_HAL_NOISE_IMMUNE_MAX     4   /* Max noise immunity level */
+#define ZM_HAL_SPUR_IMMUNE_MAX      7   /* Max spur immunity level */
+#define ZM_HAL_FIRST_STEP_MAX       2   /* Max first step level */
+
+#define ZM_HAL_ANI_OFDM_TRIG_HIGH       500
+#define ZM_HAL_ANI_OFDM_TRIG_LOW        200
+#define ZM_HAL_ANI_CCK_TRIG_HIGH        200
+#define ZM_HAL_ANI_CCK_TRIG_LOW         100
+#define ZM_HAL_ANI_NOISE_IMMUNE_LVL     4
+#define ZM_HAL_ANI_USE_OFDM_WEAK_SIG    TRUE
+#define ZM_HAL_ANI_CCK_WEAK_SIG_THR     FALSE
+#define ZM_HAL_ANI_SPUR_IMMUNE_LVL      7
+#define ZM_HAL_ANI_FIRSTEP_LVL          0
+#define ZM_HAL_ANI_RSSI_THR_HIGH        40
+#define ZM_HAL_ANI_RSSI_THR_LOW         7
+#define ZM_HAL_ANI_PERIOD               100
+
+#define ZM_HAL_EP_RND(x, mul) \
+    ((((x)%(mul)) >= ((mul)/2)) ? ((x) + ((mul) - 1)) / (mul) : (x)/(mul))
+
+s32_t BEACON_RSSI(zdev_t* dev)
+{
+    s32_t rssi;
+
+    zmw_get_wlan_dev(dev);
+
+    struct zsHpPriv *HpPriv = (struct zsHpPriv*)wd->hpPrivate;
+
+    rssi = ZM_HAL_EP_RND(HpPriv->stats.ast_nodestats.ns_avgbrssi, ZM_HAL_RSSI_EP_MULTIPLIER);
+
+    return rssi;
+}
+
+/*
+ * Setup ANI handling.  Sets all thresholds and levels to default level AND
+ * resets the channel statistics
+ */
+
+void zfHpAniAttach(zdev_t* dev)
+{
+#define N(a)     (sizeof(a) / sizeof(a[0]))
+    u32_t i;
+
+    zmw_get_wlan_dev(dev);
+
+    struct zsHpPriv *HpPriv = (struct zsHpPriv*)wd->hpPrivate;
+
+    const int totalSizeDesired[] = { -55, -55, -55, -55, -62 };
+    const int coarseHigh[]       = { -14, -14, -14, -14, -12 };
+    const int coarseLow[]        = { -64, -64, -64, -64, -70 };
+    const int firpwr[]           = { -78, -78, -78, -78, -80 };
+
+    for (i = 0; i < 5; i++)
+    {
+        HpPriv->totalSizeDesired[i] = totalSizeDesired[i];
+        HpPriv->coarseHigh[i] = coarseHigh[i];
+        HpPriv->coarseLow[i] = coarseLow[i];
+        HpPriv->firpwr[i] = firpwr[i];
+    }
+
+    /* owl has phy counters */
+    HpPriv->hasHwPhyCounters = 1;
+
+    memset((char *)&HpPriv->ani, 0, sizeof(HpPriv->ani));
+    for (i = 0; i < N(wd->regulationTable.allowChannel); i++)
+    {
+        /* New ANI stuff */
+        HpPriv->ani[i].ofdmTrigHigh = ZM_HAL_ANI_OFDM_TRIG_HIGH;
+        HpPriv->ani[i].ofdmTrigLow = ZM_HAL_ANI_OFDM_TRIG_LOW;
+        HpPriv->ani[i].cckTrigHigh = ZM_HAL_ANI_CCK_TRIG_HIGH;
+        HpPriv->ani[i].cckTrigLow = ZM_HAL_ANI_CCK_TRIG_LOW;
+        HpPriv->ani[i].rssiThrHigh = ZM_HAL_ANI_RSSI_THR_HIGH;
+        HpPriv->ani[i].rssiThrLow = ZM_HAL_ANI_RSSI_THR_LOW;
+        HpPriv->ani[i].ofdmWeakSigDetectOff = !ZM_HAL_ANI_USE_OFDM_WEAK_SIG;
+        HpPriv->ani[i].cckWeakSigThreshold = ZM_HAL_ANI_CCK_WEAK_SIG_THR;
+        HpPriv->ani[i].spurImmunityLevel = ZM_HAL_ANI_SPUR_IMMUNE_LVL;
+        HpPriv->ani[i].firstepLevel = ZM_HAL_ANI_FIRSTEP_LVL;
+        if (HpPriv->hasHwPhyCounters)
+        {
+            HpPriv->ani[i].ofdmPhyErrBase = 0;//AR_PHY_COUNTMAX - ZM_HAL_ANI_OFDM_TRIG_HIGH;
+            HpPriv->ani[i].cckPhyErrBase = 0;//AR_PHY_COUNTMAX - ZM_HAL_ANI_CCK_TRIG_HIGH;
+        }
+    }
+    if (HpPriv->hasHwPhyCounters)
+    {
+        //zm_debug_msg2("Setting OfdmErrBase = 0x", HpPriv->ani[0].ofdmPhyErrBase);
+        //zm_debug_msg2("Setting cckErrBase = 0x", HpPriv->ani[0].cckPhyErrBase);
+        //OS_REG_WRITE(ah, AR_PHY_ERR_1, HpPriv->ani[0].ofdmPhyErrBase);
+        //OS_REG_WRITE(ah, AR_PHY_ERR_2, HpPriv->ani[0].cckPhyErrBase);
+    }
+    HpPriv->aniPeriod = ZM_HAL_ANI_PERIOD;
+    //if (ath_hal_enableANI)
+    HpPriv->procPhyErr |= ZM_HAL_PROCESS_ANI;
+
+    HpPriv->stats.ast_nodestats.ns_avgbrssi = ZM_RSSI_DUMMY_MARKER;
+    HpPriv->stats.ast_nodestats.ns_avgrssi = ZM_RSSI_DUMMY_MARKER;
+    HpPriv->stats.ast_nodestats.ns_avgtxrssi = ZM_RSSI_DUMMY_MARKER;
+#undef N
+}
+
+/*
+ * Control Adaptive Noise Immunity Parameters
+ */
+u8_t zfHpAniControl(zdev_t* dev, ZM_HAL_ANI_CMD cmd, int param)
+{
+#define N(a) (sizeof(a)/sizeof(a[0]))
+    typedef s32_t TABLE[];
+
+    zmw_get_wlan_dev(dev);
+
+    struct zsHpPriv *HpPriv = (struct zsHpPriv*)wd->hpPrivate;
+
+    struct zsAniState *aniState = HpPriv->curani;
+
+    switch (cmd)
+    {
+    case ZM_HAL_ANI_NOISE_IMMUNITY_LEVEL:
+    {
+        u32_t level = param;
+
+        if (level >= N(HpPriv->totalSizeDesired))
+        {
+          zm_debug_msg1("level out of range, desired level : ", level);
+          zm_debug_msg1("max level : ", N(HpPriv->totalSizeDesired));
+          return FALSE;
+        }
+
+        zfDelayWriteInternalReg(dev, AR_PHY_DESIRED_SZ,
+                (HpPriv->regPHYDesiredSZ & ~AR_PHY_DESIRED_SZ_TOT_DES)
+                | ((HpPriv->totalSizeDesired[level] << AR_PHY_DESIRED_SZ_TOT_DES_S)
+                & AR_PHY_DESIRED_SZ_TOT_DES));
+        zfDelayWriteInternalReg(dev, AR_PHY_AGC_CTL1,
+                (HpPriv->regPHYAgcCtl1 & ~AR_PHY_AGC_CTL1_COARSE_LOW)
+                | ((HpPriv->coarseLow[level] << AR_PHY_AGC_CTL1_COARSE_LOW_S)
+                & AR_PHY_AGC_CTL1_COARSE_LOW));
+        zfDelayWriteInternalReg(dev, AR_PHY_AGC_CTL1,
+                (HpPriv->regPHYAgcCtl1 & ~AR_PHY_AGC_CTL1_COARSE_HIGH)
+                | ((HpPriv->coarseHigh[level] << AR_PHY_AGC_CTL1_COARSE_HIGH_S)
+                & AR_PHY_AGC_CTL1_COARSE_HIGH));
+        zfDelayWriteInternalReg(dev, AR_PHY_FIND_SIG,
+                (HpPriv->regPHYFindSig & ~AR_PHY_FIND_SIG_FIRPWR)
+                | ((HpPriv->firpwr[level] << AR_PHY_FIND_SIG_FIRPWR_S)
+                & AR_PHY_FIND_SIG_FIRPWR));
+        zfFlushDelayWrite(dev);
+
+        if (level > aniState->noiseImmunityLevel)
+            HpPriv->stats.ast_ani_niup++;
+        else if (level < aniState->noiseImmunityLevel)
+            HpPriv->stats.ast_ani_nidown++;
+        aniState->noiseImmunityLevel = (u8_t)level;
+        break;
+    }
+    case ZM_HAL_ANI_OFDM_WEAK_SIGNAL_DETECTION:
+    {
+        const TABLE m1ThreshLow   = { 127,   50 };
+        const TABLE m2ThreshLow   = { 127,   40 };
+        const TABLE m1Thresh      = { 127, 0x4d };
+        const TABLE m2Thresh      = { 127, 0x40 };
+        const TABLE m2CountThr    = {  31,   16 };
+        const TABLE m2CountThrLow = {  63,   48 };
+        u32_t on = param ? 1 : 0;
+
+        zfDelayWriteInternalReg(dev, AR_PHY_SFCORR_LOW,
+                (HpPriv->regPHYSfcorrLow & ~AR_PHY_SFCORR_LOW_M1_THRESH_LOW)
+                | ((m1ThreshLow[on] << AR_PHY_SFCORR_LOW_M1_THRESH_LOW_S)
+                & AR_PHY_SFCORR_LOW_M1_THRESH_LOW));
+        zfDelayWriteInternalReg(dev, AR_PHY_SFCORR_LOW,
+                (HpPriv->regPHYSfcorrLow & ~AR_PHY_SFCORR_LOW_M2_THRESH_LOW)
+                | ((m2ThreshLow[on] << AR_PHY_SFCORR_LOW_M2_THRESH_LOW_S)
+                & AR_PHY_SFCORR_LOW_M2_THRESH_LOW));
+        zfDelayWriteInternalReg(dev, AR_PHY_SFCORR,
+                (HpPriv->regPHYSfcorr & ~AR_PHY_SFCORR_M1_THRESH)
+                | ((m1Thresh[on] << AR_PHY_SFCORR_M1_THRESH_S)
+                & AR_PHY_SFCORR_M1_THRESH));
+        zfDelayWriteInternalReg(dev, AR_PHY_SFCORR,
+                (HpPriv->regPHYSfcorr & ~AR_PHY_SFCORR_M2_THRESH)
+                | ((m2Thresh[on] << AR_PHY_SFCORR_M2_THRESH_S)
+                & AR_PHY_SFCORR_M2_THRESH));
+        zfDelayWriteInternalReg(dev, AR_PHY_SFCORR,
+                (HpPriv->regPHYSfcorr & ~AR_PHY_SFCORR_M2COUNT_THR)
+                | ((m2CountThr[on] << AR_PHY_SFCORR_M2COUNT_THR_S)
+                & AR_PHY_SFCORR_M2COUNT_THR));
+        zfDelayWriteInternalReg(dev, AR_PHY_SFCORR_LOW,
+                (HpPriv->regPHYSfcorrLow & ~AR_PHY_SFCORR_LOW_M2COUNT_THR_LOW)
+                | ((m2CountThrLow[on] << AR_PHY_SFCORR_LOW_M2COUNT_THR_LOW_S)
+                & AR_PHY_SFCORR_LOW_M2COUNT_THR_LOW));
+
+        if (on)
+        {
+            zfDelayWriteInternalReg(dev, AR_PHY_SFCORR_LOW,
+                    HpPriv->regPHYSfcorrLow | AR_PHY_SFCORR_LOW_USE_SELF_CORR_LOW);
+        }
+        else
+        {
+            zfDelayWriteInternalReg(dev, AR_PHY_SFCORR_LOW,
+                    HpPriv->regPHYSfcorrLow & ~AR_PHY_SFCORR_LOW_USE_SELF_CORR_LOW);
+        }
+        zfFlushDelayWrite(dev);
+        if (!on != aniState->ofdmWeakSigDetectOff)
+        {
+            if (on)
+                HpPriv->stats.ast_ani_ofdmon++;
+            else
+                HpPriv->stats.ast_ani_ofdmoff++;
+            aniState->ofdmWeakSigDetectOff = !on;
+        }
+        break;
+    }
+    case ZM_HAL_ANI_CCK_WEAK_SIGNAL_THR:
+    {
+        const TABLE weakSigThrCck = { 8, 6 };
+        u32_t high = param ? 1 : 0;
+
+        zfDelayWriteInternalReg(dev, AR_PHY_CCK_DETECT,
+                (HpPriv->regPHYCckDetect & ~AR_PHY_CCK_DETECT_WEAK_SIG_THR_CCK)
+                | ((weakSigThrCck[high] << AR_PHY_CCK_DETECT_WEAK_SIG_THR_CCK_S)
+                & AR_PHY_CCK_DETECT_WEAK_SIG_THR_CCK));
+        zfFlushDelayWrite(dev);
+        if (high != aniState->cckWeakSigThreshold)
+        {
+            if (high)
+                HpPriv->stats.ast_ani_cckhigh++;
+            else
+                HpPriv->stats.ast_ani_ccklow++;
+            aniState->cckWeakSigThreshold = (u8_t)high;
+        }
+        break;
+    }
+    case ZM_HAL_ANI_FIRSTEP_LEVEL:
+    {
+        const TABLE firstep = { 0, 4, 8 };
+        u32_t level = param;
+
+        if (level >= N(firstep))
+        {
+            zm_debug_msg1("level out of range, desired level : ", level);
+            zm_debug_msg1("max level : ", N(firstep));
+            return FALSE;
+        }
+        zfDelayWriteInternalReg(dev, AR_PHY_FIND_SIG,
+                (HpPriv->regPHYFindSig & ~AR_PHY_FIND_SIG_FIRSTEP)
+                | ((firstep[level] << AR_PHY_FIND_SIG_FIRSTEP_S)
+                & AR_PHY_FIND_SIG_FIRSTEP));
+        zfFlushDelayWrite(dev);
+        if (level > aniState->firstepLevel)
+            HpPriv->stats.ast_ani_stepup++;
+        else if (level < aniState->firstepLevel)
+            HpPriv->stats.ast_ani_stepdown++;
+        aniState->firstepLevel = (u8_t)level;
+        break;
+    }
+    case ZM_HAL_ANI_SPUR_IMMUNITY_LEVEL:
+    {
+        const TABLE cycpwrThr1 = { 2, 4, 6, 8, 10, 12, 14, 16 };
+        u32_t level = param;
+
+        if (level >= N(cycpwrThr1))
+        {
+            zm_debug_msg1("level out of range, desired level : ", level);
+            zm_debug_msg1("max level : ", N(cycpwrThr1));
+            return FALSE;
+        }
+        zfDelayWriteInternalReg(dev, AR_PHY_TIMING5,
+                (HpPriv->regPHYTiming5 & ~AR_PHY_TIMING5_CYCPWR_THR1)
+                | ((cycpwrThr1[level] << AR_PHY_TIMING5_CYCPWR_THR1_S)
+                & AR_PHY_TIMING5_CYCPWR_THR1));
+        zfFlushDelayWrite(dev);
+        if (level > aniState->spurImmunityLevel)
+            HpPriv->stats.ast_ani_spurup++;
+        else if (level < aniState->spurImmunityLevel)
+            HpPriv->stats.ast_ani_spurdown++;
+        aniState->spurImmunityLevel = (u8_t)level;
+        break;
+    }
+    case ZM_HAL_ANI_PRESENT:
+        break;
+#ifdef AH_PRIVATE_DIAG
+    case ZM_HAL_ANI_MODE:
+        if (param == 0)
+        {
+            HpPriv->procPhyErr &= ~ZM_HAL_PROCESS_ANI;
+            /* Turn off HW counters if we have them */
+            zfHpAniDetach(dev);
+            //zfHpSetRxFilter(dev, zfHpGetRxFilter(dev) &~ HAL_RX_FILTER_PHYERR);
+        }
+        else
+        {           /* normal/auto mode */
+            HpPriv->procPhyErr |= ZM_HAL_PROCESS_ANI;
+            if (HpPriv->hasHwPhyCounters)
+            {
+                //zfHpSetRxFilter(dev, zfHpGetRxFilter(dev) &~ HAL_RX_FILTER_PHYERR);
+            }
+            else
+            {
+                //zfHpSetRxFilter(dev, zfHpGetRxFilter(dev) | HAL_RX_FILTER_PHYERR);
+            }
+        }
+        break;
+    case ZM_HAL_ANI_PHYERR_RESET:
+        HpPriv->stats.ast_ani_ofdmerrs = 0;
+        HpPriv->stats.ast_ani_cckerrs = 0;
+        break;
+#endif /* AH_PRIVATE_DIAG */
+    default:
+        zm_debug_msg1("invalid cmd ", cmd);
+        return FALSE;
+    }
+    return TRUE;
+#undef  N
+}
+
+void zfHpAniRestart(zdev_t* dev)
+{
+    struct zsAniState *aniState;
+
+    zmw_get_wlan_dev(dev);
+
+    struct zsHpPriv *HpPriv = (struct zsHpPriv*)wd->hpPrivate;
+
+    aniState = HpPriv->curani;
+
+    aniState->listenTime = 0;
+    if (HpPriv->hasHwPhyCounters)
+    {
+        //if (aniState->ofdmTrigHigh > AR_PHY_COUNTMAX)
+        //{
+        //    aniState->ofdmPhyErrBase = 0;
+        //    zm_debug_msg0("OFDM Trigger is too high for hw counters");
+        //}
+        //else
+        //    aniState->ofdmPhyErrBase = AR_PHY_COUNTMAX - aniState->ofdmTrigHigh;
+        //if (aniState->cckTrigHigh > AR_PHY_COUNTMAX)
+        //{
+        //    aniState->cckPhyErrBase = 0;
+        //    zm_debug_msg0("CCK Trigger is too high for hw counters");
+        //}
+        //else
+        //    aniState->cckPhyErrBase = AR_PHY_COUNTMAX - aniState->cckTrigHigh;
+        //zm_debug_msg2("Writing ofdmbase = 0x", aniState->ofdmPhyErrBase);
+        //zm_debug_msg2("Writing cckbase = 0x", aniState->cckPhyErrBase);
+        //OS_REG_WRITE(ah, AR_PHY_ERR_1, aniState->ofdmPhyErrBase);
+        //OS_REG_WRITE(ah, AR_PHY_ERR_2, aniState->cckPhyErrBase);
+        //OS_REG_WRITE(ah, AR_PHY_ERR_MASK_1, AR_PHY_ERR_OFDM_TIMING);
+        //OS_REG_WRITE(ah, AR_PHY_ERR_MASK_2, AR_PHY_ERR_CCK_TIMING);
+        aniState->ofdmPhyErrBase = 0;
+        aniState->cckPhyErrBase = 0;
+    }
+    aniState->ofdmPhyErrCount = 0;
+    aniState->cckPhyErrCount = 0;
+}
+
+void zfHpAniOfdmErrTrigger(zdev_t* dev)
+{
+    struct zsAniState *aniState;
+    s32_t rssi;
+
+    zmw_get_wlan_dev(dev);
+
+    struct zsHpPriv *HpPriv = (struct zsHpPriv*)wd->hpPrivate;
+
+    //HALASSERT(chan != NULL);
+
+    if ((HpPriv->procPhyErr & ZM_HAL_PROCESS_ANI) == 0)
+        return;
+
+    aniState = HpPriv->curani;
+    /* First, raise noise immunity level, up to max */
+    if (aniState->noiseImmunityLevel < ZM_HAL_NOISE_IMMUNE_MAX)
+    {
+        zfHpAniControl(dev, ZM_HAL_ANI_NOISE_IMMUNITY_LEVEL, aniState->noiseImmunityLevel + 1);
+        return;
+    }
+    /* then, raise spur immunity level, up to max */
+    if (aniState->spurImmunityLevel < ZM_HAL_SPUR_IMMUNE_MAX)
+    {
+        zfHpAniControl(dev, ZM_HAL_ANI_SPUR_IMMUNITY_LEVEL, aniState->spurImmunityLevel + 1);
+        return;
+    }
+    rssi = BEACON_RSSI(dev);
+    if (rssi > aniState->rssiThrHigh)
+    {
+        /*
+         * Beacon rssi is high, can turn off ofdm weak sig detect.
+         */
+        if (!aniState->ofdmWeakSigDetectOff)
+        {
+            zfHpAniControl(dev, ZM_HAL_ANI_OFDM_WEAK_SIGNAL_DETECTION, FALSE);
+            zfHpAniControl(dev, ZM_HAL_ANI_SPUR_IMMUNITY_LEVEL, 0);
+            return;
+        }
+        /*
+         * If weak sig detect is already off, as last resort, raise
+         * first step level
+         */
+        if (aniState->firstepLevel < ZM_HAL_FIRST_STEP_MAX)
+        {
+            zfHpAniControl(dev, ZM_HAL_ANI_FIRSTEP_LEVEL, aniState->firstepLevel + 1);
+            return;
+        }
+    }
+    else if (rssi > aniState->rssiThrLow)
+    {
+        /*
+         * Beacon rssi in mid range, need ofdm weak signal detect,
+         * but we can raise firststepLevel
+         */
+        if (aniState->ofdmWeakSigDetectOff)
+            zfHpAniControl(dev, ZM_HAL_ANI_OFDM_WEAK_SIGNAL_DETECTION, TRUE);
+        if (aniState->firstepLevel < ZM_HAL_FIRST_STEP_MAX)
+            zfHpAniControl(dev, ZM_HAL_ANI_FIRSTEP_LEVEL, aniState->firstepLevel + 1);
+        return;
+    }
+    else
+    {
+        /*
+         * Beacon rssi is low, if in 11b/g mode, turn off ofdm
+         * weak sign detction and zero firstepLevel to maximize
+         * CCK sensitivity
+         */
+        if (wd->frequency < 3000)
+        {
+            if (!aniState->ofdmWeakSigDetectOff)
+                zfHpAniControl(dev, ZM_HAL_ANI_OFDM_WEAK_SIGNAL_DETECTION, FALSE);
+            if (aniState->firstepLevel > 0)
+                zfHpAniControl(dev, ZM_HAL_ANI_FIRSTEP_LEVEL, 0);
+            return;
+        }
+    }
+}
+
+void zfHpAniCckErrTrigger(zdev_t* dev)
+{
+    struct zsAniState *aniState;
+    s32_t rssi;
+
+    zmw_get_wlan_dev(dev);
+
+    struct zsHpPriv *HpPriv = (struct zsHpPriv*)wd->hpPrivate;
+
+    //HALASSERT(chan != NULL);
+
+    if ((HpPriv->procPhyErr & ZM_HAL_PROCESS_ANI) == 0)
+        return;
+
+    /* first, raise noise immunity level, up to max */
+    aniState = HpPriv->curani;
+    if (aniState->noiseImmunityLevel < ZM_HAL_NOISE_IMMUNE_MAX)
+    {
+        zfHpAniControl(dev, ZM_HAL_ANI_NOISE_IMMUNITY_LEVEL,
+                 aniState->noiseImmunityLevel + 1);
+        return;
+    }
+    rssi = BEACON_RSSI(dev);
+    if (rssi >  aniState->rssiThrLow)
+    {
+        /*
+         * Beacon signal in mid and high range, raise firsteplevel.
+         */
+        if (aniState->firstepLevel < ZM_HAL_FIRST_STEP_MAX)
+            zfHpAniControl(dev, ZM_HAL_ANI_FIRSTEP_LEVEL, aniState->firstepLevel + 1);
+    }
+    else
+    {
+        /*
+         * Beacon rssi is low, zero firstepLevel to maximize
+         * CCK sensitivity.
+         */
+        if (wd->frequency < 3000)
+        {
+            if (aniState->firstepLevel > 0)
+                zfHpAniControl(dev, ZM_HAL_ANI_FIRSTEP_LEVEL, 0);
+        }
+    }
+}
+
+void zfHpAniLowerImmunity(zdev_t* dev)
+{
+    struct zsAniState *aniState;
+    s32_t rssi;
+
+    zmw_get_wlan_dev(dev);
+
+    struct zsHpPriv *HpPriv = (struct zsHpPriv*)wd->hpPrivate;
+
+    aniState = HpPriv->curani;
+
+    rssi = BEACON_RSSI(dev);
+    if (rssi > aniState->rssiThrHigh)
+    {
+        /*
+         * Beacon signal is high, leave ofdm weak signal detection off
+         * or it may oscillate. Let it fall through.
+         */
+    }
+    else if (rssi > aniState->rssiThrLow)
+    {
+        /*
+         * Beacon rssi in mid range, turn on ofdm weak signal
+         * detection or lower first step level.
+         */
+        if (aniState->ofdmWeakSigDetectOff)
+        {
+            zfHpAniControl(dev, ZM_HAL_ANI_OFDM_WEAK_SIGNAL_DETECTION, TRUE);
+            return;
+        }
+        if (aniState->firstepLevel > 0)
+        {
+            zfHpAniControl(dev, ZM_HAL_ANI_FIRSTEP_LEVEL, aniState->firstepLevel - 1);
+            return;
+        }
+    }
+    else
+    {
+        /*
+         * Beacon rssi is low, reduce first step level.
+         */
+        if (aniState->firstepLevel > 0)
+        {
+            zfHpAniControl(dev, ZM_HAL_ANI_FIRSTEP_LEVEL, aniState->firstepLevel - 1);
+            return;
+        }
+    }
+    /* then lower spur immunity level, down to zero */
+    if (aniState->spurImmunityLevel > 0)
+    {
+        zfHpAniControl(dev, ZM_HAL_ANI_SPUR_IMMUNITY_LEVEL, aniState->spurImmunityLevel - 1);
+        return;
+    }
+    /*
+     * if all else fails, lower noise immunity level down to a min value
+     * zero for now
+     */
+    if (aniState->noiseImmunityLevel > 0)
+    {
+        zfHpAniControl(dev, ZM_HAL_ANI_NOISE_IMMUNITY_LEVEL, aniState->noiseImmunityLevel - 1);
+        return;
+    }
+}
+
+#define CLOCK_RATE 44000    /* XXX use mac_usec or similar */
+/* convert HW counter values to ms using 11g clock rate, goo9d enough
+   for 11a and Turbo */
+
+/*
+ * Return an approximation of the time spent ``listening'' by
+ * deducting the cycles spent tx'ing and rx'ing from the total
+ * cycle count since our last call.  A return value <0 indicates
+ * an invalid/inconsistent time.
+ */
+s32_t zfHpAniGetListenTime(zdev_t* dev)
+{
+    struct zsAniState *aniState;
+    u32_t txFrameCount, rxFrameCount, cycleCount;
+    s32_t listenTime;
+
+    zmw_get_wlan_dev(dev);
+
+    struct zsHpPriv *HpPriv = (struct zsHpPriv*)wd->hpPrivate;
+
+    txFrameCount = 0;//OS_REG_READ(ah, AR_TFCNT);
+    rxFrameCount = 0;//OS_REG_READ(ah, AR_RFCNT);
+    cycleCount = 0;//OS_REG_READ(ah, AR_CCCNT);
+
+    aniState = HpPriv->curani;
+    if (aniState->cycleCount == 0 || aniState->cycleCount > cycleCount)
+    {
+        /*
+         * Cycle counter wrap (or initial call); it's not possible
+         * to accurately calculate a value because the registers
+         * right shift rather than wrap--so punt and return 0.
+         */
+        listenTime = 0;
+        HpPriv->stats.ast_ani_lzero++;
+    }
+    else
+    {
+        s32_t ccdelta = cycleCount - aniState->cycleCount;
+        s32_t rfdelta = rxFrameCount - aniState->rxFrameCount;
+        s32_t tfdelta = txFrameCount - aniState->txFrameCount;
+        listenTime = (ccdelta - rfdelta - tfdelta) / CLOCK_RATE;
+    }
+    aniState->cycleCount = cycleCount;
+    aniState->txFrameCount = txFrameCount;
+    aniState->rxFrameCount = rxFrameCount;
+    return listenTime;
+}
+
+/*
+ * Do periodic processing.  This routine is called from the
+ * driver's rx interrupt handler after processing frames.
+ */
+void zfHpAniArPoll(zdev_t* dev, u32_t listenTime, u32_t phyCnt1, u32_t phyCnt2)
+{
+    struct zsAniState *aniState;
+    //s32_t listenTime;
+
+    zmw_get_wlan_dev(dev);
+
+    struct zsHpPriv *HpPriv = (struct zsHpPriv*)wd->hpPrivate;
+
+    /*
+     * Since we're called from end of rx tasklet, we also check for
+     * AR processing now
+     */
+
+    aniState = HpPriv->curani;
+    //HpPriv->stats.ast_nodestats = *stats;       /* XXX optimize? */
+
+    //listenTime = zfHpAniGetListenTime(dev);
+    //if (listenTime < 0)
+    //{
+    //    HpPriv->stats.ast_ani_lneg++;
+    //    /* restart ANI period if listenTime is invalid */
+    //    zfHpAniRestart(dev);
+    //    return;
+    //}
+    /* XXX beware of overflow? */
+    aniState->listenTime += listenTime;
+
+    if (HpPriv->hasHwPhyCounters)
+    {
+        //u32_t phyCnt1, phyCnt2;
+        u32_t ofdmPhyErrCnt, cckPhyErrCnt;
+
+        /* NB: these are not reset-on-read */
+        //phyCnt1 = 0;//OS_REG_READ(ah, AR_PHY_ERR_1);
+        //phyCnt2 = 0;//OS_REG_READ(ah, AR_PHY_ERR_2);
+        /* XXX sometimes zero, why? */
+        //if (phyCnt1 < aniState->ofdmPhyErrBase ||
+        //    phyCnt2 < aniState->cckPhyErrBase)
+        //{
+        //    if (phyCnt1 < aniState->ofdmPhyErrBase)
+        //    {
+        //        zm_debug_msg2("phyCnt1 = 0x", phyCnt1);
+        //        zm_debug_msg2("resetting counter value to 0x", aniState->ofdmPhyErrBase);
+        //        //OS_REG_WRITE(ah, AR_PHY_ERR_1, aniState->ofdmPhyErrBase);
+        //        //OS_REG_WRITE(ah, AR_PHY_ERR_MASK_1, AR_PHY_ERR_OFDM_TIMING);
+        //    }
+        //    if (phyCnt2 < aniState->cckPhyErrBase)
+        //    {
+        //        zm_debug_msg2("phyCnt2 = 0x", phyCnt2);
+        //        zm_debug_msg2("resetting counter value to 0x", aniState->cckPhyErrBase);
+        //        //OS_REG_WRITE(ah, AR_PHY_ERR_2, aniState->cckPhyErrBase);
+        //        //OS_REG_WRITE(ah, AR_PHY_ERR_MASK_2, AR_PHY_ERR_CCK_TIMING);
+        //    }
+        //    return;     /* XXX */
+        //}
+        /* NB: only use ast_ani_*errs with AH_PRIVATE_DIAG */
+        //ofdmPhyErrCnt = phyCnt1 - aniState->ofdmPhyErrBase;
+        //HpPriv->stats.ast_ani_ofdmerrs += ofdmPhyErrCnt - aniState->ofdmPhyErrCount;
+        //aniState->ofdmPhyErrCount = ofdmPhyErrCnt;
+        ofdmPhyErrCnt = phyCnt1;
+        HpPriv->stats.ast_ani_ofdmerrs += ofdmPhyErrCnt;
+        aniState->ofdmPhyErrCount += ofdmPhyErrCnt;
+
+        //cckPhyErrCnt = phyCnt2 - aniState->cckPhyErrBase;
+        //HpPriv->stats.ast_ani_cckerrs += cckPhyErrCnt - aniState->cckPhyErrCount;
+        //aniState->cckPhyErrCount = cckPhyErrCnt;
+        cckPhyErrCnt = phyCnt2;
+        HpPriv->stats.ast_ani_cckerrs += cckPhyErrCnt;
+        aniState->cckPhyErrCount += cckPhyErrCnt;
+    }
+    /*
+     * If ani is not enabled, return after we've collected
+     * statistics
+     */
+    if ((HpPriv->procPhyErr & ZM_HAL_PROCESS_ANI) == 0)
+        return;
+    if (aniState->listenTime > 5 * HpPriv->aniPeriod)
+    {
+        /*
+         * Check to see if need to lower immunity if
+         * 5 aniPeriods have passed
+         */
+        if (aniState->ofdmPhyErrCount <= aniState->listenTime *
+             aniState->ofdmTrigLow/1000 &&
+            aniState->cckPhyErrCount <= aniState->listenTime *
+             aniState->cckTrigLow/1000)
+            zfHpAniLowerImmunity(dev);
+        zfHpAniRestart(dev);
+    }
+    else if (aniState->listenTime > HpPriv->aniPeriod)
+    {
+        /* check to see if need to raise immunity */
+        if (aniState->ofdmPhyErrCount > aniState->listenTime *
+            aniState->ofdmTrigHigh / 1000)
+        {
+            zfHpAniOfdmErrTrigger(dev);
+            zfHpAniRestart(dev);
+        }
+        else if (aniState->cckPhyErrCount > aniState->listenTime *
+               aniState->cckTrigHigh / 1000)
+        {
+            zfHpAniCckErrTrigger(dev);
+            zfHpAniRestart(dev);
+        }
+    }
+}
diff --git a/drivers/staging/otus/hal/hpani.h b/drivers/staging/otus/hal/hpani.h
new file mode 100644
index 00000000000..96e69af3c68
--- /dev/null
+++ b/drivers/staging/otus/hal/hpani.h
@@ -0,0 +1,420 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include "../80211core/cprecomp.h"
+
+typedef struct {
+	u32_t	ackrcv_bad;
+	u32_t	rts_bad;
+	u32_t	rts_good;
+	u32_t	fcs_bad;
+	u32_t	beacons;
+} ZM_HAL_MIB_STATS;
+
+/*
+ * Per-node statistics maintained by the driver for use in
+ * optimizing signal quality and other operational aspects.
+ */
+typedef struct {
+	u32_t	ns_avgbrssi;	/* average beacon rssi */
+	u32_t	ns_avgrssi;	    /* average data rssi */
+	u32_t	ns_avgtxrssi;	/* average tx rssi */
+} ZM_HAL_NODE_STATS;
+
+#define	ZM_HAL_RSSI_EP_MULTIPLIER	(1<<7)	/* pow2 to optimize out * and / */
+
+struct zsAniStats {
+    u32_t   ast_ani_niup;   /* ANI increased noise immunity */
+    u32_t   ast_ani_nidown; /* ANI decreased noise immunity */
+    u32_t   ast_ani_spurup; /* ANI increased spur immunity */
+    u32_t   ast_ani_spurdown;/* ANI descreased spur immunity */
+    u32_t   ast_ani_ofdmon; /* ANI OFDM weak signal detect on */
+    u32_t   ast_ani_ofdmoff;/* ANI OFDM weak signal detect off */
+    u32_t   ast_ani_cckhigh;/* ANI CCK weak signal threshold high */
+    u32_t   ast_ani_ccklow; /* ANI CCK weak signal threshold low */
+    u32_t   ast_ani_stepup; /* ANI increased first step level */
+    u32_t   ast_ani_stepdown;/* ANI decreased first step level */
+    u32_t   ast_ani_ofdmerrs;/* ANI cumulative ofdm phy err count */
+    u32_t   ast_ani_cckerrs;/* ANI cumulative cck phy err count */
+    u32_t   ast_ani_reset;  /* ANI parameters zero'd for non-STA */
+    u32_t   ast_ani_lzero;  /* ANI listen time forced to zero */
+    u32_t   ast_ani_lneg;   /* ANI listen time calculated < 0 */
+    ZM_HAL_MIB_STATS   ast_mibstats;   /* MIB counter stats */
+    ZM_HAL_NODE_STATS  ast_nodestats;  /* Latest rssi stats from driver */
+};
+
+/*
+ * Per-channel ANI state private to the driver.
+ */
+struct zsAniState {
+    ZM_HAL_CHANNEL c;
+    u8_t    noiseImmunityLevel;
+    u8_t    spurImmunityLevel;
+    u8_t    firstepLevel;
+    u8_t    ofdmWeakSigDetectOff;
+    u8_t    cckWeakSigThreshold;
+
+    /* Thresholds */
+    u32_t   listenTime;
+    u32_t   ofdmTrigHigh;
+    u32_t   ofdmTrigLow;
+    s32_t   cckTrigHigh;
+    s32_t   cckTrigLow;
+    s32_t   rssiThrLow;
+    s32_t   rssiThrHigh;
+
+    u32_t   noiseFloor; /* The current noise floor */
+    u32_t   txFrameCount;   /* Last txFrameCount */
+    u32_t   rxFrameCount;   /* Last rx Frame count */
+    u32_t   cycleCount; /* Last cycleCount (can detect wrap-around) */
+    u32_t   ofdmPhyErrCount;/* OFDM err count since last reset */
+    u32_t   cckPhyErrCount; /* CCK err count since last reset */
+    u32_t   ofdmPhyErrBase; /* Base value for ofdm err counter */
+    u32_t   cckPhyErrBase;  /* Base value for cck err counters */
+    s16_t   pktRssi[2]; /* Average rssi of pkts for 2 antennas */
+    s16_t   ofdmErrRssi[2]; /* Average rssi of ofdm phy errs for 2 ant */
+    s16_t   cckErrRssi[2];  /* Average rssi of cck phy errs for 2 ant */
+};
+
+typedef enum {
+	ZM_HAL_ANI_PRESENT,			/* is ANI support present */
+	ZM_HAL_ANI_NOISE_IMMUNITY_LEVEL,		/* set level */
+	ZM_HAL_ANI_OFDM_WEAK_SIGNAL_DETECTION,	/* enable/disable */
+	ZM_HAL_ANI_CCK_WEAK_SIGNAL_THR,		/* enable/disable */
+	ZM_HAL_ANI_FIRSTEP_LEVEL,			/* set level */
+	ZM_HAL_ANI_SPUR_IMMUNITY_LEVEL,		/* set level */
+	ZM_HAL_ANI_MODE,				/* 0 => manual, 1 => auto */
+	ZM_HAL_ANI_PHYERR_RESET,			/* reset phy error stats */
+} ZM_HAL_ANI_CMD;
+
+#define AR_PHY_COUNTMAX        (3 << 22)         // Max counted before intr
+#define ZM_HAL_PROCESS_ANI     0x00000001  /* ANI state setup */
+#define ZM_RSSI_DUMMY_MARKER	0x127
+
+/* PHY registers in ar5416, related base and register offsets
+   may need to be changed in otus BB */
+#define AR_PHY_BASE     0x1C5800      /* base address of phy regs */
+#define AR_PHY(_n)      (AR_PHY_BASE + ((_n)<<2))
+
+#define AR_PHY_TEST             0x1C5800          /* PHY test control */
+#define PHY_AGC_CLR             0x10000000      /* disable AGC to A2 */
+#define RFSILENT_BB             0x00002000      /* shush bb */
+
+#define AR_PHY_TURBO        0x1C5804      /* frame control register */
+#define AR_PHY_FC_TURBO_MODE        0x00000001  /* Set turbo mode bits */
+#define AR_PHY_FC_TURBO_SHORT       0x00000002  /* Set short symbols to turbo mode setting */
+#define AR_PHY_FC_DYN2040_EN        0x00000004      /* Enable dyn 20/40 mode */
+#define AR_PHY_FC_DYN2040_PRI_ONLY      0x00000008      /* dyn 20/40 - primary only */
+#define AR_PHY_FC_DYN2040_PRI_CH    0x00000010      /* dyn 20/40 - primary ch offset (0=+10MHz, 1=-10MHz)*/
+#define AR_PHY_FC_DYN2040_EXT_CH        0x00000020      /* dyn 20/40 - ext ch spacing (0=20MHz/ 1=25MHz) */
+#define AR_PHY_FC_HT_EN             0x00000040      /* ht enable */
+#define AR_PHY_FC_SHORT_GI_40       0x00000080      /* allow short GI for HT 40 */
+#define AR_PHY_FC_WALSH             0x00000100      /* walsh spatial spreading for 2 chains,2 streams TX */
+#define AR_PHY_FC_SINGLE_HT_LTF1        0x00000200      /* single length (4us) 1st HT long training symbol */
+
+#define AR_PHY_TIMING2      0x1C5810      /* Timing Control 2 */
+#define AR_PHY_TIMING2_USE_FORCE    0x00001000
+#define AR_PHY_TIMING2_FORCE_VAL    0x00000fff
+
+#define AR_PHY_TIMING3      0x1C5814      /* Timing control 3 */
+#define AR_PHY_TIMING3_DSC_MAN  0xFFFE0000
+#define AR_PHY_TIMING3_DSC_MAN_S    17
+#define AR_PHY_TIMING3_DSC_EXP  0x0001E000
+#define AR_PHY_TIMING3_DSC_EXP_S    13
+
+#define AR_PHY_CHIP_ID          0x1C5818      /* PHY chip revision ID */
+#define AR_PHY_CHIP_ID_REV_0    0x80        /* 5416 Rev 0 (owl 1.0) BB */
+#define AR_PHY_CHIP_ID_REV_1    0x81        /* 5416 Rev 1 (owl 2.0) BB */
+
+#define AR_PHY_ACTIVE       0x1C581C      /* activation register */
+#define AR_PHY_ACTIVE_EN    0x00000001  /* Activate PHY chips */
+#define AR_PHY_ACTIVE_DIS   0x00000000  /* Deactivate PHY chips */
+
+#define AR_PHY_RF_CTL2                      0x1C5824
+#define AR_PHY_TX_END_DATA_START  0x000000FF
+#define AR_PHY_TX_END_DATA_START_S  0
+#define AR_PHY_TX_END_PA_ON       0x0000FF00
+#define AR_PHY_TX_END_PA_ON_S       8
+
+
+#define AR_PHY_RF_CTL3                  0x1C5828
+#define AR_PHY_TX_END_TO_A2_RX_ON       0x00FF0000
+#define AR_PHY_TX_END_TO_A2_RX_ON_S     16
+
+#define AR_PHY_ADC_CTL      0x1C582C
+#define AR_PHY_ADC_CTL_OFF_INBUFGAIN    0x00000003
+#define AR_PHY_ADC_CTL_OFF_INBUFGAIN_S  0
+#define AR_PHY_ADC_CTL_OFF_PWDDAC   0x00002000
+#define AR_PHY_ADC_CTL_OFF_PWDBANDGAP   0x00004000 /* BB Rev 4.2+ only */
+#define AR_PHY_ADC_CTL_OFF_PWDADC   0x00008000 /* BB Rev 4.2+ only */
+#define AR_PHY_ADC_CTL_ON_INBUFGAIN 0x00030000
+#define AR_PHY_ADC_CTL_ON_INBUFGAIN_S   16
+
+#define AR_PHY_ADC_SERIAL_CTL       0x1C5830
+#define AR_PHY_SEL_INTERNAL_ADDAC   0x00000000
+#define AR_PHY_SEL_EXTERNAL_RADIO   0x00000001
+
+#define AR_PHY_RF_CTL4                    0x1C5834
+#define AR_PHY_RF_CTL4_TX_END_XPAB_OFF    0xFF000000
+#define AR_PHY_RF_CTL4_TX_END_XPAB_OFF_S  24
+#define AR_PHY_RF_CTL4_TX_END_XPAA_OFF    0x00FF0000
+#define AR_PHY_RF_CTL4_TX_END_XPAA_OFF_S  16
+#define AR_PHY_RF_CTL4_FRAME_XPAB_ON      0x0000FF00
+#define AR_PHY_RF_CTL4_FRAME_XPAB_ON_S    8
+#define AR_PHY_RF_CTL4_FRAME_XPAA_ON      0x000000FF
+#define AR_PHY_RF_CTL4_FRAME_XPAA_ON_S    0
+
+#define AR_PHY_SETTLING     0x1C5844
+#define AR_PHY_SETTLING_SWITCH  0x00003F80
+#define AR_PHY_SETTLING_SWITCH_S    7
+
+#define AR_PHY_RXGAIN       0x1C5848
+#define AR_PHY_RXGAIN_TXRX_ATTEN    0x0003F000
+#define AR_PHY_RXGAIN_TXRX_ATTEN_S  12
+#define AR_PHY_RXGAIN_TXRX_RF_MAX   0x007C0000
+#define AR_PHY_RXGAIN_TXRX_RF_MAX_S 18
+
+#define AR_PHY_DESIRED_SZ   0x1C5850
+#define AR_PHY_DESIRED_SZ_ADC       0x000000FF
+#define AR_PHY_DESIRED_SZ_ADC_S     0
+#define AR_PHY_DESIRED_SZ_PGA       0x0000FF00
+#define AR_PHY_DESIRED_SZ_PGA_S     8
+#define AR_PHY_DESIRED_SZ_TOT_DES   0x0FF00000
+#define AR_PHY_DESIRED_SZ_TOT_DES_S 20
+
+#define AR_PHY_FIND_SIG      0x1C5858
+#define AR_PHY_FIND_SIG_FIRSTEP  0x0003F000
+#define AR_PHY_FIND_SIG_FIRSTEP_S        12
+#define AR_PHY_FIND_SIG_FIRPWR   0x03FC0000
+#define AR_PHY_FIND_SIG_FIRPWR_S         18
+
+#define AR_PHY_AGC_CTL1      0x1C585C
+#define AR_PHY_AGC_CTL1_COARSE_LOW       0x00007F80
+#define AR_PHY_AGC_CTL1_COARSE_LOW_S         7
+#define AR_PHY_AGC_CTL1_COARSE_HIGH      0x003F8000
+#define AR_PHY_AGC_CTL1_COARSE_HIGH_S        15
+
+#define AR_PHY_AGC_CONTROL  0x1C5860      /* chip calibration and noise floor setting */
+#define AR_PHY_AGC_CONTROL_CAL  0x00000001  /* do internal calibration */
+#define AR_PHY_AGC_CONTROL_NF   0x00000002  /* do noise-floor calculation */
+
+#define AR_PHY_CCA              0x1C5864
+#define AR_PHY_MINCCA_PWR       0x1FF00000
+#define AR_PHY_MINCCA_PWR_S     19
+#define AR_PHY_CCA_THRESH62     0x0007F000
+#define AR_PHY_CCA_THRESH62_S   12
+
+#define AR_PHY_SFCORR_LOW    0x1C586C
+#define AR_PHY_SFCORR_LOW_USE_SELF_CORR_LOW  0x00000001
+#define AR_PHY_SFCORR_LOW_M2COUNT_THR_LOW    0x00003F00
+#define AR_PHY_SFCORR_LOW_M2COUNT_THR_LOW_S  8
+#define AR_PHY_SFCORR_LOW_M1_THRESH_LOW  0x001FC000
+#define AR_PHY_SFCORR_LOW_M1_THRESH_LOW_S    14
+#define AR_PHY_SFCORR_LOW_M2_THRESH_LOW  0x0FE00000
+#define AR_PHY_SFCORR_LOW_M2_THRESH_LOW_S    21
+
+#define AR_PHY_SFCORR       0x1C5868
+#define AR_PHY_SFCORR_M2COUNT_THR    0x0000001F
+#define AR_PHY_SFCORR_M2COUNT_THR_S  0
+#define AR_PHY_SFCORR_M1_THRESH  0x00FE0000
+#define AR_PHY_SFCORR_M1_THRESH_S    17
+#define AR_PHY_SFCORR_M2_THRESH  0x7F000000
+#define AR_PHY_SFCORR_M2_THRESH_S    24
+
+#define AR_PHY_SLEEP_CTR_CONTROL    0x1C5870
+#define AR_PHY_SLEEP_CTR_LIMIT      0x1C5874
+#define AR_PHY_SLEEP_SCAL       0x1C5878
+
+#define AR_PHY_PLL_CTL          0x1C587c      /* PLL control register */
+#define AR_PHY_PLL_CTL_40       0xaa        /* 40 MHz */
+#define AR_PHY_PLL_CTL_40_5413  0x04
+#define AR_PHY_PLL_CTL_44       0xab        /* 44 MHz for 11b, 11g */
+#define AR_PHY_PLL_CTL_44_2133  0xeb        /* 44 MHz for 11b, 11g */
+#define AR_PHY_PLL_CTL_40_2133  0xea        /* 40 MHz for 11a, turbos */
+
+#define AR_PHY_RX_DELAY     0x1C5914      /* analog pow-on time (100ns) */
+#define AR_PHY_RX_DELAY_DELAY   0x00003FFF  /* delay from wakeup to rx ena */
+
+#define AR_PHY_TIMING_CTRL4     0x1C5920      /* timing control */
+#define AR_PHY_TIMING_CTRL4_IQCORR_Q_Q_COFF 0x01F   /* Mask for kcos_theta-1 for q correction */
+#define AR_PHY_TIMING_CTRL4_IQCORR_Q_Q_COFF_S   0   /* shift for Q_COFF */
+#define AR_PHY_TIMING_CTRL4_IQCORR_Q_I_COFF 0x7E0   /* Mask for sin_theta for i correction */
+#define AR_PHY_TIMING_CTRL4_IQCORR_Q_I_COFF_S   5   /* Shift for sin_theta for i correction */
+#define AR_PHY_TIMING_CTRL4_IQCORR_ENABLE   0x800   /* enable IQ correction */
+#define AR_PHY_TIMING_CTRL4_IQCAL_LOG_COUNT_MAX 0xF000  /* Mask for max number of samples (logarithmic) */
+#define AR_PHY_TIMING_CTRL4_IQCAL_LOG_COUNT_MAX_S   12  /* Shift for max number of samples */
+#define AR_PHY_TIMING_CTRL4_DO_IQCAL    0x10000     /* perform IQ calibration */
+
+#define AR_PHY_TIMING5      0x1C5924
+#define AR_PHY_TIMING5_CYCPWR_THR1  0x000000FE
+#define AR_PHY_TIMING5_CYCPWR_THR1_S    1
+
+#define AR_PHY_POWER_TX_RATE1   0x1C5934
+#define AR_PHY_POWER_TX_RATE2   0x1C5938
+#define AR_PHY_POWER_TX_RATE_MAX    0x1C593c
+#define AR_PHY_POWER_TX_RATE_MAX_TPC_ENABLE 0x00000040
+
+#define AR_PHY_FRAME_CTL    0x1C5944
+#define AR_PHY_FRAME_CTL_TX_CLIP    0x00000038
+#define AR_PHY_FRAME_CTL_TX_CLIP_S  3
+
+#define AR_PHY_TXPWRADJ     0x1C594C      /* BB Rev 4.2+ only */
+#define AR_PHY_TXPWRADJ_CCK_GAIN_DELTA  0x00000FC0
+#define AR_PHY_TXPWRADJ_CCK_GAIN_DELTA_S    6
+#define AR_PHY_TXPWRADJ_CCK_PCDAC_INDEX 0x00FC0000
+#define AR_PHY_TXPWRADJ_CCK_PCDAC_INDEX_S   18
+
+#define AR_PHY_RADAR_0      0x1C5954      /* radar detection settings */
+#define AR_PHY_RADAR_0_ENA  0x00000001  /* Enable radar detection */
+#define AR_PHY_RADAR_0_INBAND   0x0000003e  /* Inband pulse threshold */
+#define AR_PHY_RADAR_0_INBAND_S 1
+#define AR_PHY_RADAR_0_PRSSI    0x00000FC0  /* Pulse rssi threshold */
+#define AR_PHY_RADAR_0_PRSSI_S  6
+#define AR_PHY_RADAR_0_HEIGHT   0x0003F000  /* Pulse height threshold */
+#define AR_PHY_RADAR_0_HEIGHT_S 12
+#define AR_PHY_RADAR_0_RRSSI    0x00FC0000  /* Radar rssi threshold */
+#define AR_PHY_RADAR_0_RRSSI_S  18
+#define AR_PHY_RADAR_0_FIRPWR   0x7F000000  /* Radar firpwr threshold */
+#define AR_PHY_RADAR_0_FIRPWR_S 24
+
+#define AR_PHY_SWITCH_CHAIN_0     0x1C5960
+#define AR_PHY_SWITCH_COM         0x1C5964
+
+#define AR_PHY_SIGMA_DELTA  0x1C596C      /* AR5312 only */
+#define AR_PHY_SIGMA_DELTA_ADC_SEL  0x00000003
+#define AR_PHY_SIGMA_DELTA_ADC_SEL_S    0
+#define AR_PHY_SIGMA_DELTA_FILT2    0x000000F8
+#define AR_PHY_SIGMA_DELTA_FILT2_S  3
+#define AR_PHY_SIGMA_DELTA_FILT1    0x00001F00
+#define AR_PHY_SIGMA_DELTA_FILT1_S  8
+#define AR_PHY_SIGMA_DELTA_ADC_CLIP 0x01FFE000
+#define AR_PHY_SIGMA_DELTA_ADC_CLIP_S   13
+
+#define AR_PHY_RESTART      0x1C5970      /* restart */
+#define AR_PHY_RESTART_DIV_GC   0x001C0000  /* bb_ant_fast_div_gc_limit */
+#define AR_PHY_RESTART_DIV_GC_S 18
+
+#define AR_PHY_RFBUS_REQ        0x1C597C
+#define AR_PHY_RFBUS_REQ_EN     0x00000001
+
+#define AR_PHY_RX_CHAINMASK     0x1C59a4
+
+#define AR_PHY_EXT_CCA          0x1C59bc
+#define AR_PHY_EXT_MINCCA_PWR   0xFF800000
+#define AR_PHY_EXT_MINCCA_PWR_S 23
+
+#define AR_PHY_HALFGI           0x1C59D0      /* Timing control 3 */
+#define AR_PHY_HALFGI_DSC_MAN   0x0007FFF0
+#define AR_PHY_HALFGI_DSC_MAN_S 4
+#define AR_PHY_HALFGI_DSC_EXP   0x0000000F
+#define AR_PHY_HALFGI_DSC_EXP_S 0
+
+#define AR_PHY_HEAVY_CLIP_ENABLE    0x1C59E0
+
+#define AR_PHY_M_SLEEP      0x1C59f0      /* sleep control registers */
+#define AR_PHY_REFCLKDLY    0x1C59f4
+#define AR_PHY_REFCLKPD     0x1C59f8
+
+/* PHY IQ calibration results */
+#define AR_PHY_IQCAL_RES_PWR_MEAS_I 0x1C5C10  /* power measurement for I */
+#define AR_PHY_IQCAL_RES_PWR_MEAS_Q 0x1C5C14  /* power measurement for Q */
+#define AR_PHY_IQCAL_RES_IQ_CORR_MEAS   0x1C5C18  /* IQ correlation measurement */
+
+#define AR_PHY_CURRENT_RSSI 0x1C5C1c      /* rssi of current frame rx'd */
+
+#define AR_PHY_RFBUS_GRANT       0x1C5C20
+#define AR_PHY_RFBUS_GRANT_EN    0x00000001
+
+#define AR_PHY_MODE     0x1C6200  /* Mode register */
+#define AR_PHY_MODE_AR2133  0x08    /* AR2133 */
+#define AR_PHY_MODE_AR5111  0x00    /* AR5111/AR2111 */
+#define AR_PHY_MODE_AR5112  0x08    /* AR5112*/
+#define AR_PHY_MODE_DYNAMIC 0x04    /* dynamic CCK/OFDM mode */
+#define AR_PHY_MODE_RF2GHZ  0x02    /* 2.4 GHz */
+#define AR_PHY_MODE_RF5GHZ  0x00    /* 5 GHz */
+#define AR_PHY_MODE_CCK     0x01    /* CCK */
+#define AR_PHY_MODE_OFDM    0x00    /* OFDM */
+
+#define AR_PHY_CCK_TX_CTRL  0x1C6204
+#define AR_PHY_CCK_TX_CTRL_JAPAN    0x00000010
+
+#define AR_PHY_CCK_DETECT                           0x1C6208
+#define AR_PHY_CCK_DETECT_WEAK_SIG_THR_CCK          0x0000003F
+#define AR_PHY_CCK_DETECT_WEAK_SIG_THR_CCK_S        0
+#define AR_PHY_CCK_DETECT_ANT_SWITCH_TIME           0x00001FC0 // [12:6] settling time for antenna switch
+#define AR_PHY_CCK_DETECT_ANT_SWITCH_TIME_S         6
+#define AR_PHY_CCK_DETECT_BB_ENABLE_ANT_FAST_DIV    0x2000
+
+#define AR_PHY_GAIN_2GHZ    0x1C620C
+#define AR_PHY_GAIN_2GHZ_RXTX_MARGIN    0x00FC0000
+#define AR_PHY_GAIN_2GHZ_RXTX_MARGIN_S  18
+#define AR_PHY_GAIN_2GHZ_BSW_MARGIN     0x00003C00
+#define AR_PHY_GAIN_2GHZ_BSW_MARGIN_S   10
+#define AR_PHY_GAIN_2GHZ_BSW_ATTEN      0x0000001F
+#define AR_PHY_GAIN_2GHZ_BSW_ATTEN_S    0
+
+#define AR_PHY_CCK_RXCTRL4  0x1C621C
+#define AR_PHY_CCK_RXCTRL4_FREQ_EST_SHORT   0x01F80000
+#define AR_PHY_CCK_RXCTRL4_FREQ_EST_SHORT_S 19
+
+#define AR_PHY_DAG_CTRLCCK  0x1C6228
+#define AR_PHY_DAG_CTRLCCK_EN_RSSI_THR  0x00000200 /* BB Rev 4.2+ only */
+#define AR_PHY_DAG_CTRLCCK_RSSI_THR 0x0001FC00 /* BB Rev 4.2+ only */
+#define AR_PHY_DAG_CTRLCCK_RSSI_THR_S   10     /* BB Rev 4.2+ only */
+
+#define AR_PHY_POWER_TX_RATE3   0x1C6234
+#define AR_PHY_POWER_TX_RATE4   0x1C6238
+
+#define AR_PHY_SCRM_SEQ_XR  0x1C623C
+#define AR_PHY_HEADER_DETECT_XR 0x1C6240
+#define AR_PHY_CHIRP_DETECTED_XR    0x1C6244
+#define AR_PHY_BLUETOOTH    0x1C6254
+
+#define AR_PHY_TPCRG1   0x1C6258  /* ar2413 power control */
+#define AR_PHY_TPCRG1_NUM_PD_GAIN   0x0000c000
+#define AR_PHY_TPCRG1_NUM_PD_GAIN_S 14
+
+#define AR_PHY_TPCRG1_PD_GAIN_1    0x00030000
+#define AR_PHY_TPCRG1_PD_GAIN_1_S  16
+#define AR_PHY_TPCRG1_PD_GAIN_2    0x000C0000
+#define AR_PHY_TPCRG1_PD_GAIN_2_S  18
+#define AR_PHY_TPCRG1_PD_GAIN_3    0x00300000
+#define AR_PHY_TPCRG1_PD_GAIN_3_S  20
+//
+
+#define AR_PHY_ANALOG_SWAP      0xa268
+#define AR_PHY_SWAP_ALT_CHAIN   0x00000040
+
+#define AR_PHY_TPCRG5   0x1C626C /* ar2413 power control */
+#define AR_PHY_TPCRG5_PD_GAIN_OVERLAP   0x0000000F
+#define AR_PHY_TPCRG5_PD_GAIN_OVERLAP_S     0
+#define AR_PHY_TPCRG5_PD_GAIN_BOUNDARY_1    0x000003F0
+#define AR_PHY_TPCRG5_PD_GAIN_BOUNDARY_1_S  4
+#define AR_PHY_TPCRG5_PD_GAIN_BOUNDARY_2    0x0000FC00
+#define AR_PHY_TPCRG5_PD_GAIN_BOUNDARY_2_S  10
+#define AR_PHY_TPCRG5_PD_GAIN_BOUNDARY_3    0x003F0000
+#define AR_PHY_TPCRG5_PD_GAIN_BOUNDARY_3_S  16
+#define AR_PHY_TPCRG5_PD_GAIN_BOUNDARY_4    0x0FC00000
+#define AR_PHY_TPCRG5_PD_GAIN_BOUNDARY_4_S  22
+
+#define AR_PHY_POWER_TX_RATE5   0x1C638C
+#define AR_PHY_POWER_TX_RATE6   0x1C6390
+
+#define AR_PHY_CAL_CHAINMASK    0x1C639C
+
+#define AR_PHY_POWER_TX_SUB     0x1C63C8
+#define AR_PHY_POWER_TX_RATE7   0x1C63CC
+#define AR_PHY_POWER_TX_RATE8   0x1C63D0
+#define AR_PHY_POWER_TX_RATE9   0x1C63D4
diff --git a/drivers/staging/otus/hal/hpfw2.c b/drivers/staging/otus/hal/hpfw2.c
new file mode 100644
index 00000000000..baceb029976
--- /dev/null
+++ b/drivers/staging/otus/hal/hpfw2.c
@@ -0,0 +1,1018 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include "../80211core/cprecomp.h"
+
+const u32_t zcP2FwImage[] = {
+0x0009000B, 0x4F222FE6, 0xDE947FFC, 0xE114D594,
+0x1E13D494, 0x67521E4C, 0xD494D693, 0x37402769,
+0x62528F06, 0x7201D692, 0x60602522, 0x2600C93F,
+0xD7906152, 0x2512611D, 0x264B6652, 0x2562470B,
+0x0009B00D, 0xE60095AC, 0xC84060E2, 0x2F028F03,
+0x8FF93652, 0x7F047601, 0xA05A4F26, 0x4F226EF6,
+0x410BD185, 0xD4850009, 0x0009440B, 0x450BD584,
+0xD7840009, 0xD284E1FF, 0x2712611D, 0xD4835029,
+0xE1FFCB01, 0x1209E501, 0x12112212, 0xE7202452,
+0x4718D57F, 0x2572D27F, 0xD17FE700, 0xD680D47F,
+0xE2012270, 0x24702172, 0xD67E2620, 0x2641E4FF,
+0xD57DE600, 0x666DE104, 0x76016063, 0x4000626D,
+0x8FF83212, 0xD5790545, 0x2520E201, 0xD279D778,
+0x2710E100, 0xE5802212, 0x655C6613, 0x666DD476,
+0x76046763, 0x374C626D, 0x8FF83253, 0xD4732712,
+0xD573E101, 0xD6732410, 0x2542E400, 0xE03AE501,
+0xD272D771, 0xE0390654, 0x27110654, 0x000B4F26,
+0x7FC82211, 0xD76FD16E, 0xDC70DB6F, 0xD271DE70,
+0xD572D471, 0x1F12D672, 0x1F76710C, 0x1FB877FC,
+0x1FEA1FC9, 0x72041F2B, 0xDE6FDC6E, 0x1F13EB10,
+0x1F511F44, 0x1F771F65, 0xD86C1F2C, 0xDD6DD96C,
+0xD26DEA00, 0x89003A22, 0xD1587A01, 0x88016010,
+0x56F98B03, 0x4218E201, 0xD1682622, 0x0009410B,
+0x440BD467, 0xD5670009, 0x0009450B, 0x6010D14C,
+0x8B108801, 0xE650D14B, 0x46186212, 0x8B083266,
+0x56FAD147, 0x2120E200, 0xCB016062, 0x2602A003,
+0x72012710, 0x60822122, 0x89098801, 0xE2C8D15A,
+0x622C6612, 0x89033626, 0x6010D158, 0x8BC88801,
+0x51F76792, 0x217252F6, 0xD6555191, 0x55FB2212,
+0x52FC6462, 0x55612542, 0x2252E400, 0x61436643,
+0x05DE6013, 0x36CC4608, 0x02DE2652, 0xC9036021,
+0x8B028801, 0x720162E2, 0x74012E22, 0x36B3664C,
+0x71048FEE, 0x66C2D147, 0x45286512, 0x265B4518,
+0x60822C62, 0x89018801, 0x0009A168, 0x6272D742,
+0x8B132228, 0xD726D541, 0x6552D441, 0x51436672,
+0x316C365C, 0x27622668, 0x14138D05, 0x6262D63D,
+0xB1A57201, 0xD61E2622, 0x2622E200, 0x52916692,
+0x8B013620, 0x0009A144, 0x6061A06E, 0x001C001C,
+0x001D4020, 0x0000B38E, 0xFFFF0000, 0x12340000,
+0x001E1015, 0x00201278, 0x002018A0, 0x00201922,
+0x0020128C, 0x001C3510, 0x001C3624, 0x001E212C,
+0x0020397C, 0x00203514, 0x00203984, 0x00203990,
+0x0020399C, 0x002039F8, 0x002039FC, 0x002039A4,
+0x002039A5, 0x002039A8, 0x00117700, 0x00203A12,
+0x00203578, 0x001142D8, 0x00203A14, 0x00203A16,
+0x001C3D30, 0x00117718, 0x001C3D00, 0x001C1000,
+0x001C36F8, 0x00117734, 0x001C3684, 0x00117710,
+0x001C3520, 0x00117600, 0x00117740, 0x001C1028,
+0x0020358C, 0x002039AC, 0x7FFFFFFF, 0x00201734,
+0x002032BE, 0x002022E8, 0x00203DC0, 0x002039FA,
+0x00203584, 0x002039EC, 0x001C3D2C, 0x001C36B0,
+0x0020351C, 0x0011775C, 0x8801C90F, 0xA0CF8901,
+0xD17C0009, 0x36206212, 0xD47B8904, 0x2421E200,
+0x2162A0CC, 0x6211D179, 0x89012228, 0x0009A0C3,
+0xE202D775, 0x75016571, 0x3123615D, 0x27518D02,
+0x0009A0BC, 0xD27255F2, 0x62226052, 0x40094019,
+0xC90F4009, 0x8F19880A, 0x52F31F2D, 0x40196022,
+0x40094009, 0x8808C90F, 0xA0A78901, 0x60630009,
+0x51F255F8, 0xE701CB01, 0x2502D263, 0xE1002172,
+0x2211D564, 0x74016452, 0x2542A098, 0x8B3F8805,
+0x602252F3, 0x40094019, 0xC90F4009, 0x8B168802,
+0xE5FFD45D, 0x655D6742, 0x8B102758, 0x6272D75B,
+0x8B0C3260, 0x55F257F8, 0x2762E101, 0xD5522512,
+0xD757E400, 0x62722541, 0xA0777201, 0x52F32722,
+0x40196022, 0x40094009, 0x8805C90F, 0x31B38B6E,
+0xD5508B6C, 0x615257F4, 0x7101E240, 0x64722512,
+0x1F4DD14D, 0x42182419, 0x8B033420, 0x6262D64B,
+0x26227201, 0xE200D640, 0x2621B0AA, 0x0009A056,
+0x3123E220, 0x88038B52, 0x52F38B1E, 0x40196022,
+0x40094009, 0x8803C90F, 0xD7418B16, 0x647251F4,
+0x7401D23D, 0x65122742, 0x1F5DE640, 0x46182529,
+0x8B033560, 0x6262D63B, 0x26227201, 0xE200D62E,
+0x2621B086, 0x0009A010, 0xD738D137, 0xD22A6412,
+0xE5007401, 0x21423A76, 0x22518F06, 0xEA00D634,
+0x72016262, 0x2622B074, 0x2FB2D532, 0x95406652,
+0xD4305BF1, 0x36205241, 0x60618910, 0x8B01C803,
+0x2B22E201, 0x8FF54510, 0x57F15664, 0x6272E1F0,
+0x41284118, 0x2722221B, 0x6BF2A008, 0x6BF2A006,
+0xE200D615, 0xD1152621, 0x2121E200, 0xE20256F5,
+0x42186662, 0x26284228, 0x1F6D8D0C, 0xD61FD11E,
+0x460B6511, 0x2008645D, 0x57F58904, 0x6272D11C,
+0x27222219, 0xD11BE201, 0x66122822, 0x8B012668,
+0x0009AE17, 0x450BD518, 0xD1180009, 0xAE10E600,
+0x07D12160, 0x00203A0C, 0x00203A10, 0x00203A18,
+0x001C3DC0, 0x0011772C, 0x001C3B88, 0x002039F4,
+0x0011773C, 0x00117744, 0x0000F000, 0x00117764,
+0x00117748, 0x00117768, 0x0011776C, 0x01FFFFFF,
+0x0011774C, 0x00203584, 0x001142D8, 0x00114774,
+0xFDFFFFFF, 0x00203DC0, 0x0020246C, 0x002039FA,
+0x2F962F86, 0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6,
+0xD11F7FF4, 0x6212DE1F, 0x67E25411, 0xD41E1F41,
+0x1F722F22, 0x6743D51D, 0x7794D21D, 0x5A425841,
+0x6C726942, 0x6D225B16, 0xE6006052, 0x2502CB20,
+0x7601E540, 0x3253626D, 0x62F28BFB, 0x212255F1,
+0x55F21151, 0x2E52D613, 0x14A21481, 0xD4122492,
+0x11B627C2, 0x674226D2, 0xD911DA10, 0x2A72E801,
+0x1A8C490B, 0x4218E201, 0x7F0C1A2C, 0x6EF64F26,
+0x6CF66DF6, 0x6AF66BF6, 0x000B69F6, 0x000068F6,
+0x001C3B9C, 0x001C3D98, 0x001C3700, 0x001C3500,
+0x001C5960, 0x001C8960, 0x0020358C, 0x001C3D00,
+0x00201610, 0x2F962F86, 0x2FC62FA6, 0x2FE62FD6,
+0x4F124F22, 0x7F884F02, 0xE018DEB2, 0xD4B261E0,
+0x61E30F14, 0x62107101, 0x440BE01C, 0x20080F24,
+0x8F126D03, 0xD4AD1F08, 0x6740DDAD, 0x657CD4AD,
+0x470BD7AD, 0xD2AD0009, 0x621C6120, 0x46086623,
+0x36284608, 0x3D6C4608, 0xE01C1FD8, 0xE58004FC,
+0x604C66E2, 0x3050655C, 0x2D628F17, 0x01FCE018,
+0xDEA3E500, 0x641CA008, 0x6753655D, 0x607037EC,
+0x31DC6153, 0x80147501, 0x3243625D, 0xD49D8BF4,
+0xE200D59D, 0xA27F2421, 0x20082521, 0xE0188B13,
+0xE50001FC, 0xA009DE96, 0x655D641C, 0x32EC6253,
+0x62536722, 0x32DC6672, 0x75041261, 0x3243625D,
+0xA2698BF3, 0x88012D10, 0xE0188B16, 0xE40001FC,
+0x671C2D40, 0x624DDE8A, 0x8B013273, 0x0009A25C,
+0x6DE3644D, 0x7D046243, 0x32EC6643, 0x652236DC,
+0x74086162, 0x2512AFEF, 0x8B198804, 0x01FCE018,
+0x2D70E700, 0x1FD56D1C, 0x627DDE7D, 0x8B0132D3,
+0x0009A242, 0x6173677D, 0x31EC65E3, 0x75046412,
+0x365C6673, 0x61426262, 0x21297708, 0x2412AFED,
+0x8B198805, 0x01FCE018, 0x2D70E700, 0x1FD46D1C,
+0x627DDE6F, 0x8B0132D3, 0x0009A226, 0x6173677D,
+0x31EC65E3, 0x75046412, 0x365C6673, 0x61426262,
+0x212B7708, 0x2412AFED, 0x8B598831, 0x61E6DE67,
+0x61E31F19, 0x64E27104, 0x1F4A6216, 0x1F2B6416,
+0x75E46513, 0x66536712, 0x1F4C7604, 0x64521F7D,
+0xD75F6E66, 0x27E0D25F, 0xDE5F6062, 0xC9013245,
+0x65622E00, 0x4609060A, 0x4609D15C, 0x46094509,
+0x21501F4E, 0xB2B0646D, 0x620D1F6F, 0x8B012228,
+0x0009A1EA, 0xD756DE55, 0x661C61E0, 0x6410D150,
+0x470B654C, 0x7FFC54FF, 0x2FE25EFE, 0x51FE7FFC,
+0x2F12E040, 0x55FBD14F, 0x57FD56FC, 0x04FE410B,
+0xD24D7F08, 0xE11C640D, 0x1D412D10, 0xD44B6522,
+0x67421D52, 0x1D73DE4A, 0xD24A65E2, 0x67221D54,
+0x1D75D249, 0xD2496E22, 0x66221DE6, 0x1D67A1BC,
+0x89018830, 0x0009A08E, 0xE340D538, 0x33FC6156,
+0x23126456, 0x71046153, 0x67521341, 0x13726416,
+0x7EE46E13, 0x65E66212, 0x66E3D731, 0x13246EE2,
+0x760427E0, 0x6062D22F, 0x3255DE2F, 0x2E00C901,
+0x060A6E62, 0xD12D4609, 0x4E094609, 0x13434609,
+0x646D21E0, 0xB2501F5E, 0x620D1F6F, 0x8B012228,
+0x0009A18A, 0xDE25D522, 0x61E06450, 0xD724654C,
+0x470B54FF, 0x7FFC661C, 0x06FEE054, 0x7FFC2F62,
+0xEE4001FE, 0x2F123EFC, 0x55E2D125, 0x57E456E3,
+0x64E2410B, 0xD21C7F08, 0xE11C640D, 0x1D412D10,
+0xD61A6522, 0x67621D52, 0x1D73DE19, 0xD2196EE2,
+0x62221DE4, 0xD2181D25, 0x1D266222, 0x6222D217,
+0x1D27A15A, 0x00117800, 0x00202A18, 0x00203996,
+0x002035BC, 0x00203A7C, 0x002018D0, 0x00203995,
+0x00117804, 0x00203A14, 0x00203A16, 0x00117810,
+0x00203991, 0x10624DD3, 0x00203992, 0x00203993,
+0x00114AA4, 0x00200F68, 0x001C5864, 0x001C6864,
+0x001C7864, 0x001C59BC, 0x001C69BC, 0x001C79BC,
+0x00200FC0, 0x8B048833, 0x470BD7A2, 0xA123EE00,
+0x88282DE0, 0xA0D38901, 0xDE9F0009, 0x62E1E143,
+0x3216E054, 0x0FE68F02, 0x2E21E240, 0x622D62E1,
+0x8B013217, 0x0009A0BC, 0xE50185E1, 0x8B013056,
+0x0009A0B6, 0x2D10E101, 0x64E1B111, 0x06FEE054,
+0x6261E143, 0x3517652D, 0xE6408945, 0x8B0C3563,
+0xE058E41A, 0xE5000F45, 0x72C0E05C, 0x60230F55,
+0x6703C907, 0xA014E060, 0x66530F75, 0x46214621,
+0x46214621, 0x45214621, 0xE0587618, 0x0F654521,
+0xE0034521, 0xE05C2509, 0xE0070F55, 0xE0602209,
+0xE8540F25, 0x858238FC, 0x640D65F3, 0x1844B170,
+0xDD7A8584, 0x85866C0D, 0x610D4C08, 0x410860C3,
+0xE00F0EFE, 0x18154D0B, 0x2E296207, 0x668260C3,
+0x85620FE6, 0x4D0B5185, 0x2E0B600D, 0x548460C3,
+0xB13C0FE6, 0xE05465F3, 0xE5400EFE, 0xE06C62E1,
+0x3653662D, 0x0F668D41, 0xC9036023, 0x40004008,
+0x61036403, 0xD965E070, 0x0F46E5FF, 0xE074655C,
+0x60530F96, 0x6263490B, 0x42214221, 0x42214221,
+0x42006723, 0x4200327C, 0x6C074621, 0x4621E054,
+0x606309FE, 0x4008C903, 0x790630FC, 0x6A036D2D,
+0x65F3E800, 0x64D3B124, 0xE0706EA2, 0x2AE22EC9,
+0x01FE6694, 0x666CE074, 0x470B07FE, 0x2E0B6063,
+0x65F32AE2, 0xB0FA64D3, 0x628D7801, 0x32E3EE06,
+0x7D018FE7, 0x0EFEE054, 0xE05462E1, 0x420006FE,
+0x760C8561, 0x701B302C, 0xE4006103, 0xE70465F3,
+0x68667401, 0x3973694D, 0x8FF92582, 0x65F37504,
+0x641DB0DD, 0x0EFEE054, 0x64E1B09C, 0x0009A054,
+0xD43B56F8, 0xEA01D23B, 0x26A0420B, 0x0009A04C,
+0x06FCE01C, 0x8829606C, 0x5CF88B08, 0xE200D636,
+0x52612C20, 0x642DB04B, 0x0009A03E, 0x666CE681,
+0x8B043060, 0x420BD231, 0xA03554F8, 0xE6820009,
+0x3060666C, 0xD22E8B04, 0x54F8420B, 0x0009A02C,
+0x666CE683, 0x8B0A3060, 0xDA2755F8, 0x2590E900,
+0xD82855A1, 0x2852D628, 0xA01D52A2, 0xE6922620,
+0x3060666C, 0xD2208B08, 0x5C21D824, 0x6CCC52F8,
+0x28C1E600, 0x2260A010, 0x666CE693, 0x8B063060,
+0xD61F59F8, 0xE201EA00, 0xA00529A0, 0xD6162621,
+0xD21DD41C, 0x6562420B, 0x4F067F78, 0x4F264F16,
+0x6DF66EF6, 0x6AF66CF6, 0x000B69F6, 0x4F2268F6,
+0xE240614D, 0x89323123, 0x3127E21F, 0x8B27D713,
+0xD406614D, 0xE00171E0, 0x5671440B, 0x26596507,
+0x1761A025, 0x00200FBC, 0x00117804, 0x00203470,
+0x00203A9C, 0x002018C0, 0x00117800, 0x00115F00,
+0x00116058, 0x0020397C, 0x00203990, 0x00203A1A,
+0x00203A16, 0x00203AB4, 0x002018D0, 0x001C3704,
+0xE001D490, 0x6672440B, 0x26596507, 0x4F262762,
+0x0009000B, 0x614D4F22, 0x3123E240, 0xE21F8912,
+0xD7893127, 0x614D8B08, 0x5671D286, 0x420B71E0,
+0x260BE001, 0x1761A006, 0x6672D282, 0xE001420B,
+0x2762260B, 0x000B4F26, 0xE6400009, 0x46284618,
+0x6252D57E, 0x89FC2268, 0x0009000B, 0x4618E680,
+0xD57A4628, 0x22686252, 0x000B89FC, 0xA0010009,
+0x7201E200, 0x8BFC3242, 0x0009000B, 0x4618E680,
+0xD5734628, 0x22686252, 0x000B8BFC, 0x2FE60009,
+0x7FFC4F22, 0xBFF16E53, 0x61E22F42, 0xE280D66D,
+0x54E11615, 0x16464218, 0x422855E2, 0x57E31657,
+0x16786EF2, 0x26E22E2B, 0x4F267F04, 0x6EF6AFCE,
+0x2FD62FC6, 0x4F222FE6, 0x6C53DD62, 0x6E43BFD6,
+0x2DE2BFBB, 0x0009BFD2, 0x2C1251D5, 0x1C4154D6,
+0x1C5255D7, 0x1C6356D8, 0x6EF64F26, 0x000B6DF6,
+0x61636CF6, 0xA004E600, 0x62564109, 0x24227601,
+0x36127404, 0x000B8BF9, 0xD6530009, 0x8562E500,
+0xA00B674D, 0x655D610D, 0x40006053, 0x305CD44F,
+0x024D4008, 0x3270622D, 0x75018905, 0x3213625D,
+0x000B8BF1, 0x000BE000, 0x2FE6E001, 0x54416743,
+0x4E08EE7F, 0x4E28D246, 0x25E96543, 0x60436E21,
+0x9E7562ED, 0x4529C903, 0xE60032E3, 0x8D456103,
+0x21184509, 0xD23F8B05, 0x002C6053, 0xA08AC93F,
+0x60136603, 0x8B268801, 0x880C6053, 0xD53A8B04,
+0xC93F8453, 0x6603A07F, 0x8B048808, 0x84E2DE36,
+0xA078C93F, 0x880D6603, 0x8B03D633, 0xC93F8461,
+0x6603A071, 0x88096260, 0x622C8F09, 0xE014DE2C,
+0x655C05EC, 0x60233258, 0xA064C93F, 0x60236603,
+0xA060C93F, 0x88026603, 0xE0078B5D, 0x60432509,
+0x8905C810, 0x6053D225, 0xC93F002C, 0x6603A053,
+0x6053DE23, 0xC93F00EC, 0x6603A04D, 0x88016013,
+0x60538B19, 0x8B04880C, 0x8423D21E, 0xA042C93F,
+0x88086603, 0xD51B8B04, 0xC93F8452, 0x6603A03B,
+0xD618880D, 0x84618B03, 0xA034C93F, 0x60606603,
+0xA030C93F, 0x88026603, 0xE0078B2D, 0x60432509,
+0x8923C810, 0x6053DE10, 0xC93F00EC, 0x6603A023,
+0x00000BB8, 0x00203470, 0x001C3704, 0x001C373C,
+0x001C3700, 0x001C370C, 0x00114000, 0x00114008,
+0x001142D8, 0x001142E4, 0x001142E8, 0x001142F5,
+0x001142ED, 0x001142FD, 0x00114309, 0x6053D209,
+0xC93F002C, 0x60136603, 0x8B038802, 0xC8106043,
+0x76028900, 0xC93F6063, 0x40004018, 0x1741240B,
+0x6EF6000B, 0x00114301, 0x0009A16E, 0x2FE62FD6,
+0xDD944F22, 0xA0049EB2, 0xD4930009, 0x420BD293,
+0x62D265D2, 0x8BF822E8, 0x0009A004, 0xD28FD490,
+0x55D1420B, 0x22E852D1, 0xA0048BF8, 0xD48D0009,
+0x420BD28A, 0x52D255D2, 0x8BF822E8, 0x0009A004,
+0xD286D489, 0x55D3420B, 0x22E852D3, 0xA0048BF8,
+0xD4860009, 0x420BD281, 0x52D455D4, 0x8BF822E8,
+0x6EF64F26, 0x6DF6000B, 0x2FD62FC6, 0x4F222FE6,
+0x6E636C73, 0x6D53B01A, 0x64D357F4, 0xB05F65E3,
+0xB07566C3, 0xB0A40009, 0xB0A80009, 0xB0AC0009,
+0xB0AC0009, 0xB0AF0009, 0xB03154F5, 0x6CCD6C03,
+0x4F2660C3, 0x6DF66EF6, 0x6CF6000B, 0x3412D170,
+0xD6700529, 0x2650D770, 0x2742000B, 0x0009A018,
+0x2FD62FC6, 0x4F222FE6, 0x6E636C73, 0x6D53BFEE,
+0x64D357F4, 0xB03365E3, 0xB08D66C3, 0xB00F54F5,
+0x6CCD6C03, 0x4F2660C3, 0x6DF66EF6, 0x6CF6000B,
+0xE503D162, 0xD763D462, 0x21524518, 0x2472000B,
+0xD45FD15E, 0x2162E600, 0x2462000B, 0xBF734F22,
+0xBF73E40A, 0xD25C0009, 0x4118E104, 0xE40AE500,
+0xBF692212, 0xD7592252, 0xCB206072, 0x000B4F26,
+0x4F222702, 0x410BD156, 0xD556E400, 0x4F26452B,
+0xD1552FE6, 0x66126E63, 0x92104418, 0x44084528,
+0x45002629, 0x265B4408, 0x264B4400, 0x21624708,
+0xD14E4708, 0x217227EB, 0x6EF6000B, 0x1FFF03F0,
+0x4F222FE6, 0xE101DE4A, 0xBF3DE40A, 0x67E32E12,
+0xE500776C, 0xE204E130, 0x2752E40A, 0x27522752,
+0x27522752, 0x27522752, 0x27522752, 0x27522752,
+0x27522752, 0x27522752, 0x27522752, 0x27522752,
+0x27222712, 0x27522752, 0x27522752, 0x27522752,
+0x27522752, 0x175ABF18, 0x2E62E600, 0x000B4F26,
+0xD2346EF6, 0xE441E101, 0x000B2212, 0xD1322242,
+0xE605D432, 0x000B2162, 0x000B2462, 0xD2300009,
+0xE40AE601, 0x2262AF00, 0x2FC62FB6, 0x2FE62FD6,
+0x7FFC4F22, 0x6C43DB2B, 0xED0060B2, 0x2B02CB03,
+0xC90360B2, 0x6E03A008, 0x89073DC2, 0xE46460B2,
+0xB07CC903, 0x7D016E03, 0x8BF52EE8, 0x8F043DC2,
+0xD4212FE1, 0x460BD621, 0x62F10009, 0x6023622D,
+0x89FFC801, 0x7F046023, 0x6EF64F26, 0x6CF66DF6,
+0x6BF6000B, 0x001C3B88, 0x00203AC8, 0x002018D0,
+0x00203AD0, 0x00203AD8, 0x00203AE0, 0x00203AE8,
+0x0025E720, 0x00203DBC, 0x00203980, 0x001C5968,
+0x001C3B40, 0x000F8000, 0x001D4004, 0x001C3500,
+0x002015E4, 0x00201610, 0x001C5814, 0x001C59D0,
+0x001C5830, 0x001C6268, 0x001C59A4, 0x001C639C,
+0x001C581C, 0x001C5860, 0x00203AF0, 0x002018C0,
+0x8F014411, 0x6043604B, 0x0009000B, 0x5651D52B,
+0x46286052, 0x306C000B, 0x2FC62FB6, 0x2FE62FD6,
+0x4F124F22, 0xBFF14F02, 0x6B036E43, 0xDD25DC24,
+0x0009BFEC, 0x3C0530B8, 0x4609060A, 0x46014609,
+0x020A3D65, 0x42094209, 0x32E24209, 0x4F068BF0,
+0x4F264F16, 0x6DF66EF6, 0x000B6CF6, 0x2FC66BF6,
+0x2FE62FD6, 0x4F124F22, 0xBFCF4F02, 0x6C036E43,
+0xBFCBDD13, 0x30C80009, 0x060A3D05, 0x46094609,
+0x36E24601, 0x4F068BF5, 0x4F264F16, 0x6DF66EF6,
+0x6CF6000B, 0x4F222FE6, 0xE102DE0B, 0xE403E500,
+0xBFB92E12, 0xE6062E52, 0xE7004618, 0x2E62E403,
+0x4F262E72, 0x6EF6AFB0, 0x0009000B, 0x001C1040,
+0xCCCCCCCD, 0x10624DD3, 0x001D4004, 0x2F962F86,
+0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6, 0xE5007F98,
+0x6453E710, 0x6B534728, 0xEE1ADCBC, 0x6153655D,
+0x315C4108, 0x75014108, 0x6043317C, 0x0F16665D,
+0xED0060B3, 0x21B136E3, 0x81128111, 0x11D28113,
+0x11D411D3, 0x74048FEA, 0xD8B167F2, 0x1871D9B1,
+0x58F12872, 0x1981D1B0, 0x59F22982, 0x5DF45AF3,
+0x54F65EF5, 0x21921191, 0x11A211A3, 0x11D411D5,
+0x11E611E7, 0x11481149, 0xDAA855F7, 0x57F8EE00,
+0x52F9DDA7, 0x64E3D6A7, 0x2A521A51, 0xD8A7D9A6,
+0x2D72EAEF, 0x6AAC2622, 0x6DE36EED, 0x61E34D08,
+0x41083DEC, 0x31EC4D08, 0x60B33D9C, 0x2DB14108,
+0xE05081D1, 0xE79F4108, 0x41084008, 0x81D2677C,
+0x318C60B3, 0x3472E200, 0x1DD281D3, 0xD4931D13,
+0x1D248D01, 0x65D3D48F, 0x7E01B0B2, 0x34A264ED,
+0xDA8C8BDA, 0x68A22FD2, 0x4829DD91, 0x64A22D82,
+0x694D7DFC, 0x2D92D286, 0x4E296E22, 0x2DE27D0C,
+0x6AD36822, 0xD784618D, 0x6D722A16, 0xD583D489,
+0x5E7224D2, 0x14E2D688, 0xEE005174, 0x58761414,
+0x1486D186, 0xE7105978, 0x62521498, 0x142A65E3,
+0x64E326E2, 0x644DE600, 0x48086843, 0x4808384C,
+0x6053381C, 0x28B10C86, 0x60B309CE, 0x60538191,
+0x60430ACE, 0x605381A2, 0x60B30DCE, 0x605381D3,
+0x740108CE, 0x09CE1882, 0x19E3624D, 0x32730ACE,
+0x8FE01A64, 0xD96A7504, 0x6C92E003, 0x2CB14018,
+0xDA6F6D92, 0xE05081D1, 0x40086E92, 0x619281E2,
+0x811360B3, 0xE6006492, 0x67921442, 0x17A3D468,
+0xE1FF6892, 0xE7031864, 0x46086563, 0x7501364C,
+0x665D2612, 0x8BF83673, 0xE003DC5A, 0x40186DC2,
+0x6EC22DB1, 0x81E1D25F, 0xEE0061C2, 0x64C21112,
+0x1423E024, 0xD45B65C2, 0x67C215E4, 0x8172E580,
+0x66E368C2, 0x655C8183, 0x6963666D, 0x6A6D7604,
+0x3A53394C, 0x29E28FF8, 0xDC54DB53, 0x740424B2,
+0x7F6824C2, 0x6EF64F26, 0x6CF66DF6, 0x6AF66BF6,
+0x000B69F6, 0x614268F6, 0xC8036011, 0xE5008F03,
+0x3420D23C, 0x60118B06, 0x8802C903, 0xD2398B06,
+0x8B033420, 0x65135612, 0x24225264, 0x6053000B,
+0x2FE62FD6, 0x7FEC4F22, 0x62536E53, 0x6D43E550,
+0x4508E400, 0xE101A001, 0x60435224, 0x81212211,
+0x60538123, 0x56E28122, 0x8BF53620, 0x16E4D238,
+0xE61464F3, 0x65E3420B, 0xE4FC65E1, 0x2E512549,
+0x65F361F1, 0x2F112149, 0xD13154D1, 0xE614410B,
+0x607157D1, 0x2701CB01, 0x7F141DE1, 0x6EF64F26,
+0x6DF6000B, 0x2FE62FD6, 0x7FEC4F22, 0x66536E53,
+0x6D43E5FC, 0x20596061, 0x2601CB01, 0x326052E2,
+0x12E48B06, 0x31E051E2, 0x52D18B04, 0x1E22A002,
+0x5664AFF0, 0x64F3D21E, 0x420BE614, 0x67E165E3,
+0x2719E1FC, 0x67F12E71, 0x271954D1, 0x65F3D118,
+0x410BE614, 0x52D12F71, 0xCB016021, 0x1DE12201,
+0x4F267F14, 0x000B6EF6, 0x00006DF6, 0x002039AC,
+0x0020357C, 0x00203584, 0x0020358C, 0x002035B4,
+0x00203998, 0x002039A0, 0x00100208, 0x001014C0,
+0x001E210C, 0x001C3D00, 0x002039EC, 0x001000C8,
+0x00117880, 0x00117780, 0x00040020, 0x0026C401,
+0x00200D42, 0x4F222FE6, 0xDE42624C, 0x42004208,
+0x3E2CA005, 0xD4405252, 0xBF695624, 0x65E22E62,
+0x352052E1, 0xD63D8BF6, 0x4F262622, 0x6EF6000B,
+0x2FC62FB6, 0x2FE62FD6, 0xDC394F22, 0x52C1DB39,
+0x362066C2, 0x6061891C, 0x8801C903, 0xDE348918,
+0xBF38DD35, 0x650364E3, 0x66B28503, 0x3262620D,
+0xD4328907, 0x0009BF76, 0x4D0BD431, 0xAFE60009,
+0xBF3D0009, 0xD42F64E3, 0x00094D0B, 0x0009AFDF,
+0x2262D22D, 0x6EF64F26, 0x6CF66DF6, 0x6BF6000B,
+0x2FD62FC6, 0x4F222FE6, 0xDD29DC28, 0x6E4360C2,
+0x04DE4008, 0xE614D127, 0x65E3410B, 0xD127D726,
+0x55E227E2, 0x35E05254, 0x21228F04, 0x400860C2,
+0x122202DE, 0x605365C2, 0x75014008, 0x0DE606DE,
+0xC90F6053, 0x60632C02, 0x6EF64F26, 0x000B6DF6,
+0x85436CF6, 0x650D5643, 0x622D6262, 0x35277204,
+0xE1008F0C, 0x2268960C, 0xD6158B03, 0x72015261,
+0xD6131621, 0x6262E101, 0x26227201, 0x6013000B,
+0x000001FF, 0x0020358C, 0x00203584, 0x001C3D00,
+0x002035B4, 0x0020397C, 0x002018C0, 0x0020357C,
+0x00203B18, 0x00203B1C, 0x001C3D28, 0x002039EC,
+0x002039AC, 0x00200D42, 0x002039F0, 0x002039F4,
+0x00117754, 0x2FA62F96, 0x2FC62FB6, 0x2FE62FD6,
+0x7FF84F22, 0x6C22D241, 0xC80360C3, 0xDE40896E,
+0xDA41DB40, 0x52B1D941, 0x362066B2, 0x60618945,
+0x8801C903, 0xDD3B8941, 0x420BD23D, 0x650364D3,
+0x60A12F02, 0x89328801, 0x85145153, 0x8840600C,
+0x1F118F0C, 0xD5376191, 0x641D450B, 0x8B262008,
+0xD7356691, 0x646D470B, 0x8B202008, 0x420BD233,
+0x51F154F1, 0xC8208511, 0xD1318904, 0x021EE050,
+0x01267201, 0x420BD22F, 0x200864F2, 0x64D38907,
+0x4D0BDD2D, 0xD12D65F2, 0xAFC4E601, 0xD22C2162,
+0x420B65F2, 0xD72B64E3, 0xAFBCE601, 0xD2262762,
+0x420B65F2, 0xAFB664D3, 0xDE270009, 0xDA28DD27,
+0x52D1DB28, 0x362066D2, 0x60618918, 0x8801C903,
+0xD4228914, 0x450BD516, 0x56030009, 0x8F0436E0,
+0xE2016503, 0xAFEC2A20, 0xD41F2B52, 0x420BD216,
+0xD7180009, 0x4118E101, 0x2712AFE3, 0xC80460C3,
+0xD21A8902, 0x0009420B, 0x4F267F08, 0x6DF66EF6,
+0x6BF66CF6, 0x000B6AF6, 0x000069F6, 0x001E2100,
+0x0020358C, 0x00203584, 0x00203A14, 0x001142D8,
+0x002014A6, 0x00115EA2, 0x00114774, 0x00200D8A,
+0x0020351C, 0x002016C2, 0x002014D0, 0x001E212C,
+0x00201534, 0x001C3D30, 0x00117880, 0x0020357C,
+0x0020399C, 0x00203998, 0x002035B4, 0x00200644,
+0xE601D203, 0x1265D503, 0x000B2252, 0x00001266,
+0x001C1010, 0x0000C34F, 0x0009000B, 0x0009000B,
+0x0009000B, 0x0009000B, 0xE000000B, 0xE000000B,
+0x0009000B, 0xE4FDD59D, 0xD69D6152, 0x25122149,
+0x74016052, 0x2502CB01, 0xD19A6752, 0x25722749,
+0xC8406010, 0x60628902, 0x2602CB04, 0xE1F76462,
+0x26422419, 0xE7016062, 0x2602C9CF, 0xE5026062,
+0x2602CB10, 0x47186062, 0x2602CB03, 0x000B1652,
+0xD58D1673, 0xD28ED78D, 0xE100D48E, 0x2511E600,
+0x22102711, 0x2461AFCE, 0xD28B664C, 0x362C4600,
+0xCB106060, 0x2600000B, 0xD287654C, 0x352C4500,
+0xE1EF6650, 0x000B2619, 0x664C2560, 0x4600D283,
+0x6060362C, 0x000BCB10, 0x654C2600, 0x4500D27F,
+0x6650352C, 0x2619E1EF, 0x2560000B, 0xD27A664C,
+0x362C4600, 0xCB086060, 0x2600000B, 0xD276654C,
+0x352C4500, 0xE1F76650, 0x000B2619, 0x664C2560,
+0x4600D272, 0x6060362C, 0x000BCB08, 0x654C2600,
+0x4500D26E, 0x6650352C, 0x2619E1F7, 0x2560000B,
+0xD669624C, 0x326C4200, 0xC9086020, 0x40214021,
+0x000B4021, 0x624C600C, 0x4200D664, 0x6020326C,
+0x4021C908, 0x40214021, 0x600C000B, 0x644CD160,
+0x6240341C, 0x602C000B, 0x644CD15E, 0x6240341C,
+0x602C000B, 0x4F222FE6, 0x645C6E43, 0x3467E60A,
+0xBFEB8914, 0x640C0009, 0x880160EC, 0xE00F8B02,
+0x2409A002, 0x44094409, 0xE60A624C, 0x89053263,
+0x644CBFE2, 0x6023620C, 0x8B00C880, 0x6023E200,
+0x000B4F26, 0x4F226EF6, 0x6062D64B, 0x8B038801,
+0x0009B256, 0x0009A003, 0xE640D248, 0xD6482260,
+0x4F26E200, 0x2622000B, 0xD6434F22, 0x88026062,
+0xB29F8B01, 0xD6420009, 0x4F26E200, 0x2622000B,
+0xD43ED53D, 0xE701E100, 0x000B2512, 0xD23B2470,
+0x000BE604, 0x4F222260, 0xD13BD43A, 0x0009410B,
+0xE1FDD53A, 0xD23A6650, 0xE7002619, 0x4F262560,
+0x2270000B, 0xD5374F22, 0x6152D237, 0x611DD737,
+0x64522512, 0x242BE6FF, 0xD4352542, 0x666DD22E,
+0x2762420B, 0xE1FBD52D, 0x27196750, 0x000B4F26,
+0x4F222570, 0xD128D42F, 0x0009410B, 0xE7F7D527,
+0x26796650, 0x000B4F26, 0xD5242560, 0x62509425,
+0x000B2249, 0xD5212520, 0x6250E4BF, 0x000B2249,
+0x4F222520, 0x8522D224, 0x2008600D, 0x88018911,
+0x88038944, 0x88058946, 0x88068948, 0x8808894E,
+0x88098954, 0x880A895A, 0x880B8960, 0xA06D8966,
+0xB06F0009, 0xA06A0009, 0xFF7F600C, 0x001E2148,
+0x001E1108, 0x001E1000, 0x00203A4C, 0x00203A4E,
+0x00203A6D, 0x00203A30, 0x001E103F, 0x001E105F,
+0x001E102F, 0x001E1090, 0x00203A54, 0x001E100B,
+0x00203A50, 0x00203B20, 0x002018C0, 0x001E1028,
+0x00203A6C, 0x001D4020, 0x98760000, 0x001C1000,
+0x00203B2C, 0x00203B3C, 0x00203A24, 0x0009B04C,
+0x600CA035, 0x0009B055, 0x600CA031, 0x6260D684,
+0x8B2B2228, 0x0009B061, 0x600CA029, 0x6260D680,
+0x8B232228, 0x0009B069, 0x600CA021, 0x6260D67C,
+0x8B1B2228, 0x0009B0C7, 0x600CA019, 0x6260D678,
+0x8B132228, 0x0009B0CD, 0x600CA011, 0x6260D674,
+0x8B0B2228, 0x0009B125, 0x600CA009, 0x6260D670,
+0x8B032228, 0x0009B13D, 0x600CA001, 0x4F26E000,
+0x0009000B, 0xD26CD16B, 0xD56C8412, 0x4000C90F,
+0xD76B012D, 0xE403D66B, 0xE20F611C, 0x2540E001,
+0x25202712, 0x2602000B, 0xE601D262, 0x30668523,
+0xE0008D05, 0xD663D260, 0xE0018122, 0x000B2602,
+0xD25C0009, 0x600D8523, 0x89052008, 0x8B0A8801,
+0x6060D65D, 0x2600CB01, 0xD457D65A, 0xE001E101,
+0x000B2612, 0x000B8142, 0xD152E000, 0x8513E501,
+0x640D4518, 0x66033453, 0xE0008D05, 0xD551D253,
+0x2260E001, 0x000B2502, 0x4F220009, 0x8513D149,
+0x6453650D, 0x62494419, 0x227D672E, 0x8801602C,
+0x88028909, 0x88038910, 0x8806891A, 0x88078935,
+0xA04C893B, 0xD5460009, 0x6652D746, 0x2762D446,
+0x622C6261, 0x2421A038, 0x2228625C, 0xD4438B3F,
+0x6642D540, 0x2562D440, 0x24018561, 0x6203A02C,
+0x2008605C, 0x88108907, 0x88208908, 0x88308909,
+0xA02C890A, 0xD23A0009, 0x6222A008, 0xA005D239,
+0xD2396222, 0x6222A002, 0x6262D638, 0xD432D531,
+0x66212522, 0xA00F626C, 0xD6352421, 0x6261D52D,
+0x622CD42D, 0xA0072562, 0xD6322421, 0x8561D529,
+0x2562D429, 0x62032401, 0x662D8515, 0x3617610D,
+0x65038F01, 0xB0CB2451, 0xA0010009, 0xE000E001,
+0x000B4F26, 0xD6190009, 0xD427E101, 0x65412610,
+0xD118D717, 0xE20F655D, 0x2752E001, 0x000B2620,
+0x2FE62102, 0xD20F4F22, 0x640C8523, 0x8B082448,
+0xD511D61D, 0x2621E200, 0x940F8451, 0xA0482049,
+0xDE0D8051, 0xC84060E0, 0xE2018D32, 0x89443427,
+0xD216D615, 0x2641420B, 0x0009A030, 0x0000FF7F,
+0x00203A6D, 0x00203A24, 0x00203A30, 0x001E1100,
+0x001E100C, 0x00203A50, 0x001E1000, 0x001E1001,
+0x00203A58, 0x00203A38, 0x00203A3C, 0x00203A40,
+0x00203A5C, 0x00203A60, 0x00203A64, 0x00203A68,
+0x00203E20, 0x00203E2A, 0x00203A4A, 0x002027F2,
+0x89123427, 0xD294D693, 0x2641420B, 0xCB8084E1,
+0x80E1B0F5, 0xD69160E0, 0x2E00CB04, 0xC93F6060,
+0xD68F2600, 0xA001E001, 0xE0002602, 0x000B4F26,
+0xD68C6EF6, 0xC8806060, 0xD2868919, 0x88016021,
+0xD2898B15, 0x8524E501, 0x89103056, 0xE203D187,
+0x2120D487, 0xE00B6541, 0x0656655D, 0xE40FD585,
+0x2140E702, 0xD77E2571, 0x000BE001, 0x000B2702,
+0x2FE6E000, 0xDE804F22, 0xC88084E1, 0xD57A892C,
+0x20088554, 0x61038F28, 0x8553D77C, 0x64036672,
+0x8566650C, 0x3520620C, 0xD6798B1E, 0x651CD774,
+0x2651644C, 0x60E02741, 0x8904C840, 0x420BD275,
+0xA0030009, 0xD2680009, 0x0009420B, 0x0009B09F,
+0xE201D167, 0x60E02122, 0xCB04D464, 0x60402E00,
+0x2400C93F, 0x6023A001, 0x4F26E000, 0x6EF6000B,
+0x2FB62FA6, 0x2FD62FC6, 0xDA622FE6, 0x66A1E240,
+0x3622DC5E, 0x62638900, 0x6ED36D2C, 0x4E2136D8,
+0x4E212A61, 0xDB61D460, 0xE700A00F, 0x770162B2,
+0x71026123, 0x66212B12, 0x71026213, 0x61212B12,
+0x651D666D, 0x356C4528, 0x627C2452, 0x8BED32E3,
+0xC90360D3, 0x8B108803, 0x617367B2, 0x2B127102,
+0x71026E13, 0x2B126571, 0x655D6DE1, 0x422862DD,
+0x325CE107, 0xA00C2C10, 0x88022422, 0xA0038B01,
+0x8801E203, 0xE2018B05, 0x66B22C20, 0x655D6561,
+0xE60F2452, 0x67A12C60, 0x8B052778, 0xDD38DC44,
+0xEB01EA00, 0x2DB22CA2, 0x6DF66EF6, 0x6BF66CF6,
+0x6AF6000B, 0x2FE62FD6, 0xE240DD36, 0x362266D1,
+0x62638900, 0x3678672C, 0x7703DE38, 0x47212D61,
+0x64E2D635, 0xA00E4721, 0x6562E100, 0x62537101,
+0x74012450, 0x24204219, 0x45297401, 0x74012450,
+0x24504519, 0x621C7401, 0x8BEE3273, 0x66E24200,
+0x420061D1, 0x2118362C, 0x2E628F06, 0xDD1CD728,
+0xE501E400, 0x2D522742, 0x000B6EF6, 0x2FD66DF6,
+0x4F222FE6, 0xED0AEE01, 0x64E3BC86, 0xBC8B64E3,
+0x62EC7E01, 0x8BF732D7, 0xBC8EEE01, 0x64E364E3,
+0x7E01BC93, 0x32D762EC, 0x4F268BF7, 0x000B6EF6,
+0xD1186DF6, 0xD418920D, 0x72122122, 0x2422D617,
+0xD7177204, 0x72202622, 0x2722D116, 0x000B7230,
+0x137A2122, 0x00203A4A, 0x002028FE, 0x001E1015,
+0x00203A50, 0x001E1001, 0x00203A24, 0x001E1100,
+0x00203A4E, 0x00203A3C, 0x001E1000, 0x00203A40,
+0x00203A4C, 0x002027F2, 0x001E100C, 0x00203A38,
+0x00203A54, 0x00203A58, 0x00203A5C, 0x00203A60,
+0x00203A64, 0x00203A68, 0x4F222FE6, 0xD6707FFC,
+0x88016060, 0xE2018951, 0x2620BFBB, 0xD56ED16D,
+0xDE6E6010, 0x64E36552, 0x7402C840, 0x8D22D16C,
+0xD26C7502, 0xE601D76C, 0xE7042722, 0x76016255,
+0x626C2421, 0x8FF93273, 0xD4637402, 0x6242E601,
+0x640D8528, 0x67494419, 0x275D657E, 0x81E4607C,
+0xE417D562, 0x67557601, 0x3243626C, 0x8FF92171,
+0xA0207102, 0xD25E0009, 0xE601D75B, 0xE7042722,
+0x76016255, 0x626C2421, 0x8FF93273, 0xD4527402,
+0x6242E601, 0x640D8528, 0x67494419, 0x275D657E,
+0x81E4607C, 0xE417D553, 0x67557601, 0x3243626C,
+0x8FF92171, 0x92897102, 0xD2462E21, 0x5E23D74E,
+0x64F22FE2, 0x604365F2, 0x2700C980, 0xC9606043,
+0x80716103, 0xC9036043, 0x80724519, 0x65F2605C,
+0x817266F2, 0x46194629, 0x606C4529, 0x4018645C,
+0x8173304C, 0x21185E23, 0x64F22FE2, 0x6E4C62F2,
+0x602C4219, 0x66F262F2, 0x46294018, 0x461930EC,
+0x42298174, 0x652C606C, 0x305C4018, 0x81758F07,
+0x0009BC97, 0x2228620C, 0xA00A8908, 0x60130009,
+0x8B038840, 0x0009B009, 0x0009A003, 0xE202D62F,
+0x7F042622, 0x000B4F26, 0x4F226EF6, 0x8552D52A,
+0x8830600D, 0x88318903, 0xA0348923, 0x85550009,
+0xD428D727, 0x85532701, 0x610DD627, 0x24124118,
+0x460BD426, 0xD7230009, 0xD226D425, 0x6572420B,
+0xE230D120, 0x42286712, 0x2729E620, 0x37604628,
+0xD6218B03, 0xA016E200, 0xD61F2622, 0xA012E202,
+0xD1182622, 0x6212E530, 0xE6204528, 0x46282259,
+0x89083260, 0xD41AD119, 0xE601D513, 0x2160450B,
+0x472BD718, 0x4F264F26, 0x0009000B, 0x0000060A,
+0x00203A6C, 0x001E1000, 0x00203A58, 0x00203E20,
+0x00203E2C, 0x00203DC4, 0x00203A40, 0x00203DF4,
+0x00203DF2, 0x00203DC6, 0x00203A24, 0x00203A50,
+0x00203A3C, 0x00203A38, 0x002018C0, 0x00203B48,
+0x00203B4C, 0x002018D0, 0x00203A54, 0x001E100B,
+0x00203B60, 0x00114004, 0x4F222FE6, 0x84E9DE86,
+0x2448640C, 0xB17B8901, 0xD2840009, 0x26686620,
+0x60E08902, 0x2E00C9BF, 0x000B4F26, 0x000B6EF6,
+0x2FE60009, 0xDE7E4F22, 0x60E0D67E, 0xCBC0D47E,
+0x62602E00, 0xC803602C, 0x40218904, 0x70014021,
+0x6603A002, 0x66034009, 0xD678616D, 0xE500A004,
+0x75016262, 0x74042422, 0x3213625D, 0xD2748BF8,
+0x0009420B, 0xC9BF84E2, 0x4F2680E2, 0x6EF6000B,
+0x2FE62FD6, 0x7FFC4F22, 0x6260D66E, 0x89402228,
+0xD565E100, 0x60502610, 0xCB40D46B, 0x2500440B,
+0x8D052008, 0x62E06E03, 0x7104612C, 0x2F11A006,
+0xD466D65E, 0xDD666760, 0x657C4D0B, 0xE23C6D1D,
+0x8B033D27, 0xD264D463, 0x0009420B, 0x4D214D21,
+0xA005D762, 0x66E6E400, 0x357C4508, 0x74012562,
+0x35D3654D, 0xD75E8BF7, 0x6E72E003, 0x81E14018,
+0x6E7260F1, 0x81E2700C, 0xD45A6172, 0xDD5A8113,
+0x65724D0B, 0xD64AD259, 0x2212E101, 0xC93F6060,
+0x7F042600, 0x6EF64F26, 0x6DF6000B, 0x2FC62FB6,
+0x2FE62FD6, 0xD2524F22, 0x6B436E73, 0x420B6C53,
+0x20086D63, 0x64038D1C, 0xE50ED13C, 0x32526210,
+0x60C38916, 0x804124B0, 0x814160D3, 0xA007E500,
+0x655D61BC, 0x00EC6053, 0x364C6653, 0x80647501,
+0x3213625D, 0xD6308BF5, 0xC9BF6060, 0x2600A008,
+0xD239D440, 0x6EF64F26, 0x6CF66DF6, 0x6BF6422B,
+0x6EF64F26, 0x6CF66DF6, 0x6BF6000B, 0x2F962F86,
+0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6, 0xE1007FC4,
+0x6513ECFF, 0x6B136CCD, 0xDE34D733, 0xEDFF64F3,
+0xD833EA04, 0x6053655C, 0x027D4000, 0x32C0622D,
+0x66038D0D, 0x09ED6063, 0x2491027D, 0x24217402,
+0x698202ED, 0x3928622D, 0x74022892, 0x75017104,
+0x6063625C, 0x07D532A2, 0x0EB58FE4, 0x2448641C,
+0xE6808905, 0x67F3E5C5, 0xBF8F666C, 0x7F3C655C,
+0x6EF64F26, 0x6CF66DF6, 0x6AF66BF6, 0x000B69F6,
+0xD11C68F6, 0x6012D21C, 0xCB20E405, 0x2102E500,
+0x000B2242, 0x00002252, 0x001E1017, 0x00203996,
+0x001E1015, 0x001E10BF, 0x00117800, 0x001E10FC,
+0x00200644, 0x0020399C, 0x00202A56, 0x00203B64,
+0x002018D0, 0x00203B80, 0x002018C0, 0x0011788C,
+0x00203998, 0x0020357C, 0x00201534, 0x001E2130,
+0x00202A18, 0x00203B88, 0x002039FC, 0x00203A04,
+0x00203DC0, 0x001C3500, 0x001D4004, 0xD564D163,
+0xE400D764, 0x2142E20F, 0x17411154, 0xD5622722,
+0x9669D762, 0x15412572, 0x96661562, 0xE6011565,
+0xD55F1165, 0x666CE6F8, 0x25422542, 0x25422542,
+0x25422542, 0x25622542, 0x7601E727, 0x67632572,
+0x25627797, 0xE7042572, 0x2572E248, 0xE2192522,
+0xE2702522, 0x25422542, 0x25422542, 0x25222542,
+0x2522E20C, 0x25422542, 0x25422542, 0x25422542,
+0x25422542, 0x000B154A, 0xE2081145, 0x0009422B,
+0x2FE62FD6, 0x7FFC4F22, 0xC8206043, 0x6E438D02,
+0x0009BE85, 0xC81060E3, 0xBE828901, 0x60E30009,
+0x8901C840, 0x0009BEA4, 0xC80160E3, 0xDD3D8938,
+0xC80260D0, 0x2F008D03, 0x460BD63B, 0x60F00009,
+0x8902C804, 0x460BD639, 0x62F00009, 0xC8806023,
+0x60D08902, 0x2D00C97F, 0xC8016023, 0xD6348906,
+0x0009460B, 0x0009A007, 0x51630601, 0x8902C808,
+0x460BD630, 0x60F00009, 0x8902C810, 0x420BD22E,
+0xD52E0009, 0x88026052, 0xD22D8B03, 0xA005E604,
+0x88012260, 0xD22A8B02, 0x2260E601, 0x2522E200,
+0xC88060E3, 0xD227892D, 0x60E36E20, 0x8902C880,
+0x420BD225, 0x60E30009, 0x8902C840, 0x420BD223,
+0x60E30009, 0x8902C802, 0x420BD221, 0x60E30009,
+0x890DC804, 0xDD20D11F, 0x0009410B, 0x0009BF11,
+0x0009BF4C, 0xD51ED41D, 0x2470E708, 0x25D2BF85,
+0xC80860E3, 0xD21B8905, 0x4F267F04, 0x422B6EF6,
+0x7F046DF6, 0x6EF64F26, 0x6DF6000B, 0x001C581C,
+0xA000A000, 0x001D0100, 0x001D4000, 0x00040021,
+0x001C589C, 0x001E1021, 0x00201A46, 0x00201A68,
+0x002020C8, 0x00201A80, 0x00201A8E, 0x00203A50,
+0x001E100B, 0x001E1028, 0x00201AFA, 0x00201B06,
+0x00201A96, 0x00201AB4, 0x12345678, 0x001E1000,
+0x0010F100, 0x00201AE2, 0x644CD6A7, 0x000B346C,
+0xD6A62450, 0x346C644C, 0x2450000B, 0x644CD6A4,
+0x000B346C, 0x625C2450, 0x4208616D, 0x42084119,
+0x42006019, 0x670E614C, 0xD49E321C, 0x4200207D,
+0x324CC90F, 0x2200000B, 0x4208625C, 0x42004208,
+0x324C644C, 0x4200D498, 0x000B324C, 0x2FE62260,
+0x614C4F12, 0x4100D493, 0x6710314C, 0xE29F666D,
+0x27294619, 0x6E536269, 0x672E6573, 0x4221227D,
+0x42214221, 0x7601662C, 0xE4014608, 0x34E84608,
+0x644C4600, 0x071A0467, 0x2150257B, 0x000B4F16,
+0x4F226EF6, 0xD2857FE8, 0x88016021, 0xD2848B7B,
+0x26686621, 0xD2838B77, 0x26686621, 0xE50F8B73,
+0xE401BFA2, 0xBFA4E501, 0xE586E400, 0xE400655C,
+0x2F50BFA4, 0xBFA1E401, 0xE602E506, 0x60634618,
+0x81F2E401, 0x6543BF9F, 0xE40185F2, 0xBFAB6543,
+0x85F26603, 0x6543E401, 0x6603BFB1, 0xE40265F0,
+0x6053756C, 0x80F8BF80, 0xBF82E402, 0x84F8E512,
+0x7090E402, 0x6503BF82, 0x4618E602, 0x81F66063,
+0xBF80E402, 0x85F6E500, 0x6603E402, 0xE500BF8C,
+0xE40285F6, 0xBF926603, 0xE5FEE500, 0xE010655C,
+0xBF61E403, 0xE5130F54, 0xE40EBF63, 0x05FCE010,
+0xBF63E40E, 0xE5007585, 0xBF64E403, 0xE500E640,
+0xBF71E403, 0xE500E640, 0xBF78E403, 0xE5FFE640,
+0xE014655C, 0xBF47E404, 0xE40F0F54, 0xE504BF49,
+0x05FCE014, 0xBF49E40F, 0xE5017584, 0xBF4AE640,
+0xE501E404, 0xBF57E640, 0xE501E404, 0xE404E640,
+0xAF5C7F18, 0x7F184F26, 0x000B4F26, 0x4F220009,
+0xD2427FF0, 0x88016021, 0xD2418B71, 0x26686621,
+0xD2408B6D, 0x26686621, 0xE50F8B69, 0xE401BF1C,
+0xBF1EE501, 0xE586E400, 0xE400655C, 0x2F50BF1E,
+0xBF1BE401, 0xE401E506, 0xBF1C6543, 0xE401E640,
+0xBF296543, 0xE401E640, 0xBF306543, 0x65F0E640,
+0x756CE402, 0xBEFF6053, 0xE40280F4, 0xE512BF01,
+0xE40284F4, 0xBF017090, 0xE6406503, 0xBF02E402,
+0xE640E500, 0xBF0FE402, 0xE640E500, 0xBF16E402,
+0xE5FEE500, 0x6053655C, 0xBEE5E403, 0xE51380F8,
+0xE40EBEE7, 0xE40E84F8, 0xBEE77085, 0xE5006503,
+0xBEE8E640, 0xE500E403, 0xBEF5E640, 0xE500E403,
+0xBEFCE640, 0xE5FFE403, 0x6053655C, 0xBECBE404,
+0xE40F80FC, 0xE504BECD, 0xE40F84FC, 0xBECD7083,
+0xE5016503, 0xBECEE640, 0xE501E404, 0xBEDBE640,
+0xE501E404, 0xE404E640, 0xAEE07F10, 0x7F104F26,
+0x000B4F26, 0x00000009, 0x001E102F, 0x001E1080,
+0x001E1090, 0x001E103F, 0x001E103E, 0x00203A4A,
+0x00203A4C, 0x00203A4E, 0xD21DD11C, 0x66206010,
+0x676C7001, 0x3700C90F, 0xE5008D13, 0x67106210,
+0x7701622C, 0x64232170, 0xD6166010, 0x44084408,
+0x3428C90F, 0x62602100, 0x7201D513, 0x44082620,
+0x000B354C, 0xD10F6053, 0x25586510, 0xE6008D13,
+0xD60DD40B, 0x655C6540, 0x47086753, 0x37584708,
+0x47086540, 0x24507501, 0x367C6040, 0x2400C90F,
+0x72FF6210, 0x000B2120, 0x00006063, 0x00203995,
+0x00203994, 0x00203996, 0x002035BC, 0x7FFC4F22,
+0xE680D1A8, 0x666C6212, 0xD2A72F22, 0x67F36563,
+0x420B7542, 0x7F04E404, 0x000B4F26, 0xE6800009,
+0xD2A1666C, 0xE7006563, 0x422B7540, 0xE6806473,
+0xD29D666C, 0xE7006563, 0x422B7543, 0x2F866473,
+0x2FA62F96, 0x2FC62FB6, 0x2FE62FD6, 0x7FC04F22,
+0xDB97D296, 0x72012F22, 0xD1961F21, 0x66125211,
+0x8B013620, 0x0009A0F9, 0xC9036061, 0x8B018801,
+0x0009A0F3, 0xD290DC8F, 0x64C3420B, 0x6503D18F,
+0x60111F02, 0x8B048801, 0x420BD28D, 0xAFE464C3,
+0x54530009, 0x844CEE84, 0x890130E0, 0x0009A0C3,
+0x6610D188, 0x6023626C, 0x8B718801, 0x6210D186,
+0x89662228, 0xDA86D285, 0xE0036122, 0x64221112,
+0x4018D881, 0xDD83E500, 0x814167A3, 0x77042850,
+0x647266A2, 0x6ED3D580, 0x1F457E04, 0x65521F56,
+0x64E368D2, 0x1F8874F8, 0x684369E2, 0x1F637894,
+0x1F991F74, 0x62826142, 0xD779D978, 0x1F2BD679,
+0x67726292, 0x1F1A6062, 0x2602CB20, 0xD176E600,
+0xE5401F57, 0x1F7D1F2C, 0x76011F1E, 0x3253626D,
+0x51F38BFB, 0x52F555F4, 0x25222A12, 0x55F757F6,
+0x27525AF8, 0x5DF92DA2, 0x2ED251FB, 0xD56B5EFA,
+0x54FC24E2, 0x281257FD, 0xD160D869, 0x25722942,
+0x69126782, 0x1974D866, 0xDD666A12, 0x56FE60A1,
+0x2A01CB01, 0xDA646412, 0xE9012842, 0x4A0B2D42,
+0x52FE2692, 0xD661EE01, 0x22E24E18, 0x72016262,
+0x60B22622, 0xCB01D14F, 0x2B02E202, 0x2120A03F,
+0x8B3C2228, 0xE601D55A, 0x2160E700, 0xE01C2572,
+0xC801004C, 0xD8578B0C, 0x1F8FD257, 0xE6002822,
+0x7601E57D, 0x3253626C, 0x56FF8BFB, 0x2622D253,
+0xE2FE69B2, 0x2B922929, 0x0A4CE01E, 0xE01F65F2,
+0x014C25A0, 0x741057F1, 0xEA062710, 0xDD4CE600,
+0x8446DE4C, 0x2D007601, 0x696C6844, 0x2E8039A3,
+0x8FF67E01, 0xDE487D01, 0x2EA0EA94, 0xE1007E01,
+0x7E0F2E10, 0xD12FE205, 0x64102E20, 0x6023624C,
+0x89088801, 0x55F2D22A, 0x64C3420B, 0xEE01D132,
+0xAF1A4E18, 0x55F221E2, 0x8553D13C, 0x620D6612,
+0x89063262, 0xD63BD43A, 0xE801460B, 0xAF0CD73A,
+0xD91F2782, 0x64C3490B, 0xEE01D127, 0xDA38D437,
+0x4A0B4E18, 0xAF0021E2, 0x7F400009, 0x6EF64F26,
+0x6CF66DF6, 0x6AF66BF6, 0x000B69F6, 0x4F2268F6,
+0x85467FF4, 0x2F01E681, 0x666C8547, 0x854881F1,
+0x81F2D209, 0x67F38542, 0x854381F3, 0x81F4E40C,
+0x65636053, 0x420B81F5, 0x7F0C7540, 0x000B4F26,
+0x00000009, 0x001C3D9C, 0x002023FC, 0x0011779A,
+0x001C36F8, 0x002035B4, 0x002014A6, 0x00203A16,
+0x002014D0, 0x002039A5, 0x002039A4, 0x002039A0,
+0x001C3B9C, 0x001C3704, 0x001C3D98, 0x001C3BB4,
+0x001C5960, 0x001C3500, 0x001C3D30, 0x001C8960,
+0x0020358C, 0x001C3D00, 0x00201610, 0x00117730,
+0x002039A8, 0x001C582C, 0x2000A000, 0x0000A000,
+0x0011778C, 0x00117792, 0x00117788, 0x0020397C,
+0x0020357C, 0x00201534, 0x001E2130, 0x00203DA0,
+0x002018C0, 0x2F962F86, 0x2FB62FA6, 0x2FD62FC6,
+0x4F222FE6, 0xD19B7FEC, 0x2F12E000, 0x6103D49A,
+0x1F4281F2, 0xDD9ADA99, 0xD69A6813, 0xE0014808,
+0x460BDE99, 0x38EC4800, 0x65A21F03, 0x352052A1,
+0xA23E8B01, 0x60510009, 0x8801C903, 0xA2388B01,
+0x52530009, 0x32E0DE91, 0xD9918B10, 0x64A3490B,
+0x4B0BDB90, 0xDE906403, 0xD791D690, 0xEC01D591,
+0x2E02E100, 0x271026C0, 0x2502AFDF, 0xC8018551,
+0xA1578B01, 0x62510009, 0x4200622D, 0x5E53366A,
+0x85E2226D, 0xC903642C, 0x85E36603, 0x6053650D,
+0x40214021, 0x4500C93F, 0x322A6703, 0x6053252D,
+0xC901D17F, 0x60106C03, 0x8801D97F, 0xDB7F8B05,
+0x2120E200, 0xCB0160B2, 0xD17D2B02, 0x88016011,
+0x65A28B0A, 0x8D042448, 0x9B9E6251, 0xA00322B9,
+0x919B2521, 0x2521221B, 0x37B3EB10, 0x2448895E,
+0xD4738B07, 0x22286241, 0x60638903, 0xA05781F8,
+0xD5706473, 0x46084608, 0x85E26273, 0x46006B50,
+0x362C4200, 0x2BB8C910, 0x8F1F6463, 0x26686603,
+0xD2698911, 0x062D6043, 0x4119616D, 0x6B0E6019,
+0x81F820BD, 0x880160C3, 0x646C8F2C, 0x880F6073,
+0xA0278B1B, 0xD2610009, 0x052D6043, 0x4119615D,
+0x670E6019, 0x645C207D, 0x81F8A01C, 0x890F2668,
+0x6043D25B, 0x6B5D052D, 0x60B94B19, 0x201D610E,
+0x60C381F8, 0x8F0D8801, 0x6473645C, 0xEC00A00A,
+0x6043D254, 0x625D052D, 0x60294219, 0x207D670E,
+0x81F8645C, 0x880285F8, 0x85E1890A, 0x8D07C820,
+0xE6DC6203, 0x60232269, 0x81E1A002, 0x644CE4FF,
+0x6210D149, 0x89012228, 0x644CE4FF, 0x654DEBFF,
+0x35B06BBC, 0xDB368B2B, 0x64A34B0B, 0x410BD135,
+0x54036403, 0x85446E03, 0xC948DB40, 0xDC408808,
+0xBEAC8B01, 0x64B3E502, 0x65E34C0B, 0xDB3DEC01,
+0xD13D2DC2, 0x621260B2, 0x72017001, 0x21228805,
+0x2B028F08, 0x666CE680, 0x6563D238, 0x7549E700,
+0x6473420B, 0xA030D436, 0x7FFF0009, 0x85E28000,
+0x20B9EBFC, 0x610381E2, 0x942A85E3, 0x62032049,
+0x450885F8, 0x81E2201B, 0xC90160C3, 0x40084018,
+0x40084008, 0x4000225B, 0x6023220B, 0x85E481E3,
+0x4118E108, 0x81E4201B, 0xE40262A2, 0x20B98521,
+0x67A28121, 0xCB016071, 0x85F82701, 0x89033042,
+0xECE785E2, 0x81E220C9, 0x490BD41E, 0xA03B0009,
+0x7E030009, 0x001C3D30, 0x00203DAC, 0x0020358C,
+0x001E212C, 0x00203470, 0x001C3D00, 0x00117780,
+0x002014A6, 0x00201670, 0x0011770C, 0x002039A4,
+0x002039A5, 0x002039A0, 0x002018C0, 0x001C36F8,
+0x00203A1A, 0x00203DBC, 0x00203BA0, 0x00203C20,
+0x00203CA0, 0x00203D20, 0x00203990, 0x00203584,
+0x002014D0, 0x00203A1C, 0x00203A20, 0x002023FC,
+0x00203DA4, 0x00203DA8, 0x602262F2, 0x40094019,
+0xC90F4009, 0x8B0B880A, 0x60E2DE8C, 0x40094019,
+0xC90F4009, 0x8B038808, 0xCB0160A2, 0x2802A006,
+0x65E2DE87, 0x2E527501, 0x286266A2, 0x52F366F2,
+0x2622AE83, 0xD2838551, 0xDE83C802, 0xA0958B01,
+0x420B0009, 0x4E0B64A3, 0x5E036403, 0x85E46503,
+0x4918E908, 0xD77D209B, 0xE04C81E4, 0xDC7C0B7E,
+0x7B01D97C, 0x61C207B6, 0x71016690, 0x8D062668,
+0xD4792C12, 0x420BD279, 0xA070EB01, 0x62512DB2,
+0x4B18EB0F, 0x22B9E102, 0x32104118, 0x85518B0F,
+0x2029E2FC, 0x60518151, 0xCB0172E0, 0x85E12501,
+0x202994A3, 0x85E481E1, 0xA0522049, 0x675181E4,
+0x4719677D, 0x667E6779, 0x7701276D, 0x6903607C,
+0x88014918, 0x25918F3E, 0x6B12D161, 0x21B27B01,
+0x660D85E3, 0x40216063, 0xC93F4021, 0x6C034600,
+0x262D322A, 0xC8016063, 0xDB5ED15D, 0x967D8901,
+0xE6002C6B, 0x666C67CD, 0x40006063, 0x622D021D,
+0x8D0E3270, 0x60436403, 0xE9FF021D, 0x8B013290,
+0x01C5A007, 0x626C7601, 0x3292E904, 0x646C8BEB,
+0x60434400, 0xD15004BD, 0x0B457401, 0x669D6911,
+0x89073670, 0x602D6211, 0x890388FF, 0xE201DB4B,
+0x2B2021C1, 0xECFC8551, 0x815120C9, 0xCB016051,
+0xDC472501, 0x64A34C0B, 0x51F366F2, 0x85EF2612,
+0x54F2D244, 0x650D420B, 0x0009ADE7, 0xE500DC42,
+0x420B2C52, 0x4E0B64A3, 0x54036403, 0x85446E03,
+0x6703E908, 0x65034918, 0x27998541, 0xDB323790,
+0x8F0BD932, 0x6013610D, 0x8B07C820, 0xC9486053,
+0x8B038808, 0xE501BD4B, 0x0009A005, 0x2128D233,
+0xBD448901, 0x64B3E500, 0x490B65E3, 0xADBCEC01,
+0x85F22DC2, 0x7001EE04, 0x31E7610D, 0x8D0281F2,
+0xADA97A08, 0x7F140009, 0x6EF64F26, 0x6CF66DF6,
+0x6AF66BF6, 0x000B69F6, 0xF7FF68F6, 0x2FE68000,
+0xD2234F22, 0x60E36E22, 0x8D02C840, 0xBBE522E2,
+0xE2400009, 0x2E284218, 0xBBF08901, 0x60E30009,
+0x8905C810, 0xD21CD41B, 0x0009420B, 0x0009BBEF,
+0xC80560E3, 0xBD6D8901, 0x60E30009, 0x8902C802,
+0xABEC4F26, 0x4F266EF6, 0x6EF6000B, 0x001C3D3C,
+0x00117760, 0x002014A6, 0x00201670, 0x0020351C,
+0x00203DC0, 0x00203990, 0x00203584, 0x002014D0,
+0x002039FC, 0x00203A04, 0x002039F8, 0x002039FA,
+0x00201534, 0x002018D0, 0x00203A1C, 0x00008000,
+0x001C3510, 0x00203DB4, 0x002018C0, 0x89014F22,
+0x611B600B, 0x611BB00A, 0x000B4F26, 0x600B600B,
+0x611BA004, 0x8DF12107, 0x8BF84011, 0x620D2F26,
+0x8F3E3020, 0x40180019, 0x8B0B3016, 0x31043104,
+0x31043104, 0x31043104, 0x31043104, 0x412462F6,
+0x601C000B, 0x41296219, 0x20084018, 0x31048926,
+0x31043104, 0x31043104, 0x31043104, 0x31043104,
+0x31043104, 0x31043104, 0x31043104, 0x61193104,
+0x3204221D, 0x32043204, 0x32043204, 0x32043204,
+0x32043204, 0x32043204, 0x32043204, 0x32043204,
+0x212D3204, 0x601962F6, 0x4024000B, 0x000BE000,
+0x621362F6, 0x41294228, 0x31044224, 0x31044224,
+0x31044224, 0x31044224, 0x31044224, 0x31044224,
+0x31044224, 0x31044224, 0x31044224, 0x31044224,
+0x31044224, 0x31044224, 0x31044224, 0x31044224,
+0x31044224, 0x31044224, 0x602D4224, 0x62F6000B,
+0x080A0C0E, 0x00020406, 0x1A1C1E20, 0x12141618,
+0x2E303234, 0x26282A2C, 0x3A3C3E40, 0x6C625648,
+0x41112F26, 0xE2208F18, 0x890B3123, 0x321CD204,
+0xD1026220, 0x412B312C, 0x00090009, 0x0020349A,
+0x00203450, 0x000BE000, 0x400062F6, 0x40004000,
+0x40004000, 0x40004000, 0x62F6000B, 0x40004000,
+0x40004000, 0x40004000, 0x40184000, 0x62F6000B,
+0x40004000, 0x40004000, 0x40004000, 0x40284000,
+0x62F6000B, 0x40004000, 0x40184000, 0x000B4028,
+0xC90F62F6, 0x40054005, 0x40054005, 0x62F6000B,
+0x4005C907, 0x40054005, 0x62F6000B, 0x4005C903,
+0x000B4005, 0xC90162F6, 0x000B4005, 0x000062F6,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x42707372,
+0x3D206675, 0x554E203D, 0x202C4C4C, 0x6E49677A,
+0x4E497274, 0x6D754E51, 0x0000003D, 0x61766E49,
+0x2064696C, 0x72657375, 0x20726F20, 0x2079656B,
+0x00214449, 0x6E6B6E55, 0x206E776F, 0x6D6D6F63,
+0x3D646E61, 0x00000000, 0x203A3051, 0x00000020,
+0x203A3151, 0x00000020, 0x203A3251, 0x00000020,
+0x203A3351, 0x00000020, 0x203A3451, 0x00000020,
+0x2B434741, 0x73696F4E, 0x61432065, 0x7262696C,
+0x6F697461, 0x6166206E, 0x6F206C69, 0x6974206E,
+0x0D0A656D, 0x00000000, 0x00000072, 0x00205220,
+0x62735576, 0x7473725F, 0x00000A0D, 0x62735576,
+0x7375735F, 0x646E6570, 0x00000A0D, 0x62735576,
+0x7365725F, 0x000A0D6D, 0x00000044, 0x44387570,
+0x72637365, 0x6F747069, 0x3D584572, 0x00000000,
+0x00000047, 0x72746E49, 0x6D652051, 0x2C797470,
+0x49677A20, 0x4972746E, 0x754E514E, 0x00003D6D,
+0x654C7245, 0x0000006E, 0x20746F4E, 0x756F6E65,
+0x49206867, 0x4220514E, 0x0A0D6675, 0x00000000,
+0x000000FF, 0x00020001, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x010E010D, 0x00020003, 0x01090108, 0x0002010A,
+0x02000003, 0x02020201, 0x02040203, 0x02060205,
+0x02020200, 0x02040203, 0x020C020B, 0x020E020D,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x000000FF, 0x00020001, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x010E010D, 0x00020003, 0x01090108, 0x0002010A,
+0x00030003, 0x02020201, 0x02040203, 0x02060205,
+0x02020200, 0x02040203, 0x020C020B, 0x020E020D,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x010E010D, 0x00FF010F, 0x01090108, 0x010B010A,
+0x0200010F, 0x02020201, 0x02040203, 0x02060205,
+0x02020200, 0x02040203, 0x020C020B, 0x020E020D,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x010E010D, 0x00FF010F, 0x01090108, 0x010B010A,
+0x010F010F, 0x02020201, 0x02040203, 0x02060205,
+0x02020200, 0x02040203, 0x020C020B, 0x020E020D,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00205220, 0x00000046, 0x00000059, 0x73204142,
+0x003D7165, 0x49544120, 0x0000204D, 0x00000000,
+0x00000000, 0x002E0209, 0x80000101, 0x000409FA,
+0x00FF0400, 0x05070000, 0x02000201, 0x82050700,
+0x00020002, 0x03830507, 0x07010040, 0x40030405,
+0x02090100, 0x0101002E, 0x09FA8000, 0x04000004,
+0x000000FF, 0x02010507, 0x07000040, 0x40028205,
+0x05070000, 0x00400383, 0x04050701, 0x00004002,
+0x00000000, 0x00000000, 0x07090000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, };
+
+const u32_t zcP2FwImageSize=15964;
diff --git a/drivers/staging/otus/hal/hpfwbu.c b/drivers/staging/otus/hal/hpfwbu.c
new file mode 100644
index 00000000000..f60f57ed887
--- /dev/null
+++ b/drivers/staging/otus/hal/hpfwbu.c
@@ -0,0 +1,5269 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include "../80211core/cprecomp.h"
+
+const u32_t zcFwBufImage[] = {
+0x3A4BCF18, 0xF44C076E, 0xF59452EA, 0x451BA755,
+0x140AC87A, 0xC07EE942, 0x3EF978AB, 0xF5B03DC6,
+0xB70080F0, 0xA89064EA, 0x54E2C1D6, 0xEB047DF4,
+0x1798390C, 0x00350624, 0x35B3ECF0, 0x59F2FABF,
+0xAF9D248E, 0xF9BDB9F0, 0xD05C8B47, 0xC08B5A16,
+0x990093C7, 0xD335A160, 0x1C04942C, 0xBF6E7A88,
+0xFD232B0F, 0x5C224387, 0xBF1E156C, 0xF24F2A27,
+0xFF56421D, 0xB213037C, 0x2BA67BA0, 0x4950CF8A,
+0x05F00F25, 0xA5E82085, 0x74168A0C, 0x2F2AB30B,
+0xC80C57EE, 0xB6BDF570, 0x89BC5A99, 0x7F3B5A67,
+0xF6C943B8, 0x0C9C9201, 0xE8383747, 0x0C9A72D6,
+0xE0520704, 0xA66D7F30, 0xE444A434, 0xE0C94AB7,
+0x8DD7751C, 0x1A659464, 0x6C9ABA4F, 0x792F2D2D,
+0x5936F66B, 0x061E580E, 0x59903F6C, 0x1FBFB8A0,
+0xCC822EFE, 0x4B030CAF, 0xB62457C9, 0x27E9BF15,
+0xB113A487, 0xFA0FC915, 0x447B184A, 0x5330CD51,
+0x00BCC622, 0xF30DE149, 0xFF718E1C, 0x7B5D2861,
+0xDBCA573E, 0xFB0D7BF9, 0xE1CFBAAC, 0xF99D4583,
+0x4BA7498A, 0x7CAEA7EB, 0xBA958E32, 0x36C530FF,
+0x8F88CA99, 0xF93CABC2, 0x8E47EF11, 0xFB0EED6F,
+0x5B3668A1, 0x9D63ADDE, 0xA0EEAB8C, 0x084915F1,
+0xFACAAA27, 0x209638FE, 0x1CED9EFF, 0xEEBD2335,
+0x38C6F424, 0x2D1F3D7E, 0x976E8106, 0xBE087AD2,
+0x32194845, 0x756066DB, 0xC70E3165, 0xC568DCB1,
+0x3212E4E1, 0xB5D991AD, 0x07C3CEF8, 0xDB4ABB38,
+0x1574C232, 0xF8C792BC, 0x14E62DBE, 0x5A48E7DC,
+0xEFDC5407, 0xC45B4017, 0x3B814E89, 0xF0936466,
+0x89491B2B, 0x9A359A41, 0x82287675, 0xA0F338D3,
+0x523FDD3C, 0x4E40B795, 0x458ADAA4, 0xED812957,
+0x7ADC73BC, 0x6FD7DB78, 0x2740FC04, 0x6392AEA3,
+0x185ABCEA, 0x6B50ABC3, 0x3681F07F, 0xC840F8CE,
+0x5733E7EC, 0x0805FA71, 0x0B34530A, 0x8CB3D033,
+0x81451551, 0x53B0B4EC, 0x908646D0, 0x10A3E642,
+0xF358DC34, 0xC1FA570C, 0x2B1284B0, 0x592322BB,
+0x9D587783, 0xE7D77988, 0xE1BE5D7B, 0x44B93E23,
+0xF8BE94A2, 0x506DC723, 0x6E0A7D09, 0x3FB1046F,
+0xDB3A166F, 0x9CB7D6A0, 0xE278DE6D, 0x88459334,
+0xB52BA3C9, 0x284740A2, 0x04D30792, 0x944D79CA,
+0x1D050EA9, 0xA404DB1B, 0x99526023, 0xBACE24E7,
+0xB9F20704, 0x284E6432, 0x47A593D1, 0x95F8DFCB,
+0x220C9167, 0x8FAABBBC, 0x93D34E8C, 0xCE077138,
+0x4FC18081, 0xE76DD7E5, 0x67465F6C, 0x7A479D77,
+0x74D61F82, 0x00559214, 0x2F66E42E, 0x8742A96B,
+0x62063950, 0xA2DBFAE5, 0x368B966F, 0xAB5FCCE1,
+0xCB4023B1, 0x1E7AF542, 0x05953E30, 0x8CA51CFC,
+0x2216547D, 0x29D562D4, 0xE9C9F8EE, 0xA90505C9,
+0x088D0EEB, 0xD7A290FA, 0x95E5B567, 0x53FAD3C0,
+0xB89FC625, 0x69A7519B, 0x3687C7EF, 0x7188CB55,
+0xCE5DB97E, 0xA260574A, 0xD453D173, 0x145D970B,
+0x12112CC6, 0x399839E0, 0x29C55BEB, 0xE467C71F,
+0x10B3C9D4, 0x8F1C9662, 0xF207A826, 0xE0245600,
+0x688B1812, 0x5A483031, 0x7048380A, 0x78E3D5BB,
+0x1951533D, 0x8FA5D8E3, 0xC5BE500D, 0x71DB5B2B,
+0xA17AA000, 0x408C9BE8, 0x161E12F5, 0xB1C38C45,
+0x22A88F05, 0xDE3F4405, 0x5078ADBB, 0xCE1BF1A6,
+0xB7A75B04, 0x6B8364E8, 0x0CE32E3E, 0x9BF65504,
+0x28C18157, 0x78359AC6, 0x617BF202, 0x1E76FA09,
+0x0F8E61A8, 0x6D02F0D5, 0x80356459, 0x79CEFAE7,
+0x7D00F155, 0x5C1C0128, 0xC75CA073, 0x32816090,
+0x9FF78DFC, 0x848D269C, 0xF811B314, 0xA86920FC,
+0x6F885D01, 0xACFE6525, 0xC726074D, 0xFED68599,
+0xF1D5C76A, 0x8799E5F5, 0xF85F5171, 0xD8DE2D3B,
+0xE7DD8E75, 0x43F8614A, 0x0684FC8D, 0x9683B8C8,
+0x74BE786B, 0x2514762D, 0x7D866682, 0xE711FE1F,
+0x0DE9E273, 0x12F53167, 0x4FA3A7FE, 0x2A00EB61,
+0xB3984A28, 0x4319F2B0, 0x42BA0CA2, 0x848771B6,
+0x995E945E, 0xD41115F5, 0x43D9834B, 0x54EEDC36,
+0x5C3C5407, 0x671B540E, 0xDCF18948, 0x150ED973,
+0x2D4922DE, 0xF93CA17D, 0xB24A76E5, 0xD1C01C22,
+0xF2963DD6, 0x3B860066, 0x08EF0EA4, 0x609B60CC,
+0xE2E901FA, 0x25BE9B93, 0xDF96D9BC, 0x86D415DF,
+0x75CCF6BB, 0x882D54B2, 0x7976E9AF, 0x88A0B178,
+0x5ADE5A55, 0x8A8C0112, 0xD896755A, 0xCB6789B3,
+0x8B63AE2F, 0x2545036C, 0xE4655B94, 0x20959977,
+0x29DFB4D1, 0xCDAAEBF4, 0x1C07EC05, 0x5A6F607D,
+0x88A9B31D, 0x118C74D2, 0x000BB065, 0x75C46712,
+0xEF1A58BD, 0x50ECA262, 0xCCE393B9, 0x6EDB92E8,
+0x700EF517, 0xBF6CF4AD, 0x57456DC0, 0xF629517C,
+0x40331F8B, 0xC10A454D, 0x6CCB02CF, 0x9BF11B1C,
+0xE0871437, 0x23623585, 0xF519F09C, 0x4DB2AFC8,
+0x88FCBD7B, 0xB512FE8D, 0xDE445894, 0x078AD03C,
+0x44375FB0, 0x0BABEDB1, 0x40D5E8E1, 0x13F20A86,
+0xF1406303, 0x7205C322, 0x3FC43779, 0x7A60D510,
+0x14469E04, 0xF4E77873, 0x2EAD7ECE, 0xA135D6EA,
+0x3F4C4B30, 0x21488077, 0x69F64F1C, 0xEEF4876E,
+0x63610C0B, 0xB7B24C5C, 0x324A76FE, 0x0CF651D3,
+0x9460F0B1, 0x81A83230, 0x0839CFF9, 0x70722F04,
+0xC278FB3B, 0x5DD1BDA4, 0x1E4B3DBA, 0xAE161A93,
+0x9E1033C3, 0xD938FCEC, 0xDA2B2F93, 0x28CD82EA,
+0x14AD1FAF, 0xE4EC9CB8, 0xE770AFDF, 0xEFB12898,
+0x500BE181, 0x602625C5, 0xF160631B, 0x78D3643F,
+0x4E13ED37, 0x647BB223, 0xCF18D75C, 0xF477F94C,
+0x786ECB89, 0xB3ED21F8, 0x1BEF3916, 0x240FB35A,
+0x5C69B7D9, 0x8E96290A, 0xD40DC98C, 0xD1370291,
+0x5870021E, 0x3F7CF23F, 0xFD4A6ADA, 0x36482457,
+0x926600AF, 0xDC8618BC, 0x67D3779F, 0x3422830C,
+0x87A41FBA, 0xCA0AFF53, 0x63BC45F3, 0x520BBBAE,
+0xEDE2E031, 0xB6FA9450, 0x258CA712, 0xD709C4E4,
+0x617709B2, 0xAACE0B41, 0x363DBF55, 0x701D6583,
+0x39F3C885, 0x7CD6297B, 0x078FE13B, 0xA398DABF,
+0xDB97C514, 0x039102E3, 0x5CA545AF, 0x9298BA18,
+0xD18DAF86, 0x3D70EEA2, 0x5266AD68, 0xB04945B5,
+0x402DDA5B, 0x01DC6CD1, 0x93AC5053, 0x08DF9EA9,
+0x485EBE97, 0xA5D05853, 0x6CBEE910, 0xD485F4E2,
+0x8F201D07, 0xEFC384A3, 0x7272AFBE, 0xC0B41FD7,
+0x8E54A971, 0xA7F9E0F7, 0xC21700B4, 0xC24A4ED0,
+0x5419EACF, 0xBC2D8FB1, 0x2C5B5AFF, 0x0345274C,
+0xC41DF47A, 0x37658AFF, 0x24CF3BE7, 0xA3086248,
+0xF82B5928, 0xB49A9B04, 0xD4105AEF, 0x444EBE8D,
+0x348368DF, 0xDC77A7A0, 0x68D37E0D, 0xD2EB54EE,
+0xDDAC8C33, 0xE5C93C79, 0xE4706ABF, 0x17536EFD,
+0x6C2B2B16, 0x038AA806, 0xDAD42458, 0xAE1D76A1,
+0xCC8DE95C, 0x1BA20647, 0x0521068C, 0x306FBE44,
+0x4E29D881, 0xD2A14D53, 0xA155853E, 0x44500CC4,
+0xFC4466B7, 0x5AACD51D, 0x506D3A73, 0x3F61E0FE,
+0x58F11F9D, 0xC92A2CAD, 0xD9A4F86B, 0x1FA747B1,
+0x77DEC5D2, 0xDFAB369A, 0xD471EA01, 0x724502DA,
+0x618CE21A, 0x52388BEB, 0x2E8A4CC5, 0x58332211,
+0x3FCC46E1, 0x501210E2, 0xE9D51D1A, 0x37237B55,
+0x8CE3E2F1, 0x6B2E98CC, 0xB56A11E5, 0x8819036B,
+0xA6AA2F27, 0xB0124A0E, 0x92F17364, 0xD4A89238,
+0x0507E337, 0x8ED95DEC, 0x9C014BA8, 0xBA5B11C6,
+0x9C15D38C, 0x52596C98, 0x9330DD3D, 0xD6147570,
+0x21701F1B, 0x5A2385F1, 0xE2F38C6C, 0xB3E94698,
+0x2F9C63FA, 0x7E0234D8, 0x4CDD3288, 0xE1969B5F,
+0x853B3C1D, 0xF61465A7, 0xF281C419, 0x46C5F072,
+0x9F1722DD, 0x64F2A994, 0x86AEE8A8, 0x55895E17,
+0x6047D1AC, 0x3375A934, 0x336BEACA, 0x90791174,
+0x4DACC4D2, 0x24253860, 0x2A7876FB, 0x9DBDF98D,
+0xD5BCE182, 0x67EB5F70, 0xCC06BA38, 0xE8F78715,
+0xFEB0EB44, 0xE9776E03, 0x892A0898, 0x7A070650,
+0x6D04DDC4, 0x99A5B7EA, 0x3B416BB6, 0xDADCE834,
+0xB3B03278, 0xDB73B70E, 0xB0F0224E, 0x538A4AF9,
+0xD25D6A37, 0x8F627FB0, 0x11ED9387, 0xB8C88457,
+0x0CF320CA, 0xA20E62A2, 0x1DACDD4A, 0xAB84575D,
+0x740DAF75, 0xAB9DB955, 0xFF787314, 0xA680B8E3,
+0xC976D38E, 0x1FD38F4D, 0x0AEB6633, 0xB69A03DF,
+0xB6CA8610, 0x106354C2, 0xC37D48C8, 0x3E5EED54,
+0x534CC9BA, 0xE37DFFAD, 0x9F69EB05, 0xF67217EE,
+0x50180B3D, 0xCC61C127, 0xC3598D73, 0xE5C00F01,
+0xFFE9B111, 0x5E23EA2F, 0xF6C45DCE, 0x44585E39,
+0xB02C6004, 0x37233902, 0x4F374C0D, 0x34288898,
+0xE274937D, 0xC81D472C, 0x17A43151, 0x2638F7D3,
+0x5304E5B5, 0xD5CE5EDE, 0x357FA7B3, 0xFBE27986,
+0x64E65D1F, 0xC28D1237, 0xA73D9AB3, 0x124CA6C8,
+0x770D7415, 0x5788C32C, 0x18DEFC00, 0xB3B2B06A,
+0x55CC86A0, 0x8D929309, 0x84AB381A, 0x9DEFE8DD,
+0x26C742C8, 0x952BAC34, 0x0A3B140F, 0x82A9304B,
+0x52CEC9F4, 0x47DF4D08, 0x15A116D8, 0x7B890B18,
+0xC87BEF1A, 0xB59601B6, 0xD37BFB28, 0x5D9F564D,
+0xFB002F8D, 0xE7602E57, 0xE429C852, 0x9C0A8C75,
+0xE02611DC, 0x8A1C9861, 0x7495D6DE, 0xCA059710,
+0xAE5969B8, 0xE5B2CBDC, 0xA49F6EC1, 0x85D2A553,
+0xE4719B0F, 0x40F68BBC, 0x092E24B5, 0x7B132678,
+0xD70C17E1, 0x309E6AA1, 0xE009657F, 0xA7238C7A,
+0xE0575D78, 0x1D6980E7, 0xEFCDD368, 0x19F08D93,
+0xFAC03B85, 0x51BADA8F, 0x037DF839, 0x8F4D29F4,
+0x1DC8A913, 0x50C55402, 0xDEE578F0, 0x2BA1C091,
+0x9ACA567E, 0xA8FFECFA, 0xA3C05D12, 0xF18C6283,
+0xEAAE6662, 0xB4DC6A79, 0xCEC5E782, 0x93A2E384,
+0x8F8A5E6F, 0xCA8379D5, 0x81BCD49E, 0x5FCE174B,
+0xD1543A5B, 0x845D635F, 0xD53125B9, 0x3B2121AE,
+0xF8ECDD01, 0xF84D2D11, 0x6579BC21, 0x5C2DC220,
+0x9EC1A688, 0x1148D831, 0x6C087799, 0x58944357,
+0x56F79FC6, 0x6B689B55, 0x740B5FD1, 0x9F7BFB5F,
+0x6B2F3E2D, 0x10E09273, 0x2E9E3213, 0xF3436AA0,
+0x14A9F681, 0x9087D3CE, 0x68D0430B, 0x9FAFE3EF,
+0xD45B8C61, 0xB982724A, 0x04448D7F, 0x8712E47A,
+0x2C188D15, 0x9C3F06CC, 0x6343B130, 0x56C6765C,
+0xF657BC9A, 0x15F1E973, 0x47E71181, 0x8639F5D7,
+0xC1F3FDD5, 0xDC522441, 0x56BB2908, 0xAA48AEC6,
+0xEC04087A, 0x8D375875, 0xE2941F88, 0xED31CC72,
+0x09BD8794, 0x4C81D5C1, 0x1CC96D9C, 0x98A89022,
+0xAA362C57, 0x924D583D, 0x270430E6, 0x0FD4040A,
+0xAF561155, 0x38DCD1CF, 0xE861D2AC, 0x24A2EF3C,
+0x2B7E3868, 0x13DA6C12, 0x69202EB6, 0x4A5FEC66,
+0x185417A9, 0x3C92EFF4, 0x949842E6, 0x02115D93,
+0xAD1726FF, 0x4E093D7D, 0xC3E41B9C, 0x27BBC1C1,
+0x4FFA49C7, 0x6C63D24C, 0x84255444, 0x282C3BA2,
+0x3D679D86, 0x03B410B1, 0x64DB454C, 0x535499D4,
+0x25B421A1, 0x7E68C8FE, 0x0477E3B9, 0xCEFB087D,
+0x9E59B89C, 0xBB787559, 0x1A550EE4, 0x078B48AB,
+0x73A865FE, 0xD7227471, 0x3A864049, 0xE5EE3A1D,
+0x201BC19D, 0xEB8DAE2C, 0x0E2AB31D, 0xCDAC2D79,
+0xDAAB08B1, 0x63ECD4F2, 0xC00F9716, 0xD415C6BB,
+0x8C20C39F, 0xDED8F5A2, 0x1D6A4190, 0x3D319167,
+0x56B3A26B, 0x0547BF52, 0xA056924F, 0x4DAA539A,
+0x557241D1, 0x42C9124E, 0x18723323, 0x6AD6E7EC,
+0x8E039337, 0xF6FDDD65, 0x5F3525F9, 0xC0AD9704,
+0x810EF049, 0xCE022EE0, 0x41CE7E52, 0x8E172A44,
+0x648808E2, 0xC7FF6896, 0x2AD0985C, 0x304B9631,
+0xD21EA39B, 0x279F5089, 0xCDB5C390, 0x21716A40,
+0x5E34B278, 0x39475D72, 0xBA4F4DB1, 0x8B25818F,
+0xE6E466F4, 0xC4A09DF8, 0x59F18AC7, 0x887AB5FE,
+0xEEA4BA42, 0x17371DA8, 0xA82193D1, 0x6DC30EF7,
+0xDEB9D349, 0x2B3271D4, 0x1FE83836, 0xEC755A29,
+0x05F07FCD, 0xC331D3AE, 0xC6208B76, 0x497FF280,
+0x4C579C5A, 0x22B71F94, 0x30FD620B, 0x31B71AE3,
+0xDF7D1A41, 0xF041ACA5, 0x9533261B, 0x3262D291,
+0x060E9672, 0x7D191A55, 0x6D0F0945, 0xF8C7777C,
+0x1C173808, 0x78308E77, 0xC1EEAD3B, 0x059CCD9D,
+0xA8FDBE19, 0xE47630FA, 0x88A49DE5, 0x03347DAB,
+0x4F31F969, 0xF9C62B12, 0x93AB126F, 0x8A7A3BFB,
+0x82591545, 0x2A1A2131, 0x1DEBB134, 0x449E28DD,
+0xFA7E0248, 0xC1E3A5BC, 0x1747E097, 0x4C69AA5C,
+0x1FD71B4B, 0xAC64CA6C, 0x5545F9F9, 0x5E5886F2,
+0x243DBA6C, 0x495BE163, 0x4ECF5A6C, 0x430C9019,
+0x89A980FA, 0x528945AE, 0x00CE6936, 0x9F9A73B2,
+0x9E59DC6B, 0xD57740CD, 0x0E0CB735, 0xB1202BE3,
+0xAA26C2A9, 0x267A77A6, 0x3FA12CF0, 0x4587C0AF,
+0x354ED831, 0xFFD8BD8E, 0x56CC0F26, 0x75717AE3,
+0x51B10674, 0x3E33EC26, 0x26CE80DB, 0x5C4A9140,
+0x017F6C2F, 0xF9038D9A, 0x0A22C29F, 0xBA1F7C8D,
+0x125CC934, 0x6CF66BFF, 0x48C13DCD, 0x63FC3D81,
+0x258C181D, 0x1A4C3DDD, 0x2E24BECC, 0x7C86A9ED,
+0x5BD1989C, 0x57CE595C, 0xDF291AFE, 0xEAF00887,
+0xD8DD4259, 0xDF67331E, 0x50D0CE88, 0x1FD090AE,
+0x632DA5F0, 0x95272A5B, 0x31172F25, 0x547FD7DF,
+0xAFBE11D9, 0x97189DFC, 0xC4881191, 0x1C92365D,
+0x843DEFDE, 0xCF0A399B, 0xCF327CAF, 0xDDAF0BCE,
+0x03AA7A2E, 0x411A8664, 0x6CCF7CD9, 0x61097EF5,
+0x07F3941E, 0x5BC3EB75, 0x2791945F, 0xBEBB526E,
+0x18631A34, 0x25FEBF10, 0x419834CF, 0xF642D176,
+0x372FFF10, 0x2A1BEA1B, 0x400FF345, 0x257A234A,
+0x9F15E99D, 0xE06AA1DB, 0x3A0DB315, 0x2BA30D99,
+0x0E9E831E, 0x1B25EE41, 0x8DB30E70, 0x9FBA6D64,
+0xAB8AA5E3, 0x5A96177A, 0x6BE03535, 0x97E37DCE,
+0xACA24F26, 0x5F0096F7, 0x5D02722F, 0xAF8F3EC7,
+0xA6824151, 0x70FAD406, 0xDEBA8513, 0x99C63E34,
+0x1CC4A3DF, 0x7F756508, 0xB7386527, 0x647C7FB8,
+0x43F1F4DF, 0xC7E4EC18, 0x302BA109, 0xD5E9175B,
+0x82856F77, 0x0F6D45D9, 0x95AE28B9, 0xE63385C3,
+0x8FB26619, 0xBD99F298, 0xC884B948, 0x0B596FF1,
+0xE061C3F9, 0xBC2F9A81, 0xC488CD91, 0x372EF590,
+0x3DA1BFE5, 0x10DE037B, 0x7210B4DC, 0x74E4EFF8,
+0x6365AFD2, 0x8CEABC85, 0x1D8FFD43, 0x4DE243F8,
+0xEC976FD9, 0xAD827765, 0xC679F15D, 0xC125EC31,
+0x95D3481C, 0xC4EA6EAC, 0xC8FC014F, 0x1352EB66,
+0x9C400EB5, 0x227BFAB9, 0xB12BF958, 0x85B6D782,
+0x78B6E44D, 0xE2232EEE, 0x4F101711, 0x9ABEBF69,
+0x66ACC682, 0x04AD5F55, 0xE4FC6238, 0xBA3D2266,
+0xA2BA3170, 0x083F39AB, 0xFF2075C4, 0x945C4B05,
+0x41E8C113, 0xEC7CAD67, 0x3653733E, 0x03510C3B,
+0x1E973158, 0xFBE507F3, 0x2CCD8D9A, 0x6EA9442F,
+0x0D48DE95, 0xC517BFAE, 0x04EBB5C9, 0xEFAB1823,
+0xD5FBFC0A, 0x6890F212, 0xA1C00CCD, 0x6DD561E6,
+0x20D39B1C, 0x56113FBA, 0xCF3A7FD7, 0x3AB5A0DB,
+0x3656572E, 0x7BC48CD3, 0x8902AE36, 0xD3E94AFF,
+0xC06EB447, 0xCC513C0C, 0x2544B7DD, 0x6F168877,
+0x53162607, 0x461DCEF0, 0xF47AF2BB, 0x8AF9F3CC,
+0x1EEFF9E6, 0x57CFB6B6, 0x7F712439, 0xAB20C93D,
+0x043F9003, 0x60C808BC, 0x86C2137C, 0x46ADB474,
+0x848B65F2, 0x5544789B, 0x18E9AEC7, 0xC889913E,
+0xFEB79B2F, 0xA3FBE518, 0x67922463, 0x93746398,
+0x968E160F, 0x8CA856A4, 0xA040202E, 0x660C00C6,
+0x8F0A8E62, 0xE2BA54DE, 0x4BD0C117, 0x1A1A3092,
+0x086CAA3A, 0x2BBA5676, 0x89610176, 0x00ED2F97,
+0xC72130C7, 0x5A053880, 0x7298E553, 0xD67971EA,
+0x0D41E477, 0x2FA8285F, 0xB856A190, 0x132DB916,
+0xCDFFDD11, 0xB5519A81, 0x1BC7001B, 0x97C824DC,
+0xBB4C707F, 0x90166DC2, 0x42DFAB7A, 0x90E33184,
+0x6C6B940C, 0xDC553814, 0xC4F5E7AA, 0x99434AE9,
+0x82BB09D4, 0xCB0A7DA3, 0x3A8033AE, 0x054D3481,
+0xE20AF761, 0x25F5F254, 0x7AD3AF3A, 0x23A34C29,
+0xA19C57BC, 0x39B57AD9, 0x55E1EC59, 0x5ECA4198,
+0xDB908BCD, 0x4871C3F4, 0xE7091328, 0x64A9B6EC,
+0x1CCAB2F3, 0xEDB22423, 0xFFB6A717, 0x6FA13548,
+0x361FF711, 0x24664017, 0xCBBF9970, 0x83A7B7DE,
+0x9B704690, 0x01A0B877, 0x95041B60, 0xC048F3E1,
+0xA31625F2, 0xE3DFBE27, 0xF657295B, 0x1F5C3AF5,
+0x60EE1637, 0x575EDFAC, 0x725844FB, 0x242723D0,
+0x04FA46FC, 0x1A8C3F44, 0x0E03A5FE, 0x8778079F,
+0x606E4E1A, 0x7C0AF3D5, 0x9578B266, 0x63BCE765,
+0xA8ED66D9, 0x9242377A, 0x817A5D5E, 0xD0981A98,
+0xC07F2E7F, 0x0E66F84A, 0x3635F854, 0xD7AD8359,
+0xDCF23230, 0xC1B9084C, 0xA7987FE5, 0xC3B27EB4,
+0x1F747061, 0xFD278601, 0xB6ED3B5A, 0x9CEF8AA0,
+0xA5023C46, 0xB49832AF, 0xB12055FD, 0xD85310E1,
+0x2C19ADE6, 0xEFBB17A8, 0xC246A4C7, 0xBE4B2666,
+0x13C2D7F9, 0x50063BA1, 0x9B00E02D, 0x335B9DF8,
+0xD424AF25, 0xBAE40C92, 0xE87BD6B7, 0x384D1EB1,
+0x8B91E8F4, 0x9E3FC6D5, 0x6BB1A51E, 0x21AE5533,
+0xFCB8E713, 0x188B66B1, 0x6572E9ED, 0x98829178,
+0x7BAE8CBF, 0xE00C32B4, 0xDAFC14D5, 0xEA8FC746,
+0x2C8D712E, 0x89A05FC9, 0x9A274641, 0xAC2450AD,
+0x2437784F, 0x3B1B80F0, 0x0B4A31FD, 0x277C0232,
+0xFDDC6829, 0x3F3C606C, 0x0EF62352, 0x3D07D04A,
+0x4E0939E8, 0xD59BF115, 0xA02752E7, 0x42BF7133,
+0x9FA0939E, 0x64764109, 0xD5D03EBA, 0x3D4433A3,
+0x1749B437, 0x137298B1, 0x677BE344, 0xA83CEF7E,
+0x17813A39, 0xBC71823F, 0x2070E9A7, 0x3873AEF8,
+0x5AF1E21B, 0x1F0CC692, 0xB8EFB04D, 0x1A1CC514,
+0xADED6C3D, 0xDF35A8D7, 0x6D93275E, 0x9C362545,
+0x62BF7583, 0xFC56D990, 0x0CD6A324, 0xF12A7939,
+0x52587029, 0xD00D5F16, 0x51622555, 0x1178E887,
+0x81E7BCC8, 0x92BB1C11, 0x097330E4, 0xCF8C5CAF,
+0xD076D6BC, 0xBA292918, 0xF835A829, 0x4280A51E,
+0x09CD7827, 0x11583487, 0xB8BA2CEF, 0xD598AE93,
+0x99F4FD77, 0xEB151110, 0x1571B076, 0x63F2103A,
+0x56C6BF44, 0x9E63B556, 0xFB981238, 0x5D8C978B,
+0x9501D936, 0x82A1E971, 0xE5A4F7E2, 0xC6E3727A,
+0x03329F07, 0x248ACDD6, 0x437E917B, 0x23B02B20,
+0x73F76AA0, 0x75EA06C5, 0xD7C662B3, 0x267777F8,
+0xDC96BF06, 0x54020346, 0xCBDF069B, 0x030133EC,
+0xA7EF1C2E, 0x568959AB, 0x4FC31DE0, 0x3A22890E,
+0x280F8652, 0x1BD8CB24, 0x9A8D92C9, 0x52718DE1,
+0x12033FC7, 0xD48490CC, 0x681ADEE2, 0xF91BF7B8,
+0xB8609B38, 0x34CF4BCA, 0x8F123290, 0x0D0F4FCD,
+0xC4F43323, 0x2FC04F1C, 0x4669B890, 0x1E8D2A7F,
+0x0658CAE6, 0x5489F3A3, 0x9CD362FE, 0xBA5190B1,
+0x06A58820, 0x7A9AF759, 0xDC94E672, 0xEB284B85,
+0xF8EFA022, 0x3837C379, 0x7C9E9A2A, 0xD2ED96BC,
+0x5D1E4C7E, 0x97F2169F, 0xFC3C37C2, 0xE039EDF1,
+0xDBE93909, 0x81FEAC6B, 0xFCD383FC, 0x170B91FB,
+0x05BA3243, 0x8FB2ADE1, 0x52AFB984, 0xE8262E9A,
+0x1E704638, 0x89B8DFD8, 0x18C0C641, 0x2760C7E6,
+0xD3AFF3C9, 0xC4E3543B, 0x0C0B7910, 0x1DEF7792,
+0x483D7194, 0x9AAF5864, 0x08607947, 0x626F0CF3,
+0xC0F6A486, 0xEB4525CE, 0xA8BBA8F8, 0xE450DA14,
+0x2DC4D114, 0xBCA527C9, 0x6682AA4D, 0xCBB48A5F,
+0x1B474C99, 0x7F5B526C, 0xEC435C0C, 0x9E8D3E1A,
+0x67D2EA29, 0xA3B7ADCD, 0x8328590E, 0x7345607B,
+0xB6057588, 0x1A8B034C, 0x5C8CA534, 0x8115DC5F,
+0x189C2ABE, 0xF1B92927, 0x78A3B62F, 0x4B621D49,
+0xDC176A68, 0xCBD3C1DC, 0xD82348BB, 0xEEF05FA7,
+0xC0DD3D83, 0xC1F2A7BF, 0xB2079D00, 0x14B5730E,
+0x73203CD7, 0xA8672433, 0xA171FFED, 0x9F181200,
+0x4E16A5C8, 0x56D8AC31, 0x73803D86, 0xD4685CA4,
+0xE8DE9FE2, 0xA35D2CE8, 0x808CF3E2, 0x198700AE,
+0x0034163F, 0x57BC76FE, 0x271ACF93, 0xAA3AF6D0,
+0x37003A7E, 0x450B74F4, 0x157401CB, 0xB79DDDA8,
+0xD60AB7A4, 0x3A4C8779, 0xB6990FC8, 0xA1668D5A,
+0x05B7965F, 0x7814376D, 0xFA0D2D8A, 0xD97A1142,
+0xE804DE3D, 0x4939089E, 0x78D40CAC, 0x01DEF5EA,
+0x3DD1CADA, 0x96465956, 0x6358CFB6, 0xACE02DE5,
+0xB4C9F6CE, 0xE9C95AFF, 0x70EAD28E, 0x58803693,
+0x89EF9972, 0x58F0273F, 0xDB17A277, 0x0B082B98,
+0xAAB13ABD, 0xE86381EC, 0xC18924D4, 0xE28D4348,
+0xC21895AB, 0xE17073AD, 0x9417539B, 0xA043E5F5,
+0x88FFD026, 0xD972F017, 0xD0C8B8D3, 0xB34F3D67,
+0xC525E4B5, 0x0189A5A1, 0x59224A35, 0xAA18F2D5,
+0xFC9E170C, 0x16D3795A, 0x35DB09FA, 0x1624DB1D,
+0x4A6E059F, 0xC5C88A93, 0x9051D373, 0x4B12B09C,
+0x4088AF39, 0x705394F6, 0x360F2BAC, 0x8A1F2420,
+0x641D4FA5, 0xA78B78F9, 0xA5A5302E, 0x691D2108,
+0x7CFB57FD, 0x1812FE68, 0x8A2BB5E0, 0xF181CA14,
+0x1846848E, 0xDC044F67, 0x17FCCA28, 0x21D7C5AC,
+0x4C43432F, 0xC457E26E, 0xB0C9ADD2, 0x791EE2B4,
+0x620F27BE, 0x229E0B1E, 0x746B4FFC, 0x02038738,
+0x1C7B971B, 0x05193430, 0x8645DBD7, 0x58678F98,
+0x141E912D, 0xD89C587E, 0x9FD7B43F, 0x21851D56,
+0x725311A7, 0x0605B1B2, 0xC18BF2B7, 0xC6F79EA9,
+0xBD84A01B, 0xC9B7F2DA, 0x04E47EE8, 0x1C1A14F5,
+0xBD5B4FF1, 0xE15FBC2E, 0xC4D43F01, 0x5D39AD4A,
+0xBD3BD983, 0xB2314A4B, 0x8DABA67E, 0xB5263B5A,
+0x9912F262, 0x82659C80, 0xC3610181, 0x3F229014,
+0x2685532F, 0xCE4EC210, 0xF46AB09A, 0xFAFA69C8,
+0xD1292944, 0x2EF880D9, 0xD03AAEAB, 0x0E83C435,
+0x842C482C, 0xA70951A1, 0x0E4EA07D, 0xE0332D0C,
+0x3EA27E55, 0x04721425, 0x7C8B56DC, 0x96391312,
+0xF600D78C, 0xC850517C, 0xB3F9F2AE, 0x59A99351,
+0x8D6AA838, 0xF586672E, 0xD81FE525, 0x3CEF31DF,
+0xABDC7079, 0x6E1BB8F6, 0x6B45B87B, 0x9FD2CAC4,
+0x648E357A, 0x6C57D30B, 0x23766B64, 0x8C8BD9C1,
+0x9A29001A, 0x206F47E3, 0x5F423D75, 0x293A32C4,
+0xDCC6432B, 0xA4280954, 0x457790B8, 0x11E84CEF,
+0xAB11D0BF, 0xD04258E3, 0xFB44C0CE, 0xED8231B2,
+0x0277A6B2, 0xD8E5C517, 0xCEDF4C8B, 0x19D90170,
+0x20555532, 0xFCB610B9, 0x88D5F5A9, 0xD35DC77E,
+0xEF5EA686, 0xD866959C, 0xF0886B56, 0x005CFB90,
+0x582AD255, 0x7381289F, 0xC18CED4D, 0x444F0A6B,
+0x9917AE56, 0x505A7BCD, 0xCBDC903B, 0x51EF0F3C,
+0xC4E6AF5A, 0xB148AD2F, 0x609A124A, 0xB5DA89E4,
+0x3A68C7D4, 0x98694F02, 0xE85B1766, 0x754BA5FF,
+0x1296A58E, 0x27736843, 0x9B6280BD, 0x2686032D,
+0xB428AC04, 0xB06DBA5C, 0x625FE034, 0xD4BCB25E,
+0xC91C5B3C, 0x73BB70E5, 0xA26A479A, 0x73173229,
+0x3AA1235C, 0xE16171D1, 0x42D0D42F, 0xFC624752,
+0xF1F5DCC2, 0x1B6F20A9, 0xFF9D626D, 0xDBF052C0,
+0x90E38D23, 0xFB72CC5E, 0x9186519C, 0xF2330093,
+0xE5251385, 0xA0094977, 0xE83FA066, 0x2E389CE2,
+0xD3A62E72, 0xA9422A8B, 0xC61CFD5B, 0x1B3A516A,
+0x58087800, 0x3A47462C, 0x557DDD8B, 0x94FD21D4,
+0xE1AEA942, 0x4B2CC532, 0xB2185B36, 0xDCA15259,
+0x1D044D7D, 0x781317B8, 0x49CB13E7, 0xDAAFFBC6,
+0x30A05644, 0x77B05F37, 0x065A567C, 0x94721C79,
+0x47316C60, 0x58AAC7C9, 0x410081AB, 0x7D4A36FA,
+0xCDF23455, 0x1873EF87, 0x186982B5, 0x7C78D9DA,
+0x3567D966, 0x10FF5E8E, 0xDB88E5B3, 0xFF1D39A1,
+0xB8A345A3, 0x7A7258F3, 0x9706B3CE, 0xB5ADCC26,
+0x4561EF5B, 0xB002FBF6, 0xF3F4C6FA, 0x57EC75AD,
+0xBCF37924, 0xBC05B0AD, 0x2AB19DAA, 0x0EBD25EA,
+0xF335D08C, 0xDFF79E19, 0xDD86D418, 0xECE11951,
+0xC06F4D50, 0xFD698DF8, 0xBA6192EF, 0x365A28CE,
+0x74DEC0B7, 0xE971F67B, 0xBF89DD42, 0x1E683399,
+0x164A7158, 0xA1E48475, 0xBE139E8E, 0xBDEBA7FE,
+0x74E03AEC, 0x88EA9618, 0x9B0048C2, 0x68C1DD20,
+0x8DC9FC85, 0x24B55E3B, 0x51C38BDA, 0x2ECD7B13,
+0x54D66C89, 0x69A3EBC1, 0x4B4E4F13, 0xAD37B7DF,
+0x030A1D8B, 0x85A114D9, 0x403BE495, 0xB5E40331,
+0x316E7310, 0xB36AA494, 0xDBFFCB9A, 0x5C0E5DA5,
+0x099BA9E8, 0x66826E9D, 0x0BC5849B, 0x1A20CBAB,
+0x0744FBE6, 0x2CB52040, 0x8B88533F, 0xA8A44BF1,
+0x62FEB4A8, 0xDB2ABC4D, 0x46F0B676, 0xCBD06470,
+0xDB6D71EF, 0x5DC3551A, 0x71B31A5B, 0x046D4C7F,
+0xC051A998, 0x1EC19FF9, 0xA9E21F9F, 0x7951E081,
+0x78BCBA62, 0x91B623F2, 0x8EF6A81D, 0x1023755E,
+0xCE47F5AA, 0x0EF27527, 0xE9E488D5, 0xD53E4A29,
+0x78A276E1, 0xB2100585, 0x01208E3C, 0xA38BCAFF,
+0x36221FB7, 0xB3C9194E, 0x51BD75D4, 0x9C8C73AC,
+0x7ACA9964, 0x17890C94, 0x9FDA51F4, 0xC4FDF688,
+0x2C8244B2, 0x0D834C74, 0x290973D3, 0x7F134553,
+0x296D2FC2, 0x4E08ED27, 0x1C51E53D, 0x3D892F49,
+0x945F76CC, 0x2E531E63, 0x71EE37E0, 0x9C47F346,
+0x2D8D920C, 0xC3E465BA, 0x3A72D142, 0x5B6AB80D,
+0x364C2AE7, 0x3B18389B, 0xB9442484, 0x5D687BB5,
+0x97C65A4F, 0xC7DBE8BE, 0x0F840061, 0x5A73EA89,
+0xCBBDD954, 0xAFE9CABD, 0x06ABDF95, 0xF139302D,
+0x3804FEA8, 0x7CE6542F, 0xDE47B8ED, 0xD34BE509,
+0x5EB9C9E1, 0xDC582534, 0xE77D7FC8, 0x2BEFED7E,
+0x4EA26DFD, 0x54670B81, 0x665C4531, 0x5B7A7023,
+0xA05D9A2E, 0x71BDDB2E, 0x9D51D8C2, 0xD8A665CC,
+0xA9B87A22, 0x581D28BF, 0xF9D40373, 0xE04D8F63,
+0x117B9842, 0x8868B9BE, 0x8397FAB9, 0xEF5CED75,
+0xF70F90D8, 0xD3DFD3A6, 0x1779F576, 0x3059520D,
+0xC38F4AA9, 0x6B7A6D0A, 0x4E73112A, 0x4FF9DCED,
+0xAEA1383A, 0xBAB0AA93, 0x41DBCBED, 0x266775A6,
+0x8EE0D5D5, 0xB522CB9E, 0xC6E5D0D3, 0x86E4C8FD,
+0xA894642F, 0xF69821A9, 0x88B41798, 0x4585A188,
+0x9D2130FC, 0xC5B18E0D, 0x6B92C9EE, 0x3C9289FB,
+0x1F02CBB6, 0x31FA86DE, 0x1B2295CD, 0x5B4DA19C,
+0x3134D8FC, 0xE5EABC44, 0xDF8C5095, 0xF6571881,
+0x1F2FBD62, 0xE585FE61, 0x020CEDF6, 0xD70ABC83,
+0x5F37746A, 0x6FDA3BF7, 0x5434E503, 0x44CF6915,
+0x561B2393, 0xEA4A2251, 0xA988C080, 0xE47B1791,
+0xD335CFBE, 0xEDA9DEE2, 0x4F70FB22, 0x83A2C29F,
+0xF44FA002, 0x069D25EC, 0x4D5043F5, 0x887464CA,
+0x661D1E9F, 0x98B856AD, 0x81A23FB0, 0x3693BD42,
+0xCE0AEB0B, 0x1F6E8322, 0xCBDF571B, 0x93688909,
+0xFA16A774, 0x25834437, 0xEE77FA98, 0x8DC68C60,
+0x155A8760, 0x22B8FCA3, 0x1B1BB054, 0xCA3AFFCA,
+0xC8EACEA4, 0xC86BADD9, 0x473770AB, 0x41D6E398,
+0x568B397D, 0x065C0BE5, 0x51D38A0D, 0x3BB3A0E1,
+0xBC386DCB, 0x7DCBA6B0, 0x19007254, 0x3F4FC726,
+0xF27DAE85, 0xF7FDA72A, 0x6D0B5C07, 0x64A0ED12,
+0xE26D8878, 0x210E4F6B, 0x65F92C0D, 0x4E4E2CA6,
+0x5E479D49, 0x7B287050, 0xE9A4836C, 0xC3A111A2,
+0x9B90D6FD, 0xA5F362E0, 0xADC9526B, 0x79B736E9,
+0x72A9A57B, 0x181B4E70, 0x5236F32A, 0x5567E3C9,
+0x23EFD063, 0x87113163, 0xCDF6D4F4, 0xF53A8722,
+0xB70CF941, 0x757F40C8, 0x6A652BE7, 0xD71DA5AA,
+0xF87D51C2, 0xB4A68E16, 0x763D8FEB, 0xB6DE5436,
+0x12184DCD, 0x38D1DE90, 0xB39E5209, 0x1600492A,
+0x073AE8F5, 0x0366AC0E, 0x1AD5014F, 0x398E0873,
+0xD653928E, 0x30B5B4DE, 0xAC68A06E, 0x8DAEF4D3,
+0x76A880D8, 0xF3B3BCC5, 0x2B631F58, 0x340914DB,
+0xB4771DCC, 0x7C9D4A43, 0xAFDB1138, 0x014B5A83,
+0x0D44185D, 0x20C89576, 0x994B4367, 0xA84BD792,
+0xB2E17CB1, 0x00CE5214, 0xFB93E54F, 0x03CCA7F1,
+0x956A82E6, 0x22329A71, 0x2A634374, 0xF18B7AD9,
+0x1F168BC4, 0xC2CB1EDC, 0x8E0AF6CD, 0x211AF22A,
+0xAB5DA374, 0x63F1F25E, 0xEC58D4CC, 0x48C65C46,
+0x5A7F7574, 0x7BA60047, 0x279EF299, 0xE0B77F48,
+0x647A03C3, 0xAE7C4D8F, 0xF65149D0, 0xAC9EF228,
+0xCD90B1CD, 0xCEEDA54C, 0xD8FD0A6A, 0x8D7C2291,
+0xB38EF6C1, 0x7F38E676, 0xDADD0A8F, 0x1125713C,
+0xAA78A299, 0x54033F20, 0x199C76C5, 0xCAF82A17,
+0x16F2EE8B, 0x20071D0F, 0x2CA000F8, 0x0178A24B,
+0x0029EE46, 0xA9D8C738, 0x123D2BBD, 0xEF7CAC52,
+0xBD241869, 0x435F8FF7, 0xB573A190, 0x402BFB2F,
+0xFDA3097C, 0xF3765889, 0x68E2C7D5, 0x4C26F858,
+0xD6814D1F, 0x6B043C7B, 0x173DB091, 0x95126C7C,
+0x0FE8E1BE, 0xFDEB233C, 0xB979B0CB, 0x00E00659,
+0x19952E52, 0xA0976F7E, 0x02FB462C, 0x798815C8,
+0xA2504EFE, 0x0F4811AD, 0xBA8F122E, 0x5EE5864F,
+0xD39B6799, 0x5319F6A3, 0xF6A66685, 0x988D106F,
+0x7ABA5220, 0x0320384B, 0x4DE48C79, 0xF5CB36E6,
+0x2B33270F, 0xFF4E6965, 0xD4D843D5, 0x7EEE861C,
+0xA96AE5EE, 0x310E5215, 0x6D20068E, 0xB149AE8B,
+0x0997D9EF, 0x5043FFFA, 0x0516E2B6, 0x3FCCDA32,
+0x8E604A04, 0x23012778, 0x9444A474, 0xB7F5DC24,
+0x3A58E6FB, 0x17B759FB, 0xF29C1EE7, 0x8893D2D1,
+0xC6CD235B, 0xAAB0CBCE, 0x2D84474C, 0x8A0BE027,
+0xFDB87FB5, 0xE6B507BD, 0x19B41927, 0x783FF4DA,
+0x485A1D5D, 0x8ED285C2, 0x25AFC4C5, 0xBF0D662B,
+0xC4238532, 0x4339FCCF, 0x14A784B6, 0x71665819,
+0xED76E473, 0x5F1BAE9E, 0xD0AEC17B, 0x4CE78814,
+0xD3609F61, 0xD4E49EB0, 0xE4E3EFDA, 0x9B7CAD1D,
+0xEF01ABB7, 0xD137BEE9, 0xEE87A81D, 0xD4B204FF,
+0x00B25737, 0x2770FBD1, 0x174AFF7F, 0x0A77A21C,
+0xF1B370E7, 0x9C093CB0, 0x080C1FFA, 0x83CE92D9,
+0x1707470C, 0x3303479F, 0x25F1B6AF, 0xF40EEB7F,
+0xB98A1677, 0xA54A1BA2, 0x43B4144A, 0x2F092A35,
+0x33286A77, 0xA0AB9C93, 0x4F8D70DC, 0x3A47BF6F,
+0xB6209AB5, 0xA4C94557, 0x5E757055, 0x706EAD9F,
+0x467BC02A, 0x6472A857, 0x42055C57, 0x66F2BA60,
+0x33C0536F, 0x3240BFBD, 0x3DD74E6B, 0x1F58A552,
+0x822E9577, 0xF49BFE77, 0x5490DC6D, 0x1D32BBA0,
+0x1C30B072, 0x78A4A5C0, 0x1EE88A57, 0x97CAC3C8,
+0x9912861F, 0xC916BBAF, 0xFC3A7F0E, 0xCA5E1F3A,
+0x630F09CD, 0xF6C8C210, 0xF0A12A72, 0xF3148619,
+0xDF1672E1, 0xFCE5C390, 0x29CAE554, 0xE984A45C,
+0x8A1F0A3A, 0x6A02C707, 0x8CFB3ED6, 0xC0A741BD,
+0x7A871FE5, 0x91021A69, 0x505FB05A, 0x8F85227B,
+0xC300ACF1, 0x0A1B201B, 0x224614B2, 0x54A23576,
+0x5360A5BA, 0xDCD23A31, 0xF98DF638, 0x79FF79D7,
+0xEAC8EAC3, 0x4D22C65D, 0xDFFBF1D9, 0x55FD8848,
+0x4BFD2347, 0xE2A08287, 0xE6A48824, 0x80625EA9,
+0x71AB3F7E, 0x99B84DE5, 0x6512ADBE, 0xFBF24C47,
+0x3EEF2564, 0x23DF9F1B, 0x24BE5199, 0xDEDD72D5,
+0xA2FE063B, 0x4FE520B1, 0x9E4E7BBE, 0xD615BDBE,
+0xC14E8184, 0x40F86FB1, 0xD403A65A, 0xC5AF6386,
+0x412F8434, 0x6D6012B0, 0x4EC57107, 0x3F76AF19,
+0x54A305BD, 0xEA9C4EB2, 0x584E0176, 0x20759805,
+0x1A16C84A, 0x50BB10DB, 0xE610AF45, 0x98CF1EA0,
+0x3F8C7756, 0xF9056BE0, 0xBAA66B7D, 0xF7076DCF,
+0x67F1994D, 0x92BFEB62, 0x86FBDE17, 0x389DB311,
+0x2A171F5A, 0xE14898B1, 0x4D11723F, 0x29889062,
+0xCBF3DD79, 0x2B7468FC, 0x4FB93770, 0xC5FCEFE8,
+0x8FEE6678, 0x9F4ABA9C, 0x6A6B23E1, 0xFEA7077F,
+0xC835F734, 0xCA67807C, 0x1BFBEB49, 0xB8B1E842,
+0x6A850623, 0x001C1E8D, 0x782AC01E, 0xA28A72D8,
+0x6CD66FC1, 0x77EF6F13, 0xFF40D7CF, 0x4A163DFB,
+0xDB21AA89, 0x29D03A9E, 0x3A4D1D57, 0x7A89CDC9,
+0xC5623E10, 0x8A444799, 0x1F620DF4, 0xFF876758,
+0xC9DEEF2E, 0x7F86911E, 0xE3196093, 0xA00EB422,
+0xCDB1743F, 0x4AAD1988, 0x70167700, 0x70595C5F,
+0x8E648013, 0x401D8770, 0xC762F0E7, 0xDB776926,
+0x2BDC55B3, 0x8F4AD2C1, 0x1A2EEB50, 0xBD4BF2A4,
+0xA43FFE90, 0x752935E7, 0xB02C7801, 0xDD4CD3DB,
+0x3815C394, 0xAF427695, 0x7455A8F9, 0xC444C7EC,
+0x9BC9B2C5, 0x08423BA7, 0x5D91ADD8, 0x59D866DB,
+0x0AD32258, 0x7BC397F6, 0x0EF7DB59, 0xC1034320,
+0x79073406, 0x991A12B9, 0x9D6776A0, 0x6348A5EB,
+0xBD98CDC4, 0x81A6C5C5, 0x76A3ABA6, 0xFA9CDF77,
+0x97772B59, 0xD987E42B, 0xA4B893D4, 0x61F78E38,
+0x82567691, 0xCB91CD58, 0xEEFA69AE, 0xF7D51178,
+0xA436C578, 0x99E86E08, 0xA8C3B16B, 0xD609054F,
+0x1E0ADCE8, 0x5DF6EF20, 0xEB3CC45B, 0x9FAEA24F,
+0x97F57F19, 0x66E2713F, 0x42A423C3, 0x2A21B17C,
+0x6A4C6B40, 0xFA0F4F2B, 0xD1F3F64A, 0xD0AAFA50,
+0x767D3AC2, 0x837E626D, 0x3B21279C, 0xCAE18855,
+0xFA8CA385, 0xA91BDE45, 0x1A953327, 0x733948CC,
+0x158B8CD2, 0x904AC43D, 0xA6BC8F82, 0x55F027DA,
+0x95B6BB32, 0x9265FF80, 0x8EEF0D24, 0x28F6796E,
+0x1D736700, 0xB621D4D6, 0xAB2F1A4A, 0xECD7DB83,
+0x35CAD419, 0x60604917, 0x5DE51335, 0xA3D7E122,
+0x685D04D4, 0x494739D4, 0x0060722C, 0x59149718,
+0x03C9F144, 0x43328818, 0xBB1AE189, 0xCA7B9250,
+0xC835666D, 0x83950220, 0xD774405F, 0xF6F4FCCE,
+0x0E38794D, 0xAF184A7E, 0xEF66E15B, 0xA0C2A74F,
+0x876112D5, 0x7D68C9CF, 0x8902011C, 0x6AB0E128,
+0x2A515520, 0xA99D1DA0, 0x9EACEB4D, 0xB669AA8F,
+0x6F96DCE2, 0xCFEB5CDF, 0x46EB36BD, 0xEDDF8317,
+0x4FA30C3E, 0x9541A8A1, 0xA5F75533, 0xEFE1FEF6,
+0x7F21B481, 0xDA11D5EA, 0x64642069, 0x083D2137,
+0xDF508726, 0x8F6CCC4B, 0xC5412D0A, 0x6A9F6BEA,
+0x3E3CC54F, 0x078BBB1E, 0xA6047468, 0xF1FA39C2,
+0x26143435, 0x90132EB3, 0x4216580C, 0xF6773B8C,
+0xA6B188BF, 0xE3B49523, 0x89E4563F, 0xD0B16538,
+0x2D9079FD, 0x69ABDE36, 0x669AC5EB, 0xD0618DD9,
+0x5080BFEF, 0xADC056D6, 0x72402C9C, 0x0AE79E07,
+0x8D6DF48E, 0x0502837E, 0x79BA17AD, 0xE4871C89,
+0xC4554CD5, 0x23FCB2A4, 0x646FA999, 0x212A9DB8,
+0xBD23DF0A, 0x890B5FE6, 0xB5D03292, 0x9FA3FD59,
+0xD612F8B1, 0x611365FB, 0x7E7C9FAB, 0x024194D2,
+0x46C2C617, 0xAEB0FAD9, 0xAE5D3A7E, 0xEA8B0ABB,
+0x760730A4, 0x50443E76, 0xECA64341, 0x538E5256,
+0x8A8505F5, 0xE0E4DC29, 0x105DC564, 0xC73D93D9,
+0xE3F27C90, 0x8CC01FC8, 0x400D0F76, 0xDCD01130,
+0x1E3416D4, 0x4C612E03, 0x0BFE7A5C, 0xFDB15334,
+0x5326A77F, 0x99549BDA, 0xDDE90BAB, 0x920BD872,
+0xC4B4F5DF, 0x7B39BAC2, 0x777C6694, 0xB4971103,
+0x9E7806A1, 0xD3141F2D, 0x2B40BAD0, 0x74AF248F,
+0xD1AEED43, 0x2F453736, 0x1880104E, 0xF9CD502F,
+0x7691FE59, 0x39C3FEC7, 0x72EA7BF2, 0x0C94BAB5,
+0x35D6F509, 0xAE86AC96, 0x0624C181, 0xA69DF699,
+0x5991FCE3, 0xAB20D4F1, 0xF30F1BC9, 0xB094CF62,
+0xA3B5A732, 0x3BC8C32F, 0xE7710370, 0x429A8D96,
+0xD8913A42, 0xCFBD0E4F, 0x710B7078, 0xC6501E93,
+0x241224AF, 0x978D2320, 0x8EF1064B, 0x273FAE07,
+0x316EC02C, 0xB3C16C0B, 0x8249C245, 0x21AD11CB,
+0x6265FE57, 0xA9F1D5FC, 0x0B52F1CD, 0x0381D983,
+0x2931D6B1, 0xD126CD94, 0x69D95197, 0x7CFB6AD0,
+0x46E6D50D, 0xE60BCBD2, 0x72FBB436, 0xC971A4CA,
+0xA580B9B9, 0xBC823514, 0x5D15A840, 0x87A91622,
+0x63490D13, 0x277189A8, 0x22CA2EDC, 0x1C56456D,
+0x1B5EB836, 0xD8BBF2EB, 0x20F56DFB, 0x99321E4B,
+0x9238B783, 0xE5E5D085, 0xC81DAA11, 0xEF8DD032,
+0xCEC28645, 0xFC40AAA5, 0xBFA5FC68, 0x1C2CF7C7,
+0xC0DFD194, 0x5AB730DA, 0xE3FB56A9, 0xA0AD00E9,
+0xB7BA2E2E, 0x579C8722, 0x04AA07FD, 0xF55C6C5C,
+0xE56CD6DD, 0xA7DA5100, 0x2A6BA1E5, 0x9B7E5104,
+0x81410420, 0xDC6130A8, 0x3EC8935B, 0xCC2EC782,
+0x142344EF, 0xF016E0CA, 0xA3ACFA8E, 0x019A7009,
+0xA0DAEC5D, 0xFA503565, 0xC907794E, 0x77AA4E69,
+0xB45B7E54, 0x929A056A, 0x46AA4AE1, 0x55E56EDF,
+0xFDD9D726, 0x35744D5C, 0xD6854700, 0x9A6E1EEE,
+0x0B00F6FB, 0x6BE65BFB, 0x9CF98DE0, 0xD80ACE66,
+0x1E5300E4, 0x745338DD, 0x4CB925DE, 0xB369B0D4,
+0x7A53A606, 0xD2B96E54, 0x88F96B30, 0xB72C3E19,
+0xC2A41177, 0x6206F879, 0xC1F6CD78, 0x879DA74F,
+0x763F9417, 0xD109B779, 0x6A58B34C, 0xDCD7C21A,
+0x1B0A0154, 0x45EE3A9C, 0x62C60161, 0x79E47020,
+0x42250A39, 0x9E2C2C59, 0xCE4F6206, 0xC2970386,
+0x983CC2C3, 0x0DAF0A85, 0x388626DA, 0x06A56D27,
+0x9223203A, 0x96E0148C, 0x22F0D052, 0xD5F1AA88,
+0x394BC8B9, 0x03CF58FA, 0xC0B1073C, 0xC16B35C7,
+0x7B7CF9F8, 0x2E3A24A5, 0xA19089C9, 0x4223FAE9,
+0x7751D977, 0x802E7062, 0x6D3651EF, 0x39E9B52E,
+0x946D07F8, 0x8E2EAEB7, 0xF9279A65, 0x14DEE911,
+0x8B92A149, 0x9611756E, 0x067DD22D, 0x59907967,
+0xB3417E3C, 0x3B72AB7A, 0x825D87C7, 0xCE5FA852,
+0x5D88C5F8, 0xE792BF66, 0x28DB3A4A, 0x118CA3A2,
+0xCC86284E, 0xA0AC4AE8, 0x33394B70, 0x974F96C2,
+0x86ADD3B5, 0xC87295B9, 0x1447D26F, 0xC9ECAE80,
+0x10CA01D7, 0xE04ECC68, 0xAE56597E, 0xAAA1248C,
+0x81C35460, 0x0087CA93, 0x943AABA2, 0x0AFCBFAA,
+0xEA77D5AB, 0x020D36D6, 0xF1CCBBB6, 0x8DF1426F,
+0xAE726D96, 0xA6E4C915, 0x58F15F91, 0x5B696D6F,
+0x00042B30, 0xC6AC90C3, 0xBD8E0187, 0xE73ED2E2,
+0xCEE64CF6, 0x48B56436, 0xA33994CA, 0xB3E3B7AB,
+0x060D5E14, 0xC1B176C6, 0x4A76C391, 0xD7C8DB1D,
+0x333E4998, 0xC20BAC4F, 0x523BE3E0, 0x237E87BC,
+0xE6CDBEC0, 0xC506F19C, 0x262C0039, 0x7F85A4AC,
+0x46160693, 0x2EA1BC36, 0x4CAC0DF2, 0x0066B83F,
+0xBCBC778D, 0x7F4AB507, 0x99CADB2F, 0xC95520D0,
+0xC5CBF067, 0x903ECD68, 0xF5D7B0FC, 0x08198C8F,
+0xA17879EC, 0x18C2723D, 0x5A4D6D37, 0x080198B6,
+0x3525186C, 0xEF8BE144, 0x44B05851, 0x28B5025A,
+0x0FDF085D, 0xDEB1F249, 0xA7C00F42, 0x7614A735,
+0x3BEBF467, 0x7871D305, 0xD4F63809, 0x9D044079,
+0xE585D3D6, 0xA89952F3, 0xF42C2B8E, 0x04179DA4,
+0x00A6CE87, 0x96CA92B8, 0x9DF2B156, 0x3ECF18BC,
+0xDE2509CF, 0x5CD85FCA, 0xF8A7CEEF, 0xCB7DC25E,
+0xF2847474, 0x35B501D1, 0x137BBB3E, 0x451E1BB9,
+0xD360D811, 0x792B3464, 0x4BF89A81, 0xA7E9C450,
+0x628BCB0C, 0x2AF7037D, 0xA45F628E, 0xF0EC875D,
+0x9CE3677D, 0x2CD0EA59, 0xA50A0217, 0x8BA45DD7,
+0x1735ACF1, 0x5804C4D9, 0xE619B352, 0x948F44A8,
+0xA9BF5C7F, 0x614D4F6C, 0x6D9FCA79, 0x29717B0C,
+0x50BF2D5C, 0xD5847B52, 0x0D4FAAA5, 0x1AABCA5D,
+0x779399E0, 0x58A90CD6, 0x37EC2615, 0x61B68C07,
+0xC49F4AEE, 0xFAC4D897, 0x9C68CC6D, 0xBB3352F6,
+0xF933436D, 0xD310078E, 0x2FBFA17A, 0x3D839C4C,
+0x186E69EF, 0xCBE7CC6A, 0x7434231A, 0x80F8130B,
+0x58CD7EA2, 0x2E46D714, 0x367286E2, 0xA6E2044D,
+0xC2ABC50A, 0x6FEDC9C4, 0xE2F26F03, 0x3B030D52,
+0x3674D8E7, 0x9096DF78, 0x90902892, 0x44A32190,
+0xD08D2649, 0xEFE0ED0A, 0xCE1BF4E9, 0x62C19753,
+0xFBF3D1A8, 0xD4AA5390, 0x4B32E77F, 0x9894F05E,
+0x41B9DBBE, 0xE9B09561, 0x46C883A0, 0xADD5D60F,
+0x69CE5BBE, 0xFD29CCF1, 0x2F209371, 0x4C6716E9,
+0x31E9A09F, 0x04089795, 0xB9EF9025, 0x97C6267D,
+0x63823150, 0x3AB346BA, 0xED3E0579, 0x85FC7062,
+0x37B35761, 0x4A32B6CD, 0xC38EB479, 0x203642CC,
+0x568FCAD7, 0x67D92B5D, 0xE51B8C3E, 0x02104078,
+0x026BC607, 0x5A06CDA7, 0xE27435D0, 0xC7C20CE7,
+0xFEA74022, 0x77310076, 0x35C6F953, 0xE1B199C5,
+0x262F139B, 0xFD2FE2C7, 0x3EEE02EB, 0x915A873F,
+0x2DE4AB8E, 0x2421DC15, 0xD1DD0D9E, 0xDE02B5AD,
+0x151C76CF, 0x798B90B7, 0x82EDDF4C, 0x795E18CF,
+0xF09CEC5A, 0x070ADF8F, 0xCDCF5232, 0xD498D43C,
+0xB4FC2662, 0x25678E54, 0x5D200482, 0xC31F21C9,
+0x35E5AF29, 0x8CC0E603, 0x995351AD, 0xD8EB54F6,
+0x564E35D9, 0x0C13E321, 0x34CFA33D, 0x33D1E5F9,
+0x2EAC9748, 0xFFB950D6, 0x2032206F, 0x4F871AE3,
+0xBD464C61, 0x06356EA0, 0xA15A290D, 0xA78456D0,
+0xD2F4EE88, 0x4D835908, 0x15DC87B3, 0x79EDB6C3,
+0xAEAF0F9E, 0x5C3E7EF9, 0x639A099E, 0xD375D8DA,
+0xB718510B, 0x090DF965, 0x9C8A362E, 0x25AD10BB,
+0xF9A42BE9, 0x8ADE3DF0, 0x5527424E, 0x301F0D0F,
+0x2F691C9A, 0x534FE1FC, 0x7D406016, 0xF98820A2,
+0x4D204871, 0xED145173, 0xD67ECE9A, 0x35F9F990,
+0x8ED4D787, 0x1F3F46E1, 0x5A68F171, 0x9A9D28B0,
+0xE726BD5C, 0x8119228D, 0x0ADBA4D2, 0xEA243204,
+0xE523C0D6, 0x261E3664, 0xB2D1211C, 0xB4D9293A,
+0x9C89D924, 0x15A6A3A9, 0x0D8C6C66, 0xEC04AD36,
+0x0CDF0F98, 0x9262C7DF, 0x8EE0E09B, 0x6B929EE9,
+0xDCC713BC, 0x75FD34FF, 0x2784E694, 0x23C23044,
+0xB7B04F09, 0xF10B753E, 0x2EC774DA, 0x470BE72E,
+0x054510E9, 0x9C7DDF10, 0x1466C277, 0x9F52F493,
+0x7F298608, 0xF1BA10D3, 0x8847A319, 0xEE8A63CA,
+0x8E64B34E, 0xEBB66933, 0x575ADB24, 0x041BFD76,
+0x727ED364, 0x00F4A008, 0x8F5EDA92, 0x21477637,
+0x0B360617, 0x56DC8978, 0x27F88944, 0x69B799EF,
+0xEA1E943B, 0x6FDD60B0, 0xCE2AD89F, 0xB98CCF43,
+0x2A3796BF, 0x4DD02535, 0xC6B524EA, 0x6B173341,
+0xDCE0A457, 0x91770646, 0x57A8D138, 0xFC218331,
+0xDC6B712D, 0x14C0B3B9, 0x30CA09AD, 0x759EB942,
+0xBC9634AB, 0x8F92A7E5, 0xF7F85B53, 0x6C831B3B,
+0x56A75B18, 0x43DB9F1C, 0xF81FC212, 0xB8EB9026,
+0x78A74B51, 0x870655E3, 0xA17B536D, 0xBDE866CF,
+0xFC609F11, 0xF34A7016, 0x7C4FD4DD, 0x236312F6,
+0xB50520A8, 0x4BEEA2C3, 0x2B690BA3, 0x18701667,
+0xBD791FA9, 0x236D36CF, 0x49E576CC, 0x316A77E1,
+0x93E9B0BF, 0x52715603, 0x83B9AAF2, 0x0F8F2A80,
+0xA87F764A, 0xD2079BEB, 0x48A24AB6, 0xAC370950,
+0x3077FB2F, 0x4BAFF3F5, 0x1A79926D, 0x8B369956,
+0xAD78F739, 0xED88CE42, 0xB96A7C15, 0xA7BBA2EE,
+0x47CC3233, 0x804DE962, 0xE0B431A3, 0x4A8257B8,
+0xA4B0E8E2, 0x2FFC49B8, 0xF0CDF5E5, 0xF089C32A,
+0x46328288, 0xEACBC054, 0xA48CB5CC, 0x77996530,
+0x83A4E184, 0x3C2F47D9, 0x5106177C, 0x33F1A787,
+0xA2266E7A, 0xEBC426C8, 0xD7E8ADD3, 0x2DF40477,
+0xF9E8D7BD, 0x80BD8EAB, 0xE61CE55F, 0xF6A7EF6F,
+0x5C67E1C0, 0xFBD0088A, 0x7ED37B24, 0xF5BFD58E,
+0xC29CFB0F, 0x61ECE08B, 0xA776CFD8, 0x9E0F3A05,
+0x8FC8B02F, 0xFDF82702, 0x028C2F2C, 0x169D3094,
+0xE4AA3228, 0xF2CD142D, 0x9C70574E, 0x057BFE78,
+0x782B9039, 0x0D01311F, 0x97552050, 0x6A097F2F,
+0x1B3242B8, 0xF43F32FB, 0x96004287, 0xC3DC0939,
+0x4215A0E1, 0xACD1A28A, 0x189932EC, 0x9BBA0475,
+0xFA154E5B, 0x4B4E8D01, 0x4D6B18B1, 0x31545B3C,
+0xC849C52D, 0x60958B9B, 0xE92CF090, 0xAC3E1B58,
+0x251D02A3, 0xFAEE4F8B, 0xB1CF6CCC, 0xC2A0D8B0,
+0x0501DF46, 0xD0369D94, 0xF3E11479, 0x397599F8,
+0xB90064D2, 0x341F6D57, 0x31F0141A, 0x2F899029,
+0xBC9EF6E8, 0x13B47347, 0xB93D59BB, 0x556E990F,
+0x5727BDFC, 0xBA9F5121, 0xD67BE7CA, 0xB167E84D,
+0x2C0ED0FF, 0x251FFD4A, 0xC98719F2, 0xD379D976,
+0x8B3A0A9B, 0x40BA5F66, 0xE40A93E8, 0x2F89FC04,
+0xFCBAFDD4, 0xF2424270, 0x1BDBDD15, 0x7F1459B0,
+0x5ACB6C6A, 0xFA20719F, 0x2F16FFB4, 0x820DDE50,
+0x468AAC15, 0x7816134C, 0x978D9570, 0x6745CD6D,
+0xC1E768C1, 0x15E243B5, 0xBA30AD61, 0x483FB6FE,
+0xCAA17D0F, 0x2F8F0974, 0x34AB68B4, 0xB3E864B0,
+0xC1DA3828, 0x5DAD43B0, 0x72D13B81, 0x01F274AB,
+0x9C0651AD, 0x0FC30C10, 0x0E7AA3CB, 0xDBE6B9D9,
+0xF423B9A7, 0x457B4E32, 0x40E8E269, 0x91DA042A,
+0x9DBF41E9, 0x308C0F2E, 0xCABFAC0D, 0x0E2C86B2,
+0x117BC3C6, 0xEEA538F8, 0xF31585DF, 0x0DF50281,
+0xEAA9601E, 0x8F408AFA, 0xF1144F9A, 0xA2AB2ECD,
+0xACB88685, 0x6F4EFFBD, 0x81EEF886, 0x46B02240,
+0x3C09D916, 0x4F0DAF68, 0x8337B3E3, 0x9A011BA6,
+0x4C63AC66, 0x2FCC669E, 0x0C7D15BB, 0x51279D9F,
+0xC1354779, 0xEFF940AF, 0xA956CB37, 0x0DB797E2,
+0xE665EE55, 0x79AF879D, 0x21BBC902, 0x30B264BF,
+0x411CDC98, 0xE453389F, 0x47C2C197, 0x3E6015F8,
+0xF9E7AA2B, 0xA9302474, 0x04C6888F, 0x4D118BF9,
+0x0DB7AAC0, 0x52A38EDB, 0x4DAB22F2, 0x7DBB6EAB,
+0xD4D17851, 0xFD944314, 0x40C5838C, 0xBA6EB0EF,
+0x9AA287A5, 0xF6D236F0, 0x41D9E2BA, 0x6968D776,
+0x31B1D129, 0x42C3F963, 0x27CCAD30, 0xCD61BF4E,
+0x2C7DABAB, 0xA78A9CC3, 0x7F856B6F, 0xB6D444A5,
+0x90CBB312, 0x95611781, 0x4916D531, 0xC496C30E,
+0x706D0CB7, 0x35D0064B, 0xFE26C36A, 0x6211F14B,
+0x2C2340BA, 0x58633567, 0x06B6BA8E, 0xA7EC3D8D,
+0x1071B0CD, 0x388EEFA8, 0x60D8FB1C, 0x5F99D147,
+0x52CA6EBF, 0xFA73602E, 0x0376C15C, 0x3C91B57D,
+0x9386AF17, 0x14A35A1A, 0xBDB42A39, 0x0E83C257,
+0xD4C5C775, 0xA607FA46, 0x91B9AD40, 0x7623C5D6,
+0xE3D53E6A, 0xA3C663E7, 0x5AD39BCE, 0x03B58394,
+0x38862C7A, 0x01D50B9F, 0xEAAB38EC, 0xAB3DFB8B,
+0x06795385, 0xB17F485E, 0xE2F57914, 0xB79A3BAA,
+0x13DA7886, 0x7136C7EB, 0x5E748AF7, 0xD34F16FC,
+0x968F6701, 0x99C5D7BE, 0x530F7FAC, 0xCDF5D567,
+0xE31DE0D3, 0xCF93BC68, 0x34C578AA, 0xA201F761,
+0x5CB8DC00, 0xCA24DB98, 0xF8AD7E4F, 0x808EC476,
+0x603BA751, 0x489555C6, 0xF2A03FF0, 0xD2461E9A,
+0x102C33BE, 0x7673933C, 0xC11A2424, 0x6A23C8C6,
+0x69499812, 0x19AA8510, 0xC8CDA75F, 0x34B5216A,
+0xD87F7420, 0xC8CEDB53, 0x8DF11BA2, 0xB10911C6,
+0x3F1E5955, 0xF075F4EB, 0x17874FC5, 0x0D55685B,
+0x5EE521E5, 0x46C72924, 0xF8540210, 0x5D5E4C5C,
+0xE87A133C, 0x91633DC9, 0x36B54D5D, 0xA8B5D440,
+0x7DB7D6C4, 0x5FA82C17, 0xAD679039, 0x86B3B839,
+0xDF5121B7, 0xC08B768A, 0x338A512F, 0xCF9A4F9A,
+0x5DEFBB5B, 0x4C9301B2, 0x08023702, 0x5B1D7E28,
+0xEC800505, 0x3A869E80, 0x4C50C8AE, 0xB1AE9064,
+0xAFFA34EB, 0xF2F006B9, 0xD8A9A3D1, 0x2C6C2134,
+0x677EE648, 0xBB6B6D5C, 0xA285136C, 0x6C47BF4C,
+0xAF158DC1, 0x0EF75E2B, 0x5B9C74D5, 0x9B8D4BE3,
+0xE495BE19, 0x5940B228, 0x55E62656, 0x3247E060,
+0xBF7094CD, 0x1C1AB380, 0xECEA2275, 0xB6DD8251,
+0xCCA39DD2, 0xAB85D992, 0x278197D2, 0xFB6C9FD0,
+0xBD53B458, 0x89EFE0EC, 0x52A3DFFD, 0xA6B7FF7B,
+0xFB043649, 0x93C93F79, 0xAEB4CD6D, 0x71DB5C90,
+0x9E8DFE92, 0x0F1A5B91, 0x55C5CF5D, 0x1A1847AC,
+0x8D25CF6C, 0x914FD316, 0x39FCFE20, 0xD8F66A07,
+0x2CDD3DC6, 0xE415AC72, 0x3D1BD09B, 0xA8322C59,
+0xBD3A826A, 0x2A988A40, 0xEBD8B1DD, 0x9F53EEEF,
+0xDF571816, 0xD4FCCDAE, 0xB85A1E50, 0xBE1A571F,
+0x0ED07534, 0x4C1E471A, 0x8B4D36F6, 0x0E388FC6,
+0x9ED2BC4D, 0x3E2D7F72, 0x752ACA15, 0x8960B48E,
+0x5892B3D7, 0x70F6F3CD, 0x26C485EF, 0xC83839B9,
+0xFE6C224B, 0x3547203F, 0xF73ACA84, 0x065DCDBC,
+0x8986EBDC, 0xCD59EA14, 0xC0EF58A8, 0xC5587229,
+0x484FBCEF, 0x9B8BF24D, 0x351CF946, 0xE10AA973,
+0x17919640, 0x95FF7B1C, 0x82AB65E5, 0x070BCC98,
+0x0E7CDB8D, 0x38DB27DE, 0xCA543C2B, 0x0131EB41,
+0x8300996B, 0x88B63D66, 0x03ADAC1D, 0xB205A87B,
+0xD8BDC0C6, 0x443F6071, 0x2CE69D2A, 0x6E1E5A53,
+0x4EFF93AC, 0x70322657, 0x5CCDD146, 0x04C435B6,
+0x5BF3CD69, 0x51E09115, 0x2545DFB2, 0xA52EF448,
+0x8D387046, 0x7C4F1F25, 0x2EFFD8AA, 0xFD6422B0,
+0xB82E26A7, 0xCF01CC45, 0x88899EBE, 0xDB621966,
+0xBBA1822F, 0xB264AAEB, 0x1076EAA5, 0xC24B0CD5,
+0x54D554B0, 0x4ECA7C05, 0xC8C9B053, 0x70A86D97,
+0x4E3265CA, 0xEA24F810, 0x873B172D, 0x79A74D18,
+0xEC3F49D5, 0xD1799602, 0xA21A28B6, 0x3FB99AD1,
+0xC2DB35B3, 0x63EC2E51, 0x17E4489F, 0xE8E19164,
+0x79ADD819, 0x10D66157, 0x5F621A73, 0x1CD063BA,
+0x6665815F, 0xFA0B7081, 0x6E0FA473, 0x0CE3571E,
+0xB5EAEF46, 0xAA04CF54, 0x336680CA, 0xDABBFF11,
+0x2259E797, 0xB57B4470, 0x111EB4BF, 0xC171D42B,
+0x5889A7A4, 0x419CCB3E, 0xBEA1F366, 0x41FE414B,
+0xA65CB898, 0x6C28363A, 0x8F82FC84, 0xDBED5A9C,
+0x4DBF3526, 0xF2F34E66, 0x9D2C9B11, 0x0C0D4DFB,
+0x4DBF79D4, 0xA256E86D, 0x6407376C, 0x3F3E8AFF,
+0x474B3593, 0xE55965C8, 0xCB20D358, 0x0C671A9B,
+0x169F8342, 0xD2E1C9E7, 0xBDDBAAEB, 0x93DF0C75,
+0xF27707F7, 0x5108305B, 0x4FF2C060, 0xEB9C08DE,
+0xDF11020E, 0xD2271046, 0x6D1BFD27, 0xED020CDC,
+0x2C22659B, 0x692050D9, 0xD14BE291, 0x3EBF8E86,
+0x8344B625, 0x7840B91C, 0xB702BD5F, 0x4935D318,
+0x01A22013, 0xF2A20B08, 0x651A1C38, 0x004FE633,
+0xE51DCC06, 0xF5B86138, 0x9FBFF118, 0x6F7B3CD4,
+0x028938B4, 0x071E96AE, 0xDF33DC9E, 0x79001AC7,
+0x7B5D20FC, 0x3F137794, 0x81165B04, 0x973F8FD4,
+0x0AE4CBF5, 0x7C48180B, 0x4A96BC89, 0x58066E74,
+0x86669DC6, 0xDC55A218, 0x858C3130, 0x99AEAC91,
+0x26983FC4, 0xEE4D4F06, 0xD8D6D657, 0x18EF262B,
+0x374A620F, 0x85995F9C, 0xCC814AC1, 0x39F487E0,
+0xC628177B, 0x2FAE2C39, 0x642525A2, 0xC1474F2D,
+0xBC7CD49E, 0xE81E13F7, 0x83F42BDB, 0x8AB7D99A,
+0xA8040B11, 0xD8AA68EC, 0x983B3739, 0xEE42ECDB,
+0xC9513498, 0xCAA06A14, 0xE4784094, 0xE6BEBB9E,
+0x13BE8018, 0x59E3D5D4, 0x0CF1728F, 0x963413BE,
+0x319533B7, 0x14662ABE, 0x3363B45D, 0x59A99687,
+0xBBB0FDA4, 0xCDBB8B21, 0x0240F3B1, 0x226DAC3B,
+0x30E1C49E, 0x76E076D7, 0x4B91C598, 0xB3C46E2F,
+0x4A657CC7, 0x66C3875A, 0xCBC6FC54, 0xF832EBE8,
+0xDD1EAD3D, 0xFEFDAF85, 0x8DE51B88, 0xAEAFD5D3,
+0x3E4CEA82, 0x55F47934, 0x9F8314CA, 0xD0220BC0,
+0x5ACEF81F, 0x71FDD8E9, 0x13A14ED8, 0x6F1FC1E4,
+0x75046A04, 0xC6C4FDAF, 0x4FFFF724, 0xF44FEDD6,
+0x7E1C5CBC, 0x784C6B4C, 0x8D85F220, 0x38B65C3E,
+0x8C992050, 0x2DE34C13, 0x9F2A4547, 0x48E58F65,
+0xA280B689, 0x6F540D8A, 0x10B61B39, 0x1C8A2849,
+0xA7316358, 0xDBFB7862, 0x182C553D, 0x92F04389,
+0x1FE7BADD, 0x6A724CBA, 0x970BE020, 0x93760058,
+0x2DF9E0AD, 0xCFF1F8B1, 0x170D810A, 0x45F4E6A2,
+0x37A0E8FD, 0x86D11C6D, 0x4F3C6A3A, 0x4B144452,
+0xCE9B87A1, 0x7C08C30D, 0x9CB9B0AB, 0xD55F2CC5,
+0xFF95180F, 0xF35505BD, 0xED5BDB96, 0x85CA2E41,
+0x8708B264, 0xD6079734, 0xCA76AB3D, 0xFD6CDF4F,
+0x9AAB840B, 0x92D3A5F7, 0x93A92C38, 0x0419AA7A,
+0x1D50006E, 0x126F48FF, 0xACDA412C, 0x01139454,
+0x8E23C486, 0x01D44F51, 0x7A5F6F10, 0x377D4D5E,
+0xB784E72F, 0xA9AC925F, 0xB9C66C79, 0x057331E6,
+0xCFF040E4, 0x77E8A960, 0x35E31EEC, 0xEB807A44,
+0x8594FFFC, 0xD27629B7, 0x5DDF526E, 0xBCF2F484,
+0x88805013, 0x41047850, 0xB8574ECD, 0x3E15082F,
+0x309C16DC, 0x297B6904, 0x30C39ECB, 0xD20B61AF,
+0x51A578AF, 0x4E0D24A9, 0xC61FBE5F, 0x7A89F4C6,
+0x9432299D, 0xFE261B95, 0xDD1FC4CA, 0x044BFB92,
+0x41BE56CA, 0x0A2B6831, 0xE135D75D, 0xAB2D00A0,
+0xB4374080, 0xFAA6DBD0, 0xA704C4A9, 0xD81385A4,
+0x51533312, 0xED5EDAF7, 0xE4EDFAEB, 0x74B7DAFE,
+0x9D810AA7, 0x40B91827, 0x65219BCB, 0x75431C16,
+0x94D923D3, 0x00B7AA4E, 0xB8A88FDA, 0x927278D7,
+0x7A237697, 0x45B14097, 0x2E3A562F, 0x93003322,
+0x0B88A5FF, 0xD13D4ADD, 0x6D7B7579, 0x72D834C4,
+0x0BCAA361, 0xC02E00B8, 0x15023551, 0x481C5E93,
+0x02E81A16, 0x8A846A33, 0x1239A971, 0x994818B4,
+0xFC3DBB6D, 0x43C8D2F2, 0xE3AE548C, 0x408032F1,
+0x02B05636, 0xE361A60C, 0xFE2CA292, 0x061D2374,
+0xDB285556, 0x70627EA4, 0x7FC64AF0, 0xFE100B6D,
+0x71AEB3F2, 0xA565A412, 0xA698731F, 0x49DD9767,
+0xC3627EBC, 0x75FB2DBF, 0xFDC0E971, 0xF6ED12A6,
+0xA23DC00F, 0x897E917B, 0x7F2031E0, 0x17DCE568,
+0xDF69CAD3, 0xC6FB5B6D, 0x097268B0, 0xE1102444,
+0x86DF9383, 0xBD7B9CC2, 0xBAAF7DCF, 0x985B45D1,
+0x4218E95A, 0xB2455EF4, 0xDB015F9B, 0x54CCCE76,
+0x56EDF561, 0x6F66F95E, 0xF8B1EBD0, 0xF7A39AE0,
+0xF66D8346, 0xA4677007, 0x02C4B3EB, 0x829987B0,
+0x7C0E1919, 0x51F7060B, 0x4B30F1D6, 0x85A4E0CA,
+0xEC049FA0, 0x17CBF1E4, 0x7A1AAD95, 0xEBA4C513,
+0xE8462E78, 0x54CDDA0C, 0xEE7B8378, 0x9858C8C1,
+0xBA33587C, 0x4D6F1B14, 0x7A2C0525, 0x7E6EE4D2,
+0xACA18692, 0xDD186820, 0x41198B03, 0x8AC85AB7,
+0xBD86900B, 0x36E2C354, 0xE65F9115, 0xB10645DA,
+0x7971D230, 0xC83D3583, 0x8C60C81D, 0x94DB5741,
+0x4FCB8934, 0x9A520FE2, 0xCE49446D, 0x8864E641,
+0xF5EF25A5, 0xC1DEED0A, 0xC8057F37, 0xFB305C73,
+0x392E670D, 0xA4D00D2A, 0x356A46F0, 0x2F675567,
+0xB7997CF0, 0x88AF3A4E, 0x56C9D51E, 0xDD746ECD,
+0x40CFA453, 0x5EA740CD, 0xE4DD6BB1, 0xCCB31429,
+0xA2227F3F, 0x18A1EAF0, 0xC155417B, 0x41FE735F,
+0x16D40B00, 0xC9F72AFC, 0x86B1D62D, 0x6A99A82A,
+0x09D33248, 0xEC44639C, 0x9B0AB2B2, 0x6969164C,
+0xEF602BB1, 0x0208FC6F, 0xC1109578, 0x2997AB87,
+0xE5626B14, 0xCDAF48E1, 0x20781633, 0x2EBE0A41,
+0x7379261E, 0xF216F7A1, 0x714D8258, 0x936FE68F,
+0x160856F9, 0x2A4D1416, 0xB558E412, 0x7DB196DF,
+0xDC88CCB2, 0xF37AB612, 0x7423F214, 0xD3B06A43,
+0x25A8012D, 0xC1C69FFA, 0x936F2C18, 0x56D77C19,
+0x774BFC69, 0xF5E85E24, 0xD79158C9, 0xA67C3E15,
+0xB958819E, 0x69F81278, 0xF2B35107, 0xBF2F4085,
+0x1C997A06, 0x6C238C3B, 0xC756D56E, 0xD15C1149,
+0x351E6EC4, 0x2311303F, 0x0621602C, 0xB11B6DD1,
+0xBE8E50B5, 0x34A5F589, 0xE4D308AE, 0x4344B297,
+0xA33AE98D, 0x0A303CDB, 0x388EA17B, 0x0107B5A5,
+0x38B39042, 0xFE678995, 0xB426FE69, 0x221FCF06,
+0xC45926AB, 0x21A430F9, 0x6D192D2E, 0x4168C10B,
+0x5BA6B132, 0x0519ECA7, 0x21127582, 0xF6C447E0,
+0x0C72FC31, 0x0941B3F0, 0x76F23877, 0x86CF0677,
+0xE7785105, 0xA4637864, 0x94C82B45, 0xF60FD6A0,
+0x46941C27, 0x7A33A698, 0xE1DF8BFB, 0x5249970B,
+0xDFE65E1C, 0xF4A4FB22, 0x599639F4, 0xFE0E9722,
+0x7BB48F58, 0x533465E3, 0x9E884B35, 0x2620429C,
+0x2875FFC1, 0xF11EC0CA, 0x663AF5F0, 0xB2C59C38,
+0x03556ED9, 0x271E9E39, 0x8556E062, 0x08207682,
+0xE5797F00, 0x66A362B5, 0x7ED8394D, 0x2922C374,
+0x271657BE, 0xAC15071B, 0xE296691E, 0x0FE2C740,
+0x19120FB5, 0x9ABD888A, 0xA200762C, 0x7837F41C,
+0xC6F4EA19, 0xF286ABF4, 0xFCA8998F, 0x97B0E7D5,
+0x1339C79F, 0xFED05D43, 0xB3392E71, 0xFC2A01EB,
+0xB720CBED, 0x4FA71358, 0x04A57F62, 0x3D558B0A,
+0x1DEB4D40, 0xC9C823F1, 0x470F630A, 0x08F22975,
+0x2BD85107, 0x3288A628, 0xB0C89675, 0x32D957C1,
+0x80B78426, 0x98A46953, 0xA493AF60, 0xC2B84AC4,
+0x486D658F, 0xFE119FF9, 0xB2FE565F, 0xEADB58CD,
+0x1F45F9B4, 0xCEAE62B6, 0x68EC702D, 0xF52ADDF7,
+0x0FFC0715, 0x4129E42C, 0x956AC4D9, 0x0035CD9C,
+0xF8FEBAA1, 0x29C58397, 0x7C2E2E41, 0x7BE74DAF,
+0x2791D34D, 0xB6D67B0D, 0x8F557528, 0x9DDEED5B,
+0xB3AA4BB7, 0x05E22E43, 0x4CDA600D, 0x432E2D32,
+0x405DA5BD, 0xAF23818C, 0x2F73FE09, 0xD4624626,
+0x653EFCB3, 0x77D65D3F, 0x51A3DCB3, 0x767F407C,
+0xC66452E3, 0x10B6842E, 0x93A0840E, 0xE453AD10,
+0xDE58FC3D, 0x6C227215, 0x1EE130EA, 0xB0BF64BE,
+0xA11E5D38, 0x0131B755, 0x191F70D0, 0xDB483959,
+0xAA8D2F9E, 0x5A002AA0, 0xF5A2996D, 0xFD0F95F9,
+0xD6A12864, 0x3AA48B74, 0x50F6679F, 0x0ADF5C49,
+0xE2F8CE68, 0xBF213E67, 0x5E9ACEEA, 0xCACD0EBE,
+0x6DF766A5, 0x33C0A156, 0x720868EA, 0x3112A0DC,
+0xB382350A, 0x369D9C50, 0xE8F890D0, 0x0A121399,
+0x2AB458EA, 0x51C8233D, 0xBF46403C, 0x0728CD55,
+0x23F6774B, 0x2FB59DB0, 0xFA2CF724, 0xB49FA848,
+0x5FFFA125, 0xDE2C0D15, 0x76B78C41, 0x192BA62C,
+0x4C9563E2, 0x8F742507, 0x882104E0, 0x357AD078,
+0x799E25A2, 0xEF3ED021, 0x69D54B46, 0x5EC57870,
+0x0FF418E0, 0x07C5AC7F, 0xC1ACBF9A, 0x80A830D9,
+0x837C7C5A, 0x04C11D86, 0xC14C8BC7, 0x92BA650B,
+0x94D34FA8, 0xDBDD5EDC, 0x9ED2A08F, 0xA1FAE485,
+0x5FD66C3D, 0x4CCB6F9F, 0xB7AA56B0, 0x0FB3C73A,
+0x03AF96E6, 0xDB2D38F9, 0x7AF20D60, 0xB57CBE90,
+0x20EB2D6E, 0xCF934452, 0x82EC26F6, 0x84B3737A,
+0x0972F1B7, 0x39B6DB4D, 0x13E53CC0, 0x67C41D72,
+0x94BAAC78, 0x663A9C6C, 0x36927448, 0xCFBC2610,
+0x980F53BA, 0x7E56C96A, 0x04C62DFB, 0xA471D579,
+0xDF9B2EE1, 0xE12DEBB7, 0x2DB9B042, 0xF0C74B96,
+0x6A3762E9, 0xF4DC39D9, 0x761A5884, 0xFA363D3B,
+0x92766759, 0xF3EAD441, 0x878269ED, 0x1AFFAFE5,
+0xCB432764, 0xFE19475C, 0xCF8776DA, 0x1F0AD906,
+0x7D99AC20, 0xC27317FB, 0x439944A4, 0x65D14C2D,
+0x43E45262, 0xCDE6B3BD, 0xE25C67CD, 0x321AA2E6,
+0x352A2764, 0x5569EF42, 0x005C370D, 0x290801E0,
+0x61883035, 0x2A2DBC48, 0xE2D559FF, 0x01F5DF13,
+0x69B61558, 0xE94BF364, 0x3CA76FCA, 0x2E016483,
+0xDB675F9C, 0x4FA5B6DC, 0x59A6C3EC, 0x56C6E6CF,
+0x24CD59F5, 0x46911834, 0x683B9E39, 0xB5AF6174,
+0x5C31E269, 0x679C9A12, 0x3787D3E6, 0xF1727EE6,
+0xB070882F, 0xFC37EACA, 0xBEE0783F, 0xF6218369,
+0x19372940, 0x3FF7D890, 0x69736919, 0xDD961CB9,
+0x883010F1, 0x6E472D5B, 0x2447E00D, 0xF39E1F0E,
+0x1DBD442F, 0xBE1977E0, 0xC8655F42, 0x37C84253,
+0x3480DAC4, 0x4CFE1DC8, 0xF1521AD5, 0xA45C4F8C,
+0x87FBAEE0, 0x3E41E9E2, 0xF47771E5, 0x16C74CDF,
+0xA33D4035, 0x38513A10, 0xABF3264D, 0xB8D80DF6,
+0xD9AD7256, 0xF78375B8, 0xD7661CF7, 0x1C363AF9,
+0xD425FA32, 0x001D7B98, 0xDB96A1CC, 0xA092E683,
+0x65CF5316, 0x5F282689, 0x9F52F912, 0x8958A1B7,
+0x6457A3F7, 0xAB43FADD, 0x061328C7, 0x9D31B5E3,
+0x75A77F6D, 0x4A764D4A, 0x488CE83E, 0x29887218,
+0x9A04BDD0, 0xEF331070, 0xBCD2F884, 0x6BF66A6F,
+0xB85143CB, 0xFA529278, 0x9EA3A354, 0x4A73BDAF,
+0x0CBB7563, 0xD01AE35F, 0xD2AC3DAA, 0xFC8243B7,
+0xD805D97B, 0xC162A75F, 0x1D49AC67, 0x9E1BC38C,
+0x1D06AAE8, 0xEAF80CD8, 0xCE825DD4, 0xACA3F06A,
+0x83D092EE, 0x3F2BAABC, 0x2482D120, 0xF301680C,
+0x7DAC373F, 0xF5D6178D, 0xB7E9217F, 0xCCFE8C13,
+0x976024E0, 0xA2F39F8C, 0xB6C65734, 0x10AE514A,
+0x696584CF, 0x2542113C, 0x479CB20F, 0x8D3A22E3,
+0xF7C4B88C, 0xF4F7FBE2, 0x2F553308, 0x9EA71E3A,
+0x7B958F48, 0x0927DAAB, 0xF08949B7, 0x7CD46C0E,
+0x7A892BBC, 0x882F32CE, 0x34C490C8, 0x8483ED04,
+0x07EB4EFC, 0x4BEBCD82, 0x83B15EE8, 0x8F3B78AC,
+0xF95EFDA9, 0x816BEBF9, 0x269BDA58, 0xEE373342,
+0xE09FDA9F, 0xC7651AAB, 0xB8D398B2, 0xC7F449B2,
+0x031310F5, 0xC869706F, 0xDA22F127, 0x8C68DF91,
+0xE676068A, 0xB85AAAC7, 0xD32F35BC, 0xE22DF031,
+0xFE142BD9, 0xD4FB2700, 0x2D197707, 0xA3A43A64,
+0x0C02B050, 0xE945AD56, 0x7DEE0A5D, 0x1075DE3E,
+0xD99AD91C, 0x6A7BB71D, 0x1774B3B8, 0x2228B112,
+0x0DEEE844, 0x38074EBE, 0x6DACF57B, 0x7E0094B7,
+0xCE46F8EC, 0x4DAF34F4, 0x5B961907, 0xC8236FF7,
+0xFD380AA7, 0x61EBA84A, 0xAE4892EB, 0x0F1B6365,
+0xB0C4C9A0, 0x04E6012D, 0xA5F90D01, 0xD6C8882E,
+0xBCB9C1EB, 0x0E5E0FEC, 0x53A46889, 0xA2C0FA51,
+0x520DA459, 0x3FD95FA2, 0x6E1D6FE8, 0xBC093220,
+0xAB16390A, 0x163E3D6D, 0x0A63517C, 0x3BF38F3D,
+0x88A1F66D, 0x96263536, 0x412DF008, 0x12FB126D,
+0x44441D7A, 0x31C9F726, 0xF66F60CF, 0xAE1453D4,
+0xDAEAD71B, 0x54EAEE0F, 0x948B73BB, 0x31EA3E74,
+0x355D4FDC, 0x2A1F3A9E, 0x586D08DF, 0x123AC2E8,
+0xF5AC0065, 0x8874ACAB, 0x05B03D63, 0x01BD6A4C,
+0x7A6A9880, 0x2BC16F93, 0xC4112F0C, 0x8287B40D,
+0x48EABF08, 0x29E56860, 0x0F505C84, 0x447DC08B,
+0x1665119C, 0x00347E37, 0x482EF03E, 0x01B15D44,
+0xE6C1B9FF, 0xB165E436, 0x0CF690F7, 0x7FC5BD01,
+0xB784C7F4, 0x9BE04EBB, 0x9F614431, 0x6C37A5A9,
+0x2D0DB87D, 0xF6511369, 0xE115073A, 0xF96C6AB6,
+0x04A13C3C, 0xBF30B2DA, 0x93D18FC6, 0xF67D2E47,
+0xCA089151, 0x51A6BC39, 0x8C1FCA93, 0xFBF2F2BB,
+0xAD0A3F33, 0x82AA2767, 0x81BF2313, 0x758A82B8,
+0xE103788E, 0xC00C4B5C, 0x5F52FF58, 0xABAD38F7,
+0xDA68EE9A, 0x9B6D405D, 0x803449D9, 0x6178B345,
+0x3C785FB4, 0xFEBABE55, 0x0E2458AB, 0x021F0D71,
+0x39201ED1, 0x741B1A7D, 0xE0B0AFF4, 0x45652CFF,
+0x907DA678, 0x313A93B4, 0x0B0D6B0D, 0x42C96E43,
+0xEEE3E7E1, 0xE83C83E9, 0x9052B867, 0xF9514243,
+0x61F20CB2, 0x57E1AC64, 0xC2443123, 0x432C96D4,
+0x616A824F, 0x3C8D1E06, 0x8E64222A, 0x65C1A21D,
+0x8686308A, 0x2A576A2F, 0x1CA0FF20, 0x2C8F9D3A,
+0xC98C9C69, 0x35322A29, 0xDFD33C93, 0x9634F411,
+0x0B4F8FFC, 0x3AED4B01, 0xEBBC7012, 0xED2387EA,
+0x48BF42AF, 0xD60399D6, 0x7A9B8CA9, 0x53886337,
+0x2DBB9429, 0x0A6AF764, 0xDE4D8F78, 0x1EDECEE4,
+0x4F8EE99E, 0xAF23EAFD, 0x929550B1, 0x2CBD8621,
+0x22A8FAA2, 0xBE2A0A8D, 0x06F7E794, 0x16E1F3EC,
+0x093AAEAA, 0x92D429F8, 0xBB79A7E7, 0x43EF89BB,
+0x0E097511, 0x748E68B0, 0x322C00AC, 0xA62EF42A,
+0xD03BB8BC, 0x9FF67810, 0xDE24BF03, 0x140CA6FD,
+0x68F16B41, 0x1B7C68C7, 0x32646342, 0xC5E714F8,
+0xEFFFD2B8, 0x27843628, 0xF8445F51, 0xB9E8519B,
+0x8EB01D04, 0x356FBF2F, 0x32E96BAD, 0x6A629BDE,
+0x52063313, 0x200069B0, 0xE161CF71, 0x84FB7A12,
+0x1805ADC0, 0x80F75012, 0xFE9E629E, 0x93395C33,
+0xFF075A91, 0xB61E46B8, 0xCA9FE7C8, 0x97DCCBCA,
+0xCEFFB6F8, 0x30EE7985, 0x1FABC829, 0x20B3F57B,
+0x27042B07, 0xE12C5151, 0x23482B8A, 0x7B9B8EB2,
+0xC997FEB3, 0x76AB2497, 0xD5CDA590, 0x9EBE90FD,
+0xE3732B18, 0xFF28CEC9, 0xC6582320, 0x6EF106FA,
+0x8ED74023, 0x1A0B69E5, 0x4A95DD91, 0xB41AF82C,
+0x83DF69D3, 0xC548861C, 0x2F60BA93, 0xFC815984,
+0x1A848B67, 0x1EAE87C4, 0xF7479103, 0x8E16DB51,
+0x040B95B9, 0x2A9DB812, 0x987AFCD1, 0x866DF413,
+0xBF9558ED, 0xACF1AF2F, 0xA65305CC, 0x168336F3,
+0x1E59B97F, 0x3F9F447C, 0x3D54B30D, 0xE939D598,
+0x36A40885, 0x02396794, 0xEB0F0A67, 0xCEAEA12F,
+0xC58B4AC8, 0xE6D49760, 0x0F8F2776, 0x66A8F436,
+0x31BACD7D, 0x376993DE, 0x32BD0431, 0x68BDC728,
+0x63EA6748, 0xE6B00E29, 0x7448CABC, 0x42A6517D,
+0xBB1313C4, 0xA04DC8FF, 0x3D402237, 0xA382645F,
+0x52ED55D6, 0x92D7D7B7, 0x541230FF, 0x7AFC0420,
+0x3DC4624F, 0xD9B2193D, 0xA73B9704, 0xBBDE0FF1,
+0x9EB56615, 0x8AB080B6, 0x3C4D8E14, 0x5001B43D,
+0x1EBFAA23, 0xD4AACD27, 0xCFAAB4BB, 0x6FFEE61F,
+0xAE5A7426, 0xDB942949, 0x452C0B16, 0x738E0637,
+0x36A5122D, 0xFF1F7A4E, 0x743D35CF, 0x847D54A9,
+0x42C3EABA, 0xD46728C5, 0x30B2708D, 0x4F6BE0BC,
+0x3C26790D, 0xB0B67C8A, 0xEE07EFDC, 0x9E380611,
+0xEAD6804C, 0x4EF66024, 0x8459AE38, 0x1DEAAFFB,
+0xF76573AE, 0x6CB1C8F0, 0xFFCC267E, 0x26A215F7,
+0x0B1A057C, 0x7DAB9CB7, 0xD40BCBA9, 0xE561F9FE,
+0xA44013A6, 0x7B22C0B9, 0x998A921F, 0xBD25244B,
+0x15E07FED, 0xF15B2E31, 0x54E80016, 0xA12BCE7F,
+0x658A2093, 0xB642C47B, 0xD731FC00, 0xC00E302D,
+0x55B251DC, 0x342939EB, 0x6EADB2F7, 0x0CF93318,
+0x61EBD85A, 0x99B715EF, 0x679C8D3A, 0x9CC1B803,
+0xABEF955E, 0xB8CFF9D4, 0x707A839F, 0xF5D02A7E,
+0x59E0D903, 0x5A425E3B, 0xBB61163C, 0x96ECE9AA,
+0x797B82AA, 0xA9FA6BB6, 0x797C00DC, 0xC1C1FC4C,
+0x8F7FDA66, 0x77902514, 0x6D1B843D, 0x4F881FA4,
+0xC24AD625, 0xBC237A45, 0x9A2E0F44, 0x82FAA3F3,
+0xD70E3489, 0x4F2B3417, 0x65CF65E4, 0xEAAE6A93,
+0x4BEAEC2C, 0x4918723D, 0x7D8F30B4, 0x7706F59A,
+0xCB2A7452, 0x5083D2D6, 0x4724B426, 0x84EB15DC,
+0xBAA2C6CF, 0x71FA984A, 0xDDF7A3DF, 0xB115BF1A,
+0x258AF0E3, 0xA1637D87, 0x03585DF8, 0x5EA4B80D,
+0x8641F318, 0x66EE2F24, 0xC81E505E, 0x5E640639,
+0xDB7739B8, 0x1A3B861F, 0x0F5ECC51, 0xB21C00DD,
+0x680FF30B, 0xDE697468, 0x57A43B33, 0xD7EF6B3B,
+0x4BFC7D25, 0x710F0752, 0xABAA9752, 0xCFCFD84D,
+0x3BCC1CDC, 0x2381C524, 0xB60CAD92, 0xE05BC1AA,
+0x2B887D88, 0xCD4566C5, 0x0D2976E7, 0xCB000A2C,
+0x667BECF6, 0xEFC7F221, 0x7A7584D1, 0xC41D8B2E,
+0xD9BB7D3F, 0x7CEB5626, 0x7D8165A0, 0xEE178F99,
+0x3E8A8CB7, 0x693D4501, 0xB0E228A5, 0xD55B73C1,
+0xAF9043BF, 0x6C627A2C, 0x7B9F490C, 0x7EA61899,
+0x92B980AF, 0x6D13C758, 0x2C007C73, 0x74336E0D,
+0xA39F13AC, 0x533F05D7, 0x75536CFB, 0x9708DE27,
+0xE2A14E87, 0x36673FEF, 0x71BA654F, 0xB98CD2FC,
+0x27F29A6E, 0x82478171, 0x1C2815F0, 0x8A8F4549,
+0x048A8D9B, 0x7CEE51F2, 0xA1648AC3, 0x004F8B8F,
+0xB6FE8EF0, 0x6D10A0A1, 0xAD7A24D8, 0x75039717,
+0x97847786, 0x2791CC05, 0x6937FD6F, 0x60F98115,
+0x5FAB6D35, 0xC0550A70, 0xC0F4D817, 0x7B5BFDDB,
+0xEF63B4D2, 0x6C87C6C5, 0x956D6B87, 0x69179257,
+0x10973C90, 0x8CDBE860, 0xC7C761EE, 0xF823E34E,
+0x6FA2CF3B, 0xA903ABCB, 0xC82C9B01, 0x60FE96E6,
+0xE5EC33C0, 0x73A3011C, 0x2A1B9054, 0xCF16F92D,
+0x4FAF6CC8, 0xD9DD74FE, 0xB3C639ED, 0x3F47AF63,
+0xC8E99D12, 0x92D95986, 0x835ACA6F, 0xD52930A2,
+0xC7DD54A5, 0x617FDD15, 0xE9A6D295, 0xF56C6087,
+0x7813B662, 0x1F8EA244, 0x1CDE3BAD, 0x58FC0F7B,
+0x02E31A5A, 0xA78EAC74, 0x10C06107, 0x22BA3C63,
+0xF84AD224, 0x6A8BF66C, 0x2A5CAAC5, 0x8ADC3FB5,
+0x9683451A, 0x1B52FCB4, 0x95491BA5, 0xFE6C3713,
+0xE9098CEF, 0x73C01EF9, 0x6E85EF1A, 0xEE189743,
+0x2E9E5286, 0xC1FAA665, 0xD861E384, 0x701C834D,
+0xDC5CA5CC, 0x52A3A6C4, 0xF2AF2C43, 0xC37C6465,
+0x6E94AD69, 0x98808AF4, 0xED8A99F2, 0x377257D3,
+0xE60F2096, 0x615EFCB8, 0x67A2BB3A, 0xB4DDD40F,
+0x1D47F918, 0x86F77D6E, 0xFD05D2B8, 0xE18C330C,
+0xA48260A4, 0x5615B83B, 0xBCD7D855, 0xF8073219,
+0x8622BB89, 0xD35CE05B, 0x17162483, 0x137BDB69,
+0xECD0F226, 0x61F8982A, 0x3C10ABD4, 0x2F33ABF4,
+0x9358B547, 0x58B277A7, 0x92456A7C, 0x4384B49A,
+0x5F1FF0EC, 0xA153EA4D, 0xA8E49100, 0xD3A75723,
+0xD1ADC606, 0x76C314B7, 0xBC6AB227, 0x257312AF,
+0x8B6AA1E3, 0xD87FF5E8, 0x2BAED373, 0xC848AB63,
+0xB72B1E5E, 0x730A73D8, 0x4915E5B6, 0xDF7D77AD,
+0xEAE247D7, 0x9556DDA8, 0xDE0C9C47, 0xA4E3296E,
+0x31F5BC94, 0x05258B24, 0x2837374F, 0xC7E4C81B,
+0x5A1AC819, 0x068074AE, 0xDF876732, 0xC0192EF9,
+0x7FFD84D8, 0xFF1CE148, 0x821B4AA3, 0x56674838,
+0xF9A147F4, 0x182EF58B, 0x16E17174, 0xDE27029E,
+0x8BEC55AD, 0x40646F89, 0xDBFF92FC, 0x9F24C017,
+0x711EAD18, 0xA663E1EF, 0xEF92F684, 0x4BD05E67,
+0x7E089B13, 0xCBF619BE, 0xCEBEF231, 0xC947586C,
+0x0F526C47, 0x6672600F, 0xDAAB63DD, 0x950D4FD0,
+0x199C3EC2, 0x0F201C9D, 0x06BCC8D3, 0xA7672C6D,
+0xB39C7D0C, 0xC74B0805, 0xC9BBD249, 0xACDD5396,
+0xAB7BDF8E, 0x12012B8E, 0x67236047, 0x0AE0741B,
+0x1D747E56, 0x7EC6C00C, 0xD08E8341, 0xB0ABDAD6,
+0x4FA4BDF6, 0x90CE8D0E, 0x6E734117, 0x3EF9192E,
+0xACA32DA2, 0xFDB9C58E, 0x256626B5, 0x5EA961B3,
+0xFBC15776, 0x36602B5F, 0xF8D08644, 0x5B693C23,
+0xC62EA3B1, 0xC664C7C3, 0x73BE8859, 0x17F44E8F,
+0xF9B8D923, 0xD168A3A5, 0x6CCD110C, 0xD353181F,
+0xC0E774EC, 0x5F9E127C, 0x6C824511, 0xFDA13494,
+0xCB588BA6, 0x47148694, 0xAB877E87, 0xE97F757B,
+0xF54D0A2A, 0x0FE11891, 0x5D8747FB, 0xE7800C7E,
+0xEF96298F, 0x400F458A, 0xE2D04518, 0x4B4E6EFC,
+0x9B15002C, 0x3CE1B537, 0xF5ACB9B8, 0x67030647,
+0x475FD148, 0x1E03A40A, 0x896C7C05, 0x85F70B68,
+0xC590CA84, 0x53B5440E, 0x1400F78F, 0x3ABE7F8A,
+0x19CA67FF, 0x68B54A34, 0x555988AC, 0x4AB16B4A,
+0x7511FA63, 0x248EC9EC, 0xC25AFE4F, 0x19F578E1,
+0xE92AF03D, 0xAF9DE18F, 0x2798C7A7, 0x6B46990F,
+0x41D45894, 0x74696A0A, 0xC6AAF5F8, 0x72CC10E0,
+0xDB9CA283, 0xD6BBD0F3, 0x58EA4C06, 0xDEA5E8B9,
+0x1908EBDB, 0x95D33DD5, 0x20D7013C, 0xE725C282,
+0xFD48C92F, 0xDBBA7D19, 0xC7BEBEA9, 0xB186B799,
+0xDD0DD17B, 0xD8090A41, 0xF98BC20B, 0xDD7E4B9D,
+0xEBAE4247, 0x4376FDC4, 0x7F3EFAC6, 0xA9B9A951,
+0x4AE390C4, 0x651863AF, 0x2CD42DBC, 0xC2A13962,
+0xEF0FC443, 0xAEE63246, 0x09B83E19, 0xC3C940AB,
+0x00B12826, 0xC0A30412, 0xFCF6ABCC, 0x3CFE721A,
+0x62C1F4C6, 0xE963A359, 0xAE11F3D6, 0xE490D12A,
+0xC45C928B, 0x05CCA78A, 0x1982E93F, 0x577F81CA,
+0x66D50D6E, 0xB4C7030F, 0x93092C3E, 0x118B08FF,
+0x178545B7, 0xEED74838, 0xF7D2CE48, 0x238969BC,
+0xB8EFAEAE, 0x75726A3B, 0xB1E0220F, 0xC4D60EB6,
+0x0EBC0243, 0x5FE0D6CA, 0x35456B45, 0x1F64AC2A,
+0x58484A1F, 0x2A11455D, 0x33BC4403, 0x56E4E62D,
+0x60B41E2B, 0xDB65D3F8, 0x7EC18D34, 0xF575DC85,
+0x6E0B9995, 0x1C14C91E, 0xB2A94718, 0xAEC4A823,
+0x993D374E, 0xF1E4210B, 0x8CFCC03A, 0x99BD1C28,
+0xA928E3F9, 0xBB957D0E, 0x77C865EF, 0x7FF50A45,
+0x4279A638, 0xE628FFA1, 0xBCCA171E, 0x284C9CEC,
+0xA476E346, 0x7E2F9C08, 0xBF65044F, 0x5B7C3D5B,
+0x6E60EE5D, 0xF5C99509, 0xFA352B7E, 0x6FDE8E8A,
+0xF2340FE1, 0xDF542B6C, 0x510CB30B, 0x367E7016,
+0x198A0A95, 0xA4DF508E, 0x593C2338, 0xB12BCDE1,
+0x554AD3C0, 0x4DDAB1C1, 0xD2BD1850, 0xF6E126CA,
+0xF87289C7, 0x86EC92A5, 0x4E033906, 0x52DC5F3F,
+0xCC6E2E59, 0xFF751753, 0xDF8B8BA2, 0xDBF5954A,
+0xBD367488, 0x6A0CDF1F, 0x4103139C, 0xDE49DBB0,
+0x5A8428F4, 0xA26872B1, 0x96BF7203, 0x99D5E78E,
+0x243850A6, 0x389DAD80, 0x6335D33F, 0xEC67B0A5,
+0x029C0CA9, 0xF5F6F6C9, 0xDF574C15, 0xE6D3EC29,
+0x1AA349BA, 0x453E7258, 0x7DB79BE3, 0x51FCA7F6,
+0x2B42FCA5, 0xBF0E4871, 0x58063C40, 0x193580E2,
+0x25605322, 0xBC49C479, 0x0ED70FC4, 0xA78B59A0,
+0xE6CE3E8C, 0x92EE657A, 0x63D12529, 0xF95DAF45,
+0xF92C3BF3, 0x7D514200, 0x694DF84A, 0xEF177E2D,
+0x4E119CCF, 0xA025C55D, 0xF96974D6, 0x26D13E7F,
+0x799ADC27, 0xD7925EC1, 0x8AE60BF7, 0xF9EF1A2E,
+0x89EADD3A, 0x9C28CACF, 0x63377EB7, 0x6D1EF7E5,
+0x6585B16C, 0x9972D115, 0x65F8F5E6, 0xF93DECB4,
+0x6D71605D, 0xC6FDBCB8, 0xD937BA31, 0xCED727EE,
+0xC34C5605, 0x25FA70B6, 0x5C0B7FB0, 0x8F9340F5,
+0xA3376693, 0x4498B66A, 0x2D21F377, 0xC0A4C6EA,
+0x0780736B, 0xF42D7F07, 0xE56D47E5, 0xB48C25D6,
+0xA48DA0DA, 0xFE69693F, 0xF01E19CA, 0x8A0C5C8F,
+0xDF702C23, 0xE18A93F0, 0xD4D5C91E, 0xD2A706F7,
+0x674F9E28, 0xAF0F80C7, 0x648D49E8, 0x6BE8640F,
+0xF5FCFFD5, 0x8EDC391E, 0xE583D8BC, 0x8426C090,
+0xF456A27D, 0x07249BF4, 0x054A2F45, 0xAC46B73B,
+0xB89EEDFB, 0x48EAF867, 0x69B2D7CC, 0xCA0CA0F1,
+0x38CD0428, 0x029808CF, 0x86EE75DC, 0xF4FEE9F0,
+0x6987D5E9, 0x56AB5537, 0x3DDD0940, 0x4742FF89,
+0x2C3B179E, 0xD05B5CB1, 0x3C4E9033, 0x6BCF0141,
+0xF2F6D3E2, 0xAD297B1F, 0xB1CC23D4, 0x5452038B,
+0x1751FCBE, 0x24AA465F, 0x94C62D18, 0xF49B2EC8,
+0x97AC47DF, 0xD66C19B5, 0x09AAB297, 0x89936144,
+0xD15C026B, 0x4CEC8778, 0x94050D61, 0xD812E96F,
+0xB6BD7B12, 0xA5F9BE77, 0x531A5C7A, 0x3605BA71,
+0xD500CE54, 0xE325964C, 0x323432FE, 0x580A9DC8,
+0xD25A3135, 0x089D6C9C, 0x58856F73, 0x7DFCEE30,
+0x7DE2580F, 0xF4E4488B, 0x71821DDF, 0xD194F5DC,
+0x7D070394, 0xBA28BF76, 0xAAF0A38E, 0xD4F6275E,
+0x1B742E66, 0xD9E68EA9, 0x68B0F939, 0x52AF9D7B,
+0x54A39705, 0x20F844C1, 0xE6981DDC, 0x80322E62,
+0x536235B9, 0x7A57F4FC, 0x14EBF376, 0x64BE2E5A,
+0x70A18910, 0x0FE09587, 0x10E9CA78, 0x8F90D3D2,
+0xAE74717D, 0xA544EAED, 0x6746AF3E, 0x430CB3FC,
+0xBC185576, 0xEAA35DC3, 0xDA6309D2, 0x40643F87,
+0x68859117, 0xA17AC84D, 0xD7922CA8, 0xEF7C0BEF,
+0x83337348, 0x9B4B1790, 0x8876A77E, 0xF293C9C7,
+0x20D399CD, 0xA78224BA, 0xFD1279C8, 0x8B7837C1,
+0x0F1DD415, 0xAE3FBD2E, 0xC4F77B52, 0x51E79FB3,
+0x7A856D9D, 0x14BFDAD7, 0x993FB625, 0x667C65EF,
+0x32F83338, 0xAA06EDCE, 0xACE7A099, 0xD26DAE89,
+0xDC6891CE, 0xCD2F6F04, 0x27425FB8, 0x7C301D8D,
+0x1EDEBE1A, 0xBE540AF8, 0x1D356C6A, 0x963E8639,
+0x9920CA55, 0xDEFE5F44, 0x107D5545, 0x3D079BE4,
+0xEF673F66, 0xDB3C2954, 0xDD76D666, 0x1DFBEF59,
+0x8F384B34, 0xBE6F773C, 0x079DD187, 0x2314AC8B,
+0x5FEB0114, 0x59E85CF3, 0x9BFE9190, 0xB360A31B,
+0x4F7EF967, 0xFEB0D561, 0xBFE779F2, 0xF33702B3,
+0xBB263417, 0x09607C65, 0xA877F109, 0xBB43CFF1,
+0x4A190DB2, 0x9B7BD38F, 0xAEB7C449, 0x3DB3A460,
+0x7D928522, 0xD18AC966, 0x187FE766, 0x97629792,
+0xF59D506E, 0x6FBA202C, 0x77035FF3, 0xDA068CDE,
+0xE195779A, 0xAEB92298, 0xD2A44EDD, 0x12577D85,
+0xA3B47B9E, 0x5BD07CB7, 0x4B6AE3FC, 0xBE35B6E2,
+0x9D7F7AF2, 0x9A38EA75, 0xD87FB055, 0x3339F2A3,
+0xD7CB82B4, 0x357721E4, 0xBEF46553, 0x9DE28CA3,
+0x1B1EC2DF, 0xE29B9CC0, 0xEFAE347E, 0xE5864917,
+0xA097B712, 0x6B67041E, 0x5B29542F, 0x01D96EED,
+0xF9A6DC07, 0xC0B5E3F0, 0x21E1899C, 0xE9373A86,
+0xF3176509, 0x950844A2, 0x7D24FFEB, 0x5DC0BCA0,
+0xC442B7C1, 0x37DC6EC1, 0xC65C8BA5, 0x18F0FA85,
+0x2AD80D2D, 0xC68CDCBB, 0x6AE5EC93, 0xE3955DBD,
+0x3E80C4B3, 0x50FED127, 0x743CABC0, 0xD0E91707,
+0x9BF7EB4B, 0x7A632755, 0x9A192482, 0x8F923E9E,
+0xE2E70FE5, 0x5F50AA16, 0x0EC496D1, 0xC6EC4862,
+0x040A0274, 0x2FC951C2, 0xF65D3A80, 0x8D585163,
+0xC6B529D1, 0xD2CAEE6E, 0xE3E112B7, 0x3244312F,
+0x1B393E58, 0x2444D538, 0xBE69AC21, 0xC92A0506,
+0xD1A74434, 0x49C3EA05, 0x0E53B319, 0x3843CE03,
+0x8DB8415E, 0x766B6FC7, 0x515B9E7A, 0x3BA05B32,
+0xBFAFC449, 0x31302A57, 0x1960A211, 0x66A097E0,
+0xBC65A9B4, 0x89E83065, 0x36FDBF2C, 0xDCD4664A,
+0x0ED6CFBF, 0xDD4DC6DC, 0xD76D2F00, 0xB6DA6540,
+0x9A396444, 0x28F185DE, 0xA0FEFA1D, 0xF476E0ED,
+0xEF15505A, 0x183365BF, 0x481FFD90, 0x29ABEE75,
+0x1EC90B07, 0xC10B2657, 0x0DBF6DDB, 0x52AD02B7,
+0xE87DDB54, 0xD3704106, 0xD4E2C592, 0x0CB2DD05,
+0x4BAA2FFB, 0x02611368, 0xD50F8F1C, 0x416FF25C,
+0x9A69782D, 0x268C6474, 0x2ECD4D64, 0x196DE2F5,
+0x47A8561C, 0x8C7CE6C9, 0xD2B1E2D2, 0xA038C165,
+0x3AB8844B, 0x4A699830, 0x0FFC0B17, 0x89B685AA,
+0xDA276D85, 0xE934C4CD, 0xF511226F, 0x9CDD2B1F,
+0x94F75492, 0x55ECEB42, 0x42F0A3D3, 0xD7EB482C,
+0xA78D0373, 0x62F088A6, 0x7ECF4602, 0x7A3404B6,
+0x40B36495, 0x60441DF4, 0x6722F539, 0xCFE76C48,
+0xB6B94C9F, 0x9ADB4B6A, 0x1EBBA65F, 0x5B5081AF,
+0xB764423C, 0xB6F910E3, 0x14AC4B6F, 0x5C811E82,
+0xAA36E5F1, 0x24EC82AF, 0xA2F1C050, 0x0504324C,
+0x304CED0F, 0x01E31DD9, 0xC82EC7E6, 0xD55AFFF9,
+0xFFB3047B, 0x3006F2E9, 0xC725BCD1, 0x7DCC1082,
+0xA9A22CF8, 0x64D5AF9D, 0x389C34AD, 0x7DFF37C6,
+0x41F1509D, 0x1845B3FE, 0x055C23F0, 0xC6291F5F,
+0xCDD3C7DD, 0x5F0356B4, 0x7FD2C387, 0x494A091E,
+0x50C69D3E, 0xFE769A5A, 0x63904701, 0x8960ABF2,
+0xE68EDF3A, 0x0AB57C8E, 0x0B9D0A6C, 0x51888148,
+0x50C5D533, 0xC69038FA, 0x3ACBE661, 0x0CAEB601,
+0x8C14AB6C, 0xBA86D94F, 0x0724056B, 0x0FEFFCBA,
+0x12449DDB, 0xABFFECCE, 0xB12A2BD7, 0x7260A0E8,
+0xBE184A48, 0xCFD3CA3F, 0xDF088660, 0x78EE9B67,
+0xA9EDB113, 0x4FD5D353, 0x8E348CC6, 0xD578C337,
+0xF0493BE9, 0xCCFB54EC, 0x9CEEF85C, 0x0CAAE15E,
+0x371AD12F, 0x9C5B9270, 0x2495F0DE, 0x06DE2DBB,
+0x911AE7EC, 0xEEDE3363, 0x6DD38D6C, 0x2AF7F3D9,
+0x51C8D118, 0xF23818A7, 0x95438AEA, 0x3A8A798F,
+0x230D2BEF, 0x3D16273C, 0x9C36FF83, 0x785C9537,
+0x3E42AF2F, 0x12A16741, 0xE58D0DC4, 0x33EBEFF9,
+0x6F1972DA, 0x128C9BAA, 0x858D6032, 0xDAF185E1,
+0xAE355065, 0xDE0086F3, 0x0F661A65, 0xF4334169,
+0xB1559BA6, 0x3892109A, 0xE903BA00, 0xAE0CBD58,
+0x073C21A0, 0xFCADB299, 0xB4E39AF1, 0x78475459,
+0xB46DC847, 0xDBA97661, 0x15D118F5, 0x01ED48D0,
+0x99F658BC, 0x399FDC8E, 0x44D4A919, 0x7C2CE4B9,
+0xCA0367CC, 0xCC2B9828, 0x16AACAA6, 0x7AA5B6BA,
+0xFEC77C66, 0x231B22F9, 0xC8BE0D04, 0x6FF2788C,
+0x5F9CEBB5, 0x901EAA5D, 0xDE682BBF, 0x998E70D4,
+0xBD9CCCDA, 0x6995441E, 0x5702F360, 0xBC035EED,
+0x20F60B51, 0xD57361D8, 0xC071113B, 0x73CE6CE4,
+0xC6569DC9, 0xD24B89ED, 0xA6052276, 0x8CEE2026,
+0xFBF5B58E, 0xF692DF81, 0x6B7CDD7C, 0xF5B6C04C,
+0xEC1BBA29, 0xD6AC8CDD, 0x320491F8, 0x1D812AC7,
+0x631B0051, 0xD08A4D2A, 0x569746DD, 0xAA653FCF,
+0xA92E8E70, 0xC59A6705, 0x278EA1FF, 0x63E5FA17,
+0x1C20E82D, 0x550F7CE3, 0x55CED415, 0x5F9C4C4A,
+0x7D746311, 0x5B07976A, 0x12477E31, 0xAB8113AA,
+0x796EDCEA, 0x4A90E4B4, 0xB36E6188, 0xEE7D5E0F,
+0x15CEA060, 0xB81AB2CA, 0x296D22B0, 0xFA0753E2,
+0x0D0D15BB, 0xD4AF8BD7, 0x951FA575, 0xCBEBD58A,
+0x0AF5C362, 0x9EF43FB0, 0xD97E5184, 0xA14469BC,
+0xCAE5D55E, 0x93D4CDF9, 0x95B013A8, 0x6998F35C,
+0xF1DDC0B1, 0x476F9FC7, 0xB6472B70, 0x1D55AC5C,
+0xF0E0C0C8, 0x95372BF5, 0x75CCCDBE, 0x9F9D2003,
+0xCAAD0D51, 0xEE54CC2E, 0xE5EBDBF0, 0x9B248BB3,
+0x4BF07D19, 0x542997E9, 0x17447C4B, 0xCF2B2768,
+0x86118A5B, 0x57579F12, 0xC5CD9E74, 0x97ED5724,
+0x01BD2EE4, 0x2A0403A6, 0x01833741, 0xA1E8D364,
+0x4D1A2EEA, 0x62760377, 0xA10D6861, 0x09C68E2F,
+0xAB482850, 0xACD24B74, 0x5038C8CA, 0x71DE3A93,
+0x671D25E4, 0x9EA7AC1A, 0x3E7287F5, 0x9FC963CF,
+0x73F90AB6, 0xC775D840, 0x00B868D9, 0xF6A9BE3D,
+0x17FFB472, 0x5D2389E3, 0x0D42A149, 0x2FAB1235,
+0x90A7998E, 0xD895F6EE, 0x19921013, 0xEE42EA48,
+0xC5D19A17, 0x5507890A, 0x9F893B29, 0x4FF39F19,
+0xD6EF85AD, 0x3FFB1599, 0xF1761017, 0xFC51B90D,
+0x8F6C566B, 0x44BAC7A4, 0x2B2E3755, 0xABECB8DB,
+0x5C4A1629, 0x837CC4F7, 0x3E732B0A, 0x803CE303,
+0x71865D8D, 0x346665AB, 0x58BF809B, 0x100626AA,
+0x9446AB13, 0xD53ADCDA, 0x75C0BFCD, 0x95853304,
+0xF4758E87, 0xD6B64517, 0x13293D0D, 0xEC9368FB,
+0xD449A2CC, 0xAA17B0BE, 0x9D0B85C0, 0x77BEED16,
+0x7699CAE7, 0xC776D10D, 0x962D48CE, 0x838D00BE,
+0x279AEBF9, 0x22EF837B, 0x58E46DAD, 0xB56B6305,
+0x3232D58B, 0x167969DB, 0x5B63F5B5, 0x7E82B175,
+0x05DDB402, 0x5AB29BBA, 0xF3B627D5, 0x97168C85,
+0xAD9EE022, 0x48F0CEEA, 0x84104C22, 0x690FCC19,
+0xCA2F2474, 0x76F95539, 0x9FD2B987, 0x79EFC557,
+0xCEE5DA4D, 0x27EB98F6, 0xA0628916, 0x8E05614F,
+0x8AC89026, 0x7705135E, 0x3F7E42B8, 0x7BCD773B,
+0xF98B9741, 0xCB8A514E, 0x9298220D, 0x5665FA3A,
+0xE66A1FF7, 0xAC4ECB71, 0xA7E56FEF, 0x9D1EF7F8,
+0x23566B64, 0xB4FE822E, 0x1AA53208, 0xF4545E5D,
+0xEA86C879, 0x18F6B7C2, 0xE10A17AC, 0xBD37011F,
+0xFBDF81B8, 0xA978A4EB, 0xD42437A7, 0x474E6A41,
+0xF8885248, 0xF750BAA9, 0xD238EA62, 0xD69BA74D,
+0x266EC6BF, 0xE7EDE077, 0xE8F0A303, 0x8B56A96D,
+0x41380980, 0xDDF0B16C, 0x00E83594, 0xA503EBF5,
+0x960A258E, 0x499827BD, 0x6C8E6F7B, 0x166C845D,
+0xC842C934, 0xBAEFC699, 0xD9846213, 0x832EC19B,
+0x1EAD7599, 0x221E7EE9, 0x8176A313, 0xB28D8E39,
+0xBAC29A96, 0xB964F91F, 0x3F268150, 0xD4BB7011,
+0x347EC445, 0x7FDC9E82, 0xEB70F4C9, 0xA6F38EBF,
+0x398CF137, 0xD7F88CF5, 0xCBDDCB3F, 0xA0DAFA74,
+0xD29D30AD, 0x822B6919, 0xCE059949, 0x3A946183,
+0xDE4C572D, 0xD1E6D844, 0xC43C7DAC, 0xDBBEEDD0,
+0xA656DF6D, 0x454C22A9, 0x9FA48790, 0x69B04531,
+0x99BB305F, 0x80500F71, 0xFE2363C2, 0xB67F538F,
+0x302EC0C3, 0x4A6E3458, 0x57E4CFD4, 0xE65CDAEB,
+0xF31ABB31, 0x62DF98AC, 0x894AE781, 0xB1588AB1,
+0x45D5CC3E, 0x3520F5B0, 0xC72D0CB7, 0xA1D6CBF9,
+0x742FFA63, 0xA0A5224F, 0x5EA1C85A, 0xB81E9F77,
+0x31D76C4F, 0x525257F5, 0xBFF85009, 0x2125B270,
+0x16E47E6E, 0x9128B981, 0x0D5FBE39, 0xF67A418C,
+0xCF3C71CB, 0xAC04ABE1, 0x9B550AAF, 0xB5077F18,
+0xFB7C5EC0, 0x64784DB4, 0x1E668B48, 0x84659836,
+0x604457BF, 0xF6F69C8D, 0x394301DC, 0xED0211BD,
+0x8BAC1A3A, 0xBB752FD2, 0x78B8C036, 0xBCB98E8A,
+0x33C595DE, 0xB3F3C5F8, 0x698666AC, 0xA1F42D7A,
+0x5751ACC8, 0xC069575B, 0x35D50F99, 0xB294BF38,
+0x82A4A331, 0x05147751, 0xCAE18C12, 0x9E89AAF1,
+0x3531C372, 0xB2114A88, 0x41797201, 0xDDDDEC10,
+0x01185F2A, 0xDED50CDC, 0x72156BAD, 0x88F3DB94,
+0x50450DDF, 0x6B1E7ABF, 0x3D317708, 0xFDFF5A15,
+0xDC8B1697, 0xCC2248FD, 0xD9196272, 0x4445195D,
+0x54D90281, 0x7A891C9D, 0x69FF98D5, 0xADE6D74B,
+0x26D27973, 0x0F14734F, 0x3F957FC8, 0x812AC874,
+0xEDC0F9B4, 0xD31D6D75, 0x7A2608C3, 0xD89984B1,
+0xF581081A, 0xEDB9DF6F, 0x16ECC191, 0x6B945724,
+0x1BCE8269, 0x02E6DB68, 0x56362541, 0x9D247CF4,
+0xA5265E72, 0x2C8B9413, 0x1157DB4B, 0x3145CFB2,
+0xFBDEBCF5, 0x1042B117, 0x284DAE18, 0x10575C21,
+0x1DDE578E, 0x80F59EDE, 0xCAB51C04, 0xB594BDA8,
+0x08ACEF85, 0x08C8D4C7, 0x7304D433, 0xE87D3A88,
+0x31CCFED8, 0x1D8E71E5, 0xC5A2F02C, 0xACBF3B5E,
+0xAA161BCA, 0xA10BE577, 0xF9CE41D2, 0x2B86F031,
+0x3D4A8D23, 0xED926DE4, 0x3844E21F, 0xFE57BCD0,
+0x36DC309D, 0x17137409, 0x9F6A8507, 0x14CF12EB,
+0xA770AFB5, 0x7C6DA2E4, 0x856B48B8, 0x2EA235DF,
+0x55BD1164, 0x5BD9FF0C, 0x5228C552, 0x9E719AFA,
+0x3EC3703B, 0xE06A94F3, 0x296FF0D9, 0xE468D9C9,
+0xD2A15CDC, 0x6C4EAAA2, 0x2AF3B8BF, 0x6B6EDC78,
+0x42B78972, 0x4C97A66C, 0x161C30BF, 0xCD2816DC,
+0x431BDA17, 0xD9653022, 0x67D95E39, 0xBCB18342,
+0x227982E7, 0x23C5B11B, 0x514420AB, 0x089F3A5C,
+0x2B2F8244, 0x2F2A80C8, 0xB0A90558, 0x75BAA243,
+0xE2FC4F62, 0xEB0A6104, 0xB7F221B2, 0x4ECD79DF,
+0xB3E08B8B, 0xBA25E1CB, 0xD39F3431, 0xB50202FE,
+0x78F15ECE, 0xEFF61ECF, 0xB3CDDD50, 0x3FD064A8,
+0x96B028BC, 0xB29DD4E1, 0x7E9EC629, 0xC407F4D1,
+0x8C21785B, 0xE11767BA, 0xCFE6DE26, 0x0DA98E22,
+0x33AC5670, 0x0FDBC175, 0xF11F8EF5, 0x60638843,
+0x8B67E55A, 0x3F27F75B, 0x6691FB98, 0x635A35A9,
+0xB317459C, 0xE7419C01, 0x8BAB28D7, 0xE347D791,
+0xEFC019A0, 0x45009041, 0xA6DEB3E8, 0x6F7379FF,
+0x0FF50390, 0x810BEE78, 0xAD13716B, 0xA7DBD7AB,
+0xEF439D4B, 0xDDA744A5, 0x31EDDE8D, 0xA85B71F2,
+0xDF439C70, 0xA7E3DA94, 0x525ED453, 0x3D913C32,
+0xD104CE61, 0x42F5FFED, 0x14C7625A, 0x4E5B314B,
+0xA7EAD1ED, 0xFA01D595, 0xE67BCF06, 0xE63685E2,
+0x3A32E9D3, 0x374C25F0, 0xA8E8A41D, 0xA403AEF5,
+0x901A194C, 0x17605BC9, 0x8522DD12, 0x27096BAA,
+0x017434B7, 0x99C8D2DA, 0x7F96B068, 0x8521CD09,
+0x529B46D6, 0x47852810, 0x021BC8BF, 0x93C98329,
+0x6FE73A78, 0x44DB69A9, 0xC839D490, 0xCAC42AFE,
+0xCF1ECCF4, 0x6F2E5F44, 0x795C8219, 0xA06C667B,
+0x80411F31, 0xB09926E1, 0xC62B6C18, 0x77C6E6DD,
+0x7622FC07, 0x02162DB2, 0x3EA31334, 0x6CC02B4A,
+0xAA6B81C3, 0x4424A9A5, 0x26BD2EF3, 0x334896D6,
+0xADDD2711, 0x76035757, 0x80AA328E, 0x2F39C06E,
+0x357520CB, 0xF62BDF46, 0xC59343C4, 0x7CA4CAE2,
+0x89B03EF3, 0x251A785B, 0xA4755BB9, 0x262D478D,
+0x462E6252, 0x6B5F6BED, 0xCA46E77B, 0xA2CF08AD,
+0x561E19EA, 0xBF31AA15, 0xD376F44C, 0xCC332150,
+0x8C0AEE42, 0xC06D5F91, 0xDADF8613, 0xBE0FA22C,
+0xF50AE482, 0xE3615501, 0xECC8D5AA, 0x58A7FD3E,
+0xD59B8CC9, 0x09DB0987, 0xF1D9753D, 0x9C79E20E,
+0x9A222AEA, 0xC4E58914, 0x6712E0A2, 0x8CD5C80E,
+0xEAB8AA56, 0xDBFA8D9C, 0x3515BD21, 0xB65B9E0C,
+0xF0D27FEE, 0xE33871C1, 0xEE8FE52F, 0x02ACCB3F,
+0xE9197277, 0xB7B70770, 0xA26E3581, 0x82481E7F,
+0x005AF99F, 0x8B970B4B, 0xEC74B662, 0x2F21C5A3,
+0x049DBA83, 0x495B3E1B, 0x112234B8, 0x95B42A5F,
+0x2C8FA833, 0x6D706E30, 0x2AAAEC09, 0xDE7C3377,
+0x06CE9D46, 0x7574EAAB, 0xFCB1A08D, 0x462AFB6C,
+0x192847B2, 0xCC149AC3, 0x427834CE, 0xE90180A0,
+0x946E526E, 0x6018BE4E, 0x20442F52, 0x1D39FA05,
+0x35F690AD, 0x29DB3A53, 0x6360158C, 0x3EC815F8,
+0xDED650AF, 0xFA168B37, 0x233F8A3D, 0x245009CF,
+0x71BB2237, 0x4989A01C, 0xD58AE4F1, 0x62C99EA0,
+0x48E9056E, 0x7E1A786D, 0xBF6CBAAB, 0x22669A6B,
+0x57857590, 0xE4558CE3, 0xBC6C63EC, 0x6AE02A61,
+0xA2ABFBBB, 0xD2B2FE90, 0xDF8BDB43, 0xEC2D59AC,
+0x7B6AFDC3, 0x6B001D5F, 0x3DFEE08F, 0xB9A597D6,
+0x09DEAC68, 0xE42D9E73, 0x2E33507C, 0x6525F051,
+0x0D7143C6, 0x01DD115B, 0x94180279, 0x28FC60D7,
+0xC0900603, 0xED4FBE53, 0xFC0677BD, 0x7DA2A878,
+0xA8D0EC73, 0xF6A09B2A, 0x24A129EE, 0x169BCA2F,
+0xE0BAE526, 0x5C8E2FCB, 0xA218EFFA, 0x842B61FB,
+0x87B860CD, 0x106E9B86, 0x930685F0, 0xC5A72109,
+0xFB977BD5, 0x9D3B4AC6, 0xDA378FE0, 0x0AAF747B,
+0x0408D50D, 0x488785B9, 0x81AE971D, 0x12ADFEF3,
+0xF0B64128, 0x3D4C90BB, 0xC994AAA1, 0xB854400E,
+0x901AE3DD, 0x7A4A0DE7, 0x18E07456, 0x20C38BCD,
+0x94441976, 0xE2E419C2, 0xDBD3C92F, 0x4DD63841,
+0xE2994959, 0xF41F196D, 0x0835431A, 0x93A2E9CF,
+0xB01FABED, 0xD0135535, 0xEBCEA18D, 0xC4F83A1B,
+0x5D72845C, 0x04335E3A, 0x68C4C987, 0x77178710,
+0xC5293A9A, 0x44E40AE1, 0xCE454FDE, 0x71DE89B7,
+0xA373D9D3, 0x6D19E483, 0x812896D6, 0xC3231C14,
+0xE960ABA4, 0xB7FB6F83, 0x1F7C4EB8, 0xD10DBE69,
+0x8575CF6E, 0xC03B15D5, 0x4D7F4EF3, 0xF0615F31,
+0x34E21762, 0x22D5A7A1, 0x729FA3F8, 0x2E1050FB,
+0x8A9F46DC, 0x535EB5A7, 0xD143560E, 0xF8EC3A4B,
+0x2249FD06, 0xE8E2AB08, 0x1E734127, 0xBA5B635A,
+0xD8F419DB, 0x0B5200D0, 0x8110304F, 0x3497DA80,
+0x35CA71CD, 0x0FD8227E, 0x086C74E2, 0xAB68A1AF,
+0xE3BD57EC, 0x83B42D29, 0x3C2D672D, 0x05D85CED,
+0x64F04926, 0x91364A12, 0x7FC73349, 0xEBA1FC77,
+0xECE0D20D, 0xB1DDDB9B, 0xEB6B492B, 0x0FC02BB6,
+0x56201D76, 0xED20F79E, 0xFC6034FB, 0x6A539F1D,
+0x520FECBF, 0x4E3AECF6, 0x76B01C74, 0xEFC421D4,
+0x82AC989A, 0x407A77CD, 0x6D287BFE, 0x26617425,
+0xEA2316C3, 0x8616554E, 0x9F4C4535, 0x88C0C6C1,
+0xEAC4F0F7, 0x32C7DD93, 0x41D9C37E, 0x2A9CBB2E,
+0x0591BAEF, 0x2BE43F21, 0x5E06EE4D, 0xDDDF5525,
+0xEC137DBE, 0xF0AA295C, 0xF2C9FDE2, 0x5DF9D693,
+0x10A6CAC0, 0xC6846D09, 0xF1DDABF3, 0xD56F8BBC,
+0xAA5DCE9D, 0x6F59004F, 0xB8A035BC, 0x61F47282,
+0xC89DAC9E, 0xFC7E5B3D, 0x4C5406DD, 0x54CFD147,
+0xBB44AB2A, 0x791269C0, 0x8CF66B4D, 0xD01A3190,
+0x636F45CA, 0xB32FC209, 0xCB8B9F49, 0xF46D74B9,
+0x5AFC9BD0, 0xC4C716C1, 0xF98C54F3, 0x36AFF013,
+0xB4D6D90B, 0x5F1299B6, 0xA3BFCFA4, 0xEA336AAD,
+0xCCD443DA, 0x74CA40B4, 0x31EF1614, 0x36D3FFEE,
+0x876AE252, 0xC8D62E9F, 0x6424F397, 0x1F730F2D,
+0xB20FDA53, 0xFCFEE60F, 0x676A61C3, 0x26C5E143,
+0xC201573E, 0x4A8C46BE, 0xEF87D0A9, 0xE07E80B4,
+0x34F20109, 0x8B936A70, 0x9F8E0305, 0xF3297CA0,
+0x4E7BF0E9, 0x0F374BB9, 0xCE78A01E, 0x5FE26DD8,
+0xA3826ACF, 0x321F69AB, 0x441AF14E, 0x8AC19CF7,
+0x4BFD1AD6, 0x5951ABD1, 0x098C17F0, 0xA9B75F76,
+0xA462551B, 0x6B703A12, 0xEDCB57B2, 0x8CD4C933,
+0xD338D3D8, 0xE343FC24, 0x9CDD52EB, 0x17A41942,
+0x63A8EF50, 0x215BB11A, 0xE1E25CB6, 0xB62C0A88,
+0xE58CDEC3, 0xC0E6389A, 0x2B7BEE55, 0xA3FCBD07,
+0x7CD451FE, 0xB06F6724, 0x5675A7EA, 0x141D52FC,
+0x05E86E9B, 0x53D75C3A, 0xE799AA2A, 0xE474384C,
+0x8C85E6E6, 0xA477A8D7, 0xA1E6AB0C, 0x9033E7CD,
+0x2F55D504, 0x4DAE81FB, 0xBD229A64, 0x862765C9,
+0x5B6A85F0, 0x95A39328, 0x38826CFB, 0xBF7DEBA4,
+0x42EFAB62, 0x2D0BBA60, 0xB06731AF, 0x16D4C4B0,
+0xCA4B9264, 0x3DF24AE2, 0xFED93848, 0x7CB33B08,
+0xAC9CAE9F, 0xA0F80B61, 0xA66CF713, 0x9364865F,
+0xDFA1E0B3, 0xFE6DF33F, 0x8039A612, 0x119F60BF,
+0xCEEDE309, 0xD28316A8, 0xCD61D2F5, 0x3CBEB015,
+0x85C0BF51, 0x6EDBBC15, 0x79F3D207, 0x485EE4FA,
+0xCEC302EA, 0x59D8B92D, 0x51C1FB36, 0xF4FE8B71,
+0x2DBD5718, 0x84024040, 0xFDD6590F, 0xA1CE9CC9,
+0xC4AEAB72, 0x0A2FE8BF, 0x28C33618, 0xBA4E15FB,
+0xA9C72819, 0xA3EE45D7, 0xD2DC52F1, 0x3FC84A2E,
+0x1C9DF73E, 0x632F9BDE, 0x7E9FBD20, 0x0D689B79,
+0x91E8D5C0, 0x6EE7952C, 0x905F192E, 0x2D79E712,
+0x8670A7A2, 0x1DBFC4D9, 0x64634429, 0xE636043B,
+0x643C6B0F, 0x50AF327B, 0x0E734D61, 0x2D7D6E46,
+0xB877DCD6, 0x7CCF4F1A, 0xDF4D8CF8, 0x0E7FA78E,
+0x0CBC4EC2, 0xAE9B4A22, 0x4F02D49C, 0x48F09C43,
+0x5031B1A0, 0xDCB8A1FC, 0x91C73599, 0xCF00A64D,
+0xDFCE561E, 0x8B18157D, 0xE1ED6A81, 0xCF94EF36,
+0xB412CE1A, 0x602E2076, 0x716B0F3F, 0xADEB32C0,
+0xD4E16094, 0xEC95D41F, 0x75858767, 0x438AD1A1,
+0xE61C5527, 0x0D71FBB2, 0x2A99D070, 0x5C018826,
+0xCCCC27FD, 0x053883D9, 0xF1D30EF5, 0x676AD38A,
+0xDF81AB28, 0x2257FB9D, 0x373313AE, 0x67E1FE8A,
+0xF4F66B02, 0xAFF8C7FA, 0x3B60D94D, 0xD44D0FE2,
+0x5FCDFE4B, 0xC63010B6, 0x06CFCCF4, 0x09D8DD85,
+0xAB79F2BE, 0xD5C0C498, 0x7364E4FD, 0xB295CEDF,
+0xDB89A068, 0x59A6A0C7, 0x0C823207, 0x7380FCFE,
+0x6E33C4B9, 0x0744E4F2, 0xF663BB33, 0x9EE512CE,
+0x870ED35B, 0xB4502654, 0x367CD4FD, 0x5D4238D9,
+0xEAB2B86E, 0x6E8ADDAA, 0xF080EDD6, 0x1DC90F46,
+0xB1FC9127, 0x63771392, 0x96729BF6, 0xD18E1413,
+0x5D85938D, 0xB8CED349, 0xF9B886C1, 0xCA486562,
+0xBAA9ED7A, 0x049718D8, 0x7CF8E67A, 0x1702843C,
+0x6DCDC34E, 0x93C51F83, 0x2415A4F3, 0xA8D77B3A,
+0x0FB823E8, 0x424F03C3, 0x9CAA503C, 0x7AA5433F,
+0x3BDD74FE, 0x99D3332E, 0x1E62231B, 0x90A4E595,
+0x7EDA974D, 0x43E2CD14, 0x27DB9D9F, 0x561F5CC6,
+0xA77EABA6, 0x97867B48, 0xAD6533CE, 0xEB726CF4,
+0x5857B217, 0x2D7DA10B, 0xD939C20E, 0x81F1F073,
+0xF42DEAF2, 0x3AD7780E, 0x88C77661, 0xD2E819B2,
+0xF872F581, 0x999F0C5A, 0x3887ABA4, 0x27F95B6D,
+0x991D9458, 0x9D1BB131, 0x6ECC5298, 0x9E9A7B26,
+0x6E65F271, 0xE90FA04C, 0x7B692AA0, 0x878943D5,
+0x924895E5, 0x041BC73A, 0x448E28B2, 0x61D22D1F,
+0xE7969773, 0xBC8E5980, 0x9A198852, 0xB94415C9,
+0xA02374BA, 0x340BD5F3, 0x27F2A0FF, 0x39BDB33F,
+0xCC042BCF, 0x83D6C135, 0x9C7A8D8E, 0x05823C23,
+0x2D7A3F91, 0xE792BCCA, 0xA2D82177, 0x73C82E7E,
+0xBEBC9613, 0x9F596CB0, 0x6E784AA7, 0x1B7BDA9F,
+0x846391F7, 0x852AD070, 0xF831E8CA, 0x16A78223,
+0xF68F5250, 0xE2554493, 0xD38F2AFB, 0x764BA7A8,
+0x3CAEFC55, 0x6E9B9037, 0xD87D486E, 0x7352AEA9,
+0x11987EE0, 0xDF7E84DA, 0x2838E736, 0xA8C7BAC2,
+0xF49E21EE, 0xFAD106E9, 0x7363AC6F, 0x5E9974CB,
+0xBA008BB0, 0xAF5DB3FC, 0x7AC3CFD7, 0x2D55EDC6,
+0x2C1C9AD7, 0x6A3AA494, 0x5F0E0A3A, 0x37422BFA,
+0x83B4D594, 0xB7ECCF66, 0x82FCCDD0, 0x8ECBFD79,
+0x664B9341, 0x02F178A2, 0x2095C8E0, 0xFC5F17B7,
+0x1810BA9B, 0x964E4CD1, 0xFBAED808, 0xDEE87796,
+0x63DE4F69, 0xC99275DD, 0x65242304, 0x7AB5C28B,
+0x01BB7A3B, 0xC85D7716, 0x32AFB9A3, 0x2ED2CBB1,
+0xB194218F, 0x21FE560D, 0xCB4503A5, 0x5CE0464D,
+0xC4AE9A3C, 0x061530CB, 0xEDA38E6B, 0x4029D3E6,
+0xB0C20336, 0xA37825C0, 0xC68F8B37, 0x9405AD3B,
+0x8B1A8F99, 0xA761DE8B, 0x683B3259, 0xA154C554,
+0x6BD835C9, 0x6DEAE35A, 0xBEAE6D49, 0x21D8B074,
+0x46C01B31, 0xBE9B3A16, 0x1D611EAA, 0x423AB74C,
+0x931F5AF5, 0xBB9E289A, 0xA4101132, 0x4A8BE0D7,
+0x3307E4B2, 0xDE78DB5E, 0x347EB5CE, 0x13EEE999,
+0x2C2D7955, 0xBA893EBA, 0x5DFC2EC1, 0xE7DD7A5F,
+0x5E1C64D8, 0x4552E447, 0x1837D8E4, 0x9711836B,
+0x3219F893, 0x04392C84, 0x3E94848C, 0x15E5F481,
+0x0EC58819, 0x7341D458, 0x4AE63711, 0x85C1FD1F,
+0x97B58BD7, 0xB0550EBE, 0xB9108743, 0x6F53B386,
+0x7A73F31B, 0xE07CF8B9, 0x61FF27C8, 0x06A9A8B4,
+0xEB0F2BB9, 0x46D275FB, 0xCF39B474, 0xC34F3B6D,
+0x52F2F119, 0xD87963BF, 0xC60BF16C, 0x7797D0AD,
+0x7EA4DBF0, 0xD21409C7, 0xF678A927, 0x638E67CD,
+0x93261AED, 0xEA9B25FE, 0x1EBCAFDC, 0x580CC829,
+0x58D1DA1A, 0x658881F8, 0xC48DB682, 0xD42E8CB4,
+0x1DF33D74, 0x31C04F68, 0x7D871E29, 0xAE11FD72,
+0xD7E8F8F6, 0x530D9D9C, 0x580A0715, 0x0F17B1A3,
+0xB863F42F, 0xA6A4DC08, 0x82773E76, 0x9354B309,
+0xE17D0770, 0x04E4DE5B, 0x712EA396, 0x49D37B55,
+0xAE4109BA, 0x03862DC9, 0x7BCF61D2, 0x43CA2017,
+0x23BDD50F, 0x74577459, 0x4E8F4E23, 0xBF924C1A,
+0xE4EC70CE, 0x37FBEC66, 0xA6DA8935, 0xE11F4090,
+0x5C8F9EE3, 0x19D167EC, 0x9EE4F2C5, 0x64A81E6C,
+0xB35642BB, 0x82083A01, 0x001CA1F6, 0xAA69C7E8,
+0x685F24D9, 0xE6868E31, 0x38ADD8F0, 0xA2FDD44E,
+0xEE0C491D, 0xC60B1E9A, 0xF7A89268, 0xFD784F35,
+0xC6B7335C, 0x75EFCEC1, 0xE2D9F7CF, 0xE1C364F8,
+0x7CC63B2C, 0xC179E2AD, 0x56C193A5, 0x5134FB69,
+0x35058BB5, 0x36F4BCD5, 0xDF4A08C2, 0x14AA2330,
+0x760C8CD8, 0x2C562394, 0x0BEB669B, 0x2301973A,
+0xAF5C4FF2, 0x1C770AAB, 0x25DD2087, 0x732AADC4,
+0x59054958, 0x59DDCBE4, 0x74CFC8A8, 0x7C015016,
+0x32A0276E, 0x8F1C2E93, 0x0CE91F71, 0x055C307A,
+0x435D967E, 0xF4C33704, 0x5BDF2AD7, 0x8855099C,
+0x307B2736, 0xBB6B19CB, 0x626349D3, 0x8F52ABFA,
+0x251A1ED6, 0xE0587BC0, 0x12831408, 0xDA83CABF,
+0xAB2C7DFD, 0x6BCF0271, 0x72058DF0, 0x17AFC1DD,
+0xFFC52C30, 0x551401E0, 0x9EED54DF, 0x14E951E4,
+0x14624B3F, 0x4C24650B, 0x5A65F86B, 0xE94F6143,
+0xDC7CE9CF, 0x94D5D8F3, 0x093B0A04, 0x22098D01,
+0xEDF09E7C, 0x165EDB0F, 0xD09CA774, 0xB96AA141,
+0xB5745978, 0x9D820434, 0x42B0E026, 0x96938A25,
+0x72E8634B, 0xBE36EC02, 0x42F3F74B, 0x358FA621,
+0xBD451484, 0xB43A75D1, 0xB0A57F91, 0x701A7C82,
+0x484B3F46, 0x047F78AD, 0x65F7371C, 0xEAC8A954,
+0xE59F6354, 0x3EEEFB4E, 0xF131954B, 0x1C00BAC2,
+0xE3897637, 0x5FEC83AB, 0x58CFA2C4, 0x1F4C0A6A,
+0x97956BC6, 0x63D11D7D, 0xB46179D0, 0x11039A75,
+0x1B50E088, 0x68E9476B, 0xAA68DB55, 0x8A4A051E,
+0xEFA0DDF5, 0x05A2A674, 0xFFE03E72, 0xC5A0295C,
+0x6FD4D834, 0x8E42BB94, 0xF3DFD88E, 0xBA691AD2,
+0x3458473E, 0x6269A348, 0x72962FB6, 0x86D5064B,
+0x8A153740, 0x54AC97D8, 0xED2CE057, 0x68200474,
+0xBBA8E19D, 0xBFDD08F3, 0xB0DF76D1, 0x62F29649,
+0x5AB77030, 0x1EE9A00E, 0x7DAB1C90, 0xAB608FFD,
+0x8506A853, 0x75B9339B, 0x1AE0CCBA, 0xFB60BB79,
+0x8650F92F, 0x4819E1F7, 0x0A7045A8, 0xB5BCE5F1,
+0x77A98B27, 0x03DE21E4, 0x3FE3F132, 0x106827EC,
+0xD4DC1469, 0xAAC82F9B, 0x1D5953A1, 0x8034B369,
+0xD4412B6F, 0x90FB9F25, 0x14279070, 0x6D98AF1C,
+0x3D286F37, 0x8324A732, 0x58123E4E, 0xEB051032,
+0xC15CD557, 0xEB82DE99, 0x6213434E, 0x39F0FC9C,
+0x5EBFE1C5, 0x8CEBF470, 0xFF7D8D8A, 0x740A6A3E,
+0x720D080C, 0xB73B74FA, 0x5173F96E, 0x9FC01794,
+0xDABF1C81, 0xCA813295, 0xBEA2DB8D, 0x4C7E0CE4,
+0x8051BA67, 0xE63399E2, 0x83A15EE4, 0x47F4A718,
+0xD8246E6A, 0x0B4F87BE, 0x031648B8, 0x99E3E3E6,
+0x4ABCC64F, 0x52768181, 0xE708372B, 0x2D0B1D2C,
+0x4DF52402, 0x389BE9F6, 0xDE2F3232, 0x5D43D74E,
+0xD37BB898, 0xE7272645, 0x9B5432DA, 0x9D7A9473,
+0xA69628A5, 0x583555A7, 0x255B08BD, 0xAD68EAE3,
+0x1A79982D, 0xACE09726, 0x15E576AD, 0x260EB406,
+0xA7440B46, 0x66B6D317, 0xBE6ECA3B, 0x3ADEA1C1,
+0xD80399C3, 0x0EF198D0, 0xFAEE2010, 0xEF2E8E56,
+0x5B6CC402, 0x3FD27BE2, 0x970AAB5F, 0x618C17C6,
+0x7F5022FB, 0x552FC1FA, 0x5DD82984, 0x09769539,
+0x98812D1F, 0xBD8B2539, 0xD78AD9A6, 0x1CE41D07,
+0x272A0AB7, 0x5CB7E101, 0x6F42D56A, 0x001D930E,
+0x3C17C305, 0x30AAE354, 0x2A4AABE0, 0x922BCB94,
+0x73F34C1C, 0xE07E1501, 0xCB55A3E1, 0x0CDC3669,
+0xD9C07DE7, 0x2DAB82BF, 0x963EACAA, 0x9B05E0F1,
+0xE2DA0EFA, 0x0613BFE5, 0xDFB605E9, 0x5DCCA8FD,
+0x6D433873, 0x81A9B4C5, 0xD1D1CB14, 0x9B6A9906,
+0xC104767C, 0x30101D37, 0x186FBB79, 0x8F95D488,
+0xA3094F43, 0x7F17C981, 0xFD92B3FE, 0xADAB3AB5,
+0x20D1406C, 0x9462C8E7, 0x5D64819D, 0xB3E85196,
+0x67B854FE, 0x7D039FC6, 0xAD98A85E, 0xF672E041,
+0x30FA19A9, 0x4A276EB8, 0xB7041D2E, 0x57BB21E2,
+0x4E251667, 0x15C5401E, 0xDAB59431, 0xD6C6FD1F,
+0x1726EB70, 0x900F4E84, 0xD327DE33, 0x7A0AE04B,
+0x76B1174E, 0xFD547B94, 0x370832DC, 0xDDE65CDD,
+0x74672C02, 0x164703FE, 0x34CAD31F, 0x3E692DED,
+0x4BC38FA5, 0x143F99E5, 0x61BB640E, 0xB957BC8D,
+0xC9DD9E35, 0x2B5CB310, 0xADD6EAD0, 0x91981D46,
+0xED803D57, 0x61D7737C, 0x92D3AC3E, 0x36A034CB,
+0xE1395DC5, 0x5F2070F8, 0xC5EE9F8A, 0x70546B88,
+0xC9EA230C, 0x58DC3073, 0x57CBBEB7, 0xA0B78CFE,
+0x0B3FE75B, 0x07ADACCD, 0xC292C338, 0xD70CD7E5,
+0x729D8F4E, 0x218FA041, 0x10EC1199, 0xAC1EC51D,
+0x5DECC8D1, 0xBA36230A, 0xBC41F5A5, 0x75864896,
+0xB4403D4A, 0xFEEE8F44, 0x8D94A256, 0x62BA0115,
+0x3A570C61, 0x9221C583, 0xD2981A6B, 0xFD8AAF5A,
+0x2A102D59, 0x64083BDD, 0xBD1AADE6, 0x7E6D1E99,
+0x20568A6D, 0x8DFA704B, 0x87D27122, 0x2EFDAB7D,
+0xF3AF9D39, 0xD8DED0B2, 0x2D4B34B9, 0x12F3E32C,
+0xA6BCBE65, 0x680029A1, 0x094B07B3, 0xDA5918ED,
+0xF7D0A86D, 0x1A7E18C8, 0x9285A97F, 0x2040282C,
+0x5B133531, 0xA48237AC, 0x3557BC1B, 0x7E6ED77B,
+0x436234C7, 0x9B2094DE, 0x5D967593, 0x8867D1C4,
+0x88EC3948, 0xE7F84AD4, 0x1871B3E6, 0xE8E992C6,
+0xA16DC2F8, 0x0DFDF590, 0x9B56238D, 0x329017F5,
+0xBF9BD409, 0x68BD9B1C, 0x4036C4FF, 0x3BF6D93C,
+0xAE100602, 0x90B43508, 0xA85B4013, 0x2C66EA54,
+0x227D32D7, 0x0BA526D1, 0x075213B8, 0x1A3DED07,
+0xD458DFFD, 0xDC8ACD43, 0xAC7809AB, 0x2D25408A,
+0xD8F0C887, 0xAD8CD30D, 0x4054F61E, 0xA9F0CCA3,
+0xBFEBD31D, 0x6D2BAB1E, 0xF8E42D8B, 0x6C94A4E4,
+0x1158D2A3, 0x93F44EFE, 0x8AD05A25, 0x8C229D32,
+0xB213D76E, 0xDFE63822, 0x561986EC, 0x806CA082,
+0x6DB3BF8D, 0x1E850D30, 0x8F7A44C0, 0x75BB3328,
+0x86C7BE12, 0xDE5C44BD, 0xDF4D048E, 0x968712C3,
+0xB1B41CF8, 0xCC194FE9, 0xDA2E1A8D, 0x72A08662,
+0x5ABA2536, 0x223E2013, 0xA5A923A5, 0x7565B5DD,
+0xBCA0A2B0, 0x0C29864B, 0xAAD8CB87, 0xE4C7E559,
+0x77E19E51, 0x194E54ED, 0x54DD1B54, 0x0FAD37A7,
+0x0EF6B0E3, 0x0E3A2FC8, 0xA0063995, 0xE17AE20E,
+0xDC11B7F8, 0x85F1A76D, 0xD97858D4, 0xB763E49C,
+0xB5BE7EC4, 0x3CE924C4, 0x4246019D, 0xD33DBB27,
+0x737863A7, 0x32C26BDD, 0x714897A3, 0x36091018,
+0xF26BC990, 0xDDB640B0, 0x448F5B12, 0xD7A5EB4B,
+0x5614EEA4, 0xCA4912FB, 0x011F9D6C, 0xA4FC90AB,
+0x9FB4982D, 0x20AD146F, 0x4B7AB74E, 0x107A9411,
+0x71DBA90A, 0xD510E3D2, 0x248D0D35, 0xB666229E,
+0x61EE1EEA, 0x702031B5, 0x36992A7B, 0xC90C08CB,
+0x6478995A, 0xE6C2BA7A, 0x8A9179AC, 0xC8EE2956,
+0x27B042C8, 0x48DB81D9, 0xAA39F2CB, 0x5E4D5F3C,
+0x24FFD6B9, 0x5B562C2F, 0x00FD33B6, 0x435F5F52,
+0xF392FFC1, 0x0E927C40, 0x5508CBAB, 0x976AA567,
+0xA13E7C52, 0x532109E9, 0x16B9021F, 0x60C615A1,
+0x1D23C258, 0xFD783147, 0x63600FB1, 0xAAA245F0,
+0x9B3DC1E1, 0x7B270D0D, 0x5B1632CE, 0x8B871F7F,
+0xC535EFF8, 0x73109C6A, 0xEB83D02D, 0xF7AE76FB,
+0x2E39E502, 0xA4128216, 0xF90D57E5, 0xFF0C465E,
+0x02008029, 0xE5CBBA1F, 0x4280FA3C, 0xCDBD75C8,
+0xCB4AF342, 0x17695A4E, 0xAA6162B5, 0x8660A679,
+0xD1A8701C, 0x47694CA7, 0xDA8D43FD, 0x44A4BC1B,
+0xAB34B9AA, 0xE55563DD, 0x08D4142B, 0x81197AC8,
+0x997B1DC2, 0x2E7CC50A, 0x7A326A21, 0xA76419DB,
+0xEA8B5428, 0x65729140, 0x051DAF66, 0x8871BCA9,
+0xA175E5BF, 0x60310C98, 0xB7DE8929, 0x35E2459E,
+0x08EB4547, 0x904D7B2B, 0x29382CC4, 0xCEC8664E,
+0x1E8C9C2C, 0x3B942134, 0x9CEC5D55, 0xDA548376,
+0x2E4EFD61, 0x26F65F09, 0x5A3DD7CA, 0x2FD4E58D,
+0x6B71B8C2, 0x13189115, 0x2B5542BA, 0x1CE85C2C,
+0x5B9FE09D, 0x68704BFE, 0xB15313B9, 0x3EF2729E,
+0x583ECC31, 0xA3DED8CA, 0xFCD27C3D, 0x904DAB39,
+0xFE1069A4, 0xE99A57BA, 0x112EB80C, 0xE1483C74,
+0x8A27B0D7, 0xA58F7325, 0x7CD050A1, 0x626D4F3E,
+0x51643657, 0xA967FC59, 0x5BACBC0B, 0x2CF3E459,
+0x7D8988D9, 0x53913DF8, 0x2381A6FC, 0x64D6D441,
+0x48AE9101, 0x185D9539, 0x1B044AEC, 0xB5ABCEDD,
+0xFA8ECA52, 0x8CCDD142, 0x96FD4442, 0xD865FEDF,
+0xCE4EE2FA, 0xA5160AE9, 0xC91B2B3A, 0xF993F45F,
+0x1509132C, 0x920ECC5F, 0xD813DDC1, 0x834B68E4,
+0xD5E876A0, 0x61DE0E41, 0x4C143913, 0xC7293985,
+0x17E226E7, 0x38830927, 0xDC604DF2, 0x799D1430,
+0x846585AB, 0xE5D21E38, 0x6381D136, 0x1B60633B,
+0x23B7AE14, 0x554E53CC, 0x5807A210, 0x30560866,
+0x12F79E62, 0xE27B5D45, 0x3889C1E5, 0x47F845FF,
+0xFFD9DE98, 0xB10E09D2, 0x4A184A72, 0x083D2971,
+0x8AB7478D, 0x92380377, 0x57A724EC, 0xBBBD5CA6,
+0xE2FB9D32, 0xAB6ADFC6, 0x3916DED4, 0x4E19438F,
+0xE21E15CF, 0x6AF4BCC9, 0x8D08924A, 0x1662BAA9,
+0x3064AD27, 0xB86D7EE4, 0x88624C62, 0x1A0BF3E7,
+0xF3E4A287, 0x6787F006, 0x01375D4B, 0x998BB38F,
+0x6D669A29, 0xD760B093, 0xC4768853, 0xF041100F,
+0x35DE10DD, 0xE06C8BB8, 0x2C79A902, 0x60600DAD,
+0x6E11CF5C, 0x18778777, 0x7CCE406C, 0xE54AF2EA,
+0x7472C475, 0x73DBEE7E, 0xE533DC40, 0xB07407DD,
+0xF6ACA8D3, 0xE71BD7D1, 0x4BD3514D, 0xC5C362CA,
+0x0690E5A1, 0x0FFDC8D8, 0x58188645, 0x8636413C,
+0x3412A033, 0xAF4FC340, 0xA5DFEAB8, 0xB87272E3,
+0xA4A9219F, 0x29696E90, 0x35D2F627, 0x8794DBD7,
+0x5D2D87F8, 0xFA73559D, 0x7D22F440, 0xF50197E9,
+0xEB74B829, 0x8F9649CF, 0x16F47D30, 0x5C7D9870,
+0x36FF6C0B, 0x313A92ED, 0x303B3654, 0xE3E33CCA,
+0x02C26ECC, 0x26949920, 0x4445DF20, 0x01FDBC98,
+0x49138C6F, 0x1B5555E2, 0x122B45D2, 0x4B2E0202,
+0x7B6014D4, 0xFAE0CD09, 0x77E165A0, 0xFBE76980,
+0xF5808BD3, 0xFD110E5E, 0x97450E11, 0x297F9B1F,
+0x607A2C41, 0xE384DFC9, 0x25D9A8DC, 0xF919D955,
+0x5E025993, 0xCC318847, 0x9717D2D5, 0x48F0DD1F,
+0x6CC4A8EB, 0x9BD0F4E1, 0x506F2A93, 0x18B8748E,
+0x16FFBA48, 0x552E4955, 0xB963F64F, 0xA1A34AC8,
+0x62E95CC7, 0x4D87EA89, 0x21E8C031, 0xC1F0ED07,
+0x28B7BB22, 0x0B838D04, 0x6361B440, 0xA653521C,
+0x92DA3F78, 0x4241CFED, 0xFAFCBD41, 0x3EFAB6BC,
+0x25F30607, 0x41BB70DA, 0x9FF3440A, 0x2502039E,
+0x3813EC82, 0xC6A4FD6B, 0xF8537C8C, 0x098ED49F,
+0xE0A0BD6E, 0x6BA2F2B3, 0xC35C9D9D, 0x1256E66A,
+0x790B2490, 0xD5C69889, 0x39E712FE, 0xCF73DE0B,
+0x41B3B614, 0x745ABD73, 0x654C79D8, 0x5B15923D,
+0x8C15F218, 0x585CCCF0, 0x624F7B44, 0x76BDDFDB,
+0x96F26B52, 0xE13058A1, 0x086C950E, 0x29519DEA,
+0xA42CFE04, 0x0D7A190B, 0xD0678C6A, 0xABB78679,
+0xBA48A2E4, 0x5F3DA10A, 0x11F04183, 0xAC720A3F,
+0x6A807781, 0x6F146BFB, 0xE8A67934, 0x54578834,
+0xAA60C8F0, 0x2061A1E6, 0x9E87799B, 0x68D91F86,
+0x8974F540, 0xB1C3F101, 0x99C21E56, 0xB57BA73F,
+0x8B2DAA3E, 0xF1E2D24E, 0x48F7D4EE, 0x7039FDB3,
+0xC666EEDC, 0x251F972E, 0x4D53F6BF, 0x6CC73EE7,
+0xCB07F7B9, 0x69ECB8CA, 0x363FD80C, 0x3B587AB3,
+0x738C1E5C, 0x5C9C1D92, 0xE7B52396, 0xEDE6324B,
+0xFE5B5045, 0xC90D8B3E, 0x371A0128, 0xF2C8DCF8,
+0x5B648CB5, 0x12F8E8FF, 0x5FE4BA71, 0xB925CFBE,
+0x7416E14F, 0x76489FFE, 0x1F4DE367, 0xA400F039,
+0x66390E83, 0x1AE79CEC, 0xDB573E98, 0xB6021F29,
+0xD01615E5, 0x02A2281F, 0xE85019C1, 0x027BB41F,
+0x8D9177C3, 0x79026E78, 0xF158B623, 0xBEFF5858,
+0x7B63518E, 0x8F42C08C, 0xB388227D, 0x940D607A,
+0xA4C79541, 0x9800CC91, 0xA356B535, 0x285BABB9,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0xE411E520, 0xA0024528, 0x442B4428, 0x96070009,
+0x46106246, 0x8FFB2522, 0xD4027504, 0x0009AFF5,
+0x00000FB3, 0x00200004, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x16D49357,
+0x0009000B, 0x4F222FE6, 0xDE947FFC, 0xE114D594,
+0x1E13D494, 0x67521E4C, 0xD494D693, 0x37402769,
+0x62528F06, 0x7201D692, 0x60602522, 0x2600C93F,
+0xD7906152, 0x2512611D, 0x264B6652, 0x2562470B,
+0x0009B00D, 0xE60095AC, 0xC84060E2, 0x2F028F03,
+0x8FF93652, 0x7F047601, 0xA05A4F26, 0x4F226EF6,
+0x410BD185, 0xD4850009, 0x0009440B, 0x450BD584,
+0xD7840009, 0xD284E1FF, 0x2712611D, 0xD4835029,
+0xE1FFCB01, 0x1209E501, 0x12112212, 0xE7202452,
+0x4718D57F, 0x2572D27F, 0xD17FE700, 0xD680D47F,
+0xE2012270, 0x24702172, 0xD67E2620, 0x2641E4FF,
+0xD57DE600, 0x666DE104, 0x76016063, 0x4000626D,
+0x8FF83212, 0xD5790545, 0x2520E201, 0xD279D778,
+0x2710E100, 0xE5802212, 0x655C6613, 0x666DD476,
+0x76046763, 0x374C626D, 0x8FF83253, 0xD4732712,
+0xD573E101, 0xD6732410, 0x2542E400, 0xE03AE501,
+0xD272D771, 0xE0390654, 0x27110654, 0x000B4F26,
+0x7FC82211, 0xD76FD16E, 0xDC70DB6F, 0xD271DE70,
+0xD572D471, 0x1F12D672, 0x1F76710C, 0x1FB877FC,
+0x1FEA1FC9, 0x72041F2B, 0xDE6FDC6E, 0x1F13EB10,
+0x1F511F44, 0x1F771F65, 0xD86C1F2C, 0xDD6DD96C,
+0xD26DEA00, 0x89003A22, 0xD1587A01, 0x88016010,
+0x56F98B03, 0x4218E201, 0xD1682622, 0x0009410B,
+0x440BD467, 0xD5670009, 0x0009450B, 0x6010D14C,
+0x8B108801, 0xE650D14B, 0x46186212, 0x8B083266,
+0x56FAD147, 0x2120E200, 0xCB016062, 0x2602A003,
+0x72012710, 0x60822122, 0x89098801, 0xE2C8D15A,
+0x622C6612, 0x89033626, 0x6010D158, 0x8BC88801,
+0x51F76792, 0x217252F6, 0xD6555191, 0x55FB2212,
+0x52FC6462, 0x55612542, 0x2252E400, 0x61436643,
+0x05DE6013, 0x36CC4608, 0x02DE2652, 0xC9036021,
+0x8B028801, 0x720162E2, 0x74012E22, 0x36B3664C,
+0x71048FEE, 0x66C2D147, 0x45286512, 0x265B4518,
+0x60822C62, 0x89018801, 0x0009A168, 0x6272D742,
+0x8B132228, 0xD726D541, 0x6552D441, 0x51436672,
+0x316C365C, 0x27622668, 0x14138D05, 0x6262D63D,
+0xB1A57201, 0xD61E2622, 0x2622E200, 0x52916692,
+0x8B013620, 0x0009A144, 0x6061A06E, 0x001C001C,
+0x001D4020, 0x0000B38E, 0xFFFF0000, 0x12340000,
+0x001E1015, 0x00201278, 0x002018A0, 0x00201922,
+0x0020128C, 0x001C3510, 0x001C3624, 0x001E212C,
+0x0020397C, 0x00203514, 0x00203984, 0x00203990,
+0x0020399C, 0x002039F8, 0x002039FC, 0x002039A4,
+0x002039A5, 0x002039A8, 0x00117700, 0x00203A12,
+0x00203578, 0x001142D8, 0x00203A14, 0x00203A16,
+0x001C3D30, 0x00117718, 0x001C3D00, 0x001C1000,
+0x001C36F8, 0x00117734, 0x001C3684, 0x00117710,
+0x001C3520, 0x00117600, 0x00117740, 0x001C1028,
+0x0020358C, 0x002039AC, 0x7FFFFFFF, 0x00201734,
+0x002032BE, 0x002022E8, 0x00203DC0, 0x002039FA,
+0x00203584, 0x002039EC, 0x001C3D2C, 0x001C36B0,
+0x0020351C, 0x0011775C, 0x8801C90F, 0xA0CF8901,
+0xD17C0009, 0x36206212, 0xD47B8904, 0x2421E200,
+0x2162A0CC, 0x6211D179, 0x89012228, 0x0009A0C3,
+0xE202D775, 0x75016571, 0x3123615D, 0x27518D02,
+0x0009A0BC, 0xD27255F2, 0x62226052, 0x40094019,
+0xC90F4009, 0x8F19880A, 0x52F31F2D, 0x40196022,
+0x40094009, 0x8808C90F, 0xA0A78901, 0x60630009,
+0x51F255F8, 0xE701CB01, 0x2502D263, 0xE1002172,
+0x2211D564, 0x74016452, 0x2542A098, 0x8B3F8805,
+0x602252F3, 0x40094019, 0xC90F4009, 0x8B168802,
+0xE5FFD45D, 0x655D6742, 0x8B102758, 0x6272D75B,
+0x8B0C3260, 0x55F257F8, 0x2762E101, 0xD5522512,
+0xD757E400, 0x62722541, 0xA0777201, 0x52F32722,
+0x40196022, 0x40094009, 0x8805C90F, 0x31B38B6E,
+0xD5508B6C, 0x615257F4, 0x7101E240, 0x64722512,
+0x1F4DD14D, 0x42182419, 0x8B033420, 0x6262D64B,
+0x26227201, 0xE200D640, 0x2621B0AA, 0x0009A056,
+0x3123E220, 0x88038B52, 0x52F38B1E, 0x40196022,
+0x40094009, 0x8803C90F, 0xD7418B16, 0x647251F4,
+0x7401D23D, 0x65122742, 0x1F5DE640, 0x46182529,
+0x8B033560, 0x6262D63B, 0x26227201, 0xE200D62E,
+0x2621B086, 0x0009A010, 0xD738D137, 0xD22A6412,
+0xE5007401, 0x21423A76, 0x22518F06, 0xEA00D634,
+0x72016262, 0x2622B074, 0x2FB2D532, 0x95406652,
+0xD4305BF1, 0x36205241, 0x60618910, 0x8B01C803,
+0x2B22E201, 0x8FF54510, 0x57F15664, 0x6272E1F0,
+0x41284118, 0x2722221B, 0x6BF2A008, 0x6BF2A006,
+0xE200D615, 0xD1152621, 0x2121E200, 0xE20256F5,
+0x42186662, 0x26284228, 0x1F6D8D0C, 0xD61FD11E,
+0x460B6511, 0x2008645D, 0x57F58904, 0x6272D11C,
+0x27222219, 0xD11BE201, 0x66122822, 0x8B012668,
+0x0009AE17, 0x450BD518, 0xD1180009, 0xAE10E600,
+0x07D12160, 0x00203A0C, 0x00203A10, 0x00203A18,
+0x001C3DC0, 0x0011772C, 0x001C3B88, 0x002039F4,
+0x0011773C, 0x00117744, 0x0000F000, 0x00117764,
+0x00117748, 0x00117768, 0x0011776C, 0x01FFFFFF,
+0x0011774C, 0x00203584, 0x001142D8, 0x00114774,
+0xFDFFFFFF, 0x00203DC0, 0x0020246C, 0x002039FA,
+0x2F962F86, 0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6,
+0xD11F7FF4, 0x6212DE1F, 0x67E25411, 0xD41E1F41,
+0x1F722F22, 0x6743D51D, 0x7794D21D, 0x5A425841,
+0x6C726942, 0x6D225B16, 0xE6006052, 0x2502CB20,
+0x7601E540, 0x3253626D, 0x62F28BFB, 0x212255F1,
+0x55F21151, 0x2E52D613, 0x14A21481, 0xD4122492,
+0x11B627C2, 0x674226D2, 0xD911DA10, 0x2A72E801,
+0x1A8C490B, 0x4218E201, 0x7F0C1A2C, 0x6EF64F26,
+0x6CF66DF6, 0x6AF66BF6, 0x000B69F6, 0x000068F6,
+0x001C3B9C, 0x001C3D98, 0x001C3700, 0x001C3500,
+0x001C5960, 0x001C8960, 0x0020358C, 0x001C3D00,
+0x00201610, 0x2F962F86, 0x2FC62FA6, 0x2FE62FD6,
+0x4F124F22, 0x7F884F02, 0xE018DEB2, 0xD4B261E0,
+0x61E30F14, 0x62107101, 0x440BE01C, 0x20080F24,
+0x8F126D03, 0xD4AD1F08, 0x6740DDAD, 0x657CD4AD,
+0x470BD7AD, 0xD2AD0009, 0x621C6120, 0x46086623,
+0x36284608, 0x3D6C4608, 0xE01C1FD8, 0xE58004FC,
+0x604C66E2, 0x3050655C, 0x2D628F17, 0x01FCE018,
+0xDEA3E500, 0x641CA008, 0x6753655D, 0x607037EC,
+0x31DC6153, 0x80147501, 0x3243625D, 0xD49D8BF4,
+0xE200D59D, 0xA27F2421, 0x20082521, 0xE0188B13,
+0xE50001FC, 0xA009DE96, 0x655D641C, 0x32EC6253,
+0x62536722, 0x32DC6672, 0x75041261, 0x3243625D,
+0xA2698BF3, 0x88012D10, 0xE0188B16, 0xE40001FC,
+0x671C2D40, 0x624DDE8A, 0x8B013273, 0x0009A25C,
+0x6DE3644D, 0x7D046243, 0x32EC6643, 0x652236DC,
+0x74086162, 0x2512AFEF, 0x8B198804, 0x01FCE018,
+0x2D70E700, 0x1FD56D1C, 0x627DDE7D, 0x8B0132D3,
+0x0009A242, 0x6173677D, 0x31EC65E3, 0x75046412,
+0x365C6673, 0x61426262, 0x21297708, 0x2412AFED,
+0x8B198805, 0x01FCE018, 0x2D70E700, 0x1FD46D1C,
+0x627DDE6F, 0x8B0132D3, 0x0009A226, 0x6173677D,
+0x31EC65E3, 0x75046412, 0x365C6673, 0x61426262,
+0x212B7708, 0x2412AFED, 0x8B598831, 0x61E6DE67,
+0x61E31F19, 0x64E27104, 0x1F4A6216, 0x1F2B6416,
+0x75E46513, 0x66536712, 0x1F4C7604, 0x64521F7D,
+0xD75F6E66, 0x27E0D25F, 0xDE5F6062, 0xC9013245,
+0x65622E00, 0x4609060A, 0x4609D15C, 0x46094509,
+0x21501F4E, 0xB2B0646D, 0x620D1F6F, 0x8B012228,
+0x0009A1EA, 0xD756DE55, 0x661C61E0, 0x6410D150,
+0x470B654C, 0x7FFC54FF, 0x2FE25EFE, 0x51FE7FFC,
+0x2F12E040, 0x55FBD14F, 0x57FD56FC, 0x04FE410B,
+0xD24D7F08, 0xE11C640D, 0x1D412D10, 0xD44B6522,
+0x67421D52, 0x1D73DE4A, 0xD24A65E2, 0x67221D54,
+0x1D75D249, 0xD2496E22, 0x66221DE6, 0x1D67A1BC,
+0x89018830, 0x0009A08E, 0xE340D538, 0x33FC6156,
+0x23126456, 0x71046153, 0x67521341, 0x13726416,
+0x7EE46E13, 0x65E66212, 0x66E3D731, 0x13246EE2,
+0x760427E0, 0x6062D22F, 0x3255DE2F, 0x2E00C901,
+0x060A6E62, 0xD12D4609, 0x4E094609, 0x13434609,
+0x646D21E0, 0xB2501F5E, 0x620D1F6F, 0x8B012228,
+0x0009A18A, 0xDE25D522, 0x61E06450, 0xD724654C,
+0x470B54FF, 0x7FFC661C, 0x06FEE054, 0x7FFC2F62,
+0xEE4001FE, 0x2F123EFC, 0x55E2D125, 0x57E456E3,
+0x64E2410B, 0xD21C7F08, 0xE11C640D, 0x1D412D10,
+0xD61A6522, 0x67621D52, 0x1D73DE19, 0xD2196EE2,
+0x62221DE4, 0xD2181D25, 0x1D266222, 0x6222D217,
+0x1D27A15A, 0x00117800, 0x00202A18, 0x00203996,
+0x002035BC, 0x00203A7C, 0x002018D0, 0x00203995,
+0x00117804, 0x00203A14, 0x00203A16, 0x00117810,
+0x00203991, 0x10624DD3, 0x00203992, 0x00203993,
+0x00114AA4, 0x00200F68, 0x001C5864, 0x001C6864,
+0x001C7864, 0x001C59BC, 0x001C69BC, 0x001C79BC,
+0x00200FC0, 0x8B048833, 0x470BD7A2, 0xA123EE00,
+0x88282DE0, 0xA0D38901, 0xDE9F0009, 0x62E1E143,
+0x3216E054, 0x0FE68F02, 0x2E21E240, 0x622D62E1,
+0x8B013217, 0x0009A0BC, 0xE50185E1, 0x8B013056,
+0x0009A0B6, 0x2D10E101, 0x64E1B111, 0x06FEE054,
+0x6261E143, 0x3517652D, 0xE6408945, 0x8B0C3563,
+0xE058E41A, 0xE5000F45, 0x72C0E05C, 0x60230F55,
+0x6703C907, 0xA014E060, 0x66530F75, 0x46214621,
+0x46214621, 0x45214621, 0xE0587618, 0x0F654521,
+0xE0034521, 0xE05C2509, 0xE0070F55, 0xE0602209,
+0xE8540F25, 0x858238FC, 0x640D65F3, 0x1844B170,
+0xDD7A8584, 0x85866C0D, 0x610D4C08, 0x410860C3,
+0xE00F0EFE, 0x18154D0B, 0x2E296207, 0x668260C3,
+0x85620FE6, 0x4D0B5185, 0x2E0B600D, 0x548460C3,
+0xB13C0FE6, 0xE05465F3, 0xE5400EFE, 0xE06C62E1,
+0x3653662D, 0x0F668D41, 0xC9036023, 0x40004008,
+0x61036403, 0xD965E070, 0x0F46E5FF, 0xE074655C,
+0x60530F96, 0x6263490B, 0x42214221, 0x42214221,
+0x42006723, 0x4200327C, 0x6C074621, 0x4621E054,
+0x606309FE, 0x4008C903, 0x790630FC, 0x6A036D2D,
+0x65F3E800, 0x64D3B124, 0xE0706EA2, 0x2AE22EC9,
+0x01FE6694, 0x666CE074, 0x470B07FE, 0x2E0B6063,
+0x65F32AE2, 0xB0FA64D3, 0x628D7801, 0x32E3EE06,
+0x7D018FE7, 0x0EFEE054, 0xE05462E1, 0x420006FE,
+0x760C8561, 0x701B302C, 0xE4006103, 0xE70465F3,
+0x68667401, 0x3973694D, 0x8FF92582, 0x65F37504,
+0x641DB0DD, 0x0EFEE054, 0x64E1B09C, 0x0009A054,
+0xD43B56F8, 0xEA01D23B, 0x26A0420B, 0x0009A04C,
+0x06FCE01C, 0x8829606C, 0x5CF88B08, 0xE200D636,
+0x52612C20, 0x642DB04B, 0x0009A03E, 0x666CE681,
+0x8B043060, 0x420BD231, 0xA03554F8, 0xE6820009,
+0x3060666C, 0xD22E8B04, 0x54F8420B, 0x0009A02C,
+0x666CE683, 0x8B0A3060, 0xDA2755F8, 0x2590E900,
+0xD82855A1, 0x2852D628, 0xA01D52A2, 0xE6922620,
+0x3060666C, 0xD2208B08, 0x5C21D824, 0x6CCC52F8,
+0x28C1E600, 0x2260A010, 0x666CE693, 0x8B063060,
+0xD61F59F8, 0xE201EA00, 0xA00529A0, 0xD6162621,
+0xD21DD41C, 0x6562420B, 0x4F067F78, 0x4F264F16,
+0x6DF66EF6, 0x6AF66CF6, 0x000B69F6, 0x4F2268F6,
+0xE240614D, 0x89323123, 0x3127E21F, 0x8B27D713,
+0xD406614D, 0xE00171E0, 0x5671440B, 0x26596507,
+0x1761A025, 0x00200FBC, 0x00117804, 0x00203470,
+0x00203A9C, 0x002018C0, 0x00117800, 0x00115F00,
+0x00116058, 0x0020397C, 0x00203990, 0x00203A1A,
+0x00203A16, 0x00203AB4, 0x002018D0, 0x001C3704,
+0xE001D490, 0x6672440B, 0x26596507, 0x4F262762,
+0x0009000B, 0x614D4F22, 0x3123E240, 0xE21F8912,
+0xD7893127, 0x614D8B08, 0x5671D286, 0x420B71E0,
+0x260BE001, 0x1761A006, 0x6672D282, 0xE001420B,
+0x2762260B, 0x000B4F26, 0xE6400009, 0x46284618,
+0x6252D57E, 0x89FC2268, 0x0009000B, 0x4618E680,
+0xD57A4628, 0x22686252, 0x000B89FC, 0xA0010009,
+0x7201E200, 0x8BFC3242, 0x0009000B, 0x4618E680,
+0xD5734628, 0x22686252, 0x000B8BFC, 0x2FE60009,
+0x7FFC4F22, 0xBFF16E53, 0x61E22F42, 0xE280D66D,
+0x54E11615, 0x16464218, 0x422855E2, 0x57E31657,
+0x16786EF2, 0x26E22E2B, 0x4F267F04, 0x6EF6AFCE,
+0x2FD62FC6, 0x4F222FE6, 0x6C53DD62, 0x6E43BFD6,
+0x2DE2BFBB, 0x0009BFD2, 0x2C1251D5, 0x1C4154D6,
+0x1C5255D7, 0x1C6356D8, 0x6EF64F26, 0x000B6DF6,
+0x61636CF6, 0xA004E600, 0x62564109, 0x24227601,
+0x36127404, 0x000B8BF9, 0xD6530009, 0x8562E500,
+0xA00B674D, 0x655D610D, 0x40006053, 0x305CD44F,
+0x024D4008, 0x3270622D, 0x75018905, 0x3213625D,
+0x000B8BF1, 0x000BE000, 0x2FE6E001, 0x54416743,
+0x4E08EE7F, 0x4E28D246, 0x25E96543, 0x60436E21,
+0x9E7562ED, 0x4529C903, 0xE60032E3, 0x8D456103,
+0x21184509, 0xD23F8B05, 0x002C6053, 0xA08AC93F,
+0x60136603, 0x8B268801, 0x880C6053, 0xD53A8B04,
+0xC93F8453, 0x6603A07F, 0x8B048808, 0x84E2DE36,
+0xA078C93F, 0x880D6603, 0x8B03D633, 0xC93F8461,
+0x6603A071, 0x88096260, 0x622C8F09, 0xE014DE2C,
+0x655C05EC, 0x60233258, 0xA064C93F, 0x60236603,
+0xA060C93F, 0x88026603, 0xE0078B5D, 0x60432509,
+0x8905C810, 0x6053D225, 0xC93F002C, 0x6603A053,
+0x6053DE23, 0xC93F00EC, 0x6603A04D, 0x88016013,
+0x60538B19, 0x8B04880C, 0x8423D21E, 0xA042C93F,
+0x88086603, 0xD51B8B04, 0xC93F8452, 0x6603A03B,
+0xD618880D, 0x84618B03, 0xA034C93F, 0x60606603,
+0xA030C93F, 0x88026603, 0xE0078B2D, 0x60432509,
+0x8923C810, 0x6053DE10, 0xC93F00EC, 0x6603A023,
+0x00000BB8, 0x00203470, 0x001C3704, 0x001C373C,
+0x001C3700, 0x001C370C, 0x00114000, 0x00114008,
+0x001142D8, 0x001142E4, 0x001142E8, 0x001142F5,
+0x001142ED, 0x001142FD, 0x00114309, 0x6053D209,
+0xC93F002C, 0x60136603, 0x8B038802, 0xC8106043,
+0x76028900, 0xC93F6063, 0x40004018, 0x1741240B,
+0x6EF6000B, 0x00114301, 0x0009A16E, 0x2FE62FD6,
+0xDD944F22, 0xA0049EB2, 0xD4930009, 0x420BD293,
+0x62D265D2, 0x8BF822E8, 0x0009A004, 0xD28FD490,
+0x55D1420B, 0x22E852D1, 0xA0048BF8, 0xD48D0009,
+0x420BD28A, 0x52D255D2, 0x8BF822E8, 0x0009A004,
+0xD286D489, 0x55D3420B, 0x22E852D3, 0xA0048BF8,
+0xD4860009, 0x420BD281, 0x52D455D4, 0x8BF822E8,
+0x6EF64F26, 0x6DF6000B, 0x2FD62FC6, 0x4F222FE6,
+0x6E636C73, 0x6D53B01A, 0x64D357F4, 0xB05F65E3,
+0xB07566C3, 0xB0A40009, 0xB0A80009, 0xB0AC0009,
+0xB0AC0009, 0xB0AF0009, 0xB03154F5, 0x6CCD6C03,
+0x4F2660C3, 0x6DF66EF6, 0x6CF6000B, 0x3412D170,
+0xD6700529, 0x2650D770, 0x2742000B, 0x0009A018,
+0x2FD62FC6, 0x4F222FE6, 0x6E636C73, 0x6D53BFEE,
+0x64D357F4, 0xB03365E3, 0xB08D66C3, 0xB00F54F5,
+0x6CCD6C03, 0x4F2660C3, 0x6DF66EF6, 0x6CF6000B,
+0xE503D162, 0xD763D462, 0x21524518, 0x2472000B,
+0xD45FD15E, 0x2162E600, 0x2462000B, 0xBF734F22,
+0xBF73E40A, 0xD25C0009, 0x4118E104, 0xE40AE500,
+0xBF692212, 0xD7592252, 0xCB206072, 0x000B4F26,
+0x4F222702, 0x410BD156, 0xD556E400, 0x4F26452B,
+0xD1552FE6, 0x66126E63, 0x92104418, 0x44084528,
+0x45002629, 0x265B4408, 0x264B4400, 0x21624708,
+0xD14E4708, 0x217227EB, 0x6EF6000B, 0x1FFF03F0,
+0x4F222FE6, 0xE101DE4A, 0xBF3DE40A, 0x67E32E12,
+0xE500776C, 0xE204E130, 0x2752E40A, 0x27522752,
+0x27522752, 0x27522752, 0x27522752, 0x27522752,
+0x27522752, 0x27522752, 0x27522752, 0x27522752,
+0x27222712, 0x27522752, 0x27522752, 0x27522752,
+0x27522752, 0x175ABF18, 0x2E62E600, 0x000B4F26,
+0xD2346EF6, 0xE441E101, 0x000B2212, 0xD1322242,
+0xE605D432, 0x000B2162, 0x000B2462, 0xD2300009,
+0xE40AE601, 0x2262AF00, 0x2FC62FB6, 0x2FE62FD6,
+0x7FFC4F22, 0x6C43DB2B, 0xED0060B2, 0x2B02CB03,
+0xC90360B2, 0x6E03A008, 0x89073DC2, 0xE46460B2,
+0xB07CC903, 0x7D016E03, 0x8BF52EE8, 0x8F043DC2,
+0xD4212FE1, 0x460BD621, 0x62F10009, 0x6023622D,
+0x89FFC801, 0x7F046023, 0x6EF64F26, 0x6CF66DF6,
+0x6BF6000B, 0x001C3B88, 0x00203AC8, 0x002018D0,
+0x00203AD0, 0x00203AD8, 0x00203AE0, 0x00203AE8,
+0x0025E720, 0x00203DBC, 0x00203980, 0x001C5968,
+0x001C3B40, 0x000F8000, 0x001D4004, 0x001C3500,
+0x002015E4, 0x00201610, 0x001C5814, 0x001C59D0,
+0x001C5830, 0x001C6268, 0x001C59A4, 0x001C639C,
+0x001C581C, 0x001C5860, 0x00203AF0, 0x002018C0,
+0x8F014411, 0x6043604B, 0x0009000B, 0x5651D52B,
+0x46286052, 0x306C000B, 0x2FC62FB6, 0x2FE62FD6,
+0x4F124F22, 0xBFF14F02, 0x6B036E43, 0xDD25DC24,
+0x0009BFEC, 0x3C0530B8, 0x4609060A, 0x46014609,
+0x020A3D65, 0x42094209, 0x32E24209, 0x4F068BF0,
+0x4F264F16, 0x6DF66EF6, 0x000B6CF6, 0x2FC66BF6,
+0x2FE62FD6, 0x4F124F22, 0xBFCF4F02, 0x6C036E43,
+0xBFCBDD13, 0x30C80009, 0x060A3D05, 0x46094609,
+0x36E24601, 0x4F068BF5, 0x4F264F16, 0x6DF66EF6,
+0x6CF6000B, 0x4F222FE6, 0xE102DE0B, 0xE403E500,
+0xBFB92E12, 0xE6062E52, 0xE7004618, 0x2E62E403,
+0x4F262E72, 0x6EF6AFB0, 0x0009000B, 0x001C1040,
+0xCCCCCCCD, 0x10624DD3, 0x001D4004, 0x2F962F86,
+0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6, 0xE5007F98,
+0x6453E710, 0x6B534728, 0xEE1ADCBC, 0x6153655D,
+0x315C4108, 0x75014108, 0x6043317C, 0x0F16665D,
+0xED0060B3, 0x21B136E3, 0x81128111, 0x11D28113,
+0x11D411D3, 0x74048FEA, 0xD8B167F2, 0x1871D9B1,
+0x58F12872, 0x1981D1B0, 0x59F22982, 0x5DF45AF3,
+0x54F65EF5, 0x21921191, 0x11A211A3, 0x11D411D5,
+0x11E611E7, 0x11481149, 0xDAA855F7, 0x57F8EE00,
+0x52F9DDA7, 0x64E3D6A7, 0x2A521A51, 0xD8A7D9A6,
+0x2D72EAEF, 0x6AAC2622, 0x6DE36EED, 0x61E34D08,
+0x41083DEC, 0x31EC4D08, 0x60B33D9C, 0x2DB14108,
+0xE05081D1, 0xE79F4108, 0x41084008, 0x81D2677C,
+0x318C60B3, 0x3472E200, 0x1DD281D3, 0xD4931D13,
+0x1D248D01, 0x65D3D48F, 0x7E01B0B2, 0x34A264ED,
+0xDA8C8BDA, 0x68A22FD2, 0x4829DD91, 0x64A22D82,
+0x694D7DFC, 0x2D92D286, 0x4E296E22, 0x2DE27D0C,
+0x6AD36822, 0xD784618D, 0x6D722A16, 0xD583D489,
+0x5E7224D2, 0x14E2D688, 0xEE005174, 0x58761414,
+0x1486D186, 0xE7105978, 0x62521498, 0x142A65E3,
+0x64E326E2, 0x644DE600, 0x48086843, 0x4808384C,
+0x6053381C, 0x28B10C86, 0x60B309CE, 0x60538191,
+0x60430ACE, 0x605381A2, 0x60B30DCE, 0x605381D3,
+0x740108CE, 0x09CE1882, 0x19E3624D, 0x32730ACE,
+0x8FE01A64, 0xD96A7504, 0x6C92E003, 0x2CB14018,
+0xDA6F6D92, 0xE05081D1, 0x40086E92, 0x619281E2,
+0x811360B3, 0xE6006492, 0x67921442, 0x17A3D468,
+0xE1FF6892, 0xE7031864, 0x46086563, 0x7501364C,
+0x665D2612, 0x8BF83673, 0xE003DC5A, 0x40186DC2,
+0x6EC22DB1, 0x81E1D25F, 0xEE0061C2, 0x64C21112,
+0x1423E024, 0xD45B65C2, 0x67C215E4, 0x8172E580,
+0x66E368C2, 0x655C8183, 0x6963666D, 0x6A6D7604,
+0x3A53394C, 0x29E28FF8, 0xDC54DB53, 0x740424B2,
+0x7F6824C2, 0x6EF64F26, 0x6CF66DF6, 0x6AF66BF6,
+0x000B69F6, 0x614268F6, 0xC8036011, 0xE5008F03,
+0x3420D23C, 0x60118B06, 0x8802C903, 0xD2398B06,
+0x8B033420, 0x65135612, 0x24225264, 0x6053000B,
+0x2FE62FD6, 0x7FEC4F22, 0x62536E53, 0x6D43E550,
+0x4508E400, 0xE101A001, 0x60435224, 0x81212211,
+0x60538123, 0x56E28122, 0x8BF53620, 0x16E4D238,
+0xE61464F3, 0x65E3420B, 0xE4FC65E1, 0x2E512549,
+0x65F361F1, 0x2F112149, 0xD13154D1, 0xE614410B,
+0x607157D1, 0x2701CB01, 0x7F141DE1, 0x6EF64F26,
+0x6DF6000B, 0x2FE62FD6, 0x7FEC4F22, 0x66536E53,
+0x6D43E5FC, 0x20596061, 0x2601CB01, 0x326052E2,
+0x12E48B06, 0x31E051E2, 0x52D18B04, 0x1E22A002,
+0x5664AFF0, 0x64F3D21E, 0x420BE614, 0x67E165E3,
+0x2719E1FC, 0x67F12E71, 0x271954D1, 0x65F3D118,
+0x410BE614, 0x52D12F71, 0xCB016021, 0x1DE12201,
+0x4F267F14, 0x000B6EF6, 0x00006DF6, 0x002039AC,
+0x0020357C, 0x00203584, 0x0020358C, 0x002035B4,
+0x00203998, 0x002039A0, 0x00100208, 0x001014C0,
+0x001E210C, 0x001C3D00, 0x002039EC, 0x001000C8,
+0x00117880, 0x00117780, 0x00040020, 0x0026C401,
+0x00200D42, 0x4F222FE6, 0xDE42624C, 0x42004208,
+0x3E2CA005, 0xD4405252, 0xBF695624, 0x65E22E62,
+0x352052E1, 0xD63D8BF6, 0x4F262622, 0x6EF6000B,
+0x2FC62FB6, 0x2FE62FD6, 0xDC394F22, 0x52C1DB39,
+0x362066C2, 0x6061891C, 0x8801C903, 0xDE348918,
+0xBF38DD35, 0x650364E3, 0x66B28503, 0x3262620D,
+0xD4328907, 0x0009BF76, 0x4D0BD431, 0xAFE60009,
+0xBF3D0009, 0xD42F64E3, 0x00094D0B, 0x0009AFDF,
+0x2262D22D, 0x6EF64F26, 0x6CF66DF6, 0x6BF6000B,
+0x2FD62FC6, 0x4F222FE6, 0xDD29DC28, 0x6E4360C2,
+0x04DE4008, 0xE614D127, 0x65E3410B, 0xD127D726,
+0x55E227E2, 0x35E05254, 0x21228F04, 0x400860C2,
+0x122202DE, 0x605365C2, 0x75014008, 0x0DE606DE,
+0xC90F6053, 0x60632C02, 0x6EF64F26, 0x000B6DF6,
+0x85436CF6, 0x650D5643, 0x622D6262, 0x35277204,
+0xE1008F0C, 0x2268960C, 0xD6158B03, 0x72015261,
+0xD6131621, 0x6262E101, 0x26227201, 0x6013000B,
+0x000001FF, 0x0020358C, 0x00203584, 0x001C3D00,
+0x002035B4, 0x0020397C, 0x002018C0, 0x0020357C,
+0x00203B18, 0x00203B1C, 0x001C3D28, 0x002039EC,
+0x002039AC, 0x00200D42, 0x002039F0, 0x002039F4,
+0x00117754, 0x2FA62F96, 0x2FC62FB6, 0x2FE62FD6,
+0x7FF84F22, 0x6C22D241, 0xC80360C3, 0xDE40896E,
+0xDA41DB40, 0x52B1D941, 0x362066B2, 0x60618945,
+0x8801C903, 0xDD3B8941, 0x420BD23D, 0x650364D3,
+0x60A12F02, 0x89328801, 0x85145153, 0x8840600C,
+0x1F118F0C, 0xD5376191, 0x641D450B, 0x8B262008,
+0xD7356691, 0x646D470B, 0x8B202008, 0x420BD233,
+0x51F154F1, 0xC8208511, 0xD1318904, 0x021EE050,
+0x01267201, 0x420BD22F, 0x200864F2, 0x64D38907,
+0x4D0BDD2D, 0xD12D65F2, 0xAFC4E601, 0xD22C2162,
+0x420B65F2, 0xD72B64E3, 0xAFBCE601, 0xD2262762,
+0x420B65F2, 0xAFB664D3, 0xDE270009, 0xDA28DD27,
+0x52D1DB28, 0x362066D2, 0x60618918, 0x8801C903,
+0xD4228914, 0x450BD516, 0x56030009, 0x8F0436E0,
+0xE2016503, 0xAFEC2A20, 0xD41F2B52, 0x420BD216,
+0xD7180009, 0x4118E101, 0x2712AFE3, 0xC80460C3,
+0xD21A8902, 0x0009420B, 0x4F267F08, 0x6DF66EF6,
+0x6BF66CF6, 0x000B6AF6, 0x000069F6, 0x001E2100,
+0x0020358C, 0x00203584, 0x00203A14, 0x001142D8,
+0x002014A6, 0x00115EA2, 0x00114774, 0x00200D8A,
+0x0020351C, 0x002016C2, 0x002014D0, 0x001E212C,
+0x00201534, 0x001C3D30, 0x00117880, 0x0020357C,
+0x0020399C, 0x00203998, 0x002035B4, 0x00200644,
+0xE601D203, 0x1265D503, 0x000B2252, 0x00001266,
+0x001C1010, 0x0000C34F, 0x0009000B, 0x0009000B,
+0x0009000B, 0x0009000B, 0xE000000B, 0xE000000B,
+0x0009000B, 0xE4FDD59D, 0xD69D6152, 0x25122149,
+0x74016052, 0x2502CB01, 0xD19A6752, 0x25722749,
+0xC8406010, 0x60628902, 0x2602CB04, 0xE1F76462,
+0x26422419, 0xE7016062, 0x2602C9CF, 0xE5026062,
+0x2602CB10, 0x47186062, 0x2602CB03, 0x000B1652,
+0xD58D1673, 0xD28ED78D, 0xE100D48E, 0x2511E600,
+0x22102711, 0x2461AFCE, 0xD28B664C, 0x362C4600,
+0xCB106060, 0x2600000B, 0xD287654C, 0x352C4500,
+0xE1EF6650, 0x000B2619, 0x664C2560, 0x4600D283,
+0x6060362C, 0x000BCB10, 0x654C2600, 0x4500D27F,
+0x6650352C, 0x2619E1EF, 0x2560000B, 0xD27A664C,
+0x362C4600, 0xCB086060, 0x2600000B, 0xD276654C,
+0x352C4500, 0xE1F76650, 0x000B2619, 0x664C2560,
+0x4600D272, 0x6060362C, 0x000BCB08, 0x654C2600,
+0x4500D26E, 0x6650352C, 0x2619E1F7, 0x2560000B,
+0xD669624C, 0x326C4200, 0xC9086020, 0x40214021,
+0x000B4021, 0x624C600C, 0x4200D664, 0x6020326C,
+0x4021C908, 0x40214021, 0x600C000B, 0x644CD160,
+0x6240341C, 0x602C000B, 0x644CD15E, 0x6240341C,
+0x602C000B, 0x4F222FE6, 0x645C6E43, 0x3467E60A,
+0xBFEB8914, 0x640C0009, 0x880160EC, 0xE00F8B02,
+0x2409A002, 0x44094409, 0xE60A624C, 0x89053263,
+0x644CBFE2, 0x6023620C, 0x8B00C880, 0x6023E200,
+0x000B4F26, 0x4F226EF6, 0x6062D64B, 0x8B038801,
+0x0009B256, 0x0009A003, 0xE640D248, 0xD6482260,
+0x4F26E200, 0x2622000B, 0xD6434F22, 0x88026062,
+0xB29F8B01, 0xD6420009, 0x4F26E200, 0x2622000B,
+0xD43ED53D, 0xE701E100, 0x000B2512, 0xD23B2470,
+0x000BE604, 0x4F222260, 0xD13BD43A, 0x0009410B,
+0xE1FDD53A, 0xD23A6650, 0xE7002619, 0x4F262560,
+0x2270000B, 0xD5374F22, 0x6152D237, 0x611DD737,
+0x64522512, 0x242BE6FF, 0xD4352542, 0x666DD22E,
+0x2762420B, 0xE1FBD52D, 0x27196750, 0x000B4F26,
+0x4F222570, 0xD128D42F, 0x0009410B, 0xE7F7D527,
+0x26796650, 0x000B4F26, 0xD5242560, 0x62509425,
+0x000B2249, 0xD5212520, 0x6250E4BF, 0x000B2249,
+0x4F222520, 0x8522D224, 0x2008600D, 0x88018911,
+0x88038944, 0x88058946, 0x88068948, 0x8808894E,
+0x88098954, 0x880A895A, 0x880B8960, 0xA06D8966,
+0xB06F0009, 0xA06A0009, 0xFF7F600C, 0x001E2148,
+0x001E1108, 0x001E1000, 0x00203A4C, 0x00203A4E,
+0x00203A6D, 0x00203A30, 0x001E103F, 0x001E105F,
+0x001E102F, 0x001E1090, 0x00203A54, 0x001E100B,
+0x00203A50, 0x00203B20, 0x002018C0, 0x001E1028,
+0x00203A6C, 0x001D4020, 0x98760000, 0x001C1000,
+0x00203B2C, 0x00203B3C, 0x00203A24, 0x0009B04C,
+0x600CA035, 0x0009B055, 0x600CA031, 0x6260D684,
+0x8B2B2228, 0x0009B061, 0x600CA029, 0x6260D680,
+0x8B232228, 0x0009B069, 0x600CA021, 0x6260D67C,
+0x8B1B2228, 0x0009B0C7, 0x600CA019, 0x6260D678,
+0x8B132228, 0x0009B0CD, 0x600CA011, 0x6260D674,
+0x8B0B2228, 0x0009B125, 0x600CA009, 0x6260D670,
+0x8B032228, 0x0009B13D, 0x600CA001, 0x4F26E000,
+0x0009000B, 0xD26CD16B, 0xD56C8412, 0x4000C90F,
+0xD76B012D, 0xE403D66B, 0xE20F611C, 0x2540E001,
+0x25202712, 0x2602000B, 0xE601D262, 0x30668523,
+0xE0008D05, 0xD663D260, 0xE0018122, 0x000B2602,
+0xD25C0009, 0x600D8523, 0x89052008, 0x8B0A8801,
+0x6060D65D, 0x2600CB01, 0xD457D65A, 0xE001E101,
+0x000B2612, 0x000B8142, 0xD152E000, 0x8513E501,
+0x640D4518, 0x66033453, 0xE0008D05, 0xD551D253,
+0x2260E001, 0x000B2502, 0x4F220009, 0x8513D149,
+0x6453650D, 0x62494419, 0x227D672E, 0x8801602C,
+0x88028909, 0x88038910, 0x8806891A, 0x88078935,
+0xA04C893B, 0xD5460009, 0x6652D746, 0x2762D446,
+0x622C6261, 0x2421A038, 0x2228625C, 0xD4438B3F,
+0x6642D540, 0x2562D440, 0x24018561, 0x6203A02C,
+0x2008605C, 0x88108907, 0x88208908, 0x88308909,
+0xA02C890A, 0xD23A0009, 0x6222A008, 0xA005D239,
+0xD2396222, 0x6222A002, 0x6262D638, 0xD432D531,
+0x66212522, 0xA00F626C, 0xD6352421, 0x6261D52D,
+0x622CD42D, 0xA0072562, 0xD6322421, 0x8561D529,
+0x2562D429, 0x62032401, 0x662D8515, 0x3617610D,
+0x65038F01, 0xB0CB2451, 0xA0010009, 0xE000E001,
+0x000B4F26, 0xD6190009, 0xD427E101, 0x65412610,
+0xD118D717, 0xE20F655D, 0x2752E001, 0x000B2620,
+0x2FE62102, 0xD20F4F22, 0x640C8523, 0x8B082448,
+0xD511D61D, 0x2621E200, 0x940F8451, 0xA0482049,
+0xDE0D8051, 0xC84060E0, 0xE2018D32, 0x89443427,
+0xD216D615, 0x2641420B, 0x0009A030, 0x0000FF7F,
+0x00203A6D, 0x00203A24, 0x00203A30, 0x001E1100,
+0x001E100C, 0x00203A50, 0x001E1000, 0x001E1001,
+0x00203A58, 0x00203A38, 0x00203A3C, 0x00203A40,
+0x00203A5C, 0x00203A60, 0x00203A64, 0x00203A68,
+0x00203E20, 0x00203E2A, 0x00203A4A, 0x002027F2,
+0x89123427, 0xD294D693, 0x2641420B, 0xCB8084E1,
+0x80E1B0F5, 0xD69160E0, 0x2E00CB04, 0xC93F6060,
+0xD68F2600, 0xA001E001, 0xE0002602, 0x000B4F26,
+0xD68C6EF6, 0xC8806060, 0xD2868919, 0x88016021,
+0xD2898B15, 0x8524E501, 0x89103056, 0xE203D187,
+0x2120D487, 0xE00B6541, 0x0656655D, 0xE40FD585,
+0x2140E702, 0xD77E2571, 0x000BE001, 0x000B2702,
+0x2FE6E000, 0xDE804F22, 0xC88084E1, 0xD57A892C,
+0x20088554, 0x61038F28, 0x8553D77C, 0x64036672,
+0x8566650C, 0x3520620C, 0xD6798B1E, 0x651CD774,
+0x2651644C, 0x60E02741, 0x8904C840, 0x420BD275,
+0xA0030009, 0xD2680009, 0x0009420B, 0x0009B09F,
+0xE201D167, 0x60E02122, 0xCB04D464, 0x60402E00,
+0x2400C93F, 0x6023A001, 0x4F26E000, 0x6EF6000B,
+0x2FB62FA6, 0x2FD62FC6, 0xDA622FE6, 0x66A1E240,
+0x3622DC5E, 0x62638900, 0x6ED36D2C, 0x4E2136D8,
+0x4E212A61, 0xDB61D460, 0xE700A00F, 0x770162B2,
+0x71026123, 0x66212B12, 0x71026213, 0x61212B12,
+0x651D666D, 0x356C4528, 0x627C2452, 0x8BED32E3,
+0xC90360D3, 0x8B108803, 0x617367B2, 0x2B127102,
+0x71026E13, 0x2B126571, 0x655D6DE1, 0x422862DD,
+0x325CE107, 0xA00C2C10, 0x88022422, 0xA0038B01,
+0x8801E203, 0xE2018B05, 0x66B22C20, 0x655D6561,
+0xE60F2452, 0x67A12C60, 0x8B052778, 0xDD38DC44,
+0xEB01EA00, 0x2DB22CA2, 0x6DF66EF6, 0x6BF66CF6,
+0x6AF6000B, 0x2FE62FD6, 0xE240DD36, 0x362266D1,
+0x62638900, 0x3678672C, 0x7703DE38, 0x47212D61,
+0x64E2D635, 0xA00E4721, 0x6562E100, 0x62537101,
+0x74012450, 0x24204219, 0x45297401, 0x74012450,
+0x24504519, 0x621C7401, 0x8BEE3273, 0x66E24200,
+0x420061D1, 0x2118362C, 0x2E628F06, 0xDD1CD728,
+0xE501E400, 0x2D522742, 0x000B6EF6, 0x2FD66DF6,
+0x4F222FE6, 0xED0AEE01, 0x64E3BC86, 0xBC8B64E3,
+0x62EC7E01, 0x8BF732D7, 0xBC8EEE01, 0x64E364E3,
+0x7E01BC93, 0x32D762EC, 0x4F268BF7, 0x000B6EF6,
+0xD1186DF6, 0xD418920D, 0x72122122, 0x2422D617,
+0xD7177204, 0x72202622, 0x2722D116, 0x000B7230,
+0x137A2122, 0x00203A4A, 0x002028FE, 0x001E1015,
+0x00203A50, 0x001E1001, 0x00203A24, 0x001E1100,
+0x00203A4E, 0x00203A3C, 0x001E1000, 0x00203A40,
+0x00203A4C, 0x002027F2, 0x001E100C, 0x00203A38,
+0x00203A54, 0x00203A58, 0x00203A5C, 0x00203A60,
+0x00203A64, 0x00203A68, 0x4F222FE6, 0xD6707FFC,
+0x88016060, 0xE2018951, 0x2620BFBB, 0xD56ED16D,
+0xDE6E6010, 0x64E36552, 0x7402C840, 0x8D22D16C,
+0xD26C7502, 0xE601D76C, 0xE7042722, 0x76016255,
+0x626C2421, 0x8FF93273, 0xD4637402, 0x6242E601,
+0x640D8528, 0x67494419, 0x275D657E, 0x81E4607C,
+0xE417D562, 0x67557601, 0x3243626C, 0x8FF92171,
+0xA0207102, 0xD25E0009, 0xE601D75B, 0xE7042722,
+0x76016255, 0x626C2421, 0x8FF93273, 0xD4527402,
+0x6242E601, 0x640D8528, 0x67494419, 0x275D657E,
+0x81E4607C, 0xE417D553, 0x67557601, 0x3243626C,
+0x8FF92171, 0x92897102, 0xD2462E21, 0x5E23D74E,
+0x64F22FE2, 0x604365F2, 0x2700C980, 0xC9606043,
+0x80716103, 0xC9036043, 0x80724519, 0x65F2605C,
+0x817266F2, 0x46194629, 0x606C4529, 0x4018645C,
+0x8173304C, 0x21185E23, 0x64F22FE2, 0x6E4C62F2,
+0x602C4219, 0x66F262F2, 0x46294018, 0x461930EC,
+0x42298174, 0x652C606C, 0x305C4018, 0x81758F07,
+0x0009BC97, 0x2228620C, 0xA00A8908, 0x60130009,
+0x8B038840, 0x0009B009, 0x0009A003, 0xE202D62F,
+0x7F042622, 0x000B4F26, 0x4F226EF6, 0x8552D52A,
+0x8830600D, 0x88318903, 0xA0348923, 0x85550009,
+0xD428D727, 0x85532701, 0x610DD627, 0x24124118,
+0x460BD426, 0xD7230009, 0xD226D425, 0x6572420B,
+0xE230D120, 0x42286712, 0x2729E620, 0x37604628,
+0xD6218B03, 0xA016E200, 0xD61F2622, 0xA012E202,
+0xD1182622, 0x6212E530, 0xE6204528, 0x46282259,
+0x89083260, 0xD41AD119, 0xE601D513, 0x2160450B,
+0x472BD718, 0x4F264F26, 0x0009000B, 0x0000060A,
+0x00203A6C, 0x001E1000, 0x00203A58, 0x00203E20,
+0x00203E2C, 0x00203DC4, 0x00203A40, 0x00203DF4,
+0x00203DF2, 0x00203DC6, 0x00203A24, 0x00203A50,
+0x00203A3C, 0x00203A38, 0x002018C0, 0x00203B48,
+0x00203B4C, 0x002018D0, 0x00203A54, 0x001E100B,
+0x00203B60, 0x00114004, 0x4F222FE6, 0x84E9DE86,
+0x2448640C, 0xB17B8901, 0xD2840009, 0x26686620,
+0x60E08902, 0x2E00C9BF, 0x000B4F26, 0x000B6EF6,
+0x2FE60009, 0xDE7E4F22, 0x60E0D67E, 0xCBC0D47E,
+0x62602E00, 0xC803602C, 0x40218904, 0x70014021,
+0x6603A002, 0x66034009, 0xD678616D, 0xE500A004,
+0x75016262, 0x74042422, 0x3213625D, 0xD2748BF8,
+0x0009420B, 0xC9BF84E2, 0x4F2680E2, 0x6EF6000B,
+0x2FE62FD6, 0x7FFC4F22, 0x6260D66E, 0x89402228,
+0xD565E100, 0x60502610, 0xCB40D46B, 0x2500440B,
+0x8D052008, 0x62E06E03, 0x7104612C, 0x2F11A006,
+0xD466D65E, 0xDD666760, 0x657C4D0B, 0xE23C6D1D,
+0x8B033D27, 0xD264D463, 0x0009420B, 0x4D214D21,
+0xA005D762, 0x66E6E400, 0x357C4508, 0x74012562,
+0x35D3654D, 0xD75E8BF7, 0x6E72E003, 0x81E14018,
+0x6E7260F1, 0x81E2700C, 0xD45A6172, 0xDD5A8113,
+0x65724D0B, 0xD64AD259, 0x2212E101, 0xC93F6060,
+0x7F042600, 0x6EF64F26, 0x6DF6000B, 0x2FC62FB6,
+0x2FE62FD6, 0xD2524F22, 0x6B436E73, 0x420B6C53,
+0x20086D63, 0x64038D1C, 0xE50ED13C, 0x32526210,
+0x60C38916, 0x804124B0, 0x814160D3, 0xA007E500,
+0x655D61BC, 0x00EC6053, 0x364C6653, 0x80647501,
+0x3213625D, 0xD6308BF5, 0xC9BF6060, 0x2600A008,
+0xD239D440, 0x6EF64F26, 0x6CF66DF6, 0x6BF6422B,
+0x6EF64F26, 0x6CF66DF6, 0x6BF6000B, 0x2F962F86,
+0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6, 0xE1007FC4,
+0x6513ECFF, 0x6B136CCD, 0xDE34D733, 0xEDFF64F3,
+0xD833EA04, 0x6053655C, 0x027D4000, 0x32C0622D,
+0x66038D0D, 0x09ED6063, 0x2491027D, 0x24217402,
+0x698202ED, 0x3928622D, 0x74022892, 0x75017104,
+0x6063625C, 0x07D532A2, 0x0EB58FE4, 0x2448641C,
+0xE6808905, 0x67F3E5C5, 0xBF8F666C, 0x7F3C655C,
+0x6EF64F26, 0x6CF66DF6, 0x6AF66BF6, 0x000B69F6,
+0xD11C68F6, 0x6012D21C, 0xCB20E405, 0x2102E500,
+0x000B2242, 0x00002252, 0x001E1017, 0x00203996,
+0x001E1015, 0x001E10BF, 0x00117800, 0x001E10FC,
+0x00200644, 0x0020399C, 0x00202A56, 0x00203B64,
+0x002018D0, 0x00203B80, 0x002018C0, 0x0011788C,
+0x00203998, 0x0020357C, 0x00201534, 0x001E2130,
+0x00202A18, 0x00203B88, 0x002039FC, 0x00203A04,
+0x00203DC0, 0x001C3500, 0x001D4004, 0xD564D163,
+0xE400D764, 0x2142E20F, 0x17411154, 0xD5622722,
+0x9669D762, 0x15412572, 0x96661562, 0xE6011565,
+0xD55F1165, 0x666CE6F8, 0x25422542, 0x25422542,
+0x25422542, 0x25622542, 0x7601E727, 0x67632572,
+0x25627797, 0xE7042572, 0x2572E248, 0xE2192522,
+0xE2702522, 0x25422542, 0x25422542, 0x25222542,
+0x2522E20C, 0x25422542, 0x25422542, 0x25422542,
+0x25422542, 0x000B154A, 0xE2081145, 0x0009422B,
+0x2FE62FD6, 0x7FFC4F22, 0xC8206043, 0x6E438D02,
+0x0009BE85, 0xC81060E3, 0xBE828901, 0x60E30009,
+0x8901C840, 0x0009BEA4, 0xC80160E3, 0xDD3D8938,
+0xC80260D0, 0x2F008D03, 0x460BD63B, 0x60F00009,
+0x8902C804, 0x460BD639, 0x62F00009, 0xC8806023,
+0x60D08902, 0x2D00C97F, 0xC8016023, 0xD6348906,
+0x0009460B, 0x0009A007, 0x51630601, 0x8902C808,
+0x460BD630, 0x60F00009, 0x8902C810, 0x420BD22E,
+0xD52E0009, 0x88026052, 0xD22D8B03, 0xA005E604,
+0x88012260, 0xD22A8B02, 0x2260E601, 0x2522E200,
+0xC88060E3, 0xD227892D, 0x60E36E20, 0x8902C880,
+0x420BD225, 0x60E30009, 0x8902C840, 0x420BD223,
+0x60E30009, 0x8902C802, 0x420BD221, 0x60E30009,
+0x890DC804, 0xDD20D11F, 0x0009410B, 0x0009BF11,
+0x0009BF4C, 0xD51ED41D, 0x2470E708, 0x25D2BF85,
+0xC80860E3, 0xD21B8905, 0x4F267F04, 0x422B6EF6,
+0x7F046DF6, 0x6EF64F26, 0x6DF6000B, 0x001C581C,
+0xA000A000, 0x001D0100, 0x001D4000, 0x00040021,
+0x001C589C, 0x001E1021, 0x00201A46, 0x00201A68,
+0x002020C8, 0x00201A80, 0x00201A8E, 0x00203A50,
+0x001E100B, 0x001E1028, 0x00201AFA, 0x00201B06,
+0x00201A96, 0x00201AB4, 0x12345678, 0x001E1000,
+0x0010F100, 0x00201AE2, 0x644CD6A7, 0x000B346C,
+0xD6A62450, 0x346C644C, 0x2450000B, 0x644CD6A4,
+0x000B346C, 0x625C2450, 0x4208616D, 0x42084119,
+0x42006019, 0x670E614C, 0xD49E321C, 0x4200207D,
+0x324CC90F, 0x2200000B, 0x4208625C, 0x42004208,
+0x324C644C, 0x4200D498, 0x000B324C, 0x2FE62260,
+0x614C4F12, 0x4100D493, 0x6710314C, 0xE29F666D,
+0x27294619, 0x6E536269, 0x672E6573, 0x4221227D,
+0x42214221, 0x7601662C, 0xE4014608, 0x34E84608,
+0x644C4600, 0x071A0467, 0x2150257B, 0x000B4F16,
+0x4F226EF6, 0xD2857FE8, 0x88016021, 0xD2848B7B,
+0x26686621, 0xD2838B77, 0x26686621, 0xE50F8B73,
+0xE401BFA2, 0xBFA4E501, 0xE586E400, 0xE400655C,
+0x2F50BFA4, 0xBFA1E401, 0xE602E506, 0x60634618,
+0x81F2E401, 0x6543BF9F, 0xE40185F2, 0xBFAB6543,
+0x85F26603, 0x6543E401, 0x6603BFB1, 0xE40265F0,
+0x6053756C, 0x80F8BF80, 0xBF82E402, 0x84F8E512,
+0x7090E402, 0x6503BF82, 0x4618E602, 0x81F66063,
+0xBF80E402, 0x85F6E500, 0x6603E402, 0xE500BF8C,
+0xE40285F6, 0xBF926603, 0xE5FEE500, 0xE010655C,
+0xBF61E403, 0xE5130F54, 0xE40EBF63, 0x05FCE010,
+0xBF63E40E, 0xE5007585, 0xBF64E403, 0xE500E640,
+0xBF71E403, 0xE500E640, 0xBF78E403, 0xE5FFE640,
+0xE014655C, 0xBF47E404, 0xE40F0F54, 0xE504BF49,
+0x05FCE014, 0xBF49E40F, 0xE5017584, 0xBF4AE640,
+0xE501E404, 0xBF57E640, 0xE501E404, 0xE404E640,
+0xAF5C7F18, 0x7F184F26, 0x000B4F26, 0x4F220009,
+0xD2427FF0, 0x88016021, 0xD2418B71, 0x26686621,
+0xD2408B6D, 0x26686621, 0xE50F8B69, 0xE401BF1C,
+0xBF1EE501, 0xE586E400, 0xE400655C, 0x2F50BF1E,
+0xBF1BE401, 0xE401E506, 0xBF1C6543, 0xE401E640,
+0xBF296543, 0xE401E640, 0xBF306543, 0x65F0E640,
+0x756CE402, 0xBEFF6053, 0xE40280F4, 0xE512BF01,
+0xE40284F4, 0xBF017090, 0xE6406503, 0xBF02E402,
+0xE640E500, 0xBF0FE402, 0xE640E500, 0xBF16E402,
+0xE5FEE500, 0x6053655C, 0xBEE5E403, 0xE51380F8,
+0xE40EBEE7, 0xE40E84F8, 0xBEE77085, 0xE5006503,
+0xBEE8E640, 0xE500E403, 0xBEF5E640, 0xE500E403,
+0xBEFCE640, 0xE5FFE403, 0x6053655C, 0xBECBE404,
+0xE40F80FC, 0xE504BECD, 0xE40F84FC, 0xBECD7083,
+0xE5016503, 0xBECEE640, 0xE501E404, 0xBEDBE640,
+0xE501E404, 0xE404E640, 0xAEE07F10, 0x7F104F26,
+0x000B4F26, 0x00000009, 0x001E102F, 0x001E1080,
+0x001E1090, 0x001E103F, 0x001E103E, 0x00203A4A,
+0x00203A4C, 0x00203A4E, 0xD21DD11C, 0x66206010,
+0x676C7001, 0x3700C90F, 0xE5008D13, 0x67106210,
+0x7701622C, 0x64232170, 0xD6166010, 0x44084408,
+0x3428C90F, 0x62602100, 0x7201D513, 0x44082620,
+0x000B354C, 0xD10F6053, 0x25586510, 0xE6008D13,
+0xD60DD40B, 0x655C6540, 0x47086753, 0x37584708,
+0x47086540, 0x24507501, 0x367C6040, 0x2400C90F,
+0x72FF6210, 0x000B2120, 0x00006063, 0x00203995,
+0x00203994, 0x00203996, 0x002035BC, 0x7FFC4F22,
+0xE680D1A8, 0x666C6212, 0xD2A72F22, 0x67F36563,
+0x420B7542, 0x7F04E404, 0x000B4F26, 0xE6800009,
+0xD2A1666C, 0xE7006563, 0x422B7540, 0xE6806473,
+0xD29D666C, 0xE7006563, 0x422B7543, 0x2F866473,
+0x2FA62F96, 0x2FC62FB6, 0x2FE62FD6, 0x7FC04F22,
+0xDB97D296, 0x72012F22, 0xD1961F21, 0x66125211,
+0x8B013620, 0x0009A0F9, 0xC9036061, 0x8B018801,
+0x0009A0F3, 0xD290DC8F, 0x64C3420B, 0x6503D18F,
+0x60111F02, 0x8B048801, 0x420BD28D, 0xAFE464C3,
+0x54530009, 0x844CEE84, 0x890130E0, 0x0009A0C3,
+0x6610D188, 0x6023626C, 0x8B718801, 0x6210D186,
+0x89662228, 0xDA86D285, 0xE0036122, 0x64221112,
+0x4018D881, 0xDD83E500, 0x814167A3, 0x77042850,
+0x647266A2, 0x6ED3D580, 0x1F457E04, 0x65521F56,
+0x64E368D2, 0x1F8874F8, 0x684369E2, 0x1F637894,
+0x1F991F74, 0x62826142, 0xD779D978, 0x1F2BD679,
+0x67726292, 0x1F1A6062, 0x2602CB20, 0xD176E600,
+0xE5401F57, 0x1F7D1F2C, 0x76011F1E, 0x3253626D,
+0x51F38BFB, 0x52F555F4, 0x25222A12, 0x55F757F6,
+0x27525AF8, 0x5DF92DA2, 0x2ED251FB, 0xD56B5EFA,
+0x54FC24E2, 0x281257FD, 0xD160D869, 0x25722942,
+0x69126782, 0x1974D866, 0xDD666A12, 0x56FE60A1,
+0x2A01CB01, 0xDA646412, 0xE9012842, 0x4A0B2D42,
+0x52FE2692, 0xD661EE01, 0x22E24E18, 0x72016262,
+0x60B22622, 0xCB01D14F, 0x2B02E202, 0x2120A03F,
+0x8B3C2228, 0xE601D55A, 0x2160E700, 0xE01C2572,
+0xC801004C, 0xD8578B0C, 0x1F8FD257, 0xE6002822,
+0x7601E57D, 0x3253626C, 0x56FF8BFB, 0x2622D253,
+0xE2FE69B2, 0x2B922929, 0x0A4CE01E, 0xE01F65F2,
+0x014C25A0, 0x741057F1, 0xEA062710, 0xDD4CE600,
+0x8446DE4C, 0x2D007601, 0x696C6844, 0x2E8039A3,
+0x8FF67E01, 0xDE487D01, 0x2EA0EA94, 0xE1007E01,
+0x7E0F2E10, 0xD12FE205, 0x64102E20, 0x6023624C,
+0x89088801, 0x55F2D22A, 0x64C3420B, 0xEE01D132,
+0xAF1A4E18, 0x55F221E2, 0x8553D13C, 0x620D6612,
+0x89063262, 0xD63BD43A, 0xE801460B, 0xAF0CD73A,
+0xD91F2782, 0x64C3490B, 0xEE01D127, 0xDA38D437,
+0x4A0B4E18, 0xAF0021E2, 0x7F400009, 0x6EF64F26,
+0x6CF66DF6, 0x6AF66BF6, 0x000B69F6, 0x4F2268F6,
+0x85467FF4, 0x2F01E681, 0x666C8547, 0x854881F1,
+0x81F2D209, 0x67F38542, 0x854381F3, 0x81F4E40C,
+0x65636053, 0x420B81F5, 0x7F0C7540, 0x000B4F26,
+0x00000009, 0x001C3D9C, 0x002023FC, 0x0011779A,
+0x001C36F8, 0x002035B4, 0x002014A6, 0x00203A16,
+0x002014D0, 0x002039A5, 0x002039A4, 0x002039A0,
+0x001C3B9C, 0x001C3704, 0x001C3D98, 0x001C3BB4,
+0x001C5960, 0x001C3500, 0x001C3D30, 0x001C8960,
+0x0020358C, 0x001C3D00, 0x00201610, 0x00117730,
+0x002039A8, 0x001C582C, 0x2000A000, 0x0000A000,
+0x0011778C, 0x00117792, 0x00117788, 0x0020397C,
+0x0020357C, 0x00201534, 0x001E2130, 0x00203DA0,
+0x002018C0, 0x2F962F86, 0x2FB62FA6, 0x2FD62FC6,
+0x4F222FE6, 0xD19B7FEC, 0x2F12E000, 0x6103D49A,
+0x1F4281F2, 0xDD9ADA99, 0xD69A6813, 0xE0014808,
+0x460BDE99, 0x38EC4800, 0x65A21F03, 0x352052A1,
+0xA23E8B01, 0x60510009, 0x8801C903, 0xA2388B01,
+0x52530009, 0x32E0DE91, 0xD9918B10, 0x64A3490B,
+0x4B0BDB90, 0xDE906403, 0xD791D690, 0xEC01D591,
+0x2E02E100, 0x271026C0, 0x2502AFDF, 0xC8018551,
+0xA1578B01, 0x62510009, 0x4200622D, 0x5E53366A,
+0x85E2226D, 0xC903642C, 0x85E36603, 0x6053650D,
+0x40214021, 0x4500C93F, 0x322A6703, 0x6053252D,
+0xC901D17F, 0x60106C03, 0x8801D97F, 0xDB7F8B05,
+0x2120E200, 0xCB0160B2, 0xD17D2B02, 0x88016011,
+0x65A28B0A, 0x8D042448, 0x9B9E6251, 0xA00322B9,
+0x919B2521, 0x2521221B, 0x37B3EB10, 0x2448895E,
+0xD4738B07, 0x22286241, 0x60638903, 0xA05781F8,
+0xD5706473, 0x46084608, 0x85E26273, 0x46006B50,
+0x362C4200, 0x2BB8C910, 0x8F1F6463, 0x26686603,
+0xD2698911, 0x062D6043, 0x4119616D, 0x6B0E6019,
+0x81F820BD, 0x880160C3, 0x646C8F2C, 0x880F6073,
+0xA0278B1B, 0xD2610009, 0x052D6043, 0x4119615D,
+0x670E6019, 0x645C207D, 0x81F8A01C, 0x890F2668,
+0x6043D25B, 0x6B5D052D, 0x60B94B19, 0x201D610E,
+0x60C381F8, 0x8F0D8801, 0x6473645C, 0xEC00A00A,
+0x6043D254, 0x625D052D, 0x60294219, 0x207D670E,
+0x81F8645C, 0x880285F8, 0x85E1890A, 0x8D07C820,
+0xE6DC6203, 0x60232269, 0x81E1A002, 0x644CE4FF,
+0x6210D149, 0x89012228, 0x644CE4FF, 0x654DEBFF,
+0x35B06BBC, 0xDB368B2B, 0x64A34B0B, 0x410BD135,
+0x54036403, 0x85446E03, 0xC948DB40, 0xDC408808,
+0xBEAC8B01, 0x64B3E502, 0x65E34C0B, 0xDB3DEC01,
+0xD13D2DC2, 0x621260B2, 0x72017001, 0x21228805,
+0x2B028F08, 0x666CE680, 0x6563D238, 0x7549E700,
+0x6473420B, 0xA030D436, 0x7FFF0009, 0x85E28000,
+0x20B9EBFC, 0x610381E2, 0x942A85E3, 0x62032049,
+0x450885F8, 0x81E2201B, 0xC90160C3, 0x40084018,
+0x40084008, 0x4000225B, 0x6023220B, 0x85E481E3,
+0x4118E108, 0x81E4201B, 0xE40262A2, 0x20B98521,
+0x67A28121, 0xCB016071, 0x85F82701, 0x89033042,
+0xECE785E2, 0x81E220C9, 0x490BD41E, 0xA03B0009,
+0x7E030009, 0x001C3D30, 0x00203DAC, 0x0020358C,
+0x001E212C, 0x00203470, 0x001C3D00, 0x00117780,
+0x002014A6, 0x00201670, 0x0011770C, 0x002039A4,
+0x002039A5, 0x002039A0, 0x002018C0, 0x001C36F8,
+0x00203A1A, 0x00203DBC, 0x00203BA0, 0x00203C20,
+0x00203CA0, 0x00203D20, 0x00203990, 0x00203584,
+0x002014D0, 0x00203A1C, 0x00203A20, 0x002023FC,
+0x00203DA4, 0x00203DA8, 0x602262F2, 0x40094019,
+0xC90F4009, 0x8B0B880A, 0x60E2DE8C, 0x40094019,
+0xC90F4009, 0x8B038808, 0xCB0160A2, 0x2802A006,
+0x65E2DE87, 0x2E527501, 0x286266A2, 0x52F366F2,
+0x2622AE83, 0xD2838551, 0xDE83C802, 0xA0958B01,
+0x420B0009, 0x4E0B64A3, 0x5E036403, 0x85E46503,
+0x4918E908, 0xD77D209B, 0xE04C81E4, 0xDC7C0B7E,
+0x7B01D97C, 0x61C207B6, 0x71016690, 0x8D062668,
+0xD4792C12, 0x420BD279, 0xA070EB01, 0x62512DB2,
+0x4B18EB0F, 0x22B9E102, 0x32104118, 0x85518B0F,
+0x2029E2FC, 0x60518151, 0xCB0172E0, 0x85E12501,
+0x202994A3, 0x85E481E1, 0xA0522049, 0x675181E4,
+0x4719677D, 0x667E6779, 0x7701276D, 0x6903607C,
+0x88014918, 0x25918F3E, 0x6B12D161, 0x21B27B01,
+0x660D85E3, 0x40216063, 0xC93F4021, 0x6C034600,
+0x262D322A, 0xC8016063, 0xDB5ED15D, 0x967D8901,
+0xE6002C6B, 0x666C67CD, 0x40006063, 0x622D021D,
+0x8D0E3270, 0x60436403, 0xE9FF021D, 0x8B013290,
+0x01C5A007, 0x626C7601, 0x3292E904, 0x646C8BEB,
+0x60434400, 0xD15004BD, 0x0B457401, 0x669D6911,
+0x89073670, 0x602D6211, 0x890388FF, 0xE201DB4B,
+0x2B2021C1, 0xECFC8551, 0x815120C9, 0xCB016051,
+0xDC472501, 0x64A34C0B, 0x51F366F2, 0x85EF2612,
+0x54F2D244, 0x650D420B, 0x0009ADE7, 0xE500DC42,
+0x420B2C52, 0x4E0B64A3, 0x54036403, 0x85446E03,
+0x6703E908, 0x65034918, 0x27998541, 0xDB323790,
+0x8F0BD932, 0x6013610D, 0x8B07C820, 0xC9486053,
+0x8B038808, 0xE501BD4B, 0x0009A005, 0x2128D233,
+0xBD448901, 0x64B3E500, 0x490B65E3, 0xADBCEC01,
+0x85F22DC2, 0x7001EE04, 0x31E7610D, 0x8D0281F2,
+0xADA97A08, 0x7F140009, 0x6EF64F26, 0x6CF66DF6,
+0x6AF66BF6, 0x000B69F6, 0xF7FF68F6, 0x2FE68000,
+0xD2234F22, 0x60E36E22, 0x8D02C840, 0xBBE522E2,
+0xE2400009, 0x2E284218, 0xBBF08901, 0x60E30009,
+0x8905C810, 0xD21CD41B, 0x0009420B, 0x0009BBEF,
+0xC80560E3, 0xBD6D8901, 0x60E30009, 0x8902C802,
+0xABEC4F26, 0x4F266EF6, 0x6EF6000B, 0x001C3D3C,
+0x00117760, 0x002014A6, 0x00201670, 0x0020351C,
+0x00203DC0, 0x00203990, 0x00203584, 0x002014D0,
+0x002039FC, 0x00203A04, 0x002039F8, 0x002039FA,
+0x00201534, 0x002018D0, 0x00203A1C, 0x00008000,
+0x001C3510, 0x00203DB4, 0x002018C0, 0x89014F22,
+0x611B600B, 0x611BB00A, 0x000B4F26, 0x600B600B,
+0x611BA004, 0x8DF12107, 0x8BF84011, 0x620D2F26,
+0x8F3E3020, 0x40180019, 0x8B0B3016, 0x31043104,
+0x31043104, 0x31043104, 0x31043104, 0x412462F6,
+0x601C000B, 0x41296219, 0x20084018, 0x31048926,
+0x31043104, 0x31043104, 0x31043104, 0x31043104,
+0x31043104, 0x31043104, 0x31043104, 0x61193104,
+0x3204221D, 0x32043204, 0x32043204, 0x32043204,
+0x32043204, 0x32043204, 0x32043204, 0x32043204,
+0x212D3204, 0x601962F6, 0x4024000B, 0x000BE000,
+0x621362F6, 0x41294228, 0x31044224, 0x31044224,
+0x31044224, 0x31044224, 0x31044224, 0x31044224,
+0x31044224, 0x31044224, 0x31044224, 0x31044224,
+0x31044224, 0x31044224, 0x31044224, 0x31044224,
+0x31044224, 0x31044224, 0x602D4224, 0x62F6000B,
+0x080A0C0E, 0x00020406, 0x1A1C1E20, 0x12141618,
+0x2E303234, 0x26282A2C, 0x3A3C3E40, 0x6C625648,
+0x41112F26, 0xE2208F18, 0x890B3123, 0x321CD204,
+0xD1026220, 0x412B312C, 0x00090009, 0x0020349A,
+0x00203450, 0x000BE000, 0x400062F6, 0x40004000,
+0x40004000, 0x40004000, 0x62F6000B, 0x40004000,
+0x40004000, 0x40004000, 0x40184000, 0x62F6000B,
+0x40004000, 0x40004000, 0x40004000, 0x40284000,
+0x62F6000B, 0x40004000, 0x40184000, 0x000B4028,
+0xC90F62F6, 0x40054005, 0x40054005, 0x62F6000B,
+0x4005C907, 0x40054005, 0x62F6000B, 0x4005C903,
+0x000B4005, 0xC90162F6, 0x000B4005, 0x000062F6,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x42707372,
+0x3D206675, 0x554E203D, 0x202C4C4C, 0x6E49677A,
+0x4E497274, 0x6D754E51, 0x0000003D, 0x61766E49,
+0x2064696C, 0x72657375, 0x20726F20, 0x2079656B,
+0x00214449, 0x6E6B6E55, 0x206E776F, 0x6D6D6F63,
+0x3D646E61, 0x00000000, 0x203A3051, 0x00000020,
+0x203A3151, 0x00000020, 0x203A3251, 0x00000020,
+0x203A3351, 0x00000020, 0x203A3451, 0x00000020,
+0x2B434741, 0x73696F4E, 0x61432065, 0x7262696C,
+0x6F697461, 0x6166206E, 0x6F206C69, 0x6974206E,
+0x0D0A656D, 0x00000000, 0x00000072, 0x00205220,
+0x62735576, 0x7473725F, 0x00000A0D, 0x62735576,
+0x7375735F, 0x646E6570, 0x00000A0D, 0x62735576,
+0x7365725F, 0x000A0D6D, 0x00000044, 0x44387570,
+0x72637365, 0x6F747069, 0x3D584572, 0x00000000,
+0x00000047, 0x72746E49, 0x6D652051, 0x2C797470,
+0x49677A20, 0x4972746E, 0x754E514E, 0x00003D6D,
+0x654C7245, 0x0000006E, 0x20746F4E, 0x756F6E65,
+0x49206867, 0x4220514E, 0x0A0D6675, 0x00000000,
+0x000000FF, 0x00020001, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x010E010D, 0x00020003, 0x01090108, 0x0002010A,
+0x02000003, 0x02020201, 0x02040203, 0x02060205,
+0x02020200, 0x02040203, 0x020C020B, 0x020E020D,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x000000FF, 0x00020001, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x010E010D, 0x00020003, 0x01090108, 0x0002010A,
+0x00030003, 0x02020201, 0x02040203, 0x02060205,
+0x02020200, 0x02040203, 0x020C020B, 0x020E020D,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x010E010D, 0x00FF010F, 0x01090108, 0x010B010A,
+0x0200010F, 0x02020201, 0x02040203, 0x02060205,
+0x02020200, 0x02040203, 0x020C020B, 0x020E020D,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x010E010D, 0x00FF010F, 0x01090108, 0x010B010A,
+0x010F010F, 0x02020201, 0x02040203, 0x02060205,
+0x02020200, 0x02040203, 0x020C020B, 0x020E020D,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00205220, 0x00000046, 0x00000059, 0x73204142,
+0x003D7165, 0x49544120, 0x0000204D, 0x00000000,
+0x00000000, 0x002E0209, 0x80000101, 0x000409FA,
+0x00FF0400, 0x05070000, 0x02000201, 0x82050700,
+0x00020002, 0x03830507, 0x07010040, 0x40030405,
+0x02090100, 0x0101002E, 0x09FA8000, 0x04000004,
+0x000000FF, 0x02010507, 0x07000040, 0x40028205,
+0x05070000, 0x00400383, 0x04050701, 0x00004002,
+0x00000000, 0x00000000, 0x07090000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x6621D2A8, 0x2008606D, 0xA1B18B01, 0x88100009,
+0x88118922, 0x88128920, 0x8813891E, 0x8821891C,
+0x8822891A, 0x883A8918, 0x883B8916, 0xE6448914,
+0x30604608, 0xE6488910, 0x30604608, 0xE658890C,
+0x30604608, 0x963D8908, 0x89053060, 0x3060963B,
+0x96398902, 0x8B013060, 0xE010000B, 0x8B018820,
+0xE020000B, 0x892B8837, 0x89298832, 0x89278835,
+0x89258836, 0x89238830, 0x89218838, 0x891F8839,
+0x891D8834, 0x891B8833, 0x4608E64C, 0x89173060,
+0x3060961B, 0x96198914, 0x89113060, 0x30609617,
+0x9615890E, 0x890B3060, 0x30609613, 0x96118908,
+0x89053060, 0x3060960F, 0x960D8902, 0x8B0C3060,
+0xE030000B, 0x05100165, 0x02300A10, 0x04300330,
+0x06300530, 0x0B300A30, 0x88400C30, 0xA1428B01,
+0x88410009, 0xA13E8B01, 0x88430009, 0xA13A8B01,
+0x88480009, 0xA1368B01, 0x884A0009, 0xA1328B01,
+0x884B0009, 0xA12E8B01, 0x884C0009, 0xA12A8B01,
+0xE6800009, 0x3060666C, 0xA1248B01, 0xE6810009,
+0x3060666C, 0xA11E8B01, 0xE6820009, 0x3060666C,
+0xA1188B01, 0xE6830009, 0x3060666C, 0xA1128B01,
+0xE6840009, 0x3060666C, 0xA10C8B01, 0xE6850009,
+0x3060666C, 0xA1068B01, 0xE6860009, 0x3060666C,
+0xA1008B01, 0xE6870009, 0x3060666C, 0xA0FA8B01,
+0xE6880009, 0x3060666C, 0xA0F48B01, 0xE6890009,
+0x3060666C, 0xA0EE8B01, 0xE68A0009, 0x3060666C,
+0xA0E88B01, 0xE68B0009, 0x3060666C, 0xA0E28B01,
+0xE68C0009, 0x3060666C, 0xA0DC8B01, 0xE68D0009,
+0x3060666C, 0xA0D68B01, 0xE68E0009, 0x3060666C,
+0xA0D08B01, 0xE68F0009, 0x3060666C, 0xA0CA8B01,
+0xE6900009, 0x3060666C, 0xA0C48B01, 0xE6910009,
+0x3060666C, 0xA0BE8B01, 0xE6F80009, 0x3060666C,
+0xA0B88B01, 0xE6F90009, 0x3060666C, 0xA0B28B01,
+0xE6FA0009, 0x3060666C, 0xA0AC8B01, 0xE6FB0009,
+0x3060666C, 0xA0A68B01, 0xE6FC0009, 0x3060666C,
+0xA0A08B01, 0xE6FD0009, 0x3060666C, 0xA09A8B01,
+0xE6FE0009, 0x3060666C, 0xA0948B01, 0xE6FF0009,
+0x3060666C, 0xA08E8B01, 0xE6D00009, 0x3060666C,
+0xA0888B01, 0xE6D10009, 0x3060666C, 0xA0828B01,
+0xE6D20009, 0x3060666C, 0xA07C8B01, 0xE6D30009,
+0x3060666C, 0xE6D48977, 0x3060666C, 0xE6D58973,
+0x3060666C, 0xE6D6896F, 0x3060666C, 0xE6D7896B,
+0x3060666C, 0xE6D88967, 0x3060666C, 0xA0038963,
+0x00000009, 0x00114000, 0x666CE6D9, 0x895A3060,
+0x666CE6DA, 0x89563060, 0x666CE6DB, 0x89523060,
+0x666CE6DC, 0x894E3060, 0x666CE6DD, 0x894A3060,
+0x666CE6F0, 0x89463060, 0x666CE6F1, 0x89423060,
+0x666CE6F2, 0x893E3060, 0x666CE6F3, 0x893A3060,
+0x666CE6F4, 0x89363060, 0x666CE6F5, 0x89323060,
+0x666CE6F6, 0x892E3060, 0x666CE6F7, 0x892A3060,
+0x4608E650, 0x89263060, 0x3060969A, 0x96988923,
+0x89203060, 0x30609696, 0x9694891D, 0x891A3060,
+0x30609692, 0x96908917, 0x89143060, 0x3060968E,
+0x968C8911, 0x890E3060, 0x3060968A, 0x9688890B,
+0x89083060, 0x30609686, 0x96848905, 0x89023060,
+0x30609682, 0x000B8B01, 0xE0FFE040, 0x600C000B,
+0xE000000B, 0x6243D157, 0xE4028512, 0x662D670D,
+0xE500A00E, 0x6053655D, 0x305C4000, 0x4008D152,
+0x622D021D, 0x8B023260, 0xA0047108, 0x7501041C,
+0x3273625D, 0x60438BEE, 0xC90A000B, 0x674C76FE,
+0x025C606C, 0x3723622C, 0x20088906, 0x70FF8902,
+0x6603AFF6, 0xE000000B, 0x0009000B, 0x4F124F22,
+0x326052F2, 0x34508910, 0x3470890E, 0x3750890D,
+0x3268890A, 0x04273458, 0x60733758, 0x440BD43B,
+0x306C011A, 0x6203A001, 0x4F166263, 0x000B4F26,
+0x2FE66023, 0x4F124F22, 0x6E434F02, 0x614C54F4,
+0x2F164118, 0x666C677C, 0x64EC655C, 0x46184718,
+0xBFD34518, 0x65034418, 0x60537F04, 0xC980E702,
+0x6E034718, 0x37ED4728, 0x62594519, 0x010A652E,
+0x312C225D, 0x4F06601C, 0x4F264F16, 0x6EF6000B,
+0x03400240, 0x05400440, 0x07400640, 0x09400840,
+0x11400B40, 0x0A401240, 0x4F220A50, 0x614C8451,
+0x3127620C, 0xA00C8901, 0x8452E400, 0x3127620C,
+0xA0068901, 0x8453E401, 0x3127620C, 0xE4038D01,
+0x6263E402, 0x60437201, 0x677C072C, 0x62532F76,
+0x072C7201, 0x055C066C, 0x666C677C, 0xBFA8655C,
+0x7F046413, 0x000B4F26, 0x605C600C, 0x8F068801,
+0x606C6243, 0x8B018801, 0x720AA001, 0x000B72F6,
+0x00006023, 0x00114000, 0x00114008, 0x00203374,
+0xE040D690, 0x056E614C, 0x9274D78F, 0x352C357C,
+0xE400E718, 0x626C6650, 0x89043120, 0x624C7401,
+0x8FF73273, 0x000B7501, 0xE2FF6043, 0x622C644C,
+0x890D3420, 0x8801605C, 0x965D8B03, 0xA005346C,
+0x62436243, 0x324C4208, 0x326C9657, 0x6023000B,
+0x6043000B, 0x2F962F86, 0x2FB62FA6, 0x2FD62FC6,
+0x4F222FE6, 0x92497FF4, 0x6B533526, 0x00296943,
+0xE13FCA01, 0x6E03EAFF, 0x6AAC2F10, 0x6D43EC00,
+0x62D0E808, 0x34A0642C, 0xBFCE8939, 0x3B0065E3,
+0x6CCC8F0A, 0x420062C3, 0x362C6693, 0x1FC18461,
+0x4109610C, 0x2F10A02D, 0x891C2CC8, 0x65E364D0,
+0x644CBFBB, 0x89163B02, 0x70FF60C3, 0x049C4000,
+0x644C65E3, 0x1F02BFB1, 0x8D1A30B2, 0x56F21FC1,
+0x356C6593, 0xC9038451, 0x89122008, 0x660C8451,
+0xA00E4609, 0x7C012F60, 0x328362CC, 0x8FC87D02,
+0xA0061F21, 0x06250009, 0x12C008FC, 0x62CC09B4,
+0x50F11F21, 0x8B128808, 0x7CFF6CCC, 0x60C34C00,
+0x65E3049C, 0x644CBF89, 0x8B083B06, 0x849139CC,
+0x2008C903, 0x84918903, 0x4209620C, 0x60F02F20,
+0x4F267F0C, 0x6DF66EF6, 0x6BF66CF6, 0x69F66AF6,
+0x68F6000B, 0x2F962F86, 0x2FB62FA6, 0x2FD62FC6,
+0x4F222FE6, 0x92727FFC, 0x3426E100, 0x6B436953,
+0x2F12666C, 0xCA010029, 0x8D032668, 0xE2F06E03,
+0x2F22622C, 0x6AACEAFF, 0x6C93ED00, 0x66C0E808,
+0x34A0646C, 0xBF508913, 0x3B0065E3, 0x6DDC8B0A,
+0x39DC4D00, 0xC9038491, 0x8B082008, 0xCB0F60F2,
+0x2F02A005, 0x62DC7D01, 0x8FE83283, 0x60F27C02,
+0x4F267F04, 0x6DF66EF6, 0x6BF66CF6, 0x69F66AF6,
+0x68F6000B, 0x2F962F86, 0x2FB62FA6, 0x2FD62FC6,
+0x4F222FE6, 0x4F024F12, 0x3F3C9332, 0x4308E35B,
+0x605333FC, 0x80341351, 0xE7606063, 0x80381362,
+0x4708E012, 0xE03F8136, 0xD11237FC, 0x27008138,
+0x80788074, 0xE9166053, 0x60638012, 0x21414918,
+0x6B938013, 0xEDFF6AB4, 0x6AAC61B0, 0x6C1C4A18,
+0x68C82CAB, 0x6DDD688D, 0x234238D0, 0x8B131398,
+0xD207D406, 0x0009420B, 0x432BD306, 0x09B40009,
+0x0000FE10, 0x001142D8, 0x000DDD00, 0x001160B0,
+0x002018C0, 0x00115E88, 0x342292E3, 0x8F02E100,
+0xA1616593, 0x92DD0009, 0x352CE7FF, 0xEE04677C,
+0x622C6250, 0x89043270, 0x621C7101, 0x8FF732E3,
+0xE8FC7501, 0x3488688C, 0x9ACBE064, 0x40086893,
+0x0F4438AC, 0x661C6583, 0x644CBE18, 0x64E36E0C,
+0x65E37401, 0x45086643, 0x35EC4608, 0x4508364C,
+0x45004608, 0x369C4600, 0x61A39AB5, 0xE0656763,
+0x400837AC, 0x62637114, 0x321C0F76, 0x94AB7004,
+0x61430F26, 0x359C6263, 0x7004324C, 0x0F267114,
+0x7004361C, 0x0F666753, 0x700437AC, 0x7A140F76,
+0x37AC6753, 0x66537004, 0x364C0F76, 0x74147004,
+0x354C0F66, 0x0F567004, 0x395C958F, 0xED006A93,
+0x6BD3E956, 0xEC054908, 0x4008E065, 0x60B302FE,
+0x644C042C, 0x60E32F46, 0xE06A07AC, 0x01FE4008,
+0x061C60B3, 0x058C60E3, 0x4008E065, 0x677C01FC,
+0x655C666C, 0x641CBDED, 0x7F046403, 0x60D36DDC,
+0xE0660F44, 0x04FE4008, 0x054C60B3, 0x2F56655C,
+0x07AC60E3, 0x4008E06B, 0x60B301FE, 0x60E3061C,
+0xE065058C, 0x01FC4008, 0x666C677C, 0xBDD0655C,
+0x6403641C, 0x65F37F04, 0x60D37510, 0xE0650544,
+0x07FE4008, 0x057C60C3, 0x2F56655C, 0x07AC60E3,
+0x4008E06A, 0x60C301FE, 0x60E3061C, 0xE065058C,
+0x01FC4008, 0x666C677C, 0xBDB2655C, 0x6403641C,
+0x61F37F04, 0x60D37120, 0xE0660144, 0x02FE4008,
+0x052C60C3, 0x2F56655C, 0x07AC60E3, 0x4008E06B,
+0x60C301FE, 0x60E3061C, 0xE065058C, 0x01FC4008,
+0x666C677C, 0xBD94655C, 0x6503641C, 0x64F37F04,
+0x60D3349C, 0xE0670454, 0x07FE4008, 0x057C60B3,
+0x2F56655C, 0x07AC60E3, 0x4008E06C, 0xA00501FE,
+0x0BB860B3, 0x03C2013E, 0x013F0462, 0x60E3061C,
+0xE065058C, 0x01FC4008, 0x666C677C, 0xBD70655C,
+0x6203641C, 0xE1B87F04, 0x64F3611C, 0x60D3341C,
+0xE0680424, 0x05FE4008, 0x075C60B3, 0x2F76677C,
+0x07AC60E3, 0x4008E06D, 0x60B301FE, 0x60E3061C,
+0xE065058C, 0x02FC4008, 0x666C677C, 0xBD50655C,
+0x6703642C, 0xE2C07F04, 0x66F3622C, 0x60D3362C,
+0xE0670674, 0x07FE4008, 0x027C60C3, 0x2F26622C,
+0x07AC60E3, 0x4008E06C, 0x60C302FE, 0x60E3062C,
+0xE065058C, 0x02FC4008, 0x666C677C, 0xBD30655C,
+0x6203642C, 0xE7C87F04, 0x66F3677C, 0x60D3367C,
+0xE0680624, 0x06FE4008, 0x026C60C3, 0x2F26622C,
+0x07AC60E3, 0x4008E06D, 0x60C302FE, 0x60E3062C,
+0xE065058C, 0x02FC4008, 0x666C677C, 0xBD10655C,
+0x6103642C, 0x66937F04, 0x62F37608, 0x60D3326C,
+0x02147D01, 0xE60562DC, 0x7C013263, 0x7B018D02,
+0x0009AEFA, 0x0009A17B, 0xE7FF9BD5, 0x677C35BC,
+0x6250EE08, 0x3270622C, 0x71018904, 0x32E3621C,
+0x75018FF7, 0xDDD89CC8, 0x3D4534C8, 0x4008E064,
+0x4E090E0A, 0x0FE46593, 0x702435BC, 0x64EC661C,
+0x0F56BCB4, 0x64E36E0C, 0x65E37401, 0x45086243,
+0x35EC4208, 0x4508324C, 0x45004208, 0x329C4200,
+0x61B37B0C, 0x38BC6823, 0x7114E06E, 0x40086B23,
+0x91A23B1C, 0x70040F86, 0x68236413, 0x0FB6359C,
+0x7004381C, 0x0F867414, 0x7004342C, 0x67539896,
+0x0F466253, 0x7004378C, 0x6B537814, 0x7114321C,
+0x3B8C0F76, 0x351C7004, 0x0FB69789, 0x397C7004,
+0x70040F26, 0xED006893, 0x0F56EC05, 0x6AD3E956,
+0xE06E4908, 0x02FE4008, 0x012C60A3, 0x2F16611C,
+0x078C60E3, 0x4008E073, 0x60A304FE, 0xE06E064C,
+0x0BFE4008, 0x05BC60E3, 0x4008E065, 0x677C01FC,
+0x655C666C, 0x641CBC85, 0x7F046403, 0x60D36DDC,
+0xE06F0F44, 0x04FE4008, 0x054C60A3, 0x2F56655C,
+0x078C60E3, 0x4008E074, 0x60A30BFE, 0xE06E06BC,
+0x0BFE4008, 0x05BC60E3, 0x4008E065, 0x677C01FC,
+0x655C666C, 0x641CBC65, 0x7F046403, 0x751065F3,
+0x054460D3, 0x4008E06E, 0x60C307FE, 0x655C057C,
+0x60E32F56, 0xE073078C, 0x0BFE4008, 0x06BC60C3,
+0x4008E06E, 0x60E301FE, 0xE065051C, 0x0BFC4008,
+0x666C677C, 0xBC44655C, 0x610364BC, 0x6BF37F04,
+0x60D37B20, 0xE06F0B14, 0x01FE4008, 0x041C60C3,
+0x2F46644C, 0x078C60E3, 0x4008E074, 0x60C301FE,
+0xE06E061C, 0x0BFE4008, 0x05BC60E3, 0x4008E065,
+0xA00501FC, 0x0136677C, 0x028212C0, 0x01370142,
+0x655C666C, 0x641CBC1D, 0x7F046203, 0x349C64F3,
+0x042460D3, 0x4008E070, 0x60A304FE, 0x655C054C,
+0x60E32F56, 0xE075078C, 0x0BFE4008, 0x06BC60A3,
+0x4008E06E, 0x60E301FE, 0xE065051C, 0x0BFC4008,
+0x666C677C, 0xBBFC655C, 0x610364BC, 0xEBB87F04,
+0x65F36BBC, 0x60D335BC, 0xE0710514, 0x07FE4008,
+0x047C60A3, 0x2F46644C, 0x078C60E3, 0x4008E076,
+0x60A30BFE, 0xE06E06BC, 0x01FE4008, 0x051C60E3,
+0x4008E065, 0x677C0BFC, 0x655C666C, 0x64BCBBD9,
+0x7F046103, 0x622CE2C0, 0x3B2C6BF3, 0x0B1460D3,
+0x4008E070, 0x60C302FE, 0x677C072C, 0x60E32F76,
+0xE075078C, 0x02FE4008, 0x062C60C3, 0x4008E06E,
+0x60E302FE, 0xE065052C, 0x02FC4008, 0x666C677C,
+0xBBB6655C, 0x6703642C, 0xEBC87F04, 0x66F36BBC,
+0x60D336BC, 0xE0710674, 0x06FE4008, 0x026C60C3,
+0x2F26622C, 0x078C60E3, 0x4008E076, 0x60C302FE,
+0xE06E062C, 0x02FE4008, 0x052C60E3, 0x4008E065,
+0x677C02FC, 0x655C666C, 0x642CBB93, 0x7F046103,
+0x72086293, 0x362C66F3, 0x7D0160D3, 0x62DC0614,
+0x3263E605, 0x8D027C01, 0xAEE27A01, 0x6EF30009,
+0xE2B068F3, 0x6AF3E05A, 0x389C7E18, 0x69F3622C,
+0x7A084008, 0x67F36DF3, 0x392C61F3, 0x6CA30FE6,
+0x77207D10, 0xE4007128, 0xEB0565F3, 0x604C6654,
+0x66D4626C, 0x2E604221, 0x048C6674, 0x626C2C20,
+0x09444221, 0x21207001, 0x32B3620C, 0x71016403,
+0x8FEB7E01, 0xE05A7C01, 0x6EF34008, 0x7E300BFE,
+0xEC19ED00, 0x66B365A3, 0xBB7E64DC, 0x62DC7D01,
+0x2E0032C3, 0x7E018FF6, 0x666CE6B0, 0x6BF36EF3,
+0x7B283E6C, 0xEC4CA010, 0xCCCCCCCD, 0x64D36DDC,
+0x644C74F4, 0xBB6865B3, 0x67F366E3, 0x77306503,
+0x075460D3, 0x62DC7D01, 0x8BEF32C3, 0x7B306BF3,
+0x61B367B3, 0xED8064B3, 0x71027701, 0x6DDC7403,
+0xDC37E500, 0x605CDE37, 0x091C084C, 0x0A7C668C,
+0x699C4628, 0x49284618, 0x05BC6AAC, 0x4A18269B,
+0x70046803, 0x655C26AB, 0x620C38CC, 0x38EC265B,
+0x286232D3, 0x65038FE7, 0x644CE4B8, 0x3C4C6CF3,
+0x6EF37408, 0xE2B0E658, 0x3E4C6AF3, 0x74086BF3,
+0x460861F3, 0x622C68F3, 0x7A0869F3, 0x314C7B18,
+0x386C64F3, 0x6DA3392C, 0x742867B3, 0x66C4E500,
+0x626C605C, 0x422166E4, 0x66142760, 0x2D20058C,
+0x4221626C, 0x70010954, 0x620C2420, 0x3263E605,
+0x74016503, 0x8FEA7701, 0xE05E7D01, 0x02FD4008,
+0x6D2DE9D0, 0x699C7D07, 0xEE00A00B, 0x66B365A3,
+0x64ECBAFB, 0x620367F3, 0x60EC379C, 0x70010724,
+0x62EC6E03, 0x8BF132D3, 0x4008E05F, 0xEAB008FD,
+0x6DF36AAC, 0x6BF36C8D, 0x7C0D3DAC, 0x7B28A012,
+0x0000A280, 0x001BC000, 0x64E36EEC, 0x644C74F4,
+0xBADA65B3, 0x62F366D3, 0x329C6103, 0x7E0160E3,
+0x62EC0214, 0x8BEF32C3, 0x3D9C6DF3, 0x67D36ED3,
+0xEC8061D3, 0x77027E01, 0x6CCC7103, 0xDBB9E400,
+0x604CDAB9, 0x067C041C, 0x08EC654C, 0x666C4528,
+0x46284518, 0x09DC688C, 0x4818256B, 0x70046603,
+0x699C258B, 0x620C36BC, 0x36AC259B, 0x265232C3,
+0x64038FE7, 0x4008E064, 0x70E007FC, 0x706C0CFC,
+0x0F8668CC, 0x0DFC7098, 0x6ADC706C, 0x708C0FA6,
+0x9BBF0EFE, 0xE2543EB2, 0x697CE100, 0x42088F02,
+0x0009A163, 0x4008E063, 0x6EF305FE, 0x3E2C96B3,
+0x64E3356C, 0xEDFFE703, 0x32D06250, 0x622C8D07,
+0x681C7101, 0x24203873, 0x8FF57505, 0xE0647401,
+0x0AFC4008, 0x64AC65E3, 0x661CBA18, 0xE063670C,
+0x62734008, 0x42080BFE, 0x7701327C, 0x3A2C6AB3,
+0x48086873, 0x948F6EA3, 0x3E4C387C, 0x3B8C74FF,
+0x38283A4C, 0xEC003B4C, 0x6083DD88, 0x655C05EC,
+0xE0652F56, 0x67B04008, 0x65A066E4, 0x677C01FC,
+0x655C666C, 0x641CBA1D, 0x7C017F04, 0xE40462CC,
+0x2D003243, 0x7D018FE9, 0xE063E554, 0x67F34508,
+0x375C4008, 0x966805FE, 0x356CEDFF, 0x6DDC6473,
+0xEE04E100, 0x666C6650, 0x890636D0, 0x621C7101,
+0x246032E3, 0x8FF57505, 0xE0647401, 0x02FC4008,
+0x642C6573, 0x661CB9CA, 0x6E23620C, 0xE0634E08,
+0x72013E2C, 0x0BFE4008, 0x47086723, 0x6AB3372C,
+0x68733AEC, 0x6EA338E8, 0x3E1C9140, 0x3B7C71FF,
+0x3B1C3A1C, 0x0F96704C, 0xEC00E904, 0x6083DD60,
+0x644C04EC, 0xE0652F46, 0x67B04008, 0x65A066E4,
+0x677C01FC, 0x655C666C, 0x641CB9CB, 0x7C017F04,
+0x329362CC, 0x8FEA2D00, 0xE0767D01, 0x09FE4008,
+0x70B4E454, 0x67F34408, 0x374C05FE, 0xEDFF9617,
+0x356C6473, 0xE1006DDC, 0x6650EE04, 0x36D0666C,
+0x71018906, 0x32E3621C, 0x75092460, 0x74018FF5,
+0xE064A006, 0x05BA0BB8, 0x05C905BB, 0x05DD05CA,
+0x65734008, 0x661C07FC, 0x647CB970, 0x6623620C,
+0x4608E063, 0x46004008, 0x362C0BFE, 0x68237201,
+0x3A6C6AB3, 0x48004808, 0x91676EA3, 0x3E1C382C,
+0x3B8C71FF, 0x38683A1C, 0xEC003B1C, 0x6083DD35,
+0x644C04EC, 0xE0652F46, 0x67B04008, 0x65A066E4,
+0x677C01FC, 0x655C666C, 0x641CB973, 0x7C017F04,
+0xE50862CC, 0x2D003253, 0x7D018FE9, 0x4008E063,
+0x05FEE654, 0x64F34608, 0xECFF9741, 0x357C346C,
+0xEE006CCC, 0x6250ED04, 0x32C0622C, 0x7E018906,
+0x38D368EC, 0x75092420, 0x74018FF5, 0x4008E077,
+0x700405FE, 0x649306FE, 0xEA54B9A7, 0x65F34A08,
+0x640C35AC, 0x66ECB91A, 0x6613610C, 0x4608E063,
+0x46004008, 0x361C0BFE, 0x68137101, 0x3A6C6AB3,
+0x48004808, 0x92136EA3, 0x3E2C381C, 0x3B8C72FF,
+0x38683A2C, 0xEC003B2C, 0xE077DD0B, 0x05FE4008,
+0x06FE7004, 0x6493B981, 0x0009A010, 0x060105DE,
+0x00000602, 0x0000B280, 0x001BC000, 0x001142E4,
+0x001142E8, 0x001142ED, 0x001142F5, 0x60836403,
+0x677C07EC, 0x67B02F76, 0x65A066E4, 0x666C677C,
+0xB906655C, 0x7F04644C, 0x61CC7C01, 0x3123E208,
+0x8FD22D00, 0xA0FC7D01, 0xE0630009, 0x05FE4008,
+0x96D067F3, 0x356C372C, 0xEEFF6473, 0x32E06250,
+0x622C8D08, 0x681C7101, 0x3863E608, 0x75052420,
+0x74018FF4, 0x4008E064, 0x657302FC, 0xB8B5642C,
+0x650C661C, 0x4008E063, 0x0BFE6253, 0x325C4208,
+0x6AB37501, 0x68533A2C, 0x6EA34808, 0x385C94AC,
+0x74FF3E4C, 0x3A4C3B8C, 0x3B4C3828, 0xDD96EC00,
+0x06EC6083, 0x2F66666C, 0x4008E065, 0x66E467B0,
+0x01FC65A0, 0x666C677C, 0xB8BA655C, 0x7F04641C,
+0x62CC7C01, 0x3243E404, 0x8FE92D00, 0xE5547D01,
+0x4508E063, 0x400867F3, 0x05FE375C, 0xEEFF9685,
+0x6473356C, 0xE1006EEC, 0x666C6650, 0x890736E0,
+0x621C7101, 0x3283E808, 0x75092460, 0x74018FF4,
+0x4008E064, 0x65730AFC, 0xB86764AC, 0x620C661C,
+0xE0636623, 0x40084608, 0x0BFE4600, 0x7201362C,
+0x6AB36823, 0x48083A6C, 0x6EA34800, 0x382C915E,
+0x71FF3E1C, 0x3A1C3B8C, 0x3B1C3868, 0xDD6FEC00,
+0x04EC6083, 0x2F46644C, 0x4008E065, 0x66E467B0,
+0x01FC65A0, 0x666C677C, 0xB86A655C, 0x7F04641C,
+0x62CC7C01, 0x3253E508, 0x8FE92D00, 0xE0637D01,
+0xE6544008, 0x460805FE, 0x973864F3, 0x346CECFF,
+0x6CCC357C, 0xED08EE00, 0x666C6650, 0x890636C0,
+0x62EC7E01, 0x246032D3, 0x8FF57509, 0xE0777401,
+0x05FE4008, 0x06FE7004, 0xB89E6493, 0x4808E854,
+0x358C65F3, 0xB811640C, 0x610C66EC, 0xE0636613,
+0x40084608, 0x0BFE4600, 0x7101361C, 0x6AB36813,
+0x48083A6C, 0x6EA34800, 0x381C920A, 0x72FF3E2C,
+0xA0063B8C, 0x05023A2C, 0x052A0503, 0x0572052B,
+0x38680573, 0xEC003B2C, 0xE077DD41, 0x05FE4008,
+0x06FE7004, 0x6493B871, 0x60836403, 0x677C07EC,
+0x67B02F76, 0x65A066E4, 0x666C677C, 0xB808655C,
+0x7F04644C, 0x61CC7C01, 0x3123E208, 0x8FE42D00,
+0xD3347D01, 0x0009430B, 0xE079620C, 0x0F244008,
+0x88306023, 0xA24D8B01, 0x88400009, 0xA2498B01,
+0x22280009, 0xA2458B01, 0xE5FF0009, 0x655CD42A,
+0xE03AE601, 0x8F043250, 0xE0790464, 0x4008E210,
+0xE05B0F24, 0x05FE4008, 0x3566963B, 0xA1498B01,
+0x60230009, 0x640CCB01, 0x6E23B842, 0xE118660C,
+0x890F3613, 0x4008E063, 0x04FE4608, 0x97294608,
+0x460070E0, 0x05FE347C, 0x346CB85C, 0xE0606203,
+0x0F244008, 0xCB0260E3, 0x640CB82A, 0xE118660C,
+0x890F3613, 0x4008E063, 0x04FE4608, 0x91114608,
+0x460070E0, 0x05FE341C, 0x346CB844, 0xE0616203,
+0x0F244008, 0xCB0560E3, 0x640CB812, 0xA00D660C,
+0x09B4E07A, 0x0000064D, 0x001142FD, 0x00114301,
+0x00114309, 0x00114400, 0x001142D8, 0x4008E118,
+0x8F043613, 0xE0610F64, 0xA0104008, 0xE07A0DFC,
+0x06FC4008, 0x626C70A4, 0x04FE4208, 0x97B44208,
+0x420070E0, 0x05FE347C, 0x342CB814, 0xE0796D03,
+0x00FC4008, 0xCB07DB8E, 0x430BD38E, 0x610C640C,
+0x4008E07A, 0x709C0F14, 0xE61802FC, 0x8D1C3163,
+0xE05D682C, 0x01FC4008, 0x09FC70FC, 0x04FE70FC,
+0xD385661C, 0x659C430B, 0xE07A6503, 0x01FC4008,
+0x611C70A4, 0x04FE4108, 0x97864108, 0x347C4100,
+0x430BD37E, 0xA003341C, 0xE0616C03, 0x0CFC4008,
+0xE500D67B, 0x640D8562, 0x4008E05B, 0x0AFEA036,
+0x6053655C, 0x305C4000, 0x4008D676, 0x622D026D,
+0x8F2A32A0, 0xD3746E03, 0x64AD430B, 0x2228620D,
+0xD6728927, 0x066C60E3, 0x4008E060, 0x460002FC,
+0x3E676E2C, 0x62638B00, 0x4008E060, 0x0F243867,
+0x62638D03, 0x4008E061, 0xE06102FC, 0x400861DC,
+0x0F243167, 0x8F01682C, 0x626362D3, 0x346764CC,
+0x6D238D01, 0xA00466C3, 0x75016C63, 0x3243625C,
+0xE0608BC6, 0x07FC4008, 0x617CE400, 0xE904D55C,
+0x666C6650, 0x8B013617, 0x6673677C, 0x624C7401,
+0x25603293, 0x75018FF4, 0xE03AD656, 0xE400056C,
+0x8D012558, 0xE2026243, 0x4008E061, 0x67830EFC,
+0x3E283828, 0x9119E500, 0x6053655C, 0x3A1002BC,
+0x622C8D0B, 0x3A609613, 0x32778907, 0xE0618B02,
+0x02FC4008, 0xA01D6053, 0x25580B24, 0x32878908,
+0x62838B00, 0xA0156053, 0x064D0B24, 0x099E096C,
+0x8F083277, 0xE07B6623, 0x0F164008, 0x02FC7098,
+0x01FE7068, 0x626C662C, 0x32876053, 0x0B648F02,
+0x646336E8, 0x625C7501, 0x8BCD3293, 0xE014D635,
+0xE4000644, 0xD53461DC, 0x6250E708, 0x3217622C,
+0x6DDC8B01, 0x740162D3, 0x3673664C, 0x8FF42520,
+0xE4007501, 0xD52D61CC, 0x622C6250, 0x8B013217,
+0x62C36CCC, 0x664C7401, 0x25203673, 0x75018FF4,
+0x0009A0EC, 0x4008E079, 0x642C02FC, 0x430BD319,
+0x660C6E43, 0x3653E518, 0xE0638910, 0x46084008,
+0x460804FE, 0x70E09722, 0x347C4600, 0xD31305FE,
+0x346C430B, 0xE0626203, 0x0F244008, 0xCB0660E3,
+0x430BD30C, 0x660C6403, 0x3653E518, 0xE0638928,
+0x46084008, 0x460804FE, 0x70E09708, 0x347C4600,
+0xD30605FE, 0x346C430B, 0x6C03A01D, 0x0000064D,
+0x001142E8, 0x001148E0, 0x001148BA, 0x00114934,
+0x00114000, 0x00114008, 0x00114774, 0x00114011,
+0x001142E4, 0x001142D8, 0x001142ED, 0x001142F5,
+0x4008E062, 0x60E30CFC, 0xD39CCB08, 0x6403430B,
+0xE07A610C, 0x4008E618, 0x8D1C3163, 0xE05D0F14,
+0x07FC4008, 0x09FC70FC, 0x04FE70FC, 0xD394667C,
+0x659C430B, 0xE07A6503, 0x01FC4008, 0x611C70A4,
+0x04FE4108, 0x9D744108, 0x34DC4100, 0x430BD38D,
+0xA003341C, 0xE0626D03, 0x0DFC4008, 0xE500D68A,
+0x640D8562, 0x4008E05B, 0x01FEA02C, 0x6053655C,
+0x305C4000, 0x4008D685, 0x622D026D, 0x8F203210,
+0xD3836E03, 0x641D430B, 0x2228620D, 0xD681891D,
+0x066C60E3, 0x4008E062, 0x460002FC, 0x3167612C,
+0x62638B00, 0x64CCE062, 0x34674008, 0x8F010F24,
+0x626362C3, 0x356765DC, 0x6C238D01, 0xA00466D3,
+0x75016D63, 0x3243625C, 0xE0628BD0, 0x07FC4008,
+0x617CE400, 0xE904D570, 0x622C6250, 0x8B013217,
+0x6273677C, 0x664C7401, 0x25203693, 0x75018FF4,
+0x61CCE400, 0xE708D569, 0x666C6650, 0x8B013617,
+0x66C36CCC, 0x624C7401, 0x25603273, 0x75018FF4,
+0x61DCE400, 0x6650D562, 0x3617666C, 0x6DDC8B01,
+0x740166D3, 0x3273624C, 0x8FF42560, 0xA0057501,
+0x064D0009, 0xE200D65B, 0x0624E03A, 0xE03AD659,
+0x2228026C, 0xE039894B, 0x2228026C, 0xE05B8947,
+0x0EFE4008, 0x3E669690, 0xE0798941, 0x00FC4008,
+0x8D023E66, 0xCB02640C, 0xD344640C, 0x0009430B,
+0xE05C660C, 0x07FC4008, 0x4608701C, 0x617C05FE,
+0x977A4608, 0x357C4600, 0x6613356C, 0x430BD346,
+0xD54464E3, 0x62032008, 0x0029150F, 0x6603CA01,
+0x2668E03B, 0x05648D20, 0xC8F06023, 0xD53F8909,
+0x76FF6650, 0x84512560, 0x805170FF, 0x70FF8452,
+0x60238052, 0x890FC80F, 0x6260D639, 0x26207201,
+0x70018461, 0x84628061, 0xA0057001, 0xD6318062,
+0xE03BE200, 0x162F0624, 0x4008E05B, 0x964302FE,
+0x8B653266, 0xD72BD428, 0xD52E6040, 0x4028C93F,
+0x40084008, 0x50726203, 0xC802D12B, 0xE604891A,
+0x46284618, 0x2522226B, 0xE2086040, 0x6503C93F,
+0x66034508, 0x45004508, 0x46284218, 0x6263252B,
+0x42084208, 0x252B4200, 0x4218E208, 0x252B4228,
+0x2152A062, 0x4618E614, 0x226B4628, 0x60402522,
+0xC93FE428, 0x45086503, 0x45084028, 0x45004008,
+0x40084418, 0x254BE728, 0x47184000, 0x4728250B,
+0xD412257B, 0x2152A044, 0x064D09B4, 0x001148E0,
+0x001148BA, 0x00114934, 0x00114000, 0x00114008,
+0x00114774, 0x00114011, 0x001142FD, 0x00114301,
+0x00114309, 0x001142D8, 0x00114A24, 0x001142F5,
+0x001142ED, 0x001C3694, 0x001C3BB4, 0x001142E8,
+0xE214D429, 0x42186040, 0x4028C93F, 0x40084008,
+0xD6264228, 0x2602202B, 0xE7286040, 0x6503C93F,
+0x45084508, 0x45004028, 0x40084718, 0x4008257B,
+0x4000E728, 0x250B4718, 0xD21D4728, 0x2252257B,
+0xD71C6240, 0x0724E044, 0x3F3C932C, 0x4F164F06,
+0x6EF64F26, 0x6CF66DF6, 0x6AF66BF6, 0x000B69F6,
+0x2FE668F6, 0x6243D114, 0xE4028512, 0x6E2D670D,
+0xE500A00F, 0x6053655D, 0x305C4000, 0x4008D10F,
+0x622D021D, 0x8B0332E0, 0x041C7108, 0x644CA004,
+0x625D7501, 0x8BED3273, 0x4618E602, 0x604D2469,
+0x6EF6000B, 0x000001F0, 0x001142E8, 0x001C3694,
+0x001C3BB4, 0x001142D8, 0x00114000, 0x00114008,
+0xD766D565, 0x62725151, 0x321CE340, 0x51522722,
+0x337C5271, 0x1721321C, 0x52725153, 0x321C644C,
+0x1722D15F, 0x66125255, 0x2162362C, 0x316C5173,
+0x61521713, 0xD65B5274, 0x1724321C, 0x52755154,
+0x1725321C, 0x52765158, 0x1726321C, 0x51776262,
+0x1717312C, 0x51785261, 0x1718312C, 0x51795262,
+0x1719312C, 0x517A5263, 0x171A312C, 0x517B5264,
+0x171B312C, 0x517C5265, 0x171C312C, 0x517D5266,
+0x171D312C, 0x517E5267, 0x171E312C, 0x527F5168,
+0x321CD645, 0x6262172F, 0x76946132, 0x2312312C,
+0x52316162, 0x321CD641, 0x515C1321, 0x351C5532,
+0x61621352, 0x41295235, 0x1325321C, 0x56365561,
+0x365C4529, 0x1366E538, 0x55312450, 0x71046143,
+0x66722152, 0x75086543, 0x56712562, 0x750C6543,
+0x56722562, 0x75106543, 0x56752562, 0x75146543,
+0x56732562, 0x75186543, 0x56762562, 0x751C6543,
+0x56322562, 0x75206543, 0x66322562, 0x75246543,
+0x56742562, 0x75286543, 0x56342562, 0x752C6543,
+0x55332562, 0x72306243, 0x55352252, 0x72346243,
+0x56362252, 0x24627438, 0x1341E400, 0x17412742,
+0x17451742, 0x17461743, 0x23421342, 0x13441744,
+0x13451343, 0x1346000B, 0xD510E124, 0x51572410,
+0x52581411, 0x57591422, 0x515A1473, 0x525B1414,
+0x575C1425, 0x525D1476, 0x1427E700, 0x1468565E,
+0x1469565F, 0x15781577, 0x157A1579, 0x157C157B,
+0x157E157D, 0x157F000B, 0x001C369C, 0x0020351C,
+0x00203578, 0x001C3CA0, 0x001C36F4, 0x001C3B88,
+0x6E726157, 0x21676E69, 0x69685420, 0x6F642073,
+0x656C676E, 0x746F6E20, 0x65656220, 0x6163206E,
+0x7262696C, 0x64657461, 0x0000000A, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+};
+
+const u32_t zcFwBufImageSize=83968;
diff --git a/drivers/staging/otus/hal/hpfwspiu.c b/drivers/staging/otus/hal/hpfwspiu.c
new file mode 100644
index 00000000000..eda7ff5e573
--- /dev/null
+++ b/drivers/staging/otus/hal/hpfwspiu.c
@@ -0,0 +1,655 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include "../80211core/cprecomp.h"
+
+const u32_t zcFwImageSPI[]={
+0x0009000B, 0x4F222FE6, 0xB0187FFC, 0xE6000009,
+0x943DD520, 0xC8406052, 0x2F028F03, 0x8FF93642,
+0xD41D7601, 0x4E0BDE1D, 0xD41D0009, 0x00094E0B,
+0x4E0BD41C, 0x7F040009, 0xA0214F26, 0x4F226EF6,
+0xE205D119, 0x2122E400, 0x92222142, 0x8BFD4210,
+0x450BD516, 0xD6160009, 0x0009460B, 0xE5FFD715,
+0x2752655D, 0xE1FFD714, 0xD4145079, 0x1709CB01,
+0x17112712, 0x2412E101, 0x4F26D411, 0x2410000B,
+0xDE11DD10, 0x00094D0B, 0x00094E0B, 0x0009AFFA,
+0x03E82710, 0x001C001C, 0x00116594, 0x00114EBE,
+0x001165A4, 0x001165BC, 0x001D4004, 0x00114FA0,
+0x00114378, 0x001C3510, 0x001C3624, 0x001E212C,
+0x001164FC, 0x00114700, 0x0011589C, 0x2FA62F96,
+0x2FC62FB6, 0x2FE62FD6, 0x7FC84F22, 0xD28DDD8C,
+0x61D360D0, 0x80F47101, 0x420B6010, 0x200880F8,
+0x6E038F10, 0xDB89D488, 0xD4896A40, 0x4B0BDC89,
+0x67C065AC, 0x697CDE88, 0x41086193, 0x31984108,
+0x3E1C4108, 0x66D284F8, 0x2008600C, 0x2E628F13,
+0xE40084F4, 0xDA81670C, 0x3273624D, 0xA0D38B01,
+0x644D0009, 0x35AC6543, 0x69436652, 0x39EC6B62,
+0xAFF119B1, 0x88017404, 0x84F48B15, 0x2E70E700,
+0xDA766E0C, 0x32E3627D, 0xA0C48B01, 0x677D0009,
+0x6C7366A3, 0x65737604, 0x356C3CAC, 0x6D5264C2,
+0xAFEF7708, 0xE2B024D2, 0x3020622C, 0x84F48B30,
+0x650CEC00, 0xDA691F53, 0x55F3E904, 0x325362CD,
+0xA0A88B01, 0x6CCD0009, 0x67C36EA3, 0x6BC37E04,
+0x3BEC37AC, 0x6EB26D72, 0xDB62D461, 0x00094B0B,
+0x410BD161, 0xD46164D3, 0x00094B0B, 0x450BD55E,
+0xD45F64E3, 0x00094B0B, 0x61D3E600, 0x316C666D,
+0x646D7601, 0x21E03493, 0x4E198FF7, 0x7C08AFD5,
+0x622CE2B1, 0x8B113020, 0xD552D456, 0xDA56DC4F,
+0x0009450B, 0x4A0BE400, 0xD75467C2, 0x470BDB52,
+0x4B0B0009, 0xE900E403, 0x2E90A06D, 0x622CE2B2,
+0x89683020, 0x622CE2B3, 0x8B1D3020, 0xDA45D44C,
+0x4A0BD942, 0x65960009, 0x6792D44A, 0x1F74DD3B,
+0x1F5D4D0B, 0xD639D448, 0x460BDB48, 0x55F455F4,
+0x4B0BD936, 0xD44654FD, 0x490B6503, 0x5DF51F05,
+0x1ED1EC04, 0x2EC0A047, 0x622CE2B4, 0x8B3E3020,
+0xDA34D440, 0x4A0BDD31, 0x84F40009, 0x600C6CD2,
+0x1F072F02, 0x1FC6C903, 0xE6001F08, 0xD73AE030,
+0x6CF2DB3A, 0x1F790F65, 0xA0211FBA, 0x51F6E904,
+0x6D63666D, 0x4C1536EC, 0xD2353D1C, 0x1F6B8F05,
+0x89023C93, 0xA00264D3, 0xE50455F8, 0x420B64D3,
+0x5BFB0009, 0xD61954FA, 0x460B65D3, 0x54F91B01,
+0xDA1655B1, 0x7CFC4A0B, 0x06FDE030, 0x0F657604,
+0x626D55F7, 0x8BDA3253, 0xA00484F4, 0xD4252E00,
+0x420BD20E, 0x7F3865D2, 0x6EF64F26, 0x6CF66DF6,
+0x6AF66BF6, 0x69F6000B, 0xE6006163, 0x4109A004,
+0x76016256, 0x74042422, 0x8BF93612, 0x0009000B,
+0x00117800, 0x00115FF0, 0x001164F6, 0x00114F2C,
+0x001165C0, 0x001164F5, 0x0011611C, 0x00117804,
+0x001165E0, 0x00114EBE, 0x00114F02, 0x001165F4,
+0x001165FC, 0x00116600, 0x00114BF0, 0x001148FC,
+0x00116618, 0x00116634, 0x00116640, 0x00114E56,
+0x0011664C, 0x00116658, 0x0011667C, 0x00116670,
+0x00114BC4, 0x00116688, 0x2F962F86, 0x2FB62FA6,
+0x2FD62FC6, 0x4F222FE6, 0xE5007FD8, 0x6453E110,
+0x6C534128, 0x655DEE0A, 0x46086653, 0x4608365C,
+0x361C7501, 0x675D6043, 0x60C30F66, 0x37E3ED00,
+0x816126C1, 0x81638162, 0x16D316D2, 0x8FEA16D4,
+0x68F27404, 0xDAB3D9B2, 0x29821981, 0xD1B259F1,
+0x2A921A91, 0x5BF35AF2, 0x5EF55DF4, 0x11A154F6,
+0x11B321A2, 0x11D511B2, 0x11E711D4, 0x114911E6,
+0x55F71148, 0xEE00DBA9, 0xDDA957F8, 0xD6A952F9,
+0x1B5164E3, 0xDBA82B52, 0xEAB8D8A8, 0x2D72E945,
+0x6AAC2622, 0x6EED4908, 0x4D086DE3, 0x3DEC61E3,
+0x4D084108, 0x3DBC31EC, 0x410860C3, 0x81D12DC1,
+0x4108E050, 0x41084008, 0x60C381D2, 0xE500318C,
+0x81D334A2, 0x1D131DD2, 0x8D01D494, 0xD4911D54,
+0xB08165D3, 0x64ED7E01, 0x8BDC3492, 0xDB94D18D,
+0xD28B6812, 0x1B814829, 0x2FD26412, 0x2B92694D,
+0xD98A6722, 0x1B734729, 0xD7876822, 0x1BA26A8D,
+0xD28C6B72, 0x22B2D586, 0xE0035D72, 0x5E7412D2,
+0x12E44018, 0xD6885176, 0x54781216, 0x1248E1FF,
+0xD4856792, 0x6852127A, 0x28C1E703, 0x81916952,
+0x6A52E050, 0x81A24008, 0x60C36B52, 0x6D5281B3,
+0x6E521DD2, 0x62521E63, 0x1264E600, 0x46086563,
+0x7501364C, 0x665D2612, 0x8BF83673, 0xE003D471,
+0x40186542, 0x674225C1, 0x8171D274, 0xEE006842,
+0x69421882, 0x1923E024, 0xE5806A42, 0x6B421AE4,
+0x81B266E3, 0xD46D6C42, 0x655C81C3, 0x6D63666D,
+0x616D7604, 0x31533D4C, 0x2DE28FF8, 0xD569D268,
+0x74042422, 0x7F282452, 0x6EF64F26, 0x6CF66DF6,
+0x6AF66BF6, 0x000B69F6, 0x664268F6, 0xC8036061,
+0xE5008D04, 0xC9036061, 0x8B038802, 0x65635262,
+0x24125124, 0x6053000B, 0x2FE62FD6, 0x7FEC4F22,
+0x62536E53, 0x6D43E550, 0x4508E400, 0xE101A001,
+0x60435224, 0x81212211, 0x60538123, 0x56E28122,
+0x8BF53620, 0x16E4D250, 0xE61464F3, 0x65E3420B,
+0xE4FC65E1, 0x2E512549, 0x65F361F1, 0x2F112149,
+0xD14954D1, 0xE614410B, 0x607157D1, 0x2701CB01,
+0x7F141DE1, 0x6EF64F26, 0x6DF6000B, 0x2FE62FD6,
+0x7FEC4F22, 0x66536E53, 0x6D43E5FC, 0x20596061,
+0x2601CB01, 0x326052E2, 0x12E48B06, 0x31E051E2,
+0x52D18B04, 0x1E22A002, 0x5664AFF0, 0x64F3D236,
+0x420BE614, 0x67E165E3, 0x2719E1FC, 0x67F12E71,
+0x271954D1, 0x65F3D130, 0x410BE614, 0x52D12F71,
+0xCB016021, 0x1DE12201, 0x4F267F14, 0x000B6EF6,
+0x2FE66DF6, 0x624C4F22, 0x4208DE1B, 0xA0054200,
+0x52523E2C, 0x5624D417, 0x2E62BF8E, 0x52E165E2,
+0x8BF63520, 0x2622D61B, 0x000B4F26, 0x2FB66EF6,
+0x2FD62FC6, 0x4F222FE6, 0xDB1CDC10, 0x66C252C1,
+0x89403620, 0xC9036061, 0x893C8801, 0xDD18DE0B,
+0x64E3BF63, 0x85036503, 0x620D66B2, 0x892B3262,
+0xBF9BD403, 0xD4130009, 0x00094D0B, 0x0009AFE6,
+0x001160DC, 0x001160E4, 0x001160EC, 0x00116114,
+0x001164F8, 0x00116500, 0x001000C8, 0x00101680,
+0x001E2108, 0x001C3D00, 0x00117880, 0x00117780,
+0x00040020, 0x0026C401, 0x001142F8, 0x001164DC,
+0x00114EBE, 0x0011669C, 0x64E3BF3E, 0x4D0BD406,
+0xAFBB0009, 0xD2050009, 0x4F262262, 0x6DF66EF6,
+0x000B6CF6, 0x00006BF6, 0x001166A0, 0x001C3D28,
+0x2F962F86, 0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6,
+0xD23C7FFC, 0xC8036022, 0x2F018F3D, 0x0009A061,
+0xC9036061, 0x893B8801, 0xD238D837, 0x420BD938,
+0xE4006483, 0x6A036D03, 0x5C02490B, 0xD236DB35,
+0x56D385D2, 0x650D6422, 0x4B0BE740, 0xD1326E03,
+0x64126EED, 0x214234EC, 0x3DC05DD4, 0x85D28BEF,
+0x70FF56D3, 0xE740650D, 0x6C034B0B, 0x490BDB2A,
+0x66B2E403, 0x36CC6CCD, 0xE700D928, 0x2B62E5C8,
+0x6473E650, 0x490BDC26, 0x6483655C, 0x65A34C0B,
+0xEE01D124, 0xD11C21E2, 0x66125211, 0x8BBF3620,
+0xDD22DE21, 0xDC23DB22, 0x65D252D1, 0x89183520,
+0xC9036051, 0x89148801, 0xD114D41C, 0x0009410B,
+0x36E05603, 0x65038F04, 0x2B20E201, 0x2C52AFEC,
+0xD213D419, 0x0009420B, 0xE101D618, 0xAFE34118,
+0x60F12612, 0x8902C804, 0x420BD215, 0x7F040009,
+0x6EF64F26, 0x6CF66DF6, 0x6AF66BF6, 0x000B69F6,
+0x000068F6, 0x001E2100, 0x001160E4, 0x0011453A,
+0x00114BF0, 0x00114E0C, 0x00116714, 0x001159B0,
+0x00114558, 0x001E212C, 0x00117880, 0x001160DC,
+0x001164FC, 0x001164F8, 0x00116114, 0x001C3D30,
+0x001140CC, 0xD6C2D5C1, 0x26226252, 0xC8016060,
+0x000B8BFA, 0x2FE60009, 0xBFF34F22, 0xD2BD0009,
+0xE405E100, 0x22402212, 0x6422DEB8, 0xE700D5B8,
+0x25721E42, 0xC98F8451, 0xC9F0CB10, 0x8051CB02,
+0xCB026050, 0x62522500, 0x2E22BFDC, 0xD6B250E4,
+0x4F262602, 0x6EF6000B, 0x4F222FD6, 0x0009BFDB,
+0x620CDDAE, 0x60D02D22, 0x8906C801, 0x0009BFD3,
+0x2D22620C, 0xC80160D0, 0x4F268BF8, 0x6DF6000B,
+0x4F222FE6, 0x6E43BFE8, 0xE100D2A2, 0x22E02212,
+0x6422D59E, 0xE600DE9E, 0x2E621542, 0xC9F084E1,
+0x80E1CB01, 0xCB0260E0, 0x67E22E00, 0x4F262572,
+0x6EF6AFA8, 0xE406AFE4, 0xE404AFE2, 0xBFF94F22,
+0xE4C70009, 0x644CBFDC, 0x4F26AFF6, 0xE406AFD8,
+0xE404AFD6, 0x4F222FE6, 0x6E43BFF8, 0xD58DD28D,
+0xE401E100, 0x221260E3, 0x80512240, 0x6622D187,
+0xE700DE87, 0x2E721162, 0xC9F084E1, 0x80E1CB02,
+0xCB0260E0, 0x62E22E00, 0x2122BF7C, 0xAFDF4F26,
+0x2FD66EF6, 0x4F222FE6, 0xBFCB6D53, 0xBF9B6E43,
+0xD27C0009, 0x22E061D3, 0x6022DE7D, 0x411821E9,
+0x201BC9FF, 0x2202D577, 0xD6768453, 0x60D38051,
+0xD4728053, 0xD1726762, 0x1472ED00, 0x841121D2,
+0xCB04C9F0, 0x60108011, 0x2100CB02, 0xBF516212,
+0x4F262422, 0xAFA76EF6, 0x65436DF6, 0xAFD0E4D8,
+0x6543644C, 0xAFCCE4D8, 0x2FC6644C, 0x2FE62FD6,
+0x6E534F22, 0xBF676D43, 0xD7626C63, 0x27D0D264,
+0x61E36072, 0x41182129, 0x201BC9FF, 0x2702D45D,
+0xD15B8443, 0x60E38041, 0xDE588043, 0xE6006472,
+0x21621E42, 0x65DC8411, 0x60C36203, 0x4008C907,
+0x67034008, 0xE29F6023, 0x622CC98F, 0x3520207B,
+0x80118D18, 0x7C048411, 0x60C36603, 0x6203C90F,
+0xC9F06063, 0x8011202B, 0x880B6053, 0x84118B14,
+0xC90F6603, 0xC90F7001, 0x60636203, 0x202BC9F0,
+0x8011A00A, 0x7C018411, 0x60C36603, 0x6203C90F,
+0xC9F06063, 0x8011202B, 0xCB026010, 0x62122100,
+0x2E22BEF0, 0xD63C50E4, 0x4F262602, 0x6DF66EF6,
+0x6CF6000B, 0x2FC62FB6, 0x2FE62FD6, 0x6C634F22,
+0x6E436D53, 0x6B73BF36, 0x0009BF06, 0x61D3D231,
+0xDE3322E0, 0x21E96022, 0xC9FF4118, 0xD42D201B,
+0x84432202, 0x8041D72F, 0x804360D3, 0x6622D427,
+0x1462D127, 0x14C327C2, 0x21C2EC00, 0x7B048411,
+0x60B36D03, 0x6503C90F, 0xC9F060D3, 0x8011205B,
+0xCB026010, 0x62122100, 0x4F262422, 0x6DF66EF6,
+0xAEAF6CF6, 0x2FB66BF6, 0x2FD62FC6, 0x4F222FE6,
+0x6C536D63, 0xBEFD6E43, 0xBECD6B73, 0xD2150009,
+0x22E061C3, 0x6022DE16, 0x411821E9, 0x201BC9FF,
+0x2202D110, 0xD60F8413, 0x60C38011, 0xDE0B8013,
+0xD40B6762, 0xEC006BBD, 0x1EB51E72, 0x844124C2,
+0xC9F04B21, 0x8041CB04, 0xE1406040, 0x2400CB06,
+0xE5006242, 0x4B212E22, 0x4128A014, 0x001D1200,
+0x00116528, 0x00116530, 0x00116538, 0x00116544,
+0x00FFFFFF, 0x00116534, 0x6053655D, 0x06DE4008,
+0x21627501, 0x32B3625D, 0x4F268BF6, 0x6DF66EF6,
+0xAE5F6CF6, 0x4F226BF6, 0xBF73677C, 0xAEB3644C,
+0x4F224F26, 0xBFA6677D, 0xAEAD644C, 0x4F224F26,
+0xE500E49F, 0xBF08E603, 0x4F26644C, 0x600C000B,
+0xE49F4F22, 0xE603E500, 0x644CBEFF, 0x4F264019,
+0x600D000B, 0x6543665C, 0xE403AEF7, 0x6543665C,
+0xE40BAEF3, 0xD175D674, 0x60436262, 0xC8012122,
+0x8F016010, 0xC9EFCB10, 0x62122100, 0x2622000B,
+0x4F222FE6, 0xE0004F13, 0xBE2C401E, 0xD56C6E43,
+0x2522620C, 0xE401BFE6, 0x6063D669, 0x60ECCF80,
+0x89072008, 0x89098801, 0x890D8802, 0x89118803,
+0x0009A013, 0xC9E36060, 0x2600A00F, 0xCB106060,
+0xCB04C9F7, 0x2600A009, 0xCB106060, 0xCB08C9FB,
+0x2600A003, 0xCB1C6060, 0xD5592600, 0xBE616252,
+0xE400642C, 0x4F264F17, 0x6EF6AFBC, 0x2F962F86,
+0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6, 0x60C36C7C,
+0x6A638802, 0x69538F09, 0x65436290, 0x662CE4AF,
+0xBEF7E701, 0xA00A644C, 0x2CC80009, 0x88018901,
+0x65438B05, 0xE600E4AF, 0xBEEBE701, 0xBDD1644C,
+0xED010009, 0xDE43EBAF, 0xE800A02C, 0x0009BDF4,
+0x60C3D141, 0x8802E200, 0xD5402122, 0x21B08D06,
+0x89082CC8, 0x890A8801, 0x0009A00C, 0x009C60D3,
+0xA007D639, 0xD2388061, 0xA0036083, 0xD2368021,
+0x802160D3, 0xD1356412, 0x1E42E600, 0x84512162,
+0xC9F07D01, 0x8051CB02, 0xCB026050, 0x67122500,
+0x2E72BDA0, 0x8BD13DA2, 0x0009BDF6, 0x0009BDA3,
+0x620CD627, 0x4F262622, 0x6DF66EF6, 0x6BF66CF6,
+0x69F66AF6, 0x68F6000B, 0xE702AF98, 0x2F962F86,
+0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6, 0x3F3C9331,
+0x0F569030, 0xE8FF70FC, 0x688C0F46, 0xE900A049,
+0x4018E010, 0xE50404FE, 0xBF33349C, 0x88FF6A43,
+0x901F893E, 0xE1100CFE, 0x41183C98, 0x8B033C16,
+0x64A3BE1B, 0x0009A031, 0x4018E010, 0xED000BFE,
+0xA0073BCC, 0x64D36EF3, 0xBF1F34BC, 0x2E00E501,
+0x7E017D01, 0x8BF63DC2, 0x64A3BE07, 0xA01AED00,
+0xEFF86EF3, 0x00001004, 0x001D1204, 0x0011652C,
+0x00116544, 0x001D1200, 0x00116530, 0x00116528,
+0x666C66E0, 0x89043680, 0x35BC65D3, 0xBE51E701,
+0x7D01E402, 0x3DC27E01, 0xE1108BF2, 0x391C4118,
+0x90547904, 0x391201FE, 0x93518BB2, 0x4F263F3C,
+0x6DF66EF6, 0x6BF66CF6, 0x69F66AF6, 0x68F6000B,
+0x676D6253, 0x66236543, 0xE402AEC3, 0x2FA62F96,
+0x2FC62FB6, 0x2FE62FD6, 0x697D4F22, 0x4A216A93,
+0x4A084A21, 0x6C436D63, 0xA0086B73, 0x64C36E53,
+0x669365D3, 0x6BBDBFE4, 0x3DAC3CBC, 0x6EEF3EB8,
+0x8BF42EE8, 0x4F26E000, 0x6DF66EF6, 0x6BF66CF6,
+0x000B6AF6, 0x2FA669F6, 0x2FC62FB6, 0x2FE62FD6,
+0xEC004F22, 0x6B536EC3, 0xA0066D43, 0x64D3EA01,
+0x65A3BEA8, 0x7D013C0C, 0x3EB27E01, 0x60C38BF7,
+0x6EF64F26, 0x6CF66DF6, 0x000B6BF6, 0x10046AF6,
+0x00001008, 0x0009000B, 0x2FD62FC6, 0x4F222FE6,
+0x6D436C53, 0xEE00A004, 0x7E0164D4, 0x644CBFF2,
+0x8BF93EC2, 0x6EF64F26, 0x000B6DF6, 0xE5006CF6,
+0x6643A002, 0x76017501, 0x22286260, 0xAFE38BFA,
+0x2FE60009, 0x75076253, 0xE1086753, 0x6043EE0A,
+0x4409C90F, 0x650330E2, 0x8D014409, 0xE630E637,
+0x4110365C, 0x8FF22760, 0xE00077FF, 0x000B8028,
+0x4F226EF6, 0xBFE47FEC, 0xBFD865F3, 0x7F1464F3,
+0x000B4F26, 0x4F22E000, 0xBFDA7FEC, 0x64F365F3,
+0x7406BFCD, 0x4F267F14, 0xE000000B, 0x4F222FE6,
+0x62537FEC, 0x65F36E43, 0x6423BFCB, 0x64E3BFBF,
+0x64F3BFBD, 0xBFBAD403, 0x7F140009, 0x000B4F26,
+0x00006EF6, 0x001166A4, 0xE4FDD29A, 0xD79A6122,
+0x22122149, 0x74016022, 0x2202CB01, 0xD5976622,
+0x22622649, 0xC8406070, 0x60528902, 0x2502CB04,
+0xE1F76452, 0x25422419, 0xE7016052, 0x2502C9CF,
+0xE6026052, 0x2502CB03, 0x15624718, 0x1573000B,
+0xD78CD58B, 0xD48DD28C, 0xE600E100, 0x27112511,
+0xAFD12210, 0x664C2461, 0x4600D289, 0x6060362C,
+0x000BCB10, 0x654C2600, 0x4500D285, 0x6650352C,
+0x2619E1EF, 0x2560000B, 0xD282664C, 0x362C4600,
+0xCB106060, 0x2600000B, 0xD27E654C, 0x352C4500,
+0xE1EF6650, 0x000B2619, 0x664C2560, 0x4600D278,
+0x6060362C, 0x000BCB08, 0x654C2600, 0x4500D274,
+0x6650352C, 0x2619E1F7, 0x2560000B, 0xD271664C,
+0x362C4600, 0xCB086060, 0x2600000B, 0xD26D654C,
+0x352C4500, 0xE1F76650, 0x000B2619, 0x624C2560,
+0x4200D667, 0x6020326C, 0x4021C908, 0x40214021,
+0x600C000B, 0xD663624C, 0x326C4200, 0xC9086020,
+0x40214021, 0x000B4021, 0xD15F600C, 0x341C644C,
+0x000B6240, 0xD15D602C, 0x341C644C, 0x000B6240,
+0x2FE6602C, 0x6E434F22, 0xE60A645C, 0x89143467,
+0x0009BFEB, 0x60EC640C, 0x8B028801, 0xA002E00F,
+0x44092409, 0x624C4409, 0x3263E60A, 0xBFE28905,
+0x620C644C, 0xC8806023, 0xE2008B00, 0x4F266023,
+0x6EF6000B, 0xD64A4F22, 0x88016062, 0xB2458B03,
+0xA0030009, 0xD2470009, 0x2260E640, 0xE200D646,
+0x000B4F26, 0x4F222622, 0x6062D641, 0x8B018802,
+0x0009B28E, 0xE200D640, 0x000B4F26, 0xD53C2622,
+0xE100D43C, 0x2512E701, 0x2470000B, 0xE604D239,
+0x2260000B, 0xD4394F22, 0x410BD139, 0xD5390009,
+0x6650E1FD, 0x2619D238, 0x2560E700, 0x000B4F26,
+0x4F222270, 0xD132D435, 0x0009410B, 0xE7FBD531,
+0x26796650, 0x000B4F26, 0x4F222560, 0xD12CD430,
+0x0009410B, 0xE7F7D52B, 0x26796650, 0x000B4F26,
+0xD5282560, 0x6250942D, 0x000B2249, 0xD5252520,
+0x6250E4BF, 0x000B2249, 0x4F222520, 0x8522D225,
+0x2008600D, 0x88018911, 0x88038913, 0x88058915,
+0x88068942, 0x88088948, 0x8809894E, 0x880A8954,
+0x880B895A, 0xA0678960, 0xB0690009, 0xA0640009,
+0xB077600C, 0xA0600009, 0xB080600C, 0xA05C0009,
+0xFF7F600C, 0x001E2148, 0x001E1000, 0x001E1108,
+0x00116570, 0x00116572, 0x00116591, 0x00116554,
+0x001E103F, 0x001E105F, 0x001E102F, 0x001E1090,
+0x00116578, 0x001E100B, 0x00116574, 0x001166A8,
+0x00114EBE, 0x001E1028, 0x00116590, 0x001166B4,
+0x001166C4, 0x00116548, 0x6260D684, 0x8B2B2228,
+0x0009B061, 0x600CA029, 0x6260D680, 0x8B232228,
+0x0009B069, 0x600CA021, 0x6260D67C, 0x8B1B2228,
+0x0009B0C7, 0x600CA019, 0x6260D678, 0x8B132228,
+0x0009B0CD, 0x600CA011, 0x6260D674, 0x8B0B2228,
+0x0009B125, 0x600CA009, 0x6260D670, 0x8B032228,
+0x0009B13D, 0x600CA001, 0x4F26E000, 0x0009000B,
+0xD26CD16B, 0xD56C8412, 0x4000C90F, 0xD76B012D,
+0xE403D66B, 0xE20F611C, 0x2540E001, 0x25202712,
+0x2602000B, 0xE601D262, 0x30668523, 0xE0008D05,
+0xD663D260, 0xE0018122, 0x000B2602, 0xD25C0009,
+0x600D8523, 0x89052008, 0x8B0A8801, 0x6060D65D,
+0x2600CB01, 0xD457D65A, 0xE001E101, 0x000B2612,
+0x000B8142, 0xD152E000, 0x8513E501, 0x640D4518,
+0x66033453, 0xE0008D05, 0xD551D253, 0x2260E001,
+0x000B2502, 0x4F220009, 0x8513D149, 0x6453650D,
+0x62494419, 0x227D672E, 0x8801602C, 0x88028909,
+0x88038910, 0x8806891A, 0x88078935, 0xA04C893B,
+0xD5460009, 0x6652D746, 0x2762D446, 0x622C6261,
+0x2421A038, 0x2228625C, 0xD4438B3F, 0x6642D540,
+0x2562D440, 0x24018561, 0x6203A02C, 0x2008605C,
+0x88108907, 0x88208908, 0x88308909, 0xA02C890A,
+0xD23A0009, 0x6222A008, 0xA005D239, 0xD2396222,
+0x6222A002, 0x6262D638, 0xD432D531, 0x66212522,
+0xA00F626C, 0xD6352421, 0x6261D52D, 0x622CD42D,
+0xA0072562, 0xD6322421, 0x8561D529, 0x2562D429,
+0x62032401, 0x662D8515, 0x3617610D, 0x65038F01,
+0xB0CB2451, 0xA0010009, 0xE000E001, 0x000B4F26,
+0xD6190009, 0xD427E101, 0x65412610, 0xD118D717,
+0xE20F655D, 0x2752E001, 0x000B2620, 0x2FE62102,
+0xD20F4F22, 0x640C8523, 0x8B082448, 0xD511D61D,
+0x2621E200, 0x940F8451, 0xA0482049, 0xDE0D8051,
+0xC84060E0, 0xE2018D32, 0x89443427, 0xD216D615,
+0x2641420B, 0x0009A030, 0x0000FF7F, 0x00116591,
+0x00116548, 0x00116554, 0x001E1100, 0x001E100C,
+0x00116574, 0x001E1000, 0x001E1001, 0x0011657C,
+0x0011655C, 0x00116560, 0x00116564, 0x00116580,
+0x00116584, 0x00116588, 0x0011658C, 0x00116774,
+0x0011677E, 0x0011656E, 0x00115DCA, 0x89123427,
+0xD294D693, 0x2641420B, 0xCB8084E1, 0x80E1B0F5,
+0xD69160E0, 0x2E00CB04, 0xC93F6060, 0xD68F2600,
+0xA001E001, 0xE0002602, 0x000B4F26, 0xD68C6EF6,
+0xC8806060, 0xD2868919, 0x88016021, 0xD2898B15,
+0x8524E501, 0x89103056, 0xE203D187, 0x2120D487,
+0xE00B6541, 0x0656655D, 0xE40FD585, 0x2140E702,
+0xD77E2571, 0x000BE001, 0x000B2702, 0x2FE6E000,
+0xDE804F22, 0xC88084E1, 0xD57A892C, 0x20088554,
+0x61038F28, 0x8553D77C, 0x64036672, 0x8566650C,
+0x3520620C, 0xD6798B1E, 0x651CD774, 0x2651644C,
+0x60E02741, 0x8904C840, 0x420BD275, 0xA0030009,
+0xD2680009, 0x0009420B, 0x0009B09F, 0xE201D167,
+0x60E02122, 0xCB04D464, 0x60402E00, 0x2400C93F,
+0x6023A001, 0x4F26E000, 0x6EF6000B, 0x2FB62FA6,
+0x2FD62FC6, 0xDA622FE6, 0x66A1E240, 0x3622DC5E,
+0x62638900, 0x6ED36D2C, 0x4E2136D8, 0x4E212A61,
+0xDB61D460, 0xE700A00F, 0x770162B2, 0x71026123,
+0x66212B12, 0x71026213, 0x61212B12, 0x651D666D,
+0x356C4528, 0x627C2452, 0x8BED32E3, 0xC90360D3,
+0x8B108803, 0x617367B2, 0x2B127102, 0x71026E13,
+0x2B126571, 0x655D6DE1, 0x422862DD, 0x325CE107,
+0xA00C2C10, 0x88022422, 0xA0038B01, 0x8801E203,
+0xE2018B05, 0x66B22C20, 0x655D6561, 0xE60F2452,
+0x67A12C60, 0x8B052778, 0xDD38DC44, 0xEB01EA00,
+0x2DB22CA2, 0x6DF66EF6, 0x6BF66CF6, 0x6AF6000B,
+0x2FE62FD6, 0xE240DD36, 0x362266D1, 0x62638900,
+0x3678672C, 0x7703DE38, 0x47212D61, 0x64E2D635,
+0xA00E4721, 0x6562E100, 0x62537101, 0x74012450,
+0x24204219, 0x45297401, 0x74012450, 0x24504519,
+0x621C7401, 0x8BEE3273, 0x66E24200, 0x420061D1,
+0x2118362C, 0x2E628F06, 0xDD1CD728, 0xE501E400,
+0x2D522742, 0x000B6EF6, 0x2FD66DF6, 0x4F222FE6,
+0xED0AEE01, 0x64E3BC97, 0xBC9C64E3, 0x62EC7E01,
+0x8BF732D7, 0xBC9FEE01, 0x64E364E3, 0x7E01BCA4,
+0x32D762EC, 0x4F268BF7, 0x000B6EF6, 0xD1186DF6,
+0xD418920D, 0x72122122, 0x2422D617, 0xD7177204,
+0x72202622, 0x2722D116, 0x000B7230, 0x137A2122,
+0x0011656E, 0x00115ED6, 0x001E1015, 0x00116574,
+0x001E1001, 0x00116548, 0x001E1100, 0x00116572,
+0x00116560, 0x001E1000, 0x00116564, 0x00116570,
+0x00115DCA, 0x001E100C, 0x0011655C, 0x00116578,
+0x0011657C, 0x00116580, 0x00116584, 0x00116588,
+0x0011658C, 0x4F222FE6, 0xD6507FFC, 0x88016060,
+0xE2018951, 0x2620BFBB, 0xD54ED14D, 0xDE4E6010,
+0x64E36552, 0x7402C840, 0x8D22D14C, 0xD24C7502,
+0xE601D74C, 0xE7042722, 0x76016255, 0x626C2421,
+0x8FF93273, 0xD4437402, 0x6242E601, 0x640D8528,
+0x67494419, 0x275D657E, 0x81E4607C, 0xE417D542,
+0x67557601, 0x3243626C, 0x8FF92171, 0xA0207102,
+0xD23E0009, 0xE601D73B, 0xE7042722, 0x76016255,
+0x626C2421, 0x8FF93273, 0xD4327402, 0x6242E601,
+0x640D8528, 0x67494419, 0x275D657E, 0x81E4607C,
+0xE417D533, 0x67557601, 0x3243626C, 0x8FF92171,
+0x924A7102, 0xD2262E21, 0x5E23D72E, 0x64F22FE2,
+0x604365F2, 0x2700C980, 0xC9606043, 0x80716103,
+0xC9036043, 0x80724519, 0x65F2605C, 0x817266F2,
+0x46194629, 0x606C4529, 0x4018645C, 0x8173304C,
+0x21185E23, 0x64F22FE2, 0x6E4C62F2, 0x602C4219,
+0x66F262F2, 0x46294018, 0x461930EC, 0x42298174,
+0x652C606C, 0x305C4018, 0x81758F07, 0x0009BC9D,
+0x2228620C, 0xA00A8908, 0x60130009, 0x8B038840,
+0x0009B009, 0x0009A003, 0xE202D60F, 0x7F042622,
+0x000B4F26, 0x000B6EF6, 0x060A0009, 0x00116590,
+0x001E1000, 0x0011657C, 0x00116774, 0x00116780,
+0x00116718, 0x00116564, 0x00116748, 0x00116746,
+0x0011671A, 0x00116548, 0x00116574, 0x4F222FE6,
+0x84E9DE8E, 0x2448640C, 0xB18B8901, 0xD28C0009,
+0x26686620, 0x60E08902, 0x2E00C9BF, 0x000B4F26,
+0x000B6EF6, 0x2FE60009, 0xDE864F22, 0x60E0D686,
+0xCBC0D486, 0x62602E00, 0xC803602C, 0x40218904,
+0x70014021, 0x6603A002, 0x66034009, 0xD680616D,
+0xE500A004, 0x75016262, 0x74042422, 0x3213625D,
+0xD27C8BF8, 0x0009420B, 0xC9BF84E2, 0x4F2680E2,
+0x6EF6000B, 0x2FE62FD6, 0x7FFC4F22, 0x6260D676,
+0x89402228, 0xD56DE100, 0x60502610, 0xCB40D473,
+0x2500440B, 0x8D052008, 0x62E06E03, 0x7104612C,
+0x2F11A006, 0xD46ED666, 0xDD6E6760, 0x657C4D0B,
+0xE23C6D1D, 0x8B033D27, 0xD26CD46B, 0x0009420B,
+0x4D214D21, 0xA005D76A, 0x66E6E400, 0x357C4508,
+0x74012562, 0x35D3654D, 0xD7668BF7, 0x6E72E003,
+0x81E14018, 0x6E7260F1, 0x81E2700C, 0xD4626172,
+0xDD628113, 0x65724D0B, 0xD652D261, 0x2212E101,
+0xC93F6060, 0x7F042600, 0x6EF64F26, 0x6DF6000B,
+0x2FC62FB6, 0x2FE62FD6, 0xD25A4F22, 0x6B436E73,
+0x420B6C53, 0x20086D63, 0x61038F08, 0xD24FD456,
+0x6EF64F26, 0x6CF66DF6, 0x6BF6422B, 0x21B060C3,
+0x60D38011, 0xE5008111, 0x64BCA007, 0x6053655D,
+0x665300EC, 0x7501361C, 0x625D8064, 0x8BF53243,
+0x6060D636, 0x2600C9BF, 0x6EF64F26, 0x6CF66DF6,
+0x6BF6000B, 0x7FC44F22, 0x720262F3, 0x22512F41,
+0x45297202, 0x60632251, 0xE5C4E682, 0x67F38121,
+0x655C666C, 0xE408BFBC, 0x4F267F3C, 0x0009000B,
+0x2F962F86, 0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6,
+0xE1007FC4, 0x6513ECFF, 0x6B136CCD, 0xDE34D733,
+0xEDFF64F3, 0xD833EA04, 0x6053655C, 0x027D4000,
+0x32C0622D, 0x66038D0D, 0x09ED6063, 0x2491027D,
+0x24217402, 0x698202ED, 0x3928622D, 0x74022892,
+0x75017104, 0x6063625C, 0x07D532A2, 0x0EB58FE4,
+0x2448641C, 0xE6808905, 0x67F3E5C5, 0xBF7F666C,
+0x7F3C655C, 0x6EF64F26, 0x6CF66DF6, 0x6AF66BF6,
+0x000B69F6, 0xD11C68F6, 0x6012D21C, 0xCB20E405,
+0x2102E500, 0x000B2242, 0x00002252, 0x001E1017,
+0x001164F6, 0x001E1015, 0x001E10BF, 0x00117800,
+0x001E10FC, 0x001140CC, 0x001164FC, 0x0011602E,
+0x001166D0, 0x00114F2C, 0x001166EC, 0x00114EBE,
+0x0011788C, 0x001164F8, 0x001160DC, 0x001145BC,
+0x001E2130, 0x00115FF0, 0x001166F4, 0x00116510,
+0x00116518, 0x00116710, 0x001C3500, 0x001D4004,
+0xD565D164, 0xE400D765, 0x2142E20F, 0x17411154,
+0xD5632722, 0x9669D763, 0x15412572, 0x96661562,
+0xE6011565, 0xD5601165, 0x666CE6F8, 0x25422542,
+0x25422542, 0x25422542, 0x25622542, 0x7601E727,
+0x67632572, 0x25627797, 0xE7042572, 0x2572E248,
+0xE2192522, 0xE2702522, 0x25422542, 0x25422542,
+0x25222542, 0x2522E20C, 0x25422542, 0x25422542,
+0x25422542, 0x25422542, 0x000B154A, 0xE2081145,
+0x0009422B, 0x2FE62FD6, 0x7FFC4F22, 0xC8206043,
+0x6E438D02, 0x0009BE75, 0xC81060E3, 0xBE728901,
+0x60E30009, 0x8901C840, 0x0009BE94, 0xC80160E3,
+0xDD3E8938, 0xC80260D0, 0x2F008D03, 0x460BD63C,
+0x60F00009, 0x8902C804, 0x460BD63A, 0x62F00009,
+0xC8806023, 0x60D08902, 0x2D00C97F, 0xC8016023,
+0xD6358906, 0x0009460B, 0x0009A007, 0x51630601,
+0x8902C808, 0x460BD631, 0x60F00009, 0x8902C810,
+0x420BD22F, 0xD52F0009, 0x88026052, 0xD22E8B03,
+0xA005E604, 0x88012260, 0xD22B8B02, 0x2260E601,
+0x2522E200, 0xC88060E3, 0xD628892E, 0x60E36E60,
+0x8902C880, 0x420BD226, 0x60E30009, 0x8902C840,
+0x420BD224, 0x60E30009, 0x8902C802, 0x420BD222,
+0x60E30009, 0x890EC804, 0x410BD120, 0xBF120009,
+0xBF4D0009, 0xD51E0009, 0x6050D41E, 0xC908D71E,
+0xBF842500, 0x60E32472, 0x8905C808, 0x7F04D21B,
+0x6EF64F26, 0x6DF6422B, 0x4F267F04, 0x000B6EF6,
+0x00006DF6, 0x001C581C, 0xA000A000, 0x001D0100,
+0x001D4000, 0x00040021, 0x001C589C, 0x001E1021,
+0x001150C4, 0x001150E6, 0x00115724, 0x001150FE,
+0x0011510C, 0x00116574, 0x001E100B, 0x001E1028,
+0x00115162, 0x0011516E, 0x00115114, 0x00115132,
+0x001E1000, 0x0010F100, 0x12345678, 0x0011514A,
+0x644CD6A7, 0x000B346C, 0xD6A62450, 0x346C644C,
+0x2450000B, 0x644CD6A4, 0x000B346C, 0x625C2450,
+0x4208616D, 0x42084119, 0x42006019, 0x670E614C,
+0xD49E321C, 0x4200207D, 0x324CC90F, 0x2200000B,
+0x4208625C, 0x42004208, 0x324C644C, 0x4200D498,
+0x000B324C, 0x2FE62260, 0x614C4F12, 0x4100D493,
+0x6710314C, 0xE29F666D, 0x27294619, 0x6E536269,
+0x672E6573, 0x4221227D, 0x42214221, 0x7601662C,
+0xE4014608, 0x34E84608, 0x644C4600, 0x071A0467,
+0x2150257B, 0x000B4F16, 0x4F226EF6, 0xD2857FE8,
+0x88016021, 0xD2848B7B, 0x26686621, 0xD2838B77,
+0x26686621, 0xE50F8B73, 0xE401BFA2, 0xBFA4E501,
+0xE586E400, 0xE400655C, 0x2F50BFA4, 0xBFA1E401,
+0xE602E506, 0x60634618, 0x81F2E401, 0x6543BF9F,
+0xE40185F2, 0xBFAB6543, 0x85F26603, 0x6543E401,
+0x6603BFB1, 0xE40265F0, 0x6053756C, 0x80F8BF80,
+0xBF82E402, 0x84F8E512, 0x7090E402, 0x6503BF82,
+0x4618E602, 0x81F66063, 0xBF80E402, 0x85F6E500,
+0x6603E402, 0xE500BF8C, 0xE40285F6, 0xBF926603,
+0xE5FEE500, 0xE010655C, 0xBF61E403, 0xE5130F54,
+0xE40EBF63, 0x05FCE010, 0xBF63E40E, 0xE5007585,
+0xBF64E403, 0xE500E640, 0xBF71E403, 0xE500E640,
+0xBF78E403, 0xE5FFE640, 0xE014655C, 0xBF47E404,
+0xE40F0F54, 0xE504BF49, 0x05FCE014, 0xBF49E40F,
+0xE5017584, 0xBF4AE640, 0xE501E404, 0xBF57E640,
+0xE501E404, 0xE404E640, 0xAF5C7F18, 0x7F184F26,
+0x000B4F26, 0x4F220009, 0xD2427FF0, 0x88016021,
+0xD2418B71, 0x26686621, 0xD2408B6D, 0x26686621,
+0xE50F8B69, 0xE401BF1C, 0xBF1EE501, 0xE586E400,
+0xE400655C, 0x2F50BF1E, 0xBF1BE401, 0xE401E506,
+0xBF1C6543, 0xE401E640, 0xBF296543, 0xE401E640,
+0xBF306543, 0x65F0E640, 0x756CE402, 0xBEFF6053,
+0xE40280F4, 0xE512BF01, 0xE40284F4, 0xBF017090,
+0xE6406503, 0xBF02E402, 0xE640E500, 0xBF0FE402,
+0xE640E500, 0xBF16E402, 0xE5FEE500, 0x6053655C,
+0xBEE5E403, 0xE51380F8, 0xE40EBEE7, 0xE40E84F8,
+0xBEE77085, 0xE5006503, 0xBEE8E640, 0xE500E403,
+0xBEF5E640, 0xE500E403, 0xBEFCE640, 0xE5FFE403,
+0x6053655C, 0xBECBE404, 0xE40F80FC, 0xE504BECD,
+0xE40F84FC, 0xBECD7083, 0xE5016503, 0xBECEE640,
+0xE501E404, 0xBEDBE640, 0xE501E404, 0xE404E640,
+0xAEE07F10, 0x7F104F26, 0x000B4F26, 0x00000009,
+0x001E102F, 0x001E1080, 0x001E1090, 0x001E103F,
+0x001E103E, 0x0011656E, 0x00116570, 0x00116572,
+0xD21DD11C, 0x66206010, 0x676C7001, 0x3700C90F,
+0xE5008D13, 0x67106210, 0x7701622C, 0x64232170,
+0xD6166010, 0x44084408, 0x3428C90F, 0x62602100,
+0x7201D513, 0x44082620, 0x000B354C, 0xD10F6053,
+0x25586510, 0xE6008D13, 0xD60DD40B, 0x655C6540,
+0x47086753, 0x37584708, 0x47086540, 0x24507501,
+0x367C6040, 0x2400C90F, 0x72FF6210, 0x000B2120,
+0x00006063, 0x001164F5, 0x001164F4, 0x001164F6,
+0x0011611C, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x544F0D0A, 0x53205355, 0x46204950,
+0x00003A57, 0x2074634F, 0x32203220, 0x20373030,
+0x333A3831, 0x36343A32, 0x00000000, 0x00000D0A,
+0x42707372, 0x3D206675, 0x554E203D, 0x202C4C4C,
+0x6E49677A, 0x4E497274, 0x6D754E51, 0x0000003D,
+0x52504545, 0x57204D4F, 0x65746972, 0x6461202C,
+0x003D7264, 0x6C617620, 0x0000003D, 0x00000A0D,
+0x5A205746, 0x4D435F4D, 0x4C465F44, 0x5F485341,
+0x53415245, 0x000A0D45, 0x5A205746, 0x4D435F4D,
+0x4C465F44, 0x5F485341, 0x534B4843, 0x0A0D4D55,
+0x00000000, 0x2D495053, 0x72646461, 0x0000003D,
+0x2D495053, 0x676E656C, 0x003D6874, 0x2D495053,
+0x736B6863, 0x003D6D75, 0x5A205746, 0x4D435F4D,
+0x4C465F44, 0x5F485341, 0x44414552, 0x00000A0D,
+0x61202072, 0x3D726464, 0x00000000, 0x72202020,
+0x75427073, 0x00003D66, 0x6E6B6E55, 0x206E776F,
+0x6D6D6F63, 0x3D646E61, 0x00000000, 0x00000072,
+0x00205220, 0x00000D0A, 0x62735576, 0x7473725F,
+0x00000A0D, 0x62735576, 0x7375735F, 0x646E6570,
+0x00000A0D, 0x62735576, 0x7365725F, 0x000A0D6D,
+0x72746E49, 0x6D652051, 0x2C797470, 0x49677A20,
+0x4972746E, 0x754E514E, 0x00003D6D, 0x654C7245,
+0x0000006E, 0x20746F4E, 0x756F6E65, 0x49206867,
+0x4220514E, 0x0A0D6675, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x002E0209, 0x80000101,
+0x000409FA, 0x00FF0400, 0x05070000, 0x02000201,
+0x82050700, 0x00020002, 0x03830507, 0x07010040,
+0x40020405, 0x02090000, 0x0101002E, 0x09FA8000,
+0x04000004, 0x000000FF, 0x02010507, 0x07000040,
+0x40028205, 0x05070000, 0x00400383, 0x04050701,
+0x00004002, 0x00000000, 0x00000000, 0x07090000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, };
+
+const u32_t zcFwImageSPISize=10156;
diff --git a/drivers/staging/otus/hal/hpfwu.c b/drivers/staging/otus/hal/hpfwu.c
new file mode 100644
index 00000000000..2b77cbacc6d
--- /dev/null
+++ b/drivers/staging/otus/hal/hpfwu.c
@@ -0,0 +1,1017 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include "cprecomp.h"
+
+const u32_t zcFwImage[] = {
+0x0009000B, 0x4F222FE6, 0xDE947FFC, 0xE114D594,
+0x1E13D494, 0x67521E4C, 0xD494D693, 0x37402769,
+0x62528F06, 0x7201D692, 0x60602522, 0x2600C93F,
+0xD7906152, 0x2512611D, 0x264B6652, 0x2562470B,
+0x0009B017, 0xE60095AC, 0xC84060E2, 0x2F028F03,
+0x8FF93652, 0xD4887601, 0x4E0BDE88, 0xD4880009,
+0x00094E0B, 0x4E0BD487, 0x7F040009, 0xA0524F26,
+0x4F226EF6, 0x410BD184, 0xD4840009, 0x0009440B,
+0x450BD583, 0xD7830009, 0xD283E1FF, 0x2712611D,
+0xD4825029, 0xE1FFCB01, 0x1209E501, 0x12112212,
+0xE7202452, 0x4718D57E, 0x2572D27E, 0xD17EE700,
+0xD67FD47E, 0xE2012270, 0x24702172, 0xD67D2620,
+0x2641E4FF, 0xD57CE600, 0x666DE104, 0x76016063,
+0x4000626D, 0x8FF83212, 0xD5780545, 0x2520E201,
+0xD278D777, 0xE480E100, 0x22122710, 0x6613D576,
+0x666D644C, 0x76046763, 0x375C626D, 0x8FF83243,
+0xD5722712, 0xD273D772, 0xE400E101, 0x27102511,
+0x000B4F26, 0x7FCC2242, 0xD170D56F, 0xD271DB70,
+0x1F51D471, 0xD6717508, 0x1F12D771, 0x1F55710C,
+0x1FB975FC, 0x72041F2A, 0x1F13EB10, 0x1F561F44,
+0x1F781F67, 0xD86B1F2B, 0xDD6CD96B, 0xDC6CEA00,
+0xD26DDE6C, 0x89003A22, 0xD15D7A01, 0x88016010,
+0x56F88B03, 0x4218E201, 0xD1682622, 0x0009410B,
+0x440BD467, 0xD5670009, 0x0009450B, 0x6010D150,
+0x8B108801, 0xE650D14F, 0x46186212, 0x8B083266,
+0x56F9D14B, 0x2120E200, 0xCB016062, 0x2602A003,
+0x72012710, 0x60822122, 0x89098801, 0xE2C8D15A,
+0x622C6612, 0x89033626, 0x6010D158, 0x8BC88801,
+0x51F66792, 0x217252F5, 0xD6555191, 0x55FA2212,
+0x52FB6462, 0x55612542, 0x2252E400, 0x61436643,
+0x05DE6013, 0x36CC4608, 0x07DE2652, 0xC9036071,
+0x8B028801, 0x720162E2, 0x74012E22, 0x36B3664C,
+0x71048FEE, 0x66C2D147, 0x45286512, 0x265B4518,
+0x60822C62, 0x89018801, 0x0009A168, 0x6272D742,
+0x8B132228, 0xD42BD741, 0x6772D541, 0x51536242,
+0x312C327C, 0x24222228, 0x15138D05, 0x6262D63D,
+0xB1627201, 0xD6232622, 0x2622E200, 0x52916692,
+0x8B013620, 0x0009A144, 0x6061A06E, 0x001C001C,
+0x001D4020, 0x0000B38E, 0xFFFF0000, 0x12340000,
+0x001E1015, 0x00201274, 0x002039F4, 0x002018A2,
+0x00203A00, 0x00203A18, 0x00201860, 0x0020196C,
+0x00201288, 0x001C3510, 0x001C3624, 0x001E212C,
+0x002038F4, 0x0020348C, 0x002038FC, 0x00203908,
+0x00203914, 0x00203970, 0x00203974, 0x0020391C,
+0x0020391D, 0x00203920, 0x00117700, 0x0020398C,
+0x0020398A, 0x002034F0, 0x00117710, 0x001C3D30,
+0x001C36F8, 0x00117734, 0x001C3684, 0x001C3D00,
+0x001C1000, 0x001C1028, 0x00203504, 0x00203924,
+0x00117600, 0x00117740, 0x7FFFFFFF, 0x00201730,
+0x0020332A, 0x00202334, 0x00203DA4, 0x00203972,
+0x002034FC, 0x00203964, 0x001C3D2C, 0x001C36B0,
+0x00203494, 0x0011775C, 0x8801C90F, 0xA0CF8901,
+0xD1960009, 0x36206212, 0xD4958904, 0x2421E200,
+0x2162A0CC, 0x6211D193, 0x89012228, 0x0009A0C3,
+0xE202D78F, 0x75016571, 0x3123615D, 0x27518D02,
+0x0009A0BC, 0xD28C57F2, 0x62226072, 0x40094019,
+0xC90F4009, 0x8F19880A, 0x52F31F2C, 0x40196022,
+0x40094009, 0x8808C90F, 0xA0A78901, 0x60630009,
+0xCB0154F7, 0xD27E55F2, 0xE7012402, 0xD47FE100,
+0x22112572, 0x72016242, 0x2422A098, 0x8B3F8805,
+0x602252F3, 0x40094019, 0xC90F4009, 0x8B168802,
+0xE4FFD577, 0x644D6752, 0x8B102748, 0x6272D775,
+0x8B0C3260, 0x51F255F7, 0xD26DE701, 0x21722562,
+0xD571E100, 0x64522211, 0xA0777401, 0x52F32542,
+0x40196022, 0x40094009, 0x8805C90F, 0x31B38B6E,
+0xD26A8B6C, 0x672254F4, 0x7701D569, 0x61422272,
+0x1F1CE640, 0x46182159, 0x8B033160, 0x6262D665,
+0x26227201, 0xE200D65A, 0x2621B067, 0x0009A056,
+0x3123E220, 0x88038B52, 0x52F38B1E, 0x40196022,
+0x40094009, 0x8803C90F, 0xD25B8B16, 0x672254F4,
+0x7701D557, 0x61422272, 0x1F1CE640, 0x46182159,
+0x8B033160, 0x6262D655, 0x26227201, 0xE200D648,
+0x2621B043, 0x0009A010, 0xD452D551, 0xD2446752,
+0xE1007701, 0x25723A46, 0x22118F06, 0xEA00D64E,
+0x72016262, 0x2622B031, 0x2FB2D54C, 0x95736652,
+0xD44A5BF1, 0x36205241, 0x60618910, 0x8B01C803,
+0x2B22E201, 0x8FF54510, 0x57F15664, 0x6272E1F0,
+0x41284118, 0x2722221B, 0x6BF2A008, 0x6BF2A006,
+0xE200D62F, 0xD12F2621, 0x2121E200, 0xD13CE201,
+0x66122822, 0x8B012668, 0x0009AE2B, 0x450BD539,
+0xD1390009, 0xAE24E600, 0x2F862160, 0x2FA62F96,
+0x2FC62FB6, 0x2FE62FD6, 0x7FF44F22, 0xDE34D133,
+0x54116212, 0x1F4167E2, 0x2F22D432, 0xD5321F72,
+0xD2326743, 0x58417794, 0x69425A42, 0x5B166C72,
+0x60526D22, 0xCB20E600, 0xE5402502, 0x626D7601,
+0x8BFB3253, 0x55F162F2, 0x11512122, 0xD62855F2,
+0x14812E52, 0x249214A2, 0x27C2D426, 0x26D211B6,
+0xDA256742, 0xE801D925, 0x490B2A72, 0xE2011A8C,
+0x1A2C4218, 0x4F267F0C, 0x6DF66EF6, 0x6BF66CF6,
+0x69F66AF6, 0x68F6000B, 0x000007D1, 0x00203984,
+0x00203988, 0x0020398E, 0x001C3DC0, 0x0011772C,
+0x001C3B88, 0x0020396C, 0x0011773C, 0x00117744,
+0x0000F000, 0x00117764, 0x00117748, 0x00117768,
+0x0011776C, 0x01FFFFFF, 0x0011774C, 0x002034FC,
+0x00203DA4, 0x002024F8, 0x00203972, 0x001C3B9C,
+0x001C3D98, 0x001C3700, 0x001C3500, 0x001C5960,
+0x001C8960, 0x00203504, 0x001C3D00, 0x0020160C,
+0x2F962F86, 0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6,
+0xDE957FAC, 0x61E0E014, 0x0F14D494, 0x710161E3,
+0xE0186210, 0xD2920F24, 0x0009420B, 0x450BD591,
+0x20080009, 0x8F126D03, 0xD28F1F07, 0x6720D48F,
+0x657CDD8F, 0x470BD78F, 0xD18F0009, 0x619C6910,
+0x46086613, 0x36184608, 0x3D6C4608, 0xE0181FD7,
+0xE58004FC, 0x604C66E2, 0x3050655C, 0x2D628F15,
+0x01FCE014, 0xDE85E500, 0x641CA008, 0x6753655D,
+0x607037EC, 0x39DC6953, 0x80947501, 0x3243625D,
+0xD67F8BF4, 0xA34EE200, 0x20082621, 0xE0148B13,
+0xE40001FC, 0xA009DE79, 0x644D671C, 0x35EC6543,
+0x69436652, 0x39DC6262, 0x74041921, 0x3273624D,
+0xA3388BF3, 0x88012D10, 0xE0148B17, 0xE70001FC,
+0x6D1C2D70, 0xDE6D1FD4, 0x32D3627D, 0xA32A8B01,
+0x677D0009, 0x667365E3, 0x61737504, 0x315C36EC,
+0x69126462, 0xAFEF7708, 0x88042492, 0xE0148B18,
+0xE40001FC, 0x671C2D40, 0x624DDE60, 0x8B013273,
+0x0009A311, 0x6943644D, 0x39EC62E3, 0x72046592,
+0x3D2C6D43, 0x615266D2, 0x21697408, 0x2512AFED,
+0x8B188805, 0x01FCE014, 0x2D40E400, 0xDE53671C,
+0x3273624D, 0xA2F68B01, 0x644D0009, 0x62E36943,
+0x659239EC, 0x6D437204, 0x66D23D2C, 0x74086152,
+0xAFED216B, 0x88312512, 0xD44A8B3A, 0x6146D94A,
+0x75046543, 0x67566442, 0x6E531F48, 0x65527E04,
+0x7EE462E2, 0x7E0464E2, 0x6EE21FE9, 0x5EF929E0,
+0x7E04D942, 0x1FEA60E2, 0x2900C901, 0xD9406EE2,
+0x29E04E09, 0x2F562F26, 0x56FAD93E, 0x6513490B,
+0xD13D7F08, 0xE71C6E0D, 0x1DE12D70, 0xDE3B6912,
+0x64E21D92, 0x1D43D13A, 0xD23A6512, 0x67221D54,
+0x1D75D239, 0x1D666622, 0x6262D638, 0x1D27A2AB,
+0x8B398830, 0x6596D92B, 0x67926696, 0x61967904,
+0x74E46493, 0x6E436992, 0x1F9B7E04, 0x1FEC6442,
+0xD9256EE2, 0x5EFC29E0, 0x7E04D924, 0x1FED60E2,
+0x2900C901, 0xD9226EE2, 0x29E04E09, 0x59FC7FFC,
+0xDE272F92, 0x2F164E0B, 0xD41F7F08, 0xE21C610D,
+0x1D112D20, 0xD2206442, 0xD41C1D42, 0x1D536542,
+0x6752D51B, 0xD71B1D74, 0x1D156172, 0x1D666622,
+0x6262D61A, 0x1D27A26F, 0x8B358833, 0x490BD919,
+0xA268EE00, 0x00002DE0, 0x00117800, 0x00203A1C,
+0x002018A2, 0x00202AAC, 0x0020390E, 0x00203A20,
+0x00203534, 0x002018EE, 0x0020390D, 0x00117804,
+0x0020398C, 0x00117810, 0x00203909, 0x0020390A,
+0x0020390B, 0x00200F64, 0x001C5864, 0x001C6864,
+0x001C7864, 0x001C59BC, 0x001C69BC, 0x001C79BC,
+0x00200FBC, 0x00200FB8, 0x89018828, 0x0009A0C0,
+0xE643DEB5, 0x326662E1, 0x1FEE8F02, 0x2E21E240,
+0x622D62E1, 0x8B013267, 0x0009A0AA, 0xE50185E1,
+0x8B013056, 0x0009A0A4, 0x2D10E101, 0x64E1B225,
+0xE64357FE, 0x652D6271, 0x89443567, 0x3563E640,
+0xE6008B05, 0x0F65E040, 0xA00FE11A, 0x615372C0,
+0x41214121, 0x41214121, 0x45214121, 0x45214521,
+0xC9036053, 0xE0406603, 0x71180F65, 0x2209E007,
+0x0F25E03C, 0xE044641D, 0xB2A365F3, 0xE33C0F46,
+0x853233FC, 0x620DDE95, 0x42086031, 0x6023610D,
+0x1323E944, 0x06FE4108, 0xE00F39FC, 0x13144E0B,
+0x67075D91, 0x60D32679, 0x0F6654FE, 0x51928542,
+0x600D4E0B, 0x60D3260B, 0x0F666492, 0x65F3B237,
+0x696156FE, 0xE640659D, 0x89383563, 0xD78359FE,
+0x79066591, 0xC9036053, 0x40004008, 0x61036203,
+0x0F26E050, 0x470BE0FF, 0x6C07600C, 0x6603605D,
+0x46214621, 0x46214621, 0x42006263, 0x4200326C,
+0x40214021, 0x4008C903, 0x6D2D30FC, 0xE8006A03,
+0xB25765F3, 0x6EA264D3, 0x2EC9E050, 0x66942AE2,
+0xD76E01FE, 0x606C470B, 0x2AE22E0B, 0x64D365F3,
+0x7801B1FD, 0xEE06628D, 0x8FE932E3, 0x5EFE7D01,
+0x61E1E400, 0x410085E1, 0x66E3310C, 0x760C711B,
+0xE70465F3, 0x68667401, 0x3A736A4D, 0x8FF92582,
+0x65F37504, 0x641DB1E3, 0x64E1B1A4, 0x0009A17B,
+0xD45B56F7, 0xEC01D25B, 0x26C0420B, 0x0009A173,
+0x06FCE018, 0x8829606C, 0x58F78B08, 0xE400D252,
+0x66222840, 0x646DB171, 0x0009A165, 0x666CE681,
+0x89013060, 0x0009A0AC, 0xD550D14F, 0x62126A56,
+0x212232AC, 0x54116C56, 0x34CC6253, 0x64521141,
+0x72085812, 0xD44A384C, 0x68221182, 0x5A136C42,
+0x3ACC3C8C, 0x11A324C2, 0x6C2272EC, 0x72105814,
+0x118438CC, 0x5A156822, 0x11A53A8C, 0x6A227210,
+0xD6405816, 0x118638AC, 0x52176C62, 0x112732CC,
+0x5A185861, 0x11A83A8C, 0x5C195A62, 0x11C93CAC,
+0x521A5C63, 0x112A32CC, 0x5A1B5864, 0x11AB3A8C,
+0x5C1C5A65, 0x11CC3CAC, 0x521D5C66, 0x112D32CC,
+0x5A1E5867, 0x11AE3A8C, 0x561F5A68, 0x36ACE840,
+0x116FDA2D, 0x6CA2381C, 0x7A946682, 0x286236CC,
+0x5C8162A2, 0x18C13C2C, 0x62A27A44, 0x362C5682,
+0xD6261862, 0x5A856262, 0x3A2C4229, 0x760418A5,
+0x56866262, 0x362C4229, 0x56F71866, 0x2620E238,
+0x16C15C81, 0x16226212, 0xE2005C11, 0x551216C3,
+0x55151654, 0x55131655, 0x55161656, 0x55821657,
+0x65821658, 0x55141659, 0x5584165A, 0x5583165B,
+0x5585165C, 0x5586165D, 0x1821165E, 0x11212122,
+0x11251122, 0x11261123, 0x28221822, 0x18241124,
+0x18251823, 0x1826A0C7, 0x00117804, 0x002033E8,
+0x00203A40, 0x002018A2, 0x00203494, 0x001C36A0,
+0x002034F0, 0x001C3CA0, 0x001C36F4, 0x001C3B88,
+0x666CE682, 0x8B203060, 0xEA2456F7, 0x26A0D194,
+0x16C15C17, 0x16225218, 0x16835819, 0x16A45A1A,
+0x16C55C1B, 0x1626521C, 0xE200581D, 0x551E1687,
+0x551F1658, 0x11271659, 0x11291128, 0x112B112A,
+0x112D112C, 0xA08E112E, 0xE683112F, 0x3060666C,
+0x52F78B0B, 0xEA00D883, 0x658222A0, 0x7804DC82,
+0x62822C52, 0xA07ED681, 0xE6902620, 0x3060666C,
+0xDA7F8B06, 0x00094A0B, 0xE20056F7, 0x2620A073,
+0x666CE691, 0x8B103060, 0x6222D276, 0x2228622C,
+0xD2788904, 0x0009420B, 0x0009A003, 0x420BD276,
+0x56F70009, 0xA05EE200, 0xE6922620, 0x3060666C,
+0xE0188951, 0xE6B00BFC, 0x666C62BC, 0x8B2A3260,
+0x02FCE014, 0x682CEA00, 0x62ADE904, 0x894A3283,
+0x6AADDD64, 0x3CDC6CA3, 0x7D046EC2, 0xDB68D467,
+0x32DC62A3, 0x4B0BDC67, 0x4C0B6D22, 0xD46664E3,
+0x00094B0B, 0x64D34C0B, 0x4B0BD464, 0xE6000009,
+0x666D6BE3, 0x76013B6C, 0x3293626D, 0x8FF72BD0,
+0xAFDA4D19, 0xE6B57A08, 0x3260666C, 0xD45C8B13,
+0x4B0BDB57, 0xD25B0009, 0x6022DB5B, 0xCB20E6FF,
+0x2202666D, 0xDB592B62, 0xE014E200, 0x56F72B20,
+0xA01002FC, 0xD4562620, 0x6542D256, 0x420BD456,
+0xA0080009, 0xDB520009, 0x52B1E600, 0x622CDB53,
+0x52F72B21, 0x7F542260, 0x6EF64F26, 0x6CF66DF6,
+0x6AF66BF6, 0x000B69F6, 0x4F2268F6, 0xE240614D,
+0x89143123, 0x3127E21F, 0x8B09D749, 0xD449614D,
+0xE00171E0, 0x5671440B, 0x26596507, 0x1761A007,
+0xE001D444, 0x6672440B, 0x26596507, 0x4F262762,
+0x0009000B, 0x614D4F22, 0x3123E240, 0xE21F8912,
+0xD73B3127, 0x614D8B08, 0x5671D23A, 0x420B71E0,
+0x260BE001, 0x1761A006, 0x6672D236, 0xE001420B,
+0x2762260B, 0x000B4F26, 0xE6400009, 0x46284618,
+0x6252D531, 0x89FC2268, 0x0009000B, 0x4618E680,
+0xD52D4628, 0x22686252, 0x000B89FC, 0xA0010009,
+0x7201E200, 0x8BFC3242, 0x0009000B, 0x4618E680,
+0xD5264628, 0x22686252, 0x000B8BFC, 0x2FE60009,
+0x7FFC4F22, 0xBFF16E53, 0x61E22F42, 0xE280D620,
+0x54E11615, 0x16464218, 0x422855E2, 0x57E31657,
+0x16786EF2, 0x26E22E2B, 0x4F267F04, 0x6EF6AFCE,
+0x00203494, 0x00117804, 0x002038F4, 0x00203908,
+0x0020050A, 0x00201008, 0x0020102E, 0x00203A58,
+0x002018A2, 0x002018E6, 0x00203A6C, 0x00203A74,
+0x00203A78, 0x001C3500, 0x001C1000, 0x0020398A,
+0x00117800, 0x002018EE, 0x00203A8C, 0x00203990,
+0x001C3704, 0x002033E8, 0x001C373C, 0x001C3700,
+0x001C370C, 0x2FD62FC6, 0x4F222FE6, 0x6C53DD10,
+0x6E43BFA4, 0x2DE2BF89, 0x0009BFA0, 0x2C1251D5,
+0x1C4154D6, 0x1C5255D7, 0x1C6356D8, 0x6EF64F26,
+0x000B6DF6, 0x61636CF6, 0xA004E600, 0x62564109,
+0x24227601, 0x36127404, 0x000B8BF9, 0x00000009,
+0x001C370C, 0x0009A16E, 0x2FE62FD6, 0xDD944F22,
+0xA0049EB2, 0xD4930009, 0x420BD293, 0x62D265D2,
+0x8BF822E8, 0x0009A004, 0xD28FD490, 0x55D1420B,
+0x22E852D1, 0xA0048BF8, 0xD48D0009, 0x420BD28A,
+0x52D255D2, 0x8BF822E8, 0x0009A004, 0xD286D489,
+0x55D3420B, 0x22E852D3, 0xA0048BF8, 0xD4860009,
+0x420BD281, 0x52D455D4, 0x8BF822E8, 0x6EF64F26,
+0x6DF6000B, 0x2FD62FC6, 0x4F222FE6, 0x6E636C73,
+0x6D53B01A, 0x64D357F4, 0xB05F65E3, 0xB07566C3,
+0xB0A40009, 0xB0A80009, 0xB0AC0009, 0xB0AC0009,
+0xB0AF0009, 0xB03154F5, 0x6CCD6C03, 0x4F2660C3,
+0x6DF66EF6, 0x6CF6000B, 0x3412D170, 0xD6700529,
+0x2650D770, 0x2742000B, 0x0009A018, 0x2FD62FC6,
+0x4F222FE6, 0x6E636C73, 0x6D53BFEE, 0x64D357F4,
+0xB03365E3, 0xB08D66C3, 0xB00F54F5, 0x6CCD6C03,
+0x4F2660C3, 0x6DF66EF6, 0x6CF6000B, 0xE503D162,
+0xD763D462, 0x21524518, 0x2472000B, 0xD45FD15E,
+0x2162E600, 0x2462000B, 0xBF734F22, 0xBF73E40A,
+0xD25C0009, 0x4118E104, 0xE40AE500, 0xBF692212,
+0xD7592252, 0xCB206072, 0x000B4F26, 0x4F222702,
+0x410BD156, 0xD556E400, 0x4F26452B, 0xD1552FE6,
+0x66126E63, 0x92104418, 0x44084528, 0x45002629,
+0x265B4408, 0x264B4400, 0x21624708, 0xD14E4708,
+0x217227EB, 0x6EF6000B, 0x1FFF03F0, 0x4F222FE6,
+0xE101DE4A, 0xBF3DE40A, 0x67E32E12, 0xE500776C,
+0xE204E130, 0x2752E40A, 0x27522752, 0x27522752,
+0x27522752, 0x27522752, 0x27522752, 0x27522752,
+0x27522752, 0x27522752, 0x27522752, 0x27222712,
+0x27522752, 0x27522752, 0x27522752, 0x27522752,
+0x175ABF18, 0x2E62E600, 0x000B4F26, 0xD2346EF6,
+0xE441E101, 0x000B2212, 0xD1322242, 0xE605D432,
+0x000B2162, 0x000B2462, 0xD2300009, 0xE40AE601,
+0x2262AF00, 0x2FC62FB6, 0x2FE62FD6, 0x7FFC4F22,
+0x6C43DB2B, 0xED0060B2, 0x2B02CB03, 0xC90360B2,
+0x6E03A008, 0x89073DC2, 0xE46460B2, 0xB07CC903,
+0x7D016E03, 0x8BF52EE8, 0x8F043DC2, 0xD4212FE1,
+0x460BD621, 0x62F10009, 0x6023622D, 0x89FFC801,
+0x7F046023, 0x6EF64F26, 0x6CF66DF6, 0x6BF6000B,
+0x001C3B88, 0x00203AA0, 0x002018EE, 0x00203AA8,
+0x00203AB0, 0x00203AB8, 0x00203AC0, 0x0025E720,
+0x00203DA0, 0x002038F8, 0x001C5968, 0x001C3B40,
+0x000F8000, 0x001D4004, 0x001C3500, 0x002015E0,
+0x0020160C, 0x001C5814, 0x001C59D0, 0x001C5830,
+0x001C6268, 0x001C59A4, 0x001C639C, 0x001C581C,
+0x001C5860, 0x00203AC8, 0x002018A2, 0x8F014411,
+0x6043604B, 0x0009000B, 0x5651D52B, 0x46286052,
+0x306C000B, 0x2FC62FB6, 0x2FE62FD6, 0x4F124F22,
+0xBFF14F02, 0x6B036E43, 0xDD25DC24, 0x0009BFEC,
+0x3C0530B8, 0x4609060A, 0x46014609, 0x020A3D65,
+0x42094209, 0x32E24209, 0x4F068BF0, 0x4F264F16,
+0x6DF66EF6, 0x000B6CF6, 0x2FC66BF6, 0x2FE62FD6,
+0x4F124F22, 0xBFCF4F02, 0x6C036E43, 0xBFCBDD13,
+0x30C80009, 0x060A3D05, 0x46094609, 0x36E24601,
+0x4F068BF5, 0x4F264F16, 0x6DF66EF6, 0x6CF6000B,
+0x4F222FE6, 0xE102DE0B, 0xE403E500, 0xBFB92E12,
+0xE6062E52, 0xE7004618, 0x2E62E403, 0x4F262E72,
+0x6EF6AFB0, 0x0009000B, 0x001C1040, 0xCCCCCCCD,
+0x10624DD3, 0x001D4004, 0x2F962F86, 0x2FB62FA6,
+0x2FD62FC6, 0x4F222FE6, 0xE5007F98, 0x6453E710,
+0x6B534728, 0xEE1ADCBC, 0x6153655D, 0x315C4108,
+0x75014108, 0x6043317C, 0x0F16665D, 0xED0060B3,
+0x21B136E3, 0x81128111, 0x11D28113, 0x11D411D3,
+0x74048FEA, 0xD8B167F2, 0x1871D9B1, 0x58F12872,
+0x1981D1B0, 0x59F22982, 0x5DF45AF3, 0x54F65EF5,
+0x21921191, 0x11A211A3, 0x11D411D5, 0x11E611E7,
+0x11481149, 0xDAA855F7, 0x57F8EE00, 0x52F9DDA7,
+0x64E3D6A7, 0x2A521A51, 0xD8A7D9A6, 0x2D729AD5,
+0x6EED2622, 0x4D086DE3, 0x3DEC61E3, 0x4D084108,
+0x3D9C31EC, 0x410860B3, 0x81D12DB1, 0x4108E050,
+0x4008E7B7, 0x677C4108, 0x60B381D2, 0xE200318C,
+0x81D33472, 0x1D131DD2, 0x8D01D493, 0xD4901D24,
+0xB0B365D3, 0x64ED7E01, 0x8BDA34A2, 0x2FD2DA8C,
+0xDD9268A2, 0x2D824829, 0x7DFC64A2, 0xD287694D,
+0x6E222D92, 0x7D0C4E29, 0x68222DE2, 0x618D6AD3,
+0x2A16D784, 0xD48A6D72, 0x24D2D583, 0xD6895E72,
+0x517414E2, 0x1414EE00, 0xD1875876, 0x59781486,
+0x1498E710, 0x65E36252, 0x26E2142A, 0xE60064E3,
+0x6843644D, 0x384C4808, 0x381C4808, 0x0C866053,
+0x09CE28B1, 0x819160B3, 0x0ACE6053, 0x81A26043,
+0x0DCE6053, 0x81D360B3, 0x08CE6053, 0x18827401,
+0x624D09CE, 0x0ACE19E3, 0x1A643273, 0x75048FE0,
+0xE003D96A, 0x40186C92, 0x6D922CB1, 0x81D1DA6F,
+0x6E92E050, 0x81E24008, 0x60B36192, 0x64928113,
+0x1442E600, 0xD4696792, 0x689217A3, 0x1864E1FF,
+0x6563E703, 0x364C4608, 0x26127501, 0x3673665D,
+0xDC5B8BF8, 0x6DC2E003, 0x2DB14018, 0xD2606EC2,
+0x61C281E1, 0x1112EE00, 0xE02464C2, 0x65C21423,
+0x15E4D45B, 0xE58067C2, 0x68C28172, 0x818366E3,
+0x666D655C, 0x76046963, 0x394C6A6D, 0x8FF83A53,
+0xDB5429E2, 0x24B2DC54, 0x24C27404, 0x4F267F68,
+0x6DF66EF6, 0x6BF66CF6, 0x69F66AF6, 0x68F6000B,
+0x60116142, 0x8F03C803, 0xD23DE500, 0x8B063420,
+0xC9036011, 0x8B068802, 0x3420D239, 0x56128B03,
+0x52646513, 0x000B2422, 0x01136053, 0x2FE62FD6,
+0x7FEC4F22, 0x62536E53, 0x6D43E550, 0x4508E400,
+0xE101A001, 0x60435224, 0x81212211, 0x60538123,
+0x56E28122, 0x8BF53620, 0x16E4D238, 0xE61464F3,
+0x65E3420B, 0xE4FC65E1, 0x2E512549, 0x65F361F1,
+0x2F112149, 0xD13154D1, 0xE614410B, 0x607157D1,
+0x2701CB01, 0x7F141DE1, 0x6EF64F26, 0x6DF6000B,
+0x2FE62FD6, 0x7FEC4F22, 0x66536E53, 0x6D43E5FC,
+0x20596061, 0x2601CB01, 0x326052E2, 0x12E48B06,
+0x31E051E2, 0x52D18B04, 0x1E22A002, 0x5664AFF0,
+0x64F3D21E, 0x420BE614, 0x67E165E3, 0x2719E1FC,
+0x67F12E71, 0x271954D1, 0x65F3D118, 0x410BE614,
+0x52D12F71, 0xCB016021, 0x1DE12201, 0x4F267F14,
+0x000B6EF6, 0x00006DF6, 0x00203924, 0x002034F4,
+0x002034FC, 0x00203504, 0x0020352C, 0x00203910,
+0x00203918, 0x00100208, 0x001017C0, 0x001E210C,
+0x001C3D00, 0x00203964, 0x001000C8, 0x00117880,
+0x00117780, 0x00040020, 0x0026C401, 0x00200ED6,
+0x4F222FE6, 0xDE42624C, 0x42004208, 0x3E2CA005,
+0xD4405252, 0xBF695624, 0x65E22E62, 0x352052E1,
+0xD63D8BF6, 0x4F262622, 0x6EF6000B, 0x2FC62FB6,
+0x2FE62FD6, 0xDC394F22, 0x52C1DB39, 0x362066C2,
+0x6061891C, 0x8801C903, 0xDE348918, 0xBF37DD35,
+0x650364E3, 0x66B28503, 0x3262620D, 0xD4328907,
+0x0009BF76, 0x4D0BD431, 0xAFE60009, 0xBF3D0009,
+0xD42F64E3, 0x00094D0B, 0x0009AFDF, 0x2262D22D,
+0x6EF64F26, 0x6CF66DF6, 0x6BF6000B, 0x2FD62FC6,
+0x4F222FE6, 0xDD29DC28, 0x6E4360C2, 0x04DE4008,
+0xE614D127, 0x65E3410B, 0xD127D726, 0x55E227E2,
+0x35E05254, 0x21228F04, 0x400860C2, 0x122202DE,
+0x605365C2, 0x75014008, 0x0DE606DE, 0xC90F6053,
+0x60632C02, 0x6EF64F26, 0x000B6DF6, 0x85436CF6,
+0x650D5643, 0x622D6262, 0x35277204, 0xE1008F0C,
+0x2268960C, 0xD6158B03, 0x72015261, 0xD6131621,
+0x6262E101, 0x26227201, 0x6013000B, 0x000001FF,
+0x00203504, 0x002034FC, 0x001C3D00, 0x0020352C,
+0x002038F4, 0x002018A2, 0x002034F4, 0x00203AF0,
+0x00203AF4, 0x001C3D28, 0x00203964, 0x00203924,
+0x00200ED6, 0x00203968, 0x0020396C, 0x00117754,
+0x2FC62FB6, 0x2FE62FD6, 0x7FF84F22, 0x6022D237,
+0x8D58C803, 0xDE362F01, 0xDB37DC36, 0x66C252C1,
+0x892F3620, 0xC9036061, 0x892B8801, 0xD233DD31,
+0x64D3420B, 0x1F016503, 0x880160B1, 0xD2308B04,
+0x64D3420B, 0x0009AFEA, 0x85615653, 0x8904C820,
+0xE050D72C, 0x7201027E, 0xD22B0726, 0x6453420B,
+0x89072008, 0x55F1D126, 0x64D3410B, 0xE601D727,
+0x2762AFD4, 0x55F1D226, 0x64E3420B, 0xE601D125,
+0x2162AFCC, 0xDD25DE24, 0xDC26DB25, 0x66D252D1,
+0x89183620, 0xC9036061, 0x89148801, 0xD117D41F,
+0x0009410B, 0x36E05603, 0x65038F04, 0x2B20E201,
+0x2C52AFEC, 0xD712D41C, 0x0009470B, 0xE601D115,
+0xAFE34618, 0x60F12162, 0x8907C804, 0x7F08D217,
+0x6EF64F26, 0x6CF66DF6, 0x6BF6422B, 0x4F267F08,
+0x6DF66EF6, 0x000B6CF6, 0x00006BF6, 0x001E2100,
+0x00203504, 0x002034FC, 0x0020398C, 0x002014A0,
+0x002014CC, 0x00203494, 0x002016BE, 0x001E212C,
+0x00201530, 0x001C3D30, 0x00117880, 0x002034F4,
+0x00203914, 0x00203910, 0x0020352C, 0x00200610,
+0xE601D203, 0x1265D503, 0x000B2252, 0x00001266,
+0x001C1010, 0x0000C34F, 0x0009000B, 0x2FD62FC6,
+0x4F222FE6, 0x6D436C53, 0xEE00A004, 0x7E0164D4,
+0x644CBFF2, 0x8BF93EC2, 0x6EF64F26, 0x000B6DF6,
+0xE5006CF6, 0x6643A002, 0x76017501, 0x22286260,
+0xAFE38BFA, 0x2FE60009, 0x75076253, 0xE1086753,
+0x6043EE0A, 0x4409C90F, 0x650330E2, 0x8D014409,
+0xE630E637, 0x4110365C, 0x8FF22760, 0xE00077FF,
+0x000B8028, 0x000B6EF6, 0x000BE000, 0x2FE6E000,
+0x7FEC4F22, 0x6E436253, 0xBFDC65F3, 0xBFD06423,
+0xBFCE64E3, 0xD40364F3, 0x0009BFCB, 0x4F267F14,
+0x6EF6000B, 0x00203AF8, 0xE4FDD29F, 0xD79F6122,
+0x22122149, 0x74016022, 0x2202CB01, 0xD59C6622,
+0x22622649, 0xC8406070, 0x60528902, 0x2502CB04,
+0xE1F76452, 0x25422419, 0xE7016052, 0x2502CB40,
+0xE6026052, 0x2502C9CF, 0x47186052, 0x2502CB10,
+0xCB036052, 0x15622502, 0x1573000B, 0xD78ED58D,
+0xD48FD28E, 0xE600E100, 0x27112511, 0xAFCB2210,
+0x664C2461, 0x4600D28B, 0x6060362C, 0x000BCB10,
+0x654C2600, 0x4500D287, 0x6650352C, 0x2619E1EF,
+0x2560000B, 0xD284664C, 0x362C4600, 0xCB106060,
+0x2600000B, 0xD280654C, 0x352C4500, 0xE1EF6650,
+0x000B2619, 0x664C2560, 0x4600D27A, 0x6060362C,
+0x000BCB08, 0x654C2600, 0x4500D276, 0x6650352C,
+0x2619E1F7, 0x2560000B, 0xD273664C, 0x362C4600,
+0xCB086060, 0x2600000B, 0xD26F654C, 0x352C4500,
+0xE1F76650, 0x000B2619, 0x624C2560, 0x4200D669,
+0x6020326C, 0x4021C908, 0x40214021, 0x600C000B,
+0xD665624C, 0x326C4200, 0xC9086020, 0x40214021,
+0x000B4021, 0xD161600C, 0x341C644C, 0x000B6240,
+0xD15F602C, 0x341C644C, 0x000B6240, 0x2FE6602C,
+0x6E434F22, 0xE60A645C, 0x89143467, 0x0009BFEB,
+0x60EC640C, 0x8B028801, 0xA002E00F, 0x44092409,
+0x624C4409, 0x3263E60A, 0xBFE28905, 0x620C644C,
+0xC8806023, 0xE2008B00, 0x4F266023, 0x6EF6000B,
+0xD64C4F22, 0x88016062, 0xB2578B03, 0xA0030009,
+0xD2490009, 0x2260E640, 0xE200D648, 0x000B4F26,
+0x4F222622, 0x6062D643, 0x8B018802, 0x0009B2A0,
+0xE200D642, 0x000B4F26, 0xD53E2622, 0xE100D43E,
+0x2512E701, 0x2470000B, 0xE604D23B, 0x2260000B,
+0xD43B4F22, 0x410BD13B, 0xD53B0009, 0x6650E1FD,
+0x2619D23A, 0x2560E700, 0x000B4F26, 0x4F222270,
+0xD238D537, 0xD7386152, 0x2512611D, 0xE6FF6452,
+0x2542242B, 0xD22FD435, 0x420B666D, 0xD52E2762,
+0x6750E1FB, 0x4F262719, 0x2570000B, 0xD4304F22,
+0x410BD128, 0xD5280009, 0x6650E7F7, 0x4F262679,
+0x2560000B, 0x9425D524, 0x22496250, 0x2520000B,
+0xE4BFD521, 0x22496250, 0x2520000B, 0xD2254F22,
+0x600D8522, 0x89112008, 0x89458801, 0x89478803,
+0x89498805, 0x894F8806, 0x89558808, 0x895B8809,
+0x8961880A, 0x8967880B, 0x0009A06E, 0x0009B070,
+0x600CA06B, 0x0000FF7F, 0x001E2148, 0x001E1000,
+0x001E1108, 0x002039C4, 0x002039C6, 0x002039E5,
+0x002039A8, 0x001E103F, 0x001E105F, 0x001E102F,
+0x001E1090, 0x002039CC, 0x001E100B, 0x002039C8,
+0x00203AFC, 0x002018A2, 0x001E1028, 0x002039E4,
+0x001D4020, 0x98760000, 0x001C1000, 0x00203B08,
+0x00203B18, 0x0020399C, 0x0009B04C, 0x600CA035,
+0x0009B055, 0x600CA031, 0x6260D684, 0x8B2B2228,
+0x0009B061, 0x600CA029, 0x6260D680, 0x8B232228,
+0x0009B069, 0x600CA021, 0x6260D67C, 0x8B1B2228,
+0x0009B0C7, 0x600CA019, 0x6260D678, 0x8B132228,
+0x0009B0CD, 0x600CA011, 0x6260D674, 0x8B0B2228,
+0x0009B125, 0x600CA009, 0x6260D670, 0x8B032228,
+0x0009B13D, 0x600CA001, 0x4F26E000, 0x0009000B,
+0xD26CD16B, 0xD56C8412, 0x4000C90F, 0xD76B012D,
+0xE403D66B, 0xE20F611C, 0x2540E001, 0x25202712,
+0x2602000B, 0xE601D262, 0x30668523, 0xE0008D05,
+0xD663D260, 0xE0018122, 0x000B2602, 0xD25C0009,
+0x600D8523, 0x89052008, 0x8B0A8801, 0x6060D65D,
+0x2600CB01, 0xD457D65A, 0xE001E101, 0x000B2612,
+0x000B8142, 0xD152E000, 0x8513E501, 0x640D4518,
+0x66033453, 0xE0008D05, 0xD551D253, 0x2260E001,
+0x000B2502, 0x4F220009, 0x8513D149, 0x6453650D,
+0x62494419, 0x227D672E, 0x8801602C, 0x88028909,
+0x88038910, 0x8806891A, 0x88078935, 0xA04C893B,
+0xD5460009, 0x6652D746, 0x2762D446, 0x622C6261,
+0x2421A038, 0x2228625C, 0xD4438B3F, 0x6642D540,
+0x2562D440, 0x24018561, 0x6203A02C, 0x2008605C,
+0x88108907, 0x88208908, 0x88308909, 0xA02C890A,
+0xD23A0009, 0x6222A008, 0xA005D239, 0xD2396222,
+0x6222A002, 0x6262D638, 0xD432D531, 0x66212522,
+0xA00F626C, 0xD6352421, 0x6261D52D, 0x622CD42D,
+0xA0072562, 0xD6322421, 0x8561D529, 0x2562D429,
+0x62032401, 0x662D8515, 0x3617610D, 0x65038F01,
+0xB0CB2451, 0xA0010009, 0xE000E001, 0x000B4F26,
+0xD6190009, 0xD427E101, 0x65412610, 0xD118D717,
+0xE20F655D, 0x2752E001, 0x000B2620, 0x2FE62102,
+0xD20F4F22, 0x640C8523, 0x8B082448, 0xD511D61D,
+0x2621E200, 0x940F8451, 0xA0482049, 0xDE0D8051,
+0xC84060E0, 0xE2018D32, 0x89443427, 0xD216D615,
+0x2641420B, 0x0009A030, 0x0000FF7F, 0x002039E5,
+0x0020399C, 0x002039A8, 0x001E1100, 0x001E100C,
+0x002039C8, 0x001E1000, 0x001E1001, 0x002039D0,
+0x002039B0, 0x002039B4, 0x002039B8, 0x002039D4,
+0x002039D8, 0x002039DC, 0x002039E0, 0x00203E04,
+0x00203E0E, 0x002039C2, 0x00202886, 0x89123427,
+0xD294D693, 0x2641420B, 0xCB8084E1, 0x80E1B0F5,
+0xD69160E0, 0x2E00CB04, 0xC93F6060, 0xD68F2600,
+0xA001E001, 0xE0002602, 0x000B4F26, 0xD68C6EF6,
+0xC8806060, 0xD2868919, 0x88016021, 0xD2898B15,
+0x8524E501, 0x89103056, 0xE203D187, 0x2120D487,
+0xE00B6541, 0x0656655D, 0xE40FD585, 0x2140E702,
+0xD77E2571, 0x000BE001, 0x000B2702, 0x2FE6E000,
+0xDE804F22, 0xC88084E1, 0xD57A892C, 0x20088554,
+0x61038F28, 0x8553D77C, 0x64036672, 0x8566650C,
+0x3520620C, 0xD6798B1E, 0x651CD774, 0x2651644C,
+0x60E02741, 0x8904C840, 0x420BD275, 0xA0030009,
+0xD2680009, 0x0009420B, 0x0009B09F, 0xE201D167,
+0x60E02122, 0xCB04D464, 0x60402E00, 0x2400C93F,
+0x6023A001, 0x4F26E000, 0x6EF6000B, 0x2FB62FA6,
+0x2FD62FC6, 0xDA622FE6, 0x66A1E240, 0x3622DC5E,
+0x62638900, 0x6ED36D2C, 0x4E2136D8, 0x4E212A61,
+0xDB61D460, 0xE700A00F, 0x770162B2, 0x71026123,
+0x66212B12, 0x71026213, 0x61212B12, 0x651D666D,
+0x356C4528, 0x627C2452, 0x8BED32E3, 0xC90360D3,
+0x8B108803, 0x617367B2, 0x2B127102, 0x71026E13,
+0x2B126571, 0x655D6DE1, 0x422862DD, 0x325CE107,
+0xA00C2C10, 0x88022422, 0xA0038B01, 0x8801E203,
+0xE2018B05, 0x66B22C20, 0x655D6561, 0xE60F2452,
+0x67A12C60, 0x8B052778, 0xDD38DC44, 0xEB01EA00,
+0x2DB22CA2, 0x6DF66EF6, 0x6BF66CF6, 0x6AF6000B,
+0x2FE62FD6, 0xE240DD36, 0x362266D1, 0x62638900,
+0x3678672C, 0x7703DE38, 0x47212D61, 0x64E2D635,
+0xA00E4721, 0x6562E100, 0x62537101, 0x74012450,
+0x24204219, 0x45297401, 0x74012450, 0x24504519,
+0x621C7401, 0x8BEE3273, 0x66E24200, 0x420061D1,
+0x2118362C, 0x2E628F06, 0xDD1CD728, 0xE501E400,
+0x2D522742, 0x000B6EF6, 0x2FD66DF6, 0x4F222FE6,
+0xED0AEE01, 0x64E3BC85, 0xBC8A64E3, 0x62EC7E01,
+0x8BF732D7, 0xBC8DEE01, 0x64E364E3, 0x7E01BC92,
+0x32D762EC, 0x4F268BF7, 0x000B6EF6, 0xD1186DF6,
+0xD418920D, 0x72122122, 0x2422D617, 0xD7177204,
+0x72202622, 0x2722D116, 0x000B7230, 0x137A2122,
+0x002039C2, 0x00202992, 0x001E1015, 0x002039C8,
+0x001E1001, 0x0020399C, 0x001E1100, 0x002039C6,
+0x002039B4, 0x001E1000, 0x002039B8, 0x002039C4,
+0x00202886, 0x001E100C, 0x002039B0, 0x002039CC,
+0x002039D0, 0x002039D4, 0x002039D8, 0x002039DC,
+0x002039E0, 0x4F222FE6, 0xD6707FFC, 0x88016060,
+0xE2018951, 0x2620BFBB, 0xD56ED16D, 0xDE6E6010,
+0x64E36552, 0x7402C840, 0x8D22D16C, 0xD26C7502,
+0xE601D76C, 0xE7042722, 0x76016255, 0x626C2421,
+0x8FF93273, 0xD4637402, 0x6242E601, 0x640D8528,
+0x67494419, 0x275D657E, 0x81E4607C, 0xE417D562,
+0x67557601, 0x3243626C, 0x8FF92171, 0xA0207102,
+0xD25E0009, 0xE601D75B, 0xE7042722, 0x76016255,
+0x626C2421, 0x8FF93273, 0xD4527402, 0x6242E601,
+0x640D8528, 0x67494419, 0x275D657E, 0x81E4607C,
+0xE417D553, 0x67557601, 0x3243626C, 0x8FF92171,
+0x92897102, 0xD2462E21, 0x5E23D74E, 0x64F22FE2,
+0x604365F2, 0x2700C980, 0xC9606043, 0x80716103,
+0xC9036043, 0x80724519, 0x65F2605C, 0x817266F2,
+0x46194629, 0x606C4529, 0x4018645C, 0x8173304C,
+0x21185E23, 0x64F22FE2, 0x6E4C62F2, 0x602C4219,
+0x66F262F2, 0x46294018, 0x461930EC, 0x42298174,
+0x652C606C, 0x305C4018, 0x81758F07, 0x0009BC96,
+0x2228620C, 0xA00A8908, 0x60130009, 0x8B038840,
+0x0009B009, 0x0009A003, 0xE202D62F, 0x7F042622,
+0x000B4F26, 0x4F226EF6, 0x8552D52A, 0x8830600D,
+0x88318903, 0xA0348923, 0x85550009, 0xD428D727,
+0x85532701, 0x610DD627, 0x24124118, 0x460BD426,
+0xD7230009, 0xD226D425, 0x6572420B, 0xE230D120,
+0x42286712, 0x2729E620, 0x37604628, 0xD6218B03,
+0xA016E200, 0xD61F2622, 0xA012E202, 0xD1182622,
+0x6212E530, 0xE6204528, 0x46282259, 0x89083260,
+0xD41AD119, 0xE601D513, 0x2160450B, 0x472BD718,
+0x4F264F26, 0x0009000B, 0x0000060A, 0x002039E4,
+0x001E1000, 0x002039D0, 0x00203E04, 0x00203E10,
+0x00203DA8, 0x002039B8, 0x00203DD8, 0x00203DD6,
+0x00203DAA, 0x0020399C, 0x002039C8, 0x002039B4,
+0x002039B0, 0x002018A2, 0x00203B24, 0x00203B28,
+0x002018EE, 0x002039CC, 0x001E100B, 0x00203B3C,
+0x00114004, 0x4F222FE6, 0xDE967FFC, 0x200884E9,
+0x2F008D06, 0xD695D494, 0x0009460B, 0x64F0B19A,
+0x6620D293, 0x89022668, 0xC9BF60E0, 0x7F042E00,
+0x000B4F26, 0x000B6EF6, 0x2FE60009, 0xDE8D4F22,
+0x60E0D68D, 0xCBC0D48D, 0x62602E00, 0xC803602C,
+0x40218904, 0x70014021, 0x6603A002, 0x66034009,
+0xD687616D, 0xE500A004, 0x75016262, 0x74042422,
+0x3213625D, 0xD2838BF8, 0x0009420B, 0xC9BF84E2,
+0x4F2680E2, 0x6EF6000B, 0x2FE62FD6, 0x7FFC4F22,
+0x6260D67D, 0x89442228, 0xD572E100, 0x60502610,
+0xCB40D47A, 0x2500440B, 0x8D052008, 0x62E06E03,
+0x7104612C, 0x2F11A006, 0xD475D66D, 0xDD756760,
+0x657C4D0B, 0xE23C6D1D, 0x8B033D27, 0xD267D472,
+0x0009420B, 0x4D214D21, 0xA005D770, 0x66E6E400,
+0x357C4508, 0x74012562, 0x35D3654D, 0xD76C8BF7,
+0x6172E003, 0x81114018, 0x6E7260F1, 0x81E2700C,
+0xD4686172, 0xDD688113, 0x4D0BDE68, 0xE2016572,
+0xD4672E22, 0x420BD255, 0xD6560009, 0xC93F6060,
+0x7F042600, 0x6EF64F26, 0x6DF6000B, 0x2FC62FB6,
+0x2FE62FD6, 0xD25F4F22, 0x6B436E73, 0x420B6C53,
+0x20086D63, 0x64038D1C, 0xE50ED149, 0x32526210,
+0x60C38916, 0x804124B0, 0x814160D3, 0xA007E500,
+0x655D61BC, 0x00EC6053, 0x364C6653, 0x80647501,
+0x3213625D, 0xD63B8BF5, 0xC9BF6060, 0x2600A008,
+0xD23AD44D, 0x6EF64F26, 0x6CF66DF6, 0x6BF6422B,
+0x6EF64F26, 0x6CF66DF6, 0x6BF6000B, 0x7FC44F22,
+0x720262F3, 0x22512F41, 0x45297202, 0x60632251,
+0xE5C4E682, 0x67F38121, 0x655C666C, 0xE408BFB6,
+0x4F267F3C, 0x0009000B, 0x2F962F86, 0x2FB62FA6,
+0x2FD62FC6, 0x4F222FE6, 0xE1007FC4, 0x6513ECFF,
+0x6B136CCD, 0xDE36D735, 0xEDFF64F3, 0xD835EA04,
+0x6053655C, 0x027D4000, 0x32C0622D, 0x66038D0D,
+0x09ED6063, 0x2491027D, 0x24217402, 0x698202ED,
+0x3928622D, 0x74022892, 0x75017104, 0x6063625C,
+0x07D532A2, 0x0EB58FE4, 0x2448641C, 0xE6808905,
+0x67F3E5C5, 0xBF79666C, 0x7F3C655C, 0x6EF64F26,
+0x6CF66DF6, 0x6AF66BF6, 0x000B69F6, 0xD11E68F6,
+0x6012D21E, 0xCB20E405, 0x2102E500, 0x000B2242,
+0x00002252, 0x001E1017, 0x00203B40, 0x002018A2,
+0x0020390E, 0x001E1015, 0x001E10BF, 0x00117800,
+0x001E10FC, 0x00200610, 0x00203914, 0x00202AEA,
+0x00203B44, 0x002018EE, 0x00203B60, 0x0011788C,
+0x00203910, 0x002034F4, 0x00201530, 0x001E2130,
+0x00203B68, 0x00202AAC, 0x00203B6C, 0x00203974,
+0x0020397C, 0x00203DA4, 0x001C3500, 0x001D4004,
+0xD564D163, 0xE400D764, 0x2142E20F, 0x17411154,
+0xD5622722, 0x9669D762, 0x15412572, 0x96661562,
+0xE6011565, 0xD55F1165, 0x666CE6F8, 0x25422542,
+0x25422542, 0x25422542, 0x25622542, 0x7601E727,
+0x67632572, 0x25627797, 0xE7042572, 0x2572E248,
+0xE2192522, 0xE2702522, 0x25422542, 0x25422542,
+0x25222542, 0x2522E20C, 0x25422542, 0x25422542,
+0x25422542, 0x25422542, 0x000B154A, 0xE2081145,
+0x0009422B, 0x2FE62FD6, 0x7FFC4F22, 0xC8206043,
+0x6E438D02, 0x0009BE67, 0xC81060E3, 0xBE648901,
+0x60E30009, 0x8901C840, 0x0009BE86, 0xC80160E3,
+0xDD3D8938, 0xC80260D0, 0x2F008D03, 0x460BD63B,
+0x60F00009, 0x8902C804, 0x460BD639, 0x62F00009,
+0xC8806023, 0x60D08902, 0x2D00C97F, 0xC8016023,
+0xD6348906, 0x0009460B, 0x0009A007, 0x51630601,
+0x8902C808, 0x460BD630, 0x60F00009, 0x8902C810,
+0x420BD22E, 0xD52E0009, 0x88026052, 0xD22D8B03,
+0xA005E604, 0x88012260, 0xD22A8B02, 0x2260E601,
+0x2522E200, 0xC88060E3, 0xD227892D, 0x60E36E20,
+0x8902C880, 0x420BD225, 0x60E30009, 0x8902C840,
+0x420BD223, 0x60E30009, 0x8902C802, 0x420BD221,
+0x60E30009, 0x890DC804, 0xDD20D11F, 0x0009410B,
+0x0009BF0D, 0x0009BF4C, 0xD51ED41D, 0x2470E708,
+0x25D2BF85, 0xC80860E3, 0xD21B8905, 0x4F267F04,
+0x422B6EF6, 0x7F046DF6, 0x6EF64F26, 0x6DF6000B,
+0x001C581C, 0xA000A000, 0x001D0100, 0x001D4000,
+0x00040021, 0x001C589C, 0x001E1021, 0x00201A90,
+0x00201AB2, 0x00202114, 0x00201ACA, 0x00201AD8,
+0x002039C8, 0x001E100B, 0x001E1028, 0x00201B44,
+0x00201B50, 0x00201AE0, 0x00201AFE, 0x12345678,
+0x001E1000, 0x0010F100, 0x00201B2C, 0x644CD6A7,
+0x000B346C, 0xD6A62450, 0x346C644C, 0x2450000B,
+0x644CD6A4, 0x000B346C, 0x625C2450, 0x4208616D,
+0x42084119, 0x42006019, 0x670E614C, 0xD49E321C,
+0x4200207D, 0x324CC90F, 0x2200000B, 0x4208625C,
+0x42004208, 0x324C644C, 0x4200D498, 0x000B324C,
+0x2FE62260, 0x614C4F12, 0x4100D493, 0x6710314C,
+0xE29F666D, 0x27294619, 0x6E536269, 0x672E6573,
+0x4221227D, 0x42214221, 0x7601662C, 0xE4014608,
+0x34E84608, 0x644C4600, 0x071A0467, 0x2150257B,
+0x000B4F16, 0x4F226EF6, 0xD2857FE8, 0x88016021,
+0xD2848B7B, 0x26686621, 0xD2838B77, 0x26686621,
+0xE50F8B73, 0xE401BFA2, 0xBFA4E501, 0xE586E400,
+0xE400655C, 0x2F50BFA4, 0xBFA1E401, 0xE602E506,
+0x60634618, 0x81F2E401, 0x6543BF9F, 0xE40185F2,
+0xBFAB6543, 0x85F26603, 0x6543E401, 0x6603BFB1,
+0xE40265F0, 0x6053756C, 0x80F8BF80, 0xBF82E402,
+0x84F8E512, 0x7090E402, 0x6503BF82, 0x4618E602,
+0x81F66063, 0xBF80E402, 0x85F6E500, 0x6603E402,
+0xE500BF8C, 0xE40285F6, 0xBF926603, 0xE5FEE500,
+0xE010655C, 0xBF61E403, 0xE5130F54, 0xE40EBF63,
+0x05FCE010, 0xBF63E40E, 0xE5007585, 0xBF64E403,
+0xE500E640, 0xBF71E403, 0xE500E640, 0xBF78E403,
+0xE5FFE640, 0xE014655C, 0xBF47E404, 0xE40F0F54,
+0xE504BF49, 0x05FCE014, 0xBF49E40F, 0xE5017584,
+0xBF4AE640, 0xE501E404, 0xBF57E640, 0xE501E404,
+0xE404E640, 0xAF5C7F18, 0x7F184F26, 0x000B4F26,
+0x4F220009, 0xD2427FF0, 0x88016021, 0xD2418B71,
+0x26686621, 0xD2408B6D, 0x26686621, 0xE50F8B69,
+0xE401BF1C, 0xBF1EE501, 0xE586E400, 0xE400655C,
+0x2F50BF1E, 0xBF1BE401, 0xE401E506, 0xBF1C6543,
+0xE401E640, 0xBF296543, 0xE401E640, 0xBF306543,
+0x65F0E640, 0x756CE402, 0xBEFF6053, 0xE40280F4,
+0xE512BF01, 0xE40284F4, 0xBF017090, 0xE6406503,
+0xBF02E402, 0xE640E500, 0xBF0FE402, 0xE640E500,
+0xBF16E402, 0xE5FEE500, 0x6053655C, 0xBEE5E403,
+0xE51380F8, 0xE40EBEE7, 0xE40E84F8, 0xBEE77085,
+0xE5006503, 0xBEE8E640, 0xE500E403, 0xBEF5E640,
+0xE500E403, 0xBEFCE640, 0xE5FFE403, 0x6053655C,
+0xBECBE404, 0xE40F80FC, 0xE504BECD, 0xE40F84FC,
+0xBECD7083, 0xE5016503, 0xBECEE640, 0xE501E404,
+0xBEDBE640, 0xE501E404, 0xE404E640, 0xAEE07F10,
+0x7F104F26, 0x000B4F26, 0x00000009, 0x001E102F,
+0x001E1080, 0x001E1090, 0x001E103F, 0x001E103E,
+0x002039C2, 0x002039C4, 0x002039C6, 0xD21DD11C,
+0x66206010, 0x676C7001, 0x3700C90F, 0xE5008D13,
+0x67106210, 0x7701622C, 0x64232170, 0xD6166010,
+0x44084408, 0x3428C90F, 0x62602100, 0x7201D513,
+0x44082620, 0x000B354C, 0xD10F6053, 0x25586510,
+0xE6008D13, 0xD60DD40B, 0x655C6540, 0x47086753,
+0x37584708, 0x47086540, 0x24507501, 0x367C6040,
+0x2400C90F, 0x72FF6210, 0x000B2120, 0x00006063,
+0x0020390D, 0x0020390C, 0x0020390E, 0x00203534,
+0x7FFC4F22, 0xE680D19F, 0x666C6212, 0xD29E2F22,
+0x67F36563, 0x420B7542, 0x7F04E404, 0x000B4F26,
+0xE6800009, 0xD298666C, 0xE7006563, 0x422B7540,
+0xE6806473, 0xD294666C, 0xE7006563, 0x422B7543,
+0x2F866473, 0x2FA62F96, 0x2FC62FB6, 0x2FE62FD6,
+0x7FCC4F22, 0xDC8ED28D, 0x72011F21, 0xDB8D1F22,
+0xD18EDE8D, 0x66125211, 0x8B013620, 0x0009A0E5,
+0xC9036061, 0x8B018801, 0x0009A0DF, 0xD288D487,
+0xED84420B, 0x2F025503, 0x30D0845C, 0xA0B88901,
+0xD1840009, 0x626C6610, 0x88016023, 0xD1828B68,
+0x62101FC3, 0x895B2228, 0xE003D480, 0x40186742,
+0x68421772, 0xD57EE900, 0x81816DB3, 0x7D042190,
+0x67D26AB2, 0x64E26852, 0x1F491F57, 0x740464E3,
+0x1FA46542, 0x65431F5A, 0x625275F8, 0x1F761FD5,
+0x6D531F2B, 0xDA74D773, 0x7D94D274, 0x68D21F88,
+0x6AA26972, 0xD1726022, 0x2202CB20, 0xE1401F1C,
+0x7601E600, 0x3213626D, 0x56F48BFB, 0x52F651F5,
+0x21222B62, 0x52F851F7, 0x212256F9, 0x2E6251FA,
+0x51FB2412, 0x2D822512, 0xD9662792, 0x29A2DD5F,
+0x6AD2D965, 0xD9646892, 0x68D21A84, 0x6081DA63,
+0x2801CB01, 0xD86266D2, 0x2A622962, 0xED015AFC,
+0x2AD2480B, 0x2AD24D18, 0x62D2DD5E, 0x2D227201,
+0xD15056F3, 0xE2026062, 0x2602CB01, 0x2120A03D,
+0x8B3A2228, 0xE401DD58, 0x2140E600, 0xE01C2D62,
+0xC801005C, 0xD4558B0A, 0xE600D755, 0xED7D2472,
+0x626C7601, 0x8BFB32D3, 0x24D2DD52, 0xE2FE68C2,
+0x2C822829, 0x095CE01E, 0xE01F5DF1, 0x0A5C2D90,
+0x751051F2, 0xED0621A0, 0xD74BE600, 0x8456D44B,
+0x27007601, 0x696C6854, 0x248039D3, 0x8FF67401,
+0xDA477701, 0x2A10E194, 0xE2007A01, 0x7A0F2A20,
+0xD130E805, 0x66102A80, 0x6023626C, 0x89088801,
+0xD240D42A, 0x420B65F2, 0xD131ED01, 0xAF304D18,
+0x65F221D2, 0x8553D43C, 0x620D6642, 0x89073262,
+0xD13BD43A, 0x0009410B, 0xE601D73A, 0x2762AF1A,
+0xD134D41E, 0x410B65F2, 0xD125ED01, 0xD637D436,
+0x460B4D18, 0xAF0D21D2, 0x7F340009, 0x6EF64F26,
+0x6CF66DF6, 0x6AF66BF6, 0x000B69F6, 0x4F2268F6,
+0x85467FF4, 0x2F01E681, 0x666C8547, 0x854881F1,
+0x81F2D209, 0x67F38542, 0x854381F3, 0x81F4E40C,
+0x65636053, 0x420B81F5, 0x7F0C7540, 0x000B4F26,
+0x00000009, 0x001C3D9C, 0x0020245C, 0x0011779A,
+0x001C36F8, 0x001C3B9C, 0x001C3704, 0x0020352C,
+0x002014A0, 0x0020391D, 0x0020391C, 0x00203918,
+0x001C3D98, 0x001C3BB4, 0x001C5960, 0x001C3500,
+0x001C3D30, 0x001C8960, 0x00203504, 0x001C3D00,
+0x0020160C, 0x00117730, 0x00203920, 0x001C582C,
+0x2000A000, 0x0000A000, 0x0011778C, 0x00117792,
+0x00117788, 0x002014CC, 0x002038F4, 0x002034F4,
+0x00201530, 0x001E2130, 0x00203D84, 0x002018A2,
+0x2F962F86, 0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6,
+0xD19B7FEC, 0x2F12E000, 0x6103D49A, 0x1F4281F2,
+0xDD9ADA99, 0xD69A6813, 0xE0014808, 0x460BDE99,
+0x38EC4800, 0x65A21F03, 0x352052A1, 0xA23E8B01,
+0x60510009, 0x8801C903, 0xA2388B01, 0x52530009,
+0x32E0DE91, 0xD9918B10, 0x64A3490B, 0x4B0BDB90,
+0xDE906403, 0xD791D690, 0xEC01D591, 0x2E02E100,
+0x271026C0, 0x2502AFDF, 0xC8018551, 0xA1578B01,
+0x62510009, 0x4200622D, 0x5E53366A, 0x85E2226D,
+0xC903642C, 0x85E36603, 0x6053650D, 0x40214021,
+0x4500C93F, 0x322A6703, 0x6053252D, 0xC901D17F,
+0x60106C03, 0x8801D97F, 0xDB7F8B05, 0x2120E200,
+0xCB0160B2, 0xD17D2B02, 0x88016011, 0x65A28B0A,
+0x8D042448, 0x9B9E6251, 0xA00322B9, 0x919B2521,
+0x2521221B, 0x37B3EB10, 0x2448895E, 0xD4738B07,
+0x22286241, 0x60638903, 0xA05781F8, 0xD5706473,
+0x46084608, 0x85E26273, 0x46006B50, 0x362C4200,
+0x2BB8C910, 0x8F1F6463, 0x26686603, 0xD2698911,
+0x062D6043, 0x4119616D, 0x6B0E6019, 0x81F820BD,
+0x880160C3, 0x646C8F2C, 0x880F6073, 0xA0278B1B,
+0xD2610009, 0x052D6043, 0x4119615D, 0x670E6019,
+0x645C207D, 0x81F8A01C, 0x890F2668, 0x6043D25B,
+0x6B5D052D, 0x60B94B19, 0x201D610E, 0x60C381F8,
+0x8F0D8801, 0x6473645C, 0xEC00A00A, 0x6043D254,
+0x625D052D, 0x60294219, 0x207D670E, 0x81F8645C,
+0x880285F8, 0x85E1890A, 0x8D07C820, 0xE6DC6203,
+0x60232269, 0x81E1A002, 0x644CE4FF, 0x6210D149,
+0x89012228, 0x644CE4FF, 0x654DEBFF, 0x35B06BBC,
+0xDB368B2B, 0x64A34B0B, 0x410BD135, 0x54036403,
+0x85446E03, 0xC948DB40, 0xDC408808, 0xBEAE8B01,
+0x64B3E502, 0x65E34C0B, 0xDB3DEC01, 0xD13D2DC2,
+0x621260B2, 0x72017001, 0x21228805, 0x2B028F08,
+0x666CE680, 0x6563D238, 0x7549E700, 0x6473420B,
+0xA030D436, 0x7FFF0009, 0x85E28000, 0x20B9EBFC,
+0x610381E2, 0x942A85E3, 0x62032049, 0x450885F8,
+0x81E2201B, 0xC90160C3, 0x40084018, 0x40084008,
+0x4000225B, 0x6023220B, 0x85E481E3, 0x4118E108,
+0x81E4201B, 0xE40262A2, 0x20B98521, 0x67A28121,
+0xCB016071, 0x85F82701, 0x89033042, 0xECE785E2,
+0x81E220C9, 0x490BD41E, 0xA03B0009, 0x7E030009,
+0x001C3D30, 0x00203D90, 0x00203504, 0x001E212C,
+0x002033E8, 0x001C3D00, 0x00117780, 0x002014A0,
+0x0020166C, 0x0011770C, 0x0020391C, 0x0020391D,
+0x00203918, 0x002018A2, 0x001C36F8, 0x00203990,
+0x00203DA0, 0x00203B84, 0x00203C04, 0x00203C84,
+0x00203D04, 0x00203908, 0x002034FC, 0x002014CC,
+0x00203994, 0x00203998, 0x0020245C, 0x00203D88,
+0x00203D8C, 0x602262F2, 0x40094019, 0xC90F4009,
+0x8B0B880A, 0x60E2DE8C, 0x40094019, 0xC90F4009,
+0x8B038808, 0xCB0160A2, 0x2802A006, 0x65E2DE87,
+0x2E527501, 0x286266A2, 0x52F366F2, 0x2622AE83,
+0xD2838551, 0xDE83C802, 0xA0958B01, 0x420B0009,
+0x4E0B64A3, 0x5E036403, 0x85E46503, 0x4918E908,
+0xD77D209B, 0xE04C81E4, 0xDC7C0B7E, 0x7B01D97C,
+0x61C207B6, 0x71016690, 0x8D062668, 0xD4792C12,
+0x420BD279, 0xA070EB01, 0x62512DB2, 0x4B18EB0F,
+0x22B9E102, 0x32104118, 0x85518B0F, 0x2029E2FC,
+0x60518151, 0xCB0172E0, 0x85E12501, 0x202994A3,
+0x85E481E1, 0xA0522049, 0x675181E4, 0x4719677D,
+0x667E6779, 0x7701276D, 0x6903607C, 0x88014918,
+0x25918F3E, 0x6B12D161, 0x21B27B01, 0x660D85E3,
+0x40216063, 0xC93F4021, 0x6C034600, 0x262D322A,
+0xC8016063, 0xDB5ED15D, 0x967D8901, 0xE6002C6B,
+0x666C67CD, 0x40006063, 0x622D021D, 0x8D0E3270,
+0x60436403, 0xE9FF021D, 0x8B013290, 0x01C5A007,
+0x626C7601, 0x3292E904, 0x646C8BEB, 0x60434400,
+0xD15004BD, 0x0B457401, 0x669D6911, 0x89073670,
+0x602D6211, 0x890388FF, 0xE201DB4B, 0x2B2021C1,
+0xECFC8551, 0x815120C9, 0xCB016051, 0xDC472501,
+0x64A34C0B, 0x51F366F2, 0x85EF2612, 0x54F2D244,
+0x650D420B, 0x0009ADE7, 0xE500DC42, 0x420B2C52,
+0x4E0B64A3, 0x54036403, 0x85446E03, 0x6703E908,
+0x65034918, 0x27998541, 0xDB323790, 0x8F0BD932,
+0x6013610D, 0x8B07C820, 0xC9486053, 0x8B038808,
+0xE501BD4D, 0x0009A005, 0x2128D233, 0xBD468901,
+0x64B3E500, 0x490B65E3, 0xADBCEC01, 0x85F22DC2,
+0x7001EE04, 0x31E7610D, 0x8D0281F2, 0xADA97A08,
+0x7F140009, 0x6EF64F26, 0x6CF66DF6, 0x6AF66BF6,
+0x000B69F6, 0xF7FF68F6, 0x2FE68000, 0xD2234F22,
+0x60E36E22, 0x8D02C840, 0xBBF922E2, 0xE2400009,
+0x2E284218, 0xBC048901, 0x60E30009, 0x8905C810,
+0xD21CD41B, 0x0009420B, 0x0009BC03, 0xC80560E3,
+0xBD6D8901, 0x60E30009, 0x8902C802, 0xAC004F26,
+0x4F266EF6, 0x6EF6000B, 0x001C3D3C, 0x00117760,
+0x002014A0, 0x0020166C, 0x00203494, 0x00203DA4,
+0x00203908, 0x002034FC, 0x002014CC, 0x00203974,
+0x0020397C, 0x00203970, 0x00203972, 0x00201530,
+0x002018EE, 0x00203994, 0x00008000, 0x001C3510,
+0x00203D98, 0x002018A2, 0x080A0C0E, 0x00020406,
+0x1A1C1E20, 0x12141618, 0x2E303234, 0x26282A2C,
+0x3A3C3E40, 0x6C625648, 0x41112F26, 0xE2208F18,
+0x890B3123, 0x321CD204, 0xD1026220, 0x412B312C,
+0x00090009, 0x00203412, 0x002033C8, 0x000BE000,
+0x400062F6, 0x40004000, 0x40004000, 0x40004000,
+0x62F6000B, 0x40004000, 0x40004000, 0x40004000,
+0x40184000, 0x62F6000B, 0x40004000, 0x40004000,
+0x40004000, 0x40284000, 0x62F6000B, 0x40004000,
+0x40184000, 0x000B4028, 0xC90F62F6, 0x40054005,
+0x40054005, 0x62F6000B, 0x4005C907, 0x40054005,
+0x62F6000B, 0x4005C903, 0x000B4005, 0xC90162F6,
+0x000B4005, 0x000062F6, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x544F0D0A, 0x46205355, 0x00003A57,
+0x206C754A, 0x32203120, 0x20383030, 0x323A3132,
+0x34333A38, 0x00000000, 0x00000D0A, 0x00000043,
+0x42707372, 0x3D206675, 0x554E203D, 0x202C4C4C,
+0x6E49677A, 0x4E497274, 0x6D754E51, 0x0000003D,
+0x61766E49, 0x2064696C, 0x72657375, 0x20726F20,
+0x2079656B, 0x00214449, 0x52504545, 0x57204D4F,
+0x65746972, 0x6461202C, 0x003D7264, 0x6C617620,
+0x0000003D, 0x00000A0D, 0x435F4D5A, 0x465F444D,
+0x4C445F57, 0x494E495F, 0x00000054, 0x6E6B6E55,
+0x206E776F, 0x6D6D6F63, 0x3D646E61, 0x00000000,
+0x203A3051, 0x00000020, 0x203A3151, 0x00000020,
+0x203A3251, 0x00000020, 0x203A3351, 0x00000020,
+0x203A3451, 0x00000020, 0x2B434741, 0x73696F4E,
+0x61432065, 0x7262696C, 0x6F697461, 0x6166206E,
+0x6F206C69, 0x6974206E, 0x0D0A656D, 0x00000000,
+0x00000072, 0x00205220, 0x00000D0A, 0x62735576,
+0x7473725F, 0x00000A0D, 0x62735576, 0x7375735F,
+0x646E6570, 0x00000A0D, 0x62735576, 0x7365725F,
+0x000A0D6D, 0x00000044, 0x44387570, 0x72637365,
+0x6F747069, 0x3D584572, 0x00000000, 0x00000047,
+0x00000042, 0x72746E49, 0x6D652051, 0x2C797470,
+0x49677A20, 0x4972746E, 0x754E514E, 0x00003D6D,
+0x654C7245, 0x0000006E, 0x00000049, 0x20746F4E,
+0x756F6E65, 0x49206867, 0x4220514E, 0x0A0D6675,
+0x00000000, 0x000000FF, 0x00020001, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x010E010D, 0x00020003, 0x01090108,
+0x0002010A, 0x02000003, 0x02020201, 0x02040203,
+0x02060205, 0x02020200, 0x02040203, 0x020C020B,
+0x020E020D, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x000000FF, 0x00020001, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x010E010D, 0x00020003, 0x01090108,
+0x0002010A, 0x00030003, 0x02020201, 0x02040203,
+0x02060205, 0x02020200, 0x02040203, 0x020C020B,
+0x020E020D, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x010E010D, 0x00FF010F, 0x01090108,
+0x010B010A, 0x0200010F, 0x02020201, 0x02040203,
+0x02060205, 0x02020200, 0x02040203, 0x020C020B,
+0x020E020D, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x010E010D, 0x00FF010F, 0x01090108,
+0x010B010A, 0x010F010F, 0x02020201, 0x02040203,
+0x02060205, 0x02020200, 0x02040203, 0x020C020B,
+0x020E020D, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00205220, 0x00000046, 0x00000059,
+0x73204142, 0x003D7165, 0x49544120, 0x0000204D,
+0x00000000, 0x00000000, 0x002E0209, 0x80000101,
+0x000409FA, 0x00FF0400, 0x05070000, 0x02000201,
+0x82050700, 0x00020002, 0x03830507, 0x07010040,
+0x40030405, 0x02090100, 0x0101002E, 0x09FA8000,
+0x04000004, 0x000000FF, 0x02010507, 0x07000040,
+0x40028205, 0x05070000, 0x00400383, 0x04050701,
+0x00004002, 0x00000000, 0x00000000, 0x07090000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+};
+
+const u32_t zcFwImageSize=15936;
diff --git a/drivers/staging/otus/hal/hpfwu.c.drv_ba_resend b/drivers/staging/otus/hal/hpfwu.c.drv_ba_resend
new file mode 100644
index 00000000000..7f5bcff57b5
--- /dev/null
+++ b/drivers/staging/otus/hal/hpfwu.c.drv_ba_resend
@@ -0,0 +1,742 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include "cprecomp.h"
+
+const u32_t zcFwImage[] = {
+0x0009000B, 0x4F222FE6, 0xDE297FFC, 0xE114D729,
+0x1E13D429, 0x1E4C470B, 0x0009B018, 0xA0039545,
+0x3652E600, 0x76018D04, 0xC84060E2, 0x2F028DF9,
+0xDE23D422, 0x00094E0B, 0x4E0BD422, 0xD4220009,
+0x00094E0B, 0x4F267F04, 0x6EF6A024, 0xD11F4F22,
+0x0009410B, 0x440BD41E, 0xD51E0009, 0x0009450B,
+0xE1FFD71D, 0xD21D611D, 0x50292712, 0xCB01D41C,
+0xE501E1FF, 0x22121209, 0x24521211, 0xD61AD519,
+0xE2009714, 0xD4192572, 0xD6192620, 0x4F262422,
+0x2622000B, 0xDD18DC17, 0x4C0BDE18, 0x4D0B0009,
+0x4E0B0009, 0xAFF80009, 0x27100009, 0x00000640,
+0x001C001C, 0x00200BC4, 0x0000B38E, 0x002029F8,
+0x00200F72, 0x00202A04, 0x00202A1C, 0x00200F20,
+0x00201056, 0x00200C1C, 0x001C3510, 0x001C3624,
+0x001E212C, 0x00202994, 0x00202530, 0x0020299C,
+0x002029A8, 0x00200E50, 0x002023E6, 0x00201920,
+0x2FC62F96, 0x2FE62FD6, 0x7F904F22, 0xE020DE8D,
+0xD48D61E0, 0x61E30F14, 0x62107101, 0xE024D78B,
+0x0F24470B, 0x450BD58A, 0x20080009, 0x8F116D03,
+0xDD881F0A, 0x67D0D488, 0x410BD188, 0xD288657C,
+0x6920DD88, 0x66C36C9C, 0x46084608, 0x460836C8,
+0x1FDA3D6C, 0x04FCE024, 0x66E2E580, 0x655C604C,
+0x8F163050, 0xE0202D62, 0xE50001FC, 0xDE7E641C,
+0x3243625D, 0xA32C8B01, 0x655D0009, 0x36EC6653,
+0xE02C6760, 0x69530F74, 0x39DC607E, 0xAFEF8094,
+0x20087501, 0xE0208B14, 0xE50001FC, 0xA00ADE72,
+0x655D641C, 0x39EC6953, 0x67536C92, 0x37DC62C2,
+0x75041721, 0x625D1F2C, 0x8BF23243, 0x2D10A309,
+0x8B178801, 0x01FCE020, 0x2D70E700, 0x1FD76D1C,
+0x627DDE65, 0x8B0132D3, 0x0009A2FB, 0x65E3677D,
+0x75046673, 0x36EC6C73, 0x64623C5C, 0x770869C2,
+0x2492AFEF, 0x8B188804, 0x01FCE020, 0x2D40E400,
+0xDE59671C, 0x3273624D, 0xA2E28B01, 0x644D0009,
+0x6CE36D43, 0x65D23DEC, 0x61437C04, 0x621231CC,
+0x74086952, 0xAFED2929, 0x88052592, 0xE0208B18,
+0xE40001FC, 0x671C2D40, 0x624DDE4B, 0x8B013273,
+0x0009A2C7, 0x6943644D, 0x39EC61E3, 0x71046592,
+0x3C1C6C43, 0x6D5262C2, 0x2D2B7408, 0x25D2AFED,
+0x8B1B8831, 0xD942D241, 0x72046422, 0x72046622,
+0x72046722, 0x72E86C22, 0x1F2E1F4D, 0x72046422,
+0x72046E22, 0x652229E0, 0x2950D93A, 0xDE3A2FC6,
+0x55FE4E0B, 0xE2007F04, 0x2D20A29B, 0x8B1D8830,
+0xDE33D232, 0x72046522, 0x72046122, 0x72046722,
+0x72E86922, 0x72046422, 0x72046C22, 0x6E222EC0,
+0x1F9FD62C, 0x7FFC26E0, 0x09FEE040, 0x2F92DC2B,
+0x66134C0B, 0xE2007F04, 0x2D20A27B, 0x89018828,
+0x0009A109, 0xE143DE20, 0xE04062E1, 0x3617662D,
+0x0FE68F03, 0x660302FE, 0x36172201, 0xA0F38B01,
+0xE0400009, 0xE50104FE, 0x30568541, 0xA0EB8B01,
+0xE0400009, 0x09FEE701, 0xB2612D70, 0xE0406491,
+0xE1430CFE, 0xE06862C1, 0x3517652D, 0x0F568D68,
+0x3563E640, 0xE6008B24, 0x0F65E048, 0xA02EE11A,
+0x000072C0, 0x00117800, 0x00202A20, 0x00200F72,
+0x00201FDC, 0x002029B0, 0x00202A24, 0x00200FBC,
+0x002029AF, 0x002025D4, 0x00117804, 0x00117810,
+0x002029AC, 0x002029AD, 0x00200948, 0x00200994,
+0x41216153, 0x41214121, 0x41214121, 0x45214521,
+0x60534521, 0x6603C903, 0x0F65E048, 0xE0077118,
+0xE0442209, 0x641D0F25, 0x65F3E04C, 0x0F46B291,
+0x0EFDE048, 0x0DFDE044, 0x61DD67ED, 0x41084708,
+0x0F16E050, 0xDD946073, 0x4D0B06FE, 0x6E07E00F,
+0x607326E9, 0xE0400F66, 0x65F30CFE, 0x690D85C2,
+0x01FEE050, 0x60934D0B, 0x6073260B, 0xE04C0F66,
+0x04FEB256, 0x07FEE040, 0x6271E068, 0x0F56652D,
+0x3563E640, 0xED008954, 0x0FD5E064, 0xC9036023,
+0x40004008, 0x61036903, 0x0F96E054, 0xDE7EE058,
+0x0FF6ECFF, 0xE06C6CCC, 0x60C30FE6, 0x62534E0B,
+0x42214221, 0x42214221, 0x42006723, 0x6107327C,
+0x4200E05C, 0x0F164521, 0x4521E040, 0x60530CFE,
+0x4008C903, 0x7C0630FC, 0x6E031FC6, 0x1FD56D2D,
+0x1F04A01E, 0x0FD6E060, 0x05FEE058, 0x64D3B231,
+0x62E2E05C, 0xE05409FE, 0x2E222299, 0x64D361C4,
+0x01FE661C, 0x07FEE06C, 0x6063470B, 0xE058220B,
+0xB20505FE, 0xE0642E22, 0x7D0102FD, 0x0F257201,
+0x02FDE064, 0x3262E606, 0xE0408BDC, 0x626106FE,
+0x05FEE040, 0x85514200, 0x302C750C, 0x6103701B,
+0x64F3E600, 0xE704A004, 0x76016256, 0x74042422,
+0x3273626D, 0x65F38BF8, 0x641DB1E2, 0x06FEE040,
+0x6461B19E, 0x0009A175, 0xD74DD44C, 0x470BE201,
+0xA16E2D20, 0x88290009, 0xDE4A8B07, 0x2D20E200,
+0xB16D66E2, 0xA164646D, 0xE2810009, 0x3020622C,
+0xA0A78B01, 0xE0240009, 0x626C06FC, 0x666CE682,
+0x8B213260, 0xE42452FA, 0xD43F2240, 0x12615647,
+0x12625648, 0x12635649, 0x1264564A, 0x1265564B,
+0x1266564C, 0x1267564D, 0x1268564E, 0x1269564F,
+0x1427E200, 0x14291428, 0x142B142A, 0x142D142C,
+0x142F142E, 0x1F6CA135, 0x666CE683, 0x8B073260,
+0xE60052FA, 0xD22B2260, 0x6222D62C, 0x2622A129,
+0x666CE690, 0x8B183260, 0xE60052FA, 0xD2282260,
+0x6022E605, 0x2202CB20, 0x2262D226, 0x2262E600,
+0x460BD625, 0xD2250009, 0x0009420B, 0xE601D224,
+0xD2242262, 0xA10C4618, 0xE6B02262, 0x3260666C,
+0xD5188B22, 0xD216D420, 0x75046D52, 0x6E52420B,
+0x420BD21E, 0xD41E64D3, 0x450BD511, 0xD21B0009,
+0x64E3420B, 0xD60ED41B, 0x0009460B, 0xE600E504,
+0x3253626D, 0xA0EC8B01, 0x666D0009, 0x326C62D3,
+0x22E07601, 0x4E19AFF4, 0xD214D413, 0xD4146542,
+0x0009420B, 0x0009A0DD, 0x0020248C, 0x00202A44,
+0x00200F72, 0x00117804, 0x00202538, 0x00202994,
+0x001C3500, 0x001D4004, 0x00201056, 0x00200C1C,
+0x001E212C, 0x001C3D30, 0x00202A5C, 0x00200FB4,
+0x00202A70, 0x00202A78, 0x00117800, 0x00200FBC,
+0x00202A7C, 0xD6AED4AD, 0x6262E040, 0x76046542,
+0x2452352C, 0x62626563, 0x75045641, 0x1461362C,
+0x62526653, 0x76085542, 0x1452352C, 0x55436262,
+0x352C76EC, 0x65631453, 0x56446262, 0x362C7510,
+0x66531464, 0x55456252, 0x352C7610, 0x65621455,
+0xD69C5246, 0x1426325C, 0x55476262, 0x352C7604,
+0x62621457, 0x76045548, 0x1458352C, 0x62626563,
+0x75045649, 0x1469362C, 0x564A6252, 0x362C7504,
+0x6653146A, 0x554B6252, 0x352C7604, 0x6262145B,
+0x7604554C, 0x145C352C, 0x62626563, 0x7504564D,
+0x146D362C, 0x62526653, 0x7604554E, 0x145E352C,
+0x524F6562, 0x325CD684, 0x6262142F, 0x7694054E,
+0x0456352C, 0x6263E044, 0x054E6662, 0x356C7244,
+0xE0480456, 0x054E6622, 0xD67C356C, 0x62620456,
+0x054EE054, 0x352C4229, 0x76040456, 0xE0586262,
+0x4229064E, 0x52FA362C, 0xE6380466, 0xE0442260,
+0xE048064E, 0x66421261, 0x56411262, 0x56421263,
+0x56451264, 0x56431265, 0x56461266, 0x064E1267,
+0x1268E040, 0xE050064E, 0x56441269, 0x064E126A,
+0x126BE04C, 0xE054064E, 0x064E126C, 0x126DE058,
+0xE044064E, 0xE200126E, 0xE0480426, 0x14212422,
+0x14251422, 0x14261423, 0xE0400426, 0xE0500426,
+0x04261424, 0x0426E04C, 0x0426E054, 0x0426E058,
+0x7F701F6C, 0x6EF64F26, 0x6CF66DF6, 0x69F6000B,
+0x614D4F22, 0x3123E240, 0xE21F8917, 0x89083127,
+0xD550D44F, 0x450BE001, 0x67076642, 0xA00C2679,
+0xE23F2462, 0x89083127, 0xD64AD749, 0xE00171E0,
+0x5571460B, 0x25296207, 0x4F261751, 0x0009000B,
+0x614D4F22, 0x3123E240, 0xE21F8915, 0x89073127,
+0xD240D43F, 0x420B6642, 0x260BE001, 0x2462A00B,
+0x3127E23F, 0xD73A8907, 0x5571D63A, 0x460B71E0,
+0x250BE001, 0x4F261751, 0x0009000B, 0x4618E640,
+0xD5354628, 0x22686252, 0x000B89FC, 0xE6800009,
+0x46284618, 0x6252D530, 0x89FC2268, 0x0009000B,
+0xE200A001, 0x32427201, 0x000B8BFC, 0xE6800009,
+0x46284618, 0x6252D529, 0x8BFC2268, 0x0009000B,
+0x4F222FE6, 0x6E537FFC, 0x2F42BFF1, 0xD62461E2,
+0x1615E280, 0x421854E1, 0x55E21646, 0x16574228,
+0x6EF257E3, 0x2E2B1678, 0x7F0426E2, 0xAFCE4F26,
+0x2FC66EF6, 0x2FE62FD6, 0xDD194F22, 0xBFD66C53,
+0xBFBB6E43, 0xBFD22DE2, 0x51D50009, 0x54D62C12,
+0x55D71C41, 0x56D81C52, 0x4F261C63, 0x6DF66EF6,
+0x6CF6000B, 0xE6006163, 0x4109A004, 0x76016256,
+0x74042422, 0x8BF93612, 0x0009000B, 0x00202538,
+0x001C36A0, 0x001C3CA0, 0x001C36F4, 0x001C3B88,
+0x001C3704, 0x0020248C, 0x001C373C, 0x001C3700,
+0x001C370C, 0x0009A109, 0x2FD62FC6, 0x4F222FE6,
+0x6E636D73, 0x6C53B016, 0x64C357F4, 0xB02965E3,
+0xB03D66D3, 0xB06D0009, 0xB0710009, 0xB0750009,
+0xB08A0009, 0xB08D0009, 0x4F260009, 0x6DF66EF6,
+0x6CF6A0B4, 0x3412D190, 0xD6900529, 0x2650D790,
+0x2742000B, 0x2FD62FC6, 0x4F222FE6, 0x6E636D73,
+0x6C53BFF0, 0x64C357F4, 0x66D365E3, 0x6EF64F26,
+0x6CF66DF6, 0xD1872FE6, 0x66126E63, 0x92BC4418,
+0x44084528, 0x45002629, 0x265B4408, 0x264B4400,
+0x21624708, 0xD1804708, 0x217227EB, 0x6EF6000B,
+0x4F222FE6, 0xE101DE7D, 0xBFABE40A, 0x62E32E12,
+0xE100726C, 0x2212E401, 0x22122212, 0x22122212,
+0x22422212, 0xE503E730, 0x2212E40A, 0x22122212,
+0x22122212, 0x22122212, 0x22122212, 0x22122212,
+0x22722212, 0x22122252, 0x22122212, 0x22122212,
+0x22122212, 0xBF852212, 0xE600121A, 0x4F262E62,
+0x6EF6000B, 0xE101D266, 0x2212E441, 0x2242000B,
+0xD465D164, 0x2162E605, 0x2462000B, 0xD264D563,
+0x88016050, 0xD4638B07, 0x60409668, 0x8B098801,
+0xA0079665, 0xE6000009, 0x2262D45E, 0x88016040,
+0xE6048B00, 0xAF5DE40A, 0xD25B2262, 0xE40AE601,
+0x2262AF58, 0x2FC62FB6, 0x2FE62FD6, 0xDC574F22,
+0x60C2ED00, 0xCB01EB64, 0x60C22C02, 0xA008C901,
+0x3DB26E03, 0x60C28907, 0xC901E40A, 0x6E03BF42,
+0x2EE87D01, 0x3DB28BF5, 0xD44D8B03, 0x420BD24D,
+0xE40A0009, 0x6EF64F26, 0x6CF66DF6, 0x6BF6AF32,
+0x8F014411, 0x6043604B, 0x0009000B, 0x2FC62FB6,
+0x2FE62FD6, 0x7FFC4F22, 0xED00DC40, 0xEB6460C2,
+0x2C02CB02, 0x2F0260C2, 0xA009C902, 0x3DB36E03,
+0x60C28908, 0x2F02E40A, 0xBF13C902, 0x7D016E03,
+0x8BF42EE8, 0x8B0B3DB3, 0xD236D437, 0x4F267F04,
+0x6DF66EF6, 0x422B6CF6, 0x1FFF6BF6, 0x03C40340,
+0x4F267F04, 0x6DF66EF6, 0x000B6CF6, 0xD52F6BF6,
+0x60525651, 0x000B4628, 0x2FB6306C, 0x2FD62FC6,
+0x4F222FE6, 0x4F024F12, 0x6E43BFF1, 0xDC286B03,
+0xBFECDD28, 0x30B80009, 0x060A3C05, 0x46094609,
+0x3D654601, 0x4209020A, 0x42094209, 0x8BF032E2,
+0x4F164F06, 0x6EF64F26, 0x6CF66DF6, 0x6BF6000B,
+0x4F222FE6, 0xE102DE1C, 0xE403E500, 0xBFD42E12,
+0xE6062E52, 0xE7004618, 0x2E62E403, 0x4F262E72,
+0x6EF6AFCB, 0x0009000B, 0x0025E720, 0x00202C3C,
+0x00202998, 0x001C5814, 0x001C59D0, 0x001C5830,
+0x001C6268, 0x001C59A4, 0x001C639C, 0x002029AD,
+0x001C5804, 0x002029AC, 0x001C581C, 0x001C5860,
+0x00202A90, 0x00200F72, 0x00202AA8, 0x001C1040,
+0xCCCCCCCD, 0x10624DD3, 0x001D4004, 0x2F962F86,
+0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6, 0xE4007FE0,
+0x4528E510, 0x67436C43, 0xE108A00F, 0x6043644D,
+0x0F564008, 0xEE0060C3, 0x815125C1, 0x81538152,
+0x157315E2, 0x751415E4, 0x624D7401, 0x8BED3213,
+0xDA7251F1, 0x1A1154F2, 0xD1712A12, 0x56F455F3,
+0x58F657F5, 0x21421141, 0x11521153, 0x11641165,
+0x11761177, 0x11881189, 0xD96A6DF2, 0xDB6A52F7,
+0x29D219D1, 0x2B221B21, 0xD868EB45, 0xE9B8EA50,
+0x4A084B08, 0xA020699C, 0x6EEDEE00, 0x61E36DE3,
+0x41084D08, 0x31EC3DEC, 0x41084D08, 0x60C33D8C,
+0xD75F4108, 0x81D12DC1, 0x410860A3, 0x60C381D2,
+0xE200317C, 0x81D33492, 0x1D131DD2, 0x8D01D456,
+0xD4521D24, 0x65D3B03C, 0x64ED7E01, 0x8BDC34B2,
+0xDB54D14E, 0xD24F6512, 0x1B514529, 0xD14C6412,
+0x2B72674D, 0xD6506722, 0x1B734729, 0x2FD26922,
+0x1B82689D, 0x26926912, 0x16A25A12, 0xDA465B14,
+0x5C1616B4, 0x5D1816C6, 0x6EA216D8, 0x7F2016EA,
+0x6EF64F26, 0x6CF66DF6, 0x6AF66BF6, 0x000B69F6,
+0x664268F6, 0xC8036061, 0xE5008D04, 0xC9036061,
+0x8B038802, 0x65635262, 0x24125124, 0x6053000B,
+0x2FE62FD6, 0x7FEC4F22, 0x62536E53, 0x6D43E550,
+0x4508E400, 0xE101A001, 0x60435224, 0x81212211,
+0x60538123, 0x56E28122, 0x8BF53620, 0x16E4D22F,
+0xE61464F3, 0x65E3420B, 0xE4FC65E1, 0x2E512549,
+0x65F361F1, 0x2F112149, 0xD12854D1, 0xE614410B,
+0x607157D1, 0x2701CB01, 0x7F141DE1, 0x6EF64F26,
+0x6DF6000B, 0x2FE62FD6, 0x7FEC4F22, 0x66536E53,
+0x6D43E5FC, 0x20596061, 0x2601CB01, 0x326052E2,
+0x12E48B06, 0x31E051E2, 0x52D18B04, 0x1E22A002,
+0x5664AFF0, 0x64F3D215, 0x420BE614, 0x67E165E3,
+0x2719E1FC, 0x67F12E71, 0x271954D1, 0x65F3D10F,
+0x410BE614, 0x52D12F71, 0xCB016021, 0x1DE12201,
+0x4F267F14, 0x000B6EF6, 0x00006DF6, 0x0020259C,
+0x002025A4, 0x00202594, 0x002025CC, 0x001000A0,
+0x00101640, 0x001E2108, 0x001C3D00, 0x00200904,
+0x2FC62FB6, 0x2FE62FD6, 0x7FFC4F22, 0x6022D225,
+0x8D35C803, 0xDE242F01, 0xDB25DC24, 0xED01A016,
+0xC9036061, 0x89158801, 0xD122D420, 0x0009410B,
+0x65035603, 0xC8208561, 0xE0508903, 0x720102BE,
+0xD21D0B26, 0x64E3420B, 0x21D2D11C, 0x66C252C1,
+0x8BE53620, 0xDD1AEE01, 0x4E18A00E, 0xC9036061,
+0x890D8801, 0xD713D416, 0x470BDB16, 0xD4160009,
+0x65034B0B, 0x21E2D111, 0x66D252D1, 0x8BED3620,
+0xC80460F1, 0xD2118907, 0x4F267F04, 0x6DF66EF6,
+0x422B6CF6, 0x7F046BF6, 0x6EF64F26, 0x6CF66DF6,
+0x6BF6000B, 0x001E2100, 0x002025A4, 0x0020259C,
+0x00202538, 0x00200D42, 0x00200DC4, 0x001C3D30,
+0x00202594, 0x00200D60, 0x002025CC, 0x00200100,
+0xE601D203, 0x1265D503, 0x000B2252, 0x00001266,
+0x001C1010, 0x0000C34F, 0xD62A7FFC, 0x2642644C,
+0xC8205066, 0x2F028DFC, 0x7F04000B, 0x2FD62FC6,
+0x4F222FE6, 0x6D436C53, 0xEE00A004, 0x7E0164D4,
+0x644CBFEA, 0x8BF93EC2, 0x6EF64F26, 0x000B6DF6,
+0xA0016CF6, 0x76016643, 0x22286260, 0x36488BFB,
+0x6563AFE4, 0x62532FE6, 0x67537507, 0xEE0AE108,
+0xC90F6043, 0x30E24409, 0x44096503, 0xE6378D01,
+0x365CE630, 0x27604110, 0x77FF8FF2, 0x8028E000,
+0x6EF6000B, 0xE000000B, 0xE000000B, 0x4F222FE6,
+0x62537FEC, 0x65F36E43, 0x6423BFDC, 0x64E3BFD1,
+0x64F3BFCF, 0xBFCCD404, 0x7F140009, 0x000B4F26,
+0x00006EF6, 0x001C0004, 0x00202AC4, 0xE110D5A1,
+0xE6406050, 0x2500C9FD, 0xE0FF75E9, 0x80516453,
+0x80538052, 0x80568055, 0x251075EF, 0xE1EF6250,
+0x2219E001, 0xE7202520, 0x24608052, 0x2570000B,
+0xE4FDD595, 0xE7026152, 0x25122149, 0x74016052,
+0x2502CB01, 0xD1916652, 0x25622649, 0x92C46012,
+0x2102CB08, 0xC9CF6012, 0x60122102, 0x2102CB03,
+0x000B1172, 0x4F221123, 0xD78AD589, 0xD48BD28A,
+0xE600E100, 0x27112511, 0xBFBF2210, 0xAFD72461,
+0x664C4F26, 0x4600D286, 0x6060362C, 0x000BCB10,
+0x654C2600, 0x4500D282, 0x6650352C, 0x2619E1EF,
+0x2560000B, 0xD27F664C, 0x362C4600, 0xCB106060,
+0x2600000B, 0xD27B654C, 0x352C4500, 0xE1EF6650,
+0x000B2619, 0x664C2560, 0x4600D275, 0x6060362C,
+0x000BCB08, 0x654C2600, 0x4500D271, 0x6650352C,
+0x2619E1F7, 0x2560000B, 0xD26E664C, 0x362C4600,
+0xCB086060, 0x2600000B, 0xD26A654C, 0x352C4500,
+0xE1F76650, 0x000B2619, 0x624C2560, 0x4200D664,
+0x6020326C, 0x4021C908, 0x40214021, 0x600C000B,
+0xD660624C, 0x326C4200, 0xC9086020, 0x40214021,
+0x000B4021, 0x644C600C, 0x74FFD15B, 0x6240341C,
+0x602C000B, 0x644CD159, 0x6240341C, 0x602C000B,
+0x4F222FE6, 0xE60A655C, 0x8D153567, 0xBFEA6E43,
+0x640C6453, 0x880160EC, 0xE00F8B02, 0x2409A002,
+0x44094409, 0xE60A624C, 0x89053263, 0x644CBFE2,
+0x6023620C, 0x8B00C880, 0x6023E200, 0x000B4F26,
+0x4F226EF6, 0x6062D646, 0x8B038801, 0x0009B241,
+0x0009A003, 0xE640D243, 0xD6432260, 0x4F26E200,
+0x2622000B, 0xD63E4F22, 0x88026062, 0xB28B8B01,
+0xD63D0009, 0x4F26E200, 0x2622000B, 0xD439D538,
+0xE701E100, 0x000B2512, 0x0FFF2470, 0xE604D235,
+0x2260000B, 0xD4354F22, 0x410BD135, 0xD5250009,
+0x6650E1FD, 0x2619D233, 0x2560E700, 0x000B4F26,
+0x4F222270, 0xD12ED430, 0x0009410B, 0xE7FBD51D,
+0x26796650, 0x000B4F26, 0x4F222560, 0xD128D42B,
+0x0009410B, 0xE7F7D517, 0x26796650, 0x000B4F26,
+0xD5142560, 0x62509425, 0x000B2249, 0xD5112520,
+0x6250E4BF, 0x000B2249, 0x4F222520, 0x8522D220,
+0x2008600D, 0x88018911, 0x8803893C, 0x8805893E,
+0x88068940, 0x88088946, 0x8809894C, 0x880A8952,
+0x880B8958, 0xA065895E, 0xB0670009, 0xA0620009,
+0xFF7F600C, 0x001E1028, 0x001E2148, 0x001E1108,
+0x002029DC, 0x002029DE, 0x002029E9, 0x002029C0,
+0x001E103F, 0x001E105F, 0x001E1030, 0x001E1090,
+0x002029E4, 0x001E100B, 0x002029E0, 0x00202AC8,
+0x00200F72, 0x002029E8, 0x00202AD4, 0x00202AE4,
+0x002029B4, 0x0009B04C, 0x600CA035, 0x0009B056,
+0x600CA031, 0x6260D67C, 0x8B2B2228, 0x0009B062,
+0x600CA029, 0x6260D678, 0x8B232228, 0x0009B06A,
+0x600CA021, 0x6260D674, 0x8B1B2228, 0x0009B0B4,
+0x600CA019, 0x6260D670, 0x8B132228, 0x0009B0BA,
+0x600CA011, 0x6260D66C, 0x8B0B2228, 0x0009B11A,
+0x600CA009, 0x6260D668, 0x8B032228, 0x0009B132,
+0x600CA001, 0x4F26E000, 0x0009000B, 0xD264D163,
+0xD5648412, 0x4000C90F, 0xD763012D, 0x611CE403,
+0xD662E20F, 0x27122540, 0xE0012520, 0x2602000B,
+0xE601D25A, 0x30668523, 0xE0008D06, 0xE000D258,
+0x8122D65A, 0x2602E001, 0x0009000B, 0x8523D253,
+0x2008600D, 0x88018905, 0xD6558B0A, 0xCB016060,
+0xD6522600, 0xE101D44E, 0x2612E001, 0x8142000B,
+0xE000000B, 0xE501D149, 0x45188513, 0x3453640D,
+0x8D056603, 0xD24BE000, 0xE001D548, 0x25022260,
+0x0009000B, 0xD1414F22, 0x650D8513, 0x44196453,
+0x672E6249, 0x602C227D, 0x89098801, 0x890C8802,
+0x89108803, 0x89268806, 0x89298807, 0x0009A038,
+0xD63ED53D, 0xA027E212, 0x625C2652, 0x8B2F2228,
+0xA01ED63B, 0x605C6262, 0x89052008, 0x89088810,
+0x890B8820, 0x0009A024, 0xD634D436, 0xA013E204,
+0xD7352642, 0xE20CD631, 0x2672A00E, 0xD62FD533,
+0xA009E218, 0xD4322652, 0xE20AD62C, 0x2642A004,
+0xD62AD230, 0xE22E2622, 0xD42F8515, 0x3277670D,
+0x8F012421, 0x24516503, 0x0009B0DB, 0xE001A001,
+0x4F26E000, 0x0009000B, 0xE101D61A, 0x2610D427,
+0xD7196541, 0x655DD119, 0xE001E20F, 0x26202752,
+0x2102000B, 0x4F222FE6, 0x8523D210, 0x2448640C,
+0xD61E8B08, 0xE200D512, 0x84512621, 0x20499412,
+0x8051A050, 0x60E0DE0E, 0x8D35C840, 0x3427E201,
+0xD116894C, 0x420BD216, 0xD5162141, 0xCB046052,
+0x2502A035, 0x0000FF7F, 0x002029E9, 0x002029B4,
+0x002029C0, 0x001E1100, 0x001E100C, 0x002029E0,
+0x001E1000, 0x001E1001, 0x00202C40, 0x002029C8,
+0x002029D0, 0x00202CAE, 0x00202CB2, 0x00202CBE,
+0x00202CD6, 0x00202CE0, 0x002029CC, 0x002029DA,
+0x00201DB6, 0x001E1108, 0x89173427, 0xD794D293,
+0x2241470B, 0xE5FBD693, 0x21596162, 0x84E12612,
+0xB0FFCB80, 0x60E080E1, 0xCB04D68F, 0x60602E00,
+0x2600C93F, 0xE001D68D, 0x2602A001, 0x4F26E000,
+0x6EF6000B, 0x6060D68A, 0x8919C880, 0x6021D283,
+0x8B158801, 0xE501D287, 0x30568524, 0xD1868910,
+0xD486E203, 0x65412120, 0x655DE00B, 0xD5840656,
+0xE702E40F, 0x25712140, 0xE001D77C, 0x2702000B,
+0xE000000B, 0x4F222FE6, 0x84E1DE7E, 0x8934C880,
+0x8554D578, 0x8F302008, 0xD77B6103, 0x66728553,
+0x650C6403, 0x620C8566, 0x8B263520, 0xD773D677,
+0x644C651C, 0x27412651, 0xC84060E0, 0xD2748907,
+0x0009420B, 0x6062D667, 0xA008CB04, 0xD1642602,
+0x0009410B, 0xE5FBD663, 0x24596462, 0xB0A12642,
+0xD5620009, 0x2522E201, 0xD75F60E0, 0x2E00CB04,
+0xC93F6070, 0xA0012700, 0xE0006023, 0x000B4F26,
+0x2FA66EF6, 0x2FC62FB6, 0x2FE62FD6, 0xE240DA5C,
+0xDC5966A1, 0x3123616D, 0x62638900, 0x6ED36D2C,
+0x4E2136D8, 0x4E212A61, 0xDB5BD45A, 0xE700A00F,
+0x770166B2, 0x71026163, 0x65612B12, 0x71026613,
+0x62612B12, 0x622D655D, 0x325C4228, 0x627C2422,
+0x8BED32E3, 0xC90360D3, 0x8B108803, 0xED076EB2,
+0x710261E3, 0x67132B12, 0x62E17102, 0x65712B12,
+0x655D622D, 0x352C4528, 0xA00C2CD0, 0x88022452,
+0xA0038B01, 0x8801E203, 0xE2018B05, 0x66B22C20,
+0x677D6761, 0xEB0F2472, 0x6DA12CB0, 0x8B052DD8,
+0xD432D23E, 0xE101EE00, 0x241222E2, 0x6DF66EF6,
+0x6BF66CF6, 0x6AF6000B, 0x2FE62FD6, 0xE240DD30,
+0x616D66D1, 0x89003123, 0x672C6263, 0xDE323678,
+0x2D617703, 0xD62F4721, 0x472164E2, 0xE100A00E,
+0x71016562, 0x24506253, 0x42197401, 0x74012420,
+0x24504529, 0x45197401, 0x74012450, 0x3273621C,
+0x42008BEE, 0x64D166E2, 0x362C4200, 0x8F062448,
+0xDD222E62, 0xE500DE15, 0x2D52E701, 0x6EF62E72,
+0x6DF6000B, 0x2FE62FD6, 0xEE014F22, 0xED0AA005,
+0x64E3BC97, 0x64E3BC9D, 0x62EC7E01, 0x8BF732D7,
+0xEE01A005, 0x64E3BC9E, 0x64E3BCA4, 0x62EC7E01,
+0x8BF732D7, 0x6EF64F26, 0x6DF6000B, 0x002029DA,
+0x00201EC2, 0x001E1108, 0x001E1015, 0x002029E0,
+0x001E1001, 0x002029B4, 0x001E1100, 0x002029DE,
+0x002029CC, 0x001E1000, 0x002029D0, 0x002029DC,
+0x00201DB6, 0x001E100C, 0x002029C8, 0x002029E4,
+0x2FE62FD6, 0x7FFC4F22, 0x6060D64C, 0x89488801,
+0xE101D44B, 0xD74B8548, 0x650D2610, 0x45196070,
+0x6659DD49, 0x61D3626E, 0xC840262D, 0x74027102,
+0x8D1AD746, 0xD246666C, 0xE501DE46, 0xA0042E22,
+0x6245EE04, 0x21217501, 0x625C7102, 0x8BF832E3,
+0x81D46063, 0xD540E601, 0x626CE417, 0x891E3243,
+0x76016255, 0xAFF82721, 0xD23C7702, 0xE501DE39,
+0xA0042E22, 0x6245EE04, 0x21217501, 0x625C7102,
+0x8BF832E3, 0x81D46063, 0xD535E601, 0xE417A004,
+0x76016255, 0x77022721, 0x3243626C, 0x924B8BF8,
+0xD4302D21, 0x6142D730, 0x65F22F12, 0x60536DF2,
+0x2700C980, 0xC9606053, 0x80716103, 0x6EF26053,
+0xC90365F2, 0x45294D19, 0x60DC8072, 0x81724519,
+0x605C4E29, 0x401862EC, 0x8173302C, 0x21186D42,
+0x6EF22FD2, 0x66F262F2, 0x46294219, 0x66F2656C,
+0x64EC602C, 0x46294018, 0x4619304C, 0x606C8174,
+0x305C4018, 0x81758F07, 0x0009BCBF, 0x2228620C,
+0xA00A8908, 0x60130009, 0x8B038840, 0x0009B00A,
+0x0009A003, 0xE202D611, 0x7F042622, 0x6EF64F26,
+0x6DF6000B, 0x0009000B, 0x0000060A, 0x002029E8,
+0x00202C40, 0x001E1000, 0x00202CD6, 0x00202CE2,
+0x00202C52, 0x002029D0, 0x00202C82, 0x00202C80,
+0x00202C54, 0x001E100C, 0x002029B4, 0x002029E0,
+0x4F222FE6, 0xDE907FFC, 0x200884E9, 0x2F008D06,
+0xD68FD48E, 0x0009460B, 0x64F0B146, 0x6620D28D,
+0x89022668, 0xC9BF60E0, 0x7F042E00, 0x000B4F26,
+0x000B6EF6, 0x2FE60009, 0xDE874F22, 0x60E0D687,
+0xCBC0D487, 0x62602E00, 0xC803602C, 0x40218904,
+0x70014021, 0x6603A002, 0x66034009, 0xD681616D,
+0xE500A004, 0x75016262, 0x74042422, 0x3213625D,
+0xD27D8BF8, 0x0009420B, 0xC9BF84E2, 0x4F2680E2,
+0x6EF6000B, 0x2FD62FC6, 0x4F222FE6, 0xDC727FFC,
+0x84C2D276, 0xCB40DD76, 0x80C2420B, 0x8D042008,
+0x62E06E03, 0xA006642C, 0xD66A7404, 0x6160D471,
+0x470BD771, 0x644D651C, 0x45216543, 0xA0044521,
+0x62E6E600, 0x2F227601, 0x626D2D22, 0x8BF83253,
+0xC9036043, 0x89122008, 0x89058803, 0x89068802,
+0x89078801, 0x0009A008, 0xA005E007, 0xE00380D8,
+0x80D8A002, 0x80D8E001, 0x2F2262E2, 0xE00F2D22,
+0x80D8D65E, 0xCB086060, 0x60C02600, 0x2C00C93F,
+0x4F267F04, 0x6DF66EF6, 0x6CF6000B, 0x2FC62FB6,
+0x2FE62FD6, 0xD2564F22, 0x6E436D73, 0x420B6B53,
+0x20086C63, 0x64038F08, 0xD245D452, 0x6EF64F26,
+0x6CF66DF6, 0x6BF6422B, 0x24E060B3, 0x60C38041,
+0xA0078141, 0x655DE500, 0x00DC6053, 0x324C6253,
+0x80247501, 0x6EEC625D, 0x8BF432E3, 0x6060D636,
+0x2600C9BF, 0x6EF64F26, 0x6CF66DF6, 0x6BF6000B,
+0x7FC44F22, 0x720262F3, 0x22512F41, 0x45297202,
+0x60632251, 0xE5C4E682, 0x67F38121, 0x655C666C,
+0xE408BFBC, 0x4F267F3C, 0x0009000B, 0xD237D136,
+0xE4056012, 0xE500CB20, 0x22422102, 0x2252000B,
+0xD534D133, 0xE400D734, 0x2142E20F, 0x17411154,
+0xD5322722, 0x9635D732, 0x15412572, 0x96321562,
+0xE6011565, 0xD52F1165, 0x666CE6F8, 0x25422542,
+0x25422542, 0x25422542, 0x25622542, 0x7601E727,
+0x67632572, 0x25627797, 0xE7042572, 0x2572E248,
+0xE2192522, 0xE2702522, 0x25422542, 0x25422542,
+0x25222542, 0x2522E20C, 0x25422542, 0x25422542,
+0x25422542, 0x25422542, 0x000B154A, 0xE2081145,
+0x0009422B, 0x51630601, 0x001E1017, 0x00202AF0,
+0x00200F72, 0x002029B0, 0x001E1015, 0x001E10BF,
+0x00117800, 0x001E10FC, 0x00200100, 0x0020201A,
+0x001E10F8, 0x00202AF4, 0x00200FBC, 0x001E10AE,
+0x00201FDC, 0x00202B10, 0x001C3500, 0x001D4004,
+0x001C581C, 0xA000A000, 0x001D0100, 0x001D4000,
+0x00040021, 0x001C589C, 0x2FE62FD6, 0x7FFC4F22,
+0xC8206043, 0x6E438D02, 0x0009BEBB, 0xC81060E3,
+0xBEB88901, 0x60E30009, 0x8901C840, 0x0009BEDA,
+0xC80160E3, 0xDD378936, 0xC80260D0, 0x2F008D03,
+0x460BD635, 0x60F00009, 0x8902C804, 0x460BD633,
+0x62F00009, 0xC8806023, 0x60D08902, 0x2D00C97F,
+0xC8016023, 0xD62E8904, 0x0009460B, 0x0009A005,
+0x8902C808, 0x460BD62B, 0x60F00009, 0x8902C810,
+0x420BD229, 0xD5290009, 0x88026052, 0xD2288B03,
+0xA005E604, 0x88012260, 0xD2258B02, 0x2260E601,
+0x2522E200, 0xC88060E3, 0xD622892E, 0x60E36E60,
+0x8902C880, 0x420BD220, 0x60E30009, 0x8902C840,
+0x420BD21E, 0x60E30009, 0x8902C802, 0x420BD21C,
+0x60E30009, 0x890EC804, 0x410BD11A, 0xBF150009,
+0xBF1D0009, 0xD5180009, 0x6050D418, 0xC908D718,
+0xBF542500, 0x60E32472, 0x8905C808, 0x7F04D215,
+0x6EF64F26, 0x6DF6422B, 0x4F267F04, 0x000B6EF6,
+0x00006DF6, 0x001E1021, 0x00201182, 0x002011A4,
+0x002017B0, 0x002011BC, 0x002011CC, 0x002029E0,
+0x001E100B, 0x001E1028, 0x00201222, 0x0020122E,
+0x002011D4, 0x002011F2, 0x001E1000, 0x0010F100,
+0x12345678, 0x0020120A, 0xD6A8644C, 0x346C74FF,
+0x2450000B, 0x644CD6A6, 0x000B346C, 0xD6A52450,
+0x346C644C, 0x2450000B, 0x616D625C, 0x41194208,
+0x60194208, 0x644C4200, 0x324C670E, 0x207DD19E,
+0xC90F4200, 0x000B321C, 0x67632200, 0x4208625C,
+0x42004208, 0x324C644C, 0x4200D198, 0x000B321C,
+0x2FE62270, 0x614C4F12, 0x4100D493, 0x6710314C,
+0x2729E29F, 0x65736E53, 0x4719676D, 0x672E6279,
+0x4221227D, 0x42214221, 0x7601662C, 0xE4014608,
+0x34E84608, 0x644C4600, 0x0E1A0467, 0x215025EB,
+0x000B4F16, 0x4F226EF6, 0xD2857FE8, 0x88016021,
+0xD2848B7B, 0x26686621, 0xD2838B77, 0x26686621,
+0xE50F8B73, 0xE401BFA0, 0xBFA3E501, 0xE586E400,
+0xE400655C, 0x2F50BFA3, 0xBFA0E401, 0xE602E506,
+0x60634618, 0x81F2E401, 0x6543BF9E, 0xE40185F2,
+0xBFAA6543, 0x85F26603, 0x6543E401, 0x6603BFB1,
+0xE40265F0, 0x6053756C, 0x80F8BF7E, 0xBF81E402,
+0x84F8E512, 0x7090E402, 0x6503BF81, 0x4618E602,
+0x81F66063, 0xBF7FE402, 0x85F6E500, 0x6603E402,
+0xE500BF8B, 0xE40285F6, 0xBF926603, 0xE5FEE500,
+0xE010655C, 0xBF5FE403, 0xE5130F54, 0xE40EBF62,
+0x05FCE010, 0xBF62E40E, 0xE5007585, 0xBF63E403,
+0xE500E640, 0xBF70E403, 0xE500E640, 0xBF78E403,
+0xE5FFE640, 0xE014655C, 0xBF45E404, 0xE40F0F54,
+0xE504BF48, 0x05FCE014, 0xBF48E40F, 0xE5017584,
+0xBF49E640, 0xE501E404, 0xBF56E640, 0xE501E404,
+0xE404E640, 0xAF5C7F18, 0x7F184F26, 0x000B4F26,
+0x4F220009, 0xD2427FF0, 0x88016021, 0xD2418B71,
+0x26686621, 0xD2408B6D, 0x26686621, 0xE50F8B69,
+0xE401BF1A, 0xBF1DE501, 0xE586E400, 0xE400655C,
+0x2F50BF1D, 0xBF1AE401, 0xE401E506, 0xBF1B6543,
+0xE401E640, 0xBF286543, 0xE401E640, 0xBF306543,
+0x65F0E640, 0x756CE402, 0xBEFD6053, 0xE40280F4,
+0xE512BF00, 0xE40284F4, 0xBF007090, 0xE6406503,
+0xBF01E402, 0xE640E500, 0xBF0EE402, 0xE640E500,
+0xBF16E402, 0xE5FEE500, 0x6053655C, 0xBEE3E403,
+0xE51380F8, 0xE40EBEE6, 0xE40E84F8, 0xBEE67085,
+0xE5006503, 0xBEE7E640, 0xE500E403, 0xBEF4E640,
+0xE500E403, 0xBEFCE640, 0xE5FFE403, 0x6053655C,
+0xBEC9E404, 0xE40F80FC, 0xE504BECC, 0xE40F84FC,
+0xBECC7083, 0xE5016503, 0xBECDE640, 0xE501E404,
+0xBEDAE640, 0xE501E404, 0xE404E640, 0xAEE07F10,
+0x7F104F26, 0x000B4F26, 0x00000009, 0x001E1030,
+0x001E1080, 0x001E1090, 0x001E103F, 0x001E103E,
+0x002029DA, 0x002029DC, 0x002029DE, 0xD21DD11C,
+0x66206010, 0x676C7001, 0x3700C90F, 0xE5008D13,
+0x67106210, 0x7701622C, 0x64232170, 0xD6166010,
+0x44084408, 0x3428C90F, 0x62602100, 0x7201D513,
+0x44082620, 0x000B354C, 0xD10F6053, 0x25586510,
+0xE6008D13, 0xD60DD40B, 0x655C6540, 0x47086753,
+0x37584708, 0x47086540, 0x24507501, 0x367C6040,
+0x2400C90F, 0x72FF6210, 0x000B2120, 0x00006063,
+0x002029AF, 0x002029AE, 0x002029B0, 0x002025D4,
+0x7FFC4F22, 0xE680D19D, 0x666C6212, 0xD29C2F22,
+0x67F36563, 0x420B7542, 0x7F04E404, 0x000B4F26,
+0xE6800009, 0xD296666C, 0xE7006563, 0x422B7540,
+0xE6806473, 0xD292666C, 0xE7006563, 0x422B7543,
+0x2FB66473, 0x2FD62FC6, 0x4F222FE6, 0x4D18ED01,
+0xDB8DDC8C, 0x65C252C1, 0x89203520, 0xC9036051,
+0x891C8801, 0xD189DE87, 0x64E3410B, 0x85036503,
+0x670D66B2, 0x89073762, 0xD286D485, 0x0009420B,
+0xE701D185, 0x2172AFE6, 0xDE8464E3, 0x00094E0B,
+0xD484D683, 0x410BD184, 0xAFDB26D2, 0x4F260009,
+0x6DF66EF6, 0x000B6CF6, 0x4F226BF6, 0x85467FF4,
+0x2F01E681, 0x666C8547, 0x854881F1, 0x81F2D270,
+0x67F38542, 0x854381F3, 0x81F4E40C, 0x65636053,
+0x420B81F5, 0x7F0C7540, 0x000B4F26, 0x2F860009,
+0x2FA62F96, 0x2FC62FB6, 0x2FE62FD6, 0x7FF44F22,
+0xDC6EE200, 0x2F21A136, 0xDD6D6A13, 0xE0014A08,
+0x4D0BD96C, 0x3A9C4A00, 0x1F917930, 0x66C21F02,
+0x362052C1, 0xA1218B01, 0x60610009, 0x8801C903,
+0xA11B8B01, 0x85610009, 0x8977C801, 0x85D25D63,
+0xC9036603, 0x85D36403, 0x6053650D, 0x40214021,
+0x4500C93F, 0x322A6103, 0x6053252D, 0xC901E210,
+0xD9553123, 0x6E038D21, 0x4408D757, 0x44086570,
+0x44006213, 0x25584200, 0x342C8F0E, 0x6043D253,
+0x60E3072D, 0x4B196B7D, 0x658E68B9, 0x285D8801,
+0x6B7C8F0B, 0x6B13A009, 0x6043D24D, 0x61ED0E2D,
+0x68194119, 0x287D678E, 0xD14A6BEC, 0x22286212,
+0xEBFF8901, 0xEEFF6BBC, 0x6EEC65BD, 0x8B0F35E0,
+0x4D0BDD36, 0x540364C3, 0xBF76E502, 0xD4426D03,
+0x410BD136, 0xD74165D3, 0xD441EE01, 0x27E2A01D,
+0x26E9EEFC, 0x81D26063, 0x914E85D3, 0x81D32019,
+0x450885D2, 0x81D2208B, 0xE20885D3, 0x81D3205B,
+0x421885D4, 0x81D4202B, 0x854164C2, 0x814120E9,
+0xD43465C2, 0xCB016051, 0x490B2501, 0x60C20009,
+0x52F256F1, 0x2A02CB01, 0x2622AF79, 0x420BD21B,
+0x5E0364C3, 0x85E16D03, 0x6053650D, 0x897BC820,
+0x6210D129, 0x8B112228, 0xD72785EF, 0x4221620D,
+0x42214221, 0xE501D625, 0x27504221, 0xD725D924,
+0x2621D425, 0x2960E600, 0x24612762, 0x852162C2,
+0x8B43C802, 0xD912D71E, 0xE0016270, 0x612C490B,
+0x6692D91C, 0xA03E260B, 0x7E032962, 0x001C3D9C,
+0x00201A3C, 0x002025CC, 0x00202994, 0x00200D42,
+0x00202594, 0x00200DC4, 0x001E2130, 0x00200D60,
+0x001C3D30, 0x00202C28, 0x00200F72, 0x002025A4,
+0x0020248C, 0x001C3D00, 0x00202C3C, 0x00202B28,
+0x00202BA8, 0x002029A8, 0x0020259C, 0x001E212C,
+0x00202C2C, 0x00202C30, 0x00202D10, 0x002029EE,
+0x002029EC, 0x002029F0, 0x002029F4, 0xE04CD139,
+0x7201021E, 0xD9380126, 0x6290D438, 0x72016541,
+0x29207501, 0x85E12451, 0x4618E640, 0x891D2068,
+0xD934D733, 0x665D6171, 0x6592D733, 0x641D470B,
+0xE200DE32, 0x2E20A012, 0xE90885E4, 0x49186203,
+0x32902299, 0xE5018B04, 0x64E3BEB7, 0x0009A006,
+0x2598D92B, 0xE5008902, 0x64E3BEAF, 0xD22AD429,
+0x65D3420B, 0xEE01D729, 0x27E2AED9, 0x7C0862F1,
+0x2F217201, 0xEE0462F1, 0x31E7612D, 0xAEC38901,
+0x7F0C0009, 0x6EF64F26, 0x6CF66DF6, 0x6AF66BF6,
+0x000B69F6, 0x2FE668F6, 0xD21D4F22, 0x60E36E22,
+0x8D02C840, 0xBE3322E2, 0xE2400009, 0x2E284218,
+0xBE3E8901, 0x60E30009, 0x8905C810, 0xD216D415,
+0x0009420B, 0x0009BE3D, 0xC80560E3, 0xBE8E8901,
+0x60E30009, 0x8902C802, 0xAE3A4F26, 0x4F266EF6,
+0x6EF6000B, 0x00202538, 0x002029EC, 0x002029F4,
+0x002029EE, 0x002029F0, 0x00201AA0, 0x00202D10,
+0x00008000, 0x0020259C, 0x00200D60, 0x001E212C,
+0x001C3510, 0x00202C34, 0x00200F72, 0x080A0C0E,
+0x00020406, 0x1A1C1E20, 0x12141618, 0x2E303234,
+0x26282A2C, 0x3A3C3E40, 0x6C625648, 0x41112F26,
+0xE2208F18, 0x890B3123, 0x321CD204, 0xD1026220,
+0x412B312C, 0x00090009, 0x002024B6, 0x0020246C,
+0x000BE000, 0x400062F6, 0x40004000, 0x40004000,
+0x40004000, 0x62F6000B, 0x40004000, 0x40004000,
+0x40004000, 0x40184000, 0x62F6000B, 0x40004000,
+0x40004000, 0x40004000, 0x40284000, 0x62F6000B,
+0x40004000, 0x40184000, 0x000B4028, 0xC90F62F6,
+0x40054005, 0x40054005, 0x62F6000B, 0x4005C907,
+0x40054005, 0x62F6000B, 0x4005C903, 0x000B4005,
+0xC90162F6, 0x000B4005, 0x000062F6, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x544F0D0A, 0x46205355,
+0x00003A57, 0x2079614D, 0x32203033, 0x20373030,
+0x333A3231, 0x38313A37, 0x00000000, 0x00000D0A,
+0x00000043, 0x42707372, 0x3D206675, 0x554E203D,
+0x202C4C4C, 0x6E49677A, 0x4E497274, 0x6D754E51,
+0x0000003D, 0x61766E49, 0x2064696C, 0x72657375,
+0x20726F20, 0x2079656B, 0x00214449, 0x52504545,
+0x57204D4F, 0x65746972, 0x6461202C, 0x003D7264,
+0x6C617620, 0x0000003D, 0x00000A0D, 0x6E6B6E55,
+0x206E776F, 0x6D6D6F63, 0x3D646E61, 0x00000000,
+0x61437748, 0x7262696C, 0x6F697461, 0x6620206E,
+0x0A6C6961, 0x0000000D, 0x73696F4E, 0x61432065,
+0x7262696C, 0x6F697461, 0x6166206E, 0x21216C69,
+0x00000D0A, 0x00000D0A, 0x62735576, 0x7473725F,
+0x00000A0D, 0x62735576, 0x7375735F, 0x646E6570,
+0x00000A0D, 0x62735576, 0x7365725F, 0x000A0D6D,
+0x00000042, 0x72746E49, 0x6D652051, 0x2C797470,
+0x49677A20, 0x4972746E, 0x754E514E, 0x00003D6D,
+0x20746F4E, 0x756F6E65, 0x49206867, 0x4220514E,
+0x0A0D6675, 0x00000000, 0x000000FF, 0x00020001,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x010E010D, 0x00020003,
+0x01090108, 0x0002010A, 0x00030002, 0x02020201,
+0x02040203, 0x02060205, 0x02080207, 0x020A0209,
+0x020C020B, 0x020E020D, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x010E010D, 0x00FF010F,
+0x01090108, 0x010B010A, 0x00030002, 0x02020201,
+0x02040203, 0x02060205, 0x02080207, 0x020A0209,
+0x020C020B, 0x020E020D, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00205220, 0x00000046,
+0x00000059, 0x49544120, 0x0000204D, 0x00000000,
+0x02000112, 0x40FFFFFF, 0x91700CF3, 0x20104890,
+0x02090100, 0x0101002E, 0x09FA8000, 0x04000004,
+0x000000FF, 0x02010507, 0x07000200, 0x00028205,
+0x05070002, 0x00400383, 0x04050701, 0x01004003,
+0x002E0209, 0x80000101, 0x000409FA, 0x00FF0400,
+0x05070000, 0x00400201, 0x82050700, 0x00004002,
+0x03830507, 0x07010040, 0x40030405, 0x03040100,
+0x030C0409, 0x0079005A, 0x00410044, 0x03180053,
+0x00530055, 0x00320042, 0x0030002E, 0x00570020,
+0x0041004C, 0x0000004E, 0x00000000, 0x00000000,
+0x00000709, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, };
+
+const u32_t zcFwImageSize=11540;
diff --git a/drivers/staging/otus/hal/hpfwu_2k.c b/drivers/staging/otus/hal/hpfwu_2k.c
new file mode 100644
index 00000000000..94e2caca536
--- /dev/null
+++ b/drivers/staging/otus/hal/hpfwu_2k.c
@@ -0,0 +1,1016 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include "cprecomp.h"
+
+const u32_t zcFwImage[] = {
+0x0009000B, 0x4F222FE6, 0xDE947FFC, 0xE114D594,
+0x1E13D494, 0x67521E4C, 0xD494D693, 0x37402769,
+0x62528F06, 0x7201D692, 0x60602522, 0x2600C93F,
+0xD7906152, 0x2512611D, 0x264B6652, 0x2562470B,
+0x0009B017, 0xE60095AC, 0xC84060E2, 0x2F028F03,
+0x8FF93652, 0xD4887601, 0x4E0BDE88, 0xD4880009,
+0x00094E0B, 0x4E0BD487, 0x7F040009, 0xA0524F26,
+0x4F226EF6, 0x410BD184, 0xD4840009, 0x0009440B,
+0x450BD583, 0xD7830009, 0xD283E1FF, 0x2712611D,
+0xD4825029, 0xE1FFCB01, 0x1209E501, 0x12112212,
+0xE7202452, 0x4718D57E, 0x2572D27E, 0xD17EE700,
+0xD67FD47E, 0xE2012270, 0x24702172, 0xD67D2620,
+0x2641E4FF, 0xD57CE600, 0x666DE104, 0x76016063,
+0x4000626D, 0x8FF83212, 0xD5780545, 0x2520E201,
+0xD278D777, 0xE480E100, 0x22122710, 0x6613D576,
+0x666D644C, 0x76046763, 0x375C626D, 0x8FF83243,
+0xD5722712, 0xD273D772, 0xE400E101, 0x27102511,
+0x000B4F26, 0x7FCC2242, 0xD170D56F, 0xD271DB70,
+0x1F51D471, 0xD6717508, 0x1F12D771, 0x1F55710C,
+0x1FB975FC, 0x72041F2A, 0x1F13EB10, 0x1F561F44,
+0x1F781F67, 0xD86B1F2B, 0xDD6CD96B, 0xDC6CEA00,
+0xD26DDE6C, 0x89003A22, 0xD15D7A01, 0x88016010,
+0x56F88B03, 0x4218E201, 0xD1682622, 0x0009410B,
+0x440BD467, 0xD5670009, 0x0009450B, 0x6010D150,
+0x8B108801, 0xE650D14F, 0x46186212, 0x8B083266,
+0x56F9D14B, 0x2120E200, 0xCB016062, 0x2602A003,
+0x72012710, 0x60822122, 0x89098801, 0xE2C8D15A,
+0x622C6612, 0x89033626, 0x6010D158, 0x8BC88801,
+0x51F66792, 0x217252F5, 0xD6555191, 0x55FA2212,
+0x52FB6462, 0x55612542, 0x2252E400, 0x61436643,
+0x05DE6013, 0x36CC4608, 0x07DE2652, 0xC9036071,
+0x8B028801, 0x720162E2, 0x74012E22, 0x36B3664C,
+0x71048FEE, 0x66C2D147, 0x45286512, 0x265B4518,
+0x60822C62, 0x89018801, 0x0009A168, 0x6272D742,
+0x8B132228, 0xD42BD741, 0x6772D541, 0x51536242,
+0x312C327C, 0x24222228, 0x15138D05, 0x6262D63D,
+0xB1627201, 0xD6232622, 0x2622E200, 0x52916692,
+0x8B013620, 0x0009A144, 0x6061A06E, 0x001C001C,
+0x001D4020, 0x0000B38E, 0xFFFF0000, 0x12340000,
+0x001E1015, 0x00201274, 0x002039EC, 0x002018A2,
+0x002039F8, 0x00203A10, 0x00201860, 0x00201964,
+0x00201288, 0x001C3510, 0x001C3624, 0x001E212C,
+0x002038EC, 0x00203484, 0x002038F4, 0x00203900,
+0x0020390C, 0x00203968, 0x0020396C, 0x00203914,
+0x00203915, 0x00203918, 0x00117700, 0x00203984,
+0x00203982, 0x002034E8, 0x00117710, 0x001C3D30,
+0x001C36F8, 0x00117734, 0x001C3684, 0x001C3D00,
+0x001C1000, 0x001C1028, 0x002034FC, 0x0020391C,
+0x00117600, 0x00117740, 0x7FFFFFFF, 0x00201730,
+0x00203322, 0x0020232C, 0x00203D9C, 0x0020396A,
+0x002034F4, 0x0020395C, 0x001C3D2C, 0x001C36B0,
+0x0020348C, 0x0011775C, 0x8801C90F, 0xA0CF8901,
+0xD1960009, 0x36206212, 0xD4958904, 0x2421E200,
+0x2162A0CC, 0x6211D193, 0x89012228, 0x0009A0C3,
+0xE202D78F, 0x75016571, 0x3123615D, 0x27518D02,
+0x0009A0BC, 0xD28C57F2, 0x62226072, 0x40094019,
+0xC90F4009, 0x8F19880A, 0x52F31F2C, 0x40196022,
+0x40094009, 0x8808C90F, 0xA0A78901, 0x60630009,
+0xCB0154F7, 0xD27E55F2, 0xE7012402, 0xD47FE100,
+0x22112572, 0x72016242, 0x2422A098, 0x8B3F8805,
+0x602252F3, 0x40094019, 0xC90F4009, 0x8B168802,
+0xE4FFD577, 0x644D6752, 0x8B102748, 0x6272D775,
+0x8B0C3260, 0x51F255F7, 0xD26DE701, 0x21722562,
+0xD571E100, 0x64522211, 0xA0777401, 0x52F32542,
+0x40196022, 0x40094009, 0x8805C90F, 0x31B38B6E,
+0xD26A8B6C, 0x672254F4, 0x7701D569, 0x61422272,
+0x1F1CE640, 0x46182159, 0x8B033160, 0x6262D665,
+0x26227201, 0xE200D65A, 0x2621B067, 0x0009A056,
+0x3123E220, 0x88038B52, 0x52F38B1E, 0x40196022,
+0x40094009, 0x8803C90F, 0xD25B8B16, 0x672254F4,
+0x7701D557, 0x61422272, 0x1F1CE640, 0x46182159,
+0x8B033160, 0x6262D655, 0x26227201, 0xE200D648,
+0x2621B043, 0x0009A010, 0xD452D551, 0xD2446752,
+0xE1007701, 0x25723A46, 0x22118F06, 0xEA00D64E,
+0x72016262, 0x2622B031, 0x2FB2D54C, 0x95736652,
+0xD44A5BF1, 0x36205241, 0x60618910, 0x8B01C803,
+0x2B22E201, 0x8FF54510, 0x57F15664, 0x6272E1F0,
+0x41284118, 0x2722221B, 0x6BF2A008, 0x6BF2A006,
+0xE200D62F, 0xD12F2621, 0x2121E200, 0xD13CE201,
+0x66122822, 0x8B012668, 0x0009AE2B, 0x450BD539,
+0xD1390009, 0xAE24E600, 0x2F862160, 0x2FA62F96,
+0x2FC62FB6, 0x2FE62FD6, 0x7FF44F22, 0xDE34D133,
+0x54116212, 0x1F4167E2, 0x2F22D432, 0xD5321F72,
+0xD2326743, 0x58417794, 0x69425A42, 0x5B166C72,
+0x60526D22, 0xCB20E600, 0xE5402502, 0x626D7601,
+0x8BFB3253, 0x55F162F2, 0x11512122, 0xD62855F2,
+0x14812E52, 0x249214A2, 0x27C2D426, 0x26D211B6,
+0xDA256742, 0xE801D925, 0x490B2A72, 0xE2011A8C,
+0x1A2C4218, 0x4F267F0C, 0x6DF66EF6, 0x6BF66CF6,
+0x69F66AF6, 0x68F6000B, 0x000007D1, 0x0020397C,
+0x00203980, 0x00203986, 0x001C3DC0, 0x0011772C,
+0x001C3B88, 0x00203964, 0x0011773C, 0x00117744,
+0x0000F000, 0x00117764, 0x00117748, 0x00117768,
+0x0011776C, 0x01FFFFFF, 0x0011774C, 0x002034F4,
+0x00203D9C, 0x002024F0, 0x0020396A, 0x001C3B9C,
+0x001C3D98, 0x001C3700, 0x001C3500, 0x001C5960,
+0x001C8960, 0x002034FC, 0x001C3D00, 0x0020160C,
+0x2F962F86, 0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6,
+0xDE957FAC, 0x61E0E014, 0x0F14D494, 0x710161E3,
+0xE0186210, 0xD2920F24, 0x0009420B, 0x450BD591,
+0x20080009, 0x8F126D03, 0xD28F1F07, 0x6720D48F,
+0x657CDD8F, 0x470BD78F, 0xD18F0009, 0x619C6910,
+0x46086613, 0x36184608, 0x3D6C4608, 0xE0181FD7,
+0xE58004FC, 0x604C66E2, 0x3050655C, 0x2D628F15,
+0x01FCE014, 0xDE85E500, 0x641CA008, 0x6753655D,
+0x607037EC, 0x39DC6953, 0x80947501, 0x3243625D,
+0xD67F8BF4, 0xA34EE200, 0x20082621, 0xE0148B13,
+0xE40001FC, 0xA009DE79, 0x644D671C, 0x35EC6543,
+0x69436652, 0x39DC6262, 0x74041921, 0x3273624D,
+0xA3388BF3, 0x88012D10, 0xE0148B17, 0xE70001FC,
+0x6D1C2D70, 0xDE6D1FD4, 0x32D3627D, 0xA32A8B01,
+0x677D0009, 0x667365E3, 0x61737504, 0x315C36EC,
+0x69126462, 0xAFEF7708, 0x88042492, 0xE0148B18,
+0xE40001FC, 0x671C2D40, 0x624DDE60, 0x8B013273,
+0x0009A311, 0x6943644D, 0x39EC62E3, 0x72046592,
+0x3D2C6D43, 0x615266D2, 0x21697408, 0x2512AFED,
+0x8B188805, 0x01FCE014, 0x2D40E400, 0xDE53671C,
+0x3273624D, 0xA2F68B01, 0x644D0009, 0x62E36943,
+0x659239EC, 0x6D437204, 0x66D23D2C, 0x74086152,
+0xAFED216B, 0x88312512, 0xD44A8B3A, 0x6146D94A,
+0x75046543, 0x67566442, 0x6E531F48, 0x65527E04,
+0x7EE462E2, 0x7E0464E2, 0x6EE21FE9, 0x5EF929E0,
+0x7E04D942, 0x1FEA60E2, 0x2900C901, 0xD9406EE2,
+0x29E04E09, 0x2F562F26, 0x56FAD93E, 0x6513490B,
+0xD13D7F08, 0xE71C6E0D, 0x1DE12D70, 0xDE3B6912,
+0x64E21D92, 0x1D43D13A, 0xD23A6512, 0x67221D54,
+0x1D75D239, 0x1D666622, 0x6262D638, 0x1D27A2AB,
+0x8B398830, 0x6596D92B, 0x67926696, 0x61967904,
+0x74E46493, 0x6E436992, 0x1F9B7E04, 0x1FEC6442,
+0xD9256EE2, 0x5EFC29E0, 0x7E04D924, 0x1FED60E2,
+0x2900C901, 0xD9226EE2, 0x29E04E09, 0x59FC7FFC,
+0xDE272F92, 0x2F164E0B, 0xD41F7F08, 0xE21C610D,
+0x1D112D20, 0xD2206442, 0xD41C1D42, 0x1D536542,
+0x6752D51B, 0xD71B1D74, 0x1D156172, 0x1D666622,
+0x6262D61A, 0x1D27A26F, 0x8B358833, 0x490BD919,
+0xA268EE00, 0x00002DE0, 0x00117800, 0x00203A14,
+0x002018A2, 0x00202AA4, 0x00203906, 0x00203A18,
+0x0020352C, 0x002018EE, 0x00203905, 0x00117804,
+0x00203984, 0x00117810, 0x00203901, 0x00203902,
+0x00203903, 0x00200F64, 0x001C5864, 0x001C6864,
+0x001C7864, 0x001C59BC, 0x001C69BC, 0x001C79BC,
+0x00200FBC, 0x00200FB8, 0x89018828, 0x0009A0C0,
+0xE643DEB5, 0x326662E1, 0x1FEE8F02, 0x2E21E240,
+0x622D62E1, 0x8B013267, 0x0009A0AA, 0xE50185E1,
+0x8B013056, 0x0009A0A4, 0x2D10E101, 0x64E1B225,
+0xE64357FE, 0x652D6271, 0x89443567, 0x3563E640,
+0xE6008B05, 0x0F65E040, 0xA00FE11A, 0x615372C0,
+0x41214121, 0x41214121, 0x45214121, 0x45214521,
+0xC9036053, 0xE0406603, 0x71180F65, 0x2209E007,
+0x0F25E03C, 0xE044641D, 0xB2A365F3, 0xE33C0F46,
+0x853233FC, 0x620DDE95, 0x42086031, 0x6023610D,
+0x1323E944, 0x06FE4108, 0xE00F39FC, 0x13144E0B,
+0x67075D91, 0x60D32679, 0x0F6654FE, 0x51928542,
+0x600D4E0B, 0x60D3260B, 0x0F666492, 0x65F3B237,
+0x696156FE, 0xE640659D, 0x89383563, 0xD78359FE,
+0x79066591, 0xC9036053, 0x40004008, 0x61036203,
+0x0F26E050, 0x470BE0FF, 0x6C07600C, 0x6603605D,
+0x46214621, 0x46214621, 0x42006263, 0x4200326C,
+0x40214021, 0x4008C903, 0x6D2D30FC, 0xE8006A03,
+0xB25765F3, 0x6EA264D3, 0x2EC9E050, 0x66942AE2,
+0xD76E01FE, 0x606C470B, 0x2AE22E0B, 0x64D365F3,
+0x7801B1FD, 0xEE06628D, 0x8FE932E3, 0x5EFE7D01,
+0x61E1E400, 0x410085E1, 0x66E3310C, 0x760C711B,
+0xE70465F3, 0x68667401, 0x3A736A4D, 0x8FF92582,
+0x65F37504, 0x641DB1E3, 0x64E1B1A4, 0x0009A17B,
+0xD45B56F7, 0xEC01D25B, 0x26C0420B, 0x0009A173,
+0x06FCE018, 0x8829606C, 0x58F78B08, 0xE400D252,
+0x66222840, 0x646DB171, 0x0009A165, 0x666CE681,
+0x89013060, 0x0009A0AC, 0xD550D14F, 0x62126A56,
+0x212232AC, 0x54116C56, 0x34CC6253, 0x64521141,
+0x72085812, 0xD44A384C, 0x68221182, 0x5A136C42,
+0x3ACC3C8C, 0x11A324C2, 0x6C2272EC, 0x72105814,
+0x118438CC, 0x5A156822, 0x11A53A8C, 0x6A227210,
+0xD6405816, 0x118638AC, 0x52176C62, 0x112732CC,
+0x5A185861, 0x11A83A8C, 0x5C195A62, 0x11C93CAC,
+0x521A5C63, 0x112A32CC, 0x5A1B5864, 0x11AB3A8C,
+0x5C1C5A65, 0x11CC3CAC, 0x521D5C66, 0x112D32CC,
+0x5A1E5867, 0x11AE3A8C, 0x561F5A68, 0x36ACE840,
+0x116FDA2D, 0x6CA2381C, 0x7A946682, 0x286236CC,
+0x5C8162A2, 0x18C13C2C, 0x62A27A44, 0x362C5682,
+0xD6261862, 0x5A856262, 0x3A2C4229, 0x760418A5,
+0x56866262, 0x362C4229, 0x56F71866, 0x2620E238,
+0x16C15C81, 0x16226212, 0xE2005C11, 0x551216C3,
+0x55151654, 0x55131655, 0x55161656, 0x55821657,
+0x65821658, 0x55141659, 0x5584165A, 0x5583165B,
+0x5585165C, 0x5586165D, 0x1821165E, 0x11212122,
+0x11251122, 0x11261123, 0x28221822, 0x18241124,
+0x18251823, 0x1826A0C7, 0x00117804, 0x002033E0,
+0x00203A38, 0x002018A2, 0x0020348C, 0x001C36A0,
+0x002034E8, 0x001C3CA0, 0x001C36F4, 0x001C3B88,
+0x666CE682, 0x8B203060, 0xEA2456F7, 0x26A0D194,
+0x16C15C17, 0x16225218, 0x16835819, 0x16A45A1A,
+0x16C55C1B, 0x1626521C, 0xE200581D, 0x551E1687,
+0x551F1658, 0x11271659, 0x11291128, 0x112B112A,
+0x112D112C, 0xA08E112E, 0xE683112F, 0x3060666C,
+0x52F78B0B, 0xEA00D883, 0x658222A0, 0x7804DC82,
+0x62822C52, 0xA07ED681, 0xE6902620, 0x3060666C,
+0xDA7F8B06, 0x00094A0B, 0xE20056F7, 0x2620A073,
+0x666CE691, 0x8B103060, 0x6222D276, 0x2228622C,
+0xD2788904, 0x0009420B, 0x0009A003, 0x420BD276,
+0x56F70009, 0xA05EE200, 0xE6922620, 0x3060666C,
+0xE0188951, 0xE6B00BFC, 0x666C62BC, 0x8B2A3260,
+0x02FCE014, 0x682CEA00, 0x62ADE904, 0x894A3283,
+0x6AADDD64, 0x3CDC6CA3, 0x7D046EC2, 0xDB68D467,
+0x32DC62A3, 0x4B0BDC67, 0x4C0B6D22, 0xD46664E3,
+0x00094B0B, 0x64D34C0B, 0x4B0BD464, 0xE6000009,
+0x666D6BE3, 0x76013B6C, 0x3293626D, 0x8FF72BD0,
+0xAFDA4D19, 0xE6B57A08, 0x3260666C, 0xD45C8B13,
+0x4B0BDB57, 0xD25B0009, 0x6022DB5B, 0xCB20E6FF,
+0x2202666D, 0xDB592B62, 0xE014E200, 0x56F72B20,
+0xA01002FC, 0xD4562620, 0x6542D256, 0x420BD456,
+0xA0080009, 0xDB520009, 0x52B1E600, 0x622CDB53,
+0x52F72B21, 0x7F542260, 0x6EF64F26, 0x6CF66DF6,
+0x6AF66BF6, 0x000B69F6, 0x4F2268F6, 0xE240614D,
+0x89143123, 0x3127E21F, 0x8B09D749, 0xD449614D,
+0xE00171E0, 0x5671440B, 0x26596507, 0x1761A007,
+0xE001D444, 0x6672440B, 0x26596507, 0x4F262762,
+0x0009000B, 0x614D4F22, 0x3123E240, 0xE21F8912,
+0xD73B3127, 0x614D8B08, 0x5671D23A, 0x420B71E0,
+0x260BE001, 0x1761A006, 0x6672D236, 0xE001420B,
+0x2762260B, 0x000B4F26, 0xE6400009, 0x46284618,
+0x6252D531, 0x89FC2268, 0x0009000B, 0x4618E680,
+0xD52D4628, 0x22686252, 0x000B89FC, 0xA0010009,
+0x7201E200, 0x8BFC3242, 0x0009000B, 0x4618E680,
+0xD5264628, 0x22686252, 0x000B8BFC, 0x2FE60009,
+0x7FFC4F22, 0xBFF16E53, 0x61E22F42, 0xE280D620,
+0x54E11615, 0x16464218, 0x422855E2, 0x57E31657,
+0x16786EF2, 0x26E22E2B, 0x4F267F04, 0x6EF6AFCE,
+0x0020348C, 0x00117804, 0x002038EC, 0x00203900,
+0x0020050A, 0x00201008, 0x0020102E, 0x00203A50,
+0x002018A2, 0x002018E6, 0x00203A64, 0x00203A6C,
+0x00203A70, 0x001C3500, 0x001C1000, 0x00203982,
+0x00117800, 0x002018EE, 0x00203A84, 0x00203988,
+0x001C3704, 0x002033E0, 0x001C373C, 0x001C3700,
+0x001C370C, 0x2FD62FC6, 0x4F222FE6, 0x6C53DD10,
+0x6E43BFA4, 0x2DE2BF89, 0x0009BFA0, 0x2C1251D5,
+0x1C4154D6, 0x1C5255D7, 0x1C6356D8, 0x6EF64F26,
+0x000B6DF6, 0x61636CF6, 0xA004E600, 0x62564109,
+0x24227601, 0x36127404, 0x000B8BF9, 0x00000009,
+0x001C370C, 0x0009A16E, 0x2FE62FD6, 0xDD944F22,
+0xA0049EB2, 0xD4930009, 0x420BD293, 0x62D265D2,
+0x8BF822E8, 0x0009A004, 0xD28FD490, 0x55D1420B,
+0x22E852D1, 0xA0048BF8, 0xD48D0009, 0x420BD28A,
+0x52D255D2, 0x8BF822E8, 0x0009A004, 0xD286D489,
+0x55D3420B, 0x22E852D3, 0xA0048BF8, 0xD4860009,
+0x420BD281, 0x52D455D4, 0x8BF822E8, 0x6EF64F26,
+0x6DF6000B, 0x2FD62FC6, 0x4F222FE6, 0x6E636C73,
+0x6D53B01A, 0x64D357F4, 0xB05F65E3, 0xB07566C3,
+0xB0A40009, 0xB0A80009, 0xB0AC0009, 0xB0AC0009,
+0xB0AF0009, 0xB03154F5, 0x6CCD6C03, 0x4F2660C3,
+0x6DF66EF6, 0x6CF6000B, 0x3412D170, 0xD6700529,
+0x2650D770, 0x2742000B, 0x0009A018, 0x2FD62FC6,
+0x4F222FE6, 0x6E636C73, 0x6D53BFEE, 0x64D357F4,
+0xB03365E3, 0xB08D66C3, 0xB00F54F5, 0x6CCD6C03,
+0x4F2660C3, 0x6DF66EF6, 0x6CF6000B, 0xE503D162,
+0xD763D462, 0x21524518, 0x2472000B, 0xD45FD15E,
+0x2162E600, 0x2462000B, 0xBF734F22, 0xBF73E40A,
+0xD25C0009, 0x4118E104, 0xE40AE500, 0xBF692212,
+0xD7592252, 0xCB206072, 0x000B4F26, 0x4F222702,
+0x410BD156, 0xD556E400, 0x4F26452B, 0xD1552FE6,
+0x66126E63, 0x92104418, 0x44084528, 0x45002629,
+0x265B4408, 0x264B4400, 0x21624708, 0xD14E4708,
+0x217227EB, 0x6EF6000B, 0x1FFF03F0, 0x4F222FE6,
+0xE101DE4A, 0xBF3DE40A, 0x67E32E12, 0xE500776C,
+0xE204E130, 0x2752E40A, 0x27522752, 0x27522752,
+0x27522752, 0x27522752, 0x27522752, 0x27522752,
+0x27522752, 0x27522752, 0x27522752, 0x27222712,
+0x27522752, 0x27522752, 0x27522752, 0x27522752,
+0x175ABF18, 0x2E62E600, 0x000B4F26, 0xD2346EF6,
+0xE441E101, 0x000B2212, 0xD1322242, 0xE605D432,
+0x000B2162, 0x000B2462, 0xD2300009, 0xE40AE601,
+0x2262AF00, 0x2FC62FB6, 0x2FE62FD6, 0x7FFC4F22,
+0x6C43DB2B, 0xED0060B2, 0x2B02CB03, 0xC90360B2,
+0x6E03A008, 0x89073DC2, 0xE46460B2, 0xB07CC903,
+0x7D016E03, 0x8BF52EE8, 0x8F043DC2, 0xD4212FE1,
+0x460BD621, 0x62F10009, 0x6023622D, 0x89FFC801,
+0x7F046023, 0x6EF64F26, 0x6CF66DF6, 0x6BF6000B,
+0x001C3B88, 0x00203A98, 0x002018EE, 0x00203AA0,
+0x00203AA8, 0x00203AB0, 0x00203AB8, 0x0025E720,
+0x00203D98, 0x002038F0, 0x001C5968, 0x001C3B40,
+0x000F8000, 0x001D4004, 0x001C3500, 0x002015E0,
+0x0020160C, 0x001C5814, 0x001C59D0, 0x001C5830,
+0x001C6268, 0x001C59A4, 0x001C639C, 0x001C581C,
+0x001C5860, 0x00203AC0, 0x002018A2, 0x8F014411,
+0x6043604B, 0x0009000B, 0x5651D52B, 0x46286052,
+0x306C000B, 0x2FC62FB6, 0x2FE62FD6, 0x4F124F22,
+0xBFF14F02, 0x6B036E43, 0xDD25DC24, 0x0009BFEC,
+0x3C0530B8, 0x4609060A, 0x46014609, 0x020A3D65,
+0x42094209, 0x32E24209, 0x4F068BF0, 0x4F264F16,
+0x6DF66EF6, 0x000B6CF6, 0x2FC66BF6, 0x2FE62FD6,
+0x4F124F22, 0xBFCF4F02, 0x6C036E43, 0xBFCBDD13,
+0x30C80009, 0x060A3D05, 0x46094609, 0x36E24601,
+0x4F068BF5, 0x4F264F16, 0x6DF66EF6, 0x6CF6000B,
+0x4F222FE6, 0xE102DE0B, 0xE403E500, 0xBFB92E12,
+0xE6062E52, 0xE7004618, 0x2E62E403, 0x4F262E72,
+0x6EF6AFB0, 0x0009000B, 0x001C1040, 0xCCCCCCCD,
+0x10624DD3, 0x001D4004, 0x2F962F86, 0x2FB62FA6,
+0x2FD62FC6, 0x4F222FE6, 0xE5007F98, 0x6453E710,
+0x6B534728, 0xEE1ADCBC, 0x6153655D, 0x315C4108,
+0x75014108, 0x6043317C, 0x0F16665D, 0xED0060B3,
+0x21B136E3, 0x81128111, 0x11D28113, 0x11D411D3,
+0x74048FEA, 0xD8B167F2, 0x1871D9B1, 0x58F12872,
+0x1981D1B0, 0x59F22982, 0x5DF45AF3, 0x54F65EF5,
+0x21921191, 0x11A211A3, 0x11D411D5, 0x11E611E7,
+0x11481149, 0xDAA855F7, 0x57F8EE00, 0x52F9DDA7,
+0x64E3D6A7, 0x2A521A51, 0xD8A7D9A6, 0x2D729AD5,
+0x6EED2622, 0x4D086DE3, 0x3DEC61E3, 0x4D084108,
+0x3D9C31EC, 0x410860B3, 0x81D12DB1, 0x4108E050,
+0x4008E7B7, 0x677C4108, 0x60B381D2, 0xE200318C,
+0x81D33472, 0x1D131DD2, 0x8D01D493, 0xD4901D24,
+0xB0B365D3, 0x64ED7E01, 0x8BDA34A2, 0x2FD2DA8C,
+0xDD9268A2, 0x2D824829, 0x7DFC64A2, 0xD287694D,
+0x6E222D92, 0x7D0C4E29, 0x68222DE2, 0x618D6AD3,
+0x2A16D784, 0xD48A6D72, 0x24D2D583, 0xD6895E72,
+0x517414E2, 0x1414EE00, 0xD1875876, 0x59781486,
+0x1498E710, 0x65E36252, 0x26E2142A, 0xE60064E3,
+0x6843644D, 0x384C4808, 0x381C4808, 0x0C866053,
+0x09CE28B1, 0x819160B3, 0x0ACE6053, 0x81A26043,
+0x0DCE6053, 0x81D360B3, 0x08CE6053, 0x18827401,
+0x624D09CE, 0x0ACE19E3, 0x1A643273, 0x75048FE0,
+0xE003D96A, 0x40186C92, 0x6D922CB1, 0x81D1DA6F,
+0x6E92E050, 0x81E24008, 0x60B36192, 0x64928113,
+0x1442E600, 0xD4696792, 0x689217A3, 0x1864E1FF,
+0x6563E703, 0x364C4608, 0x26127501, 0x3673665D,
+0xDC5B8BF8, 0x6DC2E003, 0x2DB14018, 0xD2606EC2,
+0x61C281E1, 0x1112EE00, 0xE02464C2, 0x65C21423,
+0x15E4D45B, 0xE58067C2, 0x68C28172, 0x818366E3,
+0x666D655C, 0x76046963, 0x394C6A6D, 0x8FF83A53,
+0xDB5429E2, 0x24B2DC54, 0x24C27404, 0x4F267F68,
+0x6DF66EF6, 0x6BF66CF6, 0x69F66AF6, 0x68F6000B,
+0x60116142, 0x8F03C803, 0xD23DE500, 0x8B063420,
+0xC9036011, 0x8B068802, 0x3420D239, 0x56128B03,
+0x52646513, 0x000B2422, 0x01136053, 0x2FE62FD6,
+0x7FEC4F22, 0x62536E53, 0x6D43E550, 0x4508E400,
+0xE101A001, 0x60435224, 0x81212211, 0x60538123,
+0x56E28122, 0x8BF53620, 0x16E4D238, 0xE61464F3,
+0x65E3420B, 0xE4FC65E1, 0x2E512549, 0x65F361F1,
+0x2F112149, 0xD13154D1, 0xE614410B, 0x607157D1,
+0x2701CB01, 0x7F141DE1, 0x6EF64F26, 0x6DF6000B,
+0x2FE62FD6, 0x7FEC4F22, 0x66536E53, 0x6D43E5FC,
+0x20596061, 0x2601CB01, 0x326052E2, 0x12E48B06,
+0x31E051E2, 0x52D18B04, 0x1E22A002, 0x5664AFF0,
+0x64F3D21E, 0x420BE614, 0x67E165E3, 0x2719E1FC,
+0x67F12E71, 0x271954D1, 0x65F3D118, 0x410BE614,
+0x52D12F71, 0xCB016021, 0x1DE12201, 0x4F267F14,
+0x000B6EF6, 0x00006DF6, 0x0020391C, 0x002034EC,
+0x002034F4, 0x002034FC, 0x00203524, 0x00203908,
+0x00203910, 0x00100208, 0x001017C0, 0x001E210C,
+0x001C3D00, 0x0020395C, 0x001000C8, 0x00117880,
+0x00117780, 0x00040020, 0x0026C401, 0x00200ED6,
+0x4F222FE6, 0xDE42624C, 0x42004208, 0x3E2CA005,
+0xD4405252, 0xBF695624, 0x65E22E62, 0x352052E1,
+0xD63D8BF6, 0x4F262622, 0x6EF6000B, 0x2FC62FB6,
+0x2FE62FD6, 0xDC394F22, 0x52C1DB39, 0x362066C2,
+0x6061891C, 0x8801C903, 0xDE348918, 0xBF37DD35,
+0x650364E3, 0x66B28503, 0x3262620D, 0xD4328907,
+0x0009BF76, 0x4D0BD431, 0xAFE60009, 0xBF3D0009,
+0xD42F64E3, 0x00094D0B, 0x0009AFDF, 0x2262D22D,
+0x6EF64F26, 0x6CF66DF6, 0x6BF6000B, 0x2FD62FC6,
+0x4F222FE6, 0xDD29DC28, 0x6E4360C2, 0x04DE4008,
+0xE614D127, 0x65E3410B, 0xD127D726, 0x55E227E2,
+0x35E05254, 0x21228F04, 0x400860C2, 0x122202DE,
+0x605365C2, 0x75014008, 0x0DE606DE, 0xC90F6053,
+0x60632C02, 0x6EF64F26, 0x000B6DF6, 0x85436CF6,
+0x650D5643, 0x622D6262, 0x35277204, 0xE1008F0C,
+0x2268960C, 0xD6158B03, 0x72015261, 0xD6131621,
+0x6262E101, 0x26227201, 0x6013000B, 0x000001FF,
+0x002034FC, 0x002034F4, 0x001C3D00, 0x00203524,
+0x002038EC, 0x002018A2, 0x002034EC, 0x00203AE8,
+0x00203AEC, 0x001C3D28, 0x0020395C, 0x0020391C,
+0x00200ED6, 0x00203960, 0x00203964, 0x00117754,
+0x2FC62FB6, 0x2FE62FD6, 0x7FF84F22, 0x6022D237,
+0x8D58C803, 0xDE362F01, 0xDB37DC36, 0x66C252C1,
+0x892F3620, 0xC9036061, 0x892B8801, 0xD233DD31,
+0x64D3420B, 0x1F016503, 0x880160B1, 0xD2308B04,
+0x64D3420B, 0x0009AFEA, 0x85615653, 0x8904C820,
+0xE050D72C, 0x7201027E, 0xD22B0726, 0x6453420B,
+0x89072008, 0x55F1D126, 0x64D3410B, 0xE601D727,
+0x2762AFD4, 0x55F1D226, 0x64E3420B, 0xE601D125,
+0x2162AFCC, 0xDD25DE24, 0xDC26DB25, 0x66D252D1,
+0x89183620, 0xC9036061, 0x89148801, 0xD117D41F,
+0x0009410B, 0x36E05603, 0x65038F04, 0x2B20E201,
+0x2C52AFEC, 0xD712D41C, 0x0009470B, 0xE601D115,
+0xAFE34618, 0x60F12162, 0x8907C804, 0x7F08D217,
+0x6EF64F26, 0x6CF66DF6, 0x6BF6422B, 0x4F267F08,
+0x6DF66EF6, 0x000B6CF6, 0x00006BF6, 0x001E2100,
+0x002034FC, 0x002034F4, 0x00203984, 0x002014A0,
+0x002014CC, 0x0020348C, 0x002016BE, 0x001E212C,
+0x00201530, 0x001C3D30, 0x00117880, 0x002034EC,
+0x0020390C, 0x00203908, 0x00203524, 0x00200610,
+0xE601D203, 0x1265D503, 0x000B2252, 0x00001266,
+0x001C1010, 0x0000C34F, 0x0009000B, 0x2FD62FC6,
+0x4F222FE6, 0x6D436C53, 0xEE00A004, 0x7E0164D4,
+0x644CBFF2, 0x8BF93EC2, 0x6EF64F26, 0x000B6DF6,
+0xE5006CF6, 0x6643A002, 0x76017501, 0x22286260,
+0xAFE38BFA, 0x2FE60009, 0x75076253, 0xE1086753,
+0x6043EE0A, 0x4409C90F, 0x650330E2, 0x8D014409,
+0xE630E637, 0x4110365C, 0x8FF22760, 0xE00077FF,
+0x000B8028, 0x000B6EF6, 0x000BE000, 0x2FE6E000,
+0x7FEC4F22, 0x6E436253, 0xBFDC65F3, 0xBFD06423,
+0xBFCE64E3, 0xD40364F3, 0x0009BFCB, 0x4F267F14,
+0x6EF6000B, 0x00203AF0, 0xE4FDD29D, 0xD79D6122,
+0x22122149, 0x74016022, 0x2202CB01, 0xD59A6622,
+0x22622649, 0xC8406070, 0x60528902, 0x2502CB04,
+0xE6016052, 0x2502CB08, 0xE4026052, 0x2502C9CF,
+0x46186052, 0x2502CB10, 0xCB036052, 0x15422502,
+0x1563000B, 0xD78ED58D, 0xD48FD28E, 0xE600E100,
+0x27112511, 0xAFCF2210, 0x664C2461, 0x4600D28B,
+0x6060362C, 0x000BCB10, 0x654C2600, 0x4500D287,
+0x6650352C, 0x2619E1EF, 0x2560000B, 0xD284664C,
+0x362C4600, 0xCB106060, 0x2600000B, 0xD280654C,
+0x352C4500, 0xE1EF6650, 0x000B2619, 0x664C2560,
+0x4600D27A, 0x6060362C, 0x000BCB08, 0x654C2600,
+0x4500D276, 0x6650352C, 0x2619E1F7, 0x2560000B,
+0xD273664C, 0x362C4600, 0xCB086060, 0x2600000B,
+0xD26F654C, 0x352C4500, 0xE1F76650, 0x000B2619,
+0x624C2560, 0x4200D669, 0x6020326C, 0x4021C908,
+0x40214021, 0x600C000B, 0xD665624C, 0x326C4200,
+0xC9086020, 0x40214021, 0x000B4021, 0xD161600C,
+0x341C644C, 0x000B6240, 0xD15F602C, 0x341C644C,
+0x000B6240, 0x2FE6602C, 0x6E434F22, 0xE60A645C,
+0x89143467, 0x0009BFEB, 0x60EC640C, 0x8B028801,
+0xA002E00F, 0x44092409, 0x624C4409, 0x3263E60A,
+0xBFE28905, 0x620C644C, 0xC8806023, 0xE2008B00,
+0x4F266023, 0x6EF6000B, 0xD64C4F22, 0x88016062,
+0xB2578B03, 0xA0030009, 0xD2490009, 0x2260E640,
+0xE200D648, 0x000B4F26, 0x4F222622, 0x6062D643,
+0x8B018802, 0x0009B2A0, 0xE200D642, 0x000B4F26,
+0xD53E2622, 0xE100D43E, 0x2512E701, 0x2470000B,
+0xE604D23B, 0x2260000B, 0xD43B4F22, 0x410BD13B,
+0xD53B0009, 0x6650E1FD, 0x2619D23A, 0x2560E700,
+0x000B4F26, 0x4F222270, 0xD238D537, 0xD7386152,
+0x2512611D, 0xE6FF6452, 0x2542242B, 0xD22FD435,
+0x420B666D, 0xD52E2762, 0x6750E1FB, 0x4F262719,
+0x2570000B, 0xD4304F22, 0x410BD128, 0xD5280009,
+0x6650E7F7, 0x4F262679, 0x2560000B, 0x9425D524,
+0x22496250, 0x2520000B, 0xE4BFD521, 0x22496250,
+0x2520000B, 0xD2254F22, 0x600D8522, 0x89112008,
+0x89458801, 0x89478803, 0x89498805, 0x894F8806,
+0x89558808, 0x895B8809, 0x8961880A, 0x8967880B,
+0x0009A06E, 0x0009B070, 0x600CA06B, 0x0000FF7F,
+0x001E2148, 0x001E1000, 0x001E1108, 0x002039BC,
+0x002039BE, 0x002039DD, 0x002039A0, 0x001E103F,
+0x001E105F, 0x001E102F, 0x001E1090, 0x002039C4,
+0x001E100B, 0x002039C0, 0x00203AF4, 0x002018A2,
+0x001E1028, 0x002039DC, 0x001D4020, 0x98760000,
+0x001C1000, 0x00203B00, 0x00203B10, 0x00203994,
+0x0009B04C, 0x600CA035, 0x0009B055, 0x600CA031,
+0x6260D684, 0x8B2B2228, 0x0009B061, 0x600CA029,
+0x6260D680, 0x8B232228, 0x0009B069, 0x600CA021,
+0x6260D67C, 0x8B1B2228, 0x0009B0C7, 0x600CA019,
+0x6260D678, 0x8B132228, 0x0009B0CD, 0x600CA011,
+0x6260D674, 0x8B0B2228, 0x0009B125, 0x600CA009,
+0x6260D670, 0x8B032228, 0x0009B13D, 0x600CA001,
+0x4F26E000, 0x0009000B, 0xD26CD16B, 0xD56C8412,
+0x4000C90F, 0xD76B012D, 0xE403D66B, 0xE20F611C,
+0x2540E001, 0x25202712, 0x2602000B, 0xE601D262,
+0x30668523, 0xE0008D05, 0xD663D260, 0xE0018122,
+0x000B2602, 0xD25C0009, 0x600D8523, 0x89052008,
+0x8B0A8801, 0x6060D65D, 0x2600CB01, 0xD457D65A,
+0xE001E101, 0x000B2612, 0x000B8142, 0xD152E000,
+0x8513E501, 0x640D4518, 0x66033453, 0xE0008D05,
+0xD551D253, 0x2260E001, 0x000B2502, 0x4F220009,
+0x8513D149, 0x6453650D, 0x62494419, 0x227D672E,
+0x8801602C, 0x88028909, 0x88038910, 0x8806891A,
+0x88078935, 0xA04C893B, 0xD5460009, 0x6652D746,
+0x2762D446, 0x622C6261, 0x2421A038, 0x2228625C,
+0xD4438B3F, 0x6642D540, 0x2562D440, 0x24018561,
+0x6203A02C, 0x2008605C, 0x88108907, 0x88208908,
+0x88308909, 0xA02C890A, 0xD23A0009, 0x6222A008,
+0xA005D239, 0xD2396222, 0x6222A002, 0x6262D638,
+0xD432D531, 0x66212522, 0xA00F626C, 0xD6352421,
+0x6261D52D, 0x622CD42D, 0xA0072562, 0xD6322421,
+0x8561D529, 0x2562D429, 0x62032401, 0x662D8515,
+0x3617610D, 0x65038F01, 0xB0CB2451, 0xA0010009,
+0xE000E001, 0x000B4F26, 0xD6190009, 0xD427E101,
+0x65412610, 0xD118D717, 0xE20F655D, 0x2752E001,
+0x000B2620, 0x2FE62102, 0xD20F4F22, 0x640C8523,
+0x8B082448, 0xD511D61D, 0x2621E200, 0x940F8451,
+0xA0482049, 0xDE0D8051, 0xC84060E0, 0xE2018D32,
+0x89443427, 0xD216D615, 0x2641420B, 0x0009A030,
+0x0000FF7F, 0x002039DD, 0x00203994, 0x002039A0,
+0x001E1100, 0x001E100C, 0x002039C0, 0x001E1000,
+0x001E1001, 0x002039C8, 0x002039A8, 0x002039AC,
+0x002039B0, 0x002039CC, 0x002039D0, 0x002039D4,
+0x002039D8, 0x00203DFC, 0x00203E06, 0x002039BA,
+0x0020287E, 0x89123427, 0xD294D693, 0x2641420B,
+0xCB8084E1, 0x80E1B0F5, 0xD69160E0, 0x2E00CB04,
+0xC93F6060, 0xD68F2600, 0xA001E001, 0xE0002602,
+0x000B4F26, 0xD68C6EF6, 0xC8806060, 0xD2868919,
+0x88016021, 0xD2898B15, 0x8524E501, 0x89103056,
+0xE203D187, 0x2120D487, 0xE00B6541, 0x0656655D,
+0xE40FD585, 0x2140E702, 0xD77E2571, 0x000BE001,
+0x000B2702, 0x2FE6E000, 0xDE804F22, 0xC88084E1,
+0xD57A892C, 0x20088554, 0x61038F28, 0x8553D77C,
+0x64036672, 0x8566650C, 0x3520620C, 0xD6798B1E,
+0x651CD774, 0x2651644C, 0x60E02741, 0x8904C840,
+0x420BD275, 0xA0030009, 0xD2680009, 0x0009420B,
+0x0009B09F, 0xE201D167, 0x60E02122, 0xCB04D464,
+0x60402E00, 0x2400C93F, 0x6023A001, 0x4F26E000,
+0x6EF6000B, 0x2FB62FA6, 0x2FD62FC6, 0xDA622FE6,
+0x66A1E240, 0x3622DC5E, 0x62638900, 0x6ED36D2C,
+0x4E2136D8, 0x4E212A61, 0xDB61D460, 0xE700A00F,
+0x770162B2, 0x71026123, 0x66212B12, 0x71026213,
+0x61212B12, 0x651D666D, 0x356C4528, 0x627C2452,
+0x8BED32E3, 0xC90360D3, 0x8B108803, 0x617367B2,
+0x2B127102, 0x71026E13, 0x2B126571, 0x655D6DE1,
+0x422862DD, 0x325CE107, 0xA00C2C10, 0x88022422,
+0xA0038B01, 0x8801E203, 0xE2018B05, 0x66B22C20,
+0x655D6561, 0xE60F2452, 0x67A12C60, 0x8B052778,
+0xDD38DC44, 0xEB01EA00, 0x2DB22CA2, 0x6DF66EF6,
+0x6BF66CF6, 0x6AF6000B, 0x2FE62FD6, 0xE240DD36,
+0x362266D1, 0x62638900, 0x3678672C, 0x7703DE38,
+0x47212D61, 0x64E2D635, 0xA00E4721, 0x6562E100,
+0x62537101, 0x74012450, 0x24204219, 0x45297401,
+0x74012450, 0x24504519, 0x621C7401, 0x8BEE3273,
+0x66E24200, 0x420061D1, 0x2118362C, 0x2E628F06,
+0xDD1CD728, 0xE501E400, 0x2D522742, 0x000B6EF6,
+0x2FD66DF6, 0x4F222FE6, 0xED0AEE01, 0x64E3BC85,
+0xBC8A64E3, 0x62EC7E01, 0x8BF732D7, 0xBC8DEE01,
+0x64E364E3, 0x7E01BC92, 0x32D762EC, 0x4F268BF7,
+0x000B6EF6, 0xD1186DF6, 0xD418920D, 0x72122122,
+0x2422D617, 0xD7177204, 0x72202622, 0x2722D116,
+0x000B7230, 0x137A2122, 0x002039BA, 0x0020298A,
+0x001E1015, 0x002039C0, 0x001E1001, 0x00203994,
+0x001E1100, 0x002039BE, 0x002039AC, 0x001E1000,
+0x002039B0, 0x002039BC, 0x0020287E, 0x001E100C,
+0x002039A8, 0x002039C4, 0x002039C8, 0x002039CC,
+0x002039D0, 0x002039D4, 0x002039D8, 0x4F222FE6,
+0xD6707FFC, 0x88016060, 0xE2018951, 0x2620BFBB,
+0xD56ED16D, 0xDE6E6010, 0x64E36552, 0x7402C840,
+0x8D22D16C, 0xD26C7502, 0xE601D76C, 0xE7042722,
+0x76016255, 0x626C2421, 0x8FF93273, 0xD4637402,
+0x6242E601, 0x640D8528, 0x67494419, 0x275D657E,
+0x81E4607C, 0xE417D562, 0x67557601, 0x3243626C,
+0x8FF92171, 0xA0207102, 0xD25E0009, 0xE601D75B,
+0xE7042722, 0x76016255, 0x626C2421, 0x8FF93273,
+0xD4527402, 0x6242E601, 0x640D8528, 0x67494419,
+0x275D657E, 0x81E4607C, 0xE417D553, 0x67557601,
+0x3243626C, 0x8FF92171, 0x92897102, 0xD2462E21,
+0x5E23D74E, 0x64F22FE2, 0x604365F2, 0x2700C980,
+0xC9606043, 0x80716103, 0xC9036043, 0x80724519,
+0x65F2605C, 0x817266F2, 0x46194629, 0x606C4529,
+0x4018645C, 0x8173304C, 0x21185E23, 0x64F22FE2,
+0x6E4C62F2, 0x602C4219, 0x66F262F2, 0x46294018,
+0x461930EC, 0x42298174, 0x652C606C, 0x305C4018,
+0x81758F07, 0x0009BC96, 0x2228620C, 0xA00A8908,
+0x60130009, 0x8B038840, 0x0009B009, 0x0009A003,
+0xE202D62F, 0x7F042622, 0x000B4F26, 0x4F226EF6,
+0x8552D52A, 0x8830600D, 0x88318903, 0xA0348923,
+0x85550009, 0xD428D727, 0x85532701, 0x610DD627,
+0x24124118, 0x460BD426, 0xD7230009, 0xD226D425,
+0x6572420B, 0xE230D120, 0x42286712, 0x2729E620,
+0x37604628, 0xD6218B03, 0xA016E200, 0xD61F2622,
+0xA012E202, 0xD1182622, 0x6212E530, 0xE6204528,
+0x46282259, 0x89083260, 0xD41AD119, 0xE601D513,
+0x2160450B, 0x472BD718, 0x4F264F26, 0x0009000B,
+0x0000060A, 0x002039DC, 0x001E1000, 0x002039C8,
+0x00203DFC, 0x00203E08, 0x00203DA0, 0x002039B0,
+0x00203DD0, 0x00203DCE, 0x00203DA2, 0x00203994,
+0x002039C0, 0x002039AC, 0x002039A8, 0x002018A2,
+0x00203B1C, 0x00203B20, 0x002018EE, 0x002039C4,
+0x001E100B, 0x00203B34, 0x00114004, 0x4F222FE6,
+0xDE967FFC, 0x200884E9, 0x2F008D06, 0xD695D494,
+0x0009460B, 0x64F0B19A, 0x6620D293, 0x89022668,
+0xC9BF60E0, 0x7F042E00, 0x000B4F26, 0x000B6EF6,
+0x2FE60009, 0xDE8D4F22, 0x60E0D68D, 0xCBC0D48D,
+0x62602E00, 0xC803602C, 0x40218904, 0x70014021,
+0x6603A002, 0x66034009, 0xD687616D, 0xE500A004,
+0x75016262, 0x74042422, 0x3213625D, 0xD2838BF8,
+0x0009420B, 0xC9BF84E2, 0x4F2680E2, 0x6EF6000B,
+0x2FE62FD6, 0x7FFC4F22, 0x6260D67D, 0x89442228,
+0xD572E100, 0x60502610, 0xCB40D47A, 0x2500440B,
+0x8D052008, 0x62E06E03, 0x7104612C, 0x2F11A006,
+0xD475D66D, 0xDD756760, 0x657C4D0B, 0xE23C6D1D,
+0x8B033D27, 0xD267D472, 0x0009420B, 0x4D214D21,
+0xA005D770, 0x66E6E400, 0x357C4508, 0x74012562,
+0x35D3654D, 0xD76C8BF7, 0x6172E003, 0x81114018,
+0x6E7260F1, 0x81E2700C, 0xD4686172, 0xDD688113,
+0x4D0BDE68, 0xE2016572, 0xD4672E22, 0x420BD255,
+0xD6560009, 0xC93F6060, 0x7F042600, 0x6EF64F26,
+0x6DF6000B, 0x2FC62FB6, 0x2FE62FD6, 0xD25F4F22,
+0x6B436E73, 0x420B6C53, 0x20086D63, 0x64038D1C,
+0xE50ED149, 0x32526210, 0x60C38916, 0x804124B0,
+0x814160D3, 0xA007E500, 0x655D61BC, 0x00EC6053,
+0x364C6653, 0x80647501, 0x3213625D, 0xD63B8BF5,
+0xC9BF6060, 0x2600A008, 0xD23AD44D, 0x6EF64F26,
+0x6CF66DF6, 0x6BF6422B, 0x6EF64F26, 0x6CF66DF6,
+0x6BF6000B, 0x7FC44F22, 0x720262F3, 0x22512F41,
+0x45297202, 0x60632251, 0xE5C4E682, 0x67F38121,
+0x655C666C, 0xE408BFB6, 0x4F267F3C, 0x0009000B,
+0x2F962F86, 0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6,
+0xE1007FC4, 0x6513ECFF, 0x6B136CCD, 0xDE36D735,
+0xEDFF64F3, 0xD835EA04, 0x6053655C, 0x027D4000,
+0x32C0622D, 0x66038D0D, 0x09ED6063, 0x2491027D,
+0x24217402, 0x698202ED, 0x3928622D, 0x74022892,
+0x75017104, 0x6063625C, 0x07D532A2, 0x0EB58FE4,
+0x2448641C, 0xE6808905, 0x67F3E5C5, 0xBF79666C,
+0x7F3C655C, 0x6EF64F26, 0x6CF66DF6, 0x6AF66BF6,
+0x000B69F6, 0xD11E68F6, 0x6012D21E, 0xCB20E405,
+0x2102E500, 0x000B2242, 0x00002252, 0x001E1017,
+0x00203B38, 0x002018A2, 0x00203906, 0x001E1015,
+0x001E10BF, 0x00117800, 0x001E10FC, 0x00200610,
+0x0020390C, 0x00202AE2, 0x00203B3C, 0x002018EE,
+0x00203B58, 0x0011788C, 0x00203908, 0x002034EC,
+0x00201530, 0x001E2130, 0x00203B60, 0x00202AA4,
+0x00203B64, 0x0020396C, 0x00203974, 0x00203D9C,
+0x001C3500, 0x001D4004, 0xD564D163, 0xE400D764,
+0x2142E20F, 0x17411154, 0xD5622722, 0x9669D762,
+0x15412572, 0x96661562, 0xE6011565, 0xD55F1165,
+0x666CE6F8, 0x25422542, 0x25422542, 0x25422542,
+0x25622542, 0x7601E727, 0x67632572, 0x25627797,
+0xE7042572, 0x2572E248, 0xE2192522, 0xE2702522,
+0x25422542, 0x25422542, 0x25222542, 0x2522E20C,
+0x25422542, 0x25422542, 0x25422542, 0x25422542,
+0x000B154A, 0xE2081145, 0x0009422B, 0x2FE62FD6,
+0x7FFC4F22, 0xC8206043, 0x6E438D02, 0x0009BE67,
+0xC81060E3, 0xBE648901, 0x60E30009, 0x8901C840,
+0x0009BE86, 0xC80160E3, 0xDD3D8938, 0xC80260D0,
+0x2F008D03, 0x460BD63B, 0x60F00009, 0x8902C804,
+0x460BD639, 0x62F00009, 0xC8806023, 0x60D08902,
+0x2D00C97F, 0xC8016023, 0xD6348906, 0x0009460B,
+0x0009A007, 0x51630601, 0x8902C808, 0x460BD630,
+0x60F00009, 0x8902C810, 0x420BD22E, 0xD52E0009,
+0x88026052, 0xD22D8B03, 0xA005E604, 0x88012260,
+0xD22A8B02, 0x2260E601, 0x2522E200, 0xC88060E3,
+0xD227892D, 0x60E36E20, 0x8902C880, 0x420BD225,
+0x60E30009, 0x8902C840, 0x420BD223, 0x60E30009,
+0x8902C802, 0x420BD221, 0x60E30009, 0x890DC804,
+0xDD20D11F, 0x0009410B, 0x0009BF0D, 0x0009BF4C,
+0xD51ED41D, 0x2470E708, 0x25D2BF85, 0xC80860E3,
+0xD21B8905, 0x4F267F04, 0x422B6EF6, 0x7F046DF6,
+0x6EF64F26, 0x6DF6000B, 0x001C581C, 0xA000A000,
+0x001D0100, 0x001D4000, 0x00040021, 0x001C589C,
+0x001E1021, 0x00201A88, 0x00201AAA, 0x0020210C,
+0x00201AC2, 0x00201AD0, 0x002039C0, 0x001E100B,
+0x001E1028, 0x00201B3C, 0x00201B48, 0x00201AD8,
+0x00201AF6, 0x12345678, 0x001E1000, 0x0010F100,
+0x00201B24, 0x644CD6A7, 0x000B346C, 0xD6A62450,
+0x346C644C, 0x2450000B, 0x644CD6A4, 0x000B346C,
+0x625C2450, 0x4208616D, 0x42084119, 0x42006019,
+0x670E614C, 0xD49E321C, 0x4200207D, 0x324CC90F,
+0x2200000B, 0x4208625C, 0x42004208, 0x324C644C,
+0x4200D498, 0x000B324C, 0x2FE62260, 0x614C4F12,
+0x4100D493, 0x6710314C, 0xE29F666D, 0x27294619,
+0x6E536269, 0x672E6573, 0x4221227D, 0x42214221,
+0x7601662C, 0xE4014608, 0x34E84608, 0x644C4600,
+0x071A0467, 0x2150257B, 0x000B4F16, 0x4F226EF6,
+0xD2857FE8, 0x88016021, 0xD2848B7B, 0x26686621,
+0xD2838B77, 0x26686621, 0xE50F8B73, 0xE401BFA2,
+0xBFA4E501, 0xE586E400, 0xE400655C, 0x2F50BFA4,
+0xBFA1E401, 0xE602E506, 0x60634618, 0x81F2E401,
+0x6543BF9F, 0xE40185F2, 0xBFAB6543, 0x85F26603,
+0x6543E401, 0x6603BFB1, 0xE40265F0, 0x6053756C,
+0x80F8BF80, 0xBF82E402, 0x84F8E512, 0x7090E402,
+0x6503BF82, 0x4618E602, 0x81F66063, 0xBF80E402,
+0x85F6E500, 0x6603E402, 0xE500BF8C, 0xE40285F6,
+0xBF926603, 0xE5FEE500, 0xE010655C, 0xBF61E403,
+0xE5130F54, 0xE40EBF63, 0x05FCE010, 0xBF63E40E,
+0xE5007585, 0xBF64E403, 0xE500E640, 0xBF71E403,
+0xE500E640, 0xBF78E403, 0xE5FFE640, 0xE014655C,
+0xBF47E404, 0xE40F0F54, 0xE504BF49, 0x05FCE014,
+0xBF49E40F, 0xE5017584, 0xBF4AE640, 0xE501E404,
+0xBF57E640, 0xE501E404, 0xE404E640, 0xAF5C7F18,
+0x7F184F26, 0x000B4F26, 0x4F220009, 0xD2427FF0,
+0x88016021, 0xD2418B71, 0x26686621, 0xD2408B6D,
+0x26686621, 0xE50F8B69, 0xE401BF1C, 0xBF1EE501,
+0xE586E400, 0xE400655C, 0x2F50BF1E, 0xBF1BE401,
+0xE401E506, 0xBF1C6543, 0xE401E640, 0xBF296543,
+0xE401E640, 0xBF306543, 0x65F0E640, 0x756CE402,
+0xBEFF6053, 0xE40280F4, 0xE512BF01, 0xE40284F4,
+0xBF017090, 0xE6406503, 0xBF02E402, 0xE640E500,
+0xBF0FE402, 0xE640E500, 0xBF16E402, 0xE5FEE500,
+0x6053655C, 0xBEE5E403, 0xE51380F8, 0xE40EBEE7,
+0xE40E84F8, 0xBEE77085, 0xE5006503, 0xBEE8E640,
+0xE500E403, 0xBEF5E640, 0xE500E403, 0xBEFCE640,
+0xE5FFE403, 0x6053655C, 0xBECBE404, 0xE40F80FC,
+0xE504BECD, 0xE40F84FC, 0xBECD7083, 0xE5016503,
+0xBECEE640, 0xE501E404, 0xBEDBE640, 0xE501E404,
+0xE404E640, 0xAEE07F10, 0x7F104F26, 0x000B4F26,
+0x00000009, 0x001E102F, 0x001E1080, 0x001E1090,
+0x001E103F, 0x001E103E, 0x002039BA, 0x002039BC,
+0x002039BE, 0xD21DD11C, 0x66206010, 0x676C7001,
+0x3700C90F, 0xE5008D13, 0x67106210, 0x7701622C,
+0x64232170, 0xD6166010, 0x44084408, 0x3428C90F,
+0x62602100, 0x7201D513, 0x44082620, 0x000B354C,
+0xD10F6053, 0x25586510, 0xE6008D13, 0xD60DD40B,
+0x655C6540, 0x47086753, 0x37584708, 0x47086540,
+0x24507501, 0x367C6040, 0x2400C90F, 0x72FF6210,
+0x000B2120, 0x00006063, 0x00203905, 0x00203904,
+0x00203906, 0x0020352C, 0x7FFC4F22, 0xE680D19F,
+0x666C6212, 0xD29E2F22, 0x67F36563, 0x420B7542,
+0x7F04E404, 0x000B4F26, 0xE6800009, 0xD298666C,
+0xE7006563, 0x422B7540, 0xE6806473, 0xD294666C,
+0xE7006563, 0x422B7543, 0x2F866473, 0x2FA62F96,
+0x2FC62FB6, 0x2FE62FD6, 0x7FCC4F22, 0xDC8ED28D,
+0x72011F21, 0xDB8D1F22, 0xD18EDE8D, 0x66125211,
+0x8B013620, 0x0009A0E5, 0xC9036061, 0x8B018801,
+0x0009A0DF, 0xD288D487, 0xED84420B, 0x2F025503,
+0x30D0845C, 0xA0B88901, 0xD1840009, 0x626C6610,
+0x88016023, 0xD1828B68, 0x62101FC3, 0x895B2228,
+0xE003D480, 0x40186742, 0x68421772, 0xD57EE900,
+0x81816DB3, 0x7D042190, 0x67D26AB2, 0x64E26852,
+0x1F491F57, 0x740464E3, 0x1FA46542, 0x65431F5A,
+0x625275F8, 0x1F761FD5, 0x6D531F2B, 0xDA74D773,
+0x7D94D274, 0x68D21F88, 0x6AA26972, 0xD1726022,
+0x2202CB20, 0xE1401F1C, 0x7601E600, 0x3213626D,
+0x56F48BFB, 0x52F651F5, 0x21222B62, 0x52F851F7,
+0x212256F9, 0x2E6251FA, 0x51FB2412, 0x2D822512,
+0xD9662792, 0x29A2DD5F, 0x6AD2D965, 0xD9646892,
+0x68D21A84, 0x6081DA63, 0x2801CB01, 0xD86266D2,
+0x2A622962, 0xED015AFC, 0x2AD2480B, 0x2AD24D18,
+0x62D2DD5E, 0x2D227201, 0xD15056F3, 0xE2026062,
+0x2602CB01, 0x2120A03D, 0x8B3A2228, 0xE401DD58,
+0x2140E600, 0xE01C2D62, 0xC801005C, 0xD4558B0A,
+0xE600D755, 0xED7D2472, 0x626C7601, 0x8BFB32D3,
+0x24D2DD52, 0xE2FE68C2, 0x2C822829, 0x095CE01E,
+0xE01F5DF1, 0x0A5C2D90, 0x751051F2, 0xED0621A0,
+0xD74BE600, 0x8456D44B, 0x27007601, 0x696C6854,
+0x248039D3, 0x8FF67401, 0xDA477701, 0x2A10E194,
+0xE2007A01, 0x7A0F2A20, 0xD130E805, 0x66102A80,
+0x6023626C, 0x89088801, 0xD240D42A, 0x420B65F2,
+0xD131ED01, 0xAF304D18, 0x65F221D2, 0x8553D43C,
+0x620D6642, 0x89073262, 0xD13BD43A, 0x0009410B,
+0xE601D73A, 0x2762AF1A, 0xD134D41E, 0x410B65F2,
+0xD125ED01, 0xD637D436, 0x460B4D18, 0xAF0D21D2,
+0x7F340009, 0x6EF64F26, 0x6CF66DF6, 0x6AF66BF6,
+0x000B69F6, 0x4F2268F6, 0x85467FF4, 0x2F01E681,
+0x666C8547, 0x854881F1, 0x81F2D209, 0x67F38542,
+0x854381F3, 0x81F4E40C, 0x65636053, 0x420B81F5,
+0x7F0C7540, 0x000B4F26, 0x00000009, 0x001C3D9C,
+0x00202454, 0x0011779A, 0x001C36F8, 0x001C3B9C,
+0x001C3704, 0x00203524, 0x002014A0, 0x00203915,
+0x00203914, 0x00203910, 0x001C3D98, 0x001C3BB4,
+0x001C5960, 0x001C3500, 0x001C3D30, 0x001C8960,
+0x002034FC, 0x001C3D00, 0x0020160C, 0x00117730,
+0x00203918, 0x001C582C, 0x2000A000, 0x0000A000,
+0x0011778C, 0x00117792, 0x00117788, 0x002014CC,
+0x002038EC, 0x002034EC, 0x00201530, 0x001E2130,
+0x00203D7C, 0x002018A2, 0x2F962F86, 0x2FB62FA6,
+0x2FD62FC6, 0x4F222FE6, 0xD19B7FEC, 0x2F12E000,
+0x6103D49A, 0x1F4281F2, 0xDD9ADA99, 0xD69A6813,
+0xE0014808, 0x460BDE99, 0x38EC4800, 0x65A21F03,
+0x352052A1, 0xA23E8B01, 0x60510009, 0x8801C903,
+0xA2388B01, 0x52530009, 0x32E0DE91, 0xD9918B10,
+0x64A3490B, 0x4B0BDB90, 0xDE906403, 0xD791D690,
+0xEC01D591, 0x2E02E100, 0x271026C0, 0x2502AFDF,
+0xC8018551, 0xA1578B01, 0x62510009, 0x4200622D,
+0x5E53366A, 0x85E2226D, 0xC903642C, 0x85E36603,
+0x6053650D, 0x40214021, 0x4500C93F, 0x322A6703,
+0x6053252D, 0xC901D17F, 0x60106C03, 0x8801D97F,
+0xDB7F8B05, 0x2120E200, 0xCB0160B2, 0xD17D2B02,
+0x88016011, 0x65A28B0A, 0x8D042448, 0x9B9E6251,
+0xA00322B9, 0x919B2521, 0x2521221B, 0x37B3EB10,
+0x2448895E, 0xD4738B07, 0x22286241, 0x60638903,
+0xA05781F8, 0xD5706473, 0x46084608, 0x85E26273,
+0x46006B50, 0x362C4200, 0x2BB8C910, 0x8F1F6463,
+0x26686603, 0xD2698911, 0x062D6043, 0x4119616D,
+0x6B0E6019, 0x81F820BD, 0x880160C3, 0x646C8F2C,
+0x880F6073, 0xA0278B1B, 0xD2610009, 0x052D6043,
+0x4119615D, 0x670E6019, 0x645C207D, 0x81F8A01C,
+0x890F2668, 0x6043D25B, 0x6B5D052D, 0x60B94B19,
+0x201D610E, 0x60C381F8, 0x8F0D8801, 0x6473645C,
+0xEC00A00A, 0x6043D254, 0x625D052D, 0x60294219,
+0x207D670E, 0x81F8645C, 0x880285F8, 0x85E1890A,
+0x8D07C820, 0xE6DC6203, 0x60232269, 0x81E1A002,
+0x644CE4FF, 0x6210D149, 0x89012228, 0x644CE4FF,
+0x654DEBFF, 0x35B06BBC, 0xDB368B2B, 0x64A34B0B,
+0x410BD135, 0x54036403, 0x85446E03, 0xC948DB40,
+0xDC408808, 0xBEAE8B01, 0x64B3E502, 0x65E34C0B,
+0xDB3DEC01, 0xD13D2DC2, 0x621260B2, 0x72017001,
+0x21228805, 0x2B028F08, 0x666CE680, 0x6563D238,
+0x7549E700, 0x6473420B, 0xA030D436, 0x7FFF0009,
+0x85E28000, 0x20B9EBFC, 0x610381E2, 0x942A85E3,
+0x62032049, 0x450885F8, 0x81E2201B, 0xC90160C3,
+0x40084018, 0x40084008, 0x4000225B, 0x6023220B,
+0x85E481E3, 0x4118E108, 0x81E4201B, 0xE40262A2,
+0x20B98521, 0x67A28121, 0xCB016071, 0x85F82701,
+0x89033042, 0xECE785E2, 0x81E220C9, 0x490BD41E,
+0xA03B0009, 0x7E030009, 0x001C3D30, 0x00203D88,
+0x002034FC, 0x001E212C, 0x002033E0, 0x001C3D00,
+0x00117780, 0x002014A0, 0x0020166C, 0x0011770C,
+0x00203914, 0x00203915, 0x00203910, 0x002018A2,
+0x001C36F8, 0x00203988, 0x00203D98, 0x00203B7C,
+0x00203BFC, 0x00203C7C, 0x00203CFC, 0x00203900,
+0x002034F4, 0x002014CC, 0x0020398C, 0x00203990,
+0x00202454, 0x00203D80, 0x00203D84, 0x602262F2,
+0x40094019, 0xC90F4009, 0x8B0B880A, 0x60E2DE8C,
+0x40094019, 0xC90F4009, 0x8B038808, 0xCB0160A2,
+0x2802A006, 0x65E2DE87, 0x2E527501, 0x286266A2,
+0x52F366F2, 0x2622AE83, 0xD2838551, 0xDE83C802,
+0xA0958B01, 0x420B0009, 0x4E0B64A3, 0x5E036403,
+0x85E46503, 0x4918E908, 0xD77D209B, 0xE04C81E4,
+0xDC7C0B7E, 0x7B01D97C, 0x61C207B6, 0x71016690,
+0x8D062668, 0xD4792C12, 0x420BD279, 0xA070EB01,
+0x62512DB2, 0x4B18EB0F, 0x22B9E102, 0x32104118,
+0x85518B0F, 0x2029E2FC, 0x60518151, 0xCB0172E0,
+0x85E12501, 0x202994A3, 0x85E481E1, 0xA0522049,
+0x675181E4, 0x4719677D, 0x667E6779, 0x7701276D,
+0x6903607C, 0x88014918, 0x25918F3E, 0x6B12D161,
+0x21B27B01, 0x660D85E3, 0x40216063, 0xC93F4021,
+0x6C034600, 0x262D322A, 0xC8016063, 0xDB5ED15D,
+0x967D8901, 0xE6002C6B, 0x666C67CD, 0x40006063,
+0x622D021D, 0x8D0E3270, 0x60436403, 0xE9FF021D,
+0x8B013290, 0x01C5A007, 0x626C7601, 0x3292E904,
+0x646C8BEB, 0x60434400, 0xD15004BD, 0x0B457401,
+0x669D6911, 0x89073670, 0x602D6211, 0x890388FF,
+0xE201DB4B, 0x2B2021C1, 0xECFC8551, 0x815120C9,
+0xCB016051, 0xDC472501, 0x64A34C0B, 0x51F366F2,
+0x85EF2612, 0x54F2D244, 0x650D420B, 0x0009ADE7,
+0xE500DC42, 0x420B2C52, 0x4E0B64A3, 0x54036403,
+0x85446E03, 0x6703E908, 0x65034918, 0x27998541,
+0xDB323790, 0x8F0BD932, 0x6013610D, 0x8B07C820,
+0xC9486053, 0x8B038808, 0xE501BD4D, 0x0009A005,
+0x2128D233, 0xBD468901, 0x64B3E500, 0x490B65E3,
+0xADBCEC01, 0x85F22DC2, 0x7001EE04, 0x31E7610D,
+0x8D0281F2, 0xADA97A08, 0x7F140009, 0x6EF64F26,
+0x6CF66DF6, 0x6AF66BF6, 0x000B69F6, 0xF7FF68F6,
+0x2FE68000, 0xD2234F22, 0x60E36E22, 0x8D02C840,
+0xBBF922E2, 0xE2400009, 0x2E284218, 0xBC048901,
+0x60E30009, 0x8905C810, 0xD21CD41B, 0x0009420B,
+0x0009BC03, 0xC80560E3, 0xBD6D8901, 0x60E30009,
+0x8902C802, 0xAC004F26, 0x4F266EF6, 0x6EF6000B,
+0x001C3D3C, 0x00117760, 0x002014A0, 0x0020166C,
+0x0020348C, 0x00203D9C, 0x00203900, 0x002034F4,
+0x002014CC, 0x0020396C, 0x00203974, 0x00203968,
+0x0020396A, 0x00201530, 0x002018EE, 0x0020398C,
+0x00008000, 0x001C3510, 0x00203D90, 0x002018A2,
+0x080A0C0E, 0x00020406, 0x1A1C1E20, 0x12141618,
+0x2E303234, 0x26282A2C, 0x3A3C3E40, 0x6C625648,
+0x41112F26, 0xE2208F18, 0x890B3123, 0x321CD204,
+0xD1026220, 0x412B312C, 0x00090009, 0x0020340A,
+0x002033C0, 0x000BE000, 0x400062F6, 0x40004000,
+0x40004000, 0x40004000, 0x62F6000B, 0x40004000,
+0x40004000, 0x40004000, 0x40184000, 0x62F6000B,
+0x40004000, 0x40004000, 0x40004000, 0x40284000,
+0x62F6000B, 0x40004000, 0x40184000, 0x000B4028,
+0xC90F62F6, 0x40054005, 0x40054005, 0x62F6000B,
+0x4005C907, 0x40054005, 0x62F6000B, 0x4005C903,
+0x000B4005, 0xC90162F6, 0x000B4005, 0x000062F6,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x544F0D0A,
+0x46205355, 0x00003A57, 0x206C754A, 0x32203120,
+0x20383030, 0x323A3132, 0x32313A37, 0x00000000,
+0x00000D0A, 0x00000043, 0x42707372, 0x3D206675,
+0x554E203D, 0x202C4C4C, 0x6E49677A, 0x4E497274,
+0x6D754E51, 0x0000003D, 0x61766E49, 0x2064696C,
+0x72657375, 0x20726F20, 0x2079656B, 0x00214449,
+0x52504545, 0x57204D4F, 0x65746972, 0x6461202C,
+0x003D7264, 0x6C617620, 0x0000003D, 0x00000A0D,
+0x435F4D5A, 0x465F444D, 0x4C445F57, 0x494E495F,
+0x00000054, 0x6E6B6E55, 0x206E776F, 0x6D6D6F63,
+0x3D646E61, 0x00000000, 0x203A3051, 0x00000020,
+0x203A3151, 0x00000020, 0x203A3251, 0x00000020,
+0x203A3351, 0x00000020, 0x203A3451, 0x00000020,
+0x2B434741, 0x73696F4E, 0x61432065, 0x7262696C,
+0x6F697461, 0x6166206E, 0x6F206C69, 0x6974206E,
+0x0D0A656D, 0x00000000, 0x00000072, 0x00205220,
+0x00000D0A, 0x62735576, 0x7473725F, 0x00000A0D,
+0x62735576, 0x7375735F, 0x646E6570, 0x00000A0D,
+0x62735576, 0x7365725F, 0x000A0D6D, 0x00000044,
+0x44387570, 0x72637365, 0x6F747069, 0x3D584572,
+0x00000000, 0x00000047, 0x00000042, 0x72746E49,
+0x6D652051, 0x2C797470, 0x49677A20, 0x4972746E,
+0x754E514E, 0x00003D6D, 0x654C7245, 0x0000006E,
+0x00000049, 0x20746F4E, 0x756F6E65, 0x49206867,
+0x4220514E, 0x0A0D6675, 0x00000000, 0x000000FF,
+0x00020001, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x010E010D,
+0x00020003, 0x01090108, 0x0002010A, 0x02000003,
+0x02020201, 0x02040203, 0x02060205, 0x02020200,
+0x02040203, 0x020C020B, 0x020E020D, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x000000FF,
+0x00020001, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x010E010D,
+0x00020003, 0x01090108, 0x0002010A, 0x00030003,
+0x02020201, 0x02040203, 0x02060205, 0x02020200,
+0x02040203, 0x020C020B, 0x020E020D, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x010E010D,
+0x00FF010F, 0x01090108, 0x010B010A, 0x0200010F,
+0x02020201, 0x02040203, 0x02060205, 0x02020200,
+0x02040203, 0x020C020B, 0x020E020D, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x010E010D,
+0x00FF010F, 0x01090108, 0x010B010A, 0x010F010F,
+0x02020201, 0x02040203, 0x02060205, 0x02020200,
+0x02040203, 0x020C020B, 0x020E020D, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00205220,
+0x00000046, 0x00000059, 0x73204142, 0x003D7165,
+0x49544120, 0x0000204D, 0x00000000, 0x00000000,
+0x002E0209, 0x80000101, 0x000409FA, 0x00FF0400,
+0x05070000, 0x02000201, 0x82050700, 0x00020002,
+0x03830507, 0x07010040, 0x40030405, 0x02090100,
+0x0101002E, 0x09FA8000, 0x04000004, 0x000000FF,
+0x02010507, 0x07000040, 0x40028205, 0x05070000,
+0x00400383, 0x04050701, 0x00004002, 0x00000000,
+0x00000000, 0x07090000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, };
+
+const u32_t zcFwImageSize=15928;
diff --git a/drivers/staging/otus/hal/hpfwu_BA.c b/drivers/staging/otus/hal/hpfwu_BA.c
new file mode 100644
index 00000000000..0c741571f2b
--- /dev/null
+++ b/drivers/staging/otus/hal/hpfwu_BA.c
@@ -0,0 +1,874 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include "cprecomp.h"
+
+const u32_t zcFwImage[] = {
+0x0009000B, 0x4F222FE6, 0xDE917FFC, 0xE114D791,
+0x1E13D491, 0x1E4C470B, 0x0009B017, 0x95C2E600,
+0xC84060E2, 0x2F028F03, 0x8FF93652, 0xD48B7601,
+0x4E0BDE8B, 0xD48B0009, 0x00094E0B, 0x4E0BD48A,
+0x7F040009, 0xA0474F26, 0x4F226EF6, 0x410BD187,
+0xD4870009, 0x0009440B, 0x450BD586, 0xD7860009,
+0x611DE1FF, 0xD1852712, 0x6012E2FF, 0xCB01D484,
+0x71DC2102, 0x71042122, 0x2122E501, 0xD5812452,
+0xD2819792, 0xE7002572, 0xD481D180, 0x2270D681,
+0x2172E201, 0x26202470, 0xE4FFD67F, 0xE6002641,
+0xE104D57E, 0x6063666D, 0x626D7601, 0x32124000,
+0x05458FF8, 0xE501D27A, 0xD17A2250, 0xD57BD47A,
+0xE700E600, 0x25722470, 0x11622162, 0x11691166,
+0x4F26116A, 0x116E000B, 0xD1757FC4, 0x2F12D875,
+0xD476D175, 0xD577D676, 0x1F87D777, 0xD97778FC,
+0x1F1BD277, 0x1F417104, 0x1F647404, 0x1F887604,
+0x71F41F1C, 0x1F42E8C8, 0x1F651F53, 0x1F991F76,
+0x1F1D1F2A, 0xDD6F688C, 0xDA70DE6F, 0xDC71DB70,
+0x00094A0B, 0x00094B0B, 0x00094C0B, 0x6010D15E,
+0x8B0F8801, 0xE950D15D, 0x49186212, 0x8B073296,
+0x56FAD159, 0x2120E200, 0xCB016062, 0x2602A002,
+0x21227201, 0x880160D2, 0xD1638907, 0x32866212,
+0xD1628903, 0x88016010, 0x64E28BDA, 0x52F751F8,
+0x55E12142, 0x2252D15E, 0x661254FB, 0x246259FC,
+0x29725711, 0x880160D2, 0x66E28B53, 0x362052E1,
+0x6061894C, 0x8801C90F, 0xD1568B48, 0x36206212,
+0xA0438903, 0x27102162, 0xD5530FA0, 0x6651E710,
+0x626D7601, 0x8F3C3273, 0x65F22561, 0x695251F2,
+0x54F359F1, 0x679252F4, 0x61426512, 0x56F66922,
+0x642252F5, 0xCB206062, 0xE6002602, 0x76011F1E,
+0x626DE110, 0x32134118, 0x51FE8FF8, 0x267256F1,
+0x56F457F2, 0x55F32752, 0x251257F5, 0x27422692,
+0x51F969E2, 0x2192D43D, 0xE90161F2, 0x2192440B,
+0x491865F2, 0xD9382592, 0xE200D539, 0x62512921,
+0x720154FD, 0x622D2521, 0x2422A003, 0xE200D932,
+0xE9012921, 0x2D92D12C, 0x26686612, 0xAF6F8B01,
+0xD6300009, 0x0009460B, 0xE700D128, 0x2170AF68,
+0x001C001C, 0x00200F7C, 0x0000B38E, 0x0020322C,
+0x0020145E, 0x00203238, 0x00203250, 0x0020141C,
+0x0020151C, 0x00200FA0, 0x001C3510, 0x001C3648,
+0x001E212C, 0x00203188, 0x00202D24, 0x00203190,
+0x0020319C, 0x002031A8, 0x002031B8, 0x002031BC,
+0x002031B0, 0x00117708, 0x002031B1, 0x002031B4,
+0x001C3D30, 0x00117718, 0x00117734, 0x001C3B9C,
+0x001C3704, 0x001C3D98, 0x001C3500, 0x001C3D00,
+0x001C36F8, 0x001C1028, 0x00202D98, 0x00201328,
+0x00202C04, 0x00201E18, 0x002034BC, 0x002031BA,
+0x00202D90, 0x002031CC, 0x002031D0, 0x00201276,
+0x002031D2, 0x00201FD0, 0x2FB62F96, 0x2FD62FC6,
+0x4F222FE6, 0xDE947F8C, 0x61E0E024, 0x0F14D493,
+0x710161E3, 0xD7926210, 0x470BE028, 0xD5910F24,
+0x0009450B, 0x6D032008, 0x1F0B8F11, 0xD48FDC8E,
+0xDD8F67C0, 0x657C4D0B, 0xDD8FD18E, 0x6B9C6910,
+0x420862B3, 0x32B84208, 0x3D2C4208, 0xE0281FDB,
+0xE58004FC, 0x604C66E2, 0x3050655C, 0x2D628F13,
+0x01FCE024, 0x641CE500, 0x625DDE84, 0x8B013243,
+0x0009A33D, 0x6753655D, 0x607037EC, 0x39DC6953,
+0xAFF27501, 0x20088094, 0xE0248B13, 0xE50001FC,
+0xA009DE7A, 0x655D641C, 0x32EC6253, 0x6C536B22,
+0x3CDC67B2, 0x75041C71, 0x3243625D, 0xA31E8BF3,
+0x88012D10, 0xE0248B16, 0xE40001FC, 0x671C2D40,
+0x624DDE6E, 0x8B013273, 0x0009A311, 0x6CE3644D,
+0x7C046943, 0x39EC6B43, 0x65923BCC, 0x74086DB2,
+0x25D2AFEF, 0x8B198804, 0x01FCE024, 0x2D70E700,
+0x1FD86D1C, 0x627DDE61, 0x8B0132D3, 0x0009A2F7,
+0x6B73677D, 0x3BEC61E3, 0x710464B2, 0x3C1C6C73,
+0x694265C2, 0x29597708, 0x2492AFED, 0x8B188805,
+0x01FCE024, 0x2D40E400, 0xDE54671C, 0x3273624D,
+0xA2DC8B01, 0x644D0009, 0x6BE36D43, 0x65D23DEC,
+0x61437B04, 0x6C1231BC, 0x74086952, 0xAFED29CB,
+0x88312592, 0xDE4A8B20, 0x65E6DB4A, 0x61E6DC4A,
+0x67E2D94A, 0x62E27E04, 0x1FEC7EE8, 0x7E0464E2,
+0x6EE21FED, 0x5BFD2BE0, 0x60B27B04, 0xC9011FBE,
+0x6BB22C00, 0x29B04B09, 0xDC412F26, 0x66134C0B,
+0xE2007F04, 0x2D20A2AB, 0x8B218830, 0xD939DE38,
+0xE06465E6, 0x720462E3, 0x672666E2, 0x6E23DC36,
+0x62227EE8, 0x6BE261E6, 0x29B01FEF, 0x7E040F16,
+0xC90160E2, 0x6EE22C00, 0x4E09DC30, 0x2F262CE0,
+0xD130E068, 0x04FE410B, 0xE2007F04, 0x2D20A287,
+0x8B058833, 0x4E0BDE2C, 0xE1000009, 0x2D10A27F,
+0x89018828, 0x0009A106, 0xE143DE20, 0xE04062E1,
+0x3217622D, 0x0FE68F04, 0x6023E240, 0x262106FE,
+0x8B013217, 0x0009A0EF, 0x02FEE040, 0x8521E401,
+0x8B013046, 0x0009A0E7, 0xE501E040, 0x2D5007FE,
+0x6471B265, 0x09FEE040, 0x6291E143, 0x652DE068,
+0x8D6B3517, 0xE6400F56, 0x8B273563, 0xE048E600,
+0xE11A0F65, 0x72C0A031, 0x00117800, 0x00203254,
+0x0020145E, 0x00202588, 0x002031A2, 0x00203258,
+0x002014AA, 0x002031A1, 0x00202DC8, 0x00117804,
+0x00117810, 0x0020319D, 0x0020319E, 0x0020319F,
+0x00200C2C, 0x00200C80, 0x00200C7C, 0x41216153,
+0x41214121, 0x41214121, 0x45214521, 0x60534521,
+0x6603C903, 0x0F65E048, 0xE0077118, 0xE0442209,
+0x641D0F25, 0x65F3E04C, 0x0F46B28C, 0x04FDE048,
+0x0BFDE044, 0x61BD674D, 0x41084708, 0x0F16E050,
+0xD29B6073, 0x420B09FE, 0x6C07E00F, 0x607329C9,
+0xE0400F96, 0x65F30EFE, 0x6D0D85E2, 0x01FEE050,
+0x60D3420B, 0x6073290B, 0xE04C0F96, 0x04FEB251,
+0x06FEE040, 0x6261E068, 0x0F56652D, 0x3563E640,
+0xE000894E, 0x602381F8, 0x4008C903, 0x6B034000,
+0xE0546103, 0xE0580FB6, 0xECFFDD85, 0x6CCC0FF6,
+0x0FD6E06C, 0x4D0B60C3, 0x42216253, 0x42214221,
+0x64234221, 0x324C4200, 0xE05C6E07, 0x45214200,
+0xE0400FE6, 0x0BFE4521, 0xC9036053, 0x30FC4008,
+0x6D037B06, 0x85F81F05, 0x6C2D1FB7, 0x1FC66E03,
+0x0FC6E060, 0x05FEE058, 0x64C3B22C, 0x33FCE354,
+0x563262D2, 0x22696132, 0x67B42D22, 0x490B5936,
+0x220B607C, 0x05FEE058, 0x64C32D22, 0x7E01B201,
+0xE70662ED, 0x8FE33273, 0xE0407C01, 0x626106FE,
+0x06FEE040, 0x85614200, 0x302C760C, 0x6103701B,
+0x64F3E500, 0x7501E704, 0x6B5D6966, 0x24923B73,
+0x74048FF9, 0xB1E465F3, 0xE040641D, 0xB1A306FE,
+0xA17C6461, 0xD4570009, 0xE201D757, 0x2D20470B,
+0x0009A175, 0x8B078829, 0xEC00DE54, 0x61E22DC0,
+0x641DB175, 0x0009A16B, 0x622CE281, 0x8B013020,
+0x0009A0B6, 0x06FCE028, 0xE682626C, 0x3260666C,
+0x56FB8B20, 0x2610E124, 0x5217D149, 0x52181621,
+0x52191622, 0x521A1623, 0x551B1624, 0x1655E200,
+0x1656551C, 0x1657551D, 0x1658551E, 0x1659551F,
+0x11281127, 0x112A1129, 0x112C112B, 0x112E112D,
+0x112FA13D, 0x666CE683, 0x8B0B3260, 0xD63752FB,
+0x2250E500, 0xD2376562, 0x22527604, 0xD6366262,
+0x2620A12D, 0x666CE690, 0x8B033260, 0x0009B1C7,
+0x0009A011, 0x666CE691, 0x8B103260, 0x6252D52B,
+0x2228622C, 0xD22D8904, 0x0009420B, 0x0009A003,
+0x420BD22B, 0x56FB0009, 0xA110E200, 0xE6B02620,
+0x3260666C, 0xE0248B34, 0xE07002FC, 0x0F16612C,
+0xEB04EC00, 0x01FEE070, 0x321362CD, 0xA0FE8B01,
+0xD21A0009, 0x6DC36CCD, 0x72043D2C, 0x312C61C3,
+0x6D126ED2, 0xD114D41B, 0x0009410B, 0x410BD11A,
+0xD41A64E3, 0x420BD210, 0xD2170009, 0x64D3420B,
+0xD60DD417, 0x0009460B, 0x61E3E600, 0x316C666D,
+0x626D7601, 0x21D032B3, 0x4D198FF7, 0x7C08AFD2,
+0xD211D410, 0xD4116542, 0x0009420B, 0x0009A0CF,
+0x00202C80, 0x00203278, 0x0020145E, 0x00117804,
+0x00202D2C, 0x00203188, 0x0020319C, 0x00200CBA,
+0x00200CE0, 0x00203290, 0x002014A2, 0x002032A4,
+0x002032AC, 0x00117800, 0x002014AA, 0x002032B0,
+0xD5B5D1B4, 0x6252E040, 0x75046612, 0x2162362C,
+0x56116256, 0x1161362C, 0x62526653, 0x76085512,
+0x1152352C, 0x55136262, 0x352C76EC, 0x65631153,
+0x56146262, 0x362C7510, 0x66531164, 0x55156252,
+0x352C7610, 0x62621155, 0x362C5616, 0xD6A31166,
+0x55176262, 0x352C7604, 0x62661157, 0x352C5518,
+0x65631158, 0x56196262, 0x362C7504, 0x62561169,
+0x362C561A, 0x6256116A, 0x362C561B, 0x6653116B,
+0x551C6252, 0x352C7604, 0x6266115C, 0x352C551D,
+0x6263115D, 0x551E6662, 0x356C7204, 0x6622115E,
+0xD58F521F, 0x112F326C, 0x061E6252, 0x362C7594,
+0xE0440166, 0x62526653, 0x7644051E, 0x0156352C,
+0x6262E048, 0x362C061E, 0xD6860166, 0x6262E054,
+0x4229051E, 0x0156352C, 0x62627604, 0x061EE058,
+0x362C4229, 0x56FB0166, 0x2620E238, 0x021EE044,
+0x1621E048, 0x16226212, 0x16235211, 0xE2005512,
+0x55151654, 0x55131655, 0x55161656, 0x051E1657,
+0x1658E040, 0xE050051E, 0x55141659, 0x051E165A,
+0x165BE04C, 0xE054051E, 0x051E165C, 0x165DE058,
+0xE044051E, 0x0126165E, 0x2122E048, 0x11221121,
+0x11231125, 0x01261126, 0x0126E040, 0x1124E050,
+0xE04C0126, 0xE0540126, 0xE0580126, 0x7F740126,
+0x6EF64F26, 0x6CF66DF6, 0x000B6BF6, 0x4F2269F6,
+0xE240614D, 0x89143123, 0x3127E21F, 0x8B09D75A,
+0xD45A614D, 0xE00171E0, 0x5671440B, 0x26596507,
+0x1761A007, 0xE001D455, 0x6672440B, 0x26596507,
+0x4F262762, 0x0009000B, 0x614D4F22, 0x3123E240,
+0xE21F8912, 0xD74C3127, 0x614D8B08, 0x5671D24B,
+0x420B71E0, 0x260BE001, 0x1761A006, 0x6672D247,
+0xE001420B, 0x2762260B, 0x000B4F26, 0xE6400009,
+0x46284618, 0x6252D542, 0x89FC2268, 0x0009000B,
+0x4618E680, 0xD53E4628, 0x22686252, 0x000B89FC,
+0xA0010009, 0x7201E200, 0x8BFC3242, 0x0009000B,
+0x4618E680, 0xD5374628, 0x22686252, 0x000B8BFC,
+0x2FE60009, 0x7FFC4F22, 0xBFF16E53, 0x61E22F42,
+0xE280D631, 0x54E11615, 0x16464218, 0x422855E2,
+0x57E31657, 0x16786EF2, 0x26E22E2B, 0x4F267F04,
+0x6EF6AFCE, 0x2FD62FC6, 0x4F222FE6, 0x6C53DD26,
+0x6E43BFD6, 0x2DE2BFBB, 0x0009BFD2, 0x2C1251D5,
+0x1C4154D6, 0x1C5255D7, 0x1C6356D8, 0x6EF64F26,
+0x000B6DF6, 0x61636CF6, 0xA004E600, 0x62564109,
+0x24227601, 0x36127404, 0x000B8BF9, 0x4F220009,
+0xD117D416, 0x0009410B, 0xD417D216, 0xE5056022,
+0x2202CB20, 0xD5152452, 0x450BE700, 0xD7142472,
+0x0009470B, 0xE601D113, 0x2162D213, 0x4F264618,
+0x2262000B, 0x00202D2C, 0x001C36A0, 0x001C3CA0,
+0x001C36F4, 0x001C3B88, 0x001C3704, 0x00202C80,
+0x001C373C, 0x001C3700, 0x001C370C, 0x002032C4,
+0x0020145E, 0x001C3500, 0x001D4004, 0x002014D4,
+0x00200FA0, 0x001E212C, 0x001C3D30, 0x0009A1A9,
+0x2FE62FD6, 0xDD8F4F22, 0xA0049EA7, 0xD48E0009,
+0x420BD28E, 0x62D265D2, 0x8BF822E8, 0x0009A004,
+0xD28AD48B, 0x55D1420B, 0x22E852D1, 0xA0048BF8,
+0xD4880009, 0x420BD285, 0x52D255D2, 0x8BF822E8,
+0x0009A004, 0xD281D484, 0x55D3420B, 0x22E852D3,
+0xA0048BF8, 0xD4810009, 0x420BD27C, 0x52D455D4,
+0x8BF822E8, 0x6EF64F26, 0x6DF6000B, 0x2FD62FC6,
+0x4F222FE6, 0x6E636D73, 0x6C53B018, 0x64C357F4,
+0xB05465E3, 0xB06A66D3, 0xB09A0009, 0xB09E0009,
+0xB0A20009, 0xB0BE0009, 0xB0C10009, 0xB1240009,
+0x4F260009, 0x6DF66EF6, 0x6CF6A023, 0x3412D16C,
+0xD66C0529, 0x2650D76C, 0x2742000B, 0x0009A014,
+0x2FD62FC6, 0x4F222FE6, 0x6E636D73, 0x6C53BFEE,
+0x64C357F4, 0xB02A65E3, 0xB10666D3, 0x4F260009,
+0x6DF66EF6, 0x6CF6A005, 0xE603D260, 0x000B4618,
+0xD25E2262, 0x000BE600, 0x4F222262, 0xE40ABF7E,
+0x0009BF7E, 0xE104D25A, 0xE5004118, 0x2212E40A,
+0x2252BF74, 0x6072D757, 0x4F26CB20, 0x2702000B,
+0xD1554F22, 0xE400410B, 0x452BD554, 0x2FE64F26,
+0x6E63D153, 0x44186612, 0x45289210, 0x26294408,
+0x44084500, 0x4400265B, 0x4708264B, 0x47082162,
+0x27EBD14C, 0x000B2172, 0x03F06EF6, 0x2FE61FFF,
+0xDE494F22, 0xE40AE101, 0x2E12BF48, 0x726C62E3,
+0xE401E100, 0x22122212, 0x22122212, 0x22122212,
+0xE7302242, 0xE40AE503, 0x22122212, 0x22122212,
+0x22122212, 0x22122212, 0x22122212, 0x22122212,
+0x22522272, 0x22122212, 0x22122212, 0x22122212,
+0x22122212, 0x121ABF22, 0x2E62E600, 0x000B4F26,
+0xD2326EF6, 0xE441E101, 0x000B2212, 0xD1302242,
+0xE605D430, 0x000B2162, 0xD52F2462, 0x6050D22F,
+0x8B0E8801, 0x6040D42E, 0x8B078801, 0x9626D52D,
+0x88016050, 0x96238B0C, 0x0009A00A, 0xA0079621,
+0xE6000009, 0x2262D426, 0x88016040, 0xE6048B00,
+0xAEF3E40A, 0xD2242262, 0xE40AE601, 0x2262AEEE,
+0x2FC62FB6, 0x2FE62FD6, 0xDC204F22, 0x60C2ED00,
+0xCB01EB64, 0x60C22C02, 0xA041C901, 0x03C46E03,
+0x034003D4, 0x001C3B88, 0x002032C8, 0x002014AA,
+0x002032D0, 0x002032D8, 0x002032E0, 0x002032E8,
+0x0025E720, 0x002034B8, 0x0020318C, 0x001C5968,
+0x001D4004, 0x001C3500, 0x0020124A, 0x00201276,
+0x001C5814, 0x001C59D0, 0x001C5830, 0x001C6268,
+0x001C59A4, 0x001C639C, 0x0020319E, 0x001C5804,
+0x0020319D, 0x0020319F, 0x001C581C, 0x001C5860,
+0x89073DB2, 0xE40A60C2, 0xBE9FC901, 0x7D016E03,
+0x8BF52EE8, 0x8B033DB2, 0xD23ED43D, 0x0009420B,
+0x4F26E40A, 0x6DF66EF6, 0xAE8F6CF6, 0x44116BF6,
+0x604B8F01, 0x000B6043, 0x2FB60009, 0x2FD62FC6,
+0x4F222FE6, 0xDC347FFC, 0x60C2ED00, 0xCB02EB64,
+0x60C22C02, 0xC9022F02, 0x6E03A009, 0x89083DB3,
+0xE40A60C2, 0xC9022F02, 0x6E03BE70, 0x2EE87D01,
+0x3DB38BF4, 0xD4298B08, 0x7F04D226, 0x6EF64F26,
+0x6CF66DF6, 0x6BF6422B, 0x4F267F04, 0x6DF66EF6,
+0x000B6CF6, 0xD5226BF6, 0x60525651, 0x000B4628,
+0x2FB6306C, 0x2FD62FC6, 0x4F222FE6, 0x4F024F12,
+0x6E43BFF1, 0xDC1B6B03, 0xBFECDD1B, 0x30B80009,
+0x060A3C05, 0x46094609, 0x3D654601, 0x4209020A,
+0x42094209, 0x8BF032E2, 0x4F164F06, 0x6EF64F26,
+0x6CF66DF6, 0x6BF6000B, 0x4F222FE6, 0xE102DE0F,
+0xE403E500, 0xBFD42E12, 0xE6062E52, 0xE7004618,
+0x2E62E403, 0x4F262E72, 0x6EF6AFCB, 0x0009000B,
+0x002032F0, 0x0020145E, 0x001C5860, 0x00203308,
+0x001C1040, 0xCCCCCCCD, 0x10624DD3, 0x001D4004,
+0x2F962F86, 0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6,
+0xE5007FD8, 0x6453E110, 0x6C534128, 0x655DEE0A,
+0x46086653, 0x4608365C, 0x361C7501, 0x675D6043,
+0x60C30F66, 0x37E3ED00, 0x816126C1, 0x81638162,
+0x16D316D2, 0x8FEA16D4, 0x68F27404, 0xDAB3D9B2,
+0x29821981, 0xD1B259F1, 0x2A921A91, 0x5BF35AF2,
+0x5EF55DF4, 0x11A154F6, 0x11B321A2, 0x11D511B2,
+0x11E711D4, 0x114911E6, 0x55F71148, 0xEE00DBA9,
+0xDDA957F8, 0xD6A952F9, 0x1B5164E3, 0xDBA82B52,
+0xEAB8D8A8, 0x2D72E945, 0x6AAC2622, 0x6EED4908,
+0x4D086DE3, 0x3DEC61E3, 0x4D084108, 0x3DBC31EC,
+0x410860C3, 0x81D12DC1, 0x4108E050, 0x41084008,
+0x60C381D2, 0xE500318C, 0x81D334A2, 0x1D131DD2,
+0x8D01D494, 0xD4911D54, 0xB08165D3, 0x64ED7E01,
+0x8BDC3492, 0xDB94D18D, 0xD28B6812, 0x1B814829,
+0x2FD26412, 0x2B92694D, 0xD98A6722, 0x1B734729,
+0xD7876822, 0x1BA26A8D, 0xD28C6B72, 0x22B2D586,
+0xE0035D72, 0x5E7412D2, 0x12E44018, 0xD6885176,
+0x54781216, 0x1248E1FF, 0xD4856792, 0x6852127A,
+0x28C1E703, 0x81916952, 0x6A52E050, 0x81A24008,
+0x60C36B52, 0x6D5281B3, 0x6E521DD2, 0x62521E63,
+0x1264E600, 0x46086563, 0x7501364C, 0x665D2612,
+0x8BF83673, 0xE003D471, 0x40186542, 0x674225C1,
+0x8171D274, 0xEE006842, 0x69421882, 0x1923E024,
+0xE5806A42, 0x6B421AE4, 0x81B266E3, 0xD46D6C42,
+0x655C81C3, 0x6D63666D, 0x616D7604, 0x31533D4C,
+0x2DE28FF8, 0xD569D268, 0x74042422, 0x7F282452,
+0x6EF64F26, 0x6CF66DF6, 0x6AF66BF6, 0x000B69F6,
+0x664268F6, 0xC8036061, 0xE5008D04, 0xC9036061,
+0x8B038802, 0x65635262, 0x24125124, 0x6053000B,
+0x2FE62FD6, 0x7FEC4F22, 0x62536E53, 0x6D43E550,
+0x4508E400, 0xE101A001, 0x60435224, 0x81212211,
+0x60538123, 0x56E28122, 0x8BF53620, 0x16E4D250,
+0xE61464F3, 0x65E3420B, 0xE4FC65E1, 0x2E512549,
+0x65F361F1, 0x2F112149, 0xD14954D1, 0xE614410B,
+0x607157D1, 0x2701CB01, 0x7F141DE1, 0x6EF64F26,
+0x6DF6000B, 0x2FE62FD6, 0x7FEC4F22, 0x66536E53,
+0x6D43E5FC, 0x20596061, 0x2601CB01, 0x326052E2,
+0x12E48B06, 0x31E051E2, 0x52D18B04, 0x1E22A002,
+0x5664AFF0, 0x64F3D236, 0x420BE614, 0x67E165E3,
+0x2719E1FC, 0x67F12E71, 0x271954D1, 0x65F3D130,
+0x410BE614, 0x52D12F71, 0xCB016021, 0x1DE12201,
+0x4F267F14, 0x000B6EF6, 0x2FE66DF6, 0x624C4F22,
+0x4208DE1B, 0xA0054200, 0x52523E2C, 0x5624D417,
+0x2E62BF8E, 0x52E165E2, 0x8BF63520, 0x2622D61B,
+0x000B4F26, 0x2FB66EF6, 0x2FD62FC6, 0x4F222FE6,
+0xDB1CDC10, 0x66C252C1, 0x89403620, 0xC9036061,
+0x893C8801, 0xDD18DE0B, 0x64E3BF63, 0x85036503,
+0x620D66B2, 0x892B3262, 0xBF9BD403, 0xD4130009,
+0x00094D0B, 0x0009AFE6, 0x00202D88, 0x00202D90,
+0x00202D98, 0x00202DC0, 0x002031A4, 0x002031AC,
+0x001000C8, 0x00101680, 0x001E2108, 0x001C3D00,
+0x00117880, 0x00117780, 0x00040020, 0x0026C401,
+0x00200B26, 0x00203188, 0x0020145E, 0x00203324,
+0x64E3BF3E, 0x4D0BD406, 0xAFBB0009, 0xD2050009,
+0x4F262262, 0x6DF66EF6, 0x000B6CF6, 0x00006BF6,
+0x00203328, 0x001C3D28, 0x2FC62FB6, 0x2FE62FD6,
+0x7FFC4F22, 0x6022D22B, 0x8D41C803, 0xDE2A2F01,
+0xDB2BDC2A, 0xED01A017, 0xC9036051, 0x89168801,
+0xD128D426, 0x0009410B, 0x61035503, 0xC8208551,
+0xE0508903, 0x720102BE, 0xD2230B26, 0x420B64E3,
+0xD6226513, 0x52C126D2, 0x352065C2, 0xDE208BE4,
+0xDB21DD20, 0x52D1DC21, 0x352065D2, 0x60518918,
+0x8801C903, 0xD41B8914, 0x460BD616, 0x57030009,
+0x8F0437E0, 0xE2016503, 0xAFEC2B20, 0xD4182C52,
+0x420BD218, 0xD6110009, 0x4118E101, 0x2612AFE3,
+0xC80460F1, 0xD2148907, 0x4F267F04, 0x6DF66EF6,
+0x422B6CF6, 0x7F046BF6, 0x6EF64F26, 0x6CF66DF6,
+0x6BF6000B, 0x001E2100, 0x00202D98, 0x00202D90,
+0x00202D2C, 0x00201162, 0x002011E4, 0x001C3D30,
+0x00117880, 0x00202D88, 0x002031A8, 0x002031A4,
+0x00202DC0, 0x00201180, 0x00200308, 0xE601D203,
+0x1265D503, 0x000B2252, 0x00001266, 0x001C1010,
+0x0000C34F, 0x0009000B, 0x2FD62FC6, 0x4F222FE6,
+0x6D436C53, 0xEE00A004, 0x7E0164D4, 0x644CBFF2,
+0x8BF93EC2, 0x6EF64F26, 0x000B6DF6, 0xE5006CF6,
+0x6643A002, 0x76017501, 0x22286260, 0xAFE38BFA,
+0x2FE60009, 0x75076253, 0xE1086753, 0x6043EE0A,
+0x4409C90F, 0x650330E2, 0x8D014409, 0xE630E637,
+0x4110365C, 0x8FF22760, 0xE00077FF, 0x000B8028,
+0x000B6EF6, 0x000BE000, 0x2FE6E000, 0x7FEC4F22,
+0x6E436253, 0xBFDC65F3, 0xBFD06423, 0xBFCE64E3,
+0xD40364F3, 0x0009BFCB, 0x4F267F14, 0x6EF6000B,
+0x0020332C, 0xE4FDD29A, 0xD79A6122, 0x22122149,
+0x74016022, 0x2202CB01, 0xD5976622, 0x22622649,
+0xC8406070, 0x60528902, 0x2502CB04, 0xE1F76452,
+0x25422419, 0xE7016052, 0x2502C9CF, 0xE6026052,
+0x2502CB03, 0x15624718, 0x1573000B, 0xD78CD58B,
+0xD48DD28C, 0xE600E100, 0x27112511, 0xAFD12210,
+0x664C2461, 0x4600D289, 0x6060362C, 0x000BCB10,
+0x654C2600, 0x4500D285, 0x6650352C, 0x2619E1EF,
+0x2560000B, 0xD282664C, 0x362C4600, 0xCB106060,
+0x2600000B, 0xD27E654C, 0x352C4500, 0xE1EF6650,
+0x000B2619, 0x664C2560, 0x4600D278, 0x6060362C,
+0x000BCB08, 0x654C2600, 0x4500D274, 0x6650352C,
+0x2619E1F7, 0x2560000B, 0xD271664C, 0x362C4600,
+0xCB086060, 0x2600000B, 0xD26D654C, 0x352C4500,
+0xE1F76650, 0x000B2619, 0x624C2560, 0x4200D667,
+0x6020326C, 0x4021C908, 0x40214021, 0x600C000B,
+0xD663624C, 0x326C4200, 0xC9086020, 0x40214021,
+0x000B4021, 0xD15F600C, 0x341C644C, 0x000B6240,
+0xD15D602C, 0x341C644C, 0x000B6240, 0x2FE6602C,
+0x6E434F22, 0xE60A645C, 0x89143467, 0x0009BFEB,
+0x60EC640C, 0x8B028801, 0xA002E00F, 0x44092409,
+0x624C4409, 0x3263E60A, 0xBFE28905, 0x620C644C,
+0xC8806023, 0xE2008B00, 0x4F266023, 0x6EF6000B,
+0xD64A4F22, 0x88016062, 0xB2458B03, 0xA0030009,
+0xD2470009, 0x2260E640, 0xE200D646, 0x000B4F26,
+0x4F222622, 0x6062D641, 0x8B018802, 0x0009B28E,
+0xE200D640, 0x000B4F26, 0xD53C2622, 0xE100D43C,
+0x2512E701, 0x2470000B, 0xE604D239, 0x2260000B,
+0xD4394F22, 0x410BD139, 0xD5390009, 0x6650E1FD,
+0x2619D238, 0x2560E700, 0x000B4F26, 0x4F222270,
+0xD132D435, 0x0009410B, 0xE7FBD531, 0x26796650,
+0x000B4F26, 0x4F222560, 0xD12CD430, 0x0009410B,
+0xE7F7D52B, 0x26796650, 0x000B4F26, 0xD5282560,
+0x6250942D, 0x000B2249, 0xD5252520, 0x6250E4BF,
+0x000B2249, 0x4F222520, 0x8522D225, 0x2008600D,
+0x88018911, 0x88038913, 0x88058915, 0x88068942,
+0x88088948, 0x8809894E, 0x880A8954, 0x880B895A,
+0xA0678960, 0xB0690009, 0xA0640009, 0xB077600C,
+0xA0600009, 0xB080600C, 0xA05C0009, 0xFF7F600C,
+0x001E2148, 0x001E1000, 0x001E1108, 0x002031FC,
+0x002031FE, 0x0020321D, 0x002031E0, 0x001E103F,
+0x001E105F, 0x001E102F, 0x001E1090, 0x00203204,
+0x001E100B, 0x00203200, 0x00203330, 0x0020145E,
+0x001E1028, 0x0020321C, 0x0020333C, 0x0020334C,
+0x002031D4, 0x6260D684, 0x8B2B2228, 0x0009B061,
+0x600CA029, 0x6260D680, 0x8B232228, 0x0009B069,
+0x600CA021, 0x6260D67C, 0x8B1B2228, 0x0009B0C7,
+0x600CA019, 0x6260D678, 0x8B132228, 0x0009B0CD,
+0x600CA011, 0x6260D674, 0x8B0B2228, 0x0009B125,
+0x600CA009, 0x6260D670, 0x8B032228, 0x0009B13D,
+0x600CA001, 0x4F26E000, 0x0009000B, 0xD26CD16B,
+0xD56C8412, 0x4000C90F, 0xD76B012D, 0xE403D66B,
+0xE20F611C, 0x2540E001, 0x25202712, 0x2602000B,
+0xE601D262, 0x30668523, 0xE0008D05, 0xD663D260,
+0xE0018122, 0x000B2602, 0xD25C0009, 0x600D8523,
+0x89052008, 0x8B0A8801, 0x6060D65D, 0x2600CB01,
+0xD457D65A, 0xE001E101, 0x000B2612, 0x000B8142,
+0xD152E000, 0x8513E501, 0x640D4518, 0x66033453,
+0xE0008D05, 0xD551D253, 0x2260E001, 0x000B2502,
+0x4F220009, 0x8513D149, 0x6453650D, 0x62494419,
+0x227D672E, 0x8801602C, 0x88028909, 0x88038910,
+0x8806891A, 0x88078935, 0xA04C893B, 0xD5460009,
+0x6652D746, 0x2762D446, 0x622C6261, 0x2421A038,
+0x2228625C, 0xD4438B3F, 0x6642D540, 0x2562D440,
+0x24018561, 0x6203A02C, 0x2008605C, 0x88108907,
+0x88208908, 0x88308909, 0xA02C890A, 0xD23A0009,
+0x6222A008, 0xA005D239, 0xD2396222, 0x6222A002,
+0x6262D638, 0xD432D531, 0x66212522, 0xA00F626C,
+0xD6352421, 0x6261D52D, 0x622CD42D, 0xA0072562,
+0xD6322421, 0x8561D529, 0x2562D429, 0x62032401,
+0x662D8515, 0x3617610D, 0x65038F01, 0xB0CB2451,
+0xA0010009, 0xE000E001, 0x000B4F26, 0xD6190009,
+0xD427E101, 0x65412610, 0xD118D717, 0xE20F655D,
+0x2752E001, 0x000B2620, 0x2FE62102, 0xD20F4F22,
+0x640C8523, 0x8B082448, 0xD511D61D, 0x2621E200,
+0x940F8451, 0xA0482049, 0xDE0D8051, 0xC84060E0,
+0xE2018D32, 0x89443427, 0xD216D615, 0x2641420B,
+0x0009A030, 0x0000FF7F, 0x0020321D, 0x002031D4,
+0x002031E0, 0x001E1100, 0x001E100C, 0x00203200,
+0x001E1000, 0x001E1001, 0x00203208, 0x002031E8,
+0x002031EC, 0x002031F0, 0x0020320C, 0x00203210,
+0x00203214, 0x00203218, 0x0020351C, 0x00203526,
+0x002031FA, 0x00202362, 0x89123427, 0xD294D693,
+0x2641420B, 0xCB8084E1, 0x80E1B0F5, 0xD69160E0,
+0x2E00CB04, 0xC93F6060, 0xD68F2600, 0xA001E001,
+0xE0002602, 0x000B4F26, 0xD68C6EF6, 0xC8806060,
+0xD2868919, 0x88016021, 0xD2898B15, 0x8524E501,
+0x89103056, 0xE203D187, 0x2120D487, 0xE00B6541,
+0x0656655D, 0xE40FD585, 0x2140E702, 0xD77E2571,
+0x000BE001, 0x000B2702, 0x2FE6E000, 0xDE804F22,
+0xC88084E1, 0xD57A892C, 0x20088554, 0x61038F28,
+0x8553D77C, 0x64036672, 0x8566650C, 0x3520620C,
+0xD6798B1E, 0x651CD774, 0x2651644C, 0x60E02741,
+0x8904C840, 0x420BD275, 0xA0030009, 0xD2680009,
+0x0009420B, 0x0009B09F, 0xE201D167, 0x60E02122,
+0xCB04D464, 0x60402E00, 0x2400C93F, 0x6023A001,
+0x4F26E000, 0x6EF6000B, 0x2FB62FA6, 0x2FD62FC6,
+0xDA622FE6, 0x66A1E240, 0x3622DC5E, 0x62638900,
+0x6ED36D2C, 0x4E2136D8, 0x4E212A61, 0xDB61D460,
+0xE700A00F, 0x770162B2, 0x71026123, 0x66212B12,
+0x71026213, 0x61212B12, 0x651D666D, 0x356C4528,
+0x627C2452, 0x8BED32E3, 0xC90360D3, 0x8B108803,
+0x617367B2, 0x2B127102, 0x71026E13, 0x2B126571,
+0x655D6DE1, 0x422862DD, 0x325CE107, 0xA00C2C10,
+0x88022422, 0xA0038B01, 0x8801E203, 0xE2018B05,
+0x66B22C20, 0x655D6561, 0xE60F2452, 0x67A12C60,
+0x8B052778, 0xDD38DC44, 0xEB01EA00, 0x2DB22CA2,
+0x6DF66EF6, 0x6BF66CF6, 0x6AF6000B, 0x2FE62FD6,
+0xE240DD36, 0x362266D1, 0x62638900, 0x3678672C,
+0x7703DE38, 0x47212D61, 0x64E2D635, 0xA00E4721,
+0x6562E100, 0x62537101, 0x74012450, 0x24204219,
+0x45297401, 0x74012450, 0x24504519, 0x621C7401,
+0x8BEE3273, 0x66E24200, 0x420061D1, 0x2118362C,
+0x2E628F06, 0xDD1CD728, 0xE501E400, 0x2D522742,
+0x000B6EF6, 0x2FD66DF6, 0x4F222FE6, 0xED0AEE01,
+0x64E3BC97, 0xBC9C64E3, 0x62EC7E01, 0x8BF732D7,
+0xBC9FEE01, 0x64E364E3, 0x7E01BCA4, 0x32D762EC,
+0x4F268BF7, 0x000B6EF6, 0xD1186DF6, 0xD418920D,
+0x72122122, 0x2422D617, 0xD7177204, 0x72202622,
+0x2722D116, 0x000B7230, 0x137A2122, 0x002031FA,
+0x0020246E, 0x001E1015, 0x00203200, 0x001E1001,
+0x002031D4, 0x001E1100, 0x002031FE, 0x002031EC,
+0x001E1000, 0x002031F0, 0x002031FC, 0x00202362,
+0x001E100C, 0x002031E8, 0x00203204, 0x00203208,
+0x0020320C, 0x00203210, 0x00203214, 0x00203218,
+0x4F222FE6, 0xD6507FFC, 0x88016060, 0xE2018951,
+0x2620BFBB, 0xD54ED14D, 0xDE4E6010, 0x64E36552,
+0x7402C840, 0x8D22D14C, 0xD24C7502, 0xE601D74C,
+0xE7042722, 0x76016255, 0x626C2421, 0x8FF93273,
+0xD4437402, 0x6242E601, 0x640D8528, 0x67494419,
+0x275D657E, 0x81E4607C, 0xE417D542, 0x67557601,
+0x3243626C, 0x8FF92171, 0xA0207102, 0xD23E0009,
+0xE601D73B, 0xE7042722, 0x76016255, 0x626C2421,
+0x8FF93273, 0xD4327402, 0x6242E601, 0x640D8528,
+0x67494419, 0x275D657E, 0x81E4607C, 0xE417D533,
+0x67557601, 0x3243626C, 0x8FF92171, 0x924A7102,
+0xD2262E21, 0x5E23D72E, 0x64F22FE2, 0x604365F2,
+0x2700C980, 0xC9606043, 0x80716103, 0xC9036043,
+0x80724519, 0x65F2605C, 0x817266F2, 0x46194629,
+0x606C4529, 0x4018645C, 0x8173304C, 0x21185E23,
+0x64F22FE2, 0x6E4C62F2, 0x602C4219, 0x66F262F2,
+0x46294018, 0x461930EC, 0x42298174, 0x652C606C,
+0x305C4018, 0x81758F07, 0x0009BC9D, 0x2228620C,
+0xA00A8908, 0x60130009, 0x8B038840, 0x0009B009,
+0x0009A003, 0xE202D60F, 0x7F042622, 0x000B4F26,
+0x000B6EF6, 0x060A0009, 0x0020321C, 0x001E1000,
+0x00203208, 0x0020351C, 0x00203528, 0x002034C0,
+0x002031F0, 0x002034F0, 0x002034EE, 0x002034C2,
+0x002031D4, 0x00203200, 0x4F222FE6, 0xDE937FFC,
+0x200884E9, 0x2F008D06, 0xD692D491, 0x0009460B,
+0x64F0B194, 0x6620D290, 0x89022668, 0xC9BF60E0,
+0x7F042E00, 0x000B4F26, 0x000B6EF6, 0x2FE60009,
+0xDE8A4F22, 0x60E0D68A, 0xCBC0D48A, 0x62602E00,
+0xC803602C, 0x40218904, 0x70014021, 0x6603A002,
+0x66034009, 0xD684616D, 0xE500A004, 0x75016262,
+0x74042422, 0x3213625D, 0xD2808BF8, 0x0009420B,
+0xC9BF84E2, 0x4F2680E2, 0x6EF6000B, 0x2FE62FD6,
+0x7FFC4F22, 0x6260D67A, 0x89442228, 0xD56FE100,
+0x60502610, 0xCB40D477, 0x2500440B, 0x8D052008,
+0x62E06E03, 0x7104612C, 0x2F11A006, 0xD472D66A,
+0xDD726760, 0x657C4D0B, 0xE23C6D1D, 0x8B033D27,
+0xD264D46F, 0x0009420B, 0x4D214D21, 0xA005D76D,
+0x66E6E400, 0x357C4508, 0x74012562, 0x35D3654D,
+0xD7698BF7, 0x6172E003, 0x81114018, 0x6E7260F1,
+0x81E2700C, 0xD4656172, 0xDD658113, 0x4D0BDE65,
+0xE2016572, 0xD4642E22, 0x420BD252, 0xD6530009,
+0xC93F6060, 0x7F042600, 0x6EF64F26, 0x6DF6000B,
+0x2FC62FB6, 0x2FE62FD6, 0xD25C4F22, 0x6B436E73,
+0x420B6C53, 0x20086D63, 0x61038F08, 0xD245D458,
+0x6EF64F26, 0x6CF66DF6, 0x6BF6422B, 0x21B060C3,
+0x60D38011, 0xE5008111, 0x64BCA007, 0x6053655D,
+0x665300EC, 0x7501361C, 0x625D8064, 0x8BF53243,
+0x6060D636, 0x2600C9BF, 0x6EF64F26, 0x6CF66DF6,
+0x6BF6000B, 0x7FC44F22, 0x720262F3, 0x22512F41,
+0x45297202, 0x60632251, 0xE5C4E682, 0x67F38121,
+0x655C666C, 0xE408BFBC, 0x4F267F3C, 0x0009000B,
+0x2F962F86, 0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6,
+0xE1007FC4, 0x6513ECFF, 0x6B136CCD, 0xDE36D735,
+0xEDFF64F3, 0xD835EA04, 0x6053655C, 0x027D4000,
+0x32C0622D, 0x66038D0D, 0x09ED6063, 0x2491027D,
+0x24217402, 0x698202ED, 0x3928622D, 0x74022892,
+0x75017104, 0x6063625C, 0x07D532A2, 0x0EB58FE4,
+0x2448641C, 0xE6808905, 0x67F3E5C5, 0xBF7F666C,
+0x7F3C655C, 0x6EF64F26, 0x6CF66DF6, 0x6AF66BF6,
+0x000B69F6, 0xD11E68F6, 0x6012D21E, 0xCB20E405,
+0x2102E500, 0x000B2242, 0x00002252, 0x001E1017,
+0x00203358, 0x0020145E, 0x002031A2, 0x001E1015,
+0x001E10BF, 0x00117800, 0x001E10FC, 0x00200308,
+0x002031A8, 0x002025C6, 0x0020335C, 0x002014AA,
+0x00203378, 0x0011788C, 0x002031A4, 0x00202D88,
+0x002011E4, 0x001E2130, 0x00203380, 0x00202588,
+0x00203384, 0x002031BC, 0x002031C4, 0x002034BC,
+0x001C3500, 0x001D4004, 0xD565D164, 0xE400D765,
+0x2142E20F, 0x17411154, 0xD5632722, 0x9669D763,
+0x15412572, 0x96661562, 0xE6011565, 0xD5601165,
+0x666CE6F8, 0x25422542, 0x25422542, 0x25422542,
+0x25622542, 0x7601E727, 0x67632572, 0x25627797,
+0xE7042572, 0x2572E248, 0xE2192522, 0xE2702522,
+0x25422542, 0x25422542, 0x25222542, 0x2522E20C,
+0x25422542, 0x25422542, 0x25422542, 0x25422542,
+0x000B154A, 0xE2081145, 0x0009422B, 0x2FE62FD6,
+0x7FFC4F22, 0xC8206043, 0x6E438D02, 0x0009BE6D,
+0xC81060E3, 0xBE6A8901, 0x60E30009, 0x8901C840,
+0x0009BE8C, 0xC80160E3, 0xDD3E8938, 0xC80260D0,
+0x2F008D03, 0x460BD63C, 0x60F00009, 0x8902C804,
+0x460BD63A, 0x62F00009, 0xC8806023, 0x60D08902,
+0x2D00C97F, 0xC8016023, 0xD6358906, 0x0009460B,
+0x0009A007, 0x51630601, 0x8902C808, 0x460BD631,
+0x60F00009, 0x8902C810, 0x420BD22F, 0xD52F0009,
+0x88026052, 0xD22E8B03, 0xA005E604, 0x88012260,
+0xD22B8B02, 0x2260E601, 0x2522E200, 0xC88060E3,
+0xD628892E, 0x60E36E60, 0x8902C880, 0x420BD226,
+0x60E30009, 0x8902C840, 0x420BD224, 0x60E30009,
+0x8902C802, 0x420BD222, 0x60E30009, 0x890EC804,
+0x410BD120, 0xBF0E0009, 0xBF4D0009, 0xD51E0009,
+0x6050D41E, 0xC908D71E, 0xBF842500, 0x60E32472,
+0x8905C808, 0x7F04D21B, 0x6EF64F26, 0x6DF6422B,
+0x4F267F04, 0x000B6EF6, 0x00006DF6, 0x001C581C,
+0xA000A000, 0x001D0100, 0x001D4000, 0x00040021,
+0x001C589C, 0x001E1021, 0x00201640, 0x00201662,
+0x00201CA0, 0x0020167A, 0x00201688, 0x00203200,
+0x001E100B, 0x001E1028, 0x002016DE, 0x002016EA,
+0x00201690, 0x002016AE, 0x001E1000, 0x0010F100,
+0x12345678, 0x002016C6, 0x644CD6A7, 0x000B346C,
+0xD6A62450, 0x346C644C, 0x2450000B, 0x644CD6A4,
+0x000B346C, 0x625C2450, 0x4208616D, 0x42084119,
+0x42006019, 0x670E614C, 0xD49E321C, 0x4200207D,
+0x324CC90F, 0x2200000B, 0x4208625C, 0x42004208,
+0x324C644C, 0x4200D498, 0x000B324C, 0x2FE62260,
+0x614C4F12, 0x4100D493, 0x6710314C, 0xE29F666D,
+0x27294619, 0x6E536269, 0x672E6573, 0x4221227D,
+0x42214221, 0x7601662C, 0xE4014608, 0x34E84608,
+0x644C4600, 0x071A0467, 0x2150257B, 0x000B4F16,
+0x4F226EF6, 0xD2857FE8, 0x88016021, 0xD2848B7B,
+0x26686621, 0xD2838B77, 0x26686621, 0xE50F8B73,
+0xE401BFA2, 0xBFA4E501, 0xE586E400, 0xE400655C,
+0x2F50BFA4, 0xBFA1E401, 0xE602E506, 0x60634618,
+0x81F2E401, 0x6543BF9F, 0xE40185F2, 0xBFAB6543,
+0x85F26603, 0x6543E401, 0x6603BFB1, 0xE40265F0,
+0x6053756C, 0x80F8BF80, 0xBF82E402, 0x84F8E512,
+0x7090E402, 0x6503BF82, 0x4618E602, 0x81F66063,
+0xBF80E402, 0x85F6E500, 0x6603E402, 0xE500BF8C,
+0xE40285F6, 0xBF926603, 0xE5FEE500, 0xE010655C,
+0xBF61E403, 0xE5130F54, 0xE40EBF63, 0x05FCE010,
+0xBF63E40E, 0xE5007585, 0xBF64E403, 0xE500E640,
+0xBF71E403, 0xE500E640, 0xBF78E403, 0xE5FFE640,
+0xE014655C, 0xBF47E404, 0xE40F0F54, 0xE504BF49,
+0x05FCE014, 0xBF49E40F, 0xE5017584, 0xBF4AE640,
+0xE501E404, 0xBF57E640, 0xE501E404, 0xE404E640,
+0xAF5C7F18, 0x7F184F26, 0x000B4F26, 0x4F220009,
+0xD2427FF0, 0x88016021, 0xD2418B71, 0x26686621,
+0xD2408B6D, 0x26686621, 0xE50F8B69, 0xE401BF1C,
+0xBF1EE501, 0xE586E400, 0xE400655C, 0x2F50BF1E,
+0xBF1BE401, 0xE401E506, 0xBF1C6543, 0xE401E640,
+0xBF296543, 0xE401E640, 0xBF306543, 0x65F0E640,
+0x756CE402, 0xBEFF6053, 0xE40280F4, 0xE512BF01,
+0xE40284F4, 0xBF017090, 0xE6406503, 0xBF02E402,
+0xE640E500, 0xBF0FE402, 0xE640E500, 0xBF16E402,
+0xE5FEE500, 0x6053655C, 0xBEE5E403, 0xE51380F8,
+0xE40EBEE7, 0xE40E84F8, 0xBEE77085, 0xE5006503,
+0xBEE8E640, 0xE500E403, 0xBEF5E640, 0xE500E403,
+0xBEFCE640, 0xE5FFE403, 0x6053655C, 0xBECBE404,
+0xE40F80FC, 0xE504BECD, 0xE40F84FC, 0xBECD7083,
+0xE5016503, 0xBECEE640, 0xE501E404, 0xBEDBE640,
+0xE501E404, 0xE404E640, 0xAEE07F10, 0x7F104F26,
+0x000B4F26, 0x00000009, 0x001E102F, 0x001E1080,
+0x001E1090, 0x001E103F, 0x001E103E, 0x002031FA,
+0x002031FC, 0x002031FE, 0xD21DD11C, 0x66206010,
+0x676C7001, 0x3700C90F, 0xE5008D13, 0x67106210,
+0x7701622C, 0x64232170, 0xD6166010, 0x44084408,
+0x3428C90F, 0x62602100, 0x7201D513, 0x44082620,
+0x000B354C, 0xD10F6053, 0x25586510, 0xE6008D13,
+0xD60DD40B, 0x655C6540, 0x47086753, 0x37584708,
+0x47086540, 0x24507501, 0x367C6040, 0x2400C90F,
+0x72FF6210, 0x000B2120, 0x00006063, 0x002031A1,
+0x002031A0, 0x002031A2, 0x00202DC8, 0x7FFC4F22,
+0xE680D19D, 0x666C6212, 0xD29C2F22, 0x67F36563,
+0x420B7542, 0x7F04E404, 0x000B4F26, 0xE6800009,
+0xD296666C, 0xE7006563, 0x422B7540, 0xE6806473,
+0xD292666C, 0xE7006563, 0x422B7543, 0x2F866473,
+0x2FA62F96, 0x2FC62FB6, 0x2FE62FD6, 0x7FF44F22,
+0xDD8CD28B, 0x72011F21, 0xDB8B1F22, 0x6AF2E840,
+0x5211D18A, 0x36206612, 0xA0A78B01, 0x60610009,
+0x8801C903, 0xA0A18B01, 0xD9840009, 0x420BD284,
+0x55036493, 0x845C6A03, 0x30E0EE84, 0xD1818B79,
+0x606C6610, 0x8B3D8801, 0x6210D17F, 0x892F2228,
+0xD57EE701, 0x64522B72, 0x1442E003, 0xD57C6252,
+0xE6004018, 0x21608121, 0xD17A6453, 0x6E527404,
+0x60126742, 0xCB20DC78, 0x76012102, 0x3283626D,
+0x25E28BFB, 0x2472DE71, 0x62E267C2, 0x1274D173,
+0x604164E2, 0x2401CB01, 0xEE0066E2, 0xDC702C62,
+0xEC012C62, 0x2DC2410B, 0x4C18EC01, 0x2BE22DC2,
+0xD764DE6C, 0xD16C60E2, 0xCB01E202, 0x27202E02,
+0x2122A02F, 0x8B2C2008, 0xE701DE68, 0xD466EC00,
+0x2170D264, 0xEE012EC2, 0x612224E2, 0x2169E6FE,
+0xE01E2212, 0x54F10C5C, 0x24C0E01F, 0x56F2025C,
+0x26207510, 0xD75EE600, 0xEE06D45E, 0x76018456,
+0x6C542700, 0x31E3616C, 0x740124C0, 0x77018FF6,
+0xE494D259, 0x72012240, 0x2250E500, 0xE605720F,
+0xD2562260, 0x65A36493, 0xEE01420B, 0xAF6F4E18,
+0x2FA22DE2, 0xD45265F2, 0x66428553, 0x3262620D,
+0xD4508907, 0x410BD150, 0xD7500009, 0xAF57E601,
+0xD43A2762, 0xDD37D149, 0x65F2410B, 0xD44CEE01,
+0x4E18D64C, 0x2DE2460B, 0x0009AF4A, 0x7F0C2FA2,
+0x6EF64F26, 0x6CF66DF6, 0x6AF66BF6, 0x000B69F6,
+0x4F2268F6, 0x85467FF4, 0x2F01E681, 0x666C8547,
+0x854881F1, 0x81F2D225, 0x67F38542, 0x854381F3,
+0x81F4E40C, 0x65636053, 0x420B81F5, 0x7F0C7540,
+0x000B4F26, 0x2F860009, 0x2FA62F96, 0x2FC62FB6,
+0x2FE62FD6, 0x7FEC4F22, 0xE800D11A, 0xD4322F12,
+0x1F416183, 0x6A13DB20, 0x4A08D630, 0xDE20E001,
+0x4A00460B, 0x1F023AEC, 0x52B166B2, 0x8B013620,
+0x0009A19B, 0xC9036061, 0x8B018801, 0x0009A195,
+0xDE275263, 0x8B4F32E0, 0x420BD20D, 0xDE2564B3,
+0xD70DD50E, 0xED01DC0B, 0x2E02E100, 0x27D02502,
+0xAFE12C10, 0x00002E16, 0x001C3D9C, 0x00201F40,
+0x0011779A, 0x001C3D30, 0x001D0104, 0x00202DC0,
+0x00201162, 0x002031B1, 0x002031B0, 0x002031AC,
+0x001C3B9C, 0x001C3500, 0x00202D98, 0x00201276,
+0x001C3D00, 0x001C36F8, 0x00117708, 0x002031B4,
+0x0011778C, 0x00117792, 0x00117788, 0x00201180,
+0x00203188, 0x00202D88, 0x002011E4, 0x001E2130,
+0x0020349C, 0x0020145E, 0x002034A8, 0x00202C80,
+0x00117780, 0x0011770C, 0xC8018561, 0x5C63897A,
+0x660385C2, 0x6403C903, 0x650D85C3, 0x40216053,
+0xC93F4021, 0x6E034500, 0x252D322A, 0xE2106053,
+0x3E23C901, 0x6D038D23, 0x4408D79D, 0x44086570,
+0x440062E3, 0x25584200, 0x342C8F0F, 0x6043D299,
+0x697D072D, 0x60994919, 0x201D610E, 0x60D381F6,
+0x8F0C8801, 0xA00A697C, 0xD29369E3, 0x052D6043,
+0x4219625D, 0x670E6029, 0x81F6207D, 0xD18F695C,
+0x22286210, 0xE9FF8901, 0xEEFF699C, 0x6EEC659D,
+0x8B0F35E0, 0x4C0BDC8A, 0x540364B3, 0xBF20E502,
+0xD4886E03, 0x410BD188, 0xD78865E3, 0xD488ED01,
+0x27D2A01E, 0x26E9EEFC, 0x81C26063, 0x97C585C3,
+0x62032079, 0x450885F6, 0x6063260B, 0x81C2252B,
+0x81C36053, 0xE10885C4, 0x201B4118, 0x62B281C4,
+0x20E98521, 0x64B28121, 0xCB016041, 0xD4792401,
+0x450BD579, 0x60B20009, 0x57F266F2, 0x2A02CB01,
+0x2672AF22, 0xD26E8561, 0x8F02C802, 0xA09F64B3,
+0x420B0009, 0xDC710009, 0x5E036503, 0x07CEE04C,
+0x7701DD6F, 0x6CD20C76, 0x7C01D664, 0x6D602DC2,
+0x89062DD8, 0xD264D463, 0xED01420B, 0xA07ED763,
+0x625127D2, 0x4118E10F, 0x2219E402, 0x32404418,
+0x85518B11, 0x20D9EDFC, 0x60518151, 0xCB017DE3,
+0x85E12501, 0x20D9D65F, 0x460B81E1, 0x6CF264B3,
+0xA06457F2, 0x6D512C72, 0x4D196DDD, 0x66DE6DD9,
+0x7D012D6D, 0x610360DC, 0x88014118, 0x25118F45,
+0x6462D653, 0x26427401, 0x660D85E3, 0x40216063,
+0xC93F4021, 0x6D034600, 0x262D322A, 0xC8016063,
+0xDC4ED14D, 0x964A8901, 0xE6002D6B, 0x0F64E010,
+0xE01064DD, 0x607C07FC, 0x021D4000, 0x3240622D,
+0x66038D12, 0x021D6063, 0x3270E7FF, 0xA00B8B01,
+0xE01001D5, 0xE60402FC, 0x0F247201, 0x3262622C,
+0x06FC8BE7, 0x4600666C, 0x01CD6063, 0x0C157101,
+0x6711D13B, 0x3C406C7D, 0x62118907, 0x88FF602D,
+0x21D18903, 0xE201DD37, 0x85512D20, 0x20D9EDFC,
+0x60518151, 0xCB01D22F, 0x420B64B3, 0xE0102501,
+0xD43102FC, 0xE001612C, 0x67F2440B, 0x85EF2702,
+0x54F1D22E, 0x650D420B, 0x0009AE7E, 0x80007E03,
+0x0009420B, 0x6E035403, 0xED088544, 0x20D94D18,
+0x8B0330D0, 0xE501BE3D, 0x0009A007, 0xDD248541,
+0x22D8620D, 0xBE348901, 0xD412E500, 0x420BD212,
+0xD71265E3, 0xAE5FED01, 0x780127D2, 0xEE04618D,
+0x8D0231E7, 0xAE4E7B08, 0x7F140009, 0x6EF64F26,
+0x6CF66DF6, 0x6AF66BF6, 0x000B69F6, 0x000068F6,
+0x002034B8, 0x0020339C, 0x0020341C, 0x0020319C,
+0x00201162, 0x00202D90, 0x00201180, 0x001E212C,
+0x002034A0, 0x002034A4, 0x0020145E, 0x00202D2C,
+0x002034BC, 0x002011E4, 0x002031BC, 0x002031C4,
+0x002031B8, 0x002031BA, 0x00202C80, 0x002014AA,
+0x00008000, 0x4F222FE6, 0x6E22D212, 0xC84060E3,
+0x22E28D02, 0x0009BCFA, 0x4218E240, 0x89012E28,
+0x0009BD05, 0xC81060E3, 0xD40B8905, 0x420BD20B,
+0xBD040009, 0x60E30009, 0x8901C805, 0x0009BDEB,
+0xC80260E3, 0x4F268902, 0x6EF6AD01, 0x000B4F26,
+0x00006EF6, 0x001C3510, 0x002034B0, 0x0020145E,
+0x080A0C0E, 0x00020406, 0x1A1C1E20, 0x12141618,
+0x2E303234, 0x26282A2C, 0x3A3C3E40, 0x6C625648,
+0x41112F26, 0xE2208F18, 0x890B3123, 0x321CD204,
+0xD1026220, 0x412B312C, 0x00090009, 0x00202CAA,
+0x00202C60, 0x000BE000, 0x400062F6, 0x40004000,
+0x40004000, 0x40004000, 0x62F6000B, 0x40004000,
+0x40004000, 0x40004000, 0x40184000, 0x62F6000B,
+0x40004000, 0x40004000, 0x40004000, 0x40284000,
+0x62F6000B, 0x40004000, 0x40184000, 0x000B4028,
+0xC90F62F6, 0x40054005, 0x40054005, 0x62F6000B,
+0x4005C907, 0x40054005, 0x62F6000B, 0x4005C903,
+0x000B4005, 0xC90162F6, 0x000B4005, 0x000062F6,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x544F0D0A,
+0x46205355, 0x00003A57, 0x206C754A, 0x32203532,
+0x20373030, 0x313A3132, 0x37323A32, 0x00000000,
+0x00000D0A, 0x00000043, 0x42707372, 0x3D206675,
+0x554E203D, 0x202C4C4C, 0x6E49677A, 0x4E497274,
+0x6D754E51, 0x0000003D, 0x61766E49, 0x2064696C,
+0x72657375, 0x20726F20, 0x2079656B, 0x00214449,
+0x52504545, 0x57204D4F, 0x65746972, 0x6461202C,
+0x003D7264, 0x6C617620, 0x0000003D, 0x00000A0D,
+0x6E6B6E55, 0x206E776F, 0x6D6D6F63, 0x3D646E61,
+0x00000000, 0x000A0D52, 0x203A3051, 0x00000020,
+0x203A3151, 0x00000020, 0x203A3251, 0x00000020,
+0x203A3351, 0x00000020, 0x203A3451, 0x00000020,
+0x61437748, 0x7262696C, 0x6F697461, 0x6620206E,
+0x0A6C6961, 0x0000000D, 0x73696F4E, 0x61432065,
+0x7262696C, 0x6F697461, 0x6166206E, 0x21216C69,
+0x00000D0A, 0x00000072, 0x00205220, 0x00000D0A,
+0x62735576, 0x7473725F, 0x00000A0D, 0x62735576,
+0x7375735F, 0x646E6570, 0x00000A0D, 0x62735576,
+0x7365725F, 0x000A0D6D, 0x00000042, 0x72746E49,
+0x6D652051, 0x2C797470, 0x49677A20, 0x4972746E,
+0x754E514E, 0x00003D6D, 0x654C7245, 0x0000006E,
+0x00000049, 0x20746F4E, 0x756F6E65, 0x49206867,
+0x4220514E, 0x0A0D6675, 0x00000000, 0x000000FF,
+0x00020001, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x010E010D,
+0x00020003, 0x01090108, 0x0002010A, 0x02000003,
+0x02020201, 0x02040203, 0x02060205, 0x02020200,
+0x02040203, 0x020C0207, 0x020E020D, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x010E010D,
+0x00FF010F, 0x01090108, 0x010B010A, 0x020000FF,
+0x02020201, 0x02040203, 0x02060205, 0x02020200,
+0x02040203, 0x020C020B, 0x020E020D, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00205220,
+0x00000046, 0x00000059, 0x73204142, 0x003D7165,
+0x49544120, 0x0000204D, 0x00000000, 0x00000000,
+0x002E0209, 0x80000101, 0x000409FA, 0x00FF0400,
+0x05070000, 0x02000201, 0x82050700, 0x00020002,
+0x03830507, 0x07010040, 0x40020405, 0x02090000,
+0x0101002E, 0x09FA8000, 0x04000004, 0x000000FF,
+0x02010507, 0x07000040, 0x40028205, 0x05070000,
+0x00400383, 0x04050701, 0x00004002, 0x00000000,
+0x00000000, 0x07090000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, };
+
+const u32_t zcFwImageSize=13656;
diff --git a/drivers/staging/otus/hal/hpfwu_FB50_mdk.c b/drivers/staging/otus/hal/hpfwu_FB50_mdk.c
new file mode 100644
index 00000000000..ed1736f945a
--- /dev/null
+++ b/drivers/staging/otus/hal/hpfwu_FB50_mdk.c
@@ -0,0 +1,721 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include "cprecomp.h"
+
+const u32_t zcFwImage[] = {
+0x0009000B, 0x4F222FE6, 0xD2287FFC, 0x0009420B,
+0x0009B019, 0x9446D526, 0xE600A003, 0x8D043642,
+0x60527601, 0x8DF9C840, 0xD4222F02, 0x4E0BDE22,
+0xD4220009, 0x00094E0B, 0x4E0BD421, 0x7F040009,
+0xA0254F26, 0x4F226EF6, 0x410BD11E, 0xD41E0009,
+0x0009440B, 0x450BD51D, 0xD71D0009, 0x611DE1FF,
+0x2712D21C, 0xD41C5029, 0xE1FFCB01, 0x1209E501,
+0x12112212, 0xD5192452, 0xD6199716, 0xE7002572,
+0x2670D218, 0x2272D618, 0x4F26E201, 0x2622000B,
+0xDD17DC16, 0x4C0BDE17, 0x4D0B0009, 0x4E0B0009,
+0xAFF80009, 0x27100009, 0x00000640, 0x0020095A,
+0x001C001C, 0x00202940, 0x00200E2A, 0x0020294C,
+0x00202964, 0x00200CF0, 0x00200F26, 0x002009C4,
+0x001C3510, 0x001C3624, 0x001E212C, 0x002028EC,
+0x00202850, 0x002028F4, 0x00202900, 0x00200BEC,
+0x00201FD4, 0x002017B8, 0x2FD62FC6, 0x4F222FE6,
+0xDEA17FA4, 0x61E0E01C, 0x7D016DE3, 0x61D00F14,
+0xD59FD49E, 0x450BE020, 0xE0200F14, 0xE78004FC,
+0x604C66E2, 0x7D7F677C, 0x1F693070, 0x2D628F17,
+0x01FCE01C, 0x641CE500, 0xD797DE96, 0x3243625D,
+0xA21A8B01, 0x655D0009, 0x31EC6153, 0xE0286C10,
+0x6D530FC4, 0x3D7C62CE, 0xAFEF2D20, 0x20087501,
+0xE01C8B15, 0xE50001FC, 0xD78BDE8A, 0x641CA00A,
+0x6C53655D, 0x66C23CEC, 0x66626253, 0x2262327C,
+0x1F697504, 0x3243625D, 0xA1F68BF2, 0x88012D10,
+0xE01C8B16, 0xE40001FC, 0x671C2D40, 0x624DDE7D,
+0x8B013273, 0x0009A1E9, 0x62E3644D, 0x72046D43,
+0x3DEC6143, 0x65D2312C, 0x74086C12, 0x25C2AFEF,
+0x8B188804, 0x01FCE01C, 0x2D40E400, 0xDE71671C,
+0x3273624D, 0xA1D08B01, 0x644D0009, 0x62E36D43,
+0x65D23DEC, 0x61437204, 0x6612312C, 0x74086C52,
+0xAFED2C69, 0x880525C2, 0xE01C8B18, 0xE40001FC,
+0x671C2D40, 0x624DDE63, 0x8B013273, 0x0009A1B5,
+0x6C43644D, 0x3CEC62E3, 0x720465C2, 0x3D2C6D43,
+0x615266D2, 0x216B7408, 0x2512AFED, 0x8B138830,
+0xE200DE58, 0x64E22D20, 0x8B042448, 0x420BD257,
+0xA19A0009, 0x55E10009, 0x57E356E2, 0xDD545CE4,
+0x2FC64D0B, 0x7F04A191, 0x89018828, 0x0009A0EA,
+0xE143DE4C, 0x622D62E1, 0x8F033217, 0x56FB1FEB,
+0x2621E240, 0x8B013217, 0x0009A0D5, 0xE1015EFB,
+0x301685E1, 0xA0CE8B01, 0xE4010009, 0x2D4055FB,
+0x6451B179, 0xE14357FB, 0xE0546271, 0x3517652D,
+0x0F568D41, 0x3563E640, 0xE6008B05, 0x0F65E034,
+0xA00FE11A, 0x615372C0, 0x41214121, 0x41214121,
+0x45214121, 0x45214521, 0xC9036053, 0xE0346603,
+0x71180F65, 0x2209E007, 0x641DE030, 0x0F2565F3,
+0x1F4EB1F1, 0x04FDE034, 0x674DE030, 0x47080CFD,
+0x607361CD, 0x4108D22B, 0xE00F0CFE, 0x1F1F420B,
+0x2CD96D07, 0x5EFB6073, 0x85E20FC6, 0x420B51FF,
+0x2C0B600D, 0x54FE6073, 0xB1BB0FC6, 0xE05465F3,
+0x652D62E1, 0xE6400F56, 0x89623563, 0xE050E100,
+0x60230F15, 0x4008C903, 0x6D034000, 0xE0406103,
+0xE0440FD6, 0xD217EEFF, 0x6EEC0FF6, 0x0F26E058,
+0x60E3420B, 0x42216253, 0x42214221, 0x66234221,
+0x326C4200, 0x45214200, 0xE0486707, 0x0F764521,
+0xC9036053, 0x40085CFB, 0x7C0630FC, 0x6E036D2D,
+0x1FD51FC6, 0x1F04A02E, 0x00117D00, 0x00202968,
+0x00200E2A, 0x00117D04, 0x00117D84, 0x00200700,
+0x0020074C, 0x00202034, 0x0FD6E04C, 0x05FEE044,
+0x64D3B189, 0x64E2E048, 0xE04006FE, 0x2E422469,
+0x01FE67C4, 0x667CE058, 0x420B02FE, 0x240B6063,
+0x05FEE044, 0xB15D2E42, 0xE05064D3, 0x7D0101FD,
+0x0F157101, 0x02FDE050, 0x3262E606, 0x56FB8BDC,
+0x55FB6261, 0x85514200, 0x302C750C, 0x6103701B,
+0x64F3E600, 0xE704A004, 0x76016256, 0x74042422,
+0x3273626D, 0x65F38BF8, 0x641DB13C, 0xB0D256FB,
+0xA0AA6461, 0xD4880009, 0xE201D588, 0x2D20450B,
+0x0009A0A3, 0x8B078829, 0xE200DE85, 0x66E22D20,
+0x646DB0A1, 0x0009A099, 0x622CE281, 0x8B3D3020,
+0xD680E738, 0xE0442D70, 0xE0480C6E, 0x6E621DC1,
+0x51611DE2, 0x54621D13, 0x55651D44, 0x57631D55,
+0x5C661D76, 0x0E6E1DC7, 0x1DE8E040, 0xE050016E,
+0x54641D19, 0x056E1D4A, 0x1D5BE04C, 0xE054076E,
+0x0C6E1D7C, 0x1DCDE058, 0xE044026E, 0xED001D2E,
+0xE04806D6, 0x16D126D2, 0x16D516D2, 0x16D616D3,
+0xE04006D6, 0xE05006D6, 0x06D616D4, 0x06D6E04C,
+0x06D6E054, 0x06D6E058, 0x1F29A057, 0x622CE282,
+0x89313020, 0x05FCE020, 0x625CE683, 0x3260666C,
+0xD65D8B07, 0x2650E500, 0x52617680, 0xA044D65B,
+0xE6902622, 0x3260666C, 0xD2578B16, 0xE500D658,
+0x60622250, 0xCB20D257, 0xE6052602, 0xD6562262,
+0x2252460B, 0x420BD255, 0xD2550009, 0x2262E601,
+0x4618D254, 0x2262A029, 0xD254D453, 0xD4546542,
+0x0009420B, 0x0009A021, 0xE524D647, 0xD5452650,
+0x16215257, 0x16225258, 0x16235259, 0x1624525A,
+0x1625525B, 0x1626525C, 0x1627525D, 0x1628525E,
+0x1F29525F, 0xE2001629, 0x15281527, 0x152A1529,
+0x152C152B, 0x152E152D, 0x7F5C152F, 0x6EF64F26,
+0x000B6DF6, 0x4F226CF6, 0xE240614D, 0x89173123,
+0x3127E21F, 0xD43B8908, 0xE001D53B, 0x6642450B,
+0x26796707, 0x2462A00C, 0x3127E23F, 0xD7358908,
+0x71E0D635, 0x460BE001, 0x62075571, 0x17512529,
+0x000B4F26, 0x4F220009, 0xE240614D, 0x89153123,
+0x3127E21F, 0xD42B8907, 0x6642D22B, 0xE001420B,
+0xA00B260B, 0xE23F2462, 0x89073127, 0xD626D725,
+0x71E05571, 0xE001460B, 0x1751250B, 0x000B4F26,
+0xE6400009, 0x46284618, 0x6252D520, 0x89FC2268,
+0x0009000B, 0x4618E680, 0xD51C4628, 0x22686252,
+0x000B89FC, 0xA0010009, 0x7201E200, 0x8BFC3242,
+0x0009000B, 0x4618E680, 0xD5154628, 0x22686252,
+0x000B8BFC, 0x00000009, 0x0020296C, 0x00200E2A,
+0x00117D04, 0x00202858, 0x00117D80, 0x002028EC,
+0x001C3500, 0x001D4004, 0x00200F26, 0x002009C4,
+0x001E212C, 0x001C3D28, 0x00117D00, 0x00200E8A,
+0x00202984, 0x001C3704, 0x00202034, 0x001C373C,
+0x001C3700, 0x4F222FE6, 0x6E537FFC, 0x2F42BFCA,
+0xD61561E2, 0x1615E280, 0x421854E1, 0x55E21646,
+0x16574228, 0x6EF257E3, 0x2E2B1678, 0x7F0426E2,
+0xAFA74F26, 0x2FC66EF6, 0x2FE62FD6, 0xDD0A4F22,
+0xBFAF6C53, 0xBF946E43, 0xBFAB2DE2, 0x51D50009,
+0x54D62C12, 0x55D71C41, 0x56D81C52, 0x4F261C63,
+0x6DF66EF6, 0x6CF6000B, 0x001C370C, 0x0009A0F8,
+0xD19B4F22, 0xD49B9299, 0x2122B00D, 0x9795E605,
+0xB0229595, 0xB0366463, 0xB03A0009, 0xB03D0009,
+0xA06C0009, 0x4F124F26, 0xD1934F02, 0x94873145,
+0x4609060A, 0x46094609, 0x00293646, 0xD78CD58F,
+0x2500CA01, 0x4F062762, 0x4F16000B, 0xBFEA4F22,
+0xB0230009, 0xA0520009, 0x2FE64F26, 0x6E63D188,
+0x44186612, 0x4528926D, 0x26294408, 0x44084500,
+0x4400265B, 0x4708264B, 0x47082162, 0x27EBD181,
+0x000B2172, 0xD1806EF6, 0xE603D480, 0x000B2162,
+0xD27F2462, 0xE40A9656, 0x2262AFB0, 0x2FC62FB6,
+0x2FE62FD6, 0xDC7B4F22, 0x2C22E201, 0xBFA5E40A,
+0x60C27C44, 0xCB01ED00, 0x60C22C02, 0xC901EB64,
+0x6E03A008, 0x89073DB2, 0xE40160C2, 0xBF95C901,
+0x7D016E03, 0x8BF52EE8, 0x8B033DB2, 0xD26FD46E,
+0x0009420B, 0x4F26E40A, 0x6DF66EF6, 0xAF856CF6,
+0x44116BF6, 0x604B8F01, 0x000B6043, 0x2F860009,
+0x2FA62F96, 0x2FC62FB6, 0x2FE62FD6, 0x7FFC4F22,
+0x6DA3EA00, 0xDC626BA3, 0x9914E864, 0x8B4E2BB8,
+0x3AE3EE0A, 0x60C2894B, 0xCB02ED00, 0x62C22C02,
+0x2F0260C2, 0xA010C902, 0x096C6E03, 0x5BB45288,
+0x1FFF09B4, 0x01FF03C4, 0x89083D83, 0xE46460C2,
+0xC9022F02, 0x6E03BF52, 0x2EE87D01, 0xD1518BF4,
+0x54C1D551, 0x66526412, 0x6269EE01, 0x4220622F,
+0x622F4219, 0x4E182299, 0x8D0322E8, 0xE4FF6423,
+0x3428229A, 0x6572D749, 0x622F6259, 0x42194220,
+0x2299622F, 0x8D0322E8, 0xE6FF6623, 0x3628229A,
+0x3468BFA7, 0x30E2EE02, 0xAFB78901, 0xD240EB01,
+0x6EECEEE6, 0xBF21E40A, 0xAFAF22E2, 0xEE0A7A01,
+0x89013AE3, 0x8B033D83, 0xD234D43A, 0x0009420B,
+0x4F267F04, 0x6DF66EF6, 0x6BF66CF6, 0x69F66AF6,
+0x68F6000B, 0x5651D534, 0x46286052, 0x306C000B,
+0x2FC62FB6, 0x2FE62FD6, 0x4F124F22, 0xBFF14F02,
+0x6B036E43, 0xDD1CDC2D, 0x0009BFEC, 0x3C0530B8,
+0x4609060A, 0x46014609, 0x020A3D65, 0x42094209,
+0x32E24209, 0x4F068BF0, 0x4F264F16, 0x6DF66EF6,
+0x000B6CF6, 0x2FE66BF6, 0xDE214F22, 0xE500E102,
+0x2E12E403, 0x2E52BFD4, 0x4618E606, 0xE403E700,
+0x2E722E62, 0xAFCB4F26, 0x4F226EF6, 0x0009BFEB,
+0xE6E6D213, 0xE40A666C, 0x2262BFC2, 0x4F26AFE3,
+0x002028F0, 0x0024CDE0, 0x10624DD3, 0x00202AF0,
+0x001C5814, 0x001C59D0, 0x001C59A4, 0x001C639C,
+0x001C5804, 0x001C581C, 0x00202998, 0x00200E2A,
+0x001C5860, 0x001C6864, 0x001C59BC, 0x001C69BC,
+0x001C947C, 0x002029B0, 0x001C1040, 0xCCCCCCCD,
+0x001D4004, 0x2F962F86, 0x2FB62FA6, 0x2FD62FC6,
+0x4F222FE6, 0xE4007FE4, 0x4528E510, 0x67436C43,
+0xE107A00F, 0x6043644D, 0x0F564008, 0xEE0060C3,
+0x815125C1, 0x81538152, 0x157315E2, 0x751415E4,
+0x624D7401, 0x8BED3213, 0xDA6F51F1, 0x1A1154F2,
+0xD16E2A12, 0x57F455F3, 0x6DF258F5, 0x1141D96C,
+0x11532142, 0x11751152, 0x11871174, 0x52F61186,
+0x19D1D668, 0xD86829D2, 0xDA68E950, 0x1621EBB4,
+0x6BBC2622, 0xA0214908, 0x6EEDEE00, 0x61E36DE3,
+0x41084D08, 0x31EC3DEC, 0x41084D08, 0x60C33D8C,
+0xE7904108, 0x81D12DC1, 0x41086093, 0x81D2677C,
+0x31AC60C3, 0x3472E200, 0x1DD281D3, 0xD4551D13,
+0x1D248D01, 0xB03AD450, 0x7E0165D3, 0x34B264ED,
+0xD14D8BDB, 0x6512DB52, 0x4529D24D, 0x64121B51,
+0x674DD14A, 0x67222B72, 0x4729D64E, 0x69221B73,
+0x689D2FD2, 0x69121B82, 0x5A122692, 0x5B1416A2,
+0x16B4DA44, 0x16C65C16, 0x16EA6EA2, 0x4F267F1C,
+0x6DF66EF6, 0x6BF66CF6, 0x69F66AF6, 0x68F6000B,
+0x60616642, 0x8D04C803, 0x6061E500, 0x8802C903,
+0x52628B03, 0x51246563, 0x000B2412, 0x2FD66053,
+0x4F222FE6, 0x6E537FEC, 0xE5506253, 0xE4006D43,
+0xA0014508, 0x5224E101, 0x22116043, 0x81238121,
+0x81226053, 0x362056E2, 0xD22F8BF5, 0x64F316E4,
+0x420BE614, 0x65E165E3, 0x2549E4FC, 0x61F12E51,
+0x214965F3, 0x54D12F11, 0x410BD127, 0x57D1E614,
+0xCB016071, 0x1DE12701, 0x4F267F14, 0x000B6EF6,
+0x2FD66DF6, 0x4F222FE6, 0x6E537FEC, 0xE5FC6653,
+0x60616D43, 0xCB012059, 0x52E22601, 0x8B063260,
+0x51E212E4, 0x8B0431E0, 0xA00252D1, 0xAFF01E22,
+0xD2155664, 0xE61464F3, 0x65E3420B, 0xE1FC67E1,
+0x2E712719, 0x54D167F1, 0xD10F2719, 0xE61465F3,
+0x2F71410B, 0x602152D1, 0x2201CB01, 0x7F141DE1,
+0x6EF64F26, 0x6DF6000B, 0x002028BC, 0x002028C4,
+0x002028B4, 0x002028E4, 0x0010008C, 0x00100EC0,
+0x001E2108, 0x001C3D00, 0x00202194, 0x2FC62FB6,
+0x2FE62FD6, 0xD6314F22, 0x60D36D62, 0x894DC803,
+0xDB30DC2F, 0x0009A02C, 0xC9036061, 0x892B8801,
+0xD22DD42B, 0x0009420B, 0x65035603, 0xC8208561,
+0xE0508903, 0x720102BE, 0x85620B26, 0x4000600D,
+0x4000366A, 0x40004624, 0x206D4624, 0xD423C903,
+0x40086E03, 0xD1224000, 0x340C410B, 0x61E3D521,
+0xD721E001, 0x450BD221, 0x64E37E30, 0x2702420B,
+0x66C252C1, 0x8BCF3620, 0x4E18EE01, 0xA011DB1C,
+0x6061EC75, 0x8801C903, 0xD4198910, 0x460BD612,
+0xD4180009, 0x470BD718, 0xD2136503, 0x64C3D113,
+0x22E2410B, 0x66B252B1, 0x8BEA3620, 0xC80460D3,
+0xD2128906, 0x6EF64F26, 0x6CF66DF6, 0x6BF6422B,
+0x6EF64F26, 0x6CF66DF6, 0x6BF6000B, 0x001E2100,
+0x002028BC, 0x00202858, 0x00200AE0, 0x002028C4,
+0x00200B62, 0x00202034, 0x001C3D30, 0x00200DF0,
+0x002028B4, 0x002028E4, 0x00200AFE, 0x002000F8,
+0xE601D237, 0x1265D537, 0x000B2252, 0xD6361266,
+0x88016062, 0xE1018B62, 0xD5342612, 0x5451D134,
+0xE0406212, 0x2122324C, 0x54115752, 0x1141347C,
+0x57125453, 0x1172374C, 0x52135755, 0x1123327C,
+0x56146452, 0x1164364C, 0x54155754, 0x1145347C,
+0x56165458, 0x1166364C, 0x6762D626, 0x327C5217,
+0x57611127, 0x327C5218, 0x57621128, 0x327C5219,
+0x57631129, 0x347C541A, 0x5764114A, 0x347C541B,
+0x5765114B, 0x347C541C, 0x5266114C, 0x372C571D,
+0x5267117D, 0x342C541E, 0x5268114E, 0x362C561F,
+0xD615116F, 0x041E6262, 0x342C7694, 0xE0440146,
+0x061E6262, 0x0166362C, 0x525CE048, 0xD60F051E,
+0x0156352C, 0xE0546262, 0x4229051E, 0x0156352C,
+0xE0585561, 0x4529061E, 0x0166365C, 0x0009000B,
+0x001C1010, 0x0000C34F, 0x001C1028, 0x001C369C,
+0x00202858, 0x001C3CA0, 0x001C36F4, 0x001C3B88,
+0xD62F7FFC, 0x2642644C, 0xC8205066, 0x2F028DFC,
+0x7F04000B, 0x2FD62FC6, 0x4F222FE6, 0x6D436C53,
+0xEE00A004, 0x7E0164D4, 0x644CBFEA, 0x8BF93EC2,
+0x6EF64F26, 0x000B6DF6, 0xA0016CF6, 0x76016643,
+0x22286260, 0x36488BFB, 0x6563AFE4, 0x2FB62F96,
+0x2FD62FC6, 0x4F222FE6, 0xEC1CED08, 0xDB196E53,
+0x61C3E90A, 0x60434B0B, 0x3092C90F, 0x66038D02,
+0x7630A001, 0x4D107637, 0x7E012E60, 0x7CFC8FF1,
+0x8058E000, 0x6EF64F26, 0x6CF66DF6, 0x000B6BF6,
+0x000B69F6, 0x000BE000, 0x2FE6E000, 0x7FEC4F22,
+0x6E436253, 0xBFD165F3, 0xBFC66423, 0xBFC464E3,
+0xD40564F3, 0x0009BFC1, 0x4F267F14, 0x6EF6000B,
+0x001C0004, 0x002020F4, 0x002029CC, 0xE110D59C,
+0xE6406050, 0x2500C9FD, 0xE0FF75E9, 0x80516453,
+0x80538052, 0x80568055, 0x251075EF, 0xE1EF6250,
+0x2219E001, 0xE7202520, 0x24608052, 0x2570000B,
+0xE4FDD590, 0xE7026152, 0x25122149, 0x74016052,
+0x2502CB01, 0xD18C6652, 0x25622649, 0x92C26012,
+0x2102CB08, 0xC9CF6012, 0x60122102, 0x2102CB03,
+0x000B1172, 0x4F221123, 0xE100D484, 0xD285D784,
+0xD5852410, 0x2711D485, 0x2211E700, 0xBFBD2511,
+0xD5832471, 0x2560E600, 0x4F26AFD2, 0xD281664C,
+0x362C4600, 0xCB106060, 0x2600000B, 0xD27D654C,
+0x352C4500, 0xE1EF6650, 0x000B2619, 0x664C2560,
+0x4600D279, 0x6060362C, 0x000BCB10, 0x654C2600,
+0x4500D275, 0x6650352C, 0x2619E1EF, 0x2560000B,
+0xD270664C, 0x362C4600, 0xCB086060, 0x2600000B,
+0xD26C654C, 0x352C4500, 0xE1F76650, 0x000B2619,
+0x664C2560, 0x4600D268, 0x6060362C, 0x000BCB08,
+0x654C2600, 0x4500D264, 0x6650352C, 0x2619E1F7,
+0x2560000B, 0xD65F624C, 0x326C4200, 0xC9086020,
+0x40214021, 0x000B4021, 0x624C600C, 0x4200D65A,
+0x6020326C, 0x4021C908, 0x40214021, 0x600C000B,
+0xD156644C, 0x341C74FF, 0x000B6240, 0xD154602C,
+0x341C644C, 0x000B6240, 0x2FE6602C, 0x655C4F22,
+0x3567E60A, 0x6E438D15, 0x6453BFEA, 0x60EC640C,
+0x8B028801, 0xA002E00F, 0x44092409, 0x624C4409,
+0x3263E60A, 0xBFE28905, 0x620C644C, 0xC8806023,
+0xE2008B00, 0x4F266023, 0x6EF6000B, 0xD6414F22,
+0x88016062, 0xB2228B03, 0xA0030009, 0xD23E0009,
+0x2260E640, 0xE200D63D, 0x000B4F26, 0x4F222622,
+0x6062D638, 0x8B018802, 0x0009B26C, 0xE200D637,
+0x000B4F26, 0x0FFF2622, 0xD433D532, 0xE701E100,
+0x000B2512, 0xD2302470, 0x000BE604, 0xD5202260,
+0x6150E4FD, 0x2149D62E, 0x2510E700, 0x2670000B,
+0xE4FBD51B, 0x22496250, 0x2520000B, 0xE4F7D518,
+0x22496250, 0x2520000B, 0xD2264F22, 0x600D8522,
+0x89112008, 0x89138801, 0x89158803, 0x89178805,
+0x89418806, 0x89478808, 0x894D8809, 0x8953880A,
+0x8959880B, 0x0009A060, 0x0009B062, 0x600CA05D,
+0x0009B070, 0x600CA059, 0x0009B07A, 0x600CA055,
+0x6260D606, 0x8B4F2228, 0x0009B086, 0x600CA04D,
+0x001E1028, 0x001E2148, 0x001E1108, 0x0020293D,
+0x0020292C, 0x0020292E, 0x00202930, 0x00202910,
+0x001E1008, 0x001E103F, 0x001E105F, 0x001E1030,
+0x001E1090, 0x00202938, 0x001E100B, 0x00202934,
+0x0020293C, 0x00202904, 0x6260D687, 0x8B232228,
+0x0009B06A, 0x600CA021, 0x6260D683, 0x8B1B2228,
+0x0009B0B4, 0x600CA019, 0x6260D67F, 0x8B132228,
+0x0009B0BA, 0x600CA011, 0x6260D67B, 0x8B0B2228,
+0x0009B11E, 0x600CA009, 0x6260D677, 0x8B032228,
+0x0009B136, 0x600CA001, 0x4F26E000, 0x0009000B,
+0xD273D172, 0xD5738412, 0x4000C90F, 0xD772012D,
+0x611CE403, 0xD671E20F, 0x27122540, 0xE0012520,
+0x2602000B, 0xE601D269, 0x30668523, 0xE0008D06,
+0xE000D267, 0x8122D669, 0x2602E001, 0x0009000B,
+0x8523D262, 0x2008600D, 0x88018905, 0xD6648B0A,
+0xCB016060, 0xD6612600, 0xE101D45D, 0x2612E001,
+0x8142000B, 0xE000000B, 0xE501D158, 0x45188513,
+0x3453640D, 0x8D056603, 0xD25AE000, 0xE001D557,
+0x25022260, 0x0009000B, 0xD1504F22, 0x650D8513,
+0x44196453, 0x672E6249, 0x602C227D, 0x89098801,
+0x890C8802, 0x89108803, 0x89268806, 0x89298807,
+0x0009A038, 0xD64DD54C, 0xA027E212, 0x625C2652,
+0x8B2F2228, 0xA01ED64A, 0x605C6262, 0x89052008,
+0x89088810, 0x890B8820, 0x0009A024, 0xD643D445,
+0xA013E204, 0xD7442642, 0xE20CD640, 0x2672A00E,
+0xD63ED542, 0xA009E218, 0xD4412652, 0xE20AD63B,
+0x2642A004, 0xD639D23F, 0xE22E2622, 0xD43E8515,
+0x3277670D, 0x8F012421, 0x24516503, 0x0009B0DF,
+0xE001A001, 0x4F26E000, 0x0009000B, 0xE101D629,
+0x2610D436, 0xD7286541, 0x655DD128, 0xE001E20F,
+0x26202752, 0x2102000B, 0x4F222FE6, 0x8523D21F,
+0x2448640C, 0xD62D8B08, 0xE200D521, 0x84512621,
+0x20499430, 0x8051A026, 0x60E0DE1D, 0x8D0BC840,
+0x3427E201, 0xD1258922, 0x420BD225, 0xD5252141,
+0xCB046052, 0x2502A00B, 0x89173427, 0xD722D21F,
+0x2241470B, 0xE5FBD61F, 0x21596162, 0x84E12612,
+0xB12DCB80, 0x60E080E1, 0xCB04D61C, 0x60602E00,
+0x2600C93F, 0xE001D609, 0x2602A001, 0x4F26E000,
+0x6EF6000B, 0x0000FF7F, 0x0020293D, 0x00202904,
+0x00202910, 0x001E1100, 0x001E100C, 0x00202934,
+0x001E1000, 0x001E1001, 0x00202AF4, 0x00202918,
+0x00202920, 0x00202B62, 0x00202B66, 0x00202B72,
+0x00202B8A, 0x00202B94, 0x0020291C, 0x0020292A,
+0x00201AB6, 0x001E1108, 0x00201BC2, 0x001E1015,
+0x6060D696, 0x8919C880, 0x6021D295, 0x8B158801,
+0xE501D294, 0x30568524, 0xD1938910, 0xD493E203,
+0x65412120, 0x655DE00B, 0xD5910656, 0xE702E40F,
+0x25712140, 0xE001D78F, 0x2702000B, 0xE000000B,
+0x4F222FE6, 0x84E1DE8C, 0x8934C880, 0x8554D585,
+0x8F302008, 0xD7896103, 0x66728553, 0x650C6403,
+0x620C8566, 0x8B263520, 0xD780D685, 0x644C651C,
+0x27412651, 0xC84060E0, 0xD2828907, 0x0009420B,
+0x6062D681, 0xA008CB04, 0xD1802602, 0x0009410B,
+0xE5FBD67D, 0x24596462, 0xB0A12642, 0xD5750009,
+0x2522E201, 0xD77A60E0, 0x2E00CB04, 0xC93F6070,
+0xA0012700, 0xE0006023, 0x000B4F26, 0x2FA66EF6,
+0x2FC62FB6, 0x2FE62FD6, 0xE240DA69, 0xDC6666A1,
+0x3123616D, 0x62638900, 0x6ED36D2C, 0x4E2136D8,
+0x4E212A61, 0xDB6CD46B, 0xE700A00F, 0x770166B2,
+0x71026163, 0x65612B12, 0x71026613, 0x62612B12,
+0x622D655D, 0x325C4228, 0x627C2422, 0x8BED32E3,
+0xC90360D3, 0x8B108803, 0xED076EB2, 0x710261E3,
+0x67132B12, 0x62E17102, 0x65712B12, 0x655D622D,
+0x352C4528, 0xA00C2CD0, 0x88022452, 0xA0038B01,
+0x8801E203, 0xE2018B05, 0x66B22C20, 0x677D6761,
+0xEB0F2472, 0x6DA12CB0, 0x8B052DD8, 0xD445D24F,
+0xE101EE00, 0x241222E2, 0x6DF66EF6, 0x6BF66CF6,
+0x6AF6000B, 0x2FE62FD6, 0xE240DD3D, 0x616D66D1,
+0x89003123, 0x672C6263, 0xDE433678, 0x2D617703,
+0xD6404721, 0x472164E2, 0xE100A00E, 0x71016562,
+0x24506253, 0x42197401, 0x74012420, 0x24504529,
+0x45197401, 0x74012450, 0x3273621C, 0x42008BEE,
+0x64D166E2, 0x362C4200, 0x8F062448, 0xDD332E62,
+0xE500DE28, 0x2D52E701, 0x6EF62E72, 0x6DF6000B,
+0x2FE62FD6, 0xEE014F22, 0xED0AA005, 0x64E3BCB6,
+0x64E3BCBC, 0x62EC7E01, 0x8BF732D7, 0xEE01A005,
+0x64E3BCBD, 0x64E3BCC3, 0x62EC7E01, 0x8BF732D7,
+0x6EF64F26, 0x6DF6000B, 0x2FE62FD6, 0x7FFC4F22,
+0x6060D61F, 0x89758801, 0xE101D41E, 0xD7128548,
+0x650D2610, 0x45196070, 0x6659DD1B, 0x61D3626E,
+0xC840262D, 0x74027102, 0x8D47D718, 0xD218666C,
+0xE501DE0A, 0xA0312E22, 0x0000EE04, 0x001E1001,
+0x0020292A, 0x00202904, 0x001E1100, 0x0020292E,
+0x0020291C, 0x00202934, 0x001E1000, 0x00202920,
+0x0020292C, 0x00201AB6, 0x001E1108, 0x00201BC2,
+0x001E1015, 0x001E100C, 0x00202918, 0x00202938,
+0x0020293C, 0x00202AF4, 0x00202B8A, 0x00202B96,
+0x00202B06, 0x75016245, 0x71022121, 0x32E3625C,
+0x60638BF8, 0xE60181D4, 0xE417D538, 0x3243626C,
+0x6255891E, 0x27217601, 0x7702AFF8, 0xDE35D234,
+0x2E22E501, 0xEE04A004, 0x75016245, 0x71022121,
+0x32E3625C, 0x60638BF8, 0xE60181D4, 0xA004D52E,
+0x6255E417, 0x27217601, 0x626C7702, 0x8BF83243,
+0x2D21924B, 0xD72AD429, 0x2F126142, 0x6DF265F2,
+0xC9806053, 0x60532700, 0x6103C960, 0x60538071,
+0x65F26EF2, 0x4D19C903, 0x80724529, 0x451960DC,
+0x4E298172, 0x62EC605C, 0x302C4018, 0x6D428173,
+0x2FD22118, 0x62F26EF2, 0x421966F2, 0x656C4629,
+0x602C66F2, 0x401864EC, 0x304C4629, 0x81744619,
+0x4018606C, 0x8F07305C, 0xBCB58175, 0x620C0009,
+0x89082228, 0x0009A00A, 0x88406013, 0xB00A8B03,
+0xA0030009, 0xD60B0009, 0x2622E202, 0x4F267F04,
+0x000B6EF6, 0x000B6DF6, 0x060A0009, 0x00202B36,
+0x00202B34, 0x00202920, 0x00202B08, 0x001E100C,
+0x00202904, 0x00202934, 0x7FFC4F22, 0x6620D27E,
+0x8D082668, 0xD47D2F60, 0x420BD27D, 0x64F00009,
+0xA0907F04, 0x7F044F26, 0x000B4F26, 0x000B0009,
+0x2FE60009, 0xDE774F22, 0x60E0D677, 0xCBC0D477,
+0x62602E00, 0xC803602C, 0x40218904, 0x70014021,
+0x6603A002, 0x66034009, 0xD671616D, 0xE500A004,
+0x75016262, 0x74042422, 0x3213625D, 0xD16D8BF8,
+0x0009410B, 0xE401D66C, 0x84E22641, 0x80E2C9BF,
+0x000B4F26, 0x2FE66EF6, 0xD5687FFC, 0x6250DE61,
+0x642C84E2, 0xCB407404, 0x80E2614D, 0x44216413,
+0xD7634421, 0xE600A004, 0x76016256, 0x27222F22,
+0x3243626D, 0x60138BF8, 0x2008C903, 0x88038912,
+0x88028905, 0x88018906, 0xA0088907, 0xE0070009,
+0x8078A005, 0xA002E003, 0xE0018078, 0x62528078,
+0x27222F22, 0xD650E00F, 0x60618078, 0x8B018801,
+0x2621E200, 0x6060D64F, 0x2600CB08, 0xC93F60E0,
+0x7F042E00, 0x6EF6000B, 0x6021D247, 0x8D188801,
+0xD2466143, 0x22106053, 0x60638021, 0xD4468121,
+0xE500A007, 0x027C605D, 0x364C6603, 0x26207001,
+0x625D6503, 0x3213611C, 0xD6408BF4, 0xC9BF6060,
+0x000B2600, 0x2FD60009, 0x4F222FE6, 0x60437FFC,
+0x8D02C820, 0xBF6A6E43, 0x60E30009, 0x8901C810,
+0x0009BF67, 0xC84060E3, 0xBF8C8901, 0x60E30009,
+0x8929C801, 0x60D0DD32, 0x8D03C802, 0xD6312F00,
+0x0009460B, 0xC80460F0, 0xD62F8902, 0x0009460B,
+0x602362F0, 0x8902C880, 0xC97F60D0, 0x60232D00,
+0x8902C801, 0x420BD229, 0xD5290009, 0x88026052,
+0xD2288B03, 0xA005E604, 0x88012260, 0xD2258B02,
+0x2260E601, 0x2522E200, 0xC88060E3, 0xD2228916,
+0x60E36E20, 0x8902C802, 0x420BD220, 0x60E30009,
+0x8902C804, 0x420BD21E, 0x60E30009, 0x8905C808,
+0x7F04D21C, 0x6EF64F26, 0x6DF6422B, 0x4F267F04,
+0x000B6EF6, 0x00006DF6, 0x001E1020, 0x002029D0,
+0x00200E2A, 0x001E1015, 0x001E10BF, 0x00117D00,
+0x001E10FC, 0x002000F8, 0x00202930, 0x00117D80,
+0x001E10F8, 0x001E10AE, 0x00117D84, 0x001E1017,
+0x001E1021, 0x0020105C, 0x0020107E, 0x00201608,
+0x00202934, 0x001E100B, 0x001E1028, 0x002010AE,
+0x002010C0, 0x002010CC, 0xD6A8644C, 0x346C74FF,
+0x2450000B, 0x644CD6A6, 0x000B346C, 0xD6A52450,
+0x346C644C, 0x2450000B, 0x616D625C, 0x41194208,
+0x60194208, 0x644C4200, 0x324C670E, 0x207DD19E,
+0xC90F4200, 0x000B321C, 0x67632200, 0x4208625C,
+0x42004208, 0x324C644C, 0x4200D198, 0x000B321C,
+0x2FE62270, 0x614C4F12, 0x4100D493, 0x6710314C,
+0x2729E29F, 0x65736E53, 0x4719676D, 0x672E6279,
+0x4221227D, 0x42214221, 0x7601662C, 0xE4014608,
+0x34E84608, 0x644C4600, 0x0E1A0467, 0x215025EB,
+0x000B4F16, 0x4F226EF6, 0xD2857FE8, 0x88016021,
+0xD2848B7B, 0x26686621, 0xD2838B77, 0x26686621,
+0xE50F8B73, 0xE401BFA0, 0xBFA3E501, 0xE586E400,
+0xE400655C, 0x2F50BFA3, 0xBFA0E401, 0xE602E506,
+0x60634618, 0x81F2E401, 0x6543BF9E, 0xE40185F2,
+0xBFAA6543, 0x85F26603, 0x6543E401, 0x6603BFB1,
+0xE40265F0, 0x6053756C, 0x80F8BF7E, 0xBF81E402,
+0x84F8E512, 0x7090E402, 0x6503BF81, 0x4618E602,
+0x81F66063, 0xBF7FE402, 0x85F6E500, 0x6603E402,
+0xE500BF8B, 0xE40285F6, 0xBF926603, 0xE5FEE500,
+0xE010655C, 0xBF5FE403, 0xE5130F54, 0xE40EBF62,
+0x05FCE010, 0xBF62E40E, 0xE5007585, 0xBF63E403,
+0xE500E640, 0xBF70E403, 0xE500E640, 0xBF78E403,
+0xE5FFE640, 0xE014655C, 0xBF45E404, 0xE40F0F54,
+0xE504BF48, 0x05FCE014, 0xBF48E40F, 0xE5017584,
+0xBF49E640, 0xE501E404, 0xBF56E640, 0xE501E404,
+0xE404E640, 0xAF5C7F18, 0x7F184F26, 0x000B4F26,
+0x4F220009, 0xD2427FF0, 0x88016021, 0xD2418B71,
+0x26686621, 0xD2408B6D, 0x26686621, 0xE50F8B69,
+0xE401BF1A, 0xBF1DE501, 0xE586E400, 0xE400655C,
+0x2F50BF1D, 0xBF1AE401, 0xE401E506, 0xBF1B6543,
+0xE401E640, 0xBF286543, 0xE401E640, 0xBF306543,
+0x65F0E640, 0x756CE402, 0xBEFD6053, 0xE40280F4,
+0xE512BF00, 0xE40284F4, 0xBF007090, 0xE6406503,
+0xBF01E402, 0xE640E500, 0xBF0EE402, 0xE640E500,
+0xBF16E402, 0xE5FEE500, 0x6053655C, 0xBEE3E403,
+0xE51380F8, 0xE40EBEE6, 0xE40E84F8, 0xBEE67085,
+0xE5006503, 0xBEE7E640, 0xE500E403, 0xBEF4E640,
+0xE500E403, 0xBEFCE640, 0xE5FFE403, 0x6053655C,
+0xBEC9E404, 0xE40F80FC, 0xE504BECC, 0xE40F84FC,
+0xBECC7083, 0xE5016503, 0xBECDE640, 0xE501E404,
+0xBEDAE640, 0xE501E404, 0xE404E640, 0xAEE07F10,
+0x7F104F26, 0x000B4F26, 0x00000009, 0x001E1030,
+0x001E1080, 0x001E1090, 0x001E103F, 0x001E103E,
+0x0020292A, 0x0020292C, 0x0020292E, 0x0009000B,
+0x666CE680, 0x6563D2A0, 0x7540E700, 0x6473422B,
+0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6, 0x4C18EC01,
+0xDA9BDB9A, 0x65B252B1, 0x89223520, 0xC9036051,
+0x891E8801, 0xD197DE95, 0x64E3410B, 0x85036503,
+0x670D66A2, 0xDD943762, 0xD494890A, 0x420BD294,
+0xD1940009, 0xE701D494, 0x21724D0B, 0x0009AFE2,
+0x420BD292, 0xD69264E3, 0x4D0BD492, 0xAFD926C2,
+0x4F260009, 0x6DF66EF6, 0x6BF66CF6, 0x6AF6000B,
+0x7FF44F22, 0xE6818546, 0x85472F01, 0x81F1666C,
+0xD27D8548, 0x854281F2, 0x81F367F3, 0xE40C8543,
+0x605381F4, 0x81F56563, 0x7540420B, 0x4F267F0C,
+0x0009000B, 0x2F962F86, 0x2FB62FA6, 0x2FD62FC6,
+0x4F222FE6, 0xE2007FEC, 0xA0CBDB7B, 0x6A132F21,
+0x4A08D27A, 0xDE7AE001, 0x4A00420B, 0x7E303AEC,
+0x1F021FE1, 0x66B2DD77, 0x362052B1, 0xA0B58B01,
+0x60610009, 0x8801C903, 0xA0AF8B01, 0x85610009,
+0x8974C801, 0xEE105163, 0xDC638512, 0xC9036603,
+0x85136403, 0x4021600D, 0xC93F4021, 0x8D2030E3,
+0xD7696503, 0x62704408, 0x44004408, 0x22284500,
+0x345C8F0C, 0x6043D265, 0x625D052D, 0x60294219,
+0x207D670E, 0x605C81F6, 0x81F8A00B, 0x6043D260,
+0x685D052D, 0x60894819, 0x209D690E, 0x605C81F6,
+0xD75C81F8, 0x22286272, 0xE0FF8902, 0x81F8600C,
+0xEEFF85F8, 0x6EEC650D, 0x8B0F35E0, 0x4E0BDE45,
+0x540364B3, 0xBF7BE502, 0xD4536803, 0x410BD147,
+0xD7526583, 0xD452E901, 0x2792A020, 0x26E9EEFC,
+0x81126063, 0x946E8513, 0x81132049, 0x45088512,
+0x62036953, 0xE50885F6, 0x8112202B, 0x45188513,
+0x8113209B, 0xD4478514, 0x8114205B, 0x851161B2,
+0x811120E9, 0x602162B2, 0x2201CB01, 0x00094C0B,
+0x56F160B2, 0xCB0152F2, 0xAF7C2A02, 0x85612622,
+0xC802DC3A, 0xD938D227, 0x8D0FD82C, 0x420B64B3,
+0x65030009, 0x480B6493, 0xE8015E03, 0x85EF2C82,
+0x650DD635, 0x64D3460B, 0x0009AF65, 0x0009420B,
+0x6E035403, 0xE5088544, 0x45186103, 0x31502159,
+0xBF258B03, 0xA007E501, 0x85410009, 0x620DD52B,
+0x89012258, 0xE500BF1C, 0x480B6493, 0xD42865E3,
+0xE801D611, 0x2C82460B, 0x0009AF45, 0x7B0862F1,
+0x2F217201, 0xEE0362F1, 0x31E7612D, 0xAF2E8901,
+0x7F140009, 0x6EF64F26, 0x6CF66DF6, 0x6AF66BF6,
+0x000B69F6, 0xFE0368F6, 0x002018B8, 0x002028E4,
+0x002028EC, 0x00200AE0, 0x00200E2A, 0x002028B4,
+0x00200B62, 0x001E2130, 0x00202AD4, 0x00200AFE,
+0x001C3D30, 0x00202AD8, 0x002028C4, 0x00202034,
+0x001C3D00, 0x00202AE4, 0x00202AF0, 0x002029D4,
+0x00202A54, 0x00202900, 0x002028BC, 0x001E212C,
+0x00202ADC, 0x00202AE0, 0x00200E8A, 0x00008000,
+0x00202AEC, 0x4F222FE6, 0x6E22D20D, 0xC84060E3,
+0x22E28D02, 0x0009BE7A, 0x4218E240, 0x89012E28,
+0x0009BE76, 0xC80560E3, 0xBECB8901, 0x60E30009,
+0x8902C802, 0xAE734F26, 0x4F266EF6, 0x6EF6000B,
+0x001C3510, 0x080A0C0E, 0x00020406, 0x1A1C1E20,
+0x12141618, 0x2E303234, 0x26282A2C, 0x3A3C3E40,
+0x6C625648, 0x41112F26, 0xE2208F18, 0x890B3123,
+0x321CD204, 0xD1026220, 0x412B312C, 0x00090009,
+0x0020205E, 0x00202014, 0x000BE000, 0x400062F6,
+0x40004000, 0x40004000, 0x40004000, 0x62F6000B,
+0x40004000, 0x40004000, 0x40004000, 0x40184000,
+0x62F6000B, 0x40004000, 0x40004000, 0x40004000,
+0x40284000, 0x62F6000B, 0x40004000, 0x40184000,
+0x000B4028, 0xC90F62F6, 0x40054005, 0x40054005,
+0x62F6000B, 0x4005C907, 0x40054005, 0x62F6000B,
+0x4005C903, 0x000B4005, 0xC90162F6, 0x000B4005,
+0x000062F6, 0x080A0C0E, 0x00020406, 0x1A1C1E20,
+0x12141618, 0x2E303234, 0x26282A2C, 0x3A3C3E40,
+0x6C625648, 0x41112F26, 0xE2208F18, 0x890B3123,
+0x321CD204, 0xD1026220, 0x412B312C, 0x00090009,
+0x0020211E, 0x002020D4, 0x000BE000, 0x400162F6,
+0x40014001, 0x40014001, 0x40014001, 0x62F6000B,
+0x40014001, 0x40014001, 0x40014001, 0x40194001,
+0x62F6000B, 0x40014001, 0x40014001, 0x40014001,
+0x40294001, 0x62F6000B, 0x40014001, 0x40194001,
+0x000B4029, 0x400462F6, 0x40044004, 0xC90F4004,
+0x62F6000B, 0x40044004, 0xC9074004, 0x62F6000B,
+0x40044004, 0x000BC903, 0x400462F6, 0x000BC901,
+0x000062F6, 0x3622E218, 0x67438F12, 0x0009A004,
+0x76FF6254, 0x74012420, 0xC8036053, 0x60438BF8,
+0x8902C803, 0x422BD22B, 0xD22B0009, 0x0009422B,
+0x2FE66473, 0x8D4A3450, 0x27786763, 0x62438947,
+0x227B225B, 0xC9016023, 0x8D203452, 0x2EE86E03,
+0x60238B15, 0x8B08C803, 0x47096643, 0x47106256,
+0x8FFB2622, 0xA0327604, 0x47010009, 0x61436673,
+0x46106255, 0x8FFB2121, 0xA0287102, 0x66430009,
+0x47106254, 0x8FFB2620, 0xA0207601, 0x61430009,
+0x2EE8357C, 0x8F15317C, 0x60236653, 0x8B07C803,
+0x76FC4709, 0x47106262, 0x21268FFB, 0x0009A00F,
+0x65634701, 0x75FE6673, 0x46106251, 0x21258FFB,
+0x0009A005, 0x626076FF, 0x8FFB4710, 0x60432124,
+0x6EF6000B, 0x00202306, 0x002027B2, 0xE21E2FE6,
+0x67633626, 0x8D1B6153, 0x3E106E43, 0x3E128916,
+0x65E38908, 0x3672E600, 0x62148910, 0x25207601,
+0x7501AFF9, 0x317C64E3, 0x6513347C, 0xE600A004,
+0x625075FF, 0x24247601, 0x8BF93672, 0x60E3A011,
+0x890831E2, 0x327C6213, 0x8B0432E6, 0x651364E3,
+0xA0086673, 0xD28F6EF6, 0x651364E3, 0x422B6673,
+0x000B6EF6, 0xE2046EF6, 0x67433622, 0x8F10356C,
+0xA004346C, 0x75FF0009, 0x76FF6250, 0x60532424,
+0x8BF8C803, 0xC8036043, 0xA1058901, 0xA2770009,
+0xA2990009, 0x2FB60009, 0x2FD62FC6, 0x7FE42FE6,
+0x6C636043, 0x66521F62, 0xC9037504, 0x1F516E53,
+0x45086503, 0xE1FC6D43, 0x2D194500, 0x1F732558,
+0x1F651F44, 0x2FD28D0B, 0x88086053, 0x88108923,
+0x8818895B, 0xA0898B01, 0xA0BD0009, 0x62630009,
+0x2D22E600, 0x7CFC7D04, 0xEB10A00D, 0xE60064E6,
+0x7CF065E6, 0x62E261E6, 0x1D512D42, 0x1D231D12,
+0x7E047D10, 0x3CB21FE1, 0x1F6589F0, 0x2FD21FC2,
+0xA0A11FE6, 0x64D21FD4, 0x44286263, 0x44294418,
+0x42184419, 0x4629242B, 0x2D424619, 0x65637D04,
+0xA0217CFD, 0x67E6EB10, 0x62E67CF0, 0x64E66673,
+0x256B4618, 0x2D5261E2, 0x65234729, 0x45184719,
+0x4229275B, 0x42191D71, 0x47186743, 0x4429227B,
+0x44196713, 0x247B4718, 0x1D431D22, 0x41194129,
+0x65137D10, 0x1FE17E04, 0x89DC3CB2, 0x1FE67EFF,
+0x1FC21F55, 0xA0672FD2, 0x6CF21FD4, 0x66C257F5,
+0x46286273, 0x42284629, 0x2C62262B, 0x7C045DF2,
+0x7DFE4729, 0xA01CEB10, 0x65E65EF1, 0x66E66273,
+0x47286753, 0x6763227B, 0x452961E6, 0x257B4728,
+0x2C2264E6, 0x65131C51, 0x45284629, 0x1C62265B,
+0x41296643, 0x216B4628, 0x44291C13, 0x67437C10,
+0x3DB27DF0, 0x1FD289E1, 0x7EFEA034, 0x51F56CF2,
+0x621366C2, 0x42284618, 0x42184619, 0x2C62262B,
+0x7C045DF2, 0x7DFF4119, 0xA01FEB10, 0x65E65EF1,
+0x64E67DF0, 0x42286253, 0x421867E6, 0x66E6212B,
+0x61432C12, 0x45194128, 0x251B4118, 0x65731C51,
+0x44194528, 0x245B4518, 0x64631C42, 0x47194428,
+0x274B4418, 0x46191C73, 0x61637C10, 0x89DE3DB2,
+0x7EFD1FD2, 0x1FC41FE6, 0x5DF2E704, 0xA00D5EF6,
+0x62E451F4, 0x66E47DFC, 0x65E464E4, 0x71012120,
+0x71012160, 0x71012140, 0x71012150, 0x89F03D72,
+0x66D357F3, 0x641365E3, 0x6EF67F1C, 0x6CF66DF6,
+0x6BF6A190, 0x00202194, 0x2FC62FB6, 0x2FE62FD6,
+0x60437FE4, 0x6C63C903, 0x66031F62, 0x460875FC,
+0x61526E43, 0x4600E2FC, 0x26682E29, 0x1F441F73,
+0x1F516D53, 0x8D0B1F15, 0x60632FE2, 0x891F8808,
+0x89538810, 0x8B018818, 0x0009A081, 0x0009A0B9,
+0xEB10A00D, 0x52D37DF0, 0x54D156D2, 0x2E1665D2,
+0x2E662E26, 0x2E427EFC, 0x1FD16153, 0x3CB27CF0,
+0x7D0489F0, 0x1F151FD6, 0x2FE21FC2, 0x1FE4A0A1,
+0x621366E2, 0x42294619, 0x42194618, 0x2E62262B,
+0x7CFF4118, 0xEB10A021, 0x54D37DF0, 0x624357D2,
+0x42194229, 0x55D1212B, 0x2E1666D2, 0x41296173,
+0x41194418, 0x2E46241B, 0x44296453, 0x44194718,
+0x2E76274B, 0x47296763, 0x47194518, 0x257B7EFC,
+0x46182E52, 0x1FD16163, 0x3CB27CF0, 0x7D0389DC,
+0x1F151FD6, 0x2FE21FC2, 0x1FE4A06B, 0x57F56EF2,
+0x627366E2, 0x46284629, 0x262B4229, 0x2E625CF2,
+0x7CFE4728, 0xA01BEB10, 0x7DF05DF1, 0x55D251D3,
+0x46296613, 0x54D1276B, 0x2E7662D2, 0x41286753,
+0x217B4729, 0x61432E16, 0x41294528, 0x2E56251B,
+0x44286523, 0x245B4529, 0x42282E46, 0x7CF06723,
+0x89E23CB2, 0x1FD67D02, 0xA03A1FC2, 0x67F21FE4,
+0x657251F5, 0x45296213, 0x45284519, 0x42194518,
+0x5CF2252B, 0x41282752, 0x7CFD4118, 0xA022EB10,
+0x7DF05DF1, 0x54D256D3, 0x45196563, 0x52D14628,
+0x4618215B, 0x6ED26543, 0x45192716, 0x265B4428,
+0x65436163, 0x45186423, 0x42284419, 0x4218254B,
+0x271664E3, 0x44196623, 0x264B2756, 0x4E282766,
+0x61E34E18, 0x3CB27CF0, 0x7D0189DB, 0x1FC21FD6,
+0xE7041F74, 0x51F45DF2, 0x5EF6A00D, 0x84E27EFC,
+0x620364E0, 0x7DFC84E1, 0x84E36503, 0x21646603,
+0x21542124, 0x3D722144, 0x57F389F0, 0x641366D3,
+0x7F1C65E3, 0x6DF66EF6, 0xA09D6CF6, 0x2F866BF6,
+0x2FA62F96, 0x2FC62FB6, 0x2FE62FD6, 0x614374E0,
+0x6A636873, 0x6B56E920, 0x6C567AE0, 0x6D567120,
+0x6E563A92, 0x64566756, 0x62566656, 0x11C121B2,
+0x11E311D2, 0x11451174, 0x8DEC1166, 0x71201127,
+0x6613A004, 0x7AFF6254, 0x76012620, 0x8BF92AA8,
+0x6EF66083, 0x6CF66DF6, 0x6AF66BF6, 0x000B69F6,
+0x2F8668F6, 0x2FA62F96, 0x2FC62FB6, 0x2FE62FD6,
+0x6A636873, 0x75E0E920, 0x56565257, 0x57545155,
+0x5D525E53, 0x6B525C51, 0x24662426, 0x24762416,
+0x7AE024E6, 0x24C624D6, 0x8DEC3A92, 0x66A324B6,
+0x6EF66783, 0x6CF66DF6, 0x6AF66BF6, 0xA04369F6,
+0x2FE668F6, 0xC8046063, 0x8D046E63, 0x62166153,
+0x24227EFC, 0x60E37404, 0x8908C818, 0x71046513,
+0x62526616, 0x24227EF8, 0xAFF41461, 0xE2047408,
+0x65133E22, 0x66E38D02, 0x6EF6A01C, 0x6EF6AF87,
+0xC8046063, 0x61638D04, 0x625275FC, 0x242671FC,
+0xC8186013, 0x75F88906, 0x66525251, 0x24662426,
+0x71F8AFF6, 0x3122E204, 0x66138F02, 0x0009AFA1,
+0x0009A00A, 0x0009A004, 0x76FF6254, 0x74012420,
+0x8BF92668, 0x6073000B, 0x0009A004, 0x625075FF,
+0x242476FF, 0x8BF92668, 0x6073000B, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x544F0D0A, 0x46205355, 0x00003A57, 0x2072614D,
+0x32203232, 0x20373030, 0x353A3731, 0x37333A32,
+0x00000000, 0x00000D0A, 0x00000043, 0x61766E49,
+0x2064696C, 0x72657375, 0x20726F20, 0x2079656B,
+0x00214449, 0x6E6B6E55, 0x206E776F, 0x6D6D6F63,
+0x3D646E61, 0x00000000, 0x61437748, 0x7262696C,
+0x6F697461, 0x6620206E, 0x0A6C6961, 0x0000000D,
+0x73696F4E, 0x61432065, 0x7262696C, 0x6F697461,
+0x6166206E, 0x21216C69, 0x00000D0A, 0x00000D0A,
+0x00000042, 0x000000FF, 0x00020001, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x010E010D, 0x00020003, 0x01090108,
+0x0002010A, 0x00030002, 0x02020201, 0x02040203,
+0x02060205, 0x02080207, 0x020A0209, 0x020C020B,
+0x020E020D, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x010E010D, 0x00FF010F, 0x01090108,
+0x010B010A, 0x00030002, 0x02020201, 0x02040203,
+0x02060205, 0x02080207, 0x020A0209, 0x020C020B,
+0x020E020D, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00000072, 0x00205220, 0x00000046,
+0x00000059, 0x73204142, 0x003D7165, 0x00000074,
+0x00000000, 0x02000112, 0x40FFFFFF, 0x12210ACE,
+0x20104890, 0x02090100, 0x0101002E, 0x09FA8000,
+0x04000004, 0x000000FF, 0x02010507, 0x07000200,
+0x00028205, 0x05070002, 0x00400383, 0x04050701,
+0x01004003, 0x002E0209, 0x80000101, 0x000409FA,
+0x00FF0400, 0x05070000, 0x00400201, 0x82050700,
+0x00004002, 0x03830507, 0x07010040, 0x40030405,
+0x03040100, 0x030C0409, 0x0079005A, 0x00410044,
+0x03180053, 0x00530055, 0x00320042, 0x0030002E,
+0x00570020, 0x0041004C, 0x0000004E, 0x00000000,
+0x00000000, 0x00000709, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, };
+
+const u32_t zcFwImageSize=11204;
diff --git a/drivers/staging/otus/hal/hpfwu_OTUS_RC.c b/drivers/staging/otus/hal/hpfwu_OTUS_RC.c
new file mode 100644
index 00000000000..089d3e0ad85
--- /dev/null
+++ b/drivers/staging/otus/hal/hpfwu_OTUS_RC.c
@@ -0,0 +1,715 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include "cprecomp.h"
+
+const u32_t zcFwImage[] = {
+0x0009000B, 0x4F222FE6, 0xDE287FFC, 0xE114D728,
+0x1E13D428, 0x1E4C470B, 0x0009B018, 0xA0039543,
+0x3652E600, 0x76018D04, 0xC84060E2, 0x2F028DF9,
+0xDE22D421, 0x00094E0B, 0x4E0BD421, 0xD4210009,
+0x00094E0B, 0x4F267F04, 0x6EF6A022, 0xD11E4F22,
+0x0009410B, 0x440BD41D, 0xD51D0009, 0x0009450B,
+0xE1FFD71C, 0xD21C611D, 0x50292712, 0xCB01E1FF,
+0xD61BD41A, 0x22121209, 0xE5011211, 0x2452E200,
+0xD5182622, 0x970FD618, 0x4F262572, 0x2620000B,
+0xDD17DC16, 0x4C0BDE17, 0x4D0B0009, 0x4E0B0009,
+0xAFF80009, 0x27100009, 0x00000640, 0x001C001C,
+0x002008EA, 0x0000B38E, 0x002028DC, 0x00200DA6,
+0x002028E8, 0x00202900, 0x00200C6C, 0x00200EA2,
+0x00200940, 0x001C3510, 0x001C3624, 0x001E212C,
+0x00202894, 0x0020288C, 0x002027F0, 0x00200B68,
+0x00201F74, 0x00201734, 0x2FD62FC6, 0x4F222FE6,
+0xDEA17FA4, 0x61E0E01C, 0x7D016DE3, 0x61D00F14,
+0xD59FD49E, 0x450BE020, 0xE0200F14, 0xE78004FC,
+0x604C66E2, 0x7D7F677C, 0x1F693070, 0x2D628F17,
+0x01FCE01C, 0x641CE500, 0xD797DE96, 0x3243625D,
+0xA21A8B01, 0x655D0009, 0x31EC6153, 0xE0286C10,
+0x6D530FC4, 0x3D7C62CE, 0xAFEF2D20, 0x20087501,
+0xE01C8B15, 0xE50001FC, 0xD78BDE8A, 0x641CA00A,
+0x6C53655D, 0x66C23CEC, 0x66626253, 0x2262327C,
+0x1F697504, 0x3243625D, 0xA1F68BF2, 0x88012D10,
+0xE01C8B16, 0xE40001FC, 0x671C2D40, 0x624DDE7D,
+0x8B013273, 0x0009A1E9, 0x62E3644D, 0x72046D43,
+0x3DEC6143, 0x65D2312C, 0x74086C12, 0x25C2AFEF,
+0x8B188804, 0x01FCE01C, 0x2D40E400, 0xDE71671C,
+0x3273624D, 0xA1D08B01, 0x644D0009, 0x62E36D43,
+0x65D23DEC, 0x61437204, 0x6612312C, 0x74086C52,
+0xAFED2C69, 0x880525C2, 0xE01C8B18, 0xE40001FC,
+0x671C2D40, 0x624DDE63, 0x8B013273, 0x0009A1B5,
+0x6C43644D, 0x3CEC62E3, 0x720465C2, 0x3D2C6D43,
+0x615266D2, 0x216B7408, 0x2512AFED, 0x8B138830,
+0xE200DE58, 0x64E22D20, 0x8B042448, 0x420BD257,
+0xA19A0009, 0x55E10009, 0x57E356E2, 0xDD545CE4,
+0x2FC64D0B, 0x7F04A191, 0x89018828, 0x0009A0EA,
+0xE143DE4C, 0x622D62E1, 0x8F033217, 0x56FB1FEB,
+0x2621E240, 0x8B013217, 0x0009A0D5, 0xE1015EFB,
+0x301685E1, 0xA0CE8B01, 0xE4010009, 0x2D4055FB,
+0x6451B179, 0xE14357FB, 0xE0546271, 0x3517652D,
+0x0F568D41, 0x3563E640, 0xE6008B05, 0x0F65E034,
+0xA00FE11A, 0x615372C0, 0x41214121, 0x41214121,
+0x45214121, 0x45214521, 0xC9036053, 0xE0346603,
+0x71180F65, 0x2209E007, 0x641DE030, 0x0F2565F3,
+0x1F4EB1F1, 0x04FDE034, 0x674DE030, 0x47080CFD,
+0x607361CD, 0x4108D22B, 0xE00F0CFE, 0x1F1F420B,
+0x2CD96D07, 0x5EFB6073, 0x85E20FC6, 0x420B51FF,
+0x2C0B600D, 0x54FE6073, 0xB1BB0FC6, 0xE05465F3,
+0x652D62E1, 0xE6400F56, 0x89623563, 0xE050E100,
+0x60230F15, 0x4008C903, 0x6D034000, 0xE0406103,
+0xE0440FD6, 0xD217EEFF, 0x6EEC0FF6, 0x0F26E058,
+0x60E3420B, 0x42216253, 0x42214221, 0x66234221,
+0x326C4200, 0x45214200, 0xE0486707, 0x0F764521,
+0xC9036053, 0x40085CFB, 0x7C0630FC, 0x6E036D2D,
+0x1FD51FC6, 0x1F04A02E, 0x00117D00, 0x00202904,
+0x00200DA6, 0x00117D04, 0x00117D84, 0x00200700,
+0x0020074C, 0x00201FD4, 0x0FD6E04C, 0x05FEE044,
+0x64D3B189, 0x64E2E048, 0xE04006FE, 0x2E422469,
+0x01FE67C4, 0x667CE058, 0x420B02FE, 0x240B6063,
+0x05FEE044, 0xB15D2E42, 0xE05064D3, 0x7D0101FD,
+0x0F157101, 0x02FDE050, 0x3262E606, 0x56FB8BDC,
+0x55FB6261, 0x85514200, 0x302C750C, 0x6103701B,
+0x64F3E600, 0xE704A004, 0x76016256, 0x74042422,
+0x3273626D, 0x65F38BF8, 0x641DB13C, 0xB0D256FB,
+0xA0AA6461, 0xD4880009, 0xE201D588, 0x2D20450B,
+0x0009A0A3, 0x8B078829, 0xE200DE85, 0x66E22D20,
+0x646DB0A1, 0x0009A099, 0x622CE281, 0x8B3D3020,
+0xD680E738, 0xE0442D70, 0xE0480C6E, 0x6E621DC1,
+0x51611DE2, 0x54621D13, 0x55651D44, 0x57631D55,
+0x5C661D76, 0x0E6E1DC7, 0x1DE8E040, 0xE050016E,
+0x54641D19, 0x056E1D4A, 0x1D5BE04C, 0xE054076E,
+0x0C6E1D7C, 0x1DCDE058, 0xE044026E, 0xED001D2E,
+0xE04806D6, 0x16D126D2, 0x16D516D2, 0x16D616D3,
+0xE04006D6, 0xE05006D6, 0x06D616D4, 0x06D6E04C,
+0x06D6E054, 0x06D6E058, 0x1F29A057, 0x622CE282,
+0x89313020, 0x05FCE020, 0x625CE683, 0x3260666C,
+0xD65D8B07, 0x2650E500, 0x52617680, 0xA044D65B,
+0xE6902622, 0x3260666C, 0xD2578B16, 0xE500D658,
+0x60622250, 0xCB20D257, 0xE6052602, 0xD6562262,
+0x2252460B, 0x420BD255, 0xD2550009, 0x2262E601,
+0x4618D254, 0x2262A029, 0xD254D453, 0xD4546542,
+0x0009420B, 0x0009A021, 0xE524D647, 0xD5452650,
+0x16215257, 0x16225258, 0x16235259, 0x1624525A,
+0x1625525B, 0x1626525C, 0x1627525D, 0x1628525E,
+0x1F29525F, 0xE2001629, 0x15281527, 0x152A1529,
+0x152C152B, 0x152E152D, 0x7F5C152F, 0x6EF64F26,
+0x000B6DF6, 0x4F226CF6, 0xE240614D, 0x89173123,
+0x3127E21F, 0xD43B8908, 0xE001D53B, 0x6642450B,
+0x26796707, 0x2462A00C, 0x3127E23F, 0xD7358908,
+0x71E0D635, 0x460BE001, 0x62075571, 0x17512529,
+0x000B4F26, 0x4F220009, 0xE240614D, 0x89153123,
+0x3127E21F, 0xD42B8907, 0x6642D22B, 0xE001420B,
+0xA00B260B, 0xE23F2462, 0x89073127, 0xD626D725,
+0x71E05571, 0xE001460B, 0x1751250B, 0x000B4F26,
+0xE6400009, 0x46284618, 0x6252D520, 0x89FC2268,
+0x0009000B, 0x4618E680, 0xD51C4628, 0x22686252,
+0x000B89FC, 0xA0010009, 0x7201E200, 0x8BFC3242,
+0x0009000B, 0x4618E680, 0xD5154628, 0x22686252,
+0x000B8BFC, 0x00000009, 0x00202908, 0x00200DA6,
+0x00117D04, 0x002027F8, 0x00117D80, 0x0020288C,
+0x001C3500, 0x001D4004, 0x00200EA2, 0x00200940,
+0x001E212C, 0x001C3D28, 0x00117D00, 0x00200E06,
+0x00202920, 0x001C3704, 0x00201FD4, 0x001C373C,
+0x001C3700, 0x4F222FE6, 0x6E537FFC, 0x2F42BFCA,
+0xD61561E2, 0x1615E280, 0x421854E1, 0x55E21646,
+0x16574228, 0x6EF257E3, 0x2E2B1678, 0x7F0426E2,
+0xAFA74F26, 0x2FC66EF6, 0x2FE62FD6, 0xDD0A4F22,
+0xBFAF6C53, 0xBF946E43, 0xBFAB2DE2, 0x51D50009,
+0x54D62C12, 0x55D71C41, 0x56D81C52, 0x4F261C63,
+0x6DF66EF6, 0x6CF6000B, 0x001C370C, 0x0009A0C0,
+0xD17B4F22, 0xD47B92B6, 0x2122B00D, 0x97B2E605,
+0xB02295B2, 0xB0366463, 0xB0360009, 0xB0390009,
+0xA0680009, 0x4F124F26, 0xD1734F02, 0x94A43145,
+0x4609060A, 0x46094609, 0x00293646, 0xD76CD56F,
+0x2500CA01, 0x4F062762, 0x4F16000B, 0xBFEA4F22,
+0xB01F0009, 0xA04E0009, 0x2FE64F26, 0x6E63D168,
+0x44186612, 0x4528928A, 0x26294408, 0x44084500,
+0x4400265B, 0x4708264B, 0x47082162, 0x27EBD161,
+0x000B2172, 0x000B6EF6, 0xD25F0009, 0xE40A9677,
+0x2262AFB4, 0x2FC62FB6, 0x2FE62FD6, 0xDC5B4F22,
+0x2C22E201, 0xBFA9E40A, 0x60C27C44, 0xCB01ED00,
+0x60C22C02, 0xC901EB64, 0x6E03A008, 0x89073DB2,
+0xE40160C2, 0xBF99C901, 0x7D016E03, 0x8BF52EE8,
+0x8B033DB2, 0xD24FD44E, 0x0009420B, 0x4F26E40A,
+0x6DF66EF6, 0xAF896CF6, 0x44116BF6, 0x604B8F01,
+0x000B6043, 0x2FB60009, 0x2FD62FC6, 0x4F222FE6,
+0xDC457FFC, 0x60C2ED00, 0xCB02EB64, 0x60C22C02,
+0xC9022F02, 0x6E03A009, 0x89083DB3, 0xE46460C2,
+0xC9022F02, 0x6E03BF6A, 0x2EE87D01, 0xD73B8BF4,
+0x617251C1, 0xDE3BDC3A, 0xD23CD13B, 0x64C23DB3,
+0x651264E2, 0x65228F09, 0xD232D439, 0x4F267F04,
+0x6DF66EF6, 0x422B6CF6, 0x7F046BF6, 0x6EF64F26,
+0x6CF66DF6, 0x6BF6000B, 0x5651D532, 0x46286052,
+0x306C000B, 0x5288096C, 0x09B45BB4, 0x03C41FFF,
+0x2FC62FB6, 0x2FE62FD6, 0x4F124F22, 0xBFEB4F02,
+0x6B036E43, 0xDD18DC28, 0x0009BFE6, 0x3C0530B8,
+0x4609060A, 0x46014609, 0x020A3D65, 0x42094209,
+0x32E24209, 0x4F068BF0, 0x4F264F16, 0x6DF66EF6,
+0x000B6CF6, 0x2FE66BF6, 0xDE1C4F22, 0xE500E102,
+0x2E12E403, 0x2E52BFD4, 0x4618E606, 0xE403E700,
+0x2E722E62, 0xAFCB4F26, 0x000B6EF6, 0x00000009,
+0x00202890, 0x0024CDE0, 0x10624DD3, 0x00202A8C,
+0x001C5814, 0x001C59D0, 0x001C5804, 0x001C581C,
+0x00202934, 0x00200DA6, 0x001C5860, 0x001C6864,
+0x001C7864, 0x001C59BC, 0x001C69BC, 0x001C79BC,
+0x0020294C, 0x001C1040, 0xCCCCCCCD, 0x001D4004,
+0x2F962F86, 0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6,
+0xE4007FE4, 0x4528E510, 0x67436C43, 0xE107A00F,
+0x6043644D, 0x0F564008, 0xEE0060C3, 0x815125C1,
+0x81538152, 0x157315E2, 0x751415E4, 0x624D7401,
+0x8BED3213, 0xDA6F51F1, 0x1A1154F2, 0xD16E2A12,
+0x57F455F3, 0x6DF258F5, 0x1141D96C, 0x11532142,
+0x11751152, 0x11871174, 0x52F61186, 0x19D1D668,
+0xD86829D2, 0xDA68E950, 0x1621EBB4, 0x6BBC2622,
+0xA0214908, 0x6EEDEE00, 0x61E36DE3, 0x41084D08,
+0x31EC3DEC, 0x41084D08, 0x60C33D8C, 0xE7904108,
+0x81D12DC1, 0x41086093, 0x81D2677C, 0x31AC60C3,
+0x3472E200, 0x1DD281D3, 0xD4551D13, 0x1D248D01,
+0xB03AD450, 0x7E0165D3, 0x34B264ED, 0xD14D8BDB,
+0x6512DB52, 0x4529D24D, 0x64121B51, 0x674DD14A,
+0x67222B72, 0x4729D64E, 0x69221B73, 0x689D2FD2,
+0x69121B82, 0x5A122692, 0x5B1416A2, 0x16B4DA44,
+0x16C65C16, 0x16EA6EA2, 0x4F267F1C, 0x6DF66EF6,
+0x6BF66CF6, 0x69F66AF6, 0x68F6000B, 0x60616642,
+0x8D04C803, 0x6061E500, 0x8802C903, 0x52628B03,
+0x51246563, 0x000B2412, 0x2FD66053, 0x4F222FE6,
+0x6E537FEC, 0xE5506253, 0xE4006D43, 0xA0014508,
+0x5224E101, 0x22116043, 0x81238121, 0x81226053,
+0x362056E2, 0xD22F8BF5, 0x64F316E4, 0x420BE614,
+0x65E165E3, 0x2549E4FC, 0x61F12E51, 0x214965F3,
+0x54D12F11, 0x410BD127, 0x57D1E614, 0xCB016071,
+0x1DE12701, 0x4F267F14, 0x000B6EF6, 0x2FD66DF6,
+0x4F222FE6, 0x6E537FEC, 0xE5FC6653, 0x60616D43,
+0xCB012059, 0x52E22601, 0x8B063260, 0x51E212E4,
+0x8B0431E0, 0xA00252D1, 0xAFF01E22, 0xD2155664,
+0xE61464F3, 0x65E3420B, 0xE1FC67E1, 0x2E712719,
+0x54D167F1, 0xD10F2719, 0xE61465F3, 0x2F71410B,
+0x602152D1, 0x2201CB01, 0x7F141DE1, 0x6EF64F26,
+0x6DF6000B, 0x0020285C, 0x00202864, 0x00202854,
+0x00202884, 0x0010008C, 0x00100EC0, 0x001E2108,
+0x001C3D00, 0x00202134, 0x2FC62FB6, 0x2FE62FD6,
+0xD6314F22, 0x60D36D62, 0x894DC803, 0xDB30DC2F,
+0x0009A02C, 0xC9036061, 0x892B8801, 0xD22DD42B,
+0x0009420B, 0x65035603, 0xC8208561, 0xE0508903,
+0x720102BE, 0x85620B26, 0x4000600D, 0x4000366A,
+0x40004624, 0x206D4624, 0xD423C903, 0x40086E03,
+0xD1224000, 0x340C410B, 0x61E3D521, 0xD721E001,
+0x450BD221, 0x64E37E30, 0x2702420B, 0x66C252C1,
+0x8BCF3620, 0x4E18EE01, 0xA011DB1C, 0x6061EC75,
+0x8801C903, 0xD4198910, 0x460BD612, 0xD4180009,
+0x470BD718, 0xD2136503, 0x64C3D113, 0x22E2410B,
+0x66B252B1, 0x8BEA3620, 0xC80460D3, 0xD2128906,
+0x6EF64F26, 0x6CF66DF6, 0x6BF6422B, 0x6EF64F26,
+0x6CF66DF6, 0x6BF6000B, 0x001E2100, 0x0020285C,
+0x002027F8, 0x00200A5C, 0x00202864, 0x00200ADE,
+0x00201FD4, 0x001C3D30, 0x00200D6C, 0x00202854,
+0x00202884, 0x00200A7A, 0x002000F8, 0xE601D237,
+0x1265D537, 0x000B2252, 0xD6361266, 0x88016062,
+0xE1018B62, 0xD5342612, 0x5451D134, 0xE0406212,
+0x2122324C, 0x54115752, 0x1141347C, 0x57125453,
+0x1172374C, 0x52135755, 0x1123327C, 0x56146452,
+0x1164364C, 0x54155754, 0x1145347C, 0x56165458,
+0x1166364C, 0x6762D626, 0x327C5217, 0x57611127,
+0x327C5218, 0x57621128, 0x327C5219, 0x57631129,
+0x347C541A, 0x5764114A, 0x347C541B, 0x5765114B,
+0x347C541C, 0x5266114C, 0x372C571D, 0x5267117D,
+0x342C541E, 0x5268114E, 0x362C561F, 0xD615116F,
+0x041E6262, 0x342C7694, 0xE0440146, 0x061E6262,
+0x0166362C, 0x525CE048, 0xD60F051E, 0x0156352C,
+0xE0546262, 0x4229051E, 0x0156352C, 0xE0585561,
+0x4529061E, 0x0166365C, 0x0009000B, 0x001C1010,
+0x0000C34F, 0x001C1028, 0x001C369C, 0x002027F8,
+0x001C3CA0, 0x001C36F4, 0x001C3B88, 0xD62F7FFC,
+0x2642644C, 0xC8205066, 0x2F028DFC, 0x7F04000B,
+0x2FD62FC6, 0x4F222FE6, 0x6D436C53, 0xEE00A004,
+0x7E0164D4, 0x644CBFEA, 0x8BF93EC2, 0x6EF64F26,
+0x000B6DF6, 0xA0016CF6, 0x76016643, 0x22286260,
+0x36488BFB, 0x6563AFE4, 0x2FB62F96, 0x2FD62FC6,
+0x4F222FE6, 0xEC1CED08, 0xDB196E53, 0x61C3E90A,
+0x60434B0B, 0x3092C90F, 0x66038D02, 0x7630A001,
+0x4D107637, 0x7E012E60, 0x7CFC8FF1, 0x8058E000,
+0x6EF64F26, 0x6CF66DF6, 0x000B6BF6, 0x000B69F6,
+0x000BE000, 0x2FE6E000, 0x7FEC4F22, 0x6E436253,
+0xBFD165F3, 0xBFC66423, 0xBFC464E3, 0xD40564F3,
+0x0009BFC1, 0x4F267F14, 0x6EF6000B, 0x001C0004,
+0x00202094, 0x00202968, 0xE110D59C, 0xE6406050,
+0x2500C9FD, 0xE0FF75E9, 0x80516453, 0x80538052,
+0x80568055, 0x251075EF, 0xE1EF6250, 0x2219E001,
+0xE7202520, 0x24608052, 0x2570000B, 0xE4FDD590,
+0xE7026152, 0x25122149, 0x74016052, 0x2502CB01,
+0xD18C6652, 0x25622649, 0x92C26012, 0x2102CB08,
+0xC9CF6012, 0x60122102, 0x2102CB03, 0x000B1172,
+0x4F221123, 0xE100D484, 0xD285D784, 0xD5852410,
+0x2711D485, 0x2211E700, 0xBFBD2511, 0xD5832471,
+0x2560E600, 0x4F26AFD2, 0xD281664C, 0x362C4600,
+0xCB106060, 0x2600000B, 0xD27D654C, 0x352C4500,
+0xE1EF6650, 0x000B2619, 0x664C2560, 0x4600D279,
+0x6060362C, 0x000BCB10, 0x654C2600, 0x4500D275,
+0x6650352C, 0x2619E1EF, 0x2560000B, 0xD270664C,
+0x362C4600, 0xCB086060, 0x2600000B, 0xD26C654C,
+0x352C4500, 0xE1F76650, 0x000B2619, 0x664C2560,
+0x4600D268, 0x6060362C, 0x000BCB08, 0x654C2600,
+0x4500D264, 0x6650352C, 0x2619E1F7, 0x2560000B,
+0xD65F624C, 0x326C4200, 0xC9086020, 0x40214021,
+0x000B4021, 0x624C600C, 0x4200D65A, 0x6020326C,
+0x4021C908, 0x40214021, 0x600C000B, 0xD156644C,
+0x341C74FF, 0x000B6240, 0xD154602C, 0x341C644C,
+0x000B6240, 0x2FE6602C, 0x655C4F22, 0x3567E60A,
+0x6E438D15, 0x6453BFEA, 0x60EC640C, 0x8B028801,
+0xA002E00F, 0x44092409, 0x624C4409, 0x3263E60A,
+0xBFE28905, 0x620C644C, 0xC8806023, 0xE2008B00,
+0x4F266023, 0x6EF6000B, 0xD6414F22, 0x88016062,
+0xB2228B03, 0xA0030009, 0xD23E0009, 0x2260E640,
+0xE200D63D, 0x000B4F26, 0x4F222622, 0x6062D638,
+0x8B018802, 0x0009B26C, 0xE200D637, 0x000B4F26,
+0x0FFF2622, 0xD433D532, 0xE701E100, 0x000B2512,
+0xD2302470, 0x000BE604, 0xD5202260, 0x6150E4FD,
+0x2149D62E, 0x2510E700, 0x2670000B, 0xE4FBD51B,
+0x22496250, 0x2520000B, 0xE4F7D518, 0x22496250,
+0x2520000B, 0xD2264F22, 0x600D8522, 0x89112008,
+0x89138801, 0x89158803, 0x89178805, 0x89418806,
+0x89478808, 0x894D8809, 0x8953880A, 0x8959880B,
+0x0009A060, 0x0009B062, 0x600CA05D, 0x0009B070,
+0x600CA059, 0x0009B07A, 0x600CA055, 0x6260D606,
+0x8B4F2228, 0x0009B086, 0x600CA04D, 0x001E1028,
+0x001E2148, 0x001E1108, 0x002028D9, 0x002028C8,
+0x002028CA, 0x002028CC, 0x002028AC, 0x001E1008,
+0x001E103F, 0x001E105F, 0x001E1030, 0x001E1090,
+0x002028D4, 0x001E100B, 0x002028D0, 0x002028D8,
+0x002028A0, 0x6260D687, 0x8B232228, 0x0009B06A,
+0x600CA021, 0x6260D683, 0x8B1B2228, 0x0009B0B4,
+0x600CA019, 0x6260D67F, 0x8B132228, 0x0009B0BA,
+0x600CA011, 0x6260D67B, 0x8B0B2228, 0x0009B11E,
+0x600CA009, 0x6260D677, 0x8B032228, 0x0009B136,
+0x600CA001, 0x4F26E000, 0x0009000B, 0xD273D172,
+0xD5738412, 0x4000C90F, 0xD772012D, 0x611CE403,
+0xD671E20F, 0x27122540, 0xE0012520, 0x2602000B,
+0xE601D269, 0x30668523, 0xE0008D06, 0xE000D267,
+0x8122D669, 0x2602E001, 0x0009000B, 0x8523D262,
+0x2008600D, 0x88018905, 0xD6648B0A, 0xCB016060,
+0xD6612600, 0xE101D45D, 0x2612E001, 0x8142000B,
+0xE000000B, 0xE501D158, 0x45188513, 0x3453640D,
+0x8D056603, 0xD25AE000, 0xE001D557, 0x25022260,
+0x0009000B, 0xD1504F22, 0x650D8513, 0x44196453,
+0x672E6249, 0x602C227D, 0x89098801, 0x890C8802,
+0x89108803, 0x89268806, 0x89298807, 0x0009A038,
+0xD64DD54C, 0xA027E212, 0x625C2652, 0x8B2F2228,
+0xA01ED64A, 0x605C6262, 0x89052008, 0x89088810,
+0x890B8820, 0x0009A024, 0xD643D445, 0xA013E204,
+0xD7442642, 0xE20CD640, 0x2672A00E, 0xD63ED542,
+0xA009E218, 0xD4412652, 0xE20AD63B, 0x2642A004,
+0xD639D23F, 0xE22E2622, 0xD43E8515, 0x3277670D,
+0x8F012421, 0x24516503, 0x0009B0DF, 0xE001A001,
+0x4F26E000, 0x0009000B, 0xE101D629, 0x2610D436,
+0xD7286541, 0x655DD128, 0xE001E20F, 0x26202752,
+0x2102000B, 0x4F222FE6, 0x8523D21F, 0x2448640C,
+0xD62D8B08, 0xE200D521, 0x84512621, 0x20499430,
+0x8051A026, 0x60E0DE1D, 0x8D0BC840, 0x3427E201,
+0xD1258922, 0x420BD225, 0xD5252141, 0xCB046052,
+0x2502A00B, 0x89173427, 0xD722D21F, 0x2241470B,
+0xE5FBD61F, 0x21596162, 0x84E12612, 0xB12DCB80,
+0x60E080E1, 0xCB04D61C, 0x60602E00, 0x2600C93F,
+0xE001D609, 0x2602A001, 0x4F26E000, 0x6EF6000B,
+0x0000FF7F, 0x002028D9, 0x002028A0, 0x002028AC,
+0x001E1100, 0x001E100C, 0x002028D0, 0x001E1000,
+0x001E1001, 0x00202A90, 0x002028B4, 0x002028BC,
+0x00202AFE, 0x00202B02, 0x00202B0E, 0x00202B26,
+0x00202B30, 0x002028B8, 0x002028C6, 0x00201A32,
+0x001E1108, 0x00201B3E, 0x001E1015, 0x6060D696,
+0x8919C880, 0x6021D295, 0x8B158801, 0xE501D294,
+0x30568524, 0xD1938910, 0xD493E203, 0x65412120,
+0x655DE00B, 0xD5910656, 0xE702E40F, 0x25712140,
+0xE001D78F, 0x2702000B, 0xE000000B, 0x4F222FE6,
+0x84E1DE8C, 0x8934C880, 0x8554D585, 0x8F302008,
+0xD7896103, 0x66728553, 0x650C6403, 0x620C8566,
+0x8B263520, 0xD780D685, 0x644C651C, 0x27412651,
+0xC84060E0, 0xD2828907, 0x0009420B, 0x6062D681,
+0xA008CB04, 0xD1802602, 0x0009410B, 0xE5FBD67D,
+0x24596462, 0xB0A12642, 0xD5750009, 0x2522E201,
+0xD77A60E0, 0x2E00CB04, 0xC93F6070, 0xA0012700,
+0xE0006023, 0x000B4F26, 0x2FA66EF6, 0x2FC62FB6,
+0x2FE62FD6, 0xE240DA69, 0xDC6666A1, 0x3123616D,
+0x62638900, 0x6ED36D2C, 0x4E2136D8, 0x4E212A61,
+0xDB6CD46B, 0xE700A00F, 0x770166B2, 0x71026163,
+0x65612B12, 0x71026613, 0x62612B12, 0x622D655D,
+0x325C4228, 0x627C2422, 0x8BED32E3, 0xC90360D3,
+0x8B108803, 0xED076EB2, 0x710261E3, 0x67132B12,
+0x62E17102, 0x65712B12, 0x655D622D, 0x352C4528,
+0xA00C2CD0, 0x88022452, 0xA0038B01, 0x8801E203,
+0xE2018B05, 0x66B22C20, 0x677D6761, 0xEB0F2472,
+0x6DA12CB0, 0x8B052DD8, 0xD445D24F, 0xE101EE00,
+0x241222E2, 0x6DF66EF6, 0x6BF66CF6, 0x6AF6000B,
+0x2FE62FD6, 0xE240DD3D, 0x616D66D1, 0x89003123,
+0x672C6263, 0xDE433678, 0x2D617703, 0xD6404721,
+0x472164E2, 0xE100A00E, 0x71016562, 0x24506253,
+0x42197401, 0x74012420, 0x24504529, 0x45197401,
+0x74012450, 0x3273621C, 0x42008BEE, 0x64D166E2,
+0x362C4200, 0x8F062448, 0xDD332E62, 0xE500DE28,
+0x2D52E701, 0x6EF62E72, 0x6DF6000B, 0x2FE62FD6,
+0xEE014F22, 0xED0AA005, 0x64E3BCB6, 0x64E3BCBC,
+0x62EC7E01, 0x8BF732D7, 0xEE01A005, 0x64E3BCBD,
+0x64E3BCC3, 0x62EC7E01, 0x8BF732D7, 0x6EF64F26,
+0x6DF6000B, 0x2FE62FD6, 0x7FFC4F22, 0x6060D61F,
+0x89758801, 0xE101D41E, 0xD7128548, 0x650D2610,
+0x45196070, 0x6659DD1B, 0x61D3626E, 0xC840262D,
+0x74027102, 0x8D47D718, 0xD218666C, 0xE501DE0A,
+0xA0312E22, 0x0000EE04, 0x001E1001, 0x002028C6,
+0x002028A0, 0x001E1100, 0x002028CA, 0x002028B8,
+0x002028D0, 0x001E1000, 0x002028BC, 0x002028C8,
+0x00201A32, 0x001E1108, 0x00201B3E, 0x001E1015,
+0x001E100C, 0x002028B4, 0x002028D4, 0x002028D8,
+0x00202A90, 0x00202B26, 0x00202B32, 0x00202AA2,
+0x75016245, 0x71022121, 0x32E3625C, 0x60638BF8,
+0xE60181D4, 0xE417D538, 0x3243626C, 0x6255891E,
+0x27217601, 0x7702AFF8, 0xDE35D234, 0x2E22E501,
+0xEE04A004, 0x75016245, 0x71022121, 0x32E3625C,
+0x60638BF8, 0xE60181D4, 0xA004D52E, 0x6255E417,
+0x27217601, 0x626C7702, 0x8BF83243, 0x2D21924B,
+0xD72AD429, 0x2F126142, 0x6DF265F2, 0xC9806053,
+0x60532700, 0x6103C960, 0x60538071, 0x65F26EF2,
+0x4D19C903, 0x80724529, 0x451960DC, 0x4E298172,
+0x62EC605C, 0x302C4018, 0x6D428173, 0x2FD22118,
+0x62F26EF2, 0x421966F2, 0x656C4629, 0x602C66F2,
+0x401864EC, 0x304C4629, 0x81744619, 0x4018606C,
+0x8F07305C, 0xBCB58175, 0x620C0009, 0x89082228,
+0x0009A00A, 0x88406013, 0xB00A8B03, 0xA0030009,
+0xD60B0009, 0x2622E202, 0x4F267F04, 0x000B6EF6,
+0x000B6DF6, 0x060A0009, 0x00202AD2, 0x00202AD0,
+0x002028BC, 0x00202AA4, 0x001E100C, 0x002028A0,
+0x002028D0, 0x7FFC4F22, 0x6620D27E, 0x8D082668,
+0xD47D2F60, 0x420BD27D, 0x64F00009, 0xA0907F04,
+0x7F044F26, 0x000B4F26, 0x000B0009, 0x2FE60009,
+0xDE774F22, 0x60E0D677, 0xCBC0D477, 0x62602E00,
+0xC803602C, 0x40218904, 0x70014021, 0x6603A002,
+0x66034009, 0xD671616D, 0xE500A004, 0x75016262,
+0x74042422, 0x3213625D, 0xD16D8BF8, 0x0009410B,
+0xE401D66C, 0x84E22641, 0x80E2C9BF, 0x000B4F26,
+0x2FE66EF6, 0xD5687FFC, 0x6250DE61, 0x642C84E2,
+0xCB407404, 0x80E2614D, 0x44216413, 0xD7634421,
+0xE600A004, 0x76016256, 0x27222F22, 0x3243626D,
+0x60138BF8, 0x2008C903, 0x88038912, 0x88028905,
+0x88018906, 0xA0088907, 0xE0070009, 0x8078A005,
+0xA002E003, 0xE0018078, 0x62528078, 0x27222F22,
+0xD650E00F, 0x60618078, 0x8B018801, 0x2621E200,
+0x6060D64F, 0x2600CB08, 0xC93F60E0, 0x7F042E00,
+0x6EF6000B, 0x6021D247, 0x8D188801, 0xD2466143,
+0x22106053, 0x60638021, 0xD4468121, 0xE500A007,
+0x027C605D, 0x364C6603, 0x26207001, 0x625D6503,
+0x3213611C, 0xD6408BF4, 0xC9BF6060, 0x000B2600,
+0x2FD60009, 0x4F222FE6, 0x60437FFC, 0x8D02C820,
+0xBF6A6E43, 0x60E30009, 0x8901C810, 0x0009BF67,
+0xC84060E3, 0xBF8C8901, 0x60E30009, 0x8929C801,
+0x60D0DD32, 0x8D03C802, 0xD6312F00, 0x0009460B,
+0xC80460F0, 0xD62F8902, 0x0009460B, 0x602362F0,
+0x8902C880, 0xC97F60D0, 0x60232D00, 0x8902C801,
+0x420BD229, 0xD5290009, 0x88026052, 0xD2288B03,
+0xA005E604, 0x88012260, 0xD2258B02, 0x2260E601,
+0x2522E200, 0xC88060E3, 0xD2228916, 0x60E36E20,
+0x8902C802, 0x420BD220, 0x60E30009, 0x8902C804,
+0x420BD21E, 0x60E30009, 0x8905C808, 0x7F04D21C,
+0x6EF64F26, 0x6DF6422B, 0x4F267F04, 0x000B6EF6,
+0x00006DF6, 0x001E1020, 0x0020296C, 0x00200DA6,
+0x001E1015, 0x001E10BF, 0x00117D00, 0x001E10FC,
+0x002000F8, 0x002028CC, 0x00117D80, 0x001E10F8,
+0x001E10AE, 0x00117D84, 0x001E1017, 0x001E1021,
+0x00200FD8, 0x00200FFA, 0x00201584, 0x002028D0,
+0x001E100B, 0x001E1028, 0x0020102A, 0x0020103C,
+0x00201048, 0xD6A8644C, 0x346C74FF, 0x2450000B,
+0x644CD6A6, 0x000B346C, 0xD6A52450, 0x346C644C,
+0x2450000B, 0x616D625C, 0x41194208, 0x60194208,
+0x644C4200, 0x324C670E, 0x207DD19E, 0xC90F4200,
+0x000B321C, 0x67632200, 0x4208625C, 0x42004208,
+0x324C644C, 0x4200D198, 0x000B321C, 0x2FE62270,
+0x614C4F12, 0x4100D493, 0x6710314C, 0x2729E29F,
+0x65736E53, 0x4719676D, 0x672E6279, 0x4221227D,
+0x42214221, 0x7601662C, 0xE4014608, 0x34E84608,
+0x644C4600, 0x0E1A0467, 0x215025EB, 0x000B4F16,
+0x4F226EF6, 0xD2857FE8, 0x88016021, 0xD2848B7B,
+0x26686621, 0xD2838B77, 0x26686621, 0xE50F8B73,
+0xE401BFA0, 0xBFA3E501, 0xE586E400, 0xE400655C,
+0x2F50BFA3, 0xBFA0E401, 0xE602E506, 0x60634618,
+0x81F2E401, 0x6543BF9E, 0xE40185F2, 0xBFAA6543,
+0x85F26603, 0x6543E401, 0x6603BFB1, 0xE40265F0,
+0x6053756C, 0x80F8BF7E, 0xBF81E402, 0x84F8E512,
+0x7090E402, 0x6503BF81, 0x4618E602, 0x81F66063,
+0xBF7FE402, 0x85F6E500, 0x6603E402, 0xE500BF8B,
+0xE40285F6, 0xBF926603, 0xE5FEE500, 0xE010655C,
+0xBF5FE403, 0xE5130F54, 0xE40EBF62, 0x05FCE010,
+0xBF62E40E, 0xE5007585, 0xBF63E403, 0xE500E640,
+0xBF70E403, 0xE500E640, 0xBF78E403, 0xE5FFE640,
+0xE014655C, 0xBF45E404, 0xE40F0F54, 0xE504BF48,
+0x05FCE014, 0xBF48E40F, 0xE5017584, 0xBF49E640,
+0xE501E404, 0xBF56E640, 0xE501E404, 0xE404E640,
+0xAF5C7F18, 0x7F184F26, 0x000B4F26, 0x4F220009,
+0xD2427FF0, 0x88016021, 0xD2418B71, 0x26686621,
+0xD2408B6D, 0x26686621, 0xE50F8B69, 0xE401BF1A,
+0xBF1DE501, 0xE586E400, 0xE400655C, 0x2F50BF1D,
+0xBF1AE401, 0xE401E506, 0xBF1B6543, 0xE401E640,
+0xBF286543, 0xE401E640, 0xBF306543, 0x65F0E640,
+0x756CE402, 0xBEFD6053, 0xE40280F4, 0xE512BF00,
+0xE40284F4, 0xBF007090, 0xE6406503, 0xBF01E402,
+0xE640E500, 0xBF0EE402, 0xE640E500, 0xBF16E402,
+0xE5FEE500, 0x6053655C, 0xBEE3E403, 0xE51380F8,
+0xE40EBEE6, 0xE40E84F8, 0xBEE67085, 0xE5006503,
+0xBEE7E640, 0xE500E403, 0xBEF4E640, 0xE500E403,
+0xBEFCE640, 0xE5FFE403, 0x6053655C, 0xBEC9E404,
+0xE40F80FC, 0xE504BECC, 0xE40F84FC, 0xBECC7083,
+0xE5016503, 0xBECDE640, 0xE501E404, 0xBEDAE640,
+0xE501E404, 0xE404E640, 0xAEE07F10, 0x7F104F26,
+0x000B4F26, 0x00000009, 0x001E1030, 0x001E1080,
+0x001E1090, 0x001E103F, 0x001E103E, 0x002028C6,
+0x002028C8, 0x002028CA, 0x0009000B, 0x666CE680,
+0x6563D2A8, 0x7540E700, 0x6473422B, 0x2FB62FA6,
+0x2FD62FC6, 0x4F222FE6, 0x4C18EC01, 0xDAA3DBA2,
+0x65B252B1, 0x89223520, 0xC9036051, 0x891E8801,
+0xD19FDE9D, 0x64E3410B, 0x85036503, 0x670D66A2,
+0xDD9C3762, 0xD49C890A, 0x420BD29C, 0xD19C0009,
+0xE701D49C, 0x21724D0B, 0x0009AFE2, 0x420BD29A,
+0xD69A64E3, 0x4D0BD49A, 0xAFD926C2, 0x4F260009,
+0x6DF66EF6, 0x6BF66CF6, 0x6AF6000B, 0x7FF44F22,
+0xE6818546, 0x85472F01, 0x81F1666C, 0xD2858548,
+0x854281F2, 0x81F367F3, 0xE40C8543, 0x605381F4,
+0x81F56563, 0x7540420B, 0x4F267F0C, 0x0009000B,
+0x2F962F86, 0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6,
+0xDC847FF0, 0xE800A0DD, 0xD2836B13, 0xE0014B08,
+0x4B00420B, 0x1F03DE81, 0x3BEC85F2, 0x2F827E30,
+0x1FE26803, 0x66C2DD7E, 0x362052C1, 0xA0C38B01,
+0x60610009, 0x8801C903, 0xA0BD8B01, 0x85610009,
+0x8965C801, 0xEE105163, 0xDA6A8512, 0xC9036603,
+0x85136403, 0x4021600D, 0xC93F4021, 0x8D1C30E3,
+0xD7706503, 0x62704408, 0x44004408, 0x22284500,
+0x345C8F0A, 0x6043D26C, 0x697D072D, 0x68994919,
+0x697C6E8E, 0x28EDA009, 0x6043D268, 0x697D072D,
+0x68994919, 0x697C6E8E, 0xEEFF28ED, 0x6EEC629D,
+0x8B0F32E0, 0x410BD152, 0x540364C3, 0xBF85E502,
+0xD45F6E03, 0x460BD654, 0xD75E65E3, 0xD45EEE01,
+0x27E2A01D, 0x26E9EEFC, 0x81126063, 0x97888513,
+0x20794208, 0x85128113, 0x8112208B, 0x202B8513,
+0x85148113, 0x4218E208, 0x8114202B, 0x854164C2,
+0x814120E9, 0xD45165C2, 0xCB016051, 0x4A0B2501,
+0x60C20009, 0x52F356F2, 0x2B02CB01, 0x2622AF8B,
+0xD2378561, 0x8D2EC802, 0x420B64C3, 0xD6480009,
+0x5E036503, 0x076EE04C, 0x7701D146, 0x60120676,
+0x8B058801, 0xEA0C85E1, 0x20AB4A18, 0x81E1A007,
+0x88026012, 0x85E18B03, 0x20A9EADF, 0x855181E1,
+0x20A9EAFC, 0x60518151, 0xCB01DA28, 0x4A0B64C3,
+0x56F22501, 0xD73851F3, 0x85EF2612, 0x470B64D3,
+0xAF58650D, 0x420B0009, 0x54030009, 0x85446E03,
+0x4A18EA08, 0x30A020A9, 0x8B03DA1A, 0xE501BF16,
+0x0009A007, 0xD62D8541, 0x2268620D, 0xBF0D8901,
+0xD423E500, 0x420BD218, 0xD72265E3, 0xEE01D428,
+0x27E24A0B, 0x0009AF37, 0x68F26083, 0x780181F2,
+0x618D7C08, 0x31E7EE03, 0xAF1D8901, 0x7F100009,
+0x6EF64F26, 0x6CF66DF6, 0x6AF66BF6, 0x000B69F6,
+0xFE0368F6, 0x00201834, 0x00202884, 0x0020288C,
+0x00200A5C, 0x00200DA6, 0x00202854, 0x00200ADE,
+0x001E2130, 0x00202A70, 0x00200A7A, 0x001C3D30,
+0x00202A74, 0x00202864, 0x00201FD4, 0x001C3D00,
+0x00202A80, 0x00202A8C, 0x00202970, 0x002029F0,
+0x0020285C, 0x001E212C, 0x00202A78, 0x00202A7C,
+0x002027F8, 0x002027F4, 0x00200E06, 0x00008000,
+0x00202A88, 0x4F222FE6, 0x6E22D20D, 0xC84060E3,
+0x22E28D02, 0x0009BE68, 0x4218E240, 0x89012E28,
+0x0009BE64, 0xC80560E3, 0xBEB98901, 0x60E30009,
+0x8902C802, 0xAE614F26, 0x4F266EF6, 0x6EF6000B,
+0x001C3510, 0x080A0C0E, 0x00020406, 0x1A1C1E20,
+0x12141618, 0x2E303234, 0x26282A2C, 0x3A3C3E40,
+0x6C625648, 0x41112F26, 0xE2208F18, 0x890B3123,
+0x321CD204, 0xD1026220, 0x412B312C, 0x00090009,
+0x00201FFE, 0x00201FB4, 0x000BE000, 0x400062F6,
+0x40004000, 0x40004000, 0x40004000, 0x62F6000B,
+0x40004000, 0x40004000, 0x40004000, 0x40184000,
+0x62F6000B, 0x40004000, 0x40004000, 0x40004000,
+0x40284000, 0x62F6000B, 0x40004000, 0x40184000,
+0x000B4028, 0xC90F62F6, 0x40054005, 0x40054005,
+0x62F6000B, 0x4005C907, 0x40054005, 0x62F6000B,
+0x4005C903, 0x000B4005, 0xC90162F6, 0x000B4005,
+0x000062F6, 0x080A0C0E, 0x00020406, 0x1A1C1E20,
+0x12141618, 0x2E303234, 0x26282A2C, 0x3A3C3E40,
+0x6C625648, 0x41112F26, 0xE2208F18, 0x890B3123,
+0x321CD204, 0xD1026220, 0x412B312C, 0x00090009,
+0x002020BE, 0x00202074, 0x000BE000, 0x400162F6,
+0x40014001, 0x40014001, 0x40014001, 0x62F6000B,
+0x40014001, 0x40014001, 0x40014001, 0x40194001,
+0x62F6000B, 0x40014001, 0x40014001, 0x40014001,
+0x40294001, 0x62F6000B, 0x40014001, 0x40194001,
+0x000B4029, 0x400462F6, 0x40044004, 0xC90F4004,
+0x62F6000B, 0x40044004, 0xC9074004, 0x62F6000B,
+0x40044004, 0x000BC903, 0x400462F6, 0x000BC901,
+0x000062F6, 0x3622E218, 0x67438F12, 0x0009A004,
+0x76FF6254, 0x74012420, 0xC8036053, 0x60438BF8,
+0x8902C803, 0x422BD22B, 0xD22B0009, 0x0009422B,
+0x2FE66473, 0x8D4A3450, 0x27786763, 0x62438947,
+0x227B225B, 0xC9016023, 0x8D203452, 0x2EE86E03,
+0x60238B15, 0x8B08C803, 0x47096643, 0x47106256,
+0x8FFB2622, 0xA0327604, 0x47010009, 0x61436673,
+0x46106255, 0x8FFB2121, 0xA0287102, 0x66430009,
+0x47106254, 0x8FFB2620, 0xA0207601, 0x61430009,
+0x2EE8357C, 0x8F15317C, 0x60236653, 0x8B07C803,
+0x76FC4709, 0x47106262, 0x21268FFB, 0x0009A00F,
+0x65634701, 0x75FE6673, 0x46106251, 0x21258FFB,
+0x0009A005, 0x626076FF, 0x8FFB4710, 0x60432124,
+0x6EF6000B, 0x002022A6, 0x00202752, 0xE21E2FE6,
+0x67633626, 0x8D1B6153, 0x3E106E43, 0x3E128916,
+0x65E38908, 0x3672E600, 0x62148910, 0x25207601,
+0x7501AFF9, 0x317C64E3, 0x6513347C, 0xE600A004,
+0x625075FF, 0x24247601, 0x8BF93672, 0x60E3A011,
+0x890831E2, 0x327C6213, 0x8B0432E6, 0x651364E3,
+0xA0086673, 0xD28F6EF6, 0x651364E3, 0x422B6673,
+0x000B6EF6, 0xE2046EF6, 0x67433622, 0x8F10356C,
+0xA004346C, 0x75FF0009, 0x76FF6250, 0x60532424,
+0x8BF8C803, 0xC8036043, 0xA1058901, 0xA2770009,
+0xA2990009, 0x2FB60009, 0x2FD62FC6, 0x7FE42FE6,
+0x6C636043, 0x66521F62, 0xC9037504, 0x1F516E53,
+0x45086503, 0xE1FC6D43, 0x2D194500, 0x1F732558,
+0x1F651F44, 0x2FD28D0B, 0x88086053, 0x88108923,
+0x8818895B, 0xA0898B01, 0xA0BD0009, 0x62630009,
+0x2D22E600, 0x7CFC7D04, 0xEB10A00D, 0xE60064E6,
+0x7CF065E6, 0x62E261E6, 0x1D512D42, 0x1D231D12,
+0x7E047D10, 0x3CB21FE1, 0x1F6589F0, 0x2FD21FC2,
+0xA0A11FE6, 0x64D21FD4, 0x44286263, 0x44294418,
+0x42184419, 0x4629242B, 0x2D424619, 0x65637D04,
+0xA0217CFD, 0x67E6EB10, 0x62E67CF0, 0x64E66673,
+0x256B4618, 0x2D5261E2, 0x65234729, 0x45184719,
+0x4229275B, 0x42191D71, 0x47186743, 0x4429227B,
+0x44196713, 0x247B4718, 0x1D431D22, 0x41194129,
+0x65137D10, 0x1FE17E04, 0x89DC3CB2, 0x1FE67EFF,
+0x1FC21F55, 0xA0672FD2, 0x6CF21FD4, 0x66C257F5,
+0x46286273, 0x42284629, 0x2C62262B, 0x7C045DF2,
+0x7DFE4729, 0xA01CEB10, 0x65E65EF1, 0x66E66273,
+0x47286753, 0x6763227B, 0x452961E6, 0x257B4728,
+0x2C2264E6, 0x65131C51, 0x45284629, 0x1C62265B,
+0x41296643, 0x216B4628, 0x44291C13, 0x67437C10,
+0x3DB27DF0, 0x1FD289E1, 0x7EFEA034, 0x51F56CF2,
+0x621366C2, 0x42284618, 0x42184619, 0x2C62262B,
+0x7C045DF2, 0x7DFF4119, 0xA01FEB10, 0x65E65EF1,
+0x64E67DF0, 0x42286253, 0x421867E6, 0x66E6212B,
+0x61432C12, 0x45194128, 0x251B4118, 0x65731C51,
+0x44194528, 0x245B4518, 0x64631C42, 0x47194428,
+0x274B4418, 0x46191C73, 0x61637C10, 0x89DE3DB2,
+0x7EFD1FD2, 0x1FC41FE6, 0x5DF2E704, 0xA00D5EF6,
+0x62E451F4, 0x66E47DFC, 0x65E464E4, 0x71012120,
+0x71012160, 0x71012140, 0x71012150, 0x89F03D72,
+0x66D357F3, 0x641365E3, 0x6EF67F1C, 0x6CF66DF6,
+0x6BF6A190, 0x00202134, 0x2FC62FB6, 0x2FE62FD6,
+0x60437FE4, 0x6C63C903, 0x66031F62, 0x460875FC,
+0x61526E43, 0x4600E2FC, 0x26682E29, 0x1F441F73,
+0x1F516D53, 0x8D0B1F15, 0x60632FE2, 0x891F8808,
+0x89538810, 0x8B018818, 0x0009A081, 0x0009A0B9,
+0xEB10A00D, 0x52D37DF0, 0x54D156D2, 0x2E1665D2,
+0x2E662E26, 0x2E427EFC, 0x1FD16153, 0x3CB27CF0,
+0x7D0489F0, 0x1F151FD6, 0x2FE21FC2, 0x1FE4A0A1,
+0x621366E2, 0x42294619, 0x42194618, 0x2E62262B,
+0x7CFF4118, 0xEB10A021, 0x54D37DF0, 0x624357D2,
+0x42194229, 0x55D1212B, 0x2E1666D2, 0x41296173,
+0x41194418, 0x2E46241B, 0x44296453, 0x44194718,
+0x2E76274B, 0x47296763, 0x47194518, 0x257B7EFC,
+0x46182E52, 0x1FD16163, 0x3CB27CF0, 0x7D0389DC,
+0x1F151FD6, 0x2FE21FC2, 0x1FE4A06B, 0x57F56EF2,
+0x627366E2, 0x46284629, 0x262B4229, 0x2E625CF2,
+0x7CFE4728, 0xA01BEB10, 0x7DF05DF1, 0x55D251D3,
+0x46296613, 0x54D1276B, 0x2E7662D2, 0x41286753,
+0x217B4729, 0x61432E16, 0x41294528, 0x2E56251B,
+0x44286523, 0x245B4529, 0x42282E46, 0x7CF06723,
+0x89E23CB2, 0x1FD67D02, 0xA03A1FC2, 0x67F21FE4,
+0x657251F5, 0x45296213, 0x45284519, 0x42194518,
+0x5CF2252B, 0x41282752, 0x7CFD4118, 0xA022EB10,
+0x7DF05DF1, 0x54D256D3, 0x45196563, 0x52D14628,
+0x4618215B, 0x6ED26543, 0x45192716, 0x265B4428,
+0x65436163, 0x45186423, 0x42284419, 0x4218254B,
+0x271664E3, 0x44196623, 0x264B2756, 0x4E282766,
+0x61E34E18, 0x3CB27CF0, 0x7D0189DB, 0x1FC21FD6,
+0xE7041F74, 0x51F45DF2, 0x5EF6A00D, 0x84E27EFC,
+0x620364E0, 0x7DFC84E1, 0x84E36503, 0x21646603,
+0x21542124, 0x3D722144, 0x57F389F0, 0x641366D3,
+0x7F1C65E3, 0x6DF66EF6, 0xA09D6CF6, 0x2F866BF6,
+0x2FA62F96, 0x2FC62FB6, 0x2FE62FD6, 0x614374E0,
+0x6A636873, 0x6B56E920, 0x6C567AE0, 0x6D567120,
+0x6E563A92, 0x64566756, 0x62566656, 0x11C121B2,
+0x11E311D2, 0x11451174, 0x8DEC1166, 0x71201127,
+0x6613A004, 0x7AFF6254, 0x76012620, 0x8BF92AA8,
+0x6EF66083, 0x6CF66DF6, 0x6AF66BF6, 0x000B69F6,
+0x2F8668F6, 0x2FA62F96, 0x2FC62FB6, 0x2FE62FD6,
+0x6A636873, 0x75E0E920, 0x56565257, 0x57545155,
+0x5D525E53, 0x6B525C51, 0x24662426, 0x24762416,
+0x7AE024E6, 0x24C624D6, 0x8DEC3A92, 0x66A324B6,
+0x6EF66783, 0x6CF66DF6, 0x6AF66BF6, 0xA04369F6,
+0x2FE668F6, 0xC8046063, 0x8D046E63, 0x62166153,
+0x24227EFC, 0x60E37404, 0x8908C818, 0x71046513,
+0x62526616, 0x24227EF8, 0xAFF41461, 0xE2047408,
+0x65133E22, 0x66E38D02, 0x6EF6A01C, 0x6EF6AF87,
+0xC8046063, 0x61638D04, 0x625275FC, 0x242671FC,
+0xC8186013, 0x75F88906, 0x66525251, 0x24662426,
+0x71F8AFF6, 0x3122E204, 0x66138F02, 0x0009AFA1,
+0x0009A00A, 0x0009A004, 0x76FF6254, 0x74012420,
+0x8BF92668, 0x6073000B, 0x0009A004, 0x625075FF,
+0x242476FF, 0x8BF92668, 0x6073000B, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x544F0D0A,
+0x46205355, 0x00003A57, 0x2072614D, 0x32203232,
+0x20373030, 0x353A3431, 0x33353A34, 0x00000000,
+0x00000D0A, 0x00000043, 0x61766E49, 0x2064696C,
+0x72657375, 0x20726F20, 0x2079656B, 0x00214449,
+0x6E6B6E55, 0x206E776F, 0x6D6D6F63, 0x3D646E61,
+0x00000000, 0x61437748, 0x7262696C, 0x6F697461,
+0x6620206E, 0x0A6C6961, 0x0000000D, 0x73696F4E,
+0x61432065, 0x7262696C, 0x6F697461, 0x6166206E,
+0x21216C69, 0x00000D0A, 0x00000D0A, 0x00000042,
+0x000000FF, 0x00020001, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x010E010D, 0x00020003, 0x01090108, 0x0002010A,
+0x00030002, 0x02020201, 0x02040203, 0x02060205,
+0x02080207, 0x020A0209, 0x020C020B, 0x020E020D,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x010E010D, 0x00FF010F, 0x01090108, 0x010B010A,
+0x00030002, 0x02020201, 0x02040203, 0x02060205,
+0x02080207, 0x020A0209, 0x020C020B, 0x020E020D,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00000072, 0x00205220, 0x00000046, 0x00000059,
+0x73204142, 0x003D7165, 0x00000074, 0x00000000,
+0x02000112, 0x40FFFFFF, 0x12210ACE, 0x20104890,
+0x02090100, 0x0101002E, 0x09FA8000, 0x04000004,
+0x000000FF, 0x02010507, 0x07000200, 0x00028205,
+0x05070002, 0x00400383, 0x04050701, 0x01004003,
+0x002E0209, 0x80000101, 0x000409FA, 0x00FF0400,
+0x05070000, 0x00400201, 0x82050700, 0x00004002,
+0x03830507, 0x07010040, 0x40030405, 0x03040100,
+0x030C0409, 0x0079005A, 0x00410044, 0x03180053,
+0x00530055, 0x00320042, 0x0030002E, 0x00570020,
+0x0041004C, 0x0000004E, 0x00000000, 0x00000000,
+0x00000709, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+};
+
+const u32_t zcFwImageSize=11104;
diff --git a/drivers/staging/otus/hal/hpfwu_txstream.c b/drivers/staging/otus/hal/hpfwu_txstream.c
new file mode 100644
index 00000000000..2b77cbacc6d
--- /dev/null
+++ b/drivers/staging/otus/hal/hpfwu_txstream.c
@@ -0,0 +1,1017 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include "cprecomp.h"
+
+const u32_t zcFwImage[] = {
+0x0009000B, 0x4F222FE6, 0xDE947FFC, 0xE114D594,
+0x1E13D494, 0x67521E4C, 0xD494D693, 0x37402769,
+0x62528F06, 0x7201D692, 0x60602522, 0x2600C93F,
+0xD7906152, 0x2512611D, 0x264B6652, 0x2562470B,
+0x0009B017, 0xE60095AC, 0xC84060E2, 0x2F028F03,
+0x8FF93652, 0xD4887601, 0x4E0BDE88, 0xD4880009,
+0x00094E0B, 0x4E0BD487, 0x7F040009, 0xA0524F26,
+0x4F226EF6, 0x410BD184, 0xD4840009, 0x0009440B,
+0x450BD583, 0xD7830009, 0xD283E1FF, 0x2712611D,
+0xD4825029, 0xE1FFCB01, 0x1209E501, 0x12112212,
+0xE7202452, 0x4718D57E, 0x2572D27E, 0xD17EE700,
+0xD67FD47E, 0xE2012270, 0x24702172, 0xD67D2620,
+0x2641E4FF, 0xD57CE600, 0x666DE104, 0x76016063,
+0x4000626D, 0x8FF83212, 0xD5780545, 0x2520E201,
+0xD278D777, 0xE480E100, 0x22122710, 0x6613D576,
+0x666D644C, 0x76046763, 0x375C626D, 0x8FF83243,
+0xD5722712, 0xD273D772, 0xE400E101, 0x27102511,
+0x000B4F26, 0x7FCC2242, 0xD170D56F, 0xD271DB70,
+0x1F51D471, 0xD6717508, 0x1F12D771, 0x1F55710C,
+0x1FB975FC, 0x72041F2A, 0x1F13EB10, 0x1F561F44,
+0x1F781F67, 0xD86B1F2B, 0xDD6CD96B, 0xDC6CEA00,
+0xD26DDE6C, 0x89003A22, 0xD15D7A01, 0x88016010,
+0x56F88B03, 0x4218E201, 0xD1682622, 0x0009410B,
+0x440BD467, 0xD5670009, 0x0009450B, 0x6010D150,
+0x8B108801, 0xE650D14F, 0x46186212, 0x8B083266,
+0x56F9D14B, 0x2120E200, 0xCB016062, 0x2602A003,
+0x72012710, 0x60822122, 0x89098801, 0xE2C8D15A,
+0x622C6612, 0x89033626, 0x6010D158, 0x8BC88801,
+0x51F66792, 0x217252F5, 0xD6555191, 0x55FA2212,
+0x52FB6462, 0x55612542, 0x2252E400, 0x61436643,
+0x05DE6013, 0x36CC4608, 0x07DE2652, 0xC9036071,
+0x8B028801, 0x720162E2, 0x74012E22, 0x36B3664C,
+0x71048FEE, 0x66C2D147, 0x45286512, 0x265B4518,
+0x60822C62, 0x89018801, 0x0009A168, 0x6272D742,
+0x8B132228, 0xD42BD741, 0x6772D541, 0x51536242,
+0x312C327C, 0x24222228, 0x15138D05, 0x6262D63D,
+0xB1627201, 0xD6232622, 0x2622E200, 0x52916692,
+0x8B013620, 0x0009A144, 0x6061A06E, 0x001C001C,
+0x001D4020, 0x0000B38E, 0xFFFF0000, 0x12340000,
+0x001E1015, 0x00201274, 0x002039F4, 0x002018A2,
+0x00203A00, 0x00203A18, 0x00201860, 0x0020196C,
+0x00201288, 0x001C3510, 0x001C3624, 0x001E212C,
+0x002038F4, 0x0020348C, 0x002038FC, 0x00203908,
+0x00203914, 0x00203970, 0x00203974, 0x0020391C,
+0x0020391D, 0x00203920, 0x00117700, 0x0020398C,
+0x0020398A, 0x002034F0, 0x00117710, 0x001C3D30,
+0x001C36F8, 0x00117734, 0x001C3684, 0x001C3D00,
+0x001C1000, 0x001C1028, 0x00203504, 0x00203924,
+0x00117600, 0x00117740, 0x7FFFFFFF, 0x00201730,
+0x0020332A, 0x00202334, 0x00203DA4, 0x00203972,
+0x002034FC, 0x00203964, 0x001C3D2C, 0x001C36B0,
+0x00203494, 0x0011775C, 0x8801C90F, 0xA0CF8901,
+0xD1960009, 0x36206212, 0xD4958904, 0x2421E200,
+0x2162A0CC, 0x6211D193, 0x89012228, 0x0009A0C3,
+0xE202D78F, 0x75016571, 0x3123615D, 0x27518D02,
+0x0009A0BC, 0xD28C57F2, 0x62226072, 0x40094019,
+0xC90F4009, 0x8F19880A, 0x52F31F2C, 0x40196022,
+0x40094009, 0x8808C90F, 0xA0A78901, 0x60630009,
+0xCB0154F7, 0xD27E55F2, 0xE7012402, 0xD47FE100,
+0x22112572, 0x72016242, 0x2422A098, 0x8B3F8805,
+0x602252F3, 0x40094019, 0xC90F4009, 0x8B168802,
+0xE4FFD577, 0x644D6752, 0x8B102748, 0x6272D775,
+0x8B0C3260, 0x51F255F7, 0xD26DE701, 0x21722562,
+0xD571E100, 0x64522211, 0xA0777401, 0x52F32542,
+0x40196022, 0x40094009, 0x8805C90F, 0x31B38B6E,
+0xD26A8B6C, 0x672254F4, 0x7701D569, 0x61422272,
+0x1F1CE640, 0x46182159, 0x8B033160, 0x6262D665,
+0x26227201, 0xE200D65A, 0x2621B067, 0x0009A056,
+0x3123E220, 0x88038B52, 0x52F38B1E, 0x40196022,
+0x40094009, 0x8803C90F, 0xD25B8B16, 0x672254F4,
+0x7701D557, 0x61422272, 0x1F1CE640, 0x46182159,
+0x8B033160, 0x6262D655, 0x26227201, 0xE200D648,
+0x2621B043, 0x0009A010, 0xD452D551, 0xD2446752,
+0xE1007701, 0x25723A46, 0x22118F06, 0xEA00D64E,
+0x72016262, 0x2622B031, 0x2FB2D54C, 0x95736652,
+0xD44A5BF1, 0x36205241, 0x60618910, 0x8B01C803,
+0x2B22E201, 0x8FF54510, 0x57F15664, 0x6272E1F0,
+0x41284118, 0x2722221B, 0x6BF2A008, 0x6BF2A006,
+0xE200D62F, 0xD12F2621, 0x2121E200, 0xD13CE201,
+0x66122822, 0x8B012668, 0x0009AE2B, 0x450BD539,
+0xD1390009, 0xAE24E600, 0x2F862160, 0x2FA62F96,
+0x2FC62FB6, 0x2FE62FD6, 0x7FF44F22, 0xDE34D133,
+0x54116212, 0x1F4167E2, 0x2F22D432, 0xD5321F72,
+0xD2326743, 0x58417794, 0x69425A42, 0x5B166C72,
+0x60526D22, 0xCB20E600, 0xE5402502, 0x626D7601,
+0x8BFB3253, 0x55F162F2, 0x11512122, 0xD62855F2,
+0x14812E52, 0x249214A2, 0x27C2D426, 0x26D211B6,
+0xDA256742, 0xE801D925, 0x490B2A72, 0xE2011A8C,
+0x1A2C4218, 0x4F267F0C, 0x6DF66EF6, 0x6BF66CF6,
+0x69F66AF6, 0x68F6000B, 0x000007D1, 0x00203984,
+0x00203988, 0x0020398E, 0x001C3DC0, 0x0011772C,
+0x001C3B88, 0x0020396C, 0x0011773C, 0x00117744,
+0x0000F000, 0x00117764, 0x00117748, 0x00117768,
+0x0011776C, 0x01FFFFFF, 0x0011774C, 0x002034FC,
+0x00203DA4, 0x002024F8, 0x00203972, 0x001C3B9C,
+0x001C3D98, 0x001C3700, 0x001C3500, 0x001C5960,
+0x001C8960, 0x00203504, 0x001C3D00, 0x0020160C,
+0x2F962F86, 0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6,
+0xDE957FAC, 0x61E0E014, 0x0F14D494, 0x710161E3,
+0xE0186210, 0xD2920F24, 0x0009420B, 0x450BD591,
+0x20080009, 0x8F126D03, 0xD28F1F07, 0x6720D48F,
+0x657CDD8F, 0x470BD78F, 0xD18F0009, 0x619C6910,
+0x46086613, 0x36184608, 0x3D6C4608, 0xE0181FD7,
+0xE58004FC, 0x604C66E2, 0x3050655C, 0x2D628F15,
+0x01FCE014, 0xDE85E500, 0x641CA008, 0x6753655D,
+0x607037EC, 0x39DC6953, 0x80947501, 0x3243625D,
+0xD67F8BF4, 0xA34EE200, 0x20082621, 0xE0148B13,
+0xE40001FC, 0xA009DE79, 0x644D671C, 0x35EC6543,
+0x69436652, 0x39DC6262, 0x74041921, 0x3273624D,
+0xA3388BF3, 0x88012D10, 0xE0148B17, 0xE70001FC,
+0x6D1C2D70, 0xDE6D1FD4, 0x32D3627D, 0xA32A8B01,
+0x677D0009, 0x667365E3, 0x61737504, 0x315C36EC,
+0x69126462, 0xAFEF7708, 0x88042492, 0xE0148B18,
+0xE40001FC, 0x671C2D40, 0x624DDE60, 0x8B013273,
+0x0009A311, 0x6943644D, 0x39EC62E3, 0x72046592,
+0x3D2C6D43, 0x615266D2, 0x21697408, 0x2512AFED,
+0x8B188805, 0x01FCE014, 0x2D40E400, 0xDE53671C,
+0x3273624D, 0xA2F68B01, 0x644D0009, 0x62E36943,
+0x659239EC, 0x6D437204, 0x66D23D2C, 0x74086152,
+0xAFED216B, 0x88312512, 0xD44A8B3A, 0x6146D94A,
+0x75046543, 0x67566442, 0x6E531F48, 0x65527E04,
+0x7EE462E2, 0x7E0464E2, 0x6EE21FE9, 0x5EF929E0,
+0x7E04D942, 0x1FEA60E2, 0x2900C901, 0xD9406EE2,
+0x29E04E09, 0x2F562F26, 0x56FAD93E, 0x6513490B,
+0xD13D7F08, 0xE71C6E0D, 0x1DE12D70, 0xDE3B6912,
+0x64E21D92, 0x1D43D13A, 0xD23A6512, 0x67221D54,
+0x1D75D239, 0x1D666622, 0x6262D638, 0x1D27A2AB,
+0x8B398830, 0x6596D92B, 0x67926696, 0x61967904,
+0x74E46493, 0x6E436992, 0x1F9B7E04, 0x1FEC6442,
+0xD9256EE2, 0x5EFC29E0, 0x7E04D924, 0x1FED60E2,
+0x2900C901, 0xD9226EE2, 0x29E04E09, 0x59FC7FFC,
+0xDE272F92, 0x2F164E0B, 0xD41F7F08, 0xE21C610D,
+0x1D112D20, 0xD2206442, 0xD41C1D42, 0x1D536542,
+0x6752D51B, 0xD71B1D74, 0x1D156172, 0x1D666622,
+0x6262D61A, 0x1D27A26F, 0x8B358833, 0x490BD919,
+0xA268EE00, 0x00002DE0, 0x00117800, 0x00203A1C,
+0x002018A2, 0x00202AAC, 0x0020390E, 0x00203A20,
+0x00203534, 0x002018EE, 0x0020390D, 0x00117804,
+0x0020398C, 0x00117810, 0x00203909, 0x0020390A,
+0x0020390B, 0x00200F64, 0x001C5864, 0x001C6864,
+0x001C7864, 0x001C59BC, 0x001C69BC, 0x001C79BC,
+0x00200FBC, 0x00200FB8, 0x89018828, 0x0009A0C0,
+0xE643DEB5, 0x326662E1, 0x1FEE8F02, 0x2E21E240,
+0x622D62E1, 0x8B013267, 0x0009A0AA, 0xE50185E1,
+0x8B013056, 0x0009A0A4, 0x2D10E101, 0x64E1B225,
+0xE64357FE, 0x652D6271, 0x89443567, 0x3563E640,
+0xE6008B05, 0x0F65E040, 0xA00FE11A, 0x615372C0,
+0x41214121, 0x41214121, 0x45214121, 0x45214521,
+0xC9036053, 0xE0406603, 0x71180F65, 0x2209E007,
+0x0F25E03C, 0xE044641D, 0xB2A365F3, 0xE33C0F46,
+0x853233FC, 0x620DDE95, 0x42086031, 0x6023610D,
+0x1323E944, 0x06FE4108, 0xE00F39FC, 0x13144E0B,
+0x67075D91, 0x60D32679, 0x0F6654FE, 0x51928542,
+0x600D4E0B, 0x60D3260B, 0x0F666492, 0x65F3B237,
+0x696156FE, 0xE640659D, 0x89383563, 0xD78359FE,
+0x79066591, 0xC9036053, 0x40004008, 0x61036203,
+0x0F26E050, 0x470BE0FF, 0x6C07600C, 0x6603605D,
+0x46214621, 0x46214621, 0x42006263, 0x4200326C,
+0x40214021, 0x4008C903, 0x6D2D30FC, 0xE8006A03,
+0xB25765F3, 0x6EA264D3, 0x2EC9E050, 0x66942AE2,
+0xD76E01FE, 0x606C470B, 0x2AE22E0B, 0x64D365F3,
+0x7801B1FD, 0xEE06628D, 0x8FE932E3, 0x5EFE7D01,
+0x61E1E400, 0x410085E1, 0x66E3310C, 0x760C711B,
+0xE70465F3, 0x68667401, 0x3A736A4D, 0x8FF92582,
+0x65F37504, 0x641DB1E3, 0x64E1B1A4, 0x0009A17B,
+0xD45B56F7, 0xEC01D25B, 0x26C0420B, 0x0009A173,
+0x06FCE018, 0x8829606C, 0x58F78B08, 0xE400D252,
+0x66222840, 0x646DB171, 0x0009A165, 0x666CE681,
+0x89013060, 0x0009A0AC, 0xD550D14F, 0x62126A56,
+0x212232AC, 0x54116C56, 0x34CC6253, 0x64521141,
+0x72085812, 0xD44A384C, 0x68221182, 0x5A136C42,
+0x3ACC3C8C, 0x11A324C2, 0x6C2272EC, 0x72105814,
+0x118438CC, 0x5A156822, 0x11A53A8C, 0x6A227210,
+0xD6405816, 0x118638AC, 0x52176C62, 0x112732CC,
+0x5A185861, 0x11A83A8C, 0x5C195A62, 0x11C93CAC,
+0x521A5C63, 0x112A32CC, 0x5A1B5864, 0x11AB3A8C,
+0x5C1C5A65, 0x11CC3CAC, 0x521D5C66, 0x112D32CC,
+0x5A1E5867, 0x11AE3A8C, 0x561F5A68, 0x36ACE840,
+0x116FDA2D, 0x6CA2381C, 0x7A946682, 0x286236CC,
+0x5C8162A2, 0x18C13C2C, 0x62A27A44, 0x362C5682,
+0xD6261862, 0x5A856262, 0x3A2C4229, 0x760418A5,
+0x56866262, 0x362C4229, 0x56F71866, 0x2620E238,
+0x16C15C81, 0x16226212, 0xE2005C11, 0x551216C3,
+0x55151654, 0x55131655, 0x55161656, 0x55821657,
+0x65821658, 0x55141659, 0x5584165A, 0x5583165B,
+0x5585165C, 0x5586165D, 0x1821165E, 0x11212122,
+0x11251122, 0x11261123, 0x28221822, 0x18241124,
+0x18251823, 0x1826A0C7, 0x00117804, 0x002033E8,
+0x00203A40, 0x002018A2, 0x00203494, 0x001C36A0,
+0x002034F0, 0x001C3CA0, 0x001C36F4, 0x001C3B88,
+0x666CE682, 0x8B203060, 0xEA2456F7, 0x26A0D194,
+0x16C15C17, 0x16225218, 0x16835819, 0x16A45A1A,
+0x16C55C1B, 0x1626521C, 0xE200581D, 0x551E1687,
+0x551F1658, 0x11271659, 0x11291128, 0x112B112A,
+0x112D112C, 0xA08E112E, 0xE683112F, 0x3060666C,
+0x52F78B0B, 0xEA00D883, 0x658222A0, 0x7804DC82,
+0x62822C52, 0xA07ED681, 0xE6902620, 0x3060666C,
+0xDA7F8B06, 0x00094A0B, 0xE20056F7, 0x2620A073,
+0x666CE691, 0x8B103060, 0x6222D276, 0x2228622C,
+0xD2788904, 0x0009420B, 0x0009A003, 0x420BD276,
+0x56F70009, 0xA05EE200, 0xE6922620, 0x3060666C,
+0xE0188951, 0xE6B00BFC, 0x666C62BC, 0x8B2A3260,
+0x02FCE014, 0x682CEA00, 0x62ADE904, 0x894A3283,
+0x6AADDD64, 0x3CDC6CA3, 0x7D046EC2, 0xDB68D467,
+0x32DC62A3, 0x4B0BDC67, 0x4C0B6D22, 0xD46664E3,
+0x00094B0B, 0x64D34C0B, 0x4B0BD464, 0xE6000009,
+0x666D6BE3, 0x76013B6C, 0x3293626D, 0x8FF72BD0,
+0xAFDA4D19, 0xE6B57A08, 0x3260666C, 0xD45C8B13,
+0x4B0BDB57, 0xD25B0009, 0x6022DB5B, 0xCB20E6FF,
+0x2202666D, 0xDB592B62, 0xE014E200, 0x56F72B20,
+0xA01002FC, 0xD4562620, 0x6542D256, 0x420BD456,
+0xA0080009, 0xDB520009, 0x52B1E600, 0x622CDB53,
+0x52F72B21, 0x7F542260, 0x6EF64F26, 0x6CF66DF6,
+0x6AF66BF6, 0x000B69F6, 0x4F2268F6, 0xE240614D,
+0x89143123, 0x3127E21F, 0x8B09D749, 0xD449614D,
+0xE00171E0, 0x5671440B, 0x26596507, 0x1761A007,
+0xE001D444, 0x6672440B, 0x26596507, 0x4F262762,
+0x0009000B, 0x614D4F22, 0x3123E240, 0xE21F8912,
+0xD73B3127, 0x614D8B08, 0x5671D23A, 0x420B71E0,
+0x260BE001, 0x1761A006, 0x6672D236, 0xE001420B,
+0x2762260B, 0x000B4F26, 0xE6400009, 0x46284618,
+0x6252D531, 0x89FC2268, 0x0009000B, 0x4618E680,
+0xD52D4628, 0x22686252, 0x000B89FC, 0xA0010009,
+0x7201E200, 0x8BFC3242, 0x0009000B, 0x4618E680,
+0xD5264628, 0x22686252, 0x000B8BFC, 0x2FE60009,
+0x7FFC4F22, 0xBFF16E53, 0x61E22F42, 0xE280D620,
+0x54E11615, 0x16464218, 0x422855E2, 0x57E31657,
+0x16786EF2, 0x26E22E2B, 0x4F267F04, 0x6EF6AFCE,
+0x00203494, 0x00117804, 0x002038F4, 0x00203908,
+0x0020050A, 0x00201008, 0x0020102E, 0x00203A58,
+0x002018A2, 0x002018E6, 0x00203A6C, 0x00203A74,
+0x00203A78, 0x001C3500, 0x001C1000, 0x0020398A,
+0x00117800, 0x002018EE, 0x00203A8C, 0x00203990,
+0x001C3704, 0x002033E8, 0x001C373C, 0x001C3700,
+0x001C370C, 0x2FD62FC6, 0x4F222FE6, 0x6C53DD10,
+0x6E43BFA4, 0x2DE2BF89, 0x0009BFA0, 0x2C1251D5,
+0x1C4154D6, 0x1C5255D7, 0x1C6356D8, 0x6EF64F26,
+0x000B6DF6, 0x61636CF6, 0xA004E600, 0x62564109,
+0x24227601, 0x36127404, 0x000B8BF9, 0x00000009,
+0x001C370C, 0x0009A16E, 0x2FE62FD6, 0xDD944F22,
+0xA0049EB2, 0xD4930009, 0x420BD293, 0x62D265D2,
+0x8BF822E8, 0x0009A004, 0xD28FD490, 0x55D1420B,
+0x22E852D1, 0xA0048BF8, 0xD48D0009, 0x420BD28A,
+0x52D255D2, 0x8BF822E8, 0x0009A004, 0xD286D489,
+0x55D3420B, 0x22E852D3, 0xA0048BF8, 0xD4860009,
+0x420BD281, 0x52D455D4, 0x8BF822E8, 0x6EF64F26,
+0x6DF6000B, 0x2FD62FC6, 0x4F222FE6, 0x6E636C73,
+0x6D53B01A, 0x64D357F4, 0xB05F65E3, 0xB07566C3,
+0xB0A40009, 0xB0A80009, 0xB0AC0009, 0xB0AC0009,
+0xB0AF0009, 0xB03154F5, 0x6CCD6C03, 0x4F2660C3,
+0x6DF66EF6, 0x6CF6000B, 0x3412D170, 0xD6700529,
+0x2650D770, 0x2742000B, 0x0009A018, 0x2FD62FC6,
+0x4F222FE6, 0x6E636C73, 0x6D53BFEE, 0x64D357F4,
+0xB03365E3, 0xB08D66C3, 0xB00F54F5, 0x6CCD6C03,
+0x4F2660C3, 0x6DF66EF6, 0x6CF6000B, 0xE503D162,
+0xD763D462, 0x21524518, 0x2472000B, 0xD45FD15E,
+0x2162E600, 0x2462000B, 0xBF734F22, 0xBF73E40A,
+0xD25C0009, 0x4118E104, 0xE40AE500, 0xBF692212,
+0xD7592252, 0xCB206072, 0x000B4F26, 0x4F222702,
+0x410BD156, 0xD556E400, 0x4F26452B, 0xD1552FE6,
+0x66126E63, 0x92104418, 0x44084528, 0x45002629,
+0x265B4408, 0x264B4400, 0x21624708, 0xD14E4708,
+0x217227EB, 0x6EF6000B, 0x1FFF03F0, 0x4F222FE6,
+0xE101DE4A, 0xBF3DE40A, 0x67E32E12, 0xE500776C,
+0xE204E130, 0x2752E40A, 0x27522752, 0x27522752,
+0x27522752, 0x27522752, 0x27522752, 0x27522752,
+0x27522752, 0x27522752, 0x27522752, 0x27222712,
+0x27522752, 0x27522752, 0x27522752, 0x27522752,
+0x175ABF18, 0x2E62E600, 0x000B4F26, 0xD2346EF6,
+0xE441E101, 0x000B2212, 0xD1322242, 0xE605D432,
+0x000B2162, 0x000B2462, 0xD2300009, 0xE40AE601,
+0x2262AF00, 0x2FC62FB6, 0x2FE62FD6, 0x7FFC4F22,
+0x6C43DB2B, 0xED0060B2, 0x2B02CB03, 0xC90360B2,
+0x6E03A008, 0x89073DC2, 0xE46460B2, 0xB07CC903,
+0x7D016E03, 0x8BF52EE8, 0x8F043DC2, 0xD4212FE1,
+0x460BD621, 0x62F10009, 0x6023622D, 0x89FFC801,
+0x7F046023, 0x6EF64F26, 0x6CF66DF6, 0x6BF6000B,
+0x001C3B88, 0x00203AA0, 0x002018EE, 0x00203AA8,
+0x00203AB0, 0x00203AB8, 0x00203AC0, 0x0025E720,
+0x00203DA0, 0x002038F8, 0x001C5968, 0x001C3B40,
+0x000F8000, 0x001D4004, 0x001C3500, 0x002015E0,
+0x0020160C, 0x001C5814, 0x001C59D0, 0x001C5830,
+0x001C6268, 0x001C59A4, 0x001C639C, 0x001C581C,
+0x001C5860, 0x00203AC8, 0x002018A2, 0x8F014411,
+0x6043604B, 0x0009000B, 0x5651D52B, 0x46286052,
+0x306C000B, 0x2FC62FB6, 0x2FE62FD6, 0x4F124F22,
+0xBFF14F02, 0x6B036E43, 0xDD25DC24, 0x0009BFEC,
+0x3C0530B8, 0x4609060A, 0x46014609, 0x020A3D65,
+0x42094209, 0x32E24209, 0x4F068BF0, 0x4F264F16,
+0x6DF66EF6, 0x000B6CF6, 0x2FC66BF6, 0x2FE62FD6,
+0x4F124F22, 0xBFCF4F02, 0x6C036E43, 0xBFCBDD13,
+0x30C80009, 0x060A3D05, 0x46094609, 0x36E24601,
+0x4F068BF5, 0x4F264F16, 0x6DF66EF6, 0x6CF6000B,
+0x4F222FE6, 0xE102DE0B, 0xE403E500, 0xBFB92E12,
+0xE6062E52, 0xE7004618, 0x2E62E403, 0x4F262E72,
+0x6EF6AFB0, 0x0009000B, 0x001C1040, 0xCCCCCCCD,
+0x10624DD3, 0x001D4004, 0x2F962F86, 0x2FB62FA6,
+0x2FD62FC6, 0x4F222FE6, 0xE5007F98, 0x6453E710,
+0x6B534728, 0xEE1ADCBC, 0x6153655D, 0x315C4108,
+0x75014108, 0x6043317C, 0x0F16665D, 0xED0060B3,
+0x21B136E3, 0x81128111, 0x11D28113, 0x11D411D3,
+0x74048FEA, 0xD8B167F2, 0x1871D9B1, 0x58F12872,
+0x1981D1B0, 0x59F22982, 0x5DF45AF3, 0x54F65EF5,
+0x21921191, 0x11A211A3, 0x11D411D5, 0x11E611E7,
+0x11481149, 0xDAA855F7, 0x57F8EE00, 0x52F9DDA7,
+0x64E3D6A7, 0x2A521A51, 0xD8A7D9A6, 0x2D729AD5,
+0x6EED2622, 0x4D086DE3, 0x3DEC61E3, 0x4D084108,
+0x3D9C31EC, 0x410860B3, 0x81D12DB1, 0x4108E050,
+0x4008E7B7, 0x677C4108, 0x60B381D2, 0xE200318C,
+0x81D33472, 0x1D131DD2, 0x8D01D493, 0xD4901D24,
+0xB0B365D3, 0x64ED7E01, 0x8BDA34A2, 0x2FD2DA8C,
+0xDD9268A2, 0x2D824829, 0x7DFC64A2, 0xD287694D,
+0x6E222D92, 0x7D0C4E29, 0x68222DE2, 0x618D6AD3,
+0x2A16D784, 0xD48A6D72, 0x24D2D583, 0xD6895E72,
+0x517414E2, 0x1414EE00, 0xD1875876, 0x59781486,
+0x1498E710, 0x65E36252, 0x26E2142A, 0xE60064E3,
+0x6843644D, 0x384C4808, 0x381C4808, 0x0C866053,
+0x09CE28B1, 0x819160B3, 0x0ACE6053, 0x81A26043,
+0x0DCE6053, 0x81D360B3, 0x08CE6053, 0x18827401,
+0x624D09CE, 0x0ACE19E3, 0x1A643273, 0x75048FE0,
+0xE003D96A, 0x40186C92, 0x6D922CB1, 0x81D1DA6F,
+0x6E92E050, 0x81E24008, 0x60B36192, 0x64928113,
+0x1442E600, 0xD4696792, 0x689217A3, 0x1864E1FF,
+0x6563E703, 0x364C4608, 0x26127501, 0x3673665D,
+0xDC5B8BF8, 0x6DC2E003, 0x2DB14018, 0xD2606EC2,
+0x61C281E1, 0x1112EE00, 0xE02464C2, 0x65C21423,
+0x15E4D45B, 0xE58067C2, 0x68C28172, 0x818366E3,
+0x666D655C, 0x76046963, 0x394C6A6D, 0x8FF83A53,
+0xDB5429E2, 0x24B2DC54, 0x24C27404, 0x4F267F68,
+0x6DF66EF6, 0x6BF66CF6, 0x69F66AF6, 0x68F6000B,
+0x60116142, 0x8F03C803, 0xD23DE500, 0x8B063420,
+0xC9036011, 0x8B068802, 0x3420D239, 0x56128B03,
+0x52646513, 0x000B2422, 0x01136053, 0x2FE62FD6,
+0x7FEC4F22, 0x62536E53, 0x6D43E550, 0x4508E400,
+0xE101A001, 0x60435224, 0x81212211, 0x60538123,
+0x56E28122, 0x8BF53620, 0x16E4D238, 0xE61464F3,
+0x65E3420B, 0xE4FC65E1, 0x2E512549, 0x65F361F1,
+0x2F112149, 0xD13154D1, 0xE614410B, 0x607157D1,
+0x2701CB01, 0x7F141DE1, 0x6EF64F26, 0x6DF6000B,
+0x2FE62FD6, 0x7FEC4F22, 0x66536E53, 0x6D43E5FC,
+0x20596061, 0x2601CB01, 0x326052E2, 0x12E48B06,
+0x31E051E2, 0x52D18B04, 0x1E22A002, 0x5664AFF0,
+0x64F3D21E, 0x420BE614, 0x67E165E3, 0x2719E1FC,
+0x67F12E71, 0x271954D1, 0x65F3D118, 0x410BE614,
+0x52D12F71, 0xCB016021, 0x1DE12201, 0x4F267F14,
+0x000B6EF6, 0x00006DF6, 0x00203924, 0x002034F4,
+0x002034FC, 0x00203504, 0x0020352C, 0x00203910,
+0x00203918, 0x00100208, 0x001017C0, 0x001E210C,
+0x001C3D00, 0x00203964, 0x001000C8, 0x00117880,
+0x00117780, 0x00040020, 0x0026C401, 0x00200ED6,
+0x4F222FE6, 0xDE42624C, 0x42004208, 0x3E2CA005,
+0xD4405252, 0xBF695624, 0x65E22E62, 0x352052E1,
+0xD63D8BF6, 0x4F262622, 0x6EF6000B, 0x2FC62FB6,
+0x2FE62FD6, 0xDC394F22, 0x52C1DB39, 0x362066C2,
+0x6061891C, 0x8801C903, 0xDE348918, 0xBF37DD35,
+0x650364E3, 0x66B28503, 0x3262620D, 0xD4328907,
+0x0009BF76, 0x4D0BD431, 0xAFE60009, 0xBF3D0009,
+0xD42F64E3, 0x00094D0B, 0x0009AFDF, 0x2262D22D,
+0x6EF64F26, 0x6CF66DF6, 0x6BF6000B, 0x2FD62FC6,
+0x4F222FE6, 0xDD29DC28, 0x6E4360C2, 0x04DE4008,
+0xE614D127, 0x65E3410B, 0xD127D726, 0x55E227E2,
+0x35E05254, 0x21228F04, 0x400860C2, 0x122202DE,
+0x605365C2, 0x75014008, 0x0DE606DE, 0xC90F6053,
+0x60632C02, 0x6EF64F26, 0x000B6DF6, 0x85436CF6,
+0x650D5643, 0x622D6262, 0x35277204, 0xE1008F0C,
+0x2268960C, 0xD6158B03, 0x72015261, 0xD6131621,
+0x6262E101, 0x26227201, 0x6013000B, 0x000001FF,
+0x00203504, 0x002034FC, 0x001C3D00, 0x0020352C,
+0x002038F4, 0x002018A2, 0x002034F4, 0x00203AF0,
+0x00203AF4, 0x001C3D28, 0x00203964, 0x00203924,
+0x00200ED6, 0x00203968, 0x0020396C, 0x00117754,
+0x2FC62FB6, 0x2FE62FD6, 0x7FF84F22, 0x6022D237,
+0x8D58C803, 0xDE362F01, 0xDB37DC36, 0x66C252C1,
+0x892F3620, 0xC9036061, 0x892B8801, 0xD233DD31,
+0x64D3420B, 0x1F016503, 0x880160B1, 0xD2308B04,
+0x64D3420B, 0x0009AFEA, 0x85615653, 0x8904C820,
+0xE050D72C, 0x7201027E, 0xD22B0726, 0x6453420B,
+0x89072008, 0x55F1D126, 0x64D3410B, 0xE601D727,
+0x2762AFD4, 0x55F1D226, 0x64E3420B, 0xE601D125,
+0x2162AFCC, 0xDD25DE24, 0xDC26DB25, 0x66D252D1,
+0x89183620, 0xC9036061, 0x89148801, 0xD117D41F,
+0x0009410B, 0x36E05603, 0x65038F04, 0x2B20E201,
+0x2C52AFEC, 0xD712D41C, 0x0009470B, 0xE601D115,
+0xAFE34618, 0x60F12162, 0x8907C804, 0x7F08D217,
+0x6EF64F26, 0x6CF66DF6, 0x6BF6422B, 0x4F267F08,
+0x6DF66EF6, 0x000B6CF6, 0x00006BF6, 0x001E2100,
+0x00203504, 0x002034FC, 0x0020398C, 0x002014A0,
+0x002014CC, 0x00203494, 0x002016BE, 0x001E212C,
+0x00201530, 0x001C3D30, 0x00117880, 0x002034F4,
+0x00203914, 0x00203910, 0x0020352C, 0x00200610,
+0xE601D203, 0x1265D503, 0x000B2252, 0x00001266,
+0x001C1010, 0x0000C34F, 0x0009000B, 0x2FD62FC6,
+0x4F222FE6, 0x6D436C53, 0xEE00A004, 0x7E0164D4,
+0x644CBFF2, 0x8BF93EC2, 0x6EF64F26, 0x000B6DF6,
+0xE5006CF6, 0x6643A002, 0x76017501, 0x22286260,
+0xAFE38BFA, 0x2FE60009, 0x75076253, 0xE1086753,
+0x6043EE0A, 0x4409C90F, 0x650330E2, 0x8D014409,
+0xE630E637, 0x4110365C, 0x8FF22760, 0xE00077FF,
+0x000B8028, 0x000B6EF6, 0x000BE000, 0x2FE6E000,
+0x7FEC4F22, 0x6E436253, 0xBFDC65F3, 0xBFD06423,
+0xBFCE64E3, 0xD40364F3, 0x0009BFCB, 0x4F267F14,
+0x6EF6000B, 0x00203AF8, 0xE4FDD29F, 0xD79F6122,
+0x22122149, 0x74016022, 0x2202CB01, 0xD59C6622,
+0x22622649, 0xC8406070, 0x60528902, 0x2502CB04,
+0xE1F76452, 0x25422419, 0xE7016052, 0x2502CB40,
+0xE6026052, 0x2502C9CF, 0x47186052, 0x2502CB10,
+0xCB036052, 0x15622502, 0x1573000B, 0xD78ED58D,
+0xD48FD28E, 0xE600E100, 0x27112511, 0xAFCB2210,
+0x664C2461, 0x4600D28B, 0x6060362C, 0x000BCB10,
+0x654C2600, 0x4500D287, 0x6650352C, 0x2619E1EF,
+0x2560000B, 0xD284664C, 0x362C4600, 0xCB106060,
+0x2600000B, 0xD280654C, 0x352C4500, 0xE1EF6650,
+0x000B2619, 0x664C2560, 0x4600D27A, 0x6060362C,
+0x000BCB08, 0x654C2600, 0x4500D276, 0x6650352C,
+0x2619E1F7, 0x2560000B, 0xD273664C, 0x362C4600,
+0xCB086060, 0x2600000B, 0xD26F654C, 0x352C4500,
+0xE1F76650, 0x000B2619, 0x624C2560, 0x4200D669,
+0x6020326C, 0x4021C908, 0x40214021, 0x600C000B,
+0xD665624C, 0x326C4200, 0xC9086020, 0x40214021,
+0x000B4021, 0xD161600C, 0x341C644C, 0x000B6240,
+0xD15F602C, 0x341C644C, 0x000B6240, 0x2FE6602C,
+0x6E434F22, 0xE60A645C, 0x89143467, 0x0009BFEB,
+0x60EC640C, 0x8B028801, 0xA002E00F, 0x44092409,
+0x624C4409, 0x3263E60A, 0xBFE28905, 0x620C644C,
+0xC8806023, 0xE2008B00, 0x4F266023, 0x6EF6000B,
+0xD64C4F22, 0x88016062, 0xB2578B03, 0xA0030009,
+0xD2490009, 0x2260E640, 0xE200D648, 0x000B4F26,
+0x4F222622, 0x6062D643, 0x8B018802, 0x0009B2A0,
+0xE200D642, 0x000B4F26, 0xD53E2622, 0xE100D43E,
+0x2512E701, 0x2470000B, 0xE604D23B, 0x2260000B,
+0xD43B4F22, 0x410BD13B, 0xD53B0009, 0x6650E1FD,
+0x2619D23A, 0x2560E700, 0x000B4F26, 0x4F222270,
+0xD238D537, 0xD7386152, 0x2512611D, 0xE6FF6452,
+0x2542242B, 0xD22FD435, 0x420B666D, 0xD52E2762,
+0x6750E1FB, 0x4F262719, 0x2570000B, 0xD4304F22,
+0x410BD128, 0xD5280009, 0x6650E7F7, 0x4F262679,
+0x2560000B, 0x9425D524, 0x22496250, 0x2520000B,
+0xE4BFD521, 0x22496250, 0x2520000B, 0xD2254F22,
+0x600D8522, 0x89112008, 0x89458801, 0x89478803,
+0x89498805, 0x894F8806, 0x89558808, 0x895B8809,
+0x8961880A, 0x8967880B, 0x0009A06E, 0x0009B070,
+0x600CA06B, 0x0000FF7F, 0x001E2148, 0x001E1000,
+0x001E1108, 0x002039C4, 0x002039C6, 0x002039E5,
+0x002039A8, 0x001E103F, 0x001E105F, 0x001E102F,
+0x001E1090, 0x002039CC, 0x001E100B, 0x002039C8,
+0x00203AFC, 0x002018A2, 0x001E1028, 0x002039E4,
+0x001D4020, 0x98760000, 0x001C1000, 0x00203B08,
+0x00203B18, 0x0020399C, 0x0009B04C, 0x600CA035,
+0x0009B055, 0x600CA031, 0x6260D684, 0x8B2B2228,
+0x0009B061, 0x600CA029, 0x6260D680, 0x8B232228,
+0x0009B069, 0x600CA021, 0x6260D67C, 0x8B1B2228,
+0x0009B0C7, 0x600CA019, 0x6260D678, 0x8B132228,
+0x0009B0CD, 0x600CA011, 0x6260D674, 0x8B0B2228,
+0x0009B125, 0x600CA009, 0x6260D670, 0x8B032228,
+0x0009B13D, 0x600CA001, 0x4F26E000, 0x0009000B,
+0xD26CD16B, 0xD56C8412, 0x4000C90F, 0xD76B012D,
+0xE403D66B, 0xE20F611C, 0x2540E001, 0x25202712,
+0x2602000B, 0xE601D262, 0x30668523, 0xE0008D05,
+0xD663D260, 0xE0018122, 0x000B2602, 0xD25C0009,
+0x600D8523, 0x89052008, 0x8B0A8801, 0x6060D65D,
+0x2600CB01, 0xD457D65A, 0xE001E101, 0x000B2612,
+0x000B8142, 0xD152E000, 0x8513E501, 0x640D4518,
+0x66033453, 0xE0008D05, 0xD551D253, 0x2260E001,
+0x000B2502, 0x4F220009, 0x8513D149, 0x6453650D,
+0x62494419, 0x227D672E, 0x8801602C, 0x88028909,
+0x88038910, 0x8806891A, 0x88078935, 0xA04C893B,
+0xD5460009, 0x6652D746, 0x2762D446, 0x622C6261,
+0x2421A038, 0x2228625C, 0xD4438B3F, 0x6642D540,
+0x2562D440, 0x24018561, 0x6203A02C, 0x2008605C,
+0x88108907, 0x88208908, 0x88308909, 0xA02C890A,
+0xD23A0009, 0x6222A008, 0xA005D239, 0xD2396222,
+0x6222A002, 0x6262D638, 0xD432D531, 0x66212522,
+0xA00F626C, 0xD6352421, 0x6261D52D, 0x622CD42D,
+0xA0072562, 0xD6322421, 0x8561D529, 0x2562D429,
+0x62032401, 0x662D8515, 0x3617610D, 0x65038F01,
+0xB0CB2451, 0xA0010009, 0xE000E001, 0x000B4F26,
+0xD6190009, 0xD427E101, 0x65412610, 0xD118D717,
+0xE20F655D, 0x2752E001, 0x000B2620, 0x2FE62102,
+0xD20F4F22, 0x640C8523, 0x8B082448, 0xD511D61D,
+0x2621E200, 0x940F8451, 0xA0482049, 0xDE0D8051,
+0xC84060E0, 0xE2018D32, 0x89443427, 0xD216D615,
+0x2641420B, 0x0009A030, 0x0000FF7F, 0x002039E5,
+0x0020399C, 0x002039A8, 0x001E1100, 0x001E100C,
+0x002039C8, 0x001E1000, 0x001E1001, 0x002039D0,
+0x002039B0, 0x002039B4, 0x002039B8, 0x002039D4,
+0x002039D8, 0x002039DC, 0x002039E0, 0x00203E04,
+0x00203E0E, 0x002039C2, 0x00202886, 0x89123427,
+0xD294D693, 0x2641420B, 0xCB8084E1, 0x80E1B0F5,
+0xD69160E0, 0x2E00CB04, 0xC93F6060, 0xD68F2600,
+0xA001E001, 0xE0002602, 0x000B4F26, 0xD68C6EF6,
+0xC8806060, 0xD2868919, 0x88016021, 0xD2898B15,
+0x8524E501, 0x89103056, 0xE203D187, 0x2120D487,
+0xE00B6541, 0x0656655D, 0xE40FD585, 0x2140E702,
+0xD77E2571, 0x000BE001, 0x000B2702, 0x2FE6E000,
+0xDE804F22, 0xC88084E1, 0xD57A892C, 0x20088554,
+0x61038F28, 0x8553D77C, 0x64036672, 0x8566650C,
+0x3520620C, 0xD6798B1E, 0x651CD774, 0x2651644C,
+0x60E02741, 0x8904C840, 0x420BD275, 0xA0030009,
+0xD2680009, 0x0009420B, 0x0009B09F, 0xE201D167,
+0x60E02122, 0xCB04D464, 0x60402E00, 0x2400C93F,
+0x6023A001, 0x4F26E000, 0x6EF6000B, 0x2FB62FA6,
+0x2FD62FC6, 0xDA622FE6, 0x66A1E240, 0x3622DC5E,
+0x62638900, 0x6ED36D2C, 0x4E2136D8, 0x4E212A61,
+0xDB61D460, 0xE700A00F, 0x770162B2, 0x71026123,
+0x66212B12, 0x71026213, 0x61212B12, 0x651D666D,
+0x356C4528, 0x627C2452, 0x8BED32E3, 0xC90360D3,
+0x8B108803, 0x617367B2, 0x2B127102, 0x71026E13,
+0x2B126571, 0x655D6DE1, 0x422862DD, 0x325CE107,
+0xA00C2C10, 0x88022422, 0xA0038B01, 0x8801E203,
+0xE2018B05, 0x66B22C20, 0x655D6561, 0xE60F2452,
+0x67A12C60, 0x8B052778, 0xDD38DC44, 0xEB01EA00,
+0x2DB22CA2, 0x6DF66EF6, 0x6BF66CF6, 0x6AF6000B,
+0x2FE62FD6, 0xE240DD36, 0x362266D1, 0x62638900,
+0x3678672C, 0x7703DE38, 0x47212D61, 0x64E2D635,
+0xA00E4721, 0x6562E100, 0x62537101, 0x74012450,
+0x24204219, 0x45297401, 0x74012450, 0x24504519,
+0x621C7401, 0x8BEE3273, 0x66E24200, 0x420061D1,
+0x2118362C, 0x2E628F06, 0xDD1CD728, 0xE501E400,
+0x2D522742, 0x000B6EF6, 0x2FD66DF6, 0x4F222FE6,
+0xED0AEE01, 0x64E3BC85, 0xBC8A64E3, 0x62EC7E01,
+0x8BF732D7, 0xBC8DEE01, 0x64E364E3, 0x7E01BC92,
+0x32D762EC, 0x4F268BF7, 0x000B6EF6, 0xD1186DF6,
+0xD418920D, 0x72122122, 0x2422D617, 0xD7177204,
+0x72202622, 0x2722D116, 0x000B7230, 0x137A2122,
+0x002039C2, 0x00202992, 0x001E1015, 0x002039C8,
+0x001E1001, 0x0020399C, 0x001E1100, 0x002039C6,
+0x002039B4, 0x001E1000, 0x002039B8, 0x002039C4,
+0x00202886, 0x001E100C, 0x002039B0, 0x002039CC,
+0x002039D0, 0x002039D4, 0x002039D8, 0x002039DC,
+0x002039E0, 0x4F222FE6, 0xD6707FFC, 0x88016060,
+0xE2018951, 0x2620BFBB, 0xD56ED16D, 0xDE6E6010,
+0x64E36552, 0x7402C840, 0x8D22D16C, 0xD26C7502,
+0xE601D76C, 0xE7042722, 0x76016255, 0x626C2421,
+0x8FF93273, 0xD4637402, 0x6242E601, 0x640D8528,
+0x67494419, 0x275D657E, 0x81E4607C, 0xE417D562,
+0x67557601, 0x3243626C, 0x8FF92171, 0xA0207102,
+0xD25E0009, 0xE601D75B, 0xE7042722, 0x76016255,
+0x626C2421, 0x8FF93273, 0xD4527402, 0x6242E601,
+0x640D8528, 0x67494419, 0x275D657E, 0x81E4607C,
+0xE417D553, 0x67557601, 0x3243626C, 0x8FF92171,
+0x92897102, 0xD2462E21, 0x5E23D74E, 0x64F22FE2,
+0x604365F2, 0x2700C980, 0xC9606043, 0x80716103,
+0xC9036043, 0x80724519, 0x65F2605C, 0x817266F2,
+0x46194629, 0x606C4529, 0x4018645C, 0x8173304C,
+0x21185E23, 0x64F22FE2, 0x6E4C62F2, 0x602C4219,
+0x66F262F2, 0x46294018, 0x461930EC, 0x42298174,
+0x652C606C, 0x305C4018, 0x81758F07, 0x0009BC96,
+0x2228620C, 0xA00A8908, 0x60130009, 0x8B038840,
+0x0009B009, 0x0009A003, 0xE202D62F, 0x7F042622,
+0x000B4F26, 0x4F226EF6, 0x8552D52A, 0x8830600D,
+0x88318903, 0xA0348923, 0x85550009, 0xD428D727,
+0x85532701, 0x610DD627, 0x24124118, 0x460BD426,
+0xD7230009, 0xD226D425, 0x6572420B, 0xE230D120,
+0x42286712, 0x2729E620, 0x37604628, 0xD6218B03,
+0xA016E200, 0xD61F2622, 0xA012E202, 0xD1182622,
+0x6212E530, 0xE6204528, 0x46282259, 0x89083260,
+0xD41AD119, 0xE601D513, 0x2160450B, 0x472BD718,
+0x4F264F26, 0x0009000B, 0x0000060A, 0x002039E4,
+0x001E1000, 0x002039D0, 0x00203E04, 0x00203E10,
+0x00203DA8, 0x002039B8, 0x00203DD8, 0x00203DD6,
+0x00203DAA, 0x0020399C, 0x002039C8, 0x002039B4,
+0x002039B0, 0x002018A2, 0x00203B24, 0x00203B28,
+0x002018EE, 0x002039CC, 0x001E100B, 0x00203B3C,
+0x00114004, 0x4F222FE6, 0xDE967FFC, 0x200884E9,
+0x2F008D06, 0xD695D494, 0x0009460B, 0x64F0B19A,
+0x6620D293, 0x89022668, 0xC9BF60E0, 0x7F042E00,
+0x000B4F26, 0x000B6EF6, 0x2FE60009, 0xDE8D4F22,
+0x60E0D68D, 0xCBC0D48D, 0x62602E00, 0xC803602C,
+0x40218904, 0x70014021, 0x6603A002, 0x66034009,
+0xD687616D, 0xE500A004, 0x75016262, 0x74042422,
+0x3213625D, 0xD2838BF8, 0x0009420B, 0xC9BF84E2,
+0x4F2680E2, 0x6EF6000B, 0x2FE62FD6, 0x7FFC4F22,
+0x6260D67D, 0x89442228, 0xD572E100, 0x60502610,
+0xCB40D47A, 0x2500440B, 0x8D052008, 0x62E06E03,
+0x7104612C, 0x2F11A006, 0xD475D66D, 0xDD756760,
+0x657C4D0B, 0xE23C6D1D, 0x8B033D27, 0xD267D472,
+0x0009420B, 0x4D214D21, 0xA005D770, 0x66E6E400,
+0x357C4508, 0x74012562, 0x35D3654D, 0xD76C8BF7,
+0x6172E003, 0x81114018, 0x6E7260F1, 0x81E2700C,
+0xD4686172, 0xDD688113, 0x4D0BDE68, 0xE2016572,
+0xD4672E22, 0x420BD255, 0xD6560009, 0xC93F6060,
+0x7F042600, 0x6EF64F26, 0x6DF6000B, 0x2FC62FB6,
+0x2FE62FD6, 0xD25F4F22, 0x6B436E73, 0x420B6C53,
+0x20086D63, 0x64038D1C, 0xE50ED149, 0x32526210,
+0x60C38916, 0x804124B0, 0x814160D3, 0xA007E500,
+0x655D61BC, 0x00EC6053, 0x364C6653, 0x80647501,
+0x3213625D, 0xD63B8BF5, 0xC9BF6060, 0x2600A008,
+0xD23AD44D, 0x6EF64F26, 0x6CF66DF6, 0x6BF6422B,
+0x6EF64F26, 0x6CF66DF6, 0x6BF6000B, 0x7FC44F22,
+0x720262F3, 0x22512F41, 0x45297202, 0x60632251,
+0xE5C4E682, 0x67F38121, 0x655C666C, 0xE408BFB6,
+0x4F267F3C, 0x0009000B, 0x2F962F86, 0x2FB62FA6,
+0x2FD62FC6, 0x4F222FE6, 0xE1007FC4, 0x6513ECFF,
+0x6B136CCD, 0xDE36D735, 0xEDFF64F3, 0xD835EA04,
+0x6053655C, 0x027D4000, 0x32C0622D, 0x66038D0D,
+0x09ED6063, 0x2491027D, 0x24217402, 0x698202ED,
+0x3928622D, 0x74022892, 0x75017104, 0x6063625C,
+0x07D532A2, 0x0EB58FE4, 0x2448641C, 0xE6808905,
+0x67F3E5C5, 0xBF79666C, 0x7F3C655C, 0x6EF64F26,
+0x6CF66DF6, 0x6AF66BF6, 0x000B69F6, 0xD11E68F6,
+0x6012D21E, 0xCB20E405, 0x2102E500, 0x000B2242,
+0x00002252, 0x001E1017, 0x00203B40, 0x002018A2,
+0x0020390E, 0x001E1015, 0x001E10BF, 0x00117800,
+0x001E10FC, 0x00200610, 0x00203914, 0x00202AEA,
+0x00203B44, 0x002018EE, 0x00203B60, 0x0011788C,
+0x00203910, 0x002034F4, 0x00201530, 0x001E2130,
+0x00203B68, 0x00202AAC, 0x00203B6C, 0x00203974,
+0x0020397C, 0x00203DA4, 0x001C3500, 0x001D4004,
+0xD564D163, 0xE400D764, 0x2142E20F, 0x17411154,
+0xD5622722, 0x9669D762, 0x15412572, 0x96661562,
+0xE6011565, 0xD55F1165, 0x666CE6F8, 0x25422542,
+0x25422542, 0x25422542, 0x25622542, 0x7601E727,
+0x67632572, 0x25627797, 0xE7042572, 0x2572E248,
+0xE2192522, 0xE2702522, 0x25422542, 0x25422542,
+0x25222542, 0x2522E20C, 0x25422542, 0x25422542,
+0x25422542, 0x25422542, 0x000B154A, 0xE2081145,
+0x0009422B, 0x2FE62FD6, 0x7FFC4F22, 0xC8206043,
+0x6E438D02, 0x0009BE67, 0xC81060E3, 0xBE648901,
+0x60E30009, 0x8901C840, 0x0009BE86, 0xC80160E3,
+0xDD3D8938, 0xC80260D0, 0x2F008D03, 0x460BD63B,
+0x60F00009, 0x8902C804, 0x460BD639, 0x62F00009,
+0xC8806023, 0x60D08902, 0x2D00C97F, 0xC8016023,
+0xD6348906, 0x0009460B, 0x0009A007, 0x51630601,
+0x8902C808, 0x460BD630, 0x60F00009, 0x8902C810,
+0x420BD22E, 0xD52E0009, 0x88026052, 0xD22D8B03,
+0xA005E604, 0x88012260, 0xD22A8B02, 0x2260E601,
+0x2522E200, 0xC88060E3, 0xD227892D, 0x60E36E20,
+0x8902C880, 0x420BD225, 0x60E30009, 0x8902C840,
+0x420BD223, 0x60E30009, 0x8902C802, 0x420BD221,
+0x60E30009, 0x890DC804, 0xDD20D11F, 0x0009410B,
+0x0009BF0D, 0x0009BF4C, 0xD51ED41D, 0x2470E708,
+0x25D2BF85, 0xC80860E3, 0xD21B8905, 0x4F267F04,
+0x422B6EF6, 0x7F046DF6, 0x6EF64F26, 0x6DF6000B,
+0x001C581C, 0xA000A000, 0x001D0100, 0x001D4000,
+0x00040021, 0x001C589C, 0x001E1021, 0x00201A90,
+0x00201AB2, 0x00202114, 0x00201ACA, 0x00201AD8,
+0x002039C8, 0x001E100B, 0x001E1028, 0x00201B44,
+0x00201B50, 0x00201AE0, 0x00201AFE, 0x12345678,
+0x001E1000, 0x0010F100, 0x00201B2C, 0x644CD6A7,
+0x000B346C, 0xD6A62450, 0x346C644C, 0x2450000B,
+0x644CD6A4, 0x000B346C, 0x625C2450, 0x4208616D,
+0x42084119, 0x42006019, 0x670E614C, 0xD49E321C,
+0x4200207D, 0x324CC90F, 0x2200000B, 0x4208625C,
+0x42004208, 0x324C644C, 0x4200D498, 0x000B324C,
+0x2FE62260, 0x614C4F12, 0x4100D493, 0x6710314C,
+0xE29F666D, 0x27294619, 0x6E536269, 0x672E6573,
+0x4221227D, 0x42214221, 0x7601662C, 0xE4014608,
+0x34E84608, 0x644C4600, 0x071A0467, 0x2150257B,
+0x000B4F16, 0x4F226EF6, 0xD2857FE8, 0x88016021,
+0xD2848B7B, 0x26686621, 0xD2838B77, 0x26686621,
+0xE50F8B73, 0xE401BFA2, 0xBFA4E501, 0xE586E400,
+0xE400655C, 0x2F50BFA4, 0xBFA1E401, 0xE602E506,
+0x60634618, 0x81F2E401, 0x6543BF9F, 0xE40185F2,
+0xBFAB6543, 0x85F26603, 0x6543E401, 0x6603BFB1,
+0xE40265F0, 0x6053756C, 0x80F8BF80, 0xBF82E402,
+0x84F8E512, 0x7090E402, 0x6503BF82, 0x4618E602,
+0x81F66063, 0xBF80E402, 0x85F6E500, 0x6603E402,
+0xE500BF8C, 0xE40285F6, 0xBF926603, 0xE5FEE500,
+0xE010655C, 0xBF61E403, 0xE5130F54, 0xE40EBF63,
+0x05FCE010, 0xBF63E40E, 0xE5007585, 0xBF64E403,
+0xE500E640, 0xBF71E403, 0xE500E640, 0xBF78E403,
+0xE5FFE640, 0xE014655C, 0xBF47E404, 0xE40F0F54,
+0xE504BF49, 0x05FCE014, 0xBF49E40F, 0xE5017584,
+0xBF4AE640, 0xE501E404, 0xBF57E640, 0xE501E404,
+0xE404E640, 0xAF5C7F18, 0x7F184F26, 0x000B4F26,
+0x4F220009, 0xD2427FF0, 0x88016021, 0xD2418B71,
+0x26686621, 0xD2408B6D, 0x26686621, 0xE50F8B69,
+0xE401BF1C, 0xBF1EE501, 0xE586E400, 0xE400655C,
+0x2F50BF1E, 0xBF1BE401, 0xE401E506, 0xBF1C6543,
+0xE401E640, 0xBF296543, 0xE401E640, 0xBF306543,
+0x65F0E640, 0x756CE402, 0xBEFF6053, 0xE40280F4,
+0xE512BF01, 0xE40284F4, 0xBF017090, 0xE6406503,
+0xBF02E402, 0xE640E500, 0xBF0FE402, 0xE640E500,
+0xBF16E402, 0xE5FEE500, 0x6053655C, 0xBEE5E403,
+0xE51380F8, 0xE40EBEE7, 0xE40E84F8, 0xBEE77085,
+0xE5006503, 0xBEE8E640, 0xE500E403, 0xBEF5E640,
+0xE500E403, 0xBEFCE640, 0xE5FFE403, 0x6053655C,
+0xBECBE404, 0xE40F80FC, 0xE504BECD, 0xE40F84FC,
+0xBECD7083, 0xE5016503, 0xBECEE640, 0xE501E404,
+0xBEDBE640, 0xE501E404, 0xE404E640, 0xAEE07F10,
+0x7F104F26, 0x000B4F26, 0x00000009, 0x001E102F,
+0x001E1080, 0x001E1090, 0x001E103F, 0x001E103E,
+0x002039C2, 0x002039C4, 0x002039C6, 0xD21DD11C,
+0x66206010, 0x676C7001, 0x3700C90F, 0xE5008D13,
+0x67106210, 0x7701622C, 0x64232170, 0xD6166010,
+0x44084408, 0x3428C90F, 0x62602100, 0x7201D513,
+0x44082620, 0x000B354C, 0xD10F6053, 0x25586510,
+0xE6008D13, 0xD60DD40B, 0x655C6540, 0x47086753,
+0x37584708, 0x47086540, 0x24507501, 0x367C6040,
+0x2400C90F, 0x72FF6210, 0x000B2120, 0x00006063,
+0x0020390D, 0x0020390C, 0x0020390E, 0x00203534,
+0x7FFC4F22, 0xE680D19F, 0x666C6212, 0xD29E2F22,
+0x67F36563, 0x420B7542, 0x7F04E404, 0x000B4F26,
+0xE6800009, 0xD298666C, 0xE7006563, 0x422B7540,
+0xE6806473, 0xD294666C, 0xE7006563, 0x422B7543,
+0x2F866473, 0x2FA62F96, 0x2FC62FB6, 0x2FE62FD6,
+0x7FCC4F22, 0xDC8ED28D, 0x72011F21, 0xDB8D1F22,
+0xD18EDE8D, 0x66125211, 0x8B013620, 0x0009A0E5,
+0xC9036061, 0x8B018801, 0x0009A0DF, 0xD288D487,
+0xED84420B, 0x2F025503, 0x30D0845C, 0xA0B88901,
+0xD1840009, 0x626C6610, 0x88016023, 0xD1828B68,
+0x62101FC3, 0x895B2228, 0xE003D480, 0x40186742,
+0x68421772, 0xD57EE900, 0x81816DB3, 0x7D042190,
+0x67D26AB2, 0x64E26852, 0x1F491F57, 0x740464E3,
+0x1FA46542, 0x65431F5A, 0x625275F8, 0x1F761FD5,
+0x6D531F2B, 0xDA74D773, 0x7D94D274, 0x68D21F88,
+0x6AA26972, 0xD1726022, 0x2202CB20, 0xE1401F1C,
+0x7601E600, 0x3213626D, 0x56F48BFB, 0x52F651F5,
+0x21222B62, 0x52F851F7, 0x212256F9, 0x2E6251FA,
+0x51FB2412, 0x2D822512, 0xD9662792, 0x29A2DD5F,
+0x6AD2D965, 0xD9646892, 0x68D21A84, 0x6081DA63,
+0x2801CB01, 0xD86266D2, 0x2A622962, 0xED015AFC,
+0x2AD2480B, 0x2AD24D18, 0x62D2DD5E, 0x2D227201,
+0xD15056F3, 0xE2026062, 0x2602CB01, 0x2120A03D,
+0x8B3A2228, 0xE401DD58, 0x2140E600, 0xE01C2D62,
+0xC801005C, 0xD4558B0A, 0xE600D755, 0xED7D2472,
+0x626C7601, 0x8BFB32D3, 0x24D2DD52, 0xE2FE68C2,
+0x2C822829, 0x095CE01E, 0xE01F5DF1, 0x0A5C2D90,
+0x751051F2, 0xED0621A0, 0xD74BE600, 0x8456D44B,
+0x27007601, 0x696C6854, 0x248039D3, 0x8FF67401,
+0xDA477701, 0x2A10E194, 0xE2007A01, 0x7A0F2A20,
+0xD130E805, 0x66102A80, 0x6023626C, 0x89088801,
+0xD240D42A, 0x420B65F2, 0xD131ED01, 0xAF304D18,
+0x65F221D2, 0x8553D43C, 0x620D6642, 0x89073262,
+0xD13BD43A, 0x0009410B, 0xE601D73A, 0x2762AF1A,
+0xD134D41E, 0x410B65F2, 0xD125ED01, 0xD637D436,
+0x460B4D18, 0xAF0D21D2, 0x7F340009, 0x6EF64F26,
+0x6CF66DF6, 0x6AF66BF6, 0x000B69F6, 0x4F2268F6,
+0x85467FF4, 0x2F01E681, 0x666C8547, 0x854881F1,
+0x81F2D209, 0x67F38542, 0x854381F3, 0x81F4E40C,
+0x65636053, 0x420B81F5, 0x7F0C7540, 0x000B4F26,
+0x00000009, 0x001C3D9C, 0x0020245C, 0x0011779A,
+0x001C36F8, 0x001C3B9C, 0x001C3704, 0x0020352C,
+0x002014A0, 0x0020391D, 0x0020391C, 0x00203918,
+0x001C3D98, 0x001C3BB4, 0x001C5960, 0x001C3500,
+0x001C3D30, 0x001C8960, 0x00203504, 0x001C3D00,
+0x0020160C, 0x00117730, 0x00203920, 0x001C582C,
+0x2000A000, 0x0000A000, 0x0011778C, 0x00117792,
+0x00117788, 0x002014CC, 0x002038F4, 0x002034F4,
+0x00201530, 0x001E2130, 0x00203D84, 0x002018A2,
+0x2F962F86, 0x2FB62FA6, 0x2FD62FC6, 0x4F222FE6,
+0xD19B7FEC, 0x2F12E000, 0x6103D49A, 0x1F4281F2,
+0xDD9ADA99, 0xD69A6813, 0xE0014808, 0x460BDE99,
+0x38EC4800, 0x65A21F03, 0x352052A1, 0xA23E8B01,
+0x60510009, 0x8801C903, 0xA2388B01, 0x52530009,
+0x32E0DE91, 0xD9918B10, 0x64A3490B, 0x4B0BDB90,
+0xDE906403, 0xD791D690, 0xEC01D591, 0x2E02E100,
+0x271026C0, 0x2502AFDF, 0xC8018551, 0xA1578B01,
+0x62510009, 0x4200622D, 0x5E53366A, 0x85E2226D,
+0xC903642C, 0x85E36603, 0x6053650D, 0x40214021,
+0x4500C93F, 0x322A6703, 0x6053252D, 0xC901D17F,
+0x60106C03, 0x8801D97F, 0xDB7F8B05, 0x2120E200,
+0xCB0160B2, 0xD17D2B02, 0x88016011, 0x65A28B0A,
+0x8D042448, 0x9B9E6251, 0xA00322B9, 0x919B2521,
+0x2521221B, 0x37B3EB10, 0x2448895E, 0xD4738B07,
+0x22286241, 0x60638903, 0xA05781F8, 0xD5706473,
+0x46084608, 0x85E26273, 0x46006B50, 0x362C4200,
+0x2BB8C910, 0x8F1F6463, 0x26686603, 0xD2698911,
+0x062D6043, 0x4119616D, 0x6B0E6019, 0x81F820BD,
+0x880160C3, 0x646C8F2C, 0x880F6073, 0xA0278B1B,
+0xD2610009, 0x052D6043, 0x4119615D, 0x670E6019,
+0x645C207D, 0x81F8A01C, 0x890F2668, 0x6043D25B,
+0x6B5D052D, 0x60B94B19, 0x201D610E, 0x60C381F8,
+0x8F0D8801, 0x6473645C, 0xEC00A00A, 0x6043D254,
+0x625D052D, 0x60294219, 0x207D670E, 0x81F8645C,
+0x880285F8, 0x85E1890A, 0x8D07C820, 0xE6DC6203,
+0x60232269, 0x81E1A002, 0x644CE4FF, 0x6210D149,
+0x89012228, 0x644CE4FF, 0x654DEBFF, 0x35B06BBC,
+0xDB368B2B, 0x64A34B0B, 0x410BD135, 0x54036403,
+0x85446E03, 0xC948DB40, 0xDC408808, 0xBEAE8B01,
+0x64B3E502, 0x65E34C0B, 0xDB3DEC01, 0xD13D2DC2,
+0x621260B2, 0x72017001, 0x21228805, 0x2B028F08,
+0x666CE680, 0x6563D238, 0x7549E700, 0x6473420B,
+0xA030D436, 0x7FFF0009, 0x85E28000, 0x20B9EBFC,
+0x610381E2, 0x942A85E3, 0x62032049, 0x450885F8,
+0x81E2201B, 0xC90160C3, 0x40084018, 0x40084008,
+0x4000225B, 0x6023220B, 0x85E481E3, 0x4118E108,
+0x81E4201B, 0xE40262A2, 0x20B98521, 0x67A28121,
+0xCB016071, 0x85F82701, 0x89033042, 0xECE785E2,
+0x81E220C9, 0x490BD41E, 0xA03B0009, 0x7E030009,
+0x001C3D30, 0x00203D90, 0x00203504, 0x001E212C,
+0x002033E8, 0x001C3D00, 0x00117780, 0x002014A0,
+0x0020166C, 0x0011770C, 0x0020391C, 0x0020391D,
+0x00203918, 0x002018A2, 0x001C36F8, 0x00203990,
+0x00203DA0, 0x00203B84, 0x00203C04, 0x00203C84,
+0x00203D04, 0x00203908, 0x002034FC, 0x002014CC,
+0x00203994, 0x00203998, 0x0020245C, 0x00203D88,
+0x00203D8C, 0x602262F2, 0x40094019, 0xC90F4009,
+0x8B0B880A, 0x60E2DE8C, 0x40094019, 0xC90F4009,
+0x8B038808, 0xCB0160A2, 0x2802A006, 0x65E2DE87,
+0x2E527501, 0x286266A2, 0x52F366F2, 0x2622AE83,
+0xD2838551, 0xDE83C802, 0xA0958B01, 0x420B0009,
+0x4E0B64A3, 0x5E036403, 0x85E46503, 0x4918E908,
+0xD77D209B, 0xE04C81E4, 0xDC7C0B7E, 0x7B01D97C,
+0x61C207B6, 0x71016690, 0x8D062668, 0xD4792C12,
+0x420BD279, 0xA070EB01, 0x62512DB2, 0x4B18EB0F,
+0x22B9E102, 0x32104118, 0x85518B0F, 0x2029E2FC,
+0x60518151, 0xCB0172E0, 0x85E12501, 0x202994A3,
+0x85E481E1, 0xA0522049, 0x675181E4, 0x4719677D,
+0x667E6779, 0x7701276D, 0x6903607C, 0x88014918,
+0x25918F3E, 0x6B12D161, 0x21B27B01, 0x660D85E3,
+0x40216063, 0xC93F4021, 0x6C034600, 0x262D322A,
+0xC8016063, 0xDB5ED15D, 0x967D8901, 0xE6002C6B,
+0x666C67CD, 0x40006063, 0x622D021D, 0x8D0E3270,
+0x60436403, 0xE9FF021D, 0x8B013290, 0x01C5A007,
+0x626C7601, 0x3292E904, 0x646C8BEB, 0x60434400,
+0xD15004BD, 0x0B457401, 0x669D6911, 0x89073670,
+0x602D6211, 0x890388FF, 0xE201DB4B, 0x2B2021C1,
+0xECFC8551, 0x815120C9, 0xCB016051, 0xDC472501,
+0x64A34C0B, 0x51F366F2, 0x85EF2612, 0x54F2D244,
+0x650D420B, 0x0009ADE7, 0xE500DC42, 0x420B2C52,
+0x4E0B64A3, 0x54036403, 0x85446E03, 0x6703E908,
+0x65034918, 0x27998541, 0xDB323790, 0x8F0BD932,
+0x6013610D, 0x8B07C820, 0xC9486053, 0x8B038808,
+0xE501BD4D, 0x0009A005, 0x2128D233, 0xBD468901,
+0x64B3E500, 0x490B65E3, 0xADBCEC01, 0x85F22DC2,
+0x7001EE04, 0x31E7610D, 0x8D0281F2, 0xADA97A08,
+0x7F140009, 0x6EF64F26, 0x6CF66DF6, 0x6AF66BF6,
+0x000B69F6, 0xF7FF68F6, 0x2FE68000, 0xD2234F22,
+0x60E36E22, 0x8D02C840, 0xBBF922E2, 0xE2400009,
+0x2E284218, 0xBC048901, 0x60E30009, 0x8905C810,
+0xD21CD41B, 0x0009420B, 0x0009BC03, 0xC80560E3,
+0xBD6D8901, 0x60E30009, 0x8902C802, 0xAC004F26,
+0x4F266EF6, 0x6EF6000B, 0x001C3D3C, 0x00117760,
+0x002014A0, 0x0020166C, 0x00203494, 0x00203DA4,
+0x00203908, 0x002034FC, 0x002014CC, 0x00203974,
+0x0020397C, 0x00203970, 0x00203972, 0x00201530,
+0x002018EE, 0x00203994, 0x00008000, 0x001C3510,
+0x00203D98, 0x002018A2, 0x080A0C0E, 0x00020406,
+0x1A1C1E20, 0x12141618, 0x2E303234, 0x26282A2C,
+0x3A3C3E40, 0x6C625648, 0x41112F26, 0xE2208F18,
+0x890B3123, 0x321CD204, 0xD1026220, 0x412B312C,
+0x00090009, 0x00203412, 0x002033C8, 0x000BE000,
+0x400062F6, 0x40004000, 0x40004000, 0x40004000,
+0x62F6000B, 0x40004000, 0x40004000, 0x40004000,
+0x40184000, 0x62F6000B, 0x40004000, 0x40004000,
+0x40004000, 0x40284000, 0x62F6000B, 0x40004000,
+0x40184000, 0x000B4028, 0xC90F62F6, 0x40054005,
+0x40054005, 0x62F6000B, 0x4005C907, 0x40054005,
+0x62F6000B, 0x4005C903, 0x000B4005, 0xC90162F6,
+0x000B4005, 0x000062F6, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x544F0D0A, 0x46205355, 0x00003A57,
+0x206C754A, 0x32203120, 0x20383030, 0x323A3132,
+0x34333A38, 0x00000000, 0x00000D0A, 0x00000043,
+0x42707372, 0x3D206675, 0x554E203D, 0x202C4C4C,
+0x6E49677A, 0x4E497274, 0x6D754E51, 0x0000003D,
+0x61766E49, 0x2064696C, 0x72657375, 0x20726F20,
+0x2079656B, 0x00214449, 0x52504545, 0x57204D4F,
+0x65746972, 0x6461202C, 0x003D7264, 0x6C617620,
+0x0000003D, 0x00000A0D, 0x435F4D5A, 0x465F444D,
+0x4C445F57, 0x494E495F, 0x00000054, 0x6E6B6E55,
+0x206E776F, 0x6D6D6F63, 0x3D646E61, 0x00000000,
+0x203A3051, 0x00000020, 0x203A3151, 0x00000020,
+0x203A3251, 0x00000020, 0x203A3351, 0x00000020,
+0x203A3451, 0x00000020, 0x2B434741, 0x73696F4E,
+0x61432065, 0x7262696C, 0x6F697461, 0x6166206E,
+0x6F206C69, 0x6974206E, 0x0D0A656D, 0x00000000,
+0x00000072, 0x00205220, 0x00000D0A, 0x62735576,
+0x7473725F, 0x00000A0D, 0x62735576, 0x7375735F,
+0x646E6570, 0x00000A0D, 0x62735576, 0x7365725F,
+0x000A0D6D, 0x00000044, 0x44387570, 0x72637365,
+0x6F747069, 0x3D584572, 0x00000000, 0x00000047,
+0x00000042, 0x72746E49, 0x6D652051, 0x2C797470,
+0x49677A20, 0x4972746E, 0x754E514E, 0x00003D6D,
+0x654C7245, 0x0000006E, 0x00000049, 0x20746F4E,
+0x756F6E65, 0x49206867, 0x4220514E, 0x0A0D6675,
+0x00000000, 0x000000FF, 0x00020001, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x010E010D, 0x00020003, 0x01090108,
+0x0002010A, 0x02000003, 0x02020201, 0x02040203,
+0x02060205, 0x02020200, 0x02040203, 0x020C020B,
+0x020E020D, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x000000FF, 0x00020001, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x010E010D, 0x00020003, 0x01090108,
+0x0002010A, 0x00030003, 0x02020201, 0x02040203,
+0x02060205, 0x02020200, 0x02040203, 0x020C020B,
+0x020E020D, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x010E010D, 0x00FF010F, 0x01090108,
+0x010B010A, 0x0200010F, 0x02020201, 0x02040203,
+0x02060205, 0x02020200, 0x02040203, 0x020C020B,
+0x020E020D, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x010E010D, 0x00FF010F, 0x01090108,
+0x010B010A, 0x010F010F, 0x02020201, 0x02040203,
+0x02060205, 0x02020200, 0x02040203, 0x020C020B,
+0x020E020D, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00FF00FF, 0x00FF00FF, 0x00FF00FF,
+0x00FF00FF, 0x00205220, 0x00000046, 0x00000059,
+0x73204142, 0x003D7165, 0x49544120, 0x0000204D,
+0x00000000, 0x00000000, 0x002E0209, 0x80000101,
+0x000409FA, 0x00FF0400, 0x05070000, 0x02000201,
+0x82050700, 0x00020002, 0x03830507, 0x07010040,
+0x40030405, 0x02090100, 0x0101002E, 0x09FA8000,
+0x04000004, 0x000000FF, 0x02010507, 0x07000040,
+0x40028205, 0x05070000, 0x00400383, 0x04050701,
+0x00004002, 0x00000000, 0x00000000, 0x07090000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+0x00000000, 0x00000000, 0x00000000, 0x00000000,
+};
+
+const u32_t zcFwImageSize=15936;
diff --git a/drivers/staging/otus/hal/hpfwuinit.c b/drivers/staging/otus/hal/hpfwuinit.c
new file mode 100644
index 00000000000..ed80ffafaff
--- /dev/null
+++ b/drivers/staging/otus/hal/hpfwuinit.c
@@ -0,0 +1,240 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include "../80211core/cprecomp.h"
+
+const u32_t zcFwImage[] = {
+0x0009000B, 0x7FFC4F22, 0xD695D494, 0x0009460B,
+0xD494E570, 0x4518B01E, 0x89042008, 0xD690D492,
+0x462B7F04, 0xB0124F26, 0xD2900009, 0x420BD490,
+0xE6000009, 0x949AD58F, 0xC8406052, 0x2F028F03,
+0x8FF93642, 0x7F047601, 0x000B4F26, 0xD28A0009,
+0x0009422B, 0x2FD62FC6, 0x4F222FE6, 0xD6877FEC,
+0x626061F3, 0x2F208461, 0x846280F1, 0x80F27110,
+0x6D438463, 0x846480F3, 0x80F46413, 0x6C538465,
+0x846680F5, 0x80F6E500, 0xD77D8467, 0x846880F7,
+0x80F8EE04, 0x80F98469, 0x80FA846A, 0x80FB846B,
+0x80FC846C, 0x80FD846D, 0x80FE846E, 0x80FF846F,
+0x6653655C, 0x7501367C, 0x665C6260, 0x242036E3,
+0x74018FF6, 0x66F32F16, 0xE7107604, 0xB00D65C3,
+0x6E0364D3, 0xD46B7F04, 0x420BD26B, 0x60E36503,
+0x4F267F14, 0x6DF66EF6, 0x6CF6000B, 0x2FB62FA6,
+0x2FD62FC6, 0x4F222FE6, 0x3F3C933A, 0x4108E141,
+0x31FCE200, 0x11733526, 0x21521162, 0x11418D02,
+0xE0FFA098, 0x4A18EA01, 0x262066F3, 0x32A27201,
+0x76018FFB, 0x6BE3EE00, 0xE0446CF3, 0x00FE4008,
+0x450BD556, 0x660361B3, 0x4008E043, 0x6DC004FE,
+0x014C6063, 0x31EC3EDC, 0x60E36E1C, 0x7B0107FC,
+0x2C703BA2, 0x8FE80FD4, 0xE0427C01, 0xEB004008,
+0x70FC07FE, 0x6EB36CB3, 0xA0200AFE, 0x2710EDFF,
+0x7C01FEE0, 0x60C36CCC, 0x657002FC, 0x6BBC3B2C,
+0x01FC60B3, 0x0F1460C3, 0x0F2460B3, 0x04FC60C3,
+0x342C7E01, 0x01FC604C, 0x251A62D3, 0xD43C225A,
+0x2750602C, 0x064E4008, 0x2D6A4D19, 0x3EA27701,
+0x66D78BDF, 0x4018E001, 0x0F646563, 0x70014519,
+0x0F544629, 0x0F647001, 0x70014619, 0x90420F64,
+0xE0450EFE, 0xEA014008, 0xE0460FF6, 0x4A184008,
+0xED0067F3, 0x0FF637AC, 0x0FF67004, 0xE345E104,
+0x7C014308, 0x6CCC33FC, 0x60C36432, 0x5531024C,
+0x6BBC3B2C, 0x045C60B3, 0x60C35A32, 0x60B30A44,
+0x60C30F24, 0x6A7006FC, 0x606C362C, 0x66E005FC,
+0x6A5C64AC, 0x626C24AA, 0x89053420, 0x4D084D08,
+0xCB0460D3, 0x600BA006, 0x7D014110, 0x8FD67701,
+0xE0007E01, 0x3F3C9308, 0x6EF64F26, 0x6CF66DF6,
+0x000B6BF6, 0x01386AF6, 0x00000120, 0x00200D54,
+0x002002BE, 0x00102800, 0x00200D64, 0x0010F00A,
+0x0010F000, 0x001C001C, 0x00103252, 0x00200DA0,
+0x0010FFFC, 0x00200D7C, 0x0020032C, 0x00200370,
+0x00200954, 0x0009000B, 0x2FD62FC6, 0x4F222FE6,
+0x6D436C53, 0xEE00A004, 0x7E0164D4, 0x644CBFF2,
+0x8BF93EC2, 0x6EF64F26, 0x000B6DF6, 0xE5006CF6,
+0x6643A002, 0x76017501, 0x22286260, 0xAFE38BFA,
+0x2FE60009, 0x75076253, 0xE1086753, 0x6043EE0A,
+0x4409C90F, 0x650330E2, 0x8D014409, 0xE630E637,
+0x4110365C, 0x8FF22760, 0xE00077FF, 0x000B8028,
+0x4F226EF6, 0xBFE47FEC, 0xBFD865F3, 0x7F1464F3,
+0x000B4F26, 0x4F22E000, 0xBFDA7FEC, 0x64F365F3,
+0x7406BFCD, 0x4F267F14, 0xE000000B, 0x4F222FE6,
+0x62537FEC, 0x65F36E43, 0x6423BFCB, 0x64E3BFBF,
+0x64F3BFBD, 0xBFBAD403, 0x7F140009, 0x000B4F26,
+0x00006EF6, 0x00200DB0, 0x89004011, 0x4111600B,
+0x4F228906, 0x611BB004, 0x000B4F26, 0x0009600B,
+0x620D2F26, 0x8F413020, 0x40180019, 0x8B0D3016,
+0x31043104, 0x31043104, 0x31043104, 0x31043104,
+0x890062F6, 0x4119310C, 0x6013000B, 0x41296219,
+0x20084018, 0x31048927, 0x31043104, 0x31043104,
+0x31043104, 0x31043104, 0x31043104, 0x31043104,
+0x31043104, 0x61193104, 0x3204221D, 0x32043204,
+0x32043204, 0x32043204, 0x32043204, 0x32043204,
+0x32043204, 0x32043204, 0x89003204, 0x4229320C,
+0x000B6023, 0xE00062F6, 0x62F6000B, 0x42286213,
+0x42244129, 0x42243104, 0x42243104, 0x42243104,
+0x42243104, 0x42243104, 0x42243104, 0x42243104,
+0x42243104, 0x42243104, 0x42243104, 0x42243104,
+0x42243104, 0x42243104, 0x42243104, 0x42243104,
+0x89003104, 0x6013310C, 0x62F6000B, 0x2F262F16,
+0x51F552F3, 0x52F22129, 0x52F41210, 0x212951F6,
+0x121152F2, 0x000B62F6, 0x000061F6, 0x51F32F16,
+0x310050F1, 0x51F48B02, 0x310050F2, 0x000B0029,
+0x000061F6, 0x51F32F16, 0x310050F1, 0x51F48B06,
+0x310050F2, 0xCA010029, 0x61F6000B, 0x000BE001,
+0x000061F6, 0x50F0000B, 0x2F262F16, 0xE10052F2,
+0x12001211, 0x000B62F6, 0x000061F6, 0x2F162F06,
+0x8B264115, 0x3103E040, 0x2F26892B, 0x52F62F36,
+0xE02053F5, 0x8B053103, 0xE3006233, 0x89093100,
+0x3108A002, 0x8B0F2338, 0xD0064F22, 0x6023400B,
+0x4F266203, 0x112151F4, 0x63F61130, 0x61F662F6,
+0x60F6000B, 0x002007F4, 0x4100C709, 0x0123011D,
+0x51F20009, 0x110150F4, 0x110050F3, 0x000B61F6,
+0x51F260F6, 0x1101E000, 0x61F61100, 0x60F6000B,
+0x01300000, 0x0128012C, 0x01200124, 0x0118011C,
+0x0106010A, 0x00FE0102, 0x00E200E6, 0x00DA00DE,
+0x00CC00D0, 0x00C400C8, 0x00A800AC, 0x00A000A4,
+0x008C0090, 0x00840088, 0x0066006A, 0x005E0062,
+0x42244300, 0x42244300, 0x42244300, 0x43286133,
+0x43084318, 0x42284308, 0x42084218, 0x41094208,
+0xAFAF4109, 0x4300221B, 0x43004224, 0x43004224,
+0x61334224, 0x43184328, 0x42184228, 0xAFA14119,
+0x4300221B, 0x43004224, 0x43004224, 0x61334224,
+0x43084328, 0x42284308, 0x42084208, 0x41094119,
+0xAF8F4109, 0x4300221B, 0x43004224, 0x43004224,
+0x61334224, 0x212D4328, 0x6213AF84, 0x42244300,
+0x42244300, 0x42244300, 0x43186133, 0x43084308,
+0x42084218, 0x41294208, 0x41094109, 0x221BAF72,
+0x42244300, 0x42244300, 0x42244300, 0x43186133,
+0x41294218, 0xAF654119, 0x4300221B, 0x43004224,
+0x43004224, 0x43004224, 0x43004224, 0x43004224,
+0x43004224, 0x4224AF56, 0x2F162F06, 0x8B264115,
+0x3103E040, 0x2F26892B, 0x52F62F36, 0xE02053F5,
+0x8B053103, 0xE2006323, 0x89093100, 0x3108A002,
+0x8B0F2228, 0xD0064F22, 0x6033400B, 0x4F266303,
+0x112151F4, 0x63F61130, 0x61F662F6, 0x60F6000B,
+0x002008B4, 0x4100C709, 0x0123011D, 0x51F20009,
+0x110150F4, 0x110050F3, 0x000B61F6, 0x51F260F6,
+0x1101E000, 0x61F61100, 0x60F6000B, 0x012E0000,
+0x0126012A, 0x011E0122, 0x0116011A, 0x01040108,
+0x00FC0100, 0x00E000E4, 0x00D800DC, 0x00CC00D0,
+0x00C400C8, 0x00A800AC, 0x00A000A4, 0x008C0090,
+0x00840088, 0x0066006A, 0x005E0062, 0x43254201,
+0x43254201, 0x43254201, 0x42296123, 0x42094219,
+0x43294209, 0x43094319, 0x41084309, 0xAFAF4108,
+0x4201231B, 0x42014325, 0x42014325, 0x61234325,
+0x42194229, 0x43194329, 0xAFA14118, 0x4201231B,
+0x42014325, 0x42014325, 0x61234325, 0x42094229,
+0x43294209, 0x43094309, 0x41084118, 0xAF8F4108,
+0x4201231B, 0x42014325, 0x42014325, 0x61234325,
+0xAF854229, 0x4201231D, 0x42014325, 0x42014325,
+0x61234325, 0x42094219, 0x43194209, 0x43094309,
+0x41084128, 0xAF734108, 0x4201231B, 0x42014325,
+0x42014325, 0x61234325, 0x43194219, 0x41184128,
+0x231BAF66, 0x43254201, 0x43254201, 0x43254201,
+0x43254201, 0x43254201, 0x43254201, 0xAF574201,
+0x00004325, 0x080A0C0E, 0x00020406, 0x1A1C1E20,
+0x12141618, 0x2E303234, 0x26282A2C, 0x3A3C3E40,
+0x6C625648, 0x41112F26, 0xE2208F18, 0x890B3123,
+0x321CD204, 0xD1026220, 0x412B312C, 0x00090009,
+0x0020081E, 0x002007D4, 0x000BE000, 0x400062F6,
+0x40004000, 0x40004000, 0x40004000, 0x62F6000B,
+0x40004000, 0x40004000, 0x40004000, 0x40184000,
+0x62F6000B, 0x40004000, 0x40004000, 0x40004000,
+0x40284000, 0x62F6000B, 0x40004000, 0x40184000,
+0x000B4028, 0xC90F62F6, 0x40054005, 0x40054005,
+0x62F6000B, 0x4005C907, 0x40054005, 0x62F6000B,
+0x4005C903, 0x000B4005, 0xC90162F6, 0x000B4005,
+0x000062F6, 0x080A0C0E, 0x00020406, 0x1A1C1E20,
+0x12141618, 0x2E303234, 0x26282A2C, 0x3A3C3E40,
+0x6C625648, 0x41112F26, 0xE2208F18, 0x890B3123,
+0x321CD204, 0xD1026220, 0x412B312C, 0x00090009,
+0x002008DE, 0x00200894, 0x000BE000, 0x400162F6,
+0x40014001, 0x40014001, 0x40014001, 0x62F6000B,
+0x40014001, 0x40014001, 0x40014001, 0x40194001,
+0x62F6000B, 0x40014001, 0x40014001, 0x40014001,
+0x40294001, 0x62F6000B, 0x40014001, 0x40194001,
+0x000B4029, 0x400462F6, 0x40044004, 0xC90F4004,
+0x62F6000B, 0x40044004, 0xC9074004, 0x62F6000B,
+0x40044004, 0x000BC903, 0x400462F6, 0x000BC901,
+0x000062F6, 0x00000000, 0x77073096, 0xEE0E612C,
+0x990951BA, 0x076DC419, 0x706AF48F, 0xE963A535,
+0x9E6495A3, 0x0EDB8832, 0x79DCB8A4, 0xE0D5E91E,
+0x97D2D988, 0x09B64C2B, 0x7EB17CBD, 0xE7B82D07,
+0x90BF1D91, 0x1DB71064, 0x6AB020F2, 0xF3B97148,
+0x84BE41DE, 0x1ADAD47D, 0x6DDDE4EB, 0xF4D4B551,
+0x83D385C7, 0x136C9856, 0x646BA8C0, 0xFD62F97A,
+0x8A65C9EC, 0x14015C4F, 0x63066CD9, 0xFA0F3D63,
+0x8D080DF5, 0x3B6E20C8, 0x4C69105E, 0xD56041E4,
+0xA2677172, 0x3C03E4D1, 0x4B04D447, 0xD20D85FD,
+0xA50AB56B, 0x35B5A8FA, 0x42B2986C, 0xDBBBC9D6,
+0xACBCF940, 0x32D86CE3, 0x45DF5C75, 0xDCD60DCF,
+0xABD13D59, 0x26D930AC, 0x51DE003A, 0xC8D75180,
+0xBFD06116, 0x21B4F4B5, 0x56B3C423, 0xCFBA9599,
+0xB8BDA50F, 0x2802B89E, 0x5F058808, 0xC60CD9B2,
+0xB10BE924, 0x2F6F7C87, 0x58684C11, 0xC1611DAB,
+0xB6662D3D, 0x76DC4190, 0x01DB7106, 0x98D220BC,
+0xEFD5102A, 0x71B18589, 0x06B6B51F, 0x9FBFE4A5,
+0xE8B8D433, 0x7807C9A2, 0x0F00F934, 0x9609A88E,
+0xE10E9818, 0x7F6A0DBB, 0x086D3D2D, 0x91646C97,
+0xE6635C01, 0x6B6B51F4, 0x1C6C6162, 0x856530D8,
+0xF262004E, 0x6C0695ED, 0x1B01A57B, 0x8208F4C1,
+0xF50FC457, 0x65B0D9C6, 0x12B7E950, 0x8BBEB8EA,
+0xFCB9887C, 0x62DD1DDF, 0x15DA2D49, 0x8CD37CF3,
+0xFBD44C65, 0x4DB26158, 0x3AB551CE, 0xA3BC0074,
+0xD4BB30E2, 0x4ADFA541, 0x3DD895D7, 0xA4D1C46D,
+0xD3D6F4FB, 0x4369E96A, 0x346ED9FC, 0xAD678846,
+0xDA60B8D0, 0x44042D73, 0x33031DE5, 0xAA0A4C5F,
+0xDD0D7CC9, 0x5005713C, 0x270241AA, 0xBE0B1010,
+0xC90C2086, 0x5768B525, 0x206F85B3, 0xB966D409,
+0xCE61E49F, 0x5EDEF90E, 0x29D9C998, 0xB0D09822,
+0xC7D7A8B4, 0x59B33D17, 0x2EB40D81, 0xB7BD5C3B,
+0xC0BA6CAD, 0xEDB88320, 0x9ABFB3B6, 0x03B6E20C,
+0x74B1D29A, 0xEAD54739, 0x9DD277AF, 0x04DB2615,
+0x73DC1683, 0xE3630B12, 0x94643B84, 0x0D6D6A3E,
+0x7A6A5AA8, 0xE40ECF0B, 0x9309FF9D, 0x0A00AE27,
+0x7D079EB1, 0xF00F9344, 0x8708A3D2, 0x1E01F268,
+0x6906C2FE, 0xF762575D, 0x806567CB, 0x196C3671,
+0x6E6B06E7, 0xFED41B76, 0x89D32BE0, 0x10DA7A5A,
+0x67DD4ACC, 0xF9B9DF6F, 0x8EBEEFF9, 0x17B7BE43,
+0x60B08ED5, 0xD6D6A3E8, 0xA1D1937E, 0x38D8C2C4,
+0x4FDFF252, 0xD1BB67F1, 0xA6BC5767, 0x3FB506DD,
+0x48B2364B, 0xD80D2BDA, 0xAF0A1B4C, 0x36034AF6,
+0x41047A60, 0xDF60EFC3, 0xA867DF55, 0x316E8EEF,
+0x4669BE79, 0xCB61B38C, 0xBC66831A, 0x256FD2A0,
+0x5268E236, 0xCC0C7795, 0xBB0B4703, 0x220216B9,
+0x5505262F, 0xC5BA3BBE, 0xB2BD0B28, 0x2BB45A92,
+0x5CB36A04, 0xC2D7FFA7, 0xB5D0CF31, 0x2CD99E8B,
+0x5BDEAE1D, 0x9B64C2B0, 0xEC63F226, 0x756AA39C,
+0x026D930A, 0x9C0906A9, 0xEB0E363F, 0x72076785,
+0x05005713, 0x95BF4A82, 0xE2B87A14, 0x7BB12BAE,
+0x0CB61B38, 0x92D28E9B, 0xE5D5BE0D, 0x7CDCEFB7,
+0x0BDBDF21, 0x86D3D2D4, 0xF1D4E242, 0x68DDB3F8,
+0x1FDA836E, 0x81BE16CD, 0xF6B9265B, 0x6FB077E1,
+0x18B74777, 0x88085AE6, 0xFF0F6A70, 0x66063BCA,
+0x11010B5C, 0x8F659EFF, 0xF862AE69, 0x616BFFD3,
+0x166CCF45, 0xA00AE278, 0xD70DD2EE, 0x4E048354,
+0x3903B3C2, 0xA7672661, 0xD06016F7, 0x4969474D,
+0x3E6E77DB, 0xAED16A4A, 0xD9D65ADC, 0x40DF0B66,
+0x37D83BF0, 0xA9BCAE53, 0xDEBB9EC5, 0x47B2CF7F,
+0x30B5FFE9, 0xBDBDF21C, 0xCABAC28A, 0x53B39330,
+0x24B4A3A6, 0xBAD03605, 0xCDD70693, 0x54DE5729,
+0x23D967BF, 0xB3667A2E, 0xC4614AB8, 0x5D681B02,
+0x2A6F2B94, 0xB40BBE37, 0xC30C8EA1, 0x5A05DF1B,
+0x2D02EF8D, 0x544F0D0A, 0x50205355, 0x20312D48,
+0x003A5746, 0x72636564, 0x69747079, 0x65206E6F,
+0x726F7272, 0x0A0D2121, 0x00000000, 0x6564667A,
+0x70797263, 0x65725F74, 0x616C7567, 0x79726F74,
+0x6261745F, 0x7220656C, 0x203D7465, 0x00000000,
+0x45485441, 0x38731652, 0x89ACFF91, 0xEE55D178,
+0xEE000D0A, };
+
+const u32_t zcFwImageSize=3508;
diff --git a/drivers/staging/otus/hal/hpmain.c b/drivers/staging/otus/hal/hpmain.c
new file mode 100644
index 00000000000..2e65c466aae
--- /dev/null
+++ b/drivers/staging/otus/hal/hpmain.c
@@ -0,0 +1,4643 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include "../80211core/cprecomp.h"
+#include "hpani.h"
+#include "hpusb.h"
+#include "otus.ini"
+
+extern const u32_t zcFwImage[];
+extern const u32_t zcFwImageSize;
+extern const u32_t zcDKFwImage[];
+extern const u32_t zcDKFwImageSize;
+extern const u32_t zcFwImageSPI[];
+extern const u32_t zcFwImageSPISize;
+
+#ifdef ZM_OTUS_LINUX_PHASE_2
+extern const u32_t zcFwBufImage[];
+extern const u32_t zcFwBufImageSize;
+extern const u32_t zcP2FwImage[];
+extern const u32_t zcP2FwImageSize;
+#endif
+extern void zfInitCmdQueue(zdev_t* dev);
+extern u16_t zfIssueCmd(zdev_t* dev, u32_t* cmd, u16_t cmdLen,
+        u16_t src, u8_t* buf);
+extern void zfIdlRsp(zdev_t* dev, u32_t* rsp, u16_t rspLen);
+extern u16_t zfDelayWriteInternalReg(zdev_t* dev, u32_t addr, u32_t val);
+extern u16_t zfFlushDelayWrite(zdev_t* dev);
+extern void zfUsbInit(zdev_t* dev);
+extern u16_t zfFirmwareDownload(zdev_t* dev, u32_t* fw, u32_t len, u32_t offset);
+extern u16_t zfFirmwareDownloadNotJump(zdev_t* dev, u32_t* fw, u32_t len, u32_t offset);
+extern void zfUsbFree(zdev_t* dev);
+extern u16_t zfCwmIsExtChanBusy(u32_t ctlBusy, u32_t extBusy);
+extern void zfCoreCwmBusy(zdev_t* dev, u16_t busy);
+
+/* Prototypes */
+void zfInitRf(zdev_t* dev, u32_t frequency);
+void zfInitPhy(zdev_t* dev, u32_t frequency, u8_t bw40);
+void zfInitMac(zdev_t* dev);
+
+void zfSetPowerCalTable(zdev_t* dev, u32_t frequency, u8_t bw40, u8_t extOffset);
+void zfInitPowerCal(zdev_t* dev);
+
+#ifdef ZM_DRV_INIT_USB_MODE
+void zfInitUsbMode(zdev_t* dev);
+u16_t zfHpUsbReset(zdev_t* dev);
+#endif
+
+/* Bank 0 1 2 3 5 6 7 */
+void zfSetRfRegs(zdev_t* dev, u32_t frequency);
+/* Bank 4 */
+void zfSetBank4AndPowerTable(zdev_t* dev, u32_t frequency, u8_t bw40,
+        u8_t extOffset);
+/* Get param for turnoffdyn */
+void zfGetHwTurnOffdynParam(zdev_t* dev,
+                            u32_t frequency, u8_t bw40, u8_t extOffset,
+                            int* delta_slope_coeff_exp,
+                            int* delta_slope_coeff_man,
+                            int* delta_slope_coeff_exp_shgi,
+                            int* delta_slope_coeff_man_shgi);
+
+void zfSelAdcClk(zdev_t* dev, u8_t bw40, u32_t frequency);
+u32_t zfHpEchoCommand(zdev_t* dev, u32_t value);
+
+
+
+#define zm_hp_priv(x) (((struct zsHpPriv*)wd->hpPrivate)->x)
+struct zsHpPriv zgHpPriv;
+
+#define ZM_FIRMWARE_WLAN_ADDR           0x200000
+#define ZM_FIRMWARE_SPI_ADDR      0x114000
+/* 0: real chip     1: FPGA test */
+#define ZM_FPGA_PHY  0
+
+#define reg_write(addr, val) zfDelayWriteInternalReg(dev, addr+0x1bc000, val)
+#define zm_min(A, B) ((A>B)? B:A)
+
+
+/******************** Intialization ********************/
+u16_t zfHpInit(zdev_t* dev, u32_t frequency)
+{
+    u16_t ret;
+    zmw_get_wlan_dev(dev);
+
+    /* Initializa HAL Plus private variables */
+    wd->hpPrivate = &zgHpPriv;
+
+    ((struct zsHpPriv*)wd->hpPrivate)->halCapability = ZM_HP_CAP_11N;
+
+    ((struct zsHpPriv*)wd->hpPrivate)->hwFrequency = 0;
+    ((struct zsHpPriv*)wd->hpPrivate)->hwBw40 = 0;
+    ((struct zsHpPriv*)wd->hpPrivate)->hwExtOffset = 0;
+
+    ((struct zsHpPriv*)wd->hpPrivate)->disableDfsCh = 0;
+
+    ((struct zsHpPriv*)wd->hpPrivate)->ledMode[0] = 1;
+    ((struct zsHpPriv*)wd->hpPrivate)->ledMode[1] = 1;
+    ((struct zsHpPriv*)wd->hpPrivate)->strongRSSI = 0;
+    ((struct zsHpPriv*)wd->hpPrivate)->rxStrongRSSI = 0;
+
+    ((struct zsHpPriv*)wd->hpPrivate)->slotType = 1;
+    ((struct zsHpPriv*)wd->hpPrivate)->aggPktNum = 0x10000a;
+
+    ((struct zsHpPriv*)wd->hpPrivate)->eepromImageIndex = 0;
+
+
+    ((struct zsHpPriv*)wd->hpPrivate)->eepromImageRdReq     = 0;
+#ifdef ZM_OTUS_RX_STREAM_MODE
+    ((struct zsHpPriv*)wd->hpPrivate)->remainBuf = NULL;
+    ((struct zsHpPriv*)wd->hpPrivate)->usbRxRemainLen = 0;
+    ((struct zsHpPriv*)wd->hpPrivate)->usbRxPktLen = 0;
+    ((struct zsHpPriv*)wd->hpPrivate)->usbRxPadLen = 0;
+    ((struct zsHpPriv*)wd->hpPrivate)->usbRxTransferLen = 0;
+#endif
+
+    ((struct zsHpPriv*)wd->hpPrivate)->enableBBHeavyClip = 1;
+    ((struct zsHpPriv*)wd->hpPrivate)->hwBBHeavyClip     = 1; // force enable 8107
+    ((struct zsHpPriv*)wd->hpPrivate)->doBBHeavyClip     = 0;
+    ((struct zsHpPriv*)wd->hpPrivate)->setValueHeavyClip = 0;
+
+
+    /* Initialize driver core */
+    zfInitCmdQueue(dev);
+
+    /* Initialize USB */
+    zfUsbInit(dev);
+
+#if ZM_SW_LOOP_BACK != 1
+
+    /* TODO : [Download FW] */
+    if (wd->modeMDKEnable)
+    {
+        /* download the MDK firmware */
+        if ((ret = zfFirmwareDownload(dev, (u32_t*)zcDKFwImage,
+                (u32_t)zcDKFwImageSize, ZM_FIRMWARE_WLAN_ADDR)) != ZM_SUCCESS)
+        {
+            /* TODO : exception handling */
+            //return 1;
+        }
+    }
+    else
+    {
+    #ifndef ZM_OTUS_LINUX_PHASE_2
+        /* donwload the normal frimware */
+        if ((ret = zfFirmwareDownload(dev, (u32_t*)zcFwImage,
+                (u32_t)zcFwImageSize, ZM_FIRMWARE_WLAN_ADDR)) != ZM_SUCCESS)
+        {
+            /* TODO : exception handling */
+            //return 1;
+        }
+    #else
+
+        // 1-PH fw: ReadMac() store some global variable
+        if ((ret = zfFirmwareDownloadNotJump(dev, (u32_t*)zcFwBufImage,
+                (u32_t)zcFwBufImageSize, 0x102800)) != ZM_SUCCESS)
+        {
+            DbgPrint("Dl zcFwBufImage failed!");
+        }
+
+        zfwSleep(dev, 1000);
+
+        if ((ret = zfFirmwareDownload(dev, (u32_t*)zcFwImage,
+                (u32_t)zcFwImageSize, ZM_FIRMWARE_WLAN_ADDR)) != ZM_SUCCESS)
+        {
+            DbgPrint("Dl zcFwBufImage failed!");
+        }
+    #endif
+    }
+#endif
+
+#ifdef ZM_DRV_INIT_USB_MODE
+    /* Init USB Mode */
+    zfInitUsbMode(dev);
+
+    /* Do the USB Reset */
+    zfHpUsbReset(dev);
+#endif
+
+/* Register setting */
+/* ZM_DRIVER_MODEL_TYPE_MDK
+ *  1=>for MDK, disable init RF, PHY, and MAC,
+ *  0=>normal init
+ */
+//#if ((ZM_SW_LOOP_BACK != 1) && (ZM_DRIVER_MODEL_TYPE_MDK !=1))
+#if ZM_SW_LOOP_BACK != 1
+    if(!wd->modeMDKEnable)
+    {
+        /* Init MAC */
+        zfInitMac(dev);
+
+    #if ZM_FW_LOOP_BACK != 1
+        /* Init PHY */
+        zfInitPhy(dev, frequency, 0);
+
+        /* Init RF */
+        zfInitRf(dev, frequency);
+
+        #if ZM_FPGA_PHY == 0
+        /* BringUp issue */
+        //zfDelayWriteInternalReg(dev, 0x9800+0x1bc000, 0x10000007);
+        //zfFlushDelayWrite(dev);
+        #endif
+
+    #endif /* end of ZM_FW_LOOP_BACK != 1 */
+    }
+#endif /* end of ((ZM_SW_LOOP_BACK != 1) && (ZM_DRIVER_MODEL_TYPE_MDK !=1)) */
+
+    zfHpEchoCommand(dev, 0xAABBCCDD);
+
+    return 0;
+}
+
+
+u16_t zfHpReinit(zdev_t* dev, u32_t frequency)
+{
+    u16_t ret;
+    zmw_get_wlan_dev(dev);
+
+    ((struct zsHpPriv*)wd->hpPrivate)->halReInit = 1;
+
+    ((struct zsHpPriv*)wd->hpPrivate)->strongRSSI = 0;
+    ((struct zsHpPriv*)wd->hpPrivate)->rxStrongRSSI = 0;
+
+#ifdef ZM_OTUS_RX_STREAM_MODE
+    if (((struct zsHpPriv*)wd->hpPrivate)->remainBuf != NULL)
+    {
+        zfwBufFree(dev, ((struct zsHpPriv*)wd->hpPrivate)->remainBuf, 0);
+    }
+    ((struct zsHpPriv*)wd->hpPrivate)->remainBuf = NULL;
+    ((struct zsHpPriv*)wd->hpPrivate)->usbRxRemainLen = 0;
+    ((struct zsHpPriv*)wd->hpPrivate)->usbRxPktLen = 0;
+    ((struct zsHpPriv*)wd->hpPrivate)->usbRxPadLen = 0;
+    ((struct zsHpPriv*)wd->hpPrivate)->usbRxTransferLen = 0;
+#endif
+
+    zfInitCmdQueue(dev);
+    zfCoreReinit(dev);
+
+    #ifndef ZM_OTUS_LINUX_PHASE_2
+    /* Download firmware */
+    if ((ret = zfFirmwareDownload(dev, (u32_t*)zcFwImage,
+            (u32_t)zcFwImageSize, ZM_FIRMWARE_WLAN_ADDR)) != ZM_SUCCESS)
+    {
+        /* TODO : exception handling */
+        //return 1;
+    }
+    #else
+    if ((ret = zfFirmwareDownload(dev, (u32_t*)zcP2FwImage,
+            (u32_t)zcP2FwImageSize, ZM_FIRMWARE_WLAN_ADDR)) != ZM_SUCCESS)
+    {
+        /* TODO : exception handling */
+        //return 1;
+    }
+    #endif
+
+#ifdef ZM_DRV_INIT_USB_MODE
+    /* Init USB Mode */
+    zfInitUsbMode(dev);
+
+    /* Do the USB Reset */
+    zfHpUsbReset(dev);
+#endif
+
+    /* Init MAC */
+    zfInitMac(dev);
+
+    /* Init PHY */
+    zfInitPhy(dev, frequency, 0);
+    /* Init RF */
+    zfInitRf(dev, frequency);
+
+    #if ZM_FPGA_PHY == 0
+    /* BringUp issue */
+    //zfDelayWriteInternalReg(dev, 0x9800+0x1bc000, 0x10000007);
+    //zfFlushDelayWrite(dev);
+    #endif
+
+    zfHpEchoCommand(dev, 0xAABBCCDD);
+
+    return 0;
+}
+
+
+u16_t zfHpRelease(zdev_t* dev)
+{
+    /* Free USB resource */
+    zfUsbFree(dev);
+
+    return 0;
+}
+
+/* MDK mode setting for dontRetransmit */
+void zfHpConfigFM(zdev_t* dev, u32_t RxMaxSize, u32_t DontRetransmit)
+{
+    u32_t cmd[3];
+    u16_t ret;
+
+    cmd[0] = 8 | (ZM_CMD_CONFIG << 8);
+    cmd[1] = RxMaxSize;          /* zgRxMaxSize */
+    cmd[2] = DontRetransmit;     /* zgDontRetransmit */
+
+    ret = zfIssueCmd(dev, cmd, 12, ZM_OID_INTERNAL_WRITE, 0);
+}
+
+const u8_t zcXpdToPd[16] =
+{
+ /* 0x0, 0x1, 0x2, 0x3, 0x4, 0x5, 0x6, 0x7, 0x8, 0x9, 0xA, 0xB, 0xC, 0xD, 0xE, 0xF */
+    0x2, 0x2, 0x2, 0x1, 0x2, 0x2, 0x6, 0x2, 0x2, 0x3, 0x7, 0x2, 0xB, 0x2, 0x2, 0x2
+};
+
+/******************** RF and PHY ********************/
+
+void zfInitPhy(zdev_t* dev,  u32_t frequency, u8_t bw40)
+{
+    u16_t i, j, k;
+    u16_t entries;
+    u16_t modesIndex = 0;
+    u16_t freqIndex = 0;
+    u32_t tmp, tmp1;
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+    u32_t eepromBoardData[15][6] = {
+    /* Register   A-20        A-20/40     G-20/40     G-20        G-Turbo    */
+        {0x9964,    0,      0,      0,      0,      0},
+        {0x9960,    0,      0,      0,      0,      0},
+        {0xb960,    0,      0,      0,      0,      0},
+        {0x9844,    0,      0,      0,      0,      0},
+        {0x9850,    0,      0,      0,      0,      0},
+        {0x9834,    0,      0,      0,      0,      0},
+        {0x9828,    0,      0,      0,      0,      0},
+        {0xc864,    0,      0,      0,      0,      0},
+        {0x9848,    0,      0,      0,      0,      0},
+        {0xb848,    0,      0,      0,      0,      0},
+        {0xa20c,    0,      0,      0,      0,      0},
+        {0xc20c,    0,      0,      0,      0,      0},
+        {0x9920,    0,      0,      0,      0,      0},
+        {0xb920,    0,      0,      0,      0,      0},
+        {0xa258,    0,      0,      0,      0,      0},
+    };
+
+    /* #1 Save the initial value of the related RIFS register settings */
+    //((struct zsHpPriv*)wd->hpPrivate)->isInitialPhy++;
+
+    /*
+     * Setup the indices for the next set of register array writes
+     * PHY mode is static20 / 2040
+     * Frequency is 2.4GHz (B) / 5GHz (A)
+     */
+    if ( frequency > ZM_CH_G_14 )
+    {
+        /* 5GHz */
+        freqIndex  = 1;
+        if (bw40)
+        {
+            modesIndex = 2;
+            zm_debug_msg0("init ar5416Modes in 2: A-20/40");
+        }
+        else
+        {
+            modesIndex = 1;
+            zm_debug_msg0("init ar5416Modes in 1: A-20");
+        }
+    }
+    else
+    {
+        /* 2.4GHz */
+        freqIndex  = 2;
+        if (bw40)
+        {
+            modesIndex = 3;
+            zm_debug_msg0("init ar5416Modes in 3: G-20/40");
+        }
+        else
+        {
+            modesIndex = 4;
+            zm_debug_msg0("init ar5416Modes in 4: G-20");
+        }
+    }
+
+
+#if ZM_FPGA_PHY == 1
+    /* Starting External Hainan Register Initialization */
+    /* TODO: */
+
+    zfwSleep(dev, 10);
+#endif
+
+    /*
+     *Set correct Baseband to analog shift setting to access analog chips.
+     */
+    //reg_write(PHY_BASE, 0x00000007);
+//    reg_write(0x9800, 0x00000007);
+
+    /*
+     * Write addac shifts
+     */
+     // do this in firmware
+
+
+
+    /* Zeroize board data */
+    for (j=0; j<15; j++)
+    {
+        for (k=1; k<=4; k++)
+        {
+            eepromBoardData[j][k] = 0;
+        }
+    }
+     /*
+     * Register setting by mode
+     */
+
+    entries = sizeof(ar5416Modes) / sizeof(*ar5416Modes);
+    zm_msg1_scan(ZM_LV_2, "Modes register setting entries=", entries);
+    for (i=0; i<entries; i++)
+    {
+#if 0
+        if ( ((struct zsHpPriv*)wd->hpPrivate)->hwNotFirstInit && (ar5416Modes[i][0] == 0xa27c) )
+        {
+            /* Force disable CR671 bit20 / 7823                                            */
+            /* The bug has to do with the polarity of the pdadc offset calibration.  There */
+            /* is an initial calibration that is OK, and there is a continuous             */
+            /* calibration that updates the pddac with the wrong polarity.  Fortunately    */
+            /* the second loop can be disabled with a bit called en_pd_dc_offset_thr.      */
+
+            reg_write(ar5416Modes[i][0], (ar5416Modes[i][modesIndex]& 0xffefffff) );
+            ((struct zsHpPriv*)wd->hpPrivate)->hwNotFirstInit = 1;
+        }
+        else
+        {
+#endif
+            /* FirstTime Init or not 0xa27c(CR671) */
+            reg_write(ar5416Modes[i][0], ar5416Modes[i][modesIndex]);
+//        }
+        /* Initialize board data */
+        for (j=0; j<15; j++)
+        {
+            if (ar5416Modes[i][0] == eepromBoardData[j][0])
+            {
+                for (k=1; k<=4; k++)
+                {
+                    eepromBoardData[j][k] = ar5416Modes[i][k];
+                }
+            }
+        }
+        /* #1 Save the initial value of the related RIFS register settings */
+        //if( ((struct zsHpPriv*)wd->hpPrivate)->isInitialPhy == 1 )
+        {
+            switch(ar5416Modes[i][0])
+            {
+                case 0x9850 :
+                    ((struct zsHpPriv*)wd->hpPrivate)->initDesiredSigSize           = ar5416Modes[i][modesIndex];
+                    break;
+                case 0x985c :
+                    ((struct zsHpPriv*)wd->hpPrivate)->initAGC                      = ar5416Modes[i][modesIndex];
+                    break;
+                case 0x9860 :
+                    ((struct zsHpPriv*)wd->hpPrivate)->initAgcControl               = ar5416Modes[i][modesIndex];
+                    break;
+                case 0x9918 :
+                    ((struct zsHpPriv*)wd->hpPrivate)->initSearchStartDelay         = ar5416Modes[i][modesIndex];
+                    break;
+                case 0x99ec :
+                    ((struct zsHpPriv*)wd->hpPrivate)->initRIFSSearchParams         = ar5416Modes[i][modesIndex];
+                    break;
+                case 0xa388 :
+                    ((struct zsHpPriv*)wd->hpPrivate)->initFastChannelChangeControl = ar5416Modes[i][modesIndex];
+                default :
+                    break;
+            }
+        }
+    }
+#if 0
+    zfFlushDelayWrite(dev);
+
+    /*
+     * Common Register setting
+     */
+    entries = sizeof(ar5416Common) / sizeof(*ar5416Common);
+    for (i=0; i<entries; i++)
+    {
+        reg_write(ar5416Common[i][0], ar5416Common[i][1]);
+    }
+    zfFlushDelayWrite(dev);
+
+    /*
+     * RF Gain setting by freqIndex
+     */
+    entries = sizeof(ar5416BB_RfGain) / sizeof(*ar5416BB_RfGain);
+    for (i=0; i<entries; i++)
+    {
+        reg_write(ar5416BB_RfGain[i][0], ar5416BB_RfGain[i][freqIndex]);
+    }
+    zfFlushDelayWrite(dev);
+
+    /*
+     * Moved ar5416InitChainMask() here to ensure the swap bit is set before
+     * the pdadc table is written.  Swap must occur before any radio dependent
+     * replicated register access.  The pdadc curve addressing in particular
+     * depends on the consistent setting of the swap bit.
+     */
+    //ar5416InitChainMask(pDev);
+
+    /* Setup the transmit power values. */
+    // TODO
+#endif
+
+    /* Update 5G board data */
+    //Ant control common
+    tmp = hpPriv->eepromImage[0x100+0x144*2/4];
+    eepromBoardData[0][1] = tmp;
+    eepromBoardData[0][2] = tmp;
+    //Ant control chain 0
+    tmp = hpPriv->eepromImage[0x100+0x140*2/4];
+    eepromBoardData[1][1] = tmp;
+    eepromBoardData[1][2] = tmp;
+    //Ant control chain 2
+    tmp = hpPriv->eepromImage[0x100+0x142*2/4];
+    eepromBoardData[2][1] = tmp;
+    eepromBoardData[2][2] = tmp;
+    //SwSettle
+    tmp = hpPriv->eepromImage[0x100+0x146*2/4];
+    tmp = (tmp >> 16) & 0x7f;
+    eepromBoardData[3][1] &= (~((u32_t)0x3f80));
+    eepromBoardData[3][1] |= (tmp << 7);
+#if 0
+    //swSettleHt40
+    tmp = hpPriv->eepromImage[0x100+0x158*2/4];
+    tmp = (tmp) & 0x7f;
+    eepromBoardData[3][2] &= (~((u32_t)0x3f80));
+    eepromBoardData[3][2] |= (tmp << 7);
+#endif
+    //adcDesired, pdaDesired
+    tmp = hpPriv->eepromImage[0x100+0x148*2/4];
+    tmp = (tmp >> 24);
+    tmp1 = hpPriv->eepromImage[0x100+0x14a*2/4];
+    tmp1 = tmp1 & 0xff;
+    tmp = tmp + (tmp1<<8);
+    eepromBoardData[4][1] &= (~((u32_t)0xffff));
+    eepromBoardData[4][1] |= tmp;
+    eepromBoardData[4][2] &= (~((u32_t)0xffff));
+    eepromBoardData[4][2] |= tmp;
+    //TxEndToXpaOff, TxFrameToXpaOn
+    tmp = hpPriv->eepromImage[0x100+0x14a*2/4];
+    tmp = (tmp >> 24) & 0xff;
+    tmp1 = hpPriv->eepromImage[0x100+0x14c*2/4];
+    tmp1 = (tmp1 >> 8) & 0xff;
+    tmp = (tmp<<24) + (tmp<<16) + (tmp1<<8) + tmp1;
+    eepromBoardData[5][1] = tmp;
+    eepromBoardData[5][2] = tmp;
+    //TxEnaToRxOm
+    tmp = hpPriv->eepromImage[0x100+0x14c*2/4] & 0xff;
+    eepromBoardData[6][1] &= (~((u32_t)0xff0000));
+    eepromBoardData[6][1] |= (tmp<<16);
+    eepromBoardData[6][2] &= (~((u32_t)0xff0000));
+    eepromBoardData[6][2] |= (tmp<<16);
+    //Thresh62
+    tmp = hpPriv->eepromImage[0x100+0x14c*2/4];
+    tmp = (tmp >> 16) & 0x7f;
+    eepromBoardData[7][1] &= (~((u32_t)0x7f000));
+    eepromBoardData[7][1] |= (tmp<<12);
+    eepromBoardData[7][2] &= (~((u32_t)0x7f000));
+    eepromBoardData[7][2] |= (tmp<<12);
+    //TxRxAtten chain_0
+    tmp = hpPriv->eepromImage[0x100+0x146*2/4];
+    tmp = (tmp >> 24) & 0x3f;
+    eepromBoardData[8][1] &= (~((u32_t)0x3f000));
+    eepromBoardData[8][1] |= (tmp<<12);
+    eepromBoardData[8][2] &= (~((u32_t)0x3f000));
+    eepromBoardData[8][2] |= (tmp<<12);
+    //TxRxAtten chain_2
+    tmp = hpPriv->eepromImage[0x100+0x148*2/4] & 0x3f;
+    eepromBoardData[9][1] &= (~((u32_t)0x3f000));
+    eepromBoardData[9][1] |= (tmp<<12);
+    eepromBoardData[9][2] &= (~((u32_t)0x3f000));
+    eepromBoardData[9][2] |= (tmp<<12);
+    //TxRxMargin chain_0
+    tmp = hpPriv->eepromImage[0x100+0x148*2/4];
+    tmp = (tmp >> 8) & 0x3f;
+    eepromBoardData[10][1] &= (~((u32_t)0xfc0000));
+    eepromBoardData[10][1] |= (tmp<<18);
+    eepromBoardData[10][2] &= (~((u32_t)0xfc0000));
+    eepromBoardData[10][2] |= (tmp<<18);
+    //TxRxMargin chain_2
+    tmp = hpPriv->eepromImage[0x100+0x148*2/4];
+    tmp = (tmp >> 16) & 0x3f;
+    eepromBoardData[11][1] &= (~((u32_t)0xfc0000));
+    eepromBoardData[11][1] |= (tmp<<18);
+    eepromBoardData[11][2] &= (~((u32_t)0xfc0000));
+    eepromBoardData[11][2] |= (tmp<<18);
+    //iqCall chain_0, iqCallQ chain_0
+    tmp = hpPriv->eepromImage[0x100+0x14e*2/4];
+    tmp = (tmp >> 24) & 0x3f;
+    tmp1 = hpPriv->eepromImage[0x100+0x150*2/4];
+    tmp1 = (tmp1 >> 8) & 0x1f;
+    tmp  = (tmp<<5) + tmp1;
+    eepromBoardData[12][1] &= (~((u32_t)0x7ff));
+    eepromBoardData[12][1] |= (tmp);
+    eepromBoardData[12][2] &= (~((u32_t)0x7ff));
+    eepromBoardData[12][2] |= (tmp);
+    //iqCall chain_2, iqCallQ chain_2
+    tmp = hpPriv->eepromImage[0x100+0x150*2/4];
+    tmp = tmp & 0x3f;
+    tmp1 = hpPriv->eepromImage[0x100+0x150*2/4];
+    tmp1 = (tmp1 >> 16) & 0x1f;
+    tmp  = (tmp<<5) + tmp1;
+    eepromBoardData[13][1] &= (~((u32_t)0x7ff));
+    eepromBoardData[13][1] |= (tmp);
+    eepromBoardData[13][2] &= (~((u32_t)0x7ff));
+    eepromBoardData[13][2] |= (tmp);
+    //bsw_Margin chain_0
+    tmp = hpPriv->eepromImage[0x100+0x156*2/4];
+    tmp = (tmp >> 16) & 0xf;
+    eepromBoardData[10][1] &= (~((u32_t)0x3c00));
+    eepromBoardData[10][1] |= (tmp << 10);
+    eepromBoardData[10][2] &= (~((u32_t)0x3c00));
+    eepromBoardData[10][2] |= (tmp << 10);
+    //xpd gain mask
+    tmp = hpPriv->eepromImage[0x100+0x14e*2/4];
+    tmp = (tmp >> 8) & 0xf;
+    eepromBoardData[14][1] &= (~((u32_t)0xf0000));
+    eepromBoardData[14][1] |= (zcXpdToPd[tmp] << 16);
+    eepromBoardData[14][2] &= (~((u32_t)0xf0000));
+    eepromBoardData[14][2] |= (zcXpdToPd[tmp] << 16);
+#if 0
+    //bsw_Atten chain_0
+    tmp = hpPriv->eepromImage[0x100+0x156*2/4];
+    tmp = (tmp) & 0x1f;
+    eepromBoardData[10][1] &= (~((u32_t)0x1f));
+    eepromBoardData[10][1] |= (tmp);
+    eepromBoardData[10][2] &= (~((u32_t)0x1f));
+    eepromBoardData[10][2] |= (tmp);
+    //bsw_Margin chain_2
+    tmp = hpPriv->eepromImage[0x100+0x156*2/4];
+    tmp = (tmp >> 24) & 0xf;
+    eepromBoardData[11][1] &= (~((u32_t)0x3c00));
+    eepromBoardData[11][1] |= (tmp << 10);
+    eepromBoardData[11][2] &= (~((u32_t)0x3c00));
+    eepromBoardData[11][2] |= (tmp << 10);
+    //bsw_Atten chain_2
+    tmp = hpPriv->eepromImage[0x100+0x156*2/4];
+    tmp = (tmp >> 8) & 0x1f;
+    eepromBoardData[11][1] &= (~((u32_t)0x1f));
+    eepromBoardData[11][1] |= (tmp);
+    eepromBoardData[11][2] &= (~((u32_t)0x1f));
+    eepromBoardData[11][2] |= (tmp);
+#endif
+
+    /* Update 2.4G board data */
+    //Ant control common
+    tmp = hpPriv->eepromImage[0x100+0x170*2/4];
+    tmp = tmp >> 24;
+    tmp1 = hpPriv->eepromImage[0x100+0x172*2/4];
+    tmp = tmp + (tmp1 << 8);
+    eepromBoardData[0][3] = tmp;
+    eepromBoardData[0][4] = tmp;
+    //Ant control chain 0
+    tmp = hpPriv->eepromImage[0x100+0x16c*2/4];
+    tmp = tmp >> 24;
+    tmp1 = hpPriv->eepromImage[0x100+0x16e*2/4];
+    tmp = tmp + (tmp1 << 8);
+    eepromBoardData[1][3] = tmp;
+    eepromBoardData[1][4] = tmp;
+    //Ant control chain 2
+    tmp = hpPriv->eepromImage[0x100+0x16e*2/4];
+    tmp = tmp >> 24;
+    tmp1 = hpPriv->eepromImage[0x100+0x170*2/4];
+    tmp = tmp + (tmp1 << 8);
+    eepromBoardData[2][3] = tmp;
+    eepromBoardData[2][4] = tmp;
+    //SwSettle
+    tmp = hpPriv->eepromImage[0x100+0x174*2/4];
+    tmp = (tmp >> 8) & 0x7f;
+    eepromBoardData[3][4] &= (~((u32_t)0x3f80));
+    eepromBoardData[3][4] |= (tmp << 7);
+#if 0
+    //swSettleHt40
+    tmp = hpPriv->eepromImage[0x100+0x184*2/4];
+    tmp = (tmp >> 24) & 0x7f;
+    eepromBoardData[3][3] &= (~((u32_t)0x3f80));
+    eepromBoardData[3][3] |= (tmp << 7);
+#endif
+    //adcDesired, pdaDesired
+    tmp = hpPriv->eepromImage[0x100+0x176*2/4];
+    tmp = (tmp >> 16) & 0xff;
+    tmp1 = hpPriv->eepromImage[0x100+0x176*2/4];
+    tmp1 = tmp1 >> 24;
+    tmp = tmp + (tmp1<<8);
+    eepromBoardData[4][3] &= (~((u32_t)0xffff));
+    eepromBoardData[4][3] |= tmp;
+    eepromBoardData[4][4] &= (~((u32_t)0xffff));
+    eepromBoardData[4][4] |= tmp;
+    //TxEndToXpaOff, TxFrameToXpaOn
+    tmp = hpPriv->eepromImage[0x100+0x178*2/4];
+    tmp = (tmp >> 16) & 0xff;
+    tmp1 = hpPriv->eepromImage[0x100+0x17a*2/4];
+    tmp1 = tmp1 & 0xff;
+    tmp = (tmp << 24) + (tmp << 16) + (tmp1 << 8) + tmp1;
+    eepromBoardData[5][3] = tmp;
+    eepromBoardData[5][4] = tmp;
+    //TxEnaToRxOm
+    tmp = hpPriv->eepromImage[0x100+0x178*2/4];
+    tmp = (tmp >> 24);
+    eepromBoardData[6][3] &= (~((u32_t)0xff0000));
+    eepromBoardData[6][3] |= (tmp<<16);
+    eepromBoardData[6][4] &= (~((u32_t)0xff0000));
+    eepromBoardData[6][4] |= (tmp<<16);
+    //Thresh62
+    tmp = hpPriv->eepromImage[0x100+0x17a*2/4];
+    tmp = (tmp >> 8) & 0x7f;
+    eepromBoardData[7][3] &= (~((u32_t)0x7f000));
+    eepromBoardData[7][3] |= (tmp<<12);
+    eepromBoardData[7][4] &= (~((u32_t)0x7f000));
+    eepromBoardData[7][4] |= (tmp<<12);
+    //TxRxAtten chain_0
+    tmp = hpPriv->eepromImage[0x100+0x174*2/4];
+    tmp = (tmp >> 16) & 0x3f;
+    eepromBoardData[8][3] &= (~((u32_t)0x3f000));
+    eepromBoardData[8][3] |= (tmp<<12);
+    eepromBoardData[8][4] &= (~((u32_t)0x3f000));
+    eepromBoardData[8][4] |= (tmp<<12);
+    //TxRxAtten chain_2
+    tmp = hpPriv->eepromImage[0x100+0x174*2/4];
+    tmp = (tmp >> 24) & 0x3f;
+    eepromBoardData[9][3] &= (~((u32_t)0x3f000));
+    eepromBoardData[9][3] |= (tmp<<12);
+    eepromBoardData[9][4] &= (~((u32_t)0x3f000));
+    eepromBoardData[9][4] |= (tmp<<12);
+    //TxRxMargin chain_0
+    tmp = hpPriv->eepromImage[0x100+0x176*2/4];
+    tmp = (tmp) & 0x3f;
+    eepromBoardData[10][3] &= (~((u32_t)0xfc0000));
+    eepromBoardData[10][3] |= (tmp<<18);
+    eepromBoardData[10][4] &= (~((u32_t)0xfc0000));
+    eepromBoardData[10][4] |= (tmp<<18);
+    //TxRxMargin chain_2
+    tmp = hpPriv->eepromImage[0x100+0x176*2/4];
+    tmp = (tmp >> 8) & 0x3f;
+    eepromBoardData[11][3] &= (~((u32_t)0xfc0000));
+    eepromBoardData[11][3] |= (tmp<<18);
+    eepromBoardData[11][4] &= (~((u32_t)0xfc0000));
+    eepromBoardData[11][4] |= (tmp<<18);
+    //iqCall chain_0, iqCallQ chain_0
+    tmp = hpPriv->eepromImage[0x100+0x17c*2/4];
+    tmp = (tmp >> 16) & 0x3f;
+    tmp1 = hpPriv->eepromImage[0x100+0x17e*2/4];
+    tmp1 = (tmp1) & 0x1f;
+    tmp  = (tmp<<5) + tmp1;
+    eepromBoardData[12][3] &= (~((u32_t)0x7ff));
+    eepromBoardData[12][3] |= (tmp);
+    eepromBoardData[12][4] &= (~((u32_t)0x7ff));
+    eepromBoardData[12][4] |= (tmp);
+    //iqCall chain_2, iqCallQ chain_2
+    tmp = hpPriv->eepromImage[0x100+0x17c*2/4];
+    tmp = (tmp>>24) & 0x3f;
+    tmp1 = hpPriv->eepromImage[0x100+0x17e*2/4];
+    tmp1 = (tmp1 >> 8) & 0x1f;
+    tmp  = (tmp<<5) + tmp1;
+    eepromBoardData[13][3] &= (~((u32_t)0x7ff));
+    eepromBoardData[13][3] |= (tmp);
+    eepromBoardData[13][4] &= (~((u32_t)0x7ff));
+    eepromBoardData[13][4] |= (tmp);
+    //xpd gain mask
+    tmp = hpPriv->eepromImage[0x100+0x17c*2/4];
+    tmp = tmp & 0xf;
+    DbgPrint("xpd=0x%x, pd=0x%x\n", tmp, zcXpdToPd[tmp]);
+    eepromBoardData[14][3] &= (~((u32_t)0xf0000));
+    eepromBoardData[14][3] |= (zcXpdToPd[tmp] << 16);
+    eepromBoardData[14][4] &= (~((u32_t)0xf0000));
+    eepromBoardData[14][4] |= (zcXpdToPd[tmp] << 16);
+#if 0
+    //bsw_Margin chain_0
+    tmp = hpPriv->eepromImage[0x100+0x184*2/4];
+    tmp = (tmp >> 8) & 0xf;
+    eepromBoardData[10][3] &= (~((u32_t)0x3c00));
+    eepromBoardData[10][3] |= (tmp << 10);
+    eepromBoardData[10][4] &= (~((u32_t)0x3c00));
+    eepromBoardData[10][4] |= (tmp << 10);
+    //bsw_Atten chain_0
+    tmp = hpPriv->eepromImage[0x100+0x182*2/4];
+    tmp = (tmp>>24) & 0x1f;
+    eepromBoardData[10][3] &= (~((u32_t)0x1f));
+    eepromBoardData[10][3] |= (tmp);
+    eepromBoardData[10][4] &= (~((u32_t)0x1f));
+    eepromBoardData[10][4] |= (tmp);
+    //bsw_Margin chain_2
+    tmp = hpPriv->eepromImage[0x100+0x184*2/4];
+    tmp = (tmp >> 16) & 0xf;
+    eepromBoardData[11][3] &= (~((u32_t)0x3c00));
+    eepromBoardData[11][3] |= (tmp << 10);
+    eepromBoardData[11][4] &= (~((u32_t)0x3c00));
+    eepromBoardData[11][4] |= (tmp << 10);
+    //bsw_Atten chain_2
+    tmp = hpPriv->eepromImage[0x100+0x184*2/4];
+    tmp = (tmp) & 0x1f;
+    eepromBoardData[11][3] &= (~((u32_t)0x1f));
+    eepromBoardData[11][3] |= (tmp);
+    eepromBoardData[11][4] &= (~((u32_t)0x1f));
+    eepromBoardData[11][4] |= (tmp);
+#endif
+
+#if 0
+    for (j=0; j<14; j++)
+    {
+        DbgPrint("%04x, %08x, %08x, %08x, %08x\n", eepromBoardData[j][0], eepromBoardData[j][1], eepromBoardData[j][2], eepromBoardData[j][3], eepromBoardData[j][4]);
+    }
+#endif
+
+    if ((hpPriv->eepromImage[0x100+0x110*2/4]&0xff) == 0x80) //FEM TYPE
+    {
+        /* Update board data to registers */
+        for (j=0; j<15; j++)
+        {
+            reg_write(eepromBoardData[j][0], eepromBoardData[j][modesIndex]);
+
+            /* #1 Save the initial value of the related RIFS register settings */
+            //if( ((struct zsHpPriv*)wd->hpPrivate)->isInitialPhy == 1 )
+            {
+                switch(eepromBoardData[j][0])
+                {
+                    case 0x9850 :
+                        ((struct zsHpPriv*)wd->hpPrivate)->initDesiredSigSize           = eepromBoardData[j][modesIndex];
+                        break;
+                    case 0x985c :
+                        ((struct zsHpPriv*)wd->hpPrivate)->initAGC                      = eepromBoardData[j][modesIndex];
+                        break;
+                    case 0x9860 :
+                        ((struct zsHpPriv*)wd->hpPrivate)->initAgcControl               = eepromBoardData[j][modesIndex];
+                        break;
+                    case 0x9918 :
+                        ((struct zsHpPriv*)wd->hpPrivate)->initSearchStartDelay         = eepromBoardData[j][modesIndex];
+                        break;
+                    case 0x99ec :
+                        ((struct zsHpPriv*)wd->hpPrivate)->initRIFSSearchParams         = eepromBoardData[j][modesIndex];
+                        break;
+                    case 0xa388 :
+                        ((struct zsHpPriv*)wd->hpPrivate)->initFastChannelChangeControl = eepromBoardData[j][modesIndex];
+                    default :
+                        break;
+                }
+            }
+        }
+    } /* if ((hpPriv->eepromImage[0x100+0x110*2/4]&0xff) == 0x80) //FEM TYPE */
+
+
+    /* Bringup issue : force tx gain */
+    //reg_write(0xa258, 0x0cc65381);
+    //reg_write(0xa274, 0x0a1a7c15);
+    zfInitPowerCal(dev);
+
+    if(frequency > ZM_CH_G_14)
+    {
+        zfDelayWriteInternalReg(dev, 0x1d4014, 0x5143);
+    }
+    else
+    {
+        zfDelayWriteInternalReg(dev, 0x1d4014, 0x5163);
+    }
+
+    zfFlushDelayWrite(dev);
+}
+
+
+void zfInitRf(zdev_t* dev, u32_t frequency)
+{
+    u32_t cmd[8];
+    u16_t ret;
+    int delta_slope_coeff_exp;
+    int delta_slope_coeff_man;
+    int delta_slope_coeff_exp_shgi;
+    int delta_slope_coeff_man_shgi;
+
+    zmw_get_wlan_dev(dev);
+
+    zm_debug_msg1(" initRf frequency = ", frequency);
+
+    if (frequency == 0)
+    {
+        frequency = 2412;
+    }
+
+    /* Bank 0 1 2 3 5 6 7 */
+    zfSetRfRegs(dev, frequency);
+    /* Bank 4 */
+    zfSetBank4AndPowerTable(dev, frequency, 0, 0);
+
+    /* stroe frequency */
+    ((struct zsHpPriv*)wd->hpPrivate)->hwFrequency = (u16_t)frequency;
+
+    zfGetHwTurnOffdynParam(dev,
+                           frequency, 0, 0,
+                           &delta_slope_coeff_exp,
+                           &delta_slope_coeff_man,
+                           &delta_slope_coeff_exp_shgi,
+                           &delta_slope_coeff_man_shgi);
+
+    /* related functions */
+    frequency = frequency*1000;
+    cmd[0] = 28 | (ZM_CMD_RF_INIT << 8);
+    cmd[1] = frequency;
+    cmd[2] = 0;//((struct zsHpPriv*)wd->hpPrivate)->hw_DYNAMIC_HT2040_EN;
+    cmd[3] = 1;//((wd->ExtOffset << 2) | ((struct zsHpPriv*)wd->hpPrivate)->hw_HT_ENABLE);
+    cmd[4] = delta_slope_coeff_exp;
+    cmd[5] = delta_slope_coeff_man;
+    cmd[6] = delta_slope_coeff_exp_shgi;
+    cmd[7] = delta_slope_coeff_man_shgi;
+
+    ret = zfIssueCmd(dev, cmd, 32, ZM_OID_INTERNAL_WRITE, 0);
+
+    // delay temporarily, wait for new PHY and RF
+    zfwSleep(dev, 1000);
+}
+
+int tn(int exp)
+{
+    int i;
+	int tmp = 1;
+    for(i=0; i<exp; i++)
+        tmp = tmp*2;
+
+    return tmp;
+}
+
+/*int zfFloor(double indata)
+{
+   if(indata<0)
+	   return (int)indata-1;
+   else
+	   return (int)indata;
+}
+*/
+u32_t reverse_bits(u32_t chan_sel)
+{
+	/* reverse_bits */
+    u32_t chansel = 0;
+	u8_t i;
+
+	for (i=0; i<8; i++)
+        chansel |= ((chan_sel>>(7-i) & 0x1) << i);
+	return chansel;
+}
+
+/* Bank 0 1 2 3 5 6 7 */
+void zfSetRfRegs(zdev_t* dev, u32_t frequency)
+{
+    u16_t entries;
+    u16_t freqIndex = 0;
+    u16_t i;
+
+    //zmw_get_wlan_dev(dev);
+
+    if ( frequency > ZM_CH_G_14 )
+    {
+        /* 5G */
+        freqIndex = 1;
+        zm_msg0_scan(ZM_LV_2, "Set to 5GHz");
+
+    }
+    else
+    {
+        /* 2.4G */
+        freqIndex = 2;
+        zm_msg0_scan(ZM_LV_2, "Set to 2.4GHz");
+    }
+
+#if 1
+    entries = sizeof(otusBank) / sizeof(*otusBank);
+    for (i=0; i<entries; i++)
+    {
+        reg_write(otusBank[i][0], otusBank[i][freqIndex]);
+    }
+#else
+    /* Bank0 */
+    entries = sizeof(ar5416Bank0) / sizeof(*ar5416Bank0);
+    for (i=0; i<entries; i++)
+    {
+        reg_write(ar5416Bank0[i][0], ar5416Bank0[i][1]);
+    }
+    /* Bank1 */
+    entries = sizeof(ar5416Bank1) / sizeof(*ar5416Bank1);
+    for (i=0; i<entries; i++)
+    {
+        reg_write(ar5416Bank1[i][0], ar5416Bank1[i][1]);
+    }
+    /* Bank2 */
+    entries = sizeof(ar5416Bank2) / sizeof(*ar5416Bank2);
+    for (i=0; i<entries; i++)
+    {
+        reg_write(ar5416Bank2[i][0], ar5416Bank2[i][1]);
+    }
+    /* Bank3 */
+    entries = sizeof(ar5416Bank3) / sizeof(*ar5416Bank3);
+    for (i=0; i<entries; i++)
+    {
+        reg_write(ar5416Bank3[i][0], ar5416Bank3[i][freqIndex]);
+    }
+    /* Bank5 */
+    reg_write (0x98b0,  0x00000013);
+    reg_write (0x98e4,  0x00000002);
+    /* Bank6 */
+    entries = sizeof(ar5416Bank6) / sizeof(*ar5416Bank6);
+    for (i=0; i<entries; i++)
+    {
+        reg_write(ar5416Bank6[i][0], ar5416Bank6[i][freqIndex]);
+    }
+    /* Bank7 */
+    entries = sizeof(ar5416Bank7) / sizeof(*ar5416Bank7);
+    for (i=0; i<entries; i++)
+    {
+        reg_write(ar5416Bank7[i][0], ar5416Bank7[i][1]);
+    }
+#endif
+
+    zfFlushDelayWrite(dev);
+}
+
+/* Bank 4 */
+void zfSetBank4AndPowerTable(zdev_t* dev, u32_t frequency, u8_t bw40,
+        u8_t extOffset)
+{
+    u32_t chup = 1;
+	u32_t bmode_LF_synth_freq = 0;
+	u32_t amode_refsel_1 = 0;
+	u32_t amode_refsel_0 = 1;
+	u32_t addr2 = 1;
+	u32_t addr1 = 0;
+	u32_t addr0 = 0;
+
+	u32_t d1;
+	u32_t d0;
+	u32_t tmp_0;
+	u32_t tmp_1;
+	u32_t data0;
+	u32_t data1;
+
+	u8_t chansel;
+	u8_t chan_sel;
+	u32_t temp_chan_sel;
+
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+
+
+    /* if enable 802.11h, need to record curent channel index in channel array */
+    if (wd->sta.DFSEnable)
+    {
+        for (i = 0; i < wd->regulationTable.allowChannelCnt; i++)
+        {
+            if (wd->regulationTable.allowChannel[i].channel == frequency)
+                break;
+        }
+        wd->regulationTable.CurChIndex = i;
+    }
+
+	if (bw40 == 1)
+	{
+        if (extOffset == 1)
+        {
+            frequency += 10;
+        }
+        else
+        {
+            frequency -= 10;
+        }
+
+	}
+
+
+	if ( frequency > 3000 )
+	{
+	    if ( frequency % 10 )
+	    {
+	        /* 5M */
+            chan_sel = (u8_t)((frequency - 4800)/5);
+            chan_sel = (u8_t)(chan_sel & 0xff);
+            chansel  = (u8_t)reverse_bits(chan_sel);
+        }
+        else
+        {
+            /* 10M : improve Tx EVM */
+            chan_sel = (u8_t)((frequency - 4800)/10);
+            chan_sel = (u8_t)(chan_sel & 0xff)<<1;
+            chansel  = (u8_t)reverse_bits(chan_sel);
+
+	        amode_refsel_1 = 1;
+	        amode_refsel_0 = 0;
+        }
+	}
+	else
+	{
+        //temp_chan_sel = (((frequency - 672)*2) - 3040)/10;
+        if (frequency == 2484)
+        {
+   	        temp_chan_sel = 10 + (frequency - 2274)/5 ;
+   	        bmode_LF_synth_freq = 1;
+        }
+        else
+        {
+            temp_chan_sel = 16 + (frequency - 2272)/5 ;
+            bmode_LF_synth_freq = 0;
+        }
+        chan_sel = (u8_t)(temp_chan_sel << 2) & 0xff;
+        chansel  = (u8_t)reverse_bits(chan_sel);
+	}
+
+	d1   = chansel;   //# 8 bits of chan
+	d0   = addr0<<7 | addr1<<6 | addr2<<5
+			| amode_refsel_0<<3 | amode_refsel_1<<2
+			| bmode_LF_synth_freq<<1 | chup;
+
+    tmp_0 = d0 & 0x1f;  //# 5-1
+    tmp_1 = d1 & 0x1f;  //# 5-1
+    data0 = tmp_1<<5 | tmp_0;
+
+    tmp_0 = d0>>5 & 0x7;  //# 8-6
+    tmp_1 = d1>>5 & 0x7;  //# 8-6
+    data1 = tmp_1<<5 | tmp_0;
+
+    /* Bank4 */
+	reg_write (0x9800+(0x2c<<2), data0);
+	reg_write (0x9800+(0x3a<<2), data1);
+	//zm_debug_msg1("0x9800+(0x2c<<2 =  ", data0);
+	//zm_debug_msg1("0x9800+(0x3a<<2 =  ", data1);
+
+
+    zfFlushDelayWrite(dev);
+
+    zfwSleep(dev, 10);
+
+    return;
+}
+
+
+struct zsPhyFreqPara
+{
+    u32_t coeff_exp;
+    u32_t coeff_man;
+    u32_t coeff_exp_shgi;
+    u32_t coeff_man_shgi;
+};
+
+struct zsPhyFreqTable
+{
+    u32_t frequency;
+    struct zsPhyFreqPara FpgaDynamicHT;
+    struct zsPhyFreqPara FpgaStaticHT;
+    struct zsPhyFreqPara ChipST20Mhz;
+    struct zsPhyFreqPara Chip2040Mhz;
+    struct zsPhyFreqPara Chip2040ExtAbove;
+};
+
+const struct zsPhyFreqTable zgPhyFreqCoeff[] =
+{
+/*Index   freq  FPGA DYNAMIC_HT2040_EN  FPGA STATIC_HT20    Real Chip static20MHz     Real Chip 2040MHz   Real Chip 2040Mhz  */
+       /* fclk =         10.8                21.6                  40                  ext below 40       ext above 40       */
+/*  0 */ {2412, {5, 23476, 5, 21128}, {4, 23476, 4, 21128}, {3, 21737, 3, 19563}, {3, 21827, 3, 19644}, {3, 21647, 3, 19482}},
+/*  1 */ {2417, {5, 23427, 5, 21084}, {4, 23427, 4, 21084}, {3, 21692, 3, 19523}, {3, 21782, 3, 19604}, {3, 21602, 3, 19442}},
+/*  2 */ {2422, {5, 23379, 5, 21041}, {4, 23379, 4, 21041}, {3, 21647, 3, 19482}, {3, 21737, 3, 19563}, {3, 21558, 3, 19402}},
+/*  3 */ {2427, {5, 23330, 5, 20997}, {4, 23330, 4, 20997}, {3, 21602, 3, 19442}, {3, 21692, 3, 19523}, {3, 21514, 3, 19362}},
+/*  4 */ {2432, {5, 23283, 5, 20954}, {4, 23283, 4, 20954}, {3, 21558, 3, 19402}, {3, 21647, 3, 19482}, {3, 21470, 3, 19323}},
+/*  5 */ {2437, {5, 23235, 5, 20911}, {4, 23235, 4, 20911}, {3, 21514, 3, 19362}, {3, 21602, 3, 19442}, {3, 21426, 3, 19283}},
+/*  6 */ {2442, {5, 23187, 5, 20868}, {4, 23187, 4, 20868}, {3, 21470, 3, 19323}, {3, 21558, 3, 19402}, {3, 21382, 3, 19244}},
+/*  7 */ {2447, {5, 23140, 5, 20826}, {4, 23140, 4, 20826}, {3, 21426, 3, 19283}, {3, 21514, 3, 19362}, {3, 21339, 3, 19205}},
+/*  8 */ {2452, {5, 23093, 5, 20783}, {4, 23093, 4, 20783}, {3, 21382, 3, 19244}, {3, 21470, 3, 19323}, {3, 21295, 3, 19166}},
+/*  9 */ {2457, {5, 23046, 5, 20741}, {4, 23046, 4, 20741}, {3, 21339, 3, 19205}, {3, 21426, 3, 19283}, {3, 21252, 3, 19127}},
+/* 10 */ {2462, {5, 22999, 5, 20699}, {4, 22999, 4, 20699}, {3, 21295, 3, 19166}, {3, 21382, 3, 19244}, {3, 21209, 3, 19088}},
+/* 11 */ {2467, {5, 22952, 5, 20657}, {4, 22952, 4, 20657}, {3, 21252, 3, 19127}, {3, 21339, 3, 19205}, {3, 21166, 3, 19050}},
+/* 12 */ {2472, {5, 22906, 5, 20615}, {4, 22906, 4, 20615}, {3, 21209, 3, 19088}, {3, 21295, 3, 19166}, {3, 21124, 3, 19011}},
+/* 13 */ {2484, {5, 22795, 5, 20516}, {4, 22795, 4, 20516}, {3, 21107, 3, 18996}, {3, 21192, 3, 19073}, {3, 21022, 3, 18920}},
+/* 14 */ {4920, {6, 23018, 6, 20716}, {5, 23018, 5, 20716}, {4, 21313, 4, 19181}, {4, 21356, 4, 19220}, {4, 21269, 4, 19142}},
+/* 15 */ {4940, {6, 22924, 6, 20632}, {5, 22924, 5, 20632}, {4, 21226, 4, 19104}, {4, 21269, 4, 19142}, {4, 21183, 4, 19065}},
+/* 16 */ {4960, {6, 22832, 6, 20549}, {5, 22832, 5, 20549}, {4, 21141, 4, 19027}, {4, 21183, 4, 19065}, {4, 21098, 4, 18988}},
+/* 17 */ {4980, {6, 22740, 6, 20466}, {5, 22740, 5, 20466}, {4, 21056, 4, 18950}, {4, 21098, 4, 18988}, {4, 21014, 4, 18912}},
+/* 18 */ {5040, {6, 22469, 6, 20223}, {5, 22469, 5, 20223}, {4, 20805, 4, 18725}, {4, 20846, 4, 18762}, {4, 20764, 4, 18687}},
+/* 19 */ {5060, {6, 22381, 6, 20143}, {5, 22381, 5, 20143}, {4, 20723, 4, 18651}, {4, 20764, 4, 18687}, {4, 20682, 4, 18614}},
+/* 20 */ {5080, {6, 22293, 6, 20063}, {5, 22293, 5, 20063}, {4, 20641, 4, 18577}, {4, 20682, 4, 18614}, {4, 20601, 4, 18541}},
+/* 21 */ {5180, {6, 21862, 6, 19676}, {5, 21862, 5, 19676}, {4, 20243, 4, 18219}, {4, 20282, 4, 18254}, {4, 20204, 4, 18183}},
+/* 22 */ {5200, {6, 21778, 6, 19600}, {5, 21778, 5, 19600}, {4, 20165, 4, 18148}, {4, 20204, 4, 18183}, {4, 20126, 4, 18114}},
+/* 23 */ {5220, {6, 21695, 6, 19525}, {5, 21695, 5, 19525}, {4, 20088, 4, 18079}, {4, 20126, 4, 18114}, {4, 20049, 4, 18044}},
+/* 24 */ {5240, {6, 21612, 6, 19451}, {5, 21612, 5, 19451}, {4, 20011, 4, 18010}, {4, 20049, 4, 18044}, {4, 19973, 4, 17976}},
+/* 25 */ {5260, {6, 21530, 6, 19377}, {5, 21530, 5, 19377}, {4, 19935, 4, 17941}, {4, 19973, 4, 17976}, {4, 19897, 4, 17907}},
+/* 26 */ {5280, {6, 21448, 6, 19303}, {5, 21448, 5, 19303}, {4, 19859, 4, 17873}, {4, 19897, 4, 17907}, {4, 19822, 4, 17840}},
+/* 27 */ {5300, {6, 21367, 6, 19230}, {5, 21367, 5, 19230}, {4, 19784, 4, 17806}, {4, 19822, 4, 17840}, {4, 19747, 4, 17772}},
+/* 28 */ {5320, {6, 21287, 6, 19158}, {5, 21287, 5, 19158}, {4, 19710, 4, 17739}, {4, 19747, 4, 17772}, {4, 19673, 4, 17706}},
+/* 29 */ {5500, {6, 20590, 6, 18531}, {5, 20590, 5, 18531}, {4, 19065, 4, 17159}, {4, 19100, 4, 17190}, {4, 19030, 4, 17127}},
+/* 30 */ {5520, {6, 20516, 6, 18464}, {5, 20516, 5, 18464}, {4, 18996, 4, 17096}, {4, 19030, 4, 17127}, {4, 18962, 4, 17065}},
+/* 31 */ {5540, {6, 20442, 6, 18397}, {5, 20442, 5, 18397}, {4, 18927, 4, 17035}, {4, 18962, 4, 17065}, {4, 18893, 4, 17004}},
+/* 32 */ {5560, {6, 20368, 6, 18331}, {5, 20368, 5, 18331}, {4, 18859, 4, 16973}, {4, 18893, 4, 17004}, {4, 18825, 4, 16943}},
+/* 33 */ {5580, {6, 20295, 6, 18266}, {5, 20295, 5, 18266}, {4, 18792, 4, 16913}, {4, 18825, 4, 16943}, {4, 18758, 4, 16882}},
+/* 34 */ {5600, {6, 20223, 6, 18200}, {5, 20223, 5, 18200}, {4, 18725, 4, 16852}, {4, 18758, 4, 16882}, {4, 18691, 4, 16822}},
+/* 35 */ {5620, {6, 20151, 6, 18136}, {5, 20151, 5, 18136}, {4, 18658, 4, 16792}, {4, 18691, 4, 16822}, {4, 18625, 4, 16762}},
+/* 36 */ {5640, {6, 20079, 6, 18071}, {5, 20079, 5, 18071}, {4, 18592, 4, 16733}, {4, 18625, 4, 16762}, {4, 18559, 4, 16703}},
+/* 37 */ {5660, {6, 20008, 6, 18007}, {5, 20008, 5, 18007}, {4, 18526, 4, 16673}, {4, 18559, 4, 16703}, {4, 18493, 4, 16644}},
+/* 38 */ {5680, {6, 19938, 6, 17944}, {5, 19938, 5, 17944}, {4, 18461, 4, 16615}, {4, 18493, 4, 16644}, {4, 18428, 4, 16586}},
+/* 39 */ {5700, {6, 19868, 6, 17881}, {5, 19868, 5, 17881}, {4, 18396, 4, 16556}, {4, 18428, 4, 16586}, {4, 18364, 4, 16527}},
+/* 40 */ {5745, {6, 19712, 6, 17741}, {5, 19712, 5, 17741}, {4, 18252, 4, 16427}, {4, 18284, 4, 16455}, {4, 18220, 4, 16398}},
+/* 41 */ {5765, {6, 19644, 6, 17679}, {5, 19644, 5, 17679}, {4, 18189, 5, 32740}, {4, 18220, 4, 16398}, {4, 18157, 5, 32683}},
+/* 42 */ {5785, {6, 19576, 6, 17618}, {5, 19576, 5, 17618}, {4, 18126, 5, 32626}, {4, 18157, 5, 32683}, {4, 18094, 5, 32570}},
+/* 43 */ {5805, {6, 19508, 6, 17558}, {5, 19508, 5, 17558}, {4, 18063, 5, 32514}, {4, 18094, 5, 32570}, {4, 18032, 5, 32458}},
+/* 44 */ {5825, {6, 19441, 6, 17497}, {5, 19441, 5, 17497}, {4, 18001, 5, 32402}, {4, 18032, 5, 32458}, {4, 17970, 5, 32347}},
+/* 45 */ {5170, {6, 21904, 6, 19714}, {5, 21904, 5, 19714}, {4, 20282, 4, 18254}, {4, 20321, 4, 18289}, {4, 20243, 4, 18219}},
+/* 46 */ {5190, {6, 21820, 6, 19638}, {5, 21820, 5, 19638}, {4, 20204, 4, 18183}, {4, 20243, 4, 18219}, {4, 20165, 4, 18148}},
+/* 47 */ {5210, {6, 21736, 6, 19563}, {5, 21736, 5, 19563}, {4, 20126, 4, 18114}, {4, 20165, 4, 18148}, {4, 20088, 4, 18079}},
+/* 48 */ {5230, {6, 21653, 6, 19488}, {5, 21653, 5, 19488}, {4, 20049, 4, 18044}, {4, 20088, 4, 18079}, {4, 20011, 4, 18010}}
+};
+/* to reduce search time, please modify this define if you add or delete channel in table */
+#define First5GChannelIndex 14
+
+void zfGetHwTurnOffdynParam(zdev_t* dev,
+                            u32_t frequency, u8_t bw40, u8_t extOffset,
+                            int* delta_slope_coeff_exp,
+                            int* delta_slope_coeff_man,
+                            int* delta_slope_coeff_exp_shgi,
+                            int* delta_slope_coeff_man_shgi)
+{
+    /* Get param for turnoffdyn */
+    u16_t i, arraySize;
+
+    //zmw_get_wlan_dev(dev);
+
+    arraySize = sizeof(zgPhyFreqCoeff)/sizeof(struct zsPhyFreqTable);
+    if (frequency < 3000)
+    {
+        /* 2.4GHz Channel */
+        for (i = 0; i < First5GChannelIndex; i++)
+        {
+            if (frequency == zgPhyFreqCoeff[i].frequency)
+                break;
+        }
+
+        if (i < First5GChannelIndex)
+        {
+        }
+        else
+        {
+            zm_msg1_scan(ZM_LV_0, "Unsupported 2.4G frequency = ", frequency);
+            return;
+        }
+    }
+    else
+    {
+        /* 5GHz Channel */
+        for (i = First5GChannelIndex; i < arraySize; i++)
+        {
+            if (frequency == zgPhyFreqCoeff[i].frequency)
+                break;
+        }
+
+        if (i < arraySize)
+        {
+        }
+        else
+        {
+            zm_msg1_scan(ZM_LV_0, "Unsupported 5G frequency = ", frequency);
+            return;
+        }
+    }
+
+    /* FPGA DYNAMIC_HT2040_EN        fclk = 10.8  */
+    /* FPGA STATIC_HT20_             fclk = 21.6  */
+    /* Real Chip                     fclk = 40    */
+    #if ZM_FPGA_PHY == 1
+    //fclk = 10.8;
+    *delta_slope_coeff_exp = zgPhyFreqCoeff[i].FpgaDynamicHT.coeff_exp;
+    *delta_slope_coeff_man = zgPhyFreqCoeff[i].FpgaDynamicHT.coeff_man;
+    *delta_slope_coeff_exp_shgi = zgPhyFreqCoeff[i].FpgaDynamicHT.coeff_exp_shgi;
+    *delta_slope_coeff_man_shgi = zgPhyFreqCoeff[i].FpgaDynamicHT.coeff_man_shgi;
+    #else
+    //fclk = 40;
+    if (bw40)
+    {
+        /* ht2040 */
+        if (extOffset == 1) {
+            *delta_slope_coeff_exp = zgPhyFreqCoeff[i].Chip2040ExtAbove.coeff_exp;
+            *delta_slope_coeff_man = zgPhyFreqCoeff[i].Chip2040ExtAbove.coeff_man;
+            *delta_slope_coeff_exp_shgi = zgPhyFreqCoeff[i].Chip2040ExtAbove.coeff_exp_shgi;
+            *delta_slope_coeff_man_shgi = zgPhyFreqCoeff[i].Chip2040ExtAbove.coeff_man_shgi;
+        }
+        else {
+            *delta_slope_coeff_exp = zgPhyFreqCoeff[i].Chip2040Mhz.coeff_exp;
+            *delta_slope_coeff_man = zgPhyFreqCoeff[i].Chip2040Mhz.coeff_man;
+            *delta_slope_coeff_exp_shgi = zgPhyFreqCoeff[i].Chip2040Mhz.coeff_exp_shgi;
+            *delta_slope_coeff_man_shgi = zgPhyFreqCoeff[i].Chip2040Mhz.coeff_man_shgi;
+        }
+    }
+    else
+    {
+        /* static 20 */
+        *delta_slope_coeff_exp = zgPhyFreqCoeff[i].ChipST20Mhz.coeff_exp;
+        *delta_slope_coeff_man = zgPhyFreqCoeff[i].ChipST20Mhz.coeff_man;
+        *delta_slope_coeff_exp_shgi = zgPhyFreqCoeff[i].ChipST20Mhz.coeff_exp_shgi;
+        *delta_slope_coeff_man_shgi = zgPhyFreqCoeff[i].ChipST20Mhz.coeff_man_shgi;
+    }
+    #endif
+}
+
+/* Main routin frequency setting function */
+/* If 2.4G/5G switch, PHY need resetting BB and RF for band switch */
+/* Do the setting switch in zfSendFrequencyCmd() */
+void zfHpSetFrequencyEx(zdev_t* dev, u32_t frequency, u8_t bw40,
+        u8_t extOffset, u8_t initRF)
+{
+    u32_t cmd[9];
+    u32_t cmdB[3];
+    u16_t ret;
+    u8_t old_band;
+    u8_t new_band;
+    u32_t checkLoopCount;
+    u32_t tmpValue;
+
+    int delta_slope_coeff_exp;
+    int delta_slope_coeff_man;
+    int delta_slope_coeff_exp_shgi;
+    int delta_slope_coeff_man_shgi;
+
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv = wd->hpPrivate;
+
+    zm_msg1_scan(ZM_LV_1, "Frequency = ", frequency);
+    zm_msg1_scan(ZM_LV_1, "bw40 = ", bw40);
+    zm_msg1_scan(ZM_LV_1, "extOffset = ", extOffset);
+
+    if ( hpPriv->coldResetNeedFreq )
+    {
+        hpPriv->coldResetNeedFreq = 0;
+        initRF = 2;
+        zm_debug_msg0("zfHpSetFrequencyEx: Do ColdReset ");
+    }
+    if ( hpPriv->isSiteSurvey == 2 )
+    {
+        /* wait time for AGC and noise calibration : not in sitesurvey and connected */
+        checkLoopCount = 2000; /* 2000*100 = 200ms */
+    }
+    else
+    {
+        /* wait time for AGC and noise calibration : in sitesurvey */
+        checkLoopCount = 1000; /* 1000*100 = 100ms */
+    }
+
+    hpPriv->latestFrequency = frequency;
+    hpPriv->latestBw40 = bw40;
+    hpPriv->latestExtOffset = extOffset;
+
+    if ((hpPriv->dot11Mode == ZM_HAL_80211_MODE_IBSS_GENERAL) ||
+        (hpPriv->dot11Mode == ZM_HAL_80211_MODE_IBSS_WPA2PSK))
+    {
+        if ( frequency <= ZM_CH_G_14 )
+        {
+            /* workaround for 11g Ad Hoc beacon distribution */
+            zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC0_CW, 0x7f0007);
+            //zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC1_AC0_AIFS, 0x1c04901c);
+        }
+    }
+
+    /* AHB, DAC, ADC clock selection by static20/ht2040 */
+    zfSelAdcClk(dev, bw40, frequency);
+
+    /* clear bb_heavy_clip_enable */
+    reg_write(0x99e0, 0x200);
+    zfFlushDelayWrite(dev);
+
+    /* Set CTS/RTS rate */
+    if ( frequency > ZM_CH_G_14 )
+    {
+        //zfHpSetRTSCTSRate(dev, 0x10b010b);  /* OFDM 6M */
+	    new_band = 1;
+	}
+    else
+    {
+        //zfHpSetRTSCTSRate(dev, 0x30003);  /* CCK 11M */
+        new_band = 0;
+    }
+
+    if (((struct zsHpPriv*)wd->hpPrivate)->hwFrequency > ZM_CH_G_14)
+        old_band = 1;
+    else
+        old_band = 0;
+
+    //Workaround for 2.4GHz only device
+    if ((hpPriv->OpFlags & 0x1) == 0)
+    {
+        if ((((struct zsHpPriv*)wd->hpPrivate)->hwFrequency == ZM_CH_G_1) && (frequency == ZM_CH_G_2))
+        {
+            /* Force to do band switching */
+            old_band = 1;
+        }
+    }
+
+    /* Notify channel switch to firmware */
+    /* TX/RX must be stopped by now */
+    cmd[0] = 0 | (ZM_CMD_FREQ_STRAT << 8);
+    ret = zfIssueCmd(dev, cmd, 8, ZM_OID_INTERNAL_WRITE, 0);
+
+    if ((initRF != 0) || (new_band != old_band)
+            || (((struct zsHpPriv*)wd->hpPrivate)->hwBw40 != bw40))
+    {
+        /* band switch */
+        zm_msg0_scan(ZM_LV_1, "=====band switch=====");
+
+        if (initRF == 2 )
+        {
+            //Cold reset BB/ADDA
+            zfDelayWriteInternalReg(dev, 0x1d4004, 0x800);
+            zfFlushDelayWrite(dev);
+            zm_msg0_scan(ZM_LV_1, "Do cold reset BB/ADDA");
+        }
+        else
+        {
+            //Warm reset BB/ADDA
+            zfDelayWriteInternalReg(dev, 0x1d4004, 0x400);
+            zfFlushDelayWrite(dev);
+        }
+
+        /* reset workaround state to default */
+        hpPriv->rxStrongRSSI = 0;
+        hpPriv->strongRSSI = 0;
+
+        zfDelayWriteInternalReg(dev, 0x1d4004, 0x0);
+        zfFlushDelayWrite(dev);
+
+        zfInitPhy(dev, frequency, bw40);
+
+//        zfiCheckRifs(dev);
+
+        /* Bank 0 1 2 3 5 6 7 */
+        zfSetRfRegs(dev, frequency);
+        /* Bank 4 */
+        zfSetBank4AndPowerTable(dev, frequency, bw40, extOffset);
+
+        cmd[0] = 32 | (ZM_CMD_RF_INIT << 8);
+    }
+    else //((new_band == old_band) && !initRF)
+    {
+       /* same band */
+
+       /* Force disable CR671 bit20 / 7823                                            */
+       /* The bug has to do with the polarity of the pdadc offset calibration.  There */
+       /* is an initial calibration that is OK, and there is a continuous             */
+       /* calibration that updates the pddac with the wrong polarity.  Fortunately    */
+       /* the second loop can be disabled with a bit called en_pd_dc_offset_thr.      */
+#if 0
+        cmdB[0] = 8 | (ZM_CMD_BITAND << 8);;
+        cmdB[1] = (0xa27c + 0x1bc000);
+        cmdB[2] = 0xffefffff;
+        ret = zfIssueCmd(dev, cmdB, 12, ZM_OID_INTERNAL_WRITE, 0);
+#endif
+
+       /* Bank 4 */
+       zfSetBank4AndPowerTable(dev, frequency, bw40, extOffset);
+
+
+        cmd[0] = 32 | (ZM_CMD_FREQUENCY << 8);
+    }
+
+    /* Compatibility for new layout UB83 */
+    /* Setting code at CR1 here move from the func:zfHwHTEnable() in firmware */
+    if (((struct zsHpPriv*)wd->hpPrivate)->halCapability & ZM_HP_CAP_11N_ONE_TX_STREAM)
+    {
+        /* UB83 : one stream */
+        tmpValue = 0;
+    }
+    else
+    {
+        /* UB81, UB82 : two stream */
+        tmpValue = 0x100;
+    }
+
+    if (1) //if (((struct zsHpPriv*)wd->hpPrivate)->hw_HT_ENABLE == 1)
+	{
+        if (bw40 == 1)
+		{
+			if (extOffset == 1) {
+            	reg_write(0x9804, tmpValue | 0x2d4); //3d4 for real
+			}
+			else {
+				reg_write(0x9804, tmpValue | 0x2c4);   //3c4 for real
+			}
+			//# Dyn HT2040.Refer to Reg 1.
+            //#[3]:single length (4us) 1st HT long training symbol; use Walsh spatial spreading for 2 chains 2 streams TX
+            //#[c]:allow short GI for HT40 packets; enable HT detection.
+            //#[4]:enable 20/40 MHz channel detection.
+        }
+        else
+	    {
+            reg_write(0x9804, tmpValue | 0x240);
+		    //# Static HT20
+            //#[3]:single length (4us) 1st HT long training symbol; use Walsh spatial spreading for 2 chains 2 streams TX
+            //#[4]:Otus don't allow short GI for HT20 packets yet; enable HT detection.
+            //#[0]:disable 20/40 MHz channel detection.
+        }
+    }
+    else
+	{
+        reg_write(0x9804, 0x0);
+		//# Legacy;# Direct Mapping for each chain.
+        //#Be modified by Oligo to add dynanic for legacy.
+        if (bw40 == 1)
+		{
+            reg_write(0x9804, 0x4);     //# Dyn Legacy .Refer to reg 1.
+        }
+        else
+		{
+            reg_write(0x9804, 0x0);    //# Static Legacy
+        }
+	}
+	zfFlushDelayWrite(dev);
+	/* end of ub83 compatibility */
+
+    /* Set Power, TPC, Gain table... */
+	zfSetPowerCalTable(dev, frequency, bw40, extOffset);
+
+
+    /* store frequency */
+    ((struct zsHpPriv*)wd->hpPrivate)->hwFrequency = (u16_t)frequency;
+    ((struct zsHpPriv*)wd->hpPrivate)->hwBw40 = bw40;
+    ((struct zsHpPriv*)wd->hpPrivate)->hwExtOffset = extOffset;
+
+    zfGetHwTurnOffdynParam(dev,
+                           frequency, bw40, extOffset,
+                           &delta_slope_coeff_exp,
+                           &delta_slope_coeff_man,
+                           &delta_slope_coeff_exp_shgi,
+                           &delta_slope_coeff_man_shgi);
+
+    /* related functions */
+    frequency = frequency*1000;
+    /* len[36] : type[0x30] : seq[?] */
+//    cmd[0] = 28 | (ZM_CMD_FREQUENCY << 8);
+    cmd[1] = frequency;
+    cmd[2] = bw40;//((struct zsHpPriv*)wd->hpPrivate)->hw_DYNAMIC_HT2040_EN;
+    cmd[3] = (extOffset<<2)|0x1;//((wd->ExtOffset << 2) | ((struct zsHpPriv*)wd->hpPrivate)->hw_HT_ENABLE);
+    cmd[4] = delta_slope_coeff_exp;
+    cmd[5] = delta_slope_coeff_man;
+    cmd[6] = delta_slope_coeff_exp_shgi;
+    cmd[7] = delta_slope_coeff_man_shgi;
+    cmd[8] = checkLoopCount;
+
+    ret = zfIssueCmd(dev, cmd, 36, ZM_CMD_SET_FREQUENCY, 0);
+
+    // delay temporarily, wait for new PHY and RF
+    //zfwSleep(dev, 1000);
+}
+
+
+/******************** Key ********************/
+
+u16_t zfHpResetKeyCache(zdev_t* dev)
+{
+    u8_t i;
+    u32_t key[4] = {0, 0, 0, 0};
+
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+    for(i=0;i<4;i++)
+    {
+        zfHpSetDefaultKey(dev, i, ZM_WEP64, key, NULL);
+    }
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_ROLL_CALL_TBL_L, 0x00);
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_ROLL_CALL_TBL_H, 0x00);
+    zfFlushDelayWrite(dev);
+
+    hpPriv->camRollCallTable = (u64_t) 0;
+
+    return 0;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfSetKey                    */
+/*      Set key.                                                        */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : success                                                     */
+/*      other : fail                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2006.1      */
+/*                                                                      */
+/************************************************************************/
+/* ! please use zfCoreSetKey() in 80211Core for SetKey */
+u32_t zfHpSetKey(zdev_t* dev, u8_t user, u8_t keyId, u8_t type,
+        u16_t* mac, u32_t* key)
+{
+    u32_t cmd[(ZM_MAX_CMD_SIZE/4)];
+    u16_t ret;
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+#if 0   /* remove to zfCoreSetKey() */
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    wd->sta.flagKeyChanging++;
+    zm_debug_msg1("   zfHpSetKey++++ ", wd->sta.flagKeyChanging);
+    zmw_leave_critical_section(dev);
+#endif
+
+    cmd[0] = 0x0000281C;
+    cmd[1] = ((u32_t)keyId<<16) + (u32_t)user;
+    cmd[2] = ((u32_t)mac[0]<<16) + (u32_t)type;
+    cmd[3] = ((u32_t)mac[2]<<16) + ((u32_t)mac[1]);
+
+    for (i=0; i<4; i++)
+    {
+        cmd[4+i] = key[i];
+    }
+
+    if (user < 64)
+    {
+        hpPriv->camRollCallTable |= ((u64_t) 1) << user;
+    }
+
+    //ret = zfIssueCmd(dev, cmd, 32, ZM_OID_INTERNAL_WRITE, NULL);
+    ret = zfIssueCmd(dev, cmd, 32, ZM_CMD_SET_KEY, NULL);
+    return ret;
+}
+
+
+u32_t zfHpSetApPairwiseKey(zdev_t* dev, u16_t* staMacAddr, u8_t type,
+        u32_t* key, u32_t* micKey, u16_t staAid)
+{
+    if ((staAid!=0) && (staAid<64))
+    {
+        zfHpSetKey(dev, (staAid-1), 0, type, staMacAddr, key);
+                if ((type == ZM_TKIP)
+#ifdef ZM_ENABLE_CENC
+         || (type == ZM_CENC)
+#endif //ZM_ENABLE_CENC
+           )
+            zfHpSetKey(dev, (staAid-1), 1, type, staMacAddr, micKey);
+        return 0;
+    }
+    return 1;
+}
+
+u32_t zfHpSetApGroupKey(zdev_t* dev, u16_t* apMacAddr, u8_t type,
+        u32_t* key, u32_t* micKey, u16_t vapId)
+{
+    zfHpSetKey(dev, ZM_USER_KEY_DEFAULT - 1 - vapId, 0, type, apMacAddr, key);	// 6D18 modify from 0 to 1 ??
+            if ((type == ZM_TKIP)
+#ifdef ZM_ENABLE_CENC
+         || (type == ZM_CENC)
+#endif //ZM_ENABLE_CENC
+           )
+        zfHpSetKey(dev, ZM_USER_KEY_DEFAULT - 1 - vapId, 1, type, apMacAddr, micKey);
+    return 0;
+}
+
+u32_t zfHpSetDefaultKey(zdev_t* dev, u8_t keyId, u8_t type, u32_t* key, u32_t* micKey)
+{
+    u16_t macAddr[3] = {0, 0, 0};
+
+    #ifdef ZM_ENABLE_IBSS_WPA2PSK
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv = wd->hpPrivate;
+
+    if ( hpPriv->dot11Mode == ZM_HAL_80211_MODE_IBSS_WPA2PSK )
+    { /* If not wpa2psk , use traditional */
+      /* Because the bug of chip , defaultkey should follow the key map rule in register 700 */
+        if ( keyId == 0 )
+            zfHpSetKey(dev, ZM_USER_KEY_DEFAULT+keyId, 0, type, macAddr, key);
+        else
+            zfHpSetKey(dev, ZM_USER_KEY_DEFAULT+keyId, 1, type, macAddr, key);
+    }
+    else
+        zfHpSetKey(dev, ZM_USER_KEY_DEFAULT+keyId, 0, type, macAddr, key);
+    #else
+        zfHpSetKey(dev, ZM_USER_KEY_DEFAULT+keyId, 0, type, macAddr, key);
+    #endif
+            if ((type == ZM_TKIP)
+
+#ifdef ZM_ENABLE_CENC
+         || (type == ZM_CENC)
+#endif //ZM_ENABLE_CENC
+           )
+    {
+        zfHpSetKey(dev, ZM_USER_KEY_DEFAULT+keyId, 1, type, macAddr, micKey);
+    }
+
+    return 0;
+}
+
+u32_t zfHpSetPerUserKey(zdev_t* dev, u8_t user, u8_t keyId, u8_t* mac, u8_t type, u32_t* key, u32_t* micKey)
+{
+#ifdef ZM_ENABLE_IBSS_WPA2PSK
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv = wd->hpPrivate;
+
+    if ( hpPriv->dot11Mode == ZM_HAL_80211_MODE_IBSS_WPA2PSK )
+    { /* If not wpa2psk , use traditional */
+        if(keyId)
+        {  /* Set Group Key */
+            zfHpSetKey(dev, user, 1, type, (u16_t *)mac, key);
+        }
+        else if(keyId == 0)
+        {  /* Set Pairwise Key */
+            zfHpSetKey(dev, user, 0, type, (u16_t *)mac, key);
+        }
+    }
+    else
+    {
+        zfHpSetKey(dev, user, keyId, type, (u16_t *)mac, key);
+    }
+#else
+    zfHpSetKey(dev, user, keyId, type, (u16_t *)mac, key);
+#endif
+
+            if ((type == ZM_TKIP)
+#ifdef ZM_ENABLE_CENC
+         || (type == ZM_CENC)
+#endif //ZM_ENABLE_CENC
+           )
+    {
+        zfHpSetKey(dev, user, keyId + 1, type, (u16_t *)mac, micKey);
+    }
+    return 0;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfHpRemoveKey               */
+/*      Remove key.                                                     */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : success                                                     */
+/*      other : fail                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Yuan-Gu Wei         ZyDAS Technology Corporation    2006.6      */
+/*                                                                      */
+/************************************************************************/
+u16_t zfHpRemoveKey(zdev_t* dev, u16_t user)
+{
+    u32_t cmd[(ZM_MAX_CMD_SIZE/4)];
+    u16_t ret = 0;
+
+    cmd[0] = 0x00002904;
+    cmd[1] = (u32_t)user;
+
+    ret = zfIssueCmd(dev, cmd, 8, ZM_OID_INTERNAL_WRITE, NULL);
+    return ret;
+}
+
+
+
+/******************** DMA ********************/
+u16_t zfHpStartRecv(zdev_t* dev)
+{
+    zfDelayWriteInternalReg(dev, 0x1c3d30, 0x100);
+    zfFlushDelayWrite(dev);
+
+    return 0;
+}
+
+u16_t zfHpStopRecv(zdev_t* dev)
+{
+    return 0;
+}
+
+
+/******************** MAC ********************/
+void zfInitMac(zdev_t* dev)
+{
+    /* ACK extension register */
+    // jhlee temp : change value 0x2c -> 0x40
+    // honda resolve short preamble problem : 0x40 -> 0x75
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_ACK_EXTENSION, 0x40); // 0x28 -> 0x2c 6522:yflee
+
+    /* TxQ0/1/2/3 Retry MAX=2 => transmit 3 times and degrade rate for retry */
+    /* PB42 AP crash issue:                                                  */
+    /* Workaround the crash issue by CTS/RTS, set retry max to zero for      */
+    /*   workaround tx underrun which enable CTS/RTS */
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_RETRY_MAX, 0); // 0x11111 => 0
+
+    /* use hardware MIC check */
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_SNIFFER, 0x2000000);
+
+    /* Set Rx threshold to 1600 */
+#if ZM_LARGEPAYLOAD_TEST == 1
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_RX_THRESHOLD, 0xc4000);
+#else
+    #ifndef ZM_DISABLE_AMSDU8K_SUPPORT
+    /* The maximum A-MSDU length is 3839/7935 */
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_RX_THRESHOLD, 0xc1f80);
+    #else
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_RX_THRESHOLD, 0xc0f80);
+    #endif
+#endif
+
+    //zfDelayWriteInternalReg(dev, ZM_MAC_REG_DYNAMIC_SIFS_ACK, 0x10A);
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_RX_PE_DELAY, 0x70);
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_EIFS_AND_SIFS, 0xa144000);
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_SLOT_TIME, 9<<10);
+
+    /* CF-END mode */
+    zfDelayWriteInternalReg(dev, 0x1c3b2c, 0x19000000);
+
+    //NAV protects ACK only (in TXOP)
+    zfDelayWriteInternalReg(dev, 0x1c3b38, 0x201);
+
+
+    /* Set Beacon PHY CTRL's TPC to 0x7, TA1=1 */
+    /* OTUS set AM to 0x1 */
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_BCN_HT1, 0x8000170);
+
+    /* TODO : wep backoff protection 0x63c */
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_BACKOFF_PROTECT, 0x105);
+
+    /* AGG test code*/
+    /* Aggregation MAX number and timeout */
+    zfDelayWriteInternalReg(dev, 0x1c3b9c, 0x10000a);
+    /* Filter any control frames, BAR is bit 24 */
+    zfDelayWriteInternalReg(dev, 0x1c368c, 0x0500ffff);
+    /* Enable deaggregator */
+    zfDelayWriteInternalReg(dev, 0x1c3c40, 0x1);
+
+    /* Basic rate */
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_BASIC_RATE, 0x150f);
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_MANDATORY_RATE, 0x150f);
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_RTS_CTS_RATE, 0x10b01bb);
+
+    /* MIMO resposne control */
+    zfDelayWriteInternalReg(dev, 0x1c3694, 0x4003C1E);/* bit 26~28  otus-AM */
+
+    /* Enable LED0 and LED1 */
+    zfDelayWriteInternalReg(dev, 0x1d0100, 0x3);
+    zfDelayWriteInternalReg(dev, 0x1d0104, 0x3);
+
+    /* switch MAC to OTUS interface */
+    zfDelayWriteInternalReg(dev, 0x1c3600, 0x3);
+
+    /* RXMAC A-MPDU length threshold */
+    zfDelayWriteInternalReg(dev, 0x1c3c50, 0xffff);
+
+	/* Phy register read timeout */
+	zfDelayWriteInternalReg(dev, 0x1c3680, 0xf00008);
+
+	/* Disable Rx TimeOut : workaround for BB.
+	 *  OTUS would interrupt the rx frame that sent by OWL TxUnderRun
+	 *  because OTUS rx timeout behavior, then OTUS would not ack the BA for
+	 *  this AMPDU from OWL.
+	 *  Fix by Perry Hwang.  2007/05/10.
+	 *  0x1c362c : Rx timeout value : bit 27~16
+	 */
+	zfDelayWriteInternalReg(dev, 0x1c362c, 0x0);
+
+    //Set USB Rx stream mode MAX packet number to 2
+    //    Max packet number = *0x1e1110 + 1
+    zfDelayWriteInternalReg(dev, 0x1e1110, 0x4);
+    //Set USB Rx stream mode timeout to 10us
+    zfDelayWriteInternalReg(dev, 0x1e1114, 0x80);
+
+    //Set CPU clock frequency to 88/80MHz
+    zfDelayWriteInternalReg(dev, 0x1D4008, 0x73);
+
+    //Set WLAN DMA interrupt mode : generate int per packet
+    zfDelayWriteInternalReg(dev, 0x1c3d7c, 0x110011);
+
+    /* 7807 */
+    /* enable func : Reset FIFO1 and FIFO2 when queue-gnt is low */
+    /* 0x1c3bb0 Bit2 */
+    /* Disable SwReset in firmware for TxHang, enable reset FIFO func. */
+    zfDelayWriteInternalReg(dev, 0x1c3bb0, 0x4);
+
+    /* Disables the CF_END frame */
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_TXOP_NOT_ENOUGH_INDICATION, 0x141E0F48);
+
+	/* Disable the SW Decrypt*/
+	zfDelayWriteInternalReg(dev, 0x1c3678, 0x70);
+    zfFlushDelayWrite(dev);
+    //---------------------
+
+    /* Set TxQs CWMIN, CWMAX, AIFS and TXO to WME STA default. */
+    zfUpdateDefaultQosParameter(dev, 0);
+
+    //zfSelAdcClk(dev, 0);
+
+    return;
+}
+
+
+u16_t zfHpSetSnifferMode(zdev_t* dev, u16_t on)
+{
+    if (on != 0)
+    {
+        zfDelayWriteInternalReg(dev, ZM_MAC_REG_SNIFFER, 0x2000001);
+    }
+    else
+    {
+        zfDelayWriteInternalReg(dev, ZM_MAC_REG_SNIFFER, 0x2000000);
+    }
+    zfFlushDelayWrite(dev);
+    return 0;
+}
+
+
+u16_t zfHpSetApStaMode(zdev_t* dev, u8_t mode)
+{
+    zmw_get_wlan_dev(dev);
+
+    struct zsHpPriv* hpPriv = wd->hpPrivate;
+    hpPriv->dot11Mode = mode;
+
+    switch(mode)
+    {
+        case ZM_HAL_80211_MODE_AP:
+            zfDelayWriteInternalReg(dev, 0x1c3700, 0x0f0000a1);
+            zfDelayWriteInternalReg(dev, 0x1c3c40, 0x1);
+            break;
+
+        case ZM_HAL_80211_MODE_STA:
+            zfDelayWriteInternalReg(dev, 0x1c3700, 0x0f000002);
+            zfDelayWriteInternalReg(dev, 0x1c3c40, 0x1);
+            break;
+
+        case ZM_HAL_80211_MODE_IBSS_GENERAL:
+            zfDelayWriteInternalReg(dev, 0x1c3700, 0x0f000000);
+            zfDelayWriteInternalReg(dev, 0x1c3c40, 0x1);
+            break;
+
+        case ZM_HAL_80211_MODE_IBSS_WPA2PSK:
+            zfDelayWriteInternalReg(dev, 0x1c3700, 0x0f0000e0);
+            zfDelayWriteInternalReg(dev, 0x1c3c40, 0x41);       // for multiple ( > 2 ) stations IBSS network
+            break;
+
+        default:
+            goto skip;
+    }
+
+    zfFlushDelayWrite(dev);
+
+skip:
+    return 0;
+}
+
+
+u16_t zfHpSetBssid(zdev_t* dev, u8_t* bssidSrc)
+{
+    u32_t  address;
+    u16_t *bssid = (u16_t *)bssidSrc;
+
+    address = bssid[0] + (((u32_t)bssid[1]) << 16);
+    zfDelayWriteInternalReg(dev, 0x1c3618, address);
+
+    address = (u32_t)bssid[2];
+    zfDelayWriteInternalReg(dev, 0x1c361C, address);
+    zfFlushDelayWrite(dev);
+    return 0;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfHpUpdateQosParameter      */
+/*      Update TxQs CWMIN, CWMAX, AIFS and TXOP.                        */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      cwminTbl : CWMIN parameter for TxQs                             */
+/*      cwmaxTbl : CWMAX parameter for TxQs                             */
+/*      aifsTbl: AIFS parameter for TxQs                                */
+/*      txopTbl : TXOP parameter for TxQs                               */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen             ZyDAS Technology Corporation    2006.6      */
+/*                                                                      */
+/************************************************************************/
+u8_t zfHpUpdateQosParameter(zdev_t* dev, u16_t* cwminTbl, u16_t* cwmaxTbl,
+        u16_t* aifsTbl, u16_t* txopTbl)
+{
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv = wd->hpPrivate;
+
+    zm_msg0_mm(ZM_LV_0, "zfHalUpdateQosParameter()");
+
+    /* Note : Do not change cwmin for Q0 in Ad Hoc mode              */
+    /*        otherwise driver will fail in Wifi beacon distribution */
+    if (hpPriv->dot11Mode == ZM_HAL_80211_MODE_STA)
+    {
+#if 0 //Restore CWmin to improve down link throughput
+        //cheating in BE traffic
+        if (wd->sta.EnableHT == 1)
+        {
+            //cheating in BE traffic
+            cwminTbl[0] = 7;//15;
+        }
+#endif
+        cwmaxTbl[0] = 127;//1023;
+        aifsTbl[0] = 2*9+10;//3 * 9 + 10;
+    }
+
+    /* CWMIN and CWMAX */
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC0_CW, cwminTbl[0]
+            + ((u32_t)cwmaxTbl[0]<<16));
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC1_CW, cwminTbl[1]
+            + ((u32_t)cwmaxTbl[1]<<16));
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC2_CW, cwminTbl[2]
+            + ((u32_t)cwmaxTbl[2]<<16));
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC3_CW, cwminTbl[3]
+            + ((u32_t)cwmaxTbl[3]<<16));
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC4_CW, cwminTbl[4]
+            + ((u32_t)cwmaxTbl[4]<<16));
+
+    /* AIFS */
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC1_AC0_AIFS, aifsTbl[0]
+            +((u32_t)aifsTbl[0]<<12)+((u32_t)aifsTbl[0]<<24));
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC3_AC2_AIFS, (aifsTbl[0]>>8)
+            +((u32_t)aifsTbl[0]<<4)+((u32_t)aifsTbl[0]<<16));
+
+    /* TXOP */
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC1_AC0_TXOP, txopTbl[0]
+            + ((u32_t)txopTbl[1]<<16));
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC3_AC2_TXOP, txopTbl[2]
+            + ((u32_t)txopTbl[3]<<16));
+
+    zfFlushDelayWrite(dev);
+
+    hpPriv->txop[0] = txopTbl[0];
+    hpPriv->txop[1] = txopTbl[1];
+    hpPriv->txop[2] = txopTbl[2];
+    hpPriv->txop[3] = txopTbl[3];
+    hpPriv->cwmin[0] = cwminTbl[0];
+    hpPriv->cwmax[0] = cwmaxTbl[0];
+    hpPriv->cwmin[1] = cwminTbl[1];
+    hpPriv->cwmax[1] = cwmaxTbl[1];
+
+    return 0;
+}
+
+
+void zfHpSetAtimWindow(zdev_t* dev, u16_t atimWin)
+{
+    zm_msg1_mm(ZM_LV_0, "Set ATIM window to ", atimWin);
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_ATIM_WINDOW, atimWin);
+    zfFlushDelayWrite(dev);
+}
+
+
+void zfHpSetBasicRateSet(zdev_t* dev, u16_t bRateBasic, u16_t gRateBasic)
+{
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_BASIC_RATE, bRateBasic
+                            | ((u16_t)gRateBasic<<8));
+    zfFlushDelayWrite(dev);
+}
+
+
+/* HT40 send by OFDM 6M    */
+/* otherwise use reg 0x638 */
+void zfHpSetRTSCTSRate(zdev_t* dev, u32_t rate)
+{
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_RTS_CTS_RATE, rate);
+    zfFlushDelayWrite(dev);
+}
+
+void zfHpSetMacAddress(zdev_t* dev, u16_t* macAddr, u16_t macAddrId)
+{
+    if (macAddrId == 0)
+    {
+        zfDelayWriteInternalReg(dev, ZM_MAC_REG_MAC_ADDR_L,
+                (((u32_t)macAddr[1])<<16) | macAddr[0]);
+        zfDelayWriteInternalReg(dev, ZM_MAC_REG_MAC_ADDR_H, macAddr[2]);
+    }
+    else if (macAddrId <= 7)
+    {
+        zfDelayWriteInternalReg(dev, ZM_MAC_REG_ACK_TABLE+((macAddrId-1)*8),
+                macAddr[0] + ((u32_t)macAddr[1]<<16));
+        zfDelayWriteInternalReg(dev, ZM_MAC_REG_ACK_TABLE+((macAddrId-1)*8)+4,
+                macAddr[2]);
+    }
+    zfFlushDelayWrite(dev);
+}
+
+void zfHpSetMulticastList(zdev_t* dev, u8_t size, u8_t* pList, u8_t bAllMulticast)
+{
+    struct zsMulticastAddr* pMacList = (struct zsMulticastAddr*) pList;
+    u8_t   i;
+    u32_t  value;
+    u32_t  swRegMulHashValueH, swRegMulHashValueL;
+
+    swRegMulHashValueH = 0x80000000;
+    swRegMulHashValueL = 0;
+
+    if ( bAllMulticast )
+    {
+        swRegMulHashValueH = swRegMulHashValueL = ~0;
+    }
+    else
+    {
+        for(i=0; i<size; i++)
+        {
+            value = pMacList[i].addr[5] >> 2;
+
+            if ( value < 32 )
+            {
+                swRegMulHashValueL |= (1 << value);
+            }
+            else
+            {
+                swRegMulHashValueH |= (1 << (value-32));
+            }
+        }
+    }
+
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_GROUP_HASH_TBL_L,
+                            swRegMulHashValueL);
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_GROUP_HASH_TBL_H,
+                            swRegMulHashValueH);
+    zfFlushDelayWrite(dev);
+    return;
+}
+
+/******************** Beacon ********************/
+void zfHpEnableBeacon(zdev_t* dev, u16_t mode, u16_t bcnInterval, u16_t dtim, u8_t enableAtim)
+{
+    u32_t  value;
+
+    zmw_get_wlan_dev(dev);
+
+    /* Beacon Ready */
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_BCN_CTRL, 0);
+    /* Beacon DMA buffer address */
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_BCN_ADDR, ZM_BEACON_BUFFER_ADDRESS);
+
+    value = bcnInterval;
+
+    value |= (((u32_t) dtim) << 16);
+
+    if (mode == ZM_MODE_AP)
+    {
+
+        value |= 0x1000000;
+    }
+    else if (mode == ZM_MODE_IBSS)
+    {
+        value |= 0x2000000;
+
+		if ( enableAtim )
+		{
+			value |= 0x4000000;
+		}
+		((struct zsHpPriv*)wd->hpPrivate)->ibssBcnEnabled = 1;
+        ((struct zsHpPriv*)wd->hpPrivate)->ibssBcnInterval = value;
+    }
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_PRETBTT, (bcnInterval-6)<<16);
+
+    /* Beacon period and beacon enable */
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_BCN_PERIOD, value);
+    zfFlushDelayWrite(dev);
+}
+
+void zfHpDisableBeacon(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    ((struct zsHpPriv*)wd->hpPrivate)->ibssBcnEnabled = 0;
+
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_BCN_PERIOD, 0);
+    zfFlushDelayWrite(dev);
+}
+
+void zfHpLedCtrl(zdev_t* dev, u16_t ledId, u8_t mode)
+{
+    u16_t state;
+    zmw_get_wlan_dev(dev);
+
+    //zm_debug_msg1("LED ID=", ledId);
+    //zm_debug_msg1("LED mode=", mode);
+    if (ledId < 2)
+    {
+        if (((struct zsHpPriv*)wd->hpPrivate)->ledMode[ledId] != mode)
+        {
+            ((struct zsHpPriv*)wd->hpPrivate)->ledMode[ledId] = mode;
+
+            state = ((struct zsHpPriv*)wd->hpPrivate)->ledMode[0]
+                    | (((struct zsHpPriv*)wd->hpPrivate)->ledMode[1]<<1);
+            zfDelayWriteInternalReg(dev, 0x1d0104, state);
+            zfFlushDelayWrite(dev);
+            //zm_debug_msg0("Update LED");
+        }
+    }
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfHpResetTxRx               */
+/*      Reset Tx and Rx Desc.                                           */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : success                                                     */
+/*      other : fail                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Chao-Wen Yang         ZyDAS Technology Corporation    2007.3    */
+/*                                                                      */
+/************************************************************************/
+u16_t zfHpUsbReset(zdev_t* dev)
+{
+    u32_t cmd[(ZM_MAX_CMD_SIZE/4)];
+    u16_t ret = 0;
+
+    //zm_debug_msg0("CWY - Reset Tx and Rx");
+
+    cmd[0] =  0 | (ZM_CMD_RESET << 8);
+
+    ret = zfIssueCmd(dev, cmd, 4, ZM_OID_INTERNAL_WRITE, NULL);
+    return ret;
+}
+
+u16_t zfHpDKReset(zdev_t* dev, u8_t flag)
+{
+    u32_t cmd[(ZM_MAX_CMD_SIZE/4)];
+    u16_t ret = 0;
+
+    //zm_debug_msg0("CWY - Reset Tx and Rx");
+
+    cmd[0] =  4 | (ZM_CMD_DKRESET << 8);
+    cmd[1] = flag;
+
+    ret = zfIssueCmd(dev, cmd, 8, ZM_OID_INTERNAL_WRITE, NULL);
+    return ret;
+}
+
+u32_t zfHpCwmUpdate(zdev_t* dev)
+{
+    //u32_t cmd[3];
+    //u16_t ret;
+    //
+    //cmd[0] = 0x00000008;
+    //cmd[1] = 0x1c36e8;
+    //cmd[2] = 0x1c36ec;
+    //
+    //ret = zfIssueCmd(dev, cmd, 12, ZM_CWM_READ, 0);
+    //return ret;
+    zmw_get_wlan_dev(dev);
+
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+    zfCoreCwmBusy(dev, zfCwmIsExtChanBusy(hpPriv->ctlBusy, hpPriv->extBusy));
+
+    hpPriv->ctlBusy = 0;
+    hpPriv->extBusy = 0;
+
+    return 0;
+}
+
+u32_t zfHpAniUpdate(zdev_t* dev)
+{
+    u32_t cmd[5];
+    u16_t ret;
+
+    cmd[0] = 0x00000010;
+    cmd[1] = 0x1c36e8;
+    cmd[2] = 0x1c36ec;
+    cmd[3] = 0x1c3cb4;
+    cmd[4] = 0x1c3cb8;
+
+    ret = zfIssueCmd(dev, cmd, 20, ZM_ANI_READ, 0);
+    return ret;
+}
+
+/*
+ * Update Beacon RSSI in ANI
+ */
+u32_t zfHpAniUpdateRssi(zdev_t* dev, u8_t rssi)
+{
+    zmw_get_wlan_dev(dev);
+
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+    hpPriv->stats.ast_nodestats.ns_avgbrssi = rssi;
+
+    return 0;
+}
+
+#define ZM_SEEPROM_MAC_ADDRESS_OFFSET   (0x1400 + (0x106<<1))
+#define ZM_SEEPROM_REGDOMAIN_OFFSET   (0x1400 + (0x104<<1))
+#define ZM_SEEPROM_VERISON_OFFSET   (0x1400 + (0x102<<1))
+#define ZM_SEEPROM_HARDWARE_TYPE_OFFSET   (0x1374)
+#define ZM_SEEPROM_HW_HEAVY_CLIP          (0x161c)
+
+u32_t zfHpGetMacAddress(zdev_t* dev)
+{
+    u32_t cmd[7];
+    u16_t ret;
+
+    cmd[0] = 0x00000000 | 24;
+    cmd[1] = ZM_SEEPROM_MAC_ADDRESS_OFFSET;
+    cmd[2] = ZM_SEEPROM_MAC_ADDRESS_OFFSET+4;
+    cmd[3] = ZM_SEEPROM_REGDOMAIN_OFFSET;
+    cmd[4] = ZM_SEEPROM_VERISON_OFFSET;
+    cmd[5] = ZM_SEEPROM_HARDWARE_TYPE_OFFSET;
+    cmd[6] = ZM_SEEPROM_HW_HEAVY_CLIP;
+
+    ret = zfIssueCmd(dev, cmd, 28, ZM_MAC_READ, 0);
+    return ret;
+}
+
+u32_t zfHpGetTransmitPower(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    struct zsHpPriv*    hpPriv  = wd->hpPrivate;
+    u16_t               tpc     = 0;
+
+    if (hpPriv->hwFrequency < 3000) {
+        tpc = hpPriv->tPow2x2g[0] & 0x3f;
+        wd->maxTxPower2 &= 0x3f;
+        tpc = (tpc > wd->maxTxPower2)? wd->maxTxPower2 : tpc;
+    } else {
+        tpc = hpPriv->tPow2x5g[0] & 0x3f;
+        wd->maxTxPower5 &= 0x3f;
+        tpc = (tpc > wd->maxTxPower5)? wd->maxTxPower5 : tpc;
+    }
+
+    return tpc;
+}
+
+u8_t zfHpGetMinTxPower(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    struct zsHpPriv*    hpPriv  = wd->hpPrivate;
+    u8_t               tpc     = 0;
+
+    if (hpPriv->hwFrequency < 3000)
+    {
+        if(wd->BandWidth40)
+        {
+            //40M
+            tpc = (hpPriv->tPow2x2gHt40[7]&0x3f);
+        }
+        else
+        {
+            //20M
+            tpc = (hpPriv->tPow2x2gHt20[7]&0x3f);
+        }
+    }
+    else
+    {
+        if(wd->BandWidth40)
+        {
+            //40M
+            tpc = (hpPriv->tPow2x5gHt40[7]&0x3f);
+        }
+        else
+        {
+            //20M
+            tpc = (hpPriv->tPow2x5gHt20[7]&0x3f);
+        }
+    }
+
+    return tpc;
+}
+
+u8_t zfHpGetMaxTxPower(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    struct zsHpPriv*    hpPriv  = wd->hpPrivate;
+    u8_t               tpc     = 0;
+
+    if (hpPriv->hwFrequency < 3000)
+    {
+        tpc = (hpPriv->tPow2xCck[0]&0x3f);
+    }
+    else
+    {
+        tpc =(hpPriv->tPow2x5g[0]&0x3f);
+    }
+
+    return tpc;
+}
+
+u32_t zfHpLoadEEPROMFromFW(zdev_t* dev)
+{
+    u32_t cmd[16];
+    u32_t ret=0, i, j;
+    zmw_get_wlan_dev(dev);
+
+    i = ((struct zsHpPriv*)wd->hpPrivate)->eepromImageRdReq;
+
+    cmd[0] = ZM_HAL_MAX_EEPROM_PRQ*4;
+
+    for (j=0; j<ZM_HAL_MAX_EEPROM_PRQ; j++)
+    {
+        cmd[j+1] = 0x1000 + (((i*ZM_HAL_MAX_EEPROM_PRQ) + j)*4);
+    }
+
+    ret = zfIssueCmd(dev, cmd, (ZM_HAL_MAX_EEPROM_PRQ+1)*4, ZM_EEPROM_READ, 0);
+
+    return ret;
+}
+
+void zfHpHeartBeat(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+    u8_t polluted = 0;
+    u8_t ackTpc;
+
+    /* Workaround : Make OTUS fire more beacon in ad hoc mode in 2.4GHz */
+    if (hpPriv->ibssBcnEnabled != 0)
+    {
+        if (hpPriv->hwFrequency <= ZM_CH_G_14)
+        {
+            if ((wd->tick % 10) == 0)
+            {
+                if ((wd->tick % 40) == 0)
+                {
+                    zfDelayWriteInternalReg(dev, ZM_MAC_REG_BCN_PERIOD, hpPriv->ibssBcnInterval-1);
+                    polluted = 1;
+                }
+                else
+                {
+                    zfDelayWriteInternalReg(dev, ZM_MAC_REG_BCN_PERIOD, hpPriv->ibssBcnInterval);
+                    polluted = 1;
+                }
+            }
+        }
+    }
+
+    if ((wd->tick & 0x3f) == 0x25)
+    {
+        /* Workaround for beacon stuck after SW reset */
+        if (hpPriv->ibssBcnEnabled != 0)
+        {
+            zfDelayWriteInternalReg(dev, ZM_MAC_REG_BCN_ADDR, ZM_BEACON_BUFFER_ADDRESS);
+            polluted = 1;
+        }
+
+        //DbgPrint("hpPriv->aggMaxDurationBE=%d", hpPriv->aggMaxDurationBE);
+        //DbgPrint("wd->sta.avgSizeOfReceivePackets=%d", wd->sta.avgSizeOfReceivePackets);
+        if (( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+            && (zfStaIsConnected(dev))
+            && (wd->sta.EnableHT == 1) //11n mode
+            && (wd->BandWidth40 == 1) //40MHz mode
+            && (wd->sta.enableDrvBA ==0) //Marvel AP
+            && (hpPriv->aggMaxDurationBE > 2000) //BE TXOP > 2ms
+            && (wd->sta.avgSizeOfReceivePackets > 1420))
+        {
+            zfDelayWriteInternalReg(dev, 0x1c3b9c, 0x8000a);
+            polluted = 1;
+        }
+        else
+        {
+            zfDelayWriteInternalReg(dev, 0x1c3b9c, hpPriv->aggPktNum);
+            polluted = 1;
+        }
+
+        if (wd->dynamicSIFSEnable == 0)
+        {
+            if (( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+                && (zfStaIsConnected(dev))
+                && (wd->sta.EnableHT == 1) //11n mode
+                && (wd->BandWidth40 == 0) //20MHz mode
+                && (wd->sta.enableDrvBA ==0)) //Marvel AP
+            {
+                zfDelayWriteInternalReg(dev, 0x1c3698, 0x5144000);
+                polluted = 1;
+            }
+            else
+            {
+                zfDelayWriteInternalReg(dev, 0x1c3698, 0xA144000);
+                polluted = 1;
+            }
+        }
+        else
+        {
+            if (( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+                && (zfStaIsConnected(dev))
+                && (wd->sta.EnableHT == 1) //11n mode
+                && (wd->sta.athOwlAp == 1)) //Atheros AP
+            {
+                if (hpPriv->retransmissionEvent)
+                {
+                    switch(hpPriv->latestSIFS)
+                    {
+                    case 0:
+                        hpPriv->latestSIFS = 1;
+                        zfDelayWriteInternalReg(dev, ZM_MAC_REG_EIFS_AND_SIFS, 0x8144000);
+                        break;
+                    case 1:
+                        hpPriv->latestSIFS = 2;
+                        zfDelayWriteInternalReg(dev, ZM_MAC_REG_EIFS_AND_SIFS, 0xa144000);
+                        break;
+                    case 2:
+                        hpPriv->latestSIFS = 3;
+                        zfDelayWriteInternalReg(dev, ZM_MAC_REG_EIFS_AND_SIFS, 0xc144000);
+                        break;
+                    case 3:
+                        hpPriv->latestSIFS = 0;
+                        zfDelayWriteInternalReg(dev, ZM_MAC_REG_EIFS_AND_SIFS, 0xa144000);
+                        break;
+                    default:
+                        hpPriv->latestSIFS = 0;
+                        zfDelayWriteInternalReg(dev, ZM_MAC_REG_EIFS_AND_SIFS, 0xa144000);
+                        break;
+                    }
+                    polluted = 1;
+                    zm_debug_msg1("##### Correct Tx retransmission issue #####, ", hpPriv->latestSIFS);
+                    hpPriv->retransmissionEvent = 0;
+                }
+            }
+            else
+            {
+                hpPriv->latestSIFS = 0;
+                hpPriv->retransmissionEvent = 0;
+                zfDelayWriteInternalReg(dev, 0x1c3698, 0xA144000);
+                polluted = 1;
+            }
+        }
+
+        if ((wd->sta.bScheduleScan == FALSE) && (wd->sta.bChannelScan == FALSE))
+        {
+#define ZM_SIGNAL_THRESHOLD  66
+        if (( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+            && (zfStaIsConnected(dev))
+            && (wd->SignalStrength > ZM_SIGNAL_THRESHOLD))
+        {
+                /* remove state handle, always rewrite register setting */
+                //if (hpPriv->strongRSSI == 0)
+            {
+                hpPriv->strongRSSI = 1;
+                /* Strong RSSI, set ACK to one Tx stream and lower Tx power 7dbm */
+                if (hpPriv->currentAckRtsTpc > (14+10))
+                {
+                    ackTpc = hpPriv->currentAckRtsTpc - 14;
+                }
+                else
+                {
+                    ackTpc = 10;
+                }
+                zfDelayWriteInternalReg(dev, 0x1c3694, ((ackTpc) << 20) | (0x1<<26));
+                zfDelayWriteInternalReg(dev, 0x1c3bb4, ((ackTpc) << 5 ) | (0x1<<11) |
+                                                       ((ackTpc) << 21) | (0x1<<27)  );
+                polluted = 1;
+            }
+        }
+        else
+        {
+                /* remove state handle, always rewrite register setting */
+                //if (hpPriv->strongRSSI == 1)
+            {
+                hpPriv->strongRSSI = 0;
+                if (hpPriv->halCapability & ZM_HP_CAP_11N_ONE_TX_STREAM)
+                {
+                    zfDelayWriteInternalReg(dev, 0x1c3694, ((hpPriv->currentAckRtsTpc&0x3f) << 20) | (0x1<<26));
+                    zfDelayWriteInternalReg(dev, 0x1c3bb4, ((hpPriv->currentAckRtsTpc&0x3f) << 5 ) | (0x1<<11) |
+                                                       ((hpPriv->currentAckRtsTpc&0x3f) << 21) | (0x1<<27)  );
+                }
+                else
+                {
+                    zfDelayWriteInternalReg(dev, 0x1c3694, ((hpPriv->currentAckRtsTpc&0x3f) << 20) | (0x5<<26));
+                    zfDelayWriteInternalReg(dev, 0x1c3bb4, ((hpPriv->currentAckRtsTpc&0x3f) << 5 ) | (0x5<<11) |
+                                                       ((hpPriv->currentAckRtsTpc&0x3f) << 21) | (0x5<<27)  );
+                }
+                polluted = 1;
+            }
+        }
+#undef ZM_SIGNAL_THRESHOLD
+        }
+
+        if ((hpPriv->halCapability & ZM_HP_CAP_11N_ONE_TX_STREAM) == 0)
+        {
+            if ((wd->sta.bScheduleScan == FALSE) && (wd->sta.bChannelScan == FALSE))
+            {
+    #define ZM_RX_SIGNAL_THRESHOLD_H  71
+    #define ZM_RX_SIGNAL_THRESHOLD_L  66
+                u8_t rxSignalThresholdH = ZM_RX_SIGNAL_THRESHOLD_H;
+                u8_t rxSignalThresholdL = ZM_RX_SIGNAL_THRESHOLD_L;
+    #undef ZM_RX_SIGNAL_THRESHOLD_H
+    #undef ZM_RX_SIGNAL_THRESHOLD_L
+
+                if (( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+                    && (zfStaIsConnected(dev))
+                    && (wd->SignalStrength > rxSignalThresholdH)
+                    )//&& (hpPriv->rxStrongRSSI == 0))
+                {
+                    hpPriv->rxStrongRSSI = 1;
+                    //zfDelayWriteInternalReg(dev, 0x1c5964, 0x1220);
+                    //zfDelayWriteInternalReg(dev, 0x1c5960, 0x900);
+                    //zfDelayWriteInternalReg(dev, 0x1c6960, 0x900);
+                    //zfDelayWriteInternalReg(dev, 0x1c7960, 0x900);
+                    if ((hpPriv->eepromImage[0x100+0x110*2/4]&0xff) == 0x80) //FEM TYPE
+                    {
+                        if (hpPriv->hwFrequency <= ZM_CH_G_14)
+                        {
+                            zfDelayWriteInternalReg(dev, 0x1c8960, 0x900);
+                        }
+                        else
+                        {
+                            zfDelayWriteInternalReg(dev, 0x1c8960, 0x9b49);
+                        }
+                    }
+                    else
+                    {
+                        zfDelayWriteInternalReg(dev, 0x1c8960, 0x0900);
+                    }
+                    polluted = 1;
+                }
+                else if (( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+                    && (zfStaIsConnected(dev))
+                    && (wd->SignalStrength > rxSignalThresholdL)
+                    )//&& (hpPriv->rxStrongRSSI == 1))
+                {
+                    //Do nothing to prevent frequently Rx switching
+                }
+                else
+                {
+                    /* remove state handle, always rewrite register setting */
+                    //if (hpPriv->rxStrongRSSI == 1)
+                    {
+                        hpPriv->rxStrongRSSI = 0;
+                        //zfDelayWriteInternalReg(dev, 0x1c5964, 0x1120);
+                        //zfDelayWriteInternalReg(dev, 0x1c5960, 0x9b40);
+                        //zfDelayWriteInternalReg(dev, 0x1c6960, 0x9b40);
+                        //zfDelayWriteInternalReg(dev, 0x1c7960, 0x9b40);
+                        if ((hpPriv->eepromImage[0x100+0x110*2/4]&0xff) == 0x80) //FEM TYPE
+                        {
+                            if (hpPriv->hwFrequency <= ZM_CH_G_14)
+                            {
+                                zfDelayWriteInternalReg(dev, 0x1c8960, 0x9b49);
+                            }
+                            else
+                            {
+                                zfDelayWriteInternalReg(dev, 0x1c8960, 0x0900);
+                            }
+                        }
+                        else
+                        {
+                            zfDelayWriteInternalReg(dev, 0x1c8960, 0x9b40);
+                        }
+                        polluted = 1;
+                    }
+                }
+
+            }
+        }
+
+        if (hpPriv->usbAcSendBytes[3] > (hpPriv->usbAcSendBytes[0]*2))
+        {
+            zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC1_AC0_TXOP, hpPriv->txop[3]);
+            polluted = 1;
+        }
+        else if (hpPriv->usbAcSendBytes[2] > (hpPriv->usbAcSendBytes[0]*2))
+        {
+            zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC1_AC0_TXOP, hpPriv->txop[2]);
+            polluted = 1;
+        }
+        else if (hpPriv->usbAcSendBytes[1] > (hpPriv->usbAcSendBytes[0]*2))
+        {
+            zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC0_CW, hpPriv->cwmin[1]+((u32_t)hpPriv->cwmax[1]<<16));
+            polluted = 1;
+        }
+        else
+        {
+            if (hpPriv->slotType == 1)
+            {
+                if ((wd->sta.enableDrvBA ==0) //Marvel AP
+                   && (hpPriv->aggMaxDurationBE > 2000)) //BE TXOP > 2ms
+                {
+                    zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC0_CW, (hpPriv->cwmin[0]/2)+((u32_t)hpPriv->cwmax[0]<<16));
+                }
+                else
+                {
+                    zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC0_CW, hpPriv->cwmin[0]+((u32_t)hpPriv->cwmax[0]<<16));
+                }
+                polluted = 1;
+            }
+            else
+            {
+                /* Compensation for 20us slot time */
+                //zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC0_CW, 58+((u32_t)hpPriv->cwmax[0]<<16));
+                zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC0_CW, hpPriv->cwmin[0]+((u32_t)hpPriv->cwmax[0]<<16));
+                polluted = 1;
+            }
+
+            if ((wd->sta.SWEncryptEnable & (ZM_SW_TKIP_ENCRY_EN|ZM_SW_WEP_ENCRY_EN)) == 0)
+            {
+                zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC1_AC0_TXOP, hpPriv->txop[0]);
+                polluted = 1;
+            }
+            else
+            {
+                zfDelayWriteInternalReg(dev, ZM_MAC_REG_AC1_AC0_TXOP, 0x30);
+                polluted = 1;
+            }
+
+        }
+        hpPriv->usbAcSendBytes[3] = 0;
+        hpPriv->usbAcSendBytes[2] = 0;
+        hpPriv->usbAcSendBytes[1] = 0;
+        hpPriv->usbAcSendBytes[0] = 0;
+    }
+
+    if (polluted == 1)
+    {
+        zfFlushDelayWrite(dev);
+    }
+
+    return;
+}
+
+/*
+ *  0x1d4008 : AHB, DAC, ADC clock selection
+ *             bit1~0  AHB_CLK : AHB clock selection,
+ *                               00 : OSC 40MHz;
+ *                               01 : 20MHz in A mode, 22MHz in G mode;
+ *                               10 : 40MHz in A mode, 44MHz in G mode;
+ *                               11 : 80MHz in A mode, 88MHz in G mode.
+ *             bit3~2  CLK_SEL : Select the clock source of clk160 in ADDAC.
+ *                               00 : PLL divider's output;
+ *                               01 : PLL divider's output divided by 2;
+ *                               10 : PLL divider's output divided by 4;
+ *                               11 : REFCLK from XTALOSCPAD.
+ */
+void zfSelAdcClk(zdev_t* dev, u8_t bw40, u32_t frequency)
+{
+    if(bw40 == 1)
+    {
+        //zfDelayWriteInternalReg(dev, 0x1D4008, 0x73);
+        zfDelayWriteInternalReg(dev, ZM_MAC_REG_DYNAMIC_SIFS_ACK, 0x10A);
+        zfFlushDelayWrite(dev);
+    }
+    else
+    {
+        //zfDelayWriteInternalReg(dev, 0x1D4008, 0x70);
+        if ( frequency <= ZM_CH_G_14 )
+        {
+            zfDelayWriteInternalReg(dev, ZM_MAC_REG_DYNAMIC_SIFS_ACK, 0x105);
+        }
+        else
+        {
+            zfDelayWriteInternalReg(dev, ZM_MAC_REG_DYNAMIC_SIFS_ACK, 0x104);
+        }
+        zfFlushDelayWrite(dev);
+    }
+}
+
+u32_t zfHpEchoCommand(zdev_t* dev, u32_t value)
+{
+    u32_t cmd[2];
+    u16_t ret;
+
+    cmd[0] = 0x00008004;
+    cmd[1] = value;
+
+    ret = zfIssueCmd(dev, cmd, 8, ZM_CMD_ECHO, NULL);
+    return ret;
+}
+
+#ifdef ZM_DRV_INIT_USB_MODE
+
+#define ZM_USB_US_STREAM_MODE               0x00000000
+#define ZM_USB_US_PACKET_MODE               0x00000008
+#define ZM_USB_DS_ENABLE                    0x00000001
+#define ZM_USB_US_ENABLE                    0x00000002
+
+#define ZM_USB_RX_STREAM_4K                 0x00000000
+#define ZM_USB_RX_STREAM_8K                 0x00000010
+#define ZM_USB_RX_STREAM_16K                0x00000020
+#define ZM_USB_RX_STREAM_32K                0x00000030
+
+#define ZM_USB_TX_STREAM_MODE               0x00000040
+
+#define ZM_USB_MODE_CTRL_REG                0x001E1108
+
+void zfInitUsbMode(zdev_t* dev)
+{
+    u32_t mode;
+    zmw_get_wlan_dev(dev);
+
+    /* TODO: Set USB mode by reading registery */
+    mode = ZM_USB_DS_ENABLE | ZM_USB_US_ENABLE | ZM_USB_US_PACKET_MODE;
+
+    zfDelayWriteInternalReg(dev, ZM_USB_MODE_CTRL_REG, mode);
+    zfFlushDelayWrite(dev);
+}
+#endif
+
+void zfDumpEepBandEdges(struct ar5416Eeprom* eepromImage);
+void zfPrintTargetPower2G(u8_t* tPow2xCck, u8_t* tPow2x2g, u8_t* tPow2x2gHt20, u8_t* tPow2x2gHt40);
+void zfPrintTargetPower5G(u8_t* tPow2x5g, u8_t* tPow2x5gHt20, u8_t* tPow2x5gHt40);
+
+
+s32_t zfInterpolateFunc(s32_t x, s32_t x1, s32_t y1, s32_t x2, s32_t y2)
+{
+    s32_t y;
+
+    if (y2 == y1)
+    {
+        y = y1;
+    }
+    else if (x == x1)
+    {
+        y = y1;
+    }
+    else if (x == x2)
+    {
+        y = y2;
+    }
+    else if (x2 != x1)
+    {
+        y = y1 + (((y2-y1) * (x-x1))/(x2-x1));
+    }
+    else
+    {
+        y = y1;
+    }
+
+    return y;
+}
+
+//#define ZM_ENABLE_TPC_WINDOWS_DEBUG
+//#define ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+
+/* the tx power offset workaround for ART vs NDIS/MDK */
+#define HALTX_POWER_OFFSET      0
+
+u8_t zfInterpolateFuncX(u8_t x, u8_t x1, u8_t y1, u8_t x2, u8_t y2)
+{
+    s32_t y;
+    s32_t inc;
+
+    #define ZM_MULTIPLIER   8
+    y = zfInterpolateFunc((s32_t)x<<ZM_MULTIPLIER,
+                          (s32_t)x1<<ZM_MULTIPLIER,
+                          (s32_t)y1<<ZM_MULTIPLIER,
+                          (s32_t)x2<<ZM_MULTIPLIER,
+                          (s32_t)y2<<ZM_MULTIPLIER);
+
+    inc = (y & (1<<(ZM_MULTIPLIER-1))) >> (ZM_MULTIPLIER-1);
+    y = (y >> ZM_MULTIPLIER) + inc;
+    #undef ZM_MULTIPLIER
+
+    return (u8_t)y;
+}
+
+u8_t zfGetInterpolatedValue(u8_t x, u8_t* x_array, u8_t* y_array)
+{
+    s32_t y;
+    u16_t xIndex;
+
+    if (x <= x_array[1])
+    {
+        xIndex = 0;
+    }
+    else if (x <= x_array[2])
+    {
+        xIndex = 1;
+    }
+    else if (x <= x_array[3])
+    {
+        xIndex = 2;
+    }
+    else //(x > x_array[3])
+    {
+        xIndex = 3;
+    }
+
+    y = zfInterpolateFuncX(x,
+            x_array[xIndex],
+            y_array[xIndex],
+            x_array[xIndex+1],
+            y_array[xIndex+1]);
+
+    return (u8_t)y;
+}
+
+u8_t zfFindFreqIndex(u8_t f, u8_t* fArray, u8_t fArraySize)
+{
+    u8_t i;
+#ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+    DbgPrint("f=%d ", f);
+    for (i=0; i<fArraySize; i++)
+    {
+        DbgPrint("%d ", fArray[i]);
+    }
+    DbgPrint("\n");
+#endif
+    i=fArraySize-2;
+    while(1)
+    {
+        if (f >= fArray[i])
+        {
+            return i;
+        }
+        if (i!=0)
+        {
+            i--;
+        }
+        else
+        {
+            return 0;
+        }
+    }
+}
+
+
+
+
+void zfInitPowerCal(zdev_t* dev)
+{
+    //Program PHY Tx power relatives registers
+#define zm_write_phy_reg(cr, val) reg_write((cr*4)+0x9800, val)
+
+    zm_write_phy_reg(79, 0x7f);
+    zm_write_phy_reg(77, 0x3f3f3f3f);
+    zm_write_phy_reg(78, 0x3f3f3f3f);
+    zm_write_phy_reg(653, 0x3f3f3f3f);
+    zm_write_phy_reg(654, 0x3f3f3f3f);
+    zm_write_phy_reg(739, 0x3f3f3f3f);
+    zm_write_phy_reg(740, 0x3f3f3f3f);
+    zm_write_phy_reg(755, 0x3f3f3f3f);
+    zm_write_phy_reg(756, 0x3f3f3f3f);
+    zm_write_phy_reg(757, 0x3f3f3f3f);
+
+#undef zm_write_phy_reg
+}
+
+
+
+void zfPrintTp(u8_t* pwr0, u8_t* vpd0, u8_t* pwr1, u8_t* vpd1)
+{
+    #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+    DbgPrint("pwr0 : %d, %d, %d, %d ,%d\n", pwr0[0], pwr0[1], pwr0[2], pwr0[3], pwr0[4]);
+    DbgPrint("vpd0 : %d, %d, %d, %d ,%d\n", vpd0[0], vpd0[1], vpd0[2], vpd0[3], vpd0[4]);
+    DbgPrint("pwr1 : %d, %d, %d, %d ,%d\n", pwr1[0], pwr1[1], pwr1[2], pwr1[3], pwr1[4]);
+    DbgPrint("vpd1 : %d, %d, %d, %d ,%d\n", vpd1[0], vpd1[1], vpd1[2], vpd1[3], vpd1[4]);
+    #endif
+}
+
+
+/*
+ * To find CTL index(0~23)
+ * return 24(AR5416_NUM_CTLS)=>no desired index found
+ */
+u8_t zfFindCtlEdgesIndex(zdev_t* dev, u8_t desired_CtlIndex)
+{
+    u8_t i;
+    struct zsHpPriv* hpPriv;
+    struct ar5416Eeprom* eepromImage;
+
+    zmw_get_wlan_dev(dev);
+
+    hpPriv = wd->hpPrivate;
+
+    eepromImage = (struct ar5416Eeprom*)&(hpPriv->eepromImage[(1024+512)/4]);
+
+    //for (i = 0; (i < AR5416_NUM_CTLS) && eepromImage->ctlIndex[i]; i++)
+    for (i = 0; i < AR5416_NUM_CTLS; i++)
+    {
+        if(desired_CtlIndex == eepromImage->ctlIndex[i])
+            break;
+    }
+    return i;
+}
+
+/**************************************************************************
+ * fbin2freq
+ *
+ * Get channel value from binary representation held in eeprom
+ * RETURNS: the frequency in MHz
+ */
+u32_t
+fbin2freq(u8_t fbin, u8_t is2GHz)
+{
+    /*
+     * Reserved value 0xFF provides an empty definition both as
+     * an fbin and as a frequency - do not convert
+     */
+    if (fbin == AR5416_BCHAN_UNUSED) {
+        return fbin;
+    }
+
+    return (u32_t)((is2GHz==1) ? (2300 + fbin) : (4800 + 5 * fbin));
+}
+
+
+u8_t zfGetMaxEdgePower(zdev_t* dev, CAL_CTL_EDGES *pCtlEdges, u32_t freq)
+{
+    u8_t i;
+    u8_t maxEdgePower;
+    u8_t is2GHz;
+    struct zsHpPriv* hpPriv;
+    struct ar5416Eeprom* eepromImage;
+
+    zmw_get_wlan_dev(dev);
+
+    hpPriv = wd->hpPrivate;
+
+    eepromImage = (struct ar5416Eeprom*)&(hpPriv->eepromImage[(1024+512)/4]);
+
+    if(freq > ZM_CH_G_14)
+        is2GHz = 0;
+    else
+        is2GHz = 1;
+
+    maxEdgePower = AR5416_MAX_RATE_POWER;
+
+    /* Get the edge power */
+    for (i = 0; (i < AR5416_NUM_BAND_EDGES) && (pCtlEdges[i].bChannel != AR5416_BCHAN_UNUSED) ; i++)
+    {
+        /*
+         * If there's an exact channel match or an inband flag set
+         * on the lower channel use the given rdEdgePower
+         */
+        if (freq == fbin2freq(pCtlEdges[i].bChannel, is2GHz))
+        {
+            maxEdgePower = pCtlEdges[i].tPower;
+            #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+            zm_dbg(("zfGetMaxEdgePower index i = %d \n", i));
+            #endif
+            break;
+        }
+        else if ((i > 0) && (freq < fbin2freq(pCtlEdges[i].bChannel, is2GHz)))
+        {
+            if (fbin2freq(pCtlEdges[i - 1].bChannel, is2GHz) < freq && pCtlEdges[i - 1].flag)
+            {
+                maxEdgePower = pCtlEdges[i - 1].tPower;
+                #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+                zm_dbg(("zfGetMaxEdgePower index i-1 = %d \n", i-1));
+                #endif
+            }
+            /* Leave loop - no more affecting edges possible in this monotonic increasing list */
+            break;
+        }
+
+    }
+
+    if( i == AR5416_NUM_BAND_EDGES )
+    {
+        if (freq > fbin2freq(pCtlEdges[i - 1].bChannel, is2GHz) && pCtlEdges[i - 1].flag)
+        {
+            maxEdgePower = pCtlEdges[i - 1].tPower;
+            #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+            zm_dbg(("zfGetMaxEdgePower index=>i-1 = %d \n", i-1));
+            #endif
+        }
+    }
+
+    zm_assert(maxEdgePower > 0);
+
+  #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+    if ( maxEdgePower == AR5416_MAX_RATE_POWER )
+    {
+        zm_dbg(("zfGetMaxEdgePower = %d !!!\n", AR5416_MAX_RATE_POWER));
+    }
+  #endif
+    return maxEdgePower;
+}
+
+u32_t zfAdjustHT40FreqOffset(zdev_t* dev, u32_t frequency, u8_t bw40, u8_t extOffset)
+{
+    u32_t newFreq = frequency;
+
+	if (bw40 == 1)
+	{
+        if (extOffset == 1)
+        {
+            newFreq += 10;
+        }
+        else
+        {
+            newFreq -= 10;
+        }
+	}
+	return newFreq;
+}
+
+u32_t zfHpCheckDoHeavyClip(zdev_t* dev, u32_t freq, CAL_CTL_EDGES *pCtlEdges, u8_t bw40)
+{
+    u32_t ret = 0;
+    u8_t i;
+    u8_t is2GHz;
+    struct zsHpPriv* hpPriv;
+
+    zmw_get_wlan_dev(dev);
+
+    hpPriv = wd->hpPrivate;
+
+    if(freq > ZM_CH_G_14)
+        is2GHz = 0;
+    else
+        is2GHz = 1;
+
+    /* HT40 force enable heavy clip */
+    if (bw40)
+    {
+        ret |= 0xf0;
+    }
+#if 1
+    /* HT20 : frequency bandedge */
+    for (i = 0; (i < AR5416_NUM_BAND_EDGES) && (pCtlEdges[i].bChannel != AR5416_BCHAN_UNUSED) ; i++)
+    {
+        if (freq == fbin2freq(pCtlEdges[i].bChannel, is2GHz))
+        {
+            if (pCtlEdges[i].flag == 0)
+            {
+                ret |= 0xf;
+            }
+            break;
+        }
+    }
+#endif
+
+    return ret;
+}
+
+
+void zfSetPowerCalTable(zdev_t* dev, u32_t frequency, u8_t bw40, u8_t extOffset)
+{
+    struct ar5416Eeprom* eepromImage;
+    u8_t pwr0[5];
+    u8_t pwr1[5];
+    u8_t vpd0[5];
+    u8_t vpd1[5];
+    u8_t vpd_chain1[128];
+    u8_t vpd_chain3[128];
+    u16_t boundary1 = 18; //CR 667
+    u16_t powerTxMax = 63; //CR 79
+    u8_t i;
+    struct zsHpPriv* hpPriv;
+    u8_t fbin;
+    u8_t index, max2gIndex, max5gIndex;
+    u8_t chain0pwrPdg0[5];
+    u8_t chain0vpdPdg0[5];
+    u8_t chain0pwrPdg1[5];
+    u8_t chain0vpdPdg1[5];
+    u8_t chain2pwrPdg0[5];
+    u8_t chain2vpdPdg0[5];
+    u8_t chain2pwrPdg1[5];
+    u8_t chain2vpdPdg1[5];
+    u8_t fbinArray[8];
+
+    /* 4 CTL */
+    u8_t ctl_i;
+    u8_t desired_CtlIndex;
+
+    u8_t ctlEdgesMaxPowerCCK = AR5416_MAX_RATE_POWER;
+    u8_t ctlEdgesMaxPower2G = AR5416_MAX_RATE_POWER;
+    u8_t ctlEdgesMaxPower2GHT20 = AR5416_MAX_RATE_POWER;
+    u8_t ctlEdgesMaxPower2GHT40 = AR5416_MAX_RATE_POWER;
+    u8_t ctlEdgesMaxPower5G = AR5416_MAX_RATE_POWER;
+    u8_t ctlEdgesMaxPower5GHT20 = AR5416_MAX_RATE_POWER;
+    u8_t ctlEdgesMaxPower5GHT40 = AR5416_MAX_RATE_POWER;
+
+    u8_t ctlOffset;
+
+    zmw_get_wlan_dev(dev);
+
+    hpPriv = wd->hpPrivate;
+
+    eepromImage = (struct ar5416Eeprom*)&(hpPriv->eepromImage[(1024+512)/4]);
+
+    // Check the total bytes of the EEPROM structure to see the dongle have been calibrated or not.
+    if (eepromImage->baseEepHeader.length == 0xffff)
+    {
+        #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+        zm_dbg(("Warning! This dongle not been calibrated\n"));
+        #endif
+        return;
+    }
+
+    #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+    DbgPrint("-----zfSetPowerCalTable : frequency=%d-----\n", frequency);
+    #endif
+    /* TODO : 1. boundary1 and powerTxMax should be refered to CR667 and CR79 */
+    /*           in otus.ini file                                          */
+
+    #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+    /* 2. Interpolate pwr and vpd test points from frequency */
+    DbgPrint("calFreqPier5G : %d, %d, %d, %d ,%d, %d, %d, %d\n",
+                                            eepromImage->calFreqPier5G[0]*5+4800,
+                                            eepromImage->calFreqPier5G[1]*5+4800,
+                                            eepromImage->calFreqPier5G[2]*5+4800,
+                                            eepromImage->calFreqPier5G[3]*5+4800,
+                                            eepromImage->calFreqPier5G[4]*5+4800,
+                                            eepromImage->calFreqPier5G[5]*5+4800,
+                                            eepromImage->calFreqPier5G[6]*5+4800,
+                                            eepromImage->calFreqPier5G[7]*5+4800
+                                            );
+    DbgPrint("calFreqPier2G : %d, %d, %d, %d\n",
+                                            eepromImage->calFreqPier2G[0]+2300,
+                                            eepromImage->calFreqPier2G[1]+2300,
+                                            eepromImage->calFreqPier2G[2]+2300,
+                                            eepromImage->calFreqPier2G[3]+2300
+                                            );
+    #endif
+    if (frequency < 3000)
+    {
+        for (i=0; i<4; i++)
+        {
+            if (eepromImage->calFreqPier2G[i] == 0xff)
+            {
+                break;
+            }
+        }
+        max2gIndex = i;
+        #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+        DbgPrint("max2gIndex : %d\n", max2gIndex);
+        #endif
+        fbin = (u8_t)(frequency - 2300);
+        index = zfFindFreqIndex(fbin, eepromImage->calFreqPier2G, max2gIndex);
+        #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+        DbgPrint("2G index : %d\n", index);
+        DbgPrint("chain 0 index\n");
+        #endif
+        zfPrintTp(&eepromImage->calPierData2G[0][index].pwrPdg[0][0],
+                  &eepromImage->calPierData2G[0][index].vpdPdg[0][0],
+                  &eepromImage->calPierData2G[0][index].pwrPdg[1][0],
+                  &eepromImage->calPierData2G[0][index].vpdPdg[1][0]
+                  );
+        #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+        DbgPrint("chain 0 index+1\n");
+        #endif
+        zfPrintTp(&eepromImage->calPierData2G[0][index+1].pwrPdg[0][0],
+                  &eepromImage->calPierData2G[0][index+1].vpdPdg[0][0],
+                  &eepromImage->calPierData2G[0][index+1].pwrPdg[1][0],
+                  &eepromImage->calPierData2G[0][index+1].vpdPdg[1][0]
+                  );
+
+        for (i=0; i<5; i++)
+        {
+            chain0pwrPdg0[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calFreqPier2G[index],
+                    eepromImage->calPierData2G[0][index].pwrPdg[0][i],
+                    eepromImage->calFreqPier2G[index+1],
+                    eepromImage->calPierData2G[0][index+1].pwrPdg[0][i]
+                    );
+            chain0vpdPdg0[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calFreqPier2G[index],
+                    eepromImage->calPierData2G[0][index].vpdPdg[0][i],
+                    eepromImage->calFreqPier2G[index+1],
+                    eepromImage->calPierData2G[0][index+1].vpdPdg[0][i]
+                    );
+            chain0pwrPdg1[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calFreqPier2G[index],
+                    eepromImage->calPierData2G[0][index].pwrPdg[1][i],
+                    eepromImage->calFreqPier2G[index+1],
+                    eepromImage->calPierData2G[0][index+1].pwrPdg[1][i]
+                    );
+            chain0vpdPdg1[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calFreqPier2G[index],
+                    eepromImage->calPierData2G[0][index].vpdPdg[1][i],
+                    eepromImage->calFreqPier2G[index+1],
+                    eepromImage->calPierData2G[0][index+1].vpdPdg[1][i]
+                    );
+
+            chain2pwrPdg0[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calFreqPier2G[index],
+                    eepromImage->calPierData2G[1][index].pwrPdg[0][i],
+                    eepromImage->calFreqPier2G[index+1],
+                    eepromImage->calPierData2G[1][index+1].pwrPdg[0][i]
+                    );
+            chain2vpdPdg0[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calFreqPier2G[index],
+                    eepromImage->calPierData2G[1][index].vpdPdg[0][i],
+                    eepromImage->calFreqPier2G[index+1],
+                    eepromImage->calPierData2G[1][index+1].vpdPdg[0][i]
+                    );
+            chain2pwrPdg1[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calFreqPier2G[index],
+                    eepromImage->calPierData2G[1][index].pwrPdg[1][i],
+                    eepromImage->calFreqPier2G[index+1],
+                    eepromImage->calPierData2G[1][index+1].pwrPdg[1][i]
+                    );
+            chain2vpdPdg1[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calFreqPier2G[index],
+                    eepromImage->calPierData2G[1][index].vpdPdg[1][i],
+                    eepromImage->calFreqPier2G[index+1],
+                    eepromImage->calPierData2G[1][index+1].vpdPdg[1][i]
+                    );
+        }
+    }
+    else
+    {
+        for (i=0; i<8; i++)
+        {
+            if (eepromImage->calFreqPier5G[i] == 0xff)
+            {
+                break;
+            }
+        }
+        max5gIndex = i;
+        #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+        DbgPrint("max5gIndex : %d\n", max5gIndex);
+        #endif
+        fbin = (u8_t)((frequency - 4800)/5);
+        index = zfFindFreqIndex(fbin, eepromImage->calFreqPier5G, max5gIndex);
+        #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+        DbgPrint("5G index : %d\n", index);
+        #endif
+
+        for (i=0; i<5; i++)
+        {
+            chain0pwrPdg0[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calFreqPier5G[index],
+                    eepromImage->calPierData5G[0][index].pwrPdg[0][i],
+                    eepromImage->calFreqPier5G[index+1],
+                    eepromImage->calPierData5G[0][index+1].pwrPdg[0][i]
+                    );
+            chain0vpdPdg0[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calFreqPier5G[index],
+                    eepromImage->calPierData5G[0][index].vpdPdg[0][i],
+                    eepromImage->calFreqPier5G[index+1],
+                    eepromImage->calPierData5G[0][index+1].vpdPdg[0][i]
+                    );
+            chain0pwrPdg1[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calFreqPier5G[index],
+                    eepromImage->calPierData5G[0][index].pwrPdg[1][i],
+                    eepromImage->calFreqPier5G[index+1],
+                    eepromImage->calPierData5G[0][index+1].pwrPdg[1][i]
+                    );
+            chain0vpdPdg1[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calFreqPier5G[index],
+                    eepromImage->calPierData5G[0][index].vpdPdg[1][i],
+                    eepromImage->calFreqPier5G[index+1],
+                    eepromImage->calPierData5G[0][index+1].vpdPdg[1][i]
+                    );
+
+            chain2pwrPdg0[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calFreqPier5G[index],
+                    eepromImage->calPierData5G[1][index].pwrPdg[0][i],
+                    eepromImage->calFreqPier5G[index+1],
+                    eepromImage->calPierData5G[1][index+1].pwrPdg[0][i]
+                    );
+            chain2vpdPdg0[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calFreqPier5G[index],
+                    eepromImage->calPierData5G[1][index].vpdPdg[0][i],
+                    eepromImage->calFreqPier5G[index+1],
+                    eepromImage->calPierData5G[1][index+1].vpdPdg[0][i]
+                    );
+            chain2pwrPdg1[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calFreqPier5G[index],
+                    eepromImage->calPierData5G[1][index].pwrPdg[1][i],
+                    eepromImage->calFreqPier5G[index+1],
+                    eepromImage->calPierData5G[1][index+1].pwrPdg[1][i]
+                    );
+            chain2vpdPdg1[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calFreqPier5G[index],
+                    eepromImage->calPierData5G[1][index].vpdPdg[1][i],
+                    eepromImage->calFreqPier5G[index+1],
+                    eepromImage->calPierData5G[1][index+1].vpdPdg[1][i]
+                    );
+        }
+
+    }
+
+
+    /* Chain 1 */
+    /* Get pwr and vpd test points from frequency */
+    for (i=0; i<5; i++)
+    {
+        pwr0[i] = chain0pwrPdg0[i]>>1;
+        vpd0[i] = chain0vpdPdg0[i];
+        pwr1[i] = chain0pwrPdg1[i]>>1;
+        vpd1[i] = chain0vpdPdg1[i];
+    }
+    #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+    DbgPrint("Test Points\n");
+    DbgPrint("pwr0 : %d, %d, %d, %d ,%d\n", pwr0[0], pwr0[1], pwr0[2], pwr0[3], pwr0[4]);
+    DbgPrint("vpd0 : %d, %d, %d, %d ,%d\n", vpd0[0], vpd0[1], vpd0[2], vpd0[3], vpd0[4]);
+    DbgPrint("pwr1 : %d, %d, %d, %d ,%d\n", pwr1[0], pwr1[1], pwr1[2], pwr1[3], pwr1[4]);
+    DbgPrint("vpd1 : %d, %d, %d, %d ,%d\n", vpd1[0], vpd1[1], vpd1[2], vpd1[3], vpd1[4]);
+    #endif
+    /* Generate the vpd arrays */
+    for (i=0; i<boundary1+1+6; i++)
+    {
+        vpd_chain1[i] = zfGetInterpolatedValue(i, &pwr0[0], &vpd0[0]);
+    }
+    for (; i<powerTxMax+1+6+6; i++)
+    {
+        vpd_chain1[i] = zfGetInterpolatedValue(i-6-6, &pwr1[0], &vpd1[0]);
+    }
+    #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+    DbgPrint("vpd_chain1\n");
+    for (i=0; i<powerTxMax+1+6+6; i+=10)
+    {
+        DbgPrint("%d, %d, %d, %d ,%d, %d, %d, %d, %d, %d\n",
+                vpd_chain1[i+0], vpd_chain1[i+1], vpd_chain1[i+2], vpd_chain1[i+3], vpd_chain1[i+4],
+                vpd_chain1[i+5], vpd_chain1[i+6], vpd_chain1[i+7], vpd_chain1[i+8], vpd_chain1[i+9]);
+    }
+    #endif
+    /* Write PHY regs 672-703 */
+    for (i=0; i<128; i+=4)
+    {
+        u32_t regAddr = 0x9800 + (672 * 4);
+        u32_t val;
+
+        val = ((u32_t)vpd_chain1[i+3]<<24) |
+                ((u32_t)vpd_chain1[i+2]<<16) |
+                ((u32_t)vpd_chain1[i+1]<<8) |
+                ((u32_t)vpd_chain1[i]);
+
+        #ifndef ZM_OTUS_LINUX_PHASE_2
+        reg_write(regAddr + i, val);  /* CR672 */
+        #endif
+    }
+
+    /* Chain 2 */
+    /* Get pwr and vpd test points from frequency */
+    for (i=0; i<5; i++)
+    {
+        pwr0[i] = chain2pwrPdg0[i]>>1;
+        vpd0[i] = chain2vpdPdg0[i];
+        pwr1[i] = chain2pwrPdg1[i]>>1;
+        vpd1[i] = chain2vpdPdg1[i];
+    }
+    #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+    DbgPrint("Test Points\n");
+    DbgPrint("pwr0 : %d, %d, %d, %d ,%d\n", pwr0[0], pwr0[1], pwr0[2], pwr0[3], pwr0[4]);
+    DbgPrint("vpd0 : %d, %d, %d, %d ,%d\n", vpd0[0], vpd0[1], vpd0[2], vpd0[3], vpd0[4]);
+    DbgPrint("pwr1 : %d, %d, %d, %d ,%d\n", pwr1[0], pwr1[1], pwr1[2], pwr1[3], pwr1[4]);
+    DbgPrint("vpd1 : %d, %d, %d, %d ,%d\n", vpd1[0], vpd1[1], vpd1[2], vpd1[3], vpd1[4]);
+    #endif
+    /* Generate the vpd arrays */
+    for (i=0; i<boundary1+1+6; i++)
+    {
+        vpd_chain3[i] = zfGetInterpolatedValue(i, &pwr0[0], &vpd0[0]);
+    }
+    for (; i<powerTxMax+1+6+6; i++)
+    {
+        vpd_chain3[i] = zfGetInterpolatedValue(i-6-6, &pwr1[0], &vpd1[0]);
+    }
+    #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+    DbgPrint("vpd_chain3\n");
+    for (i=0; i<powerTxMax+1+6+6; i+=10)
+    {
+        DbgPrint("%d, %d, %d, %d ,%d, %d, %d, %d, %d, %d\n",
+                vpd_chain3[i+0], vpd_chain3[i+1], vpd_chain3[i+2], vpd_chain3[i+3], vpd_chain3[i+4],
+                vpd_chain3[i+5], vpd_chain3[i+6], vpd_chain3[i+7], vpd_chain3[i+8], vpd_chain3[i+9]);
+    }
+    #endif
+
+    /* Write PHY regs 672-703 + 0x1000 */
+    for (i=0; i<128; i+=4)
+    {
+        u32_t regAddr = 0x9800 + (672 * 4) + 0x1000;
+        u32_t val;
+
+        val = ((u32_t)vpd_chain3[i+3]<<24) |
+                ((u32_t)vpd_chain3[i+2]<<16) |
+                ((u32_t)vpd_chain3[i+1]<<8) |
+                ((u32_t)vpd_chain3[i]);
+
+        #ifndef ZM_OTUS_LINUX_PHASE_2
+        reg_write(regAddr + i, val);  /* CR672 */
+        #endif
+    }
+
+    zfFlushDelayWrite(dev);
+
+    /* 3. Generate target power table */
+    if (frequency < 3000)
+    {
+        for (i=0; i<3; i++)
+        {
+            if (eepromImage->calTargetPowerCck[i].bChannel != 0xff)
+            {
+                fbinArray[i] = eepromImage->calTargetPowerCck[i].bChannel;
+            }
+            else
+            {
+                break;
+            }
+
+        }
+        index = zfFindFreqIndex(fbin, fbinArray, i);
+        #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+        DbgPrint("CCK index=%d\n", index);
+        #endif
+        for (i=0; i<4; i++)
+        {
+            hpPriv->tPow2xCck[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calTargetPowerCck[index].bChannel,
+                    eepromImage->calTargetPowerCck[index].tPow2x[i],
+                    eepromImage->calTargetPowerCck[index+1].bChannel,
+                    eepromImage->calTargetPowerCck[index+1].tPow2x[i]
+                    );
+        }
+
+        for (i=0; i<4; i++)
+        {
+            if (eepromImage->calTargetPower2G[i].bChannel != 0xff)
+            {
+                fbinArray[i] = eepromImage->calTargetPower2G[i].bChannel;
+            }
+            else
+            {
+                break;
+            }
+
+        }
+        index = zfFindFreqIndex(fbin, fbinArray, i);
+        #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+        DbgPrint("2G index=%d\n", index);
+        #endif
+        for (i=0; i<4; i++)
+        {
+            hpPriv->tPow2x2g[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calTargetPower2G[index].bChannel,
+                    eepromImage->calTargetPower2G[index].tPow2x[i],
+                    eepromImage->calTargetPower2G[index+1].bChannel,
+                    eepromImage->calTargetPower2G[index+1].tPow2x[i]
+                    );
+        }
+
+        for (i=0; i<4; i++)
+        {
+            if (eepromImage->calTargetPower2GHT20[i].bChannel != 0xff)
+            {
+                fbinArray[i] = eepromImage->calTargetPower2GHT20[i].bChannel;
+            }
+            else
+            {
+                break;
+            }
+
+        }
+        index = zfFindFreqIndex(fbin, fbinArray, i);
+        #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+        DbgPrint("2G HT20 index=%d\n", index);
+        #endif
+        for (i=0; i<8; i++)
+        {
+            hpPriv->tPow2x2gHt20[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calTargetPower2GHT20[index].bChannel,
+                    eepromImage->calTargetPower2GHT20[index].tPow2x[i],
+                    eepromImage->calTargetPower2GHT20[index+1].bChannel,
+                    eepromImage->calTargetPower2GHT20[index+1].tPow2x[i]
+                    );
+        }
+
+        for (i=0; i<4; i++)
+        {
+            if (eepromImage->calTargetPower2GHT40[i].bChannel != 0xff)
+            {
+                fbinArray[i] = eepromImage->calTargetPower2GHT40[i].bChannel;
+            }
+            else
+            {
+                break;
+            }
+
+        }
+        index = zfFindFreqIndex( (u8_t)zfAdjustHT40FreqOffset(dev, fbin, bw40, extOffset), fbinArray, i);
+        #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+        DbgPrint("2G HT40 index=%d\n", index);
+        #endif
+        for (i=0; i<8; i++)
+        {
+            hpPriv->tPow2x2gHt40[i] = zfInterpolateFuncX(
+                    (u8_t)zfAdjustHT40FreqOffset(dev, fbin, bw40, extOffset),
+                    eepromImage->calTargetPower2GHT40[index].bChannel,
+                    eepromImage->calTargetPower2GHT40[index].tPow2x[i],
+                    eepromImage->calTargetPower2GHT40[index+1].bChannel,
+                    eepromImage->calTargetPower2GHT40[index+1].tPow2x[i]
+                    );
+        }
+
+        zfPrintTargetPower2G(hpPriv->tPow2xCck,
+                hpPriv->tPow2x2g,
+                hpPriv->tPow2x2gHt20,
+                hpPriv->tPow2x2gHt40);
+    }
+    else
+    {
+        /* 5G */
+        for (i=0; i<8; i++)
+        {
+            if (eepromImage->calTargetPower5G[i].bChannel != 0xff)
+            {
+                fbinArray[i] = eepromImage->calTargetPower5G[i].bChannel;
+            }
+            else
+            {
+                break;
+            }
+
+        }
+        index = zfFindFreqIndex(fbin, fbinArray, i);
+        #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+        DbgPrint("5G index=%d\n", index);
+        #endif
+        for (i=0; i<4; i++)
+        {
+            hpPriv->tPow2x5g[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calTargetPower5G[index].bChannel,
+                    eepromImage->calTargetPower5G[index].tPow2x[i],
+                    eepromImage->calTargetPower5G[index+1].bChannel,
+                    eepromImage->calTargetPower5G[index+1].tPow2x[i]
+                    );
+        }
+
+        for (i=0; i<8; i++)
+        {
+            if (eepromImage->calTargetPower5GHT20[i].bChannel != 0xff)
+            {
+                fbinArray[i] = eepromImage->calTargetPower5GHT20[i].bChannel;
+            }
+            else
+            {
+                break;
+            }
+
+        }
+        index = zfFindFreqIndex(fbin, fbinArray, i);
+        #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+        DbgPrint("5G HT20 index=%d\n", index);
+        #endif
+        for (i=0; i<8; i++)
+        {
+            hpPriv->tPow2x5gHt20[i] = zfInterpolateFuncX(fbin,
+                    eepromImage->calTargetPower5GHT20[index].bChannel,
+                    eepromImage->calTargetPower5GHT20[index].tPow2x[i],
+                    eepromImage->calTargetPower5GHT20[index+1].bChannel,
+                    eepromImage->calTargetPower5GHT20[index+1].tPow2x[i]
+                    );
+        }
+
+        for (i=0; i<8; i++)
+        {
+            if (eepromImage->calTargetPower5GHT40[i].bChannel != 0xff)
+            {
+                fbinArray[i] = eepromImage->calTargetPower5GHT40[i].bChannel;
+            }
+            else
+            {
+                break;
+            }
+
+        }
+        index = zfFindFreqIndex((u8_t)zfAdjustHT40FreqOffset(dev, fbin, bw40, extOffset), fbinArray, i);
+        #ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+        DbgPrint("5G HT40 index=%d\n", index);
+        #endif
+        for (i=0; i<8; i++)
+        {
+            hpPriv->tPow2x5gHt40[i] = zfInterpolateFuncX(
+                    (u8_t)zfAdjustHT40FreqOffset(dev, fbin, bw40, extOffset),
+                    eepromImage->calTargetPower5GHT40[index].bChannel,
+                    eepromImage->calTargetPower5GHT40[index].tPow2x[i],
+                    eepromImage->calTargetPower5GHT40[index+1].bChannel,
+                    eepromImage->calTargetPower5GHT40[index+1].tPow2x[i]
+                    );
+        }
+
+        zfPrintTargetPower5G(
+                hpPriv->tPow2x5g,
+                hpPriv->tPow2x5gHt20,
+                hpPriv->tPow2x5gHt40);
+    }
+
+
+
+    /* 4. CTL */
+    /*
+     * 4.1 Get the bandedges tx power by frequency
+     *      2.4G we get ctlEdgesMaxPowerCCK
+     *                  ctlEdgesMaxPower2G
+     *                  ctlEdgesMaxPower2GHT20
+     *                  ctlEdgesMaxPower2GHT40
+     *      5G we get   ctlEdgesMaxPower5G
+     *                  ctlEdgesMaxPower5GHT20
+     *                  ctlEdgesMaxPower5GHT40
+     * 4.2 Update (3.) target power table by 4.1
+     * 4.3 Tx power offset for ART - NDIS/MDK
+     * 4.4 Write MAC reg 0x694 for ACK's TPC
+     *
+     */
+
+    //zfDumpEepBandEdges(eepromImage);
+
+    /* get the cfg from Eeprom: regionCode => RegulatoryDomain : 0x10-FFC  0x30-eu 0x40-jap */
+    desired_CtlIndex = zfHpGetRegulatoryDomain(dev);
+    if ((desired_CtlIndex == 0x30) || (desired_CtlIndex == 0x40) || (desired_CtlIndex == 0x0))
+    {
+        /* skip CTL and heavy clip */
+        hpPriv->enableBBHeavyClip = 0;
+        #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+        zm_dbg(("RegulatoryDomain = 0, skip CTL and heavy clip\n"));
+        #endif
+    }
+    else
+    {
+        hpPriv->enableBBHeavyClip = 1;
+
+        if (desired_CtlIndex == 0xff)
+        {
+            /* desired index not found */
+            desired_CtlIndex = 0x10;
+        }
+
+        /* first part : 2.4G */
+        if (frequency <= ZM_CH_G_14)
+        {
+            /* 2.4G - CTL_11B */
+            ctl_i = zfFindCtlEdgesIndex(dev, desired_CtlIndex|CTL_11B);
+            if(ctl_i<AR5416_NUM_CTLS)
+            {
+                ctlEdgesMaxPowerCCK = zfGetMaxEdgePower(dev, eepromImage->ctlData[ctl_i].ctlEdges[1], frequency);
+            }
+            #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+            zm_dbg(("CTL_11B ctl_i = %d\n", ctl_i));
+            #endif
+
+            /* 2.4G - CTL_11G */
+            ctl_i = zfFindCtlEdgesIndex(dev, desired_CtlIndex|CTL_11G);
+            if(ctl_i<AR5416_NUM_CTLS)
+            {
+                ctlEdgesMaxPower2G = zfGetMaxEdgePower(dev, eepromImage->ctlData[ctl_i].ctlEdges[1], frequency);
+            }
+            #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+            zm_dbg(("CTL_11G ctl_i = %d\n", ctl_i));
+            #endif
+
+            /* 2.4G - CTL_2GHT20 */
+            ctl_i = zfFindCtlEdgesIndex(dev, desired_CtlIndex|CTL_2GHT20);
+            if(ctl_i<AR5416_NUM_CTLS)
+            {
+                ctlEdgesMaxPower2GHT20 = zfGetMaxEdgePower(dev, eepromImage->ctlData[ctl_i].ctlEdges[1], frequency);
+            }
+            else
+            {
+                /* workaround for no data in Eeprom, replace by normal 2G */
+                ctlEdgesMaxPower2GHT20 = ctlEdgesMaxPower2G;
+            }
+            #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+            zm_dbg(("CTL_2GHT20 ctl_i = %d\n", ctl_i));
+            #endif
+
+            /* 2.4G - CTL_2GHT40 */
+            ctl_i = zfFindCtlEdgesIndex(dev, desired_CtlIndex|CTL_2GHT40);
+            if(ctl_i<AR5416_NUM_CTLS)
+            {
+                ctlEdgesMaxPower2GHT40 = zfGetMaxEdgePower(dev, eepromImage->ctlData[ctl_i].ctlEdges[1],
+                                                                zfAdjustHT40FreqOffset(dev, frequency, bw40, extOffset));
+            }
+            else
+            {
+                /* workaround for no data in Eeprom, replace by normal 2G */
+                ctlEdgesMaxPower2GHT40 = ctlEdgesMaxPower2G;
+            }
+            #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+            zm_dbg(("CTL_2GHT40 ctl_i = %d\n", ctl_i));
+            #endif
+
+
+            /* 7a17 :  */
+            /* Max power (dBm) for channel range when using DFS define by madwifi*/
+            for (i=0; i<wd->regulationTable.allowChannelCnt; i++)
+            {
+                if (wd->regulationTable.allowChannel[i].channel == frequency)
+                {
+                    if (zfHpIsDfsChannel(dev, (u16_t)frequency))
+                    {
+                        zm_debug_msg1("frequency use DFS  -- ", frequency);
+                        ctlEdgesMaxPowerCCK     = zm_min(ctlEdgesMaxPowerCCK, wd->regulationTable.allowChannel[i].maxRegTxPower*2);
+                        ctlEdgesMaxPower2G      = zm_min(ctlEdgesMaxPower2G, wd->regulationTable.allowChannel[i].maxRegTxPower*2);
+                        ctlEdgesMaxPower2GHT20  = zm_min(ctlEdgesMaxPower2GHT20, wd->regulationTable.allowChannel[i].maxRegTxPower*2);
+                        ctlEdgesMaxPower2GHT40  = zm_min(ctlEdgesMaxPower2GHT40, wd->regulationTable.allowChannel[i].maxRegTxPower*2);
+                    }
+                    break;
+                }
+            }
+
+            /* Apply ctl mode to correct target power set */
+            #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+            zm_debug_msg1("ctlEdgesMaxPowerCCK    = ", ctlEdgesMaxPowerCCK);
+            zm_debug_msg1("ctlEdgesMaxPower2G     = ", ctlEdgesMaxPower2G);
+            zm_debug_msg1("ctlEdgesMaxPower2GHT20 = ", ctlEdgesMaxPower2GHT20);
+            zm_debug_msg1("ctlEdgesMaxPower2GHT40 = ", ctlEdgesMaxPower2GHT40);
+            #endif
+            for (i=0; i<4; i++)
+            {
+                hpPriv->tPow2xCck[i] = zm_min(hpPriv->tPow2xCck[i], ctlEdgesMaxPowerCCK) + HALTX_POWER_OFFSET;
+            }
+            hpPriv->tPow2x2g24HeavyClipOffset = 0;
+            if (hpPriv->enableBBHeavyClip)
+            {
+                ctlOffset = 2;
+            }
+            else
+            {
+                ctlOffset = 0;
+            }
+            for (i=0; i<4; i++)
+            {
+                if (((frequency == 2412) || (frequency == 2462)))
+                {
+                    if (i != 0)
+                    {
+                        hpPriv->tPow2x2g[i] = zm_min(hpPriv->tPow2x2g[i], ctlEdgesMaxPower2G-ctlOffset) + HALTX_POWER_OFFSET;
+                    }
+                    else
+                    {
+                        hpPriv->tPow2x2g[i] = zm_min(hpPriv->tPow2x2g[i], ctlEdgesMaxPower2G) + HALTX_POWER_OFFSET;
+                        if (hpPriv->tPow2x2g[i] > (ctlEdgesMaxPower2G-ctlOffset))
+                        {
+                            hpPriv->tPow2x2g24HeavyClipOffset = hpPriv->tPow2x2g[i] - (ctlEdgesMaxPower2G-ctlOffset);
+                        }
+                    }
+                }
+                else
+                {
+                    hpPriv->tPow2x2g[i] = zm_min(hpPriv->tPow2x2g[i], ctlEdgesMaxPower2G) + HALTX_POWER_OFFSET;
+                }
+            }
+            for (i=0; i<8; i++)
+            {
+                if (((frequency == 2412) || (frequency == 2462)) && (i>=3))
+                {
+                    hpPriv->tPow2x2gHt20[i] = zm_min(hpPriv->tPow2x2gHt20[i], ctlEdgesMaxPower2GHT20-ctlOffset) + HALTX_POWER_OFFSET;
+                }
+                else
+                {
+                    hpPriv->tPow2x2gHt20[i] = zm_min(hpPriv->tPow2x2gHt20[i], ctlEdgesMaxPower2GHT20) + HALTX_POWER_OFFSET;
+                }
+            }
+            for (i=0; i<8; i++)
+            {
+                if ((frequency == 2412) && (i>=3))
+                {
+                    hpPriv->tPow2x2gHt40[i] = zm_min(hpPriv->tPow2x2gHt40[i], ctlEdgesMaxPower2GHT40-ctlOffset) + HALTX_POWER_OFFSET;
+                }
+                else if ((frequency == 2462) && (i>=3))
+                {
+                    hpPriv->tPow2x2gHt40[i] = zm_min(hpPriv->tPow2x2gHt40[i], ctlEdgesMaxPower2GHT40-(ctlOffset*2)) + HALTX_POWER_OFFSET;
+                }
+                else
+                {
+                    hpPriv->tPow2x2gHt40[i] = zm_min(hpPriv->tPow2x2gHt40[i], ctlEdgesMaxPower2GHT40) + HALTX_POWER_OFFSET;
+                }
+            }
+        }
+        else
+        {
+            /* 5G - CTL_11A */
+            ctl_i = zfFindCtlEdgesIndex(dev, desired_CtlIndex|CTL_11A);
+            if(ctl_i<AR5416_NUM_CTLS)
+            {
+                ctlEdgesMaxPower5G = zfGetMaxEdgePower(dev, eepromImage->ctlData[ctl_i].ctlEdges[1], frequency);
+            }
+            #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+            zm_dbg(("CTL_11A ctl_i = %d\n", ctl_i));
+            #endif
+
+            /* 5G - CTL_5GHT20 */
+            ctl_i = zfFindCtlEdgesIndex(dev, desired_CtlIndex|CTL_5GHT20);
+            if(ctl_i<AR5416_NUM_CTLS)
+            {
+                ctlEdgesMaxPower5GHT20 = zfGetMaxEdgePower(dev, eepromImage->ctlData[ctl_i].ctlEdges[1], frequency);
+            }
+            else
+            {
+                /* workaround for no data in Eeprom, replace by normal 5G */
+                ctlEdgesMaxPower5GHT20 = ctlEdgesMaxPower5G;
+            }
+            #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+            zm_dbg(("CTL_5GHT20 ctl_i = %d\n", ctl_i));
+            #endif
+
+            /* 5G - CTL_5GHT40 */
+            ctl_i = zfFindCtlEdgesIndex(dev, desired_CtlIndex|CTL_5GHT40);
+            if(ctl_i<AR5416_NUM_CTLS)
+            {
+                ctlEdgesMaxPower5GHT40 = zfGetMaxEdgePower(dev, eepromImage->ctlData[ctl_i].ctlEdges[1],
+                                                                zfAdjustHT40FreqOffset(dev, frequency, bw40, extOffset));
+            }
+            else
+            {
+                /* workaround for no data in Eeprom, replace by normal 5G */
+                ctlEdgesMaxPower5GHT40 = ctlEdgesMaxPower5G;
+            }
+            #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+            zm_dbg(("CTL_5GHT40 ctl_i = %d\n", ctl_i));
+            #endif
+
+            /* 7a17 :  */
+            /* Max power (dBm) for channel range when using DFS define by madwifi*/
+            for (i=0; i<wd->regulationTable.allowChannelCnt; i++)
+            {
+                if (wd->regulationTable.allowChannel[i].channel == frequency)
+                {
+                    if (zfHpIsDfsChannel(dev, (u16_t)frequency))
+                    {
+                        zm_debug_msg1("frequency use DFS  -- ", frequency);
+                        ctlEdgesMaxPower5G      = zm_min(ctlEdgesMaxPower5G, wd->regulationTable.allowChannel[i].maxRegTxPower*2);
+                        ctlEdgesMaxPower5GHT20  = zm_min(ctlEdgesMaxPower5GHT20, wd->regulationTable.allowChannel[i].maxRegTxPower*2);
+                        ctlEdgesMaxPower5GHT40  = zm_min(ctlEdgesMaxPower5GHT40, wd->regulationTable.allowChannel[i].maxRegTxPower*2);
+                    }
+                    break;
+                }
+            }
+
+
+            /* Apply ctl mode to correct target power set */
+            #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+            zm_debug_msg1("ctlEdgesMaxPower5G     = ", ctlEdgesMaxPower5G);
+            zm_debug_msg1("ctlEdgesMaxPower5GHT20 = ", ctlEdgesMaxPower5GHT20);
+            zm_debug_msg1("ctlEdgesMaxPower5GHT40 = ", ctlEdgesMaxPower5GHT40);
+            #endif
+            for (i=0; i<4; i++)
+            {
+                hpPriv->tPow2x5g[i] = zm_min(hpPriv->tPow2x5g[i], ctlEdgesMaxPower5G) + HALTX_POWER_OFFSET;
+            }
+            for (i=0; i<8; i++)
+            {
+                hpPriv->tPow2x5gHt20[i] = zm_min(hpPriv->tPow2x5gHt20[i], ctlEdgesMaxPower5GHT20) + HALTX_POWER_OFFSET;
+            }
+            for (i=0; i<8; i++)
+            {
+                hpPriv->tPow2x5gHt40[i] = zm_min(hpPriv->tPow2x5gHt40[i], ctlEdgesMaxPower5GHT40) + HALTX_POWER_OFFSET;
+            }
+
+        }/* end of bandedges of 5G */
+    }/* end of  if ((desired_CtlIndex = zfHpGetRegulatoryDomain(dev)) == 0) */
+
+    /* workaround */
+    /* 5. BB heavy clip */
+    /*    only 2.4G do heavy clip */
+    if (hpPriv->enableBBHeavyClip && hpPriv->hwBBHeavyClip && (frequency <= ZM_CH_G_14))
+    {
+        if (frequency <= ZM_CH_G_14)
+        {
+            ctl_i = zfFindCtlEdgesIndex(dev, desired_CtlIndex|CTL_11G);
+        }
+        else
+        {
+            ctl_i = zfFindCtlEdgesIndex(dev, desired_CtlIndex|CTL_11A);
+        }
+
+        hpPriv->setValueHeavyClip = zfHpCheckDoHeavyClip(dev, frequency, eepromImage->ctlData[ctl_i].ctlEdges[1], bw40);
+
+        if (hpPriv->setValueHeavyClip)
+        {
+            hpPriv->doBBHeavyClip = 1;
+        }
+        else
+        {
+            hpPriv->doBBHeavyClip = 0;
+        }
+        #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+        zm_dbg(("zfHpCheckDoHeavyClip ret = %02x, doBBHeavyClip = %d\n",
+                 hpPriv->setValueHeavyClip, hpPriv->doBBHeavyClip));
+        #endif
+
+        if (hpPriv->doBBHeavyClip)
+        {
+            if (hpPriv->setValueHeavyClip & 0xf0)
+            {
+                hpPriv->tPow2x2gHt40[0] -= 1;
+                hpPriv->tPow2x2gHt40[1] -= 1;
+                hpPriv->tPow2x2gHt40[2] -= 1;
+            }
+
+            if (hpPriv->setValueHeavyClip & 0xf)
+            {
+                hpPriv->tPow2x2gHt20[0] += 1;
+                hpPriv->tPow2x2gHt20[1] += 1;
+                hpPriv->tPow2x2gHt20[2] += 1;
+            }
+        }
+    }
+    else
+    {
+        hpPriv->doBBHeavyClip = 0;
+        hpPriv->setValueHeavyClip = 0;
+    }
+
+    /* Final : write MAC register for some ctrl frame Tx power */
+    /* first part : 2.4G */
+    if (frequency <= ZM_CH_G_14)
+    {
+        /* Write MAC reg 0x694 for ACK's TPC */
+        /* Write MAC reg 0xbb4 RTS and SF-CTS frame power control */
+        /* Always use two stream for low legacy rate */
+        #if 0
+        //if (hpPriv->halCapability & ZM_HP_CAP_11N_ONE_TX_STREAM)
+        //{
+            zfDelayWriteInternalReg(dev, 0x1c3694, ((hpPriv->tPow2x2g[0]&0x3f) << 20) | (0x1<<26));
+            zfDelayWriteInternalReg(dev, 0x1c3bb4, ((hpPriv->tPow2x2g[0]&0x3f) << 5 ) | (0x1<<11) |
+                                                   ((hpPriv->tPow2x2g[0]&0x3f) << 21) | (0x1<<27)  );
+        //}
+        #endif
+        #if 1
+        //else
+        {
+            #ifndef ZM_OTUS_LINUX_PHASE_2
+            zfDelayWriteInternalReg(dev, 0x1c3694, ((hpPriv->tPow2x2g[0]&0x3f) << 20) | (0x5<<26));
+            zfDelayWriteInternalReg(dev, 0x1c3bb4, ((hpPriv->tPow2x2g[0]&0x3f) << 5 ) | (0x5<<11) |
+                                                   ((hpPriv->tPow2x2g[0]&0x3f) << 21) | (0x5<<27)  );
+            #endif
+            hpPriv->currentAckRtsTpc = hpPriv->tPow2x2g[0];
+    	}
+        #endif
+        zfFlushDelayWrite(dev);
+
+        zfPrintTargetPower2G(hpPriv->tPow2xCck,
+                hpPriv->tPow2x2g,
+                hpPriv->tPow2x2gHt20,
+                hpPriv->tPow2x2gHt40);
+    }
+    else
+    {
+        /* Write MAC reg 0x694 for ACK's TPC */
+        /* Write MAC reg 0xbb4 RTS and SF-CTS frame power control */
+        /* Always use two stream for low legacy rate */
+        if (hpPriv->halCapability & ZM_HP_CAP_11N_ONE_TX_STREAM)
+        {
+            #ifndef ZM_OTUS_LINUX_PHASE_2
+            zfDelayWriteInternalReg(dev, 0x1c3694, ((hpPriv->tPow2x5g[0]&0x3f) << 20) | (0x1<<26));
+            zfDelayWriteInternalReg(dev, 0x1c3bb4, ((hpPriv->tPow2x5g[0]&0x3f) << 5 ) | (0x1<<11) |
+                                                   ((hpPriv->tPow2x5g[0]&0x3f) << 21) | (0x1<<27)  );
+            #endif
+        }
+        else
+        {
+            #ifndef ZM_OTUS_LINUX_PHASE_2
+            zfDelayWriteInternalReg(dev, 0x1c3694, ((hpPriv->tPow2x5g[0]&0x3f) << 20) | (0x5<<26));
+            zfDelayWriteInternalReg(dev, 0x1c3bb4, ((hpPriv->tPow2x5g[0]&0x3f) << 5 ) | (0x5<<11) |
+                                                   ((hpPriv->tPow2x5g[0]&0x3f) << 21) | (0x5<<27)  );
+            #endif
+            hpPriv->currentAckRtsTpc = hpPriv->tPow2x2g[0];
+        }
+
+
+        zfFlushDelayWrite(dev);
+
+        zfPrintTargetPower5G(
+                hpPriv->tPow2x5g,
+                hpPriv->tPow2x5gHt20,
+                hpPriv->tPow2x5gHt40);
+    }/* end of bandedges of 5G */
+
+}
+
+void zfDumpEepBandEdges(struct ar5416Eeprom* eepromImage)
+{
+    #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+    u8_t i, j, k;
+
+#if 0
+    zm_dbg(("\n === BandEdges index dump ==== \n"));
+
+    for (i = 0; i < AR5416_NUM_CTLS; i++)
+    {
+        zm_dbg(("%02x ", eepromImage->ctlIndex[i]));
+    }
+
+    zm_dbg(("\n === BandEdges data dump ==== \n"));
+
+    for (i = 0; i < AR5416_NUM_CTLS; i++)
+    {
+        for (j = 0; j < 2; j++)
+        {
+            for(k = 0; k < AR5416_NUM_BAND_EDGES; k++)
+            {
+                u8_t *pdata = (u8_t*)&(eepromImage->ctlData[i].ctlEdges[j][k]);
+                zm_dbg(("(%02x %02x)", pdata[0], pdata[1]));
+            }
+            zm_dbg(("\n"));
+        }
+    }
+#else
+    zm_dbg(("\n === BandEdges index dump ==== \n"));
+    for (i = 0; i < 24; i+=8)
+    {
+        zm_dbg(("%02x %02x %02x %02x %02x %02x %02x %02x",
+               eepromImage->ctlIndex[i+0], eepromImage->ctlIndex[i+1], eepromImage->ctlIndex[i+2], eepromImage->ctlIndex[i+3],
+               eepromImage->ctlIndex[i+4], eepromImage->ctlIndex[i+5], eepromImage->ctlIndex[i+6], eepromImage->ctlIndex[i+7]
+               ));
+    }
+
+    zm_dbg(("\n === BandEdges data dump ==== \n"));
+
+    for (i = 0; i < AR5416_NUM_CTLS; i++)
+    {
+        for (j = 0; j < 2; j++)
+        {
+            u8_t *pdata = (u8_t*)&(eepromImage->ctlData[i].ctlEdges[j]);
+            zm_dbg(("(%03d %02x) (%03d %02x) (%03d %02x) (%03d %02x) \n",
+                   pdata[0], pdata[1], pdata[2], pdata[3],
+                   pdata[4], pdata[5], pdata[6], pdata[7]
+                   ));
+            zm_dbg(("(%03d %02x) (%03d %02x) (%03d %02x) (%03d %02x) \n",
+                   pdata[8], pdata[9], pdata[10], pdata[11],
+                   pdata[12], pdata[13], pdata[14], pdata[15]
+                   ));
+        }
+    }
+#endif
+    #endif
+}
+
+void zfPrintTargetPower2G(u8_t* tPow2xCck, u8_t* tPow2x2g, u8_t* tPow2x2gHt20, u8_t* tPow2x2gHt40)
+{
+    //#ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+    #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+    DbgPrint("targetPwr CCK : %d, %d, %d, %d\n",
+            tPow2xCck[0],
+            tPow2xCck[1],
+            tPow2xCck[2],
+            tPow2xCck[3]
+            );
+    DbgPrint("targetPwr 2G : %d, %d, %d, %d\n",
+            tPow2x2g[0],
+            tPow2x2g[1],
+            tPow2x2g[2],
+            tPow2x2g[3]
+            );
+    DbgPrint("targetPwr 2GHT20 : %d, %d, %d, %d, %d, %d, %d, %d\n",
+            tPow2x2gHt20[0],
+            tPow2x2gHt20[1],
+            tPow2x2gHt20[2],
+            tPow2x2gHt20[3],
+            tPow2x2gHt20[4],
+            tPow2x2gHt20[5],
+            tPow2x2gHt20[6],
+            tPow2x2gHt20[7]
+            );
+    DbgPrint("targetPwr 2GHT40 : %d, %d, %d, %d, %d, %d, %d, %d\n",
+            tPow2x2gHt40[0],
+            tPow2x2gHt40[1],
+            tPow2x2gHt40[2],
+            tPow2x2gHt40[3],
+            tPow2x2gHt40[4],
+            tPow2x2gHt40[5],
+            tPow2x2gHt40[6],
+            tPow2x2gHt40[7]
+            );
+    #endif
+    return;
+}
+
+void zfPrintTargetPower5G(u8_t* tPow2x5g, u8_t* tPow2x5gHt20, u8_t* tPow2x5gHt40)
+{
+    //#ifdef ZM_ENABLE_TPC_WINDOWS_DEBUG
+    #ifdef ZM_ENABLE_BANDEDGES_WINDOWS_DEBUG
+    DbgPrint("targetPwr 5G : %d, %d, %d, %d\n",
+            tPow2x5g[0],
+            tPow2x5g[1],
+            tPow2x5g[2],
+            tPow2x5g[3]
+            );
+    DbgPrint("targetPwr 5GHT20 : %d, %d, %d, %d, %d, %d, %d, %d\n",
+            tPow2x5gHt20[0],
+            tPow2x5gHt20[1],
+            tPow2x5gHt20[2],
+            tPow2x5gHt20[3],
+            tPow2x5gHt20[4],
+            tPow2x5gHt20[5],
+            tPow2x5gHt20[6],
+            tPow2x5gHt20[7]
+            );
+    DbgPrint("targetPwr 5GHT40 : %d, %d, %d, %d, %d, %d, %d, %d\n",
+            tPow2x5gHt40[0],
+            tPow2x5gHt40[1],
+            tPow2x5gHt40[2],
+            tPow2x5gHt40[3],
+            tPow2x5gHt40[4],
+            tPow2x5gHt40[5],
+            tPow2x5gHt40[6],
+            tPow2x5gHt40[7]
+            );
+    #endif
+    return;
+}
+
+void zfHpPowerSaveSetMode(zdev_t* dev, u8_t staMode, u8_t psMode, u16_t bcnInterval)
+{
+    if ( staMode == 0 )
+    {
+        if ( psMode == 0 )
+        {
+            // Turn off pre-TBTT interrupt
+            zfDelayWriteInternalReg(dev, ZM_MAC_REG_PRETBTT, 0);
+            zfDelayWriteInternalReg(dev, ZM_MAC_REG_BCN_PERIOD, 0);
+            zfFlushDelayWrite(dev);
+        }
+        else
+        {
+            // Turn on pre-TBTT interrupt
+            zfDelayWriteInternalReg(dev, ZM_MAC_REG_PRETBTT, (bcnInterval-6)<<16);
+            zfDelayWriteInternalReg(dev, ZM_MAC_REG_BCN_PERIOD, bcnInterval);
+            zfFlushDelayWrite(dev);
+        }
+    }
+}
+
+void zfHpPowerSaveSetState(zdev_t* dev, u8_t psState)
+{
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv = wd->hpPrivate;
+
+	//DbgPrint("INTO zfHpPowerSaveSetState");
+
+    if ( psState == 0 ) //power up
+    {
+        //DbgPrint("zfHpPowerSaveSetState Wake up from PS\n");
+        reg_write(0x982C, 0x0000a000); //wake up ADDAC
+        reg_write(0x9808, 0x0);        //enable all agc gain and offset updates to a2
+        //# bank 3
+        if (((struct zsHpPriv*)wd->hpPrivate)->hwFrequency <= ZM_CH_G_14)
+        {
+            /* 11g */
+            //reg_write (0x98f0,  0x01c00018);
+            reg_write (0x98f0,  0x01c20098);//syn_on+RX_ON
+        }
+        else
+        {
+            /* 11a */
+            //reg_write (0x98f0,  0x01400018);
+            reg_write (0x98f0,  0x01420098);//syn_on+RX_ON
+        }
+
+        ////#bank 5
+        //reg_write(0x98b0,  0x00000013);
+        //reg_write(0x98e4,  0x00000002);
+
+
+        zfFlushDelayWrite(dev);
+    }
+    else //power down
+    {
+        //DbgPrint("zfHpPowerSaveSetState Go to PS\n");
+        //reg_write(0x982C, 0xa000a000);
+        reg_write(0x9808, 0x8000000);    //disable all agc gain and offset updates to a2
+        reg_write(0x982C, 0xa000a000);   //power down ADDAC
+        //# bank 3
+        if (((struct zsHpPriv*)wd->hpPrivate)->hwFrequency <= ZM_CH_G_14)
+        {
+            /* 11g */
+            reg_write (0x98f0,  0x00c00018);//syn_off+RX_off
+        }
+        else
+        {
+            /* 11a */
+            reg_write (0x98f0,  0x00400018);//syn_off+RX_off
+        }
+
+        ////#bank 5
+        //reg_write(0x98b0,  0x000e0013);
+        //reg_write(0x98e4,  0x00018002);
+
+
+        zfFlushDelayWrite(dev);
+    }
+}
+
+void zfHpSetAggPktNum(zdev_t* dev, u32_t num)
+{
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv = wd->hpPrivate;
+
+    num = (num << 16) | (0xa);
+
+    hpPriv->aggPktNum = num;
+
+    //aggregation number will be update in HAL heart beat
+    //zfDelayWriteInternalReg(dev, 0x1c3b9c, num);
+    //zfFlushDelayWrite(dev);
+}
+
+void zfHpSetMPDUDensity(zdev_t* dev, u8_t density)
+{
+    u32_t value;
+
+    if (density > ZM_MPDU_DENSITY_8US)
+    {
+        return;
+    }
+
+    /* Default value in this register */
+    value = 0x140A00 | density;
+
+    zfDelayWriteInternalReg(dev, 0x1c3ba0, value);
+    zfFlushDelayWrite(dev);
+    return;
+}
+
+void zfHpSetSlotTime(zdev_t* dev, u8_t type)
+{
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv = wd->hpPrivate;
+
+    if (type == 0)
+    {
+        //normal slot = 20us
+        hpPriv->slotType = 0;
+    }
+    else //if (type == 1)
+    {
+        //short slot = 9us
+        hpPriv->slotType = 1;
+    }
+
+    return;
+}
+
+void zfHpSetSlotTimeRegister(zdev_t* dev, u8_t type)
+{
+    if(type == 0)
+    {
+        //normal slot = 20us
+        zfDelayWriteInternalReg(dev, ZM_MAC_REG_SLOT_TIME, 20<<10);
+    }
+    else
+    {
+        //short slot = 9us
+        zfDelayWriteInternalReg(dev, ZM_MAC_REG_SLOT_TIME, 9<<10);
+    }
+}
+
+void zfHpSetRifs(zdev_t* dev, u8_t ht_enable, u8_t ht2040, u8_t g_mode)
+{
+    zfDelayWriteInternalReg(dev, 0x1c6388, 0x0c000000);
+
+    zfDelayWriteInternalReg(dev, 0x1c59ec, 0x0cc80caa);
+
+    if (ht_enable)
+    {
+        if (ht2040)
+        {
+            zfDelayWriteInternalReg(dev, 0x1c5918, 40);
+        }
+        else
+        {
+            zfDelayWriteInternalReg(dev, 0x1c5918, 20);
+        }
+    }
+
+    if (g_mode)
+    {
+        zfDelayWriteInternalReg(dev, 0x1c5850, 0xec08b4e2);
+        zfDelayWriteInternalReg(dev, 0x1c585c, 0x313a5d5e);
+    }
+    else
+    {
+        zfDelayWriteInternalReg(dev, 0x1c5850, 0xede8b4e0);
+        zfDelayWriteInternalReg(dev, 0x1c585c, 0x3139605e);
+    }
+
+    zfFlushDelayWrite(dev);
+    return;
+}
+
+void zfHpBeginSiteSurvey(zdev_t* dev, u8_t status)
+{
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+    if ( status == 1 )
+    { // Connected
+        hpPriv->isSiteSurvey = 1;
+    }
+    else
+    { // Not connected
+        hpPriv->isSiteSurvey = 0;
+    }
+
+    /* reset workaround state to default */
+//    if (hpPriv->rxStrongRSSI == 1)
+    {
+        hpPriv->rxStrongRSSI = 0;
+        if ((hpPriv->eepromImage[0x100+0x110*2/4]&0xff) == 0x80) //FEM TYPE
+        {
+            if (hpPriv->hwFrequency <= ZM_CH_G_14)
+            {
+                zfDelayWriteInternalReg(dev, 0x1c8960, 0x9b49);
+            }
+            else
+            {
+                zfDelayWriteInternalReg(dev, 0x1c8960, 0x0900);
+            }
+        }
+        else
+        {
+            zfDelayWriteInternalReg(dev, 0x1c8960, 0x9b40);
+        }
+        zfFlushDelayWrite(dev);
+    }
+//    if (hpPriv->strongRSSI == 1)
+    {
+        hpPriv->strongRSSI = 0;
+        zfDelayWriteInternalReg(dev, 0x1c3694, ((hpPriv->currentAckRtsTpc&0x3f) << 20) | (0x5<<26));
+        zfDelayWriteInternalReg(dev, 0x1c3bb4, ((hpPriv->currentAckRtsTpc&0x3f) << 5 ) | (0x5<<11) |
+                                               ((hpPriv->currentAckRtsTpc&0x3f) << 21) | (0x5<<27)  );
+        zfFlushDelayWrite(dev);
+    }
+}
+
+void zfHpFinishSiteSurvey(zdev_t* dev, u8_t status)
+{
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    if ( status == 1 )
+    {
+        hpPriv->isSiteSurvey = 2;
+    }
+    else
+    {
+        hpPriv->isSiteSurvey = 0;
+    }
+    zmw_leave_critical_section(dev);
+}
+
+u16_t zfFwRetry(zdev_t* dev, u8_t enable)
+{
+    u32_t cmd[(ZM_MAX_CMD_SIZE/4)];
+    u16_t ret = 0;
+
+    cmd[0] = 4 | (0x92 << 8);
+    cmd[1] = (enable == 1) ? 0x01 : 0x00;
+
+    ret = zfIssueCmd(dev, cmd, 8, ZM_OID_INTERNAL_WRITE, NULL);
+    return ret;
+}
+
+u16_t zfHpEnableHwRetry(zdev_t* dev)
+{
+    u16_t ret;
+
+    ret = zfFwRetry(dev, 0);
+
+    zfDelayWriteInternalReg(dev, 0x1c3b28, 0x33333);
+    zfFlushDelayWrite(dev);
+
+    return ret;
+}
+
+u16_t zfHpDisableHwRetry(zdev_t* dev)
+{
+    u16_t ret;
+
+    ret = zfFwRetry(dev, 1);
+
+    zfDelayWriteInternalReg(dev, 0x1c3b28, 0x00000);
+    zfFlushDelayWrite(dev);
+
+    return ret;
+}
+
+/* Download SPI Fw */
+#define ZM_FIRMWARE_WLAN                0
+#define ZM_FIRMWARE_SPI_FLASH           1
+
+
+u16_t zfHpFirmwareDownload(zdev_t* dev, u8_t fwType)
+{
+    u16_t ret = ZM_SUCCESS;
+
+    if (fwType == ZM_FIRMWARE_WLAN)
+    {
+        ret = zfFirmwareDownload(dev, (u32_t*)zcFwImage,
+                (u32_t)zcFwImageSize, ZM_FIRMWARE_WLAN_ADDR);
+    }
+    else if (fwType == ZM_FIRMWARE_SPI_FLASH)
+    {
+        ret = zfFirmwareDownload(dev, (u32_t*)zcFwImageSPI,
+                (u32_t)zcFwImageSPISize, ZM_FIRMWARE_SPI_ADDR);
+    }
+    else
+    {
+        zm_debug_msg1("Unknown firmware type = ", fwType);
+        ret = ZM_ERR_FIRMWARE_WRONG_TYPE;
+    }
+
+    return ret;
+}
+
+/* Enable software decryption */
+void zfHpSWDecrypt(zdev_t* dev, u8_t enable)
+{
+    u32_t value = 0x70;
+
+    /* Bit 4 for enable software decryption */
+    if (enable == 1)
+    {
+        value = 0x78;
+    }
+
+    zfDelayWriteInternalReg(dev, 0x1c3678, value);
+    zfFlushDelayWrite(dev);
+}
+
+/* Enable software encryption */
+void zfHpSWEncrypt(zdev_t* dev, u8_t enable)
+{
+    /* Because encryption by software or hardware is judged by driver in Otus,
+       we don't need to do anything in the HAL layer.
+     */
+}
+
+u32_t zfHpCapability(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+    return hpPriv->halCapability;
+}
+
+void zfHpSetRollCallTable(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+    if (hpPriv->camRollCallTable != (u64_t) 0)
+    {
+        zfDelayWriteInternalReg(dev, ZM_MAC_REG_ROLL_CALL_TBL_L, (u32_t)(hpPriv->camRollCallTable & 0xffffffff));
+        zfDelayWriteInternalReg(dev, ZM_MAC_REG_ROLL_CALL_TBL_H, (u32_t)((hpPriv->camRollCallTable >> 32) & 0xffffffff));
+        zfFlushDelayWrite(dev);
+    }
+}
+
+void zfHpSetTTSIFSTime(zdev_t* dev, u8_t sifs_time)
+{
+    u32_t reg_value = 0;
+    zmw_get_wlan_dev(dev);
+
+    sifs_time &= 0x3f;
+    reg_value = 0x14400b | (((u32_t)sifs_time)<<24);
+
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_EIFS_AND_SIFS, reg_value);
+    zfFlushDelayWrite(dev);
+}
+
+/* #3 Enable RIFS function if the RIFS pattern matched ! */
+void zfHpEnableRifs(zdev_t* dev, u8_t mode24g, u8_t modeHt, u8_t modeHt2040)
+{
+
+    /* # Enable Reset TDOMAIN
+     * $rddata = &$phyreg_read(0x9800+(738<<2));
+     * $wrdata = $rddata | (0x1 << 26) | (0x1 << 27);
+     * &$phyreg_write(0x9800+(738<<2), $wrdata);
+     */
+    reg_write (0x9800+(738<<2), 0x08000000 | (0x1 << 26) | (0x1 << 27));
+    //reg_write (0x9800+(738<<2), 0x08000000 | (0x1 << 26));
+
+    /* # reg 123: heavy clip factor, xr / RIFS search parameters */
+    reg_write (0x99ec, 0x0cc80caa);
+
+    /* # Reduce Search Start Delay for RIFS    */
+    if (modeHt == 1) /* ($HT_ENABLE == 1) */
+    {
+        if (modeHt2040 == 0x1) /* ($DYNAMIC_HT2040_EN == 0x1) */
+        {
+            reg_write(0x9800+(70<<2), 40);/*40*/
+        }
+        else
+        {
+            reg_write(0x9800+(70<<2), 20);
+            if(mode24g == 0x0)
+            {
+                /* $rddata = &$phyreg_read(0x9800+(24<<2));#0x9860;0x1c5860
+                 *$wrdata = ($rddata & 0xffffffc7) | (0x4 << 3);
+                 * &$phyreg_write(0x9800+(24<<2), $wrdata);
+                 */
+                reg_write(0x9800+(24<<2), (0x0004dd10 & 0xffffffc7) | (0x4 << 3));
+            }
+        }
+    }
+
+    if (mode24g == 0x1)
+    {
+        reg_write(0x9850, 0xece8b4e4);/*org*/
+        //reg_write(0x9850, 0xece8b4e2);
+        reg_write(0x985c, 0x313a5d5e);
+    }
+    else
+    {
+        reg_write(0x9850, 0xede8b4e4);
+        reg_write(0x985c, 0x3139605e);
+    }
+
+    zfFlushDelayWrite(dev);
+
+    return;
+}
+
+/* #4 Disable RIFS function if the RIFS timer is timeout ! */
+void zfHpDisableRifs(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    /* Disable RIFS function is to store these HW register initial value while the device plug-in and
+       re-write to these register if the RIFS function is disabled  */
+
+    // reg : 9850
+    reg_write(0x9850, ((struct zsHpPriv*)wd->hpPrivate)->initDesiredSigSize);
+
+    // reg : 985c
+    reg_write(0x985c, ((struct zsHpPriv*)wd->hpPrivate)->initAGC);
+
+    // reg : 9860
+    reg_write(0x9800+(24<<2), ((struct zsHpPriv*)wd->hpPrivate)->initAgcControl);
+
+    // reg : 9918
+    reg_write(0x9800+(70<<2), ((struct zsHpPriv*)wd->hpPrivate)->initSearchStartDelay);
+
+    // reg : 991c
+    reg_write (0x99ec, ((struct zsHpPriv*)wd->hpPrivate)->initRIFSSearchParams);
+
+    // reg : a388
+    reg_write (0x9800+(738<<2), ((struct zsHpPriv*)wd->hpPrivate)->initFastChannelChangeControl);
+
+    zfFlushDelayWrite(dev);
+
+    return;
+}
diff --git a/drivers/staging/otus/hal/hpreg.c b/drivers/staging/otus/hal/hpreg.c
new file mode 100644
index 00000000000..3cfeba8620f
--- /dev/null
+++ b/drivers/staging/otus/hal/hpreg.c
@@ -0,0 +1,2481 @@
+/*
+ * Copyright (c) 2000-2005 ZyDAS Technology Corporation
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : hpreg.c                                               */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains Regulatory Table and related function.     */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+#include "../80211core/cprecomp.h"
+#include "hpani.h"
+#include "hpreg.h"
+#include "hpusb.h"
+
+/* used throughout this file... */
+#define	N(a)	(sizeof (a) / sizeof (a[0]))
+
+#define HAL_MODE_11A_TURBO	HAL_MODE_108A
+#define HAL_MODE_11G_TURBO	HAL_MODE_108G
+
+#if 0
+enum {
+    /* test groups */
+	FCC	       = 0x10,
+	MKK	       = 0x40,
+	ETSI	   = 0x30,
+    SD_NO_CTL  = 0xe0,
+	NO_CTL	   = 0xff,
+    /* test modes */
+    CTL_MODE_M = 0x0f,
+    CTL_11A    = 0,
+	CTL_11B	   = 1,
+	CTL_11G	   = 2,
+	CTL_TURBO  = 3,
+	CTL_108G   = 4,
+    CTL_2GHT20 = 5,
+    CTL_5GHT20 = 6,
+    CTL_2GHT40 = 7,
+    CTL_5GHT40 = 8
+};
+#endif
+
+/*
+ * The following are flags for different requirements per reg domain.
+ * These requirements are either inhereted from the reg domain pair or
+ * from the unitary reg domain if the reg domain pair flags value is
+ * 0
+ */
+
+enum {
+	NO_REQ			= 0x00000000,
+	DISALLOW_ADHOC_11A	= 0x00000001,
+	DISALLOW_ADHOC_11A_TURB	= 0x00000002,
+	NEED_NFC		= 0x00000004,
+
+	ADHOC_PER_11D		= 0x00000008,  /* Start Ad-Hoc mode */
+	ADHOC_NO_11A		= 0x00000010,
+
+	PUBLIC_SAFETY_DOMAIN	= 0x00000020, 	/* public safety domain */
+	LIMIT_FRAME_4MS 	= 0x00000040, 	/* 4msec limit on the frame length */
+};
+
+#define MKK5GHZ_FLAG1 (DISALLOW_ADHOC_11A_TURB | NEED_NFC | LIMIT_FRAME_4MS)
+#define MKK5GHZ_FLAG2 (DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB | NEED_NFC| LIMIT_FRAME_4MS)
+
+typedef enum {
+	DFS_UNINIT_DOMAIN	= 0,	/* Uninitialized dfs domain */
+	DFS_FCC_DOMAIN		= 1,	/* FCC3 dfs domain */
+	DFS_ETSI_DOMAIN		= 2,	/* ETSI dfs domain */
+} HAL_DFS_DOMAIN;
+
+/*
+ * Used to set the RegDomain bitmask which chooses which frequency
+ * band specs are used.
+ */
+
+#define BMLEN 2		/* Use 2 64 bit uint for channel bitmask
+			   NB: Must agree with macro below (BM) */
+#define BMZERO {(u64_t) 0, (u64_t) 0}	/* BMLEN zeros */
+
+#if 0
+
+#define BM(_fa, _fb, _fc, _fd, _fe, _ff, _fg, _fh, _fi, _fj, _fk, _fl) \
+      {((((_fa >= 0) && (_fa < 64)) ? (((u64_t) 1) << _fa) : (u64_t) 0) | \
+	(((_fb >= 0) && (_fb < 64)) ? (((u64_t) 1) << _fb) : (u64_t) 0) | \
+	(((_fc >= 0) && (_fc < 64)) ? (((u64_t) 1) << _fc) : (u64_t) 0) | \
+	(((_fd >= 0) && (_fd < 64)) ? (((u64_t) 1) << _fd) : (u64_t) 0) | \
+	(((_fe >= 0) && (_fe < 64)) ? (((u64_t) 1) << _fe) : (u64_t) 0) | \
+	(((_ff >= 0) && (_ff < 64)) ? (((u64_t) 1) << _ff) : (u64_t) 0) | \
+	(((_fg >= 0) && (_fg < 64)) ? (((u64_t) 1) << _fg) : (u64_t) 0) | \
+	(((_fh >= 0) && (_fh < 64)) ? (((u64_t) 1) << _fh) : (u64_t) 0) | \
+	(((_fi >= 0) && (_fi < 64)) ? (((u64_t) 1) << _fi) : (u64_t) 0) | \
+	(((_fj >= 0) && (_fj < 64)) ? (((u64_t) 1) << _fj) : (u64_t) 0) | \
+	(((_fk >= 0) && (_fk < 64)) ? (((u64_t) 1) << _fk) : (u64_t) 0) | \
+	(((_fl >= 0) && (_fl < 64)) ? (((u64_t) 1) << _fl) : (u64_t) 0) | \
+	       ((((_fa > 63) && (_fa < 128)) ? (((u64_t) 1) << (_fa - 64)) : (u64_t) 0) | \
+		(((_fb > 63) && (_fb < 128)) ? (((u64_t) 1) << (_fb - 64)) : (u64_t) 0) | \
+		(((_fc > 63) && (_fc < 128)) ? (((u64_t) 1) << (_fc - 64)) : (u64_t) 0) | \
+		(((_fd > 63) && (_fd < 128)) ? (((u64_t) 1) << (_fd - 64)) : (u64_t) 0) | \
+		(((_fe > 63) && (_fe < 128)) ? (((u64_t) 1) << (_fe - 64)) : (u64_t) 0) | \
+		(((_ff > 63) && (_ff < 128)) ? (((u64_t) 1) << (_ff - 64)) : (u64_t) 0) | \
+		(((_fg > 63) && (_fg < 128)) ? (((u64_t) 1) << (_fg - 64)) : (u64_t) 0) | \
+		(((_fh > 63) && (_fh < 128)) ? (((u64_t) 1) << (_fh - 64)) : (u64_t) 0) | \
+		(((_fi > 63) && (_fi < 128)) ? (((u64_t) 1) << (_fi - 64)) : (u64_t) 0) | \
+		(((_fj > 63) && (_fj < 128)) ? (((u64_t) 1) << (_fj - 64)) : (u64_t) 0) | \
+		(((_fk > 63) && (_fk < 128)) ? (((u64_t) 1) << (_fk - 64)) : (u64_t) 0) | \
+		(((_fl > 63) && (_fl < 128)) ? (((u64_t) 1) << (_fl - 64)) : (u64_t) 0)))}
+
+#else
+
+#define BM(_fa, _fb, _fc, _fd, _fe, _ff, _fg, _fh, _fi, _fj, _fk, _fl) \
+      {((((_fa >= 0) && (_fa < 64)) ? (((u64_t) 1) << (_fa&0x3f)) : (u64_t) 0) | \
+	(((_fb >= 0) && (_fb < 64)) ? (((u64_t) 1) << (_fb&0x3f)) : (u64_t) 0) | \
+	(((_fc >= 0) && (_fc < 64)) ? (((u64_t) 1) << (_fc&0x3f)) : (u64_t) 0) | \
+	(((_fd >= 0) && (_fd < 64)) ? (((u64_t) 1) << (_fd&0x3f)) : (u64_t) 0) | \
+	(((_fe >= 0) && (_fe < 64)) ? (((u64_t) 1) << (_fe&0x3f)) : (u64_t) 0) | \
+	(((_ff >= 0) && (_ff < 64)) ? (((u64_t) 1) << (_ff&0x3f)) : (u64_t) 0) | \
+	(((_fg >= 0) && (_fg < 64)) ? (((u64_t) 1) << (_fg&0x3f)) : (u64_t) 0) | \
+	(((_fh >= 0) && (_fh < 64)) ? (((u64_t) 1) << (_fh&0x3f)) : (u64_t) 0) | \
+	(((_fi >= 0) && (_fi < 64)) ? (((u64_t) 1) << (_fi&0x3f)) : (u64_t) 0) | \
+	(((_fj >= 0) && (_fj < 64)) ? (((u64_t) 1) << (_fj&0x3f)) : (u64_t) 0) | \
+	(((_fk >= 0) && (_fk < 64)) ? (((u64_t) 1) << (_fk&0x3f)) : (u64_t) 0) | \
+	(((_fl >= 0) && (_fl < 64)) ? (((u64_t) 1) << (_fl&0x3f)) : (u64_t) 0) | \
+	       ((((_fa > 63) && (_fa < 128)) ? (((u64_t) 1) << ((_fa - 64)&0x3f)) : (u64_t) 0) | \
+		(((_fb > 63) && (_fb < 128)) ? (((u64_t) 1) << ((_fb - 64)&0x3f)) : (u64_t) 0) | \
+		(((_fc > 63) && (_fc < 128)) ? (((u64_t) 1) << ((_fc - 64)&0x3f)) : (u64_t) 0) | \
+		(((_fd > 63) && (_fd < 128)) ? (((u64_t) 1) << ((_fd - 64)&0x3f)) : (u64_t) 0) | \
+		(((_fe > 63) && (_fe < 128)) ? (((u64_t) 1) << ((_fe - 64)&0x3f)) : (u64_t) 0) | \
+		(((_ff > 63) && (_ff < 128)) ? (((u64_t) 1) << ((_ff - 64)&0x3f)) : (u64_t) 0) | \
+		(((_fg > 63) && (_fg < 128)) ? (((u64_t) 1) << ((_fg - 64)&0x3f)) : (u64_t) 0) | \
+		(((_fh > 63) && (_fh < 128)) ? (((u64_t) 1) << ((_fh - 64)&0x3f)) : (u64_t) 0) | \
+		(((_fi > 63) && (_fi < 128)) ? (((u64_t) 1) << ((_fi - 64)&0x3f)) : (u64_t) 0) | \
+		(((_fj > 63) && (_fj < 128)) ? (((u64_t) 1) << ((_fj - 64)&0x3f)) : (u64_t) 0) | \
+		(((_fk > 63) && (_fk < 128)) ? (((u64_t) 1) << ((_fk - 64)&0x3f)) : (u64_t) 0) | \
+		(((_fl > 63) && (_fl < 128)) ? (((u64_t) 1) << ((_fl - 64)&0x3f)) : (u64_t) 0)))}
+
+#endif
+
+/* Mask to check whether a domain is a multidomain or a single
+   domain */
+
+#define MULTI_DOMAIN_MASK 0xFF00
+
+
+/*
+ * The following describe the bit masks for different passive scan
+ * capability/requirements per regdomain.
+ */
+#define	NO_PSCAN	0x0ULL
+#define	PSCAN_FCC	0x0000000000000001ULL
+#define	PSCAN_FCC_T	0x0000000000000002ULL
+#define	PSCAN_ETSI	0x0000000000000004ULL
+#define	PSCAN_MKK1	0x0000000000000008ULL
+#define	PSCAN_MKK2	0x0000000000000010ULL
+#define	PSCAN_MKKA	0x0000000000000020ULL
+#define	PSCAN_MKKA_G	0x0000000000000040ULL
+#define	PSCAN_ETSIA	0x0000000000000080ULL
+#define	PSCAN_ETSIB	0x0000000000000100ULL
+#define	PSCAN_ETSIC	0x0000000000000200ULL
+#define	PSCAN_WWR	0x0000000000000400ULL
+#define	PSCAN_MKKA1	0x0000000000000800ULL
+#define	PSCAN_MKKA1_G	0x0000000000001000ULL
+#define	PSCAN_MKKA2	0x0000000000002000ULL
+#define	PSCAN_MKKA2_G	0x0000000000004000ULL
+#define	PSCAN_MKK3	0x0000000000008000ULL
+#define	PSCAN_DEFER	0x7FFFFFFFFFFFFFFFULL
+#define	IS_ECM_CHAN	0x8000000000000000ULL
+
+/*
+ * THE following table is the mapping of regdomain pairs specified by
+ * an 8 bit regdomain value to the individual unitary reg domains
+ */
+
+typedef struct reg_dmn_pair_mapping {
+	u16_t regDmnEnum;	/* 16 bit reg domain pair */
+	u16_t regDmn5GHz;	/* 5GHz reg domain */
+	u16_t regDmn2GHz;	/* 2GHz reg domain */
+	u32_t flags5GHz;		/* Requirements flags (AdHoc
+					   disallow, noise floor cal needed,
+					   etc) */
+	u32_t flags2GHz;		/* Requirements flags (AdHoc
+					   disallow, noise floor cal needed,
+					   etc) */
+	u64_t pscanMask;		/* Passive Scan flags which
+					   can override unitary domain
+					   passive scan flags.  This
+					   value is used as a mask on
+					   the unitary flags*/
+	u16_t singleCC;		/* Country code of single country if
+					   a one-on-one mapping exists */
+}  REG_DMN_PAIR_MAPPING;
+
+static REG_DMN_PAIR_MAPPING regDomainPairs[] = {
+	{NO_ENUMRD,	    FCC2,	DEBUG_REG_DMN,  NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{NULL1_WORLD,	NULL1,		WORLD,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{NULL1_ETSIB,	NULL1,		ETSIB,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{NULL1_ETSIC,	NULL1,		ETSIC,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+
+	{FCC2_FCCA,     FCC2,		FCCA,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{FCC2_WORLD,    FCC2,		WORLD,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{FCC2_ETSIC,	FCC2,		ETSIC,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{FCC3_FCCA,     FCC3,		FCCA,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{FCC3_WORLD,    FCC3,		WORLD,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{FCC4_FCCA,     FCC4,		FCCA,		DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB, NO_REQ, PSCAN_DEFER, 0 },
+	{FCC5_FCCA,     FCC5,		FCCA,		DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB, NO_REQ, PSCAN_DEFER, 0 },
+	{FCC6_FCCA,     FCC6,		FCCA,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{FCC6_WORLD,    FCC6,		WORLD,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+
+	{ETSI1_WORLD,	ETSI1,		WORLD,		DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB, NO_REQ, PSCAN_DEFER, 0 },
+	{ETSI2_WORLD,	ETSI2,		WORLD,		DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB, NO_REQ, PSCAN_DEFER, 0 },
+	{ETSI3_WORLD,	ETSI3,		WORLD,		DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB, NO_REQ, PSCAN_DEFER, 0 },
+	{ETSI4_WORLD,	ETSI4,		WORLD,		DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB, NO_REQ, PSCAN_DEFER, 0 },
+	{ETSI5_WORLD,	ETSI5,		WORLD,		DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB, NO_REQ, PSCAN_DEFER, 0 },
+	{ETSI6_WORLD,	ETSI6,		WORLD,		DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB, NO_REQ, PSCAN_DEFER, 0 },
+
+	{ETSI3_ETSIA,	ETSI3,		WORLD,		DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB, NO_REQ, PSCAN_DEFER, 0 },
+	{FRANCE_RES,	ETSI3,		WORLD,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+
+	{FCC1_WORLD,	FCC1,		WORLD,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{FCC1_FCCA,     FCC1,		FCCA,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{APL1_WORLD,	APL1,		WORLD,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{APL2_WORLD,	APL2,		WORLD,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{APL3_WORLD,	APL3,		WORLD,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{APL4_WORLD,	APL4,		WORLD,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{APL5_WORLD,	APL5,		WORLD,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{APL6_WORLD,	APL6,		WORLD,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{APL8_WORLD,	APL8,		WORLD,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{APL9_WORLD,	APL9,		WORLD,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+
+	{APL3_FCCA,     APL3,		FCCA,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{APL1_ETSIC,	APL1,		ETSIC,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{APL2_ETSIC,	APL2,		ETSIC,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{APL2_FCCA,		APL2,		FCCA,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{APL2_APLD,     APL2,		APLD,		NO_REQ, NO_REQ, PSCAN_DEFER, 0},
+	{APL7_FCCA,		APL7,		FCCA,		NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+
+	{MKK1_MKKA,     MKK1,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK1 | PSCAN_MKKA, CTRY_JAPAN },
+	{MKK1_MKKB,     MKK1,		MKKA,		MKK5GHZ_FLAG2, NEED_NFC, PSCAN_MKK1 | PSCAN_MKKA | PSCAN_MKKA_G, CTRY_JAPAN1 },
+	{MKK1_FCCA,     MKK1,		FCCA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK1, CTRY_JAPAN2 },
+	{MKK1_MKKA1,    MKK1,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK1 | PSCAN_MKKA1 | PSCAN_MKKA1_G, CTRY_JAPAN4 },
+	{MKK1_MKKA2,    MKK1,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK1 | PSCAN_MKKA2 | PSCAN_MKKA2_G, CTRY_JAPAN5 },
+	{MKK1_MKKC,     MKK1,		MKKC,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK1, CTRY_JAPAN6 },
+
+	/* MKK2 */
+	{MKK2_MKKA,     MKK2,		MKKA,		MKK5GHZ_FLAG2, NEED_NFC, PSCAN_MKK2 | PSCAN_MKKA | PSCAN_MKKA_G, CTRY_JAPAN3 },
+
+	/* MKK3 */
+	{MKK3_MKKA,     MKK3,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, NO_PSCAN, CTRY_JAPAN25 },
+	{MKK3_MKKB,     MKK3,		MKKA,		MKK5GHZ_FLAG2, NEED_NFC, PSCAN_MKKA | PSCAN_MKKA_G, CTRY_JAPAN7 },
+	{MKK3_MKKA1,    MKK3,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKKA1 | PSCAN_MKKA1_G, CTRY_JAPAN26 },
+	{MKK3_MKKA2,    MKK3,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKKA2 | PSCAN_MKKA2_G, CTRY_JAPAN8 },
+	{MKK3_MKKC,     MKK3,		MKKC,		MKK5GHZ_FLAG1, NEED_NFC, NO_PSCAN, CTRY_JAPAN9 },
+	{MKK3_FCCA,     MKK3,		FCCA,		MKK5GHZ_FLAG1, NEED_NFC, NO_PSCAN, CTRY_JAPAN27 },
+
+	/* MKK4 */
+	{MKK4_MKKB,     MKK4,		MKKA,		MKK5GHZ_FLAG2, NEED_NFC, PSCAN_MKK3 | PSCAN_MKKA | PSCAN_MKKA_G, CTRY_JAPAN10 },
+	{MKK4_MKKA1,    MKK4,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK3 | PSCAN_MKKA1 | PSCAN_MKKA1_G, CTRY_JAPAN28 },
+	{MKK4_MKKA2,    MKK4,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK3 |PSCAN_MKKA2 | PSCAN_MKKA2_G, CTRY_JAPAN11 },
+	{MKK4_MKKC,     MKK4,		MKKC,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK3, CTRY_JAPAN12 },
+	{MKK4_FCCA,     MKK4,		FCCA,		MKK5GHZ_FLAG1, NEED_NFC, NO_PSCAN, CTRY_JAPAN29 },
+	{MKK4_MKKA,     MKK4,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK3 | PSCAN_MKKA, CTRY_JAPAN36 },
+
+	/* MKK5 */
+	{MKK5_MKKB,     MKK5,		MKKA,		MKK5GHZ_FLAG2, NEED_NFC, PSCAN_MKK3 | PSCAN_MKKA | PSCAN_MKKA_G, CTRY_JAPAN13 },
+	{MKK5_MKKA2,    MKK5,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK3 | PSCAN_MKKA2 | PSCAN_MKKA2_G, CTRY_JAPAN14 },
+	{MKK5_MKKC,     MKK5,		MKKC,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK3, CTRY_JAPAN15 },
+
+	/* MKK6 */
+	{MKK6_MKKB,     MKK6,		MKKA,		MKK5GHZ_FLAG2, NEED_NFC, PSCAN_MKK1 | PSCAN_MKKA | PSCAN_MKKA_G, CTRY_JAPAN16 },
+	{MKK6_MKKA2,    MKK6,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK1 | PSCAN_MKKA2 | PSCAN_MKKA2_G, CTRY_JAPAN17 },
+	{MKK6_MKKC,     MKK6,		MKKC,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK1, CTRY_JAPAN18 },
+	{MKK6_MKKA1,    MKK6,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKKA1 | PSCAN_MKKA1_G, CTRY_JAPAN30 },
+	{MKK6_FCCA,     MKK6,		FCCA,		MKK5GHZ_FLAG1, NEED_NFC, NO_PSCAN, CTRY_JAPAN31 },
+
+	/* MKK7 */
+	{MKK7_MKKB,     MKK7,		MKKA,		MKK5GHZ_FLAG2, NEED_NFC, PSCAN_MKK1 | PSCAN_MKK3 | PSCAN_MKKA | PSCAN_MKKA_G, CTRY_JAPAN19 },
+	{MKK7_MKKA,     MKK7,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK1 | PSCAN_MKK3 | PSCAN_MKKA2 | PSCAN_MKKA2_G, CTRY_JAPAN20 },
+	{MKK7_MKKC,     MKK7,		MKKC,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK1 | PSCAN_MKK3, CTRY_JAPAN21 },
+	{MKK7_MKKA1,    MKK7,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKKA1 | PSCAN_MKKA1_G, CTRY_JAPAN32 },
+	{MKK7_FCCA,     MKK7,		FCCA,		MKK5GHZ_FLAG1, NEED_NFC, NO_PSCAN, CTRY_JAPAN33 },
+
+	/* MKK8 */
+	{MKK8_MKKB,     MKK8,		MKKA,		MKK5GHZ_FLAG2, NEED_NFC, PSCAN_MKK1 | PSCAN_MKK3 | PSCAN_MKKA | PSCAN_MKKA_G, CTRY_JAPAN22 },
+	{MKK8_MKKA2,    MKK8,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK1 | PSCAN_MKK3 | PSCAN_MKKA2 | PSCAN_MKKA2_G, CTRY_JAPAN23 },
+	{MKK8_MKKC,     MKK8,		MKKC,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK1 | PSCAN_MKK3 , CTRY_JAPAN24 },
+
+   	/* MKK9 */
+	{MKK9_MKKA,     MKK9,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, NO_PSCAN, CTRY_JAPAN34 },
+	{MKK9_FCCA,     MKK9,		FCCA,		MKK5GHZ_FLAG1, NEED_NFC, NO_PSCAN, CTRY_JAPAN37 },
+	{MKK9_MKKA1,    MKK9,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKKA1 | PSCAN_MKKA1_G, CTRY_JAPAN38 },
+	{MKK9_MKKC,     MKK9,		MKKC,		MKK5GHZ_FLAG1, NEED_NFC, NO_PSCAN, CTRY_JAPAN39 },
+	{MKK9_MKKA2,	MKK9,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK1 | PSCAN_MKK3 | PSCAN_MKKA2 | PSCAN_MKKA2_G, CTRY_JAPAN40 },
+
+	/* MKK10 */
+	{MKK10_MKKA,	MKK10,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, NO_PSCAN, CTRY_JAPAN35 },
+	{MKK10_FCCA,	MKK10,		FCCA,		MKK5GHZ_FLAG1, NEED_NFC, NO_PSCAN, CTRY_JAPAN41 },
+	{MKK10_MKKA1,	MKK10,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKKA1 | PSCAN_MKKA1_G, CTRY_JAPAN42 },
+	{MKK10_MKKC,	MKK10,		MKKC,		MKK5GHZ_FLAG1, NEED_NFC, NO_PSCAN, CTRY_JAPAN43 },
+	{MKK10_MKKA2,	MKK10,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK1 | PSCAN_MKK3 | PSCAN_MKKA2 | PSCAN_MKKA2_G, CTRY_JAPAN44 },
+
+	/* MKK11 */
+	{MKK11_MKKA,	MKK11,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, NO_PSCAN, CTRY_JAPAN45 },
+	{MKK11_FCCA,	MKK11,		FCCA,		MKK5GHZ_FLAG1, NEED_NFC, NO_PSCAN, CTRY_JAPAN46 },
+	{MKK11_MKKA1,	MKK11,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKKA1 | PSCAN_MKKA1_G, CTRY_JAPAN47 },
+	{MKK11_MKKC,	MKK11,		MKKC,		MKK5GHZ_FLAG1, NEED_NFC, NO_PSCAN, CTRY_JAPAN48 },
+	{MKK11_MKKA2,	MKK11,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK1 | PSCAN_MKK3 | PSCAN_MKKA2 | PSCAN_MKKA2_G, CTRY_JAPAN49 },
+
+	/* MKK12 */
+	{MKK12_MKKA,	MKK12,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, NO_PSCAN, CTRY_JAPAN50 },
+	{MKK12_FCCA,	MKK12,		FCCA,		MKK5GHZ_FLAG1, NEED_NFC, NO_PSCAN, CTRY_JAPAN51 },
+	{MKK12_MKKA1,	MKK12,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKKA1 | PSCAN_MKKA1_G, CTRY_JAPAN52 },
+	{MKK12_MKKC,	MKK12,		MKKC,		MKK5GHZ_FLAG1, NEED_NFC, NO_PSCAN, CTRY_JAPAN53 },
+	{MKK12_MKKA2,	MKK12,		MKKA,		MKK5GHZ_FLAG1, NEED_NFC, PSCAN_MKK1 | PSCAN_MKK3 | PSCAN_MKKA2 | PSCAN_MKKA2_G, CTRY_JAPAN54 },
+
+
+	/* These are super domains */
+	{WOR0_WORLD,	WOR0_WORLD,	WOR0_WORLD,	NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{WOR1_WORLD,	WOR1_WORLD,	WOR1_WORLD,	DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB, NO_REQ, PSCAN_DEFER, 0 },
+	{WOR2_WORLD,	WOR2_WORLD,	WOR2_WORLD,	DISALLOW_ADHOC_11A_TURB, NO_REQ, PSCAN_DEFER, 0 },
+	{WOR3_WORLD,	WOR3_WORLD,	WOR3_WORLD,	NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{WOR4_WORLD,	WOR4_WORLD,	WOR4_WORLD,	DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB, NO_REQ, PSCAN_DEFER, 0 },
+	{WOR5_ETSIC,	WOR5_ETSIC,	WOR5_ETSIC,	DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB, NO_REQ, PSCAN_DEFER, 0 },
+	{WOR01_WORLD,	WOR01_WORLD,	WOR01_WORLD,	NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{WOR02_WORLD,	WOR02_WORLD,	WOR02_WORLD,	NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{EU1_WORLD,	    EU1_WORLD,	EU1_WORLD,	NO_REQ, NO_REQ, PSCAN_DEFER, 0 },
+	{WOR9_WORLD,	WOR9_WORLD,	WOR9_WORLD,	DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB, NO_REQ, PSCAN_DEFER, 0 },
+	{WORA_WORLD,	WORA_WORLD,	WORA_WORLD,	DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB, NO_REQ, PSCAN_DEFER, 0 },
+};
+
+/*
+ * The following table is the master list for all different freqeuncy
+ * bands with the complete matrix of all possible flags and settings
+ * for each band if it is used in ANY reg domain.
+ */
+
+#define DEF_REGDMN		FCC1_FCCA
+#define	DEF_DMN_5		FCC1
+#define	DEF_DMN_2		FCCA
+#define	COUNTRY_ERD_FLAG        0x8000
+#define WORLDWIDE_ROAMING_FLAG  0x4000
+#define	SUPER_DOMAIN_MASK	0x0fff
+#define	COUNTRY_CODE_MASK	0x03ff
+#define CF_INTERFERENCE		(CHANNEL_CW_INT | CHANNEL_RADAR_INT)
+#define CHANNEL_14		(2484)	/* 802.11g operation is not permitted on channel 14 */
+#define IS_11G_CH14(_ch,_cf) \
+	(((_ch) == CHANNEL_14) && ((_cf) == CHANNEL_G))
+
+#define	YES	TRUE
+#define	NO	FALSE
+
+enum {
+	CTRY_DEBUG	= 0x1ff,		/* debug country code */
+	CTRY_DEFAULT	= 0			/* default country code */
+};
+
+typedef struct {
+	HAL_CTRY_CODE	countryCode;
+	HAL_REG_DOMAIN	regDmnEnum;
+	const char*		isoName;
+	const char*		name;
+	HAL_BOOL		allow11g;
+	HAL_BOOL		allow11aTurbo;
+	HAL_BOOL		allow11gTurbo;
+    HAL_BOOL        allow11na;      /* HT-40 allowed in 5GHz? */
+    HAL_BOOL        allow11ng;      /* HT-40 allowed in 2GHz? */
+	u16_t		outdoorChanStart;
+} COUNTRY_CODE_TO_ENUM_RD;
+
+static COUNTRY_CODE_TO_ENUM_RD allCountries[] = {
+    {CTRY_DEBUG,       NO_ENUMRD,     "DB", "DEBUG",          YES, YES, YES, YES, YES, 7000 },
+    {CTRY_DEFAULT,     DEF_REGDMN,    "NA", "NO_COUNTRY_SET", YES, YES, YES, YES, YES, 7000 },
+    {CTRY_ALBANIA,     NULL1_WORLD,   "AL", "ALBANIA",        YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_ALGERIA,     NULL1_WORLD,   "DZ", "ALGERIA",        YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_ARGENTINA,   APL3_WORLD,    "AR", "ARGENTINA",      YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_ARMENIA,     ETSI4_WORLD,   "AM", "ARMENIA",        YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_AUSTRALIA,   FCC6_WORLD,    "AU", "AUSTRALIA",      YES, YES, YES, YES, YES, 7000 },
+    {CTRY_AUSTRIA,     ETSI2_WORLD,   "AT", "AUSTRIA",        YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_AZERBAIJAN,  ETSI4_WORLD,   "AZ", "AZERBAIJAN",     YES, YES, YES, YES, YES, 7000 },
+    {CTRY_BAHRAIN,     APL6_WORLD,    "BH", "BAHRAIN",        YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_BELARUS,     ETSI1_WORLD,   "BY", "BELARUS",        YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_BELGIUM,     ETSI1_WORLD,   "BE", "BELGIUM",        YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_BELIZE,      APL1_ETSIC,    "BZ", "BELIZE",         YES, YES, YES, YES, YES, 7000 },
+    {CTRY_BOLIVIA,     APL1_ETSIC,    "BO", "BOLVIA",         YES, YES, YES, YES, YES, 7000 },
+    {CTRY_BRAZIL,      FCC3_WORLD,    "BR", "BRAZIL",         NO,  NO,  NO,  NO,  NO,  7000 },
+    {CTRY_BRUNEI_DARUSSALAM,APL1_WORLD,"BN", "BRUNEI DARUSSALAM", YES, YES, YES,  YES, YES, 7000 },
+    {CTRY_BULGARIA,    ETSI6_WORLD,   "BG", "BULGARIA",       YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_CANADA,      FCC6_FCCA,     "CA", "CANADA",         YES, YES, YES, YES, YES, 7000 },
+    {CTRY_CHILE,       APL6_WORLD,    "CL", "CHILE",          YES, YES, YES, YES, YES, 7000 },
+    {CTRY_CHINA,       APL1_WORLD,    "CN", "CHINA",          YES, YES, YES, YES, YES, 7000 },
+    {CTRY_COLOMBIA,    FCC1_FCCA,     "CO", "COLOMBIA",       YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_COSTA_RICA,  FCC1_WORLD,    "CR", "COSTA RICA",     YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_CROATIA,     ETSI3_WORLD,   "HR", "CROATIA",        YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_CYPRUS,      ETSI3_WORLD,   "CY", "CYPRUS",         YES, YES, YES, YES, YES, 7000 },
+    {CTRY_CZECH,       ETSI3_WORLD,   "CZ", "CZECH REPUBLIC", YES, NO, YES,  YES, YES, 7000 },
+    {CTRY_DENMARK,     ETSI1_WORLD,   "DK", "DENMARK",        YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_DOMINICAN_REPUBLIC,FCC1_FCCA,"DO", "DOMINICAN REPUBLIC", YES, YES, YES, YES, YES, 7000 },
+    {CTRY_ECUADOR,     FCC1_WORLD,    "EC", "ECUADOR",        YES, NO,  NO,  NO,  YES, 7000 },
+    {CTRY_EGYPT,       ETSI3_WORLD,   "EG", "EGYPT",          YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_EL_SALVADOR, FCC1_WORLD,    "SV", "EL SALVADOR",    YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_ESTONIA,     ETSI1_WORLD,   "EE", "ESTONIA",        YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_FINLAND,     ETSI1_WORLD,   "FI", "FINLAND",        YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_FRANCE,      ETSI1_WORLD,   "FR", "FRANCE",         YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_FRANCE2,     ETSI3_WORLD,   "F2", "FRANCE_RES",     YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_GEORGIA,     ETSI4_WORLD,   "GE", "GEORGIA",        YES, YES, YES, YES, YES, 7000 },
+    {CTRY_GERMANY,     ETSI1_WORLD,   "DE", "GERMANY",        YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_GREECE,      ETSI1_WORLD,   "GR", "GREECE",         YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_GUATEMALA,   FCC1_FCCA,     "GT", "GUATEMALA",      YES, YES, YES, YES, YES, 7000 },
+    {CTRY_HONDURAS,    NULL1_WORLD,   "HN", "HONDURAS",       YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_HONG_KONG,   FCC2_WORLD,    "HK", "HONG KONG",      YES, YES, YES, YES, YES, 7000 },
+    {CTRY_HUNGARY,     ETSI4_WORLD,   "HU", "HUNGARY",        YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_ICELAND,     ETSI1_WORLD,   "IS", "ICELAND",        YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_INDIA,       APL6_WORLD,    "IN", "INDIA",          YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_INDONESIA,   APL1_WORLD,    "ID", "INDONESIA",      YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_IRAN,        APL1_WORLD,    "IR", "IRAN",           YES, YES, YES, YES, YES, 7000 },
+    {CTRY_IRELAND,     ETSI1_WORLD,   "IE", "IRELAND",        YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_ISRAEL,      ETSI3_WORLD,   "IL", "ISRAEL",         YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_ISRAEL2,     NULL1_ETSIB,   "ISR","ISRAEL_RES",     YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_ITALY,       ETSI1_WORLD,   "IT", "ITALY",          YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_JAMAICA,     ETSI1_WORLD,   "JM", "JAMAICA",        YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_JAPAN,       MKK1_MKKA,     "JP", "JAPAN",          YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN1,      MKK1_MKKB,     "J1", "JAPAN1",         YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN2,      MKK1_FCCA,     "J2", "JAPAN2",         YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN3,      MKK2_MKKA,     "J3", "JAPAN3",         YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN4,      MKK1_MKKA1,    "J4", "JAPAN4",         YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN5,      MKK1_MKKA2,    "J5", "JAPAN5",         YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN6,      MKK1_MKKC,     "J6", "JAPAN6",         YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN7,      MKK3_MKKB,     "J7", "JAPAN7",         YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN8,      MKK3_MKKA2,    "J8", "JAPAN8",         YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN9,      MKK3_MKKC,     "J9", "JAPAN9",         YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN10,     MKK4_MKKB,     "J10", "JAPAN10",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN11,     MKK4_MKKA2,    "J11", "JAPAN11",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN12,     MKK4_MKKC,     "J12", "JAPAN12",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN13,     MKK5_MKKB,     "J13", "JAPAN13",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN14,     MKK5_MKKA2,    "J14", "JAPAN14",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN15,     MKK5_MKKC,     "J15", "JAPAN15",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN16,     MKK6_MKKB,     "J16", "JAPAN16",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN17,     MKK6_MKKA2,    "J17", "JAPAN17",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN18,     MKK6_MKKC,     "J18", "JAPAN18",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN19,     MKK7_MKKB,     "J19", "JAPAN19",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN20,     MKK7_MKKA,     "J20", "JAPAN20",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN21,     MKK7_MKKC,     "J21", "JAPAN21",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN22,     MKK8_MKKB,     "J22", "JAPAN22",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN23,     MKK8_MKKA2,    "J23", "JAPAN23",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN24,     MKK8_MKKC,     "J24", "JAPAN24",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN25,     MKK3_MKKA,     "J25", "JAPAN25",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN26,     MKK3_MKKA1,    "J26", "JAPAN26",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN27,     MKK3_FCCA,     "J27", "JAPAN27",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN28,     MKK4_MKKA1,    "J28", "JAPAN28",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN29,     MKK4_FCCA,     "J29", "JAPAN29",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN30,     MKK6_MKKA1,    "J30", "JAPAN30",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN31,     MKK6_FCCA,     "J31", "JAPAN31",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN32,     MKK7_MKKA1,    "J32", "JAPAN32",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN33,     MKK7_FCCA,     "J33", "JAPAN33",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN34,     MKK9_MKKA,     "J34", "JAPAN34",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN35,     MKK10_MKKA,    "J35", "JAPAN35",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN36,     MKK4_MKKA,     "J36", "JAPAN36",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN37,     MKK9_FCCA,     "J37", "JAPAN37",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN38,     MKK9_MKKA1,    "J38", "JAPAN38",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN39,     MKK9_MKKC,     "J39", "JAPAN39",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN40,     MKK10_MKKA2,   "J40", "JAPAN40",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN41,     MKK10_FCCA,    "J41", "JAPAN41",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN42,     MKK10_MKKA1,   "J42", "JAPAN42",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN43,     MKK10_MKKC,    "J43", "JAPAN43",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN44,     MKK10_MKKA2,   "J44", "JAPAN44",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN45,     MKK11_MKKA,    "J45", "JAPAN45",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN46,     MKK11_FCCA,    "J46", "JAPAN46",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN47,     MKK11_MKKA1,   "J47", "JAPAN47",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN48,     MKK11_MKKC,    "J48", "JAPAN48",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN49,     MKK11_MKKA2,   "J49", "JAPAN49",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN50,     MKK12_MKKA,    "J50", "JAPAN50",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN51,     MKK12_FCCA,    "J51", "JAPAN51",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN52,     MKK12_MKKA1,   "J52", "JAPAN52",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN53,     MKK12_MKKC,    "J53", "JAPAN53",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JAPAN54,     MKK12_MKKA2,   "J54", "JAPAN54",       YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_JORDAN,      ETSI2_WORLD,   "JO", "JORDAN",         YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_KAZAKHSTAN,  NULL1_WORLD,   "KZ", "KAZAKHSTAN",     YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_KOREA_NORTH, APL9_WORLD,    "KP", "NORTH KOREA",    YES, NO,  NO,  YES, YES, 7000 },
+    {CTRY_KOREA_ROC,   APL9_WORLD,    "KR", "KOREA REPUBLIC", YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_KOREA_ROC2,  APL2_APLD,     "K2", "KOREA REPUBLIC2",YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_KOREA_ROC3,  APL9_WORLD,    "K3", "KOREA REPUBLIC3",YES, NO,  NO,  NO,  NO,  7000 },
+    {CTRY_KUWAIT,      NULL1_WORLD,   "KW", "KUWAIT",         YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_LATVIA,      ETSI1_WORLD,   "LV", "LATVIA",         YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_LEBANON,     NULL1_WORLD,   "LB", "LEBANON",        YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_LIECHTENSTEIN,ETSI1_WORLD,  "LI", "LIECHTENSTEIN",  YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_LITHUANIA,   ETSI1_WORLD,   "LT", "LITHUANIA",      YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_LUXEMBOURG,  ETSI1_WORLD,   "LU", "LUXEMBOURG",     YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_MACAU,       FCC2_WORLD,    "MO", "MACAU",          YES, YES, YES, YES, YES, 7000 },
+    {CTRY_MACEDONIA,   NULL1_WORLD,   "MK", "MACEDONIA",      YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_MALAYSIA,    APL8_WORLD,    "MY", "MALAYSIA",       NO,  NO,  NO,  NO,  NO,  7000 },
+    {CTRY_MALTA,       ETSI1_WORLD,   "MT", "MALTA",          YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_MEXICO,      FCC1_FCCA,     "MX", "MEXICO",         YES, YES, YES, YES, YES, 7000 },
+    {CTRY_MONACO,      ETSI4_WORLD,   "MC", "MONACO",         YES, YES, YES, YES, YES, 7000 },
+    {CTRY_MOROCCO,     NULL1_WORLD,   "MA", "MOROCCO",        YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_NETHERLANDS, ETSI1_WORLD,   "NL", "NETHERLANDS",    YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_NETHERLANDS_ANT, ETSI1_WORLD, "AN", "NETHERLANDS-ANTILLES", YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_NEW_ZEALAND, FCC2_ETSIC,    "NZ", "NEW ZEALAND",    YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_NORWAY,      ETSI1_WORLD,   "NO", "NORWAY",         YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_OMAN,        APL6_WORLD,    "OM", "OMAN",           YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_PAKISTAN,    NULL1_WORLD,   "PK", "PAKISTAN",       YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_PANAMA,      FCC1_FCCA,     "PA", "PANAMA",         YES, YES, YES, YES, YES, 7000 },
+    {CTRY_PERU,        APL1_WORLD,    "PE", "PERU",           YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_PHILIPPINES, APL1_WORLD,    "PH", "PHILIPPINES",    YES, YES, YES, YES, YES, 7000 },
+    {CTRY_POLAND,      ETSI1_WORLD,   "PL", "POLAND",         YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_PORTUGAL,    ETSI1_WORLD,   "PT", "PORTUGAL",       YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_PUERTO_RICO, FCC1_FCCA,     "PR", "PUERTO RICO",    YES, YES, YES, YES, YES, 7000 },
+    {CTRY_QATAR,       NULL1_WORLD,   "QA", "QATAR",          YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_ROMANIA,     NULL1_WORLD,   "RO", "ROMANIA",        YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_RUSSIA,      NULL1_WORLD,   "RU", "RUSSIA",         YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_SAUDI_ARABIA,NULL1_WORLD,   "SA", "SAUDI ARABIA",   YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_SERBIA_MONT, ETSI1_WORLD,   "CS", "SERBIA & MONTENEGRO", YES, NO,  YES, YES,  YES, 7000 },
+    {CTRY_SINGAPORE,   APL6_WORLD,    "SG", "SINGAPORE",      YES, YES, YES, YES, YES, 7000 },
+    {CTRY_SLOVAKIA,    ETSI1_WORLD,   "SK", "SLOVAK REPUBLIC",YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_SLOVENIA,    ETSI1_WORLD,   "SI", "SLOVENIA",       YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_SOUTH_AFRICA,FCC3_WORLD,    "ZA", "SOUTH AFRICA",   YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_SPAIN,       ETSI1_WORLD,   "ES", "SPAIN",          YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_SRILANKA,    FCC3_WORLD,    "LK", "SRI LANKA",      YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_SWEDEN,      ETSI1_WORLD,   "SE", "SWEDEN",         YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_SWITZERLAND, ETSI1_WORLD,   "CH", "SWITZERLAND",    YES, NO,  YES, YES, YES, 7000 },
+    {CTRY_SYRIA,       NULL1_WORLD,   "SY", "SYRIA",          YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_TAIWAN,      APL3_FCCA,     "TW", "TAIWAN",         YES, YES, YES, YES, YES, 7000 },
+    {CTRY_THAILAND,    NULL1_WORLD,   "TH", "THAILAND",       YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_TRINIDAD_Y_TOBAGO,ETSI4_WORLD,"TT", "TRINIDAD & TOBAGO", YES, NO, YES, NO, YES, 7000 },
+    {CTRY_TUNISIA,     ETSI3_WORLD,   "TN", "TUNISIA",        YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_TURKEY,      ETSI3_WORLD,   "TR", "TURKEY",         YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_UKRAINE,     NULL1_WORLD,   "UA", "UKRAINE",        YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_UAE,         NULL1_WORLD,   "AE", "UNITED ARAB EMIRATES", YES, NO, YES, NO, YES, 7000 },
+    {CTRY_UNITED_KINGDOM, ETSI1_WORLD,"GB", "UNITED KINGDOM", YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_UNITED_STATES, FCC3_FCCA,   "US", "UNITED STATES",  YES, YES, YES, YES, YES, 5825 },
+    {CTRY_UNITED_STATES_FCC49, FCC4_FCCA,   "PS", "UNITED STATES (PUBLIC SAFETY)",  YES, YES, YES, YES, YES, 7000 },
+    {CTRY_URUGUAY,     FCC1_WORLD,    "UY", "URUGUAY",        YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_UZBEKISTAN,  FCC3_FCCA,     "UZ", "UZBEKISTAN",     YES, YES, YES, YES, YES, 7000 },
+    {CTRY_VENEZUELA,   APL2_ETSIC,    "VE", "VENEZUELA",      YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_VIET_NAM,    NULL1_WORLD,   "VN", "VIET NAM",       YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_YEMEN,       NULL1_WORLD,   "YE", "YEMEN",          YES, NO,  YES, NO,  YES, 7000 },
+    {CTRY_ZIMBABWE,    NULL1_WORLD,   "ZW", "ZIMBABWE",       YES, NO,  YES, NO,  YES, 7000 }
+};
+
+typedef struct RegDmnFreqBand {
+	u16_t	lowChannel;	/* Low channel center in MHz */
+	u16_t	highChannel;	/* High Channel center in MHz */
+	u8_t	powerDfs;	/* Max power (dBm) for channel
+					   range when using DFS */
+	u8_t	antennaMax;	/* Max allowed antenna gain */
+	u8_t	channelBW;	/* Bandwidth of the channel */
+	u8_t	channelSep;	/* Channel separation within
+					   the band */
+	u64_t	useDfs;		/* Use DFS in the RegDomain
+					   if corresponding bit is set */
+	u64_t	usePassScan;	/* Use Passive Scan in the RegDomain
+					   if corresponding bit is set */
+	u8_t	regClassId;	/* Regulatory class id */
+	u8_t	useExtChanDfs;	/* Regulatory class id */
+} REG_DMN_FREQ_BAND;
+
+/* Bit masks for DFS per regdomain */
+
+enum {
+	NO_DFS   = 0x0000000000000000ULL,
+	DFS_FCC3 = 0x0000000000000001ULL,
+	DFS_ETSI = 0x0000000000000002ULL,
+	DFS_MKK4 = 0x0000000000000004ULL,
+};
+
+/* The table of frequency bands is indexed by a bitmask.  The ordering
+ * must be consistent with the enum below.  When adding a new
+ * frequency band, be sure to match the location in the enum with the
+ * comments
+ */
+
+/*
+ * 5GHz 11A channel tags
+ */
+
+enum {
+	F1_4915_4925,
+	F1_4935_4945,
+	F1_4920_4980,
+	F1_4942_4987,
+	F1_4945_4985,
+	F1_4950_4980,
+	F1_5035_5040,
+	F1_5040_5080,
+	F1_5055_5055,
+
+	F1_5120_5240,
+
+	F1_5170_5230,
+	F2_5170_5230,
+
+	F1_5180_5240,
+	F2_5180_5240,
+	F3_5180_5240,
+	F4_5180_5240,
+	F5_5180_5240,
+	F6_5180_5240,
+	F7_5180_5240,
+
+	F1_5180_5320,
+
+	F1_5240_5280,
+
+	F1_5260_5280,
+
+	F1_5260_5320,
+	F2_5260_5320,
+	F3_5260_5320,
+	F4_5260_5320,
+	F5_5260_5320,
+	F6_5260_5320,
+	F7_5260_5320,
+
+	F1_5260_5700,
+
+	F1_5280_5320,
+
+    F1_5500_5580,
+
+	F1_5500_5620,
+
+	F1_5500_5700,
+	F2_5500_5700,
+	F3_5500_5700,
+	F4_5500_5700,
+
+    F1_5660_5700,
+
+	F1_5745_5805,
+	F2_5745_5805,
+	F3_5745_5805,
+
+	F1_5745_5825,
+	F2_5745_5825,
+	F3_5745_5825,
+	F4_5745_5825,
+	F5_5745_5825,
+	F6_5745_5825,
+
+	W1_4920_4980,
+	W1_5040_5080,
+	W1_5170_5230,
+	W1_5180_5240,
+	W1_5260_5320,
+	W1_5745_5825,
+	W1_5500_5700,
+	W2_5260_5320,
+        W2_5180_5240,
+	W2_5825_5825,
+};
+
+static REG_DMN_FREQ_BAND regDmn5GhzFreq[] = {
+	{ 4915, 4925, 23, 0, 10, 5, NO_DFS, PSCAN_MKK2, 16, 0 },		/* F1_4915_4925 */
+	{ 4935, 4945, 23, 0, 10, 5, NO_DFS, PSCAN_MKK2, 16, 0 },		/* F1_4935_4945 */
+	{ 4920, 4980, 23, 0, 20, 20, NO_DFS, PSCAN_MKK2, 7, 0 },		/* F1_4920_4980 */
+	{ 4942, 4987, 27, 6, 5,  5, DFS_FCC3, PSCAN_FCC, 0, 0 },		/* F1_4942_4987 */
+	{ 4945, 4985, 30, 6, 10, 5, DFS_FCC3, PSCAN_FCC, 0, 0 },		/* F1_4945_4985 */
+	{ 4950, 4980, 33, 6, 20, 5, DFS_FCC3, PSCAN_FCC, 0, 0 },		/* F1_4950_4980 */
+	{ 5035, 5040, 23, 0, 10, 5, NO_DFS, PSCAN_MKK2, 12, 0 },		/* F1_5035_5040 */
+	{ 5040, 5080, 23, 0, 20, 20, NO_DFS, PSCAN_MKK2, 2, 0 },		/* F1_5040_5080 */
+	{ 5055, 5055, 23, 0, 10, 5, NO_DFS, PSCAN_MKK2, 12, 0 },		/* F1_5055_5055 */
+
+	{ 5120, 5240, 5,  6, 20, 20, NO_DFS, NO_PSCAN, 0, 0 },			/* F1_5120_5240 */
+
+	{ 5170, 5230, 23, 0, 20, 20, NO_DFS, PSCAN_MKK1 | PSCAN_MKK2, 1, 0 },	/* F1_5170_5230 */
+	{ 5170, 5230, 20, 0, 20, 20, NO_DFS, PSCAN_MKK1 | PSCAN_MKK2, 1, 0 },	/* F2_5170_5230 */
+
+	{ 5180, 5240, 15, 0, 20, 20, NO_DFS, PSCAN_FCC | PSCAN_ETSI, 0, 0 },	/* F1_5180_5240 */
+	{ 5180, 5240, 17, 6, 20, 20, NO_DFS, PSCAN_FCC, 1, 0 },				/* F2_5180_5240 */
+	{ 5180, 5240, 18, 0, 20, 20, NO_DFS, PSCAN_FCC | PSCAN_ETSI, 0, 0 },	/* F3_5180_5240 */
+	{ 5180, 5240, 20, 0, 20, 20, NO_DFS, PSCAN_FCC | PSCAN_ETSI, 0, 0 },	/* F4_5180_5240 */
+	{ 5180, 5240, 23, 0, 20, 20, NO_DFS, PSCAN_FCC | PSCAN_ETSI, 0, 0 },	/* F5_5180_5240 */
+	{ 5180, 5240, 23, 6, 20, 20, NO_DFS, PSCAN_FCC, 0, 0 },				/* F6_5180_5240 */
+  { 5180, 5240, 23, 6, 20, 20, NO_DFS, NO_PSCAN, 0 },           /* F7_5180_5240 */
+
+	{ 5180, 5320, 20, 6, 20, 20, DFS_ETSI, PSCAN_ETSI, 0, 0 },			/* F1_5180_5320 */
+
+	{ 5240, 5280, 23, 0, 20, 20, DFS_FCC3, PSCAN_FCC | PSCAN_ETSI, 0, 0 },	/* F1_5240_5280 */
+
+	{ 5260, 5280, 23, 0, 20, 20, DFS_FCC3 | DFS_ETSI, PSCAN_FCC | PSCAN_ETSI, 0, 0 },	/* F1_5260_5280 */
+
+	{ 5260, 5320, 18, 0, 20, 20, DFS_FCC3 | DFS_ETSI, PSCAN_FCC | PSCAN_ETSI, 0, 0 },	/* F1_5260_5320 */
+
+	{ 5260, 5320, 20, 0, 20, 20, DFS_FCC3 | DFS_ETSI | DFS_MKK4, PSCAN_FCC | PSCAN_ETSI | PSCAN_MKK3 , 0, 0 },
+											/* F2_5260_5320 */
+
+	{ 5260, 5320, 20, 6, 20, 20, DFS_FCC3 | DFS_ETSI, PSCAN_FCC, 2, 0 },	/* F3_5260_5320 */
+	{ 5260, 5320, 23, 6, 20, 20, DFS_FCC3 | DFS_ETSI, PSCAN_FCC, 2, 0 },	/* F4_5260_5320 */
+	{ 5260, 5320, 23, 6, 20, 20, DFS_FCC3 | DFS_ETSI, PSCAN_FCC, 0, 0 },	/* F5_5260_5320 */
+	{ 5260, 5320, 30, 0, 20, 20, NO_DFS, NO_PSCAN, 0, 0 },				/* F6_5260_5320 */
+	{ 5260, 5320, 17, 6, 20, 20, DFS_ETSI, PSCAN_ETSI, 0, 0 },				/* F7_5260_5320 */
+
+	{ 5260, 5700, 5,  6, 20, 20, DFS_FCC3 | DFS_ETSI, NO_PSCAN, 0, 0 },		/* F1_5260_5700 */
+
+	{ 5280, 5320, 17, 6, 20, 20, DFS_FCC3 | DFS_ETSI, PSCAN_FCC, 0, 0 },	/* F1_5280_5320 */
+
+    { 5500, 5580, 23, 6, 20, 20, DFS_FCC3, PSCAN_FCC, 0},                           /* F1_5500_5580 */
+
+	{ 5500, 5620, 30, 6, 20, 20, DFS_ETSI, PSCAN_ETSI, 0, 0 },				/* F1_5500_5620 */
+
+	{ 5500, 5700, 20, 6, 20, 20, DFS_FCC3 | DFS_ETSI, PSCAN_FCC, 4, 0 },		/* F1_5500_5700 */
+	{ 5500, 5700, 27, 0, 20, 20, DFS_FCC3 | DFS_ETSI, PSCAN_FCC | PSCAN_ETSI, 0, 0 },	/* F2_5500_5700 */
+	{ 5500, 5700, 30, 0, 20, 20, DFS_FCC3 | DFS_ETSI, PSCAN_FCC | PSCAN_ETSI, 0, 0 },	/* F3_5500_5700 */
+	{ 5500, 5700, 20, 0, 20, 20, DFS_FCC3 | DFS_ETSI | DFS_MKK4, PSCAN_MKK3 | PSCAN_FCC, 0, 0 },
+											/* F4_5500_5700 */
+
+    { 5660, 5700, 23, 6, 20, 20, DFS_FCC3, PSCAN_FCC, 0},                           /* F1_5660_5700 */
+
+	{ 5745, 5805, 23, 0, 20, 20, NO_DFS, NO_PSCAN, 0, 0 },				/* F1_5745_5805 */
+	{ 5745, 5805, 30, 6, 20, 20, NO_DFS, NO_PSCAN, 0, 0 },				/* F2_5745_5805 */
+	{ 5745, 5805, 30, 6, 20, 20, DFS_ETSI, PSCAN_ETSI, 0, 0 },				/* F3_5745_5805 */
+	{ 5745, 5825, 5,  6, 20, 20, NO_DFS, NO_PSCAN, 0, 0 },				/* F1_5745_5825 */
+	{ 5745, 5825, 17, 0, 20, 20, NO_DFS, NO_PSCAN, 0, 0 },				/* F2_5745_5825 */
+	{ 5745, 5825, 20, 0, 20, 20, DFS_ETSI, NO_PSCAN, 0, 0 },				/* F3_5745_5825 */
+	{ 5745, 5825, 30, 0, 20, 20, NO_DFS, NO_PSCAN, 0, 0 },				/* F4_5745_5825 */
+	{ 5745, 5825, 30, 6, 20, 20, NO_DFS, NO_PSCAN, 3, 0 },			/* F5_5745_5825 */
+	{ 5745, 5825, 30, 6, 20, 20, NO_DFS, NO_PSCAN, 0, 0 },				/* F6_5745_5825 */
+
+	/*
+	 * Below are the world roaming channels
+	 * All WWR domains have no power limit, instead use the card's CTL
+	 * or max power settings.
+	 */
+	{ 4920, 4980, 30, 0, 20, 20, NO_DFS, PSCAN_WWR, 0, 0 },				/* W1_4920_4980 */
+	{ 5040, 5080, 30, 0, 20, 20, NO_DFS, PSCAN_WWR, 0 },				/* W1_5040_5080 */
+	{ 5170, 5230, 30, 0, 20, 20, DFS_FCC3 | DFS_ETSI, PSCAN_WWR, 0, 0 },		/* W1_5170_5230 */
+	{ 5180, 5240, 30, 0, 20, 20, DFS_FCC3 | DFS_ETSI, PSCAN_WWR, 0, 0 },		/* W1_5180_5240 */
+	{ 5260, 5320, 30, 0, 20, 20, DFS_FCC3 | DFS_ETSI, PSCAN_WWR, 0, 0 },		/* W1_5260_5320 */
+	{ 5745, 5825, 30, 0, 20, 20, NO_DFS, PSCAN_WWR, 0, 0 },				/* W1_5745_5825 */
+	{ 5500, 5700, 30, 0, 20, 20, DFS_FCC3 | DFS_ETSI, PSCAN_WWR, 0, 0 },		/* W1_5500_5700 */
+	{ 5260, 5320, 30, 0, 20, 20, NO_DFS, NO_PSCAN,  0, 0 },				/* W2_5260_5320 */
+	{ 5180, 5240, 30, 0, 20, 20, NO_DFS, NO_PSCAN,  0, 0 },				/* W2_5180_5240 */
+	{ 5825, 5825, 30, 0, 20, 20, NO_DFS, PSCAN_WWR, 0, 0 },				/* W2_5825_5825 */
+};
+/*
+ * 5GHz Turbo (dynamic & static) tags
+ */
+
+enum {
+	T1_5130_5210,
+	T1_5250_5330,
+	T1_5370_5490,
+	T1_5530_5650,
+
+	T1_5150_5190,
+	T1_5230_5310,
+	T1_5350_5470,
+	T1_5510_5670,
+
+	T1_5200_5240,
+	T2_5200_5240,
+	T1_5210_5210,
+	T2_5210_5210,
+
+	T1_5280_5280,
+	T2_5280_5280,
+	T1_5250_5250,
+	T1_5290_5290,
+	T1_5250_5290,
+	T2_5250_5290,
+
+	T1_5540_5660,
+	T1_5760_5800,
+	T2_5760_5800,
+
+	T1_5765_5805,
+
+	WT1_5210_5250,
+	WT1_5290_5290,
+	WT1_5540_5660,
+	WT1_5760_5800,
+};
+
+static REG_DMN_FREQ_BAND regDmn5GhzTurboFreq[] = {
+	{ 5130, 5210, 5,  6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	/* T1_5130_5210 */
+	{ 5250, 5330, 5,  6, 40, 40, DFS_FCC3, NO_PSCAN, 0, 0},	/* T1_5250_5330 */
+	{ 5370, 5490, 5,  6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	/* T1_5370_5490 */
+	{ 5530, 5650, 5,  6, 40, 40, DFS_FCC3, NO_PSCAN, 0, 0},	/* T1_5530_5650 */
+
+	{ 5150, 5190, 5,  6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	/* T1_5150_5190 */
+	{ 5230, 5310, 5,  6, 40, 40, DFS_FCC3, NO_PSCAN, 0, 0},	/* T1_5230_5310 */
+	{ 5350, 5470, 5,  6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	/* T1_5350_5470 */
+	{ 5510, 5670, 5,  6, 40, 40, DFS_FCC3, NO_PSCAN, 0, 0},	/* T1_5510_5670 */
+
+	{ 5200, 5240, 17, 6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	/* T1_5200_5240 */
+	{ 5200, 5240, 23, 6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	/* T2_5200_5240 */
+	{ 5210, 5210, 17, 6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	/* T1_5210_5210 */
+	{ 5210, 5210, 23, 0, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	/* T2_5210_5210 */
+
+	{ 5280, 5280, 23, 6, 40, 40, DFS_FCC3, PSCAN_FCC_T, 0, 0},	/* T1_5280_5280 */
+	{ 5280, 5280, 20, 6, 40, 40, DFS_FCC3, PSCAN_FCC_T, 0, 0},	/* T2_5280_5280 */
+	{ 5250, 5250, 17, 0, 40, 40, DFS_FCC3, PSCAN_FCC_T, 0, 0},	/* T1_5250_5250 */
+	{ 5290, 5290, 20, 0, 40, 40, DFS_FCC3, PSCAN_FCC_T, 0, 0},	/* T1_5290_5290 */
+	{ 5250, 5290, 20, 0, 40, 40, DFS_FCC3, PSCAN_FCC_T, 0, 0},	/* T1_5250_5290 */
+	{ 5250, 5290, 23, 6, 40, 40, DFS_FCC3, PSCAN_FCC_T, 0, 0},	/* T2_5250_5290 */
+
+	{ 5540, 5660, 20, 6, 40, 40, DFS_FCC3, PSCAN_FCC_T, 0, 0},	/* T1_5540_5660 */
+	{ 5760, 5800, 20, 0, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	/* T1_5760_5800 */
+	{ 5760, 5800, 30, 6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	/* T2_5760_5800 */
+
+	{ 5765, 5805, 30, 6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	/* T1_5765_5805 */
+
+	/*
+	 * Below are the WWR frequencies
+	 */
+
+	{ 5210, 5250, 15, 0, 40, 40, DFS_FCC3 | DFS_ETSI, PSCAN_WWR, 0, 0}, /* WT1_5210_5250 */
+	{ 5290, 5290, 18, 0, 40, 40, DFS_FCC3 | DFS_ETSI, PSCAN_WWR, 0, 0}, /* WT1_5290_5290 */
+	{ 5540, 5660, 20, 0, 40, 40, DFS_FCC3 | DFS_ETSI, PSCAN_WWR, 0, 0}, /* WT1_5540_5660 */
+	{ 5760, 5800, 20, 0, 40, 40, NO_DFS, PSCAN_WWR, 0, 0},	/* WT1_5760_5800 */
+};
+
+/*
+ * 2GHz 11b channel tags
+ */
+enum {
+	F1_2312_2372,
+	F2_2312_2372,
+
+	F1_2412_2472,
+	F2_2412_2472,
+	F3_2412_2472,
+
+	F1_2412_2462,
+	F2_2412_2462,
+
+	F1_2432_2442,
+
+	F1_2457_2472,
+
+	F1_2467_2472,
+
+	F1_2484_2484,
+	F2_2484_2484,
+
+	F1_2512_2732,
+
+	W1_2312_2372,
+	W1_2412_2412,
+	W1_2417_2432,
+	W1_2437_2442,
+	W1_2447_2457,
+	W1_2462_2462,
+	W1_2467_2467,
+	W2_2467_2467,
+	W1_2472_2472,
+	W2_2472_2472,
+	W1_2484_2484,
+	W2_2484_2484,
+};
+
+static REG_DMN_FREQ_BAND regDmn2GhzFreq[] = {
+	{ 2312, 2372, 5,  6, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* F1_2312_2372 */
+	{ 2312, 2372, 20, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* F2_2312_2372 */
+
+	{ 2412, 2472, 5,  6, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* F1_2412_2472 */
+	{ 2412, 2472, 20, 0, 20, 5, NO_DFS, PSCAN_MKKA, 0, 0},	/* F2_2412_2472 */
+	{ 2412, 2472, 30, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* F3_2412_2472 */
+
+	{ 2412, 2462, 27, 6, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* F1_2412_2462 */
+	{ 2412, 2462, 20, 0, 20, 5, NO_DFS, PSCAN_MKKA, 0, 0},	/* F2_2412_2462 */
+	{ 2432, 2442, 20, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* F1_2432_2442 */
+
+	{ 2457, 2472, 20, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* F1_2457_2472 */
+
+	{ 2467, 2472, 20, 0, 20, 5, NO_DFS, PSCAN_MKKA2 | PSCAN_MKKA, 0, 0}, /* F1_2467_2472 */
+
+	{ 2484, 2484, 5,  6, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* F1_2484_2484 */
+	{ 2484, 2484, 20, 0, 20, 5, NO_DFS, PSCAN_MKKA | PSCAN_MKKA1 | PSCAN_MKKA2, 0, 0},	/* F2_2484_2484 */
+
+	{ 2512, 2732, 5,  6, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* F1_2512_2732 */
+
+	/*
+	 * WWR have powers opened up to 20dBm.  Limits should often come from CTL/Max powers
+	 */
+
+	{ 2312, 2372, 20, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* W1_2312_2372 */
+	{ 2412, 2412, 20, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* W1_2412_2412 */
+	{ 2417, 2432, 20, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* W1_2417_2432 */
+	{ 2437, 2442, 20, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* W1_2437_2442 */
+	{ 2447, 2457, 20, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* W1_2447_2457 */
+	{ 2462, 2462, 20, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* W1_2462_2462 */
+	{ 2467, 2467, 20, 0, 20, 5, NO_DFS, PSCAN_WWR | IS_ECM_CHAN, 0, 0}, /* W1_2467_2467 */
+	{ 2467, 2467, 20, 0, 20, 5, NO_DFS, NO_PSCAN | IS_ECM_CHAN, 0, 0},	/* W2_2467_2467 */
+	{ 2472, 2472, 20, 0, 20, 5, NO_DFS, PSCAN_WWR | IS_ECM_CHAN, 0, 0}, /* W1_2472_2472 */
+	{ 2472, 2472, 20, 0, 20, 5, NO_DFS, NO_PSCAN | IS_ECM_CHAN, 0, 0},	/* W2_2472_2472 */
+	{ 2484, 2484, 20, 0, 20, 5, NO_DFS, PSCAN_WWR | IS_ECM_CHAN, 0, 0}, /* W1_2484_2484 */
+	{ 2484, 2484, 20, 0, 20, 5, NO_DFS, NO_PSCAN | IS_ECM_CHAN, 0, 0},	/* W2_2484_2484 */
+};
+
+
+/*
+ * 2GHz 11g channel tags
+ */
+
+enum {
+	G1_2312_2372,
+	G2_2312_2372,
+
+	G1_2412_2472,
+	G2_2412_2472,
+	G3_2412_2472,
+
+	G1_2412_2462,
+	G2_2412_2462,
+
+	G1_2432_2442,
+
+	G1_2457_2472,
+
+	G1_2512_2732,
+
+	G1_2467_2472 ,
+
+	WG1_2312_2372,
+	WG1_2412_2412,
+	WG1_2417_2432,
+	WG1_2437_2442,
+	WG1_2447_2457,
+	WG1_2462_2462,
+	WG1_2467_2467,
+	WG2_2467_2467,
+	WG1_2472_2472,
+	WG2_2472_2472,
+
+};
+static REG_DMN_FREQ_BAND regDmn2Ghz11gFreq[] = {
+	{ 2312, 2372, 5,  6, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* G1_2312_2372 */
+	{ 2312, 2372, 20, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* G2_2312_2372 */
+
+	{ 2412, 2472, 5,  6, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* G1_2412_2472 */
+	{ 2412, 2472, 20, 0, 20, 5,  NO_DFS, PSCAN_MKKA_G, 0, 0},	/* G2_2412_2472 */
+	{ 2412, 2472, 30, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* G3_2412_2472 */
+
+	{ 2412, 2462, 27, 6, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* G1_2412_2462 */
+	{ 2412, 2462, 20, 0, 20, 5, NO_DFS, PSCAN_MKKA_G, 0, 0},	/* G2_2412_2462 */
+	{ 2432, 2442, 20, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* G1_2432_2442 */
+
+	{ 2457, 2472, 20, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* G1_2457_2472 */
+
+	{ 2512, 2732, 5,  6, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* G1_2512_2732 */
+
+	{ 2467, 2472, 20, 0, 20, 5, NO_DFS, PSCAN_MKKA2 | PSCAN_MKKA, 0, 0 }, /* G1_2467_2472 */
+
+	/*
+	 * WWR open up the power to 20dBm
+	 */
+
+	{ 2312, 2372, 20, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* WG1_2312_2372 */
+	{ 2412, 2412, 20, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* WG1_2412_2412 */
+	{ 2417, 2432, 20, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* WG1_2417_2432 */
+	{ 2437, 2442, 20, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* WG1_2437_2442 */
+	{ 2447, 2457, 20, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* WG1_2447_2457 */
+	{ 2462, 2462, 20, 0, 20, 5, NO_DFS, NO_PSCAN, 0, 0},	/* WG1_2462_2462 */
+	{ 2467, 2467, 20, 0, 20, 5, NO_DFS, PSCAN_WWR | IS_ECM_CHAN, 0, 0}, /* WG1_2467_2467 */
+	{ 2467, 2467, 20, 0, 20, 5, NO_DFS, NO_PSCAN | IS_ECM_CHAN, 0, 0},	/* WG2_2467_2467 */
+	{ 2472, 2472, 20, 0, 20, 5, NO_DFS, PSCAN_WWR | IS_ECM_CHAN, 0, 0}, /* WG1_2472_2472 */
+	{ 2472, 2472, 20, 0, 20, 5, NO_DFS, NO_PSCAN | IS_ECM_CHAN, 0, 0},	/* WG2_2472_2472 */
+};
+/*
+ * 2GHz Dynamic turbo tags
+ */
+
+enum {
+	T1_2312_2372,
+	T1_2437_2437,
+	T2_2437_2437,
+	T3_2437_2437,
+	T1_2512_2732
+};
+
+static REG_DMN_FREQ_BAND regDmn2Ghz11gTurboFreq[] = {
+	{ 2312, 2372, 5,  6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},  /* T1_2312_2372 */
+	{ 2437, 2437, 5,  6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},  /* T1_2437_2437 */
+	{ 2437, 2437, 20, 6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},  /* T2_2437_2437 */
+	{ 2437, 2437, 18, 6, 40, 40, NO_DFS, PSCAN_WWR, 0, 0}, /* T3_2437_2437 */
+	{ 2512, 2732, 5,  6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},  /* T1_2512_2732 */
+};
+
+
+
+/*
+ * 2GHz 11n frequency tags
+ */
+enum {
+    NG1_2422_2452,
+    NG2_2422_2452,
+    NG3_2422_2452,
+
+    NG_DEMO_ALL_CHANNELS,
+};
+
+static REG_DMN_FREQ_BAND regDmn2Ghz11ngFreq[] = {
+    { 2422, 2452, 20, 0, 40, 5, NO_DFS, NO_PSCAN, 0, 0},    /* NG1_2422_2452 */
+    { 2422, 2452, 27, 0, 40, 5, NO_DFS, NO_PSCAN, 0, 0},    /* NG2_2422_2452 */
+    { 2422, 2452, 30, 0, 40, 5, NO_DFS, NO_PSCAN, 0, 0},    /* NG3_2422_2452 */
+
+	{ 2312, 2732, 27, 6, 20, 5, NO_DFS, NO_PSCAN, 0, 0},    /* NG_DEMO_ALL_CHANNELS */
+};
+
+
+/*
+ * 5GHz 11n frequency tags
+ */
+enum {
+    NA1_5190_5230,
+    NA2_5190_5230,
+    NA3_5190_5230,
+    NA4_5190_5230,
+    NA5_5190_5230,
+
+    NA1_5270_5270,
+
+    NA1_5270_5310,
+    NA2_5270_5310,
+    NA3_5270_5310,
+    NA4_5270_5310,
+
+    NA1_5310_5310,
+
+    NA1_5510_5630,
+
+    NA1_5510_5670,
+    NA2_5510_5670,
+    NA3_5510_5670,
+
+    NA1_5755_5795,
+    NA2_5755_5795,
+    NA3_5755_5795,
+    NA4_5755_5795,
+    NA5_5755_5795,
+
+    NA1_5795_5795,
+
+    NA_DEMO_ALL_CHANNELS,
+};
+
+static REG_DMN_FREQ_BAND regDmn5Ghz11naFreq[] = {
+    /*
+     * ToDo: This table needs to be completely populated with 5GHz 11n properties
+     */
+	{ 5190, 5230, 15,  0, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	    /* NA1_5190_5230 */
+	{ 5190, 5230, 17,  6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	/* NA2_5190_5230 */
+	{ 5190, 5230, 18,  6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	    /* NA3_5190_5230 */
+	{ 5190, 5230, 20,  0, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	/* NA4_5190_5230 */
+	{ 5190, 5230, 23,  6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	    /* NA5_5190_5230 */
+
+	{ 5270, 5270, 23,  6, 40, 40, DFS_FCC3|DFS_ETSI, NO_PSCAN, 0, 1},  /* NA1_5270_5270 */
+
+	{ 5270, 5310, 18,  6, 40, 40, DFS_FCC3|DFS_ETSI, NO_PSCAN, 0, 1},  /* NA1_5270_5310 */
+	{ 5270, 5310, 20,  0, 40, 40, DFS_FCC3|DFS_ETSI|DFS_MKK4, NO_PSCAN, 0, 1},  /* NA2_5270_5310 */
+	{ 5270, 5310, 23,  6, 40, 40, DFS_FCC3|DFS_ETSI, NO_PSCAN, 0, 1},  /* NA3_5270_5310 */
+	{ 5270, 5310, 30,  6, 40, 40, DFS_FCC3|DFS_ETSI, NO_PSCAN, 0, 1},  /* NA4_5270_5310 */
+
+	{ 5310, 5310, 17,  6, 40, 40, DFS_FCC3|DFS_ETSI, NO_PSCAN, 0, 1},  /* NA1_5310_5310 */
+
+	{ 5510, 5630, 30,  6, 40, 40, DFS_FCC3|DFS_ETSI, NO_PSCAN, 0, 1},  /* NA1_5510_5630 */
+
+	{ 5510, 5670, 20,  6, 40, 40, DFS_FCC3|DFS_ETSI|DFS_MKK4, NO_PSCAN, 0, 1},  /* NA1_5510_5670 */
+	{ 5510, 5670, 27,  0, 40, 40, DFS_FCC3|DFS_ETSI, NO_PSCAN, 0, 1},  /* NA2_5510_5670 */
+	{ 5510, 5670, 30,  6, 40, 40, DFS_FCC3, NO_PSCAN, 0, 1},   /* NA3_5510_5670 */
+
+	{ 5755, 5795, 17,  0, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	    /* NA1_5755_5795 */
+	{ 5755, 5795, 20,  6, 40, 40, DFS_ETSI, NO_PSCAN, 0, 0},	    /* NA2_5755_5795 */
+	{ 5755, 5795, 23,  0, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	    /* NA3_5755_5795 */
+	{ 5755, 5795, 30,  0, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	/* NA4_5755_5795 */
+	{ 5755, 5795, 30,  6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	    /* NA5_5755_5795 */
+
+	{ 5795, 5795, 30,  6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	    /* NA1_5795_5795 */
+
+    { 4920, 6100, 30,  6, 40, 40, NO_DFS, NO_PSCAN, 0, 0},	/* NA_DEMO_ALL_CHANNELS */
+};
+
+typedef struct regDomain {
+	u16_t regDmnEnum;	/* value from EnumRd table */
+	u8_t conformanceTestLimit;
+	u64_t dfsMask;	/* DFS bitmask for 5Ghz tables */
+	u64_t pscan;	/* Bitmask for passive scan */
+	u32_t flags;	/* Requirement flags (AdHoc disallow, noise
+				   floor cal needed, etc) */
+	u64_t chan11a[BMLEN];/* 128 bit bitmask for channel/band
+				   selection */
+	u64_t chan11a_turbo[BMLEN];/* 128 bit bitmask for channel/band
+				   selection */
+	u64_t chan11a_dyn_turbo[BMLEN]; /* 128 bit bitmask for channel/band
+					       selection */
+	u64_t chan11b[BMLEN];/* 128 bit bitmask for channel/band
+				   selection */
+	u64_t chan11g[BMLEN];/* 128 bit bitmask for channel/band
+				   selection */
+	u64_t chan11g_turbo[BMLEN];/* 128 bit bitmask for channel/band
+					  selection */
+	u64_t chan11ng[BMLEN];/* 128 bit bitmask for 11n in 2GHz */
+	u64_t chan11na[BMLEN];/* 128 bit bitmask for 11n in 5GHz */
+} REG_DOMAIN;
+
+static REG_DOMAIN regDomains[] = {
+
+	{DEBUG_REG_DMN, FCC, NO_DFS, NO_PSCAN, NO_REQ,
+	 BM(F1_5120_5240, F1_5260_5700, F1_5745_5825, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(T1_5130_5210, T1_5250_5330, T1_5370_5490, T1_5530_5650, T1_5150_5190, T1_5230_5310, T1_5350_5470, T1_5510_5670, -1, -1, -1, -1),
+	 BM(T1_5200_5240, T1_5280_5280, T1_5540_5660, T1_5765_5805, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(F1_2312_2372, F1_2412_2472, F1_2484_2484, F1_2512_2732, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(G1_2312_2372, G1_2412_2472, G1_2512_2732, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(T1_2312_2372, T1_2437_2437, T1_2512_2732, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(NG_DEMO_ALL_CHANNELS, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(NA_DEMO_ALL_CHANNELS, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{APL1, ETSI, NO_DFS, NO_PSCAN, NO_REQ,
+	 BM(F4_5745_5825, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA4_5755_5795, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{APL2, ETSI, NO_DFS, NO_PSCAN, NO_REQ,
+	 BM(F1_5745_5805, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA3_5755_5795, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{APL3, FCC, NO_DFS, NO_PSCAN, NO_REQ,
+	 BM(F1_5280_5320, F2_5745_5805, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA1_5310_5310, NA4_5755_5795, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{APL4, ETSI, NO_DFS, NO_PSCAN, NO_REQ,
+	 BM(F4_5180_5240,  F3_5745_5825, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA4_5190_5230, NA2_5755_5795, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{APL5, ETSI, NO_DFS, NO_PSCAN, NO_REQ,
+	 BM(F2_5745_5825, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA1_5755_5795, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{APL6, ETSI, DFS_ETSI, PSCAN_FCC_T | PSCAN_FCC , NO_REQ,
+	 BM(F4_5180_5240, F2_5260_5320, F3_5745_5825, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(T2_5210_5210, T1_5250_5290, T1_5760_5800, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA4_5190_5230, NA2_5270_5310, NA2_5755_5795, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{APL7, FCC, NO_DFS, PSCAN_FCC_T | PSCAN_FCC , NO_REQ,
+	 BM(F7_5260_5320, F4_5500_5700, F3_5745_5805, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA1_5310_5310, NA2_5755_5795, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+	{APL8, ETSI, NO_DFS, NO_PSCAN, DISALLOW_ADHOC_11A|DISALLOW_ADHOC_11A_TURB,
+	 BM(F6_5260_5320, F4_5745_5825, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA4_5270_5310, NA4_5755_5795, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{APL9, ETSI, DFS_ETSI, PSCAN_ETSI, DISALLOW_ADHOC_11A|DISALLOW_ADHOC_11A_TURB,
+	 BM(F1_5180_5320, F1_5500_5620, F3_5745_5805, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA4_5190_5230, NA2_5270_5310, NA1_5510_5630, NA4_5755_5795, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{ETSI1, ETSI, DFS_ETSI, PSCAN_ETSI, DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB,
+	 BM(W2_5180_5240, F2_5260_5320, F2_5500_5700, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA4_5190_5230, NA2_5270_5310, NA2_5510_5670, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{ETSI2, ETSI, DFS_ETSI, PSCAN_ETSI, DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB,
+	 BM(F3_5180_5240, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA3_5190_5230, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{ETSI3, ETSI, DFS_ETSI, PSCAN_ETSI, DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB,
+	 BM(W2_5180_5240, F2_5260_5320, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA4_5190_5230, NA2_5270_5310, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{ETSI4, ETSI, DFS_ETSI, PSCAN_ETSI, DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB,
+	 BM(F3_5180_5240, F1_5260_5320, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA3_5190_5230, NA1_5270_5310, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{ETSI5, ETSI, DFS_ETSI, PSCAN_ETSI, DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB,
+	 BM(F1_5180_5240, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA1_5190_5230, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{ETSI6, ETSI, DFS_ETSI, PSCAN_ETSI, DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB,
+	 BM(F5_5180_5240, F1_5260_5280, F3_5500_5700, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA5_5190_5230, NA1_5270_5270, NA3_5510_5670, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{FCC1, FCC, NO_DFS, NO_PSCAN, NO_REQ,
+	 BM(F2_5180_5240, F4_5260_5320, F5_5745_5825, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(T1_5210_5210, T2_5250_5290, T2_5760_5800, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(T1_5200_5240, T1_5280_5280, T1_5765_5805, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA2_5190_5230, NA3_5270_5310, NA4_5755_5795, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{FCC2, FCC, NO_DFS, NO_PSCAN, NO_REQ,
+	 BM(F6_5180_5240, F5_5260_5320, F6_5745_5825, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(T2_5200_5240, T1_5280_5280, T1_5765_5805, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA5_5190_5230, NA3_5270_5310, NA4_5755_5795, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{FCC3, FCC, DFS_FCC3, PSCAN_FCC | PSCAN_FCC_T, NO_REQ,
+	 BM(F2_5180_5240, F3_5260_5320, F1_5500_5700, F5_5745_5825, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(T1_5210_5210, T1_5250_5250, T1_5290_5290, T2_5760_5800, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(T1_5200_5240, T2_5280_5280, T1_5540_5660, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA2_5190_5230, NA2_5270_5310, NA3_5510_5670, NA4_5755_5795, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{FCC4, FCC, DFS_FCC3, PSCAN_FCC | PSCAN_FCC_T, NO_REQ,
+	 BM(F1_4942_4987, F1_4945_4985, F1_4950_4980, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO},
+
+	{FCC5, FCC, NO_DFS, NO_PSCAN, NO_REQ,
+	 BM(F2_5180_5240, F5_5745_5825, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA2_5190_5230, NA4_5755_5795, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+    {FCC6, FCC, DFS_FCC3, PSCAN_FCC, NO_REQ,
+	 BM(F7_5180_5240, F5_5260_5320, F1_5500_5580, F1_5660_5700, F6_5745_5825, -1, -1, -1, -1, -1, -1, -1),
+	 BM(-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(T2_5200_5240, T1_5280_5280, T1_5765_5805, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+     BMZERO,
+     BMZERO,
+	 BM(NA5_5190_5230, NA5_5755_5795, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	{MKK1, MKK, NO_DFS, PSCAN_MKK1, DISALLOW_ADHOC_11A_TURB,
+	 BM(F1_5170_5230, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO},
+
+	{MKK2, MKK, NO_DFS, PSCAN_MKK2, DISALLOW_ADHOC_11A_TURB,
+	 BM(F1_4915_4925, F1_4935_4945, F1_4920_4980, F1_5035_5040, F1_5055_5055, F1_5040_5080, F1_5170_5230, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO},
+
+	/* UNI-1 even */
+	{MKK3, MKK, NO_DFS, PSCAN_MKK3, DISALLOW_ADHOC_11A_TURB,
+	 BM(F4_5180_5240, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA4_5190_5230, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	/* UNI-1 even + UNI-2 */
+	{MKK4, MKK, DFS_MKK4, PSCAN_MKK3, DISALLOW_ADHOC_11A_TURB,
+	 BM(F4_5180_5240, F2_5260_5320, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA4_5190_5230, NA2_5270_5310, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	/* UNI-1 even + UNI-2 + mid-band */
+	{MKK5, MKK, DFS_MKK4, PSCAN_MKK3, DISALLOW_ADHOC_11A_TURB,
+	 BM(F4_5180_5240, F2_5260_5320, F4_5500_5700, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA4_5190_5230, NA2_5270_5310, NA1_5510_5670, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	/* UNI-1 odd + even */
+	{MKK6, MKK, DFS_MKK4, PSCAN_MKK1, DISALLOW_ADHOC_11A_TURB,
+	 BM(F2_5170_5230, F4_5180_5240, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA4_5190_5230, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	/* UNI-1 odd + UNI-1 even + UNI-2 */
+	{MKK7, MKK, DFS_MKK4, PSCAN_MKK1 | PSCAN_MKK3 , DISALLOW_ADHOC_11A_TURB,
+	 BM(F2_5170_5230, F4_5180_5240, F2_5260_5320, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA4_5190_5230, NA2_5270_5310, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+	/* UNI-1 odd + UNI-1 even + UNI-2 + mid-band */
+	{MKK8, MKK, DFS_MKK4, PSCAN_MKK1 | PSCAN_MKK3 , DISALLOW_ADHOC_11A_TURB,
+	 BM(F2_5170_5230, F4_5180_5240, F2_5260_5320, F4_5500_5700, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(NA4_5190_5230, NA2_5270_5310, NA1_5510_5670, -1, -1, -1, -1, -1, -1, -1, -1, -1)},
+
+    /* UNI-1 even + 4.9 GHZ */
+    {MKK9, MKK, NO_DFS, NO_PSCAN, DISALLOW_ADHOC_11A_TURB,
+     BM(F1_4915_4925, F1_4935_4945, F1_4920_4980, F1_5035_5040, F1_5055_5055, F1_5040_5080, F4_5180_5240, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO},
+
+    /* UNI-1 even + UNI-2 + 4.9 GHZ */
+	{MKK10, MKK, DFS_MKK4, PSCAN_MKK3, DISALLOW_ADHOC_11A_TURB,
+	 BM(F1_4915_4925, F1_4935_4945, F1_4920_4980, F1_5035_5040, F1_5055_5055, F1_5040_5080, F4_5180_5240, F2_5260_5320, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO},
+
+	/* UNI-1 even + UNI-2 + 4.9 GHZ + mid-band */
+	{MKK11, MKK, DFS_MKK4, PSCAN_MKK3, DISALLOW_ADHOC_11A_TURB,
+	 BM(F1_4915_4925, F1_4935_4945, F1_4920_4980, F1_5035_5040, F1_5055_5055, F1_5040_5080, F4_5180_5240, F2_5260_5320, F4_5500_5700, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO},
+
+	/* UNI-1 even + UNI-1 odd + UNI-2 + 4.9 GHZ + mid-band */
+	{MKK12, MKK, DFS_MKK4, PSCAN_MKK3, DISALLOW_ADHOC_11A_TURB,
+	 BM(F1_4915_4925, F1_4935_4945, F1_4920_4980, F1_5035_5040, F1_5055_5055, F1_5040_5080, F1_5170_5230, F4_5180_5240, F2_5260_5320, F4_5500_5700, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO},
+
+	/* Defined here to use when 2G channels are authorised for country K2 */
+	{APLD, NO_CTL, NO_DFS, NO_PSCAN, NO_REQ,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(F2_2312_2372,F2_2412_2472, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(G2_2312_2372,G2_2412_2472, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BMZERO},
+
+	{ETSIA, NO_CTL, NO_DFS, PSCAN_ETSIA, DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(F1_2457_2472,-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(G1_2457_2472,-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(T2_2437_2437,-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO},
+
+	{ETSIB, ETSI, NO_DFS, PSCAN_ETSIB, DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(F1_2432_2442,-1,-1,-1,-1,-1,-1,-1, -1, -1, -1, -1),
+	 BM(G1_2432_2442,-1,-1,-1,-1,-1,-1,-1, -1, -1, -1, -1),
+	 BM(T2_2437_2437,-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO},
+
+	{ETSIC, ETSI, NO_DFS, PSCAN_ETSIC, DISALLOW_ADHOC_11A | DISALLOW_ADHOC_11A_TURB,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(F3_2412_2472,-1,-1,-1,-1,-1,-1,-1, -1, -1, -1, -1),
+	 BM(G3_2412_2472,-1,-1,-1,-1,-1,-1,-1, -1, -1, -1, -1),
+	 BM(T2_2437_2437,-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO},
+
+	{FCCA, FCC, NO_DFS, NO_PSCAN, NO_REQ,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(F1_2412_2462,-1,-1,-1,-1,-1,-1,-1, -1, -1, -1, -1),
+	 BM(G1_2412_2462,-1,-1,-1,-1,-1,-1,-1, -1, -1, -1, -1),
+	 BM(T2_2437_2437,-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(NG2_2422_2452,-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO},
+
+	{MKKA, MKK, NO_DFS, PSCAN_MKKA | PSCAN_MKKA_G | PSCAN_MKKA1 | PSCAN_MKKA1_G | PSCAN_MKKA2 | PSCAN_MKKA2_G, DISALLOW_ADHOC_11A_TURB,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(F2_2412_2462, F1_2467_2472, F2_2484_2484, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(G2_2412_2462, G1_2467_2472, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(T2_2437_2437,-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(NG1_2422_2452,-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO},
+
+	{MKKC, MKK, NO_DFS, NO_PSCAN, NO_REQ,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(F2_2412_2472,-1,-1,-1,-1,-1,-1,-1, -1, -1, -1, -1),
+	 BM(G2_2412_2472,-1,-1,-1,-1,-1,-1,-1, -1, -1, -1, -1),
+	 BM(T2_2437_2437,-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(NG1_2422_2452,-1,-1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO},
+
+	{WORLD, ETSI, NO_DFS, NO_PSCAN, NO_REQ,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BM(F2_2412_2472,-1,-1,-1,-1,-1,-1,-1, -1, -1, -1, -1),
+	 BM(G2_2412_2472,-1,-1,-1,-1,-1,-1,-1, -1, -1, -1, -1),
+	 BM(T2_2437_2437,-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(NG1_2422_2452,-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO},
+
+	{WOR0_WORLD, NO_CTL, DFS_FCC3 | DFS_ETSI, PSCAN_WWR, ADHOC_PER_11D,
+	 BM(W1_5260_5320, W1_5180_5240, W1_5170_5230, W1_5745_5825, W1_5500_5700, -1, -1, -1, -1, -1, -1, -1),
+	 BM(WT1_5210_5250, WT1_5290_5290, WT1_5760_5800, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BM(W1_2412_2412,W1_2437_2442,W1_2462_2462,W1_2472_2472,W1_2417_2432, W1_2447_2457, W1_2467_2467, W1_2484_2484, -1, -1, -1, -1),
+	 BM(WG1_2412_2412,WG1_2437_2442,WG1_2462_2462,WG1_2472_2472,WG1_2417_2432,WG1_2447_2457,WG1_2467_2467, -1, -1, -1, -1, -1),
+	 BM(T3_2437_2437, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO},
+
+	{WOR01_WORLD, NO_CTL, DFS_FCC3 | DFS_ETSI, PSCAN_WWR, ADHOC_PER_11D,
+	 BM(W1_5260_5320, W1_5180_5240, W1_5170_5230, W1_5745_5825, W1_5500_5700, -1, -1, -1, -1, -1, -1, -1),
+	 BM(WT1_5210_5250, WT1_5290_5290, WT1_5760_5800, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BM(W1_2412_2412, W1_2437_2442, W1_2462_2462, W1_2417_2432, W1_2447_2457, -1, -1, -1, -1, -1, -1, -1),
+	 BM(WG1_2412_2412, WG1_2437_2442, WG1_2462_2462, WG1_2417_2432, WG1_2447_2457, -1, -1, -1, -1, -1, -1, -1),
+	 BM(T3_2437_2437, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO},
+
+	{WOR02_WORLD, NO_CTL, DFS_FCC3 | DFS_ETSI, PSCAN_WWR, ADHOC_PER_11D,
+	 BM(W1_5260_5320, W1_5180_5240,W1_5170_5230,W1_5745_5825,W1_5500_5700, -1, -1, -1, -1, -1, -1, -1),
+	 BM(WT1_5210_5250, WT1_5290_5290, WT1_5760_5800, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BM(W1_2412_2412,W1_2437_2442,W1_2462_2462, W1_2472_2472,W1_2417_2432, W1_2447_2457, W1_2467_2467, -1, -1, -1, -1, -1),
+	 BM(WG1_2412_2412,WG1_2437_2442,WG1_2462_2462, WG1_2472_2472,WG1_2417_2432, WG1_2447_2457, WG1_2467_2467, -1, -1, -1, -1, -1),
+	 BM(T3_2437_2437, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO},
+
+	{EU1_WORLD, NO_CTL, DFS_FCC3 | DFS_ETSI, PSCAN_WWR, ADHOC_PER_11D,
+	 BM(W1_5260_5320, W1_5180_5240,W1_5170_5230,W1_5745_5825,W1_5500_5700, -1, -1, -1, -1, -1, -1, -1),
+	 BM(WT1_5210_5250, WT1_5290_5290, WT1_5760_5800, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BM(W1_2412_2412,W1_2437_2442,W1_2462_2462, W2_2472_2472,W1_2417_2432, W1_2447_2457, W2_2467_2467, -1, -1, -1, -1, -1),
+	 BM(WG1_2412_2412,WG1_2437_2442,WG1_2462_2462, WG2_2472_2472,WG1_2417_2432, WG1_2447_2457, WG2_2467_2467, -1, -1, -1, -1, -1),
+	 BM(T3_2437_2437, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO},
+
+	{WOR1_WORLD, NO_CTL, DFS_FCC3 | DFS_ETSI, PSCAN_WWR, ADHOC_NO_11A,
+	 BM(W1_5260_5320, W1_5180_5240, W1_5170_5230, W1_5745_5825, W1_5500_5700, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BM(W1_2412_2412,W1_2437_2442,W1_2462_2462,W1_2472_2472,W1_2417_2432, W1_2447_2457, W1_2467_2467, W1_2484_2484, -1, -1, -1, -1),
+	 BM(WG1_2412_2412,WG1_2437_2442,WG1_2462_2462,WG1_2472_2472,WG1_2417_2432,WG1_2447_2457,WG1_2467_2467, -1, -1, -1, -1, -1),
+	 BM(T3_2437_2437, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO},
+
+	{WOR2_WORLD, NO_CTL, DFS_FCC3 | DFS_ETSI, PSCAN_WWR, ADHOC_NO_11A,
+	 BM(W1_5260_5320, W1_5180_5240, W1_5170_5230, W1_5745_5825, W1_5500_5700, -1, -1, -1, -1, -1, -1, -1),
+	 BM(WT1_5210_5250, WT1_5290_5290, WT1_5760_5800, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BM(W1_2412_2412,W1_2437_2442,W1_2462_2462,W1_2472_2472,W1_2417_2432, W1_2447_2457, W1_2467_2467, W1_2484_2484, -1, -1, -1, -1),
+	 BM(WG1_2412_2412,WG1_2437_2442,WG1_2462_2462,WG1_2472_2472,WG1_2417_2432,WG1_2447_2457,WG1_2467_2467, -1, -1, -1, -1, -1),
+	 BM(T3_2437_2437, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO},
+
+	{WOR3_WORLD, NO_CTL, DFS_FCC3 | DFS_ETSI, PSCAN_WWR, ADHOC_PER_11D,
+	 BM(W1_5260_5320, W1_5180_5240, W1_5170_5230, W1_5745_5825, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(WT1_5210_5250, WT1_5290_5290, WT1_5760_5800, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BM(W1_2412_2412,W1_2437_2442,W1_2462_2462,W1_2472_2472,W1_2417_2432, W1_2447_2457, W1_2467_2467, -1, -1, -1, -1, -1),
+	 BM(WG1_2412_2412,WG1_2437_2442,WG1_2462_2462,WG1_2472_2472,WG1_2417_2432,WG1_2447_2457,WG1_2467_2467,-1, -1, -1, -1, -1),
+	 BM(T3_2437_2437, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO},
+
+	{WOR4_WORLD, NO_CTL, DFS_FCC3, PSCAN_WWR, ADHOC_NO_11A,
+	 BM(W2_5260_5320, W2_5180_5240, F2_5745_5805, W2_5825_5825, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(WT1_5210_5250, WT1_5290_5290, WT1_5760_5800, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BM(W1_2412_2412,W1_2437_2442,W1_2462_2462, W1_2417_2432,W1_2447_2457,-1, -1, -1, -1, -1, -1, -1),
+	 BM(WG1_2412_2412,WG1_2437_2442,WG1_2462_2462, WG1_2417_2432,WG1_2447_2457,-1, -1, -1, -1, -1, -1, -1),
+	 BM(T3_2437_2437, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO},
+
+	{WOR5_ETSIC, NO_CTL, DFS_FCC3 | DFS_ETSI, PSCAN_WWR, ADHOC_NO_11A,
+	 BM(W1_5260_5320, W2_5180_5240, F6_5745_5825, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BM(W1_2412_2412, W1_2437_2442, W1_2462_2462, W2_2472_2472, W1_2417_2432, W1_2447_2457, W2_2467_2467, -1, -1, -1, -1, -1),
+	 BM(WG1_2412_2412, WG1_2437_2442, WG1_2462_2462, WG1_2472_2472, WG1_2417_2432, WG1_2447_2457, WG1_2467_2467, -1, -1, -1, -1, -1),
+	 BM(T3_2437_2437, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO},
+
+	{WOR9_WORLD, NO_CTL, DFS_FCC3 | DFS_ETSI, PSCAN_WWR, ADHOC_NO_11A,
+	 BM(W1_5260_5320, W1_5180_5240, W1_5745_5825, W1_5500_5700, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BM(WT1_5210_5250, WT1_5290_5290, WT1_5760_5800, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BM(W1_2412_2412, W1_2437_2442, W1_2462_2462, W1_2417_2432, W1_2447_2457, -1, -1, -1, -1, -1, -1, -1),
+	 BM(WG1_2412_2412, WG1_2437_2442, WG1_2462_2462, WG1_2417_2432, WG1_2447_2457, -1, -1, -1, -1, -1, -1, -1),
+	 BM(T3_2437_2437, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO},
+
+	{WORA_WORLD, NO_CTL, DFS_FCC3 | DFS_ETSI, PSCAN_WWR, ADHOC_NO_11A,
+	 BM(W1_5260_5320, W1_5180_5240, W1_5745_5825, W1_5500_5700, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO,
+	 BM(W1_2412_2412, W1_2437_2442, W1_2462_2462, W1_2472_2472, W1_2417_2432, W1_2447_2457, W1_2467_2467, -1, -1, -1, -1, -1),
+	 BM(WG1_2412_2412, WG1_2437_2442, WG1_2462_2462, WG1_2472_2472, WG1_2417_2432, WG1_2447_2457, WG1_2467_2467, -1, -1, -1, -1, -1),
+	 BM(T3_2437_2437, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1),
+	 BMZERO,
+	 BMZERO},
+
+	{NULL1, NO_CTL, NO_DFS, NO_PSCAN, NO_REQ,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO,
+	 BMZERO},
+};
+
+struct cmode {
+	u16_t	mode;
+	u32_t	flags;
+};
+
+static const struct cmode modes[] = {
+	{ HAL_MODE_TURBO,	CHANNEL_ST},	/* TURBO means 11a Static Turbo */
+	{ HAL_MODE_11A,		CHANNEL_A},
+	{ HAL_MODE_11B,		CHANNEL_B},
+	{ HAL_MODE_11G,		CHANNEL_G},
+	{ HAL_MODE_11G_TURBO,	CHANNEL_108G},
+	{ HAL_MODE_11A_TURBO,	CHANNEL_108A},
+	{ HAL_MODE_11NA,	CHANNEL_A_HT40},
+	{ HAL_MODE_11NA,	CHANNEL_A_HT20},
+	{ HAL_MODE_11NG,	CHANNEL_G_HT40},
+	{ HAL_MODE_11NG,	CHANNEL_G_HT20},
+};
+
+/*
+ * Return the Wireless Mode Regulatory Domain based
+ * on the country code and the wireless mode.
+ */
+u8_t GetWmRD(u16_t regionCode, u16_t channelFlag, REG_DOMAIN *rd)
+{
+	s16_t i, found, regDmn;
+	u64_t flags=NO_REQ;
+	REG_DMN_PAIR_MAPPING *regPair=NULL;
+
+	for (i=0, found=0; (i<N(regDomainPairs))&&(!found); i++)
+	{
+		if (regDomainPairs[i].regDmnEnum == regionCode)
+		{
+			regPair = &regDomainPairs[i];
+			found = 1;
+		}
+	}
+	if (!found)
+	{
+		zm_debug_msg1("Failed to find reg domain pair ", regionCode);
+		return FALSE;
+	}
+
+	if (channelFlag & ZM_REG_FLAG_CHANNEL_2GHZ)
+	{
+		regDmn = regPair->regDmn2GHz;
+		flags = regPair->flags2GHz;
+	}
+    else
+	{
+		regDmn = regPair->regDmn5GHz;
+		flags = regPair->flags5GHz;
+	}
+
+	/*
+	 * We either started with a unitary reg domain or we've found the
+	 * unitary reg domain of the pair
+	 */
+
+	for (i=0;i<N(regDomains); i++)
+	{
+		if (regDomains[i].regDmnEnum == regDmn)
+		{
+			if (rd != NULL)
+			{
+				zfMemoryCopy((u8_t *)rd, (u8_t *)&regDomains[i],
+					  sizeof(REG_DOMAIN));
+			}
+		}
+	}
+	rd->pscan &= regPair->pscanMask;
+    rd->flags = (u32_t)flags;
+	return TRUE;
+}
+
+/*
+ * Test to see if the bitmask array is all zeros
+ */
+u8_t isChanBitMaskZero(u64_t *bitmask)
+{
+	u16_t i;
+
+	for (i=0; i<BMLEN; i++) {
+		if (bitmask[i] != 0)
+			return FALSE;
+	}
+	return TRUE;
+}
+
+u8_t IS_BIT_SET(u32_t bit, u64_t *bitmask)
+{
+	u32_t byteOffset, bitnum;
+	u64_t val;
+
+	byteOffset = bit/64;
+	bitnum = bit - byteOffset*64;
+	val = ((u64_t) 1) << bitnum;
+	if (bitmask[byteOffset] & val)
+		return TRUE;
+	else
+		return FALSE;
+}
+
+
+void zfHpGetRegulationTable(zdev_t* dev, u16_t regionCode, u16_t c_lo, u16_t c_hi)
+{
+	REG_DOMAIN rd5GHz, rd2GHz;
+	const struct cmode *cm;
+	s16_t next=0,b;
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+    zmw_declare_for_critical_section();
+
+	if (!GetWmRD(regionCode, ~ZM_REG_FLAG_CHANNEL_2GHZ, &rd5GHz))
+	{
+        zm_debug_msg1("couldn't find unitary 5GHz reg domain for Region Code ", regionCode);
+		return;
+	}
+	if (!GetWmRD(regionCode, ZM_REG_FLAG_CHANNEL_2GHZ, &rd2GHz))
+	{
+        zm_debug_msg1("couldn't find unitary 2GHz reg domain for Region Code ", regionCode);
+		return;
+	}
+    if (wd->regulationTable.regionCode == regionCode)
+    {
+        zm_debug_msg1("current region code is the same with Region Code ", regionCode);
+        return;
+    }
+    else
+    {
+        wd->regulationTable.regionCode = regionCode;
+    }
+
+    next = 0;
+
+    zmw_enter_critical_section(dev);
+
+	for (cm = modes; cm < &modes[N(modes)]; cm++)
+	{
+		u16_t c;
+		u64_t *channelBM=NULL;
+		REG_DOMAIN *rd=NULL;
+		REG_DMN_FREQ_BAND *fband=NULL,*freqs=NULL;
+
+		switch (cm->mode)
+		{
+		case HAL_MODE_TURBO:
+		    //we don't have turbo mode so we disable it
+            //zm_debug_msg0("CWY - HAL_MODE_TURBO");
+            channelBM = NULL;
+			//rd = &rd5GHz;
+			//channelBM = rd->chan11a_turbo;
+			//freqs = &regDmn5GhzTurboFreq[0];
+			//ctl = rd->conformanceTestLimit | CTL_TURBO;
+			break;
+		case HAL_MODE_11A:
+		    if ((hpPriv->OpFlags & 0x1) != 0)
+		    {
+    			rd = &rd5GHz;
+    			channelBM = rd->chan11a;
+    			freqs = &regDmn5GhzFreq[0];
+    			c_lo = 4920; //from channel 184
+    			c_hi = 5825; //to   channel 165
+    			//ctl = rd->conformanceTestLimit;
+                //zm_debug_msg2("CWY - HAL_MODE_11A, channelBM = 0x", *channelBM);
+            }
+            //else
+            {
+                //channelBM = NULL;
+            }
+			break;
+		case HAL_MODE_11B:
+		    //Disable 11B mode because it only has difference with 11G in PowerDFS Data,
+		    //and we don't use this now.
+			//zm_debug_msg0("CWY - HAL_MODE_11B");
+			channelBM = NULL;
+			//rd = &rd2GHz;
+			//channelBM = rd->chan11b;
+			//freqs = &regDmn2GhzFreq[0];
+			//ctl = rd->conformanceTestLimit | CTL_11B;
+            //zm_debug_msg2("CWY - HAL_MODE_11B, channelBM = 0x", *channelBM);
+			break;
+		case HAL_MODE_11G:
+		    if ((hpPriv->OpFlags & 0x2) != 0)
+		    {
+    			rd = &rd2GHz;
+    			channelBM = rd->chan11g;
+    			freqs = &regDmn2Ghz11gFreq[0];
+    			c_lo = 2412; //from channel  1
+    			//c_hi = 2462; //to   channel 11
+                c_hi = 2472; //to   channel 13
+    			//ctl = rd->conformanceTestLimit | CTL_11G;
+                //zm_debug_msg2("CWY - HAL_MODE_11G, channelBM = 0x", *channelBM);
+            }
+            //else
+            {
+                //channelBM = NULL;
+            }
+			break;
+		case HAL_MODE_11G_TURBO:
+		    //we don't have turbo mode so we disable it
+            //zm_debug_msg0("CWY - HAL_MODE_11G_TURBO");
+            channelBM = NULL;
+			//rd = &rd2GHz;
+			//channelBM = rd->chan11g_turbo;
+			//freqs = &regDmn2Ghz11gTurboFreq[0];
+			//ctl = rd->conformanceTestLimit | CTL_108G;
+			break;
+		case HAL_MODE_11A_TURBO:
+		    //we don't have turbo mode so we disable it
+            //zm_debug_msg0("CWY - HAL_MODE_11A_TURBO");
+            channelBM = NULL;
+			//rd = &rd5GHz;
+			//channelBM = rd->chan11a_dyn_turbo;
+			//freqs = &regDmn5GhzTurboFreq[0];
+			//ctl = rd->conformanceTestLimit | CTL_108G;
+			break;
+		default:
+            zm_debug_msg1("Unkonwn HAL mode ", cm->mode);
+			continue;
+		}
+		if (channelBM == NULL)
+		{
+		    //zm_debug_msg0("CWY - channelBM is NULL");
+		    continue;
+        }
+        if (isChanBitMaskZero(channelBM))
+        {
+	        //zm_debug_msg0("CWY - BitMask is Zero");
+	        continue;
+        }
+
+        // RAY:Is it ok??
+        if (freqs == NULL )
+        {
+            continue;
+        }
+
+		for (b=0;b<64*BMLEN; b++)
+		{
+			if (IS_BIT_SET(b,channelBM))
+			{
+				fband = &freqs[b];
+
+				//zm_debug_msg1("CWY - lowChannel = ", fband->lowChannel);
+				//zm_debug_msg1("CWY - highChannel = ", fband->highChannel);
+				//zm_debug_msg1("CWY - channelSep = ", fband->channelSep);
+				for (c=fband->lowChannel; c <= fband->highChannel;
+				     c += fband->channelSep)
+				{
+					ZM_HAL_CHANNEL icv;
+
+                    //Disable all DFS channel
+                    if ((hpPriv->disableDfsCh==0) || (!(fband->useDfs & rd->dfsMask)))
+                    {
+                        if( fband->channelBW < 20 )
+                        {
+                        	/**************************************************************/
+                            /*                                                            */
+                            /*   Temporary discard channel that BW < 20MHz (5 or 10MHz)   */
+                            /*   Our architecture does not implemnt it !!!                */
+                            /*                                                            */
+                            /**************************************************************/
+                            continue;
+                        }
+					if ((c >= c_lo) && (c <= c_hi))
+					{
+					    icv.channel = c;
+					    icv.channelFlags = cm->flags;
+					    icv.maxRegTxPower = fband->powerDfs;
+					    if (fband->usePassScan & rd->pscan)
+					    	icv.channelFlags |= ZM_REG_FLAG_CHANNEL_PASSIVE;
+					    else
+					    	icv.channelFlags &= ~ZM_REG_FLAG_CHANNEL_PASSIVE;
+					    if (fband->useDfs & rd->dfsMask)
+					    	icv.privFlags = ZM_REG_FLAG_CHANNEL_DFS;
+					    else
+					    	icv.privFlags = 0;
+
+					    /* For now disable radar for FCC3 */
+					    if (fband->useDfs & rd->dfsMask & DFS_FCC3)
+					    {
+					    	icv.privFlags &= ~ZM_REG_FLAG_CHANNEL_DFS;
+					    	icv.privFlags |= ZM_REG_FLAG_CHANNEL_DFS_CLEAR;
+					    }
+
+					    if(rd->flags & LIMIT_FRAME_4MS)
+					    	icv.privFlags |= ZM_REG_FLAG_CHANNEL_DFS_CLEAR;
+
+					    icv.minTxPower = 0;
+					    icv.maxTxPower = 0;
+
+        			    zm_assert(next < 60);
+
+					    wd->regulationTable.allowChannel[next++] = icv;
+				    }
+				}
+			}
+		}
+	}
+	}
+	wd->regulationTable.allowChannelCnt = next;
+
+    #if 0
+    {
+        /* debug print */
+        u32_t i;
+        DbgPrint("\n-------------------------------------------\n");
+        DbgPrint("zfHpGetRegulationTable print all channel info regincode = 0x%x\n", wd->regulationTable.regionCode);
+        DbgPrint("index  channel  channelFlags   maxRegTxPower  privFlags  useDFS\n");
+
+        for (i=0; i<wd->regulationTable.allowChannelCnt; i++)
+        {
+            DbgPrint("%02d       %d         %04x           %02d        %x     %x\n",
+                      i,
+                      wd->regulationTable.allowChannel[i].channel,
+                      wd->regulationTable.allowChannel[i].channelFlags,
+                      wd->regulationTable.allowChannel[i].maxRegTxPower,
+                      wd->regulationTable.allowChannel[i].privFlags,
+                      wd->regulationTable.allowChannel[i].privFlags & ZM_REG_FLAG_CHANNEL_DFS);
+        }
+    }
+    #endif
+
+    zmw_leave_critical_section(dev);
+}
+
+void zfHpGetRegulationTablefromRegionCode(zdev_t* dev, u16_t regionCode)
+{
+    u16_t c_lo = 2000, c_hi = 6000; //default channel is all enable
+    u8_t isoName[3] = {'N', 'A', 0};
+
+    zfCoreSetIsoName(dev, isoName);
+
+    zfHpGetRegulationTable(dev, regionCode, c_lo, c_hi);
+}
+
+void zfHpGetRegulationTablefromCountry(zdev_t* dev, u16_t CountryCode)
+{
+    u16_t i;
+    u16_t c_lo = 2000, c_hi = 6000; //default channel is all enable
+    u16_t RegDomain;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    for (i = 0; i < N(allCountries); i++)
+    {
+        if (CountryCode == allCountries[i].countryCode)
+        {
+            RegDomain = allCountries[i].regDmnEnum;
+
+            // read the ACU country code from EEPROM
+            zfCoreSetIsoName(dev, (u8_t*)allCountries[i].isoName);
+
+            //zm_debug_msg_s("CWY - Country Name = ", allCountries[i].name);
+
+            if (wd->regulationTable.regionCode != RegDomain)
+            {
+                //zm_debug_msg0("CWY - Change regulatory table");
+
+                zfHpGetRegulationTable(dev, RegDomain, c_lo, c_hi);
+            }
+            return;
+        }
+    }
+    zm_debug_msg1("Invalid CountryCode = ", CountryCode);
+}
+
+u8_t zfHpGetRegulationTablefromISO(zdev_t* dev, u8_t *countryInfo, u8_t length)
+{
+    u16_t i;
+    u16_t RegDomain;
+    u16_t c_lo = 2000, c_hi = 6000; //default channel is all enable
+    //u8_t strLen = 2;
+
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    if (countryInfo[4] != 0x20)
+    { // with (I)ndoor/(O)utdoor info
+        //strLen = 3;
+    }
+    //zm_debug_msg_s("Desired iso name = ", isoName);
+    for (i = 0; i < N(allCountries); i++)
+    {
+        //zm_debug_msg_s("Current iso name = ", allCountries[i].isoName);
+        if (zfMemoryIsEqual((u8_t *)allCountries[i].isoName, (u8_t *)&countryInfo[2], length-1))
+        {
+            //DbgPrint("Set current iso name = %s\n", allCountries[i].isoName);
+            //zm_debug_msg0("iso name hit!!");
+
+            RegDomain = allCountries[i].regDmnEnum;
+
+            if (wd->regulationTable.regionCode != RegDomain)
+            {
+                zfHpGetRegulationTable(dev, RegDomain, c_lo, c_hi);
+            }
+
+            //while (index < (countryInfo[1]+2))
+            //{
+            //  if (countryInfo[index] <= 14)
+            //  {
+            //        /* calculate 2.4GHz low boundary channel frequency */
+            //        ch = countryInfo[index];
+            //        if ( ch == 14 )
+            //            c_lo = ZM_CH_G_14;
+            //        else
+            //            c_lo = ZM_CH_G_1 + (ch - 1) * 5;
+            //        /* calculate 2.4GHz high boundary channel frequency */
+            //        ch = countryInfo[index] + countryInfo[index + 1] - 1;
+            //        if ( ch == 14 )
+            //            c_hi = ZM_CH_G_14;
+            //        else
+            //            c_hi = ZM_CH_G_1 + (ch - 1) * 5;
+            //  }
+            //  else
+            //  {
+            //        /* calculate 5GHz low boundary channel frequency */
+            //        ch = countryInfo[index];
+            //        if ( (ch >= 184)&&(ch <= 196) )
+            //            c_lo = 4000 + ch*5;
+            //        else
+            //            c_lo = 5000 + ch*5;
+            //        /* calculate 5GHz high boundary channel frequency */
+            //        ch = countryInfo[index] + countryInfo[index + 1] - 1;
+            //        if ( (ch >= 184)&&(ch <= 196) )
+            //            c_hi = 4000 + ch*5;
+            //        else
+            //            c_hi = 5000 + ch*5;
+            //  }
+            //
+            //  zfHpGetRegulationTable(dev, RegDomain, c_lo, c_hi);
+            //
+            //  index+=3;
+            //}
+
+            return 0;
+        }
+    }
+    //zm_debug_msg_s("Invalid iso name = ", &countryInfo[2]);
+    return 1;
+}
+
+const char* zfHpGetisoNamefromregionCode(zdev_t* dev, u16_t regionCode)
+{
+    u16_t i;
+
+    for (i = 0; i < N(allCountries); i++)
+    {
+        if (allCountries[i].regDmnEnum == regionCode)
+        {
+            return allCountries[i].isoName;
+        }
+    }
+    /* no matching item, return default */
+    return allCountries[0].isoName;
+}
+
+u16_t zfHpGetRegionCodeFromIsoName(zdev_t* dev, u8_t *countryIsoName)
+{
+    u16_t i;
+    u16_t regionCode;
+
+    /* if no matching item, return default */
+    regionCode = DEF_REGDMN;
+
+    for (i = 0; i < N(allCountries); i++)
+    {
+        if (zfMemoryIsEqual((u8_t *)allCountries[i].isoName, countryIsoName, 2))
+        {
+            regionCode = allCountries[i].regDmnEnum;
+            break;
+        }
+    }
+
+    return regionCode;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfHpDeleteAllowChannel      */
+/*      Delete Allow Channel.                                           */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev  : device pointer                                           */
+/*      freq : frequency                                                */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : success                                                     */
+/*      other : fail                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Chao-Wen Yang         ZyDAS Technology Corporation    2007.3    */
+/*                                                                      */
+/************************************************************************/
+u16_t zfHpDeleteAllowChannel(zdev_t* dev, u16_t freq)
+{
+    u16_t i, bandIndex = 0;
+    u16_t dfs5GBand[][2] = {{5150, 5240}, {5260, 5350}, {5450, 5700}, {5725, 5825}};
+
+    zmw_get_wlan_dev(dev);
+    /* Find which band does this frequency belong */
+    for (i = 0; i < 4; i++)
+    {
+        if ((freq >= dfs5GBand[i][0]) && (freq <= dfs5GBand[i][1]))
+            bandIndex = i + 1;
+    }
+
+    if (bandIndex == 0)
+    {
+        /* 2.4G, don't care */
+        return 0;
+    }
+    else
+    {
+        bandIndex--;
+    }
+    /* Set all channels in this band to passive scan */
+    for (i = 0; i < wd->regulationTable.allowChannelCnt; i++)
+    {
+        if ((wd->regulationTable.allowChannel[i].channel >= dfs5GBand[bandIndex][0]) &&
+            (wd->regulationTable.allowChannel[i].channel <= dfs5GBand[bandIndex][1]))
+        {
+            /* if channel is not passive, set it to be passive and mark it */
+            if ((wd->regulationTable.allowChannel[i].channelFlags &
+                    ZM_REG_FLAG_CHANNEL_PASSIVE) == 0)
+            {
+                wd->regulationTable.allowChannel[i].channelFlags |=
+                        (ZM_REG_FLAG_CHANNEL_PASSIVE | ZM_REG_FLAG_CHANNEL_CSA);
+            }
+        }
+    }
+
+    return 0;
+}
+
+u16_t zfHpAddAllowChannel(zdev_t* dev, u16_t freq)
+{
+    u16_t i, j, arrayIndex;
+
+    zmw_get_wlan_dev(dev);
+
+    for (i = 0; i < wd->regulationTable.allowChannelCnt; i++)
+    {
+        if (wd->regulationTable.allowChannel[i].channel == freq)
+            break;
+    }
+
+    if ( i == wd->regulationTable.allowChannelCnt)
+    {
+        for (j = 0; j < wd->regulationTable.allowChannelCnt; j++)
+        {
+            if (wd->regulationTable.allowChannel[j].channel > freq)
+                break;
+        }
+
+        //zm_debug_msg1("CWY - add frequency = ", freq);
+        //zm_debug_msg1("CWY - channel array index = ", j);
+
+        arrayIndex = j;
+
+        if (arrayIndex < wd->regulationTable.allowChannelCnt)
+        {
+            for (j = wd->regulationTable.allowChannelCnt; j > arrayIndex; j--)
+                wd->regulationTable.allowChannel[j] = wd->regulationTable.allowChannel[j - 1];
+        }
+        wd->regulationTable.allowChannel[arrayIndex].channel = freq;
+
+        wd->regulationTable.allowChannelCnt++;
+    }
+
+    return 0;
+}
+
+u16_t zfHpIsDfsChannelNCS(zdev_t* dev, u16_t freq)
+{
+    u8_t flag = ZM_REG_FLAG_CHANNEL_DFS;
+    u16_t i;
+    zmw_get_wlan_dev(dev);
+
+    for (i = 0; i < wd->regulationTable.allowChannelCnt; i++)
+    {
+        //DbgPrint("DFS:freq=%d, chan=%d", freq, wd->regulationTable.allowChannel[i].channel);
+        if (wd->regulationTable.allowChannel[i].channel == freq)
+        {
+            flag = wd->regulationTable.allowChannel[i].privFlags;
+            break;
+        }
+    }
+
+    return (flag & (ZM_REG_FLAG_CHANNEL_DFS|ZM_REG_FLAG_CHANNEL_DFS_CLEAR));
+}
+
+u16_t zfHpIsDfsChannel(zdev_t* dev, u16_t freq)
+{
+    u8_t flag = ZM_REG_FLAG_CHANNEL_DFS;
+    u16_t i;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    for (i = 0; i < wd->regulationTable.allowChannelCnt; i++)
+    {
+        //DbgPrint("DFS:freq=%d, chan=%d", freq, wd->regulationTable.allowChannel[i].channel);
+        if (wd->regulationTable.allowChannel[i].channel == freq)
+        {
+            flag = wd->regulationTable.allowChannel[i].privFlags;
+            break;
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return (flag & (ZM_REG_FLAG_CHANNEL_DFS|ZM_REG_FLAG_CHANNEL_DFS_CLEAR));
+}
+
+u16_t zfHpIsAllowedChannel(zdev_t* dev, u16_t freq)
+{
+    u16_t i;
+    zmw_get_wlan_dev(dev);
+
+    for (i = 0; i < wd->regulationTable.allowChannelCnt; i++)
+    {
+        if (wd->regulationTable.allowChannel[i].channel == freq)
+        {
+            return 1;
+        }
+    }
+
+    return 0;
+}
+
+u16_t zfHpFindFirstNonDfsChannel(zdev_t* dev, u16_t aBand)
+{
+    u16_t chan = 2412;
+    u16_t i;
+    zmw_get_wlan_dev(dev);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    for (i = 0; i < wd->regulationTable.allowChannelCnt; i++)
+    {
+        if ((wd->regulationTable.allowChannel[i].privFlags & ZM_REG_FLAG_CHANNEL_DFS) != 0)
+        {
+            if (aBand)
+            {
+                if (wd->regulationTable.allowChannel[i].channel > 3000)
+                {
+                    chan = wd->regulationTable.allowChannel[i].channel;
+                    break;
+                }
+            }
+            else
+            {
+                if (wd->regulationTable.allowChannel[i].channel < 3000)
+                {
+                    chan = wd->regulationTable.allowChannel[i].channel;
+                    break;
+                }
+            }
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+
+    return chan;
+}
+
+
+/* porting from ACU */
+/* save RegulatoryDomain in hpriv */
+u8_t zfHpGetRegulatoryDomain(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+
+    switch (wd->regulationTable.regionCode)
+    {
+        case NO_ENUMRD:
+            return 0;
+            break;
+        case FCC1_FCCA:
+        case FCC1_WORLD:
+        case FCC4_FCCA:
+        case FCC5_FCCA:
+        case FCC2_WORLD:
+        case FCC2_ETSIC:
+        case FCC3_FCCA:
+        case FCC3_WORLD:
+        case FCC1:
+        case FCC2:
+        case FCC3:
+        case FCC4:
+        case FCC5:
+        case FCCA:
+            return 0x10;//WG_AMERICAS DOT11_REG_DOMAIN_FCC  United States
+            break;
+
+        case FCC2_FCCA:
+            return 0x20;//DOT11_REG_DOMAIN_DOC  Canada
+            break;
+
+        case ETSI1_WORLD:
+        case ETSI3_ETSIA:
+        case ETSI2_WORLD:
+        case ETSI3_WORLD:
+        case ETSI4_WORLD:
+        case ETSI4_ETSIC:
+        case ETSI5_WORLD:
+        case ETSI6_WORLD:
+        case ETSI_RESERVED:
+        case ETSI1:
+        case ETSI2:
+        case ETSI3:
+        case ETSI4:
+        case ETSI5:
+        case ETSI6:
+        case ETSIA:
+        case ETSIB:
+        case ETSIC:
+            return 0x30;//WG_EMEA DOT11_REG_DOMAIN_ETSI  Most of Europe
+            break;
+
+        case MKK1_MKKA:
+        case MKK1_MKKB:
+        case MKK2_MKKA:
+        case MKK1_FCCA:
+        case MKK1_MKKA1:
+        case MKK1_MKKA2:
+        case MKK1_MKKC:
+        case MKK3_MKKB:
+        case MKK3_MKKA2:
+        case MKK3_MKKC:
+        case MKK4_MKKB:
+        case MKK4_MKKA2:
+        case MKK4_MKKC:
+        case MKK5_MKKB:
+        case MKK5_MKKA2:
+        case MKK5_MKKC:
+        case MKK6_MKKB:
+        case MKK6_MKKA2:
+        case MKK6_MKKC:
+        case MKK7_MKKB:
+        case MKK7_MKKA:
+        case MKK7_MKKC:
+        case MKK8_MKKB:
+        case MKK8_MKKA2:
+        case MKK8_MKKC:
+        case MKK6_MKKA1:
+        case MKK6_FCCA:
+        case MKK7_MKKA1:
+        case MKK7_FCCA:
+        case MKK9_FCCA:
+        case MKK9_MKKA1:
+        case MKK9_MKKC:
+        case MKK9_MKKA2:
+        case MKK10_FCCA:
+        case MKK10_MKKA1:
+        case MKK10_MKKC:
+        case MKK10_MKKA2:
+        case MKK11_MKKA:
+        case MKK11_FCCA:
+        case MKK11_MKKA1:
+        case MKK11_MKKC:
+        case MKK11_MKKA2:
+        case MKK12_MKKA:
+        case MKK12_FCCA:
+        case MKK12_MKKA1:
+        case MKK12_MKKC:
+        case MKK12_MKKA2:
+        case MKK3_MKKA:
+        case MKK3_MKKA1:
+        case MKK3_FCCA:
+        case MKK4_MKKA:
+        case MKK4_MKKA1:
+        case MKK4_FCCA:
+        case MKK9_MKKA:
+        case MKK10_MKKA:
+        case MKK1:
+        case MKK2:
+        case MKK3:
+        case MKK4:
+        case MKK5:
+        case MKK6:
+        case MKK7:
+        case MKK8:
+        case MKK9:
+        case MKK10:
+        case MKK11:
+        case MKK12:
+        case MKKA:
+        case MKKC:
+            return 0x40;//WG_JAPAN DOT11_REG_DOMAIN_MKK  Japan
+            break;
+
+        default:
+            break;
+    }
+    return 0xFF;// Didn't input RegDmn by mean to distinguish by customer
+
+}
+
+
+void zfHpDisableDfsChannel(zdev_t* dev, u8_t disableFlag)
+{
+    zmw_get_wlan_dev(dev);
+
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+    hpPriv->disableDfsCh = disableFlag;
+    return;
+}
diff --git a/drivers/staging/otus/hal/hpreg.h b/drivers/staging/otus/hal/hpreg.h
new file mode 100644
index 00000000000..6f8c73fd42c
--- /dev/null
+++ b/drivers/staging/otus/hal/hpreg.h
@@ -0,0 +1,524 @@
+/*
+ * Copyright (c) 2000-2005 ZyDAS Technology Corporation
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*  Module Name : hpreg.h                                               */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains Regulatory Table definitions.              */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+
+#ifndef _HPREG_H
+#define _HPREG_H
+
+typedef u16_t HAL_CTRY_CODE;		/* country code */
+typedef u16_t HAL_REG_DOMAIN;		/* regulatory domain code */
+typedef enum {
+	AH_FALSE = 0,		/* NB: lots of code assumes false is zero */
+	AH_TRUE  = 1,
+} HAL_BOOL;
+
+
+/*
+ * Country/Region Codes from MS WINNLS.H
+ * Numbering from ISO 3166
+ */
+enum CountryCode {
+    CTRY_ALBANIA              = 8,       /* Albania */
+    CTRY_ALGERIA              = 12,      /* Algeria */
+    CTRY_ARGENTINA            = 32,      /* Argentina */
+    CTRY_ARMENIA              = 51,      /* Armenia */
+    CTRY_AUSTRALIA            = 36,      /* Australia */
+    CTRY_AUSTRIA              = 40,      /* Austria */
+    CTRY_AZERBAIJAN           = 31,      /* Azerbaijan */
+    CTRY_BAHRAIN              = 48,      /* Bahrain */
+    CTRY_BELARUS              = 112,     /* Belarus */
+    CTRY_BELGIUM              = 56,      /* Belgium */
+    CTRY_BELIZE               = 84,      /* Belize */
+    CTRY_BOLIVIA              = 68,      /* Bolivia */
+    CTRY_BOSNIA               = 70,      /* Bosnia */
+    CTRY_BRAZIL               = 76,      /* Brazil */
+    CTRY_BRUNEI_DARUSSALAM    = 96,      /* Brunei Darussalam */
+    CTRY_BULGARIA             = 100,     /* Bulgaria */
+    CTRY_CANADA               = 124,     /* Canada */
+    CTRY_CHILE                = 152,     /* Chile */
+    CTRY_CHINA                = 156,     /* People's Republic of China */
+    CTRY_COLOMBIA             = 170,     /* Colombia */
+    CTRY_COSTA_RICA           = 188,     /* Costa Rica */
+    CTRY_CROATIA              = 191,     /* Croatia */
+    CTRY_CYPRUS               = 196,     /* Cyprus */
+    CTRY_CZECH                = 203,     /* Czech Republic */
+    CTRY_DENMARK              = 208,     /* Denmark */
+    CTRY_DOMINICAN_REPUBLIC   = 214,     /* Dominican Republic */
+    CTRY_ECUADOR              = 218,     /* Ecuador */
+    CTRY_EGYPT                = 818,     /* Egypt */
+    CTRY_EL_SALVADOR          = 222,     /* El Salvador */
+    CTRY_ESTONIA              = 233,     /* Estonia */
+    CTRY_FAEROE_ISLANDS       = 234,     /* Faeroe Islands */
+    CTRY_FINLAND              = 246,     /* Finland */
+    CTRY_FRANCE               = 250,     /* France */
+    CTRY_FRANCE2              = 255,     /* France2 */
+    CTRY_GEORGIA              = 268,     /* Georgia */
+    CTRY_GERMANY              = 276,     /* Germany */
+    CTRY_GREECE               = 300,     /* Greece */
+    CTRY_GUATEMALA            = 320,     /* Guatemala */
+    CTRY_HONDURAS             = 340,     /* Honduras */
+    CTRY_HONG_KONG            = 344,     /* Hong Kong S.A.R., P.R.C. */
+    CTRY_HUNGARY              = 348,     /* Hungary */
+    CTRY_ICELAND              = 352,     /* Iceland */
+    CTRY_INDIA                = 356,     /* India */
+    CTRY_INDONESIA            = 360,     /* Indonesia */
+    CTRY_IRAN                 = 364,     /* Iran */
+    CTRY_IRAQ                 = 368,     /* Iraq */
+    CTRY_IRELAND              = 372,     /* Ireland */
+    CTRY_ISRAEL               = 376,     /* Israel */
+    CTRY_ISRAEL2              = 377,     /* Israel2 */
+    CTRY_ITALY                = 380,     /* Italy */
+    CTRY_JAMAICA              = 388,     /* Jamaica */
+    CTRY_JAPAN                = 392,     /* Japan */
+    CTRY_JAPAN1               = 393,     /* Japan (JP1) */
+    CTRY_JAPAN2               = 394,     /* Japan (JP0) */
+    CTRY_JAPAN3               = 395,     /* Japan (JP1-1) */
+    CTRY_JAPAN4               = 396,     /* Japan (JE1) */
+    CTRY_JAPAN5               = 397,     /* Japan (JE2) */
+    CTRY_JAPAN6               = 399,     /* Japan (JP6) */
+
+    CTRY_JAPAN7	              = 4007,    /* Japan (J7) */
+    CTRY_JAPAN8	              = 4008,    /* Japan (J8) */
+    CTRY_JAPAN9               = 4009,    /* Japan (J9) */
+
+    CTRY_JAPAN10              = 4010,    /* Japan (J10) */
+    CTRY_JAPAN11              = 4011,    /* Japan (J11) */
+    CTRY_JAPAN12              = 4012,    /* Japan (J12) */
+
+    CTRY_JAPAN13              = 4013,    /* Japan (J13) */
+    CTRY_JAPAN14              = 4014,    /* Japan (J14) */
+    CTRY_JAPAN15              = 4015,    /* Japan (J15) */
+
+    CTRY_JAPAN16              = 4016,    /* Japan (J16) */
+    CTRY_JAPAN17              = 4017,    /* Japan (J17) */
+    CTRY_JAPAN18              = 4018,    /* Japan (J18) */
+
+    CTRY_JAPAN19              = 4019,    /* Japan (J19) */
+    CTRY_JAPAN20              = 4020,    /* Japan (J20) */
+    CTRY_JAPAN21              = 4021,    /* Japan (J21) */
+
+    CTRY_JAPAN22              = 4022,    /* Japan (J22) */
+    CTRY_JAPAN23              = 4023,    /* Japan (J23) */
+    CTRY_JAPAN24              = 4024,    /* Japan (J24) */
+
+    CTRY_JAPAN25              = 4025,    /* Japan (J25) */
+    CTRY_JAPAN26              = 4026,    /* Japan (J26) */
+    CTRY_JAPAN27              = 4027,    /* Japan (J27) */
+
+    CTRY_JAPAN28              = 4028,    /* Japan (J28) */
+    CTRY_JAPAN29              = 4029,    /* Japan (J29) */
+    CTRY_JAPAN30              = 4030,    /* Japan (J30) */
+
+    CTRY_JAPAN31              = 4031,    /* Japan (J31) */
+    CTRY_JAPAN32              = 4032,    /* Japan (J32) */
+    CTRY_JAPAN33              = 4033,    /* Japan (J33) */
+
+    CTRY_JAPAN34              = 4034,    /* Japan (J34) */
+    CTRY_JAPAN35              = 4035,    /* Japan (J35) */
+    CTRY_JAPAN36              = 4036,    /* Japan (J36) */
+
+    CTRY_JAPAN37              = 4037,    /* Japan (J37) */
+    CTRY_JAPAN38              = 4038,    /* Japan (J38) */
+    CTRY_JAPAN39              = 4039,    /* Japan (J39) */
+
+    CTRY_JAPAN40              = 4040,    /* Japan (J40) */
+    CTRY_JAPAN41              = 4041,    /* Japan (J41) */
+    CTRY_JAPAN42              = 4042,    /* Japan (J42) */
+    CTRY_JAPAN43              = 4043,    /* Japan (J43) */
+    CTRY_JAPAN44              = 4044,    /* Japan (J44) */
+    CTRY_JAPAN45              = 4045,    /* Japan (J45) */
+    CTRY_JAPAN46              = 4046,    /* Japan (J46) */
+    CTRY_JAPAN47              = 4047,    /* Japan (J47) */
+    CTRY_JAPAN48              = 4048,    /* Japan (J48) */
+    CTRY_JAPAN49              = 4049,    /* Japan (J49) */
+
+    CTRY_JAPAN50              = 4050,    /* Japan (J50) */
+    CTRY_JAPAN51              = 4051,    /* Japan (J51) */
+    CTRY_JAPAN52              = 4052,    /* Japan (J52) */
+    CTRY_JAPAN53              = 4053,    /* Japan (J53) */
+    CTRY_JAPAN54              = 4054,    /* Japan (J54) */
+
+    CTRY_JORDAN               = 400,     /* Jordan */
+    CTRY_KAZAKHSTAN           = 398,     /* Kazakhstan */
+    CTRY_KENYA                = 404,     /* Kenya */
+    CTRY_KOREA_NORTH          = 408,     /* North Korea */
+    CTRY_KOREA_ROC            = 410,     /* South Korea */
+    CTRY_KOREA_ROC2           = 411,     /* South Korea */
+    CTRY_KOREA_ROC3           = 412,     /* South Korea */
+    CTRY_KUWAIT               = 414,     /* Kuwait */
+    CTRY_LATVIA               = 428,     /* Latvia */
+    CTRY_LEBANON              = 422,     /* Lebanon */
+    CTRY_LIBYA                = 434,     /* Libya */
+    CTRY_LIECHTENSTEIN        = 438,     /* Liechtenstein */
+    CTRY_LITHUANIA            = 440,     /* Lithuania */
+    CTRY_LUXEMBOURG           = 442,     /* Luxembourg */
+    CTRY_MACAU                = 446,     /* Macau */
+    CTRY_MACEDONIA            = 807,     /* the Former Yugoslav Republic of Macedonia */
+    CTRY_MALAYSIA             = 458,     /* Malaysia */
+    CTRY_MALTA	              = 470,     /* Malta */
+    CTRY_MEXICO               = 484,     /* Mexico */
+    CTRY_MONACO               = 492,     /* Principality of Monaco */
+    CTRY_MOROCCO              = 504,     /* Morocco */
+    CTRY_NETHERLANDS          = 528,     /* Netherlands */
+    CTRY_NETHERLANDS_ANT      = 530,     /* Netherlands-Antellis */
+    CTRY_NEW_ZEALAND          = 554,     /* New Zealand */
+    CTRY_NICARAGUA            = 558,     /* Nicaragua */
+    CTRY_NORWAY               = 578,     /* Norway */
+    CTRY_OMAN                 = 512,     /* Oman */
+    CTRY_PAKISTAN             = 586,     /* Islamic Republic of Pakistan */
+    CTRY_PANAMA               = 591,     /* Panama */
+    CTRY_PARAGUAY             = 600,     /* Paraguay */
+    CTRY_PERU                 = 604,     /* Peru */
+    CTRY_PHILIPPINES          = 608,     /* Republic of the Philippines */
+    CTRY_POLAND               = 616,     /* Poland */
+    CTRY_PORTUGAL             = 620,     /* Portugal */
+    CTRY_PUERTO_RICO          = 630,     /* Puerto Rico */
+    CTRY_QATAR                = 634,     /* Qatar */
+    CTRY_ROMANIA              = 642,     /* Romania */
+    CTRY_RUSSIA               = 643,     /* Russia */
+    CTRY_SAUDI_ARABIA         = 682,     /* Saudi Arabia */
+    CTRY_SERBIA_MONT          = 891,     /* Serbia and Montenegro */
+    CTRY_SINGAPORE            = 702,     /* Singapore */
+    CTRY_SLOVAKIA             = 703,     /* Slovak Republic */
+    CTRY_SLOVENIA             = 705,     /* Slovenia */
+    CTRY_SOUTH_AFRICA         = 710,     /* South Africa */
+    CTRY_SPAIN                = 724,     /* Spain */
+    CTRY_SRILANKA             = 144,     /* Srilanka */
+    CTRY_SWEDEN               = 752,     /* Sweden */
+    CTRY_SWITZERLAND          = 756,     /* Switzerland */
+    CTRY_SYRIA                = 760,     /* Syria */
+    CTRY_TAIWAN               = 158,     /* Taiwan */
+    CTRY_THAILAND             = 764,     /* Thailand */
+    CTRY_TRINIDAD_Y_TOBAGO    = 780,     /* Trinidad y Tobago */
+    CTRY_TUNISIA              = 788,     /* Tunisia */
+    CTRY_TURKEY               = 792,     /* Turkey */
+    CTRY_UAE                  = 784,     /* U.A.E. */
+    CTRY_UKRAINE              = 804,     /* Ukraine */
+    CTRY_UNITED_KINGDOM       = 826,     /* United Kingdom */
+    CTRY_UNITED_STATES        = 840,     /* United States */
+    CTRY_UNITED_STATES_FCC49  = 842,     /* United States (Public Safety)*/
+    CTRY_URUGUAY              = 858,     /* Uruguay */
+    CTRY_UZBEKISTAN           = 860,     /* Uzbekistan */
+    CTRY_VENEZUELA            = 862,     /* Venezuela */
+    CTRY_VIET_NAM             = 704,     /* Viet Nam */
+    CTRY_YEMEN                = 887,     /* Yemen */
+    CTRY_ZIMBABWE             = 716      /* Zimbabwe */
+};
+
+/* Enumerated Regulatory Domain Information 8 bit values indicate that
+ * the regdomain is really a pair of unitary regdomains.  12 bit values
+ * are the real unitary regdomains and are the only ones which have the
+ * frequency bitmasks and flags set.
+ */
+enum EnumRd {
+	/*
+	 * The following regulatory domain definitions are
+	 * found in the EEPROM. Each regulatory domain
+	 * can operate in either a 5GHz or 2.4GHz wireless mode or
+	 * both 5GHz and 2.4GHz wireless modes.
+	 * In general, the value holds no special
+	 * meaning and is used to decode into either specific
+	 * 2.4GHz or 5GHz wireless mode for that particular
+	 * regulatory domain.
+	 */
+	NO_ENUMRD	= 0x00,
+	NULL1_WORLD	= 0x03,		/* For 11b-only countries (no 11a allowed) */
+	NULL1_ETSIB	= 0x07,		/* Israel */
+	NULL1_ETSIC	= 0x08,
+	FCC1_FCCA	= 0x10,		/* USA */
+	FCC1_WORLD	= 0x11,		/* Hong Kong */
+	FCC4_FCCA	= 0x12,		/* USA - Public Safety */
+	FCC5_FCCA	= 0x13,		/* USA - with no DFS (UNII-1 + UNII-3 only) */
+  FCC6_FCCA       = 0x14,         /* Canada */
+
+	FCC2_FCCA	= 0x20,		/* Canada */
+	FCC2_WORLD	= 0x21,		/* Australia & HK */
+	FCC2_ETSIC	= 0x22,
+  FCC6_WORLD      = 0x23,         /* Australia */
+
+	FRANCE_RES	= 0x31,		/* Legacy France for OEM */
+	FCC3_FCCA	= 0x3A,		/* USA & Canada w/5470 band, 11h, DFS enabled */
+	FCC3_WORLD	= 0x3B,		/* USA & Canada w/5470 band, 11h, DFS enabled */
+
+	ETSI1_WORLD	= 0x37,
+	ETSI3_ETSIA	= 0x32,		/* France (optional) */
+	ETSI2_WORLD	= 0x35,		/* Hungary & others */
+	ETSI3_WORLD	= 0x36,		/* France & others */
+	ETSI4_WORLD	= 0x30,
+	ETSI4_ETSIC	= 0x38,
+	ETSI5_WORLD	= 0x39,
+	ETSI6_WORLD	= 0x34,		/* Bulgaria */
+	ETSI_RESERVED	= 0x33,		/* Reserved (Do not used) */
+
+	MKK1_MKKA	= 0x40,		/* Japan (JP1) */
+	MKK1_MKKB	= 0x41,		/* Japan (JP0) */
+	APL4_WORLD	= 0x42,		/* Singapore */
+	MKK2_MKKA	= 0x43,		/* Japan with 4.9G channels */
+	APL_RESERVED	= 0x44,		/* Reserved (Do not used)  */
+	APL2_WORLD	= 0x45,		/* Korea */
+	APL2_APLC	= 0x46,
+	APL3_WORLD	= 0x47,
+	MKK1_FCCA	= 0x48,		/* Japan (JP1-1) */
+	APL2_APLD	= 0x49,		/* Korea with 2.3G channels */
+	MKK1_MKKA1	= 0x4A,		/* Japan (JE1) */
+	MKK1_MKKA2	= 0x4B,		/* Japan (JE2) */
+	MKK1_MKKC	= 0x4C,		/* Japan (MKK1_MKKA,except Ch14) */
+
+	APL3_FCCA   = 0x50,
+	APL1_WORLD	= 0x52,		/* Latin America */
+	APL1_FCCA	= 0x53,
+	APL1_APLA	= 0x54,
+	APL1_ETSIC	= 0x55,
+	APL2_ETSIC	= 0x56,		/* Venezuela */
+	APL2_FCCA   = 0x57, 	/* new Latin America */
+	APL5_WORLD	= 0x58,		/* Chile */
+	APL6_WORLD	= 0x5B,		/* Singapore */
+	APL7_FCCA   = 0x5C,     /* Taiwan 5.47 Band */
+	APL8_WORLD  = 0x5D,     /* Malaysia 5GHz */
+	APL9_WORLD  = 0x5E,     /* Korea 5GHz */
+
+	/*
+	 * World mode SKUs
+	 */
+	WOR0_WORLD	= 0x60,		/* World0 (WO0 SKU) */
+	WOR1_WORLD	= 0x61,		/* World1 (WO1 SKU) */
+	WOR2_WORLD	= 0x62,		/* World2 (WO2 SKU) */
+	WOR3_WORLD	= 0x63,		/* World3 (WO3 SKU) */
+	WOR4_WORLD	= 0x64,		/* World4 (WO4 SKU) */
+	WOR5_ETSIC	= 0x65,		/* World5 (WO5 SKU) */
+
+	WOR01_WORLD	= 0x66,		/* World0-1 (WW0-1 SKU) */
+	WOR02_WORLD	= 0x67,		/* World0-2 (WW0-2 SKU) */
+	EU1_WORLD	= 0x68,		/* Same as World0-2 (WW0-2 SKU), except active scan ch1-13. No ch14 */
+
+	WOR9_WORLD	= 0x69,		/* World9 (WO9 SKU) */
+	WORA_WORLD	= 0x6A,		/* WorldA (WOA SKU) */
+
+	MKK3_MKKB	= 0x80,		/* Japan UNI-1 even + MKKB */
+	MKK3_MKKA2	= 0x81,		/* Japan UNI-1 even + MKKA2 */
+	MKK3_MKKC	= 0x82,		/* Japan UNI-1 even + MKKC */
+
+	MKK4_MKKB	= 0x83,		/* Japan UNI-1 even + UNI-2 + MKKB */
+	MKK4_MKKA2	= 0x84,		/* Japan UNI-1 even + UNI-2 + MKKA2 */
+	MKK4_MKKC	= 0x85,		/* Japan UNI-1 even + UNI-2 + MKKC */
+
+	MKK5_MKKB	= 0x86,		/* Japan UNI-1 even + UNI-2 + mid-band + MKKB */
+	MKK5_MKKA2	= 0x87,		/* Japan UNI-1 even + UNI-2 + mid-band + MKKA2 */
+	MKK5_MKKC	= 0x88,		/* Japan UNI-1 even + UNI-2 + mid-band + MKKC */
+
+	MKK6_MKKB	= 0x89,		/* Japan UNI-1 even + UNI-1 odd MKKB */
+	MKK6_MKKA2	= 0x8A,		/* Japan UNI-1 even + UNI-1 odd + MKKA2 */
+	MKK6_MKKC	= 0x8B,		/* Japan UNI-1 even + UNI-1 odd + MKKC */
+
+	MKK7_MKKB	= 0x8C,		/* Japan UNI-1 even + UNI-1 odd + UNI-2 + MKKB */
+	MKK7_MKKA	= 0x8D,		/* Japan UNI-1 even + UNI-1 odd + UNI-2 + MKKA2 */
+	MKK7_MKKC	= 0x8E,		/* Japan UNI-1 even + UNI-1 odd + UNI-2 + MKKC */
+
+	MKK8_MKKB	= 0x8F,		/* Japan UNI-1 even + UNI-1 odd + UNI-2 + mid-band + MKKB */
+	MKK8_MKKA2	= 0x90,		/* Japan UNI-1 even + UNI-1 odd + UNI-2 + mid-band + MKKA2 */
+	MKK8_MKKC	= 0x91,		/* Japan UNI-1 even + UNI-1 odd + UNI-2 + mid-band + MKKC */
+
+	MKK6_MKKA1      = 0xF8,         /* Japan UNI-1 even + UNI-1 odd + MKKA1 */
+	MKK6_FCCA       = 0xF9,         /* Japan UNI-1 even + UNI-1 odd + FCCA */
+	MKK7_MKKA1      = 0xFA,         /* Japan UNI-1 even + UNI-1 odd + UNI-2 + MKKA1 */
+	MKK7_FCCA       = 0xFB,         /* Japan UNI-1 even + UNI-1 odd + UNI-2 + FCCA */
+	MKK9_FCCA       = 0xFC,         /* Japan UNI-1 even + 4.9GHz + FCCA */
+	MKK9_MKKA1      = 0xFD,         /* Japan UNI-1 even + 4.9GHz + MKKA1 */
+	MKK9_MKKC       = 0xFE,         /* Japan UNI-1 even + 4.9GHz + MKKC */
+	MKK9_MKKA2      = 0xFF,         /* Japan UNI-1 even + 4.9GHz + MKKA2 */
+
+	MKK10_FCCA      = 0xD0,         /* Japan UNI-1 even + UNI-2 + 4.9GHz + FCCA */
+	MKK10_MKKA1     = 0xD1,         /* Japan UNI-1 even + UNI-2 + 4.9GHz + MKKA1 */
+	MKK10_MKKC      = 0xD2,         /* Japan UNI-1 even + UNI-2 + 4.9GHz + MKKC */
+	MKK10_MKKA2     = 0xD3,         /* Japan UNI-1 even + UNI-2 + 4.9GHz + MKKA2 */
+
+	MKK11_MKKA      = 0xD4,         /* Japan UNI-1 even + UNI-2 + Midband + 4.9GHz + MKKA */
+	MKK11_FCCA      = 0xD5,         /* Japan UNI-1 even + UNI-2 + Midband + 4.9GHz + FCCA */
+	MKK11_MKKA1     = 0xD6,         /* Japan UNI-1 even + UNI-2 + Midband + 4.9GHz + MKKA1 */
+	MKK11_MKKC      = 0xD7,         /* Japan UNI-1 even + UNI-2 + Midband + 4.9GHz + MKKC */
+	MKK11_MKKA2     = 0xD8,         /* Japan UNI-1 even + UNI-2 + Midband + 4.9GHz + MKKA2 */
+
+	MKK12_MKKA      = 0xD9,         /* Japan UNI-1 even + UNI-1 odd + UNI-2 + Midband + 4.9GHz + MKKA */
+	MKK12_FCCA      = 0xDA,         /* Japan UNI-1 even + UNI-1 odd + UNI-2 + Midband + 4.9GHz + FCCA */
+	MKK12_MKKA1     = 0xDB,         /* Japan UNI-1 even + UNI-1 odd + UNI-2 + Midband + 4.9GHz + MKKA1 */
+	MKK12_MKKC      = 0xDC,         /* Japan UNI-1 even + UNI-1 odd + UNI-2 + Midband + 4.9GHz + MKKC */
+	MKK12_MKKA2     = 0xDD,         /* Japan UNI-1 even + UNI-1 odd + UNI-2 + Midband + 4.9GHz + MKKA2 */
+
+	/* Following definitions are used only by s/w to map old
+ 	 * Japan SKUs.
+	 */
+	MKK3_MKKA       = 0xF0,         /* Japan UNI-1 even + MKKA */
+	MKK3_MKKA1      = 0xF1,         /* Japan UNI-1 even + MKKA1 */
+	MKK3_FCCA       = 0xF2,         /* Japan UNI-1 even + FCCA */
+	MKK4_MKKA       = 0xF3,         /* Japan UNI-1 even + UNI-2 + MKKA */
+	MKK4_MKKA1      = 0xF4,         /* Japan UNI-1 even + UNI-2 + MKKA1 */
+	MKK4_FCCA       = 0xF5,         /* Japan UNI-1 even + UNI-2 + FCCA */
+	MKK9_MKKA       = 0xF6,         /* Japan UNI-1 even + 4.9GHz + MKKA*/
+	MKK10_MKKA      = 0xF7,         /* Japan UNI-1 even + UNI-2 + 4.9GHz + MKKA */
+
+	/*
+	 * Regulator domains ending in a number (e.g. APL1,
+	 * MK1, ETSI4, etc) apply to 5GHz channel and power
+	 * information.  Regulator domains ending in a letter
+	 * (e.g. APLA, FCCA, etc) apply to 2.4GHz channel and
+	 * power information.
+	 */
+	APL1		= 0x0150,	/* LAT & Asia */
+	APL2		= 0x0250,	/* LAT & Asia */
+	APL3		= 0x0350,	/* Taiwan */
+	APL4		= 0x0450,	/* Jordan */
+	APL5		= 0x0550,	/* Chile */
+	APL6		= 0x0650,	/* Singapore */
+	APL7		= 0x0750,	/* Taiwan Middle */
+	APL8		= 0x0850,	/* Malaysia */
+	APL9		= 0x0950,	/* Korea (South) ROC 3 */
+
+	ETSI1		= 0x0130,	/* Europe & others */
+	ETSI2		= 0x0230,	/* Europe & others */
+	ETSI3		= 0x0330,	/* Europe & others */
+	ETSI4		= 0x0430,	/* Europe & others */
+	ETSI5		= 0x0530,	/* Europe & others */
+	ETSI6		= 0x0630,	/* Europe & others */
+	ETSIA		= 0x0A30,	/* France */
+	ETSIB		= 0x0B30,	/* Israel */
+	ETSIC		= 0x0C30,	/* Latin America */
+
+	FCC1		= 0x0110,	/* US & others */
+	FCC2		= 0x0120,	/* Canada, Australia & New Zealand */
+	FCC3		= 0x0160,	/* US w/new middle band & DFS */
+	FCC4		= 0x0165,	/* US Public Safety */
+	FCC5		= 0x0510,	/* US no DFS */
+    FCC6    = 0x0610, /* Canada & Australia */
+
+	FCCA		= 0x0A10,
+
+	APLD		= 0x0D50,	/* South Korea */
+
+	MKK1		= 0x0140,	/* Japan (UNI-1 odd)*/
+	MKK2		= 0x0240,	/* Japan (4.9 GHz + UNI-1 odd) */
+	MKK3		= 0x0340,	/* Japan (UNI-1 even) */
+	MKK4		= 0x0440,	/* Japan (UNI-1 even + UNI-2) */
+	MKK5		= 0x0540,	/* Japan (UNI-1 even + UNI-2 + mid-band) */
+	MKK6		= 0x0640,	/* Japan (UNI-1 odd + UNI-1 even) */
+	MKK7		= 0x0740,	/* Japan (UNI-1 odd + UNI-1 even + UNI-2 */
+	MKK8		= 0x0840,	/* Japan (UNI-1 odd + UNI-1 even + UNI-2 + mid-band) */
+	MKK9		= 0x0940,   /* Japan (UNI-1 even + 4.9 GHZ) */
+	MKK10		= 0x0B40,   /* Japan (UNI-1 even + UNI-2 + 4.9 GHZ) */
+	MKK11		= 0x1140,   /* Japan (UNI-1 even + UNI-2 + mid-band + 4.9 GHZ) */
+	MKK12		= 0x1240,   /* Japan (UNI-1 even + UNI-1 odd + UNI-2 + mid-band + 4.9 GHZ) */
+	MKKA		= 0x0A40,	/* Japan */
+	MKKC		= 0x0A50,
+
+	NULL1		= 0x0198,
+	WORLD		= 0x0199,
+	DEBUG_REG_DMN	= 0x01ff,
+};
+
+/* channelFlags */
+#define	ZM_REG_FLAG_CHANNEL_CW_INT	0x0002	/* CW interference detected on channel */
+#define	ZM_REG_FLAG_CHANNEL_TURBO	0x0010	/* Turbo Channel */
+#define	ZM_REG_FLAG_CHANNEL_CCK	    0x0020	/* CCK channel */
+#define	ZM_REG_FLAG_CHANNEL_OFDM	0x0040	/* OFDM channel */
+#define	ZM_REG_FLAG_CHANNEL_2GHZ	0x0080	/* 2 GHz spectrum channel. */
+#define	ZM_REG_FLAG_CHANNEL_5GHZ	0x0100	/* 5 GHz spectrum channel */
+#define	ZM_REG_FLAG_CHANNEL_PASSIVE	0x0200	/* Only passive scan allowed in the channel */
+#define	ZM_REG_FLAG_CHANNEL_DYN	    0x0400	/* dynamic CCK-OFDM channel */
+#define	ZM_REG_FLAG_CHANNEL_XR	    0x0800	/* XR channel */
+#define	ZM_REG_FLAG_CHANNEL_CSA 	0x1000	/* Channel by CSA(Channel Switch Announcement) */
+#define	ZM_REG_FLAG_CHANNEL_STURBO	0x2000	/* Static turbo, no 11a-only usage */
+#define ZM_REG_FLAG_CHANNEL_HALF    0x4000 	/* Half rate channel */
+#define ZM_REG_FLAG_CHANNEL_QUARTER 0x8000 	/* Quarter rate channel */
+
+/* channelFlags */
+#define CHANNEL_CW_INT  0x0002  /* CW interference detected on channel */
+#define CHANNEL_TURBO   0x0010  /* Turbo Channel */
+#define CHANNEL_CCK 0x0020  /* CCK channel */
+#define CHANNEL_OFDM    0x0040  /* OFDM channel */
+#define CHANNEL_2GHZ    0x0080  /* 2 GHz spectrum channel. */
+#define CHANNEL_5GHZ    0x0100  /* 5 GHz spectrum channel */
+#define CHANNEL_PASSIVE 0x0200  /* Only passive scan allowed in the channel */
+#define CHANNEL_DYN 0x0400  /* dynamic CCK-OFDM channel */
+#define CHANNEL_XR  0x0800  /* XR channel */
+#define CHANNEL_STURBO  0x2000  /* Static turbo, no 11a-only usage */
+#define CHANNEL_HALF    0x4000  /* Half rate channel */
+#define CHANNEL_QUARTER 0x8000  /* Quarter rate channel */
+#define CHANNEL_HT20    0x10000 /* HT20 channel */
+#define CHANNEL_HT40    0x20000 /* HT40 channel */
+#define CHANNEL_HT40U 	0x40000 /* control channel can be upper channel */
+#define CHANNEL_HT40L 	0x80000 /* control channel can be lower channel */
+
+/* privFlags */
+#define ZM_REG_FLAG_CHANNEL_INTERFERENCE   	0x01 /* Software use: channel interference
+				        used for as AR as well as RADAR
+				        interference detection */
+#define ZM_REG_FLAG_CHANNEL_DFS		0x02 /* DFS required on channel */
+#define ZM_REG_FLAG_CHANNEL_4MS_LIMIT	0x04 /* 4msec packet limit on this channel */
+#define ZM_REG_FLAG_CHANNEL_DFS_CLEAR       0x08 /* if channel has been checked for DFS */
+
+#define CHANNEL_A   (CHANNEL_5GHZ|CHANNEL_OFDM)
+#define CHANNEL_B   (CHANNEL_2GHZ|CHANNEL_CCK)
+#define CHANNEL_PUREG   (CHANNEL_2GHZ|CHANNEL_OFDM)
+#ifdef notdef
+#define CHANNEL_G   (CHANNEL_2GHZ|CHANNEL_DYN)
+#else
+#define CHANNEL_G   (CHANNEL_2GHZ|CHANNEL_OFDM)
+#endif
+#define CHANNEL_T   (CHANNEL_5GHZ|CHANNEL_OFDM|CHANNEL_TURBO)
+#define CHANNEL_ST  (CHANNEL_T|CHANNEL_STURBO)
+#define CHANNEL_108G    (CHANNEL_2GHZ|CHANNEL_OFDM|CHANNEL_TURBO)
+#define CHANNEL_108A    CHANNEL_T
+#define CHANNEL_X   (CHANNEL_5GHZ|CHANNEL_OFDM|CHANNEL_XR)
+#define CHANNEL_G_HT      (CHANNEL_2GHZ | CHANNEL_OFDM | CHANNEL_HT20)
+#define CHANNEL_A_HT      (CHANNEL_5GHZ | CHANNEL_OFDM | CHANNEL_HT20)
+
+#define CHANNEL_G_HT20  (CHANNEL_2GHZ|CHANNEL_HT20)
+#define CHANNEL_A_HT20  (CHANNEL_5GHZ|CHANNEL_HT20)
+#define CHANNEL_G_HT40  (CHANNEL_2GHZ|CHANNEL_HT20|CHANNEL_HT40)
+#define CHANNEL_A_HT40  (CHANNEL_5GHZ|CHANNEL_HT20|CHANNEL_HT40)
+#define CHANNEL_ALL \
+    (CHANNEL_OFDM|CHANNEL_CCK| CHANNEL_2GHZ | CHANNEL_5GHZ | CHANNEL_TURBO | CHANNEL_HT20 | CHANNEL_HT40)
+#define CHANNEL_ALL_NOTURBO     (CHANNEL_ALL &~ CHANNEL_TURBO)
+
+enum {
+    HAL_MODE_11A    = 0x001,        /* 11a channels */
+    HAL_MODE_TURBO  = 0x002,        /* 11a turbo-only channels */
+    HAL_MODE_11B    = 0x004,        /* 11b channels */
+    HAL_MODE_PUREG  = 0x008,        /* 11g channels (OFDM only) */
+#ifdef notdef
+    HAL_MODE_11G    = 0x010,        /* 11g channels (OFDM/CCK) */
+#else
+    HAL_MODE_11G    = 0x008,        /* XXX historical */
+#endif
+    HAL_MODE_108G   = 0x020,        /* 11a+Turbo channels */
+    HAL_MODE_108A   = 0x040,        /* 11g+Turbo channels */
+    HAL_MODE_XR     = 0x100,        /* XR channels */
+    HAL_MODE_11A_HALF_RATE = 0x200,     /* 11A half rate channels */
+    HAL_MODE_11A_QUARTER_RATE = 0x400,  /* 11A quarter rate channels */
+    HAL_MODE_11NG   = 0x4000,           /* 11ng channels */
+    HAL_MODE_11NA   = 0x8000,           /* 11na channels */
+    HAL_MODE_ALL    = 0xffff
+};
+
+#endif /* #ifndef _HPREG_H */
diff --git a/drivers/staging/otus/hal/hprw.c b/drivers/staging/otus/hal/hprw.c
new file mode 100644
index 00000000000..db7d4957645
--- /dev/null
+++ b/drivers/staging/otus/hal/hprw.c
@@ -0,0 +1,1557 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include "../80211core/cprecomp.h"
+#include "hpani.h"
+#include "hpusb.h"
+#include "hpreg.h"
+#include "../80211core/ratectrl.h"
+
+extern void zfIdlCmd(zdev_t* dev, u32_t* cmd, u16_t cmdLen);
+
+extern void zfCoreCwmBusy(zdev_t* dev, u16_t busy);
+u16_t zfDelayWriteInternalReg(zdev_t* dev, u32_t addr, u32_t val);
+u16_t zfFlushDelayWrite(zdev_t* dev);
+
+//#define zm_hp_priv(x) struct zsHpPriv* hpPriv=zgWlanDev.hpPrivate;
+
+void zfInitCmdQueue(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv = (struct zsHpPriv*)(wd->hpPrivate);
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+#ifdef ZM_XP_USB_MULTCMD
+    hpPriv->cmdTail = hpPriv->cmdHead = hpPriv->cmdSend = 0;
+#else
+    hpPriv->cmdTail = hpPriv->cmdHead = 0;
+#endif
+    hpPriv->cmdPending = 0;
+    hpPriv->cmd.delayWcmdCount = 0;
+    zmw_leave_critical_section(dev);
+}
+
+u16_t zfPutCmd(zdev_t* dev, u32_t* cmd, u16_t cmdLen, u16_t src, u8_t* buf)
+{
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+    /* Make sure command length < ZM_MAX_CMD_SIZE */
+    zm_assert(cmdLen <= ZM_MAX_CMD_SIZE);
+    /* Make sure command queue not full */
+    //zm_assert(((hpPriv->cmdTail+1) & (ZM_CMD_QUEUE_SIZE-1)) != hpPriv->cmdHead);
+    if (((hpPriv->cmdTail+1) & (ZM_CMD_QUEUE_SIZE-1)) == hpPriv->cmdHead ) {
+        zm_debug_msg0("CMD queue full!!");
+        return 0;
+    }
+
+    hpPriv->cmdQ[hpPriv->cmdTail].cmdLen = cmdLen;
+    hpPriv->cmdQ[hpPriv->cmdTail].src = src;
+    hpPriv->cmdQ[hpPriv->cmdTail].buf = buf;
+    for (i=0; i<(cmdLen>>2); i++)
+    {
+        hpPriv->cmdQ[hpPriv->cmdTail].cmd[i] = cmd[i];
+    }
+
+    hpPriv->cmdTail = (hpPriv->cmdTail+1) & (ZM_CMD_QUEUE_SIZE-1);
+
+    return 0;
+}
+
+u16_t zfGetCmd(zdev_t* dev, u32_t* cmd, u16_t* cmdLen, u16_t* src, u8_t** buf)
+{
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+    if (hpPriv->cmdTail == hpPriv->cmdHead)
+    {
+        return 3;
+    }
+
+    *cmdLen = hpPriv->cmdQ[hpPriv->cmdHead].cmdLen;
+    *src = hpPriv->cmdQ[hpPriv->cmdHead].src;
+    *buf = hpPriv->cmdQ[hpPriv->cmdHead].buf;
+    for (i=0; i<((*cmdLen)>>2); i++)
+    {
+        cmd[i] = hpPriv->cmdQ[hpPriv->cmdHead].cmd[i];
+    }
+
+    hpPriv->cmdHead = (hpPriv->cmdHead+1) & (ZM_CMD_QUEUE_SIZE-1);
+
+    return 0;
+}
+
+#ifdef ZM_XP_USB_MULTCMD
+void zfSendCmdEx(zdev_t* dev)
+{
+    u32_t ncmd[ZM_MAX_CMD_SIZE/4];
+    u16_t ncmdLen = 0;
+    u16_t cmdFlag = 0;
+    u16_t i;
+
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    if (hpPriv->cmdPending == 0)
+    {
+        if (hpPriv->cmdTail != hpPriv->cmdSend)
+        {
+            cmdFlag = 1;
+            /* Get queueing command */
+            ncmdLen= hpPriv->cmdQ[hpPriv->cmdSend].cmdLen;
+            for (i=0; i<(ncmdLen>>2); i++)
+            {
+                ncmd[i] = hpPriv->cmdQ[hpPriv->cmdSend].cmd[i];
+            }
+            hpPriv->cmdSend = (hpPriv->cmdSend+1) & (ZM_CMD_QUEUE_SIZE-1);
+
+            hpPriv->cmdPending = 1;
+        }
+    }
+
+    zmw_leave_critical_section(dev);
+
+    if ((cmdFlag == 1))
+    {
+        zfIdlCmd(dev, ncmd, ncmdLen);
+    }
+}
+
+void zfiSendCmdComp(zdev_t* dev)
+{
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+    hpPriv->cmdPending = 0;
+    zmw_leave_critical_section(dev);
+
+    zfSendCmdEx(dev);
+}
+#endif
+
+u16_t zfIssueCmd(zdev_t* dev, u32_t* cmd, u16_t cmdLen, u16_t src, u8_t* buf)
+{
+    u16_t cmdFlag = 0;
+    u16_t ret;
+
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+    zmw_declare_for_critical_section();
+
+    zm_msg2_mm(ZM_LV_1, "cmdLen=", cmdLen);
+
+    zmw_enter_critical_section(dev);
+
+#ifdef ZM_XP_USB_MULTCMD
+    ret = zfPutCmd(dev, cmd, cmdLen, src, buf);
+    zmw_leave_critical_section(dev);
+
+    if (ret != 0)
+    {
+        return 1;
+    }
+
+    zfSendCmdEx(dev);
+#else
+    if (hpPriv->cmdPending == 0)
+    {
+        hpPriv->cmdPending = 1;
+        cmdFlag = 1;
+    }
+    ret = zfPutCmd(dev, cmd, cmdLen, src, buf);
+
+    zmw_leave_critical_section(dev);
+
+    if (ret != 0)
+    {
+        return 1;
+    }
+
+    if (cmdFlag == 1)
+    {
+        zfIdlCmd(dev, cmd, cmdLen);
+    }
+#endif
+    return 0;
+}
+
+void zfIdlRsp(zdev_t* dev, u32_t* rsp, u16_t rspLen)
+{
+    u32_t cmd[ZM_MAX_CMD_SIZE/4];
+    u16_t cmdLen;
+    u16_t src;
+    u8_t* buf;
+    u32_t ncmd[ZM_MAX_CMD_SIZE/4];
+    u16_t ncmdLen = 0;
+    u16_t ret;
+    u16_t cmdFlag = 0;
+    u16_t i;
+    s32_t nf;
+    s32_t noisefloor[4];
+
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+
+    zmw_declare_for_critical_section();
+
+    zmw_enter_critical_section(dev);
+
+    ret = zfGetCmd(dev, cmd, &cmdLen, &src, &buf);
+    #if 0
+    zm_assert(ret == 0);
+    #else
+    if (ret != 0)
+    {
+        zm_debug_msg0("Error IdlRsp because none cmd!!\n");
+        #ifndef ZM_XP_USB_MULTCMD
+        zmw_leave_critical_section(dev);
+        return;
+        #endif
+    }
+    #endif
+#ifdef ZM_XP_USB_MULTCMD
+    zmw_leave_critical_section(dev);
+#else
+    if (hpPriv->cmdTail != hpPriv->cmdHead)
+    {
+        cmdFlag = 1;
+        /* Get queueing command */
+        ncmdLen= hpPriv->cmdQ[hpPriv->cmdHead].cmdLen;
+        for (i=0; i<(ncmdLen>>2); i++)
+        {
+            ncmd[i] = hpPriv->cmdQ[hpPriv->cmdHead].cmd[i];
+        }
+    }
+    else
+    {
+        hpPriv->cmdPending = 0;
+    }
+
+    zmw_leave_critical_section(dev);
+
+    if (cmdFlag == 1)
+    {
+        zfIdlCmd(dev, ncmd, ncmdLen);
+    }
+#endif
+    if (src == ZM_OID_READ)
+    {
+        ZM_PERFORMANCE_REG(dev, 0x11772c, rsp[1]);
+        zfwDbgReadRegDone(dev, cmd[1], rsp[1]);
+    }
+    else if (src == ZM_OID_FLASH_CHKSUM)
+    {
+        zfwDbgGetFlashChkSumDone(dev, rsp+1);
+    }
+    else if (src == ZM_OID_FLASH_READ)
+    {
+        u32_t  datalen;
+        u16_t i;
+
+        datalen = (rsp[0] & 255);
+
+        zfwDbgReadFlashDone(dev, cmd[1], rsp+1, datalen);
+    }
+    else if (src == ZM_OID_FLASH_PROGRAM)
+    {
+        /* Non do */
+    }
+    else if (src == ZM_OID_WRITE)
+    {
+        zfwDbgWriteRegDone(dev, cmd[1], cmd[2]);
+    }
+    else if (src == ZM_OID_TALLY)
+    {
+		zfCollectHWTally(dev, rsp, 0);
+    }
+    else if (src == ZM_OID_TALLY_APD)
+    {
+		zfCollectHWTally(dev, rsp, 1);
+        zfwDbgReadTallyDone(dev);
+#ifdef ZM_ENABLE_BA_RATECTRL
+        zfRateCtrlAggrSta(dev);
+#endif
+    }
+    else if (src == ZM_OID_DKTX_STATUS)
+    {
+        zm_debug_msg0("src = zm_OID_DKTX_STATUS");
+        zfwDbgQueryHwTxBusyDone(dev, rsp[1]);
+    }
+    else if (src == ZM_CMD_SET_FREQUENCY)
+    {
+
+//#ifdef ZM_OTUS_ENABLE_RETRY_FREQ_CHANGE
+#if 0
+    zm_debug_msg1("Retry Set Frequency = ", rsp[1]);
+
+    #if 1
+    // Read the Noise Floor value !
+    nf = ((rsp[2]>>19) & 0x1ff);
+    if ((nf & 0x100) != 0x0)
+    {
+        noisefloor[0] = 0 - ((nf ^ 0x1ff) + 1);
+    }
+    else
+    {
+        noisefloor[0] = nf;
+    }
+
+    zm_debug_msg1("Noise Floor[1] = ", noisefloor[0]);
+
+    nf = ((rsp[3]>>19) & 0x1ff);
+    if ((nf & 0x100) != 0x0)
+    {
+        noisefloor[1] = 0 - ((nf ^ 0x1ff) + 1);
+    }
+    else
+    {
+        noisefloor[1] = nf;
+    }
+
+    zm_debug_msg1("Noise Floor[2] = ", noisefloor[1]);
+    zm_debug_msg1("Is Site Survey = ", hpPriv->isSiteSurvey);
+    #endif
+
+        if ( (rsp[1] && hpPriv->freqRetryCounter == 0) ||
+             (((noisefloor[0]>-60)||(noisefloor[1]>-60)) && hpPriv->freqRetryCounter==0) ||
+             ((abs(noisefloor[0]-noisefloor[1])>=9) && hpPriv->freqRetryCounter==0) )
+        {
+            zm_debug_msg0("Retry to issue the frequency change command");
+
+            if ( hpPriv->recordFreqRetryCounter == 1 )
+            {
+                zm_debug_msg0("Cold Reset");
+
+                zfHpSetFrequencyEx(dev, hpPriv->latestFrequency,
+                                        hpPriv->latestBw40,
+                                        hpPriv->latestExtOffset,
+                                        2);
+
+                if ( hpPriv->isSiteSurvey != 2 )
+                {
+                    hpPriv->freqRetryCounter++;
+                }
+                hpPriv->recordFreqRetryCounter = 0;
+            }
+            else
+            {
+                zfHpSetFrequencyEx(dev, hpPriv->latestFrequency,
+                                        hpPriv->latestBw40,
+                                        hpPriv->latestExtOffset,
+                                        0);
+            }
+            hpPriv->recordFreqRetryCounter++;
+        }
+        else
+#endif
+
+/* ret: Bit0: AGC calibration   0=>finish  1=>unfinish               */
+/*      Bit1: Noise calibration 0=>finish  1=>unfinish               */
+/*      Bit2: Noise calibration finish, but NF value unexcepted => 1 */
+        if ( (rsp[1] & 0x1) || (rsp[1] & 0x4) )
+        {
+            zm_debug_msg1("Set Frequency fail : ret = ", rsp[1]);
+
+            /* 1. AGC Calibration fail                                  */
+            /* 2. Noise Calibration finish but error NoiseFloor value   */
+            /*      and not in sitesurvey, try more twice               */
+            if ( hpPriv->isSiteSurvey == 2 )
+            {
+                if ( hpPriv->recordFreqRetryCounter < 2 )
+                {
+                    /* cold reset */
+                    zfHpSetFrequencyEx(dev, hpPriv->latestFrequency,
+                                            hpPriv->latestBw40,
+                                            hpPriv->latestExtOffset,
+                                            2);
+                    hpPriv->recordFreqRetryCounter++;
+                    zm_debug_msg1("Retry to issue the frequency change command(cold reset) counter = ", hpPriv->recordFreqRetryCounter);
+                }
+                else
+                {
+                    /* Fail : we would not accept this result! */
+                    zm_debug_msg0("\n\n\n\n  Fail twice cold reset \n\n\n\n");
+                    hpPriv->coldResetNeedFreq = 0;
+                    hpPriv->recordFreqRetryCounter = 0;
+                    zfCoreSetFrequencyComplete(dev);
+                }
+            }
+            else
+            {
+                /* in sitesurvey, coldreset in next channel */
+                hpPriv->coldResetNeedFreq = 1;
+                hpPriv->recordFreqRetryCounter = 0;
+                zfCoreSetFrequencyComplete(dev);
+            }
+        }
+        else if (rsp[1] & 0x2)
+        {
+            zm_debug_msg1("Set Frequency fail 2 : ret = ", rsp[1]);
+
+            /* Noise Calibration un-finish                          */
+            /*      and not in sitesurvey, try more once            */
+            if ( hpPriv->isSiteSurvey == 2 )
+            {
+                if ( hpPriv->recordFreqRetryCounter < 1 )
+                {
+                    /* cold reset */
+                    zfHpSetFrequencyEx(dev, hpPriv->latestFrequency,
+                                            hpPriv->latestBw40,
+                                            hpPriv->latestExtOffset,
+                                            2);
+                    hpPriv->recordFreqRetryCounter++;
+                    zm_debug_msg1("2 Retry to issue the frequency change command(cold reset) counter = ", hpPriv->recordFreqRetryCounter);
+                }
+                else
+                {
+                    /* Fail : we would not accept this result! */
+                    zm_debug_msg0("\n\n\n\n  2 Fail twice cold reset \n\n\n\n");
+                    hpPriv->coldResetNeedFreq = 0;
+                    hpPriv->recordFreqRetryCounter = 0;
+                    zfCoreSetFrequencyComplete(dev);
+                }
+            }
+            else
+            {
+                /* in sitesurvey, skip this frequency */
+                hpPriv->coldResetNeedFreq = 0;
+                hpPriv->recordFreqRetryCounter = 0;
+                zfCoreSetFrequencyComplete(dev);
+            }
+        }
+        //else if (rsp[1] & 0x4)
+        //{
+        //    zm_debug_msg1("Set Frequency fail 3 : ret = ", rsp[1]);
+        //    hpPriv->coldResetNeedFreq = 0;
+        //    hpPriv->recordFreqRetryCounter = 0;
+        //    zfCoreSetFrequencyComplete(dev);
+        //}
+        else
+        {
+            //hpPriv->freqRetryCounter = 0;
+            zm_debug_msg2(" return complete, ret = ", rsp[1]);
+
+            /* set bb_heavy_clip_enable */
+            if (hpPriv->enableBBHeavyClip && hpPriv->hwBBHeavyClip &&
+                hpPriv->doBBHeavyClip)
+            {
+                u32_t setValue = 0x200;
+
+                setValue |= hpPriv->setValueHeavyClip;
+
+                //zm_dbg(("Do heavy clip setValue = %d\n", setValue));
+
+                zfDelayWriteInternalReg(dev, 0x99e0+0x1bc000, setValue);
+                zfFlushDelayWrite(dev);
+            }
+
+            hpPriv->coldResetNeedFreq = 0;
+            hpPriv->recordFreqRetryCounter = 0;
+    	    zfCoreSetFrequencyComplete(dev);
+    	}
+
+        #if 1
+        // Read the Noise Floor value !
+        nf = ((rsp[2]>>19) & 0x1ff);
+        if ((nf & 0x100) != 0x0)
+        {
+            noisefloor[0] = 0 - ((nf ^ 0x1ff) + 1);
+        }
+        else
+        {
+            noisefloor[0] = nf;
+        }
+
+        //zm_debug_msg1("Noise Floor[1] = ", noisefloor[0]);
+
+        nf = ((rsp[3]>>19) & 0x1ff);
+        if ((nf & 0x100) != 0x0)
+        {
+            noisefloor[1] = 0 - ((nf ^ 0x1ff) + 1);
+        }
+        else
+        {
+            noisefloor[1] = nf;
+        }
+
+        //zm_debug_msg1("Noise Floor[2] = ", noisefloor[1]);
+
+        nf = ((rsp[5]>>23) & 0x1ff);
+        if ((nf & 0x100) != 0x0)
+        {
+            noisefloor[2] = 0 - ((nf ^ 0x1ff) + 1);
+        }
+        else
+        {
+            noisefloor[2] = nf;
+        }
+
+        //zm_debug_msg1("Noise Floor ext[1] = ", noisefloor[2]);
+
+        nf = ((rsp[6]>>23) & 0x1ff);
+        if ((nf & 0x100) != 0x0)
+        {
+            noisefloor[3] = 0 - ((nf ^ 0x1ff) + 1);
+        }
+        else
+        {
+            noisefloor[3] = nf;
+        }
+
+        //zm_debug_msg1("Noise Floor ext[2] = ", noisefloor[3]);
+
+        //zm_debug_msg1("Is Site Survey = ", hpPriv->isSiteSurvey);
+        #endif
+    }
+    else if (src == ZM_CMD_SET_KEY)
+    {
+        zfCoreSetKeyComplete(dev);
+    }
+    else if (src == ZM_CWM_READ)
+    {
+        zm_msg2_mm(ZM_LV_0, "CWM rsp[1]=", rsp[1]);
+        zm_msg2_mm(ZM_LV_0, "CWM rsp[2]=", rsp[2]);
+        zfCoreCwmBusy(dev, zfCwmIsExtChanBusy(rsp[1], rsp[2]));
+    }
+    else if (src == ZM_MAC_READ)
+    {
+        /* rsp[1] = ZM_SEEPROM_MAC_ADDRESS_OFFSET;   */
+        /* rsp[2] = ZM_SEEPROM_MAC_ADDRESS_OFFSET+4; */
+        /* rsp[3] = ZM_SEEPROM_REGDOMAIN_OFFSET;     */
+        /* rsp[4] = ZM_SEEPROM_VERISON_OFFSET;       */
+        /* rsp[5] = ZM_SEEPROM_HARDWARE_TYPE_OFFSET; */
+        /* rsp[6] = ZM_SEEPROM_HW_HEAVY_CLIP;        */
+
+        u8_t addr[6], CCS, WWR;
+        u16_t CountryDomainCode;
+
+        /* BB heavy clip */
+        //hpPriv->eepromHeavyClipFlag = (u8_t)((rsp[6]>>24) & 0xff); // force enable 8107
+        //zm_msg2_mm(ZM_LV_0, "eepromHeavyClipFlag", hpPriv->eepromHeavyClipFlag);
+        #if 0
+        if (hpPriv->hwBBHeavyClip)
+        {
+            zm_msg0_mm(ZM_LV_0, "enable BB Heavy Clip");
+        }
+        else
+        {
+            zm_msg0_mm(ZM_LV_0, "Not enable BB Heavy Clip");
+        }
+        #endif
+        zm_msg2_mm(ZM_LV_0, "MAC rsp[1]=", rsp[1]);
+        zm_msg2_mm(ZM_LV_0, "MAC rsp[2]=", rsp[2]);
+
+        addr[0] = (u8_t)(rsp[1] & 0xff);
+        addr[1] = (u8_t)((rsp[1]>>8) & 0xff);
+        addr[2] = (u8_t)((rsp[1]>>16) & 0xff);
+        addr[3] = (u8_t)((rsp[1]>>24) & 0xff);
+        addr[4] = (u8_t)(rsp[2] & 0xff);
+        addr[5] = (u8_t)((rsp[2]>>8) & 0xff);
+/*#ifdef ZM_FB50
+        addr[0] = (u8_t)(0 & 0xff);
+        addr[1] = (u8_t)(3 & 0xff);
+        addr[2] = (u8_t)(127 & 0xff);
+        addr[3] = (u8_t)(0 & 0xff);
+        addr[4] = (u8_t)(9 & 0xff);
+        addr[5] = (u8_t)(11 & 0xff);
+#endif*/
+
+        zfDelayWriteInternalReg(dev, ZM_MAC_REG_MAC_ADDR_L,
+                ((((u32_t)addr[3])<<24) | (((u32_t)addr[2])<<16) | (((u32_t)addr[1])<<8) | addr[0]));
+        zfDelayWriteInternalReg(dev, ZM_MAC_REG_MAC_ADDR_H,
+                ((((u32_t)addr[5])<<8) | addr[4]));
+        zfFlushDelayWrite(dev);
+
+        wd->ledStruct.ledMode[0] = (u16_t)(rsp[5]&0xffff);
+        wd->ledStruct.ledMode[1] = (u16_t)(rsp[5]>>16);
+        zm_msg2_mm(ZM_LV_0, "ledMode[0]=", wd->ledStruct.ledMode[0]);
+        zm_msg2_mm(ZM_LV_0, "ledMode[1]=", wd->ledStruct.ledMode[1]);
+
+        /* Regulatory Related Setting */
+        zm_msg2_mm(ZM_LV_0, "RegDomain rsp=", rsp[3]);
+        zm_msg2_mm(ZM_LV_0, "OpFlags+EepMisc=", rsp[4]);
+        hpPriv->OpFlags = (u8_t)((rsp[4]>>16) & 0xff);
+        if ((rsp[2] >> 24) == 0x1) //Tx mask == 0x1
+        {
+            zm_msg0_mm(ZM_LV_0, "OTUS 1x2");
+            hpPriv->halCapability |= ZM_HP_CAP_11N_ONE_TX_STREAM;
+        }
+        else
+        {
+            zm_msg0_mm(ZM_LV_0, "OTUS 2x2");
+        }
+        if (hpPriv->OpFlags & 0x1)
+        {
+            hpPriv->halCapability |= ZM_HP_CAP_5G;
+        }
+        if (hpPriv->OpFlags & 0x2)
+        {
+            hpPriv->halCapability |= ZM_HP_CAP_2G;
+        }
+
+
+        CCS = (u8_t)((rsp[3] & 0x8000) >> 15);
+        WWR = (u8_t)((rsp[3] & 0x4000) >> 14);
+        CountryDomainCode = (u16_t)(rsp[3] & 0x3FFF);
+
+        if (rsp[3] != 0xffffffff)
+        {
+            if (CCS)
+            {
+                //zm_debug_msg0("CWY - Get Regulation Table from Country Code");
+                zfHpGetRegulationTablefromCountry(dev, CountryDomainCode);
+            }
+            else
+            {
+                //zm_debug_msg0("CWY - Get Regulation Table from Reg Domain");
+                zfHpGetRegulationTablefromRegionCode(dev, CountryDomainCode);
+            }
+            if (WWR)
+            {
+                //zm_debug_msg0("CWY - Enable 802.11d");
+                /* below line shall be unmarked after A band is ready */
+                //zfiWlanSetDot11DMode(dev, 1);
+            }
+        }
+        else
+        {
+            zfHpGetRegulationTablefromRegionCode(dev, NO_ENUMRD);
+        }
+
+        zfCoreMacAddressNotify(dev, addr);
+
+    }
+    else if (src == ZM_EEPROM_READ)
+    {
+#if 0
+        u8_t addr[6], CCS, WWR;
+        u16_t CountryDomainCode;
+#endif
+        for (i=0; i<ZM_HAL_MAX_EEPROM_PRQ; i++)
+        {
+            if (hpPriv->eepromImageIndex < 1024)
+            {
+                hpPriv->eepromImage[hpPriv->eepromImageIndex++] = rsp[i+1];
+            }
+        }
+
+        if (hpPriv->eepromImageIndex == (ZM_HAL_MAX_EEPROM_REQ*ZM_HAL_MAX_EEPROM_PRQ))
+        {
+            #if 0
+            for (i=0; i<1024; i++)
+            {
+                zm_msg2_mm(ZM_LV_0, "index=", i);
+                zm_msg2_mm(ZM_LV_0, "eepromImage=", hpPriv->eepromImage[i]);
+            }
+            #endif
+            zm_msg2_mm(ZM_LV_0, "MAC [1]=", hpPriv->eepromImage[0x20c/4]);
+            zm_msg2_mm(ZM_LV_0, "MAC [2]=", hpPriv->eepromImage[0x210/4]);
+#if 0
+            addr[0] = (u8_t)(hpPriv->eepromImage[0x20c/4] & 0xff);
+            addr[1] = (u8_t)((hpPriv->eepromImage[0x20c/4]>>8) & 0xff);
+            addr[2] = (u8_t)((hpPriv->eepromImage[0x20c/4]>>16) & 0xff);
+            addr[3] = (u8_t)((hpPriv->eepromImage[0x20c/4]>>24) & 0xff);
+            addr[4] = (u8_t)(hpPriv->eepromImage[0x210/4] & 0xff);
+            addr[5] = (u8_t)((hpPriv->eepromImage[0x210/4]>>8) & 0xff);
+
+            zfCoreMacAddressNotify(dev, addr);
+
+            zfDelayWriteInternalReg(dev, ZM_MAC_REG_MAC_ADDR_L,
+                    ((((u32_t)addr[3])<<24) | (((u32_t)addr[2])<<16) | (((u32_t)addr[1])<<8) | addr[0]));
+            zfDelayWriteInternalReg(dev, ZM_MAC_REG_MAC_ADDR_H,
+                    ((((u32_t)addr[5])<<8) | addr[4]));
+            zfFlushDelayWrite(dev);
+
+            /* Regulatory Related Setting */
+            zm_msg2_mm(ZM_LV_0, "RegDomain =", hpPriv->eepromImage[0x208/4]);
+            CCS = (u8_t)((hpPriv->eepromImage[0x208/4] & 0x8000) >> 15);
+            WWR = (u8_t)((hpPriv->eepromImage[0x208/4] & 0x4000) >> 14);
+            /* below line shall be unmarked after A band is ready */
+            //CountryDomainCode = (u16_t)(hpPriv->eepromImage[0x208/4] & 0x3FFF);
+            CountryDomainCode = 8;
+            if (CCS)
+            {
+                //zm_debug_msg0("CWY - Get Regulation Table from Country Code");
+                zfHpGetRegulationTablefromCountry(dev, CountryDomainCode);
+            }
+            else
+            {
+                //zm_debug_msg0("CWY - Get Regulation Table from Reg Domain");
+                zfHpGetRegulationTablefromRegionCode(dev, CountryDomainCode);
+            }
+            if (WWR)
+            {
+                //zm_debug_msg0("CWY - Enable 802.11d");
+                /* below line shall be unmarked after A band is ready */
+                //zfiWlanSetDot11DMode(dev, 1);
+            }
+#endif
+            zfCoreHalInitComplete(dev);
+        }
+        else
+        {
+            hpPriv->eepromImageRdReq++;
+            zfHpLoadEEPROMFromFW(dev);
+        }
+    }
+    else if (src == ZM_EEPROM_WRITE)
+    {
+        zfwDbgWriteEepromDone(dev, cmd[1], cmd[2]);
+    }
+    else if (src == ZM_ANI_READ)
+    {
+        u32_t cycleTime, ctlClear;
+
+        zm_msg2_mm(ZM_LV_0, "ANI rsp[1]=", rsp[1]);
+        zm_msg2_mm(ZM_LV_0, "ANI rsp[2]=", rsp[2]);
+        zm_msg2_mm(ZM_LV_0, "ANI rsp[3]=", rsp[3]);
+        zm_msg2_mm(ZM_LV_0, "ANI rsp[4]=", rsp[4]);
+
+        hpPriv->ctlBusy += rsp[1];
+        hpPriv->extBusy += rsp[2];
+
+        cycleTime = 100000; //100 miniseconds
+
+        if (cycleTime > rsp[1])
+        {
+            ctlClear = (cycleTime - rsp[1]) / 100;
+        }
+        else
+        {
+            ctlClear = 0;
+        }
+        if (wd->aniEnable)
+            zfHpAniArPoll(dev, ctlClear, rsp[3], rsp[4]);
+    }
+    else if (src == ZM_CMD_ECHO)
+    {
+        if ( ((struct zsHpPriv*)wd->hpPrivate)->halReInit )
+        {
+            zfCoreHalInitComplete(dev);
+            ((struct zsHpPriv*)wd->hpPrivate)->halReInit = 0;
+        }
+        else
+        {
+            zfHpLoadEEPROMFromFW(dev);
+        }
+    }
+    else if (src == ZM_OID_FW_DL_INIT)
+    {
+        zfwDbgDownloadFwInitDone(dev);
+    }
+    return;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfWriteRegInternalReg       */
+/*      Write on chip internal register immediately.                    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      addr : register address                                         */
+/*      val : value                                                     */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : success                                                     */
+/*      other : fail                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.11     */
+/*                                                                      */
+/************************************************************************/
+u32_t zfWriteRegInternalReg(zdev_t* dev, u32_t addr, u32_t val)
+{
+    u32_t cmd[3];
+    u16_t ret;
+
+    cmd[0] = 0x00000108;
+    cmd[1] = addr;
+    cmd[2] = val;
+
+    ret = zfIssueCmd(dev, cmd, 12, ZM_OID_INTERNAL_WRITE, NULL);
+    return ret;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfDelayWriteInternalReg     */
+/*      Write on chip internal register, write operation may be         */
+/*      postponed to form a multiple write command.                     */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      addr : register address                                         */
+/*      val : value                                                     */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : command been postponed                                      */
+/*      1 : commands been executed                                      */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.11     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfDelayWriteInternalReg(zdev_t* dev, u32_t addr, u32_t val)
+{
+    u32_t cmd[(ZM_MAX_CMD_SIZE/4)];
+    u16_t i;
+    u16_t ret;
+
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+    zmw_declare_for_critical_section();
+
+    /* enter critical section */
+    zmw_enter_critical_section(dev);
+
+    /* Store command to global buffer */
+    hpPriv->cmd.delayWcmdAddr[hpPriv->cmd.delayWcmdCount] = addr;
+    hpPriv->cmd.delayWcmdVal[hpPriv->cmd.delayWcmdCount++] = val;
+
+    /* If pending command reach size limit */
+    if ((hpPriv->cmd.delayWcmdCount) >= ((ZM_MAX_CMD_SIZE - 4) >> 3))
+    {
+        cmd[0] = 0x00000100 + (hpPriv->cmd.delayWcmdCount<<3);
+
+        /* copy command to cmd buffer */
+        for (i=0; i<hpPriv->cmd.delayWcmdCount; i++)
+        {
+            cmd[1+(i<<1)] = hpPriv->cmd.delayWcmdAddr[i];
+            cmd[2+(i<<1)] = hpPriv->cmd.delayWcmdVal[i];
+        }
+        /* reset pending command */
+        hpPriv->cmd.delayWcmdCount = 0;
+
+        /* leave critical section */
+        zmw_leave_critical_section(dev);
+
+        /* issue write command */
+        ret = zfIssueCmd(dev, cmd, 4+(i<<3), ZM_OID_INTERNAL_WRITE, NULL);
+
+        return 1;
+    }
+    else
+    {
+        /* leave critical section */
+        zmw_leave_critical_section(dev);
+
+        return 0;
+    }
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfFlushDelayWrite           */
+/*      Flush pending write command.                                    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : no pending command                                          */
+/*      1 : commands been executed                                      */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.11     */
+/*                                                                      */
+/************************************************************************/
+u16_t zfFlushDelayWrite(zdev_t* dev)
+{
+    u32_t cmd[(ZM_MAX_CMD_SIZE/4)];
+    u16_t i;
+    u16_t ret;
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+    zmw_declare_for_critical_section();
+
+    /* enter critical section */
+    zmw_enter_critical_section(dev);
+
+    /* If there is pending command */
+    if (hpPriv->cmd.delayWcmdCount > 0)
+    {
+        cmd[0] = 0x00000100 + (hpPriv->cmd.delayWcmdCount<<3);
+
+        /* copy command to cmd buffer */
+        for (i=0; i<hpPriv->cmd.delayWcmdCount; i++)
+        {
+            cmd[1+(i<<1)] = hpPriv->cmd.delayWcmdAddr[i];
+            cmd[2+(i<<1)] = hpPriv->cmd.delayWcmdVal[i];
+        }
+        /* reset pending command */
+        hpPriv->cmd.delayWcmdCount = 0;
+
+        /* leave critical section */
+        zmw_leave_critical_section(dev);
+
+        /* issue write command */
+        ret = zfIssueCmd(dev, cmd, 4+(i<<3), ZM_OID_INTERNAL_WRITE, NULL);
+
+        return 1;
+    }
+    else
+    {
+        /* leave critical section */
+        zmw_leave_critical_section(dev);
+
+        return 0;
+    }
+}
+
+
+u32_t zfiDbgDelayWriteReg(zdev_t* dev, u32_t addr, u32_t val)
+{
+	zfDelayWriteInternalReg(dev, addr, val);
+	return 0;
+}
+
+u32_t zfiDbgFlushDelayWrite(zdev_t* dev)
+{
+	zfFlushDelayWrite(dev);
+	return 0;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiDbgWriteReg              */
+/*      Write register.                                                 */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      addr : register address                                         */
+/*      val : value                                                     */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : success                                                     */
+/*      other : fail                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+u32_t zfiDbgWriteReg(zdev_t* dev, u32_t addr, u32_t val)
+{
+    u32_t cmd[3];
+    u16_t ret;
+
+    cmd[0] = 0x00000108;
+    cmd[1] = addr;
+    cmd[2] = val;
+
+    ret = zfIssueCmd(dev, cmd, 12, ZM_OID_WRITE, 0);
+    return ret;
+}
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiDbgWriteFlash            */
+/*      Write flash.                                                    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      addr : register address                                         */
+/*      val : value                                                     */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : success                                                     */
+/*      other : fail                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Yjsung        ZyDAS Technology Corporation    2007.02           */
+/*                                                                      */
+/************************************************************************/
+u32_t zfiDbgWriteFlash(zdev_t* dev, u32_t addr, u32_t val)
+{
+    u32_t cmd[3];
+    u16_t ret;
+
+    //cmd[0] = 0x0000B008;
+	/* len[0] : type[0xB0] : seq[?] */
+    cmd[0] = 8 | (ZM_CMD_WFLASH << 8);
+    cmd[1] = addr;
+    cmd[2] = val;
+
+    ret = zfIssueCmd(dev, cmd, 12, ZM_OID_WRITE, 0);
+    return ret;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiDbgWriteEeprom            */
+/*      Write EEPROM.                                                    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      addr : register address                                         */
+/*      val : value                                                     */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : success                                                     */
+/*      other : fail                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Paul        ZyDAS Technology Corporation    2007.06             */
+/*                                                                      */
+/************************************************************************/
+u32_t zfiDbgWriteEeprom(zdev_t* dev, u32_t addr, u32_t val)
+{
+    u32_t cmd[3];
+    u16_t ret;
+
+    //cmd[0] = 0x0000B008;
+	/* len[0] : type[0xB0] : seq[?] */
+    cmd[0] = 8 | (ZM_CMD_WREEPROM << 8);
+    cmd[1] = addr;
+    cmd[2] = val;
+
+    ret = zfIssueCmd(dev, cmd, 12, ZM_EEPROM_WRITE, 0);
+    return ret;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiDbgBlockWriteEeprom      */
+/*      Block Write Eeprom.                                             */
+/*                                                                      */
+/*      p.s: now,it will write 16 bytes register data per block (N=4)   */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      addr : register address                                         */
+/*      buf : input data buffer pointer                                 */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : success                                                     */
+/*      other : fail                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Paul        ZyDAS Technology Corporation    2007.06             */
+/*                                                                      */
+/************************************************************************/
+//#define N       buflen/4
+//#define SIZE    (2*N+1)
+
+u32_t zfiDbgBlockWriteEeprom(zdev_t* dev, u32_t addr, u32_t* buf)
+{
+    u32_t cmd[9];  //2N+1
+    u16_t ret,i;
+
+    //cmd[0] = 0x0000B008;
+	  /* len[0] : type[0xB0] : seq[?] */
+
+    //cmd[0] = (8*N) | (ZM_CMD_WFLASH << 8);
+    cmd[0] = 32 | (ZM_CMD_WREEPROM << 8);    //8N
+
+    for (i=0; i<4; i++)   // i<N
+    {
+        cmd[(2*i)+1] = addr+(4*i);
+        cmd[(2*i)+2] = *(buf+i);
+    }
+
+    ret = zfIssueCmd(dev, cmd, 36, ZM_EEPROM_WRITE, 0);    //8N+4
+
+    // added for EEPROMUpdate, wait a moment for prevent cmd queue full!
+    //zfwSleep(dev, 1);
+
+    return ret;
+}
+
+
+/* write EEPROM with wrlen : wrlen must be 4*n */
+/* command format : cmd_info(4) + addr(4) + eeprom(wrlen) */
+u32_t zfiDbgBlockWriteEeprom_v2(zdev_t* dev, u32_t addr, u32_t* buf, u32_t wrlen)
+{
+    u32_t cmd[16];
+    u16_t ret,i;
+
+	  /* len[0] : type[0xB0] : seq[?] */
+	  /* len = addr(4) + eeprom_block(wrlen) */
+    cmd[0] = (wrlen+4) | (ZM_CMD_MEM_WREEPROM << 8);
+    cmd[1] = addr;
+
+    for (i=0; i<(wrlen/4); i++)   // i<wrlen/4
+    {
+        cmd[2+i] = *(buf+i);
+    }
+    /* cmd_info(4) + addr(4) + eeprom(wrlen) */
+    ret = zfIssueCmd(dev, cmd, (u16_t)(wrlen+8), ZM_EEPROM_WRITE, 0);
+
+    return ret;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfDbgOpenEeprom            */
+/*      Open EEPROM.                                                    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Paul                ZyDAS Technology Corporation    2007.06     */
+/*                                                                      */
+/************************************************************************/
+void zfDbgOpenEeprom(zdev_t* dev)
+{
+    // unlock EEPROM
+    zfDelayWriteInternalReg(dev, 0x1D1400, 0x12345678);
+    zfDelayWriteInternalReg(dev, 0x1D1404, 0x55aa00ff);
+    zfDelayWriteInternalReg(dev, 0x1D1408, 0x13579ace);
+    zfDelayWriteInternalReg(dev, 0x1D1414, 0x0);
+    zfFlushDelayWrite(dev);
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfDbgCloseEeprom            */
+/*      Close EEPROM.                                                    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Paul                ZyDAS Technology Corporation    2007.05     */
+/*                                                                      */
+/************************************************************************/
+void zfDbgCloseEeprom(zdev_t* dev)
+{
+    // lock EEPROM
+    zfDelayWriteInternalReg(dev, 0x1D1400, 0x87654321);
+    //zfDelayWriteInternalReg(dev, 0x1D1404, 0xffffffff);
+    //zfDelayWriteInternalReg(dev, 0x1D1408, 0xffffffff);
+    //zfDelayWriteInternalReg(dev, 0x1D1414, 0x100);
+    zfFlushDelayWrite(dev);
+}
+#if 0
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiSeriallyWriteEeprom      */
+/*      Write EEPROM Serially.                                          */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      addr : start address of writing EEPROM                          */
+/*      buf : input data buffer                                         */
+/*      buflen : size of input data buffer                              */
+/*               (length of data write into EEPROM)                     */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*                                                                      */
+/*                                                                      */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Paul                ZyDAS Technology Corporation    2007.06     */
+/*                                                                      */
+/************************************************************************/
+u32_t zfiSeriallyWriteEeprom(zdev_t* dev, u32_t addr, u32_t* buf, u32_t buflen)
+{
+    u32_t count;
+    u16_t i,ret,blocksize;
+    u8_t  temp[2];
+
+    // per 4 bytes = 1 count
+    count = buflen/4;
+
+    // Open EEPROM
+    zfDbgOpenEeprom(dev);
+
+    // Write EEPROM
+    for (i=0; i<count; i++)
+    {
+        if (zfwWriteEeprom(dev, (addr+(4*i)), *(buf+i), 0) != 0)
+        {
+            // Update failed, Close EEPROM
+            zm_debug_msg0("zfwWriteEeprom failed \n");
+            zfDbgCloseEeprom(dev);
+            return 1;
+        }
+    }
+
+    // Close EEPROM
+    zfDbgCloseEeprom(dev);
+    return 0;
+}
+#endif
+#if 0
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiSeriallyBlockWriteEeprom */
+/*       Block Write EEPROM Serially.                                   */
+/*      (BlockWrite: per 16bytes write EEPROM once)                     */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      addr : register address                                         */
+/*      buf : input data buffer                                         */
+/*      buflen : access data size of buf                                */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : success                                                     */
+/*      other : fail                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Paul                ZyDAS Technology Corporation    2007.05     */
+/*                                                                      */
+/************************************************************************/
+u32_t zfiSeriallyBlockWriteEeprom(zdev_t* dev, u32_t addr, u32_t* buf, u32_t buflen)
+{
+    u32_t count;
+    u16_t i,ret,blocksize;
+    u8_t  temp[2];
+
+    // per 4 bytes = 1 count
+    count = buflen/4;
+
+    // Open EEPROM
+    zfDbgOpenEeprom(dev);
+
+    // Write EEPROM
+    // EEPROM Write start address from: 0x1000!?
+    // per 16bytes(N=4) block write EEPROM once
+    for (i=0; i<(count/4); i++)   // count/N
+    {
+        //zfiDbgBlockWriteEeprom(dev, (addr+(4*N*i)), buf+(N*i));
+        //zfiDbgBlockWriteEeprom(dev, (addr+(16*i)), buf+(4*i));
+        if (zfwBlockWriteEeprom(dev, (addr+(16*i)), buf+(4*i), 0) != 0)
+        {
+            zm_debug_msg0("zfiDbgBlockWriteEeprom failed \n");
+            // Close EEPROM
+            zfDbgCloseEeprom(dev);
+            return 1;
+        }
+    }
+
+    // Close EEPROM
+    zfDbgCloseEeprom(dev);
+    return 0;
+}
+#endif
+#if 0
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiDbgDumpEeprom            */
+/*      Dump EEPROM.                                                    */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      addr : start address of dumping EEPROM                          */
+/*      datalen :  length of access EEPROM data                           */
+/*      buf :  point of buffer, the buffer saved dump data              */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : success                                                     */
+/*      other : fail                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Paul                ZyDAS Technology Corporation    2007.06     */
+/*                                                                      */
+/************************************************************************/
+u32_t zfiDbgDumpEeprom(zdev_t* dev, u32_t addr, u32_t datalen, u32_t* buf)
+{
+    u32_t count;
+    u16_t i,ret;
+
+    count = datalen/4;
+
+    // over EEPROM length
+    if(datalen > 0x2000)
+    {
+        return 1;
+    }
+
+    for(i=0; i<count; i++)
+    {
+        buf[i] = zfwReadEeprom(dev, addr+(4*i));
+    }
+
+    return 0;
+}
+#endif
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiDbgReadReg               */
+/*      Read register.                                                  */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      addr : register address                                         */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : success                                                     */
+/*      other : fail                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+u32_t zfiDbgReadReg(zdev_t* dev, u32_t addr)
+{
+    u32_t cmd[2];
+    u16_t ret;
+
+    cmd[0] = 0x00000004;
+    cmd[1] = addr;
+
+    ret = zfIssueCmd(dev, cmd, 8, ZM_OID_READ, 0);
+    return ret;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiDbgReadTally             */
+/*      Read register.                                                  */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : success                                                     */
+/*      other : fail                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.10     */
+/*                                                                      */
+/************************************************************************/
+u32_t zfiDbgReadTally(zdev_t* dev)
+{
+    u32_t cmd[1];
+    u16_t ret;
+	zmw_get_wlan_dev(dev);
+
+	if ( ((struct zsHpPriv*)wd->hpPrivate)->halReInit )
+	{
+	    return 1;
+	}
+
+	/* len[0] : type[0x81] : seq[?] */
+    cmd[0] = 0 | (ZM_CMD_TALLY << 8);
+    ret = zfIssueCmd(dev, cmd, 4, ZM_OID_TALLY, 0);
+
+	/* len[0] : type[0x82] : seq[?] */
+    cmd[0] = 0 | (ZM_CMD_TALLY_APD << 8);
+    ret = zfIssueCmd(dev, cmd, 4, ZM_OID_TALLY_APD, 0);
+
+    return ret;
+}
+
+
+u32_t zfiDbgSetIFSynthesizer(zdev_t* dev, u32_t value)
+{
+    u32_t cmd[2];
+    u16_t ret;
+
+	/* len[4] : type[0x32] : seq[?] */
+    cmd[0] = 0x4 | (ZM_OID_SYNTH << 8);
+    cmd[1] = value;
+
+    ret = zfIssueCmd(dev, cmd, 8, ZM_OID_SYNTH, 0);
+    return ret;
+}
+
+u32_t zfiDbgQueryHwTxBusy(zdev_t* dev)
+{
+    u32_t cmd[1];
+    u16_t ret;
+
+	/* len[4] : type[0xC0] : seq[?] */
+	cmd[0] = 0 | (ZM_CMD_DKTX_STATUS << 8);
+
+    ret = zfIssueCmd(dev, cmd, 4, ZM_OID_DKTX_STATUS, 0);
+    return ret;
+}
+
+//Paul++
+#if 0
+u16_t zfHpBlockEraseFlash(zdev_t *dev, u32_t addr)
+{
+    u32_t cmd[(ZM_MAX_CMD_SIZE/4)];
+    u16_t ret;
+
+    cmd[0] = 0x00000004 | (ZM_CMD_FLASH_ERASE << 8);
+    cmd[1] = addr;
+
+    ret = zfIssueCmd(dev, cmd, 8, ZM_OID_INTERNAL_WRITE, NULL);
+    return ret;
+}
+#endif
+
+#if 0
+u16_t zfiDbgProgramFlash(zdev_t *dev, u32_t offset, u32_t len, u32_t *data)
+{
+    u32_t cmd[(ZM_MAX_CMD_SIZE/4)];
+    u16_t ret;
+    u16_t i;
+
+
+    cmd[0] = (ZM_CMD_FLASH_PROG << 8) | ((len+8) & 0xff);
+    cmd[1] = offset;
+    cmd[2] = len;
+
+    for (i = 0; i < (len >> 2); i++)
+    {
+         cmd[3+i] = data[i];
+    }
+
+    ret = zfIssueCmd(dev, cmd, 12, ZM_OID_FLASH_PROGRAM, NULL);
+
+    return ret;
+}
+#endif
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiDbgChipEraseFlash        */
+/*      Chip Erase Flash.                                               */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : success                                                     */
+/*      other : fail                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Paul                Atheros Technology Corporation    2007.09   */
+/*                                                                      */
+/************************************************************************/
+u16_t zfiDbgChipEraseFlash(zdev_t *dev)
+{
+    u32_t cmd[(ZM_MAX_CMD_SIZE/4)];
+    u16_t ret;
+
+    cmd[0] = 0x00000000 | (ZM_CMD_FLASH_ERASE << 8);
+
+    ret = zfIssueCmd(dev, cmd, 4, ZM_OID_INTERNAL_WRITE, NULL);
+    return ret;
+}
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiDbgGetFlashCheckSum      */
+/*      Get FlashCheckSum.                                              */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      addr : Start address of getchksum                               */
+/*      len : total lenth of calculate getchksum                        */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : success                                                     */
+/*      other : fail                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Paul                Atheros Technology Corporation    2007.08   */
+/*                                                                      */
+/************************************************************************/
+u32_t zfiDbgGetFlashCheckSum(zdev_t *dev, u32_t addr, u32_t len)
+{
+    u32_t cmd[(ZM_MAX_CMD_SIZE/4)];
+    u32_t ret;
+
+    cmd[0] = 0x00000008 | (ZM_CMD_FLASH_CHKSUM << 8);
+    cmd[1] = addr;
+    cmd[2] = len;
+
+    ret = zfIssueCmd(dev, cmd, 12, ZM_OID_FLASH_CHKSUM, NULL);
+
+    return ret;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiDbgReadFlash             */
+/*      Read Flash.                                                     */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      addr : Start address of read flash                              */
+/*      len : total lenth of read flash data                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : success                                                     */
+/*      other : fail                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Paul                Atheros Technology Corporation    2007.09   */
+/*                                                                      */
+/************************************************************************/
+u32_t zfiDbgReadFlash(zdev_t *dev, u32_t addr, u32_t len)
+{
+    u32_t cmd[(ZM_MAX_CMD_SIZE/4)];
+    u32_t ret;
+
+    cmd[0] = len | (ZM_CMD_FLASH_READ << 8);
+    cmd[1] = addr;
+
+    ret = zfIssueCmd(dev, cmd, 8, ZM_OID_FLASH_READ, NULL);
+    return ret;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiDownloadFwSet            */
+/*      Before Download FW,                                             */
+/*      Command FW to Software reset and close watch dog control.       */
+/*                                                                      */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      0 : success                                                     */
+/*      other : fail                                                    */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Paul                Atheros Technology Corporation    2007.09   */
+/*                                                                      */
+/************************************************************************/
+u32_t zfiDownloadFwSet(zdev_t *dev)
+{
+//softwarereset
+//close watch dog
+    u32_t cmd[(ZM_MAX_CMD_SIZE/4)];
+    u32_t ret;
+
+    cmd[0] = 0x00000008 | (ZM_CMD_FW_DL_INIT << 8);
+
+    ret = zfIssueCmd(dev, cmd, 12, ZM_OID_FW_DL_INIT, NULL);
+
+    return ret;
+}
+//Paul--
diff --git a/drivers/staging/otus/hal/hpusb.c b/drivers/staging/otus/hal/hpusb.c
new file mode 100644
index 00000000000..4b76de93fff
--- /dev/null
+++ b/drivers/staging/otus/hal/hpusb.c
@@ -0,0 +1,1584 @@
+/*
+ * Copyright (c) 2000-2005 ZyDAS Technology Corporation
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : ud.c                                                  */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains USB descriptor functions.                  */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+#include "../80211core/cprecomp.h"
+#include "hpani.h"
+#include "hpusb.h"
+
+extern void zfwUsbCmd(zdev_t* dev, u8_t endpt, u32_t* cmd, u16_t cmdLen);
+
+extern void zfIdlRsp(zdev_t* dev, u32_t* rsp, u16_t rspLen);
+extern u16_t zfDelayWriteInternalReg(zdev_t* dev, u32_t addr, u32_t val);
+extern u16_t zfFlushDelayWrite(zdev_t* dev);
+
+
+#define USB_ENDPOINT_TX_INDEX   1
+#define USB_ENDPOINT_RX_INDEX   2
+#define USB_ENDPOINT_INT_INDEX  3
+#define USB_ENDPOINT_CMD_INDEX  4
+
+void zfIdlCmd(zdev_t* dev, u32_t* cmd, u16_t cmdLen)
+{
+#if ZM_SW_LOOP_BACK != 1
+    zfwUsbCmd(dev, USB_ENDPOINT_CMD_INDEX, cmd, cmdLen);
+#endif
+
+    return;
+}
+
+
+/* zfAdjustCtrlSetting: fit OUTS format */
+/*     convert MIMO2 to OUTS             */
+void zfAdjustCtrlSetting(zdev_t* dev, u16_t* header, zbuf_t* buf)
+{
+    /* MIMO2 => OUTS FB-50 */
+    /* length not change, only modify format */
+
+    u32_t oldMT;
+	u32_t oldMCS;
+
+    u32_t phyCtrl;
+    u32_t oldPhyCtrl;
+
+    u16_t tpc = 0;
+
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+   /* mm */
+    if (header == NULL)
+    {
+        oldPhyCtrl = zmw_buf_readh(dev, buf, 4) | ((u32_t)zmw_buf_readh(dev, buf, 6) << 16);
+    }
+    else
+    {
+        oldPhyCtrl = header[2] | ((u32_t)header[3] <<16);
+    }
+
+	phyCtrl = 0;
+
+
+	/* MT : Bit[1~0] */
+	oldMT = oldPhyCtrl&0x3;
+	phyCtrl |= oldMT;
+    if ( oldMT == 0x3 )   /* DL-OFDM (Duplicate Legacy OFDM) */
+		phyCtrl |= 0x1;
+
+
+	/* PT : Bit[2]    HT PT: 0 Mixed mode    1 Green field */
+	phyCtrl |= (oldPhyCtrl&0x4);
+
+	/* Bandwidth control : Bit[4~3] */
+	if ( oldPhyCtrl&0x800000 )    /* Bit23 : 40M */
+	{
+		#if 0
+		if (oldMT == 0x3)             /* DL-OFDM */
+            phyCtrl |= (0x3<<3);   /* 40M duplicate */
+		else
+			phyCtrl |= (0x2<<3);   /* 40M shared */
+		#else
+		if (oldMT == 0x2 && ((struct zsHpPriv*)wd->hpPrivate)->hwBw40)
+		{
+			phyCtrl |= (0x2<<3);   /* 40M shared */
+		}
+		#endif
+	}
+	else {
+        oldPhyCtrl &= ~0x80000000;
+    }
+
+	/* MCS : Bit[24~18] */
+	oldMCS = (oldPhyCtrl&0x7f0000)>>16;  /* Bit[22~16] */
+	phyCtrl |= (oldMCS<<18);
+
+	/* Short GI : Bit[31]*/
+    phyCtrl |= (oldPhyCtrl&0x80000000);
+
+	/* AM : Antenna mask */
+	//if ((oldMT == 2) && (oldMCS > 7))
+	if (hpPriv->halCapability & ZM_HP_CAP_11N_ONE_TX_STREAM)
+	{
+	    phyCtrl |= (0x1<<15);
+	}
+	else
+	{
+	    /* HT                     Tx 2 chain */
+	    /* OFDM 6M/9M/12M/18M/24M Tx 2 chain */
+	    /* OFDM 36M/48M/54M/      Tx 1 chain */
+	    /* CCK                    Tx 2 chain */
+	    if ((oldMT == 2) || (oldMT == 3))
+	    {
+	        phyCtrl |= (0x5<<15);
+	    }
+	    else if (oldMT == 1)
+	    {
+	        if ((oldMCS == 0xb) || (oldMCS == 0xf) ||
+	            (oldMCS == 0xa) || (oldMCS == 0xe) ||
+	            (oldMCS == 0x9))                       //6M/9M/12M/18M/24M
+	        {
+	            phyCtrl |= (0x5<<15);
+	        }
+	        else
+	        {
+	            phyCtrl |= (0x1<<15);
+	        }
+	    }
+	    else //(oldMT==0)
+	    {
+	        phyCtrl |= (0x5<<15);
+	    }
+	}
+	//else
+	//    phyCtrl |= (0x1<<15);
+
+	/* TPC */
+	/* TODO : accelerating these code */
+	if (hpPriv->hwFrequency < 3000)
+	{
+        if (oldMT == 0)
+        {
+            /* CCK */
+            tpc = (hpPriv->tPow2xCck[oldMCS]&0x3f);
+        }
+        else if (oldMT == 1)
+        {
+            /* OFDM */
+            if (oldMCS == 0xc)
+            {
+                tpc = (hpPriv->tPow2x2g[3]&0x3f);
+            }
+            else if (oldMCS == 0x8)
+            {
+                tpc = (hpPriv->tPow2x2g[2]&0x3f);
+            }
+            else if (oldMCS == 0xd)
+            {
+                tpc = (hpPriv->tPow2x2g[1]&0x3f);
+            }
+            else if (oldMCS == 0x9)
+            {
+                tpc = ((hpPriv->tPow2x2g[0]-hpPriv->tPow2x2g24HeavyClipOffset)&0x3f);
+            }
+            else
+            {
+                tpc = (hpPriv->tPow2x2g[0]&0x3f);
+            }
+        }
+        else if (oldMT == 2)
+        {
+            if ( oldPhyCtrl&0x800000 )    /* Bit23 : 40M */
+            {
+                /* HT 40 */
+                tpc = (hpPriv->tPow2x2gHt40[oldMCS&0x7]&0x3f);
+            }
+            else
+            {
+                /* HT 20 */
+                tpc = (hpPriv->tPow2x2gHt20[oldMCS&0x7]&0x3f);
+            }
+        }
+    }
+    else  //5GHz
+    {
+        if (oldMT == 1)
+        {
+            /* OFDM */
+            if (oldMCS == 0xc)
+            {
+                tpc = (hpPriv->tPow2x5g[3]&0x3f);
+            }
+            else if (oldMCS == 0x8)
+            {
+                tpc = (hpPriv->tPow2x5g[2]&0x3f);
+            }
+            else if (oldMCS == 0xd)
+            {
+                tpc = (hpPriv->tPow2x5g[1]&0x3f);
+            }
+            else
+            {
+                tpc = (hpPriv->tPow2x5g[0]&0x3f);
+            }
+        }
+        else if (oldMT == 2)
+        {
+            if ( oldPhyCtrl&0x800000 )    /* Bit23 : 40M */
+            {
+                /* HT 40 */
+                tpc = (hpPriv->tPow2x5gHt40[oldMCS&0x7]&0x3f);
+            }
+            else
+            {
+                /* HT 20 */
+                tpc = (hpPriv->tPow2x5gHt20[oldMCS&0x7]&0x3f);
+            }
+        }
+    }
+
+    /* Tx power adjust for HT40 */
+	/* HT40   +1dBm */
+	if ((oldMT==2) && (oldPhyCtrl&0x800000) )
+	{
+	    tpc += 2;
+	}
+	tpc &= 0x3f;
+
+    /* Evl force tx TPC */
+    if(wd->forceTxTPC)
+    {
+        tpc = (u16_t)(wd->forceTxTPC & 0x3f);
+    }
+
+    if (hpPriv->hwFrequency < 3000) {
+        wd->maxTxPower2 &= 0x3f;
+        tpc = (tpc > wd->maxTxPower2)? wd->maxTxPower2 : tpc;
+    } else {
+        wd->maxTxPower5 &= 0x3f;
+        tpc = (tpc > wd->maxTxPower5)? wd->maxTxPower5 : tpc;
+    }
+
+
+#define ZM_MIN_TPC     5
+#define ZM_TPC_OFFSET  5
+#define ZM_SIGNAL_THRESHOLD  56
+    if ((wd->sta.bScheduleScan == FALSE) && (wd->sta.bChannelScan == FALSE))
+    {
+        if (( wd->wlanMode == ZM_MODE_INFRASTRUCTURE )
+                && (zfStaIsConnected(dev))
+                && (wd->SignalStrength > ZM_SIGNAL_THRESHOLD))
+        {
+            if (tpc > ((ZM_MIN_TPC+ZM_TPC_OFFSET)*2))
+            {
+                tpc -= (ZM_TPC_OFFSET*2);
+            }
+            else if (tpc > (ZM_MIN_TPC*2))
+            {
+                tpc = (ZM_MIN_TPC*2);
+            }
+        }
+    }
+#undef ZM_MIN_TPC
+#undef ZM_TPC_OFFSET
+#undef ZM_SIGNAL_THRESHOLD
+
+    #ifndef ZM_OTUS_LINUX_PHASE_2
+    phyCtrl |= (tpc & 0x3f) << 9;
+    #endif
+
+    /* Set bits[8:6]BF-MCS for heavy clip */
+    if ((phyCtrl&0x3) == 2)
+	{
+	    phyCtrl |= ((phyCtrl >> 12) & 0x1c0);
+    }
+
+	/* PHY control */
+    if (header == NULL)
+    {
+        zmw_buf_writeh(dev, buf, 4, (u16_t) (phyCtrl&0xffff));
+        zmw_buf_writeh(dev, buf, 6, (u16_t) (phyCtrl>>16));
+    }
+    else
+    {
+        //PHY control L
+        header[2] = (u16_t) (phyCtrl&0xffff);
+        //PHY control H
+        header[3] = (u16_t) (phyCtrl>>16);
+    }
+
+	zm_msg2_tx(ZM_LV_2, "old phy ctrl = ", oldPhyCtrl);
+    zm_msg2_tx(ZM_LV_2, "new phy ctrl = ", phyCtrl);
+	//DbgPrint("old phy ctrl =%08x \n", oldPhyCtrl);
+    //DbgPrint("new phy ctrl =%08x \n", phyCtrl);
+}
+
+
+#define EXTRA_INFO_LEN      24    //RSSI(7) + EVM(12) + PHY(1) + MACStatus(4)
+u16_t zfHpSend(zdev_t* dev, u16_t* header, u16_t headerLen,
+                u16_t* snap, u16_t snapLen,
+                u16_t* tail, u16_t tailLen, zbuf_t* buf, u16_t offset,
+                u16_t bufType, u8_t ac, u8_t keyIdx)
+{
+#if ZM_SW_LOOP_BACK == 1
+    zbuf_t *rxbuf;
+    u8_t *puRxBuf;
+    u8_t *pHdr;
+	   u8_t *psnap;
+	   u16_t plcplen = 12;
+    u16_t i;
+   	u16_t swlpOffset;
+#endif /* #if ZM_SW_LOOP_BACK == 1 */
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+    zm_msg1_tx(ZM_LV_1, "zfHpSend(), len = ", 12 + headerLen-8 + snapLen + zfwBufGetSize(dev, buf) + 4 + 8);
+
+	/* Adjust ctrl setting : 6N14 yjsung */
+    zfAdjustCtrlSetting(dev, header, buf);
+
+#if ZM_SW_LOOP_BACK != 1
+    hpPriv->usbSendBytes += zfwBufGetSize(dev, buf);
+    hpPriv->usbAcSendBytes[ac&0x3] += zfwBufGetSize(dev, buf);
+
+    /* Submit USB Out Urb */
+    zfwUsbSend(dev, USB_ENDPOINT_TX_INDEX, (u8_t *)header, headerLen,
+                  (u8_t *)snap, snapLen, (u8_t *)tail, tailLen, buf, offset);
+#endif
+
+#if ZM_SW_LOOP_BACK == 1
+
+    rxbuf = zfwBufAllocate(dev, plcplen + headerLen-8 + snapLen + (zfwBufGetSize(dev, buf)-offset) + 4 + EXTRA_INFO_LEN);
+    pHdr = (u8_t *) header+8;
+   	psnap = (u8_t *) snap;
+
+    zmw_enter_critical_section(dev);
+    /* software loop back */
+    /* Copy WLAN header and packet buffer */
+   	swlpOffset = plcplen;
+
+    for(i = 0; i < headerLen-8; i++)
+    {
+        zmw_rx_buf_writeb(dev, rxbuf, swlpOffset+i, pHdr[i]);
+    }
+
+   	swlpOffset += headerLen-8;
+
+    /* Copy SNAP header */
+    for(i = 0; i < snapLen; i++)
+    {
+		      zmw_rx_buf_writeb(dev, rxbuf, swlpOffset+i, psnap[i]);
+    }
+
+	   swlpOffset += snapLen;
+
+    /* Copy body from tx buf to rxbuf */
+    for(i = 0; i < (zfwBufGetSize(dev, buf)-offset); i++)
+    {
+        u8_t value = zmw_rx_buf_readb(dev, buf, i+offset);
+        zmw_rx_buf_writeb(dev, rxbuf, swlpOffset+i, value);
+    }
+
+	   /* total length = PLCP +         MacHeader       + Payload   + FCS + RXstatus */
+	   /*                 12  +  headerLen-8  + snapLen + buf length + 4  + 8        */
+   	zfwSetBufSetSize(dev, rxbuf, swlpOffset + (zfwBufGetSize(dev, buf)-offset) + 4 + EXTRA_INFO_LEN );
+
+    zmw_leave_critical_section(dev);
+
+    zfwBufFree(dev, buf, 0);
+
+	   //zfwDumpBuf(dev, rxbuf);
+	   //-------------------------------------------------
+
+    //zfCoreRecv(dev, rxbuf);
+
+#endif /* #if ZM_SW_LOOP_BACK */
+
+    return ZM_SUCCESS;
+}
+
+/* Report moniter Hal rx information about rssi, evm, bandwidth, SG etc */
+void zfHpQueryMonHalRxInfo(zdev_t* dev, u8_t *monHalRxInfo)
+{
+    zmw_get_wlan_dev(dev);
+    zfMemoryCopy(monHalRxInfo,
+                (u8_t*)&(((struct zsHpPriv*)wd->hpPrivate)->halRxInfo),
+                sizeof(struct zsHalRxInfo));
+}
+
+
+u8_t zfIsDataFrame(zdev_t* dev, zbuf_t* buf)
+{
+    u8_t frameType;
+    u8_t mpduInd;
+
+    mpduInd = zmw_rx_buf_readb(dev, buf, zfwBufGetSize(dev, buf)-1);
+
+    /* sinlge or First */
+    if ((mpduInd & 0x30) == 0x00 || (mpduInd & 0x30) == 0x20)
+    {
+        frameType = zmw_rx_buf_readb(dev, buf, 12);
+    }
+    else
+    {
+        frameType = zmw_rx_buf_readb(dev, buf, 0);
+    }
+
+    if((frameType & 0xf) == ZM_WLAN_DATA_FRAME)
+        return 1;
+    else
+        return 0;
+}
+
+u32_t zfcConvertRateOFDM(zdev_t* dev, zbuf_t* buf)
+{
+    // What's the default value??
+    u32_t MCS = 0;
+
+    switch(zmw_rx_buf_readb(dev, buf, 0)& 0xf)
+    {
+        case 0xb:
+            MCS = 0x4;
+            break;
+        case 0xf:
+            MCS = 0x5;
+            break;
+        case 0xa:
+            MCS = 0x6;
+            break;
+        case 0xe:
+            MCS = 0x7;
+            break;
+        case 0x9:
+            MCS = 0x8;
+            break;
+        case 0xd:
+            MCS = 0x9;
+            break;
+        case 0x8:
+            MCS = 0xa;
+            break;
+        case 0xc:
+            MCS = 0xb;
+            break;
+    }
+    return MCS;
+}
+
+u16_t zfHpGetPayloadLen(zdev_t* dev,
+                        zbuf_t* buf,
+                        u16_t len,
+                        u16_t plcpHdrLen,
+                        u32_t *rxMT,
+                        u32_t *rxMCS,
+                        u32_t *rxBW,
+                        u32_t *rxSG
+                        )
+{
+    u8_t modulation,mpduInd;
+    u16_t low, high, msb;
+    s16_t payloadLen = 0;
+
+    zmw_get_wlan_dev(dev);
+
+    mpduInd = zmw_rx_buf_readb(dev, buf, len-1);
+    modulation = zmw_rx_buf_readb(dev, buf, (len-1)) & 0x3;
+    *rxMT = modulation;
+
+    //zm_debug_msg1(" modulation= ", modulation);
+    switch (modulation) {
+    case 0: /* CCK Mode */
+        low = zmw_rx_buf_readb(dev, buf, 2);
+        high = zmw_rx_buf_readb(dev, buf, 3);
+        payloadLen = (low | high << 8) - 4;
+        if (wd->enableHALDbgInfo)
+        {
+            *rxMCS = zmw_rx_buf_readb(dev, buf, 0);
+            *rxBW  = 0;
+            *rxSG  = 0;
+        }
+        break;
+    case 1: /* Legacy-OFDM mode */
+        low = zmw_rx_buf_readb(dev, buf, 0) >> 5;
+        high = zmw_rx_buf_readb(dev, buf, 1);
+        msb = zmw_rx_buf_readb(dev, buf, 2) & 0x1;
+        payloadLen = (low | (high << 3) | (msb << 11)) - 4;
+        if (wd->enableHALDbgInfo)
+        {
+            *rxMCS = zfcConvertRateOFDM(dev, buf);
+            *rxBW  = 0;
+            *rxSG  = 0;
+        }
+        break;
+    case 2: /* HT OFDM mode */
+        //zm_debug_msg1("aggregation= ", (zmw_rx_buf_readb(dev, buf, 6) >> 3) &0x1 );
+        if ((mpduInd & 0x30) == 0x00 || (mpduInd & 0x30) == 0x10)    //single or last mpdu
+            payloadLen = len - 24 - 4 - plcpHdrLen;  // - rxStatus - fcs
+        else {
+            payloadLen = len - 4 - 4 - plcpHdrLen;  // - rxStatus - fcs
+            //zm_debug_msg1("first or middle mpdu, plcpHdrLen= ", plcpHdrLen);
+        }
+        if (wd->enableHALDbgInfo)
+        {
+            *rxMCS = zmw_rx_buf_readb(dev, buf, 3) & 0x7f;
+            *rxBW  = (zmw_rx_buf_readb(dev, buf, 3) >> 7) & 0x1;
+            *rxSG  = (zmw_rx_buf_readb(dev, buf, 6) >> 7) & 0x1;
+        }
+        break;
+    default:
+        break;
+
+    }
+    /* return the payload length - FCS */
+    if (payloadLen < 0) payloadLen = 0;
+    return payloadLen;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfiUsbRecv                  */
+/*      Callback function for USB IN Transfer.                          */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev: device pointer                                             */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Yuan-Gu Wei        ZyDAS Technology Corporation    2005.10      */
+/*                                                                      */
+/************************************************************************/
+#define ZM_INT_USE_EP2                1
+#define ZM_INT_USE_EP2_HEADER_SIZE   12
+
+#if ZM_INT_USE_EP2 == 1
+void zfiUsbRegIn(zdev_t* dev, u32_t* rsp, u16_t rspLen);
+#endif
+
+#ifdef ZM_OTUS_RX_STREAM_MODE
+void zfiUsbRecvPerPkt(zdev_t *dev, zbuf_t *buf)
+#else
+void zfiUsbRecv(zdev_t *dev, zbuf_t *buf)
+#endif
+{
+
+
+#if ZM_FW_LOOP_BACK != 1
+    u8_t mpduInd;
+    u16_t plcpHdrLen;
+    u16_t crcPlusRxStatusLen;
+    u16_t len, payloadLen=0;
+    u16_t i; //CWYang(+)
+    struct zsAdditionInfo addInfo;
+    u32_t               rxMT;
+    u32_t               rxMCS;
+    u32_t               rxBW;
+    u32_t               rxSG;
+    zmw_get_wlan_dev(dev);
+    struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+    //zm_msg0_rx(ZM_LV_0, "zfiUsbRecv()");
+
+#if ZM_INT_USE_EP2 == 1
+
+    for (i=0; i<(ZM_INT_USE_EP2_HEADER_SIZE>>1); i++)
+    {
+        if (zmw_rx_buf_readh(dev, buf, i*2) != 0xffff)
+        	break;
+    }
+
+    if (i==(ZM_INT_USE_EP2_HEADER_SIZE>>1))
+    {
+        u32_t               rsp[ZM_USB_MAX_EPINT_BUFFER/4];
+        u16_t               rspLen;
+        u32_t               rspi;
+        u8_t*               pdst = (u8_t*)rsp;
+
+        /* Interrupt Rsp */
+        rspLen = (u16_t) zfwBufGetSize(dev, buf)-ZM_INT_USE_EP2_HEADER_SIZE;
+
+        if (rspLen > 60)
+        {
+            zm_debug_msg1("Get error len by EP2 = \n", rspLen);
+            /* free USB buf */
+	          zfwBufFree(dev, buf, 0);
+	          return;
+        }
+
+        for (rspi=0; rspi<rspLen; rspi++)
+        {
+        	*pdst = zmw_rx_buf_readb(dev, buf, rspi+ZM_INT_USE_EP2_HEADER_SIZE);
+        	pdst++;
+        }
+
+        //if (adapter->zfcbUsbRegIn)
+        //    adapter->zfcbUsbRegIn(adapter, rsp, rspLen);
+        zfiUsbRegIn(dev, rsp, rspLen);
+
+	      /* free USB buf */
+	      zfwBufFree(dev, buf, 0);
+	      return;
+    }
+#endif /* end of #if ZM_INT_USE_EP2 == 1 */
+
+    ZM_PERFORMANCE_RX_MPDU(dev, buf);
+
+    if (wd->swSniffer)
+    {
+        /* airopeek: Report everything up */
+        if (wd->zfcbRecv80211 != NULL)
+        {
+            wd->zfcbRecv80211(dev, buf, NULL);
+        }
+    }
+
+    /* Read the last byte */
+    len = zfwBufGetSize(dev, buf);
+    mpduInd = zmw_rx_buf_readb(dev, buf, len-1);
+
+    /* First MPDU */
+    if((mpduInd & 0x30) == 0x20)
+    {
+        u16_t duration;
+        if (zmw_rx_buf_readb(dev, buf, 36) == 0) //AC = BE
+        {
+            duration = zmw_rx_buf_readh(dev, buf, 14);
+            if (duration > hpPriv->aggMaxDurationBE)
+            {
+                hpPriv->aggMaxDurationBE = duration;
+            }
+            else
+            {
+                if (hpPriv->aggMaxDurationBE > 10)
+                {
+                    hpPriv->aggMaxDurationBE--;
+                }
+            }
+            //DbgPrint("aggMaxDurationBE=%d", hpPriv->aggMaxDurationBE);
+        }
+    }
+
+#if 1
+    /* First MPDU or Single MPDU */
+    if(((mpduInd & 0x30) == 0x00) || ((mpduInd & 0x30) == 0x20))
+    //if ((mpduInd & 0x10) == 0x00)
+    {
+        plcpHdrLen = 12;        // PLCP header length
+    }
+    else
+    {
+        if (zmw_rx_buf_readh(dev, buf, 4) == wd->macAddr[0] &&
+            zmw_rx_buf_readh(dev, buf, 6) == wd->macAddr[1] &&
+            zmw_rx_buf_readh(dev, buf, 8) == wd->macAddr[2]) {
+            plcpHdrLen = 0;
+        }
+        else if (zmw_rx_buf_readh(dev, buf, 16) == wd->macAddr[0] &&
+                 zmw_rx_buf_readh(dev, buf, 18) == wd->macAddr[1] &&
+                 zmw_rx_buf_readh(dev, buf, 20) == wd->macAddr[2]){
+            plcpHdrLen = 12;
+        }
+        else {
+            plcpHdrLen = 0;
+        }
+    }
+
+    /* Last MPDU or Single MPDU */
+    if ((mpduInd & 0x30) == 0x00 || (mpduInd & 0x30) == 0x10)
+    {
+        crcPlusRxStatusLen = EXTRA_INFO_LEN + 4;     // Extra bytes + FCS
+    }
+    else
+    {
+        crcPlusRxStatusLen = 4 + 4;     // Extra 4 bytes + FCS
+    }
+#else
+    plcpHdrLen = 12;
+    crcPlusRxStatusLen = EXTRA_INFO_LEN + 4;     // Extra bytes + FCS
+#endif
+
+    if (len < (plcpHdrLen+10+crcPlusRxStatusLen))
+    {
+        zm_msg1_rx(ZM_LV_0, "Invalid Rx length=", len);
+        //zfwDumpBuf(dev, buf);
+
+        zfwBufFree(dev, buf, 0);
+        return;
+    }
+
+    /* display RSSI combined */
+    /*
+     * ¢z¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢{
+     * ¢x PLCP Header ¢x  MPDU  ¢x RSSI ¢x  EVM ¢x PHY Err ¢x  MAC Status ¢x
+     * ¢u¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢t
+     * ¢x     12      ¢x    n   ¢x  7   ¢x  12  ¢x    1    ¢x      4      ¢x
+     * ¢|¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢}
+     *	RSSI filed (From BB and MAC just pass them to host)
+     *   Byte1: RSSI for antenna 0.
+     *   Byte2: RSSI for antenna 1.
+     *   Byte3: RSSI for antenna 2.
+     *   Byte4: RSSI for antenna 0 extension.
+     *   Byte5: RSSI for antenna 1 extension.
+     *   Byte6: RSSI for antenna 2 extension.
+     *   Byte7: RSSI for antenna combined.
+     */
+
+    //zm_debug_msg1(" recv RSSI = ", zmw_rx_buf_readb(dev, buf, (len-1)-17));
+
+    payloadLen = zfHpGetPayloadLen(dev, buf, len, plcpHdrLen, &rxMT, &rxMCS, &rxBW, &rxSG);
+
+    /* Hal Rx info */
+    /* First MPDU or Single MPDU */
+    if(((mpduInd & 0x30) == 0x00) || ((mpduInd & 0x30) == 0x20))
+    {
+        if (wd->enableHALDbgInfo && zfIsDataFrame(dev, buf))
+        {
+            ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRxDataMT   = rxMT;
+            ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRxDataMCS  = rxMCS;
+            ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRxDataBW   = rxBW;
+            ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRxDataSG   = rxSG;
+        }
+    }
+
+    if ((plcpHdrLen + payloadLen) > len) {
+        zm_msg1_rx(ZM_LV_0, "Invalid payload length=", payloadLen);
+        zfwBufFree(dev, buf, 0);
+        return;
+    }
+
+    //Store Rx Tail Infomation before Remove--CWYang(+)
+
+#if 0
+    for (i = 0; i < crcPlusRxStatusLen-4; i++)
+    {
+       addInfo.Tail.Byte[i] =
+               zmw_rx_buf_readb(dev, buf, len - crcPlusRxStatusLen + 4 + i);
+    }
+#else
+/*
+* Brief format of OUTS chip
+* ¢z¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢s¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢{
+* ¢x PLCP Header ¢x  MPDU  ¢x RSSI ¢x  EVM ¢x PHY Err ¢x  MAC Status ¢x
+* ¢u¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢q¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢t
+* ¢x     12      ¢x    n   ¢x  7   ¢x  12  ¢x    1    ¢x      4      ¢x
+* ¢|¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢r¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢w¢}
+* RSSI:
+*       Byte 1  antenna 0
+*       Byte 2  antenna 1
+*       Byte 3  antenna 2
+*       Byte 4  antenna 0 extension
+*       Byte 5  antenna 1 extension
+*       Byte 6  antenna 2 extension
+*       Byte 7  antenna combined
+* EVM:
+*       Byte 1  Stream 0 pilot 0
+*       Byte 2  Stream 0 pilot 1
+*       Byte 3  Stream 0 pilot 2
+*       Byte 4  Stream 0 pilot 3
+*       Byte 5  Stream 0 pilot 4
+*       Byte 6  Stream 0 pilot 5
+*       Byte 7  Stream 1 pilot 0
+*       Byte 8  Stream 1 pilot 1
+*       Byte 9  Stream 1 pilot 2
+*       Byte 10 Stream 1 pilot 3
+*       Byte 11 Stream 1 pilot 4
+*       Byte 12 Stream 1 pilot 5
+*/
+
+    /* Fill the Tail information */
+    /* Last MPDU or Single MPDU */
+    if ((mpduInd & 0x30) == 0x00 || (mpduInd & 0x30) == 0x10)
+    {
+#define ZM_RX_RSSI_COMPENSATION     27
+        u8_t zm_rx_rssi_compensation = ZM_RX_RSSI_COMPENSATION;
+
+    	/* RSSI information */
+        addInfo.Tail.Data.SignalStrength1 = zmw_rx_buf_readb(dev, buf,
+                (len-1) - 17) + ((hpPriv->rxStrongRSSI == 1)?zm_rx_rssi_compensation:0);
+#undef ZM_RX_RSSI_COMPENSATION
+
+      /* EVM */
+
+      /* TODO: for RD/BB debug message */
+      /* save current rx hw infomration, report to DrvCore/Application */
+      if (wd->enableHALDbgInfo && zfIsDataFrame(dev, buf))
+      {
+            u8_t trssi;
+            for (i=0; i<7; i++)
+            {
+                trssi = zmw_rx_buf_readb(dev, buf, (len-1) - 23 + i);
+	            if (trssi&0x80)
+	            {
+                    trssi = ((~((u8_t)trssi) & 0x7f) + 1) & 0x7f;
+                }
+                ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRSSI[i] = trssi;
+
+            }
+          if (rxMT==2)
+          {
+            //if (rxBW)
+            //{
+            	  for (i=0; i<12; i++)
+                    ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRxEVM[i] =
+                                       zmw_rx_buf_readb(dev, buf, (len-1) - 16 + i);
+            //}
+            //else
+            //{
+            //	  for (i=0; i<4; i++)
+            //        ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRxEVM[i] =
+            //                           zmw_rx_buf_readb(dev, buf, (len-1) - 16 + i);
+            //}
+          }
+
+          #if 0
+          /* print */
+            zm_dbg(("MT(%d) MCS(%d) BW(%d) SG(%d) RSSI:%d,%d,%d,%d,%d,%d,%d EVM:(%d,%d,%d,%d,%d,%d)(%d,%d,%d,%d,%d,%d)\n",
+                       rxMT,
+                       rxMCS,
+                       rxBW,
+                       rxSG,
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRSSI[0],
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRSSI[1],
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRSSI[2],
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRSSI[3],
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRSSI[4],
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRSSI[5],
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRSSI[6],
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRxEVM[0],
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRxEVM[1],
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRxEVM[2],
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRxEVM[3],
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRxEVM[4],
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRxEVM[5],
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRxEVM[6],
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRxEVM[7],
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRxEVM[8],
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRxEVM[9],
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRxEVM[10],
+                       ((struct zsHpPriv*)wd->hpPrivate)->halRxInfo.currentRxEVM[11]
+                       ));
+          #endif
+      } /* if (wd->enableHALDbgInfo && zfIsDataFrame(dev, buf)) */
+
+    }
+    else
+    {
+        /* Mid or First aggregate frame without phy rx information */
+        addInfo.Tail.Data.SignalStrength1 = 0;
+    }
+
+    addInfo.Tail.Data.SignalStrength2 = 0;
+    addInfo.Tail.Data.SignalStrength3 = 0;
+    addInfo.Tail.Data.SignalQuality   = 0;
+
+    addInfo.Tail.Data.SAIndex           = zmw_rx_buf_readb(dev, buf, len - 4);
+    addInfo.Tail.Data.DAIndex           = zmw_rx_buf_readb(dev, buf, len - 3);
+    addInfo.Tail.Data.ErrorIndication   = zmw_rx_buf_readb(dev, buf, len - 2);
+    addInfo.Tail.Data.RxMacStatus       = zmw_rx_buf_readb(dev, buf, len - 1);
+
+#endif
+    /* Remove CRC and Rx Status */
+    zfwBufSetSize(dev, buf, (len-crcPlusRxStatusLen));
+    //zfwBufSetSize(dev, buf, payloadLen + plcpHdrLen);    /* payloadLen + PLCP 12 - FCS 4*/
+
+    //Store PLCP Header Infomation before Remove--CWYang(+)
+    if (plcpHdrLen != 0)
+    {
+        for (i = 0; i < plcpHdrLen; i++)
+        {
+            addInfo.PlcpHeader[i] = zmw_rx_buf_readb(dev, buf, i);
+        }
+    }
+    else
+    {
+        addInfo.PlcpHeader[0] = 0;
+    }
+    /* Remove PLCP header */
+    zfwBufRemoveHead(dev, buf, plcpHdrLen);
+
+    /* handle 802.11 frame */
+    zfCoreRecv(dev, buf, &addInfo);
+
+#else
+    /* Firmware loopback: Rx frame = Tx frame       */
+    /* convert Rx frame to fit receive frame format */
+    zbuf_t *new_buf;
+    u8_t    ctrl_offset = 8;
+    u8_t    PLCP_Len = 12;
+    u8_t    data;
+    u8_t    i;
+
+
+    /* Tx:  | ctrl_setting | Mac hdr | data | */
+    /*            8            24       x     */
+
+    /* Rx:          | PLCP | Mac hdr | data | FCS | Rxstatus | */
+    /*                 12      24        x     4       8       */
+
+    /* new allocate a rx format size buf */
+    new_buf = zfwBufAllocate(dev, zfwBufGetSize(dev, buf)-8+12+4+EXTRA_INFO_LEN);
+
+    for (i=0; i<zfwBufGetSize(dev, buf)-ctrl_offset; i++)
+    {
+        data = zmw_rx_buf_readb(dev, buf, ctrl_offset+i);
+        zmw_rx_buf_writeb(dev, new_buf, PLCP_Len+i, data);
+    }
+
+    zfwBufSetSize(dev, new_buf, zfwBufGetSize(dev, buf)-8+12+4+EXTRA_INFO_LEN);
+
+    zfwBufFree(dev, buf, 0);
+
+    /* receive the new_buf */
+    //zfCoreRecv(dev, new_buf);
+
+#endif
+
+}
+
+#ifdef ZM_OTUS_RX_STREAM_MODE
+void zfiUsbRecv(zdev_t *dev, zbuf_t *buf)
+{
+    u16_t index = 0;
+    u16_t chkIdx;
+    u32_t status = 0;
+    u16_t ii;
+    zbuf_t *newBuf;
+    zbuf_t *rxBufPool[8];
+    u16_t rxBufPoolIndex = 0;
+    struct zsHpPriv *halPriv;
+    u8_t *srcBufPtr;
+    u32_t bufferLength;
+    u16_t usbRxRemainLen;
+    u16_t usbRxPktLen;
+
+    zmw_get_wlan_dev(dev);
+
+    halPriv = (struct zsHpPriv*)wd->hpPrivate;
+    srcBufPtr = zmw_buf_get_buffer(dev, buf);
+
+    bufferLength = zfwBufGetSize(dev, buf);
+
+    /* Zero Length Transfer */
+    if (!bufferLength)
+    {
+        zfwBufFree(dev, buf, 0);
+        return;
+    }
+
+    usbRxRemainLen = halPriv->usbRxRemainLen;
+    usbRxPktLen = halPriv->usbRxTransferLen;
+
+    /* Check whether there is any data in the last transfer */
+    if (usbRxRemainLen != 0 )
+    {
+        zbuf_t *remainBufPtr = halPriv->remainBuf;
+        u8_t* BufPtr = NULL;
+
+        if ( remainBufPtr != NULL )
+        {
+            BufPtr = zmw_buf_get_buffer(dev, remainBufPtr);
+        }
+
+        index = usbRxRemainLen;
+        usbRxRemainLen -= halPriv->usbRxPadLen;
+
+        /*  Copy data */
+        if ( BufPtr != NULL )
+        {
+            zfwMemoryCopy(&(BufPtr[usbRxPktLen]), srcBufPtr, usbRxRemainLen);
+        }
+
+        usbRxPktLen += usbRxRemainLen;
+        halPriv->usbRxRemainLen = 0;
+
+        if ( remainBufPtr != NULL )
+        {
+            zfwBufSetSize(dev, remainBufPtr, usbRxPktLen);
+            rxBufPool[rxBufPoolIndex++] = remainBufPtr;
+        }
+        halPriv->remainBuf = NULL;
+    }
+
+    //zm_debug_msg1("length: %d\n", (int)pUsbRxTransfer->pRxUrb->UrbBulkOrInterruptTransfer.TransferBufferLength);
+
+    bufferLength = zfwBufGetSize(dev, buf);
+//printk("bufferLength %d\n", bufferLength);
+    while(index < bufferLength)
+    {
+        u16_t pktLen;
+        u16_t pktTag;
+        //u8_t *ptr = (u8_t*)((struct zsBuffer*)pUsbRxTransfer->buf)->data;
+        u8_t *ptr = srcBufPtr;
+
+        /* Retrieve packet length and tag */
+        pktLen = ptr[index] + (ptr[index+1] << 8);
+        pktTag = ptr[index+2] + (ptr[index+3] << 8);
+
+        if (pktTag == ZM_USB_STREAM_MODE_TAG)
+        {
+            u16_t padLen;
+
+            zm_assert(pktLen < ZM_WLAN_MAX_RX_SIZE);
+
+            //printk("Get a packet, pktLen: 0x%04x\n", pktLen);
+            #if 0
+            /* Dump data */
+            for (ii = index; ii < pkt_len+4;)
+            {
+                DbgPrint("0x%02x ",
+                        (zmw_rx_buf_readb(adapter, pUsbRxTransfer->buf, ii) & 0xff));
+
+                if ((++ii % 16) == 0)
+                    DbgPrint("\n");
+            }
+
+            DbgPrint("\n");
+            #endif
+
+            /* Calcuate the padding length, in the current design,
+               the length should be padded to 4 byte boundray. */
+            padLen = ZM_USB_STREAM_MODE_TAG_LEN - (pktLen & 0x3);
+
+            if(padLen == ZM_USB_STREAM_MODE_TAG_LEN)
+                padLen = 0;
+
+            chkIdx = index;
+            index = index + ZM_USB_STREAM_MODE_TAG_LEN + pktLen + padLen;
+
+            if (chkIdx > ZM_MAX_USB_IN_TRANSFER_SIZE)
+            {
+                zm_debug_msg1("chkIdx is too large, chkIdx: %d\n", chkIdx);
+                zm_assert(0);
+                status = 1;
+                break;
+            }
+
+            if (index > ZM_MAX_USB_IN_TRANSFER_SIZE)
+            {
+                //struct zsBuffer* BufPtr;
+                //struct zsBuffer* UsbBufPtr;
+                u8_t *BufPtr;
+                u8_t *UsbBufPtr;
+
+                halPriv->usbRxRemainLen = index - ZM_MAX_USB_IN_TRANSFER_SIZE; // - padLen;
+                halPriv->usbRxTransferLen = ZM_MAX_USB_IN_TRANSFER_SIZE -
+                        chkIdx - ZM_USB_STREAM_MODE_TAG_LEN;
+                halPriv->usbRxPadLen = padLen;
+                //check_index = index;
+
+                if (halPriv->usbRxTransferLen > ZM_WLAN_MAX_RX_SIZE)
+                {
+                    zm_debug_msg1("check_len is too large, chk_len: %d\n",
+                            halPriv->usbRxTransferLen);
+                    status = 1;
+                    break;
+                }
+
+                /* Allocate a skb buffer */
+                newBuf = zfwBufAllocate(dev, ZM_WLAN_MAX_RX_SIZE);
+
+                if ( newBuf != NULL )
+                {
+                    BufPtr = zmw_buf_get_buffer(dev, newBuf);
+                    UsbBufPtr = srcBufPtr;
+
+                    /* Copy the buffer */
+                    zfwMemoryCopy(BufPtr, &(UsbBufPtr[chkIdx+ZM_USB_STREAM_MODE_TAG_LEN]), halPriv->usbRxTransferLen);
+
+                    /* Record the buffer pointer */
+                    halPriv->remainBuf = newBuf;
+                }
+            }
+            else
+            {
+                u8_t* BufPtr;
+                u8_t* UsbBufPtr;
+
+                /* Allocate a skb buffer */
+                newBuf = zfwBufAllocate(dev, ZM_WLAN_MAX_RX_SIZE);
+                if ( newBuf != NULL )
+                {
+                    BufPtr = zmw_buf_get_buffer(dev, newBuf);
+                    UsbBufPtr = srcBufPtr;
+
+                    /* Copy the buffer */
+                    zfwMemoryCopy(BufPtr, &(UsbBufPtr[chkIdx+ZM_USB_STREAM_MODE_TAG_LEN]), pktLen);
+
+                    zfwBufSetSize(dev, newBuf, pktLen);
+                    rxBufPool[rxBufPoolIndex++] = newBuf;
+                }
+            }
+        }
+        else
+        {
+                u16_t i;
+
+                DbgPrint("Can't find tag, pkt_len: 0x%04x, tag: 0x%04x\n",
+                        pktLen, pktTag);
+
+                #if 0
+                for(i = 0; i < 32; i++)
+                {
+                    DbgPrint("%02x ", buf->data[index-16+i]);
+
+                    if ((i & 0xf) == 0xf)
+                        DbgPrint("\n");
+                }
+                #endif
+
+                break;
+        }
+    }
+
+    /* Free buffer */
+    //zfwBufFree(adapter, pUsbRxTransfer->buf, 0);
+    zfwBufFree(dev, buf, 0);
+
+    for(ii = 0; ii < rxBufPoolIndex; ii++)
+    {
+        zfiUsbRecvPerPkt(dev, rxBufPool[ii]);
+    }
+}
+#endif
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfUsbInit                   */
+/*      Initialize USB resource.                                        */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.12     */
+/*                                                                      */
+/************************************************************************/
+void zfUsbInit(zdev_t* dev)
+{
+    /* Initialize Rx & INT endpoint for receiving data & interrupt */
+    zfwUsbEnableRxEpt(dev, USB_ENDPOINT_RX_INDEX);
+    zfwUsbEnableIntEpt(dev, USB_ENDPOINT_INT_INDEX);
+
+    return;
+}
+
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfUsbFree                   */
+/*      Free PCI resource.                                              */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen Chen        ZyDAS Technology Corporation    2005.12     */
+/*                                                                      */
+/************************************************************************/
+void zfUsbFree(zdev_t* dev)
+{
+    struct zsHpPriv *halPriv;
+
+    zmw_get_wlan_dev(dev);
+
+    halPriv = (struct zsHpPriv*)wd->hpPrivate;
+
+#ifdef ZM_OTUS_RX_STREAM_MODE
+    if ( halPriv->remainBuf != NULL )
+    {
+        zfwBufFree(dev, halPriv->remainBuf, 0);
+    }
+#endif
+
+    return;
+}
+
+void zfHpSendBeacon(zdev_t* dev, zbuf_t* buf, u16_t len)
+{
+    u32_t hw, lw;
+    u16_t i;
+    zmw_get_wlan_dev(dev);
+
+    /* Write to beacon buffer (ZM_BEACON_BUFFER_ADDRESS) */
+    for (i = 0; i<len; i+=4)
+    {
+        lw = zmw_tx_buf_readh(dev, buf, i);
+        hw = zmw_tx_buf_readh(dev, buf, i+2);
+
+        zfDelayWriteInternalReg(dev, ZM_BEACON_BUFFER_ADDRESS+i, (hw<<16)+lw);
+    }
+
+    /* Beacon PCLP header */
+    if (((struct zsHpPriv*)wd->hpPrivate)->hwFrequency < 3000)
+    {
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_BCN_PLCP, ((len+4)<<(3+16))+0x0400);
+    }
+    else
+    {
+        zfDelayWriteInternalReg(dev, ZM_MAC_REG_BCN_PLCP, ((len+4)<<(16))+0x001b);
+    }
+
+    /* Beacon length (include CRC32) */
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_BCN_LENGTH, len+4);
+
+    /* Beacon Ready */
+    zfDelayWriteInternalReg(dev, ZM_MAC_REG_BCN_CTRL, 1);
+    zfFlushDelayWrite(dev);
+
+    /* Free beacon buf */
+    zfwBufFree(dev, buf, 0);
+
+    return;
+}
+
+
+#define ZM_STATUS_TX_COMP       0x00
+#define ZM_STATUS_RETRY_COMP    0x01
+#define ZM_STATUS_TX_FAILED     0x02
+void zfiUsbRegIn(zdev_t* dev, u32_t* rsp, u16_t rspLen)
+{
+    //u8_t len, type, i;
+    u8_t type;
+    u8_t *u8rsp;
+    u16_t status;
+    u32_t bitmap;
+    zmw_get_wlan_dev(dev);
+
+    zm_msg0_mm(ZM_LV_3, "zfiUsbRegIn()");
+
+    u8rsp = (u8_t *)rsp;
+
+    //len = *u8rsp;
+    type = *(u8rsp+1);
+    u8rsp = u8rsp+4;
+
+
+    /* Interrupt event */
+    if ((type & 0xC0) == 0xC0)
+    {
+        if (type == 0xC0)
+        {
+            zfCoreEvent(dev, 0, u8rsp);
+
+        }
+        else if (type == 0xC1)
+        {
+#if 0
+            {
+                u16_t i;
+                DbgPrint("rspLen=%d\n", rspLen);
+                for (i=0; i<(rspLen/4); i++)
+                {
+                    DbgPrint("rsp[%d]=0x%lx\n", i, rsp[i]);
+                }
+            }
+#endif
+            status = (u16_t)(rsp[3] >> 16);
+
+            ////6789
+            rsp[8] = rsp[8] >> 2 | (rsp[9] & 0x1) << 6;
+            switch (status)
+            {
+            case ZM_STATUS_RETRY_COMP :
+                zfCoreEvent(dev, 1, u8rsp);
+                break;
+            case ZM_STATUS_TX_FAILED :
+                zfCoreEvent(dev, 2, u8rsp);
+                break;
+            case ZM_STATUS_TX_COMP :
+                zfCoreEvent(dev, 3, u8rsp);
+                break;
+            }
+        }
+        else if (type == 0xC2)
+        {
+            zfBeaconCfgInterrupt(dev, u8rsp);
+        }
+        else if (type == 0xC3)
+        {
+            zfEndOfAtimWindowInterrupt(dev);
+        }
+        else if (type == 0xC4)
+        {
+#if 0
+            {
+                u16_t i;
+                DbgPrint("0xC2:rspLen=%d\n", rspLen);
+                for (i=0; i<(rspLen/4); i++)
+                {
+                    DbgPrint("0xC2:rsp[%d]=0x%lx\n", i, rsp[i]);
+                }
+            }
+#endif
+            bitmap = (rsp[1] >> 16) + ((rsp[2] & 0xFFFF) << 16 );
+            //zfBawCore(dev, (u16_t)rsp[1] & 0xFFFF, bitmap, (u16_t)(rsp[2] >> 16) & 0xFF);
+        }
+        else if (type == 0xC5)
+        {
+            u16_t i;
+#if 0
+
+            for (i=0; i<(rspLen/4); i++) {
+                DbgPrint("0xC5:rsp[%d]=0x%lx\n", i, rsp[i]);
+            }
+#endif
+            for (i=1; i<(rspLen/4); i++) {
+                u8rsp = (u8_t *)(rsp+i);
+                //DbgPrint("0xC5:rsp[%d]=0x%lx\n", i, ((u32_t*)u8rsp)[0]);
+                zfCoreEvent(dev, 4, u8rsp);
+            }
+        }
+        else if (type == 0xC6)
+        {
+            zm_debug_msg0("\n\n WatchDog interrupt!!! : 0xC6 \n\n");
+            if (wd->zfcbHwWatchDogNotify != NULL)
+            {
+                wd->zfcbHwWatchDogNotify(dev);
+            }
+        }
+        else if (type == 0xC8)
+        {
+            //PZSW_ADAPTER adapter;
+
+            // for SPI flash program chk Flag
+            zfwDbgProgrameFlashChkDone(dev);
+        }
+        else if (type == 0xC9)
+        {
+            struct zsHpPriv* hpPriv=wd->hpPrivate;
+
+            zm_debug_msg0("##### Tx retransmission 5 times event #####");
+
+            /* correct tx retransmission issue */
+            hpPriv->retransmissionEvent = 1;
+        }
+    }
+    else
+    {
+        zfIdlRsp(dev, rsp, rspLen);
+    }
+}
+
+
+#define ZM_PROGRAM_RAM_ADDR     0x200000 //0x1000 //0x700000
+#define FIRMWARE_DOWNLOAD       0x30
+#define FIRMWARE_DOWNLOAD_COMP  0x31
+#define FIRMWARE_CONFIRM        0x32
+
+u16_t zfFirmwareDownload(zdev_t* dev, u32_t* fw, u32_t len, u32_t offset)
+{
+    u16_t ret = ZM_SUCCESS;
+    u32_t uCodeOfst = offset;
+    u8_t *image, *ptr;
+    u32_t result;
+
+    image = (u8_t*) fw;
+    ptr = image;
+
+    while (len > 0)
+    {
+        u32_t translen = (len > 4096) ? 4096 : len;
+
+        result = zfwUsbSubmitControl(dev, FIRMWARE_DOWNLOAD,
+                                     (u16_t) (uCodeOfst >> 8),
+                                     0, image, translen);
+
+        if (result != ZM_SUCCESS)
+        {
+            zm_msg0_init(ZM_LV_0, "FIRMWARE_DOWNLOAD failed");
+            ret = 1;
+            goto exit;
+        }
+
+        len -= translen;
+        image += translen;
+        uCodeOfst += translen; // in Word (16 bit)
+
+        result = 0;
+    }
+
+    /* If download firmware success, issue a command to firmware */
+    if (ret == 0)
+    {
+        result = zfwUsbSubmitControl(dev, FIRMWARE_DOWNLOAD_COMP,
+                                     0, 0, NULL, 0);
+
+        if (result != ZM_SUCCESS)
+        {
+            zm_msg0_init(ZM_LV_0, "FIRMWARE_DOWNLOAD_COMP failed");
+            ret = 1;
+            goto exit;
+        }
+    }
+
+#if 0
+    /* PCI code */
+    /* Wait for firmware ready */
+    result = zfwUsbSubmitControl(dev, FIRMWARE_CONFIRM, USB_DIR_IN | 0x40,
+                     0, 0, &ret_value, sizeof(ret_value), HZ);
+
+    if (result != 0)
+    {
+        zm_msg0_init(ZM_LV_0, "Can't receive firmware ready: ", result);
+        ret = 1;
+    }
+#endif
+
+exit:
+
+    return ret;
+
+}
+
+u16_t zfFirmwareDownloadNotJump(zdev_t* dev, u32_t* fw, u32_t len, u32_t offset)
+{
+    u16_t ret = ZM_SUCCESS;
+    u32_t uCodeOfst = offset;
+    u8_t *image, *ptr;
+    u32_t result;
+
+    image = (u8_t*) fw;
+    ptr = image;
+
+    while (len > 0)
+    {
+        u32_t translen = (len > 4096) ? 4096 : len;
+
+        result = zfwUsbSubmitControl(dev, FIRMWARE_DOWNLOAD,
+                                     (u16_t) (uCodeOfst >> 8),
+                                     0, image, translen);
+
+        if (result != ZM_SUCCESS)
+        {
+            zm_msg0_init(ZM_LV_0, "FIRMWARE_DOWNLOAD failed");
+            ret = 1;
+            goto exit;
+        }
+
+        len -= translen;
+        image += translen;
+        uCodeOfst += translen; // in Word (16 bit)
+
+        result = 0;
+    }
+
+exit:
+
+    return ret;
+
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                  zfIdlGetFreeTxdCount        */
+/*      Get free PCI PCI TxD count.                                     */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      None                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Stephen             ZyDAS Technology Corporation    2006.6      */
+/*                                                                      */
+/************************************************************************/
+u32_t zfHpGetFreeTxdCount(zdev_t* dev)
+{
+    return zfwUsbGetFreeTxQSize(dev);
+}
+
+u32_t zfHpGetMaxTxdCount(zdev_t* dev)
+{
+    //return 8;
+    return zfwUsbGetMaxTxQSize(dev);
+}
+
+void zfiUsbRegOutComplete(zdev_t* dev)
+{
+    return;
+}
+
+extern void zfPushVtxq(zdev_t* dev);
+
+void zfiUsbOutComplete(zdev_t* dev, zbuf_t *buf, u8_t status, u8_t *hdr) {
+#ifndef ZM_ENABLE_AGGREGATION
+    if (buf) {
+        zfwBufFree(dev, buf, 0);
+    }
+#else
+    #ifdef ZM_BYPASS_AGGR_SCHEDULING
+    //Simply free the buf since BA retransmission is done in the firmware
+    if (buf)
+    {
+        zfwBufFree(dev, buf, 0);
+    }
+    zfPushVtxq(dev);
+    #else
+    zmw_get_wlan_dev(dev);
+
+    #ifdef ZM_ENABLE_FW_BA_RETRANSMISSION
+    //Simply free the buf since BA retransmission is done in the firmware
+    if (buf)
+    {
+        zfwBufFree(dev, buf, 0);
+    }
+    #else
+    u8_t agg;
+    u16_t frameType;
+
+    if(!hdr && buf) {
+        zfwBufFree(dev, buf, 0);
+        //zm_debug_msg0("buf Free due to hdr == NULL");
+        return;
+    }
+
+    if(hdr && buf) {
+        frameType = hdr[8] & 0xf;
+        agg = (u8_t)(hdr[2] >> 5 ) & 0x1;
+        //zm_debug_msg1("AGG=", agg);
+
+        if (!status) {
+            if (agg) {
+                //delete buf in ba fail queue??
+                //not ganna happen?
+            }
+            else {
+                zfwBufFree(dev, buf, 0);
+            }
+        }
+        else {
+            if (agg) {
+                //don't do anything
+                //zfwBufFree(dev, buf, 0);
+            }
+            else {
+                zfwBufFree(dev, buf, 0);
+            }
+        }
+    }
+    #endif
+
+    if (wd->state != ZM_WLAN_STATE_ENABLED) {
+        return;
+    }
+
+    if( (wd->wlanMode == ZM_MODE_AP) ||
+        (wd->wlanMode == ZM_MODE_INFRASTRUCTURE && wd->sta.EnableHT) ||
+        (wd->wlanMode == ZM_MODE_PSEUDO) ) {
+        zfAggTxScheduler(dev, 0);
+    }
+    #endif
+#endif
+
+    return;
+
+}
+
diff --git a/drivers/staging/otus/hal/hpusb.h b/drivers/staging/otus/hal/hpusb.h
new file mode 100644
index 00000000000..35a0c5668ce
--- /dev/null
+++ b/drivers/staging/otus/hal/hpusb.h
@@ -0,0 +1,437 @@
+/*
+ * Copyright (c) 2000-2005 ZyDAS Technology Corporation
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*  Module Name : ud_defs.h                                             */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains USB data structure definitions.            */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      None                                                            */
+/*                                                                      */
+/************************************************************************/
+
+#ifndef _HPUSB_H
+#define _HPUSB_H
+
+#define ZM_OTUS_ENABLE_RETRY_FREQ_CHANGE
+#define ZM_BEACON_BUFFER_ADDRESS            0x117900
+
+#define ZM_MAX_CMD_SIZE                     64
+#define ZM_HAL_MAX_EEPROM_REQ               510
+#define ZM_HAL_MAX_EEPROM_PRQ               2
+
+/* For USB STREAM mode */
+#ifdef ZM_DISABLE_AMSDU8K_SUPPORT
+#define ZM_MAX_USB_IN_TRANSFER_SIZE         4096
+#else
+#define ZM_MAX_USB_IN_TRANSFER_SIZE         8192
+#endif
+#define ZM_USB_STREAM_MODE_TAG_LEN          4
+#define ZM_USB_STREAM_MODE_TAG              0x4e00
+#define ZM_USB_MAX_EPINT_BUFFER             64
+
+struct zsCmdQ
+{
+    u16_t src;
+    u16_t cmdLen;
+    u8_t* buf;
+    u32_t cmd[ZM_MAX_CMD_SIZE/4];
+};
+
+struct zsCommand
+{
+    u16_t delayWcmdCount;
+    u32_t delayWcmdAddr[(ZM_CMD_QUEUE_SIZE-4)/4];
+    u32_t delayWcmdVal[(ZM_CMD_QUEUE_SIZE-4)/4];
+};
+
+struct zsHalRxInfo
+{
+    u32_t currentRSSI[7];       /* RSSI combined */
+    u32_t currentRxEVM[14];
+    u32_t currentRxDataMT;
+    u32_t currentRxDataMCS;
+    u32_t currentRxDataBW;
+    u32_t currentRxDataSG;
+};
+
+struct zsHpPriv
+{
+    u16_t hwFrequency;
+    u8_t  hwBw40;
+    u8_t  hwExtOffset;
+
+    u8_t  disableDfsCh;
+
+    u32_t halCapability;
+
+    /* Fortunately the second loop can be disabled with a bit */
+    /* called en_pd_dc_offset_thr                             */
+    u8_t hwNotFirstInit;
+
+    /* command queue */
+    u16_t               cmdHead;
+    u16_t               cmdTail;
+#ifdef ZM_XP_USB_MULTCMD
+    u16_t               cmdSend;  // Used for Mult send USB cmd
+#endif
+    struct zsCmdQ       cmdQ[ZM_CMD_QUEUE_SIZE];
+    u16_t               cmdPending;
+    struct zsCommand    cmd; /* buffer for delayed commands */
+    u8_t                ledMode[2];
+    u32_t               ctlBusy;
+    u32_t               extBusy;
+
+    /*
+     * ANI & Radar support.
+     */
+    u32_t   procPhyErr;         /* Process Phy errs */
+    u8_t hasHwPhyCounters;   /* Hardware has phy counters */
+    u32_t   aniPeriod;          /* ani update list period */
+    struct zsAniStats   stats;      /* various statistics */
+    struct zsAniState   *curani;    /* cached last reference */
+    struct zsAniState   ani[50];   /* per-channel state */
+
+    /*
+     * Ani tables that change between the 5416 and 5312.
+     * These get set at attach time.
+     * XXX don't belong here
+     * XXX need better explanation
+     */
+    s32_t     totalSizeDesired[5];
+    s32_t     coarseHigh[5];
+    s32_t     coarseLow[5];
+    s32_t     firpwr[5];
+
+    /*
+     * ANI related PHY register value.
+     */
+    u32_t regPHYDesiredSZ;
+    u32_t regPHYFindSig;
+    u32_t regPHYAgcCtl1;
+    u32_t regPHYSfcorr;
+    u32_t regPHYSfcorrLow;
+    u32_t regPHYTiming5;
+    u32_t regPHYCckDetect;
+
+    u32_t eepromImage[1024];
+    u32_t eepromImageIndex;
+    u32_t eepromImageRdReq;
+
+    u8_t  halReInit;
+
+    u8_t  OpFlags;
+
+    u8_t tPow2xCck[4];
+    u8_t tPow2x2g[4];
+    u8_t tPow2x2g24HeavyClipOffset;
+    u8_t tPow2x2gHt20[8];
+    u8_t tPow2x2gHt40[8];
+    u8_t tPow2x5g[4];
+    u8_t tPow2x5gHt20[8];
+    u8_t tPow2x5gHt40[8];
+
+    /* hwBBHeavyClip : used compatibility           */
+    /*             0 : dongle not support.          */
+    /*             !0: support heavy clip.          */
+    u8_t hwBBHeavyClip;
+    u8_t enableBBHeavyClip; /* 0=>force disable 1=>enable */
+    u8_t doBBHeavyClip;     /* set 1 if heavy clip need by each frequency switch */
+    u32_t setValueHeavyClip; /* save setting value for heavy clip when completed routine */
+
+    /*
+     * Rxdata RSSI, EVM, Rate etc...
+     */
+    struct zsHalRxInfo halRxInfo;
+
+    u32_t usbSendBytes;
+    u32_t usbAcSendBytes[4];
+
+    u16_t aggMaxDurationBE;
+    u32_t aggPktNum;
+
+    u16_t txop[4];
+    u16_t cwmin[4];
+    u16_t cwmax[4];
+    u8_t  strongRSSI;
+    u8_t  rxStrongRSSI;
+
+    u8_t  slotType;  //0->20us, 1=>9us
+
+#ifdef ZM_OTUS_RX_STREAM_MODE
+    u16_t usbRxRemainLen;
+    u16_t usbRxPktLen;
+    u16_t usbRxPadLen;
+    u16_t usbRxTransferLen;
+    zbuf_t  *remainBuf;
+#endif
+
+    u8_t    dot11Mode;
+
+    u8_t    ibssBcnEnabled;
+    u32_t   ibssBcnInterval;
+
+    // For re-issue the frequency change command
+    u32_t   latestFrequency;
+    u8_t    latestBw40;
+    u8_t    latestExtOffset;
+    u8_t    freqRetryCounter;
+
+    u8_t    recordFreqRetryCounter;
+    u8_t    isSiteSurvey;
+    u8_t    coldResetNeedFreq;
+
+    u64_t   camRollCallTable;
+    u8_t    currentAckRtsTpc;
+
+    /* #1 Save the initial value of the related RIFS register settings */
+    //u32_t   isInitialPhy;
+    u32_t   initDesiredSigSize;
+    u32_t   initAGC;
+    u32_t   initAgcControl;
+    u32_t   initSearchStartDelay;
+    u32_t   initRIFSSearchParams;
+    u32_t   initFastChannelChangeControl;
+
+    /* Dynamic SIFS for retransmission event */
+    u8_t    retransmissionEvent;
+    u8_t    latestSIFS;
+};
+
+extern u32_t zfHpLoadEEPROMFromFW(zdev_t* dev);
+
+
+typedef u8_t A_UINT8;
+typedef s8_t A_INT8;
+typedef u16_t A_UINT16;
+typedef u32_t A_UINT32;
+#define __ATTRIB_PACK
+
+#pragma pack (push, 1)
+
+#define AR5416_EEP_VER               0xE
+#define AR5416_EEP_VER_MINOR_MASK    0xFFF
+#define AR5416_EEP_NO_BACK_VER       0x1
+#define AR5416_EEP_MINOR_VER_2       0x2  // Adds modal params txFrameToPaOn, txFrametoDataStart, ht40PowerInc
+#define AR5416_EEP_MINOR_VER_3       0x3  // Adds modal params bswAtten, bswMargin, swSettle and base OpFlags for HT20/40 Disable
+
+// 16-bit offset location start of calibration struct
+#define AR5416_EEP_START_LOC         256
+#define AR5416_NUM_5G_CAL_PIERS      8
+#define AR5416_NUM_2G_CAL_PIERS      4
+#define AR5416_NUM_5G_20_TARGET_POWERS  8
+#define AR5416_NUM_5G_40_TARGET_POWERS  8
+#define AR5416_NUM_2G_CCK_TARGET_POWERS 3
+#define AR5416_NUM_2G_20_TARGET_POWERS  4
+#define AR5416_NUM_2G_40_TARGET_POWERS  4
+#define AR5416_NUM_CTLS              24
+#define AR5416_NUM_BAND_EDGES        8
+#define AR5416_NUM_PD_GAINS          4
+#define AR5416_PD_GAINS_IN_MASK      4
+#define AR5416_PD_GAIN_ICEPTS        5
+#define AR5416_EEPROM_MODAL_SPURS    5
+#define AR5416_MAX_RATE_POWER        63
+#define AR5416_NUM_PDADC_VALUES      128
+#define AR5416_NUM_RATES             16
+#define AR5416_BCHAN_UNUSED          0xFF
+#define AR5416_MAX_PWR_RANGE_IN_HALF_DB 64
+#define AR5416_OPFLAGS_11A           0x01
+#define AR5416_OPFLAGS_11G           0x02
+#define AR5416_OPFLAGS_5G_HT40       0x04
+#define AR5416_OPFLAGS_2G_HT40       0x08
+#define AR5416_OPFLAGS_5G_HT20       0x10
+#define AR5416_OPFLAGS_2G_HT20       0x20
+#define AR5416_EEPMISC_BIG_ENDIAN    0x01
+#define FREQ2FBIN(x,y) ((y) ? ((x) - 2300) : (((x) - 4800) / 5))
+#define AR5416_MAX_CHAINS            2
+#define AR5416_ANT_16S               25
+
+#define AR5416_NUM_ANT_CHAIN_FIELDS     7
+#define AR5416_NUM_ANT_COMMON_FIELDS    4
+#define AR5416_SIZE_ANT_CHAIN_FIELD     3
+#define AR5416_SIZE_ANT_COMMON_FIELD    4
+#define AR5416_ANT_CHAIN_MASK           0x7
+#define AR5416_ANT_COMMON_MASK          0xf
+#define AR5416_CHAIN_0_IDX              0
+#define AR5416_CHAIN_1_IDX              1
+#define AR5416_CHAIN_2_IDX              2
+
+
+/* Capabilities Enum */
+typedef enum {
+    EEPCAP_COMPRESS_DIS  = 0x0001,
+    EEPCAP_AES_DIS       = 0x0002,
+    EEPCAP_FASTFRAME_DIS = 0x0004,
+    EEPCAP_BURST_DIS     = 0x0008,
+    EEPCAP_MAXQCU_M      = 0x01F0,
+    EEPCAP_MAXQCU_S      = 4,
+    EEPCAP_HEAVY_CLIP_EN = 0x0200,
+    EEPCAP_KC_ENTRIES_M  = 0xF000,
+    EEPCAP_KC_ENTRIES_S  = 12,
+} EEPROM_CAPABILITIES;
+
+typedef enum Ar5416_Rates {
+    rate6mb,  rate9mb,  rate12mb, rate18mb,
+    rate24mb, rate36mb, rate48mb, rate54mb,
+    rate1l,   rate2l,   rate2s,   rate5_5l,
+    rate5_5s, rate11l,  rate11s,  rateXr,
+    rateHt20_0, rateHt20_1, rateHt20_2, rateHt20_3,
+    rateHt20_4, rateHt20_5, rateHt20_6, rateHt20_7,
+    rateHt40_0, rateHt40_1, rateHt40_2, rateHt40_3,
+    rateHt40_4, rateHt40_5, rateHt40_6, rateHt40_7,
+    rateDupCck, rateDupOfdm, rateExtCck, rateExtOfdm,
+    Ar5416RateSize
+} AR5416_RATES;
+
+typedef struct eepFlags {
+    A_UINT8  opFlags;
+    A_UINT8  eepMisc;
+} __ATTRIB_PACK EEP_FLAGS;
+
+#define AR5416_CHECKSUM_LOCATION (AR5416_EEP_START_LOC + 1)
+typedef struct BaseEepHeader {
+    A_UINT16  length;
+    A_UINT16  checksum;
+    A_UINT16  version;
+    EEP_FLAGS opCapFlags;
+    A_UINT16  regDmn[2];
+    A_UINT8   macAddr[6];
+    A_UINT8   rxMask;
+    A_UINT8   txMask;
+    A_UINT16  rfSilent;
+    A_UINT16  blueToothOptions;
+    A_UINT16  deviceCap;
+    A_UINT32  binBuildNumber;
+    A_UINT8   deviceType;
+    A_UINT8   futureBase[33];
+} __ATTRIB_PACK BASE_EEP_HEADER; // 64 B
+
+typedef struct spurChanStruct {
+    A_UINT16 spurChan;
+    A_UINT8  spurRangeLow;
+    A_UINT8  spurRangeHigh;
+} __ATTRIB_PACK SPUR_CHAN;
+
+typedef struct ModalEepHeader {
+    A_UINT32  antCtrlChain[AR5416_MAX_CHAINS];       // 12
+    A_UINT32  antCtrlCommon;                         // 4
+    A_INT8    antennaGainCh[AR5416_MAX_CHAINS];      // 3
+    A_UINT8   switchSettling;                        // 1
+    A_UINT8   txRxAttenCh[AR5416_MAX_CHAINS];        // 3
+    A_UINT8   rxTxMarginCh[AR5416_MAX_CHAINS];       // 3
+    A_INT8    adcDesiredSize;                        // 1
+    A_INT8    pgaDesiredSize;                        // 1
+    A_UINT8   xlnaGainCh[AR5416_MAX_CHAINS];         // 3
+    A_UINT8   txEndToXpaOff;                         // 1
+    A_UINT8   txEndToRxOn;                           // 1
+    A_UINT8   txFrameToXpaOn;                        // 1
+    A_UINT8   thresh62;                              // 1
+    A_INT8    noiseFloorThreshCh[AR5416_MAX_CHAINS]; // 3
+    A_UINT8   xpdGain;                               // 1
+    A_UINT8   xpd;                                   // 1
+    A_INT8    iqCalICh[AR5416_MAX_CHAINS];           // 1
+    A_INT8    iqCalQCh[AR5416_MAX_CHAINS];           // 1
+    A_UINT8   pdGainOverlap;                         // 1
+    A_UINT8   ob;                                    // 1
+    A_UINT8   db;                                    // 1
+    A_UINT8   xpaBiasLvl;                            // 1
+    A_UINT8   pwrDecreaseFor2Chain;                  // 1
+    A_UINT8   pwrDecreaseFor3Chain;                  // 1 -> 48 B
+    A_UINT8   txFrameToDataStart;                    // 1
+    A_UINT8   txFrameToPaOn;                         // 1
+    A_UINT8   ht40PowerIncForPdadc;                  // 1
+    A_UINT8   bswAtten[AR5416_MAX_CHAINS];           // 3
+    A_UINT8   bswMargin[AR5416_MAX_CHAINS];          // 3
+    A_UINT8   swSettleHt40;                          // 1
+    A_UINT8   futureModal[22];                       //
+    SPUR_CHAN spurChans[AR5416_EEPROM_MODAL_SPURS];  // 20 B
+} __ATTRIB_PACK MODAL_EEP_HEADER;                    // == 100 B
+
+typedef struct calDataPerFreq {
+    A_UINT8 pwrPdg[AR5416_NUM_PD_GAINS][AR5416_PD_GAIN_ICEPTS];
+    A_UINT8 vpdPdg[AR5416_NUM_PD_GAINS][AR5416_PD_GAIN_ICEPTS];
+} __ATTRIB_PACK CAL_DATA_PER_FREQ;
+
+typedef struct CalTargetPowerLegacy {
+    A_UINT8  bChannel;
+    A_UINT8  tPow2x[4];
+} __ATTRIB_PACK CAL_TARGET_POWER_LEG;
+
+typedef struct CalTargetPowerHt {
+    A_UINT8  bChannel;
+    A_UINT8  tPow2x[8];
+} __ATTRIB_PACK CAL_TARGET_POWER_HT;
+
+#if defined(ARCH_BIG_ENDIAN) || defined(BIG_ENDIAN)
+typedef struct CalCtlEdges {
+    A_UINT8  bChannel;
+    A_UINT8  flag   :2,
+             tPower :6;
+} __ATTRIB_PACK CAL_CTL_EDGES;
+#else
+typedef struct CalCtlEdges {
+    A_UINT8  bChannel;
+    A_UINT8  tPower :6,
+             flag   :2;
+} __ATTRIB_PACK CAL_CTL_EDGES;
+#endif
+
+typedef struct CalCtlData {
+    CAL_CTL_EDGES  ctlEdges[AR5416_MAX_CHAINS][AR5416_NUM_BAND_EDGES];
+} __ATTRIB_PACK CAL_CTL_DATA;
+
+typedef struct ar5416Eeprom {
+    BASE_EEP_HEADER    baseEepHeader;         // 64 B
+    A_UINT8   custData[64];                   // 64 B
+    MODAL_EEP_HEADER   modalHeader[2];        // 200 B
+    A_UINT8            calFreqPier5G[AR5416_NUM_5G_CAL_PIERS];
+    A_UINT8            calFreqPier2G[AR5416_NUM_2G_CAL_PIERS];
+    CAL_DATA_PER_FREQ  calPierData5G[AR5416_MAX_CHAINS][AR5416_NUM_5G_CAL_PIERS];
+    CAL_DATA_PER_FREQ  calPierData2G[AR5416_MAX_CHAINS][AR5416_NUM_2G_CAL_PIERS];
+    CAL_TARGET_POWER_LEG calTargetPower5G[AR5416_NUM_5G_20_TARGET_POWERS];
+    CAL_TARGET_POWER_HT  calTargetPower5GHT20[AR5416_NUM_5G_20_TARGET_POWERS];
+    CAL_TARGET_POWER_HT  calTargetPower5GHT40[AR5416_NUM_5G_40_TARGET_POWERS];
+    CAL_TARGET_POWER_LEG calTargetPowerCck[AR5416_NUM_2G_CCK_TARGET_POWERS];
+    CAL_TARGET_POWER_LEG calTargetPower2G[AR5416_NUM_2G_20_TARGET_POWERS];
+    CAL_TARGET_POWER_HT  calTargetPower2GHT20[AR5416_NUM_2G_20_TARGET_POWERS];
+    CAL_TARGET_POWER_HT  calTargetPower2GHT40[AR5416_NUM_2G_40_TARGET_POWERS];
+    A_UINT8            ctlIndex[AR5416_NUM_CTLS];
+    CAL_CTL_DATA       ctlData[AR5416_NUM_CTLS];
+    A_UINT8            padding;
+} __ATTRIB_PACK AR5416_EEPROM;
+
+#pragma pack (pop)
+
+typedef enum ConformanceTestLimits {
+    FCC        = 0x10,
+    MKK        = 0x40,
+    ETSI       = 0x30,
+    SD_NO_CTL  = 0xE0,
+    NO_CTL     = 0xFF,
+    CTL_MODE_M = 0xF,
+    CTL_11A    = 0,
+    CTL_11B    = 1,
+    CTL_11G    = 2,
+    CTL_TURBO  = 3,
+    CTL_108G   = 4,
+    CTL_2GHT20 = 5,
+    CTL_5GHT20 = 6,
+    CTL_2GHT40 = 7,
+    CTL_5GHT40 = 8,
+} ATH_CTLS;
+
+#endif /* #ifndef _HPUSB_H */
diff --git a/drivers/staging/otus/hal/otus.ini b/drivers/staging/otus/hal/otus.ini
new file mode 100644
index 00000000000..34efeb6c285
--- /dev/null
+++ b/drivers/staging/otus/hal/otus.ini
@@ -0,0 +1,414 @@
+/* 8602 : update mismatch register between NDIS and ART */
+static const u32_t ar5416Modes[][6] = {
+/* Register   A-20        A-20/40     G-20/40     G-20        G-Turbo    */
+     {0x9800, 0x00000007, 0x00000007, 0x00000007, 0x00000007, 0},
+     {0x9804, 0x00000300, 0x000003c4, 0x000003c4, 0x00000300, 0},
+     {0x9808, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x980c, 0xad848e19, 0xad848e19, 0xad848e19, 0xad848e19, 0},
+     {0x9810, 0x7d14e000, 0x7d14e000, 0x7d14e000, 0x7d14e000, 0},
+     {0x9814, 0x9c0a9f6b, 0x9c0a9f6b, 0x9c0a9f6b, 0x9c0a9f6b, 0},
+     {0x9818, 0x00000090, 0x00000090, 0x00000090, 0x00000090, 0},
+     {0x981c, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9820, 0x02020200, 0x02020200, 0x02020200, 0x02020200, 0},
+     {0x9824, 0x00000e0e, 0x00000e0e, 0x00000e0e, 0x00000e0e, 0},
+     {0x9828, 0x0a020001, 0x0a020001, 0x0a020001, 0x0a020001, 0},
+     {0x982c, 0x0000a000, 0x0000a000, 0x0000a000, 0x0000a000, 0},
+     {0x9830, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9834, 0x00000e0e, 0x00000e0e, 0x00000e0e, 0x00000e0e, 0},
+     {0x9838, 0x00000007, 0x00000007, 0x00000007, 0x00000007, 0},
+     {0x983c, 0x00200400, 0x00200400, 0x00200400, 0x00200400, 0},
+     {0x9840, 0x206a002e, 0x206a002e, 0x206a002e, 0x206a002e, 0},
+     {0x9844, 0x1372161e, 0x13721c1e, 0x13721c24, 0x137216a4, 0},
+     {0x9848, 0x001a6a65, 0x001a6a65, 0x00197a68, 0x00197a68, 0},
+     {0x984c, 0x1284233c, 0x1284233c, 0x1284233c, 0x1284233c, 0},
+     {0x9850, 0x6c48b4e4, 0x6c48b4e4, 0x6c48b0e4, 0x6c48b0e4, 0},
+     {0x9854, 0x00000859, 0x00000859, 0x00000859, 0x00000859, 0},
+     {0x9858, 0x7ec80d2e, 0x7ec80d2e, 0x7ec80d2e, 0x7ec80d2e, 0},
+     {0x985c, 0x31395c5e, 0x31395c5e, 0x31395c5e, 0x31395c5e, 0},
+     {0x9860, 0x0004dd10, 0x0004dd10, 0x0004dd20, 0x0004dd20, 0},
+     {0x9868, 0x409a4190, 0x409a4190, 0x409a4190, 0x409a4190, 0},
+     {0x986c, 0x050cb081, 0x050cb081, 0x050cb081, 0x050cb081, 0},
+     {0x9900, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9904, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9908, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x990c, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9914, 0x000007d0, 0x000007d0, 0x00000898, 0x00000898, 0},
+     {0x9918, 0x00000118, 0x00000230, 0x00000268, 0x00000134, 0},
+     {0x991c, 0x10000fff, 0x10000fff, 0x10000fff, 0x10000fff, 0},
+     {0x9920, 0x0510081c, 0x0510081c, 0x0510001c, 0x0510001c, 0},
+     {0x9924, 0xd0058a15, 0xd0058a15, 0xd0058a15, 0xd0058a15, 0},
+     {0x9928, 0x00000001, 0x00000001, 0x00000001, 0x00000001, 0},
+     {0x992c, 0x00000004, 0x00000004, 0x00000004, 0x00000004, 0},
+     {0x9934, 0x3f3f3f3f, 0x3f3f3f3f, 0x3f3f3f3f, 0x3f3f3f3f, 0},
+     {0x9938, 0x3f3f3f3f, 0x3f3f3f3f, 0x3f3f3f3f, 0x3f3f3f3f, 0},
+     {0x993c, 0x0000007f, 0x0000007f, 0x0000007f, 0x0000007f, 0},
+     {0x9944, 0xdfb81020, 0xdfb81020, 0xdfb81020, 0xdfb81020, 0},
+     {0x9948, 0x9280b212, 0x9280b212, 0x9280b212, 0x9280b212, 0},
+     {0x994c, 0x00020028, 0x00020028, 0x00020028, 0x00020028, 0},
+     {0x9954, 0x5d50e188, 0x5d50e188, 0x5d50e188, 0x5d50e188, 0},
+     {0x9958, 0x00081fff, 0x00081fff, 0x00081fff, 0x00081fff, 0},
+     {0x9960, 0x00009b40, 0x00009b40, 0x00009b40, 0x00009b40, 0},
+     {0x9964, 0x00001120, 0x00001120, 0x00001120, 0x00001120, 0},
+     {0x9970, 0x190fb515, 0x190fb515, 0x190fb515, 0x190fb515, 0},
+     {0x9974, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9978, 0x00000001, 0x00000001, 0x00000001, 0x00000001, 0},
+     {0x997c, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9980, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9984, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9988, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x998c, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9990, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9994, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9998, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x999c, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x99a0, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x99a4, 0x00000007, 0x00000007, 0x00000007, 0x00000007, 0},
+     {0x99a8, 0x001fff00, 0x001fff00, 0x001fff00, 0x001fff00, 0},
+     {0x99ac, 0x006f00c4, 0x006f00c4, 0x006f00c4, 0x006f00c4, 0},
+     {0x99b0, 0x03051000, 0x03051000, 0x03051000, 0x03051000, 0},
+     {0x99b4, 0x00000820, 0x00000820, 0x00000820, 0x00000820, 0},
+     {0x99c0, 0x038919be, 0x038919be, 0x038919be, 0x038919be, 0},
+     {0x99c4, 0x06336f77, 0x06336f77, 0x06336f77, 0x06336f77, 0},
+     {0x99c8, 0x60f6532c, 0x60f6532c, 0x60f6532c, 0x60f6532c, 0},
+     {0x99cc, 0x08f186c8, 0x08f186c8, 0x08f186c8, 0x08f186c8, 0},
+     {0x99d0, 0x00046384, 0x00046384, 0x00046384, 0x00046384, 0},
+     {0x99d4, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x99d8, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x99dc, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x99e0, 0x00000200, 0x00000200, 0x00000200, 0x00000200, 0},
+     {0x99e4, 0x64646464, 0x64646464, 0x64646464, 0x64646464, 0},
+     {0x99e8, 0x3c787878, 0x3c787878, 0x3c787878, 0x3c787878, 0},
+     {0x99ec, 0x000000aa, 0x000000aa, 0x000000aa, 0x000000aa, 0},
+     {0x99f0, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x99fc, 0x00001042, 0x00001042, 0x00001042, 0x00001042, 0},
+     {0x9a00, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9a04, 0x00000040, 0x00000040, 0x00000040, 0x00000040, 0},
+     {0x9a08, 0x00000080, 0x00000080, 0x00000080, 0x00000080, 0},
+     {0x9a0c, 0x000001a1, 0x000001a1, 0x00000141, 0x00000141, 0},
+     {0x9a10, 0x000001e1, 0x000001e1, 0x00000181, 0x00000181, 0},
+     {0x9a14, 0x00000021, 0x00000021, 0x000001c1, 0x000001c1, 0},
+     {0x9a18, 0x00000061, 0x00000061, 0x00000001, 0x00000001, 0},
+     {0x9a1c, 0x00000168, 0x00000168, 0x00000041, 0x00000041, 0},
+     {0x9a20, 0x000001a8, 0x000001a8, 0x000001a8, 0x000001a8, 0},
+     {0x9a24, 0x000001e8, 0x000001e8, 0x000001e8, 0x000001e8, 0},
+     {0x9a28, 0x00000028, 0x00000028, 0x00000028, 0x00000028, 0},
+     {0x9a2c, 0x00000068, 0x00000068, 0x00000068, 0x00000068, 0},
+     {0x9a30, 0x00000189, 0x00000189, 0x000000a8, 0x000000a8, 0},
+     {0x9a34, 0x000001c9, 0x000001c9, 0x00000169, 0x00000169, 0},
+     {0x9a38, 0x00000009, 0x00000009, 0x000001a9, 0x000001a9, 0},
+     {0x9a3c, 0x00000049, 0x00000049, 0x000001e9, 0x000001e9, 0},
+     {0x9a40, 0x00000089, 0x00000089, 0x00000029, 0x00000029, 0},
+     {0x9a44, 0x00000170, 0x00000170, 0x00000069, 0x00000069, 0},
+     {0x9a48, 0x000001b0, 0x000001b0, 0x00000190, 0x00000190, 0},
+     {0x9a4c, 0x000001f0, 0x000001f0, 0x000001d0, 0x000001d0, 0},
+     {0x9a50, 0x00000030, 0x00000030, 0x00000010, 0x00000010, 0},
+     {0x9a54, 0x00000070, 0x00000070, 0x00000050, 0x00000050, 0},
+     {0x9a58, 0x00000191, 0x00000191, 0x00000090, 0x00000090, 0},
+     {0x9a5c, 0x000001d1, 0x000001d1, 0x00000151, 0x00000151, 0},
+     {0x9a60, 0x00000011, 0x00000011, 0x00000191, 0x00000191, 0},
+     {0x9a64, 0x00000051, 0x00000051, 0x000001d1, 0x000001d1, 0},
+     {0x9a68, 0x00000091, 0x00000091, 0x00000011, 0x00000011, 0},
+     {0x9a6c, 0x000001b8, 0x000001b8, 0x00000051, 0x00000051, 0},
+     {0x9a70, 0x000001f8, 0x000001f8, 0x00000198, 0x00000198, 0},
+     {0x9a74, 0x00000038, 0x00000038, 0x000001d8, 0x000001d8, 0},
+     {0x9a78, 0x00000078, 0x00000078, 0x00000018, 0x00000018, 0},
+     {0x9a7c, 0x00000199, 0x00000199, 0x00000058, 0x00000058, 0},
+     {0x9a80, 0x000001d9, 0x000001d9, 0x00000098, 0x00000098, 0},
+     {0x9a84, 0x00000019, 0x00000019, 0x00000159, 0x00000159, 0},
+     {0x9a88, 0x00000059, 0x00000059, 0x00000199, 0x00000199, 0},
+     {0x9a8c, 0x00000099, 0x00000099, 0x000001d9, 0x000001d9, 0},
+     {0x9a90, 0x000000d9, 0x000000d9, 0x00000019, 0x00000019, 0},
+     {0x9a94, 0x000000f9, 0x000000f9, 0x00000059, 0x00000059, 0},
+     {0x9a98, 0x000000f9, 0x000000f9, 0x00000099, 0x00000099, 0},
+     {0x9a9c, 0x000000f9, 0x000000f9, 0x000000d9, 0x000000d9, 0},
+     {0x9aa0, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9aa4, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9aa8, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9aac, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9ab0, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9ab4, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9ab8, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9abc, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9ac0, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9ac4, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9ac8, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9acc, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9ad0, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9ad4, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9ad8, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9adc, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9ae0, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9ae4, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9ae8, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9aec, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9af0, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9af4, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9af8, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9afc, 0x000000f9, 0x000000f9, 0x000000f9, 0x000000f9, 0},
+     {0x9b00, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9b04, 0x00000001, 0x00000001, 0x00000001, 0x00000001, 0},
+     {0x9b08, 0x00000002, 0x00000002, 0x00000002, 0x00000002, 0},
+     {0x9b0c, 0x00000003, 0x00000003, 0x00000003, 0x00000003, 0},
+     {0x9b10, 0x00000004, 0x00000004, 0x00000004, 0x00000004, 0},
+     {0x9b14, 0x00000005, 0x00000005, 0x00000005, 0x00000005, 0},
+     {0x9b18, 0x00000008, 0x00000008, 0x00000008, 0x00000008, 0},
+     {0x9b1c, 0x00000009, 0x00000009, 0x00000009, 0x00000009, 0},
+     {0x9b20, 0x0000000a, 0x0000000a, 0x0000000a, 0x0000000a, 0},
+     {0x9b24, 0x0000000b, 0x0000000b, 0x0000000b, 0x0000000b, 0},
+     {0x9b28, 0x0000000c, 0x0000000c, 0x0000000c, 0x0000000c, 0},
+     {0x9b2c, 0x0000000d, 0x0000000d, 0x0000000d, 0x0000000d, 0},
+     {0x9b30, 0x00000010, 0x00000010, 0x00000010, 0x00000010, 0},
+     {0x9b34, 0x00000011, 0x00000011, 0x00000011, 0x00000011, 0},
+     {0x9b38, 0x00000012, 0x00000012, 0x00000012, 0x00000012, 0},
+     {0x9b3c, 0x00000013, 0x00000013, 0x00000013, 0x00000013, 0},
+     {0x9b40, 0x00000014, 0x00000014, 0x00000014, 0x00000014, 0},
+     {0x9b44, 0x00000015, 0x00000015, 0x00000015, 0x00000015, 0},
+     {0x9b48, 0x00000018, 0x00000018, 0x00000018, 0x00000018, 0},
+     {0x9b4c, 0x00000019, 0x00000019, 0x00000019, 0x00000019, 0},
+     {0x9b50, 0x0000001a, 0x0000001a, 0x0000001a, 0x0000001a, 0},
+     {0x9b54, 0x0000001b, 0x0000001b, 0x0000001b, 0x0000001b, 0},
+     {0x9b58, 0x0000001c, 0x0000001c, 0x0000001c, 0x0000001c, 0},
+     {0x9b5c, 0x0000001d, 0x0000001d, 0x0000001d, 0x0000001d, 0},
+     {0x9b60, 0x00000020, 0x00000020, 0x00000020, 0x00000020, 0},
+     {0x9b64, 0x00000021, 0x00000021, 0x00000021, 0x00000021, 0},
+     {0x9b68, 0x00000022, 0x00000022, 0x00000022, 0x00000022, 0},
+     {0x9b6c, 0x00000023, 0x00000023, 0x00000023, 0x00000023, 0},
+     {0x9b70, 0x00000024, 0x00000024, 0x00000024, 0x00000024, 0},
+     {0x9b74, 0x00000025, 0x00000025, 0x00000025, 0x00000025, 0},
+     {0x9b78, 0x00000028, 0x00000028, 0x00000028, 0x00000028, 0},
+     {0x9b7c, 0x00000029, 0x00000029, 0x00000029, 0x00000029, 0},
+     {0x9b80, 0x0000002a, 0x0000002a, 0x0000002a, 0x0000002a, 0},
+     {0x9b84, 0x0000002b, 0x0000002b, 0x0000002b, 0x0000002b, 0},
+     {0x9b88, 0x0000002c, 0x0000002c, 0x0000002c, 0x0000002c, 0},
+     {0x9b8c, 0x0000002d, 0x0000002d, 0x0000002d, 0x0000002d, 0},
+     {0x9b90, 0x00000030, 0x00000030, 0x00000030, 0x00000030, 0},
+     {0x9b94, 0x00000031, 0x00000031, 0x00000031, 0x00000031, 0},
+     {0x9b98, 0x00000032, 0x00000032, 0x00000032, 0x00000032, 0},
+     {0x9b9c, 0x00000033, 0x00000033, 0x00000033, 0x00000033, 0},
+     {0x9ba0, 0x00000034, 0x00000034, 0x00000034, 0x00000034, 0},
+     {0x9ba4, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9ba8, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9bac, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9bb0, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9bb4, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9bb8, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9bbc, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9bc0, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9bc4, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9bc8, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9bcc, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9bd0, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9bd4, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9bd8, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9bdc, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9be0, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9be4, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9be8, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9bec, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9bf0, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9bf4, 0x00000035, 0x00000035, 0x00000035, 0x00000035, 0},
+     {0x9bf8, 0x00000010, 0x00000010, 0x00000010, 0x00000010, 0},
+     {0x9bfc, 0x0000001a, 0x0000001a, 0x0000001a, 0x0000001a, 0},
+     {0x9c00, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9c0c, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9c10, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9c14, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9c18, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9c1c, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9c20, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9c24, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9c28, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9c2c, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9c30, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9c34, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9c38, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9c3c, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9cf0, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9cf4, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9cf8, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0x9cfc, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa200, 0x00000008, 0x00000008, 0x0000000e, 0x0000000e, 0},
+     {0xa204, 0x00000440, 0x00000440, 0x00000440, 0x00000440, 0},
+     {0xa208, 0xd6be4788, 0xd6be4788, 0xd03e4788, 0xd03e4788, 0},
+     {0xa20c, 0x012e8160, 0x012e8160, 0x012a8160, 0x012a8160, 0},
+     {0xa210, 0x40806333, 0x40806333, 0x40806333, 0x40806333, 0},
+     {0xa214, 0x00106c10, 0x00106c10, 0x00106c10, 0x00106c10, 0},
+     {0xa218, 0x009c4060, 0x009c4060, 0x009c4060, 0x009c4060, 0},
+     {0xa21c, 0x1883800a, 0x1883800a, 0x1883800a, 0x1883800a, 0},
+     {0xa220, 0x018830c6, 0x018830c6, 0x018830c6, 0x018830c6, 0},
+     {0xa224, 0x00000400, 0x00000400, 0x00000400, 0x00000400, 0},
+     {0xa228, 0x000009b5, 0x000009b5, 0x000009b5, 0x000009b5, 0},
+     {0xa22c, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa230, 0x00000108, 0x00000210, 0x00000210, 0x00000108, 0},
+     {0xa234, 0x3f3f3f3f, 0x3f3f3f3f, 0x3f3f3f3f, 0x3f3f3f3f, 0},
+     {0xa238, 0x3f3f3f3f, 0x3f3f3f3f, 0x3f3f3f3f, 0x3f3f3f3f, 0},
+     {0xa23c, 0x13c889af, 0x13c889af, 0x13c889af, 0x13c889af, 0},
+     {0xa240, 0x38490a20, 0x38490a20, 0x38490a20, 0x38490a20, 0},
+     {0xa244, 0x00007bb6, 0x00007bb6, 0x00007bb6, 0x00007bb6, 0},
+     {0xa248, 0x0fff3ffc, 0x0fff3ffc, 0x0fff3ffc, 0x0fff3ffc, 0},
+     {0xa24c, 0x00000001, 0x00000001, 0x00000001, 0x00000001, 0},
+     {0xa250, 0x0000a000, 0x0000a000, 0x0000a000, 0x0000a000, 0},
+     {0xa254, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa258, 0x0cc75380, 0x0cc75380, 0x0cc75380, 0x0cc75380, 0},
+     {0xa25c, 0x0f0f0f01, 0x0f0f0f01, 0x0f0f0f01, 0x0f0f0f01, 0},
+     {0xa260, 0xdfa91f01, 0xdfa91f01, 0xdfa91f01, 0xdfa91f01, 0},
+     {0xa264, 0x00418a11, 0x00418a11, 0x00418a11, 0x00418a11, 0},
+     {0xa268, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa26c, 0x09249126, 0x09249126, 0x09249126, 0x09249126, 0},
+     {0xa274, 0x0a1a9caa, 0x0a1a9caa, 0x0a1a7caa, 0x0a1a7caa, 0},
+     {0xa278, 0x1ce739ce, 0x1ce739ce, 0x1ce739ce, 0x1ce739ce, 0},
+     {0xa27c, 0x051701ce, 0x051701ce, 0x051701ce, 0x051701ce, 0},
+     {0xa300, 0x18010000, 0x18010000, 0x18010000, 0x18010000, 0},
+     {0xa304, 0x30032602, 0x30032602, 0x2e032402, 0x2e032402, 0},
+     {0xa308, 0x48073e06, 0x48073e06, 0x4a0a3c06, 0x4a0a3c06, 0},
+     {0xa30c, 0x560b4c0a, 0x560b4c0a, 0x621a540b, 0x621a540b, 0},
+     {0xa310, 0x641a600f, 0x641a600f, 0x764f6c1b, 0x764f6c1b, 0},
+     {0xa314, 0x7a4f6e1b, 0x7a4f6e1b, 0x845b7a5a, 0x845b7a5a, 0},
+     {0xa318, 0x8c5b7e5a, 0x8c5b7e5a, 0x950f8ccf, 0x950f8ccf, 0},
+     {0xa31c, 0x9d0f96cf, 0x9d0f96cf, 0xa5cf9b4f, 0xa5cf9b4f, 0},
+     {0xa320, 0xb51fa69f, 0xb51fa69f, 0xbddfaf1f, 0xbddfaf1f, 0},
+     {0xa324, 0xcb3fbd07, 0xcb3fbcbf, 0xd1ffc93f, 0xd1ffc93f, 0},
+     {0xa328, 0x0000d7bf, 0x0000d7bf, 0x00000000, 0x00000000, 0},
+     {0xa32c, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa330, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa334, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa338, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa33c, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa340, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa344, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa348, 0x3fffffff, 0x3fffffff, 0x3fffffff, 0x3fffffff, 0},
+     {0xa34c, 0x3fffffff, 0x3fffffff, 0x3fffffff, 0x3fffffff, 0},
+     {0xa350, 0x3fffffff, 0x3fffffff, 0x3fffffff, 0x3fffffff, 0},
+     {0xa354, 0x0003ffff, 0x0003ffff, 0x0003ffff, 0x0003ffff, 0},
+     {0xa358, 0x79a8aa1f, 0x79a8aa1f, 0x79a8aa1f, 0x79a8aa1f, 0},
+     {0xa388, 0x08000000, 0x08000000, 0x08000000, 0x08000000, 0},
+     {0xa38c, 0x3f3f3f3f, 0x3f3f3f3f, 0x3f3f3f3f, 0x3f3f3f3f, 0},
+     {0xa390, 0x3f3f3f3f, 0x3f3f3f3f, 0x3f3f3f3f, 0x3f3f3f3f, 0},
+     {0xa394, 0x1ce739ce, 0x1ce739ce, 0x1ce739ce, 0x1ce739ce, 0},
+     {0xa398, 0x000001ce, 0x000001ce, 0x000001ce, 0x000001ce, 0},
+     {0xa39c, 0x00000007, 0x00000007, 0x00000007, 0x00000007, 0},
+     {0xa3a0, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa3a4, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa3a8, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa3ac, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa3b0, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa3b4, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa3b8, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa3bc, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa3c0, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa3c4, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa3c8, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa3cc, 0x3f3f3f3f, 0x3f3f3f3f, 0x3f3f3f3f, 0x3f3f3f3f, 0},
+     {0xa3d0, 0x3f3f3f3f, 0x3f3f3f3f, 0x3f3f3f3f, 0x3f3f3f3f, 0},
+     {0xa3d4, 0x3f3f3f3f, 0x3f3f3f3f, 0x3f3f3f3f, 0x3f3f3f3f, 0},
+     {0xa3d8, 0x00000000, 0x00000000, 0x00000000, 0x00000000, 0},
+     {0xa3dc, 0x1ce739ce, 0x1ce739ce, 0x1ce739ce, 0x1ce739ce, 0},
+     {0xa3e0, 0x000000c0, 0x000000c0, 0x000000c0, 0x000000c0, 0},
+     {0xa848, 0x00180a65, 0x00180a65, 0x00180a68, 0x00180a68, 0},
+     {0xa920, 0x0510001c, 0x0510001c, 0x0510001c, 0x0510001c, 0},
+     {0xa960, 0x00009b40, 0x00009b40, 0x00009b40, 0x00009b40, 0},
+     {0xb20c, 0x012e8160, 0x012e8160, 0x012a8160, 0x012a8160, 0},
+     {0xb26c, 0x09249126, 0x09249126, 0x09249126, 0x09249126, 0},
+     {0xb848, 0x00180a65, 0x00180a65, 0x00180a68, 0x00180a68, 0},
+     {0xb920, 0x0510001c, 0x0510001c, 0x0510001c, 0x0510001c, 0},
+     {0xb960, 0x00009b40, 0x00009b40, 0x00009b40, 0x00009b40, 0},
+     {0xc20c, 0x012e8160, 0x012e8160, 0x012a8160, 0x012a8160, 0},
+     {0xc26c, 0x09249126, 0x09249126, 0x09249126, 0x09249126, 0},
+     //{0xc864, 0x0001ce00, 0x0001ce00, 0x0001ce00, 0x0001ce00, 0},
+     {0xc864, 0x0001c600, 0x0001c600, 0x0001c600, 0x0001c600, 0},
+     {0xc95c, 0x004b6a8e, 0x004b6a8e, 0x004b6a8e, 0x004b6a8e, 0},
+     {0xc968, 0x000003ce, 0x000003ce, 0x000003ce, 0x000003ce, 0},
+     {0xc9bc, 0x00181400, 0x00181400, 0x00181400, 0x00181400, 0},
+     {0xd270, 0x00820820, 0x00820820, 0x00820820, 0x00820820, 0},
+     {0xd35c, 0x066c420f, 0x066c420f, 0x066c420f, 0x066c420f, 0},
+     {0xd360, 0x0f282207, 0x0f282207, 0x0f282207, 0x0f282207, 0},
+     {0xd364, 0x17601685, 0x17601685, 0x17601685, 0x17601685, 0},
+     {0xd368, 0x1f801104, 0x1f801104, 0x1f801104, 0x1f801104, 0},
+     {0xd36c, 0x37a00c03, 0x37a00c03, 0x37a00c03, 0x37a00c03, 0},
+     {0xd370, 0x3fc40883, 0x3fc40883, 0x3fc40883, 0x3fc40883, 0},
+     {0xd374, 0x57c00803, 0x57c00803, 0x57c00803, 0x57c00803, 0},
+     {0xd378, 0x5fd80682, 0x5fd80682, 0x5fd80682, 0x5fd80682, 0},
+     {0xd37c, 0x7fe00482, 0x7fe00482, 0x7fe00482, 0x7fe00482, 0},
+     {0xd380, 0x7f3c7bba, 0x7f3c7bba, 0x7f3c7bba, 0x7f3c7bba, 0},
+     {0xd384, 0xf3307ff0, 0xf3307ff0, 0xf3307ff0, 0xf3307ff0, 0}
+};
+
+
+static const u32_t otusBank[][3] = {
+     //# bank 0
+     {0x98b0,  0x1e5795e5,  0x1e5795e5},
+     {0x98e0,  0x02008020,  0x02008020},
+     //# bank 1
+     {0x98b0,  0x02108421,  0x02108421},
+     {0x98ec,  0x00000008,  0x00000008},
+     //# bank 2
+     {0x98b0,  0x0e73ff17,  0x0e73ff17},
+     {0x98e0,  0x00000420,  0x00000420},
+     //# bank 3
+     {0x98f0,  0x01400018,  0x01c00018},
+     //# bank 4
+     {0x98b0,  0x000001a1,  0x000001a1},
+     {0x98e8,  0x00000001,  0x00000001},
+     //# bank 5
+     {0x98b0,  0x00000013,  0x00000013},
+     {0x98e4,  0x00000002,  0x00000002},
+     //# bank 6
+     {0x98b0,  0x00000000,  0x00000000},
+     {0x98b0,  0x00000000,  0x00000000},
+     {0x98b0,  0x00000000,  0x00000000},
+     {0x98b0,  0x00000000,  0x00000000},
+     {0x98b0,  0x00000000,  0x00000000},
+     {0x98b0,  0x00004000,  0x00004000},
+     {0x98b0,  0x00006c00,  0x00006c00},
+     {0x98b0,  0x00002c00,  0x00002c00},
+     {0x98b0,  0x00004800,  0x00004800},
+     {0x98b0,  0x00004000,  0x00004000},
+     {0x98b0,  0x00006000,  0x00006000},
+     {0x98b0,  0x00001000,  0x00001000},
+     {0x98b0,  0x00004000,  0x00004000},
+     {0x98b0,  0x00007c00,  0x00007c00},
+     {0x98b0,  0x00007c00,  0x00007c00},
+     {0x98b0,  0x00007c00,  0x00007c00},
+     {0x98b0,  0x00007c00,  0x00007c00},
+     {0x98b0,  0x00007c00,  0x00007c00},
+     {0x98b0,  0x00087c00,  0x00087c00},
+     {0x98b0,  0x00007c00,  0x00007c00},
+     {0x98b0,  0x00005400,  0x00005400},
+     {0x98b0,  0x00000c00,  0x00000c00},
+     {0x98b0,  0x00001800,  0x00001800},
+     {0x98b0,  0x00007c00,  0x00007c00},
+     {0x98b0,  0x00006c00,  0x00006c00},
+     {0x98b0,  0x00006c00,  0x00006c00},
+     {0x98b0,  0x00007c00,  0x00007c00},
+     {0x98b0,  0x00002c00,  0x00002c00},
+     {0x98b0,  0x00003c00,  0x00003c00},
+     {0x98b0,  0x00003800,  0x00003800},
+     {0x98b0,  0x00001c00,  0x00001c00},
+     {0x98b0,  0x00000800,  0x00000800},
+     {0x98b0,  0x00000408,  0x00000408},
+     {0x98b0,  0x00004c15,  0x00004c15},
+     {0x98b0,  0x00004188,  0x00004188},
+     {0x98b0,  0x0000201e,  0x0000201e},
+     {0x98b0,  0x00010408,  0x00010408},
+     {0x98b0,  0x00000801,  0x00000801},
+     {0x98b0,  0x00000c08,  0x00000c08},
+     {0x98b0,  0x0000181e,  0x0000181e},
+     {0x98b0,  0x00001016,  0x00001016},
+     {0x98b0,  0x00002800,  0x00002800},
+     {0x98b0,  0x00004010,  0x00004010},
+     {0x98b0,  0x0000081c,  0x0000081c},
+     {0x98b0,  0x00000115,  0x00000115},
+     {0x98b0,  0x00000015,  0x00000015},
+     {0x98b0,  0x00000066,  0x00000066},
+     {0x98b0,  0x0000001c,  0x0000001c},
+     {0x98b0,  0x00000000,  0x00000000},
+     {0x98b0,  0x00000004,  0x00000004},
+     {0x98b0,  0x00000015,  0x00000015},
+     {0x98b0,  0x0000001f,  0x0000001f},
+     {0x98e0,  0x00000000,  0x00000400},
+     //# bank 7
+     {0x98b0,  0x000000a0,  0x000000a0},
+     {0x98b0,  0x00000000,  0x00000000},
+     {0x98b0,  0x00000040,  0x00000040},
+     {0x98f0,  0x0000001c,  0x0000001c}
+};
diff --git a/drivers/staging/otus/ioctl.c b/drivers/staging/otus/ioctl.c
new file mode 100644
index 00000000000..7a5c1e876b0
--- /dev/null
+++ b/drivers/staging/otus/ioctl.c
@@ -0,0 +1,2913 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : ioctl.c                                               */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains Linux wireless extension related functons. */
+/*                                                                      */
+/*  NOTES                                                               */
+/*     Platform dependent.                                              */
+/*                                                                      */
+/************************************************************************/
+#include <linux/module.h>
+#include <linux/if_arp.h>
+#include <asm/uaccess.h>
+
+#include "usbdrv.h"
+
+#define ZD_IOCTL_WPA			    (SIOCDEVPRIVATE + 1)
+#define ZD_IOCTL_PARAM			    (SIOCDEVPRIVATE + 2)
+#define ZD_IOCTL_GETWPAIE		    (SIOCDEVPRIVATE + 3)
+#ifdef ZM_ENABLE_CENC
+#define ZM_IOCTL_CENC               (SIOCDEVPRIVATE + 4)
+#endif //ZM_ENABLE_CENC
+#define ZD_PARAM_ROAMING		    0x0001
+#define ZD_PARAM_PRIVACY		    0x0002
+#define ZD_PARAM_WPA			    0x0003
+#define ZD_PARAM_COUNTERMEASURES	0x0004
+#define ZD_PARAM_DROPUNENCRYPTED	0x0005
+#define ZD_PARAM_AUTH_ALGS		    0x0006
+#define ZD_PARAM_WPS_FILTER		    0x0007
+
+#ifdef ZM_ENABLE_CENC
+#define P80211_PACKET_CENCFLAG		0x0001
+#endif //ZM_ENABLE_CENC
+#define P80211_PACKET_SETKEY     	0x0003
+
+#define ZD_CMD_SET_ENCRYPT_KEY		0x0001
+#define ZD_CMD_SET_MLME			    0x0002
+#define ZD_CMD_SCAN_REQ			    0x0003
+#define ZD_CMD_SET_GENERIC_ELEMENT	0x0004
+#define ZD_CMD_GET_TSC			    0x0005
+
+#define ZD_CRYPT_ALG_NAME_LEN		16
+#define ZD_MAX_KEY_SIZE			    32
+#define ZD_MAX_GENERIC_SIZE		    64
+
+#if WIRELESS_EXT > 12
+#include <net/iw_handler.h>
+#endif
+
+extern u16_t zfLnxGetVapId(zdev_t* dev);
+
+static const u32_t channel_frequency_11A[] =
+{
+//Even element for Channel Number, Odd for Frequency
+    36,5180,
+    40,5200,
+    44,5220,
+    48,5240,
+    52,5260,
+    56,5280,
+    60,5300,
+    64,5320,
+    100,5500,
+    104,5520,
+    108,5540,
+    112,5560,
+    116,5580,
+    120,5600,
+    124,5620,
+    128,5640,
+    132,5660,
+    136,5680,
+    140,5700,
+//
+    184,4920,
+    188,4940,
+    192,4960,
+    196,4980,
+    8,5040,
+    12,5060,
+    16,5080,
+    34,5170,
+    38,5190,
+    42,5210,
+    46,5230,
+//
+    149,5745,
+    153,5765,
+    157,5785,
+    161,5805,
+    165,5825
+//
+};
+
+int usbdrv_freq2chan(u32_t freq)
+{
+    /* 2.4G Hz */
+    if (freq > 2400 && freq < 3000)
+    {
+        return ((freq-2412)/5) + 1;
+    }
+    else
+    {
+        u16_t ii;
+        u16_t num_chan = sizeof(channel_frequency_11A)/sizeof(u32_t);
+
+        for(ii = 1; ii < num_chan; ii += 2)
+        {
+            if (channel_frequency_11A[ii] == freq)
+                return channel_frequency_11A[ii-1];
+        }
+    }
+
+    return 0;
+}
+
+int usbdrv_chan2freq(int chan)
+{
+    int freq;
+
+    /* If channel number is out of range */
+    if (chan > 165 || chan <= 0)
+        return -1;
+
+    /* 2.4G band */
+    if (chan >= 1 && chan <= 13)
+    {
+        freq = (2412 + (chan - 1) * 5);
+        return freq;
+    }
+    else if (chan >= 36 && chan <= 165)
+    {
+        u16_t ii;
+        u16_t num_chan = sizeof(channel_frequency_11A)/sizeof(u32_t);
+
+        for(ii = 0; ii < num_chan; ii += 2)
+        {
+            if (channel_frequency_11A[ii] == chan)
+                return channel_frequency_11A[ii+1];
+        }
+
+        /* Can't find desired frequency */
+        if (ii == num_chan)
+           return -1;
+    }
+
+    /* Can't find deisred frequency */
+    return -1;
+}
+
+int usbdrv_ioctl_setessid(struct net_device *dev, struct iw_point *erq)
+{
+#ifdef ZM_HOSTAPD_SUPPORT
+    //struct usbdrv_private *macp = dev->ml_priv;
+    char essidbuf[IW_ESSID_MAX_SIZE+1];
+    int i;
+
+    if(!netif_running(dev))
+        return -EINVAL;
+
+    memset(essidbuf, 0, sizeof(essidbuf));
+
+    printk(KERN_ERR "usbdrv_ioctl_setessid\n");
+
+    //printk("ssidlen=%d\n", erq->length); //for any, it is 1.
+    if (erq->flags) {
+        if (erq->length > (IW_ESSID_MAX_SIZE+1))
+            return -E2BIG;
+
+        if (copy_from_user(essidbuf, erq->pointer, erq->length))
+            return -EFAULT;
+    }
+
+    //zd_DisasocAll(2);
+    //wait_ms(100);
+
+    printk(KERN_ERR "essidbuf: ");
+
+    for(i = 0; i < erq->length; i++)
+    {
+        printk(KERN_ERR "%02x ", essidbuf[i]);
+    }
+
+    printk(KERN_ERR "\n");
+
+    essidbuf[erq->length] = '\0';
+    //memcpy(macp->wd.ws.ssid, essidbuf, erq->length);
+    //macp->wd.ws.ssidLen = strlen(essidbuf)+2;
+    //macp->wd.ws.ssid[1] = strlen(essidbuf); // Update ssid length
+
+    zfiWlanSetSSID(dev, essidbuf, erq->length);
+#if 0
+    printk(KERN_ERR "macp->wd.ws.ssid: ");
+
+    for(i = 0; i < macp->wd.ws.ssidLen; i++)
+    {
+        printk(KERN_ERR "%02x ", macp->wd.ws.ssid[i]);
+    }
+
+    printk(KERN_ERR "\n");
+#endif
+    zfiWlanDisable(dev, 0);
+    zfiWlanEnable(dev);
+
+#endif
+
+    return 0;
+}
+
+int usbdrv_ioctl_getessid(struct net_device *dev, struct iw_point *erq)
+{
+     //struct usbdrv_private *macp = dev->ml_priv;
+     u8_t essidbuf[IW_ESSID_MAX_SIZE+1];
+     u8_t len;
+     u8_t i;
+
+
+     //len = macp->wd.ws.ssidLen;
+     //memcpy(essidbuf, macp->wd.ws.ssid, macp->wd.ws.ssidLen);
+     zfiWlanQuerySSID(dev, essidbuf, &len);
+
+     essidbuf[len] = 0;
+
+     printk(KERN_ERR "ESSID: ");
+
+     for(i = 0; i < len; i++)
+     {
+         printk(KERN_ERR "%c", essidbuf[i]);
+     }
+
+     printk(KERN_ERR "\n");
+
+     erq->flags= 1;
+     erq->length = strlen(essidbuf) + 1;
+
+     if (erq->pointer)
+         if (copy_to_user(erq->pointer, essidbuf, erq->length))
+             return -EFAULT;
+
+     return 0;
+}
+
+
+int usbdrv_ioctl_setrts(struct net_device *dev, struct iw_param *rrq)
+{
+
+    return 0;
+}
+
+#if WIRELESS_EXT > 14
+/*
+ * Encode a WPA or RSN information element as a custom
+ * element using the hostap format.
+ */
+u32 encode_ie(void *buf, u32 bufsize, const u8 *ie, u32 ielen, const u8 *leader, u32 leader_len)
+{
+    u8 *p;
+    u32 i;
+
+    if (bufsize < leader_len)
+        return 0;
+    p = buf;
+    memcpy(p, leader, leader_len);
+    bufsize -= leader_len;
+    p += leader_len;
+    for (i = 0; i < ielen && bufsize > 2; i++)
+        p += sprintf(p, "%02x", ie[i]);
+    return (i == ielen ? p - (u8 *)buf : 0);
+}
+#endif                                            /* WIRELESS_EXT > 14 */
+
+/*------------------------------------------------------------------*/
+/*
+ * Translate scan data returned from the card to a card independent
+ * format that the Wireless Tools will understand
+ */
+char *usbdrv_translate_scan(struct net_device *dev,
+	struct iw_request_info *info, char *current_ev,
+        char *end_buf, struct zsBssInfo *list)
+{
+    struct iw_event iwe;                          /* Temporary buffer */
+    u16_t capabilities;
+    char *current_val;                            /* For rates */
+    char *last_ev;
+    int i;
+#if WIRELESS_EXT > 14
+    char    buf[64*2 + 30];
+#endif
+
+    last_ev = current_ev;
+
+/* First entry *MUST* be the AP MAC address */
+    iwe.cmd = SIOCGIWAP;
+    iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
+    memcpy(iwe.u.ap_addr.sa_data, list->bssid, ETH_ALEN);
+    current_ev = iwe_stream_add_event(
+	info,
+	current_ev,
+	end_buf, &iwe, IW_EV_ADDR_LEN);
+
+    /* Ran out of buffer */
+    if (last_ev == current_ev)
+    {
+        return end_buf;
+    }
+
+    last_ev = current_ev;
+
+/* Other entries will be displayed in the order we give them */
+
+/* Add the ESSID */
+    iwe.u.data.length = list->ssid[1];
+    if(iwe.u.data.length > 32)
+        iwe.u.data.length = 32;
+    iwe.cmd = SIOCGIWESSID;
+    iwe.u.data.flags = 1;
+    current_ev = iwe_stream_add_point(
+	info,
+	current_ev, end_buf, &iwe, &list->ssid[2]);
+
+    /* Ran out of buffer */
+    if (last_ev == current_ev)
+    {
+        return end_buf;
+    }
+
+    last_ev = current_ev;
+
+/* Add mode */
+    iwe.cmd = SIOCGIWMODE;
+    capabilities = (list->capability[1] << 8) + list->capability[0];
+    if(capabilities & (0x01 | 0x02))
+    {
+        if(capabilities & 0x01)
+            iwe.u.mode = IW_MODE_MASTER;
+        else
+            iwe.u.mode = IW_MODE_ADHOC;
+        current_ev = iwe_stream_add_event(
+		info,
+		current_ev, end_buf, &iwe, IW_EV_UINT_LEN);
+    }
+
+    /* Ran out of buffer */
+    if (last_ev == current_ev)
+    {
+        return end_buf;
+    }
+
+    last_ev = current_ev;
+
+/* Add frequency */
+    iwe.cmd = SIOCGIWFREQ;
+    iwe.u.freq.m = list->channel;
+/* Channel frequency in KHz */
+    if (iwe.u.freq.m > 14)
+    {
+        if ((184 <= iwe.u.freq.m) && (iwe.u.freq.m<=196))
+              iwe.u.freq.m = 4000 + iwe.u.freq.m * 5;
+        else
+              iwe.u.freq.m = 5000 + iwe.u.freq.m * 5;
+    }
+    else
+    {
+        if (iwe.u.freq.m == 14)
+              iwe.u.freq.m = 2484;
+        else
+              iwe.u.freq.m = 2412 + (iwe.u.freq.m - 1) * 5;
+    }
+    iwe.u.freq.e = 6;
+    current_ev = iwe_stream_add_event(
+	info,
+    	current_ev, end_buf, &iwe, IW_EV_FREQ_LEN);
+
+    /* Ran out of buffer */
+    if (last_ev == current_ev)
+    {
+        return end_buf;
+    }
+
+    last_ev = current_ev;
+
+/* Add quality statistics */
+    iwe.cmd = IWEVQUAL;
+#if WIRELESS_EXT > 18
+    iwe.u.qual.updated = IW_QUAL_QUAL_UPDATED | IW_QUAL_LEVEL_UPDATED
+                        |IW_QUAL_NOISE_UPDATED;
+#endif
+    iwe.u.qual.level = list->signalStrength;
+    iwe.u.qual.noise = 0;
+    iwe.u.qual.qual = list->signalQuality;
+    current_ev = iwe_stream_add_event(
+	info,
+	current_ev, end_buf, &iwe, IW_EV_QUAL_LEN);
+
+    /* Ran out of buffer */
+    if (last_ev == current_ev)
+    {
+        return end_buf;
+    }
+
+    last_ev = current_ev;
+
+/* Add encryption capability */
+
+    iwe.cmd = SIOCGIWENCODE;
+    if(capabilities & 0x10)
+        iwe.u.data.flags = IW_ENCODE_ENABLED | IW_ENCODE_NOKEY;
+    else
+        iwe.u.data.flags = IW_ENCODE_DISABLED;
+
+    iwe.u.data.length = 0;
+    current_ev = iwe_stream_add_point(
+	info,
+	current_ev, end_buf, &iwe, list->ssid);
+
+    /* Ran out of buffer */
+    if (last_ev == current_ev)
+    {
+        return end_buf;
+    }
+
+    last_ev = current_ev;
+
+/* Rate : stuffing multiple values in a single event require a bit
+ * more of magic */
+    current_val = current_ev + IW_EV_LCP_LEN;
+
+    iwe.cmd = SIOCGIWRATE;
+/* Those two flags are ignored... */
+    iwe.u.bitrate.fixed = iwe.u.bitrate.disabled = 0;
+
+    for(i = 0 ; i < list->supportedRates[1] ; i++)
+    {
+/* Bit rate given in 500 kb/s units (+ 0x80) */
+        iwe.u.bitrate.value = ((list->supportedRates[i+2] & 0x7f) * 500000);
+/* Add new value to event */
+        current_val = iwe_stream_add_value(
+		info,
+		current_ev, current_val, end_buf, &iwe, IW_EV_PARAM_LEN);
+
+        /* Ran out of buffer */
+        if (last_ev == current_val)
+        {
+            return end_buf;
+        }
+
+        last_ev = current_val;
+    }
+
+    for (i = 0 ; i < list->extSupportedRates[1] ; i++)
+    {
+/* Bit rate given in 500 kb/s units (+ 0x80) */
+        iwe.u.bitrate.value = ((list->extSupportedRates[i+2] & 0x7f) * 500000);
+/* Add new value to event */
+        current_val = iwe_stream_add_value(
+		info,
+		current_ev, current_val, end_buf, &iwe, IW_EV_PARAM_LEN);
+
+        /* Ran out of buffer */
+        if (last_ev == current_val)
+        {
+            return end_buf;
+        }
+
+        last_ev = current_ev;
+    }
+
+/* Check if we added any event */
+    if((current_val - current_ev) > IW_EV_LCP_LEN)
+        current_ev = current_val;
+#if WIRELESS_EXT > 14
+#define IEEE80211_ELEMID_RSN 0x30
+    memset(&iwe, 0, sizeof(iwe));
+    iwe.cmd = IWEVCUSTOM;
+    snprintf(buf, sizeof(buf), "bcn_int=%d", (list->beaconInterval[1] << 8) + list->beaconInterval[0]);
+    iwe.u.data.length = strlen(buf);
+    current_ev = iwe_stream_add_point(
+		info,
+		current_ev, end_buf, &iwe, buf);
+
+    /* Ran out of buffer */
+    if (last_ev == current_ev)
+    {
+        return end_buf;
+    }
+
+    last_ev = current_ev;
+
+    if (list->wpaIe[1] != 0)
+    {
+        static const char rsn_leader[] = "rsn_ie=";
+        static const char wpa_leader[] = "wpa_ie=";
+
+        memset(&iwe, 0, sizeof(iwe));
+        iwe.cmd = IWEVCUSTOM;
+        if (list->wpaIe[0] == IEEE80211_ELEMID_RSN)
+            iwe.u.data.length = encode_ie(buf, sizeof(buf),
+                list->wpaIe, list->wpaIe[1]+2,
+                rsn_leader, sizeof(rsn_leader)-1);
+        else
+            iwe.u.data.length = encode_ie(buf, sizeof(buf),
+                list->wpaIe, list->wpaIe[1]+2,
+                wpa_leader, sizeof(wpa_leader)-1);
+
+        if (iwe.u.data.length != 0)
+            current_ev = iwe_stream_add_point(
+		info,
+		current_ev, end_buf, &iwe, buf);
+
+        /* Ran out of buffer */
+        if (last_ev == current_ev)
+        {
+            return end_buf;
+        }
+
+        last_ev = current_ev;
+    }
+    if (list->rsnIe[1] != 0)
+    {
+        static const char rsn_leader[] = "rsn_ie=";
+        memset(&iwe, 0, sizeof(iwe));
+        iwe.cmd = IWEVCUSTOM;
+
+        if (list->rsnIe[0] == IEEE80211_ELEMID_RSN)
+        {
+            iwe.u.data.length = encode_ie(buf, sizeof(buf),
+                list->rsnIe, list->rsnIe[1]+2,
+                rsn_leader, sizeof(rsn_leader)-1);
+            if (iwe.u.data.length != 0)
+                current_ev = iwe_stream_add_point(
+			info,
+			current_ev, end_buf,  &iwe, buf);
+
+            /* Ran out of buffer */
+            if (last_ev == current_ev)
+            {
+                return end_buf;
+            }
+
+            last_ev = current_ev;
+        }
+    }
+#endif
+/* The other data in the scan result are not really
+ * interesting, so for now drop it */
+    return current_ev;
+}
+
+int usbdrvwext_giwname(struct net_device *dev,
+            struct iw_request_info *info,
+            union iwreq_data *wrq, char *extra)
+{
+    //struct usbdrv_private *macp = dev->ml_priv;
+
+    strcpy(wrq->name, "IEEE 802.11-MIMO");
+
+    return 0;
+}
+
+int usbdrvwext_siwfreq(struct net_device *dev,
+            struct iw_request_info *info,
+            struct iw_freq *freq, char *extra)
+{
+    u32_t FreqKHz;
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    if(!netif_running(dev))
+        return -EINVAL;
+
+    if (freq->e > 1)
+        return -EINVAL;
+
+    if (freq->e == 1)
+    {
+        FreqKHz = (freq->m / 100000);
+
+        if (FreqKHz > 4000000)
+        {
+            if (FreqKHz > 5825000)
+                FreqKHz = 5825000;
+            else if (FreqKHz < 4920000)
+                FreqKHz = 4920000;
+            else if (FreqKHz < 5000000)
+                FreqKHz = (((FreqKHz - 4000000) / 5000) * 5000) + 4000000;
+            else
+                FreqKHz = (((FreqKHz - 5000000) / 5000) * 5000) + 5000000;
+        }
+        else
+        {
+            if (FreqKHz > 2484000)
+                FreqKHz = 2484000;
+            else if (FreqKHz < 2412000)
+                FreqKHz = 2412000;
+            else
+                FreqKHz = (((FreqKHz - 2412000) / 5000) * 5000) + 2412000;
+        }
+
+    }
+    else
+    {
+        FreqKHz = usbdrv_chan2freq(freq->m);
+
+        if (FreqKHz != -1)
+            FreqKHz *= 1000;
+        else
+            FreqKHz = 2412000;
+    }
+
+    //printk("freq->m: %d, freq->e: %d\n", freq->m, freq->e);
+    //printk("FreqKHz: %d\n", FreqKHz);
+
+    if (macp->DeviceOpened == 1)
+    {
+        zfiWlanSetFrequency(dev, FreqKHz, 0); // Immediate
+        //u8_t wpaieLen,wpaie[50];
+        //zfiWlanQueryWpaIe(dev, wpaie, &wpaieLen);
+        zfiWlanDisable(dev, 0);
+        zfiWlanEnable(dev);
+        //if (wpaieLen > 2)
+        //    zfiWlanSetWpaIe(dev, wpaie, wpaieLen);
+    }
+
+    return 0;
+}
+
+int usbdrvwext_giwfreq(struct net_device *dev,
+            struct iw_request_info *info,
+            struct iw_freq *freq, char *extra)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    if (macp->DeviceOpened != 1)
+        return 0;
+
+    freq->m = zfiWlanQueryFrequency(dev);
+    freq->e = 3;
+
+    return 0;
+}
+
+int usbdrvwext_siwmode(struct net_device *dev,
+            struct iw_request_info *info,
+            union iwreq_data *wrq, char *extra)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    u8_t WlanMode;
+
+    if(!netif_running(dev))
+        return -EINVAL;
+
+    if (macp->DeviceOpened != 1)
+        return 0;
+
+    switch(wrq->mode)
+    {
+        case IW_MODE_MASTER:
+            WlanMode = ZM_MODE_AP;
+            break;
+        case IW_MODE_INFRA:
+            WlanMode = ZM_MODE_INFRASTRUCTURE;
+            break;
+        case IW_MODE_ADHOC:
+            WlanMode = ZM_MODE_IBSS;
+            break;
+        default:
+            WlanMode = ZM_MODE_IBSS;
+            break;
+    }
+
+    zfiWlanSetWlanMode(dev,WlanMode);
+    zfiWlanDisable(dev, 1);
+    zfiWlanEnable(dev);
+
+    return 0;
+}
+
+int usbdrvwext_giwmode(struct net_device *dev,
+            struct iw_request_info *info,
+            __u32 *mode, char *extra)
+{
+    unsigned long irqFlag;
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    if(!netif_running(dev))
+        return -EINVAL;
+
+    if (macp->DeviceOpened != 1)
+        return 0;
+
+    spin_lock_irqsave(&macp->cs_lock, irqFlag);
+
+    switch(zfiWlanQueryWlanMode(dev))
+    {
+        case ZM_MODE_AP:
+            *mode = IW_MODE_MASTER;
+            break;
+        case ZM_MODE_INFRASTRUCTURE:
+            *mode = IW_MODE_INFRA;
+            break;
+        case ZM_MODE_IBSS:
+            *mode = IW_MODE_ADHOC;
+            break;
+        default:
+            *mode = IW_MODE_ADHOC;
+            break;
+    }
+
+    spin_unlock_irqrestore(&macp->cs_lock, irqFlag);
+
+    return 0;
+}
+
+int usbdrvwext_siwsens(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_param *sens, char *extra)
+{
+	return 0;
+}
+
+int usbdrvwext_giwsens(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_param *sens, char *extra)
+{
+	sens->value = 0;
+	sens->fixed = 1;
+
+	return 0;
+}
+
+int usbdrvwext_giwrange(struct net_device *dev,
+            struct iw_request_info *info,
+            struct iw_point *data, char *extra)
+{
+    struct iw_range *range = (struct iw_range *) extra;
+    int i, val;
+    //int num_band_a;
+    u16_t channels[60];
+    u16_t channel_num;
+
+    if(!netif_running(dev))
+        return -EINVAL;
+
+#if WIRELESS_EXT > 9
+    range->txpower_capa = IW_TXPOW_DBM;
+// XXX what about min/max_pmp, min/max_pmt, etc.
+#endif
+
+#if WIRELESS_EXT > 10
+    range->we_version_compiled = WIRELESS_EXT;
+    range->we_version_source = 13;
+
+    range->retry_capa = IW_RETRY_LIMIT;
+    range->retry_flags = IW_RETRY_LIMIT;
+    range->min_retry = 0;
+    range->max_retry = 255;
+#endif                                        /* WIRELESS_EXT > 10 */
+
+    channel_num = zfiWlanQueryAllowChannels(dev, channels);
+
+    /* Gurantee reported channel numbers is less or equal to IW_MAX_FREQUENCIES */
+    if (channel_num > IW_MAX_FREQUENCIES)
+        channel_num = IW_MAX_FREQUENCIES;
+
+    val = 0;
+
+    for (i = 0; i < channel_num; i++)
+    {
+        range->freq[val].i = usbdrv_freq2chan(channels[i]);
+        range->freq[val].m = channels[i];
+        range->freq[val].e = 6;
+        val++;
+    }
+
+    range->num_channels = channel_num;
+    range->num_frequency = channel_num;
+
+#if 0
+    range->num_channels = 14; // Only 2.4G
+
+/* XXX need to filter against the regulatory domain &| active set */
+    val = 0;
+    for (i = 1; i <= 14; i++) // B,G Bands
+    {
+        range->freq[val].i = i;
+        if (i == 14)
+              range->freq[val].m = 2484000;
+        else
+              range->freq[val].m = (2412+(i-1)*5)*1000;
+        range->freq[val].e = 3;
+        val++;
+    }
+
+    num_band_a = (IW_MAX_FREQUENCIES - val);
+
+    for (i = 0; i < num_band_a; i++) // A Bands
+    {
+        range->freq[val].i = channel_frequency_11A[2 * i];
+        range->freq[val].m = channel_frequency_11A[2 * i + 1] * 1000;
+        range->freq[val].e = 3;
+        val++;
+    }
+    // MIMO Rate Not Defined Now
+    //For 802.11a, there are too more frequency. We can't return them all
+    range->num_frequency = val;
+#endif
+
+/* Max of /proc/net/wireless */
+    range->max_qual.qual = 100; //??                  //92;
+    range->max_qual.level = 154; //??
+    range->max_qual.noise = 154; //??
+    range->sensitivity = 3; //??
+
+// XXX these need to be nsd-specific!
+    range->min_rts = 0;
+    range->max_rts = 2347;
+    range->min_frag = 256;
+    range->max_frag = 2346;
+    range->max_encoding_tokens = 4/*NUM_WEPKEYS*/; //??
+    range->num_encoding_sizes = 2; //??
+
+    range->encoding_size[0] = 5; //??               //WEP Key Encoding Size
+    range->encoding_size[1] = 13;//??
+
+// XXX what about num_bitrates/throughput?
+    range->num_bitrates = 0; //??
+
+/* estimated max throughput */
+// XXX need to cap it if we're running at ~2Mbps..
+
+    range->throughput = 300000000;
+
+    return 0;
+}
+
+int usbdrvwext_siwap(struct net_device *dev, struct iw_request_info *info,
+        struct sockaddr *MacAddr, char *extra)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    if(!netif_running(dev))
+        return -EINVAL;
+
+    if (zfiWlanQueryWlanMode(dev) == ZM_MODE_AP) // AP Mode
+        zfiWlanSetMacAddress(dev,(u16_t *)&MacAddr->sa_data[0]);
+    else                                 //STA Mode
+        zfiWlanSetBssid(dev,&MacAddr->sa_data[0]);
+
+    if (macp->DeviceOpened == 1)
+    {
+        //u8_t wpaieLen,wpaie[80];
+        //zfiWlanQueryWpaIe(dev, wpaie, &wpaieLen);
+        zfiWlanDisable(dev, 0);
+        zfiWlanEnable(dev);
+        //if (wpaieLen > 2)
+        //    zfiWlanSetWpaIe(dev, wpaie, wpaieLen);
+    }
+
+    return 0;
+}
+
+int usbdrvwext_giwap(struct net_device *dev,
+            struct iw_request_info *info,
+            struct sockaddr *MacAddr, char *extra)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    if (macp->DeviceOpened != 1)
+        return 0;
+
+    if (zfiWlanQueryWlanMode(dev) == ZM_MODE_AP) // AP Mode
+        zfiWlanQueryMacAddress(dev, &MacAddr->sa_data[0]);
+    else                                 //STA Mode
+    {
+        if (macp->adapterState == ZM_STATUS_MEDIA_CONNECT)
+        {
+            zfiWlanQueryBssid(dev, &MacAddr->sa_data[0]);
+        }
+        else
+        {
+            u8_t zero_addr[6] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
+            memcpy(&MacAddr->sa_data[0], zero_addr, sizeof(zero_addr));
+        }
+    }
+
+    return 0;
+}
+
+int usbdrvwext_iwaplist(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_point *data, char *extra)
+{
+    //Don't know how to do yet--CWYang(+)
+    return 0;
+
+}
+
+int usbdrvwext_siwscan(struct net_device *dev, struct iw_request_info *info,
+        struct iw_point *data, char *extra)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    if (macp->DeviceOpened != 1)
+        return 0;
+
+    printk("CWY - usbdrvwext_siwscan\n");
+
+    zfiWlanScan(dev);
+
+    return 0;
+}
+
+int usbdrvwext_giwscan(struct net_device *dev,
+            struct iw_request_info *info,
+            struct iw_point *data, char *extra)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    //struct zsWlanDev* wd = (struct zsWlanDev*) zmw_wlan_dev(dev);
+    char *current_ev = extra;
+    char *end_buf;
+    int i;
+    //struct zsBssList BssList;
+    struct zsBssListV1 *pBssList = kmalloc(sizeof(struct zsBssListV1), GFP_KERNEL);
+    //BssList = wd->sta.pBssList;
+    //zmw_get_wlan_dev(dev);
+
+    if (macp->DeviceOpened != 1)
+        return 0;
+
+    if (data->length == 0)
+    {
+       end_buf = extra + IW_SCAN_MAX_DATA;
+    }
+    else
+    {
+        end_buf = extra + data->length;
+    }
+
+    printk("giwscan - Report Scan Results\n");
+    //printk("giwscan - BssList Sreucture Len : %d\n", sizeof(BssList));
+    //printk("giwscan - BssList Count : %d\n", wd->sta.pBssList->bssCount);
+    //printk("giwscan - UpdateBssList Count : %d\n", wd->sta.pUpdateBssList->bssCount);
+    zfiWlanQueryBssListV1(dev, pBssList);
+    //zfiWlanQueryBssList(dev, &BssList);
+
+/* Read and parse all entries */
+    printk("giwscan - pBssList->bssCount : %d\n", pBssList->bssCount);
+    //printk("giwscan - BssList.bssCount : %d\n", BssList.bssCount);
+
+    for (i = 0; i < pBssList->bssCount; i++)
+    {
+/* Translate to WE format this entry */
+        //current_ev = usbdrv_translate_scan(dev, info, current_ev,
+        //    extra + IW_SCAN_MAX_DATA, &pBssList->bssInfo[i]);
+        current_ev = usbdrv_translate_scan(dev, info, current_ev,
+            end_buf, &pBssList->bssInfo[i]);
+
+#if WIRELESS_EXT > 16
+        if (current_ev == end_buf)
+        {
+            kfree(pBssList);
+            data->length = current_ev - extra;
+            return -E2BIG;
+        }
+#endif
+    }
+
+/* Length of data */
+    data->length = (current_ev - extra);
+    data->flags = 0;                              /* todo */
+
+    kfree(pBssList);
+
+    return 0;
+}
+
+int usbdrvwext_siwessid(struct net_device *dev,
+            struct iw_request_info *info,
+            struct iw_point *essid, char *extra)
+{
+    char EssidBuf[IW_ESSID_MAX_SIZE+1];
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    if(!netif_running(dev))
+        return -EINVAL;
+
+    if (essid->flags == 1)
+    {
+        if (essid->length > (IW_ESSID_MAX_SIZE+1))
+            return -E2BIG;
+
+        if (copy_from_user(&EssidBuf, essid->pointer, essid->length))
+            return -EFAULT;
+
+        EssidBuf[essid->length] = '\0';
+        //printk("siwessid - Set Essid : %s\n",EssidBuf);
+        //printk("siwessid - Essid Len : %d\n",essid->length);
+        //printk("siwessid - Essid Flag : %x\n",essid->flags);
+        if (macp->DeviceOpened == 1)
+        {
+            zfiWlanSetSSID(dev, EssidBuf, strlen(EssidBuf));
+            zfiWlanSetFrequency(dev, zfiWlanQueryFrequency(dev), FALSE);
+            zfiWlanSetEncryMode(dev, zfiWlanQueryEncryMode(dev));
+            //u8_t wpaieLen,wpaie[50];
+            //zfiWlanQueryWpaIe(dev, wpaie, &wpaieLen);
+            zfiWlanDisable(dev, 0);
+            zfiWlanEnable(dev);
+            //if (wpaieLen > 2)
+            //    zfiWlanSetWpaIe(dev, wpaie, wpaieLen);
+        }
+    }
+
+    return 0;
+}
+
+int usbdrvwext_giwessid(struct net_device *dev,
+            struct iw_request_info *info,
+            struct iw_point *essid, char *extra)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    u8_t EssidLen;
+    char EssidBuf[IW_ESSID_MAX_SIZE+1];
+    int ssid_len;
+
+    if(!netif_running(dev))
+        return -EINVAL;
+
+    if (macp->DeviceOpened != 1)
+        return 0;
+
+    zfiWlanQuerySSID(dev, &EssidBuf[0], &EssidLen);
+
+    /* Convert type from unsigned char to char */
+    ssid_len = (int)EssidLen;
+
+    /* Make sure the essid length is not greater than IW_ESSID_MAX_SIZE */
+    if (ssid_len > IW_ESSID_MAX_SIZE)
+        ssid_len = IW_ESSID_MAX_SIZE;
+
+    EssidBuf[ssid_len] = '\0';
+
+    essid->flags = 1;
+    essid->length = strlen(EssidBuf);
+
+    memcpy(extra, EssidBuf, essid->length);
+    // wireless.c in Kernel would handle copy_to_user -- line 679
+    /*if (essid->pointer)
+    {
+        if ( copy_to_user(essid->pointer, EssidBuf, essid->length) )
+        {
+            printk("giwessid - copy_to_user Fail\n");
+            return -EFAULT;
+        }
+    }*/
+
+    return 0;
+}
+
+int usbdrvwext_siwnickn(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_point *data, char *nickname)
+{
+    //Exist but junk--CWYang(+)
+	return 0;
+}
+
+int usbdrvwext_giwnickn(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_point *data, char *nickname)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    u8_t EssidLen;
+    char EssidBuf[IW_ESSID_MAX_SIZE+1];
+
+    if (macp->DeviceOpened != 1)
+        return 0;
+
+    zfiWlanQuerySSID(dev, &EssidBuf[0], &EssidLen);
+    EssidBuf[EssidLen] = 0;
+
+    data->flags = 1;
+    data->length = strlen(EssidBuf);
+
+    memcpy(nickname, EssidBuf, data->length);
+
+	return 0;
+}
+
+int usbdrvwext_siwrate(struct net_device *dev,
+            struct iw_request_info *info,
+            struct iw_param *frq, char *extra)
+{
+	struct usbdrv_private *macp = dev->ml_priv;
+    //Array to Define Rate Number that Send to Driver
+    u16_t zcIndextoRateBG[16] = {1000, 2000, 5500, 11000, 0, 0, 0, 0, 48000,
+                               24000, 12000, 6000, 54000, 36000, 18000, 9000};
+    u16_t zcRateToMCS[] = {0xff, 0, 1, 2, 3, 0xb, 0xf, 0xa, 0xe, 0x9, 0xd,
+                           0x8, 0xc};
+    u8_t i,RateIndex = 4;
+    u16_t RateKbps;
+
+    //printk("frq->disabled : 0x%x\n",frq->disabled);
+    //printk("frq->value : 0x%x\n",frq->value);
+
+    RateKbps = frq->value / 1000;
+    //printk("RateKbps : %d\n", RateKbps);
+    for (i = 0; i < 16; i++)
+    {
+        if (RateKbps == zcIndextoRateBG[i])
+            RateIndex = i;
+    }
+    if (zcIndextoRateBG[RateIndex] == 0)
+        RateIndex = 0xff;
+    //printk("RateIndex : %x\n", RateIndex);
+    for (i = 0; i < 13; i++)
+        if (RateIndex == zcRateToMCS[i])
+            break;
+    //printk("Index : %x\n", i);
+    if (RateKbps == 65000)
+    {
+        RateIndex = 20;
+        printk("RateIndex : %d\n", RateIndex);
+    }
+    if (macp->DeviceOpened == 1)
+    {
+        zfiWlanSetTxRate(dev, i);
+        //zfiWlanDisable(dev);
+        //zfiWlanEnable(dev);
+    }
+
+    return 0;
+}
+
+int usbdrvwext_giwrate(struct net_device *dev,
+            struct iw_request_info *info,
+            struct iw_param *frq, char *extra)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    if(!netif_running(dev))
+        return -EINVAL;
+
+    if (macp->DeviceOpened != 1)
+        return 0;
+
+    frq->fixed = 0;
+    frq->disabled = 0;
+    frq->value = zfiWlanQueryRxRate(dev) * 1000;
+
+    return 0;
+}
+
+int usbdrvwext_siwrts(struct net_device *dev,
+            struct iw_request_info *info,
+            struct iw_param *rts, char *extra)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    int val = rts->value;
+
+    if (macp->DeviceOpened != 1)
+        return 0;
+
+    if (rts->disabled)
+        val = 2347;
+
+    if ((val < 0) || (val > 2347))
+        return -EINVAL;
+
+    zfiWlanSetRtsThreshold(dev,val);
+
+    return 0;
+}
+
+int usbdrvwext_giwrts(struct net_device *dev,
+            struct iw_request_info *info,
+            struct iw_param *rts, char *extra)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    if(!netif_running(dev))
+        return -EINVAL;
+
+    if (macp->DeviceOpened != 1)
+        return 0;
+
+    rts->value = zfiWlanQueryRtsThreshold(dev);
+    rts->disabled = (rts->value >= 2347);
+    rts->fixed = 1;
+
+    return 0;
+
+}
+
+int usbdrvwext_siwfrag(struct net_device *dev,
+            struct iw_request_info *info,
+            struct iw_param *frag, char *extra)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    u16_t fragThreshold;
+
+    if (macp->DeviceOpened != 1)
+        return 0;
+
+    if (frag->disabled)
+        fragThreshold = 0;
+    else
+        fragThreshold = frag->value;
+
+    zfiWlanSetFragThreshold(dev,fragThreshold);
+
+    return 0;
+}
+
+int usbdrvwext_giwfrag(struct net_device *dev,
+            struct iw_request_info *info,
+            struct iw_param *frag, char *extra)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    u16 val;
+    unsigned long irqFlag;
+
+    if(!netif_running(dev))
+        return -EINVAL;
+
+    if (macp->DeviceOpened != 1)
+        return 0;
+
+    spin_lock_irqsave(&macp->cs_lock, irqFlag);
+
+    val = zfiWlanQueryFragThreshold(dev);
+
+    frag->value = val;
+
+    frag->disabled = (val >= 2346);
+    frag->fixed = 1;
+
+    spin_unlock_irqrestore(&macp->cs_lock, irqFlag);
+
+    return 0;
+}
+
+int usbdrvwext_siwtxpow(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_param *rrq, char *extra)
+{
+    //Not support yet--CWYng(+)
+	return 0;
+}
+
+int usbdrvwext_giwtxpow(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_param *rrq, char *extra)
+{
+    //Not support yet--CWYng(+)
+	return 0;
+}
+
+int usbdrvwext_siwretry(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_param *rrq, char *extra)
+{
+    //Do nothing--CWYang(+)
+	return 0;
+}
+
+int usbdrvwext_giwretry(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_param *rrq, char *extra)
+{
+    //Do nothing--CWYang(+)
+	return 0;
+}
+
+int usbdrvwext_siwencode(struct net_device *dev,
+            struct iw_request_info *info,
+            struct iw_point *erq, char *key)
+{
+    struct zsKeyInfo keyInfo;
+    int i, WepState = ZM_ENCRYPTION_WEP_DISABLED;
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    if(!netif_running(dev))
+        return -EINVAL;
+
+    if ((erq->flags & IW_ENCODE_DISABLED) == 0)
+    {
+        keyInfo.key = key;
+        keyInfo.keyLength = erq->length;
+        keyInfo.keyIndex = (erq->flags & IW_ENCODE_INDEX) - 1;
+        if (keyInfo.keyIndex >= 4)
+            keyInfo.keyIndex = 0;
+        keyInfo.flag = ZM_KEY_FLAG_DEFAULT_KEY;
+
+        zfiWlanSetKey(dev, keyInfo);
+        WepState = ZM_ENCRYPTION_WEP_ENABLED;
+    }
+    else
+    {
+        for (i = 1; i < 4; i++)
+            zfiWlanRemoveKey(dev, 0, i);
+        WepState = ZM_ENCRYPTION_WEP_DISABLED;
+        //zfiWlanSetEncryMode(dev, ZM_NO_WEP);
+    }
+
+    if (macp->DeviceOpened == 1)
+    {
+        zfiWlanSetWepStatus(dev, WepState);
+        zfiWlanSetFrequency(dev, zfiWlanQueryFrequency(dev), FALSE);
+        //zfiWlanSetEncryMode(dev, zfiWlanQueryEncryMode(dev));
+        //u8_t wpaieLen,wpaie[50];
+        //zfiWlanQueryWpaIe(dev, wpaie, &wpaieLen);
+        zfiWlanDisable(dev, 0);
+        zfiWlanEnable(dev);
+        //if (wpaieLen > 2)
+        //    zfiWlanSetWpaIe(dev, wpaie, wpaieLen);
+    }
+
+    return 0;
+}
+
+int usbdrvwext_giwencode(struct net_device *dev,
+            struct iw_request_info *info,
+            struct iw_point *erq, char *key)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    u8_t EncryptionMode;
+    u8_t keyLen = 0;
+
+    if (macp->DeviceOpened != 1)
+        return 0;
+
+    EncryptionMode = zfiWlanQueryEncryMode(dev);
+
+    if (EncryptionMode)
+    {
+        erq->flags = IW_ENCODE_ENABLED;
+    }
+    else
+    {
+        erq->flags = IW_ENCODE_DISABLED;
+    }
+
+/* We can't return the key, so set the proper flag and return zero */
+    erq->flags |= IW_ENCODE_NOKEY;
+    memset(key, 0, 16);
+
+/* Copy the key to the user buffer */
+    switch(EncryptionMode)
+    {
+        case ZM_WEP64:
+            keyLen = 5;
+            break;
+        case ZM_WEP128:
+            keyLen = 13;
+            break;
+        case ZM_WEP256:
+            keyLen = 29;
+            break;
+        case ZM_AES:
+            keyLen = 16;
+            break;
+        case ZM_TKIP:
+            keyLen = 32;
+            break;
+#ifdef ZM_ENABLE_CENC
+        case ZM_CENC:
+            keyLen = 32;
+            break;
+#endif //ZM_ENABLE_CENC
+        case ZM_NO_WEP:
+            keyLen = 0;
+            break;
+        default :
+            keyLen = 0;
+            printk("Unknown EncryMode\n");
+            break;
+
+    }
+    erq->length = keyLen;
+
+    return 0;
+}
+
+int usbdrvwext_siwpower(struct net_device *dev,
+            struct iw_request_info *info,
+            struct iw_param *frq, char *extra)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    u8_t PSMode;
+
+    if (macp->DeviceOpened != 1)
+        return 0;
+
+    if (frq->disabled)
+        PSMode = ZM_STA_PS_NONE;
+    else
+        PSMode = ZM_STA_PS_MAX;
+
+    zfiWlanSetPowerSaveMode(dev,PSMode);
+
+    return 0;
+}
+
+int usbdrvwext_giwpower(struct net_device *dev,
+            struct iw_request_info *info,
+            struct iw_param *frq, char *extra)
+{
+    unsigned long irqFlag;
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    if (macp->DeviceOpened != 1)
+        return 0;
+
+    spin_lock_irqsave(&macp->cs_lock, irqFlag);
+
+    if (zfiWlanQueryPowerSaveMode(dev) == ZM_STA_PS_NONE)
+        frq->disabled = 1;
+    else
+        frq->disabled = 0;
+
+    spin_unlock_irqrestore(&macp->cs_lock, irqFlag);
+
+    return 0;
+}
+
+//int usbdrvwext_setparam(struct net_device *dev, struct iw_request_info *info,
+//		   	 void *w, char *extra)
+//{
+//	struct ieee80211vap *vap = dev->ml_priv;
+//	struct ieee80211com *ic = vap->iv_ic;
+//	struct ieee80211_rsnparms *rsn = &vap->iv_bss->ni_rsn;
+//	int *i = (int *) extra;
+//	int param = i[0];		/* parameter id is 1st */
+//	int value = i[1];		/* NB: most values are TYPE_INT */
+//	int retv = 0;
+//	int j, caps;
+//	const struct ieee80211_authenticator *auth;
+//	const struct ieee80211_aclator *acl;
+//
+//	switch (param) {
+//	case IEEE80211_PARAM_AUTHMODE:
+//		switch (value) {
+//		case IEEE80211_AUTH_WPA:	/* WPA */
+//		case IEEE80211_AUTH_8021X:	/* 802.1x */
+//		case IEEE80211_AUTH_OPEN:	/* open */
+//		case IEEE80211_AUTH_SHARED:	/* shared-key */
+//		case IEEE80211_AUTH_AUTO:	/* auto */
+//			auth = ieee80211_authenticator_get(value);
+//			if (auth == NULL)
+//				return -EINVAL;
+//			break;
+//		default:
+//			return -EINVAL;
+//		}
+//		switch (value) {
+//		case IEEE80211_AUTH_WPA:	/* WPA w/ 802.1x */
+//			vap->iv_flags |= IEEE80211_F_PRIVACY;
+//			value = IEEE80211_AUTH_8021X;
+//			break;
+//		case IEEE80211_AUTH_OPEN:	/* open */
+//			vap->iv_flags &= ~(IEEE80211_F_WPA|IEEE80211_F_PRIVACY);
+//			break;
+//		case IEEE80211_AUTH_SHARED:	/* shared-key */
+//		case IEEE80211_AUTH_AUTO:	/* auto */
+//		case IEEE80211_AUTH_8021X:	/* 802.1x */
+//			vap->iv_flags &= ~IEEE80211_F_WPA;
+//			/* both require a key so mark the PRIVACY capability */
+//			vap->iv_flags |= IEEE80211_F_PRIVACY;
+//			break;
+//		}
+//		/* NB: authenticator attach/detach happens on state change */
+//		vap->iv_bss->ni_authmode = value;
+//		/* XXX mixed/mode/usage? */
+//		vap->iv_auth = auth;
+//		retv = ENETRESET;
+//		break;
+//	case IEEE80211_PARAM_PROTMODE:
+//		if (value > IEEE80211_PROT_RTSCTS)
+//			return -EINVAL;
+//		ic->ic_protmode = value;
+//		/* NB: if not operating in 11g this can wait */
+//		if (ic->ic_bsschan != IEEE80211_CHAN_ANYC &&
+//		    IEEE80211_IS_CHAN_ANYG(ic->ic_bsschan))
+//			retv = ENETRESET;
+//		break;
+//	case IEEE80211_PARAM_MCASTCIPHER:
+//		if ((vap->iv_caps & cipher2cap(value)) == 0 &&
+//		    !ieee80211_crypto_available(value))
+//			return -EINVAL;
+//		rsn->rsn_mcastcipher = value;
+//		if (vap->iv_flags & IEEE80211_F_WPA)
+//			retv = ENETRESET;
+//		break;
+//	case IEEE80211_PARAM_MCASTKEYLEN:
+//		if (!(0 < value && value < IEEE80211_KEYBUF_SIZE))
+//			return -EINVAL;
+//		/* XXX no way to verify driver capability */
+//		rsn->rsn_mcastkeylen = value;
+//		if (vap->iv_flags & IEEE80211_F_WPA)
+//			retv = ENETRESET;
+//		break;
+//	case IEEE80211_PARAM_UCASTCIPHERS:
+//		/*
+//		 * Convert cipher set to equivalent capabilities.
+//		 * NB: this logic intentionally ignores unknown and
+//		 * unsupported ciphers so folks can specify 0xff or
+//		 * similar and get all available ciphers.
+//		 */
+//		caps = 0;
+//		for (j = 1; j < 32; j++)	/* NB: skip WEP */
+//			if ((value & (1<<j)) &&
+//			    ((vap->iv_caps & cipher2cap(j)) ||
+//			     ieee80211_crypto_available(j)))
+//				caps |= 1<<j;
+//		if (caps == 0)			/* nothing available */
+//			return -EINVAL;
+//		/* XXX verify ciphers ok for unicast use? */
+//		/* XXX disallow if running as it'll have no effect */
+//		rsn->rsn_ucastcipherset = caps;
+//		if (vap->iv_flags & IEEE80211_F_WPA)
+//			retv = ENETRESET;
+//		break;
+//	case IEEE80211_PARAM_UCASTCIPHER:
+//		if ((rsn->rsn_ucastcipherset & cipher2cap(value)) == 0)
+//			return -EINVAL;
+//		rsn->rsn_ucastcipher = value;
+//		break;
+//	case IEEE80211_PARAM_UCASTKEYLEN:
+//		if (!(0 < value && value < IEEE80211_KEYBUF_SIZE))
+//			return -EINVAL;
+//		/* XXX no way to verify driver capability */
+//		rsn->rsn_ucastkeylen = value;
+//		break;
+//	case IEEE80211_PARAM_KEYMGTALGS:
+//		/* XXX check */
+//		rsn->rsn_keymgmtset = value;
+//		if (vap->iv_flags & IEEE80211_F_WPA)
+//			retv = ENETRESET;
+//		break;
+//	case IEEE80211_PARAM_RSNCAPS:
+//		/* XXX check */
+//		rsn->rsn_caps = value;
+//		if (vap->iv_flags & IEEE80211_F_WPA)
+//			retv = ENETRESET;
+//		break;
+//	case IEEE80211_PARAM_WPA:
+//		if (value > 3)
+//			return -EINVAL;
+//		/* XXX verify ciphers available */
+//		vap->iv_flags &= ~IEEE80211_F_WPA;
+//		switch (value) {
+//		case 1:
+//			vap->iv_flags |= IEEE80211_F_WPA1;
+//			break;
+//		case 2:
+//			vap->iv_flags |= IEEE80211_F_WPA2;
+//			break;
+//		case 3:
+//			vap->iv_flags |= IEEE80211_F_WPA1 | IEEE80211_F_WPA2;
+//			break;
+//		}
+//		retv = ENETRESET;		/* XXX? */
+//		break;
+//	case IEEE80211_PARAM_ROAMING:
+//		if (!(IEEE80211_ROAMING_DEVICE <= value &&
+//		    value <= IEEE80211_ROAMING_MANUAL))
+//			return -EINVAL;
+//		ic->ic_roaming = value;
+//		break;
+//	case IEEE80211_PARAM_PRIVACY:
+//		if (value) {
+//			/* XXX check for key state? */
+//			vap->iv_flags |= IEEE80211_F_PRIVACY;
+//		} else
+//			vap->iv_flags &= ~IEEE80211_F_PRIVACY;
+//		break;
+//	case IEEE80211_PARAM_DROPUNENCRYPTED:
+//		if (value)
+//			vap->iv_flags |= IEEE80211_F_DROPUNENC;
+//		else
+//			vap->iv_flags &= ~IEEE80211_F_DROPUNENC;
+//		break;
+//	case IEEE80211_PARAM_COUNTERMEASURES:
+//		if (value) {
+//			if ((vap->iv_flags & IEEE80211_F_WPA) == 0)
+//				return -EINVAL;
+//			vap->iv_flags |= IEEE80211_F_COUNTERM;
+//		} else
+//			vap->iv_flags &= ~IEEE80211_F_COUNTERM;
+//		break;
+//	case IEEE80211_PARAM_DRIVER_CAPS:
+//		vap->iv_caps = value;		/* NB: for testing */
+//		break;
+//	case IEEE80211_PARAM_MACCMD:
+//		acl = vap->iv_acl;
+//		switch (value) {
+//		case IEEE80211_MACCMD_POLICY_OPEN:
+//		case IEEE80211_MACCMD_POLICY_ALLOW:
+//		case IEEE80211_MACCMD_POLICY_DENY:
+//			if (acl == NULL) {
+//				acl = ieee80211_aclator_get("mac");
+//				if (acl == NULL || !acl->iac_attach(vap))
+//					return -EINVAL;
+//				vap->iv_acl = acl;
+//			}
+//			acl->iac_setpolicy(vap, value);
+//			break;
+//		case IEEE80211_MACCMD_FLUSH:
+//			if (acl != NULL)
+//				acl->iac_flush(vap);
+//			/* NB: silently ignore when not in use */
+//			break;
+//		case IEEE80211_MACCMD_DETACH:
+//			if (acl != NULL) {
+//				vap->iv_acl = NULL;
+//				acl->iac_detach(vap);
+//			}
+//			break;
+//		}
+//		break;
+//	case IEEE80211_PARAM_WMM:
+//		if (ic->ic_caps & IEEE80211_C_WME){
+//			if (value) {
+//				vap->iv_flags |= IEEE80211_F_WME;
+//				vap->iv_ic->ic_flags |= IEEE80211_F_WME; /* XXX needed by ic_reset */
+//			}
+//			else {
+//				vap->iv_flags &= ~IEEE80211_F_WME;
+//				vap->iv_ic->ic_flags &= ~IEEE80211_F_WME; /* XXX needed by ic_reset */
+//			}
+//			retv = ENETRESET;	/* Renegotiate for capabilities */
+//		}
+//		break;
+//	case IEEE80211_PARAM_HIDESSID:
+//		if (value)
+//			vap->iv_flags |= IEEE80211_F_HIDESSID;
+//		else
+//			vap->iv_flags &= ~IEEE80211_F_HIDESSID;
+//		retv = ENETRESET;
+//		break;
+//	case IEEE80211_PARAM_APBRIDGE:
+//		if (value == 0)
+//			vap->iv_flags |= IEEE80211_F_NOBRIDGE;
+//		else
+//			vap->iv_flags &= ~IEEE80211_F_NOBRIDGE;
+//		break;
+//	case IEEE80211_PARAM_INACT:
+//		vap->iv_inact_run = value / IEEE80211_INACT_WAIT;
+//		break;
+//	case IEEE80211_PARAM_INACT_AUTH:
+//		vap->iv_inact_auth = value / IEEE80211_INACT_WAIT;
+//		break;
+//	case IEEE80211_PARAM_INACT_INIT:
+//		vap->iv_inact_init = value / IEEE80211_INACT_WAIT;
+//		break;
+//	case IEEE80211_PARAM_ABOLT:
+//		caps = 0;
+//		/*
+//		 * Map abolt settings to capability bits;
+//		 * this also strips unknown/unwanted bits.
+//		 */
+//		if (value & IEEE80211_ABOLT_TURBO_PRIME)
+//			caps |= IEEE80211_ATHC_TURBOP;
+//		if (value & IEEE80211_ABOLT_COMPRESSION)
+//			caps |= IEEE80211_ATHC_COMP;
+//		if (value & IEEE80211_ABOLT_FAST_FRAME)
+//			caps |= IEEE80211_ATHC_FF;
+//		if (value & IEEE80211_ABOLT_XR)
+//			caps |= IEEE80211_ATHC_XR;
+//		if (value & IEEE80211_ABOLT_AR)
+//			caps |= IEEE80211_ATHC_AR;
+//		if (value & IEEE80211_ABOLT_BURST)
+//			caps |= IEEE80211_ATHC_BURST;
+//        if (value & IEEE80211_ABOLT_WME_ELE)
+//            caps |= IEEE80211_ATHC_WME;
+//		/* verify requested capabilities are supported */
+//		if ((caps & ic->ic_ath_cap) != caps)
+//			return -EINVAL;
+//		if (vap->iv_ath_cap != caps) {
+//			if ((vap->iv_ath_cap ^ caps) & IEEE80211_ATHC_TURBOP) {
+//				if (ieee80211_set_turbo(dev,  caps & IEEE80211_ATHC_TURBOP))
+//					return -EINVAL;
+//				ieee80211_scan_flush(ic);
+//			}
+//			vap->iv_ath_cap = caps;
+//			ic->ic_athcapsetup(vap->iv_ic, vap->iv_ath_cap);
+//			retv = ENETRESET;
+//		}
+//		break;
+//	case IEEE80211_PARAM_DTIM_PERIOD:
+//		if (vap->iv_opmode != IEEE80211_M_HOSTAP &&
+//		    vap->iv_opmode != IEEE80211_M_IBSS)
+//			return -EINVAL;
+//		if (IEEE80211_DTIM_MIN <= value &&
+//		    value <= IEEE80211_DTIM_MAX) {
+//			vap->iv_dtim_period = value;
+//			retv = ENETRESET;		/* requires restart */
+//		} else
+//			retv = EINVAL;
+//		break;
+//	case IEEE80211_PARAM_BEACON_INTERVAL:
+//		if (vap->iv_opmode != IEEE80211_M_HOSTAP &&
+//		    vap->iv_opmode != IEEE80211_M_IBSS)
+//			return -EINVAL;
+//		if (IEEE80211_BINTVAL_MIN <= value &&
+//		    value <= IEEE80211_BINTVAL_MAX) {
+//			ic->ic_lintval = value;		/* XXX multi-bss */
+//			retv = ENETRESET;		/* requires restart */
+//		} else
+//			retv = EINVAL;
+//		break;
+//	case IEEE80211_PARAM_DOTH:
+//		if (value) {
+//			ic->ic_flags |= IEEE80211_F_DOTH;
+//		}
+//		else
+//			ic->ic_flags &= ~IEEE80211_F_DOTH;
+//		retv = ENETRESET;	/* XXX: need something this drastic? */
+//		break;
+//	case IEEE80211_PARAM_PWRTARGET:
+//		ic->ic_curchanmaxpwr = value;
+//		break;
+//	case IEEE80211_PARAM_GENREASSOC:
+//		IEEE80211_SEND_MGMT(vap->iv_bss, IEEE80211_FC0_SUBTYPE_REASSOC_REQ, 0);
+//		break;
+//	case IEEE80211_PARAM_COMPRESSION:
+//		retv = ieee80211_setathcap(vap, IEEE80211_ATHC_COMP, value);
+//		break;
+//    case IEEE80211_PARAM_WMM_AGGRMODE:
+//        retv = ieee80211_setathcap(vap, IEEE80211_ATHC_WME, value);
+//        break;
+//	case IEEE80211_PARAM_FF:
+//		retv = ieee80211_setathcap(vap, IEEE80211_ATHC_FF, value);
+//		break;
+//	case IEEE80211_PARAM_TURBO:
+//		retv = ieee80211_setathcap(vap, IEEE80211_ATHC_TURBOP, value);
+//		if (retv == ENETRESET) {
+//			if(ieee80211_set_turbo(dev,value))
+//					return -EINVAL;
+//			ieee80211_scan_flush(ic);
+//		}
+//		break;
+//	case IEEE80211_PARAM_XR:
+//		retv = ieee80211_setathcap(vap, IEEE80211_ATHC_XR, value);
+//		break;
+//	case IEEE80211_PARAM_BURST:
+//		retv = ieee80211_setathcap(vap, IEEE80211_ATHC_BURST, value);
+//		break;
+//	case IEEE80211_PARAM_AR:
+//		retv = ieee80211_setathcap(vap, IEEE80211_ATHC_AR, value);
+//		break;
+//	case IEEE80211_PARAM_PUREG:
+//		if (value)
+//			vap->iv_flags |= IEEE80211_F_PUREG;
+//		else
+//			vap->iv_flags &= ~IEEE80211_F_PUREG;
+//		/* NB: reset only if we're operating on an 11g channel */
+//		if (ic->ic_bsschan != IEEE80211_CHAN_ANYC &&
+//		    IEEE80211_IS_CHAN_ANYG(ic->ic_bsschan))
+//			retv = ENETRESET;
+//		break;
+//	case IEEE80211_PARAM_WDS:
+//		if (value)
+//			vap->iv_flags_ext |= IEEE80211_FEXT_WDS;
+//		else
+//			vap->iv_flags_ext &= ~IEEE80211_FEXT_WDS;
+//		break;
+//	case IEEE80211_PARAM_BGSCAN:
+//		if (value) {
+//			if ((vap->iv_caps & IEEE80211_C_BGSCAN) == 0)
+//				return -EINVAL;
+//			vap->iv_flags |= IEEE80211_F_BGSCAN;
+//		} else {
+//			/* XXX racey? */
+//			vap->iv_flags &= ~IEEE80211_F_BGSCAN;
+//			ieee80211_cancel_scan(vap);	/* anything current */
+//		}
+//		break;
+//	case IEEE80211_PARAM_BGSCAN_IDLE:
+//		if (value >= IEEE80211_BGSCAN_IDLE_MIN)
+//			vap->iv_bgscanidle = value*HZ/1000;
+//		else
+//			retv = EINVAL;
+//		break;
+//	case IEEE80211_PARAM_BGSCAN_INTERVAL:
+//		if (value >= IEEE80211_BGSCAN_INTVAL_MIN)
+//			vap->iv_bgscanintvl = value*HZ;
+//		else
+//			retv = EINVAL;
+//		break;
+//	case IEEE80211_PARAM_MCAST_RATE:
+//		/* units are in KILObits per second */
+//		if (value >= 256 && value <= 54000)
+//			vap->iv_mcast_rate = value;
+//		else
+//			retv = EINVAL;
+//		break;
+//	case IEEE80211_PARAM_COVERAGE_CLASS:
+//		if (value >= 0 && value <= IEEE80211_COVERAGE_CLASS_MAX) {
+//			ic->ic_coverageclass = value;
+//			if (IS_UP_AUTO(vap))
+//				ieee80211_new_state(vap, IEEE80211_S_SCAN, 0);
+//			retv = 0;
+//		}
+//		else
+//			retv = EINVAL;
+//		break;
+//	case IEEE80211_PARAM_COUNTRY_IE:
+//		if (value)
+//			ic->ic_flags_ext |= IEEE80211_FEXT_COUNTRYIE;
+//		else
+//			ic->ic_flags_ext &= ~IEEE80211_FEXT_COUNTRYIE;
+//		retv = ENETRESET;
+//		break;
+//	case IEEE80211_PARAM_REGCLASS:
+//		if (value)
+//			ic->ic_flags_ext |= IEEE80211_FEXT_REGCLASS;
+//		else
+//			ic->ic_flags_ext &= ~IEEE80211_FEXT_REGCLASS;
+//		retv = ENETRESET;
+//		break;
+//	case IEEE80211_PARAM_SCANVALID:
+//		vap->iv_scanvalid = value*HZ;
+//		break;
+//	case IEEE80211_PARAM_ROAM_RSSI_11A:
+//		vap->iv_roam.rssi11a = value;
+//		break;
+//	case IEEE80211_PARAM_ROAM_RSSI_11B:
+//		vap->iv_roam.rssi11bOnly = value;
+//		break;
+//	case IEEE80211_PARAM_ROAM_RSSI_11G:
+//		vap->iv_roam.rssi11b = value;
+//		break;
+//	case IEEE80211_PARAM_ROAM_RATE_11A:
+//		vap->iv_roam.rate11a = value;
+//		break;
+//	case IEEE80211_PARAM_ROAM_RATE_11B:
+//		vap->iv_roam.rate11bOnly = value;
+//		break;
+//	case IEEE80211_PARAM_ROAM_RATE_11G:
+//		vap->iv_roam.rate11b = value;
+//		break;
+//	case IEEE80211_PARAM_UAPSDINFO:
+//		if (vap->iv_opmode == IEEE80211_M_HOSTAP) {
+//			if (ic->ic_caps & IEEE80211_C_UAPSD) {
+//				if (value)
+//					IEEE80211_VAP_UAPSD_ENABLE(vap);
+//				else
+//					IEEE80211_VAP_UAPSD_DISABLE(vap);
+//				retv = ENETRESET;
+//			}
+//		}
+//		else if (vap->iv_opmode == IEEE80211_M_STA) {
+//			vap->iv_uapsdinfo = value;
+//			IEEE80211_VAP_UAPSD_ENABLE(vap);
+//			retv = ENETRESET;
+//		}
+//		break;
+//	case IEEE80211_PARAM_SLEEP:
+//		/* XXX: Forced sleep for testing. Does not actually place the
+//		 *      HW in sleep mode yet. this only makes sense for STAs.
+//		 */
+//		if (value) {
+//			/* goto sleep */
+//			IEEE80211_VAP_GOTOSLEEP(vap);
+//		}
+//		else {
+//			/* wakeup */
+//			IEEE80211_VAP_WAKEUP(vap);
+//		}
+//		ieee80211_send_nulldata(ieee80211_ref_node(vap->iv_bss));
+//		break;
+//	case IEEE80211_PARAM_QOSNULL:
+//		/* Force a QoS Null for testing. */
+//		ieee80211_send_qosnulldata(vap->iv_bss, value);
+//		break;
+//	case IEEE80211_PARAM_PSPOLL:
+//		/* Force a PS-POLL for testing. */
+//		ieee80211_send_pspoll(vap->iv_bss);
+//		break;
+//	case IEEE80211_PARAM_EOSPDROP:
+//		if (vap->iv_opmode == IEEE80211_M_HOSTAP) {
+//			if (value) IEEE80211_VAP_EOSPDROP_ENABLE(vap);
+//			else IEEE80211_VAP_EOSPDROP_DISABLE(vap);
+//		}
+//		break;
+//	case IEEE80211_PARAM_MARKDFS:
+//		if (value)
+//			ic->ic_flags_ext |= IEEE80211_FEXT_MARKDFS;
+//		else
+//			ic->ic_flags_ext &= ~IEEE80211_FEXT_MARKDFS;
+//		break;
+//	case IEEE80211_PARAM_CHANBW:
+//		switch (value) {
+//			case 0:
+//				ic->ic_chanbwflag = 0;
+//				break;
+//			case 1:
+//				ic->ic_chanbwflag = IEEE80211_CHAN_HALF;
+//				break;
+//			case 2:
+//				ic->ic_chanbwflag = IEEE80211_CHAN_QUARTER;
+//				break;
+//			default:
+//				retv = EINVAL;
+//				break;
+//		}
+//		break;
+//	case IEEE80211_PARAM_SHORTPREAMBLE:
+//		if (value) {
+//			ic->ic_caps |= IEEE80211_C_SHPREAMBLE;
+//		} else {
+//			ic->ic_caps &= ~IEEE80211_C_SHPREAMBLE;
+//		}
+//		retv = ENETRESET;
+//		break;
+//	default:
+//		retv = EOPNOTSUPP;
+//		break;
+//	}
+//	/* XXX should any of these cause a rescan? */
+//	if (retv == ENETRESET)
+//		retv = IS_UP_AUTO(vap) ? ieee80211_open(vap->iv_dev) : 0;
+//	return -retv;
+//}
+
+int usbdrvwext_setmode(struct net_device *dev, struct iw_request_info *info,
+		   	 void *w, char *extra)
+{
+	return 0;
+}
+
+int usbdrvwext_getmode(struct net_device *dev, struct iw_request_info *info,
+			void *w, char *extra)
+{
+	//struct usbdrv_private *macp = dev->ml_priv;
+	struct iw_point *wri = (struct iw_point *)extra;
+	char mode[8];
+
+    strcpy(mode,"11g");
+	return (copy_to_user(wri->pointer, mode, 6) ? -EFAULT : 0);
+}
+
+int zfLnxPrivateIoctl(struct net_device *dev, struct zdap_ioctl* zdreq)
+{
+	//void* regp = macp->regp;
+	u16_t cmd;
+	//u32_t temp;
+	u32_t* p;
+	u32_t i;
+
+	cmd = zdreq->cmd;
+	switch(cmd)
+	{
+	case ZM_IOCTL_REG_READ:
+		zfiDbgReadReg(dev, zdreq->addr);
+		break;
+
+	case ZM_IOCTL_REG_WRITE:
+		zfiDbgWriteReg(dev, zdreq->addr, zdreq->value);
+		break;
+
+	case ZM_IOCTL_MEM_READ:
+		p = (u32_t *) bus_to_virt(zdreq->addr);
+		printk(KERN_DEBUG "usbdrv: read memory addr: 0x%08x value: 0x%08x\n", zdreq->addr, *p);
+		break;
+
+	case ZM_IOCTL_MEM_WRITE:
+		p = (u32_t *) bus_to_virt(zdreq->addr);
+		*p = zdreq->value;
+		printk(KERN_DEBUG "usbdrv: write value: 0x%08x to memory addr: 0x%08x\n", zdreq->value, zdreq->addr);
+		break;
+
+	case ZM_IOCTL_TALLY :
+		zfiWlanShowTally(dev);
+		if (zdreq->addr)
+			zfiWlanResetTally(dev);
+		break;
+
+	case ZM_IOCTL_TEST :
+            printk(KERN_DEBUG "ZM_IOCTL_TEST:len=%d\n", zdreq->addr);
+            //zfiWlanReadReg(dev, 0x10f400);
+            //zfiWlanReadReg(dev, 0x10f404);
+            printk("IOCTL TEST\n");
+            #if 1
+            //print packet
+            for (i=0; i<zdreq->addr; i++)
+            {
+                if ((i&0x7) == 0)
+                {
+                    printk("\n");
+                }
+                printk("%02X ", (unsigned char)zdreq->data[i]);
+            }
+            printk("\n");
+            #endif
+
+
+            #if 0 //For Test?? 1 to 0 by CWYang(-)
+            {
+            struct sk_buff* s;
+
+            /* Allocate a skb */
+            s = alloc_skb(2000, GFP_ATOMIC);
+
+            /* Copy data to skb */
+            for (i=0; i<zdreq->addr; i++)
+            {
+                s->data[i] = zdreq->data[i];
+            }
+            s->len = zdreq->addr;
+
+            /* Call zfIdlRecv() */
+            zfiRecv80211(dev, s, NULL);
+            }
+            #endif
+
+            break;
+
+
+/****************************** ZDCONFIG ******************************/
+        case ZM_IOCTL_FRAG :
+            zfiWlanSetFragThreshold(dev, zdreq->addr);
+            break;
+
+        case ZM_IOCTL_RTS :
+            zfiWlanSetRtsThreshold(dev, zdreq->addr);
+            break;
+
+        case ZM_IOCTL_SCAN :
+            zfiWlanScan(dev);
+            break;
+
+        case ZM_IOCTL_KEY :
+            {
+                u8_t key[29];
+                struct zsKeyInfo keyInfo;
+                u32_t i;
+
+                for (i=0; i<29; i++)
+                {
+                    key[i] = 0;
+                }
+
+                for (i=0; i<zdreq->addr; i++)
+                {
+                    key[i] = zdreq->data[i];
+                }
+
+                printk("key len=%d, key=%02x%02x%02x%02x%02x...\n",
+                        zdreq->addr, key[0], key[1], key[2], key[3], key[4]);
+
+                keyInfo.keyLength = zdreq->addr;
+                keyInfo.keyIndex = 0;
+                keyInfo.flag = 0;
+                keyInfo.key = key;
+                zfiWlanSetKey(dev, keyInfo);
+            }
+            break;
+
+        case ZM_IOCTL_RATE :
+            zfiWlanSetTxRate(dev, zdreq->addr);
+            break;
+
+        case ZM_IOCTL_ENCRYPTION_MODE :
+            zfiWlanSetEncryMode(dev, zdreq->addr);
+
+            zfiWlanDisable(dev, 0);
+            zfiWlanEnable(dev);
+            break;
+        //CWYang(+)
+        case ZM_IOCTL_SIGNAL_STRENGTH :
+            {
+                u8_t buffer[2];
+                zfiWlanQuerySignalInfo(dev, &buffer[0]);
+                printk("Current Signal Strength : %02d\n", buffer[0]);
+            }
+            break;
+        //CWYang(+)
+        case ZM_IOCTL_SIGNAL_QUALITY :
+            {
+                u8_t buffer[2];
+                zfiWlanQuerySignalInfo(dev, &buffer[0]);
+                printk("Current Signal Quality : %02d\n", buffer[1]);
+            }
+            break;
+
+	case ZM_IOCTL_SET_PIBSS_MODE:
+		if (zdreq->addr == 1)
+			zfiWlanSetWlanMode(dev, ZM_MODE_PSEUDO);
+		else
+			zfiWlanSetWlanMode(dev, ZM_MODE_INFRASTRUCTURE);
+
+		zfiWlanDisable(dev, 0);
+		zfiWlanEnable(dev);
+
+		break;
+/****************************** ZDCONFIG ******************************/
+
+	default :
+		printk(KERN_ERR "usbdrv: error command = %x\n", cmd);
+		break;
+	}
+
+	return 0;
+}
+
+int usbdrv_wpa_ioctl(struct net_device *dev, struct athr_wlan_param *zdparm)
+{
+    int ret = 0;
+    u8_t bc_addr[6] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
+    u8_t mac_addr[80];
+    struct zsKeyInfo keyInfo;
+    struct usbdrv_private *macp = dev->ml_priv;
+    u16_t vapId = 0;
+
+    //zmw_get_wlan_dev(dev);
+
+    switch(zdparm->cmd)
+    {
+        case ZD_CMD_SET_ENCRYPT_KEY:
+
+            /* Set up key information */
+            keyInfo.keyLength = zdparm->u.crypt.key_len;
+            keyInfo.keyIndex = zdparm->u.crypt.idx;
+            if (zfiWlanQueryWlanMode(dev) == ZM_MODE_AP) // AP Mode
+                keyInfo.flag = ZM_KEY_FLAG_AUTHENTICATOR;
+            else
+                keyInfo.flag = 0;
+            keyInfo.key = zdparm->u.crypt.key;
+            keyInfo.initIv = zdparm->u.crypt.seq;
+            keyInfo.macAddr = (u16_t *)zdparm->sta_addr;
+
+            /* Identify the MAC address information */
+            if (memcmp(zdparm->sta_addr, bc_addr, sizeof(bc_addr)) == 0)
+            {
+                keyInfo.flag |= ZM_KEY_FLAG_GK;
+            }
+            else
+            {
+                keyInfo.flag |= ZM_KEY_FLAG_PK;
+            }
+
+            if (!strcmp(zdparm->u.crypt.alg, "NONE"))
+            {
+                //u8_t zero_mac[]={0,0,0,0,0,0};
+
+                /* Set key length to zero */
+                keyInfo.keyLength = 0;
+
+                if (zdparm->sta_addr[0] & 1)//del group key
+                {
+                    //if (macp->cardSetting.WPAIeLen==0)
+                    //{//802.1x dynamic WEP
+                    //    mDynKeyMode = 0;
+                    //    mKeyFormat[0] = 0;
+                    //    mPrivacyInvoked[0]=FALSE;
+                    //    mCap[0] &= ~CAP_PRIVACY;
+                    //    macp->cardSetting.EncryOnOff[0]=0;
+                    //}
+                    //mWpaBcKeyLen = mGkInstalled = 0;
+                }
+                else
+                {
+                    //if (memcmp(zero_mac,zdparm->sta_addr, 6)==0)
+                    //{
+                    //    mDynKeyMode=0;
+                    //    mKeyFormat[0]=0;
+                    //    pSetting->DynKeyMode=0;
+                    //    pSetting->EncryMode[0]=0;
+                    //    mDynKeyMode=0;
+                    //}
+                }
+
+                printk(KERN_ERR "Set Encryption Type NONE\n");
+                return ret;
+            }
+            else if (!strcmp(zdparm->u.crypt.alg, "TKIP"))
+            {
+                zfiWlanSetEncryMode(dev, ZM_TKIP);
+                //Linux Supplicant will inverse Tx/Rx key
+                //So we inverse it back //CWYang(+)
+                //zfMemoryCopy(&temp[0], &keyInfo.key[16], 8);
+                //zfMemoryCopy(&keyInfo.key[16], keyInfo.key[24], 8);
+                //zfMemoryCopy(&keyInfo.key[24], &temp[0], 8);
+                //u8_t temp;
+                //int k;
+                //for (k = 0; k < 8; k++)
+                //{
+                //    temp = keyInfo.key[16 + k];
+                //    keyInfo.key[16 + k] = keyInfo.key[24 + k];
+                //    keyInfo.key[24 + k] = temp;
+                //}
+                //CamEncryType = ZM_TKIP;
+                ////if (idx == 0)
+                //{// Pairwise key
+                //    mKeyFormat[0] = CamEncryType;
+                //    mDynKeyMode = pSetting->DynKeyMode = DYN_KEY_TKIP;
+                //}
+            }
+            else if (!strcmp(zdparm->u.crypt.alg, "CCMP"))
+            {
+                zfiWlanSetEncryMode(dev, ZM_AES);
+                //CamEncryType = ZM_AES;
+                ////if (idx == 0)
+                //{// Pairwise key
+                //    mKeyFormat[0] = CamEncryType;
+                //    mDynKeyMode = pSetting->DynKeyMode = DYN_KEY_AES;
+                //}
+            }
+            else if (!strcmp(zdparm->u.crypt.alg, "WEP"))
+            {
+                if (keyInfo.keyLength == 5)
+                { // WEP 64
+                    zfiWlanSetEncryMode(dev, ZM_WEP64);
+                //    CamEncryType = ZM_WEP64;
+                //    tmpDynKeyMode=DYN_KEY_WEP64;
+                }
+                else if (keyInfo.keyLength == 13)
+                {//keylen=13, WEP 128
+                    zfiWlanSetEncryMode(dev, ZM_WEP128);
+                //    CamEncryType = ZM_WEP128;
+                //    tmpDynKeyMode=DYN_KEY_WEP128;
+                }
+                else
+                {
+                    zfiWlanSetEncryMode(dev, ZM_WEP256);
+                }
+
+                // For Dynamic WEP key (Non-WPA Radius), the key ID range: 0-3
+                // In WPA/RSN mode, the key ID range: 1-3, usually, a broadcast key.
+                // For WEP key setting: we set mDynKeyMode and mKeyFormat in following case:
+                //   1. For 802.1x dynamically generated WEP key method.
+                //   2. For WPA/RSN mode, but key id == 0. (But this is an impossible case)
+                // So, only check case 1.
+                //if (macp->cardSetting.WPAIeLen==0)
+                //{
+                //    mKeyFormat[0] = CamEncryType;
+                //    mDynKeyMode = pSetting->DynKeyMode = tmpDynKeyMode;
+                //    mPrivacyInvoked[0]=TRUE;
+                //    mCap[0] |= CAP_PRIVACY;
+                //    macp->cardSetting.EncryOnOff[0]=1;
+                //}
+            }
+
+            /* DUMP key context */
+//#ifdef WPA_DEBUG
+            if (keyInfo.keyLength > 0)
+            {
+                int ii;
+                printk("Otus: Key Context:\n");
+                for(ii = 0; ii < keyInfo.keyLength;)
+                {
+                    printk("0x%02x ", keyInfo.key[ii]);
+                    if((++ii % 16) == 0)
+                        printk("\n");
+                }
+                printk("\n");
+            }
+//#endif
+
+            /* Set encrypt mode */
+            //zfiWlanSetEncryMode(dev, CamEncryType);
+            vapId = zfLnxGetVapId(dev);
+            if (vapId == 0xffff)
+                keyInfo.vapId = 0;
+            else
+                keyInfo.vapId = vapId + 1;
+            keyInfo.vapAddr[0] = keyInfo.macAddr[0];
+            keyInfo.vapAddr[1] = keyInfo.macAddr[1];
+            keyInfo.vapAddr[2] = keyInfo.macAddr[2];
+
+            zfiWlanSetKey(dev, keyInfo);
+
+            //zfiWlanDisable(dev);
+            //zfiWlanEnable(dev);
+            break;
+
+        case ZD_CMD_SET_MLME:
+            printk(KERN_ERR "usbdrv_wpa_ioctl: ZD_CMD_SET_MLME\n");
+
+            /* Translate STA's address */
+            sprintf(mac_addr, "%02x:%02x:%02x:%02x:%02x:%02x", zdparm->sta_addr[0], zdparm->sta_addr[1],
+                zdparm->sta_addr[2], zdparm->sta_addr[3], zdparm->sta_addr[4], zdparm->sta_addr[5]);
+
+            switch(zdparm->u.mlme.cmd)
+            {
+                case MLME_STA_DEAUTH:
+                    printk(" -------Call zfiWlanDeauth, reason:%d\n",zdparm->u.mlme.reason_code);
+                    if(zfiWlanDeauth(dev, (u16_t*) zdparm->sta_addr, zdparm->u.mlme.reason_code) != 0)
+                        printk(KERN_ERR "Can't deauthencate STA: %s\n", mac_addr);
+                    else
+                        printk(KERN_ERR "Deauthenticate STA: %s with reason code: %d\n", mac_addr, zdparm->u.mlme.reason_code);
+                    break;
+
+                case MLME_STA_DISASSOC:
+                    printk(" -------Call zfiWlanDeauth, reason:%d\n",zdparm->u.mlme.reason_code);
+                    if(zfiWlanDeauth(dev, (u16_t*) zdparm->sta_addr, zdparm->u.mlme.reason_code) != 0)
+                        printk(KERN_ERR "Can't disassociate STA: %s\n", mac_addr);
+                    else
+                        printk(KERN_ERR "Disassociate STA: %s with reason code: %d\n", mac_addr, zdparm->u.mlme.reason_code);
+                    break;
+
+                default:
+                    printk(KERN_ERR "MLME command: 0x%04x not support\n", zdparm->u.mlme.cmd);
+                    break;
+            }
+
+            break;
+
+        case ZD_CMD_SCAN_REQ:
+            printk(KERN_ERR "usbdrv_wpa_ioctl: ZD_CMD_SCAN_REQ\n");
+            break;
+
+        case ZD_CMD_SET_GENERIC_ELEMENT:
+            printk(KERN_ERR "usbdrv_wpa_ioctl: ZD_CMD_SET_GENERIC_ELEMENT\n");
+
+            /* Copy the WPA IE */
+            //zm_msg1_mm(ZM_LV_0, "CWY - wpaie Length : ", zdparm->u.generic_elem.len);
+            printk(KERN_ERR "wpaie Length : %d\n", zdparm->u.generic_elem.len);
+            if (zfiWlanQueryWlanMode(dev) == ZM_MODE_AP) // AP Mode
+            {
+                zfiWlanSetWpaIe(dev, zdparm->u.generic_elem.data, zdparm->u.generic_elem.len);
+            }
+            else
+            {
+                macp->supLen = zdparm->u.generic_elem.len;
+                memcpy(macp->supIe, zdparm->u.generic_elem.data, zdparm->u.generic_elem.len);
+            }
+            zfiWlanSetWpaSupport(dev, 1);
+            //zfiWlanSetWpaIe(dev, zdparm->u.generic_elem.data, zdparm->u.generic_elem.len);
+            {
+                int ii;
+                u8_t len = zdparm->u.generic_elem.len;
+                u8_t *wpaie = (u8_t *)zdparm->u.generic_elem.data;
+
+                printk(KERN_ERR "wd->ap.wpaLen: %d\n", len);
+
+                /* DUMP WPA IE */
+                for(ii = 0; ii < len;)
+                {
+                    printk(KERN_ERR "0x%02x ", wpaie[ii]);
+
+                    if((++ii % 16) == 0)
+                        printk(KERN_ERR "\n");
+                }
+                printk(KERN_ERR "\n");
+            }
+
+//            #ifdef ZM_HOSTAPD_SUPPORT
+            //if (wd->wlanMode == ZM_MODE_AP)
+            //{// Update Beacon FIFO in the next TBTT.
+            //    memcpy(&mWPAIe, pSetting->WPAIe, pSetting->WPAIeLen);
+            //    printk(KERN_ERR "Copy WPA IE into mWPAIe\n");
+            //}
+//            #endif
+            break;
+
+//        #ifdef ZM_HOSTAPD_SUPPORT
+        case ZD_CMD_GET_TSC:
+            printk(KERN_ERR "usbdrv_wpa_ioctl: ZD_CMD_GET_TSC\n");
+            break;
+//        #endif
+
+        default:
+            printk(KERN_ERR "usbdrv_wpa_ioctl default: 0x%04x\n", zdparm->cmd);
+            ret = -EINVAL;
+            break;
+    }
+
+    return ret;
+}
+
+#ifdef ZM_ENABLE_CENC
+int usbdrv_cenc_ioctl(struct net_device *dev, struct zydas_cenc_param *zdparm)
+{
+    //struct usbdrv_private *macp = dev->ml_priv;
+    struct zsKeyInfo keyInfo;
+    u16_t apId;
+    u8_t bc_addr[6] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
+    int ret = 0;
+    int ii;
+
+    /* Get the AP Id */
+    apId = zfLnxGetVapId(dev);
+
+    if (apId == 0xffff)
+    {
+        apId = 0;
+    }
+    else
+    {
+        apId = apId+1;
+    }
+
+    switch (zdparm->cmd)
+    {
+        case ZM_CMD_CENC_SETCENC:
+            printk(KERN_ERR "ZM_CMD_CENC_SETCENC\n");
+            printk(KERN_ERR "length: %d\n", zdparm->len);
+            printk(KERN_ERR "policy: %d\n", zdparm->u.info.cenc_policy);
+            break;
+        case ZM_CMD_CENC_SETKEY:
+            //ret = wai_ioctl_setkey(vap, ioctl_msg);
+            printk(KERN_ERR "ZM_CMD_CENC_SETKEY\n");
+
+            printk(KERN_ERR "MAC address= ");
+            for(ii = 0; ii < 6; ii++)
+            {
+                printk(KERN_ERR "0x%02x ", zdparm->u.crypt.sta_addr[ii]);
+            }
+            printk(KERN_ERR "\n");
+
+            printk(KERN_ERR "Key Index: %d\n", zdparm->u.crypt.keyid);
+            printk(KERN_ERR "Encryption key= ");
+            for(ii = 0; ii < 16; ii++)
+            {
+                printk(KERN_ERR "0x%02x ", zdparm->u.crypt.key[ii]);
+            }
+            printk(KERN_ERR "\n");
+
+            printk(KERN_ERR "MIC key= ");
+            for(ii = 16; ii < ZM_CENC_KEY_SIZE; ii++)
+            {
+                printk(KERN_ERR "0x%02x ", zdparm->u.crypt.key[ii]);
+            }
+            printk(KERN_ERR "\n");
+
+            /* Set up key information */
+            keyInfo.keyLength = ZM_CENC_KEY_SIZE;
+            keyInfo.keyIndex = zdparm->u.crypt.keyid;
+            keyInfo.flag = ZM_KEY_FLAG_AUTHENTICATOR | ZM_KEY_FLAG_CENC;
+            keyInfo.key = zdparm->u.crypt.key;
+            keyInfo.macAddr = (u16_t *)zdparm->u.crypt.sta_addr;
+
+            /* Identify the MAC address information */
+            if (memcmp(zdparm->u.crypt.sta_addr, bc_addr, sizeof(bc_addr)) == 0)
+            {
+                keyInfo.flag |= ZM_KEY_FLAG_GK;
+                keyInfo.vapId = apId;
+                memcpy(keyInfo.vapAddr, dev->dev_addr, ETH_ALEN);
+            }
+            else
+            {
+                keyInfo.flag |= ZM_KEY_FLAG_PK;
+            }
+
+            zfiWlanSetKey(dev, keyInfo);
+
+            break;
+        case ZM_CMD_CENC_REKEY:
+            //ret = wai_ioctl_rekey(vap, ioctl_msg);
+            printk(KERN_ERR "ZM_CMD_CENC_REKEY\n");
+            break;
+        default:
+            ret = -EOPNOTSUPP;
+            break;
+
+    }
+
+    //if (retv == ENETRESET)
+    //    retv = IS_UP_AUTO(vap) ? ieee80211_open(vap->iv_dev) : 0;
+
+    return ret;
+}
+#endif //ZM_ENABLE_CENC
+/////////////////////////////////////////
+int usbdrv_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
+{
+//    struct usbdrv_private *macp;
+//    void *regp;
+    struct zdap_ioctl zdreq;
+    struct iwreq *wrq = (struct iwreq *)ifr;
+    struct athr_wlan_param zdparm;
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    int err = 0;
+    int changed = 0;
+
+//    regp = macp->regp;
+
+    if(!netif_running(dev))
+        return -EINVAL;
+
+    switch (cmd)
+    {
+            case SIOCGIWNAME:
+            strcpy(wrq->u.name, "IEEE 802.11-DS");
+            break;
+
+        case SIOCGIWAP:
+            err = usbdrvwext_giwap(dev, NULL, &wrq->u.ap_addr, NULL);
+            break;
+
+
+        case SIOCSIWAP:
+            err = usbdrvwext_siwap(dev, NULL, &wrq->u.ap_addr, NULL);
+            break;
+
+
+        case SIOCGIWMODE:
+            err = usbdrvwext_giwmode(dev, NULL, &wrq->u.mode, NULL);
+            break;
+
+
+        case SIOCSIWESSID:
+            printk(KERN_ERR "CWY - usbdrvwext_siwessid\n");
+            //err = usbdrv_ioctl_setessid(dev, &wrq->u.essid);
+            err = usbdrvwext_siwessid(dev, NULL, &wrq->u.essid, NULL);
+
+            if (! err)
+                changed = 1;
+            break;
+
+
+        case SIOCGIWESSID:
+            err = usbdrvwext_giwessid(dev, NULL, &wrq->u.essid, NULL);
+            break;
+
+
+        case SIOCSIWRTS:
+
+            err = usbdrv_ioctl_setrts(dev, &wrq->u.rts);
+            if (! err)
+                changed = 1;
+            break;
+
+
+		case SIOCIWFIRSTPRIV + 0x2: /* set_auth */
+		{
+			//printk("CWY - SIOCIWFIRSTPRIV + 0x2 (set_auth)\n");
+			if (! capable(CAP_NET_ADMIN))
+			{
+				err = -EPERM;
+				break;
+			}
+			{
+				int val = *( (int *) wrq->u.name );
+				if ((val < 0) || (val > 2))
+				{
+					err = -EINVAL;
+					break;
+				}
+				else
+				{
+					zfiWlanSetAuthenticationMode(dev, val);
+
+                    if (macp->DeviceOpened == 1)
+                    {
+                        zfiWlanDisable(dev, 0);
+                        zfiWlanEnable(dev);
+                    }
+
+					err = 0;
+					changed = 1;
+				}
+			}
+		}
+			break;
+
+		case SIOCIWFIRSTPRIV + 0x3: /* get_auth */
+		{
+		    int AuthMode = ZM_AUTH_MODE_OPEN;
+
+			//printk("CWY - SIOCIWFIRSTPRIV + 0x3 (get_auth)\n");
+
+			if (wrq->u.data.pointer)
+			{
+				wrq->u.data.flags = 1;
+
+				AuthMode = zfiWlanQueryAuthenticationMode(dev, 0);
+				if (AuthMode == ZM_AUTH_MODE_OPEN)
+				{
+					wrq->u.data.length = 12;
+
+					if (copy_to_user(wrq->u.data.pointer, "open system", 12))
+					{
+						return -EFAULT;
+					}
+				}
+				else if (AuthMode == ZM_AUTH_MODE_SHARED_KEY)
+				{
+					wrq->u.data.length = 11;
+
+					if (copy_to_user(wrq->u.data.pointer, "shared key", 11))
+					{
+						return -EFAULT;
+					}
+				}
+				else if (AuthMode == ZM_AUTH_MODE_AUTO)
+				{
+					wrq->u.data.length = 10;
+
+					if (copy_to_user(wrq->u.data.pointer, "auto mode", 10))
+					{
+						return -EFAULT;
+					}
+				}
+				else
+				{
+					return -EFAULT;
+				}
+			}
+		}
+			break;
+
+
+        case ZDAPIOCTL:    //debug command
+            if (copy_from_user(&zdreq, ifr->ifr_data, sizeof (zdreq)))
+            {
+                printk(KERN_ERR "usbdrv: copy_from_user error\n");
+                return -EFAULT;
+            }
+
+            //printk(KERN_DEBUG "usbdrv: cmd=%2x, reg=0x%04lx, value=0x%08lx\n",
+            //        zdreq.cmd, zdreq.addr, zdreq.value);
+
+			zfLnxPrivateIoctl(dev, &zdreq);
+
+            err = 0;
+            break;
+
+        case ZD_IOCTL_WPA:
+            if (copy_from_user(&zdparm, ifr->ifr_data, sizeof(struct athr_wlan_param)))
+            {
+                printk(KERN_ERR "usbdrv: copy_from_user error\n");
+                return -EFAULT;
+            }
+
+            usbdrv_wpa_ioctl(dev, &zdparm);
+            err = 0;
+            break;
+
+        case ZD_IOCTL_PARAM:
+        {
+            int *p;
+            int op;
+            int arg;
+
+            /* Point to the name field and retrieve the
+             * op and arg elements.          */
+            p = (int *)wrq->u.name;
+            op = *p++;
+            arg = *p;
+
+            if(op == ZD_PARAM_ROAMING)
+            {
+                printk(KERN_ERR "************* ZD_PARAM_ROAMING: %d\n", arg);
+                //macp->cardSetting.ap_scan=(U8)arg;
+            }
+            if(op == ZD_PARAM_PRIVACY)
+            {
+                printk(KERN_ERR "ZD_IOCTL_PRIVACY: ");
+
+                /* Turn on the privacy invoke flag */
+                if(arg)
+                {
+                //    mCap[0] |= CAP_PRIVACY;
+                //    macp->cardSetting.EncryOnOff[0] = 1;
+                    printk(KERN_ERR "enable\n");
+
+                }
+                else
+                {
+                //    mCap[0] &= ~CAP_PRIVACY;
+                //    macp->cardSetting.EncryOnOff[0] = 0;
+                    printk(KERN_ERR "disable\n");
+                }
+                                //changed=1;
+            }
+            if(op == ZD_PARAM_WPA)
+            {
+                printk(KERN_ERR "ZD_PARAM_WPA: ");
+
+                if(arg)
+                {
+                    printk(KERN_ERR "enable\n");
+
+                    if (zfiWlanQueryWlanMode(dev) != ZM_MODE_AP)
+                    {
+                        printk(KERN_ERR "Station Mode\n");
+                        //zfiWlanQueryWpaIe(dev, (u8_t *)&wpaIe, &wpalen);
+                        //printk("wpaIe : %2x,%2x,%2x\n", wpaIe[21], wpaIe[22], wpaIe[23]);
+                        //printk("rsnIe : %2x,%2x,%2x\n", wpaIe[17], wpaIe[18], wpaIe[19]);
+                        if ((macp->supIe[21] == 0x50) &&
+                            (macp->supIe[22] == 0xf2) &&
+                            (macp->supIe[23] == 0x2))
+                        {
+                            printk(KERN_ERR "wd->sta.authMode = ZM_AUTH_MODE_WPAPSK\n");
+                            //wd->sta.authMode = ZM_AUTH_MODE_WPAPSK;
+                            //wd->ws.authMode = ZM_AUTH_MODE_WPAPSK;
+                            zfiWlanSetAuthenticationMode(dev, ZM_AUTH_MODE_WPAPSK);
+                        }
+                        else if ((macp->supIe[21] == 0x50) &&
+                                 (macp->supIe[22] == 0xf2) &&
+                                 (macp->supIe[23] == 0x1))
+                        {
+                            printk(KERN_ERR "wd->sta.authMode = ZM_AUTH_MODE_WPA\n");
+                            //wd->sta.authMode = ZM_AUTH_MODE_WPA;
+                            //wd->ws.authMode = ZM_AUTH_MODE_WPA;
+                            zfiWlanSetAuthenticationMode(dev, ZM_AUTH_MODE_WPA);
+                        }
+                        else if ((macp->supIe[17] == 0xf) &&
+                                 (macp->supIe[18] == 0xac) &&
+                                 (macp->supIe[19] == 0x2))
+                        {
+                            printk(KERN_ERR "wd->sta.authMode = ZM_AUTH_MODE_WPA2PSK\n");
+                            //wd->sta.authMode = ZM_AUTH_MODE_WPA2PSK;
+                            //wd->ws.authMode = ZM_AUTH_MODE_WPA2PSK;
+                            zfiWlanSetAuthenticationMode(dev, ZM_AUTH_MODE_WPA2PSK);
+                        }
+                        else if ((macp->supIe[17] == 0xf) &&
+                                 (macp->supIe[18] == 0xac) &&
+                                 (macp->supIe[19] == 0x1))
+                        {
+                            printk(KERN_ERR "wd->sta.authMode = ZM_AUTH_MODE_WPA2\n");
+                            //wd->sta.authMode = ZM_AUTH_MODE_WPA2;
+                            //wd->ws.authMode = ZM_AUTH_MODE_WPA2;
+                            zfiWlanSetAuthenticationMode(dev, ZM_AUTH_MODE_WPA2);
+                        }
+                        if ((macp->supIe[21] == 0x50) || (macp->supIe[22] == 0xf2))//WPA or WPAPSK
+                        {
+                            if (macp->supIe[11] == 0x2)
+                            {
+                                printk(KERN_ERR "wd->sta.wepStatus = ZM_ENCRYPTION_TKIP\n");
+                                //wd->sta.wepStatus = ZM_ENCRYPTION_TKIP;
+                                //wd->ws.wepStatus = ZM_ENCRYPTION_TKIP;
+                                zfiWlanSetWepStatus(dev, ZM_ENCRYPTION_TKIP);
+                            }
+                            else
+                            {
+                                printk(KERN_ERR "wd->sta.wepStatus = ZM_ENCRYPTION_AES\n");
+                                //wd->sta.wepStatus = ZM_ENCRYPTION_AES;
+                                //wd->ws.wepStatus = ZM_ENCRYPTION_AES;
+                                zfiWlanSetWepStatus(dev, ZM_ENCRYPTION_AES);
+                            }
+                        }
+                        if ((macp->supIe[17] == 0xf) || (macp->supIe[18] == 0xac)) //WPA2 or WPA2PSK
+                        {
+                            if (macp->supIe[13] == 0x2)
+                            {
+                                printk(KERN_ERR "wd->sta.wepStatus = ZM_ENCRYPTION_TKIP\n");
+                                //wd->sta.wepStatus = ZM_ENCRYPTION_TKIP;
+                                //wd->ws.wepStatus = ZM_ENCRYPTION_TKIP;
+                                zfiWlanSetWepStatus(dev, ZM_ENCRYPTION_TKIP);
+                            }
+                            else
+                            {
+                                printk(KERN_ERR "wd->sta.wepStatus = ZM_ENCRYPTION_AES\n");
+                                //wd->sta.wepStatus = ZM_ENCRYPTION_AES;
+                                //wd->ws.wepStatus = ZM_ENCRYPTION_AES;
+                                zfiWlanSetWepStatus(dev, ZM_ENCRYPTION_AES);
+                            }
+                        }
+                    }
+                    zfiWlanSetWpaSupport(dev, 1);
+                }
+                else
+                {
+                    /* Reset the WPA related variables */
+                    printk(KERN_ERR "disable\n");
+
+                    zfiWlanSetWpaSupport(dev, 0);
+                    zfiWlanSetAuthenticationMode(dev, ZM_AUTH_MODE_OPEN);
+                    zfiWlanSetWepStatus(dev, ZM_ENCRYPTION_WEP_DISABLED);
+
+                    /* Now we only set the length in the WPA IE
+                     * field to zero.                         */
+                    //macp->cardSetting.WPAIe[1] = 0;
+                }
+            }
+            if(op == ZD_PARAM_COUNTERMEASURES)
+            {
+                printk(KERN_ERR "================ZD_PARAM_COUNTERMEASURES: ");
+
+                if(arg)
+                {
+                //    mCounterMeasureState=1;
+                    printk(KERN_ERR "enable\n");
+                }
+                else
+                {
+                //    mCounterMeasureState=0;
+                    printk(KERN_ERR "disable\n");
+                }
+            }
+            if(op == ZD_PARAM_DROPUNENCRYPTED)
+            {
+                printk(KERN_ERR "ZD_PARAM_DROPUNENCRYPTED: ");
+
+                if(arg)
+                {
+                    printk(KERN_ERR "enable\n");
+                }
+                else
+                {
+                    printk(KERN_ERR "disable\n");
+                }
+            }
+            if(op == ZD_PARAM_AUTH_ALGS)
+            {
+                printk(KERN_ERR "ZD_PARAM_AUTH_ALGS: ");
+
+                if(arg == 0)
+                {
+                    printk(KERN_ERR "OPEN_SYSTEM\n");
+                }
+                else
+                {
+                    printk(KERN_ERR "SHARED_KEY\n");
+                }
+            }
+            if(op == ZD_PARAM_WPS_FILTER)
+            {
+                printk(KERN_ERR "ZD_PARAM_WPS_FILTER: ");
+
+                if(arg)
+                {
+                //    mCounterMeasureState=1;
+                    macp->forwardMgmt = 1;
+                    printk(KERN_ERR "enable\n");
+                }
+                else
+                {
+                //    mCounterMeasureState=0;
+                    macp->forwardMgmt = 0;
+                    printk(KERN_ERR "disable\n");
+                }
+            }
+        }
+            err = 0;
+            break;
+
+        case ZD_IOCTL_GETWPAIE:
+        {
+            struct ieee80211req_wpaie req_wpaie;
+            u16_t apId, i, j;
+
+            /* Get the AP Id */
+            apId = zfLnxGetVapId(dev);
+
+            if (apId == 0xffff)
+            {
+                apId = 0;
+            }
+            else
+            {
+                apId = apId+1;
+            }
+
+            if (copy_from_user(&req_wpaie, ifr->ifr_data, sizeof(struct ieee80211req_wpaie))){
+                printk(KERN_ERR "usbdrv: copy_from_user error\n");
+                return -EFAULT;
+            }
+
+            for(i = 0; i < ZM_OAL_MAX_STA_SUPPORT; i++)
+            {
+                for(j = 0; j < IEEE80211_ADDR_LEN; j++)
+                {
+                    if (macp->stawpaie[i].wpa_macaddr[j] != req_wpaie.wpa_macaddr[j])
+                        break;
+                }
+                if (j == 6)
+                    break;
+            }
+            if (i < ZM_OAL_MAX_STA_SUPPORT)
+            {
+                //printk("ZD_IOCTL_GETWPAIE - sta index = %d\n", i);
+                memcpy(req_wpaie.wpa_ie, macp->stawpaie[i].wpa_ie, IEEE80211_MAX_IE_SIZE);
+            }
+
+            if (copy_to_user(wrq->u.data.pointer, &req_wpaie, sizeof(struct ieee80211req_wpaie)))
+            {
+                    return -EFAULT;
+            }
+        }
+
+            err = 0;
+            break;
+#ifdef ZM_ENABLE_CENC
+        case ZM_IOCTL_CENC:
+            if (copy_from_user(&macp->zd_wpa_req, ifr->ifr_data, sizeof(struct athr_wlan_param)))
+            {
+                printk(KERN_ERR "usbdrv: copy_from_user error\n");
+                return -EFAULT;
+            }
+
+            usbdrv_cenc_ioctl(dev, (struct zydas_cenc_param *)&macp->zd_wpa_req);
+            err = 0;
+            break;
+#endif //ZM_ENABLE_CENC
+        default:
+            err = -EOPNOTSUPP;
+            break;
+    }
+
+
+    return err;
+}
diff --git a/drivers/staging/otus/oal_dt.h b/drivers/staging/otus/oal_dt.h
new file mode 100644
index 00000000000..e82b9770fca
--- /dev/null
+++ b/drivers/staging/otus/oal_dt.h
@@ -0,0 +1,60 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*  Module Name : oal_dt.h                                              */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains data type definition.                      */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      Platform dependent.                                             */
+/*                                                                      */
+/************************************************************************/
+
+#ifndef _OAL_DT_H
+#define _OAL_DT_H
+
+/* Please include header files for buffer type in the beginning of this file */
+/* Please include header files for device type here */
+#include <linux/netdevice.h>
+
+typedef     unsigned long long  u64_t;
+typedef     unsigned int        u32_t;
+typedef     unsigned short      u16_t;
+typedef     unsigned char       u8_t;
+typedef     long long           s64_t;
+typedef     long                s32_t;
+typedef     short               s16_t;
+typedef     char                s8_t;
+
+#ifndef     TRUE
+#define     TRUE                (1==1)
+#endif
+
+#ifndef     FALSE
+#define     FALSE               (1==0)
+#endif
+
+#ifndef     NULL
+#define     NULL                0
+#endif
+
+/* Please include header files for buffer type in the beginning of this file */
+typedef     struct sk_buff      zbuf_t;
+
+/* Please include header files for device type in the beginning of this file */
+typedef     struct net_device   zdev_t;
+
+#endif /* #ifndef _OAL_DT_H */
diff --git a/drivers/staging/otus/oal_marc.h b/drivers/staging/otus/oal_marc.h
new file mode 100644
index 00000000000..438e4bc2e9a
--- /dev/null
+++ b/drivers/staging/otus/oal_marc.h
@@ -0,0 +1,135 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*  Module Name : oal_marc.h                                            */
+/*                                                                      */
+/*  Abstract                                                            */
+/*      This module contains warpper definitions.                       */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      Platform dependent.                                             */
+/*                                                                      */
+/************************************************************************/
+
+#ifndef _OAL_MARC_H
+#define _OAL_MARC_H
+
+#include "oal_dt.h"
+#include "usbdrv.h"
+
+#define ZM_OS_LINUX_FUNC
+
+/***** Critical section *****/
+/* Declare for critical section */
+#ifndef ZM_HALPLUS_LOCK
+#define zmw_get_wlan_dev(dev)    struct zsWlanDev *wd = (struct zsWlanDev*) ((((struct usbdrv_private*)dev->priv)->wd))
+
+#define zmw_declare_for_critical_section() unsigned long irqFlag;
+
+/* Enter critical section */
+#define zmw_enter_critical_section(dev) \
+        spin_lock_irqsave(&(((struct usbdrv_private *)(dev->priv))->cs_lock), irqFlag);
+
+/* leave critical section */
+#define zmw_leave_critical_section(dev) \
+        spin_unlock_irqrestore(&(((struct usbdrv_private *)(dev->priv))->cs_lock), irqFlag);
+#else
+#define zmw_get_wlan_dev(dev)    struct zsWlanDev *wd = zfwGetWlanDev(dev);
+
+/* Declare for critical section */
+#define zmw_declare_for_critical_section()
+
+/* Enter critical section */
+#define zmw_enter_critical_section(dev) \
+        zfwEnterCriticalSection(dev);
+
+/* leave critical section */
+#define zmw_leave_critical_section(dev) \
+        zfwLeaveCriticalSection(dev);
+#endif
+
+/***** Byte order converting *****/
+#ifdef ZM_CONFIG_BIG_ENDIAN
+#define zmw_cpu_to_le32(v)    (((v & 0xff000000) >> 24) | \
+                               ((v & 0x00ff0000) >> 8)  | \
+                               ((v & 0x0000ff00) << 8)  | \
+                               ((v & 0x000000ff) << 24))
+
+#define zmw_le32_to_cpu(v)    (((v & 0xff000000) >> 24) | \
+                               ((v & 0x00ff0000) >> 8)  | \
+                               ((v & 0x0000ff00) << 8)  | \
+                               ((v & 0x000000ff) << 24))
+
+#define zmw_cpu_to_le16(v)    (((v & 0xff00) >> 8) | \
+                               ((v & 0x00ff) << 8))
+
+#define zmw_le16_to_cpu(v)    (((v & 0xff00) >> 8) | \
+                               ((v & 0x00ff) << 8))
+#else
+#define zmw_cpu_to_le32(v)    (v)
+#define zmw_le32_to_cpu(v)    (v)
+#define zmw_cpu_to_le16(v)    (v)
+#define zmw_le16_to_cpu(v)    (v)
+#endif
+
+/***** Buffer access *****/
+/* Called to read/write buffer */
+#ifndef ZM_HALPLUS_LOCK
+
+#define zmw_buf_readb(dev, buf, offset) *(u8_t*)((u8_t*)buf->data+offset)
+#define zmw_buf_readh(dev, buf, offset) zmw_cpu_to_le16(*(u16_t*)((u8_t*)buf->data+offset))
+#define zmw_buf_writeb(dev, buf, offset, value) *(u8_t*)((u8_t*)buf->data+offset) = value
+#define zmw_buf_writeh(dev, buf, offset, value) *(u16_t*)((u8_t*)buf->data+offset) = zmw_cpu_to_le16(value)
+#define zmw_buf_get_buffer(dev, buf) (u8_t*)(buf->data)
+
+#else
+
+#define zmw_buf_readb(dev, buf, offset) zfwBufReadByte(dev, buf, offset)
+#define zmw_buf_readh(dev, buf, offset) zfwBufReadHalfWord(dev, buf, offset)
+#define zmw_buf_writeb(dev, buf, offset, value) zfwBufWriteByte(dev, buf, offset, value)
+#define zmw_buf_writeh(dev, buf, offset, value) zfwBufWriteHalfWord(dev, buf, offset, value)
+#define zmw_buf_get_buffer(dev, buf) zfwGetBuffer(dev, buf)
+
+#endif
+
+/***** Debug message *****/
+#if 0
+#define zm_debug_msg0(msg) printk("%s:%s\n", __FUNCTION__, msg);
+#define zm_debug_msg1(msg, val) printk("%s:%s%ld\n", __FUNCTION__, \
+        msg, (u32_t)val);
+#define zm_debug_msg2(msg, val) printk("%s:%s%lxh\n", __FUNCTION__, \
+        msg, (u32_t)val);
+#define zm_debug_msg_s(msg, val) printk("%s:%s%s\n", __FUNCTION__, \
+        msg, val);
+#define zm_debug_msg_p(msg, val1, val2) printk("%s:%s%01ld.%02ld\n", __FUNCTION__, \
+        msg, (val1/val2), (((val1*100)/val2)%100));
+#define zm_dbg(S) printk S
+#else
+#define zm_debug_msg0(msg)
+#define zm_debug_msg1(msg, val)
+#define zm_debug_msg2(msg, val)
+#define zm_debug_msg_s(msg, val)
+#define zm_debug_msg_p(msg, val1, val2)
+#define zm_dbg(S)
+#endif
+
+#define zm_assert(expr) if(!(expr)) {                           \
+        printk( "Atheors Assertion failed! %s,%s,%s,line=%d\n",   \
+        #expr,__FILE__,__FUNCTION__,__LINE__);                  \
+        }
+
+#define DbgPrint printk
+
+#endif /* #ifndef _OAL_MARC_H */
diff --git a/drivers/staging/otus/usbdrv.c b/drivers/staging/otus/usbdrv.c
new file mode 100644
index 00000000000..dfe07072011
--- /dev/null
+++ b/drivers/staging/otus/usbdrv.c
@@ -0,0 +1,1148 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*  Module Name : usbdrv.c                                              */
+/*                                                                      */
+/*  Abstract                                                            */
+/*     This module contains network interface up/down related functions.*/
+/*                                                                      */
+/*  NOTES                                                               */
+/*     Platform dependent.                                              */
+/*                                                                      */
+/************************************************************************/
+
+/* src/usbdrv.c */
+
+#define ZM_PIBSS_MODE   0
+#define ZM_AP_MODE      0
+#define ZM_CHANNEL      11
+#define ZM_WEP_MOME     0
+#define ZM_SHARE_AUTH   0
+#define ZM_DISABLE_XMIT 0
+
+#include "usbdrv.h"
+#include "oal_dt.h"
+#include "80211core/pub_zfi.h"
+
+#include "linux/netlink.h"
+#include "linux/rtnetlink.h"
+
+#if WIRELESS_EXT > 12
+#include <net/iw_handler.h>
+#endif
+
+#ifdef ZM_HOSTAPD_SUPPORT
+#include "athr_common.h"
+#endif
+
+extern void zfDumpDescriptor(zdev_t* dev, u16_t type);
+//extern void zfiWlanQueryMacAddress(zdev_t* dev, u8_t* addr);
+
+// ISR handler
+irqreturn_t usbdrv_intr(int, void *, struct pt_regs *);
+
+// Network Device interface related function
+int usbdrv_open(struct net_device *);
+int usbdrv_close(struct net_device *);
+int usbdrv_change_mtu(struct net_device *, int);
+int usbdrv_set_mac(struct net_device *, void *);
+int usbdrv_xmit_frame(struct sk_buff *, struct net_device *);
+void usbdrv_set_multi(struct net_device *);
+struct net_device_stats *usbdrv_get_stats(struct net_device *);
+
+//wireless extension helper functions
+int usbdrv_ioctl_setessid(struct net_device *dev, struct iw_point *erq);
+int usbdrv_ioctl_getessid(struct net_device *dev, struct iw_point *erq);
+int usbdrv_ioctl_setrts(struct net_device *dev, struct iw_param *rrq);
+/* Wireless Extension Handler functions */
+int usbdrvwext_giwmode(struct net_device *dev, struct iw_request_info* info,
+        __u32 *mode, char *extra);
+int zfLnxPrivateIoctl(struct usbdrv_private *macp, struct zdap_ioctl *zdreq);
+
+void zfLnx10msTimer(struct net_device* dev);
+int zfUnregisterWdsDev(struct net_device* parentDev, u16_t wdsId);
+int zfRegisterWdsDev(struct net_device* parentDev, u16_t wdsId);
+int zfWdsOpen(struct net_device *dev);
+int zfWdsClose(struct net_device *dev);
+int zfLnxVapOpen(struct net_device *dev);
+int zfLnxVapClose(struct net_device *dev);
+int zfLnxVapXmitFrame(struct sk_buff *skb, struct net_device *dev);
+int zfLnxRegisterVapDev(struct net_device* parentDev, u16_t vapId);
+int usbdrv_wpa_ioctl(struct net_device *dev, struct athr_wlan_param *zdparm);
+extern u16_t zfLnxGetVapId(zdev_t* dev);
+extern u16_t zfLnxCheckTxBufferCnt(zdev_t *dev);
+extern UsbTxQ_t *zfLnxGetUsbTxBuffer(zdev_t *dev);
+
+extern u16_t zfLnxAuthNotify(zdev_t* dev, u16_t* macAddr);
+extern u16_t zfLnxAsocNotify(zdev_t* dev, u16_t* macAddr, u8_t* body, u16_t bodySize, u16_t port);
+extern u16_t zfLnxDisAsocNotify(zdev_t* dev, u8_t* macAddr, u16_t port);
+extern u16_t zfLnxApConnectNotify(zdev_t* dev, u8_t* macAddr, u16_t port);
+extern void zfLnxConnectNotify(zdev_t* dev, u16_t status, u16_t* bssid);
+extern void zfLnxScanNotify(zdev_t* dev, struct zsScanResult* result);
+extern void zfLnxStatisticsNotify(zdev_t* dev, struct zsStastics* result);
+extern void zfLnxMicFailureNotify(zdev_t* dev, u16_t* addr, u16_t status);
+extern void zfLnxApMicFailureNotify(zdev_t* dev, u8_t* addr, zbuf_t* buf);
+extern void zfLnxIbssPartnerNotify(zdev_t* dev, u16_t status, struct zsPartnerNotifyEvent *event);
+extern void zfLnxMacAddressNotify(zdev_t* dev, u8_t* addr);
+extern void zfLnxSendCompleteIndication(zdev_t* dev, zbuf_t* buf);
+extern void zfLnxRecvEth(zdev_t* dev, zbuf_t* buf, u16_t port);
+extern void zfLnxRestoreBufData(zdev_t* dev, zbuf_t* buf);
+#ifdef ZM_ENABLE_CENC
+extern u16_t zfLnxCencAsocNotify(zdev_t* dev, u16_t* macAddr, u8_t* body, u16_t bodySize, u16_t port);
+#endif //ZM_ENABLE_CENC
+extern void zfLnxWatchDogNotify(zdev_t* dev);
+extern void zfLnxRecv80211(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* addInfo);
+extern u8_t zfLnxCreateThread(zdev_t *dev);
+
+/******************************************************************************
+*                        P U B L I C   D A T A
+*******************************************************************************
+*/
+
+/* Definition of Wireless Extension */
+
+#if WIRELESS_EXT > 12
+#include <net/iw_handler.h>
+#endif
+//wireless extension helper functions
+extern int usbdrv_ioctl_setessid(struct net_device *dev, struct iw_point *erq);
+extern int usbdrv_ioctl_setrts(struct net_device *dev, struct iw_param *rrq);
+/* Wireless Extension Handler functions */
+extern int usbdrvwext_giwname(struct net_device *dev, struct iw_request_info *info,
+        union iwreq_data *wrq, char *extra);
+extern int usbdrvwext_siwfreq(struct net_device *dev, struct iw_request_info *info,
+        struct iw_freq *freq, char *extra);
+extern int usbdrvwext_giwfreq(struct net_device *dev, struct iw_request_info *info,
+        struct iw_freq *freq, char *extra);
+extern int usbdrvwext_siwmode(struct net_device *dev, struct iw_request_info *info,
+        union iwreq_data *wrq, char *extra);
+extern int usbdrvwext_giwmode(struct net_device *dev, struct iw_request_info *info,
+        __u32 *mode, char *extra);
+extern int usbdrvwext_siwsens(struct net_device *dev, struct iw_request_info *info,
+		struct iw_param *sens, char *extra);
+extern int usbdrvwext_giwsens(struct net_device *dev, struct iw_request_info *info,
+		struct iw_param *sens, char *extra);
+extern int usbdrvwext_giwrange(struct net_device *dev, struct iw_request_info *info,
+        struct iw_point *data, char *extra);
+extern int usbdrvwext_siwap(struct net_device *dev, struct iw_request_info *info,
+        struct sockaddr *MacAddr, char *extra);
+extern int usbdrvwext_giwap(struct net_device *dev, struct iw_request_info *info,
+        struct sockaddr *MacAddr, char *extra);
+extern int usbdrvwext_iwaplist(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *data, char *extra);
+extern int usbdrvwext_siwscan(struct net_device *dev, struct iw_request_info *info,
+        struct iw_point *data, char *extra);
+extern int usbdrvwext_giwscan(struct net_device *dev, struct iw_request_info *info,
+        struct iw_point *data, char *extra);
+extern int usbdrvwext_siwessid(struct net_device *dev, struct iw_request_info *info,
+        struct iw_point *essid, char *extra);
+extern int usbdrvwext_giwessid(struct net_device *dev, struct iw_request_info *info,
+        struct iw_point *essid, char *extra);
+extern int usbdrvwext_siwnickn(struct net_device *dev, struct iw_request_info *info,
+	    struct iw_point *data, char *nickname);
+extern int usbdrvwext_giwnickn(struct net_device *dev, struct iw_request_info *info,
+	    struct iw_point *data, char *nickname);
+extern int usbdrvwext_siwrate(struct net_device *dev, struct iw_request_info *info,
+        struct iw_param *frq, char *extra);
+extern int usbdrvwext_giwrate(struct net_device *dev, struct iw_request_info *info,
+        struct iw_param *frq, char *extra);
+extern int usbdrvwext_siwrts(struct net_device *dev, struct iw_request_info *info,
+        struct iw_param *rts, char *extra);
+extern int usbdrvwext_giwrts(struct net_device *dev, struct iw_request_info *info,
+        struct iw_param *rts, char *extra);
+extern int usbdrvwext_siwfrag(struct net_device *dev, struct iw_request_info *info,
+        struct iw_param *frag, char *extra);
+extern int usbdrvwext_giwfrag(struct net_device *dev, struct iw_request_info *info,
+        struct iw_param *frag, char *extra);
+extern int usbdrvwext_siwtxpow(struct net_device *dev, struct iw_request_info *info,
+		struct iw_param *rrq, char *extra);
+extern int usbdrvwext_giwtxpow(struct net_device *dev, struct iw_request_info *info,
+		struct iw_param *rrq, char *extra);
+extern int usbdrvwext_siwretry(struct net_device *dev, struct iw_request_info *info,
+	    struct iw_param *rrq, char *extra);
+extern int usbdrvwext_giwretry(struct net_device *dev, struct iw_request_info *info,
+	    struct iw_param *rrq, char *extra);
+extern int usbdrvwext_siwencode(struct net_device *dev, struct iw_request_info *info,
+        struct iw_point *erq, char *key);
+extern int usbdrvwext_giwencode(struct net_device *dev, struct iw_request_info *info,
+        struct iw_point *erq, char *key);
+extern int usbdrvwext_siwpower(struct net_device *dev, struct iw_request_info *info,
+        struct iw_param *frq, char *extra);
+extern int usbdrvwext_giwpower(struct net_device *dev, struct iw_request_info *info,
+        struct iw_param *frq, char *extra);
+extern int usbdrv_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd);
+/*
+ * Structures to export the Wireless Handlers
+ */
+
+struct iw_priv_args usbdrv_private_args[] = {
+//    { SIOCIWFIRSTPRIV + 0x0, 0, 0, "list_bss" },
+//    { SIOCIWFIRSTPRIV + 0x1, 0, 0, "card_reset" },
+    { SIOCIWFIRSTPRIV + 0x2, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "set_auth" },  /* 0 - open, 1 - shared key */
+    { SIOCIWFIRSTPRIV + 0x3, 0, IW_PRIV_TYPE_CHAR | 12, "get_auth" },
+//    { SIOCIWFIRSTPRIV + 0x4, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "set_preamble" },  /* 0 - long, 1 - short */
+//    { SIOCIWFIRSTPRIV + 0x5, 0, IW_PRIV_TYPE_CHAR | 6, "get_preamble" },
+//    { SIOCIWFIRSTPRIV + 0x6, 0, 0, "cnt" },
+//    { SIOCIWFIRSTPRIV + 0x7, 0, 0, "regs" },
+//    { SIOCIWFIRSTPRIV + 0x8, 0, 0, "probe" },
+//    { SIOCIWFIRSTPRIV + 0x9, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "dbg_flag" },
+//    { SIOCIWFIRSTPRIV + 0xA, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "connect" },
+//    { SIOCIWFIRSTPRIV + 0xB, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "set_mac_mode" },
+//    { SIOCIWFIRSTPRIV + 0xC, 0, IW_PRIV_TYPE_CHAR | 12, "get_mac_mode" },
+};
+
+#if WIRELESS_EXT > 12
+static iw_handler usbdrvwext_handler[] = {
+    (iw_handler) NULL,                              /* SIOCSIWCOMMIT */
+    (iw_handler) usbdrvwext_giwname,                /* SIOCGIWNAME */
+    (iw_handler) NULL,                              /* SIOCSIWNWID */
+    (iw_handler) NULL,                              /* SIOCGIWNWID */
+    (iw_handler) usbdrvwext_siwfreq,                /* SIOCSIWFREQ */
+    (iw_handler) usbdrvwext_giwfreq,                /* SIOCGIWFREQ */
+    (iw_handler) usbdrvwext_siwmode,                /* SIOCSIWMODE */
+    (iw_handler) usbdrvwext_giwmode,                /* SIOCGIWMODE */
+    (iw_handler) usbdrvwext_siwsens,                /* SIOCSIWSENS */
+    (iw_handler) usbdrvwext_giwsens,                /* SIOCGIWSENS */
+    (iw_handler) NULL, /* not used */               /* SIOCSIWRANGE */
+    (iw_handler) usbdrvwext_giwrange,               /* SIOCGIWRANGE */
+    (iw_handler) NULL, /* not used */               /* SIOCSIWPRIV */
+    (iw_handler) NULL, /* kernel code */            /* SIOCGIWPRIV */
+    (iw_handler) NULL, /* not used */               /* SIOCSIWSTATS */
+    (iw_handler) NULL, /* kernel code */            /* SIOCGIWSTATS */
+    (iw_handler) NULL,                              /* SIOCSIWSPY */
+    (iw_handler) NULL,                              /* SIOCGIWSPY */
+    (iw_handler) NULL,                              /* -- hole -- */
+    (iw_handler) NULL,                              /* -- hole -- */
+    (iw_handler) usbdrvwext_siwap,                  /* SIOCSIWAP */
+    (iw_handler) usbdrvwext_giwap,                  /* SIOCGIWAP */
+    (iw_handler) NULL,              /* -- hole -- */
+    (iw_handler) usbdrvwext_iwaplist,               /* SIOCGIWAPLIST */
+#if WIRELESS_EXT > 13
+    (iw_handler) usbdrvwext_siwscan,                /* SIOCSIWSCAN */
+    (iw_handler) usbdrvwext_giwscan,                /* SIOCGIWSCAN */
+#else /* WIRELESS_EXT > 13 */
+    (iw_handler) NULL, /* null */                   /* SIOCSIWSCAN */
+    (iw_handler) NULL, /* null */                   /* SIOCGIWSCAN */
+#endif /* WIRELESS_EXT > 13 */
+    (iw_handler) usbdrvwext_siwessid,               /* SIOCSIWESSID */
+    (iw_handler) usbdrvwext_giwessid,               /* SIOCGIWESSID */
+
+    (iw_handler) usbdrvwext_siwnickn,               /* SIOCSIWNICKN */
+    (iw_handler) usbdrvwext_giwnickn,               /* SIOCGIWNICKN */
+    (iw_handler) NULL,                              /* -- hole -- */
+    (iw_handler) NULL,                              /* -- hole -- */
+    (iw_handler) usbdrvwext_siwrate,                /* SIOCSIWRATE */
+    (iw_handler) usbdrvwext_giwrate,                /* SIOCGIWRATE */
+    (iw_handler) usbdrvwext_siwrts,                 /* SIOCSIWRTS */
+    (iw_handler) usbdrvwext_giwrts,                 /* SIOCGIWRTS */
+    (iw_handler) usbdrvwext_siwfrag,                /* SIOCSIWFRAG */
+    (iw_handler) usbdrvwext_giwfrag,                /* SIOCGIWFRAG */
+    (iw_handler) usbdrvwext_siwtxpow,               /* SIOCSIWTXPOW */
+    (iw_handler) usbdrvwext_giwtxpow,               /* SIOCGIWTXPOW */
+    (iw_handler) usbdrvwext_siwretry,               /* SIOCSIWRETRY */
+    (iw_handler) usbdrvwext_giwretry,               /* SIOCGIWRETRY */
+    (iw_handler) usbdrvwext_siwencode,              /* SIOCSIWENCODE */
+    (iw_handler) usbdrvwext_giwencode,              /* SIOCGIWENCODE */
+    (iw_handler) usbdrvwext_siwpower,               /* SIOCSIWPOWER */
+    (iw_handler) usbdrvwext_giwpower,               /* SIOCGIWPOWER */
+};
+
+static const iw_handler usbdrv_private_handler[] =
+{
+	//(iw_handler) usbdrvwext_setparam,		/* SIOCWFIRSTPRIV+0 */
+	//(iw_handler) usbdrvwext_getparam,		/* SIOCWFIRSTPRIV+1 */
+	//(iw_handler) usbdrvwext_setkey,		    /* SIOCWFIRSTPRIV+2 */
+	//(iw_handler) usbdrvwext_setwmmparams,	/* SIOCWFIRSTPRIV+3 */
+	//(iw_handler) usbdrvwext_delkey,		    /* SIOCWFIRSTPRIV+4 */
+	//(iw_handler) usbdrvwext_getwmmparams,	/* SIOCWFIRSTPRIV+5 */
+	//(iw_handler) usbdrvwext_setmlme,		/* SIOCWFIRSTPRIV+6 */
+	//(iw_handler) usbdrvwext_getchaninfo,	/* SIOCWFIRSTPRIV+7 */
+	//(iw_handler) usbdrvwext_setoptie,		/* SIOCWFIRSTPRIV+8 */
+	//(iw_handler) usbdrvwext_getoptie,		/* SIOCWFIRSTPRIV+9 */
+	//(iw_handler) usbdrvwext_addmac,		    /* SIOCWFIRSTPRIV+10 */
+	//(iw_handler) usbdrvwext_getscanresults,	/* SIOCWFIRSTPRIV+11 */
+	//(iw_handler) usbdrvwext_delmac,		    /* SIOCWFIRSTPRIV+12 */
+	//(iw_handler) usbdrvwext_getchanlist,	/* SIOCWFIRSTPRIV+13 */
+	//(iw_handler) usbdrvwext_setchanlist,	/* SIOCWFIRSTPRIV+14 */
+	//(iw_handler) NULL,				        /* SIOCWFIRSTPRIV+15 */
+	//(iw_handler) usbdrvwext_chanswitch,	    /* SIOCWFIRSTPRIV+16 */
+	//(iw_handler) usbdrvwext_setmode,		/* SIOCWFIRSTPRIV+17 */
+	//(iw_handler) usbdrvwext_getmode,		/* SIOCWFIRSTPRIV+18 */
+    NULL,               /* SIOCIWFIRSTPRIV */
+};
+
+static struct iw_handler_def p80211wext_handler_def = {
+    .num_standard = sizeof(usbdrvwext_handler) / sizeof(iw_handler),
+    .num_private = sizeof(usbdrv_private_handler)/sizeof(iw_handler),
+    .num_private_args = sizeof(usbdrv_private_args)/sizeof(struct iw_priv_args),
+    .standard = usbdrvwext_handler,
+    .private = (iw_handler *) usbdrv_private_handler,
+    .private_args = (struct iw_priv_args *) usbdrv_private_args
+};
+#endif
+
+/* WDS */
+//struct zsWdsStruct wds[ZM_WDS_PORT_NUMBER];
+//void zfInitWdsStruct(void);
+
+/* VAP */
+struct zsVapStruct vap[ZM_VAP_PORT_NUMBER];
+void zfLnxInitVapStruct(void);
+
+
+/**
+ * usbdrv_intr - interrupt handler
+ * @irq: the IRQ number
+ * @dev_inst: the net_device struct
+ * @regs: registers (unused)
+ *
+ * This routine is the ISR for the usbdrv board. It services
+ * the RX & TX queues & starts the RU if it has stopped due
+ * to no resources.
+ */
+irqreturn_t usbdrv_intr(int irq, void *dev_inst, struct pt_regs *regs)
+{
+    struct net_device *dev;
+    struct usbdrv_private *macp;
+
+    dev = dev_inst;
+    macp = dev->ml_priv;
+
+
+    /* Read register error, card may be unpluged */
+    if (0)//(intr_status == -1)
+        return IRQ_NONE;
+
+    /* the device is closed, don't continue or else bad things may happen. */
+    if (!netif_running(dev)) {
+        return IRQ_NONE;
+    }
+
+    if (macp->driver_isolated) {
+        return IRQ_NONE;
+    }
+
+#if (WLAN_HOSTIF == WLAN_PCI)
+    //zfiIsrPci(dev);
+#endif
+
+    return IRQ_HANDLED;
+}
+
+int usbdrv_open(struct net_device *dev)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    int rc = 0;
+    u16_t size;
+    void* mem;
+    //unsigned char addr[6];
+    struct zsCbFuncTbl cbFuncTbl;
+
+    printk("Enter open()\n");
+
+//#ifndef CONFIG_SMP
+//    read_lock(&(macp->isolate_lock));
+//#endif
+    if (macp->driver_isolated) {
+        rc = -EBUSY;
+        goto exit;
+    }
+
+    size = zfiGlobalDataSize(dev);
+    if ((mem = kmalloc(size, GFP_KERNEL)) == NULL)
+    {
+        rc = -EBUSY;
+        goto exit;
+    }
+    macp->wd = mem;
+
+    memset(&cbFuncTbl, 0, sizeof(struct zsCbFuncTbl));
+    cbFuncTbl.zfcbAuthNotify = zfLnxAuthNotify;
+    cbFuncTbl.zfcbAuthNotify = zfLnxAuthNotify;
+    cbFuncTbl.zfcbAsocNotify = zfLnxAsocNotify;
+    cbFuncTbl.zfcbDisAsocNotify = zfLnxDisAsocNotify;
+    cbFuncTbl.zfcbApConnectNotify = zfLnxApConnectNotify;
+    cbFuncTbl.zfcbConnectNotify = zfLnxConnectNotify;
+    cbFuncTbl.zfcbScanNotify = zfLnxScanNotify;
+    cbFuncTbl.zfcbMicFailureNotify = zfLnxMicFailureNotify;
+    cbFuncTbl.zfcbApMicFailureNotify = zfLnxApMicFailureNotify;
+    cbFuncTbl.zfcbIbssPartnerNotify = zfLnxIbssPartnerNotify;
+    cbFuncTbl.zfcbMacAddressNotify = zfLnxMacAddressNotify;
+    cbFuncTbl.zfcbSendCompleteIndication = zfLnxSendCompleteIndication;
+    cbFuncTbl.zfcbRecvEth = zfLnxRecvEth;
+    cbFuncTbl.zfcbRecv80211 = zfLnxRecv80211;
+    cbFuncTbl.zfcbRestoreBufData = zfLnxRestoreBufData;
+#ifdef ZM_ENABLE_CENC
+    cbFuncTbl.zfcbCencAsocNotify = zfLnxCencAsocNotify;
+#endif //ZM_ENABLE_CENC
+    cbFuncTbl.zfcbHwWatchDogNotify = zfLnxWatchDogNotify;
+    zfiWlanOpen(dev, &cbFuncTbl);
+
+#if 0
+    {
+        //u16_t mac[3] = {0x1300, 0xb6d4, 0x5aaf};
+        u16_t mac[3] = {0x8000, 0x00ab, 0x0000};
+        //zfiWlanSetMacAddress(dev, mac);
+    }
+    /* MAC address */
+    zfiWlanQueryMacAddress(dev, addr);
+    dev->dev_addr[0] = addr[0];
+    dev->dev_addr[1] = addr[1];
+    dev->dev_addr[2] = addr[2];
+    dev->dev_addr[3] = addr[3];
+    dev->dev_addr[4] = addr[4];
+    dev->dev_addr[5] = addr[5];
+#endif
+    //zfwMacAddressNotify() will be called to setup dev->dev_addr[]
+
+    zfLnxCreateThread(dev);
+
+    mod_timer(&(macp->hbTimer10ms), jiffies + (1*HZ)/100);   //10 ms
+
+    netif_carrier_on(dev);
+
+    netif_start_queue(dev);
+
+#if ZM_AP_MODE == 1
+    zfiWlanSetWlanMode(dev, ZM_MODE_AP);
+    zfiWlanSetBasicRate(dev, 0xf, 0, 0);
+    zfiWlanSetSSID(dev, "OTUS_CWY", 8);
+    zfiWlanSetDtimCount(dev, 3);
+
+  #if ZM_WEP_MOME == 1
+    {
+        u8_t key[16] = {0x12, 0x34, 0x56, 0x78, 0x90};
+        struct zsKeyInfo keyInfo;
+
+        keyInfo.keyLength = 5;
+        keyInfo.keyIndex = 0;
+        keyInfo.flag = 0;
+        keyInfo.key = key;
+        zfiWlanSetKey(dev, keyInfo);
+
+        zfiWlanSetEncryMode(dev, ZM_WEP64);
+    }
+
+    #if ZM_SHARE_AUTH == 1
+    zfiWlanSetAuthenticationMode(dev, 1);
+    #endif //#if ZM_SHARE_AUTH == 1
+  #endif //#if ZM_WEP_MOME == 1
+
+#elif ZM_PIBSS_MODE == 1
+    zfiWlanSetWlanMode(dev, ZM_MODE_PSEUDO);
+#else
+    zfiWlanSetWlanMode(dev, ZM_MODE_INFRASTRUCTURE);
+#endif
+    //zfiWlanSetChannel(dev, ZM_CHANNEL, FALSE);
+    zfiWlanSetFrequency(dev, 2462000, FALSE);
+    zfiWlanSetRtsThreshold(dev, 32767);
+    zfiWlanSetFragThreshold(dev, 0);
+
+    zfiWlanEnable(dev);
+
+#ifdef ZM_ENABLE_CENC
+    macp->netlink_sk = netlink_kernel_create(NETLINK_USERSOCK, 1, NULL, THIS_MODULE);
+
+    if (macp->netlink_sk == NULL)
+    {
+        printk(KERN_ERR "Can't create NETLINK socket\n");
+    }
+#endif
+
+    macp->DeviceOpened = 1;
+exit:
+//#ifndef CONFIG_SMP
+//    read_unlock(&(macp->isolate_lock));
+//#endif
+    //zfRegisterWdsDev(dev, 0);
+    //zfLnxRegisterVapDev(dev, 0);
+
+    return rc;
+}
+
+
+
+
+/**
+ * usbdrv_get_stats - get driver statistics
+ * @dev: adapter's net_device struct
+ *
+ * This routine is called when the OS wants the adapter's stats returned.
+ * It returns the address of the net_device_stats stucture for the device.
+ * If the statistics are currently being updated, then they might be incorrect
+ * for a short while. However, since this cannot actually cause damage, no
+ * locking is used.
+ */
+
+struct net_device_stats * usbdrv_get_stats(struct net_device *dev)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    macp->drv_stats.net_stats.tx_errors =
+        macp->drv_stats.net_stats.tx_carrier_errors +
+        macp->drv_stats.net_stats.tx_aborted_errors;
+
+    macp->drv_stats.net_stats.rx_errors =
+        macp->drv_stats.net_stats.rx_crc_errors +
+        macp->drv_stats.net_stats.rx_frame_errors +
+        macp->drv_stats.net_stats.rx_length_errors;
+
+
+    return &(macp->drv_stats.net_stats);
+}
+
+
+/**
+ * usbdrv_set_mac - set the MAC address
+ * @dev: adapter's net_device struct
+ * @addr: the new address
+ *
+ * This routine sets the ethernet address of the board
+ * Returns:
+ * 0  - if successful
+ * -1 - otherwise
+ */
+
+int usbdrv_set_mac(struct net_device *dev, void *addr)
+{
+    struct usbdrv_private *macp;
+    int rc = -1;
+
+    macp = dev->ml_priv;
+    read_lock(&(macp->isolate_lock));
+
+    if (macp->driver_isolated) {
+        goto exit;
+    }
+
+    rc = 0;
+
+
+exit:
+    read_unlock(&(macp->isolate_lock));
+    return rc;
+}
+
+
+
+void
+usbdrv_isolate_driver(struct usbdrv_private *macp)
+{
+#ifndef CONFIG_SMP
+    write_lock_irq(&(macp->isolate_lock));
+#endif
+    macp->driver_isolated = TRUE;
+#ifndef CONFIG_SMP
+    write_unlock_irq(&(macp->isolate_lock));
+#endif
+
+    if (netif_running(macp->device))
+    {
+        netif_carrier_off(macp->device);
+        netif_stop_queue(macp->device);
+    }
+}
+
+#define VLAN_SIZE   	4
+int usbdrv_change_mtu(struct net_device *dev, int new_mtu)
+{
+    if ((new_mtu < 68) || (new_mtu > (ETH_DATA_LEN + VLAN_SIZE)))
+        return -EINVAL;
+
+    dev->mtu = new_mtu;
+    return 0;
+}
+
+void zfLnxUnlinkAllUrbs(struct usbdrv_private *macp);
+
+int usbdrv_close(struct net_device *dev)
+{
+extern void zfHpLedCtrl(struct net_device *dev, u16_t ledId, u8_t mode);
+
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    printk(KERN_DEBUG "usbdrv_close\n");
+
+    netif_carrier_off(macp->device);
+
+    del_timer_sync(&macp->hbTimer10ms);
+
+    printk(KERN_DEBUG "usbdrv_netif_carrier_off\n");
+
+    usbdrv_isolate_driver(macp);
+
+    printk(KERN_DEBUG "usbdrv_isolate_driver\n");
+
+    netif_carrier_off(macp->device);
+#ifdef ZM_ENABLE_CENC
+    /* CENC */
+    if (macp->netlink_sk != NULL)
+    {
+    //    sock_release(macp->netlink_sk);
+        printk(KERN_ERR "usbdrv close netlink socket\n");
+    }
+#endif //ZM_ENABLE_CENC
+#if (WLAN_HOSTIF == WLAN_PCI)
+    //free_irq(dev->irq, dev);
+#endif
+
+    /* Turn off LED */
+    zfHpLedCtrl(dev, 0, 0);
+    zfHpLedCtrl(dev, 1, 0);
+
+    /* Delay for a while */
+    mdelay(10);
+
+    /* clear WPA/RSN IE */
+    macp->supIe[1] = 0;
+
+    /* set the isolate flag to false, so usbdrv_open can be called */
+    macp->driver_isolated = FALSE;
+
+    zfiWlanClose(dev);
+    kfree(macp->wd);
+
+    zfLnxUnlinkAllUrbs(macp);
+
+    return 0;
+}
+
+
+
+
+int usbdrv_xmit_frame(struct sk_buff *skb, struct net_device *dev)
+{
+    int notify_stop = FALSE;
+    struct usbdrv_private *macp = dev->ml_priv;
+
+#if 0
+    /* Test code */
+    {
+        struct sk_buff* s;
+
+        s = skb_copy_expand(skb, 8, 0, GFP_ATOMIC);
+        skb_push(s, 8);
+        s->data[0] = 'z';
+        s->data[1] = 'y';
+        s->data[2] = 'd';
+        s->data[3] = 'a';
+        s->data[4] = 's';
+        printk("len1=%d, len2=%d", skb->len, s->len);
+        netlink_broadcast(rtnl, s, 0, RTMGRP_LINK, GFP_ATOMIC);
+    }
+#endif
+
+#if ZM_DISABLE_XMIT
+    dev_kfree_skb_irq(skb);
+#else
+    zfiTxSendEth(dev, skb, 0);
+#endif
+    macp->drv_stats.net_stats.tx_bytes += skb->len;
+    macp->drv_stats.net_stats.tx_packets++;
+
+    //dev_kfree_skb_irq(skb);
+
+    if (notify_stop) {
+        netif_carrier_off(dev);
+        netif_stop_queue(dev);
+    }
+
+    return 0;
+}
+
+
+
+
+void usbdrv_set_multi(struct net_device *dev)
+{
+
+
+    if (!(dev->flags & IFF_UP))
+        return;
+
+        return;
+
+}
+
+
+
+/**
+ * usbdrv_clear_structs - free resources
+
+ * @dev: adapter's net_device struct
+ *
+ * Free all device specific structs, unmap i/o address, etc.
+ */
+void usbdrv_clear_structs(struct net_device *dev)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+
+
+#if (WLAN_HOSTIF == WLAN_PCI)
+    iounmap(macp->regp);
+
+    pci_release_regions(macp->pdev);
+    pci_disable_device(macp->pdev);
+    pci_set_drvdata(macp->pdev, NULL);
+#endif
+
+    kfree(macp);
+
+    kfree(dev);
+
+}
+
+void usbdrv_remove1(struct pci_dev *pcid)
+{
+    struct net_device *dev;
+    struct usbdrv_private *macp;
+
+    if (!(dev = (struct net_device *) pci_get_drvdata(pcid)))
+        return;
+
+    macp = dev->ml_priv;
+    unregister_netdev(dev);
+
+    usbdrv_clear_structs(dev);
+}
+
+
+void zfLnx10msTimer(struct net_device* dev)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    mod_timer(&(macp->hbTimer10ms), jiffies + (1*HZ)/100);   //10 ms
+    zfiHeartBeat(dev);
+    return;
+}
+
+void zfLnxInitVapStruct(void)
+{
+    u16_t i;
+
+    for (i=0; i<ZM_VAP_PORT_NUMBER; i++)
+    {
+        vap[i].dev = NULL;
+        vap[i].openFlag = 0;
+    }
+}
+
+int zfLnxVapOpen(struct net_device *dev)
+{
+    u16_t vapId;
+
+    vapId = zfLnxGetVapId(dev);
+
+    if (vap[vapId].openFlag == 0)
+    {
+        vap[vapId].openFlag = 1;
+    	printk("zfLnxVapOpen : device name=%s, vap ID=%d\n", dev->name, vapId);
+    	zfiWlanSetSSID(dev, "vap1", 4);
+    	zfiWlanEnable(dev);
+    	netif_start_queue(dev);
+    }
+    else
+    {
+        printk("VAP opened error : vap ID=%d\n", vapId);
+    }
+	return 0;
+}
+
+int zfLnxVapClose(struct net_device *dev)
+{
+    u16_t vapId;
+
+    vapId = zfLnxGetVapId(dev);
+
+    if (vapId != 0xffff)
+    {
+        if (vap[vapId].openFlag == 1)
+        {
+            printk("zfLnxVapClose: device name=%s, vap ID=%d\n", dev->name, vapId);
+
+            netif_stop_queue(dev);
+            vap[vapId].openFlag = 0;
+        }
+        else
+        {
+            printk("VAP port was not opened : vap ID=%d\n", vapId);
+        }
+    }
+	return 0;
+}
+
+int zfLnxVapXmitFrame(struct sk_buff *skb, struct net_device *dev)
+{
+    int notify_stop = FALSE;
+    struct usbdrv_private *macp = dev->ml_priv;
+    u16_t vapId;
+
+    vapId = zfLnxGetVapId(dev);
+    //printk("zfLnxVapXmitFrame: vap ID=%d\n", vapId);
+    //printk("zfLnxVapXmitFrame(), skb=%lxh\n", (u32_t)skb);
+
+    if (vapId >= ZM_VAP_PORT_NUMBER)
+    {
+        dev_kfree_skb_irq(skb);
+        return 0;
+    }
+#if 1
+    if (vap[vapId].openFlag == 0)
+    {
+        dev_kfree_skb_irq(skb);
+        return 0;
+    }
+#endif
+
+
+    zfiTxSendEth(dev, skb, 0x1);
+
+    macp->drv_stats.net_stats.tx_bytes += skb->len;
+    macp->drv_stats.net_stats.tx_packets++;
+
+    //dev_kfree_skb_irq(skb);
+
+    if (notify_stop) {
+        netif_carrier_off(dev);
+        netif_stop_queue(dev);
+    }
+
+    return 0;
+}
+
+int zfLnxRegisterVapDev(struct net_device* parentDev, u16_t vapId)
+{
+    /* Allocate net device structure */
+    vap[vapId].dev = alloc_etherdev(0);
+    printk("Register vap dev=%x\n", (u32_t)vap[vapId].dev);
+
+    if(vap[vapId].dev == NULL) {
+        printk("alloc_etherdev fail\n");
+        return -ENOMEM;
+    }
+
+    /* Setup the default settings */
+    ether_setup(vap[vapId].dev);
+
+    /* MAC address */
+    memcpy(vap[vapId].dev->dev_addr, parentDev->dev_addr, ETH_ALEN);
+
+    vap[vapId].dev->irq = parentDev->irq;
+    vap[vapId].dev->base_addr = parentDev->base_addr;
+    vap[vapId].dev->mem_start = parentDev->mem_start;
+    vap[vapId].dev->mem_end = parentDev->mem_end;
+    vap[vapId].dev->ml_priv = parentDev->ml_priv;
+
+    //dev->hard_start_xmit = &zd1212_wds_xmit_frame;
+    vap[vapId].dev->hard_start_xmit = &zfLnxVapXmitFrame;
+    vap[vapId].dev->open = &zfLnxVapOpen;
+    vap[vapId].dev->stop = &zfLnxVapClose;
+    vap[vapId].dev->get_stats = &usbdrv_get_stats;
+    vap[vapId].dev->change_mtu = &usbdrv_change_mtu;
+#ifdef ZM_HOSTAPD_SUPPORT
+    vap[vapId].dev->do_ioctl = usbdrv_ioctl;
+#else
+    vap[vapId].dev->do_ioctl = NULL;
+#endif
+    vap[vapId].dev->destructor = free_netdev;
+
+    vap[vapId].dev->tx_queue_len = 0;
+
+    vap[vapId].dev->dev_addr[0] = parentDev->dev_addr[0];
+    vap[vapId].dev->dev_addr[1] = parentDev->dev_addr[1];
+    vap[vapId].dev->dev_addr[2] = parentDev->dev_addr[2];
+    vap[vapId].dev->dev_addr[3] = parentDev->dev_addr[3];
+    vap[vapId].dev->dev_addr[4] = parentDev->dev_addr[4];
+    vap[vapId].dev->dev_addr[5] = parentDev->dev_addr[5] + (vapId+1);
+
+    /* Stop the network queue first */
+    netif_stop_queue(vap[vapId].dev);
+
+    sprintf(vap[vapId].dev->name, "vap%d", vapId);
+    printk("Register VAP dev success : %s\n", vap[vapId].dev->name);
+
+    if(register_netdevice(vap[vapId].dev) != 0) {
+        printk("register VAP device fail\n");
+        vap[vapId].dev = NULL;
+        return -EINVAL;
+    }
+
+    return 0;
+}
+
+int zfLnxUnregisterVapDev(struct net_device* parentDev, u16_t vapId)
+{
+    int ret = 0;
+
+    printk("Unregister VAP dev : %s\n", vap[vapId].dev->name);
+
+    if(vap[vapId].dev != NULL) {
+        printk("Unregister vap dev=%x\n", (u32_t)vap[vapId].dev);
+        //
+        //unregister_netdevice(wds[wdsId].dev);
+        unregister_netdev(vap[vapId].dev);
+
+        printk("VAP unregister_netdevice\n");
+        vap[vapId].dev = NULL;
+    }
+    else {
+        printk("unregister VAP device: %d fail\n", vapId);
+        ret = -EINVAL;
+    }
+
+    return ret;
+}
+
+
+
+#  define SUBMIT_URB(u,f)       usb_submit_urb(u,f)
+#  define USB_ALLOC_URB(u,f)    usb_alloc_urb(u,f)
+
+//extern void zfiWlanQueryMacAddress(zdev_t* dev, u8_t* addr);
+
+extern int usbdrv_open(struct net_device *dev);
+extern int usbdrv_close(struct net_device *dev);
+extern int usbdrv_xmit_frame(struct sk_buff *skb, struct net_device *dev);
+extern int usbdrv_xmit_frame(struct sk_buff *skb, struct net_device *dev);
+extern int usbdrv_change_mtu(struct net_device *dev, int new_mtu);
+extern void usbdrv_set_multi(struct net_device *dev);
+extern int usbdrv_set_mac(struct net_device *dev, void *addr);
+extern struct net_device_stats * usbdrv_get_stats(struct net_device *dev);
+extern int usbdrv_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd);
+extern UsbTxQ_t *zfLnxGetUsbTxBuffer(struct net_device *dev);
+
+int zfLnxAllocAllUrbs(struct usbdrv_private *macp)
+{
+    struct usb_interface *interface = macp->interface;
+    struct usb_host_interface *iface_desc = &interface->altsetting[0];
+
+    struct usb_endpoint_descriptor *endpoint;
+    int i;
+
+    /* descriptor matches, let's find the endpoints needed */
+    /* check out the endpoints */
+    for (i = 0; i < iface_desc->desc.bNumEndpoints; ++i)
+    {
+        endpoint = &iface_desc->endpoint[i].desc;
+        if ((endpoint->bEndpointAddress & 0x80) &&
+            ((endpoint->bmAttributes & 3) == 0x02))
+        {
+            /* we found a bulk in endpoint */
+            printk(KERN_ERR "bulk in: wMaxPacketSize = %x\n", le16_to_cpu(endpoint->wMaxPacketSize));
+        }
+
+        if (((endpoint->bEndpointAddress & 0x80) == 0x00) &&
+            ((endpoint->bmAttributes & 3) == 0x02))
+        {
+            /* we found a bulk out endpoint */
+            printk(KERN_ERR "bulk out: wMaxPacketSize = %x\n", le16_to_cpu(endpoint->wMaxPacketSize));
+        }
+
+        if ((endpoint->bEndpointAddress & 0x80) &&
+            ((endpoint->bmAttributes & 3) == 0x03))
+        {
+            /* we found a interrupt in endpoint */
+            printk(KERN_ERR "interrupt in: wMaxPacketSize = %x\n", le16_to_cpu(endpoint->wMaxPacketSize));
+            printk(KERN_ERR "interrupt in: int_interval = %d\n", endpoint->bInterval);
+        }
+
+        if (((endpoint->bEndpointAddress & 0x80) == 0x00) &&
+            ((endpoint->bmAttributes & 3) == 0x03))
+        {
+            /* we found a interrupt out endpoint */
+            printk(KERN_ERR "interrupt out: wMaxPacketSize = %x\n", le16_to_cpu(endpoint->wMaxPacketSize));
+            printk(KERN_ERR "interrupt out: int_interval = %d\n", endpoint->bInterval);
+        }
+    }
+
+    /* Allocate all Tx URBs */
+    for (i = 0; i < ZM_MAX_TX_URB_NUM; i++)
+    {
+        macp->WlanTxDataUrb[i] = USB_ALLOC_URB(0, GFP_KERNEL);
+
+        if (macp->WlanTxDataUrb[i] == 0)
+        {
+            int j;
+
+            /* Free all urbs */
+            for (j = 0; j < i; j++)
+            {
+                usb_free_urb(macp->WlanTxDataUrb[j]);
+            }
+
+            return 0;
+        }
+    }
+
+    /* Allocate all Rx URBs */
+    for (i = 0; i < ZM_MAX_RX_URB_NUM; i++)
+    {
+        macp->WlanRxDataUrb[i] = USB_ALLOC_URB(0, GFP_KERNEL);
+
+        if (macp->WlanRxDataUrb[i] == 0)
+        {
+            int j;
+
+            /* Free all urbs */
+            for (j = 0; j < i; j++)
+            {
+                usb_free_urb(macp->WlanRxDataUrb[j]);
+            }
+
+            for (j = 0; j < ZM_MAX_TX_URB_NUM; j++)
+            {
+                usb_free_urb(macp->WlanTxDataUrb[j]);
+            }
+
+            return 0;
+        }
+    }
+
+    /* Allocate Register Read/Write USB */
+    macp->RegOutUrb = USB_ALLOC_URB(0, GFP_KERNEL);
+    macp->RegInUrb = USB_ALLOC_URB(0, GFP_KERNEL);
+
+    return 1;
+}
+
+void zfLnxFreeAllUrbs(struct usbdrv_private *macp)
+{
+    int i;
+
+    /* Free all Tx URBs */
+    for (i = 0; i < ZM_MAX_TX_URB_NUM; i++)
+    {
+        if (macp->WlanTxDataUrb[i] != NULL)
+        {
+            usb_free_urb(macp->WlanTxDataUrb[i]);
+        }
+    }
+
+    /* Free all Rx URBs */
+    for (i = 0; i < ZM_MAX_RX_URB_NUM; i++)
+    {
+        if (macp->WlanRxDataUrb[i] != NULL)
+        {
+            usb_free_urb(macp->WlanRxDataUrb[i]);
+        }
+    }
+
+    /* Free USB Register Read/Write URB */
+    usb_free_urb(macp->RegOutUrb);
+    usb_free_urb(macp->RegInUrb);
+}
+
+void zfLnxUnlinkAllUrbs(struct usbdrv_private *macp)
+{
+    int i;
+
+    /* Unlink all Tx URBs */
+    for (i = 0; i < ZM_MAX_TX_URB_NUM; i++)
+    {
+        if (macp->WlanTxDataUrb[i] != NULL)
+        {
+            usb_unlink_urb(macp->WlanTxDataUrb[i]);
+        }
+    }
+
+    /* Unlink all Rx URBs */
+    for (i = 0; i < ZM_MAX_RX_URB_NUM; i++)
+    {
+        if (macp->WlanRxDataUrb[i] != NULL)
+        {
+            usb_unlink_urb(macp->WlanRxDataUrb[i]);
+        }
+    }
+
+    /* Unlink USB Register Read/Write URB */
+    usb_unlink_urb(macp->RegOutUrb);
+
+    usb_unlink_urb(macp->RegInUrb);
+}
+
+u8_t zfLnxInitSetup(struct net_device *dev, struct usbdrv_private *macp)
+{
+    //unsigned char addr[6];
+
+    //init_MUTEX(&macp->ps_sem);
+    //init_MUTEX(&macp->reg_sem);
+    //init_MUTEX(&macp->bcn_sem);
+    //init_MUTEX(&macp->config_sem);
+
+    spin_lock_init(&(macp->cs_lock));
+#if 0
+    /* MAC address */
+    zfiWlanQueryMacAddress(dev, addr);
+    dev->dev_addr[0] = addr[0];
+    dev->dev_addr[1] = addr[1];
+    dev->dev_addr[2] = addr[2];
+    dev->dev_addr[3] = addr[3];
+    dev->dev_addr[4] = addr[4];
+    dev->dev_addr[5] = addr[5];
+#endif
+#if WIRELESS_EXT > 12
+    dev->wireless_handlers = (struct iw_handler_def *)&p80211wext_handler_def;
+#endif
+
+    dev->open = usbdrv_open;
+    dev->hard_start_xmit = usbdrv_xmit_frame;
+    dev->stop = usbdrv_close;
+    dev->change_mtu = &usbdrv_change_mtu;
+    dev->get_stats = usbdrv_get_stats;
+    dev->set_multicast_list = usbdrv_set_multi;
+    dev->set_mac_address = usbdrv_set_mac;
+    dev->do_ioctl = usbdrv_ioctl;
+
+    dev->flags |= IFF_MULTICAST;
+
+    dev->dev_addr[0] = 0x00;
+    dev->dev_addr[1] = 0x03;
+    dev->dev_addr[2] = 0x7f;
+    dev->dev_addr[3] = 0x11;
+    dev->dev_addr[4] = 0x22;
+    dev->dev_addr[5] = 0x33;
+
+    /* Initialize Heart Beat timer */
+    init_timer(&macp->hbTimer10ms);
+    macp->hbTimer10ms.data = (unsigned long)dev;
+    macp->hbTimer10ms.function = (void *)&zfLnx10msTimer;
+
+    /* Initialize WDS and VAP data structure */
+    //zfInitWdsStruct();
+    zfLnxInitVapStruct();
+
+    return 1;
+}
+
+u8_t zfLnxClearStructs(struct net_device *dev)
+{
+    u16_t ii;
+    u16_t TxQCnt;
+
+    TxQCnt = zfLnxCheckTxBufferCnt(dev);
+
+    printk(KERN_ERR "TxQCnt: %d\n", TxQCnt);
+
+    for(ii = 0; ii < TxQCnt; ii++)
+    {
+        UsbTxQ_t *TxQ = zfLnxGetUsbTxBuffer(dev);
+
+        printk(KERN_ERR "dev_kfree_skb_any\n");
+        /* Free buffer */
+        dev_kfree_skb_any(TxQ->buf);
+    }
+
+    return 0;
+}
diff --git a/drivers/staging/otus/usbdrv.h b/drivers/staging/otus/usbdrv.h
new file mode 100644
index 00000000000..a11b3b36a90
--- /dev/null
+++ b/drivers/staging/otus/usbdrv.h
@@ -0,0 +1,252 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : usbdrv.h                                              */
+/*                                                                      */
+/*  Abstract                                                            */
+/*     This module contains network interface up/down related definition*/
+/*                                                                      */
+/*  NOTES                                                               */
+/*     Platform dependent.                                              */
+/*                                                                      */
+/************************************************************************/
+
+#ifndef _USBDRV_H
+#define _USBDRV_H
+
+#define WLAN_USB    0
+#define WLAN_PCI    1
+
+#include <linux/module.h>
+#include <linux/pci.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/skbuff.h>
+#include <linux/uaccess.h>
+#include <linux/wireless.h>
+#include <linux/if_arp.h>
+#include <linux/io.h>
+
+#include "zdcompat.h"
+
+#include "oal_dt.h"
+#include "oal_marc.h"
+#include "80211core/pub_zfi.h"
+//#include "pub_zfw.h"
+#include "80211core/pub_usb.h"
+
+#include <linux/usb.h>
+/* Please include header files for device type in the beginning of this file */
+#define urb_t                       struct urb
+
+#define usb_complete_t              usb_complete_t
+#define pipe_t                      u32_t
+
+/* USB Endpoint definition */
+#define USB_WLAN_TX_PIPE                    1
+#define USB_WLAN_RX_PIPE                    2
+#define USB_REG_IN_PIPE                     3
+#define USB_REG_OUT_PIPE                    4
+
+#if (WLAN_HOSTIF == WLAN_USB)
+#include <linux/usb.h>
+#endif
+
+#ifdef ZM_HOSTAPD_SUPPORT
+#include "athr_common.h"
+#endif
+
+/**************************************************************************
+**		Descriptor Data Structure
+***************************************************************************/
+struct driver_stats {
+	struct net_device_stats net_stats;
+};
+
+#define ZM_MAX_RX_BUFFER_SIZE               8192
+
+#if ZM_USB_TX_STREAM_MODE == 1
+#define ZM_MAX_TX_AGGREGATE_NUM             4
+#define ZM_USB_TX_BUF_SIZE                  8096
+#define ZM_MAX_TX_URB_NUM                   4
+#else
+#define ZM_USB_TX_BUF_SIZE                  2048
+#define ZM_MAX_TX_URB_NUM                   8
+#endif
+#define ZM_USB_REG_MAX_BUF_SIZE             64
+#define ZM_MAX_RX_URB_NUM                   16
+#define ZM_MAX_TX_BUF_NUM                   128
+
+typedef struct UsbTxQ
+{
+    zbuf_t *buf;
+    u8_t hdr[80];
+    u16_t hdrlen;
+    u8_t snap[8];
+    u16_t snapLen;
+    u8_t tail[16];
+    u16_t tailLen;
+    u16_t offset;
+} UsbTxQ_t;
+
+
+struct zdap_ioctl {
+	u16_t cmd;                /* Command to run */
+	u32_t addr;                /* Length of the data buffer */
+	u32_t value;              /* Pointer to the data buffer */
+	u8_t	data[0x100];
+};
+
+#define ZM_OAL_MAX_STA_SUPPORT 16
+
+struct usbdrv_private
+{
+	//linux used
+	struct net_device 	*device;
+#if (WLAN_HOSTIF == WLAN_PCI)
+	struct pci_dev 		*pdev;
+#endif
+#if (WLAN_HOSTIF == WLAN_USB)
+	struct usb_device	*udev;
+	struct usb_interface    *interface;
+#endif
+	struct driver_stats drv_stats;
+	char ifname[IFNAMSIZ];
+	int	                using_dac;
+	u8_t			rev_id;		/* adapter PCI revision ID */
+	rwlock_t 		isolate_lock;
+    spinlock_t      cs_lock;
+	int 			driver_isolated;
+#if (WLAN_HOSTIF == WLAN_PCI)
+	void			*regp;
+#endif
+
+        /* timer for heart beat */
+	struct timer_list hbTimer10ms;
+
+	/* For driver core */
+	void* wd;
+
+#if (WLAN_HOSTIF == WLAN_USB)
+	u8_t                    txUsbBuf[ZM_MAX_TX_URB_NUM][ZM_USB_TX_BUF_SIZE];
+	u8_t                    regUsbReadBuf[ZM_USB_REG_MAX_BUF_SIZE];
+	u8_t                    regUsbWriteBuf[ZM_USB_REG_MAX_BUF_SIZE];
+	urb_t			*WlanTxDataUrb[ZM_MAX_TX_URB_NUM];
+	urb_t			*WlanRxDataUrb[ZM_MAX_RX_URB_NUM];
+	urb_t			*RegOutUrb;
+	urb_t			*RegInUrb;
+	UsbTxQ_t                UsbTxBufQ[ZM_MAX_TX_BUF_NUM];
+	zbuf_t                  *UsbRxBufQ[ZM_MAX_RX_URB_NUM];
+        u16_t                   TxBufHead;
+        u16_t                   TxBufTail;
+        u16_t                   TxBufCnt;
+        u16_t                   TxUrbHead;
+        u16_t                   TxUrbTail;
+        u16_t                   TxUrbCnt;
+        u16_t                   RxBufHead;
+        u16_t                   RxBufTail;
+        u16_t                   RxBufCnt;
+#endif
+
+#if ZM_USB_STREAM_MODE == 1
+        zbuf_t                  *reamin_buf;
+#endif
+
+#ifdef ZM_HOSTAPD_SUPPORT
+        struct athr_wlan_param  athr_wpa_req;
+#endif
+        struct sock             *netlink_sk;
+        u8_t            DeviceOpened; //CWYang(+)
+        u8_t            supIe[50];
+        u8_t            supLen;
+        struct ieee80211req_wpaie stawpaie[ZM_OAL_MAX_STA_SUPPORT];
+        u8_t            forwardMgmt;
+
+        struct zfCbUsbFuncTbl usbCbFunctions;
+
+        /* For keventd */
+        u32_t                   flags;
+        unsigned long           kevent_flags;
+        u16_t                   kevent_ready;
+
+        struct semaphore        ioctl_sem;
+        struct work_struct      kevent;
+        wait_queue_head_t       wait_queue_event;
+#ifdef ZM_HALPLUS_LOCK
+        unsigned long           hal_irqFlag;
+#endif
+        u16_t                   adapterState;
+};
+
+/* WDS */
+#define ZM_WDS_PORT_NUMBER  6
+
+struct zsWdsStruct
+{
+    struct net_device* dev;
+    u16_t openFlag;
+};
+
+/* VAP */
+#define ZM_VAP_PORT_NUMBER  7
+
+struct zsVapStruct
+{
+    struct net_device* dev;
+    u16_t openFlag;
+};
+
+/***************************************/
+
+#define ZM_IOCTL_REG_READ			0x01
+#define ZM_IOCTL_REG_WRITE			0x02
+#define ZM_IOCTL_MEM_DUMP			0x03
+#define ZM_IOCTL_REG_DUMP			0x05
+#define ZM_IOCTL_TXD_DUMP 			0x06
+#define ZM_IOCTL_RXD_DUMP 			0x07
+#define ZM_IOCTL_MEM_READ			0x0B
+#define ZM_IOCTL_MEM_WRITE			0x0C
+#define ZM_IOCTL_DMA_TEST           0x10
+#define ZM_IOCTL_REG_TEST           0x11
+#define ZM_IOCTL_TEST               0x80
+#define ZM_IOCTL_TALLY              0x81 //CWYang(+)
+#define ZM_IOCTL_RTS                0xA0
+#define ZM_IOCTL_MIX_MODE           0xA1
+#define ZM_IOCTL_FRAG               0xA2
+#define ZM_IOCTL_SCAN               0xA3
+#define ZM_IOCTL_KEY                0xA4
+#define ZM_IOCTL_RATE               0xA5
+#define ZM_IOCTL_ENCRYPTION_MODE    0xA6
+#define ZM_IOCTL_GET_TXCNT          0xA7
+#define ZM_IOCTL_GET_DEAGG_CNT      0xA8
+#define ZM_IOCTL_DURATION_MODE      0xA9
+#define ZM_IOCTL_SET_AES_KEY        0xAA
+#define ZM_IOCTL_SET_AES_MODE       0xAB
+#define ZM_IOCTL_SIGNAL_STRENGTH    0xAC //CWYang(+)
+#define ZM_IOCTL_SIGNAL_QUALITY     0xAD //CWYang(+)
+#define ZM_IOCTL_SET_PIBSS_MODE     0xAE
+
+#define	ZDAPIOCTL				SIOCDEVPRIVATE
+
+enum devState {
+    Opened,
+    Enabled,
+    Disabled,
+    Closed
+};
+
+#endif	/* _USBDRV_H */
+
diff --git a/drivers/staging/otus/wrap_buf.c b/drivers/staging/otus/wrap_buf.c
new file mode 100644
index 00000000000..62496a0f8e3
--- /dev/null
+++ b/drivers/staging/otus/wrap_buf.c
@@ -0,0 +1,114 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : wrap_buf.c                                            */
+/*                                                                      */
+/*  Abstract                                                            */
+/*     This module contains wrapper functions for buffer management     */
+/*                                                                      */
+/*  NOTES                                                               */
+/*     Platform dependent.                                              */
+/*                                                                      */
+/************************************************************************/
+
+#include "oal_dt.h"
+#include "usbdrv.h"
+
+
+#include <linux/netlink.h>
+
+#if WIRELESS_EXT > 12
+#include <net/iw_handler.h>
+#endif
+
+
+
+/* Called to allocate buffer, must return a continue buffer space */
+zbuf_t* zfwBufAllocate(zdev_t* dev, u16_t len)
+{
+    zbuf_t* buf;
+
+    /* Allocate SKB for packet*/
+    buf = dev_alloc_skb(len);
+
+    return buf;
+}
+
+
+/* Called to free buffer, replace below 3 functions */
+void zfwBufFree(zdev_t* dev, zbuf_t* buf, u16_t status)
+{
+    dev_kfree_skb_any(buf);
+}
+
+/* Called to adjust buffer size and head pointer */
+u16_t zfwBufRemoveHead(zdev_t* dev, zbuf_t* buf, u16_t size)
+{
+    //zm_assert(buf->len > size);
+
+    buf->data += size;
+    buf->len -= size;
+    return 0;
+}
+
+
+
+
+/* return tail if head==NULL, called to chain multiple buffer together */
+/* Used to chain Rx buffer to form a frame. if the prepared Rx buffer  */
+/* is greater than an ethernet frame(1518+32 byte), then this function    */
+/* will only be called with head=NULL.                                 */
+u16_t zfwBufChain(zdev_t* dev, zbuf_t** head, zbuf_t* tail)
+{
+
+    *head = tail;
+    return 0;
+}
+
+
+/* Called when doing infra-bss forwarding */
+u16_t zfwBufCopy(zdev_t* dev, zbuf_t* dst, zbuf_t* src)
+{
+    memcpy(dst->data, src->data, src->len);
+    dst->tail = dst->data;
+    skb_put(dst, src->len);
+    return 0;
+}
+
+
+/* Called to adjust buffer size and tail pointer */
+u16_t zfwBufSetSize(zdev_t* dev, zbuf_t* buf, u16_t size)
+{
+#ifdef NET_SKBUFF_DATA_USES_OFFSET
+    buf->tail = 0;
+    buf->len = 0;
+#else
+    buf->tail = buf->data;
+    buf->len = 0;
+#endif
+
+    skb_put(buf, size);
+    return 0;
+}
+
+u16_t zfwBufGetSize(zdev_t* dev, zbuf_t* buf)
+{
+    return buf->len;
+}
+
+void zfwCopyBufContext(zdev_t* dev, zbuf_t* source, zbuf_t* dst)
+{
+}
diff --git a/drivers/staging/otus/wrap_dbg.c b/drivers/staging/otus/wrap_dbg.c
new file mode 100644
index 00000000000..53763d9d2d8
--- /dev/null
+++ b/drivers/staging/otus/wrap_dbg.c
@@ -0,0 +1,101 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*  Module Name : wrap_dbg.c                                            */
+/*                                                                      */
+/*  Abstract                                                            */
+/*     This module contains wrapper functions for debug functions       */
+/*                                                                      */
+/*  NOTES                                                               */
+/*     Platform dependent.                                              */
+/*                                                                      */
+/************************************************************************/
+
+#include "oal_dt.h"
+#include "usbdrv.h"
+
+#include <linux/netlink.h>
+
+#if WIRELESS_EXT > 12
+#include <net/iw_handler.h>
+#endif
+
+void zfwDumpBuf(zdev_t* dev, zbuf_t* buf)
+{
+    u16_t i;
+
+    for (i=0; i<buf->len; i++)
+    {
+        printk("%02x ", *(((u8_t*)buf->data)+i));
+        if ((i&0xf)==0xf)
+        {
+            printk("\n");
+        }
+    }
+    printk("\n");
+}
+
+
+void zfwDbgReadRegDone(zdev_t* dev, u32_t addr, u32_t val)
+{
+    printk("Read addr:%x = %x\n", addr, val);
+}
+
+void zfwDbgWriteRegDone(zdev_t* dev, u32_t addr, u32_t val)
+{
+    printk("Write addr:%x = %x\n", addr, val);
+}
+
+void zfwDbgReadTallyDone(zdev_t* dev)
+{
+    //printk("Read Tall Done\n");
+}
+
+void zfwDbgWriteEepromDone(zdev_t* dev, u32_t addr, u32_t val)
+{
+}
+
+void zfwDbgQueryHwTxBusyDone(zdev_t* dev, u32_t val)
+{
+}
+
+//For Evl ++
+void zfwDbgReadFlashDone(zdev_t* dev, u32_t addr, u32_t* rspdata, u32_t datalen)
+{
+    printk("Read Flash addr:%x length:%x\n", addr, datalen);
+}
+
+void zfwDbgProgrameFlashDone(zdev_t* dev)
+{
+    printk("Program Flash Done\n");
+}
+
+void zfwDbgProgrameFlashChkDone(zdev_t* dev)
+{
+    printk("Program Flash Done\n");
+}
+
+void zfwDbgGetFlashChkSumDone(zdev_t* dev, u32_t* rspdata)
+{
+    printk("Get Flash ChkSum Done\n");
+}
+
+void zfwDbgDownloadFwInitDone(zdev_t* dev)
+{
+    printk("Download FW Init Done\n");
+}
+//For Evl --
+
+/* Leave an empty line below to remove warning message on some compiler */
diff --git a/drivers/staging/otus/wrap_ev.c b/drivers/staging/otus/wrap_ev.c
new file mode 100644
index 00000000000..966b787eef6
--- /dev/null
+++ b/drivers/staging/otus/wrap_ev.c
@@ -0,0 +1,283 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : wrap_ev.c                                            */
+/*                                                                      */
+/*  Abstract                                                            */
+/*     This module contains wrapper functions for events                */
+/*                                                                      */
+/*  NOTES                                                               */
+/*     Platform dependent.                                              */
+/*                                                                      */
+/************************************************************************/
+
+#include "oal_dt.h"
+#include "usbdrv.h"
+
+#include <linux/netlink.h>
+
+#if WIRELESS_EXT > 12
+#include <net/iw_handler.h>
+#endif
+
+
+/***** Management *****/
+u16_t zfLnxAuthNotify(zdev_t* dev, u16_t* macAddr)
+{
+    return 0;
+}
+
+u16_t zfLnxAsocNotify(zdev_t* dev, u16_t* macAddr, u8_t* body, u16_t bodySize, u16_t port)
+{
+//#ifdef ZM_HOSTAPD_SUPPORT
+    struct usbdrv_private *macp = dev->ml_priv;
+    union iwreq_data wreq;
+    u8_t *addr = (u8_t *) macAddr;
+    u16_t i, j;
+
+    memset(&wreq, 0, sizeof(wreq));
+    memcpy(wreq.addr.sa_data, macAddr, ETH_ALEN);
+    wreq.addr.sa_family = ARPHRD_ETHER;
+    printk(KERN_DEBUG "join_event of MAC: %02x:%02x:%02x:%02x:%02x:%02x\n",
+            addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]);
+
+    for(i = 0; i < ZM_OAL_MAX_STA_SUPPORT; i++)
+    {
+        for(j = 0; j < IEEE80211_ADDR_LEN; j++)
+        {
+            if ((macp->stawpaie[i].wpa_macaddr[j] != 0) &&
+                (macp->stawpaie[i].wpa_macaddr[j] != addr[j]))
+                break;
+        }
+        if (j == 6)
+            break;
+    }
+    if (i < ZM_OAL_MAX_STA_SUPPORT)
+    {
+        //printk("zfwAsocNotify - store wpa ie in macp, index = %d\n", i);
+        memcpy(macp->stawpaie[i].wpa_macaddr, macAddr, IEEE80211_ADDR_LEN);
+        memcpy(macp->stawpaie[i].wpa_ie, body, bodySize);
+    }
+    //if(macp->cardSetting.BssType == INFRASTRUCTURE_BSS) {
+    //            //wireless_send_event(macp->device, SIOCGIWSCAN, &wreq, NULL);
+    //    wireless_send_event(macp->device, SIOCGIWAP, &wreq, NULL);
+    //}
+#if WIRELESS_EXT >= 15
+    //else if(macp->cardSetting.BssType == AP_BSS) {
+//        if (port == 0)
+//        {
+            wireless_send_event(dev, IWEVREGISTERED, &wreq, NULL);
+//        }
+//        else
+//        {
+//            /* Check whether the VAP device is valid */
+//            if (vap[port].dev != NULL)
+//            {
+//                wireless_send_event(vap[port].dev, IWEVREGISTERED, &wreq, NULL);
+//            }
+//            else
+//            {
+//                printk(KERN_ERR "Can' find a valid VAP device, port: %d\n", port);
+//            }
+//        }
+    //}
+#endif
+//#endif
+
+    return 0;
+}
+
+
+/* Notification that a STA is disassociated from AP */
+/* AP mode only */
+u16_t zfLnxDisAsocNotify(zdev_t* dev, u8_t* macAddr, u16_t port)
+{
+    union iwreq_data wreq;
+    u8_t *addr = (u8_t *) macAddr;
+
+    memset(&wreq, 0, sizeof(wreq));
+    memcpy(wreq.addr.sa_data, macAddr, ETH_ALEN);
+    wreq.addr.sa_family = ARPHRD_ETHER;
+    printk(KERN_DEBUG "zfwDisAsocNotify(), MAC: %02x:%02x:%02x:%02x:%02x:%02x\n",
+            addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]);
+
+
+    return 0;
+}
+
+/* Notification that a STA is connect to AP */
+/* AP mode only */
+u16_t zfLnxApConnectNotify(zdev_t* dev, u8_t* macAddr, u16_t port)
+{
+    union iwreq_data wreq;
+    u8_t *addr = (u8_t *) macAddr;
+
+    memset(&wreq, 0, sizeof(wreq));
+    memcpy(wreq.addr.sa_data, macAddr, ETH_ALEN);
+    wreq.addr.sa_family = ARPHRD_ETHER;
+    printk(KERN_DEBUG "zfwApConnectNotify(), MAC: %02x:%02x:%02x:%02x:%02x:%02x\n",
+            addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]);
+
+
+    return 0;
+}
+
+
+
+void zfLnxConnectNotify(zdev_t* dev, u16_t status, u16_t* bssid)
+{
+    union iwreq_data wreq;
+    u8_t *addr = (u8_t *) bssid;
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    if (bssid != NULL)
+    {
+        memset(&wreq, 0, sizeof(wreq));
+        if (status == ZM_STATUS_MEDIA_CONNECT)
+            memcpy(wreq.addr.sa_data, bssid, ETH_ALEN);
+        wreq.addr.sa_family = ARPHRD_ETHER;
+
+        if (status == ZM_STATUS_MEDIA_CONNECT)
+        {
+#ifdef ZM_CONFIG_BIG_ENDIAN
+            printk(KERN_DEBUG "Connected to AP, MAC: %02x:%02x:%02x:%02x:%02x:%02x\n",
+                    addr[1], addr[0], addr[3], addr[2], addr[5], addr[4]);
+#else
+            printk(KERN_DEBUG "Connected to AP, MAC: %02x:%02x:%02x:%02x:%02x:%02x\n",
+                    addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]);
+#endif
+
+            netif_start_queue(dev);
+        }
+        else if ((status == ZM_STATUS_MEDIA_DISCONNECT) ||
+                 (status == ZM_STATUS_MEDIA_DISABLED) ||
+                 (status == ZM_STATUS_MEDIA_CONNECTION_DISABLED) ||
+	         (status == ZM_STATUS_MEDIA_CONNECTION_RESET) ||
+	         (status == ZM_STATUS_MEDIA_RESET) ||
+	         (status == ZM_STATUS_MEDIA_DISCONNECT_DEAUTH) ||
+	         (status == ZM_STATUS_MEDIA_DISCONNECT_DISASOC) ||
+	         (status == ZM_STATUS_MEDIA_DISCONNECT_BEACON_MISS) ||
+                 (status == ZM_STATUS_MEDIA_DISCONNECT_NOT_FOUND) ||
+	         (status == ZM_STATUS_MEDIA_DISCONNECT_TIMEOUT))
+        {
+            printk(KERN_DEBUG "Disconnection Notify\n");
+
+            netif_stop_queue(dev);
+        }
+
+	/* Save the connected status */
+	macp->adapterState = status;
+
+        if(zfiWlanQueryWlanMode(dev) == ZM_MODE_INFRASTRUCTURE) {
+        //            //wireless_send_event(dev, SIOCGIWSCAN, &wreq, NULL);
+            wireless_send_event(dev, SIOCGIWAP, &wreq, NULL);
+        }
+#if WIRELESS_EXT >= 15
+        else if(zfiWlanQueryWlanMode(dev) == ZM_MODE_AP) {
+            //if (port == 0)
+            //{
+                wireless_send_event(dev, IWEVREGISTERED, &wreq, NULL);
+            //}
+            //else
+            //{
+            //    /* Check whether the VAP device is valid */
+            //    if (vap[port].dev != NULL)
+            //    {
+            //        wireless_send_event(vap[port].dev, IWEVREGISTERED, &wreq, NULL);
+            //    }
+            //    else
+            //    {
+            //        printk(KERN_ERR "Can' find a valid VAP device, port: %d\n", port);
+            //    }
+            //}
+        }
+#endif
+    }
+    //return 0;
+}
+
+void zfLnxScanNotify(zdev_t* dev, struct zsScanResult* result)
+{
+    return;
+}
+
+void zfLnxStatisticsNotify(zdev_t* dev, struct zsStastics* result)
+{
+    return;
+}
+
+//void zfwMicFailureNotify(zdev_t* dev, u8_t* message, u16_t event)
+void zfLnxMicFailureNotify(zdev_t* dev, u16_t* addr, u16_t status)
+{
+	static const char *tag = "MLME-MICHAELMICFAILURE.indication";
+	union iwreq_data wrqu;
+	char buf[128];
+
+	/* TODO: needed parameters: count, type, src address */
+	//snprintf(buf, sizeof(buf), "%s(%scast addr=%s)", tag,
+	//    (status == ZM_MIC_GROUP_ERROR) ?  "broad" : "uni",
+	//    ether_sprintf((u8_t *)addr));
+
+	if (zfiWlanQueryWlanMode(dev) == ZM_MODE_INFRASTRUCTURE)
+	{
+		strcpy(buf, tag);
+	}
+
+	memset(&wrqu, 0, sizeof(wrqu));
+	wrqu.data.length = strlen(buf);
+	wireless_send_event(dev, IWEVCUSTOM, &wrqu, buf);
+}
+
+
+void zfLnxApMicFailureNotify(zdev_t* dev, u8_t* addr, zbuf_t* buf)
+{
+    union iwreq_data wreq;
+
+    memset(&wreq, 0, sizeof(wreq));
+    memcpy(wreq.addr.sa_data, addr, ETH_ALEN);
+    wreq.addr.sa_family = ARPHRD_ETHER;
+    printk(KERN_DEBUG "zfwApMicFailureNotify(), MAC: %02x:%02x:%02x:%02x:%02x:%02x\n",
+            addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]);
+
+    return;
+}
+
+// status = 0  => partner lost
+//        = 1  => partner alive
+//void zfwIbssPartnerNotify(zdev_t* dev, u8_t status)
+void zfLnxIbssPartnerNotify(zdev_t* dev, u16_t status, struct zsPartnerNotifyEvent *event)
+{
+}
+
+void zfLnxMacAddressNotify(zdev_t* dev, u8_t* addr)
+{
+    dev->dev_addr[0] = addr[0];
+    dev->dev_addr[1] = addr[1];
+    dev->dev_addr[2] = addr[2];
+    dev->dev_addr[3] = addr[3];
+    dev->dev_addr[4] = addr[4];
+    dev->dev_addr[5] = addr[5];
+}
+
+void zfLnxSendCompleteIndication(zdev_t* dev, zbuf_t* buf)
+{
+}
+
+
+void zfLnxRestoreBufData(zdev_t* dev, zbuf_t* buf) {
+
+}
+/* Leave an empty line below to remove warning message on some compiler */
diff --git a/drivers/staging/otus/wrap_mem.c b/drivers/staging/otus/wrap_mem.c
new file mode 100644
index 00000000000..8081bb2f887
--- /dev/null
+++ b/drivers/staging/otus/wrap_mem.c
@@ -0,0 +1,101 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*  Module Name : wrap_mem.c                                            */
+/*                                                                      */
+/*  Abstract                                                            */
+/*     This module contains wrapper functions for memory management     */
+/*                                                                      */
+/*  NOTES                                                               */
+/*     Platform dependent.                                              */
+/*                                                                      */
+/************************************************************************/
+
+#include "oal_dt.h"
+#include "usbdrv.h"
+
+#include <linux/netlink.h>
+
+#if WIRELESS_EXT > 12
+#include <net/iw_handler.h>
+#endif
+
+/* Memory management */
+/* Called to allocate uncached memory, allocated memory must    */
+/* in 4-byte boundary                                           */
+void* zfwMemAllocate(zdev_t* dev, u32_t size)
+{
+    void* mem = NULL;
+    mem = kmalloc(size, GFP_ATOMIC);
+    return mem;
+}
+
+
+/* Called to free allocated memory */
+void zfwMemFree(zdev_t* dev, void* mem, u32_t size)
+{
+    kfree(mem);
+    return;
+}
+
+void zfwMemoryCopy(u8_t* dst, u8_t* src, u16_t length)
+{
+    //u16_t i;
+
+    memcpy(dst, src, length);
+    //for(i=0; i<length; i++)
+    //{
+    //    dst[i] = src[i];
+    //}
+    return;
+}
+
+void zfwZeroMemory(u8_t* va, u16_t length)
+{
+    //u16_t i;
+    memset(va, 0, length);
+    //for(i=0; i<length; i++)
+    //{
+    //    va[i] = 0;
+    //}
+    return;
+}
+
+void zfwMemoryMove(u8_t* dst, u8_t* src, u16_t length)
+{
+    memcpy(dst, src, length);
+    return;
+}
+
+u8_t zfwMemoryIsEqual(u8_t* m1, u8_t* m2, u16_t length)
+{
+    //u16_t i;
+    int ret;
+
+    ret = memcmp(m1, m2, length);
+
+    return ((ret==0)?TRUE:FALSE);
+    //for(i=0; i<length; i++)
+    //{
+    //    if ( m1[i] != m2[i] )
+    //    {
+    //        return FALSE;
+    //    }
+    //}
+
+    //return TRUE;
+}
+
+/* Leave an empty line below to remove warning message on some compiler */
diff --git a/drivers/staging/otus/wrap_mis.c b/drivers/staging/otus/wrap_mis.c
new file mode 100644
index 00000000000..337918b9de9
--- /dev/null
+++ b/drivers/staging/otus/wrap_mis.c
@@ -0,0 +1,109 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : wrap_mis.c                                            */
+/*                                                                      */
+/*  Abstract                                                            */
+/*     This module contains wrapper functions for misc functions        */
+/*                                                                      */
+/*  NOTES                                                               */
+/*     Platform dependent.                                              */
+/*                                                                      */
+/************************************************************************/
+
+#include "oal_dt.h"
+#include "usbdrv.h"
+
+#include <linux/netlink.h>
+
+#if WIRELESS_EXT > 12
+#include <net/iw_handler.h>
+#endif
+
+//extern struct zsWdsStruct wds[ZM_WDS_PORT_NUMBER];
+extern struct zsVapStruct vap[ZM_VAP_PORT_NUMBER];
+extern u16_t zfLnxGetVapId(zdev_t* dev);
+
+/* Simply return 0xffff if VAP function is not supported */
+u16_t zfwGetVapId(zdev_t* dev)
+{
+    return zfLnxGetVapId(dev);
+}
+
+void zfwSleep(zdev_t* dev, u32_t ms)
+{
+    if (in_interrupt() == 0)
+    {
+        mdelay(ms);
+    }
+    else
+    {
+        int ii;
+        int iter = 100000 * ms;
+
+        for (ii = 0; ii < iter; ii++)
+        {
+
+        }
+    }
+}
+
+#ifdef ZM_HALPLUS_LOCK
+asmlinkage struct zsWlanDev *zfwGetWlanDev(zdev_t* dev)
+{
+	struct usbdrv_private *macp = dev->ml_priv;
+	return macp->wd;
+}
+
+asmlinkage void zfwEnterCriticalSection(zdev_t* dev)
+{
+	struct usbdrv_private *macp = dev->ml_priv;
+	spin_lock_irqsave(&macp->cs_lock, macp->hal_irqFlag);
+}
+
+asmlinkage void zfwLeaveCriticalSection(zdev_t* dev)
+{
+	struct usbdrv_private *macp = dev->ml_priv;
+	spin_unlock_irqrestore(&macp->cs_lock, macp->hal_irqFlag);
+}
+
+asmlinkage u8_t zfwBufReadByte(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+    return *(u8_t*)((u8_t*)buf->data+offset);
+}
+
+asmlinkage u16_t zfwBufReadHalfWord(zdev_t* dev, zbuf_t* buf, u16_t offset)
+{
+    return zmw_cpu_to_le16(*(u16_t*)((u8_t*)buf->data+offset));
+}
+
+asmlinkage void zfwBufWriteByte(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t value)
+{
+    *(u8_t*)((u8_t*)buf->data+offset) = value;
+}
+
+asmlinkage void zfwBufWriteHalfWord(zdev_t* dev, zbuf_t* buf, u16_t offset, u16_t value)
+{
+    *(u16_t*)((u8_t*)buf->data+offset) = zmw_cpu_to_le16(value);
+}
+
+asmlinkage u8_t *zfwGetBuffer(zdev_t* dev, zbuf_t* buf)
+{
+    return (u8_t*)(buf->data);
+}
+#endif
+
+/* Leave an empty line below to remove warning message on some compiler */
diff --git a/drivers/staging/otus/wrap_pkt.c b/drivers/staging/otus/wrap_pkt.c
new file mode 100644
index 00000000000..5db0004c873
--- /dev/null
+++ b/drivers/staging/otus/wrap_pkt.c
@@ -0,0 +1,173 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : wrap_pkt.c                                            */
+/*                                                                      */
+/*  Abstract                                                            */
+/*     This module contains wrapper functions for packet handling       */
+/*                                                                      */
+/*  NOTES                                                               */
+/*     Platform dependent.                                              */
+/*                                                                      */
+/************************************************************************/
+
+#include "oal_dt.h"
+#include "usbdrv.h"
+
+#include <linux/netlink.h>
+
+#if WIRELESS_EXT > 12
+#include <net/iw_handler.h>
+#endif
+
+
+
+//extern struct zsWdsStruct wds[ZM_WDS_PORT_NUMBER];
+extern struct zsVapStruct vap[ZM_VAP_PORT_NUMBER];
+
+
+/***** Rx *****/
+void zfLnxRecv80211(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* addInfo)
+{
+    u16_t frameType;
+    u16_t frameCtrl;
+    u16_t frameSubtype;
+    zbuf_t *skb1;
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    //frameCtrl = zmw_buf_readb(dev, buf, 0);
+    frameCtrl = *(u8_t*)((u8_t*)buf->data);
+    frameType = frameCtrl & 0xf;
+    frameSubtype = frameCtrl & 0xf0;
+
+    if ((frameType == 0x0) && (macp->forwardMgmt))
+    {
+        switch (frameSubtype)
+        {
+                /* Beacon */
+            case 0x80 :
+                /* Probe response */
+            case 0x50 :
+                skb1 = skb_copy(buf, GFP_ATOMIC);
+                if(skb1 != NULL)
+                {
+                    skb1->dev = dev;
+                    skb1->mac_header = skb1->data;
+	            skb1->ip_summed = CHECKSUM_NONE;
+	            skb1->pkt_type = PACKET_OTHERHOST;
+	            skb1->protocol = __constant_htons(0x0019);  /* ETH_P_80211_RAW */
+    	            netif_rx(skb1);
+	            }
+                break;
+            default:
+                break;
+        }
+    }
+
+    zfiRecv80211(dev, buf, addInfo);
+    return;
+}
+
+#define ZM_AVOID_UDP_LARGE_PACKET_FAIL
+void zfLnxRecvEth(zdev_t* dev, zbuf_t* buf, u16_t port)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+#ifdef ZM_AVOID_UDP_LARGE_PACKET_FAIL
+    zbuf_t *new_buf;
+
+    //new_buf = dev_alloc_skb(2048);
+    new_buf = dev_alloc_skb(buf->len);
+
+#ifdef NET_SKBUFF_DATA_USES_OFFSET
+    new_buf->tail = 0;
+    new_buf->len = 0;
+#else
+    new_buf->tail = new_buf->data;
+    new_buf->len = 0;
+#endif
+
+    skb_put(new_buf, buf->len);
+    memcpy(new_buf->data, buf->data, buf->len);
+
+    /* Free buffer */
+    dev_kfree_skb_any(buf);
+
+    if (port == 0)
+    {
+        new_buf->dev = dev;
+        new_buf->protocol = eth_type_trans(new_buf, dev);
+    }
+    else
+    {
+        /* VAP */
+        if (vap[0].dev != NULL)
+        {
+            new_buf->dev = vap[0].dev;
+            new_buf->protocol = eth_type_trans(new_buf, vap[0].dev);
+        }
+        else
+        {
+            new_buf->dev = dev;
+            new_buf->protocol = eth_type_trans(new_buf, dev);
+        }
+    }
+
+    new_buf->ip_summed = CHECKSUM_NONE;
+    dev->last_rx = jiffies;
+
+    switch(netif_rx(new_buf))
+#else
+    if (port == 0)
+    {
+        buf->dev = dev;
+        buf->protocol = eth_type_trans(buf, dev);
+    }
+    else
+    {
+        /* VAP */
+        if (vap[0].dev != NULL)
+        {
+            buf->dev = vap[0].dev;
+            buf->protocol = eth_type_trans(buf, vap[0].dev);
+        }
+        else
+        {
+            buf->dev = dev;
+            buf->protocol = eth_type_trans(buf, dev);
+        }
+    }
+
+    buf->ip_summed = CHECKSUM_NONE;
+    dev->last_rx = jiffies;
+
+    switch(netif_rx(buf))
+#endif
+    {
+    case NET_RX_BAD:
+    case NET_RX_DROP:
+    case NET_RX_CN_MOD:
+    case NET_RX_CN_HIGH:
+        break;
+    default:
+            macp->drv_stats.net_stats.rx_packets++;
+            macp->drv_stats.net_stats.rx_bytes += buf->len;
+        break;
+    }
+
+    return;
+}
+
+/* Leave an empty line below to remove warning message on some compiler */
diff --git a/drivers/staging/otus/wrap_sec.c b/drivers/staging/otus/wrap_sec.c
new file mode 100644
index 00000000000..f688d064175
--- /dev/null
+++ b/drivers/staging/otus/wrap_sec.c
@@ -0,0 +1,127 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : wrap_sec.c                                            */
+/*                                                                      */
+/*  Abstract                                                            */
+/*     This module contains wrapper functions for CENC.                 */
+/*                                                                      */
+/*  NOTES                                                               */
+/*     Platform dependent.                                              */
+/*                                                                      */
+/************************************************************************/
+
+#include "oal_dt.h"
+#include "usbdrv.h"
+
+#include <linux/netlink.h>
+
+#if WIRELESS_EXT > 12
+#include <net/iw_handler.h>
+#endif
+
+#ifdef ZM_ENABLE_CENC
+extern int zfLnxCencSendMsg(struct sock *netlink_sk, u_int8_t *msg, int len);
+
+u16_t zfLnxCencAsocNotify(zdev_t* dev, u16_t* macAddr, u8_t* body, u16_t bodySize, u16_t port)
+{
+    struct usbdrv_private *macp = (struct usbdrv_private *)dev->priv;
+    struct zydas_cenc_sta_info cenc_info;
+    //struct sock *netlink_sk;
+    u8_t ie_len;
+    int ii;
+
+    /* Create NETLINK socket */
+    //netlink_sk = netlink_kernel_create(NETLINK_USERSOCK, NULL);
+
+    if (macp->netlink_sk == NULL)
+    {
+        printk(KERN_ERR "NETLINK Socket is NULL\n");
+        return -1;
+    }
+
+    memset(&cenc_info, 0, sizeof(cenc_info));
+
+    //memcpy(cenc_info.gsn, vap->iv_cencmsk_keys.wk_txiv, ZM_CENC_IV_LEN);
+    zfiWlanQueryGSN(dev, cenc_info.gsn, port);
+    cenc_info.datalen += ZM_CENC_IV_LEN;
+    ie_len = body[1] + 2;
+    memcpy(cenc_info.wie, body, ie_len);
+    cenc_info.datalen += ie_len;
+
+    memcpy(cenc_info.sta_mac, macAddr, 6);
+    cenc_info.msg_type = ZM_CENC_WAI_REQUEST;
+    cenc_info.datalen += 6 + 2;
+
+    printk(KERN_ERR "===== zfwCencSendMsg, bodySize: %d =====\n", bodySize);
+
+    for(ii = 0; ii < bodySize; ii++)
+    {
+        printk(KERN_ERR "%02x ", body[ii]);
+
+        if ((ii & 0xf) == 0xf)
+        {
+            printk(KERN_ERR "\n");
+        }
+    }
+
+    zfLnxCencSendMsg(macp->netlink_sk, (u8_t *)&cenc_info, cenc_info.datalen+4);
+
+    /* Close NETLINK socket */
+    //sock_release(netlink_sk);
+
+    return 0;
+}
+#endif //ZM_ENABLE_CENC
+
+u8_t zfwCencHandleBeaconProbrespon(zdev_t* dev, u8_t *pWIEc,
+        u8_t *pPeerSSIDc, u8_t *pPeerAddrc)
+{
+    return 0;
+}
+
+u8_t zfwGetPktEncExemptionActionType(zdev_t* dev, zbuf_t* buf)
+{
+    return ZM_ENCRYPTION_EXEMPT_NO_EXEMPTION;
+}
+
+void copyToIntTxBuffer(zdev_t* dev, zbuf_t* buf, u8_t* src,
+                         u16_t offset, u16_t length)
+{
+    u16_t i;
+
+    for(i=0; i<length;i++)
+    {
+        //zmw_tx_buf_writeb(dev, buf, offset+i, src[i]);
+        *(u8_t*)((u8_t*)buf->data+offset+i) = src[i];
+    }
+}
+
+u16_t zfwStaAddIeWpaRsn(zdev_t* dev, zbuf_t* buf, u16_t offset, u8_t frameType)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    //zm_msg1_mm(ZM_LV_0, "CWY - add wpaie content Length : ", macp->supIe[1]);
+    if (macp->supIe[1] != 0)
+    {
+        copyToIntTxBuffer(dev, buf, macp->supIe, offset, macp->supIe[1]+2);
+        //memcpy(buf->data[offset], macp->supIe, macp->supIe[1]+2);
+        offset += (macp->supIe[1]+2);
+    }
+
+    return offset;
+}
+
+/* Leave an empty line below to remove warning message on some compiler */
diff --git a/drivers/staging/otus/wrap_usb.c b/drivers/staging/otus/wrap_usb.c
new file mode 100644
index 00000000000..c076e564522
--- /dev/null
+++ b/drivers/staging/otus/wrap_usb.c
@@ -0,0 +1,191 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : wrap_usb.c                                            */
+/*                                                                      */
+/*  Abstract                                                            */
+/*     This module contains wrapper functions for USB management        */
+/*                                                                      */
+/*  NOTES                                                               */
+/*     Platform dependent.                                              */
+/*                                                                      */
+/************************************************************************/
+
+#include "oal_dt.h"
+#include "usbdrv.h"
+
+#include <linux/netlink.h>
+
+#if WIRELESS_EXT > 12
+#include <net/iw_handler.h>
+#endif
+
+extern void zfLnxInitUsbTxQ(zdev_t* dev);
+extern void zfLnxInitUsbRxQ(zdev_t* dev);
+extern u32_t zfLnxSubmitRegInUrb(zdev_t *dev);
+u32_t zfLnxUsbOut(zdev_t* dev, u8_t *hdr, u16_t hdrlen, u8_t *snap, u16_t snapLen,
+        u8_t *tail, u16_t tailLen, zbuf_t *buf, u16_t offset);
+u32_t zfLnxUsbWriteReg(zdev_t* dev, u32_t* cmd, u16_t cmdLen);
+
+void zfwUsbRegisterCallBack(zdev_t* dev, struct zfCbUsbFuncTbl *zfUsbFunc) {
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    macp->usbCbFunctions.zfcbUsbRecv = zfUsbFunc->zfcbUsbRecv;
+    macp->usbCbFunctions.zfcbUsbRegIn = zfUsbFunc->zfcbUsbRegIn;
+    macp->usbCbFunctions.zfcbUsbOutComplete = zfUsbFunc->zfcbUsbOutComplete;
+
+    return;
+}
+
+u32_t zfwUsbGetFreeTxQSize(zdev_t* dev)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    u32_t        freeTxQSize;
+    unsigned long irqFlag;
+    //zmw_declare_for_critical_section();
+
+    //zmw_enter_critical_section(dev);
+    spin_lock_irqsave(&macp->cs_lock, irqFlag);
+
+    freeTxQSize = ZM_MAX_TX_BUF_NUM - macp->TxBufCnt;
+
+    //zmw_leave_critical_section(dev);
+    spin_unlock_irqrestore(&macp->cs_lock, irqFlag);
+
+    return freeTxQSize;
+}
+
+u32_t zfwUsbGetMaxTxQSize(zdev_t* dev)
+{
+    return ZM_MAX_TX_BUF_NUM;
+}
+
+u32_t zfwUsbEnableIntEpt(zdev_t *dev, u8_t endpt)
+{
+    /* Initialize USB TxQ */
+    zfLnxInitUsbTxQ(dev);
+
+    /* Initialize USB RxQ */
+    zfLnxInitUsbRxQ(dev);
+
+    /* Initialize USB Register In URB */
+    //zfwUsbSubmitRegIn(dev);
+    /* Initialize USB Register In URB */
+    zfLnxSubmitRegInUrb(dev);
+
+    return 0;
+}
+
+int zfwUsbEnableRxEpt(zdev_t* dev, u8_t endpt)
+{
+    return 0;
+}
+
+u32_t zfwUsbSubmitControl(zdev_t* dev, u8_t req, u16_t value, u16_t index, void *data, u32_t size)
+{
+    int result = 0;
+    u32_t ret = 0;
+    struct usbdrv_private *macp = dev->ml_priv;
+    u8_t* buf;
+
+    if (size > 0)
+    {
+        buf = kmalloc(size, GFP_KERNEL);
+        memcpy(buf, (u8_t*)data, size);
+    }
+    else
+    {
+        buf = NULL;
+    }
+
+#if 0
+    printk(KERN_ERR "req = 0x%02x\n", req);
+    printk(KERN_ERR "value = 0x%04x\n", value);
+    printk(KERN_ERR "index = 0x%04x\n", index);
+    printk(KERN_ERR "data = 0x%lx\n", (u32_t) data);
+    printk(KERN_ERR "size = %ld\n", size);
+#endif
+
+    result = usb_control_msg(macp->udev, usb_sndctrlpipe(macp->udev, 0),
+            req, USB_DIR_OUT | 0x40, value, index, buf, size, HZ);
+
+    if (result < 0)
+    {
+        printk("zfwUsbSubmitControl() failed, result=0x%x\n", result);
+        ret = 1;
+    }
+    kfree(buf);
+
+    return ret;
+}
+
+void zfwUsbCmd(zdev_t* dev, u8_t endpt, u32_t* cmd, u16_t cmdLen)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    u32_t ret;
+
+    //MPUsbCommand(dev, endpt, cmd, cmdLen);
+    ret = zfLnxUsbWriteReg(dev, cmd, cmdLen);
+
+    /* if zfLnxUsbWriteReg() return error, free and allocate urb, resend again */
+    if (ret != 0)
+    {
+        usb_free_urb(macp->RegOutUrb);
+        macp->RegOutUrb = usb_alloc_urb(0, GFP_ATOMIC);
+        ret = zfLnxUsbWriteReg(dev, cmd, cmdLen);
+    }
+}
+
+u32_t zfwUsbSend(zdev_t* dev, u8_t endpt, u8_t *hdr, u16_t hdrlen, u8_t *snap, u16_t snapLen,
+                u8_t *tail, u16_t tailLen, zbuf_t *buf, u16_t offset)
+{
+    u32_t status;
+
+#ifdef ZM_CONFIG_BIG_ENDIAN
+    u32_t ii = 0;
+    u16_t *pc = NULL;
+
+    pc = (u16_t *)hdr;
+    for(ii=0; ii<(hdrlen>>1); ii++)
+    {
+        pc[ii] = cpu_to_le16(pc[ii]);
+    }
+
+    pc = (u16_t *)snap;
+    for(ii=0; ii<(snapLen>>1); ii++)
+    {
+        pc[ii] = cpu_to_le16(pc[ii]);
+    }
+
+    pc = (u16_t *)tail;
+    for(ii=0; ii<(tailLen>>1); ii++)
+    {
+        pc[ii] = cpu_to_le16(pc[ii]);
+    }
+#endif
+
+    status = zfLnxUsbOut(dev, hdr, hdrlen, snap, snapLen, tail, tailLen, buf, offset);
+    if ( status == 0 )
+    {
+        return 0;
+    }
+    else
+    {
+        return 1;
+    }
+}
+
+/* Leave an empty line below to remove warning message on some compiler */
diff --git a/drivers/staging/otus/wwrap.c b/drivers/staging/otus/wwrap.c
new file mode 100644
index 00000000000..1bb5f596d6c
--- /dev/null
+++ b/drivers/staging/otus/wwrap.c
@@ -0,0 +1,1134 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*  Module Name : wwrap.c                                               */
+/*  Abstract                                                            */
+/*      This module contains wrapper functions.                         */
+/*                                                                      */
+/*  NOTES                                                               */
+/*      Platform dependent.                                             */
+/*                                                                      */
+
+/* Please include your header files here */
+#include "oal_dt.h"
+#include "usbdrv.h"
+
+#include <linux/netlink.h>
+
+#if WIRELESS_EXT > 12
+#include <net/iw_handler.h>
+#endif
+
+extern void zfiRecv80211(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* addInfo);
+extern void zfCoreRecv(zdev_t* dev, zbuf_t* buf, struct zsAdditionInfo* addInfo);
+extern void zfIdlChkRsp(zdev_t* dev, u32_t* rsp, u16_t rspLen);
+extern void zfIdlRsp(zdev_t* dev, u32_t *rsp, u16_t rspLen);
+
+
+
+//extern struct zsWdsStruct wds[ZM_WDS_PORT_NUMBER];
+extern struct zsVapStruct vap[ZM_VAP_PORT_NUMBER];
+
+u32_t zfLnxUsbSubmitTxData(zdev_t* dev);
+u32_t zfLnxUsbIn(zdev_t* dev, urb_t *urb, zbuf_t *buf);
+u32_t zfLnxSubmitRegInUrb(zdev_t *dev);
+u32_t zfLnxUsbSubmitBulkUrb(urb_t *urb, struct usb_device *usb, u16_t epnum, u16_t direction,
+        void *transfer_buffer, int buffer_length, usb_complete_t complete, void *context);
+u32_t zfLnxUsbSubmitIntUrb(urb_t *urb, struct usb_device *usb, u16_t epnum, u16_t direction,
+        void *transfer_buffer, int buffer_length, usb_complete_t complete, void *context,
+        u32_t interval);
+
+u16_t zfLnxGetFreeTxUrb(zdev_t *dev)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    u16_t idx;
+    unsigned long irqFlag;
+
+    spin_lock_irqsave(&macp->cs_lock, irqFlag);
+
+    //idx = ((macp->TxUrbTail + 1) & (ZM_MAX_TX_URB_NUM - 1));
+
+    //if (idx != macp->TxUrbHead)
+    if (macp->TxUrbCnt != 0)
+    {
+        idx = macp->TxUrbTail;
+        macp->TxUrbTail = ((macp->TxUrbTail + 1) & (ZM_MAX_TX_URB_NUM - 1));
+        macp->TxUrbCnt--;
+    }
+    else
+    {
+        //printk(KERN_ERR "macp->TxUrbCnt: %d\n", macp->TxUrbCnt);
+        idx = 0xffff;
+    }
+
+    spin_unlock_irqrestore(&macp->cs_lock, irqFlag);
+    return idx;
+}
+
+void zfLnxPutTxUrb(zdev_t *dev)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    u16_t idx;
+    unsigned long irqFlag;
+
+    spin_lock_irqsave(&macp->cs_lock, irqFlag);
+
+    idx = ((macp->TxUrbHead + 1) & (ZM_MAX_TX_URB_NUM - 1));
+
+    //if (idx != macp->TxUrbTail)
+    if (macp->TxUrbCnt < ZM_MAX_TX_URB_NUM)
+    {
+        macp->TxUrbHead = idx;
+        macp->TxUrbCnt++;
+    }
+    else
+    {
+        printk("UsbTxUrbQ inconsistent: TxUrbHead: %d, TxUrbTail: %d\n",
+                macp->TxUrbHead, macp->TxUrbTail);
+    }
+
+    spin_unlock_irqrestore(&macp->cs_lock, irqFlag);
+}
+
+u16_t zfLnxCheckTxBufferCnt(zdev_t *dev)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    u16_t TxBufCnt;
+    unsigned long irqFlag;
+
+    spin_lock_irqsave(&macp->cs_lock, irqFlag);
+
+    TxBufCnt = macp->TxBufCnt;
+
+    spin_unlock_irqrestore(&macp->cs_lock, irqFlag);
+    return TxBufCnt;
+}
+
+UsbTxQ_t *zfLnxGetUsbTxBuffer(zdev_t *dev)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    u16_t idx;
+    UsbTxQ_t *TxQ;
+    unsigned long irqFlag;
+
+    spin_lock_irqsave(&macp->cs_lock, irqFlag);
+
+    idx = ((macp->TxBufHead+1) & (ZM_MAX_TX_BUF_NUM - 1));
+
+    //if (idx != macp->TxBufTail)
+    if (macp->TxBufCnt > 0)
+    {
+        //printk("CWY - zfwGetUsbTxBuffer ,macp->TxBufCnt = %d\n", macp->TxBufCnt);
+        TxQ = (UsbTxQ_t *)&(macp->UsbTxBufQ[macp->TxBufHead]);
+        macp->TxBufHead = ((macp->TxBufHead+1) & (ZM_MAX_TX_BUF_NUM - 1));
+        macp->TxBufCnt--;
+    }
+    else
+    {
+        if (macp->TxBufHead != macp->TxBufTail)
+        {
+            printk(KERN_ERR "zfwGetUsbTxBuf UsbTxBufQ inconsistent: TxBufHead: %d, TxBufTail: %d\n",
+                    macp->TxBufHead, macp->TxBufTail);
+        }
+
+        spin_unlock_irqrestore(&macp->cs_lock, irqFlag);
+        return NULL;
+    }
+
+    spin_unlock_irqrestore(&macp->cs_lock, irqFlag);
+    return TxQ;
+}
+
+u16_t zfLnxPutUsbTxBuffer(zdev_t *dev, u8_t *hdr, u16_t hdrlen,
+        u8_t *snap, u16_t snapLen, u8_t *tail, u16_t tailLen,
+        zbuf_t *buf, u16_t offset)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    u16_t idx;
+    UsbTxQ_t *TxQ;
+    unsigned long irqFlag;
+
+    spin_lock_irqsave(&macp->cs_lock, irqFlag);
+
+    idx = ((macp->TxBufTail+1) & (ZM_MAX_TX_BUF_NUM - 1));
+
+    /* For Tx debug */
+    //zm_assert(macp->TxBufCnt >= 0); // deleted because of always true
+
+    //if (idx != macp->TxBufHead)
+    if (macp->TxBufCnt < ZM_MAX_TX_BUF_NUM)
+    {
+        //printk("CWY - zfwPutUsbTxBuffer ,macp->TxBufCnt = %d\n", macp->TxBufCnt);
+        TxQ = (UsbTxQ_t *)&(macp->UsbTxBufQ[macp->TxBufTail]);
+        memcpy(TxQ->hdr, hdr, hdrlen);
+        TxQ->hdrlen = hdrlen;
+        memcpy(TxQ->snap, snap, snapLen);
+        TxQ->snapLen = snapLen;
+        memcpy(TxQ->tail, tail, tailLen);
+        TxQ->tailLen = tailLen;
+        TxQ->buf = buf;
+        TxQ->offset = offset;
+
+        macp->TxBufTail = ((macp->TxBufTail+1) & (ZM_MAX_TX_BUF_NUM - 1));
+        macp->TxBufCnt++;
+    }
+    else
+    {
+        printk(KERN_ERR "zfLnxPutUsbTxBuffer UsbTxBufQ inconsistent: TxBufHead: %d, TxBufTail: %d, TxBufCnt: %d\n",
+            macp->TxBufHead, macp->TxBufTail, macp->TxBufCnt);
+        spin_unlock_irqrestore(&macp->cs_lock, irqFlag);
+        return 0xffff;
+    }
+
+    spin_unlock_irqrestore(&macp->cs_lock, irqFlag);
+    return 0;
+}
+
+zbuf_t *zfLnxGetUsbRxBuffer(zdev_t *dev)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    //u16_t idx;
+    zbuf_t *buf;
+    unsigned long irqFlag;
+
+    spin_lock_irqsave(&macp->cs_lock, irqFlag);
+
+    //idx = ((macp->RxBufHead+1) & (ZM_MAX_RX_URB_NUM - 1));
+
+    //if (idx != macp->RxBufTail)
+    if (macp->RxBufCnt != 0)
+    {
+        buf = macp->UsbRxBufQ[macp->RxBufHead];
+        macp->RxBufHead = ((macp->RxBufHead+1) & (ZM_MAX_RX_URB_NUM - 1));
+        macp->RxBufCnt--;
+    }
+    else
+    {
+        printk("RxBufQ inconsistent: RxBufHead: %d, RxBufTail: %d\n",
+                macp->RxBufHead, macp->RxBufTail);
+        spin_unlock_irqrestore(&macp->cs_lock, irqFlag);
+        return NULL;
+    }
+
+    spin_unlock_irqrestore(&macp->cs_lock, irqFlag);
+    return buf;
+}
+
+u32_t zfLnxPutUsbRxBuffer(zdev_t *dev, zbuf_t *buf)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    u16_t idx;
+    unsigned long irqFlag;
+
+    spin_lock_irqsave(&macp->cs_lock, irqFlag);
+
+    idx = ((macp->RxBufTail+1) & (ZM_MAX_RX_URB_NUM - 1));
+
+    //if (idx != macp->RxBufHead)
+    if (macp->RxBufCnt != ZM_MAX_RX_URB_NUM)
+    {
+        macp->UsbRxBufQ[macp->RxBufTail] = buf;
+        macp->RxBufTail = idx;
+        macp->RxBufCnt++;
+    }
+    else
+    {
+        printk("RxBufQ inconsistent: RxBufHead: %d, RxBufTail: %d\n",
+                macp->RxBufHead, macp->RxBufTail);
+        spin_unlock_irqrestore(&macp->cs_lock, irqFlag);
+        return 0xffff;
+    }
+
+    spin_unlock_irqrestore(&macp->cs_lock, irqFlag);
+    return 0;
+}
+
+void zfLnxUsbDataOut_callback(urb_t *urb)
+{
+    zdev_t* dev = urb->context;
+    //UsbTxQ_t *TxData;
+
+    /* Give the urb back */
+    zfLnxPutTxUrb(dev);
+
+    /* Check whether there is any pending buffer needed */
+    /* to be sent */
+    if (zfLnxCheckTxBufferCnt(dev) != 0)
+    {
+        //TxData = zfwGetUsbTxBuffer(dev);
+
+        //if (TxData == NULL)
+        //{
+        //    printk("Get a NULL buffer from zfwGetUsbTxBuffer\n");
+        //    return;
+        //}
+        //else
+        //{
+            zfLnxUsbSubmitTxData(dev);
+        //}
+    }
+}
+
+void zfLnxUsbDataIn_callback(urb_t *urb)
+{
+    zdev_t* dev = urb->context;
+    struct usbdrv_private *macp = dev->ml_priv;
+    zbuf_t *buf;
+    zbuf_t *new_buf;
+    int status;
+
+#if ZM_USB_STREAM_MODE == 1
+    static int remain_len = 0, check_pad = 0, check_len = 0;
+    int index = 0;
+    int chk_idx;
+    u16_t pkt_len;
+    u16_t pkt_tag;
+    u16_t ii;
+    zbuf_t *rxBufPool[8];
+    u16_t rxBufPoolIndex = 0;
+#endif
+
+    /* Check status for URB */
+    if (urb->status != 0){
+        printk("zfLnxUsbDataIn_callback() : status=0x%x\n", urb->status);
+        if ((urb->status != -ENOENT) && (urb->status != -ECONNRESET)
+            && (urb->status != -ESHUTDOWN))
+        {
+                if (urb->status == -EPIPE){
+                    //printk(KERN_ERR "nonzero read bulk status received: -EPIPE");
+                    status = -1;
+                }
+
+                if (urb->status == -EPROTO){
+                    //printk(KERN_ERR "nonzero read bulk status received: -EPROTO");
+                    status = -1;
+                }
+        }
+
+        //printk(KERN_ERR "urb->status: 0x%08x\n", urb->status);
+
+        /* Dequeue skb buffer */
+        buf = zfLnxGetUsbRxBuffer(dev);
+        dev_kfree_skb_any(buf);
+        #if 0
+        /* Enqueue skb buffer */
+        zfLnxPutUsbRxBuffer(dev, buf);
+
+        /* Submit a Rx urb */
+        zfLnxUsbIn(dev, urb, buf);
+        #endif
+        return;
+    }
+
+    if (urb->actual_length == 0)
+    {
+        printk(KERN_ERR "Get an URB whose length is zero");
+        status = -1;
+    }
+
+    /* Dequeue skb buffer */
+    buf = zfLnxGetUsbRxBuffer(dev);
+
+    //zfwBufSetSize(dev, buf, urb->actual_length);
+#ifdef NET_SKBUFF_DATA_USES_OFFSET
+    buf->tail = 0;
+    buf->len = 0;
+#else
+    buf->tail = buf->data;
+    buf->len = 0;
+#endif
+
+    if ((buf->tail + urb->actual_length) > buf->end)
+        BUG();
+
+    skb_put(buf, urb->actual_length);
+
+#if ZM_USB_STREAM_MODE == 1
+    if (remain_len != 0)
+    {
+        zbuf_t *remain_buf = macp->reamin_buf;
+
+        index = remain_len;
+        remain_len -= check_pad;
+
+        /*  Copy data */
+        memcpy(&(remain_buf->data[check_len]), buf->data, remain_len);
+        check_len += remain_len;
+        remain_len = 0;
+
+        rxBufPool[rxBufPoolIndex++] = remain_buf;
+    }
+
+    while(index < urb->actual_length)
+    {
+        pkt_len = buf->data[index] + (buf->data[index+1] << 8);
+        pkt_tag = buf->data[index+2] + (buf->data[index+3] << 8);
+
+        if (pkt_tag == 0x4e00)
+        {
+            int pad_len;
+
+            //printk("Get a packet, index: %d, pkt_len: 0x%04x\n", index, pkt_len);
+            #if 0
+            /* Dump data */
+            for (ii = index; ii < pkt_len+4;)
+            {
+                printk("%02x ", (buf->data[ii] & 0xff));
+
+                if ((++ii % 16) == 0)
+                    printk("\n");
+            }
+
+            printk("\n");
+            #endif
+
+            pad_len = 4 - (pkt_len & 0x3);
+
+            if(pad_len == 4)
+                pad_len = 0;
+
+            chk_idx = index;
+            index = index + 4 + pkt_len + pad_len;
+
+            if (index > ZM_MAX_RX_BUFFER_SIZE)
+            {
+                remain_len = index - ZM_MAX_RX_BUFFER_SIZE; // - pad_len;
+                check_len = ZM_MAX_RX_BUFFER_SIZE - chk_idx - 4;
+                check_pad = pad_len;
+
+                /* Allocate a skb buffer */
+                //new_buf = zfwBufAllocate(dev, ZM_MAX_RX_BUFFER_SIZE);
+                new_buf = dev_alloc_skb(ZM_MAX_RX_BUFFER_SIZE);
+
+                /* Set skb buffer length */
+            #ifdef NET_SKBUFF_DATA_USES_OFFSET
+                new_buf->tail = 0;
+                new_buf->len = 0;
+            #else
+                new_buf->tail = new_buf->data;
+                new_buf->len = 0;
+            #endif
+
+                skb_put(new_buf, pkt_len);
+
+                /* Copy the buffer */
+                memcpy(new_buf->data, &(buf->data[chk_idx+4]), check_len);
+
+                /* Record the buffer pointer */
+                macp->reamin_buf = new_buf;
+            }
+            else
+            {
+        #ifdef ZM_DONT_COPY_RX_BUFFER
+                if (rxBufPoolIndex == 0)
+                {
+                    new_buf = skb_clone(buf, GFP_ATOMIC);
+
+                    new_buf->data = &(buf->data[chk_idx+4]);
+                    new_buf->len = pkt_len;
+                }
+                else
+                {
+        #endif
+                /* Allocate a skb buffer */
+                new_buf = dev_alloc_skb(ZM_MAX_RX_BUFFER_SIZE);
+
+                /* Set skb buffer length */
+            #ifdef NET_SKBUFF_DATA_USES_OFFSET
+                new_buf->tail = 0;
+                new_buf->len = 0;
+            #else
+                new_buf->tail = new_buf->data;
+                new_buf->len = 0;
+            #endif
+
+                skb_put(new_buf, pkt_len);
+
+                /* Copy the buffer */
+                memcpy(new_buf->data, &(buf->data[chk_idx+4]), pkt_len);
+
+        #ifdef ZM_DONT_COPY_RX_BUFFER
+                }
+        #endif
+                rxBufPool[rxBufPoolIndex++] = new_buf;
+            }
+        }
+        else
+        {
+            printk(KERN_ERR "Can't find tag, pkt_len: 0x%04x, tag: 0x%04x\n", pkt_len, pkt_tag);
+
+            /* Free buffer */
+            dev_kfree_skb_any(buf);
+
+            /* Allocate a skb buffer */
+            new_buf = dev_alloc_skb(ZM_MAX_RX_BUFFER_SIZE);
+
+            /* Enqueue skb buffer */
+            zfLnxPutUsbRxBuffer(dev, new_buf);
+
+            /* Submit a Rx urb */
+            zfLnxUsbIn(dev, urb, new_buf);
+
+            return;
+        }
+    }
+
+    /* Free buffer */
+    dev_kfree_skb_any(buf);
+#endif
+
+    /* Allocate a skb buffer */
+    new_buf = dev_alloc_skb(ZM_MAX_RX_BUFFER_SIZE);
+
+    /* Enqueue skb buffer */
+    zfLnxPutUsbRxBuffer(dev, new_buf);
+
+    /* Submit a Rx urb */
+    zfLnxUsbIn(dev, urb, new_buf);
+
+#if ZM_USB_STREAM_MODE == 1
+    for(ii = 0; ii < rxBufPoolIndex; ii++)
+    {
+        macp->usbCbFunctions.zfcbUsbRecv(dev, rxBufPool[ii]);
+    }
+#else
+    /* pass data to upper layer */
+    macp->usbCbFunctions.zfcbUsbRecv(dev, buf);
+#endif
+}
+
+void zfLnxUsbRegOut_callback(urb_t *urb)
+{
+    //dev_t* dev = urb->context;
+
+    //printk(KERN_ERR "zfwUsbRegOut_callback\n");
+}
+
+void zfLnxUsbRegIn_callback(urb_t *urb)
+{
+    zdev_t* dev = urb->context;
+    u32_t rsp[64/4];
+    int status;
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    /* Check status for URB */
+    if (urb->status != 0){
+        printk("zfLnxUsbRegIn_callback() : status=0x%x\n", urb->status);
+        if ((urb->status != -ENOENT) && (urb->status != -ECONNRESET)
+            && (urb->status != -ESHUTDOWN))
+        {
+                if (urb->status == -EPIPE){
+                    //printk(KERN_ERR "nonzero read bulk status received: -EPIPE");
+                    status = -1;
+                }
+
+                if (urb->status == -EPROTO){
+                    //printk(KERN_ERR "nonzero read bulk status received: -EPROTO");
+                    status = -1;
+                }
+        }
+
+        //printk(KERN_ERR "urb->status: 0x%08x\n", urb->status);
+        return;
+    }
+
+    if (urb->actual_length == 0)
+    {
+        printk(KERN_ERR "Get an URB whose length is zero");
+        status = -1;
+    }
+
+    /* Copy data into respone buffer */
+    memcpy(rsp, macp->regUsbReadBuf, urb->actual_length);
+
+    /* Notify to upper layer */
+    //zfIdlChkRsp(dev, rsp, (u16_t)urb->actual_length);
+    //zfiUsbRegIn(dev, rsp, (u16_t)urb->actual_length);
+    macp->usbCbFunctions.zfcbUsbRegIn(dev, rsp, (u16_t)urb->actual_length);
+
+    /* Issue another USB IN URB */
+    zfLnxSubmitRegInUrb(dev);
+}
+
+u32_t zfLnxSubmitRegInUrb(zdev_t *dev)
+{
+    u32_t ret;
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    /* Submit a rx urb */
+    //ret = zfLnxUsbSubmitBulkUrb(macp->RegInUrb, macp->udev,
+    //        USB_REG_IN_PIPE, USB_DIR_IN, macp->regUsbReadBuf,
+    //        ZM_USB_REG_MAX_BUF_SIZE, zfLnxUsbRegIn_callback, dev);
+    //CWYang(-)
+    //if (ret != 0)
+    //    printk("zfwUsbSubmitBulkUrb fail, status: 0x%08x\n", (int)ret);
+
+    ret = zfLnxUsbSubmitIntUrb(macp->RegInUrb, macp->udev,
+            USB_REG_IN_PIPE, USB_DIR_IN, macp->regUsbReadBuf,
+            ZM_USB_REG_MAX_BUF_SIZE, zfLnxUsbRegIn_callback, dev, 1);
+
+    return ret;
+}
+
+u32_t zfLnxUsbSubmitTxData(zdev_t* dev)
+{
+    u32_t i;
+    u32_t ret;
+    u16_t freeTxUrb;
+    u8_t *puTxBuf = NULL;
+    UsbTxQ_t *TxData;
+    int len = 0;
+    struct usbdrv_private *macp = dev->ml_priv;
+#if ZM_USB_TX_STREAM_MODE == 1
+    u8_t               ii;
+    u16_t              offset = 0;
+    u16_t              usbTxAggCnt;
+    u16_t              *pUsbTxHdr;
+    UsbTxQ_t           *TxQPool[ZM_MAX_TX_AGGREGATE_NUM];
+#endif
+
+    /* First check whether there is a free URB */
+    freeTxUrb = zfLnxGetFreeTxUrb(dev);
+
+    /* If there is no any free Tx Urb */
+    if (freeTxUrb == 0xffff)
+    {
+        //printk(KERN_ERR "Can't get free Tx Urb\n");
+        //printk("CWY - Can't get free Tx Urb\n");
+        return 0xffff;
+    }
+
+#if ZM_USB_TX_STREAM_MODE == 1
+    usbTxAggCnt = zfLnxCheckTxBufferCnt(dev);
+
+    if (usbTxAggCnt >= ZM_MAX_TX_AGGREGATE_NUM)
+    {
+       usbTxAggCnt = ZM_MAX_TX_AGGREGATE_NUM;
+    }
+    else
+    {
+       usbTxAggCnt = 1;
+    }
+
+    //printk("usbTxAggCnt: %d\n", usbTxAggCnt);
+#endif
+
+#if ZM_USB_TX_STREAM_MODE == 1
+    for(ii = 0; ii < usbTxAggCnt; ii++)
+    {
+#endif
+    /* Dequeue the packet from UsbTxBufQ */
+    TxData = zfLnxGetUsbTxBuffer(dev);
+    if (TxData == NULL)
+    {
+        /* Give the urb back */
+        zfLnxPutTxUrb(dev);
+        return 0xffff;
+    }
+
+    /* Point to the freeTxUrb buffer */
+    puTxBuf = macp->txUsbBuf[freeTxUrb];
+
+#if ZM_USB_TX_STREAM_MODE == 1
+    puTxBuf += offset;
+    pUsbTxHdr = (u16_t *)puTxBuf;
+
+    /* Add the packet length and tag information */
+    *pUsbTxHdr++ = TxData->hdrlen + TxData->snapLen +
+             (TxData->buf->len - TxData->offset) +  TxData->tailLen;
+
+    *pUsbTxHdr++ = 0x697e;
+
+    puTxBuf += 4;
+#endif // #ifdef ZM_USB_TX_STREAM_MODE
+
+    /* Copy WLAN header and packet buffer into USB buffer */
+    for(i = 0; i < TxData->hdrlen; i++)
+    {
+        *puTxBuf++ = TxData->hdr[i];
+    }
+
+    /* Copy SNAP header */
+    for(i = 0; i < TxData->snapLen; i++)
+    {
+        *puTxBuf++ = TxData->snap[i];
+    }
+
+    /* Copy packet buffer */
+    for(i = 0; i < TxData->buf->len - TxData->offset; i++)
+    {
+    	//*puTxBuf++ = zmw_rx_buf_readb(dev, TxData->buf, i);
+    	*puTxBuf++ = *(u8_t*)((u8_t*)TxData->buf->data+i+TxData->offset);
+    }
+
+    /* Copy tail */
+    for(i = 0; i < TxData->tailLen; i++)
+    {
+        *puTxBuf++ = TxData->tail[i];
+    }
+
+    len = TxData->hdrlen+TxData->snapLen+TxData->buf->len+TxData->tailLen-TxData->offset;
+
+    #if 0
+    if (TxData->hdrlen != 0)
+    {
+        puTxBuf = macp->txUsbBuf[freeTxUrb];
+        for (i = 0; i < len; i++)
+        {
+            printk("%02x ", puTxBuf[i]);
+            if (i % 16 == 15)
+                printk("\n");
+        }
+        printk("\n");
+    }
+    #endif
+    #if 0
+    /* For debug purpose */
+    if(TxData->hdr[9] & 0x40)
+    {
+        int i;
+        u16_t ctrlLen = TxData->hdr[0] + (TxData->hdr[1] << 8);
+
+        if (ctrlLen != len + 4)
+        {
+        /* Dump control setting */
+        for(i = 0; i < 8; i++)
+        {
+            printk(KERN_ERR "0x%02x ", TxData->hdr[i]);
+        }
+        printk(KERN_ERR "\n");
+
+        printk(KERN_ERR "ctrLen: %d, hdrLen: %d, snapLen: %d\n", ctrlLen, TxData->hdrlen, TxData->snapLen);
+        printk(KERN_ERR "bufLen: %d, tailLen: %d, len: %d\n", TxData->buf->len, TxData->tailLen, len);
+        }
+    }
+    #endif
+
+#if ZM_USB_TX_STREAM_MODE == 1
+    // Add the Length and Tag
+    len += 4;
+
+    //printk("%d packet, length: %d\n", ii+1, len);
+
+    if (ii < (ZM_MAX_TX_AGGREGATE_NUM-1))
+    {
+        /* Pad the buffer to firmware descriptor boundary */
+        offset += (((len-1) / 4) + 1) * 4;
+    }
+
+    if (ii == (ZM_MAX_TX_AGGREGATE_NUM-1))
+    {
+        len += offset;
+    }
+
+    TxQPool[ii] = TxData;
+
+    //DbgPrint("%d packet, offset: %d\n", ii+1, pUsbTxTransfer->offset);
+
+    /* free packet */
+    //zfBufFree(dev, txData->buf);
+    }
+#endif
+    //printk("CWY - call zfwUsbSubmitBulkUrb(), len = 0x%d\n", len);
+    /* Submit a tx urb */
+    ret = zfLnxUsbSubmitBulkUrb(macp->WlanTxDataUrb[freeTxUrb], macp->udev,
+            USB_WLAN_TX_PIPE, USB_DIR_OUT, macp->txUsbBuf[freeTxUrb],
+            len, zfLnxUsbDataOut_callback, dev);
+    //CWYang(-)
+    //if (ret != 0)
+    //    printk("zfwUsbSubmitBulkUrb fail, status: 0x%08x\n", (int)ret);
+
+    /* free packet */
+    //dev_kfree_skb_any(TxData->buf);
+#if ZM_USB_TX_STREAM_MODE == 1
+    for(ii = 0; ii < usbTxAggCnt; ii++)
+        macp->usbCbFunctions.zfcbUsbOutComplete(dev, TxQPool[ii]->buf, 1, TxQPool[ii]->hdr);
+#else
+    macp->usbCbFunctions.zfcbUsbOutComplete(dev, TxData->buf, 1, TxData->hdr);
+#endif
+
+    return ret;
+}
+
+
+
+u32_t zfLnxUsbIn(zdev_t* dev, urb_t *urb, zbuf_t *buf)
+{
+    u32_t ret;
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    /* Submit a rx urb */
+    ret = zfLnxUsbSubmitBulkUrb(urb, macp->udev, USB_WLAN_RX_PIPE,
+            USB_DIR_IN, buf->data, ZM_MAX_RX_BUFFER_SIZE,
+            zfLnxUsbDataIn_callback, dev);
+    //CWYang(-)
+    //if (ret != 0)
+    //    printk("zfwUsbSubmitBulkUrb fail, status: 0x%08x\n", (int)ret);
+
+    return ret;
+}
+
+u32_t zfLnxUsbWriteReg(zdev_t* dev, u32_t* cmd, u16_t cmdLen)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+    u32_t ret;
+
+#ifdef ZM_CONFIG_BIG_ENDIAN
+    int ii = 0;
+
+    for(ii=0; ii<(cmdLen>>2); ii++)
+	cmd[ii] = cpu_to_le32(cmd[ii]);
+#endif
+
+    memcpy(macp->regUsbWriteBuf, cmd, cmdLen);
+
+    /* Issue an USB Out transfer */
+    /* Submit a tx urb */
+    ret = zfLnxUsbSubmitIntUrb(macp->RegOutUrb, macp->udev,
+            USB_REG_OUT_PIPE, USB_DIR_OUT, macp->regUsbWriteBuf,
+            cmdLen, zfLnxUsbRegOut_callback, dev, 1);
+
+    return ret;
+}
+
+
+u32_t zfLnxUsbOut(zdev_t* dev, u8_t *hdr, u16_t hdrlen, u8_t *snap, u16_t snapLen,
+        u8_t *tail, u16_t tailLen, zbuf_t *buf, u16_t offset)
+{
+    u32_t ret;
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    /* Check length of tail buffer */
+    //zm_assert((tailLen <= 16));
+
+    /* Enqueue the packet into UsbTxBufQ */
+    if (zfLnxPutUsbTxBuffer(dev, hdr, hdrlen, snap, snapLen, tail, tailLen, buf, offset) == 0xffff)
+    {
+        /* free packet */
+        //printk("CWY - zfwPutUsbTxBuffer Error, free packet\n");
+        //dev_kfree_skb_any(buf);
+        macp->usbCbFunctions.zfcbUsbOutComplete(dev, buf, 0, hdr);
+        return 0xffff;
+    }
+
+    //return 0;
+    //printk("CWY - call zfwUsbSubmitTxData()\n");
+    ret = zfLnxUsbSubmitTxData(dev);
+    return ret;
+}
+
+void zfLnxInitUsbTxQ(zdev_t* dev)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    printk(KERN_ERR "zfwInitUsbTxQ\n");
+
+    /* Zero memory for UsbTxBufQ */
+    memset(macp->UsbTxBufQ, 0, sizeof(UsbTxQ_t) * ZM_MAX_TX_URB_NUM);
+
+    macp->TxBufHead = 0;
+    macp->TxBufTail = 0;
+    macp->TxUrbHead = 0;
+    macp->TxUrbTail = 0;
+    macp->TxUrbCnt = ZM_MAX_TX_URB_NUM;
+}
+
+void zfLnxInitUsbRxQ(zdev_t* dev)
+{
+    u16_t i;
+    zbuf_t *buf;
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    /* Zero memory for UsbRxBufQ */
+    memset(macp->UsbRxBufQ, 0, sizeof(zbuf_t *) * ZM_MAX_RX_URB_NUM);
+
+    macp->RxBufHead = 0;
+
+    for (i = 0; i < ZM_MAX_RX_URB_NUM; i++)
+    {
+        //buf = zfwBufAllocate(dev, ZM_MAX_RX_BUFFER_SIZE);
+        buf = dev_alloc_skb(ZM_MAX_RX_BUFFER_SIZE);
+        macp->UsbRxBufQ[i] = buf;
+    }
+
+    //macp->RxBufTail = ZM_MAX_RX_URB_NUM - 1;
+    macp->RxBufTail = 0;
+
+    /* Submit all Rx urbs */
+    for (i = 0; i < ZM_MAX_RX_URB_NUM; i++)
+    {
+        zfLnxPutUsbRxBuffer(dev, macp->UsbRxBufQ[i]);
+        zfLnxUsbIn(dev, macp->WlanRxDataUrb[i], macp->UsbRxBufQ[i]);
+    }
+}
+
+
+
+u32_t zfLnxUsbSubmitBulkUrb(urb_t *urb, struct usb_device *usb, u16_t epnum, u16_t direction,
+        void *transfer_buffer, int buffer_length, usb_complete_t complete, void *context)
+{
+    u32_t ret;
+
+    if(direction == USB_DIR_OUT)
+    {
+        usb_fill_bulk_urb(urb, usb, usb_sndbulkpipe(usb, epnum),
+                transfer_buffer, buffer_length, complete, context);
+
+        urb->transfer_flags |= URB_ZERO_PACKET;
+    }
+    else
+    {
+        usb_fill_bulk_urb(urb, usb, usb_rcvbulkpipe(usb, epnum),
+                transfer_buffer, buffer_length, complete, context);
+    }
+
+    if (epnum == 4)
+    {
+        if (urb->hcpriv)
+        {
+            //printk("CWY - urb->hcpriv set by unknown reason, reset it\n");
+            //urb->hcpriv = 0;
+        }
+    }
+
+    ret = usb_submit_urb(urb, GFP_ATOMIC);
+    if ((epnum == 4) & (ret != 0))
+    {
+        //printk("CWY - ret = %x\n", ret);
+    }
+    return ret;
+}
+
+u32_t zfLnxUsbSubmitIntUrb(urb_t *urb, struct usb_device *usb, u16_t epnum, u16_t direction,
+        void *transfer_buffer, int buffer_length, usb_complete_t complete, void *context,
+        u32_t interval)
+{
+    u32_t ret;
+
+    if(direction == USB_DIR_OUT)
+    {
+        usb_fill_int_urb(urb, usb, usb_sndbulkpipe(usb, epnum),
+                transfer_buffer, buffer_length, complete, context, interval);
+    }
+    else
+    {
+        usb_fill_int_urb(urb, usb, usb_rcvbulkpipe(usb, epnum),
+                transfer_buffer, buffer_length, complete, context, interval);
+    }
+
+    ret = usb_submit_urb(urb, GFP_ATOMIC);
+
+    return ret;
+}
+
+#ifdef ZM_ENABLE_CENC
+int zfLnxCencSendMsg(struct sock *netlink_sk, u_int8_t *msg, int len)
+{
+#define COMMTYPE_GROUP   8
+#define WAI_K_MSG        0x11
+
+	int ret = -1;
+	int size;
+	unsigned char *old_tail;
+	struct sk_buff *skb;
+	struct nlmsghdr *nlh;
+	char *pos = NULL;
+
+	size = NLMSG_SPACE(len);
+	skb = alloc_skb(size, GFP_ATOMIC);
+
+	if(skb == NULL)
+	{
+		printk("dev_alloc_skb failure \n");
+		goto out;
+	}
+	old_tail = skb->tail;
+
+	/*ÌîдÊý¾Ý±¨Ïà¹ØÐÅÏ¢*/
+	nlh = NLMSG_PUT(skb, 0, 0, WAI_K_MSG, size-sizeof(*nlh));
+	pos = NLMSG_DATA(nlh);
+	memset(pos, 0, len);
+
+	/*´«Êäµ½Óû§¿Õ¼äµÄÊý¾Ý*/
+	memcpy(pos, msg,  len);
+	/*¼ÆËã¾­¹ý×Ö½Ú¶ÔÆäºóµÄÊý¾Ýʵ¼Ê³¤¶È*/
+	nlh->nlmsg_len = skb->tail - old_tail;
+	NETLINK_CB(skb).dst_group = COMMTYPE_GROUP;
+	netlink_broadcast(netlink_sk, skb, 0, COMMTYPE_GROUP, GFP_ATOMIC);
+	ret = 0;
+out:
+	return ret;
+nlmsg_failure: /*NLMSG_PUT ʧ°Ü£¬Ôò³·ÏúÌ×½Ó×Ö»º´æ*/
+  	if(skb)
+    		kfree_skb(skb);
+	goto out;
+
+#undef COMMTYPE_GROUP
+#undef WAI_K_MSG
+}
+#endif //ZM_ENABLE_CENC
+
+/* Simply return 0xffff if VAP function is not supported */
+u16_t zfLnxGetVapId(zdev_t* dev)
+{
+    u16_t i;
+
+    for (i=0; i<ZM_VAP_PORT_NUMBER; i++)
+    {
+        if (vap[i].dev == dev)
+        {
+            return i;
+        }
+    }
+    return 0xffff;
+}
+
+u32_t zfwReadReg(zdev_t* dev, u32_t offset)
+{
+    return 0;
+}
+
+#ifndef INIT_WORK
+#define work_struct tq_struct
+
+#define schedule_work(a)  schedule_task(a)
+
+#define flush_scheduled_work  flush_scheduled_tasks
+#define INIT_WORK(_wq, _routine, _data)  INIT_TQUEUE(_wq, _routine, _data)
+#define PREPARE_WORK(_wq, _routine, _data)  PREPARE_TQUEUE(_wq, _routine, _data)
+#endif
+
+#define KEVENT_WATCHDOG        0x00000001
+
+u32_t smp_kevent_Lock = 0;
+
+void kevent(struct work_struct *work)
+{
+    struct usbdrv_private *macp =
+               container_of(work, struct usbdrv_private, kevent);
+    zdev_t *dev = macp->device;
+
+    if (macp == NULL)
+    {
+        return;
+    }
+
+    if (test_and_set_bit(0, (void *)&smp_kevent_Lock))
+    {
+        //schedule_work(&macp->kevent);
+        return;
+    }
+
+    down(&macp->ioctl_sem);
+
+    if (test_and_clear_bit(KEVENT_WATCHDOG, &macp->kevent_flags))
+    {
+    extern u16_t zfHpStartRecv(zdev_t *dev);
+        //zfiHwWatchDogReinit(dev);
+        printk(("\n ************ Hw watchDog occur!! ************** \n"));
+        zfiWlanSuspend(dev);
+        zfiWlanResume(dev,0);
+        zfHpStartRecv(dev);
+    }
+
+    clear_bit(0, (void *)&smp_kevent_Lock);
+    up(&macp->ioctl_sem);
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                 zfLnxCreateThread            */
+/*      Create a Thread                                                 */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      always 0                                                        */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Yuan-Gu Wei         Atheros Communications, INC.    2007.3      */
+/*                                                                      */
+/************************************************************************/
+u8_t zfLnxCreateThread(zdev_t *dev)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    /* Create Mutex and keventd */
+    INIT_WORK(&macp->kevent, kevent);
+    init_MUTEX(&macp->ioctl_sem);
+
+    return 0;
+}
+
+/************************************************************************/
+/*                                                                      */
+/*    FUNCTION DESCRIPTION                 zfLnxSignalThread            */
+/*      Signal Thread with Flag                                         */
+/*                                                                      */
+/*    INPUTS                                                            */
+/*      dev : device pointer                                            */
+/*      flag : signal thread flag                                       */
+/*                                                                      */
+/*    OUTPUTS                                                           */
+/*      none                                                            */
+/*                                                                      */
+/*    AUTHOR                                                            */
+/*      Yuan-Gu Wei         Atheros Communications, INC.    2007.3      */
+/*                                                                      */
+/************************************************************************/
+void zfLnxSignalThread(zdev_t *dev, int flag)
+{
+    struct usbdrv_private *macp = dev->ml_priv;
+
+    if (macp == NULL)
+    {
+        printk("macp is NULL\n");
+        return;
+    }
+
+    if (0 && macp->kevent_ready != 1)
+    {
+        printk("Kevent not ready\n");
+        return;
+    }
+
+    set_bit(flag, &macp->kevent_flags);
+
+    if (!schedule_work(&macp->kevent))
+    {
+        //Fails is Normal
+        //printk(KERN_ERR "schedule_task failed, flag = %x\n", flag);
+    }
+}
+
+/* Notify wrapper todo redownload firmware and reinit procedure when */
+/* hardware watchdog occur : zfiHwWatchDogReinit() */
+void zfLnxWatchDogNotify(zdev_t* dev)
+{
+    zfLnxSignalThread(dev, KEVENT_WATCHDOG);
+}
+
+/* Query Durantion of Active Scan */
+void zfwGetActiveScanDur(zdev_t* dev, u8_t* Dur)
+{
+    *Dur = 30; // default 30 ms
+}
+
+void zfwGetShowZeroLengthSSID(zdev_t* dev, u8_t* Dur)
+{
+    *Dur = 0;
+}
+
diff --git a/drivers/staging/otus/zdcompat.h b/drivers/staging/otus/zdcompat.h
new file mode 100644
index 00000000000..8acf400fbc0
--- /dev/null
+++ b/drivers/staging/otus/zdcompat.h
@@ -0,0 +1,62 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : zdcompat.h                                            */
+/*                                                                      */
+/*  Abstract                                                            */
+/*     This module contains function defintion for compatibility.       */
+/*                                                                      */
+/*  NOTES                                                               */
+/*     Platform dependent.                                              */
+/*                                                                      */
+/************************************************************************/
+
+#ifndef _ZDCOMPAT_H
+#define _ZDCOMPAT_H
+
+
+#ifndef DECLARE_TASKLET
+#define tasklet_schedule(a)   schedule_task(a)
+#endif
+
+#undef netdevice_t
+typedef struct net_device netdevice_t;
+
+#ifdef WIRELESS_EXT
+#if (WIRELESS_EXT < 13)
+struct iw_request_info
+{
+        __u16           cmd;            /* Wireless Extension command */
+        __u16           flags;          /* More to come ;-) */
+};
+#endif
+#endif
+
+#ifndef IRQ_NONE
+typedef void irqreturn_t;
+#define IRQ_NONE
+#define IRQ_HANDLED
+#define IRQ_RETVAL(x)
+#endif
+
+#ifndef in_atomic
+#define in_atomic()  0
+#endif
+
+#define USB_QUEUE_BULK 0
+
+
+#endif
diff --git a/drivers/staging/otus/zdusb.c b/drivers/staging/otus/zdusb.c
new file mode 100644
index 00000000000..78f1d2224fa
--- /dev/null
+++ b/drivers/staging/otus/zdusb.c
@@ -0,0 +1,239 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : zdusb.c                                               */
+/*                                                                      */
+/*  Abstract                                                            */
+/*     This module contains plug and play handling for USB device driver*/
+/*                                                                      */
+/*  NOTES                                                               */
+/*     Platform dependent.                                              */
+/*                                                                      */
+/************************************************************************/
+
+#ifdef MODVERSIONS
+#include <linux/modversions.h>
+#endif
+
+#include <linux/module.h>
+#include <linux/usb.h>
+
+#include "usbdrv.h"
+#include "zdusb.h"
+
+int zfLnxAllocAllUrbs(struct usbdrv_private *macp);
+void zfLnxFreeAllUrbs(struct usbdrv_private *macp);
+void zfLnxUnlinkAllUrbs(struct usbdrv_private *macp);
+
+MODULE_AUTHOR("Atheros Communications");
+MODULE_DESCRIPTION("Atheros 802.11n Wireless LAN adapter");
+MODULE_LICENSE("Dual BSD/GPL");
+
+static const char driver_name[] = "Otus";
+
+/* table of devices that work with this driver */
+static struct usb_device_id zd1221_ids [] = {
+	{ USB_DEVICE(VENDOR_ATHR, PRODUCT_AR9170) },
+        { USB_DEVICE(VENDOR_DLINK, PRODUCT_DWA160A) },
+	{ USB_DEVICE(0x0846, 0x9010) },
+	{ }					/* Terminating entry */
+};
+
+MODULE_DEVICE_TABLE(usb, zd1221_ids);
+
+extern u8_t zfLnxInitSetup(struct net_device *dev, struct usbdrv_private *macp);
+extern int usbdrv_close(struct net_device *dev);
+extern u8_t zfLnxClearStructs(struct net_device *dev);
+extern int zfWdsClose(struct net_device *dev);
+extern int zfUnregisterWdsDev(struct net_device* parentDev, u16_t wdsId);
+extern int zfLnxVapClose(struct net_device *dev);
+extern int zfLnxUnregisterVapDev(struct net_device* parentDev, u16_t vapId);
+
+/* WDS */
+extern struct zsWdsStruct wds[ZM_WDS_PORT_NUMBER];
+
+/* VAP */
+extern struct zsVapStruct vap[ZM_VAP_PORT_NUMBER];
+
+static int zfLnxProbe(struct usb_interface *interface,
+	const struct usb_device_id *id)
+{
+    struct usb_device *dev = interface_to_usbdev(interface);
+
+    struct net_device *net = NULL;
+    struct usbdrv_private *macp = NULL;
+    int vendor_id, product_id;
+    int result = 0;
+
+    usb_get_dev(dev);
+
+    vendor_id = dev->descriptor.idVendor;
+    product_id = dev->descriptor.idProduct;
+
+#ifdef HMAC_DEBUG
+    printk(KERN_NOTICE "vendor_id = %04x\n", vendor_id);
+    printk(KERN_NOTICE "product_id = %04x\n", product_id);
+
+    if (dev->speed == USB_SPEED_HIGH)
+        printk(KERN_NOTICE "USB 2.0 Host\n");
+    else
+        printk(KERN_NOTICE "USB 1.1 Host\n");
+#endif
+
+    if (!(macp = kmalloc(sizeof(struct usbdrv_private), GFP_KERNEL)))
+    {
+        printk(KERN_ERR "out of memory allocating device structure\n");
+        result = -ENOMEM;
+        goto fail;
+    }
+
+    /* Zero the memory */
+    memset(macp, 0, sizeof(struct usbdrv_private));
+
+    net = alloc_etherdev(0);
+
+    if (net == NULL)
+    {
+        printk(KERN_ERR "zfLnxProbe: Not able to alloc etherdev struct\n");
+        result = -ENOMEM;
+        goto fail1;
+    }
+
+    strcpy(net->name, "ath%d");
+
+    net->ml_priv = macp;   //kernel 2.6
+    macp->udev = dev;
+    macp->device = net;
+
+    /* set up the endpoint information */
+    /* check out the endpoints */
+    macp->interface = interface;
+
+    //init_waitqueue_head(&macp->regSet_wait);
+    //init_waitqueue_head(&macp->iorwRsp_wait);
+    //init_waitqueue_head(&macp->term_wait);
+
+    if (!zfLnxAllocAllUrbs(macp))
+    {
+        result = -ENOMEM;
+        goto fail2;
+    }
+
+    if (!zfLnxInitSetup(net, macp))
+    {
+        result = -EIO;
+        goto fail3;
+    }
+    else
+    {
+        usb_set_intfdata(interface, macp);
+        SET_NETDEV_DEV(net, &interface->dev);
+
+        if (register_netdev(net) != 0)
+        {
+            usb_set_intfdata(interface, NULL);
+            goto fail3;
+        }
+    }
+
+    netif_carrier_off(net);
+    goto done;
+
+fail3:
+    zfLnxFreeAllUrbs(macp);
+fail2:
+    free_netdev(net);  //kernel 2.6
+fail1:
+    kfree(macp);
+
+fail:
+    usb_put_dev(dev);
+    macp = NULL;
+
+done:
+    return result;
+}
+
+static void zfLnxDisconnect(struct usb_interface *interface)
+{
+    struct usbdrv_private *macp = (struct usbdrv_private *) usb_get_intfdata(interface);
+
+    printk(KERN_DEBUG "zfLnxDisconnect\n");
+
+    if (!macp)
+    {
+        printk(KERN_ERR "unregistering non-existant device\n");
+        return;
+    }
+
+    if (macp->driver_isolated)
+    {
+        if (macp->device->flags & IFF_UP)
+            usbdrv_close(macp->device);
+    }
+
+#if 0
+    /* Close WDS */
+    //zfWdsClose(wds[0].dev);
+    /* Unregister WDS */
+    //zfUnregisterWdsDev(macp->device, 0);
+
+    /* Close VAP */
+    zfLnxVapClose(vap[0].dev);
+    /* Unregister VAP */
+    zfLnxUnregisterVapDev(macp->device, 0);
+#endif
+
+    zfLnxClearStructs(macp->device);
+
+    unregister_netdev(macp->device);
+
+    usb_put_dev(interface_to_usbdev(interface));
+
+    //printk(KERN_ERR "3. zfLnxUnlinkAllUrbs\n");
+    //zfLnxUnlinkAllUrbs(macp);
+
+    /* Free network interface */
+    free_netdev(macp->device);
+
+    zfLnxFreeAllUrbs(macp);
+    //zfLnxClearStructs(macp->device);
+    kfree(macp);
+    macp = NULL;
+
+    usb_set_intfdata(interface, NULL);
+}
+
+static struct usb_driver zd1221_driver = {
+	.name         = driver_name,
+	.probe        = zfLnxProbe,
+	.disconnect   = zfLnxDisconnect,
+	.id_table     = zd1221_ids,
+};
+
+int __init zfLnxIinit(void)
+{
+    printk(KERN_NOTICE "%s - version %s\n",  DRIVER_NAME, VERSIONID);
+    return usb_register(&zd1221_driver);
+}
+
+void __exit zfLnxExit(void)
+{
+    usb_deregister(&zd1221_driver);
+}
+
+module_init(zfLnxIinit);
+module_exit(zfLnxExit);
diff --git a/drivers/staging/otus/zdusb.h b/drivers/staging/otus/zdusb.h
new file mode 100644
index 00000000000..656dc212ade
--- /dev/null
+++ b/drivers/staging/otus/zdusb.h
@@ -0,0 +1,43 @@
+/*
+ * Copyright (c) 2007-2008 Atheros Communications Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+/*                                                                      */
+/*  Module Name : zdusb.h                                               */
+/*                                                                      */
+/*  Abstract                                                            */
+/*     This module contains definitions for USB device driver           */
+/*                                                                      */
+/*  NOTES                                                               */
+/*     Platform dependent.                                              */
+/*                                                                      */
+/************************************************************************/
+
+#ifndef _ZDUSB_H
+#define _ZDUSB_H
+
+#ifndef DRIVER_NAME
+#define DRIVER_NAME             "arusb"
+#endif
+
+#define VERSIONID               "0.0.0.999"
+
+/* Define these values to match your device */
+#define VENDOR_ATHR             0x0CF3  //Atheros
+#define PRODUCT_AR9170          0x9170
+
+#define VENDOR_DLINK            0x07D1  //Dlink
+#define PRODUCT_DWA160A         0x3C10
+
+#endif
diff --git a/drivers/staging/panel/Kconfig b/drivers/staging/panel/Kconfig
new file mode 100644
index 00000000000..c4b30f2a549
--- /dev/null
+++ b/drivers/staging/panel/Kconfig
@@ -0,0 +1,278 @@
+config PANEL
+	tristate "Parallel port LCD/Keypad Panel support"
+	depends on PARPORT
+	---help---
+	  Say Y here if you have an HD44780 or KS-0074 LCD connected to your
+	  parallel port. This driver also features 4 and 6-key keypads. The LCD
+	  is accessible through the /dev/lcd char device (10, 156), and the
+	  keypad through /dev/keypad (10, 185). Both require misc device to be
+	  enabled. This code can either be compiled as a module, or linked into
+	  the kernel and started at boot. If you don't understand what all this
+	  is about, say N.
+
+config PANEL_PARPORT
+	int "Default parallel port number (0=LPT1)"
+	depends on PANEL
+	range 0 255
+	default "0"
+	---help---
+	  This is the index of the parallel port the panel is connected to. One
+	  driver instance only supports one parallel port, so if your keypad
+	  and LCD are connected to two separate ports, you have to start two
+	  modules with different arguments. Numbering starts with '0' for LPT1,
+	  and so on.
+
+config PANEL_PROFILE
+	int "Default panel profile (0-5, 0=custom)"
+	depends on PANEL
+	range 0 5
+	default "5"
+	---help---
+	  To ease configuration, the driver supports different configuration
+	  profiles for past and recent wirings. These profiles can also be
+	  used to define an approximative configuration, completed by a few
+	  other options. Here are the profiles :
+
+	    0 = custom (see further)
+	    1 = 2x16 parallel LCD, old keypad
+	    2 = 2x16 serial LCD (KS-0074), new keypad
+	    3 = 2x16 parallel LCD (Hantronix), no keypad
+	    4 = 2x16 parallel LCD (Nexcom NSA1045) with Nexcom's keypad
+	    5 = 2x40 parallel LCD (old one), with old keypad
+
+	  Custom configurations allow you to define how your display is
+	  wired to the parallel port, and how it works. This is only intended
+	  for experts.
+
+config PANEL_KEYPAD
+	depends on PANEL && PANEL_PROFILE="0"
+	int "Keypad type (0=none, 1=old 6 keys, 2=new 6 keys, 3=Nexcom 4 keys)"
+	range 0 4
+	default 0
+	---help---
+	  This enables and configures a keypad connected to the parallel port.
+	  The keys will be read from character device 10,185. Valid values are :
+
+	    0 : do not enable this driver
+	    1 : old 6 keys keypad
+	    2 : new 6 keys keypad, as used on the server at www.ant-computing.com
+	    3 : Nexcom NSA1045's 4 keys keypad
+
+	  New profiles can be described in the driver source. The driver also
+	  supports simultaneous keys pressed when the keypad supports them.
+
+config PANEL_LCD
+	depends on PANEL && PANEL_PROFILE="0"
+	int "LCD type (0=none, 1=custom, 2=old //, 3=ks0074, 4=hantronix, 5=Nexcom)"
+	range 0 5
+	default 0
+	---help---
+	   This enables and configures an LCD connected to the parallel port.
+	   The driver includes an interpreter for escape codes starting with
+	   '\e[L' which are specific to the LCD, and a few ANSI codes. The
+	   driver will be registered as character device 10,156, usually
+	   under the name '/dev/lcd'. There are a total of 6 supported types :
+
+	     0 : do not enable the driver
+	     1 : custom configuration and wiring (see further)
+	     2 : 2x16 & 2x40 parallel LCD (old wiring)
+	     3 : 2x16 serial LCD (KS-0074 based)
+	     4 : 2x16 parallel LCD (Hantronix wiring)
+	     5 : 2x16 parallel LCD (Nexcom wiring)
+
+	   When type '1' is specified, other options will appear to configure
+	   more precise aspects (wiring, dimensions, protocol, ...). Please note
+	   that those values changed from the 2.4 driver for better consistency.
+
+config PANEL_LCD_HEIGHT
+	depends on PANEL && PANEL_PROFILE="0" && PANEL_LCD="1"
+	int "Number of lines on the LCD (1-2)"
+	range 1 2
+	default 2
+	---help---
+	  This is the number of visible character lines on the LCD in custom profile.
+	  It can either be 1 or 2.
+
+config PANEL_LCD_WIDTH
+	depends on PANEL && PANEL_PROFILE="0" && PANEL_LCD="1"
+	int "Number of characters per line on the LCD (1-40)"
+	range 1 40
+	default 40
+	---help---
+	  This is the number of characters per line on the LCD in custom profile.
+	  Common values are 16,20,24,40.
+
+config PANEL_LCD_BWIDTH
+	depends on PANEL && PANEL_PROFILE="0" && PANEL_LCD="1"
+	int "Internal LCD line width (1-40, 40 by default)"
+	range 1 40
+	default 40
+	---help---
+	  Most LCDs use a standard controller which supports hardware lines of 40
+	  characters, although sometimes only 16, 20 or 24 of them are really wired
+	  to the terminal. This results in some non-visible but adressable characters,
+	  and is the case for most parallel LCDs. Other LCDs, and some serial ones,
+	  however, use the same line width internally as what is visible. The KS0074
+	  for example, uses 16 characters per line for 16 visible characters per line.
+
+	  This option lets you configure the value used by your LCD in 'custom' profile.
+	  If you don't know, put '40' here.
+
+config PANEL_LCD_HWIDTH
+	depends on PANEL && PANEL_PROFILE="0" && PANEL_LCD="1"
+	int "Hardware LCD line width (1-64, 64 by default)"
+	range 1 64
+	default 64
+	---help---
+	  Most LCDs use a single address bit to differentiate line 0 and line 1. Since
+	  some of them need to be able to address 40 chars with the lower bits, they
+	  often use the immediately superior power of 2, which is 64, to address the
+	  next line.
+
+	  If you don't know what your LCD uses, in doubt let 16 here for a 2x16, and
+	  64 here for a 2x40.
+
+config PANEL_LCD_CHARSET
+	depends on PANEL && PANEL_PROFILE="0" && PANEL_LCD="1"
+	int "LCD character set (0=normal, 1=KS0074)"
+	range 0 1
+	default 0
+	---help---
+	  Some controllers such as the KS0074 use a somewhat strange character set
+	  where many symbols are at unusual places. The driver knows how to map
+	  'standard' ASCII characters to the character sets used by these controllers.
+	  Valid values are :
+
+	     0 : normal (untranslated) character set
+	     1 : KS0074 character set
+
+	  If you don't know, use the normal one (0).
+
+config PANEL_LCD_PROTO
+	depends on PANEL && PANEL_PROFILE="0" && PANEL_LCD="1"
+	int "LCD communication mode (0=parallel 8 bits, 1=serial)"
+	range 0 1
+	default 0
+	---help---
+	  This driver now supports any serial or parallel LCD wired to a parallel
+	  port. But before assigning signals, the driver needs to know if it will
+	  be driving a serial LCD or a parallel one. Serial LCDs only use 2 wires
+	  (SDA/SCL), while parallel ones use 2 or 3 wires for the control signals
+	  (E, RS, sometimes RW), and 4 or 8 for the data. Use 0 here for a 8 bits
+	  parallel LCD, and 1 for a serial LCD.
+
+config PANEL_LCD_PIN_E
+	depends on PANEL && PANEL_PROFILE="0" && PANEL_LCD="1" && PANEL_LCD_PROTO="0"
+        int "Parallel port pin number & polarity connected to the LCD E signal (-17...17) "
+	range -17 17
+	default 14
+	---help---
+	  This describes the number of the parallel port pin to which the LCD 'E'
+	  signal has been connected. It can be :
+
+	          0 : no connection (eg: connected to ground)
+	      1..17 : directly connected to any of these pins on the DB25 plug
+	    -1..-17 : connected to the same pin through an inverter (eg: transistor).
+
+	  Default for the 'E' pin in custom profile is '14' (AUTOFEED).
+
+config PANEL_LCD_PIN_RS
+	depends on PANEL && PANEL_PROFILE="0" && PANEL_LCD="1" && PANEL_LCD_PROTO="0"
+        int "Parallel port pin number & polarity connected to the LCD RS signal (-17...17) "
+	range -17 17
+	default 17
+	---help---
+	  This describes the number of the parallel port pin to which the LCD 'RS'
+	  signal has been connected. It can be :
+
+	          0 : no connection (eg: connected to ground)
+	      1..17 : directly connected to any of these pins on the DB25 plug
+	    -1..-17 : connected to the same pin through an inverter (eg: transistor).
+
+	  Default for the 'RS' pin in custom profile is '17' (SELECT IN).
+
+config PANEL_LCD_PIN_RW
+	depends on PANEL && PANEL_PROFILE="0" && PANEL_LCD="1" && PANEL_LCD_PROTO="0"
+        int "Parallel port pin number & polarity connected to the LCD RW signal (-17...17) "
+	range -17 17
+	default 16
+	---help---
+	  This describes the number of the parallel port pin to which the LCD 'RW'
+	  signal has been connected. It can be :
+
+	          0 : no connection (eg: connected to ground)
+	      1..17 : directly connected to any of these pins on the DB25 plug
+	    -1..-17 : connected to the same pin through an inverter (eg: transistor).
+
+	  Default for the 'RW' pin in custom profile is '16' (INIT).
+
+config PANEL_LCD_PIN_SCL
+	depends on PANEL && PANEL_PROFILE="0" && PANEL_LCD="1" && PANEL_LCD_PROTO!="0"
+        int "Parallel port pin number & polarity connected to the LCD SCL signal (-17...17) "
+	range -17 17
+	default 1
+	---help---
+	  This describes the number of the parallel port pin to which the serial
+	  LCD 'SCL' signal has been connected. It can be :
+
+	          0 : no connection (eg: connected to ground)
+	      1..17 : directly connected to any of these pins on the DB25 plug
+	    -1..-17 : connected to the same pin through an inverter (eg: transistor).
+
+	  Default for the 'SCL' pin in custom profile is '1' (STROBE).
+
+config PANEL_LCD_PIN_SDA
+	depends on PANEL && PANEL_PROFILE="0" && PANEL_LCD="1" && PANEL_LCD_PROTO!="0"
+        int "Parallel port pin number & polarity connected to the LCD SDA signal (-17...17) "
+	range -17 17
+	default 2
+	---help---
+	  This describes the number of the parallel port pin to which the serial
+	  LCD 'SDA' signal has been connected. It can be :
+
+	          0 : no connection (eg: connected to ground)
+	      1..17 : directly connected to any of these pins on the DB25 plug
+	    -1..-17 : connected to the same pin through an inverter (eg: transistor).
+
+	  Default for the 'SDA' pin in custom profile is '2' (D0).
+
+config PANEL_LCD_PIN_BL
+	depends on PANEL && PANEL_PROFILE="0" && PANEL_LCD="1"
+        int "Parallel port pin number & polarity connected to the LCD backlight signal (-17...17) "
+	range -17 17
+	default 0
+	---help---
+	  This describes the number of the parallel port pin to which the LCD 'BL' signal
+          has been connected. It can be :
+
+	          0 : no connection (eg: connected to ground)
+	      1..17 : directly connected to any of these pins on the DB25 plug
+	    -1..-17 : connected to the same pin through an inverter (eg: transistor).
+
+	  Default for the 'BL' pin in custom profile is '0' (uncontrolled).
+
+config PANEL_CHANGE_MESSAGE
+	depends on PANEL
+	bool "Change LCD initialization message ?"
+	default "n"
+	---help---
+	  This allows you to replace the boot message indicating the kernel version
+	  and the driver version with a custom message. This is useful on appliances
+	  where a simple 'Starting system' message can be enough to stop a customer
+	  from worrying.
+
+	  If you say 'Y' here, you'll be able to choose a message yourself. Otherwise,
+	  say 'N' and keep the default message with the version.
+
+config PANEL_BOOT_MESSAGE
+	depends on PANEL && PANEL_CHANGE_MESSAGE="y"
+	string "New initialization message"
+	default ""
+	---help---
+	  This allows you to replace the boot message indicating the kernel version
+	  and the driver version with a custom message. This is useful on appliances
+	  where a simple 'Starting system' message can be enough to stop a customer
+	  from worrying.
+
+	  An empty message will only clear the display at driver init time. Any other
+	  printf()-formatted message is valid with newline and escape codes.
diff --git a/drivers/staging/panel/Makefile b/drivers/staging/panel/Makefile
new file mode 100644
index 00000000000..747c238b82f
--- /dev/null
+++ b/drivers/staging/panel/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_PANEL)		+= panel.o
diff --git a/drivers/staging/panel/TODO b/drivers/staging/panel/TODO
new file mode 100644
index 00000000000..a4be749bcdf
--- /dev/null
+++ b/drivers/staging/panel/TODO
@@ -0,0 +1,9 @@
+TODO:
+	- checkpatch.pl cleanups
+	- Lindent
+	- review major/minor usages
+	- review userspace api
+	- see if all of this could be easier done in userspace instead.
+
+Please send patches to Greg Kroah-Hartman <greg@kroah.com> and
+Willy Tarreau <willy@meta-x.org>
diff --git a/drivers/staging/panel/lcd-panel-cgram.txt b/drivers/staging/panel/lcd-panel-cgram.txt
new file mode 100644
index 00000000000..f9ceef4322a
--- /dev/null
+++ b/drivers/staging/panel/lcd-panel-cgram.txt
@@ -0,0 +1,24 @@
+Some LCDs allow you to define up to 8 characters, mapped to ASCII
+characters 0 to 7. The escape code to define a new character is
+'\e[LG' followed by one digit from 0 to 7, representing the character
+number, and up to 8 couples of hex digits terminated by a semi-colon
+(';'). Each couple of digits represents a line, with 1-bits for each
+illuminated pixel with LSB on the right. Lines are numberred from the
+top of the character to the bottom. On a 5x7 matrix, only the 5 lower
+bits of the 7 first bytes are used for each character. If the string
+is incomplete, only complete lines will be redefined. Here are some
+examples :
+
+  printf "\e[LG0010101050D1F0C04;"  => 0 = [enter]
+  printf "\e[LG1040E1F0000000000;"  => 1 = [up]
+  printf "\e[LG2000000001F0E0400;"  => 2 = [down]
+  printf "\e[LG3040E1F001F0E0400;"  => 3 = [up-down]
+  printf "\e[LG40002060E1E0E0602;"  => 4 = [left]
+  printf "\e[LG500080C0E0F0E0C08;"  => 5 = [right]
+  printf "\e[LG60016051516141400;"  => 6 = "IP"
+
+  printf "\e[LG00103071F1F070301;"  => big speaker
+  printf "\e[LG00002061E1E060200;"  => small speaker
+
+Willy
+
diff --git a/drivers/staging/panel/panel.c b/drivers/staging/panel/panel.c
new file mode 100644
index 00000000000..5ffe269c238
--- /dev/null
+++ b/drivers/staging/panel/panel.c
@@ -0,0 +1,2193 @@
+/*
+ * Front panel driver for Linux
+ * Copyright (C) 2000-2008, Willy Tarreau <w@1wt.eu>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version
+ * 2 of the License, or (at your option) any later version.
+ *
+ * This code drives an LCD module (/dev/lcd), and a keypad (/dev/keypad)
+ * connected to a parallel printer port.
+ *
+ * The LCD module may either be an HD44780-like 8-bit parallel LCD, or a 1-bit
+ * serial module compatible with Samsung's KS0074. The pins may be connected in
+ * any combination, everything is programmable.
+ *
+ * The keypad consists in a matrix of push buttons connecting input pins to
+ * data output pins or to the ground. The combinations have to be hard-coded
+ * in the driver, though several profiles exist and adding new ones is easy.
+ *
+ * Several profiles are provided for commonly found LCD+keypad modules on the
+ * market, such as those found in Nexcom's appliances.
+ *
+ * FIXME:
+ *      - the initialization/deinitialization process is very dirty and should
+ *        be rewritten. It may even be buggy.
+ *
+ * TODO:
+ *	- document 24 keys keyboard (3 rows of 8 cols, 32 diodes + 2 inputs)
+ *      - make the LCD a part of a virtual screen of Vx*Vy
+ *	- make the inputs list smp-safe
+ *      - change the keyboard to a double mapping : signals -> key_id -> values
+ *        so that applications can change values without knowing signals
+ *
+ */
+
+#include <linux/module.h>
+
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/signal.h>
+#include <linux/sched.h>
+#include <linux/spinlock.h>
+#include <linux/smp_lock.h>
+#include <linux/interrupt.h>
+#include <linux/miscdevice.h>
+#include <linux/slab.h>
+#include <linux/ioport.h>
+#include <linux/fcntl.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <linux/ctype.h>
+#include <linux/parport.h>
+#include <linux/version.h>
+#include <linux/list.h>
+#include <linux/notifier.h>
+#include <linux/reboot.h>
+#include <linux/utsrelease.h>
+
+#include <linux/io.h>
+#include <asm/uaccess.h>
+#include <asm/system.h>
+
+#define LCD_MINOR		156
+#define KEYPAD_MINOR		185
+
+#define PANEL_VERSION		"0.9.5"
+
+#define LCD_MAXBYTES		256	/* max burst write */
+
+#define KEYPAD_BUFFER		64
+#define INPUT_POLL_TIME		(HZ/50)	/* poll the keyboard this every second */
+#define KEYPAD_REP_START	(10)	/* a key starts to repeat after this times INPUT_POLL_TIME */
+#define KEYPAD_REP_DELAY	(2)	/* a key repeats this times INPUT_POLL_TIME */
+
+#define FLASH_LIGHT_TEMPO	(200)	/* keep the light on this times INPUT_POLL_TIME for each flash */
+
+/* converts an r_str() input to an active high, bits string : 000BAOSE */
+#define PNL_PINPUT(a)		((((unsigned char)(a)) ^ 0x7F) >> 3)
+
+#define PNL_PBUSY		0x80	/* inverted input, active low */
+#define PNL_PACK		0x40	/* direct input, active low */
+#define PNL_POUTPA		0x20	/* direct input, active high */
+#define PNL_PSELECD		0x10	/* direct input, active high */
+#define PNL_PERRORP		0x08	/* direct input, active low */
+
+#define PNL_PBIDIR		0x20	/* bi-directional ports */
+#define PNL_PINTEN		0x10	/* high to read data in or-ed with data out */
+#define PNL_PSELECP		0x08	/* inverted output, active low */
+#define PNL_PINITP		0x04	/* direct output, active low */
+#define PNL_PAUTOLF		0x02	/* inverted output, active low */
+#define PNL_PSTROBE		0x01	/* inverted output */
+
+#define PNL_PD0			0x01
+#define PNL_PD1			0x02
+#define PNL_PD2			0x04
+#define PNL_PD3			0x08
+#define PNL_PD4			0x10
+#define PNL_PD5			0x20
+#define PNL_PD6			0x40
+#define PNL_PD7			0x80
+
+#define PIN_NONE		0
+#define PIN_STROBE		1
+#define PIN_D0			2
+#define PIN_D1			3
+#define PIN_D2			4
+#define PIN_D3			5
+#define PIN_D4			6
+#define PIN_D5			7
+#define PIN_D6			8
+#define PIN_D7			9
+#define PIN_AUTOLF		14
+#define PIN_INITP		16
+#define PIN_SELECP		17
+#define PIN_NOT_SET		127
+
+#define LCD_FLAG_S		0x0001
+#define LCD_FLAG_ID		0x0002
+#define LCD_FLAG_B		0x0004	/* blink on */
+#define LCD_FLAG_C		0x0008	/* cursor on */
+#define LCD_FLAG_D		0x0010	/* display on */
+#define LCD_FLAG_F		0x0020	/* large font mode */
+#define LCD_FLAG_N		0x0040	/* 2-rows mode */
+#define LCD_FLAG_L		0x0080	/* backlight enabled */
+
+#define LCD_ESCAPE_LEN		24	/* 24 chars max for an LCD escape command */
+#define LCD_ESCAPE_CHAR	27	/* use char 27 for escape command */
+
+/* macros to simplify use of the parallel port */
+#define r_ctr(x)        (parport_read_control((x)->port))
+#define r_dtr(x)        (parport_read_data((x)->port))
+#define r_str(x)        (parport_read_status((x)->port))
+#define w_ctr(x, y)     do { parport_write_control((x)->port, (y)); } while (0)
+#define w_dtr(x, y)     do { parport_write_data((x)->port, (y)); } while (0)
+
+/* this defines which bits are to be used and which ones to be ignored */
+static __u8 scan_mask_o;	/* logical or of the output bits involved in the scan matrix */
+static __u8 scan_mask_i;	/* logical or of the input bits involved in the scan matrix */
+
+typedef __u64 pmask_t;
+
+enum input_type {
+	INPUT_TYPE_STD,
+	INPUT_TYPE_KBD,
+};
+
+enum input_state {
+	INPUT_ST_LOW,
+	INPUT_ST_RISING,
+	INPUT_ST_HIGH,
+	INPUT_ST_FALLING,
+};
+
+struct logical_input {
+	struct list_head list;
+	pmask_t mask;
+	pmask_t value;
+	enum input_type type;
+	enum input_state state;
+	__u8 rise_time, fall_time;
+	__u8 rise_timer, fall_timer, high_timer;
+
+	union {
+		struct {	/* this structure is valid when type == INPUT_TYPE_STD */
+			void (*press_fct) (int);
+			void (*release_fct) (int);
+			int press_data;
+			int release_data;
+		} std;
+		struct {	/* this structure is valid when type == INPUT_TYPE_KBD */
+			/* strings can be full-length (ie. non null-terminated) */
+			char press_str[sizeof(void *) + sizeof(int)];
+			char repeat_str[sizeof(void *) + sizeof(int)];
+			char release_str[sizeof(void *) + sizeof(int)];
+		} kbd;
+	} u;
+};
+
+LIST_HEAD(logical_inputs);	/* list of all defined logical inputs */
+
+/* physical contacts history
+ * Physical contacts are a 45 bits string of 9 groups of 5 bits each.
+ * The 8 lower groups correspond to output bits 0 to 7, and the 9th group
+ * corresponds to the ground.
+ * Within each group, bits are stored in the same order as read on the port :
+ * BAPSE (busy=4, ack=3, paper empty=2, select=1, error=0).
+ * So, each __u64 (or pmask_t) is represented like this :
+ * 0000000000000000000BAPSEBAPSEBAPSEBAPSEBAPSEBAPSEBAPSEBAPSEBAPSE
+ * <-----unused------><gnd><d07><d06><d05><d04><d03><d02><d01><d00>
+ */
+static pmask_t phys_read;	/* what has just been read from the I/O ports */
+static pmask_t phys_read_prev;	/* previous phys_read */
+static pmask_t phys_curr;	/* stabilized phys_read (phys_read|phys_read_prev) */
+static pmask_t phys_prev;	/* previous phys_curr */
+static char inputs_stable;	/* 0 means that at least one logical signal needs be computed */
+
+/* these variables are specific to the keypad */
+static char keypad_buffer[KEYPAD_BUFFER];
+static int keypad_buflen;
+static int keypad_start;
+static char keypressed;
+static wait_queue_head_t keypad_read_wait;
+
+/* lcd-specific variables */
+static unsigned long int lcd_flags;	/* contains the LCD config state */
+static unsigned long int lcd_addr_x;	/* contains the LCD X offset */
+static unsigned long int lcd_addr_y;	/* contains the LCD Y offset */
+static char lcd_escape[LCD_ESCAPE_LEN + 1];	/* current escape sequence, 0 terminated */
+static int lcd_escape_len = -1;	/* not in escape state. >=0 = escape cmd len */
+
+/*
+ * Bit masks to convert LCD signals to parallel port outputs.
+ * _d_ are values for data port, _c_ are for control port.
+ * [0] = signal OFF, [1] = signal ON, [2] = mask
+ */
+#define BIT_CLR		0
+#define BIT_SET		1
+#define BIT_MSK		2
+#define BIT_STATES	3
+/*
+ * one entry for each bit on the LCD
+ */
+#define LCD_BIT_E	0
+#define LCD_BIT_RS	1
+#define LCD_BIT_RW	2
+#define LCD_BIT_BL	3
+#define LCD_BIT_CL	4
+#define LCD_BIT_DA	5
+#define LCD_BITS	6
+
+/*
+ * each bit can be either connected to a DATA or CTRL port
+ */
+#define LCD_PORT_C	0
+#define LCD_PORT_D	1
+#define LCD_PORTS	2
+
+static unsigned char lcd_bits[LCD_PORTS][LCD_BITS][BIT_STATES];
+
+/*
+ * LCD protocols
+ */
+#define LCD_PROTO_PARALLEL      0
+#define LCD_PROTO_SERIAL        1
+
+/*
+ * LCD character sets
+ */
+#define LCD_CHARSET_NORMAL      0
+#define LCD_CHARSET_KS0074      1
+
+/*
+ * LCD types
+ */
+#define LCD_TYPE_NONE		0
+#define LCD_TYPE_OLD		1
+#define LCD_TYPE_KS0074		2
+#define LCD_TYPE_HANTRONIX	3
+#define LCD_TYPE_NEXCOM		4
+#define LCD_TYPE_CUSTOM		5
+
+/*
+ * keypad types
+ */
+#define KEYPAD_TYPE_NONE	0
+#define KEYPAD_TYPE_OLD		1
+#define KEYPAD_TYPE_NEW		2
+#define KEYPAD_TYPE_NEXCOM	3
+
+/*
+ * panel profiles
+ */
+#define PANEL_PROFILE_CUSTOM	0
+#define PANEL_PROFILE_OLD	1
+#define PANEL_PROFILE_NEW	2
+#define PANEL_PROFILE_HANTRONIX	3
+#define PANEL_PROFILE_NEXCOM	4
+#define PANEL_PROFILE_LARGE	5
+
+/*
+ * Construct custom config from the kernel's configuration
+ */
+#define DEFAULT_PROFILE         PANEL_PROFILE_LARGE
+#define DEFAULT_PARPORT         0
+#define DEFAULT_LCD             LCD_TYPE_OLD
+#define DEFAULT_KEYPAD          KEYPAD_TYPE_OLD
+#define DEFAULT_LCD_WIDTH       40
+#define DEFAULT_LCD_BWIDTH      40
+#define DEFAULT_LCD_HWIDTH      64
+#define DEFAULT_LCD_HEIGHT      2
+#define DEFAULT_LCD_PROTO       LCD_PROTO_PARALLEL
+
+#define DEFAULT_LCD_PIN_E       PIN_AUTOLF
+#define DEFAULT_LCD_PIN_RS      PIN_SELECP
+#define DEFAULT_LCD_PIN_RW      PIN_INITP
+#define DEFAULT_LCD_PIN_SCL     PIN_STROBE
+#define DEFAULT_LCD_PIN_SDA     PIN_D0
+#define DEFAULT_LCD_PIN_BL      PIN_NOT_SET
+#define DEFAULT_LCD_CHARSET     LCD_CHARSET_NORMAL
+
+#ifdef CONFIG_PANEL_PROFILE
+#undef DEFAULT_PROFILE
+#define DEFAULT_PROFILE CONFIG_PANEL_PROFILE
+#endif
+
+#ifdef CONFIG_PANEL_PARPORT
+#undef DEFAULT_PARPORT
+#define DEFAULT_PARPORT CONFIG_PANEL_PARPORT
+#endif
+
+#if DEFAULT_PROFILE == 0	/* custom */
+#ifdef CONFIG_PANEL_KEYPAD
+#undef DEFAULT_KEYPAD
+#define DEFAULT_KEYPAD CONFIG_PANEL_KEYPAD
+#endif
+
+#ifdef CONFIG_PANEL_LCD
+#undef DEFAULT_LCD
+#define DEFAULT_LCD CONFIG_PANEL_LCD
+#endif
+
+#ifdef CONFIG_PANEL_LCD_WIDTH
+#undef DEFAULT_LCD_WIDTH
+#define DEFAULT_LCD_WIDTH CONFIG_PANEL_LCD_WIDTH
+#endif
+
+#ifdef CONFIG_PANEL_LCD_BWIDTH
+#undef DEFAULT_LCD_BWIDTH
+#define DEFAULT_LCD_BWIDTH CONFIG_PANEL_LCD_BWIDTH
+#endif
+
+#ifdef CONFIG_PANEL_LCD_HWIDTH
+#undef DEFAULT_LCD_HWIDTH
+#define DEFAULT_LCD_HWIDTH CONFIG_PANEL_LCD_HWIDTH
+#endif
+
+#ifdef CONFIG_PANEL_LCD_HEIGHT
+#undef DEFAULT_LCD_HEIGHT
+#define DEFAULT_LCD_HEIGHT CONFIG_PANEL_LCD_HEIGHT
+#endif
+
+#ifdef CONFIG_PANEL_LCD_PROTO
+#undef DEFAULT_LCD_PROTO
+#define DEFAULT_LCD_PROTO CONFIG_PANEL_LCD_PROTO
+#endif
+
+#ifdef CONFIG_PANEL_LCD_PIN_E
+#undef DEFAULT_LCD_PIN_E
+#define DEFAULT_LCD_PIN_E CONFIG_PANEL_LCD_PIN_E
+#endif
+
+#ifdef CONFIG_PANEL_LCD_PIN_RS
+#undef DEFAULT_LCD_PIN_RS
+#define DEFAULT_LCD_PIN_RS CONFIG_PANEL_LCD_PIN_RS
+#endif
+
+#ifdef CONFIG_PANEL_LCD_PIN_RW
+#undef DEFAULT_LCD_PIN_RW
+#define DEFAULT_LCD_PIN_RW CONFIG_PANEL_LCD_PIN_RW
+#endif
+
+#ifdef CONFIG_PANEL_LCD_PIN_SCL
+#undef DEFAULT_LCD_PIN_SCL
+#define DEFAULT_LCD_PIN_SCL CONFIG_PANEL_LCD_PIN_SCL
+#endif
+
+#ifdef CONFIG_PANEL_LCD_PIN_SDA
+#undef DEFAULT_LCD_PIN_SDA
+#define DEFAULT_LCD_PIN_SDA CONFIG_PANEL_LCD_PIN_SDA
+#endif
+
+#ifdef CONFIG_PANEL_LCD_PIN_BL
+#undef DEFAULT_LCD_PIN_BL
+#define DEFAULT_LCD_PIN_BL CONFIG_PANEL_LCD_PIN_BL
+#endif
+
+#ifdef CONFIG_PANEL_LCD_CHARSET
+#undef DEFAULT_LCD_CHARSET
+#define DEFAULT_LCD_CHARSET
+#endif
+
+#endif /* DEFAULT_PROFILE == 0 */
+
+/* global variables */
+static int keypad_open_cnt;	/* #times opened */
+static int lcd_open_cnt;	/* #times opened */
+static struct pardevice *pprt;
+
+static int lcd_initialized;
+static int keypad_initialized;
+
+static int light_tempo;
+
+static char lcd_must_clear;
+static char lcd_left_shift;
+static char init_in_progress;
+
+static void (*lcd_write_cmd) (int);
+static void (*lcd_write_data) (int);
+static void (*lcd_clear_fast) (void);
+
+static DEFINE_SPINLOCK(pprt_lock);
+static struct timer_list scan_timer;
+
+MODULE_DESCRIPTION("Generic parallel port LCD/Keypad driver");
+
+static int parport = -1;
+module_param(parport, int, 0000);
+MODULE_PARM_DESC(parport, "Parallel port index (0=lpt1, 1=lpt2, ...)");
+
+static int lcd_height = -1;
+module_param(lcd_height, int, 0000);
+MODULE_PARM_DESC(lcd_height, "Number of lines on the LCD");
+
+static int lcd_width = -1;
+module_param(lcd_width, int, 0000);
+MODULE_PARM_DESC(lcd_width, "Number of columns on the LCD");
+
+static int lcd_bwidth = -1;	/* internal buffer width (usually 40) */
+module_param(lcd_bwidth, int, 0000);
+MODULE_PARM_DESC(lcd_bwidth, "Internal LCD line width (40)");
+
+static int lcd_hwidth = -1;	/* hardware buffer width (usually 64) */
+module_param(lcd_hwidth, int, 0000);
+MODULE_PARM_DESC(lcd_hwidth, "LCD line hardware address (64)");
+
+static int lcd_enabled = -1;
+module_param(lcd_enabled, int, 0000);
+MODULE_PARM_DESC(lcd_enabled, "Deprecated option, use lcd_type instead");
+
+static int keypad_enabled = -1;
+module_param(keypad_enabled, int, 0000);
+MODULE_PARM_DESC(keypad_enabled, "Deprecated option, use keypad_type instead");
+
+static int lcd_type = -1;
+module_param(lcd_type, int, 0000);
+MODULE_PARM_DESC(lcd_type,
+		 "LCD type: 0=none, 1=old //, 2=serial ks0074, 3=hantronix //, 4=nexcom //, 5=compiled-in");
+
+static int lcd_proto = -1;
+module_param(lcd_proto, int, 0000);
+MODULE_PARM_DESC(lcd_proto, "LCD communication: 0=parallel (//), 1=serial");
+
+static int lcd_charset = -1;
+module_param(lcd_charset, int, 0000);
+MODULE_PARM_DESC(lcd_charset, "LCD character set: 0=standard, 1=KS0074");
+
+static int keypad_type = -1;
+module_param(keypad_type, int, 0000);
+MODULE_PARM_DESC(keypad_type,
+		 "Keypad type: 0=none, 1=old 6 keys, 2=new 6+1 keys, 3=nexcom 4 keys");
+
+static int profile = DEFAULT_PROFILE;
+module_param(profile, int, 0000);
+MODULE_PARM_DESC(profile,
+		 "1=16x2 old kp; 2=serial 16x2, new kp; 3=16x2 hantronix; 4=16x2 nexcom; default=40x2, old kp");
+
+/*
+ * These are the parallel port pins the LCD control signals are connected to.
+ * Set this to 0 if the signal is not used. Set it to its opposite value
+ * (negative) if the signal is negated. -MAXINT is used to indicate that the
+ * pin has not been explicitly specified.
+ *
+ * WARNING! no check will be performed about collisions with keypad !
+ */
+
+static int lcd_e_pin  = PIN_NOT_SET;
+module_param(lcd_e_pin, int, 0000);
+MODULE_PARM_DESC(lcd_e_pin,
+		 "# of the // port pin connected to LCD 'E' signal, with polarity (-17..17)");
+
+static int lcd_rs_pin = PIN_NOT_SET;
+module_param(lcd_rs_pin, int, 0000);
+MODULE_PARM_DESC(lcd_rs_pin,
+		 "# of the // port pin connected to LCD 'RS' signal, with polarity (-17..17)");
+
+static int lcd_rw_pin = PIN_NOT_SET;
+module_param(lcd_rw_pin, int, 0000);
+MODULE_PARM_DESC(lcd_rw_pin,
+		 "# of the // port pin connected to LCD 'RW' signal, with polarity (-17..17)");
+
+static int lcd_bl_pin = PIN_NOT_SET;
+module_param(lcd_bl_pin, int, 0000);
+MODULE_PARM_DESC(lcd_bl_pin,
+		 "# of the // port pin connected to LCD backlight, with polarity (-17..17)");
+
+static int lcd_da_pin = PIN_NOT_SET;
+module_param(lcd_da_pin, int, 0000);
+MODULE_PARM_DESC(lcd_da_pin,
+		 "# of the // port pin connected to serial LCD 'SDA' signal, with polarity (-17..17)");
+
+static int lcd_cl_pin = PIN_NOT_SET;
+module_param(lcd_cl_pin, int, 0000);
+MODULE_PARM_DESC(lcd_cl_pin,
+		 "# of the // port pin connected to serial LCD 'SCL' signal, with polarity (-17..17)");
+
+static unsigned char *lcd_char_conv;
+
+/* for some LCD drivers (ks0074) we need a charset conversion table. */
+static unsigned char lcd_char_conv_ks0074[256] = {
+	/*          0|8   1|9   2|A   3|B   4|C   5|D   6|E   7|F */
+	/* 0x00 */ 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
+	/* 0x08 */ 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d, 0x0e, 0x0f,
+	/* 0x10 */ 0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17,
+	/* 0x18 */ 0x18, 0x19, 0x1a, 0x1b, 0x1c, 0x1d, 0x1e, 0x1f,
+	/* 0x20 */ 0x20, 0x21, 0x22, 0x23, 0xa2, 0x25, 0x26, 0x27,
+	/* 0x28 */ 0x28, 0x29, 0x2a, 0x2b, 0x2c, 0x2d, 0x2e, 0x2f,
+	/* 0x30 */ 0x30, 0x31, 0x32, 0x33, 0x34, 0x35, 0x36, 0x37,
+	/* 0x38 */ 0x38, 0x39, 0x3a, 0x3b, 0x3c, 0x3d, 0x3e, 0x3f,
+	/* 0x40 */ 0xa0, 0x41, 0x42, 0x43, 0x44, 0x45, 0x46, 0x47,
+	/* 0x48 */ 0x48, 0x49, 0x4a, 0x4b, 0x4c, 0x4d, 0x4e, 0x4f,
+	/* 0x50 */ 0x50, 0x51, 0x52, 0x53, 0x54, 0x55, 0x56, 0x57,
+	/* 0x58 */ 0x58, 0x59, 0x5a, 0xfa, 0xfb, 0xfc, 0x1d, 0xc4,
+	/* 0x60 */ 0x96, 0x61, 0x62, 0x63, 0x64, 0x65, 0x66, 0x67,
+	/* 0x68 */ 0x68, 0x69, 0x6a, 0x6b, 0x6c, 0x6d, 0x6e, 0x6f,
+	/* 0x70 */ 0x70, 0x71, 0x72, 0x73, 0x74, 0x75, 0x76, 0x77,
+	/* 0x78 */ 0x78, 0x79, 0x7a, 0xfd, 0xfe, 0xff, 0xce, 0x20,
+	/* 0x80 */ 0x80, 0x81, 0x82, 0x83, 0x84, 0x85, 0x86, 0x87,
+	/* 0x88 */ 0x88, 0x89, 0x8a, 0x8b, 0x8c, 0x8d, 0x8e, 0x8f,
+	/* 0x90 */ 0x90, 0x91, 0x92, 0x93, 0x94, 0x95, 0x96, 0x97,
+	/* 0x98 */ 0x98, 0x99, 0x9a, 0x9b, 0x9c, 0x9d, 0x9e, 0x9f,
+	/* 0xA0 */ 0x20, 0x40, 0xb1, 0xa1, 0x24, 0xa3, 0xfe, 0x5f,
+	/* 0xA8 */ 0x22, 0xc8, 0x61, 0x14, 0x97, 0x2d, 0xad, 0x96,
+	/* 0xB0 */ 0x80, 0x8c, 0x82, 0x83, 0x27, 0x8f, 0x86, 0xdd,
+	/* 0xB8 */ 0x2c, 0x81, 0x6f, 0x15, 0x8b, 0x8a, 0x84, 0x60,
+	/* 0xC0 */ 0xe2, 0xe2, 0xe2, 0x5b, 0x5b, 0xae, 0xbc, 0xa9,
+	/* 0xC8 */ 0xc5, 0xbf, 0xc6, 0xf1, 0xe3, 0xe3, 0xe3, 0xe3,
+	/* 0xD0 */ 0x44, 0x5d, 0xa8, 0xe4, 0xec, 0xec, 0x5c, 0x78,
+	/* 0xD8 */ 0xab, 0xa6, 0xe5, 0x5e, 0x5e, 0xe6, 0xaa, 0xbe,
+	/* 0xE0 */ 0x7f, 0xe7, 0xaf, 0x7b, 0x7b, 0xaf, 0xbd, 0xc8,
+	/* 0xE8 */ 0xa4, 0xa5, 0xc7, 0xf6, 0xa7, 0xe8, 0x69, 0x69,
+	/* 0xF0 */ 0xed, 0x7d, 0xa8, 0xe4, 0xec, 0x5c, 0x5c, 0x25,
+	/* 0xF8 */ 0xac, 0xa6, 0xea, 0xef, 0x7e, 0xeb, 0xb2, 0x79,
+};
+
+char old_keypad_profile[][4][9] = {
+	{"S0", "Left\n", "Left\n", ""},
+	{"S1", "Down\n", "Down\n", ""},
+	{"S2", "Up\n", "Up\n", ""},
+	{"S3", "Right\n", "Right\n", ""},
+	{"S4", "Esc\n", "Esc\n", ""},
+	{"S5", "Ret\n", "Ret\n", ""},
+	{"", "", "", ""}
+};
+
+/* signals, press, repeat, release */
+char new_keypad_profile[][4][9] = {
+	{"S0", "Left\n", "Left\n", ""},
+	{"S1", "Down\n", "Down\n", ""},
+	{"S2", "Up\n", "Up\n", ""},
+	{"S3", "Right\n", "Right\n", ""},
+	{"S4s5", "", "Esc\n", "Esc\n"},
+	{"s4S5", "", "Ret\n", "Ret\n"},
+	{"S4S5", "Help\n", "", ""},
+	/* add new signals above this line */
+	{"", "", "", ""}
+};
+
+/* signals, press, repeat, release */
+char nexcom_keypad_profile[][4][9] = {
+	{"a-p-e-", "Down\n", "Down\n", ""},
+	{"a-p-E-", "Ret\n", "Ret\n", ""},
+	{"a-P-E-", "Esc\n", "Esc\n", ""},
+	{"a-P-e-", "Up\n", "Up\n", ""},
+	/* add new signals above this line */
+	{"", "", "", ""}
+};
+
+static char (*keypad_profile)[4][9] = old_keypad_profile;
+
+/* FIXME: this should be converted to a bit array containing signals states */
+static struct {
+	unsigned char e;	/* parallel LCD E   (data latch on falling edge) */
+	unsigned char rs;	/* parallel LCD RS  (0 = cmd, 1 = data) */
+	unsigned char rw;	/* parallel LCD R/W (0 = W, 1 = R) */
+	unsigned char bl;	/* parallel LCD backlight (0 = off, 1 = on) */
+	unsigned char cl;	/* serial LCD clock (latch on rising edge) */
+	unsigned char da;	/* serial LCD data */
+} bits;
+
+static void init_scan_timer(void);
+
+/* sets data port bits according to current signals values */
+static int set_data_bits(void)
+{
+	int val, bit;
+
+	val = r_dtr(pprt);
+	for (bit = 0; bit < LCD_BITS; bit++)
+		val &= lcd_bits[LCD_PORT_D][bit][BIT_MSK];
+
+	val |= lcd_bits[LCD_PORT_D][LCD_BIT_E][bits.e]
+	    | lcd_bits[LCD_PORT_D][LCD_BIT_RS][bits.rs]
+	    | lcd_bits[LCD_PORT_D][LCD_BIT_RW][bits.rw]
+	    | lcd_bits[LCD_PORT_D][LCD_BIT_BL][bits.bl]
+	    | lcd_bits[LCD_PORT_D][LCD_BIT_CL][bits.cl]
+	    | lcd_bits[LCD_PORT_D][LCD_BIT_DA][bits.da];
+
+	w_dtr(pprt, val);
+	return val;
+}
+
+/* sets ctrl port bits according to current signals values */
+static int set_ctrl_bits(void)
+{
+	int val, bit;
+
+	val = r_ctr(pprt);
+	for (bit = 0; bit < LCD_BITS; bit++)
+		val &= lcd_bits[LCD_PORT_C][bit][BIT_MSK];
+
+	val |= lcd_bits[LCD_PORT_C][LCD_BIT_E][bits.e]
+	    | lcd_bits[LCD_PORT_C][LCD_BIT_RS][bits.rs]
+	    | lcd_bits[LCD_PORT_C][LCD_BIT_RW][bits.rw]
+	    | lcd_bits[LCD_PORT_C][LCD_BIT_BL][bits.bl]
+	    | lcd_bits[LCD_PORT_C][LCD_BIT_CL][bits.cl]
+	    | lcd_bits[LCD_PORT_C][LCD_BIT_DA][bits.da];
+
+	w_ctr(pprt, val);
+	return val;
+}
+
+/* sets ctrl & data port bits according to current signals values */
+static void set_bits(void)
+{
+	set_data_bits();
+	set_ctrl_bits();
+}
+
+/*
+ * Converts a parallel port pin (from -25 to 25) to data and control ports
+ * masks, and data and control port bits. The signal will be considered
+ * unconnected if it's on pin 0 or an invalid pin (<-25 or >25).
+ *
+ * Result will be used this way :
+ *   out(dport, in(dport) & d_val[2] | d_val[signal_state])
+ *   out(cport, in(cport) & c_val[2] | c_val[signal_state])
+ */
+void pin_to_bits(int pin, unsigned char *d_val, unsigned char *c_val)
+{
+	int d_bit, c_bit, inv;
+
+	d_val[0] = c_val[0] = d_val[1] = c_val[1] = 0;
+	d_val[2] = c_val[2] = 0xFF;
+
+	if (pin == 0)
+		return;
+
+	inv = (pin < 0);
+	if (inv)
+		pin = -pin;
+
+	d_bit = c_bit = 0;
+
+	switch (pin) {
+	case PIN_STROBE:	/* strobe, inverted */
+		c_bit = PNL_PSTROBE;
+		inv = !inv;
+		break;
+	case PIN_D0...PIN_D7:	/* D0 - D7 = 2 - 9 */
+		d_bit = 1 << (pin - 2);
+		break;
+	case PIN_AUTOLF:	/* autofeed, inverted */
+		c_bit = PNL_PAUTOLF;
+		inv = !inv;
+		break;
+	case PIN_INITP:	/* init, direct */
+		c_bit = PNL_PINITP;
+		break;
+	case PIN_SELECP:	/* select_in, inverted */
+		c_bit = PNL_PSELECP;
+		inv = !inv;
+		break;
+	default:		/* unknown pin, ignore */
+		break;
+	}
+
+	if (c_bit) {
+		c_val[2] &= ~c_bit;
+		c_val[!inv] = c_bit;
+	} else if (d_bit) {
+		d_val[2] &= ~d_bit;
+		d_val[!inv] = d_bit;
+	}
+}
+
+/* sleeps that many milliseconds with a reschedule */
+static void long_sleep(int ms)
+{
+
+	if (in_interrupt())
+		mdelay(ms);
+	else {
+		current->state = TASK_INTERRUPTIBLE;
+		schedule_timeout((ms * HZ + 999) / 1000);
+	}
+}
+
+/* send a serial byte to the LCD panel. The caller is responsible for locking if needed. */
+static void lcd_send_serial(int byte)
+{
+	int bit;
+
+	/* the data bit is set on D0, and the clock on STROBE.
+	 * LCD reads D0 on STROBE's rising edge.
+	 */
+	for (bit = 0; bit < 8; bit++) {
+		bits.cl = BIT_CLR;	/* CLK low */
+		set_bits();
+		bits.da = byte & 1;
+		set_bits();
+		udelay(2);	/* maintain the data during 2 us before CLK up */
+		bits.cl = BIT_SET;	/* CLK high */
+		set_bits();
+		udelay(1);	/* maintain the strobe during 1 us */
+		byte >>= 1;
+	}
+}
+
+/* turn the backlight on or off */
+static void lcd_backlight(int on)
+{
+	if (lcd_bl_pin == PIN_NONE)
+		return;
+
+	/* The backlight is activated by seting the AUTOFEED line to +5V  */
+	spin_lock(&pprt_lock);
+	bits.bl = on;
+	set_bits();
+	spin_unlock(&pprt_lock);
+}
+
+/* send a command to the LCD panel in serial mode */
+static void lcd_write_cmd_s(int cmd)
+{
+	spin_lock(&pprt_lock);
+	lcd_send_serial(0x1F);	/* R/W=W, RS=0 */
+	lcd_send_serial(cmd & 0x0F);
+	lcd_send_serial((cmd >> 4) & 0x0F);
+	udelay(40);		/* the shortest command takes at least 40 us */
+	spin_unlock(&pprt_lock);
+}
+
+/* send data to the LCD panel in serial mode */
+static void lcd_write_data_s(int data)
+{
+	spin_lock(&pprt_lock);
+	lcd_send_serial(0x5F);	/* R/W=W, RS=1 */
+	lcd_send_serial(data & 0x0F);
+	lcd_send_serial((data >> 4) & 0x0F);
+	udelay(40);		/* the shortest data takes at least 40 us */
+	spin_unlock(&pprt_lock);
+}
+
+/* send a command to the LCD panel in 8 bits parallel mode */
+static void lcd_write_cmd_p8(int cmd)
+{
+	spin_lock(&pprt_lock);
+	/* present the data to the data port */
+	w_dtr(pprt, cmd);
+	udelay(20);		/* maintain the data during 20 us before the strobe */
+
+	bits.e = BIT_SET;
+	bits.rs = BIT_CLR;
+	bits.rw = BIT_CLR;
+	set_ctrl_bits();
+
+	udelay(40);		/* maintain the strobe during 40 us */
+
+	bits.e = BIT_CLR;
+	set_ctrl_bits();
+
+	udelay(120);		/* the shortest command takes at least 120 us */
+	spin_unlock(&pprt_lock);
+}
+
+/* send data to the LCD panel in 8 bits parallel mode */
+static void lcd_write_data_p8(int data)
+{
+	spin_lock(&pprt_lock);
+	/* present the data to the data port */
+	w_dtr(pprt, data);
+	udelay(20);		/* maintain the data during 20 us before the strobe */
+
+	bits.e = BIT_SET;
+	bits.rs = BIT_SET;
+	bits.rw = BIT_CLR;
+	set_ctrl_bits();
+
+	udelay(40);		/* maintain the strobe during 40 us */
+
+	bits.e = BIT_CLR;
+	set_ctrl_bits();
+
+	udelay(45);		/* the shortest data takes at least 45 us */
+	spin_unlock(&pprt_lock);
+}
+
+static void lcd_gotoxy(void)
+{
+	lcd_write_cmd(0x80	/* set DDRAM address */
+		      | (lcd_addr_y ? lcd_hwidth : 0)
+		      /* we force the cursor to stay at the end of the line if it wants to go farther */
+		      | ((lcd_addr_x < lcd_bwidth) ? lcd_addr_x &
+			 (lcd_hwidth - 1) : lcd_bwidth - 1));
+}
+
+static void lcd_print(char c)
+{
+	if (lcd_addr_x < lcd_bwidth) {
+		if (lcd_char_conv != NULL)
+			c = lcd_char_conv[(unsigned char)c];
+		lcd_write_data(c);
+		lcd_addr_x++;
+	}
+	/* prevents the cursor from wrapping onto the next line */
+	if (lcd_addr_x == lcd_bwidth)
+		lcd_gotoxy();
+}
+
+/* fills the display with spaces and resets X/Y */
+static void lcd_clear_fast_s(void)
+{
+	int pos;
+	lcd_addr_x = lcd_addr_y = 0;
+	lcd_gotoxy();
+
+	spin_lock(&pprt_lock);
+	for (pos = 0; pos < lcd_height * lcd_hwidth; pos++) {
+		lcd_send_serial(0x5F);	/* R/W=W, RS=1 */
+		lcd_send_serial(' ' & 0x0F);
+		lcd_send_serial((' ' >> 4) & 0x0F);
+		udelay(40);	/* the shortest data takes at least 40 us */
+	}
+	spin_unlock(&pprt_lock);
+
+	lcd_addr_x = lcd_addr_y = 0;
+	lcd_gotoxy();
+}
+
+/* fills the display with spaces and resets X/Y */
+static void lcd_clear_fast_p8(void)
+{
+	int pos;
+	lcd_addr_x = lcd_addr_y = 0;
+	lcd_gotoxy();
+
+	spin_lock(&pprt_lock);
+	for (pos = 0; pos < lcd_height * lcd_hwidth; pos++) {
+		/* present the data to the data port */
+		w_dtr(pprt, ' ');
+		udelay(20);	/* maintain the data during 20 us before the strobe */
+
+		bits.e = BIT_SET;
+		bits.rs = BIT_SET;
+		bits.rw = BIT_CLR;
+		set_ctrl_bits();
+
+		udelay(40);	/* maintain the strobe during 40 us */
+
+		bits.e = BIT_CLR;
+		set_ctrl_bits();
+
+		udelay(45);	/* the shortest data takes at least 45 us */
+	}
+	spin_unlock(&pprt_lock);
+
+	lcd_addr_x = lcd_addr_y = 0;
+	lcd_gotoxy();
+}
+
+/* clears the display and resets X/Y */
+static void lcd_clear_display(void)
+{
+	lcd_write_cmd(0x01);	/* clear display */
+	lcd_addr_x = lcd_addr_y = 0;
+	/* we must wait a few milliseconds (15) */
+	long_sleep(15);
+}
+
+static void lcd_init_display(void)
+{
+
+	lcd_flags = ((lcd_height > 1) ? LCD_FLAG_N : 0)
+	    | LCD_FLAG_D | LCD_FLAG_C | LCD_FLAG_B;
+
+	long_sleep(20);		/* wait 20 ms after power-up for the paranoid */
+
+	lcd_write_cmd(0x30);	/* 8bits, 1 line, small fonts */
+	long_sleep(10);
+	lcd_write_cmd(0x30);	/* 8bits, 1 line, small fonts */
+	long_sleep(10);
+	lcd_write_cmd(0x30);	/* 8bits, 1 line, small fonts */
+	long_sleep(10);
+
+	lcd_write_cmd(0x30	/* set font height and lines number */
+		      | ((lcd_flags & LCD_FLAG_F) ? 4 : 0)
+		      | ((lcd_flags & LCD_FLAG_N) ? 8 : 0)
+	    );
+	long_sleep(10);
+
+	lcd_write_cmd(0x08);	/* display off, cursor off, blink off */
+	long_sleep(10);
+
+	lcd_write_cmd(0x08	/* set display mode */
+		      | ((lcd_flags & LCD_FLAG_D) ? 4 : 0)
+		      | ((lcd_flags & LCD_FLAG_C) ? 2 : 0)
+		      | ((lcd_flags & LCD_FLAG_B) ? 1 : 0)
+	    );
+
+	lcd_backlight((lcd_flags & LCD_FLAG_L) ? 1 : 0);
+
+	long_sleep(10);
+
+	lcd_write_cmd(0x06);	/* entry mode set : increment, cursor shifting */
+
+	lcd_clear_display();
+}
+
+/*
+ * These are the file operation function for user access to /dev/lcd
+ * This function can also be called from inside the kernel, by
+ * setting file and ppos to NULL.
+ *
+ */
+
+static ssize_t lcd_write(struct file *file,
+			 const char *buf, size_t count, loff_t *ppos)
+{
+
+	const char *tmp = buf;
+	char c;
+
+	for (; count-- > 0; (ppos ? (*ppos)++ : 0), ++tmp) {
+		if (!in_interrupt() && (((count + 1) & 0x1f) == 0))
+			schedule();	/* let's be a little nice with other processes that need some CPU */
+
+		if (ppos == NULL && file == NULL)
+			c = *tmp;	/* let's not use get_user() from the kernel ! */
+		else if (get_user(c, tmp))
+			return -EFAULT;
+
+		/* first, we'll test if we're in escape mode */
+		if ((c != '\n') && lcd_escape_len >= 0) {	/* yes, let's add this char to the buffer */
+			lcd_escape[lcd_escape_len++] = c;
+			lcd_escape[lcd_escape_len] = 0;
+		} else {
+			lcd_escape_len = -1;	/* aborts any previous escape sequence */
+
+			switch (c) {
+			case LCD_ESCAPE_CHAR:	/* start of an escape sequence */
+				lcd_escape_len = 0;
+				lcd_escape[lcd_escape_len] = 0;
+				break;
+			case '\b':	/* go back one char and clear it */
+				if (lcd_addr_x > 0) {
+					if (lcd_addr_x < lcd_bwidth)	/* check if we're not at the end of the line */
+						lcd_write_cmd(0x10);	/* back one char */
+					lcd_addr_x--;
+				}
+				lcd_write_data(' ');	/* replace with a space */
+				lcd_write_cmd(0x10);	/* back one char again */
+				break;
+			case '\014':	/* quickly clear the display */
+				lcd_clear_fast();
+				break;
+			case '\n':	/* flush the remainder of the current line and go to the
+					   beginning of the next line */
+				for (; lcd_addr_x < lcd_bwidth; lcd_addr_x++)
+					lcd_write_data(' ');
+				lcd_addr_x = 0;
+				lcd_addr_y = (lcd_addr_y + 1) % lcd_height;
+				lcd_gotoxy();
+				break;
+			case '\r':	/* go to the beginning of the same line */
+				lcd_addr_x = 0;
+				lcd_gotoxy();
+				break;
+			case '\t':	/* print a space instead of the tab */
+				lcd_print(' ');
+				break;
+			default:	/* simply print this char */
+				lcd_print(c);
+				break;
+			}
+		}
+
+		/* now we'll see if we're in an escape mode and if the current
+		   escape sequence can be understood.
+		 */
+		if (lcd_escape_len >= 2) {	/* minimal length for an escape command */
+			int processed = 0;	/* 1 means the command has been processed */
+
+			if (!strcmp(lcd_escape, "[2J")) {	/* Clear the display */
+				lcd_clear_fast();	/* clear display */
+				processed = 1;
+			} else if (!strcmp(lcd_escape, "[H")) {	/* Cursor to home */
+				lcd_addr_x = lcd_addr_y = 0;
+				lcd_gotoxy();
+				processed = 1;
+			}
+			/* codes starting with ^[[L */
+			else if ((lcd_escape_len >= 3) &&
+				 (lcd_escape[0] == '[') && (lcd_escape[1] == 'L')) {	/* LCD special codes */
+
+				char *esc = lcd_escape + 2;
+				int oldflags = lcd_flags;
+
+				/* check for display mode flags */
+				switch (*esc) {
+				case 'D':	/* Display ON */
+					lcd_flags |= LCD_FLAG_D;
+					processed = 1;
+					break;
+				case 'd':	/* Display OFF */
+					lcd_flags &= ~LCD_FLAG_D;
+					processed = 1;
+					break;
+				case 'C':	/* Cursor ON */
+					lcd_flags |= LCD_FLAG_C;
+					processed = 1;
+					break;
+				case 'c':	/* Cursor OFF */
+					lcd_flags &= ~LCD_FLAG_C;
+					processed = 1;
+					break;
+				case 'B':	/* Blink ON */
+					lcd_flags |= LCD_FLAG_B;
+					processed = 1;
+					break;
+				case 'b':	/* Blink OFF */
+					lcd_flags &= ~LCD_FLAG_B;
+					processed = 1;
+					break;
+				case '+':	/* Back light ON */
+					lcd_flags |= LCD_FLAG_L;
+					processed = 1;
+					break;
+				case '-':	/* Back light OFF */
+					lcd_flags &= ~LCD_FLAG_L;
+					processed = 1;
+					break;
+				case '*':	/* flash back light using the keypad timer */
+					if (scan_timer.function != NULL) {
+						if (light_tempo == 0
+						    && ((lcd_flags & LCD_FLAG_L)
+							== 0))
+							lcd_backlight(1);
+						light_tempo = FLASH_LIGHT_TEMPO;
+					}
+					processed = 1;
+					break;
+				case 'f':	/* Small Font */
+					lcd_flags &= ~LCD_FLAG_F;
+					processed = 1;
+					break;
+				case 'F':	/* Large Font */
+					lcd_flags |= LCD_FLAG_F;
+					processed = 1;
+					break;
+				case 'n':	/* One Line */
+					lcd_flags &= ~LCD_FLAG_N;
+					processed = 1;
+					break;
+				case 'N':	/* Two Lines */
+					lcd_flags |= LCD_FLAG_N;
+					break;
+
+				case 'l':	/* Shift Cursor Left */
+					if (lcd_addr_x > 0) {
+						if (lcd_addr_x < lcd_bwidth)
+							lcd_write_cmd(0x10);	/* back one char if not at end of line */
+						lcd_addr_x--;
+					}
+					processed = 1;
+					break;
+
+				case 'r':	/* shift cursor right */
+					if (lcd_addr_x < lcd_width) {
+						if (lcd_addr_x < (lcd_bwidth - 1))
+							lcd_write_cmd(0x14);	/* allow the cursor to pass the end of the line */
+						lcd_addr_x++;
+					}
+					processed = 1;
+					break;
+
+				case 'L':	/* shift display left */
+					lcd_left_shift++;
+					lcd_write_cmd(0x18);
+					processed = 1;
+					break;
+
+				case 'R':	/* shift display right */
+					lcd_left_shift--;
+					lcd_write_cmd(0x1C);
+					processed = 1;
+					break;
+
+				case 'k':{	/* kill end of line */
+						int x;
+						for (x = lcd_addr_x; x < lcd_bwidth; x++)
+							lcd_write_data(' ');
+						lcd_gotoxy();	/* restore cursor position */
+						processed = 1;
+						break;
+					}
+				case 'I':	/* reinitialize display */
+					lcd_init_display();
+					lcd_left_shift = 0;
+					processed = 1;
+					break;
+
+				case 'G':	/* Generator : LGcxxxxx...xx; */  {
+						/* must have <c> between '0' and '7', representing the numerical
+						 * ASCII code of the redefined character, and <xx...xx> a sequence
+						 * of 16 hex digits representing 8 bytes for each character. Most
+						 * LCDs will only use 5 lower bits of the 7 first bytes.
+						 */
+
+						unsigned char cgbytes[8];
+						unsigned char cgaddr;
+						int cgoffset;
+						int shift;
+						char value;
+						int addr;
+
+						if (strchr(esc, ';') == NULL)
+							break;
+
+						esc++;
+
+						cgaddr = *(esc++) - '0';
+						if (cgaddr > 7) {
+							processed = 1;
+							break;
+						}
+
+						cgoffset = 0;
+						shift = 0;
+						value = 0;
+						while (*esc && cgoffset < 8) {
+							shift ^= 4;
+							if (*esc >= '0' && *esc <= '9')
+								value |= (*esc - '0') << shift;
+							else if (*esc >= 'A' && *esc <= 'Z')
+								value |= (*esc - 'A' + 10) << shift;
+							else if (*esc >= 'a' && *esc <= 'z')
+								value |= (*esc - 'a' + 10) << shift;
+							else {
+								esc++;
+								continue;
+							}
+
+							if (shift == 0) {
+								cgbytes[cgoffset++] = value;
+								value = 0;
+							}
+
+							esc++;
+						}
+
+						lcd_write_cmd(0x40 | (cgaddr * 8));
+						for (addr = 0; addr < cgoffset; addr++)
+							lcd_write_data(cgbytes[addr]);
+
+						lcd_gotoxy();	/* ensures that we stop writing to CGRAM */
+						processed = 1;
+						break;
+					}
+				case 'x':	/* gotoxy : LxXXX[yYYY]; */
+				case 'y':	/* gotoxy : LyYYY[xXXX]; */
+					if (strchr(esc, ';') == NULL)
+						break;
+
+					while (*esc) {
+						if (*esc == 'x') {
+							esc++;
+							lcd_addr_x = 0;
+							while (isdigit(*esc)) {
+								lcd_addr_x =
+								    lcd_addr_x *
+								    10 + (*esc -
+									  '0');
+								esc++;
+							}
+						} else if (*esc == 'y') {
+							esc++;
+							lcd_addr_y = 0;
+							while (isdigit(*esc)) {
+								lcd_addr_y =
+								    lcd_addr_y *
+								    10 + (*esc -
+									  '0');
+								esc++;
+							}
+						} else
+							break;
+					}
+
+					lcd_gotoxy();
+					processed = 1;
+					break;
+				}	/* end of switch */
+
+				/* Check wether one flag was changed */
+				if (oldflags != lcd_flags) {
+					/* check wether one of B,C,D flags was changed */
+					if ((oldflags ^ lcd_flags) &
+					    (LCD_FLAG_B | LCD_FLAG_C | LCD_FLAG_D))
+						/* set display mode */
+						lcd_write_cmd(0x08 |
+							      ((lcd_flags & LCD_FLAG_D) ? 4 : 0) |
+							      ((lcd_flags & LCD_FLAG_C) ? 2 : 0) |
+							      ((lcd_flags & LCD_FLAG_B) ? 1 : 0));
+					/* check wether one of F,N flags was changed */
+					else if ((oldflags ^ lcd_flags) &
+						 (LCD_FLAG_F | LCD_FLAG_N))
+						lcd_write_cmd(0x30 |
+							      ((lcd_flags & LCD_FLAG_F) ? 4 : 0) |
+							      ((lcd_flags & LCD_FLAG_N) ? 8 : 0));
+					/* check wether L flag was changed */
+					else if ((oldflags ^ lcd_flags) &
+						 (LCD_FLAG_L)) {
+						if (lcd_flags & (LCD_FLAG_L))
+							lcd_backlight(1);
+						else if (light_tempo == 0)	/* switch off the light only when the tempo lighting is gone */
+							lcd_backlight(0);
+					}
+				}
+			}
+
+			/* LCD special escape codes */
+			/* flush the escape sequence if it's been processed or if it is
+			   getting too long. */
+			if (processed || (lcd_escape_len >= LCD_ESCAPE_LEN))
+				lcd_escape_len = -1;
+		}		/* escape codes */
+	}
+
+	return tmp - buf;
+}
+
+static int lcd_open(struct inode *inode, struct file *file)
+{
+	if (lcd_open_cnt)
+		return -EBUSY;	/* open only once at a time */
+
+	if (file->f_mode & FMODE_READ)	/* device is write-only */
+		return -EPERM;
+
+	if (lcd_must_clear) {
+		lcd_clear_display();
+		lcd_must_clear = 0;
+	}
+	lcd_open_cnt++;
+	return 0;
+}
+
+static int lcd_release(struct inode *inode, struct file *file)
+{
+	lcd_open_cnt--;
+	return 0;
+}
+
+static struct file_operations lcd_fops = {
+	.write   = lcd_write,
+	.open    = lcd_open,
+	.release = lcd_release,
+};
+
+static struct miscdevice lcd_dev = {
+	LCD_MINOR,
+	"lcd",
+	&lcd_fops
+};
+
+/* public function usable from the kernel for any purpose */
+void panel_lcd_print(char *s)
+{
+	if (lcd_enabled && lcd_initialized)
+		lcd_write(NULL, s, strlen(s), NULL);
+}
+
+/* initialize the LCD driver */
+void lcd_init(void)
+{
+	switch (lcd_type) {
+	case LCD_TYPE_OLD:	/* parallel mode, 8 bits */
+		if (lcd_proto < 0)
+			lcd_proto = LCD_PROTO_PARALLEL;
+		if (lcd_charset < 0)
+			lcd_charset = LCD_CHARSET_NORMAL;
+		if (lcd_e_pin == PIN_NOT_SET)
+			lcd_e_pin = PIN_STROBE;
+		if (lcd_rs_pin == PIN_NOT_SET)
+			lcd_rs_pin = PIN_AUTOLF;
+
+		if (lcd_width < 0)
+			lcd_width = 40;
+		if (lcd_bwidth < 0)
+			lcd_bwidth = 40;
+		if (lcd_hwidth < 0)
+			lcd_hwidth = 64;
+		if (lcd_height < 0)
+			lcd_height = 2;
+		break;
+	case LCD_TYPE_KS0074:	/* serial mode, ks0074 */
+		if (lcd_proto < 0)
+			lcd_proto = LCD_PROTO_SERIAL;
+		if (lcd_charset < 0)
+			lcd_charset = LCD_CHARSET_KS0074;
+		if (lcd_bl_pin == PIN_NOT_SET)
+			lcd_bl_pin = PIN_AUTOLF;
+		if (lcd_cl_pin == PIN_NOT_SET)
+			lcd_cl_pin = PIN_STROBE;
+		if (lcd_da_pin == PIN_NOT_SET)
+			lcd_da_pin = PIN_D0;
+
+		if (lcd_width < 0)
+			lcd_width = 16;
+		if (lcd_bwidth < 0)
+			lcd_bwidth = 40;
+		if (lcd_hwidth < 0)
+			lcd_hwidth = 16;
+		if (lcd_height < 0)
+			lcd_height = 2;
+		break;
+	case LCD_TYPE_NEXCOM:	/* parallel mode, 8 bits, generic */
+		if (lcd_proto < 0)
+			lcd_proto = LCD_PROTO_PARALLEL;
+		if (lcd_charset < 0)
+			lcd_charset = LCD_CHARSET_NORMAL;
+		if (lcd_e_pin == PIN_NOT_SET)
+			lcd_e_pin = PIN_AUTOLF;
+		if (lcd_rs_pin == PIN_NOT_SET)
+			lcd_rs_pin = PIN_SELECP;
+		if (lcd_rw_pin == PIN_NOT_SET)
+			lcd_rw_pin = PIN_INITP;
+
+		if (lcd_width < 0)
+			lcd_width = 16;
+		if (lcd_bwidth < 0)
+			lcd_bwidth = 40;
+		if (lcd_hwidth < 0)
+			lcd_hwidth = 64;
+		if (lcd_height < 0)
+			lcd_height = 2;
+		break;
+	case LCD_TYPE_CUSTOM:	/* customer-defined */
+		if (lcd_proto < 0)
+			lcd_proto = DEFAULT_LCD_PROTO;
+		if (lcd_charset < 0)
+			lcd_charset = DEFAULT_LCD_CHARSET;
+		/* default geometry will be set later */
+		break;
+	case LCD_TYPE_HANTRONIX:	/* parallel mode, 8 bits, hantronix-like */
+	default:
+		if (lcd_proto < 0)
+			lcd_proto = LCD_PROTO_PARALLEL;
+		if (lcd_charset < 0)
+			lcd_charset = LCD_CHARSET_NORMAL;
+		if (lcd_e_pin == PIN_NOT_SET)
+			lcd_e_pin = PIN_STROBE;
+		if (lcd_rs_pin == PIN_NOT_SET)
+			lcd_rs_pin = PIN_SELECP;
+
+		if (lcd_width < 0)
+			lcd_width = 16;
+		if (lcd_bwidth < 0)
+			lcd_bwidth = 40;
+		if (lcd_hwidth < 0)
+			lcd_hwidth = 64;
+		if (lcd_height < 0)
+			lcd_height = 2;
+		break;
+	}
+
+	/* this is used to catch wrong and default values */
+	if (lcd_width <= 0)
+		lcd_width = DEFAULT_LCD_WIDTH;
+	if (lcd_bwidth <= 0)
+		lcd_bwidth = DEFAULT_LCD_BWIDTH;
+	if (lcd_hwidth <= 0)
+		lcd_hwidth = DEFAULT_LCD_HWIDTH;
+	if (lcd_height <= 0)
+		lcd_height = DEFAULT_LCD_HEIGHT;
+
+	if (lcd_proto == LCD_PROTO_SERIAL) {	/* SERIAL */
+		lcd_write_cmd = lcd_write_cmd_s;
+		lcd_write_data = lcd_write_data_s;
+		lcd_clear_fast = lcd_clear_fast_s;
+
+		if (lcd_cl_pin == PIN_NOT_SET)
+			lcd_cl_pin = DEFAULT_LCD_PIN_SCL;
+		if (lcd_da_pin == PIN_NOT_SET)
+			lcd_da_pin = DEFAULT_LCD_PIN_SDA;
+
+	} else {		/* PARALLEL */
+		lcd_write_cmd = lcd_write_cmd_p8;
+		lcd_write_data = lcd_write_data_p8;
+		lcd_clear_fast = lcd_clear_fast_p8;
+
+		if (lcd_e_pin == PIN_NOT_SET)
+			lcd_e_pin = DEFAULT_LCD_PIN_E;
+		if (lcd_rs_pin == PIN_NOT_SET)
+			lcd_rs_pin = DEFAULT_LCD_PIN_RS;
+		if (lcd_rw_pin == PIN_NOT_SET)
+			lcd_rw_pin = DEFAULT_LCD_PIN_RW;
+	}
+
+	if (lcd_bl_pin == PIN_NOT_SET)
+		lcd_bl_pin = DEFAULT_LCD_PIN_BL;
+
+	if (lcd_e_pin == PIN_NOT_SET)
+		lcd_e_pin = PIN_NONE;
+	if (lcd_rs_pin == PIN_NOT_SET)
+		lcd_rs_pin = PIN_NONE;
+	if (lcd_rw_pin == PIN_NOT_SET)
+		lcd_rw_pin = PIN_NONE;
+	if (lcd_bl_pin == PIN_NOT_SET)
+		lcd_bl_pin = PIN_NONE;
+	if (lcd_cl_pin == PIN_NOT_SET)
+		lcd_cl_pin = PIN_NONE;
+	if (lcd_da_pin == PIN_NOT_SET)
+		lcd_da_pin = PIN_NONE;
+
+	if (lcd_charset < 0)
+		lcd_charset = DEFAULT_LCD_CHARSET;
+
+	if (lcd_charset == LCD_CHARSET_KS0074)
+		lcd_char_conv = lcd_char_conv_ks0074;
+	else
+		lcd_char_conv = NULL;
+
+	if (lcd_bl_pin != PIN_NONE)
+		init_scan_timer();
+
+	pin_to_bits(lcd_e_pin, lcd_bits[LCD_PORT_D][LCD_BIT_E],
+		    lcd_bits[LCD_PORT_C][LCD_BIT_E]);
+	pin_to_bits(lcd_rs_pin, lcd_bits[LCD_PORT_D][LCD_BIT_RS],
+		    lcd_bits[LCD_PORT_C][LCD_BIT_RS]);
+	pin_to_bits(lcd_rw_pin, lcd_bits[LCD_PORT_D][LCD_BIT_RW],
+		    lcd_bits[LCD_PORT_C][LCD_BIT_RW]);
+	pin_to_bits(lcd_bl_pin, lcd_bits[LCD_PORT_D][LCD_BIT_BL],
+		    lcd_bits[LCD_PORT_C][LCD_BIT_BL]);
+	pin_to_bits(lcd_cl_pin, lcd_bits[LCD_PORT_D][LCD_BIT_CL],
+		    lcd_bits[LCD_PORT_C][LCD_BIT_CL]);
+	pin_to_bits(lcd_da_pin, lcd_bits[LCD_PORT_D][LCD_BIT_DA],
+		    lcd_bits[LCD_PORT_C][LCD_BIT_DA]);
+
+	/* before this line, we must NOT send anything to the display.
+	 * Since lcd_init_display() needs to write data, we have to
+	 * enable mark the LCD initialized just before.
+	 */
+	lcd_initialized = 1;
+	lcd_init_display();
+
+	/* display a short message */
+#ifdef CONFIG_PANEL_CHANGE_MESSAGE
+#ifdef CONFIG_PANEL_BOOT_MESSAGE
+	panel_lcd_print("\x1b[Lc\x1b[Lb\x1b[L*" CONFIG_PANEL_BOOT_MESSAGE);
+#endif
+#else
+	panel_lcd_print("\x1b[Lc\x1b[Lb\x1b[L*Linux-" UTS_RELEASE "\nPanel-"
+			PANEL_VERSION);
+#endif
+	lcd_addr_x = lcd_addr_y = 0;
+	lcd_must_clear = 1;	/* clear the display on the next device opening */
+	lcd_gotoxy();
+}
+
+/*
+ * These are the file operation function for user access to /dev/keypad
+ */
+
+static ssize_t keypad_read(struct file *file,
+			   char *buf, size_t count, loff_t *ppos)
+{
+
+	unsigned i = *ppos;
+	char *tmp = buf;
+
+	if (keypad_buflen == 0) {
+		if (file->f_flags & O_NONBLOCK)
+			return -EAGAIN;
+
+		interruptible_sleep_on(&keypad_read_wait);
+		if (signal_pending(current))
+			return -EINTR;
+	}
+
+	for (; count-- > 0 && (keypad_buflen > 0); ++i, ++tmp, --keypad_buflen) {
+		put_user(keypad_buffer[keypad_start], tmp);
+		keypad_start = (keypad_start + 1) % KEYPAD_BUFFER;
+	}
+	*ppos = i;
+
+	return tmp - buf;
+}
+
+static int keypad_open(struct inode *inode, struct file *file)
+{
+
+	if (keypad_open_cnt)
+		return -EBUSY;	/* open only once at a time */
+
+	if (file->f_mode & FMODE_WRITE)	/* device is read-only */
+		return -EPERM;
+
+	keypad_buflen = 0;	/* flush the buffer on opening */
+	keypad_open_cnt++;
+	return 0;
+}
+
+static int keypad_release(struct inode *inode, struct file *file)
+{
+	keypad_open_cnt--;
+	return 0;
+}
+
+static struct file_operations keypad_fops = {
+	.read    = keypad_read,		/* read */
+	.open    = keypad_open,		/* open */
+	.release = keypad_release,	/* close */
+};
+
+static struct miscdevice keypad_dev = {
+	KEYPAD_MINOR,
+	"keypad",
+	&keypad_fops
+};
+
+static void keypad_send_key(char *string, int max_len)
+{
+	if (init_in_progress)
+		return;
+
+	/* send the key to the device only if a process is attached to it. */
+	if (keypad_open_cnt > 0) {
+		while (max_len-- && keypad_buflen < KEYPAD_BUFFER && *string) {
+			keypad_buffer[(keypad_start + keypad_buflen++) %
+				      KEYPAD_BUFFER] = *string++;
+		}
+		wake_up_interruptible(&keypad_read_wait);
+	}
+}
+
+/* this function scans all the bits involving at least one logical signal, and puts the
+ * results in the bitfield "phys_read" (one bit per established contact), and sets
+ * "phys_read_prev" to "phys_read".
+ *
+ * Note: to debounce input signals, we will only consider as switched a signal which is
+ * stable across 2 measures. Signals which are different between two reads will be kept
+ * as they previously were in their logical form (phys_prev). A signal which has just
+ * switched will have a 1 in (phys_read ^ phys_read_prev).
+ */
+static void phys_scan_contacts(void)
+{
+	int bit, bitval;
+	char oldval;
+	char bitmask;
+	char gndmask;
+
+	phys_prev = phys_curr;
+	phys_read_prev = phys_read;
+	phys_read = 0;		/* flush all signals */
+
+	oldval = r_dtr(pprt) | scan_mask_o;	/* keep track of old value, with all outputs disabled */
+	w_dtr(pprt, oldval & ~scan_mask_o);	/* activate all keyboard outputs (active low) */
+	bitmask = PNL_PINPUT(r_str(pprt)) & scan_mask_i;	/* will have a 1 for each bit set to gnd */
+	w_dtr(pprt, oldval);	/* disable all matrix signals */
+
+	/* now that all outputs are cleared, the only active input bits are
+	 * directly connected to the ground
+	 */
+	gndmask = PNL_PINPUT(r_str(pprt)) & scan_mask_i;	/* 1 for each grounded input */
+
+	phys_read |= (pmask_t) gndmask << 40;	/* grounded inputs are signals 40-44 */
+
+	if (bitmask != gndmask) {
+		/* since clearing the outputs changed some inputs, we know that some
+		 * input signals are currently tied to some outputs. So we'll scan them.
+		 */
+		for (bit = 0; bit < 8; bit++) {
+			bitval = 1 << bit;
+
+			if (!(scan_mask_o & bitval))	/* skip unused bits */
+				continue;
+
+			w_dtr(pprt, oldval & ~bitval);	/* enable this output */
+			bitmask = PNL_PINPUT(r_str(pprt)) & ~gndmask;
+			phys_read |= (pmask_t) bitmask << (5 * bit);
+		}
+		w_dtr(pprt, oldval);	/* disable all outputs */
+	}
+	/* this is easy: use old bits when they are flapping, use new ones when stable */
+	phys_curr =
+	    (phys_prev & (phys_read ^ phys_read_prev)) | (phys_read &
+							  ~(phys_read ^
+							    phys_read_prev));
+}
+
+static void panel_process_inputs(void)
+{
+	struct list_head *item;
+	struct logical_input *input;
+
+#if 0
+	printk(KERN_DEBUG
+	       "entering panel_process_inputs with pp=%016Lx & pc=%016Lx\n",
+	       phys_prev, phys_curr);
+#endif
+
+	keypressed = 0;
+	inputs_stable = 1;
+	list_for_each(item, &logical_inputs) {
+		input = list_entry(item, struct logical_input, list);
+
+		switch (input->state) {
+		case INPUT_ST_LOW:
+			if ((phys_curr & input->mask) != input->value)
+				break;
+			/* if all needed ones were already set previously, this means that
+			 * this logical signal has been activated by the releasing of
+			 * another combined signal, so we don't want to match.
+			 * eg: AB -(release B)-> A -(release A)-> 0 : don't match A.
+			 */
+			if ((phys_prev & input->mask) == input->value)
+				break;
+			input->rise_timer = 0;
+			input->state = INPUT_ST_RISING;
+			/* no break here, fall through */
+		case INPUT_ST_RISING:
+			if ((phys_curr & input->mask) != input->value) {
+				input->state = INPUT_ST_LOW;
+				break;
+			}
+			if (input->rise_timer < input->rise_time) {
+				inputs_stable = 0;
+				input->rise_timer++;
+				break;
+			}
+			input->high_timer = 0;
+			input->state = INPUT_ST_HIGH;
+			/* no break here, fall through */
+		case INPUT_ST_HIGH:
+#if 0
+			/* FIXME:
+			 * this is an invalid test. It tries to catch transitions from single-key
+			 * to multiple-key, but doesn't take into account the contacts polarity.
+			 * The only solution to the problem is to parse keys from the most complex
+			 * to the simplest combinations, and mark them as 'caught' once a combination
+			 * matches, then unmatch it for all other ones.
+			 */
+
+			/* try to catch dangerous transitions cases :
+			 * someone adds a bit, so this signal was a false
+			 * positive resulting from a transition. We should invalidate
+			 * the signal immediately and not call the release function.
+			 * eg: 0 -(press A)-> A -(press B)-> AB : don't match A's release.
+			 */
+			if (((phys_prev & input->mask) == input->value)
+			    && ((phys_curr & input->mask) > input->value)) {
+				input->state = INPUT_ST_LOW;	/* invalidate */
+				break;
+			}
+#endif
+
+			if ((phys_curr & input->mask) == input->value) {
+				if ((input->type == INPUT_TYPE_STD)
+				    && (input->high_timer == 0)) {
+					input->high_timer++;
+					if (input->u.std.press_fct != NULL)
+						input->u.std.press_fct(input->u.
+								       std.
+								       press_data);
+				} else if (input->type == INPUT_TYPE_KBD) {
+					keypressed = 1;	/* will turn on the light */
+
+					if (input->high_timer == 0) {
+						if (input->u.kbd.press_str[0])
+							keypad_send_key(input->
+									u.kbd.
+									press_str,
+									sizeof
+									(input->
+									 u.kbd.
+									 press_str));
+					}
+
+					if (input->u.kbd.repeat_str[0]) {
+						if (input->high_timer >=
+						    KEYPAD_REP_START) {
+							input->high_timer -=
+							    KEYPAD_REP_DELAY;
+							keypad_send_key(input->
+									u.kbd.
+									repeat_str,
+									sizeof
+									(input->
+									 u.kbd.
+									 repeat_str));
+						}
+						inputs_stable = 0;	/* we will need to come back here soon */
+					}
+
+					if (input->high_timer < 255)
+						input->high_timer++;
+				}
+				break;
+			} else {
+				/* else signal falling down. Let's fall through. */
+				input->state = INPUT_ST_FALLING;
+				input->fall_timer = 0;
+			}
+			/* no break here, fall through */
+		case INPUT_ST_FALLING:
+#if 0
+			/* FIXME !!! same comment as above */
+			if (((phys_prev & input->mask) == input->value)
+			    && ((phys_curr & input->mask) > input->value)) {
+				input->state = INPUT_ST_LOW;	/* invalidate */
+				break;
+			}
+#endif
+
+			if ((phys_curr & input->mask) == input->value) {
+				if (input->type == INPUT_TYPE_KBD) {
+					keypressed = 1;	/* will turn on the light */
+
+					if (input->u.kbd.repeat_str[0]) {
+						if (input->high_timer >= KEYPAD_REP_START)
+							input->high_timer -= KEYPAD_REP_DELAY;
+						keypad_send_key(input->u.kbd.repeat_str,
+								sizeof(input->u.kbd.repeat_str));
+						inputs_stable = 0;	/* we will need to come back here soon */
+					}
+
+					if (input->high_timer < 255)
+						input->high_timer++;
+				}
+				input->state = INPUT_ST_HIGH;
+				break;
+			} else if (input->fall_timer >= input->fall_time) {
+				/* call release event */
+				if (input->type == INPUT_TYPE_STD) {
+					if (input->u.std.release_fct != NULL)
+						input->u.std.release_fct(input->u.std.release_data);
+
+				} else if (input->type == INPUT_TYPE_KBD) {
+					if (input->u.kbd.release_str[0])
+						keypad_send_key(input->u.kbd.release_str,
+								sizeof(input->u.kbd.release_str));
+				}
+
+				input->state = INPUT_ST_LOW;
+				break;
+			} else {
+				input->fall_timer++;
+				inputs_stable = 0;
+				break;
+			}
+		}
+	}
+}
+
+static void panel_scan_timer(void)
+{
+	if (keypad_enabled && keypad_initialized) {
+		if (spin_trylock(&pprt_lock)) {
+			phys_scan_contacts();
+			spin_unlock(&pprt_lock);	/* no need for the parport anymore */
+		}
+
+		if (!inputs_stable || phys_curr != phys_prev)
+			panel_process_inputs();
+	}
+
+	if (lcd_enabled && lcd_initialized) {
+		if (keypressed) {
+			if (light_tempo == 0 && ((lcd_flags & LCD_FLAG_L) == 0))
+				lcd_backlight(1);
+			light_tempo = FLASH_LIGHT_TEMPO;
+		} else if (light_tempo > 0) {
+			light_tempo--;
+			if (light_tempo == 0 && ((lcd_flags & LCD_FLAG_L) == 0))
+				lcd_backlight(0);
+		}
+	}
+
+	mod_timer(&scan_timer, jiffies + INPUT_POLL_TIME);
+}
+
+static void init_scan_timer(void)
+{
+	if (scan_timer.function != NULL)
+		return;		/* already started */
+
+	init_timer(&scan_timer);
+	scan_timer.expires = jiffies + INPUT_POLL_TIME;
+	scan_timer.data = 0;
+	scan_timer.function = (void *)&panel_scan_timer;
+	add_timer(&scan_timer);
+}
+
+/* converts a name of the form "({BbAaPpSsEe}{01234567-})*" to a series of bits.
+ * if <omask> or <imask> are non-null, they will be or'ed with the bits corresponding
+ * to out and in bits respectively.
+ * returns 1 if ok, 0 if error (in which case, nothing is written).
+ */
+static int input_name2mask(char *name, pmask_t *mask, pmask_t *value,
+			   char *imask, char *omask)
+{
+	static char sigtab[10] = "EeSsPpAaBb";
+	char im, om;
+	pmask_t m, v;
+
+	om = im = m = v = 0ULL;
+	while (*name) {
+		int in, out, bit, neg;
+		for (in = 0; (in < sizeof(sigtab)) && (sigtab[in] != *name); in++)
+			;
+		if (in >= sizeof(sigtab))
+			return 0;	/* input name not found */
+		neg = (in & 1);	/* odd (lower) names are negated */
+		in >>= 1;
+		im |= (1 << in);
+
+		name++;
+		if (isdigit(*name)) {
+			out = *name - '0';
+			om |= (1 << out);
+		} else if (*name == '-')
+			out = 8;
+		else
+			return 0;	/* unknown bit name */
+
+		bit = (out * 5) + in;
+
+		m |= 1ULL << bit;
+		if (!neg)
+			v |= 1ULL << bit;
+		name++;
+	}
+	*mask = m;
+	*value = v;
+	if (imask)
+		*imask |= im;
+	if (omask)
+		*omask |= om;
+	return 1;
+}
+
+/* tries to bind a key to the signal name <name>. The key will send the
+ * strings <press>, <repeat>, <release> for these respective events.
+ * Returns the pointer to the new key if ok, NULL if the key could not be bound.
+ */
+static struct logical_input *panel_bind_key(char *name, char *press,
+					    char *repeat, char *release)
+{
+	struct logical_input *key;
+
+	key = kmalloc(sizeof(struct logical_input), GFP_KERNEL);
+	if (!key) {
+		printk(KERN_ERR "panel: not enough memory\n");
+		return NULL;
+	}
+	memset(key, 0, sizeof(struct logical_input));
+	if (!input_name2mask(name, &key->mask, &key->value, &scan_mask_i,
+			     &scan_mask_o))
+		return NULL;
+
+	key->type = INPUT_TYPE_KBD;
+	key->state = INPUT_ST_LOW;
+	key->rise_time = 1;
+	key->fall_time = 1;
+
+#if 0
+	printk(KERN_DEBUG "bind: <%s> : m=%016Lx v=%016Lx\n", name, key->mask,
+	       key->value);
+#endif
+	strncpy(key->u.kbd.press_str, press, sizeof(key->u.kbd.press_str));
+	strncpy(key->u.kbd.repeat_str, repeat, sizeof(key->u.kbd.repeat_str));
+	strncpy(key->u.kbd.release_str, release,
+		sizeof(key->u.kbd.release_str));
+	list_add(&key->list, &logical_inputs);
+	return key;
+}
+
+#if 0
+/* tries to bind a callback function to the signal name <name>. The function
+ * <press_fct> will be called with the <press_data> arg when the signal is
+ * activated, and so on for <release_fct>/<release_data>
+ * Returns the pointer to the new signal if ok, NULL if the signal could not be bound.
+ */
+static struct logical_input *panel_bind_callback(char *name,
+						 void (*press_fct) (int),
+						 int press_data,
+						 void (*release_fct) (int),
+						 int release_data)
+{
+	struct logical_input *callback;
+
+	callback = kmalloc(sizeof(struct logical_input), GFP_KERNEL);
+	if (!callback) {
+		printk(KERN_ERR "panel: not enough memory\n");
+		return NULL;
+	}
+	memset(callback, 0, sizeof(struct logical_input));
+	if (!input_name2mask(name, &callback->mask, &callback->value,
+			     &scan_mask_i, &scan_mask_o))
+		return NULL;
+
+	callback->type = INPUT_TYPE_STD;
+	callback->state = INPUT_ST_LOW;
+	callback->rise_time = 1;
+	callback->fall_time = 1;
+	callback->u.std.press_fct = press_fct;
+	callback->u.std.press_data = press_data;
+	callback->u.std.release_fct = release_fct;
+	callback->u.std.release_data = release_data;
+	list_add(&callback->list, &logical_inputs);
+	return callback;
+}
+#endif
+
+static void keypad_init(void)
+{
+	int keynum;
+	init_waitqueue_head(&keypad_read_wait);
+	keypad_buflen = 0;	/* flushes any eventual noisy keystroke */
+
+	/* Let's create all known keys */
+
+	for (keynum = 0; keypad_profile[keynum][0][0]; keynum++) {
+		panel_bind_key(keypad_profile[keynum][0],
+			       keypad_profile[keynum][1],
+			       keypad_profile[keynum][2],
+			       keypad_profile[keynum][3]);
+	}
+
+	init_scan_timer();
+	keypad_initialized = 1;
+}
+
+/**************************************************/
+/* device initialization                          */
+/**************************************************/
+
+static int panel_notify_sys(struct notifier_block *this, unsigned long code,
+			    void *unused)
+{
+	if (lcd_enabled && lcd_initialized) {
+		switch (code) {
+		case SYS_DOWN:
+			panel_lcd_print
+			    ("\x0cReloading\nSystem...\x1b[Lc\x1b[Lb\x1b[L+");
+			break;
+		case SYS_HALT:
+			panel_lcd_print
+			    ("\x0cSystem Halted.\x1b[Lc\x1b[Lb\x1b[L+");
+			break;
+		case SYS_POWER_OFF:
+			panel_lcd_print("\x0cPower off.\x1b[Lc\x1b[Lb\x1b[L+");
+			break;
+		default:
+			break;
+		}
+	}
+	return NOTIFY_DONE;
+}
+
+static struct notifier_block panel_notifier = {
+	panel_notify_sys,
+	NULL,
+	0
+};
+
+static void panel_attach(struct parport *port)
+{
+	if (port->number != parport)
+		return;
+
+	if (pprt) {
+		printk(KERN_ERR
+		       "panel_attach(): port->number=%d parport=%d, already registered !\n",
+		       port->number, parport);
+		return;
+	}
+
+	pprt = parport_register_device(port, "panel", NULL, NULL,	/* pf, kf */
+				       NULL,
+				       /*PARPORT_DEV_EXCL */
+				       0, (void *)&pprt);
+
+	if (parport_claim(pprt)) {
+		printk(KERN_ERR
+		       "Panel: could not claim access to parport%d. Aborting.\n",
+		       parport);
+		return;
+	}
+
+	/* must init LCD first, just in case an IRQ from the keypad is generated at keypad init */
+	if (lcd_enabled) {
+		lcd_init();
+		misc_register(&lcd_dev);
+	}
+
+	if (keypad_enabled) {
+		keypad_init();
+		misc_register(&keypad_dev);
+	}
+}
+
+static void panel_detach(struct parport *port)
+{
+	if (port->number != parport)
+		return;
+
+	if (!pprt) {
+		printk(KERN_ERR
+		       "panel_detach(): port->number=%d parport=%d, nothing to unregister.\n",
+		       port->number, parport);
+		return;
+	}
+
+	if (keypad_enabled && keypad_initialized)
+		misc_deregister(&keypad_dev);
+
+	if (lcd_enabled && lcd_initialized)
+		misc_deregister(&lcd_dev);
+
+	parport_release(pprt);
+	parport_unregister_device(pprt);
+	pprt = NULL;
+}
+
+static struct parport_driver panel_driver = {
+	.name = "panel",
+	.attach = panel_attach,
+	.detach = panel_detach,
+};
+
+/* init function */
+int panel_init(void)
+{
+	/* for backwards compatibility */
+	if (keypad_type < 0)
+		keypad_type = keypad_enabled;
+
+	if (lcd_type < 0)
+		lcd_type = lcd_enabled;
+
+	if (parport < 0)
+		parport = DEFAULT_PARPORT;
+
+	/* take care of an eventual profile */
+	switch (profile) {
+	case PANEL_PROFILE_CUSTOM:	/* custom profile */
+		if (keypad_type < 0)
+			keypad_type = DEFAULT_KEYPAD;
+		if (lcd_type < 0)
+			lcd_type = DEFAULT_LCD;
+		break;
+	case PANEL_PROFILE_OLD:	/* 8 bits, 2*16, old keypad */
+		if (keypad_type < 0)
+			keypad_type = KEYPAD_TYPE_OLD;
+		if (lcd_type < 0)
+			lcd_type = LCD_TYPE_OLD;
+		if (lcd_width < 0)
+			lcd_width = 16;
+		if (lcd_hwidth < 0)
+			lcd_hwidth = 16;
+		break;
+	case PANEL_PROFILE_NEW:	/* serial, 2*16, new keypad */
+		if (keypad_type < 0)
+			keypad_type = KEYPAD_TYPE_NEW;
+		if (lcd_type < 0)
+			lcd_type = LCD_TYPE_KS0074;
+		break;
+	case PANEL_PROFILE_HANTRONIX:	/* 8 bits, 2*16 hantronix-like, no keypad */
+		if (keypad_type < 0)
+			keypad_type = KEYPAD_TYPE_NONE;
+		if (lcd_type < 0)
+			lcd_type = LCD_TYPE_HANTRONIX;
+		break;
+	case PANEL_PROFILE_NEXCOM:	/* generic 8 bits, 2*16, nexcom keypad, eg. Nexcom. */
+		if (keypad_type < 0)
+			keypad_type = KEYPAD_TYPE_NEXCOM;
+		if (lcd_type < 0)
+			lcd_type = LCD_TYPE_NEXCOM;
+		break;
+	case PANEL_PROFILE_LARGE:	/* 8 bits, 2*40, old keypad */
+		if (keypad_type < 0)
+			keypad_type = KEYPAD_TYPE_OLD;
+		if (lcd_type < 0)
+			lcd_type = LCD_TYPE_OLD;
+		break;
+	}
+
+	lcd_enabled = (lcd_type > 0);
+	keypad_enabled = (keypad_type > 0);
+
+	switch (keypad_type) {
+	case KEYPAD_TYPE_OLD:
+		keypad_profile = old_keypad_profile;
+		break;
+	case KEYPAD_TYPE_NEW:
+		keypad_profile = new_keypad_profile;
+		break;
+	case KEYPAD_TYPE_NEXCOM:
+		keypad_profile = nexcom_keypad_profile;
+		break;
+	default:
+		keypad_profile = NULL;
+		break;
+	}
+
+	/* tells various subsystems about the fact that we are initializing */
+	init_in_progress = 1;
+
+	if (parport_register_driver(&panel_driver)) {
+		printk(KERN_ERR
+		       "Panel: could not register with parport. Aborting.\n");
+		return -EIO;
+	}
+
+	if (!lcd_enabled && !keypad_enabled) {
+		/* no device enabled, let's release the parport */
+		if (pprt) {
+			parport_release(pprt);
+			parport_unregister_device(pprt);
+		}
+		parport_unregister_driver(&panel_driver);
+		printk(KERN_ERR "Panel driver version " PANEL_VERSION
+		       " disabled.\n");
+		return -ENODEV;
+	}
+
+	register_reboot_notifier(&panel_notifier);
+
+	if (pprt)
+		printk(KERN_INFO "Panel driver version " PANEL_VERSION
+		       " registered on parport%d (io=0x%lx).\n", parport,
+		       pprt->port->base);
+	else
+		printk(KERN_INFO "Panel driver version " PANEL_VERSION
+		       " not yet registered\n");
+	/* tells various subsystems about the fact that initialization is finished */
+	init_in_progress = 0;
+	return 0;
+}
+
+static int __init panel_init_module(void)
+{
+	return panel_init();
+}
+
+static void __exit panel_cleanup_module(void)
+{
+	unregister_reboot_notifier(&panel_notifier);
+
+	if (scan_timer.function != NULL)
+		del_timer(&scan_timer);
+
+	if (keypad_enabled)
+		misc_deregister(&keypad_dev);
+
+	if (lcd_enabled) {
+		panel_lcd_print("\x0cLCD driver " PANEL_VERSION
+				"\nunloaded.\x1b[Lc\x1b[Lb\x1b[L-");
+		misc_deregister(&lcd_dev);
+	}
+
+	/* TODO: free all input signals */
+
+	parport_release(pprt);
+	parport_unregister_device(pprt);
+	parport_unregister_driver(&panel_driver);
+}
+
+module_init(panel_init_module);
+module_exit(panel_cleanup_module);
+MODULE_AUTHOR("Willy Tarreau");
+MODULE_LICENSE("GPL");
+
+/*
+ * Local variables:
+ *  c-indent-level: 4
+ *  tab-width: 8
+ * End:
+ */
diff --git a/drivers/staging/poch/README b/drivers/staging/poch/README
index f65e979743b..842afd46499 100644
--- a/drivers/staging/poch/README
+++ b/drivers/staging/poch/README
@@ -1,5 +1,12 @@
 TODO:
-	- fix transmit overflows
+	- Rx block size is limited to < 2048, hardware bug?
+	- Group size is limited to < page size, kernel alloc/mmap API issues
+	- fix/workaround cache issues in circular buffer header
+	- test whether Tx is transmitting data from provided buffers
+	- handle device unplug case
+	- handle temperature above threshold
+	- use bus address instead of physical address for DMA
+	- support for snapshot mode
 	- audit userspace interfaces
 	- get reserved major/minor if needed
 
diff --git a/drivers/staging/poch/poch.c b/drivers/staging/poch/poch.c
index 0e113f9a158..ec343ef53a8 100644
--- a/drivers/staging/poch/poch.c
+++ b/drivers/staging/poch/poch.c
@@ -126,9 +126,11 @@
 #define FPGA_INT_TX_ACQ_DONE		(0x1 << 1)
 #define FPGA_INT_RX_ACQ_DONE		(0x1)
 
-#define FPGA_RX_ADC_CTL_REG		0x214
-#define FPGA_RX_ADC_CTL_CONT_CAP	(0x0)
-#define FPGA_RX_ADC_CTL_SNAP_CAP	(0x1)
+#define FPGA_RX_CTL_REG			0x214
+#define FPGA_RX_CTL_FIFO_FLUSH      	(0x1 << 9)
+#define FPGA_RX_CTL_SYNTH_DATA		(0x1 << 8)
+#define FPGA_RX_CTL_CONT_CAP		(0x0 << 1)
+#define FPGA_RX_CTL_SNAP_CAP		(0x1 << 1)
 
 #define FPGA_RX_ARM_REG			0x21C
 
@@ -299,6 +301,14 @@ static ssize_t show_direction(struct device *dev,
 }
 static DEVICE_ATTR(dir, S_IRUSR|S_IRGRP, show_direction, NULL);
 
+static unsigned long npages(unsigned long bytes)
+{
+	if (bytes % PAGE_SIZE == 0)
+		return bytes / PAGE_SIZE;
+	else
+		return (bytes / PAGE_SIZE) + 1;
+}
+
 static ssize_t show_mmap_size(struct device *dev,
 			      struct device_attribute *attr, char *buf)
 {
@@ -309,10 +319,8 @@ static ssize_t show_mmap_size(struct device *dev,
 	unsigned long header_pages;
 	unsigned long total_group_pages;
 
-	/* FIXME: We do not have to add 1, if group_size a multiple of
-	   PAGE_SIZE. */
-	group_pages = (channel->group_size / PAGE_SIZE) + 1;
-	header_pages = (channel->header_size / PAGE_SIZE) + 1;
+	group_pages = npages(channel->group_size);
+	header_pages = npages(channel->header_size);
 	total_group_pages = group_pages * channel->group_count;
 
 	mmap_size = (header_pages + total_group_pages) * PAGE_SIZE;
@@ -350,8 +358,8 @@ static int poch_channel_alloc_groups(struct channel_info *channel)
 	unsigned long group_pages;
 	unsigned long header_pages;
 
-	group_pages = (channel->group_size / PAGE_SIZE) + 1;
-	header_pages = (channel->header_size / PAGE_SIZE) + 1;
+	group_pages = npages(channel->group_size);
+	header_pages = npages(channel->header_size);
 
 	for (i = 0; i < channel->group_count; i++) {
 		struct poch_group_info *group;
@@ -384,18 +392,45 @@ static int poch_channel_alloc_groups(struct channel_info *channel)
 		group->user_offset =
 			(header_pages + (i * group_pages)) * PAGE_SIZE;
 
-		printk(KERN_INFO PFX "%ld: user_offset: 0x%lx dma: 0x%x\n", i,
-		       group->user_offset, group->dma_addr);
+		printk(KERN_INFO PFX "%ld: user_offset: 0x%lx\n", i,
+		       group->user_offset);
 	}
 
 	return 0;
 }
 
-static void channel_latch_attr(struct channel_info *channel)
+static int channel_latch_attr(struct channel_info *channel)
 {
 	channel->group_count = atomic_read(&channel->sys_group_count);
 	channel->group_size = atomic_read(&channel->sys_group_size);
 	channel->block_size = atomic_read(&channel->sys_block_size);
+
+	if (channel->group_count == 0) {
+		printk(KERN_ERR PFX "invalid group count %lu",
+		       channel->group_count);
+		return -EINVAL;
+	}
+
+	if (channel->group_size == 0 ||
+	    channel->group_size < channel->block_size) {
+		printk(KERN_ERR PFX "invalid group size %lu",
+		       channel->group_size);
+		return -EINVAL;
+	}
+
+	if (channel->block_size == 0 || (channel->block_size % 8) != 0) {
+		printk(KERN_ERR PFX "invalid block size %lu",
+		       channel->block_size);
+		return -EINVAL;
+	}
+
+	if (channel->group_size % channel->block_size != 0) {
+		printk(KERN_ERR PFX
+		       "group size should be multiple of block size");
+		return -EINVAL;
+	}
+
+	return 0;
 }
 
 /*
@@ -432,7 +467,10 @@ static void channel_dma_init(struct channel_info *channel)
 	}
 
 	printk(KERN_WARNING "block_size, group_size, group_count\n");
-	iowrite32(channel->block_size, fpga + block_size_reg);
+	/*
+	 * Block size is represented in no. of 64 bit transfers.
+	 */
+	iowrite32(channel->block_size / 8, fpga + block_size_reg);
 	iowrite32(channel->group_size / channel->block_size,
 		  fpga + block_count_reg);
 	iowrite32(channel->group_count, fpga + group_count_reg);
@@ -447,27 +485,30 @@ static void channel_dma_init(struct channel_info *channel)
 	/* The DMA address page register is shared between the RX and
 	 * TX channels, so acquire lock.
 	 */
-	spin_lock(channel->iomem_lock);
 	for (i = 0; i < channel->group_count; i++) {
 		page = i / 32;
 		group_in_page = i % 32;
 
 		group_reg = group_regs_base + (group_in_page * 4);
 
+		spin_lock(channel->iomem_lock);
 		iowrite32(page, fpga + FPGA_DMA_ADR_PAGE_REG);
 		iowrite32(channel->groups[i].dma_addr, fpga + group_reg);
+		spin_unlock(channel->iomem_lock);
 	}
+
 	for (i = 0; i < channel->group_count; i++) {
 		page = i / 32;
 		group_in_page = i % 32;
 
 		group_reg = group_regs_base + (group_in_page * 4);
 
+		spin_lock(channel->iomem_lock);
 		iowrite32(page, fpga + FPGA_DMA_ADR_PAGE_REG);
 		printk(KERN_INFO PFX "%ld: read dma_addr: 0x%x\n", i,
 		       ioread32(fpga + group_reg));
+		spin_unlock(channel->iomem_lock);
 	}
-	spin_unlock(channel->iomem_lock);
 
 }
 
@@ -538,7 +579,9 @@ static int poch_channel_init(struct channel_info *channel,
 
 	printk(KERN_WARNING "channel_latch_attr\n");
 
-	channel_latch_attr(channel);
+	ret = channel_latch_attr(channel);
+	if (ret != 0)
+		goto out;
 
 	channel->transfer = 0;
 
@@ -781,6 +824,11 @@ static int poch_open(struct inode *inode, struct file *filp)
 		iowrite32(FPGA_TX_CTL_FIFO_FLUSH
 			  | FPGA_TX_CTL_OUTPUT_CARDBUS,
 			  fpga + FPGA_TX_CTL_REG);
+	} else {
+		/* Flush RX FIFO and output data to cardbus. */
+		iowrite32(FPGA_RX_CTL_CONT_CAP
+			  | FPGA_RX_CTL_FIFO_FLUSH,
+			  fpga + FPGA_RX_CTL_REG);
 	}
 
 	atomic_inc(&channel->inited);
@@ -847,8 +895,8 @@ static int poch_mmap(struct file *filp, struct vm_area_struct *vma)
 		return -EINVAL;
 	}
 
-	group_pages = (channel->group_size / PAGE_SIZE) + 1;
-	header_pages = (channel->header_size / PAGE_SIZE) + 1;
+	group_pages = npages(channel->group_size);
+	header_pages = npages(channel->header_size);
 	total_group_pages = group_pages * channel->group_count;
 
 	size = vma->vm_end - vma->vm_start;
@@ -903,14 +951,7 @@ static int poch_channel_available(struct channel_info *channel)
 	spin_lock_irq(&channel->group_offsets_lock);
 
 	for (i = 0; i < channel->group_count; i++) {
-		if (channel->dir == CHANNEL_DIR_RX
-		    && channel->header->group_offsets[i] == -1) {
-			spin_unlock_irq(&channel->group_offsets_lock);
-			return 1;
-		}
-
-		if (channel->dir == CHANNEL_DIR_TX
-		    && channel->header->group_offsets[i] != -1) {
+		if (channel->header->group_offsets[i] != -1) {
 			spin_unlock_irq(&channel->group_offsets_lock);
 			return 1;
 		}
@@ -1058,10 +1099,7 @@ static void poch_irq_dma(struct channel_info *channel)
 
 	for (i = 0; i < groups_done; i++) {
 		j = (prev_transfer + i) % channel->group_count;
-		if (channel->dir == CHANNEL_DIR_RX)
-			group_offsets[j] = -1;
-		else
-			group_offsets[j] = groups[j].user_offset;
+		group_offsets[j] = groups[j].user_offset;
 	}
 
 	spin_unlock(&channel->group_offsets_lock);
@@ -1283,7 +1321,7 @@ static int __devinit poch_pci_probe(struct pci_dev *pdev,
 	}
 
 	ret = request_irq(pdev->irq, poch_irq_handler, IRQF_SHARED,
-			  dev->bus_id, poch_dev);
+			  dev_name(dev), poch_dev);
 	if (ret) {
 		dev_err(dev, "error requesting IRQ %u\n", pdev->irq);
 		ret = -ENOMEM;
@@ -1350,12 +1388,12 @@ static void poch_pci_remove(struct pci_dev *pdev)
 	unsigned int minor = MINOR(poch_dev->cdev.dev);
 	unsigned int id = minor / poch_dev->nchannels;
 
-	/* FIXME: unmap fpga_iomem and bridge_iomem */
-
 	poch_class_dev_unregister(poch_dev, id);
 	cdev_del(&poch_dev->cdev);
 	idr_remove(&poch_ids, id);
 	free_irq(pdev->irq, poch_dev);
+	iounmap(poch_dev->fpga_iomem);
+	iounmap(poch_dev->bridge_iomem);
 	uio_unregister_device(uio);
 	pci_release_regions(pdev);
 	pci_disable_device(pdev);
diff --git a/drivers/staging/rspiusb/Kconfig b/drivers/staging/rspiusb/Kconfig
new file mode 100644
index 00000000000..d225f6794d0
--- /dev/null
+++ b/drivers/staging/rspiusb/Kconfig
@@ -0,0 +1,6 @@
+config USB_RSPI
+	tristate "Princeton Instruments USB camera support"
+	default n
+	depends on USB && BROKEN
+	help
+	  This driver is for the Princeton Instruments USB camera device.
diff --git a/drivers/staging/rspiusb/Makefile b/drivers/staging/rspiusb/Makefile
new file mode 100644
index 00000000000..cc7aed92b0e
--- /dev/null
+++ b/drivers/staging/rspiusb/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_USB_RSPI)		+= rspiusb.o
diff --git a/drivers/staging/rspiusb/TODO b/drivers/staging/rspiusb/TODO
new file mode 100644
index 00000000000..cd6336a9254
--- /dev/null
+++ b/drivers/staging/rspiusb/TODO
@@ -0,0 +1,22 @@
+This driver is for the Princeton Instruments USB camera.
+
+It needs lots of work to get it into the main drivers/usb/ subdirectory:
+
+Any patches to do any of the following changes are greatly appreciated:
+
+	- make checkpatch.pl clean
+	- coding style fixups (typedefs, etc.)
+	- get it to build properly
+	- audit ioctls
+	- remove ioctls if possible
+	- assign proper minor number
+	- remove dbg() macro
+	- lots of general cleanups
+	- review locking
+
+Please send patches to:
+	Greg Kroah-Hartman <gregkh@suse.de>
+and CC:
+	Judd Montgomery <judd@jpilot.org>
+	Jeff Frontz <jeff.frontz@gmail.com>
+as they have this device and can test any needed changes.
diff --git a/drivers/staging/rspiusb/rspiusb.c b/drivers/staging/rspiusb/rspiusb.c
new file mode 100644
index 00000000000..ca281d6cbd7
--- /dev/null
+++ b/drivers/staging/rspiusb/rspiusb.c
@@ -0,0 +1,887 @@
+/*
+ * rspiusb.c
+ *
+ * Copyright (C) 2005, 2006 Princeton Instruments
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation version 2 of the License
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include <linux/vmalloc.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/smp_lock.h>
+#include <linux/completion.h>
+#include <linux/scatterlist.h>
+#include <linux/usb.h>
+#include <linux/mm.h>
+#include <linux/pagemap.h>
+#include <linux/ioctl.h>
+#include "rspiusb.h"
+
+#ifdef CONFIG_USB_DEBUG
+static int debug = 1;
+#else
+static int debug;
+#endif
+/* Use our own dbg macro */
+#undef dbg
+#define dbg(format, arg...) do { if (debug) printk(KERN_DEBUG __FILE__ ": " format "\n" , ## arg); } while (0)
+
+/* Version Information */
+#define DRIVER_VERSION "V1.0.1"
+#define DRIVER_AUTHOR  "Princeton Instruments"
+#define DRIVER_DESC    "PI USB2.0 Device Driver for Linux"
+
+/* Define these values to match your devices */
+#define VENDOR_ID   0x0BD7
+#define ST133_PID   0xA010
+#define PIXIS_PID   0xA026
+
+/* Get a minor range for your devices from the usb maintainer */
+#ifdef CONFIG_USB_DYNAMIC_MINORS
+#define PIUSB_MINOR_BASE    0
+#else
+#define PIUSB_MINOR_BASE    192
+#endif
+
+/* prevent races between open() and disconnect() */
+static DECLARE_MUTEX(disconnect_sem);
+
+/* Structure to hold all of our device specific stuff */
+struct device_extension {
+	struct usb_device *udev;	/* save off the usb device pointer */
+	struct usb_interface *interface;	/* the interface for this device */
+	unsigned char minor;	/* the starting minor number for this device */
+	size_t bulk_in_size_returned;
+	int bulk_in_byte_trk;
+	struct urb ***PixelUrb;
+	int frameIdx;
+	int urbIdx;
+	unsigned int *maplist_numPagesMapped;
+	int open;		/* if the port is open or not */
+	int present;		/* if the device is not disconnected */
+	int userBufMapped;	/* has the user buffer been mapped? */
+	struct scatterlist **sgl;	/* scatter-gather list for user buffer */
+	unsigned int *sgEntries;
+	struct kref kref;
+	int gotPixelData;
+	int pendingWrite;
+	char **pendedPixelUrbs;
+	int iama;		/*PIXIS or ST133 */
+	int num_frames;		/* the number of frames that will fit in the user buffer */
+	int active_frame;
+	unsigned long frameSize;
+	struct semaphore sem;
+	//FX2 specific endpoints
+	unsigned int hEP[8];
+};
+#define to_pi_dev(d) container_of( d, struct device_extension, kref )
+
+static int MapUserBuffer(struct ioctl_struct *, struct device_extension *);
+static int UnMapUserBuffer(struct device_extension *);
+static int piusb_ioctl(struct inode *inode, struct file *file, unsigned int cmd,
+		       unsigned long arg);
+static int piusb_output(struct ioctl_struct *, unsigned char *, int, struct device_extension *);
+static struct usb_driver piusb_driver;
+
+/* table of devices that work with this driver */
+static struct usb_device_id pi_device_table[] = {
+	{USB_DEVICE(VENDOR_ID, ST133_PID)},
+	{USB_DEVICE(VENDOR_ID, PIXIS_PID)},
+	{0, }			/* Terminating entry */
+};
+
+MODULE_DEVICE_TABLE(usb, pi_device_table);
+
+static int lastErr = 0;
+static int errCnt = 0;
+
+static void piusb_delete(struct kref *kref)
+{
+	struct device_extension *pdx = to_pi_dev(kref);
+
+	dev_dbg(&pdx->udev->dev, "%s\n", __func__);
+	usb_put_dev(pdx->udev);
+	kfree(pdx);
+}
+
+static int piusb_open(struct inode *inode, struct file *file)
+{
+	struct device_extension *pdx = NULL;
+	struct usb_interface *interface;
+	int subminor;
+	int retval = 0;
+
+	dbg("Piusb_Open()");
+	subminor = iminor(inode);
+	interface = usb_find_interface(&piusb_driver, subminor);
+	if (!interface) {
+		printk(KERN_ERR "%s - error, can't find device for minor %d\n",
+		       __func__, subminor);
+		retval = -ENODEV;
+		goto exit_no_device;
+	}
+
+	pdx = usb_get_intfdata(interface);
+	if (!pdx) {
+		retval = -ENODEV;
+		goto exit_no_device;
+	}
+	dbg("Alternate Setting = %d", interface->num_altsetting);
+
+	pdx->frameIdx = pdx->urbIdx = 0;
+	pdx->gotPixelData = 0;
+	pdx->pendingWrite = 0;
+	pdx->frameSize = 0;
+	pdx->num_frames = 0;
+	pdx->active_frame = 0;
+	pdx->bulk_in_byte_trk = 0;
+	pdx->userBufMapped = 0;
+	pdx->pendedPixelUrbs = NULL;
+	pdx->sgEntries = NULL;
+	pdx->sgl = NULL;
+	pdx->maplist_numPagesMapped = NULL;
+	pdx->PixelUrb = NULL;
+	pdx->bulk_in_size_returned = 0;
+	/* increment our usage count for the device */
+	kref_get(&pdx->kref);
+	/* save our object in the file's private structure */
+	file->private_data = pdx;
+      exit_no_device:
+	return retval;
+}
+
+static int piusb_release(struct inode *inode, struct file *file)
+{
+	struct device_extension *pdx;
+	int retval = 0;
+
+	dbg("Piusb_Release()");
+	pdx = (struct device_extension *)file->private_data;
+	if (pdx == NULL) {
+		dbg("%s - object is NULL", __func__);
+		return -ENODEV;
+	}
+	/* decrement the count on our device */
+	kref_put(&pdx->kref, piusb_delete);
+	return retval;
+}
+
+/**
+ *	piusb_ioctl
+ */
+static int piusb_ioctl(struct inode *inode, struct file *file, unsigned int cmd,
+		       unsigned long arg)
+{
+	struct device_extension *pdx;
+	char dummyCtlBuf[] = { 0, 0, 0, 0, 0, 0, 0, 0 };
+	unsigned long devRB = 0;
+	int i = 0;
+	int err = 0;
+	int retval = 0;
+	struct ioctl_struct ctrl;
+	unsigned char *uBuf;
+	int numbytes = 0;
+	unsigned short controlData = 0;
+
+	pdx = (struct device_extension *)file->private_data;
+	/* verify that the device wasn't unplugged */
+	if (!pdx->present) {
+		dbg("No Device Present\n");
+		return -ENODEV;
+	}
+	/* fill in your device specific stuff here */
+	if (_IOC_DIR(cmd) & _IOC_READ)
+		err = !access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd));
+	else if (_IOC_DIR(cmd) & _IOC_WRITE)
+		err = !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
+	if (err) {
+		dev_err(&pdx->udev->dev, "return with error = %d\n", err);
+		return -EFAULT;
+	}
+	switch (cmd) {
+	case PIUSB_GETVNDCMD:
+		if (copy_from_user
+		    (&ctrl, (void __user *)arg, sizeof(struct ioctl_struct)))
+			info("copy_from_user failed\n");
+		dbg("%s %x\n", "Get Vendor Command = ", ctrl.cmd);
+		retval =
+		    usb_control_msg(pdx->udev, usb_rcvctrlpipe(pdx->udev, 0),
+				    ctrl.cmd, USB_DIR_IN, 0, 0, &devRB,
+				    ctrl.numbytes, HZ * 10);
+		if (ctrl.cmd == 0xF1) {
+			dbg("FW Version returned from HW = %ld.%ld",
+			    (devRB >> 8), (devRB & 0xFF));
+		}
+		return devRB;
+	case PIUSB_SETVNDCMD:
+		if (copy_from_user
+		    (&ctrl, (void __user *)arg, sizeof(struct ioctl_struct)))
+			info("copy_from_user failed\n");
+//            dbg( "%s %x", "Set Vendor Command = ",ctrl.cmd );
+		controlData = ctrl.pData[0];
+		controlData |= (ctrl.pData[1] << 8);
+//            dbg( "%s %d", "Vendor Data =",controlData );
+		retval = usb_control_msg(pdx->udev, usb_sndctrlpipe(pdx->udev, 0), ctrl.cmd, (USB_DIR_OUT | USB_TYPE_VENDOR),	/* | USB_RECIP_ENDPOINT), */
+					 controlData,
+					 0,
+					 &dummyCtlBuf, ctrl.numbytes, HZ * 10);
+		return retval;
+		break;
+	case PIUSB_ISHIGHSPEED:
+		return ((pdx->udev->speed == USB_SPEED_HIGH) ? 1 : 0);
+		break;
+	case PIUSB_WRITEPIPE:
+		if (copy_from_user(&ctrl, (void __user *)arg, _IOC_SIZE(cmd)))
+			info("copy_from_user WRITE_DUMMY failed\n");
+		if (!access_ok(VERIFY_READ, ctrl.pData, ctrl.numbytes)) {
+			dbg("can't access pData");
+			return 0;
+		}
+		piusb_output(&ctrl, ctrl.pData /*uBuf */ , ctrl.numbytes, pdx);
+		return ctrl.numbytes;
+		break;
+	case PIUSB_USERBUFFER:
+		if (copy_from_user
+		    (&ctrl, (void __user *)arg, sizeof(struct ioctl_struct)))
+			info("copy_from_user failed\n");
+		return MapUserBuffer((struct ioctl_struct *) & ctrl, pdx);
+		break;
+	case PIUSB_UNMAP_USERBUFFER:
+		UnMapUserBuffer(pdx);
+		return 0;
+		break;
+	case PIUSB_READPIPE:
+		if (copy_from_user
+		    (&ctrl, (void __user *)arg, sizeof(struct ioctl_struct)))
+			info("copy_from_user failed\n");
+		switch (ctrl.endpoint) {
+		case 0:	//ST133 Pixel Data or PIXIS IO
+			if (pdx->iama == PIXIS_PID) {
+				unsigned int numToRead = 0;
+				unsigned int totalRead = 0;
+				uBuf = kmalloc(ctrl.numbytes, GFP_KERNEL);
+				if (!uBuf) {
+					dbg("Alloc for uBuf failed");
+					return 0;
+				}
+				numbytes = ctrl.numbytes;
+				numToRead = numbytes;
+				dbg("numbytes to read = %d", numbytes);
+				dbg("endpoint # %d", ctrl.endpoint);
+				if (copy_from_user(uBuf, ctrl.pData, numbytes))
+					dbg("copying ctrl.pData to dummyBuf failed");
+				do {
+					i = usb_bulk_msg(pdx->udev, pdx->hEP[ctrl.endpoint], (uBuf + totalRead), (numToRead > 64) ? 64 : numToRead, &numbytes, HZ * 10);	//EP0 can only handle 64 bytes at a time
+					if (i) {
+						dbg("CMD = %s, Address = 0x%02X", ((uBuf[3] == 0x02) ? "WRITE" : "READ"), uBuf[1]);
+						dbg("Number of bytes Attempted to read = %d", (int)ctrl.numbytes);
+						dbg("Blocking ReadI/O Failed with status %d", i);
+						kfree(uBuf);
+						return -1;
+					} else {
+						dbg("Pixis EP0 Read %d bytes",
+						    numbytes);
+						totalRead += numbytes;
+						numToRead -= numbytes;
+					}
+				}
+				while (numToRead);
+				memcpy(ctrl.pData, uBuf, totalRead);
+				dbg("Total Bytes Read from PIXIS EP0 = %d",
+				    totalRead);
+				ctrl.numbytes = totalRead;
+				if (copy_to_user
+				    ((struct ioctl_struct *) arg, &ctrl,
+				     sizeof(struct ioctl_struct)))
+					dbg("copy_to_user failed in IORB");
+				kfree(uBuf);
+				return ctrl.numbytes;
+			} else	//ST133 Pixel Data
+			{
+				if (!pdx->gotPixelData)
+					return 0;
+				else {
+					pdx->gotPixelData = 0;
+					ctrl.numbytes =
+					    pdx->bulk_in_size_returned;
+					pdx->bulk_in_size_returned -=
+					    pdx->frameSize;
+					for (i = 0; i < pdx->maplist_numPagesMapped[pdx->active_frame]; i++)
+						SetPageDirty(pdx->sgl[pdx->active_frame][i].page_link);
+					pdx->active_frame =
+					    ((pdx->active_frame +
+					      1) % pdx->num_frames);
+					return ctrl.numbytes;
+				}
+			}
+			break;
+		case 1:	//ST133IO
+		case 4:	//PIXIS IO
+			uBuf = kmalloc(ctrl.numbytes, GFP_KERNEL);
+			if (!uBuf) {
+				dbg("Alloc for uBuf failed");
+				return 0;
+			}
+			numbytes = ctrl.numbytes;
+//                                      dbg( "numbytes to read = %d", numbytes );
+			if (copy_from_user(uBuf, ctrl.pData, numbytes))
+				dbg("copying ctrl.pData to dummyBuf failed");
+			i = usb_bulk_msg(pdx->udev, pdx->hEP[ctrl.endpoint],
+					 uBuf, numbytes, &numbytes, HZ * 10);
+			if (i) {
+				dbg("Blocking ReadI/O Failed with status %d",
+				    i);
+				kfree(uBuf);
+				return -1;
+			} else {
+				ctrl.numbytes = numbytes;
+				memcpy(ctrl.pData, uBuf, numbytes);
+				if (copy_to_user
+				    ((struct ioctl_struct *) arg, &ctrl,
+				     sizeof(struct ioctl_struct)))
+					dbg("copy_to_user failed in IORB");
+				kfree(uBuf);
+				return ctrl.numbytes;
+			}
+			break;
+
+		case 2:	//PIXIS Ping
+		case 3:	//PIXIS Pong
+			if (!pdx->gotPixelData)
+				return 0;
+			else {
+				pdx->gotPixelData = 0;
+				ctrl.numbytes = pdx->bulk_in_size_returned;
+				pdx->bulk_in_size_returned -= pdx->frameSize;
+				for (i = 0;
+				     i <
+				     pdx->maplist_numPagesMapped[pdx->
+								 active_frame];
+				     i++)
+					SetPageDirty(pdx->sgl[pdx->active_frame][i].page_link);
+				pdx->active_frame =
+				    ((pdx->active_frame + 1) % pdx->num_frames);
+				return ctrl.numbytes;
+			}
+			break;
+		}
+		break;
+	case PIUSB_WHATCAMERA:
+		return pdx->iama;
+	case PIUSB_SETFRAMESIZE:
+		dbg("PIUSB_SETFRAMESIZE");
+		if (copy_from_user
+		    (&ctrl, (void __user *)arg, sizeof(struct ioctl_struct)))
+			info("copy_from_user failed\n");
+		pdx->frameSize = ctrl.numbytes;
+		pdx->num_frames = ctrl.numFrames;
+		if (!pdx->sgl)
+			pdx->sgl =
+			    kmalloc(sizeof(struct scatterlist *) *
+				    pdx->num_frames, GFP_KERNEL);
+		if (!pdx->sgEntries)
+			pdx->sgEntries =
+			    kmalloc(sizeof(unsigned int) * pdx->num_frames,
+				    GFP_KERNEL);
+		if (!pdx->PixelUrb)
+			pdx->PixelUrb =
+			    kmalloc(sizeof(struct urb **) * pdx->num_frames,
+				    GFP_KERNEL);
+		if (!pdx->maplist_numPagesMapped)
+			pdx->maplist_numPagesMapped =
+			    vmalloc(sizeof(unsigned int) * pdx->num_frames);
+		if (!pdx->pendedPixelUrbs)
+			pdx->pendedPixelUrbs =
+			    kmalloc(sizeof(char *) * pdx->num_frames,
+				    GFP_KERNEL);
+		return 0;
+	default:
+		dbg("%s\n", "No IOCTL found");
+		break;
+
+	}
+	/* return that we did not understand this ioctl call */
+	dbg("Returning -ENOTTY");
+	return -ENOTTY;
+}
+
+static void piusb_write_bulk_callback(struct urb *urb)
+{
+	struct device_extension *pdx = urb->context;
+	int status = urb->status;
+
+	/* sync/async unlink faults aren't errors */
+	if (status && !(status == -ENOENT || status == -ECONNRESET))
+		dev_dbg(&urb->dev->dev,
+			"%s - nonzero write bulk status received: %d",
+			__func__, status);
+
+	pdx->pendingWrite = 0;
+	usb_buffer_free(urb->dev, urb->transfer_buffer_length,
+			urb->transfer_buffer, urb->transfer_dma);
+}
+
+int piusb_output(struct ioctl_struct * io, unsigned char *uBuf, int len,
+		 struct device_extension *pdx)
+{
+	struct urb *urb = NULL;
+	int err = 0;
+	unsigned char *kbuf = NULL;
+
+	urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (urb != NULL) {
+		kbuf =
+		    usb_buffer_alloc(pdx->udev, len, GFP_KERNEL,
+				     &urb->transfer_dma);
+		if (!kbuf) {
+			info("buffer_alloc failed\n");
+			return -ENOMEM;
+		}
+		memcpy(kbuf, uBuf, len);
+		usb_fill_bulk_urb(urb, pdx->udev, pdx->hEP[io->endpoint], kbuf,
+				  len, piusb_write_bulk_callback, pdx);
+		urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
+		err = usb_submit_urb(urb, GFP_KERNEL);
+		if (err) {
+			dev_err(&pdx->udev->dev,
+				"WRITE ERROR:submit urb error = %d\n", err);
+		}
+		pdx->pendingWrite = 1;
+		usb_free_urb(urb);
+	}
+	return -EINPROGRESS;
+}
+
+static int UnMapUserBuffer(struct device_extension *pdx)
+{
+	int i = 0;
+	int k = 0;
+	unsigned int epAddr;
+	for (k = 0; k < pdx->num_frames; k++) {
+		dbg("Killing Urbs for Frame %d", k);
+		for (i = 0; i < pdx->sgEntries[k]; i++) {
+			usb_kill_urb(pdx->PixelUrb[k][i]);
+			usb_free_urb(pdx->PixelUrb[k][i]);
+			pdx->pendedPixelUrbs[k][i] = 0;
+		}
+		dbg("Urb error count = %d", errCnt);
+		errCnt = 0;
+		dbg("Urbs free'd and Killed for Frame %d", k);
+	}
+
+	for (k = 0; k < pdx->num_frames; k++) {
+		if (pdx->iama == PIXIS_PID)	//if so, which EP should we map this frame to
+		{
+			if (k % 2)	//check to see if this should use EP4(PONG)
+			{
+				epAddr = pdx->hEP[3];	//PONG, odd frames
+			} else {
+				epAddr = pdx->hEP[2];	//PING, even frames and zero
+			}
+		} else		//ST133 only has 1 endpoint for Pixel data transfer
+		{
+			epAddr = pdx->hEP[0];
+		}
+		usb_buffer_unmap_sg(pdx->udev, epAddr, pdx->sgl[k],
+				    pdx->maplist_numPagesMapped[k]);
+		for (i = 0; i < pdx->maplist_numPagesMapped[k]; i++) {
+			page_cache_release(pdx->sgl[k][i].page_link);
+		}
+		kfree(pdx->sgl[k]);
+		kfree(pdx->PixelUrb[k]);
+		kfree(pdx->pendedPixelUrbs[k]);
+		pdx->sgl[k] = NULL;
+		pdx->PixelUrb[k] = NULL;
+		pdx->pendedPixelUrbs[k] = NULL;
+	}
+	kfree(pdx->sgEntries);
+	vfree(pdx->maplist_numPagesMapped);
+	pdx->sgEntries = NULL;
+	pdx->maplist_numPagesMapped = NULL;
+	kfree(pdx->sgl);
+	kfree(pdx->pendedPixelUrbs);
+	kfree(pdx->PixelUrb);
+	pdx->sgl = NULL;
+	pdx->pendedPixelUrbs = NULL;
+	pdx->PixelUrb = NULL;
+	return 0;
+}
+
+static void piusb_readPIXEL_callback(struct urb *urb)
+{
+	int i = 0;
+	struct device_extension *pdx = urb->context;
+	int status = urb->status;
+
+	if (status && !(status == -ENOENT || status == -ECONNRESET)) {
+		dbg("%s - nonzero read bulk status received: %d", __func__,
+		    status);
+		dbg("Error in read EP2 callback");
+		dbg("FrameIndex = %d", pdx->frameIdx);
+		dbg("Bytes received before problem occurred = %d",
+		    pdx->bulk_in_byte_trk);
+		dbg("Urb Idx = %d", pdx->urbIdx);
+		pdx->pendedPixelUrbs[pdx->frameIdx][pdx->urbIdx] = 0;
+	} else {
+		pdx->bulk_in_byte_trk += urb->actual_length;
+		{
+			i = usb_submit_urb(urb, GFP_ATOMIC);	//resubmit the URB
+			if (i) {
+				errCnt++;
+				if (i != lastErr) {
+					dbg("submit urb in callback failed with error code %d", i);
+					lastErr = i;
+				}
+			} else {
+				pdx->urbIdx++;	//point to next URB when we callback
+				if (pdx->bulk_in_byte_trk >= pdx->frameSize) {
+					pdx->bulk_in_size_returned =
+					    pdx->bulk_in_byte_trk;
+					pdx->bulk_in_byte_trk = 0;
+					pdx->gotPixelData = 1;
+					pdx->frameIdx =
+					    ((pdx->frameIdx +
+					      1) % pdx->num_frames);
+					pdx->urbIdx = 0;
+				}
+			}
+		}
+	}
+}
+
+/* MapUserBuffer(
+	inputs:
+	struct ioctl_struct *io - structure containing user address, frame #, and size
+	struct device_extension *pdx - the PIUSB device extension
+	returns:
+	int - status of the task
+	Notes:
+	MapUserBuffer maps a buffer passed down through an ioctl.  The user buffer is Page Aligned by the app
+	and then passed down.  The function get_free_pages(...) does the actual mapping of the buffer from user space to
+	kernel space.  From there a scatterlist is created from all the pages.  The next function called is to usb_buffer_map_sg
+	which allocated DMA addresses for each page, even coalescing them if possible.  The DMA address is placed in the scatterlist
+	structure.  The function returns the number of DMA addresses.  This may or may not be equal to the number of pages that
+	the user buffer uses.  We then build an URB for each DMA address and then submit them.
+*/
+//int MapUserBuffer( unsigned long uaddr, unsigned long numbytes, unsigned long frameInfo, struct device_extension *pdx )
+static int MapUserBuffer(struct ioctl_struct *io, struct device_extension *pdx)
+{
+	unsigned long uaddr;
+	unsigned long numbytes;
+	int frameInfo;		//which frame we're mapping
+	unsigned int epAddr = 0;
+	unsigned long count = 0;
+	int i = 0;
+	int k = 0;
+	int err = 0;
+	struct page **maplist_p;
+	int numPagesRequired;
+	frameInfo = io->numFrames;
+	uaddr = (unsigned long)io->pData;
+	numbytes = io->numbytes;
+
+	if (pdx->iama == PIXIS_PID)	//if so, which EP should we map this frame to
+	{
+		if (frameInfo % 2)	//check to see if this should use EP4(PONG)
+		{
+			epAddr = pdx->hEP[3];	//PONG, odd frames
+		} else {
+			epAddr = pdx->hEP[2];	//PING, even frames and zero
+		}
+		dbg("Pixis Frame #%d: EP=%d", frameInfo,
+		    (epAddr == pdx->hEP[2]) ? 2 : 4);
+	} else			//ST133 only has 1 endpoint for Pixel data transfer
+	{
+		epAddr = pdx->hEP[0];
+		dbg("ST133 Frame #%d: EP=2", frameInfo);
+	}
+	count = numbytes;
+	dbg("UserAddress = 0x%08lX", uaddr);
+	dbg("numbytes = %d", (int)numbytes);
+	//number of pages to map the entire user space DMA buffer
+	numPagesRequired =
+	    ((uaddr & ~PAGE_MASK) + count + ~PAGE_MASK) >> PAGE_SHIFT;
+	dbg("Number of pages needed = %d", numPagesRequired);
+	maplist_p = vmalloc(numPagesRequired * sizeof(struct page));	//, GFP_ATOMIC);
+	if (!maplist_p) {
+		dbg("Can't Allocate Memory for maplist_p");
+		return -ENOMEM;
+	}
+	//map the user buffer to kernel memory
+	down_write(&current->mm->mmap_sem);
+	pdx->maplist_numPagesMapped[frameInfo] = get_user_pages(current, current->mm, (uaddr & PAGE_MASK), numPagesRequired, WRITE, 0,	//Don't Force
+								maplist_p,
+								NULL);
+	up_write(&current->mm->mmap_sem);
+	dbg("Number of pages mapped = %d",
+	    pdx->maplist_numPagesMapped[frameInfo]);
+	for (i = 0; i < pdx->maplist_numPagesMapped[frameInfo]; i++)
+		flush_dcache_page(maplist_p[i]);
+	if (!pdx->maplist_numPagesMapped[frameInfo]) {
+		dbg("get_user_pages() failed");
+		vfree(maplist_p);
+		return -ENOMEM;
+	}
+	//need to create a scatterlist that spans each frame that can fit into the mapped buffer
+	pdx->sgl[frameInfo] =
+	    kmalloc((pdx->maplist_numPagesMapped[frameInfo] *
+		     sizeof(struct scatterlist)), GFP_ATOMIC);
+	if (!pdx->sgl[frameInfo]) {
+		vfree(maplist_p);
+		dbg("can't allocate mem for sgl");
+		return -ENOMEM;
+	}
+	pdx->sgl[frameInfo][0].page_link = maplist_p[0];
+	pdx->sgl[frameInfo][0].offset = uaddr & ~PAGE_MASK;
+	if (pdx->maplist_numPagesMapped[frameInfo] > 1) {
+		pdx->sgl[frameInfo][0].length =
+		    PAGE_SIZE - pdx->sgl[frameInfo][0].offset;
+		count -= pdx->sgl[frameInfo][0].length;
+		for (k = 1; k < pdx->maplist_numPagesMapped[frameInfo]; k++) {
+			pdx->sgl[frameInfo][k].offset = 0;
+			pdx->sgl[frameInfo][k].page_link = maplist_p[k];
+			pdx->sgl[frameInfo][k].length =
+			    (count < PAGE_SIZE) ? count : PAGE_SIZE;
+			count -= PAGE_SIZE;	//example had PAGE_SIZE here;
+		}
+	} else {
+		pdx->sgl[frameInfo][0].length = count;
+	}
+	pdx->sgEntries[frameInfo] =
+	    usb_buffer_map_sg(pdx->udev, epAddr, pdx->sgl[frameInfo],
+			      pdx->maplist_numPagesMapped[frameInfo]);
+	dbg("number of sgEntries = %d", pdx->sgEntries[frameInfo]);
+	pdx->userBufMapped = 1;
+	vfree(maplist_p);
+	//Create and Send the URB's for each s/g entry
+	pdx->PixelUrb[frameInfo] =
+	    kmalloc(pdx->sgEntries[frameInfo] * sizeof(struct urb *),
+		    GFP_KERNEL);
+	if (!pdx->PixelUrb[frameInfo]) {
+		dbg("Can't Allocate Memory for Urb");
+		return -ENOMEM;
+	}
+	for (i = 0; i < pdx->sgEntries[frameInfo]; i++) {
+		pdx->PixelUrb[frameInfo][i] = usb_alloc_urb(0, GFP_KERNEL);	//0 because we're using BULK transfers
+		usb_fill_bulk_urb(pdx->PixelUrb[frameInfo][i],
+				  pdx->udev,
+				  epAddr,
+				  (dma_addr_t *) sg_dma_address(&pdx->
+								sgl[frameInfo]
+								[i]),
+				  sg_dma_len(&pdx->sgl[frameInfo][i]),
+				  piusb_readPIXEL_callback, (void *)pdx);
+		pdx->PixelUrb[frameInfo][i]->transfer_dma =
+		    sg_dma_address(&pdx->sgl[frameInfo][i]);
+		pdx->PixelUrb[frameInfo][i]->transfer_flags =
+		    URB_NO_TRANSFER_DMA_MAP | URB_NO_INTERRUPT;
+	}
+	pdx->PixelUrb[frameInfo][--i]->transfer_flags &= ~URB_NO_INTERRUPT;	//only interrupt when last URB completes
+	pdx->pendedPixelUrbs[frameInfo] =
+	    kmalloc((pdx->sgEntries[frameInfo] * sizeof(char)), GFP_KERNEL);
+	if (!pdx->pendedPixelUrbs[frameInfo])
+		dbg("Can't allocate Memory for pendedPixelUrbs");
+	for (i = 0; i < pdx->sgEntries[frameInfo]; i++) {
+		err = usb_submit_urb(pdx->PixelUrb[frameInfo][i], GFP_ATOMIC);
+		if (err) {
+			dbg("%s %d\n", "submit urb error =", err);
+			pdx->pendedPixelUrbs[frameInfo][i] = 0;
+			return err;
+		} else
+			pdx->pendedPixelUrbs[frameInfo][i] = 1;;
+	}
+	return 0;
+}
+
+static struct file_operations piusb_fops = {
+	.owner = THIS_MODULE,
+	.ioctl = piusb_ioctl,
+	.open = piusb_open,
+	.release = piusb_release,
+};
+
+static struct usb_class_driver piusb_class = {
+	.name = "usb/rspiusb%d",
+	.fops = &piusb_fops,
+	.minor_base = PIUSB_MINOR_BASE,
+};
+
+/**
+ *	piusb_probe
+ *
+ *	Called by the usb core when a new device is connected that it thinks
+ *	this driver might be interested in.
+ */
+static int piusb_probe(struct usb_interface *interface,
+		       const struct usb_device_id *id)
+{
+	struct device_extension *pdx = NULL;
+	struct usb_host_interface *iface_desc;
+	struct usb_endpoint_descriptor *endpoint;
+	int i;
+	int retval = -ENOMEM;
+
+	dev_dbg(&interface->dev, "%s - Looking for PI USB Hardware", __func__);
+
+	pdx = kzalloc(sizeof(struct device_extension), GFP_KERNEL);
+	if (pdx == NULL) {
+		dev_err(&interface->dev, "Out of memory\n");
+		goto error;
+	}
+	kref_init(&pdx->kref);
+	pdx->udev = usb_get_dev(interface_to_usbdev(interface));
+	pdx->interface = interface;
+	iface_desc = interface->cur_altsetting;
+
+	/* See if the device offered us matches what we can accept */
+	if ((pdx->udev->descriptor.idVendor != VENDOR_ID)
+	    || ((pdx->udev->descriptor.idProduct != PIXIS_PID)
+		&& (pdx->udev->descriptor.idProduct != ST133_PID))) {
+		return -ENODEV;
+	}
+	pdx->iama = pdx->udev->descriptor.idProduct;
+
+	if (debug) {
+		if (pdx->udev->descriptor.idProduct == PIXIS_PID)
+			dbg("PIUSB:Pixis Camera Found");
+		else
+			dbg("PIUSB:ST133 USB Controller Found");
+		if (pdx->udev->speed == USB_SPEED_HIGH)
+			dbg("Highspeed(USB2.0) Device Attached");
+		else
+			dbg("Lowspeed (USB1.1) Device Attached");
+
+		dbg("NumEndpoints in Configuration: %d",
+		    iface_desc->desc.bNumEndpoints);
+	}
+	for (i = 0; i < iface_desc->desc.bNumEndpoints; ++i) {
+		endpoint = &iface_desc->endpoint[i].desc;
+		if (debug) {
+			dbg("Endpoint[%d]->bDescriptorType = %d", i,
+			    endpoint->bDescriptorType);
+			dbg("Endpoint[%d]->bEndpointAddress = 0x%02X", i,
+			    endpoint->bEndpointAddress);
+			dbg("Endpoint[%d]->bbmAttributes = %d", i,
+			    endpoint->bmAttributes);
+			dbg("Endpoint[%d]->MaxPacketSize = %d\n", i,
+			    endpoint->wMaxPacketSize);
+		}
+		if ((endpoint->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK) ==
+		    USB_ENDPOINT_XFER_BULK) {
+			if (endpoint->bEndpointAddress & USB_DIR_IN)
+				pdx->hEP[i] =
+				    usb_rcvbulkpipe(pdx->udev,
+						    endpoint->bEndpointAddress);
+			else
+				pdx->hEP[i] =
+				    usb_sndbulkpipe(pdx->udev,
+						    endpoint->bEndpointAddress);
+		}
+	}
+	usb_set_intfdata(interface, pdx);
+	retval = usb_register_dev(interface, &piusb_class);
+	if (retval) {
+		err("Not able to get a minor for this device.");
+		usb_set_intfdata(interface, NULL);
+		goto error;
+	}
+	pdx->present = 1;
+
+	/* we can register the device now, as it is ready */
+	pdx->minor = interface->minor;
+	/* let the user know what node this device is now attached to */
+	dbg("PI USB2.0 device now attached to piusb-%d", pdx->minor);
+	return 0;
+
+      error:
+	if (pdx)
+		kref_put(&pdx->kref, piusb_delete);
+	return retval;
+}
+
+/**
+ *	piusb_disconnect
+ *
+ *	Called by the usb core when the device is removed from the system.
+ *
+ *	This routine guarantees that the driver will not submit any more urbs
+ *	by clearing pdx->udev.  It is also supposed to terminate any currently
+ *	active urbs.  Unfortunately, usb_bulk_msg(), used in piusb_read(), does
+ *	not provide any way to do this.  But at least we can cancel an active
+ *	write.
+ */
+static void piusb_disconnect(struct usb_interface *interface)
+{
+	struct device_extension *pdx;
+	int minor = interface->minor;
+	lock_kernel();
+	pdx = usb_get_intfdata(interface);
+	usb_set_intfdata(interface, NULL);
+	/* give back our minor */
+	usb_deregister_dev(interface, &piusb_class);
+	unlock_kernel();
+	/* prevent device read, write and ioctl */
+	pdx->present = 0;
+	kref_put(&pdx->kref, piusb_delete);
+	dbg("PI USB2.0 device #%d now disconnected\n", minor);
+}
+
+static struct usb_driver piusb_driver = {
+	.name = "sub",
+	.probe = piusb_probe,
+	.disconnect = piusb_disconnect,
+	.id_table = pi_device_table,
+};
+
+/**
+ *	piusb_init
+ */
+static int __init piusb_init(void)
+{
+	int result;
+	/* register this driver with the USB subsystem */
+	result = usb_register(&piusb_driver);
+	if (result) {
+		printk(KERN_ERR KBUILD_MODNAME
+		       ": usb_register failed. Error number %d\n", result);
+		return result;
+	}
+	printk(KERN_INFO KBUILD_MODNAME ":%s: %s\n", DRIVER_DESC,
+	       DRIVER_VERSION);
+	return 0;
+}
+
+/**
+ *	piusb_exit
+ */
+static void __exit piusb_exit(void)
+{
+	/* deregister this driver with the USB subsystem */
+	usb_deregister(&piusb_driver);
+}
+
+module_init(piusb_init);
+module_exit(piusb_exit);
+
+/* Module parameters */
+module_param(debug, int, 0);
+MODULE_PARM_DESC(debug, "Debug enabled or not");
+
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/rspiusb/rspiusb.h b/drivers/staging/rspiusb/rspiusb.h
new file mode 100644
index 00000000000..965cd2d8c19
--- /dev/null
+++ b/drivers/staging/rspiusb/rspiusb.h
@@ -0,0 +1,25 @@
+#ifndef __RSPIUSB_H
+#define __RSPIUSB_H
+
+#define PIUSB_MAGIC		'm'
+#define PIUSB_IOCTL_BASE	192
+#define PIUSB_GETVNDCMD		_IOR(PIUSB_MAGIC, PIUSB_IOCTL_BASE + 1, struct ioctl_struct)
+#define PIUSB_SETVNDCMD		_IOW(PIUSB_MAGIC, PIUSB_IOCTL_BASE + 2, struct ioctl_struct)
+#define PIUSB_WRITEPIPE		_IOW(PIUSB_MAGIC, PIUSB_IOCTL_BASE + 3, struct ioctl_struct)
+#define PIUSB_READPIPE		_IOR(PIUSB_MAGIC, PIUSB_IOCTL_BASE + 4, struct ioctl_struct)
+#define PIUSB_SETFRAMESIZE	_IOW(PIUSB_MAGIC, PIUSB_IOCTL_BASE + 5, struct ioctl_struct)
+#define PIUSB_WHATCAMERA	_IO(PIUSB_MAGIC,  PIUSB_IOCTL_BASE + 6)
+#define PIUSB_USERBUFFER	_IOW(PIUSB_MAGIC, PIUSB_IOCTL_BASE + 7, struct ioctl_struct)
+#define PIUSB_ISHIGHSPEED	_IO(PIUSB_MAGIC,  PIUSB_IOCTL_BASE + 8)
+#define PIUSB_UNMAP_USERBUFFER	_IOW(PIUSB_MAGIC, PIUSB_IOCTL_BASE + 9, struct ioctl_struct)
+
+struct ioctl_struct {
+	unsigned char cmd;
+	unsigned long numbytes;
+	unsigned char dir;	//1=out;0=in
+	int endpoint;
+	int numFrames;
+	unsigned char *pData;
+};
+
+#endif
diff --git a/drivers/staging/rt2860/2860_main_dev.c b/drivers/staging/rt2860/2860_main_dev.c
new file mode 100644
index 00000000000..1e38f2d1f69
--- /dev/null
+++ b/drivers/staging/rt2860/2860_main_dev.c
@@ -0,0 +1,1377 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    2870_main_dev.c
+
+    Abstract:
+    Create and register network interface.
+
+    Revision History:
+    Who         When            What
+    --------    ----------      ----------------------------------------------
+*/
+
+#include "rt_config.h"
+
+
+#ifdef MULTIPLE_CARD_SUPPORT
+// record whether the card in the card list is used in the card file
+extern UINT8  MC_CardUsed[];
+#endif // MULTIPLE_CARD_SUPPORT //
+
+
+extern INT __devinit rt28xx_probe(IN void *_dev_p, IN void *_dev_id_p,
+									IN UINT argc, OUT PRTMP_ADAPTER *ppAd);
+
+static void rx_done_tasklet(unsigned long data);
+static void mgmt_dma_done_tasklet(unsigned long data);
+static void ac0_dma_done_tasklet(unsigned long data);
+static void ac1_dma_done_tasklet(unsigned long data);
+static void ac2_dma_done_tasklet(unsigned long data);
+static void ac3_dma_done_tasklet(unsigned long data);
+static void hcca_dma_done_tasklet(unsigned long data);
+static void fifo_statistic_full_tasklet(unsigned long data);
+
+
+/*---------------------------------------------------------------------*/
+/* Symbol & Macro Definitions                                          */
+/*---------------------------------------------------------------------*/
+#define RT2860_INT_RX_DLY				(1<<0)		// bit 0
+#define RT2860_INT_TX_DLY				(1<<1)		// bit 1
+#define RT2860_INT_RX_DONE				(1<<2)		// bit 2
+#define RT2860_INT_AC0_DMA_DONE			(1<<3)		// bit 3
+#define RT2860_INT_AC1_DMA_DONE			(1<<4)		// bit 4
+#define RT2860_INT_AC2_DMA_DONE			(1<<5)		// bit 5
+#define RT2860_INT_AC3_DMA_DONE			(1<<6)		// bit 6
+#define RT2860_INT_HCCA_DMA_DONE		(1<<7)		// bit 7
+#define RT2860_INT_MGMT_DONE			(1<<8)		// bit 8
+
+#define INT_RX			RT2860_INT_RX_DONE
+
+#define INT_AC0_DLY		(RT2860_INT_AC0_DMA_DONE) //| RT2860_INT_TX_DLY)
+#define INT_AC1_DLY		(RT2860_INT_AC1_DMA_DONE) //| RT2860_INT_TX_DLY)
+#define INT_AC2_DLY		(RT2860_INT_AC2_DMA_DONE) //| RT2860_INT_TX_DLY)
+#define INT_AC3_DLY		(RT2860_INT_AC3_DMA_DONE) //| RT2860_INT_TX_DLY)
+#define INT_HCCA_DLY 	(RT2860_INT_HCCA_DMA_DONE) //| RT2860_INT_TX_DLY)
+#define INT_MGMT_DLY	RT2860_INT_MGMT_DONE
+
+/*---------------------------------------------------------------------*/
+/* Prototypes of Functions Used                                        */
+/*---------------------------------------------------------------------*/
+/* function declarations */
+static INT __devinit rt2860_init_one (struct pci_dev *pci_dev, const struct pci_device_id  *ent);
+static VOID __devexit rt2860_remove_one(struct pci_dev *pci_dev);
+static INT __devinit rt2860_probe(struct pci_dev *pci_dev, const struct pci_device_id  *ent);
+void init_thread_task(PRTMP_ADAPTER pAd);
+static void __exit rt2860_cleanup_module(void);
+static int __init rt2860_init_module(void);
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+#ifdef CONFIG_PM
+static int rt2860_suspend(struct pci_dev *pci_dev, pm_message_t state);
+static int rt2860_resume(struct pci_dev *pci_dev);
+#endif // CONFIG_PM //
+#endif
+
+
+//
+// Ralink PCI device table, include all supported chipsets
+//
+static struct pci_device_id rt2860_pci_tbl[] __devinitdata =
+{
+	{PCI_DEVICE(NIC_PCI_VENDOR_ID, NIC2860_PCI_DEVICE_ID)},		//RT28602.4G
+	{PCI_DEVICE(NIC_PCI_VENDOR_ID, NIC2860_PCIe_DEVICE_ID)},
+	{PCI_DEVICE(NIC_PCI_VENDOR_ID, NIC2760_PCI_DEVICE_ID)},
+	{PCI_DEVICE(NIC_PCI_VENDOR_ID, NIC2790_PCIe_DEVICE_ID)},
+	{PCI_DEVICE(VEN_AWT_PCI_VENDOR_ID, VEN_AWT_PCIe_DEVICE_ID)},
+    {0,}		// terminate list
+};
+
+MODULE_DEVICE_TABLE(pci, rt2860_pci_tbl);
+#ifdef CONFIG_STA_SUPPORT
+MODULE_LICENSE("GPL");
+#ifdef MODULE_VERSION
+MODULE_VERSION(STA_DRIVER_VERSION);
+#endif
+#endif // CONFIG_STA_SUPPORT //
+
+
+//
+// Our PCI driver structure
+//
+static struct pci_driver rt2860_driver =
+{
+    name:       "rt2860",
+    id_table:   rt2860_pci_tbl,
+    probe:      rt2860_init_one,
+#if LINUX_VERSION_CODE >= 0x20412
+    remove:     __devexit_p(rt2860_remove_one),
+#else
+    remove:     __devexit(rt2860_remove_one),
+#endif
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+#ifdef CONFIG_PM
+	suspend:	rt2860_suspend,
+	resume:		rt2860_resume,
+#endif
+#endif
+};
+
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+#ifdef CONFIG_PM
+
+VOID RT2860RejectPendingPackets(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	// clear PS packets
+	// clear TxSw packets
+}
+
+static int rt2860_suspend(
+	struct pci_dev *pci_dev,
+	pm_message_t state)
+{
+	struct net_device *net_dev = pci_get_drvdata(pci_dev);
+	PRTMP_ADAPTER pAd = (PRTMP_ADAPTER)NULL;
+	INT32 retval;
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("===> rt2860_suspend()\n"));
+
+	if (net_dev == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("net_dev == NULL!\n"));
+	}
+	else
+	{
+		pAd = net_dev->ml_priv;
+
+		/* we can not use IFF_UP because ra0 down but ra1 up */
+		/* and 1 suspend/resume function for 1 module, not for each interface */
+		/* so Linux will call suspend/resume function once */
+		if (VIRTUAL_IF_NUM(pAd) > 0)
+		{
+			// avoid users do suspend after interface is down
+
+			// stop interface
+			netif_carrier_off(net_dev);
+			netif_stop_queue(net_dev);
+
+			// mark device as removed from system and therefore no longer available
+			netif_device_detach(net_dev);
+
+			// mark halt flag
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS);
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF);
+
+			// take down the device
+			rt28xx_close((PNET_DEV)net_dev);
+
+			RT_MOD_DEC_USE_COUNT();
+		}
+	}
+
+	// reference to http://vovo2000.com/type-lab/linux/kernel-api/linux-kernel-api.html
+	// enable device to generate PME# when suspended
+	// pci_choose_state(): Choose the power state of a PCI device to be suspended
+	retval = pci_enable_wake(pci_dev, pci_choose_state(pci_dev, state), 1);
+	// save the PCI configuration space of a device before suspending
+	pci_save_state(pci_dev);
+	// disable PCI device after use
+	pci_disable_device(pci_dev);
+
+	retval = pci_set_power_state(pci_dev, pci_choose_state(pci_dev, state));
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<=== rt2860_suspend()\n"));
+	return retval;
+}
+
+static int rt2860_resume(
+	struct pci_dev *pci_dev)
+{
+	struct net_device *net_dev = pci_get_drvdata(pci_dev);
+	PRTMP_ADAPTER pAd = (PRTMP_ADAPTER)NULL;
+	INT32 retval;
+
+
+	// set the power state of a PCI device
+	// PCI has 4 power states, DO (normal) ~ D3(less power)
+	// in include/linux/pci.h, you can find that
+	// #define PCI_D0          ((pci_power_t __force) 0)
+	// #define PCI_D1          ((pci_power_t __force) 1)
+	// #define PCI_D2          ((pci_power_t __force) 2)
+	// #define PCI_D3hot       ((pci_power_t __force) 3)
+	// #define PCI_D3cold      ((pci_power_t __force) 4)
+	// #define PCI_UNKNOWN     ((pci_power_t __force) 5)
+	// #define PCI_POWER_ERROR ((pci_power_t __force) -1)
+	retval = pci_set_power_state(pci_dev, PCI_D0);
+
+	// restore the saved state of a PCI device
+	pci_restore_state(pci_dev);
+
+	// initialize device before it's used by a driver
+	if (pci_enable_device(pci_dev))
+	{
+		printk("pci enable fail!\n");
+		return 0;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("===> rt2860_resume()\n"));
+
+	if (net_dev == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("net_dev == NULL!\n"));
+	}
+	else
+		pAd = net_dev->ml_priv;
+
+	if (pAd != NULL)
+	{
+		/* we can not use IFF_UP because ra0 down but ra1 up */
+		/* and 1 suspend/resume function for 1 module, not for each interface */
+		/* so Linux will call suspend/resume function once */
+		if (VIRTUAL_IF_NUM(pAd) > 0)
+		{
+			// mark device as attached from system and restart if needed
+			netif_device_attach(net_dev);
+
+			if (rt28xx_open((PNET_DEV)net_dev) != 0)
+			{
+				// open fail
+				DBGPRINT(RT_DEBUG_TRACE, ("<=== rt2860_resume()\n"));
+				return 0;
+			}
+
+			// increase MODULE use count
+			RT_MOD_INC_USE_COUNT();
+
+			RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS);
+			RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF);
+
+			netif_start_queue(net_dev);
+			netif_carrier_on(net_dev);
+			netif_wake_queue(net_dev);
+		}
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<=== rt2860_resume()\n"));
+	return 0;
+}
+#endif // CONFIG_PM //
+#endif
+
+
+static INT __init rt2860_init_module(VOID)
+{
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+	return pci_register_driver(&rt2860_driver);
+#else
+    return pci_module_init(&rt2860_driver);
+#endif
+}
+
+
+//
+// Driver module unload function
+//
+static VOID __exit rt2860_cleanup_module(VOID)
+{
+    pci_unregister_driver(&rt2860_driver);
+}
+
+module_init(rt2860_init_module);
+module_exit(rt2860_cleanup_module);
+
+
+static INT __devinit rt2860_init_one (
+    IN  struct pci_dev              *pci_dev,
+    IN  const struct pci_device_id  *ent)
+{
+    INT rc;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("===> rt2860_init_one\n"));
+
+    // wake up and enable device
+    if (pci_enable_device (pci_dev))
+    {
+        rc = -EIO;
+    }
+    else
+    {
+        rc = rt2860_probe(pci_dev, ent);
+    }
+
+    DBGPRINT(RT_DEBUG_TRACE, ("<=== rt2860_init_one\n"));
+    return rc;
+}
+
+
+static VOID __devexit rt2860_remove_one(
+    IN  struct pci_dev  *pci_dev)
+{
+    struct net_device   *net_dev = pci_get_drvdata(pci_dev);
+    RTMP_ADAPTER        *pAd = net_dev->ml_priv;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("===> rt2860_remove_one\n"));
+
+	if (pAd != NULL)
+	{
+#ifdef MULTIPLE_CARD_SUPPORT
+		if ((pAd->MC_RowID >= 0) && (pAd->MC_RowID <= MAX_NUM_OF_MULTIPLE_CARD))
+			MC_CardUsed[pAd->MC_RowID] = 0; // not clear MAC address
+#endif // MULTIPLE_CARD_SUPPORT //
+
+
+
+
+		// Unregister network device
+		unregister_netdev(net_dev);
+
+		// Unmap CSR base address
+		iounmap((char *)(net_dev->base_addr));
+
+		RTMPFreeAdapter(pAd);
+
+		// release memory region
+		release_mem_region(pci_resource_start(pci_dev, 0), pci_resource_len(pci_dev, 0));
+	}
+	else
+	{
+		// Unregister network device
+		unregister_netdev(net_dev);
+
+		// Unmap CSR base address
+		iounmap((char *)(net_dev->base_addr));
+
+		// release memory region
+		release_mem_region(pci_resource_start(pci_dev, 0), pci_resource_len(pci_dev, 0));
+	}
+
+	// Free pre-allocated net_device memory
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+	free_netdev(net_dev);
+#else
+	kfree(net_dev);
+#endif
+}
+
+//
+// PCI device probe & initialization function
+//
+static INT __devinit   rt2860_probe(
+    IN  struct pci_dev              *pci_dev,
+    IN  const struct pci_device_id  *ent)
+{
+	PRTMP_ADAPTER pAd;
+    INT rv = 0;
+
+    rv = (INT)rt28xx_probe((void *)pci_dev, (void *)ent, 0, &pAd);
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE);
+	return rv;
+}
+
+
+void init_thread_task(IN PRTMP_ADAPTER pAd)
+{
+	POS_COOKIE pObj;
+
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	tasklet_init(&pObj->rx_done_task, rx_done_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->mgmt_dma_done_task, mgmt_dma_done_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->ac0_dma_done_task, ac0_dma_done_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->ac1_dma_done_task, ac1_dma_done_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->ac2_dma_done_task, ac2_dma_done_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->ac3_dma_done_task, ac3_dma_done_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->hcca_dma_done_task, hcca_dma_done_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->tbtt_task, tbtt_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->fifo_statistic_full_task, fifo_statistic_full_tasklet, (unsigned long)pAd);
+}
+
+void kill_thread_task(IN PRTMP_ADAPTER pAd)
+{
+	POS_COOKIE pObj;
+
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	tasklet_kill(&pObj->rx_done_task);
+	tasklet_kill(&pObj->mgmt_dma_done_task);
+	tasklet_kill(&pObj->ac0_dma_done_task);
+	tasklet_kill(&pObj->ac1_dma_done_task);
+	tasklet_kill(&pObj->ac2_dma_done_task);
+	tasklet_kill(&pObj->ac3_dma_done_task);
+	tasklet_kill(&pObj->hcca_dma_done_task);
+	tasklet_kill(&pObj->tbtt_task);
+	tasklet_kill(&pObj->fifo_statistic_full_task);
+}
+
+
+static void rt2860_int_enable(PRTMP_ADAPTER pAd, unsigned int mode)
+{
+	u32 regValue;
+
+	pAd->int_disable_mask &= ~(mode);
+	regValue = pAd->int_enable_reg & ~(pAd->int_disable_mask);
+	RTMP_IO_WRITE32(pAd, INT_MASK_CSR, regValue);     // 1:enable
+
+	if (regValue != 0)
+		RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_ACTIVE);
+}
+
+
+static void rt2860_int_disable(PRTMP_ADAPTER pAd, unsigned int mode)
+{
+	u32 regValue;
+
+	pAd->int_disable_mask |= mode;
+	regValue = 	pAd->int_enable_reg & ~(pAd->int_disable_mask);
+	RTMP_IO_WRITE32(pAd, INT_MASK_CSR, regValue);     // 0: disable
+
+	if (regValue == 0)
+	{
+		RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_ACTIVE);
+	}
+}
+
+static void mgmt_dma_done_tasklet(unsigned long data)
+{
+	unsigned long flags;
+	PRTMP_ADAPTER pAd = (PRTMP_ADAPTER) data;
+    INT_SOURCE_CSR_STRUC	IntSource;
+	POS_COOKIE pObj;
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return;
+
+    pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	IntSource.word = 0;
+	IntSource.field.MgmtDmaDone = 1;
+	pAd->int_pending &= ~INT_MGMT_DLY;
+
+	RTMPHandleMgmtRingDmaDoneInterrupt(pAd);
+
+	// if you use RTMP_SEM_LOCK, sometimes kernel will hang up, no any
+	// bug report output
+	RTMP_INT_LOCK(&pAd->irq_lock, flags);
+	/*
+	 * double check to avoid lose of interrupts
+	 */
+	if (pAd->int_pending & INT_MGMT_DLY)
+	{
+		tasklet_hi_schedule(&pObj->mgmt_dma_done_task);
+		RTMP_INT_UNLOCK(&pAd->irq_lock, flags);
+		return;
+	}
+
+	/* enable TxDataInt again */
+	rt2860_int_enable(pAd, INT_MGMT_DLY);
+	RTMP_INT_UNLOCK(&pAd->irq_lock, flags);
+}
+
+static void rx_done_tasklet(unsigned long data)
+{
+	unsigned long flags;
+	PRTMP_ADAPTER pAd = (PRTMP_ADAPTER) data;
+	BOOLEAN	bReschedule = 0;
+	POS_COOKIE pObj;
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return;
+
+    pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	pAd->int_pending &= ~(INT_RX);
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		bReschedule = STARxDoneInterruptHandle(pAd, 0);
+#endif // CONFIG_STA_SUPPORT //
+
+	RTMP_INT_LOCK(&pAd->irq_lock, flags);
+	/*
+	 * double check to avoid rotting packet
+	 */
+	if (pAd->int_pending & INT_RX || bReschedule)
+	{
+		tasklet_hi_schedule(&pObj->rx_done_task);
+		RTMP_INT_UNLOCK(&pAd->irq_lock, flags);
+		return;
+	}
+
+	/* enable RxINT again */
+	rt2860_int_enable(pAd, INT_RX);
+	RTMP_INT_UNLOCK(&pAd->irq_lock, flags);
+
+}
+
+void fifo_statistic_full_tasklet(unsigned long data)
+{
+	unsigned long flags;
+	PRTMP_ADAPTER pAd = (PRTMP_ADAPTER) data;
+	POS_COOKIE pObj;
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return;
+
+    pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	pAd->int_pending &= ~(FifoStaFullInt);
+	NICUpdateFifoStaCounters(pAd);
+
+	RTMP_INT_LOCK(&pAd->irq_lock, flags);
+	/*
+	 * double check to avoid rotting packet
+	 */
+	if (pAd->int_pending & FifoStaFullInt)
+	{
+		tasklet_hi_schedule(&pObj->fifo_statistic_full_task);
+		RTMP_INT_UNLOCK(&pAd->irq_lock, flags);
+		return;
+	}
+
+	/* enable RxINT again */
+
+	rt2860_int_enable(pAd, FifoStaFullInt);
+	RTMP_INT_UNLOCK(&pAd->irq_lock, flags);
+
+}
+
+static void hcca_dma_done_tasklet(unsigned long data)
+{
+	unsigned long flags;
+	PRTMP_ADAPTER pAd = (PRTMP_ADAPTER) data;
+    INT_SOURCE_CSR_STRUC	IntSource;
+	POS_COOKIE pObj;
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return;
+
+    pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+
+	IntSource.word = 0;
+	IntSource.field.HccaDmaDone = 1;
+	pAd->int_pending &= ~INT_HCCA_DLY;
+
+	RTMPHandleTxRingDmaDoneInterrupt(pAd, IntSource);
+
+	RTMP_INT_LOCK(&pAd->irq_lock, flags);
+	/*
+	 * double check to avoid lose of interrupts
+	 */
+	if (pAd->int_pending & INT_HCCA_DLY)
+	{
+		tasklet_hi_schedule(&pObj->hcca_dma_done_task);
+		RTMP_INT_UNLOCK(&pAd->irq_lock, flags);
+		return;
+	}
+
+	/* enable TxDataInt again */
+	rt2860_int_enable(pAd, INT_HCCA_DLY);
+	RTMP_INT_UNLOCK(&pAd->irq_lock, flags);
+}
+
+static void ac3_dma_done_tasklet(unsigned long data)
+{
+	unsigned long flags;
+	PRTMP_ADAPTER pAd = (PRTMP_ADAPTER) data;
+    INT_SOURCE_CSR_STRUC	IntSource;
+	POS_COOKIE pObj;
+	BOOLEAN bReschedule = 0;
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return;
+
+    pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	IntSource.word = 0;
+	IntSource.field.Ac3DmaDone = 1;
+	pAd->int_pending &= ~INT_AC3_DLY;
+
+	bReschedule = RTMPHandleTxRingDmaDoneInterrupt(pAd, IntSource);
+
+	RTMP_INT_LOCK(&pAd->irq_lock, flags);
+	/*
+	 * double check to avoid lose of interrupts
+	 */
+	if ((pAd->int_pending & INT_AC3_DLY) || bReschedule)
+	{
+		tasklet_hi_schedule(&pObj->ac3_dma_done_task);
+		RTMP_INT_UNLOCK(&pAd->irq_lock, flags);
+		return;
+	}
+
+	/* enable TxDataInt again */
+	rt2860_int_enable(pAd, INT_AC3_DLY);
+	RTMP_INT_UNLOCK(&pAd->irq_lock, flags);
+}
+
+static void ac2_dma_done_tasklet(unsigned long data)
+{
+	unsigned long flags;
+	PRTMP_ADAPTER pAd = (PRTMP_ADAPTER) data;
+    INT_SOURCE_CSR_STRUC	IntSource;
+	POS_COOKIE pObj;
+	BOOLEAN bReschedule = 0;
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return;
+
+    pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	IntSource.word = 0;
+	IntSource.field.Ac2DmaDone = 1;
+	pAd->int_pending &= ~INT_AC2_DLY;
+
+	bReschedule = RTMPHandleTxRingDmaDoneInterrupt(pAd, IntSource);
+
+	RTMP_INT_LOCK(&pAd->irq_lock, flags);
+
+	/*
+	 * double check to avoid lose of interrupts
+	 */
+	if ((pAd->int_pending & INT_AC2_DLY) || bReschedule)
+	{
+		tasklet_hi_schedule(&pObj->ac2_dma_done_task);
+		RTMP_INT_UNLOCK(&pAd->irq_lock, flags);
+		return;
+	}
+
+	/* enable TxDataInt again */
+	rt2860_int_enable(pAd, INT_AC2_DLY);
+	RTMP_INT_UNLOCK(&pAd->irq_lock, flags);
+}
+
+static void ac1_dma_done_tasklet(unsigned long data)
+{
+	unsigned long flags;
+	PRTMP_ADAPTER pAd = (PRTMP_ADAPTER) data;
+    INT_SOURCE_CSR_STRUC	IntSource;
+	POS_COOKIE pObj;
+	BOOLEAN bReschedule = 0;
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return;
+
+    pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	IntSource.word = 0;
+	IntSource.field.Ac1DmaDone = 1;
+	pAd->int_pending &= ~INT_AC1_DLY;
+
+	bReschedule = RTMPHandleTxRingDmaDoneInterrupt(pAd, IntSource);
+
+	RTMP_INT_LOCK(&pAd->irq_lock, flags);
+	/*
+	 * double check to avoid lose of interrupts
+	 */
+	if ((pAd->int_pending & INT_AC1_DLY) || bReschedule)
+	{
+		tasklet_hi_schedule(&pObj->ac1_dma_done_task);
+		RTMP_INT_UNLOCK(&pAd->irq_lock, flags);
+		return;
+	}
+
+	/* enable TxDataInt again */
+	rt2860_int_enable(pAd, INT_AC1_DLY);
+	RTMP_INT_UNLOCK(&pAd->irq_lock, flags);
+}
+
+static void ac0_dma_done_tasklet(unsigned long data)
+{
+	unsigned long flags;
+	PRTMP_ADAPTER pAd = (PRTMP_ADAPTER) data;
+    INT_SOURCE_CSR_STRUC	IntSource;
+	POS_COOKIE pObj;
+	BOOLEAN bReschedule = 0;
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return;
+
+    pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	IntSource.word = 0;
+	IntSource.field.Ac0DmaDone = 1;
+	pAd->int_pending &= ~INT_AC0_DLY;
+
+	bReschedule = RTMPHandleTxRingDmaDoneInterrupt(pAd, IntSource);
+
+	RTMP_INT_LOCK(&pAd->irq_lock, flags);
+	/*
+	 * double check to avoid lose of interrupts
+	 */
+	if ((pAd->int_pending & INT_AC0_DLY) || bReschedule)
+	{
+		tasklet_hi_schedule(&pObj->ac0_dma_done_task);
+		RTMP_INT_UNLOCK(&pAd->irq_lock, flags);
+		return;
+	}
+
+	/* enable TxDataInt again */
+	rt2860_int_enable(pAd, INT_AC0_DLY);
+	RTMP_INT_UNLOCK(&pAd->irq_lock, flags);
+}
+
+
+int print_int_count;
+
+IRQ_HANDLE_TYPE
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,19))
+rt2860_interrupt(int irq, void *dev_instance)
+#else
+rt2860_interrupt(int irq, void *dev_instance, struct pt_regs *regs)
+#endif
+{
+	struct net_device *net_dev = (struct net_device *) dev_instance;
+	PRTMP_ADAPTER pAd = net_dev->ml_priv;
+	INT_SOURCE_CSR_STRUC	IntSource;
+	POS_COOKIE pObj;
+
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+
+	/* Note 03312008: we can not return here before
+		RTMP_IO_READ32(pAd, INT_SOURCE_CSR, &IntSource.word);
+		RTMP_IO_WRITE32(pAd, INT_SOURCE_CSR, IntSource.word);
+		Or kernel will panic after ifconfig ra0 down sometimes */
+
+
+	//
+	// Inital the Interrupt source.
+	//
+	IntSource.word = 0x00000000L;
+//	McuIntSource.word = 0x00000000L;
+
+	//
+	// Get the interrupt sources & saved to local variable
+	//
+	//RTMP_IO_READ32(pAd, where, &McuIntSource.word);
+	//RTMP_IO_WRITE32(pAd, , McuIntSource.word);
+
+	//
+	// Flag fOP_STATUS_DOZE On, means ASIC put to sleep, elase means ASICK WakeUp
+	// And at the same time, clock maybe turned off that say there is no DMA service.
+	// when ASIC get to sleep.
+	// To prevent system hang on power saving.
+	// We need to check it before handle the INT_SOURCE_CSR, ASIC must be wake up.
+	//
+	// RT2661 => when ASIC is sleeping, MAC register cannot be read and written.
+	// RT2860 => when ASIC is sleeping, MAC register can be read and written.
+
+	{
+		RTMP_IO_READ32(pAd, INT_SOURCE_CSR, &IntSource.word);
+		RTMP_IO_WRITE32(pAd, INT_SOURCE_CSR, IntSource.word); // write 1 to clear
+	}
+
+	// Do nothing if Reset in progress
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS) ||
+		RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS))
+	{
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+        return  IRQ_HANDLED;
+#else
+        return;
+#endif
+	}
+
+	//
+	// Handle interrupt, walk through all bits
+	// Should start from highest priority interrupt
+	// The priority can be adjust by altering processing if statement
+	//
+
+    pAd->bPCIclkOff = FALSE;
+
+	// If required spinlock, each interrupt service routine has to acquire
+	// and release itself.
+	//
+
+	// Do nothing if NIC doesn't exist
+	if (IntSource.word == 0xffffffff)
+	{
+		RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST | fRTMP_ADAPTER_HALT_IN_PROGRESS);
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+        return  IRQ_HANDLED;
+#else
+        return;
+#endif
+	}
+
+	if (IntSource.word & TxCoherent)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, (">>>TxCoherent<<<\n"));
+		RTMPHandleRxCoherentInterrupt(pAd);
+	}
+
+	if (IntSource.word & RxCoherent)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, (">>>RxCoherent<<<\n"));
+		RTMPHandleRxCoherentInterrupt(pAd);
+	}
+
+	if (IntSource.word & FifoStaFullInt)
+	{
+#if 1
+		if ((pAd->int_disable_mask & FifoStaFullInt) == 0)
+		{
+			/* mask FifoStaFullInt */
+			rt2860_int_disable(pAd, FifoStaFullInt);
+			tasklet_hi_schedule(&pObj->fifo_statistic_full_task);
+		}
+		pAd->int_pending |= FifoStaFullInt;
+#else
+		NICUpdateFifoStaCounters(pAd);
+#endif
+	}
+
+	if (IntSource.word & INT_MGMT_DLY)
+	{
+		if ((pAd->int_disable_mask & INT_MGMT_DLY) ==0 )
+		{
+			rt2860_int_disable(pAd, INT_MGMT_DLY);
+			tasklet_hi_schedule(&pObj->mgmt_dma_done_task);
+		}
+		pAd->int_pending |= INT_MGMT_DLY ;
+	}
+
+	if (IntSource.word & INT_RX)
+	{
+		if ((pAd->int_disable_mask & INT_RX) == 0)
+		{
+			/* mask RxINT */
+			rt2860_int_disable(pAd, INT_RX);
+			tasklet_hi_schedule(&pObj->rx_done_task);
+		}
+		pAd->int_pending |= INT_RX;
+	}
+
+	if (IntSource.word & INT_HCCA_DLY)
+	{
+
+		if ((pAd->int_disable_mask & INT_HCCA_DLY) == 0)
+		{
+			/* mask TxDataInt */
+			rt2860_int_disable(pAd, INT_HCCA_DLY);
+			tasklet_hi_schedule(&pObj->hcca_dma_done_task);
+		}
+		pAd->int_pending |= INT_HCCA_DLY;
+	}
+
+	if (IntSource.word & INT_AC3_DLY)
+	{
+
+		if ((pAd->int_disable_mask & INT_AC3_DLY) == 0)
+		{
+			/* mask TxDataInt */
+			rt2860_int_disable(pAd, INT_AC3_DLY);
+			tasklet_hi_schedule(&pObj->ac3_dma_done_task);
+		}
+		pAd->int_pending |= INT_AC3_DLY;
+	}
+
+	if (IntSource.word & INT_AC2_DLY)
+	{
+
+		if ((pAd->int_disable_mask & INT_AC2_DLY) == 0)
+		{
+			/* mask TxDataInt */
+			rt2860_int_disable(pAd, INT_AC2_DLY);
+			tasklet_hi_schedule(&pObj->ac2_dma_done_task);
+		}
+		pAd->int_pending |= INT_AC2_DLY;
+	}
+
+	if (IntSource.word & INT_AC1_DLY)
+	{
+
+		pAd->int_pending |= INT_AC1_DLY;
+
+		if ((pAd->int_disable_mask & INT_AC1_DLY) == 0)
+		{
+			/* mask TxDataInt */
+			rt2860_int_disable(pAd, INT_AC1_DLY);
+			tasklet_hi_schedule(&pObj->ac1_dma_done_task);
+		}
+
+	}
+
+	if (IntSource.word & INT_AC0_DLY)
+	{
+		pAd->int_pending |= INT_AC0_DLY;
+
+		if ((pAd->int_disable_mask & INT_AC0_DLY) == 0)
+		{
+			/* mask TxDataInt */
+			rt2860_int_disable(pAd, INT_AC0_DLY);
+			tasklet_hi_schedule(&pObj->ac0_dma_done_task);
+		}
+
+	}
+
+    if (IntSource.word & PreTBTTInt)
+	{
+		RTMPHandlePreTBTTInterrupt(pAd);
+	}
+
+	if (IntSource.word & TBTTInt)
+	{
+		RTMPHandleTBTTInterrupt(pAd);
+	}
+
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (IntSource.word & AutoWakeupInt)
+			RTMPHandleTwakeupInterrupt(pAd);
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+    return  IRQ_HANDLED;
+#endif
+
+}
+
+/*
+========================================================================
+Routine Description:
+    Check the chipset vendor/product ID.
+
+Arguments:
+    _dev_p				Point to the PCI or USB device
+
+Return Value:
+    TRUE				Check ok
+	FALSE				Check fail
+
+Note:
+========================================================================
+*/
+BOOLEAN RT28XXChipsetCheck(
+	IN void *_dev_p)
+{
+	/* always TRUE */
+	return TRUE;
+}
+
+
+/*
+========================================================================
+Routine Description:
+    Init net device structure.
+
+Arguments:
+    _dev_p				Point to the PCI or USB device
+    *net_dev			Point to the net device
+	*pAd				the raxx interface data pointer
+
+Return Value:
+    TRUE				Init ok
+	FALSE				Init fail
+
+Note:
+========================================================================
+*/
+BOOLEAN RT28XXNetDevInit(
+	IN void 				*_dev_p,
+	IN struct  net_device	*net_dev,
+	IN RTMP_ADAPTER 		*pAd)
+{
+	struct pci_dev *pci_dev = (struct pci_dev *)_dev_p;
+    const CHAR	*print_name;
+    ULONG	csr_addr;
+
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+    print_name = pci_dev ? pci_name(pci_dev) : "rt2860";
+#else
+    print_name = pci_dev ? pci_dev->slot_name : "rt2860";
+#endif // LINUX_VERSION_CODE //
+
+	net_dev->base_addr = 0;
+	net_dev->irq = 0;
+
+    if (pci_request_regions(pci_dev, print_name))
+        goto err_out_free_netdev;
+
+    // interrupt IRQ number
+    net_dev->irq = pci_dev->irq;
+
+    // map physical address to virtual address for accessing register
+    csr_addr = (unsigned long) ioremap(pci_resource_start(pci_dev, 0),
+										pci_resource_len(pci_dev, 0));
+
+    if (!csr_addr)
+    {
+        DBGPRINT(RT_DEBUG_ERROR,
+				("ioremap failed for device %s, region 0x%lX @ 0x%lX\n",
+				print_name, (ULONG)pci_resource_len(pci_dev, 0),
+				(ULONG)pci_resource_start(pci_dev, 0)));
+        goto err_out_free_res;
+    }
+
+    // Save CSR virtual address and irq to device structure
+    net_dev->base_addr = csr_addr;
+    pAd->CSRBaseAddress = (PUCHAR)net_dev->base_addr;
+
+    // Set DMA master
+    pci_set_master(pci_dev);
+
+    net_dev->priv_flags = INT_MAIN;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("%s: at 0x%lx, VA 0x%lx, IRQ %d. \n",
+        	net_dev->name, (ULONG)pci_resource_start(pci_dev, 0),
+			(ULONG)csr_addr, pci_dev->irq));
+	return TRUE;
+
+
+	/* --------------------------- ERROR HANDLE --------------------------- */
+err_out_free_res:
+    pci_release_regions(pci_dev);
+err_out_free_netdev:
+	/* free netdev in caller, not here */
+	return FALSE;
+}
+
+
+/*
+========================================================================
+Routine Description:
+    Init net device structure.
+
+Arguments:
+    _dev_p				Point to the PCI or USB device
+	*pAd				the raxx interface data pointer
+
+Return Value:
+    TRUE				Config ok
+	FALSE				Config fail
+
+Note:
+========================================================================
+*/
+BOOLEAN RT28XXProbePostConfig(
+	IN void 				*_dev_p,
+	IN RTMP_ADAPTER 		*pAd,
+	IN INT32				argc)
+{
+	/* no use */
+	return TRUE;
+}
+
+
+/*
+========================================================================
+Routine Description:
+    Disable DMA.
+
+Arguments:
+	*pAd				the raxx interface data pointer
+
+Return Value:
+	None
+
+Note:
+========================================================================
+*/
+VOID RT28XXDMADisable(
+	IN RTMP_ADAPTER 		*pAd)
+{
+	WPDMA_GLO_CFG_STRUC     GloCfg;
+
+
+	RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &GloCfg.word);
+	GloCfg.word &= 0xff0;
+	GloCfg.field.EnTXWriteBackDDONE =1;
+	RTMP_IO_WRITE32(pAd, WPDMA_GLO_CFG, GloCfg.word);
+}
+
+
+/*
+========================================================================
+Routine Description:
+    Enable DMA.
+
+Arguments:
+	*pAd				the raxx interface data pointer
+
+Return Value:
+	None
+
+Note:
+========================================================================
+*/
+VOID RT28XXDMAEnable(
+	IN RTMP_ADAPTER 		*pAd)
+{
+	WPDMA_GLO_CFG_STRUC	GloCfg;
+	int i = 0;
+
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x4);
+	do
+	{
+		RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &GloCfg.word);
+		if ((GloCfg.field.TxDMABusy == 0)  && (GloCfg.field.RxDMABusy == 0))
+			break;
+
+		DBGPRINT(RT_DEBUG_TRACE, ("==>  DMABusy\n"));
+		RTMPusecDelay(1000);
+		i++;
+	}while ( i <200);
+
+	RTMPusecDelay(50);
+
+	GloCfg.field.EnTXWriteBackDDONE = 1;
+	GloCfg.field.WPDMABurstSIZE = 2;
+	GloCfg.field.EnableRxDMA = 1;
+	GloCfg.field.EnableTxDMA = 1;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<== WRITE DMA offset 0x208 = 0x%x\n", GloCfg.word));
+	RTMP_IO_WRITE32(pAd, WPDMA_GLO_CFG, GloCfg.word);
+
+}
+
+/*
+========================================================================
+Routine Description:
+    Write Beacon buffer to Asic.
+
+Arguments:
+	*pAd				the raxx interface data pointer
+
+Return Value:
+	None
+
+Note:
+========================================================================
+*/
+VOID RT28xx_UpdateBeaconToAsic(
+	IN RTMP_ADAPTER		*pAd,
+	IN INT				apidx,
+	IN ULONG			FrameLen,
+	IN ULONG			UpdatePos)
+{
+	ULONG				CapInfoPos = 0;
+	UCHAR  			*ptr, *ptr_update, *ptr_capinfo;
+	UINT  			i;
+	BOOLEAN			bBcnReq = FALSE;
+	UCHAR			bcn_idx = 0;
+
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s() : No valid Interface be found.\n", __FUNCTION__));
+		return;
+	}
+
+	if (bBcnReq == FALSE)
+	{
+		/* when the ra interface is down, do not send its beacon frame */
+		/* clear all zero */
+		for(i=0; i<TXWI_SIZE; i+=4)
+			RTMP_IO_WRITE32(pAd, pAd->BeaconOffset[bcn_idx] + i, 0x00);
+	}
+	else
+	{
+		ptr = (PUCHAR)&pAd->BeaconTxWI;
+#ifdef RT_BIG_ENDIAN
+		RTMPWIEndianChange(ptr, TYPE_TXWI);
+#endif
+		for (i=0; i<TXWI_SIZE; i+=4)  // 16-byte TXWI field
+		{
+			UINT32 longptr =  *ptr + (*(ptr+1)<<8) + (*(ptr+2)<<16) + (*(ptr+3)<<24);
+			RTMP_IO_WRITE32(pAd, pAd->BeaconOffset[bcn_idx] + i, longptr);
+			ptr += 4;
+		}
+
+		// Update CapabilityInfo in Beacon
+		for (i = CapInfoPos; i < (CapInfoPos+2); i++)
+		{
+			RTMP_IO_WRITE8(pAd, pAd->BeaconOffset[bcn_idx] + TXWI_SIZE + i, *ptr_capinfo);
+			ptr_capinfo ++;
+		}
+
+		if (FrameLen > UpdatePos)
+		{
+			for (i= UpdatePos; i< (FrameLen); i++)
+			{
+				RTMP_IO_WRITE8(pAd, pAd->BeaconOffset[bcn_idx] + TXWI_SIZE + i, *ptr_update);
+				ptr_update ++;
+			}
+		}
+
+	}
+
+}
+
+#ifdef CONFIG_STA_SUPPORT
+VOID RTMPInitPCIeLinkCtrlValue(
+	IN	PRTMP_ADAPTER	pAd)
+{
+}
+
+VOID RTMPFindHostPCIDev(
+    IN	PRTMP_ADAPTER	pAd)
+{
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+		Level = RESTORE_HALT : Restore PCI host and Ralink PCIe Link Control field to its default value.
+		Level = Other Value : Restore from dot11 power save or radio off status. And force PCI host Link Control fields to 0x1
+
+	========================================================================
+*/
+VOID RTMPPCIeLinkCtrlValueRestore(
+	IN	PRTMP_ADAPTER	pAd,
+	IN   UCHAR		Level)
+{
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+		Max : limit Host PCI and Ralink PCIe device's LINK CONTROL field's value.
+		Because now frequently set our device to mode 1 or mode 3 will cause problem.
+
+	========================================================================
+*/
+VOID RTMPPCIeLinkCtrlSetting(
+	IN	PRTMP_ADAPTER	pAd,
+	IN 	USHORT		Max)
+{
+}
+#endif // CONFIG_STA_SUPPORT //
+
+VOID rt2860_stop(struct net_device *net_dev)
+{
+    PRTMP_ADAPTER pAd = (PRTMP_ADAPTER)NULL;
+    if (net_dev == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("net_dev == NULL!\n"));
+	}
+	else
+		pAd = net_dev->ml_priv;
+
+	if (pAd != NULL)
+	{
+	    // stop interface
+		netif_carrier_off(net_dev);
+		netif_stop_queue(net_dev);
+
+		// mark device as removed from system and therefore no longer available
+		netif_device_detach(net_dev);
+
+		// mark halt flag
+		RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS);
+		RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF);
+
+		// take down the device
+		rt28xx_close((PNET_DEV)net_dev);
+		RT_MOD_DEC_USE_COUNT();
+	}
+    return;
+}
+
+/*
+ * invaild or writeback cache
+ * and convert virtual address to physical address
+ */
+dma_addr_t linux_pci_map_single(void *handle, void *ptr, size_t size, int sd_idx, int direction)
+{
+	PRTMP_ADAPTER pAd;
+	POS_COOKIE pObj;
+
+	/*
+		------ Porting Information ------
+		> For Tx Alloc:
+			mgmt packets => sd_idx = 0
+			SwIdx: pAd->MgmtRing.TxCpuIdx
+			pTxD : pAd->MgmtRing.Cell[SwIdx].AllocVa;
+
+			data packets => sd_idx = 1
+	 		TxIdx : pAd->TxRing[pTxBlk->QueIdx].TxCpuIdx
+	 		QueIdx: pTxBlk->QueIdx
+	 		pTxD  : pAd->TxRing[pTxBlk->QueIdx].Cell[TxIdx].AllocVa;
+
+	 	> For Rx Alloc:
+	 		sd_idx = -1
+	*/
+
+	pAd = (PRTMP_ADAPTER)handle;
+	pObj = (POS_COOKIE)pAd->OS_Cookie;
+
+	if (sd_idx == 1)
+	{
+		PTX_BLK		pTxBlk;
+		pTxBlk = (PTX_BLK)ptr;
+		return pci_map_single(pObj->pci_dev, pTxBlk->pSrcBufData, pTxBlk->SrcBufLen, direction);
+	}
+	else
+	{
+		return pci_map_single(pObj->pci_dev, ptr, size, direction);
+	}
+
+}
+
+void linux_pci_unmap_single(void *handle, dma_addr_t dma_addr, size_t size, int direction)
+{
+	PRTMP_ADAPTER pAd;
+	POS_COOKIE pObj;
+
+	pAd=(PRTMP_ADAPTER)handle;
+	pObj = (POS_COOKIE)pAd->OS_Cookie;
+
+	pci_unmap_single(pObj->pci_dev, dma_addr, size, direction);
+
+}
+
diff --git a/drivers/staging/rt2860/Kconfig b/drivers/staging/rt2860/Kconfig
new file mode 100644
index 00000000000..7f44e5e7246
--- /dev/null
+++ b/drivers/staging/rt2860/Kconfig
@@ -0,0 +1,5 @@
+config RT2860
+	tristate "Ralink 2860 wireless support"
+	depends on PCI && X86 && WLAN_80211
+	---help---
+	  This is an experimental driver for the Ralink 2860 wireless chip.
diff --git a/drivers/staging/rt2860/Makefile b/drivers/staging/rt2860/Makefile
new file mode 100644
index 00000000000..6162212b588
--- /dev/null
+++ b/drivers/staging/rt2860/Makefile
@@ -0,0 +1,43 @@
+obj-$(CONFIG_RT2860)	+= rt2860sta.o
+
+# TODO: all of these should be removed
+EXTRA_CFLAGS += -DLINUX -DAGGREGATION_SUPPORT -DPIGGYBACK_SUPPORT -DWMM_SUPPORT
+EXTRA_CFLAGS += -DRT2860
+EXTRA_CFLAGS += -DCONFIG_STA_SUPPORT
+EXTRA_CFLAGS += -DDBG
+EXTRA_CFLAGS += -DDOT11_N_SUPPORT
+EXTRA_CFLAGS += -DWPA_SUPPLICANT_SUPPORT
+EXTRA_CFLAGS += -DNATIVE_WPA_SUPPLICANT_SUPPORT
+
+rt2860sta-objs :=	\
+	common/md5.o		\
+	common/mlme.o		\
+	common/rtmp_wep.o	\
+	common/action.o		\
+	common/cmm_data.o	\
+	common/rtmp_init.o	\
+	common/rtmp_tkip.o	\
+	common/cmm_sync.o	\
+	common/eeprom.o		\
+	common/cmm_sanity.o	\
+	common/cmm_info.o	\
+	common/cmm_wpa.o	\
+	common/dfs.o		\
+	common/spectrum.o	\
+	sta/assoc.o		\
+	sta/aironet.o		\
+	sta/auth.o		\
+	sta/auth_rsp.o		\
+	sta/sync.o		\
+	sta/sanity.o		\
+	sta/rtmp_data.o		\
+	sta/connect.o		\
+	sta/wpa.o		\
+	rt_linux.o		\
+	rt_profile.o		\
+	rt_main_dev.o		\
+	sta_ioctl.o		\
+	common/ba_action.o	\
+	common/2860_rtmp_init.o	\
+	2860_main_dev.o		\
+	common/cmm_data_2860.o
diff --git a/drivers/staging/rt2860/TODO b/drivers/staging/rt2860/TODO
new file mode 100644
index 00000000000..2f70b0faca3
--- /dev/null
+++ b/drivers/staging/rt2860/TODO
@@ -0,0 +1,17 @@
+I'm hesitant to add a TODO file here, as the wireless developers would
+really have people help them out on the "clean" rt2860 driver that can
+be found at the rt2860.sf.net site.
+
+But, if you wish to clean up this driver instead, here's a short list of
+things that need to be done to get it into a more mergable shape:
+
+TODO:
+	- checkpatch.pl clean
+	- sparse clean
+	- port to in-kernel 80211 stack
+	- remove reading from /etc/ config files
+	- review by the wireless developer community
+
+Please send any patches or complaints about this driver to Greg
+Kroah-Hartman <greg@kroah.com> and don't bother the upstream wireless
+kernel developers about it, they want nothing to do with it.
diff --git a/drivers/staging/rt2860/aironet.h b/drivers/staging/rt2860/aironet.h
new file mode 100644
index 00000000000..1e07b19b8cd
--- /dev/null
+++ b/drivers/staging/rt2860/aironet.h
@@ -0,0 +1,210 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	aironet.h
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+	Paul Lin	04-06-15		Initial
+*/
+
+#ifndef	__AIRONET_H__
+#define	__AIRONET_H__
+
+// Measurement Type definition
+#define	MSRN_TYPE_UNUSED				0
+#define	MSRN_TYPE_CHANNEL_LOAD_REQ		1
+#define	MSRN_TYPE_NOISE_HIST_REQ		2
+#define	MSRN_TYPE_BEACON_REQ			3
+#define	MSRN_TYPE_FRAME_REQ				4
+
+// Scan Mode in Beacon Request
+#define	MSRN_SCAN_MODE_PASSIVE			0
+#define	MSRN_SCAN_MODE_ACTIVE			1
+#define	MSRN_SCAN_MODE_BEACON_TABLE		2
+
+// PHY type definition for Aironet beacon report, CCX 2 table 36-9
+#define	PHY_FH							1
+#define	PHY_DSS							2
+#define	PHY_UNUSED						3
+#define	PHY_OFDM						4
+#define	PHY_HR_DSS						5
+#define	PHY_ERP							6
+
+// RPI table in dBm
+#define	RPI_0			0			//	Power <= -87
+#define	RPI_1			1			//	-87 < Power <= -82
+#define	RPI_2			2			//	-82 < Power <= -77
+#define	RPI_3			3			//	-77 < Power <= -72
+#define	RPI_4			4			//	-72 < Power <= -67
+#define	RPI_5			5			//	-67 < Power <= -62
+#define	RPI_6			6			//	-62 < Power <= -57
+#define	RPI_7			7			//	-57 < Power
+
+// Cisco Aironet IAPP definetions
+#define	AIRONET_IAPP_TYPE					0x32
+#define	AIRONET_IAPP_SUBTYPE_REQUEST		0x01
+#define	AIRONET_IAPP_SUBTYPE_REPORT			0x81
+
+// Measurement Request detail format
+typedef	struct	_MEASUREMENT_REQUEST	{
+	UCHAR	Channel;
+	UCHAR	ScanMode;			// Use only in beacon request, other requests did not use this field
+	USHORT	Duration;
+}	MEASUREMENT_REQUEST, *PMEASUREMENT_REQUEST;
+
+// Beacon Measurement Report
+// All these field might change to UCHAR, because we didn't do anything to these report.
+// We copy all these beacons and report to CCX 2 AP.
+typedef	struct	_BEACON_REPORT	{
+	UCHAR	Channel;
+	UCHAR	Spare;
+	USHORT	Duration;
+	UCHAR	PhyType;			// Definiation is listed above table 36-9
+	UCHAR	RxPower;
+	UCHAR	BSSID[6];
+	UCHAR	ParentTSF[4];
+	UCHAR	TargetTSF[8];
+	USHORT	BeaconInterval;
+	USHORT	CapabilityInfo;
+}	BEACON_REPORT, *PBEACON_REPORT;
+
+// Frame Measurement Report (Optional)
+typedef	struct	_FRAME_REPORT	{
+	UCHAR	Channel;
+	UCHAR	Spare;
+	USHORT	Duration;
+	UCHAR	TA;
+	UCHAR	BSSID[6];
+	UCHAR	RSSI;
+	UCHAR	Count;
+}	FRAME_REPORT, *PFRAME_REPORT;
+
+#pragma pack(1)
+// Channel Load Report
+typedef	struct	_CHANNEL_LOAD_REPORT	{
+	UCHAR	Channel;
+	UCHAR	Spare;
+	USHORT	Duration;
+	UCHAR	CCABusy;
+}	CHANNEL_LOAD_REPORT, *PCHANNEL_LOAD_REPORT;
+#pragma pack()
+
+// Nosie Histogram Report
+typedef	struct	_NOISE_HIST_REPORT	{
+	UCHAR	Channel;
+	UCHAR	Spare;
+	USHORT	Duration;
+	UCHAR	Density[8];
+}	NOISE_HIST_REPORT, *PNOISE_HIST_REPORT;
+
+// Radio Management Capability element
+typedef	struct	_RADIO_MANAGEMENT_CAPABILITY	{
+	UCHAR	Eid;				// TODO: Why the Eid is 1 byte, not normal 2 bytes???
+	UCHAR	Length;
+	UCHAR	AironetOui[3];		// AIronet OUI (00 40 96)
+	UCHAR	Type;				// Type / Version
+	USHORT	Status;				// swap16 required
+}	RADIO_MANAGEMENT_CAPABILITY, *PRADIO_MANAGEMENT_CAPABILITY;
+
+// Measurement Mode Bit definition
+typedef	struct	_MEASUREMENT_MODE	{
+	UCHAR	Rsvd:4;
+	UCHAR	Report:1;
+	UCHAR	NotUsed:1;
+	UCHAR	Enable:1;
+	UCHAR	Parallel:1;
+}	MEASUREMENT_MODE, *PMEASUREMENT_MODE;
+
+// Measurement Request element, This is little endian mode
+typedef	struct	_MEASUREMENT_REQUEST_ELEMENT	{
+	USHORT				Eid;
+	USHORT				Length;				// swap16 required
+	USHORT				Token;				// non-zero unique token
+	UCHAR				Mode;				// Measurement Mode
+	UCHAR				Type;				// Measurement type
+}	MEASUREMENT_REQUEST_ELEMENT, *PMEASUREMENT_REQUEST_ELEMENT;
+
+// Measurement Report element, This is little endian mode
+typedef	struct	_MEASUREMENT_REPORT_ELEMENT	{
+	USHORT				Eid;
+	USHORT				Length;				// swap16 required
+	USHORT				Token;				// non-zero unique token
+	UCHAR				Mode;				// Measurement Mode
+	UCHAR				Type;				// Measurement type
+}	MEASUREMENT_REPORT_ELEMENT, *PMEASUREMENT_REPORT_ELEMENT;
+
+// Cisco Aironet IAPP Frame Header, Network byte order used
+typedef	struct	_AIRONET_IAPP_HEADER {
+	UCHAR	CiscoSnapHeader[8];	// 8 bytes Cisco snap header
+	USHORT	Length;				// IAPP ID & length, remember to swap16 in LE system
+	UCHAR	Type;				// IAPP type
+	UCHAR	SubType;			// IAPP subtype
+	UCHAR	DA[6];				// Destination MAC address
+	UCHAR	SA[6];				// Source MAC address
+	USHORT	Token;				// Dialog token, no need to swap16 since it is for yoken usage only
+}	AIRONET_IAPP_HEADER, *PAIRONET_IAPP_HEADER;
+
+// Radio Measurement Request frame
+typedef	struct	_AIRONET_RM_REQUEST_FRAME	{
+    AIRONET_IAPP_HEADER	IAPP;			// Common header
+	UCHAR				Delay;			// Activation Delay
+	UCHAR				Offset;			// Measurement offset
+}	AIRONET_RM_REQUEST_FRAME, *PAIRONET_RM_REQUEST_FRAME;
+
+// Radio Measurement Report frame
+typedef	struct	_AIRONET_RM_REPORT_FRAME	{
+    AIRONET_IAPP_HEADER	IAPP;			// Common header
+}	AIRONET_RM_REPORT_FRAME, *PAIRONET_RM_REPORT_FRAME;
+
+// Saved element request actions which will saved in StaCfg.
+typedef	struct	_RM_REQUEST_ACTION	{
+	MEASUREMENT_REQUEST_ELEMENT	ReqElem;		// Saved request element
+	MEASUREMENT_REQUEST			Measurement;	// Saved measurement within the request element
+}	RM_REQUEST_ACTION, *PRM_REQUEST_ACTION;
+
+// CCX administration control
+typedef	union	_CCX_CONTROL	{
+	struct	{
+		UINT32		Enable:1;			// Enable CCX2
+		UINT32		LeapEnable:1;		// Enable LEAP at CCX2
+		UINT32		RMEnable:1;			// Radio Measurement Enable
+		UINT32		DCRMEnable:1;		// Non serving channel Radio Measurement enable
+		UINT32		QOSEnable:1;		// Enable QOS for CCX 2.0 support
+		UINT32		FastRoamEnable:1;	// Enable fast roaming
+		UINT32		Rsvd:2;				// Not used
+		UINT32		dBmToRoam:8;		// the condition to roam when receiving Rssi less than this value. It's negative value.
+		UINT32		TuLimit:16;			// Limit for different channel scan
+	}	field;
+	UINT32			word;
+}	CCX_CONTROL, *PCCX_CONTROL;
+
+#endif	// __AIRONET_H__
diff --git a/drivers/staging/rt2860/ap.h b/drivers/staging/rt2860/ap.h
new file mode 100644
index 00000000000..df6db281312
--- /dev/null
+++ b/drivers/staging/rt2860/ap.h
@@ -0,0 +1,557 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    ap.h
+
+    Abstract:
+    Miniport generic portion header file
+
+    Revision History:
+    Who         When          What
+    --------    ----------    ----------------------------------------------
+    Paul Lin    08-01-2002    created
+    James Tan   09-06-2002    modified (Revise NTCRegTable)
+    John Chang  12-22-2004    modified for RT2561/2661. merge with STA driver
+*/
+#ifndef __AP_H__
+#define __AP_H__
+
+
+
+// ========================= AP RTMP.h ================================
+
+
+
+// =============================================================
+//      Function Prototypes
+// =============================================================
+
+// ap_data.c
+
+BOOLEAN APBridgeToWirelessSta(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          pHeader,
+    IN  UINT            HdrLen,
+    IN  PUCHAR          pData,
+    IN  UINT            DataLen,
+    IN  ULONG           fromwdsidx);
+
+BOOLEAN APHandleRxDoneInterrupt(
+    IN  PRTMP_ADAPTER   pAd);
+
+VOID	APSendPackets(
+	IN	NDIS_HANDLE		MiniportAdapterContext,
+	IN	PPNDIS_PACKET	ppPacketArray,
+	IN	UINT			NumberOfPackets);
+
+NDIS_STATUS APSendPacket(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PNDIS_PACKET    pPacket);
+
+
+NDIS_STATUS APHardTransmit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	UCHAR			QueIdx);
+
+VOID APRxEAPOLFrameIndicate(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID);
+
+NDIS_STATUS APCheckRxError(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PRT28XX_RXD_STRUC		pRxD,
+	IN	UCHAR			Wcid);
+
+BOOLEAN APCheckClass2Class3Error(
+    IN  PRTMP_ADAPTER   pAd,
+	IN ULONG Wcid,
+	IN  PHEADER_802_11  pHeader);
+
+VOID APHandleRxPsPoll(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pAddr,
+	IN	USHORT			Aid,
+    IN	BOOLEAN			isActive);
+
+VOID    RTMPDescriptorEndianChange(
+    IN  PUCHAR          pData,
+    IN  ULONG           DescriptorType);
+
+VOID    RTMPFrameEndianChange(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          pData,
+    IN  ULONG           Dir,
+    IN  BOOLEAN         FromRxDoneInt);
+
+// ap_assoc.c
+
+VOID APAssocStateMachineInit(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  STATE_MACHINE *S,
+    OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID  APPeerAssocReqAction(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  MLME_QUEUE_ELEM *Elem);
+
+VOID  APPeerReassocReqAction(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  MLME_QUEUE_ELEM *Elem);
+
+VOID  APPeerDisassocReqAction(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  MLME_QUEUE_ELEM *Elem);
+
+VOID MbssKickOutStas(
+	IN PRTMP_ADAPTER pAd,
+	IN INT apidx,
+	IN USHORT Reason);
+
+VOID APMlmeKickOutSta(
+    IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pStaAddr,
+	IN UCHAR Wcid,
+	IN USHORT Reason);
+
+VOID APMlmeDisassocReqAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID  APCls3errAction(
+    IN  PRTMP_ADAPTER   pAd,
+	IN 	ULONG Wcid,
+    IN	PHEADER_802_11	pHeader);
+
+
+USHORT APBuildAssociation(
+    IN PRTMP_ADAPTER pAd,
+    IN MAC_TABLE_ENTRY *pEntry,
+    IN USHORT CapabilityInfo,
+    IN UCHAR  MaxSupportedRateIn500Kbps,
+    IN UCHAR  *RSN,
+    IN UCHAR  *pRSNLen,
+    IN BOOLEAN bWmmCapable,
+    IN ULONG  RalinkIe,
+#ifdef DOT11N_DRAFT3
+    IN EXT_CAP_INFO_ELEMENT ExtCapInfo,
+#endif // DOT11N_DRAFT3 //
+	IN HT_CAPABILITY_IE		*pHtCapability,
+	IN UCHAR		 HtCapabilityLen,
+    OUT USHORT *pAid);
+
+// ap_auth.c
+
+void APAuthStateMachineInit(
+    IN PRTMP_ADAPTER pAd,
+    IN STATE_MACHINE *Sm,
+    OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID APMlmeDeauthReqAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID APCls2errAction(
+    IN PRTMP_ADAPTER pAd,
+	IN 	ULONG Wcid,
+    IN	PHEADER_802_11	pHeader);
+
+// ap_authrsp.c
+
+VOID APAuthRspStateMachineInit(
+    IN PRTMP_ADAPTER pAd,
+    IN PSTATE_MACHINE Sm,
+    IN STATE_MACHINE_FUNC Trans[]);
+
+VOID APPeerAuthAtAuthRspIdleAction(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  MLME_QUEUE_ELEM *Elem);
+
+VOID APPeerDeauthReqAction(
+    IN PRTMP_ADAPTER	pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID APPeerAuthSimpleRspGenAndSend(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PHEADER_802_11 pHdr80211,
+    IN  USHORT Alg,
+    IN  USHORT Seq,
+    IN  USHORT StatusCode);
+
+// ap_connect.c
+
+BOOLEAN BeaconTransmitRequired(
+	IN PRTMP_ADAPTER	pAd,
+	IN INT				apidx);
+
+VOID APMakeBssBeacon(
+    IN  PRTMP_ADAPTER   pAd,
+	IN	INT				apidx);
+
+VOID  APUpdateBeaconFrame(
+    IN  PRTMP_ADAPTER   pAd,
+	IN	INT				apidx);
+
+VOID APMakeAllBssBeacon(
+    IN  PRTMP_ADAPTER   pAd);
+
+VOID  APUpdateAllBeaconFrame(
+    IN  PRTMP_ADAPTER   pAd);
+
+
+// ap_sync.c
+
+VOID APSyncStateMachineInit(
+    IN PRTMP_ADAPTER pAd,
+    IN STATE_MACHINE *Sm,
+    OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID APScanTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID APInvalidStateWhenScan(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID APScanTimeoutAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID APPeerProbeReqAction(
+    IN  PRTMP_ADAPTER pAd,
+    IN  MLME_QUEUE_ELEM *Elem);
+
+VOID APPeerBeaconAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID APMlmeScanReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID APPeerBeaconAtScanAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID APScanCnclAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID ApSiteSurvey(
+	IN PRTMP_ADAPTER pAd);
+
+VOID SupportRate(
+	IN PUCHAR SupRate,
+	IN UCHAR SupRateLen,
+	IN PUCHAR ExtRate,
+	IN UCHAR ExtRateLen,
+	OUT PUCHAR *Rates,
+	OUT PUCHAR RatesLen,
+	OUT PUCHAR pMaxSupportRate);
+
+
+BOOLEAN ApScanRunning(
+	IN PRTMP_ADAPTER pAd);
+
+#ifdef DOT11N_DRAFT3
+VOID APOverlappingBSSScan(
+	IN RTMP_ADAPTER *pAd);
+#endif // DOT11N_DRAFT3 //
+
+// ap_wpa.c
+
+VOID APWpaStateMachineInit(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  STATE_MACHINE *Sm,
+    OUT STATE_MACHINE_FUNC Trans[]);
+
+// ap_mlme.c
+
+VOID APMlmePeriodicExec(
+    IN  PRTMP_ADAPTER   pAd);
+
+VOID APMlmeSelectTxRateTable(
+	IN PRTMP_ADAPTER		pAd,
+	IN PMAC_TABLE_ENTRY		pEntry,
+	IN PUCHAR				*ppTable,
+	IN PUCHAR				pTableSize,
+	IN PUCHAR				pInitTxRateIdx);
+
+VOID APMlmeSetTxRate(
+	IN PRTMP_ADAPTER		pAd,
+	IN PMAC_TABLE_ENTRY		pEntry,
+	IN PRTMP_TX_RATE_SWITCH	pTxRate);
+
+VOID APMlmeDynamicTxRateSwitching(
+    IN PRTMP_ADAPTER pAd);
+
+VOID APQuickResponeForRateUpExec(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3);
+
+BOOLEAN APMsgTypeSubst(
+    IN PRTMP_ADAPTER pAd,
+    IN PFRAME_802_11 pFrame,
+    OUT INT *Machine,
+    OUT INT *MsgType);
+
+VOID APQuickResponeForRateUpExec(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3);
+
+
+VOID RTMPSetPiggyBack(
+	IN PRTMP_ADAPTER	pAd,
+	IN BOOLEAN			bPiggyBack);
+
+VOID APAsicEvaluateRxAnt(
+	IN PRTMP_ADAPTER	pAd);
+
+VOID APAsicRxAntEvalTimeout(
+	IN PRTMP_ADAPTER	pAd);
+
+// ap.c
+
+VOID APSwitchChannel(
+	IN PRTMP_ADAPTER pAd,
+	IN INT Channel);
+
+NDIS_STATUS APInitialize(
+    IN  PRTMP_ADAPTER   pAd);
+
+VOID APShutdown(
+    IN PRTMP_ADAPTER    pAd);
+
+VOID APStartUp(
+    IN  PRTMP_ADAPTER   pAd);
+
+VOID APStop(
+    IN  PRTMP_ADAPTER   pAd);
+
+VOID APCleanupPsQueue(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PQUEUE_HEADER   pQueue);
+
+VOID MacTableReset(
+    IN  PRTMP_ADAPTER   pAd);
+
+MAC_TABLE_ENTRY *MacTableInsertEntry(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          pAddr,
+	IN	UCHAR			apidx,
+	IN BOOLEAN	CleanAll);
+
+BOOLEAN MacTableDeleteEntry(
+    IN  PRTMP_ADAPTER   pAd,
+	IN USHORT wcid,
+    IN  PUCHAR          pAddr);
+
+MAC_TABLE_ENTRY *MacTableLookup(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          pAddr);
+
+VOID MacTableMaintenance(
+    IN PRTMP_ADAPTER pAd);
+
+UINT32 MacTableAssocStaNumGet(
+	IN PRTMP_ADAPTER pAd);
+
+MAC_TABLE_ENTRY *APSsPsInquiry(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          pAddr,
+    OUT SST             *Sst,
+    OUT USHORT          *Aid,
+    OUT UCHAR           *PsMode,
+    OUT UCHAR           *Rate);
+
+BOOLEAN APPsIndicate(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          pAddr,
+	IN ULONG Wcid,
+    IN  UCHAR           Psm);
+
+VOID ApLogEvent(
+    IN PRTMP_ADAPTER    pAd,
+    IN PUCHAR           pAddr,
+    IN USHORT           Event);
+
+#ifdef DOT11_N_SUPPORT
+VOID APUpdateOperationMode(
+    IN PRTMP_ADAPTER pAd);
+#endif // DOT11_N_SUPPORT //
+
+VOID APUpdateCapabilityAndErpIe(
+	IN PRTMP_ADAPTER pAd);
+
+BOOLEAN ApCheckAccessControlList(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR        pAddr,
+	IN UCHAR         Apidx);
+
+VOID ApUpdateAccessControlList(
+    IN PRTMP_ADAPTER pAd,
+    IN UCHAR         Apidx);
+
+VOID ApEnqueueNullFrame(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR        pAddr,
+	IN UCHAR         TxRate,
+	IN UCHAR         PID,
+	IN UCHAR         apidx,
+    IN BOOLEAN       bQosNull,
+    IN BOOLEAN       bEOSP,
+    IN UCHAR         OldUP);
+
+VOID ApSendFrame(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PVOID           pBuffer,
+    IN  ULONG           Length,
+    IN  UCHAR           TxRate,
+    IN  UCHAR           PID);
+
+VOID ApEnqueueAckFrame(
+    IN PRTMP_ADAPTER pAd,
+    IN PUCHAR        pAddr,
+    IN UCHAR         TxRate,
+	IN UCHAR         apidx);
+
+UCHAR APAutoSelectChannel(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN Optimal);
+
+// ap_sanity.c
+
+
+BOOLEAN PeerAssocReqCmmSanity(
+    IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN isRessoc,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT USHORT *pCapabilityInfo,
+    OUT USHORT *pListenInterval,
+    OUT PUCHAR pApAddr,
+    OUT UCHAR *pSsidLen,
+    OUT char *Ssid,
+    OUT UCHAR *pRatesLen,
+    OUT UCHAR Rates[],
+    OUT UCHAR *RSN,
+    OUT UCHAR *pRSNLen,
+    OUT BOOLEAN *pbWmmCapable,
+    OUT ULONG  *pRalinkIe,
+#ifdef DOT11N_DRAFT3
+    OUT EXT_CAP_INFO_ELEMENT	*pExtCapInfo,
+#endif // DOT11N_DRAFT3 //
+    OUT UCHAR		 *pHtCapabilityLen,
+    OUT HT_CAPABILITY_IE *pHtCapability);
+
+BOOLEAN PeerDisassocReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT USHORT *Reason);
+
+BOOLEAN PeerDeauthReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT USHORT *Reason);
+
+BOOLEAN APPeerAuthSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+	OUT PUCHAR pAddr1,
+    OUT PUCHAR pAddr2,
+    OUT USHORT *Alg,
+    OUT USHORT *Seq,
+    OUT USHORT *Status,
+    CHAR *ChlgText);
+
+BOOLEAN APPeerProbeReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT CHAR Ssid[],
+    OUT UCHAR *SsidLen);
+
+BOOLEAN APPeerBeaconAndProbeRspSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT PUCHAR pBssid,
+    OUT CHAR Ssid[],
+    OUT UCHAR *SsidLen,
+    OUT UCHAR *BssType,
+    OUT USHORT *BeaconPeriod,
+    OUT UCHAR *Channel,
+    OUT LARGE_INTEGER *Timestamp,
+    OUT USHORT *CapabilityInfo,
+    OUT UCHAR Rate[],
+    OUT UCHAR *RateLen,
+    OUT BOOLEAN *ExtendedRateIeExist,
+    OUT UCHAR *Erp);
+
+// ap_info.c
+
+#ifdef WIN_NDIS
+NDIS_STATUS APQueryInformation(
+    IN  NDIS_HANDLE MiniportAdapterContext,
+    IN  NDIS_OID    Oid,
+    IN  PVOID       pInformationBuffer,
+    IN  ULONG       InformationBufferLength,
+    OUT PULONG      pBytesWritten,
+    OUT PULONG      pBytesNeeded);
+
+NDIS_STATUS APSetInformation(
+    IN  NDIS_HANDLE MiniportAdapterContext,
+    IN  NDIS_OID    Oid,
+    IN  PVOID       pInformationBuffer,
+    IN  ULONG       InformationBufferLength,
+    OUT PULONG      pBytesRead,
+    OUT PULONG      pBytesNeeded);
+#endif
+
+
+// ================== end of AP RTMP.h ========================
+
+
+#endif  // __AP_H__
+
diff --git a/drivers/staging/rt2860/chlist.h b/drivers/staging/rt2860/chlist.h
new file mode 100644
index 00000000000..9e15b9daeb8
--- /dev/null
+++ b/drivers/staging/rt2860/chlist.h
@@ -0,0 +1,1296 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	chlist.c
+
+	Abstract:
+
+	Revision History:
+	Who         When          What
+	--------    ----------    ----------------------------------------------
+	Fonchi Wu   2007-12-19    created
+*/
+
+#ifndef __CHLIST_H__
+#define __CHLIST_H__
+
+#include "rtmp_type.h"
+#include "rtmp_def.h"
+
+
+#define ODOR			0
+#define IDOR			1
+#define BOTH			2
+
+#define BAND_5G         0
+#define BAND_24G        1
+#define BAND_BOTH       2
+
+typedef struct _CH_DESP {
+	UCHAR FirstChannel;
+	UCHAR NumOfCh;
+	CHAR MaxTxPwr;			// dBm
+	UCHAR Geography;			// 0:out door, 1:in door, 2:both
+	BOOLEAN DfsReq;			// Dfs require, 0: No, 1: yes.
+} CH_DESP, *PCH_DESP;
+
+typedef struct _CH_REGION {
+	UCHAR CountReg[3];
+	UCHAR DfsType;			// 0: CE, 1: FCC, 2: JAP, 3:JAP_W53, JAP_W56
+	CH_DESP ChDesp[10];
+} CH_REGION, *PCH_REGION;
+
+static CH_REGION ChRegion[] =
+{
+		{	// Antigua and Berbuda
+			"AG",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  23, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100, 11, 30, BOTH, FALSE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Argentina
+			"AR",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 52,  4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149, 4,  30, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Aruba
+			"AW",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  23, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100, 11, 30, BOTH, FALSE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Australia
+			"AU",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149, 5,  30, BOTH, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// Austria
+			"AT",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, IDOR, TRUE},		// 5G, ch 36~48
+				{ 52,  4,  23, IDOR, TRUE},		// 5G, ch 52~64
+				{ 100, 11, 30, BOTH, TRUE},		// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Bahamas
+			"BS",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149, 5,  30, BOTH, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// Barbados
+			"BB",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100, 11, 30, BOTH, FALSE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Bermuda
+			"BM",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100, 11, 30, BOTH, FALSE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Brazil
+			"BR",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100, 11, 24, BOTH, FALSE},	// 5G, ch 100~140
+				{ 149, 5,  30, BOTH, FALSE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Belgium
+			"BE",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  18, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  18, IDOR, FALSE},	// 5G, ch 52~64
+				{ 0},							// end
+			}
+		},
+
+		{	// Bulgaria
+			"BG",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11, 30, ODOR, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Canada
+			"CA",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  23, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149, 5,  30, BOTH, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// Cayman IsLands
+			"KY",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100, 11, 30, BOTH, FALSE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Chile
+			"CL",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  20, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  20, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149, 5,  20, BOTH, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// China
+			"CN",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 149, 4,  27, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Colombia
+			"CO",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  17, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100, 11, 30, BOTH, FALSE},	// 5G, ch 100~140
+				{ 149, 5,  30, BOTH, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// Costa Rica
+			"CR",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  17, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149, 4,  30, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Cyprus
+			"CY",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  24, IDOR, TRUE},		// 5G, ch 52~64
+				{ 100, 11, 30, BOTH, TRUE},		// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Czech_Republic
+			"CZ",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  23, IDOR, TRUE},		// 5G, ch 52~64
+				{ 0},							// end
+			}
+		},
+
+		{	// Denmark
+			"DK",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  23, IDOR, TRUE},		// 5G, ch 52~64
+				{ 100, 11, 30, BOTH, TRUE},		// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Dominican Republic
+			"DO",
+			CE,
+			{
+				{ 1,   0,  20, BOTH, FALSE},	// 2.4 G, ch 0
+				{ 149, 4,  20, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Equador
+			"EC",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 100, 11,  27, BOTH, FALSE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// El Salvador
+			"SV",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,   23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,   30, BOTH, TRUE},	// 5G, ch 52~64
+				{ 149, 4,   36, BOTH, TRUE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// Finland
+			"FI",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,   23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,   23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// France
+			"FR",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,   23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,   23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 0},							// end
+			}
+		},
+
+		{	// Germany
+			"DE",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,   23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,   23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Greece
+			"GR",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,   23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,   23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, ODOR, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Guam
+			"GU",
+			CE,
+			{
+				{ 1,   11,  20, BOTH, FALSE},	// 2.4 G, ch 1~11
+				{ 36,  4,   17, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,   24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, FALSE},	// 5G, ch 100~140
+				{ 149,  5,  30, BOTH, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// Guatemala
+			"GT",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,   17, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,   24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149,  4,  30, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Haiti
+			"HT",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,   17, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,   24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149,  4,  30, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Honduras
+			"HN",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 149,  4,  27, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Hong Kong
+			"HK",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, FALSE},	// 5G, ch 52~64
+				{ 149,  4,  30, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Hungary
+			"HU",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 0},							// end
+			}
+		},
+
+		{	// Iceland
+			"IS",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// India
+			"IN",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 149, 	4,  24, IDOR, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Indonesia
+			"ID",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 149, 	4,  27, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Ireland
+			"IE",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, ODOR, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Israel
+			"IL",
+			CE,
+			{
+				{ 1,    3,  20, IDOR, FALSE},	// 2.4 G, ch 1~3
+				{ 4, 	6,  20, BOTH, FALSE},	// 2.4 G, ch 4~9
+				{ 10, 	4,  20, IDOR, FALSE},	// 2.4 G, ch 10~13
+				{ 0},							// end
+			}
+		},
+
+		{	// Italy
+			"IT",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, ODOR, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Japan
+			"JP",
+			JAP,
+			{
+				{ 1,   14,  20, BOTH, FALSE},	// 2.4 G, ch 1~14
+				{ 36, 	4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 0},							// end
+			}
+		},
+
+		{	// Jordan
+			"JO",
+			CE,
+			{
+				{ 1,   13,  20, IDOR, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 149, 	4,  23, IDOR, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Latvia
+			"LV",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Liechtenstein
+			"LI",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 52, 	4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Lithuania
+			"LT",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Luxemburg
+			"LU",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Malaysia
+			"MY",
+			CE,
+			{
+				{ 36, 	4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  23, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149,  5,  20, BOTH, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// Malta
+			"MT",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Marocco
+			"MA",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  24, IDOR, FALSE},	// 5G, ch 36~48
+				{ 0},							// end
+			}
+		},
+
+		{	// Mexico
+			"MX",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149,  5,  30, IDOR, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// Netherlands
+			"NL",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  24, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// New Zealand
+			"NZ",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  24, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149,  4,  30, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Norway
+			"NO",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  24, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  24, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Peru
+			"PE",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 149,  4,  27, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Portugal
+			"PT",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Poland
+			"PL",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Romania
+			"RO",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Russia
+			"RU",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 149,  4,  20, IDOR, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Saudi Arabia
+			"SA",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149,  4,  23, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Serbia_and_Montenegro
+			"CS",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 0},							// end
+			}
+		},
+
+		{	// Singapore
+			"SG",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149,  4,  20, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Slovakia
+			"SK",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Slovenia
+			"SI",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 0},							// end
+			}
+		},
+
+		{	// South Africa
+			"ZA",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, FALSE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 149,  4,  30, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// South Korea
+			"KR",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  20, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  20, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100,  8,  20, BOTH, FALSE},	// 5G, ch 100~128
+				{ 149,  4,  20, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Spain
+			"ES",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  17, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Sweden
+			"SE",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Switzerland
+			"CH",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, TRUE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 0},							// end
+			}
+		},
+
+		{	// Taiwan
+			"TW",
+			CE,
+			{
+				{ 1,   11,  30, BOTH, FALSE},	// 2.4 G, ch 1~11
+				{ 52,   4,  23, IDOR, FALSE},	// 5G, ch 52~64
+				{ 0},							// end
+			}
+		},
+
+		{	// Turkey
+			"TR",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~11
+				{ 36,   4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, BOTH, FALSE},	// 5G, ch 52~64
+				{ 0},							// end
+			}
+		},
+
+		{	// UK
+			"GB",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~11
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 52~64
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Ukraine
+			"UA",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~11
+				{ 0},							// end
+			}
+		},
+
+		{	// United_Arab_Emirates
+			"AE",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~11
+				{ 0},							// end
+			}
+		},
+
+		{	// United_States
+			"US",
+			CE,
+			{
+				{ 1,   11,  30, BOTH, FALSE},	// 2.4 G, ch 1~11
+				{ 36,   4,  17, IDOR, FALSE},	// 5G, ch 52~64
+				{ 52,   4,  24, BOTH, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 149,  5,  30, BOTH, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// Venezuela
+			"VE",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~11
+				{ 149,  4,  27, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Default
+			"",
+			CE,
+			{
+				{ 1,   11,  20, BOTH, FALSE},	// 2.4 G, ch 1~11
+				{ 36,   4,  20, BOTH, FALSE},	// 5G, ch 52~64
+				{ 52,   4,  20, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100, 11,  20, BOTH, FALSE},	// 5G, ch 100~140
+				{ 149,  5,  20, BOTH, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+};
+
+static inline PCH_REGION GetChRegion(
+	IN PUCHAR CntryCode)
+{
+	INT loop = 0;
+	PCH_REGION pChRegion = NULL;
+
+	while (strcmp(ChRegion[loop].CountReg, "") != 0)
+	{
+		if (strncmp(ChRegion[loop].CountReg, CntryCode, 2) == 0)
+		{
+			pChRegion = &ChRegion[loop];
+			break;
+		}
+		loop++;
+	}
+
+	if (pChRegion == NULL)
+		pChRegion = &ChRegion[loop];
+	return pChRegion;
+}
+
+static inline VOID ChBandCheck(
+	IN UCHAR PhyMode,
+	OUT PUCHAR pChType)
+{
+	switch(PhyMode)
+	{
+		case PHY_11A:
+#ifdef DOT11_N_SUPPORT
+		case PHY_11AN_MIXED:
+#endif // DOT11_N_SUPPORT //
+			*pChType = BAND_5G;
+			break;
+		case PHY_11ABG_MIXED:
+#ifdef DOT11_N_SUPPORT
+		case PHY_11AGN_MIXED:
+		case PHY_11ABGN_MIXED:
+#endif // DOT11_N_SUPPORT //
+			*pChType = BAND_BOTH;
+			break;
+
+		default:
+			*pChType = BAND_24G;
+			break;
+	}
+}
+
+static inline UCHAR FillChList(
+	IN PRTMP_ADAPTER pAd,
+	IN PCH_DESP pChDesp,
+	IN UCHAR Offset,
+	IN UCHAR increment)
+{
+	INT i, j, l;
+	UCHAR channel;
+
+	j = Offset;
+	for (i = 0; i < pChDesp->NumOfCh; i++)
+	{
+		channel = pChDesp->FirstChannel + i * increment;
+		for (l=0; l<MAX_NUM_OF_CHANNELS; l++)
+		{
+			if (channel == pAd->TxPower[l].Channel)
+			{
+				pAd->ChannelList[j].Power = pAd->TxPower[l].Power;
+				pAd->ChannelList[j].Power2 = pAd->TxPower[l].Power2;
+				break;
+			}
+		}
+		if (l == MAX_NUM_OF_CHANNELS)
+			continue;
+
+		pAd->ChannelList[j].Channel = pChDesp->FirstChannel + i * increment;
+		pAd->ChannelList[j].MaxTxPwr = pChDesp->MaxTxPwr;
+		pAd->ChannelList[j].DfsReq = pChDesp->DfsReq;
+		j++;
+	}
+	pAd->ChannelListNum = j;
+
+	return j;
+}
+
+static inline VOID CreateChList(
+	IN PRTMP_ADAPTER pAd,
+	IN PCH_REGION pChRegion,
+	IN UCHAR Geography)
+{
+	INT i;
+	UCHAR offset = 0;
+	PCH_DESP pChDesp;
+	UCHAR ChType;
+	UCHAR increment;
+
+	if (pChRegion == NULL)
+		return;
+
+	ChBandCheck(pAd->CommonCfg.PhyMode, &ChType);
+
+	for (i=0; i<10; i++)
+	{
+		pChDesp = &pChRegion->ChDesp[i];
+		if (pChDesp->FirstChannel == 0)
+			break;
+
+		if (ChType == BAND_5G)
+		{
+			if (pChDesp->FirstChannel <= 14)
+				continue;
+		}
+		else if (ChType == BAND_24G)
+		{
+			if (pChDesp->FirstChannel > 14)
+				continue;
+		}
+
+		if ((pChDesp->Geography == BOTH)
+			|| (pChDesp->Geography == Geography))
+        {
+			if (pChDesp->FirstChannel > 14)
+                increment = 4;
+            else
+                increment = 1;
+			offset = FillChList(pAd, pChDesp, offset, increment);
+        }
+	}
+}
+
+static inline VOID BuildChannelListEx(
+	IN PRTMP_ADAPTER pAd)
+{
+	PCH_REGION pChReg;
+
+	pChReg = GetChRegion(pAd->CommonCfg.CountryCode);
+	CreateChList(pAd, pChReg, pAd->CommonCfg.Geography);
+}
+
+static inline VOID BuildBeaconChList(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pBuf,
+	OUT	PULONG pBufLen)
+{
+	INT i;
+	ULONG TmpLen;
+	PCH_REGION pChRegion;
+	PCH_DESP pChDesp;
+	UCHAR ChType;
+
+	pChRegion = GetChRegion(pAd->CommonCfg.CountryCode);
+
+	if (pChRegion == NULL)
+		return;
+
+	ChBandCheck(pAd->CommonCfg.PhyMode, &ChType);
+	*pBufLen = 0;
+
+	for (i=0; i<10; i++)
+	{
+		pChDesp = &pChRegion->ChDesp[i];
+		if (pChDesp->FirstChannel == 0)
+			break;
+
+		if (ChType == BAND_5G)
+		{
+			if (pChDesp->FirstChannel <= 14)
+				continue;
+		}
+		else if (ChType == BAND_24G)
+		{
+			if (pChDesp->FirstChannel > 14)
+				continue;
+		}
+
+		if ((pChDesp->Geography == BOTH)
+			|| (pChDesp->Geography == pAd->CommonCfg.Geography))
+		{
+			MakeOutgoingFrame(pBuf + *pBufLen,		&TmpLen,
+								1,                 	&pChDesp->FirstChannel,
+								1,                 	&pChDesp->NumOfCh,
+								1,                 	&pChDesp->MaxTxPwr,
+								END_OF_ARGS);
+			*pBufLen += TmpLen;
+		}
+	}
+}
+
+
+#ifdef DOT11_N_SUPPORT
+static inline BOOLEAN IsValidChannel(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR channel)
+
+{
+	INT i;
+
+	for (i = 0; i < pAd->ChannelListNum; i++)
+	{
+		if (pAd->ChannelList[i].Channel == channel)
+			break;
+	}
+
+	if (i == pAd->ChannelListNum)
+		return FALSE;
+	else
+		return TRUE;
+}
+
+
+static inline UCHAR GetExtCh(
+	IN UCHAR Channel,
+	IN UCHAR Direction)
+{
+	CHAR ExtCh;
+
+	if (Direction == EXTCHA_ABOVE)
+		ExtCh = Channel + 4;
+	else
+		ExtCh = (Channel - 4) > 0 ? (Channel - 4) : 0;
+
+	return ExtCh;
+}
+
+
+static inline VOID N_ChannelCheck(
+	IN PRTMP_ADAPTER pAd)
+{
+	//UCHAR ChannelNum = pAd->ChannelListNum;
+	UCHAR Channel = pAd->CommonCfg.Channel;
+
+	if ((pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED) && (pAd->CommonCfg.RegTransmitSetting.field.BW  == BW_40))
+	{
+		if (Channel > 14)
+		{
+			if ((Channel == 36) || (Channel == 44) || (Channel == 52) || (Channel == 60) || (Channel == 100) || (Channel == 108) ||
+			    (Channel == 116) || (Channel == 124) || (Channel == 132) || (Channel == 149) || (Channel == 157))
+			{
+				pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_ABOVE;
+			}
+			else if ((Channel == 40) || (Channel == 48) || (Channel == 56) || (Channel == 64) || (Channel == 104) || (Channel == 112) ||
+					(Channel == 120) || (Channel == 128) || (Channel == 136) || (Channel == 153) || (Channel == 161))
+			{
+				pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_BELOW;
+			}
+			else
+			{
+				pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_20;
+			}
+		}
+		else
+		{
+			do
+			{
+				UCHAR ExtCh;
+				UCHAR Dir = pAd->CommonCfg.RegTransmitSetting.field.EXTCHA;
+				ExtCh = GetExtCh(Channel, Dir);
+				if (IsValidChannel(pAd, ExtCh))
+					break;
+
+				Dir = (Dir == EXTCHA_ABOVE) ? EXTCHA_BELOW : EXTCHA_ABOVE;
+				ExtCh = GetExtCh(Channel, Dir);
+				if (IsValidChannel(pAd, ExtCh))
+				{
+					pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = Dir;
+					break;
+				}
+				pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_20;
+			} while(FALSE);
+
+			if (Channel == 14)
+			{
+				pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_20;
+				//pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_NONE;	// We didn't set the ExtCh as NONE due to it'll set in RTMPSetHT()
+			}
+#if 0
+			switch (pAd->CommonCfg.CountryRegion  & 0x7f)
+			{
+				case REGION_0_BG_BAND:	// 1 -11
+				case REGION_1_BG_BAND:	// 1 - 13
+				case REGION_5_BG_BAND:	// 1 - 14
+					if (Channel <= 4)
+					{
+						pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_ABOVE;
+					}
+					else if (Channel >= 8)
+					{
+						if ((ChannelNum - Channel) < 4)
+							pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_BELOW;
+					}
+					break;
+
+				case REGION_2_BG_BAND:	// 10 - 11
+				case REGION_3_BG_BAND:	// 10 - 13
+				case REGION_4_BG_BAND:	// 14
+					pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_20;
+					break;
+
+				case REGION_6_BG_BAND:	// 3 - 9
+					if (Channel <= 5)
+						pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_ABOVE;
+					else if (Channel == 6)
+						pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_20;
+					else if (Channel >= 7)
+						pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_BELOW;
+					break;
+
+				case REGION_7_BG_BAND:  // 5 - 13
+					if (Channel <= 8)
+						pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_ABOVE;
+					else if (Channel >= 10)
+						pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_BELOW;
+					break;
+
+				default:	// Error. should never happen
+					break;
+			}
+#endif
+		}
+	}
+
+
+}
+
+
+static inline VOID N_SetCenCh(
+	IN PRTMP_ADAPTER pAd)
+{
+	if (pAd->CommonCfg.RegTransmitSetting.field.BW == BW_40)
+	{
+		if (pAd->CommonCfg.RegTransmitSetting.field.EXTCHA == EXTCHA_ABOVE)
+		{
+			pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel + 2;
+		}
+		else
+		{
+			if (pAd->CommonCfg.Channel == 14)
+				pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel - 1;
+			else
+				pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel - 2;
+		}
+	}
+	else
+	{
+		pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel;
+	}
+}
+#endif // DOT11_N_SUPPORT //
+
+
+static inline UINT8 GetCuntryMaxTxPwr(
+	IN PRTMP_ADAPTER pAd,
+	IN UINT8 channel)
+{
+	int i;
+	for (i = 0; i < pAd->ChannelListNum; i++)
+	{
+		if (pAd->ChannelList[i].Channel == channel)
+			break;
+	}
+
+	if (i == pAd->ChannelListNum)
+		return 0xff;
+	else
+		return pAd->ChannelList[i].MaxTxPwr;
+}
+#endif // __CHLIST_H__
+
diff --git a/drivers/staging/rt2860/common/2860_rtmp_init.c b/drivers/staging/rt2860/common/2860_rtmp_init.c
new file mode 100644
index 00000000000..546f304ec33
--- /dev/null
+++ b/drivers/staging/rt2860/common/2860_rtmp_init.c
@@ -0,0 +1,922 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	2860_rtmp_init.c
+
+	Abstract:
+	Miniport generic portion header file
+
+	Revision History:
+	Who         When          What
+	--------    ----------    ----------------------------------------------
+	Paul Lin    2002-08-01    created
+    John Chang  2004-08-20    RT2561/2661 use scatter-gather scheme
+    Jan Lee  2006-09-15    RT2860. Change for 802.11n , EEPROM, Led, BA, HT.
+*/
+#include "../rt_config.h"
+
+
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Allocate DMA memory blocks for send, receive
+
+	Arguments:
+		Adapter		Pointer to our adapter
+
+	Return Value:
+		NDIS_STATUS_SUCCESS
+		NDIS_STATUS_FAILURE
+		NDIS_STATUS_RESOURCES
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS	RTMPAllocTxRxRingMemory(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	NDIS_STATUS		Status = NDIS_STATUS_SUCCESS;
+	ULONG			RingBasePaHigh;
+	ULONG			RingBasePaLow;
+	PVOID			RingBaseVa;
+	INT				index, num;
+	PTXD_STRUC		pTxD;
+	PRXD_STRUC		pRxD;
+	ULONG			ErrorValue = 0;
+	PRTMP_TX_RING	pTxRing;
+	PRTMP_DMABUF	pDmaBuf;
+	PNDIS_PACKET	pPacket;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> RTMPAllocTxRxRingMemory\n"));
+	do
+	{
+		//
+		// Allocate all ring descriptors, include TxD, RxD, MgmtD.
+		// Although each size is different, to prevent cacheline and alignment
+		// issue, I intentional set them all to 64 bytes.
+		//
+		for (num=0; num<NUM_OF_TX_RING; num++)
+		{
+			ULONG  BufBasePaHigh;
+			ULONG  BufBasePaLow;
+			PVOID  BufBaseVa;
+
+			//
+			// Allocate Tx ring descriptor's memory (5 TX rings = 4 ACs + 1 HCCA)
+			//
+			pAd->TxDescRing[num].AllocSize = TX_RING_SIZE * TXD_SIZE;
+			RTMP_AllocateTxDescMemory(
+				pAd,
+				num,
+				pAd->TxDescRing[num].AllocSize,
+				FALSE,
+				&pAd->TxDescRing[num].AllocVa,
+				&pAd->TxDescRing[num].AllocPa);
+
+			if (pAd->TxDescRing[num].AllocVa == NULL)
+			{
+				ErrorValue = ERRLOG_OUT_OF_SHARED_MEMORY;
+				DBGPRINT_ERR(("Failed to allocate a big buffer\n"));
+				Status = NDIS_STATUS_RESOURCES;
+				break;
+			}
+
+			// Zero init this memory block
+			NdisZeroMemory(pAd->TxDescRing[num].AllocVa, pAd->TxDescRing[num].AllocSize);
+
+			// Save PA & VA for further operation
+			RingBasePaHigh = RTMP_GetPhysicalAddressHigh(pAd->TxDescRing[num].AllocPa);
+			RingBasePaLow  = RTMP_GetPhysicalAddressLow (pAd->TxDescRing[num].AllocPa);
+			RingBaseVa     = pAd->TxDescRing[num].AllocVa;
+
+			//
+			// Allocate all 1st TXBuf's memory for this TxRing
+			//
+			pAd->TxBufSpace[num].AllocSize = TX_RING_SIZE * TX_DMA_1ST_BUFFER_SIZE;
+			RTMP_AllocateFirstTxBuffer(
+				pAd,
+				num,
+				pAd->TxBufSpace[num].AllocSize,
+				FALSE,
+				&pAd->TxBufSpace[num].AllocVa,
+				&pAd->TxBufSpace[num].AllocPa);
+
+			if (pAd->TxBufSpace[num].AllocVa == NULL)
+			{
+				ErrorValue = ERRLOG_OUT_OF_SHARED_MEMORY;
+				DBGPRINT_ERR(("Failed to allocate a big buffer\n"));
+				Status = NDIS_STATUS_RESOURCES;
+				break;
+			}
+
+			// Zero init this memory block
+			NdisZeroMemory(pAd->TxBufSpace[num].AllocVa, pAd->TxBufSpace[num].AllocSize);
+
+			// Save PA & VA for further operation
+			BufBasePaHigh = RTMP_GetPhysicalAddressHigh(pAd->TxBufSpace[num].AllocPa);
+			BufBasePaLow  = RTMP_GetPhysicalAddressLow (pAd->TxBufSpace[num].AllocPa);
+			BufBaseVa     = pAd->TxBufSpace[num].AllocVa;
+
+			//
+			// Initialize Tx Ring Descriptor and associated buffer memory
+			//
+			pTxRing = &pAd->TxRing[num];
+			for (index = 0; index < TX_RING_SIZE; index++)
+			{
+				pTxRing->Cell[index].pNdisPacket = NULL;
+				pTxRing->Cell[index].pNextNdisPacket = NULL;
+				// Init Tx Ring Size, Va, Pa variables
+				pTxRing->Cell[index].AllocSize = TXD_SIZE;
+				pTxRing->Cell[index].AllocVa = RingBaseVa;
+				RTMP_SetPhysicalAddressHigh(pTxRing->Cell[index].AllocPa, RingBasePaHigh);
+				RTMP_SetPhysicalAddressLow (pTxRing->Cell[index].AllocPa, RingBasePaLow);
+
+				// Setup Tx Buffer size & address. only 802.11 header will store in this space
+				pDmaBuf = &pTxRing->Cell[index].DmaBuf;
+				pDmaBuf->AllocSize = TX_DMA_1ST_BUFFER_SIZE;
+				pDmaBuf->AllocVa = BufBaseVa;
+				RTMP_SetPhysicalAddressHigh(pDmaBuf->AllocPa, BufBasePaHigh);
+				RTMP_SetPhysicalAddressLow(pDmaBuf->AllocPa, BufBasePaLow);
+
+				// link the pre-allocated TxBuf to TXD
+				pTxD = (PTXD_STRUC) pTxRing->Cell[index].AllocVa;
+				pTxD->SDPtr0 = BufBasePaLow;
+				// advance to next ring descriptor address
+				pTxD->DMADONE = 1;
+#ifdef RT_BIG_ENDIAN
+				RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+#endif
+				RingBasePaLow += TXD_SIZE;
+				RingBaseVa = (PUCHAR) RingBaseVa + TXD_SIZE;
+
+				// advance to next TxBuf address
+				BufBasePaLow += TX_DMA_1ST_BUFFER_SIZE;
+				BufBaseVa = (PUCHAR) BufBaseVa + TX_DMA_1ST_BUFFER_SIZE;
+			}
+			DBGPRINT(RT_DEBUG_TRACE, ("TxRing[%d]: total %d entry allocated\n", num, index));
+		}
+		if (Status == NDIS_STATUS_RESOURCES)
+			break;
+
+		//
+		// Allocate MGMT ring descriptor's memory except Tx ring which allocated eariler
+		//
+		pAd->MgmtDescRing.AllocSize = MGMT_RING_SIZE * TXD_SIZE;
+		RTMP_AllocateMgmtDescMemory(
+			pAd,
+			pAd->MgmtDescRing.AllocSize,
+			FALSE,
+			&pAd->MgmtDescRing.AllocVa,
+			&pAd->MgmtDescRing.AllocPa);
+
+		if (pAd->MgmtDescRing.AllocVa == NULL)
+		{
+			ErrorValue = ERRLOG_OUT_OF_SHARED_MEMORY;
+			DBGPRINT_ERR(("Failed to allocate a big buffer\n"));
+			Status = NDIS_STATUS_RESOURCES;
+			break;
+		}
+
+		// Zero init this memory block
+		NdisZeroMemory(pAd->MgmtDescRing.AllocVa, pAd->MgmtDescRing.AllocSize);
+
+		// Save PA & VA for further operation
+		RingBasePaHigh = RTMP_GetPhysicalAddressHigh(pAd->MgmtDescRing.AllocPa);
+		RingBasePaLow  = RTMP_GetPhysicalAddressLow (pAd->MgmtDescRing.AllocPa);
+		RingBaseVa     = pAd->MgmtDescRing.AllocVa;
+
+		//
+		// Initialize MGMT Ring and associated buffer memory
+		//
+		for (index = 0; index < MGMT_RING_SIZE; index++)
+		{
+			pAd->MgmtRing.Cell[index].pNdisPacket = NULL;
+			pAd->MgmtRing.Cell[index].pNextNdisPacket = NULL;
+			// Init MGMT Ring Size, Va, Pa variables
+			pAd->MgmtRing.Cell[index].AllocSize = TXD_SIZE;
+			pAd->MgmtRing.Cell[index].AllocVa = RingBaseVa;
+			RTMP_SetPhysicalAddressHigh(pAd->MgmtRing.Cell[index].AllocPa, RingBasePaHigh);
+			RTMP_SetPhysicalAddressLow (pAd->MgmtRing.Cell[index].AllocPa, RingBasePaLow);
+
+			// Offset to next ring descriptor address
+			RingBasePaLow += TXD_SIZE;
+			RingBaseVa = (PUCHAR) RingBaseVa + TXD_SIZE;
+
+			// link the pre-allocated TxBuf to TXD
+			pTxD = (PTXD_STRUC) pAd->MgmtRing.Cell[index].AllocVa;
+			pTxD->DMADONE = 1;
+
+#ifdef RT_BIG_ENDIAN
+			RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+#endif
+			// no pre-allocated buffer required in MgmtRing for scatter-gather case
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("MGMT Ring: total %d entry allocated\n", index));
+
+		//
+		// Allocate RX ring descriptor's memory except Tx ring which allocated eariler
+		//
+		pAd->RxDescRing.AllocSize = RX_RING_SIZE * RXD_SIZE;
+		RTMP_AllocateRxDescMemory(
+			pAd,
+			pAd->RxDescRing.AllocSize,
+			FALSE,
+			&pAd->RxDescRing.AllocVa,
+			&pAd->RxDescRing.AllocPa);
+
+		if (pAd->RxDescRing.AllocVa == NULL)
+		{
+			ErrorValue = ERRLOG_OUT_OF_SHARED_MEMORY;
+			DBGPRINT_ERR(("Failed to allocate a big buffer\n"));
+			Status = NDIS_STATUS_RESOURCES;
+			break;
+		}
+
+		// Zero init this memory block
+		NdisZeroMemory(pAd->RxDescRing.AllocVa, pAd->RxDescRing.AllocSize);
+
+
+		printk("RX DESC %p  size = %ld\n", pAd->RxDescRing.AllocVa,
+					pAd->RxDescRing.AllocSize);
+
+		// Save PA & VA for further operation
+		RingBasePaHigh = RTMP_GetPhysicalAddressHigh(pAd->RxDescRing.AllocPa);
+		RingBasePaLow  = RTMP_GetPhysicalAddressLow (pAd->RxDescRing.AllocPa);
+		RingBaseVa     = pAd->RxDescRing.AllocVa;
+
+		//
+		// Initialize Rx Ring and associated buffer memory
+		//
+		for (index = 0; index < RX_RING_SIZE; index++)
+		{
+			// Init RX Ring Size, Va, Pa variables
+			pAd->RxRing.Cell[index].AllocSize = RXD_SIZE;
+			pAd->RxRing.Cell[index].AllocVa = RingBaseVa;
+			RTMP_SetPhysicalAddressHigh(pAd->RxRing.Cell[index].AllocPa, RingBasePaHigh);
+			RTMP_SetPhysicalAddressLow (pAd->RxRing.Cell[index].AllocPa, RingBasePaLow);
+
+			// Offset to next ring descriptor address
+			RingBasePaLow += RXD_SIZE;
+			RingBaseVa = (PUCHAR) RingBaseVa + RXD_SIZE;
+
+			// Setup Rx associated Buffer size & allocate share memory
+			pDmaBuf = &pAd->RxRing.Cell[index].DmaBuf;
+			pDmaBuf->AllocSize = RX_BUFFER_AGGRESIZE;
+			pPacket = RTMP_AllocateRxPacketBuffer(
+				pAd,
+				pDmaBuf->AllocSize,
+				FALSE,
+				&pDmaBuf->AllocVa,
+				&pDmaBuf->AllocPa);
+
+			/* keep allocated rx packet */
+			pAd->RxRing.Cell[index].pNdisPacket = pPacket;
+
+			// Error handling
+			if (pDmaBuf->AllocVa == NULL)
+			{
+				ErrorValue = ERRLOG_OUT_OF_SHARED_MEMORY;
+				DBGPRINT_ERR(("Failed to allocate RxRing's 1st buffer\n"));
+				Status = NDIS_STATUS_RESOURCES;
+				break;
+			}
+
+			// Zero init this memory block
+			NdisZeroMemory(pDmaBuf->AllocVa, pDmaBuf->AllocSize);
+
+			// Write RxD buffer address & allocated buffer length
+			pRxD = (PRXD_STRUC) pAd->RxRing.Cell[index].AllocVa;
+			pRxD->SDP0 = RTMP_GetPhysicalAddressLow(pDmaBuf->AllocPa);
+			pRxD->DDONE = 0;
+
+#ifdef RT_BIG_ENDIAN
+			RTMPDescriptorEndianChange((PUCHAR)pRxD, TYPE_RXD);
+#endif
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("Rx Ring: total %d entry allocated\n", index));
+
+	}	while (FALSE);
+
+
+	NdisZeroMemory(&pAd->FragFrame, sizeof(FRAGMENT_FRAME));
+	pAd->FragFrame.pFragPacket =  RTMP_AllocateFragPacketBuffer(pAd, RX_BUFFER_NORMSIZE);
+
+	if (pAd->FragFrame.pFragPacket == NULL)
+	{
+		Status = NDIS_STATUS_RESOURCES;
+	}
+
+	if (Status != NDIS_STATUS_SUCCESS)
+	{
+		// Log error inforamtion
+		NdisWriteErrorLogEntry(
+			pAd->AdapterHandle,
+			NDIS_ERROR_CODE_OUT_OF_RESOURCES,
+			1,
+			ErrorValue);
+	}
+
+	DBGPRINT_S(Status, ("<-- RTMPAllocTxRxRingMemory, Status=%x\n", Status));
+	return Status;
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Initialize transmit data structures
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+		Initialize all transmit releated private buffer, include those define
+		in RTMP_ADAPTER structure and all private data structures.
+
+	========================================================================
+*/
+VOID	NICInitTxRxRingAndBacklogQueue(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	//WPDMA_GLO_CFG_STRUC	GloCfg;
+	int i;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<--> NICInitTxRxRingAndBacklogQueue\n"));
+
+	// Initialize all transmit related software queues
+	InitializeQueueHeader(&pAd->TxSwQueue[QID_AC_BE]);
+	InitializeQueueHeader(&pAd->TxSwQueue[QID_AC_BK]);
+	InitializeQueueHeader(&pAd->TxSwQueue[QID_AC_VI]);
+	InitializeQueueHeader(&pAd->TxSwQueue[QID_AC_VO]);
+	InitializeQueueHeader(&pAd->TxSwQueue[QID_HCCA]);
+
+	// Init RX Ring index pointer
+	pAd->RxRing.RxSwReadIdx = 0;
+	pAd->RxRing.RxCpuIdx = RX_RING_SIZE - 1;
+
+	// Init TX rings index pointer
+		for (i=0; i<NUM_OF_TX_RING; i++)
+		{
+			pAd->TxRing[i].TxSwFreeIdx = 0;
+			pAd->TxRing[i].TxCpuIdx = 0;
+		}
+
+	// init MGMT ring index pointer
+	pAd->MgmtRing.TxSwFreeIdx = 0;
+	pAd->MgmtRing.TxCpuIdx = 0;
+
+	pAd->PrivateInfo.TxRingFullCnt       = 0;
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Reset NIC Asics. Call after rest DMA. So reset TX_CTX_IDX to zero.
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+		Reset NIC to initial state AS IS system boot up time.
+
+	========================================================================
+*/
+VOID	RTMPRingCleanUp(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			RingType)
+{
+	PTXD_STRUC		pTxD;
+	PRXD_STRUC		pRxD;
+	PQUEUE_ENTRY	pEntry;
+	PNDIS_PACKET	pPacket;
+	int				i;
+	PRTMP_TX_RING	pTxRing;
+	unsigned long	IrqFlags;
+
+	DBGPRINT(RT_DEBUG_TRACE,("RTMPRingCleanUp(RingIdx=%d, Pending-NDIS=%ld)\n", RingType, pAd->RalinkCounters.PendingNdisPacketCount));
+	switch (RingType)
+	{
+		case QID_AC_BK:
+		case QID_AC_BE:
+		case QID_AC_VI:
+		case QID_AC_VO:
+		case QID_HCCA:
+			RTMP_IRQ_LOCK(&pAd->irq_lock, IrqFlags);
+			pTxRing = &pAd->TxRing[RingType];
+
+			// We have to clean all descriptors in case some error happened with reset
+			for (i=0; i<TX_RING_SIZE; i++) // We have to scan all TX ring
+			{
+				pTxD  = (PTXD_STRUC) pTxRing->Cell[i].AllocVa;
+
+				pPacket = (PNDIS_PACKET) pTxRing->Cell[i].pNdisPacket;
+				// release scatter-and-gather NDIS_PACKET
+				if (pPacket)
+				{
+					RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+					pTxRing->Cell[i].pNdisPacket = NULL;
+				}
+
+				pPacket = (PNDIS_PACKET) pTxRing->Cell[i].pNextNdisPacket;
+				// release scatter-and-gather NDIS_PACKET
+				if (pPacket)
+				{
+					RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+					pTxRing->Cell[i].pNextNdisPacket = NULL;
+				}
+			}
+
+			RTMP_IO_READ32(pAd, TX_DTX_IDX0 + RingType * 0x10, &pTxRing->TxDmaIdx);
+			pTxRing->TxSwFreeIdx = pTxRing->TxDmaIdx;
+			pTxRing->TxCpuIdx = pTxRing->TxDmaIdx;
+			RTMP_IO_WRITE32(pAd, TX_CTX_IDX0 + RingType * 0x10, pTxRing->TxCpuIdx);
+
+			RTMP_IRQ_UNLOCK(&pAd->irq_lock, IrqFlags);
+			RTMP_IRQ_LOCK(&pAd->irq_lock, IrqFlags);
+			while (pAd->TxSwQueue[RingType].Head != NULL)
+			{
+				pEntry = RemoveHeadQueue(&pAd->TxSwQueue[RingType]);
+				pPacket = QUEUE_ENTRY_TO_PACKET(pEntry);
+				RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+				DBGPRINT(RT_DEBUG_TRACE,("Release 1 NDIS packet from s/w backlog queue\n"));
+			}
+			RTMP_IRQ_UNLOCK(&pAd->irq_lock, IrqFlags);
+			break;
+
+		case QID_MGMT:
+			// We have to clean all descriptors in case some error happened with reset
+			NdisAcquireSpinLock(&pAd->MgmtRingLock);
+
+			for (i=0; i<MGMT_RING_SIZE; i++)
+			{
+				pTxD  = (PTXD_STRUC) pAd->MgmtRing.Cell[i].AllocVa;
+
+				pPacket = (PNDIS_PACKET) pAd->MgmtRing.Cell[i].pNdisPacket;
+				// rlease scatter-and-gather NDIS_PACKET
+				if (pPacket)
+				{
+					PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr0, pTxD->SDLen0, PCI_DMA_TODEVICE);
+					RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+				}
+				pAd->MgmtRing.Cell[i].pNdisPacket = NULL;
+
+				pPacket = (PNDIS_PACKET) pAd->MgmtRing.Cell[i].pNextNdisPacket;
+				// release scatter-and-gather NDIS_PACKET
+				if (pPacket)
+				{
+					PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr1, pTxD->SDLen1, PCI_DMA_TODEVICE);
+					RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+				}
+				pAd->MgmtRing.Cell[i].pNextNdisPacket = NULL;
+
+			}
+
+			RTMP_IO_READ32(pAd, TX_MGMTDTX_IDX, &pAd->MgmtRing.TxDmaIdx);
+			pAd->MgmtRing.TxSwFreeIdx = pAd->MgmtRing.TxDmaIdx;
+			pAd->MgmtRing.TxCpuIdx = pAd->MgmtRing.TxDmaIdx;
+			RTMP_IO_WRITE32(pAd, TX_MGMTCTX_IDX, pAd->MgmtRing.TxCpuIdx);
+
+			NdisReleaseSpinLock(&pAd->MgmtRingLock);
+			pAd->RalinkCounters.MgmtRingFullCount = 0;
+			break;
+
+		case QID_RX:
+			// We have to clean all descriptors in case some error happened with reset
+			NdisAcquireSpinLock(&pAd->RxRingLock);
+
+			for (i=0; i<RX_RING_SIZE; i++)
+			{
+				pRxD  = (PRXD_STRUC) pAd->RxRing.Cell[i].AllocVa;
+				pRxD->DDONE = 0 ;
+			}
+
+			RTMP_IO_READ32(pAd, RX_DRX_IDX, &pAd->RxRing.RxDmaIdx);
+			pAd->RxRing.RxSwReadIdx = pAd->RxRing.RxDmaIdx;
+			pAd->RxRing.RxCpuIdx = ((pAd->RxRing.RxDmaIdx == 0) ? (RX_RING_SIZE-1) : (pAd->RxRing.RxDmaIdx-1));
+			RTMP_IO_WRITE32(pAd, RX_CRX_IDX, pAd->RxRing.RxCpuIdx);
+
+			NdisReleaseSpinLock(&pAd->RxRingLock);
+			break;
+
+		default:
+			break;
+	}
+}
+
+
+NDIS_STATUS AdapterBlockAllocateMemory(
+	IN PVOID	handle,
+	OUT	PVOID	*ppAd)
+{
+	PPCI_DEV pci_dev;
+	dma_addr_t	*phy_addr;
+	POS_COOKIE pObj = (POS_COOKIE) handle;
+
+	pci_dev = pObj->pci_dev;
+	phy_addr = &pObj->pAd_pa;
+
+	*ppAd = (PVOID)vmalloc(sizeof(RTMP_ADAPTER)); //pci_alloc_consistent(pci_dev, sizeof(RTMP_ADAPTER), phy_addr);
+
+	if (*ppAd)
+	{
+		NdisZeroMemory(*ppAd, sizeof(RTMP_ADAPTER));
+		((PRTMP_ADAPTER)*ppAd)->OS_Cookie = handle;
+		return (NDIS_STATUS_SUCCESS);
+	} else {
+		return (NDIS_STATUS_FAILURE);
+	}
+}
+
+
+void RTMP_AllocateTxDescMemory(
+	IN	PRTMP_ADAPTER pAd,
+	IN	UINT	Index,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress,
+	OUT	PNDIS_PHYSICAL_ADDRESS PhysicalAddress)
+{
+	POS_COOKIE pObj = (POS_COOKIE)pAd->OS_Cookie;
+
+	*VirtualAddress = (PVOID)PCI_ALLOC_CONSISTENT(pObj->pci_dev,sizeof(char)*Length, PhysicalAddress);
+
+}
+
+void RTMP_AllocateMgmtDescMemory(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress,
+	OUT	PNDIS_PHYSICAL_ADDRESS PhysicalAddress)
+{
+	POS_COOKIE pObj = (POS_COOKIE)pAd->OS_Cookie;
+
+	*VirtualAddress = (PVOID)PCI_ALLOC_CONSISTENT(pObj->pci_dev,sizeof(char)*Length, PhysicalAddress);
+
+}
+
+void RTMP_AllocateRxDescMemory(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress,
+	OUT	PNDIS_PHYSICAL_ADDRESS PhysicalAddress)
+{
+	POS_COOKIE pObj = (POS_COOKIE)pAd->OS_Cookie;
+
+	*VirtualAddress = (PVOID)PCI_ALLOC_CONSISTENT(pObj->pci_dev,sizeof(char)*Length, PhysicalAddress);
+
+}
+
+void RTMP_FreeRxDescMemory(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length,
+	IN	PVOID	VirtualAddress,
+	IN	NDIS_PHYSICAL_ADDRESS PhysicalAddress)
+{
+	POS_COOKIE pObj = (POS_COOKIE)pAd->OS_Cookie;
+
+	PCI_FREE_CONSISTENT(pObj->pci_dev, Length, VirtualAddress, PhysicalAddress);
+}
+
+
+void RTMP_AllocateFirstTxBuffer(
+	IN	PRTMP_ADAPTER pAd,
+	IN	UINT	Index,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress,
+	OUT	PNDIS_PHYSICAL_ADDRESS PhysicalAddress)
+{
+	POS_COOKIE pObj = (POS_COOKIE)pAd->OS_Cookie;
+
+	*VirtualAddress = (PVOID)PCI_ALLOC_CONSISTENT(pObj->pci_dev,sizeof(char)*Length, PhysicalAddress);
+}
+
+/*
+ * FUNCTION: Allocate a common buffer for DMA
+ * ARGUMENTS:
+ *     AdapterHandle:  AdapterHandle
+ *     Length:  Number of bytes to allocate
+ *     Cached:  Whether or not the memory can be cached
+ *     VirtualAddress:  Pointer to memory is returned here
+ *     PhysicalAddress:  Physical address corresponding to virtual address
+ */
+
+void RTMP_AllocateSharedMemory(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress,
+	OUT	PNDIS_PHYSICAL_ADDRESS PhysicalAddress)
+{
+	POS_COOKIE pObj = (POS_COOKIE)pAd->OS_Cookie;
+
+	*VirtualAddress = (PVOID)PCI_ALLOC_CONSISTENT(pObj->pci_dev,sizeof(char)*Length, PhysicalAddress);
+}
+
+VOID RTMPFreeTxRxRingMemory(
+    IN  PRTMP_ADAPTER   pAd)
+{
+	int index, num , j;
+	PRTMP_TX_RING pTxRing;
+	PTXD_STRUC	  pTxD;
+	PNDIS_PACKET  pPacket;
+	unsigned int  IrqFlags;
+
+	POS_COOKIE pObj =(POS_COOKIE) pAd->OS_Cookie;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> RTMPFreeTxRxRingMemory\n"));
+
+	// Free TxSwQueue Packet
+	for (index=0; index <NUM_OF_TX_RING; index++)
+	{
+		PQUEUE_ENTRY pEntry;
+		PNDIS_PACKET pPacket;
+		PQUEUE_HEADER   pQueue;
+
+		RTMP_IRQ_LOCK(&pAd->irq_lock, IrqFlags);
+		pQueue = &pAd->TxSwQueue[index];
+		while (pQueue->Head)
+		{
+			pEntry = RemoveHeadQueue(pQueue);
+			pPacket = QUEUE_ENTRY_TO_PACKET(pEntry);
+			RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+		}
+		RTMP_IRQ_UNLOCK(&pAd->irq_lock, IrqFlags);
+	}
+
+	// Free Tx Ring Packet
+	for (index=0;index< NUM_OF_TX_RING;index++)
+	{
+		pTxRing = &pAd->TxRing[index];
+
+		for (j=0; j< TX_RING_SIZE; j++)
+		{
+			pTxD = (PTXD_STRUC) (pTxRing->Cell[j].AllocVa);
+			pPacket = pTxRing->Cell[j].pNdisPacket;
+
+			if (pPacket)
+			{
+            	PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr0, pTxD->SDLen0, PCI_DMA_TODEVICE);
+				RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_SUCCESS);
+			}
+			//Always assign pNdisPacket as NULL after clear
+			pTxRing->Cell[j].pNdisPacket = NULL;
+
+			pPacket = pTxRing->Cell[j].pNextNdisPacket;
+
+			if (pPacket)
+			{
+            	PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr1, pTxD->SDLen1, PCI_DMA_TODEVICE);
+				RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_SUCCESS);
+			}
+			//Always assign pNextNdisPacket as NULL after clear
+			pTxRing->Cell[pTxRing->TxSwFreeIdx].pNextNdisPacket = NULL;
+
+		}
+	}
+
+	for (index = RX_RING_SIZE - 1 ; index >= 0; index--)
+	{
+		if ((pAd->RxRing.Cell[index].DmaBuf.AllocVa) && (pAd->RxRing.Cell[index].pNdisPacket))
+		{
+			PCI_UNMAP_SINGLE(pObj->pci_dev, pAd->RxRing.Cell[index].DmaBuf.AllocPa, pAd->RxRing.Cell[index].DmaBuf.AllocSize, PCI_DMA_FROMDEVICE);
+			RELEASE_NDIS_PACKET(pAd, pAd->RxRing.Cell[index].pNdisPacket, NDIS_STATUS_SUCCESS);
+		}
+	}
+	NdisZeroMemory(pAd->RxRing.Cell, RX_RING_SIZE * sizeof(RTMP_DMACB));
+
+	if (pAd->RxDescRing.AllocVa)
+    {
+    	PCI_FREE_CONSISTENT(pObj->pci_dev, pAd->RxDescRing.AllocSize, pAd->RxDescRing.AllocVa, pAd->RxDescRing.AllocPa);
+    }
+    NdisZeroMemory(&pAd->RxDescRing, sizeof(RTMP_DMABUF));
+
+	if (pAd->MgmtDescRing.AllocVa)
+	{
+		PCI_FREE_CONSISTENT(pObj->pci_dev, pAd->MgmtDescRing.AllocSize,	pAd->MgmtDescRing.AllocVa, pAd->MgmtDescRing.AllocPa);
+	}
+	NdisZeroMemory(&pAd->MgmtDescRing, sizeof(RTMP_DMABUF));
+
+	for (num = 0; num < NUM_OF_TX_RING; num++)
+	{
+    	if (pAd->TxBufSpace[num].AllocVa)
+	    {
+	    	PCI_FREE_CONSISTENT(pObj->pci_dev, pAd->TxBufSpace[num].AllocSize, pAd->TxBufSpace[num].AllocVa, pAd->TxBufSpace[num].AllocPa);
+	    }
+	    NdisZeroMemory(&pAd->TxBufSpace[num], sizeof(RTMP_DMABUF));
+
+    	if (pAd->TxDescRing[num].AllocVa)
+	    {
+	    	PCI_FREE_CONSISTENT(pObj->pci_dev, pAd->TxDescRing[num].AllocSize, pAd->TxDescRing[num].AllocVa, pAd->TxDescRing[num].AllocPa);
+	    }
+	    NdisZeroMemory(&pAd->TxDescRing[num], sizeof(RTMP_DMABUF));
+	}
+
+	if (pAd->FragFrame.pFragPacket)
+		RELEASE_NDIS_PACKET(pAd, pAd->FragFrame.pFragPacket, NDIS_STATUS_SUCCESS);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- RTMPFreeTxRxRingMemory\n"));
+}
+
+
+/*
+ * FUNCTION: Allocate a packet buffer for DMA
+ * ARGUMENTS:
+ *     AdapterHandle:  AdapterHandle
+ *     Length:  Number of bytes to allocate
+ *     Cached:  Whether or not the memory can be cached
+ *     VirtualAddress:  Pointer to memory is returned here
+ *     PhysicalAddress:  Physical address corresponding to virtual address
+ * Notes:
+ *     Cached is ignored: always cached memory
+ */
+PNDIS_PACKET RTMP_AllocateRxPacketBuffer(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress,
+	OUT	PNDIS_PHYSICAL_ADDRESS PhysicalAddress)
+{
+	PNDIS_PACKET pkt;
+
+	pkt = RTPKT_TO_OSPKT(DEV_ALLOC_SKB(Length));
+
+	if (pkt == NULL) {
+		DBGPRINT(RT_DEBUG_ERROR, ("can't allocate rx %ld size packet\n",Length));
+	}
+
+	if (pkt) {
+		RTMP_SET_PACKET_SOURCE(pkt, PKTSRC_NDIS);
+		*VirtualAddress = (PVOID) RTPKT_TO_OSPKT(pkt)->data;
+		*PhysicalAddress = PCI_MAP_SINGLE(pAd, *VirtualAddress, Length, -1, PCI_DMA_FROMDEVICE);
+	} else {
+		*VirtualAddress = (PVOID) NULL;
+		*PhysicalAddress = (NDIS_PHYSICAL_ADDRESS) NULL;
+	}
+
+	return (PNDIS_PACKET) pkt;
+}
+
+
+VOID Invalid_Remaining_Packet(
+	IN	PRTMP_ADAPTER pAd,
+	IN	 ULONG VirtualAddress)
+{
+	NDIS_PHYSICAL_ADDRESS PhysicalAddress;
+
+	PhysicalAddress = PCI_MAP_SINGLE(pAd, (void *)(VirtualAddress+1600), RX_BUFFER_NORMSIZE-1600, -1, PCI_DMA_FROMDEVICE);
+}
+
+PNDIS_PACKET GetPacketFromRxRing(
+	IN		PRTMP_ADAPTER	pAd,
+	OUT		PRT28XX_RXD_STRUC	pSaveRxD,
+	OUT		BOOLEAN			*pbReschedule,
+	IN OUT	UINT32			*pRxPending)
+{
+	PRXD_STRUC				pRxD;
+#ifdef RT_BIG_ENDIAN
+	PRXD_STRUC				pDestRxD;
+	RXD_STRUC				RxD;
+#endif
+	PNDIS_PACKET			pRxPacket = NULL;
+	PNDIS_PACKET			pNewPacket;
+	PVOID					AllocVa;
+	NDIS_PHYSICAL_ADDRESS	AllocPa;
+	BOOLEAN					bReschedule = FALSE;
+
+	RTMP_SEM_LOCK(&pAd->RxRingLock);
+
+	if (*pRxPending == 0)
+	{
+		// Get how may packets had been received
+		RTMP_IO_READ32(pAd, RX_DRX_IDX , &pAd->RxRing.RxDmaIdx);
+
+		if (pAd->RxRing.RxSwReadIdx == pAd->RxRing.RxDmaIdx)
+		{
+			// no more rx packets
+			bReschedule = FALSE;
+			goto done;
+		}
+
+		// get rx pending count
+		if (pAd->RxRing.RxDmaIdx > pAd->RxRing.RxSwReadIdx)
+			*pRxPending = pAd->RxRing.RxDmaIdx - pAd->RxRing.RxSwReadIdx;
+		else
+			*pRxPending	= pAd->RxRing.RxDmaIdx + RX_RING_SIZE - pAd->RxRing.RxSwReadIdx;
+
+	}
+
+#ifdef RT_BIG_ENDIAN
+	pDestRxD = (PRXD_STRUC) pAd->RxRing.Cell[pAd->RxRing.RxSwReadIdx].AllocVa;
+	RxD = *pDestRxD;
+	pRxD = &RxD;
+	RTMPDescriptorEndianChange((PUCHAR)pRxD, TYPE_RXD);
+#else
+	// Point to Rx indexed rx ring descriptor
+	pRxD = (PRXD_STRUC) pAd->RxRing.Cell[pAd->RxRing.RxSwReadIdx].AllocVa;
+#endif
+
+	if (pRxD->DDONE == 0)
+	{
+		*pRxPending = 0;
+		// DMAIndx had done but DDONE bit not ready
+		bReschedule = TRUE;
+		goto done;
+	}
+
+
+	// return rx descriptor
+	NdisMoveMemory(pSaveRxD, pRxD, RXD_SIZE);
+
+	pNewPacket = RTMP_AllocateRxPacketBuffer(pAd, RX_BUFFER_AGGRESIZE, FALSE, &AllocVa, &AllocPa);
+
+	if (pNewPacket)
+	{
+		// unmap the rx buffer
+		PCI_UNMAP_SINGLE(pAd, pAd->RxRing.Cell[pAd->RxRing.RxSwReadIdx].DmaBuf.AllocPa,
+					 pAd->RxRing.Cell[pAd->RxRing.RxSwReadIdx].DmaBuf.AllocSize, PCI_DMA_FROMDEVICE);
+		pRxPacket = pAd->RxRing.Cell[pAd->RxRing.RxSwReadIdx].pNdisPacket;
+
+		pAd->RxRing.Cell[pAd->RxRing.RxSwReadIdx].DmaBuf.AllocSize	= RX_BUFFER_AGGRESIZE;
+		pAd->RxRing.Cell[pAd->RxRing.RxSwReadIdx].pNdisPacket		= (PNDIS_PACKET) pNewPacket;
+		pAd->RxRing.Cell[pAd->RxRing.RxSwReadIdx].DmaBuf.AllocVa	= AllocVa;
+		pAd->RxRing.Cell[pAd->RxRing.RxSwReadIdx].DmaBuf.AllocPa	= AllocPa;
+		/* update SDP0 to new buffer of rx packet */
+		pRxD->SDP0 = AllocPa;
+	}
+	else
+	{
+		//printk("No Rx Buffer\n");
+		pRxPacket = NULL;
+		bReschedule = TRUE;
+	}
+
+	pRxD->DDONE = 0;
+
+	// had handled one rx packet
+	*pRxPending = *pRxPending - 1;
+
+	// update rx descriptor and kick rx
+#ifdef RT_BIG_ENDIAN
+	RTMPDescriptorEndianChange((PUCHAR)pRxD, TYPE_RXD);
+	WriteBackToDescriptor((PUCHAR)pDestRxD, (PUCHAR)pRxD, FALSE, TYPE_RXD);
+#endif
+	INC_RING_INDEX(pAd->RxRing.RxSwReadIdx, RX_RING_SIZE);
+
+	pAd->RxRing.RxCpuIdx = (pAd->RxRing.RxSwReadIdx == 0) ? (RX_RING_SIZE-1) : (pAd->RxRing.RxSwReadIdx-1);
+	RTMP_IO_WRITE32(pAd, RX_CRX_IDX, pAd->RxRing.RxCpuIdx);
+
+done:
+	RTMP_SEM_UNLOCK(&pAd->RxRingLock);
+	*pbReschedule = bReschedule;
+	return pRxPacket;
+}
+/* End of 2860_rtmp_init.c */
+
diff --git a/drivers/staging/rt2860/common/action.c b/drivers/staging/rt2860/common/action.c
new file mode 100644
index 00000000000..d6f530fb857
--- /dev/null
+++ b/drivers/staging/rt2860/common/action.c
@@ -0,0 +1,1031 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+	action.c
+
+    Abstract:
+    Handle association related requests either from WSTA or from local MLME
+
+    Revision History:
+    Who         When          What
+    --------    ----------    ----------------------------------------------
+	Jan Lee		2006	  	created for rt2860
+ */
+
+#include "../rt_config.h"
+#include "action.h"
+
+
+static VOID ReservedAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+/*
+    ==========================================================================
+    Description:
+        association state machine init, including state transition and timer init
+    Parameters:
+        S - pointer to the association state machine
+    Note:
+        The state machine looks like the following
+
+                                    ASSOC_IDLE
+        MT2_MLME_DISASSOC_REQ    mlme_disassoc_req_action
+        MT2_PEER_DISASSOC_REQ    peer_disassoc_action
+        MT2_PEER_ASSOC_REQ       drop
+        MT2_PEER_REASSOC_REQ     drop
+        MT2_CLS3ERR              cls3err_action
+    ==========================================================================
+ */
+VOID ActionStateMachineInit(
+    IN	PRTMP_ADAPTER	pAd,
+    IN  STATE_MACHINE *S,
+    OUT STATE_MACHINE_FUNC Trans[])
+{
+	StateMachineInit(S, (STATE_MACHINE_FUNC *)Trans, MAX_ACT_STATE, MAX_ACT_MSG, (STATE_MACHINE_FUNC)Drop, ACT_IDLE, ACT_MACHINE_BASE);
+
+	StateMachineSetAction(S, ACT_IDLE, MT2_PEER_SPECTRUM_CATE, (STATE_MACHINE_FUNC)PeerSpectrumAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_PEER_QOS_CATE, (STATE_MACHINE_FUNC)PeerQOSAction);
+
+	StateMachineSetAction(S, ACT_IDLE, MT2_PEER_DLS_CATE, (STATE_MACHINE_FUNC)ReservedAction);
+#ifdef QOS_DLS_SUPPORT
+		StateMachineSetAction(S, ACT_IDLE, MT2_PEER_DLS_CATE, (STATE_MACHINE_FUNC)PeerDLSAction);
+#endif // QOS_DLS_SUPPORT //
+
+#ifdef DOT11_N_SUPPORT
+	StateMachineSetAction(S, ACT_IDLE, MT2_PEER_BA_CATE, (STATE_MACHINE_FUNC)PeerBAAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_PEER_HT_CATE, (STATE_MACHINE_FUNC)PeerHTAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_MLME_ADD_BA_CATE, (STATE_MACHINE_FUNC)MlmeADDBAAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_MLME_ORI_DELBA_CATE, (STATE_MACHINE_FUNC)MlmeDELBAAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_MLME_REC_DELBA_CATE, (STATE_MACHINE_FUNC)MlmeDELBAAction);
+#endif // DOT11_N_SUPPORT //
+
+	StateMachineSetAction(S, ACT_IDLE, MT2_PEER_PUBLIC_CATE, (STATE_MACHINE_FUNC)PeerPublicAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_PEER_RM_CATE, (STATE_MACHINE_FUNC)PeerRMAction);
+
+	StateMachineSetAction(S, ACT_IDLE, MT2_MLME_QOS_CATE, (STATE_MACHINE_FUNC)MlmeQOSAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_MLME_DLS_CATE, (STATE_MACHINE_FUNC)MlmeDLSAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_ACT_INVALID, (STATE_MACHINE_FUNC)MlmeInvalidAction);
+}
+
+#ifdef DOT11_N_SUPPORT
+VOID MlmeADDBAAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+
+{
+	MLME_ADDBA_REQ_STRUCT *pInfo;
+	UCHAR           Addr[6];
+	PUCHAR         pOutBuffer = NULL;
+	NDIS_STATUS     NStatus;
+	ULONG		Idx;
+	FRAME_ADDBA_REQ  Frame;
+	ULONG		FrameLen;
+	BA_ORI_ENTRY			*pBAEntry = NULL;
+
+	pInfo = (MLME_ADDBA_REQ_STRUCT *)Elem->Msg;
+	NdisZeroMemory(&Frame, sizeof(FRAME_ADDBA_REQ));
+
+	if(MlmeAddBAReqSanity(pAd, Elem->Msg, Elem->MsgLen, Addr))
+	{
+		NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+		if(NStatus != NDIS_STATUS_SUCCESS)
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("BA - MlmeADDBAAction() allocate memory failed \n"));
+			return;
+		}
+		// 1. find entry
+		Idx = pAd->MacTab.Content[pInfo->Wcid].BAOriWcidArray[pInfo->TID];
+		if (Idx == 0)
+		{
+			MlmeFreeMemory(pAd, pOutBuffer);
+			DBGPRINT(RT_DEBUG_ERROR,("BA - MlmeADDBAAction() can't find BAOriEntry \n"));
+			return;
+		}
+		else
+		{
+			pBAEntry =&pAd->BATable.BAOriEntry[Idx];
+		}
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			if (ADHOC_ON(pAd))
+				ActHeaderInit(pAd, &Frame.Hdr, pInfo->pAddr, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+			else
+#ifdef QOS_DLS_SUPPORT
+			if (pAd->MacTab.Content[pInfo->Wcid].ValidAsDls)
+				ActHeaderInit(pAd, &Frame.Hdr, pInfo->pAddr, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+			else
+#endif // QOS_DLS_SUPPORT //
+				ActHeaderInit(pAd, &Frame.Hdr, pAd->CommonCfg.Bssid, pAd->CurrentAddress, pInfo->pAddr);
+
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+		Frame.Category = CATEGORY_BA;
+		Frame.Action = ADDBA_REQ;
+		Frame.BaParm.AMSDUSupported = 0;
+		Frame.BaParm.BAPolicy = IMMED_BA;
+		Frame.BaParm.TID = pInfo->TID;
+		Frame.BaParm.BufSize = pInfo->BaBufSize;
+		Frame.Token = pInfo->Token;
+		Frame.TimeOutValue = pInfo->TimeOutValue;
+		Frame.BaStartSeq.field.FragNum = 0;
+		Frame.BaStartSeq.field.StartSeq = pAd->MacTab.Content[pInfo->Wcid].TxSeq[pInfo->TID];
+
+		*(USHORT *)(&Frame.BaParm) = cpu2le16(*(USHORT *)(&Frame.BaParm));
+		Frame.TimeOutValue = cpu2le16(Frame.TimeOutValue);
+		Frame.BaStartSeq.word = cpu2le16(Frame.BaStartSeq.word);
+
+		MakeOutgoingFrame(pOutBuffer,		   &FrameLen,
+		              sizeof(FRAME_ADDBA_REQ), &Frame,
+		              END_OF_ARGS);
+		MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+		MlmeFreeMemory(pAd, pOutBuffer);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("BA - Send ADDBA request. StartSeq = %x,  FrameLen = %ld. BufSize = %d\n", Frame.BaStartSeq.field.StartSeq, FrameLen, Frame.BaParm.BufSize));
+    }
+}
+
+/*
+    ==========================================================================
+    Description:
+        send DELBA and delete BaEntry if any
+    Parametrs:
+        Elem - MLME message MLME_DELBA_REQ_STRUCT
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID MlmeDELBAAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+	MLME_DELBA_REQ_STRUCT *pInfo;
+	PUCHAR         pOutBuffer = NULL;
+	PUCHAR		   pOutBuffer2 = NULL;
+	NDIS_STATUS     NStatus;
+	ULONG		Idx;
+	FRAME_DELBA_REQ  Frame;
+	ULONG		FrameLen;
+	FRAME_BAR	FrameBar;
+
+	pInfo = (MLME_DELBA_REQ_STRUCT *)Elem->Msg;
+	// must send back DELBA
+	NdisZeroMemory(&Frame, sizeof(FRAME_DELBA_REQ));
+	DBGPRINT(RT_DEBUG_TRACE, ("==> MlmeDELBAAction(), Initiator(%d) \n", pInfo->Initiator));
+
+	if(MlmeDelBAReqSanity(pAd, Elem->Msg, Elem->MsgLen))
+	{
+		NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+		if(NStatus != NDIS_STATUS_SUCCESS)
+		{
+			DBGPRINT(RT_DEBUG_ERROR,("BA - MlmeDELBAAction() allocate memory failed 1. \n"));
+			return;
+		}
+
+		NStatus = MlmeAllocateMemory(pAd, &pOutBuffer2);  //Get an unused nonpaged memory
+		if(NStatus != NDIS_STATUS_SUCCESS)
+		{
+			MlmeFreeMemory(pAd, pOutBuffer);
+			DBGPRINT(RT_DEBUG_ERROR, ("BA - MlmeDELBAAction() allocate memory failed 2. \n"));
+			return;
+		}
+
+		// SEND BAR (Send BAR to refresh peer reordering buffer.)
+		Idx = pAd->MacTab.Content[pInfo->Wcid].BAOriWcidArray[pInfo->TID];
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			BarHeaderInit(pAd, &FrameBar, pAd->MacTab.Content[pInfo->Wcid].Addr, pAd->CurrentAddress);
+#endif // CONFIG_STA_SUPPORT //
+
+		FrameBar.StartingSeq.field.FragNum = 0; // make sure sequence not clear in DEL funciton.
+		FrameBar.StartingSeq.field.StartSeq = pAd->MacTab.Content[pInfo->Wcid].TxSeq[pInfo->TID]; // make sure sequence not clear in DEL funciton.
+		FrameBar.BarControl.TID = pInfo->TID; // make sure sequence not clear in DEL funciton.
+		FrameBar.BarControl.ACKPolicy = IMMED_BA; // make sure sequence not clear in DEL funciton.
+		FrameBar.BarControl.Compressed = 1; // make sure sequence not clear in DEL funciton.
+		FrameBar.BarControl.MTID = 0; // make sure sequence not clear in DEL funciton.
+
+		MakeOutgoingFrame(pOutBuffer2,				&FrameLen,
+					  sizeof(FRAME_BAR),	  &FrameBar,
+					  END_OF_ARGS);
+		MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer2, FrameLen);
+		MlmeFreeMemory(pAd, pOutBuffer2);
+		DBGPRINT(RT_DEBUG_TRACE,("BA - MlmeDELBAAction() . Send BAR to refresh peer reordering buffer \n"));
+
+		// SEND DELBA FRAME
+		FrameLen = 0;
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			if (ADHOC_ON(pAd))
+				ActHeaderInit(pAd, &Frame.Hdr, pAd->MacTab.Content[pInfo->Wcid].Addr, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+			else
+#ifdef QOS_DLS_SUPPORT
+			if (pAd->MacTab.Content[pInfo->Wcid].ValidAsDls)
+				ActHeaderInit(pAd, &Frame.Hdr, pAd->MacTab.Content[pInfo->Wcid].Addr, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+			else
+#endif // QOS_DLS_SUPPORT //
+				ActHeaderInit(pAd, &Frame.Hdr,  pAd->CommonCfg.Bssid, pAd->CurrentAddress, pAd->MacTab.Content[pInfo->Wcid].Addr);
+		}
+#endif // CONFIG_STA_SUPPORT //
+		Frame.Category = CATEGORY_BA;
+		Frame.Action = DELBA;
+		Frame.DelbaParm.Initiator = pInfo->Initiator;
+		Frame.DelbaParm.TID = pInfo->TID;
+		Frame.ReasonCode = 39; // Time Out
+		*(USHORT *)(&Frame.DelbaParm) = cpu2le16(*(USHORT *)(&Frame.DelbaParm));
+		Frame.ReasonCode = cpu2le16(Frame.ReasonCode);
+
+		MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+		              sizeof(FRAME_DELBA_REQ),    &Frame,
+		              END_OF_ARGS);
+		MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+		MlmeFreeMemory(pAd, pOutBuffer);
+		DBGPRINT(RT_DEBUG_TRACE, ("BA - MlmeDELBAAction() . 3 DELBA sent. Initiator(%d)\n", pInfo->Initiator));
+    	}
+}
+#endif // DOT11_N_SUPPORT //
+
+VOID MlmeQOSAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+}
+
+VOID MlmeDLSAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+}
+
+VOID MlmeInvalidAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+	//PUCHAR		   pOutBuffer = NULL;
+	//Return the receiving frame except the MSB of category filed set to 1.  7.3.1.11
+}
+
+VOID PeerQOSAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+}
+
+#ifdef QOS_DLS_SUPPORT
+VOID PeerDLSAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR	Action = Elem->Msg[LENGTH_802_11+1];
+
+	switch(Action)
+	{
+		case ACTION_DLS_REQUEST:
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			PeerDlsReqAction(pAd, Elem);
+#endif // CONFIG_STA_SUPPORT //
+			break;
+
+		case ACTION_DLS_RESPONSE:
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			PeerDlsRspAction(pAd, Elem);
+#endif // CONFIG_STA_SUPPORT //
+			break;
+
+		case ACTION_DLS_TEARDOWN:
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			PeerDlsTearDownAction(pAd, Elem);
+#endif // CONFIG_STA_SUPPORT //
+			break;
+	}
+}
+#endif // QOS_DLS_SUPPORT //
+
+#ifdef DOT11_N_SUPPORT
+VOID PeerBAAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR	Action = Elem->Msg[LENGTH_802_11+1];
+
+	switch(Action)
+	{
+		case ADDBA_REQ:
+			PeerAddBAReqAction(pAd,Elem);
+			break;
+		case ADDBA_RESP:
+			PeerAddBARspAction(pAd,Elem);
+			break;
+		case DELBA:
+			PeerDelBAAction(pAd,Elem);
+			break;
+	}
+}
+
+
+#ifdef DOT11N_DRAFT3
+
+#ifdef CONFIG_STA_SUPPORT
+VOID StaPublicAction(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR Bss2040Coexist)
+{
+	BSS_2040_COEXIST_IE		BssCoexist;
+	MLME_SCAN_REQ_STRUCT			ScanReq;
+
+	BssCoexist.word = Bss2040Coexist;
+	// AP asks Station to return a 20/40 BSS Coexistence mgmt frame.  So we first starts a scan, then send back 20/40 BSS Coexistence mgmt frame
+	if ((BssCoexist.field.InfoReq == 1) && (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_SCAN_2040)))
+	{
+		// Clear record first.  After scan , will update those bit and send back to transmiter.
+		pAd->CommonCfg.BSSCoexist2040.field.InfoReq = 1;
+		pAd->CommonCfg.BSSCoexist2040.field.Intolerant40 = 0;
+		pAd->CommonCfg.BSSCoexist2040.field.BSS20WidthReq = 0;
+		// Fill out stuff for scan request
+		ScanParmFill(pAd, &ScanReq, ZeroSsid, 0, BSS_ANY, SCAN_2040_BSS_COEXIST);
+		MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_SCAN_REQ, sizeof(MLME_SCAN_REQ_STRUCT), &ScanReq);
+		pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_LIST_SCAN;
+	}
+}
+
+
+/*
+Description : Build Intolerant Channel Rerpot from Trigger event table.
+return : how many bytes copied.
+*/
+ULONG BuildIntolerantChannelRep(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    PUCHAR  pDest)
+{
+	ULONG			FrameLen = 0;
+	ULONG			ReadOffset = 0;
+	UCHAR			i;
+	UCHAR			LastRegClass = 0xff;
+	PUCHAR			pLen;
+
+	for ( i = 0;i < MAX_TRIGGER_EVENT;i++)
+	{
+		if (pAd->CommonCfg.TriggerEventTab.EventA[i].bValid == TRUE)
+		{
+			if (pAd->CommonCfg.TriggerEventTab.EventA[i].RegClass == LastRegClass)
+			{
+				*(pDest + ReadOffset) = (UCHAR)pAd->CommonCfg.TriggerEventTab.EventA[i].Channel;
+				*pLen++;
+				ReadOffset++;
+				FrameLen++;
+			}
+			else
+			{
+				*(pDest + ReadOffset) = IE_2040_BSS_INTOLERANT_REPORT;  // IE
+				*(pDest + ReadOffset + 1) = 2;	// Len = RegClass byte + channel byte.
+				pLen = pDest + ReadOffset + 1;
+				LastRegClass = pAd->CommonCfg.TriggerEventTab.EventA[i].RegClass;
+				*(pDest + ReadOffset + 2) = LastRegClass;	// Len = RegClass byte + channel byte.
+				*(pDest + ReadOffset + 3) = (UCHAR)pAd->CommonCfg.TriggerEventTab.EventA[i].Channel;
+				FrameLen += 4;
+				ReadOffset += 4;
+			}
+
+		}
+	}
+	return FrameLen;
+}
+
+
+/*
+Description : Send 20/40 BSS Coexistence Action frame If one trigger event is triggered.
+*/
+VOID Send2040CoexistAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR  Wcid,
+	IN	BOOLEAN	bAddIntolerantCha)
+{
+	PUCHAR			pOutBuffer = NULL;
+	NDIS_STATUS 	NStatus;
+	FRAME_ACTION_HDR	Frame;
+	ULONG			FrameLen;
+	ULONG			IntolerantChaRepLen;
+
+	IntolerantChaRepLen = 0;
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if(NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("ACT - Send2040CoexistAction() allocate memory failed \n"));
+		return;
+	}
+	ActHeaderInit(pAd, &Frame.Hdr, pAd->MacTab.Content[Wcid].Addr, pAd->CommonCfg.Bssid);
+	Frame.Category = CATEGORY_PUBLIC;
+	Frame.Action = ACTION_BSS_2040_COEXIST;
+
+	MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+				  sizeof(FRAME_ACTION_HDR),	  &Frame,
+				  END_OF_ARGS);
+
+	*(pOutBuffer + FrameLen) = pAd->CommonCfg.BSSCoexist2040.word;
+	FrameLen++;
+
+	if (bAddIntolerantCha == TRUE)
+		IntolerantChaRepLen = BuildIntolerantChannelRep(pAd, pOutBuffer + FrameLen);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen + IntolerantChaRepLen);
+	DBGPRINT(RT_DEBUG_ERROR,("ACT - Send2040CoexistAction( BSSCoexist2040 = 0x%x )  \n", pAd->CommonCfg.BSSCoexist2040.word));
+
+}
+
+
+/*
+	==========================================================================
+	Description:
+	After scan, Update 20/40 BSS Coexistence IE and send out.
+	According to 802.11n D3.03 11.14.10
+
+	Parameters:
+	==========================================================================
+ */
+VOID Update2040CoexistFrameAndNotify(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR  Wcid,
+	IN	BOOLEAN	bAddIntolerantCha)
+{
+	BSS_2040_COEXIST_IE	OldValue;
+
+	OldValue.word = pAd->CommonCfg.BSSCoexist2040.word;
+	if ((pAd->CommonCfg.TriggerEventTab.EventANo > 0) || (pAd->CommonCfg.TriggerEventTab.EventBCountDown > 0))
+		pAd->CommonCfg.BSSCoexist2040.field.BSS20WidthReq = 1;
+
+	// Need to check !!!!
+	// How STA will set Intolerant40 if implementation dependent. Now we don't set this bit first.!!!!!
+	// So Only check BSS20WidthReq change.
+	if (OldValue.field.BSS20WidthReq != pAd->CommonCfg.BSSCoexist2040.field.BSS20WidthReq)
+	{
+		Send2040CoexistAction(pAd, Wcid, bAddIntolerantCha);
+	}
+}
+#endif // CONFIG_STA_SUPPORT //
+
+
+BOOLEAN ChannelSwitchSanityCheck(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR  Wcid,
+	IN    UCHAR  NewChannel,
+	IN    UCHAR  Secondary)
+{
+	UCHAR		i;
+
+	if (Wcid >= MAX_LEN_OF_MAC_TABLE)
+		return FALSE;
+
+	if ((NewChannel > 7) && (Secondary == 1))
+		return FALSE;
+
+	if ((NewChannel < 5) && (Secondary == 3))
+		return FALSE;
+
+	// 0. Check if new channel is in the channellist.
+	for (i = 0;i < pAd->ChannelListNum;i++)
+	{
+		if (pAd->ChannelList[i].Channel == NewChannel)
+		{
+			break;
+		}
+	}
+
+	if (i == pAd->ChannelListNum)
+		return FALSE;
+
+	return TRUE;
+}
+
+
+VOID ChannelSwitchAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR  Wcid,
+	IN    UCHAR  NewChannel,
+	IN    UCHAR  Secondary)
+{
+	UCHAR		BBPValue = 0;
+	ULONG		MACValue;
+
+	DBGPRINT(RT_DEBUG_TRACE,("SPECTRUM - ChannelSwitchAction(NewChannel = %d , Secondary = %d)  \n", NewChannel, Secondary));
+
+	if (ChannelSwitchSanityCheck(pAd, Wcid, NewChannel, Secondary) == FALSE)
+		return;
+
+	// 1.  Switches to BW = 20.
+	if (Secondary == 0)
+	{
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+		BBPValue&= (~0x18);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+		if (pAd->MACVersion == 0x28600100)
+		{
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, 0x16);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, 0x08);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R73, 0x11);
+			DBGPRINT(RT_DEBUG_TRACE, ("!!!rt2860C !!! \n" ));
+		}
+		pAd->CommonCfg.BBPCurrentBW = BW_20;
+		pAd->CommonCfg.Channel = NewChannel;
+		pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel;
+		AsicSwitchChannel(pAd, pAd->CommonCfg.Channel,FALSE);
+		AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+		pAd->MacTab.Content[Wcid].HTPhyMode.field.BW = 0;
+		DBGPRINT(RT_DEBUG_TRACE, ("!!!20MHz   !!! \n" ));
+	}
+	// 1.  Switches to BW = 40 And Station supports BW = 40.
+	else if (((Secondary == 1) || (Secondary == 3)) && (pAd->CommonCfg.HtCapability.HtCapInfo.ChannelWidth == 1))
+	{
+		pAd->CommonCfg.Channel = NewChannel;
+
+		if (Secondary == 1)
+		{
+			// Secondary above.
+			pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel + 2;
+			RTMP_IO_READ32(pAd, TX_BAND_CFG, &MACValue);
+			MACValue &= 0xfe;
+			RTMP_IO_WRITE32(pAd, TX_BAND_CFG, MACValue);
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+			BBPValue&= (~0x18);
+			BBPValue|= (0x10);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BBPValue);
+			BBPValue&= (~0x20);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BBPValue);
+			DBGPRINT(RT_DEBUG_TRACE, ("!!!40MHz Lower LINK UP !!! Control Channel at Below. Central = %d \n", pAd->CommonCfg.CentralChannel ));
+		}
+		else
+		{
+			// Secondary below.
+			pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel - 2;
+			RTMP_IO_READ32(pAd, TX_BAND_CFG, &MACValue);
+			MACValue &= 0xfe;
+			MACValue |= 0x1;
+			RTMP_IO_WRITE32(pAd, TX_BAND_CFG, MACValue);
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+			BBPValue&= (~0x18);
+			BBPValue|= (0x10);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BBPValue);
+			BBPValue&= (~0x20);
+			BBPValue|= (0x20);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BBPValue);
+			DBGPRINT(RT_DEBUG_TRACE, ("!!!40MHz Upper LINK UP !!! Control Channel at UpperCentral = %d \n", pAd->CommonCfg.CentralChannel ));
+		}
+		pAd->CommonCfg.BBPCurrentBW = BW_40;
+		AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+		AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+		pAd->MacTab.Content[Wcid].HTPhyMode.field.BW = 1;
+	}
+}
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+
+VOID PeerPublicAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+#ifdef DOT11N_DRAFT3
+	UCHAR	Action = Elem->Msg[LENGTH_802_11+1];
+#endif // DOT11N_DRAFT3 //
+
+	if (Elem->Wcid >= MAX_LEN_OF_MAC_TABLE)
+		return;
+
+#ifdef DOT11N_DRAFT3
+	switch(Action)
+	{
+		case ACTION_BSS_2040_COEXIST:	// Format defined in IEEE 7.4.7a.1 in 11n Draf3.03
+			{
+				//UCHAR	BssCoexist;
+				BSS_2040_COEXIST_ELEMENT		*pCoexistInfo;
+				BSS_2040_COEXIST_IE 			*pBssCoexistIe;
+				BSS_2040_INTOLERANT_CH_REPORT	*pIntolerantReport = NULL;
+
+				if (Elem->MsgLen <= (LENGTH_802_11 + sizeof(BSS_2040_COEXIST_ELEMENT)) )
+				{
+					DBGPRINT(RT_DEBUG_ERROR, ("ACTION - 20/40 BSS Coexistence Management Frame length too short! len = %ld!\n", Elem->MsgLen));
+					break;
+				}
+				DBGPRINT(RT_DEBUG_TRACE, ("ACTION - 20/40 BSS Coexistence Management action----> \n"));
+				hex_dump("CoexistenceMgmtFrame", Elem->Msg, Elem->MsgLen);
+
+
+				pCoexistInfo = (BSS_2040_COEXIST_ELEMENT *) &Elem->Msg[LENGTH_802_11+2];
+				//hex_dump("CoexistInfo", (PUCHAR)pCoexistInfo, sizeof(BSS_2040_COEXIST_ELEMENT));
+				if (Elem->MsgLen >= (LENGTH_802_11 + sizeof(BSS_2040_COEXIST_ELEMENT) + sizeof(BSS_2040_INTOLERANT_CH_REPORT)))
+				{
+					pIntolerantReport = (BSS_2040_INTOLERANT_CH_REPORT *)((PUCHAR)pCoexistInfo + sizeof(BSS_2040_COEXIST_ELEMENT));
+				}
+				//hex_dump("IntolerantReport ", (PUCHAR)pIntolerantReport, sizeof(BSS_2040_INTOLERANT_CH_REPORT));
+
+				pBssCoexistIe = (BSS_2040_COEXIST_IE *)(&pCoexistInfo->BssCoexistIe);
+
+#ifdef CONFIG_STA_SUPPORT
+				IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				{
+					if (INFRA_ON(pAd))
+					{
+						StaPublicAction(pAd, pCoexistInfo);
+					}
+				}
+#endif // CONFIG_STA_SUPPORT //
+
+			}
+			break;
+	}
+
+#endif // DOT11N_DRAFT3 //
+
+}
+
+
+static VOID ReservedAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR Category;
+
+	if (Elem->MsgLen <= LENGTH_802_11)
+	{
+		return;
+	}
+
+	Category = Elem->Msg[LENGTH_802_11];
+	DBGPRINT(RT_DEBUG_TRACE,("Rcv reserved category(%d) Action Frame\n", Category));
+	hex_dump("Reserved Action Frame", &Elem->Msg[0], Elem->MsgLen);
+}
+
+VOID PeerRMAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+
+{
+	return;
+}
+
+#ifdef DOT11_N_SUPPORT
+static VOID respond_ht_information_exchange_action(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	PUCHAR			pOutBuffer = NULL;
+	NDIS_STATUS		NStatus;
+	ULONG			FrameLen;
+	FRAME_HT_INFO	HTINFOframe, *pFrame;
+	UCHAR   		*pAddr;
+
+
+	// 2. Always send back ADDBA Response
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);	 //Get an unused nonpaged memory
+
+	if (NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("ACTION - respond_ht_information_exchange_action() allocate memory failed \n"));
+		return;
+	}
+
+	// get RA
+	pFrame = (FRAME_HT_INFO *) &Elem->Msg[0];
+	pAddr = pFrame->Hdr.Addr2;
+
+	NdisZeroMemory(&HTINFOframe, sizeof(FRAME_HT_INFO));
+	// 2-1. Prepare ADDBA Response frame.
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (ADHOC_ON(pAd))
+			ActHeaderInit(pAd, &HTINFOframe.Hdr, pAddr, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+		else
+			ActHeaderInit(pAd, &HTINFOframe.Hdr, pAd->CommonCfg.Bssid, pAd->CurrentAddress, pAddr);
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	HTINFOframe.Category = CATEGORY_HT;
+	HTINFOframe.Action = HT_INFO_EXCHANGE;
+	HTINFOframe.HT_Info.Request = 0;
+	HTINFOframe.HT_Info.Forty_MHz_Intolerant = pAd->CommonCfg.HtCapability.HtCapInfo.Forty_Mhz_Intolerant;
+	HTINFOframe.HT_Info.STA_Channel_Width	 = pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth;
+
+	MakeOutgoingFrame(pOutBuffer,					&FrameLen,
+					  sizeof(FRAME_HT_INFO),	&HTINFOframe,
+					  END_OF_ARGS);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+}
+
+
+#ifdef DOT11N_DRAFT3
+VOID SendNotifyBWActionFrame(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR  Wcid,
+	IN UCHAR apidx)
+{
+	PUCHAR			pOutBuffer = NULL;
+	NDIS_STATUS 	NStatus;
+	FRAME_ACTION_HDR	Frame;
+	ULONG			FrameLen;
+	PUCHAR			pAddr1;
+
+
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if(NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("ACT - SendNotifyBWAction() allocate memory failed \n"));
+		return;
+	}
+
+	if (Wcid == MCAST_WCID)
+		pAddr1 = &BROADCAST_ADDR[0];
+	else
+		pAddr1 = pAd->MacTab.Content[Wcid].Addr;
+	ActHeaderInit(pAd, &Frame.Hdr, pAddr1, pAd->ApCfg.MBSSID[apidx].Bssid, pAd->ApCfg.MBSSID[apidx].Bssid);
+
+	Frame.Category = CATEGORY_HT;
+	Frame.Action = NOTIFY_BW_ACTION;
+
+	MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+				  sizeof(FRAME_ACTION_HDR),	  &Frame,
+				  END_OF_ARGS);
+
+	*(pOutBuffer + FrameLen) = pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth;
+	FrameLen++;
+
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	DBGPRINT(RT_DEBUG_TRACE,("ACT - SendNotifyBWAction(NotifyBW= %d)!\n", pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth));
+
+}
+#endif // DOT11N_DRAFT3 //
+
+
+VOID PeerHTAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR	Action = Elem->Msg[LENGTH_802_11+1];
+
+	if (Elem->Wcid >= MAX_LEN_OF_MAC_TABLE)
+		return;
+
+	switch(Action)
+	{
+		case NOTIFY_BW_ACTION:
+			DBGPRINT(RT_DEBUG_TRACE,("ACTION - HT Notify Channel bandwidth action----> \n"));
+#ifdef CONFIG_STA_SUPPORT
+			if(pAd->StaActive.SupportedPhyInfo.bHtEnable == FALSE)
+			{
+				// Note, this is to patch DIR-1353 AP. When the AP set to Wep, it will use legacy mode. But AP still keeps
+				// sending BW_Notify Action frame, and cause us to linkup and linkdown.
+				// In legacy mode, don't need to parse HT action frame.
+				DBGPRINT(RT_DEBUG_TRACE,("ACTION -Ignore HT Notify Channel BW when link as legacy mode. BW = %d---> \n",
+								Elem->Msg[LENGTH_802_11+2] ));
+				break;
+			}
+#endif // CONFIG_STA_SUPPORT //
+
+			if (Elem->Msg[LENGTH_802_11+2] == 0)	// 7.4.8.2. if value is 1, keep the same as supported channel bandwidth.
+				pAd->MacTab.Content[Elem->Wcid].HTPhyMode.field.BW = 0;
+
+			break;
+		case SMPS_ACTION:
+			// 7.3.1.25
+ 			DBGPRINT(RT_DEBUG_TRACE,("ACTION - SMPS action----> \n"));
+			if (((Elem->Msg[LENGTH_802_11+2]&0x1) == 0))
+			{
+				pAd->MacTab.Content[Elem->Wcid].MmpsMode = MMPS_ENABLE;
+			}
+			else if (((Elem->Msg[LENGTH_802_11+2]&0x2) == 0))
+			{
+				pAd->MacTab.Content[Elem->Wcid].MmpsMode = MMPS_STATIC;
+			}
+			else
+			{
+				pAd->MacTab.Content[Elem->Wcid].MmpsMode = MMPS_DYNAMIC;
+			}
+
+			DBGPRINT(RT_DEBUG_TRACE,("Aid(%d) MIMO PS = %d\n", Elem->Wcid, pAd->MacTab.Content[Elem->Wcid].MmpsMode));
+			// rt2860c : add something for smps change.
+			break;
+
+		case SETPCO_ACTION:
+			break;
+		case MIMO_CHA_MEASURE_ACTION:
+			break;
+		case HT_INFO_EXCHANGE:
+			{
+				HT_INFORMATION_OCTET	*pHT_info;
+
+				pHT_info = (HT_INFORMATION_OCTET *) &Elem->Msg[LENGTH_802_11+2];
+    				// 7.4.8.10
+    				DBGPRINT(RT_DEBUG_TRACE,("ACTION - HT Information Exchange action----> \n"));
+    				if (pHT_info->Request)
+    				{
+    					respond_ht_information_exchange_action(pAd, Elem);
+    				}
+			}
+    			break;
+	}
+}
+
+
+/*
+	==========================================================================
+	Description:
+		Retry sending ADDBA Reqest.
+
+	IRQL = DISPATCH_LEVEL
+
+	Parametrs:
+	p8023Header: if this is already 802.3 format, p8023Header is NULL
+
+	Return	: TRUE if put into rx reordering buffer, shouldn't indicaterxhere.
+				FALSE , then continue indicaterx at this moment.
+	==========================================================================
+ */
+VOID ORIBATimerTimeout(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	MAC_TABLE_ENTRY	*pEntry;
+	INT			i, total;
+	UCHAR			TID;
+
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+	total = pAd->MacTab.Size * NUM_OF_TID;
+
+	for (i = 1; ((i <MAX_LEN_OF_BA_ORI_TABLE) && (total > 0)) ; i++)
+	{
+		if  (pAd->BATable.BAOriEntry[i].ORI_BA_Status == Originator_Done)
+		{
+			pEntry = &pAd->MacTab.Content[pAd->BATable.BAOriEntry[i].Wcid];
+			TID = pAd->BATable.BAOriEntry[i].TID;
+
+			ASSERT(pAd->BATable.BAOriEntry[i].Wcid < MAX_LEN_OF_MAC_TABLE);
+		}
+		total --;
+	}
+}
+
+
+VOID SendRefreshBAR(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry)
+{
+	FRAME_BAR		FrameBar;
+	ULONG			FrameLen;
+	NDIS_STATUS 	NStatus;
+	PUCHAR			pOutBuffer = NULL;
+	USHORT			Sequence;
+	UCHAR			i, TID;
+	USHORT			idx;
+	BA_ORI_ENTRY	*pBAEntry;
+
+	for (i = 0; i <NUM_OF_TID; i++)
+	{
+		idx = pEntry->BAOriWcidArray[i];
+		if (idx == 0)
+		{
+			continue;
+		}
+		pBAEntry = &pAd->BATable.BAOriEntry[idx];
+
+		if  (pBAEntry->ORI_BA_Status == Originator_Done)
+		{
+			TID = pBAEntry->TID;
+
+			ASSERT(pBAEntry->Wcid < MAX_LEN_OF_MAC_TABLE);
+
+			NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+			if(NStatus != NDIS_STATUS_SUCCESS)
+			{
+				DBGPRINT(RT_DEBUG_ERROR,("BA - MlmeADDBAAction() allocate memory failed \n"));
+				return;
+			}
+
+			Sequence = pEntry->TxSeq[TID];
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				BarHeaderInit(pAd, &FrameBar, pEntry->Addr, pAd->CurrentAddress);
+#endif // CONFIG_STA_SUPPORT //
+
+			FrameBar.StartingSeq.field.FragNum = 0; // make sure sequence not clear in DEL function.
+			FrameBar.StartingSeq.field.StartSeq = Sequence; // make sure sequence not clear in DEL funciton.
+			FrameBar.BarControl.TID = TID; // make sure sequence not clear in DEL funciton.
+
+			MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+							  sizeof(FRAME_BAR),	  &FrameBar,
+							  END_OF_ARGS);
+			if (1)	// Now we always send BAR.
+			{
+				MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+			}
+			MlmeFreeMemory(pAd, pOutBuffer);
+		}
+	}
+}
+#endif // DOT11_N_SUPPORT //
+
+VOID ActHeaderInit(
+    IN	PRTMP_ADAPTER	pAd,
+    IN OUT PHEADER_802_11 pHdr80211,
+    IN PUCHAR Addr1,
+    IN PUCHAR Addr2,
+    IN PUCHAR Addr3)
+{
+    NdisZeroMemory(pHdr80211, sizeof(HEADER_802_11));
+    pHdr80211->FC.Type = BTYPE_MGMT;
+    pHdr80211->FC.SubType = SUBTYPE_ACTION;
+
+    COPY_MAC_ADDR(pHdr80211->Addr1, Addr1);
+	COPY_MAC_ADDR(pHdr80211->Addr2, Addr2);
+    COPY_MAC_ADDR(pHdr80211->Addr3, Addr3);
+}
+
+VOID BarHeaderInit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN OUT PFRAME_BAR pCntlBar,
+	IN PUCHAR pDA,
+	IN PUCHAR pSA)
+{
+	NdisZeroMemory(pCntlBar, sizeof(FRAME_BAR));
+	pCntlBar->FC.Type = BTYPE_CNTL;
+	pCntlBar->FC.SubType = SUBTYPE_BLOCK_ACK_REQ;
+   	pCntlBar->BarControl.MTID = 0;
+	pCntlBar->BarControl.Compressed = 1;
+	pCntlBar->BarControl.ACKPolicy = 0;
+
+
+	pCntlBar->Duration = 16 + RTMPCalcDuration(pAd, RATE_1, sizeof(FRAME_BA));
+
+	COPY_MAC_ADDR(pCntlBar->Addr1, pDA);
+	COPY_MAC_ADDR(pCntlBar->Addr2, pSA);
+}
+
+
+/*
+	==========================================================================
+	Description:
+		Insert Category and action code into the action frame.
+
+	Parametrs:
+		1. frame buffer pointer.
+		2. frame length.
+		3. category code of the frame.
+		4. action code of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID InsertActField(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen,
+	IN UINT8 Category,
+	IN UINT8 ActCode)
+{
+	ULONG TempLen;
+
+	MakeOutgoingFrame(	pFrameBuf,		&TempLen,
+						1,				&Category,
+						1,				&ActCode,
+						END_OF_ARGS);
+
+	*pFrameLen = *pFrameLen + TempLen;
+
+	return;
+}
diff --git a/drivers/staging/rt2860/common/action.h b/drivers/staging/rt2860/common/action.h
new file mode 100644
index 00000000000..ce3877dce81
--- /dev/null
+++ b/drivers/staging/rt2860/common/action.h
@@ -0,0 +1,68 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	aironet.h
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+	Paul Lin	04-06-15		Initial
+*/
+
+#ifndef	__ACTION_H__
+#define	__ACTION_H__
+
+typedef struct PACKED __HT_INFO_OCTET
+{
+#ifdef RT_BIG_ENDIAN
+	UCHAR	Reserved:5;
+	UCHAR 	STA_Channel_Width:1;
+	UCHAR	Forty_MHz_Intolerant:1;
+	UCHAR	Request:1;
+#else
+	UCHAR	Request:1;
+	UCHAR	Forty_MHz_Intolerant:1;
+	UCHAR 	STA_Channel_Width:1;
+	UCHAR	Reserved:5;
+#endif
+} HT_INFORMATION_OCTET;
+
+
+typedef struct PACKED __FRAME_HT_INFO
+{
+	HEADER_802_11   		Hdr;
+	UCHAR					Category;
+	UCHAR					Action;
+	HT_INFORMATION_OCTET	HT_Info;
+}   FRAME_HT_INFO, *PFRAME_HT_INFO;
+
+#endif /* __ACTION_H__ */
+
+
diff --git a/drivers/staging/rt2860/common/ba_action.c b/drivers/staging/rt2860/common/ba_action.c
new file mode 100644
index 00000000000..8247aeb73a2
--- /dev/null
+++ b/drivers/staging/rt2860/common/ba_action.c
@@ -0,0 +1,1802 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#ifdef DOT11_N_SUPPORT
+
+#include "../rt_config.h"
+
+
+
+#define BA_ORI_INIT_SEQ		(pEntry->TxSeq[TID]) //1			// inital sequence number of BA session
+
+#define ORI_SESSION_MAX_RETRY	8
+#define ORI_BA_SESSION_TIMEOUT	(2000)	// ms
+#define REC_BA_SESSION_IDLE_TIMEOUT	(1000)	// ms
+
+#define REORDERING_PACKET_TIMEOUT		((100 * HZ)/1000)	// system ticks -- 100 ms
+#define MAX_REORDERING_PACKET_TIMEOUT	((3000 * HZ)/1000)	// system ticks -- 100 ms
+
+#define RESET_RCV_SEQ		(0xFFFF)
+
+static void ba_mpdu_blk_free(PRTMP_ADAPTER pAd, struct reordering_mpdu *mpdu_blk);
+
+
+BA_ORI_ENTRY *BATableAllocOriEntry(
+								  IN  PRTMP_ADAPTER   pAd,
+								  OUT USHORT          *Idx);
+
+BA_REC_ENTRY *BATableAllocRecEntry(
+								  IN  PRTMP_ADAPTER   pAd,
+								  OUT USHORT          *Idx);
+
+VOID BAOriSessionSetupTimeout(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3);
+
+VOID BARecSessionIdleTimeout(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3);
+
+
+BUILD_TIMER_FUNCTION(BAOriSessionSetupTimeout);
+BUILD_TIMER_FUNCTION(BARecSessionIdleTimeout);
+
+#define ANNOUNCE_REORDERING_PACKET(_pAd, _mpdu_blk)	\
+			Announce_Reordering_Packet(_pAd, _mpdu_blk);
+
+VOID BA_MaxWinSizeReasign(
+	IN PRTMP_ADAPTER	pAd,
+	IN MAC_TABLE_ENTRY  *pEntryPeer,
+	OUT UCHAR			*pWinSize)
+{
+	UCHAR MaxSize;
+
+
+	if (pAd->MACVersion >= RALINK_2883_VERSION) // 3*3
+	{
+		if (pAd->MACVersion >= RALINK_3070_VERSION)
+		{
+			if (pEntryPeer->WepStatus != Ndis802_11EncryptionDisabled)
+				MaxSize = 7; // for non-open mode
+			else
+				MaxSize = 13;
+		}
+		else
+			MaxSize = 31;
+	}
+	else if (pAd->MACVersion >= RALINK_2880E_VERSION) // 2880 e
+	{
+		if (pEntryPeer->WepStatus != Ndis802_11EncryptionDisabled)
+			MaxSize = 7; // for non-open mode
+		else
+			MaxSize = 13;
+	}
+	else
+		MaxSize = 7;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("ba> Win Size = %d, Max Size = %d\n",
+			*pWinSize, MaxSize));
+
+	if ((*pWinSize) > MaxSize)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("ba> reassign max win size from %d to %d\n",
+				*pWinSize, MaxSize));
+
+		*pWinSize = MaxSize;
+	}
+}
+
+void Announce_Reordering_Packet(IN PRTMP_ADAPTER			pAd,
+								IN struct reordering_mpdu	*mpdu)
+{
+	PNDIS_PACKET    pPacket;
+
+	pPacket = mpdu->pPacket;
+
+	if (mpdu->bAMSDU)
+	{
+		ASSERT(0);
+		BA_Reorder_AMSDU_Annnounce(pAd, pPacket);
+	}
+	else
+	{
+		//
+		// pass this 802.3 packet to upper layer or forward this packet to WM directly
+		//
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			ANNOUNCE_OR_FORWARD_802_3_PACKET(pAd, pPacket, RTMP_GET_PACKET_IF(pPacket));
+#endif // CONFIG_STA_SUPPORT //
+	}
+}
+
+/*
+ * Insert a reordering mpdu into sorted linked list by sequence no.
+ */
+BOOLEAN ba_reordering_mpdu_insertsorted(struct reordering_list *list, struct reordering_mpdu *mpdu)
+{
+
+	struct reordering_mpdu **ppScan = &list->next;
+
+	while (*ppScan != NULL)
+	{
+		if (SEQ_SMALLER((*ppScan)->Sequence, mpdu->Sequence, MAXSEQ))
+		{
+			ppScan = &(*ppScan)->next;
+		}
+		else if ((*ppScan)->Sequence == mpdu->Sequence)
+		{
+			/* give up this duplicated frame */
+			return(FALSE);
+		}
+		else
+		{
+			/* find position */
+			break;
+		}
+	}
+
+	mpdu->next = *ppScan;
+	*ppScan = mpdu;
+	list->qlen++;
+	return TRUE;
+}
+
+
+/*
+ * caller lock critical section if necessary
+ */
+static inline void ba_enqueue(struct reordering_list *list, struct reordering_mpdu *mpdu_blk)
+{
+	list->qlen++;
+	mpdu_blk->next = list->next;
+	list->next = mpdu_blk;
+}
+
+/*
+ * caller lock critical section if necessary
+ */
+static inline struct reordering_mpdu * ba_dequeue(struct reordering_list *list)
+{
+	struct reordering_mpdu *mpdu_blk = NULL;
+
+	ASSERT(list);
+
+		if (list->qlen)
+		{
+			list->qlen--;
+			mpdu_blk = list->next;
+			if (mpdu_blk)
+			{
+				list->next = mpdu_blk->next;
+				mpdu_blk->next = NULL;
+			}
+		}
+	return mpdu_blk;
+}
+
+
+static inline struct reordering_mpdu  *ba_reordering_mpdu_dequeue(struct reordering_list *list)
+{
+	return(ba_dequeue(list));
+}
+
+
+static inline struct reordering_mpdu  *ba_reordering_mpdu_probe(struct reordering_list *list)
+	{
+	ASSERT(list);
+
+		return(list->next);
+	}
+
+
+/*
+ * free all resource for reordering mechanism
+ */
+void ba_reordering_resource_release(PRTMP_ADAPTER pAd)
+{
+	BA_TABLE        *Tab;
+	PBA_REC_ENTRY   pBAEntry;
+	struct reordering_mpdu *mpdu_blk;
+	int i;
+
+	Tab = &pAd->BATable;
+
+	/* I.  release all pending reordering packet */
+	NdisAcquireSpinLock(&pAd->BATabLock);
+	for (i = 0; i < MAX_LEN_OF_BA_REC_TABLE; i++)
+	{
+		pBAEntry = &Tab->BARecEntry[i];
+		if (pBAEntry->REC_BA_Status != Recipient_NONE)
+		{
+			while ((mpdu_blk = ba_reordering_mpdu_dequeue(&pBAEntry->list)))
+			{
+				ASSERT(mpdu_blk->pPacket);
+				RELEASE_NDIS_PACKET(pAd, mpdu_blk->pPacket, NDIS_STATUS_FAILURE);
+				ba_mpdu_blk_free(pAd, mpdu_blk);
+			}
+		}
+	}
+	NdisReleaseSpinLock(&pAd->BATabLock);
+
+	ASSERT(pBAEntry->list.qlen == 0);
+	/* II. free memory of reordering mpdu table */
+	NdisAcquireSpinLock(&pAd->mpdu_blk_pool.lock);
+	os_free_mem(pAd, pAd->mpdu_blk_pool.mem);
+	NdisReleaseSpinLock(&pAd->mpdu_blk_pool.lock);
+}
+
+
+
+/*
+ * Allocate all resource for reordering mechanism
+ */
+BOOLEAN ba_reordering_resource_init(PRTMP_ADAPTER pAd, int num)
+{
+	int     i;
+	PUCHAR  mem;
+	struct reordering_mpdu *mpdu_blk;
+	struct reordering_list *freelist;
+
+	/* allocate spinlock */
+	NdisAllocateSpinLock(&pAd->mpdu_blk_pool.lock);
+
+	/* initialize freelist */
+	freelist = &pAd->mpdu_blk_pool.freelist;
+	freelist->next = NULL;
+	freelist->qlen = 0;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Allocate %d memory for BA reordering\n", (UINT32)(num*sizeof(struct reordering_mpdu))));
+
+	/* allocate number of mpdu_blk memory */
+	os_alloc_mem(pAd, (PUCHAR *)&mem, (num*sizeof(struct reordering_mpdu)));
+
+	pAd->mpdu_blk_pool.mem = mem;
+
+	if (mem == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Can't Allocate Memory for BA Reordering\n"));
+		return(FALSE);
+	}
+
+	/* build mpdu_blk free list */
+	for (i=0; i<num; i++)
+	{
+		/* get mpdu_blk */
+		mpdu_blk = (struct reordering_mpdu *) mem;
+		/* initial mpdu_blk */
+		NdisZeroMemory(mpdu_blk, sizeof(struct reordering_mpdu));
+		/* next mpdu_blk */
+		mem += sizeof(struct reordering_mpdu);
+		/* insert mpdu_blk into freelist */
+		ba_enqueue(freelist, mpdu_blk);
+	}
+
+	return(TRUE);
+}
+
+//static int blk_count=0; // sample take off, no use
+
+static struct reordering_mpdu *ba_mpdu_blk_alloc(PRTMP_ADAPTER pAd)
+{
+	struct reordering_mpdu *mpdu_blk;
+
+	NdisAcquireSpinLock(&pAd->mpdu_blk_pool.lock);
+	mpdu_blk = ba_dequeue(&pAd->mpdu_blk_pool.freelist);
+	if (mpdu_blk)
+	{
+//		blk_count++;
+		/* reset mpdu_blk */
+		NdisZeroMemory(mpdu_blk, sizeof(struct reordering_mpdu));
+	}
+	NdisReleaseSpinLock(&pAd->mpdu_blk_pool.lock);
+	return mpdu_blk;
+}
+
+static void ba_mpdu_blk_free(PRTMP_ADAPTER pAd, struct reordering_mpdu *mpdu_blk)
+{
+	ASSERT(mpdu_blk);
+
+	NdisAcquireSpinLock(&pAd->mpdu_blk_pool.lock);
+//	blk_count--;
+	ba_enqueue(&pAd->mpdu_blk_pool.freelist, mpdu_blk);
+	NdisReleaseSpinLock(&pAd->mpdu_blk_pool.lock);
+}
+
+
+static USHORT ba_indicate_reordering_mpdus_in_order(
+												   IN PRTMP_ADAPTER    pAd,
+												   IN PBA_REC_ENTRY    pBAEntry,
+												   IN USHORT           StartSeq)
+{
+	struct reordering_mpdu *mpdu_blk;
+	USHORT  LastIndSeq = RESET_RCV_SEQ;
+
+	NdisAcquireSpinLock(&pBAEntry->RxReRingLock);
+
+	while ((mpdu_blk = ba_reordering_mpdu_probe(&pBAEntry->list)))
+		{
+			/* find in-order frame */
+		if (!SEQ_STEPONE(mpdu_blk->Sequence, StartSeq, MAXSEQ))
+			{
+				break;
+			}
+			/* dequeue in-order frame from reodering list */
+			mpdu_blk = ba_reordering_mpdu_dequeue(&pBAEntry->list);
+			/* pass this frame up */
+		ANNOUNCE_REORDERING_PACKET(pAd, mpdu_blk);
+		/* move to next sequence */
+			StartSeq = mpdu_blk->Sequence;
+		LastIndSeq = StartSeq;
+		/* free mpdu_blk */
+			ba_mpdu_blk_free(pAd, mpdu_blk);
+	}
+
+	NdisReleaseSpinLock(&pBAEntry->RxReRingLock);
+
+	/* update last indicated sequence */
+	return LastIndSeq;
+}
+
+static void ba_indicate_reordering_mpdus_le_seq(
+											   IN PRTMP_ADAPTER    pAd,
+											   IN PBA_REC_ENTRY    pBAEntry,
+											   IN USHORT           Sequence)
+{
+	struct reordering_mpdu *mpdu_blk;
+
+	NdisAcquireSpinLock(&pBAEntry->RxReRingLock);
+	while ((mpdu_blk = ba_reordering_mpdu_probe(&pBAEntry->list)))
+		{
+			/* find in-order frame */
+		if ((mpdu_blk->Sequence == Sequence) || SEQ_SMALLER(mpdu_blk->Sequence, Sequence, MAXSEQ))
+		{
+			/* dequeue in-order frame from reodering list */
+			mpdu_blk = ba_reordering_mpdu_dequeue(&pBAEntry->list);
+			/* pass this frame up */
+			ANNOUNCE_REORDERING_PACKET(pAd, mpdu_blk);
+			/* free mpdu_blk */
+			ba_mpdu_blk_free(pAd, mpdu_blk);
+		}
+		else
+			{
+				break;
+			}
+	}
+	NdisReleaseSpinLock(&pBAEntry->RxReRingLock);
+}
+
+
+static void ba_refresh_reordering_mpdus(
+									   IN PRTMP_ADAPTER    pAd,
+									   PBA_REC_ENTRY       pBAEntry)
+{
+	struct reordering_mpdu *mpdu_blk;
+
+	NdisAcquireSpinLock(&pBAEntry->RxReRingLock);
+
+			/* dequeue in-order frame from reodering list */
+	while ((mpdu_blk = ba_reordering_mpdu_dequeue(&pBAEntry->list)))
+	{
+			/* pass this frame up */
+		ANNOUNCE_REORDERING_PACKET(pAd, mpdu_blk);
+
+		pBAEntry->LastIndSeq = mpdu_blk->Sequence;
+			ba_mpdu_blk_free(pAd, mpdu_blk);
+
+		/* update last indicated sequence */
+	}
+	ASSERT(pBAEntry->list.qlen == 0);
+	pBAEntry->LastIndSeq = RESET_RCV_SEQ;
+	NdisReleaseSpinLock(&pBAEntry->RxReRingLock);
+}
+
+
+//static
+void ba_flush_reordering_timeout_mpdus(
+									IN PRTMP_ADAPTER    pAd,
+									IN PBA_REC_ENTRY    pBAEntry,
+									IN ULONG            Now32)
+
+{
+	USHORT Sequence;
+
+//	if ((RTMP_TIME_AFTER((unsigned long)Now32, (unsigned long)(pBAEntry->LastIndSeqAtTimer+REORDERING_PACKET_TIMEOUT)) &&
+//		 (pBAEntry->list.qlen > ((pBAEntry->BAWinSize*7)/8))) //||
+//		(RTMP_TIME_AFTER((unsigned long)Now32, (unsigned long)(pBAEntry->LastIndSeqAtTimer+(10*REORDERING_PACKET_TIMEOUT))) &&
+//		 (pBAEntry->list.qlen > (pBAEntry->BAWinSize/8)))
+	if (RTMP_TIME_AFTER((unsigned long)Now32, (unsigned long)(pBAEntry->LastIndSeqAtTimer+(MAX_REORDERING_PACKET_TIMEOUT/6)))
+		 &&(pBAEntry->list.qlen > 1)
+		)
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("timeout[%d] (%08lx-%08lx = %d > %d): %x, flush all!\n ", pBAEntry->list.qlen, Now32, (pBAEntry->LastIndSeqAtTimer),
+			   (int)((long) Now32 - (long)(pBAEntry->LastIndSeqAtTimer)), MAX_REORDERING_PACKET_TIMEOUT,
+			   pBAEntry->LastIndSeq));
+		ba_refresh_reordering_mpdus(pAd, pBAEntry);
+		pBAEntry->LastIndSeqAtTimer = Now32;
+	}
+	else
+	if (RTMP_TIME_AFTER((unsigned long)Now32, (unsigned long)(pBAEntry->LastIndSeqAtTimer+(REORDERING_PACKET_TIMEOUT)))
+		&& (pBAEntry->list.qlen > 0)
+	   )
+		{
+//		printk("timeout[%d] (%lx-%lx = %d > %d): %x, ", pBAEntry->list.qlen, Now32, (pBAEntry->LastIndSeqAtTimer),
+//			   (int)((long) Now32 - (long)(pBAEntry->LastIndSeqAtTimer)), REORDERING_PACKET_TIMEOUT,
+//			   pBAEntry->LastIndSeq);
+    		//
+		// force LastIndSeq to shift to LastIndSeq+1
+    		//
+    		Sequence = (pBAEntry->LastIndSeq+1) & MAXSEQ;
+    		ba_indicate_reordering_mpdus_le_seq(pAd, pBAEntry, Sequence);
+    		pBAEntry->LastIndSeqAtTimer = Now32;
+			pBAEntry->LastIndSeq = Sequence;
+    		//
+    		// indicate in-order mpdus
+    		//
+    		Sequence = ba_indicate_reordering_mpdus_in_order(pAd, pBAEntry, Sequence);
+    		if (Sequence != RESET_RCV_SEQ)
+    		{
+    			pBAEntry->LastIndSeq = Sequence;
+    		}
+
+		//printk("%x, flush one!\n", pBAEntry->LastIndSeq);
+
+	}
+#if 0
+	else if (
+			 (RTMP_TIME_AFTER((unsigned long)Now32, (unsigned long)(pBAEntry->LastIndSeqAtTimer+(MAX_REORDERING_PACKET_TIMEOUT))) &&
+			  (pBAEntry->list.qlen > 1))
+			)
+		{
+		DBGPRINT(RT_DEBUG_TRACE,("timeout[%d] (%lx-%lx = %d > %d): %x\n ", pBAEntry->list.qlen, Now32, (pBAEntry->LastIndSeqAtTimer),
+			   (int)((long) Now32 - (long)(pBAEntry->LastIndSeqAtTimer)), MAX_REORDERING_PACKET_TIMEOUT,
+			   pBAEntry->LastIndSeq));
+		ba_refresh_reordering_mpdus(pAd, pBAEntry);
+			pBAEntry->LastIndSeqAtTimer = Now32;
+				}
+#endif
+}
+
+
+/*
+ * generate ADDBA request to
+ * set up BA agreement
+ */
+VOID BAOriSessionSetUp(
+					  IN PRTMP_ADAPTER    pAd,
+					  IN MAC_TABLE_ENTRY  *pEntry,
+					  IN UCHAR            TID,
+					  IN USHORT           TimeOut,
+					  IN ULONG            DelayTime,
+					  IN BOOLEAN          isForced)
+
+{
+	//MLME_ADDBA_REQ_STRUCT	AddbaReq;
+	BA_ORI_ENTRY            *pBAEntry = NULL;
+	USHORT                  Idx;
+	BOOLEAN                 Cancelled;
+
+	if ((pAd->CommonCfg.BACapability.field.AutoBA != TRUE)  &&  (isForced == FALSE))
+		return;
+
+	// if this entry is limited to use legacy tx mode, it doesn't generate BA.
+	if (RTMPStaFixedTxMode(pAd, pEntry) != FIXED_TXMODE_HT)
+		return;
+
+	if ((pEntry->BADeclineBitmap & (1<<TID)) && (isForced == FALSE))
+	{
+		// try again after 3 secs
+		DelayTime = 3000;
+//		printk("DeCline BA from Peer\n");
+//		return;
+	}
+
+
+	Idx = pEntry->BAOriWcidArray[TID];
+	if (Idx == 0)
+	{
+		// allocate a BA session
+		pBAEntry = BATableAllocOriEntry(pAd, &Idx);
+		if (pBAEntry == NULL)
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("ADDBA - MlmeADDBAAction() allocate BA session failed \n"));
+			return;
+		}
+	}
+	else
+	{
+		pBAEntry =&pAd->BATable.BAOriEntry[Idx];
+	}
+
+	if (pBAEntry->ORI_BA_Status >= Originator_WaitRes)
+	{
+		return;
+	}
+
+	pEntry->BAOriWcidArray[TID] = Idx;
+
+	// Initialize BA session
+	pBAEntry->ORI_BA_Status = Originator_WaitRes;
+	pBAEntry->Wcid = pEntry->Aid;
+	pBAEntry->BAWinSize = pAd->CommonCfg.BACapability.field.RxBAWinLimit;
+	pBAEntry->Sequence = BA_ORI_INIT_SEQ;
+	pBAEntry->Token = 1;	// (2008-01-21) Jan Lee recommends it - this token can't be 0
+	pBAEntry->TID = TID;
+	pBAEntry->TimeOutValue = TimeOut;
+	pBAEntry->pAdapter = pAd;
+
+	if (!(pEntry->TXBAbitmap & (1<<TID)))
+	{
+		RTMPInitTimer(pAd, &pBAEntry->ORIBATimer, GET_TIMER_FUNCTION(BAOriSessionSetupTimeout), pBAEntry, FALSE);
+	}
+	else
+		RTMPCancelTimer(&pBAEntry->ORIBATimer, &Cancelled);
+
+	// set timer to send ADDBA request
+	RTMPSetTimer(&pBAEntry->ORIBATimer, DelayTime);
+}
+
+VOID BAOriSessionAdd(
+			IN PRTMP_ADAPTER    pAd,
+					IN MAC_TABLE_ENTRY  *pEntry,
+			IN PFRAME_ADDBA_RSP pFrame)
+{
+	BA_ORI_ENTRY  *pBAEntry = NULL;
+	BOOLEAN       Cancelled;
+	UCHAR         TID;
+	USHORT        Idx;
+	PUCHAR          pOutBuffer2 = NULL;
+	NDIS_STATUS     NStatus;
+	ULONG           FrameLen;
+	FRAME_BAR       FrameBar;
+
+	TID = pFrame->BaParm.TID;
+	Idx = pEntry->BAOriWcidArray[TID];
+	pBAEntry =&pAd->BATable.BAOriEntry[Idx];
+
+	// Start fill in parameters.
+	if ((Idx !=0) && (pBAEntry->TID == TID) && (pBAEntry->ORI_BA_Status == Originator_WaitRes))
+	{
+		pBAEntry->BAWinSize = min(pBAEntry->BAWinSize, ((UCHAR)pFrame->BaParm.BufSize));
+		BA_MaxWinSizeReasign(pAd, pEntry, &pBAEntry->BAWinSize);
+
+		pBAEntry->TimeOutValue = pFrame->TimeOutValue;
+		pBAEntry->ORI_BA_Status = Originator_Done;
+		// reset sequence number
+		pBAEntry->Sequence = BA_ORI_INIT_SEQ;
+		// Set Bitmap flag.
+		pEntry->TXBAbitmap |= (1<<TID);
+				RTMPCancelTimer(&pBAEntry->ORIBATimer, &Cancelled);
+
+		pBAEntry->ORIBATimer.TimerValue = 0;	//pFrame->TimeOutValue;
+
+		DBGPRINT(RT_DEBUG_TRACE,("%s : TXBAbitmap = %x, BAWinSize = %d, TimeOut = %ld\n", __FUNCTION__, pEntry->TXBAbitmap,
+								 pBAEntry->BAWinSize, pBAEntry->ORIBATimer.TimerValue));
+
+		// SEND BAR ;
+		NStatus = MlmeAllocateMemory(pAd, &pOutBuffer2);  //Get an unused nonpaged memory
+		if (NStatus != NDIS_STATUS_SUCCESS)
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("BA - BAOriSessionAdd() allocate memory failed \n"));
+			return;
+		}
+
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			BarHeaderInit(pAd, &FrameBar, pAd->MacTab.Content[pBAEntry->Wcid].Addr, pAd->CurrentAddress);
+#endif // CONFIG_STA_SUPPORT //
+
+		FrameBar.StartingSeq.field.FragNum = 0;	// make sure sequence not clear in DEL function.
+		FrameBar.StartingSeq.field.StartSeq = pBAEntry->Sequence; // make sure sequence not clear in DEL funciton.
+		FrameBar.BarControl.TID = pBAEntry->TID; // make sure sequence not clear in DEL funciton.
+		MakeOutgoingFrame(pOutBuffer2,              &FrameLen,
+						  sizeof(FRAME_BAR),      &FrameBar,
+					  END_OF_ARGS);
+		MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer2, FrameLen);
+		MlmeFreeMemory(pAd, pOutBuffer2);
+
+
+		if (pBAEntry->ORIBATimer.TimerValue)
+			RTMPSetTimer(&pBAEntry->ORIBATimer, pBAEntry->ORIBATimer.TimerValue); // in mSec
+	}
+}
+
+BOOLEAN BARecSessionAdd(
+					   IN PRTMP_ADAPTER    pAd,
+					   IN MAC_TABLE_ENTRY  *pEntry,
+					   IN PFRAME_ADDBA_REQ pFrame)
+{
+	BA_REC_ENTRY            *pBAEntry = NULL;
+	BOOLEAN                 Status = TRUE;
+	BOOLEAN                 Cancelled;
+	USHORT                  Idx;
+	UCHAR                   TID;
+	UCHAR                   BAWinSize;
+	//UINT32                  Value;
+	//UINT                    offset;
+
+
+	ASSERT(pEntry);
+
+	// find TID
+	TID = pFrame->BaParm.TID;
+
+	BAWinSize = min(((UCHAR)pFrame->BaParm.BufSize), (UCHAR)pAd->CommonCfg.BACapability.field.RxBAWinLimit);
+
+	// Intel patch
+	if (BAWinSize == 0)
+	{
+		BAWinSize = 64;
+	}
+
+	Idx = pEntry->BARecWcidArray[TID];
+
+
+	if (Idx == 0)
+	{
+		pBAEntry = BATableAllocRecEntry(pAd, &Idx);
+	}
+	else
+	{
+		pBAEntry = &pAd->BATable.BARecEntry[Idx];
+		// flush all pending reordering mpdus
+		ba_refresh_reordering_mpdus(pAd, pBAEntry);
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("%s(%ld): Idx = %d, BAWinSize(req %d) = %d\n", __FUNCTION__, pAd->BATable.numAsRecipient, Idx,
+							 pFrame->BaParm.BufSize, BAWinSize));
+
+	// Start fill in parameters.
+	if (pBAEntry != NULL)
+	{
+		ASSERT(pBAEntry->list.qlen == 0);
+
+		pBAEntry->REC_BA_Status = Recipient_HandleRes;
+		pBAEntry->BAWinSize = BAWinSize;
+		pBAEntry->Wcid = pEntry->Aid;
+		pBAEntry->TID = TID;
+		pBAEntry->TimeOutValue = pFrame->TimeOutValue;
+		pBAEntry->REC_BA_Status = Recipient_Accept;
+		// initial sequence number
+		pBAEntry->LastIndSeq = RESET_RCV_SEQ; //pFrame->BaStartSeq.field.StartSeq;
+
+		printk("Start Seq = %08x\n",  pFrame->BaStartSeq.field.StartSeq);
+
+		if (pEntry->RXBAbitmap & (1<<TID))
+		{
+			RTMPCancelTimer(&pBAEntry->RECBATimer, &Cancelled);
+		}
+		else
+		{
+			RTMPInitTimer(pAd, &pBAEntry->RECBATimer, GET_TIMER_FUNCTION(BARecSessionIdleTimeout), pBAEntry, TRUE);
+		}
+
+#if 0	// for debugging
+		RTMPSetTimer(&pBAEntry->RECBATimer, REC_BA_SESSION_IDLE_TIMEOUT);
+#endif
+
+		// Set Bitmap flag.
+		pEntry->RXBAbitmap |= (1<<TID);
+		pEntry->BARecWcidArray[TID] = Idx;
+
+		pEntry->BADeclineBitmap &= ~(1<<TID);
+
+		// Set BA session mask in WCID table.
+		RT28XX_ADD_BA_SESSION_TO_ASIC(pAd, pEntry->Aid, TID);
+
+		DBGPRINT(RT_DEBUG_TRACE,("MACEntry[%d]RXBAbitmap = 0x%x. BARecWcidArray=%d\n",
+				pEntry->Aid, pEntry->RXBAbitmap, pEntry->BARecWcidArray[TID]));
+	}
+	else
+	{
+		Status = FALSE;
+		DBGPRINT(RT_DEBUG_TRACE,("Can't Accept ADDBA for %02x:%02x:%02x:%02x:%02x:%02x TID = %d\n",
+				PRINT_MAC(pEntry->Addr), TID));
+	}
+	return(Status);
+}
+
+
+BA_REC_ENTRY *BATableAllocRecEntry(
+								  IN  PRTMP_ADAPTER   pAd,
+								  OUT USHORT          *Idx)
+{
+	int             i;
+	BA_REC_ENTRY    *pBAEntry = NULL;
+
+
+	NdisAcquireSpinLock(&pAd->BATabLock);
+
+	if (pAd->BATable.numAsRecipient >= MAX_BARECI_SESSION)
+	{
+		printk("BA Recipeint Session (%ld) > %d\n", pAd->BATable.numAsRecipient,
+			MAX_BARECI_SESSION);
+		goto done;
+	}
+
+	// reserve idx 0 to identify BAWcidArray[TID] as empty
+	for (i=1; i < MAX_LEN_OF_BA_REC_TABLE; i++)
+	{
+		pBAEntry =&pAd->BATable.BARecEntry[i];
+		if ((pBAEntry->REC_BA_Status == Recipient_NONE))
+		{
+			// get one
+			pAd->BATable.numAsRecipient++;
+			pBAEntry->REC_BA_Status = Recipient_USED;
+			*Idx = i;
+			break;
+		}
+	}
+
+done:
+	NdisReleaseSpinLock(&pAd->BATabLock);
+	return pBAEntry;
+}
+
+BA_ORI_ENTRY *BATableAllocOriEntry(
+								  IN  PRTMP_ADAPTER   pAd,
+								  OUT USHORT          *Idx)
+{
+	int             i;
+	BA_ORI_ENTRY    *pBAEntry = NULL;
+
+	NdisAcquireSpinLock(&pAd->BATabLock);
+
+	if (pAd->BATable.numAsOriginator >= (MAX_LEN_OF_BA_ORI_TABLE))
+	{
+		goto done;
+	}
+
+	// reserve idx 0 to identify BAWcidArray[TID] as empty
+	for (i=1; i<MAX_LEN_OF_BA_ORI_TABLE; i++)
+	{
+		pBAEntry =&pAd->BATable.BAOriEntry[i];
+		if ((pBAEntry->ORI_BA_Status == Originator_NONE))
+		{
+			// get one
+			pAd->BATable.numAsOriginator++;
+			pBAEntry->ORI_BA_Status = Originator_USED;
+			pBAEntry->pAdapter = pAd;
+			*Idx = i;
+			break;
+		}
+	}
+
+done:
+	NdisReleaseSpinLock(&pAd->BATabLock);
+	return pBAEntry;
+}
+
+
+VOID BATableFreeOriEntry(
+						IN  PRTMP_ADAPTER   pAd,
+						IN  ULONG           Idx)
+{
+	BA_ORI_ENTRY    *pBAEntry = NULL;
+	MAC_TABLE_ENTRY *pEntry;
+
+
+	if ((Idx == 0) || (Idx >= MAX_LEN_OF_BA_ORI_TABLE))
+		return;
+
+	pBAEntry =&pAd->BATable.BAOriEntry[Idx];
+
+	if (pBAEntry->ORI_BA_Status != Originator_NONE)
+	{
+		pEntry = &pAd->MacTab.Content[pBAEntry->Wcid];
+		pEntry->BAOriWcidArray[pBAEntry->TID] = 0;
+
+
+		NdisAcquireSpinLock(&pAd->BATabLock);
+		if (pBAEntry->ORI_BA_Status == Originator_Done)
+		{
+		 	pEntry->TXBAbitmap &= (~(1<<(pBAEntry->TID) ));
+			DBGPRINT(RT_DEBUG_TRACE, ("BATableFreeOriEntry numAsOriginator= %ld\n", pAd->BATable.numAsRecipient));
+			// Erase Bitmap flag.
+		}
+
+		ASSERT(pAd->BATable.numAsOriginator != 0);
+
+		pAd->BATable.numAsOriginator -= 1;
+
+		pBAEntry->ORI_BA_Status = Originator_NONE;
+		pBAEntry->Token = 0;
+		NdisReleaseSpinLock(&pAd->BATabLock);
+	}
+}
+
+
+VOID BATableFreeRecEntry(
+						IN  PRTMP_ADAPTER   pAd,
+						IN  ULONG           Idx)
+{
+	BA_REC_ENTRY    *pBAEntry = NULL;
+	MAC_TABLE_ENTRY *pEntry;
+
+
+	if ((Idx == 0) || (Idx >= MAX_LEN_OF_BA_REC_TABLE))
+		return;
+
+	pBAEntry =&pAd->BATable.BARecEntry[Idx];
+
+	if (pBAEntry->REC_BA_Status != Recipient_NONE)
+	{
+		pEntry = &pAd->MacTab.Content[pBAEntry->Wcid];
+		pEntry->BARecWcidArray[pBAEntry->TID] = 0;
+
+		NdisAcquireSpinLock(&pAd->BATabLock);
+
+		ASSERT(pAd->BATable.numAsRecipient != 0);
+
+		pAd->BATable.numAsRecipient -= 1;
+
+		pBAEntry->REC_BA_Status = Recipient_NONE;
+		NdisReleaseSpinLock(&pAd->BATabLock);
+	}
+}
+
+
+VOID BAOriSessionTearDown(
+						 IN OUT  PRTMP_ADAPTER   pAd,
+						 IN      UCHAR           Wcid,
+						 IN      UCHAR           TID,
+						 IN      BOOLEAN         bPassive,
+						 IN      BOOLEAN         bForceSend)
+{
+	ULONG           Idx = 0;
+	BA_ORI_ENTRY    *pBAEntry;
+	BOOLEAN         Cancelled;
+
+	if (Wcid >= MAX_LEN_OF_MAC_TABLE)
+	{
+		return;
+	}
+
+	//
+	// Locate corresponding BA Originator Entry in BA Table with the (pAddr,TID).
+	//
+	Idx = pAd->MacTab.Content[Wcid].BAOriWcidArray[TID];
+	if ((Idx == 0) || (Idx >= MAX_LEN_OF_BA_ORI_TABLE))
+	{
+		if (bForceSend == TRUE)
+		{
+			// force send specified TID DelBA
+			MLME_DELBA_REQ_STRUCT   DelbaReq;
+			MLME_QUEUE_ELEM *Elem = (MLME_QUEUE_ELEM *) kmalloc(sizeof(MLME_QUEUE_ELEM), MEM_ALLOC_FLAG);
+
+			NdisZeroMemory(&DelbaReq, sizeof(DelbaReq));
+			NdisZeroMemory(Elem, sizeof(MLME_QUEUE_ELEM));
+
+			COPY_MAC_ADDR(DelbaReq.Addr, pAd->MacTab.Content[Wcid].Addr);
+			DelbaReq.Wcid = Wcid;
+			DelbaReq.TID = TID;
+			DelbaReq.Initiator = ORIGINATOR;
+#if 1
+			Elem->MsgLen  = sizeof(DelbaReq);
+			NdisMoveMemory(Elem->Msg, &DelbaReq, sizeof(DelbaReq));
+			MlmeDELBAAction(pAd, Elem);
+			kfree(Elem);
+#else
+			MlmeEnqueue(pAd, ACTION_STATE_MACHINE, MT2_MLME_ORI_DELBA_CATE, sizeof(MLME_DELBA_REQ_STRUCT), (PVOID)&DelbaReq);
+			RT28XX_MLME_HANDLER(pAd);
+#endif
+		}
+
+		return;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("%s===>Wcid=%d.TID=%d \n", __FUNCTION__, Wcid, TID));
+
+	pBAEntry = &pAd->BATable.BAOriEntry[Idx];
+	DBGPRINT(RT_DEBUG_TRACE,("\t===>Idx = %ld, Wcid=%d.TID=%d, ORI_BA_Status = %d \n", Idx, Wcid, TID, pBAEntry->ORI_BA_Status));
+	//
+	// Prepare DelBA action frame and send to the peer.
+	//
+	if ((bPassive == FALSE) && (TID == pBAEntry->TID) && (pBAEntry->ORI_BA_Status == Originator_Done))
+	{
+		MLME_DELBA_REQ_STRUCT   DelbaReq;
+		MLME_QUEUE_ELEM *Elem = (MLME_QUEUE_ELEM *) kmalloc(sizeof(MLME_QUEUE_ELEM), MEM_ALLOC_FLAG);
+
+		NdisZeroMemory(&DelbaReq, sizeof(DelbaReq));
+		NdisZeroMemory(Elem, sizeof(MLME_QUEUE_ELEM));
+
+		COPY_MAC_ADDR(DelbaReq.Addr, pAd->MacTab.Content[Wcid].Addr);
+		DelbaReq.Wcid = Wcid;
+		DelbaReq.TID = pBAEntry->TID;
+		DelbaReq.Initiator = ORIGINATOR;
+#if 1
+		Elem->MsgLen  = sizeof(DelbaReq);
+		NdisMoveMemory(Elem->Msg, &DelbaReq, sizeof(DelbaReq));
+		MlmeDELBAAction(pAd, Elem);
+		kfree(Elem);
+#else
+		MlmeEnqueue(pAd, ACTION_STATE_MACHINE, MT2_MLME_ORI_DELBA_CATE, sizeof(MLME_DELBA_REQ_STRUCT), (PVOID)&DelbaReq);
+		RT28XX_MLME_HANDLER(pAd);
+#endif
+	}
+	RTMPCancelTimer(&pBAEntry->ORIBATimer, &Cancelled);
+	BATableFreeOriEntry(pAd, Idx);
+
+	if (bPassive)
+	{
+		//BAOriSessionSetUp(pAd, &pAd->MacTab.Content[Wcid], TID, 0, 10000, TRUE);
+	}
+}
+
+VOID BARecSessionTearDown(
+						 IN OUT  PRTMP_ADAPTER   pAd,
+						 IN      UCHAR           Wcid,
+						 IN      UCHAR           TID,
+						 IN      BOOLEAN         bPassive)
+{
+	ULONG           Idx = 0;
+	BA_REC_ENTRY    *pBAEntry;
+
+	if (Wcid >= MAX_LEN_OF_MAC_TABLE)
+	{
+		return;
+	}
+
+	//
+	//  Locate corresponding BA Originator Entry in BA Table with the (pAddr,TID).
+	//
+	Idx = pAd->MacTab.Content[Wcid].BARecWcidArray[TID];
+	if (Idx == 0)
+		return;
+
+	DBGPRINT(RT_DEBUG_TRACE,("%s===>Wcid=%d.TID=%d \n", __FUNCTION__, Wcid, TID));
+
+
+	pBAEntry = &pAd->BATable.BARecEntry[Idx];
+	DBGPRINT(RT_DEBUG_TRACE,("\t===>Idx = %ld, Wcid=%d.TID=%d, REC_BA_Status = %d \n", Idx, Wcid, TID, pBAEntry->REC_BA_Status));
+	//
+	// Prepare DelBA action frame and send to the peer.
+	//
+	if ((TID == pBAEntry->TID) && (pBAEntry->REC_BA_Status == Recipient_Accept))
+	{
+		MLME_DELBA_REQ_STRUCT   DelbaReq;
+		BOOLEAN 				Cancelled;
+		MLME_QUEUE_ELEM *Elem = (MLME_QUEUE_ELEM *) kmalloc(sizeof(MLME_QUEUE_ELEM), MEM_ALLOC_FLAG);
+		//ULONG   offset;
+		//UINT32  VALUE;
+
+		RTMPCancelTimer(&pBAEntry->RECBATimer, &Cancelled);
+
+		//
+		// 1. Send DELBA Action Frame
+		//
+		if (bPassive == FALSE)
+		{
+			NdisZeroMemory(&DelbaReq, sizeof(DelbaReq));
+			NdisZeroMemory(Elem, sizeof(MLME_QUEUE_ELEM));
+
+			COPY_MAC_ADDR(DelbaReq.Addr, pAd->MacTab.Content[Wcid].Addr);
+			DelbaReq.Wcid = Wcid;
+			DelbaReq.TID = TID;
+			DelbaReq.Initiator = RECIPIENT;
+#if 1
+			Elem->MsgLen  = sizeof(DelbaReq);
+			NdisMoveMemory(Elem->Msg, &DelbaReq, sizeof(DelbaReq));
+			MlmeDELBAAction(pAd, Elem);
+			kfree(Elem);
+#else
+			MlmeEnqueue(pAd, ACTION_STATE_MACHINE, MT2_MLME_ORI_DELBA_CATE, sizeof(MLME_DELBA_REQ_STRUCT), (PVOID)&DelbaReq);
+			RT28XX_MLME_HANDLER(pAd);
+#endif
+		}
+
+
+		//
+		// 2. Free resource of BA session
+		//
+		// flush all pending reordering mpdus
+		ba_refresh_reordering_mpdus(pAd, pBAEntry);
+
+		NdisAcquireSpinLock(&pAd->BATabLock);
+
+		// Erase Bitmap flag.
+		pBAEntry->LastIndSeq = RESET_RCV_SEQ;
+		pBAEntry->BAWinSize = 0;
+		// Erase Bitmap flag at software mactable
+		pAd->MacTab.Content[Wcid].RXBAbitmap &= (~(1<<(pBAEntry->TID)));
+		pAd->MacTab.Content[Wcid].BARecWcidArray[TID] = 0;
+
+		RT28XX_DEL_BA_SESSION_FROM_ASIC(pAd, Wcid, TID);
+
+		NdisReleaseSpinLock(&pAd->BATabLock);
+
+	}
+
+	BATableFreeRecEntry(pAd, Idx);
+}
+
+VOID BASessionTearDownALL(
+						 IN OUT  PRTMP_ADAPTER pAd,
+						 IN      UCHAR Wcid)
+{
+	int i;
+
+	for (i=0; i<NUM_OF_TID; i++)
+	{
+		BAOriSessionTearDown(pAd, Wcid, i, FALSE, FALSE);
+		BARecSessionTearDown(pAd, Wcid, i, FALSE);
+	}
+}
+
+
+/*
+	==========================================================================
+	Description:
+		Retry sending ADDBA Reqest.
+
+	IRQL = DISPATCH_LEVEL
+
+	Parametrs:
+	p8023Header: if this is already 802.3 format, p8023Header is NULL
+
+	Return	: TRUE if put into rx reordering buffer, shouldn't indicaterxhere.
+				FALSE , then continue indicaterx at this moment.
+	==========================================================================
+ */
+VOID BAOriSessionSetupTimeout(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3)
+{
+	BA_ORI_ENTRY    *pBAEntry = (BA_ORI_ENTRY *)FunctionContext;
+	MAC_TABLE_ENTRY *pEntry;
+	PRTMP_ADAPTER   pAd;
+
+	if (pBAEntry == NULL)
+		return;
+
+	pAd = pBAEntry->pAdapter;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// Do nothing if monitor mode is on
+		if (MONITOR_ON(pAd))
+			return;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef RALINK_ATE
+	// Nothing to do in ATE mode.
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+	pEntry = &pAd->MacTab.Content[pBAEntry->Wcid];
+
+	if ((pBAEntry->ORI_BA_Status == Originator_WaitRes) && (pBAEntry->Token < ORI_SESSION_MAX_RETRY))
+	{
+		MLME_ADDBA_REQ_STRUCT    AddbaReq;
+
+		NdisZeroMemory(&AddbaReq, sizeof(AddbaReq));
+		COPY_MAC_ADDR(AddbaReq.pAddr, pEntry->Addr);
+		AddbaReq.Wcid = (UCHAR)(pEntry->Aid);
+		AddbaReq.TID = pBAEntry->TID;
+		AddbaReq.BaBufSize = pAd->CommonCfg.BACapability.field.RxBAWinLimit;
+		AddbaReq.TimeOutValue = 0;
+		AddbaReq.Token = pBAEntry->Token;
+		MlmeEnqueue(pAd, ACTION_STATE_MACHINE, MT2_MLME_ADD_BA_CATE, sizeof(MLME_ADDBA_REQ_STRUCT), (PVOID)&AddbaReq);
+		RT28XX_MLME_HANDLER(pAd);
+		DBGPRINT(RT_DEBUG_TRACE,("BA Ori Session Timeout(%d) : Send ADD BA again\n", pBAEntry->Token));
+
+		pBAEntry->Token++;
+		RTMPSetTimer(&pBAEntry->ORIBATimer, ORI_BA_SESSION_TIMEOUT);
+	}
+	else
+	{
+		BATableFreeOriEntry(pAd, pEntry->BAOriWcidArray[pBAEntry->TID]);
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+		Retry sending ADDBA Reqest.
+
+	IRQL = DISPATCH_LEVEL
+
+	Parametrs:
+	p8023Header: if this is already 802.3 format, p8023Header is NULL
+
+	Return	: TRUE if put into rx reordering buffer, shouldn't indicaterxhere.
+				FALSE , then continue indicaterx at this moment.
+	==========================================================================
+ */
+VOID BARecSessionIdleTimeout(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3)
+{
+
+	BA_REC_ENTRY    *pBAEntry = (BA_REC_ENTRY *)FunctionContext;
+	PRTMP_ADAPTER   pAd;
+	ULONG           Now32;
+
+	if (pBAEntry == NULL)
+		return;
+
+	if ((pBAEntry->REC_BA_Status == Recipient_Accept))
+	{
+		NdisGetSystemUpTime(&Now32);
+
+		if (RTMP_TIME_AFTER((unsigned long)Now32, (unsigned long)(pBAEntry->LastIndSeqAtTimer + REC_BA_SESSION_IDLE_TIMEOUT)))
+		{
+			pAd = pBAEntry->pAdapter;
+			// flush all pending reordering mpdus
+			ba_refresh_reordering_mpdus(pAd, pBAEntry);
+			printk("%ld: REC BA session Timeout\n", Now32);
+		}
+	}
+}
+
+
+VOID PeerAddBAReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+
+{
+	//	7.4.4.1
+	//ULONG	Idx;
+	UCHAR   Status = 1;
+	UCHAR   pAddr[6];
+	FRAME_ADDBA_RSP ADDframe;
+	PUCHAR         pOutBuffer = NULL;
+	NDIS_STATUS     NStatus;
+	PFRAME_ADDBA_REQ  pAddreqFrame = NULL;
+	//UCHAR		BufSize;
+	ULONG       FrameLen;
+	PULONG      ptemp;
+	PMAC_TABLE_ENTRY	pMacEntry;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s ==> (Wcid = %d)\n", __FUNCTION__, Elem->Wcid));
+
+	//hex_dump("AddBAReq", Elem->Msg, Elem->MsgLen);
+
+	//ADDBA Request from unknown peer, ignore this.
+	if (Elem->Wcid >= MAX_LEN_OF_MAC_TABLE)
+		return;
+
+	pMacEntry = &pAd->MacTab.Content[Elem->Wcid];
+	DBGPRINT(RT_DEBUG_TRACE,("BA - PeerAddBAReqAction----> \n"));
+	ptemp = (PULONG)Elem->Msg;
+	//DBGPRINT_RAW(RT_DEBUG_EMU, ("%08x:: %08x:: %08x:: %08x:: %08x:: %08x:: %08x:: %08x:: %08x\n", *(ptemp), *(ptemp+1), *(ptemp+2), *(ptemp+3), *(ptemp+4), *(ptemp+5), *(ptemp+6), *(ptemp+7), *(ptemp+8)));
+
+	if (PeerAddBAReqActionSanity(pAd, Elem->Msg, Elem->MsgLen, pAddr))
+	{
+
+		if ((pAd->CommonCfg.bBADecline == FALSE) && IS_HT_STA(pMacEntry))
+		{
+			pAddreqFrame = (PFRAME_ADDBA_REQ)(&Elem->Msg[0]);
+			printk("Rcv Wcid(%d) AddBAReq\n", Elem->Wcid);
+			if (BARecSessionAdd(pAd, &pAd->MacTab.Content[Elem->Wcid], pAddreqFrame))
+				Status = 0;
+			else
+				Status = 38; // more parameters have invalid values
+		}
+		else
+		{
+			Status = 37; // the request has been declined.
+		}
+	}
+
+	if (pAd->MacTab.Content[Elem->Wcid].ValidAsCLI)
+		ASSERT(pAd->MacTab.Content[Elem->Wcid].Sst == SST_ASSOC);
+
+	pAddreqFrame = (PFRAME_ADDBA_REQ)(&Elem->Msg[0]);
+	// 2. Always send back ADDBA Response
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);	 //Get an unused nonpaged memory
+	if (NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("ACTION - PeerBAAction() allocate memory failed \n"));
+		return;
+	}
+
+	NdisZeroMemory(&ADDframe, sizeof(FRAME_ADDBA_RSP));
+	// 2-1. Prepare ADDBA Response frame.
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (ADHOC_ON(pAd))
+			ActHeaderInit(pAd, &ADDframe.Hdr, pAddr, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+		else
+#ifdef QOS_DLS_SUPPORT
+		if (pAd->MacTab.Content[Elem->Wcid].ValidAsDls)
+			ActHeaderInit(pAd, &ADDframe.Hdr, pAddr, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+		else
+#endif // QOS_DLS_SUPPORT //
+			ActHeaderInit(pAd, &ADDframe.Hdr, pAd->CommonCfg.Bssid, pAd->CurrentAddress, pAddr);
+	}
+#endif // CONFIG_STA_SUPPORT //
+	ADDframe.Category = CATEGORY_BA;
+	ADDframe.Action = ADDBA_RESP;
+	ADDframe.Token = pAddreqFrame->Token;
+	// What is the Status code??  need to check.
+	ADDframe.StatusCode = Status;
+	ADDframe.BaParm.BAPolicy = IMMED_BA;
+	ADDframe.BaParm.AMSDUSupported = 0;
+	ADDframe.BaParm.TID = pAddreqFrame->BaParm.TID;
+	ADDframe.BaParm.BufSize = min(((UCHAR)pAddreqFrame->BaParm.BufSize), (UCHAR)pAd->CommonCfg.BACapability.field.RxBAWinLimit);
+	if (ADDframe.BaParm.BufSize == 0)
+	{
+		ADDframe.BaParm.BufSize = 64;
+	}
+	ADDframe.TimeOutValue = 0; //pAddreqFrame->TimeOutValue;
+
+	*(USHORT *)(&ADDframe.BaParm) = cpu2le16(*(USHORT *)(&ADDframe.BaParm));
+	ADDframe.StatusCode = cpu2le16(ADDframe.StatusCode);
+	ADDframe.TimeOutValue = cpu2le16(ADDframe.TimeOutValue);
+
+	MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+					  sizeof(FRAME_ADDBA_RSP),  &ADDframe,
+			  END_OF_ARGS);
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s(%d): TID(%d), BufSize(%d) <== \n", __FUNCTION__, Elem->Wcid, ADDframe.BaParm.TID,
+							  ADDframe.BaParm.BufSize));
+}
+
+
+VOID PeerAddBARspAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+
+{
+	//UCHAR		Idx, i;
+	//PUCHAR		   pOutBuffer = NULL;
+	PFRAME_ADDBA_RSP    pFrame = NULL;
+	//PBA_ORI_ENTRY		pBAEntry;
+
+	//ADDBA Response from unknown peer, ignore this.
+	if (Elem->Wcid >= MAX_LEN_OF_MAC_TABLE)
+		return;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s ==> Wcid(%d)\n", __FUNCTION__, Elem->Wcid));
+
+	//hex_dump("PeerAddBARspAction()", Elem->Msg, Elem->MsgLen);
+
+	if (PeerAddBARspActionSanity(pAd, Elem->Msg, Elem->MsgLen))
+	{
+		pFrame = (PFRAME_ADDBA_RSP)(&Elem->Msg[0]);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("\t\t StatusCode = %d\n", pFrame->StatusCode));
+		switch (pFrame->StatusCode)
+		{
+			case 0:
+				// I want a BAsession with this peer as an originator.
+				BAOriSessionAdd(pAd, &pAd->MacTab.Content[Elem->Wcid], pFrame);
+				break;
+			default:
+				// check status == USED ???
+				BAOriSessionTearDown(pAd, Elem->Wcid, pFrame->BaParm.TID, TRUE, FALSE);
+				break;
+		}
+		// Rcv Decline StatusCode
+		if ((pFrame->StatusCode == 37)
+#ifdef CONFIG_STA_SUPPORT
+            || ((pAd->OpMode == OPMODE_STA) && STA_TGN_WIFI_ON(pAd) && (pFrame->StatusCode != 0))
+#endif // CONFIG_STA_SUPPORT //
+            )
+		{
+			pAd->MacTab.Content[Elem->Wcid].BADeclineBitmap |= 1<<pFrame->BaParm.TID;
+		}
+	}
+}
+
+VOID PeerDelBAAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+
+{
+	//UCHAR				Idx;
+	//PUCHAR				pOutBuffer = NULL;
+	PFRAME_DELBA_REQ    pDelFrame = NULL;
+
+	DBGPRINT(RT_DEBUG_TRACE,("%s ==>\n", __FUNCTION__));
+	//DELBA Request from unknown peer, ignore this.
+	if (PeerDelBAActionSanity(pAd, Elem->Wcid, Elem->Msg, Elem->MsgLen))
+	{
+		pDelFrame = (PFRAME_DELBA_REQ)(&Elem->Msg[0]);
+		if (pDelFrame->DelbaParm.Initiator == ORIGINATOR)
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("BA - PeerDelBAAction----> ORIGINATOR\n"));
+			BARecSessionTearDown(pAd, Elem->Wcid, pDelFrame->DelbaParm.TID, TRUE);
+		}
+		else
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("BA - PeerDelBAAction----> RECIPIENT, Reason = %d\n",  pDelFrame->ReasonCode));
+			//hex_dump("DelBA Frame", pDelFrame, Elem->MsgLen);
+			BAOriSessionTearDown(pAd, Elem->Wcid, pDelFrame->DelbaParm.TID, TRUE, FALSE);
+		}
+	}
+}
+
+
+BOOLEAN CntlEnqueueForRecv(
+						  IN PRTMP_ADAPTER		pAd,
+						  IN ULONG				Wcid,
+						  IN ULONG				MsgLen,
+						  IN PFRAME_BA_REQ		pMsg)
+{
+	PFRAME_BA_REQ   pFrame = pMsg;
+	//PRTMP_REORDERBUF	pBuffer;
+	//PRTMP_REORDERBUF	pDmaBuf;
+	PBA_REC_ENTRY pBAEntry;
+	//BOOLEAN 	Result;
+	ULONG   Idx;
+	//UCHAR	NumRxPkt;
+	UCHAR	TID;//, i;
+
+	TID = (UCHAR)pFrame->BARControl.TID;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s(): BAR-Wcid(%ld), Tid (%d)\n", __FUNCTION__, Wcid, TID));
+	//hex_dump("BAR", (PCHAR) pFrame, MsgLen);
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return FALSE;
+
+	// First check the size, it MUST not exceed the mlme queue size
+	if (MsgLen > MGMT_DMA_BUFFER_SIZE)
+	{
+		DBGPRINT_ERR(("CntlEnqueueForRecv: frame too large, size = %ld \n", MsgLen));
+		return FALSE;
+	}
+	else if (MsgLen != sizeof(FRAME_BA_REQ))
+	{
+		DBGPRINT_ERR(("CntlEnqueueForRecv: BlockAck Request frame length size = %ld incorrect\n", MsgLen));
+		return FALSE;
+	}
+	else if (MsgLen != sizeof(FRAME_BA_REQ))
+	{
+		DBGPRINT_ERR(("CntlEnqueueForRecv: BlockAck Request frame length size = %ld incorrect\n", MsgLen));
+		return FALSE;
+	}
+
+	if ((Wcid < MAX_LEN_OF_MAC_TABLE) && (TID < 8))
+		{
+		// if this receiving packet is from SA that is in our OriEntry. Since WCID <9 has direct mapping. no need search.
+		Idx = pAd->MacTab.Content[Wcid].BARecWcidArray[TID];
+		pBAEntry = &pAd->BATable.BARecEntry[Idx];
+		}
+		else
+		{
+		return FALSE;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("BAR(%ld) : Tid (%d) - %04x:%04x\n", Wcid, TID, pFrame->BAStartingSeq.field.StartSeq, pBAEntry->LastIndSeq ));
+
+	if (SEQ_SMALLER(pBAEntry->LastIndSeq, pFrame->BAStartingSeq.field.StartSeq, MAXSEQ))
+	{
+		//printk("BAR Seq = %x, LastIndSeq = %x\n", pFrame->BAStartingSeq.field.StartSeq, pBAEntry->LastIndSeq);
+		ba_indicate_reordering_mpdus_le_seq(pAd, pBAEntry, pFrame->BAStartingSeq.field.StartSeq);
+		pBAEntry->LastIndSeq = (pFrame->BAStartingSeq.field.StartSeq == 0) ? MAXSEQ :(pFrame->BAStartingSeq.field.StartSeq -1);
+	}
+	//ba_refresh_reordering_mpdus(pAd, pBAEntry);
+	return TRUE;
+}
+
+/*
+Description : Send PSMP Action frame If PSMP mode switches.
+*/
+VOID SendPSMPAction(
+				   IN PRTMP_ADAPTER		pAd,
+				   IN UCHAR				Wcid,
+				   IN UCHAR				Psmp)
+{
+	PUCHAR          pOutBuffer = NULL;
+	NDIS_STATUS     NStatus;
+	//ULONG           Idx;
+	FRAME_PSMP_ACTION   Frame;
+	ULONG           FrameLen;
+
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);	 //Get an unused nonpaged memory
+	if (NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("BA - MlmeADDBAAction() allocate memory failed \n"));
+		return;
+	}
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		ActHeaderInit(pAd, &Frame.Hdr, pAd->CommonCfg.Bssid, pAd->CurrentAddress, pAd->MacTab.Content[Wcid].Addr);
+#endif // CONFIG_STA_SUPPORT //
+
+	Frame.Category = CATEGORY_HT;
+	Frame.Action = SMPS_ACTION;
+	switch (Psmp)
+	{
+		case MMPS_ENABLE:
+			Frame.Psmp = 0;
+			break;
+		case MMPS_DYNAMIC:
+			Frame.Psmp = 3;
+			break;
+		case MMPS_STATIC:
+			Frame.Psmp = 1;
+			break;
+	}
+	MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+					  sizeof(FRAME_PSMP_ACTION),      &Frame,
+					  END_OF_ARGS);
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+	DBGPRINT(RT_DEBUG_ERROR,("HT - SendPSMPAction( %d )  \n", Frame.Psmp));
+}
+
+
+#define RADIO_MEASUREMENT_REQUEST_ACTION	0
+
+typedef struct PACKED
+{
+	UCHAR	RegulatoryClass;
+	UCHAR	ChannelNumber;
+	USHORT	RandomInterval;
+	USHORT	MeasurementDuration;
+	UCHAR	MeasurementMode;
+	UCHAR   BSSID[MAC_ADDR_LEN];
+	UCHAR	ReportingCondition;
+	UCHAR	Threshold;
+	UCHAR   SSIDIE[2];			// 2 byte
+} BEACON_REQUEST;
+
+typedef struct PACKED
+{
+	UCHAR	ID;
+	UCHAR	Length;
+	UCHAR	Token;
+	UCHAR	RequestMode;
+	UCHAR	Type;
+} MEASUREMENT_REQ;
+
+
+
+
+void convert_reordering_packet_to_preAMSDU_or_802_3_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN  UCHAR			FromWhichBSSID)
+{
+	PNDIS_PACKET	pRxPkt;
+	UCHAR			Header802_3[LENGTH_802_3];
+
+	// 1. get 802.3 Header
+	// 2. remove LLC
+	// 		a. pointer pRxBlk->pData to payload
+	//      b. modify pRxBlk->DataSize
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		RTMP_802_11_REMOVE_LLC_AND_CONVERT_TO_802_3(pRxBlk, Header802_3);
+#endif // CONFIG_STA_SUPPORT //
+
+	ASSERT(pRxBlk->pRxPacket);
+	pRxPkt = RTPKT_TO_OSPKT(pRxBlk->pRxPacket);
+
+	RTPKT_TO_OSPKT(pRxPkt)->dev = get_netdev_from_bssid(pAd, FromWhichBSSID);
+	RTPKT_TO_OSPKT(pRxPkt)->data = pRxBlk->pData;
+	RTPKT_TO_OSPKT(pRxPkt)->len = pRxBlk->DataSize;
+	RTPKT_TO_OSPKT(pRxPkt)->tail = RTPKT_TO_OSPKT(pRxPkt)->data + RTPKT_TO_OSPKT(pRxPkt)->len;
+
+	//
+	// copy 802.3 header, if necessary
+	//
+	if (!RX_BLK_TEST_FLAG(pRxBlk, fRX_AMSDU))
+	{
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+#ifdef LINUX
+			NdisMoveMemory(skb_push(pRxPkt, LENGTH_802_3), Header802_3, LENGTH_802_3);
+#endif
+#ifdef UCOS
+			NdisMoveMemory(net_pkt_push(pRxPkt, LENGTH_802_3), Header802_3, LENGTH_802_3);
+#endif
+		}
+#endif // CONFIG_STA_SUPPORT //
+	}
+}
+
+
+#define INDICATE_LEGACY_OR_AMSDU(_pAd, _pRxBlk, _fromWhichBSSID)		\
+	do																	\
+	{																	\
+    	if (RX_BLK_TEST_FLAG(_pRxBlk, fRX_AMSDU))						\
+    	{																\
+    		Indicate_AMSDU_Packet(_pAd, _pRxBlk, _fromWhichBSSID);		\
+    	}																\
+		else if (RX_BLK_TEST_FLAG(_pRxBlk, fRX_EAP))					\
+		{																\
+			Indicate_EAPOL_Packet(_pAd, _pRxBlk, _fromWhichBSSID);		\
+		}																\
+    	else															\
+    	{																\
+    		Indicate_Legacy_Packet(_pAd, _pRxBlk, _fromWhichBSSID);		\
+    	}																\
+	} while (0);
+
+
+
+static VOID ba_enqueue_reordering_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PBA_REC_ENTRY	pBAEntry,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+	struct reordering_mpdu *mpdu_blk;
+	UINT16	Sequence = (UINT16) pRxBlk->pHeader->Sequence;
+
+	mpdu_blk = ba_mpdu_blk_alloc(pAd);
+	if (mpdu_blk != NULL)
+	{
+		// Write RxD buffer address & allocated buffer length
+		NdisAcquireSpinLock(&pBAEntry->RxReRingLock);
+
+		mpdu_blk->Sequence = Sequence;
+
+		mpdu_blk->bAMSDU = RX_BLK_TEST_FLAG(pRxBlk, fRX_AMSDU);
+
+		convert_reordering_packet_to_preAMSDU_or_802_3_packet(pAd, pRxBlk, FromWhichBSSID);
+
+		STATS_INC_RX_PACKETS(pAd, FromWhichBSSID);
+
+        //
+		// it is necessary for reordering packet to record
+		// which BSS it come from
+		//
+		RTMP_SET_PACKET_IF(pRxBlk->pRxPacket, FromWhichBSSID);
+
+		mpdu_blk->pPacket = pRxBlk->pRxPacket;
+
+		if (ba_reordering_mpdu_insertsorted(&pBAEntry->list, mpdu_blk) == FALSE)
+		{
+			// had been already within reordering list
+			// don't indicate
+			RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_SUCCESS);
+			ba_mpdu_blk_free(pAd, mpdu_blk);
+		}
+
+		ASSERT((0<= pBAEntry->list.qlen)  && (pBAEntry->list.qlen <= pBAEntry->BAWinSize));
+		NdisReleaseSpinLock(&pBAEntry->RxReRingLock);
+	}
+	else
+	{
+#if 0
+		DBGPRINT(RT_DEBUG_ERROR,  ("!!! (%d:%d) Can't allocate reordering mpdu blk\n",
+								   blk_count, pBAEntry->list.qlen));
+#else
+		DBGPRINT(RT_DEBUG_ERROR,  ("!!! (%d) Can't allocate reordering mpdu blk\n",
+								   pBAEntry->list.qlen));
+#endif
+		/*
+		 * flush all pending reordering mpdus
+		 * and receving mpdu to upper layer
+		 * make tcp/ip to take care reordering mechanism
+		 */
+		//ba_refresh_reordering_mpdus(pAd, pBAEntry);
+		ba_indicate_reordering_mpdus_le_seq(pAd, pBAEntry, Sequence);
+
+		pBAEntry->LastIndSeq = Sequence;
+		INDICATE_LEGACY_OR_AMSDU(pAd, pRxBlk, FromWhichBSSID);
+	}
+}
+
+
+/*
+	==========================================================================
+	Description:
+		Indicate this packet to upper layer or put it into reordering buffer
+
+	Parametrs:
+		pRxBlk         : carry necessary packet info 802.11 format
+		FromWhichBSSID : the packet received from which BSS
+
+	Return	:
+			  none
+
+	Note    :
+	          the packet queued into reordering buffer need to cover to 802.3 format
+			  or pre_AMSDU format
+	==========================================================================
+ */
+
+VOID Indicate_AMPDU_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+	USHORT				Idx;
+	PBA_REC_ENTRY		pBAEntry = NULL;
+	UINT16				Sequence = pRxBlk->pHeader->Sequence;
+	ULONG				Now32;
+	UCHAR				Wcid = pRxBlk->pRxWI->WirelessCliID;
+	UCHAR				TID = pRxBlk->pRxWI->TID;
+
+
+	if (!RX_BLK_TEST_FLAG(pRxBlk, fRX_AMSDU) &&  (pRxBlk->DataSize > MAX_RX_PKT_LEN))
+	{
+#if 0 // sample take off, no use
+		static int err_size;
+
+		err_size++;
+		if (err_size > 20) {
+			 printk("AMPDU DataSize = %d\n", pRxBlk->DataSize);
+			 hex_dump("802.11 Header", (UCHAR *)pRxBlk->pHeader, 24);
+			 hex_dump("Payload", pRxBlk->pData, 64);
+			 err_size = 0;
+		}
+#endif
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+
+
+#if 0 // test
+	/* Rec BA Session had been torn down */
+	INDICATE_LEGACY_OR_AMSDU(pAd, pRxBlk, FromWhichBSSID);
+	return;
+#endif
+
+	if (Wcid < MAX_LEN_OF_MAC_TABLE)
+	{
+		Idx = pAd->MacTab.Content[Wcid].BARecWcidArray[TID];
+		if (Idx == 0)
+		{
+			/* Rec BA Session had been torn down */
+			INDICATE_LEGACY_OR_AMSDU(pAd, pRxBlk, FromWhichBSSID);
+			return;
+		}
+		pBAEntry = &pAd->BATable.BARecEntry[Idx];
+	}
+	else
+	{
+		// impossible !!!
+		ASSERT(0);
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+
+	ASSERT(pBAEntry);
+
+	// update last rx time
+	NdisGetSystemUpTime(&Now32);
+
+	pBAEntry->rcvSeq = Sequence;
+
+
+	ba_flush_reordering_timeout_mpdus(pAd, pBAEntry, Now32);
+	pBAEntry->LastIndSeqAtTimer = Now32;
+
+	//
+	// Reset Last Indicate Sequence
+	//
+	if (pBAEntry->LastIndSeq == RESET_RCV_SEQ)
+	{
+		ASSERT((pBAEntry->list.qlen == 0) && (pBAEntry->list.next == NULL));
+
+		// reset rcv sequence of BA session
+		pBAEntry->LastIndSeq = Sequence;
+		pBAEntry->LastIndSeqAtTimer = Now32;
+		INDICATE_LEGACY_OR_AMSDU(pAd, pRxBlk, FromWhichBSSID);
+		return;
+	}
+
+
+	//
+	// I. Check if in order.
+	//
+	if (SEQ_STEPONE(Sequence, pBAEntry->LastIndSeq, MAXSEQ))
+	{
+		USHORT  LastIndSeq;
+
+		pBAEntry->LastIndSeq = Sequence;
+		INDICATE_LEGACY_OR_AMSDU(pAd, pRxBlk, FromWhichBSSID);
+ 		LastIndSeq = ba_indicate_reordering_mpdus_in_order(pAd, pBAEntry, pBAEntry->LastIndSeq);
+		if (LastIndSeq != RESET_RCV_SEQ)
+		{
+			pBAEntry->LastIndSeq = LastIndSeq;
+		}
+		pBAEntry->LastIndSeqAtTimer = Now32;
+	}
+	//
+	// II. Drop Duplicated Packet
+	//
+	else if (Sequence == pBAEntry->LastIndSeq)
+	{
+
+		// drop and release packet
+		pBAEntry->nDropPacket++;
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+	}
+	//
+	// III. Drop Old Received Packet
+	//
+	else if (SEQ_SMALLER(Sequence, pBAEntry->LastIndSeq, MAXSEQ))
+	{
+
+		// drop and release packet
+		pBAEntry->nDropPacket++;
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+	}
+	//
+	// IV. Receive Sequence within Window Size
+	//
+	else if (SEQ_SMALLER(Sequence, (((pBAEntry->LastIndSeq+pBAEntry->BAWinSize+1)) & MAXSEQ), MAXSEQ))
+	{
+		ba_enqueue_reordering_packet(pAd, pBAEntry, pRxBlk, FromWhichBSSID);
+	}
+	//
+	// V. Receive seq surpasses Win(lastseq + nMSDU). So refresh all reorder buffer
+	//
+	else
+	{
+#if 0
+		ba_refresh_reordering_mpdus(pAd, pBAEntry);
+		INDICATE_LEGACY_OR_AMSDU(pAd, pRxBlk, FromWhichBSSID);
+#else
+		LONG WinStartSeq, TmpSeq;
+
+
+		TmpSeq = Sequence - (pBAEntry->BAWinSize) -1;
+		if (TmpSeq < 0)
+		{
+			TmpSeq = (MAXSEQ+1) + TmpSeq;
+		}
+		WinStartSeq = (TmpSeq+1) & MAXSEQ;
+		ba_indicate_reordering_mpdus_le_seq(pAd, pBAEntry, WinStartSeq);
+		pBAEntry->LastIndSeq = WinStartSeq; //TmpSeq;
+
+		pBAEntry->LastIndSeqAtTimer = Now32;
+
+		ba_enqueue_reordering_packet(pAd, pBAEntry, pRxBlk, FromWhichBSSID);
+
+		TmpSeq = ba_indicate_reordering_mpdus_in_order(pAd, pBAEntry, pBAEntry->LastIndSeq);
+		if (TmpSeq != RESET_RCV_SEQ)
+		{
+			pBAEntry->LastIndSeq = TmpSeq;
+		}
+#endif
+	}
+}
+
+#endif // DOT11_N_SUPPORT //
+
diff --git a/drivers/staging/rt2860/common/cmm_data.c b/drivers/staging/rt2860/common/cmm_data.c
new file mode 100644
index 00000000000..ac549011ccb
--- /dev/null
+++ b/drivers/staging/rt2860/common/cmm_data.c
@@ -0,0 +1,3466 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+*/
+
+#include "../rt_config.h"
+
+#define MAX_TX_IN_TBTT		(16)
+
+
+UCHAR	SNAP_802_1H[] = {0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00};
+UCHAR	SNAP_BRIDGE_TUNNEL[] = {0xaa, 0xaa, 0x03, 0x00, 0x00, 0xf8};
+// Add Cisco Aironet SNAP heade for CCX2 support
+UCHAR	SNAP_AIRONET[] = {0xaa, 0xaa, 0x03, 0x00, 0x40, 0x96, 0x00, 0x00};
+UCHAR	CKIP_LLC_SNAP[] = {0xaa, 0xaa, 0x03, 0x00, 0x40, 0x96, 0x00, 0x02};
+UCHAR	EAPOL_LLC_SNAP[]= {0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00, 0x88, 0x8e};
+UCHAR	EAPOL[] = {0x88, 0x8e};
+UCHAR   TPID[] = {0x81, 0x00}; /* VLAN related */
+
+UCHAR	IPX[] = {0x81, 0x37};
+UCHAR	APPLE_TALK[] = {0x80, 0xf3};
+UCHAR	RateIdToPlcpSignal[12] = {
+	 0, /* RATE_1 */	1, /* RATE_2 */ 	2, /* RATE_5_5 */	3, /* RATE_11 */	// see BBP spec
+	11, /* RATE_6 */   15, /* RATE_9 */    10, /* RATE_12 */   14, /* RATE_18 */	// see IEEE802.11a-1999 p.14
+	 9, /* RATE_24 */  13, /* RATE_36 */	8, /* RATE_48 */   12  /* RATE_54 */ }; // see IEEE802.11a-1999 p.14
+
+UCHAR	 OfdmSignalToRateId[16] = {
+	RATE_54,  RATE_54,	RATE_54,  RATE_54,	// OFDM PLCP Signal = 0,  1,  2,  3 respectively
+	RATE_54,  RATE_54,	RATE_54,  RATE_54,	// OFDM PLCP Signal = 4,  5,  6,  7 respectively
+	RATE_48,  RATE_24,	RATE_12,  RATE_6,	// OFDM PLCP Signal = 8,  9,  10, 11 respectively
+	RATE_54,  RATE_36,	RATE_18,  RATE_9,	// OFDM PLCP Signal = 12, 13, 14, 15 respectively
+};
+
+UCHAR	 OfdmRateToRxwiMCS[12] = {
+	0,  0,	0,  0,
+	0,  1,	2,  3,	// OFDM rate 6,9,12,18 = rxwi mcs 0,1,2,3
+	4,  5,	6,  7,	// OFDM rate 24,36,48,54 = rxwi mcs 4,5,6,7
+};
+UCHAR	 RxwiMCSToOfdmRate[12] = {
+	RATE_6,  RATE_9,	RATE_12,  RATE_18,
+	RATE_24,  RATE_36,	RATE_48,  RATE_54,	// OFDM rate 6,9,12,18 = rxwi mcs 0,1,2,3
+	4,  5,	6,  7,	// OFDM rate 24,36,48,54 = rxwi mcs 4,5,6,7
+};
+
+char*   MCSToMbps[] = {"1Mbps","2Mbps","5.5Mbps","11Mbps","06Mbps","09Mbps","12Mbps","18Mbps","24Mbps","36Mbps","48Mbps","54Mbps","MM-0","MM-1","MM-2","MM-3","MM-4","MM-5","MM-6","MM-7","MM-8","MM-9","MM-10","MM-11","MM-12","MM-13","MM-14","MM-15","MM-32","ee1","ee2","ee3"};
+
+UCHAR default_cwmin[]={CW_MIN_IN_BITS, CW_MIN_IN_BITS, CW_MIN_IN_BITS-1, CW_MIN_IN_BITS-2};
+UCHAR default_sta_aifsn[]={3,7,2,2};
+
+UCHAR MapUserPriorityToAccessCategory[8] = {QID_AC_BE, QID_AC_BK, QID_AC_BK, QID_AC_BE, QID_AC_VI, QID_AC_VI, QID_AC_VO, QID_AC_VO};
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		API for MLME to transmit management frame to AP (BSS Mode)
+	or station (IBSS Mode)
+
+	Arguments:
+		pAd Pointer to our adapter
+		pData		Pointer to the outgoing 802.11 frame
+		Length		Size of outgoing management frame
+
+	Return Value:
+		NDIS_STATUS_FAILURE
+		NDIS_STATUS_PENDING
+		NDIS_STATUS_SUCCESS
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS MiniportMMRequest(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			QueIdx,
+	IN	PUCHAR			pData,
+	IN	UINT			Length)
+{
+	PNDIS_PACKET	pPacket;
+	NDIS_STATUS  	Status = NDIS_STATUS_SUCCESS;
+	ULONG	 		FreeNum;
+#ifdef RT2860
+	unsigned long	IrqFlags = 0;
+#endif // RT2860 //
+	UCHAR			IrqState;
+	UCHAR			rtmpHwHdr[TXINFO_SIZE + TXWI_SIZE]; //RTMP_HW_HDR_LEN];
+
+	ASSERT(Length <= MGMT_DMA_BUFFER_SIZE);
+
+	QueIdx=3;
+
+	// 2860C use Tx Ring
+
+	IrqState = pAd->irq_disabled;
+#ifdef RT2860
+	if ((pAd->MACVersion == 0x28600100) && (!IrqState))
+		RTMP_IRQ_LOCK(&pAd->irq_lock, IrqFlags);
+#endif // RT2860 //
+
+	do
+	{
+		// Reset is in progress, stop immediately
+		if ( RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS) ||
+			 RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST)||
+			 !RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_START_UP))
+		{
+			Status = NDIS_STATUS_FAILURE;
+			break;
+		}
+
+		// Check Free priority queue
+		// Since we use PBF Queue2 for management frame.  Its corresponding DMA ring should be using TxRing.
+
+		// 2860C use Tx Ring
+		if (pAd->MACVersion == 0x28600100)
+		{
+			FreeNum = GET_TXRING_FREENO(pAd, QueIdx);
+		}
+		else
+		{
+			FreeNum = GET_MGMTRING_FREENO(pAd);
+		}
+
+		if ((FreeNum > 0))
+		{
+			// We need to reserve space for rtmp hardware header. i.e., TxWI for RT2860 and TxInfo+TxWI for RT2870
+			NdisZeroMemory(&rtmpHwHdr, (TXINFO_SIZE + TXWI_SIZE));
+			Status = RTMPAllocateNdisPacket(pAd, &pPacket, (PUCHAR)&rtmpHwHdr, (TXINFO_SIZE + TXWI_SIZE), pData, Length);
+			if (Status != NDIS_STATUS_SUCCESS)
+			{
+				DBGPRINT(RT_DEBUG_WARN, ("MiniportMMRequest (error:: can't allocate NDIS PACKET)\n"));
+				break;
+			}
+
+			//pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_CCK;
+			//pAd->CommonCfg.MlmeRate = RATE_2;
+
+
+			Status = MlmeHardTransmit(pAd, QueIdx, pPacket);
+			if (Status != NDIS_STATUS_SUCCESS)
+				RTMPFreeNdisPacket(pAd, pPacket);
+		}
+		else
+		{
+			pAd->RalinkCounters.MgmtRingFullCount++;
+			DBGPRINT(RT_DEBUG_ERROR, ("Qidx(%d), not enough space in MgmtRing, MgmtRingFullCount=%ld!\n",
+										QueIdx, pAd->RalinkCounters.MgmtRingFullCount));
+		}
+
+	} while (FALSE);
+
+#ifdef RT2860
+	// 2860C use Tx Ring
+	if ((pAd->MACVersion == 0x28600100) && (!IrqState))
+		RTMP_IRQ_UNLOCK(&pAd->irq_lock, IrqFlags);
+#endif // RT2860 //
+
+	return Status;
+}
+
+
+#ifdef RT2860
+NDIS_STATUS MiniportMMRequestUnlock(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			QueIdx,
+	IN	PUCHAR			pData,
+	IN	UINT			Length)
+{
+	PNDIS_PACKET	pPacket;
+	NDIS_STATUS  Status = NDIS_STATUS_SUCCESS;
+	ULONG	 FreeNum;
+	TXWI_STRUC		TXWI;
+	ULONG	SW_TX_IDX;
+	PTXD_STRUC		pTxD;
+
+	QueIdx = 3;
+	ASSERT(Length <= MGMT_DMA_BUFFER_SIZE);
+
+	do
+	{
+		// Reset is in progress, stop immediately
+		if ( RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS) ||
+			 RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST)||
+			 !RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_START_UP))
+		{
+			Status = NDIS_STATUS_FAILURE;
+			break;
+		}
+
+		// Check Free priority queue
+		// Since we use PBF Queue2 for management frame.  Its corresponding DMA ring should be using TxRing.
+		// 2860C use Tx Ring
+		if (pAd->MACVersion == 0x28600100)
+		{
+			FreeNum = GET_TXRING_FREENO(pAd, QueIdx);
+			SW_TX_IDX = pAd->TxRing[QueIdx].TxCpuIdx;
+			pTxD  = (PTXD_STRUC) pAd->TxRing[QueIdx].Cell[SW_TX_IDX].AllocVa;
+		}
+		else
+		{
+			FreeNum = GET_MGMTRING_FREENO(pAd);
+			SW_TX_IDX = pAd->MgmtRing.TxCpuIdx;
+			pTxD  = (PTXD_STRUC) pAd->MgmtRing.Cell[SW_TX_IDX].AllocVa;
+		}
+		if ((FreeNum > 0))
+		{
+			NdisZeroMemory(&TXWI, TXWI_SIZE);
+			Status = RTMPAllocateNdisPacket(pAd, &pPacket, (PUCHAR)&TXWI, TXWI_SIZE, pData, Length);
+			if (Status != NDIS_STATUS_SUCCESS)
+			{
+				DBGPRINT(RT_DEBUG_WARN, ("MiniportMMRequest (error:: can't allocate NDIS PACKET)\n"));
+				break;
+			}
+
+			Status = MlmeHardTransmit(pAd, QueIdx, pPacket);
+			if (Status != NDIS_STATUS_SUCCESS)
+				RTMPFreeNdisPacket(pAd, pPacket);
+		}
+		else
+		{
+			pAd->RalinkCounters.MgmtRingFullCount++;
+			DBGPRINT(RT_DEBUG_ERROR, ("Qidx(%d), not enough space in MgmtRing\n", QueIdx));
+		}
+
+	} while (FALSE);
+
+
+	return Status;
+}
+#endif // RT2860 //
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Copy frame from waiting queue into relative ring buffer and set
+	appropriate ASIC register to kick hardware transmit function
+
+	Arguments:
+		pAd Pointer to our adapter
+		pBuffer 	Pointer to	memory of outgoing frame
+		Length		Size of outgoing management frame
+
+	Return Value:
+		NDIS_STATUS_FAILURE
+		NDIS_STATUS_PENDING
+		NDIS_STATUS_SUCCESS
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS MlmeHardTransmit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			QueIdx,
+	IN	PNDIS_PACKET	pPacket)
+{
+	if (pAd->CommonCfg.RadarDetect.RDMode != RD_NORMAL_MODE)
+	{
+		return NDIS_STATUS_FAILURE;
+	}
+
+#ifdef RT2860
+	if ( pAd->MACVersion == 0x28600100 )
+		return MlmeHardTransmitTxRing(pAd,QueIdx,pPacket);
+	else
+#endif // RT2860 //
+		return MlmeHardTransmitMgmtRing(pAd,QueIdx,pPacket);
+
+}
+
+
+#ifdef RT2860
+NDIS_STATUS MlmeHardTransmitTxRing(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR	QueIdx,
+	IN	PNDIS_PACKET	pPacket)
+{
+	PACKET_INFO 	PacketInfo;
+	PUCHAR			pSrcBufVA;
+	UINT			SrcBufLen;
+	PTXD_STRUC		pTxD;
+#ifdef RT_BIG_ENDIAN
+    PTXD_STRUC      pDestTxD;
+    TXD_STRUC       TxD;
+#endif
+	PHEADER_802_11	pHeader_802_11;
+	BOOLEAN 		bAckRequired, bInsertTimestamp;
+	ULONG			SrcBufPA;
+	UCHAR			MlmeRate;
+	ULONG			SwIdx = pAd->TxRing[QueIdx].TxCpuIdx;
+	PTXWI_STRUC 	pFirstTxWI;
+	ULONG	 FreeNum;
+	MAC_TABLE_ENTRY	*pMacEntry = NULL;
+
+
+	RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pSrcBufVA, &SrcBufLen);
+
+	if (pSrcBufVA == NULL)
+	{
+		// The buffer shouldn't be NULL
+		return NDIS_STATUS_FAILURE;
+	}
+
+	// Make sure MGMT ring resource won't be used by other threads
+	//NdisAcquireSpinLock(&pAd->TxRingLock);
+
+	FreeNum = GET_TXRING_FREENO(pAd, QueIdx);
+
+	if (FreeNum == 0)
+	{
+		//NdisReleaseSpinLock(&pAd->TxRingLock);
+		return NDIS_STATUS_FAILURE;
+	}
+
+	SwIdx = pAd->TxRing[QueIdx].TxCpuIdx;
+
+#ifndef RT_BIG_ENDIAN
+	pTxD  = (PTXD_STRUC) pAd->TxRing[QueIdx].Cell[SwIdx].AllocVa;
+#else
+    pDestTxD  = (PTXD_STRUC)pAd->TxRing[QueIdx].Cell[SwIdx].AllocVa;
+    TxD = *pDestTxD;
+    pTxD = &TxD;
+    RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+#endif
+
+	if (pAd->TxRing[QueIdx].Cell[SwIdx].pNdisPacket)
+	{
+		printk("MlmeHardTransmit Error\n");
+		return NDIS_STATUS_FAILURE;
+	}
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// outgoing frame always wakeup PHY to prevent frame lost
+		if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+			AsicForceWakeup(pAd, TRUE);
+	}
+#endif // CONFIG_STA_SUPPORT //
+	pFirstTxWI	=(PTXWI_STRUC)pSrcBufVA;
+
+	pHeader_802_11 = (PHEADER_802_11) (pSrcBufVA + TXWI_SIZE);
+	if (pHeader_802_11->Addr1[0] & 0x01)
+	{
+		MlmeRate = pAd->CommonCfg.BasicMlmeRate;
+	}
+	else
+	{
+		MlmeRate = pAd->CommonCfg.MlmeRate;
+	}
+
+	if ((pHeader_802_11->FC.Type == BTYPE_DATA) &&
+		(pHeader_802_11->FC.SubType == SUBTYPE_QOS_NULL))
+	{
+		pMacEntry = MacTableLookup(pAd, pHeader_802_11->Addr1);
+	}
+
+	// Verify Mlme rate for a / g bands.
+	if ((pAd->LatchRfRegs.Channel > 14) && (MlmeRate < RATE_6)) // 11A band
+		MlmeRate = RATE_6;
+
+	//
+	// Should not be hard code to set PwrMgmt to 0 (PWR_ACTIVE)
+	// Snice it's been set to 0 while on MgtMacHeaderInit
+	// By the way this will cause frame to be send on PWR_SAVE failed.
+	//
+	//
+	// In WMM-UAPSD, mlme frame should be set psm as power saving but probe request frame
+#ifdef CONFIG_STA_SUPPORT
+    // Data-Null packets alse pass through MMRequest in RT2860, however, we hope control the psm bit to pass APSD
+	if (pHeader_802_11->FC.Type != BTYPE_DATA)
+    {
+    	if ((pHeader_802_11->FC.SubType == SUBTYPE_PROBE_REQ) || !(pAd->CommonCfg.bAPSDCapable && pAd->CommonCfg.APEdcaParm.bAPSDCapable))
+    	{
+    		pHeader_802_11->FC.PwrMgmt = PWR_ACTIVE;
+    	}
+    	else
+    	{
+    		pHeader_802_11->FC.PwrMgmt = pAd->CommonCfg.bAPSDForcePowerSave;
+    	}
+    }
+#endif // CONFIG_STA_SUPPORT //
+
+	bInsertTimestamp = FALSE;
+	if (pHeader_802_11->FC.Type == BTYPE_CNTL) // must be PS-POLL
+	{
+		bAckRequired = FALSE;
+	}
+	else // BTYPE_MGMT or BTYPE_DATA(must be NULL frame)
+	{
+		if (pHeader_802_11->Addr1[0] & 0x01) // MULTICAST, BROADCAST
+		{
+			bAckRequired = FALSE;
+			pHeader_802_11->Duration = 0;
+		}
+		else
+		{
+			bAckRequired = TRUE;
+			pHeader_802_11->Duration = RTMPCalcDuration(pAd, MlmeRate, 14);
+			if (pHeader_802_11->FC.SubType == SUBTYPE_PROBE_RSP)
+			{
+				bInsertTimestamp = TRUE;
+			}
+		}
+	}
+	pHeader_802_11->Sequence = pAd->Sequence++;
+	if (pAd->Sequence > 0xfff)
+		pAd->Sequence = 0;
+	// Before radar detection done, mgmt frame can not be sent but probe req
+	// Because we need to use probe req to trigger driver to send probe req in passive scan
+	if ((pHeader_802_11->FC.SubType != SUBTYPE_PROBE_REQ)
+		&& (pAd->CommonCfg.bIEEE80211H == 1)
+		&& (pAd->CommonCfg.RadarDetect.RDMode != RD_NORMAL_MODE))
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("MlmeHardTransmit --> radar detect not in normal mode !!!\n"));
+		return (NDIS_STATUS_FAILURE);
+	}
+
+#ifdef RT_BIG_ENDIAN
+	RTMPFrameEndianChange(pAd, (PUCHAR)pHeader_802_11, DIR_WRITE, FALSE);
+#endif
+	//
+	// fill scatter-and-gather buffer list into TXD. Internally created NDIS PACKET
+	// should always has only one ohysical buffer, and the whole frame size equals
+	// to the first scatter buffer size
+	//
+
+	// Initialize TX Descriptor
+	// For inter-frame gap, the number is for this frame and next frame
+	// For MLME rate, we will fix as 2Mb to match other vendor's implement
+
+// management frame doesn't need encryption. so use RESERVED_WCID no matter u are sending to specific wcid or not.
+	// Only beacon use Nseq=TRUE. So here we use Nseq=FALSE.
+	if (pMacEntry == NULL)
+	{
+		RTMPWriteTxWI(pAd, pFirstTxWI, FALSE, FALSE, bInsertTimestamp, FALSE, bAckRequired, FALSE,
+		0, RESERVED_WCID, (SrcBufLen - TXWI_SIZE), PID_MGMT, 0,  (UCHAR)pAd->CommonCfg.MlmeTransmit.field.MCS, IFS_BACKOFF, FALSE, &pAd->CommonCfg.MlmeTransmit);
+	}
+	else
+	{
+		RTMPWriteTxWI(pAd, pFirstTxWI, FALSE, FALSE,
+					bInsertTimestamp, FALSE, bAckRequired, FALSE,
+					0, pMacEntry->Aid, (SrcBufLen - TXWI_SIZE),
+					pMacEntry->MaxHTPhyMode.field.MCS, 0,
+					(UCHAR)pMacEntry->MaxHTPhyMode.field.MCS,
+					IFS_BACKOFF, FALSE, &pMacEntry->MaxHTPhyMode);
+	}
+
+	pAd->TxRing[QueIdx].Cell[SwIdx].pNdisPacket = pPacket;
+	pAd->TxRing[QueIdx].Cell[SwIdx].pNextNdisPacket = NULL;
+#ifdef RT_BIG_ENDIAN
+	RTMPWIEndianChange((PUCHAR)pFirstTxWI, TYPE_TXWI);
+#endif
+	SrcBufPA = PCI_MAP_SINGLE(pAd, pSrcBufVA, SrcBufLen, 0, PCI_DMA_TODEVICE);
+
+
+	RTMPWriteTxDescriptor(pAd, pTxD, TRUE, FIFO_EDCA);
+	pTxD->LastSec0 = 1;
+	pTxD->LastSec1 = 1;
+	pTxD->SDLen0 = SrcBufLen;
+	pTxD->SDLen1 = 0;
+	pTxD->SDPtr0 = SrcBufPA;
+	pTxD->DMADONE = 0;
+
+#ifdef RT_BIG_ENDIAN
+    RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+    WriteBackToDescriptor((PUCHAR)pDestTxD, (PUCHAR)pTxD, FALSE, TYPE_TXD);
+#endif
+
+	pAd->RalinkCounters.KickTxCount++;
+	pAd->RalinkCounters.OneSecTxDoneCount++;
+
+   	// Increase TX_CTX_IDX, but write to register later.
+	INC_RING_INDEX(pAd->TxRing[QueIdx].TxCpuIdx, TX_RING_SIZE);
+
+	RTMP_IO_WRITE32(pAd, TX_CTX_IDX0 + QueIdx*0x10,  pAd->TxRing[QueIdx].TxCpuIdx);
+
+	return NDIS_STATUS_SUCCESS;
+}
+#endif // RT2860 //
+
+
+NDIS_STATUS MlmeHardTransmitMgmtRing(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR	QueIdx,
+	IN	PNDIS_PACKET	pPacket)
+{
+	PACKET_INFO 	PacketInfo;
+	PUCHAR			pSrcBufVA;
+	UINT			SrcBufLen;
+	PHEADER_802_11	pHeader_802_11;
+	BOOLEAN 		bAckRequired, bInsertTimestamp;
+	UCHAR			MlmeRate;
+	PTXWI_STRUC 	pFirstTxWI;
+	MAC_TABLE_ENTRY	*pMacEntry = NULL;
+
+	RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pSrcBufVA, &SrcBufLen);
+		RTMP_SEM_LOCK(&pAd->MgmtRingLock);
+
+
+	if (pSrcBufVA == NULL)
+	{
+		RTMP_SEM_UNLOCK(&pAd->MgmtRingLock);
+		return NDIS_STATUS_FAILURE;
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// outgoing frame always wakeup PHY to prevent frame lost
+		if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+			AsicForceWakeup(pAd, TRUE);
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	pFirstTxWI = (PTXWI_STRUC)(pSrcBufVA +  TXINFO_SIZE);
+	pHeader_802_11 = (PHEADER_802_11) (pSrcBufVA + TXINFO_SIZE + TXWI_SIZE); //TXWI_SIZE);
+
+	if (pHeader_802_11->Addr1[0] & 0x01)
+	{
+		MlmeRate = pAd->CommonCfg.BasicMlmeRate;
+	}
+	else
+	{
+		MlmeRate = pAd->CommonCfg.MlmeRate;
+	}
+
+	// Verify Mlme rate for a / g bands.
+	if ((pAd->LatchRfRegs.Channel > 14) && (MlmeRate < RATE_6)) // 11A band
+		MlmeRate = RATE_6;
+
+	if ((pHeader_802_11->FC.Type == BTYPE_DATA) &&
+		(pHeader_802_11->FC.SubType == SUBTYPE_QOS_NULL))
+	{
+		pMacEntry = MacTableLookup(pAd, pHeader_802_11->Addr1);
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// Fixed W52 with Activity scan issue in ABG_MIXED and ABGN_MIXED mode.
+		if (pAd->CommonCfg.PhyMode == PHY_11ABG_MIXED
+#ifdef DOT11_N_SUPPORT
+			|| pAd->CommonCfg.PhyMode == PHY_11ABGN_MIXED
+#endif // DOT11_N_SUPPORT //
+		)
+		{
+			if (pAd->LatchRfRegs.Channel > 14)
+				pAd->CommonCfg.MlmeTransmit.field.MODE = 1;
+			else
+				pAd->CommonCfg.MlmeTransmit.field.MODE = 0;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	//
+	// Should not be hard code to set PwrMgmt to 0 (PWR_ACTIVE)
+	// Snice it's been set to 0 while on MgtMacHeaderInit
+	// By the way this will cause frame to be send on PWR_SAVE failed.
+	//
+	// pHeader_802_11->FC.PwrMgmt = 0; // (pAd->StaCfg.Psm == PWR_SAVE);
+	//
+	// In WMM-UAPSD, mlme frame should be set psm as power saving but probe request frame
+#ifdef CONFIG_STA_SUPPORT
+    // Data-Null packets alse pass through MMRequest in RT2860, however, we hope control the psm bit to pass APSD
+	if ((pHeader_802_11->FC.Type != BTYPE_DATA) && (pHeader_802_11->FC.Type != BTYPE_CNTL))
+	{
+		if ((pAd->StaCfg.Psm == PWR_SAVE) &&
+			(pHeader_802_11->FC.SubType == SUBTYPE_ACTION))
+			pHeader_802_11->FC.PwrMgmt = PWR_SAVE;
+		else
+			pHeader_802_11->FC.PwrMgmt = PWR_ACTIVE;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	bInsertTimestamp = FALSE;
+	if (pHeader_802_11->FC.Type == BTYPE_CNTL) // must be PS-POLL
+	{
+#ifdef CONFIG_STA_SUPPORT
+		//Set PM bit in ps-poll, to fix WLK 1.2  PowerSaveMode_ext failure issue.
+		if ((pAd->OpMode == OPMODE_STA) && (pHeader_802_11->FC.SubType == SUBTYPE_PS_POLL))
+		{
+			pHeader_802_11->FC.PwrMgmt = PWR_SAVE;
+		}
+#endif // CONFIG_STA_SUPPORT //
+		bAckRequired = FALSE;
+	}
+	else // BTYPE_MGMT or BTYPE_DATA(must be NULL frame)
+	{
+		if (pHeader_802_11->Addr1[0] & 0x01) // MULTICAST, BROADCAST
+		{
+			bAckRequired = FALSE;
+			pHeader_802_11->Duration = 0;
+		}
+		else
+		{
+			bAckRequired = TRUE;
+			pHeader_802_11->Duration = RTMPCalcDuration(pAd, MlmeRate, 14);
+			if (pHeader_802_11->FC.SubType == SUBTYPE_PROBE_RSP)
+			{
+				bInsertTimestamp = TRUE;
+			}
+		}
+	}
+
+	pHeader_802_11->Sequence = pAd->Sequence++;
+	if (pAd->Sequence >0xfff)
+		pAd->Sequence = 0;
+
+	// Before radar detection done, mgmt frame can not be sent but probe req
+	// Because we need to use probe req to trigger driver to send probe req in passive scan
+	if ((pHeader_802_11->FC.SubType != SUBTYPE_PROBE_REQ)
+		&& (pAd->CommonCfg.bIEEE80211H == 1)
+		&& (pAd->CommonCfg.RadarDetect.RDMode != RD_NORMAL_MODE))
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("MlmeHardTransmit --> radar detect not in normal mode !!!\n"));
+		RTMP_SEM_UNLOCK(&pAd->MgmtRingLock);
+		return (NDIS_STATUS_FAILURE);
+	}
+
+#ifdef RT_BIG_ENDIAN
+	RTMPFrameEndianChange(pAd, (PUCHAR)pHeader_802_11, DIR_WRITE, FALSE);
+#endif
+
+	//
+	// fill scatter-and-gather buffer list into TXD. Internally created NDIS PACKET
+	// should always has only one ohysical buffer, and the whole frame size equals
+	// to the first scatter buffer size
+	//
+
+	// Initialize TX Descriptor
+	// For inter-frame gap, the number is for this frame and next frame
+	// For MLME rate, we will fix as 2Mb to match other vendor's implement
+
+// management frame doesn't need encryption. so use RESERVED_WCID no matter u are sending to specific wcid or not.
+	if (pMacEntry == NULL)
+	{
+		RTMPWriteTxWI(pAd, pFirstTxWI, FALSE, FALSE, bInsertTimestamp, FALSE, bAckRequired, FALSE,
+		0, RESERVED_WCID, (SrcBufLen - TXINFO_SIZE - TXWI_SIZE), PID_MGMT, 0,  (UCHAR)pAd->CommonCfg.MlmeTransmit.field.MCS, IFS_BACKOFF, FALSE, &pAd->CommonCfg.MlmeTransmit);
+	}
+	else
+	{
+		RTMPWriteTxWI(pAd, pFirstTxWI, FALSE, FALSE,
+					bInsertTimestamp, FALSE, bAckRequired, FALSE,
+					0, pMacEntry->Aid, (SrcBufLen - TXINFO_SIZE - TXWI_SIZE),
+					pMacEntry->MaxHTPhyMode.field.MCS, 0,
+					(UCHAR)pMacEntry->MaxHTPhyMode.field.MCS,
+					IFS_BACKOFF, FALSE, &pMacEntry->MaxHTPhyMode);
+	}
+
+#ifdef RT_BIG_ENDIAN
+	RTMPWIEndianChange((PUCHAR)pFirstTxWI, TYPE_TXWI);
+#endif
+
+	// Now do hardware-depened kick out.
+	HAL_KickOutMgmtTx(pAd, QueIdx, pPacket, pSrcBufVA, SrcBufLen);
+
+	// Make sure to release MGMT ring resource
+	RTMP_SEM_UNLOCK(&pAd->MgmtRingLock);
+	return NDIS_STATUS_SUCCESS;
+}
+
+
+/********************************************************************************
+
+	New DeQueue Procedures.
+
+ ********************************************************************************/
+
+#define DEQUEUE_LOCK(lock, bIntContext, IrqFlags) 				\
+			do{													\
+				if (bIntContext == FALSE)						\
+				RTMP_IRQ_LOCK((lock), IrqFlags);		\
+			}while(0)
+
+#define DEQUEUE_UNLOCK(lock, bIntContext, IrqFlags)				\
+			do{													\
+				if (bIntContext == FALSE)						\
+					RTMP_IRQ_UNLOCK((lock), IrqFlags);	\
+			}while(0)
+
+/*
+	========================================================================
+	Tx Path design algorithm:
+		Basically, we divide the packets into four types, Broadcast/Multicast, 11N Rate(AMPDU, AMSDU, Normal), B/G Rate(ARALINK, Normal),
+		Specific Packet Type. Following show the classification rule and policy for each kinds of packets.
+				Classification Rule=>
+					Multicast: (*addr1 & 0x01) == 0x01
+					Specific : bDHCPFrame, bARPFrame, bEAPOLFrame, etc.
+					11N Rate : If peer support HT
+								(1).AMPDU  -- If TXBA is negotiated.
+								(2).AMSDU  -- If AMSDU is capable for both peer and ourself.
+											*). AMSDU can embedded in a AMPDU, but now we didn't support it.
+								(3).Normal -- Other packets which send as 11n rate.
+
+					B/G Rate : If peer is b/g only.
+								(1).ARALINK-- If both of peer/us supprot Ralink proprietary Aggregation and the TxRate is large than RATE_6
+								(2).Normal -- Other packets which send as b/g rate.
+					Fragment:
+								The packet must be unicast, NOT A-RALINK, NOT A-MSDU, NOT 11n, then can consider about fragment.
+
+				Classified Packet Handle Rule=>
+					Multicast:
+								No ACK, 		//pTxBlk->bAckRequired = FALSE;
+								No WMM, 		//pTxBlk->bWMM = FALSE;
+								No piggyback,   //pTxBlk->bPiggyBack = FALSE;
+								Force LowRate,  //pTxBlk->bForceLowRate = TRUE;
+					Specific :	Basically, for specific packet, we should handle it specifically, but now all specific packets are use
+									the same policy to handle it.
+								Force LowRate,  //pTxBlk->bForceLowRate = TRUE;
+
+					11N Rate :
+								No piggyback,	//pTxBlk->bPiggyBack = FALSE;
+
+								(1).AMSDU
+									pTxBlk->bWMM = TRUE;
+								(2).AMPDU
+									pTxBlk->bWMM = TRUE;
+								(3).Normal
+
+					B/G Rate :
+								(1).ARALINK
+
+								(2).Normal
+	========================================================================
+*/
+static UCHAR TxPktClassification(
+	IN RTMP_ADAPTER *pAd,
+	IN PNDIS_PACKET  pPacket)
+{
+	UCHAR			TxFrameType = TX_UNKOWN_FRAME;
+	UCHAR			Wcid;
+	MAC_TABLE_ENTRY	*pMacEntry = NULL;
+#ifdef DOT11_N_SUPPORT
+	BOOLEAN			bHTRate = FALSE;
+#endif // DOT11_N_SUPPORT //
+
+	Wcid = RTMP_GET_PACKET_WCID(pPacket);
+	if (Wcid == MCAST_WCID)
+	{	// Handle for RA is Broadcast/Multicast Address.
+		return TX_MCAST_FRAME;
+	}
+
+	// Handle for unicast packets
+	pMacEntry = &pAd->MacTab.Content[Wcid];
+	if (RTMP_GET_PACKET_LOWRATE(pPacket))
+	{	// It's a specific packet need to force low rate, i.e., bDHCPFrame, bEAPOLFrame, bWAIFrame
+		TxFrameType = TX_LEGACY_FRAME;
+	}
+#ifdef DOT11_N_SUPPORT
+	else if (IS_HT_RATE(pMacEntry))
+	{	// it's a 11n capable packet
+
+		// Depends on HTPhyMode to check if the peer support the HTRate transmission.
+		// 	Currently didn't support A-MSDU embedded in A-MPDU
+		bHTRate = TRUE;
+		if (RTMP_GET_PACKET_MOREDATA(pPacket) || (pMacEntry->PsMode == PWR_SAVE))
+			TxFrameType = TX_LEGACY_FRAME;
+#ifdef UAPSD_AP_SUPPORT
+		else if (RTMP_GET_PACKET_EOSP(pPacket))
+			TxFrameType = TX_LEGACY_FRAME;
+#endif // UAPSD_AP_SUPPORT //
+		else if((pMacEntry->TXBAbitmap & (1<<(RTMP_GET_PACKET_UP(pPacket)))) != 0)
+			return TX_AMPDU_FRAME;
+		else if(CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_AMSDU_INUSED))
+			return TX_AMSDU_FRAME;
+		else
+			TxFrameType = TX_LEGACY_FRAME;
+	}
+#endif // DOT11_N_SUPPORT //
+	else
+	{	// it's a legacy b/g packet.
+		if ((CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_AGGREGATION_CAPABLE) && pAd->CommonCfg.bAggregationCapable) &&
+			(RTMP_GET_PACKET_TXRATE(pPacket) >= RATE_6) &&
+			(!(OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED) && CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_WMM_CAPABLE))))
+		{	// if peer support Ralink Aggregation, we use it.
+			TxFrameType = TX_RALINK_FRAME;
+		}
+		else
+		{
+			TxFrameType = TX_LEGACY_FRAME;
+		}
+	}
+
+	// Currently, our fragment only support when a unicast packet send as NOT-ARALINK, NOT-AMSDU and NOT-AMPDU.
+	if ((RTMP_GET_PACKET_FRAGMENTS(pPacket) > 1) && (TxFrameType == TX_LEGACY_FRAME))
+		TxFrameType = TX_FRAG_FRAME;
+
+	return TxFrameType;
+}
+
+
+BOOLEAN RTMP_FillTxBlkInfo(
+	IN RTMP_ADAPTER *pAd,
+	IN TX_BLK *pTxBlk)
+{
+	PACKET_INFO			PacketInfo;
+	PNDIS_PACKET		pPacket;
+	PMAC_TABLE_ENTRY	pMacEntry = NULL;
+
+	pPacket = pTxBlk->pPacket;
+	RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pTxBlk->pSrcBufHeader, &pTxBlk->SrcBufLen);
+
+	pTxBlk->Wcid	 	 		= RTMP_GET_PACKET_WCID(pPacket);
+	pTxBlk->apidx		 		= RTMP_GET_PACKET_IF(pPacket);
+	pTxBlk->UserPriority 		= RTMP_GET_PACKET_UP(pPacket);
+	pTxBlk->FrameGap = IFS_HTTXOP;		// ASIC determine Frame Gap
+
+	if (RTMP_GET_PACKET_CLEAR_EAP_FRAME(pTxBlk->pPacket))
+		TX_BLK_SET_FLAG(pTxBlk, fTX_bClearEAPFrame);
+	else
+		TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bClearEAPFrame);
+
+	// Default to clear this flag
+	TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bForceNonQoS);
+
+
+	if (pTxBlk->Wcid == MCAST_WCID)
+	{
+		pTxBlk->pMacEntry = NULL;
+		{
+#ifdef MCAST_RATE_SPECIFIC
+			PUCHAR pDA = GET_OS_PKT_DATAPTR(pPacket);
+			if (((*pDA & 0x01) == 0x01) && (*pDA != 0xff))
+				pTxBlk->pTransmit = &pAd->CommonCfg.MCastPhyMode;
+			else
+#endif // MCAST_RATE_SPECIFIC //
+				pTxBlk->pTransmit = &pAd->MacTab.Content[MCAST_WCID].HTPhyMode;
+		}
+
+		TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bAckRequired);	// AckRequired = FALSE, when broadcast packet in Adhoc mode.
+		//TX_BLK_SET_FLAG(pTxBlk, fTX_bForceLowRate);
+		TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bAllowFrag);
+		TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bWMM);
+		if (RTMP_GET_PACKET_MOREDATA(pPacket))
+		{
+			TX_BLK_SET_FLAG(pTxBlk, fTX_bMoreData);
+		}
+
+	}
+	else
+	{
+		pTxBlk->pMacEntry = &pAd->MacTab.Content[pTxBlk->Wcid];
+		pTxBlk->pTransmit = &pTxBlk->pMacEntry->HTPhyMode;
+
+		pMacEntry = pTxBlk->pMacEntry;
+
+
+		// For all unicast packets, need Ack unless the Ack Policy is not set as NORMAL_ACK.
+		if (pAd->CommonCfg.AckPolicy[pTxBlk->QueIdx] != NORMAL_ACK)
+			TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bAckRequired);
+		else
+			TX_BLK_SET_FLAG(pTxBlk, fTX_bAckRequired);
+
+		{
+
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			{
+
+				// If support WMM, enable it.
+				if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED))
+					TX_BLK_SET_FLAG(pTxBlk, fTX_bWMM);
+			}
+#endif // CONFIG_STA_SUPPORT //
+		}
+
+		if (pTxBlk->TxFrameType == TX_LEGACY_FRAME)
+		{
+			if ( (RTMP_GET_PACKET_LOWRATE(pPacket)) ||
+                ((pAd->OpMode == OPMODE_AP) && (pMacEntry->MaxHTPhyMode.field.MODE == MODE_CCK) && (pMacEntry->MaxHTPhyMode.field.MCS == RATE_1)))
+			{	// Specific packet, i.e., bDHCPFrame, bEAPOLFrame, bWAIFrame, need force low rate.
+				pTxBlk->pTransmit = &pAd->MacTab.Content[MCAST_WCID].HTPhyMode;
+#ifdef DOT11_N_SUPPORT
+				// Modify the WMM bit for ICV issue. If we have a packet with EOSP field need to set as 1, how to handle it???
+				if (IS_HT_STA(pTxBlk->pMacEntry) &&
+					(CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_RALINK_CHIPSET)) &&
+					((pAd->CommonCfg.bRdg == TRUE) && CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_RDG_CAPABLE)))
+				{
+					TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bWMM);
+					TX_BLK_SET_FLAG(pTxBlk, fTX_bForceNonQoS);
+				}
+#endif // DOT11_N_SUPPORT //
+			}
+
+#ifdef DOT11_N_SUPPORT
+			if ( (IS_HT_RATE(pMacEntry) == FALSE) &&
+				(CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_PIGGYBACK_CAPABLE)))
+			{	// Currently piggy-back only support when peer is operate in b/g mode.
+				TX_BLK_SET_FLAG(pTxBlk, fTX_bPiggyBack);
+			}
+#endif // DOT11_N_SUPPORT //
+
+			if (RTMP_GET_PACKET_MOREDATA(pPacket))
+			{
+				TX_BLK_SET_FLAG(pTxBlk, fTX_bMoreData);
+			}
+#ifdef UAPSD_AP_SUPPORT
+			if (RTMP_GET_PACKET_EOSP(pPacket))
+			{
+				TX_BLK_SET_FLAG(pTxBlk, fTX_bWMM_UAPSD_EOSP);
+			}
+#endif // UAPSD_AP_SUPPORT //
+		}
+		else if (pTxBlk->TxFrameType == TX_FRAG_FRAME)
+		{
+			TX_BLK_SET_FLAG(pTxBlk, fTX_bAllowFrag);
+		}
+
+		pMacEntry->DebugTxCount++;
+	}
+
+	return TRUE;
+}
+
+
+BOOLEAN CanDoAggregateTransmit(
+	IN RTMP_ADAPTER *pAd,
+	IN NDIS_PACKET *pPacket,
+	IN TX_BLK		*pTxBlk)
+{
+
+	//printk("Check if can do aggregation! TxFrameType=%d!\n", pTxBlk->TxFrameType);
+
+	if (RTMP_GET_PACKET_WCID(pPacket) == MCAST_WCID)
+		return FALSE;
+
+	if (RTMP_GET_PACKET_DHCP(pPacket) ||
+		RTMP_GET_PACKET_EAPOL(pPacket) ||
+		RTMP_GET_PACKET_WAI(pPacket))
+		return FALSE;
+
+	if ((pTxBlk->TxFrameType == TX_AMSDU_FRAME) &&
+		((pTxBlk->TotalFrameLen + GET_OS_PKT_LEN(pPacket))> (RX_BUFFER_AGGRESIZE - 100)))
+	{	// For AMSDU, allow the packets with total length < max-amsdu size
+		return FALSE;
+	}
+
+	if ((pTxBlk->TxFrameType == TX_RALINK_FRAME) &&
+		(pTxBlk->TxPacketList.Number == 2))
+	{	// For RALINK-Aggregation, allow two frames in one batch.
+		return FALSE;
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	if ((INFRA_ON(pAd)) && (pAd->OpMode == OPMODE_STA)) // must be unicast to AP
+		return TRUE;
+	else
+#endif // CONFIG_STA_SUPPORT //
+		return FALSE;
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		To do the enqueue operation and extract the first item of waiting
+		list. If a number of available shared memory segments could meet
+		the request of extracted item, the extracted item will be fragmented
+		into shared memory segments.
+
+	Arguments:
+		pAd Pointer to our adapter
+		pQueue		Pointer to Waiting Queue
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID RTMPDeQueuePacket(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  BOOLEAN         bIntContext,
+	IN  UCHAR			QIdx, /* BulkOutPipeId */
+	IN  UCHAR           Max_Tx_Packets)
+{
+	PQUEUE_ENTRY    pEntry = NULL;
+	PNDIS_PACKET 	pPacket;
+	NDIS_STATUS     Status = NDIS_STATUS_SUCCESS;
+	UCHAR           Count=0;
+	PQUEUE_HEADER   pQueue;
+	ULONG           FreeNumber[NUM_OF_TX_RING];
+	UCHAR			QueIdx, sQIdx, eQIdx;
+	unsigned long	IrqFlags = 0;
+	BOOLEAN			hasTxDesc = FALSE;
+	TX_BLK			TxBlk;
+	TX_BLK			*pTxBlk;
+
+#ifdef DBG_DIAGNOSE
+	BOOLEAN			firstRound;
+	RtmpDiagStruct	*pDiagStruct = &pAd->DiagStruct;
+#endif
+
+
+	if (QIdx == NUM_OF_TX_RING)
+	{
+		sQIdx = 0;
+		eQIdx = 3;	// 4 ACs, start from 0.
+	}
+	else
+	{
+		sQIdx = eQIdx = QIdx;
+	}
+
+	for (QueIdx=sQIdx; QueIdx <= eQIdx; QueIdx++)
+	{
+		Count=0;
+
+		RT28XX_START_DEQUEUE(pAd, QueIdx, IrqFlags);
+
+#ifdef DBG_DIAGNOSE
+		firstRound = ((QueIdx == 0) ? TRUE : FALSE);
+#endif // DBG_DIAGNOSE //
+
+		while (1)
+		{
+			if ((RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS |
+										fRTMP_ADAPTER_RADIO_OFF |
+										fRTMP_ADAPTER_RESET_IN_PROGRESS |
+										fRTMP_ADAPTER_HALT_IN_PROGRESS |
+										fRTMP_ADAPTER_NIC_NOT_EXIST))))
+			{
+				RT28XX_STOP_DEQUEUE(pAd, QueIdx, IrqFlags);
+				return;
+			}
+
+			if (Count >= Max_Tx_Packets)
+				break;
+
+			DEQUEUE_LOCK(&pAd->irq_lock, bIntContext, IrqFlags);
+			if (&pAd->TxSwQueue[QueIdx] == NULL)
+			{
+#ifdef DBG_DIAGNOSE
+				if (firstRound == TRUE)
+					pDiagStruct->TxSWQueCnt[pDiagStruct->ArrayCurIdx][0]++;
+#endif // DBG_DIAGNOSE //
+				DEQUEUE_UNLOCK(&pAd->irq_lock, bIntContext, IrqFlags);
+				break;
+			}
+
+#ifdef RT2860
+			FreeNumber[QueIdx] = GET_TXRING_FREENO(pAd, QueIdx);
+
+#ifdef DBG_DIAGNOSE
+			if (firstRound == TRUE)
+			{
+				UCHAR	txDescNumLevel, txSwQNumLevel;
+
+				txDescNumLevel = (TX_RING_SIZE - FreeNumber[QueIdx]); // Number of occupied hw desc.
+				txDescNumLevel = ((txDescNumLevel <=15) ? txDescNumLevel : 15);
+				pDiagStruct->TxDescCnt[pDiagStruct->ArrayCurIdx][txDescNumLevel]++;
+
+				txSwQNumLevel = ((pAd->TxSwQueue[QueIdx].Number <=7) ? pAd->TxSwQueue[QueIdx].Number : 8);
+				pDiagStruct->TxSWQueCnt[pDiagStruct->ArrayCurIdx][txSwQNumLevel]++;
+
+				firstRound = FALSE;
+			}
+#endif // DBG_DIAGNOSE //
+
+			if (FreeNumber[QueIdx] <= 5)
+			{
+				// free Tx(QueIdx) resources
+				RTMPFreeTXDUponTxDmaDone(pAd, QueIdx);
+				FreeNumber[QueIdx] = GET_TXRING_FREENO(pAd, QueIdx);
+			}
+#endif // RT2860 //
+
+			// probe the Queue Head
+			pQueue = &pAd->TxSwQueue[QueIdx];
+			if ((pEntry = pQueue->Head) == NULL)
+			{
+				DEQUEUE_UNLOCK(&pAd->irq_lock, bIntContext, IrqFlags);
+				break;
+			}
+
+			pTxBlk = &TxBlk;
+			NdisZeroMemory((PUCHAR)pTxBlk, sizeof(TX_BLK));
+			pTxBlk->QueIdx = QueIdx;
+
+			pPacket = QUEUE_ENTRY_TO_PKT(pEntry);
+
+			// Early check to make sure we have enoguh Tx Resource.
+			hasTxDesc = RT28XX_HAS_ENOUGH_FREE_DESC(pAd, pTxBlk, FreeNumber[QueIdx], pPacket);
+			if (!hasTxDesc)
+			{
+				pAd->PrivateInfo.TxRingFullCnt++;
+
+				DEQUEUE_UNLOCK(&pAd->irq_lock, bIntContext, IrqFlags);
+
+				break;
+			}
+
+			pTxBlk->TxFrameType = TxPktClassification(pAd, pPacket);
+			pEntry = RemoveHeadQueue(pQueue);
+			pTxBlk->TotalFrameNum++;
+			pTxBlk->TotalFragNum += RTMP_GET_PACKET_FRAGMENTS(pPacket);	// The real fragment number maybe vary
+			pTxBlk->TotalFrameLen += GET_OS_PKT_LEN(pPacket);
+			pTxBlk->pPacket = pPacket;
+			InsertTailQueue(&pTxBlk->TxPacketList, PACKET_TO_QUEUE_ENTRY(pPacket));
+
+			if (pTxBlk->TxFrameType == TX_RALINK_FRAME || pTxBlk->TxFrameType == TX_AMSDU_FRAME)
+			{
+				// Enhance SW Aggregation Mechanism
+				if (NEED_QUEUE_BACK_FOR_AGG(pAd, QueIdx, FreeNumber[QueIdx], pTxBlk->TxFrameType))
+				{
+					InsertHeadQueue(pQueue, PACKET_TO_QUEUE_ENTRY(pPacket));
+					DEQUEUE_UNLOCK(&pAd->irq_lock, bIntContext, IrqFlags);
+					break;
+				}
+
+				do{
+					if((pEntry = pQueue->Head) == NULL)
+						break;
+
+					// For TX_AMSDU_FRAME/TX_RALINK_FRAME, Need to check if next pakcet can do aggregation.
+					pPacket = QUEUE_ENTRY_TO_PKT(pEntry);
+					FreeNumber[QueIdx] = GET_TXRING_FREENO(pAd, QueIdx);
+					hasTxDesc = RT28XX_HAS_ENOUGH_FREE_DESC(pAd, pTxBlk, FreeNumber[QueIdx], pPacket);
+					if ((hasTxDesc == FALSE) || (CanDoAggregateTransmit(pAd, pPacket, pTxBlk) == FALSE))
+						break;
+
+					//Remove the packet from the TxSwQueue and insert into pTxBlk
+					pEntry = RemoveHeadQueue(pQueue);
+					ASSERT(pEntry);
+					pPacket = QUEUE_ENTRY_TO_PKT(pEntry);
+					pTxBlk->TotalFrameNum++;
+					pTxBlk->TotalFragNum += RTMP_GET_PACKET_FRAGMENTS(pPacket);	// The real fragment number maybe vary
+					pTxBlk->TotalFrameLen += GET_OS_PKT_LEN(pPacket);
+					InsertTailQueue(&pTxBlk->TxPacketList, PACKET_TO_QUEUE_ENTRY(pPacket));
+				}while(1);
+
+				if (pTxBlk->TxPacketList.Number == 1)
+					pTxBlk->TxFrameType = TX_LEGACY_FRAME;
+			}
+
+
+			Count += pTxBlk->TxPacketList.Number;
+
+				// Do HardTransmit now.
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				Status = STAHardTransmit(pAd, pTxBlk, QueIdx);
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef RT2860
+			DEQUEUE_UNLOCK(&pAd->irq_lock, bIntContext, IrqFlags);
+			// static rate also need NICUpdateFifoStaCounters() function.
+			//if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_TX_RATE_SWITCH_ENABLED))
+				NICUpdateFifoStaCounters(pAd);
+#endif // RT2860 //
+		}
+
+		RT28XX_STOP_DEQUEUE(pAd, QueIdx, IrqFlags);
+
+
+#ifdef BLOCK_NET_IF
+		if ((pAd->blockQueueTab[QueIdx].SwTxQueueBlockFlag == TRUE)
+			&& (pAd->TxSwQueue[QueIdx].Number < 1))
+		{
+			releaseNetIf(&pAd->blockQueueTab[QueIdx]);
+		}
+#endif // BLOCK_NET_IF //
+
+	}
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Calculates the duration which is required to transmit out frames
+	with given size and specified rate.
+
+	Arguments:
+		pAd 	Pointer to our adapter
+		Rate			Transmit rate
+		Size			Frame size in units of byte
+
+	Return Value:
+		Duration number in units of usec
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+USHORT	RTMPCalcDuration(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Rate,
+	IN	ULONG			Size)
+{
+	ULONG	Duration = 0;
+
+	if (Rate < RATE_FIRST_OFDM_RATE) // CCK
+	{
+		if ((Rate > RATE_1) && OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED))
+			Duration = 96;	// 72+24 preamble+plcp
+		else
+			Duration = 192; // 144+48 preamble+plcp
+
+		Duration += (USHORT)((Size << 4) / RateIdTo500Kbps[Rate]);
+		if ((Size << 4) % RateIdTo500Kbps[Rate])
+			Duration ++;
+	}
+	else if (Rate <= RATE_LAST_OFDM_RATE)// OFDM rates
+	{
+		Duration = 20 + 6;		// 16+4 preamble+plcp + Signal Extension
+		Duration += 4 * (USHORT)((11 + Size * 4) / RateIdTo500Kbps[Rate]);
+		if ((11 + Size * 4) % RateIdTo500Kbps[Rate])
+			Duration += 4;
+	}
+	else	//mimo rate
+	{
+		Duration = 20 + 6;		// 16+4 preamble+plcp + Signal Extension
+	}
+
+	return (USHORT)Duration;
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Calculates the duration which is required to transmit out frames
+	with given size and specified rate.
+
+	Arguments:
+		pTxWI		Pointer to head of each MPDU to HW.
+		Ack 		Setting for Ack requirement bit
+		Fragment	Setting for Fragment bit
+		RetryMode	Setting for retry mode
+		Ifs 		Setting for IFS gap
+		Rate		Setting for transmit rate
+		Service 	Setting for service
+		Length		Frame length
+		TxPreamble	Short or Long preamble when using CCK rates
+		QueIdx - 0-3, according to 802.11e/d4.4 June/2003
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+    See also : BASmartHardTransmit()    !!!
+
+	========================================================================
+*/
+VOID RTMPWriteTxWI(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTXWI_STRUC 	pOutTxWI,
+	IN	BOOLEAN			FRAG,
+	IN	BOOLEAN			CFACK,
+	IN	BOOLEAN			InsTimestamp,
+	IN	BOOLEAN 		AMPDU,
+	IN	BOOLEAN 		Ack,
+	IN	BOOLEAN 		NSeq,		// HW new a sequence.
+	IN	UCHAR			BASize,
+	IN	UCHAR			WCID,
+	IN	ULONG			Length,
+	IN	UCHAR 			PID,
+	IN	UCHAR			TID,
+	IN	UCHAR			TxRate,
+	IN	UCHAR			Txopmode,
+	IN	BOOLEAN			CfAck,
+	IN	HTTRANSMIT_SETTING	*pTransmit)
+{
+	PMAC_TABLE_ENTRY	pMac = NULL;
+	TXWI_STRUC 		TxWI;
+	PTXWI_STRUC 	pTxWI;
+
+	if (WCID < MAX_LEN_OF_MAC_TABLE)
+		pMac = &pAd->MacTab.Content[WCID];
+
+	//
+	// Always use Long preamble before verifiation short preamble functionality works well.
+	// Todo: remove the following line if short preamble functionality works
+	//
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED);
+	NdisZeroMemory(&TxWI, TXWI_SIZE);
+	pTxWI = &TxWI;
+
+	pTxWI->FRAG= FRAG;
+
+	pTxWI->CFACK = CFACK;
+	pTxWI->TS= InsTimestamp;
+	pTxWI->AMPDU = AMPDU;
+	pTxWI->ACK = Ack;
+	pTxWI->txop= Txopmode;
+
+	pTxWI->NSEQ = NSeq;
+	// John tune the performace with Intel Client in 20 MHz performance
+#ifdef DOT11_N_SUPPORT
+	BASize = pAd->CommonCfg.TxBASize;
+
+	if( BASize >7 )
+		BASize =7;
+	pTxWI->BAWinSize = BASize;
+	pTxWI->ShortGI = pTransmit->field.ShortGI;
+	pTxWI->STBC = pTransmit->field.STBC;
+#endif // DOT11_N_SUPPORT //
+
+	pTxWI->WirelessCliID = WCID;
+	pTxWI->MPDUtotalByteCount = Length;
+	pTxWI->PacketId = PID;
+
+	// If CCK or OFDM, BW must be 20
+	pTxWI->BW = (pTransmit->field.MODE <= MODE_OFDM) ? (BW_20) : (pTransmit->field.BW);
+#ifdef DOT11N_DRAFT3
+	if (pTxWI->BW)
+		pTxWI->BW = (pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth == 0) ? (BW_20) : (pTransmit->field.BW);
+#endif // DOT11N_DRAFT3 //
+
+	pTxWI->MCS = pTransmit->field.MCS;
+	pTxWI->PHYMODE = pTransmit->field.MODE;
+	pTxWI->CFACK = CfAck;
+
+#ifdef DOT11_N_SUPPORT
+	if (pMac)
+	{
+		if (pAd->CommonCfg.bMIMOPSEnable)
+		{
+			if ((pMac->MmpsMode == MMPS_DYNAMIC) && (pTransmit->field.MCS > 7))
+			{
+				// Dynamic MIMO Power Save Mode
+				pTxWI->MIMOps = 1;
+			}
+			else if (pMac->MmpsMode == MMPS_STATIC)
+			{
+				// Static MIMO Power Save Mode
+				if (pTransmit->field.MODE >= MODE_HTMIX && pTransmit->field.MCS > 7)
+				{
+					pTxWI->MCS = 7;
+					pTxWI->MIMOps = 0;
+				}
+			}
+		}
+		//pTxWI->MIMOps = (pMac->PsMode == PWR_MMPS)? 1:0;
+		if (pMac->bIAmBadAtheros && (pMac->WepStatus != Ndis802_11WEPDisabled))
+		{
+			pTxWI->MpduDensity = 7;
+		}
+		else
+		{
+			pTxWI->MpduDensity = pMac->MpduDensity;
+		}
+	}
+#endif // DOT11_N_SUPPORT //
+
+	pTxWI->PacketId = pTxWI->MCS;
+	NdisMoveMemory(pOutTxWI, &TxWI, sizeof(TXWI_STRUC));
+}
+
+
+VOID RTMPWriteTxWI_Data(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	OUT PTXWI_STRUC		pTxWI,
+	IN	TX_BLK				*pTxBlk)
+{
+	HTTRANSMIT_SETTING	*pTransmit;
+	PMAC_TABLE_ENTRY	pMacEntry;
+#ifdef DOT11_N_SUPPORT
+	UCHAR				BASize;
+#endif // DOT11_N_SUPPORT //
+
+
+	ASSERT(pTxWI);
+
+	pTransmit = pTxBlk->pTransmit;
+	pMacEntry = pTxBlk->pMacEntry;
+
+
+	//
+	// Always use Long preamble before verifiation short preamble functionality works well.
+	// Todo: remove the following line if short preamble functionality works
+	//
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED);
+	NdisZeroMemory(pTxWI, TXWI_SIZE);
+
+	pTxWI->FRAG		= TX_BLK_TEST_FLAG(pTxBlk, fTX_bAllowFrag);
+	pTxWI->ACK		= TX_BLK_TEST_FLAG(pTxBlk, fTX_bAckRequired);
+	pTxWI->txop		= pTxBlk->FrameGap;
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+	if (pMacEntry &&
+		(pAd->StaCfg.BssType == BSS_INFRA) &&
+		(pMacEntry->ValidAsDls == TRUE))
+		pTxWI->WirelessCliID = BSSID_WCID;
+	else
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+		pTxWI->WirelessCliID		= pTxBlk->Wcid;
+
+	pTxWI->MPDUtotalByteCount	= pTxBlk->MpduHeaderLen + pTxBlk->SrcBufLen;
+	pTxWI->CFACK				= TX_BLK_TEST_FLAG(pTxBlk, fTX_bPiggyBack);
+
+	// If CCK or OFDM, BW must be 20
+	pTxWI->BW = (pTransmit->field.MODE <= MODE_OFDM) ? (BW_20) : (pTransmit->field.BW);
+#ifdef DOT11_N_SUPPORT
+#ifdef DOT11N_DRAFT3
+	if (pTxWI->BW)
+		pTxWI->BW = (pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth == 0) ? (BW_20) : (pTransmit->field.BW);
+#endif // DOT11N_DRAFT3 //
+	pTxWI->AMPDU	= ((pTxBlk->TxFrameType == TX_AMPDU_FRAME) ? TRUE : FALSE);
+
+	// John tune the performace with Intel Client in 20 MHz performance
+	BASize = pAd->CommonCfg.TxBASize;
+	if((pTxBlk->TxFrameType == TX_AMPDU_FRAME) && (pMacEntry))
+	{
+		UCHAR		RABAOriIdx = 0;	//The RA's BA Originator table index.
+
+		RABAOriIdx = pTxBlk->pMacEntry->BAOriWcidArray[pTxBlk->UserPriority];
+		BASize = pAd->BATable.BAOriEntry[RABAOriIdx].BAWinSize;
+	}
+
+	pTxWI->TxBF = pTransmit->field.TxBF;
+	pTxWI->BAWinSize = BASize;
+	pTxWI->ShortGI = pTransmit->field.ShortGI;
+	pTxWI->STBC = pTransmit->field.STBC;
+#endif // DOT11_N_SUPPORT //
+
+	pTxWI->MCS = pTransmit->field.MCS;
+	pTxWI->PHYMODE = pTransmit->field.MODE;
+
+#ifdef DOT11_N_SUPPORT
+	if (pMacEntry)
+	{
+		if ((pMacEntry->MmpsMode == MMPS_DYNAMIC) && (pTransmit->field.MCS > 7))
+		{
+			// Dynamic MIMO Power Save Mode
+			pTxWI->MIMOps = 1;
+		}
+		else if (pMacEntry->MmpsMode == MMPS_STATIC)
+		{
+			// Static MIMO Power Save Mode
+			if (pTransmit->field.MODE >= MODE_HTMIX && pTransmit->field.MCS > 7)
+			{
+				pTxWI->MCS = 7;
+				pTxWI->MIMOps = 0;
+			}
+		}
+
+		if (pMacEntry->bIAmBadAtheros && (pMacEntry->WepStatus != Ndis802_11WEPDisabled))
+		{
+			pTxWI->MpduDensity = 7;
+		}
+		else
+		{
+			pTxWI->MpduDensity = pMacEntry->MpduDensity;
+		}
+	}
+#endif // DOT11_N_SUPPORT //
+
+#ifdef DBG_DIAGNOSE
+		if (pTxBlk->QueIdx== 0)
+		{
+			pAd->DiagStruct.TxDataCnt[pAd->DiagStruct.ArrayCurIdx]++;
+			pAd->DiagStruct.TxMcsCnt[pAd->DiagStruct.ArrayCurIdx][pTxWI->MCS]++;
+		}
+#endif // DBG_DIAGNOSE //
+
+	// for rate adapation
+	pTxWI->PacketId = pTxWI->MCS;
+}
+
+
+VOID RTMPWriteTxWI_Cache(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	OUT PTXWI_STRUC		pTxWI,
+	IN	TX_BLK				*pTxBlk)
+{
+	PHTTRANSMIT_SETTING	pTransmit;
+	PMAC_TABLE_ENTRY	pMacEntry;
+
+	//
+	// update TXWI
+	//
+	pMacEntry = pTxBlk->pMacEntry;
+	pTransmit = pTxBlk->pTransmit;
+
+	if (pMacEntry->bAutoTxRateSwitch)
+	{
+		pTxWI->txop = IFS_HTTXOP;
+
+		// If CCK or OFDM, BW must be 20
+		pTxWI->BW = (pTransmit->field.MODE <= MODE_OFDM) ? (BW_20) : (pTransmit->field.BW);
+		pTxWI->ShortGI = pTransmit->field.ShortGI;
+		pTxWI->STBC = pTransmit->field.STBC;
+
+		pTxWI->MCS = pTransmit->field.MCS;
+		pTxWI->PHYMODE = pTransmit->field.MODE;
+
+		// set PID for TxRateSwitching
+		pTxWI->PacketId = pTransmit->field.MCS;
+	}
+
+#ifdef DOT11_N_SUPPORT
+	pTxWI->AMPDU = ((pMacEntry->NoBADataCountDown == 0) ? TRUE: FALSE);
+	pTxWI->MIMOps = 0;
+
+#ifdef DOT11N_DRAFT3
+	if (pTxWI->BW)
+		pTxWI->BW = (pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth == 0) ? (BW_20) : (pTransmit->field.BW);
+#endif // DOT11N_DRAFT3 //
+
+	if (pAd->CommonCfg.bMIMOPSEnable)
+	{
+		// MIMO Power Save Mode
+		if ((pMacEntry->MmpsMode == MMPS_DYNAMIC) && (pTransmit->field.MCS > 7))
+		{
+			// Dynamic MIMO Power Save Mode
+			pTxWI->MIMOps = 1;
+		}
+		else if (pMacEntry->MmpsMode == MMPS_STATIC)
+		{
+			// Static MIMO Power Save Mode
+			if ((pTransmit->field.MODE >= MODE_HTMIX) && (pTransmit->field.MCS > 7))
+			{
+				pTxWI->MCS = 7;
+				pTxWI->MIMOps = 0;
+			}
+		}
+	}
+#endif // DOT11_N_SUPPORT //
+
+#ifdef DBG_DIAGNOSE
+	if (pTxBlk->QueIdx== 0)
+	{
+		pAd->DiagStruct.TxDataCnt[pAd->DiagStruct.ArrayCurIdx]++;
+		pAd->DiagStruct.TxMcsCnt[pAd->DiagStruct.ArrayCurIdx][pTxWI->MCS]++;
+	}
+#endif // DBG_DIAGNOSE //
+
+	pTxWI->MPDUtotalByteCount = pTxBlk->MpduHeaderLen + pTxBlk->SrcBufLen;
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Calculates the duration which is required to transmit out frames
+	with given size and specified rate.
+
+	Arguments:
+		pTxD		Pointer to transmit descriptor
+		Ack 		Setting for Ack requirement bit
+		Fragment	Setting for Fragment bit
+		RetryMode	Setting for retry mode
+		Ifs 		Setting for IFS gap
+		Rate		Setting for transmit rate
+		Service 	Setting for service
+		Length		Frame length
+		TxPreamble	Short or Long preamble when using CCK rates
+		QueIdx - 0-3, according to 802.11e/d4.4 June/2003
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	========================================================================
+*/
+VOID RTMPWriteTxDescriptor(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTXD_STRUC		pTxD,
+	IN	BOOLEAN 		bWIV,
+	IN	UCHAR			QueueSEL)
+{
+	//
+	// Always use Long preamble before verifiation short preamble functionality works well.
+	// Todo: remove the following line if short preamble functionality works
+	//
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED);
+
+	pTxD->WIV	= (bWIV) ? 1: 0;
+	pTxD->QSEL= (QueueSEL);
+	if (pAd->bGenOneHCCA == TRUE)
+		pTxD->QSEL= FIFO_HCCA;
+	pTxD->DMADONE = 0;
+}
+
+
+// should be called only when -
+// 1. MEADIA_CONNECTED
+// 2. AGGREGATION_IN_USED
+// 3. Fragmentation not in used
+// 4. either no previous frame (pPrevAddr1=NULL) .OR. previoud frame is aggregatible
+BOOLEAN TxFrameIsAggregatible(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pPrevAddr1,
+	IN	PUCHAR			p8023hdr)
+{
+
+	// can't aggregate EAPOL (802.1x) frame
+	if ((p8023hdr[12] == 0x88) && (p8023hdr[13] == 0x8e))
+		return FALSE;
+
+	// can't aggregate multicast/broadcast frame
+	if (p8023hdr[0] & 0x01)
+		return FALSE;
+
+	if (INFRA_ON(pAd)) // must be unicast to AP
+		return TRUE;
+	else if ((pPrevAddr1 == NULL) || MAC_ADDR_EQUAL(pPrevAddr1, p8023hdr)) // unicast to same STA
+		return TRUE;
+	else
+		return FALSE;
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+	   Check the MSDU Aggregation policy
+	1.HT aggregation is A-MSDU
+	2.legaacy rate aggregation is software aggregation by Ralink.
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+BOOLEAN PeerIsAggreOn(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	ULONG		   TxRate,
+	IN	PMAC_TABLE_ENTRY pMacEntry)
+{
+	ULONG	AFlags = (fCLIENT_STATUS_AMSDU_INUSED | fCLIENT_STATUS_AGGREGATION_CAPABLE);
+
+	if (pMacEntry != NULL && CLIENT_STATUS_TEST_FLAG(pMacEntry, AFlags))
+	{
+#ifdef DOT11_N_SUPPORT
+		if (pMacEntry->HTPhyMode.field.MODE >= MODE_HTMIX)
+		{
+			return TRUE;
+		}
+#endif // DOT11_N_SUPPORT //
+
+#ifdef AGGREGATION_SUPPORT
+		if (TxRate >= RATE_6 && pAd->CommonCfg.bAggregationCapable && (!(OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED) && CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_WMM_CAPABLE))))
+		{	// legacy  Ralink Aggregation support
+			return TRUE;
+		}
+#endif // AGGREGATION_SUPPORT //
+	}
+
+	return FALSE;
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Check and fine the packet waiting in SW queue with highest priority
+
+	Arguments:
+		pAd Pointer to our adapter
+
+	Return Value:
+		pQueue		Pointer to Waiting Queue
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+PQUEUE_HEADER	RTMPCheckTxSwQueue(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT PUCHAR			pQueIdx)
+{
+
+	ULONG	Number;
+
+	Number = pAd->TxSwQueue[QID_AC_BK].Number
+			 + pAd->TxSwQueue[QID_AC_BE].Number
+			 + pAd->TxSwQueue[QID_AC_VI].Number
+			 + pAd->TxSwQueue[QID_AC_VO].Number
+			 + pAd->TxSwQueue[QID_HCCA].Number;
+
+	if (pAd->TxSwQueue[QID_AC_VO].Head != NULL)
+	{
+		*pQueIdx = QID_AC_VO;
+		return (&pAd->TxSwQueue[QID_AC_VO]);
+	}
+	else if (pAd->TxSwQueue[QID_AC_VI].Head != NULL)
+	{
+		*pQueIdx = QID_AC_VI;
+		return (&pAd->TxSwQueue[QID_AC_VI]);
+	}
+	else if (pAd->TxSwQueue[QID_AC_BE].Head != NULL)
+	{
+		*pQueIdx = QID_AC_BE;
+		return (&pAd->TxSwQueue[QID_AC_BE]);
+	}
+	else if (pAd->TxSwQueue[QID_AC_BK].Head != NULL)
+	{
+		*pQueIdx = QID_AC_BK;
+		return (&pAd->TxSwQueue[QID_AC_BK]);
+	}
+	else if (pAd->TxSwQueue[QID_HCCA].Head != NULL)
+	{
+		*pQueIdx = QID_HCCA;
+		return (&pAd->TxSwQueue[QID_HCCA]);
+	}
+
+	// No packet pending in Tx Sw queue
+	*pQueIdx = QID_AC_BK;
+
+	return (NULL);
+}
+
+
+#ifdef RT2860
+BOOLEAN  RTMPFreeTXDUponTxDmaDone(
+	IN PRTMP_ADAPTER	pAd,
+	IN UCHAR			QueIdx)
+{
+	PRTMP_TX_RING pTxRing;
+	PTXD_STRUC	  pTxD;
+#ifdef	RT_BIG_ENDIAN
+    PTXD_STRUC      pDestTxD;
+#endif
+	PNDIS_PACKET  pPacket;
+	UCHAR	FREE = 0;
+	TXD_STRUC	TxD, *pOriTxD;
+	//ULONG		IrqFlags;
+	BOOLEAN			bReschedule = FALSE;
+
+
+	ASSERT(QueIdx < NUM_OF_TX_RING);
+	pTxRing = &pAd->TxRing[QueIdx];
+
+	RTMP_IO_READ32(pAd, TX_DTX_IDX0 + QueIdx * RINGREG_DIFF, &pTxRing->TxDmaIdx);
+	while (pTxRing->TxSwFreeIdx != pTxRing->TxDmaIdx)
+	{
+#ifdef RALINK_ATE
+#ifdef RALINK_28xx_QA
+		PHEADER_802_11	pHeader80211;
+
+		if ((ATE_ON(pAd)) && (pAd->ate.bQATxStart == TRUE))
+		{
+			if (pAd->ate.QID == QueIdx)
+			{
+				pAd->ate.TxDoneCount++;
+				//pAd->ate.Repeat++;
+				pAd->RalinkCounters.KickTxCount++;
+
+				/* always use QID_AC_BE and FIFO_EDCA */
+				ASSERT(pAd->ate.QID == 0);
+				pAd->ate.TxAc0++;
+
+				FREE++;
+#ifndef RT_BIG_ENDIAN
+				pTxD = (PTXD_STRUC) (pTxRing->Cell[pTxRing->TxSwFreeIdx].AllocVa);
+				pOriTxD = pTxD;
+		        NdisMoveMemory(&TxD, pTxD, sizeof(TXD_STRUC));
+				pTxD = &TxD;
+#else
+		        pDestTxD = (PTXD_STRUC) (pTxRing->Cell[pTxRing->TxSwFreeIdx].AllocVa);
+		        pOriTxD = pDestTxD ;
+		        TxD = *pDestTxD;
+		        pTxD = &TxD;
+		        RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+#endif
+				pTxD->DMADONE = 0;
+
+				pHeader80211 = pTxRing->Cell[pTxRing->TxSwFreeIdx].DmaBuf.AllocVa + sizeof(TXWI_STRUC);
+#ifdef RT_BIG_ENDIAN
+				RTMPFrameEndianChange(pAd, (PUCHAR)pHeader80211, DIR_READ, FALSE);
+#endif
+				pHeader80211->Sequence = ++pAd->ate.seq;
+#ifdef RT_BIG_ENDIAN
+				RTMPFrameEndianChange(pAd, (PUCHAR)pHeader80211, DIR_WRITE, FALSE);
+#endif
+
+				if  ((pAd->ate.bQATxStart == TRUE) && (pAd->ate.Mode & ATE_TXFRAME) && (pAd->ate.TxDoneCount < pAd->ate.TxCount))
+				{
+					pAd->RalinkCounters.TransmittedByteCount +=  (pTxD->SDLen1 + pTxD->SDLen0);
+					pAd->RalinkCounters.OneSecDmaDoneCount[QueIdx] ++;
+					INC_RING_INDEX(pTxRing->TxSwFreeIdx, TX_RING_SIZE);
+					/* get tx_tdx_idx again */
+					RTMP_IO_READ32(pAd, TX_DTX_IDX0 + QueIdx * RINGREG_DIFF ,  &pTxRing->TxDmaIdx);
+					goto kick_out;
+				}
+				else if ((pAd->ate.TxStatus == 1)/* or (pAd->ate.bQATxStart == TRUE) ??? */ && (pAd->ate.TxDoneCount == pAd->ate.TxCount))//<========================PETER
+				{
+					DBGPRINT(RT_DEBUG_TRACE,("all Tx is done\n"));
+					// Tx status enters idle mode.
+					pAd->ate.TxStatus = 0;
+				}
+				else if (!(pAd->ate.Mode & ATE_TXFRAME))
+				{
+					/* not complete sending yet, but someone press the Stop TX botton. */
+					DBGPRINT(RT_DEBUG_ERROR,("not complete sending yet, but someone pressed the Stop TX bottom\n"));
+					DBGPRINT(RT_DEBUG_ERROR,("pAd->ate.Mode = 0x%02x\n", pAd->ate.Mode));
+				}
+				else
+				{
+					DBGPRINT(RT_DEBUG_OFF,("pTxRing->TxSwFreeIdx = %d\n", pTxRing->TxSwFreeIdx));
+  				}
+#ifndef RT_BIG_ENDIAN
+	        	NdisMoveMemory(pOriTxD, pTxD, sizeof(TXD_STRUC));
+#else
+        		RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+        		*pDestTxD = TxD;
+#endif // RT_BIG_ENDIAN //
+
+				INC_RING_INDEX(pTxRing->TxSwFreeIdx, TX_RING_SIZE);
+				continue;
+			}
+		}
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+
+		// static rate also need NICUpdateFifoStaCounters() function.
+		//if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_TX_RATE_SWITCH_ENABLED))
+			NICUpdateFifoStaCounters(pAd);
+
+		/* Note : If (pAd->ate.bQATxStart == TRUE), we will never reach here. */
+		FREE++;
+#ifndef RT_BIG_ENDIAN
+                pTxD = (PTXD_STRUC) (pTxRing->Cell[pTxRing->TxSwFreeIdx].AllocVa);
+		pOriTxD = pTxD;
+                NdisMoveMemory(&TxD, pTxD, sizeof(TXD_STRUC));
+		pTxD = &TxD;
+#else
+        pDestTxD = (PTXD_STRUC) (pTxRing->Cell[pTxRing->TxSwFreeIdx].AllocVa);
+        pOriTxD = pDestTxD ;
+        TxD = *pDestTxD;
+        pTxD = &TxD;
+        RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+#endif
+
+		pTxD->DMADONE = 0;
+
+
+#ifdef RALINK_ATE
+		/* Execution of this block is not allowed when ATE is running. */
+		if (!(ATE_ON(pAd)))
+#endif // RALINK_ATE //
+/*====================================================================*/
+		{
+			pPacket = pTxRing->Cell[pTxRing->TxSwFreeIdx].pNdisPacket;
+			if (pPacket)
+			{
+#ifdef CONFIG_5VT_ENHANCE
+				if (RTMP_GET_PACKET_5VT(pPacket))
+					PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr1, 16, PCI_DMA_TODEVICE);
+				else
+#endif // CONFIG_5VT_ENHANCE //
+					PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr1, pTxD->SDLen1, PCI_DMA_TODEVICE);
+				RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_SUCCESS);
+			}
+			//Always assign pNdisPacket as NULL after clear
+			pTxRing->Cell[pTxRing->TxSwFreeIdx].pNdisPacket = NULL;
+
+			pPacket = pTxRing->Cell[pTxRing->TxSwFreeIdx].pNextNdisPacket;
+
+			ASSERT(pPacket == NULL);
+			if (pPacket)
+			{
+#ifdef CONFIG_5VT_ENHANCE
+				if (RTMP_GET_PACKET_5VT(pPacket))
+					PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr1, 16, PCI_DMA_TODEVICE);
+				else
+#endif // CONFIG_5VT_ENHANCE //
+					PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr1, pTxD->SDLen1, PCI_DMA_TODEVICE);
+				RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_SUCCESS);
+			}
+			//Always assign pNextNdisPacket as NULL after clear
+			pTxRing->Cell[pTxRing->TxSwFreeIdx].pNextNdisPacket = NULL;
+		}
+/*====================================================================*/
+
+		pAd->RalinkCounters.TransmittedByteCount +=  (pTxD->SDLen1 + pTxD->SDLen0);
+		pAd->RalinkCounters.OneSecDmaDoneCount[QueIdx] ++;
+		INC_RING_INDEX(pTxRing->TxSwFreeIdx, TX_RING_SIZE);
+		/* get tx_tdx_idx again */
+		RTMP_IO_READ32(pAd, TX_DTX_IDX0 + QueIdx * RINGREG_DIFF ,  &pTxRing->TxDmaIdx);
+#ifdef RT_BIG_ENDIAN
+        RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+        *pDestTxD = TxD;
+#else
+        NdisMoveMemory(pOriTxD, pTxD, sizeof(TXD_STRUC));
+#endif
+
+#ifdef RALINK_ATE
+#ifdef RALINK_28xx_QA
+kick_out:
+#endif // RALINK_28xx_QA //
+
+		//
+		// ATE_TXCONT mode also need to send some normal frames, so let it in.
+		// ATE_STOP must be changed not to be 0xff
+		// to prevent it from running into this block.
+		//
+		if ((pAd->ate.Mode & ATE_TXFRAME) && (pAd->ate.QID == QueIdx))
+		{
+			// TxDoneCount++ has been done if QA is used.
+			if (pAd->ate.bQATxStart == FALSE)
+			{
+				pAd->ate.TxDoneCount++;
+			}
+			if (((pAd->ate.TxCount - pAd->ate.TxDoneCount + 1) >= TX_RING_SIZE))
+			{
+				/* Note : We increase TxCpuIdx here, not TxSwFreeIdx ! */
+				INC_RING_INDEX(pAd->TxRing[QueIdx].TxCpuIdx, TX_RING_SIZE);
+#ifndef RT_BIG_ENDIAN//<==========================PETER
+				pTxD = (PTXD_STRUC) (pTxRing->Cell[pAd->TxRing[QueIdx].TxCpuIdx].AllocVa);
+				pOriTxD = pTxD;
+		        NdisMoveMemory(&TxD, pTxD, sizeof(TXD_STRUC));
+				pTxD = &TxD;
+#else
+		        pDestTxD = (PTXD_STRUC) (pTxRing->Cell[pAd->TxRing[QueIdx].TxCpuIdx].AllocVa);
+		        pOriTxD = pDestTxD ;
+		        TxD = *pDestTxD;
+		        pTxD = &TxD;
+		        RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+#endif
+				pTxD->DMADONE = 0;
+#ifndef RT_BIG_ENDIAN//<==========================PETER
+        		NdisMoveMemory(pOriTxD, pTxD, sizeof(TXD_STRUC));
+#else
+        		RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+        		*pDestTxD = TxD;
+#endif
+				// kick Tx-Ring.
+				RTMP_IO_WRITE32(pAd, TX_CTX_IDX0 + QueIdx * RINGREG_DIFF, pAd->TxRing[QueIdx].TxCpuIdx);
+				pAd->RalinkCounters.KickTxCount++;
+			}
+		}
+#endif // RALINK_ATE //
+	}
+
+
+	return  bReschedule;
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Process TX Rings DMA Done interrupt, running in DPC level
+
+	Arguments:
+		Adapter 	Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	========================================================================
+*/
+BOOLEAN	RTMPHandleTxRingDmaDoneInterrupt(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	INT_SOURCE_CSR_STRUC TxRingBitmap)
+{
+    unsigned long	IrqFlags;
+	BOOLEAN			bReschedule = FALSE;
+
+	// Make sure Tx ring resource won't be used by other threads
+
+	RTMP_IRQ_LOCK(&pAd->irq_lock, IrqFlags);
+
+	if (TxRingBitmap.field.Ac0DmaDone)
+		bReschedule = RTMPFreeTXDUponTxDmaDone(pAd, QID_AC_BE);
+
+	if (TxRingBitmap.field.HccaDmaDone)
+		bReschedule |= RTMPFreeTXDUponTxDmaDone(pAd, QID_HCCA);
+
+	if (TxRingBitmap.field.Ac3DmaDone)
+		bReschedule |= RTMPFreeTXDUponTxDmaDone(pAd, QID_AC_VO);
+
+	if (TxRingBitmap.field.Ac2DmaDone)
+		bReschedule |= RTMPFreeTXDUponTxDmaDone(pAd, QID_AC_VI);
+
+	if (TxRingBitmap.field.Ac1DmaDone)
+		bReschedule |= RTMPFreeTXDUponTxDmaDone(pAd, QID_AC_BK);
+
+	// Make sure to release Tx ring resource
+	RTMP_IRQ_UNLOCK(&pAd->irq_lock, IrqFlags);
+
+	// Dequeue outgoing frames from TxSwQueue[] and process it
+	RTMPDeQueuePacket(pAd, FALSE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+
+	return  bReschedule;
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Process MGMT ring DMA done interrupt, running in DPC level
+
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPHandleMgmtRingDmaDoneInterrupt(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	PTXD_STRUC	 pTxD;
+#ifdef RT_BIG_ENDIAN
+    PTXD_STRUC      pDestTxD;
+    TXD_STRUC       TxD;
+#endif
+	PNDIS_PACKET pPacket;
+	UCHAR	FREE = 0;
+	PRTMP_MGMT_RING pMgmtRing = &pAd->MgmtRing;
+
+	NdisAcquireSpinLock(&pAd->MgmtRingLock);
+
+	RTMP_IO_READ32(pAd, TX_MGMTDTX_IDX, &pMgmtRing->TxDmaIdx);
+	while (pMgmtRing->TxSwFreeIdx!= pMgmtRing->TxDmaIdx)
+	{
+		FREE++;
+#ifdef RT_BIG_ENDIAN
+        pDestTxD = (PTXD_STRUC) (pMgmtRing->Cell[pAd->MgmtRing.TxSwFreeIdx].AllocVa);
+        TxD = *pDestTxD;
+        pTxD = &TxD;
+		RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+#else
+		pTxD = (PTXD_STRUC) (pMgmtRing->Cell[pAd->MgmtRing.TxSwFreeIdx].AllocVa);
+#endif
+		pTxD->DMADONE = 0;
+		pPacket = pMgmtRing->Cell[pMgmtRing->TxSwFreeIdx].pNdisPacket;
+
+
+		if (pPacket)
+		{
+			PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr0, pTxD->SDLen0, PCI_DMA_TODEVICE);
+			RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_SUCCESS);
+		}
+		pMgmtRing->Cell[pMgmtRing->TxSwFreeIdx].pNdisPacket = NULL;
+
+		pPacket = pMgmtRing->Cell[pMgmtRing->TxSwFreeIdx].pNextNdisPacket;
+		if (pPacket)
+		{
+			PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr1, pTxD->SDLen1, PCI_DMA_TODEVICE);
+			RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_SUCCESS);
+		}
+		pMgmtRing->Cell[pMgmtRing->TxSwFreeIdx].pNextNdisPacket = NULL;
+		INC_RING_INDEX(pMgmtRing->TxSwFreeIdx, MGMT_RING_SIZE);
+
+#ifdef RT_BIG_ENDIAN
+        RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+        WriteBackToDescriptor((PUCHAR)pDestTxD, (PUCHAR)pTxD, TRUE, TYPE_TXD);
+#endif
+	}
+	NdisReleaseSpinLock(&pAd->MgmtRingLock);
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+	Arguments:
+		Adapter 	Pointer to our adapter. Dequeue all power safe delayed braodcast frames after beacon.
+
+	IRQL = DISPATCH_LEVEL
+
+	========================================================================
+*/
+VOID	RTMPHandleTBTTInterrupt(
+	IN PRTMP_ADAPTER pAd)
+{
+	{
+		if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+		{
+		}
+	}
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+	Arguments:
+		Adapter 	Pointer to our adapter. Rewrite beacon content before next send-out.
+
+	IRQL = DISPATCH_LEVEL
+
+	========================================================================
+*/
+VOID	RTMPHandlePreTBTTInterrupt(
+	IN PRTMP_ADAPTER pAd)
+{
+	{
+		if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("RTMPHandlePreTBTTInterrupt...\n"));
+		}
+	}
+
+
+}
+
+VOID	RTMPHandleRxCoherentInterrupt(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	WPDMA_GLO_CFG_STRUC	GloCfg;
+
+	if (pAd == NULL)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("====> pAd is NULL, return.\n"));
+		return;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("==> RTMPHandleRxCoherentInterrupt \n"));
+
+	RTMP_IO_READ32(pAd, WPDMA_GLO_CFG , &GloCfg.word);
+
+	GloCfg.field.EnTXWriteBackDDONE = 0;
+	GloCfg.field.EnableRxDMA = 0;
+	GloCfg.field.EnableTxDMA = 0;
+	RTMP_IO_WRITE32(pAd, WPDMA_GLO_CFG, GloCfg.word);
+
+	RTMPRingCleanUp(pAd, QID_AC_BE);
+	RTMPRingCleanUp(pAd, QID_AC_BK);
+	RTMPRingCleanUp(pAd, QID_AC_VI);
+	RTMPRingCleanUp(pAd, QID_AC_VO);
+	RTMPRingCleanUp(pAd, QID_HCCA);
+	RTMPRingCleanUp(pAd, QID_MGMT);
+	RTMPRingCleanUp(pAd, QID_RX);
+
+	RTMPEnableRxTx(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<== RTMPHandleRxCoherentInterrupt \n"));
+}
+
+
+VOID DBGPRINT_TX_RING(
+	IN PRTMP_ADAPTER  pAd,
+	IN UCHAR          QueIdx)
+{
+	UINT32		Ac0Base;
+	UINT32		Ac0HwIdx = 0, Ac0SwIdx = 0, AC0freeIdx;
+	int			i;
+	PULONG	ptemp;
+
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("=====================================================\n "  ));
+	switch (QueIdx)
+	{
+		case QID_AC_BE:
+			RTMP_IO_READ32(pAd, TX_BASE_PTR0, &Ac0Base);
+			RTMP_IO_READ32(pAd, TX_CTX_IDX0, &Ac0SwIdx);
+			RTMP_IO_READ32(pAd, TX_DTX_IDX0, &Ac0HwIdx);
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("All QID_AC_BE DESCRIPTOR  \n "  ));
+			for (i=0;i<TX_RING_SIZE;i++)
+			{
+				ptemp= (PULONG)pAd->TxRing[QID_AC_BE].Cell[i].AllocVa;
+				DBGPRINT_RAW(RT_DEBUG_TRACE, ("[%02d]  %08lx: %08lx: %08lx: %08lx\n " , i, *ptemp,*(ptemp+1),*(ptemp+2),*(ptemp+3)));
+			}
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("  \n "  ));
+			break;
+		case QID_AC_BK:
+			RTMP_IO_READ32(pAd, TX_BASE_PTR1, &Ac0Base);
+			RTMP_IO_READ32(pAd, TX_CTX_IDX1, &Ac0SwIdx);
+			RTMP_IO_READ32(pAd, TX_DTX_IDX1, &Ac0HwIdx);
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("All QID_AC_BK DESCRIPTOR  \n "  ));
+			for (i=0;i<TX_RING_SIZE;i++)
+			{
+				ptemp= (PULONG)pAd->TxRing[QID_AC_BK].Cell[i].AllocVa;
+				DBGPRINT_RAW(RT_DEBUG_TRACE, ("[%02d]  %08lx: %08lx: %08lx: %08lx\n " , i, *ptemp,*(ptemp+1),*(ptemp+2),*(ptemp+3)));
+			}
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("  \n "  ));
+			break;
+		case QID_AC_VI:
+			RTMP_IO_READ32(pAd, TX_BASE_PTR2, &Ac0Base);
+			RTMP_IO_READ32(pAd, TX_CTX_IDX2, &Ac0SwIdx);
+			RTMP_IO_READ32(pAd, TX_DTX_IDX2, &Ac0HwIdx);
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("All QID_AC_VI DESCRIPTOR \n "  ));
+			for (i=0;i<TX_RING_SIZE;i++)
+			{
+				ptemp= (PULONG)pAd->TxRing[QID_AC_VI].Cell[i].AllocVa;
+				DBGPRINT_RAW(RT_DEBUG_TRACE, ("[%02d]  %08lx: %08lx: %08lx: %08lx\n " , i, *ptemp,*(ptemp+1),*(ptemp+2),*(ptemp+3)));
+			}
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("  \n "  ));
+			break;
+		case QID_AC_VO:
+			RTMP_IO_READ32(pAd, TX_BASE_PTR3, &Ac0Base);
+			RTMP_IO_READ32(pAd, TX_CTX_IDX3, &Ac0SwIdx);
+			RTMP_IO_READ32(pAd, TX_DTX_IDX3, &Ac0HwIdx);
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("All QID_AC_VO DESCRIPTOR \n "  ));
+			for (i=0;i<TX_RING_SIZE;i++)
+			{
+				ptemp= (PULONG)pAd->TxRing[QID_AC_VO].Cell[i].AllocVa;
+				DBGPRINT_RAW(RT_DEBUG_TRACE, ("[%02d]  %08lx: %08lx: %08lx: %08lx\n " , i, *ptemp,*(ptemp+1),*(ptemp+2),*(ptemp+3)));
+			}
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("  \n "  ));
+			break;
+		case QID_MGMT:
+			RTMP_IO_READ32(pAd, TX_BASE_PTR5, &Ac0Base);
+			RTMP_IO_READ32(pAd, TX_CTX_IDX5, &Ac0SwIdx);
+			RTMP_IO_READ32(pAd, TX_DTX_IDX5, &Ac0HwIdx);
+			DBGPRINT_RAW(RT_DEBUG_TRACE, (" All QID_MGMT  DESCRIPTOR \n "  ));
+			for (i=0;i<MGMT_RING_SIZE;i++)
+			{
+				ptemp= (PULONG)pAd->MgmtRing.Cell[i].AllocVa;
+				DBGPRINT_RAW(RT_DEBUG_TRACE, ("[%02d]  %08lx: %08lx: %08lx: %08lx\n " , i, *ptemp,*(ptemp+1),*(ptemp+2),*(ptemp+3)));
+			}
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("  \n "  ));
+			break;
+
+		default:
+			DBGPRINT_ERR(("DBGPRINT_TX_RING(Ring %d) not supported\n", QueIdx));
+			break;
+	}
+	AC0freeIdx = pAd->TxRing[QueIdx].TxSwFreeIdx;
+
+	DBGPRINT(RT_DEBUG_TRACE,("TxRing%d, TX_DTX_IDX=%d, TX_CTX_IDX=%d\n", QueIdx, Ac0HwIdx, Ac0SwIdx));
+	DBGPRINT_RAW(RT_DEBUG_TRACE,(" 	TxSwFreeIdx[%d]", AC0freeIdx));
+	DBGPRINT_RAW(RT_DEBUG_TRACE,("	pending-NDIS=%ld\n", pAd->RalinkCounters.PendingNdisPacketCount));
+
+
+}
+
+
+VOID DBGPRINT_RX_RING(
+	IN PRTMP_ADAPTER  pAd)
+{
+	UINT32		Ac0Base;
+	UINT32		Ac0HwIdx = 0, Ac0SwIdx = 0, AC0freeIdx;
+	int			i;
+	UINT32	*ptemp;
+
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("=====================================================\n "  ));
+	RTMP_IO_READ32(pAd, RX_BASE_PTR, &Ac0Base);
+	RTMP_IO_READ32(pAd, RX_CRX_IDX, &Ac0SwIdx);
+	RTMP_IO_READ32(pAd, RX_DRX_IDX, &Ac0HwIdx);
+	AC0freeIdx = pAd->RxRing.RxSwReadIdx;
+
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("All RX DSP  \n "  ));
+	for (i=0;i<RX_RING_SIZE;i++)
+	{
+		ptemp = (UINT32 *)pAd->RxRing.Cell[i].AllocVa;
+		DBGPRINT_RAW(RT_DEBUG_TRACE, ("[%02d]  %08x: %08x: %08x: %08x\n " , i, *ptemp,*(ptemp+1),*(ptemp+2),*(ptemp+3)));
+	}
+	DBGPRINT(RT_DEBUG_TRACE,("RxRing, RX_DRX_IDX=%d, RX_CRX_IDX=%d \n", Ac0HwIdx, Ac0SwIdx));
+	DBGPRINT_RAW(RT_DEBUG_TRACE,(" 	RxSwReadIdx [%d]=", AC0freeIdx));
+	DBGPRINT_RAW(RT_DEBUG_TRACE,("	pending-NDIS=%ld\n", pAd->RalinkCounters.PendingNdisPacketCount));
+}
+#endif // RT2860 //
+
+/*
+	========================================================================
+
+	Routine Description:
+		Suspend MSDU transmission
+
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPSuspendMsduTransmission(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	DBGPRINT(RT_DEBUG_TRACE,("SCANNING, suspend MSDU transmission ...\n"));
+
+
+	//
+	// Before BSS_SCAN_IN_PROGRESS, we need to keep Current R66 value and
+	// use Lowbound as R66 value on ScanNextChannel(...)
+	//
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R66, &pAd->BbpTuning.R66CurrentValue);
+
+	// set BBP_R66 to 0x30/0x40 when scanning (AsicSwitchChannel will set R66 according to channel when scanning)
+	RTMPSetAGCInitValue(pAd, BW_20);
+
+	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS);
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Resume MSDU transmission
+
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID RTMPResumeMsduTransmission(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	DBGPRINT(RT_DEBUG_TRACE,("SCAN done, resume MSDU transmission ...\n"));
+
+
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, pAd->BbpTuning.R66CurrentValue);
+
+	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS);
+	RTMPDeQueuePacket(pAd, FALSE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+}
+
+
+UINT deaggregate_AMSDU_announce(
+	IN	PRTMP_ADAPTER	pAd,
+	PNDIS_PACKET		pPacket,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize)
+{
+	USHORT 			PayloadSize;
+	USHORT 			SubFrameSize;
+	PHEADER_802_3 	pAMSDUsubheader;
+	UINT			nMSDU;
+    UCHAR			Header802_3[14];
+
+	PUCHAR			pPayload, pDA, pSA, pRemovedLLCSNAP;
+	PNDIS_PACKET	pClonePacket;
+
+
+
+	nMSDU = 0;
+
+	while (DataSize > LENGTH_802_3)
+	{
+
+		nMSDU++;
+
+		pAMSDUsubheader = (PHEADER_802_3)pData;
+		PayloadSize = pAMSDUsubheader->Octet[1] + (pAMSDUsubheader->Octet[0]<<8);
+		SubFrameSize = PayloadSize + LENGTH_802_3;
+
+
+		if ((DataSize < SubFrameSize) || (PayloadSize > 1518 ))
+		{
+			break;
+		}
+
+		pPayload = pData + LENGTH_802_3;
+		pDA = pData;
+		pSA = pData + MAC_ADDR_LEN;
+
+		// convert to 802.3 header
+        CONVERT_TO_802_3(Header802_3, pDA, pSA, pPayload, PayloadSize, pRemovedLLCSNAP);
+
+#ifdef CONFIG_STA_SUPPORT
+		if ((Header802_3[12] == 0x88) && (Header802_3[13] == 0x8E) )
+		{
+		    // avoid local heap overflow, use dyanamic allocation
+		   MLME_QUEUE_ELEM *Elem = (MLME_QUEUE_ELEM *) kmalloc(sizeof(MLME_QUEUE_ELEM), MEM_ALLOC_FLAG);
+		   memmove(Elem->Msg+(LENGTH_802_11 + LENGTH_802_1_H), pPayload, PayloadSize);
+		   Elem->MsgLen = LENGTH_802_11 + LENGTH_802_1_H + PayloadSize;
+		   WpaEAPOLKeyAction(pAd, Elem);
+		   kfree(Elem);
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+	        	if (pRemovedLLCSNAP)
+	        	{
+	    			pPayload -= LENGTH_802_3;
+	    			PayloadSize += LENGTH_802_3;
+	    			NdisMoveMemory(pPayload, &Header802_3[0], LENGTH_802_3);
+	        	}
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+		pClonePacket = ClonePacket(pAd, pPacket, pPayload, PayloadSize);
+		if (pClonePacket)
+		{
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				ANNOUNCE_OR_FORWARD_802_3_PACKET(pAd, pClonePacket, RTMP_GET_PACKET_IF(pPacket));
+#endif // CONFIG_STA_SUPPORT //
+		}
+
+
+		// A-MSDU has padding to multiple of 4 including subframe header.
+		// align SubFrameSize up to multiple of 4
+		SubFrameSize = (SubFrameSize+3)&(~0x3);
+
+
+		if (SubFrameSize > 1528 || SubFrameSize < 32)
+		{
+			break;
+		}
+
+		if (DataSize > SubFrameSize)
+		{
+			pData += SubFrameSize;
+			DataSize -= SubFrameSize;
+		}
+		else
+		{
+			// end of A-MSDU
+			DataSize = 0;
+		}
+	}
+
+	// finally release original rx packet
+	RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_SUCCESS);
+
+	return nMSDU;
+}
+
+
+UINT BA_Reorder_AMSDU_Annnounce(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket)
+{
+	PUCHAR			pData;
+	USHORT			DataSize;
+	UINT			nMSDU = 0;
+
+	pData = (PUCHAR) GET_OS_PKT_DATAPTR(pPacket);
+	DataSize = (USHORT) GET_OS_PKT_LEN(pPacket);
+
+	nMSDU = deaggregate_AMSDU_announce(pAd, pPacket, pData, DataSize);
+
+	return nMSDU;
+}
+
+
+/*
+	==========================================================================
+	Description:
+		Look up the MAC address in the MAC table. Return NULL if not found.
+	Return:
+		pEntry - pointer to the MAC entry; NULL is not found
+	==========================================================================
+*/
+MAC_TABLE_ENTRY *MacTableLookup(
+	IN PRTMP_ADAPTER pAd,
+	PUCHAR pAddr)
+{
+	ULONG HashIdx;
+	MAC_TABLE_ENTRY *pEntry = NULL;
+
+	HashIdx = MAC_ADDR_HASH_INDEX(pAddr);
+	pEntry = pAd->MacTab.Hash[HashIdx];
+
+	while (pEntry && (pEntry->ValidAsCLI || pEntry->ValidAsWDS || pEntry->ValidAsApCli || pEntry->ValidAsMesh))
+	{
+		if (MAC_ADDR_EQUAL(pEntry->Addr, pAddr))
+		{
+			break;
+		}
+		else
+			pEntry = pEntry->pNext;
+	}
+
+	return pEntry;
+}
+
+MAC_TABLE_ENTRY *MacTableInsertEntry(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR			pAddr,
+	IN	UCHAR			apidx,
+	IN BOOLEAN	CleanAll)
+{
+	UCHAR HashIdx;
+	int i, FirstWcid;
+	MAC_TABLE_ENTRY *pEntry = NULL, *pCurrEntry;
+
+	// if FULL, return
+	if (pAd->MacTab.Size >= MAX_LEN_OF_MAC_TABLE)
+		return NULL;
+
+	FirstWcid = 1;
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	if (pAd->StaCfg.BssType == BSS_INFRA)
+		FirstWcid = 2;
+#endif // CONFIG_STA_SUPPORT //
+
+	// allocate one MAC entry
+	NdisAcquireSpinLock(&pAd->MacTabLock);
+	for (i = FirstWcid; i< MAX_LEN_OF_MAC_TABLE; i++)   // skip entry#0 so that "entry index == AID" for fast lookup
+	{
+		// pick up the first available vacancy
+		if ((pAd->MacTab.Content[i].ValidAsCLI == FALSE) &&
+			(pAd->MacTab.Content[i].ValidAsWDS == FALSE) &&
+			(pAd->MacTab.Content[i].ValidAsApCli== FALSE) &&
+			(pAd->MacTab.Content[i].ValidAsMesh == FALSE)
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+			&& (pAd->MacTab.Content[i].ValidAsDls == FALSE)
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+			)
+		{
+			pEntry = &pAd->MacTab.Content[i];
+			if (CleanAll == TRUE)
+			{
+				pEntry->MaxSupportedRate = RATE_11;
+				pEntry->CurrTxRate = RATE_11;
+				NdisZeroMemory(pEntry, sizeof(MAC_TABLE_ENTRY));
+				pEntry->PairwiseKey.KeyLen = 0;
+				pEntry->PairwiseKey.CipherAlg = CIPHER_NONE;
+			}
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+			if (apidx >= MIN_NET_DEVICE_FOR_DLS)
+			{
+				pEntry->ValidAsCLI = FALSE;
+				pEntry->ValidAsWDS = FALSE;
+				pEntry->ValidAsApCli = FALSE;
+				pEntry->ValidAsMesh = FALSE;
+				pEntry->ValidAsDls = TRUE;
+				pEntry->isCached = FALSE;
+			}
+			else
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+			{
+
+#ifdef CONFIG_STA_SUPPORT
+				IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				{
+					pEntry->ValidAsCLI = TRUE;
+					pEntry->ValidAsWDS = FALSE;
+					pEntry->ValidAsApCli = FALSE;
+					pEntry->ValidAsMesh = FALSE;
+					pEntry->ValidAsDls = FALSE;
+				}
+#endif // CONFIG_STA_SUPPORT //
+			}
+
+			pEntry->bIAmBadAtheros = FALSE;
+			pEntry->pAd = pAd;
+			pEntry->CMTimerRunning = FALSE;
+			pEntry->EnqueueEapolStartTimerRunning = EAPOL_START_DISABLE;
+			pEntry->RSNIE_Len = 0;
+			NdisZeroMemory(pEntry->R_Counter, sizeof(pEntry->R_Counter));
+			pEntry->ReTryCounter = PEER_MSG1_RETRY_TIMER_CTR;
+
+			if (pEntry->ValidAsMesh)
+				pEntry->apidx = (apidx - MIN_NET_DEVICE_FOR_MESH);
+			else if (pEntry->ValidAsApCli)
+				pEntry->apidx = (apidx - MIN_NET_DEVICE_FOR_APCLI);
+			else if (pEntry->ValidAsWDS)
+				pEntry->apidx = (apidx - MIN_NET_DEVICE_FOR_WDS);
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+			else if (pEntry->ValidAsDls)
+				pEntry->apidx = (apidx - MIN_NET_DEVICE_FOR_DLS);
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+			else
+				pEntry->apidx = apidx;
+
+			{
+
+#ifdef CONFIG_STA_SUPPORT
+				IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				{
+					pEntry->AuthMode = pAd->StaCfg.AuthMode;
+					pEntry->WepStatus = pAd->StaCfg.WepStatus;
+					pEntry->PrivacyFilter = Ndis802_11PrivFilterAcceptAll;
+#ifdef RT2860
+					AsicRemovePairwiseKeyEntry(pAd, pEntry->apidx, (UCHAR)i);
+#endif // RT2860 //
+				}
+#endif // CONFIG_STA_SUPPORT //
+			}
+
+			pEntry->GTKState = REKEY_NEGOTIATING;
+			pEntry->PairwiseKey.KeyLen = 0;
+			pEntry->PairwiseKey.CipherAlg = CIPHER_NONE;
+#ifdef CONFIG_STA_SUPPORT
+			if ((pAd->OpMode == OPMODE_STA) &&
+				(pAd->StaCfg.BssType == BSS_ADHOC))
+				pEntry->PortSecured = WPA_802_1X_PORT_SECURED;
+			else
+#ifdef QOS_DLS_SUPPORT
+			if (pEntry->ValidAsDls == TRUE)
+				pEntry->PortSecured = WPA_802_1X_PORT_SECURED;
+			else
+#endif //QOS_DLS_SUPPORT
+#endif // CONFIG_STA_SUPPORT //
+			pEntry->PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+			pEntry->PMKID_CacheIdx = ENTRY_NOT_FOUND;
+			COPY_MAC_ADDR(pEntry->Addr, pAddr);
+			pEntry->Sst = SST_NOT_AUTH;
+			pEntry->AuthState = AS_NOT_AUTH;
+			pEntry->Aid = (USHORT)i;  //0;
+			pEntry->CapabilityInfo = 0;
+			pEntry->PsMode = PWR_ACTIVE;
+			pEntry->PsQIdleCount = 0;
+			pEntry->NoDataIdleCount = 0;
+			pEntry->ContinueTxFailCnt = 0;
+			InitializeQueueHeader(&pEntry->PsQueue);
+
+
+			pAd->MacTab.Size ++;
+			// Add this entry into ASIC RX WCID search table
+			RT28XX_STA_ENTRY_ADD(pAd, pEntry);
+
+
+
+			DBGPRINT(RT_DEBUG_TRACE, ("MacTableInsertEntry - allocate entry #%d, Total= %d\n",i, pAd->MacTab.Size));
+			break;
+		}
+	}
+
+	// add this MAC entry into HASH table
+	if (pEntry)
+	{
+		HashIdx = MAC_ADDR_HASH_INDEX(pAddr);
+		if (pAd->MacTab.Hash[HashIdx] == NULL)
+		{
+			pAd->MacTab.Hash[HashIdx] = pEntry;
+		}
+		else
+		{
+			pCurrEntry = pAd->MacTab.Hash[HashIdx];
+			while (pCurrEntry->pNext != NULL)
+				pCurrEntry = pCurrEntry->pNext;
+			pCurrEntry->pNext = pEntry;
+		}
+	}
+
+	NdisReleaseSpinLock(&pAd->MacTabLock);
+	return pEntry;
+}
+
+/*
+	==========================================================================
+	Description:
+		Delete a specified client from MAC table
+	==========================================================================
+ */
+BOOLEAN MacTableDeleteEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT wcid,
+	IN PUCHAR pAddr)
+{
+	USHORT HashIdx;
+	MAC_TABLE_ENTRY *pEntry, *pPrevEntry, *pProbeEntry;
+	BOOLEAN Cancelled;
+
+	if (wcid >= MAX_LEN_OF_MAC_TABLE)
+		return FALSE;
+
+	NdisAcquireSpinLock(&pAd->MacTabLock);
+
+	HashIdx = MAC_ADDR_HASH_INDEX(pAddr);
+	pEntry = &pAd->MacTab.Content[wcid];
+
+	if (pEntry && (pEntry->ValidAsCLI || pEntry->ValidAsApCli || pEntry->ValidAsWDS || pEntry->ValidAsMesh
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+ 		|| pEntry->ValidAsDls
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+		))
+	{
+		if (MAC_ADDR_EQUAL(pEntry->Addr, pAddr))
+		{
+
+			// Delete this entry from ASIC on-chip WCID Table
+			RT28XX_STA_ENTRY_MAC_RESET(pAd, wcid);
+
+#ifdef DOT11_N_SUPPORT
+			// free resources of BA
+			BASessionTearDownALL(pAd, pEntry->Aid);
+#endif // DOT11_N_SUPPORT //
+
+
+			pPrevEntry = NULL;
+			pProbeEntry = pAd->MacTab.Hash[HashIdx];
+			ASSERT(pProbeEntry);
+
+			// update Hash list
+			do
+			{
+				if (pProbeEntry == pEntry)
+				{
+					if (pPrevEntry == NULL)
+					{
+						pAd->MacTab.Hash[HashIdx] = pEntry->pNext;
+					}
+					else
+					{
+						pPrevEntry->pNext = pEntry->pNext;
+					}
+					break;
+				}
+
+				pPrevEntry = pProbeEntry;
+				pProbeEntry = pProbeEntry->pNext;
+			} while (pProbeEntry);
+
+			// not found !!!
+			ASSERT(pProbeEntry != NULL);
+
+			RT28XX_STA_ENTRY_KEY_DEL(pAd, BSS0, wcid);
+
+
+		if (pEntry->EnqueueEapolStartTimerRunning != EAPOL_START_DISABLE)
+		{
+			RTMPCancelTimer(&pEntry->EnqueueStartForPSKTimer, &Cancelled);
+			pEntry->EnqueueEapolStartTimerRunning = EAPOL_START_DISABLE;
+		}
+
+
+   			NdisZeroMemory(pEntry, sizeof(MAC_TABLE_ENTRY));
+			pAd->MacTab.Size --;
+			DBGPRINT(RT_DEBUG_TRACE, ("MacTableDeleteEntry1 - Total= %d\n", pAd->MacTab.Size));
+		}
+		else
+		{
+			printk("\n%s: Impossible Wcid = %d !!!!!\n", __FUNCTION__, wcid);
+		}
+	}
+
+	NdisReleaseSpinLock(&pAd->MacTabLock);
+
+	//Reset operating mode when no Sta.
+	if (pAd->MacTab.Size == 0)
+	{
+#ifdef DOT11_N_SUPPORT
+		pAd->CommonCfg.AddHTInfo.AddHtInfo2.OperaionMode = 0;
+#endif // DOT11_N_SUPPORT //
+		AsicUpdateProtect(pAd, 0 /*pAd->CommonCfg.AddHTInfo.AddHtInfo2.OperaionMode*/, (ALLN_SETPROTECT), TRUE, 0 /*pAd->MacTab.fAnyStationNonGF*/);
+	}
+
+	return TRUE;
+}
+
+
+/*
+	==========================================================================
+	Description:
+		This routine reset the entire MAC table. All packets pending in
+		the power-saving queues are freed here.
+	==========================================================================
+ */
+VOID MacTableReset(
+	IN  PRTMP_ADAPTER  pAd)
+{
+	int         i;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("MacTableReset\n"));
+	//NdisAcquireSpinLock(&pAd->MacTabLock);
+
+	for (i=1; i<MAX_LEN_OF_MAC_TABLE; i++)
+	{
+#ifdef RT2860
+		RT28XX_STA_ENTRY_MAC_RESET(pAd, i);
+#endif // RT2860 //
+		if (pAd->MacTab.Content[i].ValidAsCLI == TRUE)
+	   {
+
+#ifdef DOT11_N_SUPPORT
+			// free resources of BA
+			BASessionTearDownALL(pAd, i);
+#endif // DOT11_N_SUPPORT //
+
+			pAd->MacTab.Content[i].ValidAsCLI = FALSE;
+
+
+
+
+			//AsicDelWcidTab(pAd, i);
+		}
+	}
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID AssocParmFill(
+	IN PRTMP_ADAPTER pAd,
+	IN OUT MLME_ASSOC_REQ_STRUCT *AssocReq,
+	IN PUCHAR                     pAddr,
+	IN USHORT                     CapabilityInfo,
+	IN ULONG                      Timeout,
+	IN USHORT                     ListenIntv)
+{
+	COPY_MAC_ADDR(AssocReq->Addr, pAddr);
+	// Add mask to support 802.11b mode only
+	AssocReq->CapabilityInfo = CapabilityInfo & SUPPORTED_CAPABILITY_INFO; // not cf-pollable, not cf-poll-request
+	AssocReq->Timeout = Timeout;
+	AssocReq->ListenIntv = ListenIntv;
+}
+
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID DisassocParmFill(
+	IN PRTMP_ADAPTER pAd,
+	IN OUT MLME_DISASSOC_REQ_STRUCT *DisassocReq,
+	IN PUCHAR pAddr,
+	IN USHORT Reason)
+{
+	COPY_MAC_ADDR(DisassocReq->Addr, pAddr);
+	DisassocReq->Reason = Reason;
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Check the out going frame, if this is an DHCP or ARP datagram
+	will be duplicate another frame at low data rate transmit.
+
+	Arguments:
+		pAd 		Pointer to our adapter
+		pPacket 	Pointer to outgoing Ndis frame
+
+	Return Value:
+		TRUE		To be duplicate at Low data rate transmit. (1mb)
+		FALSE		Do nothing.
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+		MAC header + IP Header + UDP Header
+		  14 Bytes	  20 Bytes
+
+		UDP Header
+		00|01|02|03|04|05|06|07|08|09|10|11|12|13|14|15|
+						Source Port
+		16|17|18|19|20|21|22|23|24|25|26|27|28|29|30|31|
+					Destination Port
+
+		port 0x43 means Bootstrap Protocol, server.
+		Port 0x44 means Bootstrap Protocol, client.
+
+	========================================================================
+*/
+
+BOOLEAN RTMPCheckDHCPFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket)
+{
+	PACKET_INFO 	PacketInfo;
+	ULONG			NumberOfBytesRead = 0;
+	ULONG			CurrentOffset = 0;
+	PVOID			pVirtualAddress = NULL;
+	UINT			NdisBufferLength;
+	PUCHAR			pSrc;
+	USHORT			Protocol;
+	UCHAR			ByteOffset36 = 0;
+	UCHAR			ByteOffset38 = 0;
+	BOOLEAN 		ReadFirstParm = TRUE;
+
+	RTMP_QueryPacketInfo(pPacket, &PacketInfo, (PUCHAR *)&pVirtualAddress, &NdisBufferLength);
+
+	NumberOfBytesRead += NdisBufferLength;
+	pSrc = (PUCHAR) pVirtualAddress;
+	Protocol = *(pSrc + 12) * 256 + *(pSrc + 13);
+
+	//
+	// Check DHCP & BOOTP protocol
+	//
+	while (NumberOfBytesRead <= PacketInfo.TotalPacketLength)
+	{
+		if ((NumberOfBytesRead >= 35) && (ReadFirstParm == TRUE))
+		{
+			CurrentOffset = 35 - (NumberOfBytesRead - NdisBufferLength);
+			ByteOffset36 = *(pSrc + CurrentOffset);
+			ReadFirstParm = FALSE;
+		}
+
+		if (NumberOfBytesRead >= 37)
+		{
+			CurrentOffset = 37 - (NumberOfBytesRead - NdisBufferLength);
+			ByteOffset38 = *(pSrc + CurrentOffset);
+			//End of Read
+			break;
+		}
+		return FALSE;
+	}
+
+	// Check for DHCP & BOOTP protocol
+	if ((ByteOffset36 != 0x44) || (ByteOffset38 != 0x43))
+		{
+		//
+		// 2054 (hex 0806) for ARP datagrams
+		// if this packet is not ARP datagrams, then do nothing
+		// ARP datagrams will also be duplicate at 1mb broadcast frames
+		//
+		if (Protocol != 0x0806 )
+			return FALSE;
+		}
+
+	return TRUE;
+}
+
+
+BOOLEAN RTMPCheckEtherType(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket)
+{
+	USHORT	TypeLen;
+	UCHAR	Byte0, Byte1;
+	PUCHAR	pSrcBuf;
+	UINT32	pktLen;
+	UINT16 	srcPort, dstPort;
+	BOOLEAN	status = TRUE;
+
+
+	pSrcBuf = GET_OS_PKT_DATAPTR(pPacket);
+	pktLen = GET_OS_PKT_LEN(pPacket);
+
+	ASSERT(pSrcBuf);
+
+	RTMP_SET_PACKET_SPECIFIC(pPacket, 0);
+
+	// get Ethernet protocol field
+	TypeLen = (pSrcBuf[12] << 8) + pSrcBuf[13];
+
+	pSrcBuf += LENGTH_802_3;	// Skip the Ethernet Header.
+
+	if (TypeLen <= 1500)
+	{	// 802.3, 802.3 LLC
+		/*
+			DestMAC(6) + SrcMAC(6) + Lenght(2) +
+			DSAP(1) + SSAP(1) + Control(1) +
+			if the DSAP = 0xAA, SSAP=0xAA, Contorl = 0x03, it has a 5-bytes SNAP header.
+				=> + SNAP (5, OriginationID(3) + etherType(2))
+		*/
+		if (pSrcBuf[0] == 0xAA && pSrcBuf[1] == 0xAA && pSrcBuf[2] == 0x03)
+		{
+			Sniff2BytesFromNdisBuffer(pSrcBuf, 6, &Byte0, &Byte1);
+			RTMP_SET_PACKET_LLCSNAP(pPacket, 1);
+			TypeLen = (USHORT)((Byte0 << 8) + Byte1);
+			pSrcBuf += 8; // Skip this LLC/SNAP header
+		}
+		else
+		{
+			//It just has 3-byte LLC header, maybe a legacy ether type frame. we didn't handle it.
+		}
+	}
+
+	// If it's a VLAN packet, get the real Type/Length field.
+	if (TypeLen == 0x8100)
+	{
+		/* 0x8100 means VLAN packets */
+
+		/* Dest. MAC Address (6-bytes) +
+		   Source MAC Address (6-bytes) +
+		   Length/Type = 802.1Q Tag Type (2-byte) +
+		   Tag Control Information (2-bytes) +
+		   Length / Type (2-bytes) +
+		   data payload (0-n bytes) +
+		   Pad (0-p bytes) +
+		   Frame Check Sequence (4-bytes) */
+
+		RTMP_SET_PACKET_VLAN(pPacket, 1);
+		Sniff2BytesFromNdisBuffer(pSrcBuf, 2, &Byte0, &Byte1);
+		TypeLen = (USHORT)((Byte0 << 8) + Byte1);
+
+		pSrcBuf += 4; // Skip the VLAN Header.
+	}
+
+	switch (TypeLen)
+	{
+		case 0x0800:
+			{
+				ASSERT((pktLen > 34));
+				if (*(pSrcBuf + 9) == 0x11)
+				{	// udp packet
+					ASSERT((pktLen > 34));	// 14 for ethernet header, 20 for IP header
+
+					pSrcBuf += 20;	// Skip the IP header
+					srcPort = OS_NTOHS(*((UINT16 *)pSrcBuf));
+					dstPort = OS_NTOHS(*((UINT16 *)(pSrcBuf +2)));
+
+					if ((srcPort==0x44 && dstPort==0x43) || (srcPort==0x43 && dstPort==0x44))
+					{	//It's a BOOTP/DHCP packet
+						RTMP_SET_PACKET_DHCP(pPacket, 1);
+					}
+				}
+			}
+			break;
+		case 0x0806:
+			{
+				//ARP Packet.
+				RTMP_SET_PACKET_DHCP(pPacket, 1);
+			}
+			break;
+		case 0x888e:
+			{
+				// EAPOL Packet.
+				RTMP_SET_PACKET_EAPOL(pPacket, 1);
+			}
+			break;
+		default:
+			status = FALSE;
+			break;
+	}
+
+	return status;
+
+}
+
+
+
+VOID Update_Rssi_Sample(
+	IN PRTMP_ADAPTER	pAd,
+	IN RSSI_SAMPLE		*pRssi,
+	IN PRXWI_STRUC		pRxWI)
+		{
+	CHAR	rssi0 = pRxWI->RSSI0;
+	CHAR	rssi1 = pRxWI->RSSI1;
+	CHAR	rssi2 = pRxWI->RSSI2;
+
+	if (rssi0 != 0)
+	{
+		pRssi->LastRssi0	= ConvertToRssi(pAd, (CHAR)rssi0, RSSI_0);
+		pRssi->AvgRssi0X8	= (pRssi->AvgRssi0X8 - pRssi->AvgRssi0) + pRssi->LastRssi0;
+		pRssi->AvgRssi0	= pRssi->AvgRssi0X8 >> 3;
+	}
+
+	if (rssi1 != 0)
+	{
+		pRssi->LastRssi1	= ConvertToRssi(pAd, (CHAR)rssi1, RSSI_1);
+		pRssi->AvgRssi1X8	= (pRssi->AvgRssi1X8 - pRssi->AvgRssi1) + pRssi->LastRssi1;
+		pRssi->AvgRssi1	= pRssi->AvgRssi1X8 >> 3;
+	}
+
+	if (rssi2 != 0)
+	{
+		pRssi->LastRssi2	= ConvertToRssi(pAd, (CHAR)rssi2, RSSI_2);
+		pRssi->AvgRssi2X8  = (pRssi->AvgRssi2X8 - pRssi->AvgRssi2) + pRssi->LastRssi2;
+		pRssi->AvgRssi2 = pRssi->AvgRssi2X8 >> 3;
+	}
+}
+
+
+
+// Normal legacy Rx packet indication
+VOID Indicate_Legacy_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+	PNDIS_PACKET	pRxPacket = pRxBlk->pRxPacket;
+	UCHAR			Header802_3[LENGTH_802_3];
+
+	// 1. get 802.3 Header
+	// 2. remove LLC
+	// 		a. pointer pRxBlk->pData to payload
+	//      b. modify pRxBlk->DataSize
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		RTMP_802_11_REMOVE_LLC_AND_CONVERT_TO_802_3(pRxBlk, Header802_3);
+#endif // CONFIG_STA_SUPPORT //
+
+	if (pRxBlk->DataSize > MAX_RX_PKT_LEN)
+	{
+
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+
+
+	STATS_INC_RX_PACKETS(pAd, FromWhichBSSID);
+
+
+	wlan_802_11_to_802_3_packet(pAd, pRxBlk, Header802_3, FromWhichBSSID);
+
+	//
+	// pass this 802.3 packet to upper layer or forward this packet to WM directly
+	//
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		ANNOUNCE_OR_FORWARD_802_3_PACKET(pAd, pRxPacket, FromWhichBSSID);
+#endif // CONFIG_STA_SUPPORT //
+
+}
+
+
+// Normal, AMPDU or AMSDU
+VOID CmmRxnonRalinkFrameIndicate(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+#ifdef DOT11_N_SUPPORT
+	if (RX_BLK_TEST_FLAG(pRxBlk, fRX_AMPDU) && (pAd->CommonCfg.bDisableReordering == 0))
+	{
+		Indicate_AMPDU_Packet(pAd, pRxBlk, FromWhichBSSID);
+	}
+	else
+#endif // DOT11_N_SUPPORT //
+	{
+#ifdef DOT11_N_SUPPORT
+		if (RX_BLK_TEST_FLAG(pRxBlk, fRX_AMSDU))
+		{
+			// handle A-MSDU
+			Indicate_AMSDU_Packet(pAd, pRxBlk, FromWhichBSSID);
+		}
+		else
+#endif // DOT11_N_SUPPORT //
+		{
+			Indicate_Legacy_Packet(pAd, pRxBlk, FromWhichBSSID);
+		}
+	}
+}
+
+
+VOID CmmRxRalinkFrameIndicate(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+	UCHAR			Header802_3[LENGTH_802_3];
+	UINT16			Msdu2Size;
+	UINT16 			Payload1Size, Payload2Size;
+	PUCHAR 			pData2;
+	PNDIS_PACKET	pPacket2 = NULL;
+
+
+
+	Msdu2Size = *(pRxBlk->pData) + (*(pRxBlk->pData+1) << 8);
+
+	if ((Msdu2Size <= 1536) && (Msdu2Size < pRxBlk->DataSize))
+	{
+		/* skip two byte MSDU2 len */
+		pRxBlk->pData += 2;
+		pRxBlk->DataSize -= 2;
+	}
+	else
+	{
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+
+	// get 802.3 Header and  remove LLC
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		RTMP_802_11_REMOVE_LLC_AND_CONVERT_TO_802_3(pRxBlk, Header802_3);
+#endif // CONFIG_STA_SUPPORT //
+
+
+	ASSERT(pRxBlk->pRxPacket);
+
+	// Ralink Aggregation frame
+	pAd->RalinkCounters.OneSecRxAggregationCount ++;
+	Payload1Size = pRxBlk->DataSize - Msdu2Size;
+	Payload2Size = Msdu2Size - LENGTH_802_3;
+
+	pData2 = pRxBlk->pData + Payload1Size + LENGTH_802_3;
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		pPacket2 = duplicate_pkt(pAd, (pData2-LENGTH_802_3), LENGTH_802_3, pData2, Payload2Size, FromWhichBSSID);
+#endif // CONFIG_STA_SUPPORT //
+
+	if (!pPacket2)
+	{
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+
+	// update payload size of 1st packet
+	pRxBlk->DataSize = Payload1Size;
+	wlan_802_11_to_802_3_packet(pAd, pRxBlk, Header802_3, FromWhichBSSID);
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		ANNOUNCE_OR_FORWARD_802_3_PACKET(pAd, pRxBlk->pRxPacket, FromWhichBSSID);
+#endif // CONFIG_STA_SUPPORT //
+
+	if (pPacket2)
+	{
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			ANNOUNCE_OR_FORWARD_802_3_PACKET(pAd, pPacket2, FromWhichBSSID);
+#endif // CONFIG_STA_SUPPORT //
+	}
+}
+
+
+#define RESET_FRAGFRAME(_fragFrame) \
+	{								\
+		_fragFrame.RxSize = 0;		\
+		_fragFrame.Sequence = 0;	\
+		_fragFrame.LastFrag = 0;	\
+		_fragFrame.Flags = 0;		\
+	}
+
+
+PNDIS_PACKET RTMPDeFragmentDataFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk)
+{
+	PHEADER_802_11	pHeader = pRxBlk->pHeader;
+	PNDIS_PACKET	pRxPacket = pRxBlk->pRxPacket;
+	UCHAR			*pData = pRxBlk->pData;
+	USHORT			DataSize = pRxBlk->DataSize;
+	PNDIS_PACKET	pRetPacket = NULL;
+	UCHAR			*pFragBuffer = NULL;
+	BOOLEAN 		bReassDone = FALSE;
+	UCHAR			HeaderRoom = 0;
+
+
+	ASSERT(pHeader);
+
+	HeaderRoom = pData - (UCHAR *)pHeader;
+
+	// Re-assemble the fragmented packets
+	if (pHeader->Frag == 0)		// Frag. Number is 0 : First frag or only one pkt
+	{
+		// the first pkt of fragment, record it.
+		if (pHeader->FC.MoreFrag)
+		{
+			ASSERT(pAd->FragFrame.pFragPacket);
+			pFragBuffer = GET_OS_PKT_DATAPTR(pAd->FragFrame.pFragPacket);
+			pAd->FragFrame.RxSize   = DataSize + HeaderRoom;
+			NdisMoveMemory(pFragBuffer,	 pHeader, pAd->FragFrame.RxSize);
+			pAd->FragFrame.Sequence = pHeader->Sequence;
+			pAd->FragFrame.LastFrag = pHeader->Frag;	   // Should be 0
+			ASSERT(pAd->FragFrame.LastFrag == 0);
+			goto done;	// end of processing this frame
+		}
+	}
+	else	//Middle & End of fragment
+	{
+		if ((pHeader->Sequence != pAd->FragFrame.Sequence) ||
+			(pHeader->Frag != (pAd->FragFrame.LastFrag + 1)))
+		{
+			// Fragment is not the same sequence or out of fragment number order
+			// Reset Fragment control blk
+			RESET_FRAGFRAME(pAd->FragFrame);
+			DBGPRINT(RT_DEBUG_ERROR, ("Fragment is not the same sequence or out of fragment number order.\n"));
+			goto done; // give up this frame
+		}
+		else if ((pAd->FragFrame.RxSize + DataSize) > MAX_FRAME_SIZE)
+		{
+			// Fragment frame is too large, it exeeds the maximum frame size.
+			// Reset Fragment control blk
+			RESET_FRAGFRAME(pAd->FragFrame);
+			DBGPRINT(RT_DEBUG_ERROR, ("Fragment frame is too large, it exeeds the maximum frame size.\n"));
+			goto done; // give up this frame
+		}
+
+        //
+		// Broadcom AP(BCM94704AGR) will send out LLC in fragment's packet, LLC only can accpet at first fragment.
+		// In this case, we will dropt it.
+		//
+		if (NdisEqualMemory(pData, SNAP_802_1H, sizeof(SNAP_802_1H)))
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("Find another LLC at Middle or End fragment(SN=%d, Frag=%d)\n", pHeader->Sequence, pHeader->Frag));
+			goto done; // give up this frame
+		}
+
+		pFragBuffer = GET_OS_PKT_DATAPTR(pAd->FragFrame.pFragPacket);
+
+		// concatenate this fragment into the re-assembly buffer
+		NdisMoveMemory((pFragBuffer + pAd->FragFrame.RxSize), pData, DataSize);
+		pAd->FragFrame.RxSize  += DataSize;
+		pAd->FragFrame.LastFrag = pHeader->Frag;	   // Update fragment number
+
+		// Last fragment
+		if (pHeader->FC.MoreFrag == FALSE)
+		{
+			bReassDone = TRUE;
+		}
+	}
+
+done:
+	// always release rx fragmented packet
+	RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+
+	// return defragmented packet if packet is reassembled completely
+	// otherwise return NULL
+	if (bReassDone)
+	{
+		PNDIS_PACKET pNewFragPacket;
+
+		// allocate a new packet buffer for fragment
+		pNewFragPacket = RTMP_AllocateFragPacketBuffer(pAd, RX_BUFFER_NORMSIZE);
+		if (pNewFragPacket)
+		{
+			// update RxBlk
+			pRetPacket = pAd->FragFrame.pFragPacket;
+			pAd->FragFrame.pFragPacket = pNewFragPacket;
+			pRxBlk->pHeader = (PHEADER_802_11) GET_OS_PKT_DATAPTR(pRetPacket);
+			pRxBlk->pData = (UCHAR *)pRxBlk->pHeader + HeaderRoom;
+			pRxBlk->DataSize = pAd->FragFrame.RxSize - HeaderRoom;
+			pRxBlk->pRxPacket = pRetPacket;
+		}
+		else
+		{
+			RESET_FRAGFRAME(pAd->FragFrame);
+		}
+	}
+
+	return pRetPacket;
+}
+
+
+VOID Indicate_AMSDU_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+	UINT			nMSDU;
+
+	update_os_packet_info(pAd, pRxBlk, FromWhichBSSID);
+	RTMP_SET_PACKET_IF(pRxBlk->pRxPacket, FromWhichBSSID);
+	nMSDU = deaggregate_AMSDU_announce(pAd, pRxBlk->pRxPacket, pRxBlk->pData, pRxBlk->DataSize);
+}
+
+VOID Indicate_EAPOL_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+	MAC_TABLE_ENTRY *pEntry = NULL;
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		pEntry = &pAd->MacTab.Content[BSSID_WCID];
+		STARxEAPOLFrameIndicate(pAd, pEntry, pRxBlk, FromWhichBSSID);
+		return;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	if (pEntry == NULL)
+	{
+		DBGPRINT(RT_DEBUG_WARN, ("Indicate_EAPOL_Packet: drop and release the invalid packet.\n"));
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+}
+
+#define BCN_TBTT_OFFSET		64	//defer 64 us
+VOID ReSyncBeaconTime(
+	IN  PRTMP_ADAPTER   pAd)
+{
+
+	UINT32  Offset;
+
+
+	Offset = (pAd->TbttTickCount) % (BCN_TBTT_OFFSET);
+
+	pAd->TbttTickCount++;
+
+	//
+	// The updated BeaconInterval Value will affect Beacon Interval after two TBTT
+	// beacasue the original BeaconInterval had been loaded into next TBTT_TIMER
+	//
+	if (Offset == (BCN_TBTT_OFFSET-2))
+	{
+		BCN_TIME_CFG_STRUC csr;
+		RTMP_IO_READ32(pAd, BCN_TIME_CFG, &csr.word);
+		csr.field.BeaconInterval = (pAd->CommonCfg.BeaconPeriod << 4) - 1 ;	// ASIC register in units of 1/16 TU = 64us
+		RTMP_IO_WRITE32(pAd, BCN_TIME_CFG, csr.word);
+	}
+	else
+	{
+		if (Offset == (BCN_TBTT_OFFSET-1))
+		{
+			BCN_TIME_CFG_STRUC csr;
+
+			RTMP_IO_READ32(pAd, BCN_TIME_CFG, &csr.word);
+			csr.field.BeaconInterval = (pAd->CommonCfg.BeaconPeriod) << 4; // ASIC register in units of 1/16 TU
+			RTMP_IO_WRITE32(pAd, BCN_TIME_CFG, csr.word);
+		}
+	}
+}
+
diff --git a/drivers/staging/rt2860/common/cmm_data_2860.c b/drivers/staging/rt2860/common/cmm_data_2860.c
new file mode 100644
index 00000000000..4f414edd658
--- /dev/null
+++ b/drivers/staging/rt2860/common/cmm_data_2860.c
@@ -0,0 +1,1240 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+*/
+
+/*
+   All functions in this file must be PCI-depended, or you should out your function
+	in other files.
+
+*/
+#include "../rt_config.h"
+
+extern RTMP_RF_REGS RF2850RegTable[];
+extern UCHAR	NUM_OF_2850_CHNL;
+
+USHORT RtmpPCI_WriteTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	BOOLEAN			bIsLast,
+	OUT	USHORT			*FreeNumber)
+{
+
+	UCHAR			*pDMAHeaderBufVA;
+	USHORT			TxIdx, RetTxIdx;
+	PTXD_STRUC		pTxD;
+	UINT32			BufBasePaLow;
+	PRTMP_TX_RING	pTxRing;
+	USHORT			hwHeaderLen;
+
+	//
+	// get Tx Ring Resource
+	//
+	pTxRing = &pAd->TxRing[pTxBlk->QueIdx];
+	TxIdx = pAd->TxRing[pTxBlk->QueIdx].TxCpuIdx;
+	pDMAHeaderBufVA = (PUCHAR) pTxRing->Cell[TxIdx].DmaBuf.AllocVa;
+	BufBasePaLow = RTMP_GetPhysicalAddressLow(pTxRing->Cell[TxIdx].DmaBuf.AllocPa);
+
+	// copy TXINFO + TXWI + WLAN Header + LLC into DMA Header Buffer
+	if (pTxBlk->TxFrameType == TX_AMSDU_FRAME)
+	{
+		hwHeaderLen = pTxBlk->MpduHeaderLen - LENGTH_AMSDU_SUBFRAMEHEAD + pTxBlk->HdrPadLen + LENGTH_AMSDU_SUBFRAMEHEAD;
+	}
+	else
+	{
+		hwHeaderLen = pTxBlk->MpduHeaderLen + pTxBlk->HdrPadLen;
+	}
+	NdisMoveMemory(pDMAHeaderBufVA, pTxBlk->HeaderBuf, TXINFO_SIZE + TXWI_SIZE + hwHeaderLen);
+
+	pTxRing->Cell[TxIdx].pNdisPacket = pTxBlk->pPacket;
+	pTxRing->Cell[TxIdx].pNextNdisPacket = NULL;
+
+	//
+	// build Tx Descriptor
+	//
+
+	pTxD = (PTXD_STRUC) pTxRing->Cell[TxIdx].AllocVa;
+	NdisZeroMemory(pTxD, TXD_SIZE);
+
+	pTxD->SDPtr0 = BufBasePaLow;
+	pTxD->SDLen0 = TXINFO_SIZE + TXWI_SIZE + hwHeaderLen; // include padding
+	pTxD->SDPtr1 = PCI_MAP_SINGLE(pAd, pTxBlk, 0, 1, PCI_DMA_TODEVICE);;
+	pTxD->SDLen1 = pTxBlk->SrcBufLen;
+	pTxD->LastSec0 = 0;
+	pTxD->LastSec1 = (bIsLast) ? 1 : 0;
+
+	RTMPWriteTxDescriptor(pAd, pTxD, FALSE, FIFO_EDCA);
+
+	RetTxIdx = TxIdx;
+	//
+	// Update Tx index
+	//
+	INC_RING_INDEX(TxIdx, TX_RING_SIZE);
+	pTxRing->TxCpuIdx = TxIdx;
+
+	*FreeNumber -= 1;
+
+	return RetTxIdx;
+}
+
+
+USHORT RtmpPCI_WriteSingleTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	BOOLEAN			bIsLast,
+	OUT	USHORT			*FreeNumber)
+{
+
+	UCHAR			*pDMAHeaderBufVA;
+	USHORT			TxIdx, RetTxIdx;
+	PTXD_STRUC		pTxD;
+#ifdef RT_BIG_ENDIAN
+    PTXD_STRUC      pDestTxD;
+    TXD_STRUC       TxD;
+#endif
+	UINT32			BufBasePaLow;
+	PRTMP_TX_RING	pTxRing;
+	USHORT			hwHeaderLen;
+
+	//
+	// get Tx Ring Resource
+	//
+	pTxRing = &pAd->TxRing[pTxBlk->QueIdx];
+	TxIdx = pAd->TxRing[pTxBlk->QueIdx].TxCpuIdx;
+	pDMAHeaderBufVA = (PUCHAR) pTxRing->Cell[TxIdx].DmaBuf.AllocVa;
+	BufBasePaLow = RTMP_GetPhysicalAddressLow(pTxRing->Cell[TxIdx].DmaBuf.AllocPa);
+
+	// copy TXINFO + TXWI + WLAN Header + LLC into DMA Header Buffer
+	hwHeaderLen = pTxBlk->MpduHeaderLen + pTxBlk->HdrPadLen;
+
+	NdisMoveMemory(pDMAHeaderBufVA, pTxBlk->HeaderBuf, TXINFO_SIZE + TXWI_SIZE + hwHeaderLen);
+
+	pTxRing->Cell[TxIdx].pNdisPacket = pTxBlk->pPacket;
+	pTxRing->Cell[TxIdx].pNextNdisPacket = NULL;
+
+	//
+	// build Tx Descriptor
+	//
+#ifndef RT_BIG_ENDIAN
+	pTxD = (PTXD_STRUC) pTxRing->Cell[TxIdx].AllocVa;
+#else
+	pDestTxD = (PTXD_STRUC) pTxRing->Cell[TxIdx].AllocVa;
+	TxD = *pDestTxD;
+	pTxD = &TxD;
+#endif
+	NdisZeroMemory(pTxD, TXD_SIZE);
+
+	pTxD->SDPtr0 = BufBasePaLow;
+	pTxD->SDLen0 = TXINFO_SIZE + TXWI_SIZE + hwHeaderLen; // include padding
+	pTxD->SDPtr1 = PCI_MAP_SINGLE(pAd, pTxBlk, 0, 1, PCI_DMA_TODEVICE);;
+	pTxD->SDLen1 = pTxBlk->SrcBufLen;
+	pTxD->LastSec0 = 0;
+	pTxD->LastSec1 = (bIsLast) ? 1 : 0;
+
+	RTMPWriteTxDescriptor(pAd, pTxD, FALSE, FIFO_EDCA);
+#ifdef RT_BIG_ENDIAN
+	RTMPWIEndianChange((PUCHAR)(pDMAHeaderBufVA + TXINFO_SIZE), TYPE_TXWI);
+	RTMPFrameEndianChange(pAd, (PUCHAR)(pDMAHeaderBufVA + TXINFO_SIZE + TXWI_SIZE), DIR_WRITE, FALSE);
+	RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+    WriteBackToDescriptor((PUCHAR)pDestTxD, (PUCHAR)pTxD, FALSE, TYPE_TXD);
+#endif // RT_BIG_ENDIAN //
+
+	RetTxIdx = TxIdx;
+	//
+	// Update Tx index
+	//
+	INC_RING_INDEX(TxIdx, TX_RING_SIZE);
+	pTxRing->TxCpuIdx = TxIdx;
+
+	*FreeNumber -= 1;
+
+	return RetTxIdx;
+}
+
+
+USHORT RtmpPCI_WriteMultiTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	UCHAR			frameNum,
+	OUT	USHORT			*FreeNumber)
+{
+	BOOLEAN bIsLast;
+	UCHAR			*pDMAHeaderBufVA;
+	USHORT			TxIdx, RetTxIdx;
+	PTXD_STRUC		pTxD;
+#ifdef RT_BIG_ENDIAN
+    PTXD_STRUC      pDestTxD;
+    TXD_STRUC       TxD;
+#endif
+	UINT32			BufBasePaLow;
+	PRTMP_TX_RING	pTxRing;
+	USHORT			hwHdrLen;
+	UINT32			firstDMALen;
+
+	bIsLast = ((frameNum == (pTxBlk->TotalFrameNum - 1)) ? 1 : 0);
+
+	//
+	// get Tx Ring Resource
+	//
+	pTxRing = &pAd->TxRing[pTxBlk->QueIdx];
+	TxIdx = pAd->TxRing[pTxBlk->QueIdx].TxCpuIdx;
+	pDMAHeaderBufVA = (PUCHAR) pTxRing->Cell[TxIdx].DmaBuf.AllocVa;
+	BufBasePaLow = RTMP_GetPhysicalAddressLow(pTxRing->Cell[TxIdx].DmaBuf.AllocPa);
+
+	if (frameNum == 0)
+	{
+		// copy TXINFO + TXWI + WLAN Header + LLC into DMA Header Buffer
+		if (pTxBlk->TxFrameType == TX_AMSDU_FRAME)
+			//hwHdrLen = ROUND_UP(pTxBlk->MpduHeaderLen-LENGTH_AMSDU_SUBFRAMEHEAD, 4)+LENGTH_AMSDU_SUBFRAMEHEAD;
+			hwHdrLen = pTxBlk->MpduHeaderLen - LENGTH_AMSDU_SUBFRAMEHEAD + pTxBlk->HdrPadLen + LENGTH_AMSDU_SUBFRAMEHEAD;
+		else if (pTxBlk->TxFrameType == TX_RALINK_FRAME)
+			//hwHdrLen = ROUND_UP(pTxBlk->MpduHeaderLen-LENGTH_ARALINK_HEADER_FIELD, 4)+LENGTH_ARALINK_HEADER_FIELD;
+			hwHdrLen = pTxBlk->MpduHeaderLen - LENGTH_ARALINK_HEADER_FIELD + pTxBlk->HdrPadLen + LENGTH_ARALINK_HEADER_FIELD;
+		else
+			//hwHdrLen = ROUND_UP(pTxBlk->MpduHeaderLen, 4);
+			hwHdrLen = pTxBlk->MpduHeaderLen + pTxBlk->HdrPadLen;
+
+		firstDMALen = TXINFO_SIZE + TXWI_SIZE + hwHdrLen;
+	}
+	else
+	{
+		firstDMALen = pTxBlk->MpduHeaderLen;
+	}
+
+	NdisMoveMemory(pDMAHeaderBufVA, pTxBlk->HeaderBuf, firstDMALen);
+
+	pTxRing->Cell[TxIdx].pNdisPacket = pTxBlk->pPacket;
+	pTxRing->Cell[TxIdx].pNextNdisPacket = NULL;
+
+	//
+	// build Tx Descriptor
+	//
+#ifndef RT_BIG_ENDIAN
+	pTxD = (PTXD_STRUC) pTxRing->Cell[TxIdx].AllocVa;
+#else
+	pDestTxD = (PTXD_STRUC) pTxRing->Cell[TxIdx].AllocVa;
+	TxD = *pDestTxD;
+	pTxD = &TxD;
+#endif
+	NdisZeroMemory(pTxD, TXD_SIZE);
+
+	pTxD->SDPtr0 = BufBasePaLow;
+	pTxD->SDLen0 = firstDMALen; // include padding
+	pTxD->SDPtr1 = PCI_MAP_SINGLE(pAd, pTxBlk, 0, 1, PCI_DMA_TODEVICE);;
+	pTxD->SDLen1 = pTxBlk->SrcBufLen;
+	pTxD->LastSec0 = 0;
+	pTxD->LastSec1 = (bIsLast) ? 1 : 0;
+
+	RTMPWriteTxDescriptor(pAd, pTxD, FALSE, FIFO_EDCA);
+
+#ifdef RT_BIG_ENDIAN
+	if (frameNum == 0)
+		RTMPFrameEndianChange(pAd, (PUCHAR)(pDMAHeaderBufVA+ TXINFO_SIZE + TXWI_SIZE), DIR_WRITE, FALSE);
+
+	if (frameNum != 0)
+		RTMPWIEndianChange((PUCHAR)(pDMAHeaderBufVA + TXINFO_SIZE), TYPE_TXWI);
+
+	RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+	WriteBackToDescriptor((PUCHAR)pDestTxD, (PUCHAR)pTxD, FALSE, TYPE_TXD);
+#endif // RT_BIG_ENDIAN //
+
+	RetTxIdx = TxIdx;
+	//
+	// Update Tx index
+	//
+	INC_RING_INDEX(TxIdx, TX_RING_SIZE);
+	pTxRing->TxCpuIdx = TxIdx;
+
+	*FreeNumber -= 1;
+
+	return RetTxIdx;
+
+}
+
+
+VOID RtmpPCI_FinalWriteTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	USHORT			totalMPDUSize,
+	IN	USHORT			FirstTxIdx)
+{
+
+	PTXWI_STRUC		pTxWI;
+	PRTMP_TX_RING	pTxRing;
+
+	//
+	// get Tx Ring Resource
+	//
+	pTxRing = &pAd->TxRing[pTxBlk->QueIdx];
+	pTxWI = (PTXWI_STRUC) pTxRing->Cell[FirstTxIdx].DmaBuf.AllocVa;
+	pTxWI->MPDUtotalByteCount = totalMPDUSize;
+#ifdef RT_BIG_ENDIAN
+	RTMPWIEndianChange((PUCHAR)pTxWI, TYPE_TXWI);
+#endif // RT_BIG_ENDIAN //
+
+}
+
+
+VOID RtmpPCIDataLastTxIdx(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			QueIdx,
+	IN	USHORT			LastTxIdx)
+{
+	PTXD_STRUC		pTxD;
+#ifdef RT_BIG_ENDIAN
+    PTXD_STRUC      pDestTxD;
+    TXD_STRUC       TxD;
+#endif
+	PRTMP_TX_RING	pTxRing;
+
+	//
+	// get Tx Ring Resource
+	//
+	pTxRing = &pAd->TxRing[QueIdx];
+
+	//
+	// build Tx Descriptor
+	//
+#ifndef RT_BIG_ENDIAN
+	pTxD = (PTXD_STRUC) pTxRing->Cell[LastTxIdx].AllocVa;
+#else
+	pDestTxD = (PTXD_STRUC) pTxRing->Cell[LastTxIdx].AllocVa;
+	TxD = *pDestTxD;
+	pTxD = &TxD;
+#endif
+
+	pTxD->LastSec1 = 1;
+
+#ifdef RT_BIG_ENDIAN
+	RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+    WriteBackToDescriptor((PUCHAR)pDestTxD, (PUCHAR)pTxD, FALSE, TYPE_TXD);
+#endif // RT_BIG_ENDIAN //
+
+}
+
+
+USHORT	RtmpPCI_WriteFragTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	UCHAR			fragNum,
+	OUT	USHORT			*FreeNumber)
+{
+	UCHAR			*pDMAHeaderBufVA;
+	USHORT			TxIdx, RetTxIdx;
+	PTXD_STRUC		pTxD;
+#ifdef RT_BIG_ENDIAN
+    PTXD_STRUC      pDestTxD;
+    TXD_STRUC       TxD;
+#endif
+	UINT32			BufBasePaLow;
+	PRTMP_TX_RING	pTxRing;
+	USHORT			hwHeaderLen;
+	UINT32			firstDMALen;
+
+	//
+	// Get Tx Ring Resource
+	//
+	pTxRing = &pAd->TxRing[pTxBlk->QueIdx];
+	TxIdx = pAd->TxRing[pTxBlk->QueIdx].TxCpuIdx;
+	pDMAHeaderBufVA = (PUCHAR) pTxRing->Cell[TxIdx].DmaBuf.AllocVa;
+	BufBasePaLow = RTMP_GetPhysicalAddressLow(pTxRing->Cell[TxIdx].DmaBuf.AllocPa);
+
+	//
+	// Copy TXINFO + TXWI + WLAN Header + LLC into DMA Header Buffer
+	//
+	hwHeaderLen = pTxBlk->MpduHeaderLen + pTxBlk->HdrPadLen;
+
+	firstDMALen = TXINFO_SIZE + TXWI_SIZE + hwHeaderLen;
+	NdisMoveMemory(pDMAHeaderBufVA, pTxBlk->HeaderBuf, firstDMALen);
+
+
+	//
+	// Build Tx Descriptor
+	//
+#ifndef RT_BIG_ENDIAN
+	pTxD = (PTXD_STRUC) pTxRing->Cell[TxIdx].AllocVa;
+#else
+	pDestTxD = (PTXD_STRUC) pTxRing->Cell[TxIdx].AllocVa;
+	TxD = *pDestTxD;
+	pTxD = &TxD;
+#endif
+	NdisZeroMemory(pTxD, TXD_SIZE);
+
+	if (fragNum == pTxBlk->TotalFragNum)
+	{
+		pTxRing->Cell[TxIdx].pNdisPacket = pTxBlk->pPacket;
+		pTxRing->Cell[TxIdx].pNextNdisPacket = NULL;
+	}
+
+	pTxD->SDPtr0 = BufBasePaLow;
+	pTxD->SDLen0 = firstDMALen; // include padding
+	pTxD->SDPtr1 = PCI_MAP_SINGLE(pAd, pTxBlk, 0, 1, PCI_DMA_TODEVICE);
+	pTxD->SDLen1 = pTxBlk->SrcBufLen;
+	pTxD->LastSec0 = 0;
+	pTxD->LastSec1 = 1;
+
+	RTMPWriteTxDescriptor(pAd, pTxD, FALSE, FIFO_EDCA);
+
+#ifdef RT_BIG_ENDIAN
+	RTMPWIEndianChange((PUCHAR)(pDMAHeaderBufVA + TXINFO_SIZE), TYPE_TXWI);
+	RTMPFrameEndianChange(pAd, (PUCHAR)(pDMAHeaderBufVA + TXINFO_SIZE + TXWI_SIZE), DIR_WRITE, FALSE);
+	RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+    WriteBackToDescriptor((PUCHAR)pDestTxD, (PUCHAR)pTxD, FALSE, TYPE_TXD);
+#endif // RT_BIG_ENDIAN //
+
+	RetTxIdx = TxIdx;
+	pTxBlk->Priv += pTxBlk->SrcBufLen;
+
+	//
+	// Update Tx index
+	//
+	INC_RING_INDEX(TxIdx, TX_RING_SIZE);
+	pTxRing->TxCpuIdx = TxIdx;
+
+	*FreeNumber -= 1;
+
+	return RetTxIdx;
+
+}
+
+/*
+	Must be run in Interrupt context
+	This function handle PCI specific TxDesc and cpu index update and kick the packet out.
+ */
+int RtmpPCIMgmtKickOut(
+	IN RTMP_ADAPTER 	*pAd,
+	IN UCHAR 			QueIdx,
+	IN PNDIS_PACKET		pPacket,
+	IN PUCHAR			pSrcBufVA,
+	IN UINT 			SrcBufLen)
+{
+	PTXD_STRUC		pTxD;
+#ifdef RT_BIG_ENDIAN
+    PTXD_STRUC      pDestTxD;
+    TXD_STRUC       TxD;
+#endif
+	ULONG			SwIdx = pAd->MgmtRing.TxCpuIdx;
+
+#ifdef RT_BIG_ENDIAN
+    pDestTxD  = (PTXD_STRUC)pAd->MgmtRing.Cell[SwIdx].AllocVa;
+    TxD = *pDestTxD;
+    pTxD = &TxD;
+    RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+#else
+	pTxD  = (PTXD_STRUC) pAd->MgmtRing.Cell[SwIdx].AllocVa;
+#endif
+
+	pAd->MgmtRing.Cell[SwIdx].pNdisPacket = pPacket;
+	pAd->MgmtRing.Cell[SwIdx].pNextNdisPacket = NULL;
+
+	RTMPWriteTxDescriptor(pAd, pTxD, TRUE, FIFO_MGMT);
+	pTxD->LastSec0 = 1;
+	pTxD->LastSec1 = 1;
+	pTxD->DMADONE = 0;
+	pTxD->SDLen1 = 0;
+	pTxD->SDPtr0 = PCI_MAP_SINGLE(pAd, pSrcBufVA, SrcBufLen, 0, PCI_DMA_TODEVICE);;
+	pTxD->SDLen0 = SrcBufLen;
+
+#ifdef RT_BIG_ENDIAN
+	RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+    WriteBackToDescriptor((PUCHAR)pDestTxD, (PUCHAR)pTxD, FALSE, TYPE_TXD);
+#endif
+
+	pAd->RalinkCounters.KickTxCount++;
+	pAd->RalinkCounters.OneSecTxDoneCount++;
+
+	// Increase TX_CTX_IDX, but write to register later.
+	INC_RING_INDEX(pAd->MgmtRing.TxCpuIdx, MGMT_RING_SIZE);
+
+	RTMP_IO_WRITE32(pAd, TX_MGMTCTX_IDX,  pAd->MgmtRing.TxCpuIdx);
+
+	return 0;
+}
+
+
+#ifdef CONFIG_STA_SUPPORT
+/*
+	========================================================================
+
+	Routine Description:
+		Check Rx descriptor, return NDIS_STATUS_FAILURE if any error dound
+
+	Arguments:
+		pRxD		Pointer to the Rx descriptor
+
+	Return Value:
+		NDIS_STATUS_SUCCESS 	No err
+		NDIS_STATUS_FAILURE 	Error
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS RTMPCheckRxError(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	PHEADER_802_11		pHeader,
+	IN	PRXWI_STRUC 		pRxWI,
+	IN  PRT28XX_RXD_STRUC 	pRxD)
+{
+	PCIPHER_KEY pWpaKey;
+	INT dBm;
+
+	// Phy errors & CRC errors
+	if (/*(pRxD->PhyErr) ||*/ (pRxD->Crc))
+	{
+		// Check RSSI for Noise Hist statistic collection.
+		dBm = (INT) (pRxWI->RSSI0) - pAd->BbpRssiToDbmDelta;
+		if (dBm <= -87)
+			pAd->StaCfg.RPIDensity[0] += 1;
+		else if (dBm <= -82)
+			pAd->StaCfg.RPIDensity[1] += 1;
+		else if (dBm <= -77)
+			pAd->StaCfg.RPIDensity[2] += 1;
+		else if (dBm <= -72)
+			pAd->StaCfg.RPIDensity[3] += 1;
+		else if (dBm <= -67)
+			pAd->StaCfg.RPIDensity[4] += 1;
+		else if (dBm <= -62)
+			pAd->StaCfg.RPIDensity[5] += 1;
+		else if (dBm <= -57)
+			pAd->StaCfg.RPIDensity[6] += 1;
+		else if (dBm > -57)
+			pAd->StaCfg.RPIDensity[7] += 1;
+
+		return(NDIS_STATUS_FAILURE);
+	}
+
+	// Add Rx size to channel load counter, we should ignore error counts
+	pAd->StaCfg.CLBusyBytes += (pRxD->SDL0 + 14);
+
+	// Drop ToDs promiscous frame, it is opened due to CCX 2 channel load statistics
+	if (pHeader != NULL)
+	{
+		if (pHeader->FC.ToDs)
+		{
+			return(NDIS_STATUS_FAILURE);
+		}
+	}
+
+	// Drop not U2M frames, cant's drop here because we will drop beacon in this case
+	// I am kind of doubting the U2M bit operation
+	// if (pRxD->U2M == 0)
+	//	return(NDIS_STATUS_FAILURE);
+
+	// drop decyption fail frame
+	if (pRxD->CipherErr)
+	{
+		if (pRxD->CipherErr == 2)
+			{DBGPRINT_RAW(RT_DEBUG_TRACE,("pRxD ERROR: ICV ok but MICErr "));}
+		else if (pRxD->CipherErr == 1)
+			{DBGPRINT_RAW(RT_DEBUG_TRACE,("pRxD ERROR: ICV Err "));}
+		else if (pRxD->CipherErr == 3)
+			DBGPRINT_RAW(RT_DEBUG_TRACE,("pRxD ERROR: Key not valid "));
+
+        if (((pRxD->CipherErr & 1) == 1) && pAd->CommonCfg.bWirelessEvent && INFRA_ON(pAd))
+            RTMPSendWirelessEvent(pAd, IW_ICV_ERROR_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+
+		DBGPRINT_RAW(RT_DEBUG_TRACE,(" %d (len=%d, Mcast=%d, MyBss=%d, Wcid=%d, KeyId=%d)\n",
+			pRxD->CipherErr,
+			pRxD->SDL0,
+			pRxD->Mcast | pRxD->Bcast,
+			pRxD->MyBss,
+			pRxWI->WirelessCliID,
+			pRxWI->KeyIndex));
+
+		//
+		// MIC Error
+		//
+		if (pRxD->CipherErr == 2)
+		{
+			pWpaKey = &pAd->SharedKey[BSS0][pRxWI->KeyIndex];
+#ifdef WPA_SUPPLICANT_SUPPORT
+            if (pAd->StaCfg.WpaSupplicantUP)
+                WpaSendMicFailureToWpaSupplicant(pAd,
+                                   (pWpaKey->Type == PAIRWISEKEY) ? TRUE:FALSE);
+            else
+#endif // WPA_SUPPLICANT_SUPPORT //
+			    RTMPReportMicError(pAd, pWpaKey);
+
+            if (((pRxD->CipherErr & 2) == 2) && pAd->CommonCfg.bWirelessEvent && INFRA_ON(pAd))
+                RTMPSendWirelessEvent(pAd, IW_MIC_ERROR_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+
+			DBGPRINT_RAW(RT_DEBUG_ERROR,("Rx MIC Value error\n"));
+		}
+
+		if (pHeader == NULL)
+			return(NDIS_STATUS_SUCCESS);
+
+		return(NDIS_STATUS_FAILURE);
+	}
+
+	return(NDIS_STATUS_SUCCESS);
+}
+
+/*
+	==========================================================================
+	Description:
+		This routine sends command to firmware and turn our chip to power save mode.
+		Both RadioOff and .11 power save function needs to call this routine.
+	Input:
+		Level = GUIRADIO_OFF  : GUI Radio Off mode
+		Level = DOT11POWERSAVE  : 802.11 power save mode
+		Level = RTMP_HALT  : When Disable device.
+
+	==========================================================================
+ */
+VOID RT28xxPciAsicRadioOff(
+	IN PRTMP_ADAPTER    pAd,
+	IN UCHAR            Level,
+	IN USHORT           TbttNumToNextWakeUp)
+{
+	WPDMA_GLO_CFG_STRUC	DmaCfg;
+	UCHAR		i, tempBBP_R3 = 0;
+	BOOLEAN		brc = FALSE, Cancelled;
+    UINT32		TbTTTime = 0;
+	UINT32		PsPollTime = 0, MACValue;
+    ULONG		BeaconPeriodTime;
+    UINT32		RxDmaIdx, RxCpuIdx;
+	DBGPRINT(RT_DEBUG_TRACE, ("AsicRadioOff ===> TxCpuIdx = %d, TxDmaIdx = %d. RxCpuIdx = %d, RxDmaIdx = %d.\n", pAd->TxRing[0].TxCpuIdx, pAd->TxRing[0].TxDmaIdx, pAd->RxRing.RxCpuIdx, pAd->RxRing.RxDmaIdx));
+
+    // Check Rx DMA busy status, if more than half is occupied, give up this radio off.
+	RTMP_IO_READ32(pAd, RX_DRX_IDX , &RxDmaIdx);
+	RTMP_IO_READ32(pAd, RX_CRX_IDX , &RxCpuIdx);
+	if ((RxDmaIdx > RxCpuIdx) && ((RxDmaIdx - RxCpuIdx) > RX_RING_SIZE/3))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("AsicRadioOff ===> return1. RxDmaIdx = %d ,  RxCpuIdx = %d. \n", RxDmaIdx, RxCpuIdx));
+		return;
+	}
+	else if ((RxCpuIdx >= RxDmaIdx) && ((RxCpuIdx - RxDmaIdx) < RX_RING_SIZE/3))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("AsicRadioOff ===> return2.  RxCpuIdx = %d. RxDmaIdx = %d ,  \n", RxCpuIdx, RxDmaIdx));
+		return;
+	}
+
+    // Once go into this function, disable tx because don't want too many packets in queue to prevent HW stops.
+	pAd->bPCIclkOffDisableTx = TRUE;
+
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE))
+	{
+	    RTMPCancelTimer(&pAd->Mlme.RadioOnOffTimer,	&Cancelled);
+	    RTMPCancelTimer(&pAd->Mlme.PsPollTimer,	&Cancelled);
+
+	    if (Level == DOT11POWERSAVE)
+		{
+			RTMP_IO_READ32(pAd, TBTT_TIMER, &TbTTTime);
+			TbTTTime &= 0x1ffff;
+			// 00. check if need to do sleep in this DTIM period.   If next beacon will arrive within 30ms , ...doesn't necessarily sleep.
+			// TbTTTime uint = 64us, LEAD_TIME unit = 1024us, PsPollTime unit = 1ms
+	        if  (((64*TbTTTime) <((LEAD_TIME*1024) + 40000)) && (TbttNumToNextWakeUp == 0))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("TbTTTime = 0x%x , give up this sleep. \n", TbTTTime));
+	            OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_DOZE);
+	            pAd->bPCIclkOffDisableTx = FALSE;
+				return;
+			}
+			else
+			{
+				PsPollTime = (64*TbTTTime- LEAD_TIME*1024)/1000;
+				PsPollTime -= 3;
+
+	            BeaconPeriodTime = pAd->CommonCfg.BeaconPeriod*102/100;
+				if (TbttNumToNextWakeUp > 0)
+					PsPollTime += ((TbttNumToNextWakeUp -1) * BeaconPeriodTime);
+
+	            pAd->Mlme.bPsPollTimerRunning = TRUE;
+				RTMPSetTimer(&pAd->Mlme.PsPollTimer, PsPollTime);
+			}
+		}
+	}
+
+    // 0. Disable Tx DMA.
+	RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &DmaCfg.word);
+	DmaCfg.field.EnableTxDMA = 0;
+	RTMP_IO_WRITE32(pAd, WPDMA_GLO_CFG, DmaCfg.word);
+
+	// 1. Wait DMA not busy
+	i = 0;
+	do
+	{
+		RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &DmaCfg.word);
+		if ((DmaCfg.field.TxDMABusy == 0) && (DmaCfg.field.RxDMABusy == 0))
+			break;
+		RTMPusecDelay(20);
+		i++;
+	}while(i < 50);
+
+	if (i >= 50)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("DMA keeps busy.  return on RT28xxPciAsicRadioOff ()\n"));
+		pAd->bPCIclkOffDisableTx = FALSE;
+		RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &DmaCfg.word);
+		DmaCfg.field.EnableTxDMA = 1;
+		RTMP_IO_WRITE32(pAd, WPDMA_GLO_CFG, DmaCfg.word);
+		return;
+	}
+
+    RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_IDLE_RADIO_OFF);
+
+    // Set to 1R.
+    tempBBP_R3 = (pAd->StaCfg.BBPR3 & 0xE7);
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, tempBBP_R3);
+
+	// In Radio Off, we turn off RF clk, So now need to call ASICSwitchChannel again.
+	if (INFRA_ON(pAd) && (pAd->CommonCfg.CentralChannel != pAd->CommonCfg.Channel)
+		&& (pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth == BW_40))
+	{
+		// Must using 40MHz.
+		AsicTurnOffRFClk(pAd, pAd->CommonCfg.CentralChannel);
+	}
+	else
+	{
+		// Must using 20MHz.
+		AsicTurnOffRFClk(pAd, pAd->CommonCfg.Channel);
+	}
+
+    // When PCI clock is off, don't want to service interrupt.
+	RTMP_IO_WRITE32(pAd, INT_MASK_CSR, AutoWakeupInt);
+
+    RTMP_IO_WRITE32(pAd, RX_CRX_IDX, pAd->RxRing.RxCpuIdx);
+	// Disable MAC Rx
+	RTMP_IO_READ32(pAd, MAC_SYS_CTRL , &MACValue);
+	MACValue &= 0xf7;
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL , MACValue);
+
+	//  2. Send Sleep command
+	RTMP_IO_WRITE32(pAd, H2M_MAILBOX_STATUS, 0xffffffff);
+	RTMP_IO_WRITE32(pAd, H2M_MAILBOX_CID, 0xffffffff);
+	AsicSendCommandToMcu(pAd, 0x30, PowerSafeCID, 0xff, 0x00);   // send POWER-SAVE command to MCU. Timeout unit:40us.
+	//  2-1. Wait command success
+	// Status = 1 : success, Status = 2, already sleep, Status = 3, Maybe MAC is busy so can't finish this task.
+	brc = AsicCheckCommanOk(pAd, PowerSafeCID);
+
+    if (brc == FALSE)
+    {
+        // try again
+    	AsicSendCommandToMcu(pAd, 0x30, PowerSafeCID, 0xff, 0x00);   // send POWER-SAVE command to MCU. Timeout unit:40us.
+    	//RTMPusecDelay(200);
+    	brc = AsicCheckCommanOk(pAd, PowerSafeCID);
+    }
+
+	//  3. After 0x30 command is ok, send radio off command. lowbyte = 0 for power safe.
+	// If 0x30 command is not ok this time, we can ignore 0x35 command. It will make sure not cause firmware'r problem.
+	if ((Level == DOT11POWERSAVE) && (brc == TRUE))
+	{
+		AsicSendCommandToMcu(pAd, 0x35, PowerRadioOffCID, 0, 0x00);	// lowbyte = 0 means to do power safe, NOT turn off radio.
+	 	//  3-1. Wait command success
+	 	AsicCheckCommanOk(pAd, PowerRadioOffCID);
+	}
+	else if (brc == TRUE)
+	{
+		AsicSendCommandToMcu(pAd, 0x35, PowerRadioOffCID, 1, 0x00);	// lowbyte = 0 means to do power safe, NOT turn off radio.
+	 	//  3-1. Wait command success
+	 	AsicCheckCommanOk(pAd, PowerRadioOffCID);
+	}
+
+    // Wait DMA not busy
+	i = 0;
+	do
+	{
+		RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &DmaCfg.word);
+		if (DmaCfg.field.RxDMABusy == 0)
+			break;
+		RTMPusecDelay(20);
+		i++;
+	}while(i < 50);
+
+	if (i >= 50)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("DMA Rx keeps busy.  on RT28xxPciAsicRadioOff ()\n"));
+	}
+	// disable DMA Rx.
+	{
+		RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &DmaCfg.word);
+		DmaCfg.field.EnableRxDMA = 0;
+		RTMP_IO_WRITE32(pAd, WPDMA_GLO_CFG, DmaCfg.word);
+	}
+
+	if (Level == DOT11POWERSAVE)
+	{
+		AUTO_WAKEUP_STRUC	AutoWakeupCfg;
+		//RTMPSetTimer(&pAd->Mlme.PsPollTimer, 90);
+
+		// we have decided to SLEEP, so at least do it for a BEACON period.
+		if (TbttNumToNextWakeUp == 0)
+			TbttNumToNextWakeUp = 1;
+
+		AutoWakeupCfg.word = 0;
+		RTMP_IO_WRITE32(pAd, AUTO_WAKEUP_CFG, AutoWakeupCfg.word);
+
+		// 1. Set auto wake up timer.
+		AutoWakeupCfg.field.NumofSleepingTbtt = TbttNumToNextWakeUp - 1;
+		AutoWakeupCfg.field.EnableAutoWakeup = 1;
+		AutoWakeupCfg.field.AutoLeadTime = LEAD_TIME;
+		RTMP_IO_WRITE32(pAd, AUTO_WAKEUP_CFG, AutoWakeupCfg.word);
+	}
+
+	//  4-1. If it's to disable our device. Need to restore PCI Configuration Space to its original value.
+	if (Level == RTMP_HALT)
+	{
+		if ((brc == TRUE) && (i < 50))
+			RTMPPCIeLinkCtrlSetting(pAd, 1);
+	}
+	//  4. Set PCI configuration Space Link Comtrol fields.  Only Radio Off needs to call this function
+	else
+	{
+		if ((brc == TRUE) && (i < 50))
+			RTMPPCIeLinkCtrlSetting(pAd, 3);
+	}
+
+    pAd->bPCIclkOffDisableTx = FALSE;
+}
+
+
+/*
+	==========================================================================
+	Description:
+		This routine sends command to firmware and turn our chip to wake up mode from power save mode.
+		Both RadioOn and .11 power save function needs to call this routine.
+	Input:
+		Level = GUIRADIO_OFF : call this function is from Radio Off to Radio On.  Need to restore PCI host value.
+		Level = other value : normal wake up function.
+
+	==========================================================================
+ */
+BOOLEAN RT28xxPciAsicRadioOn(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR     Level)
+{
+    WPDMA_GLO_CFG_STRUC	DmaCfg;
+	BOOLEAN				Cancelled, brv = TRUE;
+    UINT32			    MACValue;
+
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE))
+	{
+	    pAd->Mlme.bPsPollTimerRunning = FALSE;
+		RTMPCancelTimer(&pAd->Mlme.PsPollTimer,	&Cancelled);
+		if ((Level == GUIRADIO_OFF) || (Level == GUI_IDLE_POWER_SAVE))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("RT28xxPciAsicRadioOn ()\n"));
+			// 1. Set PCI Link Control in Configuration Space.
+			RTMPPCIeLinkCtrlValueRestore(pAd, RESTORE_WAKEUP);
+			RTMPusecDelay(6000);
+		}
+	}
+
+    pAd->bPCIclkOff = FALSE;
+
+	// 2. Send wake up command.
+	AsicSendCommandToMcu(pAd, 0x31, PowerWakeCID, 0x00, 0x00);
+
+	// 2-1. wait command ok.
+	brv = AsicCheckCommanOk(pAd, PowerWakeCID);
+    if (brv)
+    {
+    	//RTMP_IO_WRITE32(pAd, INT_MASK_CSR, (DELAYINTMASK|RxINT));
+    	NICEnableInterrupt(pAd);
+
+    	// 3. Enable Tx DMA.
+    	RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &DmaCfg.word);
+    	DmaCfg.field.EnableTxDMA = 1;
+        DmaCfg.field.EnableRxDMA = 1;
+    	RTMP_IO_WRITE32(pAd, WPDMA_GLO_CFG, DmaCfg.word);
+
+        // Eable MAC Rx
+    	RTMP_IO_READ32(pAd, MAC_SYS_CTRL , &MACValue);
+    	MACValue |= 0x8;
+    	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL , MACValue);
+
+    	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_IDLE_RADIO_OFF);
+    	if (Level == GUI_IDLE_POWER_SAVE)
+    	{
+    		// In Radio Off, we turn off RF clk, So now need to call ASICSwitchChannel again.
+    		if (INFRA_ON(pAd) && (pAd->CommonCfg.CentralChannel != pAd->CommonCfg.Channel)
+    			&& (pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth == BW_40))
+    		{
+    			// Must using 40MHz.
+    			AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+    			AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+    		}
+    		else
+    		{
+    			// Must using 20MHz.
+    			AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+    			AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+    		}
+    	}
+        return TRUE;
+    }
+    else
+        return FALSE;
+}
+
+VOID RT28xxPciStaAsicForceWakeup(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN       bFromTx)
+{
+    AUTO_WAKEUP_STRUC	AutoWakeupCfg;
+
+    if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+        return;
+
+    if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WAKEUP_NOW))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("waking up now!\n"));
+        return;
+    }
+
+    OPSTATUS_SET_FLAG(pAd, fOP_STATUS_WAKEUP_NOW);
+
+    if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE))
+    {
+        // Support PCIe Advance Power Save
+    	if (bFromTx == TRUE)
+    	{
+            pAd->Mlme.bPsPollTimerRunning = FALSE;
+    		RTMPPCIeLinkCtrlValueRestore(pAd, RESTORE_WAKEUP);
+    		RTMPusecDelay(3000);
+            DBGPRINT(RT_DEBUG_TRACE, ("=======AsicForceWakeup===bFromTx\n"));
+    	}
+
+		AutoWakeupCfg.word = 0;
+		RTMP_IO_WRITE32(pAd, AUTO_WAKEUP_CFG, AutoWakeupCfg.word);
+
+        if (RT28xxPciAsicRadioOn(pAd, DOT11POWERSAVE))
+        {
+            // In Radio Off, we turn off RF clk, So now need to call ASICSwitchChannel again.
+        	if (INFRA_ON(pAd) && (pAd->CommonCfg.CentralChannel != pAd->CommonCfg.Channel)
+        		&& (pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth == BW_40))
+        	{
+        		// Must using 40MHz.
+        		AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+        		AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+        	}
+        	else
+        	{
+        		// Must using 20MHz.
+        		AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+        		AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+        	}
+        }
+    }
+    else
+    {
+        // PCI, 2860-PCIe
+        AsicSendCommandToMcu(pAd, 0x31, 0xff, 0x00, 0x00);
+		AutoWakeupCfg.word = 0;
+		RTMP_IO_WRITE32(pAd, AUTO_WAKEUP_CFG, AutoWakeupCfg.word);
+    }
+
+    OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_DOZE);
+    OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_WAKEUP_NOW);
+    DBGPRINT(RT_DEBUG_TRACE, ("<=======RT28xxPciStaAsicForceWakeup\n"));
+}
+
+VOID RT28xxPciStaAsicSleepThenAutoWakeup(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT TbttNumToNextWakeUp)
+{
+    if (pAd->StaCfg.bRadio == FALSE)
+	{
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_DOZE);
+		return;
+	}
+    if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE))
+    {
+    	ULONG	Now = 0;
+        if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WAKEUP_NOW))
+        {
+            DBGPRINT(RT_DEBUG_TRACE, ("waking up now!\n"));
+            OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_DOZE);
+            return;
+        }
+
+		NdisGetSystemUpTime(&Now);
+		// If last send NULL fram time is too close to this receiving beacon (within 8ms), don't go to sleep for this DTM.
+		// Because Some AP can't queuing outgoing frames immediately.
+		if (((pAd->Mlme.LastSendNULLpsmTime + 8) >= Now) && (pAd->Mlme.LastSendNULLpsmTime <= Now))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("Now = %lu, LastSendNULLpsmTime=%lu :  RxCountSinceLastNULL = %lu. \n", Now, pAd->Mlme.LastSendNULLpsmTime, pAd->RalinkCounters.RxCountSinceLastNULL));
+			return;
+		}
+		else if ((pAd->RalinkCounters.RxCountSinceLastNULL > 0) && ((pAd->Mlme.LastSendNULLpsmTime + pAd->CommonCfg.BeaconPeriod) >= Now))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("Now = %lu, LastSendNULLpsmTime=%lu: RxCountSinceLastNULL = %lu > 0 \n", Now, pAd->Mlme.LastSendNULLpsmTime,  pAd->RalinkCounters.RxCountSinceLastNULL));
+			return;
+		}
+
+        RT28xxPciAsicRadioOff(pAd, DOT11POWERSAVE, TbttNumToNextWakeUp);
+    }
+    else
+    {
+        AUTO_WAKEUP_STRUC	AutoWakeupCfg;
+        // we have decided to SLEEP, so at least do it for a BEACON period.
+        if (TbttNumToNextWakeUp == 0)
+            TbttNumToNextWakeUp = 1;
+
+        AutoWakeupCfg.word = 0;
+        RTMP_IO_WRITE32(pAd, AUTO_WAKEUP_CFG, AutoWakeupCfg.word);
+        AutoWakeupCfg.field.NumofSleepingTbtt = TbttNumToNextWakeUp - 1;
+        AutoWakeupCfg.field.EnableAutoWakeup = 1;
+        AutoWakeupCfg.field.AutoLeadTime = 5;
+        RTMP_IO_WRITE32(pAd, AUTO_WAKEUP_CFG, AutoWakeupCfg.word);
+        AsicSendCommandToMcu(pAd, 0x30, 0xff, 0xff, 0x00);   // send POWER-SAVE command to MCU. Timeout 40us.
+        DBGPRINT(RT_DEBUG_TRACE, ("<-- %s, TbttNumToNextWakeUp=%d \n", __FUNCTION__, TbttNumToNextWakeUp));
+    }
+    OPSTATUS_SET_FLAG(pAd, fOP_STATUS_DOZE);
+}
+
+VOID PsPollWakeExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+	RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+	unsigned long flags;
+
+    DBGPRINT(RT_DEBUG_TRACE,("-->PsPollWakeExec \n"));
+	RTMP_INT_LOCK(&pAd->irq_lock, flags);
+    if (pAd->Mlme.bPsPollTimerRunning)
+    {
+	    RTMPPCIeLinkCtrlValueRestore(pAd, RESTORE_WAKEUP);
+    }
+    pAd->Mlme.bPsPollTimerRunning = FALSE;
+	RTMP_INT_UNLOCK(&pAd->irq_lock, flags);
+}
+
+VOID  RadioOnExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+	RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+	WPDMA_GLO_CFG_STRUC	DmaCfg;
+	BOOLEAN				Cancelled;
+
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("-->RadioOnExec() return on fOP_STATUS_DOZE == TRUE; \n"));
+		RTMPSetTimer(&pAd->Mlme.RadioOnOffTimer, 10);
+		return;
+	}
+
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("-->RadioOnExec() return on SCAN_IN_PROGRESS; \n"));
+		RTMPSetTimer(&pAd->Mlme.RadioOnOffTimer, 10);
+		return;
+	}
+    pAd->Mlme.bPsPollTimerRunning = FALSE;
+	RTMPCancelTimer(&pAd->Mlme.PsPollTimer,	&Cancelled);
+	if (pAd->StaCfg.bRadio == TRUE)
+	{
+		pAd->bPCIclkOff = FALSE;
+        RTMPRingCleanUp(pAd, QID_AC_BK);
+		RTMPRingCleanUp(pAd, QID_AC_BE);
+		RTMPRingCleanUp(pAd, QID_AC_VI);
+		RTMPRingCleanUp(pAd, QID_AC_VO);
+		RTMPRingCleanUp(pAd, QID_HCCA);
+		RTMPRingCleanUp(pAd, QID_MGMT);
+		RTMPRingCleanUp(pAd, QID_RX);
+
+		// 2. Send wake up command.
+		AsicSendCommandToMcu(pAd, 0x31, PowerWakeCID, 0x00, 0x02);
+		// 2-1. wait command ok.
+		AsicCheckCommanOk(pAd, PowerWakeCID);
+
+		// When PCI clock is off, don't want to service interrupt. So when back to clock on, enable interrupt.
+		NICEnableInterrupt(pAd);
+
+		// 3. Enable Tx DMA.
+		RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &DmaCfg.word);
+		DmaCfg.field.EnableTxDMA = 1;
+		RTMP_IO_WRITE32(pAd, WPDMA_GLO_CFG, DmaCfg.word);
+
+		// In Radio Off, we turn off RF clk, So now need to call ASICSwitchChannel again.
+		if (INFRA_ON(pAd) && (pAd->CommonCfg.CentralChannel != pAd->CommonCfg.Channel)
+			&& (pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth == BW_40))
+		{
+			// Must using 40MHz.
+			AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+			AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+		}
+		else
+		{
+			// Must using 20MHz.
+			AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+			AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+		}
+
+		// Clear Radio off flag
+		RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF);
+
+		// Set LED
+		RTMPSetLED(pAd, LED_RADIO_ON);
+
+        if (pAd->StaCfg.Psm == PWR_ACTIVE)
+        {
+    		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, pAd->StaCfg.BBPR3);
+        }
+	}
+	else
+	{
+		RT28xxPciAsicRadioOff(pAd, GUIRADIO_OFF, 0);
+	}
+}
+
+#endif // CONFIG_STA_SUPPORT //
+
+VOID RT28xxPciMlmeRadioOn(
+	IN PRTMP_ADAPTER pAd)
+{
+    if (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF))
+   		return;
+
+    DBGPRINT(RT_DEBUG_TRACE,("%s===>\n", __FUNCTION__));
+
+    if ((pAd->OpMode == OPMODE_AP) ||
+        ((pAd->OpMode == OPMODE_STA) && (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE))))
+    {
+    	NICResetFromError(pAd);
+
+    	RTMPRingCleanUp(pAd, QID_AC_BK);
+    	RTMPRingCleanUp(pAd, QID_AC_BE);
+    	RTMPRingCleanUp(pAd, QID_AC_VI);
+    	RTMPRingCleanUp(pAd, QID_AC_VO);
+    	RTMPRingCleanUp(pAd, QID_HCCA);
+    	RTMPRingCleanUp(pAd, QID_MGMT);
+    	RTMPRingCleanUp(pAd, QID_RX);
+
+    	// Enable Tx/Rx
+    	RTMPEnableRxTx(pAd);
+
+    	// Clear Radio off flag
+    	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF);
+
+	    // Set LED
+	    RTMPSetLED(pAd, LED_RADIO_ON);
+    }
+
+#ifdef CONFIG_STA_SUPPORT
+    if ((pAd->OpMode == OPMODE_STA) &&
+        (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE)))
+    {
+        BOOLEAN		Cancelled;
+
+    	RTMPPCIeLinkCtrlValueRestore(pAd, RESTORE_WAKEUP);
+
+        pAd->Mlme.bPsPollTimerRunning = FALSE;
+    	RTMPCancelTimer(&pAd->Mlme.PsPollTimer,	&Cancelled);
+    	RTMPCancelTimer(&pAd->Mlme.RadioOnOffTimer,	&Cancelled);
+    	RTMPSetTimer(&pAd->Mlme.RadioOnOffTimer, 10);
+    }
+#endif // CONFIG_STA_SUPPORT //
+}
+
+VOID RT28xxPciMlmeRadioOFF(
+	IN PRTMP_ADAPTER pAd)
+{
+    WPDMA_GLO_CFG_STRUC	GloCfg;
+	UINT32	i;
+
+    if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF))
+    	return;
+
+    DBGPRINT(RT_DEBUG_TRACE,("%s===>\n", __FUNCTION__));
+
+	// Set LED
+	RTMPSetLED(pAd, LED_RADIO_OFF);
+	// Set Radio off flag
+	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF);
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+    {
+    	BOOLEAN		Cancelled;
+    	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+    	{
+			RTMPCancelTimer(&pAd->MlmeAux.ScanTimer, &Cancelled);
+			RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS);
+    	}
+
+		if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE))
+        {
+            BOOLEAN Cancelled;
+            pAd->Mlme.bPsPollTimerRunning = FALSE;
+            RTMPCancelTimer(&pAd->Mlme.PsPollTimer,	&Cancelled);
+	        RTMPCancelTimer(&pAd->Mlme.RadioOnOffTimer,	&Cancelled);
+        }
+
+        // Link down first if any association exists
+        if (INFRA_ON(pAd) || ADHOC_ON(pAd))
+            LinkDown(pAd, FALSE);
+        RTMPusecDelay(10000);
+        //==========================================
+        // Clean up old bss table
+        BssTableInit(&pAd->ScanTab);
+    }
+#endif // CONFIG_STA_SUPPORT //
+
+	// Disable Tx/Rx DMA
+	RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &GloCfg.word);	   // disable DMA
+	GloCfg.field.EnableTxDMA = 0;
+	GloCfg.field.EnableRxDMA = 0;
+	RTMP_IO_WRITE32(pAd, WPDMA_GLO_CFG, GloCfg.word);	   // abort all TX rings
+
+
+	// MAC_SYS_CTRL => value = 0x0 => 40mA
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0);
+
+	// PWR_PIN_CFG => value = 0x0 => 40mA
+	RTMP_IO_WRITE32(pAd, PWR_PIN_CFG, 0);
+
+	// TX_PIN_CFG => value = 0x0 => 20mA
+	RTMP_IO_WRITE32(pAd, TX_PIN_CFG, 0);
+
+	if (pAd->CommonCfg.BBPCurrentBW == BW_40)
+	{
+		// Must using 40MHz.
+		AsicTurnOffRFClk(pAd, pAd->CommonCfg.CentralChannel);
+	}
+	else
+	{
+		// Must using 20MHz.
+		AsicTurnOffRFClk(pAd, pAd->CommonCfg.Channel);
+	}
+
+	// Waiting for DMA idle
+	i = 0;
+	do
+	{
+		RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &GloCfg.word);
+		if ((GloCfg.field.TxDMABusy == 0) && (GloCfg.field.RxDMABusy == 0))
+			break;
+
+		RTMPusecDelay(1000);
+	}while (i++ < 100);
+}
diff --git a/drivers/staging/rt2860/common/cmm_info.c b/drivers/staging/rt2860/common/cmm_info.c
new file mode 100644
index 00000000000..0aadf8af633
--- /dev/null
+++ b/drivers/staging/rt2860/common/cmm_info.c
@@ -0,0 +1,3417 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+*/
+
+#include "../rt_config.h"
+
+INT	Show_SSID_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_WirelessMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_TxBurst_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_TxPreamble_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_TxPower_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_Channel_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_BGProtection_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_RTSThreshold_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_FragThreshold_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+#ifdef DOT11_N_SUPPORT
+INT	Show_HtBw_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtMcs_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtGi_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtOpMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtExtcha_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtMpduDensity_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtBaWinSize_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtRdg_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtAmsdu_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtAutoBa_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+#endif // DOT11_N_SUPPORT //
+
+INT	Show_CountryRegion_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_CountryRegionABand_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_CountryCode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+#ifdef AGGREGATION_SUPPORT
+INT	Show_PktAggregate_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+#endif // AGGREGATION_SUPPORT //
+
+#ifdef WMM_SUPPORT
+INT	Show_WmmCapable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+#endif // WMM_SUPPORT //
+
+INT	Show_IEEE80211H_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+#ifdef CONFIG_STA_SUPPORT
+INT	Show_NetworkType_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+#endif // CONFIG_STA_SUPPORT //
+
+INT	Show_AuthMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_EncrypType_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_DefaultKeyID_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_Key1_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_Key2_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_Key3_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_Key4_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_WPAPSK_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+static struct {
+	CHAR *name;
+	INT (*show_proc)(PRTMP_ADAPTER pAdapter, PUCHAR arg);
+} *PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC, RTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC[] = {
+	{"SSID",					Show_SSID_Proc},
+	{"WirelessMode",			Show_WirelessMode_Proc},
+	{"TxBurst",					Show_TxBurst_Proc},
+	{"TxPreamble",				Show_TxPreamble_Proc},
+	{"TxPower",					Show_TxPower_Proc},
+	{"Channel",					Show_Channel_Proc},
+	{"BGProtection",			Show_BGProtection_Proc},
+	{"RTSThreshold",			Show_RTSThreshold_Proc},
+	{"FragThreshold",			Show_FragThreshold_Proc},
+#ifdef DOT11_N_SUPPORT
+	{"HtBw",					Show_HtBw_Proc},
+	{"HtMcs",					Show_HtMcs_Proc},
+	{"HtGi",					Show_HtGi_Proc},
+	{"HtOpMode",				Show_HtOpMode_Proc},
+	{"HtExtcha",				Show_HtExtcha_Proc},
+	{"HtMpduDensity",			Show_HtMpduDensity_Proc},
+	{"HtBaWinSize",		        Show_HtBaWinSize_Proc},
+	{"HtRdg",		        	Show_HtRdg_Proc},
+	{"HtAmsdu",		        	Show_HtAmsdu_Proc},
+	{"HtAutoBa",		        Show_HtAutoBa_Proc},
+#endif // DOT11_N_SUPPORT //
+	{"CountryRegion",			Show_CountryRegion_Proc},
+	{"CountryRegionABand",		Show_CountryRegionABand_Proc},
+	{"CountryCode",				Show_CountryCode_Proc},
+#ifdef AGGREGATION_SUPPORT
+	{"PktAggregate",			Show_PktAggregate_Proc},
+#endif
+
+#ifdef WMM_SUPPORT
+	{"WmmCapable",				Show_WmmCapable_Proc},
+#endif
+	{"IEEE80211H",				Show_IEEE80211H_Proc},
+#ifdef CONFIG_STA_SUPPORT
+    {"NetworkType",				Show_NetworkType_Proc},
+#endif // CONFIG_STA_SUPPORT //
+	{"AuthMode",				Show_AuthMode_Proc},
+	{"EncrypType",				Show_EncrypType_Proc},
+	{"DefaultKeyID",			Show_DefaultKeyID_Proc},
+	{"Key1",					Show_Key1_Proc},
+	{"Key2",					Show_Key2_Proc},
+	{"Key3",					Show_Key3_Proc},
+	{"Key4",					Show_Key4_Proc},
+	{"WPAPSK",					Show_WPAPSK_Proc},
+	{NULL, NULL}
+};
+
+/*
+    ==========================================================================
+    Description:
+        Get Driver version.
+
+    Return:
+    ==========================================================================
+*/
+INT Set_DriverVersion_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		DBGPRINT(RT_DEBUG_TRACE, ("Driver version-%s\n", STA_DRIVER_VERSION));
+#endif // CONFIG_STA_SUPPORT //
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Country Region.
+        This command will not work, if the field of CountryRegion in eeprom is programmed.
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_CountryRegion_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG region;
+
+	region = simple_strtol(arg, 0, 10);
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+	return -EOPNOTSUPP;
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+	// Country can be set only when EEPROM not programmed
+	if (pAd->CommonCfg.CountryRegion & 0x80)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Set_CountryRegion_Proc::parameter of CountryRegion in eeprom is programmed \n"));
+		return FALSE;
+	}
+
+	if((region >= 0) && (region <= REGION_MAXIMUM_BG_BAND))
+	{
+		pAd->CommonCfg.CountryRegion = (UCHAR) region;
+	}
+	else if (region == REGION_31_BG_BAND)
+	{
+		pAd->CommonCfg.CountryRegion = (UCHAR) region;
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Set_CountryRegion_Proc::parameters out of range\n"));
+		return FALSE;
+	}
+
+	// if set country region, driver needs to be reset
+	BuildChannelList(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_CountryRegion_Proc::(CountryRegion=%d)\n", pAd->CommonCfg.CountryRegion));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Country Region for A band.
+        This command will not work, if the field of CountryRegion in eeprom is programmed.
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_CountryRegionABand_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG region;
+
+	region = simple_strtol(arg, 0, 10);
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+	return -EOPNOTSUPP;
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+	// Country can be set only when EEPROM not programmed
+	if (pAd->CommonCfg.CountryRegionForABand & 0x80)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Set_CountryRegionABand_Proc::parameter of CountryRegion in eeprom is programmed \n"));
+		return FALSE;
+	}
+
+	if((region >= 0) && (region <= REGION_MAXIMUM_A_BAND))
+	{
+		pAd->CommonCfg.CountryRegionForABand = (UCHAR) region;
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Set_CountryRegionABand_Proc::parameters out of range\n"));
+		return FALSE;
+	}
+
+	// if set country region, driver needs to be reset
+	BuildChannelList(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_CountryRegionABand_Proc::(CountryRegion=%d)\n", pAd->CommonCfg.CountryRegionForABand));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Wireless Mode
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_WirelessMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG	WirelessMode;
+	INT		success = TRUE;
+
+	WirelessMode = simple_strtol(arg, 0, 10);
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		INT MaxPhyMode = PHY_11G;
+
+#ifdef DOT11_N_SUPPORT
+		MaxPhyMode = PHY_11N_5G;
+#endif // DOT11_N_SUPPORT //
+
+		if (WirelessMode <= MaxPhyMode)
+		{
+			RTMPSetPhyMode(pAd, WirelessMode);
+#ifdef DOT11_N_SUPPORT
+			if (WirelessMode >= PHY_11ABGN_MIXED)
+			{
+				pAd->CommonCfg.BACapability.field.AutoBA = TRUE;
+				pAd->CommonCfg.REGBACapability.field.AutoBA = TRUE;
+			}
+			else
+			{
+				pAd->CommonCfg.BACapability.field.AutoBA = FALSE;
+				pAd->CommonCfg.REGBACapability.field.AutoBA = FALSE;
+			}
+#endif // DOT11_N_SUPPORT //
+			// Set AdhocMode rates
+			if (pAd->StaCfg.BssType == BSS_ADHOC)
+			{
+				MlmeUpdateTxRates(pAd, FALSE, 0);
+				MakeIbssBeacon(pAd);           // re-build BEACON frame
+				AsicEnableIbssSync(pAd);       // copy to on-chip memory
+			}
+		}
+		else
+		{
+			success = FALSE;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	// it is needed to set SSID to take effect
+	if (success == TRUE)
+	{
+#ifdef DOT11_N_SUPPORT
+		SetCommonHT(pAd);
+#endif // DOT11_N_SUPPORT //
+		DBGPRINT(RT_DEBUG_TRACE, ("Set_WirelessMode_Proc::(=%ld)\n", WirelessMode));
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Set_WirelessMode_Proc::parameters out of range\n"));
+	}
+
+	return success;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Channel
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_Channel_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+ 	INT		success = TRUE;
+	UCHAR	Channel;
+
+	Channel = (UCHAR) simple_strtol(arg, 0, 10);
+
+	// check if this channel is valid
+	if (ChannelSanity(pAd, Channel) == TRUE)
+	{
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			pAd->CommonCfg.Channel = Channel;
+
+			if (MONITOR_ON(pAd))
+			{
+#ifdef DOT11_N_SUPPORT
+				N_ChannelCheck(pAd);
+				if (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED &&
+					pAd->CommonCfg.RegTransmitSetting.field.BW == BW_40)
+				{
+					N_SetCenCh(pAd);
+					AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+					AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+					DBGPRINT(RT_DEBUG_TRACE, ("BW_40, control_channel(%d), CentralChannel(%d) \n",
+								pAd->CommonCfg.Channel, pAd->CommonCfg.CentralChannel));
+				}
+				else
+#endif // DOT11_N_SUPPORT //
+				{
+					AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+					AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+					DBGPRINT(RT_DEBUG_TRACE, ("BW_20, Channel(%d)\n", pAd->CommonCfg.Channel));
+				}
+			}
+		}
+#endif // CONFIG_STA_SUPPORT //
+		success = TRUE;
+	}
+	else
+	{
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			success = FALSE;
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+
+	if (success == TRUE)
+		DBGPRINT(RT_DEBUG_TRACE, ("Set_Channel_Proc::(Channel=%d)\n", pAd->CommonCfg.Channel));
+
+	return success;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Short Slot Time Enable or Disable
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ShortSlot_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG ShortSlot;
+
+	ShortSlot = simple_strtol(arg, 0, 10);
+
+	if (ShortSlot == 1)
+		pAd->CommonCfg.bUseShortSlotTime = TRUE;
+	else if (ShortSlot == 0)
+		pAd->CommonCfg.bUseShortSlotTime = FALSE;
+	else
+		return FALSE;  //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_ShortSlot_Proc::(ShortSlot=%d)\n", pAd->CommonCfg.bUseShortSlotTime));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Tx power
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_TxPower_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG TxPower;
+	INT   success = FALSE;
+
+	TxPower = (ULONG) simple_strtol(arg, 0, 10);
+	if (TxPower <= 100)
+	{
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			pAd->CommonCfg.TxPowerDefault = TxPower;
+			pAd->CommonCfg.TxPowerPercentage = pAd->CommonCfg.TxPowerDefault;
+		}
+#endif // CONFIG_STA_SUPPORT //
+		success = TRUE;
+	}
+	else
+		success = FALSE;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_TxPower_Proc::(TxPowerPercentage=%ld)\n", pAd->CommonCfg.TxPowerPercentage));
+
+	return success;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set 11B/11G Protection
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_BGProtection_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	switch (simple_strtol(arg, 0, 10))
+	{
+		case 0: //AUTO
+			pAd->CommonCfg.UseBGProtection = 0;
+			break;
+		case 1: //Always On
+			pAd->CommonCfg.UseBGProtection = 1;
+			break;
+		case 2: //Always OFF
+			pAd->CommonCfg.UseBGProtection = 2;
+			break;
+		default:  //Invalid argument
+			return FALSE;
+	}
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_BGProtection_Proc::(BGProtection=%ld)\n", pAd->CommonCfg.UseBGProtection));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set TxPreamble
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_TxPreamble_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	RT_802_11_PREAMBLE	Preamble;
+
+	Preamble = simple_strtol(arg, 0, 10);
+
+
+	switch (Preamble)
+	{
+		case Rt802_11PreambleShort:
+			pAd->CommonCfg.TxPreamble = Preamble;
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				MlmeSetTxPreamble(pAd, Rt802_11PreambleShort);
+#endif // CONFIG_STA_SUPPORT //
+			break;
+		case Rt802_11PreambleLong:
+#ifdef CONFIG_STA_SUPPORT
+		case Rt802_11PreambleAuto:
+			// if user wants AUTO, initialize to LONG here, then change according to AP's
+			// capability upon association.
+#endif // CONFIG_STA_SUPPORT //
+			pAd->CommonCfg.TxPreamble = Preamble;
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				MlmeSetTxPreamble(pAd, Rt802_11PreambleLong);
+#endif // CONFIG_STA_SUPPORT //
+			break;
+		default: //Invalid argument
+			return FALSE;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_TxPreamble_Proc::(TxPreamble=%ld)\n", pAd->CommonCfg.TxPreamble));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set RTS Threshold
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_RTSThreshold_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	 NDIS_802_11_RTS_THRESHOLD           RtsThresh;
+
+	RtsThresh = simple_strtol(arg, 0, 10);
+
+	if((RtsThresh > 0) && (RtsThresh <= MAX_RTS_THRESHOLD))
+		pAd->CommonCfg.RtsThreshold  = (USHORT)RtsThresh;
+#ifdef CONFIG_STA_SUPPORT
+	else if (RtsThresh == 0)
+		pAd->CommonCfg.RtsThreshold = MAX_RTS_THRESHOLD;
+#endif // CONFIG_STA_SUPPORT //
+	else
+		return FALSE; //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_RTSThreshold_Proc::(RTSThreshold=%d)\n", pAd->CommonCfg.RtsThreshold));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Fragment Threshold
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_FragThreshold_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	 NDIS_802_11_FRAGMENTATION_THRESHOLD     FragThresh;
+
+	FragThresh = simple_strtol(arg, 0, 10);
+
+	if (FragThresh > MAX_FRAG_THRESHOLD || FragThresh < MIN_FRAG_THRESHOLD)
+	{
+		//Illegal FragThresh so we set it to default
+		pAd->CommonCfg.FragmentThreshold = MAX_FRAG_THRESHOLD;
+	}
+	else if (FragThresh % 2 == 1)
+	{
+		// The length of each fragment shall always be an even number of octets, except for the last fragment
+		// of an MSDU or MMPDU, which may be either an even or an odd number of octets.
+		pAd->CommonCfg.FragmentThreshold = (USHORT)(FragThresh - 1);
+	}
+	else
+	{
+		pAd->CommonCfg.FragmentThreshold = (USHORT)FragThresh;
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (pAd->CommonCfg.FragmentThreshold == MAX_FRAG_THRESHOLD)
+			pAd->CommonCfg.bUseZeroToDisableFragment = TRUE;
+		else
+			pAd->CommonCfg.bUseZeroToDisableFragment = FALSE;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_FragThreshold_Proc::(FragThreshold=%d)\n", pAd->CommonCfg.FragmentThreshold));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set TxBurst
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_TxBurst_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG TxBurst;
+
+	TxBurst = simple_strtol(arg, 0, 10);
+	if (TxBurst == 1)
+		pAd->CommonCfg.bEnableTxBurst = TRUE;
+	else if (TxBurst == 0)
+		pAd->CommonCfg.bEnableTxBurst = FALSE;
+	else
+		return FALSE;  //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_TxBurst_Proc::(TxBurst=%d)\n", pAd->CommonCfg.bEnableTxBurst));
+
+	return TRUE;
+}
+
+#ifdef AGGREGATION_SUPPORT
+/*
+    ==========================================================================
+    Description:
+        Set TxBurst
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_PktAggregate_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG aggre;
+
+	aggre = simple_strtol(arg, 0, 10);
+
+	if (aggre == 1)
+		pAd->CommonCfg.bAggregationCapable = TRUE;
+	else if (aggre == 0)
+		pAd->CommonCfg.bAggregationCapable = FALSE;
+	else
+		return FALSE;  //Invalid argument
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_PktAggregate_Proc::(AGGRE=%d)\n", pAd->CommonCfg.bAggregationCapable));
+
+	return TRUE;
+}
+#endif
+
+/*
+    ==========================================================================
+    Description:
+        Set IEEE80211H.
+        This parameter is 1 when needs radar detection, otherwise 0
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_IEEE80211H_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+    ULONG ieee80211h;
+
+	ieee80211h = simple_strtol(arg, 0, 10);
+
+	if (ieee80211h == 1)
+		pAd->CommonCfg.bIEEE80211H = TRUE;
+	else if (ieee80211h == 0)
+		pAd->CommonCfg.bIEEE80211H = FALSE;
+	else
+		return FALSE;  //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_IEEE80211H_Proc::(IEEE80211H=%d)\n", pAd->CommonCfg.bIEEE80211H));
+
+	return TRUE;
+}
+
+
+#ifdef DBG
+/*
+    ==========================================================================
+    Description:
+        For Debug information
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_Debug_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	DBGPRINT(RT_DEBUG_TRACE, ("==> Set_Debug_Proc *******************\n"));
+
+    if(simple_strtol(arg, 0, 10) <= RT_DEBUG_LOUD)
+        RTDebugLevel = simple_strtol(arg, 0, 10);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<== Set_Debug_Proc(RTDebugLevel = %ld)\n", RTDebugLevel));
+
+	return TRUE;
+}
+#endif
+
+INT	Show_DescInfo_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+#ifdef RT2860
+	INT i, QueIdx=0;
+	PRT28XX_RXD_STRUC pRxD;
+    PTXD_STRUC pTxD;
+	PRTMP_TX_RING	pTxRing = &pAd->TxRing[QueIdx];
+	PRTMP_MGMT_RING	pMgmtRing = &pAd->MgmtRing;
+	PRTMP_RX_RING	pRxRing = &pAd->RxRing;
+
+	for(i=0;i<TX_RING_SIZE;i++)
+	{
+	    pTxD = (PTXD_STRUC) pTxRing->Cell[i].AllocVa;
+	    printk("Desc #%d\n",i);
+	    hex_dump("Tx Descriptor", (char *)pTxD, 16);
+	    printk("pTxD->DMADONE = %x\n", pTxD->DMADONE);
+	}
+	printk("---------------------------------------------------\n");
+	for(i=0;i<MGMT_RING_SIZE;i++)
+	{
+	    pTxD = (PTXD_STRUC) pMgmtRing->Cell[i].AllocVa;
+	    printk("Desc #%d\n",i);
+	    hex_dump("Mgmt Descriptor", (char *)pTxD, 16);
+	    printk("pMgmt->DMADONE = %x\n", pTxD->DMADONE);
+	}
+	printk("---------------------------------------------------\n");
+	for(i=0;i<RX_RING_SIZE;i++)
+	{
+	    pRxD = (PRT28XX_RXD_STRUC) pRxRing->Cell[i].AllocVa;
+	    printk("Desc #%d\n",i);
+	    hex_dump("Rx Descriptor", (char *)pRxD, 16);
+		printk("pRxD->DDONE = %x\n", pRxD->DDONE);
+	}
+#endif // RT2860 //
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Reset statistics counter
+
+    Arguments:
+        pAdapter            Pointer to our adapter
+        arg
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ResetStatCounter_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	DBGPRINT(RT_DEBUG_TRACE, ("==>Set_ResetStatCounter_Proc\n"));
+
+	// add the most up-to-date h/w raw counters into software counters
+	NICUpdateRawCounters(pAd);
+
+	NdisZeroMemory(&pAd->WlanCounters, sizeof(COUNTER_802_11));
+	NdisZeroMemory(&pAd->Counters8023, sizeof(COUNTER_802_3));
+	NdisZeroMemory(&pAd->RalinkCounters, sizeof(COUNTER_RALINK));
+
+	return TRUE;
+}
+
+BOOLEAN RTMPCheckStrPrintAble(
+    IN  CHAR *pInPutStr,
+    IN  UCHAR strLen)
+{
+    UCHAR i=0;
+
+    for (i=0; i<strLen; i++)
+    {
+        if ((pInPutStr[i] < 0x21) ||
+            (pInPutStr[i] > 0x7E))
+            return FALSE;
+    }
+
+    return TRUE;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Remove WPA Key process
+
+	Arguments:
+		pAd 					Pointer to our adapter
+		pBuf							Pointer to the where the key stored
+
+	Return Value:
+		NDIS_SUCCESS					Add key successfully
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+#ifdef CONFIG_STA_SUPPORT
+VOID    RTMPSetDesiredRates(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  LONG            Rates)
+{
+    NDIS_802_11_RATES aryRates;
+
+    memset(&aryRates, 0x00, sizeof(NDIS_802_11_RATES));
+    switch (pAdapter->CommonCfg.PhyMode)
+    {
+        case PHY_11A: // A only
+            switch (Rates)
+            {
+                case 6000000: //6M
+                    aryRates[0] = 0x0c; // 6M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_0;
+                    break;
+                case 9000000: //9M
+                    aryRates[0] = 0x12; // 9M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_1;
+                    break;
+                case 12000000: //12M
+                    aryRates[0] = 0x18; // 12M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_2;
+                    break;
+                case 18000000: //18M
+                    aryRates[0] = 0x24; // 18M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_3;
+                    break;
+                case 24000000: //24M
+                    aryRates[0] = 0x30; // 24M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_4;
+                    break;
+                case 36000000: //36M
+                    aryRates[0] = 0x48; // 36M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_5;
+                    break;
+                case 48000000: //48M
+                    aryRates[0] = 0x60; // 48M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_6;
+                    break;
+                case 54000000: //54M
+                    aryRates[0] = 0x6c; // 54M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_7;
+                    break;
+                case -1: //Auto
+                default:
+                    aryRates[0] = 0x6c; // 54Mbps
+                    aryRates[1] = 0x60; // 48Mbps
+                    aryRates[2] = 0x48; // 36Mbps
+                    aryRates[3] = 0x30; // 24Mbps
+                    aryRates[4] = 0x24; // 18M
+                    aryRates[5] = 0x18; // 12M
+                    aryRates[6] = 0x12; // 9M
+                    aryRates[7] = 0x0c; // 6M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_AUTO;
+                    break;
+            }
+            break;
+        case PHY_11BG_MIXED: // B/G Mixed
+        case PHY_11B: // B only
+        case PHY_11ABG_MIXED: // A/B/G Mixed
+        default:
+            switch (Rates)
+            {
+                case 1000000: //1M
+                    aryRates[0] = 0x02;
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_0;
+                    break;
+                case 2000000: //2M
+                    aryRates[0] = 0x04;
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_1;
+                    break;
+                case 5000000: //5.5M
+                    aryRates[0] = 0x0b; // 5.5M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_2;
+                    break;
+                case 11000000: //11M
+                    aryRates[0] = 0x16; // 11M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_3;
+                    break;
+                case 6000000: //6M
+                    aryRates[0] = 0x0c; // 6M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_0;
+                    break;
+                case 9000000: //9M
+                    aryRates[0] = 0x12; // 9M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_1;
+                    break;
+                case 12000000: //12M
+                    aryRates[0] = 0x18; // 12M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_2;
+                    break;
+                case 18000000: //18M
+                    aryRates[0] = 0x24; // 18M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_3;
+                    break;
+                case 24000000: //24M
+                    aryRates[0] = 0x30; // 24M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_4;
+                    break;
+                case 36000000: //36M
+                    aryRates[0] = 0x48; // 36M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_5;
+                    break;
+                case 48000000: //48M
+                    aryRates[0] = 0x60; // 48M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_6;
+                    break;
+                case 54000000: //54M
+                    aryRates[0] = 0x6c; // 54M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_7;
+                    break;
+                case -1: //Auto
+                default:
+                    if (pAdapter->CommonCfg.PhyMode == PHY_11B)
+                    { //B Only
+                        aryRates[0] = 0x16; // 11Mbps
+                        aryRates[1] = 0x0b; // 5.5Mbps
+                        aryRates[2] = 0x04; // 2Mbps
+                        aryRates[3] = 0x02; // 1Mbps
+                    }
+                    else
+                    { //(B/G) Mixed or (A/B/G) Mixed
+                        aryRates[0] = 0x6c; // 54Mbps
+                        aryRates[1] = 0x60; // 48Mbps
+                        aryRates[2] = 0x48; // 36Mbps
+                        aryRates[3] = 0x30; // 24Mbps
+                        aryRates[4] = 0x16; // 11Mbps
+                        aryRates[5] = 0x0b; // 5.5Mbps
+                        aryRates[6] = 0x04; // 2Mbps
+                        aryRates[7] = 0x02; // 1Mbps
+                    }
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_AUTO;
+                    break;
+            }
+            break;
+    }
+
+    NdisZeroMemory(pAdapter->CommonCfg.DesireRate, MAX_LEN_OF_SUPPORTED_RATES);
+    NdisMoveMemory(pAdapter->CommonCfg.DesireRate, &aryRates, sizeof(NDIS_802_11_RATES));
+    DBGPRINT(RT_DEBUG_TRACE, (" RTMPSetDesiredRates (%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x)\n",
+        pAdapter->CommonCfg.DesireRate[0],pAdapter->CommonCfg.DesireRate[1],
+        pAdapter->CommonCfg.DesireRate[2],pAdapter->CommonCfg.DesireRate[3],
+        pAdapter->CommonCfg.DesireRate[4],pAdapter->CommonCfg.DesireRate[5],
+        pAdapter->CommonCfg.DesireRate[6],pAdapter->CommonCfg.DesireRate[7] ));
+    // Changing DesiredRate may affect the MAX TX rate we used to TX frames out
+    MlmeUpdateTxRates(pAdapter, FALSE, 0);
+}
+
+NDIS_STATUS RTMPWPARemoveKeyProc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PVOID			pBuf)
+{
+	PNDIS_802_11_REMOVE_KEY pKey;
+	ULONG					KeyIdx;
+	NDIS_STATUS 			Status = NDIS_STATUS_FAILURE;
+	BOOLEAN 	bTxKey; 		// Set the key as transmit key
+	BOOLEAN 	bPairwise;		// Indicate the key is pairwise key
+	BOOLEAN 	bKeyRSC;		// indicate the receive  SC set by KeyRSC value.
+								// Otherwise, it will set by the NIC.
+	BOOLEAN 	bAuthenticator; // indicate key is set by authenticator.
+	INT 		i;
+
+	DBGPRINT(RT_DEBUG_TRACE,("---> RTMPWPARemoveKeyProc\n"));
+
+	pKey = (PNDIS_802_11_REMOVE_KEY) pBuf;
+	KeyIdx = pKey->KeyIndex & 0xff;
+	// Bit 31 of Add-key, Tx Key
+	bTxKey		   = (pKey->KeyIndex & 0x80000000) ? TRUE : FALSE;
+	// Bit 30 of Add-key PairwiseKey
+	bPairwise	   = (pKey->KeyIndex & 0x40000000) ? TRUE : FALSE;
+	// Bit 29 of Add-key KeyRSC
+	bKeyRSC 	   = (pKey->KeyIndex & 0x20000000) ? TRUE : FALSE;
+	// Bit 28 of Add-key Authenticator
+	bAuthenticator = (pKey->KeyIndex & 0x10000000) ? TRUE : FALSE;
+
+	// 1. If bTx is TRUE, return failure information
+	if (bTxKey == TRUE)
+		return(NDIS_STATUS_INVALID_DATA);
+
+	// 2. Check Pairwise Key
+	if (bPairwise)
+	{
+		// a. If BSSID is broadcast, remove all pairwise keys.
+		// b. If not broadcast, remove the pairwise specified by BSSID
+		for (i = 0; i < SHARE_KEY_NUM; i++)
+		{
+			if (MAC_ADDR_EQUAL(pAd->SharedKey[BSS0][i].BssId, pKey->BSSID))
+			{
+				DBGPRINT(RT_DEBUG_TRACE,("RTMPWPARemoveKeyProc(KeyIdx=%d)\n", i));
+				pAd->SharedKey[BSS0][i].KeyLen = 0;
+				pAd->SharedKey[BSS0][i].CipherAlg = CIPHER_NONE;
+				AsicRemoveSharedKeyEntry(pAd, BSS0, (UCHAR)i);
+				Status = NDIS_STATUS_SUCCESS;
+				break;
+			}
+		}
+	}
+	// 3. Group Key
+	else
+	{
+		// a. If BSSID is broadcast, remove all group keys indexed
+		// b. If BSSID matched, delete the group key indexed.
+		DBGPRINT(RT_DEBUG_TRACE,("RTMPWPARemoveKeyProc(KeyIdx=%ld)\n", KeyIdx));
+		pAd->SharedKey[BSS0][KeyIdx].KeyLen = 0;
+		pAd->SharedKey[BSS0][KeyIdx].CipherAlg = CIPHER_NONE;
+		AsicRemoveSharedKeyEntry(pAd, BSS0, (UCHAR)KeyIdx);
+		Status = NDIS_STATUS_SUCCESS;
+	}
+
+	return (Status);
+}
+#endif // CONFIG_STA_SUPPORT //
+
+
+#ifdef CONFIG_STA_SUPPORT
+/*
+	========================================================================
+
+	Routine Description:
+		Remove All WPA Keys
+
+	Arguments:
+		pAd 					Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPWPARemoveAllKeys(
+	IN	PRTMP_ADAPTER	pAd)
+{
+
+	UCHAR 	i;
+
+	DBGPRINT(RT_DEBUG_TRACE,("RTMPWPARemoveAllKeys(AuthMode=%d, WepStatus=%d)\n", pAd->StaCfg.AuthMode, pAd->StaCfg.WepStatus));
+
+	// For WEP/CKIP, there is no need to remove it, since WinXP won't set it again after
+	// Link up. And it will be replaced if user changed it.
+	if (pAd->StaCfg.AuthMode < Ndis802_11AuthModeWPA)
+		return;
+
+	// For WPA-None, there is no need to remove it, since WinXP won't set it again after
+	// Link up. And it will be replaced if user changed it.
+	if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+		return;
+
+	// set BSSID wcid entry of the Pair-wise Key table as no-security mode
+	AsicRemovePairwiseKeyEntry(pAd, BSS0, BSSID_WCID);
+
+	// set all shared key mode as no-security.
+	for (i = 0; i < SHARE_KEY_NUM; i++)
+    {
+		DBGPRINT(RT_DEBUG_TRACE,("remove %s key #%d\n", CipherName[pAd->SharedKey[BSS0][i].CipherAlg], i));
+		NdisZeroMemory(&pAd->SharedKey[BSS0][i], sizeof(CIPHER_KEY));
+
+		AsicRemoveSharedKeyEntry(pAd, BSS0, i);
+	}
+
+}
+#endif // CONFIG_STA_SUPPORT //
+
+/*
+	========================================================================
+	Routine Description:
+		Change NIC PHY mode. Re-association may be necessary. possible settings
+		include - PHY_11B, PHY_11BG_MIXED, PHY_11A, and PHY_11ABG_MIXED
+
+	Arguments:
+		pAd - Pointer to our adapter
+		phymode  -
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	========================================================================
+*/
+VOID	RTMPSetPhyMode(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	ULONG phymode)
+{
+	INT i;
+	// the selected phymode must be supported by the RF IC encoded in E2PROM
+
+	// if no change, do nothing
+	/* bug fix
+	if (pAd->CommonCfg.PhyMode == phymode)
+		return;
+    */
+	pAd->CommonCfg.PhyMode = (UCHAR)phymode;
+
+	DBGPRINT(RT_DEBUG_TRACE,("RTMPSetPhyMode : PhyMode=%d, channel=%d \n", pAd->CommonCfg.PhyMode, pAd->CommonCfg.Channel));
+#ifdef EXT_BUILD_CHANNEL_LIST
+	BuildChannelListEx(pAd);
+#else
+	BuildChannelList(pAd);
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+	// sanity check user setting
+	for (i = 0; i < pAd->ChannelListNum; i++)
+	{
+		if (pAd->CommonCfg.Channel == pAd->ChannelList[i].Channel)
+			break;
+	}
+
+	if (i == pAd->ChannelListNum)
+	{
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			pAd->CommonCfg.Channel = FirstChannel(pAd);
+#endif // CONFIG_STA_SUPPORT //
+		DBGPRINT(RT_DEBUG_ERROR, ("RTMPSetPhyMode: channel is out of range, use first channel=%d \n", pAd->CommonCfg.Channel));
+	}
+
+	NdisZeroMemory(pAd->CommonCfg.SupRate, MAX_LEN_OF_SUPPORTED_RATES);
+	NdisZeroMemory(pAd->CommonCfg.ExtRate, MAX_LEN_OF_SUPPORTED_RATES);
+	NdisZeroMemory(pAd->CommonCfg.DesireRate, MAX_LEN_OF_SUPPORTED_RATES);
+	switch (phymode) {
+		case PHY_11B:
+			pAd->CommonCfg.SupRate[0]  = 0x82;	  // 1 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[1]  = 0x84;	  // 2 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[2]  = 0x8B;	  // 5.5 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[3]  = 0x96;	  // 11 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRateLen  = 4;
+			pAd->CommonCfg.ExtRateLen  = 0;
+			pAd->CommonCfg.DesireRate[0]  = 2;	   // 1 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[1]  = 4;	   // 2 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[2]  = 11;    // 5.5 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[3]  = 22;    // 11 mbps, in units of 0.5 Mbps
+			//pAd->CommonCfg.HTPhyMode.field.MODE = MODE_CCK; // This MODE is only FYI. not use
+			break;
+
+		case PHY_11G:
+		case PHY_11BG_MIXED:
+		case PHY_11ABG_MIXED:
+#ifdef DOT11_N_SUPPORT
+		case PHY_11N_2_4G:
+		case PHY_11ABGN_MIXED:
+		case PHY_11BGN_MIXED:
+		case PHY_11GN_MIXED:
+#endif // DOT11_N_SUPPORT //
+			pAd->CommonCfg.SupRate[0]  = 0x82;	  // 1 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[1]  = 0x84;	  // 2 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[2]  = 0x8B;	  // 5.5 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[3]  = 0x96;	  // 11 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[4]  = 0x12;	  // 9 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRate[5]  = 0x24;	  // 18 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRate[6]  = 0x48;	  // 36 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRate[7]  = 0x6c;	  // 54 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRateLen  = 8;
+			pAd->CommonCfg.ExtRate[0]  = 0x0C;	  // 6 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.ExtRate[1]  = 0x18;	  // 12 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.ExtRate[2]  = 0x30;	  // 24 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.ExtRate[3]  = 0x60;	  // 48 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.ExtRateLen  = 4;
+			pAd->CommonCfg.DesireRate[0]  = 2;	   // 1 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[1]  = 4;	   // 2 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[2]  = 11;    // 5.5 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[3]  = 22;    // 11 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[4]  = 12;    // 6 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[5]  = 18;    // 9 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[6]  = 24;    // 12 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[7]  = 36;    // 18 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[8]  = 48;    // 24 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[9]  = 72;    // 36 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[10] = 96;    // 48 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[11] = 108;   // 54 mbps, in units of 0.5 Mbps
+			break;
+
+		case PHY_11A:
+#ifdef DOT11_N_SUPPORT
+		case PHY_11AN_MIXED:
+		case PHY_11AGN_MIXED:
+		case PHY_11N_5G:
+#endif // DOT11_N_SUPPORT //
+			pAd->CommonCfg.SupRate[0]  = 0x8C;	  // 6 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[1]  = 0x12;	  // 9 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRate[2]  = 0x98;	  // 12 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[3]  = 0x24;	  // 18 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRate[4]  = 0xb0;	  // 24 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[5]  = 0x48;	  // 36 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRate[6]  = 0x60;	  // 48 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRate[7]  = 0x6c;	  // 54 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRateLen  = 8;
+			pAd->CommonCfg.ExtRateLen  = 0;
+			pAd->CommonCfg.DesireRate[0]  = 12;    // 6 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[1]  = 18;    // 9 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[2]  = 24;    // 12 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[3]  = 36;    // 18 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[4]  = 48;    // 24 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[5]  = 72;    // 36 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[6]  = 96;    // 48 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[7]  = 108;   // 54 mbps, in units of 0.5 Mbps
+			//pAd->CommonCfg.HTPhyMode.field.MODE = MODE_OFDM; // This MODE is only FYI. not use
+			break;
+
+		default:
+			break;
+	}
+
+
+	pAd->CommonCfg.BandState = UNKNOWN_BAND;
+}
+
+
+#ifdef DOT11_N_SUPPORT
+/*
+	========================================================================
+	Routine Description:
+		Caller ensures we has 802.11n support.
+		Calls at setting HT from AP/STASetinformation
+
+	Arguments:
+		pAd - Pointer to our adapter
+		phymode  -
+
+	========================================================================
+*/
+VOID	RTMPSetHT(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	OID_SET_HT_PHYMODE *pHTPhyMode)
+{
+	//ULONG	*pmcs;
+	UINT32	Value = 0;
+	UCHAR	BBPValue = 0;
+	UCHAR	BBP3Value = 0;
+	UCHAR	RxStream = pAd->CommonCfg.RxStream;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPSetHT : HT_mode(%d), ExtOffset(%d), MCS(%d), BW(%d), STBC(%d), SHORTGI(%d)\n",
+										pHTPhyMode->HtMode, pHTPhyMode->ExtOffset,
+										pHTPhyMode->MCS, pHTPhyMode->BW,
+										pHTPhyMode->STBC, pHTPhyMode->SHORTGI));
+
+	// Don't zero supportedHyPhy structure.
+	RTMPZeroMemory(&pAd->CommonCfg.HtCapability, sizeof(pAd->CommonCfg.HtCapability));
+	RTMPZeroMemory(&pAd->CommonCfg.AddHTInfo, sizeof(pAd->CommonCfg.AddHTInfo));
+	RTMPZeroMemory(&pAd->CommonCfg.NewExtChanOffset, sizeof(pAd->CommonCfg.NewExtChanOffset));
+	RTMPZeroMemory(&pAd->CommonCfg.DesiredHtPhy, sizeof(pAd->CommonCfg.DesiredHtPhy));
+
+   	if (pAd->CommonCfg.bRdg)
+	{
+		pAd->CommonCfg.HtCapability.ExtHtCapInfo.PlusHTC = 1;
+		pAd->CommonCfg.HtCapability.ExtHtCapInfo.RDGSupport = 1;
+	}
+	else
+	{
+		pAd->CommonCfg.HtCapability.ExtHtCapInfo.PlusHTC = 0;
+		pAd->CommonCfg.HtCapability.ExtHtCapInfo.RDGSupport = 0;
+	}
+
+	pAd->CommonCfg.HtCapability.HtCapParm.MaxRAmpduFactor = 3;
+	pAd->CommonCfg.DesiredHtPhy.MaxRAmpduFactor = 3;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPSetHT : RxBAWinLimit = %d\n", pAd->CommonCfg.BACapability.field.RxBAWinLimit));
+
+	// Mimo power save, A-MSDU size,
+	pAd->CommonCfg.DesiredHtPhy.AmsduEnable = (USHORT)pAd->CommonCfg.BACapability.field.AmsduEnable;
+	pAd->CommonCfg.DesiredHtPhy.AmsduSize = (UCHAR)pAd->CommonCfg.BACapability.field.AmsduSize;
+	pAd->CommonCfg.DesiredHtPhy.MimoPs = (UCHAR)pAd->CommonCfg.BACapability.field.MMPSmode;
+	pAd->CommonCfg.DesiredHtPhy.MpduDensity = (UCHAR)pAd->CommonCfg.BACapability.field.MpduDensity;
+
+	pAd->CommonCfg.HtCapability.HtCapInfo.AMsduSize = (USHORT)pAd->CommonCfg.BACapability.field.AmsduSize;
+	pAd->CommonCfg.HtCapability.HtCapInfo.MimoPs = (USHORT)pAd->CommonCfg.BACapability.field.MMPSmode;
+	pAd->CommonCfg.HtCapability.HtCapParm.MpduDensity = (UCHAR)pAd->CommonCfg.BACapability.field.MpduDensity;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPSetHT : AMsduSize = %d, MimoPs = %d, MpduDensity = %d, MaxRAmpduFactor = %d\n",
+													pAd->CommonCfg.DesiredHtPhy.AmsduSize,
+													pAd->CommonCfg.DesiredHtPhy.MimoPs,
+													pAd->CommonCfg.DesiredHtPhy.MpduDensity,
+													pAd->CommonCfg.DesiredHtPhy.MaxRAmpduFactor));
+
+	if(pHTPhyMode->HtMode == HTMODE_GF)
+	{
+		pAd->CommonCfg.HtCapability.HtCapInfo.GF = 1;
+		pAd->CommonCfg.DesiredHtPhy.GF = 1;
+	}
+	else
+		pAd->CommonCfg.DesiredHtPhy.GF = 0;
+
+	// Decide Rx MCSSet
+	switch (RxStream)
+	{
+		case 1:
+			pAd->CommonCfg.HtCapability.MCSSet[0] =  0xff;
+			pAd->CommonCfg.HtCapability.MCSSet[1] =  0x00;
+			break;
+
+		case 2:
+			pAd->CommonCfg.HtCapability.MCSSet[0] =  0xff;
+			pAd->CommonCfg.HtCapability.MCSSet[1] =  0xff;
+			break;
+
+		case 3: // 3*3
+			pAd->CommonCfg.HtCapability.MCSSet[0] =  0xff;
+			pAd->CommonCfg.HtCapability.MCSSet[1] =  0xff;
+			pAd->CommonCfg.HtCapability.MCSSet[2] =  0xff;
+			break;
+	}
+
+	if (pAd->CommonCfg.bForty_Mhz_Intolerant && (pAd->CommonCfg.Channel <= 14) && (pHTPhyMode->BW == BW_40) )
+	{
+		pHTPhyMode->BW = BW_20;
+		pAd->CommonCfg.HtCapability.HtCapInfo.Forty_Mhz_Intolerant = 1;
+	}
+
+	if(pHTPhyMode->BW == BW_40)
+	{
+		pAd->CommonCfg.HtCapability.MCSSet[4] = 0x1; // MCS 32
+		pAd->CommonCfg.HtCapability.HtCapInfo.ChannelWidth = 1;
+		if (pAd->CommonCfg.Channel <= 14)
+			pAd->CommonCfg.HtCapability.HtCapInfo.CCKmodein40 = 1;
+
+		pAd->CommonCfg.DesiredHtPhy.ChannelWidth = 1;
+		pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth = 1;
+		pAd->CommonCfg.AddHTInfo.AddHtInfo.ExtChanOffset = (pHTPhyMode->ExtOffset == EXTCHA_BELOW)? (EXTCHA_BELOW): EXTCHA_ABOVE;
+		// Set Regsiter for extension channel position.
+		RTMP_IO_READ32(pAd, TX_BAND_CFG, &Value);
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BBP3Value);
+		if ((pHTPhyMode->ExtOffset == EXTCHA_BELOW))
+		{
+			Value |= 0x1;
+			BBP3Value |= (0x20);
+			RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Value);
+		}
+		else if ((pHTPhyMode->ExtOffset == EXTCHA_ABOVE))
+		{
+			Value &= 0xfe;
+			BBP3Value &= (~0x20);
+			RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Value);
+		}
+
+		// Turn on BBP 40MHz mode now only as AP .
+		// Sta can turn on BBP 40MHz after connection with 40MHz AP. Sta only broadcast 40MHz capability before connection.
+		if ((pAd->OpMode == OPMODE_AP) || INFRA_ON(pAd) || ADHOC_ON(pAd)
+			)
+		{
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+			BBPValue &= (~0x18);
+			BBPValue |= 0x10;
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BBP3Value);
+			pAd->CommonCfg.BBPCurrentBW = BW_40;
+		}
+	}
+	else
+	{
+		pAd->CommonCfg.HtCapability.HtCapInfo.ChannelWidth = 0;
+		pAd->CommonCfg.DesiredHtPhy.ChannelWidth = 0;
+		pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth = 0;
+		pAd->CommonCfg.AddHTInfo.AddHtInfo.ExtChanOffset = EXTCHA_NONE;
+		pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel;
+		// Turn on BBP 20MHz mode by request here.
+		{
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+			BBPValue &= (~0x18);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+			pAd->CommonCfg.BBPCurrentBW = BW_20;
+		}
+	}
+
+	if(pHTPhyMode->STBC == STBC_USE)
+	{
+		pAd->CommonCfg.HtCapability.HtCapInfo.TxSTBC = 1;
+		pAd->CommonCfg.DesiredHtPhy.TxSTBC = 1;
+		pAd->CommonCfg.HtCapability.HtCapInfo.RxSTBC = 1;
+		pAd->CommonCfg.DesiredHtPhy.RxSTBC = 1;
+	}
+	else
+	{
+		pAd->CommonCfg.DesiredHtPhy.TxSTBC = 0;
+		pAd->CommonCfg.DesiredHtPhy.RxSTBC = 0;
+	}
+
+
+	if(pHTPhyMode->SHORTGI == GI_400)
+	{
+		pAd->CommonCfg.HtCapability.HtCapInfo.ShortGIfor20 = 1;
+		pAd->CommonCfg.HtCapability.HtCapInfo.ShortGIfor40 = 1;
+		pAd->CommonCfg.DesiredHtPhy.ShortGIfor20 = 1;
+		pAd->CommonCfg.DesiredHtPhy.ShortGIfor40 = 1;
+	}
+	else
+	{
+		pAd->CommonCfg.HtCapability.HtCapInfo.ShortGIfor20 = 0;
+		pAd->CommonCfg.HtCapability.HtCapInfo.ShortGIfor40 = 0;
+		pAd->CommonCfg.DesiredHtPhy.ShortGIfor20 = 0;
+		pAd->CommonCfg.DesiredHtPhy.ShortGIfor40 = 0;
+	}
+
+	// We support link adaptation for unsolicit MCS feedback, set to 2.
+	pAd->CommonCfg.HtCapability.ExtHtCapInfo.MCSFeedback = MCSFBK_NONE; //MCSFBK_UNSOLICIT;
+	pAd->CommonCfg.AddHTInfo.ControlChan = pAd->CommonCfg.Channel;
+	// 1, the extension channel above the control channel.
+
+	// EDCA parameters used for AP's own transmission
+	if (pAd->CommonCfg.APEdcaParm.bValid == FALSE)
+	{
+		pAd->CommonCfg.APEdcaParm.bValid = TRUE;
+		pAd->CommonCfg.APEdcaParm.Aifsn[0] = 3;
+		pAd->CommonCfg.APEdcaParm.Aifsn[1] = 7;
+		pAd->CommonCfg.APEdcaParm.Aifsn[2] = 1;
+		pAd->CommonCfg.APEdcaParm.Aifsn[3] = 1;
+
+		pAd->CommonCfg.APEdcaParm.Cwmin[0] = 4;
+		pAd->CommonCfg.APEdcaParm.Cwmin[1] = 4;
+		pAd->CommonCfg.APEdcaParm.Cwmin[2] = 3;
+		pAd->CommonCfg.APEdcaParm.Cwmin[3] = 2;
+
+		pAd->CommonCfg.APEdcaParm.Cwmax[0] = 6;
+		pAd->CommonCfg.APEdcaParm.Cwmax[1] = 10;
+		pAd->CommonCfg.APEdcaParm.Cwmax[2] = 4;
+		pAd->CommonCfg.APEdcaParm.Cwmax[3] = 3;
+
+		pAd->CommonCfg.APEdcaParm.Txop[0]  = 0;
+		pAd->CommonCfg.APEdcaParm.Txop[1]  = 0;
+		pAd->CommonCfg.APEdcaParm.Txop[2]  = 94;
+		pAd->CommonCfg.APEdcaParm.Txop[3]  = 47;
+	}
+	AsicSetEdcaParm(pAd, &pAd->CommonCfg.APEdcaParm);
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		RTMPSetIndividualHT(pAd, 0);
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+}
+
+/*
+	========================================================================
+	Routine Description:
+		Caller ensures we has 802.11n support.
+		Calls at setting HT from AP/STASetinformation
+
+	Arguments:
+		pAd - Pointer to our adapter
+		phymode  -
+
+	========================================================================
+*/
+VOID	RTMPSetIndividualHT(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	UCHAR				apidx)
+{
+	PRT_HT_PHY_INFO		pDesired_ht_phy = NULL;
+	UCHAR	TxStream = pAd->CommonCfg.TxStream;
+	UCHAR	DesiredMcs	= MCS_AUTO;
+
+	do
+	{
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			pDesired_ht_phy = &pAd->StaCfg.DesiredHtPhyInfo;
+			DesiredMcs = pAd->StaCfg.DesiredTransmitSetting.field.MCS;
+			//pAd->StaCfg.bAutoTxRateSwitch = (DesiredMcs == MCS_AUTO) ? TRUE : FALSE;
+				break;
+		}
+#endif // CONFIG_STA_SUPPORT //
+	} while (FALSE);
+
+	if (pDesired_ht_phy == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("RTMPSetIndividualHT: invalid apidx(%d)\n", apidx));
+		return;
+	}
+	RTMPZeroMemory(pDesired_ht_phy, sizeof(RT_HT_PHY_INFO));
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPSetIndividualHT : Desired MCS = %d\n", DesiredMcs));
+	// Check the validity of MCS
+	if ((TxStream == 1) && ((DesiredMcs >= MCS_8) && (DesiredMcs <= MCS_15)))
+	{
+		DBGPRINT(RT_DEBUG_WARN, ("RTMPSetIndividualHT: MCS(%d) is invalid in 1S, reset it as MCS_7\n", DesiredMcs));
+		DesiredMcs = MCS_7;
+	}
+
+	if ((pAd->CommonCfg.DesiredHtPhy.ChannelWidth == BW_20) && (DesiredMcs == MCS_32))
+	{
+		DBGPRINT(RT_DEBUG_WARN, ("RTMPSetIndividualHT: MCS_32 is only supported in 40-MHz, reset it as MCS_0\n"));
+		DesiredMcs = MCS_0;
+	}
+
+	pDesired_ht_phy->bHtEnable = TRUE;
+
+	// Decide desired Tx MCS
+	switch (TxStream)
+	{
+		case 1:
+			if (DesiredMcs == MCS_AUTO)
+			{
+				pDesired_ht_phy->MCSSet[0]= 0xff;
+				pDesired_ht_phy->MCSSet[1]= 0x00;
+			}
+			else if (DesiredMcs <= MCS_7)
+			{
+				pDesired_ht_phy->MCSSet[0]= 1<<DesiredMcs;
+				pDesired_ht_phy->MCSSet[1]= 0x00;
+			}
+			break;
+
+		case 2:
+			if (DesiredMcs == MCS_AUTO)
+			{
+				pDesired_ht_phy->MCSSet[0]= 0xff;
+				pDesired_ht_phy->MCSSet[1]= 0xff;
+			}
+			else if (DesiredMcs <= MCS_15)
+			{
+				ULONG mode;
+
+				mode = DesiredMcs / 8;
+				if (mode < 2)
+					pDesired_ht_phy->MCSSet[mode] = (1 << (DesiredMcs - mode * 8));
+			}
+			break;
+
+		case 3: // 3*3
+			if (DesiredMcs == MCS_AUTO)
+			{
+				/* MCS0 ~ MCS23, 3 bytes */
+				pDesired_ht_phy->MCSSet[0]= 0xff;
+				pDesired_ht_phy->MCSSet[1]= 0xff;
+				pDesired_ht_phy->MCSSet[2]= 0xff;
+			}
+			else if (DesiredMcs <= MCS_23)
+			{
+				ULONG mode;
+
+				mode = DesiredMcs / 8;
+				if (mode < 3)
+					pDesired_ht_phy->MCSSet[mode] = (1 << (DesiredMcs - mode * 8));
+			}
+			break;
+	}
+
+	if(pAd->CommonCfg.DesiredHtPhy.ChannelWidth == BW_40)
+	{
+		if (DesiredMcs == MCS_AUTO || DesiredMcs == MCS_32)
+			pDesired_ht_phy->MCSSet[4] = 0x1;
+	}
+
+	// update HT Rate setting
+    if (pAd->OpMode == OPMODE_STA)
+        MlmeUpdateHtTxRates(pAd, BSS0);
+    else
+	    MlmeUpdateHtTxRates(pAd, apidx);
+}
+
+
+/*
+	========================================================================
+	Routine Description:
+		Update HT IE from our capability.
+
+	Arguments:
+		Send all HT IE in beacon/probe rsp/assoc rsp/action frame.
+
+
+	========================================================================
+*/
+VOID	RTMPUpdateHTIE(
+	IN	RT_HT_CAPABILITY	*pRtHt,
+	IN		UCHAR				*pMcsSet,
+	OUT		HT_CAPABILITY_IE *pHtCapability,
+	OUT		ADD_HT_INFO_IE		*pAddHtInfo)
+{
+	RTMPZeroMemory(pHtCapability, sizeof(HT_CAPABILITY_IE));
+	RTMPZeroMemory(pAddHtInfo, sizeof(ADD_HT_INFO_IE));
+
+		pHtCapability->HtCapInfo.ChannelWidth = pRtHt->ChannelWidth;
+		pHtCapability->HtCapInfo.MimoPs = pRtHt->MimoPs;
+		pHtCapability->HtCapInfo.GF = pRtHt->GF;
+		pHtCapability->HtCapInfo.ShortGIfor20 = pRtHt->ShortGIfor20;
+		pHtCapability->HtCapInfo.ShortGIfor40 = pRtHt->ShortGIfor40;
+		pHtCapability->HtCapInfo.TxSTBC = pRtHt->TxSTBC;
+		pHtCapability->HtCapInfo.RxSTBC = pRtHt->RxSTBC;
+		pHtCapability->HtCapInfo.AMsduSize = pRtHt->AmsduSize;
+		pHtCapability->HtCapParm.MaxRAmpduFactor = pRtHt->MaxRAmpduFactor;
+		pHtCapability->HtCapParm.MpduDensity = pRtHt->MpduDensity;
+
+		pAddHtInfo->AddHtInfo.ExtChanOffset = pRtHt->ExtChanOffset ;
+		pAddHtInfo->AddHtInfo.RecomWidth = pRtHt->RecomWidth;
+		pAddHtInfo->AddHtInfo2.OperaionMode = pRtHt->OperaionMode;
+		pAddHtInfo->AddHtInfo2.NonGfPresent = pRtHt->NonGfPresent;
+		RTMPMoveMemory(pAddHtInfo->MCSSet, /*pRtHt->MCSSet*/pMcsSet, 4); // rt2860 only support MCS max=32, no need to copy all 16 uchar.
+
+        DBGPRINT(RT_DEBUG_TRACE,("RTMPUpdateHTIE <== \n"));
+}
+#endif // DOT11_N_SUPPORT //
+
+/*
+	========================================================================
+	Description:
+		Add Client security information into ASIC WCID table and IVEIV table.
+    Return:
+	========================================================================
+*/
+VOID	RTMPAddWcidAttributeEntry(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			BssIdx,
+	IN 	UCHAR		 	KeyIdx,
+	IN 	UCHAR		 	CipherAlg,
+	IN 	MAC_TABLE_ENTRY *pEntry)
+{
+	UINT32		WCIDAttri = 0;
+	USHORT		offset;
+	UCHAR		IVEIV = 0;
+	USHORT		Wcid = 0;
+
+	{
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			if (BssIdx > BSS0)
+			{
+				DBGPRINT(RT_DEBUG_ERROR, ("RTMPAddWcidAttributeEntry: The BSS-index(%d) is out of range for Infra link. \n", BssIdx));
+				return;
+			}
+
+			// 1.	In ADHOC mode, the AID is wcid number. And NO mesh link exists.
+			// 2.	In Infra mode, the AID:1 MUST be wcid of infra STA.
+			//					   the AID:2~ assign to mesh link entry.
+			if (pEntry && ADHOC_ON(pAd))
+				Wcid = pEntry->Aid;
+			else if (pEntry && INFRA_ON(pAd))
+			{
+#ifdef QOS_DLS_SUPPORT
+				if (pEntry->ValidAsDls == TRUE)
+					Wcid = pEntry->Aid;
+				else
+#endif // QOS_DLS_SUPPORT //
+				Wcid = BSSID_WCID;
+			}
+			else
+				Wcid = MCAST_WCID;
+		}
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+	// Update WCID attribute table
+	offset = MAC_WCID_ATTRIBUTE_BASE + (Wcid * HW_WCID_ATTRI_SIZE);
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (pEntry && pEntry->ValidAsMesh)
+			WCIDAttri = (CipherAlg<<1) | PAIRWISEKEYTABLE;
+#ifdef QOS_DLS_SUPPORT
+		else if ((pEntry) && (pEntry->ValidAsDls) &&
+					((CipherAlg == CIPHER_TKIP) ||
+				 	(CipherAlg == CIPHER_TKIP_NO_MIC) ||
+					(CipherAlg == CIPHER_AES) ||
+				 	(CipherAlg == CIPHER_NONE)))
+			WCIDAttri = (CipherAlg<<1) | PAIRWISEKEYTABLE;
+#endif // QOS_DLS_SUPPORT //
+		else
+			WCIDAttri = (CipherAlg<<1) | SHAREDKEYTABLE;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	RTMP_IO_WRITE32(pAd, offset, WCIDAttri);
+
+
+	// Update IV/EIV table
+	offset = MAC_IVEIV_TABLE_BASE + (Wcid * HW_IVEIV_ENTRY_SIZE);
+
+	// WPA mode
+	if ((CipherAlg == CIPHER_TKIP) || (CipherAlg == CIPHER_TKIP_NO_MIC) || (CipherAlg == CIPHER_AES))
+	{
+		// Eiv bit on. keyid always is 0 for pairwise key
+		IVEIV = (KeyIdx <<6) | 0x20;
+	}
+	else
+	{
+		// WEP KeyIdx is default tx key.
+		IVEIV = (KeyIdx << 6);
+	}
+
+	// For key index and ext IV bit, so only need to update the position(offset+3).
+#ifdef RT2860
+	RTMP_IO_WRITE8(pAd, offset+3, IVEIV);
+#endif // RT2860 //
+
+	DBGPRINT(RT_DEBUG_TRACE,("RTMPAddWcidAttributeEntry: WCID #%d, KeyIndex #%d, Alg=%s\n",Wcid, KeyIdx, CipherName[CipherAlg]));
+	DBGPRINT(RT_DEBUG_TRACE,("	WCIDAttri = 0x%x \n",  WCIDAttri));
+
+}
+
+/*
+    ==========================================================================
+    Description:
+        Parse encryption type
+Arguments:
+    pAdapter                    Pointer to our adapter
+    wrq                         Pointer to the ioctl argument
+
+    Return Value:
+        None
+
+    Note:
+    ==========================================================================
+*/
+CHAR *GetEncryptType(CHAR enc)
+{
+    if(enc == Ndis802_11WEPDisabled)
+        return "NONE";
+    if(enc == Ndis802_11WEPEnabled)
+    	return "WEP";
+    if(enc == Ndis802_11Encryption2Enabled)
+    	return "TKIP";
+    if(enc == Ndis802_11Encryption3Enabled)
+    	return "AES";
+	if(enc == Ndis802_11Encryption4Enabled)
+    	return "TKIPAES";
+    else
+    	return "UNKNOW";
+}
+
+CHAR *GetAuthMode(CHAR auth)
+{
+    if(auth == Ndis802_11AuthModeOpen)
+    	return "OPEN";
+    if(auth == Ndis802_11AuthModeShared)
+    	return "SHARED";
+	if(auth == Ndis802_11AuthModeAutoSwitch)
+    	return "AUTOWEP";
+    if(auth == Ndis802_11AuthModeWPA)
+    	return "WPA";
+    if(auth == Ndis802_11AuthModeWPAPSK)
+    	return "WPAPSK";
+    if(auth == Ndis802_11AuthModeWPANone)
+    	return "WPANONE";
+    if(auth == Ndis802_11AuthModeWPA2)
+    	return "WPA2";
+    if(auth == Ndis802_11AuthModeWPA2PSK)
+    	return "WPA2PSK";
+	if(auth == Ndis802_11AuthModeWPA1WPA2)
+    	return "WPA1WPA2";
+	if(auth == Ndis802_11AuthModeWPA1PSKWPA2PSK)
+    	return "WPA1PSKWPA2PSK";
+
+    	return "UNKNOW";
+}
+
+#if 1 //#ifndef UCOS
+/*
+    ==========================================================================
+    Description:
+        Get site survey results
+	Arguments:
+	    pAdapter                    Pointer to our adapter
+	    wrq                         Pointer to the ioctl argument
+
+    Return Value:
+        None
+
+    Note:
+        Usage:
+        		1.) UI needs to wait 4 seconds after issue a site survey command
+        		2.) iwpriv ra0 get_site_survey
+        		3.) UI needs to prepare at least 4096bytes to get the results
+    ==========================================================================
+*/
+#define	LINE_LEN	(4+33+20+8+10+9+7+3)	// Channel+SSID+Bssid+WepStatus+AuthMode+Signal+WiressMode+NetworkType
+VOID RTMPIoctlGetSiteSurvey(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq)
+{
+	CHAR		*msg;
+	INT 		i=0;
+	INT			WaitCnt;
+	INT 		Status=0;
+	CHAR		Ssid[MAX_LEN_OF_SSID +1];
+    INT         Rssi = 0, max_len = LINE_LEN;
+	UINT        Rssi_Quality = 0;
+	NDIS_802_11_NETWORK_TYPE    wireless_mode;
+
+	os_alloc_mem(NULL, (PUCHAR *)&msg, sizeof(CHAR)*((MAX_LEN_OF_BSS_TABLE)*max_len));
+
+	if (msg == NULL)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("RTMPIoctlGetSiteSurvey - msg memory alloc fail.\n"));
+		return;
+	}
+
+	memset(msg, 0 ,(MAX_LEN_OF_BSS_TABLE)*max_len );
+	memset(Ssid, 0 ,(MAX_LEN_OF_SSID +1));
+	sprintf(msg,"%s","\n");
+	sprintf(msg+strlen(msg),"%-4s%-33s%-20s%-8s%-10s%-9s%-7s%-3s\n",
+	    "Ch", "SSID", "BSSID", "Enc", "Auth", "Siganl(%)", "W-Mode", " NT");
+
+	WaitCnt = 0;
+#ifdef CONFIG_STA_SUPPORT
+	pAdapter->StaCfg.bScanReqIsFromWebUI = TRUE;
+	while ((ScanRunning(pAdapter) == TRUE) && (WaitCnt++ < 200))
+		OS_WAIT(500);
+#endif // CONFIG_STA_SUPPORT //
+
+	for(i=0; i<pAdapter->ScanTab.BssNr ;i++)
+	{
+		if( pAdapter->ScanTab.BssEntry[i].Channel==0)
+			break;
+
+		if((strlen(msg)+max_len ) >= IW_SCAN_MAX_DATA)
+			break;
+
+		//Channel
+		sprintf(msg+strlen(msg),"%-4d", pAdapter->ScanTab.BssEntry[i].Channel);
+		//SSID
+		memcpy(Ssid, pAdapter->ScanTab.BssEntry[i].Ssid, pAdapter->ScanTab.BssEntry[i].SsidLen);
+		Ssid[pAdapter->ScanTab.BssEntry[i].SsidLen] = '\0';
+		sprintf(msg+strlen(msg),"%-33s", Ssid);
+		//BSSID
+		sprintf(msg+strlen(msg),"%02x:%02x:%02x:%02x:%02x:%02x   ",
+			pAdapter->ScanTab.BssEntry[i].Bssid[0],
+			pAdapter->ScanTab.BssEntry[i].Bssid[1],
+			pAdapter->ScanTab.BssEntry[i].Bssid[2],
+			pAdapter->ScanTab.BssEntry[i].Bssid[3],
+			pAdapter->ScanTab.BssEntry[i].Bssid[4],
+			pAdapter->ScanTab.BssEntry[i].Bssid[5]);
+		//Encryption Type
+		sprintf(msg+strlen(msg),"%-8s",GetEncryptType(pAdapter->ScanTab.BssEntry[i].WepStatus));
+		//Authentication Mode
+		if (pAdapter->ScanTab.BssEntry[i].WepStatus == Ndis802_11WEPEnabled)
+			sprintf(msg+strlen(msg),"%-10s", "UNKNOW");
+		else
+			sprintf(msg+strlen(msg),"%-10s",GetAuthMode(pAdapter->ScanTab.BssEntry[i].AuthMode));
+		// Rssi
+		Rssi = (INT)pAdapter->ScanTab.BssEntry[i].Rssi;
+		if (Rssi >= -50)
+			Rssi_Quality = 100;
+		else if (Rssi >= -80)    // between -50 ~ -80dbm
+			Rssi_Quality = (UINT)(24 + ((Rssi + 80) * 26)/10);
+		else if (Rssi >= -90)   // between -80 ~ -90dbm
+			Rssi_Quality = (UINT)(((Rssi + 90) * 26)/10);
+		else    // < -84 dbm
+			Rssi_Quality = 0;
+		sprintf(msg+strlen(msg),"%-9d", Rssi_Quality);
+		// Wireless Mode
+		wireless_mode = NetworkTypeInUseSanity(&pAdapter->ScanTab.BssEntry[i]);
+		if (wireless_mode == Ndis802_11FH ||
+			wireless_mode == Ndis802_11DS)
+			sprintf(msg+strlen(msg),"%-7s", "11b");
+		else if (wireless_mode == Ndis802_11OFDM5)
+			sprintf(msg+strlen(msg),"%-7s", "11a");
+		else if (wireless_mode == Ndis802_11OFDM5_N)
+			sprintf(msg+strlen(msg),"%-7s", "11a/n");
+		else if (wireless_mode == Ndis802_11OFDM24)
+			sprintf(msg+strlen(msg),"%-7s", "11b/g");
+		else if (wireless_mode == Ndis802_11OFDM24_N)
+			sprintf(msg+strlen(msg),"%-7s", "11b/g/n");
+		else
+			sprintf(msg+strlen(msg),"%-7s", "unknow");
+		//Network Type
+		if (pAdapter->ScanTab.BssEntry[i].BssType == BSS_ADHOC)
+			sprintf(msg+strlen(msg),"%-3s", " Ad");
+		else
+			sprintf(msg+strlen(msg),"%-3s", " In");
+
+        sprintf(msg+strlen(msg),"\n");
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	pAdapter->StaCfg.bScanReqIsFromWebUI = FALSE;
+#endif // CONFIG_STA_SUPPORT //
+	wrq->u.data.length = strlen(msg);
+	Status = copy_to_user(wrq->u.data.pointer, msg, wrq->u.data.length);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPIoctlGetSiteSurvey - wrq->u.data.length = %d\n", wrq->u.data.length));
+	os_free_mem(NULL, (PUCHAR)msg);
+}
+
+
+#define	MAC_LINE_LEN	(14+4+4+10+10+10+6+6)	// Addr+aid+psm+datatime+rxbyte+txbyte+current tx rate+last tx rate
+VOID RTMPIoctlGetMacTable(
+	IN PRTMP_ADAPTER pAd,
+	IN struct iwreq *wrq)
+{
+	INT i;
+	RT_802_11_MAC_TABLE MacTab;
+	char *msg;
+
+	MacTab.Num = 0;
+	for (i=0; i<MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		if (pAd->MacTab.Content[i].ValidAsCLI && (pAd->MacTab.Content[i].Sst == SST_ASSOC))
+		{
+			COPY_MAC_ADDR(MacTab.Entry[MacTab.Num].Addr, &pAd->MacTab.Content[i].Addr);
+			MacTab.Entry[MacTab.Num].Aid = (UCHAR)pAd->MacTab.Content[i].Aid;
+			MacTab.Entry[MacTab.Num].Psm = pAd->MacTab.Content[i].PsMode;
+#ifdef DOT11_N_SUPPORT
+			MacTab.Entry[MacTab.Num].MimoPs = pAd->MacTab.Content[i].MmpsMode;
+#endif // DOT11_N_SUPPORT //
+
+			// Fill in RSSI per entry
+			MacTab.Entry[MacTab.Num].AvgRssi0 = pAd->MacTab.Content[i].RssiSample.AvgRssi0;
+			MacTab.Entry[MacTab.Num].AvgRssi1 = pAd->MacTab.Content[i].RssiSample.AvgRssi1;
+			MacTab.Entry[MacTab.Num].AvgRssi2 = pAd->MacTab.Content[i].RssiSample.AvgRssi2;
+
+			// the connected time per entry
+			MacTab.Entry[MacTab.Num].ConnectedTime = pAd->MacTab.Content[i].StaConnectTime;
+			MacTab.Entry[MacTab.Num].TxRate.field.MCS = pAd->MacTab.Content[i].HTPhyMode.field.MCS;
+			MacTab.Entry[MacTab.Num].TxRate.field.BW = pAd->MacTab.Content[i].HTPhyMode.field.BW;
+			MacTab.Entry[MacTab.Num].TxRate.field.ShortGI = pAd->MacTab.Content[i].HTPhyMode.field.ShortGI;
+			MacTab.Entry[MacTab.Num].TxRate.field.STBC = pAd->MacTab.Content[i].HTPhyMode.field.STBC;
+			MacTab.Entry[MacTab.Num].TxRate.field.rsv = pAd->MacTab.Content[i].HTPhyMode.field.rsv;
+			MacTab.Entry[MacTab.Num].TxRate.field.MODE = pAd->MacTab.Content[i].HTPhyMode.field.MODE;
+			MacTab.Entry[MacTab.Num].TxRate.word = pAd->MacTab.Content[i].HTPhyMode.word;
+
+			MacTab.Num += 1;
+		}
+	}
+	wrq->u.data.length = sizeof(RT_802_11_MAC_TABLE);
+	if (copy_to_user(wrq->u.data.pointer, &MacTab, wrq->u.data.length))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("%s: copy_to_user() fail\n", __FUNCTION__));
+	}
+
+	msg = (CHAR *) kmalloc(sizeof(CHAR)*(MAX_LEN_OF_MAC_TABLE*MAC_LINE_LEN), MEM_ALLOC_FLAG);
+	memset(msg, 0 ,MAX_LEN_OF_MAC_TABLE*MAC_LINE_LEN );
+	sprintf(msg,"%s","\n");
+	sprintf(msg+strlen(msg),"%-14s%-4s%-4s%-10s%-10s%-10s%-6s%-6s\n",
+		"MAC", "AID", "PSM", "LDT", "RxB", "TxB","CTxR", "LTxR");
+
+	for (i=0; i<MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		PMAC_TABLE_ENTRY pEntry = &pAd->MacTab.Content[i];
+		if (pEntry->ValidAsCLI && (pEntry->Sst == SST_ASSOC))
+		{
+			if((strlen(msg)+MAC_LINE_LEN ) >= (MAX_LEN_OF_MAC_TABLE*MAC_LINE_LEN) )
+				break;
+			sprintf(msg+strlen(msg),"%02x%02x%02x%02x%02x%02x  ",
+				pEntry->Addr[0], pEntry->Addr[1], pEntry->Addr[2],
+				pEntry->Addr[3], pEntry->Addr[4], pEntry->Addr[5]);
+			sprintf(msg+strlen(msg),"%-4d", (int)pEntry->Aid);
+			sprintf(msg+strlen(msg),"%-4d", (int)pEntry->PsMode);
+			sprintf(msg+strlen(msg),"%-10d",0/*pAd->MacTab.Content[i].HSCounter.LastDataPacketTime*/); // ToDo
+			sprintf(msg+strlen(msg),"%-10d",0/*pAd->MacTab.Content[i].HSCounter.TotalRxByteCount*/); // ToDo
+			sprintf(msg+strlen(msg),"%-10d",0/*pAd->MacTab.Content[i].HSCounter.TotalTxByteCount*/); // ToDo
+			sprintf(msg+strlen(msg),"%-6d",RateIdToMbps[pAd->MacTab.Content[i].CurrTxRate]);
+			sprintf(msg+strlen(msg),"%-6d\n",0/*RateIdToMbps[pAd->MacTab.Content[i].LastTxRate]*/); // ToDo
+		}
+	}
+	// for compatible with old API just do the printk to console
+	//wrq->u.data.length = strlen(msg);
+	//if (copy_to_user(wrq->u.data.pointer, msg, wrq->u.data.length))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("%s", msg));
+	}
+
+	kfree(msg);
+}
+#endif // UCOS //
+
+#ifdef DOT11_N_SUPPORT
+INT	Set_BASetup_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+    UCHAR mac[6], tid;
+	char *token, sepValue[] = ":", DASH = '-';
+	INT i;
+    MAC_TABLE_ENTRY *pEntry;
+
+/*
+	The BASetup inupt string format should be xx:xx:xx:xx:xx:xx-d,
+		=>The six 2 digit hex-decimal number previous are the Mac address,
+		=>The seventh decimal number is the tid value.
+*/
+	//printk("\n%s\n", arg);
+
+	if(strlen(arg) < 19)  //Mac address acceptable format 01:02:03:04:05:06 length 17 plus the "-" and tid value in decimal format.
+		return FALSE;
+
+	token = strchr(arg, DASH);
+	if ((token != NULL) && (strlen(token)>1))
+	{
+		tid = simple_strtol((token+1), 0, 10);
+		if (tid > 15)
+			return FALSE;
+
+		*token = '\0';
+		for (i = 0, token = rstrtok(arg, &sepValue[0]); token; token = rstrtok(NULL, &sepValue[0]), i++)
+		{
+			if((strlen(token) != 2) || (!isxdigit(*token)) || (!isxdigit(*(token+1))))
+				return FALSE;
+			AtoH(token, (PUCHAR)(&mac[i]), 1);
+		}
+		if(i != 6)
+			return FALSE;
+
+		printk("\n%02x:%02x:%02x:%02x:%02x:%02x-%02x\n", mac[0], mac[1],
+				mac[2], mac[3], mac[4], mac[5], tid);
+
+	    pEntry = MacTableLookup(pAd, mac);
+
+    	if (pEntry) {
+        	printk("\nSetup BA Session: Tid = %d\n", tid);
+	        BAOriSessionSetUp(pAd, pEntry, tid, 0, 100, TRUE);
+    	}
+
+		return TRUE;
+	}
+
+	return FALSE;
+
+}
+
+INT	Set_BADecline_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG bBADecline;
+
+	bBADecline = simple_strtol(arg, 0, 10);
+
+	if (bBADecline == 0)
+	{
+		pAd->CommonCfg.bBADecline = FALSE;
+	}
+	else if (bBADecline == 1)
+	{
+		pAd->CommonCfg.bBADecline = TRUE;
+	}
+	else
+	{
+		return FALSE; //Invalid argument
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_BADecline_Proc::(BADecline=%d)\n", pAd->CommonCfg.bBADecline));
+
+	return TRUE;
+}
+
+INT	Set_BAOriTearDown_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+    UCHAR mac[6], tid;
+	char *token, sepValue[] = ":", DASH = '-';
+	INT i;
+    MAC_TABLE_ENTRY *pEntry;
+
+/*
+	The BAOriTearDown inupt string format should be xx:xx:xx:xx:xx:xx-d,
+		=>The six 2 digit hex-decimal number previous are the Mac address,
+		=>The seventh decimal number is the tid value.
+*/
+    if(strlen(arg) < 19)  //Mac address acceptable format 01:02:03:04:05:06 length 17 plus the "-" and tid value in decimal format.
+		return FALSE;
+
+	token = strchr(arg, DASH);
+	if ((token != NULL) && (strlen(token)>1))
+	{
+		tid = simple_strtol((token+1), 0, 10);
+		if (tid > NUM_OF_TID)
+			return FALSE;
+
+		*token = '\0';
+		for (i = 0, token = rstrtok(arg, &sepValue[0]); token; token = rstrtok(NULL, &sepValue[0]), i++)
+		{
+			if((strlen(token) != 2) || (!isxdigit(*token)) || (!isxdigit(*(token+1))))
+				return FALSE;
+			AtoH(token, (PUCHAR)(&mac[i]), 1);
+		}
+		if(i != 6)
+			return FALSE;
+
+	    printk("\n%02x:%02x:%02x:%02x:%02x:%02x-%02x", mac[0], mac[1],
+	           mac[2], mac[3], mac[4], mac[5], tid);
+
+	    pEntry = MacTableLookup(pAd, mac);
+
+	    if (pEntry) {
+	        printk("\nTear down Ori BA Session: Tid = %d\n", tid);
+        BAOriSessionTearDown(pAd, pEntry->Aid, tid, FALSE, TRUE);
+	    }
+
+		return TRUE;
+	}
+
+	return FALSE;
+
+}
+
+INT	Set_BARecTearDown_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+    UCHAR mac[6], tid;
+	char *token, sepValue[] = ":", DASH = '-';
+	INT i;
+    MAC_TABLE_ENTRY *pEntry;
+
+    //printk("\n%s\n", arg);
+/*
+	The BARecTearDown inupt string format should be xx:xx:xx:xx:xx:xx-d,
+		=>The six 2 digit hex-decimal number previous are the Mac address,
+		=>The seventh decimal number is the tid value.
+*/
+    if(strlen(arg) < 19)  //Mac address acceptable format 01:02:03:04:05:06 length 17 plus the "-" and tid value in decimal format.
+		return FALSE;
+
+	token = strchr(arg, DASH);
+	if ((token != NULL) && (strlen(token)>1))
+	{
+		tid = simple_strtol((token+1), 0, 10);
+		if (tid > NUM_OF_TID)
+			return FALSE;
+
+		*token = '\0';
+		for (i = 0, token = rstrtok(arg, &sepValue[0]); token; token = rstrtok(NULL, &sepValue[0]), i++)
+		{
+			if((strlen(token) != 2) || (!isxdigit(*token)) || (!isxdigit(*(token+1))))
+				return FALSE;
+			AtoH(token, (PUCHAR)(&mac[i]), 1);
+		}
+		if(i != 6)
+			return FALSE;
+
+		printk("\n%02x:%02x:%02x:%02x:%02x:%02x-%02x", mac[0], mac[1],
+		       mac[2], mac[3], mac[4], mac[5], tid);
+
+		pEntry = MacTableLookup(pAd, mac);
+
+		if (pEntry) {
+		    printk("\nTear down Rec BA Session: Tid = %d\n", tid);
+		    BARecSessionTearDown(pAd, pEntry->Aid, tid, FALSE);
+		}
+
+		return TRUE;
+	}
+
+	return FALSE;
+
+}
+
+INT	Set_HtBw_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG HtBw;
+
+	HtBw = simple_strtol(arg, 0, 10);
+	if (HtBw == BW_40)
+		pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_40;
+	else if (HtBw == BW_20)
+		pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_20;
+	else
+		return FALSE;  //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtBw_Proc::(HtBw=%d)\n", pAd->CommonCfg.RegTransmitSetting.field.BW));
+
+	return TRUE;
+}
+
+INT	Set_HtMcs_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG HtMcs, Mcs_tmp;
+#ifdef CONFIG_STA_SUPPORT
+    BOOLEAN bAutoRate = FALSE;
+#endif // CONFIG_STA_SUPPORT //
+
+	Mcs_tmp = simple_strtol(arg, 0, 10);
+
+	if (Mcs_tmp <= 15 || Mcs_tmp == 32)
+		HtMcs = Mcs_tmp;
+	else
+		HtMcs = MCS_AUTO;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		pAd->StaCfg.DesiredTransmitSetting.field.MCS = HtMcs;
+		pAd->StaCfg.bAutoTxRateSwitch = (HtMcs == MCS_AUTO) ? TRUE:FALSE;
+		DBGPRINT(RT_DEBUG_TRACE, ("Set_HtMcs_Proc::(HtMcs=%d, bAutoTxRateSwitch = %d)\n",
+						pAd->StaCfg.DesiredTransmitSetting.field.MCS, pAd->StaCfg.bAutoTxRateSwitch));
+
+		if ((pAd->CommonCfg.PhyMode < PHY_11ABGN_MIXED) ||
+			(pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE < MODE_HTMIX))
+		{
+	        if ((pAd->StaCfg.DesiredTransmitSetting.field.MCS != MCS_AUTO) &&
+				(HtMcs >= 0 && HtMcs <= 3) &&
+	            (pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode == FIXED_TXMODE_CCK))
+			{
+				RTMPSetDesiredRates(pAd, (LONG) (RateIdToMbps[HtMcs] * 1000000));
+			}
+	        else if ((pAd->StaCfg.DesiredTransmitSetting.field.MCS != MCS_AUTO) &&
+					(HtMcs >= 0 && HtMcs <= 7) &&
+	            	(pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode == FIXED_TXMODE_OFDM))
+			{
+				RTMPSetDesiredRates(pAd, (LONG) (RateIdToMbps[HtMcs+4] * 1000000));
+			}
+			else
+				bAutoRate = TRUE;
+
+			if (bAutoRate)
+			{
+	            pAd->StaCfg.DesiredTransmitSetting.field.MCS = MCS_AUTO;
+				RTMPSetDesiredRates(pAd, -1);
+			}
+	        DBGPRINT(RT_DEBUG_TRACE, ("Set_HtMcs_Proc::(FixedTxMode=%d)\n",pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode));
+		}
+        if (ADHOC_ON(pAd))
+            return TRUE;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	SetCommonHT(pAd);
+
+	return TRUE;
+}
+
+INT	Set_HtGi_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG HtGi;
+
+	HtGi = simple_strtol(arg, 0, 10);
+
+	if ( HtGi == GI_400)
+		pAd->CommonCfg.RegTransmitSetting.field.ShortGI = GI_400;
+	else if ( HtGi == GI_800 )
+		pAd->CommonCfg.RegTransmitSetting.field.ShortGI = GI_800;
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtGi_Proc::(ShortGI=%d)\n",pAd->CommonCfg.RegTransmitSetting.field.ShortGI));
+
+	return TRUE;
+}
+
+
+INT	Set_HtTxBASize_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UCHAR Size;
+
+	Size = simple_strtol(arg, 0, 10);
+
+	if (Size <=0 || Size >=64)
+	{
+		Size = 8;
+	}
+	pAd->CommonCfg.TxBASize = Size-1;
+	DBGPRINT(RT_DEBUG_ERROR, ("Set_HtTxBASize ::(TxBASize= %d)\n", Size));
+
+	return TRUE;
+}
+
+
+INT	Set_HtOpMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+	if (Value == HTMODE_GF)
+		pAd->CommonCfg.RegTransmitSetting.field.HTMODE  = HTMODE_GF;
+	else if ( Value == HTMODE_MM )
+		pAd->CommonCfg.RegTransmitSetting.field.HTMODE  = HTMODE_MM;
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtOpMode_Proc::(HtOpMode=%d)\n",pAd->CommonCfg.RegTransmitSetting.field.HTMODE));
+
+	return TRUE;
+
+}
+
+INT	Set_HtStbc_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+	if (Value == STBC_USE)
+		pAd->CommonCfg.RegTransmitSetting.field.STBC = STBC_USE;
+	else if ( Value == STBC_NONE )
+		pAd->CommonCfg.RegTransmitSetting.field.STBC = STBC_NONE;
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_Stbc_Proc::(HtStbc=%d)\n",pAd->CommonCfg.RegTransmitSetting.field.STBC));
+
+	return TRUE;
+}
+
+INT	Set_HtHtc_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->HTCEnable = FALSE;
+	else if ( Value ==1 )
+        	pAd->HTCEnable = TRUE;
+	else
+		return FALSE; //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtHtc_Proc::(HtHtc=%d)\n",pAd->HTCEnable));
+
+	return TRUE;
+}
+
+INT	Set_HtExtcha_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+	if (Value == 0)
+		pAd->CommonCfg.RegTransmitSetting.field.EXTCHA  = EXTCHA_BELOW;
+	else if ( Value ==1 )
+        pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_ABOVE;
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtExtcha_Proc::(HtExtcha=%d)\n",pAd->CommonCfg.RegTransmitSetting.field.EXTCHA));
+
+	return TRUE;
+}
+
+INT	Set_HtMpduDensity_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+	if (Value <=7 && Value >= 0)
+		pAd->CommonCfg.BACapability.field.MpduDensity = Value;
+	else
+		pAd->CommonCfg.BACapability.field.MpduDensity = 4;
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtMpduDensity_Proc::(HtMpduDensity=%d)\n",pAd->CommonCfg.BACapability.field.MpduDensity));
+
+	return TRUE;
+}
+
+INT	Set_HtBaWinSize_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+
+	if (Value >=1 && Value <= 64)
+	{
+		pAd->CommonCfg.REGBACapability.field.RxBAWinLimit = Value;
+		pAd->CommonCfg.BACapability.field.RxBAWinLimit = Value;
+	}
+	else
+	{
+        pAd->CommonCfg.REGBACapability.field.RxBAWinLimit = 64;
+		pAd->CommonCfg.BACapability.field.RxBAWinLimit = 64;
+	}
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtBaWinSize_Proc::(HtBaWinSize=%d)\n",pAd->CommonCfg.BACapability.field.RxBAWinLimit));
+
+	return TRUE;
+}
+
+INT	Set_HtRdg_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+	if (Value == 0)
+		pAd->CommonCfg.bRdg = FALSE;
+	else if ( Value ==1 )
+	{
+		pAd->HTCEnable = TRUE;
+        	pAd->CommonCfg.bRdg = TRUE;
+	}
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtRdg_Proc::(HtRdg=%d)\n",pAd->CommonCfg.bRdg));
+
+	return TRUE;
+}
+
+INT	Set_HtLinkAdapt_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->bLinkAdapt = FALSE;
+	else if ( Value ==1 )
+	{
+			pAd->HTCEnable = TRUE;
+			pAd->bLinkAdapt = TRUE;
+	}
+	else
+		return FALSE; //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtLinkAdapt_Proc::(HtLinkAdapt=%d)\n",pAd->bLinkAdapt));
+
+	return TRUE;
+}
+
+INT	Set_HtAmsdu_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->CommonCfg.BACapability.field.AmsduEnable = FALSE;
+	else if ( Value == 1 )
+        pAd->CommonCfg.BACapability.field.AmsduEnable = TRUE;
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtAmsdu_Proc::(HtAmsdu=%d)\n",pAd->CommonCfg.BACapability.field.AmsduEnable));
+
+	return TRUE;
+}
+
+INT	Set_HtAutoBa_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->CommonCfg.BACapability.field.AutoBA = FALSE;
+    else if (Value == 1)
+		pAd->CommonCfg.BACapability.field.AutoBA = TRUE;
+	else
+		return FALSE; //Invalid argument
+
+    pAd->CommonCfg.REGBACapability.field.AutoBA = pAd->CommonCfg.BACapability.field.AutoBA;
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtAutoBa_Proc::(HtAutoBa=%d)\n",pAd->CommonCfg.BACapability.field.AutoBA));
+
+	return TRUE;
+
+}
+
+INT	Set_HtProtect_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->CommonCfg.bHTProtect = FALSE;
+    else if (Value == 1)
+		pAd->CommonCfg.bHTProtect = TRUE;
+	else
+		return FALSE; //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtProtect_Proc::(HtProtect=%d)\n",pAd->CommonCfg.bHTProtect));
+
+	return TRUE;
+}
+
+INT	Set_SendPSMPAction_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+    UCHAR mac[6], mode;
+	char *token, sepValue[] = ":", DASH = '-';
+	INT i;
+    MAC_TABLE_ENTRY *pEntry;
+
+    //printk("\n%s\n", arg);
+/*
+	The BARecTearDown inupt string format should be xx:xx:xx:xx:xx:xx-d,
+		=>The six 2 digit hex-decimal number previous are the Mac address,
+		=>The seventh decimal number is the mode value.
+*/
+    if(strlen(arg) < 19)  //Mac address acceptable format 01:02:03:04:05:06 length 17 plus the "-" and mode value in decimal format.
+		return FALSE;
+
+   	token = strchr(arg, DASH);
+	if ((token != NULL) && (strlen(token)>1))
+	{
+		mode = simple_strtol((token+1), 0, 10);
+		if (mode > MMPS_ENABLE)
+			return FALSE;
+
+		*token = '\0';
+		for (i = 0, token = rstrtok(arg, &sepValue[0]); token; token = rstrtok(NULL, &sepValue[0]), i++)
+		{
+			if((strlen(token) != 2) || (!isxdigit(*token)) || (!isxdigit(*(token+1))))
+				return FALSE;
+			AtoH(token, (PUCHAR)(&mac[i]), 1);
+		}
+		if(i != 6)
+			return FALSE;
+
+		printk("\n%02x:%02x:%02x:%02x:%02x:%02x-%02x", mac[0], mac[1],
+		       mac[2], mac[3], mac[4], mac[5], mode);
+
+		pEntry = MacTableLookup(pAd, mac);
+
+		if (pEntry) {
+		    printk("\nSendPSMPAction MIPS mode = %d\n", mode);
+		    SendPSMPAction(pAd, pEntry->Aid, mode);
+		}
+
+		return TRUE;
+	}
+
+	return FALSE;
+
+
+}
+
+INT	Set_HtMIMOPSmode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+	if (Value <=3 && Value >= 0)
+		pAd->CommonCfg.BACapability.field.MMPSmode = Value;
+	else
+		pAd->CommonCfg.BACapability.field.MMPSmode = 3;
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtMIMOPSmode_Proc::(MIMOPS mode=%d)\n",pAd->CommonCfg.BACapability.field.MMPSmode));
+
+	return TRUE;
+}
+
+
+INT	Set_ForceShortGI_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->WIFItestbed.bShortGI = FALSE;
+	else if (Value == 1)
+		pAd->WIFItestbed.bShortGI = TRUE;
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_ForceShortGI_Proc::(ForceShortGI=%d)\n", pAd->WIFItestbed.bShortGI));
+
+	return TRUE;
+}
+
+
+
+INT	Set_ForceGF_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->WIFItestbed.bGreenField = FALSE;
+	else if (Value == 1)
+		pAd->WIFItestbed.bGreenField = TRUE;
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_ForceGF_Proc::(ForceGF=%d)\n", pAd->WIFItestbed.bGreenField));
+
+	return TRUE;
+}
+
+INT	Set_HtMimoPs_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->CommonCfg.bMIMOPSEnable = FALSE;
+	else if (Value == 1)
+		pAd->CommonCfg.bMIMOPSEnable = TRUE;
+	else
+		return FALSE; //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtMimoPs_Proc::(HtMimoPs=%d)\n",pAd->CommonCfg.bMIMOPSEnable));
+
+	return TRUE;
+}
+#endif // DOT11_N_SUPPORT //
+
+
+#ifdef DOT11_N_SUPPORT
+INT	SetCommonHT(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	OID_SET_HT_PHYMODE		SetHT;
+
+	if (pAd->CommonCfg.PhyMode < PHY_11ABGN_MIXED)
+		return FALSE;
+
+	SetHT.PhyMode = pAd->CommonCfg.PhyMode;
+	SetHT.TransmitNo = ((UCHAR)pAd->Antenna.field.TxPath);
+	SetHT.HtMode = (UCHAR)pAd->CommonCfg.RegTransmitSetting.field.HTMODE;
+	SetHT.ExtOffset = (UCHAR)pAd->CommonCfg.RegTransmitSetting.field.EXTCHA;
+	SetHT.MCS = MCS_AUTO;
+	SetHT.BW = (UCHAR)pAd->CommonCfg.RegTransmitSetting.field.BW;
+	SetHT.STBC = (UCHAR)pAd->CommonCfg.RegTransmitSetting.field.STBC;
+	SetHT.SHORTGI = (UCHAR)pAd->CommonCfg.RegTransmitSetting.field.ShortGI;
+
+	RTMPSetHT(pAd, &SetHT);
+
+	return TRUE;
+}
+#endif // DOT11_N_SUPPORT //
+
+INT	Set_FixedTxMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UCHAR	fix_tx_mode = FIXED_TXMODE_HT;
+
+	if (strcmp(arg, "OFDM") == 0 || strcmp(arg, "ofdm") == 0)
+	{
+		fix_tx_mode = FIXED_TXMODE_OFDM;
+	}
+	else if (strcmp(arg, "CCK") == 0 || strcmp(arg, "cck") == 0)
+	{
+        fix_tx_mode = FIXED_TXMODE_CCK;
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode = fix_tx_mode;
+#endif // CONFIG_STA_SUPPORT //
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_FixedTxMode_Proc::(FixedTxMode=%d)\n", fix_tx_mode));
+
+	return TRUE;
+}
+
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+INT	Set_OpMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+#ifdef RT2860
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+#endif // RT2860 //
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Can not switch operate mode on interface up !! \n"));
+		return FALSE;
+	}
+
+	if (Value == 0)
+		pAd->OpMode = OPMODE_STA;
+	else if (Value == 1)
+		pAd->OpMode = OPMODE_AP;
+	else
+		return FALSE; //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_OpMode_Proc::(OpMode=%s)\n", pAd->OpMode == 1 ? "AP Mode" : "STA Mode"));
+
+	return TRUE;
+}
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+
+
+/////////////////////////////////////////////////////////////////////////
+PCHAR   RTMPGetRalinkAuthModeStr(
+    IN  NDIS_802_11_AUTHENTICATION_MODE authMode)
+{
+	switch(authMode)
+	{
+		case Ndis802_11AuthModeOpen:
+			return "OPEN";
+        default:
+		case Ndis802_11AuthModeWPAPSK:
+			return "WPAPSK";
+		case Ndis802_11AuthModeShared:
+			return "SHARED";
+		case Ndis802_11AuthModeWPA:
+			return "WPA";
+		case Ndis802_11AuthModeWPA2:
+			return "WPA2";
+		case Ndis802_11AuthModeWPA2PSK:
+			return "WPA2PSK";
+        case Ndis802_11AuthModeWPA1PSKWPA2PSK:
+			return "WPAPSKWPA2PSK";
+        case Ndis802_11AuthModeWPA1WPA2:
+			return "WPA1WPA2";
+		case Ndis802_11AuthModeWPANone:
+			return "WPANONE";
+	}
+}
+
+PCHAR   RTMPGetRalinkEncryModeStr(
+    IN  USHORT encryMode)
+{
+	switch(encryMode)
+	{
+	    default:
+		case Ndis802_11WEPDisabled:
+			return "NONE";
+		case Ndis802_11WEPEnabled:
+			return "WEP";
+		case Ndis802_11Encryption2Enabled:
+			return "TKIP";
+		case Ndis802_11Encryption3Enabled:
+			return "AES";
+        case Ndis802_11Encryption4Enabled:
+			return "TKIPAES";
+	}
+}
+
+INT RTMPShowCfgValue(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pName,
+	IN	PUCHAR			pBuf)
+{
+	INT	Status = 0;
+
+	for (PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC = RTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC; PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC->name; PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC++)
+	{
+		if (!strcmp(pName, PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC->name))
+		{
+			if(PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC->show_proc(pAd, pBuf))
+				Status = -EINVAL;
+			break;  //Exit for loop.
+		}
+	}
+
+	if(PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC->name == NULL)
+	{
+		sprintf(pBuf, "\n");
+		for (PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC = RTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC; PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC->name; PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC++)
+			sprintf(pBuf, "%s%s\n", pBuf, PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC->name);
+	}
+
+	return Status;
+}
+
+INT	Show_SSID_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		sprintf(pBuf, "\t%s", pAd->CommonCfg.Ssid);
+#endif // CONFIG_STA_SUPPORT //
+	return 0;
+}
+
+INT	Show_WirelessMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	switch(pAd->CommonCfg.PhyMode)
+	{
+		case PHY_11BG_MIXED:
+			sprintf(pBuf, "\t11B/G");
+			break;
+		case PHY_11B:
+			sprintf(pBuf, "\t11B");
+			break;
+		case PHY_11A:
+			sprintf(pBuf, "\t11A");
+			break;
+		case PHY_11ABG_MIXED:
+			sprintf(pBuf, "\t11A/B/G");
+			break;
+		case PHY_11G:
+			sprintf(pBuf, "\t11G");
+			break;
+#ifdef DOT11_N_SUPPORT
+		case PHY_11ABGN_MIXED:
+			sprintf(pBuf, "\t11A/B/G/N");
+			break;
+		case PHY_11N_2_4G:
+			sprintf(pBuf, "\t11N only with 2.4G");
+			break;
+		case PHY_11GN_MIXED:
+			sprintf(pBuf, "\t11G/N");
+			break;
+		case PHY_11AN_MIXED:
+			sprintf(pBuf, "\t11A/N");
+			break;
+		case PHY_11BGN_MIXED:
+			sprintf(pBuf, "\t11B/G/N");
+			break;
+		case PHY_11AGN_MIXED:
+			sprintf(pBuf, "\t11A/G/N");
+			break;
+		case PHY_11N_5G:
+			sprintf(pBuf, "\t11N only with 5G");
+			break;
+#endif // DOT11_N_SUPPORT //
+		default:
+			sprintf(pBuf, "\tUnknow Value(%d)", pAd->CommonCfg.PhyMode);
+			break;
+	}
+	return 0;
+}
+
+
+INT	Show_TxBurst_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%s", pAd->CommonCfg.bEnableTxBurst ? "TRUE":"FALSE");
+	return 0;
+}
+
+INT	Show_TxPreamble_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	switch(pAd->CommonCfg.TxPreamble)
+	{
+		case Rt802_11PreambleShort:
+			sprintf(pBuf, "\tShort");
+			break;
+		case Rt802_11PreambleLong:
+			sprintf(pBuf, "\tLong");
+			break;
+		case Rt802_11PreambleAuto:
+			sprintf(pBuf, "\tAuto");
+			break;
+		default:
+			sprintf(pBuf, "\tUnknow Value(%lu)", pAd->CommonCfg.TxPreamble);
+			break;
+	}
+
+	return 0;
+}
+
+INT	Show_TxPower_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%lu", pAd->CommonCfg.TxPowerPercentage);
+	return 0;
+}
+
+INT	Show_Channel_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%d", pAd->CommonCfg.Channel);
+	return 0;
+}
+
+INT	Show_BGProtection_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	switch(pAd->CommonCfg.UseBGProtection)
+	{
+		case 1: //Always On
+			sprintf(pBuf, "\tON");
+			break;
+		case 2: //Always OFF
+			sprintf(pBuf, "\tOFF");
+			break;
+		case 0: //AUTO
+			sprintf(pBuf, "\tAuto");
+			break;
+		default:
+			sprintf(pBuf, "\tUnknow Value(%lu)", pAd->CommonCfg.UseBGProtection);
+			break;
+	}
+	return 0;
+}
+
+INT	Show_RTSThreshold_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%u", pAd->CommonCfg.RtsThreshold);
+	return 0;
+}
+
+INT	Show_FragThreshold_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%u", pAd->CommonCfg.FragmentThreshold);
+	return 0;
+}
+
+#ifdef DOT11_N_SUPPORT
+INT	Show_HtBw_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	if (pAd->CommonCfg.RegTransmitSetting.field.BW == BW_40)
+	{
+		sprintf(pBuf, "\t40 MHz");
+	}
+	else
+	{
+        sprintf(pBuf, "\t20 MHz");
+	}
+	return 0;
+}
+
+INT	Show_HtMcs_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		sprintf(pBuf, "\t%u", pAd->StaCfg.DesiredTransmitSetting.field.MCS);
+#endif // CONFIG_STA_SUPPORT //
+	return 0;
+}
+
+INT	Show_HtGi_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	switch(pAd->CommonCfg.RegTransmitSetting.field.ShortGI)
+	{
+		case GI_400:
+			sprintf(pBuf, "\tGI_400");
+			break;
+		case GI_800:
+			sprintf(pBuf, "\tGI_800");
+			break;
+		default:
+			sprintf(pBuf, "\tUnknow Value(%u)", pAd->CommonCfg.RegTransmitSetting.field.ShortGI);
+			break;
+	}
+	return 0;
+}
+
+INT	Show_HtOpMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	switch(pAd->CommonCfg.RegTransmitSetting.field.HTMODE)
+	{
+		case HTMODE_GF:
+			sprintf(pBuf, "\tGF");
+			break;
+		case HTMODE_MM:
+			sprintf(pBuf, "\tMM");
+			break;
+		default:
+			sprintf(pBuf, "\tUnknow Value(%u)", pAd->CommonCfg.RegTransmitSetting.field.HTMODE);
+			break;
+	}
+	return 0;
+}
+
+INT	Show_HtExtcha_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	switch(pAd->CommonCfg.RegTransmitSetting.field.EXTCHA)
+	{
+		case EXTCHA_BELOW:
+			sprintf(pBuf, "\tBelow");
+			break;
+		case EXTCHA_ABOVE:
+			sprintf(pBuf, "\tAbove");
+			break;
+		default:
+			sprintf(pBuf, "\tUnknow Value(%u)", pAd->CommonCfg.RegTransmitSetting.field.EXTCHA);
+			break;
+	}
+	return 0;
+}
+
+
+INT	Show_HtMpduDensity_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%u", pAd->CommonCfg.BACapability.field.MpduDensity);
+	return 0;
+}
+
+INT	Show_HtBaWinSize_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%u", pAd->CommonCfg.BACapability.field.RxBAWinLimit);
+	return 0;
+}
+
+INT	Show_HtRdg_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%s", pAd->CommonCfg.bRdg ? "TRUE":"FALSE");
+	return 0;
+}
+
+INT	Show_HtAmsdu_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%s", pAd->CommonCfg.BACapability.field.AmsduEnable ? "TRUE":"FALSE");
+	return 0;
+}
+
+INT	Show_HtAutoBa_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%s", pAd->CommonCfg.BACapability.field.AutoBA ? "TRUE":"FALSE");
+	return 0;
+}
+#endif // DOT11_N_SUPPORT //
+
+INT	Show_CountryRegion_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%d", pAd->CommonCfg.CountryRegion);
+	return 0;
+}
+
+INT	Show_CountryRegionABand_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%d", pAd->CommonCfg.CountryRegionForABand);
+	return 0;
+}
+
+INT	Show_CountryCode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%s", pAd->CommonCfg.CountryCode);
+	return 0;
+}
+
+#ifdef AGGREGATION_SUPPORT
+INT	Show_PktAggregate_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%s", pAd->CommonCfg.bAggregationCapable ? "TRUE":"FALSE");
+	return 0;
+}
+#endif // AGGREGATION_SUPPORT //
+
+#ifdef WMM_SUPPORT
+INT	Show_WmmCapable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		sprintf(pBuf, "\t%s", pAd->CommonCfg.bWmmCapable ? "TRUE":"FALSE");
+#endif // CONFIG_STA_SUPPORT //
+
+	return 0;
+}
+#endif // WMM_SUPPORT //
+
+INT	Show_IEEE80211H_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%s", pAd->CommonCfg.bIEEE80211H ? "TRUE":"FALSE");
+	return 0;
+}
+
+#ifdef CONFIG_STA_SUPPORT
+INT	Show_NetworkType_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	switch(pAd->StaCfg.BssType)
+	{
+		case BSS_ADHOC:
+			sprintf(pBuf, "\tAdhoc");
+			break;
+		case BSS_INFRA:
+			sprintf(pBuf, "\tInfra");
+			break;
+		case BSS_ANY:
+			sprintf(pBuf, "\tAny");
+			break;
+		case BSS_MONITOR:
+			sprintf(pBuf, "\tMonitor");
+			break;
+		default:
+			sprintf(pBuf, "\tUnknow Value(%d)", pAd->StaCfg.BssType);
+			break;
+	}
+	return 0;
+}
+#endif // CONFIG_STA_SUPPORT //
+
+INT	Show_AuthMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	NDIS_802_11_AUTHENTICATION_MODE	AuthMode = Ndis802_11AuthModeOpen;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		AuthMode = pAd->StaCfg.AuthMode;
+#endif // CONFIG_STA_SUPPORT //
+
+	if ((AuthMode >= Ndis802_11AuthModeOpen) &&
+		(AuthMode <= Ndis802_11AuthModeWPA1PSKWPA2PSK))
+		sprintf(pBuf, "\t%s", RTMPGetRalinkAuthModeStr(AuthMode));
+	else
+		sprintf(pBuf, "\tUnknow Value(%d)", AuthMode);
+
+	return 0;
+}
+
+INT	Show_EncrypType_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	NDIS_802_11_WEP_STATUS	WepStatus = Ndis802_11WEPDisabled;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		WepStatus = pAd->StaCfg.WepStatus;
+#endif // CONFIG_STA_SUPPORT //
+
+	if ((WepStatus >= Ndis802_11WEPEnabled) &&
+		(WepStatus <= Ndis802_11Encryption4KeyAbsent))
+		sprintf(pBuf, "\t%s", RTMPGetRalinkEncryModeStr(WepStatus));
+	else
+		sprintf(pBuf, "\tUnknow Value(%d)", WepStatus);
+
+	return 0;
+}
+
+INT	Show_DefaultKeyID_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	UCHAR DefaultKeyId = 0;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		DefaultKeyId = pAd->StaCfg.DefaultKeyId;
+#endif // CONFIG_STA_SUPPORT //
+
+	sprintf(pBuf, "\t%d", DefaultKeyId);
+
+	return 0;
+}
+
+INT	Show_WepKey_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN  INT				KeyIdx,
+	OUT	PUCHAR			pBuf)
+{
+	UCHAR   Key[16] = {0}, KeyLength = 0;
+	INT		index = BSS0;
+
+	KeyLength = pAd->SharedKey[index][KeyIdx].KeyLen;
+	NdisMoveMemory(Key, pAd->SharedKey[index][KeyIdx].Key, KeyLength);
+
+	//check key string is ASCII or not
+    if (RTMPCheckStrPrintAble(Key, KeyLength))
+        sprintf(pBuf, "\t%s", Key);
+    else
+    {
+        int idx;
+        sprintf(pBuf, "\t");
+        for (idx = 0; idx < KeyLength; idx++)
+            sprintf(pBuf+strlen(pBuf), "%02X", Key[idx]);
+    }
+	return 0;
+}
+
+INT	Show_Key1_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	Show_WepKey_Proc(pAd, 0, pBuf);
+	return 0;
+}
+
+INT	Show_Key2_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	Show_WepKey_Proc(pAd, 1, pBuf);
+	return 0;
+}
+
+INT	Show_Key3_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	Show_WepKey_Proc(pAd, 2, pBuf);
+	return 0;
+}
+
+INT	Show_Key4_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	Show_WepKey_Proc(pAd, 3, pBuf);
+	return 0;
+}
+
+INT	Show_WPAPSK_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	INT 	idx;
+	UCHAR	PMK[32] = {0};
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		NdisMoveMemory(PMK, pAd->StaCfg.PMK, 32);
+#endif // CONFIG_STA_SUPPORT //
+
+    sprintf(pBuf, "\tPMK = ");
+    for (idx = 0; idx < 32; idx++)
+        sprintf(pBuf+strlen(pBuf), "%02X", PMK[idx]);
+
+	return 0;
+}
+
diff --git a/drivers/staging/rt2860/common/cmm_sanity.c b/drivers/staging/rt2860/common/cmm_sanity.c
new file mode 100644
index 00000000000..b0f070d4313
--- /dev/null
+++ b/drivers/staging/rt2860/common/cmm_sanity.c
@@ -0,0 +1,1633 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	sanity.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John Chang  2004-09-01      add WMM support
+*/
+#include "../rt_config.h"
+
+
+extern UCHAR	CISCO_OUI[];
+
+extern UCHAR	WPA_OUI[];
+extern UCHAR	RSN_OUI[];
+extern UCHAR	WME_INFO_ELEM[];
+extern UCHAR	WME_PARM_ELEM[];
+extern UCHAR	Ccx2QosInfo[];
+extern UCHAR	RALINK_OUI[];
+extern UCHAR	BROADCOM_OUI[];
+extern UCHAR    WPS_OUI[];
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN MlmeAddBAReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2)
+{
+    PMLME_ADDBA_REQ_STRUCT   pInfo;
+
+    pInfo = (MLME_ADDBA_REQ_STRUCT *)Msg;
+
+    if ((MsgLen != sizeof(MLME_ADDBA_REQ_STRUCT)))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("MlmeAddBAReqSanity fail - message lenght not correct.\n"));
+        return FALSE;
+    }
+
+    if ((pInfo->Wcid >= MAX_LEN_OF_MAC_TABLE))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("MlmeAddBAReqSanity fail - The peer Mac is not associated yet.\n"));
+        return FALSE;
+    }
+
+    if ((pInfo->pAddr[0]&0x01) == 0x01)
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("MlmeAddBAReqSanity fail - broadcast address not support BA\n"));
+        return FALSE;
+    }
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN MlmeDelBAReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen)
+{
+	MLME_DELBA_REQ_STRUCT *pInfo;
+	pInfo = (MLME_DELBA_REQ_STRUCT *)Msg;
+
+    if ((MsgLen != sizeof(MLME_DELBA_REQ_STRUCT)))
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("MlmeDelBAReqSanity fail - message lenght not correct.\n"));
+        return FALSE;
+    }
+
+    if ((pInfo->Wcid >= MAX_LEN_OF_MAC_TABLE))
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("MlmeDelBAReqSanity fail - The peer Mac is not associated yet.\n"));
+        return FALSE;
+    }
+
+    if ((pInfo->TID & 0xf0))
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("MlmeDelBAReqSanity fail - The peer TID is incorrect.\n"));
+        return FALSE;
+    }
+
+	if (NdisEqualMemory(pAd->MacTab.Content[pInfo->Wcid].Addr, pInfo->Addr, MAC_ADDR_LEN) == 0)
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("MlmeDelBAReqSanity fail - the peer addr dosen't exist.\n"));
+        return FALSE;
+    }
+
+    return TRUE;
+}
+
+BOOLEAN PeerAddBAReqActionSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *pMsg,
+    IN ULONG MsgLen,
+	OUT PUCHAR pAddr2)
+{
+	PFRAME_802_11 pFrame = (PFRAME_802_11)pMsg;
+	PFRAME_ADDBA_REQ pAddFrame;
+	pAddFrame = (PFRAME_ADDBA_REQ)(pMsg);
+	if (MsgLen < (sizeof(FRAME_ADDBA_REQ)))
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("PeerAddBAReqActionSanity: ADDBA Request frame length size = %ld incorrect\n", MsgLen));
+		return FALSE;
+	}
+	// we support immediate BA.
+	*(USHORT *)(&pAddFrame->BaParm) = cpu2le16(*(USHORT *)(&pAddFrame->BaParm));
+	pAddFrame->TimeOutValue = cpu2le16(pAddFrame->TimeOutValue);
+	pAddFrame->BaStartSeq.word = cpu2le16(pAddFrame->BaStartSeq.word);
+
+	if (pAddFrame->BaParm.BAPolicy != IMMED_BA)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("PeerAddBAReqActionSanity: ADDBA Request Ba Policy[%d] not support\n", pAddFrame->BaParm.BAPolicy));
+		DBGPRINT(RT_DEBUG_ERROR,("ADDBA Request. tid=%x, Bufsize=%x, AMSDUSupported=%x \n", pAddFrame->BaParm.TID, pAddFrame->BaParm.BufSize, pAddFrame->BaParm.AMSDUSupported));
+		return FALSE;
+	}
+
+	// we support immediate BA.
+	if (pAddFrame->BaParm.TID &0xfff0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("PeerAddBAReqActionSanity: ADDBA Request incorrect TID = %d\n", pAddFrame->BaParm.TID));
+		return FALSE;
+	}
+	COPY_MAC_ADDR(pAddr2, pFrame->Hdr.Addr2);
+	return TRUE;
+}
+
+BOOLEAN PeerAddBARspActionSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *pMsg,
+    IN ULONG MsgLen)
+{
+	PFRAME_ADDBA_RSP pAddFrame;
+
+	pAddFrame = (PFRAME_ADDBA_RSP)(pMsg);
+	if (MsgLen < (sizeof(FRAME_ADDBA_RSP)))
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("PeerAddBARspActionSanity: ADDBA Response frame length size = %ld incorrect\n", MsgLen));
+		return FALSE;
+	}
+	// we support immediate BA.
+	*(USHORT *)(&pAddFrame->BaParm) = cpu2le16(*(USHORT *)(&pAddFrame->BaParm));
+	pAddFrame->StatusCode = cpu2le16(pAddFrame->StatusCode);
+	pAddFrame->TimeOutValue = cpu2le16(pAddFrame->TimeOutValue);
+
+	if (pAddFrame->BaParm.BAPolicy != IMMED_BA)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("PeerAddBAReqActionSanity: ADDBA Response Ba Policy[%d] not support\n", pAddFrame->BaParm.BAPolicy));
+		return FALSE;
+	}
+
+	// we support immediate BA.
+	if (pAddFrame->BaParm.TID &0xfff0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("PeerAddBARspActionSanity: ADDBA Response incorrect TID = %d\n", pAddFrame->BaParm.TID));
+		return FALSE;
+	}
+	return TRUE;
+
+}
+
+BOOLEAN PeerDelBAActionSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN UCHAR Wcid,
+    IN VOID *pMsg,
+    IN ULONG MsgLen )
+{
+	//PFRAME_802_11 pFrame = (PFRAME_802_11)pMsg;
+	PFRAME_DELBA_REQ  pDelFrame;
+	if (MsgLen != (sizeof(FRAME_DELBA_REQ)))
+		return FALSE;
+
+	if (Wcid >= MAX_LEN_OF_MAC_TABLE)
+		return FALSE;
+
+	pDelFrame = (PFRAME_DELBA_REQ)(pMsg);
+
+	*(USHORT *)(&pDelFrame->DelbaParm) = cpu2le16(*(USHORT *)(&pDelFrame->DelbaParm));
+	pDelFrame->ReasonCode = cpu2le16(pDelFrame->ReasonCode);
+
+	if (pDelFrame->DelbaParm.TID &0xfff0)
+		return FALSE;
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN PeerBeaconAndProbeRspSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    IN UCHAR  MsgChannel,
+    OUT PUCHAR pAddr2,
+    OUT PUCHAR pBssid,
+    OUT CHAR Ssid[],
+    OUT UCHAR *pSsidLen,
+    OUT UCHAR *pBssType,
+    OUT USHORT *pBeaconPeriod,
+    OUT UCHAR *pChannel,
+    OUT UCHAR *pNewChannel,
+    OUT LARGE_INTEGER *pTimestamp,
+    OUT CF_PARM *pCfParm,
+    OUT USHORT *pAtimWin,
+    OUT USHORT *pCapabilityInfo,
+    OUT UCHAR *pErp,
+    OUT UCHAR *pDtimCount,
+    OUT UCHAR *pDtimPeriod,
+    OUT UCHAR *pBcastFlag,
+    OUT UCHAR *pMessageToMe,
+    OUT UCHAR SupRate[],
+    OUT UCHAR *pSupRateLen,
+    OUT UCHAR ExtRate[],
+    OUT UCHAR *pExtRateLen,
+    OUT	UCHAR *pCkipFlag,
+    OUT	UCHAR *pAironetCellPowerLimit,
+    OUT PEDCA_PARM       pEdcaParm,
+    OUT PQBSS_LOAD_PARM  pQbssLoad,
+    OUT PQOS_CAPABILITY_PARM pQosCapability,
+    OUT ULONG *pRalinkIe,
+    OUT UCHAR		 *pHtCapabilityLen,
+#ifdef CONFIG_STA_SUPPORT
+    OUT UCHAR		 *pPreNHtCapabilityLen,
+#endif // CONFIG_STA_SUPPORT //
+    OUT HT_CAPABILITY_IE *pHtCapability,
+	OUT UCHAR		 *AddHtInfoLen,
+	OUT ADD_HT_INFO_IE *AddHtInfo,
+	OUT UCHAR *NewExtChannelOffset,		// Ht extension channel offset(above or below)
+    OUT USHORT *LengthVIE,
+    OUT	PNDIS_802_11_VARIABLE_IEs pVIE)
+{
+    CHAR				*Ptr;
+#ifdef CONFIG_STA_SUPPORT
+	CHAR 				TimLen;
+#endif // CONFIG_STA_SUPPORT //
+    PFRAME_802_11		pFrame;
+    PEID_STRUCT         pEid;
+    UCHAR				SubType;
+    UCHAR				Sanity;
+    //UCHAR				ECWMin, ECWMax;
+    //MAC_CSR9_STRUC		Csr9;
+    ULONG				Length = 0;
+
+	// For some 11a AP which didn't have DS_IE, we use two conditions to decide the channel
+	//	1. If the AP is 11n enabled, then check the control channel.
+	//	2. If the AP didn't have any info about channel, use the channel we received this frame as the channel. (May inaccuracy!!)
+	UCHAR			CtrlChannel = 0;
+
+    // Add for 3 necessary EID field check
+    Sanity = 0;
+
+    *pAtimWin = 0;
+    *pErp = 0;
+    *pDtimCount = 0;
+    *pDtimPeriod = 0;
+    *pBcastFlag = 0;
+    *pMessageToMe = 0;
+    *pExtRateLen = 0;
+    *pCkipFlag = 0;			        // Default of CkipFlag is 0
+    *pAironetCellPowerLimit = 0xFF;  // Default of AironetCellPowerLimit is 0xFF
+    *LengthVIE = 0;					// Set the length of VIE to init value 0
+    *pHtCapabilityLen = 0;					// Set the length of VIE to init value 0
+#ifdef CONFIG_STA_SUPPORT
+	if (pAd->OpMode == OPMODE_STA)
+		*pPreNHtCapabilityLen = 0;					// Set the length of VIE to init value 0
+#endif // CONFIG_STA_SUPPORT //
+    *AddHtInfoLen = 0;					// Set the length of VIE to init value 0
+    *pRalinkIe = 0;
+    *pNewChannel = 0;
+    *NewExtChannelOffset = 0xff;	//Default 0xff means no such IE
+    pCfParm->bValid = FALSE;        // default: no IE_CF found
+    pQbssLoad->bValid = FALSE;      // default: no IE_QBSS_LOAD found
+    pEdcaParm->bValid = FALSE;      // default: no IE_EDCA_PARAMETER found
+    pQosCapability->bValid = FALSE; // default: no IE_QOS_CAPABILITY found
+
+    pFrame = (PFRAME_802_11)Msg;
+
+    // get subtype from header
+    SubType = (UCHAR)pFrame->Hdr.FC.SubType;
+
+    // get Addr2 and BSSID from header
+    COPY_MAC_ADDR(pAddr2, pFrame->Hdr.Addr2);
+    COPY_MAC_ADDR(pBssid, pFrame->Hdr.Addr3);
+
+    Ptr = pFrame->Octet;
+    Length += LENGTH_802_11;
+
+    // get timestamp from payload and advance the pointer
+    NdisMoveMemory(pTimestamp, Ptr, TIMESTAMP_LEN);
+
+	pTimestamp->u.LowPart = cpu2le32(pTimestamp->u.LowPart);
+	pTimestamp->u.HighPart = cpu2le32(pTimestamp->u.HighPart);
+
+    Ptr += TIMESTAMP_LEN;
+    Length += TIMESTAMP_LEN;
+
+    // get beacon interval from payload and advance the pointer
+    NdisMoveMemory(pBeaconPeriod, Ptr, 2);
+    Ptr += 2;
+    Length += 2;
+
+    // get capability info from payload and advance the pointer
+    NdisMoveMemory(pCapabilityInfo, Ptr, 2);
+    Ptr += 2;
+    Length += 2;
+
+    if (CAP_IS_ESS_ON(*pCapabilityInfo))
+        *pBssType = BSS_INFRA;
+    else
+        *pBssType = BSS_ADHOC;
+
+    pEid = (PEID_STRUCT) Ptr;
+
+    // get variable fields from payload and advance the pointer
+    while ((Length + 2 + pEid->Len) <= MsgLen)
+    {
+        //
+        // Secure copy VIE to VarIE[MAX_VIE_LEN] didn't overflow.
+        //
+        if ((*LengthVIE + pEid->Len + 2) >= MAX_VIE_LEN)
+        {
+            DBGPRINT(RT_DEBUG_WARN, ("PeerBeaconAndProbeRspSanity - Variable IEs out of resource [len(=%d) > MAX_VIE_LEN(=%d)]\n",
+                    (*LengthVIE + pEid->Len + 2), MAX_VIE_LEN));
+            break;
+        }
+
+        switch(pEid->Eid)
+        {
+            case IE_SSID:
+                // Already has one SSID EID in this beacon, ignore the second one
+                if (Sanity & 0x1)
+                    break;
+                if(pEid->Len <= MAX_LEN_OF_SSID)
+                {
+                    NdisMoveMemory(Ssid, pEid->Octet, pEid->Len);
+                    *pSsidLen = pEid->Len;
+                    Sanity |= 0x1;
+                }
+                else
+                {
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAndProbeRspSanity - wrong IE_SSID (len=%d)\n",pEid->Len));
+                    return FALSE;
+                }
+                break;
+
+            case IE_SUPP_RATES:
+                if(pEid->Len <= MAX_LEN_OF_SUPPORTED_RATES)
+                {
+                    Sanity |= 0x2;
+                    NdisMoveMemory(SupRate, pEid->Octet, pEid->Len);
+                    *pSupRateLen = pEid->Len;
+
+                    // TODO: 2004-09-14 not a good design here, cause it exclude extra rates
+                    // from ScanTab. We should report as is. And filter out unsupported
+                    // rates in MlmeAux.
+                    // Check against the supported rates
+                    // RTMPCheckRates(pAd, SupRate, pSupRateLen);
+                }
+                else
+                {
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAndProbeRspSanity - wrong IE_SUPP_RATES (len=%d)\n",pEid->Len));
+                    return FALSE;
+                }
+                break;
+
+            case IE_HT_CAP:
+			if (pEid->Len >= SIZE_HT_CAP_IE)  //Note: allow extension.!!
+			{
+				NdisMoveMemory(pHtCapability, pEid->Octet, sizeof(HT_CAPABILITY_IE));
+				*pHtCapabilityLen = SIZE_HT_CAP_IE;	// Nnow we only support 26 bytes.
+
+				*(USHORT *)(&pHtCapability->HtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->HtCapInfo));
+				*(USHORT *)(&pHtCapability->ExtHtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->ExtHtCapInfo));
+
+#ifdef CONFIG_STA_SUPPORT
+				IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				{
+					*pPreNHtCapabilityLen = 0;	// Nnow we only support 26 bytes.
+
+					Ptr = (PUCHAR) pVIE;
+					NdisMoveMemory(Ptr + *LengthVIE, &pEid->Eid, pEid->Len + 2);
+					*LengthVIE += (pEid->Len + 2);
+				}
+#endif // CONFIG_STA_SUPPORT //
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_WARN, ("PeerBeaconAndProbeRspSanity - wrong IE_HT_CAP. pEid->Len = %d\n", pEid->Len));
+			}
+
+		break;
+            case IE_ADD_HT:
+			if (pEid->Len >= sizeof(ADD_HT_INFO_IE))
+			{
+				// This IE allows extension, but we can ignore extra bytes beyond our knowledge , so only
+				// copy first sizeof(ADD_HT_INFO_IE)
+				NdisMoveMemory(AddHtInfo, pEid->Octet, sizeof(ADD_HT_INFO_IE));
+				*AddHtInfoLen = SIZE_ADD_HT_INFO_IE;
+
+				CtrlChannel = AddHtInfo->ControlChan;
+
+				*(USHORT *)(&AddHtInfo->AddHtInfo2) = cpu2le16(*(USHORT *)(&AddHtInfo->AddHtInfo2));
+				*(USHORT *)(&AddHtInfo->AddHtInfo3) = cpu2le16(*(USHORT *)(&AddHtInfo->AddHtInfo3));
+
+#ifdef CONFIG_STA_SUPPORT
+				IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				{
+			                Ptr = (PUCHAR) pVIE;
+			                NdisMoveMemory(Ptr + *LengthVIE, &pEid->Eid, pEid->Len + 2);
+			                *LengthVIE += (pEid->Len + 2);
+				}
+#endif // CONFIG_STA_SUPPORT //
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_WARN, ("PeerBeaconAndProbeRspSanity - wrong IE_ADD_HT. \n"));
+			}
+
+		break;
+            case IE_SECONDARY_CH_OFFSET:
+			if (pEid->Len == 1)
+			{
+				*NewExtChannelOffset = pEid->Octet[0];
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_WARN, ("PeerBeaconAndProbeRspSanity - wrong IE_SECONDARY_CH_OFFSET. \n"));
+			}
+
+		break;
+            case IE_FH_PARM:
+                DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAndProbeRspSanity(IE_FH_PARM) \n"));
+                break;
+
+            case IE_DS_PARM:
+                if(pEid->Len == 1)
+                {
+                    *pChannel = *pEid->Octet;
+#ifdef CONFIG_STA_SUPPORT
+					IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+					{
+						if (ChannelSanity(pAd, *pChannel) == 0)
+						{
+
+							return FALSE;
+						}
+					}
+#endif // CONFIG_STA_SUPPORT //
+                    Sanity |= 0x4;
+                }
+                else
+                {
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAndProbeRspSanity - wrong IE_DS_PARM (len=%d)\n",pEid->Len));
+                    return FALSE;
+                }
+                break;
+
+            case IE_CF_PARM:
+                if(pEid->Len == 6)
+                {
+                    pCfParm->bValid = TRUE;
+                    pCfParm->CfpCount = pEid->Octet[0];
+                    pCfParm->CfpPeriod = pEid->Octet[1];
+                    pCfParm->CfpMaxDuration = pEid->Octet[2] + 256 * pEid->Octet[3];
+                    pCfParm->CfpDurRemaining = pEid->Octet[4] + 256 * pEid->Octet[5];
+                }
+                else
+                {
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAndProbeRspSanity - wrong IE_CF_PARM\n"));
+                    return FALSE;
+                }
+                break;
+
+            case IE_IBSS_PARM:
+                if(pEid->Len == 2)
+                {
+                    NdisMoveMemory(pAtimWin, pEid->Octet, pEid->Len);
+                }
+                else
+                {
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAndProbeRspSanity - wrong IE_IBSS_PARM\n"));
+                    return FALSE;
+                }
+                break;
+
+#ifdef CONFIG_STA_SUPPORT
+            case IE_TIM:
+                if(INFRA_ON(pAd) && SubType == SUBTYPE_BEACON)
+                {
+                    GetTimBit((PUCHAR)pEid, pAd->StaActive.Aid, &TimLen, pBcastFlag, pDtimCount, pDtimPeriod, pMessageToMe);
+                }
+                break;
+#endif // CONFIG_STA_SUPPORT //
+            case IE_CHANNEL_SWITCH_ANNOUNCEMENT:
+                if(pEid->Len == 3)
+                {
+                	*pNewChannel = pEid->Octet[1];	//extract new channel number
+                }
+                break;
+
+            // New for WPA
+            // CCX v2 has the same IE, we need to parse that too
+            // Wifi WMM use the same IE vale, need to parse that too
+            // case IE_WPA:
+            case IE_VENDOR_SPECIFIC:
+                // Check the OUI version, filter out non-standard usage
+                if (NdisEqualMemory(pEid->Octet, RALINK_OUI, 3) && (pEid->Len == 7))
+                {
+                    //*pRalinkIe = pEid->Octet[3];
+                    if (pEid->Octet[3] != 0)
+        				*pRalinkIe = pEid->Octet[3];
+        			else
+        				*pRalinkIe = 0xf0000000; // Set to non-zero value (can't set bit0-2) to represent this is Ralink Chip. So at linkup, we will set ralinkchip flag.
+                }
+#ifdef CONFIG_STA_SUPPORT
+#ifdef DOT11_N_SUPPORT
+		// This HT IE is before IEEE draft set HT IE value.2006-09-28 by Jan.
+
+                // Other vendors had production before IE_HT_CAP value is assigned. To backward support those old-firmware AP,
+                // Check broadcom-defiend pre-802.11nD1.0 OUI for HT related IE, including HT Capatilities IE and HT Information IE
+                else if ((*pHtCapabilityLen == 0) && NdisEqualMemory(pEid->Octet, PRE_N_HT_OUI, 3) && (pEid->Len >= 4) && (pAd->OpMode == OPMODE_STA))
+                {
+                    if ((pEid->Octet[3] == OUI_PREN_HT_CAP) && (pEid->Len >= 30) && (*pHtCapabilityLen == 0))
+                    {
+                        NdisMoveMemory(pHtCapability, &pEid->Octet[4], sizeof(HT_CAPABILITY_IE));
+                        *pPreNHtCapabilityLen = SIZE_HT_CAP_IE;
+                    }
+
+                    if ((pEid->Octet[3] == OUI_PREN_ADD_HT) && (pEid->Len >= 26))
+                    {
+                        NdisMoveMemory(AddHtInfo, &pEid->Octet[4], sizeof(ADD_HT_INFO_IE));
+                        *AddHtInfoLen = SIZE_ADD_HT_INFO_IE;
+                    }
+                }
+#endif // DOT11_N_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+                else if (NdisEqualMemory(pEid->Octet, WPA_OUI, 4))
+                {
+                    // Copy to pVIE which will report to microsoft bssid list.
+                    Ptr = (PUCHAR) pVIE;
+                    NdisMoveMemory(Ptr + *LengthVIE, &pEid->Eid, pEid->Len + 2);
+                    *LengthVIE += (pEid->Len + 2);
+                }
+                else if (NdisEqualMemory(pEid->Octet, WME_PARM_ELEM, 6) && (pEid->Len == 24))
+                {
+                    PUCHAR ptr;
+                    int i;
+
+                    // parsing EDCA parameters
+                    pEdcaParm->bValid          = TRUE;
+                    pEdcaParm->bQAck           = FALSE; // pEid->Octet[0] & 0x10;
+                    pEdcaParm->bQueueRequest   = FALSE; // pEid->Octet[0] & 0x20;
+                    pEdcaParm->bTxopRequest    = FALSE; // pEid->Octet[0] & 0x40;
+                    pEdcaParm->EdcaUpdateCount = pEid->Octet[6] & 0x0f;
+                    pEdcaParm->bAPSDCapable    = (pEid->Octet[6] & 0x80) ? 1 : 0;
+                    ptr = &pEid->Octet[8];
+                    for (i=0; i<4; i++)
+                    {
+                        UCHAR aci = (*ptr & 0x60) >> 5; // b5~6 is AC INDEX
+                        pEdcaParm->bACM[aci]  = (((*ptr) & 0x10) == 0x10);   // b5 is ACM
+                        pEdcaParm->Aifsn[aci] = (*ptr) & 0x0f;               // b0~3 is AIFSN
+                        pEdcaParm->Cwmin[aci] = *(ptr+1) & 0x0f;             // b0~4 is Cwmin
+                        pEdcaParm->Cwmax[aci] = *(ptr+1) >> 4;               // b5~8 is Cwmax
+                        pEdcaParm->Txop[aci]  = *(ptr+2) + 256 * (*(ptr+3)); // in unit of 32-us
+                        ptr += 4; // point to next AC
+                    }
+                }
+                else if (NdisEqualMemory(pEid->Octet, WME_INFO_ELEM, 6) && (pEid->Len == 7))
+                {
+                    // parsing EDCA parameters
+                    pEdcaParm->bValid          = TRUE;
+                    pEdcaParm->bQAck           = FALSE; // pEid->Octet[0] & 0x10;
+                    pEdcaParm->bQueueRequest   = FALSE; // pEid->Octet[0] & 0x20;
+                    pEdcaParm->bTxopRequest    = FALSE; // pEid->Octet[0] & 0x40;
+                    pEdcaParm->EdcaUpdateCount = pEid->Octet[6] & 0x0f;
+                    pEdcaParm->bAPSDCapable    = (pEid->Octet[6] & 0x80) ? 1 : 0;
+
+                    // use default EDCA parameter
+                    pEdcaParm->bACM[QID_AC_BE]  = 0;
+                    pEdcaParm->Aifsn[QID_AC_BE] = 3;
+                    pEdcaParm->Cwmin[QID_AC_BE] = CW_MIN_IN_BITS;
+                    pEdcaParm->Cwmax[QID_AC_BE] = CW_MAX_IN_BITS;
+                    pEdcaParm->Txop[QID_AC_BE]  = 0;
+
+                    pEdcaParm->bACM[QID_AC_BK]  = 0;
+                    pEdcaParm->Aifsn[QID_AC_BK] = 7;
+                    pEdcaParm->Cwmin[QID_AC_BK] = CW_MIN_IN_BITS;
+                    pEdcaParm->Cwmax[QID_AC_BK] = CW_MAX_IN_BITS;
+                    pEdcaParm->Txop[QID_AC_BK]  = 0;
+
+                    pEdcaParm->bACM[QID_AC_VI]  = 0;
+                    pEdcaParm->Aifsn[QID_AC_VI] = 2;
+                    pEdcaParm->Cwmin[QID_AC_VI] = CW_MIN_IN_BITS-1;
+                    pEdcaParm->Cwmax[QID_AC_VI] = CW_MAX_IN_BITS;
+                    pEdcaParm->Txop[QID_AC_VI]  = 96;   // AC_VI: 96*32us ~= 3ms
+
+                    pEdcaParm->bACM[QID_AC_VO]  = 0;
+                    pEdcaParm->Aifsn[QID_AC_VO] = 2;
+                    pEdcaParm->Cwmin[QID_AC_VO] = CW_MIN_IN_BITS-2;
+                    pEdcaParm->Cwmax[QID_AC_VO] = CW_MAX_IN_BITS-1;
+                    pEdcaParm->Txop[QID_AC_VO]  = 48;   // AC_VO: 48*32us ~= 1.5ms
+                }
+                break;
+
+            case IE_EXT_SUPP_RATES:
+                if (pEid->Len <= MAX_LEN_OF_SUPPORTED_RATES)
+                {
+                    NdisMoveMemory(ExtRate, pEid->Octet, pEid->Len);
+                    *pExtRateLen = pEid->Len;
+
+                    // TODO: 2004-09-14 not a good design here, cause it exclude extra rates
+                    // from ScanTab. We should report as is. And filter out unsupported
+                    // rates in MlmeAux.
+                    // Check against the supported rates
+                    // RTMPCheckRates(pAd, ExtRate, pExtRateLen);
+                }
+                break;
+
+            case IE_ERP:
+                if (pEid->Len == 1)
+                {
+                    *pErp = (UCHAR)pEid->Octet[0];
+                }
+                break;
+
+            case IE_AIRONET_CKIP:
+                // 0. Check Aironet IE length, it must be larger or equal to 28
+                // Cisco AP350 used length as 28
+                // Cisco AP12XX used length as 30
+                if (pEid->Len < (CKIP_NEGOTIATION_LENGTH - 2))
+                    break;
+
+                // 1. Copy CKIP flag byte to buffer for process
+                *pCkipFlag = *(pEid->Octet + 8);
+                break;
+
+            case IE_AP_TX_POWER:
+                // AP Control of Client Transmit Power
+                //0. Check Aironet IE length, it must be 6
+                if (pEid->Len != 0x06)
+                    break;
+
+                // Get cell power limit in dBm
+                if (NdisEqualMemory(pEid->Octet, CISCO_OUI, 3) == 1)
+                    *pAironetCellPowerLimit = *(pEid->Octet + 4);
+                break;
+
+            // WPA2 & 802.11i RSN
+            case IE_RSN:
+                // There is no OUI for version anymore, check the group cipher OUI before copying
+                if (RTMPEqualMemory(pEid->Octet + 2, RSN_OUI, 3))
+                {
+                    // Copy to pVIE which will report to microsoft bssid list.
+                    Ptr = (PUCHAR) pVIE;
+                    NdisMoveMemory(Ptr + *LengthVIE, &pEid->Eid, pEid->Len + 2);
+                    *LengthVIE += (pEid->Len + 2);
+                }
+                break;
+#ifdef CONFIG_STA_SUPPORT
+#ifdef EXT_BUILD_CHANNEL_LIST
+			case IE_COUNTRY:
+				Ptr = (PUCHAR) pVIE;
+                NdisMoveMemory(Ptr + *LengthVIE, &pEid->Eid, pEid->Len + 2);
+                *LengthVIE += (pEid->Len + 2);
+				break;
+#endif // EXT_BUILD_CHANNEL_LIST //
+#endif // CONFIG_STA_SUPPORT //
+
+            default:
+                break;
+        }
+
+        Length = Length + 2 + pEid->Len;  // Eid[1] + Len[1]+ content[Len]
+        pEid = (PEID_STRUCT)((UCHAR*)pEid + 2 + pEid->Len);
+    }
+
+    // For some 11a AP. it did not have the channel EID, patch here
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		UCHAR LatchRfChannel = MsgChannel;
+		if ((pAd->LatchRfRegs.Channel > 14) && ((Sanity & 0x4) == 0))
+		{
+			if (CtrlChannel != 0)
+				*pChannel = CtrlChannel;
+			else
+				*pChannel = LatchRfChannel;
+			Sanity |= 0x4;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	if (Sanity != 0x7)
+	{
+		DBGPRINT(RT_DEBUG_WARN, ("PeerBeaconAndProbeRspSanity - missing field, Sanity=0x%02x\n", Sanity));
+		return FALSE;
+	}
+	else
+	{
+		return TRUE;
+	}
+
+}
+
+#ifdef DOT11N_DRAFT3
+/*
+	==========================================================================
+	Description:
+		MLME message sanity check for some IE addressed  in 802.11n d3.03.
+	Return:
+		TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+BOOLEAN PeerBeaconAndProbeRspSanity2(
+	IN PRTMP_ADAPTER pAd,
+	IN VOID *Msg,
+	IN ULONG MsgLen,
+	OUT UCHAR 	*RegClass)
+{
+	CHAR				*Ptr;
+	PFRAME_802_11		pFrame;
+	PEID_STRUCT			pEid;
+	ULONG				Length = 0;
+
+	pFrame = (PFRAME_802_11)Msg;
+
+	*RegClass = 0;
+	Ptr = pFrame->Octet;
+	Length += LENGTH_802_11;
+
+	// get timestamp from payload and advance the pointer
+	Ptr += TIMESTAMP_LEN;
+	Length += TIMESTAMP_LEN;
+
+	// get beacon interval from payload and advance the pointer
+	Ptr += 2;
+	Length += 2;
+
+	// get capability info from payload and advance the pointer
+	Ptr += 2;
+	Length += 2;
+
+	pEid = (PEID_STRUCT) Ptr;
+
+	// get variable fields from payload and advance the pointer
+	while ((Length + 2 + pEid->Len) <= MsgLen)
+	{
+		switch(pEid->Eid)
+		{
+			case IE_SUPP_REG_CLASS:
+				if(pEid->Len > 0)
+				{
+					*RegClass = *pEid->Octet;
+				}
+				else
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAndProbeRspSanity - wrong IE_SSID (len=%d)\n",pEid->Len));
+					return FALSE;
+				}
+				break;
+		}
+
+		Length = Length + 2 + pEid->Len;  // Eid[1] + Len[1]+ content[Len]
+		pEid = (PEID_STRUCT)((UCHAR*)pEid + 2 + pEid->Len);
+	}
+
+	return TRUE;
+
+}
+#endif // DOT11N_DRAFT3 //
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+ */
+BOOLEAN MlmeScanReqSanity(
+	IN PRTMP_ADAPTER pAd,
+	IN VOID *Msg,
+	IN ULONG MsgLen,
+	OUT UCHAR *pBssType,
+	OUT CHAR Ssid[],
+	OUT UCHAR *pSsidLen,
+	OUT UCHAR *pScanType)
+{
+	MLME_SCAN_REQ_STRUCT *Info;
+
+	Info = (MLME_SCAN_REQ_STRUCT *)(Msg);
+	*pBssType = Info->BssType;
+	*pSsidLen = Info->SsidLen;
+	NdisMoveMemory(Ssid, Info->Ssid, *pSsidLen);
+	*pScanType = Info->ScanType;
+
+	if ((*pBssType == BSS_INFRA || *pBssType == BSS_ADHOC || *pBssType == BSS_ANY)
+		&& (*pScanType == SCAN_ACTIVE || *pScanType == SCAN_PASSIVE
+#ifdef CONFIG_STA_SUPPORT
+		|| *pScanType == SCAN_CISCO_PASSIVE || *pScanType == SCAN_CISCO_ACTIVE
+		|| *pScanType == SCAN_CISCO_CHANNEL_LOAD || *pScanType == SCAN_CISCO_NOISE
+#endif // CONFIG_STA_SUPPORT //
+		))
+	{
+		return TRUE;
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("MlmeScanReqSanity fail - wrong BssType or ScanType\n"));
+		return FALSE;
+	}
+}
+
+// IRQL = DISPATCH_LEVEL
+UCHAR ChannelSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN UCHAR channel)
+{
+    int i;
+
+    for (i = 0; i < pAd->ChannelListNum; i ++)
+    {
+        if (channel == pAd->ChannelList[i].Channel)
+            return 1;
+    }
+    return 0;
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN PeerDeauthSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT USHORT *pReason)
+{
+    PFRAME_802_11 pFrame = (PFRAME_802_11)Msg;
+
+    COPY_MAC_ADDR(pAddr2, pFrame->Hdr.Addr2);
+    NdisMoveMemory(pReason, &pFrame->Octet[0], 2);
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN PeerAuthSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr,
+    OUT USHORT *pAlg,
+    OUT USHORT *pSeq,
+    OUT USHORT *pStatus,
+    CHAR *pChlgText)
+{
+    PFRAME_802_11 pFrame = (PFRAME_802_11)Msg;
+
+    COPY_MAC_ADDR(pAddr,   pFrame->Hdr.Addr2);
+    NdisMoveMemory(pAlg,    &pFrame->Octet[0], 2);
+    NdisMoveMemory(pSeq,    &pFrame->Octet[2], 2);
+    NdisMoveMemory(pStatus, &pFrame->Octet[4], 2);
+
+    if ((*pAlg == Ndis802_11AuthModeOpen)
+#ifdef LEAP_SUPPORT
+      || (*pAlg == CISCO_AuthModeLEAP)
+#endif // LEAP_SUPPORT //
+      )
+    {
+        if (*pSeq == 1 || *pSeq == 2)
+        {
+            return TRUE;
+        }
+        else
+        {
+            DBGPRINT(RT_DEBUG_TRACE, ("PeerAuthSanity fail - wrong Seg#\n"));
+            return FALSE;
+        }
+    }
+    else if (*pAlg == Ndis802_11AuthModeShared)
+    {
+        if (*pSeq == 1 || *pSeq == 4)
+        {
+            return TRUE;
+        }
+        else if (*pSeq == 2 || *pSeq == 3)
+        {
+            NdisMoveMemory(pChlgText, &pFrame->Octet[8], CIPHER_TEXT_LEN);
+            return TRUE;
+        }
+        else
+        {
+            DBGPRINT(RT_DEBUG_TRACE, ("PeerAuthSanity fail - wrong Seg#\n"));
+            return FALSE;
+        }
+    }
+    else
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("PeerAuthSanity fail - wrong algorithm\n"));
+        return FALSE;
+    }
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+ */
+BOOLEAN MlmeAuthReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr,
+    OUT ULONG *pTimeout,
+    OUT USHORT *pAlg)
+{
+    MLME_AUTH_REQ_STRUCT *pInfo;
+
+    pInfo  = (MLME_AUTH_REQ_STRUCT *)Msg;
+    COPY_MAC_ADDR(pAddr, pInfo->Addr);
+    *pTimeout = pInfo->Timeout;
+    *pAlg = pInfo->Alg;
+
+    if (((*pAlg == Ndis802_11AuthModeShared) ||(*pAlg == Ndis802_11AuthModeOpen)
+#ifdef LEAP_SUPPORT
+     || (*pAlg == CISCO_AuthModeLEAP)
+#endif // LEAP_SUPPORT //
+     	) &&
+        ((*pAddr & 0x01) == 0))
+    {
+        return TRUE;
+    }
+    else
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("MlmeAuthReqSanity fail - wrong algorithm\n"));
+        return FALSE;
+    }
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN MlmeAssocReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pApAddr,
+    OUT USHORT *pCapabilityInfo,
+    OUT ULONG *pTimeout,
+    OUT USHORT *pListenIntv)
+{
+    MLME_ASSOC_REQ_STRUCT *pInfo;
+
+    pInfo = (MLME_ASSOC_REQ_STRUCT *)Msg;
+    *pTimeout = pInfo->Timeout;                             // timeout
+    COPY_MAC_ADDR(pApAddr, pInfo->Addr);                   // AP address
+    *pCapabilityInfo = pInfo->CapabilityInfo;               // capability info
+    *pListenIntv = pInfo->ListenIntv;
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN PeerDisassocSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT USHORT *pReason)
+{
+    PFRAME_802_11 pFrame = (PFRAME_802_11)Msg;
+
+    COPY_MAC_ADDR(pAddr2, pFrame->Hdr.Addr2);
+    NdisMoveMemory(pReason, &pFrame->Octet[0], 2);
+
+    return TRUE;
+}
+
+/*
+	========================================================================
+	Routine Description:
+		Sanity check NetworkType (11b, 11g or 11a)
+
+	Arguments:
+		pBss - Pointer to BSS table.
+
+	Return Value:
+        Ndis802_11DS .......(11b)
+        Ndis802_11OFDM24....(11g)
+        Ndis802_11OFDM5.....(11a)
+
+	IRQL = DISPATCH_LEVEL
+
+	========================================================================
+*/
+NDIS_802_11_NETWORK_TYPE NetworkTypeInUseSanity(
+    IN PBSS_ENTRY pBss)
+{
+	NDIS_802_11_NETWORK_TYPE	NetWorkType;
+	UCHAR						rate, i;
+
+	NetWorkType = Ndis802_11DS;
+
+	if (pBss->Channel <= 14)
+	{
+		//
+		// First check support Rate.
+		//
+		for (i = 0; i < pBss->SupRateLen; i++)
+		{
+			rate = pBss->SupRate[i] & 0x7f; // Mask out basic rate set bit
+			if ((rate == 2) || (rate == 4) || (rate == 11) || (rate == 22))
+			{
+				continue;
+			}
+			else
+			{
+				//
+				// Otherwise (even rate > 108) means Ndis802_11OFDM24
+				//
+				NetWorkType = Ndis802_11OFDM24;
+				break;
+			}
+		}
+
+		//
+		// Second check Extend Rate.
+		//
+		if (NetWorkType != Ndis802_11OFDM24)
+		{
+			for (i = 0; i < pBss->ExtRateLen; i++)
+			{
+				rate = pBss->SupRate[i] & 0x7f; // Mask out basic rate set bit
+				if ((rate == 2) || (rate == 4) || (rate == 11) || (rate == 22))
+				{
+					continue;
+				}
+				else
+				{
+					//
+					// Otherwise (even rate > 108) means Ndis802_11OFDM24
+					//
+					NetWorkType = Ndis802_11OFDM24;
+					break;
+				}
+			}
+		}
+	}
+	else
+	{
+		NetWorkType = Ndis802_11OFDM5;
+	}
+
+    if (pBss->HtCapabilityLen != 0)
+    {
+        if (NetWorkType == Ndis802_11OFDM5)
+            NetWorkType = Ndis802_11OFDM5_N;
+        else
+            NetWorkType = Ndis802_11OFDM24_N;
+    }
+
+	return NetWorkType;
+}
+
+/*
+    ==========================================================================
+    Description:
+        WPA message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+ */
+BOOLEAN PeerWpaMessageSanity(
+    IN 	PRTMP_ADAPTER 		pAd,
+    IN 	PEAPOL_PACKET 		pMsg,
+    IN 	ULONG 				MsgLen,
+    IN 	UCHAR				MsgType,
+    IN 	MAC_TABLE_ENTRY  	*pEntry)
+{
+	UCHAR			mic[LEN_KEY_DESC_MIC], digest[80], KEYDATA[MAX_LEN_OF_RSNIE];
+	BOOLEAN			bReplayDiff = FALSE;
+	BOOLEAN			bWPA2 = FALSE;
+	KEY_INFO		EapolKeyInfo;
+	UCHAR			GroupKeyIndex = 0;
+
+
+	NdisZeroMemory(mic, sizeof(mic));
+	NdisZeroMemory(digest, sizeof(digest));
+	NdisZeroMemory(KEYDATA, sizeof(KEYDATA));
+	NdisZeroMemory((PUCHAR)&EapolKeyInfo, sizeof(EapolKeyInfo));
+
+	NdisMoveMemory((PUCHAR)&EapolKeyInfo, (PUCHAR)&pMsg->KeyDesc.KeyInfo, sizeof(KEY_INFO));
+
+	*((USHORT *)&EapolKeyInfo) = cpu2le16(*((USHORT *)&EapolKeyInfo));
+
+	// Choose WPA2 or not
+	if ((pEntry->AuthMode == Ndis802_11AuthModeWPA2) || (pEntry->AuthMode == Ndis802_11AuthModeWPA2PSK))
+		bWPA2 = TRUE;
+
+	// 0. Check MsgType
+	if ((MsgType > EAPOL_GROUP_MSG_2) || (MsgType < EAPOL_PAIR_MSG_1))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("The message type is invalid(%d)! \n", MsgType));
+		return FALSE;
+	}
+
+	// 1. Replay counter check
+ 	if (MsgType == EAPOL_PAIR_MSG_1 || MsgType == EAPOL_PAIR_MSG_3 || MsgType == EAPOL_GROUP_MSG_1)	// For supplicant
+    {
+    	// First validate replay counter, only accept message with larger replay counter.
+		// Let equal pass, some AP start with all zero replay counter
+		UCHAR	ZeroReplay[LEN_KEY_DESC_REPLAY];
+
+        NdisZeroMemory(ZeroReplay, LEN_KEY_DESC_REPLAY);
+		if ((RTMPCompareMemory(pMsg->KeyDesc.ReplayCounter, pEntry->R_Counter, LEN_KEY_DESC_REPLAY) != 1) &&
+			(RTMPCompareMemory(pMsg->KeyDesc.ReplayCounter, ZeroReplay, LEN_KEY_DESC_REPLAY) != 0))
+    	{
+			bReplayDiff = TRUE;
+    	}
+ 	}
+	else if (MsgType == EAPOL_PAIR_MSG_2 || MsgType == EAPOL_PAIR_MSG_4 || MsgType == EAPOL_GROUP_MSG_2)	// For authenticator
+	{
+		// check Replay Counter coresponds to MSG from authenticator, otherwise discard
+    	if (!NdisEqualMemory(pMsg->KeyDesc.ReplayCounter, pEntry->R_Counter, LEN_KEY_DESC_REPLAY))
+    	{
+			bReplayDiff = TRUE;
+    	}
+	}
+
+	// Replay Counter different condition
+	if (bReplayDiff)
+	{
+		// send wireless event - for replay counter different
+		if (pAd->CommonCfg.bWirelessEvent)
+			RTMPSendWirelessEvent(pAd, IW_REPLAY_COUNTER_DIFF_EVENT_FLAG, pEntry->Addr, pEntry->apidx, 0);
+
+		if (MsgType < EAPOL_GROUP_MSG_1)
+		{
+           	DBGPRINT(RT_DEBUG_ERROR, ("Replay Counter Different in pairwise msg %d of 4-way handshake!\n", MsgType));
+		}
+		else
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("Replay Counter Different in group msg %d of 2-way handshake!\n", (MsgType - EAPOL_PAIR_MSG_4)));
+		}
+
+		hex_dump("Receive replay counter ", pMsg->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+		hex_dump("Current replay counter ", pEntry->R_Counter, LEN_KEY_DESC_REPLAY);
+        return FALSE;
+	}
+
+	// 2. Verify MIC except Pairwise Msg1
+	if (MsgType != EAPOL_PAIR_MSG_1)
+	{
+		UCHAR			rcvd_mic[LEN_KEY_DESC_MIC];
+
+		// Record the received MIC for check later
+		NdisMoveMemory(rcvd_mic, pMsg->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+		NdisZeroMemory(pMsg->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+
+        if (pEntry->WepStatus == Ndis802_11Encryption2Enabled)	// TKIP
+        {
+            hmac_md5(pEntry->PTK, LEN_EAP_MICK, (PUCHAR)pMsg, MsgLen, mic);
+        }
+        else if (pEntry->WepStatus == Ndis802_11Encryption3Enabled)	// AES
+        {
+            HMAC_SHA1((PUCHAR)pMsg, MsgLen, pEntry->PTK, LEN_EAP_MICK, digest);
+            NdisMoveMemory(mic, digest, LEN_KEY_DESC_MIC);
+        }
+
+        if (!NdisEqualMemory(rcvd_mic, mic, LEN_KEY_DESC_MIC))
+        {
+			// send wireless event - for MIC different
+			if (pAd->CommonCfg.bWirelessEvent)
+				RTMPSendWirelessEvent(pAd, IW_MIC_DIFF_EVENT_FLAG, pEntry->Addr, pEntry->apidx, 0);
+
+			if (MsgType < EAPOL_GROUP_MSG_1)
+			{
+            	DBGPRINT(RT_DEBUG_ERROR, ("MIC Different in pairwise msg %d of 4-way handshake!\n", MsgType));
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_ERROR, ("MIC Different in group msg %d of 2-way handshake!\n", (MsgType - EAPOL_PAIR_MSG_4)));
+			}
+
+			hex_dump("Received MIC", rcvd_mic, LEN_KEY_DESC_MIC);
+			hex_dump("Desired  MIC", mic, LEN_KEY_DESC_MIC);
+
+			return FALSE;
+        }
+	}
+
+	// Extract the context of the Key Data field if it exist
+	// The field in pairwise_msg_2_WPA1(WPA2) & pairwise_msg_3_WPA1 is un-encrypted.
+	// The field in group_msg_1_WPA1(WPA2) & pairwise_msg_3_WPA2 is encrypted.
+	if (pMsg->KeyDesc.KeyDataLen[1] > 0)
+	{
+		// Decrypt this field
+		if ((MsgType == EAPOL_PAIR_MSG_3 && bWPA2) || (MsgType == EAPOL_GROUP_MSG_1))
+		{
+			if(pEntry->WepStatus == Ndis802_11Encryption3Enabled)
+			{
+				// AES
+				AES_GTK_KEY_UNWRAP(&pEntry->PTK[16], KEYDATA, pMsg->KeyDesc.KeyDataLen[1],pMsg->KeyDesc.KeyData);
+			}
+			else
+			{
+				INT 	i;
+				UCHAR   Key[32];
+				// Decrypt TKIP GTK
+				// Construct 32 bytes RC4 Key
+				NdisMoveMemory(Key, pMsg->KeyDesc.KeyIv, 16);
+				NdisMoveMemory(&Key[16], &pEntry->PTK[16], 16);
+				ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, Key, 32);
+				//discard first 256 bytes
+				for(i = 0; i < 256; i++)
+					ARCFOUR_BYTE(&pAd->PrivateInfo.WEPCONTEXT);
+				// Decrypt GTK. Becareful, there is no ICV to check the result is correct or not
+				ARCFOUR_DECRYPT(&pAd->PrivateInfo.WEPCONTEXT, KEYDATA, pMsg->KeyDesc.KeyData, pMsg->KeyDesc.KeyDataLen[1]);
+			}
+
+			if (!bWPA2 && (MsgType == EAPOL_GROUP_MSG_1))
+				GroupKeyIndex = EapolKeyInfo.KeyIndex;
+
+		}
+		else if ((MsgType == EAPOL_PAIR_MSG_2) || (MsgType == EAPOL_PAIR_MSG_3 && !bWPA2))
+		{
+			NdisMoveMemory(KEYDATA, pMsg->KeyDesc.KeyData, pMsg->KeyDesc.KeyDataLen[1]);
+		}
+		else
+		{
+
+			return TRUE;
+		}
+
+		// Parse Key Data field to
+		// 1. verify RSN IE for pairwise_msg_2_WPA1(WPA2) ,pairwise_msg_3_WPA1(WPA2)
+		// 2. verify KDE format for pairwise_msg_3_WPA2, group_msg_1_WPA2
+		// 3. update shared key for pairwise_msg_3_WPA2, group_msg_1_WPA1(WPA2)
+		if (!RTMPParseEapolKeyData(pAd, KEYDATA, pMsg->KeyDesc.KeyDataLen[1], GroupKeyIndex, MsgType, bWPA2, pEntry))
+		{
+			return FALSE;
+		}
+	}
+
+	return TRUE;
+
+}
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+BOOLEAN MlmeDlsReqSanity(
+	IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PRT_802_11_DLS *pDLS,
+    OUT PUSHORT pReason)
+{
+	MLME_DLS_REQ_STRUCT *pInfo;
+
+    pInfo = (MLME_DLS_REQ_STRUCT *)Msg;
+
+	*pDLS = pInfo->pDLS;
+	*pReason = pInfo->Reason;
+
+	return TRUE;
+}
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef QOS_DLS_SUPPORT
+BOOLEAN PeerDlsReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pDA,
+    OUT PUCHAR pSA,
+    OUT USHORT *pCapabilityInfo,
+    OUT USHORT *pDlsTimeout,
+    OUT UCHAR *pRatesLen,
+    OUT UCHAR Rates[],
+	OUT UCHAR *pHtCapabilityLen,
+    OUT HT_CAPABILITY_IE *pHtCapability)
+{
+	CHAR            *Ptr;
+    PFRAME_802_11	Fr = (PFRAME_802_11)Msg;
+	PEID_STRUCT  eid_ptr;
+
+    // to prevent caller from using garbage output value
+    *pCapabilityInfo	= 0;
+    *pDlsTimeout	= 0;
+	*pHtCapabilityLen = 0;
+
+    Ptr = Fr->Octet;
+
+	// offset to destination MAC address (Category and Action field)
+    Ptr += 2;
+
+    // get DA from payload and advance the pointer
+    NdisMoveMemory(pDA, Ptr, MAC_ADDR_LEN);
+    Ptr += MAC_ADDR_LEN;
+
+    // get SA from payload and advance the pointer
+    NdisMoveMemory(pSA, Ptr, MAC_ADDR_LEN);
+    Ptr += MAC_ADDR_LEN;
+
+    // get capability info from payload and advance the pointer
+    NdisMoveMemory(pCapabilityInfo, Ptr, 2);
+    Ptr += 2;
+
+    // get capability info from payload and advance the pointer
+    NdisMoveMemory(pDlsTimeout, Ptr, 2);
+    Ptr += 2;
+
+	// Category and Action field + DA + SA + capability + Timeout
+	eid_ptr = (PEID_STRUCT) &Fr->Octet[18];
+
+	while (((UCHAR*)eid_ptr + eid_ptr->Len + 1) < ((UCHAR*)Fr + MsgLen))
+	{
+		switch(eid_ptr->Eid)
+		{
+			case IE_SUPP_RATES:
+                if ((eid_ptr->Len <= MAX_LEN_OF_SUPPORTED_RATES) && (eid_ptr->Len > 0))
+                {
+                    NdisMoveMemory(Rates, eid_ptr->Octet, eid_ptr->Len);
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsReqSanity - IE_SUPP_RATES., Len=%d. Rates[0]=%x\n",eid_ptr->Len, Rates[0]));
+                    DBGPRINT(RT_DEBUG_TRACE, ("Rates[1]=%x %x %x %x %x %x %x\n", Rates[1], Rates[2], Rates[3], Rates[4], Rates[5], Rates[6], Rates[7]));
+                    *pRatesLen = eid_ptr->Len;
+                }
+                else
+                {
+                    *pRatesLen = 8;
+					Rates[0] = 0x82;
+					Rates[1] = 0x84;
+					Rates[2] = 0x8b;
+					Rates[3] = 0x96;
+					Rates[4] = 0x12;
+					Rates[5] = 0x24;
+					Rates[6] = 0x48;
+					Rates[7] = 0x6c;
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsReqSanity - wrong IE_SUPP_RATES., Len=%d\n",eid_ptr->Len));
+                }
+				break;
+
+			case IE_EXT_SUPP_RATES:
+                if (eid_ptr->Len + *pRatesLen <= MAX_LEN_OF_SUPPORTED_RATES)
+                {
+                    NdisMoveMemory(&Rates[*pRatesLen], eid_ptr->Octet, eid_ptr->Len);
+                    *pRatesLen = (*pRatesLen) + eid_ptr->Len;
+                }
+                else
+                {
+                    NdisMoveMemory(&Rates[*pRatesLen], eid_ptr->Octet, MAX_LEN_OF_SUPPORTED_RATES - (*pRatesLen));
+                    *pRatesLen = MAX_LEN_OF_SUPPORTED_RATES;
+                }
+				break;
+
+			case IE_HT_CAP:
+				if (eid_ptr->Len >= sizeof(HT_CAPABILITY_IE))
+				{
+					NdisMoveMemory(pHtCapability, eid_ptr->Octet, sizeof(HT_CAPABILITY_IE));
+
+					*(USHORT *)(&pHtCapability->HtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->HtCapInfo));
+					*(USHORT *)(&pHtCapability->ExtHtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->ExtHtCapInfo));
+					*pHtCapabilityLen = sizeof(HT_CAPABILITY_IE);
+
+					DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsReqSanity - IE_HT_CAP\n"));
+				}
+				else
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsReqSanity - wrong IE_HT_CAP.eid_ptr->Len = %d\n", eid_ptr->Len));
+				}
+				break;
+
+			default:
+				break;
+		}
+
+		eid_ptr = (PEID_STRUCT)((UCHAR*)eid_ptr + 2 + eid_ptr->Len);
+	}
+
+    return TRUE;
+}
+
+BOOLEAN PeerDlsRspSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pDA,
+    OUT PUCHAR pSA,
+    OUT USHORT *pCapabilityInfo,
+    OUT USHORT *pStatus,
+    OUT UCHAR *pRatesLen,
+    OUT UCHAR Rates[],
+    OUT UCHAR *pHtCapabilityLen,
+    OUT HT_CAPABILITY_IE *pHtCapability)
+{
+    CHAR            *Ptr;
+    PFRAME_802_11	Fr = (PFRAME_802_11)Msg;
+	PEID_STRUCT  eid_ptr;
+
+    // to prevent caller from using garbage output value
+    *pStatus		= 0;
+    *pCapabilityInfo	= 0;
+	*pHtCapabilityLen = 0;
+
+    Ptr = Fr->Octet;
+
+	// offset to destination MAC address (Category and Action field)
+    Ptr += 2;
+
+	// get status code from payload and advance the pointer
+    NdisMoveMemory(pStatus, Ptr, 2);
+    Ptr += 2;
+
+    // get DA from payload and advance the pointer
+    NdisMoveMemory(pDA, Ptr, MAC_ADDR_LEN);
+    Ptr += MAC_ADDR_LEN;
+
+    // get SA from payload and advance the pointer
+    NdisMoveMemory(pSA, Ptr, MAC_ADDR_LEN);
+    Ptr += MAC_ADDR_LEN;
+
+	if (pStatus == 0)
+	{
+	    // get capability info from payload and advance the pointer
+	    NdisMoveMemory(pCapabilityInfo, Ptr, 2);
+	    Ptr += 2;
+	}
+
+	// Category and Action field + status code + DA + SA + capability
+	eid_ptr = (PEID_STRUCT) &Fr->Octet[18];
+
+	while (((UCHAR*)eid_ptr + eid_ptr->Len + 1) < ((UCHAR*)Fr + MsgLen))
+	{
+		switch(eid_ptr->Eid)
+		{
+			case IE_SUPP_RATES:
+                if ((eid_ptr->Len <= MAX_LEN_OF_SUPPORTED_RATES) && (eid_ptr->Len > 0))
+                {
+                    NdisMoveMemory(Rates, eid_ptr->Octet, eid_ptr->Len);
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsRspSanity - IE_SUPP_RATES., Len=%d. Rates[0]=%x\n",eid_ptr->Len, Rates[0]));
+                    DBGPRINT(RT_DEBUG_TRACE, ("Rates[1]=%x %x %x %x %x %x %x\n", Rates[1], Rates[2], Rates[3], Rates[4], Rates[5], Rates[6], Rates[7]));
+                    *pRatesLen = eid_ptr->Len;
+                }
+                else
+                {
+                    *pRatesLen = 8;
+					Rates[0] = 0x82;
+					Rates[1] = 0x84;
+					Rates[2] = 0x8b;
+					Rates[3] = 0x96;
+					Rates[4] = 0x12;
+					Rates[5] = 0x24;
+					Rates[6] = 0x48;
+					Rates[7] = 0x6c;
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsRspSanity - wrong IE_SUPP_RATES., Len=%d\n",eid_ptr->Len));
+                }
+				break;
+
+			case IE_EXT_SUPP_RATES:
+                if (eid_ptr->Len + *pRatesLen <= MAX_LEN_OF_SUPPORTED_RATES)
+                {
+                    NdisMoveMemory(&Rates[*pRatesLen], eid_ptr->Octet, eid_ptr->Len);
+                    *pRatesLen = (*pRatesLen) + eid_ptr->Len;
+                }
+                else
+                {
+                    NdisMoveMemory(&Rates[*pRatesLen], eid_ptr->Octet, MAX_LEN_OF_SUPPORTED_RATES - (*pRatesLen));
+                    *pRatesLen = MAX_LEN_OF_SUPPORTED_RATES;
+                }
+				break;
+
+			case IE_HT_CAP:
+				if (eid_ptr->Len >= sizeof(HT_CAPABILITY_IE))
+				{
+					NdisMoveMemory(pHtCapability, eid_ptr->Octet, sizeof(HT_CAPABILITY_IE));
+
+					*(USHORT *)(&pHtCapability->HtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->HtCapInfo));
+					*(USHORT *)(&pHtCapability->ExtHtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->ExtHtCapInfo));
+					*pHtCapabilityLen = sizeof(HT_CAPABILITY_IE);
+
+					DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsRspSanity - IE_HT_CAP\n"));
+				}
+				else
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsRspSanity - wrong IE_HT_CAP.eid_ptr->Len = %d\n", eid_ptr->Len));
+				}
+				break;
+
+			default:
+				break;
+		}
+
+		eid_ptr = (PEID_STRUCT)((UCHAR*)eid_ptr + 2 + eid_ptr->Len);
+	}
+
+    return TRUE;
+}
+
+BOOLEAN PeerDlsTearDownSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pDA,
+    OUT PUCHAR pSA,
+    OUT USHORT *pReason)
+{
+    CHAR            *Ptr;
+    PFRAME_802_11	Fr = (PFRAME_802_11)Msg;
+
+    // to prevent caller from using garbage output value
+    *pReason	= 0;
+
+    Ptr = Fr->Octet;
+
+	// offset to destination MAC address (Category and Action field)
+    Ptr += 2;
+
+    // get DA from payload and advance the pointer
+    NdisMoveMemory(pDA, Ptr, MAC_ADDR_LEN);
+    Ptr += MAC_ADDR_LEN;
+
+    // get SA from payload and advance the pointer
+    NdisMoveMemory(pSA, Ptr, MAC_ADDR_LEN);
+    Ptr += MAC_ADDR_LEN;
+
+	// get reason code from payload and advance the pointer
+    NdisMoveMemory(pReason, Ptr, 2);
+    Ptr += 2;
+
+    return TRUE;
+}
+#endif // QOS_DLS_SUPPORT //
+
diff --git a/drivers/staging/rt2860/common/cmm_sync.c b/drivers/staging/rt2860/common/cmm_sync.c
new file mode 100644
index 00000000000..40e4109118e
--- /dev/null
+++ b/drivers/staging/rt2860/common/cmm_sync.c
@@ -0,0 +1,702 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	sync.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John Chang	2004-09-01      modified for rt2561/2661
+*/
+#include "../rt_config.h"
+
+// 2.4 Ghz channel plan index in the TxPower arrays.
+#define	BG_BAND_REGION_0_START	0			// 1,2,3,4,5,6,7,8,9,10,11
+#define	BG_BAND_REGION_0_SIZE	11
+#define	BG_BAND_REGION_1_START	0			// 1,2,3,4,5,6,7,8,9,10,11,12,13
+#define	BG_BAND_REGION_1_SIZE	13
+#define	BG_BAND_REGION_2_START	9			// 10,11
+#define	BG_BAND_REGION_2_SIZE	2
+#define	BG_BAND_REGION_3_START	9			// 10,11,12,13
+#define	BG_BAND_REGION_3_SIZE	4
+#define	BG_BAND_REGION_4_START	13			// 14
+#define	BG_BAND_REGION_4_SIZE	1
+#define	BG_BAND_REGION_5_START	0			// 1,2,3,4,5,6,7,8,9,10,11,12,13,14
+#define	BG_BAND_REGION_5_SIZE	14
+#define	BG_BAND_REGION_6_START	2			// 3,4,5,6,7,8,9
+#define	BG_BAND_REGION_6_SIZE	7
+#define	BG_BAND_REGION_7_START	4			// 5,6,7,8,9,10,11,12,13
+#define	BG_BAND_REGION_7_SIZE	9
+#define	BG_BAND_REGION_31_START	0			// 1,2,3,4,5,6,7,8,9,10,11,12,13,14
+#define	BG_BAND_REGION_31_SIZE	14
+
+// 5 Ghz channel plan index in the TxPower arrays.
+UCHAR A_BAND_REGION_0_CHANNEL_LIST[]={36, 40, 44, 48, 52, 56, 60, 64, 149, 153, 157, 161, 165};
+UCHAR A_BAND_REGION_1_CHANNEL_LIST[]={36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140};
+UCHAR A_BAND_REGION_2_CHANNEL_LIST[]={36, 40, 44, 48, 52, 56, 60, 64};
+UCHAR A_BAND_REGION_3_CHANNEL_LIST[]={52, 56, 60, 64, 149, 153, 157, 161};
+UCHAR A_BAND_REGION_4_CHANNEL_LIST[]={149, 153, 157, 161, 165};
+UCHAR A_BAND_REGION_5_CHANNEL_LIST[]={149, 153, 157, 161};
+UCHAR A_BAND_REGION_6_CHANNEL_LIST[]={36, 40, 44, 48};
+UCHAR A_BAND_REGION_7_CHANNEL_LIST[]={36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 149, 153, 157, 161, 165};
+UCHAR A_BAND_REGION_8_CHANNEL_LIST[]={52, 56, 60, 64};
+UCHAR A_BAND_REGION_9_CHANNEL_LIST[]={36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 132, 136, 140, 149, 153, 157, 161, 165};
+UCHAR A_BAND_REGION_10_CHANNEL_LIST[]={36, 40, 44, 48, 149, 153, 157, 161, 165};
+UCHAR A_BAND_REGION_11_CHANNEL_LIST[]={36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 149, 153, 157, 161};
+
+//BaSizeArray follows the 802.11n definition as MaxRxFactor.  2^(13+factor) bytes. When factor =0, it's about Ba buffer size =8.
+UCHAR BaSizeArray[4] = {8,16,32,64};
+
+/*
+	==========================================================================
+	Description:
+		Update StaCfg->ChannelList[] according to 1) Country Region 2) RF IC type,
+		and 3) PHY-mode user selected.
+		The outcome is used by driver when doing site survey.
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID BuildChannelList(
+	IN PRTMP_ADAPTER pAd)
+{
+	UCHAR i, j, index=0, num=0;
+	PUCHAR	pChannelList = NULL;
+
+	NdisZeroMemory(pAd->ChannelList, MAX_NUM_OF_CHANNELS * sizeof(CHANNEL_TX_POWER));
+
+	// if not 11a-only mode, channel list starts from 2.4Ghz band
+	if ((pAd->CommonCfg.PhyMode != PHY_11A)
+#ifdef DOT11_N_SUPPORT
+		&& (pAd->CommonCfg.PhyMode != PHY_11AN_MIXED) && (pAd->CommonCfg.PhyMode != PHY_11N_5G)
+#endif // DOT11_N_SUPPORT //
+	)
+	{
+		switch (pAd->CommonCfg.CountryRegion  & 0x7f)
+		{
+			case REGION_0_BG_BAND:	// 1 -11
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_0_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_0_SIZE);
+				index += BG_BAND_REGION_0_SIZE;
+				break;
+			case REGION_1_BG_BAND:	// 1 - 13
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_1_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_1_SIZE);
+				index += BG_BAND_REGION_1_SIZE;
+				break;
+			case REGION_2_BG_BAND:	// 10 - 11
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_2_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_2_SIZE);
+				index += BG_BAND_REGION_2_SIZE;
+				break;
+			case REGION_3_BG_BAND:	// 10 - 13
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_3_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_3_SIZE);
+				index += BG_BAND_REGION_3_SIZE;
+				break;
+			case REGION_4_BG_BAND:	// 14
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_4_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_4_SIZE);
+				index += BG_BAND_REGION_4_SIZE;
+				break;
+			case REGION_5_BG_BAND:	// 1 - 14
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_5_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_5_SIZE);
+				index += BG_BAND_REGION_5_SIZE;
+				break;
+			case REGION_6_BG_BAND:	// 3 - 9
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_6_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_6_SIZE);
+				index += BG_BAND_REGION_6_SIZE;
+				break;
+			case REGION_7_BG_BAND:  // 5 - 13
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_7_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_7_SIZE);
+				index += BG_BAND_REGION_7_SIZE;
+				break;
+			case REGION_31_BG_BAND:	// 1 - 14
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_31_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_31_SIZE);
+				index += BG_BAND_REGION_31_SIZE;
+				break;
+			default:            // Error. should never happen
+				break;
+		}
+		for (i=0; i<index; i++)
+			pAd->ChannelList[i].MaxTxPwr = 20;
+	}
+
+	if ((pAd->CommonCfg.PhyMode == PHY_11A) || (pAd->CommonCfg.PhyMode == PHY_11ABG_MIXED)
+#ifdef DOT11_N_SUPPORT
+		|| (pAd->CommonCfg.PhyMode == PHY_11ABGN_MIXED) || (pAd->CommonCfg.PhyMode == PHY_11AN_MIXED)
+		|| (pAd->CommonCfg.PhyMode == PHY_11AGN_MIXED) || (pAd->CommonCfg.PhyMode == PHY_11N_5G)
+#endif // DOT11_N_SUPPORT //
+	)
+	{
+		switch (pAd->CommonCfg.CountryRegionForABand & 0x7f)
+		{
+			case REGION_0_A_BAND:
+				num = sizeof(A_BAND_REGION_0_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_0_CHANNEL_LIST;
+				break;
+			case REGION_1_A_BAND:
+				num = sizeof(A_BAND_REGION_1_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_1_CHANNEL_LIST;
+				break;
+			case REGION_2_A_BAND:
+				num = sizeof(A_BAND_REGION_2_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_2_CHANNEL_LIST;
+				break;
+			case REGION_3_A_BAND:
+				num = sizeof(A_BAND_REGION_3_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_3_CHANNEL_LIST;
+				break;
+			case REGION_4_A_BAND:
+				num = sizeof(A_BAND_REGION_4_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_4_CHANNEL_LIST;
+				break;
+			case REGION_5_A_BAND:
+				num = sizeof(A_BAND_REGION_5_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_5_CHANNEL_LIST;
+				break;
+			case REGION_6_A_BAND:
+				num = sizeof(A_BAND_REGION_6_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_6_CHANNEL_LIST;
+				break;
+			case REGION_7_A_BAND:
+				num = sizeof(A_BAND_REGION_7_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_7_CHANNEL_LIST;
+				break;
+			case REGION_8_A_BAND:
+				num = sizeof(A_BAND_REGION_8_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_8_CHANNEL_LIST;
+				break;
+			case REGION_9_A_BAND:
+				num = sizeof(A_BAND_REGION_9_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_9_CHANNEL_LIST;
+				break;
+
+			case REGION_10_A_BAND:
+				num = sizeof(A_BAND_REGION_10_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_10_CHANNEL_LIST;
+				break;
+
+			case REGION_11_A_BAND:
+				num = sizeof(A_BAND_REGION_11_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_11_CHANNEL_LIST;
+				break;
+
+			default:            // Error. should never happen
+				DBGPRINT(RT_DEBUG_WARN,("countryregion=%d not support", pAd->CommonCfg.CountryRegionForABand));
+				break;
+		}
+
+		if (num != 0)
+		{
+			UCHAR RadarCh[15]={52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140};
+			for (i=0; i<num; i++)
+			{
+				for (j=0; j<MAX_NUM_OF_CHANNELS; j++)
+				{
+					if (pChannelList[i] == pAd->TxPower[j].Channel)
+						NdisMoveMemory(&pAd->ChannelList[index+i], &pAd->TxPower[j], sizeof(CHANNEL_TX_POWER));
+					}
+				for (j=0; j<15; j++)
+				{
+					if (pChannelList[i] == RadarCh[j])
+						pAd->ChannelList[index+i].DfsReq = TRUE;
+				}
+				pAd->ChannelList[index+i].MaxTxPwr = 20;
+			}
+			index += num;
+		}
+	}
+
+	pAd->ChannelListNum = index;
+	DBGPRINT(RT_DEBUG_TRACE,("country code=%d/%d, RFIC=%d, PHY mode=%d, support %d channels\n",
+		pAd->CommonCfg.CountryRegion, pAd->CommonCfg.CountryRegionForABand, pAd->RfIcType, pAd->CommonCfg.PhyMode, pAd->ChannelListNum));
+#ifdef DBG
+	for (i=0;i<pAd->ChannelListNum;i++)
+	{
+		DBGPRINT_RAW(RT_DEBUG_TRACE,("BuildChannel # %d :: Pwr0 = %d, Pwr1 =%d, \n ", pAd->ChannelList[i].Channel, pAd->ChannelList[i].Power, pAd->ChannelList[i].Power2));
+	}
+#endif
+}
+
+/*
+	==========================================================================
+	Description:
+		This routine return the first channel number according to the country
+		code selection and RF IC selection (signal band or dual band). It is called
+		whenever driver need to start a site survey of all supported channels.
+	Return:
+		ch - the first channel number of current country code setting
+
+	IRQL = PASSIVE_LEVEL
+
+	==========================================================================
+ */
+UCHAR FirstChannel(
+	IN PRTMP_ADAPTER pAd)
+{
+	return pAd->ChannelList[0].Channel;
+}
+
+/*
+	==========================================================================
+	Description:
+		This routine returns the next channel number. This routine is called
+		during driver need to start a site survey of all supported channels.
+	Return:
+		next_channel - the next channel number valid in current country code setting.
+	Note:
+		return 0 if no more next channel
+	==========================================================================
+ */
+UCHAR NextChannel(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR channel)
+{
+	int i;
+	UCHAR next_channel = 0;
+
+	for (i = 0; i < (pAd->ChannelListNum - 1); i++)
+		if (channel == pAd->ChannelList[i].Channel)
+		{
+			next_channel = pAd->ChannelList[i+1].Channel;
+			break;
+	}
+	return next_channel;
+}
+
+/*
+	==========================================================================
+	Description:
+		This routine is for Cisco Compatible Extensions 2.X
+		Spec31. AP Control of Client Transmit Power
+	Return:
+		None
+	Note:
+	   Required by Aironet dBm(mW)
+		   0dBm(1mW),   1dBm(5mW), 13dBm(20mW), 15dBm(30mW),
+		  17dBm(50mw), 20dBm(100mW)
+
+	   We supported
+		   3dBm(Lowest), 6dBm(10%), 9dBm(25%), 12dBm(50%),
+		  14dBm(75%),   15dBm(100%)
+
+		The client station's actual transmit power shall be within +/- 5dB of
+		the minimum value or next lower value.
+	==========================================================================
+ */
+VOID ChangeToCellPowerLimit(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR         AironetCellPowerLimit)
+{
+	//valud 0xFF means that hasn't found power limit information
+	//from the AP's Beacon/Probe response.
+	if (AironetCellPowerLimit == 0xFF)
+		return;
+
+	if (AironetCellPowerLimit < 6) //Used Lowest Power Percentage.
+		pAd->CommonCfg.TxPowerPercentage = 6;
+	else if (AironetCellPowerLimit < 9)
+		pAd->CommonCfg.TxPowerPercentage = 10;
+	else if (AironetCellPowerLimit < 12)
+		pAd->CommonCfg.TxPowerPercentage = 25;
+	else if (AironetCellPowerLimit < 14)
+		pAd->CommonCfg.TxPowerPercentage = 50;
+	else if (AironetCellPowerLimit < 15)
+		pAd->CommonCfg.TxPowerPercentage = 75;
+	else
+		pAd->CommonCfg.TxPowerPercentage = 100; //else used maximum
+
+	if (pAd->CommonCfg.TxPowerPercentage > pAd->CommonCfg.TxPowerDefault)
+		pAd->CommonCfg.TxPowerPercentage = pAd->CommonCfg.TxPowerDefault;
+
+}
+
+CHAR	ConvertToRssi(
+	IN PRTMP_ADAPTER pAd,
+	IN	CHAR			Rssi,
+	IN  UCHAR   RssiNumber)
+{
+	UCHAR	RssiOffset, LNAGain;
+
+	// Rssi equals to zero should be an invalid value
+	if (Rssi == 0)
+		return -99;
+
+	LNAGain = GET_LNA_GAIN(pAd);
+    if (pAd->LatchRfRegs.Channel > 14)
+    {
+        if (RssiNumber == 0)
+			RssiOffset = pAd->ARssiOffset0;
+		else if (RssiNumber == 1)
+			RssiOffset = pAd->ARssiOffset1;
+		else
+			RssiOffset = pAd->ARssiOffset2;
+    }
+    else
+    {
+        if (RssiNumber == 0)
+			RssiOffset = pAd->BGRssiOffset0;
+		else if (RssiNumber == 1)
+			RssiOffset = pAd->BGRssiOffset1;
+		else
+			RssiOffset = pAd->BGRssiOffset2;
+    }
+
+    return (-12 - RssiOffset - LNAGain - Rssi);
+}
+
+/*
+	==========================================================================
+	Description:
+		Scan next channel
+	==========================================================================
+ */
+VOID ScanNextChannel(
+	IN PRTMP_ADAPTER pAd)
+{
+	HEADER_802_11   Hdr80211;
+	PUCHAR          pOutBuffer = NULL;
+	NDIS_STATUS     NStatus;
+	ULONG           FrameLen = 0;
+	UCHAR           SsidLen = 0, ScanType = pAd->MlmeAux.ScanType, BBPValue = 0;
+#ifdef CONFIG_STA_SUPPORT
+	USHORT          Status;
+	PHEADER_802_11  pHdr80211;
+#endif // CONFIG_STA_SUPPORT //
+	UINT			ScanTimeIn5gChannel = SHORT_CHANNEL_TIME;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (MONITOR_ON(pAd))
+			return;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef RALINK_ATE
+	// Nothing to do in ATE mode.
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+	if (pAd->MlmeAux.Channel == 0)
+	{
+		if ((pAd->CommonCfg.BBPCurrentBW == BW_40)
+#ifdef CONFIG_STA_SUPPORT
+			&& (INFRA_ON(pAd)
+				|| (pAd->OpMode == OPMODE_AP))
+#endif // CONFIG_STA_SUPPORT //
+			)
+		{
+			AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+			AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+			BBPValue &= (~0x18);
+			BBPValue |= 0x10;
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+			DBGPRINT(RT_DEBUG_TRACE, ("SYNC - End of SCAN, restore to 40MHz channel %d, Total BSS[%02d]\n",pAd->CommonCfg.CentralChannel, pAd->ScanTab.BssNr));
+		}
+		else
+		{
+			AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+			AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+			DBGPRINT(RT_DEBUG_TRACE, ("SYNC - End of SCAN, restore to channel %d, Total BSS[%02d]\n",pAd->CommonCfg.Channel, pAd->ScanTab.BssNr));
+		}
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			//
+			// To prevent data lost.
+			// Send an NULL data with turned PSM bit on to current associated AP before SCAN progress.
+			// Now, we need to send an NULL data with turned PSM bit off to AP, when scan progress done
+			//
+			if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED) && (INFRA_ON(pAd)))
+			{
+				NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);
+				if (NStatus	== NDIS_STATUS_SUCCESS)
+				{
+					pHdr80211 = (PHEADER_802_11) pOutBuffer;
+					MgtMacHeaderInit(pAd, pHdr80211, SUBTYPE_NULL_FUNC, 1, pAd->CommonCfg.Bssid, pAd->CommonCfg.Bssid);
+					pHdr80211->Duration = 0;
+					pHdr80211->FC.Type = BTYPE_DATA;
+					pHdr80211->FC.PwrMgmt = (pAd->StaCfg.Psm == PWR_SAVE);
+
+					// Send using priority queue
+					MiniportMMRequest(pAd, 0, pOutBuffer, sizeof(HEADER_802_11));
+					DBGPRINT(RT_DEBUG_TRACE, ("MlmeScanReqAction -- Send PSM Data frame\n"));
+					MlmeFreeMemory(pAd, pOutBuffer);
+					RTMPusecDelay(5000);
+				}
+			}
+
+			pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+			Status = MLME_SUCCESS;
+			MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_SCAN_CONF, 2, &Status);
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+
+		RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS);
+	}
+	else
+	{
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+		// BBP and RF are not accessible in PS mode, we has to wake them up first
+		if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+			AsicForceWakeup(pAd, TRUE);
+
+			// leave PSM during scanning. otherwise we may lost ProbeRsp & BEACON
+			if (pAd->StaCfg.Psm == PWR_SAVE)
+				MlmeSetPsmBit(pAd, PWR_ACTIVE);
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+		AsicSwitchChannel(pAd, pAd->MlmeAux.Channel, TRUE);
+		AsicLockChannel(pAd, pAd->MlmeAux.Channel);
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			if (pAd->MlmeAux.Channel > 14)
+			{
+				if ((pAd->CommonCfg.bIEEE80211H == 1) && RadarChannelCheck(pAd, pAd->MlmeAux.Channel))
+				{
+					ScanType = SCAN_PASSIVE;
+					ScanTimeIn5gChannel = MIN_CHANNEL_TIME;
+				}
+			}
+
+#ifdef CARRIER_DETECTION_SUPPORT // Roger sync Carrier
+			// carrier detection
+			if (pAd->CommonCfg.CarrierDetect.Enable == TRUE)
+			{
+				ScanType = SCAN_PASSIVE;
+				ScanTimeIn5gChannel = MIN_CHANNEL_TIME;
+			}
+#endif // CARRIER_DETECTION_SUPPORT //
+		}
+
+#endif // CONFIG_STA_SUPPORT //
+
+		//Global country domain(ch1-11:active scan, ch12-14 passive scan)
+		if ((pAd->MlmeAux.Channel <= 14) && (pAd->MlmeAux.Channel >= 12) && ((pAd->CommonCfg.CountryRegion & 0x7f) == REGION_31_BG_BAND))
+		{
+			ScanType = SCAN_PASSIVE;
+		}
+
+		// We need to shorten active scan time in order for WZC connect issue
+		// Chnage the channel scan time for CISCO stuff based on its IAPP announcement
+		if (ScanType == FAST_SCAN_ACTIVE)
+			RTMPSetTimer(&pAd->MlmeAux.ScanTimer, FAST_ACTIVE_SCAN_TIME);
+#ifdef CONFIG_STA_SUPPORT
+		else if (((ScanType == SCAN_CISCO_ACTIVE) ||
+				(ScanType == SCAN_CISCO_PASSIVE) ||
+				(ScanType == SCAN_CISCO_CHANNEL_LOAD) ||
+				(ScanType == SCAN_CISCO_NOISE)) && (pAd->OpMode == OPMODE_STA))
+		{
+			if (pAd->StaCfg.CCXScanTime < 25)
+				RTMPSetTimer(&pAd->MlmeAux.ScanTimer, pAd->StaCfg.CCXScanTime * 2);
+			else
+				RTMPSetTimer(&pAd->MlmeAux.ScanTimer, pAd->StaCfg.CCXScanTime);
+		}
+#endif // CONFIG_STA_SUPPORT //
+		else // must be SCAN_PASSIVE or SCAN_ACTIVE
+		{
+			if ((pAd->CommonCfg.PhyMode == PHY_11ABG_MIXED)
+#ifdef DOT11_N_SUPPORT
+				|| (pAd->CommonCfg.PhyMode == PHY_11ABGN_MIXED) || (pAd->CommonCfg.PhyMode == PHY_11AGN_MIXED)
+#endif // DOT11_N_SUPPORT //
+			)
+			{
+				if (pAd->MlmeAux.Channel > 14)
+					RTMPSetTimer(&pAd->MlmeAux.ScanTimer, ScanTimeIn5gChannel);
+				else
+				RTMPSetTimer(&pAd->MlmeAux.ScanTimer, MIN_CHANNEL_TIME);
+			}
+			else
+				RTMPSetTimer(&pAd->MlmeAux.ScanTimer, MAX_CHANNEL_TIME);
+		}
+
+		if ((ScanType == SCAN_ACTIVE) || (ScanType == FAST_SCAN_ACTIVE) ||
+			(ScanType == SCAN_CISCO_ACTIVE))
+		{
+			NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+			if (NStatus != NDIS_STATUS_SUCCESS)
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("SYNC - ScanNextChannel() allocate memory fail\n"));
+#ifdef CONFIG_STA_SUPPORT
+				IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				{
+					pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+					Status = MLME_FAIL_NO_RESOURCE;
+					MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_SCAN_CONF, 2, &Status);
+				}
+#endif // CONFIG_STA_SUPPORT //
+
+				return;
+			}
+
+			// There is no need to send broadcast probe request if active scan is in effect.
+			if ((ScanType == SCAN_ACTIVE) || (ScanType == FAST_SCAN_ACTIVE)
+				)
+				SsidLen = pAd->MlmeAux.SsidLen;
+			else
+				SsidLen = 0;
+
+			MgtMacHeaderInit(pAd, &Hdr80211, SUBTYPE_PROBE_REQ, 0, BROADCAST_ADDR, BROADCAST_ADDR);
+			MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+							  sizeof(HEADER_802_11),    &Hdr80211,
+							  1,                        &SsidIe,
+							  1,                        &SsidLen,
+							  SsidLen,			        pAd->MlmeAux.Ssid,
+							  1,                        &SupRateIe,
+							  1,                        &pAd->CommonCfg.SupRateLen,
+							  pAd->CommonCfg.SupRateLen,  pAd->CommonCfg.SupRate,
+							  END_OF_ARGS);
+
+			if (pAd->CommonCfg.ExtRateLen)
+			{
+				ULONG Tmp;
+				MakeOutgoingFrame(pOutBuffer + FrameLen,            &Tmp,
+								  1,                                &ExtRateIe,
+								  1,                                &pAd->CommonCfg.ExtRateLen,
+								  pAd->CommonCfg.ExtRateLen,          pAd->CommonCfg.ExtRate,
+								  END_OF_ARGS);
+				FrameLen += Tmp;
+			}
+
+#ifdef DOT11_N_SUPPORT
+			if (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED)
+			{
+				ULONG	Tmp;
+				UCHAR	HtLen;
+				UCHAR	BROADCOM[4] = {0x0, 0x90, 0x4c, 0x33};
+#ifdef RT_BIG_ENDIAN
+				HT_CAPABILITY_IE HtCapabilityTmp;
+#endif
+				if (pAd->bBroadComHT == TRUE)
+				{
+					HtLen = pAd->MlmeAux.HtCapabilityLen + 4;
+#ifdef RT_BIG_ENDIAN
+					NdisMoveMemory(&HtCapabilityTmp, &pAd->MlmeAux.HtCapability, SIZE_HT_CAP_IE);
+					*(USHORT *)(&HtCapabilityTmp.HtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.HtCapInfo));
+					*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo));
+
+					MakeOutgoingFrame(pOutBuffer + FrameLen,          &Tmp,
+									1,                                &WpaIe,
+									1,                                &HtLen,
+									4,                                &BROADCOM[0],
+									pAd->MlmeAux.HtCapabilityLen,     &HtCapabilityTmp,
+									END_OF_ARGS);
+#else
+					MakeOutgoingFrame(pOutBuffer + FrameLen,          &Tmp,
+									1,                                &WpaIe,
+									1,                                &HtLen,
+									4,                                &BROADCOM[0],
+									pAd->MlmeAux.HtCapabilityLen,     &pAd->MlmeAux.HtCapability,
+									END_OF_ARGS);
+#endif // RT_BIG_ENDIAN //
+				}
+				else
+				{
+					HtLen = pAd->MlmeAux.HtCapabilityLen;
+#ifdef RT_BIG_ENDIAN
+					NdisMoveMemory(&HtCapabilityTmp, &pAd->CommonCfg.HtCapability, SIZE_HT_CAP_IE);
+					*(USHORT *)(&HtCapabilityTmp.HtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.HtCapInfo));
+					*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo));
+
+					MakeOutgoingFrame(pOutBuffer + FrameLen,          &Tmp,
+									1,                                &HtCapIe,
+									1,                                &HtLen,
+									HtLen,                            &HtCapabilityTmp,
+									END_OF_ARGS);
+#else
+					MakeOutgoingFrame(pOutBuffer + FrameLen,          &Tmp,
+									1,                                &HtCapIe,
+									1,                                &HtLen,
+									HtLen,                            &pAd->CommonCfg.HtCapability,
+									END_OF_ARGS);
+#endif // RT_BIG_ENDIAN //
+				}
+				FrameLen += Tmp;
+
+#ifdef DOT11N_DRAFT3
+				if (pAd->CommonCfg.BACapability.field.b2040CoexistScanSup == 1)
+				{
+					ULONG		Tmp;
+					HtLen = 1;
+					MakeOutgoingFrame(pOutBuffer + FrameLen,            &Tmp,
+									  1,					&ExtHtCapIe,
+									  1,					&HtLen,
+									  1,          			&pAd->CommonCfg.BSSCoexist2040.word,
+									  END_OF_ARGS);
+
+					FrameLen += Tmp;
+				}
+#endif // DOT11N_DRAFT3 //
+			}
+#endif // DOT11_N_SUPPORT //
+
+
+			MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+			MlmeFreeMemory(pAd, pOutBuffer);
+		}
+
+		// For SCAN_CISCO_PASSIVE, do nothing and silently wait for beacon or other probe reponse
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			pAd->Mlme.SyncMachine.CurrState = SCAN_LISTEN;
+#endif // CONFIG_STA_SUPPORT //
+
+	}
+}
+
+VOID MgtProbReqMacHeaderInit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN OUT PHEADER_802_11 pHdr80211,
+	IN UCHAR SubType,
+	IN UCHAR ToDs,
+	IN PUCHAR pDA,
+	IN PUCHAR pBssid)
+{
+	NdisZeroMemory(pHdr80211, sizeof(HEADER_802_11));
+
+	pHdr80211->FC.Type = BTYPE_MGMT;
+	pHdr80211->FC.SubType = SubType;
+	if (SubType == SUBTYPE_ACK)
+		pHdr80211->FC.Type = BTYPE_CNTL;
+	pHdr80211->FC.ToDs = ToDs;
+	COPY_MAC_ADDR(pHdr80211->Addr1, pDA);
+	COPY_MAC_ADDR(pHdr80211->Addr2, pAd->CurrentAddress);
+	COPY_MAC_ADDR(pHdr80211->Addr3, pBssid);
+}
+
+
diff --git a/drivers/staging/rt2860/common/cmm_wpa.c b/drivers/staging/rt2860/common/cmm_wpa.c
new file mode 100644
index 00000000000..81c332ac252
--- /dev/null
+++ b/drivers/staging/rt2860/common/cmm_wpa.c
@@ -0,0 +1,1606 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	wpa.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Jan	Lee		03-07-22		Initial
+	Paul Lin	03-11-28		Modify for supplicant
+*/
+#include "../rt_config.h"
+// WPA OUI
+UCHAR		OUI_WPA_NONE_AKM[4]		= {0x00, 0x50, 0xF2, 0x00};
+UCHAR       OUI_WPA_VERSION[4]      = {0x00, 0x50, 0xF2, 0x01};
+UCHAR       OUI_WPA_TKIP[4]     = {0x00, 0x50, 0xF2, 0x02};
+UCHAR       OUI_WPA_CCMP[4]     = {0x00, 0x50, 0xF2, 0x04};
+UCHAR       OUI_WPA_8021X_AKM[4]	= {0x00, 0x50, 0xF2, 0x01};
+UCHAR       OUI_WPA_PSK_AKM[4]      = {0x00, 0x50, 0xF2, 0x02};
+// WPA2 OUI
+UCHAR       OUI_WPA2_WEP40[4]   = {0x00, 0x0F, 0xAC, 0x01};
+UCHAR       OUI_WPA2_TKIP[4]        = {0x00, 0x0F, 0xAC, 0x02};
+UCHAR       OUI_WPA2_CCMP[4]        = {0x00, 0x0F, 0xAC, 0x04};
+UCHAR       OUI_WPA2_8021X_AKM[4]   = {0x00, 0x0F, 0xAC, 0x01};
+UCHAR       OUI_WPA2_PSK_AKM[4]   	= {0x00, 0x0F, 0xAC, 0x02};
+// MSA OUI
+UCHAR   	OUI_MSA_8021X_AKM[4]    = {0x00, 0x0F, 0xAC, 0x05};		// Not yet final - IEEE 802.11s-D1.06
+UCHAR   	OUI_MSA_PSK_AKM[4]   	= {0x00, 0x0F, 0xAC, 0x06};		// Not yet final - IEEE 802.11s-D1.06
+
+/*
+	========================================================================
+
+	Routine Description:
+		The pseudo-random function(PRF) that hashes various inputs to
+		derive a pseudo-random value. To add liveness to the pseudo-random
+		value, a nonce should be one of the inputs.
+
+		It is used to generate PTK, GTK or some specific random value.
+
+	Arguments:
+		UCHAR	*key,		-	the key material for HMAC_SHA1 use
+		INT		key_len		-	the length of key
+		UCHAR	*prefix		-	a prefix label
+		INT		prefix_len	-	the length of the label
+		UCHAR	*data		-	a specific data with variable length
+		INT		data_len	-	the length of a specific data
+		INT		len			-	the output lenght
+
+	Return Value:
+		UCHAR	*output		-	the calculated result
+
+	Note:
+		802.11i-2004	Annex H.3
+
+	========================================================================
+*/
+VOID	PRF(
+	IN	UCHAR	*key,
+	IN	INT		key_len,
+	IN	UCHAR	*prefix,
+	IN	INT		prefix_len,
+	IN	UCHAR	*data,
+	IN	INT		data_len,
+	OUT	UCHAR	*output,
+	IN	INT		len)
+{
+	INT		i;
+    UCHAR   *input;
+	INT		currentindex = 0;
+	INT		total_len;
+
+	// Allocate memory for input
+	os_alloc_mem(NULL, (PUCHAR *)&input, 1024);
+
+    if (input == NULL)
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("!!!PRF: no memory!!!\n"));
+        return;
+    }
+
+	// Generate concatenation input
+	NdisMoveMemory(input, prefix, prefix_len);
+
+	// Concatenate a single octet containing 0
+	input[prefix_len] =	0;
+
+	// Concatenate specific data
+	NdisMoveMemory(&input[prefix_len + 1], data, data_len);
+	total_len =	prefix_len + 1 + data_len;
+
+	// Concatenate a single octet containing 0
+	// This octet shall be update later
+	input[total_len] = 0;
+	total_len++;
+
+	// Iterate to calculate the result by hmac-sha-1
+	// Then concatenate to last result
+	for	(i = 0;	i <	(len + 19) / 20; i++)
+	{
+		HMAC_SHA1(input, total_len,	key, key_len, &output[currentindex]);
+		currentindex +=	20;
+
+		// update the last octet
+		input[total_len - 1]++;
+	}
+    os_free_mem(NULL, input);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		It utilizes PRF-384 or PRF-512 to derive session-specific keys from a PMK.
+		It shall be called by 4-way handshake processing.
+
+	Arguments:
+		pAd 	-	pointer to our pAdapter context
+		PMK		-	pointer to PMK
+		ANonce	-	pointer to ANonce
+		AA		-	pointer to Authenticator Address
+		SNonce	-	pointer to SNonce
+		SA		-	pointer to Supplicant Address
+		len		-	indicate the length of PTK (octet)
+
+	Return Value:
+		Output		pointer to the PTK
+
+	Note:
+		Refer to IEEE 802.11i-2004 8.5.1.2
+
+	========================================================================
+*/
+VOID WpaCountPTK(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR	*PMK,
+	IN	UCHAR	*ANonce,
+	IN	UCHAR	*AA,
+	IN	UCHAR	*SNonce,
+	IN	UCHAR	*SA,
+	OUT	UCHAR	*output,
+	IN	UINT	len)
+{
+	UCHAR	concatenation[76];
+	UINT	CurrPos = 0;
+	UCHAR	temp[32];
+	UCHAR	Prefix[] = {'P', 'a', 'i', 'r', 'w', 'i', 's', 'e', ' ', 'k', 'e', 'y', ' ',
+						'e', 'x', 'p', 'a', 'n', 's', 'i', 'o', 'n'};
+
+	// initiate the concatenation input
+	NdisZeroMemory(temp, sizeof(temp));
+	NdisZeroMemory(concatenation, 76);
+
+	// Get smaller address
+	if (RTMPCompareMemory(SA, AA, 6) == 1)
+		NdisMoveMemory(concatenation, AA, 6);
+	else
+		NdisMoveMemory(concatenation, SA, 6);
+	CurrPos += 6;
+
+	// Get larger address
+	if (RTMPCompareMemory(SA, AA, 6) == 1)
+		NdisMoveMemory(&concatenation[CurrPos], SA, 6);
+	else
+		NdisMoveMemory(&concatenation[CurrPos], AA, 6);
+
+	// store the larger mac address for backward compatible of
+	// ralink proprietary STA-key issue
+	NdisMoveMemory(temp, &concatenation[CurrPos], MAC_ADDR_LEN);
+	CurrPos += 6;
+
+	// Get smaller Nonce
+	if (RTMPCompareMemory(ANonce, SNonce, 32) == 0)
+		NdisMoveMemory(&concatenation[CurrPos], temp, 32);	// patch for ralink proprietary STA-key issue
+	else if (RTMPCompareMemory(ANonce, SNonce, 32) == 1)
+		NdisMoveMemory(&concatenation[CurrPos], SNonce, 32);
+	else
+		NdisMoveMemory(&concatenation[CurrPos], ANonce, 32);
+	CurrPos += 32;
+
+	// Get larger Nonce
+	if (RTMPCompareMemory(ANonce, SNonce, 32) == 0)
+		NdisMoveMemory(&concatenation[CurrPos], temp, 32);	// patch for ralink proprietary STA-key issue
+	else if (RTMPCompareMemory(ANonce, SNonce, 32) == 1)
+		NdisMoveMemory(&concatenation[CurrPos], ANonce, 32);
+	else
+		NdisMoveMemory(&concatenation[CurrPos], SNonce, 32);
+	CurrPos += 32;
+
+	hex_dump("concatenation=", concatenation, 76);
+
+	// Use PRF to generate PTK
+	PRF(PMK, LEN_MASTER_KEY, Prefix, 22, concatenation, 76, output, len);
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Generate random number by software.
+
+	Arguments:
+		pAd		-	pointer to our pAdapter context
+		macAddr	-	pointer to local MAC address
+
+	Return Value:
+
+	Note:
+		802.1ii-2004  Annex H.5
+
+	========================================================================
+*/
+VOID	GenRandom(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			*macAddr,
+	OUT	UCHAR			*random)
+{
+	INT		i, curr;
+	UCHAR	local[80], KeyCounter[32];
+	UCHAR	result[80];
+	ULONG	CurrentTime;
+	UCHAR	prefix[] = {'I', 'n', 'i', 't', ' ', 'C', 'o', 'u', 'n', 't', 'e', 'r'};
+
+	// Zero the related information
+	NdisZeroMemory(result, 80);
+	NdisZeroMemory(local, 80);
+	NdisZeroMemory(KeyCounter, 32);
+
+	for	(i = 0;	i <	32;	i++)
+	{
+		// copy the local MAC address
+		COPY_MAC_ADDR(local, macAddr);
+		curr =	MAC_ADDR_LEN;
+
+		// concatenate the current time
+		NdisGetSystemUpTime(&CurrentTime);
+		NdisMoveMemory(&local[curr],  &CurrentTime,	sizeof(CurrentTime));
+		curr +=	sizeof(CurrentTime);
+
+		// concatenate the last result
+		NdisMoveMemory(&local[curr],  result, 32);
+		curr +=	32;
+
+		// concatenate a variable
+		NdisMoveMemory(&local[curr],  &i,  2);
+		curr +=	2;
+
+		// calculate the result
+		PRF(KeyCounter, 32, prefix,12, local, curr, result, 32);
+	}
+
+	NdisMoveMemory(random, result,	32);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Build cipher suite in RSN-IE.
+		It only shall be called by RTMPMakeRSNIE.
+
+	Arguments:
+		pAd			-	pointer to our pAdapter context
+    	ElementID	-	indicate the WPA1 or WPA2
+    	WepStatus	-	indicate the encryption type
+		bMixCipher	-	a boolean to indicate the pairwise cipher and group
+						cipher are the same or not
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+static VOID RTMPInsertRsnIeCipher(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	UCHAR			ElementID,
+	IN	UINT			WepStatus,
+	IN	BOOLEAN			bMixCipher,
+	IN	UCHAR			FlexibleCipher,
+	OUT	PUCHAR			pRsnIe,
+	OUT	UCHAR			*rsn_len)
+{
+	UCHAR	PairwiseCnt;
+
+	*rsn_len = 0;
+
+	// decide WPA2 or WPA1
+	if (ElementID == Wpa2Ie)
+	{
+		RSNIE2	*pRsnie_cipher = (RSNIE2*)pRsnIe;
+
+		// Assign the verson as 1
+		pRsnie_cipher->version = 1;
+
+        switch (WepStatus)
+        {
+        	// TKIP mode
+            case Ndis802_11Encryption2Enabled:
+                NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA2_TKIP, 4);
+                pRsnie_cipher->ucount = 1;
+                NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA2_TKIP, 4);
+                *rsn_len = sizeof(RSNIE2);
+                break;
+
+			// AES mode
+            case Ndis802_11Encryption3Enabled:
+				if (bMixCipher)
+					NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA2_TKIP, 4);
+				else
+					NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA2_CCMP, 4);
+                pRsnie_cipher->ucount = 1;
+                NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA2_CCMP, 4);
+                *rsn_len = sizeof(RSNIE2);
+                break;
+
+			// TKIP-AES mix mode
+            case Ndis802_11Encryption4Enabled:
+                NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA2_TKIP, 4);
+
+				PairwiseCnt = 1;
+				// Insert WPA2 TKIP as the first pairwise cipher
+				if (MIX_CIPHER_WPA2_TKIP_ON(FlexibleCipher))
+				{
+                	NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA2_TKIP, 4);
+					// Insert WPA2 AES as the secondary pairwise cipher
+					if (MIX_CIPHER_WPA2_AES_ON(FlexibleCipher))
+					{
+                		NdisMoveMemory(pRsnie_cipher->ucast[0].oui + 4, OUI_WPA2_CCMP, 4);
+						PairwiseCnt = 2;
+					}
+				}
+				else
+				{
+					// Insert WPA2 AES as the first pairwise cipher
+					NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA2_CCMP, 4);
+				}
+
+                pRsnie_cipher->ucount = PairwiseCnt;
+                *rsn_len = sizeof(RSNIE2) + (4 * (PairwiseCnt - 1));
+                break;
+        }
+
+		// swap for big-endian platform
+		pRsnie_cipher->version = cpu2le16(pRsnie_cipher->version);
+	    pRsnie_cipher->ucount = cpu2le16(pRsnie_cipher->ucount);
+	}
+	else
+	{
+		RSNIE	*pRsnie_cipher = (RSNIE*)pRsnIe;
+
+		// Assign OUI and version
+		NdisMoveMemory(pRsnie_cipher->oui, OUI_WPA_VERSION, 4);
+        pRsnie_cipher->version = 1;
+
+		switch (WepStatus)
+		{
+			// TKIP mode
+            case Ndis802_11Encryption2Enabled:
+                NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA_TKIP, 4);
+                pRsnie_cipher->ucount = 1;
+                NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA_TKIP, 4);
+                *rsn_len = sizeof(RSNIE);
+                break;
+
+			// AES mode
+            case Ndis802_11Encryption3Enabled:
+				if (bMixCipher)
+					NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA_TKIP, 4);
+				else
+					NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA_CCMP, 4);
+                pRsnie_cipher->ucount = 1;
+                NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA_CCMP, 4);
+                *rsn_len = sizeof(RSNIE);
+                break;
+
+			// TKIP-AES mix mode
+            case Ndis802_11Encryption4Enabled:
+                NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA_TKIP, 4);
+
+				PairwiseCnt = 1;
+				// Insert WPA TKIP as the first pairwise cipher
+				if (MIX_CIPHER_WPA_TKIP_ON(FlexibleCipher))
+				{
+                	NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA_TKIP, 4);
+					// Insert WPA AES as the secondary pairwise cipher
+					if (MIX_CIPHER_WPA_AES_ON(FlexibleCipher))
+					{
+                		NdisMoveMemory(pRsnie_cipher->ucast[0].oui + 4, OUI_WPA_CCMP, 4);
+						PairwiseCnt = 2;
+					}
+				}
+				else
+				{
+					// Insert WPA AES as the first pairwise cipher
+					NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA_CCMP, 4);
+				}
+
+                pRsnie_cipher->ucount = PairwiseCnt;
+                *rsn_len = sizeof(RSNIE) + (4 * (PairwiseCnt - 1));
+                break;
+        }
+
+		// swap for big-endian platform
+		pRsnie_cipher->version = cpu2le16(pRsnie_cipher->version);
+	    pRsnie_cipher->ucount = cpu2le16(pRsnie_cipher->ucount);
+	}
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Build AKM suite in RSN-IE.
+		It only shall be called by RTMPMakeRSNIE.
+
+	Arguments:
+		pAd			-	pointer to our pAdapter context
+    	ElementID	-	indicate the WPA1 or WPA2
+    	AuthMode	-	indicate the authentication mode
+		apidx		-	indicate the interface index
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+static VOID RTMPInsertRsnIeAKM(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	UCHAR			ElementID,
+	IN	UINT			AuthMode,
+	IN	UCHAR			apidx,
+	OUT	PUCHAR			pRsnIe,
+	OUT	UCHAR			*rsn_len)
+{
+	RSNIE_AUTH		*pRsnie_auth;
+
+	pRsnie_auth = (RSNIE_AUTH*)(pRsnIe + (*rsn_len));
+
+	// decide WPA2 or WPA1
+	if (ElementID == Wpa2Ie)
+	{
+		switch (AuthMode)
+        {
+            case Ndis802_11AuthModeWPA2:
+            case Ndis802_11AuthModeWPA1WPA2:
+                pRsnie_auth->acount = 1;
+                	NdisMoveMemory(pRsnie_auth->auth[0].oui, OUI_WPA2_8021X_AKM, 4);
+                break;
+
+            case Ndis802_11AuthModeWPA2PSK:
+            case Ndis802_11AuthModeWPA1PSKWPA2PSK:
+                pRsnie_auth->acount = 1;
+                	NdisMoveMemory(pRsnie_auth->auth[0].oui, OUI_WPA2_PSK_AKM, 4);
+                break;
+        }
+	}
+	else
+	{
+		switch (AuthMode)
+        {
+            case Ndis802_11AuthModeWPA:
+            case Ndis802_11AuthModeWPA1WPA2:
+                pRsnie_auth->acount = 1;
+                NdisMoveMemory(pRsnie_auth->auth[0].oui, OUI_WPA_8021X_AKM, 4);
+                break;
+
+            case Ndis802_11AuthModeWPAPSK:
+            case Ndis802_11AuthModeWPA1PSKWPA2PSK:
+                pRsnie_auth->acount = 1;
+                NdisMoveMemory(pRsnie_auth->auth[0].oui, OUI_WPA_PSK_AKM, 4);
+                break;
+
+			case Ndis802_11AuthModeWPANone:
+                pRsnie_auth->acount = 1;
+                NdisMoveMemory(pRsnie_auth->auth[0].oui, OUI_WPA_NONE_AKM, 4);
+                break;
+        }
+	}
+
+	pRsnie_auth->acount = cpu2le16(pRsnie_auth->acount);
+
+	(*rsn_len) += sizeof(RSNIE_AUTH);	// update current RSNIE length
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Build capability in RSN-IE.
+		It only shall be called by RTMPMakeRSNIE.
+
+	Arguments:
+		pAd			-	pointer to our pAdapter context
+    	ElementID	-	indicate the WPA1 or WPA2
+		apidx		-	indicate the interface index
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+static VOID RTMPInsertRsnIeCap(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	UCHAR			ElementID,
+	IN	UCHAR			apidx,
+	OUT	PUCHAR			pRsnIe,
+	OUT	UCHAR			*rsn_len)
+{
+	RSN_CAPABILITIES    *pRSN_Cap;
+
+	// it could be ignored in WPA1 mode
+	if (ElementID == WpaIe)
+		return;
+
+	pRSN_Cap = (RSN_CAPABILITIES*)(pRsnIe + (*rsn_len));
+
+
+	pRSN_Cap->word = cpu2le16(pRSN_Cap->word);
+
+	(*rsn_len) += sizeof(RSN_CAPABILITIES);	// update current RSNIE length
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Build RSN IE context. It is not included element-ID and length.
+
+	Arguments:
+		pAd			-	pointer to our pAdapter context
+    	AuthMode	-	indicate the authentication mode
+    	WepStatus	-	indicate the encryption type
+		apidx		-	indicate the interface index
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID RTMPMakeRSNIE(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  UINT            AuthMode,
+    IN  UINT            WepStatus,
+	IN	UCHAR			apidx)
+{
+	PUCHAR		pRsnIe = NULL;			// primary RSNIE
+	UCHAR 		*rsnielen_cur_p = 0;	// the length of the primary RSNIE
+	UCHAR		*rsnielen_ex_cur_p = 0;	// the length of the secondary RSNIE
+	UCHAR		PrimaryRsnie;
+	BOOLEAN		bMixCipher = FALSE;	// indicate the pairwise and group cipher are different
+	UCHAR		p_offset;
+	WPA_MIX_PAIR_CIPHER		FlexibleCipher = MIX_CIPHER_NOTUSE;	// it provide the more flexible cipher combination in WPA-WPA2 and TKIPAES mode
+
+	rsnielen_cur_p = NULL;
+	rsnielen_ex_cur_p = NULL;
+
+	{
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+#ifdef WPA_SUPPLICANT_SUPPORT
+			if (pAd->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_DISABLE)
+			{
+				if (AuthMode < Ndis802_11AuthModeWPA)
+					return;
+			}
+			else
+#endif // WPA_SUPPLICANT_SUPPORT //
+			{
+				// Support WPAPSK or WPA2PSK in STA-Infra mode
+				// Support WPANone in STA-Adhoc mode
+				if ((AuthMode != Ndis802_11AuthModeWPAPSK) &&
+					(AuthMode != Ndis802_11AuthModeWPA2PSK) &&
+					(AuthMode != Ndis802_11AuthModeWPANone)
+					)
+					return;
+			}
+
+			DBGPRINT(RT_DEBUG_TRACE,("==> RTMPMakeRSNIE(STA)\n"));
+
+			// Zero RSNIE context
+			pAd->StaCfg.RSNIE_Len = 0;
+			NdisZeroMemory(pAd->StaCfg.RSN_IE, MAX_LEN_OF_RSNIE);
+
+			// Pointer to RSNIE
+			rsnielen_cur_p = &pAd->StaCfg.RSNIE_Len;
+			pRsnIe = pAd->StaCfg.RSN_IE;
+
+			bMixCipher = pAd->StaCfg.bMixCipher;
+		}
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+	// indicate primary RSNIE as WPA or WPA2
+	if ((AuthMode == Ndis802_11AuthModeWPA) ||
+		(AuthMode == Ndis802_11AuthModeWPAPSK) ||
+		(AuthMode == Ndis802_11AuthModeWPANone) ||
+		(AuthMode == Ndis802_11AuthModeWPA1WPA2) ||
+		(AuthMode == Ndis802_11AuthModeWPA1PSKWPA2PSK))
+		PrimaryRsnie = WpaIe;
+	else
+		PrimaryRsnie = Wpa2Ie;
+
+	{
+		// Build the primary RSNIE
+		// 1. insert cipher suite
+		RTMPInsertRsnIeCipher(pAd, PrimaryRsnie, WepStatus, bMixCipher, FlexibleCipher, pRsnIe, &p_offset);
+
+		// 2. insert AKM
+		RTMPInsertRsnIeAKM(pAd, PrimaryRsnie, AuthMode, apidx, pRsnIe, &p_offset);
+
+		// 3. insert capability
+		RTMPInsertRsnIeCap(pAd, PrimaryRsnie, apidx, pRsnIe, &p_offset);
+	}
+
+	// 4. update the RSNIE length
+	*rsnielen_cur_p = p_offset;
+
+	hex_dump("The primary RSNIE", pRsnIe, (*rsnielen_cur_p));
+
+
+}
+
+/*
+    ==========================================================================
+    Description:
+		Check whether the received frame is EAP frame.
+
+	Arguments:
+		pAd				-	pointer to our pAdapter context
+		pEntry			-	pointer to active entry
+		pData			-	the received frame
+		DataByteCount 	-	the received frame's length
+		FromWhichBSSID	-	indicate the interface index
+
+    Return:
+         TRUE 			-	This frame is EAP frame
+         FALSE 			-	otherwise
+    ==========================================================================
+*/
+BOOLEAN RTMPCheckWPAframe(
+    IN PRTMP_ADAPTER    pAd,
+    IN PMAC_TABLE_ENTRY	pEntry,
+    IN PUCHAR           pData,
+    IN ULONG            DataByteCount,
+	IN UCHAR			FromWhichBSSID)
+{
+	ULONG	Body_len;
+	BOOLEAN Cancelled;
+
+
+    if(DataByteCount < (LENGTH_802_1_H + LENGTH_EAPOL_H))
+        return FALSE;
+
+
+	// Skip LLC header
+    if (NdisEqualMemory(SNAP_802_1H, pData, 6) ||
+        // Cisco 1200 AP may send packet with SNAP_BRIDGE_TUNNEL
+        NdisEqualMemory(SNAP_BRIDGE_TUNNEL, pData, 6))
+    {
+        pData += 6;
+    }
+	// Skip 2-bytes EAPoL type
+    if (NdisEqualMemory(EAPOL, pData, 2))
+    {
+        pData += 2;
+    }
+    else
+        return FALSE;
+
+    switch (*(pData+1))
+    {
+        case EAPPacket:
+			Body_len = (*(pData+2)<<8) | (*(pData+3));
+            DBGPRINT(RT_DEBUG_TRACE, ("Receive EAP-Packet frame, TYPE = 0, Length = %ld\n", Body_len));
+            break;
+        case EAPOLStart:
+            DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL-Start frame, TYPE = 1 \n"));
+			if (pEntry->EnqueueEapolStartTimerRunning != EAPOL_START_DISABLE)
+            {
+            	DBGPRINT(RT_DEBUG_TRACE, ("Cancel the EnqueueEapolStartTimerRunning \n"));
+                RTMPCancelTimer(&pEntry->EnqueueStartForPSKTimer, &Cancelled);
+                pEntry->EnqueueEapolStartTimerRunning = EAPOL_START_DISABLE;
+            }
+            break;
+        case EAPOLLogoff:
+            DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOLLogoff frame, TYPE = 2 \n"));
+            break;
+        case EAPOLKey:
+			Body_len = (*(pData+2)<<8) | (*(pData+3));
+            DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL-Key frame, TYPE = 3, Length = %ld\n", Body_len));
+            break;
+        case EAPOLASFAlert:
+            DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOLASFAlert frame, TYPE = 4 \n"));
+            break;
+        default:
+            return FALSE;
+
+    }
+    return TRUE;
+}
+
+
+/*
+    ==========================================================================
+    Description:
+        ENCRYPT AES GTK before sending in EAPOL frame.
+        AES GTK length = 128 bit,  so fix blocks for aes-key-wrap as 2 in this function.
+        This function references to RFC 3394 for aes key wrap algorithm.
+    Return:
+    ==========================================================================
+*/
+VOID AES_GTK_KEY_WRAP(
+    IN UCHAR    *key,
+    IN UCHAR    *plaintext,
+    IN UCHAR    p_len,
+    OUT UCHAR   *ciphertext)
+{
+    UCHAR       A[8], BIN[16], BOUT[16];
+    UCHAR       R[512];
+    INT         num_blocks = p_len/8;   // unit:64bits
+    INT         i, j;
+    aes_context aesctx;
+    UCHAR       xor;
+
+    rtmp_aes_set_key(&aesctx, key, 128);
+
+    // Init IA
+    for (i = 0; i < 8; i++)
+        A[i] = 0xa6;
+
+    //Input plaintext
+    for (i = 0; i < num_blocks; i++)
+    {
+        for (j = 0 ; j < 8; j++)
+            R[8 * (i + 1) + j] = plaintext[8 * i + j];
+    }
+
+    // Key Mix
+    for (j = 0; j < 6; j++)
+    {
+        for(i = 1; i <= num_blocks; i++)
+        {
+            //phase 1
+            NdisMoveMemory(BIN, A, 8);
+            NdisMoveMemory(&BIN[8], &R[8 * i], 8);
+            rtmp_aes_encrypt(&aesctx, BIN, BOUT);
+
+            NdisMoveMemory(A, &BOUT[0], 8);
+            xor = num_blocks * j + i;
+            A[7] = BOUT[7] ^ xor;
+            NdisMoveMemory(&R[8 * i], &BOUT[8], 8);
+        }
+    }
+
+    // Output ciphertext
+    NdisMoveMemory(ciphertext, A, 8);
+
+    for (i = 1; i <= num_blocks; i++)
+    {
+        for (j = 0 ; j < 8; j++)
+            ciphertext[8 * i + j] = R[8 * i + j];
+    }
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Misc function to decrypt AES body
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+		This function references to	RFC	3394 for aes key unwrap algorithm.
+
+	========================================================================
+*/
+VOID	AES_GTK_KEY_UNWRAP(
+	IN	UCHAR	*key,
+	OUT	UCHAR	*plaintext,
+	IN	UCHAR    c_len,
+	IN	UCHAR	*ciphertext)
+
+{
+	UCHAR       A[8], BIN[16], BOUT[16];
+	UCHAR       xor;
+	INT         i, j;
+	aes_context aesctx;
+	UCHAR       *R;
+	INT         num_blocks = c_len/8;	// unit:64bits
+
+
+	os_alloc_mem(NULL, (PUCHAR *)&R, 512);
+
+	if (R == NULL)
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("!!!AES_GTK_KEY_UNWRAP: no memory!!!\n"));
+        return;
+    } /* End of if */
+
+	// Initialize
+	NdisMoveMemory(A, ciphertext, 8);
+	//Input plaintext
+	for(i = 0; i < (c_len-8); i++)
+	{
+		R[ i] = ciphertext[i + 8];
+	}
+
+	rtmp_aes_set_key(&aesctx, key, 128);
+
+	for(j = 5; j >= 0; j--)
+	{
+		for(i = (num_blocks-1); i > 0; i--)
+		{
+			xor = (num_blocks -1 )* j + i;
+			NdisMoveMemory(BIN, A, 8);
+			BIN[7] = A[7] ^ xor;
+			NdisMoveMemory(&BIN[8], &R[(i-1)*8], 8);
+			rtmp_aes_decrypt(&aesctx, BIN, BOUT);
+			NdisMoveMemory(A, &BOUT[0], 8);
+			NdisMoveMemory(&R[(i-1)*8], &BOUT[8], 8);
+		}
+	}
+
+	// OUTPUT
+	for(i = 0; i < c_len; i++)
+	{
+		plaintext[i] = R[i];
+	}
+
+
+	os_free_mem(NULL, R);
+}
+
+/*
+    ==========================================================================
+    Description:
+		Report the EAP message type
+
+	Arguments:
+		msg		-	EAPOL_PAIR_MSG_1
+					EAPOL_PAIR_MSG_2
+					EAPOL_PAIR_MSG_3
+					EAPOL_PAIR_MSG_4
+					EAPOL_GROUP_MSG_1
+					EAPOL_GROUP_MSG_2
+
+    Return:
+         message type string
+
+    ==========================================================================
+*/
+CHAR *GetEapolMsgType(CHAR msg)
+{
+    if(msg == EAPOL_PAIR_MSG_1)
+        return "Pairwise Message 1";
+    else if(msg == EAPOL_PAIR_MSG_2)
+        return "Pairwise Message 2";
+	else if(msg == EAPOL_PAIR_MSG_3)
+        return "Pairwise Message 3";
+	else if(msg == EAPOL_PAIR_MSG_4)
+        return "Pairwise Message 4";
+	else if(msg == EAPOL_GROUP_MSG_1)
+        return "Group Message 1";
+	else if(msg == EAPOL_GROUP_MSG_2)
+        return "Group Message 2";
+    else
+    	return "Invalid Message";
+}
+
+
+/*
+    ========================================================================
+
+    Routine Description:
+    Check Sanity RSN IE of EAPoL message
+
+    Arguments:
+
+    Return Value:
+
+
+    ========================================================================
+*/
+BOOLEAN RTMPCheckRSNIE(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pData,
+	IN  UCHAR           DataLen,
+	IN  MAC_TABLE_ENTRY *pEntry,
+	OUT	UCHAR			*Offset)
+{
+	PUCHAR              pVIE;
+	UCHAR               len;
+	PEID_STRUCT         pEid;
+	BOOLEAN				result = FALSE;
+
+	pVIE = pData;
+	len	 = DataLen;
+	*Offset = 0;
+
+	while (len > sizeof(RSNIE2))
+	{
+		pEid = (PEID_STRUCT) pVIE;
+		// WPA RSN IE
+		if ((pEid->Eid == IE_WPA) && (NdisEqualMemory(pEid->Octet, WPA_OUI, 4)))
+		{
+			if ((pEntry->AuthMode == Ndis802_11AuthModeWPA || pEntry->AuthMode == Ndis802_11AuthModeWPAPSK) &&
+				(NdisEqualMemory(pVIE, pEntry->RSN_IE, pEntry->RSNIE_Len)) &&
+				(pEntry->RSNIE_Len == (pEid->Len + 2)))
+			{
+					result = TRUE;
+			}
+
+			*Offset += (pEid->Len + 2);
+		}
+		// WPA2 RSN IE
+		else if ((pEid->Eid == IE_RSN) && (NdisEqualMemory(pEid->Octet + 2, RSN_OUI, 3)))
+		{
+			if ((pEntry->AuthMode == Ndis802_11AuthModeWPA2 || pEntry->AuthMode == Ndis802_11AuthModeWPA2PSK) &&
+				(NdisEqualMemory(pVIE, pEntry->RSN_IE, pEntry->RSNIE_Len)) &&
+				(pEntry->RSNIE_Len == (pEid->Len + 2))/* ToDo-AlbertY for mesh*/)
+			{
+					result = TRUE;
+			}
+
+			*Offset += (pEid->Len + 2);
+		}
+		else
+		{
+			break;
+		}
+
+		pVIE += (pEid->Len + 2);
+		len  -= (pEid->Len + 2);
+	}
+
+
+	return result;
+
+}
+
+
+/*
+    ========================================================================
+
+    Routine Description:
+    Parse KEYDATA field.  KEYDATA[] May contain 2 RSN IE and optionally GTK.
+    GTK  is encaptulated in KDE format at  p.83 802.11i D10
+
+    Arguments:
+
+    Return Value:
+
+    Note:
+        802.11i D10
+
+    ========================================================================
+*/
+BOOLEAN RTMPParseEapolKeyData(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pKeyData,
+	IN  UCHAR           KeyDataLen,
+	IN	UCHAR			GroupKeyIndex,
+	IN	UCHAR			MsgType,
+	IN	BOOLEAN			bWPA2,
+	IN  MAC_TABLE_ENTRY *pEntry)
+{
+    PKDE_ENCAP          pKDE = NULL;
+    PUCHAR              pMyKeyData = pKeyData;
+    UCHAR               KeyDataLength = KeyDataLen;
+    UCHAR               GTKLEN = 0;
+	UCHAR				DefaultIdx = 0;
+	UCHAR				skip_offset;
+
+	// Verify The RSN IE contained in pairewise_msg_2 && pairewise_msg_3 and skip it
+	if (MsgType == EAPOL_PAIR_MSG_2 || MsgType == EAPOL_PAIR_MSG_3)
+    {
+		// Check RSN IE whether it is WPA2/WPA2PSK
+		if (!RTMPCheckRSNIE(pAd, pKeyData, KeyDataLen, pEntry, &skip_offset))
+		{
+			// send wireless event - for RSN IE different
+			if (pAd->CommonCfg.bWirelessEvent)
+				RTMPSendWirelessEvent(pAd, IW_RSNIE_DIFF_EVENT_FLAG, pEntry->Addr, pEntry->apidx, 0);
+
+        	DBGPRINT(RT_DEBUG_ERROR, ("RSN_IE Different in msg %d of 4-way handshake!\n", MsgType));
+			hex_dump("Receive RSN_IE ", pKeyData, KeyDataLen);
+			hex_dump("Desired RSN_IE ", pEntry->RSN_IE, pEntry->RSNIE_Len);
+
+			return FALSE;
+    	}
+    	else
+		{
+			if (bWPA2 && MsgType == EAPOL_PAIR_MSG_3)
+			{
+				// skip RSN IE
+				pMyKeyData += skip_offset;
+				KeyDataLength -= skip_offset;
+				DBGPRINT(RT_DEBUG_TRACE, ("RTMPParseEapolKeyData ==> WPA2/WPA2PSK RSN IE matched in Msg 3, Length(%d) \n", skip_offset));
+			}
+			else
+				return TRUE;
+		}
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("RTMPParseEapolKeyData ==> KeyDataLength %d without RSN_IE \n", KeyDataLength));
+
+	// Parse EKD format in pairwise_msg_3_WPA2 && group_msg_1_WPA2
+	if (bWPA2 && (MsgType == EAPOL_PAIR_MSG_3 || MsgType == EAPOL_GROUP_MSG_1))
+	{
+		if (KeyDataLength >= 8)	// KDE format exclude GTK length
+    	{
+        	pKDE = (PKDE_ENCAP) pMyKeyData;
+
+
+			DefaultIdx = pKDE->GTKEncap.Kid;
+
+			// Sanity check - KED length
+			if (KeyDataLength < (pKDE->Len + 2))
+    		{
+        		DBGPRINT(RT_DEBUG_ERROR, ("ERROR: The len from KDE is too short \n"));
+        		return FALSE;
+    		}
+
+			// Get GTK length - refer to IEEE 802.11i-2004 p.82
+			GTKLEN = pKDE->Len -6;
+			if (GTKLEN < LEN_AES_KEY)
+			{
+				DBGPRINT(RT_DEBUG_ERROR, ("ERROR: GTK Key length is too short (%d) \n", GTKLEN));
+        		return FALSE;
+			}
+
+    	}
+		else
+    	{
+			DBGPRINT(RT_DEBUG_ERROR, ("ERROR: KDE format length is too short \n"));
+	        return FALSE;
+    	}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("GTK in KDE format ,DefaultKeyID=%d, KeyLen=%d \n", DefaultIdx, GTKLEN));
+		// skip it
+		pMyKeyData += 8;
+		KeyDataLength -= 8;
+
+	}
+	else if (!bWPA2 && MsgType == EAPOL_GROUP_MSG_1)
+	{
+		DefaultIdx = GroupKeyIndex;
+		DBGPRINT(RT_DEBUG_TRACE, ("GTK DefaultKeyID=%d \n", DefaultIdx));
+	}
+
+	// Sanity check - shared key index must be 1 ~ 3
+	if (DefaultIdx < 1 || DefaultIdx > 3)
+    {
+     	DBGPRINT(RT_DEBUG_ERROR, ("ERROR: GTK Key index(%d) is invalid in %s %s \n", DefaultIdx, ((bWPA2) ? "WPA2" : "WPA"), GetEapolMsgType(MsgType)));
+        return FALSE;
+    }
+
+
+#ifdef CONFIG_STA_SUPPORT
+	// Todo
+#endif // CONFIG_STA_SUPPORT //
+
+	return TRUE;
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Construct EAPoL message for WPA handshaking
+		Its format is below,
+
+		+--------------------+
+		| Protocol Version	 |  1 octet
+		+--------------------+
+		| Protocol Type		 |	1 octet
+		+--------------------+
+		| Body Length		 |  2 octets
+		+--------------------+
+		| Descriptor Type	 |	1 octet
+		+--------------------+
+		| Key Information    |	2 octets
+		+--------------------+
+		| Key Length	     |  1 octet
+		+--------------------+
+		| Key Repaly Counter |	8 octets
+		+--------------------+
+		| Key Nonce		     |  32 octets
+		+--------------------+
+		| Key IV			 |  16 octets
+		+--------------------+
+		| Key RSC			 |  8 octets
+		+--------------------+
+		| Key ID or Reserved |	8 octets
+		+--------------------+
+		| Key MIC			 |	16 octets
+		+--------------------+
+		| Key Data Length	 |	2 octets
+		+--------------------+
+		| Key Data			 |	n octets
+		+--------------------+
+
+
+	Arguments:
+		pAd			Pointer	to our adapter
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	ConstructEapolMsg(
+	IN 	PRTMP_ADAPTER    	pAd,
+    IN 	UCHAR				AuthMode,
+    IN 	UCHAR				WepStatus,
+    IN 	UCHAR				GroupKeyWepStatus,
+    IN 	UCHAR				MsgType,
+    IN	UCHAR				DefaultKeyIdx,
+    IN 	UCHAR				*ReplayCounter,
+	IN 	UCHAR				*KeyNonce,
+	IN	UCHAR				*TxRSC,
+	IN	UCHAR				*PTK,
+	IN	UCHAR				*GTK,
+	IN	UCHAR				*RSNIE,
+	IN	UCHAR				RSNIE_Len,
+    OUT PEAPOL_PACKET       pMsg)
+{
+	BOOLEAN	bWPA2 = FALSE;
+
+	// Choose WPA2 or not
+	if ((AuthMode == Ndis802_11AuthModeWPA2) || (AuthMode == Ndis802_11AuthModeWPA2PSK))
+		bWPA2 = TRUE;
+
+    // Init Packet and Fill header
+    pMsg->ProVer = EAPOL_VER;
+    pMsg->ProType = EAPOLKey;
+
+	// Default 95 bytes, the EAPoL-Key descriptor exclude Key-data field
+	pMsg->Body_Len[1] = LEN_EAPOL_KEY_MSG;
+
+	// Fill in EAPoL descriptor
+	if (bWPA2)
+		pMsg->KeyDesc.Type = WPA2_KEY_DESC;
+	else
+		pMsg->KeyDesc.Type = WPA1_KEY_DESC;
+
+	// Fill in Key information, refer to IEEE Std 802.11i-2004 page 78
+	// When either the pairwise or the group cipher is AES, the DESC_TYPE_AES(2) shall be used.
+	pMsg->KeyDesc.KeyInfo.KeyDescVer =
+        	(((WepStatus == Ndis802_11Encryption3Enabled) || (GroupKeyWepStatus == Ndis802_11Encryption3Enabled)) ? (DESC_TYPE_AES) : (DESC_TYPE_TKIP));
+
+	// Specify Key Type as Group(0) or Pairwise(1)
+	if (MsgType >= EAPOL_GROUP_MSG_1)
+		pMsg->KeyDesc.KeyInfo.KeyType = GROUPKEY;
+	else
+		pMsg->KeyDesc.KeyInfo.KeyType = PAIRWISEKEY;
+
+	// Specify Key Index, only group_msg1_WPA1
+	if (!bWPA2 && (MsgType >= EAPOL_GROUP_MSG_1))
+		pMsg->KeyDesc.KeyInfo.KeyIndex = DefaultKeyIdx;
+
+	if (MsgType == EAPOL_PAIR_MSG_3)
+		pMsg->KeyDesc.KeyInfo.Install = 1;
+
+	if ((MsgType == EAPOL_PAIR_MSG_1) || (MsgType == EAPOL_PAIR_MSG_3) || (MsgType == EAPOL_GROUP_MSG_1))
+		pMsg->KeyDesc.KeyInfo.KeyAck = 1;
+
+	if (MsgType != EAPOL_PAIR_MSG_1)
+		pMsg->KeyDesc.KeyInfo.KeyMic = 1;
+
+	if ((bWPA2 && (MsgType >= EAPOL_PAIR_MSG_3)) || (!bWPA2 && (MsgType >= EAPOL_GROUP_MSG_1)))
+    {
+       	pMsg->KeyDesc.KeyInfo.Secure = 1;
+    }
+
+	if (bWPA2 && ((MsgType == EAPOL_PAIR_MSG_3) || (MsgType == EAPOL_GROUP_MSG_1)))
+    {
+        pMsg->KeyDesc.KeyInfo.EKD_DL = 1;
+    }
+
+	// key Information element has done.
+	*(USHORT *)(&pMsg->KeyDesc.KeyInfo) = cpu2le16(*(USHORT *)(&pMsg->KeyDesc.KeyInfo));
+
+	// Fill in Key Length
+	{
+		if (MsgType >= EAPOL_GROUP_MSG_1)
+		{
+			// the length of group key cipher
+			pMsg->KeyDesc.KeyLength[1] = ((GroupKeyWepStatus == Ndis802_11Encryption2Enabled) ? TKIP_GTK_LENGTH : LEN_AES_KEY);
+		}
+		else
+		{
+			// the length of pairwise key cipher
+			pMsg->KeyDesc.KeyLength[1] = ((WepStatus == Ndis802_11Encryption2Enabled) ? LEN_TKIP_KEY : LEN_AES_KEY);
+		}
+	}
+
+ 	// Fill in replay counter
+    NdisMoveMemory(pMsg->KeyDesc.ReplayCounter, ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// Fill Key Nonce field
+	// ANonce : pairwise_msg1 & pairwise_msg3
+	// SNonce : pairwise_msg2
+	// GNonce : group_msg1_wpa1
+	if ((MsgType <= EAPOL_PAIR_MSG_3) || ((!bWPA2 && (MsgType == EAPOL_GROUP_MSG_1))))
+    	NdisMoveMemory(pMsg->KeyDesc.KeyNonce, KeyNonce, LEN_KEY_DESC_NONCE);
+
+	// Fill key IV - WPA2 as 0, WPA1 as random
+	if (!bWPA2 && (MsgType == EAPOL_GROUP_MSG_1))
+	{
+		// Suggest IV be random number plus some number,
+		NdisMoveMemory(pMsg->KeyDesc.KeyIv, &KeyNonce[16], LEN_KEY_DESC_IV);
+        pMsg->KeyDesc.KeyIv[15] += 2;
+	}
+
+    // Fill Key RSC field
+    // It contains the RSC for the GTK being installed.
+	if ((MsgType == EAPOL_PAIR_MSG_3 && bWPA2) || (MsgType == EAPOL_GROUP_MSG_1))
+	{
+        NdisMoveMemory(pMsg->KeyDesc.KeyRsc, TxRSC, 6);
+	}
+
+	// Clear Key MIC field for MIC calculation later
+    NdisZeroMemory(pMsg->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+
+	ConstructEapolKeyData(pAd,
+						  AuthMode,
+						  WepStatus,
+						  GroupKeyWepStatus,
+						  MsgType,
+						  DefaultKeyIdx,
+						  bWPA2,
+						  PTK,
+						  GTK,
+						  RSNIE,
+						  RSNIE_Len,
+						  pMsg);
+
+	// Calculate MIC and fill in KeyMic Field except Pairwise Msg 1.
+	if (MsgType != EAPOL_PAIR_MSG_1)
+	{
+		CalculateMIC(pAd, WepStatus, PTK, pMsg);
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("===> ConstructEapolMsg for %s %s\n", ((bWPA2) ? "WPA2" : "WPA"), GetEapolMsgType(MsgType)));
+	DBGPRINT(RT_DEBUG_TRACE, ("	     Body length = %d \n", pMsg->Body_Len[1]));
+	DBGPRINT(RT_DEBUG_TRACE, ("	     Key length  = %d \n", pMsg->KeyDesc.KeyLength[1]));
+
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Construct the Key Data field of EAPoL message
+
+	Arguments:
+		pAd			Pointer	to our adapter
+		Elem		Message body
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	ConstructEapolKeyData(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			AuthMode,
+	IN	UCHAR			WepStatus,
+	IN	UCHAR			GroupKeyWepStatus,
+	IN 	UCHAR			MsgType,
+	IN	UCHAR			DefaultKeyIdx,
+	IN	BOOLEAN			bWPA2Capable,
+	IN	UCHAR			*PTK,
+	IN	UCHAR			*GTK,
+	IN	UCHAR			*RSNIE,
+	IN	UCHAR			RSNIE_LEN,
+	OUT PEAPOL_PACKET   pMsg)
+{
+	UCHAR		*mpool, *Key_Data, *Rc4GTK;
+	UCHAR       ekey[(LEN_KEY_DESC_IV+LEN_EAP_EK)];
+	UCHAR		data_offset;
+
+
+	if (MsgType == EAPOL_PAIR_MSG_1 || MsgType == EAPOL_PAIR_MSG_4 || MsgType == EAPOL_GROUP_MSG_2)
+		return;
+
+	// allocate memory pool
+	os_alloc_mem(pAd, (PUCHAR *)&mpool, 1500);
+
+    if (mpool == NULL)
+		return;
+
+	/* Rc4GTK Len = 512 */
+	Rc4GTK = (UCHAR *) ROUND_UP(mpool, 4);
+	/* Key_Data Len = 512 */
+	Key_Data = (UCHAR *) ROUND_UP(Rc4GTK + 512, 4);
+
+	NdisZeroMemory(Key_Data, 512);
+	pMsg->KeyDesc.KeyDataLen[1] = 0;
+	data_offset = 0;
+
+	// Encapsulate RSNIE in pairwise_msg2 & pairwise_msg3
+	if (RSNIE_LEN && ((MsgType == EAPOL_PAIR_MSG_2) || (MsgType == EAPOL_PAIR_MSG_3)))
+	{
+		if (bWPA2Capable)
+			Key_Data[data_offset + 0] = IE_WPA2;
+		else
+			Key_Data[data_offset + 0] = IE_WPA;
+
+        Key_Data[data_offset + 1] = RSNIE_LEN;
+		NdisMoveMemory(&Key_Data[data_offset + 2], RSNIE, RSNIE_LEN);
+		data_offset += (2 + RSNIE_LEN);
+	}
+
+	// Encapsulate KDE format in pairwise_msg3_WPA2 & group_msg1_WPA2
+	if (bWPA2Capable && ((MsgType == EAPOL_PAIR_MSG_3) || (MsgType == EAPOL_GROUP_MSG_1)))
+	{
+		// Key Data Encapsulation (KDE) format - 802.11i-2004  Figure-43w and Table-20h
+        Key_Data[data_offset + 0] = 0xDD;
+
+		if (GroupKeyWepStatus == Ndis802_11Encryption3Enabled)
+		{
+			Key_Data[data_offset + 1] = 0x16;// 4+2+16(OUI+DataType+DataField)
+		}
+		else
+		{
+			Key_Data[data_offset + 1] = 0x26;// 4+2+32(OUI+DataType+DataField)
+		}
+
+        Key_Data[data_offset + 2] = 0x00;
+        Key_Data[data_offset + 3] = 0x0F;
+        Key_Data[data_offset + 4] = 0xAC;
+        Key_Data[data_offset + 5] = 0x01;
+
+		// GTK KDE format - 802.11i-2004  Figure-43x
+        Key_Data[data_offset + 6] = (DefaultKeyIdx & 0x03);
+        Key_Data[data_offset + 7] = 0x00;	// Reserved Byte
+
+		data_offset += 8;
+	}
+
+
+	// Encapsulate GTK and encrypt the key-data field with KEK.
+	// Only for pairwise_msg3_WPA2 and group_msg1
+	if ((MsgType == EAPOL_PAIR_MSG_3 && bWPA2Capable) || (MsgType == EAPOL_GROUP_MSG_1))
+	{
+		// Fill in GTK
+		if (GroupKeyWepStatus == Ndis802_11Encryption3Enabled)
+		{
+			NdisMoveMemory(&Key_Data[data_offset], GTK, LEN_AES_KEY);
+			data_offset += LEN_AES_KEY;
+		}
+		else
+		{
+			NdisMoveMemory(&Key_Data[data_offset], GTK, TKIP_GTK_LENGTH);
+			data_offset += TKIP_GTK_LENGTH;
+		}
+
+		// Still dont know why, but if not append will occur "GTK not include in MSG3"
+		// Patch for compatibility between zero config and funk
+		if (MsgType == EAPOL_PAIR_MSG_3 && bWPA2Capable)
+		{
+			if (GroupKeyWepStatus == Ndis802_11Encryption3Enabled)
+			{
+				Key_Data[data_offset + 0] = 0xDD;
+				Key_Data[data_offset + 1] = 0;
+				data_offset += 2;
+			}
+			else
+			{
+				Key_Data[data_offset + 0] = 0xDD;
+				Key_Data[data_offset + 1] = 0;
+				Key_Data[data_offset + 2] = 0;
+				Key_Data[data_offset + 3] = 0;
+				Key_Data[data_offset + 4] = 0;
+				Key_Data[data_offset + 5] = 0;
+				data_offset += 6;
+			}
+		}
+
+		// Encrypt the data material in key data field
+		if (WepStatus == Ndis802_11Encryption3Enabled)
+		{
+			AES_GTK_KEY_WRAP(&PTK[16], Key_Data, data_offset, Rc4GTK);
+            // AES wrap function will grow 8 bytes in length
+            data_offset += 8;
+		}
+		else
+		{
+			// PREPARE Encrypted  "Key DATA" field.  (Encrypt GTK with RC4, usinf PTK[16]->[31] as Key, IV-field as IV)
+			// put TxTsc in Key RSC field
+			pAd->PrivateInfo.FCSCRC32 = PPPINITFCS32;   //Init crc32.
+
+			// ekey is the contanetion of IV-field, and PTK[16]->PTK[31]
+			NdisMoveMemory(ekey, pMsg->KeyDesc.KeyIv, LEN_KEY_DESC_IV);
+			NdisMoveMemory(&ekey[LEN_KEY_DESC_IV], &PTK[16], LEN_EAP_EK);
+			ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, ekey, sizeof(ekey));  //INIT SBOX, KEYLEN+3(IV)
+			pAd->PrivateInfo.FCSCRC32 = RTMP_CALC_FCS32(pAd->PrivateInfo.FCSCRC32, Key_Data, data_offset);
+			WPAARCFOUR_ENCRYPT(&pAd->PrivateInfo.WEPCONTEXT, Rc4GTK, Key_Data, data_offset);
+		}
+
+		NdisMoveMemory(pMsg->KeyDesc.KeyData, Rc4GTK, data_offset);
+	}
+	else
+	{
+		NdisMoveMemory(pMsg->KeyDesc.KeyData, Key_Data, data_offset);
+	}
+
+	// set key data length field and total length
+	pMsg->KeyDesc.KeyDataLen[1] = data_offset;
+    pMsg->Body_Len[1] += data_offset;
+
+	os_free_mem(pAd, mpool);
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Calcaulate MIC. It is used during 4-ways handsharking.
+
+	Arguments:
+		pAd				-	pointer to our pAdapter context
+    	PeerWepStatus	-	indicate the encryption type
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID	CalculateMIC(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			PeerWepStatus,
+	IN	UCHAR			*PTK,
+	OUT PEAPOL_PACKET   pMsg)
+{
+    UCHAR   *OutBuffer;
+	ULONG	FrameLen = 0;
+	UCHAR	mic[LEN_KEY_DESC_MIC];
+	UCHAR	digest[80];
+
+	// allocate memory for MIC calculation
+	os_alloc_mem(pAd, (PUCHAR *)&OutBuffer, 512);
+
+    if (OutBuffer == NULL)
+    {
+		DBGPRINT(RT_DEBUG_ERROR, ("!!!CalculateMIC: no memory!!!\n"));
+		return;
+    }
+
+	// make a frame for calculating MIC.
+    MakeOutgoingFrame(OutBuffer,            	&FrameLen,
+                      pMsg->Body_Len[1] + 4,  	pMsg,
+                      END_OF_ARGS);
+
+	NdisZeroMemory(mic, sizeof(mic));
+
+	// Calculate MIC
+    if (PeerWepStatus == Ndis802_11Encryption3Enabled)
+ 	{
+		HMAC_SHA1(OutBuffer,  FrameLen, PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{
+		hmac_md5(PTK,  LEN_EAP_MICK, OutBuffer, FrameLen, mic);
+	}
+
+	// store the calculated MIC
+	NdisMoveMemory(pMsg->KeyDesc.KeyMic, mic, LEN_KEY_DESC_MIC);
+
+	os_free_mem(pAd, OutBuffer);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Some received frames can't decrypt by Asic, so decrypt them by software.
+
+	Arguments:
+		pAd				-	pointer to our pAdapter context
+    	PeerWepStatus	-	indicate the encryption type
+
+	Return Value:
+		NDIS_STATUS_SUCCESS		-	decryption successful
+		NDIS_STATUS_FAILURE		-	decryption failure
+
+	========================================================================
+*/
+NDIS_STATUS	RTMPSoftDecryptBroadCastData(
+	IN	PRTMP_ADAPTER					pAd,
+	IN	RX_BLK							*pRxBlk,
+	IN  NDIS_802_11_ENCRYPTION_STATUS 	GroupCipher,
+	IN  PCIPHER_KEY						pShard_key)
+{
+	PRXWI_STRUC			pRxWI = pRxBlk->pRxWI;
+
+
+
+	// handle WEP decryption
+	if (GroupCipher == Ndis802_11Encryption1Enabled)
+    {
+		if (RTMPSoftDecryptWEP(pAd, pRxBlk->pData, pRxWI->MPDUtotalByteCount, pShard_key))
+		{
+
+			//Minus IV[4] & ICV[4]
+			pRxWI->MPDUtotalByteCount -= 8;
+		}
+		else
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("ERROR : Software decrypt WEP data fails.\n"));
+			// give up this frame
+			return NDIS_STATUS_FAILURE;
+		}
+	}
+	// handle TKIP decryption
+	else if (GroupCipher == Ndis802_11Encryption2Enabled)
+	{
+		if (RTMPSoftDecryptTKIP(pAd, pRxBlk->pData, pRxWI->MPDUtotalByteCount, 0, pShard_key))
+		{
+
+			//Minus 8 bytes MIC, 8 bytes IV/EIV, 4 bytes ICV
+			pRxWI->MPDUtotalByteCount -= 20;
+		}
+        else
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("ERROR : RTMPSoftDecryptTKIP Failed\n"));
+			// give up this frame
+			return NDIS_STATUS_FAILURE;
+        }
+	}
+	// handle AES decryption
+	else if (GroupCipher == Ndis802_11Encryption3Enabled)
+	{
+		if (RTMPSoftDecryptAES(pAd, pRxBlk->pData, pRxWI->MPDUtotalByteCount , pShard_key))
+		{
+
+			//8 bytes MIC, 8 bytes IV/EIV (CCMP Header)
+			pRxWI->MPDUtotalByteCount -= 16;
+		}
+		else
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("ERROR : RTMPSoftDecryptAES Failed\n"));
+			// give up this frame
+			return NDIS_STATUS_FAILURE;
+		}
+	}
+	else
+	{
+		// give up this frame
+		return NDIS_STATUS_FAILURE;
+	}
+
+	return NDIS_STATUS_SUCCESS;
+
+}
+
diff --git a/drivers/staging/rt2860/common/dfs.c b/drivers/staging/rt2860/common/dfs.c
new file mode 100644
index 00000000000..23cf1510e2d
--- /dev/null
+++ b/drivers/staging/rt2860/common/dfs.c
@@ -0,0 +1,453 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    ap_dfs.c
+
+    Abstract:
+    Support DFS function.
+
+    Revision History:
+    Who       When            What
+    --------  ----------      ----------------------------------------------
+    Fonchi    03-12-2007      created
+*/
+
+#include "../rt_config.h"
+
+typedef struct _RADAR_DURATION_TABLE
+{
+	ULONG RDDurRegion;
+	ULONG RadarSignalDuration;
+	ULONG Tolerance;
+} RADAR_DURATION_TABLE, *PRADAR_DURATION_TABLE;
+
+
+static UCHAR RdIdleTimeTable[MAX_RD_REGION][4] =
+{
+	{9, 250, 250, 250},		// CE
+	{4, 250, 250, 250},		// FCC
+	{4, 250, 250, 250},		// JAP
+	{15, 250, 250, 250},	// JAP_W53
+	{4, 250, 250, 250}		// JAP_W56
+};
+
+/*
+	========================================================================
+
+	Routine Description:
+		Bbp Radar detection routine
+
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	Return Value:
+
+	========================================================================
+*/
+VOID BbpRadarDetectionStart(
+	IN PRTMP_ADAPTER pAd)
+{
+	UINT8 RadarPeriod;
+
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, 114, 0x02);
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, 121, 0x20);
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, 122, 0x00);
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, 123, 0x08/*0x80*/);
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, 124, 0x28);
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, 125, 0xff);
+
+#if 0
+	// toggle Rx enable bit for radar detection.
+	// it's Andy's recommand.
+	{
+		UINT32 Value;
+	RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+	Value |= (0x1 << 3);
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+	Value &= ~(0x1 << 3);
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+	}
+#endif
+	RadarPeriod = ((UINT)RdIdleTimeTable[pAd->CommonCfg.RadarDetect.RDDurRegion][0] + (UINT)pAd->CommonCfg.RadarDetect.DfsSessionTime) < 250 ?
+			(RdIdleTimeTable[pAd->CommonCfg.RadarDetect.RDDurRegion][0] + pAd->CommonCfg.RadarDetect.DfsSessionTime) : 250;
+
+	RTMP_IO_WRITE8(pAd, 0x7020, 0x1d);
+	RTMP_IO_WRITE8(pAd, 0x7021, 0x40);
+
+	RadarDetectionStart(pAd, 0, RadarPeriod);
+	return;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Bbp Radar detection routine
+
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	Return Value:
+
+	========================================================================
+*/
+VOID BbpRadarDetectionStop(
+	IN PRTMP_ADAPTER pAd)
+{
+	RTMP_IO_WRITE8(pAd, 0x7020, 0x1d);
+	RTMP_IO_WRITE8(pAd, 0x7021, 0x60);
+
+	RadarDetectionStop(pAd);
+	return;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Radar detection routine
+
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	Return Value:
+
+	========================================================================
+*/
+VOID RadarDetectionStart(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN CTSProtect,
+	IN UINT8 CTSPeriod)
+{
+	UINT8 DfsActiveTime = (pAd->CommonCfg.RadarDetect.DfsSessionTime & 0x1f);
+	UINT8 CtsProtect = (CTSProtect == 1) ? 0x02 : 0x01; // CTS protect.
+
+	if (CTSProtect != 0)
+	{
+		switch(pAd->CommonCfg.RadarDetect.RDDurRegion)
+		{
+		case FCC:
+		case JAP_W56:
+			CtsProtect = 0x03;
+			break;
+
+		case CE:
+		case JAP_W53:
+		default:
+			CtsProtect = 0x02;
+			break;
+		}
+	}
+	else
+		CtsProtect = 0x01;
+
+
+	// send start-RD with CTS protection command to MCU
+	// highbyte [7]		reserve
+	// highbyte [6:5]	0x: stop Carrier/Radar detection
+	// highbyte [10]:	Start Carrier/Radar detection without CTS protection, 11: Start Carrier/Radar detection with CTS protection
+	// highbyte [4:0]	Radar/carrier detection duration. In 1ms.
+
+	// lowbyte [7:0]	Radar/carrier detection period, in 1ms.
+	AsicSendCommandToMcu(pAd, 0x60, 0xff, CTSPeriod, DfsActiveTime | (CtsProtect << 5));
+	//AsicSendCommandToMcu(pAd, 0x63, 0xff, 10, 0);
+
+	return;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Radar detection routine
+
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	Return Value:
+		TRUE	Found radar signal
+		FALSE	Not found radar signal
+
+	========================================================================
+*/
+VOID RadarDetectionStop(
+	IN PRTMP_ADAPTER	pAd)
+{
+	DBGPRINT(RT_DEBUG_TRACE,("RadarDetectionStop.\n"));
+	AsicSendCommandToMcu(pAd, 0x60, 0xff, 0x00, 0x00);	// send start-RD with CTS protection command to MCU
+
+	return;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Radar channel check routine
+
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	Return Value:
+		TRUE	need to do radar detect
+		FALSE	need not to do radar detect
+
+	========================================================================
+*/
+BOOLEAN RadarChannelCheck(
+	IN PRTMP_ADAPTER	pAd,
+	IN UCHAR			Ch)
+{
+#if 1
+	INT		i;
+	BOOLEAN result = FALSE;
+
+	for (i=0; i<pAd->ChannelListNum; i++)
+	{
+		if (Ch == pAd->ChannelList[i].Channel)
+		{
+			result = pAd->ChannelList[i].DfsReq;
+			break;
+		}
+	}
+
+	return result;
+#else
+	INT		i;
+	UCHAR	Channel[15]={52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140};
+
+	for (i=0; i<15; i++)
+	{
+		if (Ch == Channel[i])
+		{
+			break;
+		}
+	}
+
+	if (i != 15)
+		return TRUE;
+	else
+		return FALSE;
+#endif
+}
+
+ULONG JapRadarType(
+	IN PRTMP_ADAPTER pAd)
+{
+	ULONG		i;
+	const UCHAR	Channel[15]={52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140};
+
+	if (pAd->CommonCfg.RadarDetect.RDDurRegion != JAP)
+	{
+		return pAd->CommonCfg.RadarDetect.RDDurRegion;
+	}
+
+	for (i=0; i<15; i++)
+	{
+		if (pAd->CommonCfg.Channel == Channel[i])
+		{
+			break;
+		}
+	}
+
+	if (i < 4)
+		return JAP_W53;
+	else if (i < 15)
+		return JAP_W56;
+	else
+		return JAP; // W52
+
+}
+
+ULONG RTMPBbpReadRadarDuration(
+	IN PRTMP_ADAPTER	pAd)
+{
+	UINT8 byteValue = 0;
+	ULONG result;
+
+	BBP_IO_READ8_BY_REG_ID(pAd, BBP_R115, &byteValue);
+
+	result = 0;
+	switch (byteValue)
+	{
+	case 1: // radar signal detected by pulse mode.
+	case 2: // radar signal detected by width mode.
+		result = RTMPReadRadarDuration(pAd);
+		break;
+
+	case 0: // No radar signal.
+	default:
+
+		result = 0;
+		break;
+	}
+
+	return result;
+}
+
+ULONG RTMPReadRadarDuration(
+	IN PRTMP_ADAPTER	pAd)
+{
+	ULONG result = 0;
+
+#ifdef DFS_SUPPORT
+	UINT8 duration1 = 0, duration2 = 0, duration3 = 0;
+
+	BBP_IO_READ8_BY_REG_ID(pAd, BBP_R116, &duration1);
+	BBP_IO_READ8_BY_REG_ID(pAd, BBP_R117, &duration2);
+	BBP_IO_READ8_BY_REG_ID(pAd, BBP_R118, &duration3);
+	result = (duration1 << 16) + (duration2 << 8) + duration3;
+#endif // DFS_SUPPORT //
+
+	return result;
+
+}
+
+VOID RTMPCleanRadarDuration(
+	IN PRTMP_ADAPTER	pAd)
+{
+	return;
+}
+
+/*
+    ========================================================================
+    Routine Description:
+        Radar wave detection. The API should be invoke each second.
+
+    Arguments:
+        pAd         - Adapter pointer
+
+    Return Value:
+        None
+
+    ========================================================================
+*/
+VOID ApRadarDetectPeriodic(
+	IN PRTMP_ADAPTER pAd)
+{
+	INT	i;
+
+	pAd->CommonCfg.RadarDetect.InServiceMonitorCount++;
+
+	for (i=0; i<pAd->ChannelListNum; i++)
+	{
+		if (pAd->ChannelList[i].RemainingTimeForUse > 0)
+		{
+			pAd->ChannelList[i].RemainingTimeForUse --;
+			if ((pAd->Mlme.PeriodicRound%5) == 0)
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("RadarDetectPeriodic - ch=%d, RemainingTimeForUse=%d\n", pAd->ChannelList[i].Channel, pAd->ChannelList[i].RemainingTimeForUse));
+			}
+		}
+	}
+
+	//radar detect
+	if ((pAd->CommonCfg.Channel > 14)
+		&& (pAd->CommonCfg.bIEEE80211H == 1)
+		&& RadarChannelCheck(pAd, pAd->CommonCfg.Channel))
+	{
+		RadarDetectPeriodic(pAd);
+	}
+
+	return;
+}
+
+// Periodic Radar detection, switch channel will occur in RTMPHandleTBTTInterrupt()
+// Before switch channel, driver needs doing channel switch announcement.
+VOID RadarDetectPeriodic(
+	IN PRTMP_ADAPTER	pAd)
+{
+	// need to check channel availability, after switch channel
+	if (pAd->CommonCfg.RadarDetect.RDMode != RD_SILENCE_MODE)
+			return;
+
+	// channel availability check time is 60sec, use 65 for assurance
+	if (pAd->CommonCfg.RadarDetect.RDCount++ > pAd->CommonCfg.RadarDetect.ChMovingTime)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("Not found radar signal, start send beacon and radar detection in service monitor\n\n"));
+			BbpRadarDetectionStop(pAd);
+		AsicEnableBssSync(pAd);
+		pAd->CommonCfg.RadarDetect.RDMode = RD_NORMAL_MODE;
+
+
+		return;
+	}
+
+	return;
+}
+
+
+/*
+    ==========================================================================
+    Description:
+		change channel moving time for DFS testing.
+
+	Arguments:
+	    pAdapter                    Pointer to our adapter
+	    wrq                         Pointer to the ioctl argument
+
+    Return Value:
+        None
+
+    Note:
+        Usage:
+               1.) iwpriv ra0 set ChMovTime=[value]
+    ==========================================================================
+*/
+INT Set_ChMovingTime_Proc(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR arg)
+{
+	UINT8 Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+	pAd->CommonCfg.RadarDetect.ChMovingTime = Value;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s:: %d\n", __FUNCTION__,
+		pAd->CommonCfg.RadarDetect.ChMovingTime));
+
+	return TRUE;
+}
+
+INT Set_LongPulseRadarTh_Proc(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR arg)
+{
+	UINT8 Value;
+
+	Value = simple_strtol(arg, 0, 10) > 10 ? 10 : simple_strtol(arg, 0, 10);
+
+	pAd->CommonCfg.RadarDetect.LongPulseRadarTh = Value;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s:: %d\n", __FUNCTION__,
+		pAd->CommonCfg.RadarDetect.LongPulseRadarTh));
+
+	return TRUE;
+}
+
+
diff --git a/drivers/staging/rt2860/common/eeprom.c b/drivers/staging/rt2860/common/eeprom.c
new file mode 100644
index 00000000000..bed2d666629
--- /dev/null
+++ b/drivers/staging/rt2860/common/eeprom.c
@@ -0,0 +1,244 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	eeprom.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+*/
+#include "../rt_config.h"
+
+// IRQL = PASSIVE_LEVEL
+VOID RaiseClock(
+    IN	PRTMP_ADAPTER	pAd,
+    IN  UINT32 *x)
+{
+    *x = *x | EESK;
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, *x);
+    RTMPusecDelay(1);				// Max frequency = 1MHz in Spec. definition
+}
+
+// IRQL = PASSIVE_LEVEL
+VOID LowerClock(
+    IN	PRTMP_ADAPTER	pAd,
+    IN  UINT32 *x)
+{
+    *x = *x & ~EESK;
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, *x);
+    RTMPusecDelay(1);
+}
+
+// IRQL = PASSIVE_LEVEL
+USHORT ShiftInBits(
+    IN	PRTMP_ADAPTER	pAd)
+{
+    UINT32		x,i;
+	USHORT      data=0;
+
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+
+    x &= ~( EEDO | EEDI);
+
+    for(i=0; i<16; i++)
+    {
+        data = data << 1;
+        RaiseClock(pAd, &x);
+
+        RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+
+        x &= ~(EEDI);
+        if(x & EEDO)
+            data |= 1;
+
+        LowerClock(pAd, &x);
+    }
+
+    return data;
+}
+
+// IRQL = PASSIVE_LEVEL
+VOID ShiftOutBits(
+    IN	PRTMP_ADAPTER	pAd,
+    IN  USHORT data,
+    IN  USHORT count)
+{
+    UINT32       x,mask;
+
+    mask = 0x01 << (count - 1);
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+
+    x &= ~(EEDO | EEDI);
+
+    do
+    {
+        x &= ~EEDI;
+        if(data & mask)		x |= EEDI;
+
+        RTMP_IO_WRITE32(pAd, E2PROM_CSR, x);
+
+        RaiseClock(pAd, &x);
+        LowerClock(pAd, &x);
+
+        mask = mask >> 1;
+    } while(mask);
+
+    x &= ~EEDI;
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, x);
+}
+
+// IRQL = PASSIVE_LEVEL
+VOID EEpromCleanup(
+    IN	PRTMP_ADAPTER	pAd)
+{
+    UINT32 x;
+
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+
+    x &= ~(EECS | EEDI);
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, x);
+
+    RaiseClock(pAd, &x);
+    LowerClock(pAd, &x);
+}
+
+VOID EWEN(
+	IN	PRTMP_ADAPTER	pAd)
+{
+    UINT32	x;
+
+    // reset bits and set EECS
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+    x &= ~(EEDI | EEDO | EESK);
+    x |= EECS;
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, x);
+
+	// kick a pulse
+	RaiseClock(pAd, &x);
+	LowerClock(pAd, &x);
+
+    // output the read_opcode and six pulse in that order
+    ShiftOutBits(pAd, EEPROM_EWEN_OPCODE, 5);
+    ShiftOutBits(pAd, 0, 6);
+
+    EEpromCleanup(pAd);
+}
+
+VOID EWDS(
+	IN	PRTMP_ADAPTER	pAd)
+{
+    UINT32	x;
+
+    // reset bits and set EECS
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+    x &= ~(EEDI | EEDO | EESK);
+    x |= EECS;
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, x);
+
+	// kick a pulse
+	RaiseClock(pAd, &x);
+	LowerClock(pAd, &x);
+
+    // output the read_opcode and six pulse in that order
+    ShiftOutBits(pAd, EEPROM_EWDS_OPCODE, 5);
+    ShiftOutBits(pAd, 0, 6);
+
+    EEpromCleanup(pAd);
+}
+
+// IRQL = PASSIVE_LEVEL
+USHORT RTMP_EEPROM_READ16(
+    IN	PRTMP_ADAPTER	pAd,
+    IN  USHORT Offset)
+{
+    UINT32		x;
+    USHORT		data;
+
+    Offset /= 2;
+    // reset bits and set EECS
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+    x &= ~(EEDI | EEDO | EESK);
+    x |= EECS;
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, x);
+
+	// kick a pulse
+	RaiseClock(pAd, &x);
+	LowerClock(pAd, &x);
+
+    // output the read_opcode and register number in that order
+    ShiftOutBits(pAd, EEPROM_READ_OPCODE, 3);
+    ShiftOutBits(pAd, Offset, pAd->EEPROMAddressNum);
+
+    // Now read the data (16 bits) in from the selected EEPROM word
+    data = ShiftInBits(pAd);
+
+    EEpromCleanup(pAd);
+
+    return data;
+}	//ReadEEprom
+
+VOID RTMP_EEPROM_WRITE16(
+    IN	PRTMP_ADAPTER	pAd,
+    IN  USHORT Offset,
+    IN  USHORT Data)
+{
+    UINT32 x;
+
+	Offset /= 2;
+
+	EWEN(pAd);
+
+    // reset bits and set EECS
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+    x &= ~(EEDI | EEDO | EESK);
+    x |= EECS;
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, x);
+
+	// kick a pulse
+	RaiseClock(pAd, &x);
+	LowerClock(pAd, &x);
+
+    // output the read_opcode ,register number and data in that order
+    ShiftOutBits(pAd, EEPROM_WRITE_OPCODE, 3);
+    ShiftOutBits(pAd, Offset, pAd->EEPROMAddressNum);
+	ShiftOutBits(pAd, Data, 16);		// 16-bit access
+
+    // read DO status
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+
+	EEpromCleanup(pAd);
+
+	RTMPusecDelay(10000);	//delay for twp(MAX)=10ms
+
+	EWDS(pAd);
+
+    EEpromCleanup(pAd);
+}
+
diff --git a/drivers/staging/rt2860/common/firmware.h b/drivers/staging/rt2860/common/firmware.h
new file mode 100644
index 00000000000..e72996f42c0
--- /dev/null
+++ b/drivers/staging/rt2860/common/firmware.h
@@ -0,0 +1,558 @@
+/*
+ Copyright (c) 2007, Ralink Technology Corporation
+ All rights reserved.
+
+ Redistribution.  Redistribution and use in binary form, without
+ modification, are permitted provided that the following conditions are
+ met:
+
+ 	* Redistributions must reproduce the above copyright notice and the
+ 	following disclaimer in the documentation and/or other materials
+ 	provided with the distribution.
+ 	* Neither the name of Ralink Technology Corporation nor the names of its
+ 	suppliers may be used to endorse or promote products derived from this
+ 	software without specific prior written permission.
+ 	* No reverse engineering, decompilation, or disassembly of this software
+ 	is permitted.
+
+ Limited patent license. Ralink Technology Corporation grants a world-wide,
+ royalty-free, non-exclusive license under patents it now or hereafter
+ owns or controls to make, have made, use, import, offer to sell and
+ sell ("Utilize") this software, but solely to the extent that any
+ such patent is necessary to Utilize the software alone, or in
+ combination with an operating system licensed under an approved Open
+ Source license as listed by the Open Source Initiative at
+ http://opensource.org/licenses.  The patent license shall not apply to
+ any other combinations which include this software.  No hardware per
+ se is licensed hereunder.
+
+ DISCLAIMER.  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,
+ BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
+ FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
+ TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
+ USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
+ DAMAGE.
+*/
+/* AUTO GEN PLEASE DO NOT MODIFY IT */
+/* AUTO GEN PLEASE DO NOT MODIFY IT */
+
+
+UCHAR FirmwareImage [] = {
+0x02, 0x03, 0x5e, 0x02, 0x02, 0xb1, 0x22, 0x22, 0xff, 0xff, 0xff, 0x02, 0x01, 0x82, 0xff, 0xff,
+0xff, 0xff, 0xff, 0x02, 0x00, 0x1e, 0xff, 0xff, 0xff, 0xff, 0xff, 0x02, 0x01, 0x33, 0xc0, 0xe0,
+0xc0, 0xf0, 0xc0, 0x83, 0xc0, 0x82, 0xc0, 0xd0, 0x75, 0xd0, 0x18, 0xc2, 0xaf, 0x30, 0x45, 0x03,
+0x12, 0x10, 0x09, 0x90, 0x04, 0x16, 0xe0, 0x30, 0xe3, 0x03, 0x74, 0x08, 0xf0, 0x90, 0x04, 0x14,
+0xe0, 0x20, 0xe7, 0x03, 0x02, 0x00, 0xcb, 0x74, 0x80, 0xf0, 0x90, 0x70, 0x12, 0xe0, 0xf5, 0x2f,
+0x90, 0x04, 0x04, 0xe0, 0x24, 0xcf, 0x60, 0x30, 0x14, 0x60, 0x42, 0x24, 0xe2, 0x60, 0x47, 0x14,
+0x60, 0x55, 0x24, 0x21, 0x70, 0x60, 0xe5, 0x55, 0x24, 0xfe, 0x60, 0x07, 0x14, 0x60, 0x08, 0x24,
+0x02, 0x70, 0x08, 0x7d, 0x01, 0x80, 0x28, 0x7d, 0x02, 0x80, 0x24, 0x90, 0x70, 0x10, 0xe0, 0xf5,
+0x50, 0x85, 0x2f, 0x40, 0xd2, 0x01, 0x80, 0x3e, 0xe5, 0x55, 0x64, 0x03, 0x60, 0x04, 0xe5, 0x55,
+0x70, 0x04, 0x7d, 0x02, 0x80, 0x09, 0x85, 0x2f, 0x41, 0xd2, 0x02, 0x80, 0x29, 0xad, 0x55, 0xaf,
+0x2f, 0x12, 0x02, 0x8d, 0x80, 0x20, 0x90, 0x70, 0x10, 0xe0, 0xf5, 0x47, 0x90, 0x70, 0x11, 0xe0,
+0xf5, 0x44, 0x12, 0x10, 0x25, 0x80, 0x06, 0x90, 0x70, 0x10, 0xe0, 0xf5, 0x45, 0xe4, 0xfd, 0xaf,
+0x2f, 0x12, 0x02, 0x8d, 0xd2, 0x04, 0x90, 0x70, 0x13, 0xe4, 0xf0, 0xd2, 0xaf, 0xd0, 0xd0, 0xd0,
+0x82, 0xd0, 0x83, 0xd0, 0xf0, 0xd0, 0xe0, 0x32, 0x90, 0x70, 0x2a, 0xe0, 0x30, 0xe1, 0x53, 0xc2,
+0xaf, 0x90, 0x70, 0x28, 0xe0, 0x90, 0x10, 0x1c, 0xf0, 0x90, 0x70, 0x29, 0xe0, 0x90, 0x10, 0x1d,
+0xf0, 0x90, 0x70, 0x2a, 0xe0, 0x90, 0x10, 0x1e, 0xf0, 0x90, 0x10, 0x1c, 0xe0, 0xf5, 0x30, 0x90,
+0x10, 0x1e, 0xe0, 0x20, 0xe1, 0xf3, 0x90, 0x10, 0x1c, 0xe0, 0x90, 0x70, 0x28, 0xf0, 0x90, 0x10,
+0x1d, 0xe0, 0x90, 0x70, 0x29, 0xf0, 0x90, 0x10, 0x1e, 0xe0, 0x90, 0x70, 0x2a, 0xf0, 0x30, 0x4a,
+0x0d, 0x90, 0x70, 0x24, 0xe0, 0x44, 0x01, 0xf0, 0x90, 0x02, 0x2c, 0x74, 0xff, 0xf0, 0xc2, 0x05,
+0xd2, 0xaf, 0x22, 0xc0, 0xe0, 0xc0, 0xf0, 0xc0, 0x83, 0xc0, 0x82, 0xc0, 0xd0, 0xe8, 0xc0, 0xe0,
+0xe9, 0xc0, 0xe0, 0xea, 0xc0, 0xe0, 0xeb, 0xc0, 0xe0, 0xec, 0xc0, 0xe0, 0xed, 0xc0, 0xe0, 0xee,
+0xc0, 0xe0, 0xef, 0xc0, 0xe0, 0xc2, 0xaf, 0x30, 0x45, 0x03, 0x12, 0x10, 0x12, 0xd2, 0xaf, 0xd0,
+0xe0, 0xff, 0xd0, 0xe0, 0xfe, 0xd0, 0xe0, 0xfd, 0xd0, 0xe0, 0xfc, 0xd0, 0xe0, 0xfb, 0xd0, 0xe0,
+0xfa, 0xd0, 0xe0, 0xf9, 0xd0, 0xe0, 0xf8, 0xd0, 0xd0, 0xd0, 0x82, 0xd0, 0x83, 0xd0, 0xf0, 0xd0,
+0xe0, 0x32, 0xc0, 0xe0, 0xc0, 0xf0, 0xc0, 0x83, 0xc0, 0x82, 0xc0, 0xd0, 0x75, 0xd0, 0x10, 0xc2,
+0xaf, 0x30, 0x45, 0x03, 0x12, 0x10, 0x0c, 0x30, 0x58, 0x0a, 0xe5, 0x54, 0x60, 0x04, 0x15, 0x54,
+0x80, 0x02, 0xc2, 0x58, 0x30, 0x59, 0x0a, 0xe5, 0x50, 0x60, 0x04, 0x15, 0x50, 0x80, 0x02, 0xc2,
+0x59, 0xd5, 0x53, 0x07, 0x30, 0x60, 0x04, 0x15, 0x46, 0xd2, 0x04, 0x30, 0x45, 0x03, 0x12, 0x10,
+0x0f, 0xc2, 0x8d, 0xd2, 0xaf, 0xd0, 0xd0, 0xd0, 0x82, 0xd0, 0x83, 0xd0, 0xf0, 0xd0, 0xe0, 0x32,
+0x12, 0x02, 0xd3, 0x30, 0x45, 0x03, 0x12, 0x10, 0x03, 0x30, 0x01, 0x06, 0x20, 0x09, 0x03, 0x12,
+0x10, 0x1c, 0x30, 0x02, 0x06, 0x20, 0x0a, 0x03, 0x12, 0x10, 0x1f, 0x30, 0x03, 0x06, 0x20, 0x0b,
+0x03, 0x12, 0x10, 0x1f, 0x30, 0x04, 0x06, 0x20, 0x0c, 0x03, 0x12, 0x10, 0x22, 0x20, 0x13, 0x09,
+0x20, 0x11, 0x06, 0xe5, 0x2b, 0x45, 0x2c, 0x60, 0x03, 0xd3, 0x80, 0x01, 0xc3, 0x92, 0xa9, 0x12,
+0x02, 0xec, 0x80, 0xbf, 0xc2, 0x43, 0xd2, 0x45, 0xe4, 0xf5, 0x20, 0xf5, 0x21, 0xf5, 0x53, 0xf5,
+0x46, 0xf5, 0x2b, 0xf5, 0x2c, 0xc2, 0x42, 0xf5, 0x51, 0xf5, 0x52, 0xf5, 0x55, 0x90, 0x04, 0x18,
+0x74, 0x80, 0xf0, 0x90, 0x04, 0x1a, 0x74, 0x08, 0xf0, 0xc2, 0x19, 0xc2, 0x18, 0xc2, 0x1a, 0x22,
+0xc8, 0xef, 0xc8, 0xe6, 0xfa, 0x08, 0xe6, 0x4a, 0x60, 0x0c, 0xc8, 0xef, 0xc8, 0x08, 0xe6, 0x16,
+0x18, 0x70, 0x01, 0x16, 0xc3, 0x22, 0xed, 0x24, 0xff, 0xfd, 0xec, 0x34, 0xff, 0xc8, 0xef, 0xc8,
+0xf6, 0x08, 0xc6, 0xed, 0xc6, 0xd3, 0x22, 0xd0, 0x83, 0xd0, 0x82, 0xf8, 0xe4, 0x93, 0x70, 0x12,
+0x74, 0x01, 0x93, 0x70, 0x0d, 0xa3, 0xa3, 0x93, 0xf8, 0x74, 0x01, 0x93, 0xf5, 0x82, 0x88, 0x83,
+0xe4, 0x73, 0x74, 0x02, 0x93, 0x68, 0x60, 0xef, 0xa3, 0xa3, 0xa3, 0x80, 0xdf, 0xef, 0xf4, 0x60,
+0x1f, 0xe4, 0xfe, 0x12, 0x03, 0x6a, 0xe0, 0xb4, 0xff, 0x12, 0x12, 0x03, 0x6a, 0xef, 0xf0, 0x74,
+0x1c, 0x2e, 0xf5, 0x82, 0xe4, 0x34, 0x70, 0xf5, 0x83, 0xed, 0xf0, 0x22, 0x0e, 0xbe, 0x04, 0xe3,
+0x22, 0xc0, 0xe0, 0xc0, 0xf0, 0xc0, 0x83, 0xc0, 0x82, 0xc0, 0xd0, 0x75, 0xd0, 0x08, 0xc2, 0xaf,
+0x30, 0x45, 0x03, 0x12, 0x10, 0x06, 0xd2, 0xaf, 0xd0, 0xd0, 0xd0, 0x82, 0xd0, 0x83, 0xd0, 0xf0,
+0xd0, 0xe0, 0x32, 0xc2, 0xaf, 0x12, 0x00, 0x06, 0x12, 0x02, 0x14, 0x12, 0x03, 0x1c, 0xe4, 0xf5,
+0x22, 0xf5, 0x47, 0x90, 0x04, 0x00, 0x74, 0x80, 0xf0, 0xd2, 0xaf, 0x22, 0x30, 0x45, 0x03, 0x12,
+0x10, 0x15, 0xe5, 0x20, 0x70, 0x03, 0x20, 0x10, 0x03, 0x30, 0x11, 0x03, 0x43, 0x87, 0x01, 0x22,
+0xc0, 0x2a, 0x74, 0x03, 0xc0, 0xe0, 0xc0, 0x82, 0xc0, 0x83, 0x75, 0x2a, 0x0a, 0x22, 0xc0, 0x2a,
+0x74, 0x03, 0xc0, 0xe0, 0xc0, 0x82, 0xc0, 0x83, 0x75, 0x2a, 0x18, 0x22, 0x75, 0x89, 0x02, 0xe4,
+0xf5, 0x8c, 0xf5, 0x8a, 0xf5, 0x88, 0xf5, 0xb8, 0xf5, 0xe8, 0x75, 0x90, 0x18, 0xd2, 0x8c, 0x75,
+0xa8, 0x05, 0x22, 0xce, 0xef, 0xce, 0xee, 0x60, 0x08, 0x7f, 0xff, 0x12, 0x03, 0x80, 0x1e, 0x80,
+0xf5, 0x22, 0xc8, 0xef, 0xc8, 0xe6, 0x60, 0x03, 0x16, 0xc3, 0x22, 0xed, 0x14, 0xf6, 0xd3, 0x22,
+0xc8, 0xef, 0xc8, 0xe6, 0x60, 0x06, 0x16, 0xe6, 0x24, 0xff, 0xb3, 0x22, 0xc3, 0x22, 0x78, 0x7f,
+0xe4, 0xf6, 0xd8, 0xfd, 0x75, 0x81, 0x5f, 0x02, 0x01, 0xd0, 0x74, 0x14, 0x2e, 0xf5, 0x82, 0xe4,
+0x34, 0x70, 0xf5, 0x83, 0x22, 0xef, 0x90, 0x03, 0x7e, 0x93, 0x90, 0x03, 0x00, 0x73, 0x0a, 0x18,
+0xef, 0x60, 0x03, 0x1f, 0x80, 0xfa, 0x22, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+0xff, 0xff, 0xff, 0x02, 0x10, 0x28, 0x02, 0x10, 0x3b, 0x02, 0x10, 0x3c, 0x02, 0x12, 0xb8, 0x02,
+0x12, 0xb9, 0x02, 0x13, 0x3e, 0x02, 0x13, 0x3f, 0xc3, 0x22, 0xff, 0xff, 0x02, 0x16, 0x56, 0x02,
+0x17, 0x6b, 0x02, 0x14, 0x2a, 0x02, 0x13, 0x40, 0x30, 0x05, 0x06, 0x20, 0x0d, 0x03, 0x12, 0x00,
+0xd8, 0x30, 0x06, 0x06, 0x20, 0x0e, 0x03, 0x12, 0x18, 0x5e, 0x22, 0x22, 0x90, 0x04, 0x14, 0xe0,
+0x20, 0xe7, 0x03, 0x02, 0x12, 0xb7, 0x90, 0x70, 0x12, 0xe0, 0xf5, 0x56, 0x90, 0x04, 0x04, 0xe0,
+0x12, 0x02, 0x67, 0x11, 0x4e, 0x30, 0x11, 0x25, 0x31, 0x10, 0x87, 0x33, 0x10, 0xaa, 0x34, 0x10,
+0xc3, 0x35, 0x11, 0x57, 0x50, 0x11, 0x7b, 0x51, 0x11, 0x84, 0x52, 0x11, 0x84, 0x53, 0x11, 0x84,
+0x54, 0x11, 0xc5, 0x55, 0x11, 0xdc, 0x70, 0x12, 0x07, 0x71, 0x12, 0x34, 0x72, 0x12, 0x5e, 0x80,
+0x12, 0x81, 0x83, 0x00, 0x00, 0x12, 0xb7, 0x75, 0x24, 0x05, 0x75, 0x25, 0xdc, 0x90, 0x70, 0x9f,
+0x74, 0x12, 0xf0, 0xd2, 0x18, 0xd2, 0x61, 0x75, 0x35, 0x0d, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5,
+0x56, 0xf4, 0x70, 0x03, 0x02, 0x12, 0xb7, 0x02, 0x12, 0xaa, 0xc2, 0x18, 0x90, 0x01, 0x14, 0xe0,
+0x54, 0xfd, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56, 0xf4, 0x70, 0x03, 0x02, 0x12, 0xb7,
+0x02, 0x12, 0xaa, 0xe5, 0x55, 0x64, 0x02, 0x70, 0x37, 0x90, 0x70, 0x10, 0xe0, 0x60, 0x08, 0x90,
+0x01, 0x0d, 0x74, 0x09, 0xf0, 0x80, 0x25, 0xe5, 0x34, 0x14, 0x60, 0x0a, 0x14, 0x60, 0x0f, 0x14,
+0x60, 0x14, 0x24, 0x03, 0x70, 0x16, 0x90, 0x01, 0x0d, 0x74, 0x08, 0xf0, 0x80, 0x0e, 0x90, 0x01,
+0x0d, 0x74, 0x0b, 0xf0, 0x80, 0x06, 0x90, 0x01, 0x0d, 0x74, 0x1b, 0xf0, 0x7d, 0x01, 0x80, 0x02,
+0x7d, 0x02, 0xaf, 0x56, 0x12, 0x02, 0x8d, 0x90, 0x70, 0x11, 0xe0, 0x24, 0xff, 0x92, 0x1b, 0x90,
+0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56, 0xf4, 0x70, 0x03, 0x02,
+0x12, 0xb7, 0x02, 0x12, 0xaa, 0x20, 0x02, 0x03, 0x30, 0x03, 0x1d, 0x7d, 0x02, 0xaf, 0x56, 0x12,
+0x02, 0x8d, 0x90, 0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56, 0xf4,
+0x70, 0x03, 0x02, 0x12, 0xb7, 0x02, 0x12, 0xaa, 0x85, 0x56, 0x41, 0xd2, 0x02, 0x22, 0x90, 0x70,
+0x11, 0xe0, 0x24, 0xff, 0x92, 0x1b, 0x22, 0x90, 0x70, 0x10, 0xe0, 0x54, 0x7f, 0x64, 0x02, 0x60,
+0x03, 0x02, 0x12, 0xb7, 0x90, 0x70, 0x11, 0xe0, 0x64, 0x08, 0x60, 0x08, 0xe0, 0x64, 0x20, 0x60,
+0x03, 0x02, 0x12, 0xb7, 0x75, 0x4e, 0x03, 0x75, 0x4f, 0x20, 0x22, 0x90, 0x70, 0x11, 0xe0, 0x24,
+0xff, 0x92, 0x47, 0x22, 0x90, 0x04, 0x04, 0xe0, 0x25, 0xe0, 0x24, 0x5d, 0xf5, 0x57, 0x90, 0x70,
+0x10, 0xe0, 0xff, 0x74, 0x47, 0x25, 0x57, 0xf8, 0xc6, 0xef, 0xc6, 0x90, 0x70, 0x11, 0xe0, 0xff,
+0x74, 0x48, 0x25, 0x57, 0xf8, 0xc6, 0xef, 0xc6, 0xe4, 0xfd, 0xaf, 0x56, 0x12, 0x02, 0x8d, 0x90,
+0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56, 0xf4, 0x70, 0x03, 0x02,
+0x12, 0xb7, 0x02, 0x12, 0xaa, 0xe5, 0x47, 0xb4, 0x07, 0x08, 0x90, 0x70, 0x11, 0xe0, 0x54, 0x07,
+0xf5, 0x26, 0xe4, 0xfd, 0xaf, 0x56, 0x12, 0x02, 0x8d, 0xd2, 0x04, 0x22, 0x90, 0x70, 0x10, 0xe0,
+0xfe, 0x90, 0x70, 0x11, 0xe0, 0xfd, 0xed, 0xf8, 0xe6, 0xf5, 0x57, 0xfd, 0xaf, 0x56, 0x12, 0x02,
+0x8d, 0x90, 0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56, 0xf4, 0x70,
+0x03, 0x02, 0x12, 0xb7, 0x02, 0x12, 0xaa, 0x90, 0x70, 0x10, 0xe0, 0xfe, 0x90, 0x70, 0x11, 0xe0,
+0xfd, 0xed, 0xf5, 0x82, 0x8e, 0x83, 0xe0, 0xf5, 0x57, 0xfd, 0xaf, 0x56, 0x12, 0x02, 0x8d, 0x90,
+0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56, 0xf4, 0x70, 0x03, 0x02,
+0x12, 0xb7, 0x80, 0x76, 0xe4, 0xf5, 0x4e, 0xf5, 0x4f, 0x75, 0x26, 0xff, 0xc2, 0x19, 0xc2, 0x18,
+0xc2, 0x1a, 0x75, 0x34, 0xff, 0xad, 0x57, 0xaf, 0x56, 0x12, 0x02, 0x8d, 0x90, 0x04, 0x14, 0x74,
+0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56, 0xf4, 0x60, 0x5b, 0x80, 0x4c, 0x90, 0x70,
+0x10, 0xe0, 0x24, 0xff, 0x92, 0x4a, 0xd2, 0x05, 0xad, 0x57, 0xaf, 0x56, 0x12, 0x02, 0x8d, 0x90,
+0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56, 0xf4, 0x60, 0x38, 0x80,
+0x29, 0x90, 0x70, 0x10, 0xe0, 0xf5, 0x34, 0xd3, 0x94, 0x00, 0x40, 0x07, 0x90, 0x01, 0x0d, 0xe0,
+0x54, 0xfb, 0xf0, 0xad, 0x57, 0xaf, 0x56, 0x12, 0x02, 0x8d, 0x90, 0x04, 0x14, 0x74, 0x80, 0xf0,
+0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56, 0xf4, 0x60, 0x0d, 0x90, 0x70, 0x25, 0xe0, 0x44, 0x01,
+0xf0, 0x90, 0x02, 0x2c, 0x74, 0xff, 0xf0, 0x22, 0x22, 0xe5, 0x53, 0x60, 0x03, 0x02, 0x13, 0x3d,
+0xe5, 0x4f, 0x45, 0x4e, 0x60, 0x08, 0xe5, 0x4f, 0x15, 0x4f, 0x70, 0x02, 0x15, 0x4e, 0xa2, 0x19,
+0xe4, 0x33, 0x90, 0x70, 0x90, 0xf0, 0xa2, 0x18, 0xe4, 0x33, 0xa3, 0xf0, 0x30, 0x19, 0x4d, 0x90,
+0x70, 0x98, 0x74, 0x23, 0xf0, 0xa3, 0xe5, 0x25, 0xf0, 0xe5, 0x24, 0xa3, 0xf0, 0x7f, 0x35, 0x7d,
+0x32, 0x12, 0x03, 0x42, 0x50, 0x09, 0x90, 0x10, 0x04, 0xe0, 0x54, 0xf7, 0xf0, 0xd2, 0x06, 0xe5,
+0x35, 0xd3, 0x94, 0x10, 0x40, 0x1e, 0x30, 0x1a, 0x1b, 0xc2, 0x1a, 0xa2, 0x18, 0x92, 0x19, 0x20,
+0x19, 0x12, 0x90, 0x04, 0x09, 0xe0, 0x54, 0xdd, 0xf0, 0x90, 0x10, 0x04, 0xe0, 0x44, 0x08, 0xf0,
+0xc2, 0x61, 0xd2, 0x03, 0xe5, 0x35, 0xb4, 0x0b, 0x14, 0xd2, 0x03, 0x22, 0xe4, 0xf5, 0x35, 0xa2,
+0x18, 0x92, 0x19, 0x30, 0x19, 0x07, 0x90, 0x04, 0x09, 0xe0, 0x44, 0x22, 0xf0, 0x22, 0x22, 0x22,
+0xc2, 0x4b, 0xc2, 0x4c, 0xe5, 0x44, 0x12, 0x02, 0x67, 0x13, 0x62, 0x00, 0x13, 0xf5, 0x04, 0x13,
+0xf1, 0x08, 0x13, 0xcc, 0x10, 0x13, 0x76, 0x20, 0x13, 0x96, 0x60, 0x13, 0xa7, 0xa0, 0x00, 0x00,
+0x13, 0xf7, 0x85, 0x48, 0x43, 0x85, 0x4a, 0x42, 0x85, 0x4c, 0x5e, 0xe5, 0x47, 0x64, 0x06, 0x60,
+0x03, 0x02, 0x13, 0xf7, 0x80, 0x1b, 0xe5, 0x48, 0xc4, 0x54, 0x0f, 0xf5, 0x43, 0xe5, 0x4a, 0xc4,
+0x54, 0x0f, 0xf5, 0x42, 0xe5, 0x4c, 0xc4, 0x54, 0x0f, 0xf5, 0x5e, 0xe5, 0x47, 0x64, 0x06, 0x70,
+0x66, 0x53, 0x43, 0x0f, 0x80, 0x61, 0x85, 0x49, 0x43, 0x85, 0x4b, 0x42, 0x85, 0x4d, 0x5e, 0xe5,
+0x47, 0x64, 0x06, 0x70, 0x52, 0x80, 0x1b, 0xe5, 0x49, 0xc4, 0x54, 0x0f, 0xf5, 0x43, 0xe5, 0x4b,
+0xc4, 0x54, 0x0f, 0xf5, 0x42, 0xe5, 0x4d, 0xc4, 0x54, 0x0f, 0xf5, 0x5e, 0xe5, 0x47, 0x64, 0x06,
+0x70, 0x35, 0xe5, 0x43, 0x54, 0x0f, 0x44, 0x10, 0xf5, 0x43, 0x80, 0x2b, 0xe5, 0x47, 0xb4, 0x04,
+0x06, 0x53, 0x5e, 0xfb, 0x75, 0x42, 0x09, 0xe5, 0x47, 0xb4, 0x05, 0x06, 0x43, 0x5e, 0x04, 0x75,
+0x42, 0x09, 0xe5, 0x47, 0xb4, 0x06, 0x10, 0xe5, 0x43, 0x54, 0x0f, 0x44, 0x30, 0xf5, 0x43, 0x80,
+0x06, 0xd2, 0x4b, 0x80, 0x02, 0xd2, 0x4c, 0xe4, 0xf5, 0x38, 0xe5, 0x42, 0xc4, 0x54, 0xf0, 0xff,
+0xe5, 0x43, 0x54, 0x0f, 0x4f, 0xf5, 0x5f, 0x90, 0x70, 0x44, 0xf0, 0xa3, 0xe5, 0x5e, 0xf0, 0xa3,
+0xe5, 0x4a, 0xf0, 0xa3, 0xe5, 0x48, 0xf0, 0xa3, 0xe5, 0x4c, 0xf0, 0xa3, 0xe5, 0x44, 0xf0, 0xa3,
+0xe5, 0x42, 0xf0, 0xa3, 0xe5, 0x43, 0xf0, 0xd2, 0x60, 0x22, 0xe5, 0x47, 0x60, 0x10, 0x24, 0xc0,
+0x70, 0x03, 0x12, 0x16, 0x36, 0x12, 0x14, 0x3f, 0xc2, 0xaf, 0xc2, 0x04, 0xd2, 0xaf, 0x22, 0xc2,
+0xaf, 0x90, 0x04, 0x14, 0xe0, 0x54, 0x0e, 0x60, 0x04, 0xd2, 0x1c, 0x80, 0x08, 0xe5, 0x4e, 0x45,
+0x4f, 0x24, 0xff, 0x92, 0x1c, 0xd2, 0xaf, 0x90, 0x04, 0x14, 0xe0, 0xa2, 0xe4, 0x92, 0x1d, 0x74,
+0x1e, 0xf0, 0xe5, 0x5f, 0x54, 0x0f, 0xf5, 0x2d, 0xe5, 0x38, 0x70, 0x13, 0x30, 0x1c, 0x05, 0xe5,
+0x5f, 0x20, 0xe5, 0x0b, 0x30, 0x1d, 0x19, 0xe5, 0x5f, 0x54, 0x30, 0xff, 0xbf, 0x30, 0x11, 0xe5,
+0x38, 0x70, 0x05, 0x75, 0x38, 0x0c, 0x80, 0x02, 0x15, 0x38, 0xd2, 0x6c, 0xd2, 0x6d, 0x80, 0x0f,
+0xe5, 0x5f, 0x30, 0xe6, 0x06, 0xc2, 0x6c, 0xd2, 0x6d, 0x80, 0x04, 0xd2, 0x6c, 0xc2, 0x6d, 0xe5,
+0x47, 0x64, 0x03, 0x70, 0x21, 0x30, 0x4b, 0x06, 0xc2, 0x6c, 0xd2, 0x6d, 0x80, 0x18, 0xe5, 0x38,
+0x70, 0x03, 0x30, 0x4c, 0x11, 0xc2, 0x4c, 0xe5, 0x38, 0x70, 0x05, 0x75, 0x38, 0x07, 0x80, 0x02,
+0x15, 0x38, 0xd2, 0x6c, 0xd2, 0x6d, 0x90, 0x70, 0x46, 0xe5, 0x2d, 0xf0, 0x20, 0x69, 0x07, 0xe5,
+0x5e, 0x20, 0xe0, 0x02, 0xb2, 0x68, 0x20, 0x6b, 0x07, 0xe5, 0x5e, 0x20, 0xe1, 0x02, 0xb2, 0x6a,
+0x20, 0x6d, 0x07, 0xe5, 0x5e, 0x20, 0xe2, 0x02, 0xb2, 0x6c, 0x90, 0x70, 0x47, 0xe5, 0x2d, 0xf0,
+0x75, 0x2e, 0x40, 0x20, 0x69, 0x04, 0xa2, 0x68, 0x80, 0x15, 0x30, 0x68, 0x06, 0xe5, 0x46, 0xa2,
+0xe3, 0x80, 0x0c, 0xe5, 0x46, 0x54, 0xf0, 0xff, 0xbf, 0xf0, 0x03, 0xd3, 0x80, 0x01, 0xc3, 0x92,
+0x73, 0x92, 0x72, 0x20, 0x6b, 0x04, 0xa2, 0x6a, 0x80, 0x15, 0x30, 0x6a, 0x06, 0xe5, 0x46, 0xa2,
+0xe3, 0x80, 0x0c, 0xe5, 0x46, 0x54, 0xf0, 0xff, 0xbf, 0xf0, 0x03, 0xd3, 0x80, 0x01, 0xc3, 0x92,
+0x75, 0x92, 0x74, 0x20, 0x6d, 0x04, 0xa2, 0x6c, 0x80, 0x15, 0x30, 0x6c, 0x06, 0xe5, 0x46, 0xa2,
+0xe3, 0x80, 0x0c, 0xe5, 0x46, 0x54, 0xf0, 0xff, 0xbf, 0xf0, 0x03, 0xd3, 0x80, 0x01, 0xc3, 0x92,
+0x71, 0x92, 0x70, 0x90, 0x10, 0x2f, 0xe5, 0x2e, 0xf0, 0xe5, 0x47, 0x64, 0x06, 0x70, 0x46, 0x90,
+0x02, 0x29, 0xe0, 0x54, 0xfe, 0xf0, 0xe5, 0x43, 0xc4, 0x54, 0x0f, 0x14, 0x60, 0x14, 0x24, 0xfe,
+0x60, 0x1f, 0x24, 0x03, 0x60, 0x03, 0x02, 0x16, 0x35, 0x90, 0x02, 0x28, 0xe0, 0x30, 0x47, 0x0d,
+0x80, 0x07, 0x90, 0x02, 0x28, 0xe0, 0x20, 0x47, 0x04, 0x54, 0xfe, 0xf0, 0x22, 0x44, 0x01, 0xf0,
+0x22, 0xe5, 0x46, 0x30, 0xe3, 0x04, 0x7f, 0x01, 0x80, 0x02, 0x7f, 0x00, 0x90, 0x02, 0x28, 0xe0,
+0x54, 0xfe, 0x4f, 0xf0, 0x22, 0xe5, 0x47, 0x64, 0x07, 0x60, 0x03, 0x02, 0x16, 0x35, 0xf5, 0x27,
+0x90, 0x02, 0x29, 0xe0, 0x54, 0xfc, 0xf0, 0xe5, 0x26, 0x14, 0x60, 0x26, 0x14, 0x60, 0x2e, 0x14,
+0x60, 0x36, 0x24, 0x03, 0x70, 0x5f, 0xe5, 0x46, 0x13, 0x13, 0x13, 0x54, 0x1f, 0x75, 0xf0, 0x03,
+0x84, 0xaf, 0xf0, 0x20, 0x47, 0x04, 0x7e, 0x01, 0x80, 0x02, 0x7e, 0x00, 0xef, 0x6e, 0x24, 0xff,
+0x80, 0x02, 0xa2, 0x47, 0x92, 0x39, 0xa2, 0x47, 0xb3, 0x92, 0x38, 0x80, 0x3f, 0xe5, 0x46, 0x30,
+0xe3, 0x03, 0xd3, 0x80, 0x27, 0xc3, 0x80, 0x24, 0xe5, 0x46, 0x30, 0xe3, 0x0d, 0x54, 0x70, 0xc3,
+0x94, 0x60, 0x50, 0x06, 0x7e, 0x00, 0x7f, 0x01, 0x80, 0x04, 0x7e, 0x00, 0x7f, 0x00, 0x20, 0x47,
+0x04, 0x7d, 0x01, 0x80, 0x02, 0x7d, 0x00, 0xef, 0x6d, 0x4e, 0x24, 0xff, 0x92, 0x38, 0xa2, 0x47,
+0xb3, 0x92, 0x39, 0x80, 0x07, 0xa2, 0x47, 0xb3, 0x92, 0x38, 0x92, 0x39, 0x90, 0x02, 0x28, 0xe0,
+0x54, 0xfc, 0x45, 0x27, 0xf0, 0x22, 0xe4, 0x90, 0x02, 0x29, 0xf0, 0x30, 0x47, 0x04, 0xaf, 0x45,
+0x80, 0x04, 0xe5, 0x45, 0xf4, 0xff, 0x90, 0x02, 0x28, 0xef, 0xf0, 0x22, 0x8f, 0x50, 0xd2, 0x59,
+0x22, 0x8f, 0x54, 0xd2, 0x58, 0x22, 0xe4, 0xf5, 0x30, 0xc2, 0xaf, 0xe5, 0x51, 0x14, 0x60, 0x4a,
+0x14, 0x60, 0x6a, 0x24, 0x02, 0x60, 0x03, 0x02, 0x17, 0x4c, 0xd2, 0x59, 0x75, 0x55, 0x01, 0x20,
+0x19, 0x1c, 0x90, 0x02, 0x08, 0xe0, 0x54, 0xfe, 0xf0, 0xe0, 0x20, 0xe1, 0x23, 0x90, 0x04, 0x34,
+0xe0, 0xb4, 0x02, 0x1c, 0xa3, 0xe0, 0xb4, 0x02, 0x17, 0xa3, 0xe0, 0xb4, 0x02, 0x12, 0x7f, 0x20,
+0x12, 0x16, 0x4c, 0x90, 0x10, 0x04, 0xe0, 0x54, 0xf3, 0xf0, 0x75, 0x51, 0x01, 0x02, 0x17, 0x4c,
+0xe5, 0x50, 0x70, 0x06, 0x75, 0x30, 0x03, 0x02, 0x17, 0x4c, 0x90, 0x12, 0x00, 0xe0, 0x54, 0x03,
+0x70, 0x15, 0x7f, 0x20, 0x12, 0x16, 0x4c, 0x20, 0x19, 0x07, 0x90, 0x02, 0x08, 0xe0, 0x54, 0xfb,
+0xf0, 0x75, 0x51, 0x02, 0x02, 0x17, 0x4c, 0xe5, 0x50, 0x70, 0x02, 0x80, 0x7a, 0x20, 0x19, 0x0f,
+0x90, 0x02, 0x08, 0xe0, 0x20, 0xe3, 0x6c, 0x90, 0x04, 0x37, 0xe0, 0x64, 0x22, 0x70, 0x64, 0x90,
+0x12, 0x04, 0x74, 0x0a, 0xf0, 0x30, 0x1b, 0x11, 0x90, 0x13, 0x28, 0xe0, 0x54, 0xf0, 0xf0, 0xa3,
+0xe0, 0x54, 0xf0, 0xf0, 0xa3, 0xe0, 0x54, 0xfa, 0xf0, 0x20, 0x19, 0x07, 0x90, 0x04, 0x01, 0xe0,
+0x44, 0x10, 0xf0, 0xe5, 0x34, 0xf4, 0x90, 0x04, 0x01, 0x60, 0x06, 0xe0, 0x54, 0xfb, 0xf0, 0x80,
+0x04, 0xe0, 0x54, 0xf9, 0xf0, 0x20, 0x19, 0x07, 0x90, 0x12, 0x04, 0xe0, 0x44, 0x04, 0xf0, 0xe5,
+0x34, 0xf4, 0x60, 0x14, 0x90, 0x01, 0x0d, 0xe0, 0xf5, 0x33, 0xe5, 0x34, 0xd3, 0x94, 0x02, 0x40,
+0x07, 0x90, 0x12, 0x04, 0xe0, 0x54, 0xfd, 0xf0, 0x75, 0x30, 0x01, 0x75, 0x55, 0x02, 0xe4, 0xf5,
+0x51, 0x80, 0x09, 0xe5, 0x50, 0x70, 0x05, 0x75, 0x30, 0x03, 0xf5, 0x51, 0xe5, 0x30, 0x60, 0x18,
+0xc2, 0x01, 0xe4, 0xf5, 0x51, 0xc2, 0x59, 0x20, 0x19, 0x0e, 0xad, 0x30, 0xaf, 0x40, 0x12, 0x18,
+0x2a, 0xe5, 0x30, 0xb4, 0x03, 0x02, 0xd2, 0x03, 0xd2, 0xaf, 0x22, 0xc2, 0xaf, 0x30, 0x01, 0x0e,
+0xe4, 0xf5, 0x51, 0xc2, 0x59, 0xc2, 0x01, 0x7d, 0x02, 0xaf, 0x40, 0x12, 0x18, 0x2a, 0xe5, 0x52,
+0x14, 0x60, 0x55, 0x14, 0x60, 0x2f, 0x24, 0x02, 0x60, 0x03, 0x02, 0x18, 0x27, 0xe5, 0x34, 0xf4,
+0x60, 0x23, 0xe5, 0x34, 0xd3, 0x94, 0x02, 0x40, 0x16, 0x90, 0x12, 0x04, 0xe0, 0x44, 0x02, 0xf0,
+0x90, 0x01, 0x0d, 0xe0, 0x20, 0xe3, 0x03, 0x02, 0x18, 0x27, 0x7f, 0x50, 0x12, 0x16, 0x51, 0x75,
+0x52, 0x02, 0x75, 0x55, 0x03, 0xe5, 0x34, 0xf4, 0x60, 0x0a, 0xe5, 0x54, 0x70, 0x69, 0x90, 0x01,
+0x0d, 0xe5, 0x33, 0xf0, 0x90, 0x12, 0x04, 0xe0, 0x54, 0xfb, 0xf0, 0x7f, 0x20, 0x12, 0x16, 0x51,
+0x75, 0x52, 0x01, 0x75, 0x55, 0x03, 0x80, 0x4f, 0xe5, 0x54, 0x70, 0x4b, 0x90, 0x04, 0x01, 0xe0,
+0x44, 0x0e, 0xf0, 0x20, 0x19, 0x04, 0xe0, 0x54, 0xef, 0xf0, 0x90, 0x13, 0x28, 0xe0, 0x44, 0x0f,
+0xf0, 0xa3, 0xe0, 0x44, 0x0f, 0xf0, 0xa3, 0xe0, 0x44, 0x05, 0xf0, 0x90, 0x12, 0x04, 0x74, 0x03,
+0xf0, 0x20, 0x19, 0x07, 0x90, 0x02, 0x08, 0xe0, 0x44, 0x05, 0xf0, 0x90, 0x10, 0x04, 0xe0, 0x44,
+0x0c, 0xf0, 0xe4, 0xf5, 0x52, 0xf5, 0x55, 0x30, 0x02, 0x0b, 0xc2, 0x02, 0x7d, 0x01, 0xaf, 0x41,
+0x12, 0x18, 0x2a, 0x80, 0x02, 0xc2, 0x03, 0xd2, 0xaf, 0x22, 0xef, 0xf4, 0x60, 0x2d, 0xe4, 0xfe,
+0x74, 0x14, 0x2e, 0xf5, 0x82, 0xe4, 0x34, 0x70, 0xf5, 0x83, 0xe0, 0xb4, 0xff, 0x19, 0x74, 0x14,
+0x2e, 0xf5, 0x82, 0xe4, 0x34, 0x70, 0xf5, 0x83, 0xef, 0xf0, 0x74, 0x1c, 0x2e, 0xf5, 0x82, 0xe4,
+0x34, 0x70, 0xf5, 0x83, 0xed, 0xf0, 0x22, 0x0e, 0xbe, 0x04, 0xd5, 0x22, 0x22, 0x22, 0x20, 0x19,
+0x03, 0x02, 0x19, 0x0f, 0x90, 0x70, 0x80, 0xe0, 0x04, 0xf0, 0x90, 0x04, 0x37, 0xe0, 0x30, 0xe5,
+0x03, 0x02, 0x19, 0x0b, 0x90, 0x04, 0x28, 0xe0, 0xf5, 0x31, 0xa3, 0xe0, 0xf5, 0x30, 0xf5, 0x32,
+0xe4, 0xf5, 0x37, 0x90, 0x70, 0x81, 0xe0, 0x04, 0xf0, 0x90, 0x70, 0x82, 0xe0, 0x04, 0xf0, 0xe5,
+0x32, 0x75, 0xf0, 0x80, 0xa4, 0x24, 0x00, 0xff, 0xe5, 0xf0, 0x34, 0x80, 0xfe, 0xe5, 0x30, 0x65,
+0x32, 0x70, 0x05, 0xfc, 0x7d, 0x18, 0x80, 0x04, 0x7c, 0x00, 0x7d, 0x00, 0xef, 0x2d, 0xff, 0xee,
+0x3c, 0xfe, 0x12, 0x19, 0x10, 0x50, 0x25, 0x90, 0x70, 0x83, 0xe0, 0x04, 0xf0, 0x90, 0x01, 0x14,
+0xe0, 0x44, 0x02, 0xf0, 0xe0, 0x30, 0xe1, 0x06, 0x90, 0x70, 0x92, 0x74, 0x45, 0xf0, 0x90, 0x70,
+0x93, 0xe0, 0x04, 0xf0, 0x90, 0x04, 0x01, 0xe0, 0x90, 0x70, 0x94, 0xf0, 0xe5, 0x32, 0x65, 0x31,
+0x60, 0x10, 0xe4, 0x25, 0x32, 0xff, 0xe4, 0x34, 0x80, 0x8f, 0x82, 0xf5, 0x83, 0xe0, 0xf5, 0x32,
+0x80, 0x97, 0x90, 0x04, 0x10, 0x74, 0x01, 0xf0, 0x90, 0x04, 0x28, 0xe5, 0x31, 0xf0, 0xa3, 0xe5,
+0x30, 0xf0, 0x90, 0x04, 0x11, 0x74, 0x01, 0xf0, 0x02, 0x18, 0x6a, 0xc2, 0x06, 0xd2, 0x1a, 0x22,
+0x90, 0x70, 0x84, 0xe5, 0x37, 0xf0, 0xc3, 0x94, 0x06, 0x50, 0x19, 0x8f, 0x82, 0x8e, 0x83, 0xe0,
+0xb4, 0xff, 0x07, 0x05, 0x37, 0xe4, 0xf5, 0x36, 0x80, 0x59, 0xe4, 0xf5, 0x37, 0x8f, 0x82, 0x8e,
+0x83, 0xf0, 0x80, 0x4f, 0xe5, 0x36, 0x75, 0xf0, 0x06, 0x84, 0x74, 0x08, 0x25, 0xf0, 0xf5, 0x82,
+0xe4, 0x34, 0x10, 0xf5, 0x83, 0xe0, 0xfd, 0x8f, 0x82, 0x8e, 0x83, 0xe0, 0xfc, 0x6d, 0x70, 0x30,
+0x90, 0x70, 0x88, 0xe0, 0x04, 0xf0, 0xa3, 0xe0, 0xfd, 0xd3, 0x95, 0x37, 0x40, 0x02, 0x80, 0x02,
+0xad, 0x37, 0x90, 0x70, 0x89, 0xed, 0xf0, 0x05, 0x37, 0x05, 0x36, 0xe5, 0x36, 0x75, 0xf0, 0x06,
+0x84, 0x74, 0x8a, 0x25, 0xf0, 0xf5, 0x82, 0xe4, 0x34, 0x70, 0xf5, 0x83, 0xec, 0xf0, 0x80, 0x03,
+0xe4, 0xf5, 0x37, 0x0f, 0xbf, 0x00, 0x01, 0x0e, 0xef, 0x54, 0x7f, 0x60, 0x0a, 0xe5, 0x37, 0xc3,
+0x94, 0x4e, 0x50, 0x03, 0x02, 0x19, 0x10, 0xe5, 0x37, 0xb4, 0x4e, 0x03, 0xd3, 0x80, 0x01, 0xc3,
+0x22, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0b, 0x94, 0xeb, } ;
diff --git a/drivers/staging/rt2860/common/md5.c b/drivers/staging/rt2860/common/md5.c
new file mode 100644
index 00000000000..774776b4b8c
--- /dev/null
+++ b/drivers/staging/rt2860/common/md5.c
@@ -0,0 +1,1427 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    md5.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+	jan			10-28-03		Initial
+	Rita    	11-23-04		Modify MD5 and SHA-1
+	Rita		10-14-05		Modify SHA-1 in big-endian platform
+ */
+#include "../rt_config.h"
+
+/**
+ * md5_mac:
+ * @key: pointer to	the	key	used for MAC generation
+ * @key_len: length	of the key in bytes
+ * @data: pointer to the data area for which the MAC is	generated
+ * @data_len: length of	the	data in	bytes
+ * @mac: pointer to	the	buffer holding space for the MAC; the buffer should
+ * have	space for 128-bit (16 bytes) MD5 hash value
+ *
+ * md5_mac() determines	the	message	authentication code	by using secure	hash
+ * MD5(key | data |	key).
+ */
+void md5_mac(u8 *key, size_t key_len, u8 *data, size_t data_len, u8 *mac)
+{
+	MD5_CTX	context;
+
+	MD5Init(&context);
+	MD5Update(&context,	key, key_len);
+	MD5Update(&context,	data, data_len);
+	MD5Update(&context,	key, key_len);
+	MD5Final(mac, &context);
+}
+
+/**
+ * hmac_md5:
+ * @key: pointer to	the	key	used for MAC generation
+ * @key_len: length	of the key in bytes
+ * @data: pointer to the data area for which the MAC is	generated
+ * @data_len: length of	the	data in	bytes
+ * @mac: pointer to	the	buffer holding space for the MAC; the buffer should
+ * have	space for 128-bit (16 bytes) MD5 hash value
+ *
+ * hmac_md5() determines the message authentication	code using HMAC-MD5.
+ * This	implementation is based	on the sample code presented in	RFC	2104.
+ */
+void hmac_md5(u8 *key, size_t key_len, u8 *data, size_t data_len, u8 *mac)
+{
+	MD5_CTX	context;
+    u8 k_ipad[65]; /* inner padding - key XORd with ipad */
+    u8 k_opad[65]; /* outer padding - key XORd with opad */
+    u8 tk[16];
+	int	i;
+
+	//assert(key != NULL && data != NULL && mac != NULL);
+
+	/* if key is longer	than 64	bytes reset	it to key =	MD5(key) */
+	if (key_len	> 64) {
+		MD5_CTX	ttcontext;
+
+		MD5Init(&ttcontext);
+		MD5Update(&ttcontext, key, key_len);
+		MD5Final(tk, &ttcontext);
+		//key=(PUCHAR)ttcontext.buf;
+		key	= tk;
+		key_len	= 16;
+	}
+
+	/* the HMAC_MD5	transform looks	like:
+	 *
+	 * MD5(K XOR opad, MD5(K XOR ipad, text))
+	 *
+	 * where K is an n byte	key
+	 * ipad	is the byte	0x36 repeated 64 times
+	 * opad	is the byte	0x5c repeated 64 times
+	 * and text	is the data	being protected	*/
+
+	/* start out by	storing	key	in pads	*/
+	NdisZeroMemory(k_ipad, sizeof(k_ipad));
+	NdisZeroMemory(k_opad,	sizeof(k_opad));
+	//assert(key_len < sizeof(k_ipad));
+	NdisMoveMemory(k_ipad, key,	key_len);
+	NdisMoveMemory(k_opad, key,	key_len);
+
+	/* XOR key with	ipad and opad values */
+	for	(i = 0;	i <	64;	i++) {
+		k_ipad[i] ^= 0x36;
+		k_opad[i] ^= 0x5c;
+	}
+
+	/* perform inner MD5 */
+	MD5Init(&context);					 /*	init context for 1st pass */
+	MD5Update(&context,	k_ipad,	64);	 /*	start with inner pad */
+	MD5Update(&context,	data, data_len); /*	then text of datagram */
+	MD5Final(mac, &context);			 /*	finish up 1st pass */
+
+	/* perform outer MD5 */
+	MD5Init(&context);					 /*	init context for 2nd pass */
+	MD5Update(&context,	k_opad,	64);	 /*	start with outer pad */
+	MD5Update(&context,	mac, 16);		 /*	then results of	1st	hash */
+	MD5Final(mac, &context);			 /*	finish up 2nd pass */
+}
+
+#ifndef RT_BIG_ENDIAN
+#define byteReverse(buf, len)   /* Nothing */
+#else
+void byteReverse(unsigned char *buf, unsigned longs);
+void byteReverse(unsigned char *buf, unsigned longs)
+{
+    do {
+        *(UINT32 *)buf = SWAP32(*(UINT32 *)buf);
+        buf += 4;
+    } while (--longs);
+}
+#endif
+
+
+/* ==========================  MD5 implementation =========================== */
+// four base functions for MD5
+#define MD5_F1(x, y, z) (((x) & (y)) | ((~x) & (z)))
+#define MD5_F2(x, y, z) (((x) & (z)) | ((y) & (~z)))
+#define MD5_F3(x, y, z) ((x) ^ (y) ^ (z))
+#define MD5_F4(x, y, z) ((y) ^ ((x) | (~z)))
+#define CYCLIC_LEFT_SHIFT(w, s) (((w) << (s)) | ((w) >> (32-(s))))
+
+#define	MD5Step(f, w, x, y,	z, data, t, s)	\
+	( w	+= f(x,	y, z) +	data + t,  w = (CYCLIC_LEFT_SHIFT(w, s)) & 0xffffffff, w +=	x )
+
+
+/*
+ *  Function Description:
+ *      Initiate MD5 Context satisfied in RFC 1321
+ *
+ *  Arguments:
+ *      pCtx        Pointer	to MD5 context
+ *
+ *  Return Value:
+ *      None
+ */
+VOID MD5Init(MD5_CTX *pCtx)
+{
+    pCtx->Buf[0]=0x67452301;
+    pCtx->Buf[1]=0xefcdab89;
+    pCtx->Buf[2]=0x98badcfe;
+    pCtx->Buf[3]=0x10325476;
+
+    pCtx->LenInBitCount[0]=0;
+    pCtx->LenInBitCount[1]=0;
+}
+
+
+/*
+ *  Function Description:
+ *      Update MD5 Context, allow of an arrary of octets as the next portion
+ *      of the message
+ *
+ *  Arguments:
+ *      pCtx		Pointer	to MD5 context
+ * 	    pData       Pointer to input data
+ *      LenInBytes  The length of input data (unit: byte)
+ *
+ *  Return Value:
+ *      None
+ *
+ *  Note:
+ *      Called after MD5Init or MD5Update(itself)
+ */
+VOID MD5Update(MD5_CTX *pCtx, UCHAR *pData, UINT32 LenInBytes)
+{
+
+    UINT32 TfTimes;
+    UINT32 temp;
+	unsigned int i;
+
+    temp = pCtx->LenInBitCount[0];
+
+    pCtx->LenInBitCount[0] = (UINT32) (pCtx->LenInBitCount[0] + (LenInBytes << 3));
+
+    if (pCtx->LenInBitCount[0] < temp)
+        pCtx->LenInBitCount[1]++;   //carry in
+
+    pCtx->LenInBitCount[1] += LenInBytes >> 29;
+
+    // mod 64 bytes
+    temp = (temp >> 3) & 0x3f;
+
+    // process lacks of 64-byte data
+    if (temp)
+    {
+        UCHAR *pAds = (UCHAR *) pCtx->Input + temp;
+
+        if ((temp+LenInBytes) < 64)
+        {
+            NdisMoveMemory(pAds, (UCHAR *)pData, LenInBytes);
+            return;
+        }
+
+        NdisMoveMemory(pAds, (UCHAR *)pData, 64-temp);
+        byteReverse(pCtx->Input, 16);
+        MD5Transform(pCtx->Buf, (UINT32 *)pCtx->Input);
+
+        pData += 64-temp;
+        LenInBytes -= 64-temp;
+    } // end of if (temp)
+
+
+    TfTimes = (LenInBytes >> 6);
+
+    for (i=TfTimes; i>0; i--)
+    {
+        NdisMoveMemory(pCtx->Input, (UCHAR *)pData, 64);
+        byteReverse(pCtx->Input, 16);
+        MD5Transform(pCtx->Buf, (UINT32 *)pCtx->Input);
+        pData += 64;
+        LenInBytes -= 64;
+    } // end of for
+
+    // buffering lacks of 64-byte data
+    if(LenInBytes)
+        NdisMoveMemory(pCtx->Input, (UCHAR *)pData, LenInBytes);
+
+}
+
+
+/*
+ *  Function Description:
+ *      Append padding bits and length of original message in the tail
+ *      The message digest has to be completed in the end
+ *
+ *  Arguments:
+ *      Digest		Output of Digest-Message for MD5
+ *  	pCtx        Pointer	to MD5 context
+ *
+ *  Return Value:
+ *      None
+ *
+ *  Note:
+ *      Called after MD5Update
+ */
+VOID MD5Final(UCHAR Digest[16], MD5_CTX *pCtx)
+{
+    UCHAR Remainder;
+    UCHAR PadLenInBytes;
+    UCHAR *pAppend=0;
+    unsigned int i;
+
+    Remainder = (UCHAR)((pCtx->LenInBitCount[0] >> 3) & 0x3f);
+
+    PadLenInBytes = (Remainder < 56) ? (56-Remainder) : (120-Remainder);
+
+    pAppend = (UCHAR *)pCtx->Input + Remainder;
+
+    // padding bits without crossing block(64-byte based) boundary
+    if (Remainder < 56)
+    {
+        *pAppend = 0x80;
+        PadLenInBytes --;
+
+        NdisZeroMemory((UCHAR *)pCtx->Input + Remainder+1, PadLenInBytes);
+
+		// add data-length field, from low to high
+       	for (i=0; i<4; i++)
+        {
+        	pCtx->Input[56+i] = (UCHAR)((pCtx->LenInBitCount[0] >> (i << 3)) & 0xff);
+        	pCtx->Input[60+i] = (UCHAR)((pCtx->LenInBitCount[1] >> (i << 3)) & 0xff);
+      	}
+
+        byteReverse(pCtx->Input, 16);
+        MD5Transform(pCtx->Buf, (UINT32 *)pCtx->Input);
+    } // end of if
+
+    // padding bits with crossing block(64-byte based) boundary
+    else
+    {
+        // the first block ===
+        *pAppend = 0x80;
+        PadLenInBytes --;
+
+        NdisZeroMemory((UCHAR *)pCtx->Input + Remainder+1, (64-Remainder-1));
+        PadLenInBytes -= (64 - Remainder - 1);
+
+        byteReverse(pCtx->Input, 16);
+        MD5Transform(pCtx->Buf, (UINT32 *)pCtx->Input);
+
+
+        // the second block ===
+        NdisZeroMemory((UCHAR *)pCtx->Input, PadLenInBytes);
+
+        // add data-length field
+        for (i=0; i<4; i++)
+        {
+        	pCtx->Input[56+i] = (UCHAR)((pCtx->LenInBitCount[0] >> (i << 3)) & 0xff);
+        	pCtx->Input[60+i] = (UCHAR)((pCtx->LenInBitCount[1] >> (i << 3)) & 0xff);
+      	}
+
+        byteReverse(pCtx->Input, 16);
+        MD5Transform(pCtx->Buf, (UINT32 *)pCtx->Input);
+    } // end of else
+
+
+    NdisMoveMemory((UCHAR *)Digest, (UINT32 *)pCtx->Buf, 16); // output
+    byteReverse((UCHAR *)Digest, 4);
+    NdisZeroMemory(pCtx, sizeof(pCtx)); // memory free
+}
+
+
+/*
+ *  Function Description:
+ *      The central algorithm of MD5, consists of four rounds and sixteen
+ *  	steps per round
+ *
+ *  Arguments:
+ *      Buf     Buffers of four states (output: 16 bytes)
+ * 	    Mes     Input data (input: 64 bytes)
+ *
+ *  Return Value:
+ *      None
+ *
+ *  Note:
+ *      Called by MD5Update or MD5Final
+ */
+VOID MD5Transform(UINT32 Buf[4], UINT32 Mes[16])
+{
+    UINT32 Reg[4], Temp;
+	unsigned int i;
+
+    static UCHAR LShiftVal[16] =
+    {
+        7, 12, 17, 22,
+		5, 9 , 14, 20,
+		4, 11, 16, 23,
+ 		6, 10, 15, 21,
+ 	};
+
+
+	// [equal to 4294967296*abs(sin(index))]
+    static UINT32 MD5Table[64] =
+	{
+		0xd76aa478,	0xe8c7b756,	0x242070db,	0xc1bdceee,
+		0xf57c0faf,	0x4787c62a,	0xa8304613, 0xfd469501,
+		0x698098d8,	0x8b44f7af,	0xffff5bb1,	0x895cd7be,
+    	0x6b901122,	0xfd987193,	0xa679438e,	0x49b40821,
+
+    	0xf61e2562,	0xc040b340,	0x265e5a51,	0xe9b6c7aa,
+    	0xd62f105d,	0x02441453,	0xd8a1e681,	0xe7d3fbc8,
+    	0x21e1cde6,	0xc33707d6,	0xf4d50d87,	0x455a14ed,
+    	0xa9e3e905,	0xfcefa3f8,	0x676f02d9,	0x8d2a4c8a,
+
+    	0xfffa3942,	0x8771f681,	0x6d9d6122,	0xfde5380c,
+    	0xa4beea44,	0x4bdecfa9,	0xf6bb4b60,	0xbebfbc70,
+    	0x289b7ec6,	0xeaa127fa,	0xd4ef3085,	0x04881d05,
+    	0xd9d4d039,	0xe6db99e5,	0x1fa27cf8,	0xc4ac5665,
+
+    	0xf4292244,	0x432aff97,	0xab9423a7,	0xfc93a039,
+   		0x655b59c3,	0x8f0ccc92,	0xffeff47d,	0x85845dd1,
+    	0x6fa87e4f,	0xfe2ce6e0,	0xa3014314,	0x4e0811a1,
+    	0xf7537e82,	0xbd3af235,	0x2ad7d2bb,	0xeb86d391
+	};
+
+
+    for (i=0; i<4; i++)
+        Reg[i]=Buf[i];
+
+
+    // 64 steps in MD5 algorithm
+    for (i=0; i<16; i++)
+    {
+        MD5Step(MD5_F1, Reg[0], Reg[1], Reg[2], Reg[3], Mes[i],
+                MD5Table[i], LShiftVal[i & 0x3]);
+
+        // one-word right shift
+        Temp   = Reg[3];
+        Reg[3] = Reg[2];
+        Reg[2] = Reg[1];
+        Reg[1] = Reg[0];
+        Reg[0] = Temp;
+    }
+    for (i=16; i<32; i++)
+    {
+        MD5Step(MD5_F2, Reg[0], Reg[1], Reg[2], Reg[3], Mes[(5*(i & 0xf)+1) & 0xf],
+                MD5Table[i], LShiftVal[(0x1 << 2)+(i & 0x3)]);
+
+        // one-word right shift
+        Temp   = Reg[3];
+        Reg[3] = Reg[2];
+        Reg[2] = Reg[1];
+        Reg[1] = Reg[0];
+        Reg[0] = Temp;
+    }
+    for (i=32; i<48; i++)
+    {
+        MD5Step(MD5_F3, Reg[0], Reg[1], Reg[2], Reg[3], Mes[(3*(i & 0xf)+5) & 0xf],
+                MD5Table[i], LShiftVal[(0x1 << 3)+(i & 0x3)]);
+
+        // one-word right shift
+        Temp   = Reg[3];
+        Reg[3] = Reg[2];
+        Reg[2] = Reg[1];
+        Reg[1] = Reg[0];
+        Reg[0] = Temp;
+    }
+    for (i=48; i<64; i++)
+    {
+        MD5Step(MD5_F4, Reg[0], Reg[1], Reg[2], Reg[3], Mes[(7*(i & 0xf)) & 0xf],
+                MD5Table[i], LShiftVal[(0x3 << 2)+(i & 0x3)]);
+
+        // one-word right shift
+        Temp   = Reg[3];
+        Reg[3] = Reg[2];
+        Reg[2] = Reg[1];
+        Reg[1] = Reg[0];
+        Reg[0] = Temp;
+    }
+
+
+    // (temporary)output
+    for (i=0; i<4; i++)
+        Buf[i] += Reg[i];
+
+}
+
+
+
+/* =========================  SHA-1 implementation ========================== */
+// four base functions for SHA-1
+#define SHA1_F1(b, c, d)    (((b) & (c)) | ((~b) & (d)))
+#define SHA1_F2(b, c, d)    ((b) ^ (c) ^ (d))
+#define SHA1_F3(b, c, d)    (((b) & (c)) | ((b) & (d)) | ((c) & (d)))
+
+
+#define SHA1Step(f, a, b, c, d, e, w, k)    \
+    ( e	+= ( f(b, c, d) + w + k + CYCLIC_LEFT_SHIFT(a, 5)) & 0xffffffff, \
+      b = CYCLIC_LEFT_SHIFT(b, 30) )
+
+//Initiate SHA-1 Context satisfied in RFC 3174
+VOID SHAInit(SHA_CTX *pCtx)
+{
+    pCtx->Buf[0]=0x67452301;
+    pCtx->Buf[1]=0xefcdab89;
+    pCtx->Buf[2]=0x98badcfe;
+    pCtx->Buf[3]=0x10325476;
+    pCtx->Buf[4]=0xc3d2e1f0;
+
+    pCtx->LenInBitCount[0]=0;
+    pCtx->LenInBitCount[1]=0;
+}
+
+/*
+ *  Function Description:
+ *      Update SHA-1 Context, allow of an arrary of octets as the next
+ *      portion of the message
+ *
+ *  Arguments:
+ *      pCtx		Pointer	to SHA-1 context
+ * 	    pData       Pointer to input data
+ *      LenInBytes  The length of input data (unit: byte)
+ *
+ *  Return Value:
+ *      error       indicate more than pow(2,64) bits of data
+ *
+ *  Note:
+ *      Called after SHAInit or SHAUpdate(itself)
+ */
+UCHAR SHAUpdate(SHA_CTX *pCtx, UCHAR *pData, UINT32 LenInBytes)
+{
+    UINT32 TfTimes;
+    UINT32 temp1,temp2;
+	unsigned int i;
+	UCHAR err=1;
+
+    temp1 = pCtx->LenInBitCount[0];
+    temp2 = pCtx->LenInBitCount[1];
+
+    pCtx->LenInBitCount[0] = (UINT32) (pCtx->LenInBitCount[0] + (LenInBytes << 3));
+    if (pCtx->LenInBitCount[0] < temp1)
+        pCtx->LenInBitCount[1]++;   //carry in
+
+
+    pCtx->LenInBitCount[1] = (UINT32) (pCtx->LenInBitCount[1] +(LenInBytes >> 29));
+    if (pCtx->LenInBitCount[1] < temp2)
+        return (err);   //check total length of original data
+
+
+    // mod 64 bytes
+    temp1 = (temp1 >> 3) & 0x3f;
+
+    // process lacks of 64-byte data
+    if (temp1)
+    {
+        UCHAR *pAds = (UCHAR *) pCtx->Input + temp1;
+
+        if ((temp1+LenInBytes) < 64)
+        {
+            NdisMoveMemory(pAds, (UCHAR *)pData, LenInBytes);
+            return (0);
+        }
+
+        NdisMoveMemory(pAds, (UCHAR *)pData, 64-temp1);
+        byteReverse((UCHAR *)pCtx->Input, 16);
+
+        NdisZeroMemory((UCHAR *)pCtx->Input + 64, 16);
+        SHATransform(pCtx->Buf, (UINT32 *)pCtx->Input);
+
+        pData += 64-temp1;
+        LenInBytes -= 64-temp1;
+    } // end of if (temp1)
+
+
+    TfTimes = (LenInBytes >> 6);
+
+    for (i=TfTimes; i>0; i--)
+    {
+        NdisMoveMemory(pCtx->Input, (UCHAR *)pData, 64);
+        byteReverse((UCHAR *)pCtx->Input, 16);
+
+        NdisZeroMemory((UCHAR *)pCtx->Input + 64, 16);
+        SHATransform(pCtx->Buf, (UINT32 *)pCtx->Input);
+        pData += 64;
+        LenInBytes -= 64;
+    } // end of for
+
+    // buffering lacks of 64-byte data
+    if(LenInBytes)
+        NdisMoveMemory(pCtx->Input, (UCHAR *)pData, LenInBytes);
+
+	return (0);
+
+}
+
+// Append padding bits and length of original message in the tail
+// The message digest has to be completed in the end
+VOID SHAFinal(SHA_CTX *pCtx, UCHAR Digest[20])
+{
+    UCHAR Remainder;
+    UCHAR PadLenInBytes;
+    UCHAR *pAppend=0;
+    unsigned int i;
+
+    Remainder = (UCHAR)((pCtx->LenInBitCount[0] >> 3) & 0x3f);
+
+    pAppend = (UCHAR *)pCtx->Input + Remainder;
+
+    PadLenInBytes = (Remainder < 56) ? (56-Remainder) : (120-Remainder);
+
+    // padding bits without crossing block(64-byte based) boundary
+    if (Remainder < 56)
+    {
+        *pAppend = 0x80;
+        PadLenInBytes --;
+
+        NdisZeroMemory((UCHAR *)pCtx->Input + Remainder+1, PadLenInBytes);
+
+		// add data-length field, from high to low
+        for (i=0; i<4; i++)
+        {
+        	pCtx->Input[56+i] = (UCHAR)((pCtx->LenInBitCount[1] >> ((3-i) << 3)) & 0xff);
+        	pCtx->Input[60+i] = (UCHAR)((pCtx->LenInBitCount[0] >> ((3-i) << 3)) & 0xff);
+      	}
+
+        byteReverse((UCHAR *)pCtx->Input, 16);
+        NdisZeroMemory((UCHAR *)pCtx->Input + 64, 14);
+        SHATransform(pCtx->Buf, (UINT32 *)pCtx->Input);
+    } // end of if
+
+    // padding bits with crossing block(64-byte based) boundary
+    else
+    {
+        // the first block ===
+        *pAppend = 0x80;
+        PadLenInBytes --;
+
+        NdisZeroMemory((UCHAR *)pCtx->Input + Remainder+1, (64-Remainder-1));
+        PadLenInBytes -= (64 - Remainder - 1);
+
+        byteReverse((UCHAR *)pCtx->Input, 16);
+        NdisZeroMemory((UCHAR *)pCtx->Input + 64, 16);
+        SHATransform(pCtx->Buf, (UINT32 *)pCtx->Input);
+
+
+        // the second block ===
+        NdisZeroMemory((UCHAR *)pCtx->Input, PadLenInBytes);
+
+		// add data-length field
+		for (i=0; i<4; i++)
+        {
+        	pCtx->Input[56+i] = (UCHAR)((pCtx->LenInBitCount[1] >> ((3-i) << 3)) & 0xff);
+        	pCtx->Input[60+i] = (UCHAR)((pCtx->LenInBitCount[0] >> ((3-i) << 3)) & 0xff);
+      	}
+
+        byteReverse((UCHAR *)pCtx->Input, 16);
+        NdisZeroMemory((UCHAR *)pCtx->Input + 64, 16);
+        SHATransform(pCtx->Buf, (UINT32 *)pCtx->Input);
+    } // end of else
+
+
+    //Output, bytereverse
+    for (i=0; i<20; i++)
+    {
+        Digest [i] = (UCHAR)(pCtx->Buf[i>>2] >> 8*(3-(i & 0x3)));
+    }
+
+    NdisZeroMemory(pCtx, sizeof(pCtx)); // memory free
+}
+
+
+// The central algorithm of SHA-1, consists of four rounds and
+// twenty steps per round
+VOID SHATransform(UINT32 Buf[5], UINT32 Mes[20])
+{
+    UINT32 Reg[5],Temp;
+	unsigned int i;
+    UINT32 W[80];
+
+    static UINT32 SHA1Table[4] = { 0x5a827999, 0x6ed9eba1,
+                                  0x8f1bbcdc, 0xca62c1d6 };
+
+    Reg[0]=Buf[0];
+	Reg[1]=Buf[1];
+	Reg[2]=Buf[2];
+	Reg[3]=Buf[3];
+	Reg[4]=Buf[4];
+
+    //the first octet of a word is stored in the 0th element, bytereverse
+	for(i = 0; i < 16; i++)
+    {
+    	W[i]  = (Mes[i] >> 24) & 0xff;
+        W[i] |= (Mes[i] >> 8 ) & 0xff00;
+        W[i] |= (Mes[i] << 8 ) & 0xff0000;
+        W[i] |= (Mes[i] << 24) & 0xff000000;
+    }
+
+
+    for	(i = 0; i < 64; i++)
+	    W[16+i] = CYCLIC_LEFT_SHIFT(W[i] ^ W[2+i] ^ W[8+i] ^ W[13+i], 1);
+
+
+    // 80 steps in SHA-1 algorithm
+    for (i=0; i<80; i++)
+    {
+        if (i<20)
+            SHA1Step(SHA1_F1, Reg[0], Reg[1], Reg[2], Reg[3], Reg[4],
+                     W[i], SHA1Table[0]);
+
+        else if (i>=20 && i<40)
+            SHA1Step(SHA1_F2, Reg[0], Reg[1], Reg[2], Reg[3], Reg[4],
+                     W[i], SHA1Table[1]);
+
+		else if (i>=40 && i<60)
+            SHA1Step(SHA1_F3, Reg[0], Reg[1], Reg[2], Reg[3], Reg[4],
+                      W[i], SHA1Table[2]);
+
+        else
+            SHA1Step(SHA1_F2, Reg[0], Reg[1], Reg[2], Reg[3], Reg[4],
+                     W[i], SHA1Table[3]);
+
+
+       // one-word right shift
+		Temp   = Reg[4];
+        Reg[4] = Reg[3];
+        Reg[3] = Reg[2];
+        Reg[2] = Reg[1];
+        Reg[1] = Reg[0];
+        Reg[0] = Temp;
+
+    } // end of for-loop
+
+
+    // (temporary)output
+    for (i=0; i<5; i++)
+        Buf[i] += Reg[i];
+
+}
+
+
+/* =========================  AES En/Decryption ========================== */
+
+/* forward S-box */
+static uint32 FSb[256] =
+{
+	0x63, 0x7C,	0x77, 0x7B,	0xF2, 0x6B,	0x6F, 0xC5,
+	0x30, 0x01,	0x67, 0x2B,	0xFE, 0xD7,	0xAB, 0x76,
+	0xCA, 0x82,	0xC9, 0x7D,	0xFA, 0x59,	0x47, 0xF0,
+	0xAD, 0xD4,	0xA2, 0xAF,	0x9C, 0xA4,	0x72, 0xC0,
+	0xB7, 0xFD,	0x93, 0x26,	0x36, 0x3F,	0xF7, 0xCC,
+	0x34, 0xA5,	0xE5, 0xF1,	0x71, 0xD8,	0x31, 0x15,
+	0x04, 0xC7,	0x23, 0xC3,	0x18, 0x96,	0x05, 0x9A,
+	0x07, 0x12,	0x80, 0xE2,	0xEB, 0x27,	0xB2, 0x75,
+	0x09, 0x83,	0x2C, 0x1A,	0x1B, 0x6E,	0x5A, 0xA0,
+	0x52, 0x3B,	0xD6, 0xB3,	0x29, 0xE3,	0x2F, 0x84,
+	0x53, 0xD1,	0x00, 0xED,	0x20, 0xFC,	0xB1, 0x5B,
+	0x6A, 0xCB,	0xBE, 0x39,	0x4A, 0x4C,	0x58, 0xCF,
+	0xD0, 0xEF,	0xAA, 0xFB,	0x43, 0x4D,	0x33, 0x85,
+	0x45, 0xF9,	0x02, 0x7F,	0x50, 0x3C,	0x9F, 0xA8,
+	0x51, 0xA3,	0x40, 0x8F,	0x92, 0x9D,	0x38, 0xF5,
+	0xBC, 0xB6,	0xDA, 0x21,	0x10, 0xFF,	0xF3, 0xD2,
+	0xCD, 0x0C,	0x13, 0xEC,	0x5F, 0x97,	0x44, 0x17,
+	0xC4, 0xA7,	0x7E, 0x3D,	0x64, 0x5D,	0x19, 0x73,
+	0x60, 0x81,	0x4F, 0xDC,	0x22, 0x2A,	0x90, 0x88,
+	0x46, 0xEE,	0xB8, 0x14,	0xDE, 0x5E,	0x0B, 0xDB,
+	0xE0, 0x32,	0x3A, 0x0A,	0x49, 0x06,	0x24, 0x5C,
+	0xC2, 0xD3,	0xAC, 0x62,	0x91, 0x95,	0xE4, 0x79,
+	0xE7, 0xC8,	0x37, 0x6D,	0x8D, 0xD5,	0x4E, 0xA9,
+	0x6C, 0x56,	0xF4, 0xEA,	0x65, 0x7A,	0xAE, 0x08,
+	0xBA, 0x78,	0x25, 0x2E,	0x1C, 0xA6,	0xB4, 0xC6,
+	0xE8, 0xDD,	0x74, 0x1F,	0x4B, 0xBD,	0x8B, 0x8A,
+	0x70, 0x3E,	0xB5, 0x66,	0x48, 0x03,	0xF6, 0x0E,
+	0x61, 0x35,	0x57, 0xB9,	0x86, 0xC1,	0x1D, 0x9E,
+	0xE1, 0xF8,	0x98, 0x11,	0x69, 0xD9,	0x8E, 0x94,
+	0x9B, 0x1E,	0x87, 0xE9,	0xCE, 0x55,	0x28, 0xDF,
+	0x8C, 0xA1,	0x89, 0x0D,	0xBF, 0xE6,	0x42, 0x68,
+	0x41, 0x99,	0x2D, 0x0F,	0xB0, 0x54,	0xBB, 0x16
+};
+
+/* forward table */
+#define	FT \
+\
+	V(C6,63,63,A5),	V(F8,7C,7C,84),	V(EE,77,77,99),	V(F6,7B,7B,8D),	\
+	V(FF,F2,F2,0D),	V(D6,6B,6B,BD),	V(DE,6F,6F,B1),	V(91,C5,C5,54),	\
+	V(60,30,30,50),	V(02,01,01,03),	V(CE,67,67,A9),	V(56,2B,2B,7D),	\
+	V(E7,FE,FE,19),	V(B5,D7,D7,62),	V(4D,AB,AB,E6),	V(EC,76,76,9A),	\
+	V(8F,CA,CA,45),	V(1F,82,82,9D),	V(89,C9,C9,40),	V(FA,7D,7D,87),	\
+	V(EF,FA,FA,15),	V(B2,59,59,EB),	V(8E,47,47,C9),	V(FB,F0,F0,0B),	\
+	V(41,AD,AD,EC),	V(B3,D4,D4,67),	V(5F,A2,A2,FD),	V(45,AF,AF,EA),	\
+	V(23,9C,9C,BF),	V(53,A4,A4,F7),	V(E4,72,72,96),	V(9B,C0,C0,5B),	\
+	V(75,B7,B7,C2),	V(E1,FD,FD,1C),	V(3D,93,93,AE),	V(4C,26,26,6A),	\
+	V(6C,36,36,5A),	V(7E,3F,3F,41),	V(F5,F7,F7,02),	V(83,CC,CC,4F),	\
+	V(68,34,34,5C),	V(51,A5,A5,F4),	V(D1,E5,E5,34),	V(F9,F1,F1,08),	\
+	V(E2,71,71,93),	V(AB,D8,D8,73),	V(62,31,31,53),	V(2A,15,15,3F),	\
+	V(08,04,04,0C),	V(95,C7,C7,52),	V(46,23,23,65),	V(9D,C3,C3,5E),	\
+	V(30,18,18,28),	V(37,96,96,A1),	V(0A,05,05,0F),	V(2F,9A,9A,B5),	\
+	V(0E,07,07,09),	V(24,12,12,36),	V(1B,80,80,9B),	V(DF,E2,E2,3D),	\
+	V(CD,EB,EB,26),	V(4E,27,27,69),	V(7F,B2,B2,CD),	V(EA,75,75,9F),	\
+	V(12,09,09,1B),	V(1D,83,83,9E),	V(58,2C,2C,74),	V(34,1A,1A,2E),	\
+	V(36,1B,1B,2D),	V(DC,6E,6E,B2),	V(B4,5A,5A,EE),	V(5B,A0,A0,FB),	\
+	V(A4,52,52,F6),	V(76,3B,3B,4D),	V(B7,D6,D6,61),	V(7D,B3,B3,CE),	\
+	V(52,29,29,7B),	V(DD,E3,E3,3E),	V(5E,2F,2F,71),	V(13,84,84,97),	\
+	V(A6,53,53,F5),	V(B9,D1,D1,68),	V(00,00,00,00),	V(C1,ED,ED,2C),	\
+	V(40,20,20,60),	V(E3,FC,FC,1F),	V(79,B1,B1,C8),	V(B6,5B,5B,ED),	\
+	V(D4,6A,6A,BE),	V(8D,CB,CB,46),	V(67,BE,BE,D9),	V(72,39,39,4B),	\
+	V(94,4A,4A,DE),	V(98,4C,4C,D4),	V(B0,58,58,E8),	V(85,CF,CF,4A),	\
+	V(BB,D0,D0,6B),	V(C5,EF,EF,2A),	V(4F,AA,AA,E5),	V(ED,FB,FB,16),	\
+	V(86,43,43,C5),	V(9A,4D,4D,D7),	V(66,33,33,55),	V(11,85,85,94),	\
+	V(8A,45,45,CF),	V(E9,F9,F9,10),	V(04,02,02,06),	V(FE,7F,7F,81),	\
+	V(A0,50,50,F0),	V(78,3C,3C,44),	V(25,9F,9F,BA),	V(4B,A8,A8,E3),	\
+	V(A2,51,51,F3),	V(5D,A3,A3,FE),	V(80,40,40,C0),	V(05,8F,8F,8A),	\
+	V(3F,92,92,AD),	V(21,9D,9D,BC),	V(70,38,38,48),	V(F1,F5,F5,04),	\
+	V(63,BC,BC,DF),	V(77,B6,B6,C1),	V(AF,DA,DA,75),	V(42,21,21,63),	\
+	V(20,10,10,30),	V(E5,FF,FF,1A),	V(FD,F3,F3,0E),	V(BF,D2,D2,6D),	\
+	V(81,CD,CD,4C),	V(18,0C,0C,14),	V(26,13,13,35),	V(C3,EC,EC,2F),	\
+	V(BE,5F,5F,E1),	V(35,97,97,A2),	V(88,44,44,CC),	V(2E,17,17,39),	\
+	V(93,C4,C4,57),	V(55,A7,A7,F2),	V(FC,7E,7E,82),	V(7A,3D,3D,47),	\
+	V(C8,64,64,AC),	V(BA,5D,5D,E7),	V(32,19,19,2B),	V(E6,73,73,95),	\
+	V(C0,60,60,A0),	V(19,81,81,98),	V(9E,4F,4F,D1),	V(A3,DC,DC,7F),	\
+	V(44,22,22,66),	V(54,2A,2A,7E),	V(3B,90,90,AB),	V(0B,88,88,83),	\
+	V(8C,46,46,CA),	V(C7,EE,EE,29),	V(6B,B8,B8,D3),	V(28,14,14,3C),	\
+	V(A7,DE,DE,79),	V(BC,5E,5E,E2),	V(16,0B,0B,1D),	V(AD,DB,DB,76),	\
+	V(DB,E0,E0,3B),	V(64,32,32,56),	V(74,3A,3A,4E),	V(14,0A,0A,1E),	\
+	V(92,49,49,DB),	V(0C,06,06,0A),	V(48,24,24,6C),	V(B8,5C,5C,E4),	\
+	V(9F,C2,C2,5D),	V(BD,D3,D3,6E),	V(43,AC,AC,EF),	V(C4,62,62,A6),	\
+	V(39,91,91,A8),	V(31,95,95,A4),	V(D3,E4,E4,37),	V(F2,79,79,8B),	\
+	V(D5,E7,E7,32),	V(8B,C8,C8,43),	V(6E,37,37,59),	V(DA,6D,6D,B7),	\
+	V(01,8D,8D,8C),	V(B1,D5,D5,64),	V(9C,4E,4E,D2),	V(49,A9,A9,E0),	\
+	V(D8,6C,6C,B4),	V(AC,56,56,FA),	V(F3,F4,F4,07),	V(CF,EA,EA,25),	\
+	V(CA,65,65,AF),	V(F4,7A,7A,8E),	V(47,AE,AE,E9),	V(10,08,08,18),	\
+	V(6F,BA,BA,D5),	V(F0,78,78,88),	V(4A,25,25,6F),	V(5C,2E,2E,72),	\
+	V(38,1C,1C,24),	V(57,A6,A6,F1),	V(73,B4,B4,C7),	V(97,C6,C6,51),	\
+	V(CB,E8,E8,23),	V(A1,DD,DD,7C),	V(E8,74,74,9C),	V(3E,1F,1F,21),	\
+	V(96,4B,4B,DD),	V(61,BD,BD,DC),	V(0D,8B,8B,86),	V(0F,8A,8A,85),	\
+	V(E0,70,70,90),	V(7C,3E,3E,42),	V(71,B5,B5,C4),	V(CC,66,66,AA),	\
+	V(90,48,48,D8),	V(06,03,03,05),	V(F7,F6,F6,01),	V(1C,0E,0E,12),	\
+	V(C2,61,61,A3),	V(6A,35,35,5F),	V(AE,57,57,F9),	V(69,B9,B9,D0),	\
+	V(17,86,86,91),	V(99,C1,C1,58),	V(3A,1D,1D,27),	V(27,9E,9E,B9),	\
+	V(D9,E1,E1,38),	V(EB,F8,F8,13),	V(2B,98,98,B3),	V(22,11,11,33),	\
+	V(D2,69,69,BB),	V(A9,D9,D9,70),	V(07,8E,8E,89),	V(33,94,94,A7),	\
+	V(2D,9B,9B,B6),	V(3C,1E,1E,22),	V(15,87,87,92),	V(C9,E9,E9,20),	\
+	V(87,CE,CE,49),	V(AA,55,55,FF),	V(50,28,28,78),	V(A5,DF,DF,7A),	\
+	V(03,8C,8C,8F),	V(59,A1,A1,F8),	V(09,89,89,80),	V(1A,0D,0D,17),	\
+	V(65,BF,BF,DA),	V(D7,E6,E6,31),	V(84,42,42,C6),	V(D0,68,68,B8),	\
+	V(82,41,41,C3),	V(29,99,99,B0),	V(5A,2D,2D,77),	V(1E,0F,0F,11),	\
+	V(7B,B0,B0,CB),	V(A8,54,54,FC),	V(6D,BB,BB,D6),	V(2C,16,16,3A)
+
+#define	V(a,b,c,d) 0x##a##b##c##d
+static uint32 FT0[256] = { FT };
+#undef V
+
+#define	V(a,b,c,d) 0x##d##a##b##c
+static uint32 FT1[256] = { FT };
+#undef V
+
+#define	V(a,b,c,d) 0x##c##d##a##b
+static uint32 FT2[256] = { FT };
+#undef V
+
+#define	V(a,b,c,d) 0x##b##c##d##a
+static uint32 FT3[256] = { FT };
+#undef V
+
+#undef FT
+
+/* reverse S-box */
+
+static uint32 RSb[256] =
+{
+	0x52, 0x09,	0x6A, 0xD5,	0x30, 0x36,	0xA5, 0x38,
+	0xBF, 0x40,	0xA3, 0x9E,	0x81, 0xF3,	0xD7, 0xFB,
+	0x7C, 0xE3,	0x39, 0x82,	0x9B, 0x2F,	0xFF, 0x87,
+	0x34, 0x8E,	0x43, 0x44,	0xC4, 0xDE,	0xE9, 0xCB,
+	0x54, 0x7B,	0x94, 0x32,	0xA6, 0xC2,	0x23, 0x3D,
+	0xEE, 0x4C,	0x95, 0x0B,	0x42, 0xFA,	0xC3, 0x4E,
+	0x08, 0x2E,	0xA1, 0x66,	0x28, 0xD9,	0x24, 0xB2,
+	0x76, 0x5B,	0xA2, 0x49,	0x6D, 0x8B,	0xD1, 0x25,
+	0x72, 0xF8,	0xF6, 0x64,	0x86, 0x68,	0x98, 0x16,
+	0xD4, 0xA4,	0x5C, 0xCC,	0x5D, 0x65,	0xB6, 0x92,
+	0x6C, 0x70,	0x48, 0x50,	0xFD, 0xED,	0xB9, 0xDA,
+	0x5E, 0x15,	0x46, 0x57,	0xA7, 0x8D,	0x9D, 0x84,
+	0x90, 0xD8,	0xAB, 0x00,	0x8C, 0xBC,	0xD3, 0x0A,
+	0xF7, 0xE4,	0x58, 0x05,	0xB8, 0xB3,	0x45, 0x06,
+	0xD0, 0x2C,	0x1E, 0x8F,	0xCA, 0x3F,	0x0F, 0x02,
+	0xC1, 0xAF,	0xBD, 0x03,	0x01, 0x13,	0x8A, 0x6B,
+	0x3A, 0x91,	0x11, 0x41,	0x4F, 0x67,	0xDC, 0xEA,
+	0x97, 0xF2,	0xCF, 0xCE,	0xF0, 0xB4,	0xE6, 0x73,
+	0x96, 0xAC,	0x74, 0x22,	0xE7, 0xAD,	0x35, 0x85,
+	0xE2, 0xF9,	0x37, 0xE8,	0x1C, 0x75,	0xDF, 0x6E,
+	0x47, 0xF1,	0x1A, 0x71,	0x1D, 0x29,	0xC5, 0x89,
+	0x6F, 0xB7,	0x62, 0x0E,	0xAA, 0x18,	0xBE, 0x1B,
+	0xFC, 0x56,	0x3E, 0x4B,	0xC6, 0xD2,	0x79, 0x20,
+	0x9A, 0xDB,	0xC0, 0xFE,	0x78, 0xCD,	0x5A, 0xF4,
+	0x1F, 0xDD,	0xA8, 0x33,	0x88, 0x07,	0xC7, 0x31,
+	0xB1, 0x12,	0x10, 0x59,	0x27, 0x80,	0xEC, 0x5F,
+	0x60, 0x51,	0x7F, 0xA9,	0x19, 0xB5,	0x4A, 0x0D,
+	0x2D, 0xE5,	0x7A, 0x9F,	0x93, 0xC9,	0x9C, 0xEF,
+	0xA0, 0xE0,	0x3B, 0x4D,	0xAE, 0x2A,	0xF5, 0xB0,
+	0xC8, 0xEB,	0xBB, 0x3C,	0x83, 0x53,	0x99, 0x61,
+	0x17, 0x2B,	0x04, 0x7E,	0xBA, 0x77,	0xD6, 0x26,
+	0xE1, 0x69,	0x14, 0x63,	0x55, 0x21,	0x0C, 0x7D
+};
+
+/* reverse table */
+
+#define	RT \
+\
+	V(51,F4,A7,50),	V(7E,41,65,53),	V(1A,17,A4,C3),	V(3A,27,5E,96),	\
+	V(3B,AB,6B,CB),	V(1F,9D,45,F1),	V(AC,FA,58,AB),	V(4B,E3,03,93),	\
+	V(20,30,FA,55),	V(AD,76,6D,F6),	V(88,CC,76,91),	V(F5,02,4C,25),	\
+	V(4F,E5,D7,FC),	V(C5,2A,CB,D7),	V(26,35,44,80),	V(B5,62,A3,8F),	\
+	V(DE,B1,5A,49),	V(25,BA,1B,67),	V(45,EA,0E,98),	V(5D,FE,C0,E1),	\
+	V(C3,2F,75,02),	V(81,4C,F0,12),	V(8D,46,97,A3),	V(6B,D3,F9,C6),	\
+	V(03,8F,5F,E7),	V(15,92,9C,95),	V(BF,6D,7A,EB),	V(95,52,59,DA),	\
+	V(D4,BE,83,2D),	V(58,74,21,D3),	V(49,E0,69,29),	V(8E,C9,C8,44),	\
+	V(75,C2,89,6A),	V(F4,8E,79,78),	V(99,58,3E,6B),	V(27,B9,71,DD),	\
+	V(BE,E1,4F,B6),	V(F0,88,AD,17),	V(C9,20,AC,66),	V(7D,CE,3A,B4),	\
+	V(63,DF,4A,18),	V(E5,1A,31,82),	V(97,51,33,60),	V(62,53,7F,45),	\
+	V(B1,64,77,E0),	V(BB,6B,AE,84),	V(FE,81,A0,1C),	V(F9,08,2B,94),	\
+	V(70,48,68,58),	V(8F,45,FD,19),	V(94,DE,6C,87),	V(52,7B,F8,B7),	\
+	V(AB,73,D3,23),	V(72,4B,02,E2),	V(E3,1F,8F,57),	V(66,55,AB,2A),	\
+	V(B2,EB,28,07),	V(2F,B5,C2,03),	V(86,C5,7B,9A),	V(D3,37,08,A5),	\
+	V(30,28,87,F2),	V(23,BF,A5,B2),	V(02,03,6A,BA),	V(ED,16,82,5C),	\
+	V(8A,CF,1C,2B),	V(A7,79,B4,92),	V(F3,07,F2,F0),	V(4E,69,E2,A1),	\
+	V(65,DA,F4,CD),	V(06,05,BE,D5),	V(D1,34,62,1F),	V(C4,A6,FE,8A),	\
+	V(34,2E,53,9D),	V(A2,F3,55,A0),	V(05,8A,E1,32),	V(A4,F6,EB,75),	\
+	V(0B,83,EC,39),	V(40,60,EF,AA),	V(5E,71,9F,06),	V(BD,6E,10,51),	\
+	V(3E,21,8A,F9),	V(96,DD,06,3D),	V(DD,3E,05,AE),	V(4D,E6,BD,46),	\
+	V(91,54,8D,B5),	V(71,C4,5D,05),	V(04,06,D4,6F),	V(60,50,15,FF),	\
+	V(19,98,FB,24),	V(D6,BD,E9,97),	V(89,40,43,CC),	V(67,D9,9E,77),	\
+	V(B0,E8,42,BD),	V(07,89,8B,88),	V(E7,19,5B,38),	V(79,C8,EE,DB),	\
+	V(A1,7C,0A,47),	V(7C,42,0F,E9),	V(F8,84,1E,C9),	V(00,00,00,00),	\
+	V(09,80,86,83),	V(32,2B,ED,48),	V(1E,11,70,AC),	V(6C,5A,72,4E),	\
+	V(FD,0E,FF,FB),	V(0F,85,38,56),	V(3D,AE,D5,1E),	V(36,2D,39,27),	\
+	V(0A,0F,D9,64),	V(68,5C,A6,21),	V(9B,5B,54,D1),	V(24,36,2E,3A),	\
+	V(0C,0A,67,B1),	V(93,57,E7,0F),	V(B4,EE,96,D2),	V(1B,9B,91,9E),	\
+	V(80,C0,C5,4F),	V(61,DC,20,A2),	V(5A,77,4B,69),	V(1C,12,1A,16),	\
+	V(E2,93,BA,0A),	V(C0,A0,2A,E5),	V(3C,22,E0,43),	V(12,1B,17,1D),	\
+	V(0E,09,0D,0B),	V(F2,8B,C7,AD),	V(2D,B6,A8,B9),	V(14,1E,A9,C8),	\
+	V(57,F1,19,85),	V(AF,75,07,4C),	V(EE,99,DD,BB),	V(A3,7F,60,FD),	\
+	V(F7,01,26,9F),	V(5C,72,F5,BC),	V(44,66,3B,C5),	V(5B,FB,7E,34),	\
+	V(8B,43,29,76),	V(CB,23,C6,DC),	V(B6,ED,FC,68),	V(B8,E4,F1,63),	\
+	V(D7,31,DC,CA),	V(42,63,85,10),	V(13,97,22,40),	V(84,C6,11,20),	\
+	V(85,4A,24,7D),	V(D2,BB,3D,F8),	V(AE,F9,32,11),	V(C7,29,A1,6D),	\
+	V(1D,9E,2F,4B),	V(DC,B2,30,F3),	V(0D,86,52,EC),	V(77,C1,E3,D0),	\
+	V(2B,B3,16,6C),	V(A9,70,B9,99),	V(11,94,48,FA),	V(47,E9,64,22),	\
+	V(A8,FC,8C,C4),	V(A0,F0,3F,1A),	V(56,7D,2C,D8),	V(22,33,90,EF),	\
+	V(87,49,4E,C7),	V(D9,38,D1,C1),	V(8C,CA,A2,FE),	V(98,D4,0B,36),	\
+	V(A6,F5,81,CF),	V(A5,7A,DE,28),	V(DA,B7,8E,26),	V(3F,AD,BF,A4),	\
+	V(2C,3A,9D,E4),	V(50,78,92,0D),	V(6A,5F,CC,9B),	V(54,7E,46,62),	\
+	V(F6,8D,13,C2),	V(90,D8,B8,E8),	V(2E,39,F7,5E),	V(82,C3,AF,F5),	\
+	V(9F,5D,80,BE),	V(69,D0,93,7C),	V(6F,D5,2D,A9),	V(CF,25,12,B3),	\
+	V(C8,AC,99,3B),	V(10,18,7D,A7),	V(E8,9C,63,6E),	V(DB,3B,BB,7B),	\
+	V(CD,26,78,09),	V(6E,59,18,F4),	V(EC,9A,B7,01),	V(83,4F,9A,A8),	\
+	V(E6,95,6E,65),	V(AA,FF,E6,7E),	V(21,BC,CF,08),	V(EF,15,E8,E6),	\
+	V(BA,E7,9B,D9),	V(4A,6F,36,CE),	V(EA,9F,09,D4),	V(29,B0,7C,D6),	\
+	V(31,A4,B2,AF),	V(2A,3F,23,31),	V(C6,A5,94,30),	V(35,A2,66,C0),	\
+	V(74,4E,BC,37),	V(FC,82,CA,A6),	V(E0,90,D0,B0),	V(33,A7,D8,15),	\
+	V(F1,04,98,4A),	V(41,EC,DA,F7),	V(7F,CD,50,0E),	V(17,91,F6,2F),	\
+	V(76,4D,D6,8D),	V(43,EF,B0,4D),	V(CC,AA,4D,54),	V(E4,96,04,DF),	\
+	V(9E,D1,B5,E3),	V(4C,6A,88,1B),	V(C1,2C,1F,B8),	V(46,65,51,7F),	\
+	V(9D,5E,EA,04),	V(01,8C,35,5D),	V(FA,87,74,73),	V(FB,0B,41,2E),	\
+	V(B3,67,1D,5A),	V(92,DB,D2,52),	V(E9,10,56,33),	V(6D,D6,47,13),	\
+	V(9A,D7,61,8C),	V(37,A1,0C,7A),	V(59,F8,14,8E),	V(EB,13,3C,89),	\
+	V(CE,A9,27,EE),	V(B7,61,C9,35),	V(E1,1C,E5,ED),	V(7A,47,B1,3C),	\
+	V(9C,D2,DF,59),	V(55,F2,73,3F),	V(18,14,CE,79),	V(73,C7,37,BF),	\
+	V(53,F7,CD,EA),	V(5F,FD,AA,5B),	V(DF,3D,6F,14),	V(78,44,DB,86),	\
+	V(CA,AF,F3,81),	V(B9,68,C4,3E),	V(38,24,34,2C),	V(C2,A3,40,5F),	\
+	V(16,1D,C3,72),	V(BC,E2,25,0C),	V(28,3C,49,8B),	V(FF,0D,95,41),	\
+	V(39,A8,01,71),	V(08,0C,B3,DE),	V(D8,B4,E4,9C),	V(64,56,C1,90),	\
+	V(7B,CB,84,61),	V(D5,32,B6,70),	V(48,6C,5C,74),	V(D0,B8,57,42)
+
+#define	V(a,b,c,d) 0x##a##b##c##d
+static uint32 RT0[256] = { RT };
+#undef V
+
+#define	V(a,b,c,d) 0x##d##a##b##c
+static uint32 RT1[256] = { RT };
+#undef V
+
+#define	V(a,b,c,d) 0x##c##d##a##b
+static uint32 RT2[256] = { RT };
+#undef V
+
+#define	V(a,b,c,d) 0x##b##c##d##a
+static uint32 RT3[256] = { RT };
+#undef V
+
+#undef RT
+
+/* round constants */
+
+static uint32 RCON[10] =
+{
+	0x01000000,	0x02000000,	0x04000000,	0x08000000,
+	0x10000000,	0x20000000,	0x40000000,	0x80000000,
+	0x1B000000,	0x36000000
+};
+
+/* key schedule	tables */
+
+static int KT_init = 1;
+
+static uint32 KT0[256];
+static uint32 KT1[256];
+static uint32 KT2[256];
+static uint32 KT3[256];
+
+/* platform-independant	32-bit integer manipulation	macros */
+
+#define	GET_UINT32(n,b,i)						\
+{												\
+	(n)	= (	(uint32) (b)[(i)	] << 24	)		\
+		| (	(uint32) (b)[(i) + 1] << 16	)		\
+		| (	(uint32) (b)[(i) + 2] <<  8	)		\
+		| (	(uint32) (b)[(i) + 3]		);		\
+}
+
+#define	PUT_UINT32(n,b,i)						\
+{												\
+	(b)[(i)	   ] = (uint8) ( (n) >>	24 );		\
+	(b)[(i)	+ 1] = (uint8) ( (n) >>	16 );		\
+	(b)[(i)	+ 2] = (uint8) ( (n) >>	 8 );		\
+	(b)[(i)	+ 3] = (uint8) ( (n)	   );		\
+}
+
+/* AES key scheduling routine */
+
+int	rtmp_aes_set_key( aes_context *ctx, uint8 *key, int nbits )
+{
+	int	i;
+	uint32 *RK,	*SK;
+
+	switch(	nbits )
+	{
+		case 128: ctx->nr =	10;	break;
+		case 192: ctx->nr =	12;	break;
+		case 256: ctx->nr =	14;	break;
+		default	: return( 1	);
+	}
+
+	RK = ctx->erk;
+
+	for( i = 0;	i <	(nbits >> 5); i++ )
+	{
+		GET_UINT32(	RK[i], key,	i *	4 );
+	}
+
+	/* setup encryption	round keys */
+
+	switch(	nbits )
+	{
+	case 128:
+
+		for( i = 0;	i <	10;	i++, RK	+= 4 )
+		{
+			RK[4]  = RK[0] ^ RCON[i] ^
+						( FSb[ (uint8) ( RK[3] >> 16 ) ] <<	24 ) ^
+						( FSb[ (uint8) ( RK[3] >>  8 ) ] <<	16 ) ^
+						( FSb[ (uint8) ( RK[3]		 ) ] <<	 8 ) ^
+						( FSb[ (uint8) ( RK[3] >> 24 ) ]	   );
+
+			RK[5]  = RK[1] ^ RK[4];
+			RK[6]  = RK[2] ^ RK[5];
+			RK[7]  = RK[3] ^ RK[6];
+		}
+		break;
+
+	case 192:
+
+		for( i = 0;	i <	8; i++,	RK += 6	)
+		{
+			RK[6]  = RK[0] ^ RCON[i] ^
+						( FSb[ (uint8) ( RK[5] >> 16 ) ] <<	24 ) ^
+						( FSb[ (uint8) ( RK[5] >>  8 ) ] <<	16 ) ^
+						( FSb[ (uint8) ( RK[5]		 ) ] <<	 8 ) ^
+						( FSb[ (uint8) ( RK[5] >> 24 ) ]	   );
+
+			RK[7]  = RK[1] ^ RK[6];
+			RK[8]  = RK[2] ^ RK[7];
+			RK[9]  = RK[3] ^ RK[8];
+			RK[10] = RK[4] ^ RK[9];
+			RK[11] = RK[5] ^ RK[10];
+		}
+		break;
+
+	case 256:
+
+		for( i = 0;	i <	7; i++,	RK += 8	)
+		{
+			RK[8]  = RK[0] ^ RCON[i] ^
+						( FSb[ (uint8) ( RK[7] >> 16 ) ] <<	24 ) ^
+						( FSb[ (uint8) ( RK[7] >>  8 ) ] <<	16 ) ^
+						( FSb[ (uint8) ( RK[7]		 ) ] <<	 8 ) ^
+						( FSb[ (uint8) ( RK[7] >> 24 ) ]	   );
+
+			RK[9]  = RK[1] ^ RK[8];
+			RK[10] = RK[2] ^ RK[9];
+			RK[11] = RK[3] ^ RK[10];
+
+			RK[12] = RK[4] ^
+						( FSb[ (uint8) ( RK[11]	>> 24 )	] << 24	) ^
+						( FSb[ (uint8) ( RK[11]	>> 16 )	] << 16	) ^
+						( FSb[ (uint8) ( RK[11]	>>	8 )	] <<  8	) ^
+						( FSb[ (uint8) ( RK[11]		  )	]		);
+
+			RK[13] = RK[5] ^ RK[12];
+			RK[14] = RK[6] ^ RK[13];
+			RK[15] = RK[7] ^ RK[14];
+		}
+		break;
+	}
+
+	/* setup decryption	round keys */
+
+	if(	KT_init	)
+	{
+		for( i = 0;	i <	256; i++ )
+		{
+			KT0[i] = RT0[ FSb[i] ];
+			KT1[i] = RT1[ FSb[i] ];
+			KT2[i] = RT2[ FSb[i] ];
+			KT3[i] = RT3[ FSb[i] ];
+		}
+
+		KT_init	= 0;
+	}
+
+	SK = ctx->drk;
+
+	*SK++ =	*RK++;
+	*SK++ =	*RK++;
+	*SK++ =	*RK++;
+	*SK++ =	*RK++;
+
+	for( i = 1;	i <	ctx->nr; i++ )
+	{
+		RK -= 8;
+
+		*SK++ =	KT0[ (uint8) ( *RK >> 24 ) ] ^
+				KT1[ (uint8) ( *RK >> 16 ) ] ^
+				KT2[ (uint8) ( *RK >>  8 ) ] ^
+				KT3[ (uint8) ( *RK		 ) ]; RK++;
+
+		*SK++ =	KT0[ (uint8) ( *RK >> 24 ) ] ^
+				KT1[ (uint8) ( *RK >> 16 ) ] ^
+				KT2[ (uint8) ( *RK >>  8 ) ] ^
+				KT3[ (uint8) ( *RK		 ) ]; RK++;
+
+		*SK++ =	KT0[ (uint8) ( *RK >> 24 ) ] ^
+				KT1[ (uint8) ( *RK >> 16 ) ] ^
+				KT2[ (uint8) ( *RK >>  8 ) ] ^
+				KT3[ (uint8) ( *RK		 ) ]; RK++;
+
+		*SK++ =	KT0[ (uint8) ( *RK >> 24 ) ] ^
+				KT1[ (uint8) ( *RK >> 16 ) ] ^
+				KT2[ (uint8) ( *RK >>  8 ) ] ^
+				KT3[ (uint8) ( *RK		 ) ]; RK++;
+	}
+
+	RK -= 8;
+
+	*SK++ =	*RK++;
+	*SK++ =	*RK++;
+	*SK++ =	*RK++;
+	*SK++ =	*RK++;
+
+	return(	0 );
+}
+
+/* AES 128-bit block encryption	routine	*/
+
+void rtmp_aes_encrypt(aes_context *ctx, uint8 input[16],	uint8 output[16] )
+{
+	uint32 *RK,	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3;
+
+	RK = ctx->erk;
+	GET_UINT32(	X0,	input,	0 ); X0	^= RK[0];
+	GET_UINT32(	X1,	input,	4 ); X1	^= RK[1];
+	GET_UINT32(	X2,	input,	8 ); X2	^= RK[2];
+	GET_UINT32(	X3,	input, 12 ); X3	^= RK[3];
+
+#define	AES_FROUND(X0,X1,X2,X3,Y0,Y1,Y2,Y3)		\
+{												\
+	RK += 4;									\
+												\
+	X0 = RK[0] ^ FT0[ (uint8) (	Y0 >> 24 ) ] ^	\
+				 FT1[ (uint8) (	Y1 >> 16 ) ] ^	\
+				 FT2[ (uint8) (	Y2 >>  8 ) ] ^	\
+				 FT3[ (uint8) (	Y3		 ) ];	\
+												\
+	X1 = RK[1] ^ FT0[ (uint8) (	Y1 >> 24 ) ] ^	\
+				 FT1[ (uint8) (	Y2 >> 16 ) ] ^	\
+				 FT2[ (uint8) (	Y3 >>  8 ) ] ^	\
+				 FT3[ (uint8) (	Y0		 ) ];	\
+												\
+	X2 = RK[2] ^ FT0[ (uint8) (	Y2 >> 24 ) ] ^	\
+				 FT1[ (uint8) (	Y3 >> 16 ) ] ^	\
+				 FT2[ (uint8) (	Y0 >>  8 ) ] ^	\
+				 FT3[ (uint8) (	Y1		 ) ];	\
+												\
+	X3 = RK[3] ^ FT0[ (uint8) (	Y3 >> 24 ) ] ^	\
+				 FT1[ (uint8) (	Y0 >> 16 ) ] ^	\
+				 FT2[ (uint8) (	Y1 >>  8 ) ] ^	\
+				 FT3[ (uint8) (	Y2		 ) ];	\
+}
+
+	AES_FROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 1 */
+	AES_FROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 2 */
+	AES_FROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 3 */
+	AES_FROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 4 */
+	AES_FROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 5 */
+	AES_FROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 6 */
+	AES_FROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 7 */
+	AES_FROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 8 */
+	AES_FROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 9 */
+
+	if(	ctx->nr	> 10 )
+	{
+		AES_FROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );	/* round 10	*/
+		AES_FROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );	/* round 11	*/
+	}
+
+	if(	ctx->nr	> 12 )
+	{
+		AES_FROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );	/* round 12	*/
+		AES_FROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );	/* round 13	*/
+	}
+
+	/* last	round */
+
+	RK += 4;
+
+	X0 = RK[0] ^ ( FSb[	(uint8)	( Y0 >>	24 ) ] << 24 ) ^
+				 ( FSb[	(uint8)	( Y1 >>	16 ) ] << 16 ) ^
+				 ( FSb[	(uint8)	( Y2 >>	 8 ) ] <<  8 ) ^
+				 ( FSb[	(uint8)	( Y3	   ) ]		 );
+
+	X1 = RK[1] ^ ( FSb[	(uint8)	( Y1 >>	24 ) ] << 24 ) ^
+				 ( FSb[	(uint8)	( Y2 >>	16 ) ] << 16 ) ^
+				 ( FSb[	(uint8)	( Y3 >>	 8 ) ] <<  8 ) ^
+				 ( FSb[	(uint8)	( Y0	   ) ]		 );
+
+	X2 = RK[2] ^ ( FSb[	(uint8)	( Y2 >>	24 ) ] << 24 ) ^
+				 ( FSb[	(uint8)	( Y3 >>	16 ) ] << 16 ) ^
+				 ( FSb[	(uint8)	( Y0 >>	 8 ) ] <<  8 ) ^
+				 ( FSb[	(uint8)	( Y1	   ) ]		 );
+
+	X3 = RK[3] ^ ( FSb[	(uint8)	( Y3 >>	24 ) ] << 24 ) ^
+				 ( FSb[	(uint8)	( Y0 >>	16 ) ] << 16 ) ^
+				 ( FSb[	(uint8)	( Y1 >>	 8 ) ] <<  8 ) ^
+				 ( FSb[	(uint8)	( Y2	   ) ]		 );
+
+	PUT_UINT32(	X0,	output,	 0 );
+	PUT_UINT32(	X1,	output,	 4 );
+	PUT_UINT32(	X2,	output,	 8 );
+	PUT_UINT32(	X3,	output,	12 );
+}
+
+/* AES 128-bit block decryption	routine	*/
+
+void rtmp_aes_decrypt( aes_context *ctx,	uint8 input[16], uint8 output[16] )
+{
+	uint32 *RK,	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3;
+
+	RK = ctx->drk;
+
+	GET_UINT32(	X0,	input,	0 ); X0	^= RK[0];
+	GET_UINT32(	X1,	input,	4 ); X1	^= RK[1];
+	GET_UINT32(	X2,	input,	8 ); X2	^= RK[2];
+	GET_UINT32(	X3,	input, 12 ); X3	^= RK[3];
+
+#define	AES_RROUND(X0,X1,X2,X3,Y0,Y1,Y2,Y3)		\
+{												\
+	RK += 4;									\
+												\
+	X0 = RK[0] ^ RT0[ (uint8) (	Y0 >> 24 ) ] ^	\
+				 RT1[ (uint8) (	Y3 >> 16 ) ] ^	\
+				 RT2[ (uint8) (	Y2 >>  8 ) ] ^	\
+				 RT3[ (uint8) (	Y1		 ) ];	\
+												\
+	X1 = RK[1] ^ RT0[ (uint8) (	Y1 >> 24 ) ] ^	\
+				 RT1[ (uint8) (	Y0 >> 16 ) ] ^	\
+				 RT2[ (uint8) (	Y3 >>  8 ) ] ^	\
+				 RT3[ (uint8) (	Y2		 ) ];	\
+												\
+	X2 = RK[2] ^ RT0[ (uint8) (	Y2 >> 24 ) ] ^	\
+				 RT1[ (uint8) (	Y1 >> 16 ) ] ^	\
+				 RT2[ (uint8) (	Y0 >>  8 ) ] ^	\
+				 RT3[ (uint8) (	Y3		 ) ];	\
+												\
+	X3 = RK[3] ^ RT0[ (uint8) (	Y3 >> 24 ) ] ^	\
+				 RT1[ (uint8) (	Y2 >> 16 ) ] ^	\
+				 RT2[ (uint8) (	Y1 >>  8 ) ] ^	\
+				 RT3[ (uint8) (	Y0		 ) ];	\
+}
+
+	AES_RROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 1 */
+	AES_RROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 2 */
+	AES_RROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 3 */
+	AES_RROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 4 */
+	AES_RROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 5 */
+	AES_RROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 6 */
+	AES_RROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 7 */
+	AES_RROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 8 */
+	AES_RROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 9 */
+
+	if(	ctx->nr	> 10 )
+	{
+		AES_RROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );	/* round 10	*/
+		AES_RROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );	/* round 11	*/
+	}
+
+	if(	ctx->nr	> 12 )
+	{
+		AES_RROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );	/* round 12	*/
+		AES_RROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );	/* round 13	*/
+	}
+
+	/* last	round */
+
+	RK += 4;
+
+	X0 = RK[0] ^ ( RSb[	(uint8)	( Y0 >>	24 ) ] << 24 ) ^
+				 ( RSb[	(uint8)	( Y3 >>	16 ) ] << 16 ) ^
+				 ( RSb[	(uint8)	( Y2 >>	 8 ) ] <<  8 ) ^
+				 ( RSb[	(uint8)	( Y1	   ) ]		 );
+
+	X1 = RK[1] ^ ( RSb[	(uint8)	( Y1 >>	24 ) ] << 24 ) ^
+				 ( RSb[	(uint8)	( Y0 >>	16 ) ] << 16 ) ^
+				 ( RSb[	(uint8)	( Y3 >>	 8 ) ] <<  8 ) ^
+				 ( RSb[	(uint8)	( Y2	   ) ]		 );
+
+	X2 = RK[2] ^ ( RSb[	(uint8)	( Y2 >>	24 ) ] << 24 ) ^
+				 ( RSb[	(uint8)	( Y1 >>	16 ) ] << 16 ) ^
+				 ( RSb[	(uint8)	( Y0 >>	 8 ) ] <<  8 ) ^
+				 ( RSb[	(uint8)	( Y3	   ) ]		 );
+
+	X3 = RK[3] ^ ( RSb[	(uint8)	( Y3 >>	24 ) ] << 24 ) ^
+				 ( RSb[	(uint8)	( Y2 >>	16 ) ] << 16 ) ^
+				 ( RSb[	(uint8)	( Y1 >>	 8 ) ] <<  8 ) ^
+				 ( RSb[	(uint8)	( Y0	   ) ]		 );
+
+	PUT_UINT32(	X0,	output,	 0 );
+	PUT_UINT32(	X1,	output,	 4 );
+	PUT_UINT32(	X2,	output,	 8 );
+	PUT_UINT32(	X3,	output,	12 );
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		SHA1 function
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID	HMAC_SHA1(
+	IN	UCHAR	*text,
+	IN	UINT	text_len,
+	IN	UCHAR	*key,
+	IN	UINT	key_len,
+	IN	UCHAR	*digest)
+{
+	SHA_CTX	context;
+	UCHAR	k_ipad[65]; /* inner padding - key XORd with ipad	*/
+	UCHAR	k_opad[65]; /* outer padding - key XORd with opad	*/
+	INT		i;
+
+	// if key is longer	than 64	bytes reset	it to key=SHA1(key)
+	if (key_len	> 64)
+	{
+		SHA_CTX		 tctx;
+		SHAInit(&tctx);
+		SHAUpdate(&tctx, key, key_len);
+		SHAFinal(&tctx,	key);
+		key_len	= 20;
+	}
+	NdisZeroMemory(k_ipad, sizeof(k_ipad));
+	NdisZeroMemory(k_opad, sizeof(k_opad));
+	NdisMoveMemory(k_ipad, key,	key_len);
+	NdisMoveMemory(k_opad, key,	key_len);
+
+	// XOR key with	ipad and opad values
+	for	(i = 0;	i <	64;	i++)
+	{
+		k_ipad[i] ^= 0x36;
+		k_opad[i] ^= 0x5c;
+	}
+
+	// perform inner SHA1
+	SHAInit(&context); 						/* init context for 1st pass */
+	SHAUpdate(&context,	k_ipad,	64);		/*	start with inner pad */
+	SHAUpdate(&context,	text, text_len);	/*	then text of datagram */
+	SHAFinal(&context, digest);				/* finish up 1st pass */
+
+	//perform outer	SHA1
+	SHAInit(&context);					/* init context for 2nd pass */
+	SHAUpdate(&context,	k_opad,	64);	/*	start with outer pad */
+	SHAUpdate(&context,	digest,	20);	/*	then results of	1st	hash */
+	SHAFinal(&context, digest);			/* finish up 2nd pass */
+
+}
+
+/*
+* F(P, S, c, i) = U1 xor U2 xor ... Uc
+* U1 = PRF(P, S || Int(i))
+* U2 = PRF(P, U1)
+* Uc = PRF(P, Uc-1)
+*/
+
+void F(char *password, unsigned char *ssid, int ssidlength, int iterations, int count, unsigned char *output)
+{
+    unsigned char digest[36], digest1[SHA_DIGEST_LEN];
+    int i, j;
+
+    /* U1 = PRF(P, S || int(i)) */
+    memcpy(digest, ssid, ssidlength);
+    digest[ssidlength] = (unsigned char)((count>>24) & 0xff);
+    digest[ssidlength+1] = (unsigned char)((count>>16) & 0xff);
+    digest[ssidlength+2] = (unsigned char)((count>>8) & 0xff);
+    digest[ssidlength+3] = (unsigned char)(count & 0xff);
+    HMAC_SHA1(digest, ssidlength+4, (unsigned char*) password, (int) strlen(password), digest1); // for WPA update
+
+    /* output = U1 */
+    memcpy(output, digest1, SHA_DIGEST_LEN);
+
+    for (i = 1; i < iterations; i++)
+    {
+        /* Un = PRF(P, Un-1) */
+        HMAC_SHA1(digest1, SHA_DIGEST_LEN, (unsigned char*) password, (int) strlen(password), digest); // for WPA update
+        memcpy(digest1, digest, SHA_DIGEST_LEN);
+
+        /* output = output xor Un */
+        for (j = 0; j < SHA_DIGEST_LEN; j++)
+        {
+            output[j] ^= digest[j];
+        }
+    }
+}
+/*
+* password - ascii string up to 63 characters in length
+* ssid - octet string up to 32 octets
+* ssidlength - length of ssid in octets
+* output must be 40 octets in length and outputs 256 bits of key
+*/
+int PasswordHash(char *password, unsigned char *ssid, int ssidlength, unsigned char *output)
+{
+    if ((strlen(password) > 63) || (ssidlength > 32))
+        return 0;
+
+    F(password, ssid, ssidlength, 4096, 1, output);
+    F(password, ssid, ssidlength, 4096, 2, &output[SHA_DIGEST_LEN]);
+    return 1;
+}
+
+
diff --git a/drivers/staging/rt2860/common/mlme.c b/drivers/staging/rt2860/common/mlme.c
new file mode 100644
index 00000000000..229747095ae
--- /dev/null
+++ b/drivers/staging/rt2860/common/mlme.c
@@ -0,0 +1,8667 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	mlme.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John Chang	2004-08-25		Modify from RT2500 code base
+	John Chang	2004-09-06		modified for RT2600
+*/
+
+#include "../rt_config.h"
+#include <stdarg.h>
+
+UCHAR	CISCO_OUI[] = {0x00, 0x40, 0x96};
+
+UCHAR	WPA_OUI[] = {0x00, 0x50, 0xf2, 0x01};
+UCHAR	RSN_OUI[] = {0x00, 0x0f, 0xac};
+UCHAR	WAPI_OUI[] = {0x00, 0x14, 0x72};
+UCHAR   WME_INFO_ELEM[]  = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01};
+UCHAR   WME_PARM_ELEM[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01};
+UCHAR	Ccx2QosInfo[] = {0x00, 0x40, 0x96, 0x04};
+UCHAR   RALINK_OUI[]  = {0x00, 0x0c, 0x43};
+UCHAR   BROADCOM_OUI[]  = {0x00, 0x90, 0x4c};
+UCHAR   WPS_OUI[] = {0x00, 0x50, 0xf2, 0x04};
+#ifdef CONFIG_STA_SUPPORT
+#ifdef DOT11_N_SUPPORT
+UCHAR	PRE_N_HT_OUI[]	= {0x00, 0x90, 0x4c};
+#endif // DOT11_N_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+UCHAR RateSwitchTable[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x11, 0x00,  0,  0,  0,						// Initial used item after association
+    0x00, 0x00,  0, 40, 101,
+    0x01, 0x00,  1, 40, 50,
+    0x02, 0x00,  2, 35, 45,
+    0x03, 0x00,  3, 20, 45,
+    0x04, 0x21,  0, 30, 50,
+    0x05, 0x21,  1, 20, 50,
+    0x06, 0x21,  2, 20, 50,
+    0x07, 0x21,  3, 15, 50,
+    0x08, 0x21,  4, 15, 30,
+    0x09, 0x21,  5, 10, 25,
+    0x0a, 0x21,  6,  8, 25,
+    0x0b, 0x21,  7,  8, 25,
+    0x0c, 0x20, 12,  15, 30,
+    0x0d, 0x20, 13,  8, 20,
+    0x0e, 0x20, 14,  8, 20,
+    0x0f, 0x20, 15,  8, 25,
+    0x10, 0x22, 15,  8, 25,
+    0x11, 0x00,  0,  0,  0,
+    0x12, 0x00,  0,  0,  0,
+    0x13, 0x00,  0,  0,  0,
+    0x14, 0x00,  0,  0,  0,
+    0x15, 0x00,  0,  0,  0,
+    0x16, 0x00,  0,  0,  0,
+    0x17, 0x00,  0,  0,  0,
+    0x18, 0x00,  0,  0,  0,
+    0x19, 0x00,  0,  0,  0,
+    0x1a, 0x00,  0,  0,  0,
+    0x1b, 0x00,  0,  0,  0,
+    0x1c, 0x00,  0,  0,  0,
+    0x1d, 0x00,  0,  0,  0,
+    0x1e, 0x00,  0,  0,  0,
+    0x1f, 0x00,  0,  0,  0,
+};
+
+UCHAR RateSwitchTable11B[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x04, 0x03,  0,  0,  0,						// Initial used item after association
+    0x00, 0x00,  0, 40, 101,
+    0x01, 0x00,  1, 40, 50,
+    0x02, 0x00,  2, 35, 45,
+    0x03, 0x00,  3, 20, 45,
+};
+
+UCHAR RateSwitchTable11BG[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0a, 0x00,  0,  0,  0,						// Initial used item after association
+    0x00, 0x00,  0, 40, 101,
+    0x01, 0x00,  1, 40, 50,
+    0x02, 0x00,  2, 35, 45,
+    0x03, 0x00,  3, 20, 45,
+    0x04, 0x10,  2, 20, 35,
+    0x05, 0x10,  3, 16, 35,
+    0x06, 0x10,  4, 10, 25,
+    0x07, 0x10,  5, 16, 25,
+    0x08, 0x10,  6, 10, 25,
+    0x09, 0x10,  7, 10, 13,
+};
+
+UCHAR RateSwitchTable11G[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x08, 0x00,  0,  0,  0,						// Initial used item after association
+    0x00, 0x10,  0, 20, 101,
+    0x01, 0x10,  1, 20, 35,
+    0x02, 0x10,  2, 20, 35,
+    0x03, 0x10,  3, 16, 35,
+    0x04, 0x10,  4, 10, 25,
+    0x05, 0x10,  5, 16, 25,
+    0x06, 0x10,  6, 10, 25,
+    0x07, 0x10,  7, 10, 13,
+};
+
+#ifdef DOT11_N_SUPPORT
+UCHAR RateSwitchTable11N1S[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x09, 0x00,  0,  0,  0,						// Initial used item after association
+    0x00, 0x21,  0, 30, 101,
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x21,  5, 10, 25,
+    0x06, 0x21,  6,  8, 14,
+    0x07, 0x21,  7,  8, 14,
+    0x08, 0x23,  7,  8, 14,
+};
+
+UCHAR RateSwitchTable11N2S[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0a, 0x00,  0,  0,  0,      // Initial used item after association
+    0x00, 0x21,  0, 30, 101,
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x20, 12,  15, 30,
+    0x06, 0x20, 13,  8, 20,
+    0x07, 0x20, 14,  8, 20,
+    0x08, 0x20, 15,  8, 25,
+    0x09, 0x22, 15,  8, 25,
+};
+
+UCHAR RateSwitchTable11N3S[] = {
+// Item No.	Mode	Curr-MCS	TrainUp	TrainDown	// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0a, 0x00,  0,  0,  0,      // Initial used item after association
+    0x00, 0x21,  0, 30, 101,
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x20, 12,  15, 30,
+    0x06, 0x20, 13,  8, 20,
+    0x07, 0x20, 14,  8, 20,
+    0x08, 0x20, 15,  8, 25,
+    0x09, 0x22, 15,  8, 25,
+};
+
+UCHAR RateSwitchTable11N2SForABand[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0b, 0x09,  0,  0,  0,						// Initial used item after association
+    0x00, 0x21,  0, 30, 101,
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x21,  5, 15, 30,
+    0x06, 0x20, 12,  15, 30,
+    0x07, 0x20, 13,  8, 20,
+    0x08, 0x20, 14,  8, 20,
+    0x09, 0x20, 15,  8, 25,
+    0x0a, 0x22, 15,  8, 25,
+};
+
+UCHAR RateSwitchTable11N3SForABand[] = { // 3*3
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0b, 0x09,  0,  0,  0,						// Initial used item after association
+    0x00, 0x21,  0, 30, 101,
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x21,  5, 15, 30,
+    0x06, 0x20, 12,  15, 30,
+    0x07, 0x20, 13,  8, 20,
+    0x08, 0x20, 14,  8, 20,
+    0x09, 0x20, 15,  8, 25,
+    0x0a, 0x22, 15,  8, 25,
+};
+
+UCHAR RateSwitchTable11BGN1S[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0d, 0x00,  0,  0,  0,						// Initial used item after association
+    0x00, 0x00,  0, 40, 101,
+    0x01, 0x00,  1, 40, 50,
+    0x02, 0x00,  2, 35, 45,
+    0x03, 0x00,  3, 20, 45,
+    0x04, 0x21,  0, 30,101,	//50
+    0x05, 0x21,  1, 20, 50,
+    0x06, 0x21,  2, 20, 50,
+    0x07, 0x21,  3, 15, 50,
+    0x08, 0x21,  4, 15, 30,
+    0x09, 0x21,  5, 10, 25,
+    0x0a, 0x21,  6,  8, 14,
+    0x0b, 0x21,  7,  8, 14,
+	0x0c, 0x23,  7,  8, 14,
+};
+
+UCHAR RateSwitchTable11BGN2S[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0a, 0x00,  0,  0,  0,						// Initial used item after association
+    0x00, 0x21,  0, 30,101,	//50
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x20, 12, 15, 30,
+    0x06, 0x20, 13,  8, 20,
+    0x07, 0x20, 14,  8, 20,
+    0x08, 0x20, 15,  8, 25,
+    0x09, 0x22, 15,  8, 25,
+};
+
+UCHAR RateSwitchTable11BGN3S[] = { // 3*3
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0a, 0x00,  0,  0,  0,						// Initial used item after association
+    0x00, 0x21,  0, 30,101,	//50
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 20, 50,
+    0x04, 0x21,  4, 15, 50,
+    0x05, 0x20, 20, 15, 30,
+    0x06, 0x20, 21,  8, 20,
+    0x07, 0x20, 22,  8, 20,
+    0x08, 0x20, 23,  8, 25,
+    0x09, 0x22, 23,  8, 25,
+};
+
+UCHAR RateSwitchTable11BGN2SForABand[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0b, 0x09,  0,  0,  0,						// Initial used item after association
+    0x00, 0x21,  0, 30,101,	//50
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x21,  5, 15, 30,
+    0x06, 0x20, 12, 15, 30,
+    0x07, 0x20, 13,  8, 20,
+    0x08, 0x20, 14,  8, 20,
+    0x09, 0x20, 15,  8, 25,
+    0x0a, 0x22, 15,  8, 25,
+};
+
+UCHAR RateSwitchTable11BGN3SForABand[] = { // 3*3
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0c, 0x09,  0,  0,  0,						// Initial used item after association
+    0x00, 0x21,  0, 30,101,	//50
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x21,  5, 15, 30,
+    0x06, 0x21, 12, 15, 30,
+    0x07, 0x20, 20, 15, 30,
+    0x08, 0x20, 21,  8, 20,
+    0x09, 0x20, 22,  8, 20,
+    0x0a, 0x20, 23,  8, 25,
+    0x0b, 0x22, 23,  8, 25,
+};
+#endif // DOT11_N_SUPPORT //
+
+PUCHAR ReasonString[] = {
+	/* 0  */	 "Reserved",
+	/* 1  */	 "Unspecified Reason",
+	/* 2  */	 "Previous Auth no longer valid",
+	/* 3  */	 "STA is leaving / has left",
+	/* 4  */	 "DIS-ASSOC due to inactivity",
+	/* 5  */	 "AP unable to hanle all associations",
+	/* 6  */	 "class 2 error",
+	/* 7  */	 "class 3 error",
+	/* 8  */	 "STA is leaving / has left",
+	/* 9  */	 "require auth before assoc/re-assoc",
+	/* 10 */	 "Reserved",
+	/* 11 */	 "Reserved",
+	/* 12 */	 "Reserved",
+	/* 13 */	 "invalid IE",
+	/* 14 */	 "MIC error",
+	/* 15 */	 "4-way handshake timeout",
+	/* 16 */	 "2-way (group key) handshake timeout",
+	/* 17 */	 "4-way handshake IE diff among AssosReq/Rsp/Beacon",
+	/* 18 */
+};
+
+extern UCHAR	 OfdmRateToRxwiMCS[];
+// since RT61 has better RX sensibility, we have to limit TX ACK rate not to exceed our normal data TX rate.
+// otherwise the WLAN peer may not be able to receive the ACK thus downgrade its data TX rate
+ULONG BasicRateMask[12]				= {0xfffff001 /* 1-Mbps */, 0xfffff003 /* 2 Mbps */, 0xfffff007 /* 5.5 */, 0xfffff00f /* 11 */,
+									  0xfffff01f /* 6 */	 , 0xfffff03f /* 9 */	  , 0xfffff07f /* 12 */ , 0xfffff0ff /* 18 */,
+									  0xfffff1ff /* 24 */	 , 0xfffff3ff /* 36 */	  , 0xfffff7ff /* 48 */ , 0xffffffff /* 54 */};
+
+UCHAR MULTICAST_ADDR[MAC_ADDR_LEN] = {0x1,  0x00, 0x00, 0x00, 0x00, 0x00};
+UCHAR BROADCAST_ADDR[MAC_ADDR_LEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+UCHAR ZERO_MAC_ADDR[MAC_ADDR_LEN]  = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+
+// e.g. RssiSafeLevelForTxRate[RATE_36]" means if the current RSSI is greater than
+//		this value, then it's quaranteed capable of operating in 36 mbps TX rate in
+//		clean environment.
+//								  TxRate: 1   2   5.5	11	 6	  9    12	18	 24   36   48	54	 72  100
+CHAR RssiSafeLevelForTxRate[] ={  -92, -91, -90, -87, -88, -86, -85, -83, -81, -78, -72, -71, -40, -40 };
+
+UCHAR  RateIdToMbps[]	 = { 1, 2, 5, 11, 6, 9, 12, 18, 24, 36, 48, 54, 72, 100};
+USHORT RateIdTo500Kbps[] = { 2, 4, 11, 22, 12, 18, 24, 36, 48, 72, 96, 108, 144, 200};
+
+UCHAR  SsidIe	 = IE_SSID;
+UCHAR  SupRateIe = IE_SUPP_RATES;
+UCHAR  ExtRateIe = IE_EXT_SUPP_RATES;
+#ifdef DOT11_N_SUPPORT
+UCHAR  HtCapIe = IE_HT_CAP;
+UCHAR  AddHtInfoIe = IE_ADD_HT;
+UCHAR  NewExtChanIe = IE_SECONDARY_CH_OFFSET;
+#ifdef DOT11N_DRAFT3
+UCHAR  ExtHtCapIe = IE_EXT_CAPABILITY;
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+UCHAR  ErpIe	 = IE_ERP;
+UCHAR  DsIe 	 = IE_DS_PARM;
+UCHAR  TimIe	 = IE_TIM;
+UCHAR  WpaIe	 = IE_WPA;
+UCHAR  Wpa2Ie	 = IE_WPA2;
+UCHAR  IbssIe	 = IE_IBSS_PARM;
+UCHAR  Ccx2Ie	 = IE_CCX_V2;
+
+extern UCHAR	WPA_OUI[];
+
+UCHAR	SES_OUI[] = {0x00, 0x90, 0x4c};
+
+UCHAR	ZeroSsid[32] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+	0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
+
+// Reset the RFIC setting to new series
+RTMP_RF_REGS RF2850RegTable[] = {
+//		ch	 R1 		 R2 		 R3(TX0~4=0) R4
+		{1,  0x98402ecc, 0x984c0786, 0x9816b455, 0x9800510b},
+		{2,  0x98402ecc, 0x984c0786, 0x98168a55, 0x9800519f},
+		{3,  0x98402ecc, 0x984c078a, 0x98168a55, 0x9800518b},
+		{4,  0x98402ecc, 0x984c078a, 0x98168a55, 0x9800519f},
+		{5,  0x98402ecc, 0x984c078e, 0x98168a55, 0x9800518b},
+		{6,  0x98402ecc, 0x984c078e, 0x98168a55, 0x9800519f},
+		{7,  0x98402ecc, 0x984c0792, 0x98168a55, 0x9800518b},
+		{8,  0x98402ecc, 0x984c0792, 0x98168a55, 0x9800519f},
+		{9,  0x98402ecc, 0x984c0796, 0x98168a55, 0x9800518b},
+		{10, 0x98402ecc, 0x984c0796, 0x98168a55, 0x9800519f},
+		{11, 0x98402ecc, 0x984c079a, 0x98168a55, 0x9800518b},
+		{12, 0x98402ecc, 0x984c079a, 0x98168a55, 0x9800519f},
+		{13, 0x98402ecc, 0x984c079e, 0x98168a55, 0x9800518b},
+		{14, 0x98402ecc, 0x984c07a2, 0x98168a55, 0x98005193},
+
+		// 802.11 UNI / HyperLan 2
+		{36, 0x98402ecc, 0x984c099a, 0x98158a55, 0x980ed1a3},
+		{38, 0x98402ecc, 0x984c099e, 0x98158a55, 0x980ed193},
+		{40, 0x98402ec8, 0x984c0682, 0x98158a55, 0x980ed183},
+		{44, 0x98402ec8, 0x984c0682, 0x98158a55, 0x980ed1a3},
+		{46, 0x98402ec8, 0x984c0686, 0x98158a55, 0x980ed18b},
+		{48, 0x98402ec8, 0x984c0686, 0x98158a55, 0x980ed19b},
+		{52, 0x98402ec8, 0x984c068a, 0x98158a55, 0x980ed193},
+		{54, 0x98402ec8, 0x984c068a, 0x98158a55, 0x980ed1a3},
+		{56, 0x98402ec8, 0x984c068e, 0x98158a55, 0x980ed18b},
+		{60, 0x98402ec8, 0x984c0692, 0x98158a55, 0x980ed183},
+		{62, 0x98402ec8, 0x984c0692, 0x98158a55, 0x980ed193},
+		{64, 0x98402ec8, 0x984c0692, 0x98158a55, 0x980ed1a3}, // Plugfest#4, Day4, change RFR3 left4th 9->5.
+
+		// 802.11 HyperLan 2
+		{100, 0x98402ec8, 0x984c06b2, 0x98178a55, 0x980ed783},
+
+		// 2008.04.30 modified
+		// The system team has AN to improve the EVM value
+		// for channel 102 to 108 for the RT2850/RT2750 dual band solution.
+		{102, 0x98402ec8, 0x985c06b2, 0x98578a55, 0x980ed793},
+		{104, 0x98402ec8, 0x985c06b2, 0x98578a55, 0x980ed1a3},
+		{108, 0x98402ecc, 0x985c0a32, 0x98578a55, 0x980ed193},
+
+		{110, 0x98402ecc, 0x984c0a36, 0x98178a55, 0x980ed183},
+		{112, 0x98402ecc, 0x984c0a36, 0x98178a55, 0x980ed19b},
+		{116, 0x98402ecc, 0x984c0a3a, 0x98178a55, 0x980ed1a3},
+		{118, 0x98402ecc, 0x984c0a3e, 0x98178a55, 0x980ed193},
+		{120, 0x98402ec4, 0x984c0382, 0x98178a55, 0x980ed183},
+		{124, 0x98402ec4, 0x984c0382, 0x98178a55, 0x980ed193},
+		{126, 0x98402ec4, 0x984c0382, 0x98178a55, 0x980ed15b}, // 0x980ed1bb->0x980ed15b required by Rory 20070927
+		{128, 0x98402ec4, 0x984c0382, 0x98178a55, 0x980ed1a3},
+		{132, 0x98402ec4, 0x984c0386, 0x98178a55, 0x980ed18b},
+		{134, 0x98402ec4, 0x984c0386, 0x98178a55, 0x980ed193},
+		{136, 0x98402ec4, 0x984c0386, 0x98178a55, 0x980ed19b},
+		{140, 0x98402ec4, 0x984c038a, 0x98178a55, 0x980ed183},
+
+		// 802.11 UNII
+		{149, 0x98402ec4, 0x984c038a, 0x98178a55, 0x980ed1a7},
+		{151, 0x98402ec4, 0x984c038e, 0x98178a55, 0x980ed187},
+		{153, 0x98402ec4, 0x984c038e, 0x98178a55, 0x980ed18f},
+		{157, 0x98402ec4, 0x984c038e, 0x98178a55, 0x980ed19f},
+		{159, 0x98402ec4, 0x984c038e, 0x98178a55, 0x980ed1a7},
+		{161, 0x98402ec4, 0x984c0392, 0x98178a55, 0x980ed187},
+		{165, 0x98402ec4, 0x984c0392, 0x98178a55, 0x980ed197},
+
+		// Japan
+		{184, 0x95002ccc, 0x9500491e, 0x9509be55, 0x950c0a0b},
+		{188, 0x95002ccc, 0x95004922, 0x9509be55, 0x950c0a13},
+		{192, 0x95002ccc, 0x95004926, 0x9509be55, 0x950c0a1b},
+		{196, 0x95002ccc, 0x9500492a, 0x9509be55, 0x950c0a23},
+		{208, 0x95002ccc, 0x9500493a, 0x9509be55, 0x950c0a13},
+		{212, 0x95002ccc, 0x9500493e, 0x9509be55, 0x950c0a1b},
+		{216, 0x95002ccc, 0x95004982, 0x9509be55, 0x950c0a23},
+
+		// still lack of MMAC(Japan) ch 34,38,42,46
+};
+UCHAR	NUM_OF_2850_CHNL = (sizeof(RF2850RegTable) / sizeof(RTMP_RF_REGS));
+
+FREQUENCY_ITEM FreqItems3020[] =
+{
+	/**************************************************/
+	// ISM : 2.4 to 2.483 GHz                         //
+	/**************************************************/
+	// 11g
+	/**************************************************/
+	//-CH---N-------R---K-----------
+	{1,    241,  2,  2},
+	{2,    241,	 2,  7},
+	{3,    242,	 2,  2},
+	{4,    242,	 2,  7},
+	{5,    243,	 2,  2},
+	{6,    243,	 2,  7},
+	{7,    244,	 2,  2},
+	{8,    244,	 2,  7},
+	{9,    245,	 2,  2},
+	{10,   245,	 2,  7},
+	{11,   246,	 2,  2},
+	{12,   246,	 2,  7},
+	{13,   247,	 2,  2},
+	{14,   248,	 2,  4},
+};
+#define	NUM_OF_3020_CHNL	(sizeof(FreqItems3020) / sizeof(FREQUENCY_ITEM))
+
+/*
+	==========================================================================
+	Description:
+		initialize the MLME task and its data structure (queue, spinlock,
+		timer, state machines).
+
+	IRQL = PASSIVE_LEVEL
+
+	Return:
+		always return NDIS_STATUS_SUCCESS
+
+	==========================================================================
+*/
+NDIS_STATUS MlmeInit(
+	IN PRTMP_ADAPTER pAd)
+{
+	NDIS_STATUS Status = NDIS_STATUS_SUCCESS;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> MLME Initialize\n"));
+
+	do
+	{
+		Status = MlmeQueueInit(&pAd->Mlme.Queue);
+		if(Status != NDIS_STATUS_SUCCESS)
+			break;
+
+		pAd->Mlme.bRunning = FALSE;
+		NdisAllocateSpinLock(&pAd->Mlme.TaskLock);
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			BssTableInit(&pAd->ScanTab);
+
+			// init STA state machines
+			AssocStateMachineInit(pAd, &pAd->Mlme.AssocMachine, pAd->Mlme.AssocFunc);
+			AuthStateMachineInit(pAd, &pAd->Mlme.AuthMachine, pAd->Mlme.AuthFunc);
+			AuthRspStateMachineInit(pAd, &pAd->Mlme.AuthRspMachine, pAd->Mlme.AuthRspFunc);
+			SyncStateMachineInit(pAd, &pAd->Mlme.SyncMachine, pAd->Mlme.SyncFunc);
+			WpaPskStateMachineInit(pAd, &pAd->Mlme.WpaPskMachine, pAd->Mlme.WpaPskFunc);
+			AironetStateMachineInit(pAd, &pAd->Mlme.AironetMachine, pAd->Mlme.AironetFunc);
+
+#ifdef QOS_DLS_SUPPORT
+			DlsStateMachineInit(pAd, &pAd->Mlme.DlsMachine, pAd->Mlme.DlsFunc);
+#endif // QOS_DLS_SUPPORT //
+
+
+			// Since we are using switch/case to implement it, the init is different from the above
+			// state machine init
+			MlmeCntlInit(pAd, &pAd->Mlme.CntlMachine, NULL);
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+
+
+		ActionStateMachineInit(pAd, &pAd->Mlme.ActMachine, pAd->Mlme.ActFunc);
+
+		// Init mlme periodic timer
+		RTMPInitTimer(pAd, &pAd->Mlme.PeriodicTimer, GET_TIMER_FUNCTION(MlmePeriodicExec), pAd, TRUE);
+
+		// Set mlme periodic timer
+		RTMPSetTimer(&pAd->Mlme.PeriodicTimer, MLME_TASK_EXEC_INTV);
+
+		// software-based RX Antenna diversity
+		RTMPInitTimer(pAd, &pAd->Mlme.RxAntEvalTimer, GET_TIMER_FUNCTION(AsicRxAntEvalTimeout), pAd, FALSE);
+
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef RT2860
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+	        if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE))
+	        {
+	            // only PCIe cards need these two timers
+	    		RTMPInitTimer(pAd, &pAd->Mlme.PsPollTimer, GET_TIMER_FUNCTION(PsPollWakeExec), pAd, FALSE);
+	    		RTMPInitTimer(pAd, &pAd->Mlme.RadioOnOffTimer, GET_TIMER_FUNCTION(RadioOnExec), pAd, FALSE);
+	        }
+		}
+#endif // RT2860 //
+#endif // CONFIG_STA_SUPPORT //
+
+	} while (FALSE);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- MLME Initialize\n"));
+
+	return Status;
+}
+
+/*
+	==========================================================================
+	Description:
+		main loop of the MLME
+	Pre:
+		Mlme has to be initialized, and there are something inside the queue
+	Note:
+		This function is invoked from MPSetInformation and MPReceive;
+		This task guarantee only one MlmeHandler will run.
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID MlmeHandler(
+	IN PRTMP_ADAPTER pAd)
+{
+	MLME_QUEUE_ELEM 	   *Elem = NULL;
+#ifdef APCLI_SUPPORT
+	SHORT apcliIfIndex;
+#endif
+
+	// Only accept MLME and Frame from peer side, no other (control/data) frame should
+	// get into this state machine
+
+	NdisAcquireSpinLock(&pAd->Mlme.TaskLock);
+	if(pAd->Mlme.bRunning)
+	{
+		NdisReleaseSpinLock(&pAd->Mlme.TaskLock);
+		return;
+	}
+	else
+	{
+		pAd->Mlme.bRunning = TRUE;
+	}
+	NdisReleaseSpinLock(&pAd->Mlme.TaskLock);
+
+	while (!MlmeQueueEmpty(&pAd->Mlme.Queue))
+	{
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_MLME_RESET_IN_PROGRESS) ||
+			RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS) ||
+			RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("Device Halted or Removed or MlmeRest, exit MlmeHandler! (queue num = %ld)\n", pAd->Mlme.Queue.Num));
+			break;
+		}
+
+#ifdef RALINK_ATE
+		if(ATE_ON(pAd))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now in MlmeHandler\n"));
+			break;
+		}
+#endif // RALINK_ATE //
+
+		//From message type, determine which state machine I should drive
+		if (MlmeDequeue(&pAd->Mlme.Queue, &Elem))
+		{
+
+			// if dequeue success
+			switch (Elem->Machine)
+			{
+				// STA state machines
+#ifdef	CONFIG_STA_SUPPORT
+				case ASSOC_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.AssocMachine, Elem);
+					break;
+				case AUTH_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.AuthMachine, Elem);
+					break;
+				case AUTH_RSP_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.AuthRspMachine, Elem);
+					break;
+				case SYNC_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.SyncMachine, Elem);
+					break;
+				case MLME_CNTL_STATE_MACHINE:
+					MlmeCntlMachinePerformAction(pAd, &pAd->Mlme.CntlMachine, Elem);
+					break;
+				case WPA_PSK_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.WpaPskMachine, Elem);
+					break;
+#ifdef LEAP_SUPPORT
+				case LEAP_STATE_MACHINE:
+					LeapMachinePerformAction(pAd, &pAd->Mlme.LeapMachine, Elem);
+					break;
+#endif
+				case AIRONET_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.AironetMachine, Elem);
+					break;
+
+#ifdef QOS_DLS_SUPPORT
+				case DLS_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.DlsMachine, Elem);
+					break;
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+				case ACTION_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.ActMachine, Elem);
+					break;
+
+
+
+
+				default:
+					DBGPRINT(RT_DEBUG_TRACE, ("ERROR: Illegal machine %ld in MlmeHandler()\n", Elem->Machine));
+					break;
+			} // end of switch
+
+			// free MLME element
+			Elem->Occupied = FALSE;
+			Elem->MsgLen = 0;
+
+		}
+		else {
+			DBGPRINT_ERR(("MlmeHandler: MlmeQueue empty\n"));
+		}
+	}
+
+	NdisAcquireSpinLock(&pAd->Mlme.TaskLock);
+	pAd->Mlme.bRunning = FALSE;
+	NdisReleaseSpinLock(&pAd->Mlme.TaskLock);
+}
+
+/*
+	==========================================================================
+	Description:
+		Destructor of MLME (Destroy queue, state machine, spin lock and timer)
+	Parameters:
+		Adapter - NIC Adapter pointer
+	Post:
+		The MLME task will no longer work properly
+
+	IRQL = PASSIVE_LEVEL
+
+	==========================================================================
+ */
+VOID MlmeHalt(
+	IN PRTMP_ADAPTER pAd)
+{
+	BOOLEAN 	  Cancelled;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("==> MlmeHalt\n"));
+
+	if (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST))
+	{
+		// disable BEACON generation and other BEACON related hardware timers
+		AsicDisableSync(pAd);
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+#ifdef QOS_DLS_SUPPORT
+		UCHAR		i;
+#endif // QOS_DLS_SUPPORT //
+		// Cancel pending timers
+		RTMPCancelTimer(&pAd->MlmeAux.AssocTimer,		&Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.ReassocTimer,		&Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.DisassocTimer,	&Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.AuthTimer,		&Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.BeaconTimer,		&Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.ScanTimer,		&Cancelled);
+#ifdef RT2860
+	    if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE))
+	    {
+	   	    RTMPCancelTimer(&pAd->Mlme.PsPollTimer,		&Cancelled);
+		    RTMPCancelTimer(&pAd->Mlme.RadioOnOffTimer,		&Cancelled);
+		}
+#endif // RT2860 //
+
+#ifdef QOS_DLS_SUPPORT
+		for (i=0; i<MAX_NUM_OF_DLS_ENTRY; i++)
+		{
+			RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &Cancelled);
+		}
+#endif // QOS_DLS_SUPPORT //
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	RTMPCancelTimer(&pAd->Mlme.PeriodicTimer,		&Cancelled);
+	RTMPCancelTimer(&pAd->Mlme.RxAntEvalTimer,		&Cancelled);
+
+
+
+	if (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST))
+	{
+		// Set LED
+		RTMPSetLED(pAd, LED_HALT);
+        RTMPSetSignalLED(pAd, -100);	// Force signal strength Led to be turned off, firmware is not done it.
+	}
+
+	RTMPusecDelay(5000);    //  5 msec to gurantee Ant Diversity timer canceled
+
+	MlmeQueueDestroy(&pAd->Mlme.Queue);
+	NdisFreeSpinLock(&pAd->Mlme.TaskLock);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<== MlmeHalt\n"));
+}
+
+VOID MlmeResetRalinkCounters(
+	IN  PRTMP_ADAPTER   pAd)
+{
+	pAd->RalinkCounters.LastOneSecRxOkDataCnt = pAd->RalinkCounters.OneSecRxOkDataCnt;
+	// clear all OneSecxxx counters.
+	pAd->RalinkCounters.OneSecBeaconSentCnt = 0;
+	pAd->RalinkCounters.OneSecFalseCCACnt = 0;
+	pAd->RalinkCounters.OneSecRxFcsErrCnt = 0;
+	pAd->RalinkCounters.OneSecRxOkCnt = 0;
+	pAd->RalinkCounters.OneSecTxFailCount = 0;
+	pAd->RalinkCounters.OneSecTxNoRetryOkCount = 0;
+	pAd->RalinkCounters.OneSecTxRetryOkCount = 0;
+	pAd->RalinkCounters.OneSecRxOkDataCnt = 0;
+
+	// TODO: for debug only. to be removed
+	pAd->RalinkCounters.OneSecOsTxCount[QID_AC_BE] = 0;
+	pAd->RalinkCounters.OneSecOsTxCount[QID_AC_BK] = 0;
+	pAd->RalinkCounters.OneSecOsTxCount[QID_AC_VI] = 0;
+	pAd->RalinkCounters.OneSecOsTxCount[QID_AC_VO] = 0;
+	pAd->RalinkCounters.OneSecDmaDoneCount[QID_AC_BE] = 0;
+	pAd->RalinkCounters.OneSecDmaDoneCount[QID_AC_BK] = 0;
+	pAd->RalinkCounters.OneSecDmaDoneCount[QID_AC_VI] = 0;
+	pAd->RalinkCounters.OneSecDmaDoneCount[QID_AC_VO] = 0;
+	pAd->RalinkCounters.OneSecTxDoneCount = 0;
+	pAd->RalinkCounters.OneSecRxCount = 0;
+	pAd->RalinkCounters.OneSecTxAggregationCount = 0;
+	pAd->RalinkCounters.OneSecRxAggregationCount = 0;
+
+	return;
+}
+
+unsigned long rx_AMSDU;
+unsigned long rx_Total;
+
+/*
+	==========================================================================
+	Description:
+		This routine is executed periodically to -
+		1. Decide if it's a right time to turn on PwrMgmt bit of all
+		   outgoiing frames
+		2. Calculate ChannelQuality based on statistics of the last
+		   period, so that TX rate won't toggling very frequently between a
+		   successful TX and a failed TX.
+		3. If the calculated ChannelQuality indicated current connection not
+		   healthy, then a ROAMing attempt is tried here.
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+#define ADHOC_BEACON_LOST_TIME		(8*OS_HZ)  // 8 sec
+VOID MlmePeriodicExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+	ULONG			TxTotalCnt;
+	PRTMP_ADAPTER	pAd = (RTMP_ADAPTER *)FunctionContext;
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef RT2860
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+	    // If Hardware controlled Radio enabled, we have to check GPIO pin2 every 2 second.
+		// Move code to here, because following code will return when radio is off
+		if ((pAd->Mlme.PeriodicRound % (MLME_TASK_EXEC_MULTIPLE * 2) == 0) && (pAd->StaCfg.bHardwareRadio == TRUE) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
+			(pAd->bPCIclkOff == FALSE))
+		{
+			UINT32				data = 0;
+
+			// Read GPIO pin2 as Hardware controlled radio state
+			RTMP_IO_READ32(pAd, GPIO_CTRL_CFG, &data);
+			if (data & 0x04)
+			{
+				pAd->StaCfg.bHwRadio = TRUE;
+			}
+			else
+			{
+				pAd->StaCfg.bHwRadio = FALSE;
+			}
+			if (pAd->StaCfg.bRadio != (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio))
+			{
+				pAd->StaCfg.bRadio = (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio);
+				if (pAd->StaCfg.bRadio == TRUE)
+				{
+					MlmeRadioOn(pAd);
+					// Update extra information
+					pAd->ExtraInfo = EXTRA_INFO_CLEAR;
+				}
+				else
+				{
+					MlmeRadioOff(pAd);
+					// Update extra information
+					pAd->ExtraInfo = HW_RADIO_OFF;
+				}
+			}
+		}
+	}
+#endif // RT2860 //
+#endif // CONFIG_STA_SUPPORT //
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if ((RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_HALT_IN_PROGRESS |
+								fRTMP_ADAPTER_RADIO_OFF |
+								fRTMP_ADAPTER_RADIO_MEASUREMENT |
+								fRTMP_ADAPTER_RESET_IN_PROGRESS))))
+		return;
+
+	RT28XX_MLME_PRE_SANITY_CHECK(pAd);
+
+#ifdef RALINK_ATE
+	/* Do not show RSSI until "Normal 1 second Mlme PeriodicExec". */
+	if (ATE_ON(pAd))
+	{
+		if (pAd->Mlme.PeriodicRound % MLME_TASK_EXEC_MULTIPLE != (MLME_TASK_EXEC_MULTIPLE - 1))
+	{
+			pAd->Mlme.PeriodicRound ++;
+			return;
+		}
+	}
+#endif // RALINK_ATE //
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// Do nothing if monitor mode is on
+		if (MONITOR_ON(pAd))
+			return;
+
+		if (pAd->Mlme.PeriodicRound & 0x1)
+		{
+			// This is the fix for wifi 11n extension channel overlapping test case.  for 2860D
+			if (((pAd->MACVersion & 0xffff) == 0x0101) &&
+				(STA_TGN_WIFI_ON(pAd)) &&
+				(pAd->CommonCfg.IOTestParm.bToggle == FALSE))
+
+				{
+					RTMP_IO_WRITE32(pAd, TXOP_CTRL_CFG, 0x24Bf);
+					pAd->CommonCfg.IOTestParm.bToggle = TRUE;
+				}
+				else if ((STA_TGN_WIFI_ON(pAd)) &&
+						((pAd->MACVersion & 0xffff) == 0x0101))
+				{
+					RTMP_IO_WRITE32(pAd, TXOP_CTRL_CFG, 0x243f);
+					pAd->CommonCfg.IOTestParm.bToggle = FALSE;
+				}
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	pAd->bUpdateBcnCntDone = FALSE;
+
+//	RECBATimerTimeout(SystemSpecific1,FunctionContext,SystemSpecific2,SystemSpecific3);
+	pAd->Mlme.PeriodicRound ++;
+
+	// execute every 500ms
+	if ((pAd->Mlme.PeriodicRound % 5 == 0) && RTMPAutoRateSwitchCheck(pAd)/*(OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_TX_RATE_SWITCH_ENABLED))*/)
+	{
+#ifdef CONFIG_STA_SUPPORT
+		// perform dynamic tx rate switching based on past TX history
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			if ((OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED)
+					)
+				&& (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE)))
+				MlmeDynamicTxRateSwitching(pAd);
+		}
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+	// Normal 1 second Mlme PeriodicExec.
+	if (pAd->Mlme.PeriodicRound %MLME_TASK_EXEC_MULTIPLE == 0)
+	{
+                pAd->Mlme.OneSecPeriodicRound ++;
+
+#ifdef RALINK_ATE
+    	if (ATE_ON(pAd))
+    	{
+			/* request from Baron : move this routine from later to here */
+			/* for showing Rx error count in ATE RXFRAME */
+            NICUpdateRawCounters(pAd);
+			if (pAd->ate.bRxFer == 1)
+			{
+				pAd->ate.RxTotalCnt += pAd->ate.RxCntPerSec;
+			    ate_print(KERN_EMERG "MlmePeriodicExec: Rx packet cnt = %d/%d\n", pAd->ate.RxCntPerSec, pAd->ate.RxTotalCnt);
+				pAd->ate.RxCntPerSec = 0;
+
+				if (pAd->ate.RxAntennaSel == 0)
+					ate_print(KERN_EMERG "MlmePeriodicExec: Rx AvgRssi0=%d, AvgRssi1=%d, AvgRssi2=%d\n\n",
+						pAd->ate.AvgRssi0, pAd->ate.AvgRssi1, pAd->ate.AvgRssi2);
+				else
+					ate_print(KERN_EMERG "MlmePeriodicExec: Rx AvgRssi=%d\n\n", pAd->ate.AvgRssi0);
+			}
+			MlmeResetRalinkCounters(pAd);
+			return;
+    	}
+#endif // RALINK_ATE //
+
+
+		if (rx_Total)
+		{
+
+			// reset counters
+			rx_AMSDU = 0;
+			rx_Total = 0;
+		}
+
+		// Media status changed, report to NDIS
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_MEDIA_STATE_CHANGE))
+		{
+			RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_MEDIA_STATE_CHANGE);
+			if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+			{
+				pAd->IndicateMediaState = NdisMediaStateConnected;
+				RTMP_IndicateMediaState(pAd);
+
+			}
+			else
+			{
+				pAd->IndicateMediaState = NdisMediaStateDisconnected;
+				RTMP_IndicateMediaState(pAd);
+			}
+		}
+
+		NdisGetSystemUpTime(&pAd->Mlme.Now32);
+
+		// add the most up-to-date h/w raw counters into software variable, so that
+		// the dynamic tuning mechanism below are based on most up-to-date information
+		NICUpdateRawCounters(pAd);
+
+
+#ifdef DOT11_N_SUPPORT
+   		// Need statistics after read counter. So put after NICUpdateRawCounters
+		ORIBATimerTimeout(pAd);
+#endif // DOT11_N_SUPPORT //
+
+
+		// The time period for checking antenna is according to traffic
+		if (pAd->Mlme.bEnableAutoAntennaCheck)
+		{
+			TxTotalCnt = pAd->RalinkCounters.OneSecTxNoRetryOkCount +
+							 pAd->RalinkCounters.OneSecTxRetryOkCount +
+							 pAd->RalinkCounters.OneSecTxFailCount;
+
+			if (TxTotalCnt > 50)
+			{
+				if (pAd->Mlme.OneSecPeriodicRound % 10 == 0)
+				{
+					AsicEvaluateRxAnt(pAd);
+				}
+			}
+			else
+			{
+				if (pAd->Mlme.OneSecPeriodicRound % 3 == 0)
+				{
+					AsicEvaluateRxAnt(pAd);
+				}
+			}
+		}
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			STAMlmePeriodicExec(pAd);
+#endif // CONFIG_STA_SUPPORT //
+
+		MlmeResetRalinkCounters(pAd);
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+#ifdef RT2860
+			if (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST) && (pAd->bPCIclkOff == FALSE))
+#endif // RT2860 //
+			{
+				// When Adhoc beacon is enabled and RTS/CTS is enabled, there is a chance that hardware MAC FSM will run into a deadlock
+				// and sending CTS-to-self over and over.
+				// Software Patch Solution:
+				// 1. Polling debug state register 0x10F4 every one second.
+				// 2. If in 0x10F4 the ((bit29==1) && (bit7==1)) OR ((bit29==1) && (bit5==1)), it means the deadlock has occurred.
+				// 3. If the deadlock occurred, reset MAC/BBP by setting 0x1004 to 0x0001 for a while then setting it back to 0x000C again.
+
+				UINT32	MacReg = 0;
+
+				RTMP_IO_READ32(pAd, 0x10F4, &MacReg);
+				if (((MacReg & 0x20000000) && (MacReg & 0x80)) || ((MacReg & 0x20000000) && (MacReg & 0x20)))
+				{
+					RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x1);
+					RTMPusecDelay(1);
+					RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0xC);
+
+					DBGPRINT(RT_DEBUG_WARN,("Warning, MAC specific condition occurs \n"));
+				}
+			}
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+		RT28XX_MLME_HANDLER(pAd);
+	}
+
+
+	pAd->bUpdateBcnCntDone = FALSE;
+}
+
+#ifdef CONFIG_STA_SUPPORT
+VOID STAMlmePeriodicExec(
+	PRTMP_ADAPTER pAd)
+{
+	ULONG			    TxTotalCnt;
+
+//
+// We return here in ATE mode, because the statistics
+// that ATE needs are not collected via this routine.
+//
+#ifdef RALINK_ATE
+	// It is supposed that we will never reach here in ATE mode.
+	ASSERT(!(ATE_ON(pAd)));
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+    if (pAd->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_DISABLE)
+#endif // WPA_SUPPLICANT_SUPPORT //
+    {
+    	// WPA MIC error should block association attempt for 60 seconds
+    	if (pAd->StaCfg.bBlockAssoc && (pAd->StaCfg.LastMicErrorTime + (60 * OS_HZ) < pAd->Mlme.Now32))
+    		pAd->StaCfg.bBlockAssoc = FALSE;
+    }
+
+    if ((pAd->PreMediaState != pAd->IndicateMediaState) && (pAd->CommonCfg.bWirelessEvent))
+	{
+		if (pAd->IndicateMediaState == NdisMediaStateConnected)
+		{
+			RTMPSendWirelessEvent(pAd, IW_STA_LINKUP_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+		}
+		pAd->PreMediaState = pAd->IndicateMediaState;
+	}
+
+
+
+
+   	AsicStaBbpTuning(pAd);
+
+	TxTotalCnt = pAd->RalinkCounters.OneSecTxNoRetryOkCount +
+					 pAd->RalinkCounters.OneSecTxRetryOkCount +
+					 pAd->RalinkCounters.OneSecTxFailCount;
+
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+	{
+		// update channel quality for Roaming and UI LinkQuality display
+		MlmeCalculateChannelQuality(pAd, pAd->Mlme.Now32);
+	}
+
+	// must be AFTER MlmeDynamicTxRateSwitching() because it needs to know if
+	// Radio is currently in noisy environment
+	if (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+		AsicAdjustTxPower(pAd);
+
+	if (INFRA_ON(pAd))
+	{
+#ifdef QOS_DLS_SUPPORT
+		// Check DLS time out, then tear down those session
+		RTMPCheckDLSTimeOut(pAd);
+#endif // QOS_DLS_SUPPORT //
+
+		// Is PSM bit consistent with user power management policy?
+		// This is the only place that will set PSM bit ON.
+		if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+		MlmeCheckPsmChange(pAd, pAd->Mlme.Now32);
+
+		pAd->RalinkCounters.LastOneSecTotalTxCount = TxTotalCnt;
+
+		if ((pAd->StaCfg.LastBeaconRxTime + 1*OS_HZ < pAd->Mlme.Now32) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS)) &&
+			((TxTotalCnt + pAd->RalinkCounters.OneSecRxOkCnt < 600)))
+		{
+			RTMPSetAGCInitValue(pAd, BW_20);
+			DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - No BEACON. restore R66 to the low bound(%d) \n", (0x2E + GET_LNA_GAIN(pAd))));
+		}
+
+        {
+    		if (pAd->CommonCfg.bAPSDCapable && pAd->CommonCfg.APEdcaParm.bAPSDCapable)
+    		{
+    		    // When APSD is enabled, the period changes as 20 sec
+    			if ((pAd->Mlme.OneSecPeriodicRound % 20) == 8)
+    				RTMPSendNullFrame(pAd, pAd->CommonCfg.TxRate, TRUE);
+    		}
+    		else
+    		{
+    		    // Send out a NULL frame every 10 sec to inform AP that STA is still alive (Avoid being age out)
+    			if ((pAd->Mlme.OneSecPeriodicRound % 10) == 8)
+                {
+                    if (pAd->CommonCfg.bWmmCapable)
+    					RTMPSendNullFrame(pAd, pAd->CommonCfg.TxRate, TRUE);
+                    else
+						RTMPSendNullFrame(pAd, pAd->CommonCfg.TxRate, FALSE);
+                }
+    		}
+        }
+
+		if (CQI_IS_DEAD(pAd->Mlme.ChannelQuality))
+			{
+			DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - No BEACON. Dead CQI. Auto Recovery attempt #%ld\n", pAd->RalinkCounters.BadCQIAutoRecoveryCount));
+			pAd->StaCfg.CCXAdjacentAPReportFlag = TRUE;
+			pAd->StaCfg.CCXAdjacentAPLinkDownTime = pAd->StaCfg.LastBeaconRxTime;
+
+			// Lost AP, send disconnect & link down event
+			LinkDown(pAd, FALSE);
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+            if (pAd->StaCfg.WpaSupplicantUP)
+			{
+                union iwreq_data    wrqu;
+                //send disassociate event to wpa_supplicant
+                memset(&wrqu, 0, sizeof(wrqu));
+                wrqu.data.flags = RT_DISASSOC_EVENT_FLAG;
+                wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+            }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+            {
+                union iwreq_data    wrqu;
+                memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+                wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+            }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+
+			MlmeAutoReconnectLastSSID(pAd);
+		}
+		else if (CQI_IS_BAD(pAd->Mlme.ChannelQuality))
+		{
+			pAd->RalinkCounters.BadCQIAutoRecoveryCount ++;
+			DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - Bad CQI. Auto Recovery attempt #%ld\n", pAd->RalinkCounters.BadCQIAutoRecoveryCount));
+			MlmeAutoReconnectLastSSID(pAd);
+		}
+
+		// Add auto seamless roaming
+		if (pAd->StaCfg.bFastRoaming)
+		{
+			SHORT	dBmToRoam = (SHORT)pAd->StaCfg.dBmToRoam;
+
+			DBGPRINT(RT_DEBUG_TRACE, ("Rssi=%d, dBmToRoam=%d\n", RTMPMaxRssi(pAd, pAd->StaCfg.RssiSample.LastRssi0, pAd->StaCfg.RssiSample.LastRssi1, pAd->StaCfg.RssiSample.LastRssi2), (CHAR)dBmToRoam));
+
+			if (RTMPMaxRssi(pAd, pAd->StaCfg.RssiSample.LastRssi0, pAd->StaCfg.RssiSample.LastRssi1, pAd->StaCfg.RssiSample.LastRssi2) <= (CHAR)dBmToRoam)
+			{
+				MlmeCheckForFastRoaming(pAd, pAd->Mlme.Now32);
+			}
+		}
+	}
+	else if (ADHOC_ON(pAd))
+	{
+		// 2003-04-17 john. this is a patch that driver forces a BEACON out if ASIC fails
+		// the "TX BEACON competition" for the entire past 1 sec.
+		// So that even when ASIC's BEACONgen engine been blocked
+		// by peer's BEACON due to slower system clock, this STA still can send out
+		// minimum BEACON to tell the peer I'm alive.
+		// drawback is that this BEACON won't be well aligned at TBTT boundary.
+		// EnqueueBeaconFrame(pAd);			  // software send BEACON
+
+		// if all 11b peers leave this BSS more than 5 seconds, update Tx rate,
+		// restore outgoing BEACON to support B/G-mixed mode
+		if ((pAd->CommonCfg.Channel <= 14)			   &&
+			(pAd->CommonCfg.MaxTxRate <= RATE_11)	   &&
+			(pAd->CommonCfg.MaxDesiredRate > RATE_11)  &&
+			((pAd->StaCfg.Last11bBeaconRxTime + 5*OS_HZ) < pAd->Mlme.Now32))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - last 11B peer left, update Tx rates\n"));
+			NdisMoveMemory(pAd->StaActive.SupRate, pAd->CommonCfg.SupRate, MAX_LEN_OF_SUPPORTED_RATES);
+			pAd->StaActive.SupRateLen = pAd->CommonCfg.SupRateLen;
+			MlmeUpdateTxRates(pAd, FALSE, 0);
+			MakeIbssBeacon(pAd);		// re-build BEACON frame
+			AsicEnableIbssSync(pAd);	// copy to on-chip memory
+			pAd->StaCfg.AdhocBOnlyJoined = FALSE;
+		}
+
+#ifdef DOT11_N_SUPPORT
+		if (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED)
+		{
+			if ((pAd->StaCfg.AdhocBGJoined) &&
+				((pAd->StaCfg.Last11gBeaconRxTime + 5 * OS_HZ) < pAd->Mlme.Now32))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - last 11G peer left\n"));
+				pAd->StaCfg.AdhocBGJoined = FALSE;
+			}
+
+			if ((pAd->StaCfg.Adhoc20NJoined) &&
+				((pAd->StaCfg.Last20NBeaconRxTime + 5 * OS_HZ) < pAd->Mlme.Now32))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - last 20MHz N peer left\n"));
+				pAd->StaCfg.Adhoc20NJoined = FALSE;
+			}
+		}
+#endif // DOT11_N_SUPPORT //
+
+		//radar detect
+		if ((pAd->CommonCfg.Channel > 14)
+			&& (pAd->CommonCfg.bIEEE80211H == 1)
+			&& RadarChannelCheck(pAd, pAd->CommonCfg.Channel))
+		{
+			RadarDetectPeriodic(pAd);
+		}
+
+		// If all peers leave, and this STA becomes the last one in this IBSS, then change MediaState
+		// to DISCONNECTED. But still holding this IBSS (i.e. sending BEACON) so that other STAs can
+		// join later.
+		if ((pAd->StaCfg.LastBeaconRxTime + ADHOC_BEACON_LOST_TIME < pAd->Mlme.Now32) &&
+			OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+		{
+			MLME_START_REQ_STRUCT     StartReq;
+
+			DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - excessive BEACON lost, last STA in this IBSS, MediaState=Disconnected\n"));
+			LinkDown(pAd, FALSE);
+
+			StartParmFill(pAd, &StartReq, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen);
+			MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_START_REQ, sizeof(MLME_START_REQ_STRUCT), &StartReq);
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_START;
+		}
+	}
+	else // no INFRA nor ADHOC connection
+	{
+
+		if (pAd->StaCfg.bScanReqIsFromWebUI &&
+            ((pAd->StaCfg.LastScanTime + 30 * OS_HZ) > pAd->Mlme.Now32))
+			goto SKIP_AUTO_SCAN_CONN;
+        else
+            pAd->StaCfg.bScanReqIsFromWebUI = FALSE;
+
+		if ((pAd->StaCfg.bAutoReconnect == TRUE)
+			&& RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_START_UP)
+			&& (MlmeValidateSSID(pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.AutoReconnectSsidLen) == TRUE))
+		{
+			if ((pAd->ScanTab.BssNr==0) && (pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE))
+			{
+				MLME_SCAN_REQ_STRUCT	   ScanReq;
+
+				if ((pAd->StaCfg.LastScanTime + 10 * OS_HZ) < pAd->Mlme.Now32)
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("STAMlmePeriodicExec():CNTL - ScanTab.BssNr==0, start a new ACTIVE scan SSID[%s]\n", pAd->MlmeAux.AutoReconnectSsid));
+					ScanParmFill(pAd, &ScanReq, pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.AutoReconnectSsidLen, BSS_ANY, SCAN_ACTIVE);
+					MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_SCAN_REQ, sizeof(MLME_SCAN_REQ_STRUCT), &ScanReq);
+					pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_LIST_SCAN;
+					// Reset Missed scan number
+					pAd->StaCfg.LastScanTime = pAd->Mlme.Now32;
+				}
+				else if (pAd->StaCfg.BssType == BSS_ADHOC)	// Quit the forever scan when in a very clean room
+					MlmeAutoReconnectLastSSID(pAd);
+			}
+			else if (pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE)
+			{
+				if ((pAd->Mlme.OneSecPeriodicRound % 7) == 0)
+				{
+					MlmeAutoScan(pAd);
+					pAd->StaCfg.LastScanTime = pAd->Mlme.Now32;
+				}
+				else
+				{
+#ifdef CARRIER_DETECTION_SUPPORT // Roger sync Carrier
+					if (pAd->CommonCfg.CarrierDetect.Enable == TRUE)
+					{
+						if ((pAd->Mlme.OneSecPeriodicRound % 5) == 1)
+							MlmeAutoReconnectLastSSID(pAd);
+					}
+					else
+#endif // CARRIER_DETECTION_SUPPORT //
+						MlmeAutoReconnectLastSSID(pAd);
+				}
+			}
+		}
+	}
+
+SKIP_AUTO_SCAN_CONN:
+
+#ifdef DOT11_N_SUPPORT
+    if ((pAd->MacTab.Content[BSSID_WCID].TXBAbitmap !=0) && (pAd->MacTab.fAnyBASession == FALSE))
+	{
+		pAd->MacTab.fAnyBASession = TRUE;
+		AsicUpdateProtect(pAd, HT_FORCERTSCTS,  ALLN_SETPROTECT, FALSE, FALSE);
+	}
+	else if ((pAd->MacTab.Content[BSSID_WCID].TXBAbitmap ==0) && (pAd->MacTab.fAnyBASession == TRUE))
+	{
+		pAd->MacTab.fAnyBASession = FALSE;
+		AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode,  ALLN_SETPROTECT, FALSE, FALSE);
+	}
+#endif // DOT11_N_SUPPORT //
+
+
+#ifdef DOT11_N_SUPPORT
+#ifdef DOT11N_DRAFT3
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_SCAN_2040))
+		TriEventCounterMaintenance(pAd);
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+
+	return;
+}
+
+// Link down report
+VOID LinkDownExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+
+	RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+
+	pAd->IndicateMediaState = NdisMediaStateDisconnected;
+	RTMP_IndicateMediaState(pAd);
+    pAd->ExtraInfo = GENERAL_LINK_DOWN;
+}
+
+// IRQL = DISPATCH_LEVEL
+VOID MlmeAutoScan(
+	IN PRTMP_ADAPTER pAd)
+{
+	// check CntlMachine.CurrState to avoid collision with NDIS SetOID request
+	if (pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - Driver auto scan\n"));
+		MlmeEnqueue(pAd,
+					MLME_CNTL_STATE_MACHINE,
+					OID_802_11_BSSID_LIST_SCAN,
+					0,
+					NULL);
+		RT28XX_MLME_HANDLER(pAd);
+	}
+}
+
+// IRQL = DISPATCH_LEVEL
+VOID MlmeAutoReconnectLastSSID(
+	IN PRTMP_ADAPTER pAd)
+{
+
+
+	// check CntlMachine.CurrState to avoid collision with NDIS SetOID request
+	if ((pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE) &&
+		(MlmeValidateSSID(pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.AutoReconnectSsidLen) == TRUE))
+	{
+		NDIS_802_11_SSID OidSsid;
+		OidSsid.SsidLength = pAd->MlmeAux.AutoReconnectSsidLen;
+		NdisMoveMemory(OidSsid.Ssid, pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.AutoReconnectSsidLen);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("Driver auto reconnect to last OID_802_11_SSID setting - %s, len - %d\n", pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.AutoReconnectSsidLen));
+		MlmeEnqueue(pAd,
+					MLME_CNTL_STATE_MACHINE,
+					OID_802_11_SSID,
+					sizeof(NDIS_802_11_SSID),
+					&OidSsid);
+		RT28XX_MLME_HANDLER(pAd);
+	}
+}
+#endif // CONFIG_STA_SUPPORT //
+
+/*
+	==========================================================================
+	Validate SSID for connection try and rescan purpose
+	Valid SSID will have visible chars only.
+	The valid length is from 0 to 32.
+	IRQL = DISPATCH_LEVEL
+	==========================================================================
+ */
+BOOLEAN MlmeValidateSSID(
+	IN PUCHAR	pSsid,
+	IN UCHAR	SsidLen)
+{
+	int	index;
+
+	if (SsidLen > MAX_LEN_OF_SSID)
+		return (FALSE);
+
+	// Check each character value
+	for (index = 0; index < SsidLen; index++)
+	{
+		if (pSsid[index] < 0x20)
+			return (FALSE);
+	}
+
+	// All checked
+	return (TRUE);
+}
+
+VOID MlmeSelectTxRateTable(
+	IN PRTMP_ADAPTER		pAd,
+	IN PMAC_TABLE_ENTRY		pEntry,
+	IN PUCHAR				*ppTable,
+	IN PUCHAR				pTableSize,
+	IN PUCHAR				pInitTxRateIdx)
+{
+	do
+	{
+		// decide the rate table for tuning
+		if (pAd->CommonCfg.TxRateTableSize > 0)
+		{
+			*ppTable = RateSwitchTable;
+			*pTableSize = RateSwitchTable[0];
+			*pInitTxRateIdx = RateSwitchTable[1];
+
+			break;
+		}
+
+#ifdef CONFIG_STA_SUPPORT
+		if ((pAd->OpMode == OPMODE_STA) && ADHOC_ON(pAd))
+		{
+#ifdef DOT11_N_SUPPORT
+			if ((pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED) &&
+				!pAd->StaCfg.AdhocBOnlyJoined &&
+				!pAd->StaCfg.AdhocBGJoined &&
+				(pAd->StaActive.SupportedPhyInfo.MCSSet[0] == 0xff) &&
+				((pAd->StaActive.SupportedPhyInfo.MCSSet[1] == 0x00) || (pAd->Antenna.field.TxPath == 1)))
+			{// 11N 1S Adhoc
+				*ppTable = RateSwitchTable11N1S;
+				*pTableSize = RateSwitchTable11N1S[0];
+				*pInitTxRateIdx = RateSwitchTable11N1S[1];
+
+			}
+			else if ((pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED) &&
+					!pAd->StaCfg.AdhocBOnlyJoined &&
+					!pAd->StaCfg.AdhocBGJoined &&
+					(pAd->StaActive.SupportedPhyInfo.MCSSet[0] == 0xff) &&
+					(pAd->StaActive.SupportedPhyInfo.MCSSet[1] == 0xff) &&
+					(pAd->Antenna.field.TxPath == 2))
+			{// 11N 2S Adhoc
+				if (pAd->LatchRfRegs.Channel <= 14)
+				{
+					*ppTable = RateSwitchTable11N2S;
+					*pTableSize = RateSwitchTable11N2S[0];
+					*pInitTxRateIdx = RateSwitchTable11N2S[1];
+				}
+				else
+				{
+					*ppTable = RateSwitchTable11N2SForABand;
+					*pTableSize = RateSwitchTable11N2SForABand[0];
+					*pInitTxRateIdx = RateSwitchTable11N2SForABand[1];
+				}
+
+			}
+			else
+#endif // DOT11_N_SUPPORT //
+				if (pAd->CommonCfg.PhyMode == PHY_11B)
+			{
+				*ppTable = RateSwitchTable11B;
+				*pTableSize = RateSwitchTable11B[0];
+				*pInitTxRateIdx = RateSwitchTable11B[1];
+
+			}
+	        else if((pAd->LatchRfRegs.Channel <= 14) && (pAd->StaCfg.AdhocBOnlyJoined == TRUE))
+			{
+				// USe B Table when Only b-only Station in my IBSS .
+				*ppTable = RateSwitchTable11B;
+				*pTableSize = RateSwitchTable11B[0];
+				*pInitTxRateIdx = RateSwitchTable11B[1];
+
+			}
+			else if (pAd->LatchRfRegs.Channel <= 14)
+			{
+				*ppTable = RateSwitchTable11BG;
+				*pTableSize = RateSwitchTable11BG[0];
+				*pInitTxRateIdx = RateSwitchTable11BG[1];
+
+			}
+			else
+			{
+				*ppTable = RateSwitchTable11G;
+				*pTableSize = RateSwitchTable11G[0];
+				*pInitTxRateIdx = RateSwitchTable11G[1];
+
+			}
+			break;
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef DOT11_N_SUPPORT
+		if ((pEntry->RateLen == 12) && (pEntry->HTCapability.MCSSet[0] == 0xff) &&
+			((pEntry->HTCapability.MCSSet[1] == 0x00) || (pAd->CommonCfg.TxStream == 1)))
+		{// 11BGN 1S AP
+			*ppTable = RateSwitchTable11BGN1S;
+			*pTableSize = RateSwitchTable11BGN1S[0];
+			*pInitTxRateIdx = RateSwitchTable11BGN1S[1];
+
+			break;
+		}
+
+		if ((pEntry->RateLen == 12) && (pEntry->HTCapability.MCSSet[0] == 0xff) &&
+			(pEntry->HTCapability.MCSSet[1] == 0xff) && (pAd->CommonCfg.TxStream == 2))
+		{// 11BGN 2S AP
+			if (pAd->LatchRfRegs.Channel <= 14)
+			{
+				*ppTable = RateSwitchTable11BGN2S;
+				*pTableSize = RateSwitchTable11BGN2S[0];
+				*pInitTxRateIdx = RateSwitchTable11BGN2S[1];
+
+			}
+			else
+			{
+				*ppTable = RateSwitchTable11BGN2SForABand;
+				*pTableSize = RateSwitchTable11BGN2SForABand[0];
+				*pInitTxRateIdx = RateSwitchTable11BGN2SForABand[1];
+
+			}
+			break;
+		}
+
+		if ((pEntry->HTCapability.MCSSet[0] == 0xff) && ((pEntry->HTCapability.MCSSet[1] == 0x00) || (pAd->CommonCfg.TxStream == 1)))
+		{// 11N 1S AP
+			*ppTable = RateSwitchTable11N1S;
+			*pTableSize = RateSwitchTable11N1S[0];
+			*pInitTxRateIdx = RateSwitchTable11N1S[1];
+
+			break;
+		}
+
+		if ((pEntry->HTCapability.MCSSet[0] == 0xff) && (pEntry->HTCapability.MCSSet[1] == 0xff) && (pAd->CommonCfg.TxStream == 2))
+		{// 11N 2S AP
+			if (pAd->LatchRfRegs.Channel <= 14)
+			{
+			*ppTable = RateSwitchTable11N2S;
+			*pTableSize = RateSwitchTable11N2S[0];
+			*pInitTxRateIdx = RateSwitchTable11N2S[1];
+            }
+			else
+			{
+				*ppTable = RateSwitchTable11N2SForABand;
+				*pTableSize = RateSwitchTable11N2SForABand[0];
+				*pInitTxRateIdx = RateSwitchTable11N2SForABand[1];
+			}
+
+			break;
+		}
+#endif // DOT11_N_SUPPORT //
+		//else if ((pAd->StaActive.SupRateLen == 4) && (pAd->StaActive.ExtRateLen == 0) && (pAd->StaActive.SupportedPhyInfo.MCSSet[0] == 0) && (pAd->StaActive.SupportedPhyInfo.MCSSet[1] == 0))
+		if ((pEntry->RateLen == 4)
+#ifdef DOT11_N_SUPPORT
+			&& (pEntry->HTCapability.MCSSet[0] == 0) && (pEntry->HTCapability.MCSSet[1] == 0)
+#endif // DOT11_N_SUPPORT //
+			)
+		{// B only AP
+			*ppTable = RateSwitchTable11B;
+			*pTableSize = RateSwitchTable11B[0];
+			*pInitTxRateIdx = RateSwitchTable11B[1];
+
+			break;
+		}
+
+		//else if ((pAd->StaActive.SupRateLen + pAd->StaActive.ExtRateLen > 8) && (pAd->StaActive.SupportedPhyInfo.MCSSet[0] == 0) && (pAd->StaActive.SupportedPhyInfo.MCSSet[1] == 0))
+		if ((pEntry->RateLen > 8)
+#ifdef DOT11_N_SUPPORT
+			&& (pEntry->HTCapability.MCSSet[0] == 0) && (pEntry->HTCapability.MCSSet[1] == 0)
+#endif // DOT11_N_SUPPORT //
+			)
+		{// B/G  mixed AP
+			*ppTable = RateSwitchTable11BG;
+			*pTableSize = RateSwitchTable11BG[0];
+			*pInitTxRateIdx = RateSwitchTable11BG[1];
+
+			break;
+		}
+
+		//else if ((pAd->StaActive.SupRateLen + pAd->StaActive.ExtRateLen == 8) && (pAd->StaActive.SupportedPhyInfo.MCSSet[0] == 0) && (pAd->StaActive.SupportedPhyInfo.MCSSet[1] == 0))
+		if ((pEntry->RateLen == 8)
+#ifdef DOT11_N_SUPPORT
+			&& (pEntry->HTCapability.MCSSet[0] == 0) && (pEntry->HTCapability.MCSSet[1] == 0)
+#endif // DOT11_N_SUPPORT //
+			)
+		{// G only AP
+			*ppTable = RateSwitchTable11G;
+			*pTableSize = RateSwitchTable11G[0];
+			*pInitTxRateIdx = RateSwitchTable11G[1];
+
+			break;
+		}
+#ifdef DOT11_N_SUPPORT
+#endif // DOT11_N_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+#ifdef DOT11_N_SUPPORT
+			//else if ((pAd->StaActive.SupportedPhyInfo.MCSSet[0] == 0) && (pAd->StaActive.SupportedPhyInfo.MCSSet[1] == 0))
+			if ((pEntry->HTCapability.MCSSet[0] == 0) && (pEntry->HTCapability.MCSSet[1] == 0))
+#endif // DOT11_N_SUPPORT //
+			{	// Legacy mode
+				if (pAd->CommonCfg.MaxTxRate <= RATE_11)
+				{
+					*ppTable = RateSwitchTable11B;
+					*pTableSize = RateSwitchTable11B[0];
+					*pInitTxRateIdx = RateSwitchTable11B[1];
+				}
+				else if ((pAd->CommonCfg.MaxTxRate > RATE_11) && (pAd->CommonCfg.MinTxRate > RATE_11))
+				{
+					*ppTable = RateSwitchTable11G;
+					*pTableSize = RateSwitchTable11G[0];
+					*pInitTxRateIdx = RateSwitchTable11G[1];
+
+				}
+				else
+				{
+					*ppTable = RateSwitchTable11BG;
+					*pTableSize = RateSwitchTable11BG[0];
+					*pInitTxRateIdx = RateSwitchTable11BG[1];
+				}
+				break;
+			}
+#ifdef DOT11_N_SUPPORT
+			if (pAd->LatchRfRegs.Channel <= 14)
+			{
+				if (pAd->CommonCfg.TxStream == 1)
+				{
+					*ppTable = RateSwitchTable11N1S;
+					*pTableSize = RateSwitchTable11N1S[0];
+					*pInitTxRateIdx = RateSwitchTable11N1S[1];
+					DBGPRINT_RAW(RT_DEBUG_ERROR,("DRS: unkown mode,default use 11N 1S AP \n"));
+				}
+				else
+				{
+					*ppTable = RateSwitchTable11N2S;
+					*pTableSize = RateSwitchTable11N2S[0];
+					*pInitTxRateIdx = RateSwitchTable11N2S[1];
+					DBGPRINT_RAW(RT_DEBUG_ERROR,("DRS: unkown mode,default use 11N 2S AP \n"));
+				}
+			}
+			else
+			{
+				if (pAd->CommonCfg.TxStream == 1)
+				{
+					*ppTable = RateSwitchTable11N1S;
+					*pTableSize = RateSwitchTable11N1S[0];
+					*pInitTxRateIdx = RateSwitchTable11N1S[1];
+					DBGPRINT_RAW(RT_DEBUG_ERROR,("DRS: unkown mode,default use 11N 1S AP \n"));
+				}
+				else
+				{
+					*ppTable = RateSwitchTable11N2SForABand;
+					*pTableSize = RateSwitchTable11N2SForABand[0];
+					*pInitTxRateIdx = RateSwitchTable11N2SForABand[1];
+					DBGPRINT_RAW(RT_DEBUG_ERROR,("DRS: unkown mode,default use 11N 2S AP \n"));
+				}
+			}
+#endif // DOT11_N_SUPPORT //
+			DBGPRINT_RAW(RT_DEBUG_ERROR,("DRS: unkown mode (SupRateLen=%d, ExtRateLen=%d, MCSSet[0]=0x%x, MCSSet[1]=0x%x)\n",
+				pAd->StaActive.SupRateLen, pAd->StaActive.ExtRateLen, pAd->StaActive.SupportedPhyInfo.MCSSet[0], pAd->StaActive.SupportedPhyInfo.MCSSet[1]));
+		}
+#endif // CONFIG_STA_SUPPORT //
+	} while(FALSE);
+}
+
+#ifdef CONFIG_STA_SUPPORT
+/*
+	==========================================================================
+	Description:
+		This routine checks if there're other APs out there capable for
+		roaming. Caller should call this routine only when Link up in INFRA mode
+		and channel quality is below CQI_GOOD_THRESHOLD.
+
+	IRQL = DISPATCH_LEVEL
+
+	Output:
+	==========================================================================
+ */
+VOID MlmeCheckForRoaming(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG	Now32)
+{
+	USHORT	   i;
+	BSS_TABLE  *pRoamTab = &pAd->MlmeAux.RoamTab;
+	BSS_ENTRY  *pBss;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("==> MlmeCheckForRoaming\n"));
+	// put all roaming candidates into RoamTab, and sort in RSSI order
+	BssTableInit(pRoamTab);
+	for (i = 0; i < pAd->ScanTab.BssNr; i++)
+	{
+		pBss = &pAd->ScanTab.BssEntry[i];
+
+		if ((pBss->LastBeaconRxTime + BEACON_LOST_TIME) < Now32)
+			continue;	 // AP disappear
+		if (pBss->Rssi <= RSSI_THRESHOLD_FOR_ROAMING)
+			continue;	 // RSSI too weak. forget it.
+		if (MAC_ADDR_EQUAL(pBss->Bssid, pAd->CommonCfg.Bssid))
+			continue;	 // skip current AP
+		if (pBss->Rssi < (pAd->StaCfg.RssiSample.LastRssi0 + RSSI_DELTA))
+			continue;	 // only AP with stronger RSSI is eligible for roaming
+
+		// AP passing all above rules is put into roaming candidate table
+		NdisMoveMemory(&pRoamTab->BssEntry[pRoamTab->BssNr], pBss, sizeof(BSS_ENTRY));
+		pRoamTab->BssNr += 1;
+	}
+
+	if (pRoamTab->BssNr > 0)
+	{
+		// check CntlMachine.CurrState to avoid collision with NDIS SetOID request
+		if (pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE)
+		{
+			pAd->RalinkCounters.PoorCQIRoamingCount ++;
+			DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - Roaming attempt #%ld\n", pAd->RalinkCounters.PoorCQIRoamingCount));
+			MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_MLME_ROAMING_REQ, 0, NULL);
+			RT28XX_MLME_HANDLER(pAd);
+		}
+	}
+	DBGPRINT(RT_DEBUG_TRACE, ("<== MlmeCheckForRoaming(# of candidate= %d)\n",pRoamTab->BssNr));
+}
+
+/*
+	==========================================================================
+	Description:
+		This routine checks if there're other APs out there capable for
+		roaming. Caller should call this routine only when link up in INFRA mode
+		and channel quality is below CQI_GOOD_THRESHOLD.
+
+	IRQL = DISPATCH_LEVEL
+
+	Output:
+	==========================================================================
+ */
+VOID MlmeCheckForFastRoaming(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	ULONG			Now)
+{
+	USHORT		i;
+	BSS_TABLE	*pRoamTab = &pAd->MlmeAux.RoamTab;
+	BSS_ENTRY	*pBss;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("==> MlmeCheckForFastRoaming\n"));
+	// put all roaming candidates into RoamTab, and sort in RSSI order
+	BssTableInit(pRoamTab);
+	for (i = 0; i < pAd->ScanTab.BssNr; i++)
+	{
+		pBss = &pAd->ScanTab.BssEntry[i];
+
+        if ((pBss->Rssi <= -50) && (pBss->Channel == pAd->CommonCfg.Channel))
+			continue;	 // RSSI too weak. forget it.
+		if (MAC_ADDR_EQUAL(pBss->Bssid, pAd->CommonCfg.Bssid))
+			continue;	 // skip current AP
+		if (!SSID_EQUAL(pBss->Ssid, pBss->SsidLen, pAd->CommonCfg.Ssid, pAd->CommonCfg.SsidLen))
+			continue;	 // skip different SSID
+        if (pBss->Rssi < (RTMPMaxRssi(pAd, pAd->StaCfg.RssiSample.LastRssi0, pAd->StaCfg.RssiSample.LastRssi1, pAd->StaCfg.RssiSample.LastRssi2) + RSSI_DELTA))
+			continue;	 // skip AP without better RSSI
+
+        DBGPRINT(RT_DEBUG_TRACE, ("LastRssi0 = %d, pBss->Rssi = %d\n", RTMPMaxRssi(pAd, pAd->StaCfg.RssiSample.LastRssi0, pAd->StaCfg.RssiSample.LastRssi1, pAd->StaCfg.RssiSample.LastRssi2), pBss->Rssi));
+		// AP passing all above rules is put into roaming candidate table
+		NdisMoveMemory(&pRoamTab->BssEntry[pRoamTab->BssNr], pBss, sizeof(BSS_ENTRY));
+		pRoamTab->BssNr += 1;
+	}
+
+	if (pRoamTab->BssNr > 0)
+	{
+		// check CntlMachine.CurrState to avoid collision with NDIS SetOID request
+		if (pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE)
+		{
+			pAd->RalinkCounters.PoorCQIRoamingCount ++;
+			DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - Roaming attempt #%ld\n", pAd->RalinkCounters.PoorCQIRoamingCount));
+			MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_MLME_ROAMING_REQ, 0, NULL);
+			RT28XX_MLME_HANDLER(pAd);
+		}
+	}
+	// Maybe site survey required
+	else
+	{
+		if ((pAd->StaCfg.LastScanTime + 10 * 1000) < Now)
+		{
+			// check CntlMachine.CurrState to avoid collision with NDIS SetOID request
+			DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - Roaming, No eligable entry, try new scan!\n"));
+			pAd->StaCfg.ScanCnt = 2;
+			pAd->StaCfg.LastScanTime = Now;
+			MlmeAutoScan(pAd);
+		}
+	}
+
+    DBGPRINT(RT_DEBUG_TRACE, ("<== MlmeCheckForFastRoaming (BssNr=%d)\n", pRoamTab->BssNr));
+}
+
+/*
+	==========================================================================
+	Description:
+		This routine calculates TxPER, RxPER of the past N-sec period. And
+		according to the calculation result, ChannelQuality is calculated here
+		to decide if current AP is still doing the job.
+
+		If ChannelQuality is not good, a ROAMing attempt may be tried later.
+	Output:
+		StaCfg.ChannelQuality - 0..100
+
+	IRQL = DISPATCH_LEVEL
+
+	NOTE: This routine decide channle quality based on RX CRC error ratio.
+		Caller should make sure a function call to NICUpdateRawCounters(pAd)
+		is performed right before this routine, so that this routine can decide
+		channel quality based on the most up-to-date information
+	==========================================================================
+ */
+VOID MlmeCalculateChannelQuality(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG Now32)
+{
+	ULONG TxOkCnt, TxCnt, TxPER, TxPRR;
+	ULONG RxCnt, RxPER;
+	UCHAR NorRssi;
+	CHAR  MaxRssi;
+	ULONG BeaconLostTime = BEACON_LOST_TIME;
+
+#ifdef CARRIER_DETECTION_SUPPORT // Roger sync Carrier
+	// longer beacon lost time when carrier detection enabled
+	if (pAd->CommonCfg.CarrierDetect.Enable == TRUE)
+	{
+		BeaconLostTime = BEACON_LOST_TIME + BEACON_LOST_TIME/2;
+	}
+#endif // CARRIER_DETECTION_SUPPORT //
+
+	MaxRssi = RTMPMaxRssi(pAd, pAd->StaCfg.RssiSample.LastRssi0, pAd->StaCfg.RssiSample.LastRssi1, pAd->StaCfg.RssiSample.LastRssi2);
+
+	//
+	// calculate TX packet error ratio and TX retry ratio - if too few TX samples, skip TX related statistics
+	//
+	TxOkCnt = pAd->RalinkCounters.OneSecTxNoRetryOkCount + pAd->RalinkCounters.OneSecTxRetryOkCount;
+	TxCnt = TxOkCnt + pAd->RalinkCounters.OneSecTxFailCount;
+	if (TxCnt < 5)
+	{
+		TxPER = 0;
+		TxPRR = 0;
+	}
+	else
+	{
+		TxPER = (pAd->RalinkCounters.OneSecTxFailCount * 100) / TxCnt;
+		TxPRR = ((TxCnt - pAd->RalinkCounters.OneSecTxNoRetryOkCount) * 100) / TxCnt;
+	}
+
+	//
+	// calculate RX PER - don't take RxPER into consideration if too few sample
+	//
+	RxCnt = pAd->RalinkCounters.OneSecRxOkCnt + pAd->RalinkCounters.OneSecRxFcsErrCnt;
+	if (RxCnt < 5)
+		RxPER = 0;
+	else
+		RxPER = (pAd->RalinkCounters.OneSecRxFcsErrCnt * 100) / RxCnt;
+
+	//
+	// decide ChannelQuality based on: 1)last BEACON received time, 2)last RSSI, 3)TxPER, and 4)RxPER
+	//
+	if (INFRA_ON(pAd) &&
+		(pAd->RalinkCounters.OneSecTxNoRetryOkCount < 2) && // no heavy traffic
+		(pAd->StaCfg.LastBeaconRxTime + BeaconLostTime < Now32))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("BEACON lost > %ld msec with TxOkCnt=%ld -> CQI=0\n", BeaconLostTime, TxOkCnt));
+		pAd->Mlme.ChannelQuality = 0;
+	}
+	else
+	{
+		// Normalize Rssi
+		if (MaxRssi > -40)
+			NorRssi = 100;
+		else if (MaxRssi < -90)
+			NorRssi = 0;
+		else
+			NorRssi = (MaxRssi + 90) * 2;
+
+		// ChannelQuality = W1*RSSI + W2*TxPRR + W3*RxPER	 (RSSI 0..100), (TxPER 100..0), (RxPER 100..0)
+		pAd->Mlme.ChannelQuality = (RSSI_WEIGHTING * NorRssi +
+								   TX_WEIGHTING * (100 - TxPRR) +
+								   RX_WEIGHTING* (100 - RxPER)) / 100;
+		if (pAd->Mlme.ChannelQuality >= 100)
+			pAd->Mlme.ChannelQuality = 100;
+	}
+
+}
+
+VOID MlmeSetTxRate(
+	IN PRTMP_ADAPTER		pAd,
+	IN PMAC_TABLE_ENTRY		pEntry,
+	IN PRTMP_TX_RATE_SWITCH	pTxRate)
+{
+	UCHAR	MaxMode = MODE_OFDM;
+
+#ifdef DOT11_N_SUPPORT
+	MaxMode = MODE_HTGREENFIELD;
+
+	if (pTxRate->STBC && (pAd->StaCfg.MaxHTPhyMode.field.STBC) && (pAd->Antenna.field.TxPath == 2))
+		pAd->StaCfg.HTPhyMode.field.STBC = STBC_USE;
+	else
+#endif // DOT11_N_SUPPORT //
+		pAd->StaCfg.HTPhyMode.field.STBC = STBC_NONE;
+
+	if (pTxRate->CurrMCS < MCS_AUTO)
+		pAd->StaCfg.HTPhyMode.field.MCS = pTxRate->CurrMCS;
+
+	if (pAd->StaCfg.HTPhyMode.field.MCS > 7)
+		pAd->StaCfg.HTPhyMode.field.STBC = STBC_NONE;
+
+   	if (ADHOC_ON(pAd))
+	{
+		// If peer adhoc is b-only mode, we can't send 11g rate.
+		pAd->StaCfg.HTPhyMode.field.ShortGI = GI_800;
+		pEntry->HTPhyMode.field.STBC	= STBC_NONE;
+
+		//
+		// For Adhoc MODE_CCK, driver will use AdhocBOnlyJoined flag to roll back to B only if necessary
+		//
+		pEntry->HTPhyMode.field.MODE	= pTxRate->Mode;
+		pEntry->HTPhyMode.field.ShortGI	= pAd->StaCfg.HTPhyMode.field.ShortGI;
+		pEntry->HTPhyMode.field.MCS		= pAd->StaCfg.HTPhyMode.field.MCS;
+
+		// Patch speed error in status page
+		pAd->StaCfg.HTPhyMode.field.MODE = pEntry->HTPhyMode.field.MODE;
+	}
+	else
+	{
+		if (pTxRate->Mode <= MaxMode)
+			pAd->StaCfg.HTPhyMode.field.MODE = pTxRate->Mode;
+
+#ifdef DOT11_N_SUPPORT
+		if (pTxRate->ShortGI && (pAd->StaCfg.MaxHTPhyMode.field.ShortGI))
+			pAd->StaCfg.HTPhyMode.field.ShortGI = GI_400;
+		else
+#endif // DOT11_N_SUPPORT //
+			pAd->StaCfg.HTPhyMode.field.ShortGI = GI_800;
+
+#ifdef DOT11_N_SUPPORT
+		// Reexam each bandwidth's SGI support.
+		if (pAd->StaCfg.HTPhyMode.field.ShortGI == GI_400)
+		{
+			if ((pEntry->HTPhyMode.field.BW == BW_20) && (!CLIENT_STATUS_TEST_FLAG(pEntry, fCLIENT_STATUS_SGI20_CAPABLE)))
+				pAd->StaCfg.HTPhyMode.field.ShortGI = GI_800;
+			if ((pEntry->HTPhyMode.field.BW == BW_40) && (!CLIENT_STATUS_TEST_FLAG(pEntry, fCLIENT_STATUS_SGI40_CAPABLE)))
+				pAd->StaCfg.HTPhyMode.field.ShortGI = GI_800;
+		}
+
+		// Turn RTS/CTS rate to 6Mbps.
+		if ((pEntry->HTPhyMode.field.MCS == 0) && (pAd->StaCfg.HTPhyMode.field.MCS != 0))
+		{
+			pEntry->HTPhyMode.field.MCS		= pAd->StaCfg.HTPhyMode.field.MCS;
+			if (pAd->MacTab.fAnyBASession)
+			{
+				AsicUpdateProtect(pAd, HT_FORCERTSCTS, ALLN_SETPROTECT, TRUE, (BOOLEAN)pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent);
+			}
+			else
+			{
+				AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode, ALLN_SETPROTECT, TRUE, (BOOLEAN)pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent);
+			}
+		}
+		else if ((pEntry->HTPhyMode.field.MCS == 8) && (pAd->StaCfg.HTPhyMode.field.MCS != 8))
+		{
+			pEntry->HTPhyMode.field.MCS		= pAd->StaCfg.HTPhyMode.field.MCS;
+			if (pAd->MacTab.fAnyBASession)
+			{
+				AsicUpdateProtect(pAd, HT_FORCERTSCTS, ALLN_SETPROTECT, TRUE, (BOOLEAN)pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent);
+			}
+			else
+			{
+				AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode, ALLN_SETPROTECT, TRUE, (BOOLEAN)pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent);
+			}
+		}
+		else if ((pEntry->HTPhyMode.field.MCS != 0) && (pAd->StaCfg.HTPhyMode.field.MCS == 0))
+		{
+			AsicUpdateProtect(pAd, HT_RTSCTS_6M, ALLN_SETPROTECT, TRUE, (BOOLEAN)pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent);
+
+		}
+		else if ((pEntry->HTPhyMode.field.MCS != 8) && (pAd->StaCfg.HTPhyMode.field.MCS == 8))
+		{
+			AsicUpdateProtect(pAd, HT_RTSCTS_6M, ALLN_SETPROTECT, TRUE, (BOOLEAN)pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent);
+		}
+#endif // DOT11_N_SUPPORT //
+
+		pEntry->HTPhyMode.field.STBC	= pAd->StaCfg.HTPhyMode.field.STBC;
+		pEntry->HTPhyMode.field.ShortGI	= pAd->StaCfg.HTPhyMode.field.ShortGI;
+		pEntry->HTPhyMode.field.MCS		= pAd->StaCfg.HTPhyMode.field.MCS;
+		pEntry->HTPhyMode.field.MODE	= pAd->StaCfg.HTPhyMode.field.MODE;
+#ifdef DOT11_N_SUPPORT
+		if ((pAd->StaCfg.MaxHTPhyMode.field.MODE == MODE_HTGREENFIELD) &&
+		    pAd->WIFItestbed.bGreenField)
+		    pEntry->HTPhyMode.field.MODE = MODE_HTGREENFIELD;
+#endif // DOT11_N_SUPPORT //
+	}
+
+	pAd->LastTxRate = (USHORT)(pEntry->HTPhyMode.word);
+}
+
+/*
+	==========================================================================
+	Description:
+		This routine calculates the acumulated TxPER of eaxh TxRate. And
+		according to the calculation result, change CommonCfg.TxRate which
+		is the stable TX Rate we expect the Radio situation could sustained.
+
+		CommonCfg.TxRate will change dynamically within {RATE_1/RATE_6, MaxTxRate}
+	Output:
+		CommonCfg.TxRate -
+
+	IRQL = DISPATCH_LEVEL
+
+	NOTE:
+		call this routine every second
+	==========================================================================
+ */
+VOID MlmeDynamicTxRateSwitching(
+	IN PRTMP_ADAPTER pAd)
+{
+	UCHAR					UpRateIdx = 0, DownRateIdx = 0, CurrRateIdx;
+	ULONG					i, AccuTxTotalCnt = 0, TxTotalCnt;
+	ULONG					TxErrorRatio = 0;
+	BOOLEAN					bTxRateChanged, bUpgradeQuality = FALSE;
+	PRTMP_TX_RATE_SWITCH	pCurrTxRate, pNextTxRate = NULL;
+	PUCHAR					pTable;
+	UCHAR					TableSize = 0;
+	UCHAR					InitTxRateIdx = 0, TrainUp, TrainDown;
+	CHAR					Rssi, RssiOffset = 0;
+	TX_STA_CNT1_STRUC		StaTx1;
+	TX_STA_CNT0_STRUC		TxStaCnt0;
+	ULONG					TxRetransmit = 0, TxSuccess = 0, TxFailCount = 0;
+	MAC_TABLE_ENTRY			*pEntry;
+
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+	{
+		return;
+	}
+#endif // RALINK_ATE //
+
+	/*if (pAd->Antenna.field.RxPath > 1)
+		Rssi = (pAd->StaCfg.RssiSample.AvgRssi0 + pAd->StaCfg.RssiSample.AvgRssi1) >> 1;
+	else
+		Rssi = pAd->StaCfg.RssiSample.AvgRssi0;*/
+
+	//
+	// walk through MAC table, see if need to change AP's TX rate toward each entry
+	//
+   	for (i = 1; i < MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		pEntry = &pAd->MacTab.Content[i];
+
+		// check if this entry need to switch rate automatically
+		if (RTMPCheckEntryEnableAutoRateSwitch(pAd, pEntry) == FALSE)
+			continue;
+
+		if ((pAd->MacTab.Size == 1) || (pEntry->ValidAsDls))
+		{
+			Rssi = RTMPMaxRssi(pAd, (CHAR)pAd->StaCfg.RssiSample.AvgRssi0, (CHAR)pAd->StaCfg.RssiSample.AvgRssi1, (CHAR)pAd->StaCfg.RssiSample.AvgRssi2);
+
+			// Update statistic counter
+			RTMP_IO_READ32(pAd, TX_STA_CNT0, &TxStaCnt0.word);
+			RTMP_IO_READ32(pAd, TX_STA_CNT1, &StaTx1.word);
+			pAd->bUpdateBcnCntDone = TRUE;
+			TxRetransmit = StaTx1.field.TxRetransmit;
+			TxSuccess = StaTx1.field.TxSuccess;
+			TxFailCount = TxStaCnt0.field.TxFailCount;
+			TxTotalCnt = TxRetransmit + TxSuccess + TxFailCount;
+
+			pAd->RalinkCounters.OneSecTxRetryOkCount += StaTx1.field.TxRetransmit;
+			pAd->RalinkCounters.OneSecTxNoRetryOkCount += StaTx1.field.TxSuccess;
+			pAd->RalinkCounters.OneSecTxFailCount += TxStaCnt0.field.TxFailCount;
+			pAd->WlanCounters.TransmittedFragmentCount.u.LowPart += StaTx1.field.TxSuccess;
+			pAd->WlanCounters.RetryCount.u.LowPart += StaTx1.field.TxRetransmit;
+			pAd->WlanCounters.FailedCount.u.LowPart += TxStaCnt0.field.TxFailCount;
+
+			// if no traffic in the past 1-sec period, don't change TX rate,
+			// but clear all bad history. because the bad history may affect the next
+			// Chariot throughput test
+			AccuTxTotalCnt = pAd->RalinkCounters.OneSecTxNoRetryOkCount +
+						 pAd->RalinkCounters.OneSecTxRetryOkCount +
+						 pAd->RalinkCounters.OneSecTxFailCount;
+
+			if (TxTotalCnt)
+				TxErrorRatio = ((TxRetransmit + TxFailCount) * 100) / TxTotalCnt;
+		}
+		else
+		{
+			Rssi = RTMPMaxRssi(pAd, (CHAR)pEntry->RssiSample.AvgRssi0, (CHAR)pEntry->RssiSample.AvgRssi1, (CHAR)pEntry->RssiSample.AvgRssi2);
+
+			TxTotalCnt = pEntry->OneSecTxNoRetryOkCount +
+				 pEntry->OneSecTxRetryOkCount +
+				 pEntry->OneSecTxFailCount;
+
+			if (TxTotalCnt)
+				TxErrorRatio = ((pEntry->OneSecTxRetryOkCount + pEntry->OneSecTxFailCount) * 100) / TxTotalCnt;
+		}
+
+		CurrRateIdx = pEntry->CurrTxRateIndex;
+
+		MlmeSelectTxRateTable(pAd, pEntry, &pTable, &TableSize, &InitTxRateIdx);
+
+		if (CurrRateIdx >= TableSize)
+		{
+			CurrRateIdx = TableSize - 1;
+		}
+
+		// When switch from Fixed rate -> auto rate, the REAL TX rate might be different from pAd->CommonCfg.TxRateIndex.
+		// So need to sync here.
+		pCurrTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(CurrRateIdx+1)*5];
+		if ((pEntry->HTPhyMode.field.MCS != pCurrTxRate->CurrMCS)
+			//&& (pAd->StaCfg.bAutoTxRateSwitch == TRUE)
+			)
+		{
+
+			// Need to sync Real Tx rate and our record.
+			// Then return for next DRS.
+			pCurrTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(InitTxRateIdx+1)*5];
+			pEntry->CurrTxRateIndex = InitTxRateIdx;
+			MlmeSetTxRate(pAd, pEntry, pCurrTxRate);
+
+			// reset all OneSecTx counters
+			RESET_ONE_SEC_TX_CNT(pEntry);
+			continue;
+		}
+
+		// decide the next upgrade rate and downgrade rate, if any
+		if ((CurrRateIdx > 0) && (CurrRateIdx < (TableSize - 1)))
+		{
+			UpRateIdx = CurrRateIdx + 1;
+			DownRateIdx = CurrRateIdx -1;
+		}
+		else if (CurrRateIdx == 0)
+		{
+			UpRateIdx = CurrRateIdx + 1;
+			DownRateIdx = CurrRateIdx;
+		}
+		else if (CurrRateIdx == (TableSize - 1))
+		{
+			UpRateIdx = CurrRateIdx;
+			DownRateIdx = CurrRateIdx - 1;
+		}
+
+		pCurrTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(CurrRateIdx+1)*5];
+
+#ifdef DOT11_N_SUPPORT
+		if ((Rssi > -65) && (pCurrTxRate->Mode >= MODE_HTMIX))
+		{
+			TrainUp		= (pCurrTxRate->TrainUp + (pCurrTxRate->TrainUp >> 1));
+			TrainDown	= (pCurrTxRate->TrainDown + (pCurrTxRate->TrainDown >> 1));
+		}
+		else
+#endif // DOT11_N_SUPPORT //
+		{
+			TrainUp		= pCurrTxRate->TrainUp;
+			TrainDown	= pCurrTxRate->TrainDown;
+		}
+
+		//pAd->DrsCounters.LastTimeTxRateChangeAction = pAd->DrsCounters.LastSecTxRateChangeAction;
+
+		//
+		// Keep the last time TxRateChangeAction status.
+		//
+		pEntry->LastTimeTxRateChangeAction = pEntry->LastSecTxRateChangeAction;
+
+
+
+		//
+		// CASE 1. when TX samples are fewer than 15, then decide TX rate solely on RSSI
+		//         (criteria copied from RT2500 for Netopia case)
+		//
+		if (TxTotalCnt <= 15)
+		{
+			CHAR	idx = 0;
+			UCHAR	TxRateIdx;
+			//UCHAR	MCS0 = 0, MCS1 = 0, MCS2 = 0, MCS3 = 0, MCS4 = 0, MCS7 = 0, MCS12 = 0, MCS13 = 0, MCS14 = 0, MCS15 = 0;
+			UCHAR	MCS0 = 0, MCS1 = 0, MCS2 = 0, MCS3 = 0, MCS4 = 0,  MCS5 =0, MCS6 = 0, MCS7 = 0;
+	        UCHAR	MCS12 = 0, MCS13 = 0, MCS14 = 0, MCS15 = 0;
+			UCHAR	MCS20 = 0, MCS21 = 0, MCS22 = 0, MCS23 = 0; // 3*3
+
+			// check the existence and index of each needed MCS
+			while (idx < pTable[0])
+			{
+				pCurrTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(idx+1)*5];
+
+				if (pCurrTxRate->CurrMCS == MCS_0)
+				{
+					MCS0 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_1)
+				{
+					MCS1 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_2)
+				{
+					MCS2 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_3)
+				{
+					MCS3 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_4)
+				{
+					MCS4 = idx;
+				}
+	            else if (pCurrTxRate->CurrMCS == MCS_5)
+	            {
+	                MCS5 = idx;
+	            }
+	            else if (pCurrTxRate->CurrMCS == MCS_6)
+	            {
+	                MCS6 = idx;
+	            }
+				//else if (pCurrTxRate->CurrMCS == MCS_7)
+				else if ((pCurrTxRate->CurrMCS == MCS_7) && (pCurrTxRate->ShortGI == GI_800))	// prevent the highest MCS using short GI when 1T and low throughput
+				{
+					MCS7 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_12)
+				{
+					MCS12 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_13)
+				{
+					MCS13 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_14)
+				{
+					MCS14 = idx;
+				}
+				//else if ((pCurrTxRate->CurrMCS == MCS_15)/* && (pCurrTxRate->ShortGI == GI_800)*/)	//we hope to use ShortGI as initial rate
+				else if ((pCurrTxRate->CurrMCS == MCS_15) && (pCurrTxRate->ShortGI == GI_800))	//we hope to use ShortGI as initial rate, however Atheros's chip has bugs when short GI
+				{
+					MCS15 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_20) // 3*3
+				{
+					MCS20 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_21)
+				{
+					MCS21 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_22)
+				{
+					MCS22 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_23)
+				{
+					MCS23 = idx;
+				}
+				idx ++;
+			}
+
+			if (pAd->LatchRfRegs.Channel <= 14)
+			{
+				if (pAd->NicConfig2.field.ExternalLNAForG)
+				{
+					RssiOffset = 2;
+				}
+				else
+				{
+					RssiOffset = 5;
+				}
+			}
+			else
+			{
+				if (pAd->NicConfig2.field.ExternalLNAForA)
+				{
+					RssiOffset = 5;
+				}
+				else
+				{
+					RssiOffset = 8;
+				}
+			}
+#ifdef DOT11_N_SUPPORT
+			/*if (MCS15)*/
+			if ((pTable == RateSwitchTable11BGN3S) ||
+				(pTable == RateSwitchTable11N3S) ||
+				(pTable == RateSwitchTable))
+			{// N mode with 3 stream // 3*3
+				if (MCS23 && (Rssi >= -70))
+					TxRateIdx = MCS15;
+				else if (MCS22 && (Rssi >= -72))
+					TxRateIdx = MCS14;
+        	    else if (MCS21 && (Rssi >= -76))
+					TxRateIdx = MCS13;
+				else if (MCS20 && (Rssi >= -78))
+					TxRateIdx = MCS12;
+			else if (MCS4 && (Rssi >= -82))
+				TxRateIdx = MCS4;
+			else if (MCS3 && (Rssi >= -84))
+				TxRateIdx = MCS3;
+			else if (MCS2 && (Rssi >= -86))
+				TxRateIdx = MCS2;
+			else if (MCS1 && (Rssi >= -88))
+				TxRateIdx = MCS1;
+			else
+				TxRateIdx = MCS0;
+		}
+		else if ((pTable == RateSwitchTable11BGN2S) || (pTable == RateSwitchTable11BGN2SForABand) ||(pTable == RateSwitchTable11N2S) ||(pTable == RateSwitchTable11N2SForABand)) // 3*3
+			{// N mode with 2 stream
+				if (MCS15 && (Rssi >= (-70+RssiOffset)))
+					TxRateIdx = MCS15;
+				else if (MCS14 && (Rssi >= (-72+RssiOffset)))
+					TxRateIdx = MCS14;
+				else if (MCS13 && (Rssi >= (-76+RssiOffset)))
+					TxRateIdx = MCS13;
+				else if (MCS12 && (Rssi >= (-78+RssiOffset)))
+					TxRateIdx = MCS12;
+				else if (MCS4 && (Rssi >= (-82+RssiOffset)))
+					TxRateIdx = MCS4;
+				else if (MCS3 && (Rssi >= (-84+RssiOffset)))
+					TxRateIdx = MCS3;
+				else if (MCS2 && (Rssi >= (-86+RssiOffset)))
+					TxRateIdx = MCS2;
+				else if (MCS1 && (Rssi >= (-88+RssiOffset)))
+					TxRateIdx = MCS1;
+				else
+					TxRateIdx = MCS0;
+			}
+			else if ((pTable == RateSwitchTable11BGN1S) || (pTable == RateSwitchTable11N1S))
+			{// N mode with 1 stream
+				if (MCS7 && (Rssi > (-72+RssiOffset)))
+					TxRateIdx = MCS7;
+				else if (MCS6 && (Rssi > (-74+RssiOffset)))
+					TxRateIdx = MCS6;
+				else if (MCS5 && (Rssi > (-77+RssiOffset)))
+					TxRateIdx = MCS5;
+				else if (MCS4 && (Rssi > (-79+RssiOffset)))
+					TxRateIdx = MCS4;
+				else if (MCS3 && (Rssi > (-81+RssiOffset)))
+					TxRateIdx = MCS3;
+				else if (MCS2 && (Rssi > (-83+RssiOffset)))
+					TxRateIdx = MCS2;
+				else if (MCS1 && (Rssi > (-86+RssiOffset)))
+					TxRateIdx = MCS1;
+				else
+					TxRateIdx = MCS0;
+			}
+			else
+#endif // DOT11_N_SUPPORT //
+			{// Legacy mode
+				if (MCS7 && (Rssi > -70))
+					TxRateIdx = MCS7;
+				else if (MCS6 && (Rssi > -74))
+					TxRateIdx = MCS6;
+				else if (MCS5 && (Rssi > -78))
+					TxRateIdx = MCS5;
+				else if (MCS4 && (Rssi > -82))
+					TxRateIdx = MCS4;
+				else if (MCS4 == 0)	// for B-only mode
+					TxRateIdx = MCS3;
+				else if (MCS3 && (Rssi > -85))
+					TxRateIdx = MCS3;
+				else if (MCS2 && (Rssi > -87))
+					TxRateIdx = MCS2;
+				else if (MCS1 && (Rssi > -90))
+					TxRateIdx = MCS1;
+				else
+					TxRateIdx = MCS0;
+			}
+
+			{
+				pEntry->CurrTxRateIndex = TxRateIdx;
+				pNextTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(pEntry->CurrTxRateIndex+1)*5];
+				MlmeSetTxRate(pAd, pEntry, pNextTxRate);
+			}
+
+			NdisZeroMemory(pEntry->TxQuality, sizeof(USHORT) * MAX_STEP_OF_TX_RATE_SWITCH);
+			NdisZeroMemory(pEntry->PER, sizeof(UCHAR) * MAX_STEP_OF_TX_RATE_SWITCH);
+			pEntry->fLastSecAccordingRSSI = TRUE;
+			// reset all OneSecTx counters
+			RESET_ONE_SEC_TX_CNT(pEntry);
+
+			continue;
+		}
+
+		if (pEntry->fLastSecAccordingRSSI == TRUE)
+		{
+			pEntry->fLastSecAccordingRSSI = FALSE;
+			pEntry->LastSecTxRateChangeAction = 0;
+			// reset all OneSecTx counters
+			RESET_ONE_SEC_TX_CNT(pEntry);
+
+			continue;
+		}
+
+		do
+		{
+			BOOLEAN	bTrainUpDown = FALSE;
+
+			pEntry->CurrTxRateStableTime ++;
+
+			// downgrade TX quality if PER >= Rate-Down threshold
+			if (TxErrorRatio >= TrainDown)
+			{
+				bTrainUpDown = TRUE;
+				pEntry->TxQuality[CurrRateIdx] = DRS_TX_QUALITY_WORST_BOUND;
+			}
+			// upgrade TX quality if PER <= Rate-Up threshold
+			else if (TxErrorRatio <= TrainUp)
+			{
+				bTrainUpDown = TRUE;
+				bUpgradeQuality = TRUE;
+				if (pEntry->TxQuality[CurrRateIdx])
+					pEntry->TxQuality[CurrRateIdx] --;  // quality very good in CurrRate
+
+				if (pEntry->TxRateUpPenalty)
+					pEntry->TxRateUpPenalty --;
+				else if (pEntry->TxQuality[UpRateIdx])
+					pEntry->TxQuality[UpRateIdx] --;    // may improve next UP rate's quality
+			}
+
+			pEntry->PER[CurrRateIdx] = (UCHAR)TxErrorRatio;
+
+			if (bTrainUpDown)
+			{
+				// perform DRS - consider TxRate Down first, then rate up.
+				if ((CurrRateIdx != DownRateIdx) && (pEntry->TxQuality[CurrRateIdx] >= DRS_TX_QUALITY_WORST_BOUND))
+				{
+					pEntry->CurrTxRateIndex = DownRateIdx;
+				}
+				else if ((CurrRateIdx != UpRateIdx) && (pEntry->TxQuality[UpRateIdx] <= 0))
+				{
+					pEntry->CurrTxRateIndex = UpRateIdx;
+				}
+			}
+		} while (FALSE);
+
+		// if rate-up happen, clear all bad history of all TX rates
+		if (pEntry->CurrTxRateIndex > CurrRateIdx)
+		{
+			pEntry->CurrTxRateStableTime = 0;
+			pEntry->TxRateUpPenalty = 0;
+			pEntry->LastSecTxRateChangeAction = 1; // rate UP
+			NdisZeroMemory(pEntry->TxQuality, sizeof(USHORT) * MAX_STEP_OF_TX_RATE_SWITCH);
+			NdisZeroMemory(pEntry->PER, sizeof(UCHAR) * MAX_STEP_OF_TX_RATE_SWITCH);
+
+			//
+			// For TxRate fast train up
+			//
+			if (!pAd->StaCfg.StaQuickResponeForRateUpTimerRunning)
+			{
+				RTMPSetTimer(&pAd->StaCfg.StaQuickResponeForRateUpTimer, 100);
+
+				pAd->StaCfg.StaQuickResponeForRateUpTimerRunning = TRUE;
+			}
+			bTxRateChanged = TRUE;
+		}
+		// if rate-down happen, only clear DownRate's bad history
+		else if (pEntry->CurrTxRateIndex < CurrRateIdx)
+		{
+			pEntry->CurrTxRateStableTime = 0;
+			pEntry->TxRateUpPenalty = 0;           // no penalty
+			pEntry->LastSecTxRateChangeAction = 2; // rate DOWN
+			pEntry->TxQuality[pEntry->CurrTxRateIndex] = 0;
+			pEntry->PER[pEntry->CurrTxRateIndex] = 0;
+
+			//
+			// For TxRate fast train down
+			//
+			if (!pAd->StaCfg.StaQuickResponeForRateUpTimerRunning)
+			{
+				RTMPSetTimer(&pAd->StaCfg.StaQuickResponeForRateUpTimer, 100);
+
+				pAd->StaCfg.StaQuickResponeForRateUpTimerRunning = TRUE;
+			}
+			bTxRateChanged = TRUE;
+		}
+		else
+		{
+			pEntry->LastSecTxRateChangeAction = 0; // rate no change
+			bTxRateChanged = FALSE;
+		}
+
+		pEntry->LastTxOkCount = TxSuccess;
+
+		// reset all OneSecTx counters
+		RESET_ONE_SEC_TX_CNT(pEntry);
+
+		pNextTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(pEntry->CurrTxRateIndex+1)*5];
+		if (bTxRateChanged && pNextTxRate)
+		{
+			MlmeSetTxRate(pAd, pEntry, pNextTxRate);
+		}
+	}
+}
+
+/*
+	========================================================================
+	Routine Description:
+		Station side, Auto TxRate faster train up timer call back function.
+
+	Arguments:
+		SystemSpecific1			- Not used.
+		FunctionContext			- Pointer to our Adapter context.
+		SystemSpecific2			- Not used.
+		SystemSpecific3			- Not used.
+
+	Return Value:
+		None
+
+	========================================================================
+*/
+VOID StaQuickResponeForRateUpExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+	PRTMP_ADAPTER			pAd = (PRTMP_ADAPTER)FunctionContext;
+	UCHAR					UpRateIdx = 0, DownRateIdx = 0, CurrRateIdx = 0;
+	ULONG					TxTotalCnt;
+	ULONG					TxErrorRatio = 0;
+	BOOLEAN					bTxRateChanged = TRUE; //, bUpgradeQuality = FALSE;
+	PRTMP_TX_RATE_SWITCH	pCurrTxRate, pNextTxRate = NULL;
+	PUCHAR					pTable;
+	UCHAR					TableSize = 0;
+	UCHAR					InitTxRateIdx = 0, TrainUp, TrainDown;
+	TX_STA_CNT1_STRUC		StaTx1;
+	TX_STA_CNT0_STRUC		TxStaCnt0;
+	CHAR					Rssi, ratio;
+	ULONG					TxRetransmit = 0, TxSuccess = 0, TxFailCount = 0;
+	MAC_TABLE_ENTRY			*pEntry;
+	ULONG					i;
+
+	pAd->StaCfg.StaQuickResponeForRateUpTimerRunning = FALSE;
+
+    //
+    // walk through MAC table, see if need to change AP's TX rate toward each entry
+    //
+	for (i = 1; i < MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		pEntry = &pAd->MacTab.Content[i];
+
+		// check if this entry need to switch rate automatically
+		if (RTMPCheckEntryEnableAutoRateSwitch(pAd, pEntry) == FALSE)
+			continue;
+
+		//Rssi = RTMPMaxRssi(pAd, (CHAR)pAd->StaCfg.AvgRssi0, (CHAR)pAd->StaCfg.AvgRssi1, (CHAR)pAd->StaCfg.AvgRssi2);
+	    if (pAd->Antenna.field.TxPath > 1)
+			Rssi = (pAd->StaCfg.RssiSample.AvgRssi0 + pAd->StaCfg.RssiSample.AvgRssi1) >> 1;
+		else
+			Rssi = pAd->StaCfg.RssiSample.AvgRssi0;
+
+		CurrRateIdx = pAd->CommonCfg.TxRateIndex;
+
+			MlmeSelectTxRateTable(pAd, pEntry, &pTable, &TableSize, &InitTxRateIdx);
+
+		// decide the next upgrade rate and downgrade rate, if any
+		if ((CurrRateIdx > 0) && (CurrRateIdx < (TableSize - 1)))
+		{
+			UpRateIdx = CurrRateIdx + 1;
+			DownRateIdx = CurrRateIdx -1;
+		}
+		else if (CurrRateIdx == 0)
+		{
+			UpRateIdx = CurrRateIdx + 1;
+			DownRateIdx = CurrRateIdx;
+		}
+		else if (CurrRateIdx == (TableSize - 1))
+		{
+			UpRateIdx = CurrRateIdx;
+			DownRateIdx = CurrRateIdx - 1;
+		}
+
+		pCurrTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(CurrRateIdx+1)*5];
+
+#ifdef DOT11_N_SUPPORT
+		if ((Rssi > -65) && (pCurrTxRate->Mode >= MODE_HTMIX))
+		{
+			TrainUp		= (pCurrTxRate->TrainUp + (pCurrTxRate->TrainUp >> 1));
+			TrainDown	= (pCurrTxRate->TrainDown + (pCurrTxRate->TrainDown >> 1));
+		}
+		else
+#endif // DOT11_N_SUPPORT //
+		{
+			TrainUp		= pCurrTxRate->TrainUp;
+			TrainDown	= pCurrTxRate->TrainDown;
+		}
+
+		if (pAd->MacTab.Size == 1)
+		{
+			// Update statistic counter
+			RTMP_IO_READ32(pAd, TX_STA_CNT0, &TxStaCnt0.word);
+			RTMP_IO_READ32(pAd, TX_STA_CNT1, &StaTx1.word);
+
+			TxRetransmit = StaTx1.field.TxRetransmit;
+			TxSuccess = StaTx1.field.TxSuccess;
+			TxFailCount = TxStaCnt0.field.TxFailCount;
+			TxTotalCnt = TxRetransmit + TxSuccess + TxFailCount;
+
+			pAd->RalinkCounters.OneSecTxRetryOkCount += StaTx1.field.TxRetransmit;
+			pAd->RalinkCounters.OneSecTxNoRetryOkCount += StaTx1.field.TxSuccess;
+			pAd->RalinkCounters.OneSecTxFailCount += TxStaCnt0.field.TxFailCount;
+			pAd->WlanCounters.TransmittedFragmentCount.u.LowPart += StaTx1.field.TxSuccess;
+			pAd->WlanCounters.RetryCount.u.LowPart += StaTx1.field.TxRetransmit;
+			pAd->WlanCounters.FailedCount.u.LowPart += TxStaCnt0.field.TxFailCount;
+
+			if (TxTotalCnt)
+				TxErrorRatio = ((TxRetransmit + TxFailCount) * 100) / TxTotalCnt;
+		}
+		else
+		{
+			TxTotalCnt = pEntry->OneSecTxNoRetryOkCount +
+				 pEntry->OneSecTxRetryOkCount +
+				 pEntry->OneSecTxFailCount;
+
+			if (TxTotalCnt)
+				TxErrorRatio = ((pEntry->OneSecTxRetryOkCount + pEntry->OneSecTxFailCount) * 100) / TxTotalCnt;
+		}
+
+
+		//
+		// CASE 1. when TX samples are fewer than 15, then decide TX rate solely on RSSI
+		//         (criteria copied from RT2500 for Netopia case)
+		//
+		if (TxTotalCnt <= 12)
+		{
+			NdisZeroMemory(pAd->DrsCounters.TxQuality, sizeof(USHORT) * MAX_STEP_OF_TX_RATE_SWITCH);
+			NdisZeroMemory(pAd->DrsCounters.PER, sizeof(UCHAR) * MAX_STEP_OF_TX_RATE_SWITCH);
+
+			if ((pAd->DrsCounters.LastSecTxRateChangeAction == 1) && (CurrRateIdx != DownRateIdx))
+			{
+				pAd->CommonCfg.TxRateIndex = DownRateIdx;
+				pAd->DrsCounters.TxQuality[CurrRateIdx] = DRS_TX_QUALITY_WORST_BOUND;
+			}
+			else if ((pAd->DrsCounters.LastSecTxRateChangeAction == 2) && (CurrRateIdx != UpRateIdx))
+			{
+				pAd->CommonCfg.TxRateIndex = UpRateIdx;
+			}
+
+			DBGPRINT_RAW(RT_DEBUG_TRACE,("QuickDRS: TxTotalCnt <= 15, train back to original rate \n"));
+			return;
+		}
+
+		do
+		{
+			ULONG OneSecTxNoRetryOKRationCount;
+
+			if (pAd->DrsCounters.LastTimeTxRateChangeAction == 0)
+				ratio = 5;
+			else
+				ratio = 4;
+
+			// downgrade TX quality if PER >= Rate-Down threshold
+			if (TxErrorRatio >= TrainDown)
+			{
+				pAd->DrsCounters.TxQuality[CurrRateIdx] = DRS_TX_QUALITY_WORST_BOUND;
+			}
+
+			pAd->DrsCounters.PER[CurrRateIdx] = (UCHAR)TxErrorRatio;
+
+			OneSecTxNoRetryOKRationCount = (TxSuccess * ratio);
+
+			// perform DRS - consider TxRate Down first, then rate up.
+			if ((pAd->DrsCounters.LastSecTxRateChangeAction == 1) && (CurrRateIdx != DownRateIdx))
+			{
+				if ((pAd->DrsCounters.LastTxOkCount + 2) >= OneSecTxNoRetryOKRationCount)
+				{
+					pAd->CommonCfg.TxRateIndex = DownRateIdx;
+					pAd->DrsCounters.TxQuality[CurrRateIdx] = DRS_TX_QUALITY_WORST_BOUND;
+
+				}
+
+			}
+			else if ((pAd->DrsCounters.LastSecTxRateChangeAction == 2) && (CurrRateIdx != UpRateIdx))
+			{
+				if ((TxErrorRatio >= 50) || (TxErrorRatio >= TrainDown))
+				{
+
+				}
+				else if ((pAd->DrsCounters.LastTxOkCount + 2) >= OneSecTxNoRetryOKRationCount)
+				{
+					pAd->CommonCfg.TxRateIndex = UpRateIdx;
+				}
+			}
+		}while (FALSE);
+
+		// if rate-up happen, clear all bad history of all TX rates
+		if (pAd->CommonCfg.TxRateIndex > CurrRateIdx)
+		{
+			pAd->DrsCounters.TxRateUpPenalty = 0;
+			NdisZeroMemory(pAd->DrsCounters.TxQuality, sizeof(USHORT) * MAX_STEP_OF_TX_RATE_SWITCH);
+			NdisZeroMemory(pAd->DrsCounters.PER, sizeof(UCHAR) * MAX_STEP_OF_TX_RATE_SWITCH);
+		}
+		// if rate-down happen, only clear DownRate's bad history
+		else if (pAd->CommonCfg.TxRateIndex < CurrRateIdx)
+		{
+			DBGPRINT_RAW(RT_DEBUG_TRACE,("QuickDRS: --TX rate from %d to %d \n", CurrRateIdx, pAd->CommonCfg.TxRateIndex));
+
+			pAd->DrsCounters.TxRateUpPenalty = 0;           // no penalty
+			pAd->DrsCounters.TxQuality[pAd->CommonCfg.TxRateIndex] = 0;
+			pAd->DrsCounters.PER[pAd->CommonCfg.TxRateIndex] = 0;
+		}
+		else
+		{
+			bTxRateChanged = FALSE;
+		}
+
+		pNextTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(pAd->CommonCfg.TxRateIndex+1)*5];
+		if (bTxRateChanged && pNextTxRate)
+		{
+			MlmeSetTxRate(pAd, pEntry, pNextTxRate);
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+		This routine is executed periodically inside MlmePeriodicExec() after
+		association with an AP.
+		It checks if StaCfg.Psm is consistent with user policy (recorded in
+		StaCfg.WindowsPowerMode). If not, enforce user policy. However,
+		there're some conditions to consider:
+		1. we don't support power-saving in ADHOC mode, so Psm=PWR_ACTIVE all
+		   the time when Mibss==TRUE
+		2. When link up in INFRA mode, Psm should not be switch to PWR_SAVE
+		   if outgoing traffic available in TxRing or MgmtRing.
+	Output:
+		1. change pAd->StaCfg.Psm to PWR_SAVE or leave it untouched
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID MlmeCheckPsmChange(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG	Now32)
+{
+	ULONG	PowerMode;
+
+	// condition -
+	// 1. Psm maybe ON only happen in INFRASTRUCTURE mode
+	// 2. user wants either MAX_PSP or FAST_PSP
+	// 3. but current psm is not in PWR_SAVE
+	// 4. CNTL state machine is not doing SCANning
+	// 5. no TX SUCCESS event for the past 1-sec period
+#ifdef NDIS51_MINIPORT
+	if (pAd->StaCfg.WindowsPowerProfile == NdisPowerProfileBattery)
+		PowerMode = pAd->StaCfg.WindowsBatteryPowerMode;
+	else
+#endif
+		PowerMode = pAd->StaCfg.WindowsPowerMode;
+
+	if (INFRA_ON(pAd) &&
+		(PowerMode != Ndis802_11PowerModeCAM) &&
+		(pAd->StaCfg.Psm == PWR_ACTIVE) &&
+		(pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE))
+	{
+		NdisGetSystemUpTime(&pAd->Mlme.LastSendNULLpsmTime);
+		pAd->RalinkCounters.RxCountSinceLastNULL = 0;
+		MlmeSetPsmBit(pAd, PWR_SAVE);
+		if (!(pAd->CommonCfg.bAPSDCapable && pAd->CommonCfg.APEdcaParm.bAPSDCapable))
+		{
+			RTMPSendNullFrame(pAd, pAd->CommonCfg.TxRate, FALSE);
+		}
+		else
+		{
+			RTMPSendNullFrame(pAd, pAd->CommonCfg.TxRate, TRUE);
+		}
+	}
+}
+
+// IRQL = PASSIVE_LEVEL
+// IRQL = DISPATCH_LEVEL
+VOID MlmeSetPsmBit(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT psm)
+{
+	AUTO_RSP_CFG_STRUC csr4;
+
+	pAd->StaCfg.Psm = psm;
+	RTMP_IO_READ32(pAd, AUTO_RSP_CFG, &csr4.word);
+	csr4.field.AckCtsPsmBit = (psm == PWR_SAVE)? 1:0;
+	RTMP_IO_WRITE32(pAd, AUTO_RSP_CFG, csr4.word);
+	DBGPRINT(RT_DEBUG_TRACE, ("MlmeSetPsmBit = %d\n", psm));
+}
+#endif // CONFIG_STA_SUPPORT //
+
+
+// IRQL = DISPATCH_LEVEL
+VOID MlmeSetTxPreamble(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT TxPreamble)
+{
+	AUTO_RSP_CFG_STRUC csr4;
+
+	//
+	// Always use Long preamble before verifiation short preamble functionality works well.
+	// Todo: remove the following line if short preamble functionality works
+	//
+	//TxPreamble = Rt802_11PreambleLong;
+
+	RTMP_IO_READ32(pAd, AUTO_RSP_CFG, &csr4.word);
+	if (TxPreamble == Rt802_11PreambleLong)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("MlmeSetTxPreamble (= LONG PREAMBLE)\n"));
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED);
+		csr4.field.AutoResponderPreamble = 0;
+	}
+	else
+	{
+		// NOTE: 1Mbps should always use long preamble
+		DBGPRINT(RT_DEBUG_TRACE, ("MlmeSetTxPreamble (= SHORT PREAMBLE)\n"));
+		OPSTATUS_SET_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED);
+		csr4.field.AutoResponderPreamble = 1;
+	}
+
+	RTMP_IO_WRITE32(pAd, AUTO_RSP_CFG, csr4.word);
+}
+
+/*
+    ==========================================================================
+    Description:
+        Update basic rate bitmap
+    ==========================================================================
+ */
+
+VOID UpdateBasicRateBitmap(
+    IN  PRTMP_ADAPTER   pAdapter)
+{
+    INT  i, j;
+                  /* 1  2  5.5, 11,  6,  9, 12, 18, 24, 36, 48,  54 */
+    UCHAR rate[] = { 2, 4,  11, 22, 12, 18, 24, 36, 48, 72, 96, 108 };
+    UCHAR *sup_p = pAdapter->CommonCfg.SupRate;
+    UCHAR *ext_p = pAdapter->CommonCfg.ExtRate;
+    ULONG bitmap = pAdapter->CommonCfg.BasicRateBitmap;
+
+
+    /* if A mode, always use fix BasicRateBitMap */
+    //if (pAdapter->CommonCfg.Channel == PHY_11A)
+	if (pAdapter->CommonCfg.Channel > 14)
+        pAdapter->CommonCfg.BasicRateBitmap = 0x150; /* 6, 12, 24M */
+    /* End of if */
+
+    if (pAdapter->CommonCfg.BasicRateBitmap > 4095)
+    {
+        /* (2 ^ MAX_LEN_OF_SUPPORTED_RATES) -1 */
+        return;
+    } /* End of if */
+
+    for(i=0; i<MAX_LEN_OF_SUPPORTED_RATES; i++)
+    {
+        sup_p[i] &= 0x7f;
+        ext_p[i] &= 0x7f;
+    } /* End of for */
+
+    for(i=0; i<MAX_LEN_OF_SUPPORTED_RATES; i++)
+    {
+        if (bitmap & (1 << i))
+        {
+            for(j=0; j<MAX_LEN_OF_SUPPORTED_RATES; j++)
+            {
+                if (sup_p[j] == rate[i])
+                    sup_p[j] |= 0x80;
+                /* End of if */
+            } /* End of for */
+
+            for(j=0; j<MAX_LEN_OF_SUPPORTED_RATES; j++)
+            {
+                if (ext_p[j] == rate[i])
+                    ext_p[j] |= 0x80;
+                /* End of if */
+            } /* End of for */
+        } /* End of if */
+    } /* End of for */
+} /* End of UpdateBasicRateBitmap */
+
+// IRQL = PASSIVE_LEVEL
+// IRQL = DISPATCH_LEVEL
+// bLinkUp is to identify the inital link speed.
+// TRUE indicates the rate update at linkup, we should not try to set the rate at 54Mbps.
+VOID MlmeUpdateTxRates(
+	IN PRTMP_ADAPTER 		pAd,
+	IN 	BOOLEAN		 		bLinkUp,
+	IN	UCHAR				apidx)
+{
+	int i, num;
+	UCHAR Rate = RATE_6, MaxDesire = RATE_1, MaxSupport = RATE_1;
+	UCHAR MinSupport = RATE_54;
+	ULONG BasicRateBitmap = 0;
+	UCHAR CurrBasicRate = RATE_1;
+	UCHAR *pSupRate, SupRateLen, *pExtRate, ExtRateLen;
+	PHTTRANSMIT_SETTING		pHtPhy = NULL;
+	PHTTRANSMIT_SETTING		pMaxHtPhy = NULL;
+	PHTTRANSMIT_SETTING		pMinHtPhy = NULL;
+	BOOLEAN 				*auto_rate_cur_p;
+	UCHAR					HtMcs = MCS_AUTO;
+
+	// find max desired rate
+	UpdateBasicRateBitmap(pAd);
+
+	num = 0;
+	auto_rate_cur_p = NULL;
+	for (i=0; i<MAX_LEN_OF_SUPPORTED_RATES; i++)
+	{
+		switch (pAd->CommonCfg.DesireRate[i] & 0x7f)
+		{
+			case 2:  Rate = RATE_1;   num++;   break;
+			case 4:  Rate = RATE_2;   num++;   break;
+			case 11: Rate = RATE_5_5; num++;   break;
+			case 22: Rate = RATE_11;  num++;   break;
+			case 12: Rate = RATE_6;   num++;   break;
+			case 18: Rate = RATE_9;   num++;   break;
+			case 24: Rate = RATE_12;  num++;   break;
+			case 36: Rate = RATE_18;  num++;   break;
+			case 48: Rate = RATE_24;  num++;   break;
+			case 72: Rate = RATE_36;  num++;   break;
+			case 96: Rate = RATE_48;  num++;   break;
+			case 108: Rate = RATE_54; num++;   break;
+			//default: Rate = RATE_1;   break;
+		}
+		if (MaxDesire < Rate)  MaxDesire = Rate;
+	}
+
+//===========================================================================
+//===========================================================================
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		pHtPhy 		= &pAd->StaCfg.HTPhyMode;
+		pMaxHtPhy	= &pAd->StaCfg.MaxHTPhyMode;
+		pMinHtPhy	= &pAd->StaCfg.MinHTPhyMode;
+
+		auto_rate_cur_p = &pAd->StaCfg.bAutoTxRateSwitch;
+		HtMcs 		= pAd->StaCfg.DesiredTransmitSetting.field.MCS;
+
+		if ((pAd->StaCfg.BssType == BSS_ADHOC) &&
+			(pAd->CommonCfg.PhyMode == PHY_11B) &&
+			(MaxDesire > RATE_11))
+		{
+			MaxDesire = RATE_11;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	pAd->CommonCfg.MaxDesiredRate = MaxDesire;
+	pMinHtPhy->word = 0;
+	pMaxHtPhy->word = 0;
+	pHtPhy->word = 0;
+
+	// Auto rate switching is enabled only if more than one DESIRED RATES are
+	// specified; otherwise disabled
+	if (num <= 1)
+	{
+		*auto_rate_cur_p = FALSE;
+	}
+	else
+	{
+		*auto_rate_cur_p = TRUE;
+	}
+
+#if 1
+	if (HtMcs != MCS_AUTO)
+	{
+		*auto_rate_cur_p = FALSE;
+	}
+	else
+	{
+		*auto_rate_cur_p = TRUE;
+	}
+#endif
+
+#ifdef CONFIG_STA_SUPPORT
+	if ((ADHOC_ON(pAd) || INFRA_ON(pAd)) && (pAd->OpMode == OPMODE_STA))
+	{
+		pSupRate = &pAd->StaActive.SupRate[0];
+		pExtRate = &pAd->StaActive.ExtRate[0];
+		SupRateLen = pAd->StaActive.SupRateLen;
+		ExtRateLen = pAd->StaActive.ExtRateLen;
+	}
+	else
+#endif // CONFIG_STA_SUPPORT //
+	{
+		pSupRate = &pAd->CommonCfg.SupRate[0];
+		pExtRate = &pAd->CommonCfg.ExtRate[0];
+		SupRateLen = pAd->CommonCfg.SupRateLen;
+		ExtRateLen = pAd->CommonCfg.ExtRateLen;
+	}
+
+	// find max supported rate
+	for (i=0; i<SupRateLen; i++)
+	{
+		switch (pSupRate[i] & 0x7f)
+		{
+			case 2:   Rate = RATE_1;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0001;	 break;
+			case 4:   Rate = RATE_2;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0002;	 break;
+			case 11:  Rate = RATE_5_5;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0004;	 break;
+			case 22:  Rate = RATE_11;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0008;	 break;
+			case 12:  Rate = RATE_6;	/*if (pSupRate[i] & 0x80)*/  BasicRateBitmap |= 0x0010;  break;
+			case 18:  Rate = RATE_9;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0020;	 break;
+			case 24:  Rate = RATE_12;	/*if (pSupRate[i] & 0x80)*/  BasicRateBitmap |= 0x0040;  break;
+			case 36:  Rate = RATE_18;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0080;	 break;
+			case 48:  Rate = RATE_24;	/*if (pSupRate[i] & 0x80)*/  BasicRateBitmap |= 0x0100;  break;
+			case 72:  Rate = RATE_36;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0200;	 break;
+			case 96:  Rate = RATE_48;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0400;	 break;
+			case 108: Rate = RATE_54;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0800;	 break;
+			default:  Rate = RATE_1;	break;
+		}
+		if (MaxSupport < Rate)	MaxSupport = Rate;
+
+		if (MinSupport > Rate) MinSupport = Rate;
+	}
+
+	for (i=0; i<ExtRateLen; i++)
+	{
+		switch (pExtRate[i] & 0x7f)
+		{
+			case 2:   Rate = RATE_1;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0001;	 break;
+			case 4:   Rate = RATE_2;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0002;	 break;
+			case 11:  Rate = RATE_5_5;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0004;	 break;
+			case 22:  Rate = RATE_11;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0008;	 break;
+			case 12:  Rate = RATE_6;	/*if (pExtRate[i] & 0x80)*/  BasicRateBitmap |= 0x0010;  break;
+			case 18:  Rate = RATE_9;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0020;	 break;
+			case 24:  Rate = RATE_12;	/*if (pExtRate[i] & 0x80)*/  BasicRateBitmap |= 0x0040;  break;
+			case 36:  Rate = RATE_18;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0080;	 break;
+			case 48:  Rate = RATE_24;	/*if (pExtRate[i] & 0x80)*/  BasicRateBitmap |= 0x0100;  break;
+			case 72:  Rate = RATE_36;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0200;	 break;
+			case 96:  Rate = RATE_48;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0400;	 break;
+			case 108: Rate = RATE_54;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0800;	 break;
+			default:  Rate = RATE_1;	break;
+		}
+		if (MaxSupport < Rate)	MaxSupport = Rate;
+
+		if (MinSupport > Rate) MinSupport = Rate;
+	}
+
+	RTMP_IO_WRITE32(pAd, LEGACY_BASIC_RATE, BasicRateBitmap);
+
+	// calculate the exptected ACK rate for each TX rate. This info is used to caculate
+	// the DURATION field of outgoing uniicast DATA/MGMT frame
+	for (i=0; i<MAX_LEN_OF_SUPPORTED_RATES; i++)
+	{
+		if (BasicRateBitmap & (0x01 << i))
+			CurrBasicRate = (UCHAR)i;
+		pAd->CommonCfg.ExpectedACKRate[i] = CurrBasicRate;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("MlmeUpdateTxRates[MaxSupport = %d] = MaxDesire %d Mbps\n", RateIdToMbps[MaxSupport], RateIdToMbps[MaxDesire]));
+	// max tx rate = min {max desire rate, max supported rate}
+	if (MaxSupport < MaxDesire)
+		pAd->CommonCfg.MaxTxRate = MaxSupport;
+	else
+		pAd->CommonCfg.MaxTxRate = MaxDesire;
+
+	pAd->CommonCfg.MinTxRate = MinSupport;
+	if (*auto_rate_cur_p)
+	{
+		short dbm = 0;
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			dbm = pAd->StaCfg.RssiSample.AvgRssi0 - pAd->BbpRssiToDbmDelta;
+#endif // CONFIG_STA_SUPPORT //
+		if (bLinkUp == TRUE)
+			pAd->CommonCfg.TxRate = RATE_24;
+		else
+			pAd->CommonCfg.TxRate = pAd->CommonCfg.MaxTxRate;
+
+		if (dbm < -75)
+			pAd->CommonCfg.TxRate = RATE_11;
+		else if (dbm < -70)
+			pAd->CommonCfg.TxRate = RATE_24;
+
+		// should never exceed MaxTxRate (consider 11B-only mode)
+		if (pAd->CommonCfg.TxRate > pAd->CommonCfg.MaxTxRate)
+			pAd->CommonCfg.TxRate = pAd->CommonCfg.MaxTxRate;
+
+		pAd->CommonCfg.TxRateIndex = 0;
+	}
+	else
+	{
+		pAd->CommonCfg.TxRate = pAd->CommonCfg.MaxTxRate;
+		pHtPhy->field.MCS	= (pAd->CommonCfg.MaxTxRate > 3) ? (pAd->CommonCfg.MaxTxRate - 4) : pAd->CommonCfg.MaxTxRate;
+		pHtPhy->field.MODE	= (pAd->CommonCfg.MaxTxRate > 3) ? MODE_OFDM : MODE_CCK;
+
+		pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.STBC	= pHtPhy->field.STBC;
+		pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.ShortGI	= pHtPhy->field.ShortGI;
+		pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MCS		= pHtPhy->field.MCS;
+		pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE	= pHtPhy->field.MODE;
+	}
+
+	if (pAd->CommonCfg.TxRate <= RATE_11)
+	{
+		pMaxHtPhy->field.MODE = MODE_CCK;
+		pMaxHtPhy->field.MCS = pAd->CommonCfg.TxRate;
+		pMinHtPhy->field.MCS = pAd->CommonCfg.MinTxRate;
+	}
+	else
+	{
+		pMaxHtPhy->field.MODE = MODE_OFDM;
+		pMaxHtPhy->field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.TxRate];
+		if (pAd->CommonCfg.MinTxRate >= RATE_6 && (pAd->CommonCfg.MinTxRate <= RATE_54))
+			{pMinHtPhy->field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MinTxRate];}
+		else
+			{pMinHtPhy->field.MCS = pAd->CommonCfg.MinTxRate;}
+	}
+
+	pHtPhy->word = (pMaxHtPhy->word);
+	if (bLinkUp && (pAd->OpMode == OPMODE_STA))
+	{
+			pAd->MacTab.Content[BSSID_WCID].HTPhyMode.word = pHtPhy->word;
+			pAd->MacTab.Content[BSSID_WCID].MaxHTPhyMode.word = pMaxHtPhy->word;
+			pAd->MacTab.Content[BSSID_WCID].MinHTPhyMode.word = pMinHtPhy->word;
+	}
+	else
+	{
+		switch (pAd->CommonCfg.PhyMode)
+		{
+			case PHY_11BG_MIXED:
+			case PHY_11B:
+#ifdef DOT11_N_SUPPORT
+			case PHY_11BGN_MIXED:
+#endif // DOT11_N_SUPPORT //
+				pAd->CommonCfg.MlmeRate = RATE_1;
+				pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_CCK;
+				pAd->CommonCfg.MlmeTransmit.field.MCS = RATE_1;
+				pAd->CommonCfg.RtsRate = RATE_11;
+				break;
+			case PHY_11G:
+			case PHY_11A:
+#ifdef DOT11_N_SUPPORT
+			case PHY_11AGN_MIXED:
+			case PHY_11GN_MIXED:
+			case PHY_11N_2_4G:
+			case PHY_11AN_MIXED:
+			case PHY_11N_5G:
+#endif // DOT11_N_SUPPORT //
+				pAd->CommonCfg.MlmeRate = RATE_6;
+				pAd->CommonCfg.RtsRate = RATE_6;
+				pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_OFDM;
+				pAd->CommonCfg.MlmeTransmit.field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MlmeRate];
+				break;
+			case PHY_11ABG_MIXED:
+#ifdef DOT11_N_SUPPORT
+			case PHY_11ABGN_MIXED:
+#endif // DOT11_N_SUPPORT //
+				if (pAd->CommonCfg.Channel <= 14)
+				{
+					pAd->CommonCfg.MlmeRate = RATE_1;
+					pAd->CommonCfg.RtsRate = RATE_1;
+					pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_CCK;
+					pAd->CommonCfg.MlmeTransmit.field.MCS = RATE_1;
+				}
+				else
+				{
+					pAd->CommonCfg.MlmeRate = RATE_6;
+					pAd->CommonCfg.RtsRate = RATE_6;
+					pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_OFDM;
+					pAd->CommonCfg.MlmeTransmit.field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MlmeRate];
+				}
+				break;
+			default: // error
+				pAd->CommonCfg.MlmeRate = RATE_6;
+                        	pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_OFDM;
+				pAd->CommonCfg.MlmeTransmit.field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MlmeRate];
+				pAd->CommonCfg.RtsRate = RATE_1;
+				break;
+		}
+		//
+		// Keep Basic Mlme Rate.
+		//
+		pAd->MacTab.Content[MCAST_WCID].HTPhyMode.word = pAd->CommonCfg.MlmeTransmit.word;
+		if (pAd->CommonCfg.MlmeTransmit.field.MODE == MODE_OFDM)
+			pAd->MacTab.Content[MCAST_WCID].HTPhyMode.field.MCS = OfdmRateToRxwiMCS[RATE_24];
+		else
+			pAd->MacTab.Content[MCAST_WCID].HTPhyMode.field.MCS = RATE_1;
+		pAd->CommonCfg.BasicMlmeRate = pAd->CommonCfg.MlmeRate;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, (" MlmeUpdateTxRates (MaxDesire=%d, MaxSupport=%d, MaxTxRate=%d, MinRate=%d, Rate Switching =%d)\n",
+			 RateIdToMbps[MaxDesire], RateIdToMbps[MaxSupport], RateIdToMbps[pAd->CommonCfg.MaxTxRate], RateIdToMbps[pAd->CommonCfg.MinTxRate],
+			 /*OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_TX_RATE_SWITCH_ENABLED)*/*auto_rate_cur_p));
+	DBGPRINT(RT_DEBUG_TRACE, (" MlmeUpdateTxRates (TxRate=%d, RtsRate=%d, BasicRateBitmap=0x%04lx)\n",
+			 RateIdToMbps[pAd->CommonCfg.TxRate], RateIdToMbps[pAd->CommonCfg.RtsRate], BasicRateBitmap));
+	DBGPRINT(RT_DEBUG_TRACE, ("MlmeUpdateTxRates (MlmeTransmit=0x%x, MinHTPhyMode=%x, MaxHTPhyMode=0x%x, HTPhyMode=0x%x)\n",
+			 pAd->CommonCfg.MlmeTransmit.word, pAd->MacTab.Content[BSSID_WCID].MinHTPhyMode.word ,pAd->MacTab.Content[BSSID_WCID].MaxHTPhyMode.word ,pAd->MacTab.Content[BSSID_WCID].HTPhyMode.word ));
+}
+
+#ifdef DOT11_N_SUPPORT
+/*
+	==========================================================================
+	Description:
+		This function update HT Rate setting.
+		Input Wcid value is valid for 2 case :
+		1. it's used for Station in infra mode that copy AP rate to Mactable.
+		2. OR Station 	in adhoc mode to copy peer's HT rate to Mactable.
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID MlmeUpdateHtTxRates(
+	IN PRTMP_ADAPTER 		pAd,
+	IN	UCHAR				apidx)
+{
+	UCHAR	StbcMcs; //j, StbcMcs, bitmask;
+	CHAR 	i; // 3*3
+	RT_HT_CAPABILITY 	*pRtHtCap = NULL;
+	RT_HT_PHY_INFO		*pActiveHtPhy = NULL;
+	ULONG		BasicMCS;
+	UCHAR j, bitmask;
+	PRT_HT_PHY_INFO			pDesireHtPhy = NULL;
+	PHTTRANSMIT_SETTING		pHtPhy = NULL;
+	PHTTRANSMIT_SETTING		pMaxHtPhy = NULL;
+	PHTTRANSMIT_SETTING		pMinHtPhy = NULL;
+	BOOLEAN 				*auto_rate_cur_p;
+
+	DBGPRINT(RT_DEBUG_TRACE,("MlmeUpdateHtTxRates===> \n"));
+
+	auto_rate_cur_p = NULL;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		pDesireHtPhy	= &pAd->StaCfg.DesiredHtPhyInfo;
+		pActiveHtPhy	= &pAd->StaCfg.DesiredHtPhyInfo;
+		pHtPhy 		= &pAd->StaCfg.HTPhyMode;
+		pMaxHtPhy	= &pAd->StaCfg.MaxHTPhyMode;
+		pMinHtPhy	= &pAd->StaCfg.MinHTPhyMode;
+
+		auto_rate_cur_p = &pAd->StaCfg.bAutoTxRateSwitch;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+	if ((ADHOC_ON(pAd) || INFRA_ON(pAd)) && (pAd->OpMode == OPMODE_STA))
+	{
+		if (pAd->StaActive.SupportedPhyInfo.bHtEnable == FALSE)
+			return;
+
+		pRtHtCap = &pAd->StaActive.SupportedHtPhy;
+		pActiveHtPhy = &pAd->StaActive.SupportedPhyInfo;
+		StbcMcs = (UCHAR)pAd->MlmeAux.AddHtInfo.AddHtInfo3.StbcMcs;
+		BasicMCS =pAd->MlmeAux.AddHtInfo.MCSSet[0]+(pAd->MlmeAux.AddHtInfo.MCSSet[1]<<8)+(StbcMcs<<16);
+		if ((pAd->CommonCfg.DesiredHtPhy.TxSTBC) && (pRtHtCap->RxSTBC) && (pAd->Antenna.field.TxPath == 2))
+			pMaxHtPhy->field.STBC = STBC_USE;
+		else
+			pMaxHtPhy->field.STBC = STBC_NONE;
+	}
+	else
+#endif // CONFIG_STA_SUPPORT //
+	{
+		if (pDesireHtPhy->bHtEnable == FALSE)
+			return;
+
+		pRtHtCap = &pAd->CommonCfg.DesiredHtPhy;
+		StbcMcs = (UCHAR)pAd->CommonCfg.AddHTInfo.AddHtInfo3.StbcMcs;
+		BasicMCS = pAd->CommonCfg.AddHTInfo.MCSSet[0]+(pAd->CommonCfg.AddHTInfo.MCSSet[1]<<8)+(StbcMcs<<16);
+		if ((pAd->CommonCfg.DesiredHtPhy.TxSTBC) && (pRtHtCap->RxSTBC) && (pAd->Antenna.field.TxPath == 2))
+			pMaxHtPhy->field.STBC = STBC_USE;
+		else
+			pMaxHtPhy->field.STBC = STBC_NONE;
+	}
+
+	// Decide MAX ht rate.
+	if ((pRtHtCap->GF) && (pAd->CommonCfg.DesiredHtPhy.GF))
+		pMaxHtPhy->field.MODE = MODE_HTGREENFIELD;
+	else
+		pMaxHtPhy->field.MODE = MODE_HTMIX;
+
+    if ((pAd->CommonCfg.DesiredHtPhy.ChannelWidth) && (pRtHtCap->ChannelWidth))
+		pMaxHtPhy->field.BW = BW_40;
+	else
+		pMaxHtPhy->field.BW = BW_20;
+
+    if (pMaxHtPhy->field.BW == BW_20)
+		pMaxHtPhy->field.ShortGI = (pAd->CommonCfg.DesiredHtPhy.ShortGIfor20 & pRtHtCap->ShortGIfor20);
+	else
+		pMaxHtPhy->field.ShortGI = (pAd->CommonCfg.DesiredHtPhy.ShortGIfor40 & pRtHtCap->ShortGIfor40);
+
+	for (i=23; i>=0; i--) // 3*3
+	{
+		j = i/8;
+		bitmask = (1<<(i-(j*8)));
+
+		if ((pActiveHtPhy->MCSSet[j] & bitmask) && (pDesireHtPhy->MCSSet[j] & bitmask))
+		{
+			pMaxHtPhy->field.MCS = i;
+			break;
+		}
+
+		if (i==0)
+			break;
+	}
+
+	// Copy MIN ht rate.  rt2860???
+	pMinHtPhy->field.BW = BW_20;
+	pMinHtPhy->field.MCS = 0;
+	pMinHtPhy->field.STBC = 0;
+	pMinHtPhy->field.ShortGI = 0;
+	//If STA assigns fixed rate. update to fixed here.
+#ifdef CONFIG_STA_SUPPORT
+	if ( (pAd->OpMode == OPMODE_STA) && (pDesireHtPhy->MCSSet[0] != 0xff))
+	{
+		if (pDesireHtPhy->MCSSet[4] != 0)
+		{
+			pMaxHtPhy->field.MCS = 32;
+			pMinHtPhy->field.MCS = 32;
+			DBGPRINT(RT_DEBUG_TRACE,("MlmeUpdateHtTxRates<=== Use Fixed MCS = %d\n",pMinHtPhy->field.MCS));
+		}
+
+		for (i=23; (CHAR)i >= 0; i--) // 3*3
+		{
+			j = i/8;
+			bitmask = (1<<(i-(j*8)));
+			if ( (pDesireHtPhy->MCSSet[j] & bitmask) && (pActiveHtPhy->MCSSet[j] & bitmask))
+			{
+				pMaxHtPhy->field.MCS = i;
+				pMinHtPhy->field.MCS = i;
+				break;
+			}
+			if (i==0)
+				break;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+
+	// Decide ht rate
+	pHtPhy->field.STBC = pMaxHtPhy->field.STBC;
+	pHtPhy->field.BW = pMaxHtPhy->field.BW;
+	pHtPhy->field.MODE = pMaxHtPhy->field.MODE;
+	pHtPhy->field.MCS = pMaxHtPhy->field.MCS;
+	pHtPhy->field.ShortGI = pMaxHtPhy->field.ShortGI;
+
+	// use default now. rt2860
+	if (pDesireHtPhy->MCSSet[0] != 0xff)
+		*auto_rate_cur_p = FALSE;
+	else
+		*auto_rate_cur_p = TRUE;
+
+	DBGPRINT(RT_DEBUG_TRACE, (" MlmeUpdateHtTxRates<---.AMsduSize = %d  \n", pAd->CommonCfg.DesiredHtPhy.AmsduSize ));
+	DBGPRINT(RT_DEBUG_TRACE,("TX: MCS[0] = %x (choose %d), BW = %d, ShortGI = %d, MODE = %d,  \n", pActiveHtPhy->MCSSet[0],pHtPhy->field.MCS,
+		pHtPhy->field.BW, pHtPhy->field.ShortGI, pHtPhy->field.MODE));
+	DBGPRINT(RT_DEBUG_TRACE,("MlmeUpdateHtTxRates<=== \n"));
+}
+#endif // DOT11_N_SUPPORT //
+
+// IRQL = DISPATCH_LEVEL
+VOID MlmeRadioOff(
+	IN PRTMP_ADAPTER pAd)
+{
+	RT28XX_MLME_RADIO_OFF(pAd);
+}
+
+// IRQL = DISPATCH_LEVEL
+VOID MlmeRadioOn(
+	IN PRTMP_ADAPTER pAd)
+{
+	RT28XX_MLME_RADIO_ON(pAd);
+}
+
+// ===========================================================================================
+// bss_table.c
+// ===========================================================================================
+
+
+/*! \brief initialize BSS table
+ *	\param p_tab pointer to the table
+ *	\return none
+ *	\pre
+ *	\post
+
+ IRQL = PASSIVE_LEVEL
+ IRQL = DISPATCH_LEVEL
+
+ */
+VOID BssTableInit(
+	IN BSS_TABLE *Tab)
+{
+	int i;
+
+	Tab->BssNr = 0;
+    Tab->BssOverlapNr = 0;
+	for (i = 0; i < MAX_LEN_OF_BSS_TABLE; i++)
+	{
+		NdisZeroMemory(&Tab->BssEntry[i], sizeof(BSS_ENTRY));
+		Tab->BssEntry[i].Rssi = -127;	// initial the rssi as a minimum value
+	}
+}
+
+#ifdef DOT11_N_SUPPORT
+VOID BATableInit(
+	IN PRTMP_ADAPTER pAd,
+    IN BA_TABLE *Tab)
+{
+	int i;
+
+	Tab->numAsOriginator = 0;
+	Tab->numAsRecipient = 0;
+	NdisAllocateSpinLock(&pAd->BATabLock);
+	for (i = 0; i < MAX_LEN_OF_BA_REC_TABLE; i++)
+	{
+		Tab->BARecEntry[i].REC_BA_Status = Recipient_NONE;
+		NdisAllocateSpinLock(&(Tab->BARecEntry[i].RxReRingLock));
+	}
+	for (i = 0; i < MAX_LEN_OF_BA_ORI_TABLE; i++)
+	{
+		Tab->BAOriEntry[i].ORI_BA_Status = Originator_NONE;
+	}
+}
+#endif // DOT11_N_SUPPORT //
+
+/*! \brief search the BSS table by SSID
+ *	\param p_tab pointer to the bss table
+ *	\param ssid SSID string
+ *	\return index of the table, BSS_NOT_FOUND if not in the table
+ *	\pre
+ *	\post
+ *	\note search by sequential search
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+ULONG BssTableSearch(
+	IN BSS_TABLE *Tab,
+	IN PUCHAR	 pBssid,
+	IN UCHAR	 Channel)
+{
+	UCHAR i;
+
+	for (i = 0; i < Tab->BssNr; i++)
+	{
+		//
+		// Some AP that support A/B/G mode that may used the same BSSID on 11A and 11B/G.
+		// We should distinguish this case.
+		//
+		if ((((Tab->BssEntry[i].Channel <= 14) && (Channel <= 14)) ||
+			 ((Tab->BssEntry[i].Channel > 14) && (Channel > 14))) &&
+			MAC_ADDR_EQUAL(Tab->BssEntry[i].Bssid, pBssid))
+		{
+			return i;
+		}
+	}
+	return (ULONG)BSS_NOT_FOUND;
+}
+
+ULONG BssSsidTableSearch(
+	IN BSS_TABLE *Tab,
+	IN PUCHAR	 pBssid,
+	IN PUCHAR	 pSsid,
+	IN UCHAR	 SsidLen,
+	IN UCHAR	 Channel)
+{
+	UCHAR i;
+
+	for (i = 0; i < Tab->BssNr; i++)
+	{
+		//
+		// Some AP that support A/B/G mode that may used the same BSSID on 11A and 11B/G.
+		// We should distinguish this case.
+		//
+		if ((((Tab->BssEntry[i].Channel <= 14) && (Channel <= 14)) ||
+			 ((Tab->BssEntry[i].Channel > 14) && (Channel > 14))) &&
+			MAC_ADDR_EQUAL(Tab->BssEntry[i].Bssid, pBssid) &&
+			SSID_EQUAL(pSsid, SsidLen, Tab->BssEntry[i].Ssid, Tab->BssEntry[i].SsidLen))
+		{
+			return i;
+		}
+	}
+	return (ULONG)BSS_NOT_FOUND;
+}
+
+ULONG BssTableSearchWithSSID(
+	IN BSS_TABLE *Tab,
+	IN PUCHAR	 Bssid,
+	IN PUCHAR	 pSsid,
+	IN UCHAR	 SsidLen,
+	IN UCHAR	 Channel)
+{
+	UCHAR i;
+
+	for (i = 0; i < Tab->BssNr; i++)
+	{
+		if ((((Tab->BssEntry[i].Channel <= 14) && (Channel <= 14)) ||
+			((Tab->BssEntry[i].Channel > 14) && (Channel > 14))) &&
+			MAC_ADDR_EQUAL(&(Tab->BssEntry[i].Bssid), Bssid) &&
+			(SSID_EQUAL(pSsid, SsidLen, Tab->BssEntry[i].Ssid, Tab->BssEntry[i].SsidLen) ||
+			(NdisEqualMemory(pSsid, ZeroSsid, SsidLen)) ||
+			(NdisEqualMemory(Tab->BssEntry[i].Ssid, ZeroSsid, Tab->BssEntry[i].SsidLen))))
+		{
+			return i;
+		}
+	}
+	return (ULONG)BSS_NOT_FOUND;
+}
+
+// IRQL = DISPATCH_LEVEL
+VOID BssTableDeleteEntry(
+	IN OUT	BSS_TABLE *Tab,
+	IN		PUCHAR	  pBssid,
+	IN		UCHAR	  Channel)
+{
+	UCHAR i, j;
+
+	for (i = 0; i < Tab->BssNr; i++)
+	{
+		if ((Tab->BssEntry[i].Channel == Channel) &&
+			(MAC_ADDR_EQUAL(Tab->BssEntry[i].Bssid, pBssid)))
+		{
+			for (j = i; j < Tab->BssNr - 1; j++)
+			{
+				NdisMoveMemory(&(Tab->BssEntry[j]), &(Tab->BssEntry[j + 1]), sizeof(BSS_ENTRY));
+			}
+			NdisZeroMemory(&(Tab->BssEntry[Tab->BssNr - 1]), sizeof(BSS_ENTRY));
+			Tab->BssNr -= 1;
+			return;
+		}
+	}
+}
+
+#ifdef DOT11_N_SUPPORT
+/*
+	========================================================================
+	Routine Description:
+		Delete the Originator Entry in BAtable. Or decrease numAs Originator by 1 if needed.
+
+	Arguments:
+	// IRQL = DISPATCH_LEVEL
+	========================================================================
+*/
+VOID BATableDeleteORIEntry(
+	IN OUT	PRTMP_ADAPTER pAd,
+	IN		BA_ORI_ENTRY	*pBAORIEntry)
+{
+
+	if (pBAORIEntry->ORI_BA_Status != Originator_NONE)
+	{
+		NdisAcquireSpinLock(&pAd->BATabLock);
+		if (pBAORIEntry->ORI_BA_Status == Originator_Done)
+		{
+			pAd->BATable.numAsOriginator -= 1;
+			DBGPRINT(RT_DEBUG_TRACE, ("BATableDeleteORIEntry numAsOriginator= %ld\n", pAd->BATable.numAsRecipient));
+			// Erase Bitmap flag.
+		}
+		pAd->MacTab.Content[pBAORIEntry->Wcid].TXBAbitmap &= (~(1<<(pBAORIEntry->TID) ));	// If STA mode,  erase flag here
+		pAd->MacTab.Content[pBAORIEntry->Wcid].BAOriWcidArray[pBAORIEntry->TID] = 0;	// If STA mode,  erase flag here
+		pBAORIEntry->ORI_BA_Status = Originator_NONE;
+		pBAORIEntry->Token = 1;
+		// Not clear Sequence here.
+		NdisReleaseSpinLock(&pAd->BATabLock);
+	}
+}
+#endif // DOT11_N_SUPPORT //
+
+/*! \brief
+ *	\param
+ *	\return
+ *	\pre
+ *	\post
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+VOID BssEntrySet(
+	IN PRTMP_ADAPTER	pAd,
+	OUT BSS_ENTRY *pBss,
+	IN PUCHAR pBssid,
+	IN CHAR Ssid[],
+	IN UCHAR SsidLen,
+	IN UCHAR BssType,
+	IN USHORT BeaconPeriod,
+	IN PCF_PARM pCfParm,
+	IN USHORT AtimWin,
+	IN USHORT CapabilityInfo,
+	IN UCHAR SupRate[],
+	IN UCHAR SupRateLen,
+	IN UCHAR ExtRate[],
+	IN UCHAR ExtRateLen,
+	IN HT_CAPABILITY_IE *pHtCapability,
+	IN ADD_HT_INFO_IE *pAddHtInfo,	// AP might use this additional ht info IE
+	IN UCHAR			HtCapabilityLen,
+	IN UCHAR			AddHtInfoLen,
+	IN UCHAR			NewExtChanOffset,
+	IN UCHAR Channel,
+	IN CHAR Rssi,
+	IN LARGE_INTEGER TimeStamp,
+	IN UCHAR CkipFlag,
+	IN PEDCA_PARM pEdcaParm,
+	IN PQOS_CAPABILITY_PARM pQosCapability,
+	IN PQBSS_LOAD_PARM pQbssLoad,
+	IN USHORT LengthVIE,
+	IN PNDIS_802_11_VARIABLE_IEs pVIE)
+{
+	COPY_MAC_ADDR(pBss->Bssid, pBssid);
+	// Default Hidden SSID to be TRUE, it will be turned to FALSE after coping SSID
+	pBss->Hidden = 1;
+	if (SsidLen > 0)
+	{
+		// For hidden SSID AP, it might send beacon with SSID len equal to 0
+		// Or send beacon /probe response with SSID len matching real SSID length,
+		// but SSID is all zero. such as "00-00-00-00" with length 4.
+		// We have to prevent this case overwrite correct table
+		if (NdisEqualMemory(Ssid, ZeroSsid, SsidLen) == 0)
+		{
+		    NdisZeroMemory(pBss->Ssid, MAX_LEN_OF_SSID);
+			NdisMoveMemory(pBss->Ssid, Ssid, SsidLen);
+			pBss->SsidLen = SsidLen;
+			pBss->Hidden = 0;
+		}
+	}
+	else
+		pBss->SsidLen = 0;
+	pBss->BssType = BssType;
+	pBss->BeaconPeriod = BeaconPeriod;
+	if (BssType == BSS_INFRA)
+	{
+		if (pCfParm->bValid)
+		{
+			pBss->CfpCount = pCfParm->CfpCount;
+			pBss->CfpPeriod = pCfParm->CfpPeriod;
+			pBss->CfpMaxDuration = pCfParm->CfpMaxDuration;
+			pBss->CfpDurRemaining = pCfParm->CfpDurRemaining;
+		}
+	}
+	else
+	{
+		pBss->AtimWin = AtimWin;
+	}
+
+	pBss->CapabilityInfo = CapabilityInfo;
+	// The privacy bit indicate security is ON, it maight be WEP, TKIP or AES
+	// Combine with AuthMode, they will decide the connection methods.
+	pBss->Privacy = CAP_IS_PRIVACY_ON(pBss->CapabilityInfo);
+	ASSERT(SupRateLen <= MAX_LEN_OF_SUPPORTED_RATES);
+	if (SupRateLen <= MAX_LEN_OF_SUPPORTED_RATES)
+		NdisMoveMemory(pBss->SupRate, SupRate, SupRateLen);
+	else
+		NdisMoveMemory(pBss->SupRate, SupRate, MAX_LEN_OF_SUPPORTED_RATES);
+	pBss->SupRateLen = SupRateLen;
+	ASSERT(ExtRateLen <= MAX_LEN_OF_SUPPORTED_RATES);
+	NdisMoveMemory(pBss->ExtRate, ExtRate, ExtRateLen);
+	NdisMoveMemory(&pBss->HtCapability, pHtCapability, HtCapabilityLen);
+	NdisMoveMemory(&pBss->AddHtInfo, pAddHtInfo, AddHtInfoLen);
+	pBss->NewExtChanOffset = NewExtChanOffset;
+	pBss->ExtRateLen = ExtRateLen;
+	pBss->Channel = Channel;
+	pBss->CentralChannel = Channel;
+	pBss->Rssi = Rssi;
+	// Update CkipFlag. if not exists, the value is 0x0
+	pBss->CkipFlag = CkipFlag;
+
+	// New for microsoft Fixed IEs
+	NdisMoveMemory(pBss->FixIEs.Timestamp, &TimeStamp, 8);
+	pBss->FixIEs.BeaconInterval = BeaconPeriod;
+	pBss->FixIEs.Capabilities = CapabilityInfo;
+
+	// New for microsoft Variable IEs
+	if (LengthVIE != 0)
+	{
+		pBss->VarIELen = LengthVIE;
+		NdisMoveMemory(pBss->VarIEs, pVIE, pBss->VarIELen);
+	}
+	else
+	{
+		pBss->VarIELen = 0;
+	}
+
+	pBss->AddHtInfoLen = 0;
+	pBss->HtCapabilityLen = 0;
+#ifdef DOT11_N_SUPPORT
+	if (HtCapabilityLen> 0)
+	{
+		pBss->HtCapabilityLen = HtCapabilityLen;
+		NdisMoveMemory(&pBss->HtCapability, pHtCapability, HtCapabilityLen);
+		if (AddHtInfoLen > 0)
+		{
+			pBss->AddHtInfoLen = AddHtInfoLen;
+			NdisMoveMemory(&pBss->AddHtInfo, pAddHtInfo, AddHtInfoLen);
+
+	 			if ((pAddHtInfo->ControlChan > 2)&& (pAddHtInfo->AddHtInfo.ExtChanOffset == EXTCHA_BELOW) && (pHtCapability->HtCapInfo.ChannelWidth == BW_40))
+	 			{
+	 				pBss->CentralChannel = pAddHtInfo->ControlChan - 2;
+	 			}
+	 			else if ((pAddHtInfo->AddHtInfo.ExtChanOffset == EXTCHA_ABOVE) && (pHtCapability->HtCapInfo.ChannelWidth == BW_40))
+				{
+		 				pBss->CentralChannel = pAddHtInfo->ControlChan + 2;
+				}
+		}
+	}
+#endif // DOT11_N_SUPPORT //
+
+	BssCipherParse(pBss);
+
+	// new for QOS
+	if (pEdcaParm)
+		NdisMoveMemory(&pBss->EdcaParm, pEdcaParm, sizeof(EDCA_PARM));
+	else
+		pBss->EdcaParm.bValid = FALSE;
+	if (pQosCapability)
+		NdisMoveMemory(&pBss->QosCapability, pQosCapability, sizeof(QOS_CAPABILITY_PARM));
+	else
+		pBss->QosCapability.bValid = FALSE;
+	if (pQbssLoad)
+		NdisMoveMemory(&pBss->QbssLoad, pQbssLoad, sizeof(QBSS_LOAD_PARM));
+	else
+		pBss->QbssLoad.bValid = FALSE;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		PEID_STRUCT     pEid;
+		USHORT          Length = 0;
+
+
+		NdisZeroMemory(&pBss->WpaIE.IE[0], MAX_CUSTOM_LEN);
+		NdisZeroMemory(&pBss->RsnIE.IE[0], MAX_CUSTOM_LEN);
+#ifdef EXT_BUILD_CHANNEL_LIST
+		NdisZeroMemory(&pBss->CountryString[0], 3);
+		pBss->bHasCountryIE = FALSE;
+#endif // EXT_BUILD_CHANNEL_LIST //
+		pEid = (PEID_STRUCT) pVIE;
+		while ((Length + 2 + (USHORT)pEid->Len) <= LengthVIE)
+		{
+			switch(pEid->Eid)
+			{
+				case IE_WPA:
+					if (NdisEqualMemory(pEid->Octet, WPA_OUI, 4))
+					{
+						if ((pEid->Len + 2) > MAX_CUSTOM_LEN)
+						{
+							pBss->WpaIE.IELen = 0;
+							break;
+						}
+						pBss->WpaIE.IELen = pEid->Len + 2;
+						NdisMoveMemory(pBss->WpaIE.IE, pEid, pBss->WpaIE.IELen);
+					}
+					break;
+                case IE_RSN:
+                    if (NdisEqualMemory(pEid->Octet + 2, RSN_OUI, 3))
+					{
+						if ((pEid->Len + 2) > MAX_CUSTOM_LEN)
+						{
+							pBss->RsnIE.IELen = 0;
+							break;
+						}
+						pBss->RsnIE.IELen = pEid->Len + 2;
+						NdisMoveMemory(pBss->RsnIE.IE, pEid, pBss->RsnIE.IELen);
+			}
+				break;
+#ifdef EXT_BUILD_CHANNEL_LIST
+				case IE_COUNTRY:
+					NdisMoveMemory(&pBss->CountryString[0], pEid->Octet, 3);
+					pBss->bHasCountryIE = TRUE;
+					break;
+#endif // EXT_BUILD_CHANNEL_LIST //
+            }
+			Length = Length + 2 + (USHORT)pEid->Len;  // Eid[1] + Len[1]+ content[Len]
+			pEid = (PEID_STRUCT)((UCHAR*)pEid + 2 + pEid->Len);
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+}
+
+/*!
+ *	\brief insert an entry into the bss table
+ *	\param p_tab The BSS table
+ *	\param Bssid BSSID
+ *	\param ssid SSID
+ *	\param ssid_len Length of SSID
+ *	\param bss_type
+ *	\param beacon_period
+ *	\param timestamp
+ *	\param p_cf
+ *	\param atim_win
+ *	\param cap
+ *	\param rates
+ *	\param rates_len
+ *	\param channel_idx
+ *	\return none
+ *	\pre
+ *	\post
+ *	\note If SSID is identical, the old entry will be replaced by the new one
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+ULONG BssTableSetEntry(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT BSS_TABLE *Tab,
+	IN PUCHAR pBssid,
+	IN CHAR Ssid[],
+	IN UCHAR SsidLen,
+	IN UCHAR BssType,
+	IN USHORT BeaconPeriod,
+	IN CF_PARM *CfParm,
+	IN USHORT AtimWin,
+	IN USHORT CapabilityInfo,
+	IN UCHAR SupRate[],
+	IN UCHAR SupRateLen,
+	IN UCHAR ExtRate[],
+	IN UCHAR ExtRateLen,
+	IN HT_CAPABILITY_IE *pHtCapability,
+	IN ADD_HT_INFO_IE *pAddHtInfo,	// AP might use this additional ht info IE
+	IN UCHAR			HtCapabilityLen,
+	IN UCHAR			AddHtInfoLen,
+	IN UCHAR			NewExtChanOffset,
+	IN UCHAR ChannelNo,
+	IN CHAR Rssi,
+	IN LARGE_INTEGER TimeStamp,
+	IN UCHAR CkipFlag,
+	IN PEDCA_PARM pEdcaParm,
+	IN PQOS_CAPABILITY_PARM pQosCapability,
+	IN PQBSS_LOAD_PARM pQbssLoad,
+	IN USHORT LengthVIE,
+	IN PNDIS_802_11_VARIABLE_IEs pVIE)
+{
+	ULONG	Idx;
+
+	Idx = BssTableSearchWithSSID(Tab, pBssid,  Ssid, SsidLen, ChannelNo);
+	if (Idx == BSS_NOT_FOUND)
+	{
+		if (Tab->BssNr >= MAX_LEN_OF_BSS_TABLE)
+	    {
+			//
+			// It may happen when BSS Table was full.
+			// The desired AP will not be added into BSS Table
+			// In this case, if we found the desired AP then overwrite BSS Table.
+			//
+			if(!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+			{
+				if (MAC_ADDR_EQUAL(pAd->MlmeAux.Bssid, pBssid) ||
+					SSID_EQUAL(pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen, Ssid, SsidLen))
+				{
+					Idx = Tab->BssOverlapNr;
+					BssEntrySet(pAd, &Tab->BssEntry[Idx], pBssid, Ssid, SsidLen, BssType, BeaconPeriod, CfParm, AtimWin,
+						CapabilityInfo, SupRate, SupRateLen, ExtRate, ExtRateLen,pHtCapability, pAddHtInfo,HtCapabilityLen, AddHtInfoLen,
+						NewExtChanOffset, ChannelNo, Rssi, TimeStamp, CkipFlag, pEdcaParm, pQosCapability, pQbssLoad, LengthVIE, pVIE);
+                    Tab->BssOverlapNr = (Tab->BssOverlapNr++) % MAX_LEN_OF_BSS_TABLE;
+				}
+				return Idx;
+			}
+			else
+			{
+			return BSS_NOT_FOUND;
+			}
+		}
+		Idx = Tab->BssNr;
+		BssEntrySet(pAd, &Tab->BssEntry[Idx], pBssid, Ssid, SsidLen, BssType, BeaconPeriod, CfParm, AtimWin,
+					CapabilityInfo, SupRate, SupRateLen, ExtRate, ExtRateLen,pHtCapability, pAddHtInfo,HtCapabilityLen, AddHtInfoLen,
+					NewExtChanOffset, ChannelNo, Rssi, TimeStamp, CkipFlag, pEdcaParm, pQosCapability, pQbssLoad, LengthVIE, pVIE);
+		Tab->BssNr++;
+	}
+	else
+	{
+		BssEntrySet(pAd, &Tab->BssEntry[Idx], pBssid, Ssid, SsidLen, BssType, BeaconPeriod,CfParm, AtimWin,
+					CapabilityInfo, SupRate, SupRateLen, ExtRate, ExtRateLen,pHtCapability, pAddHtInfo,HtCapabilityLen, AddHtInfoLen,
+					NewExtChanOffset, ChannelNo, Rssi, TimeStamp, CkipFlag, pEdcaParm, pQosCapability, pQbssLoad, LengthVIE, pVIE);
+	}
+
+	return Idx;
+}
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef DOT11_N_SUPPORT
+#ifdef DOT11N_DRAFT3
+VOID  TriEventInit(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	UCHAR		i;
+
+	for (i = 0;i < MAX_TRIGGER_EVENT;i++)
+		pAd->CommonCfg.TriggerEventTab.EventA[i].bValid = FALSE;
+
+	pAd->CommonCfg.TriggerEventTab.EventANo = 0;
+	pAd->CommonCfg.TriggerEventTab.EventBCountDown = 0;
+}
+
+ULONG TriEventTableSetEntry(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT TRIGGER_EVENT_TAB *Tab,
+	IN PUCHAR pBssid,
+	IN HT_CAPABILITY_IE *pHtCapability,
+	IN UCHAR			HtCapabilityLen,
+	IN UCHAR			RegClass,
+	IN UCHAR ChannelNo)
+{
+	// Event A
+	if (HtCapabilityLen == 0)
+	{
+		if (Tab->EventANo < MAX_TRIGGER_EVENT)
+		{
+			RTMPMoveMemory(Tab->EventA[Tab->EventANo].BSSID, pBssid, 6);
+			Tab->EventA[Tab->EventANo].bValid = TRUE;
+			Tab->EventA[Tab->EventANo].Channel = ChannelNo;
+			Tab->EventA[Tab->EventANo].CDCounter = pAd->CommonCfg.Dot11BssWidthChanTranDelay;
+			if (RegClass != 0)
+			{
+				// Beacon has Regulatory class IE. So use beacon's
+				Tab->EventA[Tab->EventANo].RegClass = RegClass;
+			}
+			else
+			{
+				// Use Station's Regulatory class instead.
+				if (pAd->StaActive.SupportedHtPhy.bHtEnable == TRUE)
+				{
+					if (pAd->CommonCfg.CentralChannel > pAd->CommonCfg.Channel)
+					{
+						Tab->EventA[Tab->EventANo].RegClass = 32;
+					}
+					else if (pAd->CommonCfg.CentralChannel < pAd->CommonCfg.Channel)
+						Tab->EventA[Tab->EventANo].RegClass = 33;
+				}
+				else
+					Tab->EventA[Tab->EventANo].RegClass = ??;
+
+			}
+
+			Tab->EventANo ++;
+		}
+	}
+	else if (pHtCapability->HtCapInfo.Intolerant40)
+	{
+		Tab->EventBCountDown = pAd->CommonCfg.Dot11BssWidthChanTranDelay;
+	}
+
+}
+
+/*
+	========================================================================
+	Routine Description:
+		Trigger Event table Maintainence called once every second.
+
+	Arguments:
+	// IRQL = DISPATCH_LEVEL
+	========================================================================
+*/
+VOID TriEventCounterMaintenance(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	UCHAR		i;
+	BOOLEAN			bNotify = FALSE;
+	for (i = 0;i < MAX_TRIGGER_EVENT;i++)
+	{
+		if (pAd->CommonCfg.TriggerEventTab.EventA[i].bValid && (pAd->CommonCfg.TriggerEventTab.EventA[i].CDCounter > 0))
+		{
+			pAd->CommonCfg.TriggerEventTab.EventA[i].CDCounter--;
+			if (pAd->CommonCfg.TriggerEventTab.EventA[i].CDCounter == 0)
+			{
+				pAd->CommonCfg.TriggerEventTab.EventA[i].bValid = FALSE;
+				pAd->CommonCfg.TriggerEventTab.EventANo --;
+				// Need to send 20/40 Coexistence Notify frame if has status change.
+				bNotify = TRUE;
+			}
+		}
+	}
+	if (pAd->CommonCfg.TriggerEventTab.EventBCountDown > 0)
+	{
+		pAd->CommonCfg.TriggerEventTab.EventBCountDown--;
+		if (pAd->CommonCfg.TriggerEventTab.EventBCountDown == 0)
+			bNotify = TRUE;
+	}
+
+	if (bNotify == TRUE)
+		Update2040CoexistFrameAndNotify(pAd, BSSID_WCID, TRUE);
+}
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+
+// IRQL = DISPATCH_LEVEL
+VOID BssTableSsidSort(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT BSS_TABLE *OutTab,
+	IN	CHAR Ssid[],
+	IN	UCHAR SsidLen)
+{
+	INT i;
+	BssTableInit(OutTab);
+
+	for (i = 0; i < pAd->ScanTab.BssNr; i++)
+	{
+		BSS_ENTRY *pInBss = &pAd->ScanTab.BssEntry[i];
+		BOOLEAN	bIsHiddenApIncluded = FALSE;
+
+		if (((pAd->CommonCfg.bIEEE80211H == 1) &&
+            (pAd->MlmeAux.Channel > 14) &&
+             RadarChannelCheck(pAd, pInBss->Channel))
+#ifdef CARRIER_DETECTION_SUPPORT // Roger sync Carrier
+             || (pAd->CommonCfg.CarrierDetect.Enable == TRUE)
+#endif // CARRIER_DETECTION_SUPPORT //
+            )
+		{
+			if (pInBss->Hidden)
+				bIsHiddenApIncluded = TRUE;
+		}
+
+		if ((pInBss->BssType == pAd->StaCfg.BssType) &&
+			(SSID_EQUAL(Ssid, SsidLen, pInBss->Ssid, pInBss->SsidLen) || bIsHiddenApIncluded))
+		{
+			BSS_ENTRY *pOutBss = &OutTab->BssEntry[OutTab->BssNr];
+
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+			// If no Country IE exists no Connection will be established when IEEE80211dClientMode is strict.
+			if ((pAd->StaCfg.IEEE80211dClientMode == Rt802_11_D_Strict) &&
+				(pInBss->bHasCountryIE == FALSE))
+			{
+				DBGPRINT(RT_DEBUG_TRACE,("StaCfg.IEEE80211dClientMode == Rt802_11_D_Strict, but this AP doesn't have country IE.\n"));
+				continue;
+			}
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+#ifdef DOT11_N_SUPPORT
+			// 2.4G/5G N only mode
+			if ((pInBss->HtCapabilityLen == 0) &&
+				((pAd->CommonCfg.PhyMode == PHY_11N_2_4G) || (pAd->CommonCfg.PhyMode == PHY_11N_5G)))
+			{
+				DBGPRINT(RT_DEBUG_TRACE,("STA is in N-only Mode, this AP don't have Ht capability in Beacon.\n"));
+				continue;
+			}
+#endif // DOT11_N_SUPPORT //
+
+			// New for WPA2
+			// Check the Authmode first
+			if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+			{
+				// Check AuthMode and AuthModeAux for matching, in case AP support dual-mode
+				if ((pAd->StaCfg.AuthMode != pInBss->AuthMode) && (pAd->StaCfg.AuthMode != pInBss->AuthModeAux))
+					// None matched
+					continue;
+
+				// Check cipher suite, AP must have more secured cipher than station setting
+				if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK))
+				{
+					// If it's not mixed mode, we should only let BSS pass with the same encryption
+					if (pInBss->WPA.bMixMode == FALSE)
+						if (pAd->StaCfg.WepStatus != pInBss->WPA.GroupCipher)
+							continue;
+
+					// check group cipher
+					if (pAd->StaCfg.WepStatus < pInBss->WPA.GroupCipher)
+						continue;
+
+					// check pairwise cipher, skip if none matched
+					// If profile set to AES, let it pass without question.
+					// If profile set to TKIP, we must find one mateched
+					if ((pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA.PairCipher) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA.PairCipherAux))
+						continue;
+				}
+				else if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK))
+				{
+					// If it's not mixed mode, we should only let BSS pass with the same encryption
+					if (pInBss->WPA2.bMixMode == FALSE)
+						if (pAd->StaCfg.WepStatus != pInBss->WPA2.GroupCipher)
+							continue;
+
+					// check group cipher
+					if (pAd->StaCfg.WepStatus < pInBss->WPA2.GroupCipher)
+						continue;
+
+					// check pairwise cipher, skip if none matched
+					// If profile set to AES, let it pass without question.
+					// If profile set to TKIP, we must find one mateched
+					if ((pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA2.PairCipher) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA2.PairCipherAux))
+						continue;
+				}
+			}
+			// Bss Type matched, SSID matched.
+			// We will check wepstatus for qualification Bss
+			else if (pAd->StaCfg.WepStatus != pInBss->WepStatus)
+			{
+				DBGPRINT(RT_DEBUG_TRACE,("StaCfg.WepStatus=%d, while pInBss->WepStatus=%d\n", pAd->StaCfg.WepStatus, pInBss->WepStatus));
+				//
+				// For the SESv2 case, we will not qualify WepStatus.
+				//
+				if (!pInBss->bSES)
+					continue;
+			}
+
+			// Since the AP is using hidden SSID, and we are trying to connect to ANY
+			// It definitely will fail. So, skip it.
+			// CCX also require not even try to connect it!!
+			if (SsidLen == 0)
+				continue;
+
+#ifdef DOT11_N_SUPPORT
+			// If both station and AP use 40MHz, still need to check if the 40MHZ band's legality in my country region
+			// If this 40MHz wideband is not allowed in my country list, use bandwidth 20MHZ instead,
+			if ((pInBss->CentralChannel != pInBss->Channel) &&
+				(pAd->CommonCfg.RegTransmitSetting.field.BW == BW_40))
+			{
+				if (RTMPCheckChannel(pAd, pInBss->CentralChannel, pInBss->Channel) == FALSE)
+				{
+					pAd->CommonCfg.RegTransmitSetting.field.BW = BW_20;
+					SetCommonHT(pAd);
+					pAd->CommonCfg.RegTransmitSetting.field.BW = BW_40;
+				}
+				else
+				{
+					if (pAd->CommonCfg.DesiredHtPhy.ChannelWidth == BAND_WIDTH_20)
+					{
+						SetCommonHT(pAd);
+					}
+				}
+			}
+#endif // DOT11_N_SUPPORT //
+
+			// copy matching BSS from InTab to OutTab
+			NdisMoveMemory(pOutBss, pInBss, sizeof(BSS_ENTRY));
+
+			OutTab->BssNr++;
+		}
+		else if ((pInBss->BssType == pAd->StaCfg.BssType) && (SsidLen == 0))
+		{
+			BSS_ENTRY *pOutBss = &OutTab->BssEntry[OutTab->BssNr];
+
+
+#ifdef DOT11_N_SUPPORT
+			// 2.4G/5G N only mode
+			if ((pInBss->HtCapabilityLen == 0) &&
+				((pAd->CommonCfg.PhyMode == PHY_11N_2_4G) || (pAd->CommonCfg.PhyMode == PHY_11N_5G)))
+			{
+				DBGPRINT(RT_DEBUG_TRACE,("STA is in N-only Mode, this AP don't have Ht capability in Beacon.\n"));
+				continue;
+			}
+#endif // DOT11_N_SUPPORT //
+
+			// New for WPA2
+			// Check the Authmode first
+			if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+			{
+				// Check AuthMode and AuthModeAux for matching, in case AP support dual-mode
+				if ((pAd->StaCfg.AuthMode != pInBss->AuthMode) && (pAd->StaCfg.AuthMode != pInBss->AuthModeAux))
+					// None matched
+					continue;
+
+				// Check cipher suite, AP must have more secured cipher than station setting
+				if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK))
+				{
+					// If it's not mixed mode, we should only let BSS pass with the same encryption
+					if (pInBss->WPA.bMixMode == FALSE)
+						if (pAd->StaCfg.WepStatus != pInBss->WPA.GroupCipher)
+							continue;
+
+					// check group cipher
+					if (pAd->StaCfg.WepStatus < pInBss->WPA.GroupCipher)
+						continue;
+
+					// check pairwise cipher, skip if none matched
+					// If profile set to AES, let it pass without question.
+					// If profile set to TKIP, we must find one mateched
+					if ((pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA.PairCipher) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA.PairCipherAux))
+						continue;
+				}
+				else if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK))
+				{
+					// If it's not mixed mode, we should only let BSS pass with the same encryption
+					if (pInBss->WPA2.bMixMode == FALSE)
+						if (pAd->StaCfg.WepStatus != pInBss->WPA2.GroupCipher)
+							continue;
+
+					// check group cipher
+					if (pAd->StaCfg.WepStatus < pInBss->WPA2.GroupCipher)
+						continue;
+
+					// check pairwise cipher, skip if none matched
+					// If profile set to AES, let it pass without question.
+					// If profile set to TKIP, we must find one mateched
+					if ((pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA2.PairCipher) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA2.PairCipherAux))
+						continue;
+				}
+			}
+			// Bss Type matched, SSID matched.
+			// We will check wepstatus for qualification Bss
+			else if (pAd->StaCfg.WepStatus != pInBss->WepStatus)
+					continue;
+
+#ifdef DOT11_N_SUPPORT
+			// If both station and AP use 40MHz, still need to check if the 40MHZ band's legality in my country region
+			// If this 40MHz wideband is not allowed in my country list, use bandwidth 20MHZ instead,
+			if ((pInBss->CentralChannel != pInBss->Channel) &&
+				(pAd->CommonCfg.RegTransmitSetting.field.BW == BW_40))
+			{
+				if (RTMPCheckChannel(pAd, pInBss->CentralChannel, pInBss->Channel) == FALSE)
+				{
+					pAd->CommonCfg.RegTransmitSetting.field.BW = BW_20;
+					SetCommonHT(pAd);
+					pAd->CommonCfg.RegTransmitSetting.field.BW = BW_40;
+				}
+			}
+#endif // DOT11_N_SUPPORT //
+
+			// copy matching BSS from InTab to OutTab
+			NdisMoveMemory(pOutBss, pInBss, sizeof(BSS_ENTRY));
+
+			OutTab->BssNr++;
+		}
+
+		if (OutTab->BssNr >= MAX_LEN_OF_BSS_TABLE)
+			break;
+	}
+
+	BssTableSortByRssi(OutTab);
+}
+
+
+// IRQL = DISPATCH_LEVEL
+VOID BssTableSortByRssi(
+	IN OUT BSS_TABLE *OutTab)
+{
+	INT 	  i, j;
+	BSS_ENTRY TmpBss;
+
+	for (i = 0; i < OutTab->BssNr - 1; i++)
+	{
+		for (j = i+1; j < OutTab->BssNr; j++)
+		{
+			if (OutTab->BssEntry[j].Rssi > OutTab->BssEntry[i].Rssi)
+			{
+				NdisMoveMemory(&TmpBss, &OutTab->BssEntry[j], sizeof(BSS_ENTRY));
+				NdisMoveMemory(&OutTab->BssEntry[j], &OutTab->BssEntry[i], sizeof(BSS_ENTRY));
+				NdisMoveMemory(&OutTab->BssEntry[i], &TmpBss, sizeof(BSS_ENTRY));
+			}
+		}
+	}
+}
+#endif // CONFIG_STA_SUPPORT //
+
+
+VOID BssCipherParse(
+	IN OUT	PBSS_ENTRY	pBss)
+{
+	PEID_STRUCT 		 pEid;
+	PUCHAR				pTmp;
+	PRSN_IE_HEADER_STRUCT			pRsnHeader;
+	PCIPHER_SUITE_STRUCT			pCipher;
+	PAKM_SUITE_STRUCT				pAKM;
+	USHORT							Count;
+	INT								Length;
+	NDIS_802_11_ENCRYPTION_STATUS	TmpCipher;
+
+	//
+	// WepStatus will be reset later, if AP announce TKIP or AES on the beacon frame.
+	//
+	if (pBss->Privacy)
+	{
+		pBss->WepStatus 	= Ndis802_11WEPEnabled;
+	}
+	else
+	{
+		pBss->WepStatus 	= Ndis802_11WEPDisabled;
+	}
+	// Set default to disable & open authentication before parsing variable IE
+	pBss->AuthMode		= Ndis802_11AuthModeOpen;
+	pBss->AuthModeAux	= Ndis802_11AuthModeOpen;
+
+	// Init WPA setting
+	pBss->WPA.PairCipher	= Ndis802_11WEPDisabled;
+	pBss->WPA.PairCipherAux = Ndis802_11WEPDisabled;
+	pBss->WPA.GroupCipher	= Ndis802_11WEPDisabled;
+	pBss->WPA.RsnCapability = 0;
+	pBss->WPA.bMixMode		= FALSE;
+
+	// Init WPA2 setting
+	pBss->WPA2.PairCipher	 = Ndis802_11WEPDisabled;
+	pBss->WPA2.PairCipherAux = Ndis802_11WEPDisabled;
+	pBss->WPA2.GroupCipher	 = Ndis802_11WEPDisabled;
+	pBss->WPA2.RsnCapability = 0;
+	pBss->WPA2.bMixMode 	 = FALSE;
+
+
+	Length = (INT) pBss->VarIELen;
+
+	while (Length > 0)
+	{
+		// Parse cipher suite base on WPA1 & WPA2, they should be parsed differently
+		pTmp = ((PUCHAR) pBss->VarIEs) + pBss->VarIELen - Length;
+		pEid = (PEID_STRUCT) pTmp;
+		switch (pEid->Eid)
+		{
+			case IE_WPA:
+				//Parse Cisco IE_WPA (LEAP, CCKM, etc.)
+				if ( NdisEqualMemory((pTmp+8), CISCO_OUI, 3))
+				{
+					pTmp   += 11;
+					switch (*pTmp)
+					{
+						case 1:
+						case 5:	// Although WEP is not allowed in WPA related auth mode, we parse it anyway
+							pBss->WepStatus = Ndis802_11Encryption1Enabled;
+							pBss->WPA.PairCipher = Ndis802_11Encryption1Enabled;
+							pBss->WPA.GroupCipher = Ndis802_11Encryption1Enabled;
+							break;
+						case 2:
+							pBss->WepStatus = Ndis802_11Encryption2Enabled;
+							pBss->WPA.PairCipher = Ndis802_11Encryption1Enabled;
+							pBss->WPA.GroupCipher = Ndis802_11Encryption1Enabled;
+							break;
+						case 4:
+							pBss->WepStatus = Ndis802_11Encryption3Enabled;
+							pBss->WPA.PairCipher = Ndis802_11Encryption1Enabled;
+							pBss->WPA.GroupCipher = Ndis802_11Encryption1Enabled;
+							break;
+						default:
+							break;
+					}
+
+					// if Cisco IE_WPA, break
+					break;
+				}
+				else if (NdisEqualMemory(pEid->Octet, SES_OUI, 3) && (pEid->Len == 7))
+				{
+					pBss->bSES = TRUE;
+					break;
+				}
+				else if (NdisEqualMemory(pEid->Octet, WPA_OUI, 4) != 1)
+				{
+					// if unsupported vendor specific IE
+					break;
+				}
+				// Skip OUI, version, and multicast suite
+				// This part should be improved in the future when AP supported multiple cipher suite.
+				// For now, it's OK since almost all APs have fixed cipher suite supported.
+				// pTmp = (PUCHAR) pEid->Octet;
+				pTmp   += 11;
+
+				// Cipher Suite Selectors from Spec P802.11i/D3.2 P26.
+				//	Value	   Meaning
+				//	0			None
+				//	1			WEP-40
+				//	2			Tkip
+				//	3			WRAP
+				//	4			AES
+				//	5			WEP-104
+				// Parse group cipher
+				switch (*pTmp)
+				{
+					case 1:
+					case 5:	// Although WEP is not allowed in WPA related auth mode, we parse it anyway
+						pBss->WPA.GroupCipher = Ndis802_11Encryption1Enabled;
+						break;
+					case 2:
+						pBss->WPA.GroupCipher = Ndis802_11Encryption2Enabled;
+						break;
+					case 4:
+						pBss->WPA.GroupCipher = Ndis802_11Encryption3Enabled;
+						break;
+					default:
+						break;
+				}
+				// number of unicast suite
+				pTmp   += 1;
+
+				// skip all unicast cipher suites
+				//Count = *(PUSHORT) pTmp;
+				Count = (pTmp[1]<<8) + pTmp[0];
+				pTmp   += sizeof(USHORT);
+
+				// Parsing all unicast cipher suite
+				while (Count > 0)
+				{
+					// Skip OUI
+					pTmp += 3;
+					TmpCipher = Ndis802_11WEPDisabled;
+					switch (*pTmp)
+					{
+						case 1:
+						case 5: // Although WEP is not allowed in WPA related auth mode, we parse it anyway
+							TmpCipher = Ndis802_11Encryption1Enabled;
+							break;
+						case 2:
+							TmpCipher = Ndis802_11Encryption2Enabled;
+							break;
+						case 4:
+							TmpCipher = Ndis802_11Encryption3Enabled;
+							break;
+						default:
+							break;
+					}
+					if (TmpCipher > pBss->WPA.PairCipher)
+					{
+						// Move the lower cipher suite to PairCipherAux
+						pBss->WPA.PairCipherAux = pBss->WPA.PairCipher;
+						pBss->WPA.PairCipher	= TmpCipher;
+					}
+					else
+					{
+						pBss->WPA.PairCipherAux = TmpCipher;
+					}
+					pTmp++;
+					Count--;
+				}
+
+				// 4. get AKM suite counts
+				//Count	= *(PUSHORT) pTmp;
+				Count = (pTmp[1]<<8) + pTmp[0];
+				pTmp   += sizeof(USHORT);
+				pTmp   += 3;
+
+				switch (*pTmp)
+				{
+					case 1:
+						// Set AP support WPA mode
+						if (pBss->AuthMode == Ndis802_11AuthModeOpen)
+							pBss->AuthMode = Ndis802_11AuthModeWPA;
+						else
+							pBss->AuthModeAux = Ndis802_11AuthModeWPA;
+						break;
+					case 2:
+						// Set AP support WPA mode
+						if (pBss->AuthMode == Ndis802_11AuthModeOpen)
+							pBss->AuthMode = Ndis802_11AuthModeWPAPSK;
+						else
+							pBss->AuthModeAux = Ndis802_11AuthModeWPAPSK;
+						break;
+					default:
+						break;
+				}
+				pTmp   += 1;
+
+				// Fixed for WPA-None
+				if (pBss->BssType == BSS_ADHOC)
+				{
+					pBss->AuthMode	  = Ndis802_11AuthModeWPANone;
+					pBss->AuthModeAux = Ndis802_11AuthModeWPANone;
+					pBss->WepStatus   = pBss->WPA.GroupCipher;
+					// Patched bugs for old driver
+					if (pBss->WPA.PairCipherAux == Ndis802_11WEPDisabled)
+						pBss->WPA.PairCipherAux = pBss->WPA.GroupCipher;
+				}
+				else
+					pBss->WepStatus   = pBss->WPA.PairCipher;
+
+				// Check the Pair & Group, if different, turn on mixed mode flag
+				if (pBss->WPA.GroupCipher != pBss->WPA.PairCipher)
+					pBss->WPA.bMixMode = TRUE;
+
+				break;
+
+			case IE_RSN:
+				pRsnHeader = (PRSN_IE_HEADER_STRUCT) pTmp;
+
+				// 0. Version must be 1
+				if (le2cpu16(pRsnHeader->Version) != 1)
+					break;
+				pTmp   += sizeof(RSN_IE_HEADER_STRUCT);
+
+				// 1. Check group cipher
+				pCipher = (PCIPHER_SUITE_STRUCT) pTmp;
+				if (!RTMPEqualMemory(pTmp, RSN_OUI, 3))
+					break;
+
+				// Parse group cipher
+				switch (pCipher->Type)
+				{
+					case 1:
+					case 5:	// Although WEP is not allowed in WPA related auth mode, we parse it anyway
+						pBss->WPA2.GroupCipher = Ndis802_11Encryption1Enabled;
+						break;
+					case 2:
+						pBss->WPA2.GroupCipher = Ndis802_11Encryption2Enabled;
+						break;
+					case 4:
+						pBss->WPA2.GroupCipher = Ndis802_11Encryption3Enabled;
+						break;
+					default:
+						break;
+				}
+				// set to correct offset for next parsing
+				pTmp   += sizeof(CIPHER_SUITE_STRUCT);
+
+				// 2. Get pairwise cipher counts
+				//Count = *(PUSHORT) pTmp;
+				Count = (pTmp[1]<<8) + pTmp[0];
+				pTmp   += sizeof(USHORT);
+
+				// 3. Get pairwise cipher
+				// Parsing all unicast cipher suite
+				while (Count > 0)
+				{
+					// Skip OUI
+					pCipher = (PCIPHER_SUITE_STRUCT) pTmp;
+					TmpCipher = Ndis802_11WEPDisabled;
+					switch (pCipher->Type)
+					{
+						case 1:
+						case 5: // Although WEP is not allowed in WPA related auth mode, we parse it anyway
+							TmpCipher = Ndis802_11Encryption1Enabled;
+							break;
+						case 2:
+							TmpCipher = Ndis802_11Encryption2Enabled;
+							break;
+						case 4:
+							TmpCipher = Ndis802_11Encryption3Enabled;
+							break;
+						default:
+							break;
+					}
+					if (TmpCipher > pBss->WPA2.PairCipher)
+					{
+						// Move the lower cipher suite to PairCipherAux
+						pBss->WPA2.PairCipherAux = pBss->WPA2.PairCipher;
+						pBss->WPA2.PairCipher	 = TmpCipher;
+					}
+					else
+					{
+						pBss->WPA2.PairCipherAux = TmpCipher;
+					}
+					pTmp += sizeof(CIPHER_SUITE_STRUCT);
+					Count--;
+				}
+
+				// 4. get AKM suite counts
+				//Count	= *(PUSHORT) pTmp;
+				Count = (pTmp[1]<<8) + pTmp[0];
+				pTmp   += sizeof(USHORT);
+
+				// 5. Get AKM ciphers
+				pAKM = (PAKM_SUITE_STRUCT) pTmp;
+				if (!RTMPEqualMemory(pTmp, RSN_OUI, 3))
+					break;
+
+				switch (pAKM->Type)
+				{
+					case 1:
+						// Set AP support WPA mode
+						if (pBss->AuthMode == Ndis802_11AuthModeOpen)
+							pBss->AuthMode = Ndis802_11AuthModeWPA2;
+						else
+							pBss->AuthModeAux = Ndis802_11AuthModeWPA2;
+						break;
+					case 2:
+						// Set AP support WPA mode
+						if (pBss->AuthMode == Ndis802_11AuthModeOpen)
+							pBss->AuthMode = Ndis802_11AuthModeWPA2PSK;
+						else
+							pBss->AuthModeAux = Ndis802_11AuthModeWPA2PSK;
+						break;
+					default:
+						break;
+				}
+				pTmp   += (Count * sizeof(AKM_SUITE_STRUCT));
+
+				// Fixed for WPA-None
+				if (pBss->BssType == BSS_ADHOC)
+				{
+					pBss->AuthMode = Ndis802_11AuthModeWPANone;
+					pBss->AuthModeAux = Ndis802_11AuthModeWPANone;
+					pBss->WPA.PairCipherAux = pBss->WPA2.PairCipherAux;
+					pBss->WPA.GroupCipher	= pBss->WPA2.GroupCipher;
+					pBss->WepStatus 		= pBss->WPA.GroupCipher;
+					// Patched bugs for old driver
+					if (pBss->WPA.PairCipherAux == Ndis802_11WEPDisabled)
+						pBss->WPA.PairCipherAux = pBss->WPA.GroupCipher;
+				}
+				pBss->WepStatus   = pBss->WPA2.PairCipher;
+
+				// 6. Get RSN capability
+				//pBss->WPA2.RsnCapability = *(PUSHORT) pTmp;
+				pBss->WPA2.RsnCapability = (pTmp[1]<<8) + pTmp[0];
+				pTmp += sizeof(USHORT);
+
+				// Check the Pair & Group, if different, turn on mixed mode flag
+				if (pBss->WPA2.GroupCipher != pBss->WPA2.PairCipher)
+					pBss->WPA2.bMixMode = TRUE;
+
+				break;
+			default:
+				break;
+		}
+		Length -= (pEid->Len + 2);
+	}
+}
+
+// ===========================================================================================
+// mac_table.c
+// ===========================================================================================
+
+/*! \brief generates a random mac address value for IBSS BSSID
+ *	\param Addr the bssid location
+ *	\return none
+ *	\pre
+ *	\post
+ */
+VOID MacAddrRandomBssid(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pAddr)
+{
+	INT i;
+
+	for (i = 0; i < MAC_ADDR_LEN; i++)
+	{
+		pAddr[i] = RandomByte(pAd);
+	}
+
+	pAddr[0] = (pAddr[0] & 0xfe) | 0x02;  // the first 2 bits must be 01xxxxxxxx
+}
+
+/*! \brief init the management mac frame header
+ *	\param p_hdr mac header
+ *	\param subtype subtype of the frame
+ *	\param p_ds destination address, don't care if it is a broadcast address
+ *	\return none
+ *	\pre the station has the following information in the pAd->StaCfg
+ *	 - bssid
+ *	 - station address
+ *	\post
+ *	\note this function initializes the following field
+
+ IRQL = PASSIVE_LEVEL
+ IRQL = DISPATCH_LEVEL
+
+ */
+VOID MgtMacHeaderInit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN OUT PHEADER_802_11 pHdr80211,
+	IN UCHAR SubType,
+	IN UCHAR ToDs,
+	IN PUCHAR pDA,
+	IN PUCHAR pBssid)
+{
+	NdisZeroMemory(pHdr80211, sizeof(HEADER_802_11));
+
+	pHdr80211->FC.Type = BTYPE_MGMT;
+	pHdr80211->FC.SubType = SubType;
+	pHdr80211->FC.ToDs = ToDs;
+	COPY_MAC_ADDR(pHdr80211->Addr1, pDA);
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		COPY_MAC_ADDR(pHdr80211->Addr2, pAd->CurrentAddress);
+#endif // CONFIG_STA_SUPPORT //
+	COPY_MAC_ADDR(pHdr80211->Addr3, pBssid);
+}
+
+// ===========================================================================================
+// mem_mgmt.c
+// ===========================================================================================
+
+/*!***************************************************************************
+ * This routine build an outgoing frame, and fill all information specified
+ * in argument list to the frame body. The actual frame size is the summation
+ * of all arguments.
+ * input params:
+ *		Buffer - pointer to a pre-allocated memory segment
+ *		args - a list of <int arg_size, arg> pairs.
+ *		NOTE NOTE NOTE!!!! the last argument must be NULL, otherwise this
+ *						   function will FAIL!!!
+ * return:
+ *		Size of the buffer
+ * usage:
+ *		MakeOutgoingFrame(Buffer, output_length, 2, &fc, 2, &dur, 6, p_addr1, 6,p_addr2, END_OF_ARGS);
+
+ IRQL = PASSIVE_LEVEL
+ IRQL = DISPATCH_LEVEL
+
+ ****************************************************************************/
+ULONG MakeOutgoingFrame(
+	OUT CHAR *Buffer,
+	OUT ULONG *FrameLen, ...)
+{
+	CHAR   *p;
+	int 	leng;
+	ULONG	TotLeng;
+	va_list Args;
+
+	// calculates the total length
+	TotLeng = 0;
+	va_start(Args, FrameLen);
+	do
+	{
+		leng = va_arg(Args, int);
+		if (leng == END_OF_ARGS)
+		{
+			break;
+		}
+		p = va_arg(Args, PVOID);
+		NdisMoveMemory(&Buffer[TotLeng], p, leng);
+		TotLeng = TotLeng + leng;
+	} while(TRUE);
+
+	va_end(Args); /* clean up */
+	*FrameLen = TotLeng;
+	return TotLeng;
+}
+
+// ===========================================================================================
+// mlme_queue.c
+// ===========================================================================================
+
+/*! \brief	Initialize The MLME Queue, used by MLME Functions
+ *	\param	*Queue	   The MLME Queue
+ *	\return Always	   Return NDIS_STATE_SUCCESS in this implementation
+ *	\pre
+ *	\post
+ *	\note	Because this is done only once (at the init stage), no need to be locked
+
+ IRQL = PASSIVE_LEVEL
+
+ */
+NDIS_STATUS MlmeQueueInit(
+	IN MLME_QUEUE *Queue)
+{
+	INT i;
+
+	NdisAllocateSpinLock(&Queue->Lock);
+
+	Queue->Num	= 0;
+	Queue->Head = 0;
+	Queue->Tail = 0;
+
+	for (i = 0; i < MAX_LEN_OF_MLME_QUEUE; i++)
+	{
+		Queue->Entry[i].Occupied = FALSE;
+		Queue->Entry[i].MsgLen = 0;
+		NdisZeroMemory(Queue->Entry[i].Msg, MGMT_DMA_BUFFER_SIZE);
+	}
+
+	return NDIS_STATUS_SUCCESS;
+}
+
+/*! \brief	 Enqueue a message for other threads, if they want to send messages to MLME thread
+ *	\param	*Queue	  The MLME Queue
+ *	\param	 Machine  The State Machine Id
+ *	\param	 MsgType  The Message Type
+ *	\param	 MsgLen   The Message length
+ *	\param	*Msg	  The message pointer
+ *	\return  TRUE if enqueue is successful, FALSE if the queue is full
+ *	\pre
+ *	\post
+ *	\note	 The message has to be initialized
+
+ IRQL = PASSIVE_LEVEL
+ IRQL = DISPATCH_LEVEL
+
+ */
+BOOLEAN MlmeEnqueue(
+	IN	PRTMP_ADAPTER	pAd,
+	IN ULONG Machine,
+	IN ULONG MsgType,
+	IN ULONG MsgLen,
+	IN VOID *Msg)
+{
+	INT Tail;
+	MLME_QUEUE	*Queue = (MLME_QUEUE *)&pAd->Mlme.Queue;
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return FALSE;
+
+	// First check the size, it MUST not exceed the mlme queue size
+	if (MsgLen > MGMT_DMA_BUFFER_SIZE)
+	{
+		DBGPRINT_ERR(("MlmeEnqueue: msg too large, size = %ld \n", MsgLen));
+		return FALSE;
+	}
+
+	if (MlmeQueueFull(Queue))
+	{
+		return FALSE;
+	}
+
+	NdisAcquireSpinLock(&(Queue->Lock));
+	Tail = Queue->Tail;
+	Queue->Tail++;
+	Queue->Num++;
+	if (Queue->Tail == MAX_LEN_OF_MLME_QUEUE)
+	{
+		Queue->Tail = 0;
+	}
+
+	Queue->Entry[Tail].Wcid = RESERVED_WCID;
+	Queue->Entry[Tail].Occupied = TRUE;
+	Queue->Entry[Tail].Machine = Machine;
+	Queue->Entry[Tail].MsgType = MsgType;
+	Queue->Entry[Tail].MsgLen  = MsgLen;
+
+	if (Msg != NULL)
+	{
+		NdisMoveMemory(Queue->Entry[Tail].Msg, Msg, MsgLen);
+	}
+
+	NdisReleaseSpinLock(&(Queue->Lock));
+	return TRUE;
+}
+
+/*! \brief	 This function is used when Recv gets a MLME message
+ *	\param	*Queue			 The MLME Queue
+ *	\param	 TimeStampHigh	 The upper 32 bit of timestamp
+ *	\param	 TimeStampLow	 The lower 32 bit of timestamp
+ *	\param	 Rssi			 The receiving RSSI strength
+ *	\param	 MsgLen 		 The length of the message
+ *	\param	*Msg			 The message pointer
+ *	\return  TRUE if everything ok, FALSE otherwise (like Queue Full)
+ *	\pre
+ *	\post
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+BOOLEAN MlmeEnqueueForRecv(
+	IN	PRTMP_ADAPTER	pAd,
+	IN ULONG Wcid,
+	IN ULONG TimeStampHigh,
+	IN ULONG TimeStampLow,
+	IN UCHAR Rssi0,
+	IN UCHAR Rssi1,
+	IN UCHAR Rssi2,
+	IN ULONG MsgLen,
+	IN VOID *Msg,
+	IN UCHAR Signal)
+{
+	INT 		 Tail, Machine;
+	PFRAME_802_11 pFrame = (PFRAME_802_11)Msg;
+	INT		 MsgType;
+	MLME_QUEUE	*Queue = (MLME_QUEUE *)&pAd->Mlme.Queue;
+
+#ifdef RALINK_ATE
+	/* Nothing to do in ATE mode */
+	if(ATE_ON(pAd))
+		return FALSE;
+#endif // RALINK_ATE //
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+	{
+		DBGPRINT_ERR(("MlmeEnqueueForRecv: fRTMP_ADAPTER_HALT_IN_PROGRESS\n"));
+		return FALSE;
+	}
+
+	// First check the size, it MUST not exceed the mlme queue size
+	if (MsgLen > MGMT_DMA_BUFFER_SIZE)
+	{
+		DBGPRINT_ERR(("MlmeEnqueueForRecv: frame too large, size = %ld \n", MsgLen));
+		return FALSE;
+	}
+
+	if (MlmeQueueFull(Queue))
+	{
+		return FALSE;
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (!MsgTypeSubst(pAd, pFrame, &Machine, &MsgType))
+		{
+			DBGPRINT_ERR(("MlmeEnqueueForRecv: un-recongnized mgmt->subtype=%d\n",pFrame->Hdr.FC.SubType));
+			return FALSE;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	// OK, we got all the informations, it is time to put things into queue
+	NdisAcquireSpinLock(&(Queue->Lock));
+	Tail = Queue->Tail;
+	Queue->Tail++;
+	Queue->Num++;
+	if (Queue->Tail == MAX_LEN_OF_MLME_QUEUE)
+	{
+		Queue->Tail = 0;
+	}
+	Queue->Entry[Tail].Occupied = TRUE;
+	Queue->Entry[Tail].Machine = Machine;
+	Queue->Entry[Tail].MsgType = MsgType;
+	Queue->Entry[Tail].MsgLen  = MsgLen;
+	Queue->Entry[Tail].TimeStamp.u.LowPart = TimeStampLow;
+	Queue->Entry[Tail].TimeStamp.u.HighPart = TimeStampHigh;
+	Queue->Entry[Tail].Rssi0 = Rssi0;
+	Queue->Entry[Tail].Rssi1 = Rssi1;
+	Queue->Entry[Tail].Rssi2 = Rssi2;
+	Queue->Entry[Tail].Signal = Signal;
+	Queue->Entry[Tail].Wcid = (UCHAR)Wcid;
+
+	Queue->Entry[Tail].Channel = pAd->LatchRfRegs.Channel;
+
+	if (Msg != NULL)
+	{
+		NdisMoveMemory(Queue->Entry[Tail].Msg, Msg, MsgLen);
+	}
+
+	NdisReleaseSpinLock(&(Queue->Lock));
+
+	RT28XX_MLME_HANDLER(pAd);
+
+	return TRUE;
+}
+
+
+/*! \brief	 Dequeue a message from the MLME Queue
+ *	\param	*Queue	  The MLME Queue
+ *	\param	*Elem	  The message dequeued from MLME Queue
+ *	\return  TRUE if the Elem contains something, FALSE otherwise
+ *	\pre
+ *	\post
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+BOOLEAN MlmeDequeue(
+	IN MLME_QUEUE *Queue,
+	OUT MLME_QUEUE_ELEM **Elem)
+{
+	NdisAcquireSpinLock(&(Queue->Lock));
+	*Elem = &(Queue->Entry[Queue->Head]);
+	Queue->Num--;
+	Queue->Head++;
+	if (Queue->Head == MAX_LEN_OF_MLME_QUEUE)
+	{
+		Queue->Head = 0;
+	}
+	NdisReleaseSpinLock(&(Queue->Lock));
+	return TRUE;
+}
+
+// IRQL = DISPATCH_LEVEL
+VOID	MlmeRestartStateMachine(
+	IN	PRTMP_ADAPTER	pAd)
+{
+#ifdef RT2860
+	MLME_QUEUE_ELEM		*Elem = NULL;
+#endif // RT2860 //
+#ifdef CONFIG_STA_SUPPORT
+	BOOLEAN				Cancelled;
+#endif // CONFIG_STA_SUPPORT //
+
+	DBGPRINT(RT_DEBUG_TRACE, ("MlmeRestartStateMachine \n"));
+
+#ifdef RT2860
+	NdisAcquireSpinLock(&pAd->Mlme.TaskLock);
+	if(pAd->Mlme.bRunning)
+	{
+		NdisReleaseSpinLock(&pAd->Mlme.TaskLock);
+		return;
+	}
+	else
+	{
+		pAd->Mlme.bRunning = TRUE;
+	}
+	NdisReleaseSpinLock(&pAd->Mlme.TaskLock);
+
+	// Remove all Mlme queues elements
+	while (!MlmeQueueEmpty(&pAd->Mlme.Queue))
+	{
+		//From message type, determine which state machine I should drive
+		if (MlmeDequeue(&pAd->Mlme.Queue, &Elem))
+		{
+			// free MLME element
+			Elem->Occupied = FALSE;
+			Elem->MsgLen = 0;
+
+		}
+		else {
+			DBGPRINT_ERR(("MlmeRestartStateMachine: MlmeQueue empty\n"));
+		}
+	}
+#endif // RT2860 //
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+#ifdef QOS_DLS_SUPPORT
+		UCHAR i;
+#endif // QOS_DLS_SUPPORT //
+		// Cancel all timer events
+		// Be careful to cancel new added timer
+		RTMPCancelTimer(&pAd->MlmeAux.AssocTimer,	  &Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.ReassocTimer,   &Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.DisassocTimer,  &Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.AuthTimer,	   &Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.BeaconTimer,	   &Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.ScanTimer,	   &Cancelled);
+
+#ifdef QOS_DLS_SUPPORT
+		for (i=0; i<MAX_NUM_OF_DLS_ENTRY; i++)
+		{
+			RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &Cancelled);
+		}
+#endif // QOS_DLS_SUPPORT //
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	// Change back to original channel in case of doing scan
+	AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+	AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+
+	// Resume MSDU which is turned off durning scan
+	RTMPResumeMsduTransmission(pAd);
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// Set all state machines back IDLE
+		pAd->Mlme.CntlMachine.CurrState    = CNTL_IDLE;
+		pAd->Mlme.AssocMachine.CurrState   = ASSOC_IDLE;
+		pAd->Mlme.AuthMachine.CurrState    = AUTH_REQ_IDLE;
+		pAd->Mlme.AuthRspMachine.CurrState = AUTH_RSP_IDLE;
+		pAd->Mlme.SyncMachine.CurrState    = SYNC_IDLE;
+		pAd->Mlme.ActMachine.CurrState    = ACT_IDLE;
+#ifdef QOS_DLS_SUPPORT
+		pAd->Mlme.DlsMachine.CurrState    = DLS_IDLE;
+#endif // QOS_DLS_SUPPORT //
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef RT2860
+	// Remove running state
+	NdisAcquireSpinLock(&pAd->Mlme.TaskLock);
+	pAd->Mlme.bRunning = FALSE;
+	NdisReleaseSpinLock(&pAd->Mlme.TaskLock);
+#endif // RT2860 //
+}
+
+/*! \brief	test if the MLME Queue is empty
+ *	\param	*Queue	  The MLME Queue
+ *	\return TRUE if the Queue is empty, FALSE otherwise
+ *	\pre
+ *	\post
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+BOOLEAN MlmeQueueEmpty(
+	IN MLME_QUEUE *Queue)
+{
+	BOOLEAN Ans;
+
+	NdisAcquireSpinLock(&(Queue->Lock));
+	Ans = (Queue->Num == 0);
+	NdisReleaseSpinLock(&(Queue->Lock));
+
+	return Ans;
+}
+
+/*! \brief	 test if the MLME Queue is full
+ *	\param	 *Queue 	 The MLME Queue
+ *	\return  TRUE if the Queue is empty, FALSE otherwise
+ *	\pre
+ *	\post
+
+ IRQL = PASSIVE_LEVEL
+ IRQL = DISPATCH_LEVEL
+
+ */
+BOOLEAN MlmeQueueFull(
+	IN MLME_QUEUE *Queue)
+{
+	BOOLEAN Ans;
+
+	NdisAcquireSpinLock(&(Queue->Lock));
+	Ans = (Queue->Num == MAX_LEN_OF_MLME_QUEUE || Queue->Entry[Queue->Tail].Occupied);
+	NdisReleaseSpinLock(&(Queue->Lock));
+
+	return Ans;
+}
+
+/*! \brief	 The destructor of MLME Queue
+ *	\param
+ *	\return
+ *	\pre
+ *	\post
+ *	\note	Clear Mlme Queue, Set Queue->Num to Zero.
+
+ IRQL = PASSIVE_LEVEL
+
+ */
+VOID MlmeQueueDestroy(
+	IN MLME_QUEUE *pQueue)
+{
+	NdisAcquireSpinLock(&(pQueue->Lock));
+	pQueue->Num  = 0;
+	pQueue->Head = 0;
+	pQueue->Tail = 0;
+	NdisReleaseSpinLock(&(pQueue->Lock));
+	NdisFreeSpinLock(&(pQueue->Lock));
+}
+
+/*! \brief	 To substitute the message type if the message is coming from external
+ *	\param	pFrame		   The frame received
+ *	\param	*Machine	   The state machine
+ *	\param	*MsgType	   the message type for the state machine
+ *	\return TRUE if the substitution is successful, FALSE otherwise
+ *	\pre
+ *	\post
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+#ifdef CONFIG_STA_SUPPORT
+BOOLEAN MsgTypeSubst(
+	IN PRTMP_ADAPTER  pAd,
+	IN PFRAME_802_11 pFrame,
+	OUT INT *Machine,
+	OUT INT *MsgType)
+{
+	USHORT	Seq;
+	UCHAR	EAPType;
+	PUCHAR	pData;
+
+	// Pointer to start of data frames including SNAP header
+	pData = (PUCHAR) pFrame + LENGTH_802_11;
+
+	// The only data type will pass to this function is EAPOL frame
+	if (pFrame->Hdr.FC.Type == BTYPE_DATA)
+	{
+		if (NdisEqualMemory(SNAP_AIRONET, pData, LENGTH_802_1_H))
+		{
+			// Cisco Aironet SNAP header
+			*Machine = AIRONET_STATE_MACHINE;
+			*MsgType = MT2_AIRONET_MSG;
+			return (TRUE);
+		}
+#ifdef LEAP_SUPPORT
+		if ( pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP ) //LEAP
+		{
+			// LEAP frames
+			*Machine = LEAP_STATE_MACHINE;
+			EAPType = *((UCHAR*)pFrame + LENGTH_802_11 + LENGTH_802_1_H + 1);
+			return (LeapMsgTypeSubst(EAPType, MsgType));
+		}
+		else
+#endif // LEAP_SUPPORT //
+		{
+			*Machine = WPA_PSK_STATE_MACHINE;
+			EAPType = *((UCHAR*)pFrame + LENGTH_802_11 + LENGTH_802_1_H + 1);
+			return(WpaMsgTypeSubst(EAPType, MsgType));
+		}
+	}
+
+	switch (pFrame->Hdr.FC.SubType)
+	{
+		case SUBTYPE_ASSOC_REQ:
+			*Machine = ASSOC_STATE_MACHINE;
+			*MsgType = MT2_PEER_ASSOC_REQ;
+			break;
+		case SUBTYPE_ASSOC_RSP:
+			*Machine = ASSOC_STATE_MACHINE;
+			*MsgType = MT2_PEER_ASSOC_RSP;
+			break;
+		case SUBTYPE_REASSOC_REQ:
+			*Machine = ASSOC_STATE_MACHINE;
+			*MsgType = MT2_PEER_REASSOC_REQ;
+			break;
+		case SUBTYPE_REASSOC_RSP:
+			*Machine = ASSOC_STATE_MACHINE;
+			*MsgType = MT2_PEER_REASSOC_RSP;
+			break;
+		case SUBTYPE_PROBE_REQ:
+			*Machine = SYNC_STATE_MACHINE;
+			*MsgType = MT2_PEER_PROBE_REQ;
+			break;
+		case SUBTYPE_PROBE_RSP:
+			*Machine = SYNC_STATE_MACHINE;
+			*MsgType = MT2_PEER_PROBE_RSP;
+			break;
+		case SUBTYPE_BEACON:
+			*Machine = SYNC_STATE_MACHINE;
+			*MsgType = MT2_PEER_BEACON;
+			break;
+		case SUBTYPE_ATIM:
+			*Machine = SYNC_STATE_MACHINE;
+			*MsgType = MT2_PEER_ATIM;
+			break;
+		case SUBTYPE_DISASSOC:
+			*Machine = ASSOC_STATE_MACHINE;
+			*MsgType = MT2_PEER_DISASSOC_REQ;
+			break;
+		case SUBTYPE_AUTH:
+			// get the sequence number from payload 24 Mac Header + 2 bytes algorithm
+			NdisMoveMemory(&Seq, &pFrame->Octet[2], sizeof(USHORT));
+			if (Seq == 1 || Seq == 3)
+			{
+				*Machine = AUTH_RSP_STATE_MACHINE;
+				*MsgType = MT2_PEER_AUTH_ODD;
+			}
+			else if (Seq == 2 || Seq == 4)
+			{
+				*Machine = AUTH_STATE_MACHINE;
+				*MsgType = MT2_PEER_AUTH_EVEN;
+			}
+			else
+			{
+				return FALSE;
+			}
+			break;
+		case SUBTYPE_DEAUTH:
+			*Machine = AUTH_RSP_STATE_MACHINE;
+			*MsgType = MT2_PEER_DEAUTH;
+			break;
+		case SUBTYPE_ACTION:
+			*Machine = ACTION_STATE_MACHINE;
+			//  Sometimes Sta will return with category bytes with MSB = 1, if they receive catogory out of their support
+			if ((pFrame->Octet[0]&0x7F) > MAX_PEER_CATE_MSG)
+			{
+				*MsgType = MT2_ACT_INVALID;
+			}
+			else
+			{
+				*MsgType = (pFrame->Octet[0]&0x7F);
+			}
+			break;
+		default:
+			return FALSE;
+			break;
+	}
+
+	return TRUE;
+}
+#endif // CONFIG_STA_SUPPORT //
+
+// ===========================================================================================
+// state_machine.c
+// ===========================================================================================
+
+/*! \brief Initialize the state machine.
+ *	\param *S			pointer to the state machine
+ *	\param	Trans		State machine transition function
+ *	\param	StNr		number of states
+ *	\param	MsgNr		number of messages
+ *	\param	DefFunc 	default function, when there is invalid state/message combination
+ *	\param	InitState	initial state of the state machine
+ *	\param	Base		StateMachine base, internal use only
+ *	\pre p_sm should be a legal pointer
+ *	\post
+
+ IRQL = PASSIVE_LEVEL
+
+ */
+VOID StateMachineInit(
+	IN STATE_MACHINE *S,
+	IN STATE_MACHINE_FUNC Trans[],
+	IN ULONG StNr,
+	IN ULONG MsgNr,
+	IN STATE_MACHINE_FUNC DefFunc,
+	IN ULONG InitState,
+	IN ULONG Base)
+{
+	ULONG i, j;
+
+	// set number of states and messages
+	S->NrState = StNr;
+	S->NrMsg   = MsgNr;
+	S->Base    = Base;
+
+	S->TransFunc  = Trans;
+
+	// init all state transition to default function
+	for (i = 0; i < StNr; i++)
+	{
+		for (j = 0; j < MsgNr; j++)
+		{
+			S->TransFunc[i * MsgNr + j] = DefFunc;
+		}
+	}
+
+	// set the starting state
+	S->CurrState = InitState;
+}
+
+/*! \brief This function fills in the function pointer into the cell in the state machine
+ *	\param *S	pointer to the state machine
+ *	\param St	state
+ *	\param Msg	incoming message
+ *	\param f	the function to be executed when (state, message) combination occurs at the state machine
+ *	\pre *S should be a legal pointer to the state machine, st, msg, should be all within the range, Base should be set in the initial state
+ *	\post
+
+ IRQL = PASSIVE_LEVEL
+
+ */
+VOID StateMachineSetAction(
+	IN STATE_MACHINE *S,
+	IN ULONG St,
+	IN ULONG Msg,
+	IN STATE_MACHINE_FUNC Func)
+{
+	ULONG MsgIdx;
+
+	MsgIdx = Msg - S->Base;
+
+	if (St < S->NrState && MsgIdx < S->NrMsg)
+	{
+		// boundary checking before setting the action
+		S->TransFunc[St * S->NrMsg + MsgIdx] = Func;
+	}
+}
+
+/*! \brief	 This function does the state transition
+ *	\param	 *Adapter the NIC adapter pointer
+ *	\param	 *S 	  the state machine
+ *	\param	 *Elem	  the message to be executed
+ *	\return   None
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+VOID StateMachinePerformAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN STATE_MACHINE *S,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	(*(S->TransFunc[S->CurrState * S->NrMsg + Elem->MsgType - S->Base]))(pAd, Elem);
+}
+
+/*
+	==========================================================================
+	Description:
+		The drop function, when machine executes this, the message is simply
+		ignored. This function does nothing, the message is freed in
+		StateMachinePerformAction()
+	==========================================================================
+ */
+VOID Drop(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+}
+
+// ===========================================================================================
+// lfsr.c
+// ===========================================================================================
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = PASSIVE_LEVEL
+
+	==========================================================================
+ */
+VOID LfsrInit(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG Seed)
+{
+	if (Seed == 0)
+		pAd->Mlme.ShiftReg = 1;
+	else
+		pAd->Mlme.ShiftReg = Seed;
+}
+
+/*
+	==========================================================================
+	Description:
+	==========================================================================
+ */
+UCHAR RandomByte(
+	IN PRTMP_ADAPTER pAd)
+{
+	ULONG i;
+	UCHAR R, Result;
+
+	R = 0;
+
+	if (pAd->Mlme.ShiftReg == 0)
+	NdisGetSystemUpTime((ULONG *)&pAd->Mlme.ShiftReg);
+
+	for (i = 0; i < 8; i++)
+	{
+		if (pAd->Mlme.ShiftReg & 0x00000001)
+		{
+			pAd->Mlme.ShiftReg = ((pAd->Mlme.ShiftReg ^ LFSR_MASK) >> 1) | 0x80000000;
+			Result = 1;
+		}
+		else
+		{
+			pAd->Mlme.ShiftReg = pAd->Mlme.ShiftReg >> 1;
+			Result = 0;
+		}
+		R = (R << 1) | Result;
+	}
+
+	return R;
+}
+
+VOID AsicUpdateAutoFallBackTable(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pRateTable)
+{
+	UCHAR					i;
+	HT_FBK_CFG0_STRUC		HtCfg0;
+	HT_FBK_CFG1_STRUC		HtCfg1;
+	LG_FBK_CFG0_STRUC		LgCfg0;
+	LG_FBK_CFG1_STRUC		LgCfg1;
+	PRTMP_TX_RATE_SWITCH	pCurrTxRate, pNextTxRate;
+
+	// set to initial value
+	HtCfg0.word = 0x65432100;
+	HtCfg1.word = 0xedcba988;
+	LgCfg0.word = 0xedcba988;
+	LgCfg1.word = 0x00002100;
+
+	pNextTxRate = (PRTMP_TX_RATE_SWITCH)pRateTable+1;
+	for (i = 1; i < *((PUCHAR) pRateTable); i++)
+	{
+		pCurrTxRate = (PRTMP_TX_RATE_SWITCH)pRateTable+1+i;
+		switch (pCurrTxRate->Mode)
+		{
+			case 0:		//CCK
+				break;
+			case 1:		//OFDM
+				{
+					switch(pCurrTxRate->CurrMCS)
+					{
+						case 0:
+							LgCfg0.field.OFDMMCS0FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+						case 1:
+							LgCfg0.field.OFDMMCS1FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+						case 2:
+							LgCfg0.field.OFDMMCS2FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+						case 3:
+							LgCfg0.field.OFDMMCS3FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+						case 4:
+							LgCfg0.field.OFDMMCS4FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+						case 5:
+							LgCfg0.field.OFDMMCS5FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+						case 6:
+							LgCfg0.field.OFDMMCS6FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+						case 7:
+							LgCfg0.field.OFDMMCS7FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+					}
+				}
+				break;
+#ifdef DOT11_N_SUPPORT
+			case 2:		//HT-MIX
+			case 3:		//HT-GF
+				{
+					if ((pNextTxRate->Mode >= MODE_HTMIX) && (pCurrTxRate->CurrMCS != pNextTxRate->CurrMCS))
+					{
+						switch(pCurrTxRate->CurrMCS)
+						{
+							case 0:
+								HtCfg0.field.HTMCS0FBK = pNextTxRate->CurrMCS;
+								break;
+							case 1:
+								HtCfg0.field.HTMCS1FBK = pNextTxRate->CurrMCS;
+								break;
+							case 2:
+								HtCfg0.field.HTMCS2FBK = pNextTxRate->CurrMCS;
+								break;
+							case 3:
+								HtCfg0.field.HTMCS3FBK = pNextTxRate->CurrMCS;
+								break;
+							case 4:
+								HtCfg0.field.HTMCS4FBK = pNextTxRate->CurrMCS;
+								break;
+							case 5:
+								HtCfg0.field.HTMCS5FBK = pNextTxRate->CurrMCS;
+								break;
+							case 6:
+								HtCfg0.field.HTMCS6FBK = pNextTxRate->CurrMCS;
+								break;
+							case 7:
+								HtCfg0.field.HTMCS7FBK = pNextTxRate->CurrMCS;
+								break;
+							case 8:
+								HtCfg1.field.HTMCS8FBK = pNextTxRate->CurrMCS;
+								break;
+							case 9:
+								HtCfg1.field.HTMCS9FBK = pNextTxRate->CurrMCS;
+								break;
+							case 10:
+								HtCfg1.field.HTMCS10FBK = pNextTxRate->CurrMCS;
+								break;
+							case 11:
+								HtCfg1.field.HTMCS11FBK = pNextTxRate->CurrMCS;
+								break;
+							case 12:
+								HtCfg1.field.HTMCS12FBK = pNextTxRate->CurrMCS;
+								break;
+							case 13:
+								HtCfg1.field.HTMCS13FBK = pNextTxRate->CurrMCS;
+								break;
+							case 14:
+								HtCfg1.field.HTMCS14FBK = pNextTxRate->CurrMCS;
+								break;
+							case 15:
+								HtCfg1.field.HTMCS15FBK = pNextTxRate->CurrMCS;
+								break;
+							default:
+								DBGPRINT(RT_DEBUG_ERROR, ("AsicUpdateAutoFallBackTable: not support CurrMCS=%d\n", pCurrTxRate->CurrMCS));
+						}
+					}
+				}
+				break;
+#endif // DOT11_N_SUPPORT //
+		}
+
+		pNextTxRate = pCurrTxRate;
+	}
+
+	RTMP_IO_WRITE32(pAd, HT_FBK_CFG0, HtCfg0.word);
+	RTMP_IO_WRITE32(pAd, HT_FBK_CFG1, HtCfg1.word);
+	RTMP_IO_WRITE32(pAd, LG_FBK_CFG0, LgCfg0.word);
+	RTMP_IO_WRITE32(pAd, LG_FBK_CFG1, LgCfg1.word);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Set MAC register value according operation mode.
+		OperationMode AND bNonGFExist are for MM and GF Proteciton.
+		If MM or GF mask is not set, those passing argument doesn't not take effect.
+
+		Operation mode meaning:
+		= 0 : Pure HT, no preotection.
+		= 0x01; there may be non-HT devices in both the control and extension channel, protection is optional in BSS.
+		= 0x10: No Transmission in 40M is protected.
+		= 0x11: Transmission in both 40M and 20M shall be protected
+		if (bNonGFExist)
+			we should choose not to use GF. But still set correct ASIC registers.
+	========================================================================
+*/
+VOID 	AsicUpdateProtect(
+	IN		PRTMP_ADAPTER	pAd,
+	IN 		USHORT			OperationMode,
+	IN 		UCHAR			SetMask,
+	IN		BOOLEAN			bDisableBGProtect,
+	IN		BOOLEAN			bNonGFExist)
+{
+	PROT_CFG_STRUC	ProtCfg, ProtCfg4;
+	UINT32 Protect[6];
+	USHORT			offset;
+	UCHAR			i;
+	UINT32 MacReg = 0;
+
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+#ifdef DOT11_N_SUPPORT
+	if (!(pAd->CommonCfg.bHTProtect) && (OperationMode != 8))
+	{
+		return;
+	}
+
+	if (pAd->BATable.numAsOriginator)
+	{
+		//
+		// enable the RTS/CTS to avoid channel collision
+		//
+		SetMask = ALLN_SETPROTECT;
+		OperationMode = 8;
+	}
+#endif // DOT11_N_SUPPORT //
+
+	// Config ASIC RTS threshold register
+	RTMP_IO_READ32(pAd, TX_RTS_CFG, &MacReg);
+	MacReg &= 0xFF0000FF;
+#if 0
+	MacReg |= (pAd->CommonCfg.RtsThreshold << 8);
+#else
+	// If the user want disable RtsThreshold and enbale Amsdu/Ralink-Aggregation, set the RtsThreshold as 4096
+        if ((
+#ifdef DOT11_N_SUPPORT
+			(pAd->CommonCfg.BACapability.field.AmsduEnable) ||
+#endif // DOT11_N_SUPPORT //
+			(pAd->CommonCfg.bAggregationCapable == TRUE))
+            && pAd->CommonCfg.RtsThreshold == MAX_RTS_THRESHOLD)
+        {
+			MacReg |= (0x1000 << 8);
+        }
+        else
+        {
+			MacReg |= (pAd->CommonCfg.RtsThreshold << 8);
+        }
+#endif
+
+	RTMP_IO_WRITE32(pAd, TX_RTS_CFG, MacReg);
+
+	// Initial common protection settings
+	RTMPZeroMemory(Protect, sizeof(Protect));
+	ProtCfg4.word = 0;
+	ProtCfg.word = 0;
+	ProtCfg.field.TxopAllowGF40 = 1;
+	ProtCfg.field.TxopAllowGF20 = 1;
+	ProtCfg.field.TxopAllowMM40 = 1;
+	ProtCfg.field.TxopAllowMM20 = 1;
+	ProtCfg.field.TxopAllowOfdm = 1;
+	ProtCfg.field.TxopAllowCck = 1;
+	ProtCfg.field.RTSThEn = 1;
+	ProtCfg.field.ProtectNav = ASIC_SHORTNAV;
+
+	// update PHY mode and rate
+	if (pAd->CommonCfg.Channel > 14)
+		ProtCfg.field.ProtectRate = 0x4000;
+	ProtCfg.field.ProtectRate |= pAd->CommonCfg.RtsRate;
+
+	// Handle legacy(B/G) protection
+	if (bDisableBGProtect)
+	{
+		//ProtCfg.field.ProtectRate = pAd->CommonCfg.RtsRate;
+		ProtCfg.field.ProtectCtrl = 0;
+		Protect[0] = ProtCfg.word;
+		Protect[1] = ProtCfg.word;
+	}
+	else
+	{
+		//ProtCfg.field.ProtectRate = pAd->CommonCfg.RtsRate;
+		ProtCfg.field.ProtectCtrl = 0;			// CCK do not need to be protected
+		Protect[0] = ProtCfg.word;
+		ProtCfg.field.ProtectCtrl = ASIC_CTS;	// OFDM needs using CCK to protect
+		Protect[1] = ProtCfg.word;
+	}
+
+#ifdef DOT11_N_SUPPORT
+	// Decide HT frame protection.
+	if ((SetMask & ALLN_SETPROTECT) != 0)
+	{
+		switch(OperationMode)
+		{
+			case 0x0:
+				// NO PROTECT
+				// 1.All STAs in the BSS are 20/40 MHz HT
+				// 2. in ai 20/40MHz BSS
+				// 3. all STAs are 20MHz in a 20MHz BSS
+				// Pure HT. no protection.
+
+				// MM20_PROT_CFG
+				//	Reserved (31:27)
+				// 	PROT_TXOP(25:20) -- 010111
+				//	PROT_NAV(19:18)  -- 01 (Short NAV protection)
+				//  PROT_CTRL(17:16) -- 00 (None)
+				// 	PROT_RATE(15:0)  -- 0x4004 (OFDM 24M)
+				Protect[2] = 0x01744004;
+
+				// MM40_PROT_CFG
+				//	Reserved (31:27)
+				// 	PROT_TXOP(25:20) -- 111111
+				//	PROT_NAV(19:18)  -- 01 (Short NAV protection)
+				//  PROT_CTRL(17:16) -- 00 (None)
+				// 	PROT_RATE(15:0)  -- 0x4084 (duplicate OFDM 24M)
+				Protect[3] = 0x03f44084;
+
+				// CF20_PROT_CFG
+				//	Reserved (31:27)
+				// 	PROT_TXOP(25:20) -- 010111
+				//	PROT_NAV(19:18)  -- 01 (Short NAV protection)
+				//  PROT_CTRL(17:16) -- 00 (None)
+				// 	PROT_RATE(15:0)  -- 0x4004 (OFDM 24M)
+				Protect[4] = 0x01744004;
+
+				// CF40_PROT_CFG
+				//	Reserved (31:27)
+				// 	PROT_TXOP(25:20) -- 111111
+				//	PROT_NAV(19:18)  -- 01 (Short NAV protection)
+				//  PROT_CTRL(17:16) -- 00 (None)
+				// 	PROT_RATE(15:0)  -- 0x4084 (duplicate OFDM 24M)
+				Protect[5] = 0x03f44084;
+
+				if (bNonGFExist)
+				{
+					// PROT_NAV(19:18)  -- 01 (Short NAV protectiion)
+					// PROT_CTRL(17:16) -- 01 (RTS/CTS)
+					Protect[4] = 0x01754004;
+					Protect[5] = 0x03f54084;
+				}
+				pAd->CommonCfg.IOTestParm.bRTSLongProtOn = FALSE;
+				break;
+
+ 			case 1:
+				// This is "HT non-member protection mode."
+				// If there may be non-HT STAs my BSS
+				ProtCfg.word = 0x01744004;	// PROT_CTRL(17:16) : 0 (None)
+				ProtCfg4.word = 0x03f44084; // duplicaet legacy 24M. BW set 1.
+				if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_BG_PROTECTION_INUSED))
+				{
+					ProtCfg.word = 0x01740003;	//ERP use Protection bit is set, use protection rate at Clause 18..
+					ProtCfg4.word = 0x03f40003; // Don't duplicate RTS/CTS in CCK mode. 0x03f40083;
+				}
+				//Assign Protection method for 20&40 MHz packets
+				ProtCfg.field.ProtectCtrl = ASIC_RTS;
+				ProtCfg.field.ProtectNav = ASIC_SHORTNAV;
+				ProtCfg4.field.ProtectCtrl = ASIC_RTS;
+				ProtCfg4.field.ProtectNav = ASIC_SHORTNAV;
+				Protect[2] = ProtCfg.word;
+				Protect[3] = ProtCfg4.word;
+				Protect[4] = ProtCfg.word;
+				Protect[5] = ProtCfg4.word;
+				pAd->CommonCfg.IOTestParm.bRTSLongProtOn = TRUE;
+				break;
+
+			case 2:
+				// If only HT STAs are in BSS. at least one is 20MHz. Only protect 40MHz packets
+				ProtCfg.word = 0x01744004;  // PROT_CTRL(17:16) : 0 (None)
+				ProtCfg4.word = 0x03f44084; // duplicaet legacy 24M. BW set 1.
+
+				//Assign Protection method for 40MHz packets
+				ProtCfg4.field.ProtectCtrl = ASIC_RTS;
+				ProtCfg4.field.ProtectNav = ASIC_SHORTNAV;
+				Protect[2] = ProtCfg.word;
+				Protect[3] = ProtCfg4.word;
+				if (bNonGFExist)
+				{
+					ProtCfg.field.ProtectCtrl = ASIC_RTS;
+					ProtCfg.field.ProtectNav = ASIC_SHORTNAV;
+				}
+				Protect[4] = ProtCfg.word;
+				Protect[5] = ProtCfg4.word;
+
+				pAd->CommonCfg.IOTestParm.bRTSLongProtOn = FALSE;
+				break;
+
+			case 3:
+				// HT mixed mode.	 PROTECT ALL!
+				// Assign Rate
+				ProtCfg.word = 0x01744004;	//duplicaet legacy 24M. BW set 1.
+				ProtCfg4.word = 0x03f44084;
+				// both 20MHz and 40MHz are protected. Whether use RTS or CTS-to-self depends on the
+				if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_BG_PROTECTION_INUSED))
+				{
+					ProtCfg.word = 0x01740003;	//ERP use Protection bit is set, use protection rate at Clause 18..
+					ProtCfg4.word = 0x03f40003; // Don't duplicate RTS/CTS in CCK mode. 0x03f40083
+				}
+				//Assign Protection method for 20&40 MHz packets
+				ProtCfg.field.ProtectCtrl = ASIC_RTS;
+				ProtCfg.field.ProtectNav = ASIC_SHORTNAV;
+				ProtCfg4.field.ProtectCtrl = ASIC_RTS;
+				ProtCfg4.field.ProtectNav = ASIC_SHORTNAV;
+				Protect[2] = ProtCfg.word;
+				Protect[3] = ProtCfg4.word;
+				Protect[4] = ProtCfg.word;
+				Protect[5] = ProtCfg4.word;
+				pAd->CommonCfg.IOTestParm.bRTSLongProtOn = TRUE;
+				break;
+
+			case 8:
+				// Special on for Atheros problem n chip.
+				Protect[2] = 0x01754004;
+				Protect[3] = 0x03f54084;
+				Protect[4] = 0x01754004;
+				Protect[5] = 0x03f54084;
+				pAd->CommonCfg.IOTestParm.bRTSLongProtOn = TRUE;
+				break;
+		}
+	}
+#endif // DOT11_N_SUPPORT //
+
+	offset = CCK_PROT_CFG;
+	for (i = 0;i < 6;i++)
+	{
+		if ((SetMask & (1<< i)))
+		{
+			RTMP_IO_WRITE32(pAd, offset + i*4, Protect[i]);
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicSwitchChannel(
+					  IN PRTMP_ADAPTER pAd,
+	IN	UCHAR			Channel,
+	IN	BOOLEAN			bScan)
+{
+	ULONG			R2 = 0, R3 = DEFAULT_RF_TX_POWER, R4 = 0;
+	CHAR    TxPwer = 0, TxPwer2 = DEFAULT_RF_TX_POWER; //Bbp94 = BBPR94_DEFAULT, TxPwer2 = DEFAULT_RF_TX_POWER;
+	UCHAR	index;
+	UINT32 	Value = 0; //BbpReg, Value;
+	RTMP_RF_REGS *RFRegTable;
+
+	// Search Tx power value
+	for (index = 0; index < pAd->ChannelListNum; index++)
+	{
+		if (Channel == pAd->ChannelList[index].Channel)
+		{
+			TxPwer = pAd->ChannelList[index].Power;
+			TxPwer2 = pAd->ChannelList[index].Power2;
+			break;
+		}
+	}
+
+	if (index == MAX_NUM_OF_CHANNELS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("AsicSwitchChannel: Cant find the Channel#%d \n", Channel));
+	}
+
+	{
+		RFRegTable = RF2850RegTable;
+
+		switch (pAd->RfIcType)
+		{
+			case RFIC_2820:
+			case RFIC_2850:
+			case RFIC_2720:
+			case RFIC_2750:
+
+			for (index = 0; index < NUM_OF_2850_CHNL; index++)
+			{
+				if (Channel == RFRegTable[index].Channel)
+				{
+					R2 = RFRegTable[index].R2;
+					if (pAd->Antenna.field.TxPath == 1)
+					{
+						R2 |= 0x4000;	// If TXpath is 1, bit 14 = 1;
+					}
+
+					if (pAd->Antenna.field.RxPath == 2)
+					{
+						R2 |= 0x40;	// write 1 to off Rxpath.
+					}
+					else if (pAd->Antenna.field.RxPath == 1)
+					{
+						R2 |= 0x20040;	// write 1 to off RxPath
+					}
+
+					if (Channel > 14)
+					{
+						// initialize R3, R4
+						R3 = (RFRegTable[index].R3 & 0xffffc1ff);
+						R4 = (RFRegTable[index].R4 & (~0x001f87c0)) | (pAd->RfFreqOffset << 15);
+
+						// 5G band power range: 0xF9~0X0F, TX0 Reg3 bit9/TX1 Reg4 bit6="0" means the TX power reduce 7dB
+						// R3
+						if ((TxPwer >= -7) && (TxPwer < 0))
+						{
+							TxPwer = (7+TxPwer);
+							TxPwer = (TxPwer > 0xF) ? (0xF) : (TxPwer);
+							R3 |= (TxPwer << 10);
+							DBGPRINT(RT_DEBUG_ERROR, ("AsicSwitchChannel: TxPwer=%d \n", TxPwer));
+						}
+						else
+						{
+							TxPwer = (TxPwer > 0xF) ? (0xF) : (TxPwer);
+							R3 |= (TxPwer << 10) | (1 << 9);
+						}
+
+						// R4
+						if ((TxPwer2 >= -7) && (TxPwer2 < 0))
+						{
+							TxPwer2 = (7+TxPwer2);
+							TxPwer2 = (TxPwer2 > 0xF) ? (0xF) : (TxPwer2);
+							R4 |= (TxPwer2 << 7);
+							DBGPRINT(RT_DEBUG_ERROR, ("AsicSwitchChannel: TxPwer2=%d \n", TxPwer2));
+						}
+						else
+						{
+							TxPwer2 = (TxPwer2 > 0xF) ? (0xF) : (TxPwer2);
+							R4 |= (TxPwer2 << 7) | (1 << 6);
+						}
+					}
+					else
+					{
+						R3 = (RFRegTable[index].R3 & 0xffffc1ff) | (TxPwer << 9); // set TX power0
+					R4 = (RFRegTable[index].R4 & (~0x001f87c0)) | (pAd->RfFreqOffset << 15) | (TxPwer2 <<6);// Set freq Offset & TxPwr1
+					}
+
+					// Based on BBP current mode before changing RF channel.
+					if (!bScan && (pAd->CommonCfg.BBPCurrentBW == BW_40))
+					{
+						R4 |=0x200000;
+					}
+
+					// Update variables
+					pAd->LatchRfRegs.Channel = Channel;
+					pAd->LatchRfRegs.R1 = RFRegTable[index].R1;
+					pAd->LatchRfRegs.R2 = R2;
+					pAd->LatchRfRegs.R3 = R3;
+					pAd->LatchRfRegs.R4 = R4;
+
+					// Set RF value 1's set R3[bit2] = [0]
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R1);
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R2);
+					RTMP_RF_IO_WRITE32(pAd, (pAd->LatchRfRegs.R3 & (~0x04)));
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R4);
+
+					RTMPusecDelay(200);
+
+					// Set RF value 2's set R3[bit2] = [1]
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R1);
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R2);
+					RTMP_RF_IO_WRITE32(pAd, (pAd->LatchRfRegs.R3 | 0x04));
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R4);
+
+					RTMPusecDelay(200);
+
+					// Set RF value 3's set R3[bit2] = [0]
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R1);
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R2);
+					RTMP_RF_IO_WRITE32(pAd, (pAd->LatchRfRegs.R3 & (~0x04)));
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R4);
+
+					break;
+				}
+			}
+			break;
+
+			default:
+			break;
+		}
+	}
+
+	// Change BBP setting during siwtch from a->g, g->a
+	if (Channel <= 14)
+	{
+	    ULONG	TxPinCfg = 0x00050F0A;//Gary 2007/08/09 0x050A0A
+
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R62, (0x37 - GET_LNA_GAIN(pAd)));
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R63, (0x37 - GET_LNA_GAIN(pAd)));
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R64, (0x37 - GET_LNA_GAIN(pAd)));
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R86, 0);//(0x44 - GET_LNA_GAIN(pAd)));	// According the Rory's suggestion to solve the middle range issue.
+		//RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R82, 0x62);
+
+		// Rx High power VGA offset for LNA select
+	    if (pAd->NicConfig2.field.ExternalLNAForG)
+	    {
+	        RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R82, 0x62);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R75, 0x46);
+	    }
+	    else
+	    {
+	        RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R82, 0x84);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R75, 0x50);
+	    }
+
+		// 5G band selection PIN, bit1 and bit2 are complement
+		RTMP_IO_READ32(pAd, TX_BAND_CFG, &Value);
+		Value &= (~0x6);
+		Value |= (0x04);
+		RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Value);
+
+        // Turn off unused PA or LNA when only 1T or 1R
+		if (pAd->Antenna.field.TxPath == 1)
+		{
+			TxPinCfg &= 0xFFFFFFF3;
+		}
+		if (pAd->Antenna.field.RxPath == 1)
+		{
+			TxPinCfg &= 0xFFFFF3FF;
+		}
+
+		RTMP_IO_WRITE32(pAd, TX_PIN_CFG, TxPinCfg);
+	}
+	else
+	{
+	    ULONG	TxPinCfg = 0x00050F05;//Gary 2007/8/9 0x050505
+
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R62, (0x37 - GET_LNA_GAIN(pAd)));
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R63, (0x37 - GET_LNA_GAIN(pAd)));
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R64, (0x37 - GET_LNA_GAIN(pAd)));
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R86, 0);//(0x44 - GET_LNA_GAIN(pAd)));   // According the Rory's suggestion to solve the middle range issue.
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R82, 0xF2);
+
+		// Rx High power VGA offset for LNA select
+		if (pAd->NicConfig2.field.ExternalLNAForA)
+		{
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R75, 0x46);
+		}
+		else
+		{
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R75, 0x50);
+		}
+
+		// 5G band selection PIN, bit1 and bit2 are complement
+		RTMP_IO_READ32(pAd, TX_BAND_CFG, &Value);
+		Value &= (~0x6);
+		Value |= (0x02);
+		RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Value);
+
+        // Turn off unused PA or LNA when only 1T or 1R
+		if (pAd->Antenna.field.TxPath == 1)
+		{
+			TxPinCfg &= 0xFFFFFFF3;
+	}
+		if (pAd->Antenna.field.RxPath == 1)
+		{
+			TxPinCfg &= 0xFFFFF3FF;
+	}
+
+		RTMP_IO_WRITE32(pAd, TX_PIN_CFG, TxPinCfg);
+	}
+
+    // R66 should be set according to Channel and use 20MHz when scanning
+	//RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, (0x2E + GET_LNA_GAIN(pAd)));
+	if (bScan)
+		RTMPSetAGCInitValue(pAd, BW_20);
+	else
+		RTMPSetAGCInitValue(pAd, pAd->CommonCfg.BBPCurrentBW);
+
+	//
+	// On 11A, We should delay and wait RF/BBP to be stable
+	// and the appropriate time should be 1000 micro seconds
+	// 2005/06/05 - On 11G, We also need this delay time. Otherwise it's difficult to pass the WHQL.
+	//
+	RTMPusecDelay(1000);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("SwitchChannel#%d(RF=%d, Pwr0=%lu, Pwr1=%lu, %dT) to , R1=0x%08lx, R2=0x%08lx, R3=0x%08lx, R4=0x%08lx\n",
+							  Channel,
+							  pAd->RfIcType,
+							  (R3 & 0x00003e00) >> 9,
+							  (R4 & 0x000007c0) >> 6,
+							  pAd->Antenna.field.TxPath,
+							  pAd->LatchRfRegs.R1,
+							  pAd->LatchRfRegs.R2,
+							  pAd->LatchRfRegs.R3,
+							  pAd->LatchRfRegs.R4));
+}
+
+/*
+	==========================================================================
+	Description:
+		This function is required for 2421 only, and should not be used during
+		site survey. It's only required after NIC decided to stay at a channel
+		for a longer period.
+		When this function is called, it's always after AsicSwitchChannel().
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicLockChannel(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR Channel)
+{
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID	AsicAntennaSelect(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Channel)
+{
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Antenna miscellaneous setting.
+
+	Arguments:
+		pAd						Pointer to our adapter
+		BandState				Indicate current Band State.
+
+	Return Value:
+		None
+
+	IRQL <= DISPATCH_LEVEL
+
+	Note:
+		1.) Frame End type control
+			only valid for G only (RF_2527 & RF_2529)
+			0: means DPDT, set BBP R4 bit 5 to 1
+			1: means SPDT, set BBP R4 bit 5 to 0
+
+
+	========================================================================
+*/
+VOID	AsicAntennaSetting(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	ABGBAND_STATE	BandState)
+{
+}
+
+VOID AsicRfTuningExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+}
+
+/*
+	==========================================================================
+	Description:
+		Gives CCK TX rate 2 more dB TX power.
+		This routine works only in LINK UP in INFRASTRUCTURE mode.
+
+		calculate desired Tx power in RF R3.Tx0~5,	should consider -
+		0. if current radio is a noisy environment (pAd->DrsCounters.fNoisyEnvironment)
+		1. TxPowerPercentage
+		2. auto calibration based on TSSI feedback
+		3. extra 2 db for CCK
+		4. -10 db upon very-short distance (AvgRSSI >= -40db) to AP
+
+	NOTE: Since this routine requires the value of (pAd->DrsCounters.fNoisyEnvironment),
+		it should be called AFTER MlmeDynamicTxRatSwitching()
+	==========================================================================
+ */
+VOID AsicAdjustTxPower(
+	IN PRTMP_ADAPTER pAd)
+{
+	INT			i, j;
+	CHAR		DeltaPwr = 0;
+	BOOLEAN		bAutoTxAgc = FALSE;
+	UCHAR		TssiRef, *pTssiMinusBoundary, *pTssiPlusBoundary, TxAgcStep;
+	UCHAR		BbpR1 = 0, BbpR49 = 0, idx;
+	PCHAR		pTxAgcCompensate;
+	ULONG		TxPwr[5];
+	CHAR		Value;
+
+	if (pAd->CommonCfg.BBPCurrentBW == BW_40)
+	{
+		if (pAd->CommonCfg.CentralChannel > 14)
+		{
+			TxPwr[0] = pAd->Tx40MPwrCfgABand[0];
+			TxPwr[1] = pAd->Tx40MPwrCfgABand[1];
+			TxPwr[2] = pAd->Tx40MPwrCfgABand[2];
+			TxPwr[3] = pAd->Tx40MPwrCfgABand[3];
+			TxPwr[4] = pAd->Tx40MPwrCfgABand[4];
+		}
+		else
+		{
+			TxPwr[0] = pAd->Tx40MPwrCfgGBand[0];
+			TxPwr[1] = pAd->Tx40MPwrCfgGBand[1];
+			TxPwr[2] = pAd->Tx40MPwrCfgGBand[2];
+			TxPwr[3] = pAd->Tx40MPwrCfgGBand[3];
+			TxPwr[4] = pAd->Tx40MPwrCfgGBand[4];
+		}
+	}
+	else
+	{
+		if (pAd->CommonCfg.Channel > 14)
+		{
+			TxPwr[0] = pAd->Tx20MPwrCfgABand[0];
+			TxPwr[1] = pAd->Tx20MPwrCfgABand[1];
+			TxPwr[2] = pAd->Tx20MPwrCfgABand[2];
+			TxPwr[3] = pAd->Tx20MPwrCfgABand[3];
+			TxPwr[4] = pAd->Tx20MPwrCfgABand[4];
+		}
+		else
+		{
+			TxPwr[0] = pAd->Tx20MPwrCfgGBand[0];
+			TxPwr[1] = pAd->Tx20MPwrCfgGBand[1];
+			TxPwr[2] = pAd->Tx20MPwrCfgGBand[2];
+			TxPwr[3] = pAd->Tx20MPwrCfgGBand[3];
+			TxPwr[4] = pAd->Tx20MPwrCfgGBand[4];
+		}
+	}
+
+	// TX power compensation for temperature variation based on TSSI. try every 4 second
+	if (pAd->Mlme.OneSecPeriodicRound % 4 == 0)
+	{
+		if (pAd->CommonCfg.Channel <= 14)
+		{
+			/* bg channel */
+			bAutoTxAgc         = pAd->bAutoTxAgcG;
+			TssiRef            = pAd->TssiRefG;
+			pTssiMinusBoundary = &pAd->TssiMinusBoundaryG[0];
+			pTssiPlusBoundary  = &pAd->TssiPlusBoundaryG[0];
+			TxAgcStep          = pAd->TxAgcStepG;
+			pTxAgcCompensate   = &pAd->TxAgcCompensateG;
+		}
+		else
+		{
+			/* a channel */
+			bAutoTxAgc         = pAd->bAutoTxAgcA;
+			TssiRef            = pAd->TssiRefA;
+			pTssiMinusBoundary = &pAd->TssiMinusBoundaryA[0];
+			pTssiPlusBoundary  = &pAd->TssiPlusBoundaryA[0];
+			TxAgcStep          = pAd->TxAgcStepA;
+			pTxAgcCompensate   = &pAd->TxAgcCompensateA;
+		}
+
+		if (bAutoTxAgc)
+		{
+			/* BbpR1 is unsigned char */
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R49, &BbpR49);
+
+			/* (p) TssiPlusBoundaryG[0] = 0 = (m) TssiMinusBoundaryG[0] */
+			/* compensate: +4     +3   +2   +1    0   -1   -2   -3   -4 * steps */
+			/* step value is defined in pAd->TxAgcStepG for tx power value */
+
+			/* [4]+1+[4]   p4     p3   p2   p1   o1   m1   m2   m3   m4 */
+			/* ex:         0x00 0x15 0x25 0x45 0x88 0xA0 0xB5 0xD0 0xF0
+			   above value are examined in mass factory production */
+			/*             [4]    [3]  [2]  [1]  [0]  [1]  [2]  [3]  [4] */
+
+			/* plus (+) is 0x00 ~ 0x45, minus (-) is 0xa0 ~ 0xf0 */
+			/* if value is between p1 ~ o1 or o1 ~ s1, no need to adjust tx power */
+			/* if value is 0xa5, tx power will be -= TxAgcStep*(2-1) */
+
+			if (BbpR49 > pTssiMinusBoundary[1])
+			{
+				// Reading is larger than the reference value
+				// check for how large we need to decrease the Tx power
+				for (idx = 1; idx < 5; idx++)
+				{
+					if (BbpR49 <= pTssiMinusBoundary[idx])  // Found the range
+						break;
+				}
+				// The index is the step we should decrease, idx = 0 means there is nothing to compensate
+				*pTxAgcCompensate = -(TxAgcStep * (idx-1));
+
+				DeltaPwr += (*pTxAgcCompensate);
+				DBGPRINT(RT_DEBUG_TRACE, ("-- Tx Power, BBP R1=%x, TssiRef=%x, TxAgcStep=%x, step = -%d\n",
+					BbpR49, TssiRef, TxAgcStep, idx-1));
+			}
+			else if (BbpR49 < pTssiPlusBoundary[1])
+			{
+				// Reading is smaller than the reference value
+				// check for how large we need to increase the Tx power
+				for (idx = 1; idx < 5; idx++)
+				{
+					if (BbpR49 >= pTssiPlusBoundary[idx])   // Found the range
+						break;
+				}
+				// The index is the step we should increase, idx = 0 means there is nothing to compensate
+				*pTxAgcCompensate = TxAgcStep * (idx-1);
+				DeltaPwr += (*pTxAgcCompensate);
+				DBGPRINT(RT_DEBUG_TRACE, ("++ Tx Power, BBP R1=%x, TssiRef=%x, TxAgcStep=%x, step = +%d\n",
+					BbpR49, TssiRef, TxAgcStep, idx-1));
+			}
+			else
+			{
+				*pTxAgcCompensate = 0;
+				DBGPRINT(RT_DEBUG_TRACE, ("   Tx Power, BBP R49=%x, TssiRef=%x, TxAgcStep=%x, step = +%d\n",
+					BbpR49, TssiRef, TxAgcStep, 0));
+			}
+		}
+	}
+	else
+	{
+		if (pAd->CommonCfg.Channel <= 14)
+		{
+			bAutoTxAgc         = pAd->bAutoTxAgcG;
+			pTxAgcCompensate   = &pAd->TxAgcCompensateG;
+		}
+		else
+		{
+			bAutoTxAgc         = pAd->bAutoTxAgcA;
+			pTxAgcCompensate   = &pAd->TxAgcCompensateA;
+		}
+
+		if (bAutoTxAgc)
+			DeltaPwr += (*pTxAgcCompensate);
+	}
+
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R1, &BbpR1);
+	BbpR1 &= 0xFC;
+
+#ifdef SINGLE_SKU
+	// Handle regulatory max tx power constrain
+	do
+	{
+		UCHAR    TxPwrInEEPROM = 0xFF, CountryTxPwr = 0xFF, criterion;
+		UCHAR    AdjustMaxTxPwr[40];
+
+		if (pAd->CommonCfg.Channel > 14) // 5G band
+			TxPwrInEEPROM = ((pAd->CommonCfg.DefineMaxTxPwr & 0xFF00) >> 8);
+		else // 2.4G band
+			TxPwrInEEPROM = (pAd->CommonCfg.DefineMaxTxPwr & 0x00FF);
+		CountryTxPwr = GetCuntryMaxTxPwr(pAd, pAd->CommonCfg.Channel);
+
+		// error handling, range check
+		if ((TxPwrInEEPROM > 0x50) || (CountryTxPwr > 0x50))
+		{
+			DBGPRINT(RT_DEBUG_ERROR,("AsicAdjustTxPower - Invalid max tx power (=0x%02x), CountryTxPwr=%d\n", TxPwrInEEPROM, CountryTxPwr));
+			break;
+		}
+
+		criterion = *((PUCHAR)TxPwr + 2) & 0xF;        // FAE use OFDM 6M as criterion
+
+		DBGPRINT_RAW(RT_DEBUG_TRACE,("AsicAdjustTxPower (criterion=%d, TxPwrInEEPROM=%d, CountryTxPwr=%d)\n", criterion, TxPwrInEEPROM, CountryTxPwr));
+
+		// Adjust max tx power according to the relationship of tx power in E2PROM
+		for (i=0; i<5; i++)
+		{
+			// CCK will have 4dBm larger than OFDM
+			// Therefore, we should separate to parse the tx power field
+			if (i == 0)
+			{
+				for (j=0; j<8; j++)
+				{
+					Value = (CHAR)((TxPwr[i] >> j*4) & 0x0F);
+
+					if (j < 4)
+					{
+						// CCK will have 4dBm larger than OFDM
+						AdjustMaxTxPwr[i*8+j] = TxPwrInEEPROM + (Value - criterion) + 4;
+					}
+					else
+					{
+						AdjustMaxTxPwr[i*8+j] = TxPwrInEEPROM + (Value - criterion);
+					}
+					DBGPRINT_RAW(RT_DEBUG_TRACE,("AsicAdjustTxPower (i/j=%d/%d, Value=%d, %d)\n", i, j, Value, AdjustMaxTxPwr[i*8+j]));
+				}
+			}
+			else
+			{
+				for (j=0; j<8; j++)
+				{
+					Value = (CHAR)((TxPwr[i] >> j*4) & 0x0F);
+
+					AdjustMaxTxPwr[i*8+j] = TxPwrInEEPROM + (Value - criterion);
+					DBGPRINT_RAW(RT_DEBUG_TRACE,("AsicAdjustTxPower (i/j=%d/%d, Value=%d, %d)\n", i, j, Value, AdjustMaxTxPwr[i*8+j]));
+				}
+			}
+		}
+
+		// Adjust tx power according to the relationship
+		for (i=0; i<5; i++)
+		{
+			if (TxPwr[i] != 0xffffffff)
+			{
+				for (j=0; j<8; j++)
+				{
+					Value = (CHAR)((TxPwr[i] >> j*4) & 0x0F);
+
+					// The system tx power is larger than the regulatory, the power should be restrain
+					if (AdjustMaxTxPwr[i*8+j] > CountryTxPwr)
+					{
+						// decrease to zero and don't need to take care BBPR1
+						if ((Value - (AdjustMaxTxPwr[i*8+j] - CountryTxPwr)) > 0)
+							Value -= (AdjustMaxTxPwr[i*8+j] - CountryTxPwr);
+						else
+							Value = 0;
+
+						DBGPRINT_RAW(RT_DEBUG_TRACE,("AsicAdjustTxPower (i/j=%d/%d, Value=%d, %d)\n", i, j, Value, AdjustMaxTxPwr[i*8+j]));
+					}
+					else
+						DBGPRINT_RAW(RT_DEBUG_TRACE,("AsicAdjustTxPower (i/j=%d/%d, Value=%d, %d, no change)\n", i, j, Value, AdjustMaxTxPwr[i*8+j]));
+
+						TxPwr[i] = (TxPwr[i] & ~(0x0000000F << j*4)) | (Value << j*4);
+				}
+			}
+		}
+	} while (FALSE);
+#endif // SINGLE_SKU //
+
+	/* calculate delta power based on the percentage specified from UI */
+	// E2PROM setting is calibrated for maximum TX power (i.e. 100%)
+	// We lower TX power here according to the percentage specified from UI
+	if (pAd->CommonCfg.TxPowerPercentage == 0xffffffff)       // AUTO TX POWER control
+		;
+	else if (pAd->CommonCfg.TxPowerPercentage > 90)  // 91 ~ 100% & AUTO, treat as 100% in terms of mW
+		;
+	else if (pAd->CommonCfg.TxPowerPercentage > 60)  // 61 ~ 90%, treat as 75% in terms of mW		// DeltaPwr -= 1;
+	{
+		DeltaPwr -= 1;
+	}
+	else if (pAd->CommonCfg.TxPowerPercentage > 30)  // 31 ~ 60%, treat as 50% in terms of mW		// DeltaPwr -= 3;
+	{
+		DeltaPwr -= 3;
+	}
+	else if (pAd->CommonCfg.TxPowerPercentage > 15)  // 16 ~ 30%, treat as 25% in terms of mW		// DeltaPwr -= 6;
+	{
+		BbpR1 |= 0x01;
+	}
+	else if (pAd->CommonCfg.TxPowerPercentage > 9)   // 10 ~ 15%, treat as 12.5% in terms of mW		// DeltaPwr -= 9;
+	{
+		BbpR1 |= 0x01;
+		DeltaPwr -= 3;
+	}
+	else                                           // 0 ~ 9 %, treat as MIN(~3%) in terms of mW		// DeltaPwr -= 12;
+	{
+		BbpR1 |= 0x02;
+	}
+
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R1, BbpR1);
+
+	/* reset different new tx power for different TX rate */
+	for(i=0; i<5; i++)
+	{
+		if (TxPwr[i] != 0xffffffff)
+		{
+			for (j=0; j<8; j++)
+			{
+				Value = (CHAR)((TxPwr[i] >> j*4) & 0x0F); /* 0 ~ 15 */
+
+				if ((Value + DeltaPwr) < 0)
+				{
+					Value = 0; /* min */
+				}
+				else if ((Value + DeltaPwr) > 0xF)
+				{
+					Value = 0xF; /* max */
+				}
+				else
+				{
+					Value += DeltaPwr; /* temperature compensation */
+				}
+
+				/* fill new value to CSR offset */
+				TxPwr[i] = (TxPwr[i] & ~(0x0000000F << j*4)) | (Value << j*4);
+			}
+
+			/* write tx power value to CSR */
+			/* TX_PWR_CFG_0 (8 tx rate) for	TX power for OFDM 12M/18M
+											TX power for OFDM 6M/9M
+											TX power for CCK5.5M/11M
+											TX power for CCK1M/2M */
+			/* TX_PWR_CFG_1 ~ TX_PWR_CFG_4 */
+			RTMP_IO_WRITE32(pAd, TX_PWR_CFG_0 + i*4, TxPwr[i]);
+		}
+	}
+
+}
+
+#ifdef CONFIG_STA_SUPPORT
+/*
+	==========================================================================
+	Description:
+		put PHY to sleep here, and set next wakeup timer. PHY doesn't not wakeup
+		automatically. Instead, MCU will issue a TwakeUpInterrupt to host after
+		the wakeup timer timeout. Driver has to issue a separate command to wake
+		PHY up.
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicSleepThenAutoWakeup(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT TbttNumToNextWakeUp)
+{
+    RT28XX_STA_SLEEP_THEN_AUTO_WAKEUP(pAd, TbttNumToNextWakeUp);
+}
+
+/*
+	==========================================================================
+	Description:
+		AsicForceWakeup() is used whenever manual wakeup is required
+		AsicForceSleep() should only be used when not in INFRA BSS. When
+		in INFRA BSS, we should use AsicSleepThenAutoWakeup() instead.
+	==========================================================================
+ */
+VOID AsicForceSleep(
+	IN PRTMP_ADAPTER pAd)
+{
+
+}
+
+/*
+	==========================================================================
+	Description:
+		AsicForceWakeup() is used whenever Twakeup timer (set via AsicSleepThenAutoWakeup)
+		expired.
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+	==========================================================================
+ */
+VOID AsicForceWakeup(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN    bFromTx)
+{
+    DBGPRINT(RT_DEBUG_TRACE, ("--> AsicForceWakeup \n"));
+    RT28XX_STA_FORCE_WAKEUP(pAd, bFromTx);
+}
+#endif // CONFIG_STA_SUPPORT //
+/*
+	==========================================================================
+	Description:
+		Set My BSSID
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicSetBssid(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pBssid)
+{
+	ULONG		  Addr4;
+	DBGPRINT(RT_DEBUG_TRACE, ("==============> AsicSetBssid %x:%x:%x:%x:%x:%x\n",
+		pBssid[0],pBssid[1],pBssid[2],pBssid[3], pBssid[4],pBssid[5]));
+
+	Addr4 = (ULONG)(pBssid[0])		 |
+			(ULONG)(pBssid[1] << 8)  |
+			(ULONG)(pBssid[2] << 16) |
+			(ULONG)(pBssid[3] << 24);
+	RTMP_IO_WRITE32(pAd, MAC_BSSID_DW0, Addr4);
+
+	Addr4 = 0;
+	// always one BSSID in STA mode
+	Addr4 = (ULONG)(pBssid[4]) | (ULONG)(pBssid[5] << 8);
+
+	RTMP_IO_WRITE32(pAd, MAC_BSSID_DW1, Addr4);
+}
+
+VOID AsicSetMcastWC(
+	IN PRTMP_ADAPTER pAd)
+{
+	MAC_TABLE_ENTRY *pEntry = &pAd->MacTab.Content[MCAST_WCID];
+	USHORT		offset;
+
+	pEntry->Sst        = SST_ASSOC;
+	pEntry->Aid        = MCAST_WCID;	// Softap supports 1 BSSID and use WCID=0 as multicast Wcid index
+	pEntry->PsMode     = PWR_ACTIVE;
+	pEntry->CurrTxRate = pAd->CommonCfg.MlmeRate;
+	offset = MAC_WCID_BASE + BSS0Mcast_WCID * HW_WCID_ENTRY_SIZE;
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicDelWcidTab(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR	Wcid)
+{
+	ULONG		  Addr0 = 0x0, Addr1 = 0x0;
+	ULONG 		offset;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("AsicDelWcidTab==>Wcid = 0x%x\n",Wcid));
+	offset = MAC_WCID_BASE + Wcid * HW_WCID_ENTRY_SIZE;
+	RTMP_IO_WRITE32(pAd, offset, Addr0);
+	offset += 4;
+	RTMP_IO_WRITE32(pAd, offset, Addr1);
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicEnableRDG(
+	IN PRTMP_ADAPTER pAd)
+{
+	TX_LINK_CFG_STRUC	TxLinkCfg;
+	UINT32				Data = 0;
+
+	RTMP_IO_READ32(pAd, TX_LINK_CFG, &TxLinkCfg.word);
+	TxLinkCfg.field.TxRDGEn = 1;
+	RTMP_IO_WRITE32(pAd, TX_LINK_CFG, TxLinkCfg.word);
+
+	RTMP_IO_READ32(pAd, EDCA_AC0_CFG, &Data);
+	Data  &= 0xFFFFFF00;
+	Data  |= 0x80;
+	RTMP_IO_WRITE32(pAd, EDCA_AC0_CFG, Data);
+
+	//OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED);
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicDisableRDG(
+	IN PRTMP_ADAPTER pAd)
+{
+	TX_LINK_CFG_STRUC	TxLinkCfg;
+	UINT32				Data = 0;
+
+
+	RTMP_IO_READ32(pAd, TX_LINK_CFG, &TxLinkCfg.word);
+	TxLinkCfg.field.TxRDGEn = 0;
+	RTMP_IO_WRITE32(pAd, TX_LINK_CFG, TxLinkCfg.word);
+
+	RTMP_IO_READ32(pAd, EDCA_AC0_CFG, &Data);
+
+	Data  &= 0xFFFFFF00;
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_DYNAMIC_BE_TXOP_ACTIVE)
+#ifdef DOT11_N_SUPPORT
+		&& (pAd->MacTab.fAnyStationMIMOPSDynamic == FALSE)
+#endif // DOT11_N_SUPPORT //
+	)
+	{
+		// For CWC test, change txop from 0x30 to 0x20 in TxBurst mode
+		if (pAd->CommonCfg.bEnableTxBurst)
+			Data |= 0x20;
+	}
+	RTMP_IO_WRITE32(pAd, EDCA_AC0_CFG, Data);
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicDisableSync(
+	IN PRTMP_ADAPTER pAd)
+{
+	BCN_TIME_CFG_STRUC csr;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--->Disable TSF synchronization\n"));
+
+	// 2003-12-20 disable TSF and TBTT while NIC in power-saving have side effect
+	//			  that NIC will never wakes up because TSF stops and no more
+	//			  TBTT interrupts
+	pAd->TbttTickCount = 0;
+	RTMP_IO_READ32(pAd, BCN_TIME_CFG, &csr.word);
+	csr.field.bBeaconGen = 0;
+	csr.field.bTBTTEnable = 0;
+	csr.field.TsfSyncMode = 0;
+	csr.field.bTsfTicking = 0;
+	RTMP_IO_WRITE32(pAd, BCN_TIME_CFG, csr.word);
+
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicEnableBssSync(
+	IN PRTMP_ADAPTER pAd)
+{
+	BCN_TIME_CFG_STRUC csr;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--->AsicEnableBssSync(INFRA mode)\n"));
+
+	RTMP_IO_READ32(pAd, BCN_TIME_CFG, &csr.word);
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		csr.field.BeaconInterval = pAd->CommonCfg.BeaconPeriod << 4; // ASIC register in units of 1/16 TU
+		csr.field.bTsfTicking = 1;
+		csr.field.TsfSyncMode = 1; // sync TSF in INFRASTRUCTURE mode
+		csr.field.bBeaconGen  = 0; // do NOT generate BEACON
+		csr.field.bTBTTEnable = 1;
+	}
+#endif // CONFIG_STA_SUPPORT //
+	RTMP_IO_WRITE32(pAd, BCN_TIME_CFG, csr.word);
+}
+
+/*
+	==========================================================================
+	Description:
+	Note:
+		BEACON frame in shared memory should be built ok before this routine
+		can be called. Otherwise, a garbage frame maybe transmitted out every
+		Beacon period.
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicEnableIbssSync(
+	IN PRTMP_ADAPTER pAd)
+{
+	BCN_TIME_CFG_STRUC csr9;
+	PUCHAR			ptr;
+	UINT i;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--->AsicEnableIbssSync(ADHOC mode. MPDUtotalByteCount = %d)\n", pAd->BeaconTxWI.MPDUtotalByteCount));
+
+	RTMP_IO_READ32(pAd, BCN_TIME_CFG, &csr9.word);
+	csr9.field.bBeaconGen = 0;
+	csr9.field.bTBTTEnable = 0;
+	csr9.field.bTsfTicking = 0;
+	RTMP_IO_WRITE32(pAd, BCN_TIME_CFG, csr9.word);
+
+#ifdef RT2860
+	// move BEACON TXD and frame content to on-chip memory
+	ptr = (PUCHAR)&pAd->BeaconTxWI;
+	for (i=0; i<TXWI_SIZE; i+=4)  // 16-byte TXWI field
+	{
+		UINT32 longptr =  *ptr + (*(ptr+1)<<8) + (*(ptr+2)<<16) + (*(ptr+3)<<24);
+		RTMP_IO_WRITE32(pAd, HW_BEACON_BASE0 + i, longptr);
+		ptr += 4;
+	}
+
+	// start right after the 16-byte TXWI field
+	ptr = pAd->BeaconBuf;
+	for (i=0; i< pAd->BeaconTxWI.MPDUtotalByteCount; i+=4)
+	{
+		UINT32 longptr =  *ptr + (*(ptr+1)<<8) + (*(ptr+2)<<16) + (*(ptr+3)<<24);
+		RTMP_IO_WRITE32(pAd, HW_BEACON_BASE0 + TXWI_SIZE + i, longptr);
+		ptr +=4;
+	}
+#endif // RT2860 //
+
+	// start sending BEACON
+	csr9.field.BeaconInterval = pAd->CommonCfg.BeaconPeriod << 4; // ASIC register in units of 1/16 TU
+	csr9.field.bTsfTicking = 1;
+	csr9.field.TsfSyncMode = 2; // sync TSF in IBSS mode
+	csr9.field.bTBTTEnable = 1;
+	csr9.field.bBeaconGen = 1;
+	RTMP_IO_WRITE32(pAd, BCN_TIME_CFG, csr9.word);
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicSetEdcaParm(
+	IN PRTMP_ADAPTER pAd,
+	IN PEDCA_PARM	 pEdcaParm)
+{
+	EDCA_AC_CFG_STRUC   Ac0Cfg, Ac1Cfg, Ac2Cfg, Ac3Cfg;
+	AC_TXOP_CSR0_STRUC csr0;
+	AC_TXOP_CSR1_STRUC csr1;
+	AIFSN_CSR_STRUC    AifsnCsr;
+	CWMIN_CSR_STRUC    CwminCsr;
+	CWMAX_CSR_STRUC    CwmaxCsr;
+	int i;
+
+	Ac0Cfg.word = 0;
+	Ac1Cfg.word = 0;
+	Ac2Cfg.word = 0;
+	Ac3Cfg.word = 0;
+	if ((pEdcaParm == NULL) || (pEdcaParm->bValid == FALSE))
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("AsicSetEdcaParm\n"));
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_WMM_INUSED);
+		for (i=0; i<MAX_LEN_OF_MAC_TABLE; i++)
+		{
+			if (pAd->MacTab.Content[i].ValidAsCLI || pAd->MacTab.Content[i].ValidAsApCli)
+				CLIENT_STATUS_CLEAR_FLAG(&pAd->MacTab.Content[i], fCLIENT_STATUS_WMM_CAPABLE);
+		}
+
+		//========================================================
+		//      MAC Register has a copy .
+		//========================================================
+		if( pAd->CommonCfg.bEnableTxBurst )
+		{
+			// For CWC test, change txop from 0x30 to 0x20 in TxBurst mode
+			Ac0Cfg.field.AcTxop = 0x20; // Suggest by John for TxBurst in HT Mode
+		}
+		else
+			Ac0Cfg.field.AcTxop = 0;	// QID_AC_BE
+		Ac0Cfg.field.Cwmin = CW_MIN_IN_BITS;
+		Ac0Cfg.field.Cwmax = CW_MAX_IN_BITS;
+		Ac0Cfg.field.Aifsn = 2;
+		RTMP_IO_WRITE32(pAd, EDCA_AC0_CFG, Ac0Cfg.word);
+
+		Ac1Cfg.field.AcTxop = 0;	// QID_AC_BK
+		Ac1Cfg.field.Cwmin = CW_MIN_IN_BITS;
+		Ac1Cfg.field.Cwmax = CW_MAX_IN_BITS;
+		Ac1Cfg.field.Aifsn = 2;
+		RTMP_IO_WRITE32(pAd, EDCA_AC1_CFG, Ac1Cfg.word);
+
+		if (pAd->CommonCfg.PhyMode == PHY_11B)
+		{
+			Ac2Cfg.field.AcTxop = 192;	// AC_VI: 192*32us ~= 6ms
+			Ac3Cfg.field.AcTxop = 96;	// AC_VO: 96*32us  ~= 3ms
+		}
+		else
+		{
+			Ac2Cfg.field.AcTxop = 96;	// AC_VI: 96*32us ~= 3ms
+			Ac3Cfg.field.AcTxop = 48;	// AC_VO: 48*32us ~= 1.5ms
+		}
+		Ac2Cfg.field.Cwmin = CW_MIN_IN_BITS;
+		Ac2Cfg.field.Cwmax = CW_MAX_IN_BITS;
+		Ac2Cfg.field.Aifsn = 2;
+		RTMP_IO_WRITE32(pAd, EDCA_AC2_CFG, Ac2Cfg.word);
+		Ac3Cfg.field.Cwmin = CW_MIN_IN_BITS;
+		Ac3Cfg.field.Cwmax = CW_MAX_IN_BITS;
+		Ac3Cfg.field.Aifsn = 2;
+		RTMP_IO_WRITE32(pAd, EDCA_AC3_CFG, Ac3Cfg.word);
+
+		//========================================================
+		//      DMA Register has a copy too.
+		//========================================================
+		csr0.field.Ac0Txop = 0;		// QID_AC_BE
+		csr0.field.Ac1Txop = 0;		// QID_AC_BK
+		RTMP_IO_WRITE32(pAd, WMM_TXOP0_CFG, csr0.word);
+		if (pAd->CommonCfg.PhyMode == PHY_11B)
+		{
+			csr1.field.Ac2Txop = 192;		// AC_VI: 192*32us ~= 6ms
+			csr1.field.Ac3Txop = 96;		// AC_VO: 96*32us  ~= 3ms
+		}
+		else
+		{
+			csr1.field.Ac2Txop = 96;		// AC_VI: 96*32us ~= 3ms
+			csr1.field.Ac3Txop = 48;		// AC_VO: 48*32us ~= 1.5ms
+		}
+		RTMP_IO_WRITE32(pAd, WMM_TXOP1_CFG, csr1.word);
+
+		CwminCsr.word = 0;
+		CwminCsr.field.Cwmin0 = CW_MIN_IN_BITS;
+		CwminCsr.field.Cwmin1 = CW_MIN_IN_BITS;
+		CwminCsr.field.Cwmin2 = CW_MIN_IN_BITS;
+		CwminCsr.field.Cwmin3 = CW_MIN_IN_BITS;
+		RTMP_IO_WRITE32(pAd, WMM_CWMIN_CFG, CwminCsr.word);
+
+		CwmaxCsr.word = 0;
+		CwmaxCsr.field.Cwmax0 = CW_MAX_IN_BITS;
+		CwmaxCsr.field.Cwmax1 = CW_MAX_IN_BITS;
+		CwmaxCsr.field.Cwmax2 = CW_MAX_IN_BITS;
+		CwmaxCsr.field.Cwmax3 = CW_MAX_IN_BITS;
+		RTMP_IO_WRITE32(pAd, WMM_CWMAX_CFG, CwmaxCsr.word);
+
+		RTMP_IO_WRITE32(pAd, WMM_AIFSN_CFG, 0x00002222);
+
+		NdisZeroMemory(&pAd->CommonCfg.APEdcaParm, sizeof(EDCA_PARM));
+	}
+	else
+	{
+		OPSTATUS_SET_FLAG(pAd, fOP_STATUS_WMM_INUSED);
+		//========================================================
+		//      MAC Register has a copy.
+		//========================================================
+		//
+		// Modify Cwmin/Cwmax/Txop on queue[QID_AC_VI], Recommend by Jerry 2005/07/27
+		// To degrade our VIDO Queue's throughput for WiFi WMM S3T07 Issue.
+		//
+		//pEdcaParm->Txop[QID_AC_VI] = pEdcaParm->Txop[QID_AC_VI] * 7 / 10; // rt2860c need this
+
+		Ac0Cfg.field.AcTxop =  pEdcaParm->Txop[QID_AC_BE];
+		Ac0Cfg.field.Cwmin= pEdcaParm->Cwmin[QID_AC_BE];
+		Ac0Cfg.field.Cwmax = pEdcaParm->Cwmax[QID_AC_BE];
+		Ac0Cfg.field.Aifsn = pEdcaParm->Aifsn[QID_AC_BE]; //+1;
+
+		Ac1Cfg.field.AcTxop =  pEdcaParm->Txop[QID_AC_BK];
+		Ac1Cfg.field.Cwmin = pEdcaParm->Cwmin[QID_AC_BK]; //+2;
+		Ac1Cfg.field.Cwmax = pEdcaParm->Cwmax[QID_AC_BK];
+		Ac1Cfg.field.Aifsn = pEdcaParm->Aifsn[QID_AC_BK]; //+1;
+
+		Ac2Cfg.field.AcTxop = (pEdcaParm->Txop[QID_AC_VI] * 6) / 10;
+		Ac2Cfg.field.Cwmin = pEdcaParm->Cwmin[QID_AC_VI];
+		Ac2Cfg.field.Cwmax = pEdcaParm->Cwmax[QID_AC_VI];
+		Ac2Cfg.field.Aifsn = pEdcaParm->Aifsn[QID_AC_VI];
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			// Tuning for Wi-Fi WMM S06
+			if (pAd->CommonCfg.bWiFiTest &&
+				pEdcaParm->Aifsn[QID_AC_VI] == 10)
+				Ac2Cfg.field.Aifsn -= 1;
+
+			// Tuning for TGn Wi-Fi 5.2.32
+			// STA TestBed changes in this item: conexant legacy sta ==> broadcom 11n sta
+			if (STA_TGN_WIFI_ON(pAd) &&
+				pEdcaParm->Aifsn[QID_AC_VI] == 10)
+			{
+				Ac0Cfg.field.Aifsn = 3;
+				Ac2Cfg.field.AcTxop = 5;
+			}
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+		Ac3Cfg.field.AcTxop = pEdcaParm->Txop[QID_AC_VO];
+		Ac3Cfg.field.Cwmin = pEdcaParm->Cwmin[QID_AC_VO];
+		Ac3Cfg.field.Cwmax = pEdcaParm->Cwmax[QID_AC_VO];
+		Ac3Cfg.field.Aifsn = pEdcaParm->Aifsn[QID_AC_VO];
+
+//#ifdef WIFI_TEST
+		if (pAd->CommonCfg.bWiFiTest)
+		{
+			if (Ac3Cfg.field.AcTxop == 102)
+			{
+			Ac0Cfg.field.AcTxop = pEdcaParm->Txop[QID_AC_BE] ? pEdcaParm->Txop[QID_AC_BE] : 10;
+				Ac0Cfg.field.Aifsn  = pEdcaParm->Aifsn[QID_AC_BE]-1; /* AIFSN must >= 1 */
+			Ac1Cfg.field.AcTxop = pEdcaParm->Txop[QID_AC_BK];
+				Ac1Cfg.field.Aifsn  = pEdcaParm->Aifsn[QID_AC_BK];
+			Ac2Cfg.field.AcTxop = pEdcaParm->Txop[QID_AC_VI];
+			} /* End of if */
+		}
+//#endif // WIFI_TEST //
+
+		RTMP_IO_WRITE32(pAd, EDCA_AC0_CFG, Ac0Cfg.word);
+		RTMP_IO_WRITE32(pAd, EDCA_AC1_CFG, Ac1Cfg.word);
+		RTMP_IO_WRITE32(pAd, EDCA_AC2_CFG, Ac2Cfg.word);
+		RTMP_IO_WRITE32(pAd, EDCA_AC3_CFG, Ac3Cfg.word);
+
+
+		//========================================================
+		//      DMA Register has a copy too.
+		//========================================================
+		csr0.field.Ac0Txop = Ac0Cfg.field.AcTxop;
+		csr0.field.Ac1Txop = Ac1Cfg.field.AcTxop;
+		RTMP_IO_WRITE32(pAd, WMM_TXOP0_CFG, csr0.word);
+
+		csr1.field.Ac2Txop = Ac2Cfg.field.AcTxop;
+		csr1.field.Ac3Txop = Ac3Cfg.field.AcTxop;
+		RTMP_IO_WRITE32(pAd, WMM_TXOP1_CFG, csr1.word);
+
+		CwminCsr.word = 0;
+		CwminCsr.field.Cwmin0 = pEdcaParm->Cwmin[QID_AC_BE];
+		CwminCsr.field.Cwmin1 = pEdcaParm->Cwmin[QID_AC_BK];
+		CwminCsr.field.Cwmin2 = pEdcaParm->Cwmin[QID_AC_VI];
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			CwminCsr.field.Cwmin3 = pEdcaParm->Cwmin[QID_AC_VO] - 1; //for TGn wifi test
+#endif // CONFIG_STA_SUPPORT //
+		RTMP_IO_WRITE32(pAd, WMM_CWMIN_CFG, CwminCsr.word);
+
+		CwmaxCsr.word = 0;
+		CwmaxCsr.field.Cwmax0 = pEdcaParm->Cwmax[QID_AC_BE];
+		CwmaxCsr.field.Cwmax1 = pEdcaParm->Cwmax[QID_AC_BK];
+		CwmaxCsr.field.Cwmax2 = pEdcaParm->Cwmax[QID_AC_VI];
+		CwmaxCsr.field.Cwmax3 = pEdcaParm->Cwmax[QID_AC_VO];
+		RTMP_IO_WRITE32(pAd, WMM_CWMAX_CFG, CwmaxCsr.word);
+
+		AifsnCsr.word = 0;
+		AifsnCsr.field.Aifsn0 = Ac0Cfg.field.Aifsn; //pEdcaParm->Aifsn[QID_AC_BE];
+		AifsnCsr.field.Aifsn1 = Ac1Cfg.field.Aifsn; //pEdcaParm->Aifsn[QID_AC_BK];
+		AifsnCsr.field.Aifsn2 = Ac2Cfg.field.Aifsn; //pEdcaParm->Aifsn[QID_AC_VI];
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			// Tuning for Wi-Fi WMM S06
+			if (pAd->CommonCfg.bWiFiTest &&
+				pEdcaParm->Aifsn[QID_AC_VI] == 10)
+				AifsnCsr.field.Aifsn2 = Ac2Cfg.field.Aifsn - 4;
+
+			// Tuning for TGn Wi-Fi 5.2.32
+			// STA TestBed changes in this item: conexant legacy sta ==> broadcom 11n sta
+			if (STA_TGN_WIFI_ON(pAd) &&
+				pEdcaParm->Aifsn[QID_AC_VI] == 10)
+			{
+				AifsnCsr.field.Aifsn0 = 3;
+				AifsnCsr.field.Aifsn2 = 7;
+			}
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			AifsnCsr.field.Aifsn3 = Ac3Cfg.field.Aifsn - 1; //pEdcaParm->Aifsn[QID_AC_VO]; //for TGn wifi test
+#endif // CONFIG_STA_SUPPORT //
+		RTMP_IO_WRITE32(pAd, WMM_AIFSN_CFG, AifsnCsr.word);
+
+		NdisMoveMemory(&pAd->CommonCfg.APEdcaParm, pEdcaParm, sizeof(EDCA_PARM));
+		if (!ADHOC_ON(pAd))
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("EDCA [#%d]: AIFSN CWmin CWmax  TXOP(us)  ACM\n", pEdcaParm->EdcaUpdateCount));
+			DBGPRINT(RT_DEBUG_TRACE,("     AC_BE      %2d     %2d     %2d      %4d     %d\n",
+									 pEdcaParm->Aifsn[0],
+									 pEdcaParm->Cwmin[0],
+									 pEdcaParm->Cwmax[0],
+									 pEdcaParm->Txop[0]<<5,
+									 pEdcaParm->bACM[0]));
+			DBGPRINT(RT_DEBUG_TRACE,("     AC_BK      %2d     %2d     %2d      %4d     %d\n",
+									 pEdcaParm->Aifsn[1],
+									 pEdcaParm->Cwmin[1],
+									 pEdcaParm->Cwmax[1],
+									 pEdcaParm->Txop[1]<<5,
+									 pEdcaParm->bACM[1]));
+			DBGPRINT(RT_DEBUG_TRACE,("     AC_VI      %2d     %2d     %2d      %4d     %d\n",
+									 pEdcaParm->Aifsn[2],
+									 pEdcaParm->Cwmin[2],
+									 pEdcaParm->Cwmax[2],
+									 pEdcaParm->Txop[2]<<5,
+									 pEdcaParm->bACM[2]));
+			DBGPRINT(RT_DEBUG_TRACE,("     AC_VO      %2d     %2d     %2d      %4d     %d\n",
+									 pEdcaParm->Aifsn[3],
+									 pEdcaParm->Cwmin[3],
+									 pEdcaParm->Cwmax[3],
+									 pEdcaParm->Txop[3]<<5,
+									 pEdcaParm->bACM[3]));
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID 	AsicSetSlotTime(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN bUseShortSlotTime)
+{
+	ULONG	SlotTime;
+	UINT32	RegValue = 0;
+
+#ifdef CONFIG_STA_SUPPORT
+	if (pAd->CommonCfg.Channel > 14)
+		bUseShortSlotTime = TRUE;
+#endif // CONFIG_STA_SUPPORT //
+
+	if (bUseShortSlotTime)
+		OPSTATUS_SET_FLAG(pAd, fOP_STATUS_SHORT_SLOT_INUSED);
+	else
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SHORT_SLOT_INUSED);
+
+	SlotTime = (bUseShortSlotTime)? 9 : 20;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// force using short SLOT time for FAE to demo performance when TxBurst is ON
+		if (((pAd->StaActive.SupportedPhyInfo.bHtEnable == FALSE) && (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED)))
+#ifdef DOT11_N_SUPPORT
+			|| ((pAd->StaActive.SupportedPhyInfo.bHtEnable == TRUE) && (pAd->CommonCfg.BACapability.field.Policy == BA_NOTUSE))
+#endif // DOT11_N_SUPPORT //
+			)
+		{
+			// In this case, we will think it is doing Wi-Fi test
+			// And we will not set to short slot when bEnableTxBurst is TRUE.
+		}
+		else if (pAd->CommonCfg.bEnableTxBurst)
+			SlotTime = 9;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	//
+	// For some reasons, always set it to short slot time.
+	//
+	// ToDo: Should consider capability with 11B
+	//
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (pAd->StaCfg.BssType == BSS_ADHOC)
+			SlotTime = 20;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	RTMP_IO_READ32(pAd, BKOFF_SLOT_CFG, &RegValue);
+	RegValue = RegValue & 0xFFFFFF00;
+
+	RegValue |= SlotTime;
+
+	RTMP_IO_WRITE32(pAd, BKOFF_SLOT_CFG, RegValue);
+}
+
+/*
+	========================================================================
+	Description:
+		Add Shared key information into ASIC.
+		Update shared key, TxMic and RxMic to Asic Shared key table
+		Update its cipherAlg to Asic Shared key Mode.
+
+    Return:
+	========================================================================
+*/
+VOID AsicAddSharedKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR		 BssIndex,
+	IN UCHAR		 KeyIdx,
+	IN UCHAR		 CipherAlg,
+	IN PUCHAR		 pKey,
+	IN PUCHAR		 pTxMic,
+	IN PUCHAR		 pRxMic)
+{
+	ULONG offset; //, csr0;
+	SHAREDKEY_MODE_STRUC csr1;
+#ifdef RT2860
+	INT   i;
+#endif // RT2860 //
+
+	DBGPRINT(RT_DEBUG_TRACE, ("AsicAddSharedKeyEntry BssIndex=%d, KeyIdx=%d\n", BssIndex,KeyIdx));
+//============================================================================================
+
+	DBGPRINT(RT_DEBUG_TRACE,("AsicAddSharedKeyEntry: %s key #%d\n", CipherName[CipherAlg], BssIndex*4 + KeyIdx));
+	DBGPRINT_RAW(RT_DEBUG_TRACE, (" 	Key = %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
+		pKey[0],pKey[1],pKey[2],pKey[3],pKey[4],pKey[5],pKey[6],pKey[7],pKey[8],pKey[9],pKey[10],pKey[11],pKey[12],pKey[13],pKey[14],pKey[15]));
+	if (pRxMic)
+	{
+		DBGPRINT_RAW(RT_DEBUG_TRACE, (" 	Rx MIC Key = %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
+			pRxMic[0],pRxMic[1],pRxMic[2],pRxMic[3],pRxMic[4],pRxMic[5],pRxMic[6],pRxMic[7]));
+	}
+	if (pTxMic)
+	{
+		DBGPRINT_RAW(RT_DEBUG_TRACE, (" 	Tx MIC Key = %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
+			pTxMic[0],pTxMic[1],pTxMic[2],pTxMic[3],pTxMic[4],pTxMic[5],pTxMic[6],pTxMic[7]));
+	}
+//============================================================================================
+	//
+	// fill key material - key + TX MIC + RX MIC
+	//
+#ifdef RT2860
+	offset = SHARED_KEY_TABLE_BASE + (4*BssIndex + KeyIdx)*HW_KEY_ENTRY_SIZE;
+	for (i=0; i<MAX_LEN_OF_SHARE_KEY; i++)
+	{
+		RTMP_IO_WRITE8(pAd, offset + i, pKey[i]);
+	}
+
+	offset += MAX_LEN_OF_SHARE_KEY;
+	if (pTxMic)
+	{
+		for (i=0; i<8; i++)
+		{
+			RTMP_IO_WRITE8(pAd, offset + i, pTxMic[i]);
+		}
+	}
+
+	offset += 8;
+	if (pRxMic)
+	{
+		for (i=0; i<8; i++)
+		{
+			RTMP_IO_WRITE8(pAd, offset + i, pRxMic[i]);
+		}
+	}
+#endif // RT2860 //
+
+
+	//
+	// Update cipher algorithm. WSTA always use BSS0
+	//
+	RTMP_IO_READ32(pAd, SHARED_KEY_MODE_BASE+4*(BssIndex/2), &csr1.word);
+	DBGPRINT(RT_DEBUG_TRACE,("Read: SHARED_KEY_MODE_BASE at this Bss[%d] KeyIdx[%d]= 0x%x \n", BssIndex,KeyIdx, csr1.word));
+	if ((BssIndex%2) == 0)
+	{
+		if (KeyIdx == 0)
+			csr1.field.Bss0Key0CipherAlg = CipherAlg;
+		else if (KeyIdx == 1)
+			csr1.field.Bss0Key1CipherAlg = CipherAlg;
+		else if (KeyIdx == 2)
+			csr1.field.Bss0Key2CipherAlg = CipherAlg;
+		else
+			csr1.field.Bss0Key3CipherAlg = CipherAlg;
+	}
+	else
+	{
+		if (KeyIdx == 0)
+			csr1.field.Bss1Key0CipherAlg = CipherAlg;
+		else if (KeyIdx == 1)
+			csr1.field.Bss1Key1CipherAlg = CipherAlg;
+		else if (KeyIdx == 2)
+			csr1.field.Bss1Key2CipherAlg = CipherAlg;
+		else
+			csr1.field.Bss1Key3CipherAlg = CipherAlg;
+	}
+	DBGPRINT(RT_DEBUG_TRACE,("Write: SHARED_KEY_MODE_BASE at this Bss[%d] = 0x%x \n", BssIndex, csr1.word));
+	RTMP_IO_WRITE32(pAd, SHARED_KEY_MODE_BASE+4*(BssIndex/2), csr1.word);
+
+}
+
+//	IRQL = DISPATCH_LEVEL
+VOID AsicRemoveSharedKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR		 BssIndex,
+	IN UCHAR		 KeyIdx)
+{
+	//ULONG SecCsr0;
+	SHAREDKEY_MODE_STRUC csr1;
+
+	DBGPRINT(RT_DEBUG_TRACE,("AsicRemoveSharedKeyEntry: #%d \n", BssIndex*4 + KeyIdx));
+
+	RTMP_IO_READ32(pAd, SHARED_KEY_MODE_BASE+4*(BssIndex/2), &csr1.word);
+	if ((BssIndex%2) == 0)
+	{
+		if (KeyIdx == 0)
+			csr1.field.Bss0Key0CipherAlg = 0;
+		else if (KeyIdx == 1)
+			csr1.field.Bss0Key1CipherAlg = 0;
+		else if (KeyIdx == 2)
+			csr1.field.Bss0Key2CipherAlg = 0;
+		else
+			csr1.field.Bss0Key3CipherAlg = 0;
+	}
+	else
+	{
+		if (KeyIdx == 0)
+			csr1.field.Bss1Key0CipherAlg = 0;
+		else if (KeyIdx == 1)
+			csr1.field.Bss1Key1CipherAlg = 0;
+		else if (KeyIdx == 2)
+			csr1.field.Bss1Key2CipherAlg = 0;
+		else
+			csr1.field.Bss1Key3CipherAlg = 0;
+	}
+	DBGPRINT(RT_DEBUG_TRACE,("Write: SHARED_KEY_MODE_BASE at this Bss[%d] = 0x%x \n", BssIndex, csr1.word));
+	RTMP_IO_WRITE32(pAd, SHARED_KEY_MODE_BASE+4*(BssIndex/2), csr1.word);
+	ASSERT(BssIndex < 4);
+	ASSERT(KeyIdx < 4);
+
+}
+
+
+VOID AsicUpdateWCIDAttribute(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT		WCID,
+	IN UCHAR		BssIndex,
+	IN UCHAR        CipherAlg,
+	IN BOOLEAN		bUsePairewiseKeyTable)
+{
+	ULONG   WCIDAttri = 0, offset;
+
+	//
+	// Update WCID attribute.
+	// Only TxKey could update WCID attribute.
+	//
+	offset = MAC_WCID_ATTRIBUTE_BASE + (WCID * HW_WCID_ATTRI_SIZE);
+	WCIDAttri = (BssIndex << 4) | (CipherAlg << 1) | (bUsePairewiseKeyTable);
+	RTMP_IO_WRITE32(pAd, offset, WCIDAttri);
+}
+
+VOID AsicUpdateWCIDIVEIV(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT		WCID,
+	IN ULONG        uIV,
+	IN ULONG        uEIV)
+{
+	ULONG	offset;
+
+	offset = MAC_IVEIV_TABLE_BASE + (WCID * HW_IVEIV_ENTRY_SIZE);
+
+	RTMP_IO_WRITE32(pAd, offset, uIV);
+	RTMP_IO_WRITE32(pAd, offset + 4, uEIV);
+}
+
+VOID AsicUpdateRxWCIDTable(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT		WCID,
+	IN PUCHAR        pAddr)
+{
+	ULONG offset;
+	ULONG Addr;
+
+	offset = MAC_WCID_BASE + (WCID * HW_WCID_ENTRY_SIZE);
+	Addr = pAddr[0] + (pAddr[1] << 8) +(pAddr[2] << 16) +(pAddr[3] << 24);
+	RTMP_IO_WRITE32(pAd, offset, Addr);
+	Addr = pAddr[4] + (pAddr[5] << 8);
+	RTMP_IO_WRITE32(pAd, offset + 4, Addr);
+}
+
+
+/*
+    ========================================================================
+
+    Routine Description:
+        Set Cipher Key, Cipher algorithm, IV/EIV to Asic
+
+    Arguments:
+        pAd                     Pointer to our adapter
+        WCID                    WCID Entry number.
+        BssIndex                BSSID index, station or none multiple BSSID support
+                                this value should be 0.
+        KeyIdx                  This KeyIdx will set to IV's KeyID if bTxKey enabled
+        pCipherKey              Pointer to Cipher Key.
+        bUsePairewiseKeyTable   TRUE means saved the key in SharedKey table,
+                                otherwise PairewiseKey table
+        bTxKey                  This is the transmit key if enabled.
+
+    Return Value:
+        None
+
+    Note:
+        This routine will set the relative key stuff to Asic including WCID attribute,
+        Cipher Key, Cipher algorithm and IV/EIV.
+
+        IV/EIV will be update if this CipherKey is the transmission key because
+        ASIC will base on IV's KeyID value to select Cipher Key.
+
+        If bTxKey sets to FALSE, this is not the TX key, but it could be
+        RX key
+
+    	For AP mode bTxKey must be always set to TRUE.
+    ========================================================================
+*/
+VOID AsicAddKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT		WCID,
+	IN UCHAR		BssIndex,
+	IN UCHAR		KeyIdx,
+	IN PCIPHER_KEY	pCipherKey,
+	IN BOOLEAN		bUsePairewiseKeyTable,
+	IN BOOLEAN		bTxKey)
+{
+	ULONG	offset;
+	UCHAR	IV4 = 0;
+	PUCHAR		pKey = pCipherKey->Key;
+	PUCHAR		pTxMic = pCipherKey->TxMic;
+	PUCHAR		pRxMic = pCipherKey->RxMic;
+	PUCHAR		pTxtsc = pCipherKey->TxTsc;
+	UCHAR		CipherAlg = pCipherKey->CipherAlg;
+	SHAREDKEY_MODE_STRUC csr1;
+#ifdef RT2860
+	UCHAR		i;
+#endif // RT2860 //
+
+	DBGPRINT(RT_DEBUG_TRACE, ("==> AsicAddKeyEntry\n"));
+	//
+	// 1.) decide key table offset
+	//
+	if (bUsePairewiseKeyTable)
+		offset = PAIRWISE_KEY_TABLE_BASE + (WCID * HW_KEY_ENTRY_SIZE);
+	else
+		offset = SHARED_KEY_TABLE_BASE + (4 * BssIndex + KeyIdx) * HW_KEY_ENTRY_SIZE;
+
+	//
+	// 2.) Set Key to Asic
+	//
+	//for (i = 0; i < KeyLen; i++)
+#ifdef RT2860
+	for (i = 0; i < MAX_LEN_OF_PEER_KEY; i++)
+	{
+		RTMP_IO_WRITE8(pAd, offset + i, pKey[i]);
+	}
+	offset += MAX_LEN_OF_PEER_KEY;
+
+	//
+	// 3.) Set MIC key if available
+	//
+	if (pTxMic)
+	{
+		for (i = 0; i < 8; i++)
+		{
+			RTMP_IO_WRITE8(pAd, offset + i, pTxMic[i]);
+		}
+	}
+	offset += LEN_TKIP_TXMICK;
+
+	if (pRxMic)
+	{
+		for (i = 0; i < 8; i++)
+		{
+			RTMP_IO_WRITE8(pAd, offset + i, pRxMic[i]);
+		}
+	}
+#endif // RT2860 //
+
+
+	//
+	// 4.) Modify IV/EIV if needs
+	//     This will force Asic to use this key ID by setting IV.
+	//
+	if (bTxKey)
+	{
+#ifdef RT2860
+		offset = MAC_IVEIV_TABLE_BASE + (WCID * HW_IVEIV_ENTRY_SIZE);
+		//
+		// Write IV
+		//
+		RTMP_IO_WRITE8(pAd, offset, pTxtsc[1]);
+		RTMP_IO_WRITE8(pAd, offset + 1, ((pTxtsc[1] | 0x20) & 0x7f));
+		RTMP_IO_WRITE8(pAd, offset + 2, pTxtsc[0]);
+
+		IV4 = (KeyIdx << 6);
+		if ((CipherAlg == CIPHER_TKIP) || (CipherAlg == CIPHER_TKIP_NO_MIC) ||(CipherAlg == CIPHER_AES))
+			IV4 |= 0x20;  // turn on extension bit means EIV existence
+
+		RTMP_IO_WRITE8(pAd, offset + 3, IV4);
+
+		//
+		// Write EIV
+		//
+		offset += 4;
+		for (i = 0; i < 4; i++)
+		{
+			RTMP_IO_WRITE8(pAd, offset + i, pTxtsc[i + 2]);
+		}
+#endif // RT2860 //
+
+		AsicUpdateWCIDAttribute(pAd, WCID, BssIndex, CipherAlg, bUsePairewiseKeyTable);
+	}
+
+	if (!bUsePairewiseKeyTable)
+	{
+		//
+		// Only update the shared key security mode
+		//
+		RTMP_IO_READ32(pAd, SHARED_KEY_MODE_BASE + 4 * (BssIndex / 2), &csr1.word);
+		if ((BssIndex % 2) == 0)
+		{
+			if (KeyIdx == 0)
+				csr1.field.Bss0Key0CipherAlg = CipherAlg;
+			else if (KeyIdx == 1)
+				csr1.field.Bss0Key1CipherAlg = CipherAlg;
+			else if (KeyIdx == 2)
+				csr1.field.Bss0Key2CipherAlg = CipherAlg;
+			else
+				csr1.field.Bss0Key3CipherAlg = CipherAlg;
+		}
+		else
+		{
+			if (KeyIdx == 0)
+				csr1.field.Bss1Key0CipherAlg = CipherAlg;
+			else if (KeyIdx == 1)
+				csr1.field.Bss1Key1CipherAlg = CipherAlg;
+			else if (KeyIdx == 2)
+				csr1.field.Bss1Key2CipherAlg = CipherAlg;
+			else
+				csr1.field.Bss1Key3CipherAlg = CipherAlg;
+		}
+		RTMP_IO_WRITE32(pAd, SHARED_KEY_MODE_BASE + 4 * (BssIndex / 2), csr1.word);
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<== AsicAddKeyEntry\n"));
+}
+
+
+/*
+	========================================================================
+	Description:
+		Add Pair-wise key material into ASIC.
+		Update pairwise key, TxMic and RxMic to Asic Pair-wise key table
+
+    Return:
+	========================================================================
+*/
+VOID AsicAddPairwiseKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR        pAddr,
+	IN UCHAR		WCID,
+	IN CIPHER_KEY		 *pCipherKey)
+{
+	INT i;
+	ULONG 		offset;
+	PUCHAR		 pKey = pCipherKey->Key;
+	PUCHAR		 pTxMic = pCipherKey->TxMic;
+	PUCHAR		 pRxMic = pCipherKey->RxMic;
+#ifdef DBG
+	UCHAR		CipherAlg = pCipherKey->CipherAlg;
+#endif // DBG //
+
+	// EKEY
+	offset = PAIRWISE_KEY_TABLE_BASE + (WCID * HW_KEY_ENTRY_SIZE);
+#ifdef RT2860
+	for (i=0; i<MAX_LEN_OF_PEER_KEY; i++)
+	{
+		RTMP_IO_WRITE8(pAd, offset + i, pKey[i]);
+	}
+#endif // RT2860 //
+	for (i=0; i<MAX_LEN_OF_PEER_KEY; i+=4)
+	{
+		UINT32 Value;
+		RTMP_IO_READ32(pAd, offset + i, &Value);
+	}
+
+	offset += MAX_LEN_OF_PEER_KEY;
+
+	//  MIC KEY
+	if (pTxMic)
+	{
+#ifdef RT2860
+		for (i=0; i<8; i++)
+		{
+			RTMP_IO_WRITE8(pAd, offset+i, pTxMic[i]);
+		}
+#endif // RT2860 //
+	}
+	offset += 8;
+	if (pRxMic)
+	{
+#ifdef RT2860
+		for (i=0; i<8; i++)
+		{
+			RTMP_IO_WRITE8(pAd, offset+i, pRxMic[i]);
+		}
+#endif // RT2860 //
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("AsicAddPairwiseKeyEntry: WCID #%d Alg=%s\n",WCID, CipherName[CipherAlg]));
+	DBGPRINT(RT_DEBUG_TRACE,("	Key = %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
+		pKey[0],pKey[1],pKey[2],pKey[3],pKey[4],pKey[5],pKey[6],pKey[7],pKey[8],pKey[9],pKey[10],pKey[11],pKey[12],pKey[13],pKey[14],pKey[15]));
+	if (pRxMic)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("	Rx MIC Key = %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
+			pRxMic[0],pRxMic[1],pRxMic[2],pRxMic[3],pRxMic[4],pRxMic[5],pRxMic[6],pRxMic[7]));
+	}
+	if (pTxMic)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("	Tx MIC Key = %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
+			pTxMic[0],pTxMic[1],pTxMic[2],pTxMic[3],pTxMic[4],pTxMic[5],pTxMic[6],pTxMic[7]));
+	}
+}
+/*
+	========================================================================
+	Description:
+		Remove Pair-wise key material from ASIC.
+
+    Return:
+	========================================================================
+*/
+VOID AsicRemovePairwiseKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR		 BssIdx,
+	IN UCHAR		 Wcid)
+{
+	ULONG		WCIDAttri;
+	USHORT		offset;
+
+	// re-set the entry's WCID attribute as OPEN-NONE.
+	offset = MAC_WCID_ATTRIBUTE_BASE + (Wcid * HW_WCID_ATTRI_SIZE);
+	WCIDAttri = (BssIdx<<4) | PAIRWISEKEYTABLE;
+	RTMP_IO_WRITE32(pAd, offset, WCIDAttri);
+}
+
+BOOLEAN AsicSendCommandToMcu(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR		 Command,
+	IN UCHAR		 Token,
+	IN UCHAR		 Arg0,
+	IN UCHAR		 Arg1)
+{
+	HOST_CMD_CSR_STRUC	H2MCmd;
+	H2M_MAILBOX_STRUC	H2MMailbox;
+	ULONG				i = 0;
+#ifdef RT2860
+#ifdef RALINK_ATE
+	static UINT32 j = 0;
+#endif // RALINK_ATE //
+#endif // RT2860 //
+	do
+	{
+		RTMP_IO_READ32(pAd, H2M_MAILBOX_CSR, &H2MMailbox.word);
+		if (H2MMailbox.field.Owner == 0)
+			break;
+
+		RTMPusecDelay(2);
+	} while(i++ < 100);
+
+	if (i >= 100)
+	{
+#ifdef RT2860
+#ifdef RALINK_ATE
+		if (pAd->ate.bFWLoading == TRUE)
+		{
+			/* reloading firmware when received iwpriv cmd "ATE=ATESTOP" */
+			if (j > 0)
+			{
+				if (j % 64 != 0)
+				{
+					DBGPRINT(RT_DEBUG_ERROR, ("#"));
+				}
+				else
+				{
+					DBGPRINT(RT_DEBUG_ERROR, ("\n"));
+				}
+				++j;
+			}
+			else if (j == 0)
+			{
+				DBGPRINT(RT_DEBUG_ERROR, ("Loading firmware. Please wait for a moment...\n"));
+				++j;
+			}
+		}
+		else
+#endif // RALINK_ATE //
+#endif // RT2860 //
+		{
+		DBGPRINT_ERR(("H2M_MAILBOX still hold by MCU. command fail\n"));
+		}
+		return FALSE;
+	}
+
+#ifdef RT2860
+#ifdef RALINK_ATE
+	else if (pAd->ate.bFWLoading == TRUE)
+	{
+		/* reloading of firmware is completed */
+		pAd->ate.bFWLoading = FALSE;
+		DBGPRINT(RT_DEBUG_ERROR, ("\n"));
+		j = 0;
+	}
+#endif // RALINK_ATE //
+#endif // RT2860 //
+
+	H2MMailbox.field.Owner	  = 1;	   // pass ownership to MCU
+	H2MMailbox.field.CmdToken = Token;
+	H2MMailbox.field.HighByte = Arg1;
+	H2MMailbox.field.LowByte  = Arg0;
+	RTMP_IO_WRITE32(pAd, H2M_MAILBOX_CSR, H2MMailbox.word);
+
+	H2MCmd.word 			  = 0;
+	H2MCmd.field.HostCommand  = Command;
+	RTMP_IO_WRITE32(pAd, HOST_CMD_CSR, H2MCmd.word);
+
+	if (Command != 0x80)
+	{
+	}
+
+	return TRUE;
+}
+
+#ifdef RT2860
+BOOLEAN AsicCheckCommanOk(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR		 Command)
+{
+	UINT32	CmdStatus = 0, CID = 0, i;
+	UINT32	ThisCIDMask = 0;
+
+	i = 0;
+	do
+	{
+		RTMP_IO_READ32(pAd, H2M_MAILBOX_CID, &CID);
+		// Find where the command is. Because this is randomly specified by firmware.
+		if ((CID & CID0MASK) == Command)
+		{
+			ThisCIDMask = CID0MASK;
+			break;
+		}
+		else if ((((CID & CID1MASK)>>8) & 0xff) == Command)
+		{
+			ThisCIDMask = CID1MASK;
+			break;
+		}
+		else if ((((CID & CID2MASK)>>16) & 0xff) == Command)
+		{
+			ThisCIDMask = CID2MASK;
+			break;
+		}
+		else if ((((CID & CID3MASK)>>24) & 0xff) == Command)
+		{
+			ThisCIDMask = CID3MASK;
+			break;
+		}
+
+		RTMPusecDelay(100);
+		i++;
+	}while (i < 200);
+
+	// Get CommandStatus Value
+	RTMP_IO_READ32(pAd, H2M_MAILBOX_STATUS, &CmdStatus);
+
+	// This command's status is at the same position as command. So AND command position's bitmask to read status.
+	if (i < 200)
+	{
+		// If Status is 1, the comamnd is success.
+		if (((CmdStatus & ThisCIDMask) == 0x1) || ((CmdStatus & ThisCIDMask) == 0x100)
+			|| ((CmdStatus & ThisCIDMask) == 0x10000) || ((CmdStatus & ThisCIDMask) == 0x1000000))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("--> AsicCheckCommanOk CID = 0x%x, CmdStatus= 0x%x \n", CID, CmdStatus));
+			RTMP_IO_WRITE32(pAd, H2M_MAILBOX_STATUS, 0xffffffff);
+			RTMP_IO_WRITE32(pAd, H2M_MAILBOX_CID, 0xffffffff);
+			return TRUE;
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("--> AsicCheckCommanFail1 CID = 0x%x, CmdStatus= 0x%x \n", CID, CmdStatus));
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("--> AsicCheckCommanFail2 Timeout Command = %d, CmdStatus= 0x%x \n", Command, CmdStatus));
+	}
+	// Clear Command and Status.
+	RTMP_IO_WRITE32(pAd, H2M_MAILBOX_STATUS, 0xffffffff);
+	RTMP_IO_WRITE32(pAd, H2M_MAILBOX_CID, 0xffffffff);
+
+	return FALSE;
+}
+#endif // RT2860 //
+
+/*
+	========================================================================
+
+	Routine Description:
+		Verify the support rate for different PHY type
+
+	Arguments:
+		pAd 				Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	========================================================================
+*/
+VOID	RTMPCheckRates(
+	IN		PRTMP_ADAPTER	pAd,
+	IN OUT	UCHAR			SupRate[],
+	IN OUT	UCHAR			*SupRateLen)
+{
+	UCHAR	RateIdx, i, j;
+	UCHAR	NewRate[12], NewRateLen;
+
+	NewRateLen = 0;
+
+	if (pAd->CommonCfg.PhyMode == PHY_11B)
+		RateIdx = 4;
+	else
+		RateIdx = 12;
+
+	// Check for support rates exclude basic rate bit
+	for (i = 0; i < *SupRateLen; i++)
+		for (j = 0; j < RateIdx; j++)
+			if ((SupRate[i] & 0x7f) == RateIdTo500Kbps[j])
+				NewRate[NewRateLen++] = SupRate[i];
+
+	*SupRateLen = NewRateLen;
+	NdisMoveMemory(SupRate, NewRate, NewRateLen);
+}
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef DOT11_N_SUPPORT
+BOOLEAN RTMPCheckChannel(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR		CentralChannel,
+	IN UCHAR		Channel)
+{
+	UCHAR		k;
+	UCHAR		UpperChannel = 0, LowerChannel = 0;
+	UCHAR		NoEffectChannelinList = 0;
+
+	// Find upper and lower channel according to 40MHz current operation.
+	if (CentralChannel < Channel)
+	{
+		UpperChannel = Channel;
+		if (CentralChannel > 2)
+			LowerChannel = CentralChannel - 2;
+		else
+			return FALSE;
+	}
+	else if (CentralChannel > Channel)
+	{
+		UpperChannel = CentralChannel + 2;
+		LowerChannel = Channel;
+	}
+
+	for (k = 0;k < pAd->ChannelListNum;k++)
+	{
+		if (pAd->ChannelList[k].Channel == UpperChannel)
+		{
+			NoEffectChannelinList ++;
+		}
+		if (pAd->ChannelList[k].Channel == LowerChannel)
+		{
+			NoEffectChannelinList ++;
+		}
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("Total Channel in Channel List = [%d]\n", NoEffectChannelinList));
+	if (NoEffectChannelinList == 2)
+		return TRUE;
+	else
+		return FALSE;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Verify the support rate for HT phy type
+
+	Arguments:
+		pAd 				Pointer to our adapter
+
+	Return Value:
+		FALSE if pAd->CommonCfg.SupportedHtPhy doesn't accept the pHtCapability.  (AP Mode)
+
+	IRQL = PASSIVE_LEVEL
+
+	========================================================================
+*/
+BOOLEAN 	RTMPCheckHt(
+	IN	PRTMP_ADAPTER			pAd,
+	IN	UCHAR					Wcid,
+	IN 	HT_CAPABILITY_IE		*pHtCapability,
+	IN 	ADD_HT_INFO_IE			*pAddHtInfo)
+{
+	if (Wcid >= MAX_LEN_OF_MAC_TABLE)
+		return FALSE;
+
+	// If use AMSDU, set flag.
+	if (pAd->CommonCfg.DesiredHtPhy.AmsduEnable)
+		CLIENT_STATUS_SET_FLAG(&pAd->MacTab.Content[Wcid], fCLIENT_STATUS_AMSDU_INUSED);
+	// Save Peer Capability
+	if (pHtCapability->HtCapInfo.ShortGIfor20)
+		CLIENT_STATUS_SET_FLAG(&pAd->MacTab.Content[Wcid], fCLIENT_STATUS_SGI20_CAPABLE);
+	if (pHtCapability->HtCapInfo.ShortGIfor40)
+		CLIENT_STATUS_SET_FLAG(&pAd->MacTab.Content[Wcid], fCLIENT_STATUS_SGI40_CAPABLE);
+	if (pHtCapability->HtCapInfo.TxSTBC)
+		CLIENT_STATUS_SET_FLAG(&pAd->MacTab.Content[Wcid], fCLIENT_STATUS_TxSTBC_CAPABLE);
+	if (pHtCapability->HtCapInfo.RxSTBC)
+		CLIENT_STATUS_SET_FLAG(&pAd->MacTab.Content[Wcid], fCLIENT_STATUS_RxSTBC_CAPABLE);
+	if (pAd->CommonCfg.bRdg && pHtCapability->ExtHtCapInfo.RDGSupport)
+	{
+		CLIENT_STATUS_SET_FLAG(&pAd->MacTab.Content[Wcid], fCLIENT_STATUS_RDG_CAPABLE);
+	}
+
+	if (Wcid < MAX_LEN_OF_MAC_TABLE)
+	{
+		pAd->MacTab.Content[Wcid].MpduDensity = pHtCapability->HtCapParm.MpduDensity;
+	}
+
+	// Will check ChannelWidth for MCSSet[4] below
+	pAd->MlmeAux.HtCapability.MCSSet[4] = 0x1;
+    switch (pAd->CommonCfg.RxStream)
+	{
+		case 1:
+			pAd->MlmeAux.HtCapability.MCSSet[0] = 0xff;
+			pAd->MlmeAux.HtCapability.MCSSet[1] = 0x00;
+            pAd->MlmeAux.HtCapability.MCSSet[2] = 0x00;
+            pAd->MlmeAux.HtCapability.MCSSet[3] = 0x00;
+			break;
+		case 2:
+			pAd->MlmeAux.HtCapability.MCSSet[0] = 0xff;
+			pAd->MlmeAux.HtCapability.MCSSet[1] = 0xff;
+            pAd->MlmeAux.HtCapability.MCSSet[2] = 0x00;
+            pAd->MlmeAux.HtCapability.MCSSet[3] = 0x00;
+			break;
+		case 3:
+			pAd->MlmeAux.HtCapability.MCSSet[0] = 0xff;
+			pAd->MlmeAux.HtCapability.MCSSet[1] = 0xff;
+            pAd->MlmeAux.HtCapability.MCSSet[2] = 0xff;
+            pAd->MlmeAux.HtCapability.MCSSet[3] = 0x00;
+			break;
+	}
+
+	pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth = pAddHtInfo->AddHtInfo.RecomWidth & pAd->CommonCfg.DesiredHtPhy.ChannelWidth;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("RTMPCheckHt:: HtCapInfo.ChannelWidth=%d, RecomWidth=%d, DesiredHtPhy.ChannelWidth=%d, BW40MAvailForA/G=%d/%d, PhyMode=%d \n",
+		pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth, pAddHtInfo->AddHtInfo.RecomWidth, pAd->CommonCfg.DesiredHtPhy.ChannelWidth,
+		pAd->NicConfig2.field.BW40MAvailForA, pAd->NicConfig2.field.BW40MAvailForG, pAd->CommonCfg.PhyMode));
+
+	pAd->MlmeAux.HtCapability.HtCapInfo.GF =  pHtCapability->HtCapInfo.GF &pAd->CommonCfg.DesiredHtPhy.GF;
+
+	// Send Assoc Req with my HT capability.
+	pAd->MlmeAux.HtCapability.HtCapInfo.AMsduSize =  pAd->CommonCfg.DesiredHtPhy.AmsduSize;
+	pAd->MlmeAux.HtCapability.HtCapInfo.MimoPs =  pAd->CommonCfg.DesiredHtPhy.MimoPs;
+	pAd->MlmeAux.HtCapability.HtCapInfo.ShortGIfor20 =  (pAd->CommonCfg.DesiredHtPhy.ShortGIfor20) & (pHtCapability->HtCapInfo.ShortGIfor20);
+	pAd->MlmeAux.HtCapability.HtCapInfo.ShortGIfor40 =  (pAd->CommonCfg.DesiredHtPhy.ShortGIfor40) & (pHtCapability->HtCapInfo.ShortGIfor40);
+	pAd->MlmeAux.HtCapability.HtCapInfo.TxSTBC =  (pAd->CommonCfg.DesiredHtPhy.TxSTBC)&(pHtCapability->HtCapInfo.RxSTBC);
+	pAd->MlmeAux.HtCapability.HtCapInfo.RxSTBC =  (pAd->CommonCfg.DesiredHtPhy.RxSTBC)&(pHtCapability->HtCapInfo.TxSTBC);
+	pAd->MlmeAux.HtCapability.HtCapParm.MaxRAmpduFactor = pAd->CommonCfg.DesiredHtPhy.MaxRAmpduFactor;
+    pAd->MlmeAux.HtCapability.HtCapParm.MpduDensity = pAd->CommonCfg.HtCapability.HtCapParm.MpduDensity;
+	pAd->MlmeAux.HtCapability.ExtHtCapInfo.PlusHTC = pHtCapability->ExtHtCapInfo.PlusHTC;
+	pAd->MacTab.Content[Wcid].HTCapability.ExtHtCapInfo.PlusHTC = pHtCapability->ExtHtCapInfo.PlusHTC;
+	if (pAd->CommonCfg.bRdg)
+	{
+		pAd->MlmeAux.HtCapability.ExtHtCapInfo.RDGSupport = pHtCapability->ExtHtCapInfo.RDGSupport;
+        pAd->MlmeAux.HtCapability.ExtHtCapInfo.PlusHTC = 1;
+	}
+
+    if (pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth == BW_20)
+        pAd->MlmeAux.HtCapability.MCSSet[4] = 0x0;  // BW20 can't transmit MCS32
+
+	COPY_AP_HTSETTINGS_FROM_BEACON(pAd, pHtCapability);
+	return TRUE;
+}
+#endif // DOT11_N_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+/*
+	========================================================================
+
+	Routine Description:
+		Verify the support rate for different PHY type
+
+	Arguments:
+		pAd 				Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	========================================================================
+*/
+VOID RTMPUpdateMlmeRate(
+	IN PRTMP_ADAPTER	pAd)
+{
+	UCHAR	MinimumRate;
+	UCHAR	ProperMlmeRate; //= RATE_54;
+	UCHAR	i, j, RateIdx = 12; //1, 2, 5.5, 11, 6, 9, 12, 18, 24, 36, 48, 54
+	BOOLEAN	bMatch = FALSE;
+
+	switch (pAd->CommonCfg.PhyMode)
+	{
+		case PHY_11B:
+			ProperMlmeRate = RATE_11;
+			MinimumRate = RATE_1;
+			break;
+		case PHY_11BG_MIXED:
+#ifdef DOT11_N_SUPPORT
+		case PHY_11ABGN_MIXED:
+		case PHY_11BGN_MIXED:
+#endif // DOT11_N_SUPPORT //
+			if ((pAd->MlmeAux.SupRateLen == 4) &&
+				(pAd->MlmeAux.ExtRateLen == 0))
+				// B only AP
+				ProperMlmeRate = RATE_11;
+			else
+				ProperMlmeRate = RATE_24;
+
+			if (pAd->MlmeAux.Channel <= 14)
+				MinimumRate = RATE_1;
+			else
+				MinimumRate = RATE_6;
+			break;
+		case PHY_11A:
+#ifdef DOT11_N_SUPPORT
+		case PHY_11N_2_4G:	// rt2860 need to check mlmerate for 802.11n
+		case PHY_11GN_MIXED:
+		case PHY_11AGN_MIXED:
+		case PHY_11AN_MIXED:
+		case PHY_11N_5G:
+#endif // DOT11_N_SUPPORT //
+			ProperMlmeRate = RATE_24;
+			MinimumRate = RATE_6;
+			break;
+		case PHY_11ABG_MIXED:
+			ProperMlmeRate = RATE_24;
+			if (pAd->MlmeAux.Channel <= 14)
+			   MinimumRate = RATE_1;
+			else
+				MinimumRate = RATE_6;
+			break;
+		default: // error
+			ProperMlmeRate = RATE_1;
+			MinimumRate = RATE_1;
+			break;
+	}
+
+	for (i = 0; i < pAd->MlmeAux.SupRateLen; i++)
+	{
+		for (j = 0; j < RateIdx; j++)
+		{
+			if ((pAd->MlmeAux.SupRate[i] & 0x7f) == RateIdTo500Kbps[j])
+			{
+				if (j == ProperMlmeRate)
+				{
+					bMatch = TRUE;
+					break;
+				}
+			}
+		}
+
+		if (bMatch)
+			break;
+	}
+
+	if (bMatch == FALSE)
+	{
+		for (i = 0; i < pAd->MlmeAux.ExtRateLen; i++)
+		{
+			for (j = 0; j < RateIdx; j++)
+			{
+				if ((pAd->MlmeAux.ExtRate[i] & 0x7f) == RateIdTo500Kbps[j])
+				{
+					if (j == ProperMlmeRate)
+					{
+						bMatch = TRUE;
+						break;
+					}
+				}
+			}
+
+			if (bMatch)
+				break;
+		}
+	}
+
+	if (bMatch == FALSE)
+	{
+		ProperMlmeRate = MinimumRate;
+	}
+
+	pAd->CommonCfg.MlmeRate = MinimumRate;
+	pAd->CommonCfg.RtsRate = ProperMlmeRate;
+	if (pAd->CommonCfg.MlmeRate >= RATE_6)
+	{
+		pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_OFDM;
+		pAd->CommonCfg.MlmeTransmit.field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MlmeRate];
+		pAd->MacTab.Content[BSS0Mcast_WCID].HTPhyMode.field.MODE = MODE_OFDM;
+		pAd->MacTab.Content[BSS0Mcast_WCID].HTPhyMode.field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MlmeRate];
+	}
+	else
+	{
+		pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_CCK;
+		pAd->CommonCfg.MlmeTransmit.field.MCS = pAd->CommonCfg.MlmeRate;
+		pAd->MacTab.Content[BSS0Mcast_WCID].HTPhyMode.field.MODE = MODE_CCK;
+		pAd->MacTab.Content[BSS0Mcast_WCID].HTPhyMode.field.MCS = pAd->CommonCfg.MlmeRate;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPUpdateMlmeRate ==>   MlmeTransmit = 0x%x  \n" , pAd->CommonCfg.MlmeTransmit.word));
+}
+
+CHAR RTMPMaxRssi(
+	IN PRTMP_ADAPTER	pAd,
+	IN CHAR				Rssi0,
+	IN CHAR				Rssi1,
+	IN CHAR				Rssi2)
+{
+	CHAR	larger = -127;
+
+	if ((pAd->Antenna.field.RxPath == 1) && (Rssi0 != 0))
+	{
+		larger = Rssi0;
+	}
+
+	if ((pAd->Antenna.field.RxPath >= 2) && (Rssi1 != 0))
+	{
+		larger = max(Rssi0, Rssi1);
+	}
+
+	if ((pAd->Antenna.field.RxPath == 3) && (Rssi2 != 0))
+	{
+		larger = max(larger, Rssi2);
+	}
+
+	if (larger == -127)
+		larger = 0;
+
+	return larger;
+}
+
+/*
+    ========================================================================
+    Routine Description:
+        Periodic evaluate antenna link status
+
+    Arguments:
+        pAd         - Adapter pointer
+
+    Return Value:
+        None
+
+    ========================================================================
+*/
+VOID AsicEvaluateRxAnt(
+	IN PRTMP_ADAPTER	pAd)
+{
+	UCHAR	BBPR3 = 0;
+
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS	|
+								fRTMP_ADAPTER_HALT_IN_PROGRESS	|
+								fRTMP_ADAPTER_RADIO_OFF			|
+								fRTMP_ADAPTER_NIC_NOT_EXIST		|
+								fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+			return;
+
+		if (pAd->StaCfg.Psm == PWR_SAVE)
+			return;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BBPR3);
+	BBPR3 &= (~0x18);
+	if(pAd->Antenna.field.RxPath == 3)
+	{
+		BBPR3 |= (0x10);
+	}
+	else if(pAd->Antenna.field.RxPath == 2)
+	{
+		BBPR3 |= (0x8);
+	}
+	else if(pAd->Antenna.field.RxPath == 1)
+	{
+		BBPR3 |= (0x0);
+	}
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BBPR3);
+#ifdef CONFIG_STA_SUPPORT
+#ifdef RT2860
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+    	pAd->StaCfg.BBPR3 = BBPR3;
+#endif // RT2860 //
+#endif // CONFIG_STA_SUPPORT //
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED)
+		)
+	{
+		ULONG	TxTotalCnt = pAd->RalinkCounters.OneSecTxNoRetryOkCount +
+								pAd->RalinkCounters.OneSecTxRetryOkCount +
+								pAd->RalinkCounters.OneSecTxFailCount;
+
+		if (TxTotalCnt > 50)
+		{
+			RTMPSetTimer(&pAd->Mlme.RxAntEvalTimer, 20);
+			pAd->Mlme.bLowThroughput = FALSE;
+		}
+		else
+		{
+			RTMPSetTimer(&pAd->Mlme.RxAntEvalTimer, 300);
+			pAd->Mlme.bLowThroughput = TRUE;
+		}
+	}
+}
+
+/*
+    ========================================================================
+    Routine Description:
+        After evaluation, check antenna link status
+
+    Arguments:
+        pAd         - Adapter pointer
+
+    Return Value:
+        None
+
+    ========================================================================
+*/
+VOID AsicRxAntEvalTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+	RTMP_ADAPTER	*pAd = (RTMP_ADAPTER *)FunctionContext;
+#ifdef CONFIG_STA_SUPPORT
+	UCHAR			BBPR3 = 0;
+	CHAR			larger = -127, rssi0, rssi1, rssi2;
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)	||
+			RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)		||
+			RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF)			||
+			RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST))
+			return;
+
+		if (pAd->StaCfg.Psm == PWR_SAVE)
+			return;
+
+
+		// if the traffic is low, use average rssi as the criteria
+		if (pAd->Mlme.bLowThroughput == TRUE)
+		{
+			rssi0 = pAd->StaCfg.RssiSample.LastRssi0;
+			rssi1 = pAd->StaCfg.RssiSample.LastRssi1;
+			rssi2 = pAd->StaCfg.RssiSample.LastRssi2;
+		}
+		else
+		{
+			rssi0 = pAd->StaCfg.RssiSample.AvgRssi0;
+			rssi1 = pAd->StaCfg.RssiSample.AvgRssi1;
+			rssi2 = pAd->StaCfg.RssiSample.AvgRssi2;
+		}
+
+		if(pAd->Antenna.field.RxPath == 3)
+		{
+			larger = max(rssi0, rssi1);
+
+			if (larger > (rssi2 + 20))
+				pAd->Mlme.RealRxPath = 2;
+			else
+				pAd->Mlme.RealRxPath = 3;
+		}
+		else if(pAd->Antenna.field.RxPath == 2)
+		{
+			if (rssi0 > (rssi1 + 20))
+				pAd->Mlme.RealRxPath = 1;
+			else
+				pAd->Mlme.RealRxPath = 2;
+		}
+
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BBPR3);
+		BBPR3 &= (~0x18);
+		if(pAd->Mlme.RealRxPath == 3)
+		{
+			BBPR3 |= (0x10);
+		}
+		else if(pAd->Mlme.RealRxPath == 2)
+		{
+			BBPR3 |= (0x8);
+		}
+		else if(pAd->Mlme.RealRxPath == 1)
+		{
+			BBPR3 |= (0x0);
+		}
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BBPR3);
+#ifdef RT2860
+    pAd->StaCfg.BBPR3 = BBPR3;
+#endif // RT2860 //
+	}
+
+#endif // CONFIG_STA_SUPPORT //
+
+}
+
+
+
+VOID APSDPeriodicExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+	RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+
+	if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+		return;
+
+	pAd->CommonCfg.TriggerTimerCount++;
+
+}
+
+/*
+    ========================================================================
+    Routine Description:
+        Set/reset MAC registers according to bPiggyBack parameter
+
+    Arguments:
+        pAd         - Adapter pointer
+        bPiggyBack  - Enable / Disable Piggy-Back
+
+    Return Value:
+        None
+
+    ========================================================================
+*/
+VOID RTMPSetPiggyBack(
+    IN PRTMP_ADAPTER    pAd,
+    IN BOOLEAN          bPiggyBack)
+{
+	TX_LINK_CFG_STRUC  TxLinkCfg;
+
+	RTMP_IO_READ32(pAd, TX_LINK_CFG, &TxLinkCfg.word);
+
+	TxLinkCfg.field.TxCFAckEn = bPiggyBack;
+	RTMP_IO_WRITE32(pAd, TX_LINK_CFG, TxLinkCfg.word);
+}
+
+/*
+    ========================================================================
+    Routine Description:
+        check if this entry need to switch rate automatically
+
+    Arguments:
+        pAd
+        pEntry
+
+    Return Value:
+        TURE
+        FALSE
+
+    ========================================================================
+*/
+BOOLEAN RTMPCheckEntryEnableAutoRateSwitch(
+	IN PRTMP_ADAPTER    pAd,
+	IN PMAC_TABLE_ENTRY	pEntry)
+{
+	BOOLEAN		result = TRUE;
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// only associated STA counts
+		if (pEntry && (pEntry->ValidAsCLI) && (pEntry->Sst == SST_ASSOC))
+		{
+			result = pAd->StaCfg.bAutoTxRateSwitch;
+		}
+		else
+			result = FALSE;
+
+#ifdef QOS_DLS_SUPPORT
+		if (pEntry && (pEntry->ValidAsDls))
+			result = pAd->StaCfg.bAutoTxRateSwitch;
+#endif // QOS_DLS_SUPPORT //
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+
+
+	return result;
+}
+
+
+BOOLEAN RTMPAutoRateSwitchCheck(
+	IN PRTMP_ADAPTER    pAd)
+{
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (pAd->StaCfg.bAutoTxRateSwitch)
+			return TRUE;
+	}
+#endif // CONFIG_STA_SUPPORT //
+	return FALSE;
+}
+
+
+/*
+    ========================================================================
+    Routine Description:
+        check if this entry need to fix tx legacy rate
+
+    Arguments:
+        pAd
+        pEntry
+
+    Return Value:
+        TURE
+        FALSE
+
+    ========================================================================
+*/
+UCHAR RTMPStaFixedTxMode(
+	IN PRTMP_ADAPTER    pAd,
+	IN PMAC_TABLE_ENTRY	pEntry)
+{
+	UCHAR	tx_mode = FIXED_TXMODE_HT;
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		tx_mode = (UCHAR)pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	return tx_mode;
+}
+
+/*
+    ========================================================================
+    Routine Description:
+        Overwrite HT Tx Mode by Fixed Legency Tx Mode, if specified.
+
+    Arguments:
+        pAd
+        pEntry
+
+    Return Value:
+        TURE
+        FALSE
+
+    ========================================================================
+*/
+VOID RTMPUpdateLegacyTxSetting(
+		UCHAR				fixed_tx_mode,
+		PMAC_TABLE_ENTRY	pEntry)
+{
+	HTTRANSMIT_SETTING TransmitSetting;
+
+	if (fixed_tx_mode == FIXED_TXMODE_HT)
+		return;
+
+	TransmitSetting.word = 0;
+
+	TransmitSetting.field.MODE = pEntry->HTPhyMode.field.MODE;
+	TransmitSetting.field.MCS = pEntry->HTPhyMode.field.MCS;
+
+	if (fixed_tx_mode == FIXED_TXMODE_CCK)
+	{
+		TransmitSetting.field.MODE = MODE_CCK;
+		// CCK mode allow MCS 0~3
+		if (TransmitSetting.field.MCS > MCS_3)
+			TransmitSetting.field.MCS = MCS_3;
+	}
+	else
+	{
+		TransmitSetting.field.MODE = MODE_OFDM;
+		// OFDM mode allow MCS 0~7
+		if (TransmitSetting.field.MCS > MCS_7)
+			TransmitSetting.field.MCS = MCS_7;
+	}
+
+	if (pEntry->HTPhyMode.field.MODE >= TransmitSetting.field.MODE)
+	{
+		pEntry->HTPhyMode.word = TransmitSetting.word;
+		DBGPRINT(RT_DEBUG_TRACE, ("RTMPUpdateLegacyTxSetting : wcid-%d, MODE=%s, MCS=%d \n",
+				pEntry->Aid, GetPhyMode(pEntry->HTPhyMode.field.MODE), pEntry->HTPhyMode.field.MCS));
+	}
+}
+
+#ifdef CONFIG_STA_SUPPORT
+/*
+	==========================================================================
+	Description:
+		dynamic tune BBP R66 to find a balance between sensibility and
+		noise isolation
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicStaBbpTuning(
+	IN PRTMP_ADAPTER pAd)
+{
+	UCHAR	OrigR66Value = 0, R66;//, R66UpperBound = 0x30, R66LowerBound = 0x30;
+	CHAR	Rssi;
+
+	// 2860C did not support Fase CCA, therefore can't tune
+	if (pAd->MACVersion == 0x28600100)
+		return;
+
+	//
+	// work as a STA
+	//
+	if (pAd->Mlme.CntlMachine.CurrState != CNTL_IDLE)  // no R66 tuning when SCANNING
+		return;
+
+	if ((pAd->OpMode == OPMODE_STA)
+		&& (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED)
+			)
+		&& !(OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+#ifdef RT2860
+		&& (pAd->bPCIclkOff == FALSE)
+#endif // RT2860 //
+		)
+	{
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R66, &OrigR66Value);
+		R66 = OrigR66Value;
+
+		if (pAd->Antenna.field.RxPath > 1)
+			Rssi = (pAd->StaCfg.RssiSample.AvgRssi0 + pAd->StaCfg.RssiSample.AvgRssi1) >> 1;
+		else
+			Rssi = pAd->StaCfg.RssiSample.AvgRssi0;
+
+		if (pAd->LatchRfRegs.Channel <= 14)
+		{	//BG band
+			{
+				if (Rssi > RSSI_FOR_MID_LOW_SENSIBILITY)
+				{
+					R66 = (0x2E + GET_LNA_GAIN(pAd)) + 0x10;
+					if (OrigR66Value != R66)
+					{
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+					}
+				}
+				else
+				{
+					R66 = 0x2E + GET_LNA_GAIN(pAd);
+					if (OrigR66Value != R66)
+					{
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+					}
+				}
+			}
+		}
+		else
+		{	//A band
+			if (pAd->CommonCfg.BBPCurrentBW == BW_20)
+			{
+				if (Rssi > RSSI_FOR_MID_LOW_SENSIBILITY)
+				{
+					R66 = 0x32 + (GET_LNA_GAIN(pAd)*5)/3 + 0x10;
+					if (OrigR66Value != R66)
+					{
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+					}
+				}
+				else
+				{
+					R66 = 0x32 + (GET_LNA_GAIN(pAd)*5)/3;
+					if (OrigR66Value != R66)
+					{
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+					}
+				}
+			}
+			else
+			{
+				if (Rssi > RSSI_FOR_MID_LOW_SENSIBILITY)
+				{
+					R66 = 0x3A + (GET_LNA_GAIN(pAd)*5)/3 + 0x10;
+					if (OrigR66Value != R66)
+					{
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+					}
+				}
+				else
+				{
+					R66 = 0x3A + (GET_LNA_GAIN(pAd)*5)/3;
+					if (OrigR66Value != R66)
+					{
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+					}
+				}
+			}
+		}
+
+
+	}
+}
+#endif // CONFIG_STA_SUPPORT //
+
+VOID RTMPSetAGCInitValue(
+	IN PRTMP_ADAPTER	pAd,
+	IN UCHAR			BandWidth)
+{
+	UCHAR	R66 = 0x30;
+
+	if (pAd->LatchRfRegs.Channel <= 14)
+	{	// BG band
+		R66 = 0x2E + GET_LNA_GAIN(pAd);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+	}
+	else
+	{	//A band
+		if (BandWidth == BW_20)
+		{
+			R66 = (UCHAR)(0x32 + (GET_LNA_GAIN(pAd)*5)/3);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+		}
+#ifdef DOT11_N_SUPPORT
+		else
+		{
+			R66 = (UCHAR)(0x3A + (GET_LNA_GAIN(pAd)*5)/3);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+		}
+#endif // DOT11_N_SUPPORT //
+	}
+
+}
+
+VOID AsicTurnOffRFClk(
+	IN PRTMP_ADAPTER pAd,
+	IN	UCHAR		Channel)
+{
+
+	// RF R2 bit 18 = 0
+	UINT32			R1 = 0, R2 = 0, R3 = 0;
+	UCHAR			index;
+	RTMP_RF_REGS	*RFRegTable;
+
+	RFRegTable = RF2850RegTable;
+
+	switch (pAd->RfIcType)
+	{
+		case RFIC_2820:
+		case RFIC_2850:
+		case RFIC_2720:
+		case RFIC_2750:
+
+			for (index = 0; index < NUM_OF_2850_CHNL; index++)
+			{
+				if (Channel == RFRegTable[index].Channel)
+				{
+					R1 = RFRegTable[index].R1 & 0xffffdfff;
+					R2 = RFRegTable[index].R2 & 0xfffbffff;
+					R3 = RFRegTable[index].R3 & 0xfff3ffff;
+
+					RTMP_RF_IO_WRITE32(pAd, R1);
+					RTMP_RF_IO_WRITE32(pAd, R2);
+
+					// Program R1b13 to 1, R3/b18,19 to 0, R2b18 to 0.
+					// Set RF R2 bit18=0, R3 bit[18:19]=0
+					//if (pAd->StaCfg.bRadio == FALSE)
+					if (1)
+					{
+						RTMP_RF_IO_WRITE32(pAd, R3);
+
+						DBGPRINT(RT_DEBUG_TRACE, ("AsicTurnOffRFClk#%d(RF=%d, ) , R2=0x%08x,  R3 = 0x%08x \n",
+							Channel, pAd->RfIcType, R2, R3));
+					}
+					else
+						DBGPRINT(RT_DEBUG_TRACE, ("AsicTurnOffRFClk#%d(RF=%d, ) , R2=0x%08x \n",
+							Channel, pAd->RfIcType, R2));
+					break;
+				}
+			}
+			break;
+
+		default:
+			break;
+	}
+}
+
+
+VOID AsicTurnOnRFClk(
+	IN PRTMP_ADAPTER pAd,
+	IN	UCHAR			Channel)
+{
+
+	// RF R2 bit 18 = 0
+	UINT32			R1 = 0, R2 = 0, R3 = 0;
+	UCHAR			index;
+	RTMP_RF_REGS	*RFRegTable;
+
+	RFRegTable = RF2850RegTable;
+
+	switch (pAd->RfIcType)
+	{
+		case RFIC_2820:
+		case RFIC_2850:
+		case RFIC_2720:
+		case RFIC_2750:
+
+			for (index = 0; index < NUM_OF_2850_CHNL; index++)
+			{
+				if (Channel == RFRegTable[index].Channel)
+				{
+					R3 = pAd->LatchRfRegs.R3;
+					R3 &= 0xfff3ffff;
+					R3 |= 0x00080000;
+					RTMP_RF_IO_WRITE32(pAd, R3);
+
+					R1 = RFRegTable[index].R1;
+					RTMP_RF_IO_WRITE32(pAd, R1);
+
+					R2 = RFRegTable[index].R2;
+					if (pAd->Antenna.field.TxPath == 1)
+					{
+						R2 |= 0x4000;	// If TXpath is 1, bit 14 = 1;
+					}
+
+					if (pAd->Antenna.field.RxPath == 2)
+					{
+						R2 |= 0x40;	// write 1 to off Rxpath.
+					}
+					else if (pAd->Antenna.field.RxPath == 1)
+					{
+						R2 |= 0x20040;	// write 1 to off RxPath
+					}
+					RTMP_RF_IO_WRITE32(pAd, R2);
+
+					break;
+				}
+			}
+			break;
+
+		default:
+			break;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("AsicTurnOnRFClk#%d(RF=%d, ) , R2=0x%08x\n",
+		Channel,
+		pAd->RfIcType,
+		R2));
+}
+
diff --git a/drivers/staging/rt2860/common/netif_block.c b/drivers/staging/rt2860/common/netif_block.c
new file mode 100644
index 00000000000..d3f7d087e7f
--- /dev/null
+++ b/drivers/staging/rt2860/common/netif_block.c
@@ -0,0 +1,144 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#include "../rt_config.h"
+#include "netif_block.h"
+
+static NETIF_ENTRY freeNetIfEntryPool[FREE_NETIF_POOL_SIZE];
+static LIST_HEADER freeNetIfEntryList;
+
+void initblockQueueTab(
+	IN PRTMP_ADAPTER pAd)
+{
+	int i;
+
+	initList(&freeNetIfEntryList);
+	for (i = 0; i < FREE_NETIF_POOL_SIZE; i++)
+		insertTailList(&freeNetIfEntryList, (PLIST_ENTRY)&freeNetIfEntryPool[i]);
+
+	for (i=0; i < NUM_OF_TX_RING; i++)
+		initList(&pAd->blockQueueTab[i].NetIfList);
+
+	return;
+}
+
+BOOLEAN blockNetIf(
+	IN PBLOCK_QUEUE_ENTRY pBlockQueueEntry,
+	IN PNET_DEV pNetDev)
+{
+	PNETIF_ENTRY pNetIfEntry = NULL;
+
+	if ((pNetIfEntry = (PNETIF_ENTRY)removeHeadList(&freeNetIfEntryList)) != NULL)
+	{
+		netif_stop_queue(pNetDev);
+		pNetIfEntry->pNetDev = pNetDev;
+		insertTailList(&pBlockQueueEntry->NetIfList, (PLIST_ENTRY)pNetIfEntry);
+
+		pBlockQueueEntry->SwTxQueueBlockFlag = TRUE;
+		DBGPRINT(RT_DEBUG_TRACE, ("netif_stop_queue(%s)\n", pNetDev->name));
+	}
+	else
+		return FALSE;
+
+	return TRUE;
+}
+
+VOID releaseNetIf(
+	IN PBLOCK_QUEUE_ENTRY pBlockQueueEntry)
+{
+	PNETIF_ENTRY pNetIfEntry = NULL;
+	PLIST_HEADER pNetIfList = &pBlockQueueEntry->NetIfList;
+
+	while((pNetIfEntry = (PNETIF_ENTRY)removeHeadList(pNetIfList)) !=  NULL)
+	{
+		PNET_DEV pNetDev = pNetIfEntry->pNetDev;
+		netif_wake_queue(pNetDev);
+		insertTailList(&freeNetIfEntryList, (PLIST_ENTRY)pNetIfEntry);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("netif_wake_queue(%s)\n", pNetDev->name));
+	}
+	pBlockQueueEntry->SwTxQueueBlockFlag = FALSE;
+	return;
+}
+
+
+VOID StopNetIfQueue(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR QueIdx,
+	IN PNDIS_PACKET pPacket)
+{
+	PNET_DEV NetDev = NULL;
+	UCHAR IfIdx = 0;
+	BOOLEAN valid = FALSE;
+
+#ifdef APCLI_SUPPORT
+	if (RTMP_GET_PACKET_NET_DEVICE(pPacket) >= MIN_NET_DEVICE_FOR_APCLI)
+	{
+		IfIdx = (RTMP_GET_PACKET_NET_DEVICE(pPacket) - MIN_NET_DEVICE_FOR_APCLI) % MAX_APCLI_NUM;
+		NetDev = pAd->ApCfg.ApCliTab[IfIdx].dev;
+	}
+	else
+#endif // APCLI_SUPPORT //
+#ifdef WDS_SUPPORT
+	if (RTMP_GET_PACKET_NET_DEVICE(pPacket) >= MIN_NET_DEVICE_FOR_WDS)
+	{
+		IfIdx = (RTMP_GET_PACKET_NET_DEVICE(pPacket) - MIN_NET_DEVICE_FOR_WDS) % MAX_WDS_ENTRY;
+		NetDev = pAd->WdsTab.WdsEntry[IfIdx].dev;
+	}
+	else
+#endif // WDS_SUPPORT //
+	{
+#ifdef MBSS_SUPPORT
+		if (pAd->OpMode == OPMODE_AP)
+		{
+			IfIdx = (RTMP_GET_PACKET_NET_DEVICE(pPacket) - MIN_NET_DEVICE_FOR_MBSSID) % MAX_MBSSID_NUM;
+			NetDev = pAd->ApCfg.MBSSID[IfIdx].MSSIDDev;
+		}
+		else
+		{
+			IfIdx = MAIN_MBSSID;
+			NetDev = pAd->net_dev;
+		}
+#else
+		IfIdx = MAIN_MBSSID;
+		NetDev = pAd->net_dev;
+#endif
+	}
+
+	// WMM support 4 software queues.
+	// One software queue full doesn't mean device have no capbility to transmit packet.
+	// So disable block Net-If queue function while WMM enable.
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		valid = (pAd->CommonCfg.bWmmCapable == TRUE) ? FALSE : TRUE;
+#endif // CONFIG_STA_SUPPORT //
+
+	if (valid)
+		blockNetIf(&pAd->blockQueueTab[QueIdx], NetDev);
+	return;
+}
+
diff --git a/drivers/staging/rt2860/common/netif_block.h b/drivers/staging/rt2860/common/netif_block.h
new file mode 100644
index 00000000000..6e5151c4109
--- /dev/null
+++ b/drivers/staging/rt2860/common/netif_block.h
@@ -0,0 +1,58 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#ifndef __NET_IF_BLOCK_H__
+#define __NET_IF_BLOCK_H__
+
+//#include <linux/device.h>
+#include "link_list.h"
+#include "rtmp.h"
+
+#define FREE_NETIF_POOL_SIZE 32
+
+typedef struct _NETIF_ENTRY
+{
+	struct _NETIF_ENTRY *pNext;
+	PNET_DEV pNetDev;
+} NETIF_ENTRY, *PNETIF_ENTRY;
+
+void initblockQueueTab(
+	IN PRTMP_ADAPTER pAd);
+
+BOOLEAN blockNetIf(
+	IN PBLOCK_QUEUE_ENTRY pBlockQueueEntry,
+	IN PNET_DEV pNetDev);
+
+VOID releaseNetIf(
+	IN PBLOCK_QUEUE_ENTRY pBlockQueueEntry);
+
+VOID StopNetIfQueue(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR QueIdx,
+	IN PNDIS_PACKET pPacket);
+#endif // __NET_IF_BLOCK_H__
+
diff --git a/drivers/staging/rt2860/common/rtmp_init.c b/drivers/staging/rt2860/common/rtmp_init.c
new file mode 100644
index 00000000000..84edfa52dec
--- /dev/null
+++ b/drivers/staging/rt2860/common/rtmp_init.c
@@ -0,0 +1,3744 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rtmp_init.c
+
+	Abstract:
+	Miniport generic portion header file
+
+	Revision History:
+	Who         When          What
+	--------    ----------    ----------------------------------------------
+	Paul Lin    2002-08-01    created
+    John Chang  2004-08-20    RT2561/2661 use scatter-gather scheme
+    Jan Lee  2006-09-15    RT2860. Change for 802.11n , EEPROM, Led, BA, HT.
+*/
+#include "../rt_config.h"
+#include 	"firmware.h"
+#include <linux/bitrev.h>
+
+UCHAR    BIT8[] = {0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80};
+ULONG    BIT32[] = {0x00000001, 0x00000002, 0x00000004, 0x00000008,
+					0x00000010, 0x00000020, 0x00000040, 0x00000080,
+					0x00000100, 0x00000200, 0x00000400, 0x00000800,
+					0x00001000, 0x00002000, 0x00004000, 0x00008000,
+					0x00010000, 0x00020000, 0x00040000, 0x00080000,
+					0x00100000, 0x00200000, 0x00400000, 0x00800000,
+					0x01000000, 0x02000000, 0x04000000, 0x08000000,
+					0x10000000, 0x20000000, 0x40000000, 0x80000000};
+
+char*   CipherName[] = {"none","wep64","wep128","TKIP","AES","CKIP64","CKIP128"};
+
+const unsigned short ccitt_16Table[] = {
+	0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7,
+	0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF,
+	0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6,
+	0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE,
+	0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485,
+	0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D,
+	0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4,
+	0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC,
+	0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823,
+	0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B,
+	0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12,
+	0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A,
+	0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41,
+	0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49,
+	0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70,
+	0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78,
+	0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F,
+	0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067,
+	0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E,
+	0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256,
+	0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D,
+	0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
+	0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C,
+	0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634,
+	0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB,
+	0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3,
+	0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A,
+	0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92,
+	0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9,
+	0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1,
+	0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8,
+	0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0
+};
+#define ByteCRC16(v, crc) \
+	(unsigned short)((crc << 8) ^  ccitt_16Table[((crc >> 8) ^ (v)) & 255])
+
+//
+// BBP register initialization set
+//
+REG_PAIR   BBPRegTable[] = {
+	{BBP_R65,		0x2C},		// fix rssi issue
+	{BBP_R66,		0x38},	// Also set this default value to pAd->BbpTuning.R66CurrentValue at initial
+	{BBP_R69,		0x12},
+	{BBP_R70,		0xa},	// BBP_R70 will change to 0x8 in ApStartUp and LinkUp for rt2860C, otherwise value is 0xa
+	{BBP_R73,		0x10},
+	{BBP_R81,		0x37},
+	{BBP_R82,		0x62},
+	{BBP_R83,		0x6A},
+	{BBP_R84,		0x99},	// 0x19 is for rt2860E and after. This is for extension channel overlapping IOT. 0x99 is for rt2860D and before
+	{BBP_R86,		0x00},	// middle range issue, Rory @2008-01-28
+	{BBP_R91,		0x04},	// middle range issue, Rory @2008-01-28
+	{BBP_R92,		0x00},	// middle range issue, Rory @2008-01-28
+	{BBP_R103,  	0x00}, 	// near range high-power issue, requested from Gary @2008-0528
+	{BBP_R105,		0x05},	// 0x05 is for rt2860E to turn on FEQ control. It is safe for rt2860D and before, because Bit 7:2 are reserved in rt2860D and before.
+};
+#define	NUM_BBP_REG_PARMS	(sizeof(BBPRegTable) / sizeof(REG_PAIR))
+
+//
+// RF register initialization set
+//
+
+//
+// ASIC register initialization sets
+//
+
+RTMP_REG_PAIR	MACRegTable[] =	{
+#if defined(HW_BEACON_OFFSET) && (HW_BEACON_OFFSET == 0x200)
+	{BCN_OFFSET0,			0xf8f0e8e0}, /* 0x3800(e0), 0x3A00(e8), 0x3C00(f0), 0x3E00(f8), 512B for each beacon */
+	{BCN_OFFSET1,			0x6f77d0c8}, /* 0x3200(c8), 0x3400(d0), 0x1DC0(77), 0x1BC0(6f), 512B for each beacon */
+#elif defined(HW_BEACON_OFFSET) && (HW_BEACON_OFFSET == 0x100)
+	{BCN_OFFSET0,			0xece8e4e0}, /* 0x3800, 0x3A00, 0x3C00, 0x3E00, 512B for each beacon */
+	{BCN_OFFSET1,			0xfcf8f4f0}, /* 0x3800, 0x3A00, 0x3C00, 0x3E00, 512B for each beacon */
+#else
+    #error You must re-calculate new value for BCN_OFFSET0 & BCN_OFFSET1 in MACRegTable[]!!!
+#endif // HW_BEACON_OFFSET //
+
+	{LEGACY_BASIC_RATE,		0x0000013f}, //  Basic rate set bitmap
+	{HT_BASIC_RATE,		0x00008003}, // Basic HT rate set , 20M, MCS=3, MM. Format is the same as in TXWI.
+	{MAC_SYS_CTRL,		0x00}, // 0x1004, , default Disable RX
+	{RX_FILTR_CFG,		0x17f97}, //0x1400  , RX filter control,
+	{BKOFF_SLOT_CFG,	0x209}, // default set short slot time, CC_DELAY_TIME should be 2
+	{TX_SW_CFG0,		0x0}, 		// Gary,2008-05-21 for CWC test
+	{TX_SW_CFG1,		0x80606}, // Gary,2006-08-23
+	{TX_LINK_CFG,		0x1020},		// Gary,2006-08-23
+	{TX_TIMEOUT_CFG,	0x000a2090},	// CCK has some problem. So increase timieout value. 2006-10-09// MArvek RT , Modify for 2860E ,2007-08-01
+	{MAX_LEN_CFG,		MAX_AGGREGATION_SIZE | 0x00001000},	// 0x3018, MAX frame length. Max PSDU = 16kbytes.
+	{LED_CFG,		0x7f031e46}, // Gary, 2006-08-23
+	{PBF_MAX_PCNT,			0x1F3FBF9F}, 	//0x1F3f7f9f},		//Jan, 2006/04/20
+	{TX_RTY_CFG,			0x47d01f0f},	// Jan, 2006/11/16, Set TxWI->ACK =0 in Probe Rsp Modify for 2860E ,2007-08-03
+	{AUTO_RSP_CFG,			0x00000013},	// Initial Auto_Responder, because QA will turn off Auto-Responder
+	{CCK_PROT_CFG,			0x05740003 /*0x01740003*/},	// Initial Auto_Responder, because QA will turn off Auto-Responder. And RTS threshold is enabled.
+	{OFDM_PROT_CFG,			0x05740003 /*0x01740003*/},	// Initial Auto_Responder, because QA will turn off Auto-Responder. And RTS threshold is enabled.
+	{GF20_PROT_CFG,			0x01744004},    // set 19:18 --> Short NAV for MIMO PS
+	{GF40_PROT_CFG,			0x03F44084},
+	{MM20_PROT_CFG,			0x01744004},
+#ifdef RT2860
+	{MM40_PROT_CFG,			0x03F54084},
+#endif // RT2860 //
+	{TXOP_CTRL_CFG,			0x0000583f, /*0x0000243f*/ /*0x000024bf*/},	//Extension channel backoff.
+	{TX_RTS_CFG,			0x00092b20},
+	{EXP_ACK_TIME,			0x002400ca},	// default value
+	{TXOP_HLDR_ET, 			0x00000002},
+
+	/* Jerry comments 2008/01/16: we use SIFS = 10us in CCK defaultly, but it seems that 10us
+		is too small for INTEL 2200bg card, so in MBSS mode, the delta time between beacon0
+		and beacon1 is SIFS (10us), so if INTEL 2200bg card connects to BSS0, the ping
+		will always lost. So we change the SIFS of CCK from 10us to 16us. */
+	{XIFS_TIME_CFG,			0x33a41010},
+	{PWR_PIN_CFG,			0x00000003},	// patch for 2880-E
+};
+
+
+#ifdef CONFIG_STA_SUPPORT
+RTMP_REG_PAIR	STAMACRegTable[] =	{
+	{WMM_AIFSN_CFG,		0x00002273},
+	{WMM_CWMIN_CFG,	0x00002344},
+	{WMM_CWMAX_CFG,	0x000034aa},
+};
+#endif // CONFIG_STA_SUPPORT //
+
+#define	NUM_MAC_REG_PARMS		(sizeof(MACRegTable) / sizeof(RTMP_REG_PAIR))
+#ifdef CONFIG_STA_SUPPORT
+#define	NUM_STA_MAC_REG_PARMS	(sizeof(STAMACRegTable) / sizeof(RTMP_REG_PAIR))
+#endif // CONFIG_STA_SUPPORT //
+
+
+// New 8k byte firmware size for RT3071/RT3072
+#define FIRMWAREIMAGE_MAX_LENGTH	0x2000
+#define FIRMWAREIMAGE_LENGTH		(sizeof (FirmwareImage) / sizeof(UCHAR))
+#define FIRMWARE_MAJOR_VERSION	0
+
+#define FIRMWAREIMAGEV1_LENGTH	0x1000
+#define FIRMWAREIMAGEV2_LENGTH	0x1000
+
+#ifdef RT2860
+#define FIRMWARE_MINOR_VERSION	2
+#endif // RT2860 //
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Allocate RTMP_ADAPTER data block and do some initialization
+
+	Arguments:
+		Adapter		Pointer to our adapter
+
+	Return Value:
+		NDIS_STATUS_SUCCESS
+		NDIS_STATUS_FAILURE
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS	RTMPAllocAdapterBlock(
+	IN  PVOID	handle,
+	OUT	PRTMP_ADAPTER	*ppAdapter)
+{
+	PRTMP_ADAPTER	pAd;
+	NDIS_STATUS		Status;
+	INT 			index;
+	UCHAR			*pBeaconBuf = NULL;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> RTMPAllocAdapterBlock\n"));
+
+	*ppAdapter = NULL;
+
+	do
+	{
+		// Allocate RTMP_ADAPTER memory block
+		pBeaconBuf = kmalloc(MAX_BEACON_SIZE, MEM_ALLOC_FLAG);
+		if (pBeaconBuf == NULL)
+		{
+			Status = NDIS_STATUS_FAILURE;
+			DBGPRINT_ERR(("Failed to allocate memory - BeaconBuf!\n"));
+			break;
+		}
+
+		Status = AdapterBlockAllocateMemory(handle, (PVOID *)&pAd);
+		if (Status != NDIS_STATUS_SUCCESS)
+		{
+			DBGPRINT_ERR(("Failed to allocate memory - ADAPTER\n"));
+			break;
+		}
+		pAd->BeaconBuf = pBeaconBuf;
+		printk("\n\n=== pAd = %p, size = %d ===\n\n", pAd, (UINT32)sizeof(RTMP_ADAPTER));
+
+
+		// Init spin locks
+		NdisAllocateSpinLock(&pAd->MgmtRingLock);
+#ifdef RT2860
+		NdisAllocateSpinLock(&pAd->RxRingLock);
+#endif // RT2860 //
+
+		for (index =0 ; index < NUM_OF_TX_RING; index++)
+		{
+			NdisAllocateSpinLock(&pAd->TxSwQueueLock[index]);
+			NdisAllocateSpinLock(&pAd->DeQueueLock[index]);
+			pAd->DeQueueRunning[index] = FALSE;
+		}
+
+		NdisAllocateSpinLock(&pAd->irq_lock);
+
+	} while (FALSE);
+
+	if ((Status != NDIS_STATUS_SUCCESS) && (pBeaconBuf))
+		kfree(pBeaconBuf);
+
+	*ppAdapter = pAd;
+
+	DBGPRINT_S(Status, ("<-- RTMPAllocAdapterBlock, Status=%x\n", Status));
+	return Status;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Read initial Tx power per MCS and BW from EEPROM
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPReadTxPwrPerRate(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	ULONG		data, Adata, Gdata;
+	USHORT		i, value, value2;
+	INT			Apwrdelta, Gpwrdelta;
+	UCHAR		t1,t2,t3,t4;
+	BOOLEAN		bValid, bApwrdeltaMinus = TRUE, bGpwrdeltaMinus = TRUE;
+
+	//
+	// Get power delta for 20MHz and 40MHz.
+	//
+	DBGPRINT(RT_DEBUG_TRACE, ("Txpower per Rate\n"));
+	RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_DELTA, value2);
+	Apwrdelta = 0;
+	Gpwrdelta = 0;
+
+	if ((value2 & 0xff) != 0xff)
+	{
+		if ((value2 & 0x80))
+			Gpwrdelta = (value2&0xf);
+
+		if ((value2 & 0x40))
+			bGpwrdeltaMinus = FALSE;
+		else
+			bGpwrdeltaMinus = TRUE;
+	}
+	if ((value2 & 0xff00) != 0xff00)
+	{
+		if ((value2 & 0x8000))
+			Apwrdelta = ((value2&0xf00)>>8);
+
+		if ((value2 & 0x4000))
+			bApwrdeltaMinus = FALSE;
+		else
+			bApwrdeltaMinus = TRUE;
+	}
+	DBGPRINT(RT_DEBUG_TRACE, ("Gpwrdelta = %x, Apwrdelta = %x .\n", Gpwrdelta, Apwrdelta));
+
+	//
+	// Get Txpower per MCS for 20MHz in 2.4G.
+	//
+	for (i=0; i<5; i++)
+	{
+		RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_20MHZ_2_4G + i*4, value);
+		data = value;
+		if (bApwrdeltaMinus == FALSE)
+		{
+			t1 = (value&0xf)+(Apwrdelta);
+			if (t1 > 0xf)
+				t1 = 0xf;
+			t2 = ((value&0xf0)>>4)+(Apwrdelta);
+			if (t2 > 0xf)
+				t2 = 0xf;
+			t3 = ((value&0xf00)>>8)+(Apwrdelta);
+			if (t3 > 0xf)
+				t3 = 0xf;
+			t4 = ((value&0xf000)>>12)+(Apwrdelta);
+			if (t4 > 0xf)
+				t4 = 0xf;
+		}
+		else
+		{
+			if ((value&0xf) > Apwrdelta)
+				t1 = (value&0xf)-(Apwrdelta);
+			else
+				t1 = 0;
+			if (((value&0xf0)>>4) > Apwrdelta)
+				t2 = ((value&0xf0)>>4)-(Apwrdelta);
+			else
+				t2 = 0;
+			if (((value&0xf00)>>8) > Apwrdelta)
+				t3 = ((value&0xf00)>>8)-(Apwrdelta);
+			else
+				t3 = 0;
+			if (((value&0xf000)>>12) > Apwrdelta)
+				t4 = ((value&0xf000)>>12)-(Apwrdelta);
+			else
+				t4 = 0;
+		}
+		Adata = t1 + (t2<<4) + (t3<<8) + (t4<<12);
+		if (bGpwrdeltaMinus == FALSE)
+		{
+			t1 = (value&0xf)+(Gpwrdelta);
+			if (t1 > 0xf)
+				t1 = 0xf;
+			t2 = ((value&0xf0)>>4)+(Gpwrdelta);
+			if (t2 > 0xf)
+				t2 = 0xf;
+			t3 = ((value&0xf00)>>8)+(Gpwrdelta);
+			if (t3 > 0xf)
+				t3 = 0xf;
+			t4 = ((value&0xf000)>>12)+(Gpwrdelta);
+			if (t4 > 0xf)
+				t4 = 0xf;
+		}
+		else
+		{
+			if ((value&0xf) > Gpwrdelta)
+				t1 = (value&0xf)-(Gpwrdelta);
+			else
+				t1 = 0;
+			if (((value&0xf0)>>4) > Gpwrdelta)
+				t2 = ((value&0xf0)>>4)-(Gpwrdelta);
+			else
+				t2 = 0;
+			if (((value&0xf00)>>8) > Gpwrdelta)
+				t3 = ((value&0xf00)>>8)-(Gpwrdelta);
+			else
+				t3 = 0;
+			if (((value&0xf000)>>12) > Gpwrdelta)
+				t4 = ((value&0xf000)>>12)-(Gpwrdelta);
+			else
+				t4 = 0;
+		}
+		Gdata = t1 + (t2<<4) + (t3<<8) + (t4<<12);
+
+		RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_20MHZ_2_4G + i*4 + 2, value);
+		if (bApwrdeltaMinus == FALSE)
+		{
+			t1 = (value&0xf)+(Apwrdelta);
+			if (t1 > 0xf)
+				t1 = 0xf;
+			t2 = ((value&0xf0)>>4)+(Apwrdelta);
+			if (t2 > 0xf)
+				t2 = 0xf;
+			t3 = ((value&0xf00)>>8)+(Apwrdelta);
+			if (t3 > 0xf)
+				t3 = 0xf;
+			t4 = ((value&0xf000)>>12)+(Apwrdelta);
+			if (t4 > 0xf)
+				t4 = 0xf;
+		}
+		else
+		{
+			if ((value&0xf) > Apwrdelta)
+				t1 = (value&0xf)-(Apwrdelta);
+			else
+				t1 = 0;
+			if (((value&0xf0)>>4) > Apwrdelta)
+				t2 = ((value&0xf0)>>4)-(Apwrdelta);
+			else
+				t2 = 0;
+			if (((value&0xf00)>>8) > Apwrdelta)
+				t3 = ((value&0xf00)>>8)-(Apwrdelta);
+			else
+				t3 = 0;
+			if (((value&0xf000)>>12) > Apwrdelta)
+				t4 = ((value&0xf000)>>12)-(Apwrdelta);
+			else
+				t4 = 0;
+		}
+		Adata |= ((t1<<16) + (t2<<20) + (t3<<24) + (t4<<28));
+		if (bGpwrdeltaMinus == FALSE)
+		{
+			t1 = (value&0xf)+(Gpwrdelta);
+			if (t1 > 0xf)
+				t1 = 0xf;
+			t2 = ((value&0xf0)>>4)+(Gpwrdelta);
+			if (t2 > 0xf)
+				t2 = 0xf;
+			t3 = ((value&0xf00)>>8)+(Gpwrdelta);
+			if (t3 > 0xf)
+				t3 = 0xf;
+			t4 = ((value&0xf000)>>12)+(Gpwrdelta);
+			if (t4 > 0xf)
+				t4 = 0xf;
+		}
+		else
+		{
+			if ((value&0xf) > Gpwrdelta)
+				t1 = (value&0xf)-(Gpwrdelta);
+			else
+				t1 = 0;
+			if (((value&0xf0)>>4) > Gpwrdelta)
+				t2 = ((value&0xf0)>>4)-(Gpwrdelta);
+			else
+				t2 = 0;
+			if (((value&0xf00)>>8) > Gpwrdelta)
+				t3 = ((value&0xf00)>>8)-(Gpwrdelta);
+			else
+				t3 = 0;
+			if (((value&0xf000)>>12) > Gpwrdelta)
+				t4 = ((value&0xf000)>>12)-(Gpwrdelta);
+			else
+				t4 = 0;
+		}
+		Gdata |= ((t1<<16) + (t2<<20) + (t3<<24) + (t4<<28));
+		data |= (value<<16);
+
+		pAd->Tx20MPwrCfgABand[i] = pAd->Tx40MPwrCfgABand[i] = Adata;
+		pAd->Tx20MPwrCfgGBand[i] = pAd->Tx40MPwrCfgGBand[i] = Gdata;
+
+		if (data != 0xffffffff)
+			RTMP_IO_WRITE32(pAd, TX_PWR_CFG_0 + i*4, data);
+		DBGPRINT_RAW(RT_DEBUG_TRACE, ("20MHz BW, 2.4G band-%lx,  Adata = %lx,  Gdata = %lx \n", data, Adata, Gdata));
+	}
+
+	//
+	// Check this block is valid for 40MHz in 2.4G. If invalid, use parameter for 20MHz in 2.4G
+	//
+	bValid = TRUE;
+	for (i=0; i<6; i++)
+	{
+		RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_40MHZ_2_4G + 2 + i*2, value);
+		if (((value & 0x00FF) == 0x00FF) || ((value & 0xFF00) == 0xFF00))
+		{
+			bValid = FALSE;
+			break;
+		}
+	}
+
+	//
+	// Get Txpower per MCS for 40MHz in 2.4G.
+	//
+	if (bValid)
+	{
+		for (i=0; i<4; i++)
+		{
+			RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_40MHZ_2_4G + i*4, value);
+			if (bGpwrdeltaMinus == FALSE)
+			{
+				t1 = (value&0xf)+(Gpwrdelta);
+				if (t1 > 0xf)
+					t1 = 0xf;
+				t2 = ((value&0xf0)>>4)+(Gpwrdelta);
+				if (t2 > 0xf)
+					t2 = 0xf;
+				t3 = ((value&0xf00)>>8)+(Gpwrdelta);
+				if (t3 > 0xf)
+					t3 = 0xf;
+				t4 = ((value&0xf000)>>12)+(Gpwrdelta);
+				if (t4 > 0xf)
+					t4 = 0xf;
+			}
+			else
+			{
+				if ((value&0xf) > Gpwrdelta)
+					t1 = (value&0xf)-(Gpwrdelta);
+				else
+					t1 = 0;
+				if (((value&0xf0)>>4) > Gpwrdelta)
+					t2 = ((value&0xf0)>>4)-(Gpwrdelta);
+				else
+					t2 = 0;
+				if (((value&0xf00)>>8) > Gpwrdelta)
+					t3 = ((value&0xf00)>>8)-(Gpwrdelta);
+				else
+					t3 = 0;
+				if (((value&0xf000)>>12) > Gpwrdelta)
+					t4 = ((value&0xf000)>>12)-(Gpwrdelta);
+				else
+					t4 = 0;
+			}
+			Gdata = t1 + (t2<<4) + (t3<<8) + (t4<<12);
+
+			RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_40MHZ_2_4G + i*4 + 2, value);
+			if (bGpwrdeltaMinus == FALSE)
+			{
+				t1 = (value&0xf)+(Gpwrdelta);
+				if (t1 > 0xf)
+					t1 = 0xf;
+				t2 = ((value&0xf0)>>4)+(Gpwrdelta);
+				if (t2 > 0xf)
+					t2 = 0xf;
+				t3 = ((value&0xf00)>>8)+(Gpwrdelta);
+				if (t3 > 0xf)
+					t3 = 0xf;
+				t4 = ((value&0xf000)>>12)+(Gpwrdelta);
+				if (t4 > 0xf)
+					t4 = 0xf;
+			}
+			else
+			{
+				if ((value&0xf) > Gpwrdelta)
+					t1 = (value&0xf)-(Gpwrdelta);
+				else
+					t1 = 0;
+				if (((value&0xf0)>>4) > Gpwrdelta)
+					t2 = ((value&0xf0)>>4)-(Gpwrdelta);
+				else
+					t2 = 0;
+				if (((value&0xf00)>>8) > Gpwrdelta)
+					t3 = ((value&0xf00)>>8)-(Gpwrdelta);
+				else
+					t3 = 0;
+				if (((value&0xf000)>>12) > Gpwrdelta)
+					t4 = ((value&0xf000)>>12)-(Gpwrdelta);
+				else
+					t4 = 0;
+			}
+			Gdata |= ((t1<<16) + (t2<<20) + (t3<<24) + (t4<<28));
+
+			if (i == 0)
+				pAd->Tx40MPwrCfgGBand[i+1] = (pAd->Tx40MPwrCfgGBand[i+1] & 0x0000FFFF) | (Gdata & 0xFFFF0000);
+			else
+				pAd->Tx40MPwrCfgGBand[i+1] = Gdata;
+
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("40MHz BW, 2.4G band, Gdata = %lx \n", Gdata));
+		}
+	}
+
+	//
+	// Check this block is valid for 20MHz in 5G. If invalid, use parameter for 20MHz in 2.4G
+	//
+	bValid = TRUE;
+	for (i=0; i<8; i++)
+	{
+		RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_20MHZ_5G + 2 + i*2, value);
+		if (((value & 0x00FF) == 0x00FF) || ((value & 0xFF00) == 0xFF00))
+		{
+			bValid = FALSE;
+			break;
+		}
+	}
+
+	//
+	// Get Txpower per MCS for 20MHz in 5G.
+	//
+	if (bValid)
+	{
+		for (i=0; i<5; i++)
+		{
+			RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_20MHZ_5G + i*4, value);
+			if (bApwrdeltaMinus == FALSE)
+			{
+				t1 = (value&0xf)+(Apwrdelta);
+				if (t1 > 0xf)
+					t1 = 0xf;
+				t2 = ((value&0xf0)>>4)+(Apwrdelta);
+				if (t2 > 0xf)
+					t2 = 0xf;
+				t3 = ((value&0xf00)>>8)+(Apwrdelta);
+				if (t3 > 0xf)
+					t3 = 0xf;
+				t4 = ((value&0xf000)>>12)+(Apwrdelta);
+				if (t4 > 0xf)
+					t4 = 0xf;
+			}
+			else
+			{
+				if ((value&0xf) > Apwrdelta)
+					t1 = (value&0xf)-(Apwrdelta);
+				else
+					t1 = 0;
+				if (((value&0xf0)>>4) > Apwrdelta)
+					t2 = ((value&0xf0)>>4)-(Apwrdelta);
+				else
+					t2 = 0;
+				if (((value&0xf00)>>8) > Apwrdelta)
+					t3 = ((value&0xf00)>>8)-(Apwrdelta);
+				else
+					t3 = 0;
+				if (((value&0xf000)>>12) > Apwrdelta)
+					t4 = ((value&0xf000)>>12)-(Apwrdelta);
+				else
+					t4 = 0;
+			}
+			Adata = t1 + (t2<<4) + (t3<<8) + (t4<<12);
+
+			RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_20MHZ_5G + i*4 + 2, value);
+			if (bApwrdeltaMinus == FALSE)
+			{
+				t1 = (value&0xf)+(Apwrdelta);
+				if (t1 > 0xf)
+					t1 = 0xf;
+				t2 = ((value&0xf0)>>4)+(Apwrdelta);
+				if (t2 > 0xf)
+					t2 = 0xf;
+				t3 = ((value&0xf00)>>8)+(Apwrdelta);
+				if (t3 > 0xf)
+					t3 = 0xf;
+				t4 = ((value&0xf000)>>12)+(Apwrdelta);
+				if (t4 > 0xf)
+					t4 = 0xf;
+			}
+			else
+			{
+				if ((value&0xf) > Apwrdelta)
+					t1 = (value&0xf)-(Apwrdelta);
+				else
+					t1 = 0;
+				if (((value&0xf0)>>4) > Apwrdelta)
+					t2 = ((value&0xf0)>>4)-(Apwrdelta);
+				else
+					t2 = 0;
+				if (((value&0xf00)>>8) > Apwrdelta)
+					t3 = ((value&0xf00)>>8)-(Apwrdelta);
+				else
+					t3 = 0;
+				if (((value&0xf000)>>12) > Apwrdelta)
+					t4 = ((value&0xf000)>>12)-(Apwrdelta);
+				else
+					t4 = 0;
+			}
+			Adata |= ((t1<<16) + (t2<<20) + (t3<<24) + (t4<<28));
+
+			if (i == 0)
+				pAd->Tx20MPwrCfgABand[i] = (pAd->Tx20MPwrCfgABand[i] & 0x0000FFFF) | (Adata & 0xFFFF0000);
+			else
+				pAd->Tx20MPwrCfgABand[i] = Adata;
+
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("20MHz BW, 5GHz band, Adata = %lx \n", Adata));
+		}
+	}
+
+	//
+	// Check this block is valid for 40MHz in 5G. If invalid, use parameter for 20MHz in 2.4G
+	//
+	bValid = TRUE;
+	for (i=0; i<6; i++)
+	{
+		RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_40MHZ_5G + 2 + i*2, value);
+		if (((value & 0x00FF) == 0x00FF) || ((value & 0xFF00) == 0xFF00))
+		{
+			bValid = FALSE;
+			break;
+		}
+	}
+
+	//
+	// Get Txpower per MCS for 40MHz in 5G.
+	//
+	if (bValid)
+	{
+		for (i=0; i<4; i++)
+		{
+			RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_40MHZ_5G + i*4, value);
+			if (bApwrdeltaMinus == FALSE)
+			{
+				t1 = (value&0xf)+(Apwrdelta);
+				if (t1 > 0xf)
+					t1 = 0xf;
+				t2 = ((value&0xf0)>>4)+(Apwrdelta);
+				if (t2 > 0xf)
+					t2 = 0xf;
+				t3 = ((value&0xf00)>>8)+(Apwrdelta);
+				if (t3 > 0xf)
+					t3 = 0xf;
+				t4 = ((value&0xf000)>>12)+(Apwrdelta);
+				if (t4 > 0xf)
+					t4 = 0xf;
+			}
+			else
+			{
+				if ((value&0xf) > Apwrdelta)
+					t1 = (value&0xf)-(Apwrdelta);
+				else
+					t1 = 0;
+				if (((value&0xf0)>>4) > Apwrdelta)
+					t2 = ((value&0xf0)>>4)-(Apwrdelta);
+				else
+					t2 = 0;
+				if (((value&0xf00)>>8) > Apwrdelta)
+					t3 = ((value&0xf00)>>8)-(Apwrdelta);
+				else
+					t3 = 0;
+				if (((value&0xf000)>>12) > Apwrdelta)
+					t4 = ((value&0xf000)>>12)-(Apwrdelta);
+				else
+					t4 = 0;
+			}
+			Adata = t1 + (t2<<4) + (t3<<8) + (t4<<12);
+
+			RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_40MHZ_5G + i*4 + 2, value);
+			if (bApwrdeltaMinus == FALSE)
+			{
+				t1 = (value&0xf)+(Apwrdelta);
+				if (t1 > 0xf)
+					t1 = 0xf;
+				t2 = ((value&0xf0)>>4)+(Apwrdelta);
+				if (t2 > 0xf)
+					t2 = 0xf;
+				t3 = ((value&0xf00)>>8)+(Apwrdelta);
+				if (t3 > 0xf)
+					t3 = 0xf;
+				t4 = ((value&0xf000)>>12)+(Apwrdelta);
+				if (t4 > 0xf)
+					t4 = 0xf;
+			}
+			else
+			{
+				if ((value&0xf) > Apwrdelta)
+					t1 = (value&0xf)-(Apwrdelta);
+				else
+					t1 = 0;
+				if (((value&0xf0)>>4) > Apwrdelta)
+					t2 = ((value&0xf0)>>4)-(Apwrdelta);
+				else
+					t2 = 0;
+				if (((value&0xf00)>>8) > Apwrdelta)
+					t3 = ((value&0xf00)>>8)-(Apwrdelta);
+				else
+					t3 = 0;
+				if (((value&0xf000)>>12) > Apwrdelta)
+					t4 = ((value&0xf000)>>12)-(Apwrdelta);
+				else
+					t4 = 0;
+			}
+			Adata |= ((t1<<16) + (t2<<20) + (t3<<24) + (t4<<28));
+
+			if (i == 0)
+				pAd->Tx40MPwrCfgABand[i+1] = (pAd->Tx40MPwrCfgABand[i+1] & 0x0000FFFF) | (Adata & 0xFFFF0000);
+			else
+				pAd->Tx40MPwrCfgABand[i+1] = Adata;
+
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("40MHz BW, 5GHz band, Adata = %lx \n", Adata));
+		}
+	}
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Read initial channel power parameters from EEPROM
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPReadChannelPwr(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	UCHAR				i, choffset;
+	EEPROM_TX_PWR_STRUC	    Power;
+	EEPROM_TX_PWR_STRUC	    Power2;
+
+	// Read Tx power value for all channels
+	// Value from 1 - 0x7f. Default value is 24.
+	// Power value : 2.4G 0x00 (0) ~ 0x1F (31)
+	//             : 5.5G 0xF9 (-7) ~ 0x0F (15)
+
+	// 0. 11b/g, ch1 - ch 14
+	for (i = 0; i < 7; i++)
+	{
+		RT28xx_EEPROM_READ16(pAd, EEPROM_G_TX_PWR_OFFSET + i * 2, Power.word);
+		RT28xx_EEPROM_READ16(pAd, EEPROM_G_TX2_PWR_OFFSET + i * 2, Power2.word);
+		pAd->TxPower[i * 2].Channel = i * 2 + 1;
+		pAd->TxPower[i * 2 + 1].Channel = i * 2 + 2;
+
+		if ((Power.field.Byte0 > 31) || (Power.field.Byte0 < 0))
+			pAd->TxPower[i * 2].Power = DEFAULT_RF_TX_POWER;
+		else
+			pAd->TxPower[i * 2].Power = Power.field.Byte0;
+
+		if ((Power.field.Byte1 > 31) || (Power.field.Byte1 < 0))
+			pAd->TxPower[i * 2 + 1].Power = DEFAULT_RF_TX_POWER;
+		else
+			pAd->TxPower[i * 2 + 1].Power = Power.field.Byte1;
+
+		if ((Power2.field.Byte0 > 31) || (Power2.field.Byte0 < 0))
+			pAd->TxPower[i * 2].Power2 = DEFAULT_RF_TX_POWER;
+		else
+			pAd->TxPower[i * 2].Power2 = Power2.field.Byte0;
+
+		if ((Power2.field.Byte1 > 31) || (Power2.field.Byte1 < 0))
+			pAd->TxPower[i * 2 + 1].Power2 = DEFAULT_RF_TX_POWER;
+		else
+			pAd->TxPower[i * 2 + 1].Power2 = Power2.field.Byte1;
+	}
+
+	// 1. U-NII lower/middle band: 36, 38, 40; 44, 46, 48; 52, 54, 56; 60, 62, 64 (including central frequency in BW 40MHz)
+	// 1.1 Fill up channel
+	choffset = 14;
+	for (i = 0; i < 4; i++)
+	{
+		pAd->TxPower[3 * i + choffset + 0].Channel	= 36 + i * 8 + 0;
+		pAd->TxPower[3 * i + choffset + 0].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 0].Power2	= DEFAULT_RF_TX_POWER;
+
+		pAd->TxPower[3 * i + choffset + 1].Channel	= 36 + i * 8 + 2;
+		pAd->TxPower[3 * i + choffset + 1].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 1].Power2	= DEFAULT_RF_TX_POWER;
+
+		pAd->TxPower[3 * i + choffset + 2].Channel	= 36 + i * 8 + 4;
+		pAd->TxPower[3 * i + choffset + 2].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 2].Power2	= DEFAULT_RF_TX_POWER;
+	}
+
+	// 1.2 Fill up power
+	for (i = 0; i < 6; i++)
+	{
+		RT28xx_EEPROM_READ16(pAd, EEPROM_A_TX_PWR_OFFSET + i * 2, Power.word);
+		RT28xx_EEPROM_READ16(pAd, EEPROM_A_TX2_PWR_OFFSET + i * 2, Power2.word);
+
+		if ((Power.field.Byte0 < 16) && (Power.field.Byte0 >= -7))
+			pAd->TxPower[i * 2 + choffset + 0].Power = Power.field.Byte0;
+
+		if ((Power.field.Byte1 < 16) && (Power.field.Byte1 >= -7))
+			pAd->TxPower[i * 2 + choffset + 1].Power = Power.field.Byte1;
+
+		if ((Power2.field.Byte0 < 16) && (Power2.field.Byte0 >= -7))
+			pAd->TxPower[i * 2 + choffset + 0].Power2 = Power2.field.Byte0;
+
+		if ((Power2.field.Byte1 < 16) && (Power2.field.Byte1 >= -7))
+			pAd->TxPower[i * 2 + choffset + 1].Power2 = Power2.field.Byte1;
+	}
+
+	// 2. HipperLAN 2 100, 102 ,104; 108, 110, 112; 116, 118, 120; 124, 126, 128; 132, 134, 136; 140 (including central frequency in BW 40MHz)
+	// 2.1 Fill up channel
+	choffset = 14 + 12;
+	for (i = 0; i < 5; i++)
+	{
+		pAd->TxPower[3 * i + choffset + 0].Channel	= 100 + i * 8 + 0;
+		pAd->TxPower[3 * i + choffset + 0].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 0].Power2	= DEFAULT_RF_TX_POWER;
+
+		pAd->TxPower[3 * i + choffset + 1].Channel	= 100 + i * 8 + 2;
+		pAd->TxPower[3 * i + choffset + 1].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 1].Power2	= DEFAULT_RF_TX_POWER;
+
+		pAd->TxPower[3 * i + choffset + 2].Channel	= 100 + i * 8 + 4;
+		pAd->TxPower[3 * i + choffset + 2].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 2].Power2	= DEFAULT_RF_TX_POWER;
+	}
+	pAd->TxPower[3 * 5 + choffset + 0].Channel		= 140;
+	pAd->TxPower[3 * 5 + choffset + 0].Power		= DEFAULT_RF_TX_POWER;
+	pAd->TxPower[3 * 5 + choffset + 0].Power2		= DEFAULT_RF_TX_POWER;
+
+	// 2.2 Fill up power
+	for (i = 0; i < 8; i++)
+	{
+		RT28xx_EEPROM_READ16(pAd, EEPROM_A_TX_PWR_OFFSET + (choffset - 14) + i * 2, Power.word);
+		RT28xx_EEPROM_READ16(pAd, EEPROM_A_TX2_PWR_OFFSET + (choffset - 14) + i * 2, Power2.word);
+
+		if ((Power.field.Byte0 < 16) && (Power.field.Byte0 >= -7))
+			pAd->TxPower[i * 2 + choffset + 0].Power = Power.field.Byte0;
+
+		if ((Power.field.Byte1 < 16) && (Power.field.Byte1 >= -7))
+			pAd->TxPower[i * 2 + choffset + 1].Power = Power.field.Byte1;
+
+		if ((Power2.field.Byte0 < 16) && (Power2.field.Byte0 >= -7))
+			pAd->TxPower[i * 2 + choffset + 0].Power2 = Power2.field.Byte0;
+
+		if ((Power2.field.Byte1 < 16) && (Power2.field.Byte1 >= -7))
+			pAd->TxPower[i * 2 + choffset + 1].Power2 = Power2.field.Byte1;
+	}
+
+	// 3. U-NII upper band: 149, 151, 153; 157, 159, 161; 165 (including central frequency in BW 40MHz)
+	// 3.1 Fill up channel
+	choffset = 14 + 12 + 16;
+	for (i = 0; i < 2; i++)
+	{
+		pAd->TxPower[3 * i + choffset + 0].Channel	= 149 + i * 8 + 0;
+		pAd->TxPower[3 * i + choffset + 0].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 0].Power2	= DEFAULT_RF_TX_POWER;
+
+		pAd->TxPower[3 * i + choffset + 1].Channel	= 149 + i * 8 + 2;
+		pAd->TxPower[3 * i + choffset + 1].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 1].Power2	= DEFAULT_RF_TX_POWER;
+
+		pAd->TxPower[3 * i + choffset + 2].Channel	= 149 + i * 8 + 4;
+		pAd->TxPower[3 * i + choffset + 2].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 2].Power2	= DEFAULT_RF_TX_POWER;
+	}
+	pAd->TxPower[3 * 2 + choffset + 0].Channel		= 165;
+	pAd->TxPower[3 * 2 + choffset + 0].Power		= DEFAULT_RF_TX_POWER;
+	pAd->TxPower[3 * 2 + choffset + 0].Power2		= DEFAULT_RF_TX_POWER;
+
+	// 3.2 Fill up power
+	for (i = 0; i < 4; i++)
+	{
+		RT28xx_EEPROM_READ16(pAd, EEPROM_A_TX_PWR_OFFSET + (choffset - 14) + i * 2, Power.word);
+		RT28xx_EEPROM_READ16(pAd, EEPROM_A_TX2_PWR_OFFSET + (choffset - 14) + i * 2, Power2.word);
+
+		if ((Power.field.Byte0 < 16) && (Power.field.Byte0 >= -7))
+			pAd->TxPower[i * 2 + choffset + 0].Power = Power.field.Byte0;
+
+		if ((Power.field.Byte1 < 16) && (Power.field.Byte1 >= -7))
+			pAd->TxPower[i * 2 + choffset + 1].Power = Power.field.Byte1;
+
+		if ((Power2.field.Byte0 < 16) && (Power2.field.Byte0 >= -7))
+			pAd->TxPower[i * 2 + choffset + 0].Power2 = Power2.field.Byte0;
+
+		if ((Power2.field.Byte1 < 16) && (Power2.field.Byte1 >= -7))
+			pAd->TxPower[i * 2 + choffset + 1].Power2 = Power2.field.Byte1;
+	}
+
+	// 4. Print and Debug
+	choffset = 14 + 12 + 16 + 7;
+
+
+#if 0
+	// Init the 802.11j channel number for TX channel power
+	// 0. 20MHz
+	for (i = 0; i < 3; i++)
+	{
+		pAd->TxPower11J[i].Channel = 8 + i * 4;
+		pAd->TxPower11J[i].BW = BW_20;
+	}
+
+	for (i = 0; i < 4; i++)
+	{
+		pAd->TxPower11J[i + 3].Channel = 34 + i * 4;
+		pAd->TxPower11J[i + 3].BW = BW_20;
+	}
+
+	for (i = 0; i < 4; i++)
+	{
+		pAd->TxPower11J[i + 7].Channel = 184 + i * 4;
+		pAd->TxPower11J[i + 7].BW = BW_20;
+	}
+
+	// 0. 10MHz
+	for (i = 0; i < 2; i++)
+	{
+		pAd->TxPower11J[i + 11].Channel = 7 + i;
+		pAd->TxPower11J[i + 11].BW = BW_10;
+	}
+	pAd->TxPower11J[13].Channel = 11;
+	pAd->TxPower11J[13].BW = BW_10;
+
+	for (i = 0; i < 3; i++)
+	{
+		pAd->TxPower11J[i + 14].Channel = 183 + i;
+		pAd->TxPower11J[i + 14].BW= BW_10;
+	}
+
+	for (i = 0; i < 3; i++)
+	{
+		pAd->TxPower11J[i + 17].Channel = 187 + i;
+		pAd->TxPower11J[i + 17].BW = BW_10;
+	}
+	for (i = 0; i < 10; i++)
+	{
+		Power.word = RTMP_EEPROM_READ16(pAd, EEPROM_Japan_TX_PWR_OFFSET + i * 2);
+		Power2.word = RTMP_EEPROM_READ16(pAd, EEPROM_Japan_TX2_PWR_OFFSET + i * 2);
+
+		if ((Power.field.Byte0 < 36) && (Power.field.Byte0 > -6))
+			pAd->TxPower11J[i * 2].Power = Power.field.Byte0;
+
+		if ((Power.field.Byte1 < 36) && (Power.field.Byte1 > -6))
+			pAd->TxPower11J[i * 2 + 1].Power = Power.field.Byte1;
+
+		if ((Power2.field.Byte0 < 36) && (Power2.field.Byte0 > -6))
+			pAd->TxPower11J[i * 2].Power2 = Power2.field.Byte0;
+
+		if ((Power2.field.Byte1 < 36) && (Power2.field.Byte1 > -6))
+			pAd->TxPower11J[i * 2 + 1].Power2 = Power2.field.Byte1;
+	}
+#endif
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Read the following from the registry
+		1. All the parameters
+		2. NetworkAddres
+
+	Arguments:
+		Adapter						Pointer to our adapter
+		WrapperConfigurationContext	For use by NdisOpenConfiguration
+
+	Return Value:
+		NDIS_STATUS_SUCCESS
+		NDIS_STATUS_FAILURE
+		NDIS_STATUS_RESOURCES
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS	NICReadRegParameters(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	NDIS_HANDLE			WrapperConfigurationContext
+	)
+{
+	NDIS_STATUS						Status = NDIS_STATUS_SUCCESS;
+	DBGPRINT_S(Status, ("<-- NICReadRegParameters, Status=%x\n", Status));
+	return Status;
+}
+
+
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Read initial parameters from EEPROM
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	NICReadEEPROMParameters(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			mac_addr)
+{
+	UINT32			data = 0;
+	USHORT			i, value, value2;
+	UCHAR			TmpPhy;
+	EEPROM_TX_PWR_STRUC	    Power;
+	EEPROM_VERSION_STRUC    Version;
+	EEPROM_ANTENNA_STRUC	Antenna;
+	EEPROM_NIC_CONFIG2_STRUC    NicConfig2;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> NICReadEEPROMParameters\n"));
+
+	// Init EEPROM Address Number, before access EEPROM; if 93c46, EEPROMAddressNum=6, else if 93c66, EEPROMAddressNum=8
+	RTMP_IO_READ32(pAd, E2PROM_CSR, &data);
+	DBGPRINT(RT_DEBUG_TRACE, ("--> E2PROM_CSR = 0x%x\n", data));
+
+	if((data & 0x30) == 0)
+		pAd->EEPROMAddressNum = 6;		// 93C46
+	else if((data & 0x30) == 0x10)
+		pAd->EEPROMAddressNum = 8;     // 93C66
+	else
+		pAd->EEPROMAddressNum = 8;     // 93C86
+	DBGPRINT(RT_DEBUG_TRACE, ("--> EEPROMAddressNum = %d\n", pAd->EEPROMAddressNum ));
+
+	// RT2860 MAC no longer auto load MAC address from E2PROM. Driver has to intialize
+	// MAC address registers according to E2PROM setting
+	if (mac_addr == NULL ||
+		strlen(mac_addr) != 17 ||
+		mac_addr[2] != ':'  || mac_addr[5] != ':'  || mac_addr[8] != ':' ||
+		mac_addr[11] != ':' || mac_addr[14] != ':')
+	{
+		USHORT  Addr01,Addr23,Addr45 ;
+
+		RT28xx_EEPROM_READ16(pAd, 0x04, Addr01);
+		RT28xx_EEPROM_READ16(pAd, 0x06, Addr23);
+		RT28xx_EEPROM_READ16(pAd, 0x08, Addr45);
+
+		pAd->PermanentAddress[0] = (UCHAR)(Addr01 & 0xff);
+		pAd->PermanentAddress[1] = (UCHAR)(Addr01 >> 8);
+		pAd->PermanentAddress[2] = (UCHAR)(Addr23 & 0xff);
+		pAd->PermanentAddress[3] = (UCHAR)(Addr23 >> 8);
+		pAd->PermanentAddress[4] = (UCHAR)(Addr45 & 0xff);
+		pAd->PermanentAddress[5] = (UCHAR)(Addr45 >> 8);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("Initialize MAC Address from E2PROM \n"));
+	}
+	else
+	{
+		INT		j;
+		PUCHAR	macptr;
+
+		macptr = mac_addr;
+
+		for (j=0; j<MAC_ADDR_LEN; j++)
+		{
+			AtoH(macptr, &pAd->PermanentAddress[j], 1);
+			macptr=macptr+3;
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("Initialize MAC Address from module parameter \n"));
+	}
+
+
+	{
+		//more conveninet to test mbssid, so ap's bssid &0xf1
+		if (pAd->PermanentAddress[0] == 0xff)
+			pAd->PermanentAddress[0] = RandomByte(pAd)&0xf8;
+
+		//if (pAd->PermanentAddress[5] == 0xff)
+		//	pAd->PermanentAddress[5] = RandomByte(pAd)&0xf8;
+
+		DBGPRINT_RAW(RT_DEBUG_TRACE,("E2PROM MAC: =%02x:%02x:%02x:%02x:%02x:%02x\n",
+			pAd->PermanentAddress[0], pAd->PermanentAddress[1],
+			pAd->PermanentAddress[2], pAd->PermanentAddress[3],
+			pAd->PermanentAddress[4], pAd->PermanentAddress[5]));
+		if (pAd->bLocalAdminMAC == FALSE)
+		{
+			MAC_DW0_STRUC csr2;
+			MAC_DW1_STRUC csr3;
+			COPY_MAC_ADDR(pAd->CurrentAddress, pAd->PermanentAddress);
+			csr2.field.Byte0 = pAd->CurrentAddress[0];
+			csr2.field.Byte1 = pAd->CurrentAddress[1];
+			csr2.field.Byte2 = pAd->CurrentAddress[2];
+			csr2.field.Byte3 = pAd->CurrentAddress[3];
+			RTMP_IO_WRITE32(pAd, MAC_ADDR_DW0, csr2.word);
+			csr3.word = 0;
+			csr3.field.Byte4 = pAd->CurrentAddress[4];
+			csr3.field.Byte5 = pAd->CurrentAddress[5];
+			csr3.field.U2MeMask = 0xff;
+			RTMP_IO_WRITE32(pAd, MAC_ADDR_DW1, csr3.word);
+			DBGPRINT_RAW(RT_DEBUG_TRACE,("E2PROM MAC: =%02x:%02x:%02x:%02x:%02x:%02x\n",
+				pAd->PermanentAddress[0], pAd->PermanentAddress[1],
+				pAd->PermanentAddress[2], pAd->PermanentAddress[3],
+				pAd->PermanentAddress[4], pAd->PermanentAddress[5]));
+		}
+	}
+
+	// if not return early. cause fail at emulation.
+	// Init the channel number for TX channel power
+	RTMPReadChannelPwr(pAd);
+
+	// if E2PROM version mismatch with driver's expectation, then skip
+	// all subsequent E2RPOM retieval and set a system error bit to notify GUI
+	RT28xx_EEPROM_READ16(pAd, EEPROM_VERSION_OFFSET, Version.word);
+	pAd->EepromVersion = Version.field.Version + Version.field.FaeReleaseNumber * 256;
+	DBGPRINT(RT_DEBUG_TRACE, ("E2PROM: Version = %d, FAE release #%d\n", Version.field.Version, Version.field.FaeReleaseNumber));
+
+	if (Version.field.Version > VALID_EEPROM_VERSION)
+	{
+		DBGPRINT_ERR(("E2PROM: WRONG VERSION 0x%x, should be %d\n",Version.field.Version, VALID_EEPROM_VERSION));
+		/*pAd->SystemErrorBitmap |= 0x00000001;
+
+		// hard-code default value when no proper E2PROM installed
+		pAd->bAutoTxAgcA = FALSE;
+		pAd->bAutoTxAgcG = FALSE;
+
+		// Default the channel power
+		for (i = 0; i < MAX_NUM_OF_CHANNELS; i++)
+			pAd->TxPower[i].Power = DEFAULT_RF_TX_POWER;
+
+		// Default the channel power
+		for (i = 0; i < MAX_NUM_OF_11JCHANNELS; i++)
+			pAd->TxPower11J[i].Power = DEFAULT_RF_TX_POWER;
+
+		for(i = 0; i < NUM_EEPROM_BBP_PARMS; i++)
+			pAd->EEPROMDefaultValue[i] = 0xffff;
+		return;  */
+	}
+
+	// Read BBP default value from EEPROM and store to array(EEPROMDefaultValue) in pAd
+	RT28xx_EEPROM_READ16(pAd, EEPROM_NIC1_OFFSET, value);
+	pAd->EEPROMDefaultValue[0] = value;
+
+	RT28xx_EEPROM_READ16(pAd, EEPROM_NIC2_OFFSET, value);
+	pAd->EEPROMDefaultValue[1] = value;
+
+	RT28xx_EEPROM_READ16(pAd, 0x38, value);	// Country Region
+	pAd->EEPROMDefaultValue[2] = value;
+
+	for(i = 0; i < 8; i++)
+	{
+		RT28xx_EEPROM_READ16(pAd, EEPROM_BBP_BASE_OFFSET + i*2, value);
+		pAd->EEPROMDefaultValue[i+3] = value;
+	}
+
+	// We have to parse NIC configuration 0 at here.
+	// If TSSI did not have preloaded value, it should reset the TxAutoAgc to false
+	// Therefore, we have to read TxAutoAgc control beforehand.
+	// Read Tx AGC control bit
+	Antenna.word = pAd->EEPROMDefaultValue[0];
+	if (Antenna.word == 0xFFFF)
+	{
+		Antenna.word = 0;
+		Antenna.field.RfIcType = RFIC_2820;
+		Antenna.field.TxPath = 1;
+		Antenna.field.RxPath = 2;
+		DBGPRINT(RT_DEBUG_WARN, ("E2PROM error, hard code as 0x%04x\n", Antenna.word));
+	}
+
+	// Choose the desired Tx&Rx stream.
+	if ((pAd->CommonCfg.TxStream == 0) || (pAd->CommonCfg.TxStream > Antenna.field.TxPath))
+		pAd->CommonCfg.TxStream = Antenna.field.TxPath;
+
+	if ((pAd->CommonCfg.RxStream == 0) || (pAd->CommonCfg.RxStream > Antenna.field.RxPath))
+	{
+		pAd->CommonCfg.RxStream = Antenna.field.RxPath;
+
+		if ((pAd->MACVersion < RALINK_2883_VERSION) &&
+			(pAd->CommonCfg.RxStream > 2))
+		{
+			// only 2 Rx streams for RT2860 series
+			pAd->CommonCfg.RxStream = 2;
+		}
+	}
+
+	// 3*3
+	// read value from EEPROM and set them to CSR174 ~ 177 in chain0 ~ chain2
+	// yet implement
+	for(i=0; i<3; i++)
+	{
+	}
+
+	NicConfig2.word = pAd->EEPROMDefaultValue[1];
+
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		NicConfig2.word = 0;
+		if ((NicConfig2.word & 0x00ff) == 0xff)
+		{
+			NicConfig2.word &= 0xff00;
+		}
+
+		if ((NicConfig2.word >> 8) == 0xff)
+		{
+			NicConfig2.word &= 0x00ff;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	if (NicConfig2.field.DynamicTxAgcControl == 1)
+		pAd->bAutoTxAgcA = pAd->bAutoTxAgcG = TRUE;
+	else
+		pAd->bAutoTxAgcA = pAd->bAutoTxAgcG = FALSE;
+
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("NICReadEEPROMParameters: RxPath = %d, TxPath = %d\n", Antenna.field.RxPath, Antenna.field.TxPath));
+
+	// Save the antenna for future use
+	pAd->Antenna.word = Antenna.word;
+
+	//
+	// Reset PhyMode if we don't support 802.11a
+	// Only RFIC_2850 & RFIC_2750 support 802.11a
+	//
+	if ((Antenna.field.RfIcType != RFIC_2850) && (Antenna.field.RfIcType != RFIC_2750))
+	{
+		if ((pAd->CommonCfg.PhyMode == PHY_11ABG_MIXED) ||
+			(pAd->CommonCfg.PhyMode == PHY_11A))
+			pAd->CommonCfg.PhyMode = PHY_11BG_MIXED;
+#ifdef DOT11_N_SUPPORT
+		else if ((pAd->CommonCfg.PhyMode == PHY_11ABGN_MIXED)	||
+				 (pAd->CommonCfg.PhyMode == PHY_11AN_MIXED) 	||
+				 (pAd->CommonCfg.PhyMode == PHY_11AGN_MIXED) 	||
+				 (pAd->CommonCfg.PhyMode == PHY_11N_5G))
+			pAd->CommonCfg.PhyMode = PHY_11BGN_MIXED;
+#endif // DOT11_N_SUPPORT //
+	}
+
+	// Read TSSI reference and TSSI boundary for temperature compensation. This is ugly
+	// 0. 11b/g
+	{
+		/* these are tempature reference value (0x00 ~ 0xFE)
+		   ex: 0x00 0x15 0x25 0x45 0x88 0xA0 0xB5 0xD0 0xF0
+		   TssiPlusBoundaryG [4] [3] [2] [1] [0] (smaller) +
+		   TssiMinusBoundaryG[0] [1] [2] [3] [4] (larger) */
+		RT28xx_EEPROM_READ16(pAd, 0x6E, Power.word);
+		pAd->TssiMinusBoundaryG[4] = Power.field.Byte0;
+		pAd->TssiMinusBoundaryG[3] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0x70, Power.word);
+		pAd->TssiMinusBoundaryG[2] = Power.field.Byte0;
+		pAd->TssiMinusBoundaryG[1] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0x72, Power.word);
+		pAd->TssiRefG   = Power.field.Byte0; /* reference value [0] */
+		pAd->TssiPlusBoundaryG[1] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0x74, Power.word);
+		pAd->TssiPlusBoundaryG[2] = Power.field.Byte0;
+		pAd->TssiPlusBoundaryG[3] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0x76, Power.word);
+		pAd->TssiPlusBoundaryG[4] = Power.field.Byte0;
+		pAd->TxAgcStepG = Power.field.Byte1;
+		pAd->TxAgcCompensateG = 0;
+		pAd->TssiMinusBoundaryG[0] = pAd->TssiRefG;
+		pAd->TssiPlusBoundaryG[0]  = pAd->TssiRefG;
+
+		// Disable TxAgc if the based value is not right
+		if (pAd->TssiRefG == 0xff)
+			pAd->bAutoTxAgcG = FALSE;
+
+		DBGPRINT(RT_DEBUG_TRACE,("E2PROM: G Tssi[-4 .. +4] = %d %d %d %d - %d -%d %d %d %d, step=%d, tuning=%d\n",
+			pAd->TssiMinusBoundaryG[4], pAd->TssiMinusBoundaryG[3], pAd->TssiMinusBoundaryG[2], pAd->TssiMinusBoundaryG[1],
+			pAd->TssiRefG,
+			pAd->TssiPlusBoundaryG[1], pAd->TssiPlusBoundaryG[2], pAd->TssiPlusBoundaryG[3], pAd->TssiPlusBoundaryG[4],
+			pAd->TxAgcStepG, pAd->bAutoTxAgcG));
+	}
+	// 1. 11a
+	{
+		RT28xx_EEPROM_READ16(pAd, 0xD4, Power.word);
+		pAd->TssiMinusBoundaryA[4] = Power.field.Byte0;
+		pAd->TssiMinusBoundaryA[3] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0xD6, Power.word);
+		pAd->TssiMinusBoundaryA[2] = Power.field.Byte0;
+		pAd->TssiMinusBoundaryA[1] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0xD8, Power.word);
+		pAd->TssiRefA   = Power.field.Byte0;
+		pAd->TssiPlusBoundaryA[1] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0xDA, Power.word);
+		pAd->TssiPlusBoundaryA[2] = Power.field.Byte0;
+		pAd->TssiPlusBoundaryA[3] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0xDC, Power.word);
+		pAd->TssiPlusBoundaryA[4] = Power.field.Byte0;
+		pAd->TxAgcStepA = Power.field.Byte1;
+		pAd->TxAgcCompensateA = 0;
+		pAd->TssiMinusBoundaryA[0] = pAd->TssiRefA;
+		pAd->TssiPlusBoundaryA[0]  = pAd->TssiRefA;
+
+		// Disable TxAgc if the based value is not right
+		if (pAd->TssiRefA == 0xff)
+			pAd->bAutoTxAgcA = FALSE;
+
+		DBGPRINT(RT_DEBUG_TRACE,("E2PROM: A Tssi[-4 .. +4] = %d %d %d %d - %d -%d %d %d %d, step=%d, tuning=%d\n",
+			pAd->TssiMinusBoundaryA[4], pAd->TssiMinusBoundaryA[3], pAd->TssiMinusBoundaryA[2], pAd->TssiMinusBoundaryA[1],
+			pAd->TssiRefA,
+			pAd->TssiPlusBoundaryA[1], pAd->TssiPlusBoundaryA[2], pAd->TssiPlusBoundaryA[3], pAd->TssiPlusBoundaryA[4],
+			pAd->TxAgcStepA, pAd->bAutoTxAgcA));
+	}
+	pAd->BbpRssiToDbmDelta = 0x0;
+
+	// Read frequency offset setting for RF
+	RT28xx_EEPROM_READ16(pAd, EEPROM_FREQ_OFFSET, value);
+	if ((value & 0x00FF) != 0x00FF)
+		pAd->RfFreqOffset = (ULONG) (value & 0x00FF);
+	else
+		pAd->RfFreqOffset = 0;
+	DBGPRINT(RT_DEBUG_TRACE, ("E2PROM: RF FreqOffset=0x%lx \n", pAd->RfFreqOffset));
+
+	//CountryRegion byte offset (38h)
+	value = pAd->EEPROMDefaultValue[2] >> 8;		// 2.4G band
+	value2 = pAd->EEPROMDefaultValue[2] & 0x00FF;	// 5G band
+
+	if ((value <= REGION_MAXIMUM_BG_BAND) && (value2 <= REGION_MAXIMUM_A_BAND))
+	{
+		pAd->CommonCfg.CountryRegion = ((UCHAR) value) | 0x80;
+		pAd->CommonCfg.CountryRegionForABand = ((UCHAR) value2) | 0x80;
+		TmpPhy = pAd->CommonCfg.PhyMode;
+		pAd->CommonCfg.PhyMode = 0xff;
+		RTMPSetPhyMode(pAd, TmpPhy);
+#ifdef DOT11_N_SUPPORT
+		SetCommonHT(pAd);
+#endif // DOT11_N_SUPPORT //
+	}
+
+	//
+	// Get RSSI Offset on EEPROM 0x9Ah & 0x9Ch.
+	// The valid value are (-10 ~ 10)
+	//
+	RT28xx_EEPROM_READ16(pAd, EEPROM_RSSI_BG_OFFSET, value);
+	pAd->BGRssiOffset0 = value & 0x00ff;
+	pAd->BGRssiOffset1 = (value >> 8);
+	RT28xx_EEPROM_READ16(pAd, EEPROM_RSSI_BG_OFFSET+2, value);
+	pAd->BGRssiOffset2 = value & 0x00ff;
+	pAd->ALNAGain1 = (value >> 8);
+	RT28xx_EEPROM_READ16(pAd, EEPROM_LNA_OFFSET, value);
+	pAd->BLNAGain = value & 0x00ff;
+	pAd->ALNAGain0 = (value >> 8);
+
+	// Validate 11b/g RSSI_0 offset.
+	if ((pAd->BGRssiOffset0 < -10) || (pAd->BGRssiOffset0 > 10))
+		pAd->BGRssiOffset0 = 0;
+
+	// Validate 11b/g RSSI_1 offset.
+	if ((pAd->BGRssiOffset1 < -10) || (pAd->BGRssiOffset1 > 10))
+		pAd->BGRssiOffset1 = 0;
+
+	// Validate 11b/g RSSI_2 offset.
+	if ((pAd->BGRssiOffset2 < -10) || (pAd->BGRssiOffset2 > 10))
+		pAd->BGRssiOffset2 = 0;
+
+	RT28xx_EEPROM_READ16(pAd, EEPROM_RSSI_A_OFFSET, value);
+	pAd->ARssiOffset0 = value & 0x00ff;
+	pAd->ARssiOffset1 = (value >> 8);
+	RT28xx_EEPROM_READ16(pAd, (EEPROM_RSSI_A_OFFSET+2), value);
+	pAd->ARssiOffset2 = value & 0x00ff;
+	pAd->ALNAGain2 = (value >> 8);
+
+	if (((UCHAR)pAd->ALNAGain1 == 0xFF) || (pAd->ALNAGain1 == 0x00))
+		pAd->ALNAGain1 = pAd->ALNAGain0;
+	if (((UCHAR)pAd->ALNAGain2 == 0xFF) || (pAd->ALNAGain2 == 0x00))
+		pAd->ALNAGain2 = pAd->ALNAGain0;
+
+	// Validate 11a RSSI_0 offset.
+	if ((pAd->ARssiOffset0 < -10) || (pAd->ARssiOffset0 > 10))
+		pAd->ARssiOffset0 = 0;
+
+	// Validate 11a RSSI_1 offset.
+	if ((pAd->ARssiOffset1 < -10) || (pAd->ARssiOffset1 > 10))
+		pAd->ARssiOffset1 = 0;
+
+	//Validate 11a RSSI_2 offset.
+	if ((pAd->ARssiOffset2 < -10) || (pAd->ARssiOffset2 > 10))
+		pAd->ARssiOffset2 = 0;
+
+	//
+	// Get LED Setting.
+	//
+	RT28xx_EEPROM_READ16(pAd, 0x3a, value);
+	pAd->LedCntl.word = (value&0xff00) >> 8;
+	RT28xx_EEPROM_READ16(pAd, EEPROM_LED1_OFFSET, value);
+	pAd->Led1 = value;
+	RT28xx_EEPROM_READ16(pAd, EEPROM_LED2_OFFSET, value);
+	pAd->Led2 = value;
+	RT28xx_EEPROM_READ16(pAd, EEPROM_LED3_OFFSET, value);
+	pAd->Led3 = value;
+
+	RTMPReadTxPwrPerRate(pAd);
+
+#ifdef SINGLE_SKU
+	//pAd->CommonCfg.DefineMaxTxPwr = RTMP_EEPROM_READ16(pAd, EEPROM_DEFINE_MAX_TXPWR);
+	RT28xx_EEPROM_READ16(pAd, EEPROM_DEFINE_MAX_TXPWR, pAd->CommonCfg.DefineMaxTxPwr);
+#endif // SINGLE_SKU //
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- NICReadEEPROMParameters\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Set default value from EEPROM
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	NICInitAsicFromEEPROM(
+	IN	PRTMP_ADAPTER	pAd)
+{
+#ifdef CONFIG_STA_SUPPORT
+	UINT32					data = 0;
+	UCHAR	BBPR1 = 0;
+#endif // CONFIG_STA_SUPPORT //
+	USHORT					i;
+	EEPROM_ANTENNA_STRUC	Antenna;
+	EEPROM_NIC_CONFIG2_STRUC    NicConfig2;
+	UCHAR	BBPR3 = 0;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> NICInitAsicFromEEPROM\n"));
+	for(i = 3; i < NUM_EEPROM_BBP_PARMS; i++)
+	{
+		UCHAR BbpRegIdx, BbpValue;
+
+		if ((pAd->EEPROMDefaultValue[i] != 0xFFFF) && (pAd->EEPROMDefaultValue[i] != 0))
+		{
+			BbpRegIdx = (UCHAR)(pAd->EEPROMDefaultValue[i] >> 8);
+			BbpValue  = (UCHAR)(pAd->EEPROMDefaultValue[i] & 0xff);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BbpRegIdx, BbpValue);
+		}
+	}
+
+	Antenna.word = pAd->Antenna.word;
+	pAd->Mlme.RealRxPath = (UCHAR) Antenna.field.RxPath;
+	pAd->RfIcType = (UCHAR) Antenna.field.RfIcType;
+
+	NicConfig2.word = pAd->EEPROMDefaultValue[1];
+
+
+	// Save the antenna for future use
+	pAd->NicConfig2.word = NicConfig2.word;
+
+	//
+	// Send LED Setting to MCU.
+	//
+	if (pAd->LedCntl.word == 0xFF)
+	{
+		pAd->LedCntl.word = 0x01;
+		pAd->Led1 = 0x5555;
+		pAd->Led2 = 0x2221;
+
+#ifdef RT2860
+		pAd->Led3 = 0xA9F8;
+#endif // RT2860 //
+	}
+
+	AsicSendCommandToMcu(pAd, 0x52, 0xff, (UCHAR)pAd->Led1, (UCHAR)(pAd->Led1 >> 8));
+	AsicSendCommandToMcu(pAd, 0x53, 0xff, (UCHAR)pAd->Led2, (UCHAR)(pAd->Led2 >> 8));
+	AsicSendCommandToMcu(pAd, 0x54, 0xff, (UCHAR)pAd->Led3, (UCHAR)(pAd->Led3 >> 8));
+    pAd->LedIndicatorStregth = 0xFF;
+    RTMPSetSignalLED(pAd, -100);	// Force signal strength Led to be turned off, before link up
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// Read Hardware controlled Radio state enable bit
+		if (NicConfig2.field.HardwareRadioControl == 1)
+		{
+			pAd->StaCfg.bHardwareRadio = TRUE;
+
+			// Read GPIO pin2 as Hardware controlled radio state
+			RTMP_IO_READ32(pAd, GPIO_CTRL_CFG, &data);
+			if ((data & 0x04) == 0)
+			{
+				pAd->StaCfg.bHwRadio = FALSE;
+				pAd->StaCfg.bRadio = FALSE;
+				RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF);
+			}
+		}
+		else
+			pAd->StaCfg.bHardwareRadio = FALSE;
+
+		if (pAd->StaCfg.bRadio == FALSE)
+		{
+			RTMPSetLED(pAd, LED_RADIO_OFF);
+		}
+		else
+		{
+			RTMPSetLED(pAd, LED_RADIO_ON);
+#ifdef RT2860
+			AsicSendCommandToMcu(pAd, 0x30, 0xff, 0xff, 0x02);
+			AsicSendCommandToMcu(pAd, 0x31, PowerWakeCID, 0x00, 0x00);
+			// 2-1. wait command ok.
+			AsicCheckCommanOk(pAd, PowerWakeCID);
+#endif // RT2860 //
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	// Turn off patching for cardbus controller
+	if (NicConfig2.field.CardbusAcceleration == 1)
+	{
+	}
+
+	if (NicConfig2.field.DynamicTxAgcControl == 1)
+		pAd->bAutoTxAgcA = pAd->bAutoTxAgcG = TRUE;
+	else
+		pAd->bAutoTxAgcA = pAd->bAutoTxAgcG = FALSE;
+	//
+	// Since BBP has been progamed, to make sure BBP setting will be
+	// upate inside of AsicAntennaSelect, so reset to UNKNOWN_BAND!!
+	//
+	pAd->CommonCfg.BandState = UNKNOWN_BAND;
+
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BBPR3);
+	BBPR3 &= (~0x18);
+	if(pAd->Antenna.field.RxPath == 3)
+	{
+		BBPR3 |= (0x10);
+	}
+	else if(pAd->Antenna.field.RxPath == 2)
+	{
+		BBPR3 |= (0x8);
+	}
+	else if(pAd->Antenna.field.RxPath == 1)
+	{
+		BBPR3 |= (0x0);
+	}
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BBPR3);
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// Handle the difference when 1T
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R1, &BBPR1);
+		if(pAd->Antenna.field.TxPath == 1)
+		{
+		BBPR1 &= (~0x18);
+		}
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R1, BBPR1);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("Use Hw Radio Control Pin=%d; if used Pin=%d;\n", pAd->CommonCfg.bHardwareRadio, pAd->CommonCfg.bHardwareRadio));
+	}
+#endif // CONFIG_STA_SUPPORT //
+	DBGPRINT(RT_DEBUG_TRACE, ("TxPath = %d, RxPath = %d, RFIC=%d, Polar+LED mode=%x\n", pAd->Antenna.field.TxPath, pAd->Antenna.field.RxPath, pAd->RfIcType, pAd->LedCntl.word));
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- NICInitAsicFromEEPROM\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Initialize NIC hardware
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS	NICInitializeAdapter(
+	IN	PRTMP_ADAPTER	pAd,
+	IN   BOOLEAN    bHardReset)
+{
+	NDIS_STATUS     Status = NDIS_STATUS_SUCCESS;
+	WPDMA_GLO_CFG_STRUC	GloCfg;
+#ifdef RT2860
+	UINT32			Value;
+	DELAY_INT_CFG_STRUC	IntCfg;
+#endif // RT2860 //
+	ULONG	i =0, j=0;
+	AC_TXOP_CSR0_STRUC	csr0;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> NICInitializeAdapter\n"));
+
+	// 3. Set DMA global configuration except TX_DMA_EN and RX_DMA_EN bits:
+retry:
+	i = 0;
+	do
+	{
+		RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &GloCfg.word);
+		if ((GloCfg.field.TxDMABusy == 0)  && (GloCfg.field.RxDMABusy == 0))
+			break;
+
+		RTMPusecDelay(1000);
+		i++;
+	}while ( i<100);
+	DBGPRINT(RT_DEBUG_TRACE, ("<== DMA offset 0x208 = 0x%x\n", GloCfg.word));
+	GloCfg.word &= 0xff0;
+	GloCfg.field.EnTXWriteBackDDONE =1;
+	RTMP_IO_WRITE32(pAd, WPDMA_GLO_CFG, GloCfg.word);
+
+	// Record HW Beacon offset
+	pAd->BeaconOffset[0] = HW_BEACON_BASE0;
+	pAd->BeaconOffset[1] = HW_BEACON_BASE1;
+	pAd->BeaconOffset[2] = HW_BEACON_BASE2;
+	pAd->BeaconOffset[3] = HW_BEACON_BASE3;
+	pAd->BeaconOffset[4] = HW_BEACON_BASE4;
+	pAd->BeaconOffset[5] = HW_BEACON_BASE5;
+	pAd->BeaconOffset[6] = HW_BEACON_BASE6;
+	pAd->BeaconOffset[7] = HW_BEACON_BASE7;
+
+	//
+	// write all shared Ring's base address into ASIC
+	//
+
+	// asic simulation sequence put this ahead before loading firmware.
+	// pbf hardware reset
+#ifdef RT2860
+	RTMP_IO_WRITE32(pAd, WPDMA_RST_IDX, 0x1003f);	// 0x10000 for reset rx, 0x3f resets all 6 tx rings.
+	RTMP_IO_WRITE32(pAd, PBF_SYS_CTRL, 0xe1f);
+	RTMP_IO_WRITE32(pAd, PBF_SYS_CTRL, 0xe00);
+#endif // RT2860 //
+
+	// Initialze ASIC for TX & Rx operation
+	if (NICInitializeAsic(pAd , bHardReset) != NDIS_STATUS_SUCCESS)
+	{
+		if (j++ == 0)
+		{
+			NICLoadFirmware(pAd);
+			goto retry;
+		}
+		return NDIS_STATUS_FAILURE;
+	}
+
+
+#ifdef RT2860
+	// Write AC_BK base address register
+	Value = RTMP_GetPhysicalAddressLow(pAd->TxRing[QID_AC_BK].Cell[0].AllocPa);
+	RTMP_IO_WRITE32(pAd, TX_BASE_PTR1, Value);
+	DBGPRINT(RT_DEBUG_TRACE, ("--> TX_BASE_PTR1 : 0x%x\n", Value));
+
+	// Write AC_BE base address register
+	Value = RTMP_GetPhysicalAddressLow(pAd->TxRing[QID_AC_BE].Cell[0].AllocPa);
+	RTMP_IO_WRITE32(pAd, TX_BASE_PTR0, Value);
+	DBGPRINT(RT_DEBUG_TRACE, ("--> TX_BASE_PTR0 : 0x%x\n", Value));
+
+	// Write AC_VI base address register
+	Value = RTMP_GetPhysicalAddressLow(pAd->TxRing[QID_AC_VI].Cell[0].AllocPa);
+	RTMP_IO_WRITE32(pAd, TX_BASE_PTR2, Value);
+	DBGPRINT(RT_DEBUG_TRACE, ("--> TX_BASE_PTR2 : 0x%x\n", Value));
+
+	// Write AC_VO base address register
+	Value = RTMP_GetPhysicalAddressLow(pAd->TxRing[QID_AC_VO].Cell[0].AllocPa);
+	RTMP_IO_WRITE32(pAd, TX_BASE_PTR3, Value);
+	DBGPRINT(RT_DEBUG_TRACE, ("--> TX_BASE_PTR3 : 0x%x\n", Value));
+
+	// Write HCCA base address register
+	  Value = RTMP_GetPhysicalAddressLow(pAd->TxRing[QID_HCCA].Cell[0].AllocPa);
+	  RTMP_IO_WRITE32(pAd, TX_BASE_PTR4, Value);
+	DBGPRINT(RT_DEBUG_TRACE, ("--> TX_BASE_PTR4 : 0x%x\n", Value));
+
+	// Write MGMT_BASE_CSR register
+	Value = RTMP_GetPhysicalAddressLow(pAd->MgmtRing.Cell[0].AllocPa);
+	RTMP_IO_WRITE32(pAd, TX_BASE_PTR5, Value);
+	DBGPRINT(RT_DEBUG_TRACE, ("--> TX_BASE_PTR5 : 0x%x\n", Value));
+
+	// Write RX_BASE_CSR register
+	Value = RTMP_GetPhysicalAddressLow(pAd->RxRing.Cell[0].AllocPa);
+	RTMP_IO_WRITE32(pAd, RX_BASE_PTR, Value);
+	DBGPRINT(RT_DEBUG_TRACE, ("--> RX_BASE_PTR : 0x%x\n", Value));
+
+	// Init RX Ring index pointer
+	pAd->RxRing.RxSwReadIdx = 0;
+	pAd->RxRing.RxCpuIdx = RX_RING_SIZE-1;
+	RTMP_IO_WRITE32(pAd, RX_CRX_IDX, pAd->RxRing.RxCpuIdx);
+
+	// Init TX rings index pointer
+	{
+		for (i=0; i<NUM_OF_TX_RING; i++)
+		{
+			pAd->TxRing[i].TxSwFreeIdx = 0;
+			pAd->TxRing[i].TxCpuIdx = 0;
+			RTMP_IO_WRITE32(pAd, (TX_CTX_IDX0 + i * 0x10) ,  pAd->TxRing[i].TxCpuIdx);
+		}
+	}
+
+	// init MGMT ring index pointer
+	pAd->MgmtRing.TxSwFreeIdx = 0;
+	pAd->MgmtRing.TxCpuIdx = 0;
+	RTMP_IO_WRITE32(pAd, TX_MGMTCTX_IDX,  pAd->MgmtRing.TxCpuIdx);
+
+	//
+	// set each Ring's SIZE  into ASIC. Descriptor Size is fixed by design.
+	//
+
+	// Write TX_RING_CSR0 register
+	Value = TX_RING_SIZE;
+	RTMP_IO_WRITE32(pAd, TX_MAX_CNT0, Value);
+	RTMP_IO_WRITE32(pAd, TX_MAX_CNT1, Value);
+	RTMP_IO_WRITE32(pAd, TX_MAX_CNT2, Value);
+	RTMP_IO_WRITE32(pAd, TX_MAX_CNT3, Value);
+	RTMP_IO_WRITE32(pAd, TX_MAX_CNT4, Value);
+	Value = MGMT_RING_SIZE;
+	RTMP_IO_WRITE32(pAd, TX_MGMTMAX_CNT, Value);
+
+	// Write RX_RING_CSR register
+	Value = RX_RING_SIZE;
+	RTMP_IO_WRITE32(pAd, RX_MAX_CNT, Value);
+#endif // RT2860 //
+
+
+	// WMM parameter
+	csr0.word = 0;
+	RTMP_IO_WRITE32(pAd, WMM_TXOP0_CFG, csr0.word);
+	if (pAd->CommonCfg.PhyMode == PHY_11B)
+	{
+		csr0.field.Ac0Txop = 192;	// AC_VI: 192*32us ~= 6ms
+		csr0.field.Ac1Txop = 96;	// AC_VO: 96*32us  ~= 3ms
+	}
+	else
+	{
+		csr0.field.Ac0Txop = 96;	// AC_VI: 96*32us ~= 3ms
+		csr0.field.Ac1Txop = 48;	// AC_VO: 48*32us ~= 1.5ms
+	}
+	RTMP_IO_WRITE32(pAd, WMM_TXOP1_CFG, csr0.word);
+
+
+#ifdef RT2860
+	// 3. Set DMA global configuration except TX_DMA_EN and RX_DMA_EN bits:
+	i = 0;
+	do
+	{
+		RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &GloCfg.word);
+		if ((GloCfg.field.TxDMABusy == 0)  && (GloCfg.field.RxDMABusy == 0))
+			break;
+
+		RTMPusecDelay(1000);
+		i++;
+	}while ( i < 100);
+
+	GloCfg.word &= 0xff0;
+	GloCfg.field.EnTXWriteBackDDONE =1;
+	RTMP_IO_WRITE32(pAd, WPDMA_GLO_CFG, GloCfg.word);
+
+	IntCfg.word = 0;
+	RTMP_IO_WRITE32(pAd, DELAY_INT_CFG, IntCfg.word);
+#endif // RT2860 //
+
+
+	// reset action
+	// Load firmware
+	//  Status = NICLoadFirmware(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- NICInitializeAdapter\n"));
+	return Status;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Initialize ASIC
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS	NICInitializeAsic(
+	IN	PRTMP_ADAPTER	pAd,
+	IN  BOOLEAN		bHardReset)
+{
+	ULONG			Index = 0;
+	UCHAR			R0 = 0xff;
+	UINT32			MacCsr12 = 0, Counter = 0;
+	USHORT			KeyIdx;
+	INT				i,apidx;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> NICInitializeAsic\n"));
+
+#ifdef RT2860
+	if (bHardReset == TRUE)
+	{
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x3);
+	}
+	else
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x1);
+
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x0);
+	// Initialize MAC register to default value
+	for (Index = 0; Index < NUM_MAC_REG_PARMS; Index++)
+	{
+		RTMP_IO_WRITE32(pAd, MACRegTable[Index].Register, MACRegTable[Index].Value);
+	}
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		for (Index = 0; Index < NUM_STA_MAC_REG_PARMS; Index++)
+		{
+			RTMP_IO_WRITE32(pAd, STAMACRegTable[Index].Register, STAMACRegTable[Index].Value);
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+#endif // RT2860 //
+
+
+	//
+	// Before program BBP, we need to wait BBP/RF get wake up.
+	//
+	Index = 0;
+	do
+	{
+		RTMP_IO_READ32(pAd, MAC_STATUS_CFG, &MacCsr12);
+
+		if ((MacCsr12 & 0x03) == 0)	// if BB.RF is stable
+			break;
+
+		DBGPRINT(RT_DEBUG_TRACE, ("Check MAC_STATUS_CFG  = Busy = %x\n", MacCsr12));
+		RTMPusecDelay(1000);
+	} while (Index++ < 100);
+
+    // The commands to firmware should be after these commands, these commands will init firmware
+	// PCI and USB are not the same because PCI driver needs to wait for PCI bus ready
+	RTMP_IO_WRITE32(pAd, H2M_BBP_AGENT, 0);	// initialize BBP R/W access agent
+	RTMP_IO_WRITE32(pAd, H2M_MAILBOX_CSR, 0);
+	RTMPusecDelay(1000);
+
+	// Read BBP register, make sure BBP is up and running before write new data
+	Index = 0;
+	do
+	{
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R0, &R0);
+		DBGPRINT(RT_DEBUG_TRACE, ("BBP version = %x\n", R0));
+	} while ((++Index < 20) && ((R0 == 0xff) || (R0 == 0x00)));
+	//ASSERT(Index < 20); //this will cause BSOD on Check-build driver
+
+	if ((R0 == 0xff) || (R0 == 0x00))
+		return NDIS_STATUS_FAILURE;
+
+	// Initialize BBP register to default value
+	for (Index = 0; Index < NUM_BBP_REG_PARMS; Index++)
+	{
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBPRegTable[Index].Register, BBPRegTable[Index].Value);
+	}
+
+	// for rt2860E and after, init BBP_R84 with 0x19. This is for extension channel overlapping IOT.
+	if ((pAd->MACVersion&0xffff) != 0x0101)
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R84, 0x19);
+
+
+	if (pAd->MACVersion == 0x28600100)
+	{
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, 0x16);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R73, 0x12);
+    }
+
+	if (pAd->MACVersion >= RALINK_2880E_VERSION && pAd->MACVersion < RALINK_3070_VERSION) // 3*3
+	{
+		// enlarge MAX_LEN_CFG
+		UINT32 csr;
+		RTMP_IO_READ32(pAd, MAX_LEN_CFG, &csr);
+		csr &= 0xFFF;
+		csr |= 0x2000;
+		RTMP_IO_WRITE32(pAd, MAX_LEN_CFG, csr);
+	}
+
+
+	// Add radio off control
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (pAd->StaCfg.bRadio == FALSE)
+		{
+//			RTMP_IO_WRITE32(pAd, PWR_PIN_CFG, 0x00001818);
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF);
+			DBGPRINT(RT_DEBUG_TRACE, ("Set Radio Off\n"));
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	// Clear raw counters
+	RTMP_IO_READ32(pAd, RX_STA_CNT0, &Counter);
+	RTMP_IO_READ32(pAd, RX_STA_CNT1, &Counter);
+	RTMP_IO_READ32(pAd, RX_STA_CNT2, &Counter);
+	RTMP_IO_READ32(pAd, TX_STA_CNT0, &Counter);
+	RTMP_IO_READ32(pAd, TX_STA_CNT1, &Counter);
+	RTMP_IO_READ32(pAd, TX_STA_CNT2, &Counter);
+
+	// ASIC will keep garbage value after boot
+	// Clear all seared key table when initial
+	// This routine can be ignored in radio-ON/OFF operation.
+	if (bHardReset)
+	{
+		for (KeyIdx = 0; KeyIdx < 4; KeyIdx++)
+		{
+			RTMP_IO_WRITE32(pAd, SHARED_KEY_MODE_BASE + 4*KeyIdx, 0);
+		}
+
+		// Clear all pairwise key table when initial
+		for (KeyIdx = 0; KeyIdx < 256; KeyIdx++)
+		{
+			RTMP_IO_WRITE32(pAd, MAC_WCID_ATTRIBUTE_BASE + (KeyIdx * HW_WCID_ATTRI_SIZE), 1);
+		}
+	}
+
+
+	// It isn't necessary to clear this space when not hard reset.
+	if (bHardReset == TRUE)
+	{
+		// clear all on-chip BEACON frame space
+		for (apidx = 0; apidx < HW_BEACON_MAX_COUNT; apidx++)
+		{
+			for (i = 0; i < HW_BEACON_OFFSET>>2; i+=4)
+				RTMP_IO_WRITE32(pAd, pAd->BeaconOffset[apidx] + i, 0x00);
+		}
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// for rt2860E and after, init TXOP_CTRL_CFG with 0x583f. This is for extension channel overlapping IOT.
+		if ((pAd->MACVersion&0xffff) != 0x0101)
+			RTMP_IO_WRITE32(pAd, TXOP_CTRL_CFG, 0x583f);
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- NICInitializeAsic\n"));
+	return NDIS_STATUS_SUCCESS;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Reset NIC Asics
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+		Reset NIC to initial state AS IS system boot up time.
+
+	========================================================================
+*/
+VOID	NICIssueReset(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	UINT32	Value = 0;
+	DBGPRINT(RT_DEBUG_TRACE, ("--> NICIssueReset\n"));
+
+	// Disable Rx, register value supposed will remain after reset
+	RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+	Value &= (0xfffffff3);
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+	// Issue reset and clear from reset state
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x03); // 2004-09-17 change from 0x01
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x00);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- NICIssueReset\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Check ASIC registers and find any reason the system might hang
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	========================================================================
+*/
+BOOLEAN	NICCheckForHang(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	return (FALSE);
+}
+
+VOID NICUpdateFifoStaCounters(
+	IN PRTMP_ADAPTER pAd)
+{
+	TX_STA_FIFO_STRUC	StaFifo;
+	MAC_TABLE_ENTRY		*pEntry;
+	UCHAR				i = 0;
+	UCHAR			pid = 0, wcid = 0;
+	CHAR				reTry;
+	UCHAR				succMCS;
+
+#ifdef RALINK_ATE
+	/* Nothing to do in ATE mode */
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+		do
+		{
+			RTMP_IO_READ32(pAd, TX_STA_FIFO, &StaFifo.word);
+
+			if (StaFifo.field.bValid == 0)
+				break;
+
+			wcid = (UCHAR)StaFifo.field.wcid;
+
+
+		/* ignore NoACK and MGMT frame use 0xFF as WCID */
+			if ((StaFifo.field.TxAckRequired == 0) || (wcid >= MAX_LEN_OF_MAC_TABLE))
+			{
+				i++;
+				continue;
+			}
+
+			/* PID store Tx MCS Rate */
+			pid = (UCHAR)StaFifo.field.PidType;
+
+			pEntry = &pAd->MacTab.Content[wcid];
+
+			pEntry->DebugFIFOCount++;
+
+#ifdef DOT11_N_SUPPORT
+			if (StaFifo.field.TxBF) // 3*3
+				pEntry->TxBFCount++;
+#endif // DOT11_N_SUPPORT //
+
+#ifdef UAPSD_AP_SUPPORT
+			UAPSD_SP_AUE_Handle(pAd, pEntry, StaFifo.field.TxSuccess);
+#endif // UAPSD_AP_SUPPORT //
+
+			if (!StaFifo.field.TxSuccess)
+			{
+				pEntry->FIFOCount++;
+				pEntry->OneSecTxFailCount++;
+
+				if (pEntry->FIFOCount >= 1)
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("#"));
+#if 0
+					SendRefreshBAR(pAd, pEntry);
+					pEntry->NoBADataCountDown = 64;
+#else
+#ifdef DOT11_N_SUPPORT
+					pEntry->NoBADataCountDown = 64;
+#endif // DOT11_N_SUPPORT //
+
+					if(pEntry->PsMode == PWR_ACTIVE)
+					{
+#ifdef DOT11_N_SUPPORT
+						int tid;
+						for (tid=0; tid<NUM_OF_TID; tid++)
+						{
+							BAOriSessionTearDown(pAd, pEntry->Aid,  tid, FALSE, FALSE);
+						}
+#endif // DOT11_N_SUPPORT //
+
+						// Update the continuous transmission counter except PS mode
+						pEntry->ContinueTxFailCnt++;
+					}
+					else
+					{
+						// Clear the FIFOCount when sta in Power Save mode. Basically we assume
+						//     this tx error happened due to sta just go to sleep.
+						pEntry->FIFOCount = 0;
+						pEntry->ContinueTxFailCnt = 0;
+					}
+#endif
+					//pEntry->FIFOCount = 0;
+				}
+				//pEntry->bSendBAR = TRUE;
+			}
+			else
+			{
+#ifdef DOT11_N_SUPPORT
+				if ((pEntry->PsMode != PWR_SAVE) && (pEntry->NoBADataCountDown > 0))
+				{
+					pEntry->NoBADataCountDown--;
+					if (pEntry->NoBADataCountDown==0)
+					{
+						DBGPRINT(RT_DEBUG_TRACE, ("@\n"));
+					}
+				}
+#endif // DOT11_N_SUPPORT //
+				pEntry->FIFOCount = 0;
+				pEntry->OneSecTxNoRetryOkCount++;
+				// update NoDataIdleCount when sucessful send packet to STA.
+				pEntry->NoDataIdleCount = 0;
+				pEntry->ContinueTxFailCnt = 0;
+			}
+
+			succMCS = StaFifo.field.SuccessRate & 0x7F;
+
+			reTry = pid - succMCS;
+
+			if (StaFifo.field.TxSuccess)
+			{
+				pEntry->TXMCSExpected[pid]++;
+				if (pid == succMCS)
+				{
+					pEntry->TXMCSSuccessful[pid]++;
+				}
+				else
+				{
+					pEntry->TXMCSAutoFallBack[pid][succMCS]++;
+				}
+			}
+			else
+			{
+				pEntry->TXMCSFailed[pid]++;
+			}
+
+			if (reTry > 0)
+			{
+				if ((pid >= 12) && succMCS <=7)
+				{
+					reTry -= 4;
+				}
+				pEntry->OneSecTxRetryOkCount += reTry;
+			}
+
+			i++;
+			// ASIC store 16 stack
+		} while ( i < (2*TX_RING_SIZE) );
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Read statistical counters from hardware registers and record them
+		in software variables for later on query
+
+	Arguments:
+		pAd					Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	========================================================================
+*/
+VOID NICUpdateRawCounters(
+	IN PRTMP_ADAPTER pAd)
+{
+	UINT32	OldValue;
+	RX_STA_CNT0_STRUC	 RxStaCnt0;
+	RX_STA_CNT1_STRUC   RxStaCnt1;
+	RX_STA_CNT2_STRUC   RxStaCnt2;
+	TX_STA_CNT0_STRUC 	 TxStaCnt0;
+	TX_STA_CNT1_STRUC	 StaTx1;
+	TX_STA_CNT2_STRUC	 StaTx2;
+	TX_AGG_CNT_STRUC	TxAggCnt;
+	TX_AGG_CNT0_STRUC	TxAggCnt0;
+	TX_AGG_CNT1_STRUC	TxAggCnt1;
+	TX_AGG_CNT2_STRUC	TxAggCnt2;
+	TX_AGG_CNT3_STRUC	TxAggCnt3;
+	TX_AGG_CNT4_STRUC	TxAggCnt4;
+	TX_AGG_CNT5_STRUC	TxAggCnt5;
+	TX_AGG_CNT6_STRUC	TxAggCnt6;
+	TX_AGG_CNT7_STRUC	TxAggCnt7;
+
+	RTMP_IO_READ32(pAd, RX_STA_CNT0, &RxStaCnt0.word);
+	RTMP_IO_READ32(pAd, RX_STA_CNT2, &RxStaCnt2.word);
+
+	{
+		RTMP_IO_READ32(pAd, RX_STA_CNT1, &RxStaCnt1.word);
+	    // Update RX PLCP error counter
+	    pAd->PrivateInfo.PhyRxErrCnt += RxStaCnt1.field.PlcpErr;
+		// Update False CCA counter
+		pAd->RalinkCounters.OneSecFalseCCACnt += RxStaCnt1.field.FalseCca;
+	}
+
+	// Update FCS counters
+	OldValue= pAd->WlanCounters.FCSErrorCount.u.LowPart;
+	pAd->WlanCounters.FCSErrorCount.u.LowPart += (RxStaCnt0.field.CrcErr); // >> 7);
+	if (pAd->WlanCounters.FCSErrorCount.u.LowPart < OldValue)
+		pAd->WlanCounters.FCSErrorCount.u.HighPart++;
+
+	// Add FCS error count to private counters
+	pAd->RalinkCounters.OneSecRxFcsErrCnt += RxStaCnt0.field.CrcErr;
+	OldValue = pAd->RalinkCounters.RealFcsErrCount.u.LowPart;
+	pAd->RalinkCounters.RealFcsErrCount.u.LowPart += RxStaCnt0.field.CrcErr;
+	if (pAd->RalinkCounters.RealFcsErrCount.u.LowPart < OldValue)
+		pAd->RalinkCounters.RealFcsErrCount.u.HighPart++;
+
+	// Update Duplicate Rcv check
+	pAd->RalinkCounters.DuplicateRcv += RxStaCnt2.field.RxDupliCount;
+	pAd->WlanCounters.FrameDuplicateCount.u.LowPart += RxStaCnt2.field.RxDupliCount;
+	// Update RX Overflow counter
+	pAd->Counters8023.RxNoBuffer += (RxStaCnt2.field.RxFifoOverflowCount);
+
+	if (!pAd->bUpdateBcnCntDone)
+	{
+	// Update BEACON sent count
+	RTMP_IO_READ32(pAd, TX_STA_CNT0, &TxStaCnt0.word);
+	RTMP_IO_READ32(pAd, TX_STA_CNT1, &StaTx1.word);
+	RTMP_IO_READ32(pAd, TX_STA_CNT2, &StaTx2.word);
+	pAd->RalinkCounters.OneSecBeaconSentCnt += TxStaCnt0.field.TxBeaconCount;
+	pAd->RalinkCounters.OneSecTxRetryOkCount += StaTx1.field.TxRetransmit;
+	pAd->RalinkCounters.OneSecTxNoRetryOkCount += StaTx1.field.TxSuccess;
+	pAd->RalinkCounters.OneSecTxFailCount += TxStaCnt0.field.TxFailCount;
+	pAd->WlanCounters.TransmittedFragmentCount.u.LowPart += StaTx1.field.TxSuccess;
+	pAd->WlanCounters.RetryCount.u.LowPart += StaTx1.field.TxRetransmit;
+	pAd->WlanCounters.FailedCount.u.LowPart += TxStaCnt0.field.TxFailCount;
+	}
+
+	{
+		RTMP_IO_READ32(pAd, TX_AGG_CNT, &TxAggCnt.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT0, &TxAggCnt0.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT1, &TxAggCnt1.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT2, &TxAggCnt2.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT3, &TxAggCnt3.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT4, &TxAggCnt4.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT5, &TxAggCnt5.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT6, &TxAggCnt6.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT7, &TxAggCnt7.word);
+		pAd->RalinkCounters.TxAggCount += TxAggCnt.field.AggTxCount;
+		pAd->RalinkCounters.TxNonAggCount += TxAggCnt.field.NonAggTxCount;
+		pAd->RalinkCounters.TxAgg1MPDUCount += TxAggCnt0.field.AggSize1Count;
+		pAd->RalinkCounters.TxAgg2MPDUCount += TxAggCnt0.field.AggSize2Count;
+
+		pAd->RalinkCounters.TxAgg3MPDUCount += TxAggCnt1.field.AggSize3Count;
+		pAd->RalinkCounters.TxAgg4MPDUCount += TxAggCnt1.field.AggSize4Count;
+		pAd->RalinkCounters.TxAgg5MPDUCount += TxAggCnt2.field.AggSize5Count;
+		pAd->RalinkCounters.TxAgg6MPDUCount += TxAggCnt2.field.AggSize6Count;
+
+		pAd->RalinkCounters.TxAgg7MPDUCount += TxAggCnt3.field.AggSize7Count;
+		pAd->RalinkCounters.TxAgg8MPDUCount += TxAggCnt3.field.AggSize8Count;
+		pAd->RalinkCounters.TxAgg9MPDUCount += TxAggCnt4.field.AggSize9Count;
+		pAd->RalinkCounters.TxAgg10MPDUCount += TxAggCnt4.field.AggSize10Count;
+
+		pAd->RalinkCounters.TxAgg11MPDUCount += TxAggCnt5.field.AggSize11Count;
+		pAd->RalinkCounters.TxAgg12MPDUCount += TxAggCnt5.field.AggSize12Count;
+		pAd->RalinkCounters.TxAgg13MPDUCount += TxAggCnt6.field.AggSize13Count;
+		pAd->RalinkCounters.TxAgg14MPDUCount += TxAggCnt6.field.AggSize14Count;
+
+		pAd->RalinkCounters.TxAgg15MPDUCount += TxAggCnt7.field.AggSize15Count;
+		pAd->RalinkCounters.TxAgg16MPDUCount += TxAggCnt7.field.AggSize16Count;
+
+		// Calculate the transmitted A-MPDU count
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += TxAggCnt0.field.AggSize1Count;
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt0.field.AggSize2Count / 2);
+
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt1.field.AggSize3Count / 3);
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt1.field.AggSize4Count / 4);
+
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt2.field.AggSize5Count / 5);
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt2.field.AggSize6Count / 6);
+
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt3.field.AggSize7Count / 7);
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt3.field.AggSize8Count / 8);
+
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt4.field.AggSize9Count / 9);
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt4.field.AggSize10Count / 10);
+
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt5.field.AggSize11Count / 11);
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt5.field.AggSize12Count / 12);
+
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt6.field.AggSize13Count / 13);
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt6.field.AggSize14Count / 14);
+
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt7.field.AggSize15Count / 15);
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt7.field.AggSize16Count / 16);
+	}
+
+#ifdef DBG_DIAGNOSE
+	{
+		RtmpDiagStruct	*pDiag;
+		COUNTER_RALINK	*pRalinkCounters;
+		UCHAR			ArrayCurIdx, i;
+
+		pDiag = &pAd->DiagStruct;
+		pRalinkCounters = &pAd->RalinkCounters;
+		ArrayCurIdx = pDiag->ArrayCurIdx;
+
+		if (pDiag->inited == 0)
+		{
+			NdisZeroMemory(pDiag, sizeof(struct _RtmpDiagStrcut_));
+			pDiag->ArrayStartIdx = pDiag->ArrayCurIdx = 0;
+			pDiag->inited = 1;
+		}
+		else
+		{
+			// Tx
+			pDiag->TxFailCnt[ArrayCurIdx] = TxStaCnt0.field.TxFailCount;
+			pDiag->TxAggCnt[ArrayCurIdx] = TxAggCnt.field.AggTxCount;
+			pDiag->TxNonAggCnt[ArrayCurIdx] = TxAggCnt.field.NonAggTxCount;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][0] = TxAggCnt0.field.AggSize1Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][1] = TxAggCnt0.field.AggSize2Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][2] = TxAggCnt1.field.AggSize3Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][3] = TxAggCnt1.field.AggSize4Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][4] = TxAggCnt2.field.AggSize5Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][5] = TxAggCnt2.field.AggSize6Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][6] = TxAggCnt3.field.AggSize7Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][7] = TxAggCnt3.field.AggSize8Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][8] = TxAggCnt4.field.AggSize9Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][9] = TxAggCnt4.field.AggSize10Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][10] = TxAggCnt5.field.AggSize11Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][11] = TxAggCnt5.field.AggSize12Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][12] = TxAggCnt6.field.AggSize13Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][13] = TxAggCnt6.field.AggSize14Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][14] = TxAggCnt7.field.AggSize15Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][15] = TxAggCnt7.field.AggSize16Count;
+
+			pDiag->RxCrcErrCnt[ArrayCurIdx] = RxStaCnt0.field.CrcErr;
+
+			INC_RING_INDEX(pDiag->ArrayCurIdx,  DIAGNOSE_TIME);
+			ArrayCurIdx = pDiag->ArrayCurIdx;
+			for (i =0; i < 9; i++)
+			{
+				pDiag->TxDescCnt[ArrayCurIdx][i]= 0;
+				pDiag->TxSWQueCnt[ArrayCurIdx][i] =0;
+				pDiag->TxMcsCnt[ArrayCurIdx][i] = 0;
+				pDiag->RxMcsCnt[ArrayCurIdx][i] = 0;
+			}
+			pDiag->TxDataCnt[ArrayCurIdx] = 0;
+			pDiag->TxFailCnt[ArrayCurIdx] = 0;
+			pDiag->RxDataCnt[ArrayCurIdx] = 0;
+			pDiag->RxCrcErrCnt[ArrayCurIdx]  = 0;
+			for (i = 9; i < 24; i++) // 3*3
+			{
+				pDiag->TxDescCnt[ArrayCurIdx][i] = 0;
+				pDiag->TxMcsCnt[ArrayCurIdx][i] = 0;
+				pDiag->RxMcsCnt[ArrayCurIdx][i] = 0;
+}
+
+			if (pDiag->ArrayCurIdx == pDiag->ArrayStartIdx)
+				INC_RING_INDEX(pDiag->ArrayStartIdx,  DIAGNOSE_TIME);
+		}
+
+	}
+#endif // DBG_DIAGNOSE //
+
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Reset NIC from error
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+		Reset NIC from error state
+
+	========================================================================
+*/
+VOID	NICResetFromError(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	// Reset BBP (according to alex, reset ASIC will force reset BBP
+	// Therefore, skip the reset BBP
+	// RTMP_IO_WRITE32(pAd, MAC_CSR1, 0x2);
+
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x1);
+	// Remove ASIC from reset state
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x0);
+
+	NICInitializeAdapter(pAd, FALSE);
+	NICInitAsicFromEEPROM(pAd);
+
+	// Switch to current channel, since during reset process, the connection should remains on.
+	AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+	AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		erase 8051 firmware image in MAC ASIC
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	IRQL = PASSIVE_LEVEL
+
+	========================================================================
+*/
+VOID NICEraseFirmware(
+	IN PRTMP_ADAPTER pAd)
+{
+	ULONG i;
+
+	for(i=0; i<MAX_FIRMWARE_IMAGE_SIZE; i+=4)
+		RTMP_IO_WRITE32(pAd, FIRMWARE_IMAGE_BASE + i, 0);
+
+}/* End of NICEraseFirmware */
+
+/*
+	========================================================================
+
+	Routine Description:
+		Load 8051 firmware RT2561.BIN file into MAC ASIC
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		NDIS_STATUS_SUCCESS         firmware image load ok
+		NDIS_STATUS_FAILURE         image not found
+
+	IRQL = PASSIVE_LEVEL
+
+	========================================================================
+*/
+NDIS_STATUS NICLoadFirmware(
+	IN PRTMP_ADAPTER pAd)
+{
+#ifdef BIN_IN_FILE
+#define NICLF_DEFAULT_USE()	\
+	flg_default_firm_use = TRUE; \
+	printk("%s - Use default firmware!\n", __FUNCTION__);
+
+	NDIS_STATUS		Status = NDIS_STATUS_SUCCESS;
+	PUCHAR			src;
+	struct file		*srcf;
+	INT 			retval, orgfsuid, orgfsgid, i;
+   	mm_segment_t	orgfs;
+	PUCHAR			pFirmwareImage;
+	UINT			FileLength = 0;
+	UINT32			MacReg;
+	ULONG			Index;
+	ULONG			firm;
+	BOOLEAN			flg_default_firm_use = FALSE;
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("===> %s\n", __FUNCTION__));
+
+	/* init */
+	pFirmwareImage = NULL;
+	src = RTMP_FIRMWARE_FILE_NAME;
+
+	/* save uid and gid used for filesystem access.
+	   set user and group to 0 (root) */
+	orgfsuid = current->fsuid;
+	orgfsgid = current->fsgid;
+	current->fsuid = current->fsgid = 0;
+    orgfs = get_fs();
+    set_fs(KERNEL_DS);
+
+	pAd->FirmwareVersion = (FIRMWARE_MAJOR_VERSION << 8) + \
+						   FIRMWARE_MINOR_VERSION;
+
+
+	/* allocate firmware buffer */
+    pFirmwareImage = kmalloc(MAX_FIRMWARE_IMAGE_SIZE, MEM_ALLOC_FLAG);
+    if (pFirmwareImage == NULL)
+	{
+		/* allocate fail, use default firmware array in firmware.h */
+		printk("%s - Allocate memory fail!\n", __FUNCTION__);
+		NICLF_DEFAULT_USE();
+    }
+	else
+	{
+		/* allocate ok! zero the firmware buffer */
+		memset(pFirmwareImage, 0x00, MAX_FIRMWARE_IMAGE_SIZE);
+	} /* End of if */
+
+
+	/* if ok, read firmware file from *.bin file */
+	if (flg_default_firm_use == FALSE)
+	{
+		do
+		{
+			/* open the bin file */
+			srcf = filp_open(src, O_RDONLY, 0);
+
+			if (IS_ERR(srcf))
+			{
+				printk("%s - Error %ld opening %s\n",
+					   __FUNCTION__, -PTR_ERR(srcf), src);
+				NICLF_DEFAULT_USE();
+				break;
+			} /* End of if */
+
+			/* the object must have a read method */
+			if ((srcf->f_op == NULL) || (srcf->f_op->read == NULL))
+			{
+				printk("%s - %s does not have a write method\n", __FUNCTION__, src);
+				NICLF_DEFAULT_USE();
+				break;
+			} /* End of if */
+
+			/* read the firmware from the file *.bin */
+			FileLength = srcf->f_op->read(srcf,
+										  pFirmwareImage,
+										  MAX_FIRMWARE_IMAGE_SIZE,
+										  &srcf->f_pos);
+
+			if (FileLength != MAX_FIRMWARE_IMAGE_SIZE)
+			{
+				printk("%s: error file length (=%d) in RT2860AP.BIN\n",
+					   __FUNCTION__, FileLength);
+				NICLF_DEFAULT_USE();
+				break;
+			}
+			else
+			{
+				PUCHAR ptr = pFirmwareImage;
+				USHORT crc = 0xffff;
+
+
+				/* calculate firmware CRC */
+				for(i=0; i<(MAX_FIRMWARE_IMAGE_SIZE-2); i++, ptr++)
+					crc = ByteCRC16(bitrev8(*ptr), crc);
+				/* End of for */
+
+				if ((pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-2] != \
+								(UCHAR)bitrev8((UCHAR)(crc>>8))) ||
+					(pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-1] != \
+								(UCHAR)bitrev8((UCHAR)crc)))
+				{
+					/* CRC fail */
+					printk("%s: CRC = 0x%02x 0x%02x "
+						   "error, should be 0x%02x 0x%02x\n",
+						   __FUNCTION__,
+						   pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-2],
+						   pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-1],
+						   (UCHAR)(crc>>8), (UCHAR)(crc));
+					NICLF_DEFAULT_USE();
+					break;
+				}
+				else
+				{
+					/* firmware is ok */
+					pAd->FirmwareVersion = \
+						(pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-4] << 8) +
+						pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-3];
+
+					/* check if firmware version of the file is too old */
+					if ((pAd->FirmwareVersion) < \
+											((FIRMWARE_MAJOR_VERSION << 8) +
+									  	 	 FIRMWARE_MINOR_VERSION))
+					{
+						printk("%s: firmware version too old!\n", __FUNCTION__);
+						NICLF_DEFAULT_USE();
+						break;
+					} /* End of if */
+				} /* End of if */
+
+				DBGPRINT(RT_DEBUG_TRACE,
+						 ("NICLoadFirmware: CRC ok, ver=%d.%d\n",
+						  pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-4],
+						  pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-3]));
+			} /* End of if (FileLength == MAX_FIRMWARE_IMAGE_SIZE) */
+			break;
+		} while(TRUE);
+
+		/* close firmware file */
+		if (IS_ERR(srcf))
+			;
+		else
+		{
+			retval = filp_close(srcf, NULL);
+			if (retval)
+			{
+				DBGPRINT(RT_DEBUG_ERROR,
+						 ("--> Error %d closing %s\n", -retval, src));
+			} /* End of if */
+		} /* End of if */
+	} /* End of if */
+
+
+	/* write firmware to ASIC */
+	if (flg_default_firm_use == TRUE)
+	{
+		/* use default fimeware, free allocated buffer */
+		if (pFirmwareImage != NULL)
+			kfree(pFirmwareImage);
+		/* End of if */
+
+		/* use default *.bin array */
+		pFirmwareImage = FirmwareImage;
+		FileLength = sizeof(FirmwareImage);
+	} /* End of if */
+
+	/* enable Host program ram write selection */
+	RTMP_IO_WRITE32(pAd, PBF_SYS_CTRL, 0x10000);
+
+	for(i=0; i<FileLength; i+=4)
+	{
+		firm = pFirmwareImage[i] +
+			   (pFirmwareImage[i+3] << 24) +
+			   (pFirmwareImage[i+2] << 16) +
+			   (pFirmwareImage[i+1] << 8);
+
+		RTMP_IO_WRITE32(pAd, FIRMWARE_IMAGE_BASE + i, firm);
+	} /* End of for */
+
+	RTMP_IO_WRITE32(pAd, PBF_SYS_CTRL, 0x00000);
+	RTMP_IO_WRITE32(pAd, PBF_SYS_CTRL, 0x00001);
+
+	/* initialize BBP R/W access agent */
+	RTMP_IO_WRITE32(pAd, H2M_BBP_AGENT, 0);
+	RTMP_IO_WRITE32(pAd, H2M_MAILBOX_CSR, 0);
+
+	if (flg_default_firm_use == FALSE)
+	{
+		/* use file firmware, free allocated buffer */
+		if (pFirmwareImage != NULL)
+			kfree(pFirmwareImage);
+		/* End of if */
+	} /* End of if */
+
+	set_fs(orgfs);
+	current->fsuid = orgfsuid;
+	current->fsgid = orgfsgid;
+#else
+
+	NDIS_STATUS		Status = NDIS_STATUS_SUCCESS;
+	PUCHAR			pFirmwareImage;
+	ULONG			FileLength, Index;
+	//ULONG			firm;
+	UINT32			MacReg = 0;
+
+	pFirmwareImage = FirmwareImage;
+	FileLength = sizeof(FirmwareImage);
+	RT28XX_WRITE_FIRMWARE(pAd, pFirmwareImage, FileLength);
+#endif
+
+	/* check if MCU is ready */
+	Index = 0;
+	do
+	{
+		RTMP_IO_READ32(pAd, PBF_SYS_CTRL, &MacReg);
+
+		if (MacReg & 0x80)
+			break;
+
+		RTMPusecDelay(1000);
+	} while (Index++ < 1000);
+
+    if (Index >= 1000)
+	{
+		Status = NDIS_STATUS_FAILURE;
+		DBGPRINT(RT_DEBUG_ERROR, ("NICLoadFirmware: MCU is not ready\n\n\n"));
+	} /* End of if */
+
+    DBGPRINT(RT_DEBUG_TRACE,
+			 ("<=== %s (status=%d)\n", __FUNCTION__, Status));
+    return Status;
+} /* End of NICLoadFirmware */
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Load Tx rate switching parameters
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		NDIS_STATUS_SUCCESS         firmware image load ok
+		NDIS_STATUS_FAILURE         image not found
+
+	IRQL = PASSIVE_LEVEL
+
+	Rate Table Format:
+		1. (B0: Valid Item number) (B1:Initial item from zero)
+		2. Item Number(Dec)      Mode(Hex)     Current MCS(Dec)    TrainUp(Dec)    TrainDown(Dec)
+
+	========================================================================
+*/
+NDIS_STATUS NICLoadRateSwitchingParams(
+	IN PRTMP_ADAPTER pAd)
+{
+	return NDIS_STATUS_SUCCESS;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		if  pSrc1 all zero with length Length, return 0.
+		If not all zero, return 1
+
+	Arguments:
+		pSrc1
+
+	Return Value:
+		1:			not all zero
+		0:			all zero
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+ULONG	RTMPNotAllZero(
+	IN	PVOID	pSrc1,
+	IN	ULONG	Length)
+{
+	PUCHAR	pMem1;
+	ULONG	Index = 0;
+
+	pMem1 = (PUCHAR) pSrc1;
+
+	for (Index = 0; Index < Length; Index++)
+	{
+		if (pMem1[Index] != 0x0)
+		{
+			break;
+		}
+	}
+
+	if (Index == Length)
+	{
+		return (0);
+	}
+	else
+	{
+		return (1);
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Compare two memory block
+
+	Arguments:
+		pSrc1		Pointer to first memory address
+		pSrc2		Pointer to second memory address
+
+	Return Value:
+		0:			memory is equal
+		1:			pSrc1 memory is larger
+		2:			pSrc2 memory is larger
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+ULONG	RTMPCompareMemory(
+	IN	PVOID	pSrc1,
+	IN	PVOID	pSrc2,
+	IN	ULONG	Length)
+{
+	PUCHAR	pMem1;
+	PUCHAR	pMem2;
+	ULONG	Index = 0;
+
+	pMem1 = (PUCHAR) pSrc1;
+	pMem2 = (PUCHAR) pSrc2;
+
+	for (Index = 0; Index < Length; Index++)
+	{
+		if (pMem1[Index] > pMem2[Index])
+			return (1);
+		else if (pMem1[Index] < pMem2[Index])
+			return (2);
+	}
+
+	// Equal
+	return (0);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Zero out memory block
+
+	Arguments:
+		pSrc1		Pointer to memory address
+		Length		Size
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPZeroMemory(
+	IN	PVOID	pSrc,
+	IN	ULONG	Length)
+{
+	PUCHAR	pMem;
+	ULONG	Index = 0;
+
+	pMem = (PUCHAR) pSrc;
+
+	for (Index = 0; Index < Length; Index++)
+	{
+		pMem[Index] = 0x00;
+	}
+}
+
+VOID	RTMPFillMemory(
+	IN	PVOID	pSrc,
+	IN	ULONG	Length,
+	IN	UCHAR	Fill)
+{
+	PUCHAR	pMem;
+	ULONG	Index = 0;
+
+	pMem = (PUCHAR) pSrc;
+
+	for (Index = 0; Index < Length; Index++)
+	{
+		pMem[Index] = Fill;
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Copy data from memory block 1 to memory block 2
+
+	Arguments:
+		pDest		Pointer to destination memory address
+		pSrc		Pointer to source memory address
+		Length		Copy size
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPMoveMemory(
+	OUT	PVOID	pDest,
+	IN	PVOID	pSrc,
+	IN	ULONG	Length)
+{
+	PUCHAR	pMem1;
+	PUCHAR	pMem2;
+	UINT	Index;
+
+	ASSERT((Length==0) || (pDest && pSrc));
+
+	pMem1 = (PUCHAR) pDest;
+	pMem2 = (PUCHAR) pSrc;
+
+	for (Index = 0; Index < Length; Index++)
+	{
+		pMem1[Index] = pMem2[Index];
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Initialize port configuration structure
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	UserCfgInit(
+	IN	PRTMP_ADAPTER pAd)
+{
+    UINT key_index, bss_index;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> UserCfgInit\n"));
+
+	//
+	//  part I. intialize common configuration
+	//
+
+	for(key_index=0; key_index<SHARE_KEY_NUM; key_index++)
+	{
+		for(bss_index = 0; bss_index < MAX_MBSSID_NUM; bss_index++)
+		{
+			pAd->SharedKey[bss_index][key_index].KeyLen = 0;
+			pAd->SharedKey[bss_index][key_index].CipherAlg = CIPHER_NONE;
+		}
+	}
+
+	pAd->Antenna.word = 0;
+	pAd->CommonCfg.BBPCurrentBW = BW_20;
+
+	pAd->LedCntl.word = 0;
+#ifdef RT2860
+	pAd->LedIndicatorStregth = 0;
+	pAd->RLnkCtrlOffset = 0;
+	pAd->HostLnkCtrlOffset = 0;
+#endif // RT2860 //
+
+	pAd->bAutoTxAgcA = FALSE;			// Default is OFF
+	pAd->bAutoTxAgcG = FALSE;			// Default is OFF
+	pAd->RfIcType = RFIC_2820;
+
+	// Init timer for reset complete event
+	pAd->CommonCfg.CentralChannel = 1;
+	pAd->bForcePrintTX = FALSE;
+	pAd->bForcePrintRX = FALSE;
+	pAd->bStaFifoTest = FALSE;
+	pAd->bProtectionTest = FALSE;
+	pAd->bHCCATest = FALSE;
+	pAd->bGenOneHCCA = FALSE;
+	pAd->CommonCfg.Dsifs = 10;      // in units of usec
+	pAd->CommonCfg.TxPower = 100; //mW
+	pAd->CommonCfg.TxPowerPercentage = 0xffffffff; // AUTO
+	pAd->CommonCfg.TxPowerDefault = 0xffffffff; // AUTO
+	pAd->CommonCfg.TxPreamble = Rt802_11PreambleAuto; // use Long preamble on TX by defaut
+	pAd->CommonCfg.bUseZeroToDisableFragment = FALSE;
+	pAd->CommonCfg.RtsThreshold = 2347;
+	pAd->CommonCfg.FragmentThreshold = 2346;
+	pAd->CommonCfg.UseBGProtection = 0;    // 0: AUTO
+	pAd->CommonCfg.bEnableTxBurst = TRUE; //0;
+	pAd->CommonCfg.PhyMode = 0xff;     // unknown
+	pAd->CommonCfg.BandState = UNKNOWN_BAND;
+	pAd->CommonCfg.RadarDetect.CSPeriod = 10;
+	pAd->CommonCfg.RadarDetect.CSCount = 0;
+	pAd->CommonCfg.RadarDetect.RDMode = RD_NORMAL_MODE;
+	pAd->CommonCfg.RadarDetect.ChMovingTime = 65;
+	pAd->CommonCfg.RadarDetect.LongPulseRadarTh = 3;
+	pAd->CommonCfg.bAPSDCapable = FALSE;
+	pAd->CommonCfg.bNeedSendTriggerFrame = FALSE;
+	pAd->CommonCfg.TriggerTimerCount = 0;
+	pAd->CommonCfg.bAPSDForcePowerSave = FALSE;
+	pAd->CommonCfg.bCountryFlag = FALSE;
+	pAd->CommonCfg.TxStream = 0;
+	pAd->CommonCfg.RxStream = 0;
+
+	NdisZeroMemory(&pAd->BeaconTxWI, sizeof(pAd->BeaconTxWI));
+
+#ifdef DOT11_N_SUPPORT
+	NdisZeroMemory(&pAd->CommonCfg.HtCapability, sizeof(pAd->CommonCfg.HtCapability));
+	pAd->HTCEnable = FALSE;
+	pAd->bBroadComHT = FALSE;
+	pAd->CommonCfg.bRdg = FALSE;
+
+#ifdef DOT11N_DRAFT3
+	pAd->CommonCfg.Dot11OBssScanPassiveDwell = dot11OBSSScanPassiveDwell;	// Unit : TU. 5~1000
+	pAd->CommonCfg.Dot11OBssScanActiveDwell = dot11OBSSScanActiveDwell;	// Unit : TU. 10~1000
+	pAd->CommonCfg.Dot11BssWidthTriggerScanInt = dot11BSSWidthTriggerScanInterval;	// Unit : Second
+	pAd->CommonCfg.Dot11OBssScanPassiveTotalPerChannel = dot11OBSSScanPassiveTotalPerChannel;	// Unit : TU. 200~10000
+	pAd->CommonCfg.Dot11OBssScanActiveTotalPerChannel = dot11OBSSScanActiveTotalPerChannel;	// Unit : TU. 20~10000
+	pAd->CommonCfg.Dot11BssWidthChanTranDelayFactor = dot11BSSWidthChannelTransactionDelayFactor;
+	pAd->CommonCfg.Dot11OBssScanActivityThre = dot11BSSScanActivityThreshold;	// Unit : percentage
+	pAd->CommonCfg.Dot11BssWidthChanTranDelay = (pAd->CommonCfg.Dot11BssWidthTriggerScanInt * pAd->CommonCfg.Dot11BssWidthChanTranDelayFactor);
+#endif  // DOT11N_DRAFT3 //
+
+	NdisZeroMemory(&pAd->CommonCfg.AddHTInfo, sizeof(pAd->CommonCfg.AddHTInfo));
+	pAd->CommonCfg.BACapability.field.MMPSmode = MMPS_ENABLE;
+	pAd->CommonCfg.BACapability.field.MpduDensity = 0;
+	pAd->CommonCfg.BACapability.field.Policy = IMMED_BA;
+	pAd->CommonCfg.BACapability.field.RxBAWinLimit = 64; //32;
+	pAd->CommonCfg.BACapability.field.TxBAWinLimit = 64; //32;
+	DBGPRINT(RT_DEBUG_TRACE, ("--> UserCfgInit. BACapability = 0x%x\n", pAd->CommonCfg.BACapability.word));
+
+	pAd->CommonCfg.BACapability.field.AutoBA = FALSE;
+	BATableInit(pAd, &pAd->BATable);
+
+	pAd->CommonCfg.bExtChannelSwitchAnnouncement = 1;
+	pAd->CommonCfg.bHTProtect = 1;
+	pAd->CommonCfg.bMIMOPSEnable = TRUE;
+	pAd->CommonCfg.bBADecline = FALSE;
+	pAd->CommonCfg.bDisableReordering = FALSE;
+
+	pAd->CommonCfg.TxBASize = 7;
+
+	pAd->CommonCfg.REGBACapability.word = pAd->CommonCfg.BACapability.word;
+#endif // DOT11_N_SUPPORT //
+
+	//pAd->CommonCfg.HTPhyMode.field.BW = BW_20;
+	//pAd->CommonCfg.HTPhyMode.field.MCS = MCS_AUTO;
+	//pAd->CommonCfg.HTPhyMode.field.ShortGI = GI_800;
+	//pAd->CommonCfg.HTPhyMode.field.STBC = STBC_NONE;
+	pAd->CommonCfg.TxRate = RATE_6;
+
+	pAd->CommonCfg.MlmeTransmit.field.MCS = MCS_RATE_6;
+	pAd->CommonCfg.MlmeTransmit.field.BW = BW_20;
+	pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_OFDM;
+
+	pAd->CommonCfg.BeaconPeriod = 100;     // in mSec
+
+	//
+	// part II. intialize STA specific configuration
+	//
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		RX_FILTER_SET_FLAG(pAd, fRX_FILTER_ACCEPT_DIRECT);
+		RX_FILTER_CLEAR_FLAG(pAd, fRX_FILTER_ACCEPT_MULTICAST);
+		RX_FILTER_SET_FLAG(pAd, fRX_FILTER_ACCEPT_BROADCAST);
+		RX_FILTER_SET_FLAG(pAd, fRX_FILTER_ACCEPT_ALL_MULTICAST);
+
+		pAd->StaCfg.Psm = PWR_ACTIVE;
+
+		pAd->StaCfg.OrigWepStatus = Ndis802_11EncryptionDisabled;
+		pAd->StaCfg.PairCipher = Ndis802_11EncryptionDisabled;
+		pAd->StaCfg.GroupCipher = Ndis802_11EncryptionDisabled;
+		pAd->StaCfg.bMixCipher = FALSE;
+		pAd->StaCfg.DefaultKeyId = 0;
+
+		// 802.1x port control
+		pAd->StaCfg.PrivacyFilter = Ndis802_11PrivFilter8021xWEP;
+		pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+		pAd->StaCfg.LastMicErrorTime = 0;
+		pAd->StaCfg.MicErrCnt        = 0;
+		pAd->StaCfg.bBlockAssoc      = FALSE;
+		pAd->StaCfg.WpaState         = SS_NOTUSE;
+
+		pAd->CommonCfg.NdisRadioStateOff = FALSE;		// New to support microsoft disable radio with OID command
+
+		pAd->StaCfg.RssiTrigger = 0;
+		NdisZeroMemory(&pAd->StaCfg.RssiSample, sizeof(RSSI_SAMPLE));
+		pAd->StaCfg.RssiTriggerMode = RSSI_TRIGGERED_UPON_BELOW_THRESHOLD;
+		pAd->StaCfg.AtimWin = 0;
+		pAd->StaCfg.DefaultListenCount = 3;//default listen count;
+		pAd->StaCfg.BssType = BSS_INFRA;  // BSS_INFRA or BSS_ADHOC or BSS_MONITOR
+		pAd->StaCfg.bScanReqIsFromWebUI = FALSE;
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_DOZE);
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_WAKEUP_NOW);
+
+		pAd->StaCfg.bAutoTxRateSwitch = TRUE;
+		pAd->StaCfg.DesiredTransmitSetting.field.MCS = MCS_AUTO;
+	}
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+	pAd->StaCfg.IEEE80211dClientMode = Rt802_11_D_None;
+#endif // EXT_BUILD_CHANNEL_LIST //
+#endif // CONFIG_STA_SUPPORT //
+
+	// global variables mXXXX used in MAC protocol state machines
+	OPSTATUS_SET_FLAG(pAd, fOP_STATUS_RECEIVE_DTIM);
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_ADHOC_ON);
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_INFRA_ON);
+
+	// PHY specification
+	pAd->CommonCfg.PhyMode = PHY_11BG_MIXED;		// default PHY mode
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED);  // CCK use LONG preamble
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// user desired power mode
+		pAd->StaCfg.WindowsPowerMode = Ndis802_11PowerModeCAM;
+		pAd->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeCAM;
+		pAd->StaCfg.bWindowsACCAMEnable = FALSE;
+
+#ifdef LEAP_SUPPORT
+		// CCX v1.0 releated init value
+		RTMPInitTimer(pAd, &pAd->StaCfg.LeapAuthTimer, GET_TIMER_FUNCTION(LeapAuthTimeout), pAd, FALSE);
+		pAd->StaCfg.LeapAuthMode = CISCO_AuthModeLEAPNone;
+		pAd->StaCfg.bCkipOn = FALSE;
+#endif // LEAP_SUPPORT //
+
+		RTMPInitTimer(pAd, &pAd->StaCfg.StaQuickResponeForRateUpTimer, GET_TIMER_FUNCTION(StaQuickResponeForRateUpExec), pAd, FALSE);
+		pAd->StaCfg.StaQuickResponeForRateUpTimerRunning = FALSE;
+
+		// Patch for Ndtest
+		pAd->StaCfg.ScanCnt = 0;
+
+		// CCX 2.0 control flag init
+		pAd->StaCfg.CCXEnable = FALSE;
+		pAd->StaCfg.CCXReqType = MSRN_TYPE_UNUSED;
+		pAd->StaCfg.CCXQosECWMin	= 4;
+		pAd->StaCfg.CCXQosECWMax	= 10;
+
+		pAd->StaCfg.bHwRadio  = TRUE; // Default Hardware Radio status is On
+		pAd->StaCfg.bSwRadio  = TRUE; // Default Software Radio status is On
+		pAd->StaCfg.bRadio    = TRUE; // bHwRadio && bSwRadio
+		pAd->StaCfg.bHardwareRadio = FALSE;		// Default is OFF
+		pAd->StaCfg.bShowHiddenSSID = FALSE;		// Default no show
+
+		// Nitro mode control
+		pAd->StaCfg.bAutoReconnect = TRUE;
+
+		// Save the init time as last scan time, the system should do scan after 2 seconds.
+		// This patch is for driver wake up from standby mode, system will do scan right away.
+		pAd->StaCfg.LastScanTime = 0;
+		NdisZeroMemory(pAd->nickname, IW_ESSID_MAX_SIZE+1);
+		sprintf(pAd->nickname, "%s", STA_NIC_DEVICE_NAME);
+		RTMPInitTimer(pAd, &pAd->StaCfg.WpaDisassocAndBlockAssocTimer, GET_TIMER_FUNCTION(WpaDisassocApAndBlockAssoc), pAd, FALSE);
+#ifdef WPA_SUPPLICANT_SUPPORT
+		pAd->StaCfg.IEEE8021X = FALSE;
+		pAd->StaCfg.IEEE8021x_required_keys = FALSE;
+		pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_DISABLE;
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+		pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_ENABLE;
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	// Default for extra information is not valid
+	pAd->ExtraInfo = EXTRA_INFO_CLEAR;
+
+	// Default Config change flag
+	pAd->bConfigChanged = FALSE;
+
+	//
+	// part III. AP configurations
+	//
+
+
+	//
+	// part IV. others
+	//
+	// dynamic BBP R66:sensibity tuning to overcome background noise
+	pAd->BbpTuning.bEnable                = TRUE;
+	pAd->BbpTuning.FalseCcaLowerThreshold = 100;
+	pAd->BbpTuning.FalseCcaUpperThreshold = 512;
+	pAd->BbpTuning.R66Delta               = 4;
+	pAd->Mlme.bEnableAutoAntennaCheck = TRUE;
+
+	//
+	// Also initial R66CurrentValue, RTUSBResumeMsduTransmission might use this value.
+	// if not initial this value, the default value will be 0.
+	//
+	pAd->BbpTuning.R66CurrentValue = 0x38;
+
+	pAd->Bbp94 = BBPR94_DEFAULT;
+	pAd->BbpForCCK = FALSE;
+
+	// initialize MAC table and allocate spin lock
+	NdisZeroMemory(&pAd->MacTab, sizeof(MAC_TABLE));
+	InitializeQueueHeader(&pAd->MacTab.McastPsQueue);
+	NdisAllocateSpinLock(&pAd->MacTabLock);
+
+#ifdef RALINK_ATE
+	NdisZeroMemory(&pAd->ate, sizeof(ATE_INFO));
+	pAd->ate.Mode = ATE_STOP;
+	pAd->ate.TxCount = 200;/* to exceed TX_RING_SIZE ... */
+	pAd->ate.TxLength = 1024;
+	pAd->ate.TxWI.ShortGI = 0;// LONG GI : 800 ns
+	pAd->ate.TxWI.PHYMODE = MODE_CCK;
+	pAd->ate.TxWI.MCS = 3;
+	pAd->ate.TxWI.BW = BW_20;
+	pAd->ate.Channel = 1;
+	pAd->ate.QID = QID_AC_BE;
+	pAd->ate.Addr1[0] = 0x00;
+	pAd->ate.Addr1[1] = 0x11;
+	pAd->ate.Addr1[2] = 0x22;
+	pAd->ate.Addr1[3] = 0xAA;
+	pAd->ate.Addr1[4] = 0xBB;
+	pAd->ate.Addr1[5] = 0xCC;
+	NdisMoveMemory(pAd->ate.Addr2, pAd->ate.Addr1, ETH_LENGTH_OF_ADDRESS);
+	NdisMoveMemory(pAd->ate.Addr3, pAd->ate.Addr1, ETH_LENGTH_OF_ADDRESS);
+	pAd->ate.bRxFer = 0;
+	pAd->ate.bQATxStart = FALSE;
+	pAd->ate.bQARxStart = FALSE;
+#ifdef RT2860
+	pAd->ate.bFWLoading = FALSE;
+#endif // RT2860 //
+#ifdef RALINK_28xx_QA
+	//pAd->ate.Repeat = 0;
+	pAd->ate.TxStatus = 0;
+	pAd->ate.AtePid = THREAD_PID_INIT_VALUE;
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+
+
+	pAd->CommonCfg.bWiFiTest = FALSE;
+#ifdef RT2860
+    pAd->bPCIclkOff = FALSE;
+#endif // RT2860 //
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- UserCfgInit\n"));
+}
+
+// IRQL = PASSIVE_LEVEL
+UCHAR BtoH(char ch)
+{
+	if (ch >= '0' && ch <= '9') return (ch - '0');        // Handle numerals
+	if (ch >= 'A' && ch <= 'F') return (ch - 'A' + 0xA);  // Handle capitol hex digits
+	if (ch >= 'a' && ch <= 'f') return (ch - 'a' + 0xA);  // Handle small hex digits
+	return(255);
+}
+
+//
+//  FUNCTION: AtoH(char *, UCHAR *, int)
+//
+//  PURPOSE:  Converts ascii string to network order hex
+//
+//  PARAMETERS:
+//    src    - pointer to input ascii string
+//    dest   - pointer to output hex
+//    destlen - size of dest
+//
+//  COMMENTS:
+//
+//    2 ascii bytes make a hex byte so must put 1st ascii byte of pair
+//    into upper nibble and 2nd ascii byte of pair into lower nibble.
+//
+// IRQL = PASSIVE_LEVEL
+
+void AtoH(char * src, UCHAR * dest, int destlen)
+{
+	char * srcptr;
+	PUCHAR destTemp;
+
+	srcptr = src;
+	destTemp = (PUCHAR) dest;
+
+	while(destlen--)
+	{
+		*destTemp = BtoH(*srcptr++) << 4;    // Put 1st ascii byte in upper nibble.
+		*destTemp += BtoH(*srcptr++);      // Add 2nd ascii byte to above.
+		destTemp++;
+	}
+}
+
+VOID	RTMPPatchMacBbpBug(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	ULONG	Index;
+
+	// Initialize BBP register to default value
+	for (Index = 0; Index < NUM_BBP_REG_PARMS; Index++)
+	{
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBPRegTable[Index].Register, (UCHAR)BBPRegTable[Index].Value);
+	}
+
+	// Initialize RF register to default value
+	AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+	AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+
+	// Re-init BBP register from EEPROM value
+	NICInitAsicFromEEPROM(pAd);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Init timer objects
+
+	Arguments:
+		pAd			Pointer to our adapter
+		pTimer				Timer structure
+		pTimerFunc			Function to execute when timer expired
+		Repeat				Ture for period timer
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPInitTimer(
+	IN	PRTMP_ADAPTER			pAd,
+	IN	PRALINK_TIMER_STRUCT	pTimer,
+	IN	PVOID					pTimerFunc,
+	IN	PVOID					pData,
+	IN	BOOLEAN					Repeat)
+{
+	//
+	// Set Valid to TRUE for later used.
+	// It will crash if we cancel a timer or set a timer
+	// that we haven't initialize before.
+	//
+	pTimer->Valid      = TRUE;
+
+	pTimer->PeriodicType = Repeat;
+	pTimer->State      = FALSE;
+	pTimer->cookie = (ULONG) pData;
+
+
+	RTMP_OS_Init_Timer(pAd,	&pTimer->TimerObj,	pTimerFunc, (PVOID) pTimer);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Init timer objects
+
+	Arguments:
+		pTimer				Timer structure
+		Value				Timer value in milliseconds
+
+	Return Value:
+		None
+
+	Note:
+		To use this routine, must call RTMPInitTimer before.
+
+	========================================================================
+*/
+VOID	RTMPSetTimer(
+	IN	PRALINK_TIMER_STRUCT	pTimer,
+	IN	ULONG					Value)
+{
+	if (pTimer->Valid)
+	{
+		pTimer->TimerValue = Value;
+		pTimer->State      = FALSE;
+		if (pTimer->PeriodicType == TRUE)
+		{
+			pTimer->Repeat = TRUE;
+			RTMP_SetPeriodicTimer(&pTimer->TimerObj, Value);
+		}
+		else
+		{
+			pTimer->Repeat = FALSE;
+			RTMP_OS_Add_Timer(&pTimer->TimerObj, Value);
+		}
+	}
+	else
+	{
+		DBGPRINT_ERR(("RTMPSetTimer failed, Timer hasn't been initialize!\n"));
+	}
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Init timer objects
+
+	Arguments:
+		pTimer				Timer structure
+		Value				Timer value in milliseconds
+
+	Return Value:
+		None
+
+	Note:
+		To use this routine, must call RTMPInitTimer before.
+
+	========================================================================
+*/
+VOID	RTMPModTimer(
+	IN	PRALINK_TIMER_STRUCT	pTimer,
+	IN	ULONG					Value)
+{
+	BOOLEAN	Cancel;
+
+	if (pTimer->Valid)
+	{
+		pTimer->TimerValue = Value;
+		pTimer->State      = FALSE;
+		if (pTimer->PeriodicType == TRUE)
+		{
+			RTMPCancelTimer(pTimer, &Cancel);
+			RTMPSetTimer(pTimer, Value);
+		}
+		else
+		{
+			RTMP_OS_Mod_Timer(&pTimer->TimerObj, Value);
+		}
+	}
+	else
+	{
+		DBGPRINT_ERR(("RTMPModTimer failed, Timer hasn't been initialize!\n"));
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Cancel timer objects
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+		1.) To use this routine, must call RTMPInitTimer before.
+		2.) Reset NIC to initial state AS IS system boot up time.
+
+	========================================================================
+*/
+VOID	RTMPCancelTimer(
+	IN	PRALINK_TIMER_STRUCT	pTimer,
+	OUT	BOOLEAN					*pCancelled)
+{
+	if (pTimer->Valid)
+	{
+		if (pTimer->State == FALSE)
+			pTimer->Repeat = FALSE;
+			RTMP_OS_Del_Timer(&pTimer->TimerObj, pCancelled);
+
+		if (*pCancelled == TRUE)
+			pTimer->State = TRUE;
+
+	}
+	else
+	{
+		//
+		// NdisMCancelTimer just canced the timer and not mean release the timer.
+		// And don't set the "Valid" to False. So that we can use this timer again.
+		//
+		DBGPRINT_ERR(("RTMPCancelTimer failed, Timer hasn't been initialize!\n"));
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Set LED Status
+
+	Arguments:
+		pAd						Pointer to our adapter
+		Status					LED Status
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID RTMPSetLED(
+	IN PRTMP_ADAPTER 	pAd,
+	IN UCHAR			Status)
+{
+	//ULONG			data;
+	UCHAR			HighByte = 0;
+	UCHAR			LowByte;
+
+// In ATE mode of RT2860 AP/STA, we have erased 8051 firmware.
+// So LED mode is not supported when ATE is running.
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+	LowByte = pAd->LedCntl.field.LedMode&0x7f;
+	switch (Status)
+	{
+		case LED_LINK_DOWN:
+			HighByte = 0x20;
+			AsicSendCommandToMcu(pAd, 0x50, 0xff, LowByte, HighByte);
+			pAd->LedIndicatorStregth = 0;
+			break;
+		case LED_LINK_UP:
+			if (pAd->CommonCfg.Channel > 14)
+				HighByte = 0xa0;
+			else
+				HighByte = 0x60;
+			AsicSendCommandToMcu(pAd, 0x50, 0xff, LowByte, HighByte);
+			break;
+		case LED_RADIO_ON:
+			HighByte = 0x20;
+			AsicSendCommandToMcu(pAd, 0x50, 0xff, LowByte, HighByte);
+			break;
+		case LED_HALT:
+			LowByte = 0; // Driver sets MAC register and MAC controls LED
+		case LED_RADIO_OFF:
+			HighByte = 0;
+			AsicSendCommandToMcu(pAd, 0x50, 0xff, LowByte, HighByte);
+			break;
+        case LED_WPS:
+			HighByte = 0x10;
+			AsicSendCommandToMcu(pAd, 0x50, 0xff, LowByte, HighByte);
+			break;
+		case LED_ON_SITE_SURVEY:
+			HighByte = 0x08;
+			AsicSendCommandToMcu(pAd, 0x50, 0xff, LowByte, HighByte);
+			break;
+		case LED_POWER_UP:
+			HighByte = 0x04;
+			AsicSendCommandToMcu(pAd, 0x50, 0xff, LowByte, HighByte);
+			break;
+		default:
+			DBGPRINT(RT_DEBUG_WARN, ("RTMPSetLED::Unknown Status %d\n", Status));
+			break;
+	}
+
+    //
+	// Keep LED status for LED SiteSurvey mode.
+	// After SiteSurvey, we will set the LED mode to previous status.
+	//
+	if ((Status != LED_ON_SITE_SURVEY) && (Status != LED_POWER_UP))
+		pAd->LedStatus = Status;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPSetLED::Mode=%d,HighByte=0x%02x,LowByte=0x%02x\n", pAd->LedCntl.field.LedMode, HighByte, LowByte));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Set LED Signal Stregth
+
+	Arguments:
+		pAd						Pointer to our adapter
+		Dbm						Signal Stregth
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+		Can be run on any IRQL level.
+
+		According to Microsoft Zero Config Wireless Signal Stregth definition as belows.
+		<= -90  No Signal
+		<= -81  Very Low
+		<= -71  Low
+		<= -67  Good
+		<= -57  Very Good
+		 > -57  Excellent
+	========================================================================
+*/
+VOID RTMPSetSignalLED(
+	IN PRTMP_ADAPTER 	pAd,
+	IN NDIS_802_11_RSSI Dbm)
+{
+	UCHAR		nLed = 0;
+
+	//
+	// if not Signal Stregth, then do nothing.
+	//
+	if (pAd->LedCntl.field.LedMode != LED_MODE_SIGNAL_STREGTH)
+	{
+		return;
+	}
+
+	if (Dbm <= -90)
+		nLed = 0;
+	else if (Dbm <= -81)
+		nLed = 1;
+	else if (Dbm <= -71)
+		nLed = 3;
+	else if (Dbm <= -67)
+		nLed = 7;
+	else if (Dbm <= -57)
+		nLed = 15;
+	else
+		nLed = 31;
+
+	//
+	// Update Signal Stregth to firmware if changed.
+	//
+	if (pAd->LedIndicatorStregth != nLed)
+	{
+		AsicSendCommandToMcu(pAd, 0x51, 0xff, nLed, pAd->LedCntl.field.Polarity);
+		pAd->LedIndicatorStregth = nLed;
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Enable RX
+
+	Arguments:
+		pAd						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL <= DISPATCH_LEVEL
+
+	Note:
+		Before Enable RX, make sure you have enabled Interrupt.
+	========================================================================
+*/
+VOID RTMPEnableRxTx(
+	IN PRTMP_ADAPTER	pAd)
+{
+	DBGPRINT(RT_DEBUG_TRACE, ("==> RTMPEnableRxTx\n"));
+
+	// Enable Rx DMA.
+	RT28XXDMAEnable(pAd);
+
+	// enable RX of MAC block
+	if (pAd->OpMode == OPMODE_AP)
+	{
+		UINT32 rx_filter_flag = APNORMAL;
+
+
+		RTMP_IO_WRITE32(pAd, RX_FILTR_CFG, rx_filter_flag);     // enable RX of DMA block
+	}
+	else
+	{
+		RTMP_IO_WRITE32(pAd, RX_FILTR_CFG, STANORMAL);     // Staion not drop control frame will fail WiFi Certification.
+	}
+
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0xc);
+	DBGPRINT(RT_DEBUG_TRACE, ("<== RTMPEnableRxTx\n"));
+}
+
+
diff --git a/drivers/staging/rt2860/common/rtmp_tkip.c b/drivers/staging/rt2860/common/rtmp_tkip.c
new file mode 100644
index 00000000000..a87ea3a5d3e
--- /dev/null
+++ b/drivers/staging/rt2860/common/rtmp_tkip.c
@@ -0,0 +1,1607 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rtmp_tkip.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Paul Wu		02-25-02		Initial
+*/
+
+#include "../rt_config.h"
+
+// Rotation functions on 32 bit values
+#define ROL32( A, n ) \
+	( ((A) << (n)) | ( ((A)>>(32-(n))) & ( (1UL << (n)) - 1 ) ) )
+#define ROR32( A, n ) ROL32( (A), 32-(n) )
+
+UINT Tkip_Sbox_Lower[256] =
+{
+	0xA5,0x84,0x99,0x8D,0x0D,0xBD,0xB1,0x54,
+	0x50,0x03,0xA9,0x7D,0x19,0x62,0xE6,0x9A,
+	0x45,0x9D,0x40,0x87,0x15,0xEB,0xC9,0x0B,
+	0xEC,0x67,0xFD,0xEA,0xBF,0xF7,0x96,0x5B,
+	0xC2,0x1C,0xAE,0x6A,0x5A,0x41,0x02,0x4F,
+	0x5C,0xF4,0x34,0x08,0x93,0x73,0x53,0x3F,
+	0x0C,0x52,0x65,0x5E,0x28,0xA1,0x0F,0xB5,
+	0x09,0x36,0x9B,0x3D,0x26,0x69,0xCD,0x9F,
+	0x1B,0x9E,0x74,0x2E,0x2D,0xB2,0xEE,0xFB,
+	0xF6,0x4D,0x61,0xCE,0x7B,0x3E,0x71,0x97,
+	0xF5,0x68,0x00,0x2C,0x60,0x1F,0xC8,0xED,
+	0xBE,0x46,0xD9,0x4B,0xDE,0xD4,0xE8,0x4A,
+	0x6B,0x2A,0xE5,0x16,0xC5,0xD7,0x55,0x94,
+	0xCF,0x10,0x06,0x81,0xF0,0x44,0xBA,0xE3,
+	0xF3,0xFE,0xC0,0x8A,0xAD,0xBC,0x48,0x04,
+	0xDF,0xC1,0x75,0x63,0x30,0x1A,0x0E,0x6D,
+	0x4C,0x14,0x35,0x2F,0xE1,0xA2,0xCC,0x39,
+	0x57,0xF2,0x82,0x47,0xAC,0xE7,0x2B,0x95,
+	0xA0,0x98,0xD1,0x7F,0x66,0x7E,0xAB,0x83,
+	0xCA,0x29,0xD3,0x3C,0x79,0xE2,0x1D,0x76,
+	0x3B,0x56,0x4E,0x1E,0xDB,0x0A,0x6C,0xE4,
+	0x5D,0x6E,0xEF,0xA6,0xA8,0xA4,0x37,0x8B,
+	0x32,0x43,0x59,0xB7,0x8C,0x64,0xD2,0xE0,
+	0xB4,0xFA,0x07,0x25,0xAF,0x8E,0xE9,0x18,
+	0xD5,0x88,0x6F,0x72,0x24,0xF1,0xC7,0x51,
+	0x23,0x7C,0x9C,0x21,0xDD,0xDC,0x86,0x85,
+	0x90,0x42,0xC4,0xAA,0xD8,0x05,0x01,0x12,
+	0xA3,0x5F,0xF9,0xD0,0x91,0x58,0x27,0xB9,
+	0x38,0x13,0xB3,0x33,0xBB,0x70,0x89,0xA7,
+	0xB6,0x22,0x92,0x20,0x49,0xFF,0x78,0x7A,
+	0x8F,0xF8,0x80,0x17,0xDA,0x31,0xC6,0xB8,
+	0xC3,0xB0,0x77,0x11,0xCB,0xFC,0xD6,0x3A
+};
+
+UINT Tkip_Sbox_Upper[256] =
+{
+	0xC6,0xF8,0xEE,0xF6,0xFF,0xD6,0xDE,0x91,
+	0x60,0x02,0xCE,0x56,0xE7,0xB5,0x4D,0xEC,
+	0x8F,0x1F,0x89,0xFA,0xEF,0xB2,0x8E,0xFB,
+	0x41,0xB3,0x5F,0x45,0x23,0x53,0xE4,0x9B,
+	0x75,0xE1,0x3D,0x4C,0x6C,0x7E,0xF5,0x83,
+	0x68,0x51,0xD1,0xF9,0xE2,0xAB,0x62,0x2A,
+	0x08,0x95,0x46,0x9D,0x30,0x37,0x0A,0x2F,
+	0x0E,0x24,0x1B,0xDF,0xCD,0x4E,0x7F,0xEA,
+	0x12,0x1D,0x58,0x34,0x36,0xDC,0xB4,0x5B,
+	0xA4,0x76,0xB7,0x7D,0x52,0xDD,0x5E,0x13,
+	0xA6,0xB9,0x00,0xC1,0x40,0xE3,0x79,0xB6,
+	0xD4,0x8D,0x67,0x72,0x94,0x98,0xB0,0x85,
+	0xBB,0xC5,0x4F,0xED,0x86,0x9A,0x66,0x11,
+	0x8A,0xE9,0x04,0xFE,0xA0,0x78,0x25,0x4B,
+	0xA2,0x5D,0x80,0x05,0x3F,0x21,0x70,0xF1,
+	0x63,0x77,0xAF,0x42,0x20,0xE5,0xFD,0xBF,
+	0x81,0x18,0x26,0xC3,0xBE,0x35,0x88,0x2E,
+	0x93,0x55,0xFC,0x7A,0xC8,0xBA,0x32,0xE6,
+	0xC0,0x19,0x9E,0xA3,0x44,0x54,0x3B,0x0B,
+	0x8C,0xC7,0x6B,0x28,0xA7,0xBC,0x16,0xAD,
+	0xDB,0x64,0x74,0x14,0x92,0x0C,0x48,0xB8,
+	0x9F,0xBD,0x43,0xC4,0x39,0x31,0xD3,0xF2,
+	0xD5,0x8B,0x6E,0xDA,0x01,0xB1,0x9C,0x49,
+	0xD8,0xAC,0xF3,0xCF,0xCA,0xF4,0x47,0x10,
+	0x6F,0xF0,0x4A,0x5C,0x38,0x57,0x73,0x97,
+	0xCB,0xA1,0xE8,0x3E,0x96,0x61,0x0D,0x0F,
+	0xE0,0x7C,0x71,0xCC,0x90,0x06,0xF7,0x1C,
+	0xC2,0x6A,0xAE,0x69,0x17,0x99,0x3A,0x27,
+	0xD9,0xEB,0x2B,0x22,0xD2,0xA9,0x07,0x33,
+	0x2D,0x3C,0x15,0xC9,0x87,0xAA,0x50,0xA5,
+	0x03,0x59,0x09,0x1A,0x65,0xD7,0x84,0xD0,
+	0x82,0x29,0x5A,0x1E,0x7B,0xA8,0x6D,0x2C
+};
+
+/*****************************/
+/******** SBOX Table *********/
+/*****************************/
+
+UCHAR SboxTable[256] =
+{
+	0x63, 0x7c, 0x77, 0x7b, 0xf2, 0x6b, 0x6f, 0xc5,
+	0x30, 0x01, 0x67, 0x2b, 0xfe, 0xd7, 0xab, 0x76,
+	0xca, 0x82, 0xc9, 0x7d, 0xfa, 0x59, 0x47, 0xf0,
+	0xad, 0xd4, 0xa2, 0xaf, 0x9c, 0xa4, 0x72, 0xc0,
+	0xb7, 0xfd, 0x93, 0x26, 0x36, 0x3f, 0xf7, 0xcc,
+	0x34, 0xa5, 0xe5, 0xf1, 0x71, 0xd8, 0x31, 0x15,
+	0x04, 0xc7, 0x23, 0xc3, 0x18, 0x96, 0x05, 0x9a,
+	0x07, 0x12, 0x80, 0xe2, 0xeb, 0x27, 0xb2, 0x75,
+	0x09, 0x83, 0x2c, 0x1a, 0x1b, 0x6e, 0x5a, 0xa0,
+	0x52, 0x3b, 0xd6, 0xb3, 0x29, 0xe3, 0x2f, 0x84,
+	0x53, 0xd1, 0x00, 0xed, 0x20, 0xfc, 0xb1, 0x5b,
+	0x6a, 0xcb, 0xbe, 0x39, 0x4a, 0x4c, 0x58, 0xcf,
+	0xd0, 0xef, 0xaa, 0xfb, 0x43, 0x4d, 0x33, 0x85,
+	0x45, 0xf9, 0x02, 0x7f, 0x50, 0x3c, 0x9f, 0xa8,
+	0x51, 0xa3, 0x40, 0x8f, 0x92, 0x9d, 0x38, 0xf5,
+	0xbc, 0xb6, 0xda, 0x21, 0x10, 0xff, 0xf3, 0xd2,
+	0xcd, 0x0c, 0x13, 0xec, 0x5f, 0x97, 0x44, 0x17,
+	0xc4, 0xa7, 0x7e, 0x3d, 0x64, 0x5d, 0x19, 0x73,
+	0x60, 0x81, 0x4f, 0xdc, 0x22, 0x2a, 0x90, 0x88,
+	0x46, 0xee, 0xb8, 0x14, 0xde, 0x5e, 0x0b, 0xdb,
+	0xe0, 0x32, 0x3a, 0x0a, 0x49, 0x06, 0x24, 0x5c,
+	0xc2, 0xd3, 0xac, 0x62, 0x91, 0x95, 0xe4, 0x79,
+	0xe7, 0xc8, 0x37, 0x6d, 0x8d, 0xd5, 0x4e, 0xa9,
+	0x6c, 0x56, 0xf4, 0xea, 0x65, 0x7a, 0xae, 0x08,
+	0xba, 0x78, 0x25, 0x2e, 0x1c, 0xa6, 0xb4, 0xc6,
+	0xe8, 0xdd, 0x74, 0x1f, 0x4b, 0xbd, 0x8b, 0x8a,
+	0x70, 0x3e, 0xb5, 0x66, 0x48, 0x03, 0xf6, 0x0e,
+	0x61, 0x35, 0x57, 0xb9, 0x86, 0xc1, 0x1d, 0x9e,
+	0xe1, 0xf8, 0x98, 0x11, 0x69, 0xd9, 0x8e, 0x94,
+	0x9b, 0x1e, 0x87, 0xe9, 0xce, 0x55, 0x28, 0xdf,
+	0x8c, 0xa1, 0x89, 0x0d, 0xbf, 0xe6, 0x42, 0x68,
+	0x41, 0x99, 0x2d, 0x0f, 0xb0, 0x54, 0xbb, 0x16
+};
+
+VOID xor_32(
+	IN  PUCHAR              a,
+	IN  PUCHAR              b,
+	OUT PUCHAR              out);
+
+VOID xor_128(
+	IN  PUCHAR              a,
+	IN  PUCHAR              b,
+	OUT PUCHAR              out);
+
+VOID next_key(
+	IN  PUCHAR              key,
+	IN  INT                 round);
+
+VOID byte_sub(
+	IN  PUCHAR              in,
+	OUT PUCHAR              out);
+
+VOID shift_row(
+	IN  PUCHAR              in,
+	OUT PUCHAR              out);
+
+VOID mix_column(
+	IN  PUCHAR              in,
+	OUT PUCHAR              out);
+
+UCHAR RTMPCkipSbox(
+	IN  UCHAR               a);
+//
+// Expanded IV for TKIP function.
+//
+typedef	struct	PACKED _IV_CONTROL_
+{
+	union PACKED
+	{
+		struct PACKED
+		{
+			UCHAR		rc0;
+			UCHAR		rc1;
+			UCHAR		rc2;
+
+			union PACKED
+			{
+				struct PACKED
+				{
+#ifdef RT_BIG_ENDIAN
+					UCHAR	KeyID:2;
+					UCHAR	ExtIV:1;
+					UCHAR	Rsvd:5;
+#else
+					UCHAR	Rsvd:5;
+					UCHAR	ExtIV:1;
+					UCHAR	KeyID:2;
+#endif
+				}	field;
+				UCHAR		Byte;
+			}	CONTROL;
+		}	field;
+
+		ULONG	word;
+	}	IV16;
+
+	ULONG	IV32;
+}	TKIP_IV, *PTKIP_IV;
+
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Convert from UCHAR[] to ULONG in a portable way
+
+	Arguments:
+      pMICKey		pointer to MIC Key
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+ULONG	RTMPTkipGetUInt32(
+	IN	PUCHAR	pMICKey)
+{
+	ULONG	res = 0;
+	INT		i;
+
+	for (i = 0; i < 4; i++)
+	{
+		res |= (*pMICKey++) << (8 * i);
+	}
+
+	return res;
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Convert from ULONG to UCHAR[] in a portable way
+
+	Arguments:
+      pDst			pointer to destination for convert ULONG to UCHAR[]
+      val			the value for convert
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPTkipPutUInt32(
+	IN OUT	PUCHAR		pDst,
+	IN		ULONG		val)
+{
+	INT i;
+
+	for(i = 0; i < 4; i++)
+	{
+		*pDst++ = (UCHAR) (val & 0xff);
+		val >>= 8;
+	}
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Set the MIC Key.
+
+	Arguments:
+      pAd		Pointer to our adapter
+      pMICKey		pointer to MIC Key
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID RTMPTkipSetMICKey(
+	IN	PTKIP_KEY_INFO	pTkip,
+	IN	PUCHAR			pMICKey)
+{
+	// Set the key
+	pTkip->K0 = RTMPTkipGetUInt32(pMICKey);
+	pTkip->K1 = RTMPTkipGetUInt32(pMICKey + 4);
+	// and reset the message
+	pTkip->L = pTkip->K0;
+	pTkip->R = pTkip->K1;
+	pTkip->nBytesInM = 0;
+	pTkip->M = 0;
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Calculate the MIC Value.
+
+	Arguments:
+      pAd		Pointer to our adapter
+      uChar			Append this uChar
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPTkipAppendByte(
+	IN	PTKIP_KEY_INFO	pTkip,
+	IN	UCHAR 			uChar)
+{
+	// Append the byte to our word-sized buffer
+	pTkip->M |= (uChar << (8* pTkip->nBytesInM));
+	pTkip->nBytesInM++;
+	// Process the word if it is full.
+	if( pTkip->nBytesInM >= 4 )
+	{
+		pTkip->L ^= pTkip->M;
+		pTkip->R ^= ROL32( pTkip->L, 17 );
+		pTkip->L += pTkip->R;
+		pTkip->R ^= ((pTkip->L & 0xff00ff00) >> 8) | ((pTkip->L & 0x00ff00ff) << 8);
+		pTkip->L += pTkip->R;
+		pTkip->R ^= ROL32( pTkip->L, 3 );
+		pTkip->L += pTkip->R;
+		pTkip->R ^= ROR32( pTkip->L, 2 );
+		pTkip->L += pTkip->R;
+		// Clear the buffer
+		pTkip->M = 0;
+		pTkip->nBytesInM = 0;
+	}
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Calculate the MIC Value.
+
+	Arguments:
+      pAd		Pointer to our adapter
+      pSrc			Pointer to source data for Calculate MIC Value
+      Len			Indicate the length of the source data
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPTkipAppend(
+	IN	PTKIP_KEY_INFO	pTkip,
+	IN	PUCHAR			pSrc,
+	IN	UINT			nBytes)
+{
+	// This is simple
+	while(nBytes > 0)
+	{
+		RTMPTkipAppendByte(pTkip, *pSrc++);
+		nBytes--;
+	}
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Get the MIC Value.
+
+	Arguments:
+      pAd		Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+		the MIC Value is store in pAd->PrivateInfo.MIC
+	========================================================================
+*/
+VOID	RTMPTkipGetMIC(
+	IN	PTKIP_KEY_INFO	pTkip)
+{
+	// Append the minimum padding
+	RTMPTkipAppendByte(pTkip, 0x5a );
+	RTMPTkipAppendByte(pTkip, 0 );
+	RTMPTkipAppendByte(pTkip, 0 );
+	RTMPTkipAppendByte(pTkip, 0 );
+	RTMPTkipAppendByte(pTkip, 0 );
+	// and then zeroes until the length is a multiple of 4
+	while( pTkip->nBytesInM != 0 )
+	{
+		RTMPTkipAppendByte(pTkip, 0 );
+	}
+	// The appendByte function has already computed the result.
+	RTMPTkipPutUInt32(pTkip->MIC, pTkip->L);
+	RTMPTkipPutUInt32(pTkip->MIC + 4, pTkip->R);
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Init Tkip function.
+
+	Arguments:
+      pAd		Pointer to our adapter
+		pTKey       Pointer to the Temporal Key (TK), TK shall be 128bits.
+		KeyId		TK Key ID
+		pTA			Pointer to transmitter address
+		pMICKey		pointer to MIC Key
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPInitTkipEngine(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pKey,
+	IN	UCHAR			KeyId,
+	IN	PUCHAR			pTA,
+	IN	PUCHAR			pMICKey,
+	IN	PUCHAR			pTSC,
+	OUT	PULONG			pIV16,
+	OUT	PULONG			pIV32)
+{
+	TKIP_IV	tkipIv;
+
+	// Prepare 8 bytes TKIP encapsulation for MPDU
+	NdisZeroMemory(&tkipIv, sizeof(TKIP_IV));
+	tkipIv.IV16.field.rc0 = *(pTSC + 1);
+	tkipIv.IV16.field.rc1 = (tkipIv.IV16.field.rc0 | 0x20) & 0x7f;
+	tkipIv.IV16.field.rc2 = *pTSC;
+	tkipIv.IV16.field.CONTROL.field.ExtIV = 1;  // 0: non-extended IV, 1: an extended IV
+	tkipIv.IV16.field.CONTROL.field.KeyID = KeyId;
+	NdisMoveMemory(&tkipIv.IV32, (pTSC + 2), 4);   // Copy IV
+
+	*pIV16 = tkipIv.IV16.word;
+	*pIV32 = tkipIv.IV32;
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Init MIC Value calculation function which include set MIC key &
+		calculate first 16 bytes (DA + SA + priority +  0)
+
+	Arguments:
+      pAd		Pointer to our adapter
+		pTKey       Pointer to the Temporal Key (TK), TK shall be 128bits.
+		pDA			Pointer to DA address
+		pSA			Pointer to SA address
+		pMICKey		pointer to MIC Key
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPInitMICEngine(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pKey,
+	IN	PUCHAR			pDA,
+	IN	PUCHAR			pSA,
+	IN  UCHAR           UserPriority,
+	IN	PUCHAR			pMICKey)
+{
+	ULONG Priority = UserPriority;
+
+	// Init MIC value calculation
+	RTMPTkipSetMICKey(&pAd->PrivateInfo.Tx, pMICKey);
+	// DA
+	RTMPTkipAppend(&pAd->PrivateInfo.Tx, pDA, MAC_ADDR_LEN);
+	// SA
+	RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSA, MAC_ADDR_LEN);
+	// Priority + 3 bytes of 0
+	RTMPTkipAppend(&pAd->PrivateInfo.Tx, (PUCHAR)&Priority, 4);
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Compare MIC value of received MSDU
+
+	Arguments:
+		pAd	Pointer to our adapter
+		pSrc        Pointer to the received Plain text data
+		pDA			Pointer to DA address
+		pSA			Pointer to SA address
+		pMICKey		pointer to MIC Key
+		Len         the length of the received plain text data exclude MIC value
+
+	Return Value:
+		TRUE        MIC value matched
+		FALSE       MIC value mismatched
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+BOOLEAN	RTMPTkipCompareMICValue(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pSrc,
+	IN	PUCHAR			pDA,
+	IN	PUCHAR			pSA,
+	IN	PUCHAR			pMICKey,
+	IN	UCHAR			UserPriority,
+	IN	UINT			Len)
+{
+	UCHAR	OldMic[8];
+	ULONG	Priority = UserPriority;
+
+	// Init MIC value calculation
+	RTMPTkipSetMICKey(&pAd->PrivateInfo.Rx, pMICKey);
+	// DA
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, pDA, MAC_ADDR_LEN);
+	// SA
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSA, MAC_ADDR_LEN);
+	// Priority + 3 bytes of 0
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, (PUCHAR)&Priority, 4);
+
+	// Calculate MIC value from plain text data
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSrc, Len);
+
+	// Get MIC valude from received frame
+	NdisMoveMemory(OldMic, pSrc + Len, 8);
+
+	// Get MIC value from decrypted plain data
+	RTMPTkipGetMIC(&pAd->PrivateInfo.Rx);
+
+	// Move MIC value from MSDU, this steps should move to data path.
+	// Since the MIC value might cross MPDUs.
+	if(!NdisEqualMemory(pAd->PrivateInfo.Rx.MIC, OldMic, 8))
+	{
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("RTMPTkipCompareMICValue(): TKIP MIC Error !\n"));  //MIC error.
+
+
+		return (FALSE);
+	}
+	return (TRUE);
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Compare MIC value of received MSDU
+
+	Arguments:
+		pAd	Pointer to our adapter
+		pLLC		LLC header
+		pSrc        Pointer to the received Plain text data
+		pDA			Pointer to DA address
+		pSA			Pointer to SA address
+		pMICKey		pointer to MIC Key
+		Len         the length of the received plain text data exclude MIC value
+
+	Return Value:
+		TRUE        MIC value matched
+		FALSE       MIC value mismatched
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+BOOLEAN	RTMPTkipCompareMICValueWithLLC(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pLLC,
+	IN	PUCHAR			pSrc,
+	IN	PUCHAR			pDA,
+	IN	PUCHAR			pSA,
+	IN	PUCHAR			pMICKey,
+	IN	UINT			Len)
+{
+	UCHAR	OldMic[8];
+	ULONG	Priority = 0;
+
+	// Init MIC value calculation
+	RTMPTkipSetMICKey(&pAd->PrivateInfo.Rx, pMICKey);
+	// DA
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, pDA, MAC_ADDR_LEN);
+	// SA
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSA, MAC_ADDR_LEN);
+	// Priority + 3 bytes of 0
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, (PUCHAR)&Priority, 4);
+
+	// Start with LLC header
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, pLLC, 8);
+
+	// Calculate MIC value from plain text data
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSrc, Len);
+
+	// Get MIC valude from received frame
+	NdisMoveMemory(OldMic, pSrc + Len, 8);
+
+	// Get MIC value from decrypted plain data
+	RTMPTkipGetMIC(&pAd->PrivateInfo.Rx);
+
+	// Move MIC value from MSDU, this steps should move to data path.
+	// Since the MIC value might cross MPDUs.
+	if(!NdisEqualMemory(pAd->PrivateInfo.Rx.MIC, OldMic, 8))
+	{
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("RTMPTkipCompareMICValueWithLLC(): TKIP MIC Error !\n"));  //MIC error.
+
+
+		return (FALSE);
+	}
+	return (TRUE);
+}
+/*
+	========================================================================
+
+	Routine	Description:
+		Copy frame from waiting queue into relative ring buffer and set
+	appropriate ASIC register to kick hardware transmit function
+
+	Arguments:
+		pAd		Pointer	to our adapter
+		PNDIS_PACKET	Pointer to Ndis Packet for MIC calculation
+		pEncap			Pointer to LLC encap data
+		LenEncap		Total encap length, might be 0 which indicates no encap
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPCalculateMICValue(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	PUCHAR			pEncap,
+	IN	PCIPHER_KEY		pKey,
+	IN	UCHAR			apidx)
+{
+	PACKET_INFO		PacketInfo;
+	PUCHAR			pSrcBufVA;
+	UINT			SrcBufLen;
+	PUCHAR			pSrc;
+    UCHAR           UserPriority;
+	UCHAR			vlan_offset = 0;
+
+	RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pSrcBufVA, &SrcBufLen);
+
+	UserPriority = RTMP_GET_PACKET_UP(pPacket);
+	pSrc = pSrcBufVA;
+
+	// determine if this is a vlan packet
+	if (((*(pSrc + 12) << 8) + *(pSrc + 13)) == 0x8100)
+		vlan_offset = 4;
+
+	{
+		RTMPInitMICEngine(
+			pAd,
+			pKey->Key,
+			pSrc,
+			pSrc + 6,
+			UserPriority,
+			pKey->TxMic);
+	}
+
+
+	if (pEncap != NULL)
+	{
+		// LLC encapsulation
+		RTMPTkipAppend(&pAd->PrivateInfo.Tx, pEncap, 6);
+		// Protocol Type
+		RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSrc + 12 + vlan_offset, 2);
+	}
+	SrcBufLen -= (14 + vlan_offset);
+	pSrc += (14 + vlan_offset);
+	do
+	{
+		if (SrcBufLen > 0)
+		{
+			RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSrc, SrcBufLen);
+		}
+
+		break;	// No need handle next packet
+
+	}	while (TRUE);		// End of copying payload
+
+	// Compute the final MIC Value
+	RTMPTkipGetMIC(&pAd->PrivateInfo.Tx);
+}
+
+
+/************************************************************/
+/* tkip_sbox()																*/
+/* Returns a 16 bit value from a 64K entry table. The Table */
+/* is synthesized from two 256 entry byte wide tables.		*/
+/************************************************************/
+
+UINT tkip_sbox(UINT index)
+{
+	UINT index_low;
+	UINT index_high;
+	UINT left, right;
+
+	index_low = (index % 256);
+	index_high = ((index >> 8) % 256);
+
+	left = Tkip_Sbox_Lower[index_low] + (Tkip_Sbox_Upper[index_low] * 256);
+	right = Tkip_Sbox_Upper[index_high] + (Tkip_Sbox_Lower[index_high] * 256);
+
+	return (left ^ right);
+}
+
+UINT rotr1(UINT a)
+{
+	unsigned int b;
+
+	if ((a & 0x01) == 0x01)
+	{
+		b = (a >> 1) | 0x8000;
+	}
+	else
+	{
+		b = (a >> 1) & 0x7fff;
+	}
+	b = b % 65536;
+	return b;
+}
+
+VOID RTMPTkipMixKey(
+	UCHAR *key,
+	UCHAR *ta,
+	ULONG pnl, /* Least significant 16 bits of PN */
+	ULONG pnh, /* Most significant 32 bits of PN */
+	UCHAR *rc4key,
+	UINT *p1k)
+{
+
+	UINT tsc0;
+	UINT tsc1;
+	UINT tsc2;
+
+	UINT ppk0;
+	UINT ppk1;
+	UINT ppk2;
+	UINT ppk3;
+	UINT ppk4;
+	UINT ppk5;
+
+	INT i;
+	INT j;
+
+	tsc0 = (unsigned int)((pnh >> 16) % 65536); /* msb */
+	tsc1 = (unsigned int)(pnh % 65536);
+	tsc2 = (unsigned int)(pnl % 65536); /* lsb */
+
+	/* Phase 1, step 1 */
+	p1k[0] = tsc1;
+	p1k[1] = tsc0;
+	p1k[2] = (UINT)(ta[0] + (ta[1]*256));
+	p1k[3] = (UINT)(ta[2] + (ta[3]*256));
+	p1k[4] = (UINT)(ta[4] + (ta[5]*256));
+
+	/* Phase 1, step 2 */
+	for (i=0; i<8; i++)
+	{
+		j = 2*(i & 1);
+		p1k[0] = (p1k[0] + tkip_sbox( (p1k[4] ^ ((256*key[1+j]) + key[j])) % 65536 )) % 65536;
+		p1k[1] = (p1k[1] + tkip_sbox( (p1k[0] ^ ((256*key[5+j]) + key[4+j])) % 65536 )) % 65536;
+		p1k[2] = (p1k[2] + tkip_sbox( (p1k[1] ^ ((256*key[9+j]) + key[8+j])) % 65536 )) % 65536;
+		p1k[3] = (p1k[3] + tkip_sbox( (p1k[2] ^ ((256*key[13+j]) + key[12+j])) % 65536 )) % 65536;
+		p1k[4] = (p1k[4] + tkip_sbox( (p1k[3] ^ (((256*key[1+j]) + key[j]))) % 65536 )) % 65536;
+		p1k[4] = (p1k[4] + i) % 65536;
+	}
+
+	/* Phase 2, Step 1 */
+	ppk0 = p1k[0];
+	ppk1 = p1k[1];
+	ppk2 = p1k[2];
+	ppk3 = p1k[3];
+	ppk4 = p1k[4];
+	ppk5 = (p1k[4] + tsc2) % 65536;
+
+	/* Phase2, Step 2 */
+	ppk0 = ppk0 + tkip_sbox( (ppk5 ^ ((256*key[1]) + key[0])) % 65536);
+	ppk1 = ppk1 + tkip_sbox( (ppk0 ^ ((256*key[3]) + key[2])) % 65536);
+	ppk2 = ppk2 + tkip_sbox( (ppk1 ^ ((256*key[5]) + key[4])) % 65536);
+	ppk3 = ppk3 + tkip_sbox( (ppk2 ^ ((256*key[7]) + key[6])) % 65536);
+	ppk4 = ppk4 + tkip_sbox( (ppk3 ^ ((256*key[9]) + key[8])) % 65536);
+	ppk5 = ppk5 + tkip_sbox( (ppk4 ^ ((256*key[11]) + key[10])) % 65536);
+
+	ppk0 = ppk0 + rotr1(ppk5 ^ ((256*key[13]) + key[12]));
+	ppk1 = ppk1 + rotr1(ppk0 ^ ((256*key[15]) + key[14]));
+	ppk2 = ppk2 + rotr1(ppk1);
+	ppk3 = ppk3 + rotr1(ppk2);
+	ppk4 = ppk4 + rotr1(ppk3);
+	ppk5 = ppk5 + rotr1(ppk4);
+
+	/* Phase 2, Step 3 */
+    /* Phase 2, Step 3 */
+
+	tsc0 = (unsigned int)((pnh >> 16) % 65536); /* msb */
+	tsc1 = (unsigned int)(pnh % 65536);
+	tsc2 = (unsigned int)(pnl % 65536); /* lsb */
+
+	rc4key[0] = (tsc2 >> 8) % 256;
+	rc4key[1] = (((tsc2 >> 8) % 256) | 0x20) & 0x7f;
+	rc4key[2] = tsc2 % 256;
+	rc4key[3] = ((ppk5 ^ ((256*key[1]) + key[0])) >> 1) % 256;
+
+	rc4key[4] = ppk0 % 256;
+	rc4key[5] = (ppk0 >> 8) % 256;
+
+	rc4key[6] = ppk1 % 256;
+	rc4key[7] = (ppk1 >> 8) % 256;
+
+	rc4key[8] = ppk2 % 256;
+	rc4key[9] = (ppk2 >> 8) % 256;
+
+	rc4key[10] = ppk3 % 256;
+	rc4key[11] = (ppk3 >> 8) % 256;
+
+	rc4key[12] = ppk4 % 256;
+	rc4key[13] = (ppk4 >> 8) % 256;
+
+	rc4key[14] = ppk5 % 256;
+	rc4key[15] = (ppk5 >> 8) % 256;
+}
+
+
+/************************************************/
+/* construct_mic_header1()                      */
+/* Builds the first MIC header block from       */
+/* header fields.                               */
+/************************************************/
+
+void construct_mic_header1(
+	unsigned char *mic_header1,
+	int header_length,
+	unsigned char *mpdu)
+{
+	mic_header1[0] = (unsigned char)((header_length - 2) / 256);
+	mic_header1[1] = (unsigned char)((header_length - 2) % 256);
+	mic_header1[2] = mpdu[0] & 0xcf;    /* Mute CF poll & CF ack bits */
+	mic_header1[3] = mpdu[1] & 0xc7;    /* Mute retry, more data and pwr mgt bits */
+	mic_header1[4] = mpdu[4];       /* A1 */
+	mic_header1[5] = mpdu[5];
+	mic_header1[6] = mpdu[6];
+	mic_header1[7] = mpdu[7];
+	mic_header1[8] = mpdu[8];
+	mic_header1[9] = mpdu[9];
+	mic_header1[10] = mpdu[10];     /* A2 */
+	mic_header1[11] = mpdu[11];
+	mic_header1[12] = mpdu[12];
+	mic_header1[13] = mpdu[13];
+	mic_header1[14] = mpdu[14];
+	mic_header1[15] = mpdu[15];
+}
+
+/************************************************/
+/* construct_mic_header2()                      */
+/* Builds the last MIC header block from        */
+/* header fields.                               */
+/************************************************/
+
+void construct_mic_header2(
+	unsigned char *mic_header2,
+	unsigned char *mpdu,
+	int a4_exists,
+	int qc_exists)
+{
+	int i;
+
+	for (i = 0; i<16; i++) mic_header2[i]=0x00;
+
+	mic_header2[0] = mpdu[16];    /* A3 */
+	mic_header2[1] = mpdu[17];
+	mic_header2[2] = mpdu[18];
+	mic_header2[3] = mpdu[19];
+	mic_header2[4] = mpdu[20];
+	mic_header2[5] = mpdu[21];
+
+	// In Sequence Control field, mute sequence numer bits (12-bit)
+	mic_header2[6] = mpdu[22] & 0x0f;   /* SC */
+	mic_header2[7] = 0x00; /* mpdu[23]; */
+
+	if ((!qc_exists) & a4_exists)
+	{
+		for (i=0;i<6;i++) mic_header2[8+i] = mpdu[24+i];   /* A4 */
+
+	}
+
+	if (qc_exists && (!a4_exists))
+	{
+		mic_header2[8] = mpdu[24] & 0x0f; /* mute bits 15 - 4 */
+		mic_header2[9] = mpdu[25] & 0x00;
+	}
+
+	if (qc_exists && a4_exists)
+	{
+		for (i=0;i<6;i++) mic_header2[8+i] = mpdu[24+i];   /* A4 */
+
+		mic_header2[14] = mpdu[30] & 0x0f;
+		mic_header2[15] = mpdu[31] & 0x00;
+	}
+}
+
+
+/************************************************/
+/* construct_mic_iv()                           */
+/* Builds the MIC IV from header fields and PN  */
+/************************************************/
+
+void construct_mic_iv(
+	unsigned char *mic_iv,
+	int qc_exists,
+	int a4_exists,
+	unsigned char *mpdu,
+	unsigned int payload_length,
+	unsigned char *pn_vector)
+{
+	int i;
+
+	mic_iv[0] = 0x59;
+	if (qc_exists && a4_exists)
+		mic_iv[1] = mpdu[30] & 0x0f;    /* QoS_TC           */
+	if (qc_exists && !a4_exists)
+		mic_iv[1] = mpdu[24] & 0x0f;   /* mute bits 7-4    */
+	if (!qc_exists)
+		mic_iv[1] = 0x00;
+	for (i = 2; i < 8; i++)
+		mic_iv[i] = mpdu[i + 8];                    /* mic_iv[2:7] = A2[0:5] = mpdu[10:15] */
+#ifdef CONSISTENT_PN_ORDER
+		for (i = 8; i < 14; i++)
+			mic_iv[i] = pn_vector[i - 8];           /* mic_iv[8:13] = PN[0:5] */
+#else
+		for (i = 8; i < 14; i++)
+			mic_iv[i] = pn_vector[13 - i];          /* mic_iv[8:13] = PN[5:0] */
+#endif
+	i = (payload_length / 256);
+	i = (payload_length % 256);
+	mic_iv[14] = (unsigned char) (payload_length / 256);
+	mic_iv[15] = (unsigned char) (payload_length % 256);
+
+}
+
+
+
+/************************************/
+/* bitwise_xor()                    */
+/* A 128 bit, bitwise exclusive or  */
+/************************************/
+
+void bitwise_xor(unsigned char *ina, unsigned char *inb, unsigned char *out)
+{
+	int i;
+	for (i=0; i<16; i++)
+	{
+		out[i] = ina[i] ^ inb[i];
+	}
+}
+
+
+void aes128k128d(unsigned char *key, unsigned char *data, unsigned char *ciphertext)
+{
+	int round;
+	int i;
+	unsigned char intermediatea[16];
+	unsigned char intermediateb[16];
+	unsigned char round_key[16];
+
+	for(i=0; i<16; i++) round_key[i] = key[i];
+
+	for (round = 0; round < 11; round++)
+	{
+		if (round == 0)
+		{
+			xor_128(round_key, data, ciphertext);
+			next_key(round_key, round);
+		}
+		else if (round == 10)
+		{
+			byte_sub(ciphertext, intermediatea);
+			shift_row(intermediatea, intermediateb);
+			xor_128(intermediateb, round_key, ciphertext);
+		}
+		else    /* 1 - 9 */
+		{
+			byte_sub(ciphertext, intermediatea);
+			shift_row(intermediatea, intermediateb);
+			mix_column(&intermediateb[0], &intermediatea[0]);
+			mix_column(&intermediateb[4], &intermediatea[4]);
+			mix_column(&intermediateb[8], &intermediatea[8]);
+			mix_column(&intermediateb[12], &intermediatea[12]);
+			xor_128(intermediatea, round_key, ciphertext);
+			next_key(round_key, round);
+		}
+	}
+
+}
+
+void construct_ctr_preload(
+	unsigned char *ctr_preload,
+	int a4_exists,
+	int qc_exists,
+	unsigned char *mpdu,
+	unsigned char *pn_vector,
+	int c)
+{
+
+	int i = 0;
+	for (i=0; i<16; i++) ctr_preload[i] = 0x00;
+	i = 0;
+
+	ctr_preload[0] = 0x01;                                  /* flag */
+	if (qc_exists && a4_exists) ctr_preload[1] = mpdu[30] & 0x0f;   /* QoC_Control  */
+	if (qc_exists && !a4_exists) ctr_preload[1] = mpdu[24] & 0x0f;
+
+	for (i = 2; i < 8; i++)
+		ctr_preload[i] = mpdu[i + 8];                       /* ctr_preload[2:7] = A2[0:5] = mpdu[10:15] */
+#ifdef CONSISTENT_PN_ORDER
+	  for (i = 8; i < 14; i++)
+			ctr_preload[i] =    pn_vector[i - 8];           /* ctr_preload[8:13] = PN[0:5] */
+#else
+	  for (i = 8; i < 14; i++)
+			ctr_preload[i] =    pn_vector[13 - i];          /* ctr_preload[8:13] = PN[5:0] */
+#endif
+	ctr_preload[14] =  (unsigned char) (c / 256); // Ctr
+	ctr_preload[15] =  (unsigned char) (c % 256);
+
+}
+
+
+//
+// TRUE: Success!
+// FALSE: Decrypt Error!
+//
+BOOLEAN RTMPSoftDecryptTKIP(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR	pData,
+	IN ULONG	DataByteCnt,
+	IN UCHAR    UserPriority,
+	IN PCIPHER_KEY	pWpaKey)
+{
+	UCHAR			KeyID;
+	UINT			HeaderLen;
+    UCHAR			fc0;
+	UCHAR			fc1;
+	USHORT			fc;
+	UINT			frame_type;
+	UINT			frame_subtype;
+    UINT			from_ds;
+    UINT			to_ds;
+	INT				a4_exists;
+	INT				qc_exists;
+	USHORT			duration;
+	USHORT			seq_control;
+	USHORT			qos_control;
+	UCHAR			TA[MAC_ADDR_LEN];
+	UCHAR			DA[MAC_ADDR_LEN];
+	UCHAR			SA[MAC_ADDR_LEN];
+	UCHAR			RC4Key[16];
+	UINT			p1k[5]; //for mix_key;
+	ULONG			pnl;/* Least significant 16 bits of PN */
+	ULONG			pnh;/* Most significant 32 bits of PN */
+	UINT			num_blocks;
+	UINT			payload_remainder;
+	ARCFOURCONTEXT 	ArcFourContext;
+	UINT			crc32 = 0;
+	UINT			trailfcs = 0;
+	UCHAR			MIC[8];
+	UCHAR			TrailMIC[8];
+
+#ifdef RT_BIG_ENDIAN
+	RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
+#endif
+
+	fc0 = *pData;
+	fc1 = *(pData + 1);
+
+	fc = *((PUSHORT)pData);
+
+	frame_type = ((fc0 >> 2) & 0x03);
+	frame_subtype = ((fc0 >> 4) & 0x0f);
+
+    from_ds = (fc1 & 0x2) >> 1;
+    to_ds = (fc1 & 0x1);
+
+    a4_exists = (from_ds & to_ds);
+    qc_exists = ((frame_subtype == 0x08) ||    /* Assumed QoS subtypes */
+                  (frame_subtype == 0x09) ||   /* Likely to change.    */
+                  (frame_subtype == 0x0a) ||
+                  (frame_subtype == 0x0b)
+                 );
+
+	HeaderLen = 24;
+	if (a4_exists)
+		HeaderLen += 6;
+
+	KeyID = *((PUCHAR)(pData+ HeaderLen + 3));
+	KeyID = KeyID >> 6;
+
+	if (pWpaKey[KeyID].KeyLen == 0)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptTKIP failed!(KeyID[%d] Length can not be 0)\n", KeyID));
+		return FALSE;
+	}
+
+	duration = *((PUSHORT)(pData+2));
+
+	seq_control = *((PUSHORT)(pData+22));
+
+	if (qc_exists)
+	{
+		if (a4_exists)
+		{
+			qos_control = *((PUSHORT)(pData+30));
+		}
+		else
+		{
+			qos_control = *((PUSHORT)(pData+24));
+		}
+	}
+
+	if (to_ds == 0 && from_ds == 1)
+	{
+		NdisMoveMemory(DA, pData+4, MAC_ADDR_LEN);
+		NdisMoveMemory(SA, pData+16, MAC_ADDR_LEN);
+		NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);  //BSSID
+	}
+	else if (to_ds == 0 && from_ds == 0 )
+	{
+		NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
+		NdisMoveMemory(DA, pData+4, MAC_ADDR_LEN);
+		NdisMoveMemory(SA, pData+10, MAC_ADDR_LEN);
+	}
+	else if (to_ds == 1 && from_ds == 0)
+	{
+		NdisMoveMemory(SA, pData+10, MAC_ADDR_LEN);
+		NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
+		NdisMoveMemory(DA, pData+16, MAC_ADDR_LEN);
+	}
+	else if (to_ds == 1 && from_ds == 1)
+	{
+		NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
+		NdisMoveMemory(DA, pData+16, MAC_ADDR_LEN);
+		NdisMoveMemory(SA, pData+22, MAC_ADDR_LEN);
+	}
+
+	num_blocks = (DataByteCnt - 16) / 16;
+	payload_remainder = (DataByteCnt - 16) % 16;
+
+	pnl = (*(pData + HeaderLen)) * 256 + *(pData + HeaderLen + 2);
+	pnh = *((PULONG)(pData + HeaderLen + 4));
+	pnh = cpu2le32(pnh);
+	RTMPTkipMixKey(pWpaKey[KeyID].Key, TA, pnl, pnh, RC4Key, p1k);
+
+	ARCFOUR_INIT(&ArcFourContext, RC4Key, 16);
+
+	ARCFOUR_DECRYPT(&ArcFourContext, pData + HeaderLen, pData + HeaderLen + 8, DataByteCnt - HeaderLen - 8);
+	NdisMoveMemory(&trailfcs, pData + DataByteCnt - 8 - 4, 4);
+	crc32 = RTMP_CALC_FCS32(PPPINITFCS32, pData + HeaderLen, DataByteCnt - HeaderLen - 8 - 4);  //Skip IV+EIV 8 bytes & Skip last 4 bytes(FCS).
+	crc32 ^= 0xffffffff;             /* complement */
+
+    if(crc32 != cpu2le32(trailfcs))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptTKIP, WEP Data ICV Error !\n"));	 //ICV error.
+
+		return (FALSE);
+	}
+
+	NdisMoveMemory(TrailMIC, pData + DataByteCnt - 8 - 8 - 4, 8);
+	RTMPInitMICEngine(pAd, pWpaKey[KeyID].Key, DA, SA, UserPriority, pWpaKey[KeyID].RxMic);
+	RTMPTkipAppend(&pAd->PrivateInfo.Tx, pData + HeaderLen, DataByteCnt - HeaderLen - 8 - 12);
+	RTMPTkipGetMIC(&pAd->PrivateInfo.Tx);
+	NdisMoveMemory(MIC, pAd->PrivateInfo.Tx.MIC, 8);
+
+	if (!NdisEqualMemory(MIC, TrailMIC, 8))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("RTMPSoftDecryptTKIP, WEP Data MIC Error !\n"));	 //MIC error.
+		return (FALSE);
+	}
+
+#ifdef RT_BIG_ENDIAN
+	RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
+#endif
+	return TRUE;
+}
+
+
+
+
+BOOLEAN RTMPSoftDecryptAES(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR	pData,
+	IN ULONG	DataByteCnt,
+	IN PCIPHER_KEY	pWpaKey)
+{
+	UCHAR			KeyID;
+	UINT			HeaderLen;
+	UCHAR			PN[6];
+	UINT			payload_len;
+	UINT			num_blocks;
+	UINT			payload_remainder;
+	USHORT			fc;
+	UCHAR			fc0;
+	UCHAR			fc1;
+	UINT			frame_type;
+	UINT			frame_subtype;
+	UINT			from_ds;
+	UINT			to_ds;
+	INT				a4_exists;
+	INT				qc_exists;
+	UCHAR			aes_out[16];
+	int 			payload_index;
+	UINT 			i;
+	UCHAR 			ctr_preload[16];
+	UCHAR 			chain_buffer[16];
+	UCHAR 			padded_buffer[16];
+	UCHAR 			mic_iv[16];
+	UCHAR 			mic_header1[16];
+	UCHAR 			mic_header2[16];
+	UCHAR			MIC[8];
+	UCHAR			TrailMIC[8];
+
+#ifdef RT_BIG_ENDIAN
+	RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
+#endif
+
+	fc0 = *pData;
+	fc1 = *(pData + 1);
+
+	fc = *((PUSHORT)pData);
+
+	frame_type = ((fc0 >> 2) & 0x03);
+	frame_subtype = ((fc0 >> 4) & 0x0f);
+
+	from_ds = (fc1 & 0x2) >> 1;
+	to_ds = (fc1 & 0x1);
+
+	a4_exists = (from_ds & to_ds);
+	qc_exists = ((frame_subtype == 0x08) ||    /* Assumed QoS subtypes */
+				  (frame_subtype == 0x09) ||   /* Likely to change.    */
+				  (frame_subtype == 0x0a) ||
+				  (frame_subtype == 0x0b)
+				 );
+
+	HeaderLen = 24;
+	if (a4_exists)
+		HeaderLen += 6;
+
+	KeyID = *((PUCHAR)(pData+ HeaderLen + 3));
+	KeyID = KeyID >> 6;
+
+	if (pWpaKey[KeyID].KeyLen == 0)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptAES failed!(KeyID[%d] Length can not be 0)\n", KeyID));
+		return FALSE;
+	}
+
+	PN[0] = *(pData+ HeaderLen);
+	PN[1] = *(pData+ HeaderLen + 1);
+	PN[2] = *(pData+ HeaderLen + 4);
+	PN[3] = *(pData+ HeaderLen + 5);
+	PN[4] = *(pData+ HeaderLen + 6);
+	PN[5] = *(pData+ HeaderLen + 7);
+
+	payload_len = DataByteCnt - HeaderLen - 8 - 8;	// 8 bytes for CCMP header , 8 bytes for MIC
+	payload_remainder = (payload_len) % 16;
+	num_blocks = (payload_len) / 16;
+
+
+
+	// Find start of payload
+	payload_index = HeaderLen + 8; //IV+EIV
+
+	for (i=0; i< num_blocks; i++)
+	{
+		construct_ctr_preload(ctr_preload,
+								a4_exists,
+								qc_exists,
+								pData,
+								PN,
+								i+1 );
+
+		aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
+
+		bitwise_xor(aes_out, pData + payload_index, chain_buffer);
+		NdisMoveMemory(pData + payload_index - 8, chain_buffer, 16);
+		payload_index += 16;
+	}
+
+	//
+	// If there is a short final block, then pad it
+	// encrypt it and copy the unpadded part back
+	//
+	if (payload_remainder > 0)
+	{
+		construct_ctr_preload(ctr_preload,
+								a4_exists,
+								qc_exists,
+								pData,
+								PN,
+								num_blocks + 1);
+
+		NdisZeroMemory(padded_buffer, 16);
+		NdisMoveMemory(padded_buffer, pData + payload_index, payload_remainder);
+
+		aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
+
+		bitwise_xor(aes_out, padded_buffer, chain_buffer);
+		NdisMoveMemory(pData + payload_index - 8, chain_buffer, payload_remainder);
+		payload_index += payload_remainder;
+	}
+
+	//
+	// Descrypt the MIC
+	//
+	construct_ctr_preload(ctr_preload,
+							a4_exists,
+							qc_exists,
+							pData,
+							PN,
+							0);
+	NdisZeroMemory(padded_buffer, 16);
+	NdisMoveMemory(padded_buffer, pData + payload_index, 8);
+
+	aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
+
+	bitwise_xor(aes_out, padded_buffer, chain_buffer);
+
+	NdisMoveMemory(TrailMIC, chain_buffer, 8);
+
+	//
+	// Calculate MIC
+	//
+
+	//Force the protected frame bit on
+	*(pData + 1) = *(pData + 1) | 0x40;
+
+	// Find start of payload
+	// Because the CCMP header has been removed
+	payload_index = HeaderLen;
+
+	construct_mic_iv(
+					mic_iv,
+					qc_exists,
+					a4_exists,
+					pData,
+					payload_len,
+					PN);
+
+	construct_mic_header1(
+						mic_header1,
+						HeaderLen,
+						pData);
+
+	construct_mic_header2(
+						mic_header2,
+						pData,
+						a4_exists,
+						qc_exists);
+
+	aes128k128d(pWpaKey[KeyID].Key, mic_iv, aes_out);
+	bitwise_xor(aes_out, mic_header1, chain_buffer);
+	aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
+	bitwise_xor(aes_out, mic_header2, chain_buffer);
+	aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
+
+	// iterate through each 16 byte payload block
+	for (i = 0; i < num_blocks; i++)
+	{
+		bitwise_xor(aes_out, pData + payload_index, chain_buffer);
+		payload_index += 16;
+		aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
+	}
+
+	// Add on the final payload block if it needs padding
+	if (payload_remainder > 0)
+	{
+		NdisZeroMemory(padded_buffer, 16);
+		NdisMoveMemory(padded_buffer, pData + payload_index, payload_remainder);
+
+		bitwise_xor(aes_out, padded_buffer, chain_buffer);
+		aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
+	}
+	// aes_out contains padded mic, discard most significant
+	// 8 bytes to generate 64 bit MIC
+	for (i = 0 ; i < 8; i++) MIC[i] = aes_out[i];
+
+	if (!NdisEqualMemory(MIC, TrailMIC, 8))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("RTMPSoftDecryptAES, MIC Error !\n"));	 //MIC error.
+		return FALSE;
+	}
+
+#ifdef RT_BIG_ENDIAN
+	RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
+#endif
+
+	return TRUE;
+}
+
+/****************************************/
+/* aes128k128d()                        */
+/* Performs a 128 bit AES encrypt with  */
+/* 128 bit data.                        */
+/****************************************/
+VOID xor_128(
+	IN  PUCHAR  a,
+	IN  PUCHAR  b,
+	OUT PUCHAR  out)
+{
+	INT i;
+
+	for (i=0;i<16; i++)
+	{
+		out[i] = a[i] ^ b[i];
+	}
+}
+
+VOID next_key(
+	IN  PUCHAR  key,
+	IN  INT     round)
+{
+	UCHAR       rcon;
+	UCHAR       sbox_key[4];
+	UCHAR       rcon_table[12] =
+	{
+		0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80,
+		0x1b, 0x36, 0x36, 0x36
+	};
+
+	sbox_key[0] = RTMPCkipSbox(key[13]);
+	sbox_key[1] = RTMPCkipSbox(key[14]);
+	sbox_key[2] = RTMPCkipSbox(key[15]);
+	sbox_key[3] = RTMPCkipSbox(key[12]);
+
+	rcon = rcon_table[round];
+
+	xor_32(&key[0], sbox_key, &key[0]);
+	key[0] = key[0] ^ rcon;
+
+	xor_32(&key[4], &key[0], &key[4]);
+	xor_32(&key[8], &key[4], &key[8]);
+	xor_32(&key[12], &key[8], &key[12]);
+}
+
+VOID xor_32(
+	IN  PUCHAR  a,
+	IN  PUCHAR  b,
+	OUT PUCHAR  out)
+{
+	INT i;
+
+	for (i=0;i<4; i++)
+	{
+		out[i] = a[i] ^ b[i];
+	}
+}
+
+VOID byte_sub(
+	IN  PUCHAR  in,
+	OUT PUCHAR  out)
+{
+	INT i;
+
+	for (i=0; i< 16; i++)
+	{
+		out[i] = RTMPCkipSbox(in[i]);
+	}
+}
+
+UCHAR RTMPCkipSbox(
+	IN  UCHAR   a)
+{
+	return SboxTable[(int)a];
+}
+
+VOID shift_row(
+	IN  PUCHAR  in,
+	OUT PUCHAR  out)
+{
+	out[0] =  in[0];
+	out[1] =  in[5];
+	out[2] =  in[10];
+	out[3] =  in[15];
+	out[4] =  in[4];
+	out[5] =  in[9];
+	out[6] =  in[14];
+	out[7] =  in[3];
+	out[8] =  in[8];
+	out[9] =  in[13];
+	out[10] = in[2];
+	out[11] = in[7];
+	out[12] = in[12];
+	out[13] = in[1];
+	out[14] = in[6];
+	out[15] = in[11];
+}
+
+VOID mix_column(
+	IN  PUCHAR  in,
+	OUT PUCHAR  out)
+{
+	INT         i;
+	UCHAR       add1b[4];
+	UCHAR       add1bf7[4];
+	UCHAR       rotl[4];
+	UCHAR       swap_halfs[4];
+	UCHAR       andf7[4];
+	UCHAR       rotr[4];
+	UCHAR       temp[4];
+	UCHAR       tempb[4];
+
+	for (i=0 ; i<4; i++)
+	{
+		if ((in[i] & 0x80)== 0x80)
+			add1b[i] = 0x1b;
+		else
+			add1b[i] = 0x00;
+	}
+
+	swap_halfs[0] = in[2];    /* Swap halfs */
+	swap_halfs[1] = in[3];
+	swap_halfs[2] = in[0];
+	swap_halfs[3] = in[1];
+
+	rotl[0] = in[3];        /* Rotate left 8 bits */
+	rotl[1] = in[0];
+	rotl[2] = in[1];
+	rotl[3] = in[2];
+
+	andf7[0] = in[0] & 0x7f;
+	andf7[1] = in[1] & 0x7f;
+	andf7[2] = in[2] & 0x7f;
+	andf7[3] = in[3] & 0x7f;
+
+	for (i = 3; i>0; i--)    /* logical shift left 1 bit */
+	{
+		andf7[i] = andf7[i] << 1;
+		if ((andf7[i-1] & 0x80) == 0x80)
+		{
+			andf7[i] = (andf7[i] | 0x01);
+		}
+	}
+	andf7[0] = andf7[0] << 1;
+	andf7[0] = andf7[0] & 0xfe;
+
+	xor_32(add1b, andf7, add1bf7);
+
+	xor_32(in, add1bf7, rotr);
+
+	temp[0] = rotr[0];         /* Rotate right 8 bits */
+	rotr[0] = rotr[1];
+	rotr[1] = rotr[2];
+	rotr[2] = rotr[3];
+	rotr[3] = temp[0];
+
+	xor_32(add1bf7, rotr, temp);
+	xor_32(swap_halfs, rotl,tempb);
+	xor_32(temp, tempb, out);
+}
+
diff --git a/drivers/staging/rt2860/common/rtmp_wep.c b/drivers/staging/rt2860/common/rtmp_wep.c
new file mode 100644
index 00000000000..ffe26c23795
--- /dev/null
+++ b/drivers/staging/rt2860/common/rtmp_wep.c
@@ -0,0 +1,499 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rtmp_wep.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Paul Wu		10-28-02		Initial
+*/
+
+#include "../rt_config.h"
+
+UINT FCSTAB_32[256] =
+{
+	0x00000000, 0x77073096, 0xee0e612c, 0x990951ba,
+	0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3,
+	0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988,
+	0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91,
+	0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de,
+	0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
+	0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec,
+	0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5,
+	0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172,
+	0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b,
+	0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940,
+	0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
+	0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116,
+	0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f,
+	0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924,
+	0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d,
+	0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a,
+	0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
+	0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818,
+	0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01,
+	0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e,
+	0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457,
+	0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c,
+	0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
+	0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2,
+	0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb,
+	0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0,
+	0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9,
+	0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086,
+	0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
+	0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4,
+	0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad,
+	0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a,
+	0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683,
+	0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8,
+	0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
+	0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe,
+	0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7,
+	0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc,
+	0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5,
+	0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252,
+	0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
+	0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60,
+	0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79,
+	0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236,
+	0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f,
+	0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04,
+	0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
+	0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a,
+	0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713,
+	0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38,
+	0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21,
+	0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e,
+	0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
+	0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c,
+	0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45,
+	0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2,
+	0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db,
+	0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0,
+	0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
+	0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6,
+	0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf,
+	0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94,
+	0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d
+};
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Init WEP function.
+
+	Arguments:
+      pAd		Pointer to our adapter
+		pKey        Pointer to the WEP KEY
+		KeyId		   WEP Key ID
+		KeyLen      the length of WEP KEY
+		pDest       Pointer to the destination which Encryption data will store in.
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPInitWepEngine(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pKey,
+	IN	UCHAR			KeyId,
+	IN	UCHAR			KeyLen,
+	IN OUT	PUCHAR		pDest)
+{
+	UINT i;
+	UCHAR   WEPKEY[] = {
+		//IV
+		0x00, 0x11, 0x22,
+		//WEP KEY
+		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88, 0x99, 0xAA, 0xBB, 0xCC
+	};
+
+	pAd->PrivateInfo.FCSCRC32 = PPPINITFCS32;   //Init crc32.
+
+#ifdef CONFIG_STA_SUPPORT
+    if (pAd->StaCfg.bCkipOn && (pAd->StaCfg.CkipFlag & 0x10) && (pAd->OpMode == OPMODE_STA))
+    {
+        ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, pKey, KeyLen);  //INIT SBOX, KEYLEN+3(IV)
+        NdisMoveMemory(pDest, pKey, 3);  //Append Init Vector
+    }
+    else
+#endif // CONFIG_STA_SUPPORT //
+    {
+		NdisMoveMemory(WEPKEY + 3, pKey, KeyLen);
+
+        for(i = 0; i < 3; i++)
+			WEPKEY[i] = RandomByte(pAd);   //Call mlme RandomByte() function.
+		ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, WEPKEY, KeyLen + 3);  //INIT SBOX, KEYLEN+3(IV)
+
+		NdisMoveMemory(pDest, WEPKEY, 3);  //Append Init Vector
+    }
+	*(pDest+3) = (KeyId << 6);       //Append KEYID
+
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Encrypt transimitted data
+
+	Arguments:
+      pAd		Pointer to our adapter
+      pSrc        Pointer to the transimitted source data that will be encrypt
+      pDest       Pointer to the destination where entryption data will be store in.
+      Len			Indicate the length of the source data
+
+	Return Value:
+      None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPEncryptData(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pSrc,
+	IN	PUCHAR			pDest,
+	IN	UINT			Len)
+{
+	pAd->PrivateInfo.FCSCRC32 = RTMP_CALC_FCS32(pAd->PrivateInfo.FCSCRC32, pSrc, Len);
+	ARCFOUR_ENCRYPT(&pAd->PrivateInfo.WEPCONTEXT, pDest, pSrc, Len);
+}
+
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Decrypt received WEP data
+
+	Arguments:
+		pAdapter		Pointer to our adapter
+		pSrc        Pointer to the received data
+		Len         the length of the received data
+
+	Return Value:
+		TRUE        Decrypt WEP data success
+		FALSE       Decrypt WEP data failed
+
+	Note:
+
+	========================================================================
+*/
+BOOLEAN	RTMPSoftDecryptWEP(
+	IN PRTMP_ADAPTER 	pAd,
+	IN PUCHAR			pData,
+	IN ULONG			DataByteCnt,
+	IN PCIPHER_KEY		pGroupKey)
+{
+	UINT	trailfcs;
+	UINT    crc32;
+	UCHAR	KeyIdx;
+	UCHAR   WEPKEY[] = {
+		//IV
+		0x00, 0x11, 0x22,
+		//WEP KEY
+		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88, 0x99, 0xAA, 0xBB, 0xCC
+	};
+	UCHAR 	*pPayload = (UCHAR *)pData + LENGTH_802_11;
+	ULONG	payload_len = DataByteCnt - LENGTH_802_11;
+
+	NdisMoveMemory(WEPKEY, pPayload, 3);    //Get WEP IV
+
+	KeyIdx = (*(pPayload + 3) & 0xc0) >> 6;
+	if (pGroupKey[KeyIdx].KeyLen == 0)
+		return (FALSE);
+
+	NdisMoveMemory(WEPKEY + 3, pGroupKey[KeyIdx].Key, pGroupKey[KeyIdx].KeyLen);
+	ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, WEPKEY, pGroupKey[KeyIdx].KeyLen + 3);
+	ARCFOUR_DECRYPT(&pAd->PrivateInfo.WEPCONTEXT, pPayload, pPayload + 4, payload_len - 4);
+	NdisMoveMemory(&trailfcs, pPayload + payload_len - 8, 4);
+	crc32 = RTMP_CALC_FCS32(PPPINITFCS32, pPayload, payload_len - 8);  //Skip last 4 bytes(FCS).
+	crc32 ^= 0xffffffff;             /* complement */
+
+    if(crc32 != cpu2le32(trailfcs))
+    {
+		DBGPRINT(RT_DEBUG_TRACE, ("! WEP Data CRC Error !\n"));	 //CRC error.
+		return (FALSE);
+	}
+	return (TRUE);
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		The Stream Cipher Encryption Algorithm "ARCFOUR" initialize
+
+	Arguments:
+	   Ctx         Pointer to ARCFOUR CONTEXT (SBOX)
+		pKey        Pointer to the WEP KEY
+		KeyLen      Indicate the length fo the WEP KEY
+
+	Return Value:
+	   None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	ARCFOUR_INIT(
+	IN	PARCFOURCONTEXT	Ctx,
+	IN	PUCHAR			pKey,
+	IN	UINT			KeyLen)
+{
+	UCHAR	t, u;
+	UINT	keyindex;
+	UINT	stateindex;
+	PUCHAR	state;
+	UINT	counter;
+
+	state = Ctx->STATE;
+	Ctx->X = 0;
+	Ctx->Y = 0;
+	for (counter = 0; counter < 256; counter++)
+		state[counter] = (UCHAR)counter;
+	keyindex = 0;
+	stateindex = 0;
+	for (counter = 0; counter < 256; counter++)
+	{
+		t = state[counter];
+		stateindex = (stateindex + pKey[keyindex] + t) & 0xff;
+		u = state[stateindex];
+		state[stateindex] = t;
+		state[counter] = u;
+		if (++keyindex >= KeyLen)
+			keyindex = 0;
+	}
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Get bytes from ARCFOUR CONTEXT (S-BOX)
+
+	Arguments:
+	   Ctx         Pointer to ARCFOUR CONTEXT (SBOX)
+
+	Return Value:
+	   UCHAR  - the value of the ARCFOUR CONTEXT (S-BOX)
+
+	Note:
+
+	========================================================================
+*/
+UCHAR	ARCFOUR_BYTE(
+	IN	PARCFOURCONTEXT		Ctx)
+{
+  UINT x;
+  UINT y;
+  UCHAR sx, sy;
+  PUCHAR state;
+
+  state = Ctx->STATE;
+  x = (Ctx->X + 1) & 0xff;
+  sx = state[x];
+  y = (sx + Ctx->Y) & 0xff;
+  sy = state[y];
+  Ctx->X = x;
+  Ctx->Y = y;
+  state[y] = sx;
+  state[x] = sy;
+
+  return(state[(sx + sy) & 0xff]);
+
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		The Stream Cipher Decryption Algorithm
+
+	Arguments:
+		Ctx         Pointer to ARCFOUR CONTEXT (SBOX)
+		pDest			Pointer to the Destination
+		pSrc        Pointer to the Source data
+		Len         Indicate the length of the Source data
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	ARCFOUR_DECRYPT(
+	IN	PARCFOURCONTEXT	Ctx,
+	IN	PUCHAR			pDest,
+	IN	PUCHAR			pSrc,
+	IN	UINT			Len)
+{
+	UINT i;
+
+	for (i = 0; i < Len; i++)
+		pDest[i] = pSrc[i] ^ ARCFOUR_BYTE(Ctx);
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		The Stream Cipher Encryption Algorithm
+
+	Arguments:
+		Ctx         Pointer to ARCFOUR CONTEXT (SBOX)
+		pDest			Pointer to the Destination
+		pSrc        Pointer to the Source data
+		Len         Indicate the length of the Source dta
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	ARCFOUR_ENCRYPT(
+	IN	PARCFOURCONTEXT	Ctx,
+	IN	PUCHAR			pDest,
+	IN	PUCHAR			pSrc,
+	IN	UINT			Len)
+{
+	UINT i;
+
+	for (i = 0; i < Len; i++)
+		pDest[i] = pSrc[i] ^ ARCFOUR_BYTE(Ctx);
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		The Stream Cipher Encryption Algorithm which conform to the special requirement to encrypt  GTK.
+
+	Arguments:
+		Ctx         Pointer to ARCFOUR CONTEXT (SBOX)
+		pDest			Pointer to the Destination
+		pSrc        Pointer to the Source data
+		Len         Indicate the length of the Source dta
+
+
+	========================================================================
+*/
+
+VOID	WPAARCFOUR_ENCRYPT(
+	IN	PARCFOURCONTEXT	Ctx,
+	IN	PUCHAR			pDest,
+	IN	PUCHAR			pSrc,
+	IN	UINT			Len)
+{
+	UINT i;
+        //discard first 256 bytes
+	for (i = 0; i < 256; i++)
+            ARCFOUR_BYTE(Ctx);
+
+	for (i = 0; i < Len; i++)
+		pDest[i] = pSrc[i] ^ ARCFOUR_BYTE(Ctx);
+}
+
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Calculate a new FCS given the current FCS and the new data.
+
+	Arguments:
+		Fcs	      the original FCS value
+		Cp          pointer to the data which will be calculate the FCS
+		Len         the length of the data
+
+	Return Value:
+		UINT - FCS 32 bits
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+UINT	RTMP_CALC_FCS32(
+	IN	UINT	Fcs,
+	IN	PUCHAR	Cp,
+	IN	INT		Len)
+{
+	while (Len--)
+	   Fcs = (((Fcs) >> 8) ^ FCSTAB_32[((Fcs) ^ (*Cp++)) & 0xff]);
+
+	return (Fcs);
+}
+
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Get last FCS and encrypt it to the destination
+
+	Arguments:
+		pDest			Pointer to the Destination
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPSetICV(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR	pDest)
+{
+	pAd->PrivateInfo.FCSCRC32 ^= 0xffffffff;             /* complement */
+	pAd->PrivateInfo.FCSCRC32 = cpu2le32(pAd->PrivateInfo.FCSCRC32);
+
+	ARCFOUR_ENCRYPT(&pAd->PrivateInfo.WEPCONTEXT, pDest, (PUCHAR) &pAd->PrivateInfo.FCSCRC32, 4);
+}
+
diff --git a/drivers/staging/rt2860/common/spectrum.c b/drivers/staging/rt2860/common/spectrum.c
new file mode 100644
index 00000000000..85e636a607f
--- /dev/null
+++ b/drivers/staging/rt2860/common/spectrum.c
@@ -0,0 +1,1877 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+
+    Module Name:
+	action.c
+
+    Abstract:
+    Handle association related requests either from WSTA or from local MLME
+
+    Revision History:
+    Who          When          What
+    ---------    ----------    ----------------------------------------------
+	Fonchi Wu    2008	  	   created for 802.11h
+ */
+
+#include "../rt_config.h"
+#include "action.h"
+
+VOID MeasureReqTabInit(
+	IN PRTMP_ADAPTER pAd)
+{
+	NdisAllocateSpinLock(&pAd->CommonCfg.MeasureReqTabLock);
+
+	pAd->CommonCfg.pMeasureReqTab = kmalloc(sizeof(MEASURE_REQ_TAB), GFP_ATOMIC);
+	if (pAd->CommonCfg.pMeasureReqTab)
+		NdisZeroMemory(pAd->CommonCfg.pMeasureReqTab, sizeof(MEASURE_REQ_TAB));
+	else
+		DBGPRINT(RT_DEBUG_ERROR, ("%s Fail to alloc memory for pAd->CommonCfg.pMeasureReqTab.\n", __FUNCTION__));
+
+	return;
+}
+
+VOID MeasureReqTabExit(
+	IN PRTMP_ADAPTER pAd)
+{
+	NdisFreeSpinLock(pAd->CommonCfg.MeasureReqTabLock);
+
+	if (pAd->CommonCfg.pMeasureReqTab)
+		kfree(pAd->CommonCfg.pMeasureReqTab);
+	pAd->CommonCfg.pMeasureReqTab = NULL;
+
+	return;
+}
+
+static PMEASURE_REQ_ENTRY MeasureReqLookUp(
+	IN PRTMP_ADAPTER	pAd,
+	IN UINT8			DialogToken)
+{
+	UINT HashIdx;
+	PMEASURE_REQ_TAB pTab = pAd->CommonCfg.pMeasureReqTab;
+	PMEASURE_REQ_ENTRY pEntry = NULL;
+	PMEASURE_REQ_ENTRY pPrevEntry = NULL;
+
+	if (pTab == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: pMeasureReqTab doesn't exist.\n", __FUNCTION__));
+		return NULL;
+	}
+
+	RTMP_SEM_LOCK(&pAd->CommonCfg.MeasureReqTabLock);
+
+	HashIdx = MQ_DIALOGTOKEN_HASH_INDEX(DialogToken);
+	pEntry = pTab->Hash[HashIdx];
+
+	while (pEntry)
+	{
+		if (pEntry->DialogToken == DialogToken)
+			break;
+		else
+		{
+			pPrevEntry = pEntry;
+			pEntry = pEntry->pNext;
+		}
+	}
+
+	RTMP_SEM_UNLOCK(&pAd->CommonCfg.MeasureReqTabLock);
+
+	return pEntry;
+}
+
+static PMEASURE_REQ_ENTRY MeasureReqInsert(
+	IN PRTMP_ADAPTER	pAd,
+	IN UINT8			DialogToken)
+{
+	INT i;
+	ULONG HashIdx;
+	PMEASURE_REQ_TAB pTab = pAd->CommonCfg.pMeasureReqTab;
+	PMEASURE_REQ_ENTRY pEntry = NULL, pCurrEntry;
+	ULONG Now;
+
+	if(pTab == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: pMeasureReqTab doesn't exist.\n", __FUNCTION__));
+		return NULL;
+	}
+
+	pEntry = MeasureReqLookUp(pAd, DialogToken);
+	if (pEntry == NULL)
+	{
+		RTMP_SEM_LOCK(&pAd->CommonCfg.MeasureReqTabLock);
+		for (i = 0; i < MAX_MEASURE_REQ_TAB_SIZE; i++)
+		{
+			NdisGetSystemUpTime(&Now);
+			pEntry = &pTab->Content[i];
+
+			if ((pEntry->Valid == TRUE)
+				&& RTMP_TIME_AFTER((unsigned long)Now, (unsigned long)(pEntry->lastTime + MQ_REQ_AGE_OUT)))
+			{
+				PMEASURE_REQ_ENTRY pPrevEntry = NULL;
+				ULONG HashIdx = MQ_DIALOGTOKEN_HASH_INDEX(pEntry->DialogToken);
+				PMEASURE_REQ_ENTRY pProbeEntry = pTab->Hash[HashIdx];
+
+				// update Hash list
+				do
+				{
+					if (pProbeEntry == pEntry)
+					{
+						if (pPrevEntry == NULL)
+						{
+							pTab->Hash[HashIdx] = pEntry->pNext;
+						}
+						else
+						{
+							pPrevEntry->pNext = pEntry->pNext;
+						}
+						break;
+					}
+
+					pPrevEntry = pProbeEntry;
+					pProbeEntry = pProbeEntry->pNext;
+				} while (pProbeEntry);
+
+				NdisZeroMemory(pEntry, sizeof(MEASURE_REQ_ENTRY));
+				pTab->Size--;
+
+				break;
+			}
+
+			if (pEntry->Valid == FALSE)
+				break;
+		}
+
+		if (i < MAX_MEASURE_REQ_TAB_SIZE)
+		{
+			NdisGetSystemUpTime(&Now);
+			pEntry->lastTime = Now;
+			pEntry->Valid = TRUE;
+			pEntry->DialogToken = DialogToken;
+			pTab->Size++;
+		}
+		else
+		{
+			pEntry = NULL;
+			DBGPRINT(RT_DEBUG_ERROR, ("%s: pMeasureReqTab tab full.\n", __FUNCTION__));
+		}
+
+		// add this Neighbor entry into HASH table
+		if (pEntry)
+		{
+			HashIdx = MQ_DIALOGTOKEN_HASH_INDEX(DialogToken);
+			if (pTab->Hash[HashIdx] == NULL)
+			{
+				pTab->Hash[HashIdx] = pEntry;
+			}
+			else
+			{
+				pCurrEntry = pTab->Hash[HashIdx];
+				while (pCurrEntry->pNext != NULL)
+					pCurrEntry = pCurrEntry->pNext;
+				pCurrEntry->pNext = pEntry;
+			}
+		}
+
+		RTMP_SEM_UNLOCK(&pAd->CommonCfg.MeasureReqTabLock);
+	}
+
+	return pEntry;
+}
+
+static VOID MeasureReqDelete(
+	IN PRTMP_ADAPTER	pAd,
+	IN UINT8			DialogToken)
+{
+	PMEASURE_REQ_TAB pTab = pAd->CommonCfg.pMeasureReqTab;
+	PMEASURE_REQ_ENTRY pEntry = NULL;
+
+	if(pTab == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: pMeasureReqTab doesn't exist.\n", __FUNCTION__));
+		return;
+	}
+
+	// if empty, return
+	if (pTab->Size == 0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("pMeasureReqTab empty.\n"));
+		return;
+	}
+
+	pEntry = MeasureReqLookUp(pAd, DialogToken);
+	if (pEntry != NULL)
+	{
+		PMEASURE_REQ_ENTRY pPrevEntry = NULL;
+		ULONG HashIdx = MQ_DIALOGTOKEN_HASH_INDEX(pEntry->DialogToken);
+		PMEASURE_REQ_ENTRY pProbeEntry = pTab->Hash[HashIdx];
+
+		RTMP_SEM_LOCK(&pAd->CommonCfg.MeasureReqTabLock);
+		// update Hash list
+		do
+		{
+			if (pProbeEntry == pEntry)
+			{
+				if (pPrevEntry == NULL)
+				{
+					pTab->Hash[HashIdx] = pEntry->pNext;
+				}
+				else
+				{
+					pPrevEntry->pNext = pEntry->pNext;
+				}
+				break;
+			}
+
+			pPrevEntry = pProbeEntry;
+			pProbeEntry = pProbeEntry->pNext;
+		} while (pProbeEntry);
+
+		NdisZeroMemory(pEntry, sizeof(MEASURE_REQ_ENTRY));
+		pTab->Size--;
+
+		RTMP_SEM_UNLOCK(&pAd->CommonCfg.MeasureReqTabLock);
+	}
+
+	return;
+}
+
+VOID TpcReqTabInit(
+	IN PRTMP_ADAPTER pAd)
+{
+	NdisAllocateSpinLock(&pAd->CommonCfg.TpcReqTabLock);
+
+	pAd->CommonCfg.pTpcReqTab = kmalloc(sizeof(TPC_REQ_TAB), GFP_ATOMIC);
+	if (pAd->CommonCfg.pTpcReqTab)
+		NdisZeroMemory(pAd->CommonCfg.pTpcReqTab, sizeof(TPC_REQ_TAB));
+	else
+		DBGPRINT(RT_DEBUG_ERROR, ("%s Fail to alloc memory for pAd->CommonCfg.pTpcReqTab.\n", __FUNCTION__));
+
+	return;
+}
+
+VOID TpcReqTabExit(
+	IN PRTMP_ADAPTER pAd)
+{
+	NdisFreeSpinLock(pAd->CommonCfg.TpcReqTabLock);
+
+	if (pAd->CommonCfg.pTpcReqTab)
+		kfree(pAd->CommonCfg.pTpcReqTab);
+	pAd->CommonCfg.pTpcReqTab = NULL;
+
+	return;
+}
+
+static PTPC_REQ_ENTRY TpcReqLookUp(
+	IN PRTMP_ADAPTER	pAd,
+	IN UINT8			DialogToken)
+{
+	UINT HashIdx;
+	PTPC_REQ_TAB pTab = pAd->CommonCfg.pTpcReqTab;
+	PTPC_REQ_ENTRY pEntry = NULL;
+	PTPC_REQ_ENTRY pPrevEntry = NULL;
+
+	if (pTab == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: pTpcReqTab doesn't exist.\n", __FUNCTION__));
+		return NULL;
+	}
+
+	RTMP_SEM_LOCK(&pAd->CommonCfg.TpcReqTabLock);
+
+	HashIdx = TPC_DIALOGTOKEN_HASH_INDEX(DialogToken);
+	pEntry = pTab->Hash[HashIdx];
+
+	while (pEntry)
+	{
+		if (pEntry->DialogToken == DialogToken)
+			break;
+		else
+		{
+			pPrevEntry = pEntry;
+			pEntry = pEntry->pNext;
+		}
+	}
+
+	RTMP_SEM_UNLOCK(&pAd->CommonCfg.TpcReqTabLock);
+
+	return pEntry;
+}
+
+
+static PTPC_REQ_ENTRY TpcReqInsert(
+	IN PRTMP_ADAPTER	pAd,
+	IN UINT8			DialogToken)
+{
+	INT i;
+	ULONG HashIdx;
+	PTPC_REQ_TAB pTab = pAd->CommonCfg.pTpcReqTab;
+	PTPC_REQ_ENTRY pEntry = NULL, pCurrEntry;
+	ULONG Now;
+
+	if(pTab == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: pTpcReqTab doesn't exist.\n", __FUNCTION__));
+		return NULL;
+	}
+
+	pEntry = TpcReqLookUp(pAd, DialogToken);
+	if (pEntry == NULL)
+	{
+		RTMP_SEM_LOCK(&pAd->CommonCfg.TpcReqTabLock);
+		for (i = 0; i < MAX_TPC_REQ_TAB_SIZE; i++)
+		{
+			NdisGetSystemUpTime(&Now);
+			pEntry = &pTab->Content[i];
+
+			if ((pEntry->Valid == TRUE)
+				&& RTMP_TIME_AFTER((unsigned long)Now, (unsigned long)(pEntry->lastTime + TPC_REQ_AGE_OUT)))
+			{
+				PTPC_REQ_ENTRY pPrevEntry = NULL;
+				ULONG HashIdx = TPC_DIALOGTOKEN_HASH_INDEX(pEntry->DialogToken);
+				PTPC_REQ_ENTRY pProbeEntry = pTab->Hash[HashIdx];
+
+				// update Hash list
+				do
+				{
+					if (pProbeEntry == pEntry)
+					{
+						if (pPrevEntry == NULL)
+						{
+							pTab->Hash[HashIdx] = pEntry->pNext;
+						}
+						else
+						{
+							pPrevEntry->pNext = pEntry->pNext;
+						}
+						break;
+					}
+
+					pPrevEntry = pProbeEntry;
+					pProbeEntry = pProbeEntry->pNext;
+				} while (pProbeEntry);
+
+				NdisZeroMemory(pEntry, sizeof(TPC_REQ_ENTRY));
+				pTab->Size--;
+
+				break;
+			}
+
+			if (pEntry->Valid == FALSE)
+				break;
+		}
+
+		if (i < MAX_TPC_REQ_TAB_SIZE)
+		{
+			NdisGetSystemUpTime(&Now);
+			pEntry->lastTime = Now;
+			pEntry->Valid = TRUE;
+			pEntry->DialogToken = DialogToken;
+			pTab->Size++;
+		}
+		else
+		{
+			pEntry = NULL;
+			DBGPRINT(RT_DEBUG_ERROR, ("%s: pTpcReqTab tab full.\n", __FUNCTION__));
+		}
+
+		// add this Neighbor entry into HASH table
+		if (pEntry)
+		{
+			HashIdx = TPC_DIALOGTOKEN_HASH_INDEX(DialogToken);
+			if (pTab->Hash[HashIdx] == NULL)
+			{
+				pTab->Hash[HashIdx] = pEntry;
+			}
+			else
+			{
+				pCurrEntry = pTab->Hash[HashIdx];
+				while (pCurrEntry->pNext != NULL)
+					pCurrEntry = pCurrEntry->pNext;
+				pCurrEntry->pNext = pEntry;
+			}
+		}
+
+		RTMP_SEM_UNLOCK(&pAd->CommonCfg.TpcReqTabLock);
+	}
+
+	return pEntry;
+}
+
+static VOID TpcReqDelete(
+	IN PRTMP_ADAPTER	pAd,
+	IN UINT8			DialogToken)
+{
+	PTPC_REQ_TAB pTab = pAd->CommonCfg.pTpcReqTab;
+	PTPC_REQ_ENTRY pEntry = NULL;
+
+	if(pTab == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: pTpcReqTab doesn't exist.\n", __FUNCTION__));
+		return;
+	}
+
+	// if empty, return
+	if (pTab->Size == 0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("pTpcReqTab empty.\n"));
+		return;
+	}
+
+	pEntry = TpcReqLookUp(pAd, DialogToken);
+	if (pEntry != NULL)
+	{
+		PTPC_REQ_ENTRY pPrevEntry = NULL;
+		ULONG HashIdx = TPC_DIALOGTOKEN_HASH_INDEX(pEntry->DialogToken);
+		PTPC_REQ_ENTRY pProbeEntry = pTab->Hash[HashIdx];
+
+		RTMP_SEM_LOCK(&pAd->CommonCfg.TpcReqTabLock);
+		// update Hash list
+		do
+		{
+			if (pProbeEntry == pEntry)
+			{
+				if (pPrevEntry == NULL)
+				{
+					pTab->Hash[HashIdx] = pEntry->pNext;
+				}
+				else
+				{
+					pPrevEntry->pNext = pEntry->pNext;
+				}
+				break;
+			}
+
+			pPrevEntry = pProbeEntry;
+			pProbeEntry = pProbeEntry->pNext;
+		} while (pProbeEntry);
+
+		NdisZeroMemory(pEntry, sizeof(TPC_REQ_ENTRY));
+		pTab->Size--;
+
+		RTMP_SEM_UNLOCK(&pAd->CommonCfg.TpcReqTabLock);
+	}
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Get Current TimeS tamp.
+
+	Parametrs:
+
+	Return	: Current Time Stamp.
+	==========================================================================
+ */
+static UINT64 GetCurrentTimeStamp(
+	IN PRTMP_ADAPTER pAd)
+{
+	// get current time stamp.
+	return 0;
+}
+
+/*
+	==========================================================================
+	Description:
+		Get Current Transmit Power.
+
+	Parametrs:
+
+	Return	: Current Time Stamp.
+	==========================================================================
+ */
+static UINT8 GetCurTxPwr(
+	IN PRTMP_ADAPTER pAd,
+	IN UINT8 Wcid)
+{
+	return 16; /* 16 dBm */
+}
+
+/*
+	==========================================================================
+	Description:
+		Insert Dialog Token into frame.
+
+	Parametrs:
+		1. frame buffer pointer.
+		2. frame length.
+		3. Dialog token.
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID InsertDialogToken(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen,
+	IN UINT8 DialogToken)
+{
+	ULONG TempLen;
+	MakeOutgoingFrame(pFrameBuf,	&TempLen,
+					1,				&DialogToken,
+					END_OF_ARGS);
+
+	*pFrameLen = *pFrameLen + TempLen;
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Insert TPC Request IE into frame.
+
+	Parametrs:
+		1. frame buffer pointer.
+		2. frame length.
+
+	Return	: None.
+	==========================================================================
+ */
+ static VOID InsertTpcReqIE(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen)
+{
+	ULONG TempLen;
+	ULONG Len = 0;
+	UINT8 ElementID = IE_TPC_REQUEST;
+
+	MakeOutgoingFrame(pFrameBuf,					&TempLen,
+						1,							&ElementID,
+						1,							&Len,
+						END_OF_ARGS);
+
+	*pFrameLen = *pFrameLen + TempLen;
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Insert TPC Report IE into frame.
+
+	Parametrs:
+		1. frame buffer pointer.
+		2. frame length.
+		3. Transmit Power.
+		4. Link Margin.
+
+	Return	: None.
+	==========================================================================
+ */
+ static VOID InsertTpcReportIE(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen,
+	IN UINT8 TxPwr,
+	IN UINT8 LinkMargin)
+{
+	ULONG TempLen;
+	ULONG Len = sizeof(TPC_REPORT_INFO);
+	UINT8 ElementID = IE_TPC_REPORT;
+	TPC_REPORT_INFO TpcReportIE;
+
+	TpcReportIE.TxPwr = TxPwr;
+	TpcReportIE.LinkMargin = LinkMargin;
+
+	MakeOutgoingFrame(pFrameBuf,					&TempLen,
+						1,							&ElementID,
+						1,							&Len,
+						Len,						&TpcReportIE,
+						END_OF_ARGS);
+
+	*pFrameLen = *pFrameLen + TempLen;
+
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Insert Channel Switch Announcement IE into frame.
+
+	Parametrs:
+		1. frame buffer pointer.
+		2. frame length.
+		3. channel switch announcement mode.
+		4. new selected channel.
+		5. channel switch announcement count.
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID InsertChSwAnnIE(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen,
+	IN UINT8 ChSwMode,
+	IN UINT8 NewChannel,
+	IN UINT8 ChSwCnt)
+{
+	ULONG TempLen;
+	ULONG Len = sizeof(CH_SW_ANN_INFO);
+	UINT8 ElementID = IE_CHANNEL_SWITCH_ANNOUNCEMENT;
+	CH_SW_ANN_INFO ChSwAnnIE;
+
+	ChSwAnnIE.ChSwMode = ChSwMode;
+	ChSwAnnIE.Channel = NewChannel;
+	ChSwAnnIE.ChSwCnt = ChSwCnt;
+
+	MakeOutgoingFrame(pFrameBuf,				&TempLen,
+						1,						&ElementID,
+						1,						&Len,
+						Len,					&ChSwAnnIE,
+						END_OF_ARGS);
+
+	*pFrameLen = *pFrameLen + TempLen;
+
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Insert Measure Request IE into frame.
+
+	Parametrs:
+		1. frame buffer pointer.
+		2. frame length.
+		3. Measure Token.
+		4. Measure Request Mode.
+		5. Measure Request Type.
+		6. Measure Channel.
+		7. Measure Start time.
+		8. Measure Duration.
+
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID InsertMeasureReqIE(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen,
+	IN PMEASURE_REQ_INFO pMeasureReqIE)
+{
+	ULONG TempLen;
+	UINT8 Len = sizeof(MEASURE_REQ_INFO);
+	UINT8 ElementID = IE_MEASUREMENT_REQUEST;
+
+	MakeOutgoingFrame(pFrameBuf,					&TempLen,
+						1,							&ElementID,
+						1,							&Len,
+						Len,						pMeasureReqIE,
+						END_OF_ARGS);
+
+	*pFrameLen = *pFrameLen + TempLen;
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Insert Measure Report IE into frame.
+
+	Parametrs:
+		1. frame buffer pointer.
+		2. frame length.
+		3. Measure Token.
+		4. Measure Request Mode.
+		5. Measure Request Type.
+		6. Length of Report Infomation
+		7. Pointer of Report Infomation Buffer.
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID InsertMeasureReportIE(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen,
+	IN PMEASURE_REPORT_INFO pMeasureReportIE,
+	IN UINT8 ReportLnfoLen,
+	IN PUINT8 pReportInfo)
+{
+	ULONG TempLen;
+	ULONG Len;
+	UINT8 ElementID = IE_MEASUREMENT_REPORT;
+
+	Len = sizeof(MEASURE_REPORT_INFO) + ReportLnfoLen;
+
+	MakeOutgoingFrame(pFrameBuf,					&TempLen,
+						1,							&ElementID,
+						1,							&Len,
+						Len,						pMeasureReportIE,
+						END_OF_ARGS);
+
+	*pFrameLen = *pFrameLen + TempLen;
+
+	if ((ReportLnfoLen > 0) && (pReportInfo != NULL))
+	{
+		MakeOutgoingFrame(pFrameBuf + *pFrameLen,		&TempLen,
+							ReportLnfoLen,				pReportInfo,
+							END_OF_ARGS);
+
+		*pFrameLen = *pFrameLen + TempLen;
+	}
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Prepare Measurement request action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueMeasurementReq(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UINT8 MeasureToken,
+	IN UINT8 MeasureReqMode,
+	IN UINT8 MeasureReqType,
+	IN UINT8 MeasureCh,
+	IN UINT16 MeasureDuration)
+{
+	PUCHAR pOutBuffer = NULL;
+	NDIS_STATUS NStatus;
+	ULONG FrameLen;
+	HEADER_802_11 ActHdr;
+	MEASURE_REQ_INFO MeasureReqIE;
+	UINT8 RmReqDailogToken = RandomByte(pAd);
+	UINT64 MeasureStartTime = GetCurrentTimeStamp(pAd);
+
+	// build action frame header.
+	MgtMacHeaderInit(pAd, &ActHdr, SUBTYPE_ACTION, 0, pDA,
+						pAd->CurrentAddress);
+
+	NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+	if(NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("%s() allocate memory failed \n", __FUNCTION__));
+		return;
+	}
+	NdisMoveMemory(pOutBuffer, (PCHAR)&ActHdr, sizeof(HEADER_802_11));
+	FrameLen = sizeof(HEADER_802_11);
+
+	InsertActField(pAd, (pOutBuffer + FrameLen), &FrameLen, CATEGORY_SPECTRUM, SPEC_MRQ);
+
+	// fill Dialog Token
+	InsertDialogToken(pAd, (pOutBuffer + FrameLen), &FrameLen, MeasureToken);
+
+	// prepare Measurement IE.
+	NdisZeroMemory(&MeasureReqIE, sizeof(MEASURE_REQ_INFO));
+	MeasureReqIE.Token = RmReqDailogToken;
+	MeasureReqIE.ReqMode.word = MeasureReqMode;
+	MeasureReqIE.ReqType = MeasureReqType;
+	MeasureReqIE.MeasureReq.ChNum = MeasureCh;
+	MeasureReqIE.MeasureReq.MeasureStartTime = cpu2le64(MeasureStartTime);
+	MeasureReqIE.MeasureReq.MeasureDuration = cpu2le16(MeasureDuration);
+	InsertMeasureReqIE(pAd, (pOutBuffer + FrameLen), &FrameLen, &MeasureReqIE);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Prepare Measurement report action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueMeasurementRep(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UINT8 DialogToken,
+	IN UINT8 MeasureToken,
+	IN UINT8 MeasureReqMode,
+	IN UINT8 MeasureReqType,
+	IN UINT8 ReportInfoLen,
+	IN PUINT8 pReportInfo)
+{
+	PUCHAR pOutBuffer = NULL;
+	NDIS_STATUS NStatus;
+	ULONG FrameLen;
+	HEADER_802_11 ActHdr;
+	MEASURE_REPORT_INFO MeasureRepIE;
+
+	// build action frame header.
+	MgtMacHeaderInit(pAd, &ActHdr, SUBTYPE_ACTION, 0, pDA,
+						pAd->CurrentAddress);
+
+	NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+	if(NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("%s() allocate memory failed \n", __FUNCTION__));
+		return;
+	}
+	NdisMoveMemory(pOutBuffer, (PCHAR)&ActHdr, sizeof(HEADER_802_11));
+	FrameLen = sizeof(HEADER_802_11);
+
+	InsertActField(pAd, (pOutBuffer + FrameLen), &FrameLen, CATEGORY_SPECTRUM, SPEC_MRP);
+
+	// fill Dialog Token
+	InsertDialogToken(pAd, (pOutBuffer + FrameLen), &FrameLen, DialogToken);
+
+	// prepare Measurement IE.
+	NdisZeroMemory(&MeasureRepIE, sizeof(MEASURE_REPORT_INFO));
+	MeasureRepIE.Token = MeasureToken;
+	MeasureRepIE.ReportMode.word = MeasureReqMode;
+	MeasureRepIE.ReportType = MeasureReqType;
+	InsertMeasureReportIE(pAd, (pOutBuffer + FrameLen), &FrameLen, &MeasureRepIE, ReportInfoLen, pReportInfo);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Prepare TPC Request action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueTPCReq(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UCHAR DialogToken)
+{
+	PUCHAR pOutBuffer = NULL;
+	NDIS_STATUS NStatus;
+	ULONG FrameLen;
+
+	HEADER_802_11 ActHdr;
+
+	// build action frame header.
+	MgtMacHeaderInit(pAd, &ActHdr, SUBTYPE_ACTION, 0, pDA,
+						pAd->CurrentAddress);
+
+	NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+	if(NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("%s() allocate memory failed \n", __FUNCTION__));
+		return;
+	}
+	NdisMoveMemory(pOutBuffer, (PCHAR)&ActHdr, sizeof(HEADER_802_11));
+	FrameLen = sizeof(HEADER_802_11);
+
+	InsertActField(pAd, (pOutBuffer + FrameLen), &FrameLen, CATEGORY_SPECTRUM, SPEC_TPCRQ);
+
+	// fill Dialog Token
+	InsertDialogToken(pAd, (pOutBuffer + FrameLen), &FrameLen, DialogToken);
+
+	// Insert TPC Request IE.
+	InsertTpcReqIE(pAd, (pOutBuffer + FrameLen), &FrameLen);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Prepare TPC Report action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueTPCRep(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UINT8 DialogToken,
+	IN UINT8 TxPwr,
+	IN UINT8 LinkMargin)
+{
+	PUCHAR pOutBuffer = NULL;
+	NDIS_STATUS NStatus;
+	ULONG FrameLen;
+
+	HEADER_802_11 ActHdr;
+
+	// build action frame header.
+	MgtMacHeaderInit(pAd, &ActHdr, SUBTYPE_ACTION, 0, pDA,
+						pAd->CurrentAddress);
+
+	NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+	if(NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("%s() allocate memory failed \n", __FUNCTION__));
+		return;
+	}
+	NdisMoveMemory(pOutBuffer, (PCHAR)&ActHdr, sizeof(HEADER_802_11));
+	FrameLen = sizeof(HEADER_802_11);
+
+	InsertActField(pAd, (pOutBuffer + FrameLen), &FrameLen, CATEGORY_SPECTRUM, SPEC_TPCRP);
+
+	// fill Dialog Token
+	InsertDialogToken(pAd, (pOutBuffer + FrameLen), &FrameLen, DialogToken);
+
+	// Insert TPC Request IE.
+	InsertTpcReportIE(pAd, (pOutBuffer + FrameLen), &FrameLen, TxPwr, LinkMargin);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Prepare Channel Switch Announcement action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+		2. Channel switch announcement mode.
+		2. a New selected channel.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueChSwAnn(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UINT8 ChSwMode,
+	IN UINT8 NewCh)
+{
+	PUCHAR pOutBuffer = NULL;
+	NDIS_STATUS NStatus;
+	ULONG FrameLen;
+
+	HEADER_802_11 ActHdr;
+
+	// build action frame header.
+	MgtMacHeaderInit(pAd, &ActHdr, SUBTYPE_ACTION, 0, pDA,
+						pAd->CurrentAddress);
+
+	NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+	if(NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("%s() allocate memory failed \n", __FUNCTION__));
+		return;
+	}
+	NdisMoveMemory(pOutBuffer, (PCHAR)&ActHdr, sizeof(HEADER_802_11));
+	FrameLen = sizeof(HEADER_802_11);
+
+	InsertActField(pAd, (pOutBuffer + FrameLen), &FrameLen, CATEGORY_SPECTRUM, SPEC_CHANNEL_SWITCH);
+
+	InsertChSwAnnIE(pAd, (pOutBuffer + FrameLen), &FrameLen, ChSwMode, NewCh, 0);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	return;
+}
+
+static BOOLEAN DfsRequirementCheck(
+	IN PRTMP_ADAPTER pAd,
+	IN UINT8 Channel)
+{
+	BOOLEAN Result = FALSE;
+	INT i;
+
+	do
+	{
+		// check DFS procedure is running.
+		// make sure DFS procedure won't start twice.
+		if (pAd->CommonCfg.RadarDetect.RDMode != RD_NORMAL_MODE)
+		{
+			Result = FALSE;
+			break;
+		}
+
+		// check the new channel carried from Channel Switch Announcemnet is valid.
+		for (i=0; i<pAd->ChannelListNum; i++)
+		{
+			if ((Channel == pAd->ChannelList[i].Channel)
+				&&(pAd->ChannelList[i].RemainingTimeForUse == 0))
+			{
+				// found radar signal in the channel. the channel can't use at least for 30 minutes.
+				pAd->ChannelList[i].RemainingTimeForUse = 1800;//30 min = 1800 sec
+				Result = TRUE;
+				break;
+			}
+		}
+	} while(FALSE);
+
+	return Result;
+}
+
+VOID NotifyChSwAnnToPeerAPs(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pRA,
+	IN PUCHAR pTA,
+	IN UINT8 ChSwMode,
+	IN UINT8 Channel)
+{
+#ifdef WDS_SUPPORT
+	if (!((pRA[0] & 0xff) == 0xff)) // is pRA a broadcase address.
+	{
+		INT i;
+		// info neighbor APs that Radar signal found throgh WDS link.
+		for (i = 0; i < MAX_WDS_ENTRY; i++)
+		{
+			if (ValidWdsEntry(pAd, i))
+			{
+				PUCHAR pDA = pAd->WdsTab.WdsEntry[i].PeerWdsAddr;
+
+				// DA equal to SA. have no necessary orignal AP which found Radar signal.
+				if (MAC_ADDR_EQUAL(pTA, pDA))
+					continue;
+
+				// send Channel Switch Action frame to info Neighbro APs.
+				EnqueueChSwAnn(pAd, pDA, ChSwMode, Channel);
+			}
+		}
+	}
+#endif // WDS_SUPPORT //
+}
+
+static VOID StartDFSProcedure(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR Channel,
+	IN UINT8 ChSwMode)
+{
+	// start DFS procedure
+	pAd->CommonCfg.Channel = Channel;
+#ifdef DOT11_N_SUPPORT
+	N_ChannelCheck(pAd);
+#endif // DOT11_N_SUPPORT //
+	pAd->CommonCfg.RadarDetect.RDMode = RD_SWITCHING_MODE;
+	pAd->CommonCfg.RadarDetect.CSCount = 0;
+}
+
+/*
+	==========================================================================
+	Description:
+		Channel Switch Announcement action frame sanity check.
+
+	Parametrs:
+		1. MLME message containing the received frame
+		2. message length.
+		3. Channel switch announcement infomation buffer.
+
+
+	Return	: None.
+	==========================================================================
+ */
+
+/*
+  Channel Switch Announcement IE.
+  +----+-----+-----------+------------+-----------+
+  | ID | Len |Ch Sw Mode | New Ch Num | Ch Sw Cnt |
+  +----+-----+-----------+------------+-----------+
+    1    1        1           1            1
+*/
+static BOOLEAN PeerChSwAnnSanity(
+	IN PRTMP_ADAPTER pAd,
+	IN VOID *pMsg,
+	IN ULONG MsgLen,
+	OUT PCH_SW_ANN_INFO pChSwAnnInfo)
+{
+	PFRAME_802_11 Fr = (PFRAME_802_11)pMsg;
+	PUCHAR pFramePtr = Fr->Octet;
+	BOOLEAN result = FALSE;
+	PEID_STRUCT eid_ptr;
+
+	// skip 802.11 header.
+	MsgLen -= sizeof(HEADER_802_11);
+
+	// skip category and action code.
+	pFramePtr += 2;
+	MsgLen -= 2;
+
+	if (pChSwAnnInfo == NULL)
+		return result;
+
+	eid_ptr = (PEID_STRUCT)pFramePtr;
+	while (((UCHAR*)eid_ptr + eid_ptr->Len + 1) < ((PUCHAR)pFramePtr + MsgLen))
+	{
+		switch(eid_ptr->Eid)
+		{
+			case IE_CHANNEL_SWITCH_ANNOUNCEMENT:
+				NdisMoveMemory(&pChSwAnnInfo->ChSwMode, eid_ptr->Octet, 1);
+				NdisMoveMemory(&pChSwAnnInfo->Channel, eid_ptr->Octet + 1, 1);
+				NdisMoveMemory(&pChSwAnnInfo->ChSwCnt, eid_ptr->Octet + 2, 1);
+
+				result = TRUE;
+                break;
+
+			default:
+				break;
+		}
+		eid_ptr = (PEID_STRUCT)((UCHAR*)eid_ptr + 2 + eid_ptr->Len);
+	}
+
+	return result;
+}
+
+/*
+	==========================================================================
+	Description:
+		Measurement request action frame sanity check.
+
+	Parametrs:
+		1. MLME message containing the received frame
+		2. message length.
+		3. Measurement request infomation buffer.
+
+	Return	: None.
+	==========================================================================
+ */
+static BOOLEAN PeerMeasureReqSanity(
+	IN PRTMP_ADAPTER pAd,
+	IN VOID *pMsg,
+	IN ULONG MsgLen,
+	OUT PUINT8 pDialogToken,
+	OUT PMEASURE_REQ_INFO pMeasureReqInfo)
+{
+	PFRAME_802_11 Fr = (PFRAME_802_11)pMsg;
+	PUCHAR pFramePtr = Fr->Octet;
+	BOOLEAN result = FALSE;
+	PEID_STRUCT eid_ptr;
+	PUCHAR ptr;
+	UINT64 MeasureStartTime;
+	UINT16 MeasureDuration;
+
+	// skip 802.11 header.
+	MsgLen -= sizeof(HEADER_802_11);
+
+	// skip category and action code.
+	pFramePtr += 2;
+	MsgLen -= 2;
+
+	if (pMeasureReqInfo == NULL)
+		return result;
+
+	NdisMoveMemory(pDialogToken, pFramePtr, 1);
+	pFramePtr += 1;
+	MsgLen -= 1;
+
+	eid_ptr = (PEID_STRUCT)pFramePtr;
+	while (((UCHAR*)eid_ptr + eid_ptr->Len + 1) < ((PUCHAR)pFramePtr + MsgLen))
+	{
+		switch(eid_ptr->Eid)
+		{
+			case IE_MEASUREMENT_REQUEST:
+				NdisMoveMemory(&pMeasureReqInfo->Token, eid_ptr->Octet, 1);
+				NdisMoveMemory(&pMeasureReqInfo->ReqMode.word, eid_ptr->Octet + 1, 1);
+				NdisMoveMemory(&pMeasureReqInfo->ReqType, eid_ptr->Octet + 2, 1);
+				ptr = eid_ptr->Octet + 3;
+				NdisMoveMemory(&pMeasureReqInfo->MeasureReq.ChNum, ptr, 1);
+				NdisMoveMemory(&MeasureStartTime, ptr + 1, 8);
+				pMeasureReqInfo->MeasureReq.MeasureStartTime = SWAP64(MeasureStartTime);
+				NdisMoveMemory(&MeasureDuration, ptr + 9, 2);
+				pMeasureReqInfo->MeasureReq.MeasureDuration = SWAP16(MeasureDuration);
+
+				result = TRUE;
+				break;
+
+			default:
+				break;
+		}
+		eid_ptr = (PEID_STRUCT)((UCHAR*)eid_ptr + 2 + eid_ptr->Len);
+	}
+
+	return result;
+}
+
+/*
+	==========================================================================
+	Description:
+		Measurement report action frame sanity check.
+
+	Parametrs:
+		1. MLME message containing the received frame
+		2. message length.
+		3. Measurement report infomation buffer.
+		4. basic report infomation buffer.
+
+	Return	: None.
+	==========================================================================
+ */
+
+/*
+  Measurement Report IE.
+  +----+-----+-------+-------------+--------------+----------------+
+  | ID | Len | Token | Report Mode | Measure Type | Measure Report |
+  +----+-----+-------+-------------+--------------+----------------+
+    1     1      1          1             1            variable
+
+  Basic Report.
+  +--------+------------+----------+-----+
+  | Ch Num | Start Time | Duration | Map |
+  +--------+------------+----------+-----+
+      1          8           2        1
+
+  Map Field Bit Format.
+  +-----+---------------+---------------------+-------+------------+----------+
+  | Bss | OFDM Preamble | Unidentified signal | Radar | Unmeasured | Reserved |
+  +-----+---------------+---------------------+-------+------------+----------+
+     0          1                  2              3         4          5-7
+*/
+static BOOLEAN PeerMeasureReportSanity(
+	IN PRTMP_ADAPTER pAd,
+	IN VOID *pMsg,
+	IN ULONG MsgLen,
+	OUT PUINT8 pDialogToken,
+	OUT PMEASURE_REPORT_INFO pMeasureReportInfo,
+	OUT PUINT8 pReportBuf)
+{
+	PFRAME_802_11 Fr = (PFRAME_802_11)pMsg;
+	PUCHAR pFramePtr = Fr->Octet;
+	BOOLEAN result = FALSE;
+	PEID_STRUCT eid_ptr;
+	PUCHAR ptr;
+
+	// skip 802.11 header.
+	MsgLen -= sizeof(HEADER_802_11);
+
+	// skip category and action code.
+	pFramePtr += 2;
+	MsgLen -= 2;
+
+	if (pMeasureReportInfo == NULL)
+		return result;
+
+	NdisMoveMemory(pDialogToken, pFramePtr, 1);
+	pFramePtr += 1;
+	MsgLen -= 1;
+
+	eid_ptr = (PEID_STRUCT)pFramePtr;
+	while (((UCHAR*)eid_ptr + eid_ptr->Len + 1) < ((PUCHAR)pFramePtr + MsgLen))
+	{
+		switch(eid_ptr->Eid)
+		{
+			case IE_MEASUREMENT_REPORT:
+				NdisMoveMemory(&pMeasureReportInfo->Token, eid_ptr->Octet, 1);
+				NdisMoveMemory(&pMeasureReportInfo->ReportMode, eid_ptr->Octet + 1, 1);
+				NdisMoveMemory(&pMeasureReportInfo->ReportType, eid_ptr->Octet + 2, 1);
+				if (pMeasureReportInfo->ReportType == RM_BASIC)
+				{
+					PMEASURE_BASIC_REPORT pReport = (PMEASURE_BASIC_REPORT)pReportBuf;
+					ptr = eid_ptr->Octet + 3;
+					NdisMoveMemory(&pReport->ChNum, ptr, 1);
+					NdisMoveMemory(&pReport->MeasureStartTime, ptr + 1, 8);
+					NdisMoveMemory(&pReport->MeasureDuration, ptr + 9, 2);
+					NdisMoveMemory(&pReport->Map, ptr + 11, 1);
+
+				}
+				else if (pMeasureReportInfo->ReportType == RM_CCA)
+				{
+					PMEASURE_CCA_REPORT pReport = (PMEASURE_CCA_REPORT)pReportBuf;
+					ptr = eid_ptr->Octet + 3;
+					NdisMoveMemory(&pReport->ChNum, ptr, 1);
+					NdisMoveMemory(&pReport->MeasureStartTime, ptr + 1, 8);
+					NdisMoveMemory(&pReport->MeasureDuration, ptr + 9, 2);
+					NdisMoveMemory(&pReport->CCA_Busy_Fraction, ptr + 11, 1);
+
+				}
+				else if (pMeasureReportInfo->ReportType == RM_RPI_HISTOGRAM)
+				{
+					PMEASURE_RPI_REPORT pReport = (PMEASURE_RPI_REPORT)pReportBuf;
+					ptr = eid_ptr->Octet + 3;
+					NdisMoveMemory(&pReport->ChNum, ptr, 1);
+					NdisMoveMemory(&pReport->MeasureStartTime, ptr + 1, 8);
+					NdisMoveMemory(&pReport->MeasureDuration, ptr + 9, 2);
+					NdisMoveMemory(&pReport->RPI_Density, ptr + 11, 8);
+				}
+				result = TRUE;
+                break;
+
+			default:
+				break;
+		}
+		eid_ptr = (PEID_STRUCT)((UCHAR*)eid_ptr + 2 + eid_ptr->Len);
+	}
+
+	return result;
+}
+
+/*
+	==========================================================================
+	Description:
+		TPC Request action frame sanity check.
+
+	Parametrs:
+		1. MLME message containing the received frame
+		2. message length.
+		3. Dialog Token.
+
+	Return	: None.
+	==========================================================================
+ */
+static BOOLEAN PeerTpcReqSanity(
+	IN PRTMP_ADAPTER pAd,
+	IN VOID *pMsg,
+	IN ULONG MsgLen,
+	OUT PUINT8 pDialogToken)
+{
+	PFRAME_802_11 Fr = (PFRAME_802_11)pMsg;
+	PUCHAR pFramePtr = Fr->Octet;
+	BOOLEAN result = FALSE;
+	PEID_STRUCT eid_ptr;
+
+	MsgLen -= sizeof(HEADER_802_11);
+
+	// skip category and action code.
+	pFramePtr += 2;
+	MsgLen -= 2;
+
+	if (pDialogToken == NULL)
+		return result;
+
+	NdisMoveMemory(pDialogToken, pFramePtr, 1);
+	pFramePtr += 1;
+	MsgLen -= 1;
+
+	eid_ptr = (PEID_STRUCT)pFramePtr;
+	while (((UCHAR*)eid_ptr + eid_ptr->Len + 1) < ((PUCHAR)pFramePtr + MsgLen))
+	{
+		switch(eid_ptr->Eid)
+		{
+			case IE_TPC_REQUEST:
+				result = TRUE;
+                break;
+
+			default:
+				break;
+		}
+		eid_ptr = (PEID_STRUCT)((UCHAR*)eid_ptr + 2 + eid_ptr->Len);
+	}
+
+	return result;
+}
+
+/*
+	==========================================================================
+	Description:
+		TPC Report action frame sanity check.
+
+	Parametrs:
+		1. MLME message containing the received frame
+		2. message length.
+		3. Dialog Token.
+		4. TPC Report IE.
+
+	Return	: None.
+	==========================================================================
+ */
+static BOOLEAN PeerTpcRepSanity(
+	IN PRTMP_ADAPTER pAd,
+	IN VOID *pMsg,
+	IN ULONG MsgLen,
+	OUT PUINT8 pDialogToken,
+	OUT PTPC_REPORT_INFO pTpcRepInfo)
+{
+	PFRAME_802_11 Fr = (PFRAME_802_11)pMsg;
+	PUCHAR pFramePtr = Fr->Octet;
+	BOOLEAN result = FALSE;
+	PEID_STRUCT eid_ptr;
+
+	MsgLen -= sizeof(HEADER_802_11);
+
+	// skip category and action code.
+	pFramePtr += 2;
+	MsgLen -= 2;
+
+	if (pDialogToken == NULL)
+		return result;
+
+	NdisMoveMemory(pDialogToken, pFramePtr, 1);
+	pFramePtr += 1;
+	MsgLen -= 1;
+
+	eid_ptr = (PEID_STRUCT)pFramePtr;
+	while (((UCHAR*)eid_ptr + eid_ptr->Len + 1) < ((PUCHAR)pFramePtr + MsgLen))
+	{
+		switch(eid_ptr->Eid)
+		{
+			case IE_TPC_REPORT:
+				NdisMoveMemory(&pTpcRepInfo->TxPwr, eid_ptr->Octet, 1);
+				NdisMoveMemory(&pTpcRepInfo->LinkMargin, eid_ptr->Octet + 1, 1);
+				result = TRUE;
+                break;
+
+			default:
+				break;
+		}
+		eid_ptr = (PEID_STRUCT)((UCHAR*)eid_ptr + 2 + eid_ptr->Len);
+	}
+
+	return result;
+}
+
+/*
+	==========================================================================
+	Description:
+		Channel Switch Announcement action frame handler.
+
+	Parametrs:
+		Elme - MLME message containing the received frame
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID PeerChSwAnnAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	CH_SW_ANN_INFO ChSwAnnInfo;
+	PFRAME_802_11 pFr = (PFRAME_802_11)Elem->Msg;
+#ifdef CONFIG_STA_SUPPORT
+	UCHAR index = 0, Channel = 0, NewChannel = 0;
+	ULONG Bssidx = 0;
+#endif // CONFIG_STA_SUPPORT //
+
+	NdisZeroMemory(&ChSwAnnInfo, sizeof(CH_SW_ANN_INFO));
+	if (! PeerChSwAnnSanity(pAd, Elem->Msg, Elem->MsgLen, &ChSwAnnInfo))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("Invalid Channel Switch Action Frame.\n"));
+		return;
+	}
+
+
+#ifdef CONFIG_STA_SUPPORT
+	if (pAd->OpMode == OPMODE_STA)
+	{
+		Bssidx = BssTableSearch(&pAd->ScanTab, pFr->Hdr.Addr3, pAd->CommonCfg.Channel);
+		if (Bssidx == BSS_NOT_FOUND)
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("PeerChSwAnnAction - Bssidx is not found\n"));
+			return;
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("\n****Bssidx is %d, Channel = %d\n", index, pAd->ScanTab.BssEntry[Bssidx].Channel));
+		hex_dump("SSID",pAd->ScanTab.BssEntry[Bssidx].Bssid ,6);
+
+		Channel = pAd->CommonCfg.Channel;
+		NewChannel = ChSwAnnInfo.Channel;
+
+		if ((pAd->CommonCfg.bIEEE80211H == 1) && (NewChannel != 0) && (Channel != NewChannel))
+		{
+			// Switching to channel 1 can prevent from rescanning the current channel immediately (by auto reconnection).
+			// In addition, clear the MLME queue and the scan table to discard the RX packets and previous scanning results.
+			AsicSwitchChannel(pAd, 1, FALSE);
+			AsicLockChannel(pAd, 1);
+		    LinkDown(pAd, FALSE);
+			MlmeQueueInit(&pAd->Mlme.Queue);
+			BssTableInit(&pAd->ScanTab);
+		    RTMPusecDelay(1000000);		// use delay to prevent STA do reassoc
+
+			// channel sanity check
+			for (index = 0 ; index < pAd->ChannelListNum; index++)
+			{
+				if (pAd->ChannelList[index].Channel == NewChannel)
+				{
+					pAd->ScanTab.BssEntry[Bssidx].Channel = NewChannel;
+					pAd->CommonCfg.Channel = NewChannel;
+					AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+					AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+					DBGPRINT(RT_DEBUG_TRACE, ("&&&&&&&&&&&&&&&&PeerChSwAnnAction - STA receive channel switch announcement IE (New Channel =%d)\n", NewChannel));
+					break;
+				}
+			}
+
+			if (index >= pAd->ChannelListNum)
+			{
+				DBGPRINT_ERR(("&&&&&&&&&&&&&&&&&&&&&&&&&&PeerChSwAnnAction(can not find New Channel=%d in ChannelList[%d]\n", pAd->CommonCfg.Channel, pAd->ChannelListNum));
+			}
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	return;
+}
+
+
+/*
+	==========================================================================
+	Description:
+		Measurement Request action frame handler.
+
+	Parametrs:
+		Elme - MLME message containing the received frame
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID PeerMeasureReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	PFRAME_802_11 pFr = (PFRAME_802_11)Elem->Msg;
+	UINT8 DialogToken;
+	MEASURE_REQ_INFO MeasureReqInfo;
+	MEASURE_REPORT_MODE ReportMode;
+
+	if(PeerMeasureReqSanity(pAd, Elem->Msg, Elem->MsgLen, &DialogToken, &MeasureReqInfo))
+	{
+		ReportMode.word = 0;
+		ReportMode.field.Incapable = 1;
+		EnqueueMeasurementRep(pAd, pFr->Hdr.Addr2, DialogToken, MeasureReqInfo.Token, ReportMode.word, MeasureReqInfo.ReqType, 0, NULL);
+	}
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Measurement Report action frame handler.
+
+	Parametrs:
+		Elme - MLME message containing the received frame
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID PeerMeasureReportAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	MEASURE_REPORT_INFO MeasureReportInfo;
+	PFRAME_802_11 pFr = (PFRAME_802_11)Elem->Msg;
+	UINT8 DialogToken;
+	PUINT8 pMeasureReportInfo;
+
+//	if (pAd->CommonCfg.bIEEE80211H != TRUE)
+//		return;
+
+	if ((pMeasureReportInfo = kmalloc(sizeof(MEASURE_RPI_REPORT), GFP_ATOMIC)) == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s unable to alloc memory for measure report buffer (size=%d).\n", __FUNCTION__, sizeof(MEASURE_RPI_REPORT)));
+		return;
+	}
+
+	NdisZeroMemory(&MeasureReportInfo, sizeof(MEASURE_REPORT_INFO));
+	NdisZeroMemory(pMeasureReportInfo, sizeof(MEASURE_RPI_REPORT));
+	if (PeerMeasureReportSanity(pAd, Elem->Msg, Elem->MsgLen, &DialogToken, &MeasureReportInfo, pMeasureReportInfo))
+	{
+		do {
+			PMEASURE_REQ_ENTRY pEntry = NULL;
+
+			// Not a autonomous measure report.
+			// check the dialog token field. drop it if the dialog token doesn't match.
+			if ((DialogToken != 0)
+				&& ((pEntry = MeasureReqLookUp(pAd, DialogToken)) == NULL))
+				break;
+
+			if (pEntry != NULL)
+				MeasureReqDelete(pAd, pEntry->DialogToken);
+
+			if (MeasureReportInfo.ReportType == RM_BASIC)
+			{
+				PMEASURE_BASIC_REPORT pBasicReport = (PMEASURE_BASIC_REPORT)pMeasureReportInfo;
+				if ((pBasicReport->Map.field.Radar)
+					&& (DfsRequirementCheck(pAd, pBasicReport->ChNum) == TRUE))
+				{
+					NotifyChSwAnnToPeerAPs(pAd, pFr->Hdr.Addr1, pFr->Hdr.Addr2, 1, pBasicReport->ChNum);
+					StartDFSProcedure(pAd, pBasicReport->ChNum, 1);
+				}
+			}
+		} while (FALSE);
+	}
+	else
+		DBGPRINT(RT_DEBUG_TRACE, ("Invalid Measurement Report Frame.\n"));
+
+	kfree(pMeasureReportInfo);
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		TPC Request action frame handler.
+
+	Parametrs:
+		Elme - MLME message containing the received frame
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID PeerTpcReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	PFRAME_802_11 pFr = (PFRAME_802_11)Elem->Msg;
+	PUCHAR pFramePtr = pFr->Octet;
+	UINT8 DialogToken;
+	UINT8 TxPwr = GetCurTxPwr(pAd, Elem->Wcid);
+	UINT8 LinkMargin = 0;
+	CHAR RealRssi;
+
+	// link margin: Ratio of the received signal power to the minimum desired by the station (STA). The
+	//				STA may incorporate rate information and channel conditions, including interference, into its computation
+	//				of link margin.
+
+	RealRssi = RTMPMaxRssi(pAd, ConvertToRssi(pAd, Elem->Rssi0, RSSI_0),
+								ConvertToRssi(pAd, Elem->Rssi1, RSSI_1),
+								ConvertToRssi(pAd, Elem->Rssi2, RSSI_2));
+
+	// skip Category and action code.
+	pFramePtr += 2;
+
+	// Dialog token.
+	NdisMoveMemory(&DialogToken, pFramePtr, 1);
+
+	LinkMargin = (RealRssi / MIN_RCV_PWR);
+	if (PeerTpcReqSanity(pAd, Elem->Msg, Elem->MsgLen, &DialogToken))
+		EnqueueTPCRep(pAd, pFr->Hdr.Addr2, DialogToken, TxPwr, LinkMargin);
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		TPC Report action frame handler.
+
+	Parametrs:
+		Elme - MLME message containing the received frame
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID PeerTpcRepAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UINT8 DialogToken;
+	TPC_REPORT_INFO TpcRepInfo;
+	PTPC_REQ_ENTRY pEntry = NULL;
+
+	NdisZeroMemory(&TpcRepInfo, sizeof(TPC_REPORT_INFO));
+	if (PeerTpcRepSanity(pAd, Elem->Msg, Elem->MsgLen, &DialogToken, &TpcRepInfo))
+	{
+		if ((pEntry = TpcReqLookUp(pAd, DialogToken)) != NULL)
+		{
+			TpcReqDelete(pAd, pEntry->DialogToken);
+			DBGPRINT(RT_DEBUG_TRACE, ("%s: DialogToken=%x, TxPwr=%d, LinkMargin=%d\n",
+				__FUNCTION__, DialogToken, TpcRepInfo.TxPwr, TpcRepInfo.LinkMargin));
+		}
+	}
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Spectrun action frames Handler such as channel switch annoucement,
+		measurement report, measurement request actions frames.
+
+	Parametrs:
+		Elme - MLME message containing the received frame
+
+	Return	: None.
+	==========================================================================
+ */
+VOID PeerSpectrumAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+
+	UCHAR	Action = Elem->Msg[LENGTH_802_11+1];
+
+	if (pAd->CommonCfg.bIEEE80211H != TRUE)
+		return;
+
+	switch(Action)
+	{
+		case SPEC_MRQ:
+			// current rt2860 unable do such measure specified in Measurement Request.
+			// reject all measurement request.
+			PeerMeasureReqAction(pAd, Elem);
+			break;
+
+		case SPEC_MRP:
+			PeerMeasureReportAction(pAd, Elem);
+			break;
+
+		case SPEC_TPCRQ:
+			PeerTpcReqAction(pAd, Elem);
+			break;
+
+		case SPEC_TPCRP:
+			PeerTpcRepAction(pAd, Elem);
+			break;
+
+		case SPEC_CHANNEL_SWITCH:
+{
+#ifdef DOT11N_DRAFT3
+				SEC_CHA_OFFSET_IE	Secondary;
+				CHA_SWITCH_ANNOUNCE_IE	ChannelSwitch;
+
+				// 802.11h only has Channel Switch Announcement IE.
+				RTMPMoveMemory(&ChannelSwitch, &Elem->Msg[LENGTH_802_11+4], sizeof (CHA_SWITCH_ANNOUNCE_IE));
+
+				// 802.11n D3.03 adds secondary channel offset element in the end.
+				if (Elem->MsgLen ==  (LENGTH_802_11 + 2 + sizeof (CHA_SWITCH_ANNOUNCE_IE) + sizeof (SEC_CHA_OFFSET_IE)))
+				{
+					RTMPMoveMemory(&Secondary, &Elem->Msg[LENGTH_802_11+9], sizeof (SEC_CHA_OFFSET_IE));
+				}
+				else
+				{
+					Secondary.SecondaryChannelOffset = 0;
+				}
+
+				if ((Elem->Msg[LENGTH_802_11+2] == IE_CHANNEL_SWITCH_ANNOUNCEMENT) && (Elem->Msg[LENGTH_802_11+3] == 3))
+				{
+					ChannelSwitchAction(pAd, Elem->Wcid, ChannelSwitch.NewChannel, Secondary.SecondaryChannelOffset);
+				}
+#endif // DOT11N_DRAFT3 //
+}
+			PeerChSwAnnAction(pAd, Elem);
+			break;
+	}
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+
+	Parametrs:
+
+	Return	: None.
+	==========================================================================
+ */
+INT Set_MeasureReq_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UINT Aid = 1;
+	UINT ArgIdx;
+	PUCHAR thisChar;
+
+	MEASURE_REQ_MODE MeasureReqMode;
+	UINT8 MeasureReqToken = RandomByte(pAd);
+	UINT8 MeasureReqType = RM_BASIC;
+	UINT8 MeasureCh = 1;
+
+	ArgIdx = 1;
+	while ((thisChar = strsep((char **)&arg, "-")) != NULL)
+	{
+		switch(ArgIdx)
+		{
+			case 1:	// Aid.
+				Aid = simple_strtol(thisChar, 0, 16);
+				break;
+
+			case 2: // Measurement Request Type.
+				MeasureReqType = simple_strtol(thisChar, 0, 16);
+				if (MeasureReqType > 3)
+				{
+					DBGPRINT(RT_DEBUG_ERROR, ("%s: unknow MeasureReqType(%d)\n", __FUNCTION__, MeasureReqType));
+					return TRUE;
+				}
+				break;
+
+			case 3: // Measurement channel.
+				MeasureCh = simple_strtol(thisChar, 0, 16);
+				break;
+		}
+		ArgIdx++;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s::Aid = %d, MeasureReqType=%d MeasureCh=%d\n", __FUNCTION__, Aid, MeasureReqType, MeasureCh));
+	if (!VALID_WCID(Aid))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: unknow sta of Aid(%d)\n", __FUNCTION__, Aid));
+		return TRUE;
+	}
+
+	MeasureReqMode.word = 0;
+	MeasureReqMode.field.Enable = 1;
+
+	MeasureReqInsert(pAd, MeasureReqToken);
+
+	EnqueueMeasurementReq(pAd, pAd->MacTab.Content[Aid].Addr,
+		MeasureReqToken, MeasureReqMode.word, MeasureReqType, MeasureCh, 2000);
+
+	return TRUE;
+}
+
+INT Set_TpcReq_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UINT Aid;
+
+	UINT8 TpcReqToken = RandomByte(pAd);
+
+	Aid = simple_strtol(arg, 0, 16);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s::Aid = %d\n", __FUNCTION__, Aid));
+	if (!VALID_WCID(Aid))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: unknow sta of Aid(%d)\n", __FUNCTION__, Aid));
+		return TRUE;
+	}
+
+	TpcReqInsert(pAd, TpcReqToken);
+
+	EnqueueTPCReq(pAd, pAd->MacTab.Content[Aid].Addr, TpcReqToken);
+
+	return TRUE;
+}
+
diff --git a/drivers/staging/rt2860/config.mk b/drivers/staging/rt2860/config.mk
new file mode 100644
index 00000000000..f57d7bbffb2
--- /dev/null
+++ b/drivers/staging/rt2860/config.mk
@@ -0,0 +1,245 @@
+# Support ATE function
+HAS_ATE=n
+
+# Support 28xx QA ATE function
+HAS_28xx_QA=n
+
+# Support Wpa_Supplicant
+HAS_WPA_SUPPLICANT=n
+
+# Support Native WpaSupplicant for Network Maganger
+HAS_NATIVE_WPA_SUPPLICANT_SUPPORT=n
+
+#Support Net interface block while Tx-Sw queue full
+HAS_BLOCK_NET_IF=n
+
+#Support DFS function
+HAS_DFS_SUPPORT=n
+
+#Support Carrier-Sense function
+HAS_CS_SUPPORT=n
+
+#ifdef MULTI_CARD
+# Support for Multiple Cards
+HAS_MC_SUPPORT=n
+#endif // MULTI_CARD //
+
+#Support for IEEE802.11e DLS
+HAS_QOS_DLS_SUPPORT=n
+
+#Support for EXT_CHANNEL
+HAS_EXT_BUILD_CHANNEL_LIST=n
+
+#Support for Net-SNMP
+HAS_SNMP_SUPPORT=n
+
+#Support features of Single SKU.
+HAS_SINGLE_SKU_SUPPORT=n
+
+#Support features of 802.11n
+HAS_DOT11_N_SUPPORT=y
+
+
+#################################################
+
+CC := $(CROSS_COMPILE)gcc
+LD := $(CROSS_COMPILE)ld
+
+WFLAGS := -DAGGREGATION_SUPPORT -DPIGGYBACK_SUPPORT -DWMM_SUPPORT  -DLINUX -Wall -Wstrict-prototypes -Wno-trigraphs
+
+
+#################################################
+
+#ifdef CONFIG_STA_SUPPORT
+# config for STA mode
+
+ifeq ($(RT28xx_MODE),STA)
+WFLAGS += -DCONFIG_STA_SUPPORT -DDBG
+
+ifeq ($(HAS_WPA_SUPPLICANT),y)
+WFLAGS += -DWPA_SUPPLICANT_SUPPORT
+endif
+
+ifeq ($(HAS_NATIVE_WPA_SUPPLICANT_SUPPORT),y)
+WFLAGS += -DNATIVE_WPA_SUPPLICANT_SUPPORT
+endif
+
+ifeq ($(HAS_ATE),y)
+WFLAGS += -DRALINK_ATE
+ifeq ($(HAS_28xx_QA),y)
+WFLAGS += -DRALINK_28xx_QA
+endif
+endif
+
+ifeq ($(HAS_SNMP_SUPPORT),y)
+WFLAGS += -DSNMP_SUPPORT
+endif
+
+ifeq ($(HAS_QOS_DLS_SUPPORT),y)
+WFLAGS += -DQOS_DLS_SUPPORT
+endif
+
+ifeq ($(HAS_DOT11_N_SUPPORT),y)
+WFLAGS += -DDOT11_N_SUPPORT
+endif
+
+ifeq ($(HAS_CS_SUPPORT),y)
+WFLAGS += -DCARRIER_DETECTION_SUPPORT
+endif
+
+ifeq ($(HAS_SINGLE_SKU_SUPPORT),y)
+WFLAGS += -DSINGLE_SKU
+endif
+
+endif
+# endif of ifeq ($(RT28xx_MODE),STA)
+#endif // CONFIG_STA_SUPPORT //
+
+#################################################
+
+#################################################
+
+#
+# Common compiler flag
+#
+
+
+ifeq ($(HAS_EXT_BUILD_CHANNEL_LIST),y)
+WFLAGS += -DEXT_BUILD_CHANNEL_LIST
+endif
+
+ifeq ($(CHIPSET),2860)
+WFLAGS +=-DRT2860
+endif
+
+ifeq ($(CHIPSET),2870)
+WFLAGS +=-DRT2870
+endif
+
+ifeq ($(PLATFORM),5VT)
+#WFLAGS += -DCONFIG_5VT_ENHANCE
+endif
+
+ifeq ($(HAS_BLOCK_NET_IF),y)
+WFLAGS += -DBLOCK_NET_IF
+endif
+
+ifeq ($(HAS_DFS_SUPPORT),y)
+WFLAGS += -DDFS_SUPPORT
+endif
+
+#ifdef MULTI_CARD
+ifeq ($(HAS_MC_SUPPORT),y)
+WFLAGS += -DMULTIPLE_CARD_SUPPORT
+endif
+#endif // MULTI_CARD //
+
+ifeq ($(HAS_LLTD),y)
+WFLAGS += -DLLTD_SUPPORT
+endif
+
+ifeq ($(PLATFORM),IXP)
+WFLAGS += -DRT_BIG_ENDIAN
+endif
+
+ifeq ($(PLATFORM),IKANOS_V160)
+WFLAGS += -DRT_BIG_ENDIAN -DIKANOS_VX_1X0
+endif
+
+ifeq ($(PLATFORM),IKANOS_V180)
+WFLAGS += -DRT_BIG_ENDIAN -DIKANOS_VX_1X0
+endif
+
+ifeq ($(PLATFORM),INF_TWINPASS)
+WFLAGS += -DRT_BIG_ENDIAN -DINF_TWINPASS
+endif
+
+ifeq ($(PLATFORM),INF_DANUBE)
+WFLAGS += -DINF_DANUBE -DRT_BIG_ENDIAN
+endif
+
+ifeq ($(PLATFORM),CAVM_OCTEON)
+WFLAGS += -DRT_BIG_ENDIAN
+endif
+
+ifeq ($(PLATFORM),BRCM_6358)
+WFLAGS += -DRT_BIG_ENDIAN
+endif
+
+ifeq ($(PLATFORM),INF_AMAZON_SE)
+#WFLAGS += -DRT_BIG_ENDIAN -DINF_AMAZON_SE -DBG_FT_SUPPORT
+WFLAGS += -DRT_BIG_ENDIAN -DINF_AMAZON_SE
+endif
+
+#kernel build options for 2.4
+# move to Makefile outside LINUX_SRC := /opt/star/kernel/linux-2.4.27-star
+
+ifeq ($(PLATFORM),STAR)
+CFLAGS := -D__KERNEL__ -I$(LINUX_SRC)/include -I$(RT28xx_DIR)/include -Wall -Wstrict-prototypes -Wno-trigraphs -O2 -fno-strict-aliasing -fno-common -Uarm -fno-common -pipe -mapcs-32 -D__LINUX_ARM_ARCH__=4 -march=armv4  -mshort-load-bytes -msoft-float -Uarm -DMODULE -DMODVERSIONS -include $(LINUX_SRC)/include/linux/modversions.h $(WFLAGS)
+
+export CFLAGS
+endif
+
+ifeq ($(PLATFORM),SIGMA)
+CFLAGS := -D__KERNEL__ -I$(RT28xx_DIR)/include -I$(LINUX_SRC)/include -I$(LINUX_SRC)/include/asm/gcc -I$(LINUX_SRC)/include/asm-mips/mach-tango2 -I$(LINUX_SRC)/include/asm-mips/mach-tango2 -DEM86XX_CHIP=EM86XX_CHIPID_TANGO2 -DEM86XX_REVISION=6 -I$(LINUX_SRC)/include/asm-mips/mach-generic -I$(RT2860_DIR)/include -Wall -Wundef -Wstrict-prototypes -Wno-trigraphs -fno-strict-aliasing -fno-common -ffreestanding -O2     -fomit-frame-pointer -G 0 -mno-abicalls -fno-pic -pipe  -mabi=32 -march=mips32r2 -Wa,-32 -Wa,-march=mips32r2 -Wa,-mips32r2 -Wa,--trap -DMODULE $(WFLAGS)
+
+export CFLAGS
+endif
+
+ifeq ($(PLATFORM),SIGMA_8622)
+CFLAGS := -D__KERNEL__ -I$(CROSS_COMPILE_INCLUDE)/include -I$(LINUX_SRC)/include -I$(RT28xx_DIR)/include -Wall -Wstrict-prototypes -Wno-trigraphs -O2 -fno-strict-aliasing -fno-common -fno-common -pipe -fno-builtin -D__linux__ -DNO_MM -mapcs-32 -march=armv4 -mtune=arm7tdmi -msoft-float -DMODULE -mshort-load-bytes -nostdinc -iwithprefix -DMODULE $(WFLAGS)
+export CFLAGS
+endif
+
+ifeq ($(PLATFORM),5VT)
+CFLAGS := -D__KERNEL__ -I$(LINUX_SRC)/include -I$(RT28xx_DIR)/include -mlittle-endian -Wall -Wundef -Wstrict-prototypes -Wno-trigraphs -fno-strict-aliasing -fno-common -O3 -fno-omit-frame-pointer -fno-optimize-sibling-calls -fno-omit-frame-pointer -mapcs -mno-sched-prolog -mabi=apcs-gnu -mno-thumb-interwork -D__LINUX_ARM_ARCH__=5 -march=armv5te -mtune=arm926ej-s --param max-inline-insns-single=40000  -Uarm -Wdeclaration-after-statement -Wno-pointer-sign -DMODULE $(WFLAGS)
+
+export CFLAGS
+endif
+
+ifeq ($(PLATFORM),IKANOS_V160)
+CFLAGS := -D__KERNEL__ -I$(LINUX_SRC)/include -I$(LINUX_SRC)/include/asm/gcc -I$(LINUX_SRC)/include/asm-mips/mach-tango2 -I$(LINUX_SRC)/include/asm-mips/mach-tango2 -I$(LINUX_SRC)/include/asm-mips/mach-generic -I$(RT28xx_DIR)/include -Wall -Wundef -Wstrict-prototypes -Wno-trigraphs -fno-strict-aliasing -fno-common -ffreestanding -O2 -fomit-frame-pointer -G 0 -mno-abicalls -fno-pic -pipe -march=lx4189 -Wa, -DMODULE $(WFLAGS)
+export CFLAGS
+endif
+
+ifeq ($(PLATFORM),IKANOS_V180)
+CFLAGS := -D__KERNEL__ -I$(LINUX_SRC)/include -I$(LINUX_SRC)/include/asm/gcc -I$(LINUX_SRC)/include/asm-mips/mach-tango2 -I$(LINUX_SRC)/include/asm-mips/mach-tango2 -I$(LINUX_SRC)/include/asm-mips/mach-generic -I$(RT28xx_DIR)/include -Wall -Wundef -Wstrict-prototypes -Wno-trigraphs -fno-strict-aliasing -fno-common -ffreestanding -O2 -fomit-frame-pointer -G 0 -mno-abicalls -fno-pic -pipe -mips32r2 -Wa, -DMODULE $(WFLAGS)
+export CFLAGS
+endif
+
+ifeq ($(PLATFORM),INF_TWINPASS)
+CFLAGS := -D__KERNEL__ -DMODULE -I$(LINUX_SRC)/include -I$(RT28xx_DIR)/include -Wall -Wstrict-prototypes -Wno-trigraphs -O2 -fomit-frame-pointer -fno-strict-aliasing -fno-common -G 0 -mno-abicalls -fno-pic -march=4kc -mips32 -Wa,--trap -pipe -mlong-calls $(WFLAGS)
+export CFLAGS
+endif
+
+ifeq ($(PLATFORM),INF_DANUBE)
+CFLAGS := -I$(RT28xx_DIR)/include $(WFLAGS) -Wundef -fno-strict-aliasing -fno-common -ffreestanding -Os -fomit-frame-pointer -G 0 -mno-abicalls -fno-pic -pipe -msoft-float  -mabi=32 -march=mips32 -Wa,-32 -Wa,-march=mips32 -Wa,-mips32 -Wa,--trap -I$(LINUX_SRC)/include/asm-mips/mach-generic
+export CFLAGS
+endif
+
+ifeq ($(PLATFORM),BRCM_6358)
+CFLAGS := $(WFLAGS) -I$(RT28xx_DIR)/include -nostdinc -iwithprefix include -D__KERNEL__ -Wall -Wstrict-prototypes -Wno-trigraphs -fno-strict-aliasing -fno-common -I $(LINUX_SRC)/include/asm/gcc -G 0 -mno-abicalls -fno-pic -pipe  -finline-limit=100000 -mabi=32 -march=mips32 -Wa,-32 -Wa,-march=mips32 -Wa,-mips32 -Wa,--trap -I$(LINUX_SRC)/include/asm-mips/mach-bcm963xx -I$(LINUX_SRC)/include/asm-mips/mach-generic  -Os -fomit-frame-pointer -Wdeclaration-after-statement  -DMODULE -mlong-calls
+export CFLAGS
+endif
+
+ifeq ($(PLATFORM),PC)
+    ifneq (,$(findstring 2.4,$(LINUX_SRC)))
+	# Linux 2.4
+	CFLAGS := -D__KERNEL__ -I$(LINUX_SRC)/include -I$(RT28xx_DIR)/include -O2 -fomit-frame-pointer -fno-strict-aliasing -fno-common -pipe -mpreferred-stack-boundary=2 -march=i686 -DMODULE -DMODVERSIONS -include $(LINUX_SRC)/include/linux/modversions.h $(WFLAGS)
+	export CFLAGS
+    else
+	# Linux 2.6
+	EXTRA_CFLAGS := $(WFLAGS) -I$(RT28xx_DIR)/include
+    endif
+endif
+
+ifeq ($(PLATFORM),IXP)
+        EXTRA_CFLAGS := -v $(WFLAGS) -I$(RT28xx_DIR)/include -mbig-endian
+endif
+
+ifeq ($(PLATFORM),CAVM_OCTEON)
+	EXTRA_CFLAGS := $(WFLAGS) -I$(RT28xx_DIR)/include \
+				    -mabi=64 $(WFLAGS)
+export CFLAGS
+endif
+
diff --git a/drivers/staging/rt2860/dfs.h b/drivers/staging/rt2860/dfs.h
new file mode 100644
index 00000000000..752a6352d9d
--- /dev/null
+++ b/drivers/staging/rt2860/dfs.h
@@ -0,0 +1,100 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    dfs.h
+
+    Abstract:
+    Support DFS function.
+
+    Revision History:
+    Who       When            What
+    --------  ----------      ----------------------------------------------
+    Fonchi    03-12-2007      created
+*/
+
+#define RADAR_PULSE 1
+#define RADAR_WIDTH 2
+
+#define WIDTH_RD_IDLE 0
+#define WIDTH_RD_CHECK 1
+
+
+VOID BbpRadarDetectionStart(
+	IN PRTMP_ADAPTER pAd);
+
+VOID BbpRadarDetectionStop(
+	IN PRTMP_ADAPTER pAd);
+
+VOID RadarDetectionStart(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN CTS_Protect,
+	IN UINT8 CTSPeriod);
+
+VOID RadarDetectionStop(
+	IN PRTMP_ADAPTER	pAd);
+
+VOID RadarDetectPeriodic(
+	IN PRTMP_ADAPTER	pAd);
+
+
+BOOLEAN RadarChannelCheck(
+	IN PRTMP_ADAPTER	pAd,
+	IN UCHAR			Ch);
+
+ULONG JapRadarType(
+	IN PRTMP_ADAPTER pAd);
+
+ULONG RTMPBbpReadRadarDuration(
+	IN PRTMP_ADAPTER	pAd);
+
+ULONG RTMPReadRadarDuration(
+	IN PRTMP_ADAPTER	pAd);
+
+VOID RTMPCleanRadarDuration(
+	IN PRTMP_ADAPTER	pAd);
+
+VOID RTMPPrepareRDCTSFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pDA,
+	IN	ULONG			Duration,
+	IN  UCHAR           RTSRate,
+	IN  ULONG           CTSBaseAddr,
+	IN  UCHAR			FrameGap);
+
+VOID RTMPPrepareRadarDetectParams(
+	IN PRTMP_ADAPTER	pAd);
+
+
+INT Set_ChMovingTime_Proc(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR arg);
+
+INT Set_LongPulseRadarTh_Proc(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR arg);
+
+
diff --git a/drivers/staging/rt2860/leap.h b/drivers/staging/rt2860/leap.h
new file mode 100644
index 00000000000..6818c1ff4d7
--- /dev/null
+++ b/drivers/staging/rt2860/leap.h
@@ -0,0 +1,215 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	leap.h
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+*/
+#ifndef __LEAP_H__
+#define __LEAP_H__
+
+// Messages for Associate state machine
+#define LEAP_MACHINE_BASE                   30
+
+#define LEAP_MSG_REQUEST_IDENTITY           31
+#define LEAP_MSG_REQUEST_LEAP               32
+#define LEAP_MSG_SUCCESS                    33
+#define LEAP_MSG_FAILED                     34
+#define LEAP_MSG_RESPONSE_LEAP              35
+#define LEAP_MSG_EAPOLKEY                   36
+#define LEAP_MSG_UNKNOWN                    37
+#define LEAP_MSG                            38
+//! assoc state-machine states
+#define LEAP_IDLE                           0
+#define LEAP_WAIT_IDENTITY_REQUEST          1
+#define LEAP_WAIT_CHANLLENGE_REQUEST        2
+#define LEAP_WAIT_SUCCESS                   3
+#define LEAP_WAIT_CHANLLENGE_RESPONSE       4
+#define LEAP_WAIT_EAPOLKEY                  5
+
+#define LEAP_REASON_INVALID_AUTH                    0x01
+#define LEAP_REASON_AUTH_TIMEOUT                    0x02
+#define LEAP_REASON_CHALLENGE_FROM_AP_FAILED        0x03
+#define LEAP_REASON_CHALLENGE_TO_AP_FAILED          0x04
+
+#define CISCO_AuthModeLEAP                          0x80
+#define CISCO_AuthModeLEAPNone                      0x00
+#define LEAP_AUTH_TIMEOUT                           30000
+#define LEAP_CHALLENGE_RESPONSE_LENGTH              24
+#define LEAP_CHALLENGE_REQUEST_LENGTH               8
+
+typedef struct _LEAP_EAPOL_HEADER_ {
+    UCHAR       Version;
+    UCHAR       Type;
+    UCHAR       Length[2];
+} LEAP_EAPOL_HEADER, *PLEAP_EAPOL_HEADER;
+
+typedef struct _LEAP_EAPOL_PACKET_ {
+    UCHAR       Code;
+    UCHAR       Identifier;
+    UCHAR       Length[2];
+    UCHAR       Type;
+} LEAP_EAPOL_PACKET, *PLEAP_EAPOL_PACKET;
+
+typedef struct _LEAP_EAP_CONTENTS_ {
+    UCHAR       Version;
+    UCHAR       Reserved;
+    UCHAR       Length;
+} LEAP_EAP_CONTENTS, *PLEAP_EAP_CONTENTS;
+
+/*** EAPOL key ***/
+typedef struct _EAPOL_KEY_HEADER_ {
+    UCHAR       Type;
+    UCHAR       Length[2];
+    UCHAR       Counter[8];
+    UCHAR       IV[16];
+    UCHAR       Index;
+    UCHAR       Signature[16];
+} EAPOL_KEY_HEADER, *PEAPOL_KEY_HEADER;
+
+BOOLEAN LeapMsgTypeSubst(
+    IN  UCHAR   EAPType,
+    OUT ULONG   *MsgType);
+
+VOID LeapMachinePerformAction(
+    IN PRTMP_ADAPTER    pAd,
+    IN STATE_MACHINE    *S,
+    IN MLME_QUEUE_ELEM  *Elem);
+
+VOID LeapMacHeaderInit(
+    IN  PRTMP_ADAPTER       pAd,
+    IN  OUT PHEADER_802_11  pHdr80211,
+    IN  UCHAR               wep,
+    IN  PUCHAR              pAddr3);
+
+VOID LeapStartAction(
+    IN PRTMP_ADAPTER    pAd,
+    IN MLME_QUEUE_ELEM  *Elem);
+
+VOID LeapIdentityAction(
+    IN PRTMP_ADAPTER    pAd,
+    IN MLME_QUEUE_ELEM  *Elem);
+
+VOID LeapPeerChallengeAction(
+    IN PRTMP_ADAPTER    pAd,
+    IN MLME_QUEUE_ELEM  *Elem);
+
+VOID HashPwd(
+    IN  PUCHAR  pwd,
+    IN  INT     pwdlen,
+    OUT PUCHAR  hash);
+
+VOID PeerChallengeResponse(
+    IN  PUCHAR  szChallenge,
+    IN  PUCHAR  smbPasswd,
+    OUT PUCHAR  szResponse);
+
+VOID ParityKey(
+    OUT PUCHAR  szOut,
+    IN  PUCHAR  szIn);
+
+VOID DesKey(
+    OUT ULONG   k[16][2],
+    IN  PUCHAR  key,
+    IN  INT     decrypt);
+
+VOID Des(
+    IN  ULONG   ks[16][2],
+    OUT UCHAR   block[8]);
+
+VOID DesEncrypt(
+    IN  PUCHAR  szClear,
+    IN  PUCHAR  szKey,
+    OUT PUCHAR  szOut);
+
+VOID LeapNetworkChallengeAction(
+    IN PRTMP_ADAPTER    pAd,
+    IN MLME_QUEUE_ELEM  *Elem);
+
+VOID LeapNetworkChallengeResponse(
+    IN PRTMP_ADAPTER    pAd,
+    IN MLME_QUEUE_ELEM  *Elem);
+
+VOID HashpwdHash(
+    IN  PUCHAR  hash,
+    IN  PUCHAR  hashhash);
+
+VOID ProcessSessionKey(
+    OUT PUCHAR  SessionKey,
+    IN  PUCHAR  hash2,
+    IN  PUCHAR  ChallengeToRadius,
+    IN  PUCHAR  ChallengeResponseFromRadius,
+    IN  PUCHAR  ChallengeFromRadius,
+    IN  PUCHAR  ChallengeResponseToRadius);
+
+VOID LeapEapolKeyAction(
+    IN PRTMP_ADAPTER    pAd,
+    IN MLME_QUEUE_ELEM  *Elem);
+
+VOID RogueApTableInit(
+    IN ROGUEAP_TABLE    *Tab);
+
+ULONG RogueApTableSearch(
+    IN ROGUEAP_TABLE    *Tab,
+    IN PUCHAR           pAddr);
+
+VOID RogueApEntrySet(
+    IN  PRTMP_ADAPTER   pAd,
+    OUT ROGUEAP_ENTRY   *pRogueAp,
+    IN PUCHAR           pAddr,
+    IN UCHAR            FaileCode);
+
+ULONG RogueApTableSetEntry(
+    IN  PRTMP_ADAPTER   pAd,
+    OUT ROGUEAP_TABLE  *Tab,
+    IN PUCHAR           pAddr,
+    IN UCHAR            FaileCode);
+
+VOID RogueApTableDeleteEntry(
+    IN OUT ROGUEAP_TABLE *Tab,
+    IN PUCHAR          pAddr);
+
+VOID LeapAuthTimeout(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3);
+
+VOID LeapSendRogueAPReport(
+    IN  PRTMP_ADAPTER   pAd);
+
+BOOLEAN CCKMAssocRspSanity(
+    IN PRTMP_ADAPTER    pAd,
+    IN VOID             *Msg,
+    IN ULONG            MsgLen);
+
+#endif  // __LEAP_H__
diff --git a/drivers/staging/rt2860/link_list.h b/drivers/staging/rt2860/link_list.h
new file mode 100644
index 00000000000..f6521133fd5
--- /dev/null
+++ b/drivers/staging/rt2860/link_list.h
@@ -0,0 +1,134 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#ifndef __LINK_LIST_H__
+#define __LINK_LIST_H__
+
+typedef struct _LIST_ENTRY
+{
+	struct _LIST_ENTRY *pNext;
+} LIST_ENTRY, *PLIST_ENTRY;
+
+typedef struct _LIST_HEADR
+{
+	PLIST_ENTRY pHead;
+	PLIST_ENTRY pTail;
+	UCHAR size;
+} LIST_HEADER, *PLIST_HEADER;
+
+static inline VOID initList(
+	IN PLIST_HEADER pList)
+{
+	pList->pHead = pList->pTail = NULL;
+	pList->size = 0;
+	return;
+}
+
+static inline VOID insertTailList(
+	IN PLIST_HEADER pList,
+	IN PLIST_ENTRY pEntry)
+{
+	pEntry->pNext = NULL;
+	if (pList->pTail)
+		pList->pTail->pNext = pEntry;
+	else
+		pList->pHead = pEntry;
+	pList->pTail = pEntry;
+	pList->size++;
+
+	return;
+}
+
+static inline PLIST_ENTRY removeHeadList(
+	IN PLIST_HEADER pList)
+{
+	PLIST_ENTRY pNext;
+	PLIST_ENTRY pEntry;
+
+	pEntry = pList->pHead;
+	if (pList->pHead != NULL)
+	{
+		pNext = pList->pHead->pNext;
+		pList->pHead = pNext;
+		if (pNext == NULL)
+			pList->pTail = NULL;
+		pList->size--;
+	}
+	return pEntry;
+}
+
+static inline int getListSize(
+	IN PLIST_HEADER pList)
+{
+	return pList->size;
+}
+
+static inline PLIST_ENTRY delEntryList(
+	IN PLIST_HEADER pList,
+	IN PLIST_ENTRY pEntry)
+{
+	PLIST_ENTRY pCurEntry;
+	PLIST_ENTRY pPrvEntry;
+
+	if(pList->pHead == NULL)
+		return NULL;
+
+	if(pEntry == pList->pHead)
+	{
+		pCurEntry = pList->pHead;
+		pList->pHead = pCurEntry->pNext;
+
+		if(pList->pHead == NULL)
+			pList->pTail = NULL;
+
+		pList->size--;
+		return pCurEntry;
+	}
+
+	pPrvEntry = pList->pHead;
+	pCurEntry = pPrvEntry->pNext;
+	while(pCurEntry != NULL)
+	{
+		if (pEntry == pCurEntry)
+		{
+			pPrvEntry->pNext = pCurEntry->pNext;
+
+			if(pEntry == pList->pTail)
+				pList->pTail = pPrvEntry;
+
+			pList->size--;
+			break;
+		}
+		pPrvEntry = pCurEntry;
+		pCurEntry = pPrvEntry->pNext;
+	}
+
+	return pCurEntry;
+}
+
+#endif // ___LINK_LIST_H__ //
+
diff --git a/drivers/staging/rt2860/md4.h b/drivers/staging/rt2860/md4.h
new file mode 100644
index 00000000000..f1e5b526350
--- /dev/null
+++ b/drivers/staging/rt2860/md4.h
@@ -0,0 +1,42 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#ifndef __MD4_H__
+#define __MD4_H__
+
+/* MD4 context. */
+typedef	struct	_MD4_CTX_	{
+	ULONG	state[4];        /* state (ABCD) */
+	ULONG	count[2];        /* number of bits, modulo 2^64 (lsb first) */
+	UCHAR	buffer[64];      /* input buffer */
+}	MD4_CTX;
+
+VOID MD4Init (MD4_CTX *);
+VOID MD4Update (MD4_CTX *, PUCHAR, UINT);
+VOID MD4Final (UCHAR [16], MD4_CTX *);
+
+#endif //__MD4_H__
\ No newline at end of file
diff --git a/drivers/staging/rt2860/md5.h b/drivers/staging/rt2860/md5.h
new file mode 100644
index 00000000000..d85db12170d
--- /dev/null
+++ b/drivers/staging/rt2860/md5.h
@@ -0,0 +1,107 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	md5.h
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+	jan			10-28-03		Initial
+	Rita    	11-23-04		Modify MD5 and SHA-1
+*/
+
+#ifndef	uint8
+#define	uint8  unsigned	char
+#endif
+
+#ifndef	uint32
+#define	uint32 unsigned	long int
+#endif
+
+
+#ifndef	__MD5_H__
+#define	__MD5_H__
+
+#define MD5_MAC_LEN 16
+
+typedef struct _MD5_CTX {
+    UINT32   Buf[4];             // buffers of four states
+	UCHAR   Input[64];          // input message
+	UINT32   LenInBitCount[2];   // length counter for input message, 0 up to 64 bits
+}   MD5_CTX;
+
+VOID MD5Init(MD5_CTX *pCtx);
+VOID MD5Update(MD5_CTX *pCtx, UCHAR *pData, UINT32 LenInBytes);
+VOID MD5Final(UCHAR Digest[16], MD5_CTX *pCtx);
+VOID MD5Transform(UINT32 Buf[4], UINT32 Mes[16]);
+
+void md5_mac(u8 *key, size_t key_len, u8 *data, size_t data_len, u8 *mac);
+void hmac_md5(u8 *key, size_t key_len, u8 *data, size_t data_len, u8 *mac);
+
+//
+// SHA context
+//
+typedef	struct _SHA_CTX
+{
+	UINT32   Buf[5];             // buffers of five states
+	UCHAR   Input[80];          // input message
+	UINT32   LenInBitCount[2];   // length counter for input message, 0 up to 64 bits
+
+}	SHA_CTX;
+
+VOID SHAInit(SHA_CTX *pCtx);
+UCHAR SHAUpdate(SHA_CTX *pCtx, UCHAR *pData, UINT32 LenInBytes);
+VOID SHAFinal(SHA_CTX *pCtx, UCHAR Digest[20]);
+VOID SHATransform(UINT32 Buf[5], UINT32 Mes[20]);
+
+#define SHA_DIGEST_LEN 20
+#endif // __MD5_H__
+
+/******************************************************************************/
+#ifndef	_AES_H
+#define	_AES_H
+
+typedef	struct
+{
+	uint32 erk[64];		/* encryption round	keys */
+	uint32 drk[64];		/* decryption round	keys */
+	int	nr;				/* number of rounds	*/
+}
+aes_context;
+
+int	 rtmp_aes_set_key( aes_context *ctx,	uint8 *key,	int	nbits );
+void rtmp_aes_encrypt( aes_context *ctx,	uint8 input[16], uint8 output[16] );
+void rtmp_aes_decrypt( aes_context *ctx,	uint8 input[16], uint8 output[16] );
+
+void F(char *password, unsigned char *ssid, int ssidlength, int iterations, int count, unsigned char *output);
+int PasswordHash(char *password, unsigned char *ssid, int ssidlength, unsigned char *output);
+
+#endif /* aes.h	*/
+
diff --git a/drivers/staging/rt2860/mlme.h b/drivers/staging/rt2860/mlme.h
new file mode 100644
index 00000000000..5cb61652b66
--- /dev/null
+++ b/drivers/staging/rt2860/mlme.h
@@ -0,0 +1,1447 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	mlme.h
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John Chang	2003-08-28		Created
+	John Chang  2004-09-06      modified for RT2600
+
+*/
+#ifndef __MLME_H__
+#define __MLME_H__
+
+// maximum supported capability information -
+// ESS, IBSS, Privacy, Short Preamble, Spectrum mgmt, Short Slot
+#define SUPPORTED_CAPABILITY_INFO   0x0533
+
+#define END_OF_ARGS                 -1
+#define LFSR_MASK                   0x80000057
+#define MLME_TASK_EXEC_INTV         100/*200*/       //
+#define LEAD_TIME                   5
+#define MLME_TASK_EXEC_MULTIPLE       10  /*5*/       // MLME_TASK_EXEC_MULTIPLE * MLME_TASK_EXEC_INTV = 1 sec
+#define REORDER_EXEC_INTV         	100       // 0.1 sec
+
+// The definition of Radar detection duration region
+#define CE		0
+#define FCC		1
+#define JAP		2
+#define JAP_W53	3
+#define JAP_W56	4
+#define MAX_RD_REGION 5
+
+#ifdef	NDIS51_MINIPORT
+#define BEACON_LOST_TIME            4000       // 2048 msec = 2 sec
+#else
+#define BEACON_LOST_TIME            4 * OS_HZ    // 2048 msec = 2 sec
+#endif
+
+#define DLS_TIMEOUT                 1200      // unit: msec
+#define AUTH_TIMEOUT                300       // unit: msec
+#define ASSOC_TIMEOUT               300       // unit: msec
+#define JOIN_TIMEOUT                2 * OS_HZ      // unit: msec
+#define SHORT_CHANNEL_TIME          90        // unit: msec
+#define MIN_CHANNEL_TIME            110        // unit: msec, for dual band scan
+#define MAX_CHANNEL_TIME            140       // unit: msec, for single band scan
+#define	FAST_ACTIVE_SCAN_TIME	    30 		  // Active scan waiting for probe response time
+#define CW_MIN_IN_BITS              4         // actual CwMin = 2^CW_MIN_IN_BITS - 1
+
+
+#ifdef CONFIG_STA_SUPPORT
+#ifndef CONFIG_AP_SUPPORT
+#define CW_MAX_IN_BITS              10        // actual CwMax = 2^CW_MAX_IN_BITS - 1
+#endif
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+extern UINT32 CW_MAX_IN_BITS;
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+
+// Note: RSSI_TO_DBM_OFFSET has been changed to variable for new RF (2004-0720).
+// SHould not refer to this constant anymore
+//#define RSSI_TO_DBM_OFFSET          120 // for RT2530 RSSI-115 = dBm
+#define RSSI_FOR_MID_TX_POWER       -55  // -55 db is considered mid-distance
+#define RSSI_FOR_LOW_TX_POWER       -45  // -45 db is considered very short distance and
+                                        // eligible to use a lower TX power
+#define RSSI_FOR_LOWEST_TX_POWER    -30
+//#define MID_TX_POWER_DELTA          0   // 0 db from full TX power upon mid-distance to AP
+#define LOW_TX_POWER_DELTA          6    // -3 db from full TX power upon very short distance. 1 grade is 0.5 db
+#define LOWEST_TX_POWER_DELTA       16   // -8 db from full TX power upon shortest distance. 1 grade is 0.5 db
+
+#define RSSI_TRIGGERED_UPON_BELOW_THRESHOLD     0
+#define RSSI_TRIGGERED_UPON_EXCCEED_THRESHOLD   1
+#define RSSI_THRESHOLD_FOR_ROAMING              25
+#define RSSI_DELTA                              5
+
+// Channel Quality Indication
+#define CQI_IS_GOOD(cqi)            ((cqi) >= 50)
+//#define CQI_IS_FAIR(cqi)          (((cqi) >= 20) && ((cqi) < 50))
+#define CQI_IS_POOR(cqi)            (cqi < 50)  //(((cqi) >= 5) && ((cqi) < 20))
+#define CQI_IS_BAD(cqi)             (cqi < 5)
+#define CQI_IS_DEAD(cqi)            (cqi == 0)
+
+// weighting factor to calculate Channel quality, total should be 100%
+#define RSSI_WEIGHTING                   50
+#define TX_WEIGHTING                     30
+#define RX_WEIGHTING                     20
+
+#define BSS_NOT_FOUND                    0xFFFFFFFF
+
+
+#ifdef CONFIG_STA_SUPPORT
+#define MAX_LEN_OF_MLME_QUEUE            40 //10
+#endif // CONFIG_STA_SUPPORT //
+
+#define SCAN_PASSIVE                     18		// scan with no probe request, only wait beacon and probe response
+#define SCAN_ACTIVE                      19		// scan with probe request, and wait beacon and probe response
+#define	SCAN_CISCO_PASSIVE				 20		// Single channel passive scan
+#define	SCAN_CISCO_ACTIVE				 21		// Single channel active scan
+#define	SCAN_CISCO_NOISE				 22		// Single channel passive scan for noise histogram collection
+#define	SCAN_CISCO_CHANNEL_LOAD			 23		// Single channel passive scan for channel load collection
+#define FAST_SCAN_ACTIVE                 24		// scan with probe request, and wait beacon and probe response
+
+#ifdef DOT11N_DRAFT3
+#define SCAN_2040_BSS_COEXIST                  26
+#endif // DOT11N_DRAFT3 //
+
+#define MAC_ADDR_IS_GROUP(Addr)       (((Addr[0]) & 0x01))
+#define MAC_ADDR_HASH(Addr)            (Addr[0] ^ Addr[1] ^ Addr[2] ^ Addr[3] ^ Addr[4] ^ Addr[5])
+#define MAC_ADDR_HASH_INDEX(Addr)      (MAC_ADDR_HASH(Addr) % HASH_TABLE_SIZE)
+#define TID_MAC_HASH(Addr,TID)            (TID^Addr[0] ^ Addr[1] ^ Addr[2] ^ Addr[3] ^ Addr[4] ^ Addr[5])
+#define TID_MAC_HASH_INDEX(Addr,TID)      (TID_MAC_HASH(Addr,TID) % HASH_TABLE_SIZE)
+
+// LED Control
+// assoiation ON. one LED ON. another blinking when TX, OFF when idle
+// no association, both LED off
+#define ASIC_LED_ACT_ON(pAd)        RTMP_IO_WRITE32(pAd, MAC_CSR14, 0x00031e46)
+#define ASIC_LED_ACT_OFF(pAd)       RTMP_IO_WRITE32(pAd, MAC_CSR14, 0x00001e46)
+
+// bit definition of the 2-byte pBEACON->Capability field
+#define CAP_IS_ESS_ON(x)                 (((x) & 0x0001) != 0)
+#define CAP_IS_IBSS_ON(x)                (((x) & 0x0002) != 0)
+#define CAP_IS_CF_POLLABLE_ON(x)         (((x) & 0x0004) != 0)
+#define CAP_IS_CF_POLL_REQ_ON(x)         (((x) & 0x0008) != 0)
+#define CAP_IS_PRIVACY_ON(x)             (((x) & 0x0010) != 0)
+#define CAP_IS_SHORT_PREAMBLE_ON(x)      (((x) & 0x0020) != 0)
+#define CAP_IS_PBCC_ON(x)                (((x) & 0x0040) != 0)
+#define CAP_IS_AGILITY_ON(x)             (((x) & 0x0080) != 0)
+#define CAP_IS_SPECTRUM_MGMT(x)          (((x) & 0x0100) != 0)  // 802.11e d9
+#define CAP_IS_QOS(x)                    (((x) & 0x0200) != 0)  // 802.11e d9
+#define CAP_IS_SHORT_SLOT(x)             (((x) & 0x0400) != 0)
+#define CAP_IS_APSD(x)                   (((x) & 0x0800) != 0)  // 802.11e d9
+#define CAP_IS_IMMED_BA(x)               (((x) & 0x1000) != 0)  // 802.11e d9
+#define CAP_IS_DSSS_OFDM(x)              (((x) & 0x2000) != 0)
+#define CAP_IS_DELAY_BA(x)               (((x) & 0x4000) != 0)  // 802.11e d9
+
+#define CAP_GENERATE(ess,ibss,priv,s_pre,s_slot,spectrum)  (((ess) ? 0x0001 : 0x0000) | ((ibss) ? 0x0002 : 0x0000) | ((priv) ? 0x0010 : 0x0000) | ((s_pre) ? 0x0020 : 0x0000) | ((s_slot) ? 0x0400 : 0x0000) | ((spectrum) ? 0x0100 : 0x0000))
+
+#define ERP_IS_NON_ERP_PRESENT(x)        (((x) & 0x01) != 0)    // 802.11g
+#define ERP_IS_USE_PROTECTION(x)         (((x) & 0x02) != 0)    // 802.11g
+#define ERP_IS_USE_BARKER_PREAMBLE(x)    (((x) & 0x04) != 0)    // 802.11g
+
+#define DRS_TX_QUALITY_WORST_BOUND       8// 3  // just test by gary
+#define DRS_PENALTY                      8
+
+#define BA_NOTUSE 	2
+//BA Policy subfiled value in ADDBA frame
+#define IMMED_BA 	1
+#define DELAY_BA	0
+
+// BA Initiator subfield in DELBA frame
+#define ORIGINATOR	1
+#define RECIPIENT	0
+
+// ADDBA Status Code
+#define ADDBA_RESULTCODE_SUCCESS					0
+#define ADDBA_RESULTCODE_REFUSED					37
+#define ADDBA_RESULTCODE_INVALID_PARAMETERS			38
+
+// DELBA Reason Code
+#define DELBA_REASONCODE_QSTA_LEAVING				36
+#define DELBA_REASONCODE_END_BA						37
+#define DELBA_REASONCODE_UNKNOWN_BA					38
+#define DELBA_REASONCODE_TIMEOUT					39
+
+// reset all OneSecTx counters
+#define RESET_ONE_SEC_TX_CNT(__pEntry) \
+if (((__pEntry)) != NULL) \
+{ \
+	(__pEntry)->OneSecTxRetryOkCount = 0; \
+	(__pEntry)->OneSecTxFailCount = 0; \
+	(__pEntry)->OneSecTxNoRetryOkCount = 0; \
+}
+
+//
+// 802.11 frame formats
+//
+//  HT Capability INFO field in HT Cap IE .
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+	USHORT	LSIGTxopProSup:1;
+	USHORT	Forty_Mhz_Intolerant:1;
+	USHORT	PSMP:1;
+	USHORT	CCKmodein40:1;
+	USHORT	AMsduSize:1;
+	USHORT	DelayedBA:1;	//rt2860c not support
+	USHORT	RxSTBC:2;
+	USHORT	TxSTBC:1;
+	USHORT	ShortGIfor40:1;	//for40MHz
+	USHORT	ShortGIfor20:1;
+	USHORT	GF:1;	//green field
+	USHORT	MimoPs:2;//momi power safe
+	USHORT	ChannelWidth:1;
+	USHORT	AdvCoding:1;
+#else
+	USHORT	AdvCoding:1;
+	USHORT	ChannelWidth:1;
+	USHORT	MimoPs:2;//momi power safe
+	USHORT	GF:1;	//green field
+	USHORT	ShortGIfor20:1;
+	USHORT	ShortGIfor40:1;	//for40MHz
+	USHORT	TxSTBC:1;
+	USHORT	RxSTBC:2;
+	USHORT	DelayedBA:1;	//rt2860c not support
+	USHORT	AMsduSize:1;	// only support as zero
+	USHORT	CCKmodein40:1;
+	USHORT	PSMP:1;
+	USHORT	Forty_Mhz_Intolerant:1;
+	USHORT	LSIGTxopProSup:1;
+#endif	/* !RT_BIG_ENDIAN */
+} HT_CAP_INFO, *PHT_CAP_INFO;
+
+//  HT Capability INFO field in HT Cap IE .
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+	UCHAR	rsv:3;//momi power safe
+	UCHAR	MpduDensity:3;
+	UCHAR	MaxRAmpduFactor:2;
+#else
+	UCHAR	MaxRAmpduFactor:2;
+	UCHAR	MpduDensity:3;
+	UCHAR	rsv:3;//momi power safe
+#endif /* !RT_BIG_ENDIAN */
+} HT_CAP_PARM, *PHT_CAP_PARM;
+
+//  HT Capability INFO field in HT Cap IE .
+typedef struct PACKED {
+	UCHAR	MCSSet[10];
+	UCHAR	SupRate[2];  // unit : 1Mbps
+#ifdef RT_BIG_ENDIAN
+	UCHAR	rsv:3;
+	UCHAR	MpduDensity:1;
+	UCHAR	TxStream:2;
+	UCHAR	TxRxNotEqual:1;
+	UCHAR	TxMCSSetDefined:1;
+#else
+	UCHAR	TxMCSSetDefined:1;
+	UCHAR	TxRxNotEqual:1;
+	UCHAR	TxStream:2;
+	UCHAR	MpduDensity:1;
+	UCHAR	rsv:3;
+#endif // RT_BIG_ENDIAN //
+	UCHAR	rsv3[3];
+} HT_MCS_SET, *PHT_MCS_SET;
+
+//  HT Capability INFO field in HT Cap IE .
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+	USHORT	rsv2:4;
+	USHORT	RDGSupport:1;	//reverse Direction Grant  support
+	USHORT	PlusHTC:1;	//+HTC control field support
+	USHORT	MCSFeedback:2;	//0:no MCS feedback, 2:unsolicited MCS feedback, 3:Full MCS feedback,  1:rsv.
+	USHORT	rsv:5;//momi power safe
+	USHORT	TranTime:2;
+	USHORT	Pco:1;
+#else
+	USHORT	Pco:1;
+	USHORT	TranTime:2;
+	USHORT	rsv:5;//momi power safe
+	USHORT	MCSFeedback:2;	//0:no MCS feedback, 2:unsolicited MCS feedback, 3:Full MCS feedback,  1:rsv.
+	USHORT	PlusHTC:1;	//+HTC control field support
+	USHORT	RDGSupport:1;	//reverse Direction Grant  support
+	USHORT	rsv2:4;
+#endif /* RT_BIG_ENDIAN */
+} EXT_HT_CAP_INFO, *PEXT_HT_CAP_INFO;
+
+//  HT Beamforming field in HT Cap IE .
+typedef struct PACKED _HT_BF_CAP{
+#ifdef RT_BIG_ENDIAN
+	ULONG	rsv:3;
+	ULONG	ChanEstimation:2;
+	ULONG	CSIRowBFSup:2;
+	ULONG	ComSteerBFAntSup:2;
+	ULONG	NoComSteerBFAntSup:2;
+	ULONG	CSIBFAntSup:2;
+	ULONG	MinGrouping:2;
+	ULONG	ExpComBF:2;
+	ULONG	ExpNoComBF:2;
+	ULONG	ExpCSIFbk:2;
+	ULONG	ExpComSteerCapable:1;
+	ULONG	ExpNoComSteerCapable:1;
+	ULONG	ExpCSICapable:1;
+	ULONG	Calibration:2;
+	ULONG	ImpTxBFCapable:1;
+	ULONG	TxNDPCapable:1;
+	ULONG	RxNDPCapable:1;
+	ULONG	TxSoundCapable:1;
+	ULONG	RxSoundCapable:1;
+	ULONG	TxBFRecCapable:1;
+#else
+	ULONG	TxBFRecCapable:1;
+	ULONG	RxSoundCapable:1;
+	ULONG	TxSoundCapable:1;
+	ULONG	RxNDPCapable:1;
+	ULONG	TxNDPCapable:1;
+	ULONG	ImpTxBFCapable:1;
+	ULONG	Calibration:2;
+	ULONG	ExpCSICapable:1;
+	ULONG	ExpNoComSteerCapable:1;
+	ULONG	ExpComSteerCapable:1;
+	ULONG	ExpCSIFbk:2;
+	ULONG	ExpNoComBF:2;
+	ULONG	ExpComBF:2;
+	ULONG	MinGrouping:2;
+	ULONG	CSIBFAntSup:2;
+	ULONG	NoComSteerBFAntSup:2;
+	ULONG	ComSteerBFAntSup:2;
+	ULONG	CSIRowBFSup:2;
+	ULONG	ChanEstimation:2;
+	ULONG	rsv:3;
+#endif // RT_BIG_ENDIAN //
+} HT_BF_CAP, *PHT_BF_CAP;
+
+//  HT antenna selection field in HT Cap IE .
+typedef struct PACKED _HT_AS_CAP{
+#ifdef RT_BIG_ENDIAN
+	UCHAR	rsv:1;
+	UCHAR	TxSoundPPDU:1;
+	UCHAR	RxASel:1;
+	UCHAR	AntIndFbk:1;
+	UCHAR	ExpCSIFbk:1;
+	UCHAR	AntIndFbkTxASEL:1;
+	UCHAR	ExpCSIFbkTxASEL:1;
+	UCHAR	AntSelect:1;
+#else
+	UCHAR	AntSelect:1;
+	UCHAR	ExpCSIFbkTxASEL:1;
+	UCHAR	AntIndFbkTxASEL:1;
+	UCHAR	ExpCSIFbk:1;
+	UCHAR	AntIndFbk:1;
+	UCHAR	RxASel:1;
+	UCHAR	TxSoundPPDU:1;
+	UCHAR	rsv:1;
+#endif // RT_BIG_ENDIAN //
+} HT_AS_CAP, *PHT_AS_CAP;
+
+// Draft 1.0 set IE length 26, but is extensible..
+#define SIZE_HT_CAP_IE		26
+// The structure for HT Capability IE.
+typedef struct PACKED _HT_CAPABILITY_IE{
+	HT_CAP_INFO		HtCapInfo;
+	HT_CAP_PARM		HtCapParm;
+//	HT_MCS_SET		HtMCSSet;
+	UCHAR			MCSSet[16];
+	EXT_HT_CAP_INFO	ExtHtCapInfo;
+	HT_BF_CAP		TxBFCap;	// beamforming cap. rt2860c not support beamforming.
+	HT_AS_CAP		ASCap;	//antenna selection.
+} HT_CAPABILITY_IE, *PHT_CAPABILITY_IE;
+
+
+// 802.11n draft3 related structure definitions.
+// 7.3.2.60
+#define dot11OBSSScanPassiveDwell							20	// in TU. min amount of time that the STA continously scans each channel when performing an active OBSS scan.
+#define dot11OBSSScanActiveDwell							10	// in TU.min amount of time that the STA continously scans each channel when performing an passive OBSS scan.
+#define dot11BSSWidthTriggerScanInterval					300  // in sec. max interval between scan operations to be performed to detect BSS channel width trigger events.
+#define dot11OBSSScanPassiveTotalPerChannel					200	// in TU. min total amount of time that the STA scans each channel when performing a passive OBSS scan.
+#define dot11OBSSScanActiveTotalPerChannel					20	//in TU. min total amount of time that the STA scans each channel when performing a active OBSS scan
+#define dot11BSSWidthChannelTransactionDelayFactor			5	// min ratio between the delay time in performing a switch from 20MHz BSS to 20/40 BSS operation and the maxima
+																//	interval between overlapping BSS scan operations.
+#define dot11BSSScanActivityThreshold						25	// in %%, max total time that a STA may be active on the medium during a period of
+																//	(dot11BSSWidthChannelTransactionDelayFactor * dot11BSSWidthTriggerScanInterval) seconds without
+																//	being obligated to perform OBSS Scan operations. default is 25(== 0.25%)
+
+typedef struct PACKED _OVERLAP_BSS_SCAN_IE{
+	USHORT		ScanPassiveDwell;
+	USHORT		ScanActiveDwell;
+	USHORT		TriggerScanInt;				// Trigger scan interval
+	USHORT		PassiveTalPerChannel;		// passive total per channel
+	USHORT		ActiveTalPerChannel;		// active total per channel
+	USHORT		DelayFactor;				// BSS width channel transition delay factor
+	USHORT		ScanActThre;				// Scan Activity threshold
+}OVERLAP_BSS_SCAN_IE, *POVERLAP_BSS_SCAN_IE;
+
+
+//  7.3.2.56. 20/40 Coexistence element used in  Element ID = 72 = IE_2040_BSS_COEXIST
+typedef union PACKED _BSS_2040_COEXIST_IE{
+ struct PACKED {
+ #ifdef RT_BIG_ENDIAN
+	UCHAR	rsv:5;
+	UCHAR	BSS20WidthReq:1;
+	UCHAR	Intolerant40:1;
+	UCHAR	InfoReq:1;
+ #else
+	UCHAR	InfoReq:1;
+	UCHAR	Intolerant40:1;			// Inter-BSS. set 1 when prohibits a receiving BSS from operating as a 20/40 Mhz BSS.
+	UCHAR	BSS20WidthReq:1;		// Intra-BSS set 1 when prohibits a receiving AP from operating its BSS as a 20/40MHz BSS.
+	UCHAR	rsv:5;
+#endif // RT_BIG_ENDIAN //
+    } field;
+ UCHAR   word;
+} BSS_2040_COEXIST_IE, *PBSS_2040_COEXIST_IE;
+
+
+typedef struct  _TRIGGER_EVENTA{
+	BOOLEAN			bValid;
+	UCHAR	BSSID[6];
+	UCHAR	RegClass;	// Regulatory Class
+	USHORT	Channel;
+	ULONG	CDCounter;   // Maintain a seperate count down counter for each Event A.
+} TRIGGER_EVENTA, *PTRIGGER_EVENTA;
+
+// 20/40 trigger event table
+// If one Event A delete or created, or if Event B is detected or not detected, STA should send 2040BSSCoexistence to AP.
+#define MAX_TRIGGER_EVENT		64
+typedef struct  _TRIGGER_EVENT_TAB{
+	UCHAR	EventANo;
+	TRIGGER_EVENTA	EventA[MAX_TRIGGER_EVENT];
+	ULONG			EventBCountDown;	// Count down counter for Event B.
+} TRIGGER_EVENT_TAB, *PTRIGGER_EVENT_TAB;
+
+// 7.3.27 20/40 Bss Coexistence Mgmt capability used in extended capabilities information IE( ID = 127 = IE_EXT_CAPABILITY).
+//	This is the first octet and was defined in 802.11n D3.03 and 802.11yD9.0
+typedef struct PACKED _EXT_CAP_INFO_ELEMENT{
+#ifdef RT_BIG_ENDIAN
+	UCHAR	rsv2:5;
+	UCHAR	ExtendChannelSwitch:1;
+	UCHAR	rsv:1;
+	UCHAR	BssCoexistMgmtSupport:1;
+#else
+	UCHAR	BssCoexistMgmtSupport:1;
+	UCHAR	rsv:1;
+	UCHAR	ExtendChannelSwitch:1;
+	UCHAR	rsv2:5;
+#endif // RT_BIG_ENDIAN //
+}EXT_CAP_INFO_ELEMENT, *PEXT_CAP_INFO_ELEMENT;
+
+
+// 802.11n 7.3.2.61
+typedef struct PACKED _BSS_2040_COEXIST_ELEMENT{
+	UCHAR					ElementID;		// ID = IE_2040_BSS_COEXIST = 72
+	UCHAR					Len;
+	BSS_2040_COEXIST_IE		BssCoexistIe;
+}BSS_2040_COEXIST_ELEMENT, *PBSS_2040_COEXIST_ELEMENT;
+
+
+//802.11n 7.3.2.59
+typedef struct PACKED _BSS_2040_INTOLERANT_CH_REPORT{
+	UCHAR				ElementID;		// ID = IE_2040_BSS_INTOLERANT_REPORT = 73
+	UCHAR				Len;
+	UCHAR				RegulatoryClass;
+	UCHAR				ChList[0];
+}BSS_2040_INTOLERANT_CH_REPORT, *PBSS_2040_INTOLERANT_CH_REPORT;
+
+
+// The structure for channel switch annoucement IE. This is in 802.11n D3.03
+typedef struct PACKED _CHA_SWITCH_ANNOUNCE_IE{
+	UCHAR			SwitchMode;	//channel switch mode
+	UCHAR			NewChannel;	//
+	UCHAR			SwitchCount;	//
+} CHA_SWITCH_ANNOUNCE_IE, *PCHA_SWITCH_ANNOUNCE_IE;
+
+
+// The structure for channel switch annoucement IE. This is in 802.11n D3.03
+typedef struct PACKED _SEC_CHA_OFFSET_IE{
+	UCHAR			SecondaryChannelOffset;	 // 1: Secondary above, 3: Secondary below, 0: no Secondary
+} SEC_CHA_OFFSET_IE, *PSEC_CHA_OFFSET_IE;
+
+
+// This structure is extracted from struct RT_HT_CAPABILITY
+typedef struct {
+	BOOLEAN			bHtEnable;	 // If we should use ht rate.
+	BOOLEAN			bPreNHt;	 // If we should use ht rate.
+	//Substract from HT Capability IE
+	UCHAR			MCSSet[16];	//only supoort MCS=0-15,32 ,
+} RT_HT_PHY_INFO, *PRT_HT_PHY_INFO;
+
+//This structure substracts ralink supports from all 802.11n-related features.
+//Features not listed here but contained in 802.11n spec are not supported in rt2860.
+typedef struct {
+#ifdef RT_BIG_ENDIAN
+	USHORT	rsv:5;
+	USHORT	AmsduSize:1;	// Max receiving A-MSDU size
+	USHORT	AmsduEnable:1;	// Enable to transmit A-MSDU. Suggest disable. We should use A-MPDU to gain best benifit of 802.11n
+	USHORT	RxSTBC:2;	// 2 bits
+	USHORT	TxSTBC:1;
+	USHORT	ShortGIfor40:1;	//for40MHz
+	USHORT	ShortGIfor20:1;
+	USHORT	GF:1;	//green field
+	USHORT	MimoPs:2;//mimo power safe MMPS_
+	USHORT	ChannelWidth:1;
+#else
+	USHORT	ChannelWidth:1;
+	USHORT	MimoPs:2;//mimo power safe MMPS_
+	USHORT	GF:1;	//green field
+	USHORT	ShortGIfor20:1;
+	USHORT	ShortGIfor40:1;	//for40MHz
+	USHORT	TxSTBC:1;
+	USHORT	RxSTBC:2;	// 2 bits
+	USHORT	AmsduEnable:1;	// Enable to transmit A-MSDU. Suggest disable. We should use A-MPDU to gain best benifit of 802.11n
+	USHORT	AmsduSize:1;	// Max receiving A-MSDU size
+	USHORT	rsv:5;
+#endif
+
+	//Substract from Addiont HT INFO IE
+#ifdef RT_BIG_ENDIAN
+	UCHAR	RecomWidth:1;
+	UCHAR	ExtChanOffset:2;	// Please not the difference with following 	UCHAR	NewExtChannelOffset; from 802.11n
+	UCHAR	MpduDensity:3;
+	UCHAR	MaxRAmpduFactor:2;
+#else
+	UCHAR	MaxRAmpduFactor:2;
+	UCHAR	MpduDensity:3;
+	UCHAR	ExtChanOffset:2;	// Please not the difference with following 	UCHAR	NewExtChannelOffset; from 802.11n
+	UCHAR	RecomWidth:1;
+#endif
+
+#ifdef RT_BIG_ENDIAN
+	USHORT	rsv2:11;
+	USHORT	OBSS_NonHTExist:1;
+	USHORT	rsv3:1;
+	USHORT	NonGfPresent:1;
+	USHORT	OperaionMode:2;
+#else
+	USHORT	OperaionMode:2;
+	USHORT	NonGfPresent:1;
+	USHORT	rsv3:1;
+	USHORT	OBSS_NonHTExist:1;
+	USHORT	rsv2:11;
+#endif
+
+	// New Extension Channel Offset IE
+	UCHAR	NewExtChannelOffset;
+	// Extension Capability IE = 127
+	UCHAR	BSSCoexist2040;
+} RT_HT_CAPABILITY, *PRT_HT_CAPABILITY;
+
+//   field in Addtional HT Information IE .
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+	UCHAR	SerInterGranu:3;
+	UCHAR	S_PSMPSup:1;
+	UCHAR	RifsMode:1;
+	UCHAR	RecomWidth:1;
+	UCHAR	ExtChanOffset:2;
+#else
+	UCHAR	ExtChanOffset:2;
+	UCHAR	RecomWidth:1;
+	UCHAR	RifsMode:1;
+	UCHAR	S_PSMPSup:1;	 //Indicate support for scheduled PSMP
+	UCHAR	SerInterGranu:3;	 //service interval granularity
+#endif
+} ADD_HTINFO, *PADD_HTINFO;
+
+typedef struct PACKED{
+#ifdef RT_BIG_ENDIAN
+	USHORT	rsv2:11;
+	USHORT	OBSS_NonHTExist:1;
+	USHORT	rsv:1;
+	USHORT	NonGfPresent:1;
+	USHORT	OperaionMode:2;
+#else
+	USHORT	OperaionMode:2;
+	USHORT	NonGfPresent:1;
+	USHORT	rsv:1;
+	USHORT	OBSS_NonHTExist:1;
+	USHORT	rsv2:11;
+#endif
+} ADD_HTINFO2, *PADD_HTINFO2;
+
+
+// TODO: Need sync with spec about the definition of StbcMcs. In Draft 3.03, it's reserved.
+typedef struct PACKED{
+#ifdef RT_BIG_ENDIAN
+	USHORT	rsv:4;
+	USHORT	PcoPhase:1;
+	USHORT	PcoActive:1;
+	USHORT	LsigTxopProt:1;
+	USHORT	STBCBeacon:1;
+	USHORT	DualCTSProtect:1;
+	USHORT	DualBeacon:1;
+	USHORT	StbcMcs:6;
+#else
+	USHORT	StbcMcs:6;
+	USHORT	DualBeacon:1;
+	USHORT	DualCTSProtect:1;
+	USHORT	STBCBeacon:1;
+	USHORT	LsigTxopProt:1;	// L-SIG TXOP protection full support
+	USHORT	PcoActive:1;
+	USHORT	PcoPhase:1;
+	USHORT	rsv:4;
+#endif // RT_BIG_ENDIAN //
+} ADD_HTINFO3, *PADD_HTINFO3;
+
+#define SIZE_ADD_HT_INFO_IE		22
+typedef struct  PACKED{
+	UCHAR				ControlChan;
+	ADD_HTINFO			AddHtInfo;
+	ADD_HTINFO2			AddHtInfo2;
+	ADD_HTINFO3			AddHtInfo3;
+	UCHAR				MCSSet[16];		// Basic MCS set
+} ADD_HT_INFO_IE, *PADD_HT_INFO_IE;
+
+typedef struct  PACKED{
+	UCHAR				NewExtChanOffset;
+} NEW_EXT_CHAN_IE, *PNEW_EXT_CHAN_IE;
+
+
+// 4-byte HTC field.  maybe included in any frame except non-QOS data frame.  The Order bit must set 1.
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+    UINT32		RDG:1;	//RDG / More PPDU
+    UINT32		ACConstraint:1;	//feedback request
+    UINT32		rsv:5;  //calibration sequence
+    UINT32		ZLFAnnouce:1;	// ZLF announcement
+    UINT32		CSISTEERING:2;	//CSI/ STEERING
+    UINT32		FBKReq:2;	//feedback request
+    UINT32		CalSeq:2;  //calibration sequence
+    UINT32		CalPos:2;	// calibration position
+    UINT32		MFBorASC:7;	//Link adaptation feedback containing recommended MCS. 0x7f for no feedback or not available
+    UINT32		MFS:3;	//SET to the received value of MRS. 0x111 for unsolicited MFB.
+    UINT32		MRSorASI:3;	// MRQ Sequence identifier. unchanged during entire procedure. 0x000-0x110.
+    UINT32		MRQ:1;	//MCS feedback. Request for a MCS feedback
+    UINT32		TRQ:1;	//sounding request
+    UINT32		MA:1;	//management action payload exist in (QoS Null+HTC)
+#else
+    UINT32		MA:1;	//management action payload exist in (QoS Null+HTC)
+    UINT32		TRQ:1;	//sounding request
+    UINT32		MRQ:1;	//MCS feedback. Request for a MCS feedback
+    UINT32		MRSorASI:3;	// MRQ Sequence identifier. unchanged during entire procedure. 0x000-0x110.
+    UINT32		MFS:3;	//SET to the received value of MRS. 0x111 for unsolicited MFB.
+    UINT32		MFBorASC:7;	//Link adaptation feedback containing recommended MCS. 0x7f for no feedback or not available
+    UINT32		CalPos:2;	// calibration position
+    UINT32		CalSeq:2;  //calibration sequence
+    UINT32		FBKReq:2;	//feedback request
+    UINT32		CSISTEERING:2;	//CSI/ STEERING
+    UINT32		ZLFAnnouce:1;	// ZLF announcement
+    UINT32		rsv:5;  //calibration sequence
+    UINT32		ACConstraint:1;	//feedback request
+    UINT32		RDG:1;	//RDG / More PPDU
+#endif /* !RT_BIG_ENDIAN */
+} HT_CONTROL, *PHT_CONTROL;
+
+// 2-byte QOS CONTROL field
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+    USHORT      Txop_QueueSize:8;
+    USHORT      AMsduPresent:1;
+    USHORT      AckPolicy:2;  //0: normal ACK 1:No ACK 2:scheduled under MTBA/PSMP  3: BA
+    USHORT      EOSP:1;
+    USHORT      TID:4;
+#else
+    USHORT      TID:4;
+    USHORT      EOSP:1;
+    USHORT      AckPolicy:2;  //0: normal ACK 1:No ACK 2:scheduled under MTBA/PSMP  3: BA
+    USHORT      AMsduPresent:1;
+    USHORT      Txop_QueueSize:8;
+#endif /* !RT_BIG_ENDIAN */
+} QOS_CONTROL, *PQOS_CONTROL;
+
+// 2-byte Frame control field
+typedef	struct	PACKED {
+#ifdef RT_BIG_ENDIAN
+	USHORT		Order:1;			// Strict order expected
+	USHORT		Wep:1;				// Wep data
+	USHORT		MoreData:1;			// More data bit
+	USHORT		PwrMgmt:1;			// Power management bit
+	USHORT		Retry:1;			// Retry status bit
+	USHORT		MoreFrag:1;			// More fragment bit
+	USHORT		FrDs:1;				// From DS indication
+	USHORT		ToDs:1;				// To DS indication
+	USHORT		SubType:4;			// MSDU subtype
+	USHORT		Type:2;				// MSDU type
+	USHORT		Ver:2;				// Protocol version
+#else
+	USHORT		Ver:2;				// Protocol version
+	USHORT		Type:2;				// MSDU type
+	USHORT		SubType:4;			// MSDU subtype
+	USHORT		ToDs:1;				// To DS indication
+	USHORT		FrDs:1;				// From DS indication
+	USHORT		MoreFrag:1;			// More fragment bit
+	USHORT		Retry:1;			// Retry status bit
+	USHORT		PwrMgmt:1;			// Power management bit
+	USHORT		MoreData:1;			// More data bit
+	USHORT		Wep:1;				// Wep data
+	USHORT		Order:1;			// Strict order expected
+#endif /* !RT_BIG_ENDIAN */
+} FRAME_CONTROL, *PFRAME_CONTROL;
+
+typedef	struct	PACKED _HEADER_802_11	{
+    FRAME_CONTROL   FC;
+    USHORT          Duration;
+    UCHAR           Addr1[MAC_ADDR_LEN];
+    UCHAR           Addr2[MAC_ADDR_LEN];
+	UCHAR			Addr3[MAC_ADDR_LEN];
+#ifdef RT_BIG_ENDIAN
+	USHORT			Sequence:12;
+	USHORT			Frag:4;
+#else
+	USHORT			Frag:4;
+	USHORT			Sequence:12;
+#endif /* !RT_BIG_ENDIAN */
+	UCHAR			Octet[0];
+}	HEADER_802_11, *PHEADER_802_11;
+
+typedef struct PACKED _FRAME_802_11 {
+    HEADER_802_11   Hdr;
+    UCHAR            Octet[1];
+}   FRAME_802_11, *PFRAME_802_11;
+
+// QoSNull embedding of management action. When HT Control MA field set to 1.
+typedef struct PACKED _MA_BODY {
+    UCHAR            Category;
+    UCHAR            Action;
+    UCHAR            Octet[1];
+}   MA_BODY, *PMA_BODY;
+
+typedef	struct	PACKED _HEADER_802_3	{
+    UCHAR           DAAddr1[MAC_ADDR_LEN];
+    UCHAR           SAAddr2[MAC_ADDR_LEN];
+    UCHAR           Octet[2];
+}	HEADER_802_3, *PHEADER_802_3;
+////Block ACK related format
+// 2-byte BA Parameter  field  in 	DELBA frames to terminate an already set up bA
+typedef struct PACKED{
+#ifdef RT_BIG_ENDIAN
+    USHORT      TID:4;	// value of TC os TS
+    USHORT      Initiator:1;	// 1: originator    0:recipient
+    USHORT      Rsv:11;	// always set to 0
+#else
+    USHORT      Rsv:11;	// always set to 0
+    USHORT      Initiator:1;	// 1: originator    0:recipient
+    USHORT      TID:4;	// value of TC os TS
+#endif /* !RT_BIG_ENDIAN */
+} DELBA_PARM, *PDELBA_PARM;
+
+// 2-byte BA Parameter Set field  in ADDBA frames to signal parm for setting up a BA
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+    USHORT      BufSize:10;	// number of buffe of size 2304 octetsr
+    USHORT      TID:4;	// value of TC os TS
+    USHORT      BAPolicy:1;	// 1: immediately BA    0:delayed BA
+    USHORT      AMSDUSupported:1;	// 0: not permitted		1: permitted
+#else
+    USHORT      AMSDUSupported:1;	// 0: not permitted		1: permitted
+    USHORT      BAPolicy:1;	// 1: immediately BA    0:delayed BA
+    USHORT      TID:4;	// value of TC os TS
+    USHORT      BufSize:10;	// number of buffe of size 2304 octetsr
+#endif /* !RT_BIG_ENDIAN */
+} BA_PARM, *PBA_PARM;
+
+// 2-byte BA Starting Seq CONTROL field
+typedef union PACKED {
+    struct PACKED {
+#ifdef RT_BIG_ENDIAN
+    USHORT      StartSeq:12;   // sequence number of the 1st MSDU for which this BAR is sent
+	USHORT      FragNum:4;	// always set to 0
+#else
+    USHORT      FragNum:4;	// always set to 0
+	USHORT      StartSeq:12;   // sequence number of the 1st MSDU for which this BAR is sent
+#endif /* RT_BIG_ENDIAN */
+    }   field;
+    USHORT           word;
+} BASEQ_CONTROL, *PBASEQ_CONTROL;
+
+//BAControl and BARControl are the same
+// 2-byte BA CONTROL field in BA frame
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+    USHORT      TID:4;
+    USHORT      Rsv:9;
+    USHORT      Compressed:1;
+    USHORT      MTID:1;		//EWC V1.24
+    USHORT      ACKPolicy:1; // only related to N-Delayed BA. But not support in RT2860b. 0:NormalACK  1:No ACK
+#else
+    USHORT      ACKPolicy:1; // only related to N-Delayed BA. But not support in RT2860b. 0:NormalACK  1:No ACK
+    USHORT      MTID:1;		//EWC V1.24
+    USHORT      Compressed:1;
+    USHORT      Rsv:9;
+    USHORT      TID:4;
+#endif /* !RT_BIG_ENDIAN */
+} BA_CONTROL, *PBA_CONTROL;
+
+// 2-byte BAR CONTROL field in BAR frame
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+    USHORT      TID:4;
+    USHORT      Rsv1:9;
+    USHORT      Compressed:1;
+    USHORT      MTID:1;		//if this bit1, use  FRAME_MTBA_REQ,  if 0, use FRAME_BA_REQ
+    USHORT      ACKPolicy:1;
+#else
+    USHORT      ACKPolicy:1; // 0:normal ack,  1:no ack.
+    USHORT      MTID:1;		//if this bit1, use  FRAME_MTBA_REQ,  if 0, use FRAME_BA_REQ
+    USHORT      Compressed:1;
+    USHORT      Rsv1:9;
+    USHORT      TID:4;
+#endif /* !RT_BIG_ENDIAN */
+} BAR_CONTROL, *PBAR_CONTROL;
+
+// BARControl in MTBAR frame
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+    USHORT      NumTID:4;
+    USHORT      Rsv1:9;
+    USHORT      Compressed:1;
+    USHORT      MTID:1;
+    USHORT      ACKPolicy:1;
+#else
+    USHORT      ACKPolicy:1;
+    USHORT      MTID:1;
+    USHORT      Compressed:1;
+    USHORT      Rsv1:9;
+    USHORT      NumTID:4;
+#endif /* !RT_BIG_ENDIAN */
+} MTBAR_CONTROL, *PMTBAR_CONTROL;
+
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+    USHORT      TID:4;
+    USHORT      Rsv1:12;
+#else
+    USHORT      Rsv1:12;
+    USHORT      TID:4;
+#endif /* !RT_BIG_ENDIAN */
+} PER_TID_INFO, *PPER_TID_INFO;
+
+typedef struct {
+	PER_TID_INFO      PerTID;
+	BASEQ_CONTROL 	 BAStartingSeq;
+} EACH_TID, *PEACH_TID;
+
+
+typedef struct PACKED _PSPOLL_FRAME {
+    FRAME_CONTROL   FC;
+    USHORT          Aid;
+    UCHAR           Bssid[MAC_ADDR_LEN];
+    UCHAR           Ta[MAC_ADDR_LEN];
+}   PSPOLL_FRAME, *PPSPOLL_FRAME;
+
+typedef	struct	PACKED _RTS_FRAME	{
+    FRAME_CONTROL   FC;
+    USHORT          Duration;
+    UCHAR           Addr1[MAC_ADDR_LEN];
+    UCHAR           Addr2[MAC_ADDR_LEN];
+}RTS_FRAME, *PRTS_FRAME;
+
+// BAREQ AND MTBAREQ have the same subtype BAR, 802.11n BAR use compressed bitmap.
+typedef struct PACKED _FRAME_BA_REQ {
+	FRAME_CONTROL   FC;
+	USHORT          Duration;
+	UCHAR           Addr1[MAC_ADDR_LEN];
+	UCHAR           Addr2[MAC_ADDR_LEN];
+	BAR_CONTROL  BARControl;
+	BASEQ_CONTROL 	 BAStartingSeq;
+}   FRAME_BA_REQ, *PFRAME_BA_REQ;
+
+typedef struct PACKED _FRAME_MTBA_REQ {
+	FRAME_CONTROL   FC;
+	USHORT          Duration;
+	UCHAR           Addr1[MAC_ADDR_LEN];
+	UCHAR           Addr2[MAC_ADDR_LEN];
+	MTBAR_CONTROL  MTBARControl;
+	PER_TID_INFO	PerTIDInfo;
+	BASEQ_CONTROL 	 BAStartingSeq;
+}   FRAME_MTBA_REQ, *PFRAME_MTBA_REQ;
+
+// Compressed format is mandantory in HT STA
+typedef struct PACKED _FRAME_MTBA {
+	FRAME_CONTROL   FC;
+	USHORT          Duration;
+	UCHAR           Addr1[MAC_ADDR_LEN];
+	UCHAR           Addr2[MAC_ADDR_LEN];
+	BA_CONTROL  BAControl;
+	BASEQ_CONTROL 	 BAStartingSeq;
+	UCHAR		BitMap[8];
+}   FRAME_MTBA, *PFRAME_MTBA;
+
+typedef struct PACKED _FRAME_PSMP_ACTION {
+	HEADER_802_11   Hdr;
+	UCHAR	Category;
+	UCHAR	Action;
+	UCHAR	Psmp;	// 7.3.1.25
+}   FRAME_PSMP_ACTION, *PFRAME_PSMP_ACTION;
+
+typedef struct PACKED _FRAME_ACTION_HDR {
+	HEADER_802_11   Hdr;
+	UCHAR	Category;
+	UCHAR	Action;
+}   FRAME_ACTION_HDR, *PFRAME_ACTION_HDR;
+
+//Action Frame
+//Action Frame  Category:Spectrum,  Action:Channel Switch. 7.3.2.20
+typedef struct PACKED _CHAN_SWITCH_ANNOUNCE {
+	UCHAR					ElementID;	// ID = IE_CHANNEL_SWITCH_ANNOUNCEMENT = 37
+	UCHAR					Len;
+	CHA_SWITCH_ANNOUNCE_IE	CSAnnounceIe;
+}   CHAN_SWITCH_ANNOUNCE, *PCHAN_SWITCH_ANNOUNCE;
+
+
+//802.11n : 7.3.2.20a
+typedef struct PACKED _SECOND_CHAN_OFFSET {
+	UCHAR				ElementID;		// ID = IE_SECONDARY_CH_OFFSET = 62
+	UCHAR				Len;
+	SEC_CHA_OFFSET_IE	SecChOffsetIe;
+}   SECOND_CHAN_OFFSET, *PSECOND_CHAN_OFFSET;
+
+
+typedef struct PACKED _FRAME_SPETRUM_CS {
+	HEADER_802_11   Hdr;
+	UCHAR	Category;
+	UCHAR	Action;
+	CHAN_SWITCH_ANNOUNCE	CSAnnounce;
+	SECOND_CHAN_OFFSET		SecondChannel;
+}   FRAME_SPETRUM_CS, *PFRAME_SPETRUM_CS;
+
+
+typedef struct PACKED _FRAME_ADDBA_REQ {
+	HEADER_802_11   Hdr;
+	UCHAR	Category;
+	UCHAR	Action;
+	UCHAR	Token;	// 1
+	BA_PARM		BaParm;	      //  2 - 10
+	USHORT		TimeOutValue;	// 0 - 0
+	BASEQ_CONTROL	BaStartSeq; // 0-0
+}   FRAME_ADDBA_REQ, *PFRAME_ADDBA_REQ;
+
+typedef struct PACKED _FRAME_ADDBA_RSP {
+	HEADER_802_11   Hdr;
+	UCHAR	Category;
+	UCHAR	Action;
+	UCHAR	Token;
+	USHORT	StatusCode;
+	BA_PARM		BaParm; //0 - 2
+	USHORT		TimeOutValue;
+}   FRAME_ADDBA_RSP, *PFRAME_ADDBA_RSP;
+
+typedef struct PACKED _FRAME_DELBA_REQ {
+	HEADER_802_11   Hdr;
+	UCHAR	Category;
+	UCHAR	Action;
+	DELBA_PARM		DelbaParm;
+	USHORT	ReasonCode;
+}   FRAME_DELBA_REQ, *PFRAME_DELBA_REQ;
+
+
+//7.2.1.7
+typedef struct PACKED _FRAME_BAR {
+	FRAME_CONTROL   FC;
+	USHORT          Duration;
+	UCHAR           Addr1[MAC_ADDR_LEN];
+	UCHAR           Addr2[MAC_ADDR_LEN];
+	BAR_CONTROL		BarControl;
+	BASEQ_CONTROL	StartingSeq;
+}   FRAME_BAR, *PFRAME_BAR;
+
+//7.2.1.7
+typedef struct PACKED _FRAME_BA {
+	FRAME_CONTROL   FC;
+	USHORT          Duration;
+	UCHAR           Addr1[MAC_ADDR_LEN];
+	UCHAR           Addr2[MAC_ADDR_LEN];
+	BAR_CONTROL		BarControl;
+	BASEQ_CONTROL	StartingSeq;
+	UCHAR		bitmask[8];
+}   FRAME_BA, *PFRAME_BA;
+
+
+// Radio Measuement Request Frame Format
+typedef struct PACKED _FRAME_RM_REQ_ACTION {
+	HEADER_802_11   Hdr;
+	UCHAR	Category;
+	UCHAR	Action;
+	UCHAR	Token;
+	USHORT	Repetition;
+	UCHAR   data[0];
+}   FRAME_RM_REQ_ACTION, *PFRAME_RM_REQ_ACTION;
+
+typedef struct PACKED {
+	UCHAR		ID;
+	UCHAR		Length;
+	UCHAR		ChannelSwitchMode;
+	UCHAR		NewRegClass;
+	UCHAR		NewChannelNum;
+	UCHAR		ChannelSwitchCount;
+} HT_EXT_CHANNEL_SWITCH_ANNOUNCEMENT_IE, *PHT_EXT_CHANNEL_SWITCH_ANNOUNCEMENT_IE;
+
+
+//
+// _Limit must be the 2**n - 1
+// _SEQ1 , _SEQ2 must be within 0 ~ _Limit
+//
+#define SEQ_STEPONE(_SEQ1, _SEQ2, _Limit)	((_SEQ1 == ((_SEQ2+1) & _Limit)))
+#define SEQ_SMALLER(_SEQ1, _SEQ2, _Limit)	(((_SEQ1-_SEQ2) & ((_Limit+1)>>1)))
+#define SEQ_LARGER(_SEQ1, _SEQ2, _Limit)	((_SEQ1 != _SEQ2) && !(((_SEQ1-_SEQ2) & ((_Limit+1)>>1))))
+#define SEQ_WITHIN_WIN(_SEQ1, _SEQ2, _WIN, _Limit) (SEQ_LARGER(_SEQ1, _SEQ2, _Limit) &&  \
+												SEQ_SMALLER(_SEQ1, ((_SEQ2+_WIN+1)&_Limit), _Limit))
+
+//
+// Contention-free parameter (without ID and Length)
+//
+typedef struct PACKED {
+    BOOLEAN     bValid;         // 1: variable contains valid value
+    UCHAR       CfpCount;
+    UCHAR       CfpPeriod;
+    USHORT      CfpMaxDuration;
+    USHORT      CfpDurRemaining;
+} CF_PARM, *PCF_PARM;
+
+typedef	struct	_CIPHER_SUITE	{
+	NDIS_802_11_ENCRYPTION_STATUS	PairCipher;		// Unicast cipher 1, this one has more secured cipher suite
+	NDIS_802_11_ENCRYPTION_STATUS	PairCipherAux;	// Unicast cipher 2 if AP announce two unicast cipher suite
+	NDIS_802_11_ENCRYPTION_STATUS	GroupCipher;	// Group cipher
+	USHORT							RsnCapability;	// RSN capability from beacon
+	BOOLEAN							bMixMode;		// Indicate Pair & Group cipher might be different
+}	CIPHER_SUITE, *PCIPHER_SUITE;
+
+// EDCA configuration from AP's BEACON/ProbeRsp
+typedef struct {
+    BOOLEAN     bValid;         // 1: variable contains valid value
+    BOOLEAN     bAdd;         // 1: variable contains valid value
+    BOOLEAN     bQAck;
+    BOOLEAN     bQueueRequest;
+    BOOLEAN     bTxopRequest;
+    BOOLEAN     bAPSDCapable;
+//  BOOLEAN     bMoreDataAck;
+    UCHAR       EdcaUpdateCount;
+    UCHAR       Aifsn[4];       // 0:AC_BK, 1:AC_BE, 2:AC_VI, 3:AC_VO
+    UCHAR       Cwmin[4];
+    UCHAR       Cwmax[4];
+    USHORT      Txop[4];      // in unit of 32-us
+    BOOLEAN     bACM[4];      // 1: Admission Control of AC_BK is mandattory
+} EDCA_PARM, *PEDCA_PARM;
+
+// QBSS LOAD information from QAP's BEACON/ProbeRsp
+typedef struct {
+    BOOLEAN     bValid;                     // 1: variable contains valid value
+    USHORT      StaNum;
+    UCHAR       ChannelUtilization;
+    USHORT      RemainingAdmissionControl;  // in unit of 32-us
+} QBSS_LOAD_PARM, *PQBSS_LOAD_PARM;
+
+// QBSS Info field in QSTA's assoc req
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+	UCHAR		Rsv2:1;
+	UCHAR		MaxSPLength:2;
+	UCHAR		Rsv1:1;
+	UCHAR		UAPSD_AC_BE:1;
+	UCHAR		UAPSD_AC_BK:1;
+	UCHAR		UAPSD_AC_VI:1;
+	UCHAR		UAPSD_AC_VO:1;
+#else
+    UCHAR		UAPSD_AC_VO:1;
+	UCHAR		UAPSD_AC_VI:1;
+	UCHAR		UAPSD_AC_BK:1;
+	UCHAR		UAPSD_AC_BE:1;
+	UCHAR		Rsv1:1;
+	UCHAR		MaxSPLength:2;
+	UCHAR		Rsv2:1;
+#endif /* !RT_BIG_ENDIAN */
+} QBSS_STA_INFO_PARM, *PQBSS_STA_INFO_PARM;
+
+// QBSS Info field in QAP's Beacon/ProbeRsp
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+	UCHAR		UAPSD:1;
+	UCHAR		Rsv:3;
+    UCHAR		ParamSetCount:4;
+#else
+    UCHAR		ParamSetCount:4;
+	UCHAR		Rsv:3;
+	UCHAR		UAPSD:1;
+#endif /* !RT_BIG_ENDIAN */
+} QBSS_AP_INFO_PARM, *PQBSS_AP_INFO_PARM;
+
+// QOS Capability reported in QAP's BEACON/ProbeRsp
+// QOS Capability sent out in QSTA's AssociateReq/ReAssociateReq
+typedef struct {
+    BOOLEAN     bValid;                     // 1: variable contains valid value
+    BOOLEAN     bQAck;
+    BOOLEAN     bQueueRequest;
+    BOOLEAN     bTxopRequest;
+//  BOOLEAN     bMoreDataAck;
+    UCHAR       EdcaUpdateCount;
+} QOS_CAPABILITY_PARM, *PQOS_CAPABILITY_PARM;
+
+#ifdef CONFIG_STA_SUPPORT
+typedef struct {
+    UCHAR       IELen;
+    UCHAR       IE[MAX_CUSTOM_LEN];
+} WPA_IE_;
+#endif // CONFIG_STA_SUPPORT //
+
+
+typedef struct {
+    UCHAR   Bssid[MAC_ADDR_LEN];
+    UCHAR   Channel;
+	UCHAR   CentralChannel;	//Store the wide-band central channel for 40MHz.  .used in 40MHz AP. Or this is the same as Channel.
+    UCHAR   BssType;
+    USHORT  AtimWin;
+    USHORT  BeaconPeriod;
+
+    UCHAR   SupRate[MAX_LEN_OF_SUPPORTED_RATES];
+    UCHAR   SupRateLen;
+    UCHAR   ExtRate[MAX_LEN_OF_SUPPORTED_RATES];
+    UCHAR   ExtRateLen;
+	HT_CAPABILITY_IE HtCapability;
+	UCHAR			HtCapabilityLen;
+	ADD_HT_INFO_IE AddHtInfo;	// AP might use this additional ht info IE
+	UCHAR			AddHtInfoLen;
+	UCHAR			NewExtChanOffset;
+	CHAR    Rssi;
+    UCHAR   Privacy;			// Indicate security function ON/OFF. Don't mess up with auth mode.
+	UCHAR	Hidden;
+
+    USHORT  DtimPeriod;
+    USHORT  CapabilityInfo;
+
+    USHORT  CfpCount;
+    USHORT  CfpPeriod;
+    USHORT  CfpMaxDuration;
+    USHORT  CfpDurRemaining;
+    UCHAR   SsidLen;
+    CHAR    Ssid[MAX_LEN_OF_SSID];
+
+    ULONG   LastBeaconRxTime; // OS's timestamp
+
+	BOOLEAN	bSES;
+
+	// New for WPA2
+	CIPHER_SUITE					WPA;			// AP announced WPA cipher suite
+	CIPHER_SUITE					WPA2;			// AP announced WPA2 cipher suite
+
+	// New for microsoft WPA support
+	NDIS_802_11_FIXED_IEs	FixIEs;
+	NDIS_802_11_AUTHENTICATION_MODE	AuthModeAux;	// Addition mode for WPA2 / WPA capable AP
+	NDIS_802_11_AUTHENTICATION_MODE	AuthMode;
+	NDIS_802_11_WEP_STATUS	WepStatus;				// Unicast Encryption Algorithm extract from VAR_IE
+	USHORT					VarIELen;				// Length of next VIE include EID & Length
+	UCHAR					VarIEs[MAX_VIE_LEN];
+
+	// CCX Ckip information
+    UCHAR   CkipFlag;
+
+	// CCX 2 TSF
+	UCHAR	PTSF[4];		// Parent TSF
+	UCHAR	TTSF[8];		// Target TSF
+
+    // 802.11e d9, and WMM
+	EDCA_PARM           EdcaParm;
+	QOS_CAPABILITY_PARM QosCapability;
+	QBSS_LOAD_PARM      QbssLoad;
+#ifdef CONFIG_STA_SUPPORT
+    WPA_IE_     WpaIE;
+    WPA_IE_     RsnIE;
+#ifdef EXT_BUILD_CHANNEL_LIST
+	UCHAR		CountryString[3];
+	BOOLEAN		bHasCountryIE;
+#endif // EXT_BUILD_CHANNEL_LIST //
+#endif // CONFIG_STA_SUPPORT //
+} BSS_ENTRY, *PBSS_ENTRY;
+
+typedef struct {
+    UCHAR           BssNr;
+    UCHAR           BssOverlapNr;
+    BSS_ENTRY       BssEntry[MAX_LEN_OF_BSS_TABLE];
+} BSS_TABLE, *PBSS_TABLE;
+
+
+typedef struct _MLME_QUEUE_ELEM {
+    ULONG             Machine;
+    ULONG             MsgType;
+    ULONG             MsgLen;
+    UCHAR             Msg[MGMT_DMA_BUFFER_SIZE];
+    LARGE_INTEGER     TimeStamp;
+    UCHAR             Rssi0;
+    UCHAR             Rssi1;
+    UCHAR             Rssi2;
+    UCHAR             Signal;
+    UCHAR             Channel;
+    UCHAR             Wcid;
+    BOOLEAN           Occupied;
+} MLME_QUEUE_ELEM, *PMLME_QUEUE_ELEM;
+
+typedef struct _MLME_QUEUE {
+    ULONG             Num;
+    ULONG             Head;
+    ULONG             Tail;
+    NDIS_SPIN_LOCK   Lock;
+    MLME_QUEUE_ELEM  Entry[MAX_LEN_OF_MLME_QUEUE];
+} MLME_QUEUE, *PMLME_QUEUE;
+
+typedef VOID (*STATE_MACHINE_FUNC)(VOID *Adaptor, MLME_QUEUE_ELEM *Elem);
+
+typedef struct _STATE_MACHINE {
+    ULONG                           Base;
+    ULONG                           NrState;
+    ULONG                           NrMsg;
+    ULONG                           CurrState;
+    STATE_MACHINE_FUNC             *TransFunc;
+} STATE_MACHINE, *PSTATE_MACHINE;
+
+
+// MLME AUX data structure that hold temporarliy settings during a connection attempt.
+// Once this attemp succeeds, all settings will be copy to pAd->StaActive.
+// A connection attempt (user set OID, roaming, CCX fast roaming,..) consists of
+// several steps (JOIN, AUTH, ASSOC or REASSOC) and may fail at any step. We purposely
+// separate this under-trial settings away from pAd->StaActive so that once
+// this new attempt failed, driver can auto-recover back to the active settings.
+typedef struct _MLME_AUX {
+    UCHAR               BssType;
+    UCHAR               Ssid[MAX_LEN_OF_SSID];
+    UCHAR               SsidLen;
+    UCHAR               Bssid[MAC_ADDR_LEN];
+	UCHAR				AutoReconnectSsid[MAX_LEN_OF_SSID];
+	UCHAR				AutoReconnectSsidLen;
+    USHORT              Alg;
+    UCHAR               ScanType;
+    UCHAR               Channel;
+	UCHAR               CentralChannel;
+    USHORT              Aid;
+    USHORT              CapabilityInfo;
+    USHORT              BeaconPeriod;
+    USHORT              CfpMaxDuration;
+    USHORT              CfpPeriod;
+    USHORT              AtimWin;
+
+	// Copy supported rate from desired AP's beacon. We are trying to match
+	// AP's supported and extended rate settings.
+	UCHAR		        SupRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR		        ExtRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR		        SupRateLen;
+	UCHAR		        ExtRateLen;
+	HT_CAPABILITY_IE		HtCapability;
+	UCHAR		        	HtCapabilityLen;
+	ADD_HT_INFO_IE		AddHtInfo;	// AP might use this additional ht info IE
+	UCHAR			NewExtChannelOffset;
+	//RT_HT_CAPABILITY	SupportedHtPhy;
+
+    // new for QOS
+    QOS_CAPABILITY_PARM APQosCapability;    // QOS capability of the current associated AP
+    EDCA_PARM           APEdcaParm;         // EDCA parameters of the current associated AP
+    QBSS_LOAD_PARM      APQbssLoad;         // QBSS load of the current associated AP
+
+    // new to keep Ralink specific feature
+    ULONG               APRalinkIe;
+
+    BSS_TABLE           SsidBssTab;     // AP list for the same SSID
+    BSS_TABLE           RoamTab;        // AP list eligible for roaming
+    ULONG               BssIdx;
+    ULONG               RoamIdx;
+
+	BOOLEAN				CurrReqIsFromNdis;
+
+    RALINK_TIMER_STRUCT BeaconTimer, ScanTimer;
+    RALINK_TIMER_STRUCT AuthTimer;
+    RALINK_TIMER_STRUCT AssocTimer, ReassocTimer, DisassocTimer;
+} MLME_AUX, *PMLME_AUX;
+
+typedef struct _MLME_ADDBA_REQ_STRUCT{
+	UCHAR   Wcid;	//
+	UCHAR   pAddr[MAC_ADDR_LEN];
+	UCHAR   BaBufSize;
+	USHORT	TimeOutValue;
+	UCHAR   TID;
+	UCHAR   Token;
+	USHORT	BaStartSeq;
+} MLME_ADDBA_REQ_STRUCT, *PMLME_ADDBA_REQ_STRUCT;
+
+
+typedef struct _MLME_DELBA_REQ_STRUCT{
+	UCHAR   Wcid;	//
+	UCHAR     Addr[MAC_ADDR_LEN];
+	UCHAR   TID;
+	UCHAR	Initiator;
+} MLME_DELBA_REQ_STRUCT, *PMLME_DELBA_REQ_STRUCT;
+
+// assoc struct is equal to reassoc
+typedef struct _MLME_ASSOC_REQ_STRUCT{
+    UCHAR     Addr[MAC_ADDR_LEN];
+    USHORT    CapabilityInfo;
+    USHORT    ListenIntv;
+    ULONG     Timeout;
+} MLME_ASSOC_REQ_STRUCT, *PMLME_ASSOC_REQ_STRUCT, MLME_REASSOC_REQ_STRUCT, *PMLME_REASSOC_REQ_STRUCT;
+
+typedef struct _MLME_DISASSOC_REQ_STRUCT{
+    UCHAR     Addr[MAC_ADDR_LEN];
+    USHORT    Reason;
+} MLME_DISASSOC_REQ_STRUCT, *PMLME_DISASSOC_REQ_STRUCT;
+
+typedef struct _MLME_AUTH_REQ_STRUCT {
+    UCHAR        Addr[MAC_ADDR_LEN];
+    USHORT       Alg;
+    ULONG        Timeout;
+} MLME_AUTH_REQ_STRUCT, *PMLME_AUTH_REQ_STRUCT;
+
+typedef struct _MLME_DEAUTH_REQ_STRUCT {
+    UCHAR        Addr[MAC_ADDR_LEN];
+    USHORT       Reason;
+} MLME_DEAUTH_REQ_STRUCT, *PMLME_DEAUTH_REQ_STRUCT;
+
+typedef struct {
+    ULONG      BssIdx;
+} MLME_JOIN_REQ_STRUCT;
+
+typedef struct _MLME_SCAN_REQ_STRUCT {
+    UCHAR      Bssid[MAC_ADDR_LEN];
+    UCHAR      BssType;
+    UCHAR      ScanType;
+    UCHAR      SsidLen;
+    CHAR       Ssid[MAX_LEN_OF_SSID];
+} MLME_SCAN_REQ_STRUCT, *PMLME_SCAN_REQ_STRUCT;
+
+typedef struct _MLME_START_REQ_STRUCT {
+    CHAR        Ssid[MAX_LEN_OF_SSID];
+    UCHAR       SsidLen;
+} MLME_START_REQ_STRUCT, *PMLME_START_REQ_STRUCT;
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+// structure for DLS
+typedef struct _RT_802_11_DLS {
+	USHORT						TimeOut;		// Use to time out while slience, unit: second , set by UI
+	USHORT						CountDownTimer;	// Use to time out while slience,unit: second , used by driver only
+	NDIS_802_11_MAC_ADDRESS		MacAddr;		// set by UI
+	UCHAR						Status;			// 0: none , 1: wait STAkey, 2: finish DLS setup , set by driver only
+	BOOLEAN						Valid;			// 1: valid , 0: invalid , set by UI, use to setup or tear down DLS link
+	RALINK_TIMER_STRUCT			Timer;			// Use to time out while handshake
+	USHORT						Sequence;
+	USHORT						MacTabMatchWCID;	// ASIC
+	BOOLEAN						bHTCap;
+	PVOID						pAd;
+} RT_802_11_DLS, *PRT_802_11_DLS;
+
+typedef struct _MLME_DLS_REQ_STRUCT {
+    PRT_802_11_DLS	pDLS;
+    USHORT			Reason;
+} MLME_DLS_REQ_STRUCT, *PMLME_DLS_REQ_STRUCT;
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+typedef struct PACKED {
+    UCHAR   Eid;
+    UCHAR   Len;
+    CHAR   Octet[1];
+} EID_STRUCT,*PEID_STRUCT, BEACON_EID_STRUCT, *PBEACON_EID_STRUCT;
+
+typedef struct PACKED _RTMP_TX_RATE_SWITCH
+{
+	UCHAR   ItemNo;
+#ifdef RT_BIG_ENDIAN
+	UCHAR	Rsv2:2;
+	UCHAR	Mode:2;
+	UCHAR	Rsv1:1;
+	UCHAR	BW:1;
+	UCHAR	ShortGI:1;
+	UCHAR	STBC:1;
+#else
+	UCHAR	STBC:1;
+	UCHAR	ShortGI:1;
+	UCHAR	BW:1;
+	UCHAR	Rsv1:1;
+	UCHAR	Mode:2;
+	UCHAR	Rsv2:2;
+#endif
+	UCHAR   CurrMCS;
+	UCHAR   TrainUp;
+	UCHAR   TrainDown;
+} RRTMP_TX_RATE_SWITCH, *PRTMP_TX_RATE_SWITCH;
+
+// ========================== AP mlme.h ===============================
+#define TBTT_PRELOAD_TIME       384        // usec. LomgPreamble + 24-byte at 1Mbps
+#define DEFAULT_DTIM_PERIOD     1
+
+#define MAC_TABLE_AGEOUT_TIME			300			// unit: sec
+#define MAC_TABLE_ASSOC_TIMEOUT			5			// unit: sec
+#define MAC_TABLE_FULL(Tab)				((Tab).size == MAX_LEN_OF_MAC_TABLE)
+
+// AP shall drop the sta if contine Tx fail count reach it.
+#define MAC_ENTRY_LIFE_CHECK_CNT		20			// packet cnt.
+
+// Value domain of pMacEntry->Sst
+typedef enum _Sst {
+    SST_NOT_AUTH,   // 0: equivalent to IEEE 802.11/1999 state 1
+    SST_AUTH,       // 1: equivalent to IEEE 802.11/1999 state 2
+    SST_ASSOC       // 2: equivalent to IEEE 802.11/1999 state 3
+} SST;
+
+// value domain of pMacEntry->AuthState
+typedef enum _AuthState {
+    AS_NOT_AUTH,
+    AS_AUTH_OPEN,       // STA has been authenticated using OPEN SYSTEM
+    AS_AUTH_KEY,        // STA has been authenticated using SHARED KEY
+    AS_AUTHENTICATING   // STA is waiting for AUTH seq#3 using SHARED KEY
+} AUTH_STATE;
+
+//for-wpa value domain of pMacEntry->WpaState  802.1i D3   p.114
+typedef enum _ApWpaState {
+    AS_NOTUSE,              // 0
+    AS_DISCONNECT,          // 1
+    AS_DISCONNECTED,        // 2
+    AS_INITIALIZE,          // 3
+    AS_AUTHENTICATION,      // 4
+    AS_AUTHENTICATION2,     // 5
+    AS_INITPMK,             // 6
+    AS_INITPSK,             // 7
+    AS_PTKSTART,            // 8
+    AS_PTKINIT_NEGOTIATING, // 9
+    AS_PTKINITDONE,         // 10
+    AS_UPDATEKEYS,          // 11
+    AS_INTEGRITY_FAILURE,   // 12
+    AS_KEYUPDATE,           // 13
+} AP_WPA_STATE;
+
+// for-wpa value domain of pMacEntry->WpaState  802.1i D3   p.114
+typedef enum _GTKState {
+    REKEY_NEGOTIATING,
+    REKEY_ESTABLISHED,
+    KEYERROR,
+} GTK_STATE;
+
+//  for-wpa  value domain of pMacEntry->WpaState  802.1i D3   p.114
+typedef enum _WpaGTKState {
+    SETKEYS,
+    SETKEYS_DONE,
+} WPA_GTK_STATE;
+// ====================== end of AP mlme.h ============================
+
+
+#endif	// MLME_H__
diff --git a/drivers/staging/rt2860/oid.h b/drivers/staging/rt2860/oid.h
new file mode 100644
index 00000000000..f2f91b607c4
--- /dev/null
+++ b/drivers/staging/rt2860/oid.h
@@ -0,0 +1,995 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	oid.h
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+*/
+#ifndef _OID_H_
+#define _OID_H_
+
+
+#define TRUE				1
+#define FALSE				0
+//
+// IEEE 802.11 Structures and definitions
+//
+#define MAX_TX_POWER_LEVEL              100   /* mW */
+#define MAX_RSSI_TRIGGER                -10    /* dBm */
+#define MIN_RSSI_TRIGGER                -200   /* dBm */
+#define MAX_FRAG_THRESHOLD              2346  /* byte count */
+#define MIN_FRAG_THRESHOLD              256   /* byte count */
+#define MAX_RTS_THRESHOLD               2347  /* byte count */
+
+// new types for Media Specific Indications
+// Extension channel offset
+#define EXTCHA_NONE			0
+#define EXTCHA_ABOVE		0x1
+#define EXTCHA_BELOW		0x3
+
+// BW
+#define BAND_WIDTH_20		0
+#define BAND_WIDTH_40		1
+#define BAND_WIDTH_BOTH		2
+#define BAND_WIDTH_10		3	// 802.11j has 10MHz. This definition is for internal usage. doesn't fill in the IE or other field.
+// SHORTGI
+#define GAP_INTERVAL_400	1	// only support in HT mode
+#define GAP_INTERVAL_800	0
+#define GAP_INTERVAL_BOTH	2
+
+#define NdisMediaStateConnected			1
+#define NdisMediaStateDisconnected		0
+
+#define NDIS_802_11_LENGTH_SSID         32
+#define NDIS_802_11_LENGTH_RATES        8
+#define NDIS_802_11_LENGTH_RATES_EX     16
+#define MAC_ADDR_LENGTH                 6
+#define MAX_NUM_OF_CHS					49 // 14 channels @2.4G +  12@UNII + 4 @MMAC + 11 @HiperLAN2 + 7 @Japan + 1 as NULL terminationc
+#define MAX_NUMBER_OF_EVENT				10  // entry # in EVENT table
+#define MAX_NUMBER_OF_MAC				32 // if MAX_MBSSID_NUM is 8, this value can't be larger than 211
+#define MAX_NUMBER_OF_ACL				64
+#define MAX_LENGTH_OF_SUPPORT_RATES		12    // 1, 2, 5.5, 11, 6, 9, 12, 18, 24, 36, 48, 54
+#define MAX_NUMBER_OF_DLS_ENTRY			4
+
+#ifndef UNDER_CE
+
+#define OID_GEN_MACHINE_NAME               0x0001021A
+
+#ifdef RALINK_ATE
+#define RT_QUERY_ATE_TXDONE_COUNT			0x0401
+#endif // RALINK_ATE //
+#define RT_QUERY_SIGNAL_CONTEXT				0x0402
+#define RT_SET_IAPP_PID                 	0x0404
+#define RT_SET_APD_PID						0x0405
+#define RT_SET_DEL_MAC_ENTRY				0x0406
+
+//
+// IEEE 802.11 OIDs
+//
+#define	OID_GET_SET_TOGGLE			0x8000
+
+#define	OID_802_11_NETWORK_TYPES_SUPPORTED			0x0103
+#define	OID_802_11_NETWORK_TYPE_IN_USE				0x0104
+#define	OID_802_11_RSSI_TRIGGER						0x0107
+#define	RT_OID_802_11_RSSI							0x0108 //rt2860	only , kathy
+#define	RT_OID_802_11_RSSI_1						0x0109 //rt2860	only , kathy
+#define	RT_OID_802_11_RSSI_2						0x010A //rt2860	only , kathy
+#define	OID_802_11_NUMBER_OF_ANTENNAS				0x010B
+#define	OID_802_11_RX_ANTENNA_SELECTED				0x010C
+#define	OID_802_11_TX_ANTENNA_SELECTED				0x010D
+#define	OID_802_11_SUPPORTED_RATES					0x010E
+#define	OID_802_11_ADD_WEP							0x0112
+#define	OID_802_11_REMOVE_WEP						0x0113
+#define	OID_802_11_DISASSOCIATE						0x0114
+#define	OID_802_11_PRIVACY_FILTER					0x0118
+#define	OID_802_11_ASSOCIATION_INFORMATION			0x011E
+#define	OID_802_11_TEST								0x011F
+#define	RT_OID_802_11_COUNTRY_REGION				0x0507
+#define	OID_802_11_BSSID_LIST_SCAN					0x0508
+#define	OID_802_11_SSID								0x0509
+#define	OID_802_11_BSSID							0x050A
+#define	RT_OID_802_11_RADIO							0x050B
+#define	RT_OID_802_11_PHY_MODE						0x050C
+#define	RT_OID_802_11_STA_CONFIG					0x050D
+#define	OID_802_11_DESIRED_RATES					0x050E
+#define	RT_OID_802_11_PREAMBLE						0x050F
+#define	OID_802_11_WEP_STATUS						0x0510
+#define	OID_802_11_AUTHENTICATION_MODE				0x0511
+#define	OID_802_11_INFRASTRUCTURE_MODE				0x0512
+#define	RT_OID_802_11_RESET_COUNTERS				0x0513
+#define	OID_802_11_RTS_THRESHOLD					0x0514
+#define	OID_802_11_FRAGMENTATION_THRESHOLD			0x0515
+#define	OID_802_11_POWER_MODE						0x0516
+#define	OID_802_11_TX_POWER_LEVEL					0x0517
+#define	RT_OID_802_11_ADD_WPA						0x0518
+#define	OID_802_11_REMOVE_KEY						0x0519
+#define	OID_802_11_ADD_KEY							0x0520
+#define	OID_802_11_CONFIGURATION					0x0521
+#define	OID_802_11_TX_PACKET_BURST					0x0522
+#define	RT_OID_802_11_QUERY_NOISE_LEVEL				0x0523
+#define	RT_OID_802_11_EXTRA_INFO					0x0524
+#ifdef	DBG
+#define	RT_OID_802_11_HARDWARE_REGISTER				0x0525
+#endif
+#define OID_802_11_ENCRYPTION_STATUS            OID_802_11_WEP_STATUS
+#define OID_802_11_DEAUTHENTICATION                 0x0526
+#define OID_802_11_DROP_UNENCRYPTED                 0x0527
+#define OID_802_11_MIC_FAILURE_REPORT_FRAME         0x0528
+
+// For 802.1x daemin using to require current driver configuration
+#define OID_802_11_RADIUS_QUERY_SETTING				0x0540
+
+#define	RT_OID_DEVICE_NAME							0x0607
+#define	RT_OID_VERSION_INFO							0x0608
+#define	OID_802_11_BSSID_LIST						0x0609
+#define	OID_802_3_CURRENT_ADDRESS					0x060A
+#define	OID_GEN_MEDIA_CONNECT_STATUS				0x060B
+#define	RT_OID_802_11_QUERY_LINK_STATUS				0x060C
+#define	OID_802_11_RSSI								0x060D
+#define	OID_802_11_STATISTICS						0x060E
+#define	OID_GEN_RCV_OK								0x060F
+#define	OID_GEN_RCV_NO_BUFFER						0x0610
+#define	RT_OID_802_11_QUERY_EEPROM_VERSION			0x0611
+#define	RT_OID_802_11_QUERY_FIRMWARE_VERSION		0x0612
+#define	RT_OID_802_11_QUERY_LAST_RX_RATE			0x0613
+#define	RT_OID_802_11_TX_POWER_LEVEL_1				0x0614
+#define	RT_OID_802_11_QUERY_PIDVID					0x0615
+//for WPA_SUPPLICANT_SUPPORT
+#define OID_SET_COUNTERMEASURES                     0x0616
+#define OID_802_11_SET_IEEE8021X                    0x0617
+#define OID_802_11_SET_IEEE8021X_REQUIRE_KEY        0x0618
+#define OID_802_11_PMKID                            0x0620
+#define RT_OID_WPA_SUPPLICANT_SUPPORT               0x0621
+#define RT_OID_WE_VERSION_COMPILED                  0x0622
+#define RT_OID_NEW_DRIVER                           0x0623
+
+
+//rt2860 , kathy
+#define	RT_OID_802_11_SNR_0							0x0630
+#define	RT_OID_802_11_SNR_1							0x0631
+#define	RT_OID_802_11_QUERY_LAST_TX_RATE			0x0632
+#define	RT_OID_802_11_QUERY_HT_PHYMODE				0x0633
+#define	RT_OID_802_11_SET_HT_PHYMODE				0x0634
+#define	OID_802_11_RELOAD_DEFAULTS					0x0635
+#define	RT_OID_802_11_QUERY_APSD_SETTING			0x0636
+#define	RT_OID_802_11_SET_APSD_SETTING				0x0637
+#define	RT_OID_802_11_QUERY_APSD_PSM				0x0638
+#define	RT_OID_802_11_SET_APSD_PSM					0x0639
+#define	RT_OID_802_11_QUERY_DLS						0x063A
+#define	RT_OID_802_11_SET_DLS						0x063B
+#define	RT_OID_802_11_QUERY_DLS_PARAM				0x063C
+#define	RT_OID_802_11_SET_DLS_PARAM					0x063D
+#define RT_OID_802_11_QUERY_WMM              		0x063E
+#define RT_OID_802_11_SET_WMM      					0x063F
+#define RT_OID_802_11_QUERY_IMME_BA_CAP				0x0640
+#define RT_OID_802_11_SET_IMME_BA_CAP				0x0641
+#define RT_OID_802_11_QUERY_BATABLE					0x0642
+#define RT_OID_802_11_ADD_IMME_BA					0x0643
+#define RT_OID_802_11_TEAR_IMME_BA					0x0644
+#define RT_OID_DRIVER_DEVICE_NAME                   0x0645
+#define RT_OID_802_11_QUERY_DAT_HT_PHYMODE          0x0646
+#define RT_OID_QUERY_MULTIPLE_CARD_SUPPORT          0x0647
+
+// Ralink defined OIDs
+// Dennis Lee move to platform specific
+
+#define	RT_OID_802_11_BSSID					  (OID_GET_SET_TOGGLE |	OID_802_11_BSSID)
+#define	RT_OID_802_11_SSID					  (OID_GET_SET_TOGGLE |	OID_802_11_SSID)
+#define	RT_OID_802_11_INFRASTRUCTURE_MODE	  (OID_GET_SET_TOGGLE |	OID_802_11_INFRASTRUCTURE_MODE)
+#define	RT_OID_802_11_ADD_WEP				  (OID_GET_SET_TOGGLE |	OID_802_11_ADD_WEP)
+#define	RT_OID_802_11_ADD_KEY				  (OID_GET_SET_TOGGLE |	OID_802_11_ADD_KEY)
+#define	RT_OID_802_11_REMOVE_WEP			  (OID_GET_SET_TOGGLE |	OID_802_11_REMOVE_WEP)
+#define	RT_OID_802_11_REMOVE_KEY			  (OID_GET_SET_TOGGLE |	OID_802_11_REMOVE_KEY)
+#define	RT_OID_802_11_DISASSOCIATE			  (OID_GET_SET_TOGGLE |	OID_802_11_DISASSOCIATE)
+#define	RT_OID_802_11_AUTHENTICATION_MODE	  (OID_GET_SET_TOGGLE |	OID_802_11_AUTHENTICATION_MODE)
+#define	RT_OID_802_11_PRIVACY_FILTER		  (OID_GET_SET_TOGGLE |	OID_802_11_PRIVACY_FILTER)
+#define	RT_OID_802_11_BSSID_LIST_SCAN		  (OID_GET_SET_TOGGLE |	OID_802_11_BSSID_LIST_SCAN)
+#define	RT_OID_802_11_WEP_STATUS			  (OID_GET_SET_TOGGLE |	OID_802_11_WEP_STATUS)
+#define	RT_OID_802_11_RELOAD_DEFAULTS		  (OID_GET_SET_TOGGLE |	OID_802_11_RELOAD_DEFAULTS)
+#define	RT_OID_802_11_NETWORK_TYPE_IN_USE	  (OID_GET_SET_TOGGLE |	OID_802_11_NETWORK_TYPE_IN_USE)
+#define	RT_OID_802_11_TX_POWER_LEVEL		  (OID_GET_SET_TOGGLE |	OID_802_11_TX_POWER_LEVEL)
+#define	RT_OID_802_11_RSSI_TRIGGER			  (OID_GET_SET_TOGGLE |	OID_802_11_RSSI_TRIGGER)
+#define	RT_OID_802_11_FRAGMENTATION_THRESHOLD (OID_GET_SET_TOGGLE |	OID_802_11_FRAGMENTATION_THRESHOLD)
+#define	RT_OID_802_11_RTS_THRESHOLD			  (OID_GET_SET_TOGGLE |	OID_802_11_RTS_THRESHOLD)
+#define	RT_OID_802_11_RX_ANTENNA_SELECTED	  (OID_GET_SET_TOGGLE |	OID_802_11_RX_ANTENNA_SELECTED)
+#define	RT_OID_802_11_TX_ANTENNA_SELECTED	  (OID_GET_SET_TOGGLE |	OID_802_11_TX_ANTENNA_SELECTED)
+#define	RT_OID_802_11_SUPPORTED_RATES		  (OID_GET_SET_TOGGLE |	OID_802_11_SUPPORTED_RATES)
+#define	RT_OID_802_11_DESIRED_RATES			  (OID_GET_SET_TOGGLE |	OID_802_11_DESIRED_RATES)
+#define	RT_OID_802_11_CONFIGURATION			  (OID_GET_SET_TOGGLE |	OID_802_11_CONFIGURATION)
+#define	RT_OID_802_11_POWER_MODE			  (OID_GET_SET_TOGGLE |	OID_802_11_POWER_MODE)
+
+typedef enum _NDIS_802_11_STATUS_TYPE
+{
+    Ndis802_11StatusType_Authentication,
+    Ndis802_11StatusType_MediaStreamMode,
+    Ndis802_11StatusType_PMKID_CandidateList,
+    Ndis802_11StatusTypeMax    // not a real type, defined as an upper bound
+} NDIS_802_11_STATUS_TYPE, *PNDIS_802_11_STATUS_TYPE;
+
+typedef UCHAR   NDIS_802_11_MAC_ADDRESS[6];
+
+typedef struct _NDIS_802_11_STATUS_INDICATION
+{
+    NDIS_802_11_STATUS_TYPE StatusType;
+} NDIS_802_11_STATUS_INDICATION, *PNDIS_802_11_STATUS_INDICATION;
+
+// mask for authentication/integrity fields
+#define NDIS_802_11_AUTH_REQUEST_AUTH_FIELDS        0x0f
+
+#define NDIS_802_11_AUTH_REQUEST_REAUTH             0x01
+#define NDIS_802_11_AUTH_REQUEST_KEYUPDATE          0x02
+#define NDIS_802_11_AUTH_REQUEST_PAIRWISE_ERROR     0x06
+#define NDIS_802_11_AUTH_REQUEST_GROUP_ERROR        0x0E
+
+typedef struct _NDIS_802_11_AUTHENTICATION_REQUEST
+{
+    ULONG Length;            // Length of structure
+    NDIS_802_11_MAC_ADDRESS Bssid;
+    ULONG Flags;
+} NDIS_802_11_AUTHENTICATION_REQUEST, *PNDIS_802_11_AUTHENTICATION_REQUEST;
+
+//Added new types for PMKID Candidate lists.
+typedef struct _PMKID_CANDIDATE {
+    NDIS_802_11_MAC_ADDRESS BSSID;
+    ULONG Flags;
+} PMKID_CANDIDATE, *PPMKID_CANDIDATE;
+
+typedef struct _NDIS_802_11_PMKID_CANDIDATE_LIST
+{
+    ULONG Version;       // Version of the structure
+    ULONG NumCandidates; // No. of pmkid candidates
+    PMKID_CANDIDATE CandidateList[1];
+} NDIS_802_11_PMKID_CANDIDATE_LIST, *PNDIS_802_11_PMKID_CANDIDATE_LIST;
+
+//Flags for PMKID Candidate list structure
+#define NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED	0x01
+
+// Added new types for OFDM 5G and 2.4G
+typedef enum _NDIS_802_11_NETWORK_TYPE
+{
+   Ndis802_11FH,
+   Ndis802_11DS,
+    Ndis802_11OFDM5,
+    Ndis802_11OFDM5_N,
+    Ndis802_11OFDM24,
+    Ndis802_11OFDM24_N,
+   Ndis802_11Automode,
+    Ndis802_11NetworkTypeMax    // not a real type, defined as an upper bound
+} NDIS_802_11_NETWORK_TYPE, *PNDIS_802_11_NETWORK_TYPE;
+
+typedef struct _NDIS_802_11_NETWORK_TYPE_LIST
+{
+    UINT                       NumberOfItems;  // in list below, at least 1
+   NDIS_802_11_NETWORK_TYPE    NetworkType [1];
+} NDIS_802_11_NETWORK_TYPE_LIST, *PNDIS_802_11_NETWORK_TYPE_LIST;
+
+typedef enum _NDIS_802_11_POWER_MODE
+{
+    Ndis802_11PowerModeCAM,
+    Ndis802_11PowerModeMAX_PSP,
+    Ndis802_11PowerModeFast_PSP,
+    Ndis802_11PowerModeLegacy_PSP,
+    Ndis802_11PowerModeMax      // not a real mode, defined as an upper bound
+} NDIS_802_11_POWER_MODE, *PNDIS_802_11_POWER_MODE;
+
+typedef ULONG   NDIS_802_11_TX_POWER_LEVEL; // in milliwatts
+
+//
+// Received Signal Strength Indication
+//
+typedef LONG    NDIS_802_11_RSSI;           // in dBm
+
+typedef struct _NDIS_802_11_CONFIGURATION_FH
+{
+   ULONG           Length;            // Length of structure
+   ULONG           HopPattern;        // As defined by 802.11, MSB set
+   ULONG           HopSet;            // to one if non-802.11
+   ULONG           DwellTime;         // units are Kusec
+} NDIS_802_11_CONFIGURATION_FH, *PNDIS_802_11_CONFIGURATION_FH;
+
+typedef struct _NDIS_802_11_CONFIGURATION
+{
+   ULONG                           Length;             // Length of structure
+   ULONG                           BeaconPeriod;       // units are Kusec
+   ULONG                           ATIMWindow;         // units are Kusec
+   ULONG                           DSConfig;           // Frequency, units are kHz
+   NDIS_802_11_CONFIGURATION_FH    FHConfig;
+} NDIS_802_11_CONFIGURATION, *PNDIS_802_11_CONFIGURATION;
+
+typedef struct _NDIS_802_11_STATISTICS
+{
+   ULONG           Length;             // Length of structure
+   LARGE_INTEGER   TransmittedFragmentCount;
+   LARGE_INTEGER   MulticastTransmittedFrameCount;
+   LARGE_INTEGER   FailedCount;
+   LARGE_INTEGER   RetryCount;
+   LARGE_INTEGER   MultipleRetryCount;
+   LARGE_INTEGER   RTSSuccessCount;
+   LARGE_INTEGER   RTSFailureCount;
+   LARGE_INTEGER   ACKFailureCount;
+   LARGE_INTEGER   FrameDuplicateCount;
+   LARGE_INTEGER   ReceivedFragmentCount;
+   LARGE_INTEGER   MulticastReceivedFrameCount;
+   LARGE_INTEGER   FCSErrorCount;
+   LARGE_INTEGER   TKIPLocalMICFailures;
+   LARGE_INTEGER   TKIPRemoteMICErrors;
+   LARGE_INTEGER   TKIPICVErrors;
+   LARGE_INTEGER   TKIPCounterMeasuresInvoked;
+   LARGE_INTEGER   TKIPReplays;
+   LARGE_INTEGER   CCMPFormatErrors;
+   LARGE_INTEGER   CCMPReplays;
+   LARGE_INTEGER   CCMPDecryptErrors;
+   LARGE_INTEGER   FourWayHandshakeFailures;
+} NDIS_802_11_STATISTICS, *PNDIS_802_11_STATISTICS;
+
+typedef  ULONG  NDIS_802_11_KEY_INDEX;
+typedef ULONGLONG   NDIS_802_11_KEY_RSC;
+
+#define MAX_RADIUS_SRV_NUM			2	  // 802.1x failover number
+
+typedef struct PACKED _RADIUS_SRV_INFO {
+	UINT32			radius_ip;
+	UINT32			radius_port;
+	UCHAR			radius_key[64];
+	UCHAR			radius_key_len;
+} RADIUS_SRV_INFO, *PRADIUS_SRV_INFO;
+
+typedef struct PACKED _RADIUS_KEY_INFO
+{
+	UCHAR			radius_srv_num;
+	RADIUS_SRV_INFO	radius_srv_info[MAX_RADIUS_SRV_NUM];
+	UCHAR			ieee8021xWEP;		 // dynamic WEP
+    UCHAR           key_index;
+    UCHAR           key_length;          // length of key in bytes
+    UCHAR           key_material[13];
+} RADIUS_KEY_INFO, *PRADIUS_KEY_INFO;
+
+// It's used by 802.1x daemon to require relative configuration
+typedef struct PACKED _RADIUS_CONF
+{
+    UINT32          Length;             // Length of this structure
+    UCHAR			mbss_num;			// indicate multiple BSS number
+	UINT32			own_ip_addr;
+	UINT32			retry_interval;
+	UINT32			session_timeout_interval;
+	UCHAR			EAPifname[IFNAMSIZ];
+	UCHAR			EAPifname_len;
+	UCHAR 			PreAuthifname[IFNAMSIZ];
+	UCHAR			PreAuthifname_len;
+	RADIUS_KEY_INFO	RadiusInfo[8/*MAX_MBSSID_NUM*/];
+} RADIUS_CONF, *PRADIUS_CONF;
+
+
+
+#ifdef CONFIG_STA_SUPPORT
+// Key mapping keys require a BSSID
+typedef struct _NDIS_802_11_KEY
+{
+    UINT           Length;             // Length of this structure
+    UINT           KeyIndex;
+    UINT           KeyLength;          // length of key in bytes
+    NDIS_802_11_MAC_ADDRESS BSSID;
+    NDIS_802_11_KEY_RSC KeyRSC;
+    UCHAR           KeyMaterial[1];     // variable length depending on above field
+} NDIS_802_11_KEY, *PNDIS_802_11_KEY;
+#endif // CONFIG_STA_SUPPORT //
+
+typedef struct _NDIS_802_11_REMOVE_KEY
+{
+    UINT           Length;             // Length of this structure
+    UINT           KeyIndex;
+    NDIS_802_11_MAC_ADDRESS BSSID;
+} NDIS_802_11_REMOVE_KEY, *PNDIS_802_11_REMOVE_KEY;
+
+typedef struct _NDIS_802_11_WEP
+{
+   UINT     Length;        // Length of this structure
+   UINT     KeyIndex;           // 0 is the per-client key, 1-N are the
+                                        // global keys
+   UINT     KeyLength;     // length of key in bytes
+   UCHAR     KeyMaterial[1];// variable length depending on above field
+} NDIS_802_11_WEP, *PNDIS_802_11_WEP;
+
+
+typedef enum _NDIS_802_11_NETWORK_INFRASTRUCTURE
+{
+   Ndis802_11IBSS,
+   Ndis802_11Infrastructure,
+   Ndis802_11AutoUnknown,
+   Ndis802_11Monitor,
+   Ndis802_11InfrastructureMax     // Not a real value, defined as upper bound
+} NDIS_802_11_NETWORK_INFRASTRUCTURE, *PNDIS_802_11_NETWORK_INFRASTRUCTURE;
+
+// Add new authentication modes
+typedef enum _NDIS_802_11_AUTHENTICATION_MODE
+{
+   Ndis802_11AuthModeOpen,
+   Ndis802_11AuthModeShared,
+   Ndis802_11AuthModeAutoSwitch,
+    Ndis802_11AuthModeWPA,
+    Ndis802_11AuthModeWPAPSK,
+    Ndis802_11AuthModeWPANone,
+   Ndis802_11AuthModeWPA2,
+   Ndis802_11AuthModeWPA2PSK,
+   	Ndis802_11AuthModeWPA1WPA2,
+	Ndis802_11AuthModeWPA1PSKWPA2PSK,
+   Ndis802_11AuthModeMax           // Not a real mode, defined as upper bound
+} NDIS_802_11_AUTHENTICATION_MODE, *PNDIS_802_11_AUTHENTICATION_MODE;
+
+typedef UCHAR   NDIS_802_11_RATES[NDIS_802_11_LENGTH_RATES];        // Set of 8 data rates
+typedef UCHAR   NDIS_802_11_RATES_EX[NDIS_802_11_LENGTH_RATES_EX];  // Set of 16 data rates
+
+typedef struct PACKED _NDIS_802_11_SSID
+{
+    UINT   SsidLength;         // length of SSID field below, in bytes;
+                                // this can be zero.
+    UCHAR   Ssid[NDIS_802_11_LENGTH_SSID];           // SSID information field
+} NDIS_802_11_SSID, *PNDIS_802_11_SSID;
+
+
+typedef struct PACKED _NDIS_WLAN_BSSID
+{
+   ULONG                               Length;     // Length of this structure
+   NDIS_802_11_MAC_ADDRESS             MacAddress; // BSSID
+   UCHAR                               Reserved[2];
+   NDIS_802_11_SSID                    Ssid;       // SSID
+   ULONG                               Privacy;    // WEP encryption requirement
+   NDIS_802_11_RSSI                    Rssi;       // receive signal strength in dBm
+   NDIS_802_11_NETWORK_TYPE            NetworkTypeInUse;
+   NDIS_802_11_CONFIGURATION           Configuration;
+   NDIS_802_11_NETWORK_INFRASTRUCTURE  InfrastructureMode;
+   NDIS_802_11_RATES                   SupportedRates;
+} NDIS_WLAN_BSSID, *PNDIS_WLAN_BSSID;
+
+typedef struct PACKED _NDIS_802_11_BSSID_LIST
+{
+   UINT           NumberOfItems;      // in list below, at least 1
+   NDIS_WLAN_BSSID Bssid[1];
+} NDIS_802_11_BSSID_LIST, *PNDIS_802_11_BSSID_LIST;
+
+// Added Capabilities, IELength and IEs for each BSSID
+typedef struct PACKED _NDIS_WLAN_BSSID_EX
+{
+    ULONG                               Length;             // Length of this structure
+    NDIS_802_11_MAC_ADDRESS             MacAddress;         // BSSID
+    UCHAR                               Reserved[2];
+    NDIS_802_11_SSID                    Ssid;               // SSID
+    UINT                                Privacy;            // WEP encryption requirement
+    NDIS_802_11_RSSI                    Rssi;               // receive signal
+                                                            // strength in dBm
+    NDIS_802_11_NETWORK_TYPE            NetworkTypeInUse;
+    NDIS_802_11_CONFIGURATION           Configuration;
+    NDIS_802_11_NETWORK_INFRASTRUCTURE  InfrastructureMode;
+    NDIS_802_11_RATES_EX                SupportedRates;
+    ULONG                               IELength;
+    UCHAR                               IEs[1];
+} NDIS_WLAN_BSSID_EX, *PNDIS_WLAN_BSSID_EX;
+
+typedef struct PACKED _NDIS_802_11_BSSID_LIST_EX
+{
+    UINT                   NumberOfItems;      // in list below, at least 1
+    NDIS_WLAN_BSSID_EX      Bssid[1];
+} NDIS_802_11_BSSID_LIST_EX, *PNDIS_802_11_BSSID_LIST_EX;
+
+typedef struct PACKED _NDIS_802_11_FIXED_IEs
+{
+    UCHAR Timestamp[8];
+    USHORT BeaconInterval;
+    USHORT Capabilities;
+} NDIS_802_11_FIXED_IEs, *PNDIS_802_11_FIXED_IEs;
+
+typedef struct _NDIS_802_11_VARIABLE_IEs
+{
+    UCHAR ElementID;
+    UCHAR Length;    // Number of bytes in data field
+    UCHAR data[1];
+} NDIS_802_11_VARIABLE_IEs, *PNDIS_802_11_VARIABLE_IEs;
+
+typedef  ULONG   NDIS_802_11_FRAGMENTATION_THRESHOLD;
+
+typedef  ULONG   NDIS_802_11_RTS_THRESHOLD;
+
+typedef  ULONG   NDIS_802_11_ANTENNA;
+
+typedef enum _NDIS_802_11_PRIVACY_FILTER
+{
+   Ndis802_11PrivFilterAcceptAll,
+   Ndis802_11PrivFilter8021xWEP
+} NDIS_802_11_PRIVACY_FILTER, *PNDIS_802_11_PRIVACY_FILTER;
+
+// Added new encryption types
+// Also aliased typedef to new name
+typedef enum _NDIS_802_11_WEP_STATUS
+{
+   Ndis802_11WEPEnabled,
+    Ndis802_11Encryption1Enabled = Ndis802_11WEPEnabled,
+   Ndis802_11WEPDisabled,
+    Ndis802_11EncryptionDisabled = Ndis802_11WEPDisabled,
+   Ndis802_11WEPKeyAbsent,
+    Ndis802_11Encryption1KeyAbsent = Ndis802_11WEPKeyAbsent,
+   Ndis802_11WEPNotSupported,
+    Ndis802_11EncryptionNotSupported = Ndis802_11WEPNotSupported,
+    Ndis802_11Encryption2Enabled,
+    Ndis802_11Encryption2KeyAbsent,
+    Ndis802_11Encryption3Enabled,
+    Ndis802_11Encryption3KeyAbsent,
+    Ndis802_11Encryption4Enabled,	// TKIP or AES mix
+    Ndis802_11Encryption4KeyAbsent,
+} NDIS_802_11_WEP_STATUS, *PNDIS_802_11_WEP_STATUS,
+  NDIS_802_11_ENCRYPTION_STATUS, *PNDIS_802_11_ENCRYPTION_STATUS;
+
+typedef enum _NDIS_802_11_RELOAD_DEFAULTS
+{
+   Ndis802_11ReloadWEPKeys
+} NDIS_802_11_RELOAD_DEFAULTS, *PNDIS_802_11_RELOAD_DEFAULTS;
+
+#define NDIS_802_11_AI_REQFI_CAPABILITIES      1
+#define NDIS_802_11_AI_REQFI_LISTENINTERVAL    2
+#define NDIS_802_11_AI_REQFI_CURRENTAPADDRESS  4
+
+#define NDIS_802_11_AI_RESFI_CAPABILITIES      1
+#define NDIS_802_11_AI_RESFI_STATUSCODE        2
+#define NDIS_802_11_AI_RESFI_ASSOCIATIONID     4
+
+typedef struct _NDIS_802_11_AI_REQFI
+{
+    USHORT Capabilities;
+    USHORT ListenInterval;
+    NDIS_802_11_MAC_ADDRESS  CurrentAPAddress;
+} NDIS_802_11_AI_REQFI, *PNDIS_802_11_AI_REQFI;
+
+typedef struct _NDIS_802_11_AI_RESFI
+{
+    USHORT Capabilities;
+    USHORT StatusCode;
+    USHORT AssociationId;
+} NDIS_802_11_AI_RESFI, *PNDIS_802_11_AI_RESFI;
+
+typedef struct _NDIS_802_11_ASSOCIATION_INFORMATION
+{
+    ULONG                   Length;
+    USHORT                  AvailableRequestFixedIEs;
+    NDIS_802_11_AI_REQFI    RequestFixedIEs;
+    ULONG                   RequestIELength;
+    ULONG                   OffsetRequestIEs;
+    USHORT                  AvailableResponseFixedIEs;
+    NDIS_802_11_AI_RESFI    ResponseFixedIEs;
+    ULONG                   ResponseIELength;
+    ULONG                   OffsetResponseIEs;
+} NDIS_802_11_ASSOCIATION_INFORMATION, *PNDIS_802_11_ASSOCIATION_INFORMATION;
+
+typedef struct _NDIS_802_11_AUTHENTICATION_EVENT
+{
+    NDIS_802_11_STATUS_INDICATION       Status;
+    NDIS_802_11_AUTHENTICATION_REQUEST  Request[1];
+} NDIS_802_11_AUTHENTICATION_EVENT, *PNDIS_802_11_AUTHENTICATION_EVENT;
+
+// 802.11 Media stream constraints, associated with OID_802_11_MEDIA_STREAM_MODE
+typedef enum _NDIS_802_11_MEDIA_STREAM_MODE
+{
+    Ndis802_11MediaStreamOff,
+    Ndis802_11MediaStreamOn,
+} NDIS_802_11_MEDIA_STREAM_MODE, *PNDIS_802_11_MEDIA_STREAM_MODE;
+
+// PMKID Structures
+typedef UCHAR   NDIS_802_11_PMKID_VALUE[16];
+
+#ifdef CONFIG_STA_SUPPORT
+typedef struct _BSSID_INFO
+{
+    NDIS_802_11_MAC_ADDRESS BSSID;
+    NDIS_802_11_PMKID_VALUE PMKID;
+} BSSID_INFO, *PBSSID_INFO;
+
+typedef struct _NDIS_802_11_PMKID
+{
+    UINT    Length;
+    UINT    BSSIDInfoCount;
+    BSSID_INFO BSSIDInfo[1];
+} NDIS_802_11_PMKID, *PNDIS_802_11_PMKID;
+#endif // CONFIG_STA_SUPPORT //
+
+
+typedef struct _NDIS_802_11_AUTHENTICATION_ENCRYPTION
+{
+    NDIS_802_11_AUTHENTICATION_MODE AuthModeSupported;
+    NDIS_802_11_ENCRYPTION_STATUS EncryptStatusSupported;
+} NDIS_802_11_AUTHENTICATION_ENCRYPTION, *PNDIS_802_11_AUTHENTICATION_ENCRYPTION;
+
+typedef struct _NDIS_802_11_CAPABILITY
+{
+     ULONG Length;
+     ULONG Version;
+     ULONG NoOfPMKIDs;
+     ULONG NoOfAuthEncryptPairsSupported;
+     NDIS_802_11_AUTHENTICATION_ENCRYPTION AuthenticationEncryptionSupported[1];
+} NDIS_802_11_CAPABILITY, *PNDIS_802_11_CAPABILITY;
+
+//#endif //of WIN 2k
+#endif //UNDER_CE
+
+#if WIRELESS_EXT <= 11
+#ifndef SIOCDEVPRIVATE
+#define SIOCDEVPRIVATE                              0x8BE0
+#endif
+#define SIOCIWFIRSTPRIV								SIOCDEVPRIVATE
+#endif
+
+#ifdef CONFIG_STA_SUPPORT
+#define RTPRIV_IOCTL_SET							(SIOCIWFIRSTPRIV + 0x02)
+
+#ifdef DBG
+#define RTPRIV_IOCTL_BBP                            (SIOCIWFIRSTPRIV + 0x03)
+#define RTPRIV_IOCTL_MAC                            (SIOCIWFIRSTPRIV + 0x05)
+#define RTPRIV_IOCTL_E2P                            (SIOCIWFIRSTPRIV + 0x07)
+#endif
+
+#ifdef RALINK_ATE
+#ifdef RALINK_28xx_QA
+#define RTPRIV_IOCTL_ATE							(SIOCIWFIRSTPRIV + 0x08)
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+
+#define RTPRIV_IOCTL_STATISTICS                     (SIOCIWFIRSTPRIV + 0x09)
+#define RTPRIV_IOCTL_ADD_PMKID_CACHE                (SIOCIWFIRSTPRIV + 0x0A)
+#define RTPRIV_IOCTL_RADIUS_DATA                    (SIOCIWFIRSTPRIV + 0x0C)
+#define RTPRIV_IOCTL_GSITESURVEY					(SIOCIWFIRSTPRIV + 0x0D)
+#define RT_PRIV_IOCTL								(SIOCIWFIRSTPRIV + 0x0E) // Sync. with RT61 (for wpa_supplicant)
+#define RTPRIV_IOCTL_GET_MAC_TABLE					(SIOCIWFIRSTPRIV + 0x0F)
+
+#define RTPRIV_IOCTL_SHOW							(SIOCIWFIRSTPRIV + 0x11)
+enum {
+    SHOW_CONN_STATUS = 4,
+    SHOW_DRVIER_VERION = 5,
+    SHOW_BA_INFO = 6,
+	SHOW_DESC_INFO = 7,
+    RAIO_OFF = 10,
+    RAIO_ON = 11,
+#ifdef QOS_DLS_SUPPORT
+	SHOW_DLS_ENTRY_INFO = 19,
+#endif // QOS_DLS_SUPPORT //
+	SHOW_CFG_VALUE = 20,
+};
+
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef SNMP_SUPPORT
+//SNMP ieee 802dot11, kathy , 2008_0220
+// dot11res(3)
+#define RT_OID_802_11_MANUFACTUREROUI			0x0700
+#define RT_OID_802_11_MANUFACTURERNAME			0x0701
+#define RT_OID_802_11_RESOURCETYPEIDNAME		0x0702
+
+// dot11smt(1)
+#define RT_OID_802_11_PRIVACYOPTIONIMPLEMENTED	0x0703
+#define RT_OID_802_11_POWERMANAGEMENTMODE		0x0704
+#define OID_802_11_WEPDEFAULTKEYVALUE			0x0705 // read , write
+#define OID_802_11_WEPDEFAULTKEYID				0x0706
+#define RT_OID_802_11_WEPKEYMAPPINGLENGTH		0x0707
+#define OID_802_11_SHORTRETRYLIMIT				0x0708
+#define OID_802_11_LONGRETRYLIMIT				0x0709
+#define RT_OID_802_11_PRODUCTID					0x0710
+#define RT_OID_802_11_MANUFACTUREID				0x0711
+
+// //dot11Phy(4)
+#define OID_802_11_CURRENTCHANNEL				0x0712
+
+//dot11mac
+#define RT_OID_802_11_MAC_ADDRESS				0x0713
+#endif // SNMP_SUPPORT //
+
+#define OID_802_11_BUILD_CHANNEL_EX				0x0714
+#define OID_802_11_GET_CH_LIST					0x0715
+#define OID_802_11_GET_COUNTRY_CODE				0x0716
+#define OID_802_11_GET_CHANNEL_GEOGRAPHY		0x0717
+
+#ifdef LLTD_SUPPORT
+// for consistency with RT61
+#define RT_OID_GET_PHY_MODE                         0x761
+#endif // LLTD_SUPPORT //
+
+// New for MeetingHouse Api support
+#define OID_MH_802_1X_SUPPORTED               0xFFEDC100
+
+// MIMO Tx parameter, ShortGI, MCS, STBC, etc.  these are fields in TXWI. Don't change this definition!!!
+typedef union  _HTTRANSMIT_SETTING {
+#ifdef RT_BIG_ENDIAN
+	struct	{
+	USHORT		MODE:2;	// Use definition MODE_xxx.
+	USHORT		TxBF:1;
+	USHORT		rsv:2;
+	USHORT		STBC:2;	//SPACE
+	USHORT		ShortGI:1;
+	USHORT		BW:1;	//channel bandwidth 20MHz or 40 MHz
+	USHORT   	MCS:7;                 // MCS
+	}	field;
+#else
+	struct	{
+	USHORT   	MCS:7;                 // MCS
+	USHORT		BW:1;	//channel bandwidth 20MHz or 40 MHz
+	USHORT		ShortGI:1;
+	USHORT		STBC:2;	//SPACE
+	USHORT		rsv:2;
+	USHORT		TxBF:1;
+	USHORT		MODE:2;	// Use definition MODE_xxx.
+	}	field;
+#endif
+	USHORT		word;
+ } HTTRANSMIT_SETTING, *PHTTRANSMIT_SETTING;
+
+typedef enum _RT_802_11_PREAMBLE {
+    Rt802_11PreambleLong,
+    Rt802_11PreambleShort,
+    Rt802_11PreambleAuto
+} RT_802_11_PREAMBLE, *PRT_802_11_PREAMBLE;
+
+// Only for STA, need to sync with AP
+// 2005-03-08 match current RaConfig.
+typedef enum _RT_802_11_PHY_MODE {
+	PHY_11BG_MIXED = 0,
+	PHY_11B,
+	PHY_11A,
+	PHY_11ABG_MIXED,
+	PHY_11G,
+#ifdef DOT11_N_SUPPORT
+	PHY_11ABGN_MIXED,	// both band   5
+	PHY_11N_2_4G,		// 11n-only with 2.4G band   	6
+	PHY_11GN_MIXED,	// 2.4G band      7
+	PHY_11AN_MIXED,	// 5G  band       8
+	PHY_11BGN_MIXED,	// if check 802.11b.      9
+	PHY_11AGN_MIXED,	// if check 802.11b.      10
+	PHY_11N_5G,			// 11n-only with 5G band		11
+#endif // DOT11_N_SUPPORT //
+} RT_802_11_PHY_MODE;
+
+// put all proprietery for-query objects here to reduce # of Query_OID
+typedef struct _RT_802_11_LINK_STATUS {
+    ULONG   CurrTxRate;         // in units of 0.5Mbps
+    ULONG   ChannelQuality;     // 0..100 %
+    ULONG   TxByteCount;        // both ok and fail
+    ULONG   RxByteCount;        // both ok and fail
+    ULONG	CentralChannel;		// 40MHz central channel number
+} RT_802_11_LINK_STATUS, *PRT_802_11_LINK_STATUS;
+
+typedef struct _RT_802_11_EVENT_LOG {
+    LARGE_INTEGER   SystemTime;  // timestammp via NdisGetCurrentSystemTime()
+    UCHAR           Addr[MAC_ADDR_LENGTH];
+    USHORT          Event;       // EVENT_xxx
+} RT_802_11_EVENT_LOG, *PRT_802_11_EVENT_LOG;
+
+typedef struct _RT_802_11_EVENT_TABLE {
+    ULONG       Num;
+    ULONG       Rsv;     // to align Log[] at LARGE_INEGER boundary
+    RT_802_11_EVENT_LOG   Log[MAX_NUMBER_OF_EVENT];
+} RT_802_11_EVENT_TABLE, PRT_802_11_EVENT_TABLE;
+
+// MIMO Tx parameter, ShortGI, MCS, STBC, etc.  these are fields in TXWI. Don't change this definition!!!
+typedef union  _MACHTTRANSMIT_SETTING {
+	struct	{
+	USHORT   	MCS:7;                 // MCS
+	USHORT		BW:1;	//channel bandwidth 20MHz or 40 MHz
+	USHORT		ShortGI:1;
+	USHORT		STBC:2;	//SPACE
+	USHORT		rsv:3;
+	USHORT		MODE:2;	// Use definition MODE_xxx.
+	}	field;
+	USHORT		word;
+ } MACHTTRANSMIT_SETTING, *PMACHTTRANSMIT_SETTING;
+
+typedef struct _RT_802_11_MAC_ENTRY {
+    UCHAR       Addr[MAC_ADDR_LENGTH];
+    UCHAR       Aid;
+    UCHAR       Psm;     // 0:PWR_ACTIVE, 1:PWR_SAVE
+    UCHAR		MimoPs;  // 0:MMPS_STATIC, 1:MMPS_DYNAMIC, 3:MMPS_Enabled
+    CHAR		AvgRssi0;
+	CHAR		AvgRssi1;
+	CHAR		AvgRssi2;
+	UINT32		ConnectedTime;
+    MACHTTRANSMIT_SETTING	TxRate;
+} RT_802_11_MAC_ENTRY, *PRT_802_11_MAC_ENTRY;
+
+typedef struct _RT_802_11_MAC_TABLE {
+    ULONG       Num;
+    RT_802_11_MAC_ENTRY Entry[MAX_NUMBER_OF_MAC];
+} RT_802_11_MAC_TABLE, *PRT_802_11_MAC_TABLE;
+
+// structure for query/set hardware register - MAC, BBP, RF register
+typedef struct _RT_802_11_HARDWARE_REGISTER {
+    ULONG   HardwareType;       // 0:MAC, 1:BBP, 2:RF register, 3:EEPROM
+    ULONG   Offset;             // Q/S register offset addr
+    ULONG   Data;               // R/W data buffer
+} RT_802_11_HARDWARE_REGISTER, *PRT_802_11_HARDWARE_REGISTER;
+
+typedef struct _RT_802_11_AP_CONFIG {
+    ULONG   EnableTxBurst;      // 0-disable, 1-enable
+    ULONG   EnableTurboRate;    // 0-disable, 1-enable 72/100mbps turbo rate
+    ULONG   IsolateInterStaTraffic;     // 0-disable, 1-enable isolation
+    ULONG   HideSsid;           // 0-disable, 1-enable hiding
+    ULONG   UseBGProtection;    // 0-AUTO, 1-always ON, 2-always OFF
+    ULONG   UseShortSlotTime;   // 0-no use, 1-use 9-us short slot time
+    ULONG   Rsv1;               // must be 0
+    ULONG   SystemErrorBitmap;  // ignore upon SET, return system error upon QUERY
+} RT_802_11_AP_CONFIG, *PRT_802_11_AP_CONFIG;
+
+// structure to query/set STA_CONFIG
+typedef struct _RT_802_11_STA_CONFIG {
+    ULONG   EnableTxBurst;      // 0-disable, 1-enable
+    ULONG   EnableTurboRate;    // 0-disable, 1-enable 72/100mbps turbo rate
+    ULONG   UseBGProtection;    // 0-AUTO, 1-always ON, 2-always OFF
+    ULONG   UseShortSlotTime;   // 0-no use, 1-use 9-us short slot time when applicable
+    ULONG   AdhocMode; 			// 0-11b rates only (WIFI spec), 1 - b/g mixed, 2 - g only
+    ULONG   HwRadioStatus;      // 0-OFF, 1-ON, default is 1, Read-Only
+    ULONG   Rsv1;               // must be 0
+    ULONG   SystemErrorBitmap;  // ignore upon SET, return system error upon QUERY
+} RT_802_11_STA_CONFIG, *PRT_802_11_STA_CONFIG;
+
+//
+//  For OID Query or Set about BA structure
+//
+typedef	struct	_OID_BACAP_STRUC	{
+		UCHAR		RxBAWinLimit;
+		UCHAR		TxBAWinLimit;
+		UCHAR		Policy;	// 0: DELAY_BA 1:IMMED_BA  (//BA Policy subfiled value in ADDBA frame)   2:BA-not use. other value invalid
+		UCHAR		MpduDensity;	// 0: DELAY_BA 1:IMMED_BA  (//BA Policy subfiled value in ADDBA frame)   2:BA-not use. other value invalid
+		UCHAR       	AmsduEnable;	//Enable AMSDU transmisstion
+		UCHAR       	AmsduSize;	// 0:3839, 1:7935 bytes. UINT  MSDUSizeToBytes[]	= { 3839, 7935};
+		UCHAR       	MMPSmode;	// MIMO power save more, 0:static, 1:dynamic, 2:rsv, 3:mimo enable
+		BOOLEAN		AutoBA;	// Auto BA will automatically
+} OID_BACAP_STRUC, *POID_BACAP_STRUC;
+
+typedef struct _RT_802_11_ACL_ENTRY {
+    UCHAR   Addr[MAC_ADDR_LENGTH];
+    USHORT  Rsv;
+} RT_802_11_ACL_ENTRY, *PRT_802_11_ACL_ENTRY;
+
+typedef struct PACKED _RT_802_11_ACL {
+    ULONG   Policy;             // 0-disable, 1-positive list, 2-negative list
+    ULONG   Num;
+    RT_802_11_ACL_ENTRY Entry[MAX_NUMBER_OF_ACL];
+} RT_802_11_ACL, *PRT_802_11_ACL;
+
+typedef struct _RT_802_11_WDS {
+    ULONG						Num;
+    NDIS_802_11_MAC_ADDRESS		Entry[24/*MAX_NUM_OF_WDS_LINK*/];
+	ULONG						KeyLength;
+	UCHAR						KeyMaterial[32];
+} RT_802_11_WDS, *PRT_802_11_WDS;
+
+typedef struct _RT_802_11_TX_RATES_ {
+    UCHAR       SupRateLen;
+    UCHAR       SupRate[MAX_LENGTH_OF_SUPPORT_RATES];
+    UCHAR       ExtRateLen;
+    UCHAR       ExtRate[MAX_LENGTH_OF_SUPPORT_RATES];
+} RT_802_11_TX_RATES, *PRT_802_11_TX_RATES;
+
+
+// Definition of extra information code
+#define	GENERAL_LINK_UP			0x0			// Link is Up
+#define	GENERAL_LINK_DOWN		0x1			// Link is Down
+#define	HW_RADIO_OFF			0x2			// Hardware radio off
+#define	SW_RADIO_OFF			0x3			// Software radio off
+#define	AUTH_FAIL				0x4			// Open authentication fail
+#define	AUTH_FAIL_KEYS			0x5			// Shared authentication fail
+#define	ASSOC_FAIL				0x6			// Association failed
+#define	EAP_MIC_FAILURE			0x7			// Deauthencation because MIC failure
+#define	EAP_4WAY_TIMEOUT		0x8			// Deauthencation on 4-way handshake timeout
+#define	EAP_GROUP_KEY_TIMEOUT	0x9			// Deauthencation on group key handshake timeout
+#define	EAP_SUCCESS				0xa			// EAP succeed
+#define	DETECT_RADAR_SIGNAL		0xb         // Radar signal occur in current channel
+#define EXTRA_INFO_MAX			0xb			// Indicate Last OID
+
+#define EXTRA_INFO_CLEAR		0xffffffff
+
+// This is OID setting structure. So only GF or MM as Mode. This is valid when our wirelss mode has 802.11n in use.
+typedef struct {
+	RT_802_11_PHY_MODE		PhyMode; 	//
+	UCHAR		TransmitNo;
+	UCHAR		HtMode; 	//HTMODE_GF or HTMODE_MM
+	UCHAR		ExtOffset;	//extension channel above or below
+	UCHAR		MCS;
+	UCHAR   	BW;
+	UCHAR		STBC;
+	UCHAR		SHORTGI;
+	UCHAR		rsv;
+} OID_SET_HT_PHYMODE, *POID_SET_HT_PHYMODE;
+
+#ifdef LLTD_SUPPORT
+typedef struct _RT_LLTD_ASSOICATION_ENTRY {
+    UCHAR           Addr[ETH_LENGTH_OF_ADDRESS];
+    unsigned short  MOR;        // maximum operational rate
+    UCHAR           phyMode;
+} RT_LLTD_ASSOICATION_ENTRY, *PRT_LLTD_ASSOICATION_ENTRY;
+
+typedef struct _RT_LLTD_ASSOICATION_TABLE {
+    unsigned int                Num;
+    RT_LLTD_ASSOICATION_ENTRY   Entry[MAX_NUMBER_OF_MAC];
+} RT_LLTD_ASSOICATION_TABLE, *PRT_LLTD_ASSOICATION_TABLE;
+#endif // LLTD_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+//rt2860, kathy 2007-0118
+// structure for DLS
+typedef struct _RT_802_11_DLS_UI {
+	USHORT						TimeOut;		// unit: second , set by UI
+	USHORT						CountDownTimer;	// unit: second , used by driver only
+	NDIS_802_11_MAC_ADDRESS		MacAddr;		// set by UI
+	UCHAR						Status;			// 0: none , 1: wait STAkey, 2: finish DLS setup , set by driver only
+	BOOLEAN						Valid;			// 1: valid , 0: invalid , set by UI, use to setup or tear down DLS link
+} RT_802_11_DLS_UI, *PRT_802_11_DLS_UI;
+
+typedef struct _RT_802_11_DLS_INFO {
+	RT_802_11_DLS_UI	Entry[MAX_NUMBER_OF_DLS_ENTRY];
+	UCHAR				num;
+} RT_802_11_DLS_INFO, *PRT_802_11_DLS_INFO;
+
+typedef enum _RT_802_11_DLS_MODE {
+    DLS_NONE,
+    DLS_WAIT_KEY,
+    DLS_FINISH
+} RT_802_11_DLS_MODE;
+#endif // QOS_DLS_SUPPORT //
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+#define	RT_ASSOC_EVENT_FLAG                         0x0101
+#define	RT_DISASSOC_EVENT_FLAG                      0x0102
+#define	RT_REQIE_EVENT_FLAG                         0x0103
+#define	RT_RESPIE_EVENT_FLAG                        0x0104
+#define	RT_ASSOCINFO_EVENT_FLAG                     0x0105
+#define RT_PMKIDCAND_FLAG                           0x0106
+#define RT_INTERFACE_DOWN                           0x0107
+#define RT_INTERFACE_UP                             0x0108
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+#define MAX_CUSTOM_LEN 128
+
+#ifdef CONFIG_STA_SUPPORT
+typedef enum _RT_802_11_D_CLIENT_MODE
+{
+   Rt802_11_D_None,
+   Rt802_11_D_Flexible,
+   Rt802_11_D_Strict,
+} RT_802_11_D_CLIENT_MODE, *PRT_802_11_D_CLIENT_MODE;
+#endif // CONFIG_STA_SUPPORT //
+
+typedef struct _RT_CHANNEL_LIST_INFO
+{
+	UCHAR ChannelList[MAX_NUM_OF_CHS];   // list all supported channels for site survey
+	UCHAR ChannelListNum; // number of channel in ChannelList[]
+} RT_CHANNEL_LIST_INFO, *PRT_CHANNEL_LIST_INFO;
+
+
+#endif // _OID_H_
+
diff --git a/drivers/staging/rt2860/rt2860.h b/drivers/staging/rt2860/rt2860.h
new file mode 100644
index 00000000000..01720190662
--- /dev/null
+++ b/drivers/staging/rt2860/rt2860.h
@@ -0,0 +1,349 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#ifndef __RT2860_H__
+#define __RT2860_H__
+
+#define RT28xx_CHIP_NAME	"RT2860"
+
+#define TXINFO_SIZE               0
+#define TXPADDING_SIZE      	  0
+
+/* ----------------- EEPROM Related MACRO ----------------- */
+#define RT28xx_EEPROM_READ16(pAd, offset, var)		\
+	var = RTMP_EEPROM_READ16(pAd, offset)
+
+#define RT28xx_EEPROM_WRITE16(pAd, offset, var)		\
+	RTMP_EEPROM_WRITE16(pAd, offset, var)
+
+/* ----------------- TASK/THREAD Related MACRO ----------------- */
+#define RT28XX_TASK_THREAD_INIT(pAd, Status)		\
+	init_thread_task(pAd); NICInitTxRxRingAndBacklogQueue(pAd);	\
+	Status = NDIS_STATUS_SUCCESS;
+
+/* function declarations */
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+#define IRQ_HANDLE_TYPE  irqreturn_t
+#else
+#define IRQ_HANDLE_TYPE  void
+#endif
+
+IRQ_HANDLE_TYPE
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,19))
+rt2860_interrupt(int irq, void *dev_instance);
+#else
+rt2860_interrupt(int irq, void *dev_instance, struct pt_regs *regs);
+#endif
+
+/* ----------------- Frimware Related MACRO ----------------- */
+#define RT28XX_WRITE_FIRMWARE(_pAd, _pFwImage, _FwLen)				\
+	do{																\
+		ULONG	_i, _firm;											\
+		RTMP_IO_WRITE32(_pAd, PBF_SYS_CTRL, 0x10000);				\
+																	\
+		for(_i=0; _i<_FwLen; _i+=4)									\
+		{															\
+			_firm = _pFwImage[_i] +									\
+			   (_pFwImage[_i+3] << 24) +							\
+			   (_pFwImage[_i+2] << 16) +							\
+			   (_pFwImage[_i+1] << 8);								\
+			RTMP_IO_WRITE32(_pAd, FIRMWARE_IMAGE_BASE + _i, _firm);	\
+		}															\
+		RTMP_IO_WRITE32(_pAd, PBF_SYS_CTRL, 0x00000);				\
+		RTMP_IO_WRITE32(_pAd, PBF_SYS_CTRL, 0x00001);				\
+																	\
+		/* initialize BBP R/W access agent */						\
+		RTMP_IO_WRITE32(_pAd, H2M_BBP_AGENT, 0);					\
+		RTMP_IO_WRITE32(_pAd, H2M_MAILBOX_CSR, 0);					\
+	}while(0)
+
+/* ----------------- TX Related MACRO ----------------- */
+#define RT28XX_START_DEQUEUE(pAd, QueIdx, irqFlags)		do{}while(0)
+#define RT28XX_STOP_DEQUEUE(pAd, QueIdx, irqFlags)		do{}while(0)
+
+
+#define RT28XX_HAS_ENOUGH_FREE_DESC(pAd, pTxBlk, freeNum, pPacket) \
+		((freeNum) >= (ULONG)(pTxBlk->TotalFragNum + RTMP_GET_PACKET_FRAGMENTS(pPacket) + 3)) /* rough estimate we will use 3 more descriptor. */
+#define RT28XX_RELEASE_DESC_RESOURCE(pAd, QueIdx)					\
+		do{}while(0)
+
+#define NEED_QUEUE_BACK_FOR_AGG(pAd, QueIdx, freeNum, _TxFrameType) \
+		(((freeNum != (TX_RING_SIZE-1)) && (pAd->TxSwQueue[QueIdx].Number == 0)) || (freeNum<3))
+		//(((freeNum) != (TX_RING_SIZE-1)) && (pAd->TxSwQueue[QueIdx].Number == 1 /*0*/))
+
+
+#define HAL_KickOutMgmtTx(_pAd, _QueIdx, _pPacket, _pSrcBufVA, _SrcBufLen)	\
+			RtmpPCIMgmtKickOut(_pAd, _QueIdx, _pPacket, _pSrcBufVA, _SrcBufLen)
+
+#define RTMP_PKT_TAIL_PADDING 0
+
+#define fRTMP_ADAPTER_NEED_STOP_TX	0
+
+#define HAL_WriteSubTxResource(pAd, pTxBlk, bIsLast, pFreeNumber)	\
+		/* RtmpPCI_WriteSubTxResource(pAd, pTxBlk, bIsLast, pFreeNumber)*/
+
+#define HAL_WriteTxResource(pAd, pTxBlk,bIsLast, pFreeNumber)	\
+			RtmpPCI_WriteSingleTxResource(pAd, pTxBlk, bIsLast, pFreeNumber)
+
+#define HAL_WriteFragTxResource(pAd, pTxBlk, fragNum, pFreeNumber) \
+			RtmpPCI_WriteFragTxResource(pAd, pTxBlk, fragNum, pFreeNumber)
+
+#define HAL_WriteMultiTxResource(pAd, pTxBlk,frameNum, pFreeNumber)	\
+			RtmpPCI_WriteMultiTxResource(pAd, pTxBlk, frameNum, pFreeNumber)
+
+#define HAL_FinalWriteTxResource(_pAd, _pTxBlk, _TotalMPDUSize, _FirstTxIdx)	\
+			RtmpPCI_FinalWriteTxResource(_pAd, _pTxBlk, _TotalMPDUSize, _FirstTxIdx)
+
+#define HAL_LastTxIdx(_pAd, _QueIdx,_LastTxIdx) \
+			/*RtmpPCIDataLastTxIdx(_pAd, _QueIdx,_LastTxIdx)*/
+
+#define HAL_KickOutTx(_pAd, _pTxBlk, _QueIdx)	\
+			RTMP_IO_WRITE32((_pAd), TX_CTX_IDX0+((_QueIdx)*0x10), (_pAd)->TxRing[(_QueIdx)].TxCpuIdx)
+/*			RtmpPCIDataKickOut(_pAd, _pTxBlk, _QueIdx)*/
+
+#define HAL_KickOutNullFrameTx(_pAd, _QueIdx, _pNullFrame, _frameLen)	\
+			MiniportMMRequest(_pAd, _QueIdx, _pNullFrame, _frameLen)
+
+#define GET_TXRING_FREENO(_pAd, _QueIdx) \
+	(_pAd->TxRing[_QueIdx].TxSwFreeIdx > _pAd->TxRing[_QueIdx].TxCpuIdx)	? \
+			(_pAd->TxRing[_QueIdx].TxSwFreeIdx - _pAd->TxRing[_QueIdx].TxCpuIdx - 1) \
+			 :	\
+			(_pAd->TxRing[_QueIdx].TxSwFreeIdx + TX_RING_SIZE - _pAd->TxRing[_QueIdx].TxCpuIdx - 1);
+
+
+#define GET_MGMTRING_FREENO(_pAd) \
+	(_pAd->MgmtRing.TxSwFreeIdx > _pAd->MgmtRing.TxCpuIdx)	? \
+			(_pAd->MgmtRing.TxSwFreeIdx - _pAd->MgmtRing.TxCpuIdx - 1) \
+			 :	\
+			(_pAd->MgmtRing.TxSwFreeIdx + MGMT_RING_SIZE - _pAd->MgmtRing.TxCpuIdx - 1);
+
+
+/* ----------------- RX Related MACRO ----------------- */
+
+// no use
+#define RT28XX_RCV_PKT_GET_INIT(pAd)
+#define RT28XX_RV_A_BUF_END
+//#define RT28XX_RV_ALL_BUF_END
+
+
+/* ----------------- ASIC Related MACRO ----------------- */
+// no use
+#define RT28XX_DMA_POST_WRITE(pAd)
+
+// reset MAC of a station entry to 0x000000000000
+#define RT28XX_STA_ENTRY_MAC_RESET(pAd, Wcid)						\
+	AsicDelWcidTab(pAd, Wcid);
+
+// add this entry into ASIC RX WCID search table
+#define RT28XX_STA_ENTRY_ADD(pAd, pEntry)							\
+	AsicUpdateRxWCIDTable(pAd, pEntry->Aid, pEntry->Addr);
+
+// remove Pair-wise key material from ASIC
+#define RT28XX_STA_ENTRY_KEY_DEL(pAd, BssIdx, Wcid)					\
+	AsicRemovePairwiseKeyEntry(pAd, BssIdx, (UCHAR)Wcid);
+
+// add Client security information into ASIC WCID table and IVEIV table
+#define RT28XX_STA_SECURITY_INFO_ADD(pAd, apidx, KeyID, pEntry)		\
+	RTMPAddWcidAttributeEntry(pAd, apidx, KeyID, 					\
+							pAd->SharedKey[apidx][KeyID].CipherAlg, pEntry);
+
+#define RT28XX_SECURITY_KEY_ADD(pAd, apidx, KeyID, pEntry)				\
+	{	/* update pairwise key information to ASIC Shared Key Table */	\
+		AsicAddSharedKeyEntry(pAd, apidx, KeyID,						\
+						  pAd->SharedKey[apidx][KeyID].CipherAlg,		\
+						  pAd->SharedKey[apidx][KeyID].Key,				\
+						  pAd->SharedKey[apidx][KeyID].TxMic,			\
+						  pAd->SharedKey[apidx][KeyID].RxMic);			\
+		/* update ASIC WCID attribute table and IVEIV table */			\
+		RTMPAddWcidAttributeEntry(pAd, apidx, KeyID,					\
+						  pAd->SharedKey[apidx][KeyID].CipherAlg,		\
+						  pEntry); }
+
+
+// Insert the BA bitmap to ASIC for the Wcid entry
+#define RT28XX_ADD_BA_SESSION_TO_ASIC(_pAd, _Aid, _TID)					\
+		do{																\
+			UINT32	_Value = 0, _Offset;									\
+			_Offset = MAC_WCID_BASE + (_Aid) * HW_WCID_ENTRY_SIZE + 4;	\
+			RTMP_IO_READ32((_pAd), _Offset, &_Value);					\
+			_Value |= (0x10000<<(_TID));								\
+			RTMP_IO_WRITE32((_pAd), _Offset, _Value);					\
+		}while(0)
+
+
+// Remove the BA bitmap from ASIC for the Wcid entry
+//		bitmap field starts at 0x10000 in ASIC WCID table
+#define RT28XX_DEL_BA_SESSION_FROM_ASIC(_pAd, _Wcid, _TID)				\
+		do{																\
+			UINT32	_Value = 0, _Offset;									\
+			_Offset = MAC_WCID_BASE + (_Wcid) * HW_WCID_ENTRY_SIZE + 4;	\
+			RTMP_IO_READ32((_pAd), _Offset, &_Value);					\
+			_Value &= (~(0x10000 << (_TID)));							\
+			RTMP_IO_WRITE32((_pAd), _Offset, _Value);			\
+		}while(0)
+
+
+/* ----------------- PCI/USB Related MACRO ----------------- */
+
+#define RT28XX_HANDLE_DEV_ASSIGN(handle, dev_p)				\
+	((POS_COOKIE)handle)->pci_dev = dev_p;
+
+// set driver data
+#define RT28XX_DRVDATA_SET(_a)			pci_set_drvdata(_a, net_dev);
+
+#define RT28XX_UNMAP()										\
+{	if (net_dev->base_addr)	{								\
+		iounmap((void *)(net_dev->base_addr));				\
+		release_mem_region(pci_resource_start(dev_p, 0),	\
+							pci_resource_len(dev_p, 0)); }	\
+	if (net_dev->irq) pci_release_regions(dev_p); }
+
+#ifdef PCI_MSI_SUPPORT
+#define RTMP_MSI_ENABLE(_pAd) \
+{ 	POS_COOKIE _pObj = (POS_COOKIE)(_pAd->OS_Cookie); \
+	(_pAd)->HaveMsi =	pci_enable_msi(_pObj->pci_dev) == 0 ? TRUE : FALSE; }
+
+#define RTMP_MSI_DISABLE(_pAd) \
+{ 	POS_COOKIE _pObj = (POS_COOKIE)(_pAd->OS_Cookie); \
+	if (_pAd->HaveMsi == TRUE) \
+		pci_disable_msi(_pObj->pci_dev); \
+	_pAd->HaveMsi = FALSE;	}
+#else
+#define RTMP_MSI_ENABLE(_pAd)
+#define RTMP_MSI_DISABLE(_pAd)
+#endif // PCI_MSI_SUPPORT //
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,24)
+#define SA_SHIRQ IRQF_SHARED
+#endif
+
+#define RT28XX_IRQ_REQUEST(net_dev)							\
+{	PRTMP_ADAPTER _pAd = (PRTMP_ADAPTER)((net_dev)->ml_priv);	\
+	POS_COOKIE _pObj = (POS_COOKIE)(_pAd->OS_Cookie);		\
+	RTMP_MSI_ENABLE(_pAd);									\
+	if ((retval = request_irq(_pObj->pci_dev->irq, 		\
+							rt2860_interrupt, SA_SHIRQ,		\
+							(net_dev)->name, (net_dev)))) {	\
+		printk("RT2860: request_irq  ERROR(%d)\n", retval);	\
+	return retval; } }
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+#define RT28XX_IRQ_RELEASE(net_dev)								\
+{	PRTMP_ADAPTER _pAd = (PRTMP_ADAPTER)((net_dev)->ml_priv);		\
+	POS_COOKIE _pObj = (POS_COOKIE)(_pAd->OS_Cookie);			\
+	synchronize_irq(_pObj->pci_dev->irq);						\
+	free_irq(_pObj->pci_dev->irq, (net_dev));					\
+	RTMP_MSI_DISABLE(_pAd); }
+#else
+#define RT28XX_IRQ_RELEASE(net_dev)								\
+{	PRTMP_ADAPTER _pAd = (PRTMP_ADAPTER)((net_dev)->priv);		\
+	POS_COOKIE _pObj = (POS_COOKIE)(_pAd->OS_Cookie);			\
+	free_irq(_pObj->pci_dev->irq, (net_dev));					\
+	RTMP_MSI_DISABLE(_pAd); }
+#endif
+
+#define RT28XX_IRQ_INIT(pAd)										\
+	{	pAd->int_enable_reg = ((DELAYINTMASK) |						\
+							(RxINT|TxDataInt|TxMgmtInt)) & ~(0x03);	\
+		pAd->int_disable_mask = 0;									\
+		pAd->int_pending = 0; }
+
+#define RT28XX_IRQ_ENABLE(pAd)									\
+	{	/* clear garbage ints */								\
+		RTMP_IO_WRITE32(pAd, INT_SOURCE_CSR, 0xffffffff);		\
+		NICEnableInterrupt(pAd); }
+
+#define RT28XX_PUT_DEVICE(dev_p)
+
+
+/* ----------------- MLME Related MACRO ----------------- */
+#define RT28XX_MLME_HANDLER(pAd)			MlmeHandler(pAd)
+
+#define RT28XX_MLME_PRE_SANITY_CHECK(pAd)
+
+#define RT28XX_MLME_STA_QUICK_RSP_WAKE_UP(pAd)	\
+	RTMPSetTimer(&pAd->StaCfg.StaQuickResponeForRateUpTimer, 100);
+
+#define RT28XX_MLME_RESET_STATE_MACHINE(pAd)	\
+		MlmeRestartStateMachine(pAd)
+
+#define RT28XX_HANDLE_COUNTER_MEASURE(_pAd, _pEntry)		\
+		HandleCounterMeasure(_pAd, _pEntry)
+
+/* ----------------- Power Save Related MACRO ----------------- */
+#define RT28XX_PS_POLL_ENQUEUE(pAd)				EnqueuePsPoll(pAd)
+
+//
+// Device ID & Vendor ID, these values should match EEPROM value
+//
+#define NIC2860_PCI_DEVICE_ID   0x0601
+#define NIC2860_PCIe_DEVICE_ID  0x0681
+#define NIC2760_PCI_DEVICE_ID   0x0701		// 1T/2R Cardbus ???
+#define NIC2790_PCIe_DEVICE_ID  0x0781	    // 1T/2R miniCard
+
+#define NIC_PCI_VENDOR_ID       0x1814
+
+#define VEN_AWT_PCIe_DEVICE_ID	0x1059
+#define VEN_AWT_PCI_VENDOR_ID	0x1A3B
+
+// For RTMPPCIePowerLinkCtrlRestore () function
+#define RESTORE_HALT		    1
+#define RESTORE_WAKEUP		    2
+#define RESTORE_CLOSE           3
+
+#define PowerSafeCID		1
+#define PowerRadioOffCID		2
+#define PowerWakeCID		3
+#define CID0MASK		0x000000ff
+#define CID1MASK		0x0000ff00
+#define CID2MASK		0x00ff0000
+#define CID3MASK		0xff000000
+
+#define PCI_REG_READ_WORD(pci_dev, offset, Configuration)   \
+    if (pci_read_config_word(pci_dev, offset, &reg16) == 0)     \
+        Configuration = le2cpu16(reg16);                        \
+    else                                                        \
+        Configuration = 0;
+
+#define PCI_REG_WIRTE_WORD(pci_dev, offset, Configuration)  \
+    reg16 = cpu2le16(Configuration);                        \
+    pci_write_config_word(pci_dev, offset, reg16);          \
+
+#define RT28XX_STA_FORCE_WAKEUP(pAd, bFromTx) \
+    RT28xxPciStaAsicForceWakeup(pAd, bFromTx);
+
+#define RT28XX_STA_SLEEP_THEN_AUTO_WAKEUP(pAd, TbttNumToNextWakeUp) \
+    RT28xxPciStaAsicSleepThenAutoWakeup(pAd, TbttNumToNextWakeUp);
+
+#define RT28XX_MLME_RADIO_ON(pAd) \
+    RT28xxPciMlmeRadioOn(pAd);
+
+#define RT28XX_MLME_RADIO_OFF(pAd) \
+    RT28xxPciMlmeRadioOFF(pAd);
+
+#endif //__RT2860_H__
+
diff --git a/drivers/staging/rt2860/rt28xx.h b/drivers/staging/rt2860/rt28xx.h
new file mode 100644
index 00000000000..ff23043e560
--- /dev/null
+++ b/drivers/staging/rt2860/rt28xx.h
@@ -0,0 +1,2714 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rt28xx.h
+
+	Abstract:
+	RT28xx ASIC related definition & structures
+
+	Revision History:
+	Who			When		  What
+	--------	----------	  ----------------------------------------------
+       Jan Lee           Jan-3-2006     created for RT2860c
+*/
+
+#ifndef	__RT28XX_H__
+#define	__RT28XX_H__
+
+
+//
+// PCI registers - base address 0x0000
+//
+#define PCI_CFG			0x0000
+#define PCI_EECTRL			0x0004
+#define PCI_MCUCTRL			0x0008
+
+//
+// SCH/DMA registers - base address 0x0200
+//
+// INT_SOURCE_CSR: Interrupt source register. Write one to clear corresponding bit
+//
+#define DMA_CSR0      0x200
+#define INT_SOURCE_CSR      0x200
+#ifdef RT_BIG_ENDIAN
+typedef	union	_INT_SOURCE_CSR_STRUC	{
+	struct	{
+		UINT32       	:14;
+		UINT32       	TxCoherent:1;
+		UINT32       	RxCoherent:1;
+		UINT32       	GPTimer:1;
+		UINT32       	AutoWakeup:1;//bit14
+		UINT32       	TXFifoStatusInt:1;//FIFO Statistics is full, sw should read 0x171c
+		UINT32       	PreTBTT:1;
+		UINT32       	TBTTInt:1;
+		UINT32       	RxTxCoherent:1;
+		UINT32       	MCUCommandINT:1;
+		UINT32       	MgmtDmaDone:1;
+		UINT32       	HccaDmaDone:1;
+		UINT32       	Ac3DmaDone:1;
+		UINT32       	Ac2DmaDone:1;
+		UINT32       	Ac1DmaDone:1;
+		UINT32		Ac0DmaDone:1;
+		UINT32		RxDone:1;
+		UINT32		TxDelayINT:1;	//delayed interrupt, not interrupt until several int or time limit hit
+		UINT32		RxDelayINT:1; //dealyed interrupt
+	}	field;
+	UINT32			word;
+}	INT_SOURCE_CSR_STRUC, *PINT_SOURCE_CSR_STRUC;
+#else
+typedef	union	_INT_SOURCE_CSR_STRUC	{
+	struct	{
+		UINT32		RxDelayINT:1;
+		UINT32		TxDelayINT:1;
+		UINT32		RxDone:1;
+		UINT32		Ac0DmaDone:1;//4
+		UINT32       	Ac1DmaDone:1;
+		UINT32       	Ac2DmaDone:1;
+		UINT32       	Ac3DmaDone:1;
+		UINT32       	HccaDmaDone:1; // bit7
+		UINT32       	MgmtDmaDone:1;
+		UINT32       	MCUCommandINT:1;//bit 9
+		UINT32       	RxTxCoherent:1;
+		UINT32       	TBTTInt:1;
+		UINT32       	PreTBTT:1;
+		UINT32       	TXFifoStatusInt:1;//FIFO Statistics is full, sw should read 0x171c
+		UINT32       	AutoWakeup:1;//bit14
+		UINT32       	GPTimer:1;
+		UINT32       	RxCoherent:1;//bit16
+		UINT32       	TxCoherent:1;
+		UINT32       	:14;
+	}	field;
+	UINT32			word;
+} INT_SOURCE_CSR_STRUC, *PINT_SOURCE_CSR_STRUC;
+#endif
+
+//
+// INT_MASK_CSR:   Interrupt MASK register.   1: the interrupt is mask OFF
+//
+#define INT_MASK_CSR        0x204
+#ifdef RT_BIG_ENDIAN
+typedef	union	_INT_MASK_CSR_STRUC	{
+	struct	{
+		UINT32       	TxCoherent:1;
+		UINT32       	RxCoherent:1;
+		UINT32       	:20;
+		UINT32       	MCUCommandINT:1;
+		UINT32       	MgmtDmaDone:1;
+		UINT32       	HccaDmaDone:1;
+		UINT32       	Ac3DmaDone:1;
+		UINT32       	Ac2DmaDone:1;
+		UINT32       	Ac1DmaDone:1;
+		UINT32		Ac0DmaDone:1;
+		UINT32		RxDone:1;
+		UINT32		TxDelay:1;
+		UINT32		RXDelay_INT_MSK:1;
+	}	field;
+	UINT32			word;
+}INT_MASK_CSR_STRUC, *PINT_MASK_CSR_STRUC;
+#else
+typedef	union	_INT_MASK_CSR_STRUC	{
+	struct	{
+		UINT32		RXDelay_INT_MSK:1;
+		UINT32		TxDelay:1;
+		UINT32		RxDone:1;
+		UINT32		Ac0DmaDone:1;
+		UINT32       	Ac1DmaDone:1;
+		UINT32       	Ac2DmaDone:1;
+		UINT32       	Ac3DmaDone:1;
+		UINT32       	HccaDmaDone:1;
+		UINT32       	MgmtDmaDone:1;
+		UINT32       	MCUCommandINT:1;
+		UINT32       	:20;
+		UINT32       	RxCoherent:1;
+		UINT32       	TxCoherent:1;
+	}	field;
+	UINT32			word;
+} INT_MASK_CSR_STRUC, *PINT_MASK_CSR_STRUC;
+#endif
+#define WPDMA_GLO_CFG 	0x208
+#ifdef RT_BIG_ENDIAN
+typedef	union	_WPDMA_GLO_CFG_STRUC	{
+	struct	{
+		UINT32       	HDR_SEG_LEN:16;
+		UINT32       	RXHdrScater:8;
+		UINT32       	BigEndian:1;
+		UINT32       	EnTXWriteBackDDONE:1;
+		UINT32       	WPDMABurstSIZE:2;
+		UINT32		RxDMABusy:1;
+		UINT32		EnableRxDMA:1;
+		UINT32		TxDMABusy:1;
+		UINT32		EnableTxDMA:1;
+	}	field;
+	UINT32			word;
+}WPDMA_GLO_CFG_STRUC, *PWPDMA_GLO_CFG_STRUC;
+#else
+typedef	union	_WPDMA_GLO_CFG_STRUC	{
+	struct	{
+		UINT32		EnableTxDMA:1;
+		UINT32		TxDMABusy:1;
+		UINT32		EnableRxDMA:1;
+		UINT32		RxDMABusy:1;
+		UINT32       	WPDMABurstSIZE:2;
+		UINT32       	EnTXWriteBackDDONE:1;
+		UINT32       	BigEndian:1;
+		UINT32       	RXHdrScater:8;
+		UINT32       	HDR_SEG_LEN:16;
+	}	field;
+	UINT32			word;
+} WPDMA_GLO_CFG_STRUC, *PWPDMA_GLO_CFG_STRUC;
+#endif
+#define WPDMA_RST_IDX 	0x20c
+#ifdef RT_BIG_ENDIAN
+typedef	union	_WPDMA_RST_IDX_STRUC	{
+	struct	{
+		UINT32       	:15;
+		UINT32       	RST_DRX_IDX0:1;
+		UINT32       	rsv:10;
+		UINT32       	RST_DTX_IDX5:1;
+		UINT32       	RST_DTX_IDX4:1;
+		UINT32		RST_DTX_IDX3:1;
+		UINT32		RST_DTX_IDX2:1;
+		UINT32		RST_DTX_IDX1:1;
+		UINT32		RST_DTX_IDX0:1;
+	}	field;
+	UINT32			word;
+}WPDMA_RST_IDX_STRUC, *PWPDMA_RST_IDX_STRUC;
+#else
+typedef	union	_WPDMA_RST_IDX_STRUC	{
+	struct	{
+		UINT32		RST_DTX_IDX0:1;
+		UINT32		RST_DTX_IDX1:1;
+		UINT32		RST_DTX_IDX2:1;
+		UINT32		RST_DTX_IDX3:1;
+		UINT32       	RST_DTX_IDX4:1;
+		UINT32       	RST_DTX_IDX5:1;
+		UINT32       	rsv:10;
+		UINT32       	RST_DRX_IDX0:1;
+		UINT32       	:15;
+	}	field;
+	UINT32			word;
+} WPDMA_RST_IDX_STRUC, *PWPDMA_RST_IDX_STRUC;
+#endif
+#define DELAY_INT_CFG  0x0210
+#ifdef RT_BIG_ENDIAN
+typedef	union	_DELAY_INT_CFG_STRUC	{
+	struct	{
+		UINT32       	TXDLY_INT_EN:1;
+		UINT32       	TXMAX_PINT:7;
+		UINT32       	TXMAX_PTIME:8;
+		UINT32       	RXDLY_INT_EN:1;
+		UINT32       	RXMAX_PINT:7;
+		UINT32		RXMAX_PTIME:8;
+	}	field;
+	UINT32			word;
+}DELAY_INT_CFG_STRUC, *PDELAY_INT_CFG_STRUC;
+#else
+typedef	union	_DELAY_INT_CFG_STRUC	{
+	struct	{
+		UINT32		RXMAX_PTIME:8;
+		UINT32       	RXMAX_PINT:7;
+		UINT32       	RXDLY_INT_EN:1;
+		UINT32       	TXMAX_PTIME:8;
+		UINT32       	TXMAX_PINT:7;
+		UINT32       	TXDLY_INT_EN:1;
+	}	field;
+	UINT32			word;
+} DELAY_INT_CFG_STRUC, *PDELAY_INT_CFG_STRUC;
+#endif
+#define WMM_AIFSN_CFG   0x0214
+#ifdef RT_BIG_ENDIAN
+typedef	union	_AIFSN_CSR_STRUC	{
+	struct	{
+	    UINT32   Rsv:16;
+	    UINT32   Aifsn3:4;       // for AC_VO
+	    UINT32   Aifsn2:4;       // for AC_VI
+	    UINT32   Aifsn1:4;       // for AC_BK
+	    UINT32   Aifsn0:4;       // for AC_BE
+	}	field;
+	UINT32			word;
+}	AIFSN_CSR_STRUC, *PAIFSN_CSR_STRUC;
+#else
+typedef	union	_AIFSN_CSR_STRUC	{
+	struct	{
+	    UINT32   Aifsn0:4;       // for AC_BE
+	    UINT32   Aifsn1:4;       // for AC_BK
+	    UINT32   Aifsn2:4;       // for AC_VI
+	    UINT32   Aifsn3:4;       // for AC_VO
+	    UINT32   Rsv:16;
+	}	field;
+	UINT32			word;
+}	AIFSN_CSR_STRUC, *PAIFSN_CSR_STRUC;
+#endif
+//
+// CWMIN_CSR: CWmin for each EDCA AC
+//
+#define WMM_CWMIN_CFG   0x0218
+#ifdef RT_BIG_ENDIAN
+typedef	union	_CWMIN_CSR_STRUC	{
+	struct	{
+	    UINT32   Rsv:16;
+	    UINT32   Cwmin3:4;       // for AC_VO
+	    UINT32   Cwmin2:4;       // for AC_VI
+	    UINT32   Cwmin1:4;       // for AC_BK
+	    UINT32   Cwmin0:4;       // for AC_BE
+	}	field;
+	UINT32			word;
+}	CWMIN_CSR_STRUC, *PCWMIN_CSR_STRUC;
+#else
+typedef	union	_CWMIN_CSR_STRUC	{
+	struct	{
+	    UINT32   Cwmin0:4;       // for AC_BE
+	    UINT32   Cwmin1:4;       // for AC_BK
+	    UINT32   Cwmin2:4;       // for AC_VI
+	    UINT32   Cwmin3:4;       // for AC_VO
+	    UINT32   Rsv:16;
+	}	field;
+	UINT32			word;
+}	CWMIN_CSR_STRUC, *PCWMIN_CSR_STRUC;
+#endif
+
+//
+// CWMAX_CSR: CWmin for each EDCA AC
+//
+#define WMM_CWMAX_CFG   0x021c
+#ifdef RT_BIG_ENDIAN
+typedef	union	_CWMAX_CSR_STRUC	{
+	struct	{
+	    UINT32   Rsv:16;
+	    UINT32   Cwmax3:4;       // for AC_VO
+	    UINT32   Cwmax2:4;       // for AC_VI
+	    UINT32   Cwmax1:4;       // for AC_BK
+	    UINT32   Cwmax0:4;       // for AC_BE
+	}	field;
+	UINT32			word;
+}	CWMAX_CSR_STRUC, *PCWMAX_CSR_STRUC;
+#else
+typedef	union	_CWMAX_CSR_STRUC	{
+	struct	{
+	    UINT32   Cwmax0:4;       // for AC_BE
+	    UINT32   Cwmax1:4;       // for AC_BK
+	    UINT32   Cwmax2:4;       // for AC_VI
+	    UINT32   Cwmax3:4;       // for AC_VO
+	    UINT32   Rsv:16;
+	}	field;
+	UINT32			word;
+}	CWMAX_CSR_STRUC, *PCWMAX_CSR_STRUC;
+#endif
+
+
+//
+// AC_TXOP_CSR0: AC_BK/AC_BE TXOP register
+//
+#define WMM_TXOP0_CFG    0x0220
+#ifdef RT_BIG_ENDIAN
+typedef	union	_AC_TXOP_CSR0_STRUC	{
+	struct	{
+	    USHORT  Ac1Txop;        // for AC_BE, in unit of 32us
+	    USHORT  Ac0Txop;        // for AC_BK, in unit of 32us
+	}	field;
+	UINT32			word;
+}	AC_TXOP_CSR0_STRUC, *PAC_TXOP_CSR0_STRUC;
+#else
+typedef	union	_AC_TXOP_CSR0_STRUC	{
+	struct	{
+	    USHORT  Ac0Txop;        // for AC_BK, in unit of 32us
+	    USHORT  Ac1Txop;        // for AC_BE, in unit of 32us
+	}	field;
+	UINT32			word;
+}	AC_TXOP_CSR0_STRUC, *PAC_TXOP_CSR0_STRUC;
+#endif
+
+//
+// AC_TXOP_CSR1: AC_VO/AC_VI TXOP register
+//
+#define WMM_TXOP1_CFG    0x0224
+#ifdef RT_BIG_ENDIAN
+typedef	union	_AC_TXOP_CSR1_STRUC	{
+	struct	{
+	    USHORT  Ac3Txop;        // for AC_VO, in unit of 32us
+	    USHORT  Ac2Txop;        // for AC_VI, in unit of 32us
+	}	field;
+	UINT32			word;
+}	AC_TXOP_CSR1_STRUC, *PAC_TXOP_CSR1_STRUC;
+#else
+typedef	union	_AC_TXOP_CSR1_STRUC	{
+	struct	{
+	    USHORT  Ac2Txop;        // for AC_VI, in unit of 32us
+	    USHORT  Ac3Txop;        // for AC_VO, in unit of 32us
+	}	field;
+	UINT32			word;
+}	AC_TXOP_CSR1_STRUC, *PAC_TXOP_CSR1_STRUC;
+#endif
+#define RINGREG_DIFF			0x10
+#define GPIO_CTRL_CFG    0x0228	//MAC_CSR13
+#define MCU_CMD_CFG    0x022c
+#define TX_BASE_PTR0     0x0230	//AC_BK base address
+#define TX_MAX_CNT0      0x0234
+#define TX_CTX_IDX0       0x0238
+#define TX_DTX_IDX0      0x023c
+#define TX_BASE_PTR1     0x0240 	//AC_BE base address
+#define TX_MAX_CNT1      0x0244
+#define TX_CTX_IDX1       0x0248
+#define TX_DTX_IDX1      0x024c
+#define TX_BASE_PTR2     0x0250 	//AC_VI base address
+#define TX_MAX_CNT2      0x0254
+#define TX_CTX_IDX2       0x0258
+#define TX_DTX_IDX2      0x025c
+#define TX_BASE_PTR3     0x0260 	//AC_VO base address
+#define TX_MAX_CNT3      0x0264
+#define TX_CTX_IDX3       0x0268
+#define TX_DTX_IDX3      0x026c
+#define TX_BASE_PTR4     0x0270 	//HCCA base address
+#define TX_MAX_CNT4      0x0274
+#define TX_CTX_IDX4       0x0278
+#define TX_DTX_IDX4      0x027c
+#define TX_BASE_PTR5     0x0280 	//MGMT base address
+#define  TX_MAX_CNT5     0x0284
+#define TX_CTX_IDX5       0x0288
+#define TX_DTX_IDX5      0x028c
+#define TX_MGMTMAX_CNT      TX_MAX_CNT5
+#define TX_MGMTCTX_IDX       TX_CTX_IDX5
+#define TX_MGMTDTX_IDX      TX_DTX_IDX5
+#define RX_BASE_PTR     0x0290 	//RX base address
+#define RX_MAX_CNT      0x0294
+#define RX_CRX_IDX       0x0298
+#define RX_DRX_IDX      0x029c
+#define USB_DMA_CFG      0x02a0
+#ifdef RT_BIG_ENDIAN
+typedef	union	_USB_DMA_CFG_STRUC	{
+	struct	{
+	    UINT32  TxBusy:1;   	//USB DMA TX FSM busy . debug only
+	    UINT32  RxBusy:1;        //USB DMA RX FSM busy . debug only
+	    UINT32  EpoutValid:6;        //OUT endpoint data valid. debug only
+	    UINT32  TxBulkEn:1;        //Enable USB DMA Tx
+	    UINT32  RxBulkEn:1;        //Enable USB DMA Rx
+	    UINT32  RxBulkAggEn:1;        //Enable Rx Bulk Aggregation
+	    UINT32  TxopHalt:1;        //Halt TXOP count down when TX buffer is full.
+	    UINT32  TxClear:1;        //Clear USB DMA TX path
+	    UINT32  rsv:2;
+	    UINT32  phyclear:1;        		//phy watch dog enable. write 1
+	    UINT32  RxBulkAggLmt:8;        //Rx Bulk Aggregation Limit  in unit of 1024 bytes
+	    UINT32  RxBulkAggTOut:8;        //Rx Bulk Aggregation TimeOut  in unit of 33ns
+	}	field;
+	UINT32			word;
+}	USB_DMA_CFG_STRUC, *PUSB_DMA_CFG_STRUC;
+#else
+typedef	union	_USB_DMA_CFG_STRUC	{
+	struct	{
+	    UINT32  RxBulkAggTOut:8;        //Rx Bulk Aggregation TimeOut  in unit of 33ns
+	    UINT32  RxBulkAggLmt:8;        //Rx Bulk Aggregation Limit  in unit of 256 bytes
+	    UINT32  phyclear:1;        		//phy watch dog enable. write 1
+	    UINT32  rsv:2;
+	    UINT32  TxClear:1;        //Clear USB DMA TX path
+	    UINT32  TxopHalt:1;        //Halt TXOP count down when TX buffer is full.
+	    UINT32  RxBulkAggEn:1;        //Enable Rx Bulk Aggregation
+	    UINT32  RxBulkEn:1;        //Enable USB DMA Rx
+	    UINT32  TxBulkEn:1;        //Enable USB DMA Tx
+	    UINT32  EpoutValid:6;        //OUT endpoint data valid
+	    UINT32  RxBusy:1;        //USB DMA RX FSM busy
+	    UINT32  TxBusy:1;   	//USB DMA TX FSM busy
+	}	field;
+	UINT32			word;
+}	USB_DMA_CFG_STRUC, *PUSB_DMA_CFG_STRUC;
+#endif
+
+//
+//  3  PBF  registers
+//
+//
+// Most are for debug. Driver doesn't touch PBF register.
+#define 	PBF_SYS_CTRL 	 0x0400
+#define     PBF_CFG                 0x0408
+#define 	PBF_MAX_PCNT 	 0x040C
+#define 	PBF_CTRL	 	0x0410
+#define 	PBF_INT_STA	 0x0414
+#define 	PBF_INT_ENA	 0x0418
+#define 	TXRXQ_PCNT  	 0x0438
+#define 	PBF_DBG 	 	 0x043c
+#define     PBF_CAP_CTRL     0x0440
+
+//
+//  4  MAC  registers
+//
+//
+//  4.1 MAC SYSTEM  configuration registers (offset:0x1000)
+//
+#define MAC_CSR0            0x1000
+#ifdef RT_BIG_ENDIAN
+typedef	union	_ASIC_VER_ID_STRUC	{
+	struct	{
+	    USHORT  ASICVer;        // version : 2860
+	    USHORT  ASICRev;        // reversion  : 0
+	}	field;
+	UINT32			word;
+}	ASIC_VER_ID_STRUC, *PASIC_VER_ID_STRUC;
+#else
+typedef	union	_ASIC_VER_ID_STRUC	{
+	struct	{
+	    USHORT  ASICRev;        // reversion  : 0
+	    USHORT  ASICVer;        // version : 2860
+	}	field;
+	UINT32			word;
+}	ASIC_VER_ID_STRUC, *PASIC_VER_ID_STRUC;
+#endif
+#define MAC_SYS_CTRL            0x1004		//MAC_CSR1
+#define MAC_ADDR_DW0            		0x1008		// MAC ADDR DW0
+#define MAC_ADDR_DW1           		 0x100c		// MAC ADDR DW1
+//
+// MAC_CSR2: STA MAC register 0
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_MAC_DW0_STRUC	{
+	struct	{
+		UCHAR		Byte3;		// MAC address byte 3
+		UCHAR		Byte2;		// MAC address byte 2
+		UCHAR		Byte1;		// MAC address byte 1
+		UCHAR		Byte0;		// MAC address byte 0
+	}	field;
+	UINT32			word;
+}	MAC_DW0_STRUC, *PMAC_DW0_STRUC;
+#else
+typedef	union	_MAC_DW0_STRUC	{
+	struct	{
+		UCHAR		Byte0;		// MAC address byte 0
+		UCHAR		Byte1;		// MAC address byte 1
+		UCHAR		Byte2;		// MAC address byte 2
+		UCHAR		Byte3;		// MAC address byte 3
+	}	field;
+	UINT32			word;
+}	MAC_DW0_STRUC, *PMAC_DW0_STRUC;
+#endif
+
+//
+// MAC_CSR3: STA MAC register 1
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_MAC_DW1_STRUC	{
+	struct	{
+		UCHAR		Rsvd1;
+		UCHAR		U2MeMask;
+		UCHAR		Byte5;		// MAC address byte 5
+		UCHAR		Byte4;		// MAC address byte 4
+	}	field;
+	UINT32			word;
+}	MAC_DW1_STRUC, *PMAC_DW1_STRUC;
+#else
+typedef	union	_MAC_DW1_STRUC	{
+	struct	{
+		UCHAR		Byte4;		// MAC address byte 4
+		UCHAR		Byte5;		// MAC address byte 5
+		UCHAR		U2MeMask;
+		UCHAR		Rsvd1;
+	}	field;
+	UINT32			word;
+}	MAC_DW1_STRUC, *PMAC_DW1_STRUC;
+#endif
+
+#define MAC_BSSID_DW0            		0x1010		// MAC BSSID DW0
+#define MAC_BSSID_DW1            		0x1014		// MAC BSSID DW1
+
+//
+// MAC_CSR5: BSSID register 1
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_MAC_CSR5_STRUC	{
+	struct	{
+		USHORT		Rsvd:11;
+		USHORT		MBssBcnNum:3;
+		USHORT		BssIdMode:2; // 0: one BSSID, 10: 4 BSSID,  01: 2 BSSID , 11: 8BSSID
+		UCHAR		Byte5;		 // BSSID byte 5
+		UCHAR		Byte4;		 // BSSID byte 4
+	}	field;
+	UINT32			word;
+}	MAC_CSR5_STRUC, *PMAC_CSR5_STRUC;
+#else
+typedef	union	_MAC_CSR5_STRUC	{
+	struct	{
+		UCHAR		Byte4;		 // BSSID byte 4
+		UCHAR		Byte5;		 // BSSID byte 5
+		USHORT      	BssIdMask:2; // 0: one BSSID, 10: 4 BSSID,  01: 2 BSSID , 11: 8BSSID
+		USHORT		MBssBcnNum:3;
+		USHORT		Rsvd:11;
+	}	field;
+	UINT32			word;
+}	MAC_CSR5_STRUC, *PMAC_CSR5_STRUC;
+#endif
+
+#define MAX_LEN_CFG              0x1018		// rt2860b max 16k bytes. bit12:13 Maximum PSDU length (power factor) 0:2^13, 1:2^14, 2:2^15, 3:2^16
+#define BBP_CSR_CFG            		0x101c		//
+//
+// BBP_CSR_CFG: BBP serial control register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_BBP_CSR_CFG_STRUC	{
+	struct	{
+		UINT32		:12;
+		UINT32		BBP_RW_MODE:1;		// 0: use serial mode  1:parallel
+		UINT32		BBP_PAR_DUR:1;		    // 0: 4 MAC clock cycles  1: 8 MAC clock cycles
+		UINT32		Busy:1;				// 1: ASIC is busy execute BBP programming.
+		UINT32		fRead:1;		    // 0: Write	BBP, 1:	Read BBP
+		UINT32		RegNum:8;			// Selected	BBP	register
+		UINT32		Value:8;			// Register	value to program into BBP
+	}	field;
+	UINT32			word;
+}	BBP_CSR_CFG_STRUC, *PBBP_CSR_CFG_STRUC;
+#else
+typedef	union	_BBP_CSR_CFG_STRUC	{
+	struct	{
+		UINT32		Value:8;			// Register	value to program into BBP
+		UINT32		RegNum:8;			// Selected	BBP	register
+		UINT32		fRead:1;		    // 0: Write	BBP, 1:	Read BBP
+		UINT32		Busy:1;				// 1: ASIC is busy execute BBP programming.
+		UINT32		BBP_PAR_DUR:1;		     // 0: 4 MAC clock cycles  1: 8 MAC clock cycles
+		UINT32		BBP_RW_MODE:1;		// 0: use serial mode  1:parallel
+		UINT32		:12;
+	}	field;
+	UINT32			word;
+}	BBP_CSR_CFG_STRUC, *PBBP_CSR_CFG_STRUC;
+#endif
+#define RF_CSR_CFG0            		0x1020
+//
+// RF_CSR_CFG: RF control register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_RF_CSR_CFG0_STRUC	{
+	struct	{
+		UINT32		Busy:1;		    // 0: idle 1: 8busy
+		UINT32		Sel:1;				// 0:RF_LE0 activate  1:RF_LE1 activate
+		UINT32		StandbyMode:1;		    // 0: high when stand by 1:	low when standby
+		UINT32		bitwidth:5;			// Selected	BBP	register
+		UINT32		RegIdAndContent:24;			// Register	value to program into BBP
+	}	field;
+	UINT32			word;
+}	RF_CSR_CFG0_STRUC, *PRF_CSR_CFG0_STRUC;
+#else
+typedef	union	_RF_CSR_CFG0_STRUC	{
+	struct	{
+		UINT32		RegIdAndContent:24;			// Register	value to program into BBP
+		UINT32		bitwidth:5;			// Selected	BBP	register
+		UINT32		StandbyMode:1;		    // 0: high when stand by 1:	low when standby
+		UINT32		Sel:1;				// 0:RF_LE0 activate  1:RF_LE1 activate
+		UINT32		Busy:1;		    // 0: idle 1: 8busy
+	}	field;
+	UINT32			word;
+}	RF_CSR_CFG0_STRUC, *PRF_CSR_CFG0_STRUC;
+#endif
+#define RF_CSR_CFG1           		0x1024
+#ifdef RT_BIG_ENDIAN
+typedef	union	_RF_CSR_CFG1_STRUC	{
+	struct	{
+		UINT32		rsv:7;		    // 0: idle 1: 8busy
+		UINT32		RFGap:5;			// Gap between BB_CONTROL_RF and RF_LE. 0: 3 system clock cycle (37.5usec) 1: 5 system clock cycle (62.5usec)
+		UINT32		RegIdAndContent:24;			// Register	value to program into BBP
+	}	field;
+	UINT32			word;
+}	RF_CSR_CFG1_STRUC, *PRF_CSR_CFG1_STRUC;
+#else
+typedef	union	_RF_CSR_CFG1_STRUC	{
+	struct	{
+		UINT32		RegIdAndContent:24;			// Register	value to program into BBP
+		UINT32		RFGap:5;			// Gap between BB_CONTROL_RF and RF_LE. 0: 3 system clock cycle (37.5usec) 1: 5 system clock cycle (62.5usec)
+		UINT32		rsv:7;		    // 0: idle 1: 8busy
+	}	field;
+	UINT32			word;
+}	RF_CSR_CFG1_STRUC, *PRF_CSR_CFG1_STRUC;
+#endif
+#define RF_CSR_CFG2           		0x1028		//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_RF_CSR_CFG2_STRUC	{
+	struct	{
+		UINT32		rsv:8;		    // 0: idle 1: 8busy
+		UINT32		RegIdAndContent:24;			// Register	value to program into BBP
+	}	field;
+	UINT32			word;
+}	RF_CSR_CFG2_STRUC, *PRF_CSR_CFG2_STRUC;
+#else
+typedef	union	_RF_CSR_CFG2_STRUC	{
+	struct	{
+		UINT32		RegIdAndContent:24;			// Register	value to program into BBP
+		UINT32		rsv:8;		    // 0: idle 1: 8busy
+	}	field;
+	UINT32			word;
+}	RF_CSR_CFG2_STRUC, *PRF_CSR_CFG2_STRUC;
+#endif
+#define LED_CFG           		0x102c		//  MAC_CSR14
+#ifdef RT_BIG_ENDIAN
+typedef	union	_LED_CFG_STRUC	{
+	struct	{
+		UINT32		:1;
+		UINT32		LedPolar:1;			// Led Polarity.  0: active low1: active high
+		UINT32		YLedMode:2;			// yellow Led Mode
+		UINT32		GLedMode:2;			// green Led Mode
+		UINT32		RLedMode:2;			// red Led Mode    0: off1: blinking upon TX2: periodic slow blinking3: always on
+		UINT32		rsv:2;
+		UINT32		SlowBlinkPeriod:6;			// slow blinking period. unit:1ms
+		UINT32		OffPeriod:8;			// blinking off period unit 1ms
+		UINT32		OnPeriod:8;			// blinking on period unit 1ms
+	}	field;
+	UINT32			word;
+}	LED_CFG_STRUC, *PLED_CFG_STRUC;
+#else
+typedef	union	_LED_CFG_STRUC	{
+	struct	{
+		UINT32		OnPeriod:8;			// blinking on period unit 1ms
+		UINT32		OffPeriod:8;			// blinking off period unit 1ms
+		UINT32		SlowBlinkPeriod:6;			// slow blinking period. unit:1ms
+		UINT32		rsv:2;
+		UINT32		RLedMode:2;			// red Led Mode    0: off1: blinking upon TX2: periodic slow blinking3: always on
+		UINT32		GLedMode:2;			// green Led Mode
+		UINT32		YLedMode:2;			// yellow Led Mode
+		UINT32		LedPolar:1;			// Led Polarity.  0: active low1: active high
+		UINT32		:1;
+	}	field;
+	UINT32			word;
+}	LED_CFG_STRUC, *PLED_CFG_STRUC;
+#endif
+//
+//  4.2 MAC TIMING  configuration registers (offset:0x1100)
+//
+#define XIFS_TIME_CFG             0x1100		 // MAC_CSR8  MAC_CSR9
+#ifdef RT_BIG_ENDIAN
+typedef	union	_IFS_SLOT_CFG_STRUC	{
+	struct	{
+	    UINT32  rsv:2;
+	    UINT32  BBRxendEnable:1;        //  reference RXEND signal to begin XIFS defer
+	    UINT32  EIFS:9;        //  unit 1us
+	    UINT32  OfdmXifsTime:4;        //OFDM SIFS. unit 1us. Applied after OFDM RX when MAC doesn't reference BBP signal BBRXEND
+	    UINT32  OfdmSifsTime:8;        //  unit 1us. Applied after OFDM RX/TX
+	    UINT32  CckmSifsTime:8;        //  unit 1us. Applied after CCK RX/TX
+	}	field;
+	UINT32			word;
+}	IFS_SLOT_CFG_STRUC, *PIFS_SLOT_CFG_STRUC;
+#else
+typedef	union	_IFS_SLOT_CFG_STRUC	{
+	struct	{
+	    UINT32  CckmSifsTime:8;        //  unit 1us. Applied after CCK RX/TX
+	    UINT32  OfdmSifsTime:8;        //  unit 1us. Applied after OFDM RX/TX
+	    UINT32  OfdmXifsTime:4;        //OFDM SIFS. unit 1us. Applied after OFDM RX when MAC doesn't reference BBP signal BBRXEND
+	    UINT32  EIFS:9;        //  unit 1us
+	    UINT32  BBRxendEnable:1;        //  reference RXEND signal to begin XIFS defer
+	    UINT32  rsv:2;
+	}	field;
+	UINT32			word;
+}	IFS_SLOT_CFG_STRUC, *PIFS_SLOT_CFG_STRUC;
+#endif
+
+#define BKOFF_SLOT_CFG             0x1104		 //  mac_csr9 last 8 bits
+#define NAV_TIME_CFG             0x1108		 // NAV  (MAC_CSR15)
+#define CH_TIME_CFG             0x110C		 	// Count as channel busy
+#define PBF_LIFE_TIMER             0x1110		 //TX/RX MPDU timestamp timer (free run)Unit: 1us
+#define BCN_TIME_CFG             0x1114		 // TXRX_CSR9
+
+#define BCN_OFFSET0				0x042C
+#define BCN_OFFSET1				0x0430
+
+//
+// BCN_TIME_CFG : Synchronization control register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_BCN_TIME_CFG_STRUC	{
+	struct	{
+		UINT32		TxTimestampCompensate:8;
+        UINT32       :3;
+		UINT32		bBeaconGen:1;		// Enable beacon generator
+        UINT32       bTBTTEnable:1;
+		UINT32		TsfSyncMode:2;		// Enable TSF sync, 00: disable, 01: infra mode, 10: ad-hoc mode
+		UINT32		bTsfTicking:1;		// Enable TSF auto counting
+		UINT32       BeaconInterval:16;  // in unit of 1/16 TU
+	}	field;
+	UINT32			word;
+}	BCN_TIME_CFG_STRUC, *PBCN_TIME_CFG_STRUC;
+#else
+typedef	union	_BCN_TIME_CFG_STRUC	{
+	struct	{
+		UINT32       BeaconInterval:16;  // in unit of 1/16 TU
+		UINT32		bTsfTicking:1;		// Enable TSF auto counting
+		UINT32		TsfSyncMode:2;		// Enable TSF sync, 00: disable, 01: infra mode, 10: ad-hoc mode
+        UINT32       bTBTTEnable:1;
+		UINT32		bBeaconGen:1;		// Enable beacon generator
+        UINT32       :3;
+		UINT32		TxTimestampCompensate:8;
+	}	field;
+	UINT32			word;
+}	BCN_TIME_CFG_STRUC, *PBCN_TIME_CFG_STRUC;
+#endif
+#define TBTT_SYNC_CFG            0x1118  		// txrx_csr10
+#define TSF_TIMER_DW0             0x111C  		// Local TSF timer lsb 32 bits. Read-only
+#define TSF_TIMER_DW1             0x1120  		// msb 32 bits. Read-only.
+#define TBTT_TIMER             	0x1124  		// TImer remains till next TBTT. Read-only.  TXRX_CSR14
+#define INT_TIMER_CFG              	0x1128  		//
+#define INT_TIMER_EN             	0x112c  		//  GP-timer and pre-tbtt Int enable
+#define CH_IDLE_STA              	0x1130  		//  channel idle time
+#define CH_BUSY_STA              	0x1134  		//  channle busy time
+//
+//  4.2 MAC POWER  configuration registers (offset:0x1200)
+//
+#define MAC_STATUS_CFG             0x1200		 // old MAC_CSR12
+#define PWR_PIN_CFG             0x1204		 // old MAC_CSR12
+#define AUTO_WAKEUP_CFG             0x1208		 // old MAC_CSR10
+//
+// AUTO_WAKEUP_CFG: Manual power control / status register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_AUTO_WAKEUP_STRUC	{
+	struct	{
+		UINT32		:16;
+		UINT32		EnableAutoWakeup:1;	// 0:sleep, 1:awake
+		UINT32       NumofSleepingTbtt:7;          // ForceWake has high privilege than PutToSleep when both set
+		UINT32       AutoLeadTime:8;
+	}	field;
+	UINT32			word;
+}	AUTO_WAKEUP_STRUC, *PAUTO_WAKEUP_STRUC;
+#else
+typedef	union	_AUTO_WAKEUP_STRUC	{
+	struct	{
+		UINT32       AutoLeadTime:8;
+		UINT32       NumofSleepingTbtt:7;          // ForceWake has high privilege than PutToSleep when both set
+		UINT32		EnableAutoWakeup:1;	// 0:sleep, 1:awake
+		UINT32		:16;
+	}	field;
+	UINT32			word;
+}	AUTO_WAKEUP_STRUC, *PAUTO_WAKEUP_STRUC;
+#endif
+//
+//  4.3 MAC TX  configuration registers (offset:0x1300)
+//
+
+#define EDCA_AC0_CFG	0x1300		//AC_TXOP_CSR0 0x3474
+#define EDCA_AC1_CFG	0x1304
+#define EDCA_AC2_CFG	0x1308
+#define EDCA_AC3_CFG	0x130c
+#ifdef RT_BIG_ENDIAN
+typedef	union	_EDCA_AC_CFG_STRUC	{
+	struct	{
+	    UINT32  :12;        //
+	    UINT32  Cwmax:4;        //unit power of 2
+	    UINT32  Cwmin:4;        //
+	    UINT32  Aifsn:4;        // # of slot time
+	    UINT32  AcTxop:8;        //  in unit of 32us
+	}	field;
+	UINT32			word;
+}	EDCA_AC_CFG_STRUC, *PEDCA_AC_CFG_STRUC;
+#else
+typedef	union	_EDCA_AC_CFG_STRUC	{
+	struct	{
+	    UINT32  AcTxop:8;        //  in unit of 32us
+	    UINT32  Aifsn:4;        // # of slot time
+	    UINT32  Cwmin:4;        //
+	    UINT32  Cwmax:4;        //unit power of 2
+	    UINT32  :12;       //
+	}	field;
+	UINT32			word;
+}	EDCA_AC_CFG_STRUC, *PEDCA_AC_CFG_STRUC;
+#endif
+
+#define EDCA_TID_AC_MAP	0x1310
+#define TX_PWR_CFG_0	0x1314
+#define TX_PWR_CFG_1	0x1318
+#define TX_PWR_CFG_2	0x131C
+#define TX_PWR_CFG_3	0x1320
+#define TX_PWR_CFG_4	0x1324
+#define TX_PIN_CFG		0x1328
+#define TX_BAND_CFG	0x132c		// 0x1 use upper 20MHz. 0 juse lower 20MHz
+#define TX_SW_CFG0		0x1330
+#define TX_SW_CFG1		0x1334
+#define TX_SW_CFG2		0x1338
+#define TXOP_THRES_CFG		0x133c
+#define TXOP_CTRL_CFG		0x1340
+#define TX_RTS_CFG		0x1344
+
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_RTS_CFG_STRUC	{
+	struct	{
+	    UINT32       rsv:7;
+	    UINT32       RtsFbkEn:1;    // enable rts rate fallback
+	    UINT32       RtsThres:16;    // unit:byte
+	    UINT32       AutoRtsRetryLimit:8;
+	}	field;
+	UINT32			word;
+}	TX_RTS_CFG_STRUC, *PTX_RTS_CFG_STRUC;
+#else
+typedef	union	_TX_RTS_CFG_STRUC	{
+	struct	{
+	    UINT32       AutoRtsRetryLimit:8;
+	    UINT32       RtsThres:16;    // unit:byte
+	    UINT32       RtsFbkEn:1;    // enable rts rate fallback
+	    UINT32       rsv:7;     // 1: HT non-STBC control frame enable
+	}	field;
+	UINT32			word;
+}	TX_RTS_CFG_STRUC, *PTX_RTS_CFG_STRUC;
+#endif
+#define TX_TIMEOUT_CFG	0x1348
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_TIMEOUT_CFG_STRUC	{
+	struct	{
+	    UINT32       rsv2:8;
+	    UINT32       TxopTimeout:8;	//TXOP timeout value for TXOP truncation.  It is recommended that (SLOT_TIME) > (TX_OP_TIMEOUT) > (RX_ACK_TIMEOUT)
+	    UINT32       RxAckTimeout:8;	// unit:slot. Used for TX precedure
+	    UINT32       MpduLifeTime:4;    //  expiration time = 2^(9+MPDU LIFE TIME)  us
+	    UINT32       rsv:4;
+	}	field;
+	UINT32			word;
+}	TX_TIMEOUT_CFG_STRUC, *PTX_TIMEOUT_CFG_STRUC;
+#else
+typedef	union	_TX_TIMEOUT_CFG_STRUC	{
+	struct	{
+	    UINT32       rsv:4;
+	    UINT32       MpduLifeTime:4;    //  expiration time = 2^(9+MPDU LIFE TIME)  us
+	    UINT32       RxAckTimeout:8;	// unit:slot. Used for TX precedure
+	    UINT32       TxopTimeout:8;	//TXOP timeout value for TXOP truncation.  It is recommended that (SLOT_TIME) > (TX_OP_TIMEOUT) > (RX_ACK_TIMEOUT)
+	    UINT32       rsv2:8;     // 1: HT non-STBC control frame enable
+	}	field;
+	UINT32			word;
+}	TX_TIMEOUT_CFG_STRUC, *PTX_TIMEOUT_CFG_STRUC;
+#endif
+#define TX_RTY_CFG	0x134c
+#ifdef RT_BIG_ENDIAN
+typedef	union PACKED _TX_RTY_CFG_STRUC	{
+	struct	{
+	    UINT32       rsv:1;
+	    UINT32       TxautoFBEnable:1;    // Tx retry PHY rate auto fallback enable
+	    UINT32       AggRtyMode:1;	// Aggregate MPDU retry mode.  0:expired by retry limit, 1: expired by mpdu life timer
+	    UINT32       NonAggRtyMode:1;	// Non-Aggregate MPDU retry mode.  0:expired by retry limit, 1: expired by mpdu life timer
+	    UINT32       LongRtyThre:12;	// Long retry threshoold
+	    UINT32       LongRtyLimit:8;	//long retry limit
+	    UINT32       ShortRtyLimit:8;	//  short retry limit
+
+	}	field;
+	UINT32			word;
+}	TX_RTY_CFG_STRUC, *PTX_RTY_CFG_STRUC;
+#else
+typedef	union PACKED _TX_RTY_CFG_STRUC	{
+	struct	{
+	    UINT32       ShortRtyLimit:8;	//  short retry limit
+	    UINT32       LongRtyLimit:8;	//long retry limit
+	    UINT32       LongRtyThre:12;	// Long retry threshoold
+	    UINT32       NonAggRtyMode:1;	// Non-Aggregate MPDU retry mode.  0:expired by retry limit, 1: expired by mpdu life timer
+	    UINT32       AggRtyMode:1;	// Aggregate MPDU retry mode.  0:expired by retry limit, 1: expired by mpdu life timer
+	    UINT32       TxautoFBEnable:1;    // Tx retry PHY rate auto fallback enable
+	    UINT32       rsv:1;     // 1: HT non-STBC control frame enable
+	}	field;
+	UINT32			word;
+}	TX_RTY_CFG_STRUC, *PTX_RTY_CFG_STRUC;
+#endif
+#define TX_LINK_CFG	0x1350
+#ifdef RT_BIG_ENDIAN
+typedef	union	PACKED _TX_LINK_CFG_STRUC	{
+	struct PACKED {
+	    UINT32       RemotMFS:8;	//remote MCS feedback sequence number
+	    UINT32       RemotMFB:8;    //  remote MCS feedback
+	    UINT32       rsv:3;	//
+	    UINT32       TxCFAckEn:1;	//   Piggyback CF-ACK enable
+	    UINT32       TxRDGEn:1;	// RDG TX enable
+	    UINT32       TxMRQEn:1;	//  MCS request TX enable
+	    UINT32       RemoteUMFSEnable:1;	//  remote unsolicit  MFB enable.  0: not apply remote remote unsolicit (MFS=7)
+	    UINT32       MFBEnable:1;	//  TX apply remote MFB 1:enable
+	    UINT32       RemoteMFBLifeTime:8;	//remote MFB life time. unit : 32us
+	}	field;
+	UINT32			word;
+}	TX_LINK_CFG_STRUC, *PTX_LINK_CFG_STRUC;
+#else
+typedef	union	PACKED _TX_LINK_CFG_STRUC	{
+	struct PACKED {
+	    UINT32       RemoteMFBLifeTime:8;	//remote MFB life time. unit : 32us
+	    UINT32       MFBEnable:1;	//  TX apply remote MFB 1:enable
+	    UINT32       RemoteUMFSEnable:1;	//  remote unsolicit  MFB enable.  0: not apply remote remote unsolicit (MFS=7)
+	    UINT32       TxMRQEn:1;	//  MCS request TX enable
+	    UINT32       TxRDGEn:1;	// RDG TX enable
+	    UINT32       TxCFAckEn:1;	//   Piggyback CF-ACK enable
+	    UINT32       rsv:3;	//
+	    UINT32       RemotMFB:8;    //  remote MCS feedback
+	    UINT32       RemotMFS:8;	//remote MCS feedback sequence number
+	}	field;
+	UINT32			word;
+}	TX_LINK_CFG_STRUC, *PTX_LINK_CFG_STRUC;
+#endif
+#define HT_FBK_CFG0	0x1354
+#ifdef RT_BIG_ENDIAN
+typedef	union PACKED _HT_FBK_CFG0_STRUC	{
+	struct	{
+	    UINT32       HTMCS7FBK:4;
+	    UINT32       HTMCS6FBK:4;
+	    UINT32       HTMCS5FBK:4;
+	    UINT32       HTMCS4FBK:4;
+	    UINT32       HTMCS3FBK:4;
+	    UINT32       HTMCS2FBK:4;
+	    UINT32       HTMCS1FBK:4;
+	    UINT32       HTMCS0FBK:4;
+	}	field;
+	UINT32			word;
+}	HT_FBK_CFG0_STRUC, *PHT_FBK_CFG0_STRUC;
+#else
+typedef	union PACKED _HT_FBK_CFG0_STRUC	{
+	struct	{
+	    UINT32       HTMCS0FBK:4;
+	    UINT32       HTMCS1FBK:4;
+	    UINT32       HTMCS2FBK:4;
+	    UINT32       HTMCS3FBK:4;
+	    UINT32       HTMCS4FBK:4;
+	    UINT32       HTMCS5FBK:4;
+	    UINT32       HTMCS6FBK:4;
+	    UINT32       HTMCS7FBK:4;
+	}	field;
+	UINT32			word;
+}	HT_FBK_CFG0_STRUC, *PHT_FBK_CFG0_STRUC;
+#endif
+#define HT_FBK_CFG1	0x1358
+#ifdef RT_BIG_ENDIAN
+typedef	union	_HT_FBK_CFG1_STRUC	{
+	struct	{
+	    UINT32       HTMCS15FBK:4;
+	    UINT32       HTMCS14FBK:4;
+	    UINT32       HTMCS13FBK:4;
+	    UINT32       HTMCS12FBK:4;
+	    UINT32       HTMCS11FBK:4;
+	    UINT32       HTMCS10FBK:4;
+	    UINT32       HTMCS9FBK:4;
+	    UINT32       HTMCS8FBK:4;
+	}	field;
+	UINT32			word;
+}	HT_FBK_CFG1_STRUC, *PHT_FBK_CFG1_STRUC;
+#else
+typedef	union	_HT_FBK_CFG1_STRUC	{
+	struct	{
+	    UINT32       HTMCS8FBK:4;
+	    UINT32       HTMCS9FBK:4;
+	    UINT32       HTMCS10FBK:4;
+	    UINT32       HTMCS11FBK:4;
+	    UINT32       HTMCS12FBK:4;
+	    UINT32       HTMCS13FBK:4;
+	    UINT32       HTMCS14FBK:4;
+	    UINT32       HTMCS15FBK:4;
+	}	field;
+	UINT32			word;
+}	HT_FBK_CFG1_STRUC, *PHT_FBK_CFG1_STRUC;
+#endif
+#define LG_FBK_CFG0	0x135c
+#ifdef RT_BIG_ENDIAN
+typedef	union	_LG_FBK_CFG0_STRUC	{
+	struct	{
+	    UINT32       OFDMMCS7FBK:4;	//initial value is 6
+	    UINT32       OFDMMCS6FBK:4;	//initial value is 5
+	    UINT32       OFDMMCS5FBK:4;	//initial value is 4
+	    UINT32       OFDMMCS4FBK:4;	//initial value is 3
+	    UINT32       OFDMMCS3FBK:4;	//initial value is 2
+	    UINT32       OFDMMCS2FBK:4;	//initial value is 1
+	    UINT32       OFDMMCS1FBK:4;	//initial value is 0
+	    UINT32       OFDMMCS0FBK:4;	//initial value is 0
+	}	field;
+	UINT32			word;
+}	LG_FBK_CFG0_STRUC, *PLG_FBK_CFG0_STRUC;
+#else
+typedef	union	_LG_FBK_CFG0_STRUC	{
+	struct	{
+	    UINT32       OFDMMCS0FBK:4;	//initial value is 0
+	    UINT32       OFDMMCS1FBK:4;	//initial value is 0
+	    UINT32       OFDMMCS2FBK:4;	//initial value is 1
+	    UINT32       OFDMMCS3FBK:4;	//initial value is 2
+	    UINT32       OFDMMCS4FBK:4;	//initial value is 3
+	    UINT32       OFDMMCS5FBK:4;	//initial value is 4
+	    UINT32       OFDMMCS6FBK:4;	//initial value is 5
+	    UINT32       OFDMMCS7FBK:4;	//initial value is 6
+	}	field;
+	UINT32			word;
+}	LG_FBK_CFG0_STRUC, *PLG_FBK_CFG0_STRUC;
+#endif
+#define LG_FBK_CFG1		0x1360
+#ifdef RT_BIG_ENDIAN
+typedef	union	_LG_FBK_CFG1_STRUC	{
+	struct	{
+	    UINT32       rsv:16;
+	    UINT32       CCKMCS3FBK:4;	//initial value is 2
+	    UINT32       CCKMCS2FBK:4;	//initial value is 1
+	    UINT32       CCKMCS1FBK:4;	//initial value is 0
+	    UINT32       CCKMCS0FBK:4;	//initial value is 0
+	}	field;
+	UINT32			word;
+}	LG_FBK_CFG1_STRUC, *PLG_FBK_CFG1_STRUC;
+#else
+typedef	union	_LG_FBK_CFG1_STRUC	{
+	struct	{
+	    UINT32       CCKMCS0FBK:4;	//initial value is 0
+	    UINT32       CCKMCS1FBK:4;	//initial value is 0
+	    UINT32       CCKMCS2FBK:4;	//initial value is 1
+	    UINT32       CCKMCS3FBK:4;	//initial value is 2
+	    UINT32       rsv:16;
+	}	field;
+	UINT32			word;
+}	LG_FBK_CFG1_STRUC, *PLG_FBK_CFG1_STRUC;
+#endif
+
+//=======================================================
+//================ Protection Paramater================================
+//=======================================================
+#define CCK_PROT_CFG	0x1364		//CCK Protection
+#define ASIC_SHORTNAV		1
+#define ASIC_LONGNAV		2
+#define ASIC_RTS		1
+#define ASIC_CTS		2
+#ifdef RT_BIG_ENDIAN
+typedef	union	_PROT_CFG_STRUC	{
+	struct	{
+	    UINT32       rsv:5;
+	    UINT32       RTSThEn:1;	//RTS threshold enable on CCK TX
+	    UINT32       TxopAllowGF40:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       TxopAllowGF20:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       TxopAllowMM40:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       TxopAllowMM20:1;	//CCK TXOP allowance. 0:disallow.
+	    UINT32       TxopAllowOfdm:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       TxopAllowCck:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       ProtectNav:2;	//TXOP protection type for CCK TX. 0:None, 1:ShortNAVprotect,  2:LongNAVProtect, 3:rsv
+	    UINT32       ProtectCtrl:2;	//Protection control frame type for CCK TX. 1:RTS/CTS, 2:CTS-to-self, 0:None, 3:rsv
+	    UINT32       ProtectRate:16;	//Protection control frame rate for CCK TX(RTS/CTS/CFEnd).
+	}	field;
+	UINT32			word;
+}	PROT_CFG_STRUC, *PPROT_CFG_STRUC;
+#else
+typedef	union	_PROT_CFG_STRUC	{
+	struct	{
+	    UINT32       ProtectRate:16;	//Protection control frame rate for CCK TX(RTS/CTS/CFEnd).
+	    UINT32       ProtectCtrl:2;	//Protection control frame type for CCK TX. 1:RTS/CTS, 2:CTS-to-self, 0:None, 3:rsv
+	    UINT32       ProtectNav:2;	//TXOP protection type for CCK TX. 0:None, 1:ShortNAVprotect,  2:LongNAVProtect, 3:rsv
+	    UINT32       TxopAllowCck:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       TxopAllowOfdm:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       TxopAllowMM20:1;	//CCK TXOP allowance. 0:disallow.
+	    UINT32       TxopAllowMM40:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       TxopAllowGF20:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       TxopAllowGF40:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       RTSThEn:1;	//RTS threshold enable on CCK TX
+	    UINT32       rsv:5;
+	}	field;
+	UINT32			word;
+}	PROT_CFG_STRUC, *PPROT_CFG_STRUC;
+#endif
+
+#define OFDM_PROT_CFG	0x1368		//OFDM Protection
+#define MM20_PROT_CFG	0x136C		//MM20 Protection
+#define MM40_PROT_CFG	0x1370		//MM40 Protection
+#define GF20_PROT_CFG	0x1374		//GF20 Protection
+#define GF40_PROT_CFG	0x1378		//GR40 Protection
+#define EXP_CTS_TIME	0x137C		//
+#define EXP_ACK_TIME	0x1380		//
+
+//
+//  4.4 MAC RX configuration registers (offset:0x1400)
+//
+#define RX_FILTR_CFG	0x1400			//TXRX_CSR0
+#define AUTO_RSP_CFG	0x1404			//TXRX_CSR4
+//
+// TXRX_CSR4: Auto-Responder/
+//
+#ifdef RT_BIG_ENDIAN
+typedef union _AUTO_RSP_CFG_STRUC {
+ struct {
+     UINT32        :24;
+     UINT32       AckCtsPsmBit:1;   // Power bit value in conrtrol frame
+     UINT32       DualCTSEn:1;   // Power bit value in conrtrol frame
+     UINT32       rsv:1;   // Power bit value in conrtrol frame
+     UINT32       AutoResponderPreamble:1;    // 0:long, 1:short preamble
+     UINT32       CTS40MRef:1;  // Response CTS 40MHz duplicate mode
+     UINT32       CTS40MMode:1;  // Response CTS 40MHz duplicate mode
+     UINT32       BACAckPolicyEnable:1;    // 0:long, 1:short preamble
+     UINT32       AutoResponderEnable:1;
+ } field;
+ UINT32   word;
+} AUTO_RSP_CFG_STRUC, *PAUTO_RSP_CFG_STRUC;
+#else
+typedef union _AUTO_RSP_CFG_STRUC {
+ struct {
+     UINT32       AutoResponderEnable:1;
+     UINT32       BACAckPolicyEnable:1;    // 0:long, 1:short preamble
+     UINT32       CTS40MMode:1;  // Response CTS 40MHz duplicate mode
+     UINT32       CTS40MRef:1;  // Response CTS 40MHz duplicate mode
+     UINT32       AutoResponderPreamble:1;    // 0:long, 1:short preamble
+     UINT32       rsv:1;   // Power bit value in conrtrol frame
+     UINT32       DualCTSEn:1;   // Power bit value in conrtrol frame
+     UINT32       AckCtsPsmBit:1;   // Power bit value in conrtrol frame
+     UINT32        :24;
+ } field;
+ UINT32   word;
+} AUTO_RSP_CFG_STRUC, *PAUTO_RSP_CFG_STRUC;
+#endif
+
+#define LEGACY_BASIC_RATE	0x1408	//  TXRX_CSR5           0x3054
+#define HT_BASIC_RATE		0x140c
+#define HT_CTRL_CFG		0x1410
+#define SIFS_COST_CFG		0x1414
+#define RX_PARSER_CFG		0x1418	//Set NAV for all received frames
+
+//
+//  4.5 MAC Security configuration (offset:0x1500)
+//
+#define TX_SEC_CNT0		0x1500		//
+#define RX_SEC_CNT0		0x1504		//
+#define CCMP_FC_MUTE		0x1508		//
+//
+//  4.6 HCCA/PSMP (offset:0x1600)
+//
+#define TXOP_HLDR_ADDR0		0x1600
+#define TXOP_HLDR_ADDR1		0x1604
+#define TXOP_HLDR_ET		0x1608
+#define QOS_CFPOLL_RA_DW0		0x160c
+#define QOS_CFPOLL_A1_DW1		0x1610
+#define QOS_CFPOLL_QC		0x1614
+//
+//  4.7 MAC Statistis registers (offset:0x1700)
+//
+#define RX_STA_CNT0		0x1700		//
+#define RX_STA_CNT1		0x1704		//
+#define RX_STA_CNT2		0x1708		//
+
+//
+// RX_STA_CNT0_STRUC: RX PLCP error count & RX CRC error count
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_RX_STA_CNT0_STRUC	{
+	struct	{
+	    USHORT  PhyErr;
+	    USHORT  CrcErr;
+	}	field;
+	UINT32			word;
+}	RX_STA_CNT0_STRUC, *PRX_STA_CNT0_STRUC;
+#else
+typedef	union	_RX_STA_CNT0_STRUC	{
+	struct	{
+	    USHORT  CrcErr;
+	    USHORT  PhyErr;
+	}	field;
+	UINT32			word;
+}	RX_STA_CNT0_STRUC, *PRX_STA_CNT0_STRUC;
+#endif
+
+//
+// RX_STA_CNT1_STRUC: RX False CCA count & RX LONG frame count
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_RX_STA_CNT1_STRUC	{
+	struct	{
+	    USHORT  PlcpErr;
+	    USHORT  FalseCca;
+	}	field;
+	UINT32			word;
+}	RX_STA_CNT1_STRUC, *PRX_STA_CNT1_STRUC;
+#else
+typedef	union	_RX_STA_CNT1_STRUC	{
+	struct	{
+	    USHORT  FalseCca;
+	    USHORT  PlcpErr;
+	}	field;
+	UINT32			word;
+}	RX_STA_CNT1_STRUC, *PRX_STA_CNT1_STRUC;
+#endif
+
+//
+// RX_STA_CNT2_STRUC:
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_RX_STA_CNT2_STRUC	{
+	struct	{
+	    USHORT  RxFifoOverflowCount;
+	    USHORT  RxDupliCount;
+	}	field;
+	UINT32			word;
+}	RX_STA_CNT2_STRUC, *PRX_STA_CNT2_STRUC;
+#else
+typedef	union	_RX_STA_CNT2_STRUC	{
+	struct	{
+	    USHORT  RxDupliCount;
+	    USHORT  RxFifoOverflowCount;
+	}	field;
+	UINT32			word;
+}	RX_STA_CNT2_STRUC, *PRX_STA_CNT2_STRUC;
+#endif
+#define TX_STA_CNT0		0x170C		//
+//
+// STA_CSR3: TX Beacon count
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_STA_CNT0_STRUC	{
+	struct	{
+	    USHORT  TxBeaconCount;
+	    USHORT  TxFailCount;
+	}	field;
+	UINT32			word;
+}	TX_STA_CNT0_STRUC, *PTX_STA_CNT0_STRUC;
+#else
+typedef	union	_TX_STA_CNT0_STRUC	{
+	struct	{
+	    USHORT  TxFailCount;
+	    USHORT  TxBeaconCount;
+	}	field;
+	UINT32			word;
+}	TX_STA_CNT0_STRUC, *PTX_STA_CNT0_STRUC;
+#endif
+#define TX_STA_CNT1		0x1710		//
+//
+// TX_STA_CNT1: TX tx count
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_STA_CNT1_STRUC	{
+	struct	{
+	    USHORT  TxRetransmit;
+	    USHORT  TxSuccess;
+	}	field;
+	UINT32			word;
+}	TX_STA_CNT1_STRUC, *PTX_STA_CNT1_STRUC;
+#else
+typedef	union	_TX_STA_CNT1_STRUC	{
+	struct	{
+	    USHORT  TxSuccess;
+	    USHORT  TxRetransmit;
+	}	field;
+	UINT32			word;
+}	TX_STA_CNT1_STRUC, *PTX_STA_CNT1_STRUC;
+#endif
+#define TX_STA_CNT2		0x1714		//
+//
+// TX_STA_CNT2: TX tx count
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_STA_CNT2_STRUC	{
+	struct	{
+	    USHORT  TxUnderFlowCount;
+	    USHORT  TxZeroLenCount;
+	}	field;
+	UINT32			word;
+}	TX_STA_CNT2_STRUC, *PTX_STA_CNT2_STRUC;
+#else
+typedef	union	_TX_STA_CNT2_STRUC	{
+	struct	{
+	    USHORT  TxZeroLenCount;
+	    USHORT  TxUnderFlowCount;
+	}	field;
+	UINT32			word;
+}	TX_STA_CNT2_STRUC, *PTX_STA_CNT2_STRUC;
+#endif
+#define TX_STA_FIFO		0x1718		//
+//
+// TX_STA_FIFO_STRUC: TX Result for specific PID status fifo register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union PACKED _TX_STA_FIFO_STRUC	{
+	struct	{
+		UINT32		Reserve:2;
+		UINT32		TxBF:1; // 3*3
+		UINT32		SuccessRate:13;	//include MCS, mode ,shortGI, BW settingSame format as TXWI Word 0 Bit 31-16.
+//		UINT32		SuccessRate:16;	//include MCS, mode ,shortGI, BW settingSame format as TXWI Word 0 Bit 31-16.
+		UINT32		wcid:8;		//wireless client index
+		UINT32       	TxAckRequired:1;    // ack required
+		UINT32       	TxAggre:1;    // Tx is aggregated
+		UINT32       	TxSuccess:1;   // Tx success. whether success or not
+		UINT32       	PidType:4;
+		UINT32       	bValid:1;   // 1:This register contains a valid TX result
+	}	field;
+	UINT32			word;
+}	TX_STA_FIFO_STRUC, *PTX_STA_FIFO_STRUC;
+#else
+typedef	union PACKED _TX_STA_FIFO_STRUC	{
+	struct	{
+		UINT32       	bValid:1;   // 1:This register contains a valid TX result
+		UINT32       	PidType:4;
+		UINT32       	TxSuccess:1;   // Tx No retry success
+		UINT32       	TxAggre:1;    // Tx Retry Success
+		UINT32       	TxAckRequired:1;    // Tx fail
+		UINT32		wcid:8;		//wireless client index
+//		UINT32		SuccessRate:16;	//include MCS, mode ,shortGI, BW settingSame format as TXWI Word 0 Bit 31-16.
+		UINT32		SuccessRate:13;	//include MCS, mode ,shortGI, BW settingSame format as TXWI Word 0 Bit 31-16.
+		UINT32		TxBF:1;
+		UINT32		Reserve:2;
+	}	field;
+	UINT32			word;
+}	TX_STA_FIFO_STRUC, *PTX_STA_FIFO_STRUC;
+#endif
+// Debug counter
+#define TX_AGG_CNT	0x171c
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_AGG_CNT_STRUC	{
+	struct	{
+	    USHORT  AggTxCount;
+	    USHORT  NonAggTxCount;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT_STRUC, *PTX_AGG_CNT_STRUC;
+#else
+typedef	union	_TX_AGG_CNT_STRUC	{
+	struct	{
+	    USHORT  NonAggTxCount;
+	    USHORT  AggTxCount;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT_STRUC, *PTX_AGG_CNT_STRUC;
+#endif
+// Debug counter
+#define TX_AGG_CNT0	0x1720
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_AGG_CNT0_STRUC	{
+	struct	{
+	    USHORT  AggSize2Count;
+	    USHORT  AggSize1Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT0_STRUC, *PTX_AGG_CNT0_STRUC;
+#else
+typedef	union	_TX_AGG_CNT0_STRUC	{
+	struct	{
+	    USHORT  AggSize1Count;
+	    USHORT  AggSize2Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT0_STRUC, *PTX_AGG_CNT0_STRUC;
+#endif
+// Debug counter
+#define TX_AGG_CNT1	0x1724
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_AGG_CNT1_STRUC	{
+	struct	{
+	    USHORT  AggSize4Count;
+	    USHORT  AggSize3Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT1_STRUC, *PTX_AGG_CNT1_STRUC;
+#else
+typedef	union	_TX_AGG_CNT1_STRUC	{
+	struct	{
+	    USHORT  AggSize3Count;
+	    USHORT  AggSize4Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT1_STRUC, *PTX_AGG_CNT1_STRUC;
+#endif
+#define TX_AGG_CNT2	0x1728
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_AGG_CNT2_STRUC	{
+	struct	{
+	    USHORT  AggSize6Count;
+	    USHORT  AggSize5Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT2_STRUC, *PTX_AGG_CNT2_STRUC;
+#else
+typedef	union	_TX_AGG_CNT2_STRUC	{
+	struct	{
+	    USHORT  AggSize5Count;
+	    USHORT  AggSize6Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT2_STRUC, *PTX_AGG_CNT2_STRUC;
+#endif
+// Debug counter
+#define TX_AGG_CNT3	0x172c
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_AGG_CNT3_STRUC	{
+	struct	{
+	    USHORT  AggSize8Count;
+	    USHORT  AggSize7Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT3_STRUC, *PTX_AGG_CNT3_STRUC;
+#else
+typedef	union	_TX_AGG_CNT3_STRUC	{
+	struct	{
+	    USHORT  AggSize7Count;
+	    USHORT  AggSize8Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT3_STRUC, *PTX_AGG_CNT3_STRUC;
+#endif
+// Debug counter
+#define TX_AGG_CNT4	0x1730
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_AGG_CNT4_STRUC	{
+	struct	{
+	    USHORT  AggSize10Count;
+	    USHORT  AggSize9Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT4_STRUC, *PTX_AGG_CNT4_STRUC;
+#else
+typedef	union	_TX_AGG_CNT4_STRUC	{
+	struct	{
+	    USHORT  AggSize9Count;
+	    USHORT  AggSize10Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT4_STRUC, *PTX_AGG_CNT4_STRUC;
+#endif
+#define TX_AGG_CNT5	0x1734
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_AGG_CNT5_STRUC	{
+	struct	{
+	    USHORT  AggSize12Count;
+	    USHORT  AggSize11Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT5_STRUC, *PTX_AGG_CNT5_STRUC;
+#else
+typedef	union	_TX_AGG_CNT5_STRUC	{
+	struct	{
+	    USHORT  AggSize11Count;
+	    USHORT  AggSize12Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT5_STRUC, *PTX_AGG_CNT5_STRUC;
+#endif
+#define TX_AGG_CNT6		0x1738
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_AGG_CNT6_STRUC	{
+	struct	{
+	    USHORT  AggSize14Count;
+	    USHORT  AggSize13Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT6_STRUC, *PTX_AGG_CNT6_STRUC;
+#else
+typedef	union	_TX_AGG_CNT6_STRUC	{
+	struct	{
+	    USHORT  AggSize13Count;
+	    USHORT  AggSize14Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT6_STRUC, *PTX_AGG_CNT6_STRUC;
+#endif
+#define TX_AGG_CNT7		0x173c
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_AGG_CNT7_STRUC	{
+	struct	{
+	    USHORT  AggSize16Count;
+	    USHORT  AggSize15Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT7_STRUC, *PTX_AGG_CNT7_STRUC;
+#else
+typedef	union	_TX_AGG_CNT7_STRUC	{
+	struct	{
+	    USHORT  AggSize15Count;
+	    USHORT  AggSize16Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT7_STRUC, *PTX_AGG_CNT7_STRUC;
+#endif
+#define MPDU_DENSITY_CNT		0x1740
+#ifdef RT_BIG_ENDIAN
+typedef	union	_MPDU_DEN_CNT_STRUC	{
+	struct	{
+	    USHORT  RXZeroDelCount;	//RX zero length delimiter count
+	    USHORT  TXZeroDelCount;	//TX zero length delimiter count
+	}	field;
+	UINT32			word;
+}	MPDU_DEN_CNT_STRUC, *PMPDU_DEN_CNT_STRUC;
+#else
+typedef	union	_MPDU_DEN_CNT_STRUC	{
+	struct	{
+	    USHORT  TXZeroDelCount;	//TX zero length delimiter count
+	    USHORT  RXZeroDelCount;	//RX zero length delimiter count
+	}	field;
+	UINT32			word;
+}	MPDU_DEN_CNT_STRUC, *PMPDU_DEN_CNT_STRUC;
+#endif
+//
+// TXRX control registers - base address 0x3000
+//
+// rt2860b  UNKNOWN reg use R/O Reg Addr 0x77d0 first..
+#define TXRX_CSR1           0x77d0
+
+//
+// Security key table memory, base address = 0x1000
+//
+#define MAC_WCID_BASE		0x1800 //8-bytes(use only 6-bytes) * 256 entry =
+#define HW_WCID_ENTRY_SIZE   8
+#define PAIRWISE_KEY_TABLE_BASE     0x4000      // 32-byte * 256-entry =  -byte
+#define HW_KEY_ENTRY_SIZE           0x20
+#define PAIRWISE_IVEIV_TABLE_BASE     0x6000      // 8-byte * 256-entry =  -byte
+#define MAC_IVEIV_TABLE_BASE     0x6000      // 8-byte * 256-entry =  -byte
+#define HW_IVEIV_ENTRY_SIZE   8
+#define MAC_WCID_ATTRIBUTE_BASE     0x6800      // 4-byte * 256-entry =  -byte
+#define HW_WCID_ATTRI_SIZE   4
+#define WCID_RESERVED          		0x6bfc
+#define SHARED_KEY_TABLE_BASE       0x6c00      // 32-byte * 16-entry = 512-byte
+#define SHARED_KEY_MODE_BASE       0x7000      // 32-byte * 16-entry = 512-byte
+#define HW_SHARED_KEY_MODE_SIZE   4
+#define SHAREDKEYTABLE			0
+#define PAIRWISEKEYTABLE			1
+
+
+#ifdef RT_BIG_ENDIAN
+typedef	union	_SHAREDKEY_MODE_STRUC	{
+	struct	{
+		UINT32       :1;
+		UINT32       Bss1Key3CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss1Key2CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss1Key1CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss1Key0CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss0Key3CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss0Key2CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss0Key1CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss0Key0CipherAlg:3;
+	}	field;
+	UINT32			word;
+}	SHAREDKEY_MODE_STRUC, *PSHAREDKEY_MODE_STRUC;
+#else
+typedef	union	_SHAREDKEY_MODE_STRUC	{
+	struct	{
+		UINT32       Bss0Key0CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss0Key1CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss0Key2CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss0Key3CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss1Key0CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss1Key1CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss1Key2CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss1Key3CipherAlg:3;
+		UINT32       :1;
+	}	field;
+	UINT32			word;
+}	SHAREDKEY_MODE_STRUC, *PSHAREDKEY_MODE_STRUC;
+#endif
+// 64-entry for pairwise key table
+typedef struct _HW_WCID_ENTRY {  // 8-byte per entry
+    UCHAR   Address[6];
+    UCHAR   Rsv[2];
+} HW_WCID_ENTRY, PHW_WCID_ENTRY;
+
+
+
+//
+// Other on-chip shared memory space, base = 0x2000
+//
+
+// CIS space - base address = 0x2000
+#define HW_CIS_BASE             0x2000
+
+// Carrier-sense CTS frame base address. It's where mac stores carrier-sense frame for carrier-sense function.
+#define HW_CS_CTS_BASE			0x7700
+// DFS CTS frame base address. It's where mac stores CTS frame for DFS.
+#define HW_DFS_CTS_BASE			0x7780
+#define HW_CTS_FRAME_SIZE		0x80
+
+// 2004-11-08 john - since NULL frame won't be that long (256 byte). We steal 16 tail bytes
+// to save debugging settings
+#define HW_DEBUG_SETTING_BASE   0x77f0  // 0x77f0~0x77ff total 16 bytes
+#define HW_DEBUG_SETTING_BASE2   0x7770  // 0x77f0~0x77ff total 16 bytes
+
+// In order to support maximum 8 MBSS and its maximum length is 512 for each beacon
+// Three section discontinue memory segments will be used.
+// 1. The original region for BCN 0~3
+// 2. Extract memory from FCE table for BCN 4~5
+// 3. Extract memory from Pair-wise key table for BCN 6~7
+//	  It occupied those memory of wcid 238~253 for BCN 6
+//						      and wcid 222~237 for BCN 7
+#define HW_BEACON_MAX_SIZE      0x1000 /* unit: byte */
+#define HW_BEACON_BASE0         0x7800
+#define HW_BEACON_BASE1         0x7A00
+#define HW_BEACON_BASE2         0x7C00
+#define HW_BEACON_BASE3         0x7E00
+#define HW_BEACON_BASE4         0x7200
+#define HW_BEACON_BASE5         0x7400
+#define HW_BEACON_BASE6         0x5DC0
+#define HW_BEACON_BASE7         0x5BC0
+
+#define HW_BEACON_MAX_COUNT     8
+#define HW_BEACON_OFFSET		0x0200
+#define HW_BEACON_CONTENT_LEN	(HW_BEACON_OFFSET - TXWI_SIZE)
+
+// HOST-MCU shared memory - base address = 0x2100
+#define HOST_CMD_CSR		0x404
+#define H2M_MAILBOX_CSR         0x7010
+#define H2M_MAILBOX_CID         0x7014
+#define H2M_MAILBOX_STATUS      0x701c
+#define H2M_INT_SRC             0x7024
+#define H2M_BBP_AGENT           0x7028
+#define M2H_CMD_DONE_CSR        0x000c
+#define MCU_TXOP_ARRAY_BASE     0x000c   // TODO: to be provided by Albert
+#define MCU_TXOP_ENTRY_SIZE     32       // TODO: to be provided by Albert
+#define MAX_NUM_OF_TXOP_ENTRY   16       // TODO: must be same with 8051 firmware
+#define MCU_MBOX_VERSION        0x01     // TODO: to be confirmed by Albert
+#define MCU_MBOX_VERSION_OFFSET 5        // TODO: to be provided by Albert
+
+//
+// Host DMA registers - base address 0x200 .  TX0-3=EDCAQid0-3, TX4=HCCA, TX5=MGMT,
+//
+//
+//  DMA RING DESCRIPTOR
+//
+#define E2PROM_CSR          0x0004
+#define IO_CNTL_CSR         0x77d0
+
+#ifdef RT2860
+// 8051 firmware image for RT2860 - base address = 0x4000
+#define FIRMWARE_IMAGE_BASE     0x2000
+#define MAX_FIRMWARE_IMAGE_SIZE 0x2000    // 8kbyte
+#endif // RT2860 //
+
+
+// ================================================================
+// Tx /	Rx / Mgmt ring descriptor definition
+// ================================================================
+
+// the following PID values are used to mark outgoing frame type in TXD->PID so that
+// proper TX statistics can be collected based on these categories
+// b3-2 of PID field -
+#define PID_MGMT			0x05
+#define PID_BEACON			0x0c
+#define PID_DATA_NORMALUCAST	 	0x02
+#define PID_DATA_AMPDU	 	0x04
+#define PID_DATA_NO_ACK    	0x08
+#define PID_DATA_NOT_NORM_ACK	 	0x03
+// value domain of pTxD->HostQId (4-bit: 0~15)
+#define QID_AC_BK               1   // meet ACI definition in 802.11e
+#define QID_AC_BE               0   // meet ACI definition in 802.11e
+#define QID_AC_VI               2
+#define QID_AC_VO               3
+#define QID_HCCA                4
+#define NUM_OF_TX_RING          5
+#define QID_MGMT                13
+#define QID_RX                  14
+#define QID_OTHER               15
+
+
+// ------------------------------------------------------
+// BBP & RF	definition
+// ------------------------------------------------------
+#define	BUSY		                1
+#define	IDLE		                0
+
+#define	RF_R00					    0
+#define	RF_R01					    1
+#define	RF_R02					    2
+#define	RF_R03					    3
+#define	RF_R04					    4
+#define	RF_R05					    5
+#define	RF_R06					    6
+#define	RF_R07					    7
+#define	RF_R08					    8
+#define	RF_R09					    9
+#define	RF_R10					    10
+#define	RF_R11					    11
+#define	RF_R12					    12
+#define	RF_R13					    13
+#define	RF_R14					    14
+#define	RF_R15					    15
+#define	RF_R16					    16
+#define	RF_R17					    17
+#define	RF_R18					    18
+#define	RF_R19					    19
+#define	RF_R20					    20
+#define	RF_R21					    21
+#define	RF_R22					    22
+#define	RF_R23					    23
+#define	RF_R24					    24
+#define	RF_R25					    25
+#define	RF_R26					    26
+#define	RF_R27					    27
+#define	RF_R28					    28
+#define	RF_R29					    29
+#define	RF_R30					    30
+#define	RF_R31					    31
+
+#define	BBP_R0					    0  // version
+#define	BBP_R1				        1  // TSSI
+#define	BBP_R2          			2  // TX configure
+#define BBP_R3                      3
+#define BBP_R4                      4
+#define BBP_R5                      5
+#define BBP_R6                      6
+#define	BBP_R14			            14 // RX configure
+#define BBP_R16                     16
+#define BBP_R17                     17 // RX sensibility
+#define BBP_R18                     18
+#define BBP_R21                     21
+#define BBP_R22                     22
+#define BBP_R24                     24
+#define BBP_R25                     25
+#define BBP_R49                     49 //TSSI
+#define BBP_R50                     50
+#define BBP_R51                     51
+#define BBP_R52                     52
+#define BBP_R55                     55
+#define BBP_R62                     62 // Rx SQ0 Threshold HIGH
+#define BBP_R63                     63
+#define BBP_R64                     64
+#define BBP_R65                     65
+#define BBP_R66                     66
+#define BBP_R67                     67
+#define BBP_R68                     68
+#define BBP_R69                     69
+#define BBP_R70                     70 // Rx AGC SQ CCK Xcorr threshold
+#define BBP_R73                     73
+#define BBP_R75						75
+#define BBP_R77                     77
+#define BBP_R81                     81
+#define BBP_R82                     82
+#define BBP_R83                     83
+#define BBP_R84                     84
+#define BBP_R86						86
+#define BBP_R91						91
+#define BBP_R92						92
+#define BBP_R94                     94 // Tx Gain Control
+#define BBP_R103                    103
+#define BBP_R105                    105
+#define BBP_R113                    113
+#define BBP_R114                    114
+#define BBP_R115                    115
+#define BBP_R116                    116
+#define BBP_R117                    117
+#define BBP_R118                    118
+#define BBP_R119                    119
+#define BBP_R120                    120
+#define BBP_R121                    121
+#define BBP_R122                    122
+#define BBP_R123                    123
+
+
+#define BBPR94_DEFAULT              0x06 // Add 1 value will gain 1db
+
+#define RSSI_FOR_VERY_LOW_SENSIBILITY -35
+#define RSSI_FOR_LOW_SENSIBILITY      -58
+#define RSSI_FOR_MID_LOW_SENSIBILITY  -80
+#define RSSI_FOR_MID_SENSIBILITY      -90
+
+//-------------------------------------------------------------------------
+// EEPROM definition
+//-------------------------------------------------------------------------
+#define EEDO                        0x08
+#define EEDI                        0x04
+#define EECS                        0x02
+#define EESK                        0x01
+#define EERL                        0x80
+
+#define EEPROM_WRITE_OPCODE         0x05
+#define EEPROM_READ_OPCODE          0x06
+#define EEPROM_EWDS_OPCODE          0x10
+#define EEPROM_EWEN_OPCODE          0x13
+
+#define	NUM_EEPROM_BBP_PARMS		19			// Include NIC Config 0, 1, CR, TX ALC step, BBPs
+#define	NUM_EEPROM_TX_G_PARMS		7
+#define	EEPROM_NIC1_OFFSET          0x34		// The address is from NIC config 0, not BBP register ID
+#define	EEPROM_NIC2_OFFSET          0x36		// The address is from NIC config 0, not BBP register ID
+#define	EEPROM_BBP_BASE_OFFSET		0xf0		// The address is from NIC config 0, not BBP register ID
+#define	EEPROM_G_TX_PWR_OFFSET		0x52
+#define	EEPROM_G_TX2_PWR_OFFSET		0x60
+#define EEPROM_LED1_OFFSET			0x3c
+#define EEPROM_LED2_OFFSET			0x3e
+#define EEPROM_LED3_OFFSET			0x40
+#define EEPROM_LNA_OFFSET			0x44
+#define EEPROM_RSSI_BG_OFFSET		0x46
+#define EEPROM_RSSI_A_OFFSET		0x4a
+#define EEPROM_DEFINE_MAX_TXPWR		0x4e
+#define EEPROM_TXPOWER_BYRATE_20MHZ_2_4G	0xde	// 20MHZ 2.4G tx power.
+#define EEPROM_TXPOWER_BYRATE_40MHZ_2_4G	0xee	// 40MHZ 2.4G tx power.
+#define EEPROM_TXPOWER_BYRATE_20MHZ_5G		0xfa	// 20MHZ 5G tx power.
+#define EEPROM_TXPOWER_BYRATE_40MHZ_5G		0x10a	// 40MHZ 5G tx power.
+#define EEPROM_A_TX_PWR_OFFSET      0x78
+#define EEPROM_A_TX2_PWR_OFFSET      0xa6
+#define EEPROM_VERSION_OFFSET       0x02
+#define	EEPROM_FREQ_OFFSET			0x3a
+#define EEPROM_TXPOWER_BYRATE 	0xde	// 20MHZ power.
+#define EEPROM_TXPOWER_DELTA		0x50	// 20MHZ AND 40 MHZ use different power. This is delta in 40MHZ.
+#define VALID_EEPROM_VERSION        1
+
+// PairKeyMode definition
+#define PKMODE_NONE                 0
+#define PKMODE_WEP64                1
+#define PKMODE_WEP128               2
+#define PKMODE_TKIP                 3
+#define PKMODE_AES                  4
+#define PKMODE_CKIP64               5
+#define PKMODE_CKIP128              6
+#define PKMODE_TKIP_NO_MIC          7       // MIC appended by driver: not a valid value in hardware key table
+
+// =================================================================================
+// WCID  format
+// =================================================================================
+//7.1	WCID  ENTRY  format  : 8bytes
+typedef	struct	_WCID_ENTRY_STRUC {
+	UCHAR		RXBABitmap7;    // bit0 for TID8, bit7 for TID 15
+	UCHAR		RXBABitmap0;    // bit0 for TID0, bit7 for TID 7
+	UCHAR		MAC[6];	// 0 for shared key table.  1 for pairwise key table
+}	WCID_ENTRY_STRUC, *PWCID_ENTRY_STRUC;
+
+//8.1.1	SECURITY  KEY  format  : 8DW
+// 32-byte per entry, total 16-entry for shared key table, 64-entry for pairwise key table
+typedef struct _HW_KEY_ENTRY {          // 32-byte per entry
+    UCHAR   Key[16];
+    UCHAR   TxMic[8];
+    UCHAR   RxMic[8];
+} HW_KEY_ENTRY, *PHW_KEY_ENTRY;
+
+//8.1.2	IV/EIV  format  : 2DW
+
+//8.1.3	RX attribute entry format  : 1DW
+#ifdef RT_BIG_ENDIAN
+typedef	struct	_MAC_ATTRIBUTE_STRUC {
+	UINT32		rsv:22;
+	UINT32		RXWIUDF:3;
+	UINT32		BSSIDIdx:3; //multipleBSS index for the WCID
+	UINT32		PairKeyMode:3;
+	UINT32		KeyTab:1;	// 0 for shared key table.  1 for pairwise key table
+}	MAC_ATTRIBUTE_STRUC, *PMAC_ATTRIBUTE_STRUC;
+#else
+typedef	struct	_MAC_ATTRIBUTE_STRUC {
+	UINT32		KeyTab:1;	// 0 for shared key table.  1 for pairwise key table
+	UINT32		PairKeyMode:3;
+	UINT32		BSSIDIdx:3; //multipleBSS index for the WCID
+	UINT32		RXWIUDF:3;
+	UINT32		rsv:22;
+}	MAC_ATTRIBUTE_STRUC, *PMAC_ATTRIBUTE_STRUC;
+#endif
+
+
+// =================================================================================
+// TX / RX ring descriptor format
+// =================================================================================
+
+// the first 24-byte in TXD is called TXINFO and will be DMAed to MAC block through TXFIFO.
+// MAC block use this TXINFO to control the transmission behavior of this frame.
+#define FIFO_MGMT                 0
+#define FIFO_HCCA                 1
+#define FIFO_EDCA                 2
+
+//
+// TX descriptor format, Tx	ring, Mgmt Ring
+//
+#ifdef RT_BIG_ENDIAN
+typedef	struct	PACKED _TXD_STRUC {
+	// Word 0
+	UINT32		SDPtr0;
+	// Word 1
+	UINT32		DMADONE:1;
+	UINT32		LastSec0:1;
+	UINT32		SDLen0:14;
+	UINT32		Burst:1;
+	UINT32		LastSec1:1;
+	UINT32		SDLen1:14;
+	// Word 2
+	UINT32		SDPtr1;
+	// Word 3
+	UINT32		ICO:1;
+	UINT32		UCO:1;
+	UINT32		TCO:1;
+	UINT32		rsv:2;
+	UINT32		QSEL:2;	// select on-chip FIFO ID for 2nd-stage output scheduler.0:MGMT, 1:HCCA 2:EDCA
+	UINT32		WIV:1;	// Wireless Info Valid. 1 if Driver already fill WI,  o if DMA needs to copy WI to correctposition
+	UINT32		rsv2:24;
+}	TXD_STRUC, *PTXD_STRUC;
+#else
+typedef	struct	PACKED _TXD_STRUC {
+	// Word	0
+	UINT32		SDPtr0;
+	// Word	1
+	UINT32		SDLen1:14;
+	UINT32		LastSec1:1;
+	UINT32		Burst:1;
+	UINT32		SDLen0:14;
+	UINT32		LastSec0:1;
+	UINT32		DMADONE:1;
+	//Word2
+	UINT32		SDPtr1;
+	//Word3
+	UINT32		rsv2:24;
+	UINT32		WIV:1;	// Wireless Info Valid. 1 if Driver already fill WI,  o if DMA needs to copy WI to correctposition
+	UINT32		QSEL:2;	// select on-chip FIFO ID for 2nd-stage output scheduler.0:MGMT, 1:HCCA 2:EDCA
+	UINT32		rsv:2;
+	UINT32		TCO:1;	//
+	UINT32		UCO:1;	//
+	UINT32		ICO:1;	//
+}	TXD_STRUC, *PTXD_STRUC;
+#endif
+
+
+//
+// TXD Wireless Information format for Tx ring and Mgmt Ring
+//
+//txop : for txop mode
+// 0:txop for the MPDU frame will be handles by ASIC by register
+// 1/2/3:the MPDU frame is send after PIFS/backoff/SIFS
+#ifdef RT_BIG_ENDIAN
+typedef	struct	PACKED _TXWI_STRUC {
+	// Word 0
+	UINT32		PHYMODE:2;
+	UINT32		TxBF:1;	// 3*3
+	UINT32		rsv2:1;
+	UINT32		Ifs:1;	//
+	UINT32		STBC:2;	//channel bandwidth 20MHz or 40 MHz
+	UINT32		ShortGI:1;
+	UINT32		BW:1;	//channel bandwidth 20MHz or 40 MHz
+	UINT32		MCS:7;
+
+	UINT32		rsv:6;
+	UINT32		txop:2;	//tx back off mode 0:HT TXOP rule , 1:PIFS TX ,2:Backoff, 3:sifs only when previous frame exchange is successful.
+	UINT32		MpduDensity:3;
+	UINT32		AMPDU:1;
+
+	UINT32		TS:1;
+	UINT32		CFACK:1;
+	UINT32		MIMOps:1;	// the remote peer is in dynamic MIMO-PS mode
+	UINT32		FRAG:1;		// 1 to inform TKIP engine this is a fragment.
+	// Word 1
+	UINT32		PacketId:4;
+	UINT32		MPDUtotalByteCount:12;
+	UINT32		WirelessCliID:8;
+	UINT32		BAWinSize:6;
+	UINT32		NSEQ:1;
+	UINT32		ACK:1;
+	// Word 2
+	UINT32		IV;
+	// Word 3
+	UINT32		EIV;
+}	TXWI_STRUC, *PTXWI_STRUC;
+#else
+typedef	struct	PACKED _TXWI_STRUC {
+	// Word	0
+	UINT32		FRAG:1;		// 1 to inform TKIP engine this is a fragment.
+	UINT32		MIMOps:1;	// the remote peer is in dynamic MIMO-PS mode
+	UINT32		CFACK:1;
+	UINT32		TS:1;
+
+	UINT32		AMPDU:1;
+	UINT32		MpduDensity:3;
+	UINT32		txop:2;	//FOR "THIS" frame. 0:HT TXOP rule , 1:PIFS TX ,2:Backoff, 3:sifs only when previous frame exchange is successful.
+	UINT32		rsv:6;
+
+	UINT32		MCS:7;
+	UINT32		BW:1;	//channel bandwidth 20MHz or 40 MHz
+	UINT32		ShortGI:1;
+	UINT32		STBC:2;	// 1: STBC support MCS =0-7,   2,3 : RESERVE
+	UINT32		Ifs:1;	//
+	UINT32		rsv2:1;
+	UINT32		TxBF:1;	// 3*3
+	UINT32		PHYMODE:2;
+	// Word	1
+	UINT32		ACK:1;
+	UINT32		NSEQ:1;
+	UINT32		BAWinSize:6;
+	UINT32		WirelessCliID:8;
+	UINT32		MPDUtotalByteCount:12;
+	UINT32		PacketId:4;
+	//Word2
+	UINT32		IV;
+	//Word3
+	UINT32		EIV;
+}	TXWI_STRUC, *PTXWI_STRUC;
+#endif
+//
+// Rx descriptor format, Rx	Ring
+//
+#ifdef RT2860
+#ifdef RT_BIG_ENDIAN
+typedef	struct	PACKED _RXD_STRUC	{
+	// Word 0
+	UINT32		SDP0;
+	// Word 1
+	UINT32		DDONE:1;
+	UINT32		LS0:1;
+	UINT32		SDL0:14;
+	UINT32		Rsv:2;
+	UINT32		SDL1:14;
+	// Word 2
+	UINT32		SDP1;
+	// Word 3
+	UINT32		Rsv1:13;
+	UINT32		PlcpRssil:1;// To be moved
+	UINT32		PlcpSignal:1;		// To be moved
+	UINT32		Decrypted:1;	// this frame is being decrypted.
+	UINT32		AMPDU:1;
+	UINT32		L2PAD:1;
+	UINT32		RSSI:1;
+	UINT32		HTC:1;
+	UINT32		AMSDU:1;		// rx with 802.3 header, not 802.11 header. obsolete.
+	UINT32		CipherErr:2;        // 0: decryption okay, 1:ICV error, 2:MIC error, 3:KEY not valid
+	UINT32		Crc:1;              // 1: CRC error
+	UINT32		MyBss:1;  	// 1: this frame belongs to the same BSSID
+	UINT32		Bcast:1;            // 1: this is a broadcast frame
+	UINT32		Mcast:1;            // 1: this is a multicast frame
+	UINT32		U2M:1;              // 1: this RX frame is unicast to me
+	UINT32		FRAG:1;
+	UINT32		NULLDATA:1;
+	UINT32		DATA:1;
+	UINT32		BA:1;
+
+}	RXD_STRUC, *PRXD_STRUC, RT28XX_RXD_STRUC, *PRT28XX_RXD_STRUC;
+#else
+typedef	struct	PACKED _RXD_STRUC	{
+	// Word	0
+	UINT32		SDP0;
+	// Word	1
+	UINT32		SDL1:14;
+	UINT32		Rsv:2;
+	UINT32		SDL0:14;
+	UINT32		LS0:1;
+	UINT32		DDONE:1;
+	// Word	2
+	UINT32		SDP1;
+	// Word	3
+	UINT32		BA:1;
+	UINT32		DATA:1;
+	UINT32		NULLDATA:1;
+	UINT32		FRAG:1;
+	UINT32		U2M:1;              // 1: this RX frame is unicast to me
+	UINT32		Mcast:1;            // 1: this is a multicast frame
+	UINT32		Bcast:1;            // 1: this is a broadcast frame
+	UINT32		MyBss:1;  	// 1: this frame belongs to the same BSSID
+	UINT32		Crc:1;              // 1: CRC error
+	UINT32		CipherErr:2;        // 0: decryption okay, 1:ICV error, 2:MIC error, 3:KEY not valid
+	UINT32		AMSDU:1;		// rx with 802.3 header, not 802.11 header.
+	UINT32		HTC:1;
+	UINT32		RSSI:1;
+	UINT32		L2PAD:1;
+	UINT32		AMPDU:1;
+	UINT32		Decrypted:1;	// this frame is being decrypted.
+	UINT32		PlcpSignal:1;		// To be moved
+	UINT32		PlcpRssil:1;// To be moved
+	UINT32		Rsv1:13;
+}	RXD_STRUC, *PRXD_STRUC, RT28XX_RXD_STRUC, *PRT28XX_RXD_STRUC;
+#endif
+#endif // RT2860 //
+//
+// RXWI wireless information format, in PBF. invisible in driver.
+//
+#ifdef RT_BIG_ENDIAN
+typedef	struct	PACKED _RXWI_STRUC {
+	// Word 0
+	UINT32		TID:4;
+	UINT32		MPDUtotalByteCount:12;
+	UINT32		UDF:3;
+	UINT32		BSSID:3;
+	UINT32		KeyIndex:2;
+	UINT32		WirelessCliID:8;
+	// Word 1
+	UINT32		PHYMODE:2;              // 1: this RX frame is unicast to me
+	UINT32		rsv:3;
+	UINT32		STBC:2;
+	UINT32		ShortGI:1;
+	UINT32		BW:1;
+	UINT32		MCS:7;
+	UINT32		SEQUENCE:12;
+	UINT32		FRAG:4;
+	// Word 2
+	UINT32		rsv1:8;
+	UINT32		RSSI2:8;
+	UINT32		RSSI1:8;
+	UINT32		RSSI0:8;
+	// Word 3
+	UINT32		rsv2:16;
+	UINT32		SNR1:8;
+	UINT32		SNR0:8;
+}	RXWI_STRUC, *PRXWI_STRUC;
+#else
+typedef	struct	PACKED _RXWI_STRUC {
+	// Word	0
+	UINT32		WirelessCliID:8;
+	UINT32		KeyIndex:2;
+	UINT32		BSSID:3;
+	UINT32		UDF:3;
+	UINT32		MPDUtotalByteCount:12;
+	UINT32		TID:4;
+	// Word	1
+	UINT32		FRAG:4;
+	UINT32		SEQUENCE:12;
+	UINT32		MCS:7;
+	UINT32		BW:1;
+	UINT32		ShortGI:1;
+	UINT32		STBC:2;
+	UINT32		rsv:3;
+	UINT32		PHYMODE:2;              // 1: this RX frame is unicast to me
+	//Word2
+	UINT32		RSSI0:8;
+	UINT32		RSSI1:8;
+	UINT32		RSSI2:8;
+	UINT32		rsv1:8;
+	//Word3
+	UINT32		SNR0:8;
+	UINT32		SNR1:8;
+	UINT32		rsv2:16;
+}	RXWI_STRUC, *PRXWI_STRUC;
+#endif
+
+
+// =================================================================================
+// HOST-MCU communication data structure
+// =================================================================================
+
+//
+// H2M_MAILBOX_CSR: Host-to-MCU Mailbox
+//
+#ifdef RT_BIG_ENDIAN
+typedef union  _H2M_MAILBOX_STRUC {
+    struct {
+        UINT32       Owner:8;
+        UINT32       CmdToken:8;    // 0xff tells MCU not to report CmdDoneInt after excuting the command
+        UINT32       HighByte:8;
+        UINT32       LowByte:8;
+    }   field;
+    UINT32           word;
+} H2M_MAILBOX_STRUC, *PH2M_MAILBOX_STRUC;
+#else
+typedef union  _H2M_MAILBOX_STRUC {
+    struct {
+        UINT32       LowByte:8;
+        UINT32       HighByte:8;
+        UINT32       CmdToken:8;
+        UINT32       Owner:8;
+    }   field;
+    UINT32           word;
+} H2M_MAILBOX_STRUC, *PH2M_MAILBOX_STRUC;
+#endif
+
+//
+// M2H_CMD_DONE_CSR: MCU-to-Host command complete indication
+//
+#ifdef RT_BIG_ENDIAN
+typedef union _M2H_CMD_DONE_STRUC {
+    struct  {
+        UINT32       CmdToken3;
+        UINT32       CmdToken2;
+        UINT32       CmdToken1;
+        UINT32       CmdToken0;
+    } field;
+    UINT32           word;
+} M2H_CMD_DONE_STRUC, *PM2H_CMD_DONE_STRUC;
+#else
+typedef union _M2H_CMD_DONE_STRUC {
+    struct  {
+        UINT32       CmdToken0;
+        UINT32       CmdToken1;
+        UINT32       CmdToken2;
+        UINT32       CmdToken3;
+    } field;
+    UINT32           word;
+} M2H_CMD_DONE_STRUC, *PM2H_CMD_DONE_STRUC;
+#endif
+
+
+
+//
+// MCU_LEDCS: MCU LED Control Setting.
+//
+#ifdef RT_BIG_ENDIAN
+typedef union  _MCU_LEDCS_STRUC {
+	struct	{
+		UCHAR		Polarity:1;
+		UCHAR		LedMode:7;
+	} field;
+	UCHAR				word;
+} MCU_LEDCS_STRUC, *PMCU_LEDCS_STRUC;
+#else
+typedef union  _MCU_LEDCS_STRUC {
+	struct	{
+		UCHAR		LedMode:7;
+		UCHAR		Polarity:1;
+	} field;
+	UCHAR			word;
+} MCU_LEDCS_STRUC, *PMCU_LEDCS_STRUC;
+#endif
+// =================================================================================
+// Register format
+// =================================================================================
+
+
+
+//NAV_TIME_CFG :NAV
+#ifdef RT_BIG_ENDIAN
+typedef	union	_NAV_TIME_CFG_STRUC	{
+	struct	{
+		USHORT		rsv:6;
+		USHORT		ZeroSifs:1;               // Applied zero SIFS timer after OFDM RX 0: disable
+		USHORT		Eifs:9;               // in unit of 1-us
+		UCHAR       SlotTime;    // in unit of 1-us
+		UCHAR		Sifs;               // in unit of 1-us
+	}	field;
+	UINT32			word;
+}	NAV_TIME_CFG_STRUC, *PNAV_TIME_CFG_STRUC;
+#else
+typedef	union	_NAV_TIME_CFG_STRUC	{
+	struct	{
+		UCHAR		Sifs;               // in unit of 1-us
+		UCHAR       SlotTime;    // in unit of 1-us
+		USHORT		Eifs:9;               // in unit of 1-us
+		USHORT		ZeroSifs:1;               // Applied zero SIFS timer after OFDM RX 0: disable
+		USHORT		rsv:6;
+	}	field;
+	UINT32			word;
+}	NAV_TIME_CFG_STRUC, *PNAV_TIME_CFG_STRUC;
+#endif
+
+
+
+
+
+//
+// RX_FILTR_CFG:  /RX configuration register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	RX_FILTR_CFG_STRUC	{
+	struct	{
+		UINT32		:15;
+		UINT32       DropRsvCntlType:1;
+
+        	UINT32       	DropBAR:1;       //
+		UINT32		DropBA:1;		//
+		UINT32		DropPsPoll:1;		// Drop Ps-Poll
+		UINT32		DropRts:1;		// Drop Ps-Poll
+
+		UINT32		DropCts:1;		// Drop Ps-Poll
+		UINT32		DropAck:1;		// Drop Ps-Poll
+		UINT32		DropCFEnd:1;		// Drop Ps-Poll
+		UINT32		DropCFEndAck:1;		// Drop Ps-Poll
+
+		UINT32		DropDuplicate:1;		// Drop duplicate frame
+		UINT32		DropBcast:1;		// Drop broadcast frames
+		UINT32		DropMcast:1;		// Drop multicast frames
+		UINT32		DropVerErr:1;	    // Drop version error frame
+
+		UINT32		DropNotMyBSSID:1;			// Drop fram ToDs bit is true
+		UINT32		DropNotToMe:1;		// Drop not to me unicast frame
+		UINT32		DropPhyErr:1;		// Drop physical error
+		UINT32		DropCRCErr:1;		// Drop CRC error
+	}	field;
+	UINT32			word;
+}	RX_FILTR_CFG_STRUC, *PRX_FILTR_CFG_STRUC;
+#else
+typedef	union	_RX_FILTR_CFG_STRUC	{
+	struct	{
+		UINT32		DropCRCErr:1;		// Drop CRC error
+		UINT32		DropPhyErr:1;		// Drop physical error
+		UINT32		DropNotToMe:1;		// Drop not to me unicast frame
+		UINT32		DropNotMyBSSID:1;			// Drop fram ToDs bit is true
+
+		UINT32		DropVerErr:1;	    // Drop version error frame
+		UINT32		DropMcast:1;		// Drop multicast frames
+		UINT32		DropBcast:1;		// Drop broadcast frames
+		UINT32		DropDuplicate:1;		// Drop duplicate frame
+
+		UINT32		DropCFEndAck:1;		// Drop Ps-Poll
+		UINT32		DropCFEnd:1;		// Drop Ps-Poll
+		UINT32		DropAck:1;		// Drop Ps-Poll
+		UINT32		DropCts:1;		// Drop Ps-Poll
+
+		UINT32		DropRts:1;		// Drop Ps-Poll
+		UINT32		DropPsPoll:1;		// Drop Ps-Poll
+		UINT32		DropBA:1;		//
+        	UINT32       	DropBAR:1;       //
+
+		UINT32       	DropRsvCntlType:1;
+		UINT32		:15;
+	}	field;
+	UINT32			word;
+}	RX_FILTR_CFG_STRUC, *PRX_FILTR_CFG_STRUC;
+#endif
+
+
+
+
+//
+// PHY_CSR4: RF serial control register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_PHY_CSR4_STRUC	{
+	struct	{
+		UINT32		Busy:1;				// 1: ASIC is busy execute RF programming.
+		UINT32		PLL_LD:1;			// RF PLL_LD status
+		UINT32		IFSelect:1;			// 1: select IF	to program,	0: select RF to	program
+		UINT32		NumberOfBits:5;		// Number of bits used in RFRegValue (I:20,	RFMD:22)
+		UINT32		RFRegValue:24;		// Register	value (include register	id)	serial out to RF/IF	chip.
+	}	field;
+	UINT32			word;
+}	PHY_CSR4_STRUC, *PPHY_CSR4_STRUC;
+#else
+typedef	union	_PHY_CSR4_STRUC	{
+	struct	{
+		UINT32		RFRegValue:24;		// Register	value (include register	id)	serial out to RF/IF	chip.
+		UINT32		NumberOfBits:5;		// Number of bits used in RFRegValue (I:20,	RFMD:22)
+		UINT32		IFSelect:1;			// 1: select IF	to program,	0: select RF to	program
+		UINT32		PLL_LD:1;			// RF PLL_LD status
+		UINT32		Busy:1;				// 1: ASIC is busy execute RF programming.
+	}	field;
+	UINT32			word;
+}	PHY_CSR4_STRUC, *PPHY_CSR4_STRUC;
+#endif
+
+
+//
+// SEC_CSR5: shared key table security mode register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_SEC_CSR5_STRUC	{
+	struct	{
+        UINT32       :1;
+        UINT32       Bss3Key3CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss3Key2CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss3Key1CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss3Key0CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss2Key3CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss2Key2CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss2Key1CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss2Key0CipherAlg:3;
+	}	field;
+	UINT32			word;
+}	SEC_CSR5_STRUC, *PSEC_CSR5_STRUC;
+#else
+typedef	union	_SEC_CSR5_STRUC	{
+	struct	{
+        UINT32       Bss2Key0CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss2Key1CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss2Key2CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss2Key3CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss3Key0CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss3Key1CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss3Key2CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss3Key3CipherAlg:3;
+        UINT32       :1;
+	}	field;
+	UINT32			word;
+}	SEC_CSR5_STRUC, *PSEC_CSR5_STRUC;
+#endif
+
+
+//
+// HOST_CMD_CSR: For HOST to interrupt embedded processor
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_HOST_CMD_CSR_STRUC	{
+	struct	{
+	    UINT32   Rsv:24;
+	    UINT32   HostCommand:8;
+	}	field;
+	UINT32			word;
+}	HOST_CMD_CSR_STRUC, *PHOST_CMD_CSR_STRUC;
+#else
+typedef	union	_HOST_CMD_CSR_STRUC	{
+	struct	{
+	    UINT32   HostCommand:8;
+	    UINT32   Rsv:24;
+	}	field;
+	UINT32			word;
+}	HOST_CMD_CSR_STRUC, *PHOST_CMD_CSR_STRUC;
+#endif
+
+
+//
+// AIFSN_CSR: AIFSN for each EDCA AC
+//
+
+
+
+//
+// E2PROM_CSR: EEPROM control register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_E2PROM_CSR_STRUC	{
+	struct	{
+		UINT32		Rsvd:25;
+		UINT32       LoadStatus:1;   // 1:loading, 0:done
+		UINT32		Type:1;			// 1: 93C46, 0:93C66
+		UINT32		EepromDO:1;
+		UINT32		EepromDI:1;
+		UINT32		EepromCS:1;
+		UINT32		EepromSK:1;
+		UINT32		Reload:1;		// Reload EEPROM content, write one to reload, self-cleared.
+	}	field;
+	UINT32			word;
+}	E2PROM_CSR_STRUC, *PE2PROM_CSR_STRUC;
+#else
+typedef	union	_E2PROM_CSR_STRUC	{
+	struct	{
+		UINT32		Reload:1;		// Reload EEPROM content, write one to reload, self-cleared.
+		UINT32		EepromSK:1;
+		UINT32		EepromCS:1;
+		UINT32		EepromDI:1;
+		UINT32		EepromDO:1;
+		UINT32		Type:1;			// 1: 93C46, 0:93C66
+		UINT32       LoadStatus:1;   // 1:loading, 0:done
+		UINT32		Rsvd:25;
+	}	field;
+	UINT32			word;
+}	E2PROM_CSR_STRUC, *PE2PROM_CSR_STRUC;
+#endif
+
+
+// -------------------------------------------------------------------
+//  E2PROM data layout
+// -------------------------------------------------------------------
+
+//
+// EEPROM antenna select format
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_EEPROM_ANTENNA_STRUC	{
+	struct	{
+		USHORT      Rsv:4;
+		USHORT      RfIcType:4;             // see E2PROM document
+		USHORT		TxPath:4;	// 1: 1T, 2: 2T
+		USHORT		RxPath:4;	// 1: 1R, 2: 2R, 3: 3R
+	}	field;
+	USHORT			word;
+}	EEPROM_ANTENNA_STRUC, *PEEPROM_ANTENNA_STRUC;
+#else
+typedef	union	_EEPROM_ANTENNA_STRUC	{
+	struct	{
+		USHORT		RxPath:4;	// 1: 1R, 2: 2R, 3: 3R
+		USHORT		TxPath:4;	// 1: 1T, 2: 2T
+		USHORT      RfIcType:4;             // see E2PROM document
+		USHORT      Rsv:4;
+	}	field;
+	USHORT			word;
+}	EEPROM_ANTENNA_STRUC, *PEEPROM_ANTENNA_STRUC;
+#endif
+
+#ifdef RT_BIG_ENDIAN
+typedef	union _EEPROM_NIC_CINFIG2_STRUC	{
+	struct	{
+        USHORT		Rsv2:6;					// must be 0
+		USHORT		BW40MAvailForA:1;			// 0:enable, 1:disable
+		USHORT		BW40MAvailForG:1;			// 0:enable, 1:disable
+		USHORT		EnableWPSPBC:1;                 // WPS PBC Control bit
+		USHORT		BW40MSidebandForA:1;
+		USHORT		BW40MSidebandForG:1;
+		USHORT		CardbusAcceleration:1;	// !!! NOTE: 0 - enable, 1 - disable
+		USHORT		ExternalLNAForA:1;			// external LNA enable for 5G
+		USHORT		ExternalLNAForG:1;			// external LNA enable for 2.4G
+		USHORT		DynamicTxAgcControl:1;			//
+		USHORT		HardwareRadioControl:1;	// Whether RF is controlled by driver or HW. 1:enable hw control, 0:disable
+	}	field;
+	USHORT			word;
+}	EEPROM_NIC_CONFIG2_STRUC, *PEEPROM_NIC_CONFIG2_STRUC;
+#else
+typedef	union _EEPROM_NIC_CINFIG2_STRUC	{
+	struct {
+		USHORT		HardwareRadioControl:1;	// 1:enable, 0:disable
+		USHORT		DynamicTxAgcControl:1;			//
+		USHORT		ExternalLNAForG:1;				//
+		USHORT		ExternalLNAForA:1;			// external LNA enable for 2.4G
+		USHORT		CardbusAcceleration:1;	// !!! NOTE: 0 - enable, 1 - disable
+		USHORT		BW40MSidebandForG:1;
+		USHORT		BW40MSidebandForA:1;
+		USHORT		EnableWPSPBC:1;                 // WPS PBC Control bit
+		USHORT		BW40MAvailForG:1;			// 0:enable, 1:disable
+		USHORT		BW40MAvailForA:1;			// 0:enable, 1:disable
+		USHORT		Rsv2:6;                 // must be 0
+	}	field;
+	USHORT			word;
+}	EEPROM_NIC_CONFIG2_STRUC, *PEEPROM_NIC_CONFIG2_STRUC;
+#endif
+
+//
+// TX_PWR Value valid range 0xFA(-6) ~ 0x24(36)
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_EEPROM_TX_PWR_STRUC	{
+	struct	{
+		CHAR	Byte1;				// High Byte
+		CHAR	Byte0;				// Low Byte
+	}	field;
+	USHORT	word;
+}	EEPROM_TX_PWR_STRUC, *PEEPROM_TX_PWR_STRUC;
+#else
+typedef	union	_EEPROM_TX_PWR_STRUC	{
+	struct	{
+		CHAR	Byte0;				// Low Byte
+		CHAR	Byte1;				// High Byte
+	}	field;
+	USHORT	word;
+}	EEPROM_TX_PWR_STRUC, *PEEPROM_TX_PWR_STRUC;
+#endif
+
+#ifdef RT_BIG_ENDIAN
+typedef	union	_EEPROM_VERSION_STRUC	{
+	struct	{
+		UCHAR	Version;			// High Byte
+		UCHAR	FaeReleaseNumber;	// Low Byte
+	}	field;
+	USHORT	word;
+}	EEPROM_VERSION_STRUC, *PEEPROM_VERSION_STRUC;
+#else
+typedef	union	_EEPROM_VERSION_STRUC	{
+	struct	{
+		UCHAR	FaeReleaseNumber;	// Low Byte
+		UCHAR	Version;			// High Byte
+	}	field;
+	USHORT	word;
+}	EEPROM_VERSION_STRUC, *PEEPROM_VERSION_STRUC;
+#endif
+
+#ifdef RT_BIG_ENDIAN
+typedef	union	_EEPROM_LED_STRUC	{
+	struct	{
+		USHORT	Rsvd:3;				// Reserved
+		USHORT	LedMode:5;			// Led mode.
+		USHORT	PolarityGPIO_4:1;	// Polarity GPIO#4 setting.
+		USHORT	PolarityGPIO_3:1;	// Polarity GPIO#3 setting.
+		USHORT	PolarityGPIO_2:1;	// Polarity GPIO#2 setting.
+		USHORT	PolarityGPIO_1:1;	// Polarity GPIO#1 setting.
+		USHORT	PolarityGPIO_0:1;	// Polarity GPIO#0 setting.
+		USHORT	PolarityACT:1;		// Polarity ACT setting.
+		USHORT	PolarityRDY_A:1;		// Polarity RDY_A setting.
+		USHORT	PolarityRDY_G:1;		// Polarity RDY_G setting.
+	}	field;
+	USHORT	word;
+}	EEPROM_LED_STRUC, *PEEPROM_LED_STRUC;
+#else
+typedef	union	_EEPROM_LED_STRUC	{
+	struct	{
+		USHORT	PolarityRDY_G:1;		// Polarity RDY_G setting.
+		USHORT	PolarityRDY_A:1;		// Polarity RDY_A setting.
+		USHORT	PolarityACT:1;		// Polarity ACT setting.
+		USHORT	PolarityGPIO_0:1;	// Polarity GPIO#0 setting.
+		USHORT	PolarityGPIO_1:1;	// Polarity GPIO#1 setting.
+		USHORT	PolarityGPIO_2:1;	// Polarity GPIO#2 setting.
+		USHORT	PolarityGPIO_3:1;	// Polarity GPIO#3 setting.
+		USHORT	PolarityGPIO_4:1;	// Polarity GPIO#4 setting.
+		USHORT	LedMode:5;			// Led mode.
+		USHORT	Rsvd:3;				// Reserved
+	}	field;
+	USHORT	word;
+}	EEPROM_LED_STRUC, *PEEPROM_LED_STRUC;
+#endif
+
+#ifdef RT_BIG_ENDIAN
+typedef	union	_EEPROM_TXPOWER_DELTA_STRUC	{
+	struct	{
+		UCHAR	TxPowerEnable:1;// Enable
+		UCHAR	Type:1;			// 1: plus the delta value, 0: minus the delta value
+		UCHAR	DeltaValue:6;	// Tx Power dalta value (MAX=4)
+	}	field;
+	UCHAR	value;
+}	EEPROM_TXPOWER_DELTA_STRUC, *PEEPROM_TXPOWER_DELTA_STRUC;
+#else
+typedef	union	_EEPROM_TXPOWER_DELTA_STRUC	{
+	struct	{
+		UCHAR	DeltaValue:6;	// Tx Power dalta value (MAX=4)
+		UCHAR	Type:1;			// 1: plus the delta value, 0: minus the delta value
+		UCHAR	TxPowerEnable:1;// Enable
+	}	field;
+	UCHAR	value;
+}	EEPROM_TXPOWER_DELTA_STRUC, *PEEPROM_TXPOWER_DELTA_STRUC;
+#endif
+
+//
+// QOS_CSR0: TXOP holder address0 register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_QOS_CSR0_STRUC	{
+	struct	{
+		UCHAR		Byte3;		// MAC address byte 3
+		UCHAR		Byte2;		// MAC address byte 2
+		UCHAR		Byte1;		// MAC address byte 1
+		UCHAR		Byte0;		// MAC address byte 0
+	}	field;
+	UINT32			word;
+}	QOS_CSR0_STRUC, *PQOS_CSR0_STRUC;
+#else
+typedef	union	_QOS_CSR0_STRUC	{
+	struct	{
+		UCHAR		Byte0;		// MAC address byte 0
+		UCHAR		Byte1;		// MAC address byte 1
+		UCHAR		Byte2;		// MAC address byte 2
+		UCHAR		Byte3;		// MAC address byte 3
+	}	field;
+	UINT32			word;
+}	QOS_CSR0_STRUC, *PQOS_CSR0_STRUC;
+#endif
+
+//
+// QOS_CSR1: TXOP holder address1 register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_QOS_CSR1_STRUC	{
+	struct	{
+		UCHAR		Rsvd1;
+		UCHAR		Rsvd0;
+		UCHAR		Byte5;		// MAC address byte 5
+		UCHAR		Byte4;		// MAC address byte 4
+	}	field;
+	UINT32			word;
+}	QOS_CSR1_STRUC, *PQOS_CSR1_STRUC;
+#else
+typedef	union	_QOS_CSR1_STRUC	{
+	struct	{
+		UCHAR		Byte4;		// MAC address byte 4
+		UCHAR		Byte5;		// MAC address byte 5
+		UCHAR		Rsvd0;
+		UCHAR		Rsvd1;
+	}	field;
+	UINT32			word;
+}	QOS_CSR1_STRUC, *PQOS_CSR1_STRUC;
+#endif
+
+#define	RF_CSR_CFG	0x500
+#ifdef RT_BIG_ENDIAN
+typedef	union	_RF_CSR_CFG_STRUC	{
+	struct	{
+		UINT	Rsvd1:14;				// Reserved
+		UINT	RF_CSR_KICK:1;			// kick RF register read/write
+		UINT	RF_CSR_WR:1;			// 0: read  1: write
+		UINT	Rsvd2:3;				// Reserved
+		UINT	TESTCSR_RFACC_REGNUM:5;	// RF register ID
+		UINT	RF_CSR_DATA:8;			// DATA
+	}	field;
+	UINT	word;
+}	RF_CSR_CFG_STRUC, *PRF_CSR_CFG_STRUC;
+#else
+typedef	union	_RF_CSR_CFG_STRUC	{
+	struct	{
+		UINT	RF_CSR_DATA:8;			// DATA
+		UINT	TESTCSR_RFACC_REGNUM:5;	// RF register ID
+		UINT	Rsvd2:3;				// Reserved
+		UINT	RF_CSR_WR:1;			// 0: read  1: write
+		UINT	RF_CSR_KICK:1;			// kick RF register read/write
+		UINT	Rsvd1:14;				// Reserved
+	}	field;
+	UINT	word;
+}	RF_CSR_CFG_STRUC, *PRF_CSR_CFG_STRUC;
+#endif
+
+#endif	// __RT28XX_H__
diff --git a/drivers/staging/rt2860/rt_ate.c b/drivers/staging/rt2860/rt_ate.c
new file mode 100644
index 00000000000..2f07db565c3
--- /dev/null
+++ b/drivers/staging/rt2860/rt_ate.c
@@ -0,0 +1,6025 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#include "rt_config.h"
+
+#ifdef RALINK_ATE
+UCHAR TemplateFrame[24] = {0x08/* Data type */,0x00,0x00,0x00,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x00,0xAA,0xBB,0x12,0x34,0x56,0x00,0x11,0x22,0xAA,0xBB,0xCC,0x00,0x00};	// 802.11 MAC Header, Type:Data, Length:24bytes
+extern RTMP_RF_REGS RF2850RegTable[];
+extern UCHAR NUM_OF_2850_CHNL;
+
+static CHAR CCKRateTable[] = {0, 1, 2, 3, 8, 9, 10, 11, -1}; /* CCK Mode. */
+static CHAR OFDMRateTable[] = {0, 1, 2, 3, 4, 5, 6, 7, -1}; /* OFDM Mode. */
+static CHAR HTMIXRateTable[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, -1}; /* HT Mix Mode. */
+
+static INT TxDmaBusy(
+	IN PRTMP_ADAPTER pAd);
+
+static INT RxDmaBusy(
+	IN PRTMP_ADAPTER pAd);
+
+static VOID RtmpDmaEnable(
+	IN PRTMP_ADAPTER pAd,
+	IN INT Enable);
+
+static VOID BbpSoftReset(
+	IN PRTMP_ADAPTER pAd);
+
+static VOID RtmpRfIoWrite(
+	IN PRTMP_ADAPTER pAd);
+
+static INT ATESetUpFrame(
+	IN PRTMP_ADAPTER pAd,
+	IN UINT32 TxIdx);
+
+static INT ATETxPwrHandler(
+	IN PRTMP_ADAPTER pAd,
+	IN char index);
+
+static INT ATECmdHandler(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+static int CheckMCSValid(
+	IN UCHAR Mode,
+	IN UCHAR Mcs);
+
+#ifdef RT2860
+static VOID ATEWriteTxWI(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTXWI_STRUC 	pOutTxWI,
+	IN	BOOLEAN			FRAG,
+	IN	BOOLEAN			CFACK,
+	IN	BOOLEAN			InsTimestamp,
+	IN	BOOLEAN 		AMPDU,
+	IN	BOOLEAN 		Ack,
+	IN	BOOLEAN 		NSeq,		// HW new a sequence.
+	IN	UCHAR			BASize,
+	IN	UCHAR			WCID,
+	IN	ULONG			Length,
+	IN	UCHAR 			PID,
+	IN	UCHAR			TID,
+	IN	UCHAR			TxRate,
+	IN	UCHAR			Txopmode,
+	IN	BOOLEAN			CfAck,
+	IN	HTTRANSMIT_SETTING	*pTransmit);
+#endif // RT2860 //
+
+
+static VOID SetJapanFilter(
+	IN	PRTMP_ADAPTER	pAd);
+
+/*=========================end of prototype=========================*/
+
+#ifdef RT2860
+static INT TxDmaBusy(
+	IN PRTMP_ADAPTER pAd)
+{
+	INT result;
+	WPDMA_GLO_CFG_STRUC GloCfg;
+
+	RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &GloCfg.word);	// disable DMA
+	if (GloCfg.field.TxDMABusy)
+		result = 1;
+	else
+		result = 0;
+
+	return result;
+}
+
+static INT RxDmaBusy(
+	IN PRTMP_ADAPTER pAd)
+{
+	INT result;
+	WPDMA_GLO_CFG_STRUC GloCfg;
+
+	RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &GloCfg.word);	// disable DMA
+	if (GloCfg.field.RxDMABusy)
+		result = 1;
+	else
+		result = 0;
+
+	return result;
+}
+
+static VOID RtmpDmaEnable(
+	IN PRTMP_ADAPTER pAd,
+	IN INT Enable)
+{
+	BOOLEAN value;
+	ULONG WaitCnt;
+	WPDMA_GLO_CFG_STRUC GloCfg;
+
+	value = Enable > 0 ? 1 : 0;
+
+	// check DMA is in busy mode.
+	WaitCnt = 0;
+	while (TxDmaBusy(pAd) || RxDmaBusy(pAd))
+	{
+		RTMPusecDelay(10);
+		if (WaitCnt++ > 100)
+			break;
+	}
+
+	RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &GloCfg.word);	// disable DMA
+	GloCfg.field.EnableTxDMA = value;
+	GloCfg.field.EnableRxDMA = value;
+	RTMP_IO_WRITE32(pAd, WPDMA_GLO_CFG, GloCfg.word);	// abort all TX rings
+	RTMPusecDelay(5000);
+
+	return;
+}
+#endif // RT2860 //
+
+
+static VOID BbpSoftReset(
+	IN PRTMP_ADAPTER pAd)
+{
+	UCHAR BbpData = 0;
+
+	// Soft reset, set BBP R21 bit0=1->0
+	ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R21, &BbpData);
+	BbpData |= 0x00000001; //set bit0=1
+	ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R21, BbpData);
+
+	ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R21, &BbpData);
+	BbpData &= ~(0x00000001); //set bit0=0
+	ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R21, BbpData);
+
+	return;
+}
+
+static VOID RtmpRfIoWrite(
+	IN PRTMP_ADAPTER pAd)
+{
+	// Set RF value 1's set R3[bit2] = [0]
+	RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R1);
+	RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R2);
+	RTMP_RF_IO_WRITE32(pAd, (pAd->LatchRfRegs.R3 & (~0x04)));
+	RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R4);
+
+	RTMPusecDelay(200);
+
+	// Set RF value 2's set R3[bit2] = [1]
+	RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R1);
+	RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R2);
+	RTMP_RF_IO_WRITE32(pAd, (pAd->LatchRfRegs.R3 | 0x04));
+	RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R4);
+
+	RTMPusecDelay(200);
+
+	// Set RF value 3's set R3[bit2] = [0]
+	RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R1);
+	RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R2);
+	RTMP_RF_IO_WRITE32(pAd, (pAd->LatchRfRegs.R3 & (~0x04)));
+	RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R4);
+
+	return;
+}
+
+static int CheckMCSValid(
+	UCHAR Mode,
+	UCHAR Mcs)
+{
+	int i;
+	PCHAR pRateTab;
+
+	switch(Mode)
+	{
+		case 0:
+			pRateTab = CCKRateTable;
+			break;
+		case 1:
+			pRateTab = OFDMRateTable;
+			break;
+		case 2:
+		case 3:
+			pRateTab = HTMIXRateTable;
+			break;
+		default:
+			ATEDBGPRINT(RT_DEBUG_ERROR, ("unrecognizable Tx Mode %d\n", Mode));
+			return -1;
+			break;
+	}
+
+	i = 0;
+	while(pRateTab[i] != -1)
+	{
+		if (pRateTab[i] == Mcs)
+			return 0;
+		i++;
+	}
+
+	return -1;
+}
+
+#if 1
+static INT ATETxPwrHandler(
+	IN PRTMP_ADAPTER pAd,
+	IN char index)
+{
+	ULONG R;
+	CHAR TxPower;
+	UCHAR Bbp94 = 0;
+	BOOLEAN bPowerReduce = FALSE;
+
+#ifdef RALINK_28xx_QA
+	if ((pAd->ate.bQATxStart == TRUE) || (pAd->ate.bQARxStart == TRUE))
+	{
+		/* When QA is used for Tx, pAd->ate.TxPower0/1 and real tx power
+		** are not synchronized.
+		*/
+/*
+		pAd->ate.TxPower0 = pAd->LatchRfRegs.xxx;
+		pAd->ate.TxPower1 = pAd->LatchRfRegs.xxx;
+*/
+		return 0;
+	}
+	else
+#endif // RALINK_28xx_QA //
+	{
+		TxPower = index == 0 ? pAd->ate.TxPower0 : pAd->ate.TxPower1;
+
+		if (pAd->ate.Channel <= 14)
+		{
+			if (TxPower > 31)
+			{
+				//
+				// R3, R4 can't large than 31 (0x24), 31 ~ 36 used by BBP 94
+				//
+				R = 31;
+				if (TxPower <= 36)
+					Bbp94 = BBPR94_DEFAULT + (UCHAR)(TxPower - 31);
+			}
+			else if (TxPower < 0)
+			{
+				//
+				// R3, R4 can't less than 0, -1 ~ -6 used by BBP 94
+				//
+				R = 0;
+				if (TxPower >= -6)
+					Bbp94 = BBPR94_DEFAULT + TxPower;
+			}
+			else
+			{
+				// 0 ~ 31
+				R = (ULONG) TxPower;
+				Bbp94 = BBPR94_DEFAULT;
+			}
+
+			ATEDBGPRINT(RT_DEBUG_TRACE, ("%s (TxPower=%d, R=%ld, BBP_R94=%d)\n", __FUNCTION__, TxPower, R, Bbp94));
+		}
+		else// 5.5 GHz
+		{
+			if (TxPower > 15)
+			{
+				//
+				// R3, R4 can't large than 15 (0x0F)
+				//
+				R = 15;
+			}
+			else if (TxPower < 0)
+			{
+				//
+				// R3, R4 can't less than 0
+				//
+				// -1 ~ -7
+				ASSERT((TxPower >= -7));
+				R = (ULONG)(TxPower + 7);
+				bPowerReduce = TRUE;
+			}
+			else
+			{
+				// 0 ~ 15
+				R = (ULONG) TxPower;
+			}
+
+			ATEDBGPRINT(RT_DEBUG_TRACE, ("%s (TxPower=%d, R=%lu)\n", __FUNCTION__, TxPower, R));
+		}
+
+		if (pAd->ate.Channel <= 14)
+		{
+			if (index == 0)
+			{
+				R = R << 9;		// shift TX power control to correct RF(R3) register bit position
+				R |= (pAd->LatchRfRegs.R3 & 0xffffc1ff);
+				pAd->LatchRfRegs.R3 = R;
+			}
+			else
+			{
+				R = R << 6;		// shift TX power control to correct RF(R4) register bit position
+				R |= (pAd->LatchRfRegs.R4 & 0xfffff83f);
+				pAd->LatchRfRegs.R4 = R;
+			}
+		}
+		else// 5.5GHz
+		{
+			if (bPowerReduce == FALSE)
+			{
+				if (index == 0)
+				{
+					R = (R << 10) | (1 << 9);		// shift TX power control to correct RF(R3) register bit position
+					R |= (pAd->LatchRfRegs.R3 & 0xffffc1ff);
+					pAd->LatchRfRegs.R3 = R;
+				}
+				else
+				{
+					R = (R << 7) | (1 << 6);		// shift TX power control to correct RF(R4) register bit position
+					R |= (pAd->LatchRfRegs.R4 & 0xfffff83f);
+					pAd->LatchRfRegs.R4 = R;
+				}
+			}
+			else
+			{
+				if (index == 0)
+				{
+					R = (R << 10);		// shift TX power control to correct RF(R3) register bit position
+					R |= (pAd->LatchRfRegs.R3 & 0xffffc1ff);
+
+					/* Clear bit 9 of R3 to reduce 7dB. */
+					pAd->LatchRfRegs.R3 = (R & (~(1 << 9)));
+				}
+				else
+				{
+					R = (R << 7);		// shift TX power control to correct RF(R4) register bit position
+					R |= (pAd->LatchRfRegs.R4 & 0xfffff83f);
+
+					/* Clear bit 6 of R4 to reduce 7dB. */
+					pAd->LatchRfRegs.R4 = (R & (~(1 << 6)));
+				}
+			}
+		}
+
+		RtmpRfIoWrite(pAd);
+
+		return 0;
+	}
+}
+#else// 1 //
+static INT ATETxPwrHandler(
+	IN PRTMP_ADAPTER pAd,
+	IN char index)
+{
+	ULONG R;
+	CHAR TxPower;
+	UCHAR Bbp94 = 0;
+
+#ifdef RALINK_28xx_QA
+	if ((pAd->ate.bQATxStart == TRUE) || (pAd->ate.bQARxStart == TRUE))
+	{
+		// TODO: how to get current TxPower0/1 from pAd->LatchRfRegs ?
+		/* When QA is used for Tx, pAd->ate.TxPower0/1 and real tx power
+		** are not synchronized.
+		*/
+/*
+		pAd->ate.TxPower0 = pAd->LatchRfRegs.xxx;
+		pAd->ate.TxPower1 = pAd->LatchRfRegs.xxx;
+*/
+		return 0;
+	}
+	else
+#endif // RALINK_28xx_QA //
+	{
+		TxPower = index == 0 ? pAd->ate.TxPower0 : pAd->ate.TxPower1;
+
+	if (TxPower > 31)
+	{
+		//
+		// R3, R4 can't large than 36 (0x24), 31 ~ 36 used by BBP 94
+		//
+		R = 31;
+		if (TxPower <= 36)
+			Bbp94 = BBPR94_DEFAULT + (UCHAR)(TxPower - 31);
+	}
+	else if (TxPower < 0)
+	{
+		//
+		// R3, R4 can't less than 0, -1 ~ -6 used by BBP 94
+		//
+		R = 0;
+		if (TxPower >= -6)
+			Bbp94 = BBPR94_DEFAULT + TxPower;
+	}
+	else
+	{
+		// 0 ~ 31
+		R = (ULONG) TxPower;
+		Bbp94 = BBPR94_DEFAULT;
+	}
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("%s (TxPower=%d, R3=%ld, BBP_R94=%d)\n", __FUNCTION__, TxPower, R, Bbp94));
+
+		if (pAd->ate.Channel <= 14)
+		{
+	if (index == 0)
+	{
+		R = R << 9;		// shift TX power control to correct RF(R3) register bit position
+		R |= (pAd->LatchRfRegs.R3 & 0xffffc1ff);
+		pAd->LatchRfRegs.R3 = R;
+	}
+	else
+	{
+		R = R << 6;		// shift TX power control to correct RF(R4) register bit position
+		R |= (pAd->LatchRfRegs.R4 & 0xfffff83f);
+		pAd->LatchRfRegs.R4 = R;
+	}
+		}
+		else
+		{
+			if (index == 0)
+			{
+				R = (R << 10) | (1 << 9);		// shift TX power control to correct RF(R3) register bit position
+				R |= (pAd->LatchRfRegs.R3 & 0xffffc1ff);
+				pAd->LatchRfRegs.R3 = R;
+			}
+			else
+			{
+				R = (R << 7) | (1 << 6);		// shift TX power control to correct RF(R4) register bit position
+				R |= (pAd->LatchRfRegs.R4 & 0xfffff83f);
+				pAd->LatchRfRegs.R4 = R;
+			}
+		}
+
+	RtmpRfIoWrite(pAd);
+
+	return 0;
+	}
+}
+#endif // 1 //
+/*
+    ==========================================================================
+    Description:
+        Set ATE operation mode to
+        0. ATESTART  = Start ATE Mode
+        1. ATESTOP   = Stop ATE Mode
+        2. TXCONT    = Continuous Transmit
+        3. TXCARR    = Transmit Carrier
+        4. TXFRAME   = Transmit Frames
+        5. RXFRAME   = Receive Frames
+#ifdef RALINK_28xx_QA
+        6. TXSTOP    = Stop Any Type of Transmition
+        7. RXSTOP    = Stop Receiving Frames
+#endif // RALINK_28xx_QA //
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+#ifdef RT2860
+static INT	ATECmdHandler(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UINT32			Value = 0;
+	UCHAR			BbpData;
+	UINT32			MacData = 0;
+	PTXD_STRUC		pTxD;
+	INT				index;
+	UINT			i=0, atemode;
+	PRXD_STRUC		pRxD;
+	PRTMP_TX_RING 	pTxRing = &pAd->TxRing[QID_AC_BE];
+#ifndef UCOS
+	NDIS_STATUS		Status = NDIS_STATUS_SUCCESS;
+#endif // UCOS //
+#ifdef	RT_BIG_ENDIAN
+    PTXD_STRUC      pDestTxD;
+    TXD_STRUC       TxD;
+#endif
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("===> ATECmdHandler()\n"));
+
+	ATEAsicSwitchChannel(pAd);
+	AsicLockChannel(pAd, pAd->ate.Channel);
+
+	RTMPusecDelay(5000);
+
+	// read MAC_SYS_CTRL and backup MAC_SYS_CTRL value.
+	RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &MacData);
+
+	// Default value in BBP R22 is 0x0.
+	BbpData = 0;
+
+	// clean bit4 to stop continuous Tx production test.
+	MacData &= 0xFFFFFFEF;
+
+	if (!strcmp(arg, "ATESTART")) 		//Enter ATE mode and set Tx/Rx Idle
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ATE: ATESTART\n"));
+
+#ifndef UCOS
+		// check if we have removed the firmware
+		if (!(ATE_ON(pAd)))
+		{
+			NICEraseFirmware(pAd);
+		}
+#endif // !UCOS //
+		atemode = pAd->ate.Mode;
+		pAd->ate.Mode = ATE_START;
+//		pAd->ate.TxDoneCount = pAd->ate.TxCount;
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, MacData);
+
+		if (atemode & ATE_TXCARR)
+		{
+			// No Carrier Test set BBP R22 bit7=0, bit6=0, bit[5~0]=0x0
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R22, &BbpData);
+			BbpData &= 0xFFFFFF00; //clear bit7, bit6, bit[5~0]
+		    ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+		}
+		else if (atemode & ATE_TXCARRSUPP)
+		{
+			// No Cont. TX set BBP R22 bit7=0
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R22, &BbpData);
+			BbpData &= ~(1 << 7); //set bit7=0
+			ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+
+			// No Carrier Suppression set BBP R24 bit0=0
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R24, &BbpData);
+			BbpData &= 0xFFFFFFFE; //clear bit0
+		    ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R24, BbpData);
+		}
+		// We should free some resource which was allocated when ATE_TXFRAME , ATE_STOP, and ATE_TXCONT.
+		else if ((atemode & ATE_TXFRAME) || (atemode == ATE_STOP))
+		{
+			PRTMP_TX_RING pTxRing = &pAd->TxRing[QID_AC_BE];
+
+			if (atemode & ATE_TXCONT)
+			{
+				// No Cont. TX set BBP R22 bit7=0
+				ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R22, &BbpData);
+				BbpData &= ~(1 << 7); //set bit7=0
+				ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+			}
+			// Abort Tx, Rx DMA.
+			RtmpDmaEnable(pAd, 0);
+			for (i=0; i<TX_RING_SIZE; i++)
+			{
+				PNDIS_PACKET  pPacket;
+
+#ifndef RT_BIG_ENDIAN
+			    pTxD = (PTXD_STRUC)pAd->TxRing[QID_AC_BE].Cell[i].AllocVa;
+#else
+        		pDestTxD = (PTXD_STRUC)pAd->TxRing[QID_AC_BE].Cell[i].AllocVa;
+        		TxD = *pDestTxD;
+        		pTxD = &TxD;
+        		RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+#endif
+				pTxD->DMADONE = 0;
+				pPacket = pTxRing->Cell[i].pNdisPacket;
+				if (pPacket)
+				{
+					PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr0, pTxD->SDLen0, PCI_DMA_TODEVICE);
+					RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_SUCCESS);
+				}
+				//Always assign pNdisPacket as NULL after clear
+				pTxRing->Cell[i].pNdisPacket = NULL;
+
+				pPacket = pTxRing->Cell[i].pNextNdisPacket;
+				if (pPacket)
+				{
+					PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr1, pTxD->SDLen1, PCI_DMA_TODEVICE);
+					RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_SUCCESS);
+				}
+				//Always assign pNextNdisPacket as NULL after clear
+				pTxRing->Cell[i].pNextNdisPacket = NULL;
+#ifdef RT_BIG_ENDIAN
+				RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+				WriteBackToDescriptor((PUCHAR)pDestTxD, (PUCHAR)pTxD, FALSE, TYPE_TXD);
+#endif
+			}
+			// Start Tx, RX DMA
+			RtmpDmaEnable(pAd, 1);
+		}
+		// reset Rx statistics.
+		pAd->ate.LastSNR0 = 0;
+		pAd->ate.LastSNR1 = 0;
+		pAd->ate.LastRssi0 = 0;
+		pAd->ate.LastRssi1 = 0;
+		pAd->ate.LastRssi2 = 0;
+		pAd->ate.AvgRssi0 = 0;
+		pAd->ate.AvgRssi1 = 0;
+		pAd->ate.AvgRssi2 = 0;
+		pAd->ate.AvgRssi0X8 = 0;
+		pAd->ate.AvgRssi1X8 = 0;
+		pAd->ate.AvgRssi2X8 = 0;
+		pAd->ate.NumOfAvgRssiSample = 0;
+
+#ifdef RALINK_28xx_QA
+		// Tx frame
+		pAd->ate.bQATxStart = FALSE;
+		pAd->ate.bQARxStart = FALSE;
+		pAd->ate.seq = 0;
+
+		// counters
+		pAd->ate.U2M = 0;
+		pAd->ate.OtherData = 0;
+		pAd->ate.Beacon = 0;
+		pAd->ate.OtherCount = 0;
+		pAd->ate.TxAc0 = 0;
+		pAd->ate.TxAc1 = 0;
+		pAd->ate.TxAc2 = 0;
+		pAd->ate.TxAc3 = 0;
+		pAd->ate.TxHCCA = 0;
+		pAd->ate.TxMgmt = 0;
+		pAd->ate.RSSI0 = 0;
+		pAd->ate.RSSI1 = 0;
+		pAd->ate.RSSI2 = 0;
+		pAd->ate.SNR0 = 0;
+		pAd->ate.SNR1 = 0;
+
+		// control
+		pAd->ate.TxDoneCount = 0;
+		pAd->ate.TxStatus = 0; // task Tx status // 0 --> task is idle, 1 --> task is running
+#endif // RALINK_28xx_QA //
+
+		// Soft reset BBP.
+		BbpSoftReset(pAd);
+
+
+#ifdef CONFIG_STA_SUPPORT
+		//
+		// LinkDown() has "AsicDisableSync();" and "RTMP_BBP_IO_R/W8_BY_REG_ID();" inside.
+		//
+//      LinkDown(pAd, FALSE);
+//		AsicEnableBssSync(pAd);
+#ifndef UCOS
+		netif_stop_queue(pAd->net_dev);
+#endif // !UCOS //
+		//
+		// If we skip "LinkDown()", we should disable protection
+		// to prevent from sending out RTS or CTS-to-self.
+		//
+		ATEDisableAsicProtect(pAd);
+		RTMPStationStop(pAd);
+#endif // CONFIG_STA_SUPPORT //
+
+		/* Disable Tx */
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 2);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		/* Disable Rx */
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 3);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+	}
+	else if (!strcmp(arg, "ATESTOP"))
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ATE: ATESTOP\n"));
+
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, MacData); // recover the MAC_SYS_CTRL register back.
+
+		// Disable Tx, Rx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= (0xfffffff3);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		// Abort Tx, RX DMA.
+		RtmpDmaEnable(pAd, 0);
+
+#ifndef UCOS
+		pAd->ate.bFWLoading = TRUE;
+		Status = NICLoadFirmware(pAd);
+		if (Status != NDIS_STATUS_SUCCESS)
+		{
+			ATEDBGPRINT(RT_DEBUG_ERROR, ("NICLoadFirmware failed, Status[=0x%08x]\n", Status));
+			return FALSE;
+		}
+#endif // !UCOS //
+		pAd->ate.Mode = ATE_STOP;
+
+
+#ifdef CONFIG_STA_SUPPORT
+		//
+		// Even the firmware has been loaded,
+		// we still could use ATE_BBP_IO_READ8_BY_REG_ID().
+		// But this is not suggested.
+		//
+		BbpSoftReset(pAd);
+#endif // CONFIG_STA_SUPPORT //
+
+		NICDisableInterrupt(pAd);
+
+		NICInitializeAdapter(pAd, TRUE);
+
+
+		// Reinitialize Rx Ring before Rx DMA is enabled.
+		// The nightmare of >>>RxCoherent<<< was gone !
+		for (index = 0; index < RX_RING_SIZE; index++)
+		{
+			pRxD = (PRXD_STRUC) pAd->RxRing.Cell[index].AllocVa;
+			pRxD->DDONE = 0;
+		}
+
+		// We should read EEPROM for all cases.
+		NICReadEEPROMParameters(pAd, NULL);
+		NICInitAsicFromEEPROM(pAd);
+
+		AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+		AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+
+		//
+		// Enable Interrupt
+		//
+
+		//
+		// These steps are only for APAutoSelectChannel().
+		//
+#if 0
+		//pAd->bStaFifoTest = TRUE;
+		pAd->int_enable_reg = ((DELAYINTMASK)  | (RxINT|TxDataInt|TxMgmtInt)) & ~(0x03);
+		pAd->int_disable_mask = 0;
+		pAd->int_pending = 0;
+#endif
+		RTMP_IO_WRITE32(pAd, INT_SOURCE_CSR, 0xffffffff);  // clear garbage interrupts
+		NICEnableInterrupt(pAd);
+
+
+/*=========================================================================*/
+		/* restore RX_FILTR_CFG */
+#ifdef CONFIG_STA_SUPPORT
+		/* restore RX_FILTR_CFG due to that QA maybe set it to 0x3 */
+		RTMP_IO_WRITE32(pAd, RX_FILTR_CFG, STANORMAL);
+#endif // CONFIG_STA_SUPPORT //
+/*=========================================================================*/
+
+		// Enable Tx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value |= (1 << 2);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		// Enable Tx, Rx DMA.
+		RtmpDmaEnable(pAd, 1);
+
+		// Enable Rx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value |= (1 << 3);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+
+#ifdef CONFIG_STA_SUPPORT
+		RTMPStationStart(pAd);
+#endif // CONFIG_STA_SUPPORT //
+#ifndef UCOS
+		netif_start_queue(pAd->net_dev);
+#endif // !UCOS //
+	}
+	else if (!strcmp(arg, "TXCARR"))	// Tx Carrier
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ATE: TXCARR\n"));
+		pAd->ate.Mode |= ATE_TXCARR;
+
+		// QA has done the following steps if it is used.
+		if (pAd->ate.bQATxStart == FALSE)
+		{
+			// Soft reset BBP.
+			BbpSoftReset(pAd);
+
+			// Carrier Test set BBP R22 bit7=1, bit6=1, bit[5~0]=0x01
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R22, &BbpData);
+			BbpData &= 0xFFFFFF00; //clear bit7, bit6, bit[5~0]
+			BbpData |= 0x000000C1; //set bit7=1, bit6=1, bit[5~0]=0x01
+			ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+
+			// set MAC_SYS_CTRL(0x1004) Continuous Tx Production Test (bit4) = 1
+			RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+			Value = Value | 0x00000010;
+			RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+		}
+	}
+	else if (!strcmp(arg, "TXCONT"))	// Tx Continue
+	{
+		if (pAd->ate.bQATxStart == TRUE)
+		{
+			/* set MAC_SYS_CTRL(0x1004) bit4(Continuous Tx Production Test)
+			   and bit2(MAC TX enable) back to zero. */
+			RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &MacData);
+			MacData &= 0xFFFFFFEB;
+			RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, MacData);
+
+			// set BBP R22 bit7=0
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R22, &BbpData);
+			BbpData &= 0xFFFFFF7F; //set bit7=0
+			ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+		}
+
+		/* for TxCont mode.
+		** Step 1: Send 50 packets first then wait for a moment.
+		** Step 2: Send more 50 packet then start continue mode.
+		*/
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ATE: TXCONT\n"));
+		// Step 1: send 50 packets first.
+		pAd->ate.Mode |= ATE_TXCONT;
+		pAd->ate.TxCount = 50;
+                /* Do it after Tx/Rx DMA is aborted. */
+//		pAd->ate.TxDoneCount = 0;
+
+		// Soft reset BBP.
+		BbpSoftReset(pAd);
+
+		// Abort Tx, RX DMA.
+		RtmpDmaEnable(pAd, 0);
+
+		// Fix can't smooth kick
+		{
+			RTMP_IO_READ32(pAd, TX_DTX_IDX0 + QID_AC_BE * 0x10,  &pTxRing->TxDmaIdx);
+			pTxRing->TxSwFreeIdx = pTxRing->TxDmaIdx;
+			pTxRing->TxCpuIdx = pTxRing->TxDmaIdx;
+			RTMP_IO_WRITE32(pAd, TX_CTX_IDX0 + QID_AC_BE * 0x10, pTxRing->TxCpuIdx);
+		}
+
+		pAd->ate.TxDoneCount = 0;
+
+		/* Only needed if we have to send some normal frames. */
+		SetJapanFilter(pAd);
+
+		for (i = 0; (i < TX_RING_SIZE-1) && (i < pAd->ate.TxCount); i++)
+		{
+			PNDIS_PACKET pPacket;
+			UINT32 TxIdx = pTxRing->TxCpuIdx;
+
+#ifndef RT_BIG_ENDIAN
+			pTxD = (PTXD_STRUC)pTxRing->Cell[TxIdx].AllocVa;
+#else
+			pDestTxD = (PTXD_STRUC)pTxRing->Cell[TxIdx].AllocVa;
+			TxD = *pDestTxD;
+			pTxD = &TxD;
+			RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+#endif
+			// Clean current cell.
+			pPacket = pTxRing->Cell[TxIdx].pNdisPacket;
+			if (pPacket)
+			{
+				PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr0, pTxD->SDLen0, PCI_DMA_TODEVICE);
+				RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_SUCCESS);
+			}
+			//Always assign pNdisPacket as NULL after clear
+			pTxRing->Cell[TxIdx].pNdisPacket = NULL;
+
+			pPacket = pTxRing->Cell[TxIdx].pNextNdisPacket;
+			if (pPacket)
+			{
+				PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr1, pTxD->SDLen1, PCI_DMA_TODEVICE);
+				RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_SUCCESS);
+			}
+			//Always assign pNextNdisPacket as NULL after clear
+			pTxRing->Cell[TxIdx].pNextNdisPacket = NULL;
+
+#ifdef RT_BIG_ENDIAN
+			RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+			WriteBackToDescriptor((PUCHAR)pDestTxD, (PUCHAR)pTxD, FALSE, TYPE_TXD);
+#endif
+
+			if (ATESetUpFrame(pAd, TxIdx) != 0)
+				break;
+
+			INC_RING_INDEX(pTxRing->TxCpuIdx, TX_RING_SIZE);
+		}
+
+                // Setup frame format.
+		ATESetUpFrame(pAd, pTxRing->TxCpuIdx);
+
+		// Start Tx, RX DMA.
+		RtmpDmaEnable(pAd, 1);
+
+		// Enable Tx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value |= (1 << 2);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		// Disable Rx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 3);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+#ifdef RALINK_28xx_QA
+		if (pAd->ate.bQATxStart == TRUE)
+		{
+			pAd->ate.TxStatus = 1;
+			//pAd->ate.Repeat = 0;
+		}
+#endif // RALINK_28xx_QA //
+
+		// kick Tx-Ring.
+		RTMP_IO_WRITE32(pAd, TX_CTX_IDX0 + QID_AC_BE * RINGREG_DIFF, pAd->TxRing[QID_AC_BE].TxCpuIdx);
+
+		RTMPusecDelay(5000);
+
+
+		// Step 2: send more 50 packets then start continue mode.
+		// Abort Tx, RX DMA.
+		RtmpDmaEnable(pAd, 0);
+
+		// Cont. TX set BBP R22 bit7=1
+		ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R22, &BbpData);
+		BbpData |= 0x00000080; //set bit7=1
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+
+		pAd->ate.TxCount = 50;
+
+		// Fix can't smooth kick
+		{
+			RTMP_IO_READ32(pAd, TX_DTX_IDX0 + QID_AC_BE * 0x10,  &pTxRing->TxDmaIdx);
+			pTxRing->TxSwFreeIdx = pTxRing->TxDmaIdx;
+			pTxRing->TxCpuIdx = pTxRing->TxDmaIdx;
+			RTMP_IO_WRITE32(pAd, TX_CTX_IDX0 + QID_AC_BE * 0x10, pTxRing->TxCpuIdx);
+		}
+
+		pAd->ate.TxDoneCount = 0;
+
+		SetJapanFilter(pAd);
+
+		for (i = 0; (i < TX_RING_SIZE-1) && (i < pAd->ate.TxCount); i++)
+		{
+			PNDIS_PACKET pPacket;
+			UINT32 TxIdx = pTxRing->TxCpuIdx;
+
+#ifndef RT_BIG_ENDIAN
+			pTxD = (PTXD_STRUC)pTxRing->Cell[TxIdx].AllocVa;
+#else
+			pDestTxD = (PTXD_STRUC)pTxRing->Cell[TxIdx].AllocVa;
+			TxD = *pDestTxD;
+			pTxD = &TxD;
+			RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+#endif
+			// clean current cell.
+			pPacket = pTxRing->Cell[TxIdx].pNdisPacket;
+			if (pPacket)
+			{
+				PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr0, pTxD->SDLen0, PCI_DMA_TODEVICE);
+				RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_SUCCESS);
+			}
+			//Always assign pNdisPacket as NULL after clear
+			pTxRing->Cell[TxIdx].pNdisPacket = NULL;
+
+			pPacket = pTxRing->Cell[TxIdx].pNextNdisPacket;
+			if (pPacket)
+			{
+				PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr1, pTxD->SDLen1, PCI_DMA_TODEVICE);
+				RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_SUCCESS);
+			}
+			//Always assign pNextNdisPacket as NULL after clear
+			pTxRing->Cell[TxIdx].pNextNdisPacket = NULL;
+
+#ifdef RT_BIG_ENDIAN
+			RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+			WriteBackToDescriptor((PUCHAR)pDestTxD, (PUCHAR)pTxD, FALSE, TYPE_TXD);
+#endif
+
+			if (ATESetUpFrame(pAd, TxIdx) != 0)
+				break;
+
+			INC_RING_INDEX(pTxRing->TxCpuIdx, TX_RING_SIZE);
+		}
+
+		ATESetUpFrame(pAd, pTxRing->TxCpuIdx);
+
+		// Start Tx, RX DMA.
+		RtmpDmaEnable(pAd, 1);
+
+		// Enable Tx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value |= (1 << 2);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		// Disable Rx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 3);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+#ifdef RALINK_28xx_QA
+		if (pAd->ate.bQATxStart == TRUE)
+		{
+			pAd->ate.TxStatus = 1;
+			//pAd->ate.Repeat = 0;
+		}
+#endif // RALINK_28xx_QA //
+
+		// kick Tx-Ring.
+		RTMP_IO_WRITE32(pAd, TX_CTX_IDX0 + QID_AC_BE * RINGREG_DIFF, pAd->TxRing[QID_AC_BE].TxCpuIdx);
+
+		RTMPusecDelay(500);
+
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &MacData);
+		MacData |= 0x00000010;
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, MacData);
+	}
+	else if (!strcmp(arg, "TXFRAME")) // Tx Frames
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ATE: TXFRAME(Count=%d)\n", pAd->ate.TxCount));
+		pAd->ate.Mode |= ATE_TXFRAME;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+
+		// Soft reset BBP.
+		BbpSoftReset(pAd);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, MacData);
+
+		// Abort Tx, RX DMA.
+		RtmpDmaEnable(pAd, 0);
+
+		// Fix can't smooth kick
+		{
+			RTMP_IO_READ32(pAd, TX_DTX_IDX0 + QID_AC_BE * 0x10,  &pTxRing->TxDmaIdx);
+			pTxRing->TxSwFreeIdx = pTxRing->TxDmaIdx;
+			pTxRing->TxCpuIdx = pTxRing->TxDmaIdx;
+			RTMP_IO_WRITE32(pAd, TX_CTX_IDX0 + QID_AC_BE * 0x10, pTxRing->TxCpuIdx);
+		}
+
+		pAd->ate.TxDoneCount = 0;
+
+		SetJapanFilter(pAd);
+
+		for (i = 0; (i < TX_RING_SIZE-1) && (i < pAd->ate.TxCount); i++)
+		{
+			PNDIS_PACKET pPacket;
+			UINT32 TxIdx = pTxRing->TxCpuIdx;
+
+#ifndef RT_BIG_ENDIAN
+			pTxD = (PTXD_STRUC)pTxRing->Cell[TxIdx].AllocVa;
+#else
+			pDestTxD = (PTXD_STRUC)pTxRing->Cell[TxIdx].AllocVa;
+			TxD = *pDestTxD;
+			pTxD = &TxD;
+			RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+#endif
+			// Clean current cell.
+			pPacket = pTxRing->Cell[TxIdx].pNdisPacket;
+			if (pPacket)
+			{
+				PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr0, pTxD->SDLen0, PCI_DMA_TODEVICE);
+				RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_SUCCESS);
+			}
+			//Always assign pNdisPacket as NULL after clear
+			pTxRing->Cell[TxIdx].pNdisPacket = NULL;
+
+			pPacket = pTxRing->Cell[TxIdx].pNextNdisPacket;
+			if (pPacket)
+			{
+				PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr1, pTxD->SDLen1, PCI_DMA_TODEVICE);
+				RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_SUCCESS);
+			}
+			//Always assign pNextNdisPacket as NULL after clear
+			pTxRing->Cell[TxIdx].pNextNdisPacket = NULL;
+
+#ifdef RT_BIG_ENDIAN
+			RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+			WriteBackToDescriptor((PUCHAR)pDestTxD, (PUCHAR)pTxD, FALSE, TYPE_TXD);
+#endif
+
+			if (ATESetUpFrame(pAd, TxIdx) != 0)
+				break;
+
+			INC_RING_INDEX(pTxRing->TxCpuIdx, TX_RING_SIZE);
+
+		}
+
+		ATESetUpFrame(pAd, pTxRing->TxCpuIdx);
+
+		// Start Tx, Rx DMA.
+		RtmpDmaEnable(pAd, 1);
+
+		// Enable Tx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value |= (1 << 2);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+#ifdef RALINK_28xx_QA
+		// add this for LoopBack mode
+		if (pAd->ate.bQARxStart == FALSE)
+		{
+			// Disable Rx
+			RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+			Value &= ~(1 << 3);
+			RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+		}
+
+		if (pAd->ate.bQATxStart == TRUE)
+		{
+			pAd->ate.TxStatus = 1;
+			//pAd->ate.Repeat = 0;
+		}
+#else
+		// Disable Rx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 3);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+#endif // RALINK_28xx_QA //
+
+		RTMP_IO_READ32(pAd, TX_DTX_IDX0 + QID_AC_BE * RINGREG_DIFF, &pAd->TxRing[QID_AC_BE].TxDmaIdx);
+		// kick Tx-Ring.
+		RTMP_IO_WRITE32(pAd, TX_CTX_IDX0 + QID_AC_BE * RINGREG_DIFF, pAd->TxRing[QID_AC_BE].TxCpuIdx);
+
+		pAd->RalinkCounters.KickTxCount++;
+	}
+#ifdef RALINK_28xx_QA
+	else if (!strcmp(arg, "TXSTOP"))
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ATE: TXSTOP\n"));
+		atemode = pAd->ate.Mode;
+		pAd->ate.Mode &= ATE_TXSTOP;
+		pAd->ate.bQATxStart = FALSE;
+//		pAd->ate.TxDoneCount = pAd->ate.TxCount;
+
+		if (atemode & ATE_TXCARR)
+		{
+			// No Carrier Test set BBP R22 bit7=0, bit6=0, bit[5~0]=0x0
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R22, &BbpData);
+			BbpData &= 0xFFFFFF00; //clear bit7, bit6, bit[5~0]
+		    ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+		}
+		else if (atemode & ATE_TXCARRSUPP)
+		{
+			// No Cont. TX set BBP R22 bit7=0
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R22, &BbpData);
+			BbpData &= ~(1 << 7); //set bit7=0
+			ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+
+			// No Carrier Suppression set BBP R24 bit0=0
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R24, &BbpData);
+			BbpData &= 0xFFFFFFFE; //clear bit0
+		    ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R24, BbpData);
+		}
+		// We should free some resource which allocate when ATE_TXFRAME , ATE_STOP, and ATE_TXCONT.
+		else if ((atemode & ATE_TXFRAME) || (atemode == ATE_STOP))
+		{
+
+			PRTMP_TX_RING pTxRing = &pAd->TxRing[QID_AC_BE];
+
+			if (atemode & ATE_TXCONT)
+			{
+				// No Cont. TX set BBP R22 bit7=0
+				ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R22, &BbpData);
+				BbpData &= ~(1 << 7); //set bit7=0
+				ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+			}
+
+			// Abort Tx, Rx DMA.
+			RtmpDmaEnable(pAd, 0);
+			for (i=0; i<TX_RING_SIZE; i++)
+			{
+				PNDIS_PACKET  pPacket;
+
+#ifndef RT_BIG_ENDIAN
+			    pTxD = (PTXD_STRUC)pAd->TxRing[QID_AC_BE].Cell[i].AllocVa;
+#else
+        		pDestTxD = (PTXD_STRUC)pAd->TxRing[QID_AC_BE].Cell[i].AllocVa;
+        		TxD = *pDestTxD;
+        		pTxD = &TxD;
+        		RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+#endif
+				pTxD->DMADONE = 0;
+				pPacket = pTxRing->Cell[i].pNdisPacket;
+				if (pPacket)
+				{
+					PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr0, pTxD->SDLen0, PCI_DMA_TODEVICE);
+					RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_SUCCESS);
+				}
+				//Always assign pNdisPacket as NULL after clear
+				pTxRing->Cell[i].pNdisPacket = NULL;
+
+				pPacket = pTxRing->Cell[i].pNextNdisPacket;
+				if (pPacket)
+				{
+					PCI_UNMAP_SINGLE(pAd, pTxD->SDPtr1, pTxD->SDLen1, PCI_DMA_TODEVICE);
+					RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_SUCCESS);
+				}
+				//Always assign pNextNdisPacket as NULL after clear
+				pTxRing->Cell[i].pNextNdisPacket = NULL;
+#ifdef RT_BIG_ENDIAN
+				RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+				WriteBackToDescriptor((PUCHAR)pDestTxD, (PUCHAR)pTxD, FALSE, TYPE_TXD);
+#endif
+			}
+			// Enable Tx, Rx DMA
+			RtmpDmaEnable(pAd, 1);
+
+		}
+
+		// control
+//		pAd->ate.TxDoneCount = 0;
+		pAd->ate.TxStatus = 0; // task Tx status // 0 --> task is idle, 1 --> task is running
+
+		// Soft reset BBP.
+		BbpSoftReset(pAd);
+
+		// Disable Tx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 2);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+	}
+	else if (!strcmp(arg, "RXSTOP"))
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ATE: RXSTOP\n"));
+		atemode = pAd->ate.Mode;
+		pAd->ate.Mode &= ATE_RXSTOP;
+		pAd->ate.bQARxStart = FALSE;
+//		pAd->ate.TxDoneCount = pAd->ate.TxCount;
+
+		if (atemode & ATE_TXCARR)
+		{
+			;
+		}
+		else if (atemode & ATE_TXCARRSUPP)
+		{
+			;
+		}
+
+		// We should free some resource which was allocated when ATE_TXFRAME , ATE_STOP, and ATE_TXCONT.
+		else if ((atemode & ATE_TXFRAME) || (atemode == ATE_STOP))
+		{
+			if (atemode & ATE_TXCONT)
+			{
+				;
+			}
+		}
+
+		// control
+//		pAd->ate.TxDoneCount = 0;
+//		pAd->ate.TxStatus = 0; // task Tx status // 0 --> task is idle, 1 --> task is running
+
+		// Soft reset BBP.
+		BbpSoftReset(pAd);
+
+		// Disable Rx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 3);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+	}
+#endif // RALINK_28xx_QA //
+	else if (!strcmp(arg, "RXFRAME")) // Rx Frames
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ATE: RXFRAME\n"));
+
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, MacData);
+
+		pAd->ate.Mode |= ATE_RXFRAME;
+
+		// Disable Tx of MAC block.
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 2);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		// Enable Rx of MAC block.
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value |= (1 << 3);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+	}
+	else
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ATE: Invalid arg!\n"));
+		return FALSE;
+	}
+	RTMPusecDelay(5000);
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("<=== ATECmdHandler()\n"));
+
+	return TRUE;
+}
+#endif // RT2860 //
+/*                                                           */
+/*                                                           */
+/*=======================End of RT2860=======================*/
+
+
+/*======================Start of RT2870======================*/
+/*                                                           */
+/*                                                           */
+
+
+INT	Set_ATE_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	if (ATECmdHandler(pAd, arg))
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_Proc Success\n"));
+
+
+		return TRUE;
+	}
+	else
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_Proc Failed\n"));
+		return FALSE;
+	}
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE ADDR1=DA for TxFrame(AP  : To DS = 0 ; From DS = 1)
+        or
+        Set ATE ADDR3=DA for TxFrame(STA : To DS = 1 ; From DS = 0)
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_DA_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	CHAR				*value;
+	INT					i;
+
+	if(strlen(arg) != 17)  //Mac address acceptable format 01:02:03:04:05:06 length 17
+		return FALSE;
+
+    for (i=0, value = rstrtok(arg, ":"); value; value = rstrtok(NULL, ":"))
+	{
+		if((strlen(value) != 2) || (!isxdigit(*value)) || (!isxdigit(*(value+1))) )
+			return FALSE;  //Invalid
+
+
+#ifdef CONFIG_STA_SUPPORT
+		AtoH(value, &pAd->ate.Addr3[i++], 1);
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+	if(i != 6)
+		return FALSE;  //Invalid
+
+
+#ifdef CONFIG_STA_SUPPORT
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_DA_Proc (DA = %2X:%2X:%2X:%2X:%2X:%2X)\n", pAd->ate.Addr3[0],
+		pAd->ate.Addr3[1], pAd->ate.Addr3[2], pAd->ate.Addr3[3], pAd->ate.Addr3[4], pAd->ate.Addr3[5]));
+#endif // CONFIG_STA_SUPPORT //
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_DA_Proc Success\n"));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE ADDR3=SA for TxFrame(AP  : To DS = 0 ; From DS = 1)
+        or
+        Set ATE ADDR2=SA for TxFrame(STA : To DS = 1 ; From DS = 0)
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_SA_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	CHAR				*value;
+	INT					i;
+
+	if(strlen(arg) != 17)  //Mac address acceptable format 01:02:03:04:05:06 length 17
+		return FALSE;
+
+    for (i=0, value = rstrtok(arg, ":"); value; value = rstrtok(NULL, ":"))
+	{
+		if((strlen(value) != 2) || (!isxdigit(*value)) || (!isxdigit(*(value+1))) )
+			return FALSE;  //Invalid
+
+
+#ifdef CONFIG_STA_SUPPORT
+		AtoH(value, &pAd->ate.Addr2[i++], 1);
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+	if(i != 6)
+		return FALSE;  //Invalid
+
+
+#ifdef CONFIG_STA_SUPPORT
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_SA_Proc (SA = %2X:%2X:%2X:%2X:%2X:%2X)\n", pAd->ate.Addr2[0],
+		pAd->ate.Addr2[1], pAd->ate.Addr2[2], pAd->ate.Addr2[3], pAd->ate.Addr2[4], pAd->ate.Addr2[5]));
+#endif // CONFIG_STA_SUPPORT //
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_SA_Proc Success\n"));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE ADDR2=BSSID for TxFrame(AP  : To DS = 0 ; From DS = 1)
+        or
+        Set ATE ADDR1=BSSID for TxFrame(STA : To DS = 1 ; From DS = 0)
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_BSSID_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	CHAR				*value;
+	INT					i;
+
+	if(strlen(arg) != 17)  //Mac address acceptable format 01:02:03:04:05:06 length 17
+		return FALSE;
+
+    for (i=0, value = rstrtok(arg, ":"); value; value = rstrtok(NULL, ":"))
+	{
+		if((strlen(value) != 2) || (!isxdigit(*value)) || (!isxdigit(*(value+1))) )
+			return FALSE;  //Invalid
+
+
+#ifdef CONFIG_STA_SUPPORT
+		AtoH(value, &pAd->ate.Addr1[i++], 1);
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+	if(i != 6)
+		return FALSE;  //Invalid
+
+
+#ifdef CONFIG_STA_SUPPORT
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_BSSID_Proc (BSSID = %2X:%2X:%2X:%2X:%2X:%2X)\n",	pAd->ate.Addr1[0],
+		pAd->ate.Addr1[1], pAd->ate.Addr1[2], pAd->ate.Addr1[3], pAd->ate.Addr1[4], pAd->ate.Addr1[5]));
+#endif // CONFIG_STA_SUPPORT //
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_BSSID_Proc Success\n"));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Tx Channel
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_CHANNEL_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UCHAR channel;
+
+	channel = simple_strtol(arg, 0, 10);
+
+	if ((channel < 1) || (channel > 216))// to allow A band channel : ((channel < 1) || (channel > 14))
+	{
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_CHANNEL_Proc::Out of range, it should be in range of 1~14.\n"));
+		return FALSE;
+	}
+	pAd->ate.Channel = channel;
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_CHANNEL_Proc (ATE Channel = %d)\n", pAd->ate.Channel));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_CHANNEL_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Tx Power0
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_POWER0_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	CHAR TxPower;
+
+	TxPower = simple_strtol(arg, 0, 10);
+
+	if (pAd->ate.Channel <= 14)
+	{
+		if ((TxPower > 31) || (TxPower < 0))
+		{
+			ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_POWER0_Proc::Out of range (Value=%d)\n", TxPower));
+			return FALSE;
+		}
+	}
+	else// 5.5GHz
+	{
+		if ((TxPower > 15) || (TxPower < -7))
+		{
+			ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_POWER0_Proc::Out of range (Value=%d)\n", TxPower));
+			return FALSE;
+		}
+	}
+
+	pAd->ate.TxPower0 = TxPower;
+	ATETxPwrHandler(pAd, 0);
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_POWER0_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Tx Power1
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_POWER1_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	CHAR TxPower;
+
+	TxPower = simple_strtol(arg, 0, 10);
+
+	if (pAd->ate.Channel <= 14)
+	{
+	if ((TxPower > 31) || (TxPower < 0))
+	{
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_POWER1_Proc::Out of range (Value=%d)\n", TxPower));
+		return FALSE;
+	}
+	}
+	else
+	{
+		if ((TxPower > 15) || (TxPower < -7))
+		{
+			ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_POWER1_Proc::Out of range (Value=%d)\n", TxPower));
+			return FALSE;
+		}
+	}
+
+	pAd->ate.TxPower1 = TxPower;
+	ATETxPwrHandler(pAd, 1);
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_POWER1_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Tx Antenna
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_Antenna_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	CHAR value;
+
+	value = simple_strtol(arg, 0, 10);
+
+	if ((value > 2) || (value < 0))
+	{
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_Antenna_Proc::Out of range (Value=%d)\n", value));
+		return FALSE;
+	}
+
+	pAd->ate.TxAntennaSel = value;
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_TX_Antenna_Proc (Antenna = %d)\n", pAd->ate.TxAntennaSel));
+	ATEDBGPRINT(RT_DEBUG_TRACE,("Ralink: Set_ATE_TX_Antenna_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Rx Antenna
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_RX_Antenna_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	CHAR value;
+
+	value = simple_strtol(arg, 0, 10);
+
+	if ((value > 3) || (value < 0))
+	{
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_RX_Antenna_Proc::Out of range (Value=%d)\n", value));
+		return FALSE;
+	}
+
+	pAd->ate.RxAntennaSel = value;
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_RX_Antenna_Proc (Antenna = %d)\n", pAd->ate.RxAntennaSel));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_RX_Antenna_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE RF frequence offset
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_FREQOFFSET_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UCHAR RFFreqOffset;
+	ULONG R4;
+
+	RFFreqOffset = simple_strtol(arg, 0, 10);
+
+	if(RFFreqOffset >= 64)
+	{
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_FREQOFFSET_Proc::Out of range, it should be in range of 0~63.\n"));
+		return FALSE;
+	}
+
+	pAd->ate.RFFreqOffset = RFFreqOffset;
+	R4 = pAd->ate.RFFreqOffset << 15;		// shift TX power control to correct RF register bit position
+	R4 |= (pAd->LatchRfRegs.R4 & ((~0x001f8000)));
+	pAd->LatchRfRegs.R4 = R4;
+
+	RtmpRfIoWrite(pAd);
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_TX_FREQOFFSET_Proc (RFFreqOffset = %d)\n", pAd->ate.RFFreqOffset));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_FREQOFFSET_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE RF BW
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_BW_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	int i;
+	UCHAR value = 0;
+	UCHAR BBPCurrentBW;
+
+	BBPCurrentBW = simple_strtol(arg, 0, 10);
+
+	if(BBPCurrentBW == 0)
+		pAd->ate.TxWI.BW = BW_20;
+	else
+		pAd->ate.TxWI.BW = BW_40;
+
+	if(pAd->ate.TxWI.BW == BW_20)
+	{
+		if(pAd->ate.Channel <= 14)
+		{
+ 		for (i=0; i<5; i++)
+ 		{
+				if (pAd->Tx20MPwrCfgGBand[i] != 0xffffffff)
+				{
+					RTMP_IO_WRITE32(pAd, TX_PWR_CFG_0 + i*4, pAd->Tx20MPwrCfgGBand[i]);
+					RTMPusecDelay(5000);
+				}
+			}
+		}
+		else
+		{
+			for (i=0; i<5; i++)
+			{
+				if (pAd->Tx20MPwrCfgABand[i] != 0xffffffff)
+ 			{
+					RTMP_IO_WRITE32(pAd, TX_PWR_CFG_0 + i*4, pAd->Tx20MPwrCfgABand[i]);
+ 				RTMPusecDelay(5000);
+ 			}
+ 		}
+		}
+
+		//Set BBP R4 bit[4:3]=0:0
+ 		ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &value);
+ 		value &= (~0x18);
+ 		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, value);
+
+  		//Set BBP R66=0x3C
+ 		value = 0x3C;
+ 		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, value);
+		//Set BBP R68=0x0B
+		//to improve Rx sensitivity.
+		value = 0x0B;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R68, value);
+		//Set BBP R69=0x16
+		value = 0x16;
+ 		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, value);
+		//Set BBP R70=0x08
+		value = 0x08;
+ 		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, value);
+		//Set BBP R73=0x11
+		value = 0x11;
+ 		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R73, value);
+
+	    // If Channel=14, Bandwidth=20M and Mode=CCK, Set BBP R4 bit5=1
+	    // (Japan filter coefficients)
+	    // This segment of code will only works when ATETXMODE and ATECHANNEL
+	    // were set to MODE_CCK and 14 respectively before ATETXBW is set to 0.
+	    //=====================================================================
+		if (pAd->ate.Channel == 14)
+		{
+			int TxMode = pAd->ate.TxWI.PHYMODE;
+			if (TxMode == MODE_CCK)
+			{
+				// when Channel==14 && Mode==CCK && BandWidth==20M, BBP R4 bit5=1
+ 				ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &value);
+				value |= 0x20; //set bit5=1
+ 				ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, value);
+			}
+		}
+
+	    //=====================================================================
+		// If bandwidth != 40M, RF Reg4 bit 21 = 0.
+		pAd->LatchRfRegs.R4 &= ~0x00200000;
+		RtmpRfIoWrite(pAd);
+	}
+	else if(pAd->ate.TxWI.BW == BW_40)
+	{
+		if(pAd->ate.Channel <= 14)
+		{
+			for (i=0; i<5; i++)
+			{
+				if (pAd->Tx40MPwrCfgGBand[i] != 0xffffffff)
+				{
+					RTMP_IO_WRITE32(pAd, TX_PWR_CFG_0 + i*4, pAd->Tx40MPwrCfgGBand[i]);
+					RTMPusecDelay(5000);
+				}
+			}
+		}
+		else
+		{
+			for (i=0; i<5; i++)
+			{
+				if (pAd->Tx40MPwrCfgABand[i] != 0xffffffff)
+				{
+					RTMP_IO_WRITE32(pAd, TX_PWR_CFG_0 + i*4, pAd->Tx40MPwrCfgABand[i]);
+					RTMPusecDelay(5000);
+				}
+			}
+#ifdef DOT11_N_SUPPORT
+			if ((pAd->ate.TxWI.PHYMODE >= MODE_HTMIX) && (pAd->ate.TxWI.MCS == 7))
+			{
+    			value = 0x28;
+    			ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R67, value);
+			}
+#endif // DOT11_N_SUPPORT //
+		}
+
+		//Set BBP R4 bit[4:3]=1:0
+		ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &value);
+		value &= (~0x18);
+		value |= 0x10;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, value);
+
+		//Set BBP R66=0x3C
+		value = 0x3C;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, value);
+		//Set BBP R68=0x0C
+		//to improve Rx sensitivity.
+		value = 0x0C;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R68, value);
+		//Set BBP R69=0x1A
+		value = 0x1A;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, value);
+		//Set BBP R70=0x0A
+		value = 0x0A;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, value);
+		//Set BBP R73=0x16
+		value = 0x16;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R73, value);
+
+		// If bandwidth = 40M, set RF Reg4 bit 21 = 1.
+		pAd->LatchRfRegs.R4 |= 0x00200000;
+		RtmpRfIoWrite(pAd);
+	}
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_TX_BW_Proc (BBPCurrentBW = %d)\n", pAd->ate.TxWI.BW));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_BW_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Tx frame length
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_LENGTH_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	pAd->ate.TxLength = simple_strtol(arg, 0, 10);
+
+	if((pAd->ate.TxLength < 24) || (pAd->ate.TxLength > (MAX_FRAME_SIZE - 34/* == 2312 */)))
+	{
+		pAd->ate.TxLength = (MAX_FRAME_SIZE - 34/* == 2312 */);
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_LENGTH_Proc::Out of range, it should be in range of 24~%d.\n", (MAX_FRAME_SIZE - 34/* == 2312 */)));
+		return FALSE;
+	}
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_TX_LENGTH_Proc (TxLength = %d)\n", pAd->ate.TxLength));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_LENGTH_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Tx frame count
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_COUNT_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	pAd->ate.TxCount = simple_strtol(arg, 0, 10);
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_TX_COUNT_Proc (TxCount = %d)\n", pAd->ate.TxCount));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_COUNT_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Tx frame MCS
+
+        Return:
+        	TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_MCS_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UCHAR MCS;
+	int result;
+
+	MCS = simple_strtol(arg, 0, 10);
+	result = CheckMCSValid(pAd->ate.TxWI.PHYMODE, MCS);
+
+	if (result != -1)
+	{
+		pAd->ate.TxWI.MCS = (UCHAR)MCS;
+	}
+	else
+	{
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_MCS_Proc::Out of range, refer to rate table.\n"));
+		return FALSE;
+	}
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_TX_MCS_Proc (MCS = %d)\n", pAd->ate.TxWI.MCS));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_MCS_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Tx frame Mode
+        0: MODE_CCK
+        1: MODE_OFDM
+        2: MODE_HTMIX
+        3: MODE_HTGREENFIELD
+
+        Return:
+        	TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_MODE_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	pAd->ate.TxWI.PHYMODE = simple_strtol(arg, 0, 10);
+
+	if(pAd->ate.TxWI.PHYMODE > 3)
+	{
+		pAd->ate.TxWI.PHYMODE = 0;
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_MODE_Proc::Out of range. it should be in range of 0~3\n"));
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("0: CCK, 1: OFDM, 2: HT_MIX, 3: HT_GREEN_FIELD.\n"));
+		return FALSE;
+	}
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_TX_MODE_Proc (TxMode = %d)\n", pAd->ate.TxWI.PHYMODE));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_MODE_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Tx frame GI
+
+        Return:
+        	TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_GI_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	pAd->ate.TxWI.ShortGI = simple_strtol(arg, 0, 10);
+
+	if(pAd->ate.TxWI.ShortGI > 1)
+	{
+		pAd->ate.TxWI.ShortGI = 0;
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_GI_Proc::Out of range\n"));
+		return FALSE;
+	}
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_TX_GI_Proc (GI = %d)\n", pAd->ate.TxWI.ShortGI));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_GI_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+    ==========================================================================
+ */
+INT	Set_ATE_RX_FER_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	pAd->ate.bRxFer = simple_strtol(arg, 0, 10);
+
+	if (pAd->ate.bRxFer == 1)
+	{
+		pAd->ate.RxCntPerSec = 0;
+		pAd->ate.RxTotalCnt = 0;
+	}
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_RX_FER_Proc (bRxFer = %d)\n", pAd->ate.bRxFer));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_RX_FER_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+INT Set_ATE_Read_RF_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ate_print(KERN_EMERG "R1 = %lx\n", pAd->LatchRfRegs.R1);
+	ate_print(KERN_EMERG "R2 = %lx\n", pAd->LatchRfRegs.R2);
+	ate_print(KERN_EMERG "R3 = %lx\n", pAd->LatchRfRegs.R3);
+	ate_print(KERN_EMERG "R4 = %lx\n", pAd->LatchRfRegs.R4);
+
+	return TRUE;
+}
+
+INT Set_ATE_Write_RF1_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UINT32 value = simple_strtol(arg, 0, 16);
+
+	pAd->LatchRfRegs.R1 = value;
+	RtmpRfIoWrite(pAd);
+
+	return TRUE;
+}
+
+INT Set_ATE_Write_RF2_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UINT32 value = simple_strtol(arg, 0, 16);
+
+	pAd->LatchRfRegs.R2 = value;
+	RtmpRfIoWrite(pAd);
+
+	return TRUE;
+}
+
+INT Set_ATE_Write_RF3_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UINT32 value = simple_strtol(arg, 0, 16);
+
+	pAd->LatchRfRegs.R3 = value;
+	RtmpRfIoWrite(pAd);
+
+	return TRUE;
+}
+
+INT Set_ATE_Write_RF4_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UINT32 value = simple_strtol(arg, 0, 16);
+
+	pAd->LatchRfRegs.R4 = value;
+	RtmpRfIoWrite(pAd);
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Load and Write EEPROM from a binary file prepared in advance.
+
+        Return:
+        	TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+#ifndef UCOS
+INT Set_ATE_Load_E2P_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	BOOLEAN		    ret = FALSE;
+	PUCHAR			src = EEPROM_BIN_FILE_NAME;
+	struct file		*srcf;
+	INT32 			retval, orgfsuid, orgfsgid;
+   	mm_segment_t	orgfs;
+	USHORT 			WriteEEPROM[(EEPROM_SIZE/2)];
+	UINT32			FileLength = 0;
+	UINT32 			value = simple_strtol(arg, 0, 10);
+
+	ATEDBGPRINT(RT_DEBUG_ERROR, ("===> %s (value=%d)\n\n", __FUNCTION__, value));
+
+	if (value > 0)
+	{
+		/* zero the e2p buffer */
+		NdisZeroMemory((PUCHAR)WriteEEPROM, EEPROM_SIZE);
+
+		/* save uid and gid used for filesystem access.
+	    ** set user and group to 0 (root)
+	    */
+		orgfsuid = current->fsuid;
+		orgfsgid = current->fsgid;
+		/* as root */
+		current->fsuid = current->fsgid = 0;
+    	orgfs = get_fs();
+    	set_fs(KERNEL_DS);
+
+		do
+		{
+			/* open the bin file */
+			srcf = filp_open(src, O_RDONLY, 0);
+
+			if (IS_ERR(srcf))
+			{
+				ate_print("%s - Error %ld opening %s\n", __FUNCTION__, -PTR_ERR(srcf), src);
+				break;
+			}
+
+			/* the object must have a read method */
+			if ((srcf->f_op == NULL) || (srcf->f_op->read == NULL))
+			{
+				ate_print("%s - %s does not have a read method\n", __FUNCTION__, src);
+				break;
+			}
+
+			/* read the firmware from the file *.bin */
+			FileLength = srcf->f_op->read(srcf,
+										  (PUCHAR)WriteEEPROM,
+										  EEPROM_SIZE,
+										  &srcf->f_pos);
+
+			if (FileLength != EEPROM_SIZE)
+			{
+				ate_print("%s: error file length (=%d) in e2p.bin\n",
+					   __FUNCTION__, FileLength);
+				break;
+			}
+			else
+			{
+				/* write the content of .bin file to EEPROM */
+				rt_ee_write_all(pAd, WriteEEPROM);
+				ret = TRUE;
+			}
+			break;
+		} while(TRUE);
+
+		/* close firmware file */
+		if (IS_ERR(srcf))
+		{
+				;
+		}
+		else
+		{
+			retval = filp_close(srcf, NULL);
+			if (retval)
+			{
+				ATEDBGPRINT(RT_DEBUG_ERROR, ("--> Error %d closing %s\n", -retval, src));
+
+			}
+		}
+
+		/* restore */
+		set_fs(orgfs);
+		current->fsuid = orgfsuid;
+		current->fsgid = orgfsgid;
+	}
+    ATEDBGPRINT(RT_DEBUG_ERROR, ("<=== %s (ret=%d)\n", __FUNCTION__, ret));
+
+    return ret;
+
+}
+#else
+INT Set_ATE_Load_E2P_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	USHORT 			WriteEEPROM[(EEPROM_SIZE/2)];
+	struct iwreq	*wrq = (struct iwreq *)arg;
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("===> %s (wrq->u.data.length = %d)\n\n", __FUNCTION__, wrq->u.data.length));
+
+	if (wrq->u.data.length != EEPROM_SIZE)
+	{
+		ate_print("%s: error length (=%d) from host\n",
+			   __FUNCTION__, wrq->u.data.length);
+		return FALSE;
+	}
+	else/* (wrq->u.data.length == EEPROM_SIZE) */
+	{
+		/* zero the e2p buffer */
+		NdisZeroMemory((PUCHAR)WriteEEPROM, EEPROM_SIZE);
+
+		/* fill the local buffer */
+		NdisMoveMemory((PUCHAR)WriteEEPROM, wrq->u.data.pointer, wrq->u.data.length);
+
+		do
+		{
+				/* write the content of .bin file to EEPROM */
+				rt_ee_write_all(pAd, WriteEEPROM);
+
+		} while(FALSE);
+		}
+
+    ATEDBGPRINT(RT_DEBUG_TRACE, ("<=== %s\n", __FUNCTION__));
+
+    return TRUE;
+
+}
+#endif // !UCOS //
+
+INT Set_ATE_Read_E2P_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	USHORT buffer[EEPROM_SIZE/2];
+	USHORT *p;
+	int i;
+
+	rt_ee_read_all(pAd, (USHORT *)buffer);
+	p = buffer;
+	for (i = 0; i < (EEPROM_SIZE/2); i++)
+	{
+		ate_print("%4.4x ", *p);
+		if (((i+1) % 16) == 0)
+			ate_print("\n");
+		p++;
+	}
+	return TRUE;
+}
+
+INT	Set_ATE_Show_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ate_print("Mode=%d\n", pAd->ate.Mode);
+	ate_print("TxPower0=%d\n", pAd->ate.TxPower0);
+	ate_print("TxPower1=%d\n", pAd->ate.TxPower1);
+	ate_print("TxAntennaSel=%d\n", pAd->ate.TxAntennaSel);
+	ate_print("RxAntennaSel=%d\n", pAd->ate.RxAntennaSel);
+	ate_print("BBPCurrentBW=%d\n", pAd->ate.TxWI.BW);
+	ate_print("GI=%d\n", pAd->ate.TxWI.ShortGI);
+	ate_print("MCS=%d\n", pAd->ate.TxWI.MCS);
+	ate_print("TxMode=%d\n", pAd->ate.TxWI.PHYMODE);
+	ate_print("Addr1=%02x:%02x:%02x:%02x:%02x:%02x\n",
+		pAd->ate.Addr1[0], pAd->ate.Addr1[1], pAd->ate.Addr1[2], pAd->ate.Addr1[3], pAd->ate.Addr1[4], pAd->ate.Addr1[5]);
+	ate_print("Addr2=%02x:%02x:%02x:%02x:%02x:%02x\n",
+		pAd->ate.Addr2[0], pAd->ate.Addr2[1], pAd->ate.Addr2[2], pAd->ate.Addr2[3], pAd->ate.Addr2[4], pAd->ate.Addr2[5]);
+	ate_print("Addr3=%02x:%02x:%02x:%02x:%02x:%02x\n",
+		pAd->ate.Addr3[0], pAd->ate.Addr3[1], pAd->ate.Addr3[2], pAd->ate.Addr3[3], pAd->ate.Addr3[4], pAd->ate.Addr3[5]);
+	ate_print("Channel=%d\n", pAd->ate.Channel);
+	ate_print("TxLength=%d\n", pAd->ate.TxLength);
+	ate_print("TxCount=%u\n", pAd->ate.TxCount);
+	ate_print("RFFreqOffset=%d\n", pAd->ate.RFFreqOffset);
+	ate_print(KERN_EMERG "Set_ATE_Show_Proc Success\n");
+	return TRUE;
+}
+
+INT	Set_ATE_Help_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ate_print("ATE=ATESTART, ATESTOP, TXCONT, TXCARR, TXFRAME, RXFRAME\n");
+	ate_print("ATEDA\n");
+	ate_print("ATESA\n");
+	ate_print("ATEBSSID\n");
+	ate_print("ATECHANNEL, range:0~14(unless A band !)\n");
+	ate_print("ATETXPOW0, set power level of antenna 1.\n");
+	ate_print("ATETXPOW1, set power level of antenna 2.\n");
+	ate_print("ATETXANT, set TX antenna. 0:all, 1:antenna one, 2:antenna two.\n");
+	ate_print("ATERXANT, set RX antenna.0:all, 1:antenna one, 2:antenna two, 3:antenna three.\n");
+	ate_print("ATETXFREQOFFSET, set frequency offset, range 0~63\n");
+	ate_print("ATETXBW, set BandWidth, 0:20MHz, 1:40MHz.\n");
+	ate_print("ATETXLEN, set Frame length, range 24~%d\n", (MAX_FRAME_SIZE - 34/* == 2312 */));
+	ate_print("ATETXCNT, set how many frame going to transmit.\n");
+	ate_print("ATETXMCS, set MCS, reference to rate table.\n");
+	ate_print("ATETXMODE, set Mode 0:CCK, 1:OFDM, 2:HT-Mix, 3:GreenField, reference to rate table.\n");
+	ate_print("ATETXGI, set GI interval, 0:Long, 1:Short\n");
+	ate_print("ATERXFER, 0:disable Rx Frame error rate. 1:enable Rx Frame error rate.\n");
+	ate_print("ATERRF, show all RF registers.\n");
+	ate_print("ATEWRF1, set RF1 register.\n");
+	ate_print("ATEWRF2, set RF2 register.\n");
+	ate_print("ATEWRF3, set RF3 register.\n");
+	ate_print("ATEWRF4, set RF4 register.\n");
+	ate_print("ATELDE2P, load EEPROM from .bin file.\n");
+	ate_print("ATERE2P, display all EEPROM content.\n");
+	ate_print("ATESHOW, display all parameters of ATE.\n");
+	ate_print("ATEHELP, online help.\n");
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	AsicSwitchChannel() dedicated for ATE.
+
+    ==========================================================================
+*/
+VOID ATEAsicSwitchChannel(
+    IN PRTMP_ADAPTER pAd)
+{
+	UINT32 R2 = 0, R3 = DEFAULT_RF_TX_POWER, R4 = 0, Value = 0;
+	CHAR TxPwer = 0, TxPwer2 = 0;
+	UCHAR index, BbpValue = 0, R66 = 0x30;
+	RTMP_RF_REGS *RFRegTable;
+	UCHAR Channel;
+
+#ifdef RALINK_28xx_QA
+	if ((pAd->ate.bQATxStart == TRUE) || (pAd->ate.bQARxStart == TRUE))
+	{
+		if (pAd->ate.Channel != pAd->LatchRfRegs.Channel)
+		{
+			pAd->ate.Channel = pAd->LatchRfRegs.Channel;
+		}
+		return;
+	}
+	else
+#endif // RALINK_28xx_QA //
+	Channel = pAd->ate.Channel;
+
+	// Select antenna
+	AsicAntennaSelect(pAd, Channel);
+
+	// fill Tx power value
+	TxPwer = pAd->ate.TxPower0;
+	TxPwer2 = pAd->ate.TxPower1;
+
+	RFRegTable = RF2850RegTable;
+
+	switch (pAd->RfIcType)
+	{
+		/* But only 2850 and 2750 support 5.5GHz band... */
+		case RFIC_2820:
+		case RFIC_2850:
+		case RFIC_2720:
+		case RFIC_2750:
+
+			for (index = 0; index < NUM_OF_2850_CHNL; index++)
+			{
+				if (Channel == RFRegTable[index].Channel)
+				{
+					R2 = RFRegTable[index].R2;
+					if (pAd->Antenna.field.TxPath == 1)
+					{
+						R2 |= 0x4000;	// If TXpath is 1, bit 14 = 1;
+					}
+
+					if (pAd->Antenna.field.RxPath == 2)
+					{
+						switch (pAd->ate.RxAntennaSel)
+						{
+							case 1:
+								R2 |= 0x20040;
+								ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BbpValue);
+								BbpValue &= 0xE4;
+								BbpValue |= 0x00;
+								ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BbpValue);
+								break;
+							case 2:
+								R2 |= 0x10040;
+								ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BbpValue);
+								BbpValue &= 0xE4;
+								BbpValue |= 0x01;
+								ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BbpValue);
+								break;
+							default:
+								R2 |= 0x40;
+								ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BbpValue);
+								BbpValue &= 0xE4;
+								/* Only enable two Antenna to receive. */
+								BbpValue |= 0x08;
+								ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BbpValue);
+								break;
+						}
+					}
+					else if (pAd->Antenna.field.RxPath == 1)
+					{
+						R2 |= 0x20040;	// write 1 to off RxPath
+					}
+
+					if (pAd->Antenna.field.TxPath == 2)
+					{
+						if (pAd->ate.TxAntennaSel == 1)
+						{
+							R2 |= 0x4000;	// If TX Antenna select is 1 , bit 14 = 1; Disable Ant 2
+							ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R1, &BbpValue);
+							BbpValue &= 0xE7;		//11100111B
+							ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R1, BbpValue);
+						}
+						else if (pAd->ate.TxAntennaSel == 2)
+						{
+							R2 |= 0x8000;	// If TX Antenna select is 2 , bit 15 = 1; Disable Ant 1
+							ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R1, &BbpValue);
+							BbpValue &= 0xE7;
+							BbpValue |= 0x08;
+							ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R1, BbpValue);
+						}
+						else
+						{
+							ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R1, &BbpValue);
+							BbpValue &= 0xE7;
+							BbpValue |= 0x10;
+							ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R1, BbpValue);
+						}
+					}
+					if (pAd->Antenna.field.RxPath == 3)
+					{
+						switch (pAd->ate.RxAntennaSel)
+						{
+							case 1:
+								R2 |= 0x20040;
+								ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BbpValue);
+								BbpValue &= 0xE4;
+								BbpValue |= 0x00;
+								ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BbpValue);
+								break;
+							case 2:
+								R2 |= 0x10040;
+								ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BbpValue);
+								BbpValue &= 0xE4;
+								BbpValue |= 0x01;
+								ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BbpValue);
+								break;
+							case 3:
+								R2 |= 0x30000;
+								ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BbpValue);
+								BbpValue &= 0xE4;
+								BbpValue |= 0x02;
+								ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BbpValue);
+								break;
+							default:
+								ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BbpValue);
+								BbpValue &= 0xE4;
+								BbpValue |= 0x10;
+								ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BbpValue);
+								break;
+						}
+					}
+
+					if (Channel > 14)
+					{
+						// initialize R3, R4
+						R3 = (RFRegTable[index].R3 & 0xffffc1ff);
+						R4 = (RFRegTable[index].R4 & (~0x001f87c0)) | (pAd->ate.RFFreqOffset << 15);
+
+                        // According the Rory's suggestion to solve the middle range issue.
+						// 5.5G band power range: 0xF9~0X0F, TX0 Reg3 bit9/TX1 Reg4 bit6="0" means the TX power reduce 7dB
+						// R3
+						if ((TxPwer >= -7) && (TxPwer < 0))
+						{
+							TxPwer = (7+TxPwer);
+							TxPwer = (TxPwer > 0xF) ? (0xF) : (TxPwer);
+							R3 |= (TxPwer << 10);
+							ATEDBGPRINT(RT_DEBUG_TRACE, ("ATEAsicSwitchChannel: TxPwer=%d \n", TxPwer));
+						}
+						else
+						{
+							TxPwer = (TxPwer > 0xF) ? (0xF) : (TxPwer);
+							R3 |= (TxPwer << 10) | (1 << 9);
+						}
+
+						// R4
+						if ((TxPwer2 >= -7) && (TxPwer2 < 0))
+						{
+							TxPwer2 = (7+TxPwer2);
+							TxPwer2 = (TxPwer2 > 0xF) ? (0xF) : (TxPwer2);
+							R4 |= (TxPwer2 << 7);
+							ATEDBGPRINT(RT_DEBUG_TRACE, ("ATEAsicSwitchChannel: TxPwer2=%d \n", TxPwer2));
+						}
+						else
+						{
+							TxPwer2 = (TxPwer2 > 0xF) ? (0xF) : (TxPwer2);
+							R4 |= (TxPwer2 << 7) | (1 << 6);
+						}
+					}
+					else
+					{
+						R3 = (RFRegTable[index].R3 & 0xffffc1ff) | (TxPwer << 9); // set TX power0
+						R4 = (RFRegTable[index].R4 & (~0x001f87c0)) | (pAd->ate.RFFreqOffset << 15) | (TxPwer2 <<6);// Set freq offset & TxPwr1
+					}
+
+					// Based on BBP current mode before changing RF channel.
+					if (pAd->ate.TxWI.BW == BW_40)
+					{
+						R4 |=0x200000;
+					}
+
+					// Update variables
+					pAd->LatchRfRegs.Channel = Channel;
+					pAd->LatchRfRegs.R1 = RFRegTable[index].R1;
+					pAd->LatchRfRegs.R2 = R2;
+					pAd->LatchRfRegs.R3 = R3;
+					pAd->LatchRfRegs.R4 = R4;
+
+					RtmpRfIoWrite(pAd);
+
+					break;
+				}
+			}
+			break;
+
+		default:
+			break;
+	}
+
+	// Change BBP setting during switch from a->g, g->a
+	if (Channel <= 14)
+	{
+	    ULONG	TxPinCfg = 0x00050F0A;// 2007.10.09 by Brian : 0x0005050A ==> 0x00050F0A
+
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R62, (0x37 - GET_LNA_GAIN(pAd)));
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R63, (0x37 - GET_LNA_GAIN(pAd)));
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R64, (0x37 - GET_LNA_GAIN(pAd)));
+
+		/* For 1T/2R chip only... */
+	    if (pAd->NicConfig2.field.ExternalLNAForG)
+	    {
+	        ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R82, 0x62);
+	    }
+	    else
+	    {
+	        ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R82, 0x84);
+	    }
+
+        // According the Rory's suggestion to solve the middle range issue.
+		ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R86, &BbpValue);
+		ASSERT((BbpValue == 0x00));
+		if ((BbpValue != 0x00))
+		{
+			ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R86, 0x00);
+		}
+
+		// 5.5GHz band selection PIN, bit1 and bit2 are complement
+		RTMP_IO_READ32(pAd, TX_BAND_CFG, &Value);
+		Value &= (~0x6);
+		Value |= (0x04);
+		RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Value);
+
+        // Turn off unused PA or LNA when only 1T or 1R.
+		if (pAd->Antenna.field.TxPath == 1)
+		{
+			TxPinCfg &= 0xFFFFFFF3;
+		}
+		if (pAd->Antenna.field.RxPath == 1)
+		{
+			TxPinCfg &= 0xFFFFF3FF;
+		}
+
+		RTMP_IO_WRITE32(pAd, TX_PIN_CFG, TxPinCfg);
+	}
+	else
+	{
+	    ULONG	TxPinCfg = 0x00050F05;//2007.10.09 by Brian : 0x00050505 ==> 0x00050F05
+
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R62, (0x37 - GET_LNA_GAIN(pAd)));
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R63, (0x37 - GET_LNA_GAIN(pAd)));
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R64, (0x37 - GET_LNA_GAIN(pAd)));
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R82, 0xF2);
+
+        // According the Rory's suggestion to solve the middle range issue.
+		ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R86, &BbpValue);
+		ASSERT((BbpValue == 0x00));
+		if ((BbpValue != 0x00))
+		{
+			ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R86, 0x00);
+		}
+		ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R91, &BbpValue);
+		ASSERT((BbpValue == 0x04));
+
+		ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R92, &BbpValue);
+		ASSERT((BbpValue == 0x00));
+
+		// 5.5GHz band selection PIN, bit1 and bit2 are complement
+		RTMP_IO_READ32(pAd, TX_BAND_CFG, &Value);
+		Value &= (~0x6);
+		Value |= (0x02);
+		RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Value);
+
+        // Turn off unused PA or LNA when only 1T or 1R.
+		if (pAd->Antenna.field.TxPath == 1)
+		{
+			TxPinCfg &= 0xFFFFFFF3;
+	    }
+		if (pAd->Antenna.field.RxPath == 1)
+		{
+			TxPinCfg &= 0xFFFFF3FF;
+		}
+
+		RTMP_IO_WRITE32(pAd, TX_PIN_CFG, TxPinCfg);
+	}
+
+    // R66 should be set according to Channel and use 20MHz when scanning
+	if (Channel <= 14)
+	{
+		// BG band
+		R66 = 0x2E + GET_LNA_GAIN(pAd);
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+	}
+	else
+	{
+		// 5.5 GHz band
+		if (pAd->ate.TxWI.BW == BW_20)
+		{
+			R66 = (UCHAR)(0x32 + (GET_LNA_GAIN(pAd)*5)/3);
+    		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+		}
+		else
+		{
+			R66 = (UCHAR)(0x3A + (GET_LNA_GAIN(pAd)*5)/3);
+			ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+		}
+	}
+
+	//
+	// On 11A, We should delay and wait RF/BBP to be stable
+	// and the appropriate time should be 1000 micro seconds
+	// 2005/06/05 - On 11G, We also need this delay time. Otherwise it's difficult to pass the WHQL.
+	//
+	RTMPusecDelay(1000);
+
+	if (Channel > 14)
+	{
+		// When 5.5GHz band the LSB of TxPwr will be used to reduced 7dB or not.
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("SwitchChannel#%d(RF=%d, %dT) to , R1=0x%08lx, R2=0x%08lx, R3=0x%08lx, R4=0x%08lx\n",
+								  Channel,
+								  pAd->RfIcType,
+								  pAd->Antenna.field.TxPath,
+								  pAd->LatchRfRegs.R1,
+								  pAd->LatchRfRegs.R2,
+								  pAd->LatchRfRegs.R3,
+								  pAd->LatchRfRegs.R4));
+	}
+	else
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("SwitchChannel#%d(RF=%d, Pwr0=%u, Pwr1=%u, %dT) to , R1=0x%08lx, R2=0x%08lx, R3=0x%08lx, R4=0x%08lx\n",
+								  Channel,
+								  pAd->RfIcType,
+								  (R3 & 0x00003e00) >> 9,
+								  (R4 & 0x000007c0) >> 6,
+								  pAd->Antenna.field.TxPath,
+								  pAd->LatchRfRegs.R1,
+								  pAd->LatchRfRegs.R2,
+								  pAd->LatchRfRegs.R3,
+								  pAd->LatchRfRegs.R4));
+    }
+}
+
+//
+// In fact, no one will call this routine so far !
+//
+/*
+	==========================================================================
+	Description:
+		Gives CCK TX rate 2 more dB TX power.
+		This routine works only in ATE mode.
+
+		calculate desired Tx power in RF R3.Tx0~5,	should consider -
+		0. if current radio is a noisy environment (pAd->DrsCounters.fNoisyEnvironment)
+		1. TxPowerPercentage
+		2. auto calibration based on TSSI feedback
+		3. extra 2 db for CCK
+		4. -10 db upon very-short distance (AvgRSSI >= -40db) to AP
+
+	NOTE: Since this routine requires the value of (pAd->DrsCounters.fNoisyEnvironment),
+		it should be called AFTER MlmeDynamicTxRateSwitching()
+	==========================================================================
+ */
+VOID ATEAsicAdjustTxPower(
+	IN PRTMP_ADAPTER pAd)
+{
+	INT			i, j;
+	CHAR		DeltaPwr = 0;
+	BOOLEAN		bAutoTxAgc = FALSE;
+	UCHAR		TssiRef, *pTssiMinusBoundary, *pTssiPlusBoundary, TxAgcStep;
+	UCHAR		BbpR49 = 0, idx;
+	PCHAR		pTxAgcCompensate;
+	ULONG		TxPwr[5];
+	CHAR		Value;
+
+	/* no one calls this procedure so far */
+	if (pAd->ate.TxWI.BW == BW_40)
+	{
+		if (pAd->ate.Channel > 14)
+		{
+			TxPwr[0] = pAd->Tx40MPwrCfgABand[0];
+			TxPwr[1] = pAd->Tx40MPwrCfgABand[1];
+			TxPwr[2] = pAd->Tx40MPwrCfgABand[2];
+			TxPwr[3] = pAd->Tx40MPwrCfgABand[3];
+			TxPwr[4] = pAd->Tx40MPwrCfgABand[4];
+		}
+		else
+		{
+			TxPwr[0] = pAd->Tx40MPwrCfgGBand[0];
+			TxPwr[1] = pAd->Tx40MPwrCfgGBand[1];
+			TxPwr[2] = pAd->Tx40MPwrCfgGBand[2];
+			TxPwr[3] = pAd->Tx40MPwrCfgGBand[3];
+			TxPwr[4] = pAd->Tx40MPwrCfgGBand[4];
+		}
+	}
+	else
+	{
+		if (pAd->ate.Channel > 14)
+		{
+			TxPwr[0] = pAd->Tx20MPwrCfgABand[0];
+			TxPwr[1] = pAd->Tx20MPwrCfgABand[1];
+			TxPwr[2] = pAd->Tx20MPwrCfgABand[2];
+			TxPwr[3] = pAd->Tx20MPwrCfgABand[3];
+			TxPwr[4] = pAd->Tx20MPwrCfgABand[4];
+		}
+		else
+		{
+			TxPwr[0] = pAd->Tx20MPwrCfgGBand[0];
+			TxPwr[1] = pAd->Tx20MPwrCfgGBand[1];
+			TxPwr[2] = pAd->Tx20MPwrCfgGBand[2];
+			TxPwr[3] = pAd->Tx20MPwrCfgGBand[3];
+			TxPwr[4] = pAd->Tx20MPwrCfgGBand[4];
+		}
+	}
+
+	// TX power compensation for temperature variation based on TSSI.
+	// Do it per 4 seconds.
+	if (pAd->Mlme.OneSecPeriodicRound % 4 == 0)
+	{
+		if (pAd->ate.Channel <= 14)
+		{
+			/* bg channel */
+			bAutoTxAgc         = pAd->bAutoTxAgcG;
+			TssiRef            = pAd->TssiRefG;
+			pTssiMinusBoundary = &pAd->TssiMinusBoundaryG[0];
+			pTssiPlusBoundary  = &pAd->TssiPlusBoundaryG[0];
+			TxAgcStep          = pAd->TxAgcStepG;
+			pTxAgcCompensate   = &pAd->TxAgcCompensateG;
+		}
+		else
+		{
+			/* a channel */
+			bAutoTxAgc         = pAd->bAutoTxAgcA;
+			TssiRef            = pAd->TssiRefA;
+			pTssiMinusBoundary = &pAd->TssiMinusBoundaryA[0];
+			pTssiPlusBoundary  = &pAd->TssiPlusBoundaryA[0];
+			TxAgcStep          = pAd->TxAgcStepA;
+			pTxAgcCompensate   = &pAd->TxAgcCompensateA;
+		}
+
+		if (bAutoTxAgc)
+		{
+			/* BbpR49 is unsigned char */
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R49, &BbpR49);
+
+			/* (p) TssiPlusBoundaryG[0] = 0 = (m) TssiMinusBoundaryG[0] */
+			/* compensate: +4     +3   +2   +1    0   -1   -2   -3   -4 * steps */
+			/* step value is defined in pAd->TxAgcStepG for tx power value */
+
+			/* [4]+1+[4]   p4     p3   p2   p1   o1   m1   m2   m3   m4 */
+			/* ex:         0x00 0x15 0x25 0x45 0x88 0xA0 0xB5 0xD0 0xF0
+			   above value are examined in mass factory production */
+			/*             [4]    [3]  [2]  [1]  [0]  [1]  [2]  [3]  [4] */
+
+			/* plus is 0x10 ~ 0x40, minus is 0x60 ~ 0x90 */
+			/* if value is between p1 ~ o1 or o1 ~ s1, no need to adjust tx power */
+			/* if value is 0x65, tx power will be -= TxAgcStep*(2-1) */
+
+			if (BbpR49 > pTssiMinusBoundary[1])
+			{
+				// Reading is larger than the reference value.
+				// Check for how large we need to decrease the Tx power.
+				for (idx = 1; idx < 5; idx++)
+				{
+					if (BbpR49 <= pTssiMinusBoundary[idx])  // Found the range
+						break;
+				}
+				// The index is the step we should decrease, idx = 0 means there is nothing to compensate
+//				if (R3 > (ULONG) (TxAgcStep * (idx-1)))
+					*pTxAgcCompensate = -(TxAgcStep * (idx-1));
+//				else
+//					*pTxAgcCompensate = -((UCHAR)R3);
+
+				DeltaPwr += (*pTxAgcCompensate);
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("-- Tx Power, BBP R1=%x, TssiRef=%x, TxAgcStep=%x, step = -%d\n",
+					BbpR49, TssiRef, TxAgcStep, idx-1));
+			}
+			else if (BbpR49 < pTssiPlusBoundary[1])
+			{
+				// Reading is smaller than the reference value
+				// check for how large we need to increase the Tx power
+				for (idx = 1; idx < 5; idx++)
+				{
+					if (BbpR49 >= pTssiPlusBoundary[idx])   // Found the range
+						break;
+				}
+				// The index is the step we should increase, idx = 0 means there is nothing to compensate
+				*pTxAgcCompensate = TxAgcStep * (idx-1);
+				DeltaPwr += (*pTxAgcCompensate);
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("++ Tx Power, BBP R1=%x, TssiRef=%x, TxAgcStep=%x, step = +%d\n",
+					BbpR49, TssiRef, TxAgcStep, idx-1));
+			}
+			else
+			{
+				*pTxAgcCompensate = 0;
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("   Tx Power, BBP R1=%x, TssiRef=%x, TxAgcStep=%x, step = +%d\n",
+					BbpR49, TssiRef, TxAgcStep, 0));
+			}
+		}
+	}
+	else
+	{
+		if (pAd->ate.Channel <= 14)
+		{
+			bAutoTxAgc         = pAd->bAutoTxAgcG;
+			pTxAgcCompensate   = &pAd->TxAgcCompensateG;
+		}
+		else
+		{
+			bAutoTxAgc         = pAd->bAutoTxAgcA;
+			pTxAgcCompensate   = &pAd->TxAgcCompensateA;
+		}
+
+		if (bAutoTxAgc)
+			DeltaPwr += (*pTxAgcCompensate);
+	}
+
+	/* calculate delta power based on the percentage specified from UI */
+	// E2PROM setting is calibrated for maximum TX power (i.e. 100%)
+	// We lower TX power here according to the percentage specified from UI
+	if (pAd->CommonCfg.TxPowerPercentage == 0xffffffff)       // AUTO TX POWER control
+		;
+	else if (pAd->CommonCfg.TxPowerPercentage > 90)  // 91 ~ 100% & AUTO, treat as 100% in terms of mW
+		;
+	else if (pAd->CommonCfg.TxPowerPercentage > 60)  // 61 ~ 90%, treat as 75% in terms of mW
+	{
+		DeltaPwr -= 1;
+	}
+	else if (pAd->CommonCfg.TxPowerPercentage > 30)  // 31 ~ 60%, treat as 50% in terms of mW
+	{
+		DeltaPwr -= 3;
+	}
+	else if (pAd->CommonCfg.TxPowerPercentage > 15)  // 16 ~ 30%, treat as 25% in terms of mW
+	{
+		DeltaPwr -= 6;
+	}
+	else if (pAd->CommonCfg.TxPowerPercentage > 9)   // 10 ~ 15%, treat as 12.5% in terms of mW
+	{
+		DeltaPwr -= 9;
+	}
+	else                                           // 0 ~ 9 %, treat as MIN(~3%) in terms of mW
+	{
+		DeltaPwr -= 12;
+	}
+
+	/* reset different new tx power for different TX rate */
+	for(i=0; i<5; i++)
+	{
+		if (TxPwr[i] != 0xffffffff)
+		{
+			for (j=0; j<8; j++)
+			{
+				Value = (CHAR)((TxPwr[i] >> j*4) & 0x0F); /* 0 ~ 15 */
+
+				if ((Value + DeltaPwr) < 0)
+				{
+					Value = 0; /* min */
+				}
+				else if ((Value + DeltaPwr) > 0xF)
+				{
+					Value = 0xF; /* max */
+				}
+				else
+				{
+					Value += DeltaPwr; /* temperature compensation */
+				}
+
+				/* fill new value to CSR offset */
+				TxPwr[i] = (TxPwr[i] & ~(0x0000000F << j*4)) | (Value << j*4);
+			}
+
+			/* write tx power value to CSR */
+			/* TX_PWR_CFG_0 (8 tx rate) for	TX power for OFDM 12M/18M
+											TX power for OFDM 6M/9M
+											TX power for CCK5.5M/11M
+											TX power for CCK1M/2M */
+			/* TX_PWR_CFG_1 ~ TX_PWR_CFG_4 */
+			RTMP_IO_WRITE32(pAd, TX_PWR_CFG_0 + i*4, TxPwr[i]);
+
+
+		}
+	}
+
+}
+
+/*
+	========================================================================
+	Routine Description:
+		Write TxWI for ATE mode.
+
+	Return Value:
+		None
+	========================================================================
+*/
+#ifdef RT2860
+static VOID ATEWriteTxWI(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTXWI_STRUC 	pOutTxWI,
+	IN	BOOLEAN			FRAG,
+	IN	BOOLEAN			CFACK,
+	IN	BOOLEAN			InsTimestamp,
+	IN	BOOLEAN 		AMPDU,
+	IN	BOOLEAN 		Ack,
+	IN	BOOLEAN 		NSeq,		// HW new a sequence.
+	IN	UCHAR			BASize,
+	IN	UCHAR			WCID,
+	IN	ULONG			Length,
+	IN	UCHAR 			PID,
+	IN	UCHAR			TID,
+	IN	UCHAR			TxRate,
+	IN	UCHAR			Txopmode,
+	IN	BOOLEAN			CfAck,
+	IN	HTTRANSMIT_SETTING	*pTransmit)
+{
+	TXWI_STRUC 		TxWI;
+	PTXWI_STRUC 	pTxWI;
+
+	//
+	// Always use Long preamble before verifiation short preamble functionality works well.
+	// Todo: remove the following line if short preamble functionality works
+	//
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED);
+	NdisZeroMemory(&TxWI, TXWI_SIZE);
+	pTxWI = &TxWI;
+
+	pTxWI->FRAG= FRAG;
+
+	pTxWI->CFACK = CFACK;
+	pTxWI->TS= InsTimestamp;
+	pTxWI->AMPDU = AMPDU;
+	pTxWI->ACK = Ack;
+	pTxWI->txop= Txopmode;
+
+	pTxWI->NSEQ = NSeq;
+	// John tune the performace with Intel Client in 20 MHz performance
+	if( BASize >7 )
+		BASize =7;
+
+	pTxWI->BAWinSize = BASize;
+	pTxWI->WirelessCliID = WCID;
+	pTxWI->MPDUtotalByteCount = Length;
+	pTxWI->PacketId = PID;
+
+	// If CCK or OFDM, BW must be 20
+	pTxWI->BW = (pTransmit->field.MODE <= MODE_OFDM) ? (BW_20) : (pTransmit->field.BW);
+	pTxWI->ShortGI = pTransmit->field.ShortGI;
+	pTxWI->STBC = pTransmit->field.STBC;
+
+	pTxWI->MCS = pTransmit->field.MCS;
+	pTxWI->PHYMODE = pTransmit->field.MODE;
+	pTxWI->CFACK = CfAck;
+	pTxWI->MIMOps = 0;
+	pTxWI->MpduDensity = 0;
+
+	pTxWI->PacketId = pTxWI->MCS;
+	NdisMoveMemory(pOutTxWI, &TxWI, sizeof(TXWI_STRUC));
+
+        return;
+}
+#endif // RT2860 //
+
+/*
+	========================================================================
+
+	Routine Description:
+		Disable protection for ATE.
+	========================================================================
+*/
+VOID ATEDisableAsicProtect(
+	IN		PRTMP_ADAPTER	pAd)
+{
+	PROT_CFG_STRUC	ProtCfg, ProtCfg4;
+	UINT32 Protect[6];
+	USHORT			offset;
+	UCHAR			i;
+	UINT32 MacReg = 0;
+
+	// Config ASIC RTS threshold register
+	RTMP_IO_READ32(pAd, TX_RTS_CFG, &MacReg);
+	MacReg &= 0xFF0000FF;
+	MacReg |= (pAd->CommonCfg.RtsThreshold << 8);
+	RTMP_IO_WRITE32(pAd, TX_RTS_CFG, MacReg);
+
+	// Initial common protection settings
+	RTMPZeroMemory(Protect, sizeof(Protect));
+	ProtCfg4.word = 0;
+	ProtCfg.word = 0;
+	ProtCfg.field.TxopAllowGF40 = 1;
+	ProtCfg.field.TxopAllowGF20 = 1;
+	ProtCfg.field.TxopAllowMM40 = 1;
+	ProtCfg.field.TxopAllowMM20 = 1;
+	ProtCfg.field.TxopAllowOfdm = 1;
+	ProtCfg.field.TxopAllowCck = 1;
+	ProtCfg.field.RTSThEn = 1;
+	ProtCfg.field.ProtectNav = ASIC_SHORTNAV;
+
+	// Handle legacy(B/G) protection
+	ProtCfg.field.ProtectRate = pAd->CommonCfg.RtsRate;
+	ProtCfg.field.ProtectCtrl = 0;
+	Protect[0] = ProtCfg.word;
+	Protect[1] = ProtCfg.word;
+
+	// NO PROTECT
+	// 1.All STAs in the BSS are 20/40 MHz HT
+	// 2. in ai 20/40MHz BSS
+	// 3. all STAs are 20MHz in a 20MHz BSS
+	// Pure HT. no protection.
+
+	// MM20_PROT_CFG
+	//	Reserved (31:27)
+	// 	PROT_TXOP(25:20) -- 010111
+	//	PROT_NAV(19:18)  -- 01 (Short NAV protection)
+	//  PROT_CTRL(17:16) -- 00 (None)
+	// 	PROT_RATE(15:0)  -- 0x4004 (OFDM 24M)
+	Protect[2] = 0x01744004;
+
+	// MM40_PROT_CFG
+	//	Reserved (31:27)
+	// 	PROT_TXOP(25:20) -- 111111
+	//	PROT_NAV(19:18)  -- 01 (Short NAV protection)
+	//  PROT_CTRL(17:16) -- 00 (None)
+	// 	PROT_RATE(15:0)  -- 0x4084 (duplicate OFDM 24M)
+	Protect[3] = 0x03f44084;
+
+	// CF20_PROT_CFG
+	//	Reserved (31:27)
+	// 	PROT_TXOP(25:20) -- 010111
+	//	PROT_NAV(19:18)  -- 01 (Short NAV protection)
+	//  PROT_CTRL(17:16) -- 00 (None)
+	// 	PROT_RATE(15:0)  -- 0x4004 (OFDM 24M)
+	Protect[4] = 0x01744004;
+
+	// CF40_PROT_CFG
+	//	Reserved (31:27)
+	// 	PROT_TXOP(25:20) -- 111111
+	//	PROT_NAV(19:18)  -- 01 (Short NAV protection)
+	//  PROT_CTRL(17:16) -- 00 (None)
+	// 	PROT_RATE(15:0)  -- 0x4084 (duplicate OFDM 24M)
+	Protect[5] = 0x03f44084;
+
+	pAd->CommonCfg.IOTestParm.bRTSLongProtOn = FALSE;
+
+	offset = CCK_PROT_CFG;
+	for (i = 0;i < 6;i++)
+		RTMP_IO_WRITE32(pAd, offset + i*4, Protect[i]);
+
+}
+
+
+/* There are two ways to convert Rssi */
+#if 1
+//
+// The way used with GET_LNA_GAIN().
+//
+CHAR ATEConvertToRssi(
+	IN PRTMP_ADAPTER pAd,
+	IN	CHAR	Rssi,
+	IN  UCHAR   RssiNumber)
+{
+	UCHAR	RssiOffset, LNAGain;
+
+	// Rssi equals to zero should be an invalid value
+	if (Rssi == 0)
+		return -99;
+
+	LNAGain = GET_LNA_GAIN(pAd);
+	if (pAd->LatchRfRegs.Channel > 14)
+	{
+		if (RssiNumber == 0)
+			RssiOffset = pAd->ARssiOffset0;
+		else if (RssiNumber == 1)
+			RssiOffset = pAd->ARssiOffset1;
+		else
+			RssiOffset = pAd->ARssiOffset2;
+	}
+	else
+	{
+		if (RssiNumber == 0)
+			RssiOffset = pAd->BGRssiOffset0;
+		else if (RssiNumber == 1)
+			RssiOffset = pAd->BGRssiOffset1;
+		else
+			RssiOffset = pAd->BGRssiOffset2;
+	}
+
+	return (-12 - RssiOffset - LNAGain - Rssi);
+}
+#else
+//
+// The way originally used in ATE of rt2860ap.
+//
+CHAR ATEConvertToRssi(
+	IN PRTMP_ADAPTER pAd,
+	IN	CHAR			Rssi,
+	IN  UCHAR   RssiNumber)
+{
+	UCHAR	RssiOffset, LNAGain;
+
+	// Rssi equals to zero should be an invalid value
+	if (Rssi == 0)
+		return -99;
+
+    if (pAd->LatchRfRegs.Channel > 14)
+    {
+        LNAGain = pAd->ALNAGain;
+        if (RssiNumber == 0)
+			RssiOffset = pAd->ARssiOffset0;
+		else if (RssiNumber == 1)
+			RssiOffset = pAd->ARssiOffset1;
+		else
+			RssiOffset = pAd->ARssiOffset2;
+    }
+    else
+    {
+        LNAGain = pAd->BLNAGain;
+        if (RssiNumber == 0)
+			RssiOffset = pAd->BGRssiOffset0;
+		else if (RssiNumber == 1)
+			RssiOffset = pAd->BGRssiOffset1;
+		else
+			RssiOffset = pAd->BGRssiOffset2;
+    }
+
+    return (-32 - RssiOffset + LNAGain - Rssi);
+}
+#endif /* end of #if 1 */
+
+/*
+	========================================================================
+
+	Routine Description:
+		Set Japan filter coefficients if needed.
+	Note:
+		This routine should only be called when
+		entering TXFRAME mode or TXCONT mode.
+
+	========================================================================
+*/
+static VOID SetJapanFilter(
+	IN		PRTMP_ADAPTER	pAd)
+{
+	UCHAR			BbpData = 0;
+
+	//
+	// If Channel=14 and Bandwidth=20M and Mode=CCK, set BBP R4 bit5=1
+	// (Japan Tx filter coefficients)when (TXFRAME or TXCONT).
+	//
+	ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BbpData);
+
+    if ((pAd->ate.TxWI.PHYMODE == MODE_CCK) && (pAd->ate.Channel == 14) && (pAd->ate.TxWI.BW == BW_20))
+    {
+        BbpData |= 0x20;    // turn on
+        ATEDBGPRINT(RT_DEBUG_TRACE, ("SetJapanFilter!!!\n"));
+    }
+    else
+    {
+		BbpData &= 0xdf;    // turn off
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ClearJapanFilter!!!\n"));
+    }
+
+	ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BbpData);
+}
+
+VOID ATESampleRssi(
+	IN PRTMP_ADAPTER	pAd,
+	IN PRXWI_STRUC		pRxWI)
+{
+	/* There are two ways to collect RSSI. */
+#if 1
+	//pAd->LastRxRate = (USHORT)((pRxWI->MCS) + (pRxWI->BW <<7) + (pRxWI->ShortGI <<8)+ (pRxWI->PHYMODE <<14)) ;
+	if (pRxWI->RSSI0 != 0)
+	{
+		pAd->ate.LastRssi0	= ATEConvertToRssi(pAd, (CHAR) pRxWI->RSSI0, RSSI_0);
+		pAd->ate.AvgRssi0X8	= (pAd->ate.AvgRssi0X8 - pAd->ate.AvgRssi0) + pAd->ate.LastRssi0;
+		pAd->ate.AvgRssi0  	= pAd->ate.AvgRssi0X8 >> 3;
+	}
+	if (pRxWI->RSSI1 != 0)
+	{
+		pAd->ate.LastRssi1	= ATEConvertToRssi(pAd, (CHAR) pRxWI->RSSI1, RSSI_1);
+		pAd->ate.AvgRssi1X8	= (pAd->ate.AvgRssi1X8 - pAd->ate.AvgRssi1) + pAd->ate.LastRssi1;
+		pAd->ate.AvgRssi1	= pAd->ate.AvgRssi1X8 >> 3;
+	}
+	if (pRxWI->RSSI2 != 0)
+	{
+		pAd->ate.LastRssi2	= ATEConvertToRssi(pAd, (CHAR) pRxWI->RSSI2, RSSI_2);
+		pAd->ate.AvgRssi2X8	= (pAd->ate.AvgRssi2X8 - pAd->ate.AvgRssi2) + pAd->ate.LastRssi2;
+		pAd->ate.AvgRssi2	= pAd->ate.AvgRssi2X8 >> 3;
+	}
+
+	pAd->ate.LastSNR0 = (CHAR)(pRxWI->SNR0);// CHAR ==> UCHAR ?
+	pAd->ate.LastSNR1 = (CHAR)(pRxWI->SNR1);// CHAR ==> UCHAR ?
+
+	pAd->ate.NumOfAvgRssiSample ++;
+#else
+	pAd->ate.LastSNR0 = (CHAR)(pRxWI->SNR0);
+	pAd->ate.LastSNR1 = (CHAR)(pRxWI->SNR1);
+	pAd->ate.RxCntPerSec++;
+	pAd->ate.LastRssi0 = ATEConvertToRssi(pAd, (CHAR) pRxWI->RSSI0, RSSI_0);
+	pAd->ate.LastRssi1 = ATEConvertToRssi(pAd, (CHAR) pRxWI->RSSI1, RSSI_1);
+	pAd->ate.LastRssi2 = ATEConvertToRssi(pAd, (CHAR) pRxWI->RSSI2, RSSI_2);
+	pAd->ate.AvgRssi0X8 = (pAd->ate.AvgRssi0X8 - pAd->ate.AvgRssi0) + pAd->ate.LastRssi0;
+	pAd->ate.AvgRssi0 = pAd->ate.AvgRssi0X8 >> 3;
+	pAd->ate.AvgRssi1X8 = (pAd->ate.AvgRssi1X8 - pAd->ate.AvgRssi1) + pAd->ate.LastRssi1;
+	pAd->ate.AvgRssi1 = pAd->ate.AvgRssi1X8 >> 3;
+	pAd->ate.AvgRssi2X8 = (pAd->ate.AvgRssi2X8 - pAd->ate.AvgRssi2) + pAd->ate.LastRssi2;
+	pAd->ate.AvgRssi2 = pAd->ate.AvgRssi2X8 >> 3;
+	pAd->ate.NumOfAvgRssiSample ++;
+#endif
+}
+
+#ifdef CONFIG_STA_SUPPORT
+VOID RTMPStationStop(
+    IN  PRTMP_ADAPTER   pAd)
+{
+//	BOOLEAN       Cancelled;
+
+    ATEDBGPRINT(RT_DEBUG_TRACE, ("==> RTMPStationStop\n"));
+
+#if 0
+	RTMPCancelTimer(&pAd->MlmeAux.AssocTimer,      &Cancelled);
+	RTMPCancelTimer(&pAd->MlmeAux.ReassocTimer,    &Cancelled);
+	RTMPCancelTimer(&pAd->MlmeAux.DisassocTimer,   &Cancelled);
+	RTMPCancelTimer(&pAd->MlmeAux.AuthTimer,       &Cancelled);
+	RTMPCancelTimer(&pAd->MlmeAux.BeaconTimer,     &Cancelled);
+	RTMPCancelTimer(&pAd->MlmeAux.ScanTimer,       &Cancelled);
+#endif
+	// For rx statistics, we need to keep this timer running.
+//	RTMPCancelTimer(&pAd->Mlme.PeriodicTimer,      &Cancelled);
+
+    ATEDBGPRINT(RT_DEBUG_TRACE, ("<== RTMPStationStop\n"));
+}
+
+VOID RTMPStationStart(
+    IN  PRTMP_ADAPTER   pAd)
+{
+    ATEDBGPRINT(RT_DEBUG_TRACE, ("==> RTMPStationStart\n"));
+#ifdef RT2860
+	pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+	//
+	// We did not cancel this timer when entering ATE mode.
+	//
+//	RTMPSetTimer(&pAd->Mlme.PeriodicTimer, MLME_TASK_EXEC_INTV);
+#endif // RT2860 //
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("<== RTMPStationStart\n"));
+}
+#endif // CONFIG_STA_SUPPORT //
+
+/*
+	==========================================================================
+	Description:
+		Setup Frame format.
+	NOTE:
+		This routine should only be used in ATE mode.
+	==========================================================================
+ */
+#ifdef RT2860
+static INT ATESetUpFrame(
+	IN PRTMP_ADAPTER pAd,
+	IN UINT32 TxIdx)
+{
+	UINT j;
+	PTXD_STRUC pTxD;
+#ifdef RT_BIG_ENDIAN
+    PTXD_STRUC      pDestTxD;
+    TXD_STRUC       TxD;
+#endif
+	PNDIS_PACKET pPacket;
+	PUCHAR pDest;
+	PVOID AllocVa;
+	NDIS_PHYSICAL_ADDRESS AllocPa;
+	HTTRANSMIT_SETTING	TxHTPhyMode;
+
+	PRTMP_TX_RING pTxRing = &pAd->TxRing[QID_AC_BE];
+	PTXWI_STRUC pTxWI = (PTXWI_STRUC) pTxRing->Cell[TxIdx].DmaBuf.AllocVa;
+	PUCHAR pDMAHeaderBufVA = (PUCHAR) pTxRing->Cell[TxIdx].DmaBuf.AllocVa;
+
+#ifdef RALINK_28xx_QA
+	PHEADER_802_11	pHeader80211;
+#endif // RALINK_28xx_QA //
+
+	if (pAd->ate.bQATxStart == TRUE)
+	{
+		// always use QID_AC_BE and FIFO_EDCA
+
+		// fill TxWI
+		TxHTPhyMode.field.BW = pAd->ate.TxWI.BW;
+		TxHTPhyMode.field.ShortGI = pAd->ate.TxWI.ShortGI;
+		TxHTPhyMode.field.STBC = 0;
+		TxHTPhyMode.field.MCS = pAd->ate.TxWI.MCS;
+		TxHTPhyMode.field.MODE = pAd->ate.TxWI.PHYMODE;
+		ATEWriteTxWI(pAd, pTxWI, pAd->ate.TxWI.FRAG, pAd->ate.TxWI.CFACK, pAd->ate.TxWI.TS,  pAd->ate.TxWI.AMPDU, pAd->ate.TxWI.ACK, pAd->ate.TxWI.NSEQ,
+			pAd->ate.TxWI.BAWinSize, 0, pAd->ate.TxWI.MPDUtotalByteCount, pAd->ate.TxWI.PacketId, 0, 0, pAd->ate.TxWI.txop/*IFS_HTTXOP*/, pAd->ate.TxWI.CFACK/*FALSE*/, &TxHTPhyMode);
+	}
+	else
+	{
+		TxHTPhyMode.field.BW = pAd->ate.TxWI.BW;
+		TxHTPhyMode.field.ShortGI = pAd->ate.TxWI.ShortGI;
+		TxHTPhyMode.field.STBC = 0;
+		TxHTPhyMode.field.MCS = pAd->ate.TxWI.MCS;
+		TxHTPhyMode.field.MODE = pAd->ate.TxWI.PHYMODE;
+		ATEWriteTxWI(pAd, pTxWI, FALSE, FALSE, FALSE,  FALSE, FALSE, FALSE,
+			4, 0, pAd->ate.TxLength, 0, 0, 0, IFS_HTTXOP, FALSE, &TxHTPhyMode);
+	}
+
+	// fill 802.11 header.
+#ifdef RALINK_28xx_QA
+	if (pAd->ate.bQATxStart == TRUE)
+	{
+		NdisMoveMemory(pDMAHeaderBufVA+TXWI_SIZE, pAd->ate.Header, pAd->ate.HLen);
+	}
+	else
+#endif // RALINK_28xx_QA //
+	{
+		NdisMoveMemory(pDMAHeaderBufVA+TXWI_SIZE, TemplateFrame, LENGTH_802_11);
+		NdisMoveMemory(pDMAHeaderBufVA+TXWI_SIZE+4, pAd->ate.Addr1, ETH_LENGTH_OF_ADDRESS);
+		NdisMoveMemory(pDMAHeaderBufVA+TXWI_SIZE+10, pAd->ate.Addr2, ETH_LENGTH_OF_ADDRESS);
+		NdisMoveMemory(pDMAHeaderBufVA+TXWI_SIZE+16, pAd->ate.Addr3, ETH_LENGTH_OF_ADDRESS);
+	}
+
+#ifdef RT_BIG_ENDIAN
+	RTMPFrameEndianChange(pAd, (((PUCHAR)pDMAHeaderBufVA)+TXWI_SIZE), DIR_READ, FALSE);
+#endif // RT_BIG_ENDIAN //
+
+	/* alloc buffer for payload */
+#ifdef RALINK_28xx_QA
+	if (pAd->ate.bQATxStart == TRUE)
+	{
+		/* Why not use RTMP_AllocateTxPacketBuffer() instead of RTMP_AllocateRxPacketBuffer()? */
+		pPacket = RTMP_AllocateRxPacketBuffer(pAd, pAd->ate.DLen + 0x100, FALSE, &AllocVa, &AllocPa);
+	}
+	else
+#endif // RALINK_28xx_QA //
+	{
+		/* Why not use RTMP_AllocateTxPacketBuffer() instead of RTMP_AllocateRxPacketBuffer()? */
+		pPacket = RTMP_AllocateRxPacketBuffer(pAd, pAd->ate.TxLength, FALSE, &AllocVa, &AllocPa);
+	}
+
+	if (pPacket == NULL)
+	{
+		pAd->ate.TxCount = 0;
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("%s fail to alloc packet space.\n", __FUNCTION__));
+		return -1;
+	}
+	pTxRing->Cell[TxIdx].pNextNdisPacket = pPacket;
+
+	pDest = (PUCHAR) AllocVa;
+
+#ifdef RALINK_28xx_QA
+	if (pAd->ate.bQATxStart == TRUE)
+	{
+		RTPKT_TO_OSPKT(pPacket)->len = pAd->ate.DLen;
+	}
+	else
+#endif // RALINK_28xx_QA //
+	{
+		RTPKT_TO_OSPKT(pPacket)->len = pAd->ate.TxLength - LENGTH_802_11;
+	}
+
+	// Prepare frame payload
+#ifdef RALINK_28xx_QA
+	if (pAd->ate.bQATxStart == TRUE)
+	{
+		// copy pattern
+		if ((pAd->ate.PLen != 0))
+		{
+			int j;
+
+			for (j = 0; j < pAd->ate.DLen; j+=pAd->ate.PLen)
+			{
+				memcpy(RTPKT_TO_OSPKT(pPacket)->data + j, pAd->ate.Pattern, pAd->ate.PLen);
+			}
+		}
+	}
+	else
+#endif // RALINK_28xx_QA //
+	{
+		for(j = 0; j < RTPKT_TO_OSPKT(pPacket)->len; j++)
+			pDest[j] = 0xA5;
+	}
+
+	//
+	// build Tx Descriptor
+	//
+#ifndef RT_BIG_ENDIAN
+	pTxD = (PTXD_STRUC) pTxRing->Cell[TxIdx].AllocVa;
+#else
+    pDestTxD  = (PTXD_STRUC)pTxRing->Cell[TxIdx].AllocVa;
+    TxD = *pDestTxD;
+    pTxD = &TxD;
+#endif // !RT_BIG_ENDIAN //
+
+#ifdef RALINK_28xx_QA
+	if (pAd->ate.bQATxStart == TRUE)
+	{
+		// prepare TxD
+		NdisZeroMemory(pTxD, TXD_SIZE);
+		RTMPWriteTxDescriptor(pAd, pTxD, FALSE, FIFO_EDCA);
+		// build TX DESC
+		pTxD->SDPtr0 = RTMP_GetPhysicalAddressLow(pTxRing->Cell[TxIdx].DmaBuf.AllocPa);
+		pTxD->SDLen0 = TXWI_SIZE + pAd->ate.HLen;
+		pTxD->LastSec0 = 0;
+		pTxD->SDPtr1 = AllocPa;
+		pTxD->SDLen1 = RTPKT_TO_OSPKT(pPacket)->len;
+		pTxD->LastSec1 = 1;
+
+		pDest = (PUCHAR)pTxWI;
+		pDest += TXWI_SIZE;
+		pHeader80211 = (PHEADER_802_11)pDest;
+
+		// modify sequence number....
+		if (pAd->ate.TxDoneCount == 0)
+		{
+			pAd->ate.seq = pHeader80211->Sequence;
+		}
+		else
+			pHeader80211->Sequence = ++pAd->ate.seq;
+	}
+	else
+#endif // RALINK_28xx_QA //
+	{
+		NdisZeroMemory(pTxD, TXD_SIZE);
+		RTMPWriteTxDescriptor(pAd, pTxD, FALSE, FIFO_EDCA);
+		// build TX DESC
+		pTxD->SDPtr0 = RTMP_GetPhysicalAddressLow (pTxRing->Cell[TxIdx].DmaBuf.AllocPa);
+		pTxD->SDLen0 = TXWI_SIZE + LENGTH_802_11;
+		pTxD->LastSec0 = 0;
+		pTxD->SDPtr1 = AllocPa;
+		pTxD->SDLen1 = RTPKT_TO_OSPKT(pPacket)->len;
+		pTxD->LastSec1 = 1;
+	}
+
+#ifdef RT_BIG_ENDIAN
+	RTMPWIEndianChange((PUCHAR)pTxWI, TYPE_TXWI);
+	RTMPFrameEndianChange(pAd, (((PUCHAR)pDMAHeaderBufVA)+TXWI_SIZE), DIR_WRITE, FALSE);
+    RTMPDescriptorEndianChange((PUCHAR)pTxD, TYPE_TXD);
+    WriteBackToDescriptor((PUCHAR)pDestTxD, (PUCHAR)pTxD, FALSE, TYPE_TXD);
+#endif // RT_BIG_ENDIAN //
+	return 0;
+}
+/*                                                           */
+/*                                                           */
+/*=======================End of RT2860=======================*/
+#endif // RT2860 //
+
+
+VOID rt_ee_read_all(PRTMP_ADAPTER pAd, USHORT *Data)
+{
+	USHORT i;
+	USHORT value;
+
+	for (i = 0 ; i < EEPROM_SIZE/2 ; )
+	{
+		/* "value" is expecially for some compilers... */
+		RT28xx_EEPROM_READ16(pAd, i*2, value);
+		Data[i] = value;
+		i++;
+	}
+}
+
+VOID rt_ee_write_all(PRTMP_ADAPTER pAd, USHORT *Data)
+{
+	USHORT i;
+	USHORT value;
+
+	for (i = 0 ; i < EEPROM_SIZE/2 ; )
+	{
+		/* "value" is expecially for some compilers... */
+		value = Data[i];
+		RT28xx_EEPROM_WRITE16(pAd, i*2, value);
+		i ++;
+	}
+}
+#ifdef RALINK_28xx_QA
+VOID ATE_QA_Statistics(
+	IN PRTMP_ADAPTER			pAd,
+	IN PRXWI_STRUC				pRxWI,
+	IN PRT28XX_RXD_STRUC		pRxD,
+	IN PHEADER_802_11			pHeader)
+{
+	// update counter first
+	if (pHeader != NULL)
+	{
+		if (pHeader->FC.Type == BTYPE_DATA)
+		{
+			if (pRxD->U2M)
+				pAd->ate.U2M++;
+			else
+				pAd->ate.OtherData++;
+		}
+		else if (pHeader->FC.Type == BTYPE_MGMT)
+		{
+			if (pHeader->FC.SubType == SUBTYPE_BEACON)
+				pAd->ate.Beacon++;
+			else
+				pAd->ate.OtherCount++;
+		}
+		else if (pHeader->FC.Type == BTYPE_CNTL)
+		{
+			pAd->ate.OtherCount++;
+		}
+	}
+	pAd->ate.RSSI0 = pRxWI->RSSI0;
+	pAd->ate.RSSI1 = pRxWI->RSSI1;
+	pAd->ate.RSSI2 = pRxWI->RSSI2;
+	pAd->ate.SNR0 = pRxWI->SNR0;
+	pAd->ate.SNR1 = pRxWI->SNR1;
+}
+
+/* command id with Cmd Type == 0x0008(for 28xx)/0x0005(for iNIC) */
+#define RACFG_CMD_RF_WRITE_ALL		0x0000
+#define RACFG_CMD_E2PROM_READ16		0x0001
+#define RACFG_CMD_E2PROM_WRITE16	0x0002
+#define RACFG_CMD_E2PROM_READ_ALL	0x0003
+#define RACFG_CMD_E2PROM_WRITE_ALL	0x0004
+#define RACFG_CMD_IO_READ			0x0005
+#define RACFG_CMD_IO_WRITE			0x0006
+#define RACFG_CMD_IO_READ_BULK		0x0007
+#define RACFG_CMD_BBP_READ8			0x0008
+#define RACFG_CMD_BBP_WRITE8		0x0009
+#define RACFG_CMD_BBP_READ_ALL		0x000a
+#define RACFG_CMD_GET_COUNTER		0x000b
+#define RACFG_CMD_CLEAR_COUNTER		0x000c
+
+#define RACFG_CMD_RSV1				0x000d
+#define RACFG_CMD_RSV2				0x000e
+#define RACFG_CMD_RSV3				0x000f
+
+#define RACFG_CMD_TX_START			0x0010
+#define RACFG_CMD_GET_TX_STATUS		0x0011
+#define RACFG_CMD_TX_STOP			0x0012
+#define RACFG_CMD_RX_START			0x0013
+#define RACFG_CMD_RX_STOP			0x0014
+#define RACFG_CMD_GET_NOISE_LEVEL	0x0015
+
+#define RACFG_CMD_ATE_START			0x0080
+#define RACFG_CMD_ATE_STOP			0x0081
+
+#define RACFG_CMD_ATE_START_TX_CARRIER		0x0100
+#define RACFG_CMD_ATE_START_TX_CONT			0x0101
+#define RACFG_CMD_ATE_START_TX_FRAME		0x0102
+#define RACFG_CMD_ATE_SET_BW	            0x0103
+#define RACFG_CMD_ATE_SET_TX_POWER0	        0x0104
+#define RACFG_CMD_ATE_SET_TX_POWER1			0x0105
+#define RACFG_CMD_ATE_SET_FREQ_OFFSET		0x0106
+#define RACFG_CMD_ATE_GET_STATISTICS		0x0107
+#define RACFG_CMD_ATE_RESET_COUNTER			0x0108
+#define RACFG_CMD_ATE_SEL_TX_ANTENNA		0x0109
+#define RACFG_CMD_ATE_SEL_RX_ANTENNA		0x010a
+#define RACFG_CMD_ATE_SET_PREAMBLE			0x010b
+#define RACFG_CMD_ATE_SET_CHANNEL			0x010c
+#define RACFG_CMD_ATE_SET_ADDR1				0x010d
+#define RACFG_CMD_ATE_SET_ADDR2				0x010e
+#define RACFG_CMD_ATE_SET_ADDR3				0x010f
+#define RACFG_CMD_ATE_SET_RATE				0x0110
+#define RACFG_CMD_ATE_SET_TX_FRAME_LEN		0x0111
+#define RACFG_CMD_ATE_SET_TX_FRAME_COUNT	0x0112
+#define RACFG_CMD_ATE_START_RX_FRAME		0x0113
+#define RACFG_CMD_ATE_E2PROM_READ_BULK	0x0114
+#define RACFG_CMD_ATE_E2PROM_WRITE_BULK	0x0115
+#define RACFG_CMD_ATE_IO_WRITE_BULK		0x0116
+#define RACFG_CMD_ATE_BBP_READ_BULK		0x0117
+#define RACFG_CMD_ATE_BBP_WRITE_BULK	0x0118
+#define RACFG_CMD_ATE_RF_READ_BULK		0x0119
+#define RACFG_CMD_ATE_RF_WRITE_BULK		0x011a
+
+
+
+#define A2Hex(_X, _p) 				\
+{									\
+	UCHAR *p;						\
+	_X = 0;							\
+	p = _p;							\
+	while (((*p >= 'a') && (*p <= 'f')) || ((*p >= 'A') && (*p <= 'F')) || ((*p >= '0') && (*p <= '9')))		\
+	{												\
+		if ((*p >= 'a') && (*p <= 'f'))				\
+			_X = _X * 16 + *p - 87;					\
+		else if ((*p >= 'A') && (*p <= 'F'))		\
+			_X = _X * 16 + *p - 55;					\
+		else if ((*p >= '0') && (*p <= '9'))		\
+			_X = _X * 16 + *p - 48;					\
+		p++;										\
+	}												\
+}
+
+
+static VOID memcpy_exl(PRTMP_ADAPTER pAd, UCHAR *dst, UCHAR *src, ULONG len);
+static VOID memcpy_exs(PRTMP_ADAPTER pAd, UCHAR *dst, UCHAR *src, ULONG len);
+static VOID RTMP_IO_READ_BULK(PRTMP_ADAPTER pAd, UCHAR *dst, UCHAR *src, UINT32 len);
+
+#ifdef UCOS
+int ate_copy_to_user(
+	IN PUCHAR payload,
+	IN PUCHAR msg,
+	IN INT    len)
+{
+	memmove(payload, msg, len);
+	return 0;
+}
+
+#undef	copy_to_user
+#define copy_to_user(x,y,z) ate_copy_to_user((PUCHAR)x, (PUCHAR)y, z)
+#endif // UCOS //
+
+#define	LEN_OF_ARG 16
+
+VOID RtmpDoAte(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq)
+{
+	unsigned short Command_Id;
+	struct ate_racfghdr *pRaCfg;
+	INT	Status = NDIS_STATUS_SUCCESS;
+
+
+
+	if((pRaCfg = kmalloc(sizeof(struct ate_racfghdr), GFP_KERNEL)) == NULL)
+	{
+		Status = -EINVAL;
+		return;
+	}
+
+	NdisZeroMemory(pRaCfg, sizeof(struct ate_racfghdr));
+
+    if (copy_from_user((PUCHAR)pRaCfg, wrq->u.data.pointer, wrq->u.data.length))
+	{
+		Status = -EFAULT;
+		kfree(pRaCfg);
+		return;
+	}
+
+
+	Command_Id = ntohs(pRaCfg->command_id);
+
+	ATEDBGPRINT(RT_DEBUG_TRACE,("\n%s: Command_Id = 0x%04x !\n", __FUNCTION__, Command_Id));
+
+	switch (Command_Id)
+	{
+ 		// We will get this command when QA starts.
+		case RACFG_CMD_ATE_START:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_START\n"));
+
+				// prepare feedback as soon as we can to avoid QA timeout.
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("wrq->u.data.length = %d\n", wrq->u.data.length));
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_TRACE, ("copy_to_user() fail in case RACFG_CMD_ATE_START\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_START is done !\n"));
+				}
+				Set_ATE_Proc(pAdapter, "ATESTART");
+			}
+			break;
+
+ 		// We will get this command either QA is closed or ated is killed by user.
+		case RACFG_CMD_ATE_STOP:
+			{
+#ifndef UCOS
+				INT32 ret;
+#endif // !UCOS //
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_STOP\n"));
+
+				// Distinguish this command came from QA(via ated)
+				// or ate daemon according to the existence of pid in payload.
+				// No need to prepare feedback if this cmd came directly from ate daemon.
+				pRaCfg->length = ntohs(pRaCfg->length);
+
+				if (pRaCfg->length == sizeof(pAdapter->ate.AtePid))
+				{
+					// This command came from QA.
+					// Get the pid of ATE daemon.
+					memcpy((UCHAR *)&pAdapter->ate.AtePid,
+						(&pRaCfg->data[0]) - 2/* == &(pRaCfg->status) */,
+						sizeof(pAdapter->ate.AtePid));
+
+					// prepare feedback as soon as we can to avoid QA timeout.
+					pRaCfg->length = htons(2);
+					pRaCfg->status = htons(0);
+
+					wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+										+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+										+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+					ATEDBGPRINT(RT_DEBUG_TRACE, ("wrq->u.data.length = %d\n", wrq->u.data.length));
+
+	            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+	            	{
+	            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_STOP\n"));
+	                    Status = -EFAULT;
+	            	}
+
+					//
+					// kill ATE daemon when leaving ATE mode.
+					// We must kill ATE daemon first before setting ATESTOP,
+					// or Microsoft will report sth. wrong.
+#ifndef UCOS
+					ret = KILL_THREAD_PID(pAdapter->ate.AtePid, SIGTERM, 1);
+					if (ret)
+					{
+						ATEDBGPRINT(RT_DEBUG_ERROR, ("%s: unable to signal thread\n", pAdapter->net_dev->name));
+					}
+#endif // !UCOS //
+				}
+
+				// AP might have in ATE_STOP mode due to cmd from QA.
+				if (ATE_ON(pAdapter))
+				{
+					// Someone has killed ate daemon while QA GUI is still open.
+					Set_ATE_Proc(pAdapter, "ATESTOP");
+					ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_AP_START is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_RF_WRITE_ALL:
+			{
+				UINT32 R1, R2, R3, R4;
+				USHORT channel;
+
+				memcpy(&R1, pRaCfg->data-2, 4);
+				memcpy(&R2, pRaCfg->data+2, 4);
+				memcpy(&R3, pRaCfg->data+6, 4);
+				memcpy(&R4, pRaCfg->data+10, 4);
+				memcpy(&channel, pRaCfg->data+14, 2);
+
+				pAdapter->LatchRfRegs.R1 = ntohl(R1);
+				pAdapter->LatchRfRegs.R2 = ntohl(R2);
+				pAdapter->LatchRfRegs.R3 = ntohl(R3);
+				pAdapter->LatchRfRegs.R4 = ntohl(R4);
+				pAdapter->LatchRfRegs.Channel = ntohs(channel);
+
+				RTMP_RF_IO_WRITE32(pAdapter, pAdapter->LatchRfRegs.R1);
+				RTMP_RF_IO_WRITE32(pAdapter, pAdapter->LatchRfRegs.R2);
+				RTMP_RF_IO_WRITE32(pAdapter, pAdapter->LatchRfRegs.R3);
+				RTMP_RF_IO_WRITE32(pAdapter, pAdapter->LatchRfRegs.R4);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("wrq->u.data.length = %d\n", wrq->u.data.length));
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_RF_WRITE_ALL\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_RF_WRITE_ALL is done !\n"));
+				}
+			}
+            break;
+
+		case RACFG_CMD_E2PROM_READ16:
+			{
+				USHORT	offset, value, tmp;
+
+				offset = ntohs(pRaCfg->status);
+				/* "tmp" is expecially for some compilers... */
+				RT28xx_EEPROM_READ16(pAdapter, offset, tmp);
+				value = tmp;
+				value = htons(value);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("EEPROM Read offset = 0x%04x, value = 0x%04x\n", offset, value));
+
+				// prepare feedback
+				pRaCfg->length = htons(4);
+				pRaCfg->status = htons(0);
+				memcpy(pRaCfg->data, &value, 2);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("sizeof(struct ate_racfghdr) = %d\n", sizeof(struct ate_racfghdr)));
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("wrq->u.data.length = %d\n", wrq->u.data.length));
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_E2PROM_READ16\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_E2PROM_READ16 is done !\n"));
+				}
+           	}
+			break;
+
+		case RACFG_CMD_E2PROM_WRITE16:
+			{
+				USHORT	offset, value;
+
+				offset = ntohs(pRaCfg->status);
+				memcpy(&value, pRaCfg->data, 2);
+				value = ntohs(value);
+				RT28xx_EEPROM_WRITE16(pAdapter, offset, value);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_E2PROM_WRITE16\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_E2PROM_WRITE16 is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_E2PROM_READ_ALL:
+			{
+				USHORT buffer[EEPROM_SIZE/2];
+
+				rt_ee_read_all(pAdapter,(USHORT *)buffer);
+				memcpy_exs(pAdapter, pRaCfg->data, (UCHAR *)buffer, EEPROM_SIZE);
+
+				// prepare feedback
+				pRaCfg->length = htons(2+EEPROM_SIZE);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_E2PROM_READ_ALL\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_E2PROM_READ_ALL is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_E2PROM_WRITE_ALL:
+			{
+				USHORT buffer[EEPROM_SIZE/2];
+
+				NdisZeroMemory((UCHAR *)buffer, EEPROM_SIZE);
+				memcpy_exs(pAdapter, (UCHAR *)buffer, (UCHAR *)&pRaCfg->status, EEPROM_SIZE);
+				rt_ee_write_all(pAdapter,(USHORT *)buffer);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_E2PROM_WRITE_ALL\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_ERROR, ("RACFG_CMD_E2PROM_WRITE_ALL is done !\n"));
+				}
+
+			}
+			break;
+
+		case RACFG_CMD_IO_READ:
+			{
+				UINT32	offset;
+				UINT32	value;
+
+				memcpy(&offset, &pRaCfg->status, 4);
+				offset = ntohl(offset);
+
+				// We do not need the base address.
+				// So just extract the offset out.
+				offset &= 0x0000FFFF;
+				RTMP_IO_READ32(pAdapter, offset, &value);
+				value = htonl(value);
+
+				// prepare feedback
+				pRaCfg->length = htons(6);
+				pRaCfg->status = htons(0);
+				memcpy(pRaCfg->data, &value, 4);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_IO_READ\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_IO_READ is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_IO_WRITE:
+			{
+				UINT32	offset, value;
+
+				memcpy(&offset, pRaCfg->data-2, 4);
+				memcpy(&value, pRaCfg->data+2, 4);
+
+				offset = ntohl(offset);
+
+				// We do not need the base address.
+				// So just extract out the offset.
+				offset &= 0x0000FFFF;
+				value = ntohl(value);
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_IO_WRITE: offset = %x, value = %x\n", offset, value));
+				RTMP_IO_WRITE32(pAdapter, offset, value);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_IO_WRITE\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_IO_WRITE is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_IO_READ_BULK:
+			{
+				UINT32	offset;
+				USHORT	len;
+
+				memcpy(&offset, &pRaCfg->status, 4);
+				offset = ntohl(offset);
+
+				// We do not need the base address.
+				// So just extract the offset.
+				offset &= 0x0000FFFF;
+				memcpy(&len, pRaCfg->data+2, 2);
+				len = ntohs(len);
+
+				if (len > 371)
+				{
+					ATEDBGPRINT(RT_DEBUG_TRACE,("len is too large, make it smaller\n"));
+					pRaCfg->length = htons(2);
+					pRaCfg->status = htons(1);
+					break;
+				}
+
+				RTMP_IO_READ_BULK(pAdapter, pRaCfg->data, (UCHAR *)offset, len*4);// unit in four bytes
+
+				// prepare feedback
+				pRaCfg->length = htons(2+len*4);// unit in four bytes
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_IO_READ_BULK\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_IO_READ_BULK is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_BBP_READ8:
+			{
+				USHORT	offset;
+				UCHAR	value;
+
+				value = 0;
+				offset = ntohs(pRaCfg->status);
+
+				if (ATE_ON(pAdapter))
+				{
+					ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, offset,  &value);
+				}
+				else
+				{
+					RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, offset,  &value);
+				}
+				// prepare feedback
+				pRaCfg->length = htons(3);
+				pRaCfg->status = htons(0);
+				pRaCfg->data[0] = value;
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("BBP value = %x\n", value));
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_BBP_READ8\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_BBP_READ8 is done !\n"));
+				}
+			}
+			break;
+		case RACFG_CMD_BBP_WRITE8:
+			{
+				USHORT	offset;
+				UCHAR	value;
+
+				offset = ntohs(pRaCfg->status);
+				memcpy(&value, pRaCfg->data, 1);
+
+				if (ATE_ON(pAdapter))
+				{
+					ATE_BBP_IO_WRITE8_BY_REG_ID(pAdapter, offset,  value);
+				}
+				else
+				{
+					RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, offset,  value);
+				}
+
+				if ((offset == BBP_R1) || (offset == BBP_R3))
+				{
+					SyncTxRxConfig(pAdapter, offset, value);
+				}
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_BBP_WRITE8\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_BBP_WRITE8 is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_BBP_READ_ALL:
+			{
+				USHORT j;
+
+				for (j = 0; j < 137; j++)
+				{
+					pRaCfg->data[j] = 0;
+
+					if (ATE_ON(pAdapter))
+					{
+						ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, j,  &pRaCfg->data[j]);
+					}
+					else
+					{
+						RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, j,  &pRaCfg->data[j]);
+					}
+				}
+
+				// prepare feedback
+				pRaCfg->length = htons(2+137);
+				pRaCfg->status = htons(0);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_BBP_READ_ALL\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_BBP_READ_ALL is done !\n"));
+				}
+			}
+
+			break;
+
+		case RACFG_CMD_ATE_E2PROM_READ_BULK:
+		{
+			USHORT offset;
+			USHORT len;
+			USHORT buffer[EEPROM_SIZE/2];
+
+			offset = ntohs(pRaCfg->status);
+			memcpy(&len, pRaCfg->data, 2);
+			len = ntohs(len);
+
+			rt_ee_read_all(pAdapter,(USHORT *)buffer);
+			if (offset + len <= EEPROM_SIZE)
+				memcpy_exs(pAdapter, pRaCfg->data, (UCHAR *)buffer+offset, len);
+			else
+				ATEDBGPRINT(RT_DEBUG_ERROR, ("exceed EEPROM size\n"));
+
+			// prepare feedback
+			pRaCfg->length = htons(2+len);
+			pRaCfg->status = htons(0);
+			wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            {
+            	ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_E2PROM_READ_BULK\n"));
+                Status = -EFAULT;
+            }
+			else
+			{
+               	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_E2PROM_READ_BULK is done !\n"));
+			}
+
+		}
+			break;
+
+		case RACFG_CMD_ATE_E2PROM_WRITE_BULK:
+		{
+			USHORT offset;
+			USHORT len;
+			USHORT buffer[EEPROM_SIZE/2];
+
+			offset = ntohs(pRaCfg->status);
+			memcpy(&len, pRaCfg->data, 2);
+			len = ntohs(len);
+
+			rt_ee_read_all(pAdapter,(USHORT *)buffer);
+			memcpy_exs(pAdapter, (UCHAR *)buffer + offset, (UCHAR *)pRaCfg->data + 2, len);
+			rt_ee_write_all(pAdapter,(USHORT *)buffer);
+
+			// prepare feedback
+			pRaCfg->length = htons(2);
+			pRaCfg->status = htons(0);
+			wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+								+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+								+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+            if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            {
+            	ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_E2PROM_WRITE_BULK\n"));
+                   Status = -EFAULT;
+            }
+			else
+			{
+               	ATEDBGPRINT(RT_DEBUG_ERROR, ("RACFG_CMD_ATE_E2PROM_WRITE_BULK is done !\n"));
+			}
+
+		}
+			break;
+
+		case RACFG_CMD_ATE_IO_WRITE_BULK:
+		{
+			UINT32 offset, i, value;
+			USHORT len;
+
+			memcpy(&offset, &pRaCfg->status, 4);
+			offset = ntohl(offset);
+			memcpy(&len, pRaCfg->data+2, 2);
+			len = ntohs(len);
+
+			for (i = 0; i < len; i += 4)
+			{
+				memcpy_exl(pAdapter, (UCHAR *)&value, pRaCfg->data+4+i, 4);
+				printk("Write %x %x\n", offset + i, value);
+				RTMP_IO_WRITE32(pAdapter, (offset +i) & 0xffff, value);
+			}
+
+			// prepare feedback
+			pRaCfg->length = htons(2);
+			pRaCfg->status = htons(0);
+			wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+								+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+								+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+            if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            {
+            	ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_IO_WRITE_BULK\n"));
+                   Status = -EFAULT;
+            }
+			else
+			{
+               	ATEDBGPRINT(RT_DEBUG_ERROR, ("RACFG_CMD_ATE_IO_WRITE_BULK is done !\n"));
+			}
+
+		}
+			break;
+
+		case RACFG_CMD_ATE_BBP_READ_BULK:
+		{
+			USHORT offset;
+			USHORT len;
+			USHORT j;
+
+			offset = ntohs(pRaCfg->status);
+			memcpy(&len, pRaCfg->data, 2);
+			len = ntohs(len);
+
+
+			for (j = offset; j < (offset+len); j++)
+			{
+				pRaCfg->data[j - offset] = 0;
+
+				if (pAdapter->ate.Mode == ATE_STOP)
+				{
+					RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, j,  &pRaCfg->data[j - offset]);
+				}
+				else
+				{
+					ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, j,  &pRaCfg->data[j - offset]);
+				}
+			}
+
+			// prepare feedback
+			pRaCfg->length = htons(2+len);
+			pRaCfg->status = htons(0);
+			wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+								+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+								+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            {
+            	ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_BBP_READ_BULK\n"));
+                   Status = -EFAULT;
+            }
+			else
+			{
+               	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_BBP_READ_BULK is done !\n"));
+			}
+
+		}
+			break;
+
+		case RACFG_CMD_ATE_BBP_WRITE_BULK:
+		{
+			USHORT offset;
+			USHORT len;
+			USHORT j;
+			UCHAR *value;
+
+			offset = ntohs(pRaCfg->status);
+			memcpy(&len, pRaCfg->data, 2);
+			len = ntohs(len);
+
+			for (j = offset; j < (offset+len); j++)
+			{
+				value = pRaCfg->data + 2 + (j - offset);
+				if (pAdapter->ate.Mode == ATE_STOP)
+				{
+					RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, j,  *value);
+				}
+				else
+				{
+					ATE_BBP_IO_WRITE8_BY_REG_ID(pAdapter, j,  *value);
+				}
+			}
+
+			// prepare feedback
+			pRaCfg->length = htons(2);
+			pRaCfg->status = htons(0);
+			wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+								+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+								+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            {
+            	ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_BBP_WRITE_BULK\n"));
+                   Status = -EFAULT;
+            }
+			else
+			{
+               	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_BBP_WRITE_BULK is done !\n"));
+			}
+		}
+			break;
+
+#ifdef CONFIG_RALINK_RT3052
+		case RACFG_CMD_ATE_RF_READ_BULK:
+		{
+			USHORT offset;
+			USHORT len;
+			USHORT j;
+
+			offset = ntohs(pRaCfg->status);
+			memcpy(&len, pRaCfg->data, 2);
+			len = ntohs(len);
+
+			for (j = offset; j < (offset+len); j++)
+			{
+				pRaCfg->data[j - offset] = 0;
+				RT30xxReadRFRegister(pAdapter, j,  &pRaCfg->data[j - offset]);
+			}
+
+			// prepare feedback
+			pRaCfg->length = htons(2+len);
+			pRaCfg->status = htons(0);
+			wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+								+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+								+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            {
+            	ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_RF_READ_BULK\n"));
+                   Status = -EFAULT;
+            }
+			else
+			{
+               	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_RF_READ_BULK is done !\n"));
+			}
+
+		}
+			break;
+
+		case RACFG_CMD_ATE_RF_WRITE_BULK:
+		{
+			USHORT offset;
+			USHORT len;
+			USHORT j;
+			UCHAR *value;
+
+			offset = ntohs(pRaCfg->status);
+			memcpy(&len, pRaCfg->data, 2);
+			len = ntohs(len);
+
+			for (j = offset; j < (offset+len); j++)
+			{
+				value = pRaCfg->data + 2 + (j - offset);
+				RT30xxWriteRFRegister(pAdapter, j,  *value);
+			}
+
+			// prepare feedback
+			pRaCfg->length = htons(2);
+			pRaCfg->status = htons(0);
+			wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+								+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+								+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            {
+            	ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_RF_WRITE_BULK\n"));
+                   Status = -EFAULT;
+            }
+			else
+			{
+               	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_RF_WRITE_BULK is done !\n"));
+			}
+
+		}
+			break;
+#endif
+
+
+		case RACFG_CMD_GET_NOISE_LEVEL:
+			{
+				UCHAR	channel;
+				INT32   buffer[3][10];/* 3 : RxPath ; 10 : no. of per rssi samples */
+
+				channel = (ntohs(pRaCfg->status) & 0x00FF);
+				CalNoiseLevel(pAdapter, channel, buffer);
+				memcpy_exl(pAdapter, (UCHAR *)pRaCfg->data, (UCHAR *)&(buffer[0][0]), (sizeof(INT32)*3*10));
+
+				// prepare feedback
+				pRaCfg->length = htons(2 + (sizeof(INT32)*3*10));
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_GET_NOISE_LEVEL\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_GET_NOISE_LEVEL is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_GET_COUNTER:
+			{
+				memcpy_exl(pAdapter, &pRaCfg->data[0], (UCHAR *)&pAdapter->ate.U2M, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[4], (UCHAR *)&pAdapter->ate.OtherData, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[8], (UCHAR *)&pAdapter->ate.Beacon, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[12], (UCHAR *)&pAdapter->ate.OtherCount, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[16], (UCHAR *)&pAdapter->ate.TxAc0, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[20], (UCHAR *)&pAdapter->ate.TxAc1, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[24], (UCHAR *)&pAdapter->ate.TxAc2, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[28], (UCHAR *)&pAdapter->ate.TxAc3, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[32], (UCHAR *)&pAdapter->ate.TxHCCA, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[36], (UCHAR *)&pAdapter->ate.TxMgmt, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[40], (UCHAR *)&pAdapter->ate.RSSI0, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[44], (UCHAR *)&pAdapter->ate.RSSI1, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[48], (UCHAR *)&pAdapter->ate.RSSI2, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[52], (UCHAR *)&pAdapter->ate.SNR0, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[56], (UCHAR *)&pAdapter->ate.SNR1, 4);
+
+				pRaCfg->length = htons(2+60);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_GET_COUNTER\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_GET_COUNTER is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_CLEAR_COUNTER:
+			{
+				pAdapter->ate.U2M = 0;
+				pAdapter->ate.OtherData = 0;
+				pAdapter->ate.Beacon = 0;
+				pAdapter->ate.OtherCount = 0;
+				pAdapter->ate.TxAc0 = 0;
+				pAdapter->ate.TxAc1 = 0;
+				pAdapter->ate.TxAc2 = 0;
+				pAdapter->ate.TxAc3 = 0;
+				pAdapter->ate.TxHCCA = 0;
+				pAdapter->ate.TxMgmt = 0;
+				pAdapter->ate.TxDoneCount = 0;
+
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_CLEAR_COUNTER\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_CLEAR_COUNTER is done !\n"));
+				}
+			}
+
+			break;
+
+		case RACFG_CMD_TX_START:
+			{
+				USHORT *p;
+				USHORT	err = 1;
+				UCHAR   Bbp22Value = 0, Bbp24Value = 0;
+
+				if ((pAdapter->ate.TxStatus != 0) && (pAdapter->ate.Mode & ATE_TXFRAME))
+				{
+					ATEDBGPRINT(RT_DEBUG_TRACE,("Ate Tx is already running, to run next Tx, you must stop it first\n"));
+					err = 2;
+					goto TX_START_ERROR;
+				}
+				else if ((pAdapter->ate.TxStatus != 0) && !(pAdapter->ate.Mode & ATE_TXFRAME))
+				{
+					int i = 0;
+
+					while ((i++ < 10) && (pAdapter->ate.TxStatus != 0))
+					{
+						RTMPusecDelay(5000);
+					}
+
+					// force it to stop
+					pAdapter->ate.TxStatus = 0;
+					pAdapter->ate.TxDoneCount = 0;
+					//pAdapter->ate.Repeat = 0;
+					pAdapter->ate.bQATxStart = FALSE;
+				}
+
+				// If pRaCfg->length == 0, this "RACFG_CMD_TX_START" is for Carrier test or Carrier Suppression.
+				if (ntohs(pRaCfg->length) != 0)
+				{
+					// Get frame info
+
+					NdisMoveMemory(&pAdapter->ate.TxWI, pRaCfg->data + 2, 16);
+#ifdef RT_BIG_ENDIAN
+					RTMPWIEndianChange((PUCHAR)&pAdapter->ate.TxWI, TYPE_TXWI);
+#endif // RT_BIG_ENDIAN //
+
+					NdisMoveMemory(&pAdapter->ate.TxCount, pRaCfg->data + 18, 4);
+					pAdapter->ate.TxCount = ntohl(pAdapter->ate.TxCount);
+
+					p = (USHORT *)(&pRaCfg->data[22]);
+					//p = pRaCfg->data + 22;
+					// always use QID_AC_BE
+					pAdapter->ate.QID = 0;
+					p = (USHORT *)(&pRaCfg->data[24]);
+					//p = pRaCfg->data + 24;
+					pAdapter->ate.HLen = ntohs(*p);
+
+					if (pAdapter->ate.HLen > 32)
+					{
+						ATEDBGPRINT(RT_DEBUG_ERROR,("pAdapter->ate.HLen > 32\n"));
+						err = 3;
+						goto TX_START_ERROR;
+					}
+
+					NdisMoveMemory(&pAdapter->ate.Header, pRaCfg->data + 26, pAdapter->ate.HLen);
+
+
+					pAdapter->ate.PLen = ntohs(pRaCfg->length) - (pAdapter->ate.HLen + 28);
+
+					if (pAdapter->ate.PLen > 32)
+					{
+						ATEDBGPRINT(RT_DEBUG_ERROR,("pAdapter->ate.PLen > 32\n"));
+						err = 4;
+						goto TX_START_ERROR;
+					}
+
+					NdisMoveMemory(&pAdapter->ate.Pattern, pRaCfg->data + 26 + pAdapter->ate.HLen, pAdapter->ate.PLen);
+					pAdapter->ate.DLen = pAdapter->ate.TxWI.MPDUtotalByteCount - pAdapter->ate.HLen;
+				}
+
+				ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R22, &Bbp22Value);
+
+				switch (Bbp22Value)
+				{
+					case BBP22_TXFRAME:
+						{
+							if (pAdapter->ate.TxCount == 0)
+							{
+#ifdef RT2860
+								pAdapter->ate.TxCount = 0xFFFFFFFF;
+#endif // RT2860 //
+							}
+							ATEDBGPRINT(RT_DEBUG_TRACE,("START TXFRAME\n"));
+							pAdapter->ate.bQATxStart = TRUE;
+							Set_ATE_Proc(pAdapter, "TXFRAME");
+						}
+						break;
+
+					case BBP22_TXCONT_OR_CARRSUPP:
+						{
+							ATEDBGPRINT(RT_DEBUG_TRACE,("BBP22_TXCONT_OR_CARRSUPP\n"));
+							ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, 24, &Bbp24Value);
+
+							switch (Bbp24Value)
+							{
+								case BBP24_TXCONT:
+									{
+										ATEDBGPRINT(RT_DEBUG_TRACE,("START TXCONT\n"));
+										pAdapter->ate.bQATxStart = TRUE;
+										Set_ATE_Proc(pAdapter, "TXCONT");
+									}
+									break;
+
+								case BBP24_CARRSUPP:
+									{
+										ATEDBGPRINT(RT_DEBUG_TRACE,("START TXCARRSUPP\n"));
+										pAdapter->ate.bQATxStart = TRUE;
+										pAdapter->ate.Mode |= ATE_TXCARRSUPP;
+									}
+									break;
+
+								default:
+									{
+										ATEDBGPRINT(RT_DEBUG_ERROR,("Unknown Start TX subtype !"));
+									}
+									break;
+							}
+						}
+						break;
+
+					case BBP22_TXCARR:
+						{
+							ATEDBGPRINT(RT_DEBUG_TRACE,("START TXCARR\n"));
+							pAdapter->ate.bQATxStart = TRUE;
+							Set_ATE_Proc(pAdapter, "TXCARR");
+						}
+						break;
+
+					default:
+						{
+							ATEDBGPRINT(RT_DEBUG_ERROR,("Unknown Start TX subtype !"));
+						}
+						break;
+				}
+
+				if (pAdapter->ate.bQATxStart == TRUE)
+				{
+					// prepare feedback
+					pRaCfg->length = htons(2);
+					pRaCfg->status = htons(0);
+
+					wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+										+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+										+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+	            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+	            	{
+	            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() was failed in case RACFG_CMD_TX_START\n"));
+	                    Status = -EFAULT;
+	            	}
+					else
+					{
+	                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_TX_START is done !\n"));
+					}
+					break;
+				}
+
+TX_START_ERROR:
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(err);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_TX_START\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("feedback of TX_START_ERROR is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_GET_TX_STATUS:
+			{
+				UINT32 count;
+
+				// prepare feedback
+				pRaCfg->length = htons(6);
+				pRaCfg->status = htons(0);
+				count = htonl(pAdapter->ate.TxDoneCount);
+				NdisMoveMemory(pRaCfg->data, &count, 4);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_GET_TX_STATUS\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_GET_TX_STATUS is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_TX_STOP:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_TX_STOP\n"));
+
+				Set_ATE_Proc(pAdapter, "TXSTOP");
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_TRACE, ("copy_to_user() fail in case RACFG_CMD_TX_STOP\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_TX_STOP is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_RX_START:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_RX_START\n"));
+
+				pAdapter->ate.bQARxStart = TRUE;
+				Set_ATE_Proc(pAdapter, "RXFRAME");
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_RX_START\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_RX_START is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_RX_STOP:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_RX_STOP\n"));
+
+				Set_ATE_Proc(pAdapter, "RXSTOP");
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_RX_STOP\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_RX_STOP is done !\n"));
+				}
+			}
+			break;
+
+		/* The following cases are for new ATE GUI(not QA). */
+		/*==================================================*/
+		case RACFG_CMD_ATE_START_TX_CARRIER:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_START_TX_CARRIER\n"));
+
+				Set_ATE_Proc(pAdapter, "TXCARR");
+
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("wrq->u.data.length = %d\n", wrq->u.data.length));
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_START_TX_CARRIER\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_START_TX_CARRIER is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_START_TX_CONT:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_START_TX_CONT\n"));
+
+				Set_ATE_Proc(pAdapter, "TXCONT");
+
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("wrq->u.data.length = %d\n", wrq->u.data.length));
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_START_TX_CONT\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_START_TX_CONT is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_START_TX_FRAME:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_START_TX_FRAME\n"));
+
+				Set_ATE_Proc(pAdapter, "TXFRAME");
+
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("wrq->u.data.length = %d\n", wrq->u.data.length));
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_START_TX_FRAME\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_START_TX_FRAME is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_BW:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_BW\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+
+				Set_ATE_TX_BW_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_BW\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_BW is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_TX_POWER0:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_TX_POWER0\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+				Set_ATE_TX_POWER0_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_TX_POWER0\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_TX_POWER0 is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_TX_POWER1:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_TX_POWER1\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+				Set_ATE_TX_POWER1_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_TX_POWER1\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_TX_POWER1 is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_FREQ_OFFSET:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_FREQ_OFFSET\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+				Set_ATE_TX_FREQOFFSET_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_FREQ_OFFSET\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_FREQ_OFFSET is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_GET_STATISTICS:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_GET_STATISTICS\n"));
+
+				memcpy_exl(pAdapter, &pRaCfg->data[0], (UCHAR *)&pAdapter->ate.TxDoneCount, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[4], (UCHAR *)&pAdapter->WlanCounters.RetryCount.u.LowPart, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[8], (UCHAR *)&pAdapter->WlanCounters.FailedCount.u.LowPart, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[12], (UCHAR *)&pAdapter->WlanCounters.RTSSuccessCount.u.LowPart, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[16], (UCHAR *)&pAdapter->WlanCounters.RTSFailureCount.u.LowPart, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[20], (UCHAR *)&pAdapter->WlanCounters.ReceivedFragmentCount.QuadPart, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[24], (UCHAR *)&pAdapter->WlanCounters.FCSErrorCount.u.LowPart, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[28], (UCHAR *)&pAdapter->Counters8023.RxNoBuffer, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[32], (UCHAR *)&pAdapter->WlanCounters.FrameDuplicateCount.u.LowPart, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[36], (UCHAR *)&pAdapter->RalinkCounters.OneSecFalseCCACnt, 4);
+
+				if (pAdapter->ate.RxAntennaSel == 0)
+				{
+					INT32 RSSI0 = 0;
+					INT32 RSSI1 = 0;
+					INT32 RSSI2 = 0;
+
+					RSSI0 = (INT32)(pAdapter->ate.LastRssi0 - pAdapter->BbpRssiToDbmDelta);
+					RSSI1 = (INT32)(pAdapter->ate.LastRssi1 - pAdapter->BbpRssiToDbmDelta);
+					RSSI2 = (INT32)(pAdapter->ate.LastRssi2 - pAdapter->BbpRssiToDbmDelta);
+					memcpy_exl(pAdapter, &pRaCfg->data[40], (UCHAR *)&RSSI0, 4);
+					memcpy_exl(pAdapter, &pRaCfg->data[44], (UCHAR *)&RSSI1, 4);
+					memcpy_exl(pAdapter, &pRaCfg->data[48], (UCHAR *)&RSSI2, 4);
+					pRaCfg->length = htons(2+52);
+				}
+				else
+				{
+					INT32 RSSI0 = 0;
+
+					RSSI0 = (INT32)(pAdapter->ate.LastRssi0 - pAdapter->BbpRssiToDbmDelta);
+					memcpy_exl(pAdapter, &pRaCfg->data[40], (UCHAR *)&RSSI0, 4);
+					pRaCfg->length = htons(2+44);
+				}
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_GET_STATISTICS\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_GET_STATISTICS is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_RESET_COUNTER:
+			{
+				SHORT    value = 1;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_RESET_COUNTER\n"));
+
+				sprintf((PCHAR)str, "%d", value);
+				Set_ResetStatCounter_Proc(pAdapter, str);
+
+				pAdapter->ate.TxDoneCount = 0;
+
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_RESET_COUNTER\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_RESET_COUNTER is done !\n"));
+				}
+			}
+
+			break;
+
+		case RACFG_CMD_ATE_SEL_TX_ANTENNA:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SEL_TX_ANTENNA\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+				Set_ATE_TX_Antenna_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SEL_TX_ANTENNA\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SEL_TX_ANTENNA is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SEL_RX_ANTENNA:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SEL_RX_ANTENNA\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+				Set_ATE_RX_Antenna_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SEL_RX_ANTENNA\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SEL_RX_ANTENNA is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_PREAMBLE:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_PREAMBLE\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+				Set_ATE_TX_MODE_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_PREAMBLE\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_PREAMBLE is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_CHANNEL:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_CHANNEL\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+				Set_ATE_CHANNEL_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_CHANNEL\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_CHANNEL is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_ADDR1:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_ADDR1\n"));
+
+				// Addr is an array of UCHAR,
+				// so no need to perform endian swap.
+				memcpy(pAdapter->ate.Addr1, (PUCHAR)(pRaCfg->data - 2), MAC_ADDR_LEN);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_ADDR1\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+					ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_ADDR1 is done !\n (ADDR1 = %2X:%2X:%2X:%2X:%2X:%2X)\n", pAdapter->ate.Addr1[0],
+						pAdapter->ate.Addr1[1], pAdapter->ate.Addr1[2], pAdapter->ate.Addr1[3], pAdapter->ate.Addr1[4], pAdapter->ate.Addr1[5]));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_ADDR2:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_ADDR2\n"));
+
+				// Addr is an array of UCHAR,
+				// so no need to perform endian swap.
+				memcpy(pAdapter->ate.Addr2, (PUCHAR)(pRaCfg->data - 2), MAC_ADDR_LEN);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_ADDR2\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+					ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_ADDR2 is done !\n (ADDR2 = %2X:%2X:%2X:%2X:%2X:%2X)\n", pAdapter->ate.Addr2[0],
+						pAdapter->ate.Addr2[1], pAdapter->ate.Addr2[2], pAdapter->ate.Addr2[3], pAdapter->ate.Addr2[4], pAdapter->ate.Addr2[5]));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_ADDR3:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_ADDR3\n"));
+
+				// Addr is an array of UCHAR,
+				// so no need to perform endian swap.
+				memcpy(pAdapter->ate.Addr3, (PUCHAR)(pRaCfg->data - 2), MAC_ADDR_LEN);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_ADDR3\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+					ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_ADDR3 is done !\n (ADDR3 = %2X:%2X:%2X:%2X:%2X:%2X)\n", pAdapter->ate.Addr3[0],
+						pAdapter->ate.Addr3[1], pAdapter->ate.Addr3[2], pAdapter->ate.Addr3[3], pAdapter->ate.Addr3[4], pAdapter->ate.Addr3[5]));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_RATE:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_RATE\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+				Set_ATE_TX_MCS_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_RATE\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_RATE is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_TX_FRAME_LEN:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_TX_FRAME_LEN\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+				Set_ATE_TX_LENGTH_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_TX_FRAME_LEN\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_TX_FRAME_LEN is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_TX_FRAME_COUNT:
+			{
+				USHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_TX_FRAME_COUNT\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+#ifdef RT2860
+				/* TX_FRAME_COUNT == 0 means tx infinitely */
+				if (value == 0)
+				{
+					/* Use TxCount = 0xFFFFFFFF to approximate the infinity. */
+					pAdapter->ate.TxCount = 0xFFFFFFFF;
+					ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_TX_COUNT_Proc (TxCount = %d)\n", pAdapter->ate.TxCount));
+					ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_COUNT_Proc Success\n"));
+
+
+				}
+				else
+#endif // RT2860 //
+				{
+					sprintf((PCHAR)str, "%d", value);
+					Set_ATE_TX_COUNT_Proc(pAdapter, str);
+				}
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_TX_FRAME_COUNT\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_TX_FRAME_COUNT is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_START_RX_FRAME:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_RX_START\n"));
+
+				Set_ATE_Proc(pAdapter, "RXFRAME");
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_RX_START\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_RX_START is done !\n"));
+				}
+			}
+			break;
+		default:
+			break;
+	}
+    ASSERT(pRaCfg != NULL);
+    if (pRaCfg != NULL)
+    {
+    kfree(pRaCfg);
+    }
+	return;
+}
+
+VOID BubbleSort(INT32 n, INT32 a[])
+{
+	INT32 k, j, temp;
+
+	for (k = n-1;  k>0;  k--)
+	{
+		for (j = 0; j<k; j++)
+		{
+			if(a[j] > a[j+1])
+			{
+				temp = a[j];
+				a[j]=a[j+1];
+				a[j+1]=temp;
+			}
+		}
+	}
+}
+
+VOID CalNoiseLevel(PRTMP_ADAPTER pAd, UCHAR channel, INT32 RSSI[3][10])
+{
+	INT32		RSSI0, RSSI1, RSSI2;
+ 	CHAR		Rssi0Offset, Rssi1Offset, Rssi2Offset;
+	UCHAR		BbpR50Rssi0 = 0, BbpR51Rssi1 = 0, BbpR52Rssi2 = 0;
+	UCHAR		Org_BBP66value = 0, Org_BBP69value = 0, Org_BBP70value = 0, data = 0;
+	USHORT		LNA_Gain = 0;
+	INT32       j = 0;
+	UCHAR		Org_Channel = pAd->ate.Channel;
+	USHORT	    GainValue = 0, OffsetValue = 0;
+
+	ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R66, &Org_BBP66value);
+	ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R69, &Org_BBP69value);
+	ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R70, &Org_BBP70value);
+
+	//**********************************************************************
+	// Read the value of LNA gain and Rssi offset
+	//**********************************************************************
+	RT28xx_EEPROM_READ16(pAd, EEPROM_LNA_OFFSET, GainValue);
+
+	// for Noise Level
+	if (channel <= 14)
+	{
+		LNA_Gain = GainValue & 0x00FF;
+
+		RT28xx_EEPROM_READ16(pAd, EEPROM_RSSI_BG_OFFSET, OffsetValue);
+		Rssi0Offset = OffsetValue & 0x00FF;
+		Rssi1Offset = (OffsetValue & 0xFF00) >> 8;
+		RT28xx_EEPROM_READ16(pAd, (EEPROM_RSSI_BG_OFFSET + 2)/* 0x48 */, OffsetValue);
+		Rssi2Offset = OffsetValue & 0x00FF;
+	}
+	else
+	{
+		LNA_Gain = (GainValue & 0xFF00) >> 8;
+
+		RT28xx_EEPROM_READ16(pAd, EEPROM_RSSI_A_OFFSET, OffsetValue);
+		Rssi0Offset = OffsetValue & 0x00FF;
+		Rssi1Offset = (OffsetValue & 0xFF00) >> 8;
+		RT28xx_EEPROM_READ16(pAd, (EEPROM_RSSI_A_OFFSET + 2)/* 0x4C */, OffsetValue);
+		Rssi2Offset = OffsetValue & 0x00FF;
+	}
+	//**********************************************************************
+	{
+		pAd->ate.Channel = channel;
+		ATEAsicSwitchChannel(pAd);
+		mdelay(5);
+
+		data = 0x10;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, data);
+		data = 0x40;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, data);
+		data = 0x40;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, data);
+		mdelay(5);
+
+		// Start Rx
+		pAd->ate.bQARxStart = TRUE;
+		Set_ATE_Proc(pAd, "RXFRAME");
+
+		mdelay(5);
+
+		for (j = 0; j < 10; j++)
+		{
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R50, &BbpR50Rssi0);
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R51, &BbpR51Rssi1);
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R52, &BbpR52Rssi2);
+
+			mdelay(10);
+
+			// Calculate RSSI 0
+			if (BbpR50Rssi0 == 0)
+			{
+				RSSI0 = -100;
+			}
+			else
+			{
+				RSSI0 = (INT32)(-12 - BbpR50Rssi0 - LNA_Gain - Rssi0Offset);
+			}
+			RSSI[0][j] = RSSI0;
+
+			if ( pAd->Antenna.field.RxPath >= 2 ) // 2R
+			{
+				// Calculate RSSI 1
+				if (BbpR51Rssi1 == 0)
+				{
+					RSSI1 = -100;
+				}
+				else
+				{
+					RSSI1 = (INT32)(-12 - BbpR51Rssi1 - LNA_Gain - Rssi1Offset);
+				}
+				RSSI[1][j] = RSSI1;
+			}
+
+			if ( pAd->Antenna.field.RxPath >= 3 ) // 3R
+			{
+				// Calculate RSSI 2
+				if (BbpR52Rssi2 == 0)
+					RSSI2 = -100;
+				else
+					RSSI2 = (INT32)(-12 - BbpR52Rssi2 - LNA_Gain - Rssi2Offset);
+
+				RSSI[2][j] = RSSI2;
+			}
+		}
+
+		// Stop Rx
+		Set_ATE_Proc(pAd, "RXSTOP");
+
+		mdelay(5);
+
+#if 0// Debug Message................
+		ate_print("\n**********************************************************\n");
+		ate_print("Noise Level: Channel %d\n", channel);
+		ate_print("RSSI0 = %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n",
+			RSSI[0][0], RSSI[0][1], RSSI[0][2],
+			RSSI[0][3], RSSI[0][4], RSSI[0][5],
+			RSSI[0][6], RSSI[0][7], RSSI[0][8],
+			RSSI[0][9]);
+		if ( pAd->Antenna.field.RxPath >= 2 ) // 2R
+		{
+			ate_print("RSSI1 = %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n",
+				RSSI[1][0], RSSI[1][1], RSSI[1][2],
+				RSSI[1][3], RSSI[1][4], RSSI[1][5],
+				RSSI[1][6], RSSI[1][7], RSSI[1][8],
+				RSSI[1][9]);
+		}
+		if ( pAd->Antenna.field.RxPath >= 3 ) // 3R
+		{
+			ate_print("RSSI2 = %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n",
+				RSSI[2][0], RSSI[2][1], RSSI[2][2],
+				RSSI[2][3], RSSI[2][4], RSSI[2][5],
+				RSSI[2][6], RSSI[2][7], RSSI[2][8],
+				RSSI[2][9]);
+		}
+#endif // 0 //
+		BubbleSort(10, RSSI[0]);	// 1R
+
+		if ( pAd->Antenna.field.RxPath >= 2 ) // 2R
+		{
+			BubbleSort(10, RSSI[1]);
+		}
+
+		if ( pAd->Antenna.field.RxPath >= 3 ) // 3R
+		{
+			BubbleSort(10, RSSI[2]);
+		}
+
+#if 0// Debug Message................
+		ate_print("\nAfter Sorting....Channel %d\n", channel);
+		ate_print("RSSI0 = %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n",
+			RSSI[0][0], RSSI[0][1], RSSI[0][2],
+			RSSI[0][3], RSSI[0][4], RSSI[0][5],
+			RSSI[0][6], RSSI[0][7], RSSI[0][8],
+			RSSI[0][9]);
+		if ( pAd->Antenna.field.RxPath >= 2 ) // 2R
+		{
+			ate_print("RSSI1 = %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n",
+				RSSI[1][0], RSSI[1][1], RSSI[1][2],
+				RSSI[1][3], RSSI[1][4], RSSI[1][5],
+				RSSI[1][6], RSSI[1][7], RSSI[1][8],
+				RSSI[1][9]);
+		}
+		if ( pAd->Antenna.field.RxPath >= 3 ) // 3R
+		{
+			ate_print("RSSI2 = %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n",
+				RSSI[2][0], RSSI[2][1], RSSI[2][2],
+				RSSI[2][3], RSSI[2][4], RSSI[2][5],
+				RSSI[2][6], RSSI[2][7], RSSI[2][8],
+				RSSI[2][9]);
+		}
+		ate_print("**********************************************************\n");
+#endif // 0 //
+	}
+
+	pAd->ate.Channel = Org_Channel;
+	ATEAsicSwitchChannel(pAd);
+
+	// Restore original value
+    ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, Org_BBP66value);
+    ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, Org_BBP69value);
+    ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, Org_BBP70value);
+
+	return;
+}
+
+BOOLEAN SyncTxRxConfig(PRTMP_ADAPTER pAd, USHORT offset, UCHAR value)
+{
+	UCHAR tmp = 0, bbp_data = 0;
+
+	if (ATE_ON(pAd))
+	{
+		ATE_BBP_IO_READ8_BY_REG_ID(pAd, offset, &bbp_data);
+	}
+	else
+	{
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, offset, &bbp_data);
+	}
+
+	/* confirm again */
+	ASSERT(bbp_data == value);
+
+	switch(offset)
+	{
+		case BBP_R1:
+			/* Need to sync. tx configuration with legacy ATE. */
+			tmp = (bbp_data & ((1 << 4) | (1 << 3))/* 0x18 */) >> 3;
+		    switch(tmp)
+		    {
+				/* The BBP R1 bit[4:3] = 2 :: Both DACs will be used by QA. */
+		        case 2:
+					/* All */
+					pAd->ate.TxAntennaSel = 0;
+		            break;
+				/* The BBP R1 bit[4:3] = 0 :: DAC 0 will be used by QA. */
+		        case 0:
+					/* Antenna one */
+					pAd->ate.TxAntennaSel = 1;
+		            break;
+				/* The BBP R1 bit[4:3] = 1 :: DAC 1 will be used by QA. */
+		        case 1:
+					/* Antenna two */
+					pAd->ate.TxAntennaSel = 2;
+		            break;
+		        default:
+		            DBGPRINT(RT_DEBUG_TRACE, ("%s -- Sth. wrong!  : return FALSE; \n", __FUNCTION__));
+		            return FALSE;
+		    }
+			break;/* case BBP_R1 */
+
+		case BBP_R3:
+			/* Need to sync. rx configuration with legacy ATE. */
+			tmp = (bbp_data & ((1 << 1) | (1 << 0))/* 0x03 */);
+		    switch(tmp)
+		    {
+				/* The BBP R3 bit[1:0] = 3 :: All ADCs will be used by QA. */
+		        case 3:
+					/* All */
+					pAd->ate.RxAntennaSel = 0;
+		            break;
+				/* The BBP R3 bit[1:0] = 0 :: ADC 0 will be used by QA, */
+				/* unless the BBP R3 bit[4:3] = 2 */
+		        case 0:
+					/* Antenna one */
+					pAd->ate.RxAntennaSel = 1;
+					tmp = ((bbp_data & ((1 << 4) | (1 << 3))/* 0x03 */) >> 3);
+					if (tmp == 2)// 3R
+					{
+						/* Default : All ADCs will be used by QA */
+						pAd->ate.RxAntennaSel = 0;
+					}
+		            break;
+				/* The BBP R3 bit[1:0] = 1 :: ADC 1 will be used by QA. */
+		        case 1:
+					/* Antenna two */
+					pAd->ate.RxAntennaSel = 2;
+		            break;
+				/* The BBP R3 bit[1:0] = 2 :: ADC 2 will be used by QA. */
+		        case 2:
+					/* Antenna three */
+					pAd->ate.RxAntennaSel = 3;
+		            break;
+		        default:
+		            DBGPRINT(RT_DEBUG_ERROR, ("%s -- Impossible!  : return FALSE; \n", __FUNCTION__));
+		            return FALSE;
+		    }
+			break;/* case BBP_R3 */
+
+        default:
+            DBGPRINT(RT_DEBUG_ERROR, ("%s -- Sth. wrong!  : return FALSE; \n", __FUNCTION__));
+            return FALSE;
+
+	}
+	return TRUE;
+}
+
+static VOID memcpy_exl(PRTMP_ADAPTER pAd, UCHAR *dst, UCHAR *src, ULONG len)
+{
+	ULONG i, Value = 0;
+	ULONG *pDst, *pSrc;
+	UCHAR *p8;
+
+	p8 = src;
+	pDst = (ULONG *) dst;
+	pSrc = (ULONG *) src;
+
+	for (i = 0 ; i < (len/4); i++)
+	{
+		/* For alignment issue, we need a variable "Value". */
+		memmove(&Value, pSrc, 4);
+		Value = htonl(Value);
+		memmove(pDst, &Value, 4);
+		pDst++;
+		pSrc++;
+	}
+	if ((len % 4) != 0)
+	{
+		/* wish that it will never reach here */
+		memmove(&Value, pSrc, (len % 4));
+		Value = htonl(Value);
+		memmove(pDst, &Value, (len % 4));
+	}
+}
+
+static VOID memcpy_exs(PRTMP_ADAPTER pAd, UCHAR *dst, UCHAR *src, ULONG len)
+{
+	ULONG i;
+	UCHAR *pDst, *pSrc;
+
+	pDst = dst;
+	pSrc = src;
+
+	for (i = 0; i < (len/2); i++)
+	{
+		memmove(pDst, pSrc, 2);
+		*((USHORT *)pDst) = htons(*((USHORT *)pDst));
+		pDst+=2;
+		pSrc+=2;
+	}
+
+	if ((len % 2) != 0)
+	{
+		memmove(pDst, pSrc, 1);
+	}
+}
+
+static VOID RTMP_IO_READ_BULK(PRTMP_ADAPTER pAd, UCHAR *dst, UCHAR *src, UINT32 len)
+{
+	UINT32 i, Value;
+	UINT32 *pDst, *pSrc;
+
+	pDst = (UINT32 *) dst;
+	pSrc = (UINT32 *) src;
+
+	for (i = 0 ; i < (len/4); i++)
+	{
+		RTMP_IO_READ32(pAd, (ULONG)pSrc, &Value);
+		Value = htonl(Value);
+		memmove(pDst, &Value, 4);
+		pDst++;
+		pSrc++;
+	}
+	return;
+}
+
+// TODO:
+#if 0
+/* These work only when RALINK_ATE is defined */
+INT Set_TxStart_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG value = simple_strtol(arg, 0, 10);
+	UCHAR buffer[26] = {0x88, 0x02, 0x2c, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x00, 0x55, 0x44, 0x33, 0x22, 0x11, 0xc0, 0x22, 0x00, 0x00};
+	POS_COOKIE pObj;
+
+	if (pAd->ate.TxStatus != 0)
+		return FALSE;
+
+	pAd->ate.TxInfo = 0x04000000;
+	bzero(&pAd->ate.TxWI, sizeof(TXWI_STRUC));
+	pAd->ate.TxWI.PHYMODE = 0;// MODE_CCK
+	pAd->ate.TxWI.MPDUtotalByteCount = 1226;
+	pAd->ate.TxWI.MCS = 3;
+	//pAd->ate.Mode = ATE_START;
+	pAd->ate.Mode |= ATE_TXFRAME;
+	pAd->ate.TxCount = value;
+	pAd->ate.QID = 0;
+	pAd->ate.HLen = 26;
+	pAd->ate.PLen = 0;
+	pAd->ate.DLen = 1200;
+	memcpy(pAd->ate.Header, buffer, 26);
+	pAd->ate.bQATxStart = TRUE;
+	//pObj = (POS_COOKIE) pAd->OS_Cookie;
+	//tasklet_hi_schedule(&pObj->AteTxTask);
+	return TRUE;
+}
+#endif  /* end of #if 0 */
+
+INT Set_TxStop_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ATEDBGPRINT(RT_DEBUG_TRACE,("Set_TxStop_Proc\n"));
+
+	if (Set_ATE_Proc(pAd, "TXSTOP"))
+	{
+	return TRUE;
+}
+	else
+	{
+		return FALSE;
+	}
+}
+
+INT Set_RxStop_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ATEDBGPRINT(RT_DEBUG_TRACE,("Set_RxStop_Proc\n"));
+
+	if (Set_ATE_Proc(pAd, "RXSTOP"))
+	{
+	return TRUE;
+}
+	else
+	{
+		return FALSE;
+	}
+}
+
+#if 0
+INT Set_EEWrite_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	USHORT offset = 0, value;
+	PUCHAR p2 = arg;
+
+	while((*p2 != ':') && (*p2 != '\0'))
+	{
+		p2++;
+	}
+
+	if (*p2 == ':')
+	{
+		A2Hex(offset, arg);
+		A2Hex(value, p2+ 1);
+	}
+	else
+	{
+		A2Hex(value, arg);
+	}
+
+	if (offset >= EEPROM_SIZE)
+	{
+		ate_print("Offset can not exceed EEPROM_SIZE( == 0x%04x)\n", EEPROM_SIZE);
+		return FALSE;
+	}
+
+	RTMP_EEPROM_WRITE16(pAd, offset, value);
+
+	return TRUE;
+}
+
+INT Set_BBPRead_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UCHAR value = 0, offset;
+
+	A2Hex(offset, arg);
+
+	if (ATE_ON(pAd))
+	{
+		ATE_BBP_IO_READ8_BY_REG_ID(pAd, offset,  &value);
+	}
+	else
+	{
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, offset,  &value);
+	}
+
+	ate_print("%x\n", value);
+
+	return TRUE;
+}
+
+
+INT Set_BBPWrite_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	USHORT offset = 0;
+	PUCHAR p2 = arg;
+	UCHAR value;
+
+	while((*p2 != ':') && (*p2 != '\0'))
+	{
+		p2++;
+	}
+
+	if (*p2 == ':')
+	{
+		A2Hex(offset, arg);
+		A2Hex(value, p2+ 1);
+	}
+	else
+	{
+		A2Hex(value, arg);
+	}
+
+	if (ATE_ON(pAd))
+	{
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, offset,  value);
+	}
+	else
+	{
+		RTNP_BBP_IO_WRITE8_BY_REG_ID(pAd, offset,  value);
+	}
+
+	return TRUE;
+}
+
+INT Set_RFWrite_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	PUCHAR p2, p3, p4;
+	ULONG R1, R2, R3, R4;
+
+	p2 = arg;
+
+	while((*p2 != ':') && (*p2 != '\0'))
+	{
+		p2++;
+	}
+
+	if (*p2 != ':')
+		return FALSE;
+
+	p3 = p2 + 1;
+
+	while((*p3 != ':') && (*p3 != '\0'))
+	{
+		p3++;
+	}
+
+	if (*p3 != ':')
+		return FALSE;
+
+	p4 = p3 + 1;
+
+	while((*p4 != ':') && (*p4 != '\0'))
+	{
+		p4++;
+	}
+
+	if (*p4 != ':')
+		return FALSE;
+
+
+	A2Hex(R1, arg);
+	A2Hex(R2, p2 + 1);
+	A2Hex(R3, p3 + 1);
+	A2Hex(R4, p4 + 1);
+
+	RTMP_RF_IO_WRITE32(pAd, R1);
+	RTMP_RF_IO_WRITE32(pAd, R2);
+	RTMP_RF_IO_WRITE32(pAd, R3);
+	RTMP_RF_IO_WRITE32(pAd, R4);
+
+	return TRUE;
+}
+#endif  // end of #if 0 //
+#endif	// RALINK_28xx_QA //
+
+#endif	// RALINK_ATE //
+
diff --git a/drivers/staging/rt2860/rt_ate.h b/drivers/staging/rt2860/rt_ate.h
new file mode 100644
index 00000000000..48aa70d2f01
--- /dev/null
+++ b/drivers/staging/rt2860/rt_ate.h
@@ -0,0 +1,353 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#ifndef __ATE_H__
+#define __ATE_H__
+
+#ifndef UCOS
+#define ate_print printk
+#define ATEDBGPRINT DBGPRINT
+#ifdef RT2860
+#define EEPROM_SIZE								0x200
+#ifdef CONFIG_STA_SUPPORT
+#define EEPROM_BIN_FILE_NAME  "/etc/Wireless/RT2860STA/e2p.bin"
+#endif // CONFIG_STA_SUPPORT //
+#endif // RT2860 //
+
+#else // !UCOS //
+#define fATE_LOAD_EEPROM						0x0C43
+#ifdef CONFIG_PRINTK
+extern INT ConsoleResponse(IN PUCHAR buff);
+extern int (*remote_display)(char *);
+extern void puts (const char *s);
+
+/* specificly defined to redirect and show ate-related messages to host. */
+/* Try to define ate_print as a macro. */
+#define ate_print(fmt, args...)                 \
+do{   int (*org_remote_display)(char *) = NULL;   \
+	org_remote_display = remote_display;\
+	/* Save original "remote_display" */\
+	remote_display = (int (*)(char *))ConsoleResponse;           \
+	printk(fmt, ## args);                       \
+	/* Restore the remote_display function pointer */        \
+	remote_display = org_remote_display; }while(0)
+
+#define ATEDBGPRINT(Level, Fmt)    	\
+{                                   \
+    if ((Level) <= RTDebugLevel)      \
+    {                               \
+        ate_print Fmt;					\
+    }                               \
+}
+#endif // CONFIG_PRINTK //
+#endif // !UCOS //
+
+#define ATE_ON(_p)              (((_p)->ate.Mode) != ATE_STOP)
+
+/* RT2880_iNIC will define "RT2860". */
+#ifdef RT2860
+#define ATE_BBP_IO_READ8_BY_REG_ID(_A, _I, _pV)        \
+{                                                       \
+    BBP_CSR_CFG_STRUC  BbpCsr;                             \
+    int             i, k;                               \
+    for (i=0; i<MAX_BUSY_COUNT; i++)                    \
+    {                                                   \
+        RTMP_IO_READ32(_A, BBP_CSR_CFG, &BbpCsr.word);     \
+        if (BbpCsr.field.Busy == BUSY)                  \
+        {                                               \
+            continue;                                   \
+        }                                               \
+        BbpCsr.word = 0;                                \
+        BbpCsr.field.fRead = 1;                         \
+        BbpCsr.field.BBP_RW_MODE = 1;                         \
+        BbpCsr.field.Busy = 1;                          \
+        BbpCsr.field.RegNum = _I;                       \
+        RTMP_IO_WRITE32(_A, BBP_CSR_CFG, BbpCsr.word);     \
+        for (k=0; k<MAX_BUSY_COUNT; k++)                \
+        {                                               \
+            RTMP_IO_READ32(_A, BBP_CSR_CFG, &BbpCsr.word); \
+            if (BbpCsr.field.Busy == IDLE)              \
+                break;                                  \
+        }                                               \
+        if ((BbpCsr.field.Busy == IDLE) &&              \
+            (BbpCsr.field.RegNum == _I))                \
+        {                                               \
+            *(_pV) = (UCHAR)BbpCsr.field.Value;         \
+            break;                                      \
+        }                                               \
+    }                                                   \
+    if (BbpCsr.field.Busy == BUSY)                      \
+    {                                                   \
+        ATEDBGPRINT(RT_DEBUG_ERROR, ("BBP read R%d fail\n", _I));      \
+        *(_pV) = (_A)->BbpWriteLatch[_I];               \
+    }                                                   \
+}
+
+#define ATE_BBP_IO_WRITE8_BY_REG_ID(_A, _I, _V)        \
+{                                                       \
+    BBP_CSR_CFG_STRUC  BbpCsr;                             \
+    int             BusyCnt;                            \
+    for (BusyCnt=0; BusyCnt<MAX_BUSY_COUNT; BusyCnt++)  \
+    {                                                   \
+        RTMP_IO_READ32(_A, BBP_CSR_CFG, &BbpCsr.word);     \
+        if (BbpCsr.field.Busy == BUSY)                  \
+            continue;                                   \
+        BbpCsr.word = 0;                                \
+        BbpCsr.field.fRead = 0;                         \
+        BbpCsr.field.BBP_RW_MODE = 1;                         \
+        BbpCsr.field.Busy = 1;                          \
+        BbpCsr.field.Value = _V;                        \
+        BbpCsr.field.RegNum = _I;                       \
+        RTMP_IO_WRITE32(_A, BBP_CSR_CFG, BbpCsr.word);     \
+        (_A)->BbpWriteLatch[_I] = _V;                   \
+        break;                                          \
+    }                                                   \
+    if (BusyCnt == MAX_BUSY_COUNT)                      \
+    {                                                   \
+        ATEDBGPRINT(RT_DEBUG_ERROR, ("BBP write R%d fail\n", _I));     \
+    }                                                   \
+}
+#endif // RT2860 //
+
+/* RT2880_iNIC will define RT2860. */
+#ifdef RT2860
+#define EEPROM_SIZE								0x200
+/* iNIC has its own EEPROM_BIN_FILE_NAME */
+#ifndef UCOS
+#ifdef CONFIG_STA_SUPPORT
+#define EEPROM_BIN_FILE_NAME  "/etc/Wireless/RT2860STA/e2p.bin"
+#endif // CONFIG_STA_SUPPORT //
+#endif // !UCOS //
+#endif // RT2860 //
+
+
+
+VOID rt_ee_read_all(
+	IN  PRTMP_ADAPTER   pAd,
+	OUT USHORT *Data);
+
+
+VOID rt_ee_write_all(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  USHORT *Data);
+
+INT Set_ATE_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_DA_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_SA_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_BSSID_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_CHANNEL_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_POWER0_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_POWER1_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_Antenna_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_RX_Antenna_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_FREQOFFSET_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_BW_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_LENGTH_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_COUNT_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_MCS_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_MODE_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_GI_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+
+INT	Set_ATE_RX_FER_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_ATE_Read_RF_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_ATE_Write_RF1_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_ATE_Write_RF2_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_ATE_Write_RF3_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_ATE_Write_RF4_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_ATE_Load_E2P_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_ATE_Read_E2P_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_Show_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_Help_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+#ifdef RALINK_ATE
+#ifdef RALINK_28xx_QA
+VOID ATE_QA_Statistics(
+	IN PRTMP_ADAPTER		pAd,
+	IN PRXWI_STRUC			pRxWI,
+	IN PRT28XX_RXD_STRUC    p28xxRxD,
+	IN PHEADER_802_11		pHeader);
+
+VOID RtmpDoAte(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq);
+
+VOID BubbleSort(
+	IN  INT32 n,
+	IN  INT32 a[]);
+
+VOID CalNoiseLevel(
+	IN  PRTMP_ADAPTER   pAdapter,
+	IN  UCHAR           channel,
+	OUT INT32           buffer[3][10]);
+
+BOOLEAN SyncTxRxConfig(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	USHORT			offset,
+	IN	UCHAR			value);
+
+#if 0
+INT Set_TxStart_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif  // 0 //
+
+INT Set_TxStop_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_RxStop_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+#if 0
+INT Set_EERead_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_EEWrite_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_BBPRead_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_BBPWrite_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_RFWrite_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif // end of #if 0 //
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+
+VOID ATEAsicSwitchChannel(
+	IN PRTMP_ADAPTER pAd);
+
+VOID ATEAsicAdjustTxPower(
+	IN PRTMP_ADAPTER pAd);
+
+VOID ATEDisableAsicProtect(
+	IN		PRTMP_ADAPTER	pAd);
+
+CHAR ATEConvertToRssi(
+	IN PRTMP_ADAPTER  pAd,
+	IN CHAR				Rssi,
+	IN UCHAR    RssiNumber);
+
+VOID ATESampleRssi(
+	IN PRTMP_ADAPTER	pAd,
+	IN PRXWI_STRUC		pRxWI);
+
+
+#ifdef CONFIG_STA_SUPPORT
+VOID RTMPStationStop(
+    IN  PRTMP_ADAPTER   pAd);
+
+VOID RTMPStationStart(
+    IN  PRTMP_ADAPTER   pAd);
+#endif // CONFIG_STA_SUPPORT //
+#endif // __ATE_H__ //
diff --git a/drivers/staging/rt2860/rt_config.h b/drivers/staging/rt2860/rt_config.h
new file mode 100644
index 00000000000..7ee7a405b02
--- /dev/null
+++ b/drivers/staging/rt2860/rt_config.h
@@ -0,0 +1,101 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rt_config.h
+
+	Abstract:
+	Central header file to maintain all include files for all NDIS
+	miniport driver routines.
+
+	Revision History:
+	Who         When          What
+	--------    ----------    ----------------------------------------------
+	Paul Lin    08-01-2002    created
+
+*/
+#ifndef	__RT_CONFIG_H__
+#define	__RT_CONFIG_H__
+
+#include    "rtmp_type.h"
+#ifdef UCOS
+#include "includes.h"
+#include <stdio.h>
+#include 	"rt_ucos.h"
+#endif
+
+#ifdef LINUX
+#include	"rt_linux.h"
+#endif
+#include    "rtmp_def.h"
+#include    "rt28xx.h"
+
+#ifdef RT2860
+#include	"rt2860.h"
+#endif // RT2860 //
+
+
+#include    "oid.h"
+#include    "mlme.h"
+#include    "wpa.h"
+#include    "md5.h"
+#include    "rtmp.h"
+#include	"ap.h"
+#include	"dfs.h"
+#include	"chlist.h"
+#include	"spectrum.h"
+
+#ifdef LEAP_SUPPORT
+#include    "leap.h"
+#endif // LEAP_SUPPORT //
+
+#ifdef BLOCK_NET_IF
+#include "netif_block.h"
+#endif // BLOCK_NET_IF //
+
+#ifdef IGMP_SNOOP_SUPPORT
+#include "igmp_snoop.h"
+#endif // IGMP_SNOOP_SUPPORT //
+
+#ifdef RALINK_ATE
+#include "rt_ate.h"
+#endif // RALINK_ATE //
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+#ifndef WPA_SUPPLICANT_SUPPORT
+#error "Build for being controlled by NetworkManager or wext, please set HAS_WPA_SUPPLICANT=y and HAS_NATIVE_WPA_SUPPLICANT_SUPPORT=y"
+#endif // WPA_SUPPLICANT_SUPPORT //
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef IKANOS_VX_1X0
+#include	"vr_ikans.h"
+#endif // IKANOS_VX_1X0 //
+
+#endif	// __RT_CONFIG_H__
+
diff --git a/drivers/staging/rt2860/rt_linux.c b/drivers/staging/rt2860/rt_linux.c
new file mode 100644
index 00000000000..374c174c88e
--- /dev/null
+++ b/drivers/staging/rt2860/rt_linux.c
@@ -0,0 +1,1054 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#include "rt_config.h"
+
+ULONG	RTDebugLevel = RT_DEBUG_ERROR;
+
+BUILD_TIMER_FUNCTION(MlmePeriodicExec);
+BUILD_TIMER_FUNCTION(AsicRxAntEvalTimeout);
+BUILD_TIMER_FUNCTION(APSDPeriodicExec);
+BUILD_TIMER_FUNCTION(AsicRfTuningExec);
+
+
+#ifdef CONFIG_STA_SUPPORT
+BUILD_TIMER_FUNCTION(BeaconTimeout);
+BUILD_TIMER_FUNCTION(ScanTimeout);
+BUILD_TIMER_FUNCTION(AuthTimeout);
+BUILD_TIMER_FUNCTION(AssocTimeout);
+BUILD_TIMER_FUNCTION(ReassocTimeout);
+BUILD_TIMER_FUNCTION(DisassocTimeout);
+BUILD_TIMER_FUNCTION(LinkDownExec);
+#ifdef LEAP_SUPPORT
+BUILD_TIMER_FUNCTION(LeapAuthTimeout);
+#endif
+BUILD_TIMER_FUNCTION(StaQuickResponeForRateUpExec);
+BUILD_TIMER_FUNCTION(WpaDisassocApAndBlockAssoc);
+#ifdef RT2860
+BUILD_TIMER_FUNCTION(PsPollWakeExec);
+BUILD_TIMER_FUNCTION(RadioOnExec);
+#endif // RT2860 //
+#ifdef QOS_DLS_SUPPORT
+BUILD_TIMER_FUNCTION(DlsTimeoutAction);
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+// for wireless system event message
+char const *pWirelessSysEventText[IW_SYS_EVENT_TYPE_NUM] = {
+	// system status event
+    "had associated successfully",							/* IW_ASSOC_EVENT_FLAG */
+    "had disassociated",									/* IW_DISASSOC_EVENT_FLAG */
+    "had deauthenticated",									/* IW_DEAUTH_EVENT_FLAG */
+    "had been aged-out and disassociated",					/* IW_AGEOUT_EVENT_FLAG */
+    "occurred CounterMeasures attack",						/* IW_COUNTER_MEASURES_EVENT_FLAG */
+    "occurred replay counter different in Key Handshaking",	/* IW_REPLAY_COUNTER_DIFF_EVENT_FLAG */
+    "occurred RSNIE different in Key Handshaking",			/* IW_RSNIE_DIFF_EVENT_FLAG */
+    "occurred MIC different in Key Handshaking",			/* IW_MIC_DIFF_EVENT_FLAG */
+    "occurred ICV error in RX",								/* IW_ICV_ERROR_EVENT_FLAG */
+    "occurred MIC error in RX",								/* IW_MIC_ERROR_EVENT_FLAG */
+	"Group Key Handshaking timeout",						/* IW_GROUP_HS_TIMEOUT_EVENT_FLAG */
+	"Pairwise Key Handshaking timeout",						/* IW_PAIRWISE_HS_TIMEOUT_EVENT_FLAG */
+	"RSN IE sanity check failure",							/* IW_RSNIE_SANITY_FAIL_EVENT_FLAG */
+	"set key done in WPA/WPAPSK",							/* IW_SET_KEY_DONE_WPA1_EVENT_FLAG */
+	"set key done in WPA2/WPA2PSK",                         /* IW_SET_KEY_DONE_WPA2_EVENT_FLAG */
+	"connects with our wireless client",                    /* IW_STA_LINKUP_EVENT_FLAG */
+	"disconnects with our wireless client",                 /* IW_STA_LINKDOWN_EVENT_FLAG */
+	"scan completed"										/* IW_SCAN_COMPLETED_EVENT_FLAG */
+	"scan terminate!! Busy!! Enqueue fail!!"				/* IW_SCAN_ENQUEUE_FAIL_EVENT_FLAG */
+	};
+
+// for wireless IDS_spoof_attack event message
+char const *pWirelessSpoofEventText[IW_SPOOF_EVENT_TYPE_NUM] = {
+    "detected conflict SSID",								/* IW_CONFLICT_SSID_EVENT_FLAG */
+    "detected spoofed association response",				/* IW_SPOOF_ASSOC_RESP_EVENT_FLAG */
+    "detected spoofed reassociation responses",				/* IW_SPOOF_REASSOC_RESP_EVENT_FLAG */
+    "detected spoofed probe response",						/* IW_SPOOF_PROBE_RESP_EVENT_FLAG */
+    "detected spoofed beacon",								/* IW_SPOOF_BEACON_EVENT_FLAG */
+    "detected spoofed disassociation",						/* IW_SPOOF_DISASSOC_EVENT_FLAG */
+    "detected spoofed authentication",						/* IW_SPOOF_AUTH_EVENT_FLAG */
+    "detected spoofed deauthentication",					/* IW_SPOOF_DEAUTH_EVENT_FLAG */
+    "detected spoofed unknown management frame",			/* IW_SPOOF_UNKNOWN_MGMT_EVENT_FLAG */
+	"detected replay attack"								/* IW_REPLAY_ATTACK_EVENT_FLAG */
+	};
+
+// for wireless IDS_flooding_attack event message
+char const *pWirelessFloodEventText[IW_FLOOD_EVENT_TYPE_NUM] = {
+	"detected authentication flooding",						/* IW_FLOOD_AUTH_EVENT_FLAG */
+    "detected association request flooding",				/* IW_FLOOD_ASSOC_REQ_EVENT_FLAG */
+    "detected reassociation request flooding",				/* IW_FLOOD_REASSOC_REQ_EVENT_FLAG */
+    "detected probe request flooding",						/* IW_FLOOD_PROBE_REQ_EVENT_FLAG */
+    "detected disassociation flooding",						/* IW_FLOOD_DISASSOC_EVENT_FLAG */
+    "detected deauthentication flooding",					/* IW_FLOOD_DEAUTH_EVENT_FLAG */
+    "detected 802.1x eap-request flooding"					/* IW_FLOOD_EAP_REQ_EVENT_FLAG */
+	};
+
+/* timeout -- ms */
+VOID RTMP_SetPeriodicTimer(
+	IN	NDIS_MINIPORT_TIMER *pTimer,
+	IN	unsigned long timeout)
+{
+	timeout = ((timeout*HZ) / 1000);
+	pTimer->expires = jiffies + timeout;
+	add_timer(pTimer);
+}
+
+/* convert NdisMInitializeTimer --> RTMP_OS_Init_Timer */
+VOID RTMP_OS_Init_Timer(
+	IN	PRTMP_ADAPTER pAd,
+	IN	NDIS_MINIPORT_TIMER *pTimer,
+	IN	TIMER_FUNCTION function,
+	IN	PVOID data)
+{
+	init_timer(pTimer);
+    pTimer->data = (unsigned long)data;
+    pTimer->function = function;
+}
+
+
+VOID RTMP_OS_Add_Timer(
+	IN	NDIS_MINIPORT_TIMER		*pTimer,
+	IN	unsigned long timeout)
+{
+	if (timer_pending(pTimer))
+		return;
+
+	timeout = ((timeout*HZ) / 1000);
+	pTimer->expires = jiffies + timeout;
+	add_timer(pTimer);
+}
+
+VOID RTMP_OS_Mod_Timer(
+	IN	NDIS_MINIPORT_TIMER		*pTimer,
+	IN	unsigned long timeout)
+{
+	timeout = ((timeout*HZ) / 1000);
+	mod_timer(pTimer, jiffies + timeout);
+}
+
+VOID RTMP_OS_Del_Timer(
+	IN	NDIS_MINIPORT_TIMER		*pTimer,
+	OUT	BOOLEAN					*pCancelled)
+{
+	if (timer_pending(pTimer))
+	{
+		*pCancelled = del_timer_sync(pTimer);
+	}
+	else
+	{
+		*pCancelled = TRUE;
+	}
+
+}
+
+VOID RTMP_OS_Release_Packet(
+	IN	PRTMP_ADAPTER pAd,
+	IN	PQUEUE_ENTRY  pEntry)
+{
+	//RTMPFreeNdisPacket(pAd, (struct sk_buff *)pEntry);
+}
+
+// Unify all delay routine by using udelay
+VOID RTMPusecDelay(
+	IN	ULONG	usec)
+{
+	ULONG	i;
+
+	for (i = 0; i < (usec / 50); i++)
+		udelay(50);
+
+	if (usec % 50)
+		udelay(usec % 50);
+}
+
+void RTMP_GetCurrentSystemTime(LARGE_INTEGER *time)
+{
+	time->u.LowPart = jiffies;
+}
+
+// pAd MUST allow to be NULL
+NDIS_STATUS os_alloc_mem(
+	IN	PRTMP_ADAPTER pAd,
+	OUT	PUCHAR *mem,
+	IN	ULONG  size)
+{
+	*mem = (PUCHAR) kmalloc(size, GFP_ATOMIC);
+	if (*mem)
+		return (NDIS_STATUS_SUCCESS);
+	else
+		return (NDIS_STATUS_FAILURE);
+}
+
+// pAd MUST allow to be NULL
+NDIS_STATUS os_free_mem(
+	IN	PRTMP_ADAPTER pAd,
+	IN	PUCHAR mem)
+{
+
+	ASSERT(mem);
+	kfree(mem);
+	return (NDIS_STATUS_SUCCESS);
+}
+
+
+PNDIS_PACKET RTMP_AllocateFragPacketBuffer(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length)
+{
+	struct sk_buff *pkt;
+
+	pkt = dev_alloc_skb(Length);
+
+	if (pkt == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("can't allocate frag rx %ld size packet\n",Length));
+	}
+
+	if (pkt)
+	{
+		RTMP_SET_PACKET_SOURCE(OSPKT_TO_RTPKT(pkt), PKTSRC_NDIS);
+	}
+
+	return (PNDIS_PACKET) pkt;
+}
+
+
+PNDIS_PACKET RTMP_AllocateTxPacketBuffer(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress)
+{
+	struct sk_buff *pkt;
+
+	pkt = dev_alloc_skb(Length);
+
+	if (pkt == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("can't allocate tx %ld size packet\n",Length));
+	}
+
+	if (pkt)
+	{
+		RTMP_SET_PACKET_SOURCE(OSPKT_TO_RTPKT(pkt), PKTSRC_NDIS);
+		*VirtualAddress = (PVOID) pkt->data;
+	}
+	else
+	{
+		*VirtualAddress = (PVOID) NULL;
+	}
+
+	return (PNDIS_PACKET) pkt;
+}
+
+
+VOID build_tx_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	PUCHAR	pFrame,
+	IN	ULONG	FrameLen)
+{
+
+	struct sk_buff	*pTxPkt;
+
+	ASSERT(pPacket);
+	pTxPkt = RTPKT_TO_OSPKT(pPacket);
+
+	NdisMoveMemory(skb_put(pTxPkt, FrameLen), pFrame, FrameLen);
+}
+
+VOID	RTMPFreeAdapter(
+	IN	PRTMP_ADAPTER	pAd)
+{
+    POS_COOKIE os_cookie;
+	int index;
+
+	os_cookie=(POS_COOKIE)pAd->OS_Cookie;
+
+	kfree(pAd->BeaconBuf);
+
+
+	NdisFreeSpinLock(&pAd->MgmtRingLock);
+
+#ifdef RT2860
+	NdisFreeSpinLock(&pAd->RxRingLock);
+#endif // RT2860 //
+
+	for (index =0 ; index < NUM_OF_TX_RING; index++)
+	{
+    	NdisFreeSpinLock(&pAd->TxSwQueueLock[index]);
+		NdisFreeSpinLock(&pAd->DeQueueLock[index]);
+		pAd->DeQueueRunning[index] = FALSE;
+	}
+
+	NdisFreeSpinLock(&pAd->irq_lock);
+
+	vfree(pAd); // pci_free_consistent(os_cookie->pci_dev,sizeof(RTMP_ADAPTER),pAd,os_cookie->pAd_pa);
+	kfree(os_cookie);
+}
+
+BOOLEAN OS_Need_Clone_Packet(void)
+{
+	return (FALSE);
+}
+
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		clone an input NDIS PACKET to another one. The new internally created NDIS PACKET
+		must have only one NDIS BUFFER
+		return - byte copied. 0 means can't create NDIS PACKET
+		NOTE: internally created NDIS_PACKET should be destroyed by RTMPFreeNdisPacket
+
+	Arguments:
+		pAd 	Pointer to our adapter
+		pInsAMSDUHdr	EWC A-MSDU format has extra 14-bytes header. if TRUE, insert this 14-byte hdr in front of MSDU.
+		*pSrcTotalLen			return total packet length. This lenght is calculated with 802.3 format packet.
+
+	Return Value:
+		NDIS_STATUS_SUCCESS
+		NDIS_STATUS_FAILURE
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS RTMPCloneNdisPacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	BOOLEAN			pInsAMSDUHdr,
+	IN	PNDIS_PACKET	pInPacket,
+	OUT PNDIS_PACKET   *ppOutPacket)
+{
+
+	struct sk_buff *pkt;
+
+	ASSERT(pInPacket);
+	ASSERT(ppOutPacket);
+
+	// 1. Allocate a packet
+	pkt = dev_alloc_skb(2048);
+
+	if (pkt == NULL)
+	{
+		return NDIS_STATUS_FAILURE;
+	}
+
+ 	skb_put(pkt, GET_OS_PKT_LEN(pInPacket));
+	NdisMoveMemory(pkt->data, GET_OS_PKT_DATAPTR(pInPacket), GET_OS_PKT_LEN(pInPacket));
+	*ppOutPacket = OSPKT_TO_RTPKT(pkt);
+
+
+	RTMP_SET_PACKET_SOURCE(OSPKT_TO_RTPKT(pkt), PKTSRC_NDIS);
+
+	printk("###Clone###\n");
+
+	return NDIS_STATUS_SUCCESS;
+}
+
+
+// the allocated NDIS PACKET must be freed via RTMPFreeNdisPacket()
+NDIS_STATUS RTMPAllocateNdisPacket(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT PNDIS_PACKET   *ppPacket,
+	IN	PUCHAR			pHeader,
+	IN	UINT			HeaderLen,
+	IN	PUCHAR			pData,
+	IN	UINT			DataLen)
+{
+	PNDIS_PACKET	pPacket;
+	ASSERT(pData);
+	ASSERT(DataLen);
+
+	// 1. Allocate a packet
+	pPacket = (PNDIS_PACKET *) dev_alloc_skb(HeaderLen + DataLen + TXPADDING_SIZE);
+	if (pPacket == NULL)
+ 	{
+		*ppPacket = NULL;
+#ifdef DEBUG
+		printk("RTMPAllocateNdisPacket Fail\n\n");
+#endif
+		return NDIS_STATUS_FAILURE;
+	}
+
+	// 2. clone the frame content
+	if (HeaderLen > 0)
+		NdisMoveMemory(GET_OS_PKT_DATAPTR(pPacket), pHeader, HeaderLen);
+	if (DataLen > 0)
+		NdisMoveMemory(GET_OS_PKT_DATAPTR(pPacket) + HeaderLen, pData, DataLen);
+
+	// 3. update length of packet
+ 	skb_put(GET_OS_PKT_TYPE(pPacket), HeaderLen+DataLen);
+
+	RTMP_SET_PACKET_SOURCE(pPacket, PKTSRC_NDIS);
+//	printk("%s : pPacket = %p, len = %d\n", __FUNCTION__, pPacket, GET_OS_PKT_LEN(pPacket));
+	*ppPacket = pPacket;
+	return NDIS_STATUS_SUCCESS;
+}
+
+/*
+  ========================================================================
+  Description:
+	This routine frees a miniport internally allocated NDIS_PACKET and its
+	corresponding NDIS_BUFFER and allocated memory.
+  ========================================================================
+*/
+VOID RTMPFreeNdisPacket(
+	IN PRTMP_ADAPTER pAd,
+	IN PNDIS_PACKET  pPacket)
+{
+	dev_kfree_skb_any(RTPKT_TO_OSPKT(pPacket));
+}
+
+
+// IRQL = DISPATCH_LEVEL
+// NOTE: we do have an assumption here, that Byte0 and Byte1 always reasid at the same
+//			 scatter gather buffer
+NDIS_STATUS Sniff2BytesFromNdisBuffer(
+	IN	PNDIS_BUFFER	pFirstBuffer,
+	IN	UCHAR			DesiredOffset,
+	OUT PUCHAR			pByte0,
+	OUT PUCHAR			pByte1)
+{
+    *pByte0 = *(PUCHAR)(pFirstBuffer + DesiredOffset);
+    *pByte1 = *(PUCHAR)(pFirstBuffer + DesiredOffset + 1);
+
+	return NDIS_STATUS_SUCCESS;
+}
+
+
+void RTMP_QueryPacketInfo(
+	IN  PNDIS_PACKET pPacket,
+	OUT PACKET_INFO  *pPacketInfo,
+	OUT PUCHAR		 *pSrcBufVA,
+	OUT	UINT		 *pSrcBufLen)
+{
+	pPacketInfo->BufferCount = 1;
+	pPacketInfo->pFirstBuffer = GET_OS_PKT_DATAPTR(pPacket);
+	pPacketInfo->PhysicalBufferCount = 1;
+	pPacketInfo->TotalPacketLength = GET_OS_PKT_LEN(pPacket);
+
+	*pSrcBufVA = GET_OS_PKT_DATAPTR(pPacket);
+	*pSrcBufLen = GET_OS_PKT_LEN(pPacket);
+}
+
+void RTMP_QueryNextPacketInfo(
+	IN  PNDIS_PACKET *ppPacket,
+	OUT PACKET_INFO  *pPacketInfo,
+	OUT PUCHAR		 *pSrcBufVA,
+	OUT	UINT		 *pSrcBufLen)
+{
+	PNDIS_PACKET pPacket = NULL;
+
+	if (*ppPacket)
+		pPacket = GET_OS_PKT_NEXT(*ppPacket);
+
+	if (pPacket)
+	{
+		pPacketInfo->BufferCount = 1;
+		pPacketInfo->pFirstBuffer = GET_OS_PKT_DATAPTR(pPacket);
+		pPacketInfo->PhysicalBufferCount = 1;
+		pPacketInfo->TotalPacketLength = GET_OS_PKT_LEN(pPacket);
+
+		*pSrcBufVA = GET_OS_PKT_DATAPTR(pPacket);
+		*pSrcBufLen = GET_OS_PKT_LEN(pPacket);
+		*ppPacket = GET_OS_PKT_NEXT(pPacket);
+	}
+	else
+	{
+		pPacketInfo->BufferCount = 0;
+		pPacketInfo->pFirstBuffer = NULL;
+		pPacketInfo->PhysicalBufferCount = 0;
+		pPacketInfo->TotalPacketLength = 0;
+
+		*pSrcBufVA = NULL;
+		*pSrcBufLen = 0;
+		*ppPacket = NULL;
+	}
+}
+
+// not yet support MBSS
+PNET_DEV get_netdev_from_bssid(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			FromWhichBSSID)
+{
+    PNET_DEV dev_p = NULL;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		dev_p = pAd->net_dev;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	ASSERT(dev_p);
+	return dev_p; /* return one of MBSS */
+}
+
+PNDIS_PACKET DuplicatePacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	UCHAR			FromWhichBSSID)
+{
+	struct sk_buff	*skb;
+	PNDIS_PACKET	pRetPacket = NULL;
+	USHORT			DataSize;
+	UCHAR			*pData;
+
+	DataSize = (USHORT) GET_OS_PKT_LEN(pPacket);
+	pData = (PUCHAR) GET_OS_PKT_DATAPTR(pPacket);
+
+
+	skb = skb_clone(RTPKT_TO_OSPKT(pPacket), MEM_ALLOC_FLAG);
+	if (skb)
+	{
+		skb->dev = get_netdev_from_bssid(pAd, FromWhichBSSID);
+		pRetPacket = OSPKT_TO_RTPKT(skb);
+	}
+
+	return pRetPacket;
+
+}
+
+PNDIS_PACKET duplicate_pkt(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pHeader802_3,
+    IN  UINT            HdrLen,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize,
+	IN	UCHAR			FromWhichBSSID)
+{
+	struct sk_buff	*skb;
+	PNDIS_PACKET	pPacket = NULL;
+
+
+	if ((skb = __dev_alloc_skb(HdrLen + DataSize + 2, MEM_ALLOC_FLAG)) != NULL)
+	{
+		skb_reserve(skb, 2);
+		NdisMoveMemory(skb->tail, pHeader802_3, HdrLen);
+		skb_put(skb, HdrLen);
+		NdisMoveMemory(skb->tail, pData, DataSize);
+		skb_put(skb, DataSize);
+		skb->dev = get_netdev_from_bssid(pAd, FromWhichBSSID);
+		pPacket = OSPKT_TO_RTPKT(skb);
+	}
+
+	return pPacket;
+}
+
+
+#define TKIP_TX_MIC_SIZE		8
+PNDIS_PACKET duplicate_pkt_with_TKIP_MIC(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket)
+{
+	struct sk_buff	*skb, *newskb;
+
+
+	skb = RTPKT_TO_OSPKT(pPacket);
+	if (skb_tailroom(skb) < TKIP_TX_MIC_SIZE)
+	{
+		// alloc a new skb and copy the packet
+		newskb = skb_copy_expand(skb, skb_headroom(skb), TKIP_TX_MIC_SIZE, GFP_ATOMIC);
+		dev_kfree_skb_any(skb);
+		if (newskb == NULL)
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("Extend Tx.MIC for packet failed!, dropping packet!\n"));
+			return NULL;
+		}
+		skb = newskb;
+	}
+
+	return OSPKT_TO_RTPKT(skb);
+}
+
+
+
+
+PNDIS_PACKET ClonePacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize)
+{
+	struct sk_buff	*pRxPkt;
+	struct sk_buff	*pClonedPkt;
+
+	ASSERT(pPacket);
+	pRxPkt = RTPKT_TO_OSPKT(pPacket);
+
+	// clone the packet
+	pClonedPkt = skb_clone(pRxPkt, MEM_ALLOC_FLAG);
+
+	if (pClonedPkt)
+	{
+    	// set the correct dataptr and data len
+    	pClonedPkt->dev = pRxPkt->dev;
+    	pClonedPkt->data = pData;
+    	pClonedPkt->len = DataSize;
+    	pClonedPkt->tail = pClonedPkt->data + pClonedPkt->len;
+		ASSERT(DataSize < 1530);
+	}
+	return pClonedPkt;
+}
+
+//
+// change OS packet DataPtr and DataLen
+//
+void  update_os_packet_info(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN  UCHAR			FromWhichBSSID)
+{
+	struct sk_buff	*pOSPkt;
+
+	ASSERT(pRxBlk->pRxPacket);
+	pOSPkt = RTPKT_TO_OSPKT(pRxBlk->pRxPacket);
+
+	pOSPkt->dev = get_netdev_from_bssid(pAd, FromWhichBSSID);
+	pOSPkt->data = pRxBlk->pData;
+	pOSPkt->len = pRxBlk->DataSize;
+	pOSPkt->tail = pOSPkt->data + pOSPkt->len;
+}
+
+
+void wlan_802_11_to_802_3_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	PUCHAR			pHeader802_3,
+	IN  UCHAR			FromWhichBSSID)
+{
+	struct sk_buff	*pOSPkt;
+
+	ASSERT(pRxBlk->pRxPacket);
+	ASSERT(pHeader802_3);
+
+	pOSPkt = RTPKT_TO_OSPKT(pRxBlk->pRxPacket);
+
+	pOSPkt->dev = get_netdev_from_bssid(pAd, FromWhichBSSID);
+	pOSPkt->data = pRxBlk->pData;
+	pOSPkt->len = pRxBlk->DataSize;
+	pOSPkt->tail = pOSPkt->data + pOSPkt->len;
+
+	//
+	// copy 802.3 header
+	//
+	//
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		NdisMoveMemory(skb_push(pOSPkt, LENGTH_802_3), pHeader802_3, LENGTH_802_3);
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+
+
+void announce_802_3_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket)
+{
+
+	struct sk_buff	*pRxPkt;
+
+	ASSERT(pPacket);
+
+	pRxPkt = RTPKT_TO_OSPKT(pPacket);
+
+    /* Push up the protocol stack */
+#ifdef IKANOS_VX_1X0
+	IKANOS_DataFrameRx(pAd, pRxPkt->dev, pRxPkt, pRxPkt->len);
+#else
+	pRxPkt->protocol = eth_type_trans(pRxPkt, pRxPkt->dev);
+
+	netif_rx(pRxPkt);
+#endif // IKANOS_VX_1X0 //
+}
+
+
+PRTMP_SCATTER_GATHER_LIST
+rt_get_sg_list_from_packet(PNDIS_PACKET pPacket, RTMP_SCATTER_GATHER_LIST *sg)
+{
+	sg->NumberOfElements = 1;
+	sg->Elements[0].Address =  GET_OS_PKT_DATAPTR(pPacket);
+	sg->Elements[0].Length = GET_OS_PKT_LEN(pPacket);
+	return (sg);
+}
+
+void hex_dump(char *str, unsigned char *pSrcBufVA, unsigned int SrcBufLen)
+{
+	unsigned char *pt;
+	int x;
+
+	if (RTDebugLevel < RT_DEBUG_TRACE)
+		return;
+
+	pt = pSrcBufVA;
+	printk("%s: %p, len = %d\n",str,  pSrcBufVA, SrcBufLen);
+	for (x=0; x<SrcBufLen; x++)
+	{
+		if (x % 16 == 0)
+			printk("0x%04x : ", x);
+		printk("%02x ", ((unsigned char)pt[x]));
+		if (x%16 == 15) printk("\n");
+	}
+	printk("\n");
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Send log message through wireless event
+
+		Support standard iw_event with IWEVCUSTOM. It is used below.
+
+		iwreq_data.data.flags is used to store event_flag that is defined by user.
+		iwreq_data.data.length is the length of the event log.
+
+		The format of the event log is composed of the entry's MAC address and
+		the desired log message (refer to pWirelessEventText).
+
+			ex: 11:22:33:44:55:66 has associated successfully
+
+		p.s. The requirement of Wireless Extension is v15 or newer.
+
+	========================================================================
+*/
+VOID RTMPSendWirelessEvent(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Event_flag,
+	IN	PUCHAR 			pAddr,
+	IN	UCHAR			BssIdx,
+	IN	CHAR			Rssi)
+{
+#if WIRELESS_EXT >= 15
+
+	union 	iwreq_data      wrqu;
+	PUCHAR 	pBuf = NULL, pBufPtr = NULL;
+	USHORT	event, type, BufLen;
+	UCHAR	event_table_len = 0;
+
+	type = Event_flag & 0xFF00;
+	event = Event_flag & 0x00FF;
+
+	switch (type)
+	{
+		case IW_SYS_EVENT_FLAG_START:
+			event_table_len = IW_SYS_EVENT_TYPE_NUM;
+			break;
+
+		case IW_SPOOF_EVENT_FLAG_START:
+			event_table_len = IW_SPOOF_EVENT_TYPE_NUM;
+			break;
+
+		case IW_FLOOD_EVENT_FLAG_START:
+			event_table_len = IW_FLOOD_EVENT_TYPE_NUM;
+			break;
+	}
+
+	if (event_table_len == 0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s : The type(%0x02x) is not valid.\n", __FUNCTION__, type));
+		return;
+	}
+
+	if (event >= event_table_len)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s : The event(%0x02x) is not valid.\n", __FUNCTION__, event));
+		return;
+	}
+
+	//Allocate memory and copy the msg.
+	if((pBuf = kmalloc(IW_CUSTOM_MAX_LEN, GFP_ATOMIC)) != NULL)
+	{
+		//Prepare the payload
+		memset(pBuf, 0, IW_CUSTOM_MAX_LEN);
+
+		pBufPtr = pBuf;
+
+		if (pAddr)
+			pBufPtr += sprintf(pBufPtr, "(RT2860) STA(%02x:%02x:%02x:%02x:%02x:%02x) ", PRINT_MAC(pAddr));
+		else if (BssIdx < MAX_MBSSID_NUM)
+			pBufPtr += sprintf(pBufPtr, "(RT2860) BSS(ra%d) ", BssIdx);
+		else
+			pBufPtr += sprintf(pBufPtr, "(RT2860) ");
+
+		if (type == IW_SYS_EVENT_FLAG_START)
+			pBufPtr += sprintf(pBufPtr, "%s", pWirelessSysEventText[event]);
+		else if (type == IW_SPOOF_EVENT_FLAG_START)
+			pBufPtr += sprintf(pBufPtr, "%s (RSSI=%d)", pWirelessSpoofEventText[event], Rssi);
+		else if (type == IW_FLOOD_EVENT_FLAG_START)
+			pBufPtr += sprintf(pBufPtr, "%s", pWirelessFloodEventText[event]);
+		else
+			pBufPtr += sprintf(pBufPtr, "%s", "unknown event");
+
+		pBufPtr[pBufPtr - pBuf] = '\0';
+		BufLen = pBufPtr - pBuf;
+
+		memset(&wrqu, 0, sizeof(wrqu));
+	    wrqu.data.flags = Event_flag;
+		wrqu.data.length = BufLen;
+
+		//send wireless event
+	    wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, pBuf);
+
+		//DBGPRINT(RT_DEBUG_TRACE, ("%s : %s\n", __FUNCTION__, pBuf));
+
+		kfree(pBuf);
+	}
+	else
+		DBGPRINT(RT_DEBUG_ERROR, ("%s : Can't allocate memory for wireless event.\n", __FUNCTION__));
+#else
+	DBGPRINT(RT_DEBUG_ERROR, ("%s : The Wireless Extension MUST be v15 or newer.\n", __FUNCTION__));
+#endif  /* WIRELESS_EXT >= 15 */
+}
+
+
+#ifdef CONFIG_STA_SUPPORT
+void send_monitor_packets(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk)
+{
+    struct sk_buff	*pOSPkt;
+    wlan_ng_prism2_header *ph;
+    int rate_index = 0;
+    USHORT header_len = 0;
+    UCHAR temp_header[40] = {0};
+
+    u_int32_t ralinkrate[256] = {2,4,11,22, 12,18,24,36,48,72,96,  108,   109, 110, 111, 112, 13, 26, 39, 52,78,104, 117, 130, 26, 52, 78,104, 156, 208, 234, 260, 27, 54,81,108,162, 216, 243, 270, // Last 38
+	54, 108, 162, 216, 324, 432, 486, 540,  14, 29, 43, 57, 87, 115, 130, 144, 29, 59,87,115, 173, 230,260, 288, 30, 60,90,120,180,240,270,300,60,120,180,240,360,480,540,600, 0,1,2,3,4,5,6,7,8,9,10,
+	11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30,31,32,33,34,35,36,37,38,39,40,41,42,43,44,45,46,47,48,49,50,51,52,53,54,55,56,57,58,59,60,61,62,63,64,65,66,67,68,69,70,71,72,73,74,75,76,77,78,79,80};
+
+
+    ASSERT(pRxBlk->pRxPacket);
+    if (pRxBlk->DataSize < 10)
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("%s : Size is too small! (%d)\n", __FUNCTION__, pRxBlk->DataSize));
+		goto err_free_sk_buff;
+    }
+
+    if (pRxBlk->DataSize + sizeof(wlan_ng_prism2_header) > RX_BUFFER_AGGRESIZE)
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("%s : Size is too large! (%d)\n", __FUNCTION__, pRxBlk->DataSize + sizeof(wlan_ng_prism2_header)));
+		goto err_free_sk_buff;
+    }
+
+    pOSPkt = RTPKT_TO_OSPKT(pRxBlk->pRxPacket);
+	pOSPkt->dev = get_netdev_from_bssid(pAd, BSS0);
+    if (pRxBlk->pHeader->FC.Type == BTYPE_DATA)
+    {
+        pRxBlk->DataSize -= LENGTH_802_11;
+        if ((pRxBlk->pHeader->FC.ToDs == 1) &&
+            (pRxBlk->pHeader->FC.FrDs == 1))
+            header_len = LENGTH_802_11_WITH_ADDR4;
+        else
+            header_len = LENGTH_802_11;
+
+        // QOS
+    	if (pRxBlk->pHeader->FC.SubType & 0x08)
+    	{
+    	    header_len += 2;
+    		// Data skip QOS contorl field
+    		pRxBlk->DataSize -=2;
+    	}
+
+    	// Order bit: A-Ralink or HTC+
+    	if (pRxBlk->pHeader->FC.Order)
+    	{
+    	    header_len += 4;
+			// Data skip HTC contorl field
+			pRxBlk->DataSize -= 4;
+    	}
+
+        // Copy Header
+        if (header_len <= 40)
+            NdisMoveMemory(temp_header, pRxBlk->pData, header_len);
+
+        // skip HW padding
+    	if (pRxBlk->RxD.L2PAD)
+    	    pRxBlk->pData += (header_len + 2);
+        else
+            pRxBlk->pData += header_len;
+    } //end if
+
+
+	if (pRxBlk->DataSize < pOSPkt->len) {
+        skb_trim(pOSPkt,pRxBlk->DataSize);
+    } else {
+        skb_put(pOSPkt,(pRxBlk->DataSize - pOSPkt->len));
+    } //end if
+
+    if ((pRxBlk->pData - pOSPkt->data) > 0) {
+	    skb_put(pOSPkt,(pRxBlk->pData - pOSPkt->data));
+	    skb_pull(pOSPkt,(pRxBlk->pData - pOSPkt->data));
+    } //end if
+
+    if (skb_headroom(pOSPkt) < (sizeof(wlan_ng_prism2_header)+ header_len)) {
+        if (pskb_expand_head(pOSPkt, (sizeof(wlan_ng_prism2_header) + header_len), 0, GFP_ATOMIC)) {
+	        DBGPRINT(RT_DEBUG_ERROR, ("%s : Reallocate header size of sk_buff fail!\n", __FUNCTION__));
+			goto err_free_sk_buff;
+	    } //end if
+    } //end if
+
+    if (header_len > 0)
+        NdisMoveMemory(skb_push(pOSPkt, header_len), temp_header, header_len);
+
+    ph = (wlan_ng_prism2_header *) skb_push(pOSPkt, sizeof(wlan_ng_prism2_header));
+	NdisZeroMemory(ph, sizeof(wlan_ng_prism2_header));
+
+    ph->msgcode		    = DIDmsg_lnxind_wlansniffrm;
+	ph->msglen		    = sizeof(wlan_ng_prism2_header);
+	strcpy(ph->devname, pAd->net_dev->name);
+
+    ph->hosttime.did = DIDmsg_lnxind_wlansniffrm_hosttime;
+	ph->hosttime.status = 0;
+	ph->hosttime.len = 4;
+	ph->hosttime.data = jiffies;
+
+	ph->mactime.did = DIDmsg_lnxind_wlansniffrm_mactime;
+	ph->mactime.status = 0;
+	ph->mactime.len = 0;
+	ph->mactime.data = 0;
+
+    ph->istx.did = DIDmsg_lnxind_wlansniffrm_istx;
+	ph->istx.status = 0;
+	ph->istx.len = 0;
+	ph->istx.data = 0;
+
+    ph->channel.did = DIDmsg_lnxind_wlansniffrm_channel;
+	ph->channel.status = 0;
+	ph->channel.len = 4;
+
+    ph->channel.data = (u_int32_t)pAd->CommonCfg.Channel;
+
+    ph->rssi.did = DIDmsg_lnxind_wlansniffrm_rssi;
+	ph->rssi.status = 0;
+	ph->rssi.len = 4;
+    ph->rssi.data = (u_int32_t)RTMPMaxRssi(pAd, ConvertToRssi(pAd, pRxBlk->pRxWI->RSSI0, RSSI_0), ConvertToRssi(pAd, pRxBlk->pRxWI->RSSI1, RSSI_1), ConvertToRssi(pAd, pRxBlk->pRxWI->RSSI2, RSSI_2));;
+
+	ph->signal.did = DIDmsg_lnxind_wlansniffrm_signal;
+	ph->signal.status = 0;
+	ph->signal.len = 4;
+	ph->signal.data = 0; //rssi + noise;
+
+	ph->noise.did = DIDmsg_lnxind_wlansniffrm_noise;
+	ph->noise.status = 0;
+	ph->noise.len = 4;
+	ph->noise.data = 0;
+
+#ifdef DOT11_N_SUPPORT
+    if (pRxBlk->pRxWI->PHYMODE >= MODE_HTMIX)
+    {
+    	rate_index = 16 + ((UCHAR)pRxBlk->pRxWI->BW *16) + ((UCHAR)pRxBlk->pRxWI->ShortGI *32) + ((UCHAR)pRxBlk->pRxWI->MCS);
+    }
+    else
+#endif // DOT11_N_SUPPORT //
+	if (pRxBlk->pRxWI->PHYMODE == MODE_OFDM)
+    	rate_index = (UCHAR)(pRxBlk->pRxWI->MCS) + 4;
+    else
+    	rate_index = (UCHAR)(pRxBlk->pRxWI->MCS);
+    if (rate_index < 0)
+        rate_index = 0;
+    if (rate_index > 255)
+        rate_index = 255;
+
+	ph->rate.did = DIDmsg_lnxind_wlansniffrm_rate;
+	ph->rate.status = 0;
+	ph->rate.len = 4;
+    ph->rate.data = ralinkrate[rate_index];
+
+	ph->frmlen.did = DIDmsg_lnxind_wlansniffrm_frmlen;
+    ph->frmlen.status = 0;
+	ph->frmlen.len = 4;
+	ph->frmlen.data	= (u_int32_t)pRxBlk->DataSize;
+
+
+    pOSPkt->pkt_type = PACKET_OTHERHOST;
+    pOSPkt->protocol = eth_type_trans(pOSPkt, pOSPkt->dev);
+    pOSPkt->ip_summed = CHECKSUM_NONE;
+    netif_rx(pOSPkt);
+
+    return;
+
+err_free_sk_buff:
+	RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+	return;
+
+}
+#endif // CONFIG_STA_SUPPORT //
+
+
+void rtmp_os_thread_init(PUCHAR pThreadName, PVOID pNotify)
+{
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+	daemonize(pThreadName /*"%s",pAd->net_dev->name*/);
+
+	allow_signal(SIGTERM);
+	allow_signal(SIGKILL);
+	current->flags |= PF_NOFREEZE;
+#else
+	unsigned long flags;
+
+	daemonize();
+	reparent_to_init();
+	strcpy(current->comm, pThreadName);
+
+	siginitsetinv(&current->blocked, sigmask(SIGTERM) | sigmask(SIGKILL));
+
+	/* Allow interception of SIGKILL only
+	 * Don't allow other signals to interrupt the transmission */
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,4,22)
+	spin_lock_irqsave(&current->sigmask_lock, flags);
+	flush_signals(current);
+	recalc_sigpending(current);
+	spin_unlock_irqrestore(&current->sigmask_lock, flags);
+#endif
+#endif
+
+    /* signal that we've started the thread */
+	complete(pNotify);
+
+}
+
+void RTMP_IndicateMediaState(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	if (pAd->CommonCfg.bWirelessEvent)
+	{
+		if (pAd->IndicateMediaState == NdisMediaStateConnected)
+		{
+			RTMPSendWirelessEvent(pAd, IW_STA_LINKUP_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+		}
+		else
+		{
+			RTMPSendWirelessEvent(pAd, IW_STA_LINKDOWN_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+		}
+	}
+}
+
diff --git a/drivers/staging/rt2860/rt_linux.h b/drivers/staging/rt2860/rt_linux.h
new file mode 100644
index 00000000000..0cc7cf23d76
--- /dev/null
+++ b/drivers/staging/rt2860/rt_linux.h
@@ -0,0 +1,926 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+/***********************************************************************/
+/*                                                                     */
+/*   Program:    rt_linux.c                                            */
+/*   Created:    4/21/2006 1:17:38 PM                                  */
+/*   Author:     Wu Xi-Kun                                             */
+/*   Comments:   `description`                                         */
+/*                                                                     */
+/*---------------------------------------------------------------------*/
+/*                                                                     */
+/* History:                                                            */
+/*    Revision 1.1 4/21/2006 1:17:38 PM  xsikun                        */
+/*    Initial revision                                                 */
+/*                                                                     */
+/***********************************************************************/
+
+#include "rtmp_type.h"
+#include <linux/module.h>
+#include <linux/version.h>
+#include <linux/kernel.h>
+
+#include <linux/spinlock.h>
+#include <linux/init.h>
+#include <linux/string.h>
+#include <linux/timer.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/pci.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/skbuff.h>
+#include <linux/ethtool.h>
+#include <linux/wireless.h>
+#include <linux/proc_fs.h>
+#include <linux/delay.h>
+#include <linux/if_arp.h>
+#include <linux/ctype.h>
+#include <linux/vmalloc.h>
+
+
+#include <linux/wireless.h>
+#include <net/iw_handler.h>
+
+// load firmware
+#define __KERNEL_SYSCALLS__
+#include <linux/unistd.h>
+#include <asm/uaccess.h>
+
+
+#define MEM_ALLOC_FLAG      (GFP_ATOMIC) //(GFP_DMA | GFP_ATOMIC)
+
+#ifndef IFNAMSIZ
+#define IFNAMSIZ 16
+#endif
+
+//#define CONFIG_CKIP_SUPPORT
+
+#undef __inline
+#define __inline	   static inline
+
+typedef int (*HARD_START_XMIT_FUNC)(struct sk_buff *skb, struct net_device *net_dev);
+
+// add by kathy
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef RT2860
+#define STA_PROFILE_PATH			"/etc/Wireless/RT2860STA/RT2860STA.dat"
+#define STA_RTMP_FIRMWARE_FILE_NAME "/etc/Wireless/RT2860STA/RT2860STA.bin"
+#define STA_NIC_DEVICE_NAME			"RT2860STA"
+#define STA_DRIVER_VERSION			"1.8.0.0"
+#ifdef MULTIPLE_CARD_SUPPORT
+#define CARD_INFO_PATH			"/etc/Wireless/RT2860STA/RT2860STACard.dat"
+#endif // MULTIPLE_CARD_SUPPORT //
+#endif // RT2860 //
+
+
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef RT2860
+#ifndef PCI_DEVICE
+#define PCI_DEVICE(vend,dev) \
+	.vendor = (vend), .device = (dev), \
+	.subvendor = PCI_ANY_ID, .subdevice = PCI_ANY_ID
+#endif // PCI_DEVICE //
+#endif // RT2860 //
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+
+#define RTMP_TIME_AFTER(a,b)		\
+	(typecheck(unsigned long, (unsigned long)a) && \
+	 typecheck(unsigned long, (unsigned long)b) && \
+	 ((long)(b) - (long)(a) < 0))
+
+#define RTMP_TIME_AFTER_EQ(a,b)	\
+	(typecheck(unsigned long, (unsigned long)a) && \
+	 typecheck(unsigned long, (unsigned long)b) && \
+	 ((long)(a) - (long)(b) >= 0))
+#define RTMP_TIME_BEFORE(a,b)	RTMP_TIME_AFTER_EQ(b,a)
+#else
+#define RTMP_TIME_AFTER(a,b) time_after(a, b)
+#endif
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+#define RT_MOD_INC_USE_COUNT() \
+	if (!try_module_get(THIS_MODULE)) \
+	{ \
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: cannot reserve module\n", __FUNCTION__)); \
+		return -1; \
+	}
+
+#define RT_MOD_DEC_USE_COUNT() module_put(THIS_MODULE);
+#else
+#define RT_MOD_INC_USE_COUNT()	MOD_INC_USE_COUNT;
+#define RT_MOD_DEC_USE_COUNT() MOD_DEC_USE_COUNT;
+#endif
+
+#define OS_HZ			HZ
+
+#define ETH_LENGTH_OF_ADDRESS	6
+
+#define IN
+#define OUT
+
+#define NDIS_STATUS                             INT
+#define NDIS_STATUS_SUCCESS                     0x00
+#define NDIS_STATUS_FAILURE                     0x01
+#define NDIS_STATUS_INVALID_DATA				0x02
+#define NDIS_STATUS_RESOURCES                   0x03
+
+#define MIN_NET_DEVICE_FOR_AID			0x00		//0x00~0x3f
+#define MIN_NET_DEVICE_FOR_MBSSID		0x00		//0x00,0x10,0x20,0x30
+#define MIN_NET_DEVICE_FOR_WDS			0x10		//0x40,0x50,0x60,0x70
+#define MIN_NET_DEVICE_FOR_APCLI		0x20
+#define MIN_NET_DEVICE_FOR_MESH			0x30
+#ifdef CONFIG_STA_SUPPORT
+#define MIN_NET_DEVICE_FOR_DLS			0x40
+#endif // CONFIG_STA_SUPPORT //
+
+
+#ifdef CONFIG_STA_SUPPORT
+#define NDIS_PACKET_TYPE_DIRECTED		0
+#define NDIS_PACKET_TYPE_MULTICAST		1
+#define NDIS_PACKET_TYPE_BROADCAST		2
+#define NDIS_PACKET_TYPE_ALL_MULTICAST	3
+#endif // CONFIG_STA_SUPPORT //
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
+typedef	struct pid *	THREAD_PID;
+#define	THREAD_PID_INIT_VALUE	NULL
+#define	GET_PID(_v)	find_get_pid(_v)
+#define	GET_PID_NUMBER(_v)	pid_nr(_v)
+#define CHECK_PID_LEGALITY(_pid)	if (pid_nr(_pid) >= 0)
+#define KILL_THREAD_PID(_A, _B, _C)	kill_pid(_A, _B, _C)
+#else
+typedef	pid_t	THREAD_PID;
+#define	THREAD_PID_INIT_VALUE	-1
+#define	GET_PID(_v)	_v
+#define	GET_PID_NUMBER(_v)	_v
+#define CHECK_PID_LEGALITY(_pid)	if (_pid >= 0)
+#define KILL_THREAD_PID(_A, _B, _C)	kill_proc(_A, _B, _C)
+#endif
+
+struct os_lock  {
+	spinlock_t		lock;
+	unsigned long  	flags;
+};
+
+
+struct os_cookie {
+#ifdef RT2860
+	struct pci_dev 			*pci_dev;
+	struct pci_dev 			*parent_pci_dev;
+	dma_addr_t		  		pAd_pa;
+#endif // RT2860 //
+
+
+	struct tasklet_struct 	rx_done_task;
+	struct tasklet_struct 	mgmt_dma_done_task;
+	struct tasklet_struct 	ac0_dma_done_task;
+	struct tasklet_struct 	ac1_dma_done_task;
+	struct tasklet_struct 	ac2_dma_done_task;
+	struct tasklet_struct 	ac3_dma_done_task;
+	struct tasklet_struct 	hcca_dma_done_task;
+	struct tasklet_struct	tbtt_task;
+#ifdef RT2860
+	struct tasklet_struct	fifo_statistic_full_task;
+#endif // RT2860 //
+
+
+	unsigned long			apd_pid; //802.1x daemon pid
+	INT						ioctl_if_type;
+	INT 					ioctl_if;
+};
+
+typedef struct _VIRTUAL_ADAPTER
+{
+	struct net_device		*RtmpDev;
+	struct net_device		*VirtualDev;
+} VIRTUAL_ADAPTER, PVIRTUAL_ADAPTER;
+
+#undef  ASSERT
+#define ASSERT(x)                                                               \
+{                                                                               \
+    if (!(x))                                                                   \
+    {                                                                           \
+        printk(KERN_WARNING __FILE__ ":%d assert " #x "failed\n", __LINE__);    \
+    }                                                                           \
+}
+
+typedef struct os_cookie	* POS_COOKIE;
+typedef struct pci_dev 		* PPCI_DEV;
+typedef struct net_device	* PNET_DEV;
+typedef void				* PNDIS_PACKET;
+typedef char				NDIS_PACKET;
+typedef PNDIS_PACKET		* PPNDIS_PACKET;
+typedef	dma_addr_t			NDIS_PHYSICAL_ADDRESS;
+typedef	dma_addr_t			* PNDIS_PHYSICAL_ADDRESS;
+typedef spinlock_t			NDIS_SPIN_LOCK;
+typedef struct timer_list	NDIS_MINIPORT_TIMER;
+typedef void				* NDIS_HANDLE;
+typedef char 				* PNDIS_BUFFER;
+
+
+
+void hex_dump(char *str, unsigned char *pSrcBufVA, unsigned int SrcBufLen);
+
+dma_addr_t linux_pci_map_single(void *handle, void *ptr, size_t size, int sd_idx, int direction);
+void linux_pci_unmap_single(void *handle, dma_addr_t dma_addr, size_t size, int direction);
+
+
+////////////////////////////////////////
+// MOVE TO rtmp.h ?
+/////////////////////////////////////////
+#define PKTSRC_NDIS             0x7f
+#define PKTSRC_DRIVER           0x0f
+#define PRINT_MAC(addr)	\
+	addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]
+
+
+#define RT2860_PCI_DEVICE_ID		0x0601
+
+#ifdef RT2860
+#define PCI_MAP_SINGLE(_handle, _ptr, _size, _sd_idx, _dir) \
+	linux_pci_map_single(_handle, _ptr, _size, _sd_idx, _dir)
+
+#define PCI_UNMAP_SINGLE(_handle, _ptr, _size, _dir) \
+	linux_pci_unmap_single(_handle, _ptr, _size, _dir)
+
+#define PCI_ALLOC_CONSISTENT(_pci_dev, _size, _ptr) \
+	pci_alloc_consistent(_pci_dev, _size, _ptr)
+
+#define PCI_FREE_CONSISTENT(_pci_dev, _size, _virtual_addr, _physical_addr) \
+	pci_free_consistent(_pci_dev, _size, _virtual_addr, _physical_addr)
+
+#define DEV_ALLOC_SKB(_length) \
+	dev_alloc_skb(_length)
+#endif // RT2860 //
+
+
+
+#define BEACON_FRAME_DMA_CACHE_WBACK(_ptr, _size)	\
+	dma_cache_wback(_ptr, _size)
+
+
+//////////////////////////////////////////
+//
+//////////////////////////////////////////
+
+
+#define NdisMIndicateStatus(_w, _x, _y, _z)
+
+
+typedef struct timer_list	RTMP_OS_TIMER;
+
+
+
+typedef struct  _RALINK_TIMER_STRUCT    {
+    RTMP_OS_TIMER		TimerObj;       // Ndis Timer object
+	BOOLEAN				Valid;			// Set to True when call RTMPInitTimer
+    BOOLEAN             State;          // True if timer cancelled
+    BOOLEAN	      		PeriodicType;	// True if timer is periodic timer
+    BOOLEAN             Repeat;         // True if periodic timer
+    ULONG               TimerValue;     // Timer value in milliseconds
+	ULONG				cookie;			// os specific object
+}   RALINK_TIMER_STRUCT, *PRALINK_TIMER_STRUCT;
+
+
+
+
+//#define DBG	1
+
+//
+//  MACRO for debugging information
+//
+
+#ifdef DBG
+extern ULONG    RTDebugLevel;
+
+#define DBGPRINT_RAW(Level, Fmt)    \
+{                                   \
+    if (Level <= RTDebugLevel)      \
+    {                               \
+        printk Fmt;               \
+    }                               \
+}
+
+#define DBGPRINT(Level, Fmt)    DBGPRINT_RAW(Level, Fmt)
+
+
+#define DBGPRINT_ERR(Fmt)           \
+{                                   \
+    printk("ERROR!!! ");          \
+    printk Fmt;                  \
+}
+
+#define DBGPRINT_S(Status, Fmt)		\
+{									\
+	printk Fmt;					\
+}
+
+
+#else
+#define DBGPRINT(Level, Fmt)
+#define DBGPRINT_RAW(Level, Fmt)
+#define DBGPRINT_S(Status, Fmt)
+#define DBGPRINT_ERR(Fmt)
+#endif
+
+
+//
+//  spin_lock enhanced for Nested spin lock
+//
+#define NdisAllocateSpinLock(__lock)      \
+{                                       \
+    spin_lock_init((spinlock_t *)(__lock));               \
+}
+
+#define NdisFreeSpinLock(lock)          \
+{                                       \
+}
+
+
+#define RTMP_SEM_LOCK(__lock)					\
+{												\
+	spin_lock_bh((spinlock_t *)(__lock));				\
+}
+
+#define RTMP_SEM_UNLOCK(__lock)					\
+{												\
+	spin_unlock_bh((spinlock_t *)(__lock));				\
+}
+
+// sample, use semaphore lock to replace IRQ lock, 2007/11/15
+#define RTMP_IRQ_LOCK(__lock, __irqflags)			\
+{													\
+	__irqflags = 0;									\
+	spin_lock_bh((spinlock_t *)(__lock));			\
+	pAd->irq_disabled |= 1; \
+}
+
+#define RTMP_IRQ_UNLOCK(__lock, __irqflag)			\
+{													\
+	pAd->irq_disabled &= 0; \
+	spin_unlock_bh((spinlock_t *)(__lock));			\
+}
+
+#define RTMP_INT_LOCK(__lock, __irqflags)			\
+{													\
+	spin_lock_irqsave((spinlock_t *)__lock, __irqflags);	\
+}
+
+#define RTMP_INT_UNLOCK(__lock, __irqflag)			\
+{													\
+	spin_unlock_irqrestore((spinlock_t *)(__lock), ((unsigned long)__irqflag));	\
+}
+
+#ifdef RT2860
+#if defined(INF_TWINPASS) || defined(INF_DANUBE) || defined(IKANOS_VX_1X0)
+//Patch for ASIC turst read/write bug, needs to remove after metel fix
+#define RTMP_IO_READ32(_A, _R, _pV)									\
+{																	\
+    if ((_A)->bPCIclkOff == FALSE)                                      \
+    {                                                                   \
+	(*_pV = readl((void *)((_A)->CSRBaseAddress + MAC_CSR0)));		\
+	(*_pV = readl((void *)((_A)->CSRBaseAddress + (_R))));			\
+	(*_pV = SWAP32(*((UINT32 *)(_pV))));                           \
+    }                                                                   \
+}
+#define RTMP_IO_READ8(_A, _R, _pV)									\
+{																	\
+	(*_pV = readl((void *)((_A)->CSRBaseAddress + MAC_CSR0)));		\
+	(*_pV = readb((void *)((_A)->CSRBaseAddress + (_R))));			\
+}
+#define RTMP_IO_WRITE32(_A, _R, _V)									\
+{																	\
+    if ((_A)->bPCIclkOff == FALSE)                                      \
+    {                                                                   \
+	UINT32	_Val;													\
+	_Val = readl((void *)((_A)->CSRBaseAddress + MAC_CSR0));		\
+	_Val = SWAP32(_V);												\
+	writel(_Val, (void *)((_A)->CSRBaseAddress + (_R)));			\
+    }                                                                   \
+}
+#define RTMP_IO_WRITE8(_A, _R, _V)									\
+{																	\
+	UINT	Val;													\
+	Val = readl((void *)((_A)->CSRBaseAddress + MAC_CSR0));		\
+	writeb((_V), (PUCHAR)((_A)->CSRBaseAddress + (_R)));			\
+}
+#define RTMP_IO_WRITE16(_A, _R, _V)									\
+{																	\
+	UINT	Val;													\
+	Val = readl((void *)((_A)->CSRBaseAddress + MAC_CSR0));		\
+	writew(SWAP16((_V)), (PUSHORT)((_A)->CSRBaseAddress + (_R)));	\
+}
+#else
+//Patch for ASIC turst read/write bug, needs to remove after metel fix
+#define RTMP_IO_READ32(_A, _R, _pV)								\
+{																\
+    if ((_A)->bPCIclkOff == FALSE)                                  \
+    {                                                               \
+		(*_pV = readl((void *)((_A)->CSRBaseAddress + MAC_CSR0)));		\
+		(*_pV = readl((void *)((_A)->CSRBaseAddress + (_R))));			\
+    }                                                               \
+    else															\
+		*_pV = 0;													\
+}
+#define RTMP_IO_READ8(_A, _R, _pV)								\
+{																\
+	(*_pV = readl((void *)((_A)->CSRBaseAddress + MAC_CSR0)));			\
+	(*_pV = readb((void *)((_A)->CSRBaseAddress + (_R))));				\
+}
+#define RTMP_IO_WRITE32(_A, _R, _V)												\
+{																				\
+    if ((_A)->bPCIclkOff == FALSE)                                  \
+    {                                                               \
+	UINT	Val;																\
+	Val = readl((void *)((_A)->CSRBaseAddress + MAC_CSR0));			\
+	writel(_V, (void *)((_A)->CSRBaseAddress + (_R)));								\
+    }                                                               \
+}
+#if defined(BRCM_6358)
+#define RTMP_IO_WRITE8(_A, _R, _V)            \
+{                    \
+	ULONG Val;                \
+	UCHAR _i;                \
+	_i = (_R & 0x3);             \
+	Val = readl((void *)((_A)->CSRBaseAddress + (_R - _i)));   \
+	Val = Val & (~(0x000000ff << ((_i)*8)));         \
+	Val = Val | ((ULONG)_V << ((_i)*8));         \
+	writel((Val), (void *)((_A)->CSRBaseAddress + (_R - _i)));    \
+}
+#else
+#define RTMP_IO_WRITE8(_A, _R, _V)												\
+{																				\
+	UINT	Val;																\
+	Val = readl((void *)((_A)->CSRBaseAddress + MAC_CSR0));			\
+	writeb((_V), (PUCHAR)((_A)->CSRBaseAddress + (_R)));		\
+}
+#endif
+#define RTMP_IO_WRITE16(_A, _R, _V)												\
+{																				\
+	UINT	Val;																\
+	Val = readl((void *)((_A)->CSRBaseAddress + MAC_CSR0));			\
+	writew((_V), (PUSHORT)((_A)->CSRBaseAddress + (_R)));	\
+}
+#endif
+#endif // RT2860 //
+
+
+#ifndef wait_event_interruptible_timeout
+#define __wait_event_interruptible_timeout(wq, condition, ret) \
+do { \
+        wait_queue_t __wait; \
+        init_waitqueue_entry(&__wait, current); \
+        add_wait_queue(&wq, &__wait); \
+        for (;;) { \
+                set_current_state(TASK_INTERRUPTIBLE); \
+                if (condition) \
+                        break; \
+                if (!signal_pending(current)) { \
+                        ret = schedule_timeout(ret); \
+                        if (!ret) \
+                                break; \
+                        continue; \
+                } \
+                ret = -ERESTARTSYS; \
+                break; \
+        } \
+        current->state = TASK_RUNNING; \
+        remove_wait_queue(&wq, &__wait); \
+} while (0)
+
+#define wait_event_interruptible_timeout(wq, condition, timeout) \
+({ \
+        long __ret = timeout; \
+        if (!(condition)) \
+                __wait_event_interruptible_timeout(wq, condition, __ret); \
+        __ret; \
+})
+#endif
+#define ONE_TICK 1
+#define OS_WAIT(_time) \
+{	int _i; \
+	long _loop = ((_time)/(1000/OS_HZ)) > 0 ? ((_time)/(1000/OS_HZ)) : 1;\
+	wait_queue_head_t _wait; \
+	init_waitqueue_head(&_wait); \
+	for (_i=0; _i<(_loop); _i++) \
+		wait_event_interruptible_timeout(_wait, 0, ONE_TICK); }
+
+
+/* Modified by Wu Xi-Kun 4/21/2006 */
+typedef void (*TIMER_FUNCTION)(unsigned long);
+
+#define COPY_MAC_ADDR(Addr1, Addr2)             memcpy((Addr1), (Addr2), MAC_ADDR_LEN)
+
+#define MlmeAllocateMemory(_pAd, _ppVA) os_alloc_mem(_pAd, _ppVA, MGMT_DMA_BUFFER_SIZE)
+#define MlmeFreeMemory(_pAd, _pVA)     os_free_mem(_pAd, _pVA)
+
+#ifdef RT2860
+#define BUILD_TIMER_FUNCTION(_func)												\
+void linux_##_func(unsigned long data)											\
+{																				\
+	PRALINK_TIMER_STRUCT	pTimer = (PRALINK_TIMER_STRUCT) data;				\
+																				\
+	_func(NULL, (PVOID) pTimer->cookie, NULL, pTimer); 							\
+	if (pTimer->Repeat)															\
+		RTMP_OS_Add_Timer(&pTimer->TimerObj, pTimer->TimerValue);				\
+}
+#endif // RT2860 //
+
+
+
+#define DECLARE_TIMER_FUNCTION(_func)			\
+void linux_##_func(unsigned long data)
+
+#define GET_TIMER_FUNCTION(_func)				\
+		linux_##_func
+
+DECLARE_TIMER_FUNCTION(MlmePeriodicExec);
+DECLARE_TIMER_FUNCTION(MlmeRssiReportExec);
+DECLARE_TIMER_FUNCTION(AsicRxAntEvalTimeout);
+DECLARE_TIMER_FUNCTION(APSDPeriodicExec);
+DECLARE_TIMER_FUNCTION(AsicRfTuningExec);
+
+
+#ifdef CONFIG_STA_SUPPORT
+DECLARE_TIMER_FUNCTION(BeaconTimeout);
+DECLARE_TIMER_FUNCTION(ScanTimeout);
+DECLARE_TIMER_FUNCTION(AuthTimeout);
+DECLARE_TIMER_FUNCTION(AssocTimeout);
+DECLARE_TIMER_FUNCTION(ReassocTimeout);
+DECLARE_TIMER_FUNCTION(DisassocTimeout);
+DECLARE_TIMER_FUNCTION(LinkDownExec);
+#ifdef LEAP_SUPPORT
+DECLARE_TIMER_FUNCTION(LeapAuthTimeout);
+#endif
+DECLARE_TIMER_FUNCTION(StaQuickResponeForRateUpExec);
+DECLARE_TIMER_FUNCTION(WpaDisassocApAndBlockAssoc);
+DECLARE_TIMER_FUNCTION(PsPollWakeExec);
+DECLARE_TIMER_FUNCTION(RadioOnExec);
+
+#ifdef QOS_DLS_SUPPORT
+DECLARE_TIMER_FUNCTION(DlsTimeoutAction);
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+void RTMP_GetCurrentSystemTime(LARGE_INTEGER *time);
+
+
+/*
+ * packet helper
+ * 	- convert internal rt packet to os packet or
+ *             os packet to rt packet
+ */
+#define RTPKT_TO_OSPKT(_p)		((struct sk_buff *)(_p))
+#define OSPKT_TO_RTPKT(_p)		((PNDIS_PACKET)(_p))
+
+#define GET_OS_PKT_DATAPTR(_pkt) \
+		(RTPKT_TO_OSPKT(_pkt)->data)
+
+#define GET_OS_PKT_LEN(_pkt) \
+		(RTPKT_TO_OSPKT(_pkt)->len)
+
+#define GET_OS_PKT_DATATAIL(_pkt) \
+		(RTPKT_TO_OSPKT(_pkt)->tail)
+
+#define GET_OS_PKT_HEAD(_pkt) \
+		(RTPKT_TO_OSPKT(_pkt)->head)
+
+#define GET_OS_PKT_END(_pkt) \
+		(RTPKT_TO_OSPKT(_pkt)->end)
+
+#define GET_OS_PKT_NETDEV(_pkt) \
+		(RTPKT_TO_OSPKT(_pkt)->dev)
+
+#define GET_OS_PKT_TYPE(_pkt) \
+		(RTPKT_TO_OSPKT(_pkt))
+
+#define GET_OS_PKT_NEXT(_pkt) \
+		(RTPKT_TO_OSPKT(_pkt)->next)
+
+
+#define OS_NTOHS(_Val) \
+		(ntohs(_Val))
+#define OS_HTONS(_Val) \
+		(htons(_Val))
+#define OS_NTOHL(_Val) \
+		(ntohl(_Val))
+#define OS_HTONL(_Val) \
+		(htonl(_Val))
+
+/* statistics counter */
+#define STATS_INC_RX_PACKETS(_pAd, _dev)
+#define STATS_INC_TX_PACKETS(_pAd, _dev)
+
+#define STATS_INC_RX_BYTESS(_pAd, _dev, len)
+#define STATS_INC_TX_BYTESS(_pAd, _dev, len)
+
+#define STATS_INC_RX_ERRORS(_pAd, _dev)
+#define STATS_INC_TX_ERRORS(_pAd, _dev)
+
+#define STATS_INC_RX_DROPPED(_pAd, _dev)
+#define STATS_INC_TX_DROPPED(_pAd, _dev)
+
+
+#define CB_OFF  10
+
+
+//   check DDK NDIS_PACKET data structure and find out only MiniportReservedEx[0..7] can be used by our driver without
+//   ambiguity. Fields after pPacket->MiniportReservedEx[8] may be used by other wrapper layer thus crashes the driver
+//
+
+// User Priority
+#define RTMP_SET_PACKET_UP(_p, _prio)			(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+0] = _prio)
+#define RTMP_GET_PACKET_UP(_p)					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+0])
+
+// Fragment #
+#define RTMP_SET_PACKET_FRAGMENTS(_p, _num)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+1] = _num)
+#define RTMP_GET_PACKET_FRAGMENTS(_p)			(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+1])
+
+// 0x0 ~0x7f: TX to AP's own BSS which has the specified AID. if AID>127, set bit 7 in RTMP_SET_PACKET_EMACTAB too.
+//(this value also as MAC(on-chip WCID) table index)
+// 0x80~0xff: TX to a WDS link. b0~6: WDS index
+#define RTMP_SET_PACKET_WCID(_p, _wdsidx)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+2] = _wdsidx)
+#define RTMP_GET_PACKET_WCID(_p)          		((UCHAR)(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+2]))
+
+// 0xff: PKTSRC_NDIS, others: local TX buffer index. This value affects how to a packet
+#define RTMP_SET_PACKET_SOURCE(_p, _pktsrc)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+3] = _pktsrc)
+#define RTMP_GET_PACKET_SOURCE(_p)       		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+3])
+
+// RTS/CTS-to-self protection method
+#define RTMP_SET_PACKET_RTS(_p, _num)      		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+4] = _num)
+#define RTMP_GET_PACKET_RTS(_p)          		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+4])
+// see RTMP_S(G)ET_PACKET_EMACTAB
+
+// TX rate index
+#define RTMP_SET_PACKET_TXRATE(_p, _rate)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+5] = _rate)
+#define RTMP_GET_PACKET_TXRATE(_p)		  		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+5])
+
+// From which Interface
+#define RTMP_SET_PACKET_IF(_p, _ifdx)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+6] = _ifdx)
+#define RTMP_GET_PACKET_IF(_p)		  		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+6])
+#define RTMP_SET_PACKET_NET_DEVICE_MBSSID(_p, _bss)		RTMP_SET_PACKET_IF((_p), (_bss))
+#define RTMP_SET_PACKET_NET_DEVICE_WDS(_p, _bss)		RTMP_SET_PACKET_IF((_p), ((_bss) + MIN_NET_DEVICE_FOR_WDS))
+#define RTMP_SET_PACKET_NET_DEVICE_APCLI(_p, _idx)   	RTMP_SET_PACKET_IF((_p), ((_idx) + MIN_NET_DEVICE_FOR_APCLI))
+#define RTMP_SET_PACKET_NET_DEVICE_MESH(_p, _idx)   	RTMP_SET_PACKET_IF((_p), ((_idx) + MIN_NET_DEVICE_FOR_MESH))
+#define RTMP_GET_PACKET_NET_DEVICE_MBSSID(_p)			RTMP_GET_PACKET_IF((_p))
+#define RTMP_GET_PACKET_NET_DEVICE(_p)					RTMP_GET_PACKET_IF((_p))
+
+#define RTMP_SET_PACKET_MOREDATA(_p, _morebit)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+7] = _morebit)
+#define RTMP_GET_PACKET_MOREDATA(_p)				(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+7])
+
+
+#if 0
+//#define RTMP_SET_PACKET_DHCP(_p, _flg)   	(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11] = _flg)
+//#define RTMP_GET_PACKET_DHCP(_p)         	(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11])
+#else
+//
+//	Sepcific Pakcet Type definition
+//
+#define RTMP_PACKET_SPECIFIC_CB_OFFSET	11
+
+#define RTMP_PACKET_SPECIFIC_DHCP		0x01
+#define RTMP_PACKET_SPECIFIC_EAPOL		0x02
+#define RTMP_PACKET_SPECIFIC_IPV4		0x04
+#define RTMP_PACKET_SPECIFIC_WAI		0x08
+#define RTMP_PACKET_SPECIFIC_VLAN		0x10
+#define RTMP_PACKET_SPECIFIC_LLCSNAP	0x20
+
+//Specific
+#define RTMP_SET_PACKET_SPECIFIC(_p, _flg)	   	(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11] = _flg)
+
+//DHCP
+#define RTMP_SET_PACKET_DHCP(_p, _flg)   													\
+			do{																				\
+				if (_flg)																	\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) |= (RTMP_PACKET_SPECIFIC_DHCP);		\
+				else																		\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) &= (!RTMP_PACKET_SPECIFIC_DHCP);	\
+			}while(0)
+#define RTMP_GET_PACKET_DHCP(_p)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11] & RTMP_PACKET_SPECIFIC_DHCP)
+
+//EAPOL
+#define RTMP_SET_PACKET_EAPOL(_p, _flg)   													\
+			do{																				\
+				if (_flg)																	\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) |= (RTMP_PACKET_SPECIFIC_EAPOL);		\
+				else																		\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) &= (!RTMP_PACKET_SPECIFIC_EAPOL);	\
+			}while(0)
+#define RTMP_GET_PACKET_EAPOL(_p)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11] & RTMP_PACKET_SPECIFIC_EAPOL)
+
+//WAI
+#define RTMP_SET_PACKET_WAI(_p, _flg)   													\
+			do{																				\
+				if (_flg)																	\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) |= (RTMP_PACKET_SPECIFIC_WAI);		\
+				else																		\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) &= (!RTMP_PACKET_SPECIFIC_WAI);	\
+			}while(0)
+#define RTMP_GET_PACKET_WAI(_p)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11] & RTMP_PACKET_SPECIFIC_WAI)
+
+#define RTMP_GET_PACKET_LOWRATE(_p)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11] & (RTMP_PACKET_SPECIFIC_EAPOL | RTMP_PACKET_SPECIFIC_DHCP | RTMP_PACKET_SPECIFIC_WAI))
+
+//VLAN
+#define RTMP_SET_PACKET_VLAN(_p, _flg)   													\
+			do{																				\
+				if (_flg)																	\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) |= (RTMP_PACKET_SPECIFIC_VLAN);		\
+				else																		\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) &= (!RTMP_PACKET_SPECIFIC_VLAN);	\
+			}while(0)
+#define RTMP_GET_PACKET_VLAN(_p)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11] & RTMP_PACKET_SPECIFIC_VLAN)
+
+//LLC/SNAP
+#define RTMP_SET_PACKET_LLCSNAP(_p, _flg)   													\
+			do{																				\
+				if (_flg)																	\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) |= (RTMP_PACKET_SPECIFIC_LLCSNAP);		\
+				else																		\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) &= (!RTMP_PACKET_SPECIFIC_LLCSNAP);		\
+			}while(0)
+
+#define RTMP_GET_PACKET_LLCSNAP(_p)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11] & RTMP_PACKET_SPECIFIC_LLCSNAP)
+
+// IP
+#define RTMP_SET_PACKET_IPV4(_p, _flg)														\
+			do{																				\
+				if (_flg)																	\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) |= (RTMP_PACKET_SPECIFIC_IPV4);		\
+				else																		\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) &= (!RTMP_PACKET_SPECIFIC_IPV4);	\
+			}while(0)
+
+#define RTMP_GET_PACKET_IPV4(_p)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11] & RTMP_PACKET_SPECIFIC_IPV4)
+
+#endif
+
+
+// If this flag is set, it indicates that this EAPoL frame MUST be clear.
+#define RTMP_SET_PACKET_CLEAR_EAP_FRAME(_p, _flg)   (RTPKT_TO_OSPKT(_p)->cb[CB_OFF+12] = _flg)
+#define RTMP_GET_PACKET_CLEAR_EAP_FRAME(_p)         (RTPKT_TO_OSPKT(_p)->cb[CB_OFF+12])
+
+#define RTMP_SET_PACKET_5VT(_p, _flg)   (RTPKT_TO_OSPKT(_p)->cb[CB_OFF+22] = _flg)
+#define RTMP_GET_PACKET_5VT(_p)         (RTPKT_TO_OSPKT(_p)->cb[CB_OFF+22])
+
+#ifdef CONFIG_5VT_ENHANCE
+#define BRIDGE_TAG 0x35564252    // depends on 5VT define in br_input.c
+#endif
+
+
+#define NDIS_SET_PACKET_STATUS(_p, _status)
+
+
+#define GET_SG_LIST_FROM_PACKET(_p, _sc)	\
+    rt_get_sg_list_from_packet(_p, _sc)
+
+#define NdisMoveMemory(Destination, Source, Length) memmove(Destination, Source, Length)
+#define NdisZeroMemory(Destination, Length)         memset(Destination, 0, Length)
+#define NdisFillMemory(Destination, Length, Fill)   memset(Destination, Fill, Length)
+#define NdisEqualMemory(Source1, Source2, Length)   (!memcmp(Source1, Source2, Length))
+#define RTMPEqualMemory(Source1, Source2, Length)	(!memcmp(Source1, Source2, Length))
+
+
+#define RTMP_INC_REF(_A)		0
+#define RTMP_DEC_REF(_A)		0
+#define RTMP_GET_REF(_A)		0
+
+
+
+/*
+ * ULONG
+ * RTMP_GetPhysicalAddressLow(
+ *   IN NDIS_PHYSICAL_ADDRESS  PhysicalAddress);
+ */
+#define RTMP_GetPhysicalAddressLow(PhysicalAddress)		(PhysicalAddress)
+
+/*
+ * ULONG
+ * RTMP_GetPhysicalAddressHigh(
+ *   IN NDIS_PHYSICAL_ADDRESS  PhysicalAddress);
+ */
+#define RTMP_GetPhysicalAddressHigh(PhysicalAddress)		(0)
+
+/*
+ * VOID
+ * RTMP_SetPhysicalAddressLow(
+ *   IN NDIS_PHYSICAL_ADDRESS  PhysicalAddress,
+ *   IN ULONG  Value);
+ */
+#define RTMP_SetPhysicalAddressLow(PhysicalAddress, Value)	\
+			PhysicalAddress = Value;
+
+/*
+ * VOID
+ * RTMP_SetPhysicalAddressHigh(
+ *   IN NDIS_PHYSICAL_ADDRESS  PhysicalAddress,
+ *   IN ULONG  Value);
+ */
+#define RTMP_SetPhysicalAddressHigh(PhysicalAddress, Value)
+
+
+//CONTAINING_RECORD(pEntry, NDIS_PACKET, MiniportReservedEx);
+#define QUEUE_ENTRY_TO_PACKET(pEntry) \
+	(PNDIS_PACKET)(pEntry)
+
+#define PACKET_TO_QUEUE_ENTRY(pPacket) \
+	(PQUEUE_ENTRY)(pPacket)
+
+
+#ifndef CONTAINING_RECORD
+#define CONTAINING_RECORD(address, type, field)			\
+((type *)((PCHAR)(address) - offsetof(type, field)))
+#endif
+
+
+#define RELEASE_NDIS_PACKET(_pAd, _pPacket, _Status)                    \
+{                                                                       \
+        RTMPFreeNdisPacket(_pAd, _pPacket);                             \
+}
+
+
+#define SWITCH_PhyAB(_pAA, _pBB)    \
+{                                                                           \
+    ULONG	AABasePaHigh;                           \
+    ULONG	AABasePaLow;                           \
+    ULONG	BBBasePaHigh;                           \
+    ULONG	BBBasePaLow;                           \
+    BBBasePaHigh = RTMP_GetPhysicalAddressHigh(_pBB);                                                 \
+    BBBasePaLow = RTMP_GetPhysicalAddressLow(_pBB);                                                 \
+    AABasePaHigh = RTMP_GetPhysicalAddressHigh(_pAA);                                                 \
+    AABasePaLow = RTMP_GetPhysicalAddressLow(_pAA);                                                 \
+    RTMP_SetPhysicalAddressHigh(_pAA, BBBasePaHigh);                                                 \
+    RTMP_SetPhysicalAddressLow(_pAA, BBBasePaLow);                                                 \
+    RTMP_SetPhysicalAddressHigh(_pBB, AABasePaHigh);                                                 \
+    RTMP_SetPhysicalAddressLow(_pBB, AABasePaLow);                                                 \
+}
+
+
+#define NdisWriteErrorLogEntry(_a, _b, _c, _d)
+#define NdisMAllocateMapRegisters(_a, _b, _c, _d, _e)		NDIS_STATUS_SUCCESS
+
+
+#define NdisAcquireSpinLock		RTMP_SEM_LOCK
+#define NdisReleaseSpinLock		RTMP_SEM_UNLOCK
+
+static inline void NdisGetSystemUpTime(ULONG *time)
+{
+	*time = jiffies;
+}
+
+//pPacket = CONTAINING_RECORD(pEntry, NDIS_PACKET, MiniportReservedEx);
+#define QUEUE_ENTRY_TO_PKT(pEntry) \
+		((PNDIS_PACKET) (pEntry))
+
+int rt28xx_packet_xmit(struct sk_buff *skb);
+
+
+
+void rtmp_os_thread_init(PUCHAR pThreadName, PVOID pNotify);
+
+#ifdef RT2860
+#if !defined(PCI_CAP_ID_EXP)
+#define PCI_CAP_ID_EXP			    0x10
+#endif
+
+#if !defined(PCI_EXP_LNKCTL)
+#define PCI_EXP_LNKCTL			    0x10
+#endif
+
+#if !defined(PCI_CLASS_BRIDGE_PCI)
+#define PCI_CLASS_BRIDGE_PCI		0x0604
+#endif
+
+#define PCIBUS_INTEL_VENDOR         0x8086
+#endif // RT2860 //
+
+
diff --git a/drivers/staging/rt2860/rt_main_dev.c b/drivers/staging/rt2860/rt_main_dev.c
new file mode 100644
index 00000000000..3873c478f4e
--- /dev/null
+++ b/drivers/staging/rt2860/rt_main_dev.c
@@ -0,0 +1,1686 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    rt_main_dev.c
+
+    Abstract:
+    Create and register network interface.
+
+    Revision History:
+    Who         When            What
+    --------    ----------      ----------------------------------------------
+	Sample		Mar/21/07		Merge RT2870 and RT2860 drivers.
+*/
+
+#include "rt_config.h"
+
+#define FORTY_MHZ_INTOLERANT_INTERVAL	(60*1000) // 1 min
+
+#ifdef MULTIPLE_CARD_SUPPORT
+// record whether the card in the card list is used in the card file
+UINT8  MC_CardUsed[MAX_NUM_OF_MULTIPLE_CARD];
+// record used card mac address in the card list
+static UINT8  MC_CardMac[MAX_NUM_OF_MULTIPLE_CARD][6];
+#endif // MULTIPLE_CARD_SUPPORT //
+
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+UINT32 CW_MAX_IN_BITS;
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+
+/*---------------------------------------------------------------------*/
+/* Private Variables Used                                              */
+/*---------------------------------------------------------------------*/
+//static RALINK_TIMER_STRUCT     PeriodicTimer;
+
+char *mac = "";		   // default 00:00:00:00:00:00
+char *hostname = "";		   // default CMPC
+#if LINUX_VERSION_CODE <= KERNEL_VERSION(2,6,12)
+MODULE_PARM (mac, "s");
+#else
+module_param (mac, charp, 0);
+#endif
+MODULE_PARM_DESC (mac, "rt28xx: wireless mac addr");
+
+
+/*---------------------------------------------------------------------*/
+/* Prototypes of Functions Used                                        */
+/*---------------------------------------------------------------------*/
+#ifdef DOT11_N_SUPPORT
+extern BOOLEAN ba_reordering_resource_init(PRTMP_ADAPTER pAd, int num);
+extern void ba_reordering_resource_release(PRTMP_ADAPTER pAd);
+#endif // DOT11_N_SUPPORT //
+extern NDIS_STATUS NICLoadRateSwitchingParams(IN PRTMP_ADAPTER pAd);
+
+#ifdef RT2860
+extern void init_thread_task(PRTMP_ADAPTER pAd);
+#endif // RT2860 //
+
+// public function prototype
+INT __devinit rt28xx_probe(IN void *_dev_p, IN void *_dev_id_p,
+							IN UINT argc, OUT PRTMP_ADAPTER *ppAd);
+
+// private function prototype
+static int rt28xx_init(IN struct net_device *net_dev);
+INT rt28xx_send_packets(IN struct sk_buff *skb_p, IN struct net_device *net_dev);
+
+#if LINUX_VERSION_CODE <= 0x20402	// Red Hat 7.1
+struct net_device *alloc_netdev(
+	int sizeof_priv,
+	const char *mask,
+	void (*setup)(struct net_device *));
+#endif // LINUX_VERSION_CODE //
+
+static void CfgInitHook(PRTMP_ADAPTER pAd);
+
+#ifdef CONFIG_STA_SUPPORT
+extern	const struct iw_handler_def rt28xx_iw_handler_def;
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+extern	const struct iw_handler_def rt28xx_ap_iw_handler_def;
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+
+#if WIRELESS_EXT >= 12
+// This function will be called when query /proc
+struct iw_statistics *rt28xx_get_wireless_stats(
+    IN struct net_device *net_dev);
+#endif
+
+struct net_device_stats *RT28xx_get_ether_stats(
+    IN  struct net_device *net_dev);
+
+/*
+========================================================================
+Routine Description:
+    Close raxx interface.
+
+Arguments:
+	*net_dev			the raxx interface pointer
+
+Return Value:
+    0					Open OK
+	otherwise			Open Fail
+
+Note:
+	1. if open fail, kernel will not call the close function.
+	2. Free memory for
+		(1) Mlme Memory Handler:		MlmeHalt()
+		(2) TX & RX:					RTMPFreeTxRxRingMemory()
+		(3) BA Reordering: 				ba_reordering_resource_release()
+========================================================================
+*/
+int MainVirtualIF_close(IN struct net_device *net_dev)
+{
+    RTMP_ADAPTER *pAd = net_dev->ml_priv;
+
+	// Sanity check for pAd
+	if (pAd == NULL)
+		return 0; // close ok
+
+	netif_carrier_off(pAd->net_dev);
+	netif_stop_queue(pAd->net_dev);
+
+
+	VIRTUAL_IF_DOWN(pAd);
+
+	RT_MOD_DEC_USE_COUNT();
+
+	return 0; // close ok
+}
+
+/*
+========================================================================
+Routine Description:
+    Open raxx interface.
+
+Arguments:
+	*net_dev			the raxx interface pointer
+
+Return Value:
+    0					Open OK
+	otherwise			Open Fail
+
+Note:
+	1. if open fail, kernel will not call the close function.
+	2. Free memory for
+		(1) Mlme Memory Handler:		MlmeHalt()
+		(2) TX & RX:					RTMPFreeTxRxRingMemory()
+		(3) BA Reordering: 				ba_reordering_resource_release()
+========================================================================
+*/
+int MainVirtualIF_open(IN struct net_device *net_dev)
+{
+    RTMP_ADAPTER *pAd = net_dev->ml_priv;
+
+	// Sanity check for pAd
+	if (pAd == NULL)
+		return 0; // close ok
+
+	if (VIRTUAL_IF_UP(pAd) != 0)
+		return -1;
+
+	// increase MODULE use count
+	RT_MOD_INC_USE_COUNT();
+
+	netif_start_queue(net_dev);
+	netif_carrier_on(net_dev);
+	netif_wake_queue(net_dev);
+
+	return 0;
+}
+
+/*
+========================================================================
+Routine Description:
+    Close raxx interface.
+
+Arguments:
+	*net_dev			the raxx interface pointer
+
+Return Value:
+    0					Open OK
+	otherwise			Open Fail
+
+Note:
+	1. if open fail, kernel will not call the close function.
+	2. Free memory for
+		(1) Mlme Memory Handler:		MlmeHalt()
+		(2) TX & RX:					RTMPFreeTxRxRingMemory()
+		(3) BA Reordering: 				ba_reordering_resource_release()
+========================================================================
+*/
+int rt28xx_close(IN PNET_DEV dev)
+{
+	struct net_device * net_dev = (struct net_device *)dev;
+    RTMP_ADAPTER	*pAd = net_dev->ml_priv;
+	BOOLEAN 		Cancelled = FALSE;
+	UINT32			i = 0;
+
+
+    DBGPRINT(RT_DEBUG_TRACE, ("===> rt28xx_close\n"));
+
+	// Sanity check for pAd
+	if (pAd == NULL)
+		return 0; // close ok
+
+
+#ifdef WDS_SUPPORT
+	WdsDown(pAd);
+#endif // WDS_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+#ifdef RT2860
+		RTMPPCIeLinkCtrlValueRestore(pAd, RESTORE_CLOSE);
+#endif // RT2860 //
+
+		// If dirver doesn't wake up firmware here,
+		// NICLoadFirmware will hang forever when interface is up again.
+		if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+        {
+		    AsicForceWakeup(pAd, TRUE);
+        }
+
+#ifdef QOS_DLS_SUPPORT
+		// send DLS-TEAR_DOWN message,
+		if (pAd->CommonCfg.bDLSCapable)
+		{
+			UCHAR i;
+
+			// tear down local dls table entry
+			for (i=0; i<MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+			{
+				if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+				{
+					RTMPSendDLSTearDownFrame(pAd, pAd->StaCfg.DLSEntry[i].MacAddr);
+					pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+					pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+				}
+			}
+
+			// tear down peer dls table entry
+			for (i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+			{
+				if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+				{
+					RTMPSendDLSTearDownFrame(pAd, pAd->StaCfg.DLSEntry[i].MacAddr);
+					pAd->StaCfg.DLSEntry[i].Status = DLS_NONE;
+					pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+				}
+			}
+			RT28XX_MLME_HANDLER(pAd);
+		}
+#endif // QOS_DLS_SUPPORT //
+
+		if (INFRA_ON(pAd) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+		{
+			MLME_DISASSOC_REQ_STRUCT	DisReq;
+			MLME_QUEUE_ELEM *MsgElem = (MLME_QUEUE_ELEM *) kmalloc(sizeof(MLME_QUEUE_ELEM), MEM_ALLOC_FLAG);
+
+			COPY_MAC_ADDR(DisReq.Addr, pAd->CommonCfg.Bssid);
+			DisReq.Reason =  REASON_DEAUTH_STA_LEAVING;
+
+			MsgElem->Machine = ASSOC_STATE_MACHINE;
+			MsgElem->MsgType = MT2_MLME_DISASSOC_REQ;
+			MsgElem->MsgLen = sizeof(MLME_DISASSOC_REQ_STRUCT);
+			NdisMoveMemory(MsgElem->Msg, &DisReq, sizeof(MLME_DISASSOC_REQ_STRUCT));
+
+			// Prevent to connect AP again in STAMlmePeriodicExec
+			pAd->MlmeAux.AutoReconnectSsidLen= 32;
+			NdisZeroMemory(pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.AutoReconnectSsidLen);
+
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_DISASSOC;
+			MlmeDisassocReqAction(pAd, MsgElem);
+			kfree(MsgElem);
+
+			RTMPusecDelay(1000);
+		}
+
+
+#ifdef CCX_SUPPORT
+		RTMPCancelTimer(&pAd->StaCfg.LeapAuthTimer, &Cancelled);
+#endif
+
+		RTMPCancelTimer(&pAd->StaCfg.StaQuickResponeForRateUpTimer, &Cancelled);
+		RTMPCancelTimer(&pAd->StaCfg.WpaDisassocAndBlockAssocTimer, &Cancelled);
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+		{
+			union iwreq_data    wrqu;
+			// send wireless event to wpa_supplicant for infroming interface down.
+			memset(&wrqu, 0, sizeof(wrqu));
+			wrqu.data.flags = RT_INTERFACE_DOWN;
+			wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+		}
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+		MlmeRadioOff(pAd);
+#ifdef RT2860
+		pAd->bPCIclkOff = FALSE;
+#endif // RT2860 //
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS);
+
+	for (i = 0 ; i < NUM_OF_TX_RING; i++)
+	{
+		while (pAd->DeQueueRunning[i] == TRUE)
+		{
+			printk("Waiting for TxQueue[%d] done..........\n", i);
+			RTMPusecDelay(1000);
+		}
+	}
+
+	// Stop Mlme state machine
+	MlmeHalt(pAd);
+
+	// Close kernel threads or tasklets
+	kill_thread_task(pAd);
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		MacTableReset(pAd);
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+
+	MeasureReqTabExit(pAd);
+	TpcReqTabExit(pAd);
+
+
+#ifdef RT2860
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_ACTIVE))
+	{
+		NICDisableInterrupt(pAd);
+	}
+
+	// Disable Rx, register value supposed will remain after reset
+	NICIssueReset(pAd);
+
+	// Free IRQ
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+		// Deregister interrupt function
+		RT28XX_IRQ_RELEASE(net_dev)
+		RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE);
+	}
+#endif // RT2860 //
+
+
+	// Free Ring or USB buffers
+	RTMPFreeTxRxRingMemory(pAd);
+
+	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS);
+
+#ifdef DOT11_N_SUPPORT
+	// Free BA reorder resource
+	ba_reordering_resource_release(pAd);
+#endif // DOT11_N_SUPPORT //
+
+
+	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_START_UP);
+
+	return 0; // close ok
+} /* End of rt28xx_close */
+
+static int rt28xx_init(IN struct net_device *net_dev)
+{
+	PRTMP_ADAPTER 			pAd = (PRTMP_ADAPTER)net_dev->ml_priv;
+	UINT					index;
+	UCHAR					TmpPhy;
+	NDIS_STATUS				Status;
+	UINT32 		MacCsr0 = 0;
+
+
+#ifdef DOT11_N_SUPPORT
+	// Allocate BA Reordering memory
+	ba_reordering_resource_init(pAd, MAX_REORDERING_MPDU_NUM);
+#endif // DOT11_N_SUPPORT //
+
+	// Make sure MAC gets ready.
+	index = 0;
+	do
+	{
+		RTMP_IO_READ32(pAd, MAC_CSR0, &MacCsr0);
+		pAd->MACVersion = MacCsr0;
+
+		if ((pAd->MACVersion != 0x00) && (pAd->MACVersion != 0xFFFFFFFF))
+			break;
+
+		RTMPusecDelay(10);
+	} while (index++ < 100);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("MAC_CSR0  [ Ver:Rev=0x%08x]\n", pAd->MACVersion));
+
+	// Disable DMA
+	RT28XXDMADisable(pAd);
+
+
+	// Load 8051 firmware
+	Status = NICLoadFirmware(pAd);
+	if (Status != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT_ERR(("NICLoadFirmware failed, Status[=0x%08x]\n", Status));
+		goto err1;
+	}
+
+	NICLoadRateSwitchingParams(pAd);
+
+	// Disable interrupts here which is as soon as possible
+	// This statement should never be true. We might consider to remove it later
+#ifdef RT2860
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_ACTIVE))
+	{
+		NICDisableInterrupt(pAd);
+	}
+#endif // RT2860 //
+
+	Status = RTMPAllocTxRxRingMemory(pAd);
+	if (Status != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT_ERR(("RTMPAllocDMAMemory failed, Status[=0x%08x]\n", Status));
+		goto err1;
+	}
+
+	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE);
+
+	// initialize MLME
+	//
+
+	Status = MlmeInit(pAd);
+	if (Status != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT_ERR(("MlmeInit failed, Status[=0x%08x]\n", Status));
+		goto err2;
+	}
+
+	// Initialize pAd->StaCfg, pAd->ApCfg, pAd->CommonCfg to manufacture default
+	//
+	UserCfgInit(pAd);
+
+
+	RT28XX_TASK_THREAD_INIT(pAd, Status);
+	if (Status != NDIS_STATUS_SUCCESS)
+		goto err1;
+
+	CfgInitHook(pAd);
+
+
+#ifdef BLOCK_NET_IF
+	initblockQueueTab(pAd);
+#endif // BLOCK_NET_IF //
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		NdisAllocateSpinLock(&pAd->MacTabLock);
+#endif // CONFIG_STA_SUPPORT //
+
+	MeasureReqTabInit(pAd);
+	TpcReqTabInit(pAd);
+
+	//
+	// Init the hardware, we need to init asic before read registry, otherwise mac register will be reset
+	//
+	Status = NICInitializeAdapter(pAd, TRUE);
+	if (Status != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT_ERR(("NICInitializeAdapter failed, Status[=0x%08x]\n", Status));
+		if (Status != NDIS_STATUS_SUCCESS)
+		goto err3;
+	}
+
+	// Read parameters from Config File
+	Status = RTMPReadParametersHook(pAd);
+
+	printk("1. Phy Mode = %d\n", pAd->CommonCfg.PhyMode);
+	if (Status != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT_ERR(("NICReadRegParameters failed, Status[=0x%08x]\n",Status));
+		goto err4;
+	}
+
+
+
+   	//Init Ba Capability parameters.
+#ifdef DOT11_N_SUPPORT
+	pAd->CommonCfg.DesiredHtPhy.MpduDensity = (UCHAR)pAd->CommonCfg.BACapability.field.MpduDensity;
+	pAd->CommonCfg.DesiredHtPhy.AmsduEnable = (USHORT)pAd->CommonCfg.BACapability.field.AmsduEnable;
+	pAd->CommonCfg.DesiredHtPhy.AmsduSize = (USHORT)pAd->CommonCfg.BACapability.field.AmsduSize;
+	pAd->CommonCfg.DesiredHtPhy.MimoPs = (USHORT)pAd->CommonCfg.BACapability.field.MMPSmode;
+	// UPdata to HT IE
+	pAd->CommonCfg.HtCapability.HtCapInfo.MimoPs = (USHORT)pAd->CommonCfg.BACapability.field.MMPSmode;
+	pAd->CommonCfg.HtCapability.HtCapInfo.AMsduSize = (USHORT)pAd->CommonCfg.BACapability.field.AmsduSize;
+	pAd->CommonCfg.HtCapability.HtCapParm.MpduDensity = (UCHAR)pAd->CommonCfg.BACapability.field.MpduDensity;
+#endif // DOT11_N_SUPPORT //
+
+	printk("2. Phy Mode = %d\n", pAd->CommonCfg.PhyMode);
+
+	// We should read EEPROM for all cases.  rt2860b
+	NICReadEEPROMParameters(pAd, mac);
+
+	printk("3. Phy Mode = %d\n", pAd->CommonCfg.PhyMode);
+
+	NICInitAsicFromEEPROM(pAd); //rt2860b
+
+	// Set PHY to appropriate mode
+	TmpPhy = pAd->CommonCfg.PhyMode;
+	pAd->CommonCfg.PhyMode = 0xff;
+	RTMPSetPhyMode(pAd, TmpPhy);
+#ifdef DOT11_N_SUPPORT
+	SetCommonHT(pAd);
+#endif // DOT11_N_SUPPORT //
+
+	// No valid channels.
+	if (pAd->ChannelListNum == 0)
+	{
+		printk("Wrong configuration. No valid channel found. Check \"ContryCode\" and \"ChannelGeography\" setting.\n");
+		goto err4;
+	}
+
+#ifdef DOT11_N_SUPPORT
+	printk("MCS Set = %02x %02x %02x %02x %02x\n", pAd->CommonCfg.HtCapability.MCSSet[0],
+           pAd->CommonCfg.HtCapability.MCSSet[1], pAd->CommonCfg.HtCapability.MCSSet[2],
+           pAd->CommonCfg.HtCapability.MCSSet[3], pAd->CommonCfg.HtCapability.MCSSet[4]);
+#endif // DOT11_N_SUPPORT //
+
+#ifdef IKANOS_VX_1X0
+	VR_IKANOS_FP_Init(pAd->ApCfg.BssidNum, pAd->PermanentAddress);
+#endif // IKANOS_VX_1X0 //
+
+		//
+	// Initialize RF register to default value
+	//
+	AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+	AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+
+	// 8051 firmware require the signal during booting time.
+	AsicSendCommandToMcu(pAd, 0x72, 0xFF, 0x00, 0x00);
+
+	if (pAd && (Status != NDIS_STATUS_SUCCESS))
+	{
+		//
+		// Undo everything if it failed
+		//
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+		{
+			RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE);
+		}
+	}
+	else if (pAd)
+	{
+		// Microsoft HCT require driver send a disconnect event after driver initialization.
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED);
+		RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_MEDIA_STATE_CHANGE);
+		DBGPRINT(RT_DEBUG_TRACE, ("NDIS_STATUS_MEDIA_DISCONNECT Event B!\n"));
+
+
+	}// end of else
+
+
+	DBGPRINT_S(Status, ("<==== RTMPInitialize, Status=%x\n", Status));
+
+	return TRUE;
+
+
+err4:
+err3:
+	MlmeHalt(pAd);
+err2:
+	RTMPFreeTxRxRingMemory(pAd);
+err1:
+
+#ifdef DOT11_N_SUPPORT
+	os_free_mem(pAd, pAd->mpdu_blk_pool.mem); // free BA pool
+#endif // DOT11_N_SUPPORT //
+	RT28XX_IRQ_RELEASE(net_dev);
+
+	// shall not set ml_priv to NULL here because the ml_priv didn't been free yet.
+	//net_dev->ml_priv = 0;
+#ifdef INF_AMAZON_SE
+err0:
+#endif // INF_AMAZON_SE //
+	printk("!!! %s Initialized fail !!!\n", RT28xx_CHIP_NAME);
+	return FALSE;
+} /* End of rt28xx_init */
+
+
+/*
+========================================================================
+Routine Description:
+    Open raxx interface.
+
+Arguments:
+	*net_dev			the raxx interface pointer
+
+Return Value:
+    0					Open OK
+	otherwise			Open Fail
+
+Note:
+========================================================================
+*/
+int rt28xx_open(IN PNET_DEV dev)
+{
+	struct net_device * net_dev = (struct net_device *)dev;
+	PRTMP_ADAPTER pAd = net_dev->ml_priv;
+	int retval = 0;
+ 	POS_COOKIE pObj;
+
+
+	// Sanity check for pAd
+	if (pAd == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -1;
+	}
+
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+	if (pAd->OpMode == OPMODE_AP)
+	{
+		CW_MAX_IN_BITS = 6;
+	}
+	else if (pAd->OpMode == OPMODE_STA)
+	{
+		CW_MAX_IN_BITS = 10;
+	}
+
+#if WIRELESS_EXT >= 12
+	if (net_dev->priv_flags == INT_MAIN)
+	{
+		if (pAd->OpMode == OPMODE_AP)
+			net_dev->wireless_handlers = (struct iw_handler_def *) &rt28xx_ap_iw_handler_def;
+		else if (pAd->OpMode == OPMODE_STA)
+			net_dev->wireless_handlers = (struct iw_handler_def *) &rt28xx_iw_handler_def;
+	}
+#endif // WIRELESS_EXT >= 12 //
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef RT2860
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+    	// If dirver doesn't wake up firmware here,
+    	// NICLoadFirmware will hang forever when interface is up again.
+    	// RT2860 PCI
+    	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE) &&
+        	OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE))
+    	{
+        	AUTO_WAKEUP_STRUC AutoWakeupCfg;
+			AsicForceWakeup(pAd, TRUE);
+        	AutoWakeupCfg.word = 0;
+	    	RTMP_IO_WRITE32(pAd, AUTO_WAKEUP_CFG, AutoWakeupCfg.word);
+        	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_DOZE);
+    	}
+	}
+#endif // RT2860 //
+#endif // CONFIG_STA_SUPPORT //
+
+	// Init
+ 	pObj = (POS_COOKIE)pAd->OS_Cookie;
+
+	// reset Adapter flags
+	RTMP_CLEAR_FLAGS(pAd);
+
+	// Request interrupt service routine for PCI device
+	// register the interrupt routine with the os
+	RT28XX_IRQ_REQUEST(net_dev);
+
+
+	// Init BssTab & ChannelInfo tabbles for auto channel select.
+
+
+	// Chip & other init
+	if (rt28xx_init(net_dev) == FALSE)
+		goto err;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		NdisZeroMemory(pAd->StaCfg.dev_name, 16);
+		NdisMoveMemory(pAd->StaCfg.dev_name, net_dev->name, strlen(net_dev->name));
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	// Set up the Mac address
+	NdisMoveMemory(net_dev->dev_addr, (void *) pAd->CurrentAddress, 6);
+
+	// Init IRQ parameters
+	RT28XX_IRQ_INIT(pAd);
+
+	// Various AP function init
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+		{
+			union iwreq_data    wrqu;
+			// send wireless event to wpa_supplicant for infroming interface down.
+			memset(&wrqu, 0, sizeof(wrqu));
+			wrqu.data.flags = RT_INTERFACE_UP;
+			wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+		}
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	// Enable Interrupt
+	RT28XX_IRQ_ENABLE(pAd);
+
+	// Now Enable RxTx
+	RTMPEnableRxTx(pAd);
+	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_START_UP);
+
+	{
+	UINT32 reg = 0;
+	RTMP_IO_READ32(pAd, 0x1300, &reg);  // clear garbage interrupts
+	printk("0x1300 = %08x\n", reg);
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef RT2860
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+        RTMPInitPCIeLinkCtrlValue(pAd);
+#endif // RT2860 //
+#endif // CONFIG_STA_SUPPORT //
+
+	return (retval);
+
+err:
+	return (-1);
+} /* End of rt28xx_open */
+
+
+/* Must not be called for mdev and apdev */
+static NDIS_STATUS rt_ieee80211_if_setup(struct net_device *dev, PRTMP_ADAPTER pAd)
+{
+	NDIS_STATUS Status;
+	INT     i=0;
+	CHAR    slot_name[IFNAMSIZ];
+	struct net_device   *device;
+
+
+	//ether_setup(dev);
+	dev->hard_start_xmit = rt28xx_send_packets;
+
+#ifdef IKANOS_VX_1X0
+	dev->hard_start_xmit = IKANOS_DataFramesTx;
+#endif // IKANOS_VX_1X0 //
+
+#ifdef CONFIG_STA_SUPPORT
+#if WIRELESS_EXT >= 12
+	if (pAd->OpMode == OPMODE_STA)
+	{
+		dev->wireless_handlers = &rt28xx_iw_handler_def;
+	}
+#endif //WIRELESS_EXT >= 12
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+#if WIRELESS_EXT >= 12
+	if (pAd->OpMode == OPMODE_AP)
+	{
+		dev->wireless_handlers = &rt28xx_ap_iw_handler_def;
+	}
+#endif //WIRELESS_EXT >= 12
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+
+#if WIRELESS_EXT < 21
+		dev->get_wireless_stats = rt28xx_get_wireless_stats;
+#endif
+	dev->get_stats = RT28xx_get_ether_stats;
+	dev->open = MainVirtualIF_open; //rt28xx_open;
+	dev->stop = MainVirtualIF_close; //rt28xx_close;
+	dev->priv_flags = INT_MAIN;
+	dev->do_ioctl = rt28xx_ioctl;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,24)
+    dev->validate_addr = NULL;
+#endif
+	// find available device name
+	for (i = 0; i < 8; i++)
+	{
+#ifdef MULTIPLE_CARD_SUPPORT
+		if (pAd->MC_RowID >= 0)
+			sprintf(slot_name, "ra%02d_%d", pAd->MC_RowID, i);
+		else
+#endif // MULTIPLE_CARD_SUPPORT //
+		sprintf(slot_name, "ra%d", i);
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,24)
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,26)
+        device = dev_get_by_name(dev_net(dev), slot_name);
+#else
+        device = dev_get_by_name(dev->nd_net, slot_name);
+#endif
+#else
+		device = dev_get_by_name(slot_name);
+#endif
+		if (device != NULL) dev_put(device);
+#else
+		for (device = dev_base; device != NULL; device = device->next)
+		{
+			if (strncmp(device->name, slot_name, 4) == 0)
+				break;
+		}
+#endif
+		if(device == NULL)
+			break;
+	}
+
+	if(i == 8)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("No available slot name\n"));
+		Status = NDIS_STATUS_FAILURE;
+	}
+	else
+	{
+#ifdef MULTIPLE_CARD_SUPPORT
+		if (pAd->MC_RowID >= 0)
+	        sprintf(dev->name, "ra%02d_%d", pAd->MC_RowID, i);
+		else
+#endif // MULTIPLE_CARD_SUPPORT //
+		sprintf(dev->name, "ra%d", i);
+		Status = NDIS_STATUS_SUCCESS;
+	}
+
+	return Status;
+
+}
+
+
+#ifdef MULTIPLE_CARD_SUPPORT
+/*
+========================================================================
+Routine Description:
+    Get card profile path.
+
+Arguments:
+    pAd
+
+Return Value:
+    TRUE		- Find a card profile
+	FALSE		- use default profile
+
+Note:
+========================================================================
+*/
+extern INT RTMPGetKeyParameter(
+    IN  PCHAR   key,
+    OUT PCHAR   dest,
+    IN  INT     destsize,
+    IN  PCHAR   buffer);
+
+BOOLEAN RTMP_CardInfoRead(
+	IN	PRTMP_ADAPTER pAd)
+{
+#define MC_SELECT_CARDID		0	/* use CARD ID (0 ~ 31) to identify different cards */
+#define MC_SELECT_MAC			1	/* use CARD MAC to identify different cards */
+#define MC_SELECT_CARDTYPE		2	/* use CARD type (abgn or bgn) to identify different cards */
+
+#define LETTER_CASE_TRANSLATE(txt_p, card_id)			\
+	{	UINT32 _len; char _char;						\
+		for(_len=0; _len<strlen(card_id); _len++) {		\
+			_char = *(txt_p + _len);					\
+			if (('A' <= _char) && (_char <= 'Z'))		\
+				*(txt_p+_len) = 'a'+(_char-'A');		\
+		} }
+
+	struct file *srcf;
+	INT retval, orgfsuid, orgfsgid;
+   	mm_segment_t orgfs;
+	CHAR *buffer, *tmpbuf, card_id_buf[30], RFIC_word[30];
+	BOOLEAN flg_match_ok = FALSE;
+	INT32 card_select_method;
+	INT32 card_free_id, card_nouse_id, card_same_mac_id, card_match_id;
+	EEPROM_ANTENNA_STRUC antenna;
+	USHORT addr01, addr23, addr45;
+	UINT8 mac[6];
+	UINT32 data, card_index;
+	UCHAR *start_ptr;
+
+
+	// init
+	buffer = kmalloc(MAX_INI_BUFFER_SIZE, MEM_ALLOC_FLAG);
+	if (buffer == NULL)
+        return FALSE;
+
+	tmpbuf = kmalloc(MAX_PARAM_BUFFER_SIZE, MEM_ALLOC_FLAG);
+	if(tmpbuf == NULL)
+	{
+		kfree(buffer);
+        return NDIS_STATUS_FAILURE;
+	}
+
+	orgfsuid = current->fsuid;
+	orgfsgid = current->fsgid;
+	current->fsuid = current->fsgid = 0;
+    orgfs = get_fs();
+    set_fs(KERNEL_DS);
+
+	// get RF IC type
+	RTMP_IO_READ32(pAd, E2PROM_CSR, &data);
+
+	if ((data & 0x30) == 0)
+		pAd->EEPROMAddressNum = 6;	// 93C46
+	else if ((data & 0x30) == 0x10)
+		pAd->EEPROMAddressNum = 8;	// 93C66
+	else
+		pAd->EEPROMAddressNum = 8;	// 93C86
+
+	RT28xx_EEPROM_READ16(pAd, EEPROM_NIC1_OFFSET, antenna.word);
+
+	if ((antenna.field.RfIcType == RFIC_2850) ||
+		(antenna.field.RfIcType == RFIC_2750))
+	{
+		/* ABGN card */
+		strcpy(RFIC_word, "abgn");
+	}
+	else
+	{
+		/* BGN card */
+		strcpy(RFIC_word, "bgn");
+	}
+
+	// get MAC address
+	RT28xx_EEPROM_READ16(pAd, 0x04, addr01);
+	RT28xx_EEPROM_READ16(pAd, 0x06, addr23);
+	RT28xx_EEPROM_READ16(pAd, 0x08, addr45);
+
+	mac[0] = (UCHAR)(addr01 & 0xff);
+	mac[1] = (UCHAR)(addr01 >> 8);
+	mac[2] = (UCHAR)(addr23 & 0xff);
+	mac[3] = (UCHAR)(addr23 >> 8);
+	mac[4] = (UCHAR)(addr45 & 0xff);
+	mac[5] = (UCHAR)(addr45 >> 8);
+
+	// open card information file
+	srcf = filp_open(CARD_INFO_PATH, O_RDONLY, 0);
+	if (IS_ERR(srcf))
+	{
+		/* card information file does not exist */
+			DBGPRINT(RT_DEBUG_TRACE,
+				("--> Error %ld opening %s\n", -PTR_ERR(srcf), CARD_INFO_PATH));
+		return FALSE;
+	}
+
+	if (srcf->f_op && srcf->f_op->read)
+	{
+		/* card information file exists so reading the card information */
+		memset(buffer, 0x00, MAX_INI_BUFFER_SIZE);
+		retval = srcf->f_op->read(srcf, buffer, MAX_INI_BUFFER_SIZE, &srcf->f_pos);
+		if (retval < 0)
+		{
+			/* read fail */
+				DBGPRINT(RT_DEBUG_TRACE,
+					("--> Read %s error %d\n", CARD_INFO_PATH, -retval));
+		}
+		else
+		{
+			/* get card selection method */
+			memset(tmpbuf, 0x00, MAX_PARAM_BUFFER_SIZE);
+			card_select_method = MC_SELECT_CARDTYPE; // default
+
+			if (RTMPGetKeyParameter("SELECT", tmpbuf, 256, buffer))
+			{
+				if (strcmp(tmpbuf, "CARDID") == 0)
+					card_select_method = MC_SELECT_CARDID;
+				else if (strcmp(tmpbuf, "MAC") == 0)
+					card_select_method = MC_SELECT_MAC;
+				else if (strcmp(tmpbuf, "CARDTYPE") == 0)
+					card_select_method = MC_SELECT_CARDTYPE;
+			}
+
+			DBGPRINT(RT_DEBUG_TRACE,
+					("MC> Card Selection = %d\n", card_select_method));
+
+			// init
+			card_free_id = -1;
+			card_nouse_id = -1;
+			card_same_mac_id = -1;
+			card_match_id = -1;
+
+			// search current card information records
+			for(card_index=0;
+				card_index<MAX_NUM_OF_MULTIPLE_CARD;
+				card_index++)
+			{
+				if ((*(UINT32 *)&MC_CardMac[card_index][0] == 0) &&
+					(*(UINT16 *)&MC_CardMac[card_index][4] == 0))
+				{
+					// MAC is all-0 so the entry is available
+					MC_CardUsed[card_index] = 0;
+
+					if (card_free_id < 0)
+						card_free_id = card_index; // 1st free entry
+				}
+				else
+				{
+					if (memcmp(MC_CardMac[card_index], mac, 6) == 0)
+					{
+						// we find the entry with same MAC
+						if (card_same_mac_id < 0)
+							card_same_mac_id = card_index; // 1st same entry
+					}
+					else
+					{
+						// MAC is not all-0 but used flag == 0
+						if ((MC_CardUsed[card_index] == 0) &&
+							(card_nouse_id < 0))
+						{
+							card_nouse_id = card_index; // 1st available entry
+						}
+					}
+				}
+			}
+
+			DBGPRINT(RT_DEBUG_TRACE,
+					("MC> Free = %d, Same = %d, NOUSE = %d\n",
+					card_free_id, card_same_mac_id, card_nouse_id));
+
+			if ((card_same_mac_id >= 0) &&
+				((card_select_method == MC_SELECT_CARDID) ||
+				(card_select_method == MC_SELECT_CARDTYPE)))
+			{
+				// same MAC entry is found
+				card_match_id = card_same_mac_id;
+
+				if (card_select_method == MC_SELECT_CARDTYPE)
+				{
+					// for CARDTYPE
+					sprintf(card_id_buf, "%02dCARDTYPE%s",
+							card_match_id, RFIC_word);
+
+					if ((start_ptr=rtstrstruncasecmp(buffer, card_id_buf)) != NULL)
+					{
+						// we found the card ID
+						LETTER_CASE_TRANSLATE(start_ptr, card_id_buf);
+					}
+				}
+			}
+			else
+			{
+				// the card is 1st plug-in, try to find the match card profile
+				switch(card_select_method)
+				{
+					case MC_SELECT_CARDID: // CARDID
+					default:
+						if (card_free_id >= 0)
+							card_match_id = card_free_id;
+						else
+							card_match_id = card_nouse_id;
+						break;
+
+					case MC_SELECT_MAC: // MAC
+						sprintf(card_id_buf, "MAC%02x:%02x:%02x:%02x:%02x:%02x",
+								mac[0], mac[1], mac[2],
+								mac[3], mac[4], mac[5]);
+
+						/* try to find the key word in the card file */
+						if ((start_ptr=rtstrstruncasecmp(buffer, card_id_buf)) != NULL)
+						{
+							LETTER_CASE_TRANSLATE(start_ptr, card_id_buf);
+
+							/* get the row ID (2 ASCII characters) */
+							start_ptr -= 2;
+							card_id_buf[0] = *(start_ptr);
+							card_id_buf[1] = *(start_ptr+1);
+							card_id_buf[2] = 0x00;
+
+							card_match_id = simple_strtol(card_id_buf, 0, 10);
+						}
+						break;
+
+					case MC_SELECT_CARDTYPE: // CARDTYPE
+						card_nouse_id = -1;
+
+						for(card_index=0;
+							card_index<MAX_NUM_OF_MULTIPLE_CARD;
+							card_index++)
+						{
+							sprintf(card_id_buf, "%02dCARDTYPE%s",
+									card_index, RFIC_word);
+
+							if ((start_ptr=rtstrstruncasecmp(buffer,
+														card_id_buf)) != NULL)
+							{
+								LETTER_CASE_TRANSLATE(start_ptr, card_id_buf);
+
+								if (MC_CardUsed[card_index] == 0)
+								{
+									/* current the card profile is not used */
+									if ((*(UINT32 *)&MC_CardMac[card_index][0] == 0) &&
+										(*(UINT16 *)&MC_CardMac[card_index][4] == 0))
+									{
+										// find it and no previous card use it
+										card_match_id = card_index;
+										break;
+									}
+									else
+									{
+										// ever a card use it
+										if (card_nouse_id < 0)
+											card_nouse_id = card_index;
+									}
+								}
+							}
+						}
+
+						// if not find a free one, use the available one
+						if (card_match_id < 0)
+							card_match_id = card_nouse_id;
+						break;
+				}
+			}
+
+			if (card_match_id >= 0)
+			{
+				// make up search keyword
+				switch(card_select_method)
+				{
+					case MC_SELECT_CARDID: // CARDID
+						sprintf(card_id_buf, "%02dCARDID", card_match_id);
+						break;
+
+					case MC_SELECT_MAC: // MAC
+						sprintf(card_id_buf,
+								"%02dmac%02x:%02x:%02x:%02x:%02x:%02x",
+								card_match_id,
+								mac[0], mac[1], mac[2],
+								mac[3], mac[4], mac[5]);
+						break;
+
+					case MC_SELECT_CARDTYPE: // CARDTYPE
+					default:
+						sprintf(card_id_buf, "%02dcardtype%s",
+								card_match_id, RFIC_word);
+						break;
+				}
+
+				DBGPRINT(RT_DEBUG_TRACE, ("Search Keyword = %s\n", card_id_buf));
+
+				// read card file path
+				if (RTMPGetKeyParameter(card_id_buf, tmpbuf, 256, buffer))
+				{
+					if (strlen(tmpbuf) < sizeof(pAd->MC_FileName))
+					{
+						// backup card information
+						pAd->MC_RowID = card_match_id; /* base 0 */
+						MC_CardUsed[card_match_id] = 1;
+						memcpy(MC_CardMac[card_match_id], mac, sizeof(mac));
+
+						// backup card file path
+						NdisMoveMemory(pAd->MC_FileName, tmpbuf , strlen(tmpbuf));
+						pAd->MC_FileName[strlen(tmpbuf)] = '\0';
+						flg_match_ok = TRUE;
+
+						DBGPRINT(RT_DEBUG_TRACE,
+								("Card Profile Name = %s\n", pAd->MC_FileName));
+					}
+					else
+					{
+						DBGPRINT(RT_DEBUG_ERROR,
+								("Card Profile Name length too large!\n"));
+					}
+				}
+				else
+				{
+					DBGPRINT(RT_DEBUG_ERROR,
+							("Can not find search key word in card.dat!\n"));
+				}
+
+				if ((flg_match_ok != TRUE) &&
+					(card_match_id < MAX_NUM_OF_MULTIPLE_CARD))
+				{
+					MC_CardUsed[card_match_id] = 0;
+					memset(MC_CardMac[card_match_id], 0, sizeof(mac));
+				}
+			} // if (card_match_id >= 0)
+		}
+	}
+
+	// close file
+	retval = filp_close(srcf, NULL);
+	set_fs(orgfs);
+	current->fsuid = orgfsuid;
+	current->fsgid = orgfsgid;
+	kfree(buffer);
+	kfree(tmpbuf);
+	return flg_match_ok;
+}
+#endif // MULTIPLE_CARD_SUPPORT //
+
+
+/*
+========================================================================
+Routine Description:
+    Probe RT28XX chipset.
+
+Arguments:
+    _dev_p				Point to the PCI or USB device
+	_dev_id_p			Point to the PCI or USB device ID
+
+Return Value:
+    0					Probe OK
+	-ENODEV				Probe Fail
+
+Note:
+========================================================================
+*/
+INT __devinit   rt28xx_probe(
+    IN  void *_dev_p,
+    IN  void *_dev_id_p,
+	IN  UINT argc,
+	OUT PRTMP_ADAPTER *ppAd)
+{
+    struct  net_device	*net_dev;
+    PRTMP_ADAPTER       pAd = (PRTMP_ADAPTER) NULL;
+    INT                 status;
+	PVOID				handle;
+#ifdef RT2860
+	struct pci_dev *dev_p = (struct pci_dev *)_dev_p;
+#endif // RT2860 //
+
+
+#ifdef CONFIG_STA_SUPPORT
+    DBGPRINT(RT_DEBUG_TRACE, ("STA Driver version-%s\n", STA_DRIVER_VERSION));
+#endif // CONFIG_STA_SUPPORT //
+
+#if LINUX_VERSION_CODE <= 0x20402       // Red Hat 7.1
+    net_dev = alloc_netdev(sizeof(PRTMP_ADAPTER), "eth%d", ether_setup);
+#else
+    net_dev = alloc_etherdev(sizeof(PRTMP_ADAPTER));
+#endif
+    if (net_dev == NULL)
+    {
+        printk("alloc_netdev failed\n");
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,15)
+		module_put(THIS_MODULE);
+#endif //LINUX_VERSION_CODE < KERNEL_VERSION(2,6,15)
+#else
+		MOD_DEC_USE_COUNT;
+#endif
+        goto err_out;
+    }
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,24)
+    SET_MODULE_OWNER(net_dev);
+#endif
+
+	netif_stop_queue(net_dev);
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+/* for supporting Network Manager */
+/* Set the sysfs physical device reference for the network logical device
+ * if set prior to registration will cause a symlink during initialization.
+ */
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0))
+    SET_NETDEV_DEV(net_dev, &(dev_p->dev));
+#endif
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+
+	// Allocate RTMP_ADAPTER miniport adapter structure
+	handle = kmalloc(sizeof(struct os_cookie), GFP_KERNEL);
+	RT28XX_HANDLE_DEV_ASSIGN(handle, dev_p);
+
+	status = RTMPAllocAdapterBlock(handle, &pAd);
+	if (status != NDIS_STATUS_SUCCESS)
+		goto err_out_free_netdev;
+
+	net_dev->ml_priv = (PVOID)pAd;
+    pAd->net_dev = net_dev; // must be before RT28XXNetDevInit()
+
+	RT28XXNetDevInit(_dev_p, net_dev, pAd);
+
+#ifdef CONFIG_STA_SUPPORT
+    pAd->StaCfg.OriDevType = net_dev->type;
+#endif // CONFIG_STA_SUPPORT //
+
+	// Post config
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
+	if (RT28XXProbePostConfig(_dev_p, pAd, argc) == FALSE)
+		goto err_out_unmap;
+#else
+	if (RT28XXProbePostConfig(_dev_p, pAd, 0) == FALSE)
+		goto err_out_unmap;
+#endif // LINUX_VERSION_CODE //
+
+#ifdef CONFIG_STA_SUPPORT
+	pAd->OpMode = OPMODE_STA;
+#endif // CONFIG_STA_SUPPORT //
+
+
+#ifdef MULTIPLE_CARD_SUPPORT
+	// find its profile path
+	pAd->MC_RowID = -1; // use default profile path
+	RTMP_CardInfoRead(pAd);
+
+	if (pAd->MC_RowID == -1)
+#ifdef CONFIG_STA_SUPPORT
+		strcpy(pAd->MC_FileName, STA_PROFILE_PATH);
+#endif // CONFIG_STA_SUPPORT //
+
+	DBGPRINT(RT_DEBUG_TRACE,
+			("MC> ROW = %d, PATH = %s\n", pAd->MC_RowID, pAd->MC_FileName));
+#endif // MULTIPLE_CARD_SUPPORT //
+
+	// sample move
+	if (rt_ieee80211_if_setup(net_dev, pAd) != NDIS_STATUS_SUCCESS)
+		goto err_out_unmap;
+
+    // Register this device
+    status = register_netdev(net_dev);
+    if (status)
+        goto err_out_unmap;
+
+    // Set driver data
+	RT28XX_DRVDATA_SET(_dev_p);
+
+	*ppAd = pAd;
+    return 0; // probe ok
+
+
+	/* --------------------------- ERROR HANDLE --------------------------- */
+err_out_unmap:
+	RTMPFreeAdapter(pAd);
+	RT28XX_UNMAP();
+
+err_out_free_netdev:
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+    free_netdev(net_dev);
+#else
+	kfree(net_dev);
+#endif
+
+err_out:
+	RT28XX_PUT_DEVICE(dev_p);
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
+	return (LONG)NULL;
+#else
+    return -ENODEV; /* probe fail */
+#endif // LINUX_VERSION_CODE //
+} /* End of rt28xx_probe */
+
+
+/*
+========================================================================
+Routine Description:
+    The entry point for Linux kernel sent packet to our driver.
+
+Arguments:
+    sk_buff *skb		the pointer refer to a sk_buffer.
+
+Return Value:
+    0
+
+Note:
+	This function is the entry point of Tx Path for Os delivery packet to
+	our driver. You only can put OS-depened & STA/AP common handle procedures
+	in here.
+========================================================================
+*/
+int rt28xx_packet_xmit(struct sk_buff *skb)
+{
+	struct net_device *net_dev = skb->dev;
+	PRTMP_ADAPTER pAd = net_dev->ml_priv;
+	int status = 0;
+	PNDIS_PACKET pPacket = (PNDIS_PACKET) skb;
+
+	/* RT2870STA does this in RTMPSendPackets() */
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+	{
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_RESOURCES);
+		return 0;
+	}
+#endif // RALINK_ATE //
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// Drop send request since we are in monitor mode
+		if (MONITOR_ON(pAd))
+		{
+			RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+			goto done;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+        // EapolStart size is 18
+	if (skb->len < 14)
+	{
+		//printk("bad packet size: %d\n", pkt->len);
+		hex_dump("bad packet", skb->data, skb->len);
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+		goto done;
+	}
+
+	RTMP_SET_PACKET_5VT(pPacket, 0);
+#ifdef CONFIG_5VT_ENHANCE
+    if (*(int*)(skb->cb) == BRIDGE_TAG) {
+		RTMP_SET_PACKET_5VT(pPacket, 1);
+    }
+#endif
+
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+
+		STASendPackets((NDIS_HANDLE)pAd, (PPNDIS_PACKET) &pPacket, 1);
+	}
+
+#endif // CONFIG_STA_SUPPORT //
+
+	status = 0;
+done:
+
+	return status;
+}
+
+
+/*
+========================================================================
+Routine Description:
+    Send a packet to WLAN.
+
+Arguments:
+    skb_p           points to our adapter
+    dev_p           which WLAN network interface
+
+Return Value:
+    0: transmit successfully
+    otherwise: transmit fail
+
+Note:
+========================================================================
+*/
+INT rt28xx_send_packets(
+	IN struct sk_buff 		*skb_p,
+	IN struct net_device 	*net_dev)
+{
+    RTMP_ADAPTER *pAd = net_dev->ml_priv;
+	if (!(net_dev->flags & IFF_UP))
+	{
+		RELEASE_NDIS_PACKET(pAd, (PNDIS_PACKET)skb_p, NDIS_STATUS_FAILURE);
+		return 0;
+	}
+
+	NdisZeroMemory((PUCHAR)&skb_p->cb[CB_OFF], 15);
+	RTMP_SET_PACKET_NET_DEVICE_MBSSID(skb_p, MAIN_MBSSID);
+
+	return rt28xx_packet_xmit(skb_p);
+
+} /* End of MBSS_VirtualIF_PacketSend */
+
+
+
+
+#if LINUX_VERSION_CODE <= 0x20402	// Red Hat 7.1
+struct net_device *alloc_netdev(
+	int sizeof_priv,
+	const char *mask,
+	void (*setup)(struct net_device *))
+{
+    struct net_device	*dev;
+    INT					alloc_size;
+
+
+    /* ensure 32-byte alignment of the private area */
+    alloc_size = sizeof (*dev) + sizeof_priv + 31;
+
+    dev = (struct net_device *) kmalloc(alloc_size, GFP_KERNEL);
+    if (dev == NULL)
+    {
+        DBGPRINT(RT_DEBUG_ERROR,
+				("alloc_netdev: Unable to allocate device memory.\n"));
+        return NULL;
+    }
+
+    memset(dev, 0, alloc_size);
+
+    if (sizeof_priv)
+        dev->priv = (void *) (((long)(dev + 1) + 31) & ~31);
+
+    setup(dev);
+    strcpy(dev->name, mask);
+
+    return dev;
+}
+#endif // LINUX_VERSION_CODE //
+
+
+void CfgInitHook(PRTMP_ADAPTER pAd)
+{
+	pAd->bBroadComHT = TRUE;
+} /* End of CfgInitHook */
+
+
+#if WIRELESS_EXT >= 12
+// This function will be called when query /proc
+struct iw_statistics *rt28xx_get_wireless_stats(
+    IN struct net_device *net_dev)
+{
+	PRTMP_ADAPTER pAd = net_dev->ml_priv;
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("rt28xx_get_wireless_stats --->\n"));
+
+	pAd->iw_stats.status = 0; // Status - device dependent for now
+
+	// link quality
+	pAd->iw_stats.qual.qual = ((pAd->Mlme.ChannelQuality * 12)/10 + 10);
+	if(pAd->iw_stats.qual.qual > 100)
+		pAd->iw_stats.qual.qual = 100;
+
+#ifdef CONFIG_STA_SUPPORT
+	if (pAd->OpMode == OPMODE_STA)
+		pAd->iw_stats.qual.level = RTMPMaxRssi(pAd, pAd->StaCfg.RssiSample.LastRssi0, pAd->StaCfg.RssiSample.LastRssi1, pAd->StaCfg.RssiSample.LastRssi2);
+#endif // CONFIG_STA_SUPPORT //
+
+	pAd->iw_stats.qual.noise = pAd->BbpWriteLatch[66]; // noise level (dBm)
+
+	pAd->iw_stats.qual.noise += 256 - 143;
+	pAd->iw_stats.qual.updated = 1;     // Flags to know if updated
+#ifdef IW_QUAL_DBM
+	pAd->iw_stats.qual.updated |= IW_QUAL_DBM;	// Level + Noise are dBm
+#endif // IW_QUAL_DBM //
+
+	pAd->iw_stats.discard.nwid = 0;     // Rx : Wrong nwid/essid
+	pAd->iw_stats.miss.beacon = 0;      // Missed beacons/superframe
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<--- rt28xx_get_wireless_stats\n"));
+	return &pAd->iw_stats;
+} /* End of rt28xx_get_wireless_stats */
+#endif // WIRELESS_EXT //
+
+
+
+void tbtt_tasklet(unsigned long data)
+{
+#define MAX_TX_IN_TBTT		(16)
+
+}
+
+INT rt28xx_ioctl(
+	IN	struct net_device	*net_dev,
+	IN	OUT	struct ifreq	*rq,
+	IN	INT					cmd)
+{
+	VIRTUAL_ADAPTER	*pVirtualAd = NULL;
+	RTMP_ADAPTER	*pAd = NULL;
+	INT				ret = 0;
+
+	if (net_dev->priv_flags == INT_MAIN)
+	{
+		pAd = net_dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = net_dev->ml_priv;
+		pAd = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAd == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		ret = rt28xx_sta_ioctl(net_dev, rq, cmd);
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	return ret;
+}
+
+/*
+    ========================================================================
+
+    Routine Description:
+        return ethernet statistics counter
+
+    Arguments:
+        net_dev                     Pointer to net_device
+
+    Return Value:
+        net_device_stats*
+
+    Note:
+
+    ========================================================================
+*/
+struct net_device_stats *RT28xx_get_ether_stats(
+    IN  struct net_device *net_dev)
+{
+    RTMP_ADAPTER *pAd = NULL;
+
+	if (net_dev)
+		pAd = net_dev->ml_priv;
+
+	if (pAd)
+	{
+
+		pAd->stats.rx_packets = pAd->WlanCounters.ReceivedFragmentCount.QuadPart;
+		pAd->stats.tx_packets = pAd->WlanCounters.TransmittedFragmentCount.QuadPart;
+
+		pAd->stats.rx_bytes = pAd->RalinkCounters.ReceivedByteCount;
+		pAd->stats.tx_bytes = pAd->RalinkCounters.TransmittedByteCount;
+
+		pAd->stats.rx_errors = pAd->Counters8023.RxErrors;
+		pAd->stats.tx_errors = pAd->Counters8023.TxErrors;
+
+		pAd->stats.rx_dropped = 0;
+		pAd->stats.tx_dropped = 0;
+
+	    pAd->stats.multicast = pAd->WlanCounters.MulticastReceivedFrameCount.QuadPart;   // multicast packets received
+	    pAd->stats.collisions = pAd->Counters8023.OneCollision + pAd->Counters8023.MoreCollisions;  // Collision packets
+
+	    pAd->stats.rx_length_errors = 0;
+	    pAd->stats.rx_over_errors = pAd->Counters8023.RxNoBuffer;                   // receiver ring buff overflow
+	    pAd->stats.rx_crc_errors = 0;//pAd->WlanCounters.FCSErrorCount;     // recved pkt with crc error
+	    pAd->stats.rx_frame_errors = pAd->Counters8023.RcvAlignmentErrors;          // recv'd frame alignment error
+	    pAd->stats.rx_fifo_errors = pAd->Counters8023.RxNoBuffer;                   // recv'r fifo overrun
+	    pAd->stats.rx_missed_errors = 0;                                            // receiver missed packet
+
+	    // detailed tx_errors
+	    pAd->stats.tx_aborted_errors = 0;
+	    pAd->stats.tx_carrier_errors = 0;
+	    pAd->stats.tx_fifo_errors = 0;
+	    pAd->stats.tx_heartbeat_errors = 0;
+	    pAd->stats.tx_window_errors = 0;
+
+	    // for cslip etc
+	    pAd->stats.rx_compressed = 0;
+	    pAd->stats.tx_compressed = 0;
+
+		return &pAd->stats;
+	}
+	else
+    	return NULL;
+}
+
diff --git a/drivers/staging/rt2860/rt_profile.c b/drivers/staging/rt2860/rt_profile.c
new file mode 100644
index 00000000000..cd7ffc8a6e8
--- /dev/null
+++ b/drivers/staging/rt2860/rt_profile.c
@@ -0,0 +1,1981 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#include "rt_config.h"
+
+#ifdef DOT11_N_SUPPORT
+static void HTParametersHook(
+	IN	PRTMP_ADAPTER pAd,
+	IN	CHAR		  *pValueStr,
+	IN	CHAR		  *pInput);
+#endif // DOT11_N_SUPPORT //
+
+#define ETH_MAC_ADDR_STR_LEN 17  // in format of xx:xx:xx:xx:xx:xx
+
+// We assume the s1 is a sting, s2 is a memory space with 6 bytes. and content of s1 will be changed.
+BOOLEAN rtstrmactohex(char *s1, char *s2)
+{
+	int i = 0;
+	char *ptokS = s1, *ptokE = s1;
+
+	if (strlen(s1) != ETH_MAC_ADDR_STR_LEN)
+		return FALSE;
+
+	while((*ptokS) != '\0')
+	{
+		if((ptokE = strchr(ptokS, ':')) != NULL)
+			*ptokE++ = '\0';
+		if ((strlen(ptokS) != 2) || (!isxdigit(*ptokS)) || (!isxdigit(*(ptokS+1))))
+			break; // fail
+		AtoH(ptokS, &s2[i++], 1);
+		ptokS = ptokE;
+		if (i == 6)
+			break; // parsing finished
+	}
+
+	return ( i == 6 ? TRUE : FALSE);
+
+}
+
+
+// we assume the s1 and s2 both are strings.
+BOOLEAN rtstrcasecmp(char *s1, char *s2)
+{
+	char *p1 = s1, *p2 = s2;
+
+	if (strlen(s1) != strlen(s2))
+		return FALSE;
+
+	while(*p1 != '\0')
+	{
+		if((*p1 != *p2) && ((*p1 ^ *p2) != 0x20))
+			return FALSE;
+		p1++;
+		p2++;
+	}
+
+	return TRUE;
+}
+
+// we assume the s1 (buffer) and s2 (key) both are strings.
+char * rtstrstruncasecmp(char * s1, char * s2)
+{
+	INT l1, l2, i;
+	char temp1, temp2;
+
+	l2 = strlen(s2);
+	if (!l2)
+		return (char *) s1;
+
+	l1 = strlen(s1);
+
+	while (l1 >= l2)
+	{
+		l1--;
+
+		for(i=0; i<l2; i++)
+		{
+			temp1 = *(s1+i);
+			temp2 = *(s2+i);
+
+			if (('a' <= temp1) && (temp1 <= 'z'))
+				temp1 = 'A'+(temp1-'a');
+			if (('a' <= temp2) && (temp2 <= 'z'))
+				temp2 = 'A'+(temp2-'a');
+
+			if (temp1 != temp2)
+				break;
+		}
+
+		if (i == l2)
+			return (char *) s1;
+
+		s1++;
+	}
+
+	return NULL; // not found
+}
+
+//add by kathy
+
+ /**
+  * strstr - Find the first substring in a %NUL terminated string
+  * @s1: The string to be searched
+  * @s2: The string to search for
+  */
+char * rtstrstr(const char * s1,const char * s2)
+{
+	INT l1, l2;
+
+	l2 = strlen(s2);
+	if (!l2)
+		return (char *) s1;
+
+	l1 = strlen(s1);
+
+	while (l1 >= l2)
+	{
+		l1--;
+		if (!memcmp(s1,s2,l2))
+			return (char *) s1;
+		s1++;
+	}
+
+	return NULL;
+}
+
+/**
+ * rstrtok - Split a string into tokens
+ * @s: The string to be searched
+ * @ct: The characters to search for
+ * * WARNING: strtok is deprecated, use strsep instead. However strsep is not compatible with old architecture.
+ */
+char * __rstrtok;
+char * rstrtok(char * s,const char * ct)
+{
+	char *sbegin, *send;
+
+	sbegin  = s ? s : __rstrtok;
+	if (!sbegin)
+	{
+		return NULL;
+	}
+
+	sbegin += strspn(sbegin,ct);
+	if (*sbegin == '\0')
+	{
+		__rstrtok = NULL;
+		return( NULL );
+	}
+
+	send = strpbrk( sbegin, ct);
+	if (send && *send != '\0')
+		*send++ = '\0';
+
+	__rstrtok = send;
+
+	return (sbegin);
+}
+
+/**
+ * delimitcnt - return the count of a given delimiter in a given string.
+ * @s: The string to be searched.
+ * @ct: The delimiter to search for.
+ * Notice : We suppose the delimiter is a single-char string(for example : ";").
+ */
+INT delimitcnt(char * s,const char * ct)
+{
+	INT count = 0;
+	/* point to the beginning of the line */
+	const char *token = s;
+
+	for ( ;; )
+	{
+		token = strpbrk(token, ct); /* search for delimiters */
+
+        if ( token == NULL )
+		{
+			/* advanced to the terminating null character */
+			break;
+		}
+		/* skip the delimiter */
+	    ++token;
+
+		/*
+		 * Print the found text: use len with %.*s to specify field width.
+		 */
+
+		/* accumulate delimiter count */
+	    ++count;
+	}
+    return count;
+}
+
+/*
+  * converts the Internet host address from the standard numbers-and-dots notation
+  * into binary data.
+  * returns nonzero if the address is valid, zero if not.
+  */
+int rtinet_aton(const char *cp, unsigned int *addr)
+{
+	unsigned int 	val;
+	int         	base, n;
+	char        	c;
+	unsigned int    parts[4];
+	unsigned int    *pp = parts;
+
+	for (;;)
+    {
+         /*
+          * Collect number up to ``.''.
+          * Values are specified as for C:
+          *	0x=hex, 0=octal, other=decimal.
+          */
+         val = 0;
+         base = 10;
+         if (*cp == '0')
+         {
+             if (*++cp == 'x' || *cp == 'X')
+                 base = 16, cp++;
+             else
+                 base = 8;
+         }
+         while ((c = *cp) != '\0')
+         {
+             if (isdigit((unsigned char) c))
+             {
+                 val = (val * base) + (c - '0');
+                 cp++;
+                 continue;
+             }
+             if (base == 16 && isxdigit((unsigned char) c))
+             {
+                 val = (val << 4) +
+                     (c + 10 - (islower((unsigned char) c) ? 'a' : 'A'));
+                 cp++;
+                 continue;
+             }
+             break;
+         }
+         if (*cp == '.')
+         {
+             /*
+              * Internet format: a.b.c.d a.b.c   (with c treated as 16-bits)
+              * a.b     (with b treated as 24 bits)
+              */
+             if (pp >= parts + 3 || val > 0xff)
+                 return 0;
+             *pp++ = val, cp++;
+         }
+         else
+             break;
+     }
+
+     /*
+      * Check for trailing junk.
+      */
+     while (*cp)
+         if (!isspace((unsigned char) *cp++))
+             return 0;
+
+     /*
+      * Concoct the address according to the number of parts specified.
+      */
+     n = pp - parts + 1;
+     switch (n)
+     {
+
+         case 1:         /* a -- 32 bits */
+             break;
+
+         case 2:         /* a.b -- 8.24 bits */
+             if (val > 0xffffff)
+                 return 0;
+             val |= parts[0] << 24;
+             break;
+
+         case 3:         /* a.b.c -- 8.8.16 bits */
+             if (val > 0xffff)
+                 return 0;
+             val |= (parts[0] << 24) | (parts[1] << 16);
+             break;
+
+         case 4:         /* a.b.c.d -- 8.8.8.8 bits */
+             if (val > 0xff)
+                 return 0;
+             val |= (parts[0] << 24) | (parts[1] << 16) | (parts[2] << 8);
+             break;
+     }
+
+     *addr = htonl(val);
+     return 1;
+
+}
+
+/*
+    ========================================================================
+
+    Routine Description:
+        Find key section for Get key parameter.
+
+    Arguments:
+        buffer                      Pointer to the buffer to start find the key section
+        section                     the key of the secion to be find
+
+    Return Value:
+        NULL                        Fail
+        Others                      Success
+    ========================================================================
+*/
+PUCHAR  RTMPFindSection(
+    IN  PCHAR   buffer)
+{
+    CHAR temp_buf[32];
+    PUCHAR  ptr;
+
+    strcpy(temp_buf, "Default");
+
+    if((ptr = rtstrstr(buffer, temp_buf)) != NULL)
+            return (ptr+strlen("\n"));
+        else
+            return NULL;
+}
+
+/*
+    ========================================================================
+
+    Routine Description:
+        Get key parameter.
+
+    Arguments:
+        key                         Pointer to key string
+        dest                        Pointer to destination
+        destsize                    The datasize of the destination
+        buffer                      Pointer to the buffer to start find the key
+
+    Return Value:
+        TRUE                        Success
+        FALSE                       Fail
+
+    Note:
+        This routine get the value with the matched key (case case-sensitive)
+    ========================================================================
+*/
+INT RTMPGetKeyParameter(
+    IN  PCHAR   key,
+    OUT PCHAR   dest,
+    IN  INT     destsize,
+    IN  PCHAR   buffer)
+{
+    UCHAR *temp_buf1 = NULL;
+    UCHAR *temp_buf2 = NULL;
+    CHAR *start_ptr;
+    CHAR *end_ptr;
+    CHAR *ptr;
+    CHAR *offset = 0;
+    INT  len;
+
+	//temp_buf1 = kmalloc(MAX_PARAM_BUFFER_SIZE, MEM_ALLOC_FLAG);
+	os_alloc_mem(NULL, &temp_buf1, MAX_PARAM_BUFFER_SIZE);
+
+	if(temp_buf1 == NULL)
+        return (FALSE);
+
+	//temp_buf2 = kmalloc(MAX_PARAM_BUFFER_SIZE, MEM_ALLOC_FLAG);
+	os_alloc_mem(NULL, &temp_buf2, MAX_PARAM_BUFFER_SIZE);
+	if(temp_buf2 == NULL)
+	{
+		os_free_mem(NULL, temp_buf1);
+        return (FALSE);
+	}
+
+    //find section
+    if((offset = RTMPFindSection(buffer)) == NULL)
+    {
+    	os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    strcpy(temp_buf1, "\n");
+    strcat(temp_buf1, key);
+    strcat(temp_buf1, "=");
+
+    //search key
+    if((start_ptr=rtstrstr(offset, temp_buf1))==NULL)
+    {
+		os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    start_ptr+=strlen("\n");
+    if((end_ptr=rtstrstr(start_ptr, "\n"))==NULL)
+       end_ptr=start_ptr+strlen(start_ptr);
+
+    if (end_ptr<start_ptr)
+    {
+		os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    NdisMoveMemory(temp_buf2, start_ptr, end_ptr-start_ptr);
+    temp_buf2[end_ptr-start_ptr]='\0';
+    len = strlen(temp_buf2);
+    strcpy(temp_buf1, temp_buf2);
+    if((start_ptr=rtstrstr(temp_buf1, "=")) == NULL)
+    {
+		os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    strcpy(temp_buf2, start_ptr+1);
+    ptr = temp_buf2;
+    //trim space or tab
+    while(*ptr != 0x00)
+    {
+        if( (*ptr == ' ') || (*ptr == '\t') )
+            ptr++;
+        else
+           break;
+    }
+
+    len = strlen(ptr);
+    memset(dest, 0x00, destsize);
+    strncpy(dest, ptr, len >= destsize ?  destsize: len);
+
+	os_free_mem(NULL, temp_buf1);
+    os_free_mem(NULL, temp_buf2);
+    return TRUE;
+}
+
+/*
+    ========================================================================
+
+    Routine Description:
+        Get key parameter.
+
+    Arguments:
+        key                         Pointer to key string
+        dest                        Pointer to destination
+        destsize                    The datasize of the destination
+        buffer                      Pointer to the buffer to start find the key
+
+    Return Value:
+        TRUE                        Success
+        FALSE                       Fail
+
+    Note:
+        This routine get the value with the matched key (case case-sensitive).
+        It is called for parsing SSID and any key string.
+    ========================================================================
+*/
+INT RTMPGetCriticalParameter(
+    IN  PCHAR   key,
+    OUT PCHAR   dest,
+    IN  INT     destsize,
+    IN  PCHAR   buffer)
+{
+    UCHAR *temp_buf1 = NULL;
+    UCHAR *temp_buf2 = NULL;
+    CHAR *start_ptr;
+    CHAR *end_ptr;
+    CHAR *ptr;
+    CHAR *offset = 0;
+    INT  len;
+
+	//temp_buf1 = kmalloc(MAX_PARAM_BUFFER_SIZE, MEM_ALLOC_FLAG);
+	os_alloc_mem(NULL, &temp_buf1, MAX_PARAM_BUFFER_SIZE);
+
+	if(temp_buf1 == NULL)
+        return (FALSE);
+
+	//temp_buf2 = kmalloc(MAX_PARAM_BUFFER_SIZE, MEM_ALLOC_FLAG);
+	os_alloc_mem(NULL, &temp_buf2, MAX_PARAM_BUFFER_SIZE);
+	if(temp_buf2 == NULL)
+	{
+		os_free_mem(NULL, temp_buf1);
+        return (FALSE);
+	}
+
+    //find section
+    if((offset = RTMPFindSection(buffer)) == NULL)
+    {
+    	os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    strcpy(temp_buf1, "\n");
+    strcat(temp_buf1, key);
+    strcat(temp_buf1, "=");
+
+    //search key
+    if((start_ptr=rtstrstr(offset, temp_buf1))==NULL)
+    {
+		os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    start_ptr+=strlen("\n");
+    if((end_ptr=rtstrstr(start_ptr, "\n"))==NULL)
+       end_ptr=start_ptr+strlen(start_ptr);
+
+    if (end_ptr<start_ptr)
+    {
+		os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    NdisMoveMemory(temp_buf2, start_ptr, end_ptr-start_ptr);
+    temp_buf2[end_ptr-start_ptr]='\0';
+    len = strlen(temp_buf2);
+    strcpy(temp_buf1, temp_buf2);
+    if((start_ptr=rtstrstr(temp_buf1, "=")) == NULL)
+    {
+		os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    strcpy(temp_buf2, start_ptr+1);
+    ptr = temp_buf2;
+
+    //trim tab
+    /* We cannot trim space(' ') for SSID and key string. */
+    while(*ptr != 0x00)
+    {
+        //if( (*ptr == ' ') || (*ptr == '\t') )
+        if( (*ptr == '\t') )
+            ptr++;
+        else
+           break;
+    }
+
+    len = strlen(ptr);
+    memset(dest, 0x00, destsize);
+    strncpy(dest, ptr, len >= destsize ?  destsize: len);
+
+	os_free_mem(NULL, temp_buf1);
+    os_free_mem(NULL, temp_buf2);
+    return TRUE;
+}
+
+/*
+    ========================================================================
+
+    Routine Description:
+        Get multiple key parameter.
+
+    Arguments:
+        key                         Pointer to key string
+        dest                        Pointer to destination
+        destsize                    The datasize of the destination
+        buffer                      Pointer to the buffer to start find the key
+
+    Return Value:
+        TRUE                        Success
+        FALSE                       Fail
+
+    Note:
+        This routine get the value with the matched key (case case-sensitive)
+    ========================================================================
+*/
+INT RTMPGetKeyParameterWithOffset(
+    IN  PCHAR   key,
+    OUT PCHAR   dest,
+    OUT	USHORT	*end_offset,
+    IN  INT     destsize,
+    IN  PCHAR   buffer,
+    IN	BOOLEAN	bTrimSpace)
+{
+    UCHAR *temp_buf1 = NULL;
+    UCHAR *temp_buf2 = NULL;
+    CHAR *start_ptr;
+    CHAR *end_ptr;
+    CHAR *ptr;
+    CHAR *offset = 0;
+    INT  len;
+
+	if (*end_offset >= MAX_INI_BUFFER_SIZE)
+		return (FALSE);
+
+	os_alloc_mem(NULL, &temp_buf1, MAX_PARAM_BUFFER_SIZE);
+
+	if(temp_buf1 == NULL)
+        return (FALSE);
+
+	os_alloc_mem(NULL, &temp_buf2, MAX_PARAM_BUFFER_SIZE);
+	if(temp_buf2 == NULL)
+	{
+		os_free_mem(NULL, temp_buf1);
+        return (FALSE);
+	}
+
+    //find section
+	if(*end_offset == 0)
+    {
+		if ((offset = RTMPFindSection(buffer)) == NULL)
+		{
+			os_free_mem(NULL, temp_buf1);
+	    	os_free_mem(NULL, temp_buf2);
+    	    return (FALSE);
+		}
+    }
+	else
+		offset = buffer + (*end_offset);
+
+    strcpy(temp_buf1, "\n");
+    strcat(temp_buf1, key);
+    strcat(temp_buf1, "=");
+
+    //search key
+    if((start_ptr=rtstrstr(offset, temp_buf1))==NULL)
+    {
+		os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    start_ptr+=strlen("\n");
+    if((end_ptr=rtstrstr(start_ptr, "\n"))==NULL)
+       end_ptr=start_ptr+strlen(start_ptr);
+
+    if (end_ptr<start_ptr)
+    {
+		os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+	*end_offset = end_ptr - buffer;
+
+    NdisMoveMemory(temp_buf2, start_ptr, end_ptr-start_ptr);
+    temp_buf2[end_ptr-start_ptr]='\0';
+    len = strlen(temp_buf2);
+    strcpy(temp_buf1, temp_buf2);
+    if((start_ptr=rtstrstr(temp_buf1, "=")) == NULL)
+    {
+		os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    strcpy(temp_buf2, start_ptr+1);
+    ptr = temp_buf2;
+    //trim space or tab
+    while(*ptr != 0x00)
+    {
+        if((bTrimSpace && (*ptr == ' ')) || (*ptr == '\t') )
+            ptr++;
+        else
+           break;
+    }
+
+    len = strlen(ptr);
+    memset(dest, 0x00, destsize);
+    strncpy(dest, ptr, len >= destsize ?  destsize: len);
+
+	os_free_mem(NULL, temp_buf1);
+    os_free_mem(NULL, temp_buf2);
+    return TRUE;
+}
+
+
+static int rtmp_parse_key_buffer_from_file(IN  PRTMP_ADAPTER pAd,IN  char *buffer,IN  ULONG KeyType,IN  INT BSSIdx,IN  INT KeyIdx)
+{
+	PUCHAR		keybuff;
+	INT			i = BSSIdx, idx = KeyIdx;
+	ULONG		KeyLen;
+	UCHAR		CipherAlg = CIPHER_WEP64;
+
+	keybuff = buffer;
+	KeyLen = strlen(keybuff);
+
+	if (KeyType == 1)
+	{//Ascii
+		if( (KeyLen == 5) || (KeyLen == 13))
+		{
+			pAd->SharedKey[i][idx].KeyLen = KeyLen;
+			NdisMoveMemory(pAd->SharedKey[i][idx].Key, keybuff, KeyLen);
+			if (KeyLen == 5)
+				CipherAlg = CIPHER_WEP64;
+			else
+				CipherAlg = CIPHER_WEP128;
+			pAd->SharedKey[i][idx].CipherAlg = CipherAlg;
+
+			DBGPRINT(RT_DEBUG_TRACE, ("I/F(ra%d) Key%dStr=%s and type=%s\n", i, idx+1, keybuff, (KeyType == 0) ? "Hex":"Ascii"));
+			return 1;
+		}
+		else
+		{//Invalid key length
+			DBGPRINT(RT_DEBUG_ERROR, ("Key%dStr is Invalid key length! KeyLen = %ld!\n", idx+1, KeyLen));
+			return 0;
+		}
+	}
+	else
+	{//Hex type
+		if( (KeyLen == 10) || (KeyLen == 26))
+		{
+			pAd->SharedKey[i][idx].KeyLen = KeyLen / 2;
+			AtoH(keybuff, pAd->SharedKey[i][idx].Key, KeyLen / 2);
+			if (KeyLen == 10)
+				CipherAlg = CIPHER_WEP64;
+			else
+				CipherAlg = CIPHER_WEP128;
+			pAd->SharedKey[i][idx].CipherAlg = CipherAlg;
+
+			DBGPRINT(RT_DEBUG_TRACE, ("I/F(ra%d) Key%dStr=%s and type=%s\n", i, idx+1, keybuff, (KeyType == 0) ? "Hex":"Ascii"));
+			return 1;
+		}
+		else
+		{//Invalid key length
+			DBGPRINT(RT_DEBUG_ERROR, ("I/F(ra%d) Key%dStr is Invalid key length! KeyLen = %ld!\n", i, idx+1, KeyLen));
+			return 0;
+		}
+	}
+}
+static void rtmp_read_key_parms_from_file(IN  PRTMP_ADAPTER pAd, char *tmpbuf, char *buffer)
+{
+	char		tok_str[16];
+	PUCHAR		macptr;
+	INT			i = 0, idx;
+	ULONG		KeyType[MAX_MBSSID_NUM];
+	ULONG		KeyIdx;
+
+	NdisZeroMemory(KeyType, MAX_MBSSID_NUM);
+
+	//DefaultKeyID
+	if(RTMPGetKeyParameter("DefaultKeyID", tmpbuf, 25, buffer))
+	{
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			KeyIdx = simple_strtol(tmpbuf, 0, 10);
+			if((KeyIdx >= 1 ) && (KeyIdx <= 4))
+				pAd->StaCfg.DefaultKeyId = (UCHAR) (KeyIdx - 1);
+			else
+				pAd->StaCfg.DefaultKeyId = 0;
+
+			DBGPRINT(RT_DEBUG_TRACE, ("DefaultKeyID(0~3)=%d\n", pAd->StaCfg.DefaultKeyId));
+		}
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+
+	for (idx = 0; idx < 4; idx++)
+	{
+		sprintf(tok_str, "Key%dType", idx + 1);
+		//Key1Type
+		if (RTMPGetKeyParameter(tok_str, tmpbuf, 128, buffer))
+		{
+		    for (i = 0, macptr = rstrtok(tmpbuf,";"); macptr; macptr = rstrtok(NULL,";"), i++)
+		    {
+			    KeyType[i] = simple_strtol(macptr, 0, 10);
+		    }
+
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			{
+				sprintf(tok_str, "Key%dStr", idx + 1);
+				if (RTMPGetCriticalParameter(tok_str, tmpbuf, 128, buffer))
+				{
+					rtmp_parse_key_buffer_from_file(pAd, tmpbuf, KeyType[BSS0], BSS0, idx);
+				}
+			}
+#endif // CONFIG_STA_SUPPORT //
+		}
+	}
+}
+
+
+#ifdef CONFIG_STA_SUPPORT
+static void rtmp_read_sta_wmm_parms_from_file(IN  PRTMP_ADAPTER pAd, char *tmpbuf, char *buffer)
+{
+	PUCHAR					macptr;
+	INT						i=0;
+	BOOLEAN					bWmmEnable = FALSE;
+
+	//WmmCapable
+	if(RTMPGetKeyParameter("WmmCapable", tmpbuf, 32, buffer))
+	{
+		if(simple_strtol(tmpbuf, 0, 10) != 0) //Enable
+		{
+			pAd->CommonCfg.bWmmCapable = TRUE;
+			bWmmEnable = TRUE;
+		}
+		else //Disable
+		{
+			pAd->CommonCfg.bWmmCapable = FALSE;
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("WmmCapable=%d\n", pAd->CommonCfg.bWmmCapable));
+	}
+
+#ifdef QOS_DLS_SUPPORT
+	//DLSCapable
+	if(RTMPGetKeyParameter("DLSCapable", tmpbuf, 32, buffer))
+	{
+		if(simple_strtol(tmpbuf, 0, 10) != 0)  //Enable
+		{
+			pAd->CommonCfg.bDLSCapable = TRUE;
+		}
+		else //Disable
+		{
+			pAd->CommonCfg.bDLSCapable = FALSE;
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("bDLSCapable=%d\n", pAd->CommonCfg.bDLSCapable));
+	}
+#endif // QOS_DLS_SUPPORT //
+
+	//AckPolicy for AC_BK, AC_BE, AC_VI, AC_VO
+	if(RTMPGetKeyParameter("AckPolicy", tmpbuf, 32, buffer))
+	{
+		for (i = 0, macptr = rstrtok(tmpbuf,";"); macptr; macptr = rstrtok(NULL,";"), i++)
+		{
+			pAd->CommonCfg.AckPolicy[i] = (UCHAR)simple_strtol(macptr, 0, 10);
+
+			DBGPRINT(RT_DEBUG_TRACE, ("AckPolicy[%d]=%d\n", i, pAd->CommonCfg.AckPolicy[i]));
+		}
+	}
+
+	if (bWmmEnable)
+	{
+		//APSDCapable
+		if(RTMPGetKeyParameter("APSDCapable", tmpbuf, 10, buffer))
+		{
+			if(simple_strtol(tmpbuf, 0, 10) != 0)  //Enable
+				pAd->CommonCfg.bAPSDCapable = TRUE;
+			else
+				pAd->CommonCfg.bAPSDCapable = FALSE;
+
+			DBGPRINT(RT_DEBUG_TRACE, ("APSDCapable=%d\n", pAd->CommonCfg.bAPSDCapable));
+		}
+
+		//APSDAC for AC_BE, AC_BK, AC_VI, AC_VO
+		if(RTMPGetKeyParameter("APSDAC", tmpbuf, 32, buffer))
+		{
+			BOOLEAN apsd_ac[4];
+
+			for (i = 0, macptr = rstrtok(tmpbuf,";"); macptr; macptr = rstrtok(NULL,";"), i++)
+			{
+				apsd_ac[i] = (BOOLEAN)simple_strtol(macptr, 0, 10);
+
+				DBGPRINT(RT_DEBUG_TRACE, ("APSDAC%d  %d\n", i,  apsd_ac[i]));
+			}
+
+			pAd->CommonCfg.bAPSDAC_BE = apsd_ac[0];
+			pAd->CommonCfg.bAPSDAC_BK = apsd_ac[1];
+			pAd->CommonCfg.bAPSDAC_VI = apsd_ac[2];
+			pAd->CommonCfg.bAPSDAC_VO = apsd_ac[3];
+		}
+	}
+
+}
+#endif // CONFIG_STA_SUPPORT //
+
+
+NDIS_STATUS	RTMPReadParametersHook(
+	IN	PRTMP_ADAPTER pAd)
+{
+	PUCHAR					src = NULL;
+	struct file				*srcf;
+	INT 					retval, orgfsuid, orgfsgid;
+   	mm_segment_t			orgfs;
+	CHAR					*buffer;
+	CHAR					*tmpbuf;
+	ULONG					RtsThresh;
+	ULONG					FragThresh;
+#ifdef CONFIG_STA_SUPPORT
+	UCHAR	                keyMaterial[40];
+#endif // CONFIG_STA_SUPPORT //
+
+
+	PUCHAR					macptr;
+	INT						i = 0;
+
+	buffer = kmalloc(MAX_INI_BUFFER_SIZE, MEM_ALLOC_FLAG);
+	if(buffer == NULL)
+        return NDIS_STATUS_FAILURE;
+
+	tmpbuf = kmalloc(MAX_PARAM_BUFFER_SIZE, MEM_ALLOC_FLAG);
+	if(tmpbuf == NULL)
+	{
+		kfree(buffer);
+        return NDIS_STATUS_FAILURE;
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		src = STA_PROFILE_PATH;
+#endif // CONFIG_STA_SUPPORT //
+#ifdef MULTIPLE_CARD_SUPPORT
+	src = pAd->MC_FileName;
+#endif // MULTIPLE_CARD_SUPPORT //
+
+	// Save uid and gid used for filesystem access.
+	// Set user and group to 0 (root)
+	orgfsuid = current_fsuid();
+	orgfsgid = current_fsgid();
+	/* Hm, can't really do this nicely anymore, so rely on these files
+	 * being set to the proper permission to read them... */
+	/* current->cred->fsuid = current->cred->fsgid = 0; */
+    orgfs = get_fs();
+    set_fs(KERNEL_DS);
+
+	if (src && *src)
+	{
+		srcf = filp_open(src, O_RDONLY, 0);
+		if (IS_ERR(srcf))
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("--> Error %ld opening %s\n", -PTR_ERR(srcf),src));
+		}
+		else
+		{
+			// The object must have a read method
+			if (srcf->f_op && srcf->f_op->read)
+			{
+				memset(buffer, 0x00, MAX_INI_BUFFER_SIZE);
+				retval=srcf->f_op->read(srcf, buffer, MAX_INI_BUFFER_SIZE, &srcf->f_pos);
+				if (retval < 0)
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("--> Read %s error %d\n", src, -retval));
+				}
+				else
+				{
+					// set file parameter to portcfg
+					//CountryRegion
+					if(RTMPGetKeyParameter("CountryRegion", tmpbuf, 25, buffer))
+					{
+						pAd->CommonCfg.CountryRegion = (UCHAR) simple_strtol(tmpbuf, 0, 10);
+						DBGPRINT(RT_DEBUG_TRACE, ("CountryRegion=%d\n", pAd->CommonCfg.CountryRegion));
+					}
+					//CountryRegionABand
+					if(RTMPGetKeyParameter("CountryRegionABand", tmpbuf, 25, buffer))
+					{
+						pAd->CommonCfg.CountryRegionForABand= (UCHAR) simple_strtol(tmpbuf, 0, 10);
+						DBGPRINT(RT_DEBUG_TRACE, ("CountryRegionABand=%d\n", pAd->CommonCfg.CountryRegionForABand));
+					}
+					//CountryCode
+					if(RTMPGetKeyParameter("CountryCode", tmpbuf, 25, buffer))
+					{
+						NdisMoveMemory(pAd->CommonCfg.CountryCode, tmpbuf , 2);
+#ifdef CONFIG_STA_SUPPORT
+#ifdef EXT_BUILD_CHANNEL_LIST
+						IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+							NdisMoveMemory(pAd->StaCfg.StaOriCountryCode, tmpbuf , 2);
+#endif // EXT_BUILD_CHANNEL_LIST //
+#endif // CONFIG_STA_SUPPORT //
+						if (strlen(pAd->CommonCfg.CountryCode) != 0)
+						{
+							pAd->CommonCfg.bCountryFlag = TRUE;
+						}
+						DBGPRINT(RT_DEBUG_TRACE, ("CountryCode=%s\n", pAd->CommonCfg.CountryCode));
+					}
+					//ChannelGeography
+					if(RTMPGetKeyParameter("ChannelGeography", tmpbuf, 25, buffer))
+					{
+						UCHAR Geography = (UCHAR) simple_strtol(tmpbuf, 0, 10);
+						if (Geography <= BOTH)
+						{
+							pAd->CommonCfg.Geography = Geography;
+							pAd->CommonCfg.CountryCode[2] =
+								(pAd->CommonCfg.Geography == BOTH) ? ' ' : ((pAd->CommonCfg.Geography == IDOR) ? 'I' : 'O');
+#ifdef CONFIG_STA_SUPPORT
+#ifdef EXT_BUILD_CHANNEL_LIST
+						IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+							pAd->StaCfg.StaOriGeography = pAd->CommonCfg.Geography;
+#endif // EXT_BUILD_CHANNEL_LIST //
+#endif // CONFIG_STA_SUPPORT //
+							DBGPRINT(RT_DEBUG_TRACE, ("ChannelGeography=%d\n", pAd->CommonCfg.Geography));
+						}
+					}
+					else
+					{
+						pAd->CommonCfg.Geography = BOTH;
+						pAd->CommonCfg.CountryCode[2] = ' ';
+					}
+
+
+#ifdef CONFIG_STA_SUPPORT
+					IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+					{
+						//SSID
+						if (RTMPGetCriticalParameter("SSID", tmpbuf, 256, buffer))
+						{
+							if (strlen(tmpbuf) <= 32)
+							{
+			 					pAd->CommonCfg.SsidLen = (UCHAR) strlen(tmpbuf);
+								NdisZeroMemory(pAd->CommonCfg.Ssid, NDIS_802_11_LENGTH_SSID);
+								NdisMoveMemory(pAd->CommonCfg.Ssid, tmpbuf, pAd->CommonCfg.SsidLen);
+								pAd->MlmeAux.AutoReconnectSsidLen = pAd->CommonCfg.SsidLen;
+								NdisZeroMemory(pAd->MlmeAux.AutoReconnectSsid, NDIS_802_11_LENGTH_SSID);
+								NdisMoveMemory(pAd->MlmeAux.AutoReconnectSsid, tmpbuf, pAd->MlmeAux.AutoReconnectSsidLen);
+								pAd->MlmeAux.SsidLen = pAd->CommonCfg.SsidLen;
+								NdisZeroMemory(pAd->MlmeAux.Ssid, NDIS_802_11_LENGTH_SSID);
+								NdisMoveMemory(pAd->MlmeAux.Ssid, tmpbuf, pAd->MlmeAux.SsidLen);
+								DBGPRINT(RT_DEBUG_TRACE, ("%s::(SSID=%s)\n", __FUNCTION__, tmpbuf));
+							}
+						}
+					}
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+					IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+					{
+						//NetworkType
+						if (RTMPGetKeyParameter("NetworkType", tmpbuf, 25, buffer))
+						{
+							pAd->bConfigChanged = TRUE;
+							if (strcmp(tmpbuf, "Adhoc") == 0)
+								pAd->StaCfg.BssType = BSS_ADHOC;
+							else //Default Infrastructure mode
+								pAd->StaCfg.BssType = BSS_INFRA;
+							// Reset Ralink supplicant to not use, it will be set to start when UI set PMK key
+							pAd->StaCfg.WpaState = SS_NOTUSE;
+							DBGPRINT(RT_DEBUG_TRACE, ("%s::(NetworkType=%d)\n", __FUNCTION__, pAd->StaCfg.BssType));
+						}
+					}
+#endif // CONFIG_STA_SUPPORT //
+					//Channel
+					if(RTMPGetKeyParameter("Channel", tmpbuf, 10, buffer))
+					{
+						pAd->CommonCfg.Channel = (UCHAR) simple_strtol(tmpbuf, 0, 10);
+						DBGPRINT(RT_DEBUG_TRACE, ("Channel=%d\n", pAd->CommonCfg.Channel));
+					}
+					//WirelessMode
+					if(RTMPGetKeyParameter("WirelessMode", tmpbuf, 10, buffer))
+					{
+						int value  = 0, maxPhyMode = PHY_11G;
+
+#ifdef DOT11_N_SUPPORT
+						maxPhyMode = PHY_11N_5G;
+#endif // DOT11_N_SUPPORT //
+
+						value = simple_strtol(tmpbuf, 0, 10);
+
+						if (value <= maxPhyMode)
+						{
+							pAd->CommonCfg.PhyMode = value;
+						}
+						DBGPRINT(RT_DEBUG_TRACE, ("PhyMode=%d\n", pAd->CommonCfg.PhyMode));
+					}
+                    //BasicRate
+					if(RTMPGetKeyParameter("BasicRate", tmpbuf, 10, buffer))
+					{
+						pAd->CommonCfg.BasicRateBitmap = (ULONG) simple_strtol(tmpbuf, 0, 10);
+						DBGPRINT(RT_DEBUG_TRACE, ("BasicRate=%ld\n", pAd->CommonCfg.BasicRateBitmap));
+					}
+					//BeaconPeriod
+					if(RTMPGetKeyParameter("BeaconPeriod", tmpbuf, 10, buffer))
+					{
+						pAd->CommonCfg.BeaconPeriod = (USHORT) simple_strtol(tmpbuf, 0, 10);
+						DBGPRINT(RT_DEBUG_TRACE, ("BeaconPeriod=%d\n", pAd->CommonCfg.BeaconPeriod));
+					}
+                    //TxPower
+					if(RTMPGetKeyParameter("TxPower", tmpbuf, 10, buffer))
+					{
+						pAd->CommonCfg.TxPowerPercentage = (ULONG) simple_strtol(tmpbuf, 0, 10);
+#ifdef CONFIG_STA_SUPPORT
+						IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+							pAd->CommonCfg.TxPowerDefault = pAd->CommonCfg.TxPowerPercentage;
+#endif // CONFIG_STA_SUPPORT //
+						DBGPRINT(RT_DEBUG_TRACE, ("TxPower=%ld\n", pAd->CommonCfg.TxPowerPercentage));
+					}
+					//BGProtection
+					if(RTMPGetKeyParameter("BGProtection", tmpbuf, 10, buffer))
+					{
+						switch (simple_strtol(tmpbuf, 0, 10))
+						{
+							case 1: //Always On
+								pAd->CommonCfg.UseBGProtection = 1;
+								break;
+							case 2: //Always OFF
+								pAd->CommonCfg.UseBGProtection = 2;
+								break;
+							case 0: //AUTO
+							default:
+								pAd->CommonCfg.UseBGProtection = 0;
+								break;
+						}
+						DBGPRINT(RT_DEBUG_TRACE, ("BGProtection=%ld\n", pAd->CommonCfg.UseBGProtection));
+					}
+					//OLBCDetection
+					if(RTMPGetKeyParameter("DisableOLBC", tmpbuf, 10, buffer))
+					{
+						switch (simple_strtol(tmpbuf, 0, 10))
+						{
+							case 1: //disable OLBC Detection
+								pAd->CommonCfg.DisableOLBCDetect = 1;
+								break;
+							case 0: //enable OLBC Detection
+								pAd->CommonCfg.DisableOLBCDetect = 0;
+								break;
+							default:
+								pAd->CommonCfg.DisableOLBCDetect= 0;
+								break;
+						}
+						DBGPRINT(RT_DEBUG_TRACE, ("OLBCDetection=%ld\n", pAd->CommonCfg.DisableOLBCDetect));
+					}
+					//TxPreamble
+					if(RTMPGetKeyParameter("TxPreamble", tmpbuf, 10, buffer))
+					{
+						switch (simple_strtol(tmpbuf, 0, 10))
+						{
+							case Rt802_11PreambleShort:
+								pAd->CommonCfg.TxPreamble = Rt802_11PreambleShort;
+								break;
+							case Rt802_11PreambleLong:
+							default:
+								pAd->CommonCfg.TxPreamble = Rt802_11PreambleLong;
+								break;
+						}
+						DBGPRINT(RT_DEBUG_TRACE, ("TxPreamble=%ld\n", pAd->CommonCfg.TxPreamble));
+					}
+					//RTSThreshold
+					if(RTMPGetKeyParameter("RTSThreshold", tmpbuf, 10, buffer))
+					{
+						RtsThresh = simple_strtol(tmpbuf, 0, 10);
+						if( (RtsThresh >= 1) && (RtsThresh <= MAX_RTS_THRESHOLD) )
+							pAd->CommonCfg.RtsThreshold  = (USHORT)RtsThresh;
+						else
+							pAd->CommonCfg.RtsThreshold = MAX_RTS_THRESHOLD;
+
+						DBGPRINT(RT_DEBUG_TRACE, ("RTSThreshold=%d\n", pAd->CommonCfg.RtsThreshold));
+					}
+					//FragThreshold
+					if(RTMPGetKeyParameter("FragThreshold", tmpbuf, 10, buffer))
+					{
+						FragThresh = simple_strtol(tmpbuf, 0, 10);
+						pAd->CommonCfg.bUseZeroToDisableFragment = FALSE;
+
+						if (FragThresh > MAX_FRAG_THRESHOLD || FragThresh < MIN_FRAG_THRESHOLD)
+						{ //illegal FragThresh so we set it to default
+							pAd->CommonCfg.FragmentThreshold = MAX_FRAG_THRESHOLD;
+							pAd->CommonCfg.bUseZeroToDisableFragment = TRUE;
+						}
+						else if (FragThresh % 2 == 1)
+						{
+							// The length of each fragment shall always be an even number of octets, except for the last fragment
+							// of an MSDU or MMPDU, which may be either an even or an odd number of octets.
+							pAd->CommonCfg.FragmentThreshold = (USHORT)(FragThresh - 1);
+						}
+						else
+						{
+							pAd->CommonCfg.FragmentThreshold = (USHORT)FragThresh;
+						}
+						//pAd->CommonCfg.AllowFragSize = (pAd->CommonCfg.FragmentThreshold) - LENGTH_802_11 - LENGTH_CRC;
+						DBGPRINT(RT_DEBUG_TRACE, ("FragThreshold=%d\n", pAd->CommonCfg.FragmentThreshold));
+					}
+					//TxBurst
+					if(RTMPGetKeyParameter("TxBurst", tmpbuf, 10, buffer))
+					{
+						if(simple_strtol(tmpbuf, 0, 10) != 0)  //Enable
+							pAd->CommonCfg.bEnableTxBurst = TRUE;
+						else //Disable
+							pAd->CommonCfg.bEnableTxBurst = FALSE;
+						DBGPRINT(RT_DEBUG_TRACE, ("TxBurst=%d\n", pAd->CommonCfg.bEnableTxBurst));
+					}
+
+#ifdef AGGREGATION_SUPPORT
+					//PktAggregate
+					if(RTMPGetKeyParameter("PktAggregate", tmpbuf, 10, buffer))
+					{
+						if(simple_strtol(tmpbuf, 0, 10) != 0)  //Enable
+							pAd->CommonCfg.bAggregationCapable = TRUE;
+						else //Disable
+							pAd->CommonCfg.bAggregationCapable = FALSE;
+#ifdef PIGGYBACK_SUPPORT
+						pAd->CommonCfg.bPiggyBackCapable = pAd->CommonCfg.bAggregationCapable;
+#endif // PIGGYBACK_SUPPORT //
+						DBGPRINT(RT_DEBUG_TRACE, ("PktAggregate=%d\n", pAd->CommonCfg.bAggregationCapable));
+					}
+#else
+					pAd->CommonCfg.bAggregationCapable = FALSE;
+					pAd->CommonCfg.bPiggyBackCapable = FALSE;
+#endif // AGGREGATION_SUPPORT //
+
+					// WmmCapable
+
+#ifdef CONFIG_STA_SUPPORT
+					IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+						rtmp_read_sta_wmm_parms_from_file(pAd, tmpbuf, buffer);
+#endif // CONFIG_STA_SUPPORT //
+
+					//ShortSlot
+					if(RTMPGetKeyParameter("ShortSlot", tmpbuf, 10, buffer))
+					{
+						if(simple_strtol(tmpbuf, 0, 10) != 0)  //Enable
+							pAd->CommonCfg.bUseShortSlotTime = TRUE;
+						else //Disable
+							pAd->CommonCfg.bUseShortSlotTime = FALSE;
+
+						DBGPRINT(RT_DEBUG_TRACE, ("ShortSlot=%d\n", pAd->CommonCfg.bUseShortSlotTime));
+					}
+					//IEEE80211H
+					if(RTMPGetKeyParameter("IEEE80211H", tmpbuf, 10, buffer))
+					{
+					    for (i = 0, macptr = rstrtok(tmpbuf,";"); macptr; macptr = rstrtok(NULL,";"), i++)
+					    {
+    						if(simple_strtol(macptr, 0, 10) != 0)  //Enable
+    							pAd->CommonCfg.bIEEE80211H = TRUE;
+    						else //Disable
+    							pAd->CommonCfg.bIEEE80211H = FALSE;
+
+    						DBGPRINT(RT_DEBUG_TRACE, ("IEEE80211H=%d\n", pAd->CommonCfg.bIEEE80211H));
+					    }
+					}
+					//CSPeriod
+					if(RTMPGetKeyParameter("CSPeriod", tmpbuf, 10, buffer))
+					{
+					    if(simple_strtol(tmpbuf, 0, 10) != 0)
+							pAd->CommonCfg.RadarDetect.CSPeriod = simple_strtol(tmpbuf, 0, 10);
+						else
+							pAd->CommonCfg.RadarDetect.CSPeriod = 0;
+
+   						DBGPRINT(RT_DEBUG_TRACE, ("CSPeriod=%d\n", pAd->CommonCfg.RadarDetect.CSPeriod));
+					}
+
+					//RDRegion
+					if(RTMPGetKeyParameter("RDRegion", tmpbuf, 128, buffer))
+					{
+						if ((strncmp(tmpbuf, "JAP_W53", 7) == 0) || (strncmp(tmpbuf, "jap_w53", 7) == 0))
+						{
+							pAd->CommonCfg.RadarDetect.RDDurRegion = JAP_W53;
+							pAd->CommonCfg.RadarDetect.DfsSessionTime = 15;
+						}
+						else if ((strncmp(tmpbuf, "JAP_W56", 7) == 0) || (strncmp(tmpbuf, "jap_w56", 7) == 0))
+						{
+							pAd->CommonCfg.RadarDetect.RDDurRegion = JAP_W56;
+							pAd->CommonCfg.RadarDetect.DfsSessionTime = 13;
+						}
+						else if ((strncmp(tmpbuf, "JAP", 3) == 0) || (strncmp(tmpbuf, "jap", 3) == 0))
+						{
+							pAd->CommonCfg.RadarDetect.RDDurRegion = JAP;
+							pAd->CommonCfg.RadarDetect.DfsSessionTime = 5;
+						}
+						else  if ((strncmp(tmpbuf, "FCC", 3) == 0) || (strncmp(tmpbuf, "fcc", 3) == 0))
+						{
+							pAd->CommonCfg.RadarDetect.RDDurRegion = FCC;
+							pAd->CommonCfg.RadarDetect.DfsSessionTime = 5;
+						}
+						else if ((strncmp(tmpbuf, "CE", 2) == 0) || (strncmp(tmpbuf, "ce", 2) == 0))
+						{
+							pAd->CommonCfg.RadarDetect.RDDurRegion = CE;
+							pAd->CommonCfg.RadarDetect.DfsSessionTime = 13;
+						}
+						else
+						{
+							pAd->CommonCfg.RadarDetect.RDDurRegion = CE;
+							pAd->CommonCfg.RadarDetect.DfsSessionTime = 13;
+						}
+
+						DBGPRINT(RT_DEBUG_TRACE, ("RDRegion=%d\n", pAd->CommonCfg.RadarDetect.RDDurRegion));
+					}
+					else
+					{
+						pAd->CommonCfg.RadarDetect.RDDurRegion = CE;
+						pAd->CommonCfg.RadarDetect.DfsSessionTime = 13;
+					}
+
+					//WirelessEvent
+					if(RTMPGetKeyParameter("WirelessEvent", tmpbuf, 10, buffer))
+					{
+#if WIRELESS_EXT >= 15
+					    if(simple_strtol(tmpbuf, 0, 10) != 0)
+							pAd->CommonCfg.bWirelessEvent = simple_strtol(tmpbuf, 0, 10);
+						else
+							pAd->CommonCfg.bWirelessEvent = 0;	// disable
+#else
+						pAd->CommonCfg.bWirelessEvent = 0;	// disable
+#endif
+   						DBGPRINT(RT_DEBUG_TRACE, ("WirelessEvent=%d\n", pAd->CommonCfg.bWirelessEvent));
+					}
+					if(RTMPGetKeyParameter("WiFiTest", tmpbuf, 10, buffer))
+					{
+					    if(simple_strtol(tmpbuf, 0, 10) != 0)
+							pAd->CommonCfg.bWiFiTest= simple_strtol(tmpbuf, 0, 10);
+						else
+							pAd->CommonCfg.bWiFiTest = 0;	// disable
+
+   						DBGPRINT(RT_DEBUG_TRACE, ("WiFiTest=%d\n", pAd->CommonCfg.bWiFiTest));
+					}
+					//AuthMode
+					if(RTMPGetKeyParameter("AuthMode", tmpbuf, 128, buffer))
+					{
+#ifdef CONFIG_STA_SUPPORT
+						IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+						{
+							if ((strcmp(tmpbuf, "WEPAUTO") == 0) || (strcmp(tmpbuf, "wepauto") == 0))
+							    pAd->StaCfg.AuthMode = Ndis802_11AuthModeAutoSwitch;
+							else if ((strcmp(tmpbuf, "SHARED") == 0) || (strcmp(tmpbuf, "shared") == 0))
+							    pAd->StaCfg.AuthMode = Ndis802_11AuthModeShared;
+							else if ((strcmp(tmpbuf, "WPAPSK") == 0) || (strcmp(tmpbuf, "wpapsk") == 0))
+							    pAd->StaCfg.AuthMode = Ndis802_11AuthModeWPAPSK;
+							else if ((strcmp(tmpbuf, "WPANONE") == 0) || (strcmp(tmpbuf, "wpanone") == 0))
+							    pAd->StaCfg.AuthMode = Ndis802_11AuthModeWPANone;
+							else if ((strcmp(tmpbuf, "WPA2PSK") == 0) || (strcmp(tmpbuf, "wpa2psk") == 0))
+							    pAd->StaCfg.AuthMode = Ndis802_11AuthModeWPA2PSK;
+#ifdef WPA_SUPPLICANT_SUPPORT
+							else if ((strcmp(tmpbuf, "WPA") == 0) || (strcmp(tmpbuf, "wpa") == 0))
+							    pAd->StaCfg.AuthMode = Ndis802_11AuthModeWPA;
+							else if ((strcmp(tmpbuf, "WPA2") == 0) || (strcmp(tmpbuf, "wpa2") == 0))
+							    pAd->StaCfg.AuthMode = Ndis802_11AuthModeWPA2;
+#endif // WPA_SUPPLICANT_SUPPORT //
+				                        else
+				                            pAd->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+
+				                        pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+
+							DBGPRINT(RT_DEBUG_TRACE, ("%s::(EncrypType=%d)\n", __FUNCTION__, pAd->StaCfg.WepStatus));
+						}
+#endif // CONFIG_STA_SUPPORT //
+					}
+					//EncrypType
+					if(RTMPGetKeyParameter("EncrypType", tmpbuf, 128, buffer))
+					{
+
+#ifdef CONFIG_STA_SUPPORT
+						IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+						{
+							if ((strcmp(tmpbuf, "WEP") == 0) || (strcmp(tmpbuf, "wep") == 0))
+								pAd->StaCfg.WepStatus	= Ndis802_11WEPEnabled;
+							else if ((strcmp(tmpbuf, "TKIP") == 0) || (strcmp(tmpbuf, "tkip") == 0))
+								pAd->StaCfg.WepStatus	= Ndis802_11Encryption2Enabled;
+							else if ((strcmp(tmpbuf, "AES") == 0) || (strcmp(tmpbuf, "aes") == 0))
+								pAd->StaCfg.WepStatus	= Ndis802_11Encryption3Enabled;
+							else
+								pAd->StaCfg.WepStatus	= Ndis802_11WEPDisabled;
+
+							// Update all wepstatus related
+							pAd->StaCfg.PairCipher		= pAd->StaCfg.WepStatus;
+							pAd->StaCfg.GroupCipher 	= pAd->StaCfg.WepStatus;
+							pAd->StaCfg.OrigWepStatus 	= pAd->StaCfg.WepStatus;
+							pAd->StaCfg.bMixCipher 		= FALSE;
+
+							DBGPRINT(RT_DEBUG_TRACE, ("%s::(EncrypType=%d)\n", __FUNCTION__, pAd->StaCfg.WepStatus));
+						}
+#endif // CONFIG_STA_SUPPORT //
+					}
+
+
+
+#ifdef CONFIG_STA_SUPPORT
+					IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+					{
+						if(RTMPGetCriticalParameter("WPAPSK", tmpbuf, 512, buffer))
+						{
+							int     err=0;
+
+							tmpbuf[strlen(tmpbuf)] = '\0'; // make STA can process .$^& for WPAPSK input
+
+							if ((pAd->StaCfg.AuthMode != Ndis802_11AuthModeWPAPSK) &&
+								(pAd->StaCfg.AuthMode != Ndis802_11AuthModeWPA2PSK) &&
+								(pAd->StaCfg.AuthMode != Ndis802_11AuthModeWPANone)
+								)
+							{
+								err = 1;
+							}
+							else if ((strlen(tmpbuf) >= 8) && (strlen(tmpbuf) < 64))
+							{
+								PasswordHash((char *)tmpbuf, pAd->CommonCfg.Ssid, pAd->CommonCfg.SsidLen, keyMaterial);
+								NdisMoveMemory(pAd->StaCfg.PMK, keyMaterial, 32);
+
+							}
+							else if (strlen(tmpbuf) == 64)
+							{
+								AtoH(tmpbuf, keyMaterial, 32);
+								NdisMoveMemory(pAd->StaCfg.PMK, keyMaterial, 32);
+							}
+							else
+							{
+								err = 1;
+								DBGPRINT(RT_DEBUG_ERROR, ("%s::(WPAPSK key-string required 8 ~ 64 characters!)\n", __FUNCTION__));
+							}
+
+							if (err == 0)
+	                        			{
+	                        				if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+									(pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK))
+								{
+									// Start STA supplicant state machine
+									pAd->StaCfg.WpaState = SS_START;
+								}
+								else if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+								{
+									pAd->StaCfg.WpaState = SS_NOTUSE;
+								}
+
+								DBGPRINT(RT_DEBUG_TRACE, ("%s::(WPAPSK=%s)\n", __FUNCTION__, tmpbuf));
+							}
+						}
+					}
+#endif // CONFIG_STA_SUPPORT //
+
+					//DefaultKeyID, KeyType, KeyStr
+					rtmp_read_key_parms_from_file(pAd, tmpbuf, buffer);
+
+#ifdef DOT11_N_SUPPORT
+					HTParametersHook(pAd, tmpbuf, buffer);
+#endif // DOT11_N_SUPPORT //
+
+
+#ifdef CARRIER_DETECTION_SUPPORT
+						//CarrierDetect
+						if(RTMPGetKeyParameter("CarrierDetect", tmpbuf, 128, buffer))
+						{
+							if ((strncmp(tmpbuf, "0", 1) == 0))
+								pAd->CommonCfg.CarrierDetect.Enable = FALSE;
+							else if ((strncmp(tmpbuf, "1", 1) == 0))
+								pAd->CommonCfg.CarrierDetect.Enable = TRUE;
+							else
+								pAd->CommonCfg.CarrierDetect.Enable = FALSE;
+
+							DBGPRINT(RT_DEBUG_TRACE, ("CarrierDetect.Enable=%d\n", pAd->CommonCfg.CarrierDetect.Enable));
+						}
+						else
+							pAd->CommonCfg.CarrierDetect.Enable = FALSE;
+#endif // CARRIER_DETECTION_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+					IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+					{
+						//PSMode
+						if (RTMPGetKeyParameter("PSMode", tmpbuf, 10, buffer))
+						{
+							if (pAd->StaCfg.BssType == BSS_INFRA)
+							{
+								if ((strcmp(tmpbuf, "MAX_PSP") == 0) || (strcmp(tmpbuf, "max_psp") == 0))
+								{
+									// do NOT turn on PSM bit here, wait until MlmeCheckForPsmChange()
+									// to exclude certain situations.
+									//	   MlmeSetPsm(pAd, PWR_SAVE);
+									OPSTATUS_SET_FLAG(pAd, fOP_STATUS_RECEIVE_DTIM);
+									if (pAd->StaCfg.bWindowsACCAMEnable == FALSE)
+										pAd->StaCfg.WindowsPowerMode = Ndis802_11PowerModeMAX_PSP;
+									pAd->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeMAX_PSP;
+									pAd->StaCfg.DefaultListenCount = 5;
+								}
+								else if ((strcmp(tmpbuf, "Fast_PSP") == 0) || (strcmp(tmpbuf, "fast_psp") == 0)
+									|| (strcmp(tmpbuf, "FAST_PSP") == 0))
+								{
+									// do NOT turn on PSM bit here, wait until MlmeCheckForPsmChange()
+									// to exclude certain situations.
+									//	   MlmeSetPsmBit(pAd, PWR_SAVE);
+									OPSTATUS_SET_FLAG(pAd, fOP_STATUS_RECEIVE_DTIM);
+									if (pAd->StaCfg.bWindowsACCAMEnable == FALSE)
+										pAd->StaCfg.WindowsPowerMode = Ndis802_11PowerModeFast_PSP;
+									pAd->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeFast_PSP;
+									pAd->StaCfg.DefaultListenCount = 3;
+								}
+								else if ((strcmp(tmpbuf, "Legacy_PSP") == 0) || (strcmp(tmpbuf, "legacy_psp") == 0)
+									|| (strcmp(tmpbuf, "LEGACY_PSP") == 0))
+								{
+									// do NOT turn on PSM bit here, wait until MlmeCheckForPsmChange()
+									// to exclude certain situations.
+									//	   MlmeSetPsmBit(pAd, PWR_SAVE);
+									OPSTATUS_SET_FLAG(pAd, fOP_STATUS_RECEIVE_DTIM);
+									if (pAd->StaCfg.bWindowsACCAMEnable == FALSE)
+										pAd->StaCfg.WindowsPowerMode = Ndis802_11PowerModeLegacy_PSP;
+									pAd->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeLegacy_PSP;
+									pAd->StaCfg.DefaultListenCount = 3;
+								}
+								else
+								{ //Default Ndis802_11PowerModeCAM
+									// clear PSM bit immediately
+									MlmeSetPsmBit(pAd, PWR_ACTIVE);
+									OPSTATUS_SET_FLAG(pAd, fOP_STATUS_RECEIVE_DTIM);
+									if (pAd->StaCfg.bWindowsACCAMEnable == FALSE)
+										pAd->StaCfg.WindowsPowerMode = Ndis802_11PowerModeCAM;
+									pAd->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeCAM;
+								}
+								DBGPRINT(RT_DEBUG_TRACE, ("PSMode=%ld\n", pAd->StaCfg.WindowsPowerMode));
+							}
+						}
+						// FastRoaming
+						if (RTMPGetKeyParameter("FastRoaming", tmpbuf, 32, buffer))
+						{
+							if (simple_strtol(tmpbuf, 0, 10) == 0)
+								pAd->StaCfg.bFastRoaming = FALSE;
+							else
+								pAd->StaCfg.bFastRoaming = TRUE;
+
+							DBGPRINT(RT_DEBUG_TRACE, ("FastRoaming=%d\n", pAd->StaCfg.bFastRoaming));
+						}
+						// RoamThreshold
+						if (RTMPGetKeyParameter("RoamThreshold", tmpbuf, 32, buffer))
+						{
+							long lInfo = simple_strtol(tmpbuf, 0, 10);
+
+							if (lInfo > 90 || lInfo < 60)
+								pAd->StaCfg.dBmToRoam = -70;
+							else
+								pAd->StaCfg.dBmToRoam = (CHAR)(-1)*lInfo;
+
+							DBGPRINT(RT_DEBUG_TRACE, ("RoamThreshold=%d  dBm\n", pAd->StaCfg.dBmToRoam));
+						}
+
+						if(RTMPGetKeyParameter("TGnWifiTest", tmpbuf, 10, buffer))
+						{
+							if(simple_strtol(tmpbuf, 0, 10) == 0)
+								pAd->StaCfg.bTGnWifiTest = FALSE;
+							else
+								pAd->StaCfg.bTGnWifiTest = TRUE;
+								DBGPRINT(RT_DEBUG_TRACE, ("TGnWifiTest=%d\n", pAd->StaCfg.bTGnWifiTest));
+						}
+					}
+#endif // CONFIG_STA_SUPPORT //
+
+				}
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("--> %s does not have a write method\n", src));
+			}
+
+			retval=filp_close(srcf,NULL);
+
+			if (retval)
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("--> Error %d closing %s\n", -retval, src));
+			}
+		}
+	}
+
+	set_fs(orgfs);
+
+#if 0
+	current->cred->fsuid = orgfsuid;
+	current->cred->fsgid = orgfsgid;
+#endif
+
+	kfree(buffer);
+	kfree(tmpbuf);
+
+	return (NDIS_STATUS_SUCCESS);
+}
+
+#ifdef DOT11_N_SUPPORT
+static void	HTParametersHook(
+	IN	PRTMP_ADAPTER pAd,
+	IN	CHAR		  *pValueStr,
+	IN	CHAR		  *pInput)
+{
+
+	INT Value;
+
+    if (RTMPGetKeyParameter("HT_PROTECT", pValueStr, 25, pInput))
+    {
+        Value = simple_strtol(pValueStr, 0, 10);
+        if (Value == 0)
+        {
+            pAd->CommonCfg.bHTProtect = FALSE;
+        }
+        else
+        {
+            pAd->CommonCfg.bHTProtect = TRUE;
+        }
+        DBGPRINT(RT_DEBUG_TRACE, ("HT: Protection  = %s\n", (Value==0) ? "Disable" : "Enable"));
+    }
+
+    if (RTMPGetKeyParameter("HT_MIMOPSEnable", pValueStr, 25, pInput))
+    {
+        Value = simple_strtol(pValueStr, 0, 10);
+        if (Value == 0)
+        {
+            pAd->CommonCfg.bMIMOPSEnable = FALSE;
+        }
+        else
+        {
+            pAd->CommonCfg.bMIMOPSEnable = TRUE;
+        }
+        DBGPRINT(RT_DEBUG_TRACE, ("HT: MIMOPSEnable  = %s\n", (Value==0) ? "Disable" : "Enable"));
+    }
+
+
+    if (RTMPGetKeyParameter("HT_MIMOPSMode", pValueStr, 25, pInput))
+    {
+        Value = simple_strtol(pValueStr, 0, 10);
+        if (Value > MMPS_ENABLE)
+        {
+			pAd->CommonCfg.BACapability.field.MMPSmode = MMPS_ENABLE;
+        }
+        else
+        {
+            //TODO: add mimo power saving mechanism
+            pAd->CommonCfg.BACapability.field.MMPSmode = MMPS_ENABLE;
+			//pAd->CommonCfg.BACapability.field.MMPSmode = Value;
+        }
+        DBGPRINT(RT_DEBUG_TRACE, ("HT: MIMOPS Mode  = %d\n", Value));
+    }
+
+    if (RTMPGetKeyParameter("HT_BADecline", pValueStr, 25, pInput))
+    {
+        Value = simple_strtol(pValueStr, 0, 10);
+        if (Value == 0)
+        {
+            pAd->CommonCfg.bBADecline = FALSE;
+        }
+        else
+        {
+            pAd->CommonCfg.bBADecline = TRUE;
+        }
+        DBGPRINT(RT_DEBUG_TRACE, ("HT: BA Decline  = %s\n", (Value==0) ? "Disable" : "Enable"));
+    }
+
+
+    if (RTMPGetKeyParameter("HT_DisableReordering", pValueStr, 25, pInput))
+    {
+        Value = simple_strtol(pValueStr, 0, 10);
+        if (Value == 0)
+        {
+            pAd->CommonCfg.bDisableReordering = FALSE;
+        }
+        else
+        {
+            pAd->CommonCfg.bDisableReordering = TRUE;
+        }
+        DBGPRINT(RT_DEBUG_TRACE, ("HT: DisableReordering  = %s\n", (Value==0) ? "Disable" : "Enable"));
+    }
+
+    if (RTMPGetKeyParameter("HT_AutoBA", pValueStr, 25, pInput))
+    {
+        Value = simple_strtol(pValueStr, 0, 10);
+        if (Value == 0)
+        {
+            pAd->CommonCfg.BACapability.field.AutoBA = FALSE;
+        }
+        else
+        {
+            pAd->CommonCfg.BACapability.field.AutoBA = TRUE;
+        }
+        pAd->CommonCfg.REGBACapability.field.AutoBA = pAd->CommonCfg.BACapability.field.AutoBA;
+        DBGPRINT(RT_DEBUG_TRACE, ("HT: Auto BA  = %s\n", (Value==0) ? "Disable" : "Enable"));
+    }
+
+	// Tx_+HTC frame
+    if (RTMPGetKeyParameter("HT_HTC", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+		if (Value == 0)
+		{
+			pAd->HTCEnable = FALSE;
+		}
+		else
+		{
+            		pAd->HTCEnable = TRUE;
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: Tx +HTC frame = %s\n", (Value==0) ? "Disable" : "Enable"));
+	}
+
+	// Enable HT Link Adaptation Control
+	if (RTMPGetKeyParameter("HT_LinkAdapt", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+		if (Value == 0)
+		{
+			pAd->bLinkAdapt = FALSE;
+		}
+		else
+		{
+			pAd->HTCEnable = TRUE;
+			pAd->bLinkAdapt = TRUE;
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: Link Adaptation Control = %s\n", (Value==0) ? "Disable" : "Enable(+HTC)"));
+	}
+
+	// Reverse Direction Mechanism
+    if (RTMPGetKeyParameter("HT_RDG", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+		if (Value == 0)
+		{
+			pAd->CommonCfg.bRdg = FALSE;
+		}
+		else
+		{
+			pAd->HTCEnable = TRUE;
+            pAd->CommonCfg.bRdg = TRUE;
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: RDG = %s\n", (Value==0) ? "Disable" : "Enable(+HTC)"));
+	}
+
+
+
+
+	// Tx A-MSUD ?
+    if (RTMPGetKeyParameter("HT_AMSDU", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+		if (Value == 0)
+		{
+			pAd->CommonCfg.BACapability.field.AmsduEnable = FALSE;
+		}
+		else
+		{
+            pAd->CommonCfg.BACapability.field.AmsduEnable = TRUE;
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: Tx A-MSDU = %s\n", (Value==0) ? "Disable" : "Enable"));
+	}
+
+	// MPDU Density
+    if (RTMPGetKeyParameter("HT_MpduDensity", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+		if (Value <=7 && Value >= 0)
+		{
+			pAd->CommonCfg.BACapability.field.MpduDensity = Value;
+			DBGPRINT(RT_DEBUG_TRACE, ("HT: MPDU Density = %d\n", Value));
+		}
+		else
+		{
+			pAd->CommonCfg.BACapability.field.MpduDensity = 4;
+			DBGPRINT(RT_DEBUG_TRACE, ("HT: MPDU Density = %d (Default)\n", 4));
+		}
+	}
+
+	// Max Rx BA Window Size
+    if (RTMPGetKeyParameter("HT_BAWinSize", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+
+		if (Value >=1 && Value <= 64)
+		{
+			pAd->CommonCfg.REGBACapability.field.RxBAWinLimit = Value;
+			pAd->CommonCfg.BACapability.field.RxBAWinLimit = Value;
+			DBGPRINT(RT_DEBUG_TRACE, ("HT: BA Windw Size = %d\n", Value));
+		}
+		else
+		{
+            pAd->CommonCfg.REGBACapability.field.RxBAWinLimit = 64;
+			pAd->CommonCfg.BACapability.field.RxBAWinLimit = 64;
+			DBGPRINT(RT_DEBUG_TRACE, ("HT: BA Windw Size = 64 (Defualt)\n"));
+		}
+
+	}
+
+	// Guard Interval
+	if (RTMPGetKeyParameter("HT_GI", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+
+		if (Value == GI_400)
+		{
+			pAd->CommonCfg.RegTransmitSetting.field.ShortGI = GI_400;
+		}
+		else
+		{
+			pAd->CommonCfg.RegTransmitSetting.field.ShortGI = GI_800;
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: Guard Interval = %s\n", (Value==GI_400) ? "400" : "800" ));
+	}
+
+	// HT Operation Mode : Mixed Mode , Green Field
+	if (RTMPGetKeyParameter("HT_OpMode", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+
+		if (Value == HTMODE_GF)
+		{
+
+			pAd->CommonCfg.RegTransmitSetting.field.HTMODE  = HTMODE_GF;
+		}
+		else
+		{
+			pAd->CommonCfg.RegTransmitSetting.field.HTMODE  = HTMODE_MM;
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: Operate Mode = %s\n", (Value==HTMODE_GF) ? "Green Field" : "Mixed Mode" ));
+	}
+
+	// Fixed Tx mode : CCK, OFDM
+	if (RTMPGetKeyParameter("FixedTxMode", pValueStr, 25, pInput))
+	{
+		UCHAR	fix_tx_mode;
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			fix_tx_mode = FIXED_TXMODE_HT;
+
+			if (strcmp(pValueStr, "OFDM") == 0 || strcmp(pValueStr, "ofdm") == 0)
+			{
+				fix_tx_mode = FIXED_TXMODE_OFDM;
+			}
+			else if (strcmp(pValueStr, "CCK") == 0 || strcmp(pValueStr, "cck") == 0)
+			{
+		        fix_tx_mode = FIXED_TXMODE_CCK;
+			}
+			else if (strcmp(pValueStr, "HT") == 0 || strcmp(pValueStr, "ht") == 0)
+			{
+		        fix_tx_mode = FIXED_TXMODE_HT;
+		}
+		else
+		{
+				Value = simple_strtol(pValueStr, 0, 10);
+				// 1 : CCK
+				// 2 : OFDM
+				// otherwise : HT
+				if (Value == FIXED_TXMODE_CCK || Value == FIXED_TXMODE_OFDM)
+					fix_tx_mode = Value;
+				else
+					fix_tx_mode = FIXED_TXMODE_HT;
+		}
+
+			pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode = fix_tx_mode;
+			DBGPRINT(RT_DEBUG_TRACE, ("Fixed Tx Mode = %d\n", fix_tx_mode));
+
+		}
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+
+	// Channel Width
+	if (RTMPGetKeyParameter("HT_BW", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+
+		if (Value == BW_40)
+		{
+			pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_40;
+		}
+		else
+		{
+            pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_20;
+		}
+
+#ifdef MCAST_RATE_SPECIFIC
+		pAd->CommonCfg.MCastPhyMode.field.BW = pAd->CommonCfg.RegTransmitSetting.field.BW;
+#endif // MCAST_RATE_SPECIFIC //
+
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: Channel Width = %s\n", (Value==BW_40) ? "40 MHz" : "20 MHz" ));
+	}
+
+	if (RTMPGetKeyParameter("HT_EXTCHA", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+
+		if (Value == 0)
+		{
+
+			pAd->CommonCfg.RegTransmitSetting.field.EXTCHA  = EXTCHA_BELOW;
+		}
+		else
+		{
+            pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_ABOVE;
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: Ext Channel = %s\n", (Value==0) ? "BELOW" : "ABOVE" ));
+	}
+
+	// MSC
+	if (RTMPGetKeyParameter("HT_MCS", pValueStr, 50, pInput))
+	{
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			Value = simple_strtol(pValueStr, 0, 10);
+
+			if ((Value >= 0 && Value <= 23) || (Value == 32)) // 3*3
+		{
+				pAd->StaCfg.DesiredTransmitSetting.field.MCS  = Value;
+				pAd->StaCfg.bAutoTxRateSwitch = FALSE;
+				DBGPRINT(RT_DEBUG_TRACE, ("HT: MCS = %d\n", pAd->StaCfg.DesiredTransmitSetting.field.MCS));
+		}
+		else
+		{
+				pAd->StaCfg.DesiredTransmitSetting.field.MCS  = MCS_AUTO;
+				pAd->StaCfg.bAutoTxRateSwitch = TRUE;
+				DBGPRINT(RT_DEBUG_TRACE, ("HT: MCS = AUTO\n"));
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+	// STBC
+    if (RTMPGetKeyParameter("HT_STBC", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+		if (Value == STBC_USE)
+		{
+			pAd->CommonCfg.RegTransmitSetting.field.STBC = STBC_USE;
+		}
+		else
+		{
+			pAd->CommonCfg.RegTransmitSetting.field.STBC = STBC_NONE;
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: STBC = %d\n", pAd->CommonCfg.RegTransmitSetting.field.STBC));
+	}
+
+	// 40_Mhz_Intolerant
+	if (RTMPGetKeyParameter("HT_40MHZ_INTOLERANT", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+		if (Value == 0)
+		{
+			pAd->CommonCfg.bForty_Mhz_Intolerant = FALSE;
+		}
+		else
+		{
+			pAd->CommonCfg.bForty_Mhz_Intolerant = TRUE;
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: 40MHZ INTOLERANT = %d\n", pAd->CommonCfg.bForty_Mhz_Intolerant));
+	}
+	//HT_TxStream
+	if(RTMPGetKeyParameter("HT_TxStream", pValueStr, 10, pInput))
+	{
+		switch (simple_strtol(pValueStr, 0, 10))
+		{
+			case 1:
+				pAd->CommonCfg.TxStream = 1;
+				break;
+			case 2:
+				pAd->CommonCfg.TxStream = 2;
+				break;
+			case 3: // 3*3
+			default:
+				pAd->CommonCfg.TxStream = 3;
+
+				if (pAd->MACVersion < RALINK_2883_VERSION)
+					pAd->CommonCfg.TxStream = 2; // only 2 tx streams for RT2860 series
+				break;
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: Tx Stream = %d\n", pAd->CommonCfg.TxStream));
+	}
+	//HT_RxStream
+	if(RTMPGetKeyParameter("HT_RxStream", pValueStr, 10, pInput))
+	{
+		switch (simple_strtol(pValueStr, 0, 10))
+		{
+			case 1:
+				pAd->CommonCfg.RxStream = 1;
+				break;
+			case 2:
+				pAd->CommonCfg.RxStream = 2;
+				break;
+			case 3:
+			default:
+				pAd->CommonCfg.RxStream = 3;
+
+				if (pAd->MACVersion < RALINK_2883_VERSION)
+					pAd->CommonCfg.RxStream = 2; // only 2 rx streams for RT2860 series
+				break;
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: Rx Stream = %d\n", pAd->CommonCfg.RxStream));
+	}
+
+}
+#endif // DOT11_N_SUPPORT //
+
diff --git a/drivers/staging/rt2860/rtmp.h b/drivers/staging/rt2860/rtmp.h
new file mode 100644
index 00000000000..411954206c2
--- /dev/null
+++ b/drivers/staging/rt2860/rtmp.h
@@ -0,0 +1,7177 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    rtmp.h
+
+    Abstract:
+    Miniport generic portion header file
+
+    Revision History:
+    Who         When          What
+    --------    ----------    ----------------------------------------------
+    Paul Lin    2002-08-01    created
+    James Tan   2002-09-06    modified (Revise NTCRegTable)
+    John Chang  2004-09-06    modified for RT2600
+*/
+#ifndef __RTMP_H__
+#define __RTMP_H__
+
+#include "link_list.h"
+#include "spectrum_def.h"
+
+
+#ifdef CONFIG_STA_SUPPORT
+#include "aironet.h"
+#endif // CONFIG_STA_SUPPORT //
+
+//#define DBG_DIAGNOSE		1
+
+#if defined(CONFIG_AP_SUPPORT) && defined(CONFIG_STA_SUPPORT)
+#define IF_DEV_CONFIG_OPMODE_ON_AP(_pAd)	if(_pAd->OpMode == OPMODE_AP)
+#define IF_DEV_CONFIG_OPMODE_ON_STA(_pAd)	if(_pAd->OpMode == OPMODE_STA)
+#else
+#define IF_DEV_CONFIG_OPMODE_ON_AP(_pAd)
+#define IF_DEV_CONFIG_OPMODE_ON_STA(_pAd)
+#endif
+
+#define VIRTUAL_IF_INC(__pAd) ((__pAd)->VirtualIfCnt++)
+#define VIRTUAL_IF_DEC(__pAd) ((__pAd)->VirtualIfCnt--)
+#define VIRTUAL_IF_NUM(__pAd) ((__pAd)->VirtualIfCnt)
+
+
+
+//
+//  NDIS Version definitions
+//
+#ifdef  NDIS50_MINIPORT
+#define RTMP_NDIS_MAJOR_VERSION     5
+#define RTMP_NDIS_MINOR_VERSION     0
+#endif
+
+#ifdef  NDIS51_MINIPORT
+#define RTMP_NDIS_MAJOR_VERSION     5
+#define RTMP_NDIS_MINOR_VERSION     1
+#endif
+
+extern  char    NIC_VENDOR_DESC[];
+extern  int     NIC_VENDOR_DESC_LEN;
+
+extern  unsigned char   SNAP_AIRONET[];
+extern  unsigned char   CipherSuiteCiscoCCKM[];
+extern  unsigned char   CipherSuiteCiscoCCKMLen;
+extern	unsigned char	CipherSuiteCiscoCCKM24[];
+extern	unsigned char	CipherSuiteCiscoCCKM24Len;
+extern  unsigned char   CipherSuiteCCXTkip[];
+extern  unsigned char   CipherSuiteCCXTkipLen;
+extern  unsigned char   CISCO_OUI[];
+extern  UCHAR	BaSizeArray[4];
+
+extern UCHAR BROADCAST_ADDR[MAC_ADDR_LEN];
+extern UCHAR MULTICAST_ADDR[MAC_ADDR_LEN];
+extern UCHAR ZERO_MAC_ADDR[MAC_ADDR_LEN];
+extern ULONG BIT32[32];
+extern UCHAR BIT8[8];
+extern char* CipherName[];
+extern char* MCSToMbps[];
+extern UCHAR	 RxwiMCSToOfdmRate[12];
+extern UCHAR SNAP_802_1H[6];
+extern UCHAR SNAP_BRIDGE_TUNNEL[6];
+extern UCHAR SNAP_AIRONET[8];
+extern UCHAR CKIP_LLC_SNAP[8];
+extern UCHAR EAPOL_LLC_SNAP[8];
+extern UCHAR EAPOL[2];
+extern UCHAR IPX[2];
+extern UCHAR APPLE_TALK[2];
+extern UCHAR RateIdToPlcpSignal[12]; // see IEEE802.11a-1999 p.14
+extern UCHAR	 OfdmRateToRxwiMCS[];
+extern UCHAR OfdmSignalToRateId[16] ;
+extern UCHAR default_cwmin[4];
+extern UCHAR default_cwmax[4];
+extern UCHAR default_sta_aifsn[4];
+extern UCHAR MapUserPriorityToAccessCategory[8];
+
+extern USHORT RateUpPER[];
+extern USHORT RateDownPER[];
+extern UCHAR  Phy11BNextRateDownward[];
+extern UCHAR  Phy11BNextRateUpward[];
+extern UCHAR  Phy11BGNextRateDownward[];
+extern UCHAR  Phy11BGNextRateUpward[];
+extern UCHAR  Phy11ANextRateDownward[];
+extern UCHAR  Phy11ANextRateUpward[];
+extern CHAR   RssiSafeLevelForTxRate[];
+extern UCHAR  RateIdToMbps[];
+extern USHORT RateIdTo500Kbps[];
+
+extern UCHAR  CipherSuiteWpaNoneTkip[];
+extern UCHAR  CipherSuiteWpaNoneTkipLen;
+
+extern UCHAR  CipherSuiteWpaNoneAes[];
+extern UCHAR  CipherSuiteWpaNoneAesLen;
+
+extern UCHAR  SsidIe;
+extern UCHAR  SupRateIe;
+extern UCHAR  ExtRateIe;
+
+#ifdef DOT11_N_SUPPORT
+extern UCHAR  HtCapIe;
+extern UCHAR  AddHtInfoIe;
+extern UCHAR  NewExtChanIe;
+#ifdef DOT11N_DRAFT3
+extern UCHAR  ExtHtCapIe;
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+
+extern UCHAR  ErpIe;
+extern UCHAR  DsIe;
+extern UCHAR  TimIe;
+extern UCHAR  WpaIe;
+extern UCHAR  Wpa2Ie;
+extern UCHAR  IbssIe;
+extern UCHAR  Ccx2Ie;
+
+extern UCHAR  WPA_OUI[];
+extern UCHAR  RSN_OUI[];
+extern UCHAR  WME_INFO_ELEM[];
+extern UCHAR  WME_PARM_ELEM[];
+extern UCHAR  Ccx2QosInfo[];
+extern UCHAR  Ccx2IeInfo[];
+extern UCHAR  RALINK_OUI[];
+extern UCHAR  PowerConstraintIE[];
+
+
+extern UCHAR  RateSwitchTable[];
+extern UCHAR  RateSwitchTable11B[];
+extern UCHAR  RateSwitchTable11G[];
+extern UCHAR  RateSwitchTable11BG[];
+
+#ifdef DOT11_N_SUPPORT
+extern UCHAR  RateSwitchTable11BGN1S[];
+extern UCHAR  RateSwitchTable11BGN2S[];
+extern UCHAR  RateSwitchTable11BGN2SForABand[];
+extern UCHAR  RateSwitchTable11N1S[];
+extern UCHAR  RateSwitchTable11N2S[];
+extern UCHAR  RateSwitchTable11N2SForABand[];
+
+#ifdef CONFIG_STA_SUPPORT
+extern UCHAR  PRE_N_HT_OUI[];
+#endif // CONFIG_STA_SUPPORT //
+#endif // DOT11_N_SUPPORT //
+
+#define	MAXSEQ		(0xFFF)
+
+#ifdef RALINK_ATE
+typedef	struct _ATE_INFO {
+	UCHAR	Mode;
+	CHAR	TxPower0;
+	CHAR	TxPower1;
+	CHAR    TxAntennaSel;
+	CHAR    RxAntennaSel;
+	TXWI_STRUC  TxWI; 	  // TXWI
+	USHORT	QID;
+	UCHAR	Addr1[MAC_ADDR_LEN];
+	UCHAR	Addr2[MAC_ADDR_LEN];
+	UCHAR	Addr3[MAC_ADDR_LEN];
+	UCHAR	Channel;
+	UINT32	TxLength;
+	UINT32	TxCount;
+	UINT32	TxDoneCount; // Tx DMA Done
+	UINT32	RFFreqOffset;
+	BOOLEAN	bRxFer;
+	BOOLEAN	bQATxStart; // Have compiled QA in and use it to ATE tx.
+	BOOLEAN	bQARxStart;	// Have compiled QA in and use it to ATE rx.
+#ifdef RT2860
+	BOOLEAN	bFWLoading;	// Reload firmware when ATE is done.
+#endif // RT2860 //
+	UINT32	RxTotalCnt;
+	UINT32	RxCntPerSec;
+
+	CHAR	LastSNR0;             // last received SNR
+	CHAR    LastSNR1;             // last received SNR for 2nd  antenna
+	CHAR    LastRssi0;            // last received RSSI
+	CHAR    LastRssi1;            // last received RSSI for 2nd  antenna
+	CHAR    LastRssi2;            // last received RSSI for 3rd  antenna
+	CHAR    AvgRssi0;             // last 8 frames' average RSSI
+	CHAR    AvgRssi1;             // last 8 frames' average RSSI
+	CHAR    AvgRssi2;             // last 8 frames' average RSSI
+	SHORT   AvgRssi0X8;           // sum of last 8 frames' RSSI
+	SHORT   AvgRssi1X8;           // sum of last 8 frames' RSSI
+	SHORT   AvgRssi2X8;           // sum of last 8 frames' RSSI
+
+	UINT32	NumOfAvgRssiSample;
+
+#ifdef RALINK_28xx_QA
+	// Tx frame
+	USHORT		HLen; // Header Length
+	USHORT		PLen; // Pattern Length
+	UCHAR 		Header[32]; // Header buffer
+	UCHAR		Pattern[32]; // Pattern buffer
+	USHORT		DLen; // Data Length
+	USHORT		seq;
+	UINT32		CID;
+	THREAD_PID 		AtePid;
+	// counters
+	UINT32		U2M;
+	UINT32		OtherData;
+	UINT32		Beacon;
+	UINT32		OtherCount;
+	UINT32		TxAc0;
+	UINT32		TxAc1;
+	UINT32		TxAc2;
+	UINT32		TxAc3;
+	UINT32		TxHCCA;
+	UINT32		TxMgmt;
+	UINT32		RSSI0;
+	UINT32		RSSI1;
+	UINT32		RSSI2;
+	UINT32		SNR0;
+	UINT32		SNR1;
+	// control
+	//UINT32		Repeat; // Tx Cpu count
+	UCHAR		TxStatus; // task Tx status // 0 --> task is idle, 1 --> task is running
+#endif // RALINK_28xx_QA //
+}	ATE_INFO, *PATE_INFO;
+
+#ifdef RALINK_28xx_QA
+struct ate_racfghdr {
+ 	UINT32		magic_no;
+	USHORT		command_type;
+	USHORT		command_id;
+	USHORT		length;
+	USHORT		sequence;
+	USHORT		status;
+	UCHAR		data[2046];
+}  __attribute__((packed));
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+
+#ifdef DOT11_N_SUPPORT
+struct reordering_mpdu
+{
+	struct reordering_mpdu	*next;
+	PNDIS_PACKET			pPacket;		/* coverted to 802.3 frame */
+	int						Sequence;		/* sequence number of MPDU */
+	BOOLEAN					bAMSDU;
+};
+
+struct reordering_list
+{
+	struct reordering_mpdu *next;
+	int 	qlen;
+};
+
+struct reordering_mpdu_pool
+{
+	PVOID					mem;
+	NDIS_SPIN_LOCK			lock;
+	struct reordering_list 	freelist;
+};
+#endif // DOT11_N_SUPPORT //
+
+typedef struct 	_RSSI_SAMPLE {
+	CHAR			LastRssi0;             // last received RSSI
+	CHAR			LastRssi1;             // last received RSSI
+	CHAR			LastRssi2;             // last received RSSI
+	CHAR			AvgRssi0;
+	CHAR			AvgRssi1;
+	CHAR			AvgRssi2;
+	SHORT			AvgRssi0X8;
+	SHORT			AvgRssi1X8;
+	SHORT			AvgRssi2X8;
+} RSSI_SAMPLE;
+
+//
+//  Queue structure and macros
+//
+typedef struct  _QUEUE_ENTRY    {
+	struct _QUEUE_ENTRY     *Next;
+}   QUEUE_ENTRY, *PQUEUE_ENTRY;
+
+// Queue structure
+typedef struct  _QUEUE_HEADER   {
+	PQUEUE_ENTRY    Head;
+	PQUEUE_ENTRY    Tail;
+	ULONG           Number;
+}   QUEUE_HEADER, *PQUEUE_HEADER;
+
+#define InitializeQueueHeader(QueueHeader)              \
+{                                                       \
+	(QueueHeader)->Head = (QueueHeader)->Tail = NULL;   \
+	(QueueHeader)->Number = 0;                          \
+}
+
+#define RemoveHeadQueue(QueueHeader)                \
+(QueueHeader)->Head;                                \
+{                                                   \
+	PQUEUE_ENTRY pNext;                             \
+	if ((QueueHeader)->Head != NULL)				\
+	{												\
+		pNext = (QueueHeader)->Head->Next;          \
+		(QueueHeader)->Head = pNext;                \
+		if (pNext == NULL)                          \
+			(QueueHeader)->Tail = NULL;             \
+		(QueueHeader)->Number--;                    \
+	}												\
+}
+
+#define InsertHeadQueue(QueueHeader, QueueEntry)            \
+{                                                           \
+		((PQUEUE_ENTRY)QueueEntry)->Next = (QueueHeader)->Head; \
+		(QueueHeader)->Head = (PQUEUE_ENTRY)(QueueEntry);       \
+		if ((QueueHeader)->Tail == NULL)                        \
+			(QueueHeader)->Tail = (PQUEUE_ENTRY)(QueueEntry);   \
+		(QueueHeader)->Number++;                                \
+}
+
+#define InsertTailQueue(QueueHeader, QueueEntry)                \
+{                                                               \
+	((PQUEUE_ENTRY)QueueEntry)->Next = NULL;                    \
+	if ((QueueHeader)->Tail)                                    \
+		(QueueHeader)->Tail->Next = (PQUEUE_ENTRY)(QueueEntry); \
+	else                                                        \
+		(QueueHeader)->Head = (PQUEUE_ENTRY)(QueueEntry);       \
+	(QueueHeader)->Tail = (PQUEUE_ENTRY)(QueueEntry);           \
+	(QueueHeader)->Number++;                                    \
+}
+
+//
+//  Macros for flag and ref count operations
+//
+#define RTMP_SET_FLAG(_M, _F)       ((_M)->Flags |= (_F))
+#define RTMP_CLEAR_FLAG(_M, _F)     ((_M)->Flags &= ~(_F))
+#define RTMP_CLEAR_FLAGS(_M)        ((_M)->Flags = 0)
+#define RTMP_TEST_FLAG(_M, _F)      (((_M)->Flags & (_F)) != 0)
+#define RTMP_TEST_FLAGS(_M, _F)     (((_M)->Flags & (_F)) == (_F))
+
+#define OPSTATUS_SET_FLAG(_pAd, _F)     ((_pAd)->CommonCfg.OpStatusFlags |= (_F))
+#define OPSTATUS_CLEAR_FLAG(_pAd, _F)   ((_pAd)->CommonCfg.OpStatusFlags &= ~(_F))
+#define OPSTATUS_TEST_FLAG(_pAd, _F)    (((_pAd)->CommonCfg.OpStatusFlags & (_F)) != 0)
+
+#define CLIENT_STATUS_SET_FLAG(_pEntry,_F)      ((_pEntry)->ClientStatusFlags |= (_F))
+#define CLIENT_STATUS_CLEAR_FLAG(_pEntry,_F)    ((_pEntry)->ClientStatusFlags &= ~(_F))
+#define CLIENT_STATUS_TEST_FLAG(_pEntry,_F)     (((_pEntry)->ClientStatusFlags & (_F)) != 0)
+
+#define RX_FILTER_SET_FLAG(_pAd, _F)    ((_pAd)->CommonCfg.PacketFilter |= (_F))
+#define RX_FILTER_CLEAR_FLAG(_pAd, _F)  ((_pAd)->CommonCfg.PacketFilter &= ~(_F))
+#define RX_FILTER_TEST_FLAG(_pAd, _F)   (((_pAd)->CommonCfg.PacketFilter & (_F)) != 0)
+
+#ifdef CONFIG_STA_SUPPORT
+#define STA_NO_SECURITY_ON(_p)          (_p->StaCfg.WepStatus == Ndis802_11EncryptionDisabled)
+#define STA_WEP_ON(_p)                  (_p->StaCfg.WepStatus == Ndis802_11Encryption1Enabled)
+#define STA_TKIP_ON(_p)                 (_p->StaCfg.WepStatus == Ndis802_11Encryption2Enabled)
+#define STA_AES_ON(_p)                  (_p->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+
+#define STA_TGN_WIFI_ON(_p)             (_p->StaCfg.bTGnWifiTest == TRUE)
+#endif // CONFIG_STA_SUPPORT //
+
+#define CKIP_KP_ON(_p)				((((_p)->StaCfg.CkipFlag) & 0x10) && ((_p)->StaCfg.bCkipCmicOn == TRUE))
+#define CKIP_CMIC_ON(_p)			((((_p)->StaCfg.CkipFlag) & 0x08) && ((_p)->StaCfg.bCkipCmicOn == TRUE))
+
+
+#define INC_RING_INDEX(_idx, _RingSize)    \
+{                                          \
+    (_idx) = (_idx+1) % (_RingSize);       \
+}
+
+#define IS_RT3070(_pAd)				(((_pAd)->MACVersion & 0xffff0000) == 0x30700000)
+
+#define RING_PACKET_INIT(_TxRing, _idx)    \
+{                                          \
+    _TxRing->Cell[_idx].pNdisPacket = NULL;                              \
+    _TxRing->Cell[_idx].pNextNdisPacket = NULL;                              \
+}
+
+#define TXDT_INIT(_TxD)    \
+{                                          \
+	NdisZeroMemory(_TxD, TXD_SIZE);	\
+	_TxD->DMADONE = 1;                              \
+}
+
+//Set last data segment
+#define RING_SET_LASTDS(_TxD, _IsSD0)    \
+{                                          \
+    if (_IsSD0) {_TxD->LastSec0 = 1;}     \
+    else {_TxD->LastSec1 = 1;}     \
+}
+
+// Increase TxTsc value for next transmission
+// TODO:
+// When i==6, means TSC has done one full cycle, do re-keying stuff follow specs
+// Should send a special event microsoft defined to request re-key
+#define INC_TX_TSC(_tsc)                                \
+{                                                       \
+    int i=0;                                            \
+    while (++_tsc[i] == 0x0)                            \
+    {                                                   \
+        i++;                                            \
+        if (i == 6)                                     \
+            break;                                      \
+    }                                                   \
+}
+
+#ifdef DOT11_N_SUPPORT
+// StaActive.SupportedHtPhy.MCSSet is copied from AP beacon.  Don't need to update here.
+#define COPY_HTSETTINGS_FROM_MLME_AUX_TO_ACTIVE_CFG(_pAd)                                 \
+{                                                                                       \
+	_pAd->StaActive.SupportedHtPhy.ChannelWidth = _pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth;      \
+	_pAd->StaActive.SupportedHtPhy.MimoPs = _pAd->MlmeAux.HtCapability.HtCapInfo.MimoPs;      \
+	_pAd->StaActive.SupportedHtPhy.GF = _pAd->MlmeAux.HtCapability.HtCapInfo.GF;      \
+	_pAd->StaActive.SupportedHtPhy.ShortGIfor20 = _pAd->MlmeAux.HtCapability.HtCapInfo.ShortGIfor20;      \
+	_pAd->StaActive.SupportedHtPhy.ShortGIfor40 = _pAd->MlmeAux.HtCapability.HtCapInfo.ShortGIfor40;      \
+	_pAd->StaActive.SupportedHtPhy.TxSTBC = _pAd->MlmeAux.HtCapability.HtCapInfo.TxSTBC;      \
+	_pAd->StaActive.SupportedHtPhy.RxSTBC = _pAd->MlmeAux.HtCapability.HtCapInfo.RxSTBC;      \
+	_pAd->StaActive.SupportedHtPhy.ExtChanOffset = _pAd->MlmeAux.AddHtInfo.AddHtInfo.ExtChanOffset;      \
+	_pAd->StaActive.SupportedHtPhy.RecomWidth = _pAd->MlmeAux.AddHtInfo.AddHtInfo.RecomWidth;      \
+	_pAd->StaActive.SupportedHtPhy.OperaionMode = _pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode;      \
+	_pAd->StaActive.SupportedHtPhy.NonGfPresent = _pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent;      \
+	NdisMoveMemory((_pAd)->MacTab.Content[BSSID_WCID].HTCapability.MCSSet, (_pAd)->StaActive.SupportedPhyInfo.MCSSet, sizeof(UCHAR) * 16);\
+}
+
+#define COPY_AP_HTSETTINGS_FROM_BEACON(_pAd, _pHtCapability)                                 \
+{                                                                                       \
+	_pAd->MacTab.Content[BSSID_WCID].AMsduSize = (UCHAR)(_pHtCapability->HtCapInfo.AMsduSize);	\
+	_pAd->MacTab.Content[BSSID_WCID].MmpsMode= (UCHAR)(_pHtCapability->HtCapInfo.MimoPs);	\
+	_pAd->MacTab.Content[BSSID_WCID].MaxRAmpduFactor = (UCHAR)(_pHtCapability->HtCapParm.MaxRAmpduFactor);	\
+}
+#endif // DOT11_N_SUPPORT //
+
+//
+// MACRO for 32-bit PCI register read / write
+//
+// Usage : RTMP_IO_READ32(
+//              PRTMP_ADAPTER pAd,
+//              ULONG Register_Offset,
+//              PULONG  pValue)
+//
+//         RTMP_IO_WRITE32(
+//              PRTMP_ADAPTER pAd,
+//              ULONG Register_Offset,
+//              ULONG Value)
+//
+
+//
+// BBP & RF are using indirect access. Before write any value into it.
+// We have to make sure there is no outstanding command pending via checking busy bit.
+//
+#define MAX_BUSY_COUNT  100         // Number of retry before failing access BBP & RF indirect register
+//
+#ifdef RT2860
+#define RTMP_RF_IO_WRITE32(_A, _V)                  \
+{                                                   \
+    PHY_CSR4_STRUC  Value;                          \
+    ULONG           BusyCnt = 0;                    \
+    if ((_A)->bPCIclkOff) 	                \
+    {												\
+        return;										\
+    }                                               \
+    do {                                            \
+        RTMP_IO_READ32(_A, RF_CSR_CFG0, &Value.word);  \
+        if (Value.field.Busy == IDLE)               \
+            break;                                  \
+        BusyCnt++;                                  \
+    }   while (BusyCnt < MAX_BUSY_COUNT);           \
+    if (BusyCnt < MAX_BUSY_COUNT)                   \
+    {                                               \
+        RTMP_IO_WRITE32(_A, RF_CSR_CFG0, _V);          \
+    }                                               \
+}
+
+#define BBP_IO_READ8_BY_REG_ID(_A, _I, _pV)        \
+{                                                       \
+    BBP_CSR_CFG_STRUC  BbpCsr;                             \
+    int             i, k;                               \
+    for (i=0; i<MAX_BUSY_COUNT; i++)                    \
+    {                                                   \
+        RTMP_IO_READ32(_A, BBP_CSR_CFG, &BbpCsr.word);     \
+        if (BbpCsr.field.Busy == BUSY)                  \
+        {                                               \
+            continue;                                   \
+        }                                               \
+        BbpCsr.word = 0;                                \
+        BbpCsr.field.fRead = 1;                         \
+        BbpCsr.field.BBP_RW_MODE = 1;                         \
+        BbpCsr.field.Busy = 1;                          \
+        BbpCsr.field.RegNum = _I;                       \
+        RTMP_IO_WRITE32(_A, BBP_CSR_CFG, BbpCsr.word);     \
+        for (k=0; k<MAX_BUSY_COUNT; k++)                \
+        {                                               \
+            RTMP_IO_READ32(_A, BBP_CSR_CFG, &BbpCsr.word); \
+            if (BbpCsr.field.Busy == IDLE)              \
+                break;                                  \
+        }                                               \
+        if ((BbpCsr.field.Busy == IDLE) &&              \
+            (BbpCsr.field.RegNum == _I))                \
+        {                                               \
+            *(_pV) = (UCHAR)BbpCsr.field.Value;         \
+            break;                                      \
+        }                                               \
+    }                                                   \
+    if (BbpCsr.field.Busy == BUSY)                      \
+    {                                                   \
+        DBGPRINT_ERR(("DFS BBP read R%d fail\n", _I));      \
+        *(_pV) = (_A)->BbpWriteLatch[_I];               \
+    }                                                   \
+}
+
+//#define RTMP_BBP_IO_READ8_BY_REG_ID(_A, _I, _pV)    {}
+// Read BBP register by register's ID. Generate PER to test BA
+#define RTMP_BBP_IO_READ8_BY_REG_ID(_A, _I, _pV)        \
+{                                                       \
+    BBP_CSR_CFG_STRUC  BbpCsr;                             \
+    int             i, k;                               \
+    if ((_A)->bPCIclkOff == FALSE)                     \
+    {                                                   \
+    for (i=0; i<MAX_BUSY_COUNT; i++)                    \
+    {                                                   \
+		RTMP_IO_READ32(_A, H2M_BBP_AGENT, &BbpCsr.word);				\
+        if (BbpCsr.field.Busy == BUSY)                  \
+        {                                               \
+            continue;                                   \
+        }                                               \
+        BbpCsr.word = 0;                                \
+        BbpCsr.field.fRead = 1;                         \
+        BbpCsr.field.BBP_RW_MODE = 1;                         \
+        BbpCsr.field.Busy = 1;                          \
+        BbpCsr.field.RegNum = _I;                       \
+		RTMP_IO_WRITE32(_A, H2M_BBP_AGENT, BbpCsr.word);				\
+		AsicSendCommandToMcu(_A, 0x80, 0xff, 0x0, 0x0);					\
+		RTMPusecDelay(1000);							\
+        for (k=0; k<MAX_BUSY_COUNT; k++)                \
+        {                                               \
+			RTMP_IO_READ32(_A, H2M_BBP_AGENT, &BbpCsr.word);			\
+            if (BbpCsr.field.Busy == IDLE)              \
+                break;                                  \
+        }                                               \
+        if ((BbpCsr.field.Busy == IDLE) &&              \
+            (BbpCsr.field.RegNum == _I))                \
+        {                                               \
+            *(_pV) = (UCHAR)BbpCsr.field.Value;         \
+            break;                                      \
+        }                                               \
+    }                                                   \
+    if (BbpCsr.field.Busy == BUSY)                      \
+    {                                                   \
+		DBGPRINT_ERR(("BBP read R%d=0x%x fail\n", _I, BbpCsr.word));	\
+        *(_pV) = (_A)->BbpWriteLatch[_I];               \
+		RTMP_IO_READ32(_A, H2M_BBP_AGENT, &BbpCsr.word);				\
+		BbpCsr.field.Busy = 0;                          \
+		RTMP_IO_WRITE32(_A, H2M_BBP_AGENT, BbpCsr.word);				\
+    }                                                   \
+    }                   \
+}
+
+#define BBP_IO_WRITE8_BY_REG_ID(_A, _I, _V)        \
+{                                                       \
+    BBP_CSR_CFG_STRUC  BbpCsr;                             \
+    int             BusyCnt;                            \
+    for (BusyCnt=0; BusyCnt<MAX_BUSY_COUNT; BusyCnt++)  \
+    {                                                   \
+        RTMP_IO_READ32(_A, BBP_CSR_CFG, &BbpCsr.word);     \
+        if (BbpCsr.field.Busy == BUSY)                  \
+            continue;                                   \
+        BbpCsr.word = 0;                                \
+        BbpCsr.field.fRead = 0;                         \
+        BbpCsr.field.BBP_RW_MODE = 1;                         \
+        BbpCsr.field.Busy = 1;                          \
+        BbpCsr.field.Value = _V;                        \
+        BbpCsr.field.RegNum = _I;                       \
+        RTMP_IO_WRITE32(_A, BBP_CSR_CFG, BbpCsr.word);     \
+        (_A)->BbpWriteLatch[_I] = _V;                   \
+        break;                                          \
+    }                                                   \
+    if (BusyCnt == MAX_BUSY_COUNT)                      \
+    {                                                   \
+        DBGPRINT_ERR(("BBP write R%d fail\n", _I));     \
+    }                                                   \
+}
+
+// Write BBP register by register's ID & value
+#define RTMP_BBP_IO_WRITE8_BY_REG_ID(_A, _I, _V)        \
+{                                                       \
+    BBP_CSR_CFG_STRUC  BbpCsr;                             \
+    int             BusyCnt;                            \
+    if ((_A)->bPCIclkOff == FALSE)                     \
+    {                                                   \
+    for (BusyCnt=0; BusyCnt<MAX_BUSY_COUNT; BusyCnt++)  \
+    {                                                   \
+		RTMP_IO_READ32(_A, H2M_BBP_AGENT, &BbpCsr.word);				\
+        if (BbpCsr.field.Busy == BUSY)                  \
+            continue;                                   \
+        BbpCsr.word = 0;                                \
+        BbpCsr.field.fRead = 0;                         \
+        BbpCsr.field.BBP_RW_MODE = 1;                         \
+        BbpCsr.field.Busy = 1;                          \
+        BbpCsr.field.Value = _V;                        \
+        BbpCsr.field.RegNum = _I;                       \
+		RTMP_IO_WRITE32(_A, H2M_BBP_AGENT, BbpCsr.word);				\
+		AsicSendCommandToMcu(_A, 0x80, 0xff, 0x0, 0x0);					\
+            if (_A->OpMode == OPMODE_AP)                    \
+		RTMPusecDelay(1000);							\
+        (_A)->BbpWriteLatch[_I] = _V;                   \
+        break;                                          \
+    }                                                   \
+    if (BusyCnt == MAX_BUSY_COUNT)                      \
+    {                                                   \
+		DBGPRINT_ERR(("BBP write R%d=0x%x fail\n", _I, BbpCsr.word));	\
+		RTMP_IO_READ32(_A, H2M_BBP_AGENT, &BbpCsr.word);				\
+		BbpCsr.field.Busy = 0;                          \
+		RTMP_IO_WRITE32(_A, H2M_BBP_AGENT, BbpCsr.word);				\
+    }                                                   \
+    }                                                   \
+}
+#endif // RT2860 //
+
+
+#define     MAP_CHANNEL_ID_TO_KHZ(ch, khz)  {               \
+                switch (ch)                                 \
+                {                                           \
+                    case 1:     khz = 2412000;   break;     \
+                    case 2:     khz = 2417000;   break;     \
+                    case 3:     khz = 2422000;   break;     \
+                    case 4:     khz = 2427000;   break;     \
+                    case 5:     khz = 2432000;   break;     \
+                    case 6:     khz = 2437000;   break;     \
+                    case 7:     khz = 2442000;   break;     \
+                    case 8:     khz = 2447000;   break;     \
+                    case 9:     khz = 2452000;   break;     \
+                    case 10:    khz = 2457000;   break;     \
+                    case 11:    khz = 2462000;   break;     \
+                    case 12:    khz = 2467000;   break;     \
+                    case 13:    khz = 2472000;   break;     \
+                    case 14:    khz = 2484000;   break;     \
+                    case 36:  /* UNII */  khz = 5180000;   break;     \
+                    case 40:  /* UNII */  khz = 5200000;   break;     \
+                    case 44:  /* UNII */  khz = 5220000;   break;     \
+                    case 48:  /* UNII */  khz = 5240000;   break;     \
+                    case 52:  /* UNII */  khz = 5260000;   break;     \
+                    case 56:  /* UNII */  khz = 5280000;   break;     \
+                    case 60:  /* UNII */  khz = 5300000;   break;     \
+                    case 64:  /* UNII */  khz = 5320000;   break;     \
+                    case 149: /* UNII */  khz = 5745000;   break;     \
+                    case 153: /* UNII */  khz = 5765000;   break;     \
+                    case 157: /* UNII */  khz = 5785000;   break;     \
+                    case 161: /* UNII */  khz = 5805000;   break;     \
+                    case 165: /* UNII */  khz = 5825000;   break;     \
+                    case 100: /* HiperLAN2 */  khz = 5500000;   break;     \
+                    case 104: /* HiperLAN2 */  khz = 5520000;   break;     \
+                    case 108: /* HiperLAN2 */  khz = 5540000;   break;     \
+                    case 112: /* HiperLAN2 */  khz = 5560000;   break;     \
+                    case 116: /* HiperLAN2 */  khz = 5580000;   break;     \
+                    case 120: /* HiperLAN2 */  khz = 5600000;   break;     \
+                    case 124: /* HiperLAN2 */  khz = 5620000;   break;     \
+                    case 128: /* HiperLAN2 */  khz = 5640000;   break;     \
+                    case 132: /* HiperLAN2 */  khz = 5660000;   break;     \
+                    case 136: /* HiperLAN2 */  khz = 5680000;   break;     \
+                    case 140: /* HiperLAN2 */  khz = 5700000;   break;     \
+                    case 34:  /* Japan MMAC */   khz = 5170000;   break;   \
+                    case 38:  /* Japan MMAC */   khz = 5190000;   break;   \
+                    case 42:  /* Japan MMAC */   khz = 5210000;   break;   \
+                    case 46:  /* Japan MMAC */   khz = 5230000;   break;   \
+                    case 184: /* Japan */   khz = 4920000;   break;   \
+                    case 188: /* Japan */   khz = 4940000;   break;   \
+                    case 192: /* Japan */   khz = 4960000;   break;   \
+                    case 196: /* Japan */   khz = 4980000;   break;   \
+                    case 208: /* Japan, means J08 */   khz = 5040000;   break;   \
+                    case 212: /* Japan, means J12 */   khz = 5060000;   break;   \
+                    case 216: /* Japan, means J16 */   khz = 5080000;   break;   \
+                    default:    khz = 2412000;   break;     \
+                }                                           \
+            }
+
+#define     MAP_KHZ_TO_CHANNEL_ID(khz, ch)  {               \
+                switch (khz)                                \
+                {                                           \
+                    case 2412000:    ch = 1;     break;     \
+                    case 2417000:    ch = 2;     break;     \
+                    case 2422000:    ch = 3;     break;     \
+                    case 2427000:    ch = 4;     break;     \
+                    case 2432000:    ch = 5;     break;     \
+                    case 2437000:    ch = 6;     break;     \
+                    case 2442000:    ch = 7;     break;     \
+                    case 2447000:    ch = 8;     break;     \
+                    case 2452000:    ch = 9;     break;     \
+                    case 2457000:    ch = 10;    break;     \
+                    case 2462000:    ch = 11;    break;     \
+                    case 2467000:    ch = 12;    break;     \
+                    case 2472000:    ch = 13;    break;     \
+                    case 2484000:    ch = 14;    break;     \
+                    case 5180000:    ch = 36;  /* UNII */  break;     \
+                    case 5200000:    ch = 40;  /* UNII */  break;     \
+                    case 5220000:    ch = 44;  /* UNII */  break;     \
+                    case 5240000:    ch = 48;  /* UNII */  break;     \
+                    case 5260000:    ch = 52;  /* UNII */  break;     \
+                    case 5280000:    ch = 56;  /* UNII */  break;     \
+                    case 5300000:    ch = 60;  /* UNII */  break;     \
+                    case 5320000:    ch = 64;  /* UNII */  break;     \
+                    case 5745000:    ch = 149; /* UNII */  break;     \
+                    case 5765000:    ch = 153; /* UNII */  break;     \
+                    case 5785000:    ch = 157; /* UNII */  break;     \
+                    case 5805000:    ch = 161; /* UNII */  break;     \
+                    case 5825000:    ch = 165; /* UNII */  break;     \
+                    case 5500000:    ch = 100; /* HiperLAN2 */  break;     \
+                    case 5520000:    ch = 104; /* HiperLAN2 */  break;     \
+                    case 5540000:    ch = 108; /* HiperLAN2 */  break;     \
+                    case 5560000:    ch = 112; /* HiperLAN2 */  break;     \
+                    case 5580000:    ch = 116; /* HiperLAN2 */  break;     \
+                    case 5600000:    ch = 120; /* HiperLAN2 */  break;     \
+                    case 5620000:    ch = 124; /* HiperLAN2 */  break;     \
+                    case 5640000:    ch = 128; /* HiperLAN2 */  break;     \
+                    case 5660000:    ch = 132; /* HiperLAN2 */  break;     \
+                    case 5680000:    ch = 136; /* HiperLAN2 */  break;     \
+                    case 5700000:    ch = 140; /* HiperLAN2 */  break;     \
+                    case 5170000:    ch = 34;  /* Japan MMAC */   break;   \
+                    case 5190000:    ch = 38;  /* Japan MMAC */   break;   \
+                    case 5210000:    ch = 42;  /* Japan MMAC */   break;   \
+                    case 5230000:    ch = 46;  /* Japan MMAC */   break;   \
+                    case 4920000:    ch = 184; /* Japan */  break;   \
+                    case 4940000:    ch = 188; /* Japan */  break;   \
+                    case 4960000:    ch = 192; /* Japan */  break;   \
+                    case 4980000:    ch = 196; /* Japan */  break;   \
+                    case 5040000:    ch = 208; /* Japan, means J08 */  break;   \
+                    case 5060000:    ch = 212; /* Japan, means J12 */  break;   \
+                    case 5080000:    ch = 216; /* Japan, means J16 */  break;   \
+                    default:         ch = 1;     break;     \
+                }                                           \
+            }
+
+//
+// Common fragment list structure -  Identical to the scatter gather frag list structure
+//
+#define NIC_MAX_PHYS_BUF_COUNT              8
+
+typedef struct _RTMP_SCATTER_GATHER_ELEMENT {
+    PVOID		Address;
+    ULONG		Length;
+    PULONG		Reserved;
+} RTMP_SCATTER_GATHER_ELEMENT, *PRTMP_SCATTER_GATHER_ELEMENT;
+
+
+typedef struct _RTMP_SCATTER_GATHER_LIST {
+    ULONG  NumberOfElements;
+    PULONG Reserved;
+    RTMP_SCATTER_GATHER_ELEMENT Elements[NIC_MAX_PHYS_BUF_COUNT];
+} RTMP_SCATTER_GATHER_LIST, *PRTMP_SCATTER_GATHER_LIST;
+
+//
+//  Some utility macros
+//
+#ifndef min
+#define min(_a, _b)     (((_a) < (_b)) ? (_a) : (_b))
+#endif
+
+#ifndef max
+#define max(_a, _b)     (((_a) > (_b)) ? (_a) : (_b))
+#endif
+
+#define GET_LNA_GAIN(_pAd)	((_pAd->LatchRfRegs.Channel <= 14) ? (_pAd->BLNAGain) : ((_pAd->LatchRfRegs.Channel <= 64) ? (_pAd->ALNAGain0) : ((_pAd->LatchRfRegs.Channel <= 128) ? (_pAd->ALNAGain1) : (_pAd->ALNAGain2))))
+
+#define INC_COUNTER64(Val)          (Val.QuadPart++)
+
+#define INFRA_ON(_p)                (OPSTATUS_TEST_FLAG(_p, fOP_STATUS_INFRA_ON))
+#define ADHOC_ON(_p)                (OPSTATUS_TEST_FLAG(_p, fOP_STATUS_ADHOC_ON))
+#define MONITOR_ON(_p)              (((_p)->StaCfg.BssType) == BSS_MONITOR)
+#define IDLE_ON(_p)                 (!INFRA_ON(_p) && !ADHOC_ON(_p))
+
+// Check LEAP & CCKM flags
+#define LEAP_ON(_p)                 (((_p)->StaCfg.LeapAuthMode) == CISCO_AuthModeLEAP)
+#define LEAP_CCKM_ON(_p)            ((((_p)->StaCfg.LeapAuthMode) == CISCO_AuthModeLEAP) && ((_p)->StaCfg.LeapAuthInfo.CCKM == TRUE))
+
+// if orginal Ethernet frame contains no LLC/SNAP, then an extra LLC/SNAP encap is required
+#define EXTRA_LLCSNAP_ENCAP_FROM_PKT_START(_pBufVA, _pExtraLlcSnapEncap)		\
+{																\
+	if (((*(_pBufVA + 12) << 8) + *(_pBufVA + 13)) > 1500)		\
+	{															\
+		_pExtraLlcSnapEncap = SNAP_802_1H;						\
+		if (NdisEqualMemory(IPX, _pBufVA + 12, 2) || 			\
+			NdisEqualMemory(APPLE_TALK, _pBufVA + 12, 2))		\
+		{														\
+			_pExtraLlcSnapEncap = SNAP_BRIDGE_TUNNEL;			\
+		}														\
+	}															\
+	else														\
+	{															\
+		_pExtraLlcSnapEncap = NULL;								\
+	}															\
+}
+
+// New Define for new Tx Path.
+#define EXTRA_LLCSNAP_ENCAP_FROM_PKT_OFFSET(_pBufVA, _pExtraLlcSnapEncap)	\
+{																\
+	if (((*(_pBufVA) << 8) + *(_pBufVA + 1)) > 1500)			\
+	{															\
+		_pExtraLlcSnapEncap = SNAP_802_1H;						\
+		if (NdisEqualMemory(IPX, _pBufVA, 2) || 				\
+			NdisEqualMemory(APPLE_TALK, _pBufVA, 2))			\
+		{														\
+			_pExtraLlcSnapEncap = SNAP_BRIDGE_TUNNEL;			\
+		}														\
+	}															\
+	else														\
+	{															\
+		_pExtraLlcSnapEncap = NULL;								\
+	}															\
+}
+
+
+#define MAKE_802_3_HEADER(_p, _pMac1, _pMac2, _pType)                   \
+{                                                                       \
+    NdisMoveMemory(_p, _pMac1, MAC_ADDR_LEN);                           \
+    NdisMoveMemory((_p + MAC_ADDR_LEN), _pMac2, MAC_ADDR_LEN);          \
+    NdisMoveMemory((_p + MAC_ADDR_LEN * 2), _pType, LENGTH_802_3_TYPE); \
+}
+
+// if pData has no LLC/SNAP (neither RFC1042 nor Bridge tunnel), keep it that way.
+// else if the received frame is LLC/SNAP-encaped IPX or APPLETALK, preserve the LLC/SNAP field
+// else remove the LLC/SNAP field from the result Ethernet frame
+// Patch for WHQL only, which did not turn on Netbios but use IPX within its payload
+// Note:
+//     _pData & _DataSize may be altered (remove 8-byte LLC/SNAP) by this MACRO
+//     _pRemovedLLCSNAP: pointer to removed LLC/SNAP; NULL is not removed
+#define CONVERT_TO_802_3(_p8023hdr, _pDA, _pSA, _pData, _DataSize, _pRemovedLLCSNAP)      \
+{                                                                       \
+    char LLC_Len[2];                                                    \
+                                                                        \
+    _pRemovedLLCSNAP = NULL;                                            \
+    if (NdisEqualMemory(SNAP_802_1H, _pData, 6)  ||                     \
+        NdisEqualMemory(SNAP_BRIDGE_TUNNEL, _pData, 6))                 \
+    {                                                                   \
+        PUCHAR pProto = _pData + 6;                                     \
+                                                                        \
+        if ((NdisEqualMemory(IPX, pProto, 2) || NdisEqualMemory(APPLE_TALK, pProto, 2)) &&  \
+            NdisEqualMemory(SNAP_802_1H, _pData, 6))                    \
+        {                                                               \
+            LLC_Len[0] = (UCHAR)(_DataSize / 256);                      \
+            LLC_Len[1] = (UCHAR)(_DataSize % 256);                      \
+            MAKE_802_3_HEADER(_p8023hdr, _pDA, _pSA, LLC_Len);          \
+        }                                                               \
+        else                                                            \
+        {                                                               \
+            MAKE_802_3_HEADER(_p8023hdr, _pDA, _pSA, pProto);           \
+            _pRemovedLLCSNAP = _pData;                                  \
+            _DataSize -= LENGTH_802_1_H;                                \
+            _pData += LENGTH_802_1_H;                                   \
+        }                                                               \
+    }                                                                   \
+    else                                                                \
+    {                                                                   \
+        LLC_Len[0] = (UCHAR)(_DataSize / 256);                          \
+        LLC_Len[1] = (UCHAR)(_DataSize % 256);                          \
+        MAKE_802_3_HEADER(_p8023hdr, _pDA, _pSA, LLC_Len);              \
+    }                                                                   \
+}
+
+#define SWITCH_AB( _pAA, _pBB)    \
+{                                                                           \
+    PVOID pCC;                                                          \
+    pCC = _pBB;                                                 \
+    _pBB = _pAA;                                                 \
+    _pAA = pCC;                                                 \
+}
+
+// Enqueue this frame to MLME engine
+// We need to enqueue the whole frame because MLME need to pass data type
+// information from 802.11 header
+#ifdef RT2860
+#define REPORT_MGMT_FRAME_TO_MLME(_pAd, Wcid, _pFrame, _FrameSize, _Rssi0, _Rssi1, _Rssi2, _PlcpSignal)        \
+{                                                                                       \
+    UINT32 High32TSF, Low32TSF;                                                          \
+    RTMP_IO_READ32(_pAd, TSF_TIMER_DW1, &High32TSF);                                       \
+    RTMP_IO_READ32(_pAd, TSF_TIMER_DW0, &Low32TSF);                                        \
+    MlmeEnqueueForRecv(_pAd, Wcid, High32TSF, Low32TSF, (UCHAR)_Rssi0, (UCHAR)_Rssi1,(UCHAR)_Rssi2,_FrameSize, _pFrame, (UCHAR)_PlcpSignal);   \
+}
+#endif // RT2860 //
+
+#define NDIS_QUERY_BUFFER(_NdisBuf, _ppVA, _pBufLen)                    \
+    NdisQueryBuffer(_NdisBuf, _ppVA, _pBufLen)
+
+#define MAC_ADDR_EQUAL(pAddr1,pAddr2)           RTMPEqualMemory((PVOID)(pAddr1), (PVOID)(pAddr2), MAC_ADDR_LEN)
+#define SSID_EQUAL(ssid1, len1, ssid2, len2)    ((len1==len2) && (RTMPEqualMemory(ssid1, ssid2, len1)))
+
+//
+// Check if it is Japan W53(ch52,56,60,64) channel.
+//
+#define JapanChannelCheck(channel)  ((channel == 52) || (channel == 56) || (channel == 60) || (channel == 64))
+
+#ifdef CONFIG_STA_SUPPORT
+#define STA_PORT_SECURED(_pAd) \
+{ \
+	_pAd->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED; \
+	NdisAcquireSpinLock(&_pAd->MacTabLock); \
+	_pAd->MacTab.Content[BSSID_WCID].PortSecured = _pAd->StaCfg.PortSecured; \
+	NdisReleaseSpinLock(&_pAd->MacTabLock); \
+}
+#endif // CONFIG_STA_SUPPORT //
+
+
+//
+// Register set pair for initialzation register set definition
+//
+typedef struct  _RTMP_REG_PAIR
+{
+	ULONG   Register;
+	ULONG   Value;
+} RTMP_REG_PAIR, *PRTMP_REG_PAIR;
+
+typedef struct  _REG_PAIR
+{
+	UCHAR   Register;
+	UCHAR   Value;
+} REG_PAIR, *PREG_PAIR;
+
+//
+// Register set pair for initialzation register set definition
+//
+typedef struct  _RTMP_RF_REGS
+{
+	UCHAR   Channel;
+	ULONG   R1;
+	ULONG   R2;
+	ULONG   R3;
+	ULONG   R4;
+} RTMP_RF_REGS, *PRTMP_RF_REGS;
+
+typedef struct _FREQUENCY_ITEM {
+	UCHAR	Channel;
+	UCHAR	N;
+	UCHAR	R;
+	UCHAR	K;
+} FREQUENCY_ITEM, *PFREQUENCY_ITEM;
+
+//
+//  Data buffer for DMA operation, the buffer must be contiguous physical memory
+//  Both DMA to / from CPU use the same structure.
+//
+typedef struct  _RTMP_DMABUF
+{
+	ULONG                   AllocSize;
+	PVOID                   AllocVa;            // TxBuf virtual address
+	NDIS_PHYSICAL_ADDRESS   AllocPa;            // TxBuf physical address
+} RTMP_DMABUF, *PRTMP_DMABUF;
+
+
+typedef	union	_HEADER_802_11_SEQ{
+#ifdef RT_BIG_ENDIAN
+    struct {
+   	USHORT			Sequence:12;
+	USHORT			Frag:4;
+    }   field;
+#else
+    struct {
+	USHORT			Frag:4;
+	USHORT			Sequence:12;
+    }   field;
+#endif
+    USHORT           value;
+}	HEADER_802_11_SEQ, *PHEADER_802_11_SEQ;
+
+//
+//  Data buffer for DMA operation, the buffer must be contiguous physical memory
+//  Both DMA to / from CPU use the same structure.
+//
+typedef struct  _RTMP_REORDERBUF
+{
+	BOOLEAN			IsFull;
+	PVOID                   AllocVa;            // TxBuf virtual address
+	UCHAR			Header802_3[14];
+	HEADER_802_11_SEQ			Sequence;	//support compressed bitmap BA, so no consider fragment in BA
+	UCHAR 		DataOffset;
+	USHORT 		Datasize;
+	ULONG                   AllocSize;
+#ifdef RT2860
+	NDIS_PHYSICAL_ADDRESS   AllocPa;            // TxBuf physical address
+#endif // RT2860 //
+}   RTMP_REORDERBUF, *PRTMP_REORDERBUF;
+
+//
+// Control block (Descriptor) for all ring descriptor DMA operation, buffer must be
+// contiguous physical memory. NDIS_PACKET stored the binding Rx packet descriptor
+// which won't be released, driver has to wait until upper layer return the packet
+// before giveing up this rx ring descriptor to ASIC. NDIS_BUFFER is assocaited pair
+// to describe the packet buffer. For Tx, NDIS_PACKET stored the tx packet descriptor
+// which driver should ACK upper layer when the tx is physically done or failed.
+//
+typedef struct _RTMP_DMACB
+{
+	ULONG                   AllocSize;          // Control block size
+	PVOID                   AllocVa;            // Control block virtual address
+	NDIS_PHYSICAL_ADDRESS   AllocPa;            // Control block physical address
+	PNDIS_PACKET pNdisPacket;
+	PNDIS_PACKET pNextNdisPacket;
+
+	RTMP_DMABUF             DmaBuf;             // Associated DMA buffer structure
+} RTMP_DMACB, *PRTMP_DMACB;
+
+typedef struct _RTMP_TX_BUF
+{
+	PQUEUE_ENTRY    Next;
+	UCHAR           Index;
+	ULONG                   AllocSize;          // Control block size
+	PVOID                   AllocVa;            // Control block virtual address
+	NDIS_PHYSICAL_ADDRESS   AllocPa;            // Control block physical address
+} RTMP_TXBUF, *PRTMP_TXBUF;
+
+typedef struct _RTMP_RX_BUF
+{
+	BOOLEAN           InUse;
+	ULONG           	ByBaRecIndex;
+	RTMP_REORDERBUF	MAP_RXBuf[MAX_RX_REORDERBUF];
+} RTMP_RXBUF, *PRTMP_RXBUF;
+typedef struct _RTMP_TX_RING
+{
+	RTMP_DMACB  Cell[TX_RING_SIZE];
+	UINT32		TxCpuIdx;
+	UINT32		TxDmaIdx;
+	UINT32		TxSwFreeIdx; 	// software next free tx index
+} RTMP_TX_RING, *PRTMP_TX_RING;
+
+typedef struct _RTMP_RX_RING
+{
+	RTMP_DMACB  Cell[RX_RING_SIZE];
+	UINT32		RxCpuIdx;
+	UINT32		RxDmaIdx;
+	INT32		RxSwReadIdx; 	// software next read index
+} RTMP_RX_RING, *PRTMP_RX_RING;
+
+typedef struct _RTMP_MGMT_RING
+{
+	RTMP_DMACB  Cell[MGMT_RING_SIZE];
+	UINT32		TxCpuIdx;
+	UINT32		TxDmaIdx;
+	UINT32		TxSwFreeIdx; // software next free tx index
+} RTMP_MGMT_RING, *PRTMP_MGMT_RING;
+
+//
+//  Statistic counter structure
+//
+typedef struct _COUNTER_802_3
+{
+	// General Stats
+	ULONG       GoodTransmits;
+	ULONG       GoodReceives;
+	ULONG       TxErrors;
+	ULONG       RxErrors;
+	ULONG       RxNoBuffer;
+
+	// Ethernet Stats
+	ULONG       RcvAlignmentErrors;
+	ULONG       OneCollision;
+	ULONG       MoreCollisions;
+
+} COUNTER_802_3, *PCOUNTER_802_3;
+
+typedef struct _COUNTER_802_11 {
+	ULONG           Length;
+	LARGE_INTEGER   LastTransmittedFragmentCount;
+	LARGE_INTEGER   TransmittedFragmentCount;
+	LARGE_INTEGER   MulticastTransmittedFrameCount;
+	LARGE_INTEGER   FailedCount;
+	LARGE_INTEGER   RetryCount;
+	LARGE_INTEGER   MultipleRetryCount;
+	LARGE_INTEGER   RTSSuccessCount;
+	LARGE_INTEGER   RTSFailureCount;
+	LARGE_INTEGER   ACKFailureCount;
+	LARGE_INTEGER   FrameDuplicateCount;
+	LARGE_INTEGER   ReceivedFragmentCount;
+	LARGE_INTEGER   MulticastReceivedFrameCount;
+	LARGE_INTEGER   FCSErrorCount;
+} COUNTER_802_11, *PCOUNTER_802_11;
+
+typedef struct _COUNTER_RALINK {
+	ULONG           TransmittedByteCount;   // both successful and failure, used to calculate TX throughput
+	ULONG           ReceivedByteCount;      // both CRC okay and CRC error, used to calculate RX throughput
+	ULONG           BeenDisassociatedCount;
+	ULONG           BadCQIAutoRecoveryCount;
+	ULONG           PoorCQIRoamingCount;
+	ULONG           MgmtRingFullCount;
+	ULONG           RxCountSinceLastNULL;
+	ULONG           RxCount;
+	ULONG           RxRingErrCount;
+	ULONG           KickTxCount;
+	ULONG           TxRingErrCount;
+	LARGE_INTEGER   RealFcsErrCount;
+	ULONG           PendingNdisPacketCount;
+
+	ULONG           OneSecOsTxCount[NUM_OF_TX_RING];
+	ULONG           OneSecDmaDoneCount[NUM_OF_TX_RING];
+	UINT32          OneSecTxDoneCount;
+	ULONG           OneSecRxCount;
+	UINT32          OneSecTxAggregationCount;
+	UINT32          OneSecRxAggregationCount;
+
+	UINT32   		OneSecFrameDuplicateCount;
+
+
+	UINT32          OneSecTxNoRetryOkCount;
+	UINT32          OneSecTxRetryOkCount;
+	UINT32          OneSecTxFailCount;
+	UINT32          OneSecFalseCCACnt;      // CCA error count, for debug purpose, might move to global counter
+	UINT32          OneSecRxOkCnt;          // RX without error
+	UINT32          OneSecRxOkDataCnt;      // unicast-to-me DATA frame count
+	UINT32          OneSecRxFcsErrCnt;      // CRC error
+	UINT32          OneSecBeaconSentCnt;
+	UINT32          LastOneSecTotalTxCount; // OneSecTxNoRetryOkCount + OneSecTxRetryOkCount + OneSecTxFailCount
+	UINT32          LastOneSecRxOkDataCnt;  // OneSecRxOkDataCnt
+	ULONG		DuplicateRcv;
+	ULONG		TxAggCount;
+	ULONG		TxNonAggCount;
+	ULONG		TxAgg1MPDUCount;
+	ULONG		TxAgg2MPDUCount;
+	ULONG		TxAgg3MPDUCount;
+	ULONG		TxAgg4MPDUCount;
+	ULONG		TxAgg5MPDUCount;
+	ULONG		TxAgg6MPDUCount;
+	ULONG		TxAgg7MPDUCount;
+	ULONG		TxAgg8MPDUCount;
+	ULONG		TxAgg9MPDUCount;
+	ULONG		TxAgg10MPDUCount;
+	ULONG		TxAgg11MPDUCount;
+	ULONG		TxAgg12MPDUCount;
+	ULONG		TxAgg13MPDUCount;
+	ULONG		TxAgg14MPDUCount;
+	ULONG		TxAgg15MPDUCount;
+	ULONG		TxAgg16MPDUCount;
+
+	LARGE_INTEGER       TransmittedOctetsInAMSDU;
+	LARGE_INTEGER       TransmittedAMSDUCount;
+	LARGE_INTEGER       ReceivedOctesInAMSDUCount;
+	LARGE_INTEGER       ReceivedAMSDUCount;
+	LARGE_INTEGER       TransmittedAMPDUCount;
+	LARGE_INTEGER       TransmittedMPDUsInAMPDUCount;
+	LARGE_INTEGER       TransmittedOctetsInAMPDUCount;
+	LARGE_INTEGER       MPDUInReceivedAMPDUCount;
+} COUNTER_RALINK, *PCOUNTER_RALINK;
+
+typedef struct _PID_COUNTER {
+	ULONG           TxAckRequiredCount;      // CRC error
+	ULONG           TxAggreCount;
+	ULONG           TxSuccessCount; // OneSecTxNoRetryOkCount + OneSecTxRetryOkCount + OneSecTxFailCount
+	ULONG		LastSuccessRate;
+} PID_COUNTER, *PPID_COUNTER;
+
+typedef struct _COUNTER_DRS {
+	// to record the each TX rate's quality. 0 is best, the bigger the worse.
+	USHORT          TxQuality[MAX_STEP_OF_TX_RATE_SWITCH];
+	UCHAR           PER[MAX_STEP_OF_TX_RATE_SWITCH];
+	UCHAR           TxRateUpPenalty;      // extra # of second penalty due to last unstable condition
+	ULONG           CurrTxRateStableTime; // # of second in current TX rate
+	BOOLEAN         fNoisyEnvironment;
+	BOOLEAN         fLastSecAccordingRSSI;
+	UCHAR           LastSecTxRateChangeAction; // 0: no change, 1:rate UP, 2:rate down
+	UCHAR			LastTimeTxRateChangeAction; //Keep last time value of LastSecTxRateChangeAction
+	ULONG			LastTxOkCount;
+} COUNTER_DRS, *PCOUNTER_DRS;
+
+//
+//  Arcfour Structure Added by PaulWu
+//
+typedef struct  _ARCFOUR
+{
+	UINT            X;
+	UINT            Y;
+	UCHAR           STATE[256];
+} ARCFOURCONTEXT, *PARCFOURCONTEXT;
+
+// MIMO Tx parameter, ShortGI, MCS, STBC, etc.  these are fields in TXWI too. just copy to TXWI.
+typedef struct  _RECEIVE_SETTING {
+#ifdef RT_BIG_ENDIAN
+	USHORT		MIMO:1;
+	USHORT		OFDM:1;
+	USHORT		rsv:3;
+	USHORT		STBC:2;	//SPACE
+	USHORT		ShortGI:1;
+	USHORT		Mode:2;	//channel bandwidth 20MHz or 40 MHz
+	USHORT   	NumOfRX:2;                 // MIMO. WE HAVE 3R
+#else
+	USHORT   	NumOfRX:2;                 // MIMO. WE HAVE 3R
+	USHORT		Mode:2;	//channel bandwidth 20MHz or 40 MHz
+	USHORT		ShortGI:1;
+	USHORT		STBC:2;	//SPACE
+	USHORT		rsv:3;
+	USHORT		OFDM:1;
+	USHORT		MIMO:1;
+#endif
+ } RECEIVE_SETTING, *PRECEIVE_SETTING;
+
+// Shared key data structure
+typedef struct  _WEP_KEY {
+	UCHAR   KeyLen;                     // Key length for each key, 0: entry is invalid
+	UCHAR   Key[MAX_LEN_OF_KEY];        // right now we implement 4 keys, 128 bits max
+} WEP_KEY, *PWEP_KEY;
+
+typedef struct _CIPHER_KEY {
+	UCHAR   Key[16];            // right now we implement 4 keys, 128 bits max
+	UCHAR   RxMic[8];			// make alignment
+	UCHAR   TxMic[8];
+	UCHAR   TxTsc[6];           // 48bit TSC value
+	UCHAR   RxTsc[6];           // 48bit TSC value
+	UCHAR   CipherAlg;          // 0-none, 1:WEP64, 2:WEP128, 3:TKIP, 4:AES, 5:CKIP64, 6:CKIP128
+	UCHAR   KeyLen;
+#ifdef CONFIG_STA_SUPPORT
+	UCHAR   BssId[6];
+#endif // CONFIG_STA_SUPPORT //
+            // Key length for each key, 0: entry is invalid
+	UCHAR   Type;               // Indicate Pairwise/Group when reporting MIC error
+} CIPHER_KEY, *PCIPHER_KEY;
+
+typedef struct _BBP_TUNING_STRUCT {
+	BOOLEAN     Enable;
+	UCHAR       FalseCcaCountUpperBound;  // 100 per sec
+	UCHAR       FalseCcaCountLowerBound;  // 10 per sec
+	UCHAR       R17LowerBound;            // specified in E2PROM
+	UCHAR       R17UpperBound;            // 0x68 according to David Tung
+	UCHAR       CurrentR17Value;
+} BBP_TUNING, *PBBP_TUNING;
+
+typedef struct _SOFT_RX_ANT_DIVERSITY_STRUCT {
+	UCHAR     EvaluatePeriod;		 // 0:not evalute status, 1: evaluate status, 2: switching status
+	UCHAR     Pair1PrimaryRxAnt;     // 0:Ant-E1, 1:Ant-E2
+	UCHAR     Pair1SecondaryRxAnt;   // 0:Ant-E1, 1:Ant-E2
+	UCHAR     Pair2PrimaryRxAnt;     // 0:Ant-E3, 1:Ant-E4
+	UCHAR     Pair2SecondaryRxAnt;   // 0:Ant-E3, 1:Ant-E4
+	SHORT     Pair1AvgRssi[2];       // AvgRssi[0]:E1, AvgRssi[1]:E2
+	SHORT     Pair2AvgRssi[2];       // AvgRssi[0]:E3, AvgRssi[1]:E4
+	SHORT     Pair1LastAvgRssi;      //
+	SHORT     Pair2LastAvgRssi;      //
+	ULONG     RcvPktNumWhenEvaluate;
+	BOOLEAN   FirstPktArrivedWhenEvaluate;
+	RALINK_TIMER_STRUCT    RxAntDiversityTimer;
+} SOFT_RX_ANT_DIVERSITY, *PSOFT_RX_ANT_DIVERSITY;
+
+typedef struct _LEAP_AUTH_INFO {
+	BOOLEAN         Enabled;        //Ture: Enable LEAP Authentication
+	BOOLEAN         CCKM;           //Ture: Use Fast Reauthentication with CCKM
+	UCHAR           Reserve[2];
+	UCHAR           UserName[256];  //LEAP, User name
+	ULONG           UserNameLen;
+	UCHAR           Password[256];  //LEAP, User Password
+	ULONG           PasswordLen;
+} LEAP_AUTH_INFO, *PLEAP_AUTH_INFO;
+
+typedef struct {
+	UCHAR        Addr[MAC_ADDR_LEN];
+	UCHAR        ErrorCode[2];  //00 01-Invalid authentication type
+								//00 02-Authentication timeout
+								//00 03-Challenge from AP failed
+								//00 04-Challenge to AP failed
+	BOOLEAN      Reported;
+} ROGUEAP_ENTRY, *PROGUEAP_ENTRY;
+
+typedef struct {
+	UCHAR               RogueApNr;
+	ROGUEAP_ENTRY       RogueApEntry[MAX_LEN_OF_BSS_TABLE];
+} ROGUEAP_TABLE, *PROGUEAP_TABLE;
+
+typedef struct {
+	BOOLEAN     Enable;
+	UCHAR       Delta;
+	BOOLEAN     PlusSign;
+} CCK_TX_POWER_CALIBRATE, *PCCK_TX_POWER_CALIBRATE;
+
+//
+// Receive Tuple Cache Format
+//
+typedef struct  _TUPLE_CACHE    {
+	BOOLEAN         Valid;
+	UCHAR           MacAddress[MAC_ADDR_LEN];
+	USHORT          Sequence;
+	USHORT          Frag;
+} TUPLE_CACHE, *PTUPLE_CACHE;
+
+//
+// Fragment Frame structure
+//
+typedef struct  _FRAGMENT_FRAME {
+	PNDIS_PACKET    pFragPacket;
+	ULONG       RxSize;
+	USHORT      Sequence;
+	USHORT      LastFrag;
+	ULONG       Flags;          // Some extra frame information. bit 0: LLC presented
+} FRAGMENT_FRAME, *PFRAGMENT_FRAME;
+
+
+//
+// Packet information for NdisQueryPacket
+//
+typedef struct  _PACKET_INFO    {
+	UINT            PhysicalBufferCount;    // Physical breaks of buffer descripor chained
+	UINT            BufferCount ;           // Number of Buffer descriptor chained
+	UINT            TotalPacketLength ;     // Self explained
+	PNDIS_BUFFER    pFirstBuffer;           // Pointer to first buffer descriptor
+} PACKET_INFO, *PPACKET_INFO;
+
+//
+// Tkip Key structure which RC4 key & MIC calculation
+//
+typedef struct  _TKIP_KEY_INFO  {
+	UINT        nBytesInM;  // # bytes in M for MICKEY
+	ULONG       IV16;
+	ULONG       IV32;
+	ULONG       K0;         // for MICKEY Low
+	ULONG       K1;         // for MICKEY Hig
+	ULONG       L;          // Current state for MICKEY
+	ULONG       R;          // Current state for MICKEY
+	ULONG       M;          // Message accumulator for MICKEY
+	UCHAR       RC4KEY[16];
+	UCHAR       MIC[8];
+} TKIP_KEY_INFO, *PTKIP_KEY_INFO;
+
+//
+// Private / Misc data, counters for driver internal use
+//
+typedef struct  __PRIVATE_STRUC {
+	UINT       SystemResetCnt;         // System reset counter
+	UINT       TxRingFullCnt;          // Tx ring full occurrance number
+	UINT       PhyRxErrCnt;            // PHY Rx error count, for debug purpose, might move to global counter
+	// Variables for WEP encryption / decryption in rtmp_wep.c
+	UINT       FCSCRC32;
+	ARCFOURCONTEXT  WEPCONTEXT;
+	// Tkip stuff
+	TKIP_KEY_INFO   Tx;
+	TKIP_KEY_INFO   Rx;
+} PRIVATE_STRUC, *PPRIVATE_STRUC;
+
+// structure to tune BBP R66 (BBP TUNING)
+typedef struct _BBP_R66_TUNING {
+	BOOLEAN     bEnable;
+	USHORT      FalseCcaLowerThreshold;  // default 100
+	USHORT      FalseCcaUpperThreshold;  // default 512
+	UCHAR       R66Delta;
+	UCHAR       R66CurrentValue;
+	BOOLEAN		R66LowerUpperSelect; //Before LinkUp, Used LowerBound or UpperBound as R66 value.
+} BBP_R66_TUNING, *PBBP_R66_TUNING;
+
+// structure to store channel TX power
+typedef struct _CHANNEL_TX_POWER {
+	USHORT     RemainingTimeForUse;		//unit: sec
+	UCHAR      Channel;
+#ifdef DOT11N_DRAFT3
+	BOOLEAN       bEffectedChannel;	// For BW 40 operating in 2.4GHz , the "effected channel" is the channel that is covered in 40Mhz.
+#endif // DOT11N_DRAFT3 //
+	CHAR       Power;
+	CHAR       Power2;
+	UCHAR      MaxTxPwr;
+	UCHAR      DfsReq;
+} CHANNEL_TX_POWER, *PCHANNEL_TX_POWER;
+
+// structure to store 802.11j channel TX power
+typedef struct _CHANNEL_11J_TX_POWER {
+	UCHAR      Channel;
+	UCHAR      BW;	// BW_10 or BW_20
+	CHAR       Power;
+	CHAR       Power2;
+	USHORT     RemainingTimeForUse;		//unit: sec
+} CHANNEL_11J_TX_POWER, *PCHANNEL_11J_TX_POWER;
+
+typedef enum _ABGBAND_STATE_ {
+	UNKNOWN_BAND,
+	BG_BAND,
+	A_BAND,
+} ABGBAND_STATE;
+
+typedef struct _MLME_STRUCT {
+#ifdef CONFIG_STA_SUPPORT
+	// STA state machines
+	STATE_MACHINE           CntlMachine;
+	STATE_MACHINE           AssocMachine;
+	STATE_MACHINE           AuthMachine;
+	STATE_MACHINE           AuthRspMachine;
+	STATE_MACHINE           SyncMachine;
+	STATE_MACHINE           WpaPskMachine;
+	STATE_MACHINE           LeapMachine;
+	STATE_MACHINE           AironetMachine;
+	STATE_MACHINE_FUNC      AssocFunc[ASSOC_FUNC_SIZE];
+	STATE_MACHINE_FUNC      AuthFunc[AUTH_FUNC_SIZE];
+	STATE_MACHINE_FUNC      AuthRspFunc[AUTH_RSP_FUNC_SIZE];
+	STATE_MACHINE_FUNC      SyncFunc[SYNC_FUNC_SIZE];
+	STATE_MACHINE_FUNC      WpaPskFunc[WPA_PSK_FUNC_SIZE];
+	STATE_MACHINE_FUNC      AironetFunc[AIRONET_FUNC_SIZE];
+#endif // CONFIG_STA_SUPPORT //
+	STATE_MACHINE_FUNC      ActFunc[ACT_FUNC_SIZE];
+	// Action
+	STATE_MACHINE           ActMachine;
+
+
+#ifdef QOS_DLS_SUPPORT
+	STATE_MACHINE			DlsMachine;
+	STATE_MACHINE_FUNC      DlsFunc[DLS_FUNC_SIZE];
+#endif // QOS_DLS_SUPPORT //
+
+
+
+
+	ULONG                   ChannelQuality;  // 0..100, Channel Quality Indication for Roaming
+	ULONG                   Now32;           // latch the value of NdisGetSystemUpTime()
+	ULONG                   LastSendNULLpsmTime;
+
+	BOOLEAN                 bRunning;
+	NDIS_SPIN_LOCK          TaskLock;
+	MLME_QUEUE              Queue;
+
+	UINT                    ShiftReg;
+
+	RALINK_TIMER_STRUCT     PeriodicTimer;
+	RALINK_TIMER_STRUCT     APSDPeriodicTimer;
+	RALINK_TIMER_STRUCT     LinkDownTimer;
+	RALINK_TIMER_STRUCT     LinkUpTimer;
+#ifdef RT2860
+    UCHAR                   bPsPollTimerRunning;
+    RALINK_TIMER_STRUCT     PsPollTimer;
+	RALINK_TIMER_STRUCT     RadioOnOffTimer;
+#endif // RT2860 //
+	ULONG                   PeriodicRound;
+	ULONG                   OneSecPeriodicRound;
+
+	UCHAR					RealRxPath;
+	BOOLEAN					bLowThroughput;
+	BOOLEAN					bEnableAutoAntennaCheck;
+	RALINK_TIMER_STRUCT		RxAntEvalTimer;
+
+
+} MLME_STRUCT, *PMLME_STRUCT;
+
+// structure for radar detection and channel switch
+typedef struct _RADAR_DETECT_STRUCT {
+	UCHAR		CSCount;			//Channel switch counter
+	UCHAR		CSPeriod;			//Channel switch period (beacon count)
+	UCHAR		RDCount;			//Radar detection counter
+	UCHAR		RDMode;				//Radar Detection mode
+	UCHAR		RDDurRegion;		//Radar detection duration region
+	UCHAR		BBPR16;
+	UCHAR		BBPR17;
+	UCHAR		BBPR18;
+	UCHAR		BBPR21;
+	UCHAR		BBPR22;
+	UCHAR		BBPR64;
+	ULONG		InServiceMonitorCount; // unit: sec
+	UINT8		DfsSessionTime;
+	BOOLEAN		bFastDfs;
+	UINT8		ChMovingTime;
+	UINT8		LongPulseRadarTh;
+} RADAR_DETECT_STRUCT, *PRADAR_DETECT_STRUCT;
+
+#ifdef CARRIER_DETECTION_SUPPORT
+typedef enum CD_STATE_n
+{
+	CD_NORMAL,
+	CD_SILENCE,
+	CD_MAX_STATE
+} CD_STATE;
+
+typedef struct CARRIER_DETECTION_s
+{
+	BOOLEAN					Enable;
+	UINT8					CDSessionTime;
+	UINT8					CDPeriod;
+	CD_STATE				CD_State;
+} CARRIER_DETECTION, *PCARRIER_DETECTION;
+#endif // CARRIER_DETECTION_SUPPORT //
+
+typedef enum _REC_BLOCKACK_STATUS
+{
+    Recipient_NONE=0,
+	Recipient_USED,
+	Recipient_HandleRes,
+    Recipient_Accept
+} REC_BLOCKACK_STATUS, *PREC_BLOCKACK_STATUS;
+
+typedef enum _ORI_BLOCKACK_STATUS
+{
+    Originator_NONE=0,
+	Originator_USED,
+    Originator_WaitRes,
+    Originator_Done
+} ORI_BLOCKACK_STATUS, *PORI_BLOCKACK_STATUS;
+
+#ifdef DOT11_N_SUPPORT
+typedef struct _BA_ORI_ENTRY{
+	UCHAR   Wcid;
+	UCHAR   TID;
+	UCHAR   BAWinSize;
+	UCHAR   Token;
+// Sequence is to fill every outgoing QoS DATA frame's sequence field in 802.11 header.
+	USHORT	Sequence;
+	USHORT	TimeOutValue;
+	ORI_BLOCKACK_STATUS  ORI_BA_Status;
+	RALINK_TIMER_STRUCT ORIBATimer;
+	PVOID	pAdapter;
+} BA_ORI_ENTRY, *PBA_ORI_ENTRY;
+
+typedef struct _BA_REC_ENTRY {
+	UCHAR   Wcid;
+	UCHAR   TID;
+	UCHAR   BAWinSize;	// 7.3.1.14. each buffer is capable of holding a max AMSDU or MSDU.
+	USHORT		LastIndSeq;
+	USHORT		TimeOutValue;
+	RALINK_TIMER_STRUCT RECBATimer;
+	ULONG		LastIndSeqAtTimer;
+	ULONG		nDropPacket;
+	ULONG		rcvSeq;
+	REC_BLOCKACK_STATUS  REC_BA_Status;
+	NDIS_SPIN_LOCK          RxReRingLock;                 // Rx Ring spinlock
+	PVOID	pAdapter;
+	struct reordering_list	list;
+} BA_REC_ENTRY, *PBA_REC_ENTRY;
+
+
+typedef struct {
+	ULONG		numAsRecipient;		// I am recipient of numAsRecipient clients. These client are in the BARecEntry[]
+	ULONG		numAsOriginator;	// I am originator of 	numAsOriginator clients. These clients are in the BAOriEntry[]
+	BA_ORI_ENTRY       BAOriEntry[MAX_LEN_OF_BA_ORI_TABLE];
+	BA_REC_ENTRY       BARecEntry[MAX_LEN_OF_BA_REC_TABLE];
+} BA_TABLE, *PBA_TABLE;
+
+//For QureyBATableOID use;
+typedef struct  PACKED _OID_BA_REC_ENTRY{
+	UCHAR   MACAddr[MAC_ADDR_LEN];
+	UCHAR   BaBitmap;   // if (BaBitmap&(1<<TID)), this session with{MACAddr, TID}exists, so read BufSize[TID] for BufferSize
+	UCHAR   rsv;
+	UCHAR   BufSize[8];
+	REC_BLOCKACK_STATUS	REC_BA_Status[8];
+} OID_BA_REC_ENTRY, *POID_BA_REC_ENTRY;
+
+//For QureyBATableOID use;
+typedef struct  PACKED _OID_BA_ORI_ENTRY{
+	UCHAR   MACAddr[MAC_ADDR_LEN];
+	UCHAR   BaBitmap;  // if (BaBitmap&(1<<TID)), this session with{MACAddr, TID}exists, so read BufSize[TID] for BufferSize, read ORI_BA_Status[TID] for status
+	UCHAR   rsv;
+	UCHAR   BufSize[8];
+	ORI_BLOCKACK_STATUS  ORI_BA_Status[8];
+} OID_BA_ORI_ENTRY, *POID_BA_ORI_ENTRY;
+
+typedef struct _QUERYBA_TABLE{
+	OID_BA_ORI_ENTRY       BAOriEntry[32];
+	OID_BA_REC_ENTRY       BARecEntry[32];
+	UCHAR   OriNum;// Number of below BAOriEntry
+	UCHAR   RecNum;// Number of below BARecEntry
+} QUERYBA_TABLE, *PQUERYBA_TABLE;
+
+typedef	union	_BACAP_STRUC	{
+#ifdef RT_BIG_ENDIAN
+	struct	{
+		UINT32     :4;
+		UINT32     b2040CoexistScanSup:1;		//As Sta, support do 2040 coexistence scan for AP. As Ap, support monitor trigger event to check if can use BW 40MHz.
+		UINT32     bHtAdhoc:1;			// adhoc can use ht rate.
+		UINT32     MMPSmode:2;	// MIMO power save more, 0:static, 1:dynamic, 2:rsv, 3:mimo enable
+		UINT32     AmsduSize:1;	// 0:3839, 1:7935 bytes. UINT  MSDUSizeToBytes[]	= { 3839, 7935};
+		UINT32     AmsduEnable:1;	//Enable AMSDU transmisstion
+		UINT32		MpduDensity:3;
+		UINT32		Policy:2;	// 0: DELAY_BA 1:IMMED_BA  (//BA Policy subfiled value in ADDBA frame)   2:BA-not use
+		UINT32		AutoBA:1;	// automatically BA
+		UINT32		TxBAWinLimit:8;
+		UINT32		RxBAWinLimit:8;
+	}	field;
+#else
+	struct	{
+		UINT32		RxBAWinLimit:8;
+		UINT32		TxBAWinLimit:8;
+		UINT32		AutoBA:1;	// automatically BA
+		UINT32		Policy:2;	// 0: DELAY_BA 1:IMMED_BA  (//BA Policy subfiled value in ADDBA frame)   2:BA-not use
+		UINT32		MpduDensity:3;
+		UINT32       	AmsduEnable:1;	//Enable AMSDU transmisstion
+		UINT32       	AmsduSize:1;	// 0:3839, 1:7935 bytes. UINT  MSDUSizeToBytes[]	= { 3839, 7935};
+		UINT32       	MMPSmode:2;	// MIMO power save more, 0:static, 1:dynamic, 2:rsv, 3:mimo enable
+		UINT32       	bHtAdhoc:1;			// adhoc can use ht rate.
+		UINT32       	b2040CoexistScanSup:1;		//As Sta, support do 2040 coexistence scan for AP. As Ap, support monitor trigger event to check if can use BW 40MHz.
+		UINT32       	:4;
+	}	field;
+#endif
+	UINT32			word;
+} BACAP_STRUC, *PBACAP_STRUC;
+#endif // DOT11_N_SUPPORT //
+
+//This structure is for all 802.11n card InterOptibilityTest action. Reset all Num every n second.  (Details see MLMEPeriodic)
+typedef	struct	_IOT_STRUC	{
+	UCHAR			Threshold[2];
+	UCHAR			ReorderTimeOutNum[MAX_LEN_OF_BA_REC_TABLE];	// compare with threshold[0]
+	UCHAR			RefreshNum[MAX_LEN_OF_BA_REC_TABLE];	// compare with threshold[1]
+	ULONG			OneSecInWindowCount;
+	ULONG			OneSecFrameDuplicateCount;
+	ULONG			OneSecOutWindowCount;
+	UCHAR			DelOriAct;
+	UCHAR			DelRecAct;
+	UCHAR			RTSShortProt;
+	UCHAR			RTSLongProt;
+	BOOLEAN			bRTSLongProtOn;
+#ifdef CONFIG_STA_SUPPORT
+	BOOLEAN			bLastAtheros;
+    BOOLEAN			bCurrentAtheros;
+    BOOLEAN         bNowAtherosBurstOn;
+	BOOLEAN			bNextDisableRxBA;
+    BOOLEAN			bToggle;
+#endif // CONFIG_STA_SUPPORT //
+} IOT_STRUC, *PIOT_STRUC;
+
+// This is the registry setting for 802.11n transmit setting.  Used in advanced page.
+typedef union _REG_TRANSMIT_SETTING {
+#ifdef RT_BIG_ENDIAN
+ struct {
+         UINT32  rsv:13;
+		 UINT32  EXTCHA:2;
+		 UINT32  HTMODE:1;
+		 UINT32  TRANSNO:2;
+		 UINT32  STBC:1; //SPACE
+		 UINT32  ShortGI:1;
+		 UINT32  BW:1; //channel bandwidth 20MHz or 40 MHz
+		 UINT32  TxBF:1; // 3*3
+		 UINT32  rsv0:10;
+    } field;
+#else
+ struct {
+		 UINT32  rsv0:10;
+		 UINT32  TxBF:1;
+         UINT32  BW:1; //channel bandwidth 20MHz or 40 MHz
+         UINT32  ShortGI:1;
+         UINT32  STBC:1; //SPACE
+         UINT32  TRANSNO:2;
+         UINT32  HTMODE:1;
+         UINT32  EXTCHA:2;
+         UINT32  rsv:13;
+    } field;
+#endif
+ UINT32   word;
+} REG_TRANSMIT_SETTING, *PREG_TRANSMIT_SETTING;
+
+typedef union  _DESIRED_TRANSMIT_SETTING {
+#ifdef RT_BIG_ENDIAN
+	struct	{
+			USHORT		rsv:3;
+			USHORT		FixedTxMode:2;			// If MCS isn't AUTO, fix rate in CCK, OFDM or HT mode.
+			USHORT		PhyMode:4;
+			USHORT   	MCS:7;                 // MCS
+	}	field;
+#else
+	struct	{
+			USHORT   	MCS:7;                 	// MCS
+			USHORT		PhyMode:4;
+			USHORT	 	FixedTxMode:2;			// If MCS isn't AUTO, fix rate in CCK, OFDM or HT mode.
+			USHORT		rsv:3;
+	}	field;
+#endif
+	USHORT		word;
+ } DESIRED_TRANSMIT_SETTING, *PDESIRED_TRANSMIT_SETTING;
+
+typedef struct {
+	BOOLEAN		IsRecipient;
+	UCHAR   MACAddr[MAC_ADDR_LEN];
+	UCHAR   TID;
+	UCHAR   nMSDU;
+	USHORT   TimeOut;
+	BOOLEAN bAllTid;  // If True, delete all TID for BA sessions with this MACaddr.
+} OID_ADD_BA_ENTRY, *POID_ADD_BA_ENTRY;
+
+//
+// Multiple SSID structure
+//
+#define WLAN_MAX_NUM_OF_TIM			((MAX_LEN_OF_MAC_TABLE >> 3) + 1) /* /8 + 1 */
+#define WLAN_CT_TIM_BCMC_OFFSET		0 /* unit: 32B */
+
+/* clear bcmc TIM bit */
+#define WLAN_MR_TIM_BCMC_CLEAR(apidx) \
+	pAd->ApCfg.MBSSID[apidx].TimBitmaps[WLAN_CT_TIM_BCMC_OFFSET] &= ~BIT8[0];
+
+/* set bcmc TIM bit */
+#define WLAN_MR_TIM_BCMC_SET(apidx) \
+	pAd->ApCfg.MBSSID[apidx].TimBitmaps[WLAN_CT_TIM_BCMC_OFFSET] |= BIT8[0];
+
+/* clear a station PS TIM bit */
+#define WLAN_MR_TIM_BIT_CLEAR(ad_p, apidx, wcid) \
+	{	UCHAR tim_offset = wcid >> 3; \
+		UCHAR bit_offset = wcid & 0x7; \
+		ad_p->ApCfg.MBSSID[apidx].TimBitmaps[tim_offset] &= (~BIT8[bit_offset]); }
+
+/* set a station PS TIM bit */
+#define WLAN_MR_TIM_BIT_SET(ad_p, apidx, wcid) \
+	{	UCHAR tim_offset = wcid >> 3; \
+		UCHAR bit_offset = wcid & 0x7; \
+		ad_p->ApCfg.MBSSID[apidx].TimBitmaps[tim_offset] |= BIT8[bit_offset]; }
+
+
+typedef struct _MULTISSID_STRUCT {
+	UCHAR								Bssid[MAC_ADDR_LEN];
+    UCHAR                               SsidLen;
+    CHAR                                Ssid[MAX_LEN_OF_SSID];
+    USHORT                              CapabilityInfo;
+
+    PNET_DEV                   			MSSIDDev;
+
+	NDIS_802_11_AUTHENTICATION_MODE     AuthMode;
+	NDIS_802_11_WEP_STATUS              WepStatus;
+	NDIS_802_11_WEP_STATUS				GroupKeyWepStatus;
+	WPA_MIX_PAIR_CIPHER					WpaMixPairCipher;
+
+	ULONG								TxCount;
+	ULONG								RxCount;
+	ULONG								ReceivedByteCount;
+	ULONG								TransmittedByteCount;
+	ULONG								RxErrorCount;
+	ULONG								RxDropCount;
+
+	HTTRANSMIT_SETTING					HTPhyMode, MaxHTPhyMode, MinHTPhyMode;// For transmit phy setting in TXWI.
+	RT_HT_PHY_INFO						DesiredHtPhyInfo;
+	DESIRED_TRANSMIT_SETTING        	DesiredTransmitSetting; // Desired transmit setting. this is for reading registry setting only. not useful.
+	BOOLEAN								bAutoTxRateSwitch;
+
+	UCHAR                               DefaultKeyId;
+
+	UCHAR								TxRate;       // RATE_1, RATE_2, RATE_5_5, RATE_11, ...
+	UCHAR     							DesiredRates[MAX_LEN_OF_SUPPORTED_RATES];// OID_802_11_DESIRED_RATES
+	UCHAR								DesiredRatesIndex;
+	UCHAR     							MaxTxRate;            // RATE_1, RATE_2, RATE_5_5, RATE_11
+
+	UCHAR								TimBitmaps[WLAN_MAX_NUM_OF_TIM];
+
+    // WPA
+    UCHAR                               GMK[32];
+    UCHAR                               PMK[32];
+	UCHAR								GTK[32];
+    BOOLEAN                             IEEE8021X;
+    BOOLEAN                             PreAuth;
+    UCHAR                               GNonce[32];
+    UCHAR                               PortSecured;
+    NDIS_802_11_PRIVACY_FILTER          PrivacyFilter;
+    UCHAR                               BANClass3Data;
+    ULONG                               IsolateInterStaTraffic;
+
+    UCHAR                               RSNIE_Len[2];
+    UCHAR                               RSN_IE[2][MAX_LEN_OF_RSNIE];
+
+
+    UCHAR                   			TimIELocationInBeacon;
+    UCHAR                   			CapabilityInfoLocationInBeacon;
+    // outgoing BEACON frame buffer and corresponding TXWI
+	// PTXWI_STRUC                           BeaconTxWI; //
+    CHAR                                BeaconBuf[MAX_BEACON_SIZE]; // NOTE: BeaconBuf should be 4-byte aligned
+
+    BOOLEAN                             bHideSsid;
+	UINT16								StationKeepAliveTime; // unit: second
+
+    USHORT                              VLAN_VID;
+    USHORT                              VLAN_Priority;
+
+    RT_802_11_ACL						AccessControlList;
+
+	// EDCA Qos
+    BOOLEAN								bWmmCapable;	// 0:disable WMM, 1:enable WMM
+    BOOLEAN								bDLSCapable;	// 0:disable DLS, 1:enable DLS
+
+	UCHAR           					DlsPTK[64];		// Due to windows dirver count on meetinghouse to handle 4-way shake
+
+	// For 802.1x daemon setting per BSS
+	UCHAR								radius_srv_num;
+	RADIUS_SRV_INFO						radius_srv_info[MAX_RADIUS_SRV_NUM];
+
+#ifdef RTL865X_SOC
+	unsigned int						mylinkid;
+#endif
+
+
+	UINT32					RcvdConflictSsidCount;
+	UINT32					RcvdSpoofedAssocRespCount;
+	UINT32					RcvdSpoofedReassocRespCount;
+	UINT32					RcvdSpoofedProbeRespCount;
+	UINT32					RcvdSpoofedBeaconCount;
+	UINT32					RcvdSpoofedDisassocCount;
+	UINT32					RcvdSpoofedAuthCount;
+	UINT32					RcvdSpoofedDeauthCount;
+	UINT32					RcvdSpoofedUnknownMgmtCount;
+	UINT32					RcvdReplayAttackCount;
+
+	CHAR					RssiOfRcvdConflictSsid;
+	CHAR					RssiOfRcvdSpoofedAssocResp;
+	CHAR					RssiOfRcvdSpoofedReassocResp;
+	CHAR					RssiOfRcvdSpoofedProbeResp;
+	CHAR					RssiOfRcvdSpoofedBeacon;
+	CHAR					RssiOfRcvdSpoofedDisassoc;
+	CHAR					RssiOfRcvdSpoofedAuth;
+	CHAR					RssiOfRcvdSpoofedDeauth;
+	CHAR					RssiOfRcvdSpoofedUnknownMgmt;
+	CHAR					RssiOfRcvdReplayAttack;
+
+	BOOLEAN					bBcnSntReq;
+	UCHAR					BcnBufIdx;
+} MULTISSID_STRUCT, *PMULTISSID_STRUCT;
+
+
+
+#ifdef DOT11N_DRAFT3
+typedef enum _BSS2040COEXIST_FLAG{
+	BSS_2040_COEXIST_DISABLE = 0,
+	BSS_2040_COEXIST_TIMER_FIRED  = 1,
+	BSS_2040_COEXIST_INFO_SYNC = 2,
+	BSS_2040_COEXIST_INFO_NOTIFY = 4,
+}BSS2040COEXIST_FLAG;
+#endif // DOT11N_DRAFT3 //
+
+// configuration common to OPMODE_AP as well as OPMODE_STA
+typedef struct _COMMON_CONFIG {
+
+	BOOLEAN		bCountryFlag;
+	UCHAR		CountryCode[3];
+	UCHAR		Geography;
+	UCHAR       CountryRegion;      // Enum of country region, 0:FCC, 1:IC, 2:ETSI, 3:SPAIN, 4:France, 5:MKK, 6:MKK1, 7:Israel
+	UCHAR       CountryRegionForABand;	// Enum of country region for A band
+	UCHAR       PhyMode;            // PHY_11A, PHY_11B, PHY_11BG_MIXED, PHY_ABG_MIXED
+	USHORT      Dsifs;              // in units of usec
+	ULONG       PacketFilter;       // Packet filter for receiving
+
+	CHAR        Ssid[MAX_LEN_OF_SSID]; // NOT NULL-terminated
+	UCHAR       SsidLen;               // the actual ssid length in used
+	UCHAR       LastSsidLen;               // the actual ssid length in used
+	CHAR        LastSsid[MAX_LEN_OF_SSID]; // NOT NULL-terminated
+	UCHAR		LastBssid[MAC_ADDR_LEN];
+
+	UCHAR       Bssid[MAC_ADDR_LEN];
+	USHORT      BeaconPeriod;
+	UCHAR       Channel;
+	UCHAR       CentralChannel;    	// Central Channel when using 40MHz is indicating. not real channel.
+
+	UCHAR       SupRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR       SupRateLen;
+	UCHAR       ExtRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR       ExtRateLen;
+	UCHAR       DesireRate[MAX_LEN_OF_SUPPORTED_RATES];      // OID_802_11_DESIRED_RATES
+	UCHAR       MaxDesiredRate;
+	UCHAR       ExpectedACKRate[MAX_LEN_OF_SUPPORTED_RATES];
+
+	ULONG       BasicRateBitmap;        // backup basic ratebitmap
+
+	BOOLEAN		bAPSDCapable;
+	BOOLEAN		bInServicePeriod;
+	BOOLEAN		bAPSDAC_BE;
+	BOOLEAN		bAPSDAC_BK;
+	BOOLEAN		bAPSDAC_VI;
+	BOOLEAN		bAPSDAC_VO;
+	BOOLEAN		bNeedSendTriggerFrame;
+	BOOLEAN		bAPSDForcePowerSave;	// Force power save mode, should only use in APSD-STAUT
+	ULONG		TriggerTimerCount;
+	UCHAR		MaxSPLength;
+	UCHAR		BBPCurrentBW;	// BW_10, 	BW_20, BW_40
+	REG_TRANSMIT_SETTING        RegTransmitSetting; //registry transmit setting. this is for reading registry setting only. not useful.
+	UCHAR       TxRate;                 // Same value to fill in TXD. TxRate is 6-bit
+	UCHAR       MaxTxRate;              // RATE_1, RATE_2, RATE_5_5, RATE_11
+	UCHAR       TxRateIndex;            // Tx rate index in RateSwitchTable
+	UCHAR       TxRateTableSize;        // Valid Tx rate table size in RateSwitchTable
+	UCHAR       MinTxRate;              // RATE_1, RATE_2, RATE_5_5, RATE_11
+	UCHAR       RtsRate;                // RATE_xxx
+	HTTRANSMIT_SETTING	MlmeTransmit;   // MGMT frame PHY rate setting when operatin at Ht rate.
+	UCHAR       MlmeRate;               // RATE_xxx, used to send MLME frames
+	UCHAR       BasicMlmeRate;          // Default Rate for sending MLME frames
+
+	USHORT      RtsThreshold;           // in unit of BYTE
+	USHORT      FragmentThreshold;      // in unit of BYTE
+
+	UCHAR       TxPower;                // in unit of mW
+	ULONG       TxPowerPercentage;      // 0~100 %
+	ULONG       TxPowerDefault;         // keep for TxPowerPercentage
+
+#ifdef DOT11_N_SUPPORT
+	BACAP_STRUC        BACapability; //   NO USE = 0XFF  ;  IMMED_BA =1  ;  DELAY_BA=0
+	BACAP_STRUC        REGBACapability; //   NO USE = 0XFF  ;  IMMED_BA =1  ;  DELAY_BA=0
+#endif // DOT11_N_SUPPORT //
+	IOT_STRUC		IOTestParm;	// 802.11n InterOpbility Test Parameter;
+	ULONG       TxPreamble;             // Rt802_11PreambleLong, Rt802_11PreambleShort, Rt802_11PreambleAuto
+	BOOLEAN     bUseZeroToDisableFragment;     // Microsoft use 0 as disable
+	ULONG       UseBGProtection;        // 0: auto, 1: always use, 2: always not use
+	BOOLEAN     bUseShortSlotTime;      // 0: disable, 1 - use short slot (9us)
+	BOOLEAN     bEnableTxBurst;         // 1: enble TX PACKET BURST, 0: disable TX PACKET BURST
+	BOOLEAN     bAggregationCapable;      // 1: enable TX aggregation when the peer supports it
+	BOOLEAN     bPiggyBackCapable;		// 1: enable TX piggy-back according MAC's version
+	BOOLEAN     bIEEE80211H;			// 1: enable IEEE802.11h spec.
+	ULONG		DisableOLBCDetect;		// 0: enable OLBC detect; 1 disable OLBC detect
+
+#ifdef DOT11_N_SUPPORT
+	BOOLEAN				bRdg;
+#endif // DOT11_N_SUPPORT //
+	BOOLEAN             bWmmCapable;        // 0:disable WMM, 1:enable WMM
+	QOS_CAPABILITY_PARM APQosCapability;    // QOS capability of the current associated AP
+	EDCA_PARM           APEdcaParm;         // EDCA parameters of the current associated AP
+	QBSS_LOAD_PARM      APQbssLoad;         // QBSS load of the current associated AP
+	UCHAR               AckPolicy[4];       // ACK policy of the specified AC. see ACK_xxx
+#ifdef CONFIG_STA_SUPPORT
+	BOOLEAN				bDLSCapable;		// 0:disable DLS, 1:enable DLS
+#endif // CONFIG_STA_SUPPORT //
+	// a bitmap of BOOLEAN flags. each bit represent an operation status of a particular
+	// BOOLEAN control, either ON or OFF. These flags should always be accessed via
+	// OPSTATUS_TEST_FLAG(), OPSTATUS_SET_FLAG(), OP_STATUS_CLEAR_FLAG() macros.
+	// see fOP_STATUS_xxx in RTMP_DEF.C for detail bit definition
+	ULONG               OpStatusFlags;
+
+	BOOLEAN				NdisRadioStateOff; //For HCT 12.0, set this flag to TRUE instead of called MlmeRadioOff.
+	ABGBAND_STATE		BandState;		// For setting BBP used on B/G or A mode.
+
+	// IEEE802.11H--DFS.
+	RADAR_DETECT_STRUCT	RadarDetect;
+
+#ifdef CARRIER_DETECTION_SUPPORT
+	CARRIER_DETECTION		CarrierDetect;
+#endif // CARRIER_DETECTION_SUPPORT //
+
+#ifdef DOT11_N_SUPPORT
+	// HT
+	UCHAR			BASize;		// USer desired BAWindowSize. Should not exceed our max capability
+	//RT_HT_CAPABILITY	SupportedHtPhy;
+	RT_HT_CAPABILITY	DesiredHtPhy;
+	HT_CAPABILITY_IE		HtCapability;
+	ADD_HT_INFO_IE		AddHTInfo;	// Useful as AP.
+	//This IE is used with channel switch announcement element when changing to a new 40MHz.
+	//This IE is included in channel switch ammouncement frames 7.4.1.5, beacons, probe Rsp.
+	NEW_EXT_CHAN_IE	NewExtChanOffset;	//7.3.2.20A, 1 if extension channel is above the control channel, 3 if below, 0 if not present
+
+#ifdef DOT11N_DRAFT3
+	UCHAR					Bss2040CoexistFlag;		// bit 0: bBssCoexistTimerRunning, bit 1: NeedSyncAddHtInfo.
+	RALINK_TIMER_STRUCT	Bss2040CoexistTimer;
+
+	//This IE is used for 20/40 BSS Coexistence.
+	BSS_2040_COEXIST_IE		BSS2040CoexistInfo;
+	// ====== 11n D3.0 =======================>
+	USHORT					Dot11OBssScanPassiveDwell;				// Unit : TU. 5~1000
+	USHORT					Dot11OBssScanActiveDwell;				// Unit : TU. 10~1000
+	USHORT					Dot11BssWidthTriggerScanInt;			// Unit : Second
+	USHORT					Dot11OBssScanPassiveTotalPerChannel;	// Unit : TU. 200~10000
+	USHORT					Dot11OBssScanActiveTotalPerChannel;	// Unit : TU. 20~10000
+	USHORT					Dot11BssWidthChanTranDelayFactor;
+	USHORT					Dot11OBssScanActivityThre;				// Unit : percentage
+
+	ULONG					Dot11BssWidthChanTranDelay;			// multiple of (Dot11BssWidthTriggerScanInt * Dot11BssWidthChanTranDelayFactor)
+	ULONG					CountDownCtr;	// CountDown Counter from (Dot11BssWidthTriggerScanInt * Dot11BssWidthChanTranDelayFactor)
+
+	NDIS_SPIN_LOCK          TriggerEventTabLock;
+	BSS_2040_COEXIST_IE		LastBSSCoexist2040;
+	BSS_2040_COEXIST_IE		BSSCoexist2040;
+	TRIGGER_EVENT_TAB		TriggerEventTab;
+	UCHAR					ChannelListIdx;
+	// <====== 11n D3.0 =======================
+	BOOLEAN					bOverlapScanning;
+#endif // DOT11N_DRAFT3 //
+
+    BOOLEAN                 bHTProtect;
+    BOOLEAN                 bMIMOPSEnable;
+    BOOLEAN					bBADecline;
+	BOOLEAN					bDisableReordering;
+	BOOLEAN					bForty_Mhz_Intolerant;
+	BOOLEAN					bExtChannelSwitchAnnouncement;
+	BOOLEAN					bRcvBSSWidthTriggerEvents;
+	ULONG					LastRcvBSSWidthTriggerEventsTime;
+
+	UCHAR					TxBASize;
+#endif // DOT11_N_SUPPORT //
+
+	// Enable wireless event
+	BOOLEAN				bWirelessEvent;
+	BOOLEAN				bWiFiTest;				// Enable this parameter for WiFi test
+
+	// Tx & Rx Stream number selection
+	UCHAR				TxStream;
+	UCHAR				RxStream;
+
+	// transmit phy mode, trasmit rate for Multicast.
+#ifdef MCAST_RATE_SPECIFIC
+	UCHAR				McastTransmitMcs;
+	UCHAR				McastTransmitPhyMode;
+#endif // MCAST_RATE_SPECIFIC //
+
+	BOOLEAN     		bHardwareRadio;     // Hardware controlled Radio enabled
+
+
+
+ 	NDIS_SPIN_LOCK			MeasureReqTabLock;
+	PMEASURE_REQ_TAB		pMeasureReqTab;
+
+	NDIS_SPIN_LOCK			TpcReqTabLock;
+	PTPC_REQ_TAB			pTpcReqTab;
+
+	// transmit phy mode, trasmit rate for Multicast.
+#ifdef MCAST_RATE_SPECIFIC
+	HTTRANSMIT_SETTING		MCastPhyMode;
+#endif // MCAST_RATE_SPECIFIC //
+
+#ifdef SINGLE_SKU
+	UINT16					DefineMaxTxPwr;
+#endif // SINGLE_SKU //
+
+
+} COMMON_CONFIG, *PCOMMON_CONFIG;
+
+
+#ifdef CONFIG_STA_SUPPORT
+/* Modified by Wu Xi-Kun 4/21/2006 */
+// STA configuration and status
+typedef struct _STA_ADMIN_CONFIG {
+	// GROUP 1 -
+	//   User configuration loaded from Registry, E2PROM or OID_xxx. These settings describe
+	//   the user intended configuration, but not necessary fully equal to the final
+	//   settings in ACTIVE BSS after negotiation/compromize with the BSS holder (either
+	//   AP or IBSS holder).
+	//   Once initialized, user configuration can only be changed via OID_xxx
+	UCHAR       BssType;              // BSS_INFRA or BSS_ADHOC
+	USHORT      AtimWin;          // used when starting a new IBSS
+
+	// GROUP 2 -
+	//   User configuration loaded from Registry, E2PROM or OID_xxx. These settings describe
+	//   the user intended configuration, and should be always applied to the final
+	//   settings in ACTIVE BSS without compromising with the BSS holder.
+	//   Once initialized, user configuration can only be changed via OID_xxx
+	UCHAR       RssiTrigger;
+	UCHAR       RssiTriggerMode;      // RSSI_TRIGGERED_UPON_BELOW_THRESHOLD or RSSI_TRIGGERED_UPON_EXCCEED_THRESHOLD
+	USHORT      DefaultListenCount;   // default listen count;
+	ULONG       WindowsPowerMode;           // Power mode for AC power
+	ULONG       WindowsBatteryPowerMode;    // Power mode for battery if exists
+	BOOLEAN     bWindowsACCAMEnable;        // Enable CAM power mode when AC on
+	BOOLEAN     bAutoReconnect;         // Set to TRUE when setting OID_802_11_SSID with no matching BSSID
+	ULONG       WindowsPowerProfile;    // Windows power profile, for NDIS5.1 PnP
+
+	// MIB:ieee802dot11.dot11smt(1).dot11StationConfigTable(1)
+	USHORT      Psm;                  // power management mode   (PWR_ACTIVE|PWR_SAVE)
+	USHORT      DisassocReason;
+	UCHAR       DisassocSta[MAC_ADDR_LEN];
+	USHORT      DeauthReason;
+	UCHAR       DeauthSta[MAC_ADDR_LEN];
+	USHORT      AuthFailReason;
+	UCHAR       AuthFailSta[MAC_ADDR_LEN];
+
+	NDIS_802_11_PRIVACY_FILTER          PrivacyFilter;  // PrivacyFilter enum for 802.1X
+	NDIS_802_11_AUTHENTICATION_MODE     AuthMode;       // This should match to whatever microsoft defined
+	NDIS_802_11_WEP_STATUS              WepStatus;
+	NDIS_802_11_WEP_STATUS				OrigWepStatus;	// Original wep status set from OID
+
+	// Add to support different cipher suite for WPA2/WPA mode
+	NDIS_802_11_ENCRYPTION_STATUS		GroupCipher;		// Multicast cipher suite
+	NDIS_802_11_ENCRYPTION_STATUS		PairCipher;			// Unicast cipher suite
+	BOOLEAN								bMixCipher;			// Indicate current Pair & Group use different cipher suites
+	USHORT								RsnCapability;
+
+	NDIS_802_11_WEP_STATUS              GroupKeyWepStatus;
+
+	UCHAR		PMK[32];                // WPA PSK mode PMK
+	UCHAR       PTK[64];                // WPA PSK mode PTK
+	UCHAR		GTK[32];				// GTK from authenticator
+	BSSID_INFO	SavedPMK[PMKID_NO];
+	UINT		SavedPMKNum;			// Saved PMKID number
+
+	UCHAR		DefaultKeyId;
+
+
+	// WPA 802.1x port control, WPA_802_1X_PORT_SECURED, WPA_802_1X_PORT_NOT_SECURED
+	UCHAR       PortSecured;
+
+	// For WPA countermeasures
+	ULONG       LastMicErrorTime;   // record last MIC error time
+	ULONG       MicErrCnt;          // Should be 0, 1, 2, then reset to zero (after disassoiciation).
+	BOOLEAN     bBlockAssoc;        // Block associate attempt for 60 seconds after counter measure occurred.
+	// For WPA-PSK supplicant state
+	WPA_STATE   WpaState;           // Default is SS_NOTUSE and handled by microsoft 802.1x
+	UCHAR       ReplayCounter[8];
+	UCHAR       ANonce[32];         // ANonce for WPA-PSK from aurhenticator
+	UCHAR       SNonce[32];         // SNonce for WPA-PSK
+
+	UCHAR       LastSNR0;             // last received BEACON's SNR
+	UCHAR       LastSNR1;            // last received BEACON's SNR for 2nd  antenna
+	RSSI_SAMPLE RssiSample;
+	ULONG       NumOfAvgRssiSample;
+
+	ULONG       LastBeaconRxTime;     // OS's timestamp of the last BEACON RX time
+	ULONG       Last11bBeaconRxTime;  // OS's timestamp of the last 11B BEACON RX time
+	ULONG		Last11gBeaconRxTime;	// OS's timestamp of the last 11G BEACON RX time
+	ULONG		Last20NBeaconRxTime;	// OS's timestamp of the last 20MHz N BEACON RX time
+
+	ULONG       LastScanTime;       // Record last scan time for issue BSSID_SCAN_LIST
+	ULONG       ScanCnt;            // Scan counts since most recent SSID, BSSID, SCAN OID request
+	BOOLEAN     bSwRadio;           // Software controlled Radio On/Off, TRUE: On
+	BOOLEAN     bHwRadio;           // Hardware controlled Radio On/Off, TRUE: On
+	BOOLEAN     bRadio;             // Radio state, And of Sw & Hw radio state
+	BOOLEAN     bHardwareRadio;     // Hardware controlled Radio enabled
+	BOOLEAN     bShowHiddenSSID;    // Show all known SSID in SSID list get operation
+
+    BOOLEAN		AdhocBOnlyJoined;	// Indicate Adhoc B Join.
+    BOOLEAN		AdhocBGJoined;		// Indicate Adhoc B/G Join.
+    BOOLEAN		Adhoc20NJoined;		// Indicate Adhoc 20MHz N Join.
+
+	// New for WPA, windows want us to to keep association information and
+	// Fixed IEs from last association response
+	NDIS_802_11_ASSOCIATION_INFORMATION     AssocInfo;
+	USHORT       ReqVarIELen;                // Length of next VIE include EID & Length
+	UCHAR       ReqVarIEs[MAX_VIE_LEN];		// The content saved here should be little-endian format.
+	USHORT       ResVarIELen;                // Length of next VIE include EID & Length
+	UCHAR       ResVarIEs[MAX_VIE_LEN];
+
+	UCHAR       RSNIE_Len;
+	UCHAR       RSN_IE[MAX_LEN_OF_RSNIE];	// The content saved here should be little-endian format.
+
+	// New variables used for CCX 1.0
+	BOOLEAN             bCkipOn;
+	BOOLEAN             bCkipCmicOn;
+	UCHAR               CkipFlag;
+	UCHAR               GIV[3];  //for CCX iv
+	UCHAR               RxSEQ[4];
+	UCHAR               TxSEQ[4];
+	UCHAR               CKIPMIC[4];
+	UCHAR               LeapAuthMode;
+	LEAP_AUTH_INFO      LeapAuthInfo;
+	UCHAR               HashPwd[16];
+	UCHAR               NetworkChallenge[8];
+	UCHAR               NetworkChallengeResponse[24];
+	UCHAR               PeerChallenge[8];
+
+	UCHAR               PeerChallengeResponse[24];
+	UCHAR               SessionKey[16]; //Network session keys (NSK)
+	RALINK_TIMER_STRUCT LeapAuthTimer;
+	ROGUEAP_TABLE       RogueApTab;   //Cisco CCX1 Rogue AP Detection
+
+	// New control flags for CCX
+	CCX_CONTROL         CCXControl;                 // Master administration state
+	BOOLEAN             CCXEnable;                  // Actual CCX state
+	UCHAR               CCXScanChannel;             // Selected channel for CCX beacon request
+	USHORT              CCXScanTime;                // Time out to wait for beacon and probe response
+	UCHAR               CCXReqType;                 // Current processing CCX request type
+	BSS_TABLE           CCXBssTab;                  // BSS Table
+	UCHAR               FrameReportBuf[2048];       // Buffer for creating frame report
+	USHORT              FrameReportLen;             // Current Frame report length
+	ULONG               CLBusyBytes;                // Save the total bytes received durning channel load scan time
+	USHORT              RPIDensity[8];              // Array for RPI density collection
+	// Start address of each BSS table within FrameReportBuf
+	// It's important to update the RxPower of the corresponding Bss
+	USHORT              BssReportOffset[MAX_LEN_OF_BSS_TABLE];
+	USHORT              BeaconToken;                // Token for beacon report
+	ULONG               LastBssIndex;               // Most current reported Bss index
+	RM_REQUEST_ACTION   MeasurementRequest[16];     // Saved measurement request
+	UCHAR               RMReqCnt;                   // Number of measurement request saved.
+	UCHAR               CurrentRMReqIdx;            // Number of measurement request saved.
+	BOOLEAN             ParallelReq;                // Parallel measurement, only one request performed,
+													// It must be the same channel with maximum duration
+	USHORT              ParallelDuration;           // Maximum duration for parallel measurement
+	UCHAR               ParallelChannel;            // Only one channel with parallel measurement
+	USHORT              IAPPToken;                  // IAPP dialog token
+	UCHAR               CCXQosECWMin;               // Cisco QOS ECWMin for AC 0
+	UCHAR               CCXQosECWMax;               // Cisco QOS ECWMax for AC 0
+	// Hack for channel load and noise histogram parameters
+	UCHAR               NHFactor;                   // Parameter for Noise histogram
+	UCHAR               CLFactor;                   // Parameter for channel load
+
+	UCHAR               KRK[16];        //Key Refresh Key.
+	UCHAR               BTK[32];        //Base Transient Key
+	BOOLEAN             CCKMLinkUpFlag;
+	ULONG               CCKMRN;    //(Re)Association request number.
+	LARGE_INTEGER       CCKMBeaconAtJoinTimeStamp;  //TSF timer for Re-assocaite to the new AP
+	UCHAR               AironetCellPowerLimit;      //in dBm
+	UCHAR               AironetIPAddress[4];        //eg. 192.168.1.1
+	BOOLEAN             CCXAdjacentAPReportFlag;    //flag for determining report Assoc Lost time
+	CHAR                CCXAdjacentAPSsid[MAX_LEN_OF_SSID]; //Adjacent AP's SSID report
+	UCHAR               CCXAdjacentAPSsidLen;               // the actual ssid length in used
+	UCHAR               CCXAdjacentAPBssid[MAC_ADDR_LEN];         //Adjacent AP's BSSID report
+	USHORT              CCXAdjacentAPChannel;
+	ULONG               CCXAdjacentAPLinkDownTime;  //for Spec S32.
+
+	RALINK_TIMER_STRUCT	StaQuickResponeForRateUpTimer;
+	BOOLEAN				StaQuickResponeForRateUpTimerRunning;
+
+	UCHAR           	DtimCount;      // 0.. DtimPeriod-1
+	UCHAR           	DtimPeriod;     // default = 3
+
+#ifdef QOS_DLS_SUPPORT
+	RT_802_11_DLS		DLSEntry[MAX_NUM_OF_DLS_ENTRY];
+	UCHAR				DlsReplayCounter[8];
+#endif // QOS_DLS_SUPPORT //
+	////////////////////////////////////////////////////////////////////////////////////////
+	// This is only for WHQL test.
+	BOOLEAN				WhqlTest;
+	////////////////////////////////////////////////////////////////////////////////////////
+
+    RALINK_TIMER_STRUCT WpaDisassocAndBlockAssocTimer;
+    // Fast Roaming
+	BOOLEAN		        bFastRoaming;       // 0:disable fast roaming, 1:enable fast roaming
+	CHAR		        dBmToRoam;          // the condition to roam when receiving Rssi less than this value. It's negative value.
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+    BOOLEAN             IEEE8021X;
+    BOOLEAN             IEEE8021x_required_keys;
+    CIPHER_KEY	        DesireSharedKey[4];	// Record user desired WEP keys
+    UCHAR               DesireSharedKeyId;
+
+    // 0: driver ignores wpa_supplicant
+    // 1: wpa_supplicant initiates scanning and AP selection
+    // 2: driver takes care of scanning, AP selection, and IEEE 802.11 association parameters
+    UCHAR               WpaSupplicantUP;
+	UCHAR				WpaSupplicantScanCount;
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+    CHAR                dev_name[16];
+    USHORT              OriDevType;
+
+    BOOLEAN             bTGnWifiTest;
+	BOOLEAN			    bScanReqIsFromWebUI;
+
+	HTTRANSMIT_SETTING				HTPhyMode, MaxHTPhyMode, MinHTPhyMode;// For transmit phy setting in TXWI.
+	DESIRED_TRANSMIT_SETTING       	DesiredTransmitSetting;
+	RT_HT_PHY_INFO					DesiredHtPhyInfo;
+	BOOLEAN							bAutoTxRateSwitch;
+
+#ifdef RT2860
+    UCHAR       BBPR3;
+#endif // RT2860 //
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+	UCHAR				IEEE80211dClientMode;
+	UCHAR				StaOriCountryCode[3];
+	UCHAR				StaOriGeography;
+#endif // EXT_BUILD_CHANNEL_LIST //
+} STA_ADMIN_CONFIG, *PSTA_ADMIN_CONFIG;
+
+// This data structure keep the current active BSS/IBSS's configuration that this STA
+// had agreed upon joining the network. Which means these parameters are usually decided
+// by the BSS/IBSS creator instead of user configuration. Data in this data structurre
+// is valid only when either ADHOC_ON(pAd) or INFRA_ON(pAd) is TRUE.
+// Normally, after SCAN or failed roaming attempts, we need to recover back to
+// the current active settings.
+typedef struct _STA_ACTIVE_CONFIG {
+	USHORT      Aid;
+	USHORT      AtimWin;                // in kusec; IBSS parameter set element
+	USHORT      CapabilityInfo;
+	USHORT      CfpMaxDuration;
+	USHORT      CfpPeriod;
+
+	// Copy supported rate from desired AP's beacon. We are trying to match
+	// AP's supported and extended rate settings.
+	UCHAR       SupRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR       ExtRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR       SupRateLen;
+	UCHAR       ExtRateLen;
+	// Copy supported ht from desired AP's beacon. We are trying to match
+	RT_HT_PHY_INFO		SupportedPhyInfo;
+	RT_HT_CAPABILITY	SupportedHtPhy;
+} STA_ACTIVE_CONFIG, *PSTA_ACTIVE_CONFIG;
+#endif // CONFIG_STA_SUPPORT //
+
+// ----------- start of AP --------------------------
+// AUTH-RSP State Machine Aux data structure
+typedef struct _AP_MLME_AUX {
+	UCHAR               Addr[MAC_ADDR_LEN];
+	USHORT              Alg;
+	CHAR                Challenge[CIPHER_TEXT_LEN];
+} AP_MLME_AUX, *PAP_MLME_AUX;
+
+// structure to define WPA Group Key Rekey Interval
+typedef struct PACKED _RT_802_11_WPA_REKEY {
+	ULONG ReKeyMethod;          // mechanism for rekeying: 0:disable, 1: time-based, 2: packet-based
+	ULONG ReKeyInterval;        // time-based: seconds, packet-based: kilo-packets
+} RT_WPA_REKEY,*PRT_WPA_REKEY, RT_802_11_WPA_REKEY, *PRT_802_11_WPA_REKEY;
+
+typedef struct _MAC_TABLE_ENTRY {
+	//Choose 1 from ValidAsWDS and ValidAsCLI  to validize.
+	BOOLEAN		ValidAsCLI;		// Sta mode, set this TRUE after Linkup,too.
+	BOOLEAN		ValidAsWDS;	// This is WDS Entry. only for AP mode.
+	BOOLEAN		ValidAsApCli;   //This is a AP-Client entry, only for AP mode which enable AP-Client functions.
+	BOOLEAN		ValidAsMesh;
+	BOOLEAN		ValidAsDls;	// This is DLS Entry. only for STA mode.
+	BOOLEAN		isCached;
+	BOOLEAN		bIAmBadAtheros;	// Flag if this is Atheros chip that has IOT problem.  We need to turn on RTS/CTS protection.
+
+	UCHAR         	EnqueueEapolStartTimerRunning;  // Enqueue EAPoL-Start for triggering EAP SM
+	//jan for wpa
+	// record which entry revoke MIC Failure , if it leaves the BSS itself, AP won't update aMICFailTime MIB
+	UCHAR           CMTimerRunning;
+	UCHAR           apidx;			// MBSS number
+	UCHAR           RSNIE_Len;
+	UCHAR           RSN_IE[MAX_LEN_OF_RSNIE];
+	UCHAR           ANonce[LEN_KEY_DESC_NONCE];
+	UCHAR           R_Counter[LEN_KEY_DESC_REPLAY];
+	UCHAR           PTK[64];
+	UCHAR           ReTryCounter;
+	RALINK_TIMER_STRUCT                 RetryTimer;
+	RALINK_TIMER_STRUCT					EnqueueStartForPSKTimer;	// A timer which enqueue EAPoL-Start for triggering PSK SM
+	NDIS_802_11_AUTHENTICATION_MODE     AuthMode;   // This should match to whatever microsoft defined
+	NDIS_802_11_WEP_STATUS              WepStatus;
+	AP_WPA_STATE    WpaState;
+	GTK_STATE       GTKState;
+	USHORT          PortSecured;
+	NDIS_802_11_PRIVACY_FILTER  PrivacyFilter;      // PrivacyFilter enum for 802.1X
+	CIPHER_KEY      PairwiseKey;
+	PVOID           pAd;
+    INT				PMKID_CacheIdx;
+    UCHAR			PMKID[LEN_PMKID];
+
+
+	UCHAR           Addr[MAC_ADDR_LEN];
+	UCHAR           PsMode;
+	SST             Sst;
+	AUTH_STATE      AuthState; // for SHARED KEY authentication state machine used only
+	BOOLEAN			IsReassocSta;	// Indicate whether this is a reassociation procedure
+	USHORT          Aid;
+	USHORT          CapabilityInfo;
+	UCHAR           LastRssi;
+	ULONG           NoDataIdleCount;
+	UINT16			StationKeepAliveCount; // unit: second
+	ULONG           PsQIdleCount;
+	QUEUE_HEADER    PsQueue;
+
+	UINT32			StaConnectTime;		// the live time of this station since associated with AP
+
+
+#ifdef DOT11_N_SUPPORT
+	BOOLEAN			bSendBAR;
+	USHORT			NoBADataCountDown;
+
+	UINT32   		CachedBuf[16];		// UINT (4 bytes) for alignment
+	UINT			TxBFCount; // 3*3
+#endif // DOT11_N_SUPPORT //
+	UINT			FIFOCount;
+	UINT			DebugFIFOCount;
+	UINT			DebugTxCount;
+    BOOLEAN			bDlsInit;
+
+
+//====================================================
+//WDS entry needs these
+// rt2860 add this. if ValidAsWDS==TRUE, MatchWDSTabIdx is the index in WdsTab.MacTab
+	UINT			MatchWDSTabIdx;
+	UCHAR           MaxSupportedRate;
+	UCHAR           CurrTxRate;
+	UCHAR           CurrTxRateIndex;
+	// to record the each TX rate's quality. 0 is best, the bigger the worse.
+	USHORT          TxQuality[MAX_STEP_OF_TX_RATE_SWITCH];
+	UINT32			OneSecTxNoRetryOkCount;
+	UINT32          OneSecTxRetryOkCount;
+	UINT32          OneSecTxFailCount;
+	UINT32			ContinueTxFailCnt;
+	UINT32          CurrTxRateStableTime; // # of second in current TX rate
+	UCHAR           TxRateUpPenalty;      // extra # of second penalty due to last unstable condition
+//====================================================
+
+
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+	UINT			MatchDlsEntryIdx; // indicate the index in pAd->StaCfg.DLSEntry
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+	BOOLEAN         fNoisyEnvironment;
+	BOOLEAN			fLastSecAccordingRSSI;
+	UCHAR           LastSecTxRateChangeAction; // 0: no change, 1:rate UP, 2:rate down
+	CHAR			LastTimeTxRateChangeAction; //Keep last time value of LastSecTxRateChangeAction
+	ULONG			LastTxOkCount;
+	UCHAR           PER[MAX_STEP_OF_TX_RATE_SWITCH];
+
+	// a bitmap of BOOLEAN flags. each bit represent an operation status of a particular
+	// BOOLEAN control, either ON or OFF. These flags should always be accessed via
+	// CLIENT_STATUS_TEST_FLAG(), CLIENT_STATUS_SET_FLAG(), CLIENT_STATUS_CLEAR_FLAG() macros.
+	// see fOP_STATUS_xxx in RTMP_DEF.C for detail bit definition. fCLIENT_STATUS_AMSDU_INUSED
+	ULONG           ClientStatusFlags;
+
+	// TODO: Shall we move that to DOT11_N_SUPPORT???
+	HTTRANSMIT_SETTING	HTPhyMode, MaxHTPhyMode, MinHTPhyMode;// For transmit phy setting in TXWI.
+
+#ifdef DOT11_N_SUPPORT
+	// HT EWC MIMO-N used parameters
+	USHORT		RXBAbitmap;	// fill to on-chip  RXWI_BA_BITMASK in 8.1.3RX attribute entry format
+	USHORT		TXBAbitmap;	// This bitmap as originator, only keep in software used to mark AMPDU bit in TXWI
+	USHORT		TXAutoBAbitmap;
+	USHORT		BADeclineBitmap;
+	USHORT		BARecWcidArray[NUM_OF_TID];	// The mapping wcid of recipient session. if RXBAbitmap bit is masked
+	USHORT		BAOriWcidArray[NUM_OF_TID]; // The mapping wcid of originator session. if TXBAbitmap bit is masked
+	USHORT		BAOriSequence[NUM_OF_TID]; // The mapping wcid of originator session. if TXBAbitmap bit is masked
+
+	// 802.11n features.
+	UCHAR		MpduDensity;
+	UCHAR		MaxRAmpduFactor;
+	UCHAR		AMsduSize;
+	UCHAR		MmpsMode;	// MIMO power save more.
+
+	HT_CAPABILITY_IE		HTCapability;
+
+#ifdef DOT11N_DRAFT3
+	UCHAR		BSS2040CoexistenceMgmtSupport;
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+
+	BOOLEAN		bAutoTxRateSwitch;
+
+	UCHAR       RateLen;
+	struct _MAC_TABLE_ENTRY *pNext;
+    USHORT      TxSeq[NUM_OF_TID];
+	USHORT		NonQosDataSeq;
+
+	RSSI_SAMPLE	RssiSample;
+
+	UINT32			TXMCSExpected[16];
+	UINT32			TXMCSSuccessful[16];
+	UINT32			TXMCSFailed[16];
+	UINT32			TXMCSAutoFallBack[16][16];
+} MAC_TABLE_ENTRY, *PMAC_TABLE_ENTRY;
+
+typedef struct _MAC_TABLE {
+	USHORT			Size;
+	MAC_TABLE_ENTRY *Hash[HASH_TABLE_SIZE];
+	MAC_TABLE_ENTRY Content[MAX_LEN_OF_MAC_TABLE];
+	QUEUE_HEADER    McastPsQueue;
+	ULONG           PsQIdleCount;
+	BOOLEAN         fAnyStationInPsm;
+	BOOLEAN         fAnyStationBadAtheros;	// Check if any Station is atheros 802.11n Chip.  We need to use RTS/CTS with Atheros 802,.11n chip.
+	BOOLEAN			fAnyTxOPForceDisable;	// Check if it is necessary to disable BE TxOP
+#ifdef DOT11_N_SUPPORT
+	BOOLEAN         fAnyStationIsLegacy;	// Check if I use legacy rate to transmit to my BSS Station/
+	BOOLEAN         fAnyStationNonGF;		// Check if any Station can't support GF.
+	BOOLEAN         fAnyStation20Only;		// Check if any Station can't support GF.
+	BOOLEAN			fAnyStationMIMOPSDynamic; // Check if any Station is MIMO Dynamic
+	BOOLEAN         fAnyBASession;   // Check if there is BA session.  Force turn on RTS/CTS
+#endif // DOT11_N_SUPPORT //
+} MAC_TABLE, *PMAC_TABLE;
+
+#ifdef DOT11_N_SUPPORT
+#define IS_HT_STA(_pMacEntry)	\
+	(_pMacEntry->MaxHTPhyMode.field.MODE >= MODE_HTMIX)
+
+#define IS_HT_RATE(_pMacEntry)	\
+	(_pMacEntry->HTPhyMode.field.MODE >= MODE_HTMIX)
+
+#define PEER_IS_HT_RATE(_pMacEntry)	\
+	(_pMacEntry->HTPhyMode.field.MODE >= MODE_HTMIX)
+#endif // DOT11_N_SUPPORT //
+
+typedef struct _WDS_ENTRY {
+	BOOLEAN         Valid;
+	UCHAR           Addr[MAC_ADDR_LEN];
+	ULONG           NoDataIdleCount;
+	struct _WDS_ENTRY *pNext;
+} WDS_ENTRY, *PWDS_ENTRY;
+
+typedef struct  _WDS_TABLE_ENTRY {
+	USHORT			Size;
+	UCHAR           WdsAddr[MAC_ADDR_LEN];
+	WDS_ENTRY       *Hash[HASH_TABLE_SIZE];
+	WDS_ENTRY       Content[MAX_LEN_OF_MAC_TABLE];
+	UCHAR           MaxSupportedRate;
+	UCHAR           CurrTxRate;
+	USHORT          TxQuality[MAX_LEN_OF_SUPPORTED_RATES];
+	USHORT          OneSecTxOkCount;
+	USHORT          OneSecTxRetryOkCount;
+	USHORT          OneSecTxFailCount;
+	ULONG           CurrTxRateStableTime; // # of second in current TX rate
+	UCHAR           TxRateUpPenalty;      // extra # of second penalty due to last unstable condition
+} WDS_TABLE_ENTRY, *PWDS_TABLE_ENTRY;
+
+typedef struct _RT_802_11_WDS_ENTRY {
+	PNET_DEV			dev;
+	UCHAR				Valid;
+	UCHAR				PhyMode;
+	UCHAR				PeerWdsAddr[MAC_ADDR_LEN];
+	UCHAR				MacTabMatchWCID;	// ASIC
+	NDIS_802_11_WEP_STATUS  WepStatus;
+	UCHAR					KeyIdx;
+	CIPHER_KEY          	WdsKey;
+	HTTRANSMIT_SETTING				HTPhyMode, MaxHTPhyMode, MinHTPhyMode;
+	RT_HT_PHY_INFO					DesiredHtPhyInfo;
+	BOOLEAN							bAutoTxRateSwitch;
+	DESIRED_TRANSMIT_SETTING       	DesiredTransmitSetting; // Desired transmit setting.
+} RT_802_11_WDS_ENTRY, *PRT_802_11_WDS_ENTRY;
+
+typedef struct _WDS_TABLE {
+	UCHAR               Mode;
+	ULONG               Size;
+	RT_802_11_WDS_ENTRY	WdsEntry[MAX_WDS_ENTRY];
+} WDS_TABLE, *PWDS_TABLE;
+
+typedef struct _APCLI_STRUCT {
+	PNET_DEV				dev;
+#ifdef RTL865X_SOC
+	unsigned int            mylinkid;
+#endif
+	BOOLEAN                 Enable;	// Set it as 1 if the apcli interface was configured to "1"  or by iwpriv cmd "ApCliEnable"
+	BOOLEAN                 Valid;	// Set it as 1 if the apcli interface associated success to remote AP.
+	UCHAR					MacTabWCID;	//WCID value, which point to the entry of ASIC Mac table.
+	UCHAR                   SsidLen;
+	CHAR                    Ssid[MAX_LEN_OF_SSID];
+
+	UCHAR                   CfgSsidLen;
+	CHAR                    CfgSsid[MAX_LEN_OF_SSID];
+	UCHAR                   CfgApCliBssid[ETH_LENGTH_OF_ADDRESS];
+	UCHAR                   CurrentAddress[ETH_LENGTH_OF_ADDRESS];
+
+	ULONG                   ApCliRcvBeaconTime;
+
+	ULONG                   CtrlCurrState;
+	ULONG                   SyncCurrState;
+	ULONG                   AuthCurrState;
+	ULONG                   AssocCurrState;
+	ULONG					WpaPskCurrState;
+
+	USHORT                  AuthReqCnt;
+	USHORT                  AssocReqCnt;
+
+	ULONG                   ClientStatusFlags;
+	UCHAR                   MpduDensity;
+
+	NDIS_802_11_AUTHENTICATION_MODE     AuthMode;   // This should match to whatever microsoft defined
+	NDIS_802_11_WEP_STATUS              WepStatus;
+
+	// Add to support different cipher suite for WPA2/WPA mode
+	NDIS_802_11_ENCRYPTION_STATUS		GroupCipher;		// Multicast cipher suite
+	NDIS_802_11_ENCRYPTION_STATUS		PairCipher;			// Unicast cipher suite
+	BOOLEAN								bMixCipher;			// Indicate current Pair & Group use different cipher suites
+	USHORT								RsnCapability;
+
+	UCHAR		PSK[100];				// reserve PSK key material
+	UCHAR       PSKLen;
+	UCHAR       PMK[32];                // WPA PSK mode PMK
+	UCHAR		GTK[32];				// GTK from authenticator
+
+	CIPHER_KEY      SharedKey[SHARE_KEY_NUM];
+	UCHAR           DefaultKeyId;
+
+	// store RSN_IE built by driver
+	UCHAR		RSN_IE[MAX_LEN_OF_RSNIE];  // The content saved here should be convert to little-endian format.
+	UCHAR		RSNIE_Len;
+
+	// For WPA countermeasures
+	ULONG       LastMicErrorTime;   // record last MIC error time
+	BOOLEAN                 bBlockAssoc; // Block associate attempt for 60 seconds after counter measure occurred.
+
+	// For WPA-PSK supplicant state
+	UCHAR       	SNonce[32];         // SNonce for WPA-PSK
+	UCHAR			GNonce[32];			// GNonce for WPA-PSK from authenticator
+
+#ifdef WSC_AP_SUPPORT
+	WSC_CTRL	           WscControl;
+#endif // WSC_AP_SUPPORT //
+
+	HTTRANSMIT_SETTING				HTPhyMode, MaxHTPhyMode, MinHTPhyMode;
+	RT_HT_PHY_INFO					DesiredHtPhyInfo;
+	BOOLEAN							bAutoTxRateSwitch;
+	DESIRED_TRANSMIT_SETTING       	DesiredTransmitSetting; // Desired transmit setting.
+} APCLI_STRUCT, *PAPCLI_STRUCT;
+
+// ----------- end of AP ----------------------------
+
+#ifdef BLOCK_NET_IF
+typedef struct _BLOCK_QUEUE_ENTRY
+{
+	BOOLEAN SwTxQueueBlockFlag;
+	LIST_HEADER NetIfList;
+} BLOCK_QUEUE_ENTRY, *PBLOCK_QUEUE_ENTRY;
+#endif // BLOCK_NET_IF //
+
+struct wificonf
+{
+	BOOLEAN	bShortGI;
+	BOOLEAN bGreenField;
+};
+
+
+
+
+typedef struct _INF_PCI_CONFIG
+{
+	PUCHAR                  CSRBaseAddress;     // PCI MMIO Base Address, all access will use
+}INF_PCI_CONFIG;
+
+typedef struct _INF_USB_CONFIG
+{
+	UINT                BulkInEpAddr;		// bulk-in endpoint address
+	UINT                BulkOutEpAddr[6];	// bulk-out endpoint address
+
+}INF_USB_CONFIG;
+
+#ifdef IKANOS_VX_1X0
+	typedef void (*IkanosWlanTxCbFuncP)(void *, void *);
+
+	struct IKANOS_TX_INFO
+	{
+		struct net_device *netdev;
+		IkanosWlanTxCbFuncP *fp;
+	};
+#endif // IKANOS_VX_1X0 //
+
+#ifdef NINTENDO_AP
+typedef struct _NINDO_CTRL_BLOCK {
+
+	RT_NINTENDO_TABLE	DS_TABLE;
+
+#ifdef CHIP25XX
+	spinlock_t			NINTENDO_TABLE_Lock;
+#else
+	NDIS_SPIN_LOCK		NINTENDO_TABLE_Lock;
+#endif // CHIP25XX //
+
+	UCHAR				NINTENDO_UP_BUFFER[512];
+	UCHAR				Local_KeyIdx;
+	CIPHER_KEY			Local_SharedKey;
+	UCHAR				Local_bHideSsid;
+	UCHAR				Local_AuthMode;
+	UCHAR				Local_WepStatus;
+	USHORT				Local_CapabilityInfo;
+} NINDO_CTRL_BLOCK;
+#endif // NINTENDO_AP //
+
+
+#ifdef DBG_DIAGNOSE
+#define DIAGNOSE_TIME	10   // 10 sec
+typedef struct _RtmpDiagStrcut_
+{	// Diagnosis Related element
+	unsigned char		inited;
+	unsigned char 	qIdx;
+	unsigned char 	ArrayStartIdx;
+	unsigned char		ArrayCurIdx;
+	// Tx Related Count
+	USHORT			TxDataCnt[DIAGNOSE_TIME];
+	USHORT			TxFailCnt[DIAGNOSE_TIME];
+	USHORT			TxDescCnt[DIAGNOSE_TIME][24]; // 3*3	// TxDesc queue length in scale of 0~14, >=15
+	USHORT			TxMcsCnt[DIAGNOSE_TIME][24]; // 3*3
+	USHORT			TxSWQueCnt[DIAGNOSE_TIME][9];		// TxSwQueue length in scale of 0, 1, 2, 3, 4, 5, 6, 7, >=8
+
+	USHORT			TxAggCnt[DIAGNOSE_TIME];
+	USHORT			TxNonAggCnt[DIAGNOSE_TIME];
+	USHORT			TxAMPDUCnt[DIAGNOSE_TIME][24]; // 3*3 // 10 sec, TxDMA APMDU Aggregation count in range from 0 to 15, in setp of 1.
+	USHORT			TxRalinkCnt[DIAGNOSE_TIME];			// TxRalink Aggregation Count in 1 sec scale.
+	USHORT			TxAMSDUCnt[DIAGNOSE_TIME];			// TxAMSUD Aggregation Count in 1 sec scale.
+
+	// Rx Related Count
+	USHORT			RxDataCnt[DIAGNOSE_TIME];			// Rx Total Data count.
+	USHORT			RxCrcErrCnt[DIAGNOSE_TIME];
+	USHORT			RxMcsCnt[DIAGNOSE_TIME][24]; // 3*3
+}RtmpDiagStruct;
+#endif // DBG_DIAGNOSE //
+
+
+//
+//  The miniport adapter structure
+//
+typedef struct _RTMP_ADAPTER
+{
+	PVOID					OS_Cookie;	// save specific structure relative to OS
+	PNET_DEV				net_dev;
+	ULONG					VirtualIfCnt;
+
+#ifdef RT2860
+    USHORT		            LnkCtrlBitMask;
+    USHORT		            RLnkCtrlConfiguration;
+    USHORT                  RLnkCtrlOffset;
+    USHORT		            HostLnkCtrlConfiguration;
+    USHORT                  HostLnkCtrlOffset;
+	USHORT		            PCIePowerSaveLevel;
+   	BOOLEAN					bPCIclkOff;						// flag that indicate if the PICE power status in Configuration SPace..
+	BOOLEAN					bPCIclkOffDisableTx;			//
+
+
+/*****************************************************************************************/
+/*      PCI related parameters                                                           */
+/*****************************************************************************************/
+	PUCHAR                  CSRBaseAddress;     // PCI MMIO Base Address, all access will use
+
+	UINT					int_enable_reg;
+	UINT					int_disable_mask;
+	UINT					int_pending;
+
+
+	RTMP_DMABUF             TxBufSpace[NUM_OF_TX_RING]; // Shared memory of all 1st pre-allocated TxBuf associated with each TXD
+	RTMP_DMABUF             RxDescRing;                 // Shared memory for RX descriptors
+	RTMP_DMABUF             TxDescRing[NUM_OF_TX_RING]; 	// Shared memory for Tx descriptors
+	RTMP_TX_RING            TxRing[NUM_OF_TX_RING];     	// AC0~4 + HCCA
+#endif // RT2860 //
+
+
+	NDIS_SPIN_LOCK          irq_lock;
+	UCHAR                   irq_disabled;
+
+
+
+/*****************************************************************************************/
+	/*      Both PCI/USB related parameters                                                  */
+/*****************************************************************************************/
+
+
+/*****************************************************************************************/
+/*      Tx related parameters                                                           */
+/*****************************************************************************************/
+	BOOLEAN                 DeQueueRunning[NUM_OF_TX_RING];  // for ensuring RTUSBDeQueuePacket get call once
+	NDIS_SPIN_LOCK          DeQueueLock[NUM_OF_TX_RING];
+
+
+	// resource for software backlog queues
+	QUEUE_HEADER            TxSwQueue[NUM_OF_TX_RING];  // 4 AC + 1 HCCA
+	NDIS_SPIN_LOCK          TxSwQueueLock[NUM_OF_TX_RING];	// TxSwQueue spinlock
+
+	RTMP_DMABUF             MgmtDescRing;               	// Shared memory for MGMT descriptors
+	RTMP_MGMT_RING          MgmtRing;
+	NDIS_SPIN_LOCK          MgmtRingLock;               	// Prio Ring spinlock
+
+
+/*****************************************************************************************/
+/*      Rx related parameters                                                           */
+/*****************************************************************************************/
+
+#ifdef RT2860
+	RTMP_RX_RING            RxRing;
+	NDIS_SPIN_LOCK          RxRingLock;                 // Rx Ring spinlock
+#endif // RT2860 //
+
+
+
+/*****************************************************************************************/
+/*      ASIC related parameters                                                          */
+/*****************************************************************************************/
+	UINT32               	MACVersion;      	// MAC version. Record rt2860C(0x28600100) or rt2860D (0x28600101)..
+
+	// ---------------------------
+	// E2PROM
+	// ---------------------------
+	ULONG                   EepromVersion;          // byte 0: version, byte 1: revision, byte 2~3: unused
+	UCHAR                   EEPROMAddressNum;       // 93c46=6  93c66=8
+	USHORT                  EEPROMDefaultValue[NUM_EEPROM_BBP_PARMS];
+	ULONG                   FirmwareVersion;        // byte 0: Minor version, byte 1: Major version, otherwise unused.
+
+	// ---------------------------
+	// BBP Control
+	// ---------------------------
+	UCHAR                   BbpWriteLatch[140];     // record last BBP register value written via BBP_IO_WRITE/BBP_IO_WRITE_VY_REG_ID
+	UCHAR                   BbpRssiToDbmDelta;
+	BBP_R66_TUNING          BbpTuning;
+
+	// ----------------------------
+	// RFIC control
+	// ----------------------------
+	UCHAR                   RfIcType;       // RFIC_xxx
+	ULONG                   RfFreqOffset;   // Frequency offset for channel switching
+	RTMP_RF_REGS            LatchRfRegs;    // latch th latest RF programming value since RF IC doesn't support READ
+
+	EEPROM_ANTENNA_STRUC    Antenna;                            // Since ANtenna definition is different for a & g. We need to save it for future reference.
+	EEPROM_NIC_CONFIG2_STRUC    NicConfig2;
+
+	// This soft Rx Antenna Diversity mechanism is used only when user set
+	// RX Antenna = DIVERSITY ON
+	SOFT_RX_ANT_DIVERSITY   RxAnt;
+
+	UCHAR                   RFProgSeq;
+	CHANNEL_TX_POWER        TxPower[MAX_NUM_OF_CHANNELS];       // Store Tx power value for all channels.
+	CHANNEL_TX_POWER        ChannelList[MAX_NUM_OF_CHANNELS];   // list all supported channels for site survey
+	CHANNEL_11J_TX_POWER    TxPower11J[MAX_NUM_OF_11JCHANNELS];       // 802.11j channel and bw
+	CHANNEL_11J_TX_POWER    ChannelList11J[MAX_NUM_OF_11JCHANNELS];   // list all supported channels for site survey
+
+	UCHAR                   ChannelListNum;                     // number of channel in ChannelList[]
+	UCHAR					Bbp94;
+	BOOLEAN					BbpForCCK;
+	ULONG		Tx20MPwrCfgABand[5];
+	ULONG		Tx20MPwrCfgGBand[5];
+	ULONG		Tx40MPwrCfgABand[5];
+	ULONG		Tx40MPwrCfgGBand[5];
+
+	BOOLEAN     bAutoTxAgcA;                // Enable driver auto Tx Agc control
+	UCHAR	    TssiRefA;					// Store Tssi reference value as 25 temperature.
+	UCHAR	    TssiPlusBoundaryA[5];		// Tssi boundary for increase Tx power to compensate.
+	UCHAR	    TssiMinusBoundaryA[5];		// Tssi boundary for decrease Tx power to compensate.
+	UCHAR	    TxAgcStepA;					// Store Tx TSSI delta increment / decrement value
+	CHAR		TxAgcCompensateA;			// Store the compensation (TxAgcStep * (idx-1))
+
+	BOOLEAN     bAutoTxAgcG;                // Enable driver auto Tx Agc control
+	UCHAR	    TssiRefG;					// Store Tssi reference value as 25 temperature.
+	UCHAR	    TssiPlusBoundaryG[5];		// Tssi boundary for increase Tx power to compensate.
+	UCHAR	    TssiMinusBoundaryG[5];		// Tssi boundary for decrease Tx power to compensate.
+	UCHAR	    TxAgcStepG;					// Store Tx TSSI delta increment / decrement value
+	CHAR		TxAgcCompensateG;			// Store the compensation (TxAgcStep * (idx-1))
+
+	//+++For RT2870, the parameteres is start from BGRssiOffset1 ~ BGRssiOffset3
+	CHAR		BGRssiOffset0;				// Store B/G RSSI#0 Offset value on EEPROM 0x46h
+	CHAR		BGRssiOffset1;				// Store B/G RSSI#1 Offset value
+	CHAR		BGRssiOffset2;				// Store B/G RSSI#2 Offset value
+	//---
+
+	//+++For RT2870, the parameteres is start from ARssiOffset1 ~ ARssiOffset3
+	CHAR		ARssiOffset0;				// Store A RSSI#0 Offset value on EEPROM 0x4Ah
+	CHAR		ARssiOffset1;				// Store A RSSI#1 Offset value
+	CHAR		ARssiOffset2;				// Store A RSSI#2 Offset value
+	//---
+
+	CHAR		BLNAGain;					// Store B/G external LNA#0 value on EEPROM 0x44h
+	CHAR		ALNAGain0;					// Store A external LNA#0 value for ch36~64
+	CHAR		ALNAGain1;					// Store A external LNA#1 value for ch100~128
+	CHAR		ALNAGain2;					// Store A external LNA#2 value for ch132~165
+
+	// ----------------------------
+	// LED control
+	// ----------------------------
+	MCU_LEDCS_STRUC		LedCntl;
+	USHORT				Led1;	// read from EEPROM 0x3c
+	USHORT				Led2;	// EEPROM 0x3e
+	USHORT				Led3;	// EEPROM 0x40
+	UCHAR				LedIndicatorStregth;
+	UCHAR				RssiSingalstrengthOffet;
+    BOOLEAN				bLedOnScanning;
+	UCHAR				LedStatus;
+
+/*****************************************************************************************/
+/*      802.11 related parameters                                                        */
+/*****************************************************************************************/
+	// outgoing BEACON frame buffer and corresponding TXD
+	TXWI_STRUC              	BeaconTxWI;
+	PUCHAR						BeaconBuf;
+	USHORT						BeaconOffset[HW_BEACON_MAX_COUNT];
+
+	// pre-build PS-POLL and NULL frame upon link up. for efficiency purpose.
+	PSPOLL_FRAME            	PsPollFrame;
+	HEADER_802_11           	NullFrame;
+
+//=========AP===========
+
+
+//=======STA===========
+#ifdef CONFIG_STA_SUPPORT
+/* Modified by Wu Xi-Kun 4/21/2006 */
+	// -----------------------------------------------
+	// STA specific configuration & operation status
+	// used only when pAd->OpMode == OPMODE_STA
+	// -----------------------------------------------
+	STA_ADMIN_CONFIG        StaCfg;           // user desired settings
+	STA_ACTIVE_CONFIG       StaActive;         // valid only when ADHOC_ON(pAd) || INFRA_ON(pAd)
+	CHAR                    nickname[IW_ESSID_MAX_SIZE+1]; // nickname, only used in the iwconfig i/f
+	NDIS_MEDIA_STATE        PreMediaState;
+#endif // CONFIG_STA_SUPPORT //
+
+//=======Common===========
+	// OP mode: either AP or STA
+	UCHAR                   OpMode;                     // OPMODE_STA, OPMODE_AP
+
+	NDIS_MEDIA_STATE        IndicateMediaState;			// Base on Indication state, default is NdisMediaStateDisConnected
+
+	// MAT related parameters
+
+	// configuration: read from Registry & E2PROM
+	BOOLEAN                 bLocalAdminMAC;             // Use user changed MAC
+	UCHAR                   PermanentAddress[MAC_ADDR_LEN];    // Factory default MAC address
+	UCHAR                   CurrentAddress[MAC_ADDR_LEN];      // User changed MAC address
+
+	// ------------------------------------------------------
+	// common configuration to both OPMODE_STA and OPMODE_AP
+	// ------------------------------------------------------
+	COMMON_CONFIG           CommonCfg;
+	MLME_STRUCT             Mlme;
+
+	// AP needs those vaiables for site survey feature.
+	MLME_AUX                MlmeAux;           // temporary settings used during MLME state machine
+	BSS_TABLE               ScanTab;           // store the latest SCAN result
+
+	//About MacTab, the sta driver will use #0 and #1 for multicast and AP.
+	MAC_TABLE                 MacTab;     // ASIC on-chip WCID entry table.  At TX, ASIC always use key according to this on-chip table.
+	NDIS_SPIN_LOCK          MacTabLock;
+
+#ifdef DOT11_N_SUPPORT
+	BA_TABLE			BATable;
+#endif // DOT11_N_SUPPORT //
+	NDIS_SPIN_LOCK          BATabLock;
+	RALINK_TIMER_STRUCT RECBATimer;
+
+	// encryption/decryption KEY tables
+	CIPHER_KEY              SharedKey[MAX_MBSSID_NUM][4]; // STA always use SharedKey[BSS0][0..3]
+
+		// RX re-assembly buffer for fragmentation
+	FRAGMENT_FRAME          FragFrame;                  // Frame storage for fragment frame
+
+	// various Counters
+	COUNTER_802_3           Counters8023;               // 802.3 counters
+	COUNTER_802_11          WlanCounters;               // 802.11 MIB counters
+	COUNTER_RALINK          RalinkCounters;             // Ralink propriety counters
+	COUNTER_DRS             DrsCounters;                // counters for Dynamic TX Rate Switching
+	PRIVATE_STRUC           PrivateInfo;                // Private information & counters
+
+	// flags, see fRTMP_ADAPTER_xxx flags
+	ULONG                   Flags;                      // Represent current device status
+
+	// current TX sequence #
+	USHORT                  Sequence;
+
+#ifdef UNDER_CE
+	NDIS_HANDLE             hGiISR;
+#endif
+
+
+	// Control disconnect / connect event generation
+	//+++Didn't used anymore
+	ULONG                   LinkDownTime;
+	//---
+	ULONG                   LastRxRate;
+	ULONG                   LastTxRate;
+	//+++Used only for Station
+	BOOLEAN                 bConfigChanged;         // Config Change flag for the same SSID setting
+	//---
+
+	ULONG                   ExtraInfo;              // Extra information for displaying status
+	ULONG                   SystemErrorBitmap;      // b0: E2PROM version error
+
+	//+++Didn't used anymore
+	ULONG                   MacIcVersion;           // MAC/BBP serial interface issue solved after ver.D
+	//---
+
+	// ---------------------------
+	// System event log
+	// ---------------------------
+	RT_802_11_EVENT_TABLE   EventTab;
+
+
+	BOOLEAN		HTCEnable;
+
+	/*****************************************************************************************/
+	/*      Statistic related parameters                                                     */
+	/*****************************************************************************************/
+
+	BOOLEAN						bUpdateBcnCntDone;
+	ULONG						watchDogMacDeadlock;	// prevent MAC/BBP into deadlock condition
+	// ----------------------------
+	// DEBUG paramerts
+	// ----------------------------
+	BOOLEAN		bBanAllBaSetup;
+	BOOLEAN		bPromiscuous;
+
+	// ----------------------------
+	// rt2860c emulation-use Parameters
+	// ----------------------------
+	ULONG		rtsaccu[30];
+	ULONG		ctsaccu[30];
+	ULONG		cfendaccu[30];
+	ULONG		bacontent[16];
+	ULONG		rxint[RX_RING_SIZE+1];
+	UCHAR		rcvba[60];
+	BOOLEAN		bLinkAdapt;
+	BOOLEAN		bForcePrintTX;
+	BOOLEAN		bForcePrintRX;
+	BOOLEAN		bDisablescanning;		//defined in RT2870 USB
+	BOOLEAN		bStaFifoTest;
+	BOOLEAN		bProtectionTest;
+	BOOLEAN		bHCCATest;
+	BOOLEAN		bGenOneHCCA;
+	BOOLEAN		bBroadComHT;
+	//+++Following add from RT2870 USB.
+	ULONG		BulkOutReq;
+	ULONG		BulkOutComplete;
+	ULONG		BulkOutCompleteOther;
+	ULONG		BulkOutCompleteCancel;	// seems not use now?
+	ULONG		BulkInReq;
+	ULONG		BulkInComplete;
+	ULONG		BulkInCompleteFail;
+	//---
+
+    struct wificonf			WIFItestbed;
+
+#ifdef RALINK_ATE
+	ATE_INFO				ate;
+#endif // RALINK_ATE //
+
+#ifdef DOT11_N_SUPPORT
+	struct reordering_mpdu_pool mpdu_blk_pool;
+#endif // DOT11_N_SUPPORT //
+
+	ULONG					OneSecondnonBEpackets;		// record non BE packets per second
+
+#if WIRELESS_EXT >= 12
+    struct iw_statistics    iw_stats;
+#endif
+
+	struct net_device_stats	stats;
+
+#ifdef BLOCK_NET_IF
+	BLOCK_QUEUE_ENTRY		blockQueueTab[NUM_OF_TX_RING];
+#endif // BLOCK_NET_IF //
+
+
+
+#ifdef MULTIPLE_CARD_SUPPORT
+	INT32					MC_RowID;
+	UCHAR					MC_FileName[256];
+#endif // MULTIPLE_CARD_SUPPORT //
+
+	ULONG					TbttTickCount;
+#ifdef PCI_MSI_SUPPORT
+	BOOLEAN					HaveMsi;
+#endif // PCI_MSI_SUPPORT //
+
+
+	UCHAR					is_on;
+
+#define TIME_BASE			(1000000/OS_HZ)
+#define TIME_ONE_SECOND		(1000000/TIME_BASE)
+	UCHAR					flg_be_adjust;
+	ULONG					be_adjust_last_time;
+
+#ifdef NINTENDO_AP
+	NINDO_CTRL_BLOCK		nindo_ctrl_block;
+#endif // NINTENDO_AP //
+
+
+#ifdef IKANOS_VX_1X0
+	struct IKANOS_TX_INFO	IkanosTxInfo;
+	struct IKANOS_TX_INFO	IkanosRxInfo[MAX_MBSSID_NUM + MAX_WDS_ENTRY + MAX_APCLI_NUM + MAX_MESH_NUM];
+#endif // IKANOS_VX_1X0 //
+
+
+#ifdef DBG_DIAGNOSE
+	RtmpDiagStruct	DiagStruct;
+#endif // DBG_DIAGNOSE //
+
+
+	UINT8					PM_FlgSuspend;
+} RTMP_ADAPTER, *PRTMP_ADAPTER;
+
+//
+// Cisco IAPP format
+//
+typedef struct  _CISCO_IAPP_CONTENT_
+{
+	USHORT     Length;        //IAPP Length
+	UCHAR      MessageType;      //IAPP type
+	UCHAR      FunctionCode;     //IAPP function type
+	UCHAR      DestinaionMAC[MAC_ADDR_LEN];
+	UCHAR      SourceMAC[MAC_ADDR_LEN];
+	USHORT     Tag;           //Tag(element IE) - Adjacent AP report
+	USHORT     TagLength;     //Length of element not including 4 byte header
+	UCHAR      OUI[4];           //0x00, 0x40, 0x96, 0x00
+	UCHAR      PreviousAP[MAC_ADDR_LEN];       //MAC Address of access point
+	USHORT     Channel;
+	USHORT     SsidLen;
+	UCHAR      Ssid[MAX_LEN_OF_SSID];
+	USHORT     Seconds;          //Seconds that the client has been disassociated.
+} CISCO_IAPP_CONTENT, *PCISCO_IAPP_CONTENT;
+
+#define DELAYINTMASK		0x0003fffb
+#define INTMASK				0x0003fffb
+#define IndMask				0x0003fffc
+#define RxINT				0x00000005	// Delayed Rx or indivi rx
+#define TxDataInt			0x000000fa	// Delayed Tx or indivi tx
+#define TxMgmtInt			0x00000102	// Delayed Tx or indivi tx
+#define TxCoherent			0x00020000	// tx coherent
+#define RxCoherent			0x00010000	// rx coherent
+#define McuCommand			0x00000200	// mcu
+#define PreTBTTInt			0x00001000	// Pre-TBTT interrupt
+#define TBTTInt				0x00000800		// TBTT interrupt
+#define GPTimeOutInt			0x00008000		// GPtimeout interrupt
+#define AutoWakeupInt		0x00004000		// AutoWakeupInt interrupt
+#define FifoStaFullInt			0x00002000	//  fifo statistics full interrupt
+
+
+typedef struct _RX_BLK_
+{
+	RT28XX_RXD_STRUC	RxD;
+	PRXWI_STRUC			pRxWI;
+	PHEADER_802_11		pHeader;
+	PNDIS_PACKET		pRxPacket;
+	UCHAR				*pData;
+	USHORT				DataSize;
+	USHORT				Flags;
+	UCHAR				UserPriority;	// for calculate TKIP MIC using
+} RX_BLK;
+
+
+#define RX_BLK_SET_FLAG(_pRxBlk, _flag)		(_pRxBlk->Flags |= _flag)
+#define RX_BLK_TEST_FLAG(_pRxBlk, _flag)	(_pRxBlk->Flags & _flag)
+#define RX_BLK_CLEAR_FLAG(_pRxBlk, _flag)	(_pRxBlk->Flags &= ~(_flag))
+
+
+#define fRX_WDS			0x0001
+#define fRX_AMSDU       0x0002
+#define fRX_ARALINK     0x0004
+#define fRX_HTC         0x0008
+#define fRX_PAD         0x0010
+#define fRX_AMPDU       0x0020
+#define fRX_QOS			0x0040
+#define fRX_INFRA		0x0080
+#define fRX_EAP			0x0100
+#define fRX_MESH		0x0200
+#define fRX_APCLI		0x0400
+#define fRX_DLS			0x0800
+#define fRX_WPI			0x1000
+
+#define LENGTH_AMSDU_SUBFRAMEHEAD	14
+#define LENGTH_ARALINK_SUBFRAMEHEAD	14
+#define LENGTH_ARALINK_HEADER_FIELD	 2
+
+#define TX_UNKOWN_FRAME			0x00
+#define TX_MCAST_FRAME			0x01
+#define TX_LEGACY_FRAME			0x02
+#define TX_AMPDU_FRAME			0x04
+#define TX_AMSDU_FRAME			0x08
+#define TX_RALINK_FRAME			0x10
+#define TX_FRAG_FRAME			0x20
+
+
+//	Currently the sizeof(TX_BLK) is 148 bytes.
+typedef struct _TX_BLK_
+{
+	UCHAR				QueIdx;
+	UCHAR				TxFrameType;				// Indicate the Transmission type of the all frames in one batch
+	UCHAR				TotalFrameNum;				// Total frame number want to send-out in one batch
+	USHORT				TotalFragNum;				// Total frame fragments required in one batch
+	USHORT				TotalFrameLen;				// Total length of all frames want to send-out in one batch
+
+	QUEUE_HEADER		TxPacketList;
+	MAC_TABLE_ENTRY		*pMacEntry;					// NULL: packet with 802.11 RA field is multicast/broadcast address
+	HTTRANSMIT_SETTING	*pTransmit;
+
+	// Following structure used for the characteristics of a specific packet.
+	PNDIS_PACKET		pPacket;
+	PUCHAR				pSrcBufHeader;				// Reference to the head of sk_buff->data
+	PUCHAR				pSrcBufData;				// Reference to the sk_buff->data, will changed depends on hanlding progresss
+	UINT				SrcBufLen;					// Length of packet payload which not including Layer 2 header
+	PUCHAR				pExtraLlcSnapEncap;			// NULL means no extra LLC/SNAP is required
+	UCHAR				HeaderBuf[80];				// TempBuffer for TX_INFO + TX_WI + 802.11 Header + padding + AMSDU SubHeader + LLC/SNAP
+	UCHAR				MpduHeaderLen;				// 802.11 header length NOT including the padding
+	UCHAR				HdrPadLen;					// recording Header Padding Length;
+	UCHAR				apidx;						// The interface associated to this packet
+	UCHAR				Wcid;						// The MAC entry associated to this packet
+	UCHAR				UserPriority;				// priority class of packet
+	UCHAR				FrameGap;					// what kind of IFS this packet use
+	UCHAR				MpduReqNum;					// number of fragments of this frame
+	UCHAR				TxRate;						// TODO: Obsoleted? Should change to MCS?
+	UCHAR				CipherAlg;					// cipher alogrithm
+	PCIPHER_KEY			pKey;
+
+
+
+	USHORT				Flags;						//See following definitions for detail.
+
+	//YOU SHOULD NOT TOUCH IT! Following parameters are used for hardware-depended layer.
+	ULONG				Priv;						// Hardware specific value saved in here.
+} TX_BLK, *PTX_BLK;
+
+
+#define fTX_bRtsRequired		0x0001	// Indicate if need send RTS frame for protection. Not used in RT2860/RT2870.
+#define fTX_bAckRequired       	0x0002	// the packet need ack response
+#define fTX_bPiggyBack     		0x0004	// Legacy device use Piggback or not
+#define fTX_bHTRate         	0x0008	// allow to use HT rate
+#define fTX_bForceNonQoS       	0x0010	// force to transmit frame without WMM-QoS in HT mode
+#define fTX_bAllowFrag       	0x0020	// allow to fragment the packet, A-MPDU, A-MSDU, A-Ralink is not allowed to fragment
+#define fTX_bMoreData			0x0040	// there are more data packets in PowerSave Queue
+#define fTX_bWMM				0x0080	// QOS Data
+
+#define fTX_bClearEAPFrame		0x0100
+
+#define TX_BLK_ASSIGN_FLAG(_pTxBlk, _flag, value)	\
+		do {										\
+			if (value) 								\
+				(_pTxBlk->Flags |= _flag) 			\
+			else 									\
+				(_pTxBlk->Flags &= ~(_flag))		\
+		}while(0)
+
+#define TX_BLK_SET_FLAG(_pTxBlk, _flag)		(_pTxBlk->Flags |= _flag)
+#define TX_BLK_TEST_FLAG(_pTxBlk, _flag)	(((_pTxBlk->Flags & _flag) == _flag) ? 1 : 0)
+#define TX_BLK_CLEAR_FLAG(_pTxBlk, _flag)	(_pTxBlk->Flags &= ~(_flag))
+
+
+
+
+
+//------------------------------------------------------------------------------------------
+
+
+#ifdef RT2860
+//
+// Enable & Disable NIC interrupt via writing interrupt mask register
+// Since it use ADAPTER structure, it have to be put after structure definition.
+//
+__inline    VOID    NICDisableInterrupt(
+    IN  PRTMP_ADAPTER   pAd)
+{
+	RTMP_IO_WRITE32(pAd, INT_MASK_CSR, 0x0);     // 0: disable
+	//RTMP_IO_WRITE32(pAd, PBF_INT_ENA, 0x0);	 	// 0x418 is for firmware . SW doesn't handle here.
+	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_ACTIVE);
+}
+
+__inline    VOID    NICEnableInterrupt(
+    IN  PRTMP_ADAPTER   pAd)
+{
+	//
+	// Flag "fOP_STATUS_DOZE" On, means ASIC put to sleep, else means ASIC WakeUp
+	// To prevent System hang, we should enalbe the interrupt when
+	// ASIC is already Wake Up.
+	//
+    // RT2661 => when ASIC is sleeping, MAC register cannot be read and written.
+	// RT2860 => when ASIC is sleeping, MAC register can be read and written.
+	//if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+	{
+		RTMP_IO_WRITE32(pAd, INT_MASK_CSR, pAd->int_enable_reg /*DELAYINTMASK*/);     // 1:enable
+	}
+	//else
+	//	DBGPRINT(RT_DEBUG_TRACE, ("fOP_STATUS_DOZE !\n"));
+
+	//RTMP_IO_WRITE32(pAd, PBF_INT_ENA, 0x00000030); // 1 : enable
+	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_ACTIVE);
+}
+#endif // RT2860 //
+
+#ifdef RT_BIG_ENDIAN
+static inline VOID	WriteBackToDescriptor(
+	IN  PUCHAR			Dest,
+ 	IN	PUCHAR			Src,
+    IN  BOOLEAN			DoEncrypt,
+	IN  ULONG           DescriptorType)
+{
+	UINT32 *p1, *p2;
+
+	p1 = ((UINT32 *)Dest);
+	p2 = ((UINT32 *)Src);
+
+	*p1 = *p2;
+	*(p1+2) = *(p2+2);
+	*(p1+3) = *(p2+3);
+	*(p1+1) = *(p2+1); // Word 1; this must be written back last
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Endian conversion of Tx/Rx descriptor .
+
+	Arguments:
+		pAd 	Pointer to our adapter
+		pData			Pointer to Tx/Rx descriptor
+		DescriptorType	Direction of the frame
+
+	Return Value:
+		None
+
+	Note:
+		Call this function when read or update descriptor
+	========================================================================
+*/
+static inline VOID	RTMPWIEndianChange(
+	IN	PUCHAR			pData,
+	IN	ULONG			DescriptorType)
+{
+	int size;
+	int i;
+
+	size = ((DescriptorType == TYPE_TXWI) ? TXWI_SIZE : RXWI_SIZE);
+
+	if(DescriptorType == TYPE_TXWI)
+	{
+		*((UINT32 *)(pData)) = SWAP32(*((UINT32 *)(pData)));		// Byte 0~3
+		*((UINT32 *)(pData + 4)) = SWAP32(*((UINT32 *)(pData+4)));	// Byte 4~7
+	}
+	else
+	{
+		for(i=0; i < size/4 ; i++)
+			*(((UINT32 *)pData) +i) = SWAP32(*(((UINT32 *)pData)+i));
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Endian conversion of Tx/Rx descriptor .
+
+	Arguments:
+		pAd 	Pointer to our adapter
+		pData			Pointer to Tx/Rx descriptor
+		DescriptorType	Direction of the frame
+
+	Return Value:
+		None
+
+	Note:
+		Call this function when read or update descriptor
+	========================================================================
+*/
+#ifdef RT2860
+static inline VOID	RTMPDescriptorEndianChange(
+	IN	PUCHAR			pData,
+	IN	ULONG			DescriptorType)
+{
+	*((UINT32 *)(pData)) = SWAP32(*((UINT32 *)(pData)));		// Byte 0~3
+	*((UINT32 *)(pData + 8)) = SWAP32(*((UINT32 *)(pData+8)));	// Byte 8~11
+	*((UINT32 *)(pData +12)) = SWAP32(*((UINT32 *)(pData + 12)));	// Byte 12~15
+	*((UINT32 *)(pData + 4)) = SWAP32(*((UINT32 *)(pData + 4)));				// Byte 4~7, this must be swapped last
+}
+#endif // RT2860 //
+
+/*
+	========================================================================
+
+	Routine Description:
+		Endian conversion of all kinds of 802.11 frames .
+
+	Arguments:
+		pAd 	Pointer to our adapter
+		pData			Pointer to the 802.11 frame structure
+		Dir 			Direction of the frame
+		FromRxDoneInt	Caller is from RxDone interrupt
+
+	Return Value:
+		None
+
+	Note:
+		Call this function when read or update buffer data
+	========================================================================
+*/
+static inline VOID	RTMPFrameEndianChange(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pData,
+	IN	ULONG			Dir,
+	IN	BOOLEAN 		FromRxDoneInt)
+{
+	PHEADER_802_11 pFrame;
+	PUCHAR	pMacHdr;
+
+	// swab 16 bit fields - Frame Control field
+	if(Dir == DIR_READ)
+	{
+		*(USHORT *)pData = SWAP16(*(USHORT *)pData);
+	}
+
+	pFrame = (PHEADER_802_11) pData;
+	pMacHdr = (PUCHAR) pFrame;
+
+	// swab 16 bit fields - Duration/ID field
+	*(USHORT *)(pMacHdr + 2) = SWAP16(*(USHORT *)(pMacHdr + 2));
+
+	// swab 16 bit fields - Sequence Control field
+	*(USHORT *)(pMacHdr + 22) = SWAP16(*(USHORT *)(pMacHdr + 22));
+
+	if(pFrame->FC.Type == BTYPE_MGMT)
+	{
+		switch(pFrame->FC.SubType)
+		{
+			case SUBTYPE_ASSOC_REQ:
+			case SUBTYPE_REASSOC_REQ:
+				// swab 16 bit fields - CapabilityInfo field
+				pMacHdr += sizeof(HEADER_802_11);
+				*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+
+				// swab 16 bit fields - Listen Interval field
+				pMacHdr += 2;
+				*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+				break;
+
+			case SUBTYPE_ASSOC_RSP:
+			case SUBTYPE_REASSOC_RSP:
+				// swab 16 bit fields - CapabilityInfo field
+				pMacHdr += sizeof(HEADER_802_11);
+				*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+
+				// swab 16 bit fields - Status Code field
+				pMacHdr += 2;
+				*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+
+				// swab 16 bit fields - AID field
+				pMacHdr += 2;
+				*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+				break;
+
+			case SUBTYPE_AUTH:
+				// If from APHandleRxDoneInterrupt routine, it is still a encrypt format.
+				// The convertion is delayed to RTMPHandleDecryptionDoneInterrupt.
+				if(!FromRxDoneInt && pFrame->FC.Wep == 1)
+					break;
+				else
+				{
+					// swab 16 bit fields - Auth Alg No. field
+					pMacHdr += sizeof(HEADER_802_11);
+					*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+
+					// swab 16 bit fields - Auth Seq No. field
+					pMacHdr += 2;
+					*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+
+					// swab 16 bit fields - Status Code field
+					pMacHdr += 2;
+					*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+				}
+				break;
+
+			case SUBTYPE_BEACON:
+			case SUBTYPE_PROBE_RSP:
+				// swab 16 bit fields - BeaconInterval field
+				pMacHdr += (sizeof(HEADER_802_11) + TIMESTAMP_LEN);
+				*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+
+				// swab 16 bit fields - CapabilityInfo field
+				pMacHdr += sizeof(USHORT);
+				*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+				break;
+
+			case SUBTYPE_DEAUTH:
+			case SUBTYPE_DISASSOC:
+				// swab 16 bit fields - Reason code field
+				pMacHdr += sizeof(HEADER_802_11);
+				*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+				break;
+		}
+	}
+	else if( pFrame->FC.Type == BTYPE_DATA )
+	{
+	}
+	else if(pFrame->FC.Type == BTYPE_CNTL)
+	{
+		switch(pFrame->FC.SubType)
+		{
+			case SUBTYPE_BLOCK_ACK_REQ:
+				{
+					PFRAME_BA_REQ pBAReq = (PFRAME_BA_REQ)pFrame;
+					*(USHORT *)(&pBAReq->BARControl) = SWAP16(*(USHORT *)(&pBAReq->BARControl));
+					pBAReq->BAStartingSeq.word = SWAP16(pBAReq->BAStartingSeq.word);
+				}
+				break;
+			case SUBTYPE_BLOCK_ACK:
+				// For Block Ack packet, the HT_CONTROL field is in the same offset with Addr3
+				*(UINT32 *)(&pFrame->Addr3[0]) = SWAP32(*(UINT32 *)(&pFrame->Addr3[0]));
+				break;
+
+			case SUBTYPE_ACK:
+				//For ACK packet, the HT_CONTROL field is in the same offset with Addr2
+				*(UINT32 *)(&pFrame->Addr2[0])=	SWAP32(*(UINT32 *)(&pFrame->Addr2[0]));
+				break;
+		}
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("Invalid Frame Type!!!\n"));
+	}
+
+	// swab 16 bit fields - Frame Control
+	if(Dir == DIR_WRITE)
+	{
+		*(USHORT *)pData = SWAP16(*(USHORT *)pData);
+	}
+}
+#endif // RT_BIG_ENDIAN //
+
+
+static inline VOID ConvertMulticastIP2MAC(
+	IN PUCHAR pIpAddr,
+	IN PUCHAR *ppMacAddr,
+	IN UINT16 ProtoType)
+{
+	if (pIpAddr == NULL)
+		return;
+
+	if (ppMacAddr == NULL || *ppMacAddr == NULL)
+		return;
+
+	switch (ProtoType)
+	{
+		case ETH_P_IPV6:
+//			memset(*ppMacAddr, 0, ETH_LENGTH_OF_ADDRESS);
+			*(*ppMacAddr) = 0x33;
+			*(*ppMacAddr + 1) = 0x33;
+			*(*ppMacAddr + 2) = pIpAddr[12];
+			*(*ppMacAddr + 3) = pIpAddr[13];
+			*(*ppMacAddr + 4) = pIpAddr[14];
+			*(*ppMacAddr + 5) = pIpAddr[15];
+			break;
+
+		case ETH_P_IP:
+		default:
+//			memset(*ppMacAddr, 0, ETH_LENGTH_OF_ADDRESS);
+			*(*ppMacAddr) = 0x01;
+			*(*ppMacAddr + 1) = 0x00;
+			*(*ppMacAddr + 2) = 0x5e;
+			*(*ppMacAddr + 3) = pIpAddr[1] & 0x7f;
+			*(*ppMacAddr + 4) = pIpAddr[2];
+			*(*ppMacAddr + 5) = pIpAddr[3];
+			break;
+	}
+
+	return;
+}
+
+BOOLEAN RTMPCheckForHang(
+	IN  NDIS_HANDLE MiniportAdapterContext
+	);
+
+VOID  RTMPHalt(
+	IN  NDIS_HANDLE MiniportAdapterContext
+	);
+
+//
+//  Private routines in rtmp_init.c
+//
+NDIS_STATUS RTMPAllocAdapterBlock(
+	IN PVOID			handle,
+	OUT PRTMP_ADAPTER   *ppAdapter
+	);
+
+NDIS_STATUS RTMPAllocTxRxRingMemory(
+	IN  PRTMP_ADAPTER   pAd
+	);
+
+NDIS_STATUS RTMPFindAdapter(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  NDIS_HANDLE     WrapperConfigurationContext
+	);
+
+NDIS_STATUS	RTMPReadParametersHook(
+	IN	PRTMP_ADAPTER pAd
+	);
+
+VOID RTMPFreeAdapter(
+	IN  PRTMP_ADAPTER   pAd
+	);
+
+NDIS_STATUS NICReadRegParameters(
+	IN  PRTMP_ADAPTER       pAd,
+	IN  NDIS_HANDLE         WrapperConfigurationContext
+	);
+
+
+VOID NICReadEEPROMParameters(
+	IN  PRTMP_ADAPTER       pAd,
+	IN	PUCHAR				mac_addr);
+
+VOID NICInitAsicFromEEPROM(
+	IN  PRTMP_ADAPTER       pAd);
+
+VOID NICInitTxRxRingAndBacklogQueue(
+	IN  PRTMP_ADAPTER   pAd);
+
+NDIS_STATUS NICInitializeAdapter(
+	IN  PRTMP_ADAPTER   pAd,
+	IN   BOOLEAN    bHardReset);
+
+NDIS_STATUS NICInitializeAsic(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  BOOLEAN		bHardReset);
+
+VOID NICIssueReset(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID RTMPRingCleanUp(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           RingType);
+
+VOID RxTest(
+	IN  PRTMP_ADAPTER   pAd);
+
+NDIS_STATUS DbgSendPacket(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PNDIS_PACKET    pPacket);
+
+VOID UserCfgInit(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID NICResetFromError(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID NICEraseFirmware(
+	IN PRTMP_ADAPTER pAd);
+
+NDIS_STATUS NICLoadFirmware(
+	IN  PRTMP_ADAPTER   pAd);
+
+NDIS_STATUS NICLoadRateSwitchingParams(
+	IN PRTMP_ADAPTER pAd);
+
+BOOLEAN NICCheckForHang(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID NICUpdateFifoStaCounters(
+	IN PRTMP_ADAPTER pAd);
+
+VOID NICUpdateRawCounters(
+	IN  PRTMP_ADAPTER   pAd);
+
+ULONG	RTMPNotAllZero(
+	IN	PVOID	pSrc1,
+	IN	ULONG	Length);
+
+VOID RTMPZeroMemory(
+	IN  PVOID   pSrc,
+	IN  ULONG   Length);
+
+ULONG RTMPCompareMemory(
+	IN  PVOID   pSrc1,
+	IN  PVOID   pSrc2,
+	IN  ULONG   Length);
+
+VOID RTMPMoveMemory(
+	OUT PVOID   pDest,
+	IN  PVOID   pSrc,
+	IN  ULONG   Length);
+
+VOID AtoH(
+	char	*src,
+	UCHAR	*dest,
+	int		destlen);
+
+UCHAR BtoH(
+	char ch);
+
+VOID RTMPPatchMacBbpBug(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID RTMPPatchCardBus(
+	IN	PRTMP_ADAPTER	pAdapter);
+
+VOID RTMPPatchRalinkCardBus(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	ULONG			Bus);
+
+ULONG RTMPReadCBConfig(
+	IN	ULONG	Bus,
+	IN	ULONG	Slot,
+	IN	ULONG	Func,
+	IN	ULONG	Offset);
+
+VOID RTMPWriteCBConfig(
+	IN	ULONG	Bus,
+	IN	ULONG	Slot,
+	IN	ULONG	Func,
+	IN	ULONG	Offset,
+	IN	ULONG	Value);
+
+VOID RTMPInitTimer(
+	IN  PRTMP_ADAPTER           pAd,
+	IN  PRALINK_TIMER_STRUCT    pTimer,
+	IN  PVOID                   pTimerFunc,
+	IN	PVOID					pData,
+	IN  BOOLEAN                 Repeat);
+
+VOID RTMPSetTimer(
+	IN  PRALINK_TIMER_STRUCT    pTimer,
+	IN  ULONG                   Value);
+
+
+VOID RTMPModTimer(
+	IN	PRALINK_TIMER_STRUCT	pTimer,
+	IN	ULONG					Value);
+
+VOID RTMPCancelTimer(
+	IN  PRALINK_TIMER_STRUCT    pTimer,
+	OUT BOOLEAN                 *pCancelled);
+
+VOID RTMPSetLED(
+	IN PRTMP_ADAPTER 	pAd,
+	IN UCHAR			Status);
+
+VOID RTMPSetSignalLED(
+	IN PRTMP_ADAPTER 	pAd,
+	IN NDIS_802_11_RSSI Dbm);
+
+VOID RTMPEnableRxTx(
+	IN PRTMP_ADAPTER	pAd);
+
+//
+// prototype in action.c
+//
+VOID ActionStateMachineInit(
+    IN	PRTMP_ADAPTER	pAd,
+    IN  STATE_MACHINE *S,
+    OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID MlmeADDBAAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID MlmeDELBAAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID MlmeDLSAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID MlmeInvalidAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID MlmeQOSAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+#ifdef DOT11_N_SUPPORT
+VOID PeerAddBAReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID PeerAddBARspAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID PeerDelBAAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID PeerBAAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+#endif // DOT11_N_SUPPORT //
+
+VOID SendPSMPAction(
+	IN PRTMP_ADAPTER	pAd,
+	IN UCHAR			Wcid,
+	IN UCHAR			Psmp);
+
+
+#ifdef DOT11N_DRAFT3
+VOID SendBSS2040CoexistMgmtAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR	Wcid,
+	IN	UCHAR	apidx,
+	IN	UCHAR	InfoReq);
+
+VOID SendNotifyBWActionFrame(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR  Wcid,
+	IN UCHAR apidx);
+
+BOOLEAN ChannelSwitchSanityCheck(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR  Wcid,
+	IN    UCHAR  NewChannel,
+	IN    UCHAR  Secondary);
+
+VOID ChannelSwitchAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR  Wcid,
+	IN    UCHAR  Channel,
+	IN    UCHAR  Secondary);
+
+ULONG BuildIntolerantChannelRep(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    PUCHAR  pDest);
+
+VOID Update2040CoexistFrameAndNotify(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR  Wcid,
+	IN	BOOLEAN	bAddIntolerantCha);
+
+VOID Send2040CoexistAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR  Wcid,
+	IN	BOOLEAN	bAddIntolerantCha);
+#endif // DOT11N_DRAFT3 //
+
+VOID PeerRMAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID PeerPublicAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+#ifdef CONFIG_STA_SUPPORT
+VOID StaPublicAction(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR Bss2040Coexist);
+#endif // CONFIG_STA_SUPPORT //
+
+
+VOID PeerBSSTranAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+#ifdef DOT11_N_SUPPORT
+VOID PeerHTAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+#endif // DOT11_N_SUPPORT //
+
+VOID PeerQOSAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+#ifdef QOS_DLS_SUPPORT
+VOID PeerDLSAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+#endif // QOS_DLS_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+VOID DlsParmFill(
+	IN PRTMP_ADAPTER pAd,
+	IN OUT MLME_DLS_REQ_STRUCT *pDlsReq,
+	IN PRT_802_11_DLS pDls,
+	IN USHORT reason);
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef DOT11_N_SUPPORT
+VOID RECBATimerTimeout(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3);
+
+VOID ORIBATimerTimeout(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID SendRefreshBAR(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry);
+#endif // DOT11_N_SUPPORT //
+
+VOID ActHeaderInit(
+    IN	PRTMP_ADAPTER	pAd,
+    IN OUT PHEADER_802_11 pHdr80211,
+    IN PUCHAR Addr1,
+    IN PUCHAR Addr2,
+    IN PUCHAR Addr3);
+
+VOID BarHeaderInit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN OUT PFRAME_BAR pCntlBar,
+	IN PUCHAR pDA,
+	IN PUCHAR pSA);
+
+VOID InsertActField(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen,
+	IN UINT8 Category,
+	IN UINT8 ActCode);
+
+BOOLEAN QosBADataParse(
+	IN PRTMP_ADAPTER	pAd,
+	IN BOOLEAN bAMSDU,
+	IN PUCHAR p8023Header,
+	IN UCHAR	WCID,
+	IN UCHAR	TID,
+	IN USHORT Sequence,
+	IN UCHAR DataOffset,
+	IN USHORT Datasize,
+	IN UINT   CurRxIndex);
+
+#ifdef DOT11_N_SUPPORT
+BOOLEAN CntlEnqueueForRecv(
+    IN	PRTMP_ADAPTER	pAd,
+	IN ULONG Wcid,
+    IN ULONG MsgLen,
+	IN PFRAME_BA_REQ pMsg);
+
+VOID BaAutoManSwitch(
+	IN	PRTMP_ADAPTER	pAd);
+#endif // DOT11_N_SUPPORT //
+
+VOID HTIOTCheck(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR     BatRecIdx);
+
+//
+// Private routines in rtmp_data.c
+//
+BOOLEAN RTMPHandleRxDoneInterrupt(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID RTMPHandleTxDoneInterrupt(
+	IN  PRTMP_ADAPTER   pAd);
+
+BOOLEAN RTMPHandleTxRingDmaDoneInterrupt(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  INT_SOURCE_CSR_STRUC TxRingBitmap);
+
+VOID RTMPHandleMgmtRingDmaDoneInterrupt(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID RTMPHandleTBTTInterrupt(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID RTMPHandlePreTBTTInterrupt(
+	IN  PRTMP_ADAPTER   pAd);
+
+void RTMPHandleTwakeupInterrupt(
+	IN PRTMP_ADAPTER pAd);
+
+VOID	RTMPHandleRxCoherentInterrupt(
+	IN	PRTMP_ADAPTER	pAd);
+
+BOOLEAN TxFrameIsAggregatible(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pPrevAddr1,
+	IN  PUCHAR          p8023hdr);
+
+BOOLEAN PeerIsAggreOn(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  ULONG          TxRate,
+    IN  PMAC_TABLE_ENTRY pMacEntry);
+
+NDIS_STATUS Sniff2BytesFromNdisBuffer(
+	IN  PNDIS_BUFFER    pFirstBuffer,
+	IN  UCHAR           DesiredOffset,
+	OUT PUCHAR          pByte0,
+	OUT PUCHAR          pByte1);
+
+NDIS_STATUS STASendPacket(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PNDIS_PACKET    pPacket);
+
+VOID STASendPackets(
+	IN  NDIS_HANDLE     MiniportAdapterContext,
+	IN  PPNDIS_PACKET   ppPacketArray,
+	IN  UINT            NumberOfPackets);
+
+VOID RTMPDeQueuePacket(
+	IN  PRTMP_ADAPTER   pAd,
+   	IN	BOOLEAN			bIntContext,
+	IN  UCHAR			QueIdx,
+	IN	UCHAR			Max_Tx_Packets);
+
+NDIS_STATUS	RTMPHardTransmit(
+	IN PRTMP_ADAPTER	pAd,
+	IN PNDIS_PACKET		pPacket,
+	IN  UCHAR			QueIdx,
+	OUT	PULONG			pFreeTXDLeft);
+
+NDIS_STATUS	STAHardTransmit(
+	IN PRTMP_ADAPTER	pAd,
+	IN TX_BLK			*pTxBlk,
+	IN  UCHAR			QueIdx);
+
+VOID STARxEAPOLFrameIndicate(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID);
+
+NDIS_STATUS RTMPFreeTXDRequest(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           RingType,
+	IN  UCHAR           NumberRequired,
+	IN 	PUCHAR          FreeNumberIs);
+
+NDIS_STATUS MlmeHardTransmit(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR	QueIdx,
+	IN  PNDIS_PACKET    pPacket);
+
+NDIS_STATUS MlmeHardTransmitMgmtRing(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR	QueIdx,
+	IN  PNDIS_PACKET    pPacket);
+
+NDIS_STATUS MlmeHardTransmitTxRing(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR	QueIdx,
+	IN  PNDIS_PACKET    pPacket);
+
+USHORT  RTMPCalcDuration(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           Rate,
+	IN  ULONG           Size);
+
+VOID RTMPWriteTxWI(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTXWI_STRUC		pTxWI,
+	IN  BOOLEAN    		FRAG,
+	IN  BOOLEAN    		CFACK,
+	IN  BOOLEAN    		InsTimestamp,
+	IN	BOOLEAN			AMPDU,
+	IN	BOOLEAN			Ack,
+	IN	BOOLEAN			NSeq,		// HW new a sequence.
+	IN	UCHAR			BASize,
+	IN	UCHAR			WCID,
+	IN	ULONG			Length,
+	IN  UCHAR      		PID,
+	IN	UCHAR			TID,
+	IN	UCHAR			TxRate,
+	IN	UCHAR			Txopmode,
+	IN	BOOLEAN			CfAck,
+	IN	HTTRANSMIT_SETTING	*pTransmit);
+
+
+VOID RTMPWriteTxWI_Data(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	OUT PTXWI_STRUC		pTxWI,
+	IN	TX_BLK				*pTxBlk);
+
+
+VOID RTMPWriteTxWI_Cache(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	OUT PTXWI_STRUC		pTxWI,
+	IN	TX_BLK				*pTxBlk);
+
+VOID RTMPWriteTxDescriptor(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTXD_STRUC		pTxD,
+	IN	BOOLEAN			bWIV,
+	IN	UCHAR			QSEL);
+
+VOID RTMPSuspendMsduTransmission(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID RTMPResumeMsduTransmission(
+	IN  PRTMP_ADAPTER   pAd);
+
+NDIS_STATUS MiniportMMRequest(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	UCHAR			QueIdx,
+	IN	PUCHAR			pData,
+	IN  UINT            Length);
+
+VOID RTMPSendNullFrame(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           TxRate,
+	IN	BOOLEAN			bQosNull);
+
+VOID RTMPSendDisassociationFrame(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID RTMPSendRTSFrame(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pDA,
+	IN	unsigned int	NextMpduSize,
+	IN  UCHAR           TxRate,
+	IN  UCHAR           RTSRate,
+	IN  USHORT          AckDuration,
+	IN  UCHAR           QueIdx,
+	IN  UCHAR			FrameGap);
+
+
+NDIS_STATUS RTMPApplyPacketFilter(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PRT28XX_RXD_STRUC      pRxD,
+	IN  PHEADER_802_11  pHeader);
+
+PQUEUE_HEADER   RTMPCheckTxSwQueue(
+	IN  PRTMP_ADAPTER   pAd,
+	OUT UCHAR           *QueIdx);
+
+#ifdef CONFIG_STA_SUPPORT
+VOID RTMPReportMicError(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PCIPHER_KEY     pWpaKey);
+
+VOID	WpaMicFailureReportFrame(
+	IN  PRTMP_ADAPTER    pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID    WpaDisassocApAndBlockAssoc(
+    IN  PVOID SystemSpecific1,
+    IN  PVOID FunctionContext,
+    IN  PVOID SystemSpecific2,
+    IN  PVOID SystemSpecific3);
+#endif // CONFIG_STA_SUPPORT //
+
+NDIS_STATUS RTMPCloneNdisPacket(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	BOOLEAN    pInsAMSDUHdr,
+	IN  PNDIS_PACKET    pInPacket,
+	OUT PNDIS_PACKET   *ppOutPacket);
+
+NDIS_STATUS RTMPAllocateNdisPacket(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PNDIS_PACKET    *pPacket,
+	IN  PUCHAR          pHeader,
+	IN  UINT            HeaderLen,
+	IN  PUCHAR          pData,
+	IN  UINT            DataLen);
+
+VOID RTMPFreeNdisPacket(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PNDIS_PACKET    pPacket);
+
+BOOLEAN RTMPFreeTXDUponTxDmaDone(
+	IN PRTMP_ADAPTER    pAd,
+	IN UCHAR            QueIdx);
+
+BOOLEAN RTMPCheckDHCPFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket);
+
+
+BOOLEAN RTMPCheckEtherType(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket);
+
+
+VOID RTMPCckBbpTuning(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UINT			TxRate);
+
+//
+// Private routines in rtmp_wep.c
+//
+VOID RTMPInitWepEngine(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pKey,
+	IN  UCHAR           KeyId,
+	IN  UCHAR           KeyLen,
+	IN  PUCHAR          pDest);
+
+VOID RTMPEncryptData(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pSrc,
+	IN  PUCHAR          pDest,
+	IN  UINT            Len);
+
+BOOLEAN	RTMPDecryptData(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PUCHAR			pSrc,
+	IN	UINT			Len,
+	IN	UINT			idx);
+
+BOOLEAN	RTMPSoftDecryptWEP(
+	IN PRTMP_ADAPTER 	pAd,
+	IN PUCHAR			pData,
+	IN ULONG			DataByteCnt,
+	IN PCIPHER_KEY		pGroupKey);
+
+VOID RTMPSetICV(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pDest);
+
+VOID ARCFOUR_INIT(
+	IN  PARCFOURCONTEXT Ctx,
+	IN  PUCHAR          pKey,
+	IN  UINT            KeyLen);
+
+UCHAR   ARCFOUR_BYTE(
+	IN  PARCFOURCONTEXT     Ctx);
+
+VOID ARCFOUR_DECRYPT(
+	IN  PARCFOURCONTEXT Ctx,
+	IN  PUCHAR          pDest,
+	IN  PUCHAR          pSrc,
+	IN  UINT            Len);
+
+VOID ARCFOUR_ENCRYPT(
+	IN  PARCFOURCONTEXT Ctx,
+	IN  PUCHAR          pDest,
+	IN  PUCHAR          pSrc,
+	IN  UINT            Len);
+
+VOID WPAARCFOUR_ENCRYPT(
+	IN  PARCFOURCONTEXT Ctx,
+	IN  PUCHAR          pDest,
+	IN  PUCHAR          pSrc,
+	IN  UINT            Len);
+
+UINT RTMP_CALC_FCS32(
+	IN  UINT   Fcs,
+	IN  PUCHAR  Cp,
+	IN  INT     Len);
+
+//
+// MLME routines
+//
+
+// Asic/RF/BBP related functions
+
+VOID AsicAdjustTxPower(
+	IN PRTMP_ADAPTER pAd);
+
+VOID 	AsicUpdateProtect(
+	IN		PRTMP_ADAPTER	pAd,
+	IN 		USHORT			OperaionMode,
+	IN 		UCHAR			SetMask,
+	IN		BOOLEAN			bDisableBGProtect,
+	IN		BOOLEAN			bNonGFExist);
+
+VOID AsicSwitchChannel(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	UCHAR			Channel,
+	IN	BOOLEAN			bScan);
+
+VOID AsicLockChannel(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR Channel) ;
+
+VOID AsicAntennaSelect(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           Channel);
+
+VOID AsicAntennaSetting(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	ABGBAND_STATE	BandState);
+
+VOID AsicRfTuningExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+#ifdef CONFIG_STA_SUPPORT
+VOID AsicSleepThenAutoWakeup(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  USHORT TbttNumToNextWakeUp);
+
+VOID AsicForceSleep(
+	IN PRTMP_ADAPTER pAd);
+
+VOID AsicForceWakeup(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN    bFromTx);
+#endif // CONFIG_STA_SUPPORT //
+
+VOID AsicSetBssid(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR pBssid);
+
+VOID AsicSetMcastWC(
+	IN PRTMP_ADAPTER pAd);
+
+VOID AsicDelWcidTab(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR	Wcid);
+
+VOID AsicEnableRDG(
+	IN PRTMP_ADAPTER pAd);
+
+VOID AsicDisableRDG(
+	IN PRTMP_ADAPTER pAd);
+
+VOID AsicDisableSync(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID AsicEnableBssSync(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID AsicEnableIbssSync(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID AsicSetEdcaParm(
+	IN PRTMP_ADAPTER pAd,
+	IN PEDCA_PARM    pEdcaParm);
+
+VOID AsicSetSlotTime(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN bUseShortSlotTime);
+
+VOID AsicAddSharedKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR         BssIndex,
+	IN UCHAR         KeyIdx,
+	IN UCHAR         CipherAlg,
+	IN PUCHAR        pKey,
+	IN PUCHAR        pTxMic,
+	IN PUCHAR        pRxMic);
+
+VOID AsicRemoveSharedKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR         BssIndex,
+	IN UCHAR         KeyIdx);
+
+VOID AsicUpdateWCIDAttribute(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT		WCID,
+	IN UCHAR		BssIndex,
+	IN UCHAR        CipherAlg,
+	IN BOOLEAN		bUsePairewiseKeyTable);
+
+VOID AsicUpdateWCIDIVEIV(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT		WCID,
+	IN ULONG        uIV,
+	IN ULONG        uEIV);
+
+VOID AsicUpdateRxWCIDTable(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT		WCID,
+	IN PUCHAR        pAddr);
+
+VOID AsicAddKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT		WCID,
+	IN UCHAR		BssIndex,
+	IN UCHAR		KeyIdx,
+	IN PCIPHER_KEY	pCipherKey,
+	IN BOOLEAN		bUsePairewiseKeyTable,
+	IN BOOLEAN		bTxKey);
+
+VOID AsicAddPairwiseKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR        pAddr,
+	IN UCHAR		WCID,
+	IN CIPHER_KEY		 *pCipherKey);
+
+VOID AsicRemovePairwiseKeyEntry(
+	IN PRTMP_ADAPTER  pAd,
+	IN UCHAR		 BssIdx,
+	IN UCHAR		 Wcid);
+
+BOOLEAN AsicSendCommandToMcu(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR         Command,
+	IN UCHAR         Token,
+	IN UCHAR         Arg0,
+	IN UCHAR         Arg1);
+
+#ifdef RT2860
+BOOLEAN AsicCheckCommanOk(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR		 Command);
+#endif // RT2860 //
+
+VOID MacAddrRandomBssid(
+	IN  PRTMP_ADAPTER   pAd,
+	OUT PUCHAR pAddr);
+
+VOID MgtMacHeaderInit(
+	IN  PRTMP_ADAPTER     pAd,
+	IN OUT PHEADER_802_11 pHdr80211,
+	IN UCHAR SubType,
+	IN UCHAR ToDs,
+	IN PUCHAR pDA,
+	IN PUCHAR pBssid);
+
+VOID MlmeRadioOff(
+	IN PRTMP_ADAPTER pAd);
+
+VOID MlmeRadioOn(
+	IN PRTMP_ADAPTER pAd);
+
+
+VOID BssTableInit(
+	IN BSS_TABLE *Tab);
+
+#ifdef DOT11_N_SUPPORT
+VOID BATableInit(
+	IN PRTMP_ADAPTER pAd,
+    IN BA_TABLE *Tab);
+#endif // DOT11_N_SUPPORT //
+
+ULONG BssTableSearch(
+	IN BSS_TABLE *Tab,
+	IN PUCHAR pBssid,
+	IN UCHAR Channel);
+
+ULONG BssSsidTableSearch(
+	IN BSS_TABLE *Tab,
+	IN PUCHAR    pBssid,
+	IN PUCHAR    pSsid,
+	IN UCHAR     SsidLen,
+	IN UCHAR     Channel);
+
+ULONG BssTableSearchWithSSID(
+	IN BSS_TABLE *Tab,
+	IN PUCHAR    Bssid,
+	IN PUCHAR    pSsid,
+	IN UCHAR     SsidLen,
+	IN UCHAR     Channel);
+
+VOID BssTableDeleteEntry(
+	IN OUT  PBSS_TABLE pTab,
+	IN      PUCHAR pBssid,
+	IN      UCHAR Channel);
+
+#ifdef DOT11_N_SUPPORT
+VOID BATableDeleteORIEntry(
+	IN OUT	PRTMP_ADAPTER pAd,
+	IN		BA_ORI_ENTRY	*pBAORIEntry);
+
+VOID BATableDeleteRECEntry(
+	IN OUT	PRTMP_ADAPTER pAd,
+	IN		BA_REC_ENTRY	*pBARECEntry);
+
+VOID BATableTearORIEntry(
+	IN OUT	PRTMP_ADAPTER pAd,
+	IN		UCHAR TID,
+	IN		UCHAR Wcid,
+	IN		BOOLEAN bForceDelete,
+	IN		BOOLEAN ALL);
+
+VOID BATableTearRECEntry(
+	IN OUT	PRTMP_ADAPTER pAd,
+	IN		UCHAR TID,
+	IN		UCHAR WCID,
+	IN		BOOLEAN ALL);
+#endif // DOT11_N_SUPPORT //
+
+VOID  BssEntrySet(
+	IN  PRTMP_ADAPTER   pAd,
+	OUT PBSS_ENTRY pBss,
+	IN PUCHAR pBssid,
+	IN CHAR Ssid[],
+	IN UCHAR SsidLen,
+	IN UCHAR BssType,
+	IN USHORT BeaconPeriod,
+	IN PCF_PARM CfParm,
+	IN USHORT AtimWin,
+	IN USHORT CapabilityInfo,
+	IN UCHAR SupRate[],
+	IN UCHAR SupRateLen,
+	IN UCHAR ExtRate[],
+	IN UCHAR ExtRateLen,
+	IN HT_CAPABILITY_IE *pHtCapability,
+	IN ADD_HT_INFO_IE *pAddHtInfo,	// AP might use this additional ht info IE
+	IN UCHAR			HtCapabilityLen,
+	IN UCHAR			AddHtInfoLen,
+	IN UCHAR			NewExtChanOffset,
+	IN UCHAR Channel,
+	IN CHAR Rssi,
+	IN LARGE_INTEGER TimeStamp,
+	IN UCHAR CkipFlag,
+	IN PEDCA_PARM pEdcaParm,
+	IN PQOS_CAPABILITY_PARM pQosCapability,
+	IN PQBSS_LOAD_PARM pQbssLoad,
+	IN USHORT LengthVIE,
+	IN PNDIS_802_11_VARIABLE_IEs pVIE);
+
+ULONG  BssTableSetEntry(
+	IN  PRTMP_ADAPTER   pAd,
+	OUT PBSS_TABLE pTab,
+	IN PUCHAR pBssid,
+	IN CHAR Ssid[],
+	IN UCHAR SsidLen,
+	IN UCHAR BssType,
+	IN USHORT BeaconPeriod,
+	IN CF_PARM *CfParm,
+	IN USHORT AtimWin,
+	IN USHORT CapabilityInfo,
+	IN UCHAR SupRate[],
+	IN UCHAR SupRateLen,
+	IN UCHAR ExtRate[],
+	IN UCHAR ExtRateLen,
+	IN HT_CAPABILITY_IE *pHtCapability,
+	IN ADD_HT_INFO_IE *pAddHtInfo,	// AP might use this additional ht info IE
+	IN UCHAR			HtCapabilityLen,
+	IN UCHAR			AddHtInfoLen,
+	IN UCHAR			NewExtChanOffset,
+	IN UCHAR Channel,
+	IN CHAR Rssi,
+	IN LARGE_INTEGER TimeStamp,
+	IN UCHAR CkipFlag,
+	IN PEDCA_PARM pEdcaParm,
+	IN PQOS_CAPABILITY_PARM pQosCapability,
+	IN PQBSS_LOAD_PARM pQbssLoad,
+	IN USHORT LengthVIE,
+	IN PNDIS_802_11_VARIABLE_IEs pVIE);
+
+#ifdef DOT11_N_SUPPORT
+VOID BATableInsertEntry(
+    IN	PRTMP_ADAPTER	pAd,
+	IN USHORT Aid,
+    IN USHORT		TimeOutValue,
+	IN USHORT		StartingSeq,
+    IN UCHAR TID,
+	IN UCHAR BAWinSize,
+	IN UCHAR OriginatorStatus,
+    IN BOOLEAN IsRecipient);
+
+#ifdef DOT11N_DRAFT3
+VOID Bss2040CoexistTimeOut(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+
+VOID  TriEventInit(
+	IN	PRTMP_ADAPTER	pAd);
+
+ULONG TriEventTableSetEntry(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT TRIGGER_EVENT_TAB *Tab,
+	IN PUCHAR pBssid,
+	IN HT_CAPABILITY_IE *pHtCapability,
+	IN UCHAR			HtCapabilityLen,
+	IN UCHAR			RegClass,
+	IN UCHAR ChannelNo);
+
+VOID TriEventCounterMaintenance(
+	IN	PRTMP_ADAPTER	pAd);
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+
+VOID BssTableSsidSort(
+	IN  PRTMP_ADAPTER   pAd,
+	OUT BSS_TABLE *OutTab,
+	IN  CHAR Ssid[],
+	IN  UCHAR SsidLen);
+
+VOID  BssTableSortByRssi(
+	IN OUT BSS_TABLE *OutTab);
+
+VOID BssCipherParse(
+	IN OUT  PBSS_ENTRY  pBss);
+
+NDIS_STATUS  MlmeQueueInit(
+	IN MLME_QUEUE *Queue);
+
+VOID  MlmeQueueDestroy(
+	IN MLME_QUEUE *Queue);
+
+BOOLEAN MlmeEnqueue(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG Machine,
+	IN ULONG MsgType,
+	IN ULONG MsgLen,
+	IN VOID *Msg);
+
+BOOLEAN MlmeEnqueueForRecv(
+	IN  PRTMP_ADAPTER   pAd,
+	IN ULONG Wcid,
+	IN ULONG TimeStampHigh,
+	IN ULONG TimeStampLow,
+	IN UCHAR Rssi0,
+	IN UCHAR Rssi1,
+	IN UCHAR Rssi2,
+	IN ULONG MsgLen,
+	IN PVOID Msg,
+	IN UCHAR Signal);
+
+
+BOOLEAN MlmeDequeue(
+	IN MLME_QUEUE *Queue,
+	OUT MLME_QUEUE_ELEM **Elem);
+
+VOID    MlmeRestartStateMachine(
+	IN  PRTMP_ADAPTER   pAd);
+
+BOOLEAN  MlmeQueueEmpty(
+	IN MLME_QUEUE *Queue);
+
+BOOLEAN  MlmeQueueFull(
+	IN MLME_QUEUE *Queue);
+
+BOOLEAN  MsgTypeSubst(
+	IN PRTMP_ADAPTER pAd,
+	IN PFRAME_802_11 pFrame,
+	OUT INT *Machine,
+	OUT INT *MsgType);
+
+VOID StateMachineInit(
+	IN STATE_MACHINE *Sm,
+	IN STATE_MACHINE_FUNC Trans[],
+	IN ULONG StNr,
+	IN ULONG MsgNr,
+	IN STATE_MACHINE_FUNC DefFunc,
+	IN ULONG InitState,
+	IN ULONG Base);
+
+VOID StateMachineSetAction(
+	IN STATE_MACHINE *S,
+	IN ULONG St,
+	ULONG Msg,
+	IN STATE_MACHINE_FUNC F);
+
+VOID StateMachinePerformAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN STATE_MACHINE *S,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID Drop(
+	IN  PRTMP_ADAPTER   pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID AssocStateMachineInit(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  STATE_MACHINE *Sm,
+	OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID ReassocTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID AssocTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID DisassocTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+//----------------------------------------------
+VOID MlmeDisassocReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID MlmeAssocReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID MlmeReassocReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID MlmeDisassocReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerAssocRspAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerReassocRspAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerDisassocAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID DisassocTimeoutAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID AssocTimeoutAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID  ReassocTimeoutAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID  Cls3errAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR pAddr);
+
+VOID SwitchBetweenWepAndCkip(
+	IN PRTMP_ADAPTER pAd);
+
+VOID  InvalidStateWhenAssoc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID  InvalidStateWhenReassoc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID InvalidStateWhenDisassociate(
+	IN  PRTMP_ADAPTER pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+
+VOID  ComposePsPoll(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID  ComposeNullFrame(
+	IN  PRTMP_ADAPTER pAd);
+
+VOID  AssocPostProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR pAddr2,
+	IN  USHORT CapabilityInfo,
+	IN  USHORT Aid,
+	IN  UCHAR SupRate[],
+	IN  UCHAR SupRateLen,
+	IN  UCHAR ExtRate[],
+	IN  UCHAR ExtRateLen,
+	IN PEDCA_PARM pEdcaParm,
+	IN HT_CAPABILITY_IE		*pHtCapability,
+	IN  UCHAR HtCapabilityLen,
+	IN ADD_HT_INFO_IE		*pAddHtInfo);
+
+VOID AuthStateMachineInit(
+	IN  PRTMP_ADAPTER   pAd,
+	IN PSTATE_MACHINE sm,
+	OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID AuthTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID MlmeAuthReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerAuthRspAtSeq2Action(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerAuthRspAtSeq4Action(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID AuthTimeoutAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID Cls2errAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR pAddr);
+
+VOID MlmeDeauthReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID InvalidStateWhenAuth(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+//=============================================
+
+VOID AuthRspStateMachineInit(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PSTATE_MACHINE Sm,
+	IN  STATE_MACHINE_FUNC Trans[]);
+
+VOID PeerDeauthAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID PeerAuthSimpleRspGenAndSend(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PHEADER_802_11  pHdr80211,
+	IN  USHORT Alg,
+	IN  USHORT Seq,
+	IN  USHORT Reason,
+	IN  USHORT Status);
+
+//
+// Private routines in dls.c
+//
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+void DlsStateMachineInit(
+    IN PRTMP_ADAPTER pAd,
+    IN STATE_MACHINE *Sm,
+    OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID MlmeDlsReqAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID PeerDlsReqAction(
+    IN PRTMP_ADAPTER	pAd,
+    IN MLME_QUEUE_ELEM	*Elem);
+
+VOID PeerDlsRspAction(
+    IN PRTMP_ADAPTER	pAd,
+    IN MLME_QUEUE_ELEM	*Elem);
+
+VOID MlmeDlsTearDownAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID PeerDlsTearDownAction(
+    IN PRTMP_ADAPTER	pAd,
+    IN MLME_QUEUE_ELEM	*Elem);
+
+VOID RTMPCheckDLSTimeOut(
+	IN PRTMP_ADAPTER	pAd);
+
+BOOLEAN RTMPRcvFrameDLSCheck(
+	IN PRTMP_ADAPTER	pAd,
+	IN PHEADER_802_11	pHeader,
+	IN ULONG			Len,
+	IN PRT28XX_RXD_STRUC	pRxD);
+
+INT	RTMPCheckDLSFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN  PUCHAR          pDA);
+
+VOID RTMPSendDLSTearDownFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN  PUCHAR          pDA);
+
+NDIS_STATUS RTMPSendSTAKeyRequest(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pDA);
+
+NDIS_STATUS RTMPSendSTAKeyHandShake(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pDA);
+
+VOID DlsTimeoutAction(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+BOOLEAN MlmeDlsReqSanity(
+	IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PRT_802_11_DLS *pDLS,
+    OUT PUSHORT pReason);
+
+INT Set_DlsEntryInfo_Display_Proc(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR arg);
+
+MAC_TABLE_ENTRY *MacTableInsertDlsEntry(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR	pAddr,
+	IN  UINT	DlsEntryIdx);
+
+BOOLEAN MacTableDeleteDlsEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT wcid,
+	IN PUCHAR pAddr);
+
+MAC_TABLE_ENTRY *DlsEntryTableLookup(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR	pAddr,
+	IN BOOLEAN	bResetIdelCount);
+
+MAC_TABLE_ENTRY *DlsEntryTableLookupByWcid(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR	wcid,
+	IN PUCHAR	pAddr,
+	IN BOOLEAN	bResetIdelCount);
+
+INT	Set_DlsAddEntry_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_DlsTearDownEntry_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef QOS_DLS_SUPPORT
+BOOLEAN PeerDlsReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pDA,
+    OUT PUCHAR pSA,
+    OUT USHORT *pCapabilityInfo,
+    OUT USHORT *pDlsTimeout,
+    OUT UCHAR *pRatesLen,
+    OUT UCHAR Rates[],
+    OUT UCHAR *pHtCapabilityLen,
+    OUT HT_CAPABILITY_IE *pHtCapability);
+
+BOOLEAN PeerDlsRspSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pDA,
+    OUT PUCHAR pSA,
+    OUT USHORT *pCapabilityInfo,
+    OUT USHORT *pStatus,
+    OUT UCHAR *pRatesLen,
+    OUT UCHAR Rates[],
+    OUT UCHAR *pHtCapabilityLen,
+    OUT HT_CAPABILITY_IE *pHtCapability);
+
+BOOLEAN PeerDlsTearDownSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pDA,
+    OUT PUCHAR pSA,
+    OUT USHORT *pReason);
+#endif // QOS_DLS_SUPPORT //
+
+//========================================
+
+VOID SyncStateMachineInit(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  STATE_MACHINE *Sm,
+	OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID BeaconTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID ScanTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID MlmeScanReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID InvalidStateWhenScan(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID InvalidStateWhenJoin(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID InvalidStateWhenStart(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerBeacon(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID EnqueueProbeRequest(
+	IN PRTMP_ADAPTER pAd);
+
+BOOLEAN ScanRunning(
+		IN PRTMP_ADAPTER pAd);
+//=========================================
+
+VOID MlmeCntlInit(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  STATE_MACHINE *S,
+	OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID MlmeCntlMachinePerformAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  STATE_MACHINE *S,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID CntlIdleProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID CntlOidScanProc(
+	IN  PRTMP_ADAPTER pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID CntlOidSsidProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM * Elem);
+
+VOID CntlOidRTBssidProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM * Elem);
+
+VOID CntlMlmeRoamingProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM * Elem);
+
+VOID CntlWaitDisassocProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID CntlWaitJoinProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID CntlWaitReassocProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID CntlWaitStartProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID CntlWaitAuthProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID CntlWaitAuthProc2(
+	IN  PRTMP_ADAPTER pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID CntlWaitAssocProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+#ifdef QOS_DLS_SUPPORT
+VOID CntlOidDLSSetupProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+#endif // QOS_DLS_SUPPORT //
+
+VOID LinkUp(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR BssType);
+
+VOID LinkDown(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  BOOLEAN         IsReqFromAP);
+
+VOID IterateOnBssTab(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID IterateOnBssTab2(
+	IN  PRTMP_ADAPTER   pAd);;
+
+VOID JoinParmFill(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  OUT MLME_JOIN_REQ_STRUCT *JoinReq,
+	IN  ULONG BssIdx);
+
+VOID AssocParmFill(
+	IN  PRTMP_ADAPTER   pAd,
+	IN OUT MLME_ASSOC_REQ_STRUCT *AssocReq,
+	IN  PUCHAR pAddr,
+	IN  USHORT CapabilityInfo,
+	IN  ULONG Timeout,
+	IN  USHORT ListenIntv);
+
+VOID ScanParmFill(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  OUT MLME_SCAN_REQ_STRUCT *ScanReq,
+	IN  CHAR Ssid[],
+	IN  UCHAR SsidLen,
+	IN  UCHAR BssType,
+	IN  UCHAR ScanType);
+
+VOID DisassocParmFill(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  OUT MLME_DISASSOC_REQ_STRUCT *DisassocReq,
+	IN  PUCHAR pAddr,
+	IN  USHORT Reason);
+
+VOID StartParmFill(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  OUT MLME_START_REQ_STRUCT *StartReq,
+	IN  CHAR Ssid[],
+	IN  UCHAR SsidLen);
+
+VOID AuthParmFill(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  OUT MLME_AUTH_REQ_STRUCT *AuthReq,
+	IN  PUCHAR pAddr,
+	IN  USHORT Alg);
+
+VOID EnqueuePsPoll(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID EnqueueBeaconFrame(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID MlmeJoinReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID MlmeScanReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID MlmeStartReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID ScanTimeoutAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID BeaconTimeoutAtJoinAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerBeaconAtScanAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerBeaconAtJoinAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerBeacon(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerProbeReqAction(
+	IN  PRTMP_ADAPTER pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID ScanNextChannel(
+	IN  PRTMP_ADAPTER   pAd);
+
+ULONG MakeIbssBeacon(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID CCXAdjacentAPReport(
+	IN  PRTMP_ADAPTER   pAd);
+
+BOOLEAN MlmeScanReqSanity(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  VOID *Msg,
+	IN  ULONG MsgLen,
+	OUT UCHAR *BssType,
+	OUT CHAR ssid[],
+	OUT UCHAR *SsidLen,
+	OUT UCHAR *ScanType);
+
+BOOLEAN PeerBeaconAndProbeRspSanity(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  VOID *Msg,
+	IN  ULONG MsgLen,
+	IN  UCHAR MsgChannel,
+	OUT PUCHAR pAddr2,
+	OUT PUCHAR pBssid,
+	OUT CHAR Ssid[],
+	OUT UCHAR *pSsidLen,
+	OUT UCHAR *pBssType,
+	OUT USHORT *pBeaconPeriod,
+	OUT UCHAR *pChannel,
+	OUT UCHAR *pNewChannel,
+	OUT LARGE_INTEGER *pTimestamp,
+	OUT CF_PARM *pCfParm,
+	OUT USHORT *pAtimWin,
+	OUT USHORT *pCapabilityInfo,
+	OUT UCHAR *pErp,
+	OUT UCHAR *pDtimCount,
+	OUT UCHAR *pDtimPeriod,
+	OUT UCHAR *pBcastFlag,
+	OUT UCHAR *pMessageToMe,
+	OUT UCHAR SupRate[],
+	OUT UCHAR *pSupRateLen,
+	OUT UCHAR ExtRate[],
+	OUT UCHAR *pExtRateLen,
+	OUT	UCHAR *pCkipFlag,
+	OUT	UCHAR *pAironetCellPowerLimit,
+	OUT PEDCA_PARM       pEdcaParm,
+	OUT PQBSS_LOAD_PARM  pQbssLoad,
+	OUT PQOS_CAPABILITY_PARM pQosCapability,
+	OUT ULONG *pRalinkIe,
+	OUT UCHAR		 *pHtCapabilityLen,
+#ifdef CONFIG_STA_SUPPORT
+	OUT UCHAR		 *pPreNHtCapabilityLen,
+#endif // CONFIG_STA_SUPPORT //
+	OUT HT_CAPABILITY_IE *pHtCapability,
+	OUT UCHAR		 *AddHtInfoLen,
+	OUT ADD_HT_INFO_IE *AddHtInfo,
+	OUT UCHAR *NewExtChannel,
+	OUT USHORT *LengthVIE,
+	OUT PNDIS_802_11_VARIABLE_IEs pVIE);
+
+BOOLEAN PeerAddBAReqActionSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *pMsg,
+    IN ULONG MsgLen,
+	OUT PUCHAR pAddr2);
+
+BOOLEAN PeerAddBARspActionSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *pMsg,
+    IN ULONG MsgLen);
+
+BOOLEAN PeerDelBAActionSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN UCHAR Wcid,
+    IN VOID *pMsg,
+    IN ULONG MsgLen);
+
+BOOLEAN MlmeAssocReqSanity(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  VOID *Msg,
+	IN  ULONG MsgLen,
+	OUT PUCHAR pApAddr,
+	OUT USHORT *CapabilityInfo,
+	OUT ULONG *Timeout,
+	OUT USHORT *ListenIntv);
+
+BOOLEAN MlmeAuthReqSanity(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  VOID *Msg,
+	IN  ULONG MsgLen,
+	OUT PUCHAR pAddr,
+	OUT ULONG *Timeout,
+	OUT USHORT *Alg);
+
+BOOLEAN MlmeStartReqSanity(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  VOID *Msg,
+	IN  ULONG MsgLen,
+	OUT CHAR Ssid[],
+	OUT UCHAR *Ssidlen);
+
+BOOLEAN PeerAuthSanity(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  VOID *Msg,
+	IN  ULONG MsgLen,
+	OUT PUCHAR pAddr,
+	OUT USHORT *Alg,
+	OUT USHORT *Seq,
+	OUT USHORT *Status,
+	OUT CHAR ChlgText[]);
+
+BOOLEAN PeerAssocRspSanity(
+	IN  PRTMP_ADAPTER   pAd,
+    IN VOID *pMsg,
+	IN  ULONG MsgLen,
+	OUT PUCHAR pAddr2,
+	OUT USHORT *pCapabilityInfo,
+	OUT USHORT *pStatus,
+	OUT USHORT *pAid,
+	OUT UCHAR SupRate[],
+	OUT UCHAR *pSupRateLen,
+	OUT UCHAR ExtRate[],
+	OUT UCHAR *pExtRateLen,
+    OUT HT_CAPABILITY_IE		*pHtCapability,
+    OUT ADD_HT_INFO_IE		*pAddHtInfo,	// AP might use this additional ht info IE
+    OUT UCHAR			*pHtCapabilityLen,
+    OUT UCHAR			*pAddHtInfoLen,
+    OUT UCHAR			*pNewExtChannelOffset,
+	OUT PEDCA_PARM pEdcaParm,
+	OUT UCHAR *pCkipFlag);
+
+BOOLEAN PeerDisassocSanity(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  VOID *Msg,
+	IN  ULONG MsgLen,
+	OUT PUCHAR pAddr2,
+	OUT USHORT *Reason);
+
+BOOLEAN PeerWpaMessageSanity(
+    IN 	PRTMP_ADAPTER 		pAd,
+    IN 	PEAPOL_PACKET 		pMsg,
+    IN 	ULONG 				MsgLen,
+    IN 	UCHAR				MsgType,
+    IN 	MAC_TABLE_ENTRY  	*pEntry);
+
+BOOLEAN PeerDeauthSanity(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  VOID *Msg,
+	IN  ULONG MsgLen,
+	OUT PUCHAR pAddr2,
+	OUT USHORT *Reason);
+
+BOOLEAN PeerProbeReqSanity(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  VOID *Msg,
+	IN  ULONG MsgLen,
+	OUT PUCHAR pAddr2,
+	OUT CHAR Ssid[],
+	OUT UCHAR *pSsidLen);
+
+BOOLEAN GetTimBit(
+	IN  CHAR *Ptr,
+	IN  USHORT Aid,
+	OUT UCHAR *TimLen,
+	OUT UCHAR *BcastFlag,
+	OUT UCHAR *DtimCount,
+	OUT UCHAR *DtimPeriod,
+	OUT UCHAR *MessageToMe);
+
+UCHAR ChannelSanity(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR channel);
+
+NDIS_802_11_NETWORK_TYPE NetworkTypeInUseSanity(
+	IN PBSS_ENTRY pBss);
+
+BOOLEAN MlmeDelBAReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen);
+
+BOOLEAN MlmeAddBAReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2);
+
+ULONG MakeOutgoingFrame(
+	OUT CHAR *Buffer,
+	OUT ULONG *Length, ...);
+
+VOID  LfsrInit(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  ULONG Seed);
+
+UCHAR RandomByte(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID AsicUpdateAutoFallBackTable(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pTxRate);
+
+VOID  MlmePeriodicExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID LinkDownExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID LinkUpExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID STAMlmePeriodicExec(
+	PRTMP_ADAPTER pAd);
+
+VOID MlmeAutoScan(
+	IN PRTMP_ADAPTER pAd);
+
+VOID MlmeAutoReconnectLastSSID(
+	IN PRTMP_ADAPTER pAd);
+
+BOOLEAN MlmeValidateSSID(
+	IN PUCHAR pSsid,
+	IN UCHAR  SsidLen);
+
+VOID MlmeCheckForRoaming(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG    Now32);
+
+VOID MlmeCheckForFastRoaming(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  ULONG           Now);
+
+VOID MlmeDynamicTxRateSwitching(
+	IN PRTMP_ADAPTER pAd);
+
+VOID MlmeSetTxRate(
+	IN PRTMP_ADAPTER		pAd,
+	IN PMAC_TABLE_ENTRY		pEntry,
+	IN PRTMP_TX_RATE_SWITCH	pTxRate);
+
+VOID MlmeSelectTxRateTable(
+	IN PRTMP_ADAPTER		pAd,
+	IN PMAC_TABLE_ENTRY		pEntry,
+	IN PUCHAR				*ppTable,
+	IN PUCHAR				pTableSize,
+	IN PUCHAR				pInitTxRateIdx);
+
+VOID MlmeCalculateChannelQuality(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG Now);
+
+VOID MlmeCheckPsmChange(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG    Now32);
+
+VOID MlmeSetPsmBit(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT psm);
+
+VOID MlmeSetTxPreamble(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT TxPreamble);
+
+VOID UpdateBasicRateBitmap(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID MlmeUpdateTxRates(
+	IN PRTMP_ADAPTER 	pAd,
+	IN 	BOOLEAN		 	bLinkUp,
+	IN	UCHAR			apidx);
+
+#ifdef DOT11_N_SUPPORT
+VOID MlmeUpdateHtTxRates(
+	IN PRTMP_ADAPTER 		pAd,
+	IN	UCHAR				apidx);
+#endif // DOT11_N_SUPPORT //
+
+VOID    RTMPCheckRates(
+	IN      PRTMP_ADAPTER   pAd,
+	IN OUT  UCHAR           SupRate[],
+	IN OUT  UCHAR           *SupRateLen);
+
+#ifdef CONFIG_STA_SUPPORT
+BOOLEAN RTMPCheckChannel(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR		CentralChannel,
+	IN UCHAR		Channel);
+#endif // CONFIG_STA_SUPPORT //
+
+BOOLEAN 	RTMPCheckHt(
+	IN		PRTMP_ADAPTER	pAd,
+	IN		UCHAR	Wcid,
+	IN OUT	HT_CAPABILITY_IE			*pHtCapability,
+	IN OUT	ADD_HT_INFO_IE			*pAddHtInfo);
+
+VOID StaQuickResponeForRateUpExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID AsicBbpTuning1(
+	IN PRTMP_ADAPTER pAd);
+
+VOID AsicBbpTuning2(
+	IN PRTMP_ADAPTER pAd);
+
+VOID RTMPUpdateMlmeRate(
+	IN PRTMP_ADAPTER	pAd);
+
+CHAR RTMPMaxRssi(
+	IN PRTMP_ADAPTER	pAd,
+	IN CHAR				Rssi0,
+	IN CHAR				Rssi1,
+	IN CHAR				Rssi2);
+
+VOID AsicEvaluateRxAnt(
+	IN PRTMP_ADAPTER	pAd);
+
+VOID AsicRxAntEvalTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID APSDPeriodicExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+BOOLEAN RTMPCheckEntryEnableAutoRateSwitch(
+	IN PRTMP_ADAPTER    pAd,
+	IN PMAC_TABLE_ENTRY	pEntry);
+
+UCHAR RTMPStaFixedTxMode(
+	IN PRTMP_ADAPTER    pAd,
+	IN PMAC_TABLE_ENTRY	pEntry);
+
+VOID RTMPUpdateLegacyTxSetting(
+		UCHAR				fixed_tx_mode,
+		PMAC_TABLE_ENTRY	pEntry);
+
+BOOLEAN RTMPAutoRateSwitchCheck(
+	IN PRTMP_ADAPTER    pAd);
+
+NDIS_STATUS MlmeInit(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID MlmeHandler(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID MlmeHalt(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID MlmeResetRalinkCounters(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID BuildChannelList(
+	IN PRTMP_ADAPTER pAd);
+
+UCHAR FirstChannel(
+	IN  PRTMP_ADAPTER   pAd);
+
+UCHAR NextChannel(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR channel);
+
+VOID ChangeToCellPowerLimit(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR         AironetCellPowerLimit);
+
+VOID RaiseClock(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UINT32 *x);
+
+VOID LowerClock(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UINT32 *x);
+
+USHORT ShiftInBits(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID ShiftOutBits(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  USHORT data,
+	IN  USHORT count);
+
+VOID EEpromCleanup(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID EWDS(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID EWEN(
+	IN  PRTMP_ADAPTER   pAd);
+
+USHORT RTMP_EEPROM_READ16(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  USHORT Offset);
+
+VOID RTMP_EEPROM_WRITE16(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  USHORT Offset,
+	IN  USHORT Data);
+
+//
+// Prototypes of function definition in rtmp_tkip.c
+//
+VOID    RTMPInitTkipEngine(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pTKey,
+	IN  UCHAR           KeyId,
+	IN  PUCHAR          pTA,
+	IN  PUCHAR          pMICKey,
+	IN  PUCHAR          pTSC,
+	OUT PULONG          pIV16,
+	OUT PULONG          pIV32);
+
+VOID    RTMPInitMICEngine(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pKey,
+	IN  PUCHAR          pDA,
+	IN  PUCHAR          pSA,
+	IN  UCHAR           UserPriority,
+	IN  PUCHAR          pMICKey);
+
+BOOLEAN RTMPTkipCompareMICValue(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pSrc,
+	IN  PUCHAR          pDA,
+	IN  PUCHAR          pSA,
+	IN  PUCHAR          pMICKey,
+	IN	UCHAR			UserPriority,
+	IN  UINT            Len);
+
+VOID    RTMPCalculateMICValue(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PNDIS_PACKET    pPacket,
+	IN  PUCHAR          pEncap,
+	IN  PCIPHER_KEY     pKey,
+	IN	UCHAR			apidx);
+
+BOOLEAN RTMPTkipCompareMICValueWithLLC(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pLLC,
+	IN  PUCHAR          pSrc,
+	IN  PUCHAR          pDA,
+	IN  PUCHAR          pSA,
+	IN  PUCHAR          pMICKey,
+	IN  UINT            Len);
+
+VOID    RTMPTkipAppendByte(
+	IN  PTKIP_KEY_INFO  pTkip,
+	IN  UCHAR           uChar);
+
+VOID    RTMPTkipAppend(
+	IN  PTKIP_KEY_INFO  pTkip,
+	IN  PUCHAR          pSrc,
+	IN  UINT            nBytes);
+
+VOID    RTMPTkipGetMIC(
+	IN  PTKIP_KEY_INFO  pTkip);
+
+BOOLEAN RTMPSoftDecryptTKIP(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR	pData,
+	IN ULONG	DataByteCnt,
+	IN UCHAR    UserPriority,
+	IN PCIPHER_KEY	pWpaKey);
+
+BOOLEAN RTMPSoftDecryptAES(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR	pData,
+	IN ULONG	DataByteCnt,
+	IN PCIPHER_KEY	pWpaKey);
+
+//
+// Prototypes of function definition in cmm_info.c
+//
+NDIS_STATUS RTMPWPARemoveKeyProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PVOID           pBuf);
+
+VOID    RTMPWPARemoveAllKeys(
+	IN  PRTMP_ADAPTER   pAd);
+
+BOOLEAN RTMPCheckStrPrintAble(
+    IN  CHAR *pInPutStr,
+    IN  UCHAR strLen);
+
+VOID    RTMPSetPhyMode(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  ULONG phymode);
+
+VOID	RTMPUpdateHTIE(
+	IN	RT_HT_CAPABILITY	*pRtHt,
+	IN		UCHAR				*pMcsSet,
+	OUT		HT_CAPABILITY_IE *pHtCapability,
+	OUT		ADD_HT_INFO_IE		*pAddHtInfo);
+
+VOID	RTMPAddWcidAttributeEntry(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			BssIdx,
+	IN 	UCHAR		 	KeyIdx,
+	IN 	UCHAR		 	CipherAlg,
+	IN 	MAC_TABLE_ENTRY *pEntry);
+
+CHAR *GetEncryptType(
+	CHAR enc);
+
+CHAR *GetAuthMode(
+	CHAR auth);
+
+VOID RTMPIoctlGetSiteSurvey(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq);
+
+VOID RTMPIoctlGetMacTable(
+	IN PRTMP_ADAPTER pAd,
+	IN struct iwreq *wrq);
+
+VOID RTMPIndicateWPA2Status(
+	IN  PRTMP_ADAPTER  pAdapter);
+
+VOID	RTMPOPModeSwitching(
+	IN	PRTMP_ADAPTER	pAd);
+
+#ifdef CONFIG_STA_SUPPORT
+VOID    RTMPAddBSSIDCipher(
+    IN  PRTMP_ADAPTER   pAd,
+	IN	UCHAR	Aid,
+    IN  PNDIS_802_11_KEY    pKey,
+    IN  UCHAR   CipherAlg);
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef DOT11_N_SUPPORT
+VOID	RTMPSetHT(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	OID_SET_HT_PHYMODE *pHTPhyMode);
+
+VOID	RTMPSetIndividualHT(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	UCHAR				apidx);
+#endif // DOT11_N_SUPPORT //
+
+VOID RTMPSendWirelessEvent(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Event_flag,
+	IN	PUCHAR 			pAddr,
+	IN  UCHAR			BssIdx,
+	IN	CHAR			Rssi);
+
+VOID	NICUpdateCntlCounters(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PHEADER_802_11	pHeader,
+	IN    UCHAR			SubType,
+	IN	PRXWI_STRUC 	pRxWI);
+//
+// prototype in wpa.c
+//
+BOOLEAN WpaMsgTypeSubst(
+	IN  UCHAR   EAPType,
+	OUT INT		*MsgType);
+
+VOID WpaPskStateMachineInit(
+	IN  PRTMP_ADAPTER       pAd,
+	IN  STATE_MACHINE       *S,
+	OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID WpaEAPOLKeyAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID    WpaPairMsg1Action(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID    WpaPairMsg3Action(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID    WpaGroupMsg1Action(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID    WpaMacHeaderInit(
+	IN      PRTMP_ADAPTER   pAd,
+	IN OUT  PHEADER_802_11  pHdr80211,
+	IN      UCHAR           wep,
+	IN      PUCHAR          pAddr1);
+
+VOID    Wpa2PairMsg1Action(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  MLME_QUEUE_ELEM *Elem);
+
+VOID    Wpa2PairMsg3Action(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  MLME_QUEUE_ELEM *Elem);
+
+BOOLEAN ParseKeyData(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          pKeyData,
+    IN  UCHAR           KeyDataLen,
+	IN	UCHAR			bPairewise);
+
+VOID    RTMPToWirelessSta(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pHeader802_3,
+    IN  UINT            HdrLen,
+	IN  PUCHAR          pData,
+    IN  UINT            DataLen,
+    IN	BOOLEAN			is4wayFrame);
+
+VOID    HMAC_SHA1(
+	IN  UCHAR   *text,
+	IN  UINT    text_len,
+	IN  UCHAR   *key,
+	IN  UINT    key_len,
+	IN  UCHAR   *digest);
+
+VOID    PRF(
+	IN  UCHAR   *key,
+	IN  INT     key_len,
+	IN  UCHAR   *prefix,
+	IN  INT     prefix_len,
+	IN  UCHAR   *data,
+	IN  INT     data_len,
+	OUT UCHAR   *output,
+	IN  INT     len);
+
+VOID    CCKMPRF(
+	IN  UCHAR   *key,
+	IN  INT     key_len,
+	IN  UCHAR   *data,
+	IN  INT     data_len,
+	OUT UCHAR   *output,
+	IN  INT     len);
+
+VOID WpaCountPTK(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR   *PMK,
+	IN  UCHAR   *ANonce,
+	IN  UCHAR   *AA,
+	IN  UCHAR   *SNonce,
+	IN  UCHAR   *SA,
+	OUT UCHAR   *output,
+	IN  UINT    len);
+
+VOID    GenRandom(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	UCHAR			*macAddr,
+	OUT	UCHAR			*random);
+
+//
+// prototype in aironet.c
+//
+VOID    AironetStateMachineInit(
+	IN  PRTMP_ADAPTER       pAd,
+	IN  STATE_MACHINE       *S,
+	OUT STATE_MACHINE_FUNC  Trans[]);
+
+VOID    AironetMsgAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID    AironetRequestAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID    ChannelLoadRequestAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           Index);
+
+VOID    NoiseHistRequestAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           Index);
+
+VOID    BeaconRequestAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           Index);
+
+VOID    AironetReportAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID    ChannelLoadReportAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           Index);
+
+VOID    NoiseHistReportAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           Index);
+
+VOID    AironetFinalReportAction(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID    BeaconReportAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           Index);
+
+VOID    AironetAddBeaconReport(
+	IN  PRTMP_ADAPTER       pAd,
+	IN  ULONG               Index,
+	IN  PMLME_QUEUE_ELEM    pElem);
+
+VOID    AironetCreateBeaconReportFromBssTable(
+	IN  PRTMP_ADAPTER       pAd);
+
+VOID    DBGPRINT_TX_RING(
+	IN PRTMP_ADAPTER  pAd,
+	IN UCHAR          QueIdx);
+
+VOID DBGPRINT_RX_RING(
+	IN PRTMP_ADAPTER  pAd);
+
+CHAR    ConvertToRssi(
+	IN PRTMP_ADAPTER  pAd,
+	IN CHAR				Rssi,
+	IN UCHAR    RssiNumber);
+
+
+#ifdef DOT11N_DRAFT3
+VOID BuildEffectedChannelList(
+	IN PRTMP_ADAPTER pAd);
+#endif // DOT11N_DRAFT3 //
+
+
+VOID APAsicEvaluateRxAnt(
+	IN PRTMP_ADAPTER	pAd);
+
+
+VOID APAsicRxAntEvalTimeout(
+	IN PRTMP_ADAPTER	pAd);
+
+//
+// function prototype in cmm_wpa.c
+//
+BOOLEAN RTMPCheckWPAframe(
+	IN PRTMP_ADAPTER pAd,
+	IN PMAC_TABLE_ENTRY	pEntry,
+	IN PUCHAR 			pData,
+	IN ULONG 			DataByteCount,
+	IN UCHAR			FromWhichBSSID);
+
+VOID AES_GTK_KEY_UNWRAP(
+	IN  UCHAR   *key,
+	OUT UCHAR   *plaintext,
+	IN	UCHAR	c_len,
+	IN  UCHAR   *ciphertext);
+
+BOOLEAN RTMPCheckRSNIE(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pData,
+	IN  UCHAR           DataLen,
+	IN  MAC_TABLE_ENTRY *pEntry,
+	OUT	UCHAR			*Offset);
+
+BOOLEAN RTMPParseEapolKeyData(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pKeyData,
+	IN  UCHAR           KeyDataLen,
+	IN	UCHAR			GroupKeyIndex,
+	IN	UCHAR			MsgType,
+	IN	BOOLEAN			bWPA2,
+	IN  MAC_TABLE_ENTRY *pEntry);
+
+VOID	ConstructEapolMsg(
+	IN 	PRTMP_ADAPTER    	pAd,
+    IN 	UCHAR				PeerAuthMode,
+    IN 	UCHAR				PeerWepStatus,
+    IN 	UCHAR				MyGroupKeyWepStatus,
+    IN 	UCHAR				MsgType,
+    IN	UCHAR				DefaultKeyIdx,
+    IN 	UCHAR				*ReplayCounter,
+	IN 	UCHAR				*KeyNonce,
+	IN	UCHAR				*TxRSC,
+	IN	UCHAR				*PTK,
+	IN	UCHAR				*GTK,
+	IN	UCHAR				*RSNIE,
+	IN	UCHAR				RSNIE_Len,
+    OUT PEAPOL_PACKET       pMsg);
+
+VOID	CalculateMIC(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			PeerWepStatus,
+	IN	UCHAR			*PTK,
+	OUT PEAPOL_PACKET   pMsg);
+
+NDIS_STATUS	RTMPSoftDecryptBroadCastData(
+	IN	PRTMP_ADAPTER					pAd,
+	IN	RX_BLK							*pRxBlk,
+	IN  NDIS_802_11_ENCRYPTION_STATUS 	GroupCipher,
+	IN  PCIPHER_KEY						pShard_key);
+
+VOID	ConstructEapolKeyData(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			PeerAuthMode,
+	IN	UCHAR			PeerWepStatus,
+	IN	UCHAR			GroupKeyWepStatus,
+	IN 	UCHAR			MsgType,
+	IN	UCHAR			DefaultKeyIdx,
+	IN	BOOLEAN			bWPA2Capable,
+	IN	UCHAR			*PTK,
+	IN	UCHAR			*GTK,
+	IN	UCHAR			*RSNIE,
+	IN	UCHAR			RSNIE_LEN,
+	OUT PEAPOL_PACKET   pMsg);
+
+VOID RTMPMakeRSNIE(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UINT            AuthMode,
+	IN  UINT            WepStatus,
+	IN	UCHAR			apidx);
+
+//
+// function prototype in ap_wpa.c
+//
+
+BOOLEAN APWpaMsgTypeSubst(
+	IN UCHAR    EAPType,
+	OUT INT *MsgType) ;
+
+MAC_TABLE_ENTRY *PACInquiry(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  ULONG           Wcid);
+
+BOOLEAN RTMPCheckMcast(
+	IN PRTMP_ADAPTER pAd,
+	IN PEID_STRUCT      eid_ptr,
+	IN MAC_TABLE_ENTRY  *pEntry);
+
+BOOLEAN RTMPCheckUcast(
+	IN PRTMP_ADAPTER pAd,
+	IN PEID_STRUCT      eid_ptr,
+	IN MAC_TABLE_ENTRY  *pEntry);
+
+BOOLEAN RTMPCheckAUTH(
+	IN PRTMP_ADAPTER pAd,
+	IN PEID_STRUCT      eid_ptr,
+	IN MAC_TABLE_ENTRY  *pEntry);
+
+VOID WPAStart4WayHS(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MAC_TABLE_ENTRY *pEntry,
+	IN	ULONG			TimeInterval);
+
+VOID WPAStart2WayGroupHS(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MAC_TABLE_ENTRY *pEntry);
+
+VOID APWpaEAPPacketAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID APWpaEAPOLStartAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID APWpaEAPOLLogoffAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID APWpaEAPOLKeyAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID APWpaEAPOLASFAlertAction(
+	IN  PRTMP_ADAPTER    pAd,
+	IN  MLME_QUEUE_ELEM  *Elem);
+
+VOID HandleCounterMeasure(
+	IN PRTMP_ADAPTER pAd,
+	IN MAC_TABLE_ENTRY  *pEntry);
+
+VOID PeerPairMsg2Action(
+	IN PRTMP_ADAPTER pAd,
+	IN MAC_TABLE_ENTRY  *pEntry,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID PeerPairMsg4Action(
+	IN PRTMP_ADAPTER pAd,
+	IN MAC_TABLE_ENTRY  *pEntry,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID CMTimerExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID WPARetryExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID EnqueueStartForPSKExec(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3);
+
+VOID RTMPHandleSTAKey(
+    IN PRTMP_ADAPTER    pAdapter,
+    IN MAC_TABLE_ENTRY  *pEntry,
+    IN MLME_QUEUE_ELEM  *Elem);
+
+VOID PeerGroupMsg2Action(
+	IN  PRTMP_ADAPTER    pAd,
+	IN  PMAC_TABLE_ENTRY pEntry,
+	IN  VOID             *Msg,
+	IN  UINT             MsgLen);
+
+VOID PairDisAssocAction(
+	IN  PRTMP_ADAPTER    pAd,
+	IN  PMAC_TABLE_ENTRY pEntry,
+	IN  USHORT           Reason);
+
+VOID MlmeDeAuthAction(
+	IN  PRTMP_ADAPTER    pAd,
+	IN  PMAC_TABLE_ENTRY pEntry,
+	IN  USHORT           Reason);
+
+VOID GREKEYPeriodicExec(
+	IN  PVOID   SystemSpecific1,
+	IN  PVOID   FunctionContext,
+	IN  PVOID   SystemSpecific2,
+	IN  PVOID   SystemSpecific3);
+
+VOID CountGTK(
+	IN  UCHAR   *PMK,
+	IN  UCHAR   *GNonce,
+	IN  UCHAR   *AA,
+	OUT UCHAR   *output,
+	IN  UINT    len);
+
+VOID    GetSmall(
+	IN  PVOID   pSrc1,
+	IN  PVOID   pSrc2,
+	OUT PUCHAR  out,
+	IN  ULONG   Length);
+
+VOID    GetLarge(
+	IN  PVOID   pSrc1,
+	IN  PVOID   pSrc2,
+	OUT PUCHAR  out,
+	IN  ULONG   Length);
+
+VOID APGenRandom(
+	IN PRTMP_ADAPTER pAd,
+	OUT UCHAR       *random);
+
+VOID AES_GTK_KEY_WRAP(
+	IN UCHAR *key,
+	IN UCHAR *plaintext,
+	IN UCHAR p_len,
+	OUT UCHAR *ciphertext);
+
+VOID    WpaSend(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          pPacket,
+    IN  ULONG           Len);
+
+VOID    APToWirelessSta(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MAC_TABLE_ENTRY *pEntry,
+	IN  PUCHAR          pHeader802_3,
+	IN  UINT            HdrLen,
+	IN  PUCHAR          pData,
+	IN  UINT            DataLen,
+    IN	BOOLEAN			bClearFrame);
+
+VOID RTMPAddPMKIDCache(
+	IN  PRTMP_ADAPTER   		pAd,
+	IN	INT						apidx,
+	IN	PUCHAR				pAddr,
+	IN	UCHAR					*PMKID,
+	IN	UCHAR					*PMK);
+
+INT RTMPSearchPMKIDCache(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	INT				apidx,
+	IN	PUCHAR		pAddr);
+
+VOID RTMPDeletePMKIDCache(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	INT				apidx,
+	IN  INT				idx);
+
+VOID RTMPMaintainPMKIDCache(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID	RTMPSendTriggerFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PVOID			pBuffer,
+	IN	ULONG			Length,
+	IN  UCHAR           TxRate,
+	IN	BOOLEAN			bQosNull);
+
+
+/* timeout -- ms */
+VOID RTMP_SetPeriodicTimer(
+	IN	NDIS_MINIPORT_TIMER *pTimer,
+	IN	unsigned long timeout);
+
+VOID RTMP_OS_Init_Timer(
+	IN	PRTMP_ADAPTER pAd,
+	IN	NDIS_MINIPORT_TIMER *pTimer,
+	IN	TIMER_FUNCTION function,
+	IN	PVOID data);
+
+VOID RTMP_OS_Add_Timer(
+	IN	NDIS_MINIPORT_TIMER	*pTimer,
+	IN	unsigned long timeout);
+
+VOID RTMP_OS_Mod_Timer(
+	IN	NDIS_MINIPORT_TIMER	*pTimer,
+	IN	unsigned long timeout);
+
+
+VOID RTMP_OS_Del_Timer(
+	IN	NDIS_MINIPORT_TIMER	*pTimer,
+	OUT	BOOLEAN				 *pCancelled);
+
+
+VOID RTMP_OS_Release_Packet(
+	IN	PRTMP_ADAPTER pAd,
+	IN	PQUEUE_ENTRY  pEntry);
+
+VOID RTMPusecDelay(
+	IN	ULONG	usec);
+
+NDIS_STATUS os_alloc_mem(
+	IN	PRTMP_ADAPTER pAd,
+	OUT	PUCHAR *mem,
+	IN	ULONG  size);
+
+NDIS_STATUS os_free_mem(
+	IN	PRTMP_ADAPTER pAd,
+	IN	PUCHAR mem);
+
+
+void RTMP_AllocateSharedMemory(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress,
+	OUT	PNDIS_PHYSICAL_ADDRESS PhysicalAddress);
+
+VOID RTMPFreeTxRxRingMemory(
+    IN  PRTMP_ADAPTER   pAd);
+
+NDIS_STATUS AdapterBlockAllocateMemory(
+	IN PVOID	handle,
+	OUT	PVOID	*ppAd);
+
+void RTMP_AllocateTxDescMemory(
+	IN	PRTMP_ADAPTER pAd,
+	IN	UINT	Index,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress,
+	OUT	PNDIS_PHYSICAL_ADDRESS PhysicalAddress);
+
+void RTMP_AllocateFirstTxBuffer(
+	IN	PRTMP_ADAPTER pAd,
+	IN	UINT	Index,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress,
+	OUT	PNDIS_PHYSICAL_ADDRESS PhysicalAddress);
+
+void RTMP_AllocateMgmtDescMemory(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress,
+	OUT	PNDIS_PHYSICAL_ADDRESS PhysicalAddress);
+
+void RTMP_AllocateRxDescMemory(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress,
+	OUT	PNDIS_PHYSICAL_ADDRESS PhysicalAddress);
+
+PNDIS_PACKET RTMP_AllocateRxPacketBuffer(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress,
+	OUT	PNDIS_PHYSICAL_ADDRESS PhysicalAddress);
+
+PNDIS_PACKET RTMP_AllocateTxPacketBuffer(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress);
+
+PNDIS_PACKET RTMP_AllocateFragPacketBuffer(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length);
+
+void RTMP_QueryPacketInfo(
+	IN  PNDIS_PACKET pPacket,
+	OUT PACKET_INFO  *pPacketInfo,
+	OUT PUCHAR		 *pSrcBufVA,
+	OUT	UINT		 *pSrcBufLen);
+
+void RTMP_QueryNextPacketInfo(
+	IN  PNDIS_PACKET *ppPacket,
+	OUT PACKET_INFO  *pPacketInfo,
+	OUT PUCHAR		 *pSrcBufVA,
+	OUT	UINT		 *pSrcBufLen);
+
+
+BOOLEAN RTMP_FillTxBlkInfo(
+	IN RTMP_ADAPTER *pAd,
+	IN TX_BLK *pTxBlk);
+
+
+PRTMP_SCATTER_GATHER_LIST
+rt_get_sg_list_from_packet(PNDIS_PACKET pPacket, RTMP_SCATTER_GATHER_LIST *sg);
+
+
+ void announce_802_3_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket);
+
+
+UINT BA_Reorder_AMSDU_Annnounce(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket);
+
+
+UINT Handle_AMSDU_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize,
+	IN  UCHAR			FromWhichBSSID);
+
+
+void convert_802_11_to_802_3_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	PUCHAR			p8023hdr,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize,
+	IN  UCHAR			FromWhichBSSID);
+
+
+PNET_DEV get_netdev_from_bssid(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			FromWhichBSSID);
+
+
+PNDIS_PACKET duplicate_pkt(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pHeader802_3,
+    IN  UINT            HdrLen,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize,
+	IN	UCHAR			FromWhichBSSID);
+
+
+PNDIS_PACKET duplicate_pkt_with_TKIP_MIC(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pOldPkt);
+
+PNDIS_PACKET duplicate_pkt_with_VLAN(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pHeader802_3,
+    IN  UINT            HdrLen,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize,
+	IN	UCHAR			FromWhichBSSID);
+
+PNDIS_PACKET duplicate_pkt_with_WPI(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	UINT32			ext_head_len,
+	IN	UINT32			ext_tail_len);
+
+UCHAR VLAN_8023_Header_Copy(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pHeader802_3,
+	IN	UINT            HdrLen,
+	OUT PUCHAR			pData,
+	IN	UCHAR			FromWhichBSSID);
+
+#ifdef DOT11_N_SUPPORT
+void ba_flush_reordering_timeout_mpdus(
+	IN PRTMP_ADAPTER	pAd,
+	IN PBA_REC_ENTRY	pBAEntry,
+	IN ULONG			Now32);
+
+
+VOID BAOriSessionSetUp(
+			IN PRTMP_ADAPTER    pAd,
+			IN MAC_TABLE_ENTRY	*pEntry,
+			IN UCHAR			TID,
+			IN USHORT			TimeOut,
+			IN ULONG			DelayTime,
+			IN BOOLEAN		isForced);
+
+VOID BASessionTearDownALL(
+	IN OUT	PRTMP_ADAPTER pAd,
+	IN		UCHAR Wcid);
+#endif // DOT11_N_SUPPORT //
+
+BOOLEAN OS_Need_Clone_Packet(void);
+
+
+VOID build_tx_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	PUCHAR	pFrame,
+	IN	ULONG	FrameLen);
+
+
+VOID BAOriSessionTearDown(
+	IN OUT	PRTMP_ADAPTER	pAd,
+	IN		UCHAR			Wcid,
+	IN		UCHAR			TID,
+	IN		BOOLEAN			bPassive,
+	IN		BOOLEAN			bForceSend);
+
+VOID BARecSessionTearDown(
+	IN OUT	PRTMP_ADAPTER	pAd,
+	IN		UCHAR			Wcid,
+	IN		UCHAR			TID,
+	IN		BOOLEAN			bPassive);
+
+BOOLEAN ba_reordering_resource_init(PRTMP_ADAPTER pAd, int num);
+void ba_reordering_resource_release(PRTMP_ADAPTER pAd);
+
+ULONG AutoChBssInsertEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pBssid,
+	IN CHAR Ssid[],
+	IN UCHAR SsidLen,
+	IN UCHAR ChannelNo,
+	IN CHAR Rssi);
+
+void AutoChBssTableInit(
+	IN PRTMP_ADAPTER pAd);
+
+void ChannelInfoInit(
+	IN PRTMP_ADAPTER pAd);
+
+void AutoChBssTableDestroy(
+	IN PRTMP_ADAPTER pAd);
+
+void ChannelInfoDestroy(
+	IN PRTMP_ADAPTER pAd);
+
+UCHAR New_ApAutoSelectChannel(
+	IN PRTMP_ADAPTER pAd);
+
+BOOLEAN rtstrmactohex(
+	IN char *s1,
+	IN char *s2);
+
+BOOLEAN rtstrcasecmp(
+	IN char *s1,
+	IN char *s2);
+
+char *rtstrstruncasecmp(
+	IN char *s1,
+	IN char *s2);
+
+char    *rtstrstr(
+	IN	const char * s1,
+	IN	const char * s2);
+
+char *rstrtok(
+	IN char * s,
+	IN const char * ct);
+
+int rtinet_aton(
+	const char *cp,
+	unsigned int *addr);
+
+////////// common ioctl functions //////////
+INT Set_DriverVersion_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_CountryRegion_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_CountryRegionABand_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_WirelessMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_Channel_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ShortSlot_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_TxPower_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_BGProtection_Proc(
+	IN  PRTMP_ADAPTER		pAd,
+	IN  PUCHAR			arg);
+
+INT Set_TxPreamble_Proc(
+	IN  PRTMP_ADAPTER		pAd,
+	IN  PUCHAR			arg);
+
+INT Set_RTSThreshold_Proc(
+	IN  PRTMP_ADAPTER		pAd,
+	IN  PUCHAR			arg);
+
+INT Set_FragThreshold_Proc(
+	IN  PRTMP_ADAPTER		pAd,
+	IN  PUCHAR			arg);
+
+INT Set_TxBurst_Proc(
+	IN  PRTMP_ADAPTER		pAd,
+	IN  PUCHAR			arg);
+
+#ifdef AGGREGATION_SUPPORT
+INT	Set_PktAggregate_Proc(
+	IN  PRTMP_ADAPTER		pAd,
+	IN  PUCHAR			arg);
+#endif
+
+INT	Set_IEEE80211H_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+#ifdef DBG
+INT	Set_Debug_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif
+
+INT	Show_DescInfo_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ResetStatCounter_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+#ifdef DOT11_N_SUPPORT
+INT	Set_BASetup_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_BADecline_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_BAOriTearDown_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_BARecTearDown_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtBw_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtMcs_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtGi_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtOpMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtStbc_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtHtc_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtExtcha_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtMpduDensity_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtBaWinSize_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtRdg_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtLinkAdapt_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtAmsdu_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtAutoBa_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtProtect_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtMimoPs_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+
+INT	Set_ForceShortGI_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ForceGF_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	SetCommonHT(
+	IN	PRTMP_ADAPTER	pAd);
+
+INT	Set_SendPSMPAction_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtMIMOPSmode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+
+INT	Set_HtTxBASize_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif // DOT11_N_SUPPORT //
+
+
+
+#ifdef CONFIG_STA_SUPPORT
+//Dls ,	kathy
+VOID RTMPSendDLSTearDownFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pDA);
+
+#ifdef DOT11_N_SUPPORT
+//Block ACK
+VOID QueryBATABLE(
+	IN  PRTMP_ADAPTER pAd,
+	OUT PQUERYBA_TABLE pBAT);
+#endif // DOT11_N_SUPPORT //
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+INT	    WpaCheckEapCode(
+	IN  PRTMP_ADAPTER   	pAd,
+	IN  PUCHAR				pFrame,
+	IN  USHORT				FrameLen,
+	IN  USHORT				OffSet);
+
+VOID    WpaSendMicFailureToWpaSupplicant(
+    IN  PRTMP_ADAPTER       pAd,
+    IN  BOOLEAN             bUnicast);
+
+VOID    SendAssocIEsToWpaSupplicant(
+    IN  PRTMP_ADAPTER       pAd);
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+int wext_notify_event_assoc(
+	IN  RTMP_ADAPTER *pAd);
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+
+#endif // CONFIG_STA_SUPPORT //
+
+
+
+#ifdef DOT11_N_SUPPORT
+VOID Handle_BSS_Width_Trigger_Events(
+	IN PRTMP_ADAPTER pAd);
+
+void build_ext_channel_switch_ie(
+	IN PRTMP_ADAPTER pAd,
+	IN HT_EXT_CHANNEL_SWITCH_ANNOUNCEMENT_IE *pIE);
+#endif // DOT11_N_SUPPORT //
+
+
+BOOLEAN APRxDoneInterruptHandle(
+	IN	PRTMP_ADAPTER	pAd);
+
+BOOLEAN STARxDoneInterruptHandle(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	BOOLEAN			argc);
+
+#ifdef DOT11_N_SUPPORT
+// AMPDU packet indication
+VOID Indicate_AMPDU_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID);
+
+// AMSDU packet indication
+VOID Indicate_AMSDU_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID);
+#endif // DOT11_N_SUPPORT //
+
+// Normal legacy Rx packet indication
+VOID Indicate_Legacy_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID);
+
+VOID Indicate_EAPOL_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID);
+
+void  update_os_packet_info(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID);
+
+void wlan_802_11_to_802_3_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	PUCHAR			pHeader802_3,
+	IN  UCHAR			FromWhichBSSID);
+
+UINT deaggregate_AMSDU_announce(
+	IN	PRTMP_ADAPTER	pAd,
+	PNDIS_PACKET		pPacket,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize);
+
+
+#ifdef CONFIG_STA_SUPPORT
+// remove LLC and get 802_3 Header
+#define  RTMP_802_11_REMOVE_LLC_AND_CONVERT_TO_802_3(_pRxBlk, _pHeader802_3)	\
+{																				\
+	PUCHAR _pRemovedLLCSNAP = NULL, _pDA, _pSA;                                 \
+																				\
+	if (RX_BLK_TEST_FLAG(_pRxBlk, fRX_MESH))                                    \
+	{                                                                           \
+		_pDA = _pRxBlk->pHeader->Addr3;                                         \
+		_pSA = (PUCHAR)_pRxBlk->pHeader + sizeof(HEADER_802_11);                \
+	}                                                                           \
+	else                                                                        \
+	{                                                                           \
+		if (RX_BLK_TEST_FLAG(_pRxBlk, fRX_INFRA))                              	\
+		{                                                                       \
+			_pDA = _pRxBlk->pHeader->Addr1;                                     \
+		if (RX_BLK_TEST_FLAG(_pRxBlk, fRX_DLS))									\
+			_pSA = _pRxBlk->pHeader->Addr2;										\
+		else																	\
+			_pSA = _pRxBlk->pHeader->Addr3;                                     \
+		}                                                                       \
+		else                                                                    \
+		{                                                                       \
+			_pDA = _pRxBlk->pHeader->Addr1;                                     \
+			_pSA = _pRxBlk->pHeader->Addr2;                                     \
+		}                                                                       \
+	}                                                                           \
+																				\
+	CONVERT_TO_802_3(_pHeader802_3, _pDA, _pSA, _pRxBlk->pData, 				\
+		_pRxBlk->DataSize, _pRemovedLLCSNAP);                                   \
+}
+#endif // CONFIG_STA_SUPPORT //
+
+
+BOOLEAN APFowardWirelessStaToWirelessSta(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	ULONG			FromWhichBSSID);
+
+VOID Announce_or_Forward_802_3_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	UCHAR			FromWhichBSSID);
+
+VOID Sta_Announce_or_Forward_802_3_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	UCHAR			FromWhichBSSID);
+
+
+#ifdef CONFIG_STA_SUPPORT
+#define ANNOUNCE_OR_FORWARD_802_3_PACKET(_pAd, _pPacket, _FromWhichBSS)\
+			Sta_Announce_or_Forward_802_3_Packet(_pAd, _pPacket, _FromWhichBSS);
+			//announce_802_3_packet(_pAd, _pPacket);
+#endif // CONFIG_STA_SUPPORT //
+
+
+PNDIS_PACKET DuplicatePacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	UCHAR			FromWhichBSSID);
+
+
+PNDIS_PACKET ClonePacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize);
+
+
+// Normal, AMPDU or AMSDU
+VOID CmmRxnonRalinkFrameIndicate(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID);
+
+VOID CmmRxRalinkFrameIndicate(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID);
+
+VOID Update_Rssi_Sample(
+	IN PRTMP_ADAPTER	pAd,
+	IN RSSI_SAMPLE		*pRssi,
+	IN PRXWI_STRUC		pRxWI);
+
+PNDIS_PACKET GetPacketFromRxRing(
+	IN		PRTMP_ADAPTER	pAd,
+	OUT		PRT28XX_RXD_STRUC		pSaveRxD,
+	OUT		BOOLEAN			*pbReschedule,
+	IN OUT	UINT32			*pRxPending);
+
+PNDIS_PACKET RTMPDeFragmentDataFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk);
+
+////////////////////////////////////////
+
+
+
+
+
+#ifdef SNMP_SUPPORT
+//for snmp , kathy
+typedef struct _DefaultKeyIdxValue
+{
+	UCHAR	KeyIdx;
+	UCHAR	Value[16];
+} DefaultKeyIdxValue, *PDefaultKeyIdxValue;
+#endif
+
+
+#ifdef CONFIG_STA_SUPPORT
+enum {
+	DIDmsg_lnxind_wlansniffrm		= 0x00000044,
+	DIDmsg_lnxind_wlansniffrm_hosttime	= 0x00010044,
+	DIDmsg_lnxind_wlansniffrm_mactime	= 0x00020044,
+	DIDmsg_lnxind_wlansniffrm_channel	= 0x00030044,
+	DIDmsg_lnxind_wlansniffrm_rssi		= 0x00040044,
+	DIDmsg_lnxind_wlansniffrm_sq		= 0x00050044,
+	DIDmsg_lnxind_wlansniffrm_signal	= 0x00060044,
+	DIDmsg_lnxind_wlansniffrm_noise		= 0x00070044,
+	DIDmsg_lnxind_wlansniffrm_rate		= 0x00080044,
+	DIDmsg_lnxind_wlansniffrm_istx		= 0x00090044,
+	DIDmsg_lnxind_wlansniffrm_frmlen	= 0x000A0044
+};
+enum {
+	P80211ENUM_msgitem_status_no_value	= 0x00
+};
+enum {
+	P80211ENUM_truth_false			= 0x00,
+	P80211ENUM_truth_true			= 0x01
+};
+
+/* Definition from madwifi */
+typedef struct {
+        UINT32 did;
+        UINT16 status;
+        UINT16 len;
+        UINT32 data;
+} p80211item_uint32_t;
+
+typedef struct {
+        UINT32 msgcode;
+        UINT32 msglen;
+#define WLAN_DEVNAMELEN_MAX 16
+        UINT8 devname[WLAN_DEVNAMELEN_MAX];
+        p80211item_uint32_t hosttime;
+        p80211item_uint32_t mactime;
+        p80211item_uint32_t channel;
+        p80211item_uint32_t rssi;
+        p80211item_uint32_t sq;
+        p80211item_uint32_t signal;
+        p80211item_uint32_t noise;
+        p80211item_uint32_t rate;
+        p80211item_uint32_t istx;
+        p80211item_uint32_t frmlen;
+} wlan_ng_prism2_header;
+
+/* The radio capture header precedes the 802.11 header. */
+typedef struct PACKED _ieee80211_radiotap_header {
+    UINT8	it_version;	/* Version 0. Only increases
+				 * for drastic changes,
+				 * introduction of compatible
+				 * new fields does not count.
+				 */
+    UINT8	it_pad;
+    UINT16     it_len;         /* length of the whole
+				 * header in bytes, including
+				 * it_version, it_pad,
+				 * it_len, and data fields.
+				 */
+    UINT32   it_present;	/* A bitmap telling which
+					 * fields are present. Set bit 31
+					 * (0x80000000) to extend the
+					 * bitmap by another 32 bits.
+					 * Additional extensions are made
+					 * by setting bit 31.
+					 */
+}ieee80211_radiotap_header ;
+
+enum ieee80211_radiotap_type {
+    IEEE80211_RADIOTAP_TSFT = 0,
+    IEEE80211_RADIOTAP_FLAGS = 1,
+    IEEE80211_RADIOTAP_RATE = 2,
+    IEEE80211_RADIOTAP_CHANNEL = 3,
+    IEEE80211_RADIOTAP_FHSS = 4,
+    IEEE80211_RADIOTAP_DBM_ANTSIGNAL = 5,
+    IEEE80211_RADIOTAP_DBM_ANTNOISE = 6,
+    IEEE80211_RADIOTAP_LOCK_QUALITY = 7,
+    IEEE80211_RADIOTAP_TX_ATTENUATION = 8,
+    IEEE80211_RADIOTAP_DB_TX_ATTENUATION = 9,
+    IEEE80211_RADIOTAP_DBM_TX_POWER = 10,
+    IEEE80211_RADIOTAP_ANTENNA = 11,
+    IEEE80211_RADIOTAP_DB_ANTSIGNAL = 12,
+    IEEE80211_RADIOTAP_DB_ANTNOISE = 13
+};
+
+#define WLAN_RADIOTAP_PRESENT (			\
+	(1 << IEEE80211_RADIOTAP_TSFT)	|	\
+	(1 << IEEE80211_RADIOTAP_FLAGS) |	\
+	(1 << IEEE80211_RADIOTAP_RATE)  | 	\
+	 0)
+
+typedef struct _wlan_radiotap_header {
+	ieee80211_radiotap_header wt_ihdr;
+	INT64 wt_tsft;
+	UINT8 wt_flags;
+	UINT8 wt_rate;
+} wlan_radiotap_header;
+/* Definition from madwifi */
+
+void send_monitor_packets(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk);
+
+#if WIRELESS_EXT >= 12
+// This function will be called when query /proc
+struct iw_statistics *rt28xx_get_wireless_stats(
+    IN struct net_device *net_dev);
+#endif
+
+VOID    RTMPSetDesiredRates(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  LONG            Rates);
+#endif // CONFIG_STA_SUPPORT //
+
+INT	Set_FixedTxMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+INT	Set_OpMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+
+static inline char* GetPhyMode(
+	int Mode)
+{
+	switch(Mode)
+	{
+		case MODE_CCK:
+			return "CCK";
+
+		case MODE_OFDM:
+			return "OFDM";
+#ifdef DOT11_N_SUPPORT
+		case MODE_HTMIX:
+			return "HTMIX";
+
+		case MODE_HTGREENFIELD:
+			return "GREEN";
+#endif // DOT11_N_SUPPORT //
+		default:
+			return "N/A";
+	}
+}
+
+
+static inline char* GetBW(
+	int BW)
+{
+	switch(BW)
+	{
+		case BW_10:
+			return "10M";
+
+		case BW_20:
+			return "20M";
+#ifdef DOT11_N_SUPPORT
+		case BW_40:
+			return "40M";
+#endif // DOT11_N_SUPPORT //
+		default:
+			return "N/A";
+	}
+}
+
+
+VOID RT28xxThreadTerminate(
+	IN RTMP_ADAPTER *pAd);
+
+BOOLEAN RT28XXChipsetCheck(
+	IN void *_dev_p);
+
+BOOLEAN RT28XXNetDevInit(
+	IN void 				*_dev_p,
+	IN struct  net_device	*net_dev,
+	IN RTMP_ADAPTER 		*pAd);
+
+BOOLEAN RT28XXProbePostConfig(
+	IN void 				*_dev_p,
+	IN RTMP_ADAPTER 		*pAd,
+	IN INT32				argc);
+
+VOID RT28XXDMADisable(
+	IN RTMP_ADAPTER 		*pAd);
+
+VOID RT28XXDMAEnable(
+	IN RTMP_ADAPTER 		*pAd);
+
+VOID RT28xx_UpdateBeaconToAsic(
+	IN RTMP_ADAPTER * pAd,
+	IN INT apidx,
+	IN ULONG BeaconLen,
+	IN ULONG UpdatePos);
+
+INT rt28xx_ioctl(
+	IN	struct net_device	*net_dev,
+	IN	OUT	struct ifreq	*rq,
+	IN	INT			cmd);
+
+
+#ifdef CONFIG_STA_SUPPORT
+INT rt28xx_sta_ioctl(
+	IN	struct net_device	*net_dev,
+	IN	OUT	struct ifreq	*rq,
+	IN	INT			cmd);
+#endif // CONFIG_STA_SUPPORT //
+
+BOOLEAN RT28XXSecurityKeyAdd(
+	IN		PRTMP_ADAPTER		pAd,
+	IN		ULONG				apidx,
+	IN		ULONG				KeyIdx,
+	IN		MAC_TABLE_ENTRY 	*pEntry);
+
+////////////////////////////////////////
+PNDIS_PACKET GetPacketFromRxRing(
+	IN		PRTMP_ADAPTER	pAd,
+	OUT		PRT28XX_RXD_STRUC	pSaveRxD,
+	OUT		BOOLEAN			*pbReschedule,
+	IN OUT	UINT32			*pRxPending);
+
+
+void kill_thread_task(PRTMP_ADAPTER pAd);
+
+void tbtt_tasklet(unsigned long data);
+
+#ifdef RT2860
+//
+// Function Prototype in cmm_data_2860.c
+//
+USHORT RtmpPCI_WriteTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	BOOLEAN			bIsLast,
+	OUT	USHORT			*FreeNumber);
+
+USHORT RtmpPCI_WriteSingleTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	BOOLEAN			bIsLast,
+	OUT	USHORT			*FreeNumber);
+
+USHORT RtmpPCI_WriteMultiTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	UCHAR			frameNum,
+	OUT	USHORT			*FreeNumber);
+
+USHORT	RtmpPCI_WriteFragTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	UCHAR			fragNum,
+	OUT	USHORT			*FreeNumber);
+
+USHORT RtmpPCI_WriteSubTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	BOOLEAN			bIsLast,
+	OUT	USHORT			*FreeNumber);
+
+VOID RtmpPCI_FinalWriteTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	USHORT			totalMPDUSize,
+	IN	USHORT			FirstTxIdx);
+
+VOID RtmpPCIDataLastTxIdx(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			QueIdx,
+	IN	USHORT			LastTxIdx);
+
+VOID RtmpPCIDataKickOut(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	UCHAR			QueIdx);
+
+
+int RtmpPCIMgmtKickOut(
+	IN RTMP_ADAPTER 	*pAd,
+	IN UCHAR 			QueIdx,
+	IN PNDIS_PACKET		pPacket,
+	IN PUCHAR			pSrcBufVA,
+	IN UINT 			SrcBufLen);
+
+
+NDIS_STATUS RTMPCheckRxError(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PHEADER_802_11  pHeader,
+	IN	PRXWI_STRUC	pRxWI,
+	IN  PRT28XX_RXD_STRUC      pRxD);
+
+#ifdef CONFIG_STA_SUPPORT
+VOID RTMPInitPCIeLinkCtrlValue(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID RTMPFindHostPCIDev(
+    IN	PRTMP_ADAPTER	pAd);
+
+VOID RTMPPCIeLinkCtrlValueRestore(
+	IN	PRTMP_ADAPTER	pAd,
+	IN   UCHAR		Level);
+
+VOID RTMPPCIeLinkCtrlSetting(
+	IN	PRTMP_ADAPTER	pAd,
+	IN 	USHORT		Max);
+
+VOID RT28xxPciAsicRadioOff(
+	IN PRTMP_ADAPTER    pAd,
+	IN UCHAR            Level,
+	IN USHORT           TbttNumToNextWakeUp);
+
+BOOLEAN RT28xxPciAsicRadioOn(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR     Level);
+
+VOID RT28xxPciStaAsicForceWakeup(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN       bFromTx);
+
+VOID RT28xxPciStaAsicSleepThenAutoWakeup(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT TbttNumToNextWakeUp);
+
+VOID PsPollWakeExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID  RadioOnExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+#endif // CONFIG_STA_SUPPORT //
+
+VOID RT28xxPciMlmeRadioOn(
+	IN PRTMP_ADAPTER pAd);
+
+VOID RT28xxPciMlmeRadioOFF(
+	IN PRTMP_ADAPTER pAd);
+#endif // RT2860 //
+
+VOID AsicTurnOffRFClk(
+	IN PRTMP_ADAPTER    pAd,
+	IN	UCHAR           Channel);
+
+VOID AsicTurnOnRFClk(
+	IN PRTMP_ADAPTER 	pAd,
+	IN	UCHAR			Channel);
+
+
+////////////////////////////////////////
+
+VOID QBSS_LoadInit(
+ 	IN		RTMP_ADAPTER	*pAd);
+
+UINT32 QBSS_LoadElementAppend(
+ 	IN		RTMP_ADAPTER	*pAd,
+	OUT		UINT8			*buf_p);
+
+VOID QBSS_LoadUpdate(
+ 	IN		RTMP_ADAPTER	*pAd);
+
+///////////////////////////////////////
+INT RTMPShowCfgValue(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pName,
+	IN	PUCHAR			pBuf);
+
+PCHAR   RTMPGetRalinkAuthModeStr(
+    IN  NDIS_802_11_AUTHENTICATION_MODE authMode);
+
+PCHAR   RTMPGetRalinkEncryModeStr(
+    IN  USHORT encryMode);
+//////////////////////////////////////
+
+#ifdef CONFIG_STA_SUPPORT
+VOID AsicStaBbpTuning(
+	IN PRTMP_ADAPTER pAd);
+#endif // CONFIG_STA_SUPPORT //
+
+void RTMP_IndicateMediaState(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID ReSyncBeaconTime(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID RTMPSetAGCInitValue(
+	IN PRTMP_ADAPTER	pAd,
+	IN UCHAR			BandWidth);
+
+int rt28xx_close(IN PNET_DEV dev);
+int rt28xx_open(IN PNET_DEV dev);
+
+__inline INT VIRTUAL_IF_UP(PRTMP_ADAPTER pAd)
+{
+extern VOID MeshMakeBeacon(IN PRTMP_ADAPTER pAd, IN UCHAR idx);
+extern VOID MeshUpdateBeaconFrame(IN PRTMP_ADAPTER pAd, IN UCHAR idx);
+
+	if (VIRTUAL_IF_NUM(pAd) == 0)
+	{
+		if (rt28xx_open(pAd->net_dev) != 0)
+			return -1;
+	}
+	else
+	{
+	}
+	VIRTUAL_IF_INC(pAd);
+	return 0;
+}
+
+__inline VOID VIRTUAL_IF_DOWN(PRTMP_ADAPTER pAd)
+{
+	VIRTUAL_IF_DEC(pAd);
+	if (VIRTUAL_IF_NUM(pAd) == 0)
+		rt28xx_close(pAd->net_dev);
+	return;
+}
+
+
+#endif  // __RTMP_H__
+
diff --git a/drivers/staging/rt2860/rtmp_ckipmic.h b/drivers/staging/rt2860/rtmp_ckipmic.h
new file mode 100644
index 00000000000..a3d949a39d3
--- /dev/null
+++ b/drivers/staging/rt2860/rtmp_ckipmic.h
@@ -0,0 +1,113 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rtmp_ckipmic.h
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+*/
+#ifndef	__RTMP_CKIPMIC_H__
+#define	__RTMP_CKIPMIC_H__
+
+typedef	struct	_MIC_CONTEXT	{
+	/* --- MMH context                            */
+	UCHAR		CK[16];				/* the key                                    */
+	UCHAR		coefficient[16];	/* current aes counter mode coefficients      */
+	ULONGLONG	accum;				/* accumulated mic, reduced to u32 in final() */
+	UINT		position;			/* current position (byte offset) in message  */
+	UCHAR		part[4];			/* for conversion of message to u32 for mmh   */
+}	MIC_CONTEXT, *PMIC_CONTEXT;
+
+VOID	CKIP_key_permute(
+	OUT	UCHAR	*PK,			/* output permuted key */
+	IN	UCHAR	*CK,			/* input CKIP key */
+	IN	UCHAR	toDsFromDs,		/* input toDs/FromDs bits */
+	IN	UCHAR	*piv);			/* input pointer to IV */
+
+VOID	RTMPCkipMicInit(
+	IN	PMIC_CONTEXT		pContext,
+	IN	PUCHAR				CK);
+
+VOID RTMPMicUpdate(
+    IN  PMIC_CONTEXT        pContext,
+    IN  PUCHAR              pOctets,
+    IN  INT                 len);
+
+ULONG RTMPMicGetCoefficient(
+    IN  PMIC_CONTEXT         pContext);
+
+VOID xor_128(
+    IN  PUCHAR              a,
+    IN  PUCHAR              b,
+    OUT PUCHAR              out);
+
+UCHAR RTMPCkipSbox(
+    IN  UCHAR               a);
+
+VOID xor_32(
+    IN  PUCHAR              a,
+    IN  PUCHAR              b,
+    OUT PUCHAR              out);
+
+VOID next_key(
+    IN  PUCHAR              key,
+    IN  INT                 round);
+
+VOID byte_sub(
+    IN  PUCHAR              in,
+    OUT PUCHAR              out);
+
+VOID shift_row(
+    IN  PUCHAR              in,
+    OUT PUCHAR              out);
+
+VOID mix_column(
+    IN  PUCHAR              in,
+    OUT PUCHAR              out);
+
+VOID RTMPAesEncrypt(
+    IN  PUCHAR              key,
+    IN  PUCHAR              data,
+    IN  PUCHAR              ciphertext);
+
+VOID RTMPMicFinal(
+    IN  PMIC_CONTEXT        pContext,
+    OUT UCHAR               digest[4]);
+
+VOID RTMPCkipInsertCMIC(
+    IN  PRTMP_ADAPTER   pAd,
+    OUT PUCHAR          pMIC,
+    IN  PUCHAR          p80211hdr,
+    IN  PNDIS_PACKET    pPacket,
+    IN  PCIPHER_KEY     pKey,
+    IN  PUCHAR          mic_snap);
+
+#endif //__RTMP_CKIPMIC_H__
diff --git a/drivers/staging/rt2860/rtmp_def.h b/drivers/staging/rt2860/rtmp_def.h
new file mode 100644
index 00000000000..bb6f37b0bdc
--- /dev/null
+++ b/drivers/staging/rt2860/rtmp_def.h
@@ -0,0 +1,1588 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    rtmp_def.h
+
+    Abstract:
+    Miniport related definition header
+
+    Revision History:
+    Who         When          What
+    --------    ----------    ----------------------------------------------
+    Paul Lin    08-01-2002    created
+    John Chang  08-05-2003    add definition for 11g & other drafts
+*/
+#ifndef __RTMP_DEF_H__
+#define __RTMP_DEF_H__
+
+#include "oid.h"
+
+//
+//  Debug information verbosity: lower values indicate higher urgency
+//
+#define RT_DEBUG_OFF        0
+#define RT_DEBUG_ERROR      1
+#define RT_DEBUG_WARN       2
+#define RT_DEBUG_TRACE      3
+#define RT_DEBUG_INFO       4
+#define RT_DEBUG_LOUD       5
+
+#define NIC_TAG             ((ULONG)'0682')
+#define NIC_DBG_STRING      ("**RT28xx**")
+
+#ifdef SNMP_SUPPORT
+// for snmp
+// to get manufacturer OUI, kathy, 2008_0220
+#define ManufacturerOUI_LEN			3
+#define ManufacturerNAME			("Ralink Technology Company.")
+#define	ResourceTypeIdName			("Ralink_ID")
+#endif
+
+
+#define RALINK_2883_VERSION		((UINT32)0x28830300)
+#define RALINK_2880E_VERSION	((UINT32)0x28720200)
+#define RALINK_3070_VERSION		((UINT32)0x30700200)
+
+//
+// NDIS version in use by the NIC driver.
+// The high byte is the major version. The low byte is the minor version.
+//
+#ifdef  NDIS51_MINIPORT
+#define NIC_DRIVER_VERSION      0x0501
+#else
+#define NIC_DRIVER_VERSION      0x0500
+#endif
+
+//
+// NDIS media type, current is ethernet, change if native wireless supported
+//
+#define NIC_MEDIA_TYPE          NdisMedium802_3
+#define NIC_PCI_HDR_LENGTH      0xe2
+#define NIC_MAX_PACKET_SIZE     2304
+#define NIC_HEADER_SIZE         14
+#define MAX_MAP_REGISTERS_NEEDED 32
+#define MIN_MAP_REGISTERS_NEEDED 2   //Todo: should consider fragment issue.
+
+//
+// interface type, we use PCI
+//
+#define NIC_INTERFACE_TYPE      NdisInterfacePci
+#define NIC_INTERRUPT_MODE      NdisInterruptLevelSensitive
+
+//
+// buffer size passed in NdisMQueryAdapterResources
+// We should only need three adapter resources (IO, interrupt and memory),
+// Some devices get extra resources, so have room for 10 resources
+//                    UF_SIZE   (sizeof(NDIS_RESOURCE_LIST) + (10*sizeof(CM_PARTIAL_RESOURCE_DESCRIPTOR)))
+
+
+#define NIC_RESOURCE_B//
+// IO space length
+//
+#define NIC_MAP_IOSPACE_LENGTH  sizeof(CSR_STRUC)
+
+#define MAX_RX_PKT_LEN	1520
+
+//
+// Entry number for each DMA descriptor ring
+//
+
+#ifdef RT2860
+#define TX_RING_SIZE            64 //64
+#define MGMT_RING_SIZE          128
+#define RX_RING_SIZE            128 //64
+#define MAX_TX_PROCESS          TX_RING_SIZE //8
+#define MAX_DMA_DONE_PROCESS    TX_RING_SIZE
+#define MAX_TX_DONE_PROCESS     TX_RING_SIZE //8
+#define LOCAL_TXBUF_SIZE        2
+#endif // RT2860 //
+
+
+#ifdef MULTIPLE_CARD_SUPPORT
+// MC: Multple Cards
+#define MAX_NUM_OF_MULTIPLE_CARD		32
+#endif // MULTIPLE_CARD_SUPPORT //
+
+#define MAX_RX_PROCESS          128 //64 //32
+#define NUM_OF_LOCAL_TXBUF      2
+#define TXD_SIZE                16
+#define TXWI_SIZE               16
+#define RXD_SIZE               	16
+#define RXWI_SIZE             	16
+// TXINFO_SIZE + TXWI_SIZE + 802.11 Header Size + AMSDU sub frame header
+#define TX_DMA_1ST_BUFFER_SIZE  96    // only the 1st physical buffer is pre-allocated
+#define MGMT_DMA_BUFFER_SIZE    1536 //2048
+#define RX_BUFFER_AGGRESIZE     3840 //3904 //3968 //4096 //2048 //4096
+#define RX_BUFFER_NORMSIZE      3840 //3904 //3968 //4096 //2048 //4096
+#define TX_BUFFER_NORMSIZE		RX_BUFFER_NORMSIZE
+#define MAX_FRAME_SIZE          2346                    // Maximum 802.11 frame size
+#define MAX_AGGREGATION_SIZE    3840 //3904 //3968 //4096
+#define MAX_NUM_OF_TUPLE_CACHE  2
+#define MAX_MCAST_LIST_SIZE     32
+#define MAX_LEN_OF_VENDOR_DESC  64
+//#define MAX_SIZE_OF_MCAST_PSQ   (NUM_OF_LOCAL_TXBUF >> 2) // AP won't spend more than 1/4 of total buffers on M/BCAST PSQ
+#define MAX_SIZE_OF_MCAST_PSQ               32
+
+#define MAX_RX_PROCESS_CNT	(RX_RING_SIZE)
+
+
+#define MAX_PACKETS_IN_QUEUE				(512) //(512)    // to pass WMM A5-WPAPSK
+#define MAX_PACKETS_IN_MCAST_PS_QUEUE		32
+#define MAX_PACKETS_IN_PS_QUEUE				128	//32
+#define WMM_NUM_OF_AC                       4  /* AC0, AC1, AC2, and AC3 */
+
+
+
+// RxFilter
+#define STANORMAL	 0x17f97
+#define APNORMAL	 0x15f97
+//
+//  RTMP_ADAPTER flags
+//
+#define fRTMP_ADAPTER_MAP_REGISTER          0x00000001
+#define fRTMP_ADAPTER_INTERRUPT_IN_USE      0x00000002
+#define fRTMP_ADAPTER_HARDWARE_ERROR        0x00000004
+#define fRTMP_ADAPTER_SCATTER_GATHER        0x00000008
+#define fRTMP_ADAPTER_SEND_PACKET_ERROR     0x00000010
+#define fRTMP_ADAPTER_MLME_RESET_IN_PROGRESS 0x00000020
+#define fRTMP_ADAPTER_HALT_IN_PROGRESS      0x00000040
+#define fRTMP_ADAPTER_RESET_IN_PROGRESS     0x00000080
+#define fRTMP_ADAPTER_NIC_NOT_EXIST         0x00000100
+#define fRTMP_ADAPTER_TX_RING_ALLOCATED     0x00000200
+#define fRTMP_ADAPTER_REMOVE_IN_PROGRESS    0x00000400
+#define fRTMP_ADAPTER_MIMORATE_INUSED       0x00000800
+#define fRTMP_ADAPTER_RX_RING_ALLOCATED     0x00001000
+#define fRTMP_ADAPTER_INTERRUPT_ACTIVE      0x00002000
+#define fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS  0x00004000
+#define	fRTMP_ADAPTER_REASSOC_IN_PROGRESS	0x00008000
+#define	fRTMP_ADAPTER_MEDIA_STATE_PENDING	0x00010000
+#define	fRTMP_ADAPTER_RADIO_OFF				0x00020000
+#define fRTMP_ADAPTER_BULKOUT_RESET			0x00040000
+#define	fRTMP_ADAPTER_BULKIN_RESET			0x00080000
+#define fRTMP_ADAPTER_RDG_ACTIVE			0x00100000
+#define fRTMP_ADAPTER_DYNAMIC_BE_TXOP_ACTIVE 0x00200000
+#define fRTMP_ADAPTER_SCAN_2040 			0x04000000
+#define	fRTMP_ADAPTER_RADIO_MEASUREMENT		0x08000000
+
+#define fRTMP_ADAPTER_START_UP         		0x10000000	//Devive already initialized and enabled Tx/Rx.
+#define fRTMP_ADAPTER_MEDIA_STATE_CHANGE    0x20000000
+#define fRTMP_ADAPTER_IDLE_RADIO_OFF        0x40000000
+
+//
+//  STA operation status flags
+//
+#define fOP_STATUS_INFRA_ON                 0x00000001
+#define fOP_STATUS_ADHOC_ON                 0x00000002
+#define fOP_STATUS_BG_PROTECTION_INUSED     0x00000004
+#define fOP_STATUS_SHORT_SLOT_INUSED        0x00000008
+#define fOP_STATUS_SHORT_PREAMBLE_INUSED    0x00000010
+#define fOP_STATUS_RECEIVE_DTIM             0x00000020
+#define fOP_STATUS_MEDIA_STATE_CONNECTED    0x00000080
+#define fOP_STATUS_WMM_INUSED               0x00000100
+#define fOP_STATUS_AGGREGATION_INUSED       0x00000200
+#define fOP_STATUS_DOZE                     0x00000400  // debug purpose
+#define fOP_STATUS_PIGGYBACK_INUSED         0x00000800  // piggy-back, and aggregation
+#define fOP_STATUS_APSD_INUSED				0x00001000
+#define fOP_STATUS_TX_AMSDU_INUSED			0x00002000
+#define fOP_STATUS_MAX_RETRY_ENABLED		0x00004000
+#define fOP_STATUS_WAKEUP_NOW               0x00008000
+#define fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE       0x00020000
+
+#ifdef DOT11N_DRAFT3
+#define fOP_STATUS_SCAN_2040               	    0x00040000
+#endif // DOT11N_DRAFT3 //
+
+#define CCKSETPROTECT		0x1
+#define OFDMSETPROTECT		0x2
+#define MM20SETPROTECT		0x4
+#define MM40SETPROTECT		0x8
+#define GF20SETPROTECT		0x10
+#define GR40SETPROTECT		0x20
+#define ALLN_SETPROTECT		(GR40SETPROTECT | GF20SETPROTECT | MM40SETPROTECT | MM20SETPROTECT)
+
+//
+//  AP's client table operation status flags
+//
+#define fCLIENT_STATUS_WMM_CAPABLE          0x00000001  // CLIENT can parse QOS DATA frame
+#define fCLIENT_STATUS_AGGREGATION_CAPABLE  0x00000002  // CLIENT can receive Ralink's proprietary TX aggregation frame
+#define fCLIENT_STATUS_PIGGYBACK_CAPABLE    0x00000004  // CLIENT support piggy-back
+#define fCLIENT_STATUS_AMSDU_INUSED			0x00000008
+#define fCLIENT_STATUS_SGI20_CAPABLE		0x00000010
+#define fCLIENT_STATUS_SGI40_CAPABLE		0x00000020
+#define fCLIENT_STATUS_TxSTBC_CAPABLE		0x00000040
+#define fCLIENT_STATUS_RxSTBC_CAPABLE		0x00000080
+#define fCLIENT_STATUS_HTC_CAPABLE			0x00000100
+#define fCLIENT_STATUS_RDG_CAPABLE			0x00000200
+#define fCLIENT_STATUS_MCSFEEDBACK_CAPABLE  0x00000400
+#define fCLIENT_STATUS_APSD_CAPABLE         0x00000800  /* UAPSD STATION */
+
+#ifdef DOT11N_DRAFT3
+#define fCLIENT_STATUS_BSSCOEXIST_CAPABLE	0x00001000
+#endif // DOT11N_DRAFT3 //
+
+#define fCLIENT_STATUS_RALINK_CHIPSET		0x00100000
+//
+//  STA configuration flags
+//
+
+// 802.11n Operating Mode Definition. 0-3 also used in ASICUPdateProtect switch case
+#define HT_NO_PROTECT	0
+#define HT_LEGACY_PROTECT	1
+#define HT_40_PROTECT	2
+#define HT_2040_PROTECT	3
+#define HT_RTSCTS_6M	7
+//following is our own definition in order to turn on our ASIC protection register in INFRASTRUCTURE.
+#define HT_ATHEROS	8	// rt2860c has problem with atheros chip. we need to turn on RTS/CTS .
+#define HT_FORCERTSCTS	9	// Force turn on RTS/CTS first. then go to evaluate if this force RTS is necessary.
+
+//
+// RX Packet Filter control flags. Apply on pAd->PacketFilter
+//
+#define fRX_FILTER_ACCEPT_DIRECT            NDIS_PACKET_TYPE_DIRECTED
+#define fRX_FILTER_ACCEPT_MULTICAST         NDIS_PACKET_TYPE_MULTICAST
+#define fRX_FILTER_ACCEPT_BROADCAST         NDIS_PACKET_TYPE_BROADCAST
+#define fRX_FILTER_ACCEPT_ALL_MULTICAST     NDIS_PACKET_TYPE_ALL_MULTICAST
+
+//
+// Error code section
+//
+// NDIS_ERROR_CODE_ADAPTER_NOT_FOUND
+#define ERRLOG_READ_PCI_SLOT_FAILED     0x00000101L
+#define ERRLOG_WRITE_PCI_SLOT_FAILED    0x00000102L
+#define ERRLOG_VENDOR_DEVICE_NOMATCH    0x00000103L
+
+// NDIS_ERROR_CODE_ADAPTER_DISABLED
+#define ERRLOG_BUS_MASTER_DISABLED      0x00000201L
+
+// NDIS_ERROR_CODE_UNSUPPORTED_CONFIGURATION
+#define ERRLOG_INVALID_SPEED_DUPLEX     0x00000301L
+#define ERRLOG_SET_SECONDARY_FAILED     0x00000302L
+
+// NDIS_ERROR_CODE_OUT_OF_RESOURCES
+#define ERRLOG_OUT_OF_MEMORY            0x00000401L
+#define ERRLOG_OUT_OF_SHARED_MEMORY     0x00000402L
+#define ERRLOG_OUT_OF_MAP_REGISTERS     0x00000403L
+#define ERRLOG_OUT_OF_BUFFER_POOL       0x00000404L
+#define ERRLOG_OUT_OF_NDIS_BUFFER       0x00000405L
+#define ERRLOG_OUT_OF_PACKET_POOL       0x00000406L
+#define ERRLOG_OUT_OF_NDIS_PACKET       0x00000407L
+#define ERRLOG_OUT_OF_LOOKASIDE_MEMORY  0x00000408L
+
+// NDIS_ERROR_CODE_HARDWARE_FAILURE
+#define ERRLOG_SELFTEST_FAILED          0x00000501L
+#define ERRLOG_INITIALIZE_ADAPTER       0x00000502L
+#define ERRLOG_REMOVE_MINIPORT          0x00000503L
+
+// NDIS_ERROR_CODE_RESOURCE_CONFLICT
+#define ERRLOG_MAP_IO_SPACE             0x00000601L
+#define ERRLOG_QUERY_ADAPTER_RESOURCES  0x00000602L
+#define ERRLOG_NO_IO_RESOURCE           0x00000603L
+#define ERRLOG_NO_INTERRUPT_RESOURCE    0x00000604L
+#define ERRLOG_NO_MEMORY_RESOURCE       0x00000605L
+
+
+// WDS definition
+#define	MAX_WDS_ENTRY               4
+#define WDS_PAIRWISE_KEY_OFFSET     60    // WDS links uses pairwise key#60 ~ 63 in ASIC pairwise key table
+
+#define	WDS_DISABLE_MODE            0
+#define	WDS_RESTRICT_MODE           1
+#define	WDS_BRIDGE_MODE             2
+#define	WDS_REPEATER_MODE           3
+#define	WDS_LAZY_MODE               4
+
+
+#define MAX_MESH_NUM				0
+
+#define MAX_APCLI_NUM				0
+#ifdef APCLI_SUPPORT
+#undef	MAX_APCLI_NUM
+#define MAX_APCLI_NUM				1
+#endif // APCLI_SUPPORT //
+
+#define MAX_MBSSID_NUM				1
+#ifdef MBSS_SUPPORT
+#undef	MAX_MBSSID_NUM
+#define MAX_MBSSID_NUM				(8 - MAX_MESH_NUM - MAX_APCLI_NUM)
+#endif // MBSS_SUPPORT //
+
+/* sanity check for apidx */
+#define MBSS_MR_APIDX_SANITY_CHECK(apidx) \
+    { if (apidx > MAX_MBSSID_NUM) { \
+          printk("%s> Error! apidx = %d > MAX_MBSSID_NUM!\n", __FUNCTION__, apidx); \
+	  apidx = MAIN_MBSSID; } }
+
+#define VALID_WCID(_wcid)	((_wcid) > 0 && (_wcid) < MAX_LEN_OF_MAC_TABLE )
+
+#define MAIN_MBSSID                 0
+#define FIRST_MBSSID                1
+
+
+#define MAX_BEACON_SIZE				512
+// If the MAX_MBSSID_NUM is larger than 6,
+// it shall reserve some WCID space(wcid 222~253) for beacon frames.
+// -	these wcid 238~253 are reserved for beacon#6(ra6).
+// -	these wcid 222~237 are reserved for beacon#7(ra7).
+#if defined(MAX_MBSSID_NUM) && (MAX_MBSSID_NUM == 8)
+#define HW_RESERVED_WCID	222
+#elif defined(MAX_MBSSID_NUM) && (MAX_MBSSID_NUM == 7)
+#define HW_RESERVED_WCID	238
+#else
+#define HW_RESERVED_WCID	255
+#endif
+
+// Then dedicate wcid of DFS and Carrier-Sense.
+#define DFS_CTS_WCID 		(HW_RESERVED_WCID - 1)
+#define CS_CTS_WCID 		(HW_RESERVED_WCID - 2)
+#define LAST_SPECIFIC_WCID	(HW_RESERVED_WCID - 2)
+
+// If MAX_MBSSID_NUM is 8, the maximum available wcid for the associated STA is 211.
+// If MAX_MBSSID_NUM is 7, the maximum available wcid for the associated STA is 228.
+#define MAX_AVAILABLE_CLIENT_WCID	(LAST_SPECIFIC_WCID - MAX_MBSSID_NUM - 1)
+
+// TX need WCID to find Cipher Key
+// these wcid 212 ~ 219 are reserved for bc/mc packets if MAX_MBSSID_NUM is 8.
+#define GET_GroupKey_WCID(__wcid, __bssidx) \
+	{										\
+		__wcid = LAST_SPECIFIC_WCID - (MAX_MBSSID_NUM) + __bssidx;	\
+	}
+
+#define IsGroupKeyWCID(__wcid) (((__wcid) < LAST_SPECIFIC_WCID) && ((__wcid) >= (LAST_SPECIFIC_WCID - (MAX_MBSSID_NUM))))
+
+
+// definition to support multiple BSSID
+#define BSS0                            0
+#define BSS1                            1
+#define BSS2                            2
+#define BSS3                            3
+#define BSS4                            4
+#define BSS5                            5
+#define BSS6                            6
+#define BSS7                            7
+
+
+//============================================================
+// Length definitions
+#define PEER_KEY_NO                     2
+#define MAC_ADDR_LEN                    6
+#define TIMESTAMP_LEN                   8
+#define MAX_LEN_OF_SUPPORTED_RATES      MAX_LENGTH_OF_SUPPORT_RATES // 1, 2, 5.5, 11, 6, 9, 12, 18, 24, 36, 48, 54
+#define MAX_LEN_OF_KEY                  32      // 32 octets == 256 bits, Redefine for WPA
+#define MAX_NUM_OF_CHANNELS             MAX_NUM_OF_CHS      // 14 channels @2.4G +  12@UNII + 4 @MMAC + 11 @HiperLAN2 + 7 @Japan + 1 as NULL termination
+#define MAX_NUM_OF_11JCHANNELS             20      // 14 channels @2.4G +  12@UNII + 4 @MMAC + 11 @HiperLAN2 + 7 @Japan + 1 as NULL termination
+#define MAX_LEN_OF_SSID                 32
+#define CIPHER_TEXT_LEN                 128
+#define HASH_TABLE_SIZE                 256
+#define MAX_VIE_LEN                     1024   // New for WPA cipher suite variable IE sizes.
+#define MAX_SUPPORT_MCS             32
+
+//============================================================
+// ASIC WCID Table definition.
+//============================================================
+#define BSSID_WCID		1	// in infra mode, always put bssid with this WCID
+#define MCAST_WCID	0x0
+#define BSS0Mcast_WCID	0x0
+#define BSS1Mcast_WCID	0xf8
+#define BSS2Mcast_WCID	0xf9
+#define BSS3Mcast_WCID	0xfa
+#define BSS4Mcast_WCID	0xfb
+#define BSS5Mcast_WCID	0xfc
+#define BSS6Mcast_WCID	0xfd
+#define BSS7Mcast_WCID	0xfe
+#define RESERVED_WCID		0xff
+
+#define MAX_NUM_OF_ACL_LIST				MAX_NUMBER_OF_ACL
+
+#define MAX_LEN_OF_MAC_TABLE            MAX_NUMBER_OF_MAC // if MAX_MBSSID_NUM is 8, this value can't be larger than 211
+
+#if MAX_LEN_OF_MAC_TABLE>MAX_AVAILABLE_CLIENT_WCID
+#error MAX_LEN_OF_MAC_TABLE can not be larger than MAX_AVAILABLE_CLIENT_WCID!!!!
+#endif
+
+#define MAX_NUM_OF_WDS_LINK_PERBSSID	            3
+#define MAX_NUM_OF_WDS_LINK	            (MAX_NUM_OF_WDS_LINK_PERBSSID*MAX_MBSSID_NUM)
+#define MAX_NUM_OF_EVENT                MAX_NUMBER_OF_EVENT
+#define WDS_LINK_START_WCID				(MAX_LEN_OF_MAC_TABLE-1)
+
+#define NUM_OF_TID			8
+#define MAX_AID_BA                    4
+#define MAX_LEN_OF_BA_REC_TABLE          ((NUM_OF_TID * MAX_LEN_OF_MAC_TABLE)/2)//   (NUM_OF_TID*MAX_AID_BA + 32)	 //Block ACK recipient
+#define MAX_LEN_OF_BA_ORI_TABLE          ((NUM_OF_TID * MAX_LEN_OF_MAC_TABLE)/2)//   (NUM_OF_TID*MAX_AID_BA + 32)   // Block ACK originator
+#define MAX_LEN_OF_BSS_TABLE             64
+#define MAX_REORDERING_MPDU_NUM			 512
+
+// key related definitions
+#define SHARE_KEY_NUM                   4
+#define MAX_LEN_OF_SHARE_KEY            16    // byte count
+#define MAX_LEN_OF_PEER_KEY             16    // byte count
+#define PAIRWISE_KEY_NUM                64    // in MAC ASIC pairwise key table
+#define GROUP_KEY_NUM                   4
+#define PMK_LEN                         32
+#define WDS_PAIRWISE_KEY_OFFSET         60    // WDS links uses pairwise key#60 ~ 63 in ASIC pairwise key table
+#define	PMKID_NO                        4     // Number of PMKID saved supported
+#define MAX_LEN_OF_MLME_BUFFER          2048
+
+// power status related definitions
+#define PWR_ACTIVE                      0
+#define PWR_SAVE                        1
+#define PWR_MMPS                        2			//MIMO power save
+
+// Auth and Assoc mode related definitions
+#define AUTH_MODE_OPEN                  0x00
+#define AUTH_MODE_KEY                   0x01
+
+// BSS Type definitions
+#define BSS_ADHOC                       0  // = Ndis802_11IBSS
+#define BSS_INFRA                       1  // = Ndis802_11Infrastructure
+#define BSS_ANY                         2  // = Ndis802_11AutoUnknown
+#define BSS_MONITOR			            3  // = Ndis802_11Monitor
+
+
+// Reason code definitions
+#define REASON_RESERVED                 0
+#define REASON_UNSPECIFY                1
+#define REASON_NO_LONGER_VALID          2
+#define REASON_DEAUTH_STA_LEAVING       3
+#define REASON_DISASSOC_INACTIVE        4
+#define REASON_DISASSPC_AP_UNABLE       5
+#define REASON_CLS2ERR                  6
+#define REASON_CLS3ERR                  7
+#define REASON_DISASSOC_STA_LEAVING     8
+#define REASON_STA_REQ_ASSOC_NOT_AUTH   9
+#define REASON_INVALID_IE               13
+#define REASON_MIC_FAILURE              14
+#define REASON_4_WAY_TIMEOUT            15
+#define REASON_GROUP_KEY_HS_TIMEOUT     16
+#define REASON_IE_DIFFERENT             17
+#define REASON_MCIPHER_NOT_VALID        18
+#define REASON_UCIPHER_NOT_VALID        19
+#define REASON_AKMP_NOT_VALID           20
+#define REASON_UNSUPPORT_RSNE_VER       21
+#define REASON_INVALID_RSNE_CAP         22
+#define REASON_8021X_AUTH_FAIL          23
+#define REASON_CIPHER_SUITE_REJECTED    24
+#define REASON_DECLINED                 37
+
+#define REASON_QOS_UNSPECIFY              32
+#define REASON_QOS_LACK_BANDWIDTH         33
+#define REASON_POOR_CHANNEL_CONDITION     34
+#define REASON_QOS_OUTSIDE_TXOP_LIMITION  35
+#define REASON_QOS_QSTA_LEAVING_QBSS      36
+#define REASON_QOS_UNWANTED_MECHANISM     37
+#define REASON_QOS_MECH_SETUP_REQUIRED    38
+#define REASON_QOS_REQUEST_TIMEOUT        39
+#define REASON_QOS_CIPHER_NOT_SUPPORT     45
+
+// Status code definitions
+#define MLME_SUCCESS                    0
+#define MLME_UNSPECIFY_FAIL             1
+#define MLME_CANNOT_SUPPORT_CAP         10
+#define MLME_REASSOC_DENY_ASSOC_EXIST   11
+#define MLME_ASSOC_DENY_OUT_SCOPE       12
+#define MLME_ALG_NOT_SUPPORT            13
+#define MLME_SEQ_NR_OUT_OF_SEQUENCE     14
+#define MLME_REJ_CHALLENGE_FAILURE      15
+#define MLME_REJ_TIMEOUT                  16
+#define MLME_ASSOC_REJ_UNABLE_HANDLE_STA  17
+#define MLME_ASSOC_REJ_DATA_RATE          18
+
+#define MLME_ASSOC_REJ_NO_EXT_RATE        22
+#define MLME_ASSOC_REJ_NO_EXT_RATE_PBCC   23
+#define MLME_ASSOC_REJ_NO_CCK_OFDM        24
+
+#define MLME_QOS_UNSPECIFY                32
+#define MLME_REQUEST_DECLINED             37
+#define MLME_REQUEST_WITH_INVALID_PARAM   38
+#define MLME_DLS_NOT_ALLOW_IN_QBSS        48
+#define MLME_DEST_STA_NOT_IN_QBSS         49
+#define MLME_DEST_STA_IS_NOT_A_QSTA       50
+
+#define MLME_INVALID_FORMAT             0x51
+#define MLME_FAIL_NO_RESOURCE           0x52
+#define MLME_STATE_MACHINE_REJECT       0x53
+#define MLME_MAC_TABLE_FAIL             0x54
+
+// IE code
+#define IE_SSID                         0
+#define IE_SUPP_RATES                   1
+#define IE_FH_PARM                      2
+#define IE_DS_PARM                      3
+#define IE_CF_PARM                      4
+#define IE_TIM                          5
+#define IE_IBSS_PARM                    6
+#define IE_COUNTRY                      7     // 802.11d
+#define IE_802_11D_REQUEST              10    // 802.11d
+#define IE_QBSS_LOAD                    11    // 802.11e d9
+#define IE_EDCA_PARAMETER               12    // 802.11e d9
+#define IE_TSPEC                        13    // 802.11e d9
+#define IE_TCLAS                        14    // 802.11e d9
+#define IE_SCHEDULE                     15    // 802.11e d9
+#define IE_CHALLENGE_TEXT               16
+#define IE_POWER_CONSTRAINT             32    // 802.11h d3.3
+#define IE_POWER_CAPABILITY             33    // 802.11h d3.3
+#define IE_TPC_REQUEST                  34    // 802.11h d3.3
+#define IE_TPC_REPORT                   35    // 802.11h d3.3
+#define IE_SUPP_CHANNELS                36    // 802.11h d3.3
+#define IE_CHANNEL_SWITCH_ANNOUNCEMENT  37    // 802.11h d3.3
+#define IE_MEASUREMENT_REQUEST          38    // 802.11h d3.3
+#define IE_MEASUREMENT_REPORT           39    // 802.11h d3.3
+#define IE_QUIET                        40    // 802.11h d3.3
+#define IE_IBSS_DFS                     41    // 802.11h d3.3
+#define IE_ERP                          42    // 802.11g
+#define IE_TS_DELAY                     43    // 802.11e d9
+#define IE_TCLAS_PROCESSING             44    // 802.11e d9
+#define IE_QOS_CAPABILITY               46    // 802.11e d6
+#define IE_HT_CAP                       45    // 802.11n d1. HT CAPABILITY. ELEMENT ID TBD
+#define IE_AP_CHANNEL_REPORT			51    // 802.11k d6
+#define IE_HT_CAP2                         52    // 802.11n d1. HT CAPABILITY. ELEMENT ID TBD
+#define IE_RSN                          48    // 802.11i d3.0
+#define IE_WPA2                         48    // WPA2
+#define IE_EXT_SUPP_RATES               50    // 802.11g
+#define IE_SUPP_REG_CLASS               59    // 802.11y. Supported regulatory classes.
+#define IE_EXT_CHANNEL_SWITCH_ANNOUNCEMENT	60	// 802.11n
+#define IE_ADD_HT                         61    // 802.11n d1. ADDITIONAL HT CAPABILITY. ELEMENT ID TBD
+#define IE_ADD_HT2                        53    // 802.11n d1. ADDITIONAL HT CAPABILITY. ELEMENT ID TBD
+
+
+// For 802.11n D3.03
+//#define IE_NEW_EXT_CHA_OFFSET             62    // 802.11n d1. New extension channel offset elemet
+#define IE_SECONDARY_CH_OFFSET		62	// 802.11n D3.03	Secondary Channel Offset element
+#define IE_2040_BSS_COEXIST               72    // 802.11n D3.0.3
+#define IE_2040_BSS_INTOLERANT_REPORT     73    // 802.11n D3.03
+#define IE_OVERLAPBSS_SCAN_PARM           74    // 802.11n D3.03
+#define IE_EXT_CAPABILITY                127   // 802.11n D3.03
+
+
+#define IE_WPA                          221   // WPA
+#define IE_VENDOR_SPECIFIC              221   // Wifi WMM (WME)
+
+#define OUI_BROADCOM_HT              51   //
+#define OUI_BROADCOM_HTADD              52   //
+#define OUI_PREN_HT_CAP              51   //
+#define OUI_PREN_ADD_HT              52   //
+
+// CCX information
+#define IE_AIRONET_CKIP                 133   // CCX1.0 ID 85H for CKIP
+#define IE_AP_TX_POWER                  150   // CCX 2.0 for AP transmit power
+#define IE_MEASUREMENT_CAPABILITY       221   // CCX 2.0
+#define IE_CCX_V2                       221
+#define IE_AIRONET_IPADDRESS            149   // CCX ID 95H for IP Address
+#define IE_AIRONET_CCKMREASSOC          156   // CCX ID 9CH for CCKM Reassociation Request element
+#define CKIP_NEGOTIATION_LENGTH         30
+#define AIRONET_IPADDRESS_LENGTH        10
+#define AIRONET_CCKMREASSOC_LENGTH      24
+
+// ========================================================
+// MLME state machine definition
+// ========================================================
+
+// STA MLME state mahcines
+#define ASSOC_STATE_MACHINE             1
+#define AUTH_STATE_MACHINE              2
+#define AUTH_RSP_STATE_MACHINE          3
+#define SYNC_STATE_MACHINE              4
+#define MLME_CNTL_STATE_MACHINE         5
+#define WPA_PSK_STATE_MACHINE           6
+#define LEAP_STATE_MACHINE              7
+#define AIRONET_STATE_MACHINE           8
+#define ACTION_STATE_MACHINE           9
+
+// AP MLME state machines
+#define AP_ASSOC_STATE_MACHINE          11
+#define AP_AUTH_STATE_MACHINE           12
+#define AP_AUTH_RSP_STATE_MACHINE       13
+#define AP_SYNC_STATE_MACHINE           14
+#define AP_CNTL_STATE_MACHINE           15
+#define AP_WPA_STATE_MACHINE            16
+
+#ifdef QOS_DLS_SUPPORT
+#define DLS_STATE_MACHINE               26
+#endif // QOS_DLS_SUPPORT //
+
+//
+// STA's CONTROL/CONNECT state machine: states, events, total function #
+//
+#define CNTL_IDLE                       0
+#define CNTL_WAIT_DISASSOC              1
+#define CNTL_WAIT_JOIN                  2
+#define CNTL_WAIT_REASSOC               3
+#define CNTL_WAIT_START                 4
+#define CNTL_WAIT_AUTH                  5
+#define CNTL_WAIT_ASSOC                 6
+#define CNTL_WAIT_AUTH2                 7
+#define CNTL_WAIT_OID_LIST_SCAN         8
+#define CNTL_WAIT_OID_DISASSOC          9
+
+#define MT2_ASSOC_CONF                  34
+#define MT2_AUTH_CONF                   35
+#define MT2_DEAUTH_CONF                 36
+#define MT2_DISASSOC_CONF               37
+#define MT2_REASSOC_CONF                38
+#define MT2_PWR_MGMT_CONF               39
+#define MT2_JOIN_CONF                   40
+#define MT2_SCAN_CONF                   41
+#define MT2_START_CONF                  42
+#define MT2_GET_CONF                    43
+#define MT2_SET_CONF                    44
+#define MT2_RESET_CONF                  45
+#define MT2_MLME_ROAMING_REQ            52
+
+#define CNTL_FUNC_SIZE                  1
+
+//
+// STA's ASSOC state machine: states, events, total function #
+//
+#define ASSOC_IDLE                      0
+#define ASSOC_WAIT_RSP                  1
+#define REASSOC_WAIT_RSP                2
+#define DISASSOC_WAIT_RSP               3
+#define MAX_ASSOC_STATE                 4
+
+#define ASSOC_MACHINE_BASE              0
+#define MT2_MLME_ASSOC_REQ              0
+#define MT2_MLME_REASSOC_REQ            1
+#define MT2_MLME_DISASSOC_REQ           2
+#define MT2_PEER_DISASSOC_REQ           3
+#define MT2_PEER_ASSOC_REQ              4
+#define MT2_PEER_ASSOC_RSP              5
+#define MT2_PEER_REASSOC_REQ            6
+#define MT2_PEER_REASSOC_RSP            7
+#define MT2_DISASSOC_TIMEOUT            8
+#define MT2_ASSOC_TIMEOUT               9
+#define MT2_REASSOC_TIMEOUT             10
+#define MAX_ASSOC_MSG                   11
+
+#define ASSOC_FUNC_SIZE                 (MAX_ASSOC_STATE * MAX_ASSOC_MSG)
+
+//
+// ACT state machine: states, events, total function #
+//
+#define ACT_IDLE                      0
+#define MAX_ACT_STATE                 1
+
+#define ACT_MACHINE_BASE              0
+
+//Those PEER_xx_CATE number is based on real Categary value in IEEE spec. Please don'es modify it by your self.
+//Category
+#define MT2_PEER_SPECTRUM_CATE              0
+#define MT2_PEER_QOS_CATE              1
+#define MT2_PEER_DLS_CATE             2
+#define MT2_PEER_BA_CATE             3
+#define MT2_PEER_PUBLIC_CATE             4
+#define MT2_PEER_RM_CATE             5
+#define MT2_PEER_HT_CATE             7	//	7.4.7
+#define MAX_PEER_CATE_MSG                   7
+#define MT2_MLME_ADD_BA_CATE             8
+#define MT2_MLME_ORI_DELBA_CATE             9
+#define MT2_MLME_REC_DELBA_CATE             10
+#define MT2_MLME_QOS_CATE              11
+#define MT2_MLME_DLS_CATE             12
+#define MT2_ACT_INVALID             13
+#define MAX_ACT_MSG                   14
+
+//Category field
+#define CATEGORY_SPECTRUM		0
+#define CATEGORY_QOS			1
+#define CATEGORY_DLS			2
+#define CATEGORY_BA			3
+#define CATEGORY_PUBLIC		4
+#define CATEGORY_RM			5
+#define CATEGORY_HT			7
+
+
+// DLS Action frame definition
+#define ACTION_DLS_REQUEST			0
+#define ACTION_DLS_RESPONSE			1
+#define ACTION_DLS_TEARDOWN			2
+
+//Spectrum  Action field value 802.11h 7.4.1
+#define SPEC_MRQ	0	// Request
+#define SPEC_MRP	1	//Report
+#define SPEC_TPCRQ	2
+#define SPEC_TPCRP	3
+#define SPEC_CHANNEL_SWITCH	4
+
+
+//BA  Action field value
+#define ADDBA_REQ	0
+#define ADDBA_RESP	1
+#define DELBA   2
+
+//Public's  Action field value in Public Category.  Some in 802.11y and some in 11n
+#define ACTION_BSS_2040_COEXIST				0	// 11n
+#define ACTION_DSE_ENABLEMENT					1	// 11y D9.0
+#define ACTION_DSE_DEENABLEMENT				2	// 11y D9.0
+#define ACTION_DSE_REG_LOCATION_ANNOUNCE	3	// 11y D9.0
+#define ACTION_EXT_CH_SWITCH_ANNOUNCE		4	// 11y D9.0
+#define ACTION_DSE_MEASUREMENT_REQ			5	// 11y D9.0
+#define ACTION_DSE_MEASUREMENT_REPORT		6	// 11y D9.0
+#define ACTION_MEASUREMENT_PILOT_ACTION		7  	// 11y D9.0
+#define ACTION_DSE_POWER_CONSTRAINT			8	// 11y D9.0
+
+
+//HT  Action field value
+#define NOTIFY_BW_ACTION				0
+#define SMPS_ACTION						1
+#define PSMP_ACTION   					2
+#define SETPCO_ACTION					3
+#define MIMO_CHA_MEASURE_ACTION			4
+#define MIMO_N_BEACONFORM				5
+#define MIMO_BEACONFORM					6
+#define ANTENNA_SELECT					7
+#define HT_INFO_EXCHANGE				8
+
+#define ACT_FUNC_SIZE                 (MAX_ACT_STATE * MAX_ACT_MSG)
+//
+// STA's AUTHENTICATION state machine: states, evvents, total function #
+//
+#define AUTH_REQ_IDLE                   0
+#define AUTH_WAIT_SEQ2                  1
+#define AUTH_WAIT_SEQ4                  2
+#define MAX_AUTH_STATE                  3
+
+#define AUTH_MACHINE_BASE               0
+#define MT2_MLME_AUTH_REQ               0
+#define MT2_PEER_AUTH_EVEN              1
+#define MT2_AUTH_TIMEOUT                2
+#define MAX_AUTH_MSG                    3
+
+#define AUTH_FUNC_SIZE                  (MAX_AUTH_STATE * MAX_AUTH_MSG)
+
+//
+// STA's AUTH_RSP state machine: states, events, total function #
+//
+#define AUTH_RSP_IDLE                   0
+#define AUTH_RSP_WAIT_CHAL              1
+#define MAX_AUTH_RSP_STATE              2
+
+#define AUTH_RSP_MACHINE_BASE           0
+#define MT2_AUTH_CHALLENGE_TIMEOUT      0
+#define MT2_PEER_AUTH_ODD               1
+#define MT2_PEER_DEAUTH                 2
+#define MAX_AUTH_RSP_MSG                3
+
+#define AUTH_RSP_FUNC_SIZE              (MAX_AUTH_RSP_STATE * MAX_AUTH_RSP_MSG)
+
+//
+// STA's SYNC state machine: states, events, total function #
+//
+#define SYNC_IDLE                       0  // merge NO_BSS,IBSS_IDLE,IBSS_ACTIVE and BSS in to 1 state
+#define JOIN_WAIT_BEACON                1
+#define SCAN_LISTEN                     2
+#define MAX_SYNC_STATE                  3
+
+#define SYNC_MACHINE_BASE               0
+#define MT2_MLME_SCAN_REQ               0
+#define MT2_MLME_JOIN_REQ               1
+#define MT2_MLME_START_REQ              2
+#define MT2_PEER_BEACON                 3
+#define MT2_PEER_PROBE_RSP              4
+#define MT2_PEER_ATIM                   5
+#define MT2_SCAN_TIMEOUT                6
+#define MT2_BEACON_TIMEOUT              7
+#define MT2_ATIM_TIMEOUT                8
+#define MT2_PEER_PROBE_REQ              9
+#define MAX_SYNC_MSG                    10
+
+#define SYNC_FUNC_SIZE                  (MAX_SYNC_STATE * MAX_SYNC_MSG)
+
+//Messages for the DLS state machine
+#define DLS_IDLE						0
+#define MAX_DLS_STATE					1
+
+#define DLS_MACHINE_BASE				0
+#define MT2_MLME_DLS_REQ			    0
+#define MT2_PEER_DLS_REQ			    1
+#define MT2_PEER_DLS_RSP			    2
+#define MT2_MLME_DLS_TEAR_DOWN		    3
+#define MT2_PEER_DLS_TEAR_DOWN		    4
+#define MAX_DLS_MSG				        5
+
+#define DLS_FUNC_SIZE					(MAX_DLS_STATE * MAX_DLS_MSG)
+
+//
+// STA's WPA-PSK State machine: states, events, total function #
+//
+#define WPA_PSK_IDLE					0
+#define MAX_WPA_PSK_STATE				1
+
+#define WPA_MACHINE_BASE                0
+#define MT2_EAPPacket                   0
+#define MT2_EAPOLStart                  1
+#define MT2_EAPOLLogoff                 2
+#define MT2_EAPOLKey                    3
+#define MT2_EAPOLASFAlert               4
+#define MAX_WPA_PSK_MSG                 5
+
+#define	WPA_PSK_FUNC_SIZE				(MAX_WPA_PSK_STATE * MAX_WPA_PSK_MSG)
+
+//
+// STA's CISCO-AIRONET State machine: states, events, total function #
+//
+#define AIRONET_IDLE					0
+#define	AIRONET_SCANNING				1
+#define MAX_AIRONET_STATE				2
+
+#define AIRONET_MACHINE_BASE		    0
+#define MT2_AIRONET_MSG				    0
+#define MT2_AIRONET_SCAN_REQ		    1
+#define MT2_AIRONET_SCAN_DONE		    2
+#define MAX_AIRONET_MSG				    3
+
+#define	AIRONET_FUNC_SIZE				(MAX_AIRONET_STATE * MAX_AIRONET_MSG)
+
+//
+// AP's CONTROL/CONNECT state machine: states, events, total function #
+//
+#define AP_CNTL_FUNC_SIZE               1
+
+//
+// AP's ASSOC state machine: states, events, total function #
+//
+#define AP_ASSOC_IDLE                   0
+#define AP_MAX_ASSOC_STATE              1
+
+#define AP_ASSOC_MACHINE_BASE           0
+#define APMT2_MLME_DISASSOC_REQ         0
+#define APMT2_PEER_DISASSOC_REQ         1
+#define APMT2_PEER_ASSOC_REQ            2
+#define APMT2_PEER_REASSOC_REQ          3
+#define APMT2_CLS3ERR                   4
+#define AP_MAX_ASSOC_MSG                5
+
+#define AP_ASSOC_FUNC_SIZE              (AP_MAX_ASSOC_STATE * AP_MAX_ASSOC_MSG)
+
+//
+// AP's AUTHENTICATION state machine: states, events, total function #
+//
+#define AP_AUTH_REQ_IDLE                0
+#define AP_MAX_AUTH_STATE               1
+
+#define AP_AUTH_MACHINE_BASE            0
+#define APMT2_MLME_DEAUTH_REQ           0
+#define APMT2_CLS2ERR                   1
+#define AP_MAX_AUTH_MSG                 2
+
+#define AP_AUTH_FUNC_SIZE               (AP_MAX_AUTH_STATE * AP_MAX_AUTH_MSG)
+
+//
+// AP's AUTH-RSP state machine: states, events, total function #
+//
+#define AP_AUTH_RSP_IDLE                0
+#define AP_MAX_AUTH_RSP_STATE           1
+
+#define AP_AUTH_RSP_MACHINE_BASE        0
+#define APMT2_AUTH_CHALLENGE_TIMEOUT    0
+#define APMT2_PEER_AUTH_ODD             1
+#define APMT2_PEER_DEAUTH               2
+#define AP_MAX_AUTH_RSP_MSG             3
+
+#define AP_AUTH_RSP_FUNC_SIZE           (AP_MAX_AUTH_RSP_STATE * AP_MAX_AUTH_RSP_MSG)
+
+//
+// AP's SYNC state machine: states, events, total function #
+//
+#define AP_SYNC_IDLE                    0
+#define AP_SCAN_LISTEN					1
+#define AP_MAX_SYNC_STATE               2
+
+#define AP_SYNC_MACHINE_BASE            0
+#define APMT2_PEER_PROBE_REQ            0
+#define APMT2_PEER_BEACON               1
+#define APMT2_MLME_SCAN_REQ				2
+#define APMT2_PEER_PROBE_RSP			3
+#define APMT2_SCAN_TIMEOUT				4
+#define APMT2_MLME_SCAN_CNCL			5
+#define AP_MAX_SYNC_MSG                 6
+
+#define AP_SYNC_FUNC_SIZE               (AP_MAX_SYNC_STATE * AP_MAX_SYNC_MSG)
+
+//
+// AP's WPA state machine: states, events, total function #
+//
+#define AP_WPA_PTK                      0
+#define AP_MAX_WPA_PTK_STATE            1
+
+#define AP_WPA_MACHINE_BASE             0
+#define APMT2_EAPPacket                 0
+#define APMT2_EAPOLStart                1
+#define APMT2_EAPOLLogoff               2
+#define APMT2_EAPOLKey                  3
+#define APMT2_EAPOLASFAlert             4
+#define AP_MAX_WPA_MSG                  5
+
+#define AP_WPA_FUNC_SIZE                (AP_MAX_WPA_PTK_STATE * AP_MAX_WPA_MSG)
+
+#ifdef APCLI_SUPPORT
+//ApCli authentication state machine
+#define APCLI_AUTH_REQ_IDLE                0
+#define APCLI_AUTH_WAIT_SEQ2               1
+#define APCLI_AUTH_WAIT_SEQ4               2
+#define APCLI_MAX_AUTH_STATE               3
+
+#define APCLI_AUTH_MACHINE_BASE            0
+#define APCLI_MT2_MLME_AUTH_REQ            0
+#define APCLI_MT2_MLME_DEAUTH_REQ          1
+#define APCLI_MT2_PEER_AUTH_EVEN           2
+#define APCLI_MT2_PEER_DEAUTH              3
+#define APCLI_MT2_AUTH_TIMEOUT             4
+#define APCLI_MAX_AUTH_MSG                 5
+
+#define APCLI_AUTH_FUNC_SIZE               (APCLI_MAX_AUTH_STATE * APCLI_MAX_AUTH_MSG)
+
+//ApCli association state machine
+#define APCLI_ASSOC_IDLE                   0
+#define APCLI_ASSOC_WAIT_RSP               1
+#define APCLI_MAX_ASSOC_STATE              2
+
+#define APCLI_ASSOC_MACHINE_BASE           0
+#define APCLI_MT2_MLME_ASSOC_REQ           0
+#define APCLI_MT2_MLME_DISASSOC_REQ        1
+#define APCLI_MT2_PEER_DISASSOC_REQ        2
+#define APCLI_MT2_PEER_ASSOC_RSP           3
+#define APCLI_MT2_ASSOC_TIMEOUT            4
+#define APCLI_MAX_ASSOC_MSG                5
+
+#define APCLI_ASSOC_FUNC_SIZE              (APCLI_MAX_ASSOC_STATE * APCLI_MAX_ASSOC_MSG)
+
+//ApCli sync state machine
+#define APCLI_SYNC_IDLE                   0  // merge NO_BSS,IBSS_IDLE,IBSS_ACTIVE and BSS in to 1 state
+#define APCLI_JOIN_WAIT_PROBE_RSP         1
+#define APCLI_MAX_SYNC_STATE              2
+
+#define APCLI_SYNC_MACHINE_BASE           0
+#define APCLI_MT2_MLME_PROBE_REQ          0
+#define APCLI_MT2_PEER_PROBE_RSP          1
+#define APCLI_MT2_PROBE_TIMEOUT           2
+#define APCLI_MAX_SYNC_MSG                3
+
+#define APCLI_SYNC_FUNC_SIZE              (APCLI_MAX_SYNC_STATE * APCLI_MAX_SYNC_MSG)
+
+//ApCli ctrl state machine
+#define APCLI_CTRL_DISCONNECTED           0  // merge NO_BSS,IBSS_IDLE,IBSS_ACTIVE and BSS in to 1 state
+#define APCLI_CTRL_PROBE                  1
+#define APCLI_CTRL_AUTH                   2
+#define APCLI_CTRL_AUTH_2                 3
+#define APCLI_CTRL_ASSOC                  4
+#define APCLI_CTRL_DEASSOC                5
+#define APCLI_CTRL_CONNECTED              6
+#define APCLI_MAX_CTRL_STATE              7
+
+#define APCLI_CTRL_MACHINE_BASE           0
+#define APCLI_CTRL_JOIN_REQ               0
+#define APCLI_CTRL_PROBE_RSP              1
+#define APCLI_CTRL_AUTH_RSP               2
+#define APCLI_CTRL_DISCONNECT_REQ         3
+#define APCLI_CTRL_PEER_DISCONNECT_REQ    4
+#define APCLI_CTRL_ASSOC_RSP              5
+#define APCLI_CTRL_DEASSOC_RSP            6
+#define APCLI_CTRL_JOIN_REQ_TIMEOUT       7
+#define APCLI_CTRL_AUTH_REQ_TIMEOUT       8
+#define APCLI_CTRL_ASSOC_REQ_TIMEOUT      9
+#define APCLI_MAX_CTRL_MSG                10
+
+#define APCLI_CTRL_FUNC_SIZE              (APCLI_MAX_CTRL_STATE * APCLI_MAX_CTRL_MSG)
+
+#endif	// APCLI_SUPPORT //
+
+
+// =============================================================================
+
+// value domain of 802.11 header FC.Tyte, which is b3..b2 of the 1st-byte of MAC header
+#define BTYPE_MGMT                  0
+#define BTYPE_CNTL                  1
+#define BTYPE_DATA                  2
+
+// value domain of 802.11 MGMT frame's FC.subtype, which is b7..4 of the 1st-byte of MAC header
+#define SUBTYPE_ASSOC_REQ           0
+#define SUBTYPE_ASSOC_RSP           1
+#define SUBTYPE_REASSOC_REQ         2
+#define SUBTYPE_REASSOC_RSP         3
+#define SUBTYPE_PROBE_REQ           4
+#define SUBTYPE_PROBE_RSP           5
+#define SUBTYPE_BEACON              8
+#define SUBTYPE_ATIM                9
+#define SUBTYPE_DISASSOC            10
+#define SUBTYPE_AUTH                11
+#define SUBTYPE_DEAUTH              12
+#define SUBTYPE_ACTION              13
+#define SUBTYPE_ACTION_NO_ACK              14
+
+// value domain of 802.11 CNTL frame's FC.subtype, which is b7..4 of the 1st-byte of MAC header
+#define SUBTYPE_WRAPPER       	7
+#define SUBTYPE_BLOCK_ACK_REQ       8
+#define SUBTYPE_BLOCK_ACK           9
+#define SUBTYPE_PS_POLL             10
+#define SUBTYPE_RTS                 11
+#define SUBTYPE_CTS                 12
+#define SUBTYPE_ACK                 13
+#define SUBTYPE_CFEND               14
+#define SUBTYPE_CFEND_CFACK         15
+
+// value domain of 802.11 DATA frame's FC.subtype, which is b7..4 of the 1st-byte of MAC header
+#define SUBTYPE_DATA                0
+#define SUBTYPE_DATA_CFACK          1
+#define SUBTYPE_DATA_CFPOLL         2
+#define SUBTYPE_DATA_CFACK_CFPOLL   3
+#define SUBTYPE_NULL_FUNC           4
+#define SUBTYPE_CFACK               5
+#define SUBTYPE_CFPOLL              6
+#define SUBTYPE_CFACK_CFPOLL        7
+#define SUBTYPE_QDATA               8
+#define SUBTYPE_QDATA_CFACK         9
+#define SUBTYPE_QDATA_CFPOLL        10
+#define SUBTYPE_QDATA_CFACK_CFPOLL  11
+#define SUBTYPE_QOS_NULL            12
+#define SUBTYPE_QOS_CFACK           13
+#define SUBTYPE_QOS_CFPOLL          14
+#define SUBTYPE_QOS_CFACK_CFPOLL    15
+
+// ACK policy of QOS Control field bit 6:5
+#define NORMAL_ACK                  0x00  // b6:5 = 00
+#define NO_ACK                      0x20  // b6:5 = 01
+#define NO_EXPLICIT_ACK             0x40  // b6:5 = 10
+#define BLOCK_ACK                   0x60  // b6:5 = 11
+
+//
+// rtmp_data.c use these definition
+//
+#define LENGTH_802_11               24
+#define LENGTH_802_11_AND_H         30
+#define LENGTH_802_11_CRC_H         34
+#define LENGTH_802_11_CRC           28
+#define LENGTH_802_11_WITH_ADDR4    30
+#define LENGTH_802_3                14
+#define LENGTH_802_3_TYPE           2
+#define LENGTH_802_1_H              8
+#define LENGTH_EAPOL_H              4
+#define LENGTH_WMMQOS_H				2
+#define LENGTH_CRC                  4
+#define MAX_SEQ_NUMBER              0x0fff
+#define LENGTH_802_3_NO_TYPE		12
+#define LENGTH_802_1Q				4 /* VLAN related */
+
+// STA_CSR4.field.TxResult
+#define TX_RESULT_SUCCESS           0
+#define TX_RESULT_ZERO_LENGTH       1
+#define TX_RESULT_UNDER_RUN         2
+#define TX_RESULT_OHY_ERROR         4
+#define TX_RESULT_RETRY_FAIL        6
+
+// All PHY rate summary in TXD
+// Preamble MODE in TxD
+#define MODE_CCK	0
+#define MODE_OFDM   1
+#ifdef DOT11_N_SUPPORT
+#define MODE_HTMIX	2
+#define MODE_HTGREENFIELD	3
+#endif // DOT11_N_SUPPORT //
+// MCS for CCK.  BW.SGI.STBC are reserved
+#define MCS_LONGP_RATE_1                      0	 // long preamble CCK 1Mbps
+#define MCS_LONGP_RATE_2                      1	// long preamble CCK 1Mbps
+#define MCS_LONGP_RATE_5_5                    2
+#define MCS_LONGP_RATE_11                     3
+#define MCS_SHORTP_RATE_1                      4	 // long preamble CCK 1Mbps. short is forbidden in 1Mbps
+#define MCS_SHORTP_RATE_2                      5	// short preamble CCK 2Mbps
+#define MCS_SHORTP_RATE_5_5                    6
+#define MCS_SHORTP_RATE_11                     7
+// To send duplicate legacy OFDM. set BW=BW_40.  SGI.STBC are reserved
+#define MCS_RATE_6                      0   // legacy OFDM
+#define MCS_RATE_9                      1   // OFDM
+#define MCS_RATE_12                     2   // OFDM
+#define MCS_RATE_18                     3   // OFDM
+#define MCS_RATE_24                     4  // OFDM
+#define MCS_RATE_36                     5   // OFDM
+#define MCS_RATE_48                     6  // OFDM
+#define MCS_RATE_54                     7 // OFDM
+// HT
+#define MCS_0		0	// 1S
+#define MCS_1		1
+#define MCS_2		2
+#define MCS_3		3
+#define MCS_4		4
+#define MCS_5		5
+#define MCS_6		6
+#define MCS_7		7
+#define MCS_8		8	// 2S
+#define MCS_9		9
+#define MCS_10		10
+#define MCS_11		11
+#define MCS_12		12
+#define MCS_13		13
+#define MCS_14		14
+#define MCS_15		15
+#define MCS_16		16	// 3*3
+#define MCS_17		17
+#define MCS_18		18
+#define MCS_19		19
+#define MCS_20		20
+#define MCS_21		21
+#define MCS_22		22
+#define MCS_23		23
+#define MCS_32		32
+#define MCS_AUTO		33
+
+#ifdef DOT11_N_SUPPORT
+// OID_HTPHYMODE
+// MODE
+#define HTMODE_MM	0
+#define HTMODE_GF	1
+#endif // DOT11_N_SUPPORT //
+
+// Fixed Tx MODE - HT, CCK or OFDM
+#define FIXED_TXMODE_HT		0
+#define FIXED_TXMODE_CCK	1
+#define FIXED_TXMODE_OFDM 	2
+// BW
+#define BW_20		BAND_WIDTH_20
+#define BW_40		BAND_WIDTH_40
+#define BW_BOTH		BAND_WIDTH_BOTH
+#define BW_10		BAND_WIDTH_10	// 802.11j has 10MHz. This definition is for internal usage. doesn't fill in the IE or other field.
+
+#ifdef DOT11_N_SUPPORT
+// SHORTGI
+#define GI_400		GAP_INTERVAL_400	// only support in HT mode
+#define GI_BOTH		GAP_INTERVAL_BOTH
+#endif // DOT11_N_SUPPORT //
+#define GI_800		GAP_INTERVAL_800
+// STBC
+#define STBC_NONE	0
+#ifdef DOT11_N_SUPPORT
+#define STBC_USE	1	// limited use in rt2860b phy
+#define RXSTBC_ONE	1	// rx support of one spatial stream
+#define RXSTBC_TWO	2	// rx support of 1 and 2 spatial stream
+#define RXSTBC_THR	3	// rx support of 1~3 spatial stream
+// MCS FEEDBACK
+#define MCSFBK_NONE	0  // not support mcs feedback /
+#define MCSFBK_RSV	1	// reserved
+#define MCSFBK_UNSOLICIT	2	// only support unsolict mcs feedback
+#define MCSFBK_MRQ	3	// response to both MRQ and unsolict mcs feedback
+
+// MIMO power safe
+#define	MMPS_STATIC	0
+#define	MMPS_DYNAMIC		1
+#define   MMPS_RSV		2
+#define MMPS_ENABLE		3
+
+
+// A-MSDU size
+#define	AMSDU_0	0
+#define	AMSDU_1		1
+
+#endif // DOT11_N_SUPPORT //
+
+// MCS use 7 bits
+#define TXRATEMIMO		0x80
+#define TXRATEMCS		0x7F
+#define TXRATEOFDM		0x7F
+#define RATE_1                      0
+#define RATE_2                      1
+#define RATE_5_5                    2
+#define RATE_11                     3
+#define RATE_6                      4   // OFDM
+#define RATE_9                      5   // OFDM
+#define RATE_12                     6   // OFDM
+#define RATE_18                     7   // OFDM
+#define RATE_24                     8   // OFDM
+#define RATE_36                     9   // OFDM
+#define RATE_48                     10  // OFDM
+#define RATE_54                     11  // OFDM
+#define RATE_FIRST_OFDM_RATE        RATE_6
+#define RATE_LAST_OFDM_RATE        	RATE_54
+#define RATE_6_5                    12  // HT mix
+#define RATE_13                     13  // HT mix
+#define RATE_19_5                   14  // HT mix
+#define RATE_26                     15  // HT mix
+#define RATE_39                     16  // HT mix
+#define RATE_52                     17  // HT mix
+#define RATE_58_5                   18  // HT mix
+#define RATE_65                     19  // HT mix
+#define RATE_78                     20  // HT mix
+#define RATE_104                    21  // HT mix
+#define RATE_117                    22  // HT mix
+#define RATE_130                    23  // HT mix
+//#define RATE_AUTO_SWITCH            255 // for StaCfg.FixedTxRate only
+#define HTRATE_0                      12
+#define RATE_FIRST_MM_RATE        HTRATE_0
+#define RATE_FIRST_HT_RATE        HTRATE_0
+#define RATE_LAST_HT_RATE        HTRATE_0
+
+// pTxWI->txop
+#define IFS_HTTXOP                 0	// The txop will be handles by ASIC.
+#define IFS_PIFS                    1
+#define IFS_SIFS                    2
+#define IFS_BACKOFF                 3
+
+// pTxD->RetryMode
+#define LONG_RETRY                  1
+#define SHORT_RETRY                 0
+
+// Country Region definition
+#define REGION_MINIMUM_BG_BAND            0
+#define REGION_0_BG_BAND                  0       // 1-11
+#define REGION_1_BG_BAND                  1       // 1-13
+#define REGION_2_BG_BAND                  2       // 10-11
+#define REGION_3_BG_BAND                  3       // 10-13
+#define REGION_4_BG_BAND                  4       // 14
+#define REGION_5_BG_BAND                  5       // 1-14
+#define REGION_6_BG_BAND                  6       // 3-9
+#define REGION_7_BG_BAND                  7       // 5-13
+#define REGION_31_BG_BAND                 31       // 5-13
+#define REGION_MAXIMUM_BG_BAND            7
+
+#define REGION_MINIMUM_A_BAND             0
+#define REGION_0_A_BAND                   0       // 36, 40, 44, 48, 52, 56, 60, 64, 149, 153, 157, 161, 165
+#define REGION_1_A_BAND                   1       // 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140
+#define REGION_2_A_BAND                   2       // 36, 40, 44, 48, 52, 56, 60, 64
+#define REGION_3_A_BAND                   3       // 52, 56, 60, 64, 149, 153, 157, 161
+#define REGION_4_A_BAND                   4       // 149, 153, 157, 161, 165
+#define REGION_5_A_BAND                   5       // 149, 153, 157, 161
+#define REGION_6_A_BAND                   6       // 36, 40, 44, 48
+#define REGION_7_A_BAND                   7       // 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 149, 153, 157, 161, 165
+#define REGION_8_A_BAND                   8       // 52, 56, 60, 64
+#define REGION_9_A_BAND                   9       // 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 132, 136, 140, 149, 153, 157, 161, 165
+#define REGION_10_A_BAND                  10	  // 36, 40, 44, 48, 149, 153, 157, 161, 165
+#define REGION_11_A_BAND                  11	  // 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 149, 153, 157, 161
+#define REGION_MAXIMUM_A_BAND             11
+
+// pTxD->CipherAlg
+#define CIPHER_NONE                 0
+#define CIPHER_WEP64                1
+#define CIPHER_WEP128               2
+#define CIPHER_TKIP                 3
+#define CIPHER_AES                  4
+#define CIPHER_CKIP64               5
+#define CIPHER_CKIP128              6
+#define CIPHER_TKIP_NO_MIC          7       // MIC appended by driver: not a valid value in hardware key table
+#define CIPHER_SMS4					8
+
+// value domain of pAd->RfIcType
+#define RFIC_2820                   1       // 2.4G 2T3R
+#define RFIC_2850                   2       // 2.4G/5G 2T3R
+#define RFIC_2720                   3       // 2.4G 1T2R
+#define RFIC_2750                   4       // 2.4G/5G 1T2R
+#define RFIC_3020                   5       // 2.4G 1T1R
+#define RFIC_2020                   6       // 2.4G B/G
+
+// LED Status.
+#define LED_LINK_DOWN               0
+#define LED_LINK_UP                 1
+#define LED_RADIO_OFF               2
+#define LED_RADIO_ON                3
+#define LED_HALT                    4
+#define LED_WPS                     5
+#define LED_ON_SITE_SURVEY          6
+#define LED_POWER_UP                7
+
+// value domain of pAd->LedCntl.LedMode and E2PROM
+#define LED_MODE_DEFAULT            0
+#define LED_MODE_TWO_LED			1
+#define LED_MODE_SIGNAL_STREGTH		8  // EEPROM define =8
+
+// RC4 init value, used fro WEP & TKIP
+#define PPPINITFCS32                0xffffffff   /* Initial FCS value */
+
+// value domain of pAd->StaCfg.PortSecured. 802.1X controlled port definition
+#define WPA_802_1X_PORT_SECURED     1
+#define WPA_802_1X_PORT_NOT_SECURED 2
+
+#define PAIRWISE_KEY                1
+#define GROUP_KEY                   2
+
+//definition of DRS
+#define MAX_STEP_OF_TX_RATE_SWITCH	32
+
+
+// pre-allocated free NDIS PACKET/BUFFER poll for internal usage
+#define MAX_NUM_OF_FREE_NDIS_PACKET 128
+
+//Block ACK
+#define MAX_TX_REORDERBUF   64
+#define MAX_RX_REORDERBUF   64
+#define DEFAULT_TX_TIMEOUT   30
+#define DEFAULT_RX_TIMEOUT   30
+
+// definition of Recipient or Originator
+#define I_RECIPIENT                  TRUE
+#define I_ORIGINATOR                   FALSE
+
+#define DEFAULT_BBP_TX_POWER        0
+#define DEFAULT_RF_TX_POWER         5
+
+#define MAX_INI_BUFFER_SIZE			4096
+#define MAX_PARAM_BUFFER_SIZE		(2048) // enough for ACL (18*64)
+											//18 : the length of Mac address acceptable format "01:02:03:04:05:06;")
+											//64 : MAX_NUM_OF_ACL_LIST
+// definition of pAd->OpMode
+#define OPMODE_STA                  0
+#define OPMODE_AP                   1
+//#define OPMODE_L3_BRG               2       // as AP and STA at the same time
+
+#ifdef RT_BIG_ENDIAN
+#define DIR_READ                    0
+#define DIR_WRITE                   1
+#define TYPE_TXD                    0
+#define TYPE_RXD                    1
+#define TYPE_TXINFO					0
+#define TYPE_RXINFO					1
+#define TYPE_TXWI					0
+#define TYPE_RXWI					1
+#endif
+
+// ========================= AP rtmp_def.h ===========================
+// value domain for pAd->EventTab.Log[].Event
+#define EVENT_RESET_ACCESS_POINT    0 // Log = "hh:mm:ss   Restart Access Point"
+#define EVENT_ASSOCIATED            1 // Log = "hh:mm:ss   STA 00:01:02:03:04:05 associated"
+#define EVENT_DISASSOCIATED         2 // Log = "hh:mm:ss   STA 00:01:02:03:04:05 left this BSS"
+#define EVENT_AGED_OUT              3 // Log = "hh:mm:ss   STA 00:01:02:03:04:05 was aged-out and removed from this BSS"
+#define EVENT_COUNTER_M             4
+#define EVENT_INVALID_PSK           5
+#define EVENT_MAX_EVENT_TYPE        6
+// ==== end of AP rtmp_def.h ============
+
+// definition RSSI Number
+#define RSSI_0					0
+#define RSSI_1					1
+#define RSSI_2					2
+
+// definition of radar detection
+#define RD_NORMAL_MODE				0	// Not found radar signal
+#define RD_SWITCHING_MODE			1	// Found radar signal, and doing channel switch
+#define RD_SILENCE_MODE				2	// After channel switch, need to be silence a while to ensure radar not found
+
+//Driver defined cid for mapping status and command.
+#define  SLEEPCID	0x11
+#define  WAKECID	0x22
+#define  QUERYPOWERCID	0x33
+#define  OWNERMCU	0x1
+#define  OWNERCPU	0x0
+
+// MBSSID definition
+#define ENTRY_NOT_FOUND             0xFF
+
+
+/* After Linux 2.6.9,
+ * VLAN module use Private (from user) interface flags (netdevice->priv_flags).
+ * #define IFF_802_1Q_VLAN 0x1         --    802.1Q VLAN device.  in if.h
+ * ref to ip_sabotage_out() [ out->priv_flags & IFF_802_1Q_VLAN ] in br_netfilter.c
+ *
+ * For this reason, we MUST use EVEN value in priv_flags
+ */
+#define INT_MAIN                    0x0100
+#define INT_MBSSID                  0x0200
+#define INT_WDS                     0x0300
+#define INT_APCLI                   0x0400
+#define INT_MESH                   	0x0500
+
+// Use bitmap to allow coexist of ATE_TXFRAME and ATE_RXFRAME(i.e.,to support LoopBack mode)
+#ifdef RALINK_ATE
+#define	ATE_START                   0x00   // Start ATE
+#define	ATE_STOP                    0x80   // Stop ATE
+#define	ATE_TXCONT                  0x05   // Continuous Transmit
+#define	ATE_TXCARR                  0x09   // Transmit Carrier
+#define	ATE_TXCARRSUPP              0x11   // Transmit Carrier Suppression
+#define	ATE_TXFRAME                 0x01   // Transmit Frames
+#define	ATE_RXFRAME                 0x02   // Receive Frames
+#ifdef RALINK_28xx_QA
+#define ATE_TXSTOP                  0xe2   // Stop Transmition(i.e., TXCONT, TXCARR, TXCARRSUPP, and TXFRAME)
+#define ATE_RXSTOP					0xfd   // Stop receiving Frames
+#define	BBP22_TXFRAME     			0x00   // Transmit Frames
+#define	BBP22_TXCONT_OR_CARRSUPP    0x80   // Continuous Transmit or Carrier Suppression
+#define	BBP22_TXCARR                0xc1   // Transmit Carrier
+#define	BBP24_TXCONT                0x00   // Continuous Transmit
+#define	BBP24_CARRSUPP              0x01   // Carrier Suppression
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+
+// WEP Key TYPE
+#define WEP_HEXADECIMAL_TYPE    0
+#define WEP_ASCII_TYPE          1
+
+
+
+// WIRELESS EVENTS definition
+/* Max number of char in custom event, refer to wireless_tools.28/wireless.20.h */
+#define IW_CUSTOM_MAX_LEN				  			255	/* In bytes */
+
+// For system event - start
+#define	IW_SYS_EVENT_FLAG_START                     0x0200
+#define	IW_ASSOC_EVENT_FLAG                         0x0200
+#define	IW_DISASSOC_EVENT_FLAG                      0x0201
+#define	IW_DEAUTH_EVENT_FLAG                      	0x0202
+#define	IW_AGEOUT_EVENT_FLAG                      	0x0203
+#define	IW_COUNTER_MEASURES_EVENT_FLAG              0x0204
+#define	IW_REPLAY_COUNTER_DIFF_EVENT_FLAG           0x0205
+#define	IW_RSNIE_DIFF_EVENT_FLAG           			0x0206
+#define	IW_MIC_DIFF_EVENT_FLAG           			0x0207
+#define IW_ICV_ERROR_EVENT_FLAG						0x0208
+#define IW_MIC_ERROR_EVENT_FLAG						0x0209
+#define IW_GROUP_HS_TIMEOUT_EVENT_FLAG				0x020A
+#define	IW_PAIRWISE_HS_TIMEOUT_EVENT_FLAG			0x020B
+#define IW_RSNIE_SANITY_FAIL_EVENT_FLAG				0x020C
+#define IW_SET_KEY_DONE_WPA1_EVENT_FLAG				0x020D
+#define IW_SET_KEY_DONE_WPA2_EVENT_FLAG				0x020E
+#define IW_STA_LINKUP_EVENT_FLAG					0x020F
+#define IW_STA_LINKDOWN_EVENT_FLAG					0x0210
+#define IW_SCAN_COMPLETED_EVENT_FLAG				0x0211
+#define IW_SCAN_ENQUEUE_FAIL_EVENT_FLAG				0x0212
+// if add new system event flag, please upadte the IW_SYS_EVENT_FLAG_END
+#define	IW_SYS_EVENT_FLAG_END                       0x0212
+#define	IW_SYS_EVENT_TYPE_NUM						(IW_SYS_EVENT_FLAG_END - IW_SYS_EVENT_FLAG_START + 1)
+// For system event - end
+
+// For spoof attack event - start
+#define	IW_SPOOF_EVENT_FLAG_START                   0x0300
+#define IW_CONFLICT_SSID_EVENT_FLAG					0x0300
+#define IW_SPOOF_ASSOC_RESP_EVENT_FLAG				0x0301
+#define IW_SPOOF_REASSOC_RESP_EVENT_FLAG			0x0302
+#define IW_SPOOF_PROBE_RESP_EVENT_FLAG				0x0303
+#define IW_SPOOF_BEACON_EVENT_FLAG					0x0304
+#define IW_SPOOF_DISASSOC_EVENT_FLAG				0x0305
+#define IW_SPOOF_AUTH_EVENT_FLAG					0x0306
+#define IW_SPOOF_DEAUTH_EVENT_FLAG					0x0307
+#define IW_SPOOF_UNKNOWN_MGMT_EVENT_FLAG			0x0308
+#define IW_REPLAY_ATTACK_EVENT_FLAG					0x0309
+// if add new spoof attack event flag, please upadte the IW_SPOOF_EVENT_FLAG_END
+#define	IW_SPOOF_EVENT_FLAG_END                     0x0309
+#define	IW_SPOOF_EVENT_TYPE_NUM						(IW_SPOOF_EVENT_FLAG_END - IW_SPOOF_EVENT_FLAG_START + 1)
+// For spoof attack event - end
+
+// For flooding attack event - start
+#define	IW_FLOOD_EVENT_FLAG_START                   0x0400
+#define IW_FLOOD_AUTH_EVENT_FLAG					0x0400
+#define IW_FLOOD_ASSOC_REQ_EVENT_FLAG				0x0401
+#define IW_FLOOD_REASSOC_REQ_EVENT_FLAG				0x0402
+#define IW_FLOOD_PROBE_REQ_EVENT_FLAG				0x0403
+#define IW_FLOOD_DISASSOC_EVENT_FLAG				0x0404
+#define IW_FLOOD_DEAUTH_EVENT_FLAG					0x0405
+#define IW_FLOOD_EAP_REQ_EVENT_FLAG					0x0406
+// if add new flooding attack event flag, please upadte the IW_FLOOD_EVENT_FLAG_END
+#define	IW_FLOOD_EVENT_FLAG_END                   	0x0406
+#define	IW_FLOOD_EVENT_TYPE_NUM						(IW_FLOOD_EVENT_FLAG_END - IW_FLOOD_EVENT_FLAG_START + 1)
+// For flooding attack - end
+
+// End - WIRELESS EVENTS definition
+
+#ifdef CONFIG_STA_SUPPORT
+// definition for DLS, kathy
+#define	MAX_NUM_OF_INIT_DLS_ENTRY   1
+#define	MAX_NUM_OF_DLS_ENTRY        MAX_NUMBER_OF_DLS_ENTRY
+
+//Block ACK , rt2860, kathy
+#define MAX_TX_REORDERBUF		64
+#define MAX_RX_REORDERBUF		64
+#define DEFAULT_TX_TIMEOUT		30
+#define DEFAULT_RX_TIMEOUT		30
+#ifndef CONFIG_AP_SUPPORT
+#define MAX_BARECI_SESSION		8
+#endif
+
+#ifndef IW_ESSID_MAX_SIZE
+/* Maximum size of the ESSID and pAd->nickname strings */
+#define IW_ESSID_MAX_SIZE   		32
+#endif
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef MCAST_RATE_SPECIFIC
+#define MCAST_DISABLE	0
+#define MCAST_CCK		1
+#define MCAST_OFDM		2
+#define MCAST_HTMIX		3
+#endif // MCAST_RATE_SPECIFIC //
+
+// For AsicRadioOff/AsicRadioOn function
+#define DOT11POWERSAVE		0
+#define GUIRADIO_OFF		1
+#define RTMP_HALT		    2
+#define GUI_IDLE_POWER_SAVE		3
+// --
+
+
+// definition for WpaSupport flag
+#define WPA_SUPPLICANT_DISABLE				0
+#define WPA_SUPPLICANT_ENABLE				1
+#define	WPA_SUPPLICANT_ENABLE_WITH_WEB_UI	2
+
+// Endian byte swapping codes
+#define SWAP16(x) \
+    ((UINT16)( \
+    (((UINT16)(x) & (UINT16) 0x00ffU) << 8) | \
+    (((UINT16)(x) & (UINT16) 0xff00U) >> 8) ))
+
+#define SWAP32(x) \
+    ((UINT32)( \
+    (((UINT32)(x) & (UINT32) 0x000000ffUL) << 24) | \
+    (((UINT32)(x) & (UINT32) 0x0000ff00UL) <<  8) | \
+    (((UINT32)(x) & (UINT32) 0x00ff0000UL) >>  8) | \
+    (((UINT32)(x) & (UINT32) 0xff000000UL) >> 24) ))
+
+#define SWAP64(x) \
+    ((UINT64)( \
+    (UINT64)(((UINT64)(x) & (UINT64) 0x00000000000000ffULL) << 56) | \
+    (UINT64)(((UINT64)(x) & (UINT64) 0x000000000000ff00ULL) << 40) | \
+    (UINT64)(((UINT64)(x) & (UINT64) 0x0000000000ff0000ULL) << 24) | \
+    (UINT64)(((UINT64)(x) & (UINT64) 0x00000000ff000000ULL) <<  8) | \
+    (UINT64)(((UINT64)(x) & (UINT64) 0x000000ff00000000ULL) >>  8) | \
+    (UINT64)(((UINT64)(x) & (UINT64) 0x0000ff0000000000ULL) >> 24) | \
+    (UINT64)(((UINT64)(x) & (UINT64) 0x00ff000000000000ULL) >> 40) | \
+    (UINT64)(((UINT64)(x) & (UINT64) 0xff00000000000000ULL) >> 56) ))
+
+#ifdef RT_BIG_ENDIAN
+
+#define cpu2le64(x) SWAP64((x))
+#define le2cpu64(x) SWAP64((x))
+#define cpu2le32(x) SWAP32((x))
+#define le2cpu32(x) SWAP32((x))
+#define cpu2le16(x) SWAP16((x))
+#define le2cpu16(x) SWAP16((x))
+#define cpu2be64(x) ((UINT64)(x))
+#define be2cpu64(x) ((UINT64)(x))
+#define cpu2be32(x) ((UINT32)(x))
+#define be2cpu32(x) ((UINT32)(x))
+#define cpu2be16(x) ((UINT16)(x))
+#define be2cpu16(x) ((UINT16)(x))
+
+#else   // Little_Endian
+
+#define cpu2le64(x) ((UINT64)(x))
+#define le2cpu64(x) ((UINT64)(x))
+#define cpu2le32(x) ((UINT32)(x))
+#define le2cpu32(x) ((UINT32)(x))
+#define cpu2le16(x) ((UINT16)(x))
+#define le2cpu16(x) ((UINT16)(x))
+#define cpu2be64(x) SWAP64((x))
+#define be2cpu64(x) SWAP64((x))
+#define cpu2be32(x) SWAP32((x))
+#define be2cpu32(x) SWAP32((x))
+#define cpu2be16(x) SWAP16((x))
+#define be2cpu16(x) SWAP16((x))
+
+#endif  // RT_BIG_ENDIAN
+
+#endif  // __RTMP_DEF_H__
+
+
diff --git a/drivers/staging/rt2860/rtmp_type.h b/drivers/staging/rt2860/rtmp_type.h
new file mode 100644
index 00000000000..1fd7df1e179
--- /dev/null
+++ b/drivers/staging/rt2860/rtmp_type.h
@@ -0,0 +1,94 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    rtmp_type.h
+
+    Abstract:
+
+    Revision History:
+    Who         When            What
+    --------    ----------      ----------------------------------------------
+    Name        Date            Modification logs
+    Paul Lin    1-2-2004
+*/
+#ifndef __RTMP_TYPE_H__
+#define __RTMP_TYPE_H__
+
+#define PACKED  __attribute__ ((packed))
+
+// Put platform dependent declaration here
+// For example, linux type definition
+typedef unsigned char		UINT8;
+typedef unsigned short		UINT16;
+typedef unsigned int		UINT32;
+typedef unsigned long long	UINT64;
+typedef int					INT32;
+typedef long long 			INT64;
+
+typedef unsigned char *			PUINT8;
+typedef unsigned short *		PUINT16;
+typedef unsigned int *			PUINT32;
+typedef unsigned long long *	PUINT64;
+typedef int	*					PINT32;
+typedef long long * 			PINT64;
+
+typedef signed char			CHAR;
+typedef signed short		SHORT;
+typedef signed int			INT;
+typedef signed long			LONG;
+typedef signed long long	LONGLONG;
+
+
+typedef unsigned char		UCHAR;
+typedef unsigned short		USHORT;
+typedef unsigned int		UINT;
+typedef unsigned long		ULONG;
+typedef unsigned long long	ULONGLONG;
+
+typedef unsigned char		BOOLEAN;
+typedef void				VOID;
+
+typedef VOID *				PVOID;
+typedef CHAR *				PCHAR;
+typedef UCHAR * 			PUCHAR;
+typedef USHORT *			PUSHORT;
+typedef LONG *				PLONG;
+typedef ULONG *				PULONG;
+typedef UINT *				PUINT;
+
+typedef unsigned int	NDIS_MEDIA_STATE;
+
+typedef union _LARGE_INTEGER {
+    struct {
+        UINT LowPart;
+        INT32 HighPart;
+    } u;
+    INT64 QuadPart;
+} LARGE_INTEGER;
+
+#endif  // __RTMP_TYPE_H__
+
diff --git a/drivers/staging/rt2860/spectrum.h b/drivers/staging/rt2860/spectrum.h
new file mode 100644
index 00000000000..60f25dbdba9
--- /dev/null
+++ b/drivers/staging/rt2860/spectrum.h
@@ -0,0 +1,322 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+*/
+
+#ifndef __SPECTRUM_H__
+#define __SPECTRUM_H__
+
+#include "rtmp_type.h"
+#include "spectrum_def.h"
+
+typedef struct PACKED _TPC_REPORT_INFO
+{
+	UINT8 TxPwr;
+	UINT8 LinkMargin;
+} TPC_REPORT_INFO, *PTPC_REPORT_INFO;
+
+typedef struct PACKED _CH_SW_ANN_INFO
+{
+	UINT8 ChSwMode;
+	UINT8 Channel;
+	UINT8 ChSwCnt;
+} CH_SW_ANN_INFO, *PCH_SW_ANN_INFO;
+
+typedef union PACKED _MEASURE_REQ_MODE
+{
+#ifdef RT_BIG_ENDIAN
+	struct PACKED
+	{
+		UINT8 Rev1:4;
+		UINT8 Report:1;
+		UINT8 Request:1;
+		UINT8 Enable:1;
+		UINT8 Rev0:1;
+	} field;
+#else
+	struct PACKED
+	{
+		UINT8 Rev0:1;
+		UINT8 Enable:1;
+		UINT8 Request:1;
+		UINT8 Report:1;
+		UINT8 Rev1:4;
+	} field;
+#endif // RT_BIG_ENDIAN //
+	UINT8 word;
+} MEASURE_REQ_MODE, *PMEASURE_REQ_MODE;
+
+typedef struct PACKED _MEASURE_REQ
+{
+	UINT8 ChNum;
+	UINT64 MeasureStartTime;
+	UINT16 MeasureDuration;
+} MEASURE_REQ, *PMEASURE_REQ;
+
+typedef struct PACKED _MEASURE_REQ_INFO
+{
+	UINT8 Token;
+	MEASURE_REQ_MODE ReqMode;
+	UINT8 ReqType;
+	MEASURE_REQ MeasureReq;
+} MEASURE_REQ_INFO, *PMEASURE_REQ_INFO;
+
+typedef union PACKED _MEASURE_BASIC_REPORT_MAP
+{
+#ifdef RT_BIG_ENDIAN
+	struct PACKED
+	{
+		UINT8 Rev:3;
+		UINT8 Unmeasure:1;
+		UINT8 Radar:1;
+		UINT8 UnidentifiedSignal:1;
+		UINT8 OfdmPreamble:1;
+		UINT8 BSS:1;
+	} field;
+#else
+	struct PACKED
+	{
+		UINT8 BSS:1;
+		UINT8 OfdmPreamble:1;
+		UINT8 UnidentifiedSignal:1;
+		UINT8 Radar:1;
+		UINT8 Unmeasure:1;
+		UINT8 Rev:3;
+	} field;
+#endif // RT_BIG_ENDIAN //
+	UINT8 word;
+} MEASURE_BASIC_REPORT_MAP, *PMEASURE_BASIC_REPORT_MAP;
+
+typedef struct PACKED _MEASURE_BASIC_REPORT
+{
+	UINT8 ChNum;
+	UINT64 MeasureStartTime;
+	UINT16 MeasureDuration;
+	MEASURE_BASIC_REPORT_MAP Map;
+} MEASURE_BASIC_REPORT, *PMEASURE_BASIC_REPORT;
+
+typedef struct PACKED _MEASURE_CCA_REPORT
+{
+	UINT8 ChNum;
+	UINT64 MeasureStartTime;
+	UINT16 MeasureDuration;
+	UINT8 CCA_Busy_Fraction;
+} MEASURE_CCA_REPORT, *PMEASURE_CCA_REPORT;
+
+typedef struct PACKED _MEASURE_RPI_REPORT
+{
+	UINT8 ChNum;
+	UINT64 MeasureStartTime;
+	UINT16 MeasureDuration;
+	UINT8 RPI_Density[8];
+} MEASURE_RPI_REPORT, *PMEASURE_RPI_REPORT;
+
+typedef union PACKED _MEASURE_REPORT_MODE
+{
+	struct PACKED
+	{
+#ifdef RT_BIG_ENDIAN
+		UINT8 Rev:5;
+		UINT8 Refused:1;
+		UINT8 Incapable:1;
+		UINT8 Late:1;
+#else
+		UINT8 Late:1;
+		UINT8 Incapable:1;
+		UINT8 Refused:1;
+		UINT8 Rev:5;
+#endif // RT_BIG_ENDIAN //
+	} field;
+	UINT8 word;
+} MEASURE_REPORT_MODE, *PMEASURE_REPORT_MODE;
+
+typedef struct PACKED _MEASURE_REPORT_INFO
+{
+	UINT8 Token;
+	MEASURE_REPORT_MODE ReportMode;
+	UINT8 ReportType;
+	UINT8 Octect[0];
+} MEASURE_REPORT_INFO, *PMEASURE_REPORT_INFO;
+
+typedef struct PACKED _QUIET_INFO
+{
+	UINT8 QuietCnt;
+	UINT8 QuietPeriod;
+	UINT8 QuietDuration;
+	UINT8 QuietOffset;
+} QUIET_INFO, *PQUIET_INFO;
+
+/*
+	==========================================================================
+	Description:
+		Prepare Measurement request action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueMeasurementReq(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UINT8 MeasureToken,
+	IN UINT8 MeasureReqMode,
+	IN UINT8 MeasureReqType,
+	IN UINT8 MeasureCh,
+	IN UINT16 MeasureDuration);
+
+/*
+	==========================================================================
+	Description:
+		Prepare Measurement report action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueMeasurementRep(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UINT8 DialogToken,
+	IN UINT8 MeasureToken,
+	IN UINT8 MeasureReqMode,
+	IN UINT8 MeasureReqType,
+	IN UINT8 ReportInfoLen,
+	IN PUINT8 pReportInfo);
+
+/*
+	==========================================================================
+	Description:
+		Prepare TPC Request action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueTPCReq(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UCHAR DialogToken);
+
+/*
+	==========================================================================
+	Description:
+		Prepare TPC Report action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueTPCRep(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UINT8 DialogToken,
+	IN UINT8 TxPwr,
+	IN UINT8 LinkMargin);
+
+/*
+	==========================================================================
+	Description:
+		Prepare Channel Switch Announcement action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+		2. Channel switch announcement mode.
+		2. a New selected channel.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueChSwAnn(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UINT8 ChSwMode,
+	IN UINT8 NewCh);
+
+/*
+	==========================================================================
+	Description:
+		Spectrun action frames Handler such as channel switch annoucement,
+		measurement report, measurement request actions frames.
+
+	Parametrs:
+		Elme - MLME message containing the received frame
+
+	Return	: None.
+	==========================================================================
+ */
+VOID PeerSpectrumAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+/*
+	==========================================================================
+	Description:
+
+	Parametrs:
+
+	Return	: None.
+	==========================================================================
+ */
+INT Set_MeasureReq_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_TpcReq_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+VOID MeasureReqTabInit(
+	IN PRTMP_ADAPTER pAd);
+
+VOID MeasureReqTabExit(
+	IN PRTMP_ADAPTER pAd);
+
+VOID TpcReqTabInit(
+	IN PRTMP_ADAPTER pAd);
+
+VOID TpcReqTabExit(
+	IN PRTMP_ADAPTER pAd);
+
+VOID NotifyChSwAnnToPeerAPs(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pRA,
+	IN PUCHAR pTA,
+	IN UINT8 ChSwMode,
+	IN UINT8 Channel);
+#endif // __SPECTRUM_H__ //
+
diff --git a/drivers/staging/rt2860/spectrum_def.h b/drivers/staging/rt2860/spectrum_def.h
new file mode 100644
index 00000000000..4ca4817bba0
--- /dev/null
+++ b/drivers/staging/rt2860/spectrum_def.h
@@ -0,0 +1,95 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+	spectrum_def.h
+
+    Abstract:
+    Handle association related requests either from WSTA or from local MLME
+
+    Revision History:
+    Who          When          What
+    ---------    ----------    ----------------------------------------------
+	Fonchi Wu    2008	  	   created for 802.11h
+ */
+
+#ifndef __SPECTRUM_DEF_H__
+#define __SPECTRUM_DEF_H__
+
+#define MAX_MEASURE_REQ_TAB_SIZE		3
+#define MAX_HASH_MEASURE_REQ_TAB_SIZE	MAX_MEASURE_REQ_TAB_SIZE
+
+#define MAX_TPC_REQ_TAB_SIZE			3
+#define MAX_HASH_TPC_REQ_TAB_SIZE		MAX_TPC_REQ_TAB_SIZE
+
+#define MIN_RCV_PWR				100		/* Negative value ((dBm) */
+
+#define RM_TPC_REQ				0
+#define RM_MEASURE_REQ			1
+
+#define RM_BASIC				0
+#define RM_CCA					1
+#define RM_RPI_HISTOGRAM		2
+
+#define TPC_REQ_AGE_OUT			500		/* ms */
+#define MQ_REQ_AGE_OUT			500		/* ms */
+
+#define TPC_DIALOGTOKEN_HASH_INDEX(_DialogToken)	((_DialogToken) % MAX_HASH_TPC_REQ_TAB_SIZE)
+#define MQ_DIALOGTOKEN_HASH_INDEX(_DialogToken)		((_DialogToken) % MAX_MEASURE_REQ_TAB_SIZE)
+
+typedef struct _MEASURE_REQ_ENTRY
+{
+	struct _MEASURE_REQ_ENTRY *pNext;
+	ULONG lastTime;
+	BOOLEAN	Valid;
+	UINT8 DialogToken;
+	UINT8 MeasureDialogToken[3];	// 0:basic measure, 1: CCA measure, 2: RPI_Histogram measure.
+} MEASURE_REQ_ENTRY, *PMEASURE_REQ_ENTRY;
+
+typedef struct _MEASURE_REQ_TAB
+{
+	UCHAR Size;
+	PMEASURE_REQ_ENTRY Hash[MAX_HASH_MEASURE_REQ_TAB_SIZE];
+	MEASURE_REQ_ENTRY Content[MAX_MEASURE_REQ_TAB_SIZE];
+} MEASURE_REQ_TAB, *PMEASURE_REQ_TAB;
+
+typedef struct _TPC_REQ_ENTRY
+{
+	struct _TPC_REQ_ENTRY *pNext;
+	ULONG lastTime;
+	BOOLEAN Valid;
+	UINT8 DialogToken;
+} TPC_REQ_ENTRY, *PTPC_REQ_ENTRY;
+
+typedef struct _TPC_REQ_TAB
+{
+	UCHAR Size;
+	PTPC_REQ_ENTRY Hash[MAX_HASH_TPC_REQ_TAB_SIZE];
+	TPC_REQ_ENTRY Content[MAX_TPC_REQ_TAB_SIZE];
+} TPC_REQ_TAB, *PTPC_REQ_TAB;
+
+#endif // __SPECTRUM_DEF_H__ //
+
diff --git a/drivers/staging/rt2860/sta/aironet.c b/drivers/staging/rt2860/sta/aironet.c
new file mode 100644
index 00000000000..4af4a190618
--- /dev/null
+++ b/drivers/staging/rt2860/sta/aironet.c
@@ -0,0 +1,1312 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	aironet.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Paul Lin	04-06-15		Initial
+*/
+#include "../rt_config.h"
+
+/*
+	==========================================================================
+	Description:
+		association	state machine init,	including state	transition and timer init
+	Parameters:
+		S -	pointer	to the association state machine
+	==========================================================================
+ */
+VOID	AironetStateMachineInit(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	STATE_MACHINE		*S,
+	OUT	STATE_MACHINE_FUNC	Trans[])
+{
+	StateMachineInit(S,	Trans, MAX_AIRONET_STATE, MAX_AIRONET_MSG, (STATE_MACHINE_FUNC)Drop, AIRONET_IDLE, AIRONET_MACHINE_BASE);
+	StateMachineSetAction(S, AIRONET_IDLE, MT2_AIRONET_MSG, (STATE_MACHINE_FUNC)AironetMsgAction);
+	StateMachineSetAction(S, AIRONET_IDLE, MT2_AIRONET_SCAN_REQ, (STATE_MACHINE_FUNC)AironetRequestAction);
+	StateMachineSetAction(S, AIRONET_SCANNING, MT2_AIRONET_SCAN_DONE, (STATE_MACHINE_FUNC)AironetReportAction);
+}
+
+/*
+	==========================================================================
+	Description:
+		This is	state machine function.
+		When receiving EAPOL packets which is  for 802.1x key management.
+		Use	both in	WPA, and WPAPSK	case.
+		In this	function, further dispatch to different	functions according	to the received	packet.	 3 categories are :
+		  1.  normal 4-way pairwisekey and 2-way groupkey handshake
+		  2.  MIC error	(Countermeasures attack)  report packet	from STA.
+		  3.  Request for pairwise/group key update	from STA
+	Return:
+	==========================================================================
+*/
+VOID	AironetMsgAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MLME_QUEUE_ELEM	*Elem)
+{
+	USHORT							Length;
+	UCHAR							Index, i;
+	PUCHAR							pData;
+	PAIRONET_RM_REQUEST_FRAME		pRMReq;
+	PRM_REQUEST_ACTION				pReqElem;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("-----> AironetMsgAction\n"));
+
+	// 0. Get Aironet IAPP header first
+	pRMReq = (PAIRONET_RM_REQUEST_FRAME) &Elem->Msg[LENGTH_802_11];
+	pData  = (PUCHAR) &Elem->Msg[LENGTH_802_11];
+
+	// 1. Change endian format form network to little endian
+	Length = be2cpu16(pRMReq->IAPP.Length);
+
+	// 2.0 Sanity check, this should only happen when CCX 2.0 support is enabled
+	if (pAd->StaCfg.CCXEnable != TRUE)
+		return;
+
+	// 2.1 Radio measurement must be on
+	if (pAd->StaCfg.CCXControl.field.RMEnable != 1)
+		return;
+
+	// 2.2. Debug print all bit information
+	DBGPRINT(RT_DEBUG_TRACE, ("IAPP ID & Length %d\n", Length));
+	DBGPRINT(RT_DEBUG_TRACE, ("IAPP Type %x\n", pRMReq->IAPP.Type));
+	DBGPRINT(RT_DEBUG_TRACE, ("IAPP SubType %x\n", pRMReq->IAPP.SubType));
+	DBGPRINT(RT_DEBUG_TRACE, ("IAPP Dialog Token %x\n", pRMReq->IAPP.Token));
+	DBGPRINT(RT_DEBUG_TRACE, ("IAPP Activation Delay %x\n", pRMReq->Delay));
+	DBGPRINT(RT_DEBUG_TRACE, ("IAPP Measurement Offset %x\n", pRMReq->Offset));
+
+	// 3. Check IAPP frame type, it must be 0x32 for Cisco Aironet extension
+	if (pRMReq->IAPP.Type != AIRONET_IAPP_TYPE)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Wrong IAPP type for Cisco Aironet extension\n"));
+		return;
+	}
+
+	// 4. Check IAPP frame subtype, it must be 0x01 for Cisco Aironet extension request.
+	//    Since we are acting as client only, we will disregards reply subtype.
+	if (pRMReq->IAPP.SubType != AIRONET_IAPP_SUBTYPE_REQUEST)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Wrong IAPP subtype for Cisco Aironet extension\n"));
+		return;
+	}
+
+	// 5. Verify Destination MAC and Source MAC, both should be all zeros.
+	if (! MAC_ADDR_EQUAL(pRMReq->IAPP.DA, ZERO_MAC_ADDR))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Wrong IAPP DA for Cisco Aironet extension, it's not Zero\n"));
+		return;
+	}
+
+	if (! MAC_ADDR_EQUAL(pRMReq->IAPP.SA, ZERO_MAC_ADDR))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Wrong IAPP SA for Cisco Aironet extension, it's not Zero\n"));
+		return;
+	}
+
+	// 6. Reinit all report related fields
+	NdisZeroMemory(pAd->StaCfg.FrameReportBuf, 2048);
+	NdisZeroMemory(pAd->StaCfg.BssReportOffset, sizeof(USHORT) * MAX_LEN_OF_BSS_TABLE);
+	NdisZeroMemory(pAd->StaCfg.MeasurementRequest, sizeof(RM_REQUEST_ACTION) * 4);
+
+	// 7. Point to the start of first element report element
+	pAd->StaCfg.FrameReportLen   = LENGTH_802_11 + sizeof(AIRONET_IAPP_HEADER);
+	DBGPRINT(RT_DEBUG_TRACE, ("FR len = %d\n", pAd->StaCfg.FrameReportLen));
+	pAd->StaCfg.LastBssIndex     = 0xff;
+	pAd->StaCfg.RMReqCnt         = 0;
+	pAd->StaCfg.ParallelReq      = FALSE;
+	pAd->StaCfg.ParallelDuration = 0;
+	pAd->StaCfg.ParallelChannel  = 0;
+	pAd->StaCfg.IAPPToken        = pRMReq->IAPP.Token;
+	pAd->StaCfg.CurrentRMReqIdx  = 0;
+	pAd->StaCfg.CLBusyBytes      = 0;
+	// Reset the statistics
+	for (i = 0; i < 8; i++)
+		pAd->StaCfg.RPIDensity[i] = 0;
+
+	Index = 0;
+
+	// 8. Save dialog token for report
+	pAd->StaCfg.IAPPToken = pRMReq->IAPP.Token;
+
+	// Save Activation delay & measurement offset, Not really needed
+
+	// 9. Point to the first request element
+	pData += sizeof(AIRONET_RM_REQUEST_FRAME);
+	//    Length should exclude the CISCO Aironet SNAP header
+	Length -= (sizeof(AIRONET_RM_REQUEST_FRAME) - LENGTH_802_1_H);
+
+	// 10. Start Parsing the Measurement elements.
+	//    Be careful about multiple MR elements within one frames.
+	while (Length > 0)
+	{
+		pReqElem = (PRM_REQUEST_ACTION) pData;
+		switch (pReqElem->ReqElem.Eid)
+		{
+			case IE_MEASUREMENT_REQUEST:
+				// From the example, it seems we only need to support one request in one frame
+				// There is no multiple request in one frame.
+				// Besides, looks like we need to take care the measurement request only.
+				// The measurement request is always 4 bytes.
+
+				// Start parsing this type of request.
+				// 0. Eid is IE_MEASUREMENT_REQUEST
+				// 1. Length didn't include Eid and Length field, it always be 8.
+				// 2. Measurement Token, we nned to save it for the corresponding report.
+				// 3. Measurement Mode, Although there are definitions, but we din't see value other than
+				//    0 from test specs examples.
+				// 4. Measurement Type, this is what we need to do.
+				switch (pReqElem->ReqElem.Type)
+				{
+					case MSRN_TYPE_CHANNEL_LOAD_REQ:
+					case MSRN_TYPE_NOISE_HIST_REQ:
+					case MSRN_TYPE_BEACON_REQ:
+						// Check the Enable non-serving channel measurement control
+						if (pAd->StaCfg.CCXControl.field.DCRMEnable == 0)
+						{
+							// Check channel before enqueue the action
+							if (pReqElem->Measurement.Channel != pAd->CommonCfg.Channel)
+								break;
+						}
+						else
+						{
+							// If off channel measurement, check the TU duration limit
+							if (pReqElem->Measurement.Channel != pAd->CommonCfg.Channel)
+								if (pReqElem->Measurement.Duration > pAd->StaCfg.CCXControl.field.TuLimit)
+									break;
+						}
+
+						// Save requests and execute actions later
+						NdisMoveMemory(&pAd->StaCfg.MeasurementRequest[Index], pReqElem, sizeof(RM_REQUEST_ACTION));
+						Index += 1;
+						break;
+
+					case MSRN_TYPE_FRAME_REQ:
+						// Since it's option, we will support later
+						// FrameRequestAction(pAd, pData);
+						break;
+
+					default:
+						break;
+				}
+
+				// Point to next Measurement request
+				pData  += sizeof(RM_REQUEST_ACTION);
+				Length -= sizeof(RM_REQUEST_ACTION);
+				break;
+
+			// We accept request only, all others are dropped
+			case IE_MEASUREMENT_REPORT:
+			case IE_AP_TX_POWER:
+			case IE_MEASUREMENT_CAPABILITY:
+			default:
+				return;
+		}
+	}
+
+	// 11. Update some flags and index
+	pAd->StaCfg.RMReqCnt = Index;
+
+	if (Index)
+	{
+		MlmeEnqueue(pAd, AIRONET_STATE_MACHINE, MT2_AIRONET_SCAN_REQ, 0, NULL);
+		RT28XX_MLME_HANDLER(pAd);
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<----- AironetMsgAction\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	AironetRequestAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MLME_QUEUE_ELEM	*Elem)
+{
+	PRM_REQUEST_ACTION	pReq;
+
+	// 1. Point to next request element
+	pReq = (PRM_REQUEST_ACTION) &pAd->StaCfg.MeasurementRequest[pAd->StaCfg.CurrentRMReqIdx];
+
+	// 2. Parse measurement type and call appropriate functions
+	if (pReq->ReqElem.Type == MSRN_TYPE_CHANNEL_LOAD_REQ)
+		// Channel Load measurement request
+		ChannelLoadRequestAction(pAd, pAd->StaCfg.CurrentRMReqIdx);
+	else if (pReq->ReqElem.Type == MSRN_TYPE_NOISE_HIST_REQ)
+		// Noise Histogram measurement request
+		NoiseHistRequestAction(pAd, pAd->StaCfg.CurrentRMReqIdx);
+	else if (pReq->ReqElem.Type == MSRN_TYPE_BEACON_REQ)
+		// Beacon measurement request
+		BeaconRequestAction(pAd, pAd->StaCfg.CurrentRMReqIdx);
+	else
+		// Unknown. Do nothing and return, this should never happen
+		return;
+
+	// 3. Peek into the next request, if it's parallel, we will update the scan time to the largest one
+	if ((pAd->StaCfg.CurrentRMReqIdx + 1) < pAd->StaCfg.RMReqCnt)
+	{
+		pReq = (PRM_REQUEST_ACTION) &pAd->StaCfg.MeasurementRequest[pAd->StaCfg.CurrentRMReqIdx + 1];
+		// Check for parallel bit
+		if ((pReq->ReqElem.Mode & 0x01) && (pReq->Measurement.Channel == pAd->StaCfg.CCXScanChannel))
+		{
+			// Update parallel mode request information
+			pAd->StaCfg.ParallelReq = TRUE;
+			pAd->StaCfg.CCXScanTime = ((pReq->Measurement.Duration > pAd->StaCfg.CCXScanTime) ?
+			(pReq->Measurement.Duration) : (pAd->StaCfg.CCXScanTime));
+		}
+	}
+
+	// 4. Call RT28XX_MLME_HANDLER to execute the request mlme commands, Scan request is the only one used
+	RT28XX_MLME_HANDLER(pAd);
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Prepare channel load report action, special scan operation added
+		to support
+
+	Arguments:
+		pAd	Pointer	to our adapter
+		pData		Start from element ID
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	ChannelLoadRequestAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Index)
+{
+	PRM_REQUEST_ACTION				pReq;
+	MLME_SCAN_REQ_STRUCT			ScanReq;
+	UCHAR							ZeroSsid[32];
+	NDIS_STATUS						NStatus;
+	PUCHAR							pOutBuffer = NULL;
+	PHEADER_802_11					pNullFrame;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("ChannelLoadRequestAction ----->\n"));
+
+	pReq = (PRM_REQUEST_ACTION) &pAd->StaCfg.MeasurementRequest[Index];
+	NdisZeroMemory(ZeroSsid, 32);
+
+	// Prepare for special scan request
+	// The scan definition is different with our Active, Passive scan definition.
+	// For CCX2, Active means send out probe request with broadcast BSSID.
+	// Passive means no probe request sent, only listen to the beacons.
+	// The channel scanned is fixed as specified, no need to scan all channels.
+	// The scan wait time is specified in the request too.
+	// Passive scan Mode
+
+	// Control state machine is not idle, reject the request
+	if ((pAd->Mlme.CntlMachine.CurrState != CNTL_IDLE) && (Index == 0))
+		return;
+
+	// Fill out stuff for scan request
+	ScanParmFill(pAd, &ScanReq, ZeroSsid, 0, BSS_ANY, SCAN_CISCO_CHANNEL_LOAD);
+	MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_SCAN_REQ, sizeof(MLME_SCAN_REQ_STRUCT), &ScanReq);
+	pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_LIST_SCAN;
+
+	// Reset some internal control flags to make sure this scan works.
+	BssTableInit(&pAd->StaCfg.CCXBssTab);
+	pAd->StaCfg.ScanCnt        = 0;
+	pAd->StaCfg.CCXScanChannel = pReq->Measurement.Channel;
+	pAd->StaCfg.CCXScanTime    = pReq->Measurement.Duration;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Duration %d, Channel %d!\n", pReq->Measurement.Duration, pReq->Measurement.Channel));
+
+	// If it's non serving channel scan, send out a null frame with PSM bit on.
+	if (pAd->StaCfg.CCXScanChannel != pAd->CommonCfg.Channel)
+	{
+		// Use MLME enqueue method
+		NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+		if (NStatus	!= NDIS_STATUS_SUCCESS)
+			return;
+
+		pNullFrame = (PHEADER_802_11) pOutBuffer;;
+		// Make the power save Null frame with PSM bit on
+		MgtMacHeaderInit(pAd, pNullFrame, SUBTYPE_NULL_FUNC, 1, pAd->CommonCfg.Bssid, pAd->CommonCfg.Bssid);
+		pNullFrame->Duration 	= 0;
+		pNullFrame->FC.Type 	= BTYPE_DATA;
+		pNullFrame->FC.PwrMgmt	= PWR_SAVE;
+
+		// Send using priority queue
+		MiniportMMRequest(pAd, 0, pOutBuffer, sizeof(HEADER_802_11));
+		MlmeFreeMemory(pAd, pOutBuffer);
+		DBGPRINT(RT_DEBUG_TRACE, ("Send PSM Data frame for off channel RM\n"));
+		RTMPusecDelay(5000);
+	}
+
+	pAd->StaCfg.CCXReqType     = MSRN_TYPE_CHANNEL_LOAD_REQ;
+	pAd->StaCfg.CLBusyBytes    = 0;
+	// Enable Rx with promiscuous reception
+	RTMP_IO_WRITE32(pAd, RX_FILTR_CFG, 0x1010);
+
+	// Set channel load measurement flag
+	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_RADIO_MEASUREMENT);
+
+	pAd->Mlme.AironetMachine.CurrState = AIRONET_SCANNING;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("ChannelLoadRequestAction <-----\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Prepare noise histogram report action, special scan operation added
+		to support
+
+	Arguments:
+		pAd	Pointer	to our adapter
+		pData		Start from element ID
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	NoiseHistRequestAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Index)
+{
+	PRM_REQUEST_ACTION				pReq;
+	MLME_SCAN_REQ_STRUCT			ScanReq;
+	UCHAR							ZeroSsid[32], i;
+	NDIS_STATUS						NStatus;
+	PUCHAR							pOutBuffer = NULL;
+	PHEADER_802_11					pNullFrame;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("NoiseHistRequestAction ----->\n"));
+
+	pReq = (PRM_REQUEST_ACTION) &pAd->StaCfg.MeasurementRequest[Index];
+	NdisZeroMemory(ZeroSsid, 32);
+
+	// Prepare for special scan request
+	// The scan definition is different with our Active, Passive scan definition.
+	// For CCX2, Active means send out probe request with broadcast BSSID.
+	// Passive means no probe request sent, only listen to the beacons.
+	// The channel scanned is fixed as specified, no need to scan all channels.
+	// The scan wait time is specified in the request too.
+	// Passive scan Mode
+
+	// Control state machine is not idle, reject the request
+	if ((pAd->Mlme.CntlMachine.CurrState != CNTL_IDLE) && (Index == 0))
+		return;
+
+	// Fill out stuff for scan request
+	ScanParmFill(pAd, &ScanReq, ZeroSsid, 0, BSS_ANY, SCAN_CISCO_NOISE);
+	MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_SCAN_REQ, sizeof(MLME_SCAN_REQ_STRUCT), &ScanReq);
+	pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_LIST_SCAN;
+
+	// Reset some internal control flags to make sure this scan works.
+	BssTableInit(&pAd->StaCfg.CCXBssTab);
+	pAd->StaCfg.ScanCnt        = 0;
+	pAd->StaCfg.CCXScanChannel = pReq->Measurement.Channel;
+	pAd->StaCfg.CCXScanTime    = pReq->Measurement.Duration;
+	pAd->StaCfg.CCXReqType     = MSRN_TYPE_NOISE_HIST_REQ;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Duration %d, Channel %d!\n", pReq->Measurement.Duration, pReq->Measurement.Channel));
+
+	// If it's non serving channel scan, send out a null frame with PSM bit on.
+	if (pAd->StaCfg.CCXScanChannel != pAd->CommonCfg.Channel)
+	{
+		// Use MLME enqueue method
+		NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+		if (NStatus	!= NDIS_STATUS_SUCCESS)
+			return;
+
+		pNullFrame = (PHEADER_802_11) pOutBuffer;
+		// Make the power save Null frame with PSM bit on
+		MgtMacHeaderInit(pAd, pNullFrame, SUBTYPE_NULL_FUNC, 1, pAd->CommonCfg.Bssid, pAd->CommonCfg.Bssid);
+		pNullFrame->Duration 	= 0;
+		pNullFrame->FC.Type  	= BTYPE_DATA;
+		pNullFrame->FC.PwrMgmt	= PWR_SAVE;
+
+		// Send using priority queue
+		MiniportMMRequest(pAd, 0, pOutBuffer, sizeof(HEADER_802_11));
+		MlmeFreeMemory(pAd, pOutBuffer);
+		DBGPRINT(RT_DEBUG_TRACE, ("Send PSM Data frame for off channel RM\n"));
+		RTMPusecDelay(5000);
+	}
+
+	// Reset the statistics
+	for (i = 0; i < 8; i++)
+		pAd->StaCfg.RPIDensity[i] = 0;
+
+	// Enable Rx with promiscuous reception
+	RTMP_IO_WRITE32(pAd, RX_FILTR_CFG, 0x1010);
+
+	// Set channel load measurement flag
+	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_RADIO_MEASUREMENT);
+
+	pAd->Mlme.AironetMachine.CurrState = AIRONET_SCANNING;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("NoiseHistRequestAction <-----\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Prepare Beacon report action, special scan operation added
+		to support
+
+	Arguments:
+		pAd	Pointer	to our adapter
+		pData		Start from element ID
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	BeaconRequestAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Index)
+{
+	PRM_REQUEST_ACTION				pReq;
+	NDIS_STATUS						NStatus;
+	PUCHAR							pOutBuffer = NULL;
+	PHEADER_802_11					pNullFrame;
+	MLME_SCAN_REQ_STRUCT			ScanReq;
+	UCHAR							ZeroSsid[32];
+
+	DBGPRINT(RT_DEBUG_TRACE, ("BeaconRequestAction ----->\n"));
+
+	pReq = (PRM_REQUEST_ACTION) &pAd->StaCfg.MeasurementRequest[Index];
+	NdisZeroMemory(ZeroSsid, 32);
+
+	// Prepare for special scan request
+	// The scan definition is different with our Active, Passive scan definition.
+	// For CCX2, Active means send out probe request with broadcast BSSID.
+	// Passive means no probe request sent, only listen to the beacons.
+	// The channel scanned is fixed as specified, no need to scan all channels.
+	// The scan wait time is specified in the request too.
+	if (pReq->Measurement.ScanMode == MSRN_SCAN_MODE_PASSIVE)
+	{
+		// Passive scan Mode
+		DBGPRINT(RT_DEBUG_TRACE, ("Passive Scan Mode!\n"));
+
+		// Control state machine is not idle, reject the request
+		if ((pAd->Mlme.CntlMachine.CurrState != CNTL_IDLE) && (Index == 0))
+			return;
+
+		// Fill out stuff for scan request
+		ScanParmFill(pAd, &ScanReq, ZeroSsid, 0, BSS_ANY, SCAN_CISCO_PASSIVE);
+		MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_SCAN_REQ, sizeof(MLME_SCAN_REQ_STRUCT), &ScanReq);
+		pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_LIST_SCAN;
+
+		// Reset some internal control flags to make sure this scan works.
+		BssTableInit(&pAd->StaCfg.CCXBssTab);
+		pAd->StaCfg.ScanCnt        = 0;
+		pAd->StaCfg.CCXScanChannel = pReq->Measurement.Channel;
+		pAd->StaCfg.CCXScanTime    = pReq->Measurement.Duration;
+		pAd->StaCfg.CCXReqType     = MSRN_TYPE_BEACON_REQ;
+		DBGPRINT(RT_DEBUG_TRACE, ("Duration %d!\n", pReq->Measurement.Duration));
+
+		// If it's non serving channel scan, send out a null frame with PSM bit on.
+		if (pAd->StaCfg.CCXScanChannel != pAd->CommonCfg.Channel)
+		{
+			// Use MLME enqueue method
+			NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+			if (NStatus	!= NDIS_STATUS_SUCCESS)
+				return;
+
+			pNullFrame = (PHEADER_802_11) pOutBuffer;
+			// Make the power save Null frame with PSM bit on
+			MgtMacHeaderInit(pAd, pNullFrame, SUBTYPE_NULL_FUNC, 1, pAd->CommonCfg.Bssid, pAd->CommonCfg.Bssid);
+			pNullFrame->Duration 	= 0;
+			pNullFrame->FC.Type     = BTYPE_DATA;
+			pNullFrame->FC.PwrMgmt  = PWR_SAVE;
+
+			// Send using priority queue
+			MiniportMMRequest(pAd, 0, pOutBuffer, sizeof(HEADER_802_11));
+			MlmeFreeMemory(pAd, pOutBuffer);
+			DBGPRINT(RT_DEBUG_TRACE, ("Send PSM Data frame for off channel RM\n"));
+			RTMPusecDelay(5000);
+		}
+
+		pAd->Mlme.AironetMachine.CurrState = AIRONET_SCANNING;
+	}
+	else if (pReq->Measurement.ScanMode == MSRN_SCAN_MODE_ACTIVE)
+	{
+		// Active scan Mode
+		DBGPRINT(RT_DEBUG_TRACE, ("Active Scan Mode!\n"));
+
+		// Control state machine is not idle, reject the request
+		if (pAd->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+			return;
+
+		// Fill out stuff for scan request
+		ScanParmFill(pAd, &ScanReq, ZeroSsid, 0, BSS_ANY, SCAN_CISCO_ACTIVE);
+		MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_SCAN_REQ, sizeof(MLME_SCAN_REQ_STRUCT), &ScanReq);
+		pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_LIST_SCAN;
+
+		// Reset some internal control flags to make sure this scan works.
+		BssTableInit(&pAd->StaCfg.CCXBssTab);
+		pAd->StaCfg.ScanCnt        = 0;
+		pAd->StaCfg.CCXScanChannel = pReq->Measurement.Channel;
+		pAd->StaCfg.CCXScanTime    = pReq->Measurement.Duration;
+		pAd->StaCfg.CCXReqType     = MSRN_TYPE_BEACON_REQ;
+		DBGPRINT(RT_DEBUG_TRACE, ("Duration %d!\n", pReq->Measurement.Duration));
+
+		// If it's non serving channel scan, send out a null frame with PSM bit on.
+		if (pAd->StaCfg.CCXScanChannel != pAd->CommonCfg.Channel)
+		{
+			// Use MLME enqueue method
+			NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+			if (NStatus	!= NDIS_STATUS_SUCCESS)
+				return;
+
+			pNullFrame = (PHEADER_802_11) pOutBuffer;
+			// Make the power save Null frame with PSM bit on
+			MgtMacHeaderInit(pAd, pNullFrame, SUBTYPE_NULL_FUNC, 1, pAd->CommonCfg.Bssid, pAd->CommonCfg.Bssid);
+			pNullFrame->Duration 	= 0;
+			pNullFrame->FC.Type     = BTYPE_DATA;
+			pNullFrame->FC.PwrMgmt  = PWR_SAVE;
+
+			// Send using priority queue
+			MiniportMMRequest(pAd, 0, pOutBuffer, sizeof(HEADER_802_11));
+			MlmeFreeMemory(pAd, pOutBuffer);
+			DBGPRINT(RT_DEBUG_TRACE, ("Send PSM Data frame for off channel RM\n"));
+			RTMPusecDelay(5000);
+		}
+
+		pAd->Mlme.AironetMachine.CurrState = AIRONET_SCANNING;
+	}
+	else if (pReq->Measurement.ScanMode == MSRN_SCAN_MODE_BEACON_TABLE)
+	{
+		// Beacon report Mode, report all the APS in current bss table
+		DBGPRINT(RT_DEBUG_TRACE, ("Beacon Report Mode!\n"));
+
+		// Copy current BSS table to CCX table, we can omit this step later on.
+		NdisMoveMemory(&pAd->StaCfg.CCXBssTab, &pAd->ScanTab, sizeof(BSS_TABLE));
+
+		// Create beacon report from Bss table
+		AironetCreateBeaconReportFromBssTable(pAd);
+
+		// Set state to scanning
+		pAd->Mlme.AironetMachine.CurrState = AIRONET_SCANNING;
+
+		// Enqueue report request
+		// Cisco scan request is finished, prepare beacon report
+		MlmeEnqueue(pAd, AIRONET_STATE_MACHINE, MT2_AIRONET_SCAN_DONE, 0, NULL);
+	}
+	else
+	{
+		// Wrong scan Mode
+		DBGPRINT(RT_DEBUG_TRACE, ("Wrong Scan Mode!\n"));
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("BeaconRequestAction <-----\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	AironetReportAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MLME_QUEUE_ELEM	*Elem)
+{
+	PRM_REQUEST_ACTION	pReq;
+	ULONG				Now32;
+
+    NdisGetSystemUpTime(&Now32);
+	pAd->StaCfg.LastBeaconRxTime = Now32;
+
+	pReq = (PRM_REQUEST_ACTION) &pAd->StaCfg.MeasurementRequest[pAd->StaCfg.CurrentRMReqIdx];
+
+	DBGPRINT(RT_DEBUG_TRACE, ("AironetReportAction ----->\n"));
+
+	// 1. Parse measurement type and call appropriate functions
+	if (pReq->ReqElem.Type == MSRN_TYPE_CHANNEL_LOAD_REQ)
+		// Channel Load measurement request
+		ChannelLoadReportAction(pAd, pAd->StaCfg.CurrentRMReqIdx);
+	else if (pReq->ReqElem.Type == MSRN_TYPE_NOISE_HIST_REQ)
+		// Noise Histogram measurement request
+		NoiseHistReportAction(pAd, pAd->StaCfg.CurrentRMReqIdx);
+	else if (pReq->ReqElem.Type == MSRN_TYPE_BEACON_REQ)
+		// Beacon measurement request
+		BeaconReportAction(pAd, pAd->StaCfg.CurrentRMReqIdx);
+	else
+		// Unknown. Do nothing and return
+		;
+
+	// 2. Point to the correct index of action element, start from 0
+	pAd->StaCfg.CurrentRMReqIdx++;
+
+	// 3. Check for parallel actions
+	if (pAd->StaCfg.ParallelReq == TRUE)
+	{
+		pReq = (PRM_REQUEST_ACTION) &pAd->StaCfg.MeasurementRequest[pAd->StaCfg.CurrentRMReqIdx];
+
+		// Process next action right away
+		if (pReq->ReqElem.Type == MSRN_TYPE_CHANNEL_LOAD_REQ)
+			// Channel Load measurement request
+			ChannelLoadReportAction(pAd, pAd->StaCfg.CurrentRMReqIdx);
+		else if (pReq->ReqElem.Type == MSRN_TYPE_NOISE_HIST_REQ)
+			// Noise Histogram measurement request
+			NoiseHistReportAction(pAd, pAd->StaCfg.CurrentRMReqIdx);
+
+		pAd->StaCfg.ParallelReq = FALSE;
+		pAd->StaCfg.CurrentRMReqIdx++;
+	}
+
+	if (pAd->StaCfg.CurrentRMReqIdx >= pAd->StaCfg.RMReqCnt)
+	{
+		// 4. There is no more unprocessed measurement request, go for transmit this report
+		AironetFinalReportAction(pAd);
+		pAd->Mlme.AironetMachine.CurrState = AIRONET_IDLE;
+	}
+	else
+	{
+		pReq = (PRM_REQUEST_ACTION) &pAd->StaCfg.MeasurementRequest[pAd->StaCfg.CurrentRMReqIdx];
+
+		if (pReq->Measurement.Channel != pAd->CommonCfg.Channel)
+		{
+			RTMPusecDelay(100000);
+		}
+
+		// 5. There are more requests to be measure
+		MlmeEnqueue(pAd, AIRONET_STATE_MACHINE, MT2_AIRONET_SCAN_REQ, 0, NULL);
+		RT28XX_MLME_HANDLER(pAd);
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("AironetReportAction <-----\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	AironetFinalReportAction(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	PUCHAR					pDest;
+	PAIRONET_IAPP_HEADER	pIAPP;
+	PHEADER_802_11			pHeader;
+	UCHAR					AckRate = RATE_2;
+	USHORT					AckDuration = 0;
+	NDIS_STATUS				NStatus;
+	PUCHAR					pOutBuffer = NULL;
+	ULONG					FrameLen = 0;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("AironetFinalReportAction ----->\n"));
+
+	// 0. Set up the frame pointer, Frame was inited at the end of message action
+	pDest = &pAd->StaCfg.FrameReportBuf[LENGTH_802_11];
+
+	// 1. Update report IAPP fields
+	pIAPP = (PAIRONET_IAPP_HEADER) pDest;
+
+	// 2. Copy Cisco SNAP header
+	NdisMoveMemory(pIAPP->CiscoSnapHeader, SNAP_AIRONET, LENGTH_802_1_H);
+
+	// 3. network order for this 16bit length
+	pIAPP->Length  = cpu2be16(pAd->StaCfg.FrameReportLen - LENGTH_802_11 - LENGTH_802_1_H);
+
+	// 3.1 sanity check the report length, ignore it if there is nothing to report
+	if (be2cpu16(pIAPP->Length) <= 18)
+		return;
+
+	// 4. Type must be 0x32
+	pIAPP->Type    = AIRONET_IAPP_TYPE;
+
+	// 5. SubType for report must be 0x81
+	pIAPP->SubType = AIRONET_IAPP_SUBTYPE_REPORT;
+
+	// 6. DA is not used and must be zero, although the whole frame was cleared at the start of function
+	//    We will do it again here. We can use BSSID instead
+	COPY_MAC_ADDR(pIAPP->DA, pAd->CommonCfg.Bssid);
+
+	// 7. SA is the client reporting which must be our MAC
+	COPY_MAC_ADDR(pIAPP->SA, pAd->CurrentAddress);
+
+	// 8. Copy the saved dialog token
+	pIAPP->Token = pAd->StaCfg.IAPPToken;
+
+	// 9. Make the Report frame 802.11 header
+	//    Reuse function in wpa.c
+	pHeader = (PHEADER_802_11) pAd->StaCfg.FrameReportBuf;
+	pAd->Sequence ++;
+	WpaMacHeaderInit(pAd, pHeader, 0, pAd->CommonCfg.Bssid);
+
+	// ACK size	is 14 include CRC, and its rate	is based on real time information
+	AckRate     = pAd->CommonCfg.ExpectedACKRate[pAd->CommonCfg.MlmeRate];
+	AckDuration = RTMPCalcDuration(pAd, AckRate, 14);
+	pHeader->Duration = pAd->CommonCfg.Dsifs + AckDuration;
+
+	// Use MLME enqueue method
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if (NStatus	!= NDIS_STATUS_SUCCESS)
+		return;
+
+	// 10. Prepare report frame with dynamic outbuffer. Just simply copy everything.
+	MakeOutgoingFrame(pOutBuffer,                       &FrameLen,
+	                  pAd->StaCfg.FrameReportLen, pAd->StaCfg.FrameReportBuf,
+		              END_OF_ARGS);
+
+	// 11. Send using priority queue
+	MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	pAd->StaCfg.CCXReqType = MSRN_TYPE_UNUSED;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("AironetFinalReportAction <-----\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	ChannelLoadReportAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Index)
+{
+	PMEASUREMENT_REPORT_ELEMENT	pReport;
+	PCHANNEL_LOAD_REPORT		pLoad;
+	PUCHAR						pDest;
+	UCHAR						CCABusyFraction;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("ChannelLoadReportAction ----->\n"));
+
+	// Disable Rx with promiscuous reception, make it back to normal
+	RTMP_IO_WRITE32(pAd, RX_FILTR_CFG, STANORMAL); // Staion not drop control frame will fail WiFi Certification.
+
+	// 0. Setup pointer for processing beacon & probe response
+	pDest = (PUCHAR) &pAd->StaCfg.FrameReportBuf[pAd->StaCfg.FrameReportLen];
+	pReport = (PMEASUREMENT_REPORT_ELEMENT) pDest;
+
+	// 1. Fill Measurement report element field.
+	pReport->Eid    = IE_MEASUREMENT_REPORT;
+	// Fixed Length at 9, not include Eid and length fields
+	pReport->Length = 9;
+	pReport->Token  = pAd->StaCfg.MeasurementRequest[Index].ReqElem.Token;
+	pReport->Mode   = pAd->StaCfg.MeasurementRequest[Index].ReqElem.Mode;
+	pReport->Type   = MSRN_TYPE_CHANNEL_LOAD_REQ;
+
+	// 2. Fill channel report measurement data
+	pDest += sizeof(MEASUREMENT_REPORT_ELEMENT);
+	pLoad  = (PCHANNEL_LOAD_REPORT) pDest;
+	pLoad->Channel  = pAd->StaCfg.MeasurementRequest[Index].Measurement.Channel;
+	pLoad->Spare    = 0;
+	pLoad->Duration = pAd->StaCfg.MeasurementRequest[Index].Measurement.Duration;
+
+	// 3. Calculate the CCA Busy Fraction
+	//    (Bytes + ACK size) * 8 / Tx speed * 255 / 1000 / measurement duration, use 24 us Tx speed
+	//     =  (Bytes + ACK) / 12 / duration
+	//     9 is the good value for pAd->StaCfg.CLFactor
+	// CCABusyFraction = (UCHAR) (pAd->StaCfg.CLBusyBytes / 9 / pLoad->Duration);
+	CCABusyFraction = (UCHAR) (pAd->StaCfg.CLBusyBytes / pAd->StaCfg.CLFactor / pLoad->Duration);
+	if (CCABusyFraction < 10)
+			CCABusyFraction = (UCHAR) (pAd->StaCfg.CLBusyBytes / 3 / pLoad->Duration) + 1;
+
+	pLoad->CCABusy = CCABusyFraction;
+	DBGPRINT(RT_DEBUG_TRACE, ("CLBusyByte %ld, Duration %d, Result, %d\n", pAd->StaCfg.CLBusyBytes, pLoad->Duration, CCABusyFraction));
+
+	DBGPRINT(RT_DEBUG_TRACE, ("FrameReportLen %d\n", pAd->StaCfg.FrameReportLen));
+	pAd->StaCfg.FrameReportLen += (sizeof(MEASUREMENT_REPORT_ELEMENT) + sizeof(CHANNEL_LOAD_REPORT));
+	DBGPRINT(RT_DEBUG_TRACE, ("FrameReportLen %d\n", pAd->StaCfg.FrameReportLen));
+
+	// 4. Clear channel load measurement flag
+	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_RADIO_MEASUREMENT);
+
+	// 5. reset to idle state
+	pAd->Mlme.AironetMachine.CurrState = AIRONET_IDLE;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("ChannelLoadReportAction <-----\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	NoiseHistReportAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Index)
+{
+	PMEASUREMENT_REPORT_ELEMENT	pReport;
+	PNOISE_HIST_REPORT			pNoise;
+	PUCHAR						pDest;
+	UCHAR						i,NoiseCnt;
+	USHORT						TotalRPICnt, TotalRPISum;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("NoiseHistReportAction ----->\n"));
+
+	// 0. Disable Rx with promiscuous reception, make it back to normal
+	RTMP_IO_WRITE32(pAd, RX_FILTR_CFG, STANORMAL); // Staion not drop control frame will fail WiFi Certification.
+	// 1. Setup pointer for processing beacon & probe response
+	pDest = (PUCHAR) &pAd->StaCfg.FrameReportBuf[pAd->StaCfg.FrameReportLen];
+	pReport = (PMEASUREMENT_REPORT_ELEMENT) pDest;
+
+	// 2. Fill Measurement report element field.
+	pReport->Eid    = IE_MEASUREMENT_REPORT;
+	// Fixed Length at 16, not include Eid and length fields
+	pReport->Length = 16;
+	pReport->Token  = pAd->StaCfg.MeasurementRequest[Index].ReqElem.Token;
+	pReport->Mode   = pAd->StaCfg.MeasurementRequest[Index].ReqElem.Mode;
+	pReport->Type   = MSRN_TYPE_NOISE_HIST_REQ;
+
+	// 3. Fill noise histogram report measurement data
+	pDest += sizeof(MEASUREMENT_REPORT_ELEMENT);
+	pNoise  = (PNOISE_HIST_REPORT) pDest;
+	pNoise->Channel  = pAd->StaCfg.MeasurementRequest[Index].Measurement.Channel;
+	pNoise->Spare    = 0;
+	pNoise->Duration = pAd->StaCfg.MeasurementRequest[Index].Measurement.Duration;
+	// 4. Fill Noise histogram, the total RPI counts should be 0.4 * TU
+	//    We estimate 4000 normal packets received durning 10 seconds test.
+	//    Adjust it if required.
+	// 3 is a good value for pAd->StaCfg.NHFactor
+	// TotalRPICnt = pNoise->Duration * 3 / 10;
+	TotalRPICnt = pNoise->Duration * pAd->StaCfg.NHFactor / 10;
+	TotalRPISum = 0;
+
+	for (i = 0; i < 8; i++)
+	{
+		TotalRPISum += pAd->StaCfg.RPIDensity[i];
+		DBGPRINT(RT_DEBUG_TRACE, ("RPI %d Conuts %d\n", i, pAd->StaCfg.RPIDensity[i]));
+	}
+
+	// Double check if the counter is larger than our expectation.
+	// We will replace it with the total number plus a fraction.
+	if (TotalRPISum > TotalRPICnt)
+		TotalRPICnt = TotalRPISum + pNoise->Duration / 20;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Total RPI Conuts %d\n", TotalRPICnt));
+
+	// 5. Initialize noise count for the total summation of 0xff
+	NoiseCnt = 0;
+	for (i = 1; i < 8; i++)
+	{
+		pNoise->Density[i] = (UCHAR) (pAd->StaCfg.RPIDensity[i] * 255 / TotalRPICnt);
+		if ((pNoise->Density[i] == 0) && (pAd->StaCfg.RPIDensity[i] != 0))
+			pNoise->Density[i]++;
+		NoiseCnt += pNoise->Density[i];
+		DBGPRINT(RT_DEBUG_TRACE, ("Reported RPI[%d]  = 0x%02x\n", i, pNoise->Density[i]));
+	}
+
+	// 6. RPI[0] represents the rest of counts
+	pNoise->Density[0] = 0xff - NoiseCnt;
+	DBGPRINT(RT_DEBUG_TRACE, ("Reported RPI[0]  = 0x%02x\n", pNoise->Density[0]));
+
+	pAd->StaCfg.FrameReportLen += (sizeof(MEASUREMENT_REPORT_ELEMENT) + sizeof(NOISE_HIST_REPORT));
+
+	// 7. Clear channel load measurement flag
+	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_RADIO_MEASUREMENT);
+
+	// 8. reset to idle state
+	pAd->Mlme.AironetMachine.CurrState = AIRONET_IDLE;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("NoiseHistReportAction <-----\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Prepare Beacon report action,
+
+	Arguments:
+		pAd	Pointer	to our adapter
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	BeaconReportAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Index)
+{
+	DBGPRINT(RT_DEBUG_TRACE, ("BeaconReportAction ----->\n"));
+
+	// Looks like we don't have anything thing need to do here.
+	// All measurement report already finished in AddBeaconReport
+	// The length is in the FrameReportLen
+
+	// reset Beacon index for next beacon request
+	pAd->StaCfg.LastBssIndex = 0xff;
+
+	// reset to idle state
+	pAd->Mlme.AironetMachine.CurrState = AIRONET_IDLE;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("BeaconReportAction <-----\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+		Index		Current BSSID in CCXBsstab entry index
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID	AironetAddBeaconReport(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	ULONG				Index,
+	IN	PMLME_QUEUE_ELEM	pElem)
+{
+	PVOID						pMsg;
+	PUCHAR						pSrc, pDest;
+	UCHAR						ReqIdx;
+	ULONG						MsgLen;
+	USHORT						Length;
+	PFRAME_802_11				pFrame;
+	PMEASUREMENT_REPORT_ELEMENT	pReport;
+	PEID_STRUCT			        pEid;
+	PBEACON_REPORT				pBeaconReport;
+	PBSS_ENTRY					pBss;
+
+	// 0. Setup pointer for processing beacon & probe response
+	pMsg   = pElem->Msg;
+	MsgLen = pElem->MsgLen;
+	pFrame = (PFRAME_802_11) pMsg;
+	pSrc   = pFrame->Octet;				// Start from AP TSF
+	pBss   = (PBSS_ENTRY) &pAd->StaCfg.CCXBssTab.BssEntry[Index];
+	ReqIdx = pAd->StaCfg.CurrentRMReqIdx;
+
+	// 1 Check the Index, if we already create this entry, only update the average RSSI
+	if ((Index <= pAd->StaCfg.LastBssIndex) && (pAd->StaCfg.LastBssIndex != 0xff))
+	{
+		pDest  = (PUCHAR) &pAd->StaCfg.FrameReportBuf[pAd->StaCfg.BssReportOffset[Index]];
+		// Point to bss report information
+		pDest += sizeof(MEASUREMENT_REPORT_ELEMENT);
+		pBeaconReport = (PBEACON_REPORT) pDest;
+
+		// Update Rx power, in dBm
+		// Get the original RSSI readback from BBP
+		pBeaconReport->RxPower += pAd->BbpRssiToDbmDelta;
+		// Average the Rssi reading
+		pBeaconReport->RxPower  = (pBeaconReport->RxPower + pBss->Rssi) / 2;
+		// Get to dBm format
+		pBeaconReport->RxPower -= pAd->BbpRssiToDbmDelta;
+
+		DBGPRINT(RT_DEBUG_TRACE, ("Bssid %02x:%02x:%02x:%02x:%02x:%02x ",
+			pBss->Bssid[0], pBss->Bssid[1], pBss->Bssid[2],
+			pBss->Bssid[3], pBss->Bssid[4], pBss->Bssid[5]));
+		DBGPRINT(RT_DEBUG_TRACE, ("RxPower[%ld] Rssi %d, Avg Rssi %d\n", Index, (pBss->Rssi - pAd->BbpRssiToDbmDelta), pBeaconReport->RxPower - 256));
+		DBGPRINT(RT_DEBUG_TRACE, ("FrameReportLen = %d\n", pAd->StaCfg.BssReportOffset[Index]));
+
+		// Update other information here
+
+		// Done
+		return;
+	}
+
+	// 2. Update reported Index
+	pAd->StaCfg.LastBssIndex = Index;
+
+	// 3. Setup the buffer address for copying this BSSID into reporting frame
+	//    The offset should start after 802.11 header and report frame header.
+	pDest = (PUCHAR) &pAd->StaCfg.FrameReportBuf[pAd->StaCfg.FrameReportLen];
+
+	// 4. Save the start offset of each Bss in report frame
+	pAd->StaCfg.BssReportOffset[Index] = pAd->StaCfg.FrameReportLen;
+
+	// 5. Fill Measurement report fields
+	pReport = (PMEASUREMENT_REPORT_ELEMENT) pDest;
+	pReport->Eid = IE_MEASUREMENT_REPORT;
+	pReport->Length = 0;
+	pReport->Token  = pAd->StaCfg.MeasurementRequest[ReqIdx].ReqElem.Token;
+	pReport->Mode   = pAd->StaCfg.MeasurementRequest[ReqIdx].ReqElem.Mode;
+	pReport->Type   = MSRN_TYPE_BEACON_REQ;
+	Length          = sizeof(MEASUREMENT_REPORT_ELEMENT);
+	pDest          += sizeof(MEASUREMENT_REPORT_ELEMENT);
+
+	// 6. Start thebeacon report format
+	pBeaconReport = (PBEACON_REPORT) pDest;
+	pDest        += sizeof(BEACON_REPORT);
+	Length       += sizeof(BEACON_REPORT);
+
+	// 7. Copy Channel number
+	pBeaconReport->Channel        = pBss->Channel;
+	pBeaconReport->Spare          = 0;
+	pBeaconReport->Duration       = pAd->StaCfg.MeasurementRequest[ReqIdx].Measurement.Duration;
+	pBeaconReport->PhyType        = ((pBss->SupRateLen+pBss->ExtRateLen > 4) ? PHY_ERP : PHY_DSS);
+	// 8. Rx power, in dBm
+	pBeaconReport->RxPower        = pBss->Rssi - pAd->BbpRssiToDbmDelta;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Bssid %02x:%02x:%02x:%02x:%02x:%02x ",
+		pBss->Bssid[0], pBss->Bssid[1], pBss->Bssid[2],
+		pBss->Bssid[3], pBss->Bssid[4], pBss->Bssid[5]));
+	DBGPRINT(RT_DEBUG_TRACE, ("RxPower[%ld], Rssi %d\n", Index, pBeaconReport->RxPower - 256));
+	DBGPRINT(RT_DEBUG_TRACE, ("FrameReportLen = %d\n", pAd->StaCfg.FrameReportLen));
+
+	pBeaconReport->BeaconInterval = pBss->BeaconPeriod;
+	COPY_MAC_ADDR(pBeaconReport->BSSID, pFrame->Hdr.Addr3);
+	NdisMoveMemory(pBeaconReport->ParentTSF, pSrc, 4);
+	NdisMoveMemory(pBeaconReport->TargetTSF, &pElem->TimeStamp.u.LowPart, 4);
+	NdisMoveMemory(&pBeaconReport->TargetTSF[4], &pElem->TimeStamp.u.HighPart, 4);
+
+	// 9. Skip the beacon frame and offset to start of capabilityinfo since we already processed capabilityinfo
+	pSrc += (TIMESTAMP_LEN + 2);
+	pBeaconReport->CapabilityInfo = *(USHORT *)pSrc;
+
+	// 10. Point to start of element ID
+	pSrc += 2;
+	pEid = (PEID_STRUCT) pSrc;
+
+	// 11. Start process all variable Eid oayload and add the appropriate to the frame report
+	while (((PUCHAR) pEid + pEid->Len + 1) < ((PUCHAR) pFrame + MsgLen))
+	{
+		// Only limited EID are required to report for CCX 2. It includes SSID, Supported rate,
+		// FH paramenter set, DS parameter set, CF parameter set, IBSS parameter set,
+		// TIM (report first 4 bytes only, radio measurement capability
+		switch (pEid->Eid)
+		{
+			case IE_SSID:
+			case IE_SUPP_RATES:
+			case IE_FH_PARM:
+			case IE_DS_PARM:
+			case IE_CF_PARM:
+			case IE_IBSS_PARM:
+				NdisMoveMemory(pDest, pEid, pEid->Len + 2);
+				pDest  += (pEid->Len + 2);
+				Length += (pEid->Len + 2);
+				break;
+
+			case IE_MEASUREMENT_CAPABILITY:
+				// Since this IE is duplicated with WPA security IE, we has to do sanity check before
+				// recognize it.
+				// 1. It also has fixed 6 bytes IE length.
+				if (pEid->Len != 6)
+					break;
+				// 2. Check the Cisco Aironet OUI
+				if (NdisEqualMemory(CISCO_OUI, (pSrc + 2), 3))
+				{
+					// Matched, this is what we want
+					NdisMoveMemory(pDest, pEid, pEid->Len + 2);
+					pDest  += (pEid->Len + 2);
+					Length += (pEid->Len + 2);
+				}
+				break;
+
+			case IE_TIM:
+				if (pEid->Len > 4)
+				{
+					// May truncate and report the first 4 bytes only, with the eid & len, total should be 6
+					NdisMoveMemory(pDest, pEid, 6);
+					pDest  += 6;
+					Length += 6;
+				}
+				else
+				{
+					NdisMoveMemory(pDest, pEid, pEid->Len + 2);
+					pDest  += (pEid->Len + 2);
+					Length += (pEid->Len + 2);
+				}
+				break;
+
+			default:
+				break;
+		}
+		// 12. Move to next element ID
+		pSrc += (2 + pEid->Len);
+		pEid = (PEID_STRUCT) pSrc;
+	}
+
+	// 13. Update the length in the header, not include EID and length
+	pReport->Length = Length - 4;
+
+	// 14. Update the frame report buffer data length
+	pAd->StaCfg.FrameReportLen += Length;
+	DBGPRINT(RT_DEBUG_TRACE, ("FR len = %d\n", pAd->StaCfg.FrameReportLen));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+		Index		Current BSSID in CCXBsstab entry index
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID	AironetCreateBeaconReportFromBssTable(
+	IN	PRTMP_ADAPTER		pAd)
+{
+	PMEASUREMENT_REPORT_ELEMENT	pReport;
+	PBEACON_REPORT				pBeaconReport;
+	UCHAR						Index, ReqIdx;
+	USHORT						Length;
+	PUCHAR						pDest;
+	PBSS_ENTRY					pBss;
+
+	// 0. setup base pointer
+	ReqIdx = pAd->StaCfg.CurrentRMReqIdx;
+
+	for (Index = 0; Index < pAd->StaCfg.CCXBssTab.BssNr; Index++)
+	{
+		// 1. Setup the buffer address for copying this BSSID into reporting frame
+		//    The offset should start after 802.11 header and report frame header.
+		pDest  = (PUCHAR) &pAd->StaCfg.FrameReportBuf[pAd->StaCfg.FrameReportLen];
+		pBss   = (PBSS_ENTRY) &pAd->StaCfg.CCXBssTab.BssEntry[Index];
+		Length = 0;
+
+		// 2. Fill Measurement report fields
+		pReport         = (PMEASUREMENT_REPORT_ELEMENT) pDest;
+		pReport->Eid    = IE_MEASUREMENT_REPORT;
+		pReport->Length = 0;
+		pReport->Token  = pAd->StaCfg.MeasurementRequest[ReqIdx].ReqElem.Token;
+		pReport->Mode   = pAd->StaCfg.MeasurementRequest[ReqIdx].ReqElem.Mode;
+		pReport->Type   = MSRN_TYPE_BEACON_REQ;
+		Length          = sizeof(MEASUREMENT_REPORT_ELEMENT);
+		pDest          += sizeof(MEASUREMENT_REPORT_ELEMENT);
+
+		// 3. Start the beacon report format
+		pBeaconReport = (PBEACON_REPORT) pDest;
+		pDest        += sizeof(BEACON_REPORT);
+		Length       += sizeof(BEACON_REPORT);
+
+		// 4. Copy Channel number
+		pBeaconReport->Channel        = pBss->Channel;
+		pBeaconReport->Spare          = 0;
+		pBeaconReport->Duration       = pAd->StaCfg.MeasurementRequest[ReqIdx].Measurement.Duration;
+		pBeaconReport->PhyType        = ((pBss->SupRateLen+pBss->ExtRateLen > 4) ? PHY_ERP : PHY_DSS);
+		pBeaconReport->RxPower        = pBss->Rssi - pAd->BbpRssiToDbmDelta;
+		pBeaconReport->BeaconInterval = pBss->BeaconPeriod;
+		pBeaconReport->CapabilityInfo = pBss->CapabilityInfo;
+		COPY_MAC_ADDR(pBeaconReport->BSSID, pBss->Bssid);
+		NdisMoveMemory(pBeaconReport->ParentTSF, pBss->PTSF, 4);
+		NdisMoveMemory(pBeaconReport->TargetTSF, pBss->TTSF, 8);
+
+		// 5. Create SSID
+		*pDest++ = 0x00;
+		*pDest++ = pBss->SsidLen;
+		NdisMoveMemory(pDest, pBss->Ssid, pBss->SsidLen);
+		pDest  += pBss->SsidLen;
+		Length += (2 + pBss->SsidLen);
+
+		// 6. Create SupportRates
+		*pDest++ = 0x01;
+		*pDest++ = pBss->SupRateLen;
+		NdisMoveMemory(pDest, pBss->SupRate, pBss->SupRateLen);
+		pDest  += pBss->SupRateLen;
+		Length += (2 + pBss->SupRateLen);
+
+		// 7. DS Parameter
+		*pDest++ = 0x03;
+		*pDest++ = 1;
+		*pDest++ = pBss->Channel;
+		Length  += 3;
+
+		// 8. IBSS parameter if presents
+		if (pBss->BssType == BSS_ADHOC)
+		{
+			*pDest++ = 0x06;
+			*pDest++ = 2;
+			*(PUSHORT) pDest = pBss->AtimWin;
+			pDest   += 2;
+			Length  += 4;
+		}
+
+		// 9. Update length field, not include EID and length
+		pReport->Length = Length - 4;
+
+		// 10. Update total frame size
+		pAd->StaCfg.FrameReportLen += Length;
+	}
+}
diff --git a/drivers/staging/rt2860/sta/assoc.c b/drivers/staging/rt2860/sta/assoc.c
new file mode 100644
index 00000000000..42db753eed7
--- /dev/null
+++ b/drivers/staging/rt2860/sta/assoc.c
@@ -0,0 +1,1826 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	assoc.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John		2004-9-3		porting from RT2500
+*/
+#include "../rt_config.h"
+
+UCHAR	CipherWpaTemplate[] = {
+		0xdd, 					// WPA IE
+		0x16,					// Length
+		0x00, 0x50, 0xf2, 0x01,	// oui
+		0x01, 0x00,				// Version
+		0x00, 0x50, 0xf2, 0x02,	// Multicast
+		0x01, 0x00,				// Number of unicast
+		0x00, 0x50, 0xf2, 0x02,	// unicast
+		0x01, 0x00,				// number of authentication method
+		0x00, 0x50, 0xf2, 0x01	// authentication
+		};
+
+UCHAR	CipherWpa2Template[] = {
+		0x30,					// RSN IE
+		0x14,					// Length
+		0x01, 0x00,				// Version
+		0x00, 0x0f, 0xac, 0x02,	// group cipher, TKIP
+		0x01, 0x00,				// number of pairwise
+		0x00, 0x0f, 0xac, 0x02,	// unicast
+		0x01, 0x00,				// number of authentication method
+		0x00, 0x0f, 0xac, 0x02,	// authentication
+		0x00, 0x00,				// RSN capability
+		};
+
+UCHAR	Ccx2IeInfo[] = { 0x00, 0x40, 0x96, 0x03, 0x02};
+
+/*
+	==========================================================================
+	Description:
+		association state machine init, including state transition and timer init
+	Parameters:
+		S - pointer to the association state machine
+
+	IRQL = PASSIVE_LEVEL
+
+	==========================================================================
+ */
+VOID AssocStateMachineInit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN  STATE_MACHINE *S,
+	OUT STATE_MACHINE_FUNC Trans[])
+{
+	StateMachineInit(S, Trans, MAX_ASSOC_STATE, MAX_ASSOC_MSG, (STATE_MACHINE_FUNC)Drop, ASSOC_IDLE, ASSOC_MACHINE_BASE);
+
+	// first column
+	StateMachineSetAction(S, ASSOC_IDLE, MT2_MLME_ASSOC_REQ, (STATE_MACHINE_FUNC)MlmeAssocReqAction);
+	StateMachineSetAction(S, ASSOC_IDLE, MT2_MLME_REASSOC_REQ, (STATE_MACHINE_FUNC)MlmeReassocReqAction);
+	StateMachineSetAction(S, ASSOC_IDLE, MT2_MLME_DISASSOC_REQ, (STATE_MACHINE_FUNC)MlmeDisassocReqAction);
+	StateMachineSetAction(S, ASSOC_IDLE, MT2_PEER_DISASSOC_REQ, (STATE_MACHINE_FUNC)PeerDisassocAction);
+
+	// second column
+	StateMachineSetAction(S, ASSOC_WAIT_RSP, MT2_MLME_ASSOC_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenAssoc);
+	StateMachineSetAction(S, ASSOC_WAIT_RSP, MT2_MLME_REASSOC_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenReassoc);
+	StateMachineSetAction(S, ASSOC_WAIT_RSP, MT2_MLME_DISASSOC_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenDisassociate);
+	StateMachineSetAction(S, ASSOC_WAIT_RSP, MT2_PEER_DISASSOC_REQ, (STATE_MACHINE_FUNC)PeerDisassocAction);
+	StateMachineSetAction(S, ASSOC_WAIT_RSP, MT2_PEER_ASSOC_RSP, (STATE_MACHINE_FUNC)PeerAssocRspAction);
+	//
+	// Patch 3Com AP MOde:3CRWE454G72
+	// We send Assoc request frame to this AP, it always send Reassoc Rsp not Associate Rsp.
+	//
+	StateMachineSetAction(S, ASSOC_WAIT_RSP, MT2_PEER_REASSOC_RSP, (STATE_MACHINE_FUNC)PeerAssocRspAction);
+	StateMachineSetAction(S, ASSOC_WAIT_RSP, MT2_ASSOC_TIMEOUT, (STATE_MACHINE_FUNC)AssocTimeoutAction);
+
+	// third column
+	StateMachineSetAction(S, REASSOC_WAIT_RSP, MT2_MLME_ASSOC_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenAssoc);
+	StateMachineSetAction(S, REASSOC_WAIT_RSP, MT2_MLME_REASSOC_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenReassoc);
+	StateMachineSetAction(S, REASSOC_WAIT_RSP, MT2_MLME_DISASSOC_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenDisassociate);
+	StateMachineSetAction(S, REASSOC_WAIT_RSP, MT2_PEER_DISASSOC_REQ, (STATE_MACHINE_FUNC)PeerDisassocAction);
+	StateMachineSetAction(S, REASSOC_WAIT_RSP, MT2_PEER_REASSOC_RSP, (STATE_MACHINE_FUNC)PeerReassocRspAction);
+	//
+	// Patch, AP doesn't send Reassociate Rsp frame to Station.
+	//
+	StateMachineSetAction(S, REASSOC_WAIT_RSP, MT2_PEER_ASSOC_RSP, (STATE_MACHINE_FUNC)PeerReassocRspAction);
+	StateMachineSetAction(S, REASSOC_WAIT_RSP, MT2_REASSOC_TIMEOUT, (STATE_MACHINE_FUNC)ReassocTimeoutAction);
+
+	// fourth column
+	StateMachineSetAction(S, DISASSOC_WAIT_RSP, MT2_MLME_ASSOC_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenAssoc);
+	StateMachineSetAction(S, DISASSOC_WAIT_RSP, MT2_MLME_REASSOC_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenReassoc);
+	StateMachineSetAction(S, DISASSOC_WAIT_RSP, MT2_MLME_DISASSOC_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenDisassociate);
+	StateMachineSetAction(S, DISASSOC_WAIT_RSP, MT2_PEER_DISASSOC_REQ, (STATE_MACHINE_FUNC)PeerDisassocAction);
+	StateMachineSetAction(S, DISASSOC_WAIT_RSP, MT2_DISASSOC_TIMEOUT, (STATE_MACHINE_FUNC)DisassocTimeoutAction);
+
+	// initialize the timer
+	RTMPInitTimer(pAd, &pAd->MlmeAux.AssocTimer, GET_TIMER_FUNCTION(AssocTimeout), pAd, FALSE);
+	RTMPInitTimer(pAd, &pAd->MlmeAux.ReassocTimer, GET_TIMER_FUNCTION(ReassocTimeout), pAd, FALSE);
+	RTMPInitTimer(pAd, &pAd->MlmeAux.DisassocTimer, GET_TIMER_FUNCTION(DisassocTimeout), pAd, FALSE);
+}
+
+/*
+	==========================================================================
+	Description:
+		Association timeout procedure. After association timeout, this function
+		will be called and it will put a message into the MLME queue
+	Parameters:
+		Standard timer parameters
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AssocTimeout(IN PVOID SystemSpecific1,
+				 IN PVOID FunctionContext,
+				 IN PVOID SystemSpecific2,
+				 IN PVOID SystemSpecific3)
+{
+	RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return;
+
+	MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_ASSOC_TIMEOUT, 0, NULL);
+	RT28XX_MLME_HANDLER(pAd);
+}
+
+/*
+	==========================================================================
+	Description:
+		Reassociation timeout procedure. After reassociation timeout, this
+		function will be called and put a message into the MLME queue
+	Parameters:
+		Standard timer parameters
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID ReassocTimeout(IN PVOID SystemSpecific1,
+					IN PVOID FunctionContext,
+					IN PVOID SystemSpecific2,
+					IN PVOID SystemSpecific3)
+{
+	RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return;
+
+	MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_REASSOC_TIMEOUT, 0, NULL);
+	RT28XX_MLME_HANDLER(pAd);
+}
+
+/*
+	==========================================================================
+	Description:
+		Disassociation timeout procedure. After disassociation timeout, this
+		function will be called and put a message into the MLME queue
+	Parameters:
+		Standard timer parameters
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID DisassocTimeout(IN PVOID SystemSpecific1,
+					IN PVOID FunctionContext,
+					IN PVOID SystemSpecific2,
+					IN PVOID SystemSpecific3)
+{
+	RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return;
+
+	MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_DISASSOC_TIMEOUT, 0, NULL);
+	RT28XX_MLME_HANDLER(pAd);
+}
+
+/*
+	==========================================================================
+	Description:
+		mlme assoc req handling procedure
+	Parameters:
+		Adapter - Adapter pointer
+		Elem - MLME Queue Element
+	Pre:
+		the station has been authenticated and the following information is stored in the config
+			-# SSID
+			-# supported rates and their length
+			-# listen interval (Adapter->StaCfg.default_listen_count)
+			-# Transmit power  (Adapter->StaCfg.tx_power)
+	Post  :
+		-# An association request frame is generated and sent to the air
+		-# Association timer starts
+		-# Association state -> ASSOC_WAIT_RSP
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID MlmeAssocReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR			ApAddr[6];
+	HEADER_802_11	AssocHdr;
+	UCHAR			Ccx2Len = 5;
+	UCHAR			WmeIe[9] = {IE_VENDOR_SPECIFIC, 0x07, 0x00, 0x50, 0xf2, 0x02, 0x00, 0x01, 0x00};
+	USHORT			ListenIntv;
+	ULONG			Timeout;
+	USHORT			CapabilityInfo;
+	BOOLEAN			TimerCancelled;
+	PUCHAR			pOutBuffer = NULL;
+	NDIS_STATUS		NStatus;
+	ULONG			FrameLen = 0;
+	ULONG			tmp;
+	USHORT			VarIesOffset;
+	UCHAR			CkipFlag;
+	UCHAR			CkipNegotiationBuffer[CKIP_NEGOTIATION_LENGTH];
+	UCHAR			AironetCkipIe = IE_AIRONET_CKIP;
+	UCHAR			AironetCkipLen = CKIP_NEGOTIATION_LENGTH;
+	UCHAR			AironetIPAddressIE = IE_AIRONET_IPADDRESS;
+	UCHAR			AironetIPAddressLen = AIRONET_IPADDRESS_LENGTH;
+	UCHAR			AironetIPAddressBuffer[AIRONET_IPADDRESS_LENGTH] = {0x00, 0x40, 0x96, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00};
+	USHORT			Status;
+
+	// Block all authentication request durning WPA block period
+	if (pAd->StaCfg.bBlockAssoc == TRUE)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - Block Assoc request durning WPA block period!\n"));
+		pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+		Status = MLME_STATE_MACHINE_REJECT;
+		MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_ASSOC_CONF, 2, &Status);
+	}
+	// check sanity first
+	else if (MlmeAssocReqSanity(pAd, Elem->Msg, Elem->MsgLen, ApAddr, &CapabilityInfo, &Timeout, &ListenIntv))
+	{
+		RTMPCancelTimer(&pAd->MlmeAux.AssocTimer, &TimerCancelled);
+		COPY_MAC_ADDR(pAd->MlmeAux.Bssid, ApAddr);
+
+		// Get an unused nonpaged memory
+		NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);
+		if (NStatus != NDIS_STATUS_SUCCESS)
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("ASSOC - MlmeAssocReqAction() allocate memory failed \n"));
+			pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+			Status = MLME_FAIL_NO_RESOURCE;
+			MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_ASSOC_CONF, 2, &Status);
+			return;
+		}
+
+		// Add by James 03/06/27
+		pAd->StaCfg.AssocInfo.Length = sizeof(NDIS_802_11_ASSOCIATION_INFORMATION);
+		// Association don't need to report MAC address
+		pAd->StaCfg.AssocInfo.AvailableRequestFixedIEs =
+			NDIS_802_11_AI_REQFI_CAPABILITIES | NDIS_802_11_AI_REQFI_LISTENINTERVAL;
+		pAd->StaCfg.AssocInfo.RequestFixedIEs.Capabilities = CapabilityInfo;
+		pAd->StaCfg.AssocInfo.RequestFixedIEs.ListenInterval = ListenIntv;
+		// Only reassociate need this
+		//COPY_MAC_ADDR(pAd->StaCfg.AssocInfo.RequestFixedIEs.CurrentAPAddress, ApAddr);
+		pAd->StaCfg.AssocInfo.OffsetRequestIEs = sizeof(NDIS_802_11_ASSOCIATION_INFORMATION);
+
+        NdisZeroMemory(pAd->StaCfg.ReqVarIEs, MAX_VIE_LEN);
+		// First add SSID
+		VarIesOffset = 0;
+		NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, &SsidIe, 1);
+		VarIesOffset += 1;
+		NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, &pAd->MlmeAux.SsidLen, 1);
+		VarIesOffset += 1;
+		NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen);
+		VarIesOffset += pAd->MlmeAux.SsidLen;
+
+		// Second add Supported rates
+		NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, &SupRateIe, 1);
+		VarIesOffset += 1;
+		NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, &pAd->MlmeAux.SupRateLen, 1);
+		VarIesOffset += 1;
+		NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, pAd->MlmeAux.SupRate, pAd->MlmeAux.SupRateLen);
+		VarIesOffset += pAd->MlmeAux.SupRateLen;
+		// End Add by James
+
+        if ((pAd->CommonCfg.Channel > 14) &&
+            (pAd->CommonCfg.bIEEE80211H == TRUE))
+            CapabilityInfo |= 0x0100;
+
+		DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - Send ASSOC request...\n"));
+		MgtMacHeaderInit(pAd, &AssocHdr, SUBTYPE_ASSOC_REQ, 0, ApAddr, ApAddr);
+
+		// Build basic frame first
+		MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+						  sizeof(HEADER_802_11),	&AssocHdr,
+						  2,						&CapabilityInfo,
+						  2,						&ListenIntv,
+						  1,						&SsidIe,
+						  1,						&pAd->MlmeAux.SsidLen,
+						  pAd->MlmeAux.SsidLen, 	pAd->MlmeAux.Ssid,
+						  1,						&SupRateIe,
+						  1,						&pAd->MlmeAux.SupRateLen,
+						  pAd->MlmeAux.SupRateLen,  pAd->MlmeAux.SupRate,
+						  END_OF_ARGS);
+
+		if (pAd->MlmeAux.ExtRateLen != 0)
+		{
+			MakeOutgoingFrame(pOutBuffer + FrameLen,    &tmp,
+							  1,                        &ExtRateIe,
+							  1,                        &pAd->MlmeAux.ExtRateLen,
+							  pAd->MlmeAux.ExtRateLen,  pAd->MlmeAux.ExtRate,
+							  END_OF_ARGS);
+			FrameLen += tmp;
+		}
+
+#ifdef DOT11_N_SUPPORT
+		// HT
+		if ((pAd->MlmeAux.HtCapabilityLen > 0) && (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+		{
+			ULONG TmpLen;
+			UCHAR HtLen;
+			UCHAR BROADCOM[4] = {0x0, 0x90, 0x4c, 0x33};
+			if (pAd->StaActive.SupportedPhyInfo.bPreNHt == TRUE)
+			{
+				HtLen = SIZE_HT_CAP_IE + 4;
+				MakeOutgoingFrame(pOutBuffer + FrameLen,            &TmpLen,
+							  1,                                &WpaIe,
+							  1,                                &HtLen,
+							  4,                                &BROADCOM[0],
+							 pAd->MlmeAux.HtCapabilityLen,          &pAd->MlmeAux.HtCapability,
+							  END_OF_ARGS);
+			}
+			else
+			{
+#ifdef RT_BIG_ENDIAN
+		        HT_CAPABILITY_IE HtCapabilityTmp;
+#endif
+
+#ifndef RT_BIG_ENDIAN
+				MakeOutgoingFrame(pOutBuffer + FrameLen,            &TmpLen,
+							  1,                                &HtCapIe,
+							  1,                                &pAd->MlmeAux.HtCapabilityLen,
+							 pAd->MlmeAux.HtCapabilityLen,          &pAd->MlmeAux.HtCapability,
+							  END_OF_ARGS);
+#else
+                NdisZeroMemory(&HtCapabilityTmp, sizeof(HT_CAPABILITY_IE));
+                NdisMoveMemory(&HtCapabilityTmp, &pAd->MlmeAux.HtCapability, pAd->MlmeAux.HtCapabilityLen);
+        		*(USHORT *)(&HtCapabilityTmp.HtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.HtCapInfo));
+        		*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo));
+
+        		MakeOutgoingFrame(pOutBuffer + FrameLen,         &TmpLen,
+        							1,                           &HtCapIe,
+        							1,                           &pAd->MlmeAux.HtCapabilityLen,
+        							pAd->MlmeAux.HtCapabilityLen,&HtCapabilityTmp,
+        							END_OF_ARGS);
+#endif
+			}
+			FrameLen += TmpLen;
+		}
+#endif // DOT11_N_SUPPORT //
+
+		// add Ralink proprietary IE to inform AP this STA is going to use AGGREGATION or PIGGY-BACK+AGGREGATION
+		// Case I: (Aggregation + Piggy-Back)
+		// 1. user enable aggregation, AND
+		// 2. Mac support piggy-back
+		// 3. AP annouces it's PIGGY-BACK+AGGREGATION-capable in BEACON
+		// Case II: (Aggregation)
+		// 1. user enable aggregation, AND
+		// 2. AP annouces it's AGGREGATION-capable in BEACON
+		if (pAd->CommonCfg.bAggregationCapable)
+		{
+			if ((pAd->CommonCfg.bPiggyBackCapable) && ((pAd->MlmeAux.APRalinkIe & 0x00000003) == 3))
+			{
+				ULONG TmpLen;
+				UCHAR RalinkIe[9] = {IE_VENDOR_SPECIFIC, 7, 0x00, 0x0c, 0x43, 0x03, 0x00, 0x00, 0x00};
+				MakeOutgoingFrame(pOutBuffer+FrameLen,           &TmpLen,
+								  9,                             RalinkIe,
+								  END_OF_ARGS);
+				FrameLen += TmpLen;
+			}
+			else if (pAd->MlmeAux.APRalinkIe & 0x00000001)
+			{
+				ULONG TmpLen;
+				UCHAR RalinkIe[9] = {IE_VENDOR_SPECIFIC, 7, 0x00, 0x0c, 0x43, 0x01, 0x00, 0x00, 0x00};
+				MakeOutgoingFrame(pOutBuffer+FrameLen,           &TmpLen,
+								  9,                             RalinkIe,
+								  END_OF_ARGS);
+				FrameLen += TmpLen;
+			}
+		}
+		else
+		{
+			ULONG TmpLen;
+			UCHAR RalinkIe[9] = {IE_VENDOR_SPECIFIC, 7, 0x00, 0x0c, 0x43, 0x06, 0x00, 0x00, 0x00};
+			MakeOutgoingFrame(pOutBuffer+FrameLen,		 &TmpLen,
+							  9,						 RalinkIe,
+							  END_OF_ARGS);
+			FrameLen += TmpLen;
+		}
+
+		if (pAd->MlmeAux.APEdcaParm.bValid)
+		{
+			if (pAd->CommonCfg.bAPSDCapable && pAd->MlmeAux.APEdcaParm.bAPSDCapable)
+			{
+				QBSS_STA_INFO_PARM QosInfo;
+
+				NdisZeroMemory(&QosInfo, sizeof(QBSS_STA_INFO_PARM));
+				QosInfo.UAPSD_AC_BE = pAd->CommonCfg.bAPSDAC_BE;
+				QosInfo.UAPSD_AC_BK = pAd->CommonCfg.bAPSDAC_BK;
+				QosInfo.UAPSD_AC_VI = pAd->CommonCfg.bAPSDAC_VI;
+				QosInfo.UAPSD_AC_VO = pAd->CommonCfg.bAPSDAC_VO;
+				QosInfo.MaxSPLength = pAd->CommonCfg.MaxSPLength;
+				WmeIe[8] |= *(PUCHAR)&QosInfo;
+			}
+			else
+			{
+                // The Parameter Set Count is set to ¡§0¡¨ in the association request frames
+                // WmeIe[8] |= (pAd->MlmeAux.APEdcaParm.EdcaUpdateCount & 0x0f);
+			}
+
+			MakeOutgoingFrame(pOutBuffer + FrameLen,    &tmp,
+							  9,                        &WmeIe[0],
+							  END_OF_ARGS);
+			FrameLen += tmp;
+		}
+
+		//
+		// Let WPA(#221) Element ID on the end of this association frame.
+		// Otherwise some AP will fail on parsing Element ID and set status fail on Assoc Rsp.
+		// For example: Put Vendor Specific IE on the front of WPA IE.
+		// This happens on AP (Model No:Linksys WRK54G)
+		//
+		if (((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+            (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK) ||
+            (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
+            (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2)
+			)
+            )
+		{
+			UCHAR RSNIe = IE_WPA;
+
+			if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK) ||
+                (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2))
+			{
+				RSNIe = IE_WPA2;
+			}
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+#ifdef SIOCSIWGENIE
+			if (pAd->StaCfg.WpaSupplicantUP != 1)
+#endif // SIOCSIWGENIE //
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+            	RTMPMakeRSNIE(pAd, pAd->StaCfg.AuthMode, pAd->StaCfg.WepStatus, BSS0);
+
+            // Check for WPA PMK cache list
+			if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2)
+			{
+			    INT     idx;
+                BOOLEAN FoundPMK = FALSE;
+				// Search chched PMKID, append it if existed
+				for (idx = 0; idx < PMKID_NO; idx++)
+				{
+					if (NdisEqualMemory(ApAddr, &pAd->StaCfg.SavedPMK[idx].BSSID, 6))
+					{
+						FoundPMK = TRUE;
+						break;
+					}
+				}
+
+				if (FoundPMK)
+				{
+					// Set PMK number
+					*(PUSHORT) &pAd->StaCfg.RSN_IE[pAd->StaCfg.RSNIE_Len] = 1;
+					NdisMoveMemory(&pAd->StaCfg.RSN_IE[pAd->StaCfg.RSNIE_Len + 2], &pAd->StaCfg.SavedPMK[idx].PMKID, 16);
+                    pAd->StaCfg.RSNIE_Len += 18;
+				}
+			}
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+#ifdef SIOCSIWGENIE
+			if (pAd->StaCfg.WpaSupplicantUP == 1)
+			{
+				MakeOutgoingFrame(pOutBuffer + FrameLen,    		&tmp,
+		                        	pAd->StaCfg.RSNIE_Len,			pAd->StaCfg.RSN_IE,
+		                        	END_OF_ARGS);
+			}
+			else
+#endif
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+			{
+				MakeOutgoingFrame(pOutBuffer + FrameLen,    		&tmp,
+				              		1,                              &RSNIe,
+		                        	1,                              &pAd->StaCfg.RSNIE_Len,
+		                        	pAd->StaCfg.RSNIE_Len,			pAd->StaCfg.RSN_IE,
+		                        	END_OF_ARGS);
+			}
+
+			FrameLen += tmp;
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+#ifdef SIOCSIWGENIE
+			if (pAd->StaCfg.WpaSupplicantUP != 1)
+#endif
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+			{
+	            // Append Variable IE
+	            NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, &RSNIe, 1);
+	            VarIesOffset += 1;
+	            NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, &pAd->StaCfg.RSNIE_Len, 1);
+	            VarIesOffset += 1;
+			}
+			NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, pAd->StaCfg.RSN_IE, pAd->StaCfg.RSNIE_Len);
+			VarIesOffset += pAd->StaCfg.RSNIE_Len;
+
+			// Set Variable IEs Length
+			pAd->StaCfg.ReqVarIELen = VarIesOffset;
+		}
+
+		// We have update that at PeerBeaconAtJoinRequest()
+		CkipFlag = pAd->StaCfg.CkipFlag;
+		if (CkipFlag != 0)
+		{
+			NdisZeroMemory(CkipNegotiationBuffer, CKIP_NEGOTIATION_LENGTH);
+			CkipNegotiationBuffer[2] = 0x66;
+			// Make it try KP & MIC, since we have to follow the result from AssocRsp
+			CkipNegotiationBuffer[8] = 0x18;
+			CkipNegotiationBuffer[CKIP_NEGOTIATION_LENGTH - 1] = 0x22;
+			CkipFlag = 0x18;
+
+			MakeOutgoingFrame(pOutBuffer + FrameLen, 	&tmp,
+						1,						  		&AironetCkipIe,
+						1,						  		&AironetCkipLen,
+						AironetCkipLen, 		  		CkipNegotiationBuffer,
+						END_OF_ARGS);
+			FrameLen += tmp;
+		}
+
+		// Add CCX v2 request if CCX2 admin state is on
+		if (pAd->StaCfg.CCXControl.field.Enable == 1)
+		{
+
+			//
+			// Add AironetIPAddressIE for Cisco CCX 2.X
+			// Add CCX Version
+			//
+			MakeOutgoingFrame(pOutBuffer + FrameLen, &tmp,
+						1,							&AironetIPAddressIE,
+						1,							&AironetIPAddressLen,
+						AironetIPAddressLen,		AironetIPAddressBuffer,
+						1,							&Ccx2Ie,
+						1,							&Ccx2Len,
+						Ccx2Len,				    Ccx2IeInfo,
+						END_OF_ARGS);
+			FrameLen += tmp;
+
+			//
+			// Add CipherSuite CCKM or LeapTkip if setting.
+			//
+#ifdef LEAP_SUPPORT
+			if (LEAP_CCKM_ON(pAd))
+			{
+				MakeOutgoingFrame(pOutBuffer + FrameLen,	&tmp,
+						CipherSuiteCiscoCCKMLen,		CipherSuiteCiscoCCKM,
+						END_OF_ARGS);
+				FrameLen += tmp;
+
+				// Third add RSN
+				NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, CipherSuiteCiscoCCKM, CipherSuiteCiscoCCKMLen); //Save CipherSuite
+				VarIesOffset += CipherSuiteCiscoCCKMLen;
+			}
+			else if ((pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP) && (pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled))
+			{
+				MakeOutgoingFrame(pOutBuffer + FrameLen, &tmp,
+						CipherSuiteCCXTkipLen,	    CipherSuiteCCXTkip,
+						END_OF_ARGS);
+				FrameLen += tmp;
+
+				// Third add RSN
+				NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, CipherSuiteCCXTkip, CipherSuiteCCXTkipLen);
+				VarIesOffset += CipherSuiteCCXTkipLen;
+			}
+#endif // LEAP_SUPPORT //
+
+			// Add by James 03/06/27
+			// Set Variable IEs Length
+			pAd->StaCfg.ReqVarIELen = VarIesOffset;
+			pAd->StaCfg.AssocInfo.RequestIELength = VarIesOffset;
+
+			// OffsetResponseIEs follow ReqVarIE
+			pAd->StaCfg.AssocInfo.OffsetResponseIEs = sizeof(NDIS_802_11_ASSOCIATION_INFORMATION) + pAd->StaCfg.ReqVarIELen;
+			// End Add by James
+		}
+
+
+		MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+		MlmeFreeMemory(pAd, pOutBuffer);
+
+		RTMPSetTimer(&pAd->MlmeAux.AssocTimer, Timeout);
+		pAd->Mlme.AssocMachine.CurrState = ASSOC_WAIT_RSP;
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("ASSOC - MlmeAssocReqAction() sanity check failed. BUG!!!!!! \n"));
+		pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+		Status = MLME_INVALID_FORMAT;
+		MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_ASSOC_CONF, 2, &Status);
+	}
+
+}
+
+/*
+	==========================================================================
+	Description:
+		mlme reassoc req handling procedure
+	Parameters:
+		Elem -
+	Pre:
+		-# SSID  (Adapter->StaCfg.ssid[])
+		-# BSSID (AP address, Adapter->StaCfg.bssid)
+		-# Supported rates (Adapter->StaCfg.supported_rates[])
+		-# Supported rates length (Adapter->StaCfg.supported_rates_len)
+		-# Tx power (Adapter->StaCfg.tx_power)
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID MlmeReassocReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR			ApAddr[6];
+	HEADER_802_11	ReassocHdr;
+	UCHAR			Ccx2Len = 5;
+	UCHAR			WmeIe[9] = {IE_VENDOR_SPECIFIC, 0x07, 0x00, 0x50, 0xf2, 0x02, 0x00, 0x01, 0x00};
+	USHORT			CapabilityInfo, ListenIntv;
+	ULONG			Timeout;
+	ULONG			FrameLen = 0;
+	BOOLEAN			TimerCancelled;
+	NDIS_STATUS		NStatus;
+	ULONG			tmp;
+	PUCHAR			pOutBuffer = NULL;
+//CCX 2.X
+#ifdef LEAP_SUPPORT
+	UCHAR			CkipFlag;
+	UCHAR			CkipNegotiationBuffer[CKIP_NEGOTIATION_LENGTH];
+	UCHAR			AironetCkipIe = IE_AIRONET_CKIP;
+	UCHAR			AironetCkipLen = CKIP_NEGOTIATION_LENGTH;
+	UCHAR			AironetIPAddressIE = IE_AIRONET_IPADDRESS;
+	UCHAR			AironetIPAddressLen = AIRONET_IPADDRESS_LENGTH;
+	UCHAR			AironetIPAddressBuffer[AIRONET_IPADDRESS_LENGTH] = {0x00, 0x40, 0x96, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00};
+	UCHAR			AironetCCKMReassocIE = IE_AIRONET_CCKMREASSOC;
+	UCHAR			AironetCCKMReassocLen = AIRONET_CCKMREASSOC_LENGTH;
+	UCHAR			AironetCCKMReassocBuffer[AIRONET_CCKMREASSOC_LENGTH];
+	UCHAR			AironetOUI[] = {0x00, 0x40, 0x96, 0x00};
+	UCHAR			MICMN[16];
+	UCHAR			CalcMicBuffer[80];
+	ULONG			CalcMicBufferLen = 0;
+#endif // LEAP_SUPPORT //
+	USHORT			Status;
+
+	// Block all authentication request durning WPA block period
+	if (pAd->StaCfg.bBlockAssoc == TRUE)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - Block ReAssoc request durning WPA block period!\n"));
+		pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+		Status = MLME_STATE_MACHINE_REJECT;
+		MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_REASSOC_CONF, 2, &Status);
+	}
+	// the parameters are the same as the association
+	else if(MlmeAssocReqSanity(pAd, Elem->Msg, Elem->MsgLen, ApAddr, &CapabilityInfo, &Timeout, &ListenIntv))
+	{
+		RTMPCancelTimer(&pAd->MlmeAux.ReassocTimer, &TimerCancelled);
+
+		NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+		if(NStatus != NDIS_STATUS_SUCCESS)
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("ASSOC - MlmeReassocReqAction() allocate memory failed \n"));
+			pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+			Status = MLME_FAIL_NO_RESOURCE;
+			MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_REASSOC_CONF, 2, &Status);
+			return;
+		}
+
+		COPY_MAC_ADDR(pAd->MlmeAux.Bssid, ApAddr);
+
+		// make frame, use bssid as the AP address??
+		DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - Send RE-ASSOC request...\n"));
+		MgtMacHeaderInit(pAd, &ReassocHdr, SUBTYPE_REASSOC_REQ, 0, ApAddr, ApAddr);
+		MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+						  sizeof(HEADER_802_11),    &ReassocHdr,
+						  2,                        &CapabilityInfo,
+						  2,                        &ListenIntv,
+						  MAC_ADDR_LEN,             ApAddr,
+						  1,                        &SsidIe,
+						  1,                        &pAd->MlmeAux.SsidLen,
+						  pAd->MlmeAux.SsidLen,     pAd->MlmeAux.Ssid,
+						  1,                        &SupRateIe,
+						  1,						&pAd->MlmeAux.SupRateLen,
+						  pAd->MlmeAux.SupRateLen,  pAd->MlmeAux.SupRate,
+						  END_OF_ARGS);
+
+		if (pAd->MlmeAux.ExtRateLen != 0)
+		{
+			MakeOutgoingFrame(pOutBuffer + FrameLen,        &tmp,
+							  1,                            &ExtRateIe,
+							  1,                            &pAd->MlmeAux.ExtRateLen,
+							  pAd->MlmeAux.ExtRateLen,	    pAd->MlmeAux.ExtRate,
+							  END_OF_ARGS);
+			FrameLen += tmp;
+		}
+
+		if (pAd->MlmeAux.APEdcaParm.bValid)
+		{
+			if (pAd->CommonCfg.bAPSDCapable && pAd->MlmeAux.APEdcaParm.bAPSDCapable)
+			{
+				QBSS_STA_INFO_PARM QosInfo;
+
+				NdisZeroMemory(&QosInfo, sizeof(QBSS_STA_INFO_PARM));
+				QosInfo.UAPSD_AC_BE = pAd->CommonCfg.bAPSDAC_BE;
+				QosInfo.UAPSD_AC_BK = pAd->CommonCfg.bAPSDAC_BK;
+				QosInfo.UAPSD_AC_VI = pAd->CommonCfg.bAPSDAC_VI;
+				QosInfo.UAPSD_AC_VO = pAd->CommonCfg.bAPSDAC_VO;
+				QosInfo.MaxSPLength = pAd->CommonCfg.MaxSPLength;
+				WmeIe[8] |= *(PUCHAR)&QosInfo;
+			}
+
+			MakeOutgoingFrame(pOutBuffer + FrameLen,    &tmp,
+							  9,                        &WmeIe[0],
+							  END_OF_ARGS);
+			FrameLen += tmp;
+		}
+
+#ifdef DOT11_N_SUPPORT
+		// HT
+		if ((pAd->MlmeAux.HtCapabilityLen > 0) && (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+		{
+			ULONG TmpLen;
+			UCHAR HtLen;
+			UCHAR BROADCOM[4] = {0x0, 0x90, 0x4c, 0x33};
+			if (pAd->StaActive.SupportedPhyInfo.bPreNHt == TRUE)
+			{
+				HtLen = SIZE_HT_CAP_IE + 4;
+				MakeOutgoingFrame(pOutBuffer + FrameLen,            &TmpLen,
+							  1,                                &WpaIe,
+							  1,                                &HtLen,
+							  4,                                &BROADCOM[0],
+							 pAd->MlmeAux.HtCapabilityLen,          &pAd->MlmeAux.HtCapability,
+							  END_OF_ARGS);
+			}
+			else
+			{
+				MakeOutgoingFrame(pOutBuffer + FrameLen,            &TmpLen,
+							  1,                                &HtCapIe,
+							  1,                                &pAd->MlmeAux.HtCapabilityLen,
+							 pAd->MlmeAux.HtCapabilityLen,          &pAd->MlmeAux.HtCapability,
+							  END_OF_ARGS);
+			}
+			FrameLen += TmpLen;
+		}
+#endif // DOT11_N_SUPPORT //
+
+		// add Ralink proprietary IE to inform AP this STA is going to use AGGREGATION or PIGGY-BACK+AGGREGATION
+		// Case I: (Aggregation + Piggy-Back)
+		// 1. user enable aggregation, AND
+		// 2. Mac support piggy-back
+		// 3. AP annouces it's PIGGY-BACK+AGGREGATION-capable in BEACON
+		// Case II: (Aggregation)
+		// 1. user enable aggregation, AND
+		// 2. AP annouces it's AGGREGATION-capable in BEACON
+		if (pAd->CommonCfg.bAggregationCapable)
+		{
+			if ((pAd->CommonCfg.bPiggyBackCapable) && ((pAd->MlmeAux.APRalinkIe & 0x00000003) == 3))
+			{
+				ULONG TmpLen;
+				UCHAR RalinkIe[9] = {IE_VENDOR_SPECIFIC, 7, 0x00, 0x0c, 0x43, 0x03, 0x00, 0x00, 0x00};
+				MakeOutgoingFrame(pOutBuffer+FrameLen,           &TmpLen,
+								  9,                             RalinkIe,
+								  END_OF_ARGS);
+				FrameLen += TmpLen;
+			}
+			else if (pAd->MlmeAux.APRalinkIe & 0x00000001)
+			{
+				ULONG TmpLen;
+				UCHAR RalinkIe[9] = {IE_VENDOR_SPECIFIC, 7, 0x00, 0x0c, 0x43, 0x01, 0x00, 0x00, 0x00};
+				MakeOutgoingFrame(pOutBuffer+FrameLen,           &TmpLen,
+								  9,                             RalinkIe,
+								  END_OF_ARGS);
+				FrameLen += TmpLen;
+			}
+		}
+		else
+		{
+			ULONG TmpLen;
+			UCHAR RalinkIe[9] = {IE_VENDOR_SPECIFIC, 7, 0x00, 0x0c, 0x43, 0x04, 0x00, 0x00, 0x00};
+			MakeOutgoingFrame(pOutBuffer+FrameLen,		 &TmpLen,
+							  9,						 RalinkIe,
+							  END_OF_ARGS);
+			FrameLen += TmpLen;
+		}
+#ifdef LEAP_SUPPORT
+		if (LEAP_CCKM_ON(pAd) && (pAd->StaCfg.CCKMLinkUpFlag == TRUE))
+		{
+			CkipFlag = pAd->StaCfg.CkipFlag;	// We have update that at PeerBeaconAtJoinRequest()
+			if (CkipFlag != 0)
+			{
+				NdisZeroMemory(CkipNegotiationBuffer, CKIP_NEGOTIATION_LENGTH);
+				CkipNegotiationBuffer[2] = 0x66;
+				// Make it try KP & MIC, since we have to follow the result from AssocRsp
+				CkipNegotiationBuffer[8] = 0x18;
+				CkipNegotiationBuffer[CKIP_NEGOTIATION_LENGTH - 1] = 0x22;
+
+				MakeOutgoingFrame(pOutBuffer + FrameLen,            &tmp,
+									1,                              &AironetCkipIe,
+									1,                              &AironetCkipLen,
+									AironetCkipLen,                 CkipNegotiationBuffer,
+									END_OF_ARGS);
+				FrameLen += tmp;
+			}
+
+			MakeOutgoingFrame(pOutBuffer + FrameLen, &tmp,
+							1,                              &AironetIPAddressIE,
+							1,                              &AironetIPAddressLen,
+							AironetIPAddressLen,            AironetIPAddressBuffer,
+							END_OF_ARGS);
+			FrameLen += tmp;
+
+			//
+			// The RN is incremented before each reassociation request.
+			//
+			pAd->StaCfg.CCKMRN++;
+			//
+			// Calculate MIC = hmac-md5(krk, STA-ID|BSSID|RSNIE|TSF|RN);
+			//
+			COPY_MAC_ADDR(CalcMicBuffer, pAd->CurrentAddress);
+			CalcMicBufferLen = MAC_ADDR_LEN;
+			COPY_MAC_ADDR(CalcMicBuffer + CalcMicBufferLen, pAd->MlmeAux.Bssid);
+			CalcMicBufferLen += MAC_ADDR_LEN;
+			NdisMoveMemory(CalcMicBuffer + CalcMicBufferLen, CipherSuiteCiscoCCKM, CipherSuiteCiscoCCKMLen);
+			CalcMicBufferLen += CipherSuiteCiscoCCKMLen;
+			NdisMoveMemory(CalcMicBuffer + CalcMicBufferLen, (PUCHAR) &pAd->StaCfg.CCKMBeaconAtJoinTimeStamp, sizeof(pAd->StaCfg.CCKMBeaconAtJoinTimeStamp));
+			CalcMicBufferLen += sizeof(pAd->StaCfg.CCKMBeaconAtJoinTimeStamp);
+			NdisMoveMemory(CalcMicBuffer + CalcMicBufferLen, (PUCHAR)&pAd->StaCfg.CCKMRN, sizeof(pAd->StaCfg.CCKMRN));
+			CalcMicBufferLen += sizeof(pAd->StaCfg.CCKMRN);
+			hmac_md5(pAd->StaCfg.KRK, LEN_EAP_MICK, CalcMicBuffer, CalcMicBufferLen, MICMN);
+
+			//
+			// fill up CCKM reassociation request element
+			//
+			NdisMoveMemory(AironetCCKMReassocBuffer, AironetOUI, 4);
+			NdisMoveMemory(AironetCCKMReassocBuffer + 4, (PUCHAR)&pAd->StaCfg.CCKMBeaconAtJoinTimeStamp, 8);
+			NdisMoveMemory(AironetCCKMReassocBuffer + 12, (PUCHAR) &pAd->StaCfg.CCKMRN, 4);
+			NdisMoveMemory(AironetCCKMReassocBuffer +16, MICMN, 8);
+
+			MakeOutgoingFrame(pOutBuffer + FrameLen, &tmp,
+							1,                      &AironetCCKMReassocIE,
+							1,                      &AironetCCKMReassocLen,
+							AironetCCKMReassocLen,  AironetCCKMReassocBuffer,
+							END_OF_ARGS);
+			FrameLen += tmp;
+
+			MakeOutgoingFrame(pOutBuffer + FrameLen, &tmp,
+							CipherSuiteCiscoCCKMLen,CipherSuiteCiscoCCKM,
+							END_OF_ARGS);
+			FrameLen += tmp;
+		}
+#endif // LEAP_SUPPORT //
+
+		// Add CCX v2 request if CCX2 admin state is on
+		if (pAd->StaCfg.CCXControl.field.Enable == 1)
+		{
+			//
+			// Add CCX Version
+			//
+			MakeOutgoingFrame(pOutBuffer + FrameLen, &tmp,
+						1,							&Ccx2Ie,
+						1,							&Ccx2Len,
+						Ccx2Len,				    Ccx2IeInfo,
+						END_OF_ARGS);
+			FrameLen += tmp;
+		}
+
+		MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+		MlmeFreeMemory(pAd, pOutBuffer);
+
+		RTMPSetTimer(&pAd->MlmeAux.ReassocTimer, Timeout); /* in mSec */
+		pAd->Mlme.AssocMachine.CurrState = REASSOC_WAIT_RSP;
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("ASSOC - MlmeReassocReqAction() sanity check failed. BUG!!!! \n"));
+		pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+		Status = MLME_INVALID_FORMAT;
+		MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_REASSOC_CONF, 2, &Status);
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+		Upper layer issues disassoc request
+	Parameters:
+		Elem -
+
+	IRQL = PASSIVE_LEVEL
+
+	==========================================================================
+ */
+VOID MlmeDisassocReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	PMLME_DISASSOC_REQ_STRUCT pDisassocReq;
+	HEADER_802_11         DisassocHdr;
+	PHEADER_802_11        pDisassocHdr;
+	PUCHAR                pOutBuffer = NULL;
+	ULONG                 FrameLen = 0;
+	NDIS_STATUS           NStatus;
+	BOOLEAN               TimerCancelled;
+	ULONG                 Timeout = 0;
+	USHORT                Status;
+
+#ifdef QOS_DLS_SUPPORT
+	// send DLS-TEAR_DOWN message,
+	if (pAd->CommonCfg.bDLSCapable)
+	{
+		UCHAR i;
+
+		// tear down local dls table entry
+		for (i=0; i<MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+		{
+			if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+			{
+				RTMPSendDLSTearDownFrame(pAd, pAd->StaCfg.DLSEntry[i].MacAddr);
+				pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+				pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+			}
+		}
+
+		// tear down peer dls table entry
+		for (i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+		{
+			if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+			{
+				RTMPSendDLSTearDownFrame(pAd, pAd->StaCfg.DLSEntry[i].MacAddr);
+				pAd->StaCfg.DLSEntry[i].Status = DLS_NONE;
+				pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+			}
+		}
+	}
+#endif // QOS_DLS_SUPPORT //
+
+	// skip sanity check
+	pDisassocReq = (PMLME_DISASSOC_REQ_STRUCT)(Elem->Msg);
+
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if (NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - MlmeDisassocReqAction() allocate memory failed\n"));
+		pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+		Status = MLME_FAIL_NO_RESOURCE;
+		MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_DISASSOC_CONF, 2, &Status);
+		return;
+	}
+
+
+
+	RTMPCancelTimer(&pAd->MlmeAux.DisassocTimer, &TimerCancelled);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - Send DISASSOC request[BSSID::%02x:%02x:%02x:%02x:%02x:%02x (Reason=%d)\n",
+				pDisassocReq->Addr[0], pDisassocReq->Addr[1], pDisassocReq->Addr[2],
+				pDisassocReq->Addr[3], pDisassocReq->Addr[4], pDisassocReq->Addr[5], pDisassocReq->Reason));
+	MgtMacHeaderInit(pAd, &DisassocHdr, SUBTYPE_DISASSOC, 0, pDisassocReq->Addr, pDisassocReq->Addr);	// patch peap ttls switching issue
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+					  sizeof(HEADER_802_11),&DisassocHdr,
+					  2,                    &pDisassocReq->Reason,
+					  END_OF_ARGS);
+	MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+
+	// To patch Instance and Buffalo(N) AP
+	// Driver has to send deauth to Instance AP, but Buffalo(N) needs to send disassoc to reset Authenticator's state machine
+	// Therefore, we send both of them.
+	pDisassocHdr = (PHEADER_802_11)pOutBuffer;
+	pDisassocHdr->FC.SubType = SUBTYPE_DEAUTH;
+	MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	pAd->StaCfg.DisassocReason = REASON_DISASSOC_STA_LEAVING;
+	COPY_MAC_ADDR(pAd->StaCfg.DisassocSta, pDisassocReq->Addr);
+
+	RTMPSetTimer(&pAd->MlmeAux.DisassocTimer, Timeout); /* in mSec */
+	pAd->Mlme.AssocMachine.CurrState = DISASSOC_WAIT_RSP;
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+    if (pAd->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_DISABLE)
+	{
+        union iwreq_data    wrqu;
+        //send disassociate event to wpa_supplicant
+        memset(&wrqu, 0, sizeof(wrqu));
+        wrqu.data.flags = RT_DISASSOC_EVENT_FLAG;
+        wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+    }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+    {
+        union iwreq_data    wrqu;
+        memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+        wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+    }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+
+}
+
+/*
+	==========================================================================
+	Description:
+		peer sends assoc rsp back
+	Parameters:
+		Elme - MLME message containing the received frame
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID PeerAssocRspAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT        CapabilityInfo, Status, Aid;
+	UCHAR         SupRate[MAX_LEN_OF_SUPPORTED_RATES], SupRateLen;
+	UCHAR         ExtRate[MAX_LEN_OF_SUPPORTED_RATES], ExtRateLen;
+	UCHAR         Addr2[MAC_ADDR_LEN];
+	BOOLEAN       TimerCancelled;
+	UCHAR         CkipFlag;
+	EDCA_PARM     EdcaParm;
+	HT_CAPABILITY_IE		HtCapability;
+	ADD_HT_INFO_IE		AddHtInfo;	// AP might use this additional ht info IE
+	UCHAR			HtCapabilityLen;
+	UCHAR			AddHtInfoLen;
+	UCHAR			NewExtChannelOffset = 0xff;
+
+	if (PeerAssocRspSanity(pAd, Elem->Msg, Elem->MsgLen, Addr2, &CapabilityInfo, &Status, &Aid, SupRate, &SupRateLen, ExtRate, &ExtRateLen,
+		&HtCapability,&AddHtInfo, &HtCapabilityLen,&AddHtInfoLen,&NewExtChannelOffset, &EdcaParm, &CkipFlag))
+	{
+		// The frame is for me ?
+		if(MAC_ADDR_EQUAL(Addr2, pAd->MlmeAux.Bssid))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("PeerAssocRspAction():ASSOC - receive ASSOC_RSP to me (status=%d)\n", Status));
+#ifdef DOT11_N_SUPPORT
+			DBGPRINT(RT_DEBUG_TRACE, ("PeerAssocRspAction():MacTable [%d].AMsduSize = %d. ClientStatusFlags = 0x%lx \n",Elem->Wcid, pAd->MacTab.Content[BSSID_WCID].AMsduSize, pAd->MacTab.Content[BSSID_WCID].ClientStatusFlags));
+#endif // DOT11_N_SUPPORT //
+			RTMPCancelTimer(&pAd->MlmeAux.AssocTimer, &TimerCancelled);
+			if(Status == MLME_SUCCESS)
+			{
+				// go to procedure listed on page 376
+				AssocPostProc(pAd, Addr2, CapabilityInfo, Aid, SupRate, SupRateLen, ExtRate, ExtRateLen,
+					&EdcaParm, &HtCapability, HtCapabilityLen, &AddHtInfo);
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+                if (pAd->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_DISABLE)
+                {
+                    union iwreq_data    wrqu;
+
+                    SendAssocIEsToWpaSupplicant(pAd);
+                    memset(&wrqu, 0, sizeof(wrqu));
+                    wrqu.data.flags = RT_ASSOC_EVENT_FLAG;
+                    wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+                }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+                {
+                    union iwreq_data    wrqu;
+                    wext_notify_event_assoc(pAd);
+
+                    memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+                    memcpy(wrqu.ap_addr.sa_data, pAd->MlmeAux.Bssid, MAC_ADDR_LEN);
+                    wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+
+                }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+
+
+				pAd->StaCfg.CkipFlag = CkipFlag;
+				if (CkipFlag & 0x18)
+				{
+					NdisZeroMemory(pAd->StaCfg.TxSEQ, 4);
+					NdisZeroMemory(pAd->StaCfg.RxSEQ, 4);
+					NdisZeroMemory(pAd->StaCfg.CKIPMIC, 4);
+					pAd->StaCfg.GIV[0] = RandomByte(pAd);
+					pAd->StaCfg.GIV[1] = RandomByte(pAd);
+					pAd->StaCfg.GIV[2] = RandomByte(pAd);
+					pAd->StaCfg.bCkipOn = TRUE;
+					DBGPRINT(RT_DEBUG_TRACE, ("<CCX> pAd->StaCfg.CkipFlag = 0x%02x\n", pAd->StaCfg.CkipFlag));
+				}
+			}
+			else
+			{
+				// Faile on Association, we need to check the status code
+				// Is that a Rogue AP?
+#ifdef LEAP_SUPPORT
+				if ((pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP) && (Status == MLME_ALG_NOT_SUPPORT))
+				{ //Possibly Rogue AP
+					RogueApTableSetEntry(pAd, &pAd->StaCfg.RogueApTab, pAd->MlmeAux.Bssid, LEAP_REASON_INVALID_AUTH);
+				}
+#endif // LEAP_SUPPORT //
+			}
+			pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+			MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_ASSOC_CONF, 2, &Status);
+		}
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - PeerAssocRspAction() sanity check fail\n"));
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+		peer sends reassoc rsp
+	Parametrs:
+		Elem - MLME message cntaining the received frame
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID PeerReassocRspAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT      CapabilityInfo;
+	USHORT      Status;
+	USHORT      Aid;
+	UCHAR       SupRate[MAX_LEN_OF_SUPPORTED_RATES], SupRateLen;
+	UCHAR       ExtRate[MAX_LEN_OF_SUPPORTED_RATES], ExtRateLen;
+	UCHAR       Addr2[MAC_ADDR_LEN];
+	UCHAR       CkipFlag;
+	BOOLEAN     TimerCancelled;
+	EDCA_PARM   EdcaParm;
+	HT_CAPABILITY_IE		HtCapability;
+	ADD_HT_INFO_IE		AddHtInfo;	// AP might use this additional ht info IE
+	UCHAR			HtCapabilityLen;
+	UCHAR			AddHtInfoLen;
+	UCHAR			NewExtChannelOffset = 0xff;
+
+	if(PeerAssocRspSanity(pAd, Elem->Msg, Elem->MsgLen, Addr2, &CapabilityInfo, &Status, &Aid, SupRate, &SupRateLen, ExtRate, &ExtRateLen,
+								&HtCapability,	&AddHtInfo, &HtCapabilityLen, &AddHtInfoLen,&NewExtChannelOffset, &EdcaParm, &CkipFlag))
+	{
+		if(MAC_ADDR_EQUAL(Addr2, pAd->MlmeAux.Bssid)) // The frame is for me ?
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - receive REASSOC_RSP to me (status=%d)\n", Status));
+			RTMPCancelTimer(&pAd->MlmeAux.ReassocTimer, &TimerCancelled);
+
+			if(Status == MLME_SUCCESS)
+			{
+				// go to procedure listed on page 376
+				AssocPostProc(pAd, Addr2, CapabilityInfo, Aid, SupRate, SupRateLen, ExtRate, ExtRateLen,
+					 &EdcaParm, &HtCapability, HtCapabilityLen, &AddHtInfo);
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+                if (pAd->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_DISABLE)
+                {
+                    union iwreq_data    wrqu;
+
+                    SendAssocIEsToWpaSupplicant(pAd);
+                    memset(&wrqu, 0, sizeof(wrqu));
+                    wrqu.data.flags = RT_ASSOC_EVENT_FLAG;
+                    wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+                }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+                {
+                    union iwreq_data    wrqu;
+                    wext_notify_event_assoc(pAd);
+
+                    memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+                    memcpy(wrqu.ap_addr.sa_data, pAd->MlmeAux.Bssid, MAC_ADDR_LEN);
+                    wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+
+                }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+
+			}
+
+			//
+			// Cisco Leap CCKM supported Re-association.
+			//
+#ifdef LEAP_SUPPORT
+			if (LEAP_CCKM_ON(pAd) && (pAd->StaCfg.CCKMLinkUpFlag == TRUE))
+			{
+				if (CCKMAssocRspSanity(pAd, Elem->Msg, Elem->MsgLen) == TRUE)
+				{
+					pAd->StaCfg.CkipFlag = CkipFlag;
+					if (CkipFlag & 0x18)
+					{
+						NdisZeroMemory(pAd->StaCfg.TxSEQ, 4);
+						NdisZeroMemory(pAd->StaCfg.RxSEQ, 4);
+						NdisZeroMemory(pAd->StaCfg.CKIPMIC, 4);
+						pAd->StaCfg.GIV[0] = RandomByte(pAd);
+						pAd->StaCfg.GIV[1] = RandomByte(pAd);
+						pAd->StaCfg.GIV[2] = RandomByte(pAd);
+						pAd->StaCfg.bCkipOn = TRUE;
+						DBGPRINT(RT_DEBUG_TRACE, ("<CCX> pAd->StaCfg.CkipFlag = 0x%02x\n", pAd->StaCfg.CkipFlag));
+					}
+
+					pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+					MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_REASSOC_CONF, 2, &Status);
+				}
+				else
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - CCKMAssocRspSanity() sanity check fail\n"));
+				}
+			}
+			else
+#endif // LEAP_SUPPORT //
+			{
+				// CkipFlag is no use for reassociate
+				pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+				MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_REASSOC_CONF, 2, &Status);
+			}
+		}
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - PeerReassocRspAction() sanity check fail\n"));
+	}
+
+}
+
+/*
+	==========================================================================
+	Description:
+		procedures on IEEE 802.11/1999 p.376
+	Parametrs:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AssocPostProc(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pAddr2,
+	IN USHORT CapabilityInfo,
+	IN USHORT Aid,
+	IN UCHAR SupRate[],
+	IN UCHAR SupRateLen,
+	IN UCHAR ExtRate[],
+	IN UCHAR ExtRateLen,
+	IN PEDCA_PARM pEdcaParm,
+	IN HT_CAPABILITY_IE		*pHtCapability,
+	IN UCHAR HtCapabilityLen,
+	IN ADD_HT_INFO_IE		*pAddHtInfo)	// AP might use this additional ht info IE
+{
+	ULONG Idx;
+
+	pAd->MlmeAux.BssType = BSS_INFRA;
+	COPY_MAC_ADDR(pAd->MlmeAux.Bssid, pAddr2);
+	pAd->MlmeAux.Aid = Aid;
+	pAd->MlmeAux.CapabilityInfo = CapabilityInfo & SUPPORTED_CAPABILITY_INFO;
+#ifdef DOT11_N_SUPPORT
+	// Some HT AP might lost WMM IE. We add WMM ourselves. beacuase HT requires QoS on.
+	if ((HtCapabilityLen > 0) && (pEdcaParm->bValid == FALSE))
+	{
+		pEdcaParm->bValid = TRUE;
+		pEdcaParm->Aifsn[0] = 3;
+		pEdcaParm->Aifsn[1] = 7;
+		pEdcaParm->Aifsn[2] = 2;
+		pEdcaParm->Aifsn[3] = 2;
+
+		pEdcaParm->Cwmin[0] = 4;
+		pEdcaParm->Cwmin[1] = 4;
+		pEdcaParm->Cwmin[2] = 3;
+		pEdcaParm->Cwmin[3] = 2;
+
+		pEdcaParm->Cwmax[0] = 10;
+		pEdcaParm->Cwmax[1] = 10;
+		pEdcaParm->Cwmax[2] = 4;
+		pEdcaParm->Cwmax[3] = 3;
+
+		pEdcaParm->Txop[0]  = 0;
+		pEdcaParm->Txop[1]  = 0;
+		pEdcaParm->Txop[2]  = 96;
+		pEdcaParm->Txop[3]  = 48;
+
+	}
+#endif // DOT11_N_SUPPORT //
+
+	NdisMoveMemory(&pAd->MlmeAux.APEdcaParm, pEdcaParm, sizeof(EDCA_PARM));
+
+	// filter out un-supported rates
+	pAd->MlmeAux.SupRateLen = SupRateLen;
+	NdisMoveMemory(pAd->MlmeAux.SupRate, SupRate, SupRateLen);
+	RTMPCheckRates(pAd, pAd->MlmeAux.SupRate, &pAd->MlmeAux.SupRateLen);
+
+	// filter out un-supported rates
+	pAd->MlmeAux.ExtRateLen = ExtRateLen;
+	NdisMoveMemory(pAd->MlmeAux.ExtRate, ExtRate, ExtRateLen);
+	RTMPCheckRates(pAd, pAd->MlmeAux.ExtRate, &pAd->MlmeAux.ExtRateLen);
+
+#ifdef DOT11_N_SUPPORT
+	if (HtCapabilityLen > 0)
+	{
+		RTMPCheckHt(pAd, BSSID_WCID, pHtCapability, pAddHtInfo);
+	}
+	DBGPRINT(RT_DEBUG_TRACE, ("AssocPostProc===>  AP.AMsduSize = %d. ClientStatusFlags = 0x%lx \n", pAd->MacTab.Content[BSSID_WCID].AMsduSize, pAd->MacTab.Content[BSSID_WCID].ClientStatusFlags));
+
+	DBGPRINT(RT_DEBUG_TRACE, ("AssocPostProc===>    (Mmps=%d, AmsduSize=%d, )\n",
+		pAd->MacTab.Content[BSSID_WCID].MmpsMode, pAd->MacTab.Content[BSSID_WCID].AMsduSize));
+#endif // DOT11_N_SUPPORT //
+
+	// Set New WPA information
+	Idx = BssTableSearch(&pAd->ScanTab, pAddr2, pAd->MlmeAux.Channel);
+	if (Idx == BSS_NOT_FOUND)
+	{
+		DBGPRINT_ERR(("ASSOC - Can't find BSS after receiving Assoc response\n"));
+	}
+	else
+	{
+		// Init variable
+		pAd->MacTab.Content[BSSID_WCID].RSNIE_Len = 0;
+		NdisZeroMemory(pAd->MacTab.Content[BSSID_WCID].RSN_IE, MAX_LEN_OF_RSNIE);
+
+		// Store appropriate RSN_IE for WPA SM negotiation later
+		if ((pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA) && (pAd->ScanTab.BssEntry[Idx].VarIELen != 0))
+		{
+			PUCHAR              pVIE;
+			USHORT              len;
+			PEID_STRUCT         pEid;
+
+			pVIE = pAd->ScanTab.BssEntry[Idx].VarIEs;
+			len	 = pAd->ScanTab.BssEntry[Idx].VarIELen;
+
+			while (len > 0)
+			{
+				pEid = (PEID_STRUCT) pVIE;
+				// For WPA/WPAPSK
+				if ((pEid->Eid == IE_WPA) && (NdisEqualMemory(pEid->Octet, WPA_OUI, 4))
+					&& (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA || pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK))
+				{
+					NdisMoveMemory(pAd->MacTab.Content[BSSID_WCID].RSN_IE, pVIE, (pEid->Len + 2));
+					pAd->MacTab.Content[BSSID_WCID].RSNIE_Len = (pEid->Len + 2);
+					DBGPRINT(RT_DEBUG_TRACE, ("AssocPostProc===> Store RSN_IE for WPA SM negotiation \n"));
+				}
+				// For WPA2/WPA2PSK
+				else if ((pEid->Eid == IE_RSN) && (NdisEqualMemory(pEid->Octet + 2, RSN_OUI, 3))
+					&& (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2 || pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK))
+				{
+					NdisMoveMemory(pAd->MacTab.Content[BSSID_WCID].RSN_IE, pVIE, (pEid->Len + 2));
+					pAd->MacTab.Content[BSSID_WCID].RSNIE_Len = (pEid->Len + 2);
+					DBGPRINT(RT_DEBUG_TRACE, ("AssocPostProc===> Store RSN_IE for WPA2 SM negotiation \n"));
+				}
+
+				pVIE += (pEid->Len + 2);
+				len  -= (pEid->Len + 2);
+			}
+		}
+
+		if (pAd->MacTab.Content[BSSID_WCID].RSNIE_Len == 0)
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("AssocPostProc===> no RSN_IE \n"));
+		}
+		else
+		{
+			hex_dump("RSN_IE", pAd->MacTab.Content[BSSID_WCID].RSN_IE, pAd->MacTab.Content[BSSID_WCID].RSNIE_Len);
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+		left part of IEEE 802.11/1999 p.374
+	Parameters:
+		Elem - MLME message containing the received frame
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID PeerDisassocAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR         Addr2[MAC_ADDR_LEN];
+	USHORT        Reason;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - PeerDisassocAction()\n"));
+	if(PeerDisassocSanity(pAd, Elem->Msg, Elem->MsgLen, Addr2, &Reason))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - PeerDisassocAction() Reason = %d\n", Reason));
+		if (INFRA_ON(pAd) && MAC_ADDR_EQUAL(pAd->CommonCfg.Bssid, Addr2))
+		{
+
+			if (pAd->CommonCfg.bWirelessEvent)
+			{
+				RTMPSendWirelessEvent(pAd, IW_DISASSOC_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+			}
+
+
+#ifdef LEAP_SUPPORT
+			if (pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP)
+			{
+				// Cisco_LEAP has start a timer
+				// We should cancel it if using LEAP
+				RTMPCancelTimer(&pAd->StaCfg.LeapAuthTimer, &TimerCancelled);
+				//Check is it mach the LEAP Authentication failed as possible a Rogue AP
+				//on it's PortSecured not equal to WPA_802_1X_PORT_SECURED while process the Association.
+				if ((pAd->Mlme.LeapMachine.CurrState != LEAP_IDLE) && (pAd->StaCfg.PortSecured != WPA_802_1X_PORT_SECURED))
+				{
+					RogueApTableSetEntry(pAd, &pAd->StaCfg.RogueApTab, Addr2, LEAP_REASON_AUTH_TIMEOUT);
+				}
+			}
+#endif	// LEAP_SUPPORT //
+			//
+			// Get Current System time and Turn on AdjacentAPReport
+			//
+			NdisGetSystemUpTime(&pAd->StaCfg.CCXAdjacentAPLinkDownTime);
+			pAd->StaCfg.CCXAdjacentAPReportFlag = TRUE;
+			LinkDown(pAd, TRUE);
+			pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+            if (pAd->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_DISABLE)
+			{
+                union iwreq_data    wrqu;
+                //send disassociate event to wpa_supplicant
+                memset(&wrqu, 0, sizeof(wrqu));
+                wrqu.data.flags = RT_DISASSOC_EVENT_FLAG;
+                wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+            }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+            {
+                union iwreq_data    wrqu;
+                memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+                wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+            }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+		}
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - PeerDisassocAction() sanity check fail\n"));
+	}
+
+}
+
+/*
+	==========================================================================
+	Description:
+		what the state machine will do after assoc timeout
+	Parameters:
+		Elme -
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AssocTimeoutAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT  Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - AssocTimeoutAction\n"));
+	pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+	Status = MLME_REJ_TIMEOUT;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_ASSOC_CONF, 2, &Status);
+}
+
+/*
+	==========================================================================
+	Description:
+		what the state machine will do after reassoc timeout
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID ReassocTimeoutAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT  Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - ReassocTimeoutAction\n"));
+	pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+	Status = MLME_REJ_TIMEOUT;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_REASSOC_CONF, 2, &Status);
+}
+
+/*
+	==========================================================================
+	Description:
+		what the state machine will do after disassoc timeout
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID DisassocTimeoutAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT  Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - DisassocTimeoutAction\n"));
+	pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+	Status = MLME_SUCCESS;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_DISASSOC_CONF, 2, &Status);
+}
+
+VOID InvalidStateWhenAssoc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT  Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - InvalidStateWhenAssoc(state=%ld), reset ASSOC state machine\n",
+		pAd->Mlme.AssocMachine.CurrState));
+	pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+	Status = MLME_STATE_MACHINE_REJECT;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_ASSOC_CONF, 2, &Status);
+}
+
+VOID InvalidStateWhenReassoc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - InvalidStateWhenReassoc(state=%ld), reset ASSOC state machine\n",
+		pAd->Mlme.AssocMachine.CurrState));
+	pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+	Status = MLME_STATE_MACHINE_REJECT;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_REASSOC_CONF, 2, &Status);
+}
+
+VOID InvalidStateWhenDisassociate(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - InvalidStateWhenDisassoc(state=%ld), reset ASSOC state machine\n",
+		pAd->Mlme.AssocMachine.CurrState));
+	pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+	Status = MLME_STATE_MACHINE_REJECT;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_DISASSOC_CONF, 2, &Status);
+}
+
+/*
+	==========================================================================
+	Description:
+		right part of IEEE 802.11/1999 page 374
+	Note:
+		This event should never cause ASSOC state machine perform state
+		transition, and has no relationship with CNTL machine. So we separate
+		this routine as a service outside of ASSOC state transition table.
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID Cls3errAction(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR        pAddr)
+{
+	HEADER_802_11         DisassocHdr;
+	PHEADER_802_11        pDisassocHdr;
+	PUCHAR                pOutBuffer = NULL;
+	ULONG                 FrameLen = 0;
+	NDIS_STATUS           NStatus;
+	USHORT                Reason = REASON_CLS3ERR;
+
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if (NStatus != NDIS_STATUS_SUCCESS)
+		return;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - Class 3 Error, Send DISASSOC frame\n"));
+	MgtMacHeaderInit(pAd, &DisassocHdr, SUBTYPE_DISASSOC, 0, pAddr, pAd->CommonCfg.Bssid);	// patch peap ttls switching issue
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+					  sizeof(HEADER_802_11),&DisassocHdr,
+					  2,                    &Reason,
+					  END_OF_ARGS);
+	MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+
+	// To patch Instance and Buffalo(N) AP
+	// Driver has to send deauth to Instance AP, but Buffalo(N) needs to send disassoc to reset Authenticator's state machine
+	// Therefore, we send both of them.
+	pDisassocHdr = (PHEADER_802_11)pOutBuffer;
+	pDisassocHdr->FC.SubType = SUBTYPE_DEAUTH;
+	MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	pAd->StaCfg.DisassocReason = REASON_CLS3ERR;
+	COPY_MAC_ADDR(pAd->StaCfg.DisassocSta, pAddr);
+}
+
+ /*
+	 ==========================================================================
+	 Description:
+		 Switch between WEP and CKIP upon new association up.
+	 Parameters:
+
+	 IRQL = DISPATCH_LEVEL
+
+	 ==========================================================================
+  */
+VOID SwitchBetweenWepAndCkip(
+	IN PRTMP_ADAPTER pAd)
+{
+	int            i;
+	SHAREDKEY_MODE_STRUC  csr1;
+
+	// if KP is required. change the CipherAlg in hardware shard key table from WEP
+	// to CKIP. else remain as WEP
+	if (pAd->StaCfg.bCkipOn && (pAd->StaCfg.CkipFlag & 0x10))
+	{
+		// modify hardware key table so that MAC use correct algorithm to decrypt RX
+		RTMP_IO_READ32(pAd, SHARED_KEY_MODE_BASE, &csr1.word);
+		if (csr1.field.Bss0Key0CipherAlg == CIPHER_WEP64)
+			csr1.field.Bss0Key0CipherAlg = CIPHER_CKIP64;
+		else if (csr1.field.Bss0Key0CipherAlg == CIPHER_WEP128)
+			csr1.field.Bss0Key0CipherAlg = CIPHER_CKIP128;
+
+		if (csr1.field.Bss0Key1CipherAlg == CIPHER_WEP64)
+			csr1.field.Bss0Key1CipherAlg = CIPHER_CKIP64;
+		else if (csr1.field.Bss0Key1CipherAlg == CIPHER_WEP128)
+			csr1.field.Bss0Key1CipherAlg = CIPHER_CKIP128;
+
+		if (csr1.field.Bss0Key2CipherAlg == CIPHER_WEP64)
+			csr1.field.Bss0Key2CipherAlg = CIPHER_CKIP64;
+		else if (csr1.field.Bss0Key2CipherAlg == CIPHER_WEP128)
+			csr1.field.Bss0Key2CipherAlg = CIPHER_CKIP128;
+
+		if (csr1.field.Bss0Key3CipherAlg == CIPHER_WEP64)
+			csr1.field.Bss0Key3CipherAlg = CIPHER_CKIP64;
+		else if (csr1.field.Bss0Key3CipherAlg == CIPHER_WEP128)
+			csr1.field.Bss0Key3CipherAlg = CIPHER_CKIP128;
+		RTMP_IO_WRITE32(pAd, SHARED_KEY_MODE_BASE, csr1.word);
+		DBGPRINT(RT_DEBUG_TRACE, ("SwitchBetweenWepAndCkip: modify BSS0 cipher to %s\n", CipherName[csr1.field.Bss0Key0CipherAlg]));
+
+		// modify software key table so that driver can specify correct algorithm in TXD upon TX
+		for (i=0; i<SHARE_KEY_NUM; i++)
+		{
+			if (pAd->SharedKey[BSS0][i].CipherAlg == CIPHER_WEP64)
+				pAd->SharedKey[BSS0][i].CipherAlg = CIPHER_CKIP64;
+			else if (pAd->SharedKey[BSS0][i].CipherAlg == CIPHER_WEP128)
+				pAd->SharedKey[BSS0][i].CipherAlg = CIPHER_CKIP128;
+		}
+	}
+
+	// else if KP NOT inused. change the CipherAlg in hardware shard key table from CKIP
+	// to WEP.
+	else
+	{
+		// modify hardware key table so that MAC use correct algorithm to decrypt RX
+		RTMP_IO_READ32(pAd, SHARED_KEY_MODE_BASE, &csr1.word);
+		if (csr1.field.Bss0Key0CipherAlg == CIPHER_CKIP64)
+			csr1.field.Bss0Key0CipherAlg = CIPHER_WEP64;
+		else if (csr1.field.Bss0Key0CipherAlg == CIPHER_CKIP128)
+			csr1.field.Bss0Key0CipherAlg = CIPHER_WEP128;
+
+		if (csr1.field.Bss0Key1CipherAlg == CIPHER_CKIP64)
+			csr1.field.Bss0Key1CipherAlg = CIPHER_WEP64;
+		else if (csr1.field.Bss0Key1CipherAlg == CIPHER_CKIP128)
+			csr1.field.Bss0Key1CipherAlg = CIPHER_WEP128;
+
+		if (csr1.field.Bss0Key2CipherAlg == CIPHER_CKIP64)
+			csr1.field.Bss0Key2CipherAlg = CIPHER_WEP64;
+		else if (csr1.field.Bss0Key2CipherAlg == CIPHER_CKIP128)
+			csr1.field.Bss0Key2CipherAlg = CIPHER_WEP128;
+
+		if (csr1.field.Bss0Key3CipherAlg == CIPHER_CKIP64)
+			csr1.field.Bss0Key3CipherAlg = CIPHER_WEP64;
+		else if (csr1.field.Bss0Key3CipherAlg == CIPHER_CKIP128)
+			csr1.field.Bss0Key3CipherAlg = CIPHER_WEP128;
+
+		// modify software key table so that driver can specify correct algorithm in TXD upon TX
+		for (i=0; i<SHARE_KEY_NUM; i++)
+		{
+			if (pAd->SharedKey[BSS0][i].CipherAlg == CIPHER_CKIP64)
+				pAd->SharedKey[BSS0][i].CipherAlg = CIPHER_WEP64;
+			else if (pAd->SharedKey[BSS0][i].CipherAlg == CIPHER_CKIP128)
+				pAd->SharedKey[BSS0][i].CipherAlg = CIPHER_WEP128;
+		}
+
+		//
+		// On WPA-NONE, must update CipherAlg.
+		// Because the OID_802_11_WEP_STATUS was been set after OID_802_11_ADD_KEY
+		// and CipherAlg will be CIPHER_NONE by Windows ZeroConfig.
+		// So we need to update CipherAlg after connect.
+		//
+		if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+		{
+			for (i = 0; i < SHARE_KEY_NUM; i++)
+			{
+				if (pAd->SharedKey[BSS0][i].KeyLen != 0)
+				{
+					if (pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled)
+					{
+						pAd->SharedKey[BSS0][i].CipherAlg = CIPHER_TKIP;
+					}
+					else if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+					{
+						pAd->SharedKey[BSS0][i].CipherAlg = CIPHER_AES;
+					}
+				}
+				else
+				{
+					pAd->SharedKey[BSS0][i].CipherAlg = CIPHER_NONE;
+				}
+			}
+
+			csr1.field.Bss0Key0CipherAlg = pAd->SharedKey[BSS0][0].CipherAlg;
+			csr1.field.Bss0Key1CipherAlg = pAd->SharedKey[BSS0][1].CipherAlg;
+			csr1.field.Bss0Key2CipherAlg = pAd->SharedKey[BSS0][2].CipherAlg;
+			csr1.field.Bss0Key3CipherAlg = pAd->SharedKey[BSS0][3].CipherAlg;
+		}
+		RTMP_IO_WRITE32(pAd, SHARED_KEY_MODE_BASE, csr1.word);
+		DBGPRINT(RT_DEBUG_TRACE, ("SwitchBetweenWepAndCkip: modify BSS0 cipher to %s\n", CipherName[csr1.field.Bss0Key0CipherAlg]));
+	}
+}
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+VOID    SendAssocIEsToWpaSupplicant(
+    IN  PRTMP_ADAPTER pAd)
+{
+    union iwreq_data    wrqu;
+    unsigned char custom[IW_CUSTOM_MAX] = {0};
+
+    if ((pAd->StaCfg.ReqVarIELen + 17) <= IW_CUSTOM_MAX)
+    {
+        sprintf(custom, "ASSOCINFO_ReqIEs=");
+	    NdisMoveMemory(custom+17, pAd->StaCfg.ReqVarIEs, pAd->StaCfg.ReqVarIELen);
+	    memset(&wrqu, 0, sizeof(wrqu));
+        wrqu.data.length = pAd->StaCfg.ReqVarIELen + 17;
+        wrqu.data.flags = RT_REQIE_EVENT_FLAG;
+        wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, custom);
+
+        memset(&wrqu, 0, sizeof(wrqu));
+        wrqu.data.flags = RT_ASSOCINFO_EVENT_FLAG;
+        wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+    }
+    else
+        DBGPRINT(RT_DEBUG_TRACE, ("pAd->StaCfg.ReqVarIELen + 17 > MAX_CUSTOM_LEN\n"));
+
+    return;
+}
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+int wext_notify_event_assoc(
+	IN  RTMP_ADAPTER *pAd)
+{
+    union iwreq_data    wrqu;
+    char custom[IW_CUSTOM_MAX] = {0};
+
+#if WIRELESS_EXT > 17
+    if (pAd->StaCfg.ReqVarIELen <= IW_CUSTOM_MAX)
+    {
+        wrqu.data.length = pAd->StaCfg.ReqVarIELen;
+        memcpy(custom, pAd->StaCfg.ReqVarIEs, pAd->StaCfg.ReqVarIELen);
+        wireless_send_event(pAd->net_dev, IWEVASSOCREQIE, &wrqu, custom);
+    }
+    else
+        DBGPRINT(RT_DEBUG_TRACE, ("pAd->StaCfg.ReqVarIELen > MAX_CUSTOM_LEN\n"));
+#else
+    if (((pAd->StaCfg.ReqVarIELen*2) + 17) <= IW_CUSTOM_MAX)
+    {
+        UCHAR   idx;
+        wrqu.data.length = (pAd->StaCfg.ReqVarIELen*2) + 17;
+        sprintf(custom, "ASSOCINFO(ReqIEs=");
+        for (idx=0; idx<pAd->StaCfg.ReqVarIELen; idx++)
+                sprintf(custom, "%s%02x", custom, pAd->StaCfg.ReqVarIEs[idx]);
+        wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, custom);
+    }
+    else
+        DBGPRINT(RT_DEBUG_TRACE, ("(pAd->StaCfg.ReqVarIELen*2) + 17 > MAX_CUSTOM_LEN\n"));
+#endif
+
+	return 0;
+
+}
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+
diff --git a/drivers/staging/rt2860/sta/auth.c b/drivers/staging/rt2860/sta/auth.c
new file mode 100644
index 00000000000..73fb8d6ea76
--- /dev/null
+++ b/drivers/staging/rt2860/sta/auth.c
@@ -0,0 +1,474 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	auth.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John		2004-9-3		porting from RT2500
+*/
+#include "../rt_config.h"
+
+/*
+    ==========================================================================
+    Description:
+        authenticate state machine init, including state transition and timer init
+    Parameters:
+        Sm - pointer to the auth state machine
+    Note:
+        The state machine looks like this
+
+                        AUTH_REQ_IDLE           AUTH_WAIT_SEQ2                   AUTH_WAIT_SEQ4
+    MT2_MLME_AUTH_REQ   mlme_auth_req_action    invalid_state_when_auth          invalid_state_when_auth
+    MT2_PEER_AUTH_EVEN  drop                    peer_auth_even_at_seq2_action    peer_auth_even_at_seq4_action
+    MT2_AUTH_TIMEOUT    Drop                    auth_timeout_action              auth_timeout_action
+
+	IRQL = PASSIVE_LEVEL
+
+    ==========================================================================
+ */
+
+void AuthStateMachineInit(
+    IN PRTMP_ADAPTER pAd,
+    IN STATE_MACHINE *Sm,
+    OUT STATE_MACHINE_FUNC Trans[])
+{
+    StateMachineInit(Sm, Trans, MAX_AUTH_STATE, MAX_AUTH_MSG, (STATE_MACHINE_FUNC)Drop, AUTH_REQ_IDLE, AUTH_MACHINE_BASE);
+
+    // the first column
+    StateMachineSetAction(Sm, AUTH_REQ_IDLE, MT2_MLME_AUTH_REQ, (STATE_MACHINE_FUNC)MlmeAuthReqAction);
+
+    // the second column
+    StateMachineSetAction(Sm, AUTH_WAIT_SEQ2, MT2_MLME_AUTH_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenAuth);
+    StateMachineSetAction(Sm, AUTH_WAIT_SEQ2, MT2_PEER_AUTH_EVEN, (STATE_MACHINE_FUNC)PeerAuthRspAtSeq2Action);
+    StateMachineSetAction(Sm, AUTH_WAIT_SEQ2, MT2_AUTH_TIMEOUT, (STATE_MACHINE_FUNC)AuthTimeoutAction);
+
+    // the third column
+    StateMachineSetAction(Sm, AUTH_WAIT_SEQ4, MT2_MLME_AUTH_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenAuth);
+    StateMachineSetAction(Sm, AUTH_WAIT_SEQ4, MT2_PEER_AUTH_EVEN, (STATE_MACHINE_FUNC)PeerAuthRspAtSeq4Action);
+    StateMachineSetAction(Sm, AUTH_WAIT_SEQ4, MT2_AUTH_TIMEOUT, (STATE_MACHINE_FUNC)AuthTimeoutAction);
+
+	RTMPInitTimer(pAd, &pAd->MlmeAux.AuthTimer, GET_TIMER_FUNCTION(AuthTimeout), pAd, FALSE);
+}
+
+/*
+    ==========================================================================
+    Description:
+        function to be executed at timer thread when auth timer expires
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID AuthTimeout(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3)
+{
+    RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+
+    DBGPRINT(RT_DEBUG_TRACE,("AUTH - AuthTimeout\n"));
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return;
+
+	// send a de-auth to reset AP's state machine (Patch AP-Dir635)
+	if (pAd->Mlme.AuthMachine.CurrState == AUTH_WAIT_SEQ2)
+		Cls2errAction(pAd, pAd->MlmeAux.Bssid);
+
+
+    MlmeEnqueue(pAd, AUTH_STATE_MACHINE, MT2_AUTH_TIMEOUT, 0, NULL);
+    RT28XX_MLME_HANDLER(pAd);
+}
+
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID MlmeAuthReqAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+    UCHAR              Addr[6];
+    USHORT             Alg, Seq, Status;
+    ULONG              Timeout;
+    HEADER_802_11      AuthHdr;
+    BOOLEAN            TimerCancelled;
+    NDIS_STATUS        NStatus;
+    PUCHAR             pOutBuffer = NULL;
+    ULONG              FrameLen = 0;
+
+	// Block all authentication request durning WPA block period
+	if (pAd->StaCfg.bBlockAssoc == TRUE)
+	{
+        DBGPRINT(RT_DEBUG_TRACE, ("AUTH - Block Auth request durning WPA block period!\n"));
+        pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+        Status = MLME_STATE_MACHINE_REJECT;
+        MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_AUTH_CONF, 2, &Status);
+	}
+    else if(MlmeAuthReqSanity(pAd, Elem->Msg, Elem->MsgLen, Addr, &Timeout, &Alg))
+    {
+        // reset timer
+        RTMPCancelTimer(&pAd->MlmeAux.AuthTimer, &TimerCancelled);
+        COPY_MAC_ADDR(pAd->MlmeAux.Bssid, Addr);
+        pAd->MlmeAux.Alg  = Alg;
+        Seq = 1;
+        Status = MLME_SUCCESS;
+
+        NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+        if(NStatus != NDIS_STATUS_SUCCESS)
+        {
+            DBGPRINT(RT_DEBUG_TRACE, ("AUTH - MlmeAuthReqAction(Alg:%d) allocate memory failed\n", Alg));
+            pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+            Status = MLME_FAIL_NO_RESOURCE;
+            MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_AUTH_CONF, 2, &Status);
+            return;
+        }
+
+        DBGPRINT(RT_DEBUG_TRACE, ("AUTH - Send AUTH request seq#1 (Alg=%d)...\n", Alg));
+        MgtMacHeaderInit(pAd, &AuthHdr, SUBTYPE_AUTH, 0, Addr, pAd->MlmeAux.Bssid);
+        MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+                          sizeof(HEADER_802_11),&AuthHdr,
+                          2,                    &Alg,
+                          2,                    &Seq,
+                          2,                    &Status,
+                          END_OF_ARGS);
+        MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+    	MlmeFreeMemory(pAd, pOutBuffer);
+
+        RTMPSetTimer(&pAd->MlmeAux.AuthTimer, Timeout);
+        pAd->Mlme.AuthMachine.CurrState = AUTH_WAIT_SEQ2;
+    }
+    else
+    {
+        DBGPRINT_ERR(("AUTH - MlmeAuthReqAction() sanity check failed\n"));
+        pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+        Status = MLME_INVALID_FORMAT;
+        MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_AUTH_CONF, 2, &Status);
+    }
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID PeerAuthRspAtSeq2Action(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+    UCHAR         Addr2[MAC_ADDR_LEN];
+    USHORT        Seq, Status, RemoteStatus, Alg;
+    UCHAR         ChlgText[CIPHER_TEXT_LEN];
+    UCHAR         CyperChlgText[CIPHER_TEXT_LEN + 8 + 8];
+    UCHAR         Element[2];
+    HEADER_802_11 AuthHdr;
+    BOOLEAN       TimerCancelled;
+    PUCHAR        pOutBuffer = NULL;
+    NDIS_STATUS   NStatus;
+    ULONG         FrameLen = 0;
+    USHORT        Status2;
+
+    if (PeerAuthSanity(pAd, Elem->Msg, Elem->MsgLen, Addr2, &Alg, &Seq, &Status, ChlgText))
+    {
+        if (MAC_ADDR_EQUAL(pAd->MlmeAux.Bssid, Addr2) && Seq == 2)
+        {
+            DBGPRINT(RT_DEBUG_TRACE, ("AUTH - Receive AUTH_RSP seq#2 to me (Alg=%d, Status=%d)\n", Alg, Status));
+            RTMPCancelTimer(&pAd->MlmeAux.AuthTimer, &TimerCancelled);
+
+            if (Status == MLME_SUCCESS)
+            {
+                // Authentication Mode "LEAP" has allow for CCX 1.X
+                if ((pAd->MlmeAux.Alg == Ndis802_11AuthModeOpen)
+#ifdef LEAP_SUPPORT
+					|| (pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP)
+#endif // LEAP_SUPPORT //
+				)
+                {
+                    pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+#ifdef LEAP_SUPPORT
+                    pAd->Mlme.LeapMachine.CurrState = LEAP_IDLE;
+#endif // LEAP_SUPPORT //
+                    MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_AUTH_CONF, 2, &Status);
+                }
+                else
+                {
+                    // 2. shared key, need to be challenged
+                    Seq++;
+                    RemoteStatus = MLME_SUCCESS;
+
+					// Get an unused nonpaged memory
+                    NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);
+                    if(NStatus != NDIS_STATUS_SUCCESS)
+                    {
+                        DBGPRINT(RT_DEBUG_TRACE, ("AUTH - PeerAuthRspAtSeq2Action() allocate memory fail\n"));
+                        pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+                        Status2 = MLME_FAIL_NO_RESOURCE;
+                        MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_AUTH_CONF, 2, &Status2);
+                        return;
+                    }
+
+                    DBGPRINT(RT_DEBUG_TRACE, ("AUTH - Send AUTH request seq#3...\n"));
+                    MgtMacHeaderInit(pAd, &AuthHdr, SUBTYPE_AUTH, 0, Addr2, pAd->MlmeAux.Bssid);
+                    AuthHdr.FC.Wep = 1;
+                    // Encrypt challenge text & auth information
+                    RTMPInitWepEngine(
+                    	pAd,
+                    	pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key,
+                    	pAd->StaCfg.DefaultKeyId,
+                    	pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].KeyLen,
+                    	CyperChlgText);
+
+					Alg = cpu2le16(*(USHORT *)&Alg);
+					Seq = cpu2le16(*(USHORT *)&Seq);
+					RemoteStatus= cpu2le16(*(USHORT *)&RemoteStatus);
+
+					RTMPEncryptData(pAd, (PUCHAR) &Alg, CyperChlgText + 4, 2);
+					RTMPEncryptData(pAd, (PUCHAR) &Seq, CyperChlgText + 6, 2);
+					RTMPEncryptData(pAd, (PUCHAR) &RemoteStatus, CyperChlgText + 8, 2);
+					Element[0] = 16;
+					Element[1] = 128;
+					RTMPEncryptData(pAd, Element, CyperChlgText + 10, 2);
+					RTMPEncryptData(pAd, ChlgText, CyperChlgText + 12, 128);
+					RTMPSetICV(pAd, CyperChlgText + 140);
+                    MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+                                      sizeof(HEADER_802_11),    &AuthHdr,
+                                      CIPHER_TEXT_LEN + 16,     CyperChlgText,
+                                      END_OF_ARGS);
+                    MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+                	MlmeFreeMemory(pAd, pOutBuffer);
+
+                    RTMPSetTimer(&pAd->MlmeAux.AuthTimer, AUTH_TIMEOUT);
+                    pAd->Mlme.AuthMachine.CurrState = AUTH_WAIT_SEQ4;
+                }
+            }
+            else
+            {
+#ifdef LEAP_SUPPORT
+                if (pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP)
+                {
+                    //Invalid Authentication possible rogue AP
+                    //Add this Ap to Rogue AP.
+                    RogueApTableSetEntry(pAd, &pAd->StaCfg.RogueApTab, Addr2, LEAP_REASON_INVALID_AUTH);
+				}
+#endif // LEAP_SUPPORT //
+                pAd->StaCfg.AuthFailReason = Status;
+                COPY_MAC_ADDR(pAd->StaCfg.AuthFailSta, Addr2);
+                pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+                MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_AUTH_CONF, 2, &Status);
+            }
+        }
+    }
+    else
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("AUTH - PeerAuthSanity() sanity check fail\n"));
+    }
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID PeerAuthRspAtSeq4Action(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+    UCHAR         Addr2[MAC_ADDR_LEN];
+    USHORT        Alg, Seq, Status;
+    CHAR          ChlgText[CIPHER_TEXT_LEN];
+    BOOLEAN       TimerCancelled;
+
+    if(PeerAuthSanity(pAd, Elem->Msg, Elem->MsgLen, Addr2, &Alg, &Seq, &Status, ChlgText))
+    {
+        if(MAC_ADDR_EQUAL(pAd->MlmeAux.Bssid, Addr2) && Seq == 4)
+        {
+            DBGPRINT(RT_DEBUG_TRACE, ("AUTH - Receive AUTH_RSP seq#4 to me\n"));
+            RTMPCancelTimer(&pAd->MlmeAux.AuthTimer, &TimerCancelled);
+
+            if (Status != MLME_SUCCESS)
+            {
+                pAd->StaCfg.AuthFailReason = Status;
+                COPY_MAC_ADDR(pAd->StaCfg.AuthFailSta, Addr2);
+            }
+
+            pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+            MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_AUTH_CONF, 2, &Status);
+        }
+    }
+    else
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("AUTH - PeerAuthRspAtSeq4Action() sanity check fail\n"));
+    }
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID MlmeDeauthReqAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+    MLME_DEAUTH_REQ_STRUCT *pInfo;
+    HEADER_802_11 DeauthHdr;
+    PUCHAR        pOutBuffer = NULL;
+    NDIS_STATUS   NStatus;
+    ULONG         FrameLen = 0;
+    USHORT        Status;
+
+    pInfo = (MLME_DEAUTH_REQ_STRUCT *)Elem->Msg;
+
+    NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+    if (NStatus != NDIS_STATUS_SUCCESS)
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("AUTH - MlmeDeauthReqAction() allocate memory fail\n"));
+        pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+        Status = MLME_FAIL_NO_RESOURCE;
+        MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_DEAUTH_CONF, 2, &Status);
+        return;
+    }
+
+    DBGPRINT(RT_DEBUG_TRACE, ("AUTH - Send DE-AUTH request (Reason=%d)...\n", pInfo->Reason));
+    MgtMacHeaderInit(pAd, &DeauthHdr, SUBTYPE_DEAUTH, 0, pInfo->Addr, pAd->MlmeAux.Bssid);
+    MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+                      sizeof(HEADER_802_11),&DeauthHdr,
+                      2,                    &pInfo->Reason,
+                      END_OF_ARGS);
+    MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+    pAd->StaCfg.DeauthReason = pInfo->Reason;
+    COPY_MAC_ADDR(pAd->StaCfg.DeauthSta, pInfo->Addr);
+    pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+    Status = MLME_SUCCESS;
+    MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_DEAUTH_CONF, 2, &Status);
+
+	// send wireless event - for deauthentication
+	if (pAd->CommonCfg.bWirelessEvent)
+		RTMPSendWirelessEvent(pAd, IW_DEAUTH_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID AuthTimeoutAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+    USHORT Status;
+    DBGPRINT(RT_DEBUG_TRACE, ("AUTH - AuthTimeoutAction\n"));
+    pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+    Status = MLME_REJ_TIMEOUT;
+    MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_AUTH_CONF, 2, &Status);
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID InvalidStateWhenAuth(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+    USHORT Status;
+    DBGPRINT(RT_DEBUG_TRACE, ("AUTH - InvalidStateWhenAuth (state=%ld), reset AUTH state machine\n", pAd->Mlme.AuthMachine.CurrState));
+    pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+    Status = MLME_STATE_MACHINE_REJECT;
+    MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_AUTH_CONF, 2, &Status);
+}
+
+/*
+    ==========================================================================
+    Description:
+        Some STA/AP
+    Note:
+        This action should never trigger AUTH state transition, therefore we
+        separate it from AUTH state machine, and make it as a standalone service
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID Cls2errAction(
+    IN PRTMP_ADAPTER pAd,
+    IN PUCHAR pAddr)
+{
+    HEADER_802_11 DeauthHdr;
+    PUCHAR        pOutBuffer = NULL;
+    NDIS_STATUS   NStatus;
+    ULONG         FrameLen = 0;
+    USHORT        Reason = REASON_CLS2ERR;
+
+    NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+    if (NStatus != NDIS_STATUS_SUCCESS)
+        return;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("AUTH - Class 2 error, Send DEAUTH frame...\n"));
+    MgtMacHeaderInit(pAd, &DeauthHdr, SUBTYPE_DEAUTH, 0, pAddr, pAd->MlmeAux.Bssid);
+    MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+                      sizeof(HEADER_802_11),&DeauthHdr,
+                      2,                    &Reason,
+                      END_OF_ARGS);
+    MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+    pAd->StaCfg.DeauthReason = Reason;
+    COPY_MAC_ADDR(pAd->StaCfg.DeauthSta, pAddr);
+}
+
+
diff --git a/drivers/staging/rt2860/sta/auth_rsp.c b/drivers/staging/rt2860/sta/auth_rsp.c
new file mode 100644
index 00000000000..f7aa4b99cf5
--- /dev/null
+++ b/drivers/staging/rt2860/sta/auth_rsp.c
@@ -0,0 +1,167 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	auth_rsp.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John		2004-10-1		copy from RT2560
+*/
+#include "../rt_config.h"
+
+/*
+    ==========================================================================
+    Description:
+        authentication state machine init procedure
+    Parameters:
+        Sm - the state machine
+
+	IRQL = PASSIVE_LEVEL
+
+    ==========================================================================
+ */
+VOID AuthRspStateMachineInit(
+    IN PRTMP_ADAPTER pAd,
+    IN PSTATE_MACHINE Sm,
+    IN STATE_MACHINE_FUNC Trans[])
+{
+    StateMachineInit(Sm, Trans, MAX_AUTH_RSP_STATE, MAX_AUTH_RSP_MSG, (STATE_MACHINE_FUNC)Drop, AUTH_RSP_IDLE, AUTH_RSP_MACHINE_BASE);
+
+    // column 1
+    StateMachineSetAction(Sm, AUTH_RSP_IDLE, MT2_PEER_DEAUTH, (STATE_MACHINE_FUNC)PeerDeauthAction);
+
+    // column 2
+    StateMachineSetAction(Sm, AUTH_RSP_WAIT_CHAL, MT2_PEER_DEAUTH, (STATE_MACHINE_FUNC)PeerDeauthAction);
+
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+*/
+VOID PeerAuthSimpleRspGenAndSend(
+    IN PRTMP_ADAPTER pAd,
+    IN PHEADER_802_11 pHdr80211,
+    IN USHORT Alg,
+    IN USHORT Seq,
+    IN USHORT Reason,
+    IN USHORT Status)
+{
+    HEADER_802_11     AuthHdr;
+    ULONG             FrameLen = 0;
+    PUCHAR            pOutBuffer = NULL;
+    NDIS_STATUS       NStatus;
+
+    if (Reason != MLME_SUCCESS)
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("Peer AUTH fail...\n"));
+        return;
+    }
+
+	//Get an unused nonpaged memory
+    NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);
+    if (NStatus != NDIS_STATUS_SUCCESS)
+        return;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Send AUTH response (seq#2)...\n"));
+    MgtMacHeaderInit(pAd, &AuthHdr, SUBTYPE_AUTH, 0, pHdr80211->Addr2, pAd->MlmeAux.Bssid);
+    MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+                      sizeof(HEADER_802_11),    &AuthHdr,
+                      2,                        &Alg,
+                      2,                        &Seq,
+                      2,                        &Reason,
+                      END_OF_ARGS);
+    MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+*/
+VOID PeerDeauthAction(
+    IN PRTMP_ADAPTER pAd,
+    IN PMLME_QUEUE_ELEM Elem)
+{
+    UCHAR       Addr2[MAC_ADDR_LEN];
+    USHORT      Reason;
+
+    if (PeerDeauthSanity(pAd, Elem->Msg, Elem->MsgLen, Addr2, &Reason))
+    {
+        if (INFRA_ON(pAd) && MAC_ADDR_EQUAL(Addr2, pAd->CommonCfg.Bssid))
+        {
+            DBGPRINT(RT_DEBUG_TRACE,("AUTH_RSP - receive DE-AUTH from our AP (Reason=%d)\n", Reason));
+
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+            {
+                union iwreq_data    wrqu;
+                memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+                wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+            }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+
+
+			// send wireless event - for deauthentication
+			if (pAd->CommonCfg.bWirelessEvent)
+				RTMPSendWirelessEvent(pAd, IW_DEAUTH_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+
+            LinkDown(pAd, TRUE);
+
+            // Authentication Mode Cisco_LEAP has start a timer
+            // We should cancel it if using LEAP
+#ifdef LEAP_SUPPORT
+            if (pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP)
+            {
+                RTMPCancelTimer(&pAd->StaCfg.LeapAuthTimer, &TimerCancelled);
+                //Check is it mach the LEAP Authentication failed as possible a Rogue AP
+                //on it's PortSecured not equal to WPA_802_1X_PORT_SECURED while process the Authenticaton.
+                if ((pAd->StaCfg.PortSecured != WPA_802_1X_PORT_SECURED) && (pAd->Mlme.LeapMachine.CurrState != LEAP_IDLE))
+                {
+                    RogueApTableSetEntry(pAd, &pAd->StaCfg.RogueApTab, Addr2, LEAP_REASON_AUTH_TIMEOUT);
+                }
+            }
+#endif // LEAP_SUPPORT //
+        }
+    }
+    else
+    {
+        DBGPRINT(RT_DEBUG_TRACE,("AUTH_RSP - PeerDeauthAction() sanity check fail\n"));
+    }
+}
+
diff --git a/drivers/staging/rt2860/sta/connect.c b/drivers/staging/rt2860/sta/connect.c
new file mode 100644
index 00000000000..36f28f8b4aa
--- /dev/null
+++ b/drivers/staging/rt2860/sta/connect.c
@@ -0,0 +1,2751 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	connect.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John			2004-08-08			Major modification from RT2560
+*/
+#include "../rt_config.h"
+
+UCHAR	CipherSuiteWpaNoneTkip[] = {
+		0x00, 0x50, 0xf2, 0x01,	// oui
+		0x01, 0x00,				// Version
+		0x00, 0x50, 0xf2, 0x02,	// Multicast
+		0x01, 0x00,				// Number of unicast
+		0x00, 0x50, 0xf2, 0x02,	// unicast
+		0x01, 0x00,				// number of authentication method
+		0x00, 0x50, 0xf2, 0x00	// authentication
+		};
+UCHAR	CipherSuiteWpaNoneTkipLen = (sizeof(CipherSuiteWpaNoneTkip) / sizeof(UCHAR));
+
+UCHAR	CipherSuiteWpaNoneAes[] = {
+		0x00, 0x50, 0xf2, 0x01,	// oui
+		0x01, 0x00,				// Version
+		0x00, 0x50, 0xf2, 0x04,	// Multicast
+		0x01, 0x00,				// Number of unicast
+		0x00, 0x50, 0xf2, 0x04,	// unicast
+		0x01, 0x00,				// number of authentication method
+		0x00, 0x50, 0xf2, 0x00	// authentication
+		};
+UCHAR	CipherSuiteWpaNoneAesLen = (sizeof(CipherSuiteWpaNoneAes) / sizeof(UCHAR));
+
+// The following MACRO is called after 1. starting an new IBSS, 2. succesfully JOIN an IBSS,
+// or 3. succesfully ASSOCIATE to a BSS, 4. successfully RE_ASSOCIATE to a BSS
+// All settings successfuly negotiated furing MLME state machines become final settings
+// and are copied to pAd->StaActive
+#define COPY_SETTINGS_FROM_MLME_AUX_TO_ACTIVE_CFG(_pAd)                                 \
+{                                                                                       \
+	(_pAd)->CommonCfg.SsidLen = (_pAd)->MlmeAux.SsidLen;                                \
+	NdisMoveMemory((_pAd)->CommonCfg.Ssid, (_pAd)->MlmeAux.Ssid, (_pAd)->MlmeAux.SsidLen); \
+	COPY_MAC_ADDR((_pAd)->CommonCfg.Bssid, (_pAd)->MlmeAux.Bssid);                      \
+	(_pAd)->CommonCfg.Channel = (_pAd)->MlmeAux.Channel;                                \
+	(_pAd)->CommonCfg.CentralChannel = (_pAd)->MlmeAux.CentralChannel;                  \
+	(_pAd)->StaActive.Aid = (_pAd)->MlmeAux.Aid;                                        \
+	(_pAd)->StaActive.AtimWin = (_pAd)->MlmeAux.AtimWin;                                \
+	(_pAd)->StaActive.CapabilityInfo = (_pAd)->MlmeAux.CapabilityInfo;                  \
+	(_pAd)->CommonCfg.BeaconPeriod = (_pAd)->MlmeAux.BeaconPeriod;                      \
+	(_pAd)->StaActive.CfpMaxDuration = (_pAd)->MlmeAux.CfpMaxDuration;                  \
+	(_pAd)->StaActive.CfpPeriod = (_pAd)->MlmeAux.CfpPeriod;                            \
+	(_pAd)->StaActive.SupRateLen = (_pAd)->MlmeAux.SupRateLen;                          \
+	NdisMoveMemory((_pAd)->StaActive.SupRate, (_pAd)->MlmeAux.SupRate, (_pAd)->MlmeAux.SupRateLen);\
+	(_pAd)->StaActive.ExtRateLen = (_pAd)->MlmeAux.ExtRateLen;                          \
+	NdisMoveMemory((_pAd)->StaActive.ExtRate, (_pAd)->MlmeAux.ExtRate, (_pAd)->MlmeAux.ExtRateLen);\
+	NdisMoveMemory(&(_pAd)->CommonCfg.APEdcaParm, &(_pAd)->MlmeAux.APEdcaParm, sizeof(EDCA_PARM));\
+	NdisMoveMemory(&(_pAd)->CommonCfg.APQosCapability, &(_pAd)->MlmeAux.APQosCapability, sizeof(QOS_CAPABILITY_PARM));\
+	NdisMoveMemory(&(_pAd)->CommonCfg.APQbssLoad, &(_pAd)->MlmeAux.APQbssLoad, sizeof(QBSS_LOAD_PARM));\
+	COPY_MAC_ADDR((_pAd)->MacTab.Content[BSSID_WCID].Addr, (_pAd)->MlmeAux.Bssid);      \
+	(_pAd)->MacTab.Content[BSSID_WCID].Aid = (_pAd)->MlmeAux.Aid;                       \
+	(_pAd)->MacTab.Content[BSSID_WCID].PairwiseKey.CipherAlg = (_pAd)->StaCfg.PairCipher;\
+	COPY_MAC_ADDR((_pAd)->MacTab.Content[BSSID_WCID].PairwiseKey.BssId, (_pAd)->MlmeAux.Bssid);\
+	(_pAd)->MacTab.Content[BSSID_WCID].RateLen = (_pAd)->StaActive.SupRateLen + (_pAd)->StaActive.ExtRateLen;\
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = PASSIVE_LEVEL
+
+	==========================================================================
+*/
+VOID MlmeCntlInit(
+	IN PRTMP_ADAPTER pAd,
+	IN STATE_MACHINE *S,
+	OUT STATE_MACHINE_FUNC Trans[])
+{
+	// Control state machine differs from other state machines, the interface
+	// follows the standard interface
+	pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID MlmeCntlMachinePerformAction(
+	IN PRTMP_ADAPTER pAd,
+	IN STATE_MACHINE *S,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	switch(pAd->Mlme.CntlMachine.CurrState)
+	{
+		case CNTL_IDLE:
+			{
+				CntlIdleProc(pAd, Elem);
+			}
+			break;
+		case CNTL_WAIT_DISASSOC:
+			CntlWaitDisassocProc(pAd, Elem);
+			break;
+		case CNTL_WAIT_JOIN:
+			CntlWaitJoinProc(pAd, Elem);
+			break;
+
+		// CNTL_WAIT_REASSOC is the only state in CNTL machine that does
+		// not triggered directly or indirectly by "RTMPSetInformation(OID_xxx)".
+		// Therefore not protected by NDIS's "only one outstanding OID request"
+		// rule. Which means NDIS may SET OID in the middle of ROAMing attempts.
+		// Current approach is to block new SET request at RTMPSetInformation()
+		// when CntlMachine.CurrState is not CNTL_IDLE
+		case CNTL_WAIT_REASSOC:
+			CntlWaitReassocProc(pAd, Elem);
+			break;
+
+		case CNTL_WAIT_START:
+			CntlWaitStartProc(pAd, Elem);
+			break;
+		case CNTL_WAIT_AUTH:
+			CntlWaitAuthProc(pAd, Elem);
+			break;
+		case CNTL_WAIT_AUTH2:
+			CntlWaitAuthProc2(pAd, Elem);
+			break;
+		case CNTL_WAIT_ASSOC:
+			CntlWaitAssocProc(pAd, Elem);
+			break;
+
+		case CNTL_WAIT_OID_LIST_SCAN:
+			if(Elem->MsgType == MT2_SCAN_CONF)
+			{
+				// Resume TxRing after SCANING complete. We hope the out-of-service time
+				// won't be too long to let upper layer time-out the waiting frames
+				RTMPResumeMsduTransmission(pAd);
+				if (pAd->StaCfg.CCXReqType != MSRN_TYPE_UNUSED)
+				{
+					// Cisco scan request is finished, prepare beacon report
+					MlmeEnqueue(pAd, AIRONET_STATE_MACHINE, MT2_AIRONET_SCAN_DONE, 0, NULL);
+				}
+				pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+
+                //
+				// Set LED status to previous status.
+				//
+				if (pAd->bLedOnScanning)
+				{
+					pAd->bLedOnScanning = FALSE;
+					RTMPSetLED(pAd, pAd->LedStatus);
+				}
+#ifdef DOT11N_DRAFT3
+				// AP sent a 2040Coexistence mgmt frame, then station perform a scan, and then send back the respone.
+				if (pAd->CommonCfg.BSSCoexist2040.field.InfoReq == 1)
+				{
+					Update2040CoexistFrameAndNotify(pAd, BSSID_WCID, TRUE);
+				}
+#endif // DOT11N_DRAFT3 //
+			}
+			break;
+
+		case CNTL_WAIT_OID_DISASSOC:
+			if (Elem->MsgType == MT2_DISASSOC_CONF)
+			{
+				LinkDown(pAd, FALSE);
+				pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+			}
+			break;
+		default:
+			DBGPRINT_ERR(("!ERROR! CNTL - Illegal message type(=%ld)", Elem->MsgType));
+			break;
+	}
+}
+
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlIdleProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	MLME_DISASSOC_REQ_STRUCT   DisassocReq;
+
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF))
+		return;
+
+	switch(Elem->MsgType)
+	{
+		case OID_802_11_SSID:
+			CntlOidSsidProc(pAd, Elem);
+			break;
+
+		case OID_802_11_BSSID:
+			CntlOidRTBssidProc(pAd,Elem);
+			break;
+
+		case OID_802_11_BSSID_LIST_SCAN:
+			CntlOidScanProc(pAd,Elem);
+			break;
+
+		case OID_802_11_DISASSOCIATE:
+#ifdef RALINK_ATE
+			if(ATE_ON(pAd))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+				break;
+			}
+#endif // RALINK_ATE //
+			DisassocParmFill(pAd, &DisassocReq, pAd->CommonCfg.Bssid, REASON_DISASSOC_STA_LEAVING);
+			MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_DISASSOC_REQ, sizeof(MLME_DISASSOC_REQ_STRUCT), &DisassocReq);
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_DISASSOC;
+#ifdef WPA_SUPPLICANT_SUPPORT
+            if (pAd->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_ENABLE_WITH_WEB_UI)
+#endif // WPA_SUPPLICANT_SUPPORT //
+            {
+    			// Set the AutoReconnectSsid to prevent it reconnect to old SSID
+    			// Since calling this indicate user don't want to connect to that SSID anymore.
+    			pAd->MlmeAux.AutoReconnectSsidLen= 32;
+    			NdisZeroMemory(pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.AutoReconnectSsidLen);
+            }
+			break;
+
+		case MT2_MLME_ROAMING_REQ:
+			CntlMlmeRoamingProc(pAd, Elem);
+			break;
+
+        case OID_802_11_MIC_FAILURE_REPORT_FRAME:
+            WpaMicFailureReportFrame(pAd, Elem);
+            break;
+
+#ifdef QOS_DLS_SUPPORT
+		case RT_OID_802_11_SET_DLS_PARAM:
+			CntlOidDLSSetupProc(pAd, Elem);
+			break;
+#endif // QOS_DLS_SUPPORT //
+
+		default:
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - Illegal message in CntlIdleProc(MsgType=%ld)\n",Elem->MsgType));
+			break;
+	}
+}
+
+VOID CntlOidScanProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	MLME_SCAN_REQ_STRUCT       ScanReq;
+	ULONG                      BssIdx = BSS_NOT_FOUND;
+	BSS_ENTRY                  CurrBss;
+
+#ifdef RALINK_ATE
+/* Disable scanning when ATE is running. */
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+
+	// record current BSS if network is connected.
+	// 2003-2-13 do not include current IBSS if this is the only STA in this IBSS.
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+	{
+		BssIdx = BssSsidTableSearch(&pAd->ScanTab, pAd->CommonCfg.Bssid, pAd->CommonCfg.Ssid, pAd->CommonCfg.SsidLen, pAd->CommonCfg.Channel);
+		if (BssIdx != BSS_NOT_FOUND)
+		{
+			NdisMoveMemory(&CurrBss, &pAd->ScanTab.BssEntry[BssIdx], sizeof(BSS_ENTRY));
+		}
+	}
+
+	// clean up previous SCAN result, add current BSS back to table if any
+	BssTableInit(&pAd->ScanTab);
+	if (BssIdx != BSS_NOT_FOUND)
+	{
+		// DDK Note: If the NIC is associated with a particular BSSID and SSID
+		//    that are not contained in the list of BSSIDs generated by this scan, the
+		//    BSSID description of the currently associated BSSID and SSID should be
+		//    appended to the list of BSSIDs in the NIC's database.
+		// To ensure this, we append this BSS as the first entry in SCAN result
+		NdisMoveMemory(&pAd->ScanTab.BssEntry[0], &CurrBss, sizeof(BSS_ENTRY));
+		pAd->ScanTab.BssNr = 1;
+	}
+
+	ScanParmFill(pAd, &ScanReq, "", 0, BSS_ANY, SCAN_ACTIVE);
+	MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_SCAN_REQ,
+		sizeof(MLME_SCAN_REQ_STRUCT), &ScanReq);
+	pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_LIST_SCAN;
+}
+
+/*
+	==========================================================================
+	Description:
+		Before calling this routine, user desired SSID should already been
+		recorded in CommonCfg.Ssid[]
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlOidSsidProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM * Elem)
+{
+	PNDIS_802_11_SSID          pOidSsid = (NDIS_802_11_SSID *)Elem->Msg;
+	MLME_DISASSOC_REQ_STRUCT   DisassocReq;
+	ULONG					   Now;
+
+	// Step 1. record the desired user settings to MlmeAux
+	NdisZeroMemory(pAd->MlmeAux.Ssid, MAX_LEN_OF_SSID);
+	NdisMoveMemory(pAd->MlmeAux.Ssid, pOidSsid->Ssid, pOidSsid->SsidLength);
+	pAd->MlmeAux.SsidLen = (UCHAR)pOidSsid->SsidLength;
+	NdisZeroMemory(pAd->MlmeAux.Bssid, MAC_ADDR_LEN);
+	pAd->MlmeAux.BssType = pAd->StaCfg.BssType;
+
+
+	//
+	// Update Reconnect Ssid, that user desired to connect.
+	//
+	NdisZeroMemory(pAd->MlmeAux.AutoReconnectSsid, MAX_LEN_OF_SSID);
+	NdisMoveMemory(pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen);
+	pAd->MlmeAux.AutoReconnectSsidLen = pAd->MlmeAux.SsidLen;
+
+	// step 2. find all matching BSS in the lastest SCAN result (inBssTab)
+	//    & log them into MlmeAux.SsidBssTab for later-on iteration. Sort by RSSI order
+	BssTableSsidSort(pAd, &pAd->MlmeAux.SsidBssTab, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("CntlOidSsidProc():CNTL - %d BSS of %d BSS match the desire (%d)SSID - %s\n",
+			pAd->MlmeAux.SsidBssTab.BssNr, pAd->ScanTab.BssNr, pAd->MlmeAux.SsidLen, pAd->MlmeAux.Ssid));
+	NdisGetSystemUpTime(&Now);
+
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED) &&
+		(pAd->CommonCfg.SsidLen == pAd->MlmeAux.SsidBssTab.BssEntry[0].SsidLen) &&
+		NdisEqualMemory(pAd->CommonCfg.Ssid, pAd->MlmeAux.SsidBssTab.BssEntry[0].Ssid, pAd->CommonCfg.SsidLen) &&
+		MAC_ADDR_EQUAL(pAd->CommonCfg.Bssid, pAd->MlmeAux.SsidBssTab.BssEntry[0].Bssid))
+	{
+		// Case 1. already connected with an AP who has the desired SSID
+		//         with highest RSSI
+
+		// Add checking Mode "LEAP" for CCX 1.0
+		if (((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
+			 (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+			 (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) ||
+			 (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
+#ifdef LEAP_SUPPORT
+			 || (pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP)
+#endif // LEAP_SUPPORT //
+			 ) &&
+			(pAd->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
+		{
+			// case 1.1 For WPA, WPA-PSK, if the 1x port is not secured, we have to redo
+			//          connection process
+			DBGPRINT(RT_DEBUG_TRACE, ("CntlOidSsidProc():CNTL - disassociate with current AP...\n"));
+			DisassocParmFill(pAd, &DisassocReq, pAd->CommonCfg.Bssid, REASON_DISASSOC_STA_LEAVING);
+			MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_DISASSOC_REQ,
+						sizeof(MLME_DISASSOC_REQ_STRUCT), &DisassocReq);
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_DISASSOC;
+		}
+		else if (pAd->bConfigChanged == TRUE)
+		{
+			// case 1.2 Important Config has changed, we have to reconnect to the same AP
+			DBGPRINT(RT_DEBUG_TRACE, ("CntlOidSsidProc():CNTL - disassociate with current AP Because config changed...\n"));
+			DisassocParmFill(pAd, &DisassocReq, pAd->CommonCfg.Bssid, REASON_DISASSOC_STA_LEAVING);
+			MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_DISASSOC_REQ,
+						sizeof(MLME_DISASSOC_REQ_STRUCT), &DisassocReq);
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_DISASSOC;
+		}
+		else
+		{
+			// case 1.3. already connected to the SSID with highest RSSI.
+			DBGPRINT(RT_DEBUG_TRACE, ("CntlOidSsidProc():CNTL - already with this BSSID. ignore this SET_SSID request\n"));
+			//
+			// (HCT 12.1) 1c_wlan_mediaevents required
+			// media connect events are indicated when associating with the same AP
+			//
+			if (INFRA_ON(pAd))
+			{
+				//
+				// Since MediaState already is NdisMediaStateConnected
+				// We just indicate the connect event again to meet the WHQL required.
+				//
+				pAd->IndicateMediaState = NdisMediaStateConnected;
+				RTMP_IndicateMediaState(pAd);
+                pAd->ExtraInfo = GENERAL_LINK_UP;   // Update extra information to link is up
+			}
+
+			pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+            {
+                union iwreq_data    wrqu;
+
+                memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+                memcpy(wrqu.ap_addr.sa_data, pAd->MlmeAux.Bssid, MAC_ADDR_LEN);
+                wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+
+            }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+		}
+	}
+	else if (INFRA_ON(pAd))
+	{
+		//
+		// For RT61
+		// [88888] OID_802_11_SSID should have returned NDTEST_WEP_AP2(Returned: )
+		// RT61 may lost SSID, and not connect to NDTEST_WEP_AP2 and will connect to NDTEST_WEP_AP2 by Autoreconnect
+		// But media status is connected, so the SSID not report correctly.
+		//
+		if (!SSID_EQUAL(pAd->CommonCfg.Ssid, pAd->CommonCfg.SsidLen, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen))
+		{
+			//
+			// Different SSID means not Roaming case, so we let LinkDown() to Indicate a disconnect event.
+			//
+			pAd->MlmeAux.CurrReqIsFromNdis = TRUE;
+		}
+		// case 2. active INFRA association existent
+		//    roaming is done within miniport driver, nothing to do with configuration
+		//    utility. so upon a new SET(OID_802_11_SSID) is received, we just
+		//    disassociate with the current associated AP,
+		//    then perform a new association with this new SSID, no matter the
+		//    new/old SSID are the same or not.
+		DBGPRINT(RT_DEBUG_TRACE, ("CntlOidSsidProc():CNTL - disassociate with current AP...\n"));
+		DisassocParmFill(pAd, &DisassocReq, pAd->CommonCfg.Bssid, REASON_DISASSOC_STA_LEAVING);
+		MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_DISASSOC_REQ,
+					sizeof(MLME_DISASSOC_REQ_STRUCT), &DisassocReq);
+		pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_DISASSOC;
+	}
+	else
+	{
+		if (ADHOC_ON(pAd))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("CntlOidSsidProc():CNTL - drop current ADHOC\n"));
+			LinkDown(pAd, FALSE);
+			OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED);
+			pAd->IndicateMediaState = NdisMediaStateDisconnected;
+			RTMP_IndicateMediaState(pAd);
+            pAd->ExtraInfo = GENERAL_LINK_DOWN;
+			DBGPRINT(RT_DEBUG_TRACE, ("CntlOidSsidProc():NDIS_STATUS_MEDIA_DISCONNECT Event C!\n"));
+		}
+
+		if ((pAd->MlmeAux.SsidBssTab.BssNr == 0) &&
+			(pAd->StaCfg.bAutoReconnect == TRUE) &&
+			(pAd->MlmeAux.BssType == BSS_INFRA) &&
+			(MlmeValidateSSID(pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen) == TRUE)
+			)
+		{
+			MLME_SCAN_REQ_STRUCT       ScanReq;
+
+			DBGPRINT(RT_DEBUG_TRACE, ("CntlOidSsidProc():CNTL - No matching BSS, start a new scan\n"));
+			ScanParmFill(pAd, &ScanReq, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen, BSS_ANY, SCAN_ACTIVE);
+			MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_SCAN_REQ, sizeof(MLME_SCAN_REQ_STRUCT), &ScanReq);
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_LIST_SCAN;
+			// Reset Missed scan number
+			pAd->StaCfg.LastScanTime = Now;
+		}
+		else
+		{
+			pAd->MlmeAux.BssIdx = 0;
+			IterateOnBssTab(pAd);
+		}
+	}
+}
+
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlOidRTBssidProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM * Elem)
+{
+	ULONG       BssIdx;
+	PUCHAR      pOidBssid = (PUCHAR)Elem->Msg;
+	MLME_DISASSOC_REQ_STRUCT    DisassocReq;
+	MLME_JOIN_REQ_STRUCT        JoinReq;
+
+#ifdef RALINK_ATE
+/* No need to perform this routine when ATE is running. */
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+	// record user desired settings
+	COPY_MAC_ADDR(pAd->MlmeAux.Bssid, pOidBssid);
+	pAd->MlmeAux.BssType = pAd->StaCfg.BssType;
+
+	//
+	// Update Reconnect Ssid, that user desired to connect.
+	//
+	NdisZeroMemory(pAd->MlmeAux.AutoReconnectSsid, MAX_LEN_OF_SSID);
+	pAd->MlmeAux.AutoReconnectSsidLen = pAd->MlmeAux.SsidLen;
+	NdisMoveMemory(pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen);
+
+	// find the desired BSS in the latest SCAN result table
+	BssIdx = BssTableSearch(&pAd->ScanTab, pOidBssid, pAd->MlmeAux.Channel);
+	if (BssIdx == BSS_NOT_FOUND)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("CNTL - BSSID not found. reply NDIS_STATUS_NOT_ACCEPTED\n"));
+		pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+		return;
+	}
+
+	// copy the matched BSS entry from ScanTab to MlmeAux.SsidBssTab. Why?
+	// Because we need this entry to become the JOIN target in later on SYNC state machine
+	pAd->MlmeAux.BssIdx = 0;
+	pAd->MlmeAux.SsidBssTab.BssNr = 1;
+	NdisMoveMemory(&pAd->MlmeAux.SsidBssTab.BssEntry[0], &pAd->ScanTab.BssEntry[BssIdx], sizeof(BSS_ENTRY));
+
+	// 2002-11-26 skip the following checking. i.e. if user wants to re-connect to same AP
+	//   we just follow normal procedure. The reason of user doing this may because he/she changed
+	//   AP to another channel, but we still received BEACON from it thus don't claim Link Down.
+	//   Since user knows he's changed AP channel, he'll re-connect again. By skipping the following
+	//   checking, we'll disassociate then re-do normal association with this AP at the new channel.
+	// 2003-1-6 Re-enable this feature based on microsoft requirement which prefer not to re-do
+	//   connection when setting the same BSSID.
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED) &&
+		MAC_ADDR_EQUAL(pAd->CommonCfg.Bssid, pOidBssid))
+	{
+		// already connected to the same BSSID, go back to idle state directly
+		DBGPRINT(RT_DEBUG_TRACE, ("CNTL - already in this BSSID. ignore this SET_BSSID request\n"));
+		pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+            {
+                union iwreq_data    wrqu;
+
+                memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+                memcpy(wrqu.ap_addr.sa_data, pAd->MlmeAux.Bssid, MAC_ADDR_LEN);
+                wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+
+            }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+	}
+	else
+	{
+		if (INFRA_ON(pAd))
+		{
+			// disassoc from current AP first
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - disassociate with current AP ...\n"));
+			DisassocParmFill(pAd, &DisassocReq, pAd->CommonCfg.Bssid, REASON_DISASSOC_STA_LEAVING);
+			MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_DISASSOC_REQ,
+						sizeof(MLME_DISASSOC_REQ_STRUCT), &DisassocReq);
+
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_DISASSOC;
+		}
+		else
+		{
+			if (ADHOC_ON(pAd))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("CNTL - drop current ADHOC\n"));
+				LinkDown(pAd, FALSE);
+				OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED);
+				pAd->IndicateMediaState = NdisMediaStateDisconnected;
+				RTMP_IndicateMediaState(pAd);
+                pAd->ExtraInfo = GENERAL_LINK_DOWN;
+				DBGPRINT(RT_DEBUG_TRACE, ("NDIS_STATUS_MEDIA_DISCONNECT Event C!\n"));
+			}
+
+			// Change the wepstatus to original wepstatus
+			pAd->StaCfg.WepStatus   = pAd->StaCfg.OrigWepStatus;
+			pAd->StaCfg.PairCipher  = pAd->StaCfg.OrigWepStatus;
+			pAd->StaCfg.GroupCipher = pAd->StaCfg.OrigWepStatus;
+
+			// Check cipher suite, AP must have more secured cipher than station setting
+			// Set the Pairwise and Group cipher to match the intended AP setting
+			// We can only connect to AP with less secured cipher setting
+			if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK))
+			{
+				pAd->StaCfg.GroupCipher = pAd->ScanTab.BssEntry[BssIdx].WPA.GroupCipher;
+
+				if (pAd->StaCfg.WepStatus == pAd->ScanTab.BssEntry[BssIdx].WPA.PairCipher)
+					pAd->StaCfg.PairCipher = pAd->ScanTab.BssEntry[BssIdx].WPA.PairCipher;
+				else if (pAd->ScanTab.BssEntry[BssIdx].WPA.PairCipherAux != Ndis802_11WEPDisabled)
+					pAd->StaCfg.PairCipher = pAd->ScanTab.BssEntry[BssIdx].WPA.PairCipherAux;
+				else	// There is no PairCipher Aux, downgrade our capability to TKIP
+					pAd->StaCfg.PairCipher = Ndis802_11Encryption2Enabled;
+			}
+			else if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK))
+			{
+				pAd->StaCfg.GroupCipher = pAd->ScanTab.BssEntry[BssIdx].WPA2.GroupCipher;
+
+				if (pAd->StaCfg.WepStatus == pAd->ScanTab.BssEntry[BssIdx].WPA2.PairCipher)
+					pAd->StaCfg.PairCipher = pAd->ScanTab.BssEntry[BssIdx].WPA2.PairCipher;
+				else if (pAd->ScanTab.BssEntry[BssIdx].WPA2.PairCipherAux != Ndis802_11WEPDisabled)
+					pAd->StaCfg.PairCipher = pAd->ScanTab.BssEntry[BssIdx].WPA2.PairCipherAux;
+				else	// There is no PairCipher Aux, downgrade our capability to TKIP
+					pAd->StaCfg.PairCipher = Ndis802_11Encryption2Enabled;
+
+				// RSN capability
+				pAd->StaCfg.RsnCapability = pAd->ScanTab.BssEntry[BssIdx].WPA2.RsnCapability;
+			}
+
+			// Set Mix cipher flag
+			pAd->StaCfg.bMixCipher = (pAd->StaCfg.PairCipher == pAd->StaCfg.GroupCipher) ? FALSE : TRUE;
+			if (pAd->StaCfg.bMixCipher == TRUE)
+			{
+				// If mix cipher, re-build RSNIE
+				RTMPMakeRSNIE(pAd, pAd->StaCfg.AuthMode, pAd->StaCfg.WepStatus, 0);
+			}
+			// No active association, join the BSS immediately
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - joining %02x:%02x:%02x:%02x:%02x:%02x ...\n",
+				pOidBssid[0],pOidBssid[1],pOidBssid[2],pOidBssid[3],pOidBssid[4],pOidBssid[5]));
+
+			JoinParmFill(pAd, &JoinReq, pAd->MlmeAux.BssIdx);
+			MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_JOIN_REQ, sizeof(MLME_JOIN_REQ_STRUCT), &JoinReq);
+
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_JOIN;
+		}
+	}
+}
+
+// Roaming is the only external request triggering CNTL state machine
+// despite of other "SET OID" operation. All "SET OID" related oerations
+// happen in sequence, because no other SET OID will be sent to this device
+// until the the previous SET operation is complete (successful o failed).
+// So, how do we quarantee this ROAMING request won't corrupt other "SET OID"?
+// or been corrupted by other "SET OID"?
+//
+// IRQL = DISPATCH_LEVEL
+VOID CntlMlmeRoamingProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	// TODO:
+	// AP in different channel may show lower RSSI than actual value??
+	// should we add a weighting factor to compensate it?
+	DBGPRINT(RT_DEBUG_TRACE,("CNTL - Roaming in MlmeAux.RoamTab...\n"));
+
+	NdisMoveMemory(&pAd->MlmeAux.SsidBssTab, &pAd->MlmeAux.RoamTab, sizeof(pAd->MlmeAux.RoamTab));
+	pAd->MlmeAux.SsidBssTab.BssNr = pAd->MlmeAux.RoamTab.BssNr;
+
+	BssTableSortByRssi(&pAd->MlmeAux.SsidBssTab);
+	pAd->MlmeAux.BssIdx = 0;
+	IterateOnBssTab(pAd);
+}
+
+#ifdef QOS_DLS_SUPPORT
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlOidDLSSetupProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	PRT_802_11_DLS		pDLS = (PRT_802_11_DLS)Elem->Msg;
+	MLME_DLS_REQ_STRUCT	MlmeDlsReq;
+	INT					i;
+	USHORT				reason = REASON_UNSPECIFY;
+
+	DBGPRINT(RT_DEBUG_TRACE,("CNTL - (OID set %02x:%02x:%02x:%02x:%02x:%02x with Valid=%d, Status=%d, TimeOut=%d, CountDownTimer=%d)\n",
+		pDLS->MacAddr[0], pDLS->MacAddr[1], pDLS->MacAddr[2], pDLS->MacAddr[3], pDLS->MacAddr[4], pDLS->MacAddr[5],
+		pDLS->Valid, pDLS->Status, pDLS->TimeOut, pDLS->CountDownTimer));
+
+	if (!pAd->CommonCfg.bDLSCapable)
+		return;
+
+	// DLS will not be supported when Adhoc mode
+	if (INFRA_ON(pAd))
+	{
+		for (i = 0; i < MAX_NUM_OF_DLS_ENTRY; i++)
+		{
+			if (pDLS->Valid && pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH) &&
+				(pDLS->TimeOut == pAd->StaCfg.DLSEntry[i].TimeOut) && MAC_ADDR_EQUAL(pDLS->MacAddr, pAd->StaCfg.DLSEntry[i].MacAddr))
+			{
+				// 1. Same setting, just drop it
+				DBGPRINT(RT_DEBUG_TRACE,("CNTL - setting unchanged\n"));
+				break;
+			}
+			else if (!pDLS->Valid && pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH) &&
+				MAC_ADDR_EQUAL(pDLS->MacAddr, pAd->StaCfg.DLSEntry[i].MacAddr))
+			{
+				// 2. Disable DLS link case, just tear down DLS link
+				reason = REASON_QOS_UNWANTED_MECHANISM;
+				pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+				pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+				DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+				MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_TEAR_DOWN, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+				DBGPRINT(RT_DEBUG_TRACE,("CNTL - start tear down procedure\n"));
+				break;
+			}
+			else if ((i < MAX_NUM_OF_DLS_ENTRY) && pDLS->Valid && !pAd->StaCfg.DLSEntry[i].Valid)
+			{
+				// 3. Enable case, start DLS setup procedure
+				NdisMoveMemory(&pAd->StaCfg.DLSEntry[i], pDLS, sizeof(RT_802_11_DLS_UI));
+
+				//Update countdown timer
+				pAd->StaCfg.DLSEntry[i].CountDownTimer = pAd->StaCfg.DLSEntry[i].TimeOut;
+				DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+				MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_REQ, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+				DBGPRINT(RT_DEBUG_TRACE,("CNTL - DLS setup case\n"));
+				break;
+			}
+			else if ((i < MAX_NUM_OF_DLS_ENTRY) && pDLS->Valid && pAd->StaCfg.DLSEntry[i].Valid &&
+				(pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH) && !MAC_ADDR_EQUAL(pDLS->MacAddr, pAd->StaCfg.DLSEntry[i].MacAddr))
+			{
+				// 4. update mac case, tear down old DLS and setup new DLS
+				reason = REASON_QOS_UNWANTED_MECHANISM;
+				pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+				pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+				DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+				MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_TEAR_DOWN, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+				NdisMoveMemory(&pAd->StaCfg.DLSEntry[i], pDLS, sizeof(RT_802_11_DLS_UI));
+				DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+				MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_REQ, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+				DBGPRINT(RT_DEBUG_TRACE,("CNTL - DLS tear down and restart case\n"));
+				break;
+			}
+			else if (pDLS->Valid && pAd->StaCfg.DLSEntry[i].Valid &&
+				MAC_ADDR_EQUAL(pDLS->MacAddr, pAd->StaCfg.DLSEntry[i].MacAddr) && (pAd->StaCfg.DLSEntry[i].TimeOut != pDLS->TimeOut))
+			{
+				// 5. update timeout case, start DLS setup procedure (no tear down)
+				pAd->StaCfg.DLSEntry[i].TimeOut	= pDLS->TimeOut;
+				//Update countdown timer
+				pAd->StaCfg.DLSEntry[i].CountDownTimer = pAd->StaCfg.DLSEntry[i].TimeOut;
+				DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+				MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_REQ, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+				DBGPRINT(RT_DEBUG_TRACE,("CNTL - DLS update timeout case\n"));
+				break;
+			}
+			else if (pDLS->Valid && pAd->StaCfg.DLSEntry[i].Valid &&
+				(pAd->StaCfg.DLSEntry[i].Status != DLS_FINISH) && MAC_ADDR_EQUAL(pDLS->MacAddr, pAd->StaCfg.DLSEntry[i].MacAddr))
+			{
+				// 6. re-setup case, start DLS setup procedure (no tear down)
+				DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+				MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_REQ, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+				DBGPRINT(RT_DEBUG_TRACE,("CNTL - DLS retry setup procedure\n"));
+				break;
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_WARN,("CNTL - DLS not changed in entry - %d - Valid=%d, Status=%d, TimeOut=%d\n",
+					i, pAd->StaCfg.DLSEntry[i].Valid, pAd->StaCfg.DLSEntry[i].Status, pAd->StaCfg.DLSEntry[i].TimeOut));
+			}
+		}
+	}
+}
+#endif // QOS_DLS_SUPPORT //
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlWaitDisassocProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	MLME_START_REQ_STRUCT     StartReq;
+
+	if (Elem->MsgType == MT2_DISASSOC_CONF)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("CNTL - Dis-associate successful\n"));
+
+	    if (pAd->CommonCfg.bWirelessEvent)
+		{
+			RTMPSendWirelessEvent(pAd, IW_DISASSOC_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+		}
+
+		LinkDown(pAd, FALSE);
+
+		// case 1. no matching BSS, and user wants ADHOC, so we just start a new one
+		if ((pAd->MlmeAux.SsidBssTab.BssNr==0) && (pAd->StaCfg.BssType == BSS_ADHOC))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - No matching BSS, start a new ADHOC (Ssid=%s)...\n",pAd->MlmeAux.Ssid));
+			StartParmFill(pAd, &StartReq, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen);
+			MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_START_REQ, sizeof(MLME_START_REQ_STRUCT), &StartReq);
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_START;
+		}
+		// case 2. try each matched BSS
+		else
+		{
+			pAd->MlmeAux.BssIdx = 0;
+
+			IterateOnBssTab(pAd);
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlWaitJoinProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT                      Reason;
+	MLME_AUTH_REQ_STRUCT        AuthReq;
+
+	if (Elem->MsgType == MT2_JOIN_CONF)
+	{
+		NdisMoveMemory(&Reason, Elem->Msg, sizeof(USHORT));
+		if (Reason == MLME_SUCCESS)
+		{
+			// 1. joined an IBSS, we are pretty much done here
+			if (pAd->MlmeAux.BssType == BSS_ADHOC)
+			{
+			    //
+				// 5G bands rules of Japan:
+				// Ad hoc must be disabled in W53(ch52,56,60,64) channels.
+				//
+				if ( (pAd->CommonCfg.bIEEE80211H == 1) &&
+                      RadarChannelCheck(pAd, pAd->CommonCfg.Channel)
+				   )
+				{
+					pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+					DBGPRINT(RT_DEBUG_TRACE, ("CNTL - Channel=%d, Join adhoc on W53(52,56,60,64) Channels are not accepted\n", pAd->CommonCfg.Channel));
+					return;
+				}
+
+				LinkUp(pAd, BSS_ADHOC);
+				pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+				DBGPRINT(RT_DEBUG_TRACE, ("CNTL - join the IBSS = %02x:%02x:%02x:%02x:%02x:%02x ...\n",
+				pAd->CommonCfg.Bssid[0],pAd->CommonCfg.Bssid[1],pAd->CommonCfg.Bssid[2],
+				pAd->CommonCfg.Bssid[3],pAd->CommonCfg.Bssid[4],pAd->CommonCfg.Bssid[5]));
+
+                pAd->IndicateMediaState = NdisMediaStateConnected;
+                pAd->ExtraInfo = GENERAL_LINK_UP;
+			}
+			// 2. joined a new INFRA network, start from authentication
+			else
+			{
+#ifdef LEAP_SUPPORT
+				// Add AuthMode "LEAP" for CCX 1.X
+				if (pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP)
+				{
+					AuthParmFill(pAd, &AuthReq, pAd->MlmeAux.Bssid, CISCO_AuthModeLEAP);
+				}
+				else
+#endif // LEAP_SUPPORT //
+				{
+					// either Ndis802_11AuthModeShared or Ndis802_11AuthModeAutoSwitch, try shared key first
+					if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeShared) ||
+						(pAd->StaCfg.AuthMode == Ndis802_11AuthModeAutoSwitch))
+					{
+						AuthParmFill(pAd, &AuthReq, pAd->MlmeAux.Bssid, Ndis802_11AuthModeShared);
+					}
+					else
+					{
+						AuthParmFill(pAd, &AuthReq, pAd->MlmeAux.Bssid, Ndis802_11AuthModeOpen);
+					}
+				}
+				MlmeEnqueue(pAd, AUTH_STATE_MACHINE, MT2_MLME_AUTH_REQ,
+							sizeof(MLME_AUTH_REQ_STRUCT), &AuthReq);
+
+				pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_AUTH;
+			}
+		}
+		else
+		{
+			// 3. failed, try next BSS
+			pAd->MlmeAux.BssIdx++;
+			IterateOnBssTab(pAd);
+		}
+	}
+}
+
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlWaitStartProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT      Result;
+
+	if (Elem->MsgType == MT2_START_CONF)
+	{
+		NdisMoveMemory(&Result, Elem->Msg, sizeof(USHORT));
+		if (Result == MLME_SUCCESS)
+		{
+		    //
+			// 5G bands rules of Japan:
+			// Ad hoc must be disabled in W53(ch52,56,60,64) channels.
+			//
+			if ( (pAd->CommonCfg.bIEEE80211H == 1) &&
+                  RadarChannelCheck(pAd, pAd->CommonCfg.Channel)
+			   )
+			{
+				pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+				DBGPRINT(RT_DEBUG_TRACE, ("CNTL - Channel=%d, Start adhoc on W53(52,56,60,64) Channels are not accepted\n", pAd->CommonCfg.Channel));
+				return;
+			}
+#ifdef DOT11_N_SUPPORT
+			if (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED)
+			{
+				N_ChannelCheck(pAd);
+				SetCommonHT(pAd);
+				NdisMoveMemory(&pAd->MlmeAux.AddHtInfo, &pAd->CommonCfg.AddHTInfo, sizeof(ADD_HT_INFO_IE));
+				RTMPCheckHt(pAd, BSSID_WCID, &pAd->CommonCfg.HtCapability, &pAd->CommonCfg.AddHTInfo);
+				pAd->StaActive.SupportedPhyInfo.bHtEnable = TRUE;
+				NdisZeroMemory(&pAd->StaActive.SupportedPhyInfo.MCSSet[0], 16);
+				NdisMoveMemory(&pAd->StaActive.SupportedPhyInfo.MCSSet[0], &pAd->CommonCfg.HtCapability.MCSSet[0], 16);
+				COPY_HTSETTINGS_FROM_MLME_AUX_TO_ACTIVE_CFG(pAd);
+
+				if ((pAd->CommonCfg.HtCapability.HtCapInfo.ChannelWidth  == BW_40) &&
+					(pAd->CommonCfg.AddHTInfo.AddHtInfo.ExtChanOffset == EXTCHA_ABOVE))
+				{
+					pAd->MlmeAux.CentralChannel = pAd->CommonCfg.Channel + 2;
+				}
+				else if ((pAd->CommonCfg.HtCapability.HtCapInfo.ChannelWidth  == BW_40) &&
+						 (pAd->CommonCfg.AddHTInfo.AddHtInfo.ExtChanOffset == EXTCHA_BELOW))
+				{
+					pAd->MlmeAux.CentralChannel = pAd->CommonCfg.Channel - 2;
+				}
+			}
+			else
+#endif // DOT11_N_SUPPORT //
+			{
+				pAd->StaActive.SupportedPhyInfo.bHtEnable = FALSE;
+			}
+			LinkUp(pAd, BSS_ADHOC);
+			pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+			// Before send beacon, driver need do radar detection
+			if ((pAd->CommonCfg.Channel > 14 )
+				&& (pAd->CommonCfg.bIEEE80211H == 1)
+				&& RadarChannelCheck(pAd, pAd->CommonCfg.Channel))
+			{
+				pAd->CommonCfg.RadarDetect.RDMode = RD_SILENCE_MODE;
+				pAd->CommonCfg.RadarDetect.RDCount = 0;
+#ifdef DFS_SUPPORT
+				BbpRadarDetectionStart(pAd);
+#endif // DFS_SUPPORT //
+			}
+
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - start a new IBSS = %02x:%02x:%02x:%02x:%02x:%02x ...\n",
+				pAd->CommonCfg.Bssid[0],pAd->CommonCfg.Bssid[1],pAd->CommonCfg.Bssid[2],
+				pAd->CommonCfg.Bssid[3],pAd->CommonCfg.Bssid[4],pAd->CommonCfg.Bssid[5]));
+		}
+		else
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - Start IBSS fail. BUG!!!!!\n"));
+			pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlWaitAuthProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT                       Reason;
+	MLME_ASSOC_REQ_STRUCT        AssocReq;
+	MLME_AUTH_REQ_STRUCT         AuthReq;
+
+	if (Elem->MsgType == MT2_AUTH_CONF)
+	{
+		NdisMoveMemory(&Reason, Elem->Msg, sizeof(USHORT));
+		if (Reason == MLME_SUCCESS)
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - AUTH OK\n"));
+			AssocParmFill(pAd, &AssocReq, pAd->MlmeAux.Bssid, pAd->MlmeAux.CapabilityInfo,
+						  ASSOC_TIMEOUT, pAd->StaCfg.DefaultListenCount);
+
+#ifdef LEAP_SUPPORT
+			//
+			// Cisco Leap CCKM supported Re-association.
+			//
+			if (LEAP_CCKM_ON(pAd) && (pAd->StaCfg.CCKMLinkUpFlag == TRUE))
+			{
+				//if CCKM is turn on , that's mean Fast Reauthentication
+				//Use CCKM Reassociation instead of normal association for Fast Roaming.
+				MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_REASSOC_REQ,
+							sizeof(MLME_ASSOC_REQ_STRUCT), &AssocReq);
+
+				pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_REASSOC;
+			}
+			else
+#endif // LEAP_SUPPORT //
+			{
+				MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_ASSOC_REQ,
+							sizeof(MLME_ASSOC_REQ_STRUCT), &AssocReq);
+
+				pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_ASSOC;
+			}
+		}
+		else
+		{
+			// This fail may because of the AP already keep us in its MAC table without
+			// ageing-out. The previous authentication attempt must have let it remove us.
+			// so try Authentication again may help. For D-Link DWL-900AP+ compatibility.
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - AUTH FAIL, try again...\n"));
+#ifdef LEAP_SUPPORT
+			//Add AuthMode "LEAP" for CCX 1.X
+			if (pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP)
+			{
+				AuthParmFill(pAd, &AuthReq, pAd->MlmeAux.Bssid, CISCO_AuthModeLEAP);
+			}
+			else
+#endif // LEAP_SUPPORT //
+			{
+				if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeShared) ||
+					(pAd->StaCfg.AuthMode == Ndis802_11AuthModeAutoSwitch))
+				{
+					// either Ndis802_11AuthModeShared or Ndis802_11AuthModeAutoSwitch, try shared key first
+					AuthParmFill(pAd, &AuthReq, pAd->MlmeAux.Bssid, Ndis802_11AuthModeShared);
+				}
+				else
+				{
+					AuthParmFill(pAd, &AuthReq, pAd->MlmeAux.Bssid, Ndis802_11AuthModeOpen);
+				}
+			}
+			MlmeEnqueue(pAd, AUTH_STATE_MACHINE, MT2_MLME_AUTH_REQ,
+						sizeof(MLME_AUTH_REQ_STRUCT), &AuthReq);
+
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_AUTH2;
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlWaitAuthProc2(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT                       Reason;
+	MLME_ASSOC_REQ_STRUCT        AssocReq;
+	MLME_AUTH_REQ_STRUCT         AuthReq;
+
+	if (Elem->MsgType == MT2_AUTH_CONF)
+	{
+		NdisMoveMemory(&Reason, Elem->Msg, sizeof(USHORT));
+		if (Reason == MLME_SUCCESS)
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - AUTH OK\n"));
+			AssocParmFill(pAd, &AssocReq, pAd->MlmeAux.Bssid, pAd->MlmeAux.CapabilityInfo,
+						  ASSOC_TIMEOUT, pAd->StaCfg.DefaultListenCount);
+			MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_ASSOC_REQ,
+						sizeof(MLME_ASSOC_REQ_STRUCT), &AssocReq);
+
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_ASSOC;
+		}
+		else
+		{
+#ifdef LEAP_SUPPORT
+			// Process LEAP first, since it use different control variable
+			// We don't want to affect other poven operation
+			if (pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP)
+			{
+				// LEAP Auth not success, try next BSS
+				DBGPRINT(RT_DEBUG_TRACE, ("CNTL - *LEAP* AUTH FAIL, give up; try next BSS\n"));
+				DBGPRINT(RT_DEBUG_TRACE, ("Total match BSSID [=%d]\n", pAd->MlmeAux.SsidBssTab.BssNr));
+				pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+				pAd->MlmeAux.BssIdx++;
+				IterateOnBssTab(pAd);
+			}
+			else
+#endif // LEAP_SUPPORT //
+			if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeAutoSwitch) &&
+				 (pAd->MlmeAux.Alg == Ndis802_11AuthModeShared))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("CNTL - AUTH FAIL, try OPEN system...\n"));
+				AuthParmFill(pAd, &AuthReq, pAd->MlmeAux.Bssid, Ndis802_11AuthModeOpen);
+				MlmeEnqueue(pAd, AUTH_STATE_MACHINE, MT2_MLME_AUTH_REQ,
+							sizeof(MLME_AUTH_REQ_STRUCT), &AuthReq);
+
+				pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_AUTH2;
+			}
+			else
+			{
+				// not success, try next BSS
+				DBGPRINT(RT_DEBUG_TRACE, ("CNTL - AUTH FAIL, give up; try next BSS\n"));
+				pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE; //???????
+				pAd->MlmeAux.BssIdx++;
+				IterateOnBssTab(pAd);
+			}
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlWaitAssocProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT      Reason;
+
+	if (Elem->MsgType == MT2_ASSOC_CONF)
+	{
+		NdisMoveMemory(&Reason, Elem->Msg, sizeof(USHORT));
+		if (Reason == MLME_SUCCESS)
+		{
+			LinkUp(pAd, BSS_INFRA);
+			pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - Association successful on BSS #%ld\n",pAd->MlmeAux.BssIdx));
+
+			if (pAd->CommonCfg.bWirelessEvent)
+			{
+				RTMPSendWirelessEvent(pAd, IW_ASSOC_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+			}
+		}
+		else
+		{
+			// not success, try next BSS
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - Association fails on BSS #%ld\n",pAd->MlmeAux.BssIdx));
+			pAd->MlmeAux.BssIdx++;
+			IterateOnBssTab(pAd);
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlWaitReassocProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT      Result;
+
+	if (Elem->MsgType == MT2_REASSOC_CONF)
+	{
+		NdisMoveMemory(&Result, Elem->Msg, sizeof(USHORT));
+		if (Result == MLME_SUCCESS)
+		{
+			//
+			// NDIS requires a new Link UP indication but no Link Down for RE-ASSOC
+			//
+			LinkUp(pAd, BSS_INFRA);
+
+			// send wireless event - for association
+			if (pAd->CommonCfg.bWirelessEvent)
+				RTMPSendWirelessEvent(pAd, IW_ASSOC_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+
+
+#ifdef LEAP_SUPPORT
+			if (LEAP_CCKM_ON(pAd))
+			{
+				STA_PORT_SECURED(pAd);
+				pAd->StaCfg.WpaState = SS_FINISH;
+			}
+#endif // LEAP_SUPPORT //
+			pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - Re-assocition successful on BSS #%ld\n", pAd->MlmeAux.RoamIdx));
+		}
+		else
+		{
+			// reassoc failed, try to pick next BSS in the BSS Table
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - Re-assocition fails on BSS #%ld\n", pAd->MlmeAux.RoamIdx));
+			pAd->MlmeAux.RoamIdx++;
+			IterateOnBssTab2(pAd);
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID LinkUp(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR BssType)
+{
+	ULONG	Now;
+	UINT32	Data;
+	BOOLEAN	Cancelled;
+	UCHAR	Value = 0, idx;
+	MAC_TABLE_ENTRY *pEntry = NULL, *pCurrEntry;
+
+	pEntry = &pAd->MacTab.Content[BSSID_WCID];
+
+	//
+	// ASSOC - DisassocTimeoutAction
+	// CNTL - Dis-associate successful
+	// !!! LINK DOWN !!!
+	// [88888] OID_802_11_SSID should have returned NDTEST_WEP_AP2(Returned: )
+	//
+	// To prevent DisassocTimeoutAction to call Link down after we link up,
+	// cancel the DisassocTimer no matter what it start or not.
+	//
+	RTMPCancelTimer(&pAd->MlmeAux.DisassocTimer,  &Cancelled);
+
+	COPY_SETTINGS_FROM_MLME_AUX_TO_ACTIVE_CFG(pAd);
+
+#ifdef DOT11_N_SUPPORT
+	COPY_HTSETTINGS_FROM_MLME_AUX_TO_ACTIVE_CFG(pAd);
+#endif // DOT11_N_SUPPORT //
+	// It's quite difficult to tell if a newly added KEY is WEP or CKIP until a new BSS
+	// is formed (either ASSOC/RE-ASSOC done or IBSS started. LinkUP should be a safe place
+	// to examine if cipher algorithm switching is required.
+	//rt2860b. Don't know why need this
+	SwitchBetweenWepAndCkip(pAd);
+
+#ifdef RT2860
+	// Before power save before link up function, We will force use 1R.
+	// So after link up, check Rx antenna # again.
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &Value);
+	if(pAd->Antenna.field.RxPath == 3)
+	{
+		Value |= (0x10);
+	}
+	else if(pAd->Antenna.field.RxPath == 2)
+	{
+		Value |= (0x8);
+	}
+	else if(pAd->Antenna.field.RxPath == 1)
+	{
+		Value |= (0x0);
+	}
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, Value);
+	pAd->StaCfg.BBPR3 = Value;
+#endif // RT2860 //
+
+	if (BssType == BSS_ADHOC)
+	{
+		OPSTATUS_SET_FLAG(pAd, fOP_STATUS_ADHOC_ON);
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_INFRA_ON);
+
+#ifdef CARRIER_DETECTION_SUPPORT // Roger sync Carrier
+		// No carrier detection when adhoc
+		// CarrierDetectionStop(pAd);
+		pAd->CommonCfg.CarrierDetect.CD_State = CD_NORMAL;
+#endif // CARRIER_DETECTION_SUPPORT //
+
+		DBGPRINT(RT_DEBUG_TRACE, ("!!!Adhoc LINK UP !!! \n" ));
+	}
+	else
+	{
+		OPSTATUS_SET_FLAG(pAd, fOP_STATUS_INFRA_ON);
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_ADHOC_ON);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("!!!Infra LINK UP !!! \n" ));
+	}
+
+	// 3*3
+	// reset Tx beamforming bit
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &Value);
+	Value &= (~0x01);
+	Value |= pAd->CommonCfg.RegTransmitSetting.field.TxBF;
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, Value);
+
+#ifdef DOT11_N_SUPPORT
+	// Change to AP channel
+    if ((pAd->CommonCfg.CentralChannel > pAd->CommonCfg.Channel) && (pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth == BW_40))
+	{
+		// Must using 40MHz.
+		pAd->CommonCfg.BBPCurrentBW = BW_40;
+		AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+		AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &Value);
+		Value &= (~0x18);
+		Value |= 0x10;
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, Value);
+
+		//  RX : control channel at lower
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &Value);
+		Value &= (~0x20);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, Value);
+#ifdef RT2860
+        pAd->StaCfg.BBPR3 = Value;
+#endif // RT2860 //
+
+		RTMP_IO_READ32(pAd, TX_BAND_CFG, &Data);
+		Data &= 0xfffffffe;
+		RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Data);
+
+		if (pAd->MACVersion == 0x28600100)
+		{
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, 0x1A);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, 0x0A);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R73, 0x16);
+            DBGPRINT(RT_DEBUG_TRACE, ("!!!rt2860C !!! \n" ));
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("!!!40MHz Lower LINK UP !!! Control Channel at Below. Central = %d \n", pAd->CommonCfg.CentralChannel ));
+	}
+	else if ((pAd->CommonCfg.CentralChannel < pAd->CommonCfg.Channel) && (pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth == BW_40))
+    {
+	    // Must using 40MHz.
+		pAd->CommonCfg.BBPCurrentBW = BW_40;
+		AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+	    AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &Value);
+		Value &= (~0x18);
+		Value |= 0x10;
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, Value);
+
+		RTMP_IO_READ32(pAd, TX_BAND_CFG, &Data);
+		Data |= 0x1;
+		RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Data);
+
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &Value);
+	    Value |= (0x20);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, Value);
+#ifdef RT2860
+        pAd->StaCfg.BBPR3 = Value;
+#endif // RT2860 //
+
+		if (pAd->MACVersion == 0x28600100)
+		{
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, 0x1A);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, 0x0A);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R73, 0x16);
+			    DBGPRINT(RT_DEBUG_TRACE, ("!!!rt2860C !!! \n" ));
+		}
+
+	    DBGPRINT(RT_DEBUG_TRACE, ("!!! 40MHz Upper LINK UP !!! Control Channel at UpperCentral = %d \n", pAd->CommonCfg.CentralChannel ));
+    }
+    else
+#endif // DOT11_N_SUPPORT //
+    {
+	    pAd->CommonCfg.BBPCurrentBW = BW_20;
+		pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel;
+		AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+		AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &Value);
+		Value &= (~0x18);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, Value);
+
+		RTMP_IO_READ32(pAd, TX_BAND_CFG, &Data);
+		Data &= 0xfffffffe;
+		RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Data);
+
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &Value);
+		Value &= (~0x20);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, Value);
+#ifdef RT2860
+        pAd->StaCfg.BBPR3 = Value;
+#endif // RT2860 //
+
+		if (pAd->MACVersion == 0x28600100)
+		{
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, 0x16);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, 0x08);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R73, 0x11);
+            DBGPRINT(RT_DEBUG_TRACE, ("!!!rt2860C !!! \n" ));
+		}
+
+	    DBGPRINT(RT_DEBUG_TRACE, ("!!! 20MHz LINK UP !!! \n" ));
+    }
+
+	RTMPSetAGCInitValue(pAd, pAd->CommonCfg.BBPCurrentBW);
+	//
+	// Save BBP_R66 value, it will be used in RTUSBResumeMsduTransmission
+	//
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R66, &pAd->BbpTuning.R66CurrentValue);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("!!! LINK UP !!! (BssType=%d, AID=%d, ssid=%s, Channel=%d, CentralChannel = %d)\n",
+		BssType, pAd->StaActive.Aid, pAd->CommonCfg.Ssid, pAd->CommonCfg.Channel, pAd->CommonCfg.CentralChannel));
+
+#ifdef DOT11_N_SUPPORT
+	DBGPRINT(RT_DEBUG_TRACE, ("!!! LINK UP !!! (Density =%d, )\n", pAd->MacTab.Content[BSSID_WCID].MpduDensity));
+#endif // DOT11_N_SUPPORT //
+
+		AsicSetBssid(pAd, pAd->CommonCfg.Bssid);
+
+	AsicSetSlotTime(pAd, TRUE);
+	AsicSetEdcaParm(pAd, &pAd->CommonCfg.APEdcaParm);
+
+	// Call this for RTS protectionfor legacy rate, we will always enable RTS threshold, but normally it will not hit
+	AsicUpdateProtect(pAd, 0, (OFDMSETPROTECT | CCKSETPROTECT), TRUE, FALSE);
+
+#ifdef DOT11_N_SUPPORT
+	if ((pAd->StaActive.SupportedPhyInfo.bHtEnable == TRUE))
+	{
+		// Update HT protectionfor based on AP's operating mode.
+    	if (pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent == 1)
+    	{
+    		AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode,  ALLN_SETPROTECT, FALSE, TRUE);
+    	}
+    	else
+   			AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode,  ALLN_SETPROTECT, FALSE, FALSE);
+	}
+#endif // DOT11_N_SUPPORT //
+
+	NdisZeroMemory(&pAd->DrsCounters, sizeof(COUNTER_DRS));
+
+	NdisGetSystemUpTime(&Now);
+	pAd->StaCfg.LastBeaconRxTime = Now;   // last RX timestamp
+
+	if ((pAd->CommonCfg.TxPreamble != Rt802_11PreambleLong) &&
+		CAP_IS_SHORT_PREAMBLE_ON(pAd->StaActive.CapabilityInfo))
+	{
+		MlmeSetTxPreamble(pAd, Rt802_11PreambleShort);
+	}
+
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED);
+
+	if (pAd->CommonCfg.RadarDetect.RDMode == RD_SILENCE_MODE)
+	{
+#ifdef DFS_SUPPORT
+		RadarDetectionStop(pAd);
+#endif // DFS_SUPPORT //
+	}
+	pAd->CommonCfg.RadarDetect.RDMode = RD_NORMAL_MODE;
+
+	if (BssType == BSS_ADHOC)
+	{
+		MakeIbssBeacon(pAd);
+		if ((pAd->CommonCfg.Channel > 14)
+			&& (pAd->CommonCfg.bIEEE80211H == 1)
+			&& RadarChannelCheck(pAd, pAd->CommonCfg.Channel))
+		{
+			; //Do nothing
+		}
+		else
+		{
+			AsicEnableIbssSync(pAd);
+		}
+
+		// In ad hoc mode, use MAC table from index 1.
+		// p.s ASIC use all 0xff as termination of WCID table search.To prevent it's 0xff-ff-ff-ff-ff-ff, Write 0 here.
+		RTMP_IO_WRITE32(pAd, MAC_WCID_BASE, 0x00);
+		RTMP_IO_WRITE32(pAd, 0x1808, 0x00);
+
+		// If WEP is enabled, add key material and cipherAlg into Asic
+		// Fill in Shared Key Table(offset: 0x6c00) and Shared Key Mode(offset: 0x7000)
+
+		if (pAd->StaCfg.WepStatus == Ndis802_11WEPEnabled)
+		{
+			PUCHAR	Key;
+			UCHAR 	CipherAlg;
+
+			for (idx=0; idx < SHARE_KEY_NUM; idx++)
+        	{
+				CipherAlg = pAd->SharedKey[BSS0][idx].CipherAlg;
+    			Key = pAd->SharedKey[BSS0][idx].Key;
+
+				if (pAd->SharedKey[BSS0][idx].KeyLen > 0)
+				{
+					// Set key material and cipherAlg to Asic
+    				AsicAddSharedKeyEntry(pAd, BSS0, idx, CipherAlg, Key, NULL, NULL);
+
+                    if (idx == pAd->StaCfg.DefaultKeyId)
+					{
+						// Update WCID attribute table and IVEIV table for this group key table
+						RTMPAddWcidAttributeEntry(pAd, BSS0, idx, CipherAlg, NULL);
+					}
+				}
+
+
+			}
+		}
+		// If WPANone is enabled, add key material and cipherAlg into Asic
+		// Fill in Shared Key Table(offset: 0x6c00) and Shared Key Mode(offset: 0x7000)
+		else if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+		{
+			pAd->StaCfg.DefaultKeyId = 0;	// always be zero
+
+            NdisZeroMemory(&pAd->SharedKey[BSS0][0], sizeof(CIPHER_KEY));
+							pAd->SharedKey[BSS0][0].KeyLen = LEN_TKIP_EK;
+			NdisMoveMemory(pAd->SharedKey[BSS0][0].Key, pAd->StaCfg.PMK, LEN_TKIP_EK);
+
+            if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
+            {
+    			NdisMoveMemory(pAd->SharedKey[BSS0][0].RxMic, &pAd->StaCfg.PMK[16], LEN_TKIP_RXMICK);
+    			NdisMoveMemory(pAd->SharedKey[BSS0][0].TxMic, &pAd->StaCfg.PMK[16], LEN_TKIP_TXMICK);
+            }
+
+			// Decide its ChiperAlg
+			if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
+				pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_TKIP;
+			else if (pAd->StaCfg.PairCipher == Ndis802_11Encryption3Enabled)
+				pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_AES;
+			else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Unknow Cipher (=%d), set Cipher to AES\n", pAd->StaCfg.PairCipher));
+				pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_AES;
+            }
+
+			// Set key material and cipherAlg to Asic
+			AsicAddSharedKeyEntry(pAd,
+								  BSS0,
+								  0,
+								  pAd->SharedKey[BSS0][0].CipherAlg,
+								  pAd->SharedKey[BSS0][0].Key,
+								  pAd->SharedKey[BSS0][0].TxMic,
+								  pAd->SharedKey[BSS0][0].RxMic);
+
+            // Update WCID attribute table and IVEIV table for this group key table
+			RTMPAddWcidAttributeEntry(pAd, BSS0, 0, pAd->SharedKey[BSS0][0].CipherAlg, NULL);
+
+		}
+
+	}
+	else // BSS_INFRA
+	{
+		// Check the new SSID with last SSID
+		while (Cancelled == TRUE)
+		{
+			if (pAd->CommonCfg.LastSsidLen == pAd->CommonCfg.SsidLen)
+			{
+				if (RTMPCompareMemory(pAd->CommonCfg.LastSsid, pAd->CommonCfg.Ssid, pAd->CommonCfg.LastSsidLen) == 0)
+				{
+					// Link to the old one no linkdown is required.
+					break;
+				}
+			}
+			// Send link down event before set to link up
+			pAd->IndicateMediaState = NdisMediaStateDisconnected;
+			RTMP_IndicateMediaState(pAd);
+            pAd->ExtraInfo = GENERAL_LINK_DOWN;
+			DBGPRINT(RT_DEBUG_TRACE, ("NDIS_STATUS_MEDIA_DISCONNECT Event AA!\n"));
+			break;
+		}
+
+		//
+		// On WPA mode, Remove All Keys if not connect to the last BSSID
+		// Key will be set after 4-way handshake.
+		//
+		if ((pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA))
+		{
+			ULONG 		IV;
+
+			// Remove all WPA keys
+			RTMPWPARemoveAllKeys(pAd);
+			pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+			pAd->StaCfg.PrivacyFilter = Ndis802_11PrivFilter8021xWEP;
+
+			// Fixed connection failed with Range Maximizer - 515 AP (Marvell Chip) when security is WPAPSK/TKIP
+			// If IV related values are too large in GroupMsg2, AP would ignore this message.
+			IV = 0;
+			IV |= (pAd->StaCfg.DefaultKeyId << 30);
+			AsicUpdateWCIDIVEIV(pAd, BSSID_WCID, IV, 0);
+		}
+		// NOTE:
+		// the decision of using "short slot time" or not may change dynamically due to
+		// new STA association to the AP. so we have to decide that upon parsing BEACON, not here
+
+		// NOTE:
+		// the decision to use "RTC/CTS" or "CTS-to-self" protection or not may change dynamically
+		// due to new STA association to the AP. so we have to decide that upon parsing BEACON, not here
+
+		ComposePsPoll(pAd);
+		ComposeNullFrame(pAd);
+
+			AsicEnableBssSync(pAd);
+
+		// Add BSSID to WCID search table
+		AsicUpdateRxWCIDTable(pAd, BSSID_WCID, pAd->CommonCfg.Bssid);
+
+		NdisAcquireSpinLock(&pAd->MacTabLock);
+		// add this BSSID entry into HASH table
+		{
+			UCHAR HashIdx;
+
+			//pEntry = &pAd->MacTab.Content[BSSID_WCID];
+			HashIdx = MAC_ADDR_HASH_INDEX(pAd->CommonCfg.Bssid);
+			if (pAd->MacTab.Hash[HashIdx] == NULL)
+			{
+				pAd->MacTab.Hash[HashIdx] = pEntry;
+			}
+			else
+			{
+				pCurrEntry = pAd->MacTab.Hash[HashIdx];
+				while (pCurrEntry->pNext != NULL)
+					pCurrEntry = pCurrEntry->pNext;
+				pCurrEntry->pNext = pEntry;
+			}
+		}
+		NdisReleaseSpinLock(&pAd->MacTabLock);
+
+
+		// If WEP is enabled, add paiewise and shared key
+#ifdef WPA_SUPPLICANT_SUPPORT
+        if (((pAd->StaCfg.WpaSupplicantUP)&&
+             (pAd->StaCfg.WepStatus == Ndis802_11WEPEnabled)&&
+             (pAd->StaCfg.PortSecured == WPA_802_1X_PORT_SECURED)) ||
+            ((pAd->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_DISABLE)&&
+              (pAd->StaCfg.WepStatus == Ndis802_11WEPEnabled)))
+#else
+		if (pAd->StaCfg.WepStatus == Ndis802_11WEPEnabled)
+#endif // WPA_SUPPLICANT_SUPPORT //
+		{
+			PUCHAR	Key;
+			UCHAR 	CipherAlg;
+
+			for (idx=0; idx < SHARE_KEY_NUM; idx++)
+        	{
+				CipherAlg = pAd->SharedKey[BSS0][idx].CipherAlg;
+    			Key = pAd->SharedKey[BSS0][idx].Key;
+
+				if (pAd->SharedKey[BSS0][idx].KeyLen > 0)
+				{
+					// Set key material and cipherAlg to Asic
+    				AsicAddSharedKeyEntry(pAd, BSS0, idx, CipherAlg, Key, NULL, NULL);
+
+					if (idx == pAd->StaCfg.DefaultKeyId)
+					{
+						// Assign group key info
+						RTMPAddWcidAttributeEntry(pAd, BSS0, idx, CipherAlg, NULL);
+
+						// Assign pairwise key info
+						RTMPAddWcidAttributeEntry(pAd, BSS0, idx, CipherAlg, pEntry);
+					}
+				}
+			}
+		}
+
+		// only INFRASTRUCTURE mode need to indicate connectivity immediately; ADHOC mode
+		// should wait until at least 2 active nodes in this BSSID.
+		OPSTATUS_SET_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED);
+
+        // For GUI ++
+		if (pAd->StaCfg.AuthMode < Ndis802_11AuthModeWPA)
+		{
+			pAd->IndicateMediaState = NdisMediaStateConnected;
+			pAd->ExtraInfo = GENERAL_LINK_UP;
+		}
+        // --
+		RTMP_IndicateMediaState(pAd);
+
+		// Add BSSID in my MAC Table.
+        NdisAcquireSpinLock(&pAd->MacTabLock);
+		RTMPMoveMemory(pAd->MacTab.Content[BSSID_WCID].Addr, pAd->CommonCfg.Bssid, MAC_ADDR_LEN);
+		pAd->MacTab.Content[BSSID_WCID].Aid = BSSID_WCID;
+		pAd->MacTab.Content[BSSID_WCID].pAd = pAd;
+		pAd->MacTab.Content[BSSID_WCID].ValidAsCLI = TRUE;	//Although this is bssid..still set ValidAsCl
+		pAd->MacTab.Size = 1;	// infra mode always set MACtab size =1.
+		pAd->MacTab.Content[BSSID_WCID].Sst = SST_ASSOC;
+		pAd->MacTab.Content[BSSID_WCID].AuthState = SST_ASSOC;
+		pAd->MacTab.Content[BSSID_WCID].WepStatus = pAd->StaCfg.WepStatus;
+        NdisReleaseSpinLock(&pAd->MacTabLock);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("!!! LINK UP !!!  ClientStatusFlags=%lx)\n",
+			pAd->MacTab.Content[BSSID_WCID].ClientStatusFlags));
+
+		MlmeUpdateTxRates(pAd, TRUE, BSS0);
+#ifdef DOT11_N_SUPPORT
+		MlmeUpdateHtTxRates(pAd, BSS0);
+		DBGPRINT(RT_DEBUG_TRACE, ("!!! LINK UP !! (StaActive.bHtEnable =%d, )\n", pAd->StaActive.SupportedPhyInfo.bHtEnable));
+#endif // DOT11_N_SUPPORT //
+
+		//
+		// Report Adjacent AP report.
+		//
+#ifdef LEAP_SUPPORT
+		CCXAdjacentAPReport(pAd);
+#endif // LEAP_SUPPORT //
+
+		if (pAd->CommonCfg.bAggregationCapable)
+		{
+			if ((pAd->CommonCfg.bPiggyBackCapable) && (pAd->MlmeAux.APRalinkIe & 0x00000003) == 3)
+			{
+
+				OPSTATUS_SET_FLAG(pAd, fOP_STATUS_PIGGYBACK_INUSED);
+				OPSTATUS_SET_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED);
+                RTMPSetPiggyBack(pAd, TRUE);
+				DBGPRINT(RT_DEBUG_TRACE, ("Turn on Piggy-Back\n"));
+			}
+			else if (pAd->MlmeAux.APRalinkIe & 0x00000001)
+			{
+				OPSTATUS_SET_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED);
+			}
+		}
+
+		if (pAd->MlmeAux.APRalinkIe != 0x0)
+		{
+#ifdef DOT11_N_SUPPORT
+			if (CLIENT_STATUS_TEST_FLAG(&pAd->MacTab.Content[BSSID_WCID], fCLIENT_STATUS_RDG_CAPABLE))
+			{
+				AsicEnableRDG(pAd);
+			}
+#endif // DOT11_N_SUPPORT //
+			OPSTATUS_SET_FLAG(pAd, fCLIENT_STATUS_RALINK_CHIPSET);
+			CLIENT_STATUS_SET_FLAG(&pAd->MacTab.Content[BSSID_WCID], fCLIENT_STATUS_RALINK_CHIPSET);
+		}
+		else
+		{
+			OPSTATUS_CLEAR_FLAG(pAd, fCLIENT_STATUS_RALINK_CHIPSET);
+			CLIENT_STATUS_CLEAR_FLAG(&pAd->MacTab.Content[BSSID_WCID], fCLIENT_STATUS_RALINK_CHIPSET);
+		}
+	}
+
+#ifdef DOT11_N_SUPPORT
+	DBGPRINT(RT_DEBUG_TRACE, ("NDIS_STATUS_MEDIA_CONNECT Event B!.BACapability = %x. ClientStatusFlags = %lx\n", pAd->CommonCfg.BACapability.word, pAd->MacTab.Content[BSSID_WCID].ClientStatusFlags));
+#endif // DOT11_N_SUPPORT //
+
+	// Set LED
+	RTMPSetLED(pAd, LED_LINK_UP);
+
+	pAd->Mlme.PeriodicRound = 0;
+	pAd->Mlme.OneSecPeriodicRound = 0;
+	pAd->bConfigChanged = FALSE;        // Reset config flag
+	pAd->ExtraInfo = GENERAL_LINK_UP;   // Update extra information to link is up
+
+	// Set asic auto fall back
+	{
+		PUCHAR					pTable;
+		UCHAR					TableSize = 0;
+
+		MlmeSelectTxRateTable(pAd, &pAd->MacTab.Content[BSSID_WCID], &pTable, &TableSize, &pAd->CommonCfg.TxRateIndex);
+		AsicUpdateAutoFallBackTable(pAd, pTable);
+	}
+
+	NdisAcquireSpinLock(&pAd->MacTabLock);
+    pEntry->HTPhyMode.word = pAd->StaCfg.HTPhyMode.word;
+    pEntry->MaxHTPhyMode.word = pAd->StaCfg.HTPhyMode.word;
+	if (pAd->StaCfg.bAutoTxRateSwitch == FALSE)
+	{
+		pEntry->bAutoTxRateSwitch = FALSE;
+#ifdef DOT11_N_SUPPORT
+		if (pEntry->HTPhyMode.field.MCS == 32)
+			pEntry->HTPhyMode.field.ShortGI = GI_800;
+
+		if ((pEntry->HTPhyMode.field.MCS > MCS_7) || (pEntry->HTPhyMode.field.MCS == 32))
+			pEntry->HTPhyMode.field.STBC = STBC_NONE;
+#endif // DOT11_N_SUPPORT //
+		// If the legacy mode is set, overwrite the transmit setting of this entry.
+		if (pEntry->HTPhyMode.field.MODE <= MODE_OFDM)
+			RTMPUpdateLegacyTxSetting((UCHAR)pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode, pEntry);
+	}
+	else
+		pEntry->bAutoTxRateSwitch = TRUE;
+	NdisReleaseSpinLock(&pAd->MacTabLock);
+
+	//  Let Link Status Page display first initial rate.
+	pAd->LastTxRate = (USHORT)(pEntry->HTPhyMode.word);
+	// Select DAC according to HT or Legacy
+	if (pAd->StaActive.SupportedPhyInfo.MCSSet[0] != 0x00)
+	{
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R1, &Value);
+		Value &= (~0x18);
+		if (pAd->Antenna.field.TxPath == 2)
+		{
+		    Value |= 0x10;
+		}
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R1, Value);
+	}
+	else
+	{
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R1, &Value);
+		Value &= (~0x18);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R1, Value);
+	}
+
+#ifdef DOT11_N_SUPPORT
+	if (pAd->StaActive.SupportedPhyInfo.bHtEnable == FALSE)
+	{
+	}
+	else if (pEntry->MaxRAmpduFactor == 0)
+	{
+	    // If HT AP doesn't support MaxRAmpduFactor = 1, we need to set max PSDU to 0.
+	    // Because our Init value is 1 at MACRegTable.
+		RTMP_IO_WRITE32(pAd, MAX_LEN_CFG, 0x0fff);
+	}
+#endif // DOT11_N_SUPPORT //
+
+	// Patch for Marvel AP to gain high throughput
+	// Need to set as following,
+	// 1. Set txop in register-EDCA_AC0_CFG as 0x60
+	// 2. Set EnTXWriteBackDDONE in register-WPDMA_GLO_CFG as zero
+	// 3. PBF_MAX_PCNT as 0x1F3FBF9F
+	// 4. kick per two packets when dequeue
+	//
+	// Txop can only be modified when RDG is off, WMM is disable and TxBurst is enable
+	//
+	// if 1. Legacy AP WMM on,  or 2. 11n AP, AMPDU disable.  Force turn off burst no matter what bEnableTxBurst is.
+#ifdef DOT11_N_SUPPORT
+	if (((pAd->StaActive.SupportedPhyInfo.bHtEnable == FALSE) && (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED)))
+		|| ((pAd->StaActive.SupportedPhyInfo.bHtEnable == TRUE) && (pAd->CommonCfg.BACapability.field.Policy == BA_NOTUSE)))
+	{
+		RTMP_IO_READ32(pAd, EDCA_AC0_CFG, &Data);
+		Data  &= 0xFFFFFF00;
+		RTMP_IO_WRITE32(pAd, EDCA_AC0_CFG, Data);
+
+		RTMP_IO_WRITE32(pAd, PBF_MAX_PCNT, 0x1F3F7F9F);
+		DBGPRINT(RT_DEBUG_TRACE, ("Txburst 1\n"));
+	}
+	else
+#endif // DOT11_N_SUPPORT //
+	if (pAd->CommonCfg.bEnableTxBurst)
+	{
+		RTMP_IO_READ32(pAd, EDCA_AC0_CFG, &Data);
+		Data  &= 0xFFFFFF00;
+		Data  |= 0x60;
+		RTMP_IO_WRITE32(pAd, EDCA_AC0_CFG, Data);
+		pAd->CommonCfg.IOTestParm.bNowAtherosBurstOn = TRUE;
+
+		RTMP_IO_WRITE32(pAd, PBF_MAX_PCNT, 0x1F3FBF9F);
+		DBGPRINT(RT_DEBUG_TRACE, ("Txburst 2\n"));
+	}
+	else
+	{
+		RTMP_IO_READ32(pAd, EDCA_AC0_CFG, &Data);
+		Data  &= 0xFFFFFF00;
+		RTMP_IO_WRITE32(pAd, EDCA_AC0_CFG, Data);
+
+		RTMP_IO_WRITE32(pAd, PBF_MAX_PCNT, 0x1F3F7F9F);
+		DBGPRINT(RT_DEBUG_TRACE, ("Txburst 3\n"));
+	}
+
+#ifdef DOT11_N_SUPPORT
+	// Re-check to turn on TX burst or not.
+	if ((pAd->CommonCfg.IOTestParm.bLastAtheros == TRUE) && ((STA_WEP_ON(pAd))||(STA_TKIP_ON(pAd))))
+	{
+		pAd->CommonCfg.IOTestParm.bNextDisableRxBA = TRUE;
+		if (pAd->CommonCfg.bEnableTxBurst)
+		{
+		    UINT32 MACValue = 0;
+			// Force disable  TXOP value in this case. The same action in MLMEUpdateProtect too.
+			// I didn't change PBF_MAX_PCNT setting.
+			RTMP_IO_READ32(pAd, EDCA_AC0_CFG, &MACValue);
+			MACValue  &= 0xFFFFFF00;
+			RTMP_IO_WRITE32(pAd, EDCA_AC0_CFG, MACValue);
+			pAd->CommonCfg.IOTestParm.bNowAtherosBurstOn = FALSE;
+		}
+	}
+	else
+	{
+		pAd->CommonCfg.IOTestParm.bNextDisableRxBA = FALSE;
+	}
+#endif // DOT11_N_SUPPORT //
+
+	pAd->CommonCfg.IOTestParm.bLastAtheros = FALSE;
+	COPY_MAC_ADDR(pAd->CommonCfg.LastBssid, pAd->CommonCfg.Bssid);
+	DBGPRINT(RT_DEBUG_TRACE, ("!!!pAd->bNextDisableRxBA= %d \n", pAd->CommonCfg.IOTestParm.bNextDisableRxBA));
+	// BSSID add in one MAC entry too.  Because in Tx, ASIC need to check Cipher and IV/EIV, BAbitmap
+	// Pther information in MACTab.Content[BSSID_WCID] is not necessary for driver.
+	// Note: As STA, The MACTab.Content[BSSID_WCID]. PairwiseKey and Shared Key for BSS0 are the same.
+
+    if (pAd->StaCfg.WepStatus <= Ndis802_11WEPDisabled)
+    {
+        pAd->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+		pAd->StaCfg.PrivacyFilter = Ndis802_11PrivFilterAcceptAll;
+	}
+
+	NdisAcquireSpinLock(&pAd->MacTabLock);
+	pEntry->PortSecured = pAd->StaCfg.PortSecured;
+	NdisReleaseSpinLock(&pAd->MacTabLock);
+
+    //
+	// Patch Atheros AP TX will breakdown issue.
+	// AP Model: DLink DWL-8200AP
+	//
+	if (INFRA_ON(pAd) && OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED) && STA_TKIP_ON(pAd))
+	{
+		RTMP_IO_WRITE32(pAd, RX_PARSER_CFG, 0x01);
+	}
+	else
+	{
+		RTMP_IO_WRITE32(pAd, RX_PARSER_CFG, 0x00);
+	}
+
+	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS);
+
+#ifdef DOT11_N_SUPPORT
+#ifdef DOT11N_DRAFT3
+	if ((pAd->CommonCfg.BACapability.field.b2040CoexistScanSup) && (pAd->CommonCfg.Channel <= 11))
+	{
+		OPSTATUS_SET_FLAG(pAd, fOP_STATUS_SCAN_2040);
+		BuildEffectedChannelList(pAd);
+	}
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+}
+
+/*
+	==========================================================================
+
+	Routine	Description:
+		Disconnect current BSSID
+
+	Arguments:
+		pAd				- Pointer to our adapter
+		IsReqFromAP		- Request from AP
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+		We need more information to know it's this requst from AP.
+		If yes! we need to do extra handling, for example, remove the WPA key.
+		Otherwise on 4-way handshaking will faied, since the WPA key didn't be
+		remove while auto reconnect.
+		Disconnect request from AP, it means we will start afresh 4-way handshaking
+		on WPA mode.
+
+	==========================================================================
+*/
+VOID LinkDown(
+	IN PRTMP_ADAPTER pAd,
+	IN  BOOLEAN      IsReqFromAP)
+{
+	UCHAR			    i, ByteValue = 0;
+
+	// Do nothing if monitor mode is on
+	if (MONITOR_ON(pAd))
+		return;
+
+#ifdef RALINK_ATE
+	// Nothing to do in ATE mode.
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+    if (pAd->CommonCfg.bWirelessEvent)
+	{
+		RTMPSendWirelessEvent(pAd, IW_STA_LINKDOWN_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("!!! LINK DOWN !!!\n"));
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED);
+
+#ifdef RT2860
+    if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE))
+    {
+	    BOOLEAN Cancelled;
+        pAd->Mlme.bPsPollTimerRunning = FALSE;
+        RTMPCancelTimer(&pAd->Mlme.PsPollTimer,	&Cancelled);
+    }
+
+    if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+    {
+        AUTO_WAKEUP_STRUC AutoWakeupCfg;
+		AsicForceWakeup(pAd, TRUE);
+        AutoWakeupCfg.word = 0;
+	    RTMP_IO_WRITE32(pAd, AUTO_WAKEUP_CFG, AutoWakeupCfg.word);
+        OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_DOZE);
+    }
+
+    pAd->bPCIclkOff = FALSE;
+#endif // RT2860 //
+	if (ADHOC_ON(pAd))		// Adhoc mode link down
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("!!! LINK DOWN 1!!!\n"));
+
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_ADHOC_ON);
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED);
+		pAd->IndicateMediaState = NdisMediaStateDisconnected;
+		RTMP_IndicateMediaState(pAd);
+        pAd->ExtraInfo = GENERAL_LINK_DOWN;
+		BssTableDeleteEntry(&pAd->ScanTab, pAd->CommonCfg.Bssid, pAd->CommonCfg.Channel);
+		DBGPRINT(RT_DEBUG_TRACE, ("!!! MacTab.Size=%d !!!\n", pAd->MacTab.Size));
+	}
+	else					// Infra structure mode
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("!!! LINK DOWN 2!!!\n"));
+
+#ifdef QOS_DLS_SUPPORT
+		// DLS tear down frame must be sent before link down
+		// send DLS-TEAR_DOWN message
+		if (pAd->CommonCfg.bDLSCapable)
+		{
+			// tear down local dls table entry
+			for (i=0; i<MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+			{
+				if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+				{
+					pAd->StaCfg.DLSEntry[i].Status = DLS_NONE;
+					RTMPSendDLSTearDownFrame(pAd, pAd->StaCfg.DLSEntry[i].MacAddr);
+				}
+			}
+
+			// tear down peer dls table entry
+			for (i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+			{
+				if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status ==  DLS_FINISH))
+				{
+					pAd->StaCfg.DLSEntry[i].Status = DLS_NONE;
+					RTMPSendDLSTearDownFrame(pAd, pAd->StaCfg.DLSEntry[i].MacAddr);
+				}
+			}
+		}
+#endif // QOS_DLS_SUPPORT //
+
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_INFRA_ON);
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED);
+
+		// Saved last SSID for linkup comparison
+		pAd->CommonCfg.LastSsidLen = pAd->CommonCfg.SsidLen;
+		NdisMoveMemory(pAd->CommonCfg.LastSsid, pAd->CommonCfg.Ssid, pAd->CommonCfg.LastSsidLen);
+		COPY_MAC_ADDR(pAd->CommonCfg.LastBssid, pAd->CommonCfg.Bssid);
+		if (pAd->MlmeAux.CurrReqIsFromNdis == TRUE)
+		{
+			pAd->IndicateMediaState = NdisMediaStateDisconnected;
+			RTMP_IndicateMediaState(pAd);
+            pAd->ExtraInfo = GENERAL_LINK_DOWN;
+			DBGPRINT(RT_DEBUG_TRACE, ("NDIS_STATUS_MEDIA_DISCONNECT Event A!\n"));
+			pAd->MlmeAux.CurrReqIsFromNdis = FALSE;
+		}
+		else
+		{
+            //
+			// If disassociation request is from NDIS, then we don't need to delete BSSID from entry.
+			// Otherwise lost beacon or receive De-Authentication from AP,
+			// then we should delete BSSID from BssTable.
+			// If we don't delete from entry, roaming will fail.
+			//
+			BssTableDeleteEntry(&pAd->ScanTab, pAd->CommonCfg.Bssid, pAd->CommonCfg.Channel);
+		}
+
+		// restore back to -
+		//      1. long slot (20 us) or short slot (9 us) time
+		//      2. turn on/off RTS/CTS and/or CTS-to-self protection
+		//      3. short preamble
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_BG_PROTECTION_INUSED);
+
+		if (pAd->StaCfg.CCXAdjacentAPReportFlag == TRUE)
+		{
+			//
+			// Record current AP's information.
+			// for later used reporting Adjacent AP report.
+			//
+			pAd->StaCfg.CCXAdjacentAPChannel = pAd->CommonCfg.Channel;
+			pAd->StaCfg.CCXAdjacentAPSsidLen = pAd->CommonCfg.SsidLen;
+			NdisMoveMemory(pAd->StaCfg.CCXAdjacentAPSsid, pAd->CommonCfg.Ssid, pAd->StaCfg.CCXAdjacentAPSsidLen);
+			COPY_MAC_ADDR(pAd->StaCfg.CCXAdjacentAPBssid, pAd->CommonCfg.Bssid);
+		}
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+		// Country IE of the AP will be evaluated and will be used.
+		if (pAd->StaCfg.IEEE80211dClientMode != Rt802_11_D_None)
+		{
+			NdisMoveMemory(&pAd->CommonCfg.CountryCode[0], &pAd->StaCfg.StaOriCountryCode[0], 2);
+			pAd->CommonCfg.Geography = pAd->StaCfg.StaOriGeography;
+			BuildChannelListEx(pAd);
+		}
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+	}
+
+	for (i=1; i<MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		if (pAd->MacTab.Content[i].ValidAsCLI == TRUE)
+			MacTableDeleteEntry(pAd, pAd->MacTab.Content[i].Aid, pAd->MacTab.Content[i].Addr);
+	}
+
+	pAd->StaCfg.CCXQosECWMin	= 4;
+	pAd->StaCfg.CCXQosECWMax	= 10;
+
+	AsicSetSlotTime(pAd, TRUE); //FALSE);
+	AsicSetEdcaParm(pAd, NULL);
+
+	// Set LED
+	RTMPSetLED(pAd, LED_LINK_DOWN);
+    pAd->LedIndicatorStregth = 0xF0;
+    RTMPSetSignalLED(pAd, -100);	// Force signal strength Led to be turned off, firmware is not done it.
+
+		AsicDisableSync(pAd);
+
+	pAd->Mlme.PeriodicRound = 0;
+	pAd->Mlme.OneSecPeriodicRound = 0;
+
+	if (pAd->StaCfg.BssType == BSS_INFRA)
+	{
+		// Remove StaCfg Information after link down
+		NdisZeroMemory(pAd->CommonCfg.Bssid, MAC_ADDR_LEN);
+		NdisZeroMemory(pAd->CommonCfg.Ssid, MAX_LEN_OF_SSID);
+		pAd->CommonCfg.SsidLen = 0;
+	}
+#ifdef DOT11_N_SUPPORT
+	NdisZeroMemory(&pAd->MlmeAux.HtCapability, sizeof(HT_CAPABILITY_IE));
+	NdisZeroMemory(&pAd->MlmeAux.AddHtInfo, sizeof(ADD_HT_INFO_IE));
+	pAd->MlmeAux.HtCapabilityLen = 0;
+	pAd->MlmeAux.NewExtChannelOffset = 0xff;
+#endif // DOT11_N_SUPPORT //
+
+	// Reset WPA-PSK state. Only reset when supplicant enabled
+	if (pAd->StaCfg.WpaState != SS_NOTUSE)
+	{
+		pAd->StaCfg.WpaState = SS_START;
+		// Clear Replay counter
+		NdisZeroMemory(pAd->StaCfg.ReplayCounter, 8);
+
+#ifdef QOS_DLS_SUPPORT
+		if (pAd->CommonCfg.bDLSCapable)
+			NdisZeroMemory(pAd->StaCfg.DlsReplayCounter, 8);
+#endif // QOS_DLS_SUPPORT //
+	}
+
+
+	//
+	// if link down come from AP, we need to remove all WPA keys on WPA mode.
+	// otherwise will cause 4-way handshaking failed, since the WPA key not empty.
+	//
+	if ((IsReqFromAP) && (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA))
+	{
+		// Remove all WPA keys
+		RTMPWPARemoveAllKeys(pAd);
+	}
+
+	// 802.1x port control
+#ifdef WPA_SUPPLICANT_SUPPORT
+	// Prevent clear PortSecured here with static WEP
+	// NetworkManger set security policy first then set SSID to connect AP.
+	if (pAd->StaCfg.WpaSupplicantUP &&
+		(pAd->StaCfg.WepStatus == Ndis802_11WEPEnabled) &&
+		(pAd->StaCfg.IEEE8021X == FALSE))
+	{
+		pAd->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+	}
+	else
+#endif // WPA_SUPPLICANT_SUPPORT //
+	{
+		pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+		pAd->StaCfg.PrivacyFilter = Ndis802_11PrivFilter8021xWEP;
+	}
+
+	NdisAcquireSpinLock(&pAd->MacTabLock);
+	pAd->MacTab.Content[BSSID_WCID].PortSecured = pAd->StaCfg.PortSecured;
+	NdisReleaseSpinLock(&pAd->MacTabLock);
+
+	pAd->StaCfg.MicErrCnt = 0;
+
+	// Turn off Ckip control flag
+	pAd->StaCfg.bCkipOn = FALSE;
+	pAd->StaCfg.CCXEnable = FALSE;
+
+    pAd->IndicateMediaState = NdisMediaStateDisconnected;
+	// Update extra information to link is up
+	pAd->ExtraInfo = GENERAL_LINK_DOWN;
+
+    pAd->StaCfg.AdhocBOnlyJoined = FALSE;
+	pAd->StaCfg.AdhocBGJoined = FALSE;
+	pAd->StaCfg.Adhoc20NJoined = FALSE;
+    pAd->StaActive.SupportedPhyInfo.bHtEnable = FALSE;
+
+	// Reset the Current AP's IP address
+	NdisZeroMemory(pAd->StaCfg.AironetIPAddress, 4);
+
+	// Clean association information
+	NdisZeroMemory(&pAd->StaCfg.AssocInfo, sizeof(NDIS_802_11_ASSOCIATION_INFORMATION));
+	pAd->StaCfg.AssocInfo.Length = sizeof(NDIS_802_11_ASSOCIATION_INFORMATION);
+	pAd->StaCfg.ReqVarIELen = 0;
+	pAd->StaCfg.ResVarIELen = 0;
+
+	//
+	// Reset RSSI value after link down
+	//
+	pAd->StaCfg.RssiSample.AvgRssi0 = 0;
+	pAd->StaCfg.RssiSample.AvgRssi0X8 = 0;
+	pAd->StaCfg.RssiSample.AvgRssi1 = 0;
+	pAd->StaCfg.RssiSample.AvgRssi1X8 = 0;
+	pAd->StaCfg.RssiSample.AvgRssi2 = 0;
+	pAd->StaCfg.RssiSample.AvgRssi2X8 = 0;
+
+	// Restore MlmeRate
+	pAd->CommonCfg.MlmeRate = pAd->CommonCfg.BasicMlmeRate;
+	pAd->CommonCfg.RtsRate = pAd->CommonCfg.BasicMlmeRate;
+
+#ifdef DOT11_N_SUPPORT
+	//
+	// After Link down, reset piggy-back setting in ASIC. Disable RDG.
+	//
+	if (pAd->CommonCfg.BBPCurrentBW == BW_40)
+	{
+		pAd->CommonCfg.BBPCurrentBW = BW_20;
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &ByteValue);
+		ByteValue &= (~0x18);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, ByteValue);
+	}
+#endif // DOT11_N_SUPPORT //
+	// Reset DAC
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R1, &ByteValue);
+	ByteValue &= (~0x18);
+	if (pAd->Antenna.field.TxPath == 2)
+	{
+		ByteValue |= 0x10;
+	}
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R1, ByteValue);
+
+	RTMPSetPiggyBack(pAd,FALSE);
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_PIGGYBACK_INUSED);
+
+#ifdef DOT11_N_SUPPORT
+	pAd->CommonCfg.BACapability.word = pAd->CommonCfg.REGBACapability.word;
+#endif // DOT11_N_SUPPORT //
+
+	// Restore all settings in the following.
+	AsicUpdateProtect(pAd, 0, (ALLN_SETPROTECT|CCKSETPROTECT|OFDMSETPROTECT), TRUE, FALSE);
+	AsicDisableRDG(pAd);
+	pAd->CommonCfg.IOTestParm.bCurrentAtheros = FALSE;
+	pAd->CommonCfg.IOTestParm.bNowAtherosBurstOn = FALSE;
+
+#ifdef DOT11_N_SUPPORT
+#ifdef DOT11N_DRAFT3
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SCAN_2040);
+	pAd->CommonCfg.BSSCoexist2040.word = 0;
+	TriEventInit(pAd);
+	for (i = 0; i < (pAd->ChannelListNum - 1); i++)
+	{
+		pAd->ChannelList[i].bEffectedChannel = FALSE;
+	}
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+
+	RTMP_IO_WRITE32(pAd, MAX_LEN_CFG, 0x1fff);
+	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS);
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+	if (pAd->StaCfg.WpaSupplicantUP) {
+		union iwreq_data    wrqu;
+		//send disassociate event to wpa_supplicant
+		memset(&wrqu, 0, sizeof(wrqu));
+		wrqu.data.flags = RT_DISASSOC_EVENT_FLAG;
+		wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+	}
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+	{
+		union iwreq_data    wrqu;
+		memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+		wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+	}
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID IterateOnBssTab(
+	IN PRTMP_ADAPTER pAd)
+{
+	MLME_START_REQ_STRUCT   StartReq;
+	MLME_JOIN_REQ_STRUCT    JoinReq;
+	ULONG                   BssIdx;
+
+	// Change the wepstatus to original wepstatus
+	pAd->StaCfg.WepStatus   = pAd->StaCfg.OrigWepStatus;
+	pAd->StaCfg.PairCipher  = pAd->StaCfg.OrigWepStatus;
+	pAd->StaCfg.GroupCipher = pAd->StaCfg.OrigWepStatus;
+
+	BssIdx = pAd->MlmeAux.BssIdx;
+	if (BssIdx < pAd->MlmeAux.SsidBssTab.BssNr)
+	{
+		// Check cipher suite, AP must have more secured cipher than station setting
+		// Set the Pairwise and Group cipher to match the intended AP setting
+		// We can only connect to AP with less secured cipher setting
+		if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK))
+		{
+			pAd->StaCfg.GroupCipher = pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA.GroupCipher;
+
+			if (pAd->StaCfg.WepStatus == pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA.PairCipher)
+				pAd->StaCfg.PairCipher = pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA.PairCipher;
+			else if (pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA.PairCipherAux != Ndis802_11WEPDisabled)
+				pAd->StaCfg.PairCipher = pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA.PairCipherAux;
+			else	// There is no PairCipher Aux, downgrade our capability to TKIP
+				pAd->StaCfg.PairCipher = Ndis802_11Encryption2Enabled;
+		}
+		else if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK))
+		{
+			pAd->StaCfg.GroupCipher = pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA2.GroupCipher;
+
+			if (pAd->StaCfg.WepStatus == pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA2.PairCipher)
+				pAd->StaCfg.PairCipher = pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA2.PairCipher;
+			else if (pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA2.PairCipherAux != Ndis802_11WEPDisabled)
+				pAd->StaCfg.PairCipher = pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA2.PairCipherAux;
+			else	// There is no PairCipher Aux, downgrade our capability to TKIP
+				pAd->StaCfg.PairCipher = Ndis802_11Encryption2Enabled;
+
+			// RSN capability
+			pAd->StaCfg.RsnCapability = pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA2.RsnCapability;
+		}
+
+		// Set Mix cipher flag
+		pAd->StaCfg.bMixCipher = (pAd->StaCfg.PairCipher == pAd->StaCfg.GroupCipher) ? FALSE : TRUE;
+		if (pAd->StaCfg.bMixCipher == TRUE)
+		{
+			// If mix cipher, re-build RSNIE
+			RTMPMakeRSNIE(pAd, pAd->StaCfg.AuthMode, pAd->StaCfg.WepStatus, 0);
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("CNTL - iterate BSS %ld of %d\n", BssIdx, pAd->MlmeAux.SsidBssTab.BssNr));
+		JoinParmFill(pAd, &JoinReq, BssIdx);
+		MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_JOIN_REQ, sizeof(MLME_JOIN_REQ_STRUCT),
+					&JoinReq);
+		pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_JOIN;
+	}
+	else if (pAd->StaCfg.BssType == BSS_ADHOC)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("CNTL - All BSS fail; start a new ADHOC (Ssid=%s)...\n",pAd->MlmeAux.Ssid));
+		StartParmFill(pAd, &StartReq, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen);
+		MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_START_REQ, sizeof(MLME_START_REQ_STRUCT), &StartReq);
+		pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_START;
+	}
+	else // no more BSS
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("CNTL - All roaming failed, stay @ ch #%d\n", pAd->CommonCfg.Channel));
+		AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+		AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+		pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+	}
+}
+
+// for re-association only
+// IRQL = DISPATCH_LEVEL
+VOID IterateOnBssTab2(
+	IN PRTMP_ADAPTER pAd)
+{
+	MLME_REASSOC_REQ_STRUCT ReassocReq;
+	ULONG                   BssIdx;
+	BSS_ENTRY               *pBss;
+
+	BssIdx = pAd->MlmeAux.RoamIdx;
+	pBss = &pAd->MlmeAux.RoamTab.BssEntry[BssIdx];
+
+	if (BssIdx < pAd->MlmeAux.RoamTab.BssNr)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("CNTL - iterate BSS %ld of %d\n", BssIdx, pAd->MlmeAux.RoamTab.BssNr));
+
+		AsicSwitchChannel(pAd, pBss->Channel, FALSE);
+		AsicLockChannel(pAd, pBss->Channel);
+
+		// reassociate message has the same structure as associate message
+		AssocParmFill(pAd, &ReassocReq, pBss->Bssid, pBss->CapabilityInfo,
+					  ASSOC_TIMEOUT, pAd->StaCfg.DefaultListenCount);
+		MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_REASSOC_REQ,
+					sizeof(MLME_REASSOC_REQ_STRUCT), &ReassocReq);
+
+		pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_REASSOC;
+	}
+	else // no more BSS
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("CNTL - All fast roaming failed, back to ch #%d\n",pAd->CommonCfg.Channel));
+		AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+		AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+		pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID JoinParmFill(
+	IN PRTMP_ADAPTER pAd,
+	IN OUT MLME_JOIN_REQ_STRUCT *JoinReq,
+	IN ULONG BssIdx)
+{
+	JoinReq->BssIdx = BssIdx;
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID ScanParmFill(
+	IN PRTMP_ADAPTER pAd,
+	IN OUT MLME_SCAN_REQ_STRUCT *ScanReq,
+	IN CHAR Ssid[],
+	IN UCHAR SsidLen,
+	IN UCHAR BssType,
+	IN UCHAR ScanType)
+{
+    NdisZeroMemory(ScanReq->Ssid, MAX_LEN_OF_SSID);
+	ScanReq->SsidLen = SsidLen;
+	NdisMoveMemory(ScanReq->Ssid, Ssid, SsidLen);
+	ScanReq->BssType = BssType;
+	ScanReq->ScanType = ScanType;
+}
+
+#ifdef QOS_DLS_SUPPORT
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID DlsParmFill(
+	IN PRTMP_ADAPTER pAd,
+	IN OUT MLME_DLS_REQ_STRUCT *pDlsReq,
+	IN PRT_802_11_DLS pDls,
+	IN USHORT reason)
+{
+	pDlsReq->pDLS = pDls;
+	pDlsReq->Reason = reason;
+}
+#endif // QOS_DLS_SUPPORT //
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID StartParmFill(
+	IN PRTMP_ADAPTER pAd,
+	IN OUT MLME_START_REQ_STRUCT *StartReq,
+	IN CHAR Ssid[],
+	IN UCHAR SsidLen)
+{
+	ASSERT(SsidLen <= MAX_LEN_OF_SSID);
+	NdisMoveMemory(StartReq->Ssid, Ssid, SsidLen);
+	StartReq->SsidLen = SsidLen;
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID AuthParmFill(
+	IN PRTMP_ADAPTER pAd,
+	IN OUT MLME_AUTH_REQ_STRUCT *AuthReq,
+	IN PUCHAR pAddr,
+	IN USHORT Alg)
+{
+	COPY_MAC_ADDR(AuthReq->Addr, pAddr);
+	AuthReq->Alg = Alg;
+	AuthReq->Timeout = AUTH_TIMEOUT;
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+#ifdef RT2860
+VOID ComposePsPoll(
+	IN PRTMP_ADAPTER pAd)
+{
+	NdisZeroMemory(&pAd->PsPollFrame, sizeof(PSPOLL_FRAME));
+	pAd->PsPollFrame.FC.Type = BTYPE_CNTL;
+	pAd->PsPollFrame.FC.SubType = SUBTYPE_PS_POLL;
+	pAd->PsPollFrame.Aid = pAd->StaActive.Aid | 0xC000;
+	COPY_MAC_ADDR(pAd->PsPollFrame.Bssid, pAd->CommonCfg.Bssid);
+	COPY_MAC_ADDR(pAd->PsPollFrame.Ta, pAd->CurrentAddress);
+}
+
+// IRQL = DISPATCH_LEVEL
+VOID ComposeNullFrame(
+	IN PRTMP_ADAPTER pAd)
+{
+	NdisZeroMemory(&pAd->NullFrame, sizeof(HEADER_802_11));
+	pAd->NullFrame.FC.Type = BTYPE_DATA;
+	pAd->NullFrame.FC.SubType = SUBTYPE_NULL_FUNC;
+	pAd->NullFrame.FC.ToDs = 1;
+	COPY_MAC_ADDR(pAd->NullFrame.Addr1, pAd->CommonCfg.Bssid);
+	COPY_MAC_ADDR(pAd->NullFrame.Addr2, pAd->CurrentAddress);
+	COPY_MAC_ADDR(pAd->NullFrame.Addr3, pAd->CommonCfg.Bssid);
+}
+#endif // RT2860 //
+
+
+
+
+/*
+	==========================================================================
+	Description:
+		Pre-build a BEACON frame in the shared memory
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+ULONG MakeIbssBeacon(
+	IN PRTMP_ADAPTER pAd)
+{
+	UCHAR         DsLen = 1, IbssLen = 2;
+	UCHAR         LocalErpIe[3] = {IE_ERP, 1, 0x04};
+	HEADER_802_11 BcnHdr;
+	USHORT        CapabilityInfo;
+	LARGE_INTEGER FakeTimestamp;
+	ULONG         FrameLen = 0;
+	PTXWI_STRUC	  pTxWI = &pAd->BeaconTxWI;
+	CHAR         *pBeaconFrame = pAd->BeaconBuf;
+	BOOLEAN       Privacy;
+	UCHAR         SupRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR         SupRateLen = 0;
+	UCHAR         ExtRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR         ExtRateLen = 0;
+	UCHAR         RSNIe = IE_WPA;
+
+	if ((pAd->CommonCfg.PhyMode == PHY_11B) && (pAd->CommonCfg.Channel <= 14))
+	{
+		SupRate[0] = 0x82; // 1 mbps
+		SupRate[1] = 0x84; // 2 mbps
+		SupRate[2] = 0x8b; // 5.5 mbps
+		SupRate[3] = 0x96; // 11 mbps
+		SupRateLen = 4;
+		ExtRateLen = 0;
+	}
+	else if (pAd->CommonCfg.Channel > 14)
+	{
+		SupRate[0]  = 0x8C;    // 6 mbps, in units of 0.5 Mbps, basic rate
+		SupRate[1]  = 0x12;    // 9 mbps, in units of 0.5 Mbps
+		SupRate[2]  = 0x98;    // 12 mbps, in units of 0.5 Mbps, basic rate
+		SupRate[3]  = 0x24;    // 18 mbps, in units of 0.5 Mbps
+		SupRate[4]  = 0xb0;    // 24 mbps, in units of 0.5 Mbps, basic rate
+		SupRate[5]  = 0x48;    // 36 mbps, in units of 0.5 Mbps
+		SupRate[6]  = 0x60;    // 48 mbps, in units of 0.5 Mbps
+		SupRate[7]  = 0x6c;    // 54 mbps, in units of 0.5 Mbps
+		SupRateLen  = 8;
+		ExtRateLen  = 0;
+
+		//
+		// Also Update MlmeRate & RtsRate for G only & A only
+		//
+		pAd->CommonCfg.MlmeRate = RATE_6;
+		pAd->CommonCfg.RtsRate = RATE_6;
+		pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_OFDM;
+		pAd->CommonCfg.MlmeTransmit.field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MlmeRate];
+		pAd->MacTab.Content[BSS0Mcast_WCID].HTPhyMode.field.MODE = MODE_OFDM;
+		pAd->MacTab.Content[BSS0Mcast_WCID].HTPhyMode.field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MlmeRate];
+	}
+	else
+	{
+		SupRate[0] = 0x82; // 1 mbps
+		SupRate[1] = 0x84; // 2 mbps
+		SupRate[2] = 0x8b; // 5.5 mbps
+		SupRate[3] = 0x96; // 11 mbps
+		SupRateLen = 4;
+
+		ExtRate[0]  = 0x0C;    // 6 mbps, in units of 0.5 Mbps,
+		ExtRate[1]  = 0x12;    // 9 mbps, in units of 0.5 Mbps
+		ExtRate[2]  = 0x18;    // 12 mbps, in units of 0.5 Mbps,
+		ExtRate[3]  = 0x24;    // 18 mbps, in units of 0.5 Mbps
+		ExtRate[4]  = 0x30;    // 24 mbps, in units of 0.5 Mbps,
+		ExtRate[5]  = 0x48;    // 36 mbps, in units of 0.5 Mbps
+		ExtRate[6]  = 0x60;    // 48 mbps, in units of 0.5 Mbps
+		ExtRate[7]  = 0x6c;    // 54 mbps, in units of 0.5 Mbps
+		ExtRateLen  = 8;
+	}
+
+	pAd->StaActive.SupRateLen = SupRateLen;
+	NdisMoveMemory(pAd->StaActive.SupRate, SupRate, SupRateLen);
+	pAd->StaActive.ExtRateLen = ExtRateLen;
+	NdisMoveMemory(pAd->StaActive.ExtRate, ExtRate, ExtRateLen);
+
+	// compose IBSS beacon frame
+	MgtMacHeaderInit(pAd, &BcnHdr, SUBTYPE_BEACON, 0, BROADCAST_ADDR, pAd->CommonCfg.Bssid);
+	Privacy = (pAd->StaCfg.WepStatus == Ndis802_11Encryption1Enabled) ||
+			  (pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) ||
+			  (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled);
+	CapabilityInfo = CAP_GENERATE(0, 1, Privacy, (pAd->CommonCfg.TxPreamble == Rt802_11PreambleShort), 0, 0);
+
+	MakeOutgoingFrame(pBeaconFrame,                &FrameLen,
+					  sizeof(HEADER_802_11),           &BcnHdr,
+					  TIMESTAMP_LEN,                   &FakeTimestamp,
+					  2,                               &pAd->CommonCfg.BeaconPeriod,
+					  2,                               &CapabilityInfo,
+					  1,                               &SsidIe,
+					  1,                               &pAd->CommonCfg.SsidLen,
+					  pAd->CommonCfg.SsidLen,          pAd->CommonCfg.Ssid,
+					  1,                               &SupRateIe,
+					  1,                               &SupRateLen,
+					  SupRateLen,                      SupRate,
+					  1,                               &DsIe,
+					  1,                               &DsLen,
+					  1,                               &pAd->CommonCfg.Channel,
+					  1,                               &IbssIe,
+					  1,                               &IbssLen,
+					  2,                               &pAd->StaActive.AtimWin,
+					  END_OF_ARGS);
+
+	// add ERP_IE and EXT_RAE IE of in 802.11g
+	if (ExtRateLen)
+	{
+		ULONG	tmp;
+
+		MakeOutgoingFrame(pBeaconFrame + FrameLen,         &tmp,
+						  3,                               LocalErpIe,
+						  1,                               &ExtRateIe,
+						  1,                               &ExtRateLen,
+						  ExtRateLen,                      ExtRate,
+						  END_OF_ARGS);
+		FrameLen += tmp;
+	}
+
+	// If adhoc secruity is set for WPA-None, append the cipher suite IE
+	if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+	{
+		ULONG tmp;
+        RTMPMakeRSNIE(pAd, pAd->StaCfg.AuthMode, pAd->StaCfg.WepStatus, BSS0);
+
+		MakeOutgoingFrame(pBeaconFrame + FrameLen,        	&tmp,
+						  1,                              	&RSNIe,
+						  1,                            	&pAd->StaCfg.RSNIE_Len,
+						  pAd->StaCfg.RSNIE_Len,      		pAd->StaCfg.RSN_IE,
+						  END_OF_ARGS);
+		FrameLen += tmp;
+	}
+
+#ifdef DOT11_N_SUPPORT
+	if ((pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+	{
+		ULONG TmpLen;
+		UCHAR HtLen, HtLen1;
+
+#ifdef RT_BIG_ENDIAN
+		HT_CAPABILITY_IE HtCapabilityTmp;
+		ADD_HT_INFO_IE	addHTInfoTmp;
+		USHORT	b2lTmp, b2lTmp2;
+#endif
+
+		// add HT Capability IE
+		HtLen = sizeof(pAd->CommonCfg.HtCapability);
+		HtLen1 = sizeof(pAd->CommonCfg.AddHTInfo);
+#ifndef RT_BIG_ENDIAN
+		MakeOutgoingFrame(pBeaconFrame+FrameLen,	&TmpLen,
+						  1,						&HtCapIe,
+						  1,						&HtLen,
+						  HtLen,					&pAd->CommonCfg.HtCapability,
+						  1,						&AddHtInfoIe,
+						  1,						&HtLen1,
+						  HtLen1,					&pAd->CommonCfg.AddHTInfo,
+						  END_OF_ARGS);
+#else
+		NdisMoveMemory(&HtCapabilityTmp, &pAd->CommonCfg.HtCapability, HtLen);
+		*(USHORT *)(&HtCapabilityTmp.HtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.HtCapInfo));
+		*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo));
+
+		NdisMoveMemory(&addHTInfoTmp, &pAd->CommonCfg.AddHTInfo, HtLen1);
+		*(USHORT *)(&addHTInfoTmp.AddHtInfo2) = SWAP16(*(USHORT *)(&addHTInfoTmp.AddHtInfo2));
+		*(USHORT *)(&addHTInfoTmp.AddHtInfo3) = SWAP16(*(USHORT *)(&addHTInfoTmp.AddHtInfo3));
+
+		MakeOutgoingFrame(pBeaconFrame+FrameLen,	&TmpLen,
+						  1,						&HtCapIe,
+						  1,						&HtLen,
+						  HtLen,					&HtCapabilityTmp,
+						  1,						&AddHtInfoIe,
+						  1,						&HtLen1,
+						  HtLen1,					&addHTInfoTmp,
+						  END_OF_ARGS);
+#endif
+		FrameLen += TmpLen;
+	}
+#endif // DOT11_N_SUPPORT //
+
+	//beacon use reserved WCID 0xff
+    if (pAd->CommonCfg.Channel > 14)
+    {
+	RTMPWriteTxWI(pAd, pTxWI, FALSE, FALSE,  TRUE, FALSE, FALSE, TRUE, 0, 0xff, FrameLen,
+		PID_MGMT, PID_BEACON, RATE_1, IFS_HTTXOP, FALSE, &pAd->CommonCfg.MlmeTransmit);
+    }
+    else
+    {
+        // Set to use 1Mbps for Adhoc beacon.
+		HTTRANSMIT_SETTING Transmit;
+        Transmit.word = 0;
+        RTMPWriteTxWI(pAd, pTxWI, FALSE, FALSE,  TRUE, FALSE, FALSE, TRUE, 0, 0xff, FrameLen,
+    		PID_MGMT, PID_BEACON, RATE_1, IFS_HTTXOP, FALSE, &Transmit);
+    }
+
+#ifdef RT_BIG_ENDIAN
+	RTMPFrameEndianChange(pAd, pBeaconFrame, DIR_WRITE, FALSE);
+	RTMPWIEndianChange((PUCHAR)pTxWI, TYPE_TXWI);
+#endif
+
+    DBGPRINT(RT_DEBUG_TRACE, ("MakeIbssBeacon (len=%ld), SupRateLen=%d, ExtRateLen=%d, Channel=%d, PhyMode=%d\n",
+					FrameLen, SupRateLen, ExtRateLen, pAd->CommonCfg.Channel, pAd->CommonCfg.PhyMode));
+	return FrameLen;
+}
+
+
diff --git a/drivers/staging/rt2860/sta/dls.c b/drivers/staging/rt2860/sta/dls.c
new file mode 100644
index 00000000000..78fb28976fd
--- /dev/null
+++ b/drivers/staging/rt2860/sta/dls.c
@@ -0,0 +1,2201 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    dls.c
+
+    Abstract:
+    Handle WMM-DLS state machine
+
+    Revision History:
+    Who         When          What
+    --------    ----------    ----------------------------------------------
+    Rory Chen   02-14-2006
+	Arvin Tai	06-03-2008	  Modified for RT28xx
+ */
+
+#include "../rt_config.h"
+
+/*
+    ==========================================================================
+    Description:
+        dls state machine init, including state transition and timer init
+    Parameters:
+        Sm - pointer to the dls state machine
+    Note:
+        The state machine looks like this
+
+                            DLS_IDLE
+    MT2_MLME_DLS_REQUEST   MlmeDlsReqAction
+    MT2_PEER_DLS_REQUEST   PeerDlsReqAction
+    MT2_PEER_DLS_RESPONSE  PeerDlsRspAction
+    MT2_MLME_DLS_TEARDOWN  MlmeTearDownAction
+    MT2_PEER_DLS_TEARDOWN  PeerTearDownAction
+
+	IRQL = PASSIVE_LEVEL
+
+    ==========================================================================
+ */
+void DlsStateMachineInit(
+    IN PRTMP_ADAPTER pAd,
+    IN STATE_MACHINE *Sm,
+    OUT STATE_MACHINE_FUNC Trans[])
+{
+	UCHAR	i;
+
+    StateMachineInit(Sm, (STATE_MACHINE_FUNC*)Trans, MAX_DLS_STATE, MAX_DLS_MSG, (STATE_MACHINE_FUNC)Drop, DLS_IDLE, DLS_MACHINE_BASE);
+
+    // the first column
+    StateMachineSetAction(Sm, DLS_IDLE, MT2_MLME_DLS_REQ, (STATE_MACHINE_FUNC)MlmeDlsReqAction);
+    StateMachineSetAction(Sm, DLS_IDLE, MT2_PEER_DLS_REQ, (STATE_MACHINE_FUNC)PeerDlsReqAction);
+    StateMachineSetAction(Sm, DLS_IDLE, MT2_PEER_DLS_RSP, (STATE_MACHINE_FUNC)PeerDlsRspAction);
+    StateMachineSetAction(Sm, DLS_IDLE, MT2_MLME_DLS_TEAR_DOWN, (STATE_MACHINE_FUNC)MlmeDlsTearDownAction);
+    StateMachineSetAction(Sm, DLS_IDLE, MT2_PEER_DLS_TEAR_DOWN, (STATE_MACHINE_FUNC)PeerDlsTearDownAction);
+
+	for (i=0; i<MAX_NUM_OF_DLS_ENTRY; i++)
+	{
+		pAd->StaCfg.DLSEntry[i].pAd = pAd;
+		RTMPInitTimer(pAd, &pAd->StaCfg.DLSEntry[i].Timer, GET_TIMER_FUNCTION(DlsTimeoutAction), pAd, FALSE);
+	}
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID MlmeDlsReqAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+	PUCHAR			pOutBuffer = NULL;
+	NDIS_STATUS		NStatus;
+	ULONG			FrameLen = 0;
+	HEADER_802_11	DlsReqHdr;
+	PRT_802_11_DLS	pDLS = NULL;
+	UCHAR			Category = CATEGORY_DLS;
+	UCHAR			Action = ACTION_DLS_REQUEST;
+	ULONG			tmp;
+	USHORT			reason;
+	ULONG			Timeout;
+	BOOLEAN			TimerCancelled;
+
+	if(!MlmeDlsReqSanity(pAd, Elem->Msg, Elem->MsgLen, &pDLS, &reason))
+		return;
+
+	DBGPRINT(RT_DEBUG_TRACE,("DLS - MlmeDlsReqAction() \n"));
+
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if (NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("DLS - MlmeDlsReqAction() allocate memory failed \n"));
+		return;
+	}
+
+	ActHeaderInit(pAd, &DlsReqHdr, pAd->CommonCfg.Bssid, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+
+	// Build basic frame first
+	MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+					sizeof(HEADER_802_11),		&DlsReqHdr,
+					1,							&Category,
+					1,							&Action,
+					6,							&pDLS->MacAddr,
+					6,							pAd->CurrentAddress,
+					2,							&pAd->StaActive.CapabilityInfo,
+					2,							&pDLS->TimeOut,
+					1,							&SupRateIe,
+					1,							&pAd->MlmeAux.SupRateLen,
+					pAd->MlmeAux.SupRateLen,	pAd->MlmeAux.SupRate,
+					END_OF_ARGS);
+
+	if (pAd->MlmeAux.ExtRateLen != 0)
+	{
+		MakeOutgoingFrame(pOutBuffer + FrameLen,	&tmp,
+						  1,						&ExtRateIe,
+						  1,						&pAd->MlmeAux.ExtRateLen,
+						  pAd->MlmeAux.ExtRateLen,	pAd->MlmeAux.ExtRate,
+						  END_OF_ARGS);
+		FrameLen += tmp;
+	}
+
+#ifdef DOT11_N_SUPPORT
+	if ((pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+	{
+		UCHAR HtLen;
+
+#ifdef RT_BIG_ENDIAN
+		HT_CAPABILITY_IE HtCapabilityTmp;
+#endif
+
+		// add HT Capability IE
+		HtLen = sizeof(HT_CAPABILITY_IE);
+#ifndef RT_BIG_ENDIAN
+		MakeOutgoingFrame(pOutBuffer + FrameLen,	&tmp,
+							1,						&HtCapIe,
+							1,						&HtLen,
+							HtLen,					&pAd->CommonCfg.HtCapability,
+							END_OF_ARGS);
+#else
+		NdisMoveMemory(&HtCapabilityTmp, &pAd->CommonCfg.HtCapability, HtLen);
+							*(USHORT *)(&HtCapabilityTmp.HtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.HtCapInfo));
+							*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo));
+
+		MakeOutgoingFrame(pOutBuffer + FrameLen,	&tmp,
+							1,						&HtCapIe,
+							1,						&HtLen,
+							HtLen,					&HtCapabilityTmp,
+							END_OF_ARGS);
+#endif
+		FrameLen = FrameLen + tmp;
+	}
+#endif // DOT11_N_SUPPORT //
+
+	RTMPCancelTimer(&pDLS->Timer, &TimerCancelled);
+	Timeout = DLS_TIMEOUT;
+	RTMPSetTimer(&pDLS->Timer, Timeout);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID PeerDlsReqAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+	PUCHAR			pOutBuffer = NULL;
+	NDIS_STATUS		NStatus;
+	ULONG			FrameLen = 0;
+	USHORT			StatusCode = MLME_SUCCESS;
+	HEADER_802_11	DlsRspHdr;
+	UCHAR			Category = CATEGORY_DLS;
+	UCHAR			Action = ACTION_DLS_RESPONSE;
+	ULONG			tmp;
+	USHORT			CapabilityInfo;
+	UCHAR			DA[MAC_ADDR_LEN], SA[MAC_ADDR_LEN];
+	USHORT			DLSTimeOut;
+	SHORT			i;
+	ULONG			Timeout;
+	BOOLEAN			TimerCancelled;
+	PRT_802_11_DLS	pDLS = NULL;
+	UCHAR			MaxSupportedRateIn500Kbps = 0;
+    UCHAR			SupportedRatesLen;
+    UCHAR			SupportedRates[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR			HtCapabilityLen;
+	HT_CAPABILITY_IE	HtCapability;
+
+	if (!PeerDlsReqSanity(pAd, Elem->Msg, Elem->MsgLen, DA, SA, &CapabilityInfo, &DLSTimeOut,
+							&SupportedRatesLen, &SupportedRates[0], &HtCapabilityLen, &HtCapability))
+		return;
+
+    // supported rates array may not be sorted. sort it and find the maximum rate
+    for (i = 0; i < SupportedRatesLen; i++)
+    {
+        if (MaxSupportedRateIn500Kbps < (SupportedRates[i] & 0x7f))
+            MaxSupportedRateIn500Kbps = SupportedRates[i] & 0x7f;
+    }
+
+	DBGPRINT(RT_DEBUG_TRACE,("DLS - PeerDlsReqAction() from %02x:%02x:%02x:%02x:%02x:%02x\n", SA[0], SA[1], SA[2], SA[3], SA[4], SA[5]));
+
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if (NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("DLS - PeerDlsReqAction() allocate memory failed \n"));
+		return;
+	}
+
+	if (!INFRA_ON(pAd))
+	{
+		StatusCode = MLME_REQUEST_DECLINED;
+	}
+	else if (!pAd->CommonCfg.bWmmCapable)
+	{
+		StatusCode = MLME_DEST_STA_IS_NOT_A_QSTA;
+	}
+	else if (!pAd->CommonCfg.bDLSCapable)
+	{
+		StatusCode = MLME_REQUEST_DECLINED;
+	}
+	else
+	{
+		// find table to update parameters
+		for (i = (MAX_NUM_OF_DLS_ENTRY-1); i >= 0; i--)
+		{
+			if (pAd->StaCfg.DLSEntry[i].Valid && MAC_ADDR_EQUAL(SA, pAd->StaCfg.DLSEntry[i].MacAddr))
+			{
+				if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+					pAd->StaCfg.DLSEntry[i].Status = DLS_WAIT_KEY;
+				else
+				{
+					RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+					pAd->StaCfg.DLSEntry[i].Status = DLS_FINISH;
+				}
+
+				pAd->StaCfg.DLSEntry[i].Sequence = 0;
+				pAd->StaCfg.DLSEntry[i].TimeOut = DLSTimeOut;
+				pAd->StaCfg.DLSEntry[i].CountDownTimer = DLSTimeOut;
+				if (HtCapabilityLen != 0)
+					pAd->StaCfg.DLSEntry[i].bHTCap = TRUE;
+				else
+					pAd->StaCfg.DLSEntry[i].bHTCap = FALSE;
+				pDLS = &pAd->StaCfg.DLSEntry[i];
+				break;
+			}
+		}
+
+		// can not find in table, create a new one
+		if (i < 0)
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("DLS - PeerDlsReqAction() can not find same entry \n"));
+			for (i=(MAX_NUM_OF_DLS_ENTRY - 1); i >= MAX_NUM_OF_INIT_DLS_ENTRY; i--)
+			{
+				if (!pAd->StaCfg.DLSEntry[i].Valid)
+				{
+					MAC_TABLE_ENTRY *pEntry;
+					UCHAR MaxSupportedRate = RATE_11;
+
+					if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+					{
+						pAd->StaCfg.DLSEntry[i].Status = DLS_WAIT_KEY;
+					}
+					else
+					{
+						RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+						pAd->StaCfg.DLSEntry[i].Status = DLS_FINISH;
+					}
+
+					pAd->StaCfg.DLSEntry[i].Sequence = 0;
+					pAd->StaCfg.DLSEntry[i].Valid = TRUE;
+					pAd->StaCfg.DLSEntry[i].TimeOut = DLSTimeOut;
+					pAd->StaCfg.DLSEntry[i].CountDownTimer = DLSTimeOut;
+					NdisMoveMemory(pAd->StaCfg.DLSEntry[i].MacAddr, SA, MAC_ADDR_LEN);
+					if (HtCapabilityLen != 0)
+						pAd->StaCfg.DLSEntry[i].bHTCap = TRUE;
+					else
+						pAd->StaCfg.DLSEntry[i].bHTCap = FALSE;
+					pDLS = &pAd->StaCfg.DLSEntry[i];
+					pEntry = MacTableInsertDlsEntry(pAd, SA, i);
+
+					switch (MaxSupportedRateIn500Kbps)
+					{
+						case 108: MaxSupportedRate = RATE_54;   break;
+						case 96:  MaxSupportedRate = RATE_48;   break;
+						case 72:  MaxSupportedRate = RATE_36;   break;
+						case 48:  MaxSupportedRate = RATE_24;   break;
+						case 36:  MaxSupportedRate = RATE_18;   break;
+						case 24:  MaxSupportedRate = RATE_12;   break;
+						case 18:  MaxSupportedRate = RATE_9;    break;
+						case 12:  MaxSupportedRate = RATE_6;    break;
+						case 22:  MaxSupportedRate = RATE_11;   break;
+						case 11:  MaxSupportedRate = RATE_5_5;  break;
+						case 4:   MaxSupportedRate = RATE_2;    break;
+						case 2:   MaxSupportedRate = RATE_1;    break;
+						default:  MaxSupportedRate = RATE_11;   break;
+					}
+
+					pEntry->MaxSupportedRate = min(pAd->CommonCfg.MaxTxRate, MaxSupportedRate);
+
+					if (pEntry->MaxSupportedRate < RATE_FIRST_OFDM_RATE)
+					{
+						pEntry->MaxHTPhyMode.field.MODE = MODE_CCK;
+						pEntry->MaxHTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+						pEntry->MinHTPhyMode.field.MODE = MODE_CCK;
+						pEntry->MinHTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+						pEntry->HTPhyMode.field.MODE = MODE_CCK;
+						pEntry->HTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+					}
+					else
+					{
+						pEntry->MaxHTPhyMode.field.MODE = MODE_OFDM;
+						pEntry->MaxHTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+						pEntry->MinHTPhyMode.field.MODE = MODE_OFDM;
+						pEntry->MinHTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+						pEntry->HTPhyMode.field.MODE = MODE_OFDM;
+						pEntry->HTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+					}
+
+					pEntry->MaxHTPhyMode.field.BW = BW_20;
+					pEntry->MinHTPhyMode.field.BW = BW_20;
+
+#ifdef DOT11_N_SUPPORT
+					pEntry->HTCapability.MCSSet[0] = 0;
+					pEntry->HTCapability.MCSSet[1] = 0;
+
+					// If this Entry supports 802.11n, upgrade to HT rate.
+					if ((HtCapabilityLen != 0) && (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+					{
+						UCHAR	j, bitmask; //k,bitmask;
+						CHAR    ii;
+
+						DBGPRINT(RT_DEBUG_OFF, ("DLS - PeerDlsReqAction() Receive Peer HT Capable STA from %02x:%02x:%02x:%02x:%02x:%02x\n",
+									SA[0], SA[1], SA[2], SA[3], SA[4], SA[5]));
+
+						if ((HtCapability.HtCapInfo.GF) && (pAd->CommonCfg.DesiredHtPhy.GF))
+						{
+							pEntry->MaxHTPhyMode.field.MODE = MODE_HTGREENFIELD;
+						}
+						else
+						{
+							pEntry->MaxHTPhyMode.field.MODE = MODE_HTMIX;
+							pAd->MacTab.fAnyStationNonGF = TRUE;
+							pAd->CommonCfg.AddHTInfo.AddHtInfo2.NonGfPresent = 1;
+						}
+
+						if ((HtCapability.HtCapInfo.ChannelWidth) && (pAd->CommonCfg.DesiredHtPhy.ChannelWidth))
+						{
+							pEntry->MaxHTPhyMode.field.BW= BW_40;
+							pEntry->MaxHTPhyMode.field.ShortGI = ((pAd->CommonCfg.DesiredHtPhy.ShortGIfor40)&(HtCapability.HtCapInfo.ShortGIfor40));
+						}
+						else
+						{
+							pEntry->MaxHTPhyMode.field.BW = BW_20;
+							pEntry->MaxHTPhyMode.field.ShortGI = ((pAd->CommonCfg.DesiredHtPhy.ShortGIfor20)&(HtCapability.HtCapInfo.ShortGIfor20));
+							pAd->MacTab.fAnyStation20Only = TRUE;
+						}
+
+						// find max fixed rate
+						for (ii=15; ii>=0; ii--)
+						{
+							j = ii/8;
+							bitmask = (1<<(ii-(j*8)));
+							if ( (pAd->StaCfg.DesiredHtPhyInfo.MCSSet[j]&bitmask) && (HtCapability.MCSSet[j]&bitmask))
+							{
+								pEntry->MaxHTPhyMode.field.MCS = ii;
+								break;
+							}
+							if (ii==0)
+								break;
+						}
+
+
+						if (pAd->StaCfg.DesiredTransmitSetting.field.MCS != MCS_AUTO)
+						{
+
+							printk("@@@ pAd->CommonCfg.RegTransmitSetting.field.MCS = %d\n",
+								pAd->StaCfg.DesiredTransmitSetting.field.MCS);
+							if (pAd->StaCfg.DesiredTransmitSetting.field.MCS == 32)
+							{
+								// Fix MCS as HT Duplicated Mode
+								pEntry->MaxHTPhyMode.field.BW = 1;
+								pEntry->MaxHTPhyMode.field.MODE = MODE_HTMIX;
+								pEntry->MaxHTPhyMode.field.STBC = 0;
+								pEntry->MaxHTPhyMode.field.ShortGI = 0;
+								pEntry->MaxHTPhyMode.field.MCS = 32;
+							}
+							else if (pEntry->MaxHTPhyMode.field.MCS > pAd->StaCfg.HTPhyMode.field.MCS)
+							{
+								// STA supports fixed MCS
+								pEntry->MaxHTPhyMode.field.MCS = pAd->StaCfg.HTPhyMode.field.MCS;
+							}
+						}
+
+						pEntry->MaxHTPhyMode.field.STBC = (HtCapability.HtCapInfo.RxSTBC & (pAd->CommonCfg.DesiredHtPhy.TxSTBC));
+						pEntry->MpduDensity = HtCapability.HtCapParm.MpduDensity;
+						pEntry->MaxRAmpduFactor = HtCapability.HtCapParm.MaxRAmpduFactor;
+						pEntry->MmpsMode = (UCHAR)HtCapability.HtCapInfo.MimoPs;
+						pEntry->AMsduSize = (UCHAR)HtCapability.HtCapInfo.AMsduSize;
+						pEntry->HTPhyMode.word = pEntry->MaxHTPhyMode.word;
+
+						if (HtCapability.HtCapInfo.ShortGIfor20)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_SGI20_CAPABLE);
+						if (HtCapability.HtCapInfo.ShortGIfor40)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_SGI40_CAPABLE);
+						if (HtCapability.HtCapInfo.TxSTBC)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_TxSTBC_CAPABLE);
+						if (HtCapability.HtCapInfo.RxSTBC)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_RxSTBC_CAPABLE);
+						if (HtCapability.ExtHtCapInfo.PlusHTC)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_HTC_CAPABLE);
+						if (pAd->CommonCfg.bRdg && HtCapability.ExtHtCapInfo.RDGSupport)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_RDG_CAPABLE);
+						if (HtCapability.ExtHtCapInfo.MCSFeedback == 0x03)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_MCSFEEDBACK_CAPABLE);
+
+						NdisMoveMemory(&pEntry->HTCapability, &HtCapability, sizeof(HT_CAPABILITY_IE));
+					}
+#endif // DOT11_N_SUPPORT //
+
+					pEntry->HTPhyMode.word = pEntry->MaxHTPhyMode.word;
+					pEntry->CurrTxRate = pEntry->MaxSupportedRate;
+					CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_WMM_CAPABLE);
+
+					if (pAd->StaCfg.bAutoTxRateSwitch == TRUE)
+					{
+						PUCHAR pTable;
+						UCHAR TableSize = 0;
+
+						MlmeSelectTxRateTable(pAd, pEntry, &pTable, &TableSize, &pEntry->CurrTxRateIndex);
+						pEntry->bAutoTxRateSwitch = TRUE;
+					}
+					else
+					{
+						pEntry->HTPhyMode.field.MODE	= pAd->StaCfg.HTPhyMode.field.MODE;
+						pEntry->HTPhyMode.field.MCS	= pAd->StaCfg.HTPhyMode.field.MCS;
+						pEntry->bAutoTxRateSwitch = FALSE;
+
+						RTMPUpdateLegacyTxSetting((UCHAR)pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode, pEntry);
+					}
+					pEntry->RateLen = SupportedRatesLen;
+
+					break;
+				}
+			}
+		}
+		StatusCode = MLME_SUCCESS;
+
+		// can not find in table, create a new one
+		if (i < 0)
+		{
+			StatusCode = MLME_QOS_UNSPECIFY;
+			DBGPRINT(RT_DEBUG_ERROR,("DLS - PeerDlsReqAction() DLSEntry table full(only can support %d DLS session) \n", MAX_NUM_OF_DLS_ENTRY - MAX_NUM_OF_INIT_DLS_ENTRY));
+		}
+		else
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("DLS - PeerDlsReqAction() use entry(%d) %02x:%02x:%02x:%02x:%02x:%02x\n",
+				i, SA[0], SA[1], SA[2], SA[3], SA[4], SA[5]));
+		}
+	}
+
+	ActHeaderInit(pAd, &DlsRspHdr, pAd->CommonCfg.Bssid, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+
+	// Build basic frame first
+	if (StatusCode == MLME_SUCCESS)
+	{
+		MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+						sizeof(HEADER_802_11),		&DlsRspHdr,
+						1,							&Category,
+						1,							&Action,
+						2,							&StatusCode,
+						6,							SA,
+						6,							pAd->CurrentAddress,
+						2,							&pAd->StaActive.CapabilityInfo,
+						1,							&SupRateIe,
+						1,							&pAd->MlmeAux.SupRateLen,
+						pAd->MlmeAux.SupRateLen,	pAd->MlmeAux.SupRate,
+						END_OF_ARGS);
+
+		if (pAd->MlmeAux.ExtRateLen != 0)
+		{
+			MakeOutgoingFrame(pOutBuffer + FrameLen,	&tmp,
+							  1,						&ExtRateIe,
+							  1,						&pAd->MlmeAux.ExtRateLen,
+							  pAd->MlmeAux.ExtRateLen, 	pAd->MlmeAux.ExtRate,
+							  END_OF_ARGS);
+			FrameLen += tmp;
+		}
+
+#ifdef DOT11_N_SUPPORT
+		if ((pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+		{
+			UCHAR HtLen;
+
+#ifdef RT_BIG_ENDIAN
+			HT_CAPABILITY_IE HtCapabilityTmp;
+#endif
+
+			// add HT Capability IE
+			HtLen = sizeof(HT_CAPABILITY_IE);
+#ifndef RT_BIG_ENDIAN
+			MakeOutgoingFrame(pOutBuffer + FrameLen,	&tmp,
+								1,						&HtCapIe,
+								1,						&HtLen,
+								HtLen,					&pAd->CommonCfg.HtCapability,
+								END_OF_ARGS);
+#else
+			NdisMoveMemory(&HtCapabilityTmp, &pAd->CommonCfg.HtCapability, HtLen);
+								*(USHORT *)(&HtCapabilityTmp.HtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.HtCapInfo));
+								*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo));
+
+			MakeOutgoingFrame(pOutBuffer + FrameLen,	&tmp,
+								1,						&HtCapIe,
+								1,						&HtLen,
+								HtLen,					&HtCapabilityTmp,
+								END_OF_ARGS);
+#endif
+			FrameLen = FrameLen + tmp;
+		}
+#endif // DOT11_N_SUPPORT //
+
+		if (pDLS && (pDLS->Status != DLS_FINISH))
+		{
+			RTMPCancelTimer(&pDLS->Timer, &TimerCancelled);
+			Timeout = DLS_TIMEOUT;
+			RTMPSetTimer(&pDLS->Timer, Timeout);
+		}
+	}
+	else
+	{
+		MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+						sizeof(HEADER_802_11),		&DlsRspHdr,
+						1,							&Category,
+						1,							&Action,
+						2,							&StatusCode,
+						6,							SA,
+						6,							pAd->CurrentAddress,
+						END_OF_ARGS);
+	}
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID PeerDlsRspAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT		CapabilityInfo;
+	UCHAR		DA[MAC_ADDR_LEN], SA[MAC_ADDR_LEN];
+	USHORT		StatusCode;
+	SHORT		i;
+	BOOLEAN		TimerCancelled;
+	UCHAR		MaxSupportedRateIn500Kbps = 0;
+    UCHAR		SupportedRatesLen;
+    UCHAR		SupportedRates[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR		HtCapabilityLen;
+	HT_CAPABILITY_IE	HtCapability;
+
+	if (!pAd->CommonCfg.bDLSCapable)
+		return;
+
+	if (!INFRA_ON(pAd))
+		return;
+
+	if (!PeerDlsRspSanity(pAd, Elem->Msg, Elem->MsgLen, DA, SA, &CapabilityInfo, &StatusCode,
+							&SupportedRatesLen, &SupportedRates[0], &HtCapabilityLen, &HtCapability))
+		return;
+
+    // supported rates array may not be sorted. sort it and find the maximum rate
+    for (i=0; i<SupportedRatesLen; i++)
+    {
+        if (MaxSupportedRateIn500Kbps < (SupportedRates[i] & 0x7f))
+            MaxSupportedRateIn500Kbps = SupportedRates[i] & 0x7f;
+    }
+
+	DBGPRINT(RT_DEBUG_TRACE,("DLS - PeerDlsRspAction() from %02x:%02x:%02x:%02x:%02x:%02x with StatusCode=%d, CapabilityInfo=0x%x\n",
+		SA[0], SA[1], SA[2], SA[3], SA[4], SA[5], StatusCode, CapabilityInfo));
+
+	for (i = 0; i < MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+	{
+		if (pAd->StaCfg.DLSEntry[i].Valid && MAC_ADDR_EQUAL(SA, pAd->StaCfg.DLSEntry[i].MacAddr))
+		{
+			if (StatusCode == MLME_SUCCESS)
+			{
+				MAC_TABLE_ENTRY *pEntry;
+				UCHAR MaxSupportedRate = RATE_11;
+
+				pEntry = MacTableInsertDlsEntry(pAd, SA, i);
+
+				switch (MaxSupportedRateIn500Kbps)
+				{
+					case 108: MaxSupportedRate = RATE_54;   break;
+					case 96:  MaxSupportedRate = RATE_48;   break;
+					case 72:  MaxSupportedRate = RATE_36;   break;
+					case 48:  MaxSupportedRate = RATE_24;   break;
+					case 36:  MaxSupportedRate = RATE_18;   break;
+					case 24:  MaxSupportedRate = RATE_12;   break;
+					case 18:  MaxSupportedRate = RATE_9;    break;
+					case 12:  MaxSupportedRate = RATE_6;    break;
+					case 22:  MaxSupportedRate = RATE_11;   break;
+					case 11:  MaxSupportedRate = RATE_5_5;  break;
+					case 4:   MaxSupportedRate = RATE_2;    break;
+					case 2:   MaxSupportedRate = RATE_1;    break;
+					default:  MaxSupportedRate = RATE_11;   break;
+				}
+
+				pEntry->MaxSupportedRate = min(pAd->CommonCfg.MaxTxRate, MaxSupportedRate);
+
+				if (pEntry->MaxSupportedRate < RATE_FIRST_OFDM_RATE)
+				{
+					pEntry->MaxHTPhyMode.field.MODE = MODE_CCK;
+					pEntry->MaxHTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+					pEntry->MinHTPhyMode.field.MODE = MODE_CCK;
+					pEntry->MinHTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+					pEntry->HTPhyMode.field.MODE = MODE_CCK;
+					pEntry->HTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+				}
+				else
+				{
+					pEntry->MaxHTPhyMode.field.MODE = MODE_OFDM;
+					pEntry->MaxHTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+					pEntry->MinHTPhyMode.field.MODE = MODE_OFDM;
+					pEntry->MinHTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+					pEntry->HTPhyMode.field.MODE = MODE_OFDM;
+					pEntry->HTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+				}
+
+				pEntry->MaxHTPhyMode.field.BW = BW_20;
+				pEntry->MinHTPhyMode.field.BW = BW_20;
+
+#ifdef DOT11_N_SUPPORT
+				pEntry->HTCapability.MCSSet[0] = 0;
+				pEntry->HTCapability.MCSSet[1] = 0;
+
+				// If this Entry supports 802.11n, upgrade to HT rate.
+				if ((HtCapabilityLen != 0) && (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+				{
+					UCHAR	j, bitmask; //k,bitmask;
+					CHAR    ii;
+
+					DBGPRINT(RT_DEBUG_OFF, ("DLS - PeerDlsRspAction Receive Peer HT Capable STA from %02x:%02x:%02x:%02x:%02x:%02x\n",
+								SA[0], SA[1], SA[2], SA[3], SA[4], SA[5]));
+
+					if ((HtCapability.HtCapInfo.GF) && (pAd->CommonCfg.DesiredHtPhy.GF))
+					{
+						pEntry->MaxHTPhyMode.field.MODE = MODE_HTGREENFIELD;
+					}
+					else
+					{
+						pEntry->MaxHTPhyMode.field.MODE = MODE_HTMIX;
+						pAd->MacTab.fAnyStationNonGF = TRUE;
+						pAd->CommonCfg.AddHTInfo.AddHtInfo2.NonGfPresent = 1;
+					}
+
+					if ((HtCapability.HtCapInfo.ChannelWidth) && (pAd->CommonCfg.DesiredHtPhy.ChannelWidth))
+					{
+						pEntry->MaxHTPhyMode.field.BW= BW_40;
+						pEntry->MaxHTPhyMode.field.ShortGI = ((pAd->CommonCfg.DesiredHtPhy.ShortGIfor40)&(HtCapability.HtCapInfo.ShortGIfor40));
+					}
+					else
+					{
+						pEntry->MaxHTPhyMode.field.BW = BW_20;
+						pEntry->MaxHTPhyMode.field.ShortGI = ((pAd->CommonCfg.DesiredHtPhy.ShortGIfor20)&(HtCapability.HtCapInfo.ShortGIfor20));
+						pAd->MacTab.fAnyStation20Only = TRUE;
+					}
+
+					// find max fixed rate
+					for (ii=15; ii>=0; ii--)
+					{
+						j = ii/8;
+						bitmask = (1<<(ii-(j*8)));
+						if ( (pAd->StaCfg.DesiredHtPhyInfo.MCSSet[j]&bitmask) && (HtCapability.MCSSet[j]&bitmask))
+						{
+							pEntry->MaxHTPhyMode.field.MCS = ii;
+							break;
+						}
+						if (ii==0)
+							break;
+					}
+
+					if (pAd->StaCfg.DesiredTransmitSetting.field.MCS != MCS_AUTO)
+					{
+						if (pAd->StaCfg.DesiredTransmitSetting.field.MCS == 32)
+						{
+							// Fix MCS as HT Duplicated Mode
+							pEntry->MaxHTPhyMode.field.BW = 1;
+							pEntry->MaxHTPhyMode.field.MODE = MODE_HTMIX;
+							pEntry->MaxHTPhyMode.field.STBC = 0;
+							pEntry->MaxHTPhyMode.field.ShortGI = 0;
+							pEntry->MaxHTPhyMode.field.MCS = 32;
+						}
+						else if (pEntry->MaxHTPhyMode.field.MCS > pAd->StaCfg.HTPhyMode.field.MCS)
+						{
+							// STA supports fixed MCS
+							pEntry->MaxHTPhyMode.field.MCS = pAd->StaCfg.HTPhyMode.field.MCS;
+						}
+					}
+
+					pEntry->MaxHTPhyMode.field.STBC = (HtCapability.HtCapInfo.RxSTBC & (pAd->CommonCfg.DesiredHtPhy.TxSTBC));
+					pEntry->MpduDensity = HtCapability.HtCapParm.MpduDensity;
+					pEntry->MaxRAmpduFactor = HtCapability.HtCapParm.MaxRAmpduFactor;
+					pEntry->MmpsMode = (UCHAR)HtCapability.HtCapInfo.MimoPs;
+					pEntry->AMsduSize = (UCHAR)HtCapability.HtCapInfo.AMsduSize;
+					pEntry->HTPhyMode.word = pEntry->MaxHTPhyMode.word;
+
+					if (HtCapability.HtCapInfo.ShortGIfor20)
+						CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_SGI20_CAPABLE);
+					if (HtCapability.HtCapInfo.ShortGIfor40)
+						CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_SGI40_CAPABLE);
+					if (HtCapability.HtCapInfo.TxSTBC)
+						CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_TxSTBC_CAPABLE);
+					if (HtCapability.HtCapInfo.RxSTBC)
+						CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_RxSTBC_CAPABLE);
+					if (HtCapability.ExtHtCapInfo.PlusHTC)
+						CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_HTC_CAPABLE);
+					if (pAd->CommonCfg.bRdg && HtCapability.ExtHtCapInfo.RDGSupport)
+						CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_RDG_CAPABLE);
+					if (HtCapability.ExtHtCapInfo.MCSFeedback == 0x03)
+						CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_MCSFEEDBACK_CAPABLE);
+
+					NdisMoveMemory(&pEntry->HTCapability, &HtCapability, sizeof(HT_CAPABILITY_IE));
+				}
+#endif // DOT11_N_SUPPORT //
+				pEntry->HTPhyMode.word = pEntry->MaxHTPhyMode.word;
+				pEntry->CurrTxRate = pEntry->MaxSupportedRate;
+				CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_WMM_CAPABLE);
+
+				if (pAd->StaCfg.bAutoTxRateSwitch == TRUE)
+				{
+					PUCHAR pTable;
+					UCHAR TableSize = 0;
+
+					MlmeSelectTxRateTable(pAd, pEntry, &pTable, &TableSize, &pEntry->CurrTxRateIndex);
+					pEntry->bAutoTxRateSwitch = TRUE;
+				}
+				else
+				{
+					pEntry->HTPhyMode.field.MODE	= pAd->StaCfg.HTPhyMode.field.MODE;
+					pEntry->HTPhyMode.field.MCS	= pAd->StaCfg.HTPhyMode.field.MCS;
+					pEntry->bAutoTxRateSwitch = FALSE;
+
+					RTMPUpdateLegacyTxSetting((UCHAR)pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode, pEntry);
+				}
+				pEntry->RateLen = SupportedRatesLen;
+
+				if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+				{
+					// If support WPA or WPA2, start STAKey hand shake,
+					// If failed hand shake, just tear down peer DLS
+					if (RTMPSendSTAKeyRequest(pAd, pAd->StaCfg.DLSEntry[i].MacAddr) != NDIS_STATUS_SUCCESS)
+					{
+						MLME_DLS_REQ_STRUCT	MlmeDlsReq;
+						USHORT				reason = REASON_QOS_CIPHER_NOT_SUPPORT;
+
+						DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+						MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_TEAR_DOWN, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+						pAd->StaCfg.DLSEntry[i].Status = DLS_NONE;
+						pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+						DBGPRINT(RT_DEBUG_ERROR,("DLS - PeerDlsRspAction failed when call RTMPSendSTAKeyRequest \n"));
+					}
+					else
+					{
+						pAd->StaCfg.DLSEntry[i].Status = DLS_WAIT_KEY;
+						DBGPRINT(RT_DEBUG_TRACE,("DLS - waiting for STAKey handshake procedure\n"));
+					}
+				}
+				else
+				{
+					RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+					pAd->StaCfg.DLSEntry[i].Status = DLS_FINISH;
+					DBGPRINT(RT_DEBUG_TRACE,("DLS - PeerDlsRspAction() from %02x:%02x:%02x:%02x:%02x:%02x Succeed with WEP or no security\n", SA[0], SA[1], SA[2], SA[3], SA[4], SA[5]));
+				}
+
+				//initialize seq no for DLS frames.
+				pAd->StaCfg.DLSEntry[i].Sequence = 0;
+				if (HtCapabilityLen != 0)
+					pAd->StaCfg.DLSEntry[i].bHTCap = TRUE;
+				else
+					pAd->StaCfg.DLSEntry[i].bHTCap = FALSE;
+			}
+			else
+			{
+				// DLS setup procedure failed.
+				pAd->StaCfg.DLSEntry[i].Status = DLS_NONE;
+				pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+				RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+				DBGPRINT(RT_DEBUG_ERROR,("DLS - PeerDlsRspAction failed with StatusCode=%d \n", StatusCode));
+			}
+		}
+	}
+
+	if (i >= MAX_NUM_OF_INIT_DLS_ENTRY)
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("DLS - PeerDlsRspAction() update timeout value \n"));
+		for (i=(MAX_NUM_OF_DLS_ENTRY-1); i>=MAX_NUM_OF_INIT_DLS_ENTRY; i--)
+		{
+			if (pAd->StaCfg.DLSEntry[i].Valid && MAC_ADDR_EQUAL(SA, pAd->StaCfg.DLSEntry[i].MacAddr))
+			{
+				if (StatusCode == MLME_SUCCESS)
+				{
+					MAC_TABLE_ENTRY *pEntry;
+					UCHAR MaxSupportedRate = RATE_11;
+
+					pEntry = MacTableInsertDlsEntry(pAd, SA, i);
+
+					switch (MaxSupportedRateIn500Kbps)
+					{
+						case 108: MaxSupportedRate = RATE_54;   break;
+						case 96:  MaxSupportedRate = RATE_48;   break;
+						case 72:  MaxSupportedRate = RATE_36;   break;
+						case 48:  MaxSupportedRate = RATE_24;   break;
+						case 36:  MaxSupportedRate = RATE_18;   break;
+						case 24:  MaxSupportedRate = RATE_12;   break;
+						case 18:  MaxSupportedRate = RATE_9;    break;
+						case 12:  MaxSupportedRate = RATE_6;    break;
+						case 22:  MaxSupportedRate = RATE_11;   break;
+						case 11:  MaxSupportedRate = RATE_5_5;  break;
+						case 4:   MaxSupportedRate = RATE_2;    break;
+						case 2:   MaxSupportedRate = RATE_1;    break;
+						default:  MaxSupportedRate = RATE_11;   break;
+					}
+
+					pEntry->MaxSupportedRate = min(pAd->CommonCfg.MaxTxRate, MaxSupportedRate);
+
+					if (pEntry->MaxSupportedRate < RATE_FIRST_OFDM_RATE)
+					{
+						pEntry->MaxHTPhyMode.field.MODE = MODE_CCK;
+						pEntry->MaxHTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+						pEntry->MinHTPhyMode.field.MODE = MODE_CCK;
+						pEntry->MinHTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+						pEntry->HTPhyMode.field.MODE = MODE_CCK;
+						pEntry->HTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+					}
+					else
+					{
+						pEntry->MaxHTPhyMode.field.MODE = MODE_OFDM;
+						pEntry->MaxHTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+						pEntry->MinHTPhyMode.field.MODE = MODE_OFDM;
+						pEntry->MinHTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+						pEntry->HTPhyMode.field.MODE = MODE_OFDM;
+						pEntry->HTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+					}
+
+					pEntry->MaxHTPhyMode.field.BW = BW_20;
+					pEntry->MinHTPhyMode.field.BW = BW_20;
+
+#ifdef DOT11_N_SUPPORT
+					pEntry->HTCapability.MCSSet[0] = 0;
+					pEntry->HTCapability.MCSSet[1] = 0;
+
+					// If this Entry supports 802.11n, upgrade to HT rate.
+					if ((HtCapabilityLen != 0) && (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+					{
+						UCHAR	j, bitmask; //k,bitmask;
+						CHAR    ii;
+
+						DBGPRINT(RT_DEBUG_OFF, ("DLS - PeerDlsRspAction Receive Peer HT Capable STA from %02x:%02x:%02x:%02x:%02x:%02x\n",
+									SA[0], SA[1], SA[2], SA[3], SA[4], SA[5]));
+
+						if ((HtCapability.HtCapInfo.GF) && (pAd->CommonCfg.DesiredHtPhy.GF))
+						{
+							pEntry->MaxHTPhyMode.field.MODE = MODE_HTGREENFIELD;
+						}
+						else
+						{
+							pEntry->MaxHTPhyMode.field.MODE = MODE_HTMIX;
+							pAd->MacTab.fAnyStationNonGF = TRUE;
+							pAd->CommonCfg.AddHTInfo.AddHtInfo2.NonGfPresent = 1;
+						}
+
+						if ((HtCapability.HtCapInfo.ChannelWidth) && (pAd->CommonCfg.DesiredHtPhy.ChannelWidth))
+						{
+							pEntry->MaxHTPhyMode.field.BW= BW_40;
+							pEntry->MaxHTPhyMode.field.ShortGI = ((pAd->CommonCfg.DesiredHtPhy.ShortGIfor40)&(HtCapability.HtCapInfo.ShortGIfor40));
+						}
+						else
+						{
+							pEntry->MaxHTPhyMode.field.BW = BW_20;
+							pEntry->MaxHTPhyMode.field.ShortGI = ((pAd->CommonCfg.DesiredHtPhy.ShortGIfor20)&(HtCapability.HtCapInfo.ShortGIfor20));
+							pAd->MacTab.fAnyStation20Only = TRUE;
+						}
+
+						// find max fixed rate
+						for (ii=15; ii>=0; ii--)
+						{
+							j = ii/8;
+							bitmask = (1<<(ii-(j*8)));
+							if ( (pAd->StaCfg.DesiredHtPhyInfo.MCSSet[j]&bitmask) && (HtCapability.MCSSet[j]&bitmask))
+							{
+								pEntry->MaxHTPhyMode.field.MCS = ii;
+								break;
+							}
+							if (ii==0)
+								break;
+						}
+
+						if (pAd->StaCfg.DesiredTransmitSetting.field.MCS != MCS_AUTO)
+						{
+							printk("@@@ pAd->CommonCfg.RegTransmitSetting.field.MCS = %d\n",
+								pAd->StaCfg.DesiredTransmitSetting.field.MCS);
+							if (pAd->StaCfg.DesiredTransmitSetting.field.MCS == 32)
+							{
+								// Fix MCS as HT Duplicated Mode
+								pEntry->MaxHTPhyMode.field.BW = 1;
+								pEntry->MaxHTPhyMode.field.MODE = MODE_HTMIX;
+								pEntry->MaxHTPhyMode.field.STBC = 0;
+								pEntry->MaxHTPhyMode.field.ShortGI = 0;
+								pEntry->MaxHTPhyMode.field.MCS = 32;
+							}
+							else if (pEntry->MaxHTPhyMode.field.MCS > pAd->StaCfg.HTPhyMode.field.MCS)
+							{
+								// STA supports fixed MCS
+								pEntry->MaxHTPhyMode.field.MCS = pAd->StaCfg.HTPhyMode.field.MCS;
+							}
+						}
+
+						pEntry->MaxHTPhyMode.field.STBC = (HtCapability.HtCapInfo.RxSTBC & (pAd->CommonCfg.DesiredHtPhy.TxSTBC));
+						pEntry->MpduDensity = HtCapability.HtCapParm.MpduDensity;
+						pEntry->MaxRAmpduFactor = HtCapability.HtCapParm.MaxRAmpduFactor;
+						pEntry->MmpsMode = (UCHAR)HtCapability.HtCapInfo.MimoPs;
+						pEntry->AMsduSize = (UCHAR)HtCapability.HtCapInfo.AMsduSize;
+						pEntry->HTPhyMode.word = pEntry->MaxHTPhyMode.word;
+
+						if (HtCapability.HtCapInfo.ShortGIfor20)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_SGI20_CAPABLE);
+						if (HtCapability.HtCapInfo.ShortGIfor40)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_SGI40_CAPABLE);
+						if (HtCapability.HtCapInfo.TxSTBC)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_TxSTBC_CAPABLE);
+						if (HtCapability.HtCapInfo.RxSTBC)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_RxSTBC_CAPABLE);
+						if (HtCapability.ExtHtCapInfo.PlusHTC)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_HTC_CAPABLE);
+						if (pAd->CommonCfg.bRdg && HtCapability.ExtHtCapInfo.RDGSupport)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_RDG_CAPABLE);
+						if (HtCapability.ExtHtCapInfo.MCSFeedback == 0x03)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_MCSFEEDBACK_CAPABLE);
+
+						NdisMoveMemory(&pEntry->HTCapability, &HtCapability, sizeof(HT_CAPABILITY_IE));
+					}
+#endif // DOT11_N_SUPPORT //
+
+					pEntry->HTPhyMode.word = pEntry->MaxHTPhyMode.word;
+					pEntry->CurrTxRate = pEntry->MaxSupportedRate;
+					CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_WMM_CAPABLE);
+
+					if (pAd->StaCfg.bAutoTxRateSwitch == TRUE)
+					{
+						PUCHAR pTable;
+						UCHAR TableSize = 0;
+
+						MlmeSelectTxRateTable(pAd, pEntry, &pTable, &TableSize, &pEntry->CurrTxRateIndex);
+						pEntry->bAutoTxRateSwitch = TRUE;
+					}
+					else
+					{
+						pEntry->HTPhyMode.field.MODE	= pAd->StaCfg.HTPhyMode.field.MODE;
+						pEntry->HTPhyMode.field.MCS	= pAd->StaCfg.HTPhyMode.field.MCS;
+						pEntry->bAutoTxRateSwitch = FALSE;
+
+						RTMPUpdateLegacyTxSetting((UCHAR)pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode, pEntry);
+					}
+					pEntry->RateLen = SupportedRatesLen;
+
+					if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+					{
+						// If support WPA or WPA2, start STAKey hand shake,
+						// If failed hand shake, just tear down peer DLS
+						if (RTMPSendSTAKeyRequest(pAd, pAd->StaCfg.DLSEntry[i].MacAddr) != NDIS_STATUS_SUCCESS)
+						{
+							MLME_DLS_REQ_STRUCT	MlmeDlsReq;
+							USHORT				reason = REASON_QOS_CIPHER_NOT_SUPPORT;
+
+							DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+							MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_TEAR_DOWN, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+							pAd->StaCfg.DLSEntry[i].Status = DLS_NONE;
+							pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+							DBGPRINT(RT_DEBUG_ERROR,("DLS - PeerDlsRspAction failed when call RTMPSendSTAKeyRequest \n"));
+						}
+						else
+						{
+							pAd->StaCfg.DLSEntry[i].Status = DLS_WAIT_KEY;
+							DBGPRINT(RT_DEBUG_TRACE,("DLS - waiting for STAKey handshake procedure\n"));
+						}
+					}
+					else
+					{
+						RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+						pAd->StaCfg.DLSEntry[i].Status = DLS_FINISH;
+						DBGPRINT(RT_DEBUG_TRACE,("DLS - PeerDlsRspAction() from %02x:%02x:%02x:%02x:%02x:%02x Succeed with WEP or no security\n", SA[0], SA[1], SA[2], SA[3], SA[4], SA[5]));
+					}
+					pAd->StaCfg.DLSEntry[i].Sequence = 0;
+					if (HtCapabilityLen != 0)
+						pAd->StaCfg.DLSEntry[i].bHTCap = TRUE;
+					else
+						pAd->StaCfg.DLSEntry[i].bHTCap = FALSE;
+				}
+				else
+				{
+					// DLS setup procedure failed.
+					pAd->StaCfg.DLSEntry[i].Status = DLS_NONE;
+					pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+					RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+					DBGPRINT(RT_DEBUG_ERROR,("DLS - PeerDlsRspAction failed with StatusCode=%d \n", StatusCode));
+				}
+			}
+		}
+	}
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID MlmeDlsTearDownAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+	PUCHAR			pOutBuffer = NULL;
+	NDIS_STATUS		NStatus;
+	ULONG			FrameLen = 0;
+	UCHAR			Category = CATEGORY_DLS;
+	UCHAR			Action = ACTION_DLS_TEARDOWN;
+	USHORT			ReasonCode = REASON_QOS_UNSPECIFY;
+	HEADER_802_11	DlsTearDownHdr;
+	PRT_802_11_DLS	pDLS;
+	BOOLEAN			TimerCancelled;
+	UCHAR			i;
+
+	if(!MlmeDlsReqSanity(pAd, Elem->Msg, Elem->MsgLen, &pDLS, &ReasonCode))
+		return;
+
+	DBGPRINT(RT_DEBUG_TRACE,("DLS - MlmeDlsTearDownAction() with ReasonCode=%d \n", ReasonCode));
+
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if (NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("DLS - MlmeDlsTearDownAction() allocate memory failed \n"));
+		return;
+	}
+
+	ActHeaderInit(pAd, &DlsTearDownHdr, pAd->CommonCfg.Bssid, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+
+	// Build basic frame first
+	MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+					sizeof(HEADER_802_11),		&DlsTearDownHdr,
+					1,							&Category,
+					1,							&Action,
+					6,							&pDLS->MacAddr,
+					6,							pAd->CurrentAddress,
+					2,							&ReasonCode,
+					END_OF_ARGS);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+	RTMPCancelTimer(&pDLS->Timer, &TimerCancelled);
+
+	// Remove key in local dls table entry
+	for (i = 0; i < MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+	{
+		if (MAC_ADDR_EQUAL(pDLS->MacAddr, pAd->StaCfg.DLSEntry[i].MacAddr))
+		{
+			MacTableDeleteDlsEntry(pAd, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID, pAd->StaCfg.DLSEntry[i].MacAddr);
+		}
+	}
+
+	// clear peer dls table entry
+	for (i = MAX_NUM_OF_INIT_DLS_ENTRY; i < MAX_NUM_OF_DLS_ENTRY; i++)
+	{
+		if (MAC_ADDR_EQUAL(pDLS->MacAddr, pAd->StaCfg.DLSEntry[i].MacAddr))
+		{
+			pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+			pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+			RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+			MacTableDeleteDlsEntry(pAd, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID, pAd->StaCfg.DLSEntry[i].MacAddr);
+		}
+	}
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID PeerDlsTearDownAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR			DA[MAC_ADDR_LEN], SA[MAC_ADDR_LEN];
+	USHORT			ReasonCode;
+	UINT			i;
+	BOOLEAN			TimerCancelled;
+
+	if (!pAd->CommonCfg.bDLSCapable)
+		return;
+
+	if (!INFRA_ON(pAd))
+		return;
+
+	if (!PeerDlsTearDownSanity(pAd, Elem->Msg, Elem->MsgLen, DA, SA, &ReasonCode))
+		return;
+
+	DBGPRINT(RT_DEBUG_TRACE,("DLS - PeerDlsTearDownAction() from %02x:%02x:%02x:%02x:%02x:%02x with ReasonCode=%d\n", SA[0], SA[1], SA[2], SA[3], SA[4], SA[5], ReasonCode));
+
+	// clear local dls table entry
+	for (i=0; i<MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+	{
+		if (pAd->StaCfg.DLSEntry[i].Valid && MAC_ADDR_EQUAL(SA, pAd->StaCfg.DLSEntry[i].MacAddr))
+		{
+			pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+			pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+			RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+			//AsicDelWcidTab(pAd, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID);
+			//AsicRemovePairwiseKeyEntry(pAd, BSS0, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID);
+			MacTableDeleteDlsEntry(pAd, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID, pAd->StaCfg.DLSEntry[i].MacAddr);
+		}
+	}
+
+	// clear peer dls table entry
+	for (i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+	{
+		if (pAd->StaCfg.DLSEntry[i].Valid && MAC_ADDR_EQUAL(SA, pAd->StaCfg.DLSEntry[i].MacAddr))
+		{
+			pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+			pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+			RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+			//AsicDelWcidTab(pAd, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID);
+			//AsicRemovePairwiseKeyEntry(pAd, BSS0, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID);
+			MacTableDeleteDlsEntry(pAd, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID, pAd->StaCfg.DLSEntry[i].MacAddr);
+		}
+	}
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID RTMPCheckDLSTimeOut(
+	IN PRTMP_ADAPTER	pAd)
+{
+	ULONG				i;
+	MLME_DLS_REQ_STRUCT	MlmeDlsReq;
+	USHORT				reason = REASON_QOS_UNSPECIFY;
+
+	if (! pAd->CommonCfg.bDLSCapable)
+		return;
+
+	if (! INFRA_ON(pAd))
+		return;
+
+	// If timeout value is equaled to zero, it means always not be timeout.
+
+	// update local dls table entry
+	for (i=0; i<MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+	{
+		if ((pAd->StaCfg.DLSEntry[i].Valid) && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH)
+			&& (pAd->StaCfg.DLSEntry[i].TimeOut != 0))
+		{
+			pAd->StaCfg.DLSEntry[i].CountDownTimer --;
+
+			if (pAd->StaCfg.DLSEntry[i].CountDownTimer == 0)
+			{
+				reason = REASON_QOS_REQUEST_TIMEOUT;
+				pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+				pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+				DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+				MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_TEAR_DOWN, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+			}
+		}
+	}
+
+	// update peer dls table entry
+	for (i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+	{
+		if ((pAd->StaCfg.DLSEntry[i].Valid) && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH)
+			&& (pAd->StaCfg.DLSEntry[i].TimeOut != 0))
+		{
+			pAd->StaCfg.DLSEntry[i].CountDownTimer --;
+
+			if (pAd->StaCfg.DLSEntry[i].CountDownTimer == 0)
+			{
+				reason = REASON_QOS_REQUEST_TIMEOUT;
+				pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+				pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+				DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+				MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_TEAR_DOWN, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+			}
+		}
+	}
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN RTMPRcvFrameDLSCheck(
+	IN PRTMP_ADAPTER	pAd,
+	IN PHEADER_802_11	pHeader,
+	IN ULONG			Len,
+	IN PRT28XX_RXD_STRUC	pRxD)
+{
+	ULONG			i;
+	BOOLEAN			bFindEntry = FALSE;
+	BOOLEAN			bSTAKeyFrame = FALSE;
+	PEAPOL_PACKET	pEap;
+	PUCHAR			pProto, pAddr = NULL;
+	PUCHAR			pSTAKey = NULL;
+	UCHAR			ZeroReplay[LEN_KEY_DESC_REPLAY];
+	UCHAR			Mic[16], OldMic[16];
+	UCHAR			digest[80];
+	UCHAR			DlsPTK[80];
+	UCHAR			temp[64];
+	BOOLEAN			TimerCancelled;
+	CIPHER_KEY		PairwiseKey;
+
+
+	if (! pAd->CommonCfg.bDLSCapable)
+		return bSTAKeyFrame;
+
+	if (! INFRA_ON(pAd))
+		return bSTAKeyFrame;
+
+	if (! (pHeader->FC.SubType & 0x08))
+		return bSTAKeyFrame;
+
+	if (Len < LENGTH_802_11 + 6 + 2 + 2)
+		return bSTAKeyFrame;
+
+	pProto	= (PUCHAR)pHeader + LENGTH_802_11 + 2 + 6;	// QOS Control field , 0xAA 0xAA 0xAA 0x00 0x00 0x00
+	pAddr	= pHeader->Addr2;
+
+	// L2PAD bit on will pad 2 bytes at LLC
+	if (pRxD->L2PAD)
+	{
+		pProto += 2;
+	}
+
+	if (RTMPEqualMemory(EAPOL, pProto, 2) && (pAd->StaCfg.AuthMode >=  Ndis802_11AuthModeWPA))
+	{
+		pEap = (PEAPOL_PACKET) (pProto + 2);
+
+		DBGPRINT(RT_DEBUG_TRACE,("DLS - Sniff Len=%ld, DataLen=%d, KeyMic=%d, Install=%d, KeyAck=%d, Secure=%d, EKD_DL=%d, Error=%d, Request=%d\n", Len,
+			                                                             (LENGTH_802_11 + 6 + 2 + 2 + sizeof(EAPOL_PACKET) - MAX_LEN_OF_RSNIE + 16),
+			                                                             pEap->KeyDesc.KeyInfo.KeyMic,
+			                                                             pEap->KeyDesc.KeyInfo.Install,
+			                                                             pEap->KeyDesc.KeyInfo.KeyAck,
+			                                                             pEap->KeyDesc.KeyInfo.Secure,
+			                                                             pEap->KeyDesc.KeyInfo.EKD_DL,
+			                                                             pEap->KeyDesc.KeyInfo.Error,
+			                                                             pEap->KeyDesc.KeyInfo.Request));
+
+		if ((Len >= (LENGTH_802_11 + 6 + 2 + 2 + sizeof(EAPOL_PACKET) - MAX_LEN_OF_RSNIE + 16)) && pEap->KeyDesc.KeyInfo.KeyMic
+			&& pEap->KeyDesc.KeyInfo.Install && pEap->KeyDesc.KeyInfo.KeyAck && pEap->KeyDesc.KeyInfo.Secure
+			&& pEap->KeyDesc.KeyInfo.EKD_DL && !pEap->KeyDesc.KeyInfo.Error && !pEap->KeyDesc.KeyInfo.Request)
+		{
+			// First validate replay counter, only accept message with larger replay counter
+			// Let equal pass, some AP start with all zero replay counter
+			NdisZeroMemory(ZeroReplay, LEN_KEY_DESC_REPLAY);
+			if ((RTMPCompareMemory(pEap->KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY) != 1) &&
+				(RTMPCompareMemory(pEap->KeyDesc.ReplayCounter, ZeroReplay, LEN_KEY_DESC_REPLAY) != 0))
+				return bSTAKeyFrame;
+
+			//RTMPMoveMemory(pAd->StaCfg.ReplayCounter, pEap->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+			RTMPMoveMemory(pAd->StaCfg.DlsReplayCounter, pEap->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+			DBGPRINT(RT_DEBUG_TRACE,("DLS - Sniff replay counter (%02x-%02x-%02x-%02x-%02x-%02x-%02x-%02x) Len=%ld, KeyDataLen=%d\n",
+				pAd->StaCfg.ReplayCounter[0], pAd->StaCfg.ReplayCounter[1], pAd->StaCfg.ReplayCounter[2],
+				pAd->StaCfg.ReplayCounter[3], pAd->StaCfg.ReplayCounter[4],	pAd->StaCfg.ReplayCounter[5],
+				pAd->StaCfg.ReplayCounter[6], pAd->StaCfg.ReplayCounter[7], Len, pEap->KeyDesc.KeyData[1]));
+
+			// put these code segment to get the replay counter
+			if (pAd->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED)
+				return bSTAKeyFrame;
+
+			// Check MIC value
+			// Save the MIC and replace with zero
+			// use proprietary PTK
+			NdisZeroMemory(temp, 64);
+			NdisMoveMemory(temp, "IEEE802.11 WIRELESS ACCESS POINT", 32);
+			WpaCountPTK(pAd, temp, temp, pAd->CommonCfg.Bssid, temp, pAd->CurrentAddress, DlsPTK, LEN_PTK);
+
+			NdisMoveMemory(OldMic, pEap->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+			NdisZeroMemory(pEap->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+			if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+			{
+				// AES
+				HMAC_SHA1((PUCHAR) pEap, pEap->Body_Len[1] + 4, DlsPTK, LEN_EAP_MICK, digest);
+				NdisMoveMemory(Mic,	digest,	LEN_KEY_DESC_MIC);
+			}
+			else
+			{
+				hmac_md5(DlsPTK, LEN_EAP_MICK, (PUCHAR) pEap, pEap->Body_Len[1] + 4, Mic);
+			}
+
+			if (!NdisEqualMemory(OldMic, Mic, LEN_KEY_DESC_MIC))
+			{
+				DBGPRINT(RT_DEBUG_ERROR, ("MIC Different in Msg1 of STAKey handshake! \n"));
+				return bSTAKeyFrame;
+			}
+			else
+				DBGPRINT(RT_DEBUG_TRACE, ("MIC VALID in Msg1 of STAKey handshake! \n"));
+#if 1
+			if ((pEap->KeyDesc.KeyData[0] == 0xDD) && (pEap->KeyDesc.KeyData[2] == 0x00) && (pEap->KeyDesc.KeyData[3] == 0x0C)
+				&& (pEap->KeyDesc.KeyData[4] == 0x43) && (pEap->KeyDesc.KeyData[5] == 0x02))
+			{
+				pAddr			= pEap->KeyDesc.KeyData + 8;		// Tpe(1), Len(1), OUI(3), DataType(1), Reserved(2)
+				pSTAKey			= pEap->KeyDesc.KeyData + 14;	// Tpe(1), Len(1), OUI(3), DataType(1), Reserved(2), STAKey_Mac_Addr(6)
+
+				DBGPRINT(RT_DEBUG_TRACE,("DLS - Receive STAKey Message-1 from %02x:%02x:%02x:%02x:%02x:%02x Len=%ld, KeyDataLen=%d\n",
+					pAddr[0], pAddr[1], pAddr[2], pAddr[3], pAddr[4], pAddr[5], Len, pEap->KeyDesc.KeyData[1]));
+
+				bSTAKeyFrame = TRUE;
+			}
+#else
+			if ((pEap->KeyDesc.KeyData[0] == 0xDD) && (pEap->KeyDesc.KeyData[2] == 0x00) && (pEap->KeyDesc.KeyData[3] == 0x0F)
+				&& (pEap->KeyDesc.KeyData[4] == 0xAC) && (pEap->KeyDesc.KeyData[5] == 0x02))
+			{
+				pAddr			= pEap->KeyDesc.KeyData + 8;		// Tpe(1), Len(1), OUI(3), DataType(1), Reserved(2)
+				pSTAKey			= pEap->KeyDesc.KeyData + 14;	// Tpe(1), Len(1), OUI(3), DataType(1), Reserved(2), STAKey_Mac_Addr(6)
+
+				DBGPRINT(RT_DEBUG_TRACE,("DLS - Receive STAKey Message-1 from %02x:%02x:%02x:%02x:%02x:%02x Len=%d, KeyDataLen=%d\n",
+					pAddr[0], pAddr[1], pAddr[2], pAddr[3], pAddr[4], pAddr[5], Len, pEap->KeyDesc.KeyData[1]));
+
+				bSTAKeyFrame = TRUE;
+			}
+#endif
+
+		}
+		else if (Len >= (LENGTH_802_11 + 6 + 2 + 2 + sizeof(EAPOL_PACKET) - MAX_LEN_OF_RSNIE))
+		{
+#if 0
+			RTMPMoveMemory(pAd->StaCfg.ReplayCounter, pEap->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+#endif
+			RTMPMoveMemory(pAd->StaCfg.DlsReplayCounter, pEap->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+			DBGPRINT(RT_DEBUG_TRACE,("DLS - Sniff replay counter 2(%02x-%02x-%02x-%02x-%02x-%02x-%02x-%02x) Len=%ld, KeyDataLen=%d\n",
+				pAd->StaCfg.ReplayCounter[0], pAd->StaCfg.ReplayCounter[1], pAd->StaCfg.ReplayCounter[2],
+				pAd->StaCfg.ReplayCounter[3], pAd->StaCfg.ReplayCounter[4],	pAd->StaCfg.ReplayCounter[5],
+				pAd->StaCfg.ReplayCounter[6], pAd->StaCfg.ReplayCounter[7], Len, pEap->KeyDesc.KeyData[1]));
+
+		}
+	}
+
+	// If timeout value is equaled to zero, it means always not be timeout.
+	// update local dls table entry
+	for (i= 0; i < MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+	{
+		if (pAd->StaCfg.DLSEntry[i].Valid && MAC_ADDR_EQUAL(pAddr, pAd->StaCfg.DLSEntry[i].MacAddr))
+		{
+			if (bSTAKeyFrame)
+			{
+				PMAC_TABLE_ENTRY pEntry;
+
+				// STAKey frame, add pairwise key table
+				pAd->StaCfg.DLSEntry[i].Status = DLS_FINISH;
+				RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+
+				PairwiseKey.KeyLen = LEN_TKIP_EK;
+				NdisMoveMemory(PairwiseKey.Key, &pSTAKey[0], LEN_TKIP_EK);
+				NdisMoveMemory(PairwiseKey.TxMic, &pSTAKey[16], LEN_TKIP_RXMICK);
+				NdisMoveMemory(PairwiseKey.RxMic, &pSTAKey[24], LEN_TKIP_TXMICK);
+
+				PairwiseKey.CipherAlg = pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg;
+
+				pEntry = DlsEntryTableLookup(pAd, pAd->StaCfg.DLSEntry[i].MacAddr, TRUE);
+				//AsicAddKeyEntry(pAd, (USHORT)(i + 2), BSS0, 0, &PairwiseKey, TRUE, TRUE);	// reserve 0 for multicast, 1 for unicast
+				//AsicUpdateRxWCIDTable(pAd, (USHORT)(i + 2), pAddr);
+				// Add Pair-wise key to Asic
+#ifdef RT2860
+            	AsicAddPairwiseKeyEntry(pAd,
+										pAd->StaCfg.DLSEntry[i].MacAddr,
+										(UCHAR)pAd->StaCfg.DLSEntry[i].MacTabMatchWCID,
+										&PairwiseKey);
+
+				RTMPAddWcidAttributeEntry(pAd,
+										  BSS0,
+										  0,
+										  PairwiseKey.CipherAlg,
+										  pEntry);
+
+#endif // RT2860 //
+				NdisMoveMemory(&pEntry->PairwiseKey, &PairwiseKey, sizeof(CIPHER_KEY));
+				DBGPRINT(RT_DEBUG_TRACE,("DLS - Receive STAKey Message-1 (Peer STA MAC Address STAKey) \n"));
+
+				RTMPSendSTAKeyHandShake(pAd, pAd->StaCfg.DLSEntry[i].MacAddr);
+
+				DBGPRINT(RT_DEBUG_TRACE,("DLS - Finish STAKey handshake procedure (Initiator side)\n"));
+			}
+			else
+			{
+				// Data frame, update timeout value
+				if (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH)
+				{
+					pAd->StaCfg.DLSEntry[i].CountDownTimer = pAd->StaCfg.DLSEntry[i].TimeOut;
+					//AsicUpdateRxWCIDTable(pAd, (USHORT)(i + 2), pAddr);
+				}
+			}
+
+			bFindEntry = TRUE;
+		}
+	}
+
+	// update peer dls table entry
+	for (i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+	{
+		if (pAd->StaCfg.DLSEntry[i].Valid && MAC_ADDR_EQUAL(pAddr, pAd->StaCfg.DLSEntry[i].MacAddr))
+		{
+			if (bSTAKeyFrame)
+			{
+				PMAC_TABLE_ENTRY pEntry = NULL;
+
+				// STAKey frame, add pairwise key table, and send STAkey Msg-2
+				pAd->StaCfg.DLSEntry[i].Status = DLS_FINISH;
+				RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+
+				PairwiseKey.KeyLen = LEN_TKIP_EK;
+				NdisMoveMemory(PairwiseKey.Key, &pSTAKey[0], LEN_TKIP_EK);
+				NdisMoveMemory(PairwiseKey.TxMic, &pSTAKey[16], LEN_TKIP_RXMICK);
+				NdisMoveMemory(PairwiseKey.RxMic, &pSTAKey[24], LEN_TKIP_TXMICK);
+
+				PairwiseKey.CipherAlg = pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg;
+
+				pEntry = DlsEntryTableLookup(pAd, pAd->StaCfg.DLSEntry[i].MacAddr, TRUE);
+				//AsicAddKeyEntry(pAd, (USHORT)(i + 2), BSS0, 0, &PairwiseKey, TRUE, TRUE);	// reserve 0 for multicast, 1 for unicast
+				//AsicUpdateRxWCIDTable(pAd, (USHORT)(i + 2), pAddr);
+				// Add Pair-wise key to Asic
+#ifdef RT2860
+            	AsicAddPairwiseKeyEntry(pAd,
+										pAd->StaCfg.DLSEntry[i].MacAddr,
+										(UCHAR)pAd->StaCfg.DLSEntry[i].MacTabMatchWCID,
+										&PairwiseKey);
+
+				RTMPAddWcidAttributeEntry(pAd,
+										  BSS0,
+										  0,
+										  PairwiseKey.CipherAlg,
+										  pEntry);
+#endif // RT2860 //
+				NdisMoveMemory(&pEntry->PairwiseKey, &PairwiseKey, sizeof(CIPHER_KEY));
+				DBGPRINT(RT_DEBUG_TRACE,("DLS - Receive STAKey Message-1 (Initiator STA MAC Address STAKey)\n"));
+
+				// If support WPA or WPA2, start STAKey hand shake,
+				// If failed hand shake, just tear down peer DLS
+				if (RTMPSendSTAKeyHandShake(pAd, pAddr) != NDIS_STATUS_SUCCESS)
+				{
+					MLME_DLS_REQ_STRUCT	MlmeDlsReq;
+					USHORT				reason = REASON_QOS_CIPHER_NOT_SUPPORT;
+
+					pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+					pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+					DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+					MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_TEAR_DOWN, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+				}
+				else
+				{
+					DBGPRINT(RT_DEBUG_TRACE,("DLS - Finish STAKey handshake procedure (Peer side)\n"));
+				}
+			}
+			else
+			{
+				// Data frame, update timeout value
+				if (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH)
+				{
+					pAd->StaCfg.DLSEntry[i].CountDownTimer = pAd->StaCfg.DLSEntry[i].TimeOut;
+				}
+			}
+
+			bFindEntry = TRUE;
+		}
+	}
+
+
+	return bSTAKeyFrame;
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Check if the frame can be sent through DLS direct link interface
+
+	Arguments:
+		pAd		Pointer	to adapter
+
+	Return Value:
+		DLS entry index
+
+	Note:
+
+	========================================================================
+*/
+INT	RTMPCheckDLSFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN  PUCHAR          pDA)
+{
+	INT rval = -1;
+	INT	i;
+
+	if (!pAd->CommonCfg.bDLSCapable)
+		return rval;
+
+	if (!INFRA_ON(pAd))
+		return rval;
+
+	do{
+		// check local dls table entry
+		for (i=0; i<MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+		{
+			if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH) &&
+				MAC_ADDR_EQUAL(pDA, pAd->StaCfg.DLSEntry[i].MacAddr))
+			{
+				rval = i;
+				break;
+			}
+		}
+
+		// check peer dls table entry
+		for (i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+		{
+			if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH) &&
+				MAC_ADDR_EQUAL(pDA, pAd->StaCfg.DLSEntry[i].MacAddr))
+			{
+				rval = i;
+				break;
+			}
+		}
+	} while (FALSE);
+
+	return rval;
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID RTMPSendDLSTearDownFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN  PUCHAR          pDA)
+{
+	PUCHAR			pOutBuffer = NULL;
+	NDIS_STATUS		NStatus;
+	HEADER_802_11	DlsTearDownHdr;
+	ULONG			FrameLen = 0;
+	USHORT			Reason = REASON_QOS_QSTA_LEAVING_QBSS;
+	UCHAR			Category = CATEGORY_DLS;
+	UCHAR			Action = ACTION_DLS_TEARDOWN;
+	UCHAR			i = 0;
+
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS) ||
+		RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS))
+		return;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Send DLS TearDown Frame \n"));
+
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if (NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("ASSOC - RTMPSendDLSTearDownFrame() allocate memory failed \n"));
+		return;
+	}
+
+	ActHeaderInit(pAd, &DlsTearDownHdr, pAd->CommonCfg.Bssid, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+	MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+					sizeof(HEADER_802_11),		&DlsTearDownHdr,
+					1,							&Category,
+					1,							&Action,
+					6,							pDA,
+					6,							pAd->CurrentAddress,
+					2,							&Reason,
+					END_OF_ARGS);
+
+	MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	// Remove key in local dls table entry
+	for (i = 0; i < MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+	{
+		if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH)
+			&& MAC_ADDR_EQUAL(pDA, pAd->StaCfg.DLSEntry[i].MacAddr))
+		{
+			MacTableDeleteDlsEntry(pAd, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID, pAd->StaCfg.DLSEntry[i].MacAddr);
+		}
+	}
+
+	// Remove key in peer dls table entry
+	for (i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+	{
+		if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH)
+			&& MAC_ADDR_EQUAL(pDA, pAd->StaCfg.DLSEntry[i].MacAddr))
+		{
+			MacTableDeleteDlsEntry(pAd, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID, pAd->StaCfg.DLSEntry[i].MacAddr);
+		}
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Send DLS TearDown Frame and remove key in (i=%d) \n", i));
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+NDIS_STATUS RTMPSendSTAKeyRequest(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pDA)
+{
+	UCHAR				Header802_3[14];
+	NDIS_STATUS			NStatus;
+	ULONG				FrameLen = 0;
+	EAPOL_PACKET		Packet;
+	UCHAR				Mic[16];
+	UCHAR				digest[80];
+	PUCHAR				pOutBuffer = NULL;
+	PNDIS_PACKET		pNdisPacket;
+	UCHAR				temp[64];
+	UCHAR				DlsPTK[80];
+
+	DBGPRINT(RT_DEBUG_TRACE,("DLS - RTMPSendSTAKeyRequest() to %02x:%02x:%02x:%02x:%02x:%02x\n", pDA[0], pDA[1], pDA[2], pDA[3], pDA[4], pDA[5]));
+
+	pAd->Sequence ++;
+	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
+
+	// Zero message body
+	NdisZeroMemory(&Packet, sizeof(Packet));
+	Packet.ProVer = EAPOL_VER;
+	Packet.ProType    = EAPOLKey;
+	Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE + 6 + MAC_ADDR_LEN;		// data field contain KDE andPeer MAC address
+
+	// STAKey Message is as EAPOL-Key(1,1,0,0,G/0,0,0, MIC, 0,Peer MAC KDE)
+	if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK))
+    {
+        Packet.KeyDesc.Type = WPA1_KEY_DESC;
+    }
+    else if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK))
+    {
+        Packet.KeyDesc.Type = WPA2_KEY_DESC;
+    }
+
+	// Key descriptor version
+	Packet.KeyDesc.KeyInfo.KeyDescVer =
+		(((pAd->StaCfg.PairCipher == Ndis802_11Encryption3Enabled) || (pAd->StaCfg.GroupCipher == Ndis802_11Encryption3Enabled)) ? (DESC_TYPE_AES) : (DESC_TYPE_TKIP));
+
+	Packet.KeyDesc.KeyInfo.KeyMic	= 1;
+	Packet.KeyDesc.KeyInfo.Secure	= 1;
+	Packet.KeyDesc.KeyInfo.Request	= 1;
+
+	Packet.KeyDesc.KeyDataLen[1]	= 12;
+
+	// use our own OUI to distinguish proprietary with standard.
+	Packet.KeyDesc.KeyData[0]		= 0xDD;
+	Packet.KeyDesc.KeyData[1]		= 0x0A;
+	Packet.KeyDesc.KeyData[2]		= 0x00;
+	Packet.KeyDesc.KeyData[3]		= 0x0C;
+	Packet.KeyDesc.KeyData[4]		= 0x43;
+	Packet.KeyDesc.KeyData[5]		= 0x03;
+	NdisMoveMemory(&Packet.KeyDesc.KeyData[6], pDA, MAC_ADDR_LEN);
+
+	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pAd->StaCfg.DlsReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// Allocate buffer for transmitting message
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);
+	if (NStatus	!= NDIS_STATUS_SUCCESS)
+		return NStatus;
+
+	// Prepare EAPOL frame for MIC calculation
+	// Be careful, only EAPOL frame is counted for MIC calculation
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+		              Packet.Body_Len[1] + 4,    &Packet,
+		              END_OF_ARGS);
+
+	// use proprietary PTK
+	NdisZeroMemory(temp, 64);
+	NdisMoveMemory(temp, "IEEE802.11 WIRELESS ACCESS POINT", 32);
+	WpaCountPTK(pAd, temp, temp, pAd->CommonCfg.Bssid, temp, pAd->CurrentAddress, DlsPTK, LEN_PTK);
+
+	// calculate MIC
+	if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+	{
+		// AES
+		NdisZeroMemory(digest,	sizeof(digest));
+		HMAC_SHA1(pOutBuffer, FrameLen, DlsPTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Packet.KeyDesc.KeyMic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{
+		NdisZeroMemory(Mic,	sizeof(Mic));
+		hmac_md5(DlsPTK, LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
+		NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
+	}
+
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+	                  sizeof(Header802_3),	Header802_3,
+		              Packet.Body_Len[1] + 4,	&Packet,
+		              END_OF_ARGS);
+
+	NStatus = RTMPAllocateNdisPacket(pAd, &pNdisPacket, NULL, 0, pOutBuffer, FrameLen);
+	if (NStatus == NDIS_STATUS_SUCCESS)
+	{
+		RTMP_SET_PACKET_WCID(pNdisPacket, BSSID_WCID);
+		STASendPacket(pAd, pNdisPacket);
+		RTMPDeQueuePacket(pAd, FALSE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+	}
+
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPSendSTAKeyRequest- Send STAKey request (NStatus=%x, FrameLen=%ld)\n", NStatus, FrameLen));
+
+	return NStatus;
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+NDIS_STATUS RTMPSendSTAKeyHandShake(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pDA)
+{
+	UCHAR				Header802_3[14];
+	NDIS_STATUS			NStatus;
+	ULONG				FrameLen = 0;
+	EAPOL_PACKET		Packet;
+	UCHAR				Mic[16];
+	UCHAR				digest[80];
+	PUCHAR				pOutBuffer = NULL;
+	PNDIS_PACKET		pNdisPacket;
+	UCHAR				temp[64];
+	UCHAR				DlsPTK[80];			// Due to dirver can not get PTK, use proprietary PTK
+
+	DBGPRINT(RT_DEBUG_TRACE,("DLS - RTMPSendSTAKeyHandShake() to %02x:%02x:%02x:%02x:%02x:%02x\n", pDA[0], pDA[1], pDA[2], pDA[3], pDA[4], pDA[5]));
+
+	pAd->Sequence ++;
+	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
+
+	// Zero message body
+	NdisZeroMemory(&Packet, sizeof(Packet));
+	Packet.ProVer = EAPOL_VER;
+	Packet.ProType    = EAPOLKey;
+	Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE + 6 + MAC_ADDR_LEN;		// data field contain KDE and Peer MAC address
+
+	// STAKey Message is as EAPOL-Key(1,1,0,0,G/0,0,0, MIC, 0,Peer MAC KDE)
+	if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK))
+    {
+        Packet.KeyDesc.Type = WPA1_KEY_DESC;
+    }
+    else if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK))
+    {
+        Packet.KeyDesc.Type = WPA2_KEY_DESC;
+    }
+
+	// Key descriptor version
+	Packet.KeyDesc.KeyInfo.KeyDescVer =
+		(((pAd->StaCfg.PairCipher == Ndis802_11Encryption3Enabled) || (pAd->StaCfg.GroupCipher == Ndis802_11Encryption3Enabled)) ? (DESC_TYPE_AES) : (DESC_TYPE_TKIP));
+
+	Packet.KeyDesc.KeyInfo.KeyMic	= 1;
+	Packet.KeyDesc.KeyInfo.Secure	= 1;
+
+	Packet.KeyDesc.KeyDataLen[1]	= 12;
+
+	// use our own OUI to distinguish proprietary with standard.
+	Packet.KeyDesc.KeyData[0]		= 0xDD;
+	Packet.KeyDesc.KeyData[1]		= 0x0A;
+	Packet.KeyDesc.KeyData[2]		= 0x00;
+	Packet.KeyDesc.KeyData[3]		= 0x0C;
+	Packet.KeyDesc.KeyData[4]		= 0x43;
+	Packet.KeyDesc.KeyData[5]		= 0x03;
+	NdisMoveMemory(&Packet.KeyDesc.KeyData[6], pDA, MAC_ADDR_LEN);
+
+	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pAd->StaCfg.DlsReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// Allocate buffer for transmitting message
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);
+	if (NStatus	!= NDIS_STATUS_SUCCESS)
+		return NStatus;
+
+	// Prepare EAPOL frame for MIC calculation
+	// Be careful, only EAPOL frame is counted for MIC calculation
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+		              Packet.Body_Len[1] + 4,    &Packet,
+		              END_OF_ARGS);
+
+	// use proprietary PTK
+	NdisZeroMemory(temp, 64);
+	NdisMoveMemory(temp, "IEEE802.11 WIRELESS ACCESS POINT", 32);
+	WpaCountPTK(pAd, temp, temp, pAd->CommonCfg.Bssid, temp, pAd->CurrentAddress, DlsPTK, LEN_PTK);
+
+	// calculate MIC
+	if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+	{
+		// AES
+		NdisZeroMemory(digest,	sizeof(digest));
+		HMAC_SHA1(pOutBuffer, FrameLen, DlsPTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Packet.KeyDesc.KeyMic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{
+		NdisZeroMemory(Mic,	sizeof(Mic));
+		hmac_md5(DlsPTK, LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
+		NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
+	}
+
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+	                  sizeof(Header802_3),	Header802_3,
+		              Packet.Body_Len[1] + 4,	&Packet,
+		              END_OF_ARGS);
+
+	NStatus = RTMPAllocateNdisPacket(pAd, &pNdisPacket, NULL, 0, pOutBuffer, FrameLen);
+	if (NStatus == NDIS_STATUS_SUCCESS)
+	{
+		RTMP_SET_PACKET_WCID(pNdisPacket, BSSID_WCID);
+		STASendPacket(pAd, pNdisPacket);
+		RTMPDeQueuePacket(pAd, FALSE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+	}
+
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPSendSTAKeyHandShake- Send STAKey Message-2 (NStatus=%x, FrameLen=%ld)\n", NStatus, FrameLen));
+
+	return NStatus;
+}
+
+VOID DlsTimeoutAction(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+	MLME_DLS_REQ_STRUCT		MlmeDlsReq;
+	USHORT					reason;
+	PRT_802_11_DLS			pDLS = (PRT_802_11_DLS)FunctionContext;
+	PRTMP_ADAPTER			pAd = pDLS->pAd;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("DlsTimeout - Tear down DLS links (%02x:%02x:%02x:%02x:%02x:%02x)\n",
+		pDLS->MacAddr[0], pDLS->MacAddr[1], pDLS->MacAddr[2], pDLS->MacAddr[3], pDLS->MacAddr[4], pDLS->MacAddr[5]));
+
+	if ((pDLS) && (pDLS->Valid))
+	{
+		reason			= REASON_QOS_REQUEST_TIMEOUT;
+		pDLS->Valid		= FALSE;
+		pDLS->Status	= DLS_NONE;
+		DlsParmFill(pAd, &MlmeDlsReq, pDLS, reason);
+		MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_TEAR_DOWN, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+		RT28XX_MLME_HANDLER(pAd);
+	}
+}
+
+/*
+================================================================
+Description : because DLS and CLI share the same WCID table in ASIC.
+Mesh entry also insert to pAd->MacTab.content[].  Such is marked as ValidAsDls = TRUE.
+Also fills the pairwise key.
+Because front MAX_AID_BA entries have direct mapping to BAEntry, which is only used as CLI. So we insert Dls
+from index MAX_AID_BA.
+================================================================
+*/
+MAC_TABLE_ENTRY *MacTableInsertDlsEntry(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR	pAddr,
+	IN  UINT	DlsEntryIdx)
+{
+	PMAC_TABLE_ENTRY pEntry = NULL;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("====> MacTableInsertDlsEntry\n"));
+	// if FULL, return
+	if (pAd->MacTab.Size >= MAX_LEN_OF_MAC_TABLE)
+		return NULL;
+
+	do
+	{
+		if((pEntry = DlsEntryTableLookup(pAd, pAddr, TRUE)) != NULL)
+			break;
+
+		// allocate one MAC entry
+		pEntry = MacTableInsertEntry(pAd, pAddr, DlsEntryIdx + MIN_NET_DEVICE_FOR_DLS, TRUE);
+		if (pEntry)
+		{
+			pAd->StaCfg.DLSEntry[DlsEntryIdx].MacTabMatchWCID = pEntry->Aid;
+			pEntry->MatchDlsEntryIdx = DlsEntryIdx;
+			pEntry->AuthMode = pAd->StaCfg.AuthMode;
+			pEntry->WepStatus = pAd->StaCfg.WepStatus;
+			pEntry->PortSecured = WPA_802_1X_PORT_SECURED;
+
+			DBGPRINT(RT_DEBUG_TRACE, ("MacTableInsertDlsEntry - allocate entry #%d, Total= %d\n",pEntry->Aid, pAd->MacTab.Size));
+
+			// If legacy WEP is used, set pair-wise cipherAlg into WCID attribute table for this entry
+			if ((pEntry->ValidAsDls) && (pEntry->WepStatus == Ndis802_11WEPEnabled))
+			{
+				UCHAR KeyIdx = 0;
+				UCHAR CipherAlg = 0;
+
+				KeyIdx	= pAd->StaCfg.DefaultKeyId;
+
+				CipherAlg 	= pAd->SharedKey[BSS0][KeyIdx].CipherAlg;
+
+				RTMPAddWcidAttributeEntry(pAd,
+											BSS0,
+											pAd->StaCfg.DefaultKeyId,
+											pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
+											pEntry);
+			}
+
+			break;
+		}
+	} while(FALSE);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<==== MacTableInsertDlsEntry\n"));
+
+	return pEntry;
+}
+
+
+/*
+	==========================================================================
+	Description:
+		Delete all Mesh Entry in pAd->MacTab
+	==========================================================================
+ */
+BOOLEAN MacTableDeleteDlsEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT wcid,
+	IN PUCHAR pAddr)
+{
+	DBGPRINT(RT_DEBUG_TRACE, ("====> MacTableDeleteDlsEntry\n"));
+
+	if (!VALID_WCID(wcid))
+		return FALSE;
+
+	MacTableDeleteEntry(pAd, wcid, pAddr);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<==== MacTableDeleteDlsEntry\n"));
+
+	return TRUE;
+}
+
+MAC_TABLE_ENTRY *DlsEntryTableLookup(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR	pAddr,
+	IN BOOLEAN	bResetIdelCount)
+{
+	ULONG HashIdx;
+	MAC_TABLE_ENTRY *pEntry = NULL;
+
+	RTMP_SEM_LOCK(&pAd->MacTabLock);
+	HashIdx = MAC_ADDR_HASH_INDEX(pAddr);
+	pEntry = pAd->MacTab.Hash[HashIdx];
+
+	while (pEntry)
+	{
+		if ((pEntry->ValidAsDls == TRUE)
+			&& MAC_ADDR_EQUAL(pEntry->Addr, pAddr))
+		{
+			if(bResetIdelCount)
+				pEntry->NoDataIdleCount = 0;
+			break;
+		}
+		else
+			pEntry = pEntry->pNext;
+	}
+
+	RTMP_SEM_UNLOCK(&pAd->MacTabLock);
+	return pEntry;
+}
+
+MAC_TABLE_ENTRY *DlsEntryTableLookupByWcid(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR	wcid,
+	IN PUCHAR	pAddr,
+	IN BOOLEAN	bResetIdelCount)
+{
+	ULONG DLsIndex;
+	PMAC_TABLE_ENTRY pCurEntry = NULL;
+	PMAC_TABLE_ENTRY pEntry = NULL;
+
+	if (!VALID_WCID(wcid))
+		return NULL;
+
+	RTMP_SEM_LOCK(&pAd->MacTabLock);
+
+	do
+	{
+		pCurEntry = &pAd->MacTab.Content[wcid];
+
+		DLsIndex = 0xff;
+		if ((pCurEntry) && (pCurEntry->ValidAsDls== TRUE))
+		{
+			DLsIndex = pCurEntry->MatchDlsEntryIdx;
+		}
+
+		if (DLsIndex == 0xff)
+			break;
+
+		if (MAC_ADDR_EQUAL(pCurEntry->Addr, pAddr))
+		{
+			if(bResetIdelCount)
+				pCurEntry->NoDataIdleCount = 0;
+			pEntry = pCurEntry;
+			break;
+		}
+	} while(FALSE);
+
+	RTMP_SEM_UNLOCK(&pAd->MacTabLock);
+
+	return pEntry;
+}
+
+INT Set_DlsEntryInfo_Display_Proc(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR arg)
+{
+	INT i;
+
+	printk("\n%-19s%-8s\n", "MAC", "TIMEOUT\n");
+	for (i=0; i<MAX_NUM_OF_DLS_ENTRY; i++)
+	{
+		if ((pAd->StaCfg.DLSEntry[i].Valid) && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+		{
+			PMAC_TABLE_ENTRY pEntry = &pAd->MacTab.Content[pAd->StaCfg.DLSEntry[i].MacTabMatchWCID];
+
+			printk("%02x:%02x:%02x:%02x:%02x:%02x  ",
+				pAd->StaCfg.DLSEntry[i].MacAddr[0], pAd->StaCfg.DLSEntry[i].MacAddr[1], pAd->StaCfg.DLSEntry[i].MacAddr[2],
+				pAd->StaCfg.DLSEntry[i].MacAddr[3], pAd->StaCfg.DLSEntry[i].MacAddr[4], pAd->StaCfg.DLSEntry[i].MacAddr[5]);
+			printk("%-8d\n", pAd->StaCfg.DLSEntry[i].TimeOut);
+
+			printk("\n");
+			printk("\n%-19s%-4s%-4s%-4s%-4s%-8s%-7s%-7s%-7s%-10s%-6s%-6s%-6s%-6s\n",
+				   "MAC", "AID", "BSS", "PSM", "WMM", "MIMOPS", "RSSI0", "RSSI1", "RSSI2", "PhMd", "BW", "MCS", "SGI", "STBC");
+			printk("%02X:%02X:%02X:%02X:%02X:%02X  ",
+				pEntry->Addr[0], pEntry->Addr[1], pEntry->Addr[2],
+				pEntry->Addr[3], pEntry->Addr[4], pEntry->Addr[5]);
+			printk("%-4d", (int)pEntry->Aid);
+			printk("%-4d", (int)pEntry->apidx);
+			printk("%-4d", (int)pEntry->PsMode);
+			printk("%-4d", (int)CLIENT_STATUS_TEST_FLAG(pEntry, fCLIENT_STATUS_WMM_CAPABLE));
+			printk("%-8d", (int)pEntry->MmpsMode);
+			printk("%-7d", pEntry->RssiSample.AvgRssi0);
+			printk("%-7d", pEntry->RssiSample.AvgRssi1);
+			printk("%-7d", pEntry->RssiSample.AvgRssi2);
+			printk("%-10s", GetPhyMode(pEntry->HTPhyMode.field.MODE));
+			printk("%-6s", GetBW(pEntry->HTPhyMode.field.BW));
+			printk("%-6d", pEntry->HTPhyMode.field.MCS);
+			printk("%-6d", pEntry->HTPhyMode.field.ShortGI);
+			printk("%-6d", pEntry->HTPhyMode.field.STBC);
+			printk("%-10d, %d, %d%%\n", pEntry->DebugFIFOCount, pEntry->DebugTxCount,
+						(pEntry->DebugTxCount) ? ((pEntry->DebugTxCount-pEntry->DebugFIFOCount)*100/pEntry->DebugTxCount) : 0);
+			printk("\n");
+
+		}
+	}
+
+	return TRUE;
+}
+
+INT	Set_DlsAddEntry_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+    UCHAR	mac[MAC_ADDR_LEN];
+	USHORT	Timeout;
+	char *token, sepValue[] = ":", DASH = '-';
+	INT i;
+    RT_802_11_DLS	Dls;
+
+    if(strlen(arg) < 19)  //Mac address acceptable format 01:02:03:04:05:06 length 17 plus the "-" and timeout value in decimal format.
+		return FALSE;
+
+	token = strchr(arg, DASH);
+	if ((token != NULL) && (strlen(token)>1))
+	{
+		Timeout = simple_strtol((token+1), 0, 10);
+
+		*token = '\0';
+		for (i = 0, token = rstrtok(arg, &sepValue[0]); token; token = rstrtok(NULL, &sepValue[0]), i++)
+		{
+			if((strlen(token) != 2) || (!isxdigit(*token)) || (!isxdigit(*(token+1))))
+				return FALSE;
+			AtoH(token, (PUCHAR)(&mac[i]), 1);
+		}
+		if(i != 6)
+			return FALSE;
+
+	    printk("\n%02x:%02x:%02x:%02x:%02x:%02x-%d", mac[0], mac[1],
+	           mac[2], mac[3], mac[4], mac[5], (int)Timeout);
+
+		NdisZeroMemory(&Dls, sizeof(RT_802_11_DLS));
+		Dls.TimeOut = Timeout;
+		COPY_MAC_ADDR(Dls.MacAddr, mac);
+		Dls.Valid = 1;
+
+		MlmeEnqueue(pAd,
+					MLME_CNTL_STATE_MACHINE,
+					RT_OID_802_11_SET_DLS_PARAM,
+					sizeof(RT_802_11_DLS),
+					&Dls);
+
+		return TRUE;
+	}
+
+	return FALSE;
+
+}
+
+INT	Set_DlsTearDownEntry_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UCHAR			macAddr[MAC_ADDR_LEN];
+	CHAR			*value;
+	INT				i;
+	RT_802_11_DLS	Dls;
+
+	if(strlen(arg) != 17)  //Mac address acceptable format 01:02:03:04:05:06 length 17
+		return FALSE;
+
+	for (i=0, value = rstrtok(arg,":"); value; value = rstrtok(NULL,":"))
+	{
+		if((strlen(value) != 2) || (!isxdigit(*value)) || (!isxdigit(*(value+1))) )
+			return FALSE;  //Invalid
+
+		AtoH(value, &macAddr[i++], 2);
+	}
+
+	printk("\n%02x:%02x:%02x:%02x:%02x:%02x", macAddr[0], macAddr[1],
+	           macAddr[2], macAddr[3], macAddr[4], macAddr[5]);
+
+	NdisZeroMemory(&Dls, sizeof(RT_802_11_DLS));
+	COPY_MAC_ADDR(Dls.MacAddr, macAddr);
+	Dls.Valid = 0;
+
+	MlmeEnqueue(pAd,
+				MLME_CNTL_STATE_MACHINE,
+				RT_OID_802_11_SET_DLS_PARAM,
+				sizeof(RT_802_11_DLS),
+				&Dls);
+
+	return TRUE;
+}
+
diff --git a/drivers/staging/rt2860/sta/rtmp_data.c b/drivers/staging/rt2860/sta/rtmp_data.c
new file mode 100644
index 00000000000..36aff247cd9
--- /dev/null
+++ b/drivers/staging/rt2860/sta/rtmp_data.c
@@ -0,0 +1,2614 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rtmp_data.c
+
+	Abstract:
+	Data path subroutines
+
+	Revision History:
+	Who 		When			What
+	--------	----------		----------------------------------------------
+	John		      Aug/17/04		major modification for RT2561/2661
+	Jan Lee	      Mar/17/06		major modification for RT2860 New Ring Design
+*/
+#include "../rt_config.h"
+
+
+
+VOID STARxEAPOLFrameIndicate(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+	PRT28XX_RXD_STRUC	pRxD = &(pRxBlk->RxD);
+	PRXWI_STRUC		pRxWI = pRxBlk->pRxWI;
+	UCHAR			*pTmpBuf;
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+	if (pAd->StaCfg.WpaSupplicantUP)
+	{
+		// All EAPoL frames have to pass to upper layer (ex. WPA_SUPPLICANT daemon)
+		// TBD : process fragmented EAPol frames
+		{
+			// In 802.1x mode, if the received frame is EAP-SUCCESS packet, turn on the PortSecured variable
+			if ( pAd->StaCfg.IEEE8021X == TRUE &&
+				 (EAP_CODE_SUCCESS == WpaCheckEapCode(pAd, pRxBlk->pData, pRxBlk->DataSize, LENGTH_802_1_H)))
+			{
+				PUCHAR	Key;
+				UCHAR 	CipherAlg;
+				int     idx = 0;
+
+				DBGPRINT_RAW(RT_DEBUG_TRACE, ("Receive EAP-SUCCESS Packet\n"));
+				STA_PORT_SECURED(pAd);
+
+                if (pAd->StaCfg.IEEE8021x_required_keys == FALSE)
+                {
+                    idx = pAd->StaCfg.DesireSharedKeyId;
+                    CipherAlg = pAd->StaCfg.DesireSharedKey[idx].CipherAlg;
+					Key = pAd->StaCfg.DesireSharedKey[idx].Key;
+
+                    if (pAd->StaCfg.DesireSharedKey[idx].KeyLen > 0)
+    				{
+#ifdef RT2860
+						MAC_TABLE_ENTRY *pEntry = &pAd->MacTab.Content[BSSID_WCID];
+
+						// Set key material and cipherAlg to Asic
+						AsicAddSharedKeyEntry(pAd, BSS0, idx, CipherAlg, Key, NULL, NULL);
+
+						// Assign group key info
+						RTMPAddWcidAttributeEntry(pAd, BSS0, idx, CipherAlg, NULL);
+
+						// Assign pairwise key info
+						RTMPAddWcidAttributeEntry(pAd, BSS0, idx, CipherAlg, pEntry);
+
+                        pAd->IndicateMediaState = NdisMediaStateConnected;
+                        pAd->ExtraInfo = GENERAL_LINK_UP;
+#endif // RT2860 //
+						// For Preventing ShardKey Table is cleared by remove key procedure.
+    					pAd->SharedKey[BSS0][idx].CipherAlg = CipherAlg;
+						pAd->SharedKey[BSS0][idx].KeyLen = pAd->StaCfg.DesireSharedKey[idx].KeyLen;
+						NdisMoveMemory(pAd->SharedKey[BSS0][idx].Key,
+									   pAd->StaCfg.DesireSharedKey[idx].Key,
+									   pAd->StaCfg.DesireSharedKey[idx].KeyLen);
+    				}
+				}
+			}
+
+			Indicate_Legacy_Packet(pAd, pRxBlk, FromWhichBSSID);
+			return;
+		}
+	}
+	else
+#endif // WPA_SUPPLICANT_SUPPORT //
+	{
+		// Special DATA frame that has to pass to MLME
+		//	 1. Cisco Aironet frames for CCX2. We need pass it to MLME for special process
+		//	 2. EAPOL handshaking frames when driver supplicant enabled, pass to MLME for special process
+		{
+			pTmpBuf = pRxBlk->pData - LENGTH_802_11;
+			NdisMoveMemory(pTmpBuf, pRxBlk->pHeader, LENGTH_802_11);
+			REPORT_MGMT_FRAME_TO_MLME(pAd, pRxWI->WirelessCliID, pTmpBuf, pRxBlk->DataSize + LENGTH_802_11, pRxWI->RSSI0, pRxWI->RSSI1, pRxWI->RSSI2, pRxD->PlcpSignal);
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("!!! report EAPOL/AIRONET DATA to MLME (len=%d) !!!\n", pRxBlk->DataSize));
+		}
+	}
+
+	RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+	return;
+
+}
+
+VOID STARxDataFrameAnnounce(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+
+	// non-EAP frame
+	if (!RTMPCheckWPAframe(pAd, pEntry, pRxBlk->pData, pRxBlk->DataSize, FromWhichBSSID))
+	{
+		{
+			// drop all non-EAP DATA frame before
+			// this client's Port-Access-Control is secured
+			if (pRxBlk->pHeader->FC.Wep)
+			{
+				// unsupported cipher suite
+				if (pAd->StaCfg.WepStatus == Ndis802_11EncryptionDisabled)
+				{
+					// release packet
+					RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+					return;
+				}
+			}
+			else
+			{
+				// encryption in-use but receive a non-EAPOL clear text frame, drop it
+				if ((pAd->StaCfg.WepStatus != Ndis802_11EncryptionDisabled) &&
+					(pAd->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
+				{
+					// release packet
+					RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+					return;
+				}
+			}
+		}
+		RX_BLK_CLEAR_FLAG(pRxBlk, fRX_EAP);
+		if (!RX_BLK_TEST_FLAG(pRxBlk, fRX_ARALINK))
+		{
+			// Normal legacy, AMPDU or AMSDU
+			CmmRxnonRalinkFrameIndicate(pAd, pRxBlk, FromWhichBSSID);
+
+		}
+		else
+		{
+			// ARALINK
+			CmmRxRalinkFrameIndicate(pAd, pEntry, pRxBlk, FromWhichBSSID);
+		}
+#ifdef QOS_DLS_SUPPORT
+		RX_BLK_CLEAR_FLAG(pRxBlk, fRX_DLS);
+#endif // QOS_DLS_SUPPORT //
+	}
+	else
+	{
+		RX_BLK_SET_FLAG(pRxBlk, fRX_EAP);
+#ifdef DOT11_N_SUPPORT
+		if (RX_BLK_TEST_FLAG(pRxBlk, fRX_AMPDU) && (pAd->CommonCfg.bDisableReordering == 0))
+		{
+			Indicate_AMPDU_Packet(pAd, pRxBlk, FromWhichBSSID);
+		}
+		else
+#endif // DOT11_N_SUPPORT //
+		{
+			// Determin the destination of the EAP frame
+			//  to WPA state machine or upper layer
+			STARxEAPOLFrameIndicate(pAd, pEntry, pRxBlk, FromWhichBSSID);
+		}
+	}
+}
+
+
+// For TKIP frame, calculate the MIC value
+BOOLEAN STACheckTkipMICValue(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry,
+	IN	RX_BLK			*pRxBlk)
+{
+	PHEADER_802_11	pHeader = pRxBlk->pHeader;
+	UCHAR			*pData = pRxBlk->pData;
+	USHORT			DataSize = pRxBlk->DataSize;
+	UCHAR			UserPriority = pRxBlk->UserPriority;
+	PCIPHER_KEY		pWpaKey;
+	UCHAR			*pDA, *pSA;
+
+	pWpaKey = &pAd->SharedKey[BSS0][pRxBlk->pRxWI->KeyIndex];
+
+	pDA = pHeader->Addr1;
+	if (RX_BLK_TEST_FLAG(pRxBlk, fRX_INFRA))
+	{
+		pSA = pHeader->Addr3;
+	}
+	else
+	{
+		pSA = pHeader->Addr2;
+	}
+
+	if (RTMPTkipCompareMICValue(pAd,
+								pData,
+								pDA,
+								pSA,
+								pWpaKey->RxMic,
+								UserPriority,
+								DataSize) == FALSE)
+	{
+		DBGPRINT_RAW(RT_DEBUG_ERROR,("Rx MIC Value error 2\n"));
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+		if (pAd->StaCfg.WpaSupplicantUP)
+		{
+			WpaSendMicFailureToWpaSupplicant(pAd, (pWpaKey->Type == PAIRWISEKEY) ? TRUE : FALSE);
+		}
+		else
+#endif // WPA_SUPPLICANT_SUPPORT //
+		{
+			RTMPReportMicError(pAd, pWpaKey);
+		}
+
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+		return FALSE;
+	}
+
+	return TRUE;
+}
+
+
+//
+// All Rx routines use RX_BLK structure to hande rx events
+// It is very important to build pRxBlk attributes
+//  1. pHeader pointer to 802.11 Header
+//  2. pData pointer to payload including LLC (just skip Header)
+//  3. set payload size including LLC to DataSize
+//  4. set some flags with RX_BLK_SET_FLAG()
+//
+VOID STAHandleRxDataFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk)
+{
+	PRT28XX_RXD_STRUC				pRxD = &(pRxBlk->RxD);
+	PRXWI_STRUC						pRxWI = pRxBlk->pRxWI;
+	PHEADER_802_11					pHeader = pRxBlk->pHeader;
+	PNDIS_PACKET					pRxPacket = pRxBlk->pRxPacket;
+	BOOLEAN 						bFragment = FALSE;
+	MAC_TABLE_ENTRY	    			*pEntry = NULL;
+	UCHAR							FromWhichBSSID = BSS0;
+	UCHAR                           UserPriority = 0;
+
+	{
+		// before LINK UP, all DATA frames are rejected
+		if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+		{
+			// release packet
+			RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+			return;
+		}
+
+#ifdef QOS_DLS_SUPPORT
+		//if ((pHeader->FC.FrDs == 0) && (pHeader->FC.ToDs == 0))
+		if (RTMPRcvFrameDLSCheck(pAd, pHeader, pRxWI->MPDUtotalByteCount, pRxD))
+		{
+			return;
+		}
+#endif // QOS_DLS_SUPPORT //
+
+		// Drop not my BSS frames
+		if (pRxD->MyBss == 0)
+		{
+			{
+				// release packet
+				RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+				return;
+			}
+		}
+
+		pAd->RalinkCounters.RxCountSinceLastNULL++;
+		if (pAd->CommonCfg.bAPSDCapable && pAd->CommonCfg.APEdcaParm.bAPSDCapable && (pHeader->FC.SubType & 0x08))
+		{
+			UCHAR *pData;
+			DBGPRINT(RT_DEBUG_TRACE,("bAPSDCapable\n"));
+
+			// Qos bit 4
+			pData = (PUCHAR)pHeader + LENGTH_802_11;
+			if ((*pData >> 4) & 0x01)
+			{
+				DBGPRINT(RT_DEBUG_TRACE,("RxDone- Rcv EOSP frame, driver may fall into sleep\n"));
+				pAd->CommonCfg.bInServicePeriod = FALSE;
+
+				// Force driver to fall into sleep mode when rcv EOSP frame
+				if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+				{
+					USHORT  TbttNumToNextWakeUp;
+					USHORT  NextDtim = pAd->StaCfg.DtimPeriod;
+					ULONG   Now;
+
+					NdisGetSystemUpTime(&Now);
+					NextDtim -= (USHORT)(Now - pAd->StaCfg.LastBeaconRxTime)/pAd->CommonCfg.BeaconPeriod;
+
+					TbttNumToNextWakeUp = pAd->StaCfg.DefaultListenCount;
+					if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_RECEIVE_DTIM) && (TbttNumToNextWakeUp > NextDtim))
+						TbttNumToNextWakeUp = NextDtim;
+
+					MlmeSetPsmBit(pAd, PWR_SAVE);
+					// if WMM-APSD is failed, try to disable following line
+					AsicSleepThenAutoWakeup(pAd, TbttNumToNextWakeUp);
+				}
+			}
+
+			if ((pHeader->FC.MoreData) && (pAd->CommonCfg.bInServicePeriod))
+			{
+				DBGPRINT(RT_DEBUG_TRACE,("Sending another trigger frame when More Data bit is set to 1\n"));
+			}
+		}
+
+		// Drop NULL, CF-ACK(no data), CF-POLL(no data), and CF-ACK+CF-POLL(no data) data frame
+		if ((pHeader->FC.SubType & 0x04)) // bit 2 : no DATA
+		{
+			// release packet
+			RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+			return;
+		}
+
+	    // Drop not my BSS frame (we can not only check the MyBss bit in RxD)
+#ifdef QOS_DLS_SUPPORT
+	    if (!pAd->CommonCfg.bDLSCapable)
+	    {
+#endif // QOS_DLS_SUPPORT //
+		if (INFRA_ON(pAd))
+		{
+			// Infrastructure mode, check address 2 for BSSID
+			if (!RTMPEqualMemory(&pHeader->Addr2, &pAd->CommonCfg.Bssid, 6))
+			{
+				// Receive frame not my BSSID
+	            // release packet
+	            RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+				return;
+			}
+		}
+		else	// Ad-Hoc mode or Not associated
+		{
+			// Ad-Hoc mode, check address 3 for BSSID
+			if (!RTMPEqualMemory(&pHeader->Addr3, &pAd->CommonCfg.Bssid, 6))
+			{
+				// Receive frame not my BSSID
+	            // release packet
+	            RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+				return;
+			}
+		}
+#ifdef QOS_DLS_SUPPORT
+	    }
+#endif // QOS_DLS_SUPPORT //
+
+		//
+		// find pEntry
+		//
+		if (pRxWI->WirelessCliID < MAX_LEN_OF_MAC_TABLE)
+		{
+			pEntry = &pAd->MacTab.Content[pRxWI->WirelessCliID];
+		}
+		else
+		{
+			// 1. release packet if infra mode
+			// 2. new a pEntry if ad-hoc mode
+			RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+			return;
+		}
+
+		// infra or ad-hoc
+		if (INFRA_ON(pAd))
+		{
+			RX_BLK_SET_FLAG(pRxBlk, fRX_INFRA);
+#ifdef QOS_DLS_SUPPORT
+			if ((pHeader->FC.FrDs == 0) && (pHeader->FC.ToDs == 0))
+				RX_BLK_SET_FLAG(pRxBlk, fRX_DLS);
+			else
+#endif // QOS_DLS_SUPPORT //
+			ASSERT(pRxWI->WirelessCliID == BSSID_WCID);
+		}
+
+		// check Atheros Client
+		if ((pEntry->bIAmBadAtheros == FALSE) &&  (pRxD->AMPDU == 1) && (pHeader->FC.Retry ))
+		{
+			pEntry->bIAmBadAtheros = TRUE;
+			pAd->CommonCfg.IOTestParm.bCurrentAtheros = TRUE;
+			pAd->CommonCfg.IOTestParm.bLastAtheros = TRUE;
+			if (!STA_AES_ON(pAd))
+			{
+				AsicUpdateProtect(pAd, 8, ALLN_SETPROTECT, TRUE, FALSE);
+			}
+		}
+	}
+
+	pRxBlk->pData = (UCHAR *)pHeader;
+
+	//
+	// update RxBlk->pData, DataSize
+	// 802.11 Header, QOS, HTC, Hw Padding
+	//
+
+	// 1. skip 802.11 HEADER
+	{
+		pRxBlk->pData += LENGTH_802_11;
+		pRxBlk->DataSize -= LENGTH_802_11;
+	}
+
+	// 2. QOS
+	if (pHeader->FC.SubType & 0x08)
+	{
+		RX_BLK_SET_FLAG(pRxBlk, fRX_QOS);
+		UserPriority = *(pRxBlk->pData) & 0x0f;
+		// bit 7 in QoS Control field signals the HT A-MSDU format
+		if ((*pRxBlk->pData) & 0x80)
+		{
+			RX_BLK_SET_FLAG(pRxBlk, fRX_AMSDU);
+		}
+
+		// skip QOS contorl field
+		pRxBlk->pData += 2;
+		pRxBlk->DataSize -=2;
+	}
+	pRxBlk->UserPriority = UserPriority;
+
+	// 3. Order bit: A-Ralink or HTC+
+	if (pHeader->FC.Order)
+	{
+#ifdef AGGREGATION_SUPPORT
+		if ((pRxWI->PHYMODE <= MODE_OFDM) && (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED)))
+		{
+			RX_BLK_SET_FLAG(pRxBlk, fRX_ARALINK);
+		}
+		else
+#endif
+		{
+#ifdef DOT11_N_SUPPORT
+			RX_BLK_SET_FLAG(pRxBlk, fRX_HTC);
+			// skip HTC contorl field
+			pRxBlk->pData += 4;
+			pRxBlk->DataSize -= 4;
+#endif // DOT11_N_SUPPORT //
+		}
+	}
+
+	// 4. skip HW padding
+	if (pRxD->L2PAD)
+	{
+		// just move pData pointer
+		// because DataSize excluding HW padding
+		RX_BLK_SET_FLAG(pRxBlk, fRX_PAD);
+		pRxBlk->pData += 2;
+	}
+
+#ifdef DOT11_N_SUPPORT
+	if (pRxD->BA)
+	{
+		RX_BLK_SET_FLAG(pRxBlk, fRX_AMPDU);
+	}
+#endif // DOT11_N_SUPPORT //
+
+
+	//
+	// Case I  Process Broadcast & Multicast data frame
+	//
+	if (pRxD->Bcast || pRxD->Mcast)
+	{
+		INC_COUNTER64(pAd->WlanCounters.MulticastReceivedFrameCount);
+
+		// Drop Mcast/Bcast frame with fragment bit on
+		if (pHeader->FC.MoreFrag)
+		{
+			// release packet
+			RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+			return;
+		}
+
+		// Filter out Bcast frame which AP relayed for us
+		if (pHeader->FC.FrDs && MAC_ADDR_EQUAL(pHeader->Addr3, pAd->CurrentAddress))
+		{
+			// release packet
+			RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+			return;
+		}
+
+		Indicate_Legacy_Packet(pAd, pRxBlk, FromWhichBSSID);
+		return;
+	}
+	else if (pRxD->U2M)
+	{
+		pAd->LastRxRate = (USHORT)((pRxWI->MCS) + (pRxWI->BW <<7) + (pRxWI->ShortGI <<8)+ (pRxWI->PHYMODE <<14)) ;
+
+
+#ifdef QOS_DLS_SUPPORT
+        if (RX_BLK_TEST_FLAG(pRxBlk, fRX_DLS))
+		{
+			MAC_TABLE_ENTRY *pDlsEntry = NULL;
+
+			pDlsEntry = DlsEntryTableLookupByWcid(pAd, pRxWI->WirelessCliID, pHeader->Addr2, TRUE);
+										                        if(pDlsEntry)
+			Update_Rssi_Sample(pAd, &pDlsEntry->RssiSample, pRxWI);
+		}
+		else
+#endif // QOS_DLS_SUPPORT //
+		if (ADHOC_ON(pAd))
+		{
+			pEntry = MacTableLookup(pAd, pHeader->Addr2);
+			if (pEntry)
+				Update_Rssi_Sample(pAd, &pEntry->RssiSample, pRxWI);
+		}
+
+
+		Update_Rssi_Sample(pAd, &pAd->StaCfg.RssiSample, pRxWI);
+
+		pAd->StaCfg.LastSNR0 = (UCHAR)(pRxWI->SNR0);
+		pAd->StaCfg.LastSNR1 = (UCHAR)(pRxWI->SNR1);
+
+		pAd->RalinkCounters.OneSecRxOkDataCnt++;
+
+
+    	if (!((pHeader->Frag == 0) && (pHeader->FC.MoreFrag == 0)))
+    	{
+    		// re-assemble the fragmented packets
+    		// return complete frame (pRxPacket) or NULL
+    		bFragment = TRUE;
+    		pRxPacket = RTMPDeFragmentDataFrame(pAd, pRxBlk);
+    	}
+
+    	if (pRxPacket)
+    	{
+			pEntry = &pAd->MacTab.Content[pRxWI->WirelessCliID];
+
+    		// process complete frame
+    		if (bFragment && (pRxD->Decrypted) && (pEntry->WepStatus == Ndis802_11Encryption2Enabled))
+    		{
+				// Minus MIC length
+				pRxBlk->DataSize -= 8;
+
+    			// For TKIP frame, calculate the MIC value
+    			if (STACheckTkipMICValue(pAd, pEntry, pRxBlk) == FALSE)
+    			{
+    				return;
+    			}
+    		}
+
+    		STARxDataFrameAnnounce(pAd, pEntry, pRxBlk, FromWhichBSSID);
+			return;
+    	}
+    	else
+    	{
+    		// just return
+    		// because RTMPDeFragmentDataFrame() will release rx packet,
+    		// if packet is fragmented
+    		return;
+    	}
+	}
+
+	ASSERT(0);
+	// release packet
+	RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+}
+
+VOID STAHandleRxMgmtFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk)
+{
+	PRT28XX_RXD_STRUC	pRxD = &(pRxBlk->RxD);
+	PRXWI_STRUC		pRxWI = pRxBlk->pRxWI;
+	PHEADER_802_11	pHeader = pRxBlk->pHeader;
+	PNDIS_PACKET	pRxPacket = pRxBlk->pRxPacket;
+
+	do
+	{
+
+		// We should collect RSSI not only U2M data but also my beacon
+		if ((pHeader->FC.SubType == SUBTYPE_BEACON) && (MAC_ADDR_EQUAL(&pAd->CommonCfg.Bssid, &pHeader->Addr2)))
+		{
+			Update_Rssi_Sample(pAd, &pAd->StaCfg.RssiSample, pRxWI);
+
+			pAd->StaCfg.LastSNR0 = (UCHAR)(pRxWI->SNR0);
+			pAd->StaCfg.LastSNR1 = (UCHAR)(pRxWI->SNR1);
+		}
+
+		// First check the size, it MUST not exceed the mlme queue size
+		if (pRxWI->MPDUtotalByteCount > MGMT_DMA_BUFFER_SIZE)
+		{
+			DBGPRINT_ERR(("STAHandleRxMgmtFrame: frame too large, size = %d \n", pRxWI->MPDUtotalByteCount));
+			break;
+		}
+
+		REPORT_MGMT_FRAME_TO_MLME(pAd, pRxWI->WirelessCliID, pHeader, pRxWI->MPDUtotalByteCount,
+									pRxWI->RSSI0, pRxWI->RSSI1, pRxWI->RSSI2, pRxD->PlcpSignal);
+	} while (FALSE);
+
+	RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_SUCCESS);
+}
+
+VOID STAHandleRxControlFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk)
+{
+#ifdef DOT11_N_SUPPORT
+	PRXWI_STRUC		pRxWI = pRxBlk->pRxWI;
+#endif // DOT11_N_SUPPORT //
+	PHEADER_802_11	pHeader = pRxBlk->pHeader;
+	PNDIS_PACKET	pRxPacket = pRxBlk->pRxPacket;
+
+	switch (pHeader->FC.SubType)
+	{
+		case SUBTYPE_BLOCK_ACK_REQ:
+#ifdef DOT11_N_SUPPORT
+			{
+				CntlEnqueueForRecv(pAd, pRxWI->WirelessCliID, (pRxWI->MPDUtotalByteCount), (PFRAME_BA_REQ)pHeader);
+			}
+			break;
+#endif // DOT11_N_SUPPORT //
+		case SUBTYPE_BLOCK_ACK:
+		case SUBTYPE_ACK:
+		default:
+			break;
+	}
+
+	RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Process RxDone interrupt, running in DPC level
+
+	Arguments:
+		pAd Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+		This routine has to maintain Rx ring read pointer.
+		Need to consider QOS DATA format when converting to 802.3
+	========================================================================
+*/
+BOOLEAN STARxDoneInterruptHandle(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	BOOLEAN			argc)
+{
+	NDIS_STATUS			Status;
+	UINT32			RxProcessed, RxPending;
+	BOOLEAN			bReschedule = FALSE;
+	RT28XX_RXD_STRUC	*pRxD;
+	UCHAR			*pData;
+	PRXWI_STRUC		pRxWI;
+	PNDIS_PACKET	pRxPacket;
+	PHEADER_802_11	pHeader;
+	RX_BLK			RxCell;
+
+	RxProcessed = RxPending = 0;
+
+	// process whole rx ring
+	while (1)
+	{
+
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF |
+								fRTMP_ADAPTER_RESET_IN_PROGRESS |
+								fRTMP_ADAPTER_HALT_IN_PROGRESS |
+								fRTMP_ADAPTER_NIC_NOT_EXIST) ||
+			!RTMP_TEST_FLAG(pAd,fRTMP_ADAPTER_START_UP))
+		{
+			break;
+		}
+
+#ifdef RT2860
+		if (RxProcessed++ > MAX_RX_PROCESS_CNT)
+		{
+			// need to reschedule rx handle
+			bReschedule = TRUE;
+			break;
+		}
+#endif // RT2860 //
+
+		RxProcessed ++; // test
+
+		// 1. allocate a new data packet into rx ring to replace received packet
+		//    then processing the received packet
+		// 2. the callee must take charge of release of packet
+		// 3. As far as driver is concerned ,
+		//    the rx packet must
+		//      a. be indicated to upper layer or
+		//      b. be released if it is discarded
+		pRxPacket = GetPacketFromRxRing(pAd, &(RxCell.RxD), &bReschedule, &RxPending);
+		if (pRxPacket == NULL)
+		{
+			// no more packet to process
+			break;
+		}
+
+		// get rx ring descriptor
+		pRxD = &(RxCell.RxD);
+		// get rx data buffer
+		pData	= GET_OS_PKT_DATAPTR(pRxPacket);
+		pRxWI	= (PRXWI_STRUC) pData;
+		pHeader = (PHEADER_802_11) (pData+RXWI_SIZE) ;
+
+#ifdef RT_BIG_ENDIAN
+	    RTMPFrameEndianChange(pAd, (PUCHAR)pHeader, DIR_READ, TRUE);
+		RTMPWIEndianChange((PUCHAR)pRxWI, TYPE_RXWI);
+#endif
+
+		// build RxCell
+		RxCell.pRxWI = pRxWI;
+		RxCell.pHeader = pHeader;
+		RxCell.pRxPacket = pRxPacket;
+		RxCell.pData = (UCHAR *) pHeader;
+		RxCell.DataSize = pRxWI->MPDUtotalByteCount;
+		RxCell.Flags = 0;
+
+		// Increase Total receive byte counter after real data received no mater any error or not
+		pAd->RalinkCounters.ReceivedByteCount +=  pRxWI->MPDUtotalByteCount;
+		pAd->RalinkCounters.RxCount ++;
+
+		INC_COUNTER64(pAd->WlanCounters.ReceivedFragmentCount);
+
+		if (pRxWI->MPDUtotalByteCount < 14)
+			Status = NDIS_STATUS_FAILURE;
+
+        if (MONITOR_ON(pAd))
+		{
+            send_monitor_packets(pAd, &RxCell);
+			break;
+		}
+		/* RT2870 invokes STARxDoneInterruptHandle() in rtusb_bulk.c */
+#ifdef RALINK_ATE
+		if (ATE_ON(pAd))
+		{
+			pAd->ate.RxCntPerSec++;
+			ATESampleRssi(pAd, pRxWI);
+#ifdef RALINK_28xx_QA
+			if (pAd->ate.bQARxStart == TRUE)
+			{
+				/* (*pRxD) has been swapped in GetPacketFromRxRing() */
+				ATE_QA_Statistics(pAd, pRxWI, pRxD,	pHeader);
+			}
+#endif // RALINK_28xx_QA //
+			RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_SUCCESS);
+			continue;
+		}
+#endif // RALINK_ATE //
+
+		// Check for all RxD errors
+		Status = RTMPCheckRxError(pAd, pHeader, pRxWI, pRxD);
+
+		// Handle the received frame
+		if (Status == NDIS_STATUS_SUCCESS)
+		{
+			switch (pHeader->FC.Type)
+			{
+				// CASE I, receive a DATA frame
+				case BTYPE_DATA:
+				{
+					// process DATA frame
+					STAHandleRxDataFrame(pAd, &RxCell);
+				}
+				break;
+				// CASE II, receive a MGMT frame
+				case BTYPE_MGMT:
+				{
+					STAHandleRxMgmtFrame(pAd, &RxCell);
+				}
+				break;
+				// CASE III. receive a CNTL frame
+				case BTYPE_CNTL:
+				{
+					STAHandleRxControlFrame(pAd, &RxCell);
+				}
+				break;
+				// discard other type
+				default:
+					RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+					break;
+			}
+		}
+		else
+		{
+			pAd->Counters8023.RxErrors++;
+			// discard this frame
+			RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+		}
+	}
+
+	return bReschedule;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	IRQL = DISPATCH_LEVEL
+
+	========================================================================
+*/
+VOID	RTMPHandleTwakeupInterrupt(
+	IN PRTMP_ADAPTER pAd)
+{
+	AsicForceWakeup(pAd, FALSE);
+}
+
+/*
+========================================================================
+Routine Description:
+    Early checking and OS-depened parsing for Tx packet send to our STA driver.
+
+Arguments:
+    NDIS_HANDLE 	MiniportAdapterContext	Pointer refer to the device handle, i.e., the pAd.
+	PPNDIS_PACKET	ppPacketArray			The packet array need to do transmission.
+	UINT			NumberOfPackets			Number of packet in packet array.
+
+Return Value:
+	NONE
+
+Note:
+	This function do early checking and classification for send-out packet.
+	You only can put OS-depened & STA related code in here.
+========================================================================
+*/
+VOID STASendPackets(
+	IN	NDIS_HANDLE		MiniportAdapterContext,
+	IN	PPNDIS_PACKET	ppPacketArray,
+	IN	UINT			NumberOfPackets)
+{
+	UINT			Index;
+	PRTMP_ADAPTER	pAd = (PRTMP_ADAPTER) MiniportAdapterContext;
+	PNDIS_PACKET	pPacket;
+	BOOLEAN			allowToSend = FALSE;
+
+
+	for (Index = 0; Index < NumberOfPackets; Index++)
+	{
+		pPacket = ppPacketArray[Index];
+
+		do
+		{
+			if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS) ||
+				RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS) ||
+				RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF))
+			{
+				// Drop send request since hardware is in reset state
+					break;
+			}
+			else if (!INFRA_ON(pAd) && !ADHOC_ON(pAd))
+			{
+				// Drop send request since there are no physical connection yet
+					break;
+			}
+			else
+			{
+				// Record that orignal packet source is from NDIS layer,so that
+				// later on driver knows how to release this NDIS PACKET
+#ifdef QOS_DLS_SUPPORT
+				MAC_TABLE_ENTRY *pEntry;
+				PUCHAR pSrcBufVA = GET_OS_PKT_DATAPTR(pPacket);
+
+				pEntry = MacTableLookup(pAd, pSrcBufVA);
+				if (pEntry && (pEntry->ValidAsDls == TRUE))
+				{
+					RTMP_SET_PACKET_WCID(pPacket, pEntry->Aid);
+				}
+				else
+#endif // QOS_DLS_SUPPORT //
+				RTMP_SET_PACKET_WCID(pPacket, 0); // this field is useless when in STA mode
+				RTMP_SET_PACKET_SOURCE(pPacket, PKTSRC_NDIS);
+				NDIS_SET_PACKET_STATUS(pPacket, NDIS_STATUS_PENDING);
+				pAd->RalinkCounters.PendingNdisPacketCount++;
+
+				allowToSend = TRUE;
+			}
+		} while(FALSE);
+
+		if (allowToSend == TRUE)
+			STASendPacket(pAd, pPacket);
+		else
+			RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+	}
+
+	// Dequeue outgoing frames from TxSwQueue[] and process it
+	RTMPDeQueuePacket(pAd, FALSE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+
+}
+
+
+/*
+========================================================================
+Routine Description:
+	This routine is used to do packet parsing and classification for Tx packet
+	to STA device, and it will en-queue packets to our TxSwQueue depends on AC
+	class.
+
+Arguments:
+	pAd    		Pointer to our adapter
+	pPacket 	Pointer to send packet
+
+Return Value:
+	NDIS_STATUS_SUCCESS			If succes to queue the packet into TxSwQueue.
+	NDIS_STATUS_FAILURE			If failed to do en-queue.
+
+Note:
+	You only can put OS-indepened & STA related code in here.
+========================================================================
+*/
+NDIS_STATUS STASendPacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket)
+{
+	PACKET_INFO 	PacketInfo;
+	PUCHAR			pSrcBufVA;
+	UINT			SrcBufLen;
+	UINT			AllowFragSize;
+	UCHAR			NumberOfFrag;
+	UCHAR			QueIdx, UserPriority;
+	MAC_TABLE_ENTRY *pEntry = NULL;
+	unsigned int 	IrqFlags;
+	UCHAR			FlgIsIP = 0;
+	UCHAR			Rate;
+
+	// Prepare packet information structure for buffer descriptor
+	// chained within a single NDIS packet.
+	RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pSrcBufVA, &SrcBufLen);
+
+	if (pSrcBufVA == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("STASendPacket --> pSrcBufVA == NULL !!!SrcBufLen=%x\n",SrcBufLen));
+		// Resourece is low, system did not allocate virtual address
+		// return NDIS_STATUS_FAILURE directly to upper layer
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+		return NDIS_STATUS_FAILURE;
+	}
+
+
+	if (SrcBufLen < 14)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("STASendPacket --> Ndis Packet buffer error !!!\n"));
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+		return (NDIS_STATUS_FAILURE);
+	}
+
+	// In HT rate adhoc mode, A-MPDU is often used. So need to lookup BA Table and MAC Entry.
+	// Note multicast packets in adhoc also use BSSID_WCID index.
+	{
+		if(INFRA_ON(pAd))
+		{
+#ifdef QOS_DLS_SUPPORT
+			USHORT	tmpWcid;
+
+			tmpWcid = RTMP_GET_PACKET_WCID(pPacket);
+			if (VALID_WCID(tmpWcid) &&
+				(pAd->MacTab.Content[tmpWcid].ValidAsDls== TRUE))
+			{
+				pEntry = &pAd->MacTab.Content[tmpWcid];
+				Rate = pAd->MacTab.Content[tmpWcid].CurrTxRate;
+			}
+			else
+#endif // QOS_DLS_SUPPORT //
+			{
+			pEntry = &pAd->MacTab.Content[BSSID_WCID];
+			RTMP_SET_PACKET_WCID(pPacket, BSSID_WCID);
+			Rate = pAd->CommonCfg.TxRate;
+		}
+		}
+		else if (ADHOC_ON(pAd))
+		{
+			if (*pSrcBufVA & 0x01)
+			{
+				RTMP_SET_PACKET_WCID(pPacket, MCAST_WCID);
+				pEntry = &pAd->MacTab.Content[MCAST_WCID];
+			}
+			else
+			{
+				pEntry = MacTableLookup(pAd, pSrcBufVA);
+			}
+			Rate = pAd->CommonCfg.TxRate;
+		}
+	}
+
+	if (!pEntry)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("STASendPacket->Cannot find pEntry(%2x:%2x:%2x:%2x:%2x:%2x) in MacTab!\n", PRINT_MAC(pSrcBufVA)));
+		// Resourece is low, system did not allocate virtual address
+		// return NDIS_STATUS_FAILURE directly to upper layer
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+		return NDIS_STATUS_FAILURE;
+	}
+
+	if (ADHOC_ON(pAd)
+		)
+	{
+		RTMP_SET_PACKET_WCID(pPacket, (UCHAR)pEntry->Aid);
+	}
+
+	//
+	// Check the Ethernet Frame type of this packet, and set the RTMP_SET_PACKET_SPECIFIC flags.
+	//		Here we set the PACKET_SPECIFIC flags(LLC, VLAN, DHCP/ARP, EAPOL).
+	RTMPCheckEtherType(pAd, pPacket);
+
+
+
+	//
+	// WPA 802.1x secured port control - drop all non-802.1x frame before port secured
+	//
+	if (((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
+		 (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+		 (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) ||
+		 (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
+#ifdef WPA_SUPPLICANT_SUPPORT
+		  || (pAd->StaCfg.IEEE8021X == TRUE)
+#endif // WPA_SUPPLICANT_SUPPORT //
+#ifdef LEAP_SUPPORT
+		  || (pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP)
+#endif // LEAP_SUPPORT //
+		  )
+		  && ((pAd->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED) || (pAd->StaCfg.MicErrCnt >= 2))
+		  && (RTMP_GET_PACKET_EAPOL(pPacket)== FALSE)
+		  )
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("STASendPacket --> Drop packet before port secured !!!\n"));
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+
+		return (NDIS_STATUS_FAILURE);
+	}
+
+
+	// STEP 1. Decide number of fragments required to deliver this MSDU.
+	//	   The estimation here is not very accurate because difficult to
+	//	   take encryption overhead into consideration here. The result
+	//	   "NumberOfFrag" is then just used to pre-check if enough free
+	//	   TXD are available to hold this MSDU.
+
+
+	if (*pSrcBufVA & 0x01)	// fragmentation not allowed on multicast & broadcast
+		NumberOfFrag = 1;
+	else if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED))
+		NumberOfFrag = 1;	// Aggregation overwhelms fragmentation
+	else if (CLIENT_STATUS_TEST_FLAG(pEntry, fCLIENT_STATUS_AMSDU_INUSED))
+		NumberOfFrag = 1;	// Aggregation overwhelms fragmentation
+#ifdef DOT11_N_SUPPORT
+	else if ((pAd->StaCfg.HTPhyMode.field.MODE == MODE_HTMIX) || (pAd->StaCfg.HTPhyMode.field.MODE == MODE_HTGREENFIELD))
+		NumberOfFrag = 1;	// MIMO RATE overwhelms fragmentation
+#endif // DOT11_N_SUPPORT //
+	else
+	{
+		// The calculated "NumberOfFrag" is a rough estimation because of various
+		// encryption/encapsulation overhead not taken into consideration. This number is just
+		// used to make sure enough free TXD are available before fragmentation takes place.
+		// In case the actual required number of fragments of an NDIS packet
+		// excceeds "NumberOfFrag"caculated here and not enough free TXD available, the
+		// last fragment (i.e. last MPDU) will be dropped in RTMPHardTransmit() due to out of
+		// resource, and the NDIS packet will be indicated NDIS_STATUS_FAILURE. This should
+		// rarely happen and the penalty is just like a TX RETRY fail. Affordable.
+
+		AllowFragSize = (pAd->CommonCfg.FragmentThreshold) - LENGTH_802_11 - LENGTH_CRC;
+		NumberOfFrag = ((PacketInfo.TotalPacketLength - LENGTH_802_3 + LENGTH_802_1_H) / AllowFragSize) + 1;
+		// To get accurate number of fragmentation, Minus 1 if the size just match to allowable fragment size
+		if (((PacketInfo.TotalPacketLength - LENGTH_802_3 + LENGTH_802_1_H) % AllowFragSize) == 0)
+		{
+			NumberOfFrag--;
+		}
+	}
+
+	// Save fragment number to Ndis packet reserved field
+	RTMP_SET_PACKET_FRAGMENTS(pPacket, NumberOfFrag);
+
+
+	// STEP 2. Check the requirement of RTS:
+	//	   If multiple fragment required, RTS is required only for the first fragment
+	//	   if the fragment size large than RTS threshold
+	//     For RT28xx, Let ASIC send RTS/CTS
+	RTMP_SET_PACKET_RTS(pPacket, 0);
+	RTMP_SET_PACKET_TXRATE(pPacket, pAd->CommonCfg.TxRate);
+
+	//
+	// STEP 3. Traffic classification. outcome = <UserPriority, QueIdx>
+	//
+	UserPriority = 0;
+	QueIdx		 = QID_AC_BE;
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED))
+	{
+		USHORT Protocol;
+		UCHAR  LlcSnapLen = 0, Byte0, Byte1;
+		do
+		{
+			// get Ethernet protocol field
+			Protocol = (USHORT)((pSrcBufVA[12] << 8) + pSrcBufVA[13]);
+			if (Protocol <= 1500)
+			{
+				// get Ethernet protocol field from LLC/SNAP
+				if (Sniff2BytesFromNdisBuffer(PacketInfo.pFirstBuffer, LENGTH_802_3 + 6, &Byte0, &Byte1) != NDIS_STATUS_SUCCESS)
+					break;
+
+				Protocol = (USHORT)((Byte0 << 8) + Byte1);
+				LlcSnapLen = 8;
+			}
+
+			// always AC_BE for non-IP packet
+			if (Protocol != 0x0800)
+				break;
+
+			// get IP header
+			if (Sniff2BytesFromNdisBuffer(PacketInfo.pFirstBuffer, LENGTH_802_3 + LlcSnapLen, &Byte0, &Byte1) != NDIS_STATUS_SUCCESS)
+				break;
+
+			// return AC_BE if packet is not IPv4
+			if ((Byte0 & 0xf0) != 0x40)
+				break;
+
+			FlgIsIP = 1;
+			UserPriority = (Byte1 & 0xe0) >> 5;
+			QueIdx = MapUserPriorityToAccessCategory[UserPriority];
+
+			// TODO: have to check ACM bit. apply TSPEC if ACM is ON
+			// TODO: downgrade UP & QueIdx before passing ACM
+			if (pAd->CommonCfg.APEdcaParm.bACM[QueIdx])
+			{
+				UserPriority = 0;
+				QueIdx		 = QID_AC_BE;
+			}
+		} while (FALSE);
+	}
+
+	RTMP_SET_PACKET_UP(pPacket, UserPriority);
+
+
+
+	// Make sure SendTxWait queue resource won't be used by other threads
+	RTMP_IRQ_LOCK(&pAd->irq_lock, IrqFlags);
+	if (pAd->TxSwQueue[QueIdx].Number >= MAX_PACKETS_IN_QUEUE)
+	{
+		RTMP_IRQ_UNLOCK(&pAd->irq_lock, IrqFlags);
+#ifdef BLOCK_NET_IF
+		StopNetIfQueue(pAd, QueIdx, pPacket);
+#endif // BLOCK_NET_IF //
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+
+		return NDIS_STATUS_FAILURE;
+	}
+	else
+	{
+		InsertTailQueue(&pAd->TxSwQueue[QueIdx], PACKET_TO_QUEUE_ENTRY(pPacket));
+	}
+	RTMP_IRQ_UNLOCK(&pAd->irq_lock, IrqFlags);
+
+#ifdef DOT11_N_SUPPORT
+    if ((pAd->CommonCfg.BACapability.field.AutoBA == TRUE)&&
+        (pAd->StaActive.SupportedPhyInfo.bHtEnable == TRUE))
+	{
+		if (((pEntry->TXBAbitmap & (1<<UserPriority)) == 0) &&
+            ((pEntry->BADeclineBitmap & (1<<UserPriority)) == 0) &&
+            (pEntry->PortSecured == WPA_802_1X_PORT_SECURED)
+			 // For IOT compatibility, if
+			 // 1. It is Ralink chip or
+			 // 2. It is OPEN or AES mode,
+			 // then BA session can be bulit.
+			 && ((pEntry->ValidAsCLI && pAd->MlmeAux.APRalinkIe != 0x0) ||
+			 	 (pEntry->WepStatus == Ndis802_11WEPDisabled || pEntry->WepStatus == Ndis802_11Encryption3Enabled))
+			)
+		{
+			BAOriSessionSetUp(pAd, pEntry, 0, 0, 10, FALSE);
+		}
+	}
+#endif // DOT11_N_SUPPORT //
+
+	pAd->RalinkCounters.OneSecOsTxCount[QueIdx]++; // TODO: for debug only. to be removed
+	return NDIS_STATUS_SUCCESS;
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		This subroutine will scan through releative ring descriptor to find
+		out avaliable free ring descriptor and compare with request size.
+
+	Arguments:
+		pAd Pointer to our adapter
+		QueIdx		Selected TX Ring
+
+	Return Value:
+		NDIS_STATUS_FAILURE 	Not enough free descriptor
+		NDIS_STATUS_SUCCESS 	Enough free descriptor
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+#ifdef RT2860
+NDIS_STATUS RTMPFreeTXDRequest(
+	IN		PRTMP_ADAPTER	pAd,
+	IN		UCHAR			QueIdx,
+	IN		UCHAR			NumberRequired,
+	IN		PUCHAR			FreeNumberIs)
+{
+	ULONG		FreeNumber = 0;
+	NDIS_STATUS 	Status = NDIS_STATUS_FAILURE;
+
+	switch (QueIdx)
+	{
+		case QID_AC_BK:
+		case QID_AC_BE:
+		case QID_AC_VI:
+		case QID_AC_VO:
+		case QID_HCCA:
+			if (pAd->TxRing[QueIdx].TxSwFreeIdx > pAd->TxRing[QueIdx].TxCpuIdx)
+				FreeNumber = pAd->TxRing[QueIdx].TxSwFreeIdx - pAd->TxRing[QueIdx].TxCpuIdx - 1;
+			else
+				FreeNumber = pAd->TxRing[QueIdx].TxSwFreeIdx + TX_RING_SIZE - pAd->TxRing[QueIdx].TxCpuIdx - 1;
+
+			if (FreeNumber >= NumberRequired)
+				Status = NDIS_STATUS_SUCCESS;
+			break;
+
+		case QID_MGMT:
+			if (pAd->MgmtRing.TxSwFreeIdx > pAd->MgmtRing.TxCpuIdx)
+				FreeNumber = pAd->MgmtRing.TxSwFreeIdx - pAd->MgmtRing.TxCpuIdx - 1;
+			else
+				FreeNumber = pAd->MgmtRing.TxSwFreeIdx + MGMT_RING_SIZE - pAd->MgmtRing.TxCpuIdx - 1;
+
+			if (FreeNumber >= NumberRequired)
+				Status = NDIS_STATUS_SUCCESS;
+			break;
+
+		default:
+			DBGPRINT(RT_DEBUG_ERROR,("RTMPFreeTXDRequest::Invalid QueIdx(=%d)\n", QueIdx));
+			break;
+	}
+	*FreeNumberIs = (UCHAR)FreeNumber;
+
+	return (Status);
+}
+#endif // RT2860 //
+
+
+
+VOID RTMPSendDisassociationFrame(
+	IN	PRTMP_ADAPTER	pAd)
+{
+}
+
+VOID	RTMPSendNullFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			TxRate,
+	IN	BOOLEAN 		bQosNull)
+{
+	UCHAR	NullFrame[48];
+	ULONG	Length;
+	PHEADER_802_11	pHeader_802_11;
+
+
+#ifdef RALINK_ATE
+	if(ATE_ON(pAd))
+	{
+		return;
+	}
+#endif // RALINK_ATE //
+
+    // WPA 802.1x secured port control
+    if (((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
+         (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+         (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) ||
+         (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
+#ifdef WPA_SUPPLICANT_SUPPORT
+			  || (pAd->StaCfg.IEEE8021X == TRUE)
+#endif
+        ) &&
+       (pAd->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
+	{
+		return;
+	}
+
+	NdisZeroMemory(NullFrame, 48);
+	Length = sizeof(HEADER_802_11);
+
+	pHeader_802_11 = (PHEADER_802_11) NullFrame;
+
+	pHeader_802_11->FC.Type = BTYPE_DATA;
+	pHeader_802_11->FC.SubType = SUBTYPE_NULL_FUNC;
+	pHeader_802_11->FC.ToDs = 1;
+	COPY_MAC_ADDR(pHeader_802_11->Addr1, pAd->CommonCfg.Bssid);
+	COPY_MAC_ADDR(pHeader_802_11->Addr2, pAd->CurrentAddress);
+	COPY_MAC_ADDR(pHeader_802_11->Addr3, pAd->CommonCfg.Bssid);
+
+	if (pAd->CommonCfg.bAPSDForcePowerSave)
+	{
+		pHeader_802_11->FC.PwrMgmt = PWR_SAVE;
+	}
+	else
+	{
+		pHeader_802_11->FC.PwrMgmt = (pAd->StaCfg.Psm == PWR_SAVE) ? 1: 0;
+	}
+	pHeader_802_11->Duration = pAd->CommonCfg.Dsifs + RTMPCalcDuration(pAd, TxRate, 14);
+
+	pAd->Sequence++;
+	pHeader_802_11->Sequence = pAd->Sequence;
+
+	// Prepare QosNull function frame
+	if (bQosNull)
+	{
+		pHeader_802_11->FC.SubType = SUBTYPE_QOS_NULL;
+
+		// copy QOS control bytes
+		NullFrame[Length]	=  0;
+		NullFrame[Length+1] =  0;
+		Length += 2;// if pad with 2 bytes for alignment, APSD will fail
+	}
+
+	HAL_KickOutNullFrameTx(pAd, 0, NullFrame, Length);
+
+}
+
+// IRQL = DISPATCH_LEVEL
+VOID	RTMPSendRTSFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pDA,
+	IN	unsigned int	NextMpduSize,
+	IN	UCHAR			TxRate,
+	IN	UCHAR			RTSRate,
+	IN	USHORT			AckDuration,
+	IN	UCHAR			QueIdx,
+	IN	UCHAR			FrameGap)
+{
+}
+
+
+
+// --------------------------------------------------------
+//  FIND ENCRYPT KEY AND DECIDE CIPHER ALGORITHM
+//		Find the WPA key, either Group or Pairwise Key
+//		LEAP + TKIP also use WPA key.
+// --------------------------------------------------------
+// Decide WEP bit and cipher suite to be used. Same cipher suite should be used for whole fragment burst
+// In Cisco CCX 2.0 Leap Authentication
+//		   WepStatus is Ndis802_11Encryption1Enabled but the key will use PairwiseKey
+//		   Instead of the SharedKey, SharedKey Length may be Zero.
+VOID STAFindCipherAlgorithm(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk)
+{
+	NDIS_802_11_ENCRYPTION_STATUS	Cipher;				// To indicate cipher used for this packet
+	UCHAR							CipherAlg = CIPHER_NONE;		// cipher alogrithm
+	UCHAR							KeyIdx = 0xff;
+	PUCHAR							pSrcBufVA;
+	PCIPHER_KEY						pKey = NULL;
+
+	pSrcBufVA = GET_OS_PKT_DATAPTR(pTxBlk->pPacket);
+
+	{
+	    // Select Cipher
+	    if ((*pSrcBufVA & 0x01) && (ADHOC_ON(pAd)))
+	        Cipher = pAd->StaCfg.GroupCipher; // Cipher for Multicast or Broadcast
+	    else
+	        Cipher = pAd->StaCfg.PairCipher; // Cipher for Unicast
+
+		if (RTMP_GET_PACKET_EAPOL(pTxBlk->pPacket))
+		{
+			ASSERT(pAd->SharedKey[BSS0][0].CipherAlg <= CIPHER_CKIP128);
+
+			// 4-way handshaking frame must be clear
+			if (!(TX_BLK_TEST_FLAG(pTxBlk, fTX_bClearEAPFrame)) && (pAd->SharedKey[BSS0][0].CipherAlg) &&
+				(pAd->SharedKey[BSS0][0].KeyLen))
+			{
+				CipherAlg = pAd->SharedKey[BSS0][0].CipherAlg;
+				KeyIdx = 0;
+			}
+		}
+		else if (Cipher == Ndis802_11Encryption1Enabled)
+		{
+#ifdef LEAP_SUPPORT
+			if (pAd->StaCfg.CkipFlag & 0x10) // Cisco CKIP KP is on
+			{
+				if (LEAP_CCKM_ON(pAd))
+				{
+					if (((*pSrcBufVA & 0x01) && (ADHOC_ON(pAd))))
+						KeyIdx = 1;
+					else
+						KeyIdx = 0;
+				}
+				else
+					KeyIdx = pAd->StaCfg.DefaultKeyId;
+			}
+			else if (pAd->StaCfg.CkipFlag & 0x08) // only CKIP CMIC
+				KeyIdx = pAd->StaCfg.DefaultKeyId;
+			else if (LEAP_CCKM_ON(pAd))
+			{
+				if ((*pSrcBufVA & 0x01) && (ADHOC_ON(pAd)))
+					KeyIdx = 1;
+				else
+					KeyIdx = 0;
+			}
+			else	// standard WEP64 or WEP128
+#endif // LEAP_SUPPORT //
+				KeyIdx = pAd->StaCfg.DefaultKeyId;
+		}
+		else if ((Cipher == Ndis802_11Encryption2Enabled) ||
+				 (Cipher == Ndis802_11Encryption3Enabled))
+		{
+			if ((*pSrcBufVA & 0x01) && (ADHOC_ON(pAd))) // multicast
+				KeyIdx = pAd->StaCfg.DefaultKeyId;
+			else if (pAd->SharedKey[BSS0][0].KeyLen)
+				KeyIdx = 0;
+			else
+				KeyIdx = pAd->StaCfg.DefaultKeyId;
+		}
+
+		if (KeyIdx == 0xff)
+			CipherAlg = CIPHER_NONE;
+		else if ((Cipher == Ndis802_11EncryptionDisabled) || (pAd->SharedKey[BSS0][KeyIdx].KeyLen == 0))
+			CipherAlg = CIPHER_NONE;
+#ifdef WPA_SUPPLICANT_SUPPORT
+	    else if ( pAd->StaCfg.WpaSupplicantUP &&
+	             (Cipher == Ndis802_11Encryption1Enabled) &&
+	             (pAd->StaCfg.IEEE8021X == TRUE) &&
+	             (pAd->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
+	        CipherAlg = CIPHER_NONE;
+#endif // WPA_SUPPLICANT_SUPPORT //
+		else
+		{
+			//Header_802_11.FC.Wep = 1;
+			CipherAlg = pAd->SharedKey[BSS0][KeyIdx].CipherAlg;
+			pKey = &pAd->SharedKey[BSS0][KeyIdx];
+		}
+	}
+
+	pTxBlk->CipherAlg = CipherAlg;
+	pTxBlk->pKey = pKey;
+}
+
+
+VOID STABuildCommon802_11Header(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  TX_BLK          *pTxBlk)
+{
+
+	HEADER_802_11	*pHeader_802_11;
+#ifdef QOS_DLS_SUPPORT
+	BOOLEAN	bDLSFrame = FALSE;
+	INT	DlsEntryIndex = 0;
+#endif // QOS_DLS_SUPPORT //
+
+	//
+	// MAKE A COMMON 802.11 HEADER
+	//
+
+	// normal wlan header size : 24 octets
+	pTxBlk->MpduHeaderLen = sizeof(HEADER_802_11);
+
+	pHeader_802_11 = (HEADER_802_11 *) &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
+
+	NdisZeroMemory(pHeader_802_11, sizeof(HEADER_802_11));
+
+	pHeader_802_11->FC.FrDs = 0;
+	pHeader_802_11->FC.Type = BTYPE_DATA;
+	pHeader_802_11->FC.SubType = ((TX_BLK_TEST_FLAG(pTxBlk, fTX_bWMM)) ? SUBTYPE_QDATA : SUBTYPE_DATA);
+
+#ifdef QOS_DLS_SUPPORT
+	if (INFRA_ON(pAd))
+	{
+		// Check if the frame can be sent through DLS direct link interface
+		// If packet can be sent through DLS, then force aggregation disable. (Hard to determine peer STA's capability)
+		DlsEntryIndex = RTMPCheckDLSFrame(pAd, pTxBlk->pSrcBufHeader);
+		if (DlsEntryIndex >= 0)
+			bDLSFrame = TRUE;
+		else
+			bDLSFrame = FALSE;
+	}
+#endif // QOS_DLS_SUPPORT //
+
+    if (pTxBlk->pMacEntry)
+	{
+		if (TX_BLK_TEST_FLAG(pTxBlk, fTX_bForceNonQoS))
+		{
+			pHeader_802_11->Sequence = pTxBlk->pMacEntry->NonQosDataSeq;
+			pTxBlk->pMacEntry->NonQosDataSeq = (pTxBlk->pMacEntry->NonQosDataSeq+1) & MAXSEQ;
+		}
+		else
+		{
+#ifdef QOS_DLS_SUPPORT
+			if (bDLSFrame)
+			{
+				pHeader_802_11->Sequence = pAd->StaCfg.DLSEntry[DlsEntryIndex].Sequence;
+				pAd->StaCfg.DLSEntry[DlsEntryIndex].Sequence = (pAd->StaCfg.DLSEntry[DlsEntryIndex].Sequence+1) & MAXSEQ;
+			}
+			else
+#endif // QOS_DLS_SUPPORT //
+			{
+    	    pHeader_802_11->Sequence = pTxBlk->pMacEntry->TxSeq[pTxBlk->UserPriority];
+    	    pTxBlk->pMacEntry->TxSeq[pTxBlk->UserPriority] = (pTxBlk->pMacEntry->TxSeq[pTxBlk->UserPriority]+1) & MAXSEQ;
+    	}
+	}
+	}
+	else
+	{
+		pHeader_802_11->Sequence = pAd->Sequence;
+		pAd->Sequence = (pAd->Sequence+1) & MAXSEQ; // next sequence
+	}
+
+	pHeader_802_11->Frag = 0;
+
+	pHeader_802_11->FC.MoreData = TX_BLK_TEST_FLAG(pTxBlk, fTX_bMoreData);
+
+	{
+		if (INFRA_ON(pAd))
+		{
+#ifdef QOS_DLS_SUPPORT
+			if (bDLSFrame)
+			{
+				COPY_MAC_ADDR(pHeader_802_11->Addr1, pTxBlk->pSrcBufHeader);
+				COPY_MAC_ADDR(pHeader_802_11->Addr2, pAd->CurrentAddress);
+				COPY_MAC_ADDR(pHeader_802_11->Addr3, pAd->CommonCfg.Bssid);
+				pHeader_802_11->FC.ToDs = 0;
+			}
+			else
+#endif // QOS_DLS_SUPPORT //
+			{
+			COPY_MAC_ADDR(pHeader_802_11->Addr1, pAd->CommonCfg.Bssid);
+			COPY_MAC_ADDR(pHeader_802_11->Addr2, pAd->CurrentAddress);
+			COPY_MAC_ADDR(pHeader_802_11->Addr3, pTxBlk->pSrcBufHeader);
+			pHeader_802_11->FC.ToDs = 1;
+		}
+		}
+		else if (ADHOC_ON(pAd))
+		{
+			COPY_MAC_ADDR(pHeader_802_11->Addr1, pTxBlk->pSrcBufHeader);
+			COPY_MAC_ADDR(pHeader_802_11->Addr2, pAd->CurrentAddress);
+			COPY_MAC_ADDR(pHeader_802_11->Addr3, pAd->CommonCfg.Bssid);
+			pHeader_802_11->FC.ToDs = 0;
+		}
+	}
+
+	if (pTxBlk->CipherAlg != CIPHER_NONE)
+		pHeader_802_11->FC.Wep = 1;
+
+	// -----------------------------------------------------------------
+	// STEP 2. MAKE A COMMON 802.11 HEADER SHARED BY ENTIRE FRAGMENT BURST. Fill sequence later.
+	// -----------------------------------------------------------------
+	if (pAd->CommonCfg.bAPSDForcePowerSave)
+    	pHeader_802_11->FC.PwrMgmt = PWR_SAVE;
+	else
+    	pHeader_802_11->FC.PwrMgmt = (pAd->StaCfg.Psm == PWR_SAVE);
+}
+
+#ifdef DOT11_N_SUPPORT
+VOID STABuildCache802_11Header(
+	IN RTMP_ADAPTER		*pAd,
+	IN TX_BLK			*pTxBlk,
+	IN UCHAR			*pHeader)
+{
+	MAC_TABLE_ENTRY	*pMacEntry;
+	PHEADER_802_11	pHeader80211;
+
+	pHeader80211 = (PHEADER_802_11)pHeader;
+	pMacEntry = pTxBlk->pMacEntry;
+
+	//
+	// Update the cached 802.11 HEADER
+	//
+
+	// normal wlan header size : 24 octets
+	pTxBlk->MpduHeaderLen = sizeof(HEADER_802_11);
+
+	// More Bit
+	pHeader80211->FC.MoreData = TX_BLK_TEST_FLAG(pTxBlk, fTX_bMoreData);
+
+	// Sequence
+	pHeader80211->Sequence = pMacEntry->TxSeq[pTxBlk->UserPriority];
+    pMacEntry->TxSeq[pTxBlk->UserPriority] = (pMacEntry->TxSeq[pTxBlk->UserPriority]+1) & MAXSEQ;
+
+	{
+		// Check if the frame can be sent through DLS direct link interface
+		// If packet can be sent through DLS, then force aggregation disable. (Hard to determine peer STA's capability)
+#ifdef QOS_DLS_SUPPORT
+		BOOLEAN	bDLSFrame = FALSE;
+		INT	DlsEntryIndex = 0;
+
+		DlsEntryIndex = RTMPCheckDLSFrame(pAd, pTxBlk->pSrcBufHeader);
+		if (DlsEntryIndex >= 0)
+			bDLSFrame = TRUE;
+		else
+			bDLSFrame = FALSE;
+#endif // QOS_DLS_SUPPORT //
+
+		// The addr3 of normal packet send from DS is Dest Mac address.
+#ifdef QOS_DLS_SUPPORT
+		if (bDLSFrame)
+		{
+			COPY_MAC_ADDR(pHeader80211->Addr1, pTxBlk->pSrcBufHeader);
+			COPY_MAC_ADDR(pHeader80211->Addr3, pAd->CommonCfg.Bssid);
+			pHeader80211->FC.ToDs = 0;
+		}
+		else
+#endif // QOS_DLS_SUPPORT //
+		if (ADHOC_ON(pAd))
+			COPY_MAC_ADDR(pHeader80211->Addr3, pAd->CommonCfg.Bssid);
+		else
+			COPY_MAC_ADDR(pHeader80211->Addr3, pTxBlk->pSrcBufHeader);
+	}
+
+	// -----------------------------------------------------------------
+	// STEP 2. MAKE A COMMON 802.11 HEADER SHARED BY ENTIRE FRAGMENT BURST. Fill sequence later.
+	// -----------------------------------------------------------------
+	if (pAd->CommonCfg.bAPSDForcePowerSave)
+    	pHeader80211->FC.PwrMgmt = PWR_SAVE;
+	else
+    	pHeader80211->FC.PwrMgmt = (pAd->StaCfg.Psm == PWR_SAVE);
+}
+#endif // DOT11_N_SUPPORT //
+
+static inline PUCHAR STA_Build_ARalink_Frame_Header(
+	IN RTMP_ADAPTER *pAd,
+	IN TX_BLK		*pTxBlk)
+{
+	PUCHAR			pHeaderBufPtr;
+	HEADER_802_11	*pHeader_802_11;
+	PNDIS_PACKET	pNextPacket;
+	UINT32			nextBufLen;
+	PQUEUE_ENTRY	pQEntry;
+
+	STAFindCipherAlgorithm(pAd, pTxBlk);
+	STABuildCommon802_11Header(pAd, pTxBlk);
+
+
+	pHeaderBufPtr = &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
+	pHeader_802_11 = (HEADER_802_11 *) pHeaderBufPtr;
+
+	// steal "order" bit to mark "aggregation"
+	pHeader_802_11->FC.Order = 1;
+
+	// skip common header
+	pHeaderBufPtr += pTxBlk->MpduHeaderLen;
+
+	if (TX_BLK_TEST_FLAG(pTxBlk, fTX_bWMM))
+	{
+		//
+		// build QOS Control bytes
+		//
+		*pHeaderBufPtr = (pTxBlk->UserPriority & 0x0F);
+
+		*(pHeaderBufPtr+1) = 0;
+		pHeaderBufPtr +=2;
+		pTxBlk->MpduHeaderLen += 2;
+	}
+
+	// padding at front of LLC header. LLC header should at 4-bytes aligment.
+	pTxBlk->HdrPadLen = (ULONG)pHeaderBufPtr;
+	pHeaderBufPtr = (PCHAR)ROUND_UP(pHeaderBufPtr, 4);
+	pTxBlk->HdrPadLen = (ULONG)(pHeaderBufPtr - pTxBlk->HdrPadLen);
+
+	// For RA Aggregation,
+	// put the 2nd MSDU length(extra 2-byte field) after QOS_CONTROL in little endian format
+	pQEntry = pTxBlk->TxPacketList.Head;
+	pNextPacket = QUEUE_ENTRY_TO_PKT(pQEntry);
+	nextBufLen = GET_OS_PKT_LEN(pNextPacket);
+	if (RTMP_GET_PACKET_VLAN(pNextPacket))
+		nextBufLen -= LENGTH_802_1Q;
+
+	*pHeaderBufPtr = (UCHAR)nextBufLen & 0xff;
+	*(pHeaderBufPtr+1) = (UCHAR)(nextBufLen >> 8);
+
+	pHeaderBufPtr += 2;
+	pTxBlk->MpduHeaderLen += 2;
+
+	return pHeaderBufPtr;
+
+}
+
+#ifdef DOT11_N_SUPPORT
+static inline PUCHAR STA_Build_AMSDU_Frame_Header(
+	IN RTMP_ADAPTER *pAd,
+	IN TX_BLK		*pTxBlk)
+{
+	PUCHAR			pHeaderBufPtr;//, pSaveBufPtr;
+	HEADER_802_11	*pHeader_802_11;
+
+
+	STAFindCipherAlgorithm(pAd, pTxBlk);
+	STABuildCommon802_11Header(pAd, pTxBlk);
+
+	pHeaderBufPtr = &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
+	pHeader_802_11 = (HEADER_802_11 *) pHeaderBufPtr;
+
+	// skip common header
+	pHeaderBufPtr += pTxBlk->MpduHeaderLen;
+
+	//
+	// build QOS Control bytes
+	//
+	*pHeaderBufPtr = (pTxBlk->UserPriority & 0x0F);
+
+	//
+	// A-MSDU packet
+	//
+	*pHeaderBufPtr |= 0x80;
+
+	*(pHeaderBufPtr+1) = 0;
+	pHeaderBufPtr +=2;
+	pTxBlk->MpduHeaderLen += 2;
+
+	//pSaveBufPtr = pHeaderBufPtr;
+
+	//
+	// padding at front of LLC header
+	// LLC header should locate at 4-octets aligment
+	//
+	// @@@ MpduHeaderLen excluding padding @@@
+	//
+	pTxBlk->HdrPadLen = (ULONG)pHeaderBufPtr;
+	pHeaderBufPtr = (PCHAR) ROUND_UP(pHeaderBufPtr, 4);
+	pTxBlk->HdrPadLen = (ULONG)(pHeaderBufPtr - pTxBlk->HdrPadLen);
+
+	return pHeaderBufPtr;
+
+}
+
+
+VOID STA_AMPDU_Frame_Tx(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk)
+{
+	HEADER_802_11	*pHeader_802_11;
+	PUCHAR			pHeaderBufPtr;
+	USHORT			FreeNumber;
+	MAC_TABLE_ENTRY	*pMacEntry;
+	BOOLEAN			bVLANPkt;
+	PQUEUE_ENTRY	pQEntry;
+
+	ASSERT(pTxBlk);
+
+	while(pTxBlk->TxPacketList.Head)
+	{
+		pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
+		pTxBlk->pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
+		if ( RTMP_FillTxBlkInfo(pAd, pTxBlk) != TRUE)
+		{
+			RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
+			continue;
+		}
+
+		bVLANPkt = (RTMP_GET_PACKET_VLAN(pTxBlk->pPacket) ? TRUE : FALSE);
+
+		pMacEntry = pTxBlk->pMacEntry;
+		if (pMacEntry->isCached)
+		{
+			// NOTE: Please make sure the size of pMacEntry->CachedBuf[] is smaller than pTxBlk->HeaderBuf[]!!!!
+			NdisMoveMemory((PUCHAR)&pTxBlk->HeaderBuf[TXINFO_SIZE], (PUCHAR)&pMacEntry->CachedBuf[0], TXWI_SIZE + sizeof(HEADER_802_11));
+			pHeaderBufPtr = (PUCHAR)(&pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE]);
+			STABuildCache802_11Header(pAd, pTxBlk, pHeaderBufPtr);
+		}
+		else
+		{
+			STAFindCipherAlgorithm(pAd, pTxBlk);
+			STABuildCommon802_11Header(pAd, pTxBlk);
+
+			pHeaderBufPtr = &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
+		}
+
+
+		pHeader_802_11 = (HEADER_802_11 *) pHeaderBufPtr;
+
+		// skip common header
+		pHeaderBufPtr += pTxBlk->MpduHeaderLen;
+
+		//
+		// build QOS Control bytes
+		//
+		*pHeaderBufPtr = (pTxBlk->UserPriority & 0x0F);
+		*(pHeaderBufPtr+1) = 0;
+		pHeaderBufPtr +=2;
+		pTxBlk->MpduHeaderLen += 2;
+
+		//
+		// build HTC+
+		// HTC control filed following QoS field
+		//
+		if ((pAd->CommonCfg.bRdg == TRUE) && CLIENT_STATUS_TEST_FLAG(pTxBlk->pMacEntry, fCLIENT_STATUS_RDG_CAPABLE))
+		{
+			if (pMacEntry->isCached == FALSE)
+			{
+				// mark HTC bit
+				pHeader_802_11->FC.Order = 1;
+
+				NdisZeroMemory(pHeaderBufPtr, 4);
+				*(pHeaderBufPtr+3) |= 0x80;
+			}
+			pHeaderBufPtr += 4;
+			pTxBlk->MpduHeaderLen += 4;
+		}
+
+		//pTxBlk->MpduHeaderLen = pHeaderBufPtr - pTxBlk->HeaderBuf - TXWI_SIZE - TXINFO_SIZE;
+		ASSERT(pTxBlk->MpduHeaderLen >= 24);
+
+		// skip 802.3 header
+		pTxBlk->pSrcBufData = pTxBlk->pSrcBufHeader + LENGTH_802_3;
+		pTxBlk->SrcBufLen  -= LENGTH_802_3;
+
+		// skip vlan tag
+		if (bVLANPkt)
+		{
+			pTxBlk->pSrcBufData	+= LENGTH_802_1Q;
+			pTxBlk->SrcBufLen	-= LENGTH_802_1Q;
+		}
+
+		//
+		// padding at front of LLC header
+		// LLC header should locate at 4-octets aligment
+		//
+		// @@@ MpduHeaderLen excluding padding @@@
+		//
+		pTxBlk->HdrPadLen = (ULONG)pHeaderBufPtr;
+		pHeaderBufPtr = (PCHAR) ROUND_UP(pHeaderBufPtr, 4);
+		pTxBlk->HdrPadLen = (ULONG)(pHeaderBufPtr - pTxBlk->HdrPadLen);
+
+		{
+
+			//
+			// Insert LLC-SNAP encapsulation - 8 octets
+			//
+			EXTRA_LLCSNAP_ENCAP_FROM_PKT_OFFSET(pTxBlk->pSrcBufData-2, pTxBlk->pExtraLlcSnapEncap);
+			if (pTxBlk->pExtraLlcSnapEncap)
+			{
+				NdisMoveMemory(pHeaderBufPtr, pTxBlk->pExtraLlcSnapEncap, 6);
+				pHeaderBufPtr += 6;
+				// get 2 octets (TypeofLen)
+				NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufData-2, 2);
+				pHeaderBufPtr += 2;
+				pTxBlk->MpduHeaderLen += LENGTH_802_1_H;
+			}
+
+		}
+
+		if (pMacEntry->isCached)
+		{
+            RTMPWriteTxWI_Cache(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
+		}
+		else
+		{
+			RTMPWriteTxWI_Data(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
+
+			NdisZeroMemory((PUCHAR)(&pMacEntry->CachedBuf[0]), sizeof(pMacEntry->CachedBuf));
+			NdisMoveMemory((PUCHAR)(&pMacEntry->CachedBuf[0]), (PUCHAR)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), (pHeaderBufPtr - (PUCHAR)(&pTxBlk->HeaderBuf[TXINFO_SIZE])));
+			pMacEntry->isCached = TRUE;
+		}
+
+		// calculate Transmitted AMPDU count and ByteCount
+		{
+			pAd->RalinkCounters.TransmittedMPDUsInAMPDUCount.u.LowPart ++;
+			pAd->RalinkCounters.TransmittedOctetsInAMPDUCount.QuadPart += pTxBlk->SrcBufLen;
+		}
+
+		//FreeNumber = GET_TXRING_FREENO(pAd, QueIdx);
+
+		HAL_WriteTxResource(pAd, pTxBlk, TRUE, &FreeNumber);
+
+		//
+		// Kick out Tx
+		//
+		HAL_KickOutTx(pAd, pTxBlk, pTxBlk->QueIdx);
+
+		pAd->RalinkCounters.KickTxCount++;
+		pAd->RalinkCounters.OneSecTxDoneCount++;
+	}
+
+}
+
+
+VOID STA_AMSDU_Frame_Tx(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk)
+{
+	PUCHAR			pHeaderBufPtr;
+	USHORT			FreeNumber;
+	USHORT			subFramePayloadLen = 0;	// AMSDU Subframe length without AMSDU-Header / Padding.
+	USHORT			totalMPDUSize=0;
+	UCHAR			*subFrameHeader;
+	UCHAR			padding = 0;
+	USHORT			FirstTx = 0, LastTxIdx = 0;
+	BOOLEAN			bVLANPkt;
+	int 			frameNum = 0;
+	PQUEUE_ENTRY	pQEntry;
+
+
+	ASSERT(pTxBlk);
+
+	ASSERT((pTxBlk->TxPacketList.Number > 1));
+
+	while(pTxBlk->TxPacketList.Head)
+	{
+		pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
+		pTxBlk->pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
+		if (RTMP_FillTxBlkInfo(pAd, pTxBlk) != TRUE)
+		{
+			RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
+			continue;
+		}
+
+		bVLANPkt = (RTMP_GET_PACKET_VLAN(pTxBlk->pPacket) ? TRUE : FALSE);
+
+		// skip 802.3 header
+		pTxBlk->pSrcBufData = pTxBlk->pSrcBufHeader + LENGTH_802_3;
+		pTxBlk->SrcBufLen  -= LENGTH_802_3;
+
+		// skip vlan tag
+		if (bVLANPkt)
+		{
+			pTxBlk->pSrcBufData	+= LENGTH_802_1Q;
+			pTxBlk->SrcBufLen	-= LENGTH_802_1Q;
+		}
+
+		if (frameNum == 0)
+		{
+			pHeaderBufPtr = STA_Build_AMSDU_Frame_Header(pAd, pTxBlk);
+
+			// NOTE: TxWI->MPDUtotalByteCount will be updated after final frame was handled.
+			RTMPWriteTxWI_Data(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
+		}
+		else
+		{
+			pHeaderBufPtr = &pTxBlk->HeaderBuf[0];
+			padding = ROUND_UP(LENGTH_AMSDU_SUBFRAMEHEAD + subFramePayloadLen, 4) - (LENGTH_AMSDU_SUBFRAMEHEAD + subFramePayloadLen);
+			NdisZeroMemory(pHeaderBufPtr, padding + LENGTH_AMSDU_SUBFRAMEHEAD);
+			pHeaderBufPtr += padding;
+			pTxBlk->MpduHeaderLen = padding;
+		}
+
+		//
+		// A-MSDU subframe
+		//   DA(6)+SA(6)+Length(2) + LLC/SNAP Encap
+		//
+		subFrameHeader = pHeaderBufPtr;
+		subFramePayloadLen = pTxBlk->SrcBufLen;
+
+		NdisMoveMemory(subFrameHeader, pTxBlk->pSrcBufHeader, 12);
+
+
+		pHeaderBufPtr += LENGTH_AMSDU_SUBFRAMEHEAD;
+		pTxBlk->MpduHeaderLen += LENGTH_AMSDU_SUBFRAMEHEAD;
+
+
+		//
+		// Insert LLC-SNAP encapsulation - 8 octets
+		//
+		EXTRA_LLCSNAP_ENCAP_FROM_PKT_OFFSET(pTxBlk->pSrcBufData-2, pTxBlk->pExtraLlcSnapEncap);
+
+		subFramePayloadLen = pTxBlk->SrcBufLen;
+
+		if (pTxBlk->pExtraLlcSnapEncap)
+		{
+			NdisMoveMemory(pHeaderBufPtr, pTxBlk->pExtraLlcSnapEncap, 6);
+			pHeaderBufPtr += 6;
+			// get 2 octets (TypeofLen)
+			NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufData-2, 2);
+			pHeaderBufPtr += 2;
+			pTxBlk->MpduHeaderLen += LENGTH_802_1_H;
+			subFramePayloadLen += LENGTH_802_1_H;
+		}
+
+		// update subFrame Length field
+		subFrameHeader[12] = (subFramePayloadLen & 0xFF00) >> 8;
+		subFrameHeader[13] = subFramePayloadLen & 0xFF;
+
+		totalMPDUSize += pTxBlk->MpduHeaderLen + pTxBlk->SrcBufLen;
+
+		if (frameNum ==0)
+			FirstTx = HAL_WriteMultiTxResource(pAd, pTxBlk, frameNum, &FreeNumber);
+		else
+			LastTxIdx = HAL_WriteMultiTxResource(pAd, pTxBlk, frameNum, &FreeNumber);
+
+		frameNum++;
+
+		pAd->RalinkCounters.KickTxCount++;
+		pAd->RalinkCounters.OneSecTxDoneCount++;
+
+		// calculate Transmitted AMSDU Count and ByteCount
+		{
+			pAd->RalinkCounters.TransmittedAMSDUCount.u.LowPart ++;
+			pAd->RalinkCounters.TransmittedOctetsInAMSDU.QuadPart += totalMPDUSize;
+		}
+
+	}
+
+	HAL_FinalWriteTxResource(pAd, pTxBlk, totalMPDUSize, FirstTx);
+	HAL_LastTxIdx(pAd, pTxBlk->QueIdx, LastTxIdx);
+
+	//
+	// Kick out Tx
+	//
+	HAL_KickOutTx(pAd, pTxBlk, pTxBlk->QueIdx);
+}
+#endif // DOT11_N_SUPPORT //
+
+VOID STA_Legacy_Frame_Tx(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk)
+{
+	HEADER_802_11	*pHeader_802_11;
+	PUCHAR			pHeaderBufPtr;
+	USHORT			FreeNumber;
+	BOOLEAN			bVLANPkt;
+	PQUEUE_ENTRY	pQEntry;
+
+	ASSERT(pTxBlk);
+
+
+	pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
+	pTxBlk->pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
+	if (RTMP_FillTxBlkInfo(pAd, pTxBlk) != TRUE)
+	{
+		RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+
+	if (pTxBlk->TxFrameType == TX_MCAST_FRAME)
+	{
+		INC_COUNTER64(pAd->WlanCounters.MulticastTransmittedFrameCount);
+	}
+
+	if (RTMP_GET_PACKET_RTS(pTxBlk->pPacket))
+		TX_BLK_SET_FLAG(pTxBlk, fTX_bRtsRequired);
+	else
+		TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bRtsRequired);
+
+	bVLANPkt = (RTMP_GET_PACKET_VLAN(pTxBlk->pPacket) ? TRUE : FALSE);
+
+	if (pTxBlk->TxRate < pAd->CommonCfg.MinTxRate)
+		pTxBlk->TxRate = pAd->CommonCfg.MinTxRate;
+
+	STAFindCipherAlgorithm(pAd, pTxBlk);
+	STABuildCommon802_11Header(pAd, pTxBlk);
+
+
+	// skip 802.3 header
+	pTxBlk->pSrcBufData = pTxBlk->pSrcBufHeader + LENGTH_802_3;
+	pTxBlk->SrcBufLen  -= LENGTH_802_3;
+
+	// skip vlan tag
+	if (bVLANPkt)
+	{
+		pTxBlk->pSrcBufData	+= LENGTH_802_1Q;
+		pTxBlk->SrcBufLen	-= LENGTH_802_1Q;
+	}
+
+	pHeaderBufPtr = &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
+	pHeader_802_11 = (HEADER_802_11 *) pHeaderBufPtr;
+
+	// skip common header
+	pHeaderBufPtr += pTxBlk->MpduHeaderLen;
+
+	if (TX_BLK_TEST_FLAG(pTxBlk, fTX_bWMM))
+	{
+		//
+		// build QOS Control bytes
+		//
+		*pHeaderBufPtr = (pTxBlk->UserPriority & 0x0F);
+		*(pHeaderBufPtr+1) = 0;
+		pHeaderBufPtr +=2;
+		pTxBlk->MpduHeaderLen += 2;
+	}
+
+	// The remaining content of MPDU header should locate at 4-octets aligment
+	pTxBlk->HdrPadLen = (ULONG)pHeaderBufPtr;
+	pHeaderBufPtr = (PCHAR) ROUND_UP(pHeaderBufPtr, 4);
+	pTxBlk->HdrPadLen = (ULONG)(pHeaderBufPtr - pTxBlk->HdrPadLen);
+
+	{
+
+		//
+		// Insert LLC-SNAP encapsulation - 8 octets
+		//
+		//
+   		// if original Ethernet frame contains no LLC/SNAP,
+		// then an extra LLC/SNAP encap is required
+		//
+		EXTRA_LLCSNAP_ENCAP_FROM_PKT_START(pTxBlk->pSrcBufHeader, pTxBlk->pExtraLlcSnapEncap);
+		if (pTxBlk->pExtraLlcSnapEncap)
+		{
+			UCHAR vlan_size;
+
+			NdisMoveMemory(pHeaderBufPtr, pTxBlk->pExtraLlcSnapEncap, 6);
+			pHeaderBufPtr += 6;
+			// skip vlan tag
+			vlan_size =  (bVLANPkt) ? LENGTH_802_1Q : 0;
+			// get 2 octets (TypeofLen)
+			NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufHeader+12+vlan_size, 2);
+			pHeaderBufPtr += 2;
+			pTxBlk->MpduHeaderLen += LENGTH_802_1_H;
+		}
+
+	}
+
+	//
+	// prepare for TXWI
+	// use Wcid as Key Index
+	//
+
+	RTMPWriteTxWI_Data(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
+
+	//FreeNumber = GET_TXRING_FREENO(pAd, QueIdx);
+
+	HAL_WriteTxResource(pAd, pTxBlk, TRUE, &FreeNumber);
+
+	pAd->RalinkCounters.KickTxCount++;
+	pAd->RalinkCounters.OneSecTxDoneCount++;
+
+	//
+	// Kick out Tx
+	//
+	HAL_KickOutTx(pAd, pTxBlk, pTxBlk->QueIdx);
+}
+
+
+VOID STA_ARalink_Frame_Tx(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk)
+{
+	PUCHAR			pHeaderBufPtr;
+	USHORT			FreeNumber;
+	USHORT			totalMPDUSize=0;
+	USHORT			FirstTx, LastTxIdx;
+	int 			frameNum = 0;
+	BOOLEAN			bVLANPkt;
+	PQUEUE_ENTRY	pQEntry;
+
+
+	ASSERT(pTxBlk);
+
+	ASSERT((pTxBlk->TxPacketList.Number== 2));
+
+
+	FirstTx = LastTxIdx = 0;  // Is it ok init they as 0?
+	while(pTxBlk->TxPacketList.Head)
+	{
+		pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
+		pTxBlk->pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
+
+		if (RTMP_FillTxBlkInfo(pAd, pTxBlk) != TRUE)
+		{
+			RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
+			continue;
+		}
+
+		bVLANPkt = (RTMP_GET_PACKET_VLAN(pTxBlk->pPacket) ? TRUE : FALSE);
+
+		// skip 802.3 header
+		pTxBlk->pSrcBufData = pTxBlk->pSrcBufHeader + LENGTH_802_3;
+		pTxBlk->SrcBufLen  -= LENGTH_802_3;
+
+		// skip vlan tag
+		if (bVLANPkt)
+		{
+			pTxBlk->pSrcBufData	+= LENGTH_802_1Q;
+			pTxBlk->SrcBufLen	-= LENGTH_802_1Q;
+		}
+
+		if (frameNum == 0)
+		{	// For first frame, we need to create the 802.11 header + padding(optional) + RA-AGG-LEN + SNAP Header
+
+			pHeaderBufPtr = STA_Build_ARalink_Frame_Header(pAd, pTxBlk);
+
+			// It's ok write the TxWI here, because the TxWI->MPDUtotalByteCount
+			//	will be updated after final frame was handled.
+			RTMPWriteTxWI_Data(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
+
+
+			//
+			// Insert LLC-SNAP encapsulation - 8 octets
+			//
+			EXTRA_LLCSNAP_ENCAP_FROM_PKT_OFFSET(pTxBlk->pSrcBufData-2, pTxBlk->pExtraLlcSnapEncap);
+
+			if (pTxBlk->pExtraLlcSnapEncap)
+			{
+				NdisMoveMemory(pHeaderBufPtr, pTxBlk->pExtraLlcSnapEncap, 6);
+				pHeaderBufPtr += 6;
+				// get 2 octets (TypeofLen)
+				NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufData-2, 2);
+				pHeaderBufPtr += 2;
+				pTxBlk->MpduHeaderLen += LENGTH_802_1_H;
+			}
+		}
+		else
+		{	// For second aggregated frame, we need create the 802.3 header to headerBuf, because PCI will copy it to SDPtr0.
+
+			pHeaderBufPtr = &pTxBlk->HeaderBuf[0];
+			pTxBlk->MpduHeaderLen = 0;
+
+			// A-Ralink sub-sequent frame header is the same as 802.3 header.
+			//   DA(6)+SA(6)+FrameType(2)
+			NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufHeader, 12);
+			pHeaderBufPtr += 12;
+			// get 2 octets (TypeofLen)
+			NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufData-2, 2);
+			pHeaderBufPtr += 2;
+			pTxBlk->MpduHeaderLen = LENGTH_ARALINK_SUBFRAMEHEAD;
+		}
+
+		totalMPDUSize += pTxBlk->MpduHeaderLen + pTxBlk->SrcBufLen;
+
+		//FreeNumber = GET_TXRING_FREENO(pAd, QueIdx);
+		if (frameNum ==0)
+			FirstTx = HAL_WriteMultiTxResource(pAd, pTxBlk, frameNum, &FreeNumber);
+		else
+			LastTxIdx = HAL_WriteMultiTxResource(pAd, pTxBlk, frameNum, &FreeNumber);
+
+		frameNum++;
+
+		pAd->RalinkCounters.OneSecTxAggregationCount++;
+		pAd->RalinkCounters.KickTxCount++;
+		pAd->RalinkCounters.OneSecTxDoneCount++;
+
+	}
+
+	HAL_FinalWriteTxResource(pAd, pTxBlk, totalMPDUSize, FirstTx);
+	HAL_LastTxIdx(pAd, pTxBlk->QueIdx, LastTxIdx);
+
+	//
+	// Kick out Tx
+	//
+	HAL_KickOutTx(pAd, pTxBlk, pTxBlk->QueIdx);
+
+}
+
+
+VOID STA_Fragment_Frame_Tx(
+	IN RTMP_ADAPTER *pAd,
+	IN TX_BLK		*pTxBlk)
+{
+	HEADER_802_11	*pHeader_802_11;
+	PUCHAR			pHeaderBufPtr;
+	USHORT			FreeNumber;
+	UCHAR 			fragNum = 0;
+	PACKET_INFO		PacketInfo;
+	USHORT			EncryptionOverhead = 0;
+	UINT32			FreeMpduSize, SrcRemainingBytes;
+	USHORT			AckDuration;
+	UINT 			NextMpduSize;
+	BOOLEAN			bVLANPkt;
+	PQUEUE_ENTRY	pQEntry;
+
+
+	ASSERT(pTxBlk);
+
+	pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
+	pTxBlk->pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
+	if (RTMP_FillTxBlkInfo(pAd, pTxBlk) != TRUE)
+	{
+		RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+
+	ASSERT(TX_BLK_TEST_FLAG(pTxBlk, fTX_bAllowFrag));
+	bVLANPkt = (RTMP_GET_PACKET_VLAN(pTxBlk->pPacket) ? TRUE : FALSE);
+
+	STAFindCipherAlgorithm(pAd, pTxBlk);
+	STABuildCommon802_11Header(pAd, pTxBlk);
+
+	if (pTxBlk->CipherAlg == CIPHER_TKIP)
+	{
+		pTxBlk->pPacket = duplicate_pkt_with_TKIP_MIC(pAd, pTxBlk->pPacket);
+		if (pTxBlk->pPacket == NULL)
+			return;
+		RTMP_QueryPacketInfo(pTxBlk->pPacket, &PacketInfo, &pTxBlk->pSrcBufHeader, &pTxBlk->SrcBufLen);
+	}
+
+	// skip 802.3 header
+	pTxBlk->pSrcBufData = pTxBlk->pSrcBufHeader + LENGTH_802_3;
+	pTxBlk->SrcBufLen  -= LENGTH_802_3;
+
+
+	// skip vlan tag
+	if (bVLANPkt)
+	{
+		pTxBlk->pSrcBufData	+= LENGTH_802_1Q;
+		pTxBlk->SrcBufLen	-= LENGTH_802_1Q;
+	}
+
+	pHeaderBufPtr = &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
+	pHeader_802_11 = (HEADER_802_11 *)pHeaderBufPtr;
+
+
+	// skip common header
+	pHeaderBufPtr += pTxBlk->MpduHeaderLen;
+
+	if (TX_BLK_TEST_FLAG(pTxBlk, fTX_bWMM))
+	{
+		//
+		// build QOS Control bytes
+		//
+		*pHeaderBufPtr = (pTxBlk->UserPriority & 0x0F);
+
+		*(pHeaderBufPtr+1) = 0;
+		pHeaderBufPtr +=2;
+		pTxBlk->MpduHeaderLen += 2;
+	}
+
+	//
+	// padding at front of LLC header
+	// LLC header should locate at 4-octets aligment
+	//
+	pTxBlk->HdrPadLen = (ULONG)pHeaderBufPtr;
+	pHeaderBufPtr = (PCHAR) ROUND_UP(pHeaderBufPtr, 4);
+	pTxBlk->HdrPadLen = (ULONG)(pHeaderBufPtr - pTxBlk->HdrPadLen);
+
+
+
+	//
+	// Insert LLC-SNAP encapsulation - 8 octets
+	//
+	//
+   	// if original Ethernet frame contains no LLC/SNAP,
+	// then an extra LLC/SNAP encap is required
+	//
+	EXTRA_LLCSNAP_ENCAP_FROM_PKT_START(pTxBlk->pSrcBufHeader, pTxBlk->pExtraLlcSnapEncap);
+	if (pTxBlk->pExtraLlcSnapEncap)
+	{
+		UCHAR vlan_size;
+
+		NdisMoveMemory(pHeaderBufPtr, pTxBlk->pExtraLlcSnapEncap, 6);
+		pHeaderBufPtr += 6;
+		// skip vlan tag
+		vlan_size =  (bVLANPkt) ? LENGTH_802_1Q : 0;
+		// get 2 octets (TypeofLen)
+		NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufHeader+12+vlan_size, 2);
+		pHeaderBufPtr += 2;
+		pTxBlk->MpduHeaderLen += LENGTH_802_1_H;
+	}
+
+
+	// If TKIP is used and fragmentation is required. Driver has to
+	//	append TKIP MIC at tail of the scatter buffer
+	//	MAC ASIC will only perform IV/EIV/ICV insertion but no TKIP MIC
+	if (pTxBlk->CipherAlg == CIPHER_TKIP)
+	{
+
+		// NOTE: DON'T refer the skb->len directly after following copy. Becasue the length is not adjust
+		//			to correct lenght, refer to pTxBlk->SrcBufLen for the packet length in following progress.
+		NdisMoveMemory(pTxBlk->pSrcBufData + pTxBlk->SrcBufLen, &pAd->PrivateInfo.Tx.MIC[0], 8);
+		//skb_put((RTPKT_TO_OSPKT(pTxBlk->pPacket))->tail, 8);
+		pTxBlk->SrcBufLen += 8;
+		pTxBlk->TotalFrameLen += 8;
+		pTxBlk->CipherAlg = CIPHER_TKIP_NO_MIC;
+	}
+
+	//
+	// calcuate the overhead bytes that encryption algorithm may add. This
+	// affects the calculate of "duration" field
+	//
+	if ((pTxBlk->CipherAlg == CIPHER_WEP64) || (pTxBlk->CipherAlg == CIPHER_WEP128))
+		EncryptionOverhead = 8; //WEP: IV[4] + ICV[4];
+	else if (pTxBlk->CipherAlg == CIPHER_TKIP_NO_MIC)
+		EncryptionOverhead = 12;//TKIP: IV[4] + EIV[4] + ICV[4], MIC will be added to TotalPacketLength
+	else if (pTxBlk->CipherAlg == CIPHER_TKIP)
+		EncryptionOverhead = 20;//TKIP: IV[4] + EIV[4] + ICV[4] + MIC[8]
+	else if (pTxBlk->CipherAlg == CIPHER_AES)
+		EncryptionOverhead = 16;	// AES: IV[4] + EIV[4] + MIC[8]
+	else
+		EncryptionOverhead = 0;
+
+	// decide how much time an ACK/CTS frame will consume in the air
+	AckDuration = RTMPCalcDuration(pAd, pAd->CommonCfg.ExpectedACKRate[pTxBlk->TxRate], 14);
+
+	// Init the total payload length of this frame.
+	SrcRemainingBytes = pTxBlk->SrcBufLen;
+
+	pTxBlk->TotalFragNum = 0xff;
+
+	do {
+
+		FreeMpduSize = pAd->CommonCfg.FragmentThreshold - LENGTH_CRC;
+
+		FreeMpduSize -= pTxBlk->MpduHeaderLen;
+
+		if (SrcRemainingBytes <= FreeMpduSize)
+		{	// this is the last or only fragment
+
+			pTxBlk->SrcBufLen = SrcRemainingBytes;
+
+			pHeader_802_11->FC.MoreFrag = 0;
+			pHeader_802_11->Duration = pAd->CommonCfg.Dsifs + AckDuration;
+
+			// Indicate the lower layer that this's the last fragment.
+			pTxBlk->TotalFragNum = fragNum;
+		}
+		else
+		{	// more fragment is required
+
+			pTxBlk->SrcBufLen = FreeMpduSize;
+
+			NextMpduSize = min(((UINT)SrcRemainingBytes - pTxBlk->SrcBufLen), ((UINT)pAd->CommonCfg.FragmentThreshold));
+			pHeader_802_11->FC.MoreFrag = 1;
+			pHeader_802_11->Duration = (3 * pAd->CommonCfg.Dsifs) + (2 * AckDuration) + RTMPCalcDuration(pAd, pTxBlk->TxRate, NextMpduSize + EncryptionOverhead);
+		}
+
+		if (fragNum == 0)
+			pTxBlk->FrameGap = IFS_HTTXOP;
+		else
+			pTxBlk->FrameGap = IFS_SIFS;
+
+		RTMPWriteTxWI_Data(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
+
+		HAL_WriteFragTxResource(pAd, pTxBlk, fragNum, &FreeNumber);
+
+		pAd->RalinkCounters.KickTxCount++;
+		pAd->RalinkCounters.OneSecTxDoneCount++;
+
+		// Update the frame number, remaining size of the NDIS packet payload.
+
+		// space for 802.11 header.
+		if (fragNum == 0 && pTxBlk->pExtraLlcSnapEncap)
+			pTxBlk->MpduHeaderLen -= LENGTH_802_1_H;
+
+		fragNum++;
+		SrcRemainingBytes -= pTxBlk->SrcBufLen;
+		pTxBlk->pSrcBufData += pTxBlk->SrcBufLen;
+
+		pHeader_802_11->Frag++;	 // increase Frag #
+
+	}while(SrcRemainingBytes > 0);
+
+	//
+	// Kick out Tx
+	//
+	HAL_KickOutTx(pAd, pTxBlk, pTxBlk->QueIdx);
+}
+
+
+#define RELEASE_FRAMES_OF_TXBLK(_pAd, _pTxBlk, _pQEntry, _Status) 										\
+		while(_pTxBlk->TxPacketList.Head)														\
+		{																						\
+			_pQEntry = RemoveHeadQueue(&_pTxBlk->TxPacketList);									\
+			RELEASE_NDIS_PACKET(_pAd, QUEUE_ENTRY_TO_PACKET(_pQEntry), _Status);	\
+		}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Copy frame from waiting queue into relative ring buffer and set
+	appropriate ASIC register to kick hardware encryption before really
+	sent out to air.
+
+	Arguments:
+		pAd 	Pointer to our adapter
+		PNDIS_PACKET	Pointer to outgoing Ndis frame
+		NumberOfFrag	Number of fragment required
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS STAHardTransmit(
+	IN PRTMP_ADAPTER	pAd,
+	IN TX_BLK 			*pTxBlk,
+	IN	UCHAR			QueIdx)
+{
+	NDIS_PACKET		*pPacket;
+	PQUEUE_ENTRY	pQEntry;
+
+	// ---------------------------------------------
+	// STEP 0. DO SANITY CHECK AND SOME EARLY PREPARATION.
+	// ---------------------------------------------
+	//
+	ASSERT(pTxBlk->TxPacketList.Number);
+	if (pTxBlk->TxPacketList.Head == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("pTxBlk->TotalFrameNum == %ld!\n", pTxBlk->TxPacketList.Number));
+		return NDIS_STATUS_FAILURE;
+	}
+
+	pPacket = QUEUE_ENTRY_TO_PACKET(pTxBlk->TxPacketList.Head);
+
+#if 0 //def CARRIER_DETECTION_SUPPORT // Roger sync Carrier
+		if ((pAd->CommonCfg.CarrierDetect.Enable == TRUE) && (isCarrierDetectExist(pAd) == TRUE))
+	{
+		DBGPRINT(RT_DEBUG_INFO,("STAHardTransmit --> radar detect not in normal mode !!!\n"));
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+		return (NDIS_STATUS_FAILURE);
+	}
+#endif // CARRIER_DETECTION_SUPPORT //
+
+	// ------------------------------------------------------------------
+	// STEP 1. WAKE UP PHY
+	//		outgoing frame always wakeup PHY to prevent frame lost and
+	//		turn off PSM bit to improve performance
+	// ------------------------------------------------------------------
+	// not to change PSM bit, just send this frame out?
+	if ((pAd->StaCfg.Psm == PWR_SAVE) && OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+	{
+	    DBGPRINT_RAW(RT_DEBUG_TRACE, ("AsicForceWakeup At HardTx\n"));
+		AsicForceWakeup(pAd, TRUE);
+	}
+
+	// It should not change PSM bit, when APSD turn on.
+	if ((!(pAd->CommonCfg.bAPSDCapable && pAd->CommonCfg.APEdcaParm.bAPSDCapable) && (pAd->CommonCfg.bAPSDForcePowerSave == FALSE))
+		|| (RTMP_GET_PACKET_EAPOL(pTxBlk->pPacket))
+		|| (RTMP_GET_PACKET_WAI(pTxBlk->pPacket)))
+	{
+		if ((pAd->StaCfg.Psm == PWR_SAVE) &&
+            (pAd->StaCfg.WindowsPowerMode == Ndis802_11PowerModeFast_PSP))
+			MlmeSetPsmBit(pAd, PWR_ACTIVE);
+	}
+
+	switch (pTxBlk->TxFrameType)
+	{
+#ifdef DOT11_N_SUPPORT
+		case TX_AMPDU_FRAME:
+				STA_AMPDU_Frame_Tx(pAd, pTxBlk);
+			break;
+		case TX_AMSDU_FRAME:
+				STA_AMSDU_Frame_Tx(pAd, pTxBlk);
+			break;
+#endif // DOT11_N_SUPPORT //
+		case TX_LEGACY_FRAME:
+				STA_Legacy_Frame_Tx(pAd, pTxBlk);
+			break;
+		case TX_MCAST_FRAME:
+				STA_Legacy_Frame_Tx(pAd, pTxBlk);
+			break;
+		case TX_RALINK_FRAME:
+				STA_ARalink_Frame_Tx(pAd, pTxBlk);
+			break;
+		case TX_FRAG_FRAME:
+				STA_Fragment_Frame_Tx(pAd, pTxBlk);
+			break;
+		default:
+			{
+				// It should not happened!
+				DBGPRINT(RT_DEBUG_ERROR, ("Send a pacekt was not classified!! It should not happen!\n"));
+				while(pTxBlk->TxPacketList.Number)
+				{
+					pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
+					pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
+					if (pPacket)
+						RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+				}
+			}
+			break;
+	}
+
+	return (NDIS_STATUS_SUCCESS);
+
+}
+
+ULONG  HashBytesPolynomial(UCHAR *value, unsigned int len)
+{
+   unsigned char *word = value;
+   unsigned int ret = 0;
+   unsigned int i;
+
+   for(i=0; i < len; i++)
+   {
+	  int mod = i % 32;
+	  ret ^=(unsigned int) (word[i]) << mod;
+	  ret ^=(unsigned int) (word[i]) >> (32 - mod);
+   }
+   return ret;
+}
+
+VOID Sta_Announce_or_Forward_802_3_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	UCHAR			FromWhichBSSID)
+{
+	if (TRUE
+		)
+	{
+		announce_802_3_packet(pAd, pPacket);
+	}
+	else
+	{
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+	}
+}
+
diff --git a/drivers/staging/rt2860/sta/sanity.c b/drivers/staging/rt2860/sta/sanity.c
new file mode 100644
index 00000000000..239872464be
--- /dev/null
+++ b/drivers/staging/rt2860/sta/sanity.c
@@ -0,0 +1,420 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	sanity.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John Chang  2004-09-01      add WMM support
+*/
+#include "../rt_config.h"
+
+extern UCHAR	CISCO_OUI[];
+
+extern UCHAR	WPA_OUI[];
+extern UCHAR	RSN_OUI[];
+extern UCHAR	WME_INFO_ELEM[];
+extern UCHAR	WME_PARM_ELEM[];
+extern UCHAR	Ccx2QosInfo[];
+extern UCHAR	RALINK_OUI[];
+extern UCHAR	BROADCOM_OUI[];
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+ */
+BOOLEAN MlmeStartReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT CHAR Ssid[],
+    OUT UCHAR *pSsidLen)
+{
+    MLME_START_REQ_STRUCT *Info;
+
+    Info = (MLME_START_REQ_STRUCT *)(Msg);
+
+    if (Info->SsidLen > MAX_LEN_OF_SSID)
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("MlmeStartReqSanity fail - wrong SSID length\n"));
+        return FALSE;
+    }
+
+    *pSsidLen = Info->SsidLen;
+    NdisMoveMemory(Ssid, Info->Ssid, *pSsidLen);
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+    IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN PeerAssocRspSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *pMsg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT USHORT *pCapabilityInfo,
+    OUT USHORT *pStatus,
+    OUT USHORT *pAid,
+    OUT UCHAR SupRate[],
+    OUT UCHAR *pSupRateLen,
+    OUT UCHAR ExtRate[],
+    OUT UCHAR *pExtRateLen,
+    OUT HT_CAPABILITY_IE		*pHtCapability,
+    OUT ADD_HT_INFO_IE		*pAddHtInfo,	// AP might use this additional ht info IE
+    OUT UCHAR			*pHtCapabilityLen,
+    OUT UCHAR			*pAddHtInfoLen,
+    OUT UCHAR			*pNewExtChannelOffset,
+    OUT PEDCA_PARM pEdcaParm,
+    OUT UCHAR *pCkipFlag)
+{
+    CHAR          IeType, *Ptr;
+    PFRAME_802_11 pFrame = (PFRAME_802_11)pMsg;
+    PEID_STRUCT   pEid;
+    ULONG         Length = 0;
+
+	*pNewExtChannelOffset = 0xff;
+	*pHtCapabilityLen = 0;
+	*pAddHtInfoLen = 0;
+    COPY_MAC_ADDR(pAddr2, pFrame->Hdr.Addr2);
+    Ptr = pFrame->Octet;
+    Length += LENGTH_802_11;
+
+    NdisMoveMemory(pCapabilityInfo, &pFrame->Octet[0], 2);
+    Length += 2;
+    NdisMoveMemory(pStatus,         &pFrame->Octet[2], 2);
+    Length += 2;
+    *pCkipFlag = 0;
+    *pExtRateLen = 0;
+    pEdcaParm->bValid = FALSE;
+
+    if (*pStatus != MLME_SUCCESS)
+        return TRUE;
+
+    NdisMoveMemory(pAid, &pFrame->Octet[4], 2);
+    Length += 2;
+
+    // Aid already swaped byte order in RTMPFrameEndianChange() for big endian platform
+    *pAid = (*pAid) & 0x3fff; // AID is low 14-bit
+
+    // -- get supported rates from payload and advance the pointer
+    IeType = pFrame->Octet[6];
+    *pSupRateLen = pFrame->Octet[7];
+    if ((IeType != IE_SUPP_RATES) || (*pSupRateLen > MAX_LEN_OF_SUPPORTED_RATES))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("PeerAssocRspSanity fail - wrong SupportedRates IE\n"));
+        return FALSE;
+    }
+    else
+        NdisMoveMemory(SupRate, &pFrame->Octet[8], *pSupRateLen);
+
+    Length = Length + 2 + *pSupRateLen;
+
+    // many AP implement proprietary IEs in non-standard order, we'd better
+    // tolerate mis-ordered IEs to get best compatibility
+    pEid = (PEID_STRUCT) &pFrame->Octet[8 + (*pSupRateLen)];
+
+    // get variable fields from payload and advance the pointer
+    while ((Length + 2 + pEid->Len) <= MsgLen)
+    {
+        switch (pEid->Eid)
+        {
+            case IE_EXT_SUPP_RATES:
+                if (pEid->Len <= MAX_LEN_OF_SUPPORTED_RATES)
+                {
+                    NdisMoveMemory(ExtRate, pEid->Octet, pEid->Len);
+                    *pExtRateLen = pEid->Len;
+                }
+                break;
+
+             case IE_HT_CAP:
+            case IE_HT_CAP2:
+			if (pEid->Len >= SIZE_HT_CAP_IE)  //Note: allow extension.!!
+			{
+				NdisMoveMemory(pHtCapability, pEid->Octet, SIZE_HT_CAP_IE);
+
+				*(USHORT *)(&pHtCapability->HtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->HtCapInfo));
+				*(USHORT *)(&pHtCapability->ExtHtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->ExtHtCapInfo));
+
+				*pHtCapabilityLen = SIZE_HT_CAP_IE;
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_WARN, ("PeerAssocRspSanity - wrong IE_HT_CAP. \n"));
+			}
+
+		break;
+#ifdef DOT11_N_SUPPORT
+            case IE_ADD_HT:
+            case IE_ADD_HT2:
+			if (pEid->Len >= sizeof(ADD_HT_INFO_IE))
+			{
+				// This IE allows extension, but we can ignore extra bytes beyond our knowledge , so only
+				// copy first sizeof(ADD_HT_INFO_IE)
+				NdisMoveMemory(pAddHtInfo, pEid->Octet, sizeof(ADD_HT_INFO_IE));
+
+				*(USHORT *)(&pAddHtInfo->AddHtInfo2) = cpu2le16(*(USHORT *)(&pAddHtInfo->AddHtInfo2));
+				*(USHORT *)(&pAddHtInfo->AddHtInfo3) = cpu2le16(*(USHORT *)(&pAddHtInfo->AddHtInfo3));
+
+				*pAddHtInfoLen = SIZE_ADD_HT_INFO_IE;
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_WARN, ("PeerAssocRspSanity - wrong IE_ADD_HT. \n"));
+			}
+
+		break;
+            case IE_SECONDARY_CH_OFFSET:
+			if (pEid->Len == 1)
+			{
+				*pNewExtChannelOffset = pEid->Octet[0];
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_WARN, ("PeerAssocRspSanity - wrong IE_SECONDARY_CH_OFFSET. \n"));
+			}
+#endif // DOT11_N_SUPPORT //
+		break;
+            case IE_AIRONET_CKIP:
+                // 0. Check Aironet IE length, it must be larger or equal to 28
+                //    Cisco's AP VxWork version(will not be supported) used this IE length as 28
+                //    Cisco's AP IOS version used this IE length as 30
+                if (pEid->Len < (CKIP_NEGOTIATION_LENGTH - 2))
+                break;
+
+                // 1. Copy CKIP flag byte to buffer for process
+                *pCkipFlag = *(pEid->Octet + 8);
+                break;
+
+            case IE_AIRONET_IPADDRESS:
+                if (pEid->Len != 0x0A)
+                break;
+
+                // Get Cisco Aironet IP information
+                if (NdisEqualMemory(pEid->Octet, CISCO_OUI, 3) == 1)
+                    NdisMoveMemory(pAd->StaCfg.AironetIPAddress, pEid->Octet + 4, 4);
+                break;
+
+            // CCX2, WMM use the same IE value
+            // case IE_CCX_V2:
+            case IE_VENDOR_SPECIFIC:
+                // handle WME PARAMTER ELEMENT
+                if (NdisEqualMemory(pEid->Octet, WME_PARM_ELEM, 6) && (pEid->Len == 24))
+                {
+                    PUCHAR ptr;
+                    int i;
+
+                    // parsing EDCA parameters
+                    pEdcaParm->bValid          = TRUE;
+                    pEdcaParm->bQAck           = FALSE; // pEid->Octet[0] & 0x10;
+                    pEdcaParm->bQueueRequest   = FALSE; // pEid->Octet[0] & 0x20;
+                    pEdcaParm->bTxopRequest    = FALSE; // pEid->Octet[0] & 0x40;
+                    //pEdcaParm->bMoreDataAck    = FALSE; // pEid->Octet[0] & 0x80;
+                    pEdcaParm->EdcaUpdateCount = pEid->Octet[6] & 0x0f;
+                    pEdcaParm->bAPSDCapable    = (pEid->Octet[6] & 0x80) ? 1 : 0;
+                    ptr = &pEid->Octet[8];
+                    for (i=0; i<4; i++)
+                    {
+                        UCHAR aci = (*ptr & 0x60) >> 5; // b5~6 is AC INDEX
+                        pEdcaParm->bACM[aci]  = (((*ptr) & 0x10) == 0x10);   // b5 is ACM
+                        pEdcaParm->Aifsn[aci] = (*ptr) & 0x0f;               // b0~3 is AIFSN
+                        pEdcaParm->Cwmin[aci] = *(ptr+1) & 0x0f;             // b0~4 is Cwmin
+                        pEdcaParm->Cwmax[aci] = *(ptr+1) >> 4;               // b5~8 is Cwmax
+                        pEdcaParm->Txop[aci]  = *(ptr+2) + 256 * (*(ptr+3)); // in unit of 32-us
+                        ptr += 4; // point to next AC
+                    }
+                }
+
+                // handle CCX IE
+                else
+                {
+                    // 0. Check the size and CCX admin control
+                    if (pAd->StaCfg.CCXControl.field.Enable == 0)
+                        break;
+                    if (pEid->Len != 5)
+                        break;
+
+                    // Turn CCX2 if matched
+                    if (NdisEqualMemory(pEid->Octet, Ccx2IeInfo, 5) == 1)
+                        pAd->StaCfg.CCXEnable = TRUE;
+                    break;
+                }
+                break;
+
+            default:
+                DBGPRINT(RT_DEBUG_TRACE, ("PeerAssocRspSanity - ignore unrecognized EID = %d\n", pEid->Eid));
+                break;
+        }
+
+        Length = Length + 2 + pEid->Len;
+        pEid = (PEID_STRUCT)((UCHAR*)pEid + 2 + pEid->Len);
+    }
+
+    // Force CCX2 enable to TRUE for those AP didn't replay CCX v2 IE, we still force it to be on
+    if (pAd->StaCfg.CCXControl.field.Enable == 1)
+        pAd->StaCfg.CCXEnable = TRUE;
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN PeerProbeReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT CHAR Ssid[],
+    OUT UCHAR *pSsidLen)
+{
+    UCHAR         Idx;
+    UCHAR	      RateLen;
+    CHAR          IeType;
+    PFRAME_802_11 pFrame = (PFRAME_802_11)Msg;
+
+    COPY_MAC_ADDR(pAddr2, pFrame->Hdr.Addr2);
+
+    if ((pFrame->Octet[0] != IE_SSID) || (pFrame->Octet[1] > MAX_LEN_OF_SSID))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("PeerProbeReqSanity fail - wrong SSID IE(Type=%d,Len=%d)\n",pFrame->Octet[0],pFrame->Octet[1]));
+        return FALSE;
+    }
+
+    *pSsidLen = pFrame->Octet[1];
+    NdisMoveMemory(Ssid, &pFrame->Octet[2], *pSsidLen);
+
+    Idx = *pSsidLen + 2;
+
+    // -- get supported rates from payload and advance the pointer
+    IeType = pFrame->Octet[Idx];
+    RateLen = pFrame->Octet[Idx + 1];
+    if (IeType != IE_SUPP_RATES)
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("PeerProbeReqSanity fail - wrong SupportRates IE(Type=%d,Len=%d)\n",pFrame->Octet[Idx],pFrame->Octet[Idx+1]));
+        return FALSE;
+    }
+    else
+    {
+        if ((pAd->CommonCfg.PhyMode == PHY_11G) && (RateLen < 8))
+            return (FALSE);
+    }
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN GetTimBit(
+    IN CHAR *Ptr,
+    IN USHORT Aid,
+    OUT UCHAR *TimLen,
+    OUT UCHAR *BcastFlag,
+    OUT UCHAR *DtimCount,
+    OUT UCHAR *DtimPeriod,
+    OUT UCHAR *MessageToMe)
+{
+    UCHAR          BitCntl, N1, N2, MyByte, MyBit;
+    CHAR          *IdxPtr;
+
+    IdxPtr = Ptr;
+
+    IdxPtr ++;
+    *TimLen = *IdxPtr;
+
+    // get DTIM Count from TIM element
+    IdxPtr ++;
+    *DtimCount = *IdxPtr;
+
+    // get DTIM Period from TIM element
+    IdxPtr++;
+    *DtimPeriod = *IdxPtr;
+
+    // get Bitmap Control from TIM element
+    IdxPtr++;
+    BitCntl = *IdxPtr;
+
+    if ((*DtimCount == 0) && (BitCntl & 0x01))
+        *BcastFlag = TRUE;
+    else
+        *BcastFlag = FALSE;
+
+    // Parse Partial Virtual Bitmap from TIM element
+    N1 = BitCntl & 0xfe;    // N1 is the first bitmap byte#
+    N2 = *TimLen - 4 + N1;  // N2 is the last bitmap byte#
+
+    if ((Aid < (N1 << 3)) || (Aid >= ((N2 + 1) << 3)))
+        *MessageToMe = FALSE;
+    else
+    {
+        MyByte = (Aid >> 3) - N1;                       // my byte position in the bitmap byte-stream
+        MyBit = Aid % 16 - ((MyByte & 0x01)? 8:0);
+
+        IdxPtr += (MyByte + 1);
+
+        //if (*IdxPtr)
+        //    DBGPRINT(RT_DEBUG_WARN, ("TIM bitmap = 0x%02x\n", *IdxPtr));
+
+        if (*IdxPtr & (0x01 << MyBit))
+            *MessageToMe = TRUE;
+        else
+            *MessageToMe = FALSE;
+    }
+
+    return TRUE;
+}
+
diff --git a/drivers/staging/rt2860/sta/sync.c b/drivers/staging/rt2860/sta/sync.c
new file mode 100644
index 00000000000..d196f85395c
--- /dev/null
+++ b/drivers/staging/rt2860/sta/sync.c
@@ -0,0 +1,1959 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	sync.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John Chang	2004-09-01      modified for rt2561/2661
+	Jan Lee		2006-08-01      modified for rt2860 for 802.11n
+*/
+#include "../rt_config.h"
+
+#define AC0_DEF_TXOP		0
+#define AC1_DEF_TXOP		0
+#define AC2_DEF_TXOP		94
+#define AC3_DEF_TXOP		47
+
+VOID	AdhocTurnOnQos(
+	IN  PRTMP_ADAPTER pAd)
+{
+	// Turn on QOs if use HT rate.
+	if (pAd->CommonCfg.APEdcaParm.bValid == FALSE)
+	{
+		pAd->CommonCfg.APEdcaParm.bValid = TRUE;
+		pAd->CommonCfg.APEdcaParm.Aifsn[0] = 3;
+		pAd->CommonCfg.APEdcaParm.Aifsn[1] = 7;
+		pAd->CommonCfg.APEdcaParm.Aifsn[2] = 1;
+		pAd->CommonCfg.APEdcaParm.Aifsn[3] = 1;
+
+		pAd->CommonCfg.APEdcaParm.Cwmin[0] = 4;
+		pAd->CommonCfg.APEdcaParm.Cwmin[1] = 4;
+		pAd->CommonCfg.APEdcaParm.Cwmin[2] = 3;
+		pAd->CommonCfg.APEdcaParm.Cwmin[3] = 2;
+
+		pAd->CommonCfg.APEdcaParm.Cwmax[0] = 10;
+		pAd->CommonCfg.APEdcaParm.Cwmax[1] = 6;
+		pAd->CommonCfg.APEdcaParm.Cwmax[2] = 4;
+		pAd->CommonCfg.APEdcaParm.Cwmax[3] = 3;
+
+		pAd->CommonCfg.APEdcaParm.Txop[0]  = 0;
+		pAd->CommonCfg.APEdcaParm.Txop[1]  = 0;
+		pAd->CommonCfg.APEdcaParm.Txop[2]  = AC2_DEF_TXOP;
+		pAd->CommonCfg.APEdcaParm.Txop[3]  = AC3_DEF_TXOP;
+	}
+	AsicSetEdcaParm(pAd, &pAd->CommonCfg.APEdcaParm);
+}
+
+/*
+	==========================================================================
+	Description:
+		The sync state machine,
+	Parameters:
+		Sm - pointer to the state machine
+	Note:
+		the state machine looks like the following
+
+	==========================================================================
+ */
+VOID SyncStateMachineInit(
+	IN PRTMP_ADAPTER pAd,
+	IN STATE_MACHINE *Sm,
+	OUT STATE_MACHINE_FUNC Trans[])
+{
+	StateMachineInit(Sm, Trans, MAX_SYNC_STATE, MAX_SYNC_MSG, (STATE_MACHINE_FUNC)Drop, SYNC_IDLE, SYNC_MACHINE_BASE);
+
+	// column 1
+	StateMachineSetAction(Sm, SYNC_IDLE, MT2_MLME_SCAN_REQ, (STATE_MACHINE_FUNC)MlmeScanReqAction);
+	StateMachineSetAction(Sm, SYNC_IDLE, MT2_MLME_JOIN_REQ, (STATE_MACHINE_FUNC)MlmeJoinReqAction);
+	StateMachineSetAction(Sm, SYNC_IDLE, MT2_MLME_START_REQ, (STATE_MACHINE_FUNC)MlmeStartReqAction);
+	StateMachineSetAction(Sm, SYNC_IDLE, MT2_PEER_BEACON, (STATE_MACHINE_FUNC)PeerBeacon);
+	StateMachineSetAction(Sm, SYNC_IDLE, MT2_PEER_PROBE_REQ, (STATE_MACHINE_FUNC)PeerProbeReqAction);
+
+	//column 2
+	StateMachineSetAction(Sm, JOIN_WAIT_BEACON, MT2_MLME_SCAN_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenScan);
+	StateMachineSetAction(Sm, JOIN_WAIT_BEACON, MT2_MLME_JOIN_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenJoin);
+	StateMachineSetAction(Sm, JOIN_WAIT_BEACON, MT2_MLME_START_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenStart);
+	StateMachineSetAction(Sm, JOIN_WAIT_BEACON, MT2_PEER_BEACON, (STATE_MACHINE_FUNC)PeerBeaconAtJoinAction);
+	StateMachineSetAction(Sm, JOIN_WAIT_BEACON, MT2_BEACON_TIMEOUT, (STATE_MACHINE_FUNC)BeaconTimeoutAtJoinAction);
+
+	// column 3
+	StateMachineSetAction(Sm, SCAN_LISTEN, MT2_MLME_SCAN_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenScan);
+	StateMachineSetAction(Sm, SCAN_LISTEN, MT2_MLME_JOIN_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenJoin);
+	StateMachineSetAction(Sm, SCAN_LISTEN, MT2_MLME_START_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenStart);
+	StateMachineSetAction(Sm, SCAN_LISTEN, MT2_PEER_BEACON, (STATE_MACHINE_FUNC)PeerBeaconAtScanAction);
+	StateMachineSetAction(Sm, SCAN_LISTEN, MT2_PEER_PROBE_RSP, (STATE_MACHINE_FUNC)PeerBeaconAtScanAction);
+	StateMachineSetAction(Sm, SCAN_LISTEN, MT2_SCAN_TIMEOUT, (STATE_MACHINE_FUNC)ScanTimeoutAction);
+
+	// timer init
+	RTMPInitTimer(pAd, &pAd->MlmeAux.BeaconTimer, GET_TIMER_FUNCTION(BeaconTimeout), pAd, FALSE);
+	RTMPInitTimer(pAd, &pAd->MlmeAux.ScanTimer, GET_TIMER_FUNCTION(ScanTimeout), pAd, FALSE);
+}
+
+/*
+	==========================================================================
+	Description:
+		Beacon timeout handler, executed in timer thread
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID BeaconTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+	RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+
+	DBGPRINT(RT_DEBUG_TRACE,("SYNC - BeaconTimeout\n"));
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS))
+		return;
+
+#ifdef DOT11_N_SUPPORT
+	if ((pAd->CommonCfg.BBPCurrentBW == BW_40)
+		)
+	{
+		UCHAR        BBPValue = 0;
+		AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+		AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+		BBPValue &= (~0x18);
+		BBPValue |= 0x10;
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+		DBGPRINT(RT_DEBUG_TRACE, ("SYNC - End of SCAN, restore to 40MHz channel %d, Total BSS[%02d]\n",pAd->CommonCfg.CentralChannel, pAd->ScanTab.BssNr));
+	}
+#endif // DOT11_N_SUPPORT //
+
+	MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_BEACON_TIMEOUT, 0, NULL);
+	RT28XX_MLME_HANDLER(pAd);
+}
+
+/*
+	==========================================================================
+	Description:
+		Scan timeout handler, executed in timer thread
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID ScanTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+	RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS))
+		return;
+
+	if (MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_SCAN_TIMEOUT, 0, NULL))
+	{
+		RT28XX_MLME_HANDLER(pAd);
+	}
+	else
+	{
+		// To prevent SyncMachine.CurrState is SCAN_LISTEN forever.
+		pAd->MlmeAux.Channel = 0;
+		ScanNextChannel(pAd);
+		if (pAd->CommonCfg.bWirelessEvent)
+		{
+			RTMPSendWirelessEvent(pAd, IW_SCAN_ENQUEUE_FAIL_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+		MLME SCAN req state machine procedure
+	==========================================================================
+ */
+VOID MlmeScanReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR          Ssid[MAX_LEN_OF_SSID], SsidLen, ScanType, BssType, BBPValue = 0;
+	BOOLEAN        TimerCancelled;
+	ULONG		   Now;
+	USHORT         Status;
+	PHEADER_802_11 pHdr80211;
+	PUCHAR         pOutBuffer = NULL;
+	NDIS_STATUS    NStatus;
+
+	// Check the total scan tries for one single OID command
+	// If this is the CCX 2.0 Case, skip that!
+	if ( !RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_START_UP))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("SYNC - MlmeScanReqAction before Startup\n"));
+		return;
+	}
+
+	// Increase the scan retry counters.
+	pAd->StaCfg.ScanCnt++;
+
+#ifdef RT2860
+    if ((OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE)) &&
+        (IDLE_ON(pAd)) &&
+		(pAd->StaCfg.bRadio == TRUE) &&
+		(RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_IDLE_RADIO_OFF)))
+	{
+		RT28xxPciAsicRadioOn(pAd, GUI_IDLE_POWER_SAVE);
+	}
+#endif // RT2860 //
+
+	// first check the parameter sanity
+	if (MlmeScanReqSanity(pAd,
+						  Elem->Msg,
+						  Elem->MsgLen,
+						  &BssType,
+						  Ssid,
+						  &SsidLen,
+						  &ScanType))
+	{
+
+		// Check for channel load and noise hist request
+		// Suspend MSDU only at scan request, not the last two mentioned
+		if ((ScanType == SCAN_CISCO_NOISE) || (ScanType == SCAN_CISCO_CHANNEL_LOAD))
+		{
+			if (pAd->StaCfg.CCXScanChannel != pAd->CommonCfg.Channel)
+				RTMPSuspendMsduTransmission(pAd);			// Suspend MSDU transmission here
+		}
+		else
+		{
+			// Suspend MSDU transmission here
+			RTMPSuspendMsduTransmission(pAd);
+		}
+
+		//
+		// To prevent data lost.
+		// Send an NULL data with turned PSM bit on to current associated AP before SCAN progress.
+		// And should send an NULL data with turned PSM bit off to AP, when scan progress done
+		//
+		if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED) && (INFRA_ON(pAd)))
+		{
+			NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);
+			if (NStatus	== NDIS_STATUS_SUCCESS)
+			{
+				pHdr80211 = (PHEADER_802_11) pOutBuffer;
+				MgtMacHeaderInit(pAd, pHdr80211, SUBTYPE_NULL_FUNC, 1, pAd->CommonCfg.Bssid, pAd->CommonCfg.Bssid);
+				pHdr80211->Duration = 0;
+				pHdr80211->FC.Type = BTYPE_DATA;
+				pHdr80211->FC.PwrMgmt = PWR_SAVE;
+
+				// Send using priority queue
+				MiniportMMRequest(pAd, 0, pOutBuffer, sizeof(HEADER_802_11));
+				DBGPRINT(RT_DEBUG_TRACE, ("MlmeScanReqAction -- Send PSM Data frame for off channel RM\n"));
+				MlmeFreeMemory(pAd, pOutBuffer);
+				RTMPusecDelay(5000);
+			}
+		}
+
+		NdisGetSystemUpTime(&Now);
+		pAd->StaCfg.LastScanTime = Now;
+		// reset all the timers
+		RTMPCancelTimer(&pAd->MlmeAux.BeaconTimer, &TimerCancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.ScanTimer, &TimerCancelled);
+
+		// record desired BSS parameters
+		pAd->MlmeAux.BssType = BssType;
+		pAd->MlmeAux.ScanType = ScanType;
+		pAd->MlmeAux.SsidLen = SsidLen;
+        NdisZeroMemory(pAd->MlmeAux.Ssid, MAX_LEN_OF_SSID);
+		NdisMoveMemory(pAd->MlmeAux.Ssid, Ssid, SsidLen);
+
+		// start from the first channel
+		pAd->MlmeAux.Channel = FirstChannel(pAd);
+
+		// Change the scan channel when dealing with CCX beacon report
+		if ((ScanType == SCAN_CISCO_PASSIVE) || (ScanType == SCAN_CISCO_ACTIVE) ||
+			(ScanType == SCAN_CISCO_CHANNEL_LOAD) || (ScanType == SCAN_CISCO_NOISE))
+			pAd->MlmeAux.Channel = pAd->StaCfg.CCXScanChannel;
+
+		// Let BBP register at 20MHz to do scan
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+		BBPValue &= (~0x18);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+		DBGPRINT(RT_DEBUG_TRACE, ("SYNC - BBP R4 to 20MHz.l\n"));
+		ScanNextChannel(pAd);
+	}
+	else
+	{
+		DBGPRINT_ERR(("SYNC - MlmeScanReqAction() sanity check fail\n"));
+		pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+		Status = MLME_INVALID_FORMAT;
+		MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_SCAN_CONF, 2, &Status);
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+		MLME JOIN req state machine procedure
+	==========================================================================
+ */
+VOID MlmeJoinReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR        BBPValue = 0;
+	BSS_ENTRY    *pBss;
+	BOOLEAN       TimerCancelled;
+	HEADER_802_11 Hdr80211;
+	NDIS_STATUS   NStatus;
+	ULONG         FrameLen = 0;
+	PUCHAR        pOutBuffer = NULL;
+	PUCHAR        pSupRate = NULL;
+	UCHAR         SupRateLen;
+	PUCHAR        pExtRate = NULL;
+	UCHAR         ExtRateLen;
+	UCHAR         ASupRate[] = {0x8C, 0x12, 0x98, 0x24, 0xb0, 0x48, 0x60, 0x6C};
+	UCHAR         ASupRateLen = sizeof(ASupRate)/sizeof(UCHAR);
+	MLME_JOIN_REQ_STRUCT *pInfo = (MLME_JOIN_REQ_STRUCT *)(Elem->Msg);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("SYNC - MlmeJoinReqAction(BSS #%ld)\n", pInfo->BssIdx));
+
+#ifdef RT2860
+    if ((OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE)) &&
+        (IDLE_ON(pAd)) &&
+		(pAd->StaCfg.bRadio == TRUE) &&
+		(RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_IDLE_RADIO_OFF)))
+	{
+		RT28xxPciAsicRadioOn(pAd, GUI_IDLE_POWER_SAVE);
+	}
+#endif // RT2860 //
+
+	// reset all the timers
+	RTMPCancelTimer(&pAd->MlmeAux.ScanTimer, &TimerCancelled);
+	RTMPCancelTimer(&pAd->MlmeAux.BeaconTimer, &TimerCancelled);
+
+	pBss = &pAd->MlmeAux.SsidBssTab.BssEntry[pInfo->BssIdx];
+
+	// record the desired SSID & BSSID we're waiting for
+	COPY_MAC_ADDR(pAd->MlmeAux.Bssid, pBss->Bssid);
+
+	// If AP's SSID is not hidden, it is OK for updating ssid to MlmeAux again.
+	if (pBss->Hidden == 0)
+	{
+		NdisMoveMemory(pAd->MlmeAux.Ssid, pBss->Ssid, pBss->SsidLen);
+		pAd->MlmeAux.SsidLen = pBss->SsidLen;
+	}
+
+	pAd->MlmeAux.BssType = pBss->BssType;
+	pAd->MlmeAux.Channel = pBss->Channel;
+	pAd->MlmeAux.CentralChannel = pBss->CentralChannel;
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+	// Country IE of the AP will be evaluated and will be used.
+	if ((pAd->StaCfg.IEEE80211dClientMode != Rt802_11_D_None) &&
+		(pBss->bHasCountryIE == TRUE))
+	{
+		NdisMoveMemory(&pAd->CommonCfg.CountryCode[0], &pBss->CountryString[0], 2);
+		if (pBss->CountryString[2] == 'I')
+			pAd->CommonCfg.Geography = IDOR;
+		else if (pBss->CountryString[2] == 'O')
+			pAd->CommonCfg.Geography = ODOR;
+		else
+			pAd->CommonCfg.Geography = BOTH;
+		BuildChannelListEx(pAd);
+	}
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+	// Let BBP register at 20MHz to do scan
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+	BBPValue &= (~0x18);
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+	DBGPRINT(RT_DEBUG_TRACE, ("SYNC - BBP R4 to 20MHz.l\n"));
+
+	// switch channel and waiting for beacon timer
+	AsicSwitchChannel(pAd, pAd->MlmeAux.Channel, FALSE);
+	AsicLockChannel(pAd, pAd->MlmeAux.Channel);
+	RTMPSetTimer(&pAd->MlmeAux.BeaconTimer, JOIN_TIMEOUT);
+
+    do
+	{
+		if (((pAd->CommonCfg.bIEEE80211H == 1) &&
+            (pAd->MlmeAux.Channel > 14) &&
+             RadarChannelCheck(pAd, pAd->MlmeAux.Channel))
+#ifdef CARRIER_DETECTION_SUPPORT // Roger sync Carrier
+             || (pAd->CommonCfg.CarrierDetect.Enable == TRUE)
+#endif // CARRIER_DETECTION_SUPPORT //
+            )
+		{
+			//
+			// We can't send any Probe request frame to meet 802.11h.
+			//
+			if (pBss->Hidden == 0)
+				break;
+		}
+
+		//
+		// send probe request
+		//
+		NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);
+		if (NStatus == NDIS_STATUS_SUCCESS)
+		{
+			if (pAd->MlmeAux.Channel <= 14)
+			{
+				pSupRate = pAd->CommonCfg.SupRate;
+				SupRateLen = pAd->CommonCfg.SupRateLen;
+				pExtRate = pAd->CommonCfg.ExtRate;
+				ExtRateLen = pAd->CommonCfg.ExtRateLen;
+			}
+			else
+			{
+				//
+				// Overwrite Support Rate, CCK rate are not allowed
+				//
+				pSupRate = ASupRate;
+				SupRateLen = ASupRateLen;
+				ExtRateLen = 0;
+			}
+
+			if (pAd->MlmeAux.BssType == BSS_INFRA)
+				MgtMacHeaderInit(pAd, &Hdr80211, SUBTYPE_PROBE_REQ, 0, pAd->MlmeAux.Bssid, pAd->MlmeAux.Bssid);
+			else
+				MgtMacHeaderInit(pAd, &Hdr80211, SUBTYPE_PROBE_REQ, 0, BROADCAST_ADDR, BROADCAST_ADDR);
+
+			MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+							  sizeof(HEADER_802_11),    &Hdr80211,
+							  1,                        &SsidIe,
+							  1,                        &pAd->MlmeAux.SsidLen,
+							  pAd->MlmeAux.SsidLen,	    pAd->MlmeAux.Ssid,
+							  1,                        &SupRateIe,
+							  1,                        &SupRateLen,
+							  SupRateLen,               pSupRate,
+							  END_OF_ARGS);
+
+			if (ExtRateLen)
+			{
+				ULONG Tmp;
+				MakeOutgoingFrame(pOutBuffer + FrameLen,            &Tmp,
+								  1,                                &ExtRateIe,
+								  1,                                &ExtRateLen,
+								  ExtRateLen,                       pExtRate,
+								  END_OF_ARGS);
+				FrameLen += Tmp;
+			}
+
+
+			MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+			MlmeFreeMemory(pAd, pOutBuffer);
+		}
+    } while (FALSE);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("SYNC - Switch to ch %d, Wait BEACON from %02x:%02x:%02x:%02x:%02x:%02x\n",
+		pBss->Channel, pBss->Bssid[0], pBss->Bssid[1], pBss->Bssid[2], pBss->Bssid[3], pBss->Bssid[4], pBss->Bssid[5]));
+
+	pAd->Mlme.SyncMachine.CurrState = JOIN_WAIT_BEACON;
+}
+
+/*
+	==========================================================================
+	Description:
+		MLME START Request state machine procedure, starting an IBSS
+	==========================================================================
+ */
+VOID MlmeStartReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR         Ssid[MAX_LEN_OF_SSID], SsidLen;
+	BOOLEAN       TimerCancelled;
+
+	// New for WPA security suites
+	UCHAR						VarIE[MAX_VIE_LEN]; 	// Total VIE length = MAX_VIE_LEN - -5
+	NDIS_802_11_VARIABLE_IEs	*pVIE = NULL;
+	LARGE_INTEGER				TimeStamp;
+	BOOLEAN Privacy;
+	USHORT Status;
+
+	// Init Variable IE structure
+	pVIE = (PNDIS_802_11_VARIABLE_IEs) VarIE;
+	pVIE->Length = 0;
+	TimeStamp.u.LowPart  = 0;
+	TimeStamp.u.HighPart = 0;
+
+	if (MlmeStartReqSanity(pAd, Elem->Msg, Elem->MsgLen, Ssid, &SsidLen))
+	{
+		// reset all the timers
+		RTMPCancelTimer(&pAd->MlmeAux.ScanTimer, &TimerCancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.BeaconTimer, &TimerCancelled);
+
+		//
+		// Start a new IBSS. All IBSS parameters are decided now....
+		//
+		DBGPRINT(RT_DEBUG_TRACE, ("MlmeStartReqAction - Start a new IBSS. All IBSS parameters are decided now.... \n"));
+		pAd->MlmeAux.BssType           = BSS_ADHOC;
+		NdisMoveMemory(pAd->MlmeAux.Ssid, Ssid, SsidLen);
+		pAd->MlmeAux.SsidLen           = SsidLen;
+
+		// generate a radom number as BSSID
+		MacAddrRandomBssid(pAd, pAd->MlmeAux.Bssid);
+		DBGPRINT(RT_DEBUG_TRACE, ("MlmeStartReqAction - generate a radom number as BSSID \n"));
+
+		Privacy = (pAd->StaCfg.WepStatus == Ndis802_11Encryption1Enabled) ||
+				  (pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) ||
+				  (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled);
+		pAd->MlmeAux.CapabilityInfo    = CAP_GENERATE(0,1,Privacy, (pAd->CommonCfg.TxPreamble == Rt802_11PreambleShort), 1, 0);
+		pAd->MlmeAux.BeaconPeriod      = pAd->CommonCfg.BeaconPeriod;
+		pAd->MlmeAux.AtimWin           = pAd->StaCfg.AtimWin;
+		pAd->MlmeAux.Channel           = pAd->CommonCfg.Channel;
+
+		pAd->CommonCfg.CentralChannel  = pAd->CommonCfg.Channel;
+		pAd->MlmeAux.CentralChannel    = pAd->CommonCfg.CentralChannel;
+
+		pAd->MlmeAux.SupRateLen= pAd->CommonCfg.SupRateLen;
+		NdisMoveMemory(pAd->MlmeAux.SupRate, pAd->CommonCfg.SupRate, MAX_LEN_OF_SUPPORTED_RATES);
+		RTMPCheckRates(pAd, pAd->MlmeAux.SupRate, &pAd->MlmeAux.SupRateLen);
+		pAd->MlmeAux.ExtRateLen = pAd->CommonCfg.ExtRateLen;
+		NdisMoveMemory(pAd->MlmeAux.ExtRate, pAd->CommonCfg.ExtRate, MAX_LEN_OF_SUPPORTED_RATES);
+		RTMPCheckRates(pAd, pAd->MlmeAux.ExtRate, &pAd->MlmeAux.ExtRateLen);
+#ifdef DOT11_N_SUPPORT
+		if (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED)
+		{
+			RTMPUpdateHTIE(&pAd->CommonCfg.DesiredHtPhy, &pAd->StaCfg.DesiredHtPhyInfo.MCSSet[0], &pAd->MlmeAux.HtCapability, &pAd->MlmeAux.AddHtInfo);
+			pAd->MlmeAux.HtCapabilityLen = sizeof(HT_CAPABILITY_IE);
+			// Not turn pAd->StaActive.SupportedHtPhy.bHtEnable = TRUE here.
+			DBGPRINT(RT_DEBUG_TRACE, ("SYNC -pAd->StaActive.SupportedHtPhy.bHtEnable = TRUE\n"));
+		}
+		else
+#endif // DOT11_N_SUPPORT //
+		{
+			pAd->MlmeAux.HtCapabilityLen = 0;
+			pAd->StaActive.SupportedPhyInfo.bHtEnable = FALSE;
+		}
+		// temporarily not support QOS in IBSS
+		NdisZeroMemory(&pAd->MlmeAux.APEdcaParm, sizeof(EDCA_PARM));
+		NdisZeroMemory(&pAd->MlmeAux.APQbssLoad, sizeof(QBSS_LOAD_PARM));
+		NdisZeroMemory(&pAd->MlmeAux.APQosCapability, sizeof(QOS_CAPABILITY_PARM));
+
+		AsicSwitchChannel(pAd, pAd->MlmeAux.Channel, FALSE);
+		AsicLockChannel(pAd, pAd->MlmeAux.Channel);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("SYNC - MlmeStartReqAction(ch= %d,sup rates= %d, ext rates=%d)\n",
+			pAd->MlmeAux.Channel, pAd->MlmeAux.SupRateLen, pAd->MlmeAux.ExtRateLen));
+
+		pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+		Status = MLME_SUCCESS;
+		MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_START_CONF, 2, &Status);
+	}
+	else
+	{
+		DBGPRINT_ERR(("SYNC - MlmeStartReqAction() sanity check fail.\n"));
+		pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+		Status = MLME_INVALID_FORMAT;
+		MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_START_CONF, 2, &Status);
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+		peer sends beacon back when scanning
+	==========================================================================
+ */
+VOID PeerBeaconAtScanAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR           Bssid[MAC_ADDR_LEN], Addr2[MAC_ADDR_LEN];
+	UCHAR           Ssid[MAX_LEN_OF_SSID], BssType, Channel, NewChannel,
+					SsidLen, DtimCount, DtimPeriod, BcastFlag, MessageToMe;
+	CF_PARM         CfParm;
+	USHORT          BeaconPeriod, AtimWin, CapabilityInfo;
+	PFRAME_802_11   pFrame;
+	LARGE_INTEGER   TimeStamp;
+	UCHAR           Erp;
+	UCHAR         	SupRate[MAX_LEN_OF_SUPPORTED_RATES], ExtRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR		  	SupRateLen, ExtRateLen;
+	USHORT 			LenVIE;
+	UCHAR			CkipFlag;
+	UCHAR			AironetCellPowerLimit;
+	EDCA_PARM       EdcaParm;
+	QBSS_LOAD_PARM  QbssLoad;
+	QOS_CAPABILITY_PARM QosCapability;
+	ULONG						RalinkIe;
+	UCHAR						VarIE[MAX_VIE_LEN];		// Total VIE length = MAX_VIE_LEN - -5
+	NDIS_802_11_VARIABLE_IEs	*pVIE = NULL;
+	HT_CAPABILITY_IE		HtCapability;
+	ADD_HT_INFO_IE		AddHtInfo;	// AP might use this additional ht info IE
+	UCHAR			HtCapabilityLen = 0, PreNHtCapabilityLen = 0;
+	UCHAR			AddHtInfoLen;
+	UCHAR			NewExtChannelOffset = 0xff;
+
+	pFrame = (PFRAME_802_11) Elem->Msg;
+	// Init Variable IE structure
+	pVIE = (PNDIS_802_11_VARIABLE_IEs) VarIE;
+	pVIE->Length = 0;
+#ifdef DOT11_N_SUPPORT
+    RTMPZeroMemory(&HtCapability, sizeof(HtCapability));
+	RTMPZeroMemory(&AddHtInfo, sizeof(ADD_HT_INFO_IE));
+#endif // DOT11_N_SUPPORT //
+
+	if (PeerBeaconAndProbeRspSanity(pAd,
+								Elem->Msg,
+								Elem->MsgLen,
+								Elem->Channel,
+								Addr2,
+								Bssid,
+								Ssid,
+								&SsidLen,
+								&BssType,
+								&BeaconPeriod,
+								&Channel,
+								&NewChannel,
+								&TimeStamp,
+								&CfParm,
+								&AtimWin,
+								&CapabilityInfo,
+								&Erp,
+								&DtimCount,
+								&DtimPeriod,
+								&BcastFlag,
+								&MessageToMe,
+								SupRate,
+								&SupRateLen,
+								ExtRate,
+								&ExtRateLen,
+								&CkipFlag,
+								&AironetCellPowerLimit,
+								&EdcaParm,
+								&QbssLoad,
+								&QosCapability,
+								&RalinkIe,
+								&HtCapabilityLen,
+								&PreNHtCapabilityLen,
+								&HtCapability,
+								&AddHtInfoLen,
+								&AddHtInfo,
+								&NewExtChannelOffset,
+								&LenVIE,
+								pVIE))
+	{
+		ULONG Idx;
+		CHAR Rssi = 0;
+
+		Idx = BssTableSearch(&pAd->ScanTab, Bssid, Channel);
+		if (Idx != BSS_NOT_FOUND)
+			Rssi = pAd->ScanTab.BssEntry[Idx].Rssi;
+
+		Rssi = RTMPMaxRssi(pAd, ConvertToRssi(pAd, Elem->Rssi0, RSSI_0), ConvertToRssi(pAd, Elem->Rssi1, RSSI_1), ConvertToRssi(pAd, Elem->Rssi2, RSSI_2));
+
+
+#ifdef DOT11_N_SUPPORT
+		if ((HtCapabilityLen > 0) || (PreNHtCapabilityLen > 0))
+			HtCapabilityLen = SIZE_HT_CAP_IE;
+#endif // DOT11_N_SUPPORT //
+		if ((pAd->StaCfg.CCXReqType != MSRN_TYPE_UNUSED) && (Channel == pAd->StaCfg.CCXScanChannel))
+		{
+			Idx = BssTableSetEntry(pAd, &pAd->StaCfg.CCXBssTab, Bssid, Ssid, SsidLen, BssType, BeaconPeriod,
+						 &CfParm, AtimWin, CapabilityInfo, SupRate, SupRateLen,ExtRate, ExtRateLen, &HtCapability,
+						 &AddHtInfo, HtCapabilityLen, AddHtInfoLen, NewExtChannelOffset, Channel, Rssi, TimeStamp, CkipFlag,
+						 &EdcaParm, &QosCapability, &QbssLoad, LenVIE, pVIE);
+			if (Idx != BSS_NOT_FOUND)
+			{
+				NdisMoveMemory(pAd->StaCfg.CCXBssTab.BssEntry[Idx].PTSF, &Elem->Msg[24], 4);
+				NdisMoveMemory(&pAd->StaCfg.CCXBssTab.BssEntry[Idx].TTSF[0], &Elem->TimeStamp.u.LowPart, 4);
+				NdisMoveMemory(&pAd->StaCfg.CCXBssTab.BssEntry[Idx].TTSF[4], &Elem->TimeStamp.u.LowPart, 4);
+				if (pAd->StaCfg.CCXReqType == MSRN_TYPE_BEACON_REQ)
+					AironetAddBeaconReport(pAd, Idx, Elem);
+			}
+		}
+		else
+		{
+			Idx = BssTableSetEntry(pAd, &pAd->ScanTab, Bssid, Ssid, SsidLen, BssType, BeaconPeriod,
+						  &CfParm, AtimWin, CapabilityInfo, SupRate, SupRateLen, ExtRate, ExtRateLen,  &HtCapability,
+						 &AddHtInfo, HtCapabilityLen, AddHtInfoLen, NewExtChannelOffset, Channel, Rssi, TimeStamp, CkipFlag,
+						 &EdcaParm, &QosCapability, &QbssLoad, LenVIE, pVIE);
+#ifdef DOT11_N_SUPPORT
+#ifdef DOT11N_DRAFT3
+			if (pAd->ChannelList[pAd->CommonCfg.ChannelListIdx].bEffectedChannel == TRUE)
+			{
+				UCHAR		RegClass;
+				PeerBeaconAndProbeRspSanity2(pAd, Elem->Msg, Elem->MsgLen, &RegClass);
+				TriEventTableSetEntry(pAd, &pAd->CommonCfg.TriggerEventTab, Bssid, &HtCapability, HtCapabilityLen, RegClass, Channel);
+			}
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+			if (Idx != BSS_NOT_FOUND)
+			{
+				NdisMoveMemory(pAd->ScanTab.BssEntry[Idx].PTSF, &Elem->Msg[24], 4);
+				NdisMoveMemory(&pAd->ScanTab.BssEntry[Idx].TTSF[0], &Elem->TimeStamp.u.LowPart, 4);
+				NdisMoveMemory(&pAd->ScanTab.BssEntry[Idx].TTSF[4], &Elem->TimeStamp.u.LowPart, 4);
+			}
+		}
+	}
+	// sanity check fail, ignored
+}
+
+/*
+	==========================================================================
+	Description:
+		When waiting joining the (I)BSS, beacon received from external
+	==========================================================================
+ */
+VOID PeerBeaconAtJoinAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR         Bssid[MAC_ADDR_LEN], Addr2[MAC_ADDR_LEN];
+	UCHAR         Ssid[MAX_LEN_OF_SSID], SsidLen, BssType, Channel, MessageToMe,
+				  DtimCount, DtimPeriod, BcastFlag, NewChannel;
+	LARGE_INTEGER TimeStamp;
+	USHORT        BeaconPeriod, AtimWin, CapabilityInfo;
+	CF_PARM       Cf;
+	BOOLEAN       TimerCancelled;
+	UCHAR         Erp;
+	UCHAR         SupRate[MAX_LEN_OF_SUPPORTED_RATES], ExtRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR		  SupRateLen, ExtRateLen;
+	UCHAR         CkipFlag;
+	USHORT 		  LenVIE;
+	UCHAR		  AironetCellPowerLimit;
+	EDCA_PARM       EdcaParm;
+	QBSS_LOAD_PARM  QbssLoad;
+	QOS_CAPABILITY_PARM QosCapability;
+	USHORT        Status;
+	UCHAR						VarIE[MAX_VIE_LEN];		// Total VIE length = MAX_VIE_LEN - -5
+	NDIS_802_11_VARIABLE_IEs	*pVIE = NULL;
+	ULONG           RalinkIe;
+	ULONG         Idx;
+	HT_CAPABILITY_IE		HtCapability;
+	ADD_HT_INFO_IE		AddHtInfo;	// AP might use this additional ht info IE
+	UCHAR				HtCapabilityLen = 0, PreNHtCapabilityLen = 0;
+	UCHAR			AddHtInfoLen;
+	UCHAR			NewExtChannelOffset = 0xff;
+#ifdef DOT11_N_SUPPORT
+	UCHAR			CentralChannel;
+#endif // DOT11_N_SUPPORT //
+
+	// Init Variable IE structure
+	pVIE = (PNDIS_802_11_VARIABLE_IEs) VarIE;
+	pVIE->Length = 0;
+    RTMPZeroMemory(&HtCapability, sizeof(HtCapability));
+	RTMPZeroMemory(&AddHtInfo, sizeof(ADD_HT_INFO_IE));
+
+
+	if (PeerBeaconAndProbeRspSanity(pAd,
+								Elem->Msg,
+								Elem->MsgLen,
+								Elem->Channel,
+								Addr2,
+								Bssid,
+								Ssid,
+								&SsidLen,
+								&BssType,
+								&BeaconPeriod,
+								&Channel,
+								&NewChannel,
+								&TimeStamp,
+								&Cf,
+								&AtimWin,
+								&CapabilityInfo,
+								&Erp,
+								&DtimCount,
+								&DtimPeriod,
+								&BcastFlag,
+								&MessageToMe,
+								SupRate,
+								&SupRateLen,
+								ExtRate,
+								&ExtRateLen,
+								&CkipFlag,
+								&AironetCellPowerLimit,
+								&EdcaParm,
+								&QbssLoad,
+								&QosCapability,
+								&RalinkIe,
+								&HtCapabilityLen,
+								&PreNHtCapabilityLen,
+								&HtCapability,
+								&AddHtInfoLen,
+								&AddHtInfo,
+								&NewExtChannelOffset,
+								&LenVIE,
+								pVIE))
+	{
+		// Disqualify 11b only adhoc when we are in 11g only adhoc mode
+		if ((BssType == BSS_ADHOC) && (pAd->CommonCfg.PhyMode == PHY_11G) && ((SupRateLen+ExtRateLen)< 12))
+			return;
+
+		// BEACON from desired BSS/IBSS found. We should be able to decide most
+		// BSS parameters here.
+		// Q. But what happen if this JOIN doesn't conclude a successful ASSOCIATEION?
+		//    Do we need to receover back all parameters belonging to previous BSS?
+		// A. Should be not. There's no back-door recover to previous AP. It still need
+		//    a new JOIN-AUTH-ASSOC sequence.
+		if (MAC_ADDR_EQUAL(pAd->MlmeAux.Bssid, Bssid))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("SYNC - receive desired BEACON at JoinWaitBeacon... Channel = %d\n", Channel));
+			RTMPCancelTimer(&pAd->MlmeAux.BeaconTimer, &TimerCancelled);
+
+			// Update RSSI to prevent No signal display when cards first initialized
+			pAd->StaCfg.RssiSample.LastRssi0	= ConvertToRssi(pAd, Elem->Rssi0, RSSI_0);
+			pAd->StaCfg.RssiSample.LastRssi1	= ConvertToRssi(pAd, Elem->Rssi1, RSSI_1);
+			pAd->StaCfg.RssiSample.LastRssi2	= ConvertToRssi(pAd, Elem->Rssi2, RSSI_2);
+			pAd->StaCfg.RssiSample.AvgRssi0	= pAd->StaCfg.RssiSample.LastRssi0;
+			pAd->StaCfg.RssiSample.AvgRssi0X8	= pAd->StaCfg.RssiSample.AvgRssi0 << 3;
+			pAd->StaCfg.RssiSample.AvgRssi1	= pAd->StaCfg.RssiSample.LastRssi1;
+			pAd->StaCfg.RssiSample.AvgRssi1X8	= pAd->StaCfg.RssiSample.AvgRssi1 << 3;
+			pAd->StaCfg.RssiSample.AvgRssi2	= pAd->StaCfg.RssiSample.LastRssi2;
+			pAd->StaCfg.RssiSample.AvgRssi2X8	= pAd->StaCfg.RssiSample.AvgRssi2 << 3;
+
+			//
+			// We need to check if SSID only set to any, then we can record the current SSID.
+			// Otherwise will cause hidden SSID association failed.
+			//
+			if (pAd->MlmeAux.SsidLen == 0)
+			{
+				NdisMoveMemory(pAd->MlmeAux.Ssid, Ssid, SsidLen);
+				pAd->MlmeAux.SsidLen = SsidLen;
+			}
+			else
+			{
+				Idx = BssSsidTableSearch(&pAd->ScanTab, Bssid, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen, Channel);
+
+				if (Idx != BSS_NOT_FOUND)
+				{
+					//
+					// Multiple SSID case, used correct CapabilityInfo
+					//
+					CapabilityInfo = pAd->ScanTab.BssEntry[Idx].CapabilityInfo;
+				}
+			}
+			NdisMoveMemory(pAd->MlmeAux.Bssid, Bssid, MAC_ADDR_LEN);
+			pAd->MlmeAux.CapabilityInfo = CapabilityInfo & SUPPORTED_CAPABILITY_INFO;
+			pAd->MlmeAux.BssType = BssType;
+			pAd->MlmeAux.BeaconPeriod = BeaconPeriod;
+			pAd->MlmeAux.Channel = Channel;
+			pAd->MlmeAux.AtimWin = AtimWin;
+			pAd->MlmeAux.CfpPeriod = Cf.CfpPeriod;
+			pAd->MlmeAux.CfpMaxDuration = Cf.CfpMaxDuration;
+			pAd->MlmeAux.APRalinkIe = RalinkIe;
+
+			// Copy AP's supported rate to MlmeAux for creating assoication request
+			// Also filter out not supported rate
+			pAd->MlmeAux.SupRateLen = SupRateLen;
+			NdisMoveMemory(pAd->MlmeAux.SupRate, SupRate, SupRateLen);
+			RTMPCheckRates(pAd, pAd->MlmeAux.SupRate, &pAd->MlmeAux.SupRateLen);
+			pAd->MlmeAux.ExtRateLen = ExtRateLen;
+			NdisMoveMemory(pAd->MlmeAux.ExtRate, ExtRate, ExtRateLen);
+			RTMPCheckRates(pAd, pAd->MlmeAux.ExtRate, &pAd->MlmeAux.ExtRateLen);
+
+            NdisZeroMemory(pAd->StaActive.SupportedPhyInfo.MCSSet, 16);
+#ifdef DOT11_N_SUPPORT
+			pAd->MlmeAux.NewExtChannelOffset = NewExtChannelOffset;
+			pAd->MlmeAux.HtCapabilityLen = HtCapabilityLen;
+
+			// filter out un-supported ht rates
+			if (((HtCapabilityLen > 0) || (PreNHtCapabilityLen > 0)) && (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+			{
+				RTMPZeroMemory(&pAd->MlmeAux.HtCapability, SIZE_HT_CAP_IE);
+   				RTMPMoveMemory(&pAd->MlmeAux.AddHtInfo, &AddHtInfo, SIZE_ADD_HT_INFO_IE);
+
+				// StaActive.SupportedHtPhy.MCSSet stores Peer AP's 11n Rx capability
+				NdisMoveMemory(pAd->StaActive.SupportedPhyInfo.MCSSet, HtCapability.MCSSet, 16);
+				pAd->MlmeAux.NewExtChannelOffset = NewExtChannelOffset;
+				pAd->MlmeAux.HtCapabilityLen = SIZE_HT_CAP_IE;
+				pAd->StaActive.SupportedPhyInfo.bHtEnable = TRUE;
+				if (PreNHtCapabilityLen > 0)
+					pAd->StaActive.SupportedPhyInfo.bPreNHt = TRUE;
+				RTMPCheckHt(pAd, BSSID_WCID, &HtCapability, &AddHtInfo);
+				// Copy AP Parameter to StaActive.  This is also in LinkUp.
+				DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAtJoinAction! (MpduDensity=%d, MaxRAmpduFactor=%d, BW=%d)\n",
+					pAd->StaActive.SupportedHtPhy.MpduDensity, pAd->StaActive.SupportedHtPhy.MaxRAmpduFactor, HtCapability.HtCapInfo.ChannelWidth));
+
+				if (AddHtInfoLen > 0)
+				{
+					CentralChannel = AddHtInfo.ControlChan;
+		 			// Check again the Bandwidth capability of this AP.
+		 			if ((AddHtInfo.ControlChan > 2)&& (AddHtInfo.AddHtInfo.ExtChanOffset == EXTCHA_BELOW) && (HtCapability.HtCapInfo.ChannelWidth == BW_40))
+		 			{
+		 				CentralChannel = AddHtInfo.ControlChan - 2;
+		 			}
+		 			else if ((AddHtInfo.AddHtInfo.ExtChanOffset == EXTCHA_ABOVE) && (HtCapability.HtCapInfo.ChannelWidth == BW_40))
+		 			{
+		 				CentralChannel = AddHtInfo.ControlChan + 2;
+		 			}
+
+					// Check Error .
+					if (pAd->MlmeAux.CentralChannel != CentralChannel)
+		 				DBGPRINT(RT_DEBUG_ERROR, ("PeerBeaconAtJoinAction HT===>Beacon Central Channel = %d, Control Channel = %d. Mlmeaux CentralChannel = %d\n", CentralChannel, AddHtInfo.ControlChan, pAd->MlmeAux.CentralChannel));
+
+		 			DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAtJoinAction HT===>Central Channel = %d, Control Channel = %d,  .\n", CentralChannel, AddHtInfo.ControlChan));
+
+				}
+
+			}
+			else
+#endif // DOT11_N_SUPPORT //
+			{
+   				// To prevent error, let legacy AP must have same CentralChannel and Channel.
+				if ((HtCapabilityLen == 0) && (PreNHtCapabilityLen == 0))
+					pAd->MlmeAux.CentralChannel = pAd->MlmeAux.Channel;
+
+				pAd->StaActive.SupportedPhyInfo.bHtEnable = FALSE;
+				RTMPZeroMemory(&pAd->MlmeAux.HtCapability, SIZE_HT_CAP_IE);
+				RTMPZeroMemory(&pAd->MlmeAux.AddHtInfo, SIZE_ADD_HT_INFO_IE);
+			}
+
+			RTMPUpdateMlmeRate(pAd);
+
+			// copy QOS related information
+			if ((pAd->CommonCfg.bWmmCapable)
+#ifdef DOT11_N_SUPPORT
+				 || (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED)
+#endif // DOT11_N_SUPPORT //
+				)
+			{
+				NdisMoveMemory(&pAd->MlmeAux.APEdcaParm, &EdcaParm, sizeof(EDCA_PARM));
+				NdisMoveMemory(&pAd->MlmeAux.APQbssLoad, &QbssLoad, sizeof(QBSS_LOAD_PARM));
+				NdisMoveMemory(&pAd->MlmeAux.APQosCapability, &QosCapability, sizeof(QOS_CAPABILITY_PARM));
+			}
+			else
+			{
+				NdisZeroMemory(&pAd->MlmeAux.APEdcaParm, sizeof(EDCA_PARM));
+				NdisZeroMemory(&pAd->MlmeAux.APQbssLoad, sizeof(QBSS_LOAD_PARM));
+				NdisZeroMemory(&pAd->MlmeAux.APQosCapability, sizeof(QOS_CAPABILITY_PARM));
+			}
+
+			DBGPRINT(RT_DEBUG_TRACE, ("SYNC - after JOIN, SupRateLen=%d, ExtRateLen=%d\n",
+										pAd->MlmeAux.SupRateLen, pAd->MlmeAux.ExtRateLen));
+
+#ifdef LEAP_SUPPORT
+			// Update CkipFlag
+			pAd->StaCfg.CkipFlag = CkipFlag;
+
+			// Keep TimeStamp for Re-Association used.
+			if (LEAP_CCKM_ON(pAd) && (pAd->StaCfg.CCKMLinkUpFlag == TRUE))
+				pAd->StaCfg.CCKMBeaconAtJoinTimeStamp = TimeStamp;
+#endif // LEAP_SUPPORT //
+
+			if (AironetCellPowerLimit != 0xFF)
+			{
+				//We need to change our TxPower for CCX 2.0 AP Control of Client Transmit Power
+				ChangeToCellPowerLimit(pAd, AironetCellPowerLimit);
+			}
+			else  //Used the default TX Power Percentage.
+				pAd->CommonCfg.TxPowerPercentage = pAd->CommonCfg.TxPowerDefault;
+
+			pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+			Status = MLME_SUCCESS;
+			MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_JOIN_CONF, 2, &Status);
+		}
+		// not to me BEACON, ignored
+	}
+	// sanity check fail, ignore this frame
+}
+
+/*
+	==========================================================================
+	Description:
+		receive BEACON from peer
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID PeerBeacon(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR         Bssid[MAC_ADDR_LEN], Addr2[MAC_ADDR_LEN];
+	CHAR          Ssid[MAX_LEN_OF_SSID];
+	CF_PARM       CfParm;
+	UCHAR         SsidLen, MessageToMe=0, BssType, Channel, NewChannel, index=0;
+	UCHAR         DtimCount=0, DtimPeriod=0, BcastFlag=0;
+	USHORT        CapabilityInfo, AtimWin, BeaconPeriod;
+	LARGE_INTEGER TimeStamp;
+	USHORT        TbttNumToNextWakeUp;
+	UCHAR         Erp;
+	UCHAR         SupRate[MAX_LEN_OF_SUPPORTED_RATES], ExtRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR		  SupRateLen, ExtRateLen;
+	UCHAR		  CkipFlag;
+	USHORT        LenVIE;
+	UCHAR		  AironetCellPowerLimit;
+	EDCA_PARM       EdcaParm;
+	QBSS_LOAD_PARM  QbssLoad;
+	QOS_CAPABILITY_PARM QosCapability;
+	ULONG           RalinkIe;
+	// New for WPA security suites
+	UCHAR						VarIE[MAX_VIE_LEN];		// Total VIE length = MAX_VIE_LEN - -5
+	NDIS_802_11_VARIABLE_IEs	*pVIE = NULL;
+	HT_CAPABILITY_IE		HtCapability;
+	ADD_HT_INFO_IE		AddHtInfo;	// AP might use this additional ht info IE
+	UCHAR			HtCapabilityLen, PreNHtCapabilityLen;
+	UCHAR			AddHtInfoLen;
+	UCHAR			NewExtChannelOffset = 0xff;
+
+
+#ifdef RALINK_ATE
+    if (ATE_ON(pAd))
+    {
+		return;
+    }
+#endif // RALINK_ATE //
+
+	if (!(INFRA_ON(pAd) || ADHOC_ON(pAd)
+		))
+		return;
+
+	// Init Variable IE structure
+	pVIE = (PNDIS_802_11_VARIABLE_IEs) VarIE;
+	pVIE->Length = 0;
+    RTMPZeroMemory(&HtCapability, sizeof(HtCapability));
+	RTMPZeroMemory(&AddHtInfo, sizeof(ADD_HT_INFO_IE));
+
+	if (PeerBeaconAndProbeRspSanity(pAd,
+								Elem->Msg,
+								Elem->MsgLen,
+								Elem->Channel,
+								Addr2,
+								Bssid,
+								Ssid,
+								&SsidLen,
+								&BssType,
+								&BeaconPeriod,
+								&Channel,
+								&NewChannel,
+								&TimeStamp,
+								&CfParm,
+								&AtimWin,
+								&CapabilityInfo,
+								&Erp,
+								&DtimCount,
+								&DtimPeriod,
+								&BcastFlag,
+								&MessageToMe,
+								SupRate,
+								&SupRateLen,
+								ExtRate,
+								&ExtRateLen,
+								&CkipFlag,
+								&AironetCellPowerLimit,
+								&EdcaParm,
+								&QbssLoad,
+								&QosCapability,
+								&RalinkIe,
+								&HtCapabilityLen,
+								&PreNHtCapabilityLen,
+								&HtCapability,
+								&AddHtInfoLen,
+								&AddHtInfo,
+								&NewExtChannelOffset,
+								&LenVIE,
+								pVIE))
+	{
+		BOOLEAN is_my_bssid, is_my_ssid;
+		ULONG   Bssidx, Now;
+		BSS_ENTRY *pBss;
+		CHAR		RealRssi = RTMPMaxRssi(pAd, ConvertToRssi(pAd, Elem->Rssi0, RSSI_0), ConvertToRssi(pAd, Elem->Rssi1, RSSI_1), ConvertToRssi(pAd, Elem->Rssi2, RSSI_2));
+
+		is_my_bssid = MAC_ADDR_EQUAL(Bssid, pAd->CommonCfg.Bssid)? TRUE : FALSE;
+		is_my_ssid = SSID_EQUAL(Ssid, SsidLen, pAd->CommonCfg.Ssid, pAd->CommonCfg.SsidLen)? TRUE:FALSE;
+
+
+		// ignore BEACON not for my SSID
+		if ((! is_my_ssid) && (! is_my_bssid))
+			return;
+
+		// It means STA waits disassoc completely from this AP, ignores this beacon.
+		if (pAd->Mlme.CntlMachine.CurrState == CNTL_WAIT_DISASSOC)
+			return;
+
+#ifdef DOT11_N_SUPPORT
+		// Copy Control channel for this BSSID.
+		if (AddHtInfoLen != 0)
+			Channel = AddHtInfo.ControlChan;
+
+		if ((HtCapabilityLen > 0) || (PreNHtCapabilityLen > 0))
+			HtCapabilityLen = SIZE_HT_CAP_IE;
+#endif // DOT11_N_SUPPORT //
+
+		//
+		// Housekeeping "SsidBssTab" table for later-on ROAMing usage.
+		//
+		Bssidx = BssTableSearch(&pAd->ScanTab, Bssid, Channel);
+		if (Bssidx == BSS_NOT_FOUND)
+		{
+			// discover new AP of this network, create BSS entry
+			Bssidx = BssTableSetEntry(pAd, &pAd->ScanTab, Bssid, Ssid, SsidLen, BssType, BeaconPeriod,
+						 &CfParm, AtimWin, CapabilityInfo, SupRate, SupRateLen, ExtRate, ExtRateLen,
+						&HtCapability, &AddHtInfo,HtCapabilityLen,AddHtInfoLen,NewExtChannelOffset, Channel,
+						RealRssi, TimeStamp, CkipFlag, &EdcaParm, &QosCapability,
+						&QbssLoad, LenVIE, pVIE);
+			if (Bssidx == BSS_NOT_FOUND) // return if BSS table full
+				return;
+
+			NdisMoveMemory(pAd->ScanTab.BssEntry[Bssidx].PTSF, &Elem->Msg[24], 4);
+			NdisMoveMemory(&pAd->ScanTab.BssEntry[Bssidx].TTSF[0], &Elem->TimeStamp.u.LowPart, 4);
+			NdisMoveMemory(&pAd->ScanTab.BssEntry[Bssidx].TTSF[4], &Elem->TimeStamp.u.LowPart, 4);
+
+
+
+		}
+
+		if ((pAd->CommonCfg.bIEEE80211H == 1) && (NewChannel != 0) && (Channel != NewChannel))
+		{
+			// Switching to channel 1 can prevent from rescanning the current channel immediately (by auto reconnection).
+			// In addition, clear the MLME queue and the scan table to discard the RX packets and previous scanning results.
+			AsicSwitchChannel(pAd, 1, FALSE);
+			AsicLockChannel(pAd, 1);
+		    LinkDown(pAd, FALSE);
+			MlmeQueueInit(&pAd->Mlme.Queue);
+			BssTableInit(&pAd->ScanTab);
+		    RTMPusecDelay(1000000);		// use delay to prevent STA do reassoc
+
+			// channel sanity check
+			for (index = 0 ; index < pAd->ChannelListNum; index++)
+			{
+				if (pAd->ChannelList[index].Channel == NewChannel)
+				{
+					pAd->ScanTab.BssEntry[Bssidx].Channel = NewChannel;
+					pAd->CommonCfg.Channel = NewChannel;
+					AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+					AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+					DBGPRINT(RT_DEBUG_TRACE, ("PeerBeacon - STA receive channel switch announcement IE (New Channel =%d)\n", NewChannel));
+					break;
+				}
+			}
+
+			if (index >= pAd->ChannelListNum)
+			{
+				DBGPRINT_ERR(("PeerBeacon(can not find New Channel=%d in ChannelList[%d]\n", pAd->CommonCfg.Channel, pAd->ChannelListNum));
+			}
+		}
+
+		// if the ssid matched & bssid unmatched, we should select the bssid with large value.
+		// This might happened when two STA start at the same time
+		if ((! is_my_bssid) && ADHOC_ON(pAd))
+		{
+			INT	i;
+
+			// Add the safeguard against the mismatch of adhoc wep status
+			if (pAd->StaCfg.WepStatus != pAd->ScanTab.BssEntry[Bssidx].WepStatus)
+			{
+				return;
+			}
+
+			// collapse into the ADHOC network which has bigger BSSID value.
+			for (i = 0; i < 6; i++)
+			{
+				if (Bssid[i] > pAd->CommonCfg.Bssid[i])
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("SYNC - merge to the IBSS with bigger BSSID=%02x:%02x:%02x:%02x:%02x:%02x\n",
+						Bssid[0], Bssid[1], Bssid[2], Bssid[3], Bssid[4], Bssid[5]));
+					AsicDisableSync(pAd);
+					COPY_MAC_ADDR(pAd->CommonCfg.Bssid, Bssid);
+					AsicSetBssid(pAd, pAd->CommonCfg.Bssid);
+					MakeIbssBeacon(pAd);        // re-build BEACON frame
+					AsicEnableIbssSync(pAd);    // copy BEACON frame to on-chip memory
+					is_my_bssid = TRUE;
+					break;
+				}
+				else if (Bssid[i] < pAd->CommonCfg.Bssid[i])
+					break;
+			}
+		}
+
+
+		NdisGetSystemUpTime(&Now);
+		pBss = &pAd->ScanTab.BssEntry[Bssidx];
+		pBss->Rssi = RealRssi;       // lastest RSSI
+		pBss->LastBeaconRxTime = Now;   // last RX timestamp
+
+		//
+		// BEACON from my BSSID - either IBSS or INFRA network
+		//
+		if (is_my_bssid)
+		{
+			RXWI_STRUC	RxWI;
+
+			pAd->StaCfg.DtimCount = DtimCount;
+			pAd->StaCfg.DtimPeriod = DtimPeriod;
+			pAd->StaCfg.LastBeaconRxTime = Now;
+
+
+			RxWI.RSSI0 = Elem->Rssi0;
+			RxWI.RSSI1 = Elem->Rssi1;
+			RxWI.RSSI2 = Elem->Rssi2;
+
+			Update_Rssi_Sample(pAd, &pAd->StaCfg.RssiSample, &RxWI);
+			if (AironetCellPowerLimit != 0xFF)
+			{
+				//
+				// We get the Cisco (ccx) "TxPower Limit" required
+				// Changed to appropriate TxPower Limit for Ciso Compatible Extensions
+				//
+				ChangeToCellPowerLimit(pAd, AironetCellPowerLimit);
+			}
+			else
+			{
+				//
+				// AironetCellPowerLimit equal to 0xFF means the Cisco (ccx) "TxPower Limit" not exist.
+				// Used the default TX Power Percentage, that set from UI.
+				//
+				pAd->CommonCfg.TxPowerPercentage = pAd->CommonCfg.TxPowerDefault;
+			}
+
+			// at least one 11b peer joined. downgrade the MaxTxRate to 11Mbps
+			// after last 11b peer left for several seconds, we'll auto switch back to 11G rate
+			// in MlmePeriodicExec()
+			if (ADHOC_ON(pAd) && (CAP_IS_IBSS_ON(CapabilityInfo)))
+			{
+				BOOLEAN	bRestart;
+                BOOLEAN	bnRestart;
+
+				bRestart = FALSE;
+                bnRestart = FALSE;
+
+				do
+				{
+					if ((SupRateLen+ExtRateLen <= 4) && (pAd->CommonCfg.MaxTxRate > RATE_11))
+					{
+						if (pAd->StaCfg.AdhocBOnlyJoined == FALSE)
+						{
+							DBGPRINT(RT_DEBUG_TRACE, ("SYNC - 11b peer joined. down-grade to 11b TX rates \n"));
+							bRestart = TRUE;
+							NdisMoveMemory(pAd->StaActive.SupRate, SupRate, MAX_LEN_OF_SUPPORTED_RATES);
+							pAd->StaActive.SupRateLen = SupRateLen;
+							NdisMoveMemory(pAd->StaActive.ExtRate, ExtRate, MAX_LEN_OF_SUPPORTED_RATES);
+							pAd->StaActive.ExtRateLen = ExtRateLen;
+							pAd->StaCfg.AdhocBOnlyJoined = TRUE;
+							pAd->StaActive.SupportedPhyInfo.bHtEnable = FALSE;
+							AsicSetEdcaParm(pAd, NULL);
+						}
+
+						// this timestamp is for MlmePeriodicExec() to check if all 11B peers have left
+						pAd->StaCfg.Last11bBeaconRxTime = Now;
+						break;
+					}
+#ifdef DOT11_N_SUPPORT
+					// Update Ht Phy.
+					if ((pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+					{
+						if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED) &&
+							!pAd->StaCfg.AdhocBGJoined &&
+							!pAd->StaCfg.AdhocBOnlyJoined)
+							AdhocTurnOnQos(pAd);
+
+						// Handle rate switch issue when Adhoc mode
+						if ((SupRateLen+ExtRateLen >= 8) && (HtCapability.MCSSet[0] == 0) && (HtCapability.MCSSet[1] == 0))
+						{
+							if (pAd->StaCfg.AdhocBGJoined == FALSE)
+							{
+								DBGPRINT(RT_DEBUG_TRACE, ("SYNC - 11g peer joined. down-grade to 11g TX rates \n"));
+								bRestart = TRUE;
+								NdisMoveMemory(pAd->StaActive.SupRate, SupRate, MAX_LEN_OF_SUPPORTED_RATES);
+								pAd->StaActive.SupRateLen = SupRateLen;
+								NdisMoveMemory(pAd->StaActive.ExtRate, ExtRate, MAX_LEN_OF_SUPPORTED_RATES);
+								pAd->StaActive.ExtRateLen = ExtRateLen;
+								pAd->StaCfg.AdhocBGJoined = TRUE;
+								pAd->StaActive.SupportedPhyInfo.bHtEnable = FALSE;
+								AsicSetEdcaParm(pAd, NULL);
+							}
+
+							// this timestamp is for MlmePeriodicExec() to check if all 11g peers have left
+							pAd->StaCfg.Last11gBeaconRxTime = Now;
+							break;
+						}
+						else if (!pAd->StaCfg.AdhocBGJoined &&
+								 !pAd->StaCfg.AdhocBOnlyJoined &&
+								 (pAd->CommonCfg.RegTransmitSetting.field.BW == BW_40) &&
+								 (HtCapability.HtCapInfo.ChannelWidth == BW_20))
+						{
+							if (pAd->StaCfg.Adhoc20NJoined == FALSE)
+							{
+								pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel;
+
+								pAd->StaCfg.Adhoc20NJoined = TRUE;
+								NdisMoveMemory(&pAd->MlmeAux.HtCapability, &HtCapability, SIZE_HT_CAP_IE);
+								if (AddHtInfoLen != 0)
+									NdisMoveMemory(&pAd->MlmeAux.AddHtInfo, &AddHtInfo, AddHtInfoLen);
+								NdisMoveMemory(pAd->StaActive.SupportedPhyInfo.MCSSet, HtCapability.MCSSet, 16);
+
+								RTMPCheckHt(pAd, Elem->Wcid, &pAd->MlmeAux.HtCapability, &pAd->MlmeAux.AddHtInfo);
+								COPY_HTSETTINGS_FROM_MLME_AUX_TO_ACTIVE_CFG(pAd);
+								pAd->StaActive.SupportedPhyInfo.bHtEnable = TRUE;
+								bRestart = TRUE;
+								bnRestart = TRUE;
+							}
+							// this timestamp is for MlmePeriodicExec() to check if all 20MHz N peers have left
+							pAd->StaCfg.Last20NBeaconRxTime = Now;
+						}
+
+					}
+					else
+#endif // DOT11_N_SUPPORT //
+					{
+						RTMPZeroMemory(&pAd->MlmeAux.HtCapability, SIZE_HT_CAP_IE);
+						RTMPZeroMemory(&pAd->MlmeAux.AddHtInfo, SIZE_ADD_HT_INFO_IE);
+					}
+				}while (FALSE);
+
+				// If peer Adhoc is legacy mode, I don't need to call MlmeUpdateHtTxRates no matter I support HT or not
+				if ((bRestart == TRUE) && (bnRestart == FALSE))
+				{
+					MlmeUpdateTxRates(pAd, FALSE, 0);
+					MakeIbssBeacon(pAd);        // re-build BEACON frame
+					AsicEnableIbssSync(pAd);    // copy to on-chip memory
+				}
+#ifdef DOT11_N_SUPPORT
+				else if ((bRestart == TRUE) && (bnRestart == TRUE))
+				{
+					MlmeUpdateTxRates(pAd, FALSE, BSS0);
+					MlmeUpdateHtTxRates(pAd, BSS0);
+					MakeIbssBeacon(pAd);        // re-build BEACON frame
+					AsicEnableIbssSync(pAd);    // copy to on-chip memory
+				}
+#endif // DOT11_N_SUPPORT //
+
+				// At least another peer in this IBSS, declare MediaState as CONNECTED
+				if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+				{
+					OPSTATUS_SET_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED);
+
+					pAd->IndicateMediaState = NdisMediaStateConnected;
+					RTMP_IndicateMediaState(pAd);
+	                pAd->ExtraInfo = GENERAL_LINK_UP;
+					AsicSetBssid(pAd, pAd->CommonCfg.Bssid);
+
+					// 2003/03/12 - john
+					// Make sure this entry in "ScanTab" table, thus complies to Microsoft's policy that
+					// "site survey" result should always include the current connected network.
+					//
+					Bssidx = BssTableSearch(&pAd->ScanTab, Bssid, Channel);
+					if (Bssidx == BSS_NOT_FOUND)
+					{
+						Bssidx = BssTableSetEntry(pAd, &pAd->ScanTab, Bssid, Ssid, SsidLen, BssType, BeaconPeriod,
+									&CfParm, AtimWin, CapabilityInfo, SupRate, SupRateLen, ExtRate, ExtRateLen, &HtCapability,
+									&AddHtInfo, HtCapabilityLen, AddHtInfoLen, NewExtChannelOffset, Channel, RealRssi, TimeStamp, 0,
+									&EdcaParm, &QosCapability, &QbssLoad, LenVIE, pVIE);
+					}
+					DBGPRINT(RT_DEBUG_TRACE, ("ADHOC  fOP_STATUS_MEDIA_STATE_CONNECTED.\n"));
+				}
+
+				// Ad-hoc mode is using MAC address as BA session. So we need to continuously find newly joined adhoc station by receiving beacon.
+				// To prevent always check this, we use wcid == RESERVED_WCID to recognize it as newly joined adhoc station.
+				if (ADHOC_ON(pAd) && (Elem->Wcid == RESERVED_WCID))
+				{
+					UCHAR	idx;
+					MAC_TABLE_ENTRY *pEntry;
+
+					// look up the existing table
+					pEntry = MacTableLookup(pAd, Addr2);
+					if (pEntry == NULL)
+					{
+						// Another adhoc joining, add to our MAC table.
+						pEntry = MacTableInsertEntry(pAd, Addr2, BSS0, FALSE);
+						if (pEntry)
+						{
+							pEntry->Sst = SST_ASSOC;
+							idx = pAd->StaCfg.DefaultKeyId;
+							// After InsertEntry, Write to ASIC on-chip table.
+							RT28XX_STA_SECURITY_INFO_ADD(pAd, BSS0, idx, pEntry);
+							DBGPRINT(RT_DEBUG_TRACE, ("ADHOC %x:%x:%x:%x:%x:%x  join in.Entry=%d\n", Addr2[0],Addr2[1],Addr2[2],Addr2[3],Addr2[4],Addr2[5], pEntry->Aid));
+
+							pEntry->HTPhyMode.word = pAd->StaCfg.HTPhyMode.word;
+	                        if (HtCapabilityLen <= 0)
+	                        {
+	                            pEntry->HTPhyMode.field.STBC = 0;
+	                            pEntry->HTPhyMode.field.BW = 0;
+	                            pEntry->HTPhyMode.field.ShortGI = 0;
+	                            if ((SupRateLen+ExtRateLen <= 4) && (pAd->CommonCfg.Channel <= 14))
+	        					{
+	        						pEntry->HTPhyMode.field.MODE = MODE_CCK;
+	        					}
+	        					else
+	        					{
+	        						pEntry->HTPhyMode.field.MODE = MODE_OFDM;
+	        					}
+								MlmeUpdateTxRates(pAd, FALSE, 0);
+	                        }
+#ifdef DOT11_N_SUPPORT
+							else
+							{
+								MlmeUpdateTxRates(pAd, FALSE, 0);
+								MlmeUpdateHtTxRates(pAd, BSS0);
+							}
+#endif // DOT11_N_SUPPORT //
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+	                        if (pAd->StaCfg.WpaSupplicantUP)
+	                        {
+	                            union iwreq_data    wrqu;
+
+	                            SendAssocIEsToWpaSupplicant(pAd);
+	                            memset(&wrqu, 0, sizeof(wrqu));
+	                            wrqu.data.flags = RT_ASSOC_EVENT_FLAG;
+	                            wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+	                        }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+	                        {
+	                            union iwreq_data    wrqu;
+	                            wext_notify_event_assoc(pAd);
+
+	                            memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+	                            memcpy(wrqu.ap_addr.sa_data, pAd->MlmeAux.Bssid, MAC_ADDR_LEN);
+	                            wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+
+	                        }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+						}
+					}
+				}
+			}
+
+			if (INFRA_ON(pAd))
+			{
+				BOOLEAN bUseShortSlot, bUseBGProtection;
+
+				// decide to use/change to -
+				//      1. long slot (20 us) or short slot (9 us) time
+				//      2. turn on/off RTS/CTS and/or CTS-to-self protection
+				//      3. short preamble
+
+				//bUseShortSlot = pAd->CommonCfg.bUseShortSlotTime && CAP_IS_SHORT_SLOT(CapabilityInfo);
+				bUseShortSlot = CAP_IS_SHORT_SLOT(CapabilityInfo);
+				if (bUseShortSlot != OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_SHORT_SLOT_INUSED))
+					AsicSetSlotTime(pAd, bUseShortSlot);
+
+				bUseBGProtection = (pAd->CommonCfg.UseBGProtection == 1) ||    // always use
+								   ((pAd->CommonCfg.UseBGProtection == 0) && ERP_IS_USE_PROTECTION(Erp));
+
+				if (pAd->CommonCfg.Channel > 14) // always no BG protection in A-band. falsely happened when switching A/G band to a dual-band AP
+					bUseBGProtection = FALSE;
+
+				if (bUseBGProtection != OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_BG_PROTECTION_INUSED))
+				{
+					if (bUseBGProtection)
+					{
+						OPSTATUS_SET_FLAG(pAd, fOP_STATUS_BG_PROTECTION_INUSED);
+						AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode, (OFDMSETPROTECT|CCKSETPROTECT|ALLN_SETPROTECT),FALSE,(pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent == 1));
+					}
+					else
+					{
+						OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_BG_PROTECTION_INUSED);
+						AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode, (OFDMSETPROTECT|CCKSETPROTECT|ALLN_SETPROTECT),TRUE,(pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent == 1));
+					}
+
+					DBGPRINT(RT_DEBUG_WARN, ("SYNC - AP changed B/G protection to %d\n", bUseBGProtection));
+				}
+
+#ifdef DOT11_N_SUPPORT
+				// check Ht protection mode. and adhere to the Non-GF device indication by AP.
+				if ((AddHtInfoLen != 0) &&
+					((AddHtInfo.AddHtInfo2.OperaionMode != pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode) ||
+					(AddHtInfo.AddHtInfo2.NonGfPresent != pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent)))
+				{
+					pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent = AddHtInfo.AddHtInfo2.NonGfPresent;
+					pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode = AddHtInfo.AddHtInfo2.OperaionMode;
+					if (pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent == 1)
+				{
+						AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode, ALLN_SETPROTECT, FALSE, TRUE);
+					}
+					else
+						AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode, ALLN_SETPROTECT, FALSE, FALSE);
+
+					DBGPRINT(RT_DEBUG_TRACE, ("SYNC - AP changed N OperaionMode to %d\n", pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode));
+				}
+#endif // DOT11_N_SUPPORT //
+
+				if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED) &&
+					ERP_IS_USE_BARKER_PREAMBLE(Erp))
+				{
+					MlmeSetTxPreamble(pAd, Rt802_11PreambleLong);
+					DBGPRINT(RT_DEBUG_TRACE, ("SYNC - AP forced to use LONG preamble\n"));
+				}
+
+				if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED)    &&
+					(EdcaParm.bValid == TRUE)                          &&
+					(EdcaParm.EdcaUpdateCount != pAd->CommonCfg.APEdcaParm.EdcaUpdateCount))
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("SYNC - AP change EDCA parameters(from %d to %d)\n",
+						pAd->CommonCfg.APEdcaParm.EdcaUpdateCount,
+						EdcaParm.EdcaUpdateCount));
+					AsicSetEdcaParm(pAd, &EdcaParm);
+				}
+
+				// copy QOS related information
+				NdisMoveMemory(&pAd->CommonCfg.APQbssLoad, &QbssLoad, sizeof(QBSS_LOAD_PARM));
+				NdisMoveMemory(&pAd->CommonCfg.APQosCapability, &QosCapability, sizeof(QOS_CAPABILITY_PARM));
+			}
+
+			// only INFRASTRUCTURE mode support power-saving feature
+			if ((INFRA_ON(pAd) && (pAd->StaCfg.Psm == PWR_SAVE)) || (pAd->CommonCfg.bAPSDForcePowerSave))
+			{
+				UCHAR FreeNumber;
+				//  1. AP has backlogged unicast-to-me frame, stay AWAKE, send PSPOLL
+				//  2. AP has backlogged broadcast/multicast frame and we want those frames, stay AWAKE
+				//  3. we have outgoing frames in TxRing or MgmtRing, better stay AWAKE
+				//  4. Psm change to PWR_SAVE, but AP not been informed yet, we better stay AWAKE
+				//  5. otherwise, put PHY back to sleep to save battery.
+				if (MessageToMe)
+				{
+#ifdef RT2860
+					if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE))
+					{
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, pAd->StaCfg.BBPR3);
+						// Turn clk to 80Mhz.
+					}
+#endif // RT2860 //
+					if (pAd->CommonCfg.bAPSDCapable && pAd->CommonCfg.APEdcaParm.bAPSDCapable &&
+						pAd->CommonCfg.bAPSDAC_BE && pAd->CommonCfg.bAPSDAC_BK && pAd->CommonCfg.bAPSDAC_VI && pAd->CommonCfg.bAPSDAC_VO)
+					{
+						pAd->CommonCfg.bNeedSendTriggerFrame = TRUE;
+					}
+					else
+						RT28XX_PS_POLL_ENQUEUE(pAd);
+				}
+				else if (BcastFlag && (DtimCount == 0) && OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_RECEIVE_DTIM))
+				{
+#ifdef RT2860
+					if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE))
+					{
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, pAd->StaCfg.BBPR3);
+					}
+#endif // RT2860 //
+				}
+				else if ((pAd->TxSwQueue[QID_AC_BK].Number != 0)													||
+						(pAd->TxSwQueue[QID_AC_BE].Number != 0)														||
+						(pAd->TxSwQueue[QID_AC_VI].Number != 0)														||
+						(pAd->TxSwQueue[QID_AC_VO].Number != 0)														||
+						(RTMPFreeTXDRequest(pAd, QID_AC_BK, TX_RING_SIZE - 1, &FreeNumber) != NDIS_STATUS_SUCCESS)	||
+						(RTMPFreeTXDRequest(pAd, QID_AC_BE, TX_RING_SIZE - 1, &FreeNumber) != NDIS_STATUS_SUCCESS)	||
+						(RTMPFreeTXDRequest(pAd, QID_AC_VI, TX_RING_SIZE - 1, &FreeNumber) != NDIS_STATUS_SUCCESS)	||
+						(RTMPFreeTXDRequest(pAd, QID_AC_VO, TX_RING_SIZE - 1, &FreeNumber) != NDIS_STATUS_SUCCESS)	||
+						(RTMPFreeTXDRequest(pAd, QID_MGMT, MGMT_RING_SIZE - 1, &FreeNumber) != NDIS_STATUS_SUCCESS))
+				{
+					// TODO: consider scheduled HCCA. might not be proper to use traditional DTIM-based power-saving scheme
+					// can we cheat here (i.e. just check MGMT & AC_BE) for better performance?
+#ifdef RT2860
+					if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE))
+					{
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, pAd->StaCfg.BBPR3);
+					}
+#endif // RT2860 //
+				}
+				else
+				{
+					USHORT NextDtim = DtimCount;
+
+					if (NextDtim == 0)
+						NextDtim = DtimPeriod;
+
+					TbttNumToNextWakeUp = pAd->StaCfg.DefaultListenCount;
+					if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_RECEIVE_DTIM) && (TbttNumToNextWakeUp > NextDtim))
+						TbttNumToNextWakeUp = NextDtim;
+
+					if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+					{
+						AsicSleepThenAutoWakeup(pAd, TbttNumToNextWakeUp);
+					}
+				}
+			}
+		}
+		// not my BSSID, ignore it
+	}
+	// sanity check fail, ignore this frame
+}
+
+/*
+	==========================================================================
+	Description:
+		Receive PROBE REQ from remote peer when operating in IBSS mode
+	==========================================================================
+ */
+VOID PeerProbeReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR         Addr2[MAC_ADDR_LEN];
+	CHAR          Ssid[MAX_LEN_OF_SSID];
+	UCHAR         SsidLen;
+#ifdef DOT11_N_SUPPORT
+	UCHAR		  HtLen, AddHtLen, NewExtLen;
+#endif // DOT11_N_SUPPORT //
+	HEADER_802_11 ProbeRspHdr;
+	NDIS_STATUS   NStatus;
+	PUCHAR        pOutBuffer = NULL;
+	ULONG         FrameLen = 0;
+	LARGE_INTEGER FakeTimestamp;
+	UCHAR         DsLen = 1, IbssLen = 2;
+	UCHAR         LocalErpIe[3] = {IE_ERP, 1, 0};
+	BOOLEAN       Privacy;
+	USHORT        CapabilityInfo;
+	UCHAR		  RSNIe = IE_WPA;
+
+	if (! ADHOC_ON(pAd))
+		return;
+
+	if (PeerProbeReqSanity(pAd, Elem->Msg, Elem->MsgLen, Addr2, Ssid, &SsidLen))
+	{
+		if ((SsidLen == 0) || SSID_EQUAL(Ssid, SsidLen, pAd->CommonCfg.Ssid, pAd->CommonCfg.SsidLen))
+		{
+			// allocate and send out ProbeRsp frame
+			NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+			if (NStatus != NDIS_STATUS_SUCCESS)
+				return;
+
+			//pAd->StaCfg.AtimWin = 0;  // ??????
+
+			Privacy = (pAd->StaCfg.WepStatus == Ndis802_11Encryption1Enabled) ||
+					  (pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) ||
+					  (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled);
+			CapabilityInfo = CAP_GENERATE(0, 1, Privacy, (pAd->CommonCfg.TxPreamble == Rt802_11PreambleShort), 0, 0);
+
+			MakeOutgoingFrame(pOutBuffer,                   &FrameLen,
+							  sizeof(HEADER_802_11),        &ProbeRspHdr,
+							  TIMESTAMP_LEN,                &FakeTimestamp,
+							  2,                            &pAd->CommonCfg.BeaconPeriod,
+							  2,                            &CapabilityInfo,
+							  1,                            &SsidIe,
+							  1,                            &pAd->CommonCfg.SsidLen,
+							  pAd->CommonCfg.SsidLen,       pAd->CommonCfg.Ssid,
+							  1,                            &SupRateIe,
+							  1,                            &pAd->StaActive.SupRateLen,
+							  pAd->StaActive.SupRateLen,    pAd->StaActive.SupRate,
+							  1,                            &DsIe,
+							  1,                            &DsLen,
+							  1,                            &pAd->CommonCfg.Channel,
+							  1,                            &IbssIe,
+							  1,                            &IbssLen,
+							  2,                            &pAd->StaActive.AtimWin,
+							  END_OF_ARGS);
+
+			if (pAd->StaActive.ExtRateLen)
+			{
+				ULONG tmp;
+				MakeOutgoingFrame(pOutBuffer + FrameLen,        &tmp,
+								  3,                            LocalErpIe,
+								  1,                            &ExtRateIe,
+								  1,                            &pAd->StaActive.ExtRateLen,
+								  pAd->StaActive.ExtRateLen,    &pAd->StaActive.ExtRate,
+								  END_OF_ARGS);
+				FrameLen += tmp;
+			}
+
+			// If adhoc secruity is set for WPA-None, append the cipher suite IE
+			if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+			{
+				ULONG tmp;
+				MakeOutgoingFrame(pOutBuffer + FrameLen,        	&tmp,
+						  			1,                              &RSNIe,
+						  			1,                            	&pAd->StaCfg.RSNIE_Len,
+						  			pAd->StaCfg.RSNIE_Len,      	pAd->StaCfg.RSN_IE,
+						  			END_OF_ARGS);
+				FrameLen += tmp;
+			}
+#ifdef DOT11_N_SUPPORT
+			if (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED)
+			{
+				ULONG TmpLen;
+				UCHAR	BROADCOM[4] = {0x0, 0x90, 0x4c, 0x33};
+				HtLen = sizeof(pAd->CommonCfg.HtCapability);
+				AddHtLen = sizeof(pAd->CommonCfg.AddHTInfo);
+				NewExtLen = 1;
+				//New extension channel offset IE is included in Beacon, Probe Rsp or channel Switch Announcement Frame
+				if (pAd->bBroadComHT == TRUE)
+				{
+					MakeOutgoingFrame(pOutBuffer + FrameLen,            &TmpLen,
+								  1,                                &WpaIe,
+								  4,                                &BROADCOM[0],
+								 pAd->MlmeAux.HtCapabilityLen,          &pAd->MlmeAux.HtCapability,
+								  END_OF_ARGS);
+				}
+				else
+				{
+				MakeOutgoingFrame(pOutBuffer + FrameLen,            &TmpLen,
+								  1,                                &HtCapIe,
+								  1,                                &HtLen,
+								 sizeof(HT_CAPABILITY_IE),          &pAd->CommonCfg.HtCapability,
+								  1,                                &AddHtInfoIe,
+								  1,                                &AddHtLen,
+								 sizeof(ADD_HT_INFO_IE),          &pAd->CommonCfg.AddHTInfo,
+								  1,                                &NewExtChanIe,
+								  1,                                &NewExtLen,
+								 sizeof(NEW_EXT_CHAN_IE),          &pAd->CommonCfg.NewExtChanOffset,
+								  END_OF_ARGS);
+				}
+				FrameLen += TmpLen;
+			}
+#endif // DOT11_N_SUPPORT //
+			MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+			MlmeFreeMemory(pAd, pOutBuffer);
+		}
+	}
+}
+
+VOID BeaconTimeoutAtJoinAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("SYNC - BeaconTimeoutAtJoinAction\n"));
+	pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+	Status = MLME_REJ_TIMEOUT;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_JOIN_CONF, 2, &Status);
+}
+
+/*
+	==========================================================================
+	Description:
+		Scan timeout procedure. basically add channel index by 1 and rescan
+	==========================================================================
+ */
+VOID ScanTimeoutAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	pAd->MlmeAux.Channel = NextChannel(pAd, pAd->MlmeAux.Channel);
+
+	// Only one channel scanned for CISCO beacon request
+	if ((pAd->MlmeAux.ScanType == SCAN_CISCO_ACTIVE) ||
+		(pAd->MlmeAux.ScanType == SCAN_CISCO_PASSIVE) ||
+		(pAd->MlmeAux.ScanType == SCAN_CISCO_NOISE) ||
+		(pAd->MlmeAux.ScanType == SCAN_CISCO_CHANNEL_LOAD))
+		pAd->MlmeAux.Channel = 0;
+
+	// this routine will stop if pAd->MlmeAux.Channel == 0
+	ScanNextChannel(pAd);
+}
+
+/*
+	==========================================================================
+	Description:
+	==========================================================================
+ */
+VOID InvalidStateWhenScan(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("AYNC - InvalidStateWhenScan(state=%ld). Reset SYNC machine\n", pAd->Mlme.SyncMachine.CurrState));
+	pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+	Status = MLME_STATE_MACHINE_REJECT;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_SCAN_CONF, 2, &Status);
+}
+
+/*
+	==========================================================================
+	Description:
+	==========================================================================
+ */
+VOID InvalidStateWhenJoin(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("InvalidStateWhenJoin(state=%ld). Reset SYNC machine\n", pAd->Mlme.SyncMachine.CurrState));
+	pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+	Status = MLME_STATE_MACHINE_REJECT;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_JOIN_CONF, 2, &Status);
+}
+
+/*
+	==========================================================================
+	Description:
+	==========================================================================
+ */
+VOID InvalidStateWhenStart(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("InvalidStateWhenStart(state=%ld). Reset SYNC machine\n", pAd->Mlme.SyncMachine.CurrState));
+	pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+	Status = MLME_STATE_MACHINE_REJECT;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_START_CONF, 2, &Status);
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID EnqueuePsPoll(
+	IN PRTMP_ADAPTER pAd)
+{
+#ifdef RALINK_ATE
+    if (ATE_ON(pAd))
+    {
+		return;
+    }
+#endif // RALINK_ATE //
+
+
+	if (pAd->StaCfg.WindowsPowerMode == Ndis802_11PowerModeLegacy_PSP)
+    	pAd->PsPollFrame.FC.PwrMgmt = PWR_SAVE;
+	MiniportMMRequest(pAd, 0, (PUCHAR)&pAd->PsPollFrame, sizeof(PSPOLL_FRAME));
+}
+
+
+/*
+	==========================================================================
+	Description:
+	==========================================================================
+ */
+VOID EnqueueProbeRequest(
+	IN PRTMP_ADAPTER pAd)
+{
+	NDIS_STATUS     NState;
+	PUCHAR          pOutBuffer;
+	ULONG           FrameLen = 0;
+	HEADER_802_11   Hdr80211;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("force out a ProbeRequest ...\n"));
+
+	NState = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if (NState == NDIS_STATUS_SUCCESS)
+	{
+		MgtMacHeaderInit(pAd, &Hdr80211, SUBTYPE_PROBE_REQ, 0, BROADCAST_ADDR, BROADCAST_ADDR);
+
+		// this ProbeRequest explicitly specify SSID to reduce unwanted ProbeResponse
+		MakeOutgoingFrame(pOutBuffer,                     &FrameLen,
+						  sizeof(HEADER_802_11),          &Hdr80211,
+						  1,                              &SsidIe,
+						  1,                              &pAd->CommonCfg.SsidLen,
+						  pAd->CommonCfg.SsidLen,		  pAd->CommonCfg.Ssid,
+						  1,                              &SupRateIe,
+						  1,                              &pAd->StaActive.SupRateLen,
+						  pAd->StaActive.SupRateLen,      pAd->StaActive.SupRate,
+						  END_OF_ARGS);
+		MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+		MlmeFreeMemory(pAd, pOutBuffer);
+	}
+
+}
+
+#ifdef DOT11_N_SUPPORT
+#ifdef DOT11N_DRAFT3
+VOID BuildEffectedChannelList(
+	IN PRTMP_ADAPTER pAd)
+{
+	UCHAR		EChannel[11];
+	UCHAR		i, j, k;
+	UCHAR		UpperChannel = 0, LowerChannel = 0;
+
+	RTMPZeroMemory(EChannel, 11);
+	i = 0;
+	// Find upper channel and lower channel.
+	if (pAd->CommonCfg.CentralChannel < pAd->CommonCfg.Channel)
+	{
+		UpperChannel = pAd->CommonCfg.Channel;
+		LowerChannel = pAd->CommonCfg.CentralChannel;
+	}
+	else if (pAd->CommonCfg.CentralChannel > pAd->CommonCfg.Channel)
+	{
+		UpperChannel = pAd->CommonCfg.CentralChannel;
+		LowerChannel = pAd->CommonCfg.Channel;
+	}
+	else
+	{
+		return;
+	}
+
+	// Record channels that is below lower channel..
+	if (LowerChannel > 1)
+	{
+		EChannel[0] = LowerChannel - 1;
+		i = 1;
+		if (LowerChannel > 2)
+		{
+			EChannel[1] = LowerChannel - 2;
+			i = 2;
+			if (LowerChannel > 3)
+			{
+				EChannel[2] = LowerChannel - 3;
+				i = 3;
+			}
+		}
+	}
+	// Record channels that is between  lower channel and upper channel.
+	for (k = LowerChannel;k < UpperChannel;k++)
+	{
+		EChannel[i] = k;
+		i++;
+	}
+	// Record channels that is above upper channel..
+	if (LowerChannel < 11)
+	{
+		EChannel[i] = UpperChannel + 1;
+		i++;
+		if (LowerChannel < 10)
+		{
+			EChannel[i] = LowerChannel + 2;
+			i++;
+			if (LowerChannel < 9)
+			{
+				EChannel[i] = LowerChannel + 3;
+				i++;
+			}
+		}
+	}
+	//
+	for (j = 0;j < i;j++)
+	{
+		for (k = 0;k < pAd->ChannelListNum;k++)
+		{
+			if (pAd->ChannelList[k].Channel == EChannel[j])
+			{
+				pAd->ChannelList[k].bEffectedChannel = TRUE;
+				DBGPRINT(RT_DEBUG_TRACE,(" EffectedChannel( =%d)\n", EChannel[j]));
+				break;
+			}
+		}
+	}
+}
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+
+BOOLEAN ScanRunning(
+		IN PRTMP_ADAPTER pAd)
+{
+	return (pAd->Mlme.SyncMachine.CurrState == SCAN_LISTEN) ? TRUE : FALSE;
+}
+
diff --git a/drivers/staging/rt2860/sta/wpa.c b/drivers/staging/rt2860/sta/wpa.c
new file mode 100644
index 00000000000..774c6567ae5
--- /dev/null
+++ b/drivers/staging/rt2860/sta/wpa.c
@@ -0,0 +1,2086 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	wpa.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Jan	Lee		03-07-22		Initial
+	Paul Lin	03-11-28		Modify for supplicant
+*/
+#include "../rt_config.h"
+
+#define		WPARSNIE	0xdd
+#define		WPA2RSNIE	0x30
+
+//extern UCHAR BIT8[];
+UCHAR	CipherWpaPskTkip[] = {
+		0xDD, 0x16,				// RSN IE
+		0x00, 0x50, 0xf2, 0x01,	// oui
+		0x01, 0x00,				// Version
+		0x00, 0x50, 0xf2, 0x02,	// Multicast
+		0x01, 0x00,				// Number of unicast
+		0x00, 0x50, 0xf2, 0x02,	// unicast
+		0x01, 0x00,				// number of authentication method
+		0x00, 0x50, 0xf2, 0x02	// authentication
+		};
+UCHAR	CipherWpaPskTkipLen = (sizeof(CipherWpaPskTkip) / sizeof(UCHAR));
+
+UCHAR	CipherWpaPskAes[] = {
+		0xDD, 0x16, 			// RSN IE
+		0x00, 0x50, 0xf2, 0x01,	// oui
+		0x01, 0x00,				// Version
+		0x00, 0x50, 0xf2, 0x04,	// Multicast
+		0x01, 0x00,				// Number of unicast
+		0x00, 0x50, 0xf2, 0x04,	// unicast
+		0x01, 0x00,				// number of authentication method
+		0x00, 0x50, 0xf2, 0x02	// authentication
+		};
+UCHAR	CipherWpaPskAesLen = (sizeof(CipherWpaPskAes) / sizeof(UCHAR));
+
+UCHAR	CipherSuiteCiscoCCKM[] = {
+		0xDD, 0x16,				// RSN IE
+		0x00, 0x50, 0xf2, 0x01, // oui
+		0x01, 0x00,				// Version
+		0x00, 0x40, 0x96, 0x01, // Multicast
+		0x01, 0x00,				// Number of uicast
+		0x00, 0x40, 0x96, 0x01, // unicast
+		0x01, 0x00,				// number of authentication method
+		0x00, 0x40, 0x96, 0x00  // Authentication
+		};
+UCHAR	CipherSuiteCiscoCCKMLen = (sizeof(CipherSuiteCiscoCCKM) / sizeof(UCHAR));
+
+UCHAR	CipherSuiteCiscoCCKM24[] = {
+		0xDD, 0x18,				// RSN IE
+		0x00, 0x50, 0xf2, 0x01, // oui
+		0x01, 0x00,				// Version
+		0x00, 0x40, 0x96, 0x01, // Multicast
+		0x01, 0x00,				// Number of uicast
+		0x00, 0x40, 0x96, 0x01, // unicast
+		0x01, 0x00,				// number of authentication method
+		0x00, 0x40, 0x96, 0x00,
+		0x28, 0x00// Authentication
+		};
+
+UCHAR	CipherSuiteCiscoCCKM24Len = (sizeof(CipherSuiteCiscoCCKM24) / sizeof(UCHAR));
+
+UCHAR	CipherSuiteCCXTkip[] = {
+		0xDD, 0x16,				// RSN IE
+		0x00, 0x50, 0xf2, 0x01,	// oui
+		0x01, 0x00,				// Version
+		0x00, 0x50, 0xf2, 0x02,	// Multicast
+		0x01, 0x00,				// Number of unicast
+		0x00, 0x50, 0xf2, 0x02,	// unicast
+		0x01, 0x00,				// number of authentication method
+		0x00, 0x50, 0xf2, 0x01	// authentication
+		};
+UCHAR	CipherSuiteCCXTkipLen = (sizeof(CipherSuiteCCXTkip) / sizeof(UCHAR));
+
+UCHAR	CCX_LLC_HDR[] = {0xAA, 0xAA, 0x03, 0x00, 0x40, 0x96, 0x00, 0x02};
+UCHAR	LLC_NORMAL[] = {0xAA, 0xAA, 0x03, 0x00, 0x00, 0x00};
+
+UCHAR	EAPOL_FRAME[] = {0x88, 0x8E};
+
+BOOLEAN CheckRSNIE(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pData,
+	IN  UCHAR           DataLen,
+	OUT	UCHAR			*Offset);
+
+void inc_byte_array(UCHAR *counter, int len);
+
+/*
+	========================================================================
+
+	Routine Description:
+		Classify WPA EAP message type
+
+	Arguments:
+		EAPType		Value of EAP message type
+		MsgType		Internal Message definition for MLME state machine
+
+	Return Value:
+		TRUE		Found appropriate message type
+		FALSE		No appropriate message type
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+		All these constants are defined in wpa.h
+		For supplicant, there is only EAPOL Key message avaliable
+
+	========================================================================
+*/
+BOOLEAN	WpaMsgTypeSubst(
+	IN	UCHAR	EAPType,
+	OUT	INT		*MsgType)
+{
+	switch (EAPType)
+	{
+		case EAPPacket:
+			*MsgType = MT2_EAPPacket;
+			break;
+		case EAPOLStart:
+			*MsgType = MT2_EAPOLStart;
+			break;
+		case EAPOLLogoff:
+			*MsgType = MT2_EAPOLLogoff;
+			break;
+		case EAPOLKey:
+			*MsgType = MT2_EAPOLKey;
+			break;
+		case EAPOLASFAlert:
+			*MsgType = MT2_EAPOLASFAlert;
+			break;
+		default:
+			return FALSE;
+	}
+	return TRUE;
+}
+
+/*
+	==========================================================================
+	Description:
+		association	state machine init,	including state	transition and timer init
+	Parameters:
+		S -	pointer	to the association state machine
+	==========================================================================
+ */
+VOID WpaPskStateMachineInit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	STATE_MACHINE *S,
+	OUT	STATE_MACHINE_FUNC Trans[])
+{
+	StateMachineInit(S,	Trans, MAX_WPA_PSK_STATE, MAX_WPA_PSK_MSG, (STATE_MACHINE_FUNC)Drop, WPA_PSK_IDLE, WPA_MACHINE_BASE);
+	StateMachineSetAction(S, WPA_PSK_IDLE, MT2_EAPOLKey, (STATE_MACHINE_FUNC)WpaEAPOLKeyAction);
+}
+
+/*
+	==========================================================================
+	Description:
+		This is	state machine function.
+		When receiving EAPOL packets which is  for 802.1x key management.
+		Use	both in	WPA, and WPAPSK	case.
+		In this	function, further dispatch to different	functions according	to the received	packet.	 3 categories are :
+		  1.  normal 4-way pairwisekey and 2-way groupkey handshake
+		  2.  MIC error	(Countermeasures attack)  report packet	from STA.
+		  3.  Request for pairwise/group key update	from STA
+	Return:
+	==========================================================================
+*/
+VOID WpaEAPOLKeyAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MLME_QUEUE_ELEM	*Elem)
+
+{
+	INT             MsgType = EAPOL_MSG_INVALID;
+	PKEY_DESCRIPTER pKeyDesc;
+	PHEADER_802_11  pHeader; //red
+	UCHAR           ZeroReplay[LEN_KEY_DESC_REPLAY];
+	UCHAR EapolVr;
+	KEY_INFO		peerKeyInfo;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("-----> WpaEAPOLKeyAction\n"));
+
+	// Get 802.11 header first
+	pHeader = (PHEADER_802_11) Elem->Msg;
+
+	// Get EAPoL-Key Descriptor
+	pKeyDesc = (PKEY_DESCRIPTER) &Elem->Msg[(LENGTH_802_11 + LENGTH_802_1_H + LENGTH_EAPOL_H)];
+
+	NdisZeroMemory((PUCHAR)&peerKeyInfo, sizeof(peerKeyInfo));
+	NdisMoveMemory((PUCHAR)&peerKeyInfo, (PUCHAR)&pKeyDesc->KeyInfo, sizeof(KEY_INFO));
+
+	*((USHORT *)&peerKeyInfo) = cpu2le16(*((USHORT *)&peerKeyInfo));
+
+
+	// 1. Check EAPOL frame version and type
+	EapolVr	= (UCHAR) Elem->Msg[LENGTH_802_11+LENGTH_802_1_H];
+
+    if (((EapolVr != EAPOL_VER) && (EapolVr != EAPOL_VER2)) || ((pKeyDesc->Type != WPA1_KEY_DESC) && (pKeyDesc->Type != WPA2_KEY_DESC)))
+	{
+        DBGPRINT(RT_DEBUG_ERROR, ("Key descripter does not match with WPA rule\n"));
+			return;
+	}
+
+	// First validate replay counter, only accept message with larger replay counter
+	// Let equal pass, some AP start with all zero replay counter
+	NdisZeroMemory(ZeroReplay, LEN_KEY_DESC_REPLAY);
+
+	if((RTMPCompareMemory(pKeyDesc->ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY) != 1) &&
+		(RTMPCompareMemory(pKeyDesc->ReplayCounter, ZeroReplay, LEN_KEY_DESC_REPLAY) != 0))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("   ReplayCounter not match   \n"));
+		return;
+	}
+
+	// Process WPA2PSK frame
+	if(pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
+	{
+		if((peerKeyInfo.KeyType == PAIRWISEKEY) &&
+			(peerKeyInfo.EKD_DL == 0) &&
+			(peerKeyInfo.KeyAck == 1) &&
+			(peerKeyInfo.KeyMic == 0) &&
+			(peerKeyInfo.Secure == 0) &&
+			(peerKeyInfo.Error == 0) &&
+			(peerKeyInfo.Request == 0))
+		{
+			MsgType = EAPOL_PAIR_MSG_1;
+			DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL Key Pairwise Message 1\n"));
+		} else if((peerKeyInfo.KeyType == PAIRWISEKEY) &&
+			(peerKeyInfo.EKD_DL  == 1) &&
+			(peerKeyInfo.KeyAck == 1) &&
+			(peerKeyInfo.KeyMic == 1) &&
+			(peerKeyInfo.Secure == 1) &&
+			(peerKeyInfo.Error == 0) &&
+			(peerKeyInfo.Request == 0))
+		{
+			MsgType = EAPOL_PAIR_MSG_3;
+			DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL Key Pairwise Message 3\n"));
+		} else if((peerKeyInfo.KeyType == GROUPKEY) &&
+			(peerKeyInfo.EKD_DL == 1) &&
+			(peerKeyInfo.KeyAck == 1) &&
+			(peerKeyInfo.KeyMic == 1) &&
+			(peerKeyInfo.Secure == 1) &&
+			(peerKeyInfo.Error == 0) &&
+			(peerKeyInfo.Request == 0))
+		{
+			MsgType = EAPOL_GROUP_MSG_1;
+			DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL Key Group Message 1\n"));
+		}
+
+		// We will assume link is up (assoc suceess and port not secured).
+		// All state has to be able to process message from previous state
+		switch(pAd->StaCfg.WpaState)
+		{
+		case SS_START:
+			if(MsgType == EAPOL_PAIR_MSG_1)
+			{
+				Wpa2PairMsg1Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_MSG_3;
+			}
+			break;
+
+		case SS_WAIT_MSG_3:
+			if(MsgType == EAPOL_PAIR_MSG_1)
+			{
+				Wpa2PairMsg1Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_MSG_3;
+			}
+			else if(MsgType == EAPOL_PAIR_MSG_3)
+			{
+				Wpa2PairMsg3Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_GROUP;
+			}
+			break;
+
+		case SS_WAIT_GROUP:		// When doing group key exchange
+		case SS_FINISH:			// This happened when update group key
+			if(MsgType == EAPOL_PAIR_MSG_1)
+			{
+			    // Reset port secured variable
+				pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+				Wpa2PairMsg1Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_MSG_3;
+			}
+			else if(MsgType == EAPOL_PAIR_MSG_3)
+			{
+			    // Reset port secured variable
+				pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+				Wpa2PairMsg3Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_GROUP;
+			}
+			else if(MsgType == EAPOL_GROUP_MSG_1)
+			{
+				WpaGroupMsg1Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_FINISH;
+			}
+			break;
+
+		default:
+			break;
+		}
+	}
+	// Process WPAPSK Frame
+	// Classify message Type, either pairwise message 1, 3, or group message 1 for supplicant
+	else if(pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK)
+	{
+		if((peerKeyInfo.KeyType == PAIRWISEKEY) &&
+			(peerKeyInfo.KeyIndex == 0) &&
+			(peerKeyInfo.KeyAck == 1) &&
+			(peerKeyInfo.KeyMic == 0) &&
+			(peerKeyInfo.Secure == 0) &&
+			(peerKeyInfo.Error == 0) &&
+			(peerKeyInfo.Request == 0))
+		{
+			MsgType = EAPOL_PAIR_MSG_1;
+			DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL Key Pairwise Message 1\n"));
+		}
+		else if((peerKeyInfo.KeyType == PAIRWISEKEY) &&
+			(peerKeyInfo.KeyIndex == 0) &&
+			(peerKeyInfo.KeyAck == 1) &&
+			(peerKeyInfo.KeyMic == 1) &&
+			(peerKeyInfo.Secure == 0) &&
+			(peerKeyInfo.Error == 0) &&
+			(peerKeyInfo.Request == 0))
+		{
+			MsgType = EAPOL_PAIR_MSG_3;
+			DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL Key Pairwise Message 3\n"));
+		}
+		else if((peerKeyInfo.KeyType == GROUPKEY) &&
+			(peerKeyInfo.KeyIndex != 0) &&
+			(peerKeyInfo.KeyAck == 1) &&
+			(peerKeyInfo.KeyMic == 1) &&
+			(peerKeyInfo.Secure == 1) &&
+			(peerKeyInfo.Error == 0) &&
+			(peerKeyInfo.Request == 0))
+		{
+			MsgType = EAPOL_GROUP_MSG_1;
+			DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL Key Group Message 1\n"));
+		}
+
+		// We will assume link is up (assoc suceess and port not secured).
+		// All state has to be able to process message from previous state
+		switch(pAd->StaCfg.WpaState)
+		{
+		case SS_START:
+			if(MsgType == EAPOL_PAIR_MSG_1)
+			{
+				WpaPairMsg1Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_MSG_3;
+			}
+			break;
+
+		case SS_WAIT_MSG_3:
+			if(MsgType == EAPOL_PAIR_MSG_1)
+			{
+				WpaPairMsg1Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_MSG_3;
+			}
+			else if(MsgType == EAPOL_PAIR_MSG_3)
+			{
+				WpaPairMsg3Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_GROUP;
+			}
+			break;
+
+		case SS_WAIT_GROUP:		// When doing group key exchange
+		case SS_FINISH:			// This happened when update group key
+			if(MsgType == EAPOL_PAIR_MSG_1)
+			{
+				WpaPairMsg1Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_MSG_3;
+				// Reset port secured variable
+				pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+			}
+			else if(MsgType == EAPOL_PAIR_MSG_3)
+			{
+				WpaPairMsg3Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_GROUP;
+				// Reset port secured variable
+				pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+			}
+			else if(MsgType == EAPOL_GROUP_MSG_1)
+			{
+				WpaGroupMsg1Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_FINISH;
+			}
+			break;
+
+		default:
+			break;
+		}
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<----- WpaEAPOLKeyAction\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Process Pairwise key 4-way handshaking
+
+	Arguments:
+		pAd	Pointer	to our adapter
+		Elem		Message body
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	WpaPairMsg1Action(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem)
+{
+	PHEADER_802_11      pHeader;
+	UCHAR				*mpool, *PTK, *digest;
+	PUCHAR              pOutBuffer = NULL;
+	UCHAR               Header802_3[14];
+	ULONG               FrameLen = 0;
+	PEAPOL_PACKET       pMsg1;
+	EAPOL_PACKET        Packet;
+	UCHAR               Mic[16];
+
+	DBGPRINT(RT_DEBUG_TRACE, ("WpaPairMsg1Action ----->\n"));
+
+	// allocate memory pool
+	os_alloc_mem(pAd, (PUCHAR *)&mpool, 256);
+
+	if (mpool == NULL)
+		return;
+
+	// PTK Len = 80.
+	PTK = (UCHAR *) ROUND_UP(mpool, 4);
+	// digest Len = 80.
+	digest = (UCHAR *) ROUND_UP(PTK + 80, 4);
+
+	pHeader = (PHEADER_802_11) Elem->Msg;
+
+	// Process message 1 from authenticator
+	pMsg1 = (PEAPOL_PACKET) &Elem->Msg[LENGTH_802_11 + LENGTH_802_1_H];
+
+	// 1. Save Replay counter, it will use to verify message 3 and construct message 2
+	NdisMoveMemory(pAd->StaCfg.ReplayCounter, pMsg1->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// 2. Save ANonce
+	NdisMoveMemory(pAd->StaCfg.ANonce, pMsg1->KeyDesc.KeyNonce, LEN_KEY_DESC_NONCE);
+
+	// Generate random SNonce
+	GenRandom(pAd, pAd->CurrentAddress, pAd->StaCfg.SNonce);
+
+	// Calc PTK(ANonce, SNonce)
+	WpaCountPTK(pAd,
+		pAd->StaCfg.PMK,
+		pAd->StaCfg.ANonce,
+		pAd->CommonCfg.Bssid,
+		pAd->StaCfg.SNonce,
+		pAd->CurrentAddress,
+		PTK,
+		LEN_PTK);
+
+	// Save key to PTK entry
+	NdisMoveMemory(pAd->StaCfg.PTK, PTK, LEN_PTK);
+
+	// init 802.3 header and Fill Packet
+	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
+
+	// Zero Message 2 body
+	NdisZeroMemory(&Packet, sizeof(Packet));
+	Packet.ProVer	= EAPOL_VER;
+	Packet.ProType	= EAPOLKey;
+	//
+	// Message 2 as  EAPOL-Key(0,1,0,0,0,P,0,SNonce,MIC,RSN IE)
+	//
+	Packet.KeyDesc.Type = WPA1_KEY_DESC;
+	// 1. Key descriptor version and appropriate RSN IE
+	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
+	{
+		Packet.KeyDesc.KeyInfo.KeyDescVer = 2;
+	}
+	else	  // TKIP
+	{
+		Packet.KeyDesc.KeyInfo.KeyDescVer = 1;
+	}
+
+	// fill in Data Material and its length
+	Packet.KeyDesc.KeyData[0] = IE_WPA;
+	Packet.KeyDesc.KeyData[1] = pAd->StaCfg.RSNIE_Len;
+	Packet.KeyDesc.KeyDataLen[1] = pAd->StaCfg.RSNIE_Len + 2;
+	NdisMoveMemory(&Packet.KeyDesc.KeyData[2], pAd->StaCfg.RSN_IE, pAd->StaCfg.RSNIE_Len);
+
+	// Update packet length after decide Key data payload
+	Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE + Packet.KeyDesc.KeyDataLen[1];
+
+	// Update Key length
+	Packet.KeyDesc.KeyLength[0] = pMsg1->KeyDesc.KeyLength[0];
+	Packet.KeyDesc.KeyLength[1] = pMsg1->KeyDesc.KeyLength[1];
+	// 2. Key Type PeerKey
+	Packet.KeyDesc.KeyInfo.KeyType = PAIRWISEKEY;
+
+	// 3. KeyMic field presented
+	Packet.KeyDesc.KeyInfo.KeyMic  = 1;
+
+	//Convert to little-endian format.
+	*((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));
+
+
+	// 4. Fill SNonce
+	NdisMoveMemory(Packet.KeyDesc.KeyNonce, pAd->StaCfg.SNonce, LEN_KEY_DESC_NONCE);
+
+	// 5. Key Replay Count
+	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// Send EAPOL(0, 1, 0, 0, 0, P, 0, SNonce, MIC, RSN_IE)
+	// Out buffer for transmitting message 2
+	MlmeAllocateMemory(pAd, (PUCHAR *)&pOutBuffer);  // allocate memory
+	if(pOutBuffer == NULL)
+	{
+		os_free_mem(pAd, mpool);
+		return;
+	}
+	// Prepare EAPOL frame for MIC calculation
+	// Be careful, only EAPOL frame is counted for MIC calculation
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+		Packet.Body_Len[1] + 4,    &Packet,
+		END_OF_ARGS);
+
+	// 6. Prepare and Fill MIC value
+	NdisZeroMemory(Mic, sizeof(Mic));
+	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
+	{	// AES
+
+		HMAC_SHA1(pOutBuffer, FrameLen, PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{	// TKIP
+		hmac_md5(PTK,  LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
+	}
+	NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
+
+	//hex_dump("MIC", Mic, LEN_KEY_DESC_MIC);
+
+		MakeOutgoingFrame(pOutBuffer,           	&FrameLen,
+			  			LENGTH_802_3,     			&Header802_3,
+						Packet.Body_Len[1] + 4,    &Packet,
+						END_OF_ARGS);
+
+
+	// 5. Copy frame to Tx ring and send Msg 2 to authenticator
+	RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, TRUE);
+
+	MlmeFreeMemory(pAd, (PUCHAR)pOutBuffer);
+	os_free_mem(pAd, (PUCHAR)mpool);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("WpaPairMsg1Action <-----\n"));
+}
+
+VOID Wpa2PairMsg1Action(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem)
+{
+	PHEADER_802_11      pHeader;
+	UCHAR				*mpool, *PTK, *digest;
+	PUCHAR              pOutBuffer = NULL;
+	UCHAR               Header802_3[14];
+	ULONG               FrameLen = 0;
+	PEAPOL_PACKET       pMsg1;
+	EAPOL_PACKET        Packet;
+	UCHAR               Mic[16];
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Wpa2PairMsg1Action ----->\n"));
+
+	// allocate memory pool
+	os_alloc_mem(pAd, (PUCHAR *)&mpool, 256);
+
+	if (mpool == NULL)
+		return;
+
+	// PTK Len = 80.
+	PTK = (UCHAR *) ROUND_UP(mpool, 4);
+	// digest Len = 80.
+	digest = (UCHAR *) ROUND_UP(PTK + 80, 4);
+
+	pHeader = (PHEADER_802_11) Elem->Msg;
+
+	// Process message 1 from authenticator
+		pMsg1 = (PEAPOL_PACKET)	&Elem->Msg[LENGTH_802_11 + LENGTH_802_1_H];
+
+	// 1. Save Replay counter, it will use to verify message 3 and construct message 2
+	NdisMoveMemory(pAd->StaCfg.ReplayCounter, pMsg1->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// 2. Save ANonce
+	NdisMoveMemory(pAd->StaCfg.ANonce, pMsg1->KeyDesc.KeyNonce, LEN_KEY_DESC_NONCE);
+
+	// Generate random SNonce
+	GenRandom(pAd, pAd->CurrentAddress, pAd->StaCfg.SNonce);
+
+	if(pMsg1->KeyDesc.KeyDataLen[1] > 0 )
+	{
+		// cached PMKID
+	}
+
+	// Calc PTK(ANonce, SNonce)
+	WpaCountPTK(pAd,
+		pAd->StaCfg.PMK,
+		pAd->StaCfg.ANonce,
+		pAd->CommonCfg.Bssid,
+		pAd->StaCfg.SNonce,
+		pAd->CurrentAddress,
+		PTK,
+		LEN_PTK);
+
+	// Save key to PTK entry
+	NdisMoveMemory(pAd->StaCfg.PTK, PTK, LEN_PTK);
+
+	// init 802.3 header and Fill Packet
+	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
+
+	// Zero message 2 body
+	NdisZeroMemory(&Packet, sizeof(Packet));
+	Packet.ProVer	= EAPOL_VER;
+	Packet.ProType	= EAPOLKey;
+	//
+	// Message 2 as  EAPOL-Key(0,1,0,0,0,P,0,SNonce,MIC,RSN IE)
+	//
+	Packet.KeyDesc.Type = WPA2_KEY_DESC;
+
+	// 1. Key descriptor version and appropriate RSN IE
+	if(pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+	{
+		Packet.KeyDesc.KeyInfo.KeyDescVer = 2;
+	}
+	else	  // TKIP
+	{
+		Packet.KeyDesc.KeyInfo.KeyDescVer = 1;
+	}
+
+	// fill in Data Material and its length
+	Packet.KeyDesc.KeyData[0] = IE_WPA2;
+	Packet.KeyDesc.KeyData[1] = pAd->StaCfg.RSNIE_Len;
+	Packet.KeyDesc.KeyDataLen[1] = pAd->StaCfg.RSNIE_Len + 2;
+	NdisMoveMemory(&Packet.KeyDesc.KeyData[2], pAd->StaCfg.RSN_IE, pAd->StaCfg.RSNIE_Len);
+
+	// Update packet length after decide Key data payload
+	Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE + Packet.KeyDesc.KeyDataLen[1];
+
+	// 2. Key Type PeerKey
+	Packet.KeyDesc.KeyInfo.KeyType = PAIRWISEKEY;
+
+	// 3. KeyMic field presented
+	Packet.KeyDesc.KeyInfo.KeyMic  = 1;
+
+	// Update Key Length
+	Packet.KeyDesc.KeyLength[0] = 0;
+	Packet.KeyDesc.KeyLength[1] = pMsg1->KeyDesc.KeyLength[1];
+
+	// 4. Fill SNonce
+	NdisMoveMemory(Packet.KeyDesc.KeyNonce, pAd->StaCfg.SNonce, LEN_KEY_DESC_NONCE);
+
+	// 5. Key Replay Count
+	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// Convert to little-endian format.
+	*((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));
+
+	// Send EAPOL-Key(0,1,0,0,0,P,0,SNonce,MIC,RSN IE)
+	// Out buffer for transmitting message 2
+	MlmeAllocateMemory(pAd,  (PUCHAR *)&pOutBuffer);  // allocate memory
+	if(pOutBuffer == NULL)
+	{
+		os_free_mem(pAd, mpool);
+		return;
+	}
+
+	// Prepare EAPOL frame for MIC calculation
+	// Be careful, only EAPOL frame is counted for MIC calculation
+	MakeOutgoingFrame(pOutBuffer,        &FrameLen,
+		Packet.Body_Len[1] + 4, &Packet,
+		END_OF_ARGS);
+
+	// 6. Prepare and Fill MIC value
+	NdisZeroMemory(Mic, sizeof(Mic));
+	if(pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+	{
+		// AES
+		HMAC_SHA1(pOutBuffer, FrameLen, PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{
+		hmac_md5(PTK,  LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
+	}
+	NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
+
+
+	// Make  Transmitting frame
+	MakeOutgoingFrame(pOutBuffer,           	&FrameLen,
+			  			LENGTH_802_3,     		&Header802_3,
+						Packet.Body_Len[1] + 4, &Packet,
+						END_OF_ARGS);
+
+
+	// 5. Copy frame to Tx ring
+	RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, TRUE);
+
+	MlmeFreeMemory(pAd, pOutBuffer);
+	os_free_mem(pAd, mpool);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Wpa2PairMsg1Action <-----\n"));
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Process Pairwise key 4-way handshaking
+
+	Arguments:
+		pAd	Pointer	to our adapter
+		Elem		Message body
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	WpaPairMsg3Action(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MLME_QUEUE_ELEM	*Elem)
+
+{
+	PHEADER_802_11      pHeader;
+	PUCHAR          	pOutBuffer = NULL;
+	UCHAR               Header802_3[14];
+	ULONG           	FrameLen = 0;
+	EAPOL_PACKET        Packet;
+	PEAPOL_PACKET       pMsg3;
+	UCHAR           	Mic[16], OldMic[16];
+	MAC_TABLE_ENTRY 	*pEntry = NULL;
+	UCHAR				skip_offset;
+	KEY_INFO			peerKeyInfo;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("WpaPairMsg3Action ----->\n"));
+
+	// Record 802.11 header & the received EAPOL packet Msg3
+	pHeader = (PHEADER_802_11) Elem->Msg;
+	pMsg3 = (PEAPOL_PACKET)	&Elem->Msg[LENGTH_802_11 + LENGTH_802_1_H];
+
+	NdisZeroMemory((PUCHAR)&peerKeyInfo, sizeof(peerKeyInfo));
+	NdisMoveMemory((PUCHAR)&peerKeyInfo, (PUCHAR)&pMsg3->KeyDesc.KeyInfo, sizeof(KEY_INFO));
+
+	*((USHORT*)&peerKeyInfo) = cpu2le16(*((USHORT*)&peerKeyInfo));
+
+
+	// 1. Verify cipher type match
+	if (pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled && (peerKeyInfo.KeyDescVer != 2))
+	{
+		return;
+	}
+	else if(pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled && (peerKeyInfo.KeyDescVer != 1))
+	{
+		return;
+	}
+
+	// Verify RSN IE
+	//if (!RTMPEqualMemory(pMsg3->KeyDesc.KeyData, pAd->MacTab.Content[BSSID_WCID].RSN_IE, pAd->MacTab.Content[BSSID_WCID].RSNIE_Len))
+	if (!CheckRSNIE(pAd, pMsg3->KeyDesc.KeyData, pMsg3->KeyDesc.KeyDataLen[1], &skip_offset))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("RSN_IE Different in Msg 3 of WPA1 4-way handshake!! \n"));
+		hex_dump("The original RSN_IE", pAd->MacTab.Content[BSSID_WCID].RSN_IE, pAd->MacTab.Content[BSSID_WCID].RSNIE_Len);
+		hex_dump("The received RSN_IE", pMsg3->KeyDesc.KeyData, pMsg3->KeyDesc.KeyDataLen[1]);
+		return;
+	}
+	else
+		DBGPRINT(RT_DEBUG_TRACE, ("RSN_IE VALID in Msg 3 of WPA1 4-way handshake!! \n"));
+
+
+	// 2. Check MIC value
+	// Save the MIC and replace with zero
+	NdisMoveMemory(OldMic, pMsg3->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+	NdisZeroMemory(pMsg3->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
+	{
+		// AES
+		UCHAR digest[80];
+
+		HMAC_SHA1((PUCHAR) pMsg3, pMsg3->Body_Len[1] + 4, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else	// TKIP
+	{
+		hmac_md5(pAd->StaCfg.PTK, LEN_EAP_MICK, (PUCHAR) pMsg3, pMsg3->Body_Len[1] + 4, Mic);
+	}
+
+	if(!NdisEqualMemory(OldMic, Mic, LEN_KEY_DESC_MIC))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, (" MIC Different in msg 3 of 4-way handshake!!!!!!!!!! \n"));
+		return;
+	}
+	else
+		DBGPRINT(RT_DEBUG_TRACE, (" MIC VALID in msg 3 of 4-way handshake!!!!!!!!!! \n"));
+
+	// 3. Check Replay Counter, it has to be larger than last one. No need to be exact one larger
+	if(RTMPCompareMemory(pMsg3->KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY) != 1)
+		return;
+
+	// Update new replay counter
+	NdisMoveMemory(pAd->StaCfg.ReplayCounter, pMsg3->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// 4. Double check ANonce
+	if(!NdisEqualMemory(pAd->StaCfg.ANonce, pMsg3->KeyDesc.KeyNonce, LEN_KEY_DESC_NONCE))
+		return;
+
+	// init 802.3 header and Fill Packet
+	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
+
+	// Zero Message 4 body
+	NdisZeroMemory(&Packet, sizeof(Packet));
+	Packet.ProVer	= EAPOL_VER;
+	Packet.ProType	= EAPOLKey;
+	Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE;		// No data field
+
+	//
+	// Message 4 as  EAPOL-Key(0,1,0,0,0,P,0,0,MIC,0)
+	//
+	Packet.KeyDesc.Type = WPA1_KEY_DESC;
+
+	// Key descriptor version and appropriate RSN IE
+	Packet.KeyDesc.KeyInfo.KeyDescVer = peerKeyInfo.KeyDescVer;
+
+	// Update Key Length
+	Packet.KeyDesc.KeyLength[0] = pMsg3->KeyDesc.KeyLength[0];
+	Packet.KeyDesc.KeyLength[1] = pMsg3->KeyDesc.KeyLength[1];
+
+	// Key Type PeerKey
+	Packet.KeyDesc.KeyInfo.KeyType = PAIRWISEKEY;
+
+	// KeyMic field presented
+	Packet.KeyDesc.KeyInfo.KeyMic  = 1;
+
+	// In Msg3,  KeyInfo.secure =0 if Group Key HS to come. 1 if no group key HS
+	// Station sends Msg4  KeyInfo.secure should be the same as that in Msg.3
+	Packet.KeyDesc.KeyInfo.Secure= peerKeyInfo.Secure;
+
+	// Convert to little-endian format.
+	*((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));
+
+	// Key Replay count
+	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pMsg3->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// Out buffer for transmitting message 4
+	MlmeAllocateMemory(pAd, (PUCHAR *)&pOutBuffer);  // allocate memory
+	if(pOutBuffer == NULL)
+		return;
+
+	// Prepare EAPOL frame for MIC calculation
+	// Be careful, only EAPOL frame is counted for MIC calculation
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+		Packet.Body_Len[1] + 4,    &Packet,
+		END_OF_ARGS);
+
+	// Prepare and Fill MIC value
+	NdisZeroMemory(Mic, sizeof(Mic));
+	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
+	{
+		// AES
+		UCHAR digest[80];
+
+		HMAC_SHA1(pOutBuffer, FrameLen, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{
+		hmac_md5(pAd->StaCfg.PTK, LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
+	}
+	NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
+
+	// Update PTK
+	// Prepare pair-wise key information into shared key table
+	NdisZeroMemory(&pAd->SharedKey[BSS0][0], sizeof(CIPHER_KEY));
+	pAd->SharedKey[BSS0][0].KeyLen = LEN_TKIP_EK;
+    NdisMoveMemory(pAd->SharedKey[BSS0][0].Key, &pAd->StaCfg.PTK[32], LEN_TKIP_EK);
+	NdisMoveMemory(pAd->SharedKey[BSS0][0].RxMic, &pAd->StaCfg.PTK[48], LEN_TKIP_RXMICK);
+	NdisMoveMemory(pAd->SharedKey[BSS0][0].TxMic, &pAd->StaCfg.PTK[48+LEN_TKIP_RXMICK], LEN_TKIP_TXMICK);
+
+	// Decide its ChiperAlg
+	if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
+		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_TKIP;
+	else if (pAd->StaCfg.PairCipher == Ndis802_11Encryption3Enabled)
+		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_AES;
+	else
+		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_NONE;
+
+	// Update these related information to MAC_TABLE_ENTRY
+	pEntry = &pAd->MacTab.Content[BSSID_WCID];
+	NdisMoveMemory(pEntry->PairwiseKey.Key, &pAd->StaCfg.PTK[32], LEN_TKIP_EK);
+	NdisMoveMemory(pEntry->PairwiseKey.RxMic, &pAd->StaCfg.PTK[48], LEN_TKIP_RXMICK);
+	NdisMoveMemory(pEntry->PairwiseKey.TxMic, &pAd->StaCfg.PTK[48+LEN_TKIP_RXMICK], LEN_TKIP_TXMICK);
+	pEntry->PairwiseKey.CipherAlg = pAd->SharedKey[BSS0][0].CipherAlg;
+
+	// Update pairwise key information to ASIC Shared Key Table
+	AsicAddSharedKeyEntry(pAd,
+						  BSS0,
+						  0,
+						  pAd->SharedKey[BSS0][0].CipherAlg,
+						  pAd->SharedKey[BSS0][0].Key,
+						  pAd->SharedKey[BSS0][0].TxMic,
+						  pAd->SharedKey[BSS0][0].RxMic);
+
+	// Update ASIC WCID attribute table and IVEIV table
+	RTMPAddWcidAttributeEntry(pAd,
+							  BSS0,
+							  0,
+							  pAd->SharedKey[BSS0][0].CipherAlg,
+							  pEntry);
+
+	// Make transmitting frame
+	MakeOutgoingFrame(pOutBuffer,           	&FrameLen,
+			  			LENGTH_802_3,     		&Header802_3,
+						Packet.Body_Len[1] + 4, &Packet,
+						END_OF_ARGS);
+
+
+	// Copy frame to Tx ring and Send Message 4 to authenticator
+	RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, TRUE);
+
+	MlmeFreeMemory(pAd, (PUCHAR)pOutBuffer);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("WpaPairMsg3Action <-----\n"));
+}
+
+VOID    Wpa2PairMsg3Action(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem)
+
+{
+	PHEADER_802_11      pHeader;
+	PUCHAR              pOutBuffer = NULL;
+	UCHAR               Header802_3[14];
+	ULONG               FrameLen = 0;
+	EAPOL_PACKET        Packet;
+	PEAPOL_PACKET       pMsg3;
+	UCHAR               Mic[16], OldMic[16];
+	UCHAR               *mpool, *KEYDATA, *digest;
+	UCHAR               Key[32];
+	MAC_TABLE_ENTRY 	*pEntry = NULL;
+	KEY_INFO			peerKeyInfo;
+
+	// allocate memory
+	os_alloc_mem(pAd, (PUCHAR *)&mpool, 1024);
+
+	if(mpool == NULL)
+		return;
+
+	// KEYDATA Len = 512.
+	KEYDATA = (UCHAR *) ROUND_UP(mpool, 4);
+	// digest Len = 80.
+	digest = (UCHAR *) ROUND_UP(KEYDATA + 512, 4);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Wpa2PairMsg3Action ----->\n"));
+
+	pHeader = (PHEADER_802_11) Elem->Msg;
+
+	// Process message 3 frame.
+	pMsg3 = (PEAPOL_PACKET)	&Elem->Msg[LENGTH_802_11 + LENGTH_802_1_H];
+
+	NdisZeroMemory((PUCHAR)&peerKeyInfo, sizeof(peerKeyInfo));
+	NdisMoveMemory((PUCHAR)&peerKeyInfo, (PUCHAR)&pMsg3->KeyDesc.KeyInfo, sizeof(KEY_INFO));
+
+	*((USHORT*)&peerKeyInfo) = cpu2le16(*((USHORT*)&peerKeyInfo));
+
+	// 1. Verify cipher type match
+	if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled && (peerKeyInfo.KeyDescVer!= 2))
+	{
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+	else if(pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled && (peerKeyInfo.KeyDescVer != 1))
+	{
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+
+	// 2. Check MIC value
+	// Save the MIC and replace with zero
+	NdisMoveMemory(OldMic, pMsg3->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+	NdisZeroMemory(pMsg3->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+	if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+	{
+		// AES
+		HMAC_SHA1((PUCHAR) pMsg3, pMsg3->Body_Len[1] + 4, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{
+		hmac_md5(pAd->StaCfg.PTK, LEN_EAP_MICK, (PUCHAR) pMsg3, pMsg3->Body_Len[1] + 4, Mic);
+	}
+
+	if(!NdisEqualMemory(OldMic, Mic, LEN_KEY_DESC_MIC))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, (" MIC Different in msg 3 of 4-way handshake!!!!!!!!!! \n"));
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+	else
+		DBGPRINT(RT_DEBUG_TRACE, (" MIC VALID in msg 3 of 4-way handshake!!!!!!!!!! \n"));
+
+	// 3. Check Replay Counter, it has to be larger than last one. No need to be exact one larger
+	if(RTMPCompareMemory(pMsg3->KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY) != 1)
+	{
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+
+	// Update new replay counter
+	NdisMoveMemory(pAd->StaCfg.ReplayCounter, pMsg3->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// 4. Double check ANonce
+	if(!NdisEqualMemory(pAd->StaCfg.ANonce, pMsg3->KeyDesc.KeyNonce, LEN_KEY_DESC_NONCE))
+	{
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+
+	// Obtain GTK
+	// 5. Decrypt GTK from Key Data
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("EKD = %d\n", peerKeyInfo.EKD_DL));
+	if(pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+	{
+		// Decrypt AES GTK
+		AES_GTK_KEY_UNWRAP(&pAd->StaCfg.PTK[16], KEYDATA, pMsg3->KeyDesc.KeyDataLen[1],pMsg3->KeyDesc.KeyData);
+	}
+	else	  // TKIP
+	{
+		INT i;
+		// Decrypt TKIP GTK
+		// Construct 32 bytes RC4 Key
+		NdisMoveMemory(Key, pMsg3->KeyDesc.KeyIv, 16);
+		NdisMoveMemory(&Key[16], &pAd->StaCfg.PTK[16], 16);
+		ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, Key, 32);
+		//discard first 256 bytes
+		for(i = 0; i < 256; i++)
+			ARCFOUR_BYTE(&pAd->PrivateInfo.WEPCONTEXT);
+		// Decrypt GTK. Becareful, there is no ICV to check the result is correct or not
+		ARCFOUR_DECRYPT(&pAd->PrivateInfo.WEPCONTEXT, KEYDATA, pMsg3->KeyDesc.KeyData, pMsg3->KeyDesc.KeyDataLen[1]);
+	}
+
+	if (!ParseKeyData(pAd, KEYDATA, pMsg3->KeyDesc.KeyDataLen[1], 1))
+	{
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+
+	// Update GTK to ASIC
+	// Update group key information to ASIC Shared Key Table
+	AsicAddSharedKeyEntry(pAd,
+						  BSS0,
+						  pAd->StaCfg.DefaultKeyId,
+						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
+						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key,
+						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic,
+						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic);
+
+	// Update ASIC WCID attribute table and IVEIV table
+	RTMPAddWcidAttributeEntry(pAd,
+							  BSS0,
+							  pAd->StaCfg.DefaultKeyId,
+							  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
+							  NULL);
+
+	// init 802.3 header and Fill Packet
+	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
+
+	// Zero message 4 body
+	NdisZeroMemory(&Packet, sizeof(Packet));
+	Packet.ProVer	= EAPOL_VER;
+	Packet.ProType	= EAPOLKey;
+	Packet.Body_Len[1]  	= sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE;		// No data field
+
+	//
+	// Message 4 as  EAPOL-Key(0,1,0,0,0,P,0,0,MIC,0)
+	//
+	Packet.KeyDesc.Type = WPA2_KEY_DESC;
+
+	// Key descriptor version and appropriate RSN IE
+	Packet.KeyDesc.KeyInfo.KeyDescVer = peerKeyInfo.KeyDescVer;
+
+	// Update Key Length
+	Packet.KeyDesc.KeyLength[0] = pMsg3->KeyDesc.KeyLength[0];
+	Packet.KeyDesc.KeyLength[1] = pMsg3->KeyDesc.KeyLength[1];
+
+	// Key Type PeerKey
+	Packet.KeyDesc.KeyInfo.KeyType = PAIRWISEKEY;
+
+	// KeyMic field presented
+	Packet.KeyDesc.KeyInfo.KeyMic  = 1;
+	Packet.KeyDesc.KeyInfo.Secure = 1;
+
+	// Convert to little-endian format.
+	*((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));
+
+	// Key Replay count
+	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pMsg3->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// Out buffer for transmitting message 4
+	MlmeAllocateMemory(pAd, (PUCHAR *)&pOutBuffer);  // allocate memory
+	if(pOutBuffer == NULL)
+	{
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+
+	// Prepare EAPOL frame for MIC calculation
+	// Be careful, only EAPOL frame is counted for MIC calculation
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+		Packet.Body_Len[1] + 4,    &Packet,
+		END_OF_ARGS);
+
+	// Prepare and Fill MIC value
+	NdisZeroMemory(Mic, sizeof(Mic));
+	if(pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+	{
+		// AES
+		HMAC_SHA1(pOutBuffer, FrameLen, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{
+		hmac_md5(pAd->StaCfg.PTK, LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
+	}
+	NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
+
+	// Update PTK
+	// Prepare pair-wise key information into shared key table
+	NdisZeroMemory(&pAd->SharedKey[BSS0][0], sizeof(CIPHER_KEY));
+	pAd->SharedKey[BSS0][0].KeyLen = LEN_TKIP_EK;
+    NdisMoveMemory(pAd->SharedKey[BSS0][0].Key, &pAd->StaCfg.PTK[32], LEN_TKIP_EK);
+	NdisMoveMemory(pAd->SharedKey[BSS0][0].RxMic, &pAd->StaCfg.PTK[48], LEN_TKIP_RXMICK);
+	NdisMoveMemory(pAd->SharedKey[BSS0][0].TxMic, &pAd->StaCfg.PTK[48+LEN_TKIP_RXMICK], LEN_TKIP_TXMICK);
+
+	// Decide its ChiperAlg
+	if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
+		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_TKIP;
+	else if (pAd->StaCfg.PairCipher == Ndis802_11Encryption3Enabled)
+		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_AES;
+	else
+		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_NONE;
+
+	// Update these related information to MAC_TABLE_ENTRY
+	pEntry = &pAd->MacTab.Content[BSSID_WCID];
+	NdisMoveMemory(&pEntry->PairwiseKey.Key, &pAd->StaCfg.PTK[32], LEN_TKIP_EK);
+	NdisMoveMemory(&pEntry->PairwiseKey.RxMic, &pAd->StaCfg.PTK[48], LEN_TKIP_RXMICK);
+	NdisMoveMemory(&pEntry->PairwiseKey.TxMic, &pAd->StaCfg.PTK[48+LEN_TKIP_RXMICK], LEN_TKIP_TXMICK);
+	pEntry->PairwiseKey.CipherAlg = pAd->SharedKey[BSS0][0].CipherAlg;
+
+	// Update pairwise key information to ASIC Shared Key Table
+	AsicAddSharedKeyEntry(pAd,
+						  BSS0,
+						  0,
+						  pAd->SharedKey[BSS0][0].CipherAlg,
+						  pAd->SharedKey[BSS0][0].Key,
+						  pAd->SharedKey[BSS0][0].TxMic,
+						  pAd->SharedKey[BSS0][0].RxMic);
+
+	// Update ASIC WCID attribute table and IVEIV table
+	RTMPAddWcidAttributeEntry(pAd,
+							  BSS0,
+							  0,
+							  pAd->SharedKey[BSS0][0].CipherAlg,
+							  pEntry);
+
+	// Make  Transmitting frame
+	MakeOutgoingFrame(pOutBuffer,           	&FrameLen,
+			  			LENGTH_802_3,     		&Header802_3,
+						Packet.Body_Len[1] + 4, &Packet,
+						END_OF_ARGS);
+
+
+	// Copy frame to Tx ring and Send Message 4 to authenticator
+	RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, TRUE);
+
+	// set 802.1x port control
+	STA_PORT_SECURED(pAd);
+
+    // Indicate Connected for GUI
+    pAd->IndicateMediaState = NdisMediaStateConnected;
+
+	MlmeFreeMemory(pAd, (PUCHAR)pOutBuffer);
+	os_free_mem(pAd, (PUCHAR)mpool);
+
+
+	// send wireless event - for set key done WPA2
+	if (pAd->CommonCfg.bWirelessEvent)
+		RTMPSendWirelessEvent(pAd, IW_SET_KEY_DONE_WPA2_EVENT_FLAG, pEntry->Addr, BSS0, 0);
+
+	DBGPRINT(RT_DEBUG_ERROR, ("Wpa2PairMsg3Action <-----\n"));
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Process Group key 2-way handshaking
+
+	Arguments:
+		pAd	Pointer	to our adapter
+		Elem		Message body
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	WpaGroupMsg1Action(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MLME_QUEUE_ELEM	*Elem)
+
+{
+	PUCHAR              pOutBuffer = NULL;
+	UCHAR               Header802_3[14];
+	ULONG               FrameLen = 0;
+	EAPOL_PACKET        Packet;
+	PEAPOL_PACKET       pGroup;
+	UCHAR               *mpool, *digest, *KEYDATA;
+	UCHAR               Mic[16], OldMic[16];
+	UCHAR               GTK[32], Key[32];
+	KEY_INFO			peerKeyInfo;
+
+	// allocate memory
+	os_alloc_mem(pAd, (PUCHAR *)&mpool, 1024);
+
+	if(mpool == NULL)
+		return;
+
+	// digest Len = 80.
+	digest = (UCHAR *) ROUND_UP(mpool, 4);
+	// KEYDATA Len = 512.
+	KEYDATA = (UCHAR *) ROUND_UP(digest + 80, 4);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("WpaGroupMsg1Action ----->\n"));
+
+	// Process Group Message 1 frame. skip 802.11 header(24) & LLC_SNAP header(8)
+	pGroup = (PEAPOL_PACKET) &Elem->Msg[LENGTH_802_11 + LENGTH_802_1_H];
+
+	NdisZeroMemory((PUCHAR)&peerKeyInfo, sizeof(peerKeyInfo));
+	NdisMoveMemory((PUCHAR)&peerKeyInfo, (PUCHAR)&pGroup->KeyDesc.KeyInfo, sizeof(KEY_INFO));
+
+	*((USHORT*)&peerKeyInfo) = cpu2le16(*((USHORT*)&peerKeyInfo));
+
+	// 0. Check cipher type match
+	if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled && (peerKeyInfo.KeyDescVer != 2))
+	{
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+	else if (pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled && (peerKeyInfo.KeyDescVer != 1))
+	{
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+
+	// 1. Verify Replay counter
+	//    Check Replay Counter, it has to be larger than last one. No need to be exact one larger
+	if(RTMPCompareMemory(pGroup->KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY) != 1)
+	{
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+
+	// Update new replay counter
+	NdisMoveMemory(pAd->StaCfg.ReplayCounter, pGroup->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// 2. Verify MIC is valid
+	// Save the MIC and replace with zero
+	NdisMoveMemory(OldMic, pGroup->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+	NdisZeroMemory(pGroup->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+
+	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
+	{	// AES
+		HMAC_SHA1((PUCHAR) pGroup, pGroup->Body_Len[1] + 4, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{	// TKIP
+		hmac_md5(pAd->StaCfg.PTK, LEN_EAP_MICK, (PUCHAR) pGroup, pGroup->Body_Len[1] + 4, Mic);
+	}
+
+	if(!NdisEqualMemory(OldMic, Mic, LEN_KEY_DESC_MIC))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, (" MIC Different in group msg 1 of 2-way handshake!!!!!!!!!! \n"));
+		MlmeFreeMemory(pAd, (PUCHAR)mpool);
+		return;
+	}
+	else
+		DBGPRINT(RT_DEBUG_TRACE, (" MIC VALID in group msg 1 of 2-way handshake!!!!!!!!!! \n"));
+
+
+	// 3. Decrypt GTK from Key Data
+	if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+	{
+		// Decrypt AES GTK
+		AES_GTK_KEY_UNWRAP(&pAd->StaCfg.PTK[16], KEYDATA,  pGroup->KeyDesc.KeyDataLen[1], pGroup->KeyDesc.KeyData);
+	}
+	else	// TKIP
+	{
+		INT i;
+
+		// Decrypt TKIP GTK
+		// Construct 32 bytes RC4 Key
+		NdisMoveMemory(Key, pGroup->KeyDesc.KeyIv, 16);
+		NdisMoveMemory(&Key[16], &pAd->StaCfg.PTK[16], 16);
+		ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, Key, 32);
+		//discard first 256 bytes
+		for(i = 0; i < 256; i++)
+			ARCFOUR_BYTE(&pAd->PrivateInfo.WEPCONTEXT);
+		// Decrypt GTK. Becareful, there is no ICV to check the result is correct or not
+		ARCFOUR_DECRYPT(&pAd->PrivateInfo.WEPCONTEXT, KEYDATA, pGroup->KeyDesc.KeyData, pGroup->KeyDesc.KeyDataLen[1]);
+	}
+
+	// Process decrypted key data material
+	// Parse keyData to handle KDE format for WPA2PSK
+	if (peerKeyInfo.EKD_DL)
+	{
+		if (!ParseKeyData(pAd, KEYDATA, pGroup->KeyDesc.KeyDataLen[1], 0))
+		{
+			os_free_mem(pAd, (PUCHAR)mpool);
+			return;
+		}
+	}
+	else	// WPAPSK
+	{
+		// set key material, TxMic and RxMic for WPAPSK
+		NdisMoveMemory(GTK, KEYDATA, 32);
+		NdisMoveMemory(pAd->StaCfg.GTK, GTK, 32);
+		pAd->StaCfg.DefaultKeyId = peerKeyInfo.KeyIndex;
+
+		// Prepare pair-wise key information into shared key table
+		NdisZeroMemory(&pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId], sizeof(CIPHER_KEY));
+		pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].KeyLen = LEN_TKIP_EK;
+		NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key, GTK, LEN_TKIP_EK);
+		NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic, &GTK[16], LEN_TKIP_RXMICK);
+		NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic, &GTK[24], LEN_TKIP_TXMICK);
+
+		// Update Shared Key CipherAlg
+		pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_NONE;
+		if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption2Enabled)
+			pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_TKIP;
+		else if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption3Enabled)
+			pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_AES;
+
+    	//hex_dump("Group Key :", pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key, LEN_TKIP_EK);
+	}
+
+	// Update group key information to ASIC Shared Key Table
+	AsicAddSharedKeyEntry(pAd,
+						  BSS0,
+						  pAd->StaCfg.DefaultKeyId,
+						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
+						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key,
+						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic,
+						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic);
+
+	// Update ASIC WCID attribute table and IVEIV table
+	RTMPAddWcidAttributeEntry(pAd,
+							  BSS0,
+							  pAd->StaCfg.DefaultKeyId,
+							  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
+							  NULL);
+
+	// set 802.1x port control
+	STA_PORT_SECURED(pAd);
+
+    // Indicate Connected for GUI
+    pAd->IndicateMediaState = NdisMediaStateConnected;
+
+	// init header and Fill Packet
+	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
+
+	// Zero Group message 1 body
+	NdisZeroMemory(&Packet, sizeof(Packet));
+	Packet.ProVer	= EAPOL_VER;
+	Packet.ProType	= EAPOLKey;
+	Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE;		// No data field
+
+	//
+	// Group Message 2 as  EAPOL-Key(1,0,0,0,G,0,0,MIC,0)
+	//
+	if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
+	{
+		Packet.KeyDesc.Type = WPA2_KEY_DESC;
+	}
+	else
+	{
+		Packet.KeyDesc.Type = WPA1_KEY_DESC;
+	}
+
+	// Key descriptor version and appropriate RSN IE
+	Packet.KeyDesc.KeyInfo.KeyDescVer = peerKeyInfo.KeyDescVer;
+
+	// Update Key Length
+	Packet.KeyDesc.KeyLength[0] = pGroup->KeyDesc.KeyLength[0];
+	Packet.KeyDesc.KeyLength[1] = pGroup->KeyDesc.KeyLength[1];
+
+	// Key Index as G-Msg 1
+	if(pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK)
+		Packet.KeyDesc.KeyInfo.KeyIndex = peerKeyInfo.KeyIndex;
+
+	// Key Type Group key
+	Packet.KeyDesc.KeyInfo.KeyType = GROUPKEY;
+
+	// KeyMic field presented
+	Packet.KeyDesc.KeyInfo.KeyMic  = 1;
+
+	// Secure bit
+	Packet.KeyDesc.KeyInfo.Secure  = 1;
+
+	// Convert to little-endian format.
+	*((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));
+
+	// Key Replay count
+	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pGroup->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// Out buffer for transmitting group message 2
+	MlmeAllocateMemory(pAd, (PUCHAR *)&pOutBuffer);  // allocate memory
+	if(pOutBuffer == NULL)
+	{
+		MlmeFreeMemory(pAd, (PUCHAR)mpool);
+		return;
+	}
+
+	// Prepare EAPOL frame for MIC calculation
+	// Be careful, only EAPOL frame is counted for MIC calculation
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+		Packet.Body_Len[1] + 4,    &Packet,
+		END_OF_ARGS);
+
+	// Prepare and Fill MIC value
+	NdisZeroMemory(Mic, sizeof(Mic));
+	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
+	{
+		// AES
+		HMAC_SHA1(pOutBuffer, FrameLen, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{
+		hmac_md5(pAd->StaCfg.PTK, LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
+	}
+	NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
+
+
+	MakeOutgoingFrame(pOutBuffer,       		&FrameLen,
+						LENGTH_802_3,     		&Header802_3,
+						Packet.Body_Len[1] + 4, &Packet,
+						END_OF_ARGS);
+
+
+	// 5. Copy frame to Tx ring and prepare for encryption
+	RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, FALSE);
+
+	// 6 Free allocated memory
+	MlmeFreeMemory(pAd, (PUCHAR)pOutBuffer);
+	os_free_mem(pAd, (PUCHAR)mpool);
+
+	// send wireless event - for set key done WPA2
+	if (pAd->CommonCfg.bWirelessEvent)
+		RTMPSendWirelessEvent(pAd, IW_SET_KEY_DONE_WPA2_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("WpaGroupMsg1Action <-----\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Init WPA MAC header
+
+	Arguments:
+		pAd	Pointer	to our adapter
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	WpaMacHeaderInit(
+	IN		PRTMP_ADAPTER	pAd,
+	IN OUT	PHEADER_802_11	pHdr80211,
+	IN		UCHAR			wep,
+	IN		PUCHAR		    pAddr1)
+{
+	NdisZeroMemory(pHdr80211, sizeof(HEADER_802_11));
+	pHdr80211->FC.Type	= BTYPE_DATA;
+	pHdr80211->FC.ToDs	= 1;
+	if (wep	== 1)
+		pHdr80211->FC.Wep = 1;
+
+	 //	Addr1: BSSID, Addr2: SA, Addr3:	DA
+	COPY_MAC_ADDR(pHdr80211->Addr1, pAddr1);
+	COPY_MAC_ADDR(pHdr80211->Addr2, pAd->CurrentAddress);
+	COPY_MAC_ADDR(pHdr80211->Addr3, pAd->CommonCfg.Bssid);
+	pHdr80211->Sequence =	pAd->Sequence;
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Copy frame from waiting queue into relative ring buffer and set
+	appropriate ASIC register to kick hardware encryption before really
+	sent out to air.
+
+	Arguments:
+		pAd		Pointer	to our adapter
+		PNDIS_PACKET	Pointer to outgoing Ndis frame
+		NumberOfFrag	Number of fragment required
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID    RTMPToWirelessSta(
+	IN	PRTMP_ADAPTER	pAd,
+	IN  PUCHAR          pHeader802_3,
+    IN  UINT            HdrLen,
+	IN  PUCHAR          pData,
+    IN  UINT            DataLen,
+    IN	BOOLEAN			is4wayFrame)
+
+{
+	NDIS_STATUS     Status;
+	PNDIS_PACKET    pPacket;
+	UCHAR   Index;
+
+	do
+	{
+		// 1. build a NDIS packet and call RTMPSendPacket();
+		//    be careful about how/when to release this internal allocated NDIS PACKET buffer
+		Status = RTMPAllocateNdisPacket(pAd, &pPacket, pHeader802_3, HdrLen, pData, DataLen);
+		if (Status != NDIS_STATUS_SUCCESS)
+			break;
+
+		if (is4wayFrame)
+			RTMP_SET_PACKET_CLEAR_EAP_FRAME(pPacket, 1);
+		else
+			RTMP_SET_PACKET_CLEAR_EAP_FRAME(pPacket, 0);
+
+		// 2. send out the packet
+		Status = STASendPacket(pAd, pPacket);
+		if(Status == NDIS_STATUS_SUCCESS)
+		{
+			// Dequeue one frame from TxSwQueue0..3 queue and process it
+			// There are three place calling dequeue for TX ring.
+			// 1. Here, right after queueing the frame.
+			// 2. At the end of TxRingTxDone service routine.
+			// 3. Upon NDIS call RTMPSendPackets
+			if((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS)) &&
+				(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)))
+			{
+				for(Index = 0; Index < 5; Index ++)
+					if(pAd->TxSwQueue[Index].Number > 0)
+						RTMPDeQueuePacket(pAd, FALSE, Index, MAX_TX_PROCESS);
+			}
+		}
+	} while(FALSE);
+
+}
+
+/*
+    ========================================================================
+
+    Routine Description:
+    Check Sanity RSN IE form AP
+
+    Arguments:
+
+    Return Value:
+
+
+    ========================================================================
+*/
+BOOLEAN CheckRSNIE(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pData,
+	IN  UCHAR           DataLen,
+	OUT	UCHAR			*Offset)
+{
+	PUCHAR              pVIE;
+	UCHAR               len;
+	PEID_STRUCT         pEid;
+	BOOLEAN				result = FALSE;
+
+	pVIE = pData;
+	len	 = DataLen;
+	*Offset = 0;
+
+	while (len > sizeof(RSNIE2))
+	{
+		pEid = (PEID_STRUCT) pVIE;
+		// WPA RSN IE
+		if ((pEid->Eid == IE_WPA) && (NdisEqualMemory(pEid->Octet, WPA_OUI, 4)))
+		{
+			if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA || pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) &&
+				(NdisEqualMemory(pVIE, pAd->MacTab.Content[BSSID_WCID].RSN_IE, pAd->MacTab.Content[BSSID_WCID].RSNIE_Len)) &&
+				(pAd->MacTab.Content[BSSID_WCID].RSNIE_Len == (pEid->Len + 2)))
+			{
+					DBGPRINT(RT_DEBUG_TRACE, ("CheckRSNIE ==> WPA/WPAPSK RSN IE matched in Msg 3, Length(%d) \n", (pEid->Len + 2)));
+					result = TRUE;
+			}
+
+			*Offset += (pEid->Len + 2);
+		}
+		// WPA2 RSN IE
+		else if ((pEid->Eid == IE_RSN) && (NdisEqualMemory(pEid->Octet + 2, RSN_OUI, 3)))
+		{
+			if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2 || pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK) &&
+				(NdisEqualMemory(pVIE, pAd->MacTab.Content[BSSID_WCID].RSN_IE, pAd->MacTab.Content[BSSID_WCID].RSNIE_Len)) &&
+				(pAd->MacTab.Content[BSSID_WCID].RSNIE_Len == (pEid->Len + 2)))
+			{
+					DBGPRINT(RT_DEBUG_TRACE, ("CheckRSNIE ==> WPA2/WPA2PSK RSN IE matched in Msg 3, Length(%d) \n", (pEid->Len + 2)));
+					result = TRUE;
+			}
+
+			*Offset += (pEid->Len + 2);
+		}
+		else
+		{
+			break;
+		}
+
+		pVIE += (pEid->Len + 2);
+		len  -= (pEid->Len + 2);
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("CheckRSNIE ==> skip_offset(%d) \n", *Offset));
+
+	return result;
+
+}
+
+
+/*
+    ========================================================================
+
+    Routine Description:
+    Parse KEYDATA field.  KEYDATA[] May contain 2 RSN IE and optionally GTK.
+    GTK  is encaptulated in KDE format at  p.83 802.11i D10
+
+    Arguments:
+
+    Return Value:
+
+    Note:
+        802.11i D10
+
+    ========================================================================
+*/
+BOOLEAN ParseKeyData(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pKeyData,
+	IN  UCHAR           KeyDataLen,
+	IN	UCHAR			bPairewise)
+{
+    PKDE_ENCAP          pKDE = NULL;
+    PUCHAR              pMyKeyData = pKeyData;
+    UCHAR               KeyDataLength = KeyDataLen;
+    UCHAR               GTKLEN;
+	UCHAR				skip_offset;
+
+	// Verify The RSN IE contained in Pairewise-Msg 3 and skip it
+	if (bPairewise)
+    {
+		// Check RSN IE whether it is WPA2/WPA2PSK
+		if (!CheckRSNIE(pAd, pKeyData, KeyDataLen, &skip_offset))
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("ParseKeyData ==> WPA2/WPA2PSK RSN IE mismatched \n"));
+			hex_dump("Get KEYDATA :", pKeyData, KeyDataLen);
+			return FALSE;
+    	}
+    	else
+		{
+			// skip RSN IE
+			pMyKeyData += skip_offset;
+			KeyDataLength -= skip_offset;
+
+			//DBGPRINT(RT_DEBUG_TRACE, ("ParseKeyData ==> WPA2/WPA2PSK RSN IE matched in Msg 3, Length(%d) \n", skip_offset));
+		}
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("ParseKeyData ==> KeyDataLength %d without RSN_IE \n", KeyDataLength));
+
+	// Parse EKD format
+	if (KeyDataLength >= 8)
+    {
+        pKDE = (PKDE_ENCAP) pMyKeyData;
+    }
+	else
+    {
+		DBGPRINT(RT_DEBUG_ERROR, ("ERROR: KeyDataLength is too short \n"));
+        return FALSE;
+    }
+
+
+	// Sanity check - shared key index should not be 0
+	if (pKDE->GTKEncap.Kid == 0)
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("ERROR: GTK Key index zero \n"));
+        return FALSE;
+    }
+
+	// Sanity check - KED length
+	if (KeyDataLength < (pKDE->Len + 2))
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("ERROR: The len from KDE is too short \n"));
+        return FALSE;
+    }
+
+	// Get GTK length - refer to IEEE 802.11i-2004 p.82
+	GTKLEN = pKDE->Len -6;
+
+	if (GTKLEN < LEN_AES_KEY)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("ERROR: GTK Key length is too short (%d) \n", GTKLEN));
+        return FALSE;
+	}
+	else
+		DBGPRINT(RT_DEBUG_TRACE, ("GTK Key with KDE formet got index=%d, len=%d \n", pKDE->GTKEncap.Kid, GTKLEN));
+
+	// Update GTK
+	// set key material, TxMic and RxMic for WPAPSK
+	NdisMoveMemory(pAd->StaCfg.GTK, pKDE->GTKEncap.GTK, 32);
+	pAd->StaCfg.DefaultKeyId = pKDE->GTKEncap.Kid;
+
+	// Update shared key table
+	NdisZeroMemory(&pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId], sizeof(CIPHER_KEY));
+	pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].KeyLen = LEN_TKIP_EK;
+	NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key, pKDE->GTKEncap.GTK, LEN_TKIP_EK);
+	NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic, &pKDE->GTKEncap.GTK[16], LEN_TKIP_RXMICK);
+	NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic, &pKDE->GTKEncap.GTK[24], LEN_TKIP_TXMICK);
+
+	// Update Shared Key CipherAlg
+	pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_NONE;
+	if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption2Enabled)
+		pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_TKIP;
+	else if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption3Enabled)
+		pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_AES;
+
+	return TRUE;
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Cisco CCKM PRF function
+
+	Arguments:
+		key				Cisco Base Transient Key (BTK)
+		key_len			The key length of the BTK
+		data			Ruquest Number(RN) + BSSID
+		data_len		The length of the data
+		output			Store for PTK(Pairwise transient keys)
+		len				The length of the output
+	Return Value:
+		None
+
+	Note:
+		802.1i	Annex F.9
+
+	========================================================================
+*/
+VOID CCKMPRF(
+	IN	UCHAR	*key,
+	IN	INT		key_len,
+	IN	UCHAR	*data,
+	IN	INT		data_len,
+	OUT	UCHAR	*output,
+	IN	INT		len)
+{
+	INT		i;
+	UCHAR	input[1024];
+	INT		currentindex = 0;
+	INT		total_len;
+
+	NdisMoveMemory(input, data, data_len);
+	total_len = data_len;
+	input[total_len] = 0;
+	total_len++;
+	for	(i = 0;	i <	(len + 19) / 20; i++)
+	{
+		HMAC_SHA1(input, total_len,	key, key_len, &output[currentindex]);
+		currentindex +=	20;
+		input[total_len - 1]++;
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Process MIC error indication and record MIC error timer.
+
+	Arguments:
+		pAd 	Pointer to our adapter
+		pWpaKey 		Pointer to the WPA key structure
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPReportMicError(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PCIPHER_KEY 	pWpaKey)
+{
+	ULONG	Now;
+    UCHAR   unicastKey = (pWpaKey->Type == PAIRWISE_KEY ? 1:0);
+
+	// Record Last MIC error time and count
+	Now = jiffies;
+	if (pAd->StaCfg.MicErrCnt == 0)
+	{
+		pAd->StaCfg.MicErrCnt++;
+		pAd->StaCfg.LastMicErrorTime = Now;
+        NdisZeroMemory(pAd->StaCfg.ReplayCounter, 8);
+	}
+	else if (pAd->StaCfg.MicErrCnt == 1)
+	{
+		if ((pAd->StaCfg.LastMicErrorTime + (60 * OS_HZ)) < Now)
+		{
+			// Update Last MIC error time, this did not violate two MIC errors within 60 seconds
+			pAd->StaCfg.LastMicErrorTime = Now;
+		}
+		else
+		{
+
+			if (pAd->CommonCfg.bWirelessEvent)
+				RTMPSendWirelessEvent(pAd, IW_COUNTER_MEASURES_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+
+			pAd->StaCfg.LastMicErrorTime = Now;
+			// Violate MIC error counts, MIC countermeasures kicks in
+			pAd->StaCfg.MicErrCnt++;
+		}
+	}
+	else
+	{
+		// MIC error count >= 2
+		// This should not happen
+		;
+	}
+    MlmeEnqueue(pAd,
+				MLME_CNTL_STATE_MACHINE,
+				OID_802_11_MIC_FAILURE_REPORT_FRAME,
+				1,
+				&unicastKey);
+
+    if (pAd->StaCfg.MicErrCnt == 2)
+    {
+        RTMPSetTimer(&pAd->StaCfg.WpaDisassocAndBlockAssocTimer, 100);
+    }
+}
+
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#define	LENGTH_EAP_H    4
+// If the received frame is EAP-Packet ,find out its EAP-Code (Request(0x01), Response(0x02), Success(0x03), Failure(0x04)).
+INT	    WpaCheckEapCode(
+	IN  PRTMP_ADAPTER   		pAd,
+	IN  PUCHAR				pFrame,
+	IN  USHORT				FrameLen,
+	IN  USHORT				OffSet)
+{
+
+	PUCHAR	pData;
+	INT	result = 0;
+
+	if( FrameLen < OffSet + LENGTH_EAPOL_H + LENGTH_EAP_H )
+		return result;
+
+	pData = pFrame + OffSet; // skip offset bytes
+
+	if(*(pData+1) == EAPPacket) 	// 802.1x header - Packet Type
+	{
+		 result = *(pData+4);		// EAP header - Code
+	}
+
+	return result;
+}
+
+VOID    WpaSendMicFailureToWpaSupplicant(
+    IN  PRTMP_ADAPTER    pAd,
+    IN  BOOLEAN          bUnicast)
+{
+    union iwreq_data    wrqu;
+    char custom[IW_CUSTOM_MAX] = {0};
+
+    sprintf(custom, "MLME-MICHAELMICFAILURE.indication");
+    if (bUnicast)
+        sprintf(custom, "%s unicast", custom);
+    wrqu.data.length = strlen(custom);
+    wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, custom);
+
+    return;
+}
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+VOID	WpaMicFailureReportFrame(
+	IN  PRTMP_ADAPTER   pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	PUCHAR              pOutBuffer = NULL;
+	UCHAR               Header802_3[14];
+	ULONG               FrameLen = 0;
+	EAPOL_PACKET        Packet;
+	UCHAR               Mic[16];
+    BOOLEAN             bUnicast;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("WpaMicFailureReportFrame ----->\n"));
+
+    bUnicast = (Elem->Msg[0] == 1 ? TRUE:FALSE);
+	pAd->Sequence = ((pAd->Sequence) + 1) & (MAX_SEQ_NUMBER);
+
+	// init 802.3 header and Fill Packet
+	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
+
+	NdisZeroMemory(&Packet, sizeof(Packet));
+	Packet.ProVer	= EAPOL_VER;
+	Packet.ProType	= EAPOLKey;
+
+	Packet.KeyDesc.Type = WPA1_KEY_DESC;
+
+    // Request field presented
+    Packet.KeyDesc.KeyInfo.Request = 1;
+
+	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
+	{
+		Packet.KeyDesc.KeyInfo.KeyDescVer = 2;
+	}
+	else	  // TKIP
+	{
+		Packet.KeyDesc.KeyInfo.KeyDescVer = 1;
+	}
+
+    Packet.KeyDesc.KeyInfo.KeyType = (bUnicast ? PAIRWISEKEY : GROUPKEY);
+
+	// KeyMic field presented
+	Packet.KeyDesc.KeyInfo.KeyMic  = 1;
+
+    // Error field presented
+	Packet.KeyDesc.KeyInfo.Error  = 1;
+
+	// Update packet length after decide Key data payload
+	Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE;
+
+	// Key Replay Count
+	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY);
+    inc_byte_array(pAd->StaCfg.ReplayCounter, 8);
+
+	// Convert to little-endian format.
+	*((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));
+
+
+	MlmeAllocateMemory(pAd, (PUCHAR *)&pOutBuffer);  // allocate memory
+	if(pOutBuffer == NULL)
+	{
+		return;
+	}
+
+	// Prepare EAPOL frame for MIC calculation
+	// Be careful, only EAPOL frame is counted for MIC calculation
+	MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+		              Packet.Body_Len[1] + 4,   &Packet,
+		              END_OF_ARGS);
+
+	// Prepare and Fill MIC value
+	NdisZeroMemory(Mic, sizeof(Mic));
+	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
+	{	// AES
+        UCHAR digest[20] = {0};
+		HMAC_SHA1(pOutBuffer, FrameLen, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{	// TKIP
+		hmac_md5(pAd->StaCfg.PTK,  LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
+	}
+	NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
+
+    MakeOutgoingFrame(pOutBuffer,           	&FrameLen,
+    	  			LENGTH_802_3,     			&Header802_3,
+    				Packet.Body_Len[1] + 4,     &Packet,
+    				END_OF_ARGS);
+
+	// opy frame to Tx ring and send MIC failure report frame to authenticator
+	RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, FALSE);
+
+	MlmeFreeMemory(pAd, (PUCHAR)pOutBuffer);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("WpaMicFailureReportFrame <-----\n"));
+}
+
+/** from wpa_supplicant
+ * inc_byte_array - Increment arbitrary length byte array by one
+ * @counter: Pointer to byte array
+ * @len: Length of the counter in bytes
+ *
+ * This function increments the last byte of the counter by one and continues
+ * rolling over to more significant bytes if the byte was incremented from
+ * 0xff to 0x00.
+ */
+void inc_byte_array(UCHAR *counter, int len)
+{
+	int pos = len - 1;
+	while (pos >= 0) {
+		counter[pos]++;
+		if (counter[pos] != 0)
+			break;
+		pos--;
+	}
+}
+
+VOID WpaDisassocApAndBlockAssoc(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3)
+{
+    RTMP_ADAPTER                *pAd = (PRTMP_ADAPTER)FunctionContext;
+    MLME_DISASSOC_REQ_STRUCT    DisassocReq;
+
+	// disassoc from current AP first
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPReportMicError - disassociate with current AP after sending second continuous EAPOL frame\n"));
+	DisassocParmFill(pAd, &DisassocReq, pAd->CommonCfg.Bssid, REASON_MIC_FAILURE);
+	MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_DISASSOC_REQ, sizeof(MLME_DISASSOC_REQ_STRUCT), &DisassocReq);
+
+	pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_DISASSOC;
+	pAd->StaCfg.bBlockAssoc = TRUE;
+}
+
diff --git a/drivers/staging/rt2860/sta_ioctl.c b/drivers/staging/rt2860/sta_ioctl.c
new file mode 100644
index 00000000000..9347d11b586
--- /dev/null
+++ b/drivers/staging/rt2860/sta_ioctl.c
@@ -0,0 +1,6944 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    sta_ioctl.c
+
+    Abstract:
+    IOCTL related subroutines
+
+    Revision History:
+    Who         When          What
+    --------    ----------    ----------------------------------------------
+    Rory Chen   01-03-2003    created
+	Rory Chen   02-14-2005    modify to support RT61
+*/
+
+#include	"rt_config.h"
+
+#ifdef DBG
+extern ULONG    RTDebugLevel;
+#endif
+
+#define NR_WEP_KEYS 				4
+#define WEP_SMALL_KEY_LEN 			(40/8)
+#define WEP_LARGE_KEY_LEN 			(104/8)
+
+#define GROUP_KEY_NO                4
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
+#define IWE_STREAM_ADD_EVENT(_A, _B, _C, _D, _E)		iwe_stream_add_event(_A, _B, _C, _D, _E)
+#define IWE_STREAM_ADD_POINT(_A, _B, _C, _D, _E)		iwe_stream_add_point(_A, _B, _C, _D, _E)
+#define IWE_STREAM_ADD_VALUE(_A, _B, _C, _D, _E, _F)	iwe_stream_add_value(_A, _B, _C, _D, _E, _F)
+#else
+#define IWE_STREAM_ADD_EVENT(_A, _B, _C, _D, _E)		iwe_stream_add_event(_B, _C, _D, _E)
+#define IWE_STREAM_ADD_POINT(_A, _B, _C, _D, _E)		iwe_stream_add_point(_B, _C, _D, _E)
+#define IWE_STREAM_ADD_VALUE(_A, _B, _C, _D, _E, _F)	iwe_stream_add_value(_B, _C, _D, _E, _F)
+#endif
+
+extern UCHAR    CipherWpa2Template[];
+extern UCHAR    CipherWpaPskTkip[];
+extern UCHAR    CipherWpaPskTkipLen;
+
+typedef struct PACKED _RT_VERSION_INFO{
+    UCHAR       DriverVersionW;
+    UCHAR       DriverVersionX;
+    UCHAR       DriverVersionY;
+    UCHAR       DriverVersionZ;
+    UINT        DriverBuildYear;
+    UINT        DriverBuildMonth;
+    UINT        DriverBuildDay;
+} RT_VERSION_INFO, *PRT_VERSION_INFO;
+
+struct iw_priv_args privtab[] = {
+{ RTPRIV_IOCTL_SET,
+  IW_PRIV_TYPE_CHAR | 1024, 0,
+  "set"},
+
+{ RTPRIV_IOCTL_SHOW, 0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK,
+  ""},
+{ RTPRIV_IOCTL_SHOW, IW_PRIV_TYPE_CHAR | 1024, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK,
+  ""},
+/* --- sub-ioctls definitions --- */
+    { SHOW_CONN_STATUS,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "connStatus" },
+	{ SHOW_DRVIER_VERION,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "driverVer" },
+    { SHOW_BA_INFO,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "bainfo" },
+	{ SHOW_DESC_INFO,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "descinfo" },
+    { RAIO_OFF,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "radio_off" },
+	{ RAIO_ON,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "radio_on" },
+#ifdef QOS_DLS_SUPPORT
+	{ SHOW_DLS_ENTRY_INFO,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "dlsentryinfo" },
+#endif // QOS_DLS_SUPPORT //
+	{ SHOW_CFG_VALUE,
+	  IW_PRIV_TYPE_CHAR | 1024, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "show" },
+/* --- sub-ioctls relations --- */
+
+#ifdef DBG
+{ RTPRIV_IOCTL_BBP,
+  IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK,
+  "bbp"},
+{ RTPRIV_IOCTL_MAC,
+  IW_PRIV_TYPE_CHAR | 1024, IW_PRIV_TYPE_CHAR | 1024,
+  "mac"},
+{ RTPRIV_IOCTL_E2P,
+  IW_PRIV_TYPE_CHAR | 1024, IW_PRIV_TYPE_CHAR | 1024,
+  "e2p"},
+#endif  /* DBG */
+
+{ RTPRIV_IOCTL_STATISTICS,
+  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK,
+  "stat"},
+{ RTPRIV_IOCTL_GSITESURVEY,
+  0, IW_PRIV_TYPE_CHAR | 1024,
+  "get_site_survey"},
+};
+
+INT Set_SSID_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+#ifdef WMM_SUPPORT
+INT	Set_WmmCapable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif
+
+INT Set_NetworkType_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_AuthMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_EncrypType_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_DefaultKeyID_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_Key1_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_Key2_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_Key3_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_Key4_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_WPAPSK_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+
+INT Set_PSMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+INT Set_Wpa_Support(
+    IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef DBG
+VOID RTMPIoctlBBP(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq);
+
+VOID RTMPIoctlMAC(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq);
+
+VOID RTMPIoctlE2PROM(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  struct iwreq    *wrq);
+#endif // DBG //
+
+
+NDIS_STATUS RTMPWPANoneAddKeyProc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN	PVOID			pBuf);
+
+INT Set_FragTest_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+#ifdef DOT11_N_SUPPORT
+INT Set_TGnWifiTest_Proc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          arg);
+#endif // DOT11_N_SUPPORT //
+
+INT Set_LongRetryLimit_Proc(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PUCHAR			arg);
+
+INT Set_ShortRetryLimit_Proc(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PUCHAR			arg);
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+INT Set_Ieee80211dClientMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+#ifdef CARRIER_DETECTION_SUPPORT
+INT Set_CarrierDetect_Proc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          arg);
+#endif // CARRIER_DETECTION_SUPPORT //
+
+static struct {
+	CHAR *name;
+	INT (*set_proc)(PRTMP_ADAPTER pAdapter, PUCHAR arg);
+} *PRTMP_PRIVATE_SET_PROC, RTMP_PRIVATE_SUPPORT_PROC[] = {
+	{"DriverVersion",				Set_DriverVersion_Proc},
+	{"CountryRegion",				Set_CountryRegion_Proc},
+	{"CountryRegionABand",			Set_CountryRegionABand_Proc},
+	{"SSID",						Set_SSID_Proc},
+	{"WirelessMode",				Set_WirelessMode_Proc},
+	{"TxBurst",					Set_TxBurst_Proc},
+	{"TxPreamble",				Set_TxPreamble_Proc},
+	{"TxPower",					Set_TxPower_Proc},
+	{"Channel",					Set_Channel_Proc},
+	{"BGProtection",				Set_BGProtection_Proc},
+	{"RTSThreshold",				Set_RTSThreshold_Proc},
+	{"FragThreshold",				Set_FragThreshold_Proc},
+#ifdef DOT11_N_SUPPORT
+	{"HtBw",		                Set_HtBw_Proc},
+	{"HtMcs",		                Set_HtMcs_Proc},
+	{"HtGi",		                Set_HtGi_Proc},
+	{"HtOpMode",		            Set_HtOpMode_Proc},
+	{"HtExtcha",		            Set_HtExtcha_Proc},
+	{"HtMpduDensity",		        Set_HtMpduDensity_Proc},
+	{"HtBaWinSize",		        	Set_HtBaWinSize_Proc},
+	{"HtRdg",		        		Set_HtRdg_Proc},
+	{"HtAmsdu",		        		Set_HtAmsdu_Proc},
+	{"HtAutoBa",		        	Set_HtAutoBa_Proc},
+	{"HtBaDecline",					Set_BADecline_Proc},
+	{"HtProtect",		        	Set_HtProtect_Proc},
+	{"HtMimoPs",		        	Set_HtMimoPs_Proc},
+#endif // DOT11_N_SUPPORT //
+
+#ifdef AGGREGATION_SUPPORT
+	{"PktAggregate",				Set_PktAggregate_Proc},
+#endif
+
+#ifdef WMM_SUPPORT
+	{"WmmCapable",					Set_WmmCapable_Proc},
+#endif
+	{"IEEE80211H",					Set_IEEE80211H_Proc},
+    {"NetworkType",                 Set_NetworkType_Proc},
+	{"AuthMode",					Set_AuthMode_Proc},
+	{"EncrypType",					Set_EncrypType_Proc},
+	{"DefaultKeyID",				Set_DefaultKeyID_Proc},
+	{"Key1",						Set_Key1_Proc},
+	{"Key2",						Set_Key2_Proc},
+	{"Key3",						Set_Key3_Proc},
+	{"Key4",						Set_Key4_Proc},
+	{"WPAPSK",						Set_WPAPSK_Proc},
+	{"ResetCounter",				Set_ResetStatCounter_Proc},
+	{"PSMode",                      Set_PSMode_Proc},
+#ifdef DBG
+	{"Debug",						Set_Debug_Proc},
+#endif
+
+#ifdef RALINK_ATE
+	{"ATE",							Set_ATE_Proc},
+	{"ATEDA",						Set_ATE_DA_Proc},
+	{"ATESA",						Set_ATE_SA_Proc},
+	{"ATEBSSID",					Set_ATE_BSSID_Proc},
+	{"ATECHANNEL",					Set_ATE_CHANNEL_Proc},
+	{"ATETXPOW0",					Set_ATE_TX_POWER0_Proc},
+	{"ATETXPOW1",					Set_ATE_TX_POWER1_Proc},
+	{"ATETXANT",					Set_ATE_TX_Antenna_Proc},
+	{"ATERXANT",					Set_ATE_RX_Antenna_Proc},
+	{"ATETXFREQOFFSET",				Set_ATE_TX_FREQOFFSET_Proc},
+	{"ATETXBW",						Set_ATE_TX_BW_Proc},
+	{"ATETXLEN",					Set_ATE_TX_LENGTH_Proc},
+	{"ATETXCNT",					Set_ATE_TX_COUNT_Proc},
+	{"ATETXMCS",					Set_ATE_TX_MCS_Proc},
+	{"ATETXMODE",					Set_ATE_TX_MODE_Proc},
+	{"ATETXGI",						Set_ATE_TX_GI_Proc},
+	{"ATERXFER",					Set_ATE_RX_FER_Proc},
+	{"ATERRF",						Set_ATE_Read_RF_Proc},
+	{"ATEWRF1",						Set_ATE_Write_RF1_Proc},
+	{"ATEWRF2",						Set_ATE_Write_RF2_Proc},
+	{"ATEWRF3",						Set_ATE_Write_RF3_Proc},
+	{"ATEWRF4",						Set_ATE_Write_RF4_Proc},
+	{"ATELDE2P",				    Set_ATE_Load_E2P_Proc},
+	{"ATERE2P",						Set_ATE_Read_E2P_Proc},
+	{"ATESHOW",						Set_ATE_Show_Proc},
+	{"ATEHELP",						Set_ATE_Help_Proc},
+
+#ifdef RALINK_28xx_QA
+	{"TxStop",						Set_TxStop_Proc},
+	{"RxStop",						Set_RxStop_Proc},
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+    {"WpaSupport",                  Set_Wpa_Support},
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+
+
+	{"FixedTxMode",                 Set_FixedTxMode_Proc},
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+	{"OpMode",						Set_OpMode_Proc},
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+#ifdef DOT11_N_SUPPORT
+    {"TGnWifiTest",                 Set_TGnWifiTest_Proc},
+    {"ForceGF",		        		Set_ForceGF_Proc},
+#endif // DOT11_N_SUPPORT //
+#ifdef QOS_DLS_SUPPORT
+	{"DlsAddEntry",					Set_DlsAddEntry_Proc},
+	{"DlsTearDownEntry",			Set_DlsTearDownEntry_Proc},
+#endif // QOS_DLS_SUPPORT //
+	{"LongRetry",	        		Set_LongRetryLimit_Proc},
+	{"ShortRetry",	        		Set_ShortRetryLimit_Proc},
+#ifdef EXT_BUILD_CHANNEL_LIST
+	{"11dClientMode",				Set_Ieee80211dClientMode_Proc},
+#endif // EXT_BUILD_CHANNEL_LIST //
+#ifdef CARRIER_DETECTION_SUPPORT
+	{"CarrierDetect",				Set_CarrierDetect_Proc},
+#endif // CARRIER_DETECTION_SUPPORT //
+
+	{NULL,}
+};
+
+
+VOID RTMPAddKey(
+	IN	PRTMP_ADAPTER	    pAd,
+	IN	PNDIS_802_11_KEY    pKey)
+{
+	ULONG				KeyIdx;
+	MAC_TABLE_ENTRY  	*pEntry;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("RTMPAddKey ------>\n"));
+
+	if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+	{
+		if (pKey->KeyIndex & 0x80000000)
+		{
+		    if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+            {
+                NdisZeroMemory(pAd->StaCfg.PMK, 32);
+                NdisMoveMemory(pAd->StaCfg.PMK, pKey->KeyMaterial, pKey->KeyLength);
+                goto end;
+            }
+		    // Update PTK
+		    NdisZeroMemory(&pAd->SharedKey[BSS0][0], sizeof(CIPHER_KEY));
+            pAd->SharedKey[BSS0][0].KeyLen = LEN_TKIP_EK;
+            NdisMoveMemory(pAd->SharedKey[BSS0][0].Key, pKey->KeyMaterial, LEN_TKIP_EK);
+#ifdef WPA_SUPPLICANT_SUPPORT
+            if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
+            {
+                NdisMoveMemory(pAd->SharedKey[BSS0][0].RxMic, pKey->KeyMaterial + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+                NdisMoveMemory(pAd->SharedKey[BSS0][0].TxMic, pKey->KeyMaterial + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+            }
+            else
+#endif // WPA_SUPPLICANT_SUPPORT //
+            {
+            	NdisMoveMemory(pAd->SharedKey[BSS0][0].TxMic, pKey->KeyMaterial + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+                NdisMoveMemory(pAd->SharedKey[BSS0][0].RxMic, pKey->KeyMaterial + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+            }
+
+            // Decide its ChiperAlg
+        	if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
+        		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_TKIP;
+        	else if (pAd->StaCfg.PairCipher == Ndis802_11Encryption3Enabled)
+        		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_AES;
+        	else
+        		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_NONE;
+
+            // Update these related information to MAC_TABLE_ENTRY
+        	pEntry = &pAd->MacTab.Content[BSSID_WCID];
+            NdisMoveMemory(pEntry->PairwiseKey.Key, pAd->SharedKey[BSS0][0].Key, LEN_TKIP_EK);
+        	NdisMoveMemory(pEntry->PairwiseKey.RxMic, pAd->SharedKey[BSS0][0].RxMic, LEN_TKIP_RXMICK);
+        	NdisMoveMemory(pEntry->PairwiseKey.TxMic, pAd->SharedKey[BSS0][0].TxMic, LEN_TKIP_TXMICK);
+        	pEntry->PairwiseKey.CipherAlg = pAd->SharedKey[BSS0][0].CipherAlg;
+
+        	// Update pairwise key information to ASIC Shared Key Table
+        	AsicAddSharedKeyEntry(pAd,
+        						  BSS0,
+        						  0,
+        						  pAd->SharedKey[BSS0][0].CipherAlg,
+        						  pAd->SharedKey[BSS0][0].Key,
+        						  pAd->SharedKey[BSS0][0].TxMic,
+        						  pAd->SharedKey[BSS0][0].RxMic);
+
+        	// Update ASIC WCID attribute table and IVEIV table
+        	RTMPAddWcidAttributeEntry(pAd,
+        							  BSS0,
+        							  0,
+        							  pAd->SharedKey[BSS0][0].CipherAlg,
+        							  pEntry);
+
+            if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA2)
+            {
+                // set 802.1x port control
+				STA_PORT_SECURED(pAd);
+
+                // Indicate Connected for GUI
+                pAd->IndicateMediaState = NdisMediaStateConnected;
+            }
+		}
+        else
+        {
+            // Update GTK
+            pAd->StaCfg.DefaultKeyId = (pKey->KeyIndex & 0xFF);
+            NdisZeroMemory(&pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId], sizeof(CIPHER_KEY));
+            pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].KeyLen = LEN_TKIP_EK;
+            NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key, pKey->KeyMaterial, LEN_TKIP_EK);
+#ifdef WPA_SUPPLICANT_SUPPORT
+            if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption2Enabled)
+            {
+                NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic, pKey->KeyMaterial + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+                NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic, pKey->KeyMaterial + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+            }
+            else
+#endif // WPA_SUPPLICANT_SUPPORT //
+            {
+            	NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic, pKey->KeyMaterial + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+                NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic, pKey->KeyMaterial + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+            }
+
+            // Update Shared Key CipherAlg
+    		pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_NONE;
+    		if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption2Enabled)
+    			pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_TKIP;
+    		else if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption3Enabled)
+    			pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_AES;
+
+            // Update group key information to ASIC Shared Key Table
+        	AsicAddSharedKeyEntry(pAd,
+        						  BSS0,
+        						  pAd->StaCfg.DefaultKeyId,
+        						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
+        						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key,
+        						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic,
+        						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic);
+
+        	// Update ASIC WCID attribute table and IVEIV table
+        	RTMPAddWcidAttributeEntry(pAd,
+        							  BSS0,
+        							  pAd->StaCfg.DefaultKeyId,
+        							  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
+        							  NULL);
+
+            // set 802.1x port control
+			STA_PORT_SECURED(pAd);
+
+            // Indicate Connected for GUI
+            pAd->IndicateMediaState = NdisMediaStateConnected;
+        }
+	}
+	else	// dynamic WEP from wpa_supplicant
+	{
+		UCHAR	CipherAlg;
+    	PUCHAR	Key;
+
+		if(pKey->KeyLength == 32)
+			goto end;
+
+		KeyIdx = pKey->KeyIndex & 0x0fffffff;
+
+		if (KeyIdx < 4)
+		{
+			// it is a default shared key, for Pairwise key setting
+			if (pKey->KeyIndex & 0x80000000)
+			{
+				pEntry = MacTableLookup(pAd, pKey->BSSID);
+
+				if (pEntry)
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("RTMPAddKey: Set Pair-wise Key\n"));
+
+					// set key material and key length
+ 					pEntry->PairwiseKey.KeyLen = (UCHAR)pKey->KeyLength;
+					NdisMoveMemory(pEntry->PairwiseKey.Key, &pKey->KeyMaterial, pKey->KeyLength);
+
+					// set Cipher type
+					if (pKey->KeyLength == 5)
+						pEntry->PairwiseKey.CipherAlg = CIPHER_WEP64;
+					else
+						pEntry->PairwiseKey.CipherAlg = CIPHER_WEP128;
+
+					// Add Pair-wise key to Asic
+					AsicAddPairwiseKeyEntry(
+						pAd,
+						pEntry->Addr,
+						(UCHAR)pEntry->Aid,
+                		&pEntry->PairwiseKey);
+
+					// update WCID attribute table and IVEIV table for this entry
+					RTMPAddWcidAttributeEntry(
+						pAd,
+						BSS0,
+						KeyIdx, // The value may be not zero
+						pEntry->PairwiseKey.CipherAlg,
+						pEntry);
+
+				}
+			}
+			else
+            {
+				// Default key for tx (shared key)
+				pAd->StaCfg.DefaultKeyId = (UCHAR) KeyIdx;
+
+				// set key material and key length
+				pAd->SharedKey[BSS0][KeyIdx].KeyLen = (UCHAR) pKey->KeyLength;
+				NdisMoveMemory(pAd->SharedKey[BSS0][KeyIdx].Key, &pKey->KeyMaterial, pKey->KeyLength);
+
+				// Set Ciper type
+				if (pKey->KeyLength == 5)
+					pAd->SharedKey[BSS0][KeyIdx].CipherAlg = CIPHER_WEP64;
+				else
+					pAd->SharedKey[BSS0][KeyIdx].CipherAlg = CIPHER_WEP128;
+
+    			CipherAlg = pAd->SharedKey[BSS0][KeyIdx].CipherAlg;
+    			Key = pAd->SharedKey[BSS0][KeyIdx].Key;
+
+				// Set Group key material to Asic
+    			AsicAddSharedKeyEntry(pAd, BSS0, KeyIdx, CipherAlg, Key, NULL, NULL);
+
+				// Update WCID attribute table and IVEIV table for this group key table
+				RTMPAddWcidAttributeEntry(pAd, BSS0, KeyIdx, CipherAlg, NULL);
+
+			}
+		}
+	}
+end:
+	return;
+}
+
+char * rtstrchr(const char * s, int c)
+{
+    for(; *s != (char) c; ++s)
+        if (*s == '\0')
+            return NULL;
+    return (char *) s;
+}
+
+/*
+This is required for LinEX2004/kernel2.6.7 to provide iwlist scanning function
+*/
+
+int
+rt_ioctl_giwname(struct net_device *dev,
+		   struct iw_request_info *info,
+		   char *name, char *extra)
+{
+//	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+
+#ifdef RT2860
+    strncpy(name, "RT2860 Wireless", IFNAMSIZ);
+#endif // RT2860 //
+	return 0;
+}
+
+int rt_ioctl_siwfreq(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_freq *freq, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+	int 	chan = -1;
+
+    //check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        return -ENETDOWN;
+    }
+
+
+	if (freq->e > 1)
+		return -EINVAL;
+
+	if((freq->e == 0) && (freq->m <= 1000))
+		chan = freq->m;	// Setting by channel number
+	else
+		MAP_KHZ_TO_CHANNEL_ID( (freq->m /100) , chan); // Setting by frequency - search the table , like 2.412G, 2.422G,
+
+    if (ChannelSanity(pAdapter, chan) == TRUE)
+    {
+	pAdapter->CommonCfg.Channel = chan;
+	DBGPRINT(RT_DEBUG_ERROR, ("==>rt_ioctl_siwfreq::SIOCSIWFREQ[cmd=0x%x] (Channel=%d)\n", SIOCSIWFREQ, pAdapter->CommonCfg.Channel));
+    }
+    else
+        return -EINVAL;
+
+	return 0;
+}
+int rt_ioctl_giwfreq(struct net_device *dev,
+		   struct iw_request_info *info,
+		   struct iw_freq *freq, char *extra)
+{
+    VIRTUAL_ADAPTER *pVirtualAd = NULL;
+	PRTMP_ADAPTER pAdapter = NULL;
+	UCHAR ch;
+	ULONG	m;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+		ch = pAdapter->CommonCfg.Channel;
+
+	DBGPRINT(RT_DEBUG_TRACE,("==>rt_ioctl_giwfreq  %d\n", ch));
+
+    MAP_CHANNEL_ID_TO_KHZ(ch, m);
+	freq->m = m * 100;
+	freq->e = 1;
+	return 0;
+}
+
+int rt_ioctl_siwmode(struct net_device *dev,
+		   struct iw_request_info *info,
+		   __u32 *mode, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+
+	//check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+    	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+       	return -ENETDOWN;
+    }
+
+	switch (*mode)
+	{
+		case IW_MODE_ADHOC:
+			Set_NetworkType_Proc(pAdapter, "Adhoc");
+			break;
+		case IW_MODE_INFRA:
+			Set_NetworkType_Proc(pAdapter, "Infra");
+			break;
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,4,20))
+        case IW_MODE_MONITOR:
+			Set_NetworkType_Proc(pAdapter, "Monitor");
+			break;
+#endif
+		default:
+			DBGPRINT(RT_DEBUG_TRACE, ("===>rt_ioctl_siwmode::SIOCSIWMODE (unknown %d)\n", *mode));
+			return -EINVAL;
+	}
+
+	// Reset Ralink supplicant to not use, it will be set to start when UI set PMK key
+	pAdapter->StaCfg.WpaState = SS_NOTUSE;
+
+	return 0;
+}
+
+int rt_ioctl_giwmode(struct net_device *dev,
+		   struct iw_request_info *info,
+		   __u32 *mode, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	if (ADHOC_ON(pAdapter))
+		*mode = IW_MODE_ADHOC;
+    else if (INFRA_ON(pAdapter))
+		*mode = IW_MODE_INFRA;
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,4,20))
+    else if (MONITOR_ON(pAdapter))
+    {
+        *mode = IW_MODE_MONITOR;
+    }
+#endif
+    else
+        *mode = IW_MODE_AUTO;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("==>rt_ioctl_giwmode(mode=%d)\n", *mode));
+	return 0;
+}
+
+int rt_ioctl_siwsens(struct net_device *dev,
+		   struct iw_request_info *info,
+		   char *name, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+
+	//check if the interface is down
+    	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    	{
+        	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        	return -ENETDOWN;
+    	}
+
+	return 0;
+}
+
+int rt_ioctl_giwsens(struct net_device *dev,
+		   struct iw_request_info *info,
+		   char *name, char *extra)
+{
+	return 0;
+}
+
+int rt_ioctl_giwrange(struct net_device *dev,
+		   struct iw_request_info *info,
+		   struct iw_point *data, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+	struct iw_range *range = (struct iw_range *) extra;
+	u16 val;
+	int i;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE ,("===>rt_ioctl_giwrange\n"));
+	data->length = sizeof(struct iw_range);
+	memset(range, 0, sizeof(struct iw_range));
+
+	range->txpower_capa = IW_TXPOW_DBM;
+
+	if (INFRA_ON(pAdapter)||ADHOC_ON(pAdapter))
+	{
+		range->min_pmp = 1 * 1024;
+		range->max_pmp = 65535 * 1024;
+		range->min_pmt = 1 * 1024;
+		range->max_pmt = 1000 * 1024;
+		range->pmp_flags = IW_POWER_PERIOD;
+		range->pmt_flags = IW_POWER_TIMEOUT;
+		range->pm_capa = IW_POWER_PERIOD | IW_POWER_TIMEOUT |
+			IW_POWER_UNICAST_R | IW_POWER_ALL_R;
+	}
+
+	range->we_version_compiled = WIRELESS_EXT;
+	range->we_version_source = 14;
+
+	range->retry_capa = IW_RETRY_LIMIT;
+	range->retry_flags = IW_RETRY_LIMIT;
+	range->min_retry = 0;
+	range->max_retry = 255;
+
+	range->num_channels =  pAdapter->ChannelListNum;
+
+	val = 0;
+	for (i = 1; i <= range->num_channels; i++)
+	{
+		u32 m;
+		range->freq[val].i = pAdapter->ChannelList[i-1].Channel;
+		MAP_CHANNEL_ID_TO_KHZ(pAdapter->ChannelList[i-1].Channel, m);
+		range->freq[val].m = m * 100; /* HZ */
+
+		range->freq[val].e = 1;
+		val++;
+		if (val == IW_MAX_FREQUENCIES)
+			break;
+	}
+	range->num_frequency = val;
+
+	range->max_qual.qual = 100; /* what is correct max? This was not
+					* documented exactly. At least
+					* 69 has been observed. */
+	range->max_qual.level = 0; /* dB */
+	range->max_qual.noise = 0; /* dB */
+
+	/* What would be suitable values for "average/typical" qual? */
+	range->avg_qual.qual = 20;
+	range->avg_qual.level = -60;
+	range->avg_qual.noise = -95;
+	range->sensitivity = 3;
+
+	range->max_encoding_tokens = NR_WEP_KEYS;
+	range->num_encoding_sizes = 2;
+	range->encoding_size[0] = 5;
+	range->encoding_size[1] = 13;
+
+	range->min_rts = 0;
+	range->max_rts = 2347;
+	range->min_frag = 256;
+	range->max_frag = 2346;
+
+#if WIRELESS_EXT > 17
+	/* IW_ENC_CAPA_* bit field */
+	range->enc_capa = IW_ENC_CAPA_WPA | IW_ENC_CAPA_WPA2 |
+					IW_ENC_CAPA_CIPHER_TKIP | IW_ENC_CAPA_CIPHER_CCMP;
+#endif
+
+	return 0;
+}
+
+int rt_ioctl_siwap(struct net_device *dev,
+		      struct iw_request_info *info,
+		      struct sockaddr *ap_addr, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+    NDIS_802_11_MAC_ADDRESS Bssid;
+
+	//check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+       	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+       	return -ENETDOWN;
+    }
+
+	if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+    {
+        RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+        DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+    }
+
+    // tell CNTL state machine to call NdisMSetInformationComplete() after completing
+    // this request, because this request is initiated by NDIS.
+    pAdapter->MlmeAux.CurrReqIsFromNdis = FALSE;
+	// Prevent to connect AP again in STAMlmePeriodicExec
+	pAdapter->MlmeAux.AutoReconnectSsidLen= 32;
+
+    memset(Bssid, 0, MAC_ADDR_LEN);
+    memcpy(Bssid, ap_addr->sa_data, MAC_ADDR_LEN);
+    MlmeEnqueue(pAdapter,
+                MLME_CNTL_STATE_MACHINE,
+                OID_802_11_BSSID,
+                sizeof(NDIS_802_11_MAC_ADDRESS),
+                (VOID *)&Bssid);
+
+    DBGPRINT(RT_DEBUG_TRACE, ("IOCTL::SIOCSIWAP %02x:%02x:%02x:%02x:%02x:%02x\n",
+        Bssid[0], Bssid[1], Bssid[2], Bssid[3], Bssid[4], Bssid[5]));
+
+	return 0;
+}
+
+int rt_ioctl_giwap(struct net_device *dev,
+		      struct iw_request_info *info,
+		      struct sockaddr *ap_addr, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	if (INFRA_ON(pAdapter) || ADHOC_ON(pAdapter))
+	{
+		ap_addr->sa_family = ARPHRD_ETHER;
+		memcpy(ap_addr->sa_data, &pAdapter->CommonCfg.Bssid, ETH_ALEN);
+	}
+#ifdef WPA_SUPPLICANT_SUPPORT
+    // Add for RT2870
+    else if (pAdapter->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_DISABLE)
+    {
+        ap_addr->sa_family = ARPHRD_ETHER;
+        memcpy(ap_addr->sa_data, &pAdapter->MlmeAux.Bssid, ETH_ALEN);
+    }
+#endif // WPA_SUPPLICANT_SUPPORT //
+	else
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("IOCTL::SIOCGIWAP(=EMPTY)\n"));
+		return -ENOTCONN;
+	}
+
+	return 0;
+}
+
+/*
+ * Units are in db above the noise floor. That means the
+ * rssi values reported in the tx/rx descriptors in the
+ * driver are the SNR expressed in db.
+ *
+ * If you assume that the noise floor is -95, which is an
+ * excellent assumption 99.5 % of the time, then you can
+ * derive the absolute signal level (i.e. -95 + rssi).
+ * There are some other slight factors to take into account
+ * depending on whether the rssi measurement is from 11b,
+ * 11g, or 11a.   These differences are at most 2db and
+ * can be documented.
+ *
+ * NB: various calculations are based on the orinoco/wavelan
+ *     drivers for compatibility
+ */
+static void set_quality(PRTMP_ADAPTER pAdapter,
+                        struct iw_quality *iq,
+                        signed char rssi)
+{
+	__u8 ChannelQuality;
+
+	// Normalize Rssi
+	if (rssi >= -50)
+		ChannelQuality = 100;
+	else if (rssi >= -80) // between -50 ~ -80dbm
+		ChannelQuality = (__u8)(24 + ((rssi + 80) * 26)/10);
+	else if (rssi >= -90)   // between -80 ~ -90dbm
+        ChannelQuality = (__u8)((rssi + 90) * 26)/10;
+	else
+		ChannelQuality = 0;
+
+    iq->qual = (__u8)ChannelQuality;
+
+    iq->level = (__u8)(rssi);
+    iq->noise = (pAdapter->BbpWriteLatch[66] > pAdapter->BbpTuning.FalseCcaUpperThreshold) ? ((__u8)pAdapter->BbpTuning.FalseCcaUpperThreshold) : ((__u8) pAdapter->BbpWriteLatch[66]); 	// noise level (dBm)
+    iq->noise += 256 - 143;
+    iq->updated = pAdapter->iw_stats.qual.updated;
+}
+
+int rt_ioctl_iwaplist(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_point *data, char *extra)
+{
+ 	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+
+	struct sockaddr addr[IW_MAX_AP];
+	struct iw_quality qual[IW_MAX_AP];
+	int i;
+
+   	//check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+       	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+		data->length = 0;
+		return 0;
+        //return -ENETDOWN;
+	}
+
+	for (i = 0; i <IW_MAX_AP ; i++)
+	{
+		if (i >=  pAdapter->ScanTab.BssNr)
+			break;
+		addr[i].sa_family = ARPHRD_ETHER;
+			memcpy(addr[i].sa_data, &pAdapter->ScanTab.BssEntry[i].Bssid, MAC_ADDR_LEN);
+		set_quality(pAdapter, &qual[i], pAdapter->ScanTab.BssEntry[i].Rssi);
+	}
+	data->length = i;
+	memcpy(extra, &addr, i*sizeof(addr[0]));
+	data->flags = 1;		/* signal quality present (sort of) */
+	memcpy(extra + i*sizeof(addr[0]), &qual, i*sizeof(qual[i]));
+
+	return 0;
+}
+
+#ifdef SIOCGIWSCAN
+int rt_ioctl_siwscan(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_point *data, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+
+	ULONG								Now;
+	int Status = NDIS_STATUS_SUCCESS;
+
+	//check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+		return -ENETDOWN;
+	}
+
+	if (MONITOR_ON(pAdapter))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("!!! Driver is in Monitor Mode now !!!\n"));
+        return -EINVAL;
+    }
+
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+	if (pAdapter->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_ENABLE)
+	{
+		pAdapter->StaCfg.WpaSupplicantScanCount++;
+	}
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+    pAdapter->StaCfg.bScanReqIsFromWebUI = TRUE;
+	if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+		return 0;
+	do{
+		Now = jiffies;
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+		if ((pAdapter->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_ENABLE) &&
+			(pAdapter->StaCfg.WpaSupplicantScanCount > 3))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("!!! WpaSupplicantScanCount > 3\n"));
+			Status = NDIS_STATUS_SUCCESS;
+			break;
+		}
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+		if ((OPSTATUS_TEST_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED)) &&
+			((pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
+			(pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK)) &&
+			(pAdapter->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("!!! Link UP, Port Not Secured! ignore this set::OID_802_11_BSSID_LIST_SCAN\n"));
+			Status = NDIS_STATUS_SUCCESS;
+			break;
+		}
+
+		if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+		{
+			RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+			DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+		}
+
+		// tell CNTL state machine to call NdisMSetInformationComplete() after completing
+		// this request, because this request is initiated by NDIS.
+		pAdapter->MlmeAux.CurrReqIsFromNdis = FALSE;
+		// Reset allowed scan retries
+		pAdapter->StaCfg.ScanCnt = 0;
+		pAdapter->StaCfg.LastScanTime = Now;
+
+		MlmeEnqueue(pAdapter,
+			MLME_CNTL_STATE_MACHINE,
+			OID_802_11_BSSID_LIST_SCAN,
+			0,
+			NULL);
+
+		Status = NDIS_STATUS_SUCCESS;
+		RT28XX_MLME_HANDLER(pAdapter);
+	}while(0);
+	return 0;
+}
+
+int rt_ioctl_giwscan(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_point *data, char *extra)
+{
+
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+	int i=0;
+	char *current_ev = extra, *previous_ev = extra;
+	char *end_buf;
+	char *current_val, custom[MAX_CUSTOM_LEN] = {0};
+#ifndef IWEVGENIE
+	char idx;
+#endif // IWEVGENIE //
+	struct iw_event iwe;
+
+	if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+    {
+		/*
+		 * Still scanning, indicate the caller should try again.
+		 */
+		return -EAGAIN;
+	}
+
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+	if (pAdapter->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_ENABLE)
+	{
+		pAdapter->StaCfg.WpaSupplicantScanCount = 0;
+	}
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+	if (pAdapter->ScanTab.BssNr == 0)
+	{
+		data->length = 0;
+		return 0;
+	}
+
+#if WIRELESS_EXT >= 17
+    if (data->length > 0)
+        end_buf = extra + data->length;
+    else
+        end_buf = extra + IW_SCAN_MAX_DATA;
+#else
+    end_buf = extra + IW_SCAN_MAX_DATA;
+#endif
+
+	for (i = 0; i < pAdapter->ScanTab.BssNr; i++)
+	{
+		if (current_ev >= end_buf)
+        {
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+        }
+
+		//MAC address
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWAP;
+		iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
+		memcpy(iwe.u.ap_addr.sa_data, &pAdapter->ScanTab.BssEntry[i].Bssid, ETH_ALEN);
+
+        previous_ev = current_ev;
+		current_ev = IWE_STREAM_ADD_EVENT(info, current_ev,end_buf, &iwe, IW_EV_ADDR_LEN);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//ESSID
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWESSID;
+		iwe.u.data.length = pAdapter->ScanTab.BssEntry[i].SsidLen;
+		iwe.u.data.flags = 1;
+
+        previous_ev = current_ev;
+		current_ev = IWE_STREAM_ADD_POINT(info, current_ev,end_buf, &iwe, pAdapter->ScanTab.BssEntry[i].Ssid);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//Network Type
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWMODE;
+		if (pAdapter->ScanTab.BssEntry[i].BssType == Ndis802_11IBSS)
+		{
+			iwe.u.mode = IW_MODE_ADHOC;
+		}
+		else if (pAdapter->ScanTab.BssEntry[i].BssType == Ndis802_11Infrastructure)
+		{
+			iwe.u.mode = IW_MODE_INFRA;
+		}
+		else
+		{
+			iwe.u.mode = IW_MODE_AUTO;
+		}
+		iwe.len = IW_EV_UINT_LEN;
+
+        previous_ev = current_ev;
+		current_ev = IWE_STREAM_ADD_EVENT(info, current_ev, end_buf, &iwe,  IW_EV_UINT_LEN);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//Channel and Frequency
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWFREQ;
+		if (INFRA_ON(pAdapter) || ADHOC_ON(pAdapter))
+			iwe.u.freq.m = pAdapter->ScanTab.BssEntry[i].Channel;
+		else
+			iwe.u.freq.m = pAdapter->ScanTab.BssEntry[i].Channel;
+		iwe.u.freq.e = 0;
+		iwe.u.freq.i = 0;
+
+		previous_ev = current_ev;
+		current_ev = IWE_STREAM_ADD_EVENT(info, current_ev,end_buf, &iwe, IW_EV_FREQ_LEN);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+        //Add quality statistics
+        //================================
+        memset(&iwe, 0, sizeof(iwe));
+    	iwe.cmd = IWEVQUAL;
+    	iwe.u.qual.level = 0;
+    	iwe.u.qual.noise = 0;
+        set_quality(pAdapter, &iwe.u.qual, pAdapter->ScanTab.BssEntry[i].Rssi);
+    	current_ev = IWE_STREAM_ADD_EVENT(info, current_ev, end_buf, &iwe, IW_EV_QUAL_LEN);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//Encyption key
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWENCODE;
+		if (CAP_IS_PRIVACY_ON (pAdapter->ScanTab.BssEntry[i].CapabilityInfo ))
+			iwe.u.data.flags =IW_ENCODE_ENABLED | IW_ENCODE_NOKEY;
+		else
+			iwe.u.data.flags = IW_ENCODE_DISABLED;
+
+        previous_ev = current_ev;
+        current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf,&iwe, (char *)pAdapter->SharedKey[BSS0][(iwe.u.data.flags & IW_ENCODE_INDEX)-1].Key);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//Bit Rate
+		//================================
+		if (pAdapter->ScanTab.BssEntry[i].SupRateLen)
+        {
+            UCHAR tmpRate = pAdapter->ScanTab.BssEntry[i].SupRate[pAdapter->ScanTab.BssEntry[i].SupRateLen-1];
+			memset(&iwe, 0, sizeof(iwe));
+			iwe.cmd = SIOCGIWRATE;
+    		current_val = current_ev + IW_EV_LCP_LEN;
+            if (tmpRate == 0x82)
+                iwe.u.bitrate.value =  1 * 1000000;
+            else if (tmpRate == 0x84)
+                iwe.u.bitrate.value =  2 * 1000000;
+            else if (tmpRate == 0x8B)
+                iwe.u.bitrate.value =  5.5 * 1000000;
+            else if (tmpRate == 0x96)
+                iwe.u.bitrate.value =  11 * 1000000;
+            else
+    		    iwe.u.bitrate.value =  (tmpRate/2) * 1000000;
+
+			iwe.u.bitrate.disabled = 0;
+			current_val = IWE_STREAM_ADD_VALUE(info, current_ev,
+				current_val, end_buf, &iwe,
+    			IW_EV_PARAM_LEN);
+
+        	if((current_val-current_ev)>IW_EV_LCP_LEN)
+            	current_ev = current_val;
+        	else
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+        }
+
+#ifdef IWEVGENIE
+		//WPA IE
+		if (pAdapter->ScanTab.BssEntry[i].WpaIE.IELen > 0)
+		{
+			memset(&iwe, 0, sizeof(iwe));
+			memset(&custom[0], 0, MAX_CUSTOM_LEN);
+			memcpy(custom, &(pAdapter->ScanTab.BssEntry[i].WpaIE.IE[0]),
+						   pAdapter->ScanTab.BssEntry[i].WpaIE.IELen);
+			iwe.cmd = IWEVGENIE;
+			iwe.u.data.length = pAdapter->ScanTab.BssEntry[i].WpaIE.IELen;
+			current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf, &iwe, custom);
+			if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+		}
+
+		//WPA2 IE
+        if (pAdapter->ScanTab.BssEntry[i].RsnIE.IELen > 0)
+        {
+        	memset(&iwe, 0, sizeof(iwe));
+			memset(&custom[0], 0, MAX_CUSTOM_LEN);
+			memcpy(custom, &(pAdapter->ScanTab.BssEntry[i].RsnIE.IE[0]),
+						   pAdapter->ScanTab.BssEntry[i].RsnIE.IELen);
+			iwe.cmd = IWEVGENIE;
+			iwe.u.data.length = pAdapter->ScanTab.BssEntry[i].RsnIE.IELen;
+			current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf, &iwe, custom);
+			if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+        }
+#else
+        //WPA IE
+		//================================
+        if (pAdapter->ScanTab.BssEntry[i].WpaIE.IELen > 0)
+        {
+    		NdisZeroMemory(&iwe, sizeof(iwe));
+			memset(&custom[0], 0, MAX_CUSTOM_LEN);
+    		iwe.cmd = IWEVCUSTOM;
+            iwe.u.data.length = (pAdapter->ScanTab.BssEntry[i].WpaIE.IELen * 2) + 7;
+            NdisMoveMemory(custom, "wpa_ie=", 7);
+            for (idx = 0; idx < pAdapter->ScanTab.BssEntry[i].WpaIE.IELen; idx++)
+                sprintf(custom, "%s%02x", custom, pAdapter->ScanTab.BssEntry[i].WpaIE.IE[idx]);
+            previous_ev = current_ev;
+    		current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf, &iwe,  custom);
+            if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+        }
+
+        //WPA2 IE
+        if (pAdapter->ScanTab.BssEntry[i].RsnIE.IELen > 0)
+        {
+    		NdisZeroMemory(&iwe, sizeof(iwe));
+			memset(&custom[0], 0, MAX_CUSTOM_LEN);
+    		iwe.cmd = IWEVCUSTOM;
+            iwe.u.data.length = (pAdapter->ScanTab.BssEntry[i].RsnIE.IELen * 2) + 7;
+            NdisMoveMemory(custom, "rsn_ie=", 7);
+			for (idx = 0; idx < pAdapter->ScanTab.BssEntry[i].RsnIE.IELen; idx++)
+                sprintf(custom, "%s%02x", custom, pAdapter->ScanTab.BssEntry[i].RsnIE.IE[idx]);
+            previous_ev = current_ev;
+    		current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf, &iwe,  custom);
+            if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+        }
+#endif // IWEVGENIE //
+	}
+
+	data->length = current_ev - extra;
+    pAdapter->StaCfg.bScanReqIsFromWebUI = FALSE;
+	DBGPRINT(RT_DEBUG_ERROR ,("===>rt_ioctl_giwscan. %d(%d) BSS returned, data->length = %d\n",i , pAdapter->ScanTab.BssNr, data->length));
+	return 0;
+}
+#endif
+
+int rt_ioctl_siwessid(struct net_device *dev,
+			 struct iw_request_info *info,
+			 struct iw_point *data, char *essid)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+
+	//check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+       	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+       	return -ENETDOWN;
+    }
+
+	if (data->flags)
+	{
+		PCHAR	pSsidString = NULL;
+
+		// Includes null character.
+		if (data->length > (IW_ESSID_MAX_SIZE + 1))
+			return -E2BIG;
+
+		pSsidString = (CHAR *) kmalloc(MAX_LEN_OF_SSID+1, MEM_ALLOC_FLAG);
+		if (pSsidString)
+		{
+			NdisZeroMemory(pSsidString, MAX_LEN_OF_SSID+1);
+			NdisMoveMemory(pSsidString, essid, data->length);
+			if (Set_SSID_Proc(pAdapter, pSsidString) == FALSE)
+				return -EINVAL;
+		}
+		else
+			return -ENOMEM;
+	}
+	else
+	{
+		// ANY ssid
+		if (Set_SSID_Proc(pAdapter, "") == FALSE)
+			return -EINVAL;
+    }
+	return 0;
+}
+
+int rt_ioctl_giwessid(struct net_device *dev,
+			 struct iw_request_info *info,
+			 struct iw_point *data, char *essid)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	data->flags = 1;
+    if (MONITOR_ON(pAdapter))
+    {
+        data->length  = 0;
+        return 0;
+    }
+
+	if (OPSTATUS_TEST_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED))
+	{
+		DBGPRINT(RT_DEBUG_TRACE ,("MediaState is connected\n"));
+		data->length = pAdapter->CommonCfg.SsidLen;
+		memcpy(essid, pAdapter->CommonCfg.Ssid, pAdapter->CommonCfg.SsidLen);
+	}
+	else
+	{//the ANY ssid was specified
+		data->length  = 0;
+		DBGPRINT(RT_DEBUG_TRACE ,("MediaState is not connected, ess\n"));
+	}
+
+	return 0;
+
+}
+
+int rt_ioctl_siwnickn(struct net_device *dev,
+			 struct iw_request_info *info,
+			 struct iw_point *data, char *nickname)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+
+    //check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+        DBGPRINT(RT_DEBUG_TRACE ,("INFO::Network is down!\n"));
+        return -ENETDOWN;
+    }
+
+	if (data->length > IW_ESSID_MAX_SIZE)
+		return -EINVAL;
+
+	memset(pAdapter->nickname, 0, IW_ESSID_MAX_SIZE + 1);
+	memcpy(pAdapter->nickname, nickname, data->length);
+
+
+	return 0;
+}
+
+int rt_ioctl_giwnickn(struct net_device *dev,
+			 struct iw_request_info *info,
+			 struct iw_point *data, char *nickname)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	if (data->length > strlen(pAdapter->nickname) + 1)
+		data->length = strlen(pAdapter->nickname) + 1;
+	if (data->length > 0) {
+		memcpy(nickname, pAdapter->nickname, data->length-1);
+		nickname[data->length-1] = '\0';
+	}
+	return 0;
+}
+
+int rt_ioctl_siwrts(struct net_device *dev,
+		       struct iw_request_info *info,
+		       struct iw_param *rts, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+	u16 val;
+
+    //check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        return -ENETDOWN;
+    }
+
+	if (rts->disabled)
+		val = MAX_RTS_THRESHOLD;
+	else if (rts->value < 0 || rts->value > MAX_RTS_THRESHOLD)
+		return -EINVAL;
+	else if (rts->value == 0)
+	    val = MAX_RTS_THRESHOLD;
+	else
+		val = rts->value;
+
+	if (val != pAdapter->CommonCfg.RtsThreshold)
+		pAdapter->CommonCfg.RtsThreshold = val;
+
+	return 0;
+}
+
+int rt_ioctl_giwrts(struct net_device *dev,
+		       struct iw_request_info *info,
+		       struct iw_param *rts, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	//check if the interface is down
+    	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    	{
+      		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        	return -ENETDOWN;
+    	}
+
+	rts->value = pAdapter->CommonCfg.RtsThreshold;
+	rts->disabled = (rts->value == MAX_RTS_THRESHOLD);
+	rts->fixed = 1;
+
+	return 0;
+}
+
+int rt_ioctl_siwfrag(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_param *frag, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+	u16 val;
+
+	//check if the interface is down
+    	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    	{
+      		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        	return -ENETDOWN;
+    	}
+
+	if (frag->disabled)
+		val = MAX_FRAG_THRESHOLD;
+	else if (frag->value >= MIN_FRAG_THRESHOLD || frag->value <= MAX_FRAG_THRESHOLD)
+        val = __cpu_to_le16(frag->value & ~0x1); /* even numbers only */
+	else if (frag->value == 0)
+	    val = MAX_FRAG_THRESHOLD;
+	else
+		return -EINVAL;
+
+	pAdapter->CommonCfg.FragmentThreshold = val;
+	return 0;
+}
+
+int rt_ioctl_giwfrag(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_param *frag, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	//check if the interface is down
+    	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    	{
+      		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        	return -ENETDOWN;
+    	}
+
+	frag->value = pAdapter->CommonCfg.FragmentThreshold;
+	frag->disabled = (frag->value == MAX_FRAG_THRESHOLD);
+	frag->fixed = 1;
+
+	return 0;
+}
+
+#define MAX_WEP_KEY_SIZE 13
+#define MIN_WEP_KEY_SIZE 5
+int rt_ioctl_siwencode(struct net_device *dev,
+			  struct iw_request_info *info,
+			  struct iw_point *erq, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+
+	//check if the interface is down
+    	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    	{
+      		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        	return -ENETDOWN;
+    	}
+
+	if ((erq->length == 0) &&
+        (erq->flags & IW_ENCODE_DISABLED))
+	{
+		pAdapter->StaCfg.PairCipher = Ndis802_11WEPDisabled;
+		pAdapter->StaCfg.GroupCipher = Ndis802_11WEPDisabled;
+		pAdapter->StaCfg.WepStatus = Ndis802_11WEPDisabled;
+        pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+        goto done;
+	}
+	else if ((erq->length == 0) &&
+             (erq->flags & IW_ENCODE_RESTRICTED || erq->flags & IW_ENCODE_OPEN))
+	{
+		STA_PORT_SECURED(pAdapter);
+		pAdapter->StaCfg.PairCipher = Ndis802_11WEPEnabled;
+		pAdapter->StaCfg.GroupCipher = Ndis802_11WEPEnabled;
+		pAdapter->StaCfg.WepStatus = Ndis802_11WEPEnabled;
+        pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+		if (erq->flags & IW_ENCODE_RESTRICTED)
+			pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeShared;
+    	else
+			pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+        goto done;
+	}
+
+    if (erq->length > 0)
+	{
+		int keyIdx = (erq->flags & IW_ENCODE_INDEX) - 1;
+		/* Check the size of the key */
+		if (erq->length > MAX_WEP_KEY_SIZE) {
+			return -EINVAL;
+		}
+		/* Check key index */
+		if ((keyIdx < 0) || (keyIdx >= NR_WEP_KEYS))
+        {
+            DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::Wrong keyIdx=%d! Using default key instead (%d)\n",
+                                        keyIdx, pAdapter->StaCfg.DefaultKeyId));
+
+            //Using default key
+			keyIdx = pAdapter->StaCfg.DefaultKeyId;
+        }
+
+        NdisZeroMemory(pAdapter->SharedKey[BSS0][keyIdx].Key,  16);
+
+		if (erq->length == MAX_WEP_KEY_SIZE)
+        {
+			pAdapter->SharedKey[BSS0][keyIdx].KeyLen = MAX_WEP_KEY_SIZE;
+            pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_WEP128;
+		}
+		else if (erq->length == MIN_WEP_KEY_SIZE)
+        {
+            pAdapter->SharedKey[BSS0][keyIdx].KeyLen = MIN_WEP_KEY_SIZE;
+            pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_WEP64;
+		}
+		else
+			/* Disable the key */
+			pAdapter->SharedKey[BSS0][keyIdx].KeyLen = 0;
+
+		/* Check if the key is not marked as invalid */
+		if(!(erq->flags & IW_ENCODE_NOKEY)) {
+			/* Copy the key in the driver */
+			NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].Key, extra, erq->length);
+        }
+	}
+    else
+			{
+		/* Do we want to just set the transmit key index ? */
+		int index = (erq->flags & IW_ENCODE_INDEX) - 1;
+		if ((index >= 0) && (index < 4))
+        {
+			pAdapter->StaCfg.DefaultKeyId = index;
+            }
+        else
+			/* Don't complain if only change the mode */
+			if(!erq->flags & IW_ENCODE_MODE) {
+				return -EINVAL;
+		}
+	}
+
+done:
+    DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::erq->flags=%x\n",erq->flags));
+	DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::AuthMode=%x\n",pAdapter->StaCfg.AuthMode));
+	DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::DefaultKeyId=%x, KeyLen = %d\n",pAdapter->StaCfg.DefaultKeyId , pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen));
+	DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::WepStatus=%x\n",pAdapter->StaCfg.WepStatus));
+	return 0;
+}
+
+int
+rt_ioctl_giwencode(struct net_device *dev,
+			  struct iw_request_info *info,
+			  struct iw_point *erq, char *key)
+{
+	int kid;
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	//check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+	kid = erq->flags & IW_ENCODE_INDEX;
+	DBGPRINT(RT_DEBUG_TRACE, ("===>rt_ioctl_giwencode %d\n", erq->flags & IW_ENCODE_INDEX));
+
+	if (pAdapter->StaCfg.WepStatus == Ndis802_11WEPDisabled)
+	{
+		erq->length = 0;
+		erq->flags = IW_ENCODE_DISABLED;
+	}
+	else if ((kid > 0) && (kid <=4))
+	{
+		// copy wep key
+		erq->flags = kid ;			/* NB: base 1 */
+		if (erq->length > pAdapter->SharedKey[BSS0][kid-1].KeyLen)
+			erq->length = pAdapter->SharedKey[BSS0][kid-1].KeyLen;
+		memcpy(key, pAdapter->SharedKey[BSS0][kid-1].Key, erq->length);
+		//if ((kid == pAdapter->PortCfg.DefaultKeyId))
+		//erq->flags |= IW_ENCODE_ENABLED;	/* XXX */
+		if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeShared)
+			erq->flags |= IW_ENCODE_RESTRICTED;		/* XXX */
+		else
+			erq->flags |= IW_ENCODE_OPEN;		/* XXX */
+
+	}
+	else if (kid == 0)
+	{
+		if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeShared)
+			erq->flags |= IW_ENCODE_RESTRICTED;		/* XXX */
+		else
+			erq->flags |= IW_ENCODE_OPEN;		/* XXX */
+		erq->length = pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen;
+		memcpy(key, pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].Key, erq->length);
+		// copy default key ID
+		if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeShared)
+			erq->flags |= IW_ENCODE_RESTRICTED;		/* XXX */
+		else
+			erq->flags |= IW_ENCODE_OPEN;		/* XXX */
+		erq->flags = pAdapter->StaCfg.DefaultKeyId + 1;			/* NB: base 1 */
+		erq->flags |= IW_ENCODE_ENABLED;	/* XXX */
+	}
+
+	return 0;
+
+}
+
+static int
+rt_ioctl_setparam(struct net_device *dev, struct iw_request_info *info,
+			 void *w, char *extra)
+{
+    VIRTUAL_ADAPTER	*pVirtualAd = NULL;
+	PRTMP_ADAPTER pAdapter;
+	POS_COOKIE pObj;
+	char *this_char = extra;
+	char *value;
+	int  Status=0;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+	pObj = (POS_COOKIE) pAdapter->OS_Cookie;
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	{
+		pObj->ioctl_if_type = INT_MAIN;
+        pObj->ioctl_if = MAIN_MBSSID;
+	}
+
+	//check if the interface is down
+    	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    	{
+      		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+			return -ENETDOWN;
+    	}
+
+	if (!*this_char)
+		return -EINVAL;
+
+	if ((value = rtstrchr(this_char, '=')) != NULL)
+	    *value++ = 0;
+
+	if (!value)
+	    return -EINVAL;
+
+	// reject setting nothing besides ANY ssid(ssidLen=0)
+    if (!*value && (strcmp(this_char, "SSID") != 0))
+        return -EINVAL;
+
+	for (PRTMP_PRIVATE_SET_PROC = RTMP_PRIVATE_SUPPORT_PROC; PRTMP_PRIVATE_SET_PROC->name; PRTMP_PRIVATE_SET_PROC++)
+	{
+	    if (strcmp(this_char, PRTMP_PRIVATE_SET_PROC->name) == 0)
+	    {
+	        if(!PRTMP_PRIVATE_SET_PROC->set_proc(pAdapter, value))
+	        {	//FALSE:Set private failed then return Invalid argument
+			    Status = -EINVAL;
+	        }
+		    break;	//Exit for loop.
+	    }
+	}
+
+	if(PRTMP_PRIVATE_SET_PROC->name == NULL)
+	{  //Not found argument
+	    Status = -EINVAL;
+	    DBGPRINT(RT_DEBUG_TRACE, ("===>rt_ioctl_setparam:: (iwpriv) Not Support Set Command [%s=%s]\n", this_char, value));
+	}
+
+    return Status;
+}
+
+
+static int
+rt_private_get_statistics(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra)
+{
+	INT				Status = 0;
+    PRTMP_ADAPTER   pAd = dev->ml_priv;
+
+    if (extra == NULL)
+    {
+        wrq->length = 0;
+        return -EIO;
+    }
+
+    memset(extra, 0x00, IW_PRIV_SIZE_MASK);
+    sprintf(extra, "\n\n");
+
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+	{
+	    sprintf(extra+strlen(extra), "Tx success                      = %ld\n", (ULONG)pAd->ate.TxDoneCount);
+	    //sprintf(extra+strlen(extra), "Tx success without retry        = %ld\n", (ULONG)pAd->ate.TxDoneCount);
+	}
+	else
+#endif // RALINK_ATE //
+	{
+    sprintf(extra+strlen(extra), "Tx success                      = %ld\n", (ULONG)pAd->WlanCounters.TransmittedFragmentCount.QuadPart);
+    sprintf(extra+strlen(extra), "Tx success without retry        = %ld\n", (ULONG)pAd->WlanCounters.TransmittedFragmentCount.QuadPart - (ULONG)pAd->WlanCounters.RetryCount.QuadPart);
+	}
+    sprintf(extra+strlen(extra), "Tx success after retry          = %ld\n", (ULONG)pAd->WlanCounters.RetryCount.QuadPart);
+    sprintf(extra+strlen(extra), "Tx fail to Rcv ACK after retry  = %ld\n", (ULONG)pAd->WlanCounters.FailedCount.QuadPart);
+    sprintf(extra+strlen(extra), "RTS Success Rcv CTS             = %ld\n", (ULONG)pAd->WlanCounters.RTSSuccessCount.QuadPart);
+    sprintf(extra+strlen(extra), "RTS Fail Rcv CTS                = %ld\n", (ULONG)pAd->WlanCounters.RTSFailureCount.QuadPart);
+
+    sprintf(extra+strlen(extra), "Rx success                      = %ld\n", (ULONG)pAd->WlanCounters.ReceivedFragmentCount.QuadPart);
+    sprintf(extra+strlen(extra), "Rx with CRC                     = %ld\n", (ULONG)pAd->WlanCounters.FCSErrorCount.QuadPart);
+    sprintf(extra+strlen(extra), "Rx drop due to out of resource  = %ld\n", (ULONG)pAd->Counters8023.RxNoBuffer);
+    sprintf(extra+strlen(extra), "Rx duplicate frame              = %ld\n", (ULONG)pAd->WlanCounters.FrameDuplicateCount.QuadPart);
+
+    sprintf(extra+strlen(extra), "False CCA (one second)          = %ld\n", (ULONG)pAd->RalinkCounters.OneSecFalseCCACnt);
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+	{
+		if (pAd->ate.RxAntennaSel == 0)
+		{
+    		sprintf(extra+strlen(extra), "RSSI-A                          = %ld\n", (LONG)(pAd->ate.LastRssi0 - pAd->BbpRssiToDbmDelta));
+			sprintf(extra+strlen(extra), "RSSI-B (if available)           = %ld\n", (LONG)(pAd->ate.LastRssi1 - pAd->BbpRssiToDbmDelta));
+			sprintf(extra+strlen(extra), "RSSI-C (if available)           = %ld\n\n", (LONG)(pAd->ate.LastRssi2 - pAd->BbpRssiToDbmDelta));
+		}
+		else
+		{
+    		sprintf(extra+strlen(extra), "RSSI                            = %ld\n", (LONG)(pAd->ate.LastRssi0 - pAd->BbpRssiToDbmDelta));
+		}
+	}
+	else
+#endif // RALINK_ATE //
+	{
+    	sprintf(extra+strlen(extra), "RSSI-A                          = %ld\n", (LONG)(pAd->StaCfg.RssiSample.LastRssi0 - pAd->BbpRssiToDbmDelta));
+        sprintf(extra+strlen(extra), "RSSI-B (if available)           = %ld\n", (LONG)(pAd->StaCfg.RssiSample.LastRssi1 - pAd->BbpRssiToDbmDelta));
+        sprintf(extra+strlen(extra), "RSSI-C (if available)           = %ld\n\n", (LONG)(pAd->StaCfg.RssiSample.LastRssi2 - pAd->BbpRssiToDbmDelta));
+	}
+#ifdef WPA_SUPPLICANT_SUPPORT
+    sprintf(extra+strlen(extra), "WpaSupplicantUP                 = %d\n\n", pAd->StaCfg.WpaSupplicantUP);
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+
+    wrq->length = strlen(extra) + 1; // 1: size of '\0'
+    DBGPRINT(RT_DEBUG_TRACE, ("<== rt_private_get_statistics, wrq->length = %d\n", wrq->length));
+
+    return Status;
+}
+
+#ifdef DOT11_N_SUPPORT
+void	getBaInfo(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pOutBuf)
+{
+	INT i, j;
+	BA_ORI_ENTRY *pOriBAEntry;
+	BA_REC_ENTRY *pRecBAEntry;
+
+	for (i=0; i<MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		PMAC_TABLE_ENTRY pEntry = &pAd->MacTab.Content[i];
+		if (((pEntry->ValidAsCLI || pEntry->ValidAsApCli) && (pEntry->Sst == SST_ASSOC))
+			|| (pEntry->ValidAsWDS) || (pEntry->ValidAsMesh))
+		{
+			sprintf(pOutBuf, "%s\n%02X:%02X:%02X:%02X:%02X:%02X (Aid = %d) (AP) -\n",
+                pOutBuf,
+				pEntry->Addr[0], pEntry->Addr[1], pEntry->Addr[2],
+				pEntry->Addr[3], pEntry->Addr[4], pEntry->Addr[5], pEntry->Aid);
+
+			sprintf(pOutBuf, "%s[Recipient]\n", pOutBuf);
+			for (j=0; j < NUM_OF_TID; j++)
+			{
+				if (pEntry->BARecWcidArray[j] != 0)
+				{
+					pRecBAEntry =&pAd->BATable.BARecEntry[pEntry->BARecWcidArray[j]];
+					sprintf(pOutBuf, "%sTID=%d, BAWinSize=%d, LastIndSeq=%d, ReorderingPkts=%d\n", pOutBuf, j, pRecBAEntry->BAWinSize, pRecBAEntry->LastIndSeq, pRecBAEntry->list.qlen);
+				}
+			}
+			sprintf(pOutBuf, "%s\n", pOutBuf);
+
+			sprintf(pOutBuf, "%s[Originator]\n", pOutBuf);
+			for (j=0; j < NUM_OF_TID; j++)
+			{
+				if (pEntry->BAOriWcidArray[j] != 0)
+				{
+					pOriBAEntry =&pAd->BATable.BAOriEntry[pEntry->BAOriWcidArray[j]];
+					sprintf(pOutBuf, "%sTID=%d, BAWinSize=%d, StartSeq=%d, CurTxSeq=%d\n", pOutBuf, j, pOriBAEntry->BAWinSize, pOriBAEntry->Sequence, pEntry->TxSeq[j]);
+				}
+			}
+			sprintf(pOutBuf, "%s\n\n", pOutBuf);
+		}
+        if (strlen(pOutBuf) > (IW_PRIV_SIZE_MASK - 30))
+                break;
+	}
+
+	return;
+}
+#endif // DOT11_N_SUPPORT //
+
+static int
+rt_private_show(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra)
+{
+    INT				Status = 0;
+    VIRTUAL_ADAPTER	*pVirtualAd = NULL;
+    PRTMP_ADAPTER   pAd;
+	POS_COOKIE		pObj;
+    u32             subcmd = wrq->flags;
+
+	if (dev->priv_flags == INT_MAIN)
+		pAd = dev->ml_priv;
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		pAd = pVirtualAd->RtmpDev->ml_priv;
+	}
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	if (pAd == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+    if (extra == NULL)
+    {
+        wrq->length = 0;
+        return -EIO;
+    }
+    memset(extra, 0x00, IW_PRIV_SIZE_MASK);
+
+	{
+		pObj->ioctl_if_type = INT_MAIN;
+        pObj->ioctl_if = MAIN_MBSSID;
+	}
+
+    switch(subcmd)
+    {
+
+        case SHOW_CONN_STATUS:
+            if (MONITOR_ON(pAd))
+            {
+#ifdef DOT11_N_SUPPORT
+                if (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED &&
+                    pAd->CommonCfg.RegTransmitSetting.field.BW)
+                    sprintf(extra, "Monitor Mode(CentralChannel %d)\n", pAd->CommonCfg.CentralChannel);
+                else
+#endif // DOT11_N_SUPPORT //
+                    sprintf(extra, "Monitor Mode(Channel %d)\n", pAd->CommonCfg.Channel);
+            }
+            else
+            {
+                if (pAd->IndicateMediaState == NdisMediaStateConnected)
+            	{
+            	    if (INFRA_ON(pAd))
+                    {
+                    sprintf(extra, "Connected(AP: %s[%02X:%02X:%02X:%02X:%02X:%02X])\n",
+                                    pAd->CommonCfg.Ssid,
+                                    pAd->CommonCfg.Bssid[0],
+                                    pAd->CommonCfg.Bssid[1],
+                                    pAd->CommonCfg.Bssid[2],
+                                    pAd->CommonCfg.Bssid[3],
+                                    pAd->CommonCfg.Bssid[4],
+                                    pAd->CommonCfg.Bssid[5]);
+            		DBGPRINT(RT_DEBUG_TRACE ,("Ssid=%s ,Ssidlen = %d\n",pAd->CommonCfg.Ssid, pAd->CommonCfg.SsidLen));
+            	}
+                    else if (ADHOC_ON(pAd))
+                        sprintf(extra, "Connected\n");
+            	}
+            	else
+            	{
+            	    sprintf(extra, "Disconnected\n");
+            		DBGPRINT(RT_DEBUG_TRACE ,("ConnStatus is not connected\n"));
+            	}
+            }
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+        case SHOW_DRVIER_VERION:
+            sprintf(extra, "Driver version-%s, %s %s\n", STA_DRIVER_VERSION, __DATE__, __TIME__ );
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+#ifdef DOT11_N_SUPPORT
+        case SHOW_BA_INFO:
+            getBaInfo(pAd, extra);
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+#endif // DOT11_N_SUPPORT //
+		case SHOW_DESC_INFO:
+			{
+				Show_DescInfo_Proc(pAd, NULL);
+				wrq->length = 0; // 1: size of '\0'
+			}
+			break;
+        case RAIO_OFF:
+            if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+            {
+                sprintf(extra, "Scanning\n");
+                wrq->length = strlen(extra) + 1; // 1: size of '\0'
+                break;
+            }
+            pAd->StaCfg.bSwRadio = FALSE;
+            if (pAd->StaCfg.bRadio != (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio))
+            {
+                pAd->StaCfg.bRadio = (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio);
+                if (pAd->StaCfg.bRadio == FALSE)
+                {
+                    MlmeRadioOff(pAd);
+                    // Update extra information
+					pAd->ExtraInfo = SW_RADIO_OFF;
+                }
+            }
+            sprintf(extra, "Radio Off\n");
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+        case RAIO_ON:
+            if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+            {
+                sprintf(extra, "Scanning\n");
+                wrq->length = strlen(extra) + 1; // 1: size of '\0'
+                break;
+            }
+            pAd->StaCfg.bSwRadio = TRUE;
+            //if (pAd->StaCfg.bRadio != (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio))
+            {
+                pAd->StaCfg.bRadio = (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio);
+                if (pAd->StaCfg.bRadio == TRUE)
+                {
+                    MlmeRadioOn(pAd);
+                    // Update extra information
+					pAd->ExtraInfo = EXTRA_INFO_CLEAR;
+                }
+            }
+            sprintf(extra, "Radio On\n");
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+
+
+#ifdef QOS_DLS_SUPPORT
+		case SHOW_DLS_ENTRY_INFO:
+			{
+				Set_DlsEntryInfo_Display_Proc(pAd, NULL);
+				wrq->length = 0; // 1: size of '\0'
+			}
+			break;
+#endif // QOS_DLS_SUPPORT //
+
+		case SHOW_CFG_VALUE:
+			{
+				Status = RTMPShowCfgValue(pAd, wrq->pointer, extra);
+				if (Status == 0)
+					wrq->length = strlen(extra) + 1; // 1: size of '\0'
+			}
+			break;
+        default:
+            DBGPRINT(RT_DEBUG_TRACE, ("%s - unknow subcmd = %d\n", __FUNCTION__, subcmd));
+            break;
+    }
+
+    return Status;
+}
+
+#ifdef SIOCSIWMLME
+int rt_ioctl_siwmlme(struct net_device *dev,
+			   struct iw_request_info *info,
+			   union iwreq_data *wrqu,
+			   char *extra)
+{
+	PRTMP_ADAPTER   pAd = dev->ml_priv;
+	struct iw_mlme *pMlme = (struct iw_mlme *)wrqu->data.pointer;
+	MLME_QUEUE_ELEM				MsgElem;
+	MLME_DISASSOC_REQ_STRUCT	DisAssocReq;
+	MLME_DEAUTH_REQ_STRUCT      DeAuthReq;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("====> %s\n", __FUNCTION__));
+
+	if (pMlme == NULL)
+		return -EINVAL;
+
+	switch(pMlme->cmd)
+	{
+#ifdef IW_MLME_DEAUTH
+		case IW_MLME_DEAUTH:
+			DBGPRINT(RT_DEBUG_TRACE, ("====> %s - IW_MLME_DEAUTH\n", __FUNCTION__));
+			COPY_MAC_ADDR(DeAuthReq.Addr, pAd->CommonCfg.Bssid);
+			DeAuthReq.Reason = pMlme->reason_code;
+			MsgElem.MsgLen = sizeof(MLME_DEAUTH_REQ_STRUCT);
+			NdisMoveMemory(MsgElem.Msg, &DeAuthReq, sizeof(MLME_DEAUTH_REQ_STRUCT));
+			MlmeDeauthReqAction(pAd, &MsgElem);
+			if (INFRA_ON(pAd))
+			{
+			    LinkDown(pAd, FALSE);
+			    pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+			}
+			break;
+#endif // IW_MLME_DEAUTH //
+#ifdef IW_MLME_DISASSOC
+		case IW_MLME_DISASSOC:
+			DBGPRINT(RT_DEBUG_TRACE, ("====> %s - IW_MLME_DISASSOC\n", __FUNCTION__));
+			COPY_MAC_ADDR(DisAssocReq.Addr, pAd->CommonCfg.Bssid);
+			DisAssocReq.Reason =  pMlme->reason_code;
+
+			MsgElem.Machine = ASSOC_STATE_MACHINE;
+			MsgElem.MsgType = MT2_MLME_DISASSOC_REQ;
+			MsgElem.MsgLen = sizeof(MLME_DISASSOC_REQ_STRUCT);
+			NdisMoveMemory(MsgElem.Msg, &DisAssocReq, sizeof(MLME_DISASSOC_REQ_STRUCT));
+
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_DISASSOC;
+			MlmeDisassocReqAction(pAd, &MsgElem);
+			break;
+#endif // IW_MLME_DISASSOC //
+		default:
+			DBGPRINT(RT_DEBUG_TRACE, ("====> %s - Unknow Command\n", __FUNCTION__));
+			break;
+	}
+
+	return 0;
+}
+#endif // SIOCSIWMLME //
+
+#if WIRELESS_EXT > 17
+int rt_ioctl_siwauth(struct net_device *dev,
+			  struct iw_request_info *info,
+			  union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER   pAdapter = dev->ml_priv;
+	struct iw_param *param = &wrqu->param;
+
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+	switch (param->flags & IW_AUTH_INDEX) {
+    	case IW_AUTH_WPA_VERSION:
+            if (param->value == IW_AUTH_WPA_VERSION_WPA)
+            {
+                pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPAPSK;
+				if (pAdapter->StaCfg.BssType == BSS_ADHOC)
+					pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPANone;
+            }
+            else if (param->value == IW_AUTH_WPA_VERSION_WPA2)
+                pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA2PSK;
+
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_WPA_VERSION - param->value = %d!\n", __FUNCTION__, param->value));
+            break;
+    	case IW_AUTH_CIPHER_PAIRWISE:
+            if (param->value == IW_AUTH_CIPHER_NONE)
+            {
+                pAdapter->StaCfg.WepStatus = Ndis802_11WEPDisabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11WEPDisabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_WEP40 ||
+                     param->value == IW_AUTH_CIPHER_WEP104)
+            {
+                pAdapter->StaCfg.WepStatus = Ndis802_11WEPEnabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11WEPEnabled;
+#ifdef WPA_SUPPLICANT_SUPPORT
+                pAdapter->StaCfg.IEEE8021X = FALSE;
+#endif // WPA_SUPPLICANT_SUPPORT //
+            }
+            else if (param->value == IW_AUTH_CIPHER_TKIP)
+            {
+                pAdapter->StaCfg.WepStatus = Ndis802_11Encryption2Enabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11Encryption2Enabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_CCMP)
+            {
+                pAdapter->StaCfg.WepStatus = Ndis802_11Encryption3Enabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11Encryption3Enabled;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_CIPHER_PAIRWISE - param->value = %d!\n", __FUNCTION__, param->value));
+            break;
+    	case IW_AUTH_CIPHER_GROUP:
+            if (param->value == IW_AUTH_CIPHER_NONE)
+            {
+                pAdapter->StaCfg.GroupCipher = Ndis802_11WEPDisabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_WEP40 ||
+                     param->value == IW_AUTH_CIPHER_WEP104)
+            {
+                pAdapter->StaCfg.GroupCipher = Ndis802_11WEPEnabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_TKIP)
+            {
+                pAdapter->StaCfg.GroupCipher = Ndis802_11Encryption2Enabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_CCMP)
+            {
+                pAdapter->StaCfg.GroupCipher = Ndis802_11Encryption3Enabled;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_CIPHER_GROUP - param->value = %d!\n", __FUNCTION__, param->value));
+            break;
+    	case IW_AUTH_KEY_MGMT:
+            if (param->value == IW_AUTH_KEY_MGMT_802_1X)
+            {
+                if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK)
+                {
+                    pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA;
+#ifdef WPA_SUPPLICANT_SUPPORT
+                    pAdapter->StaCfg.IEEE8021X = FALSE;
+#endif // WPA_SUPPLICANT_SUPPORT //
+                }
+                else if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
+                {
+                    pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA2;
+#ifdef WPA_SUPPLICANT_SUPPORT
+                    pAdapter->StaCfg.IEEE8021X = FALSE;
+#endif // WPA_SUPPLICANT_SUPPORT //
+                }
+#ifdef WPA_SUPPLICANT_SUPPORT
+                else
+                    // WEP 1x
+                    pAdapter->StaCfg.IEEE8021X = TRUE;
+#endif // WPA_SUPPLICANT_SUPPORT //
+            }
+            else if (param->value == 0)
+            {
+				STA_PORT_SECURED(pAdapter);
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_KEY_MGMT - param->value = %d!\n", __FUNCTION__, param->value));
+            break;
+    	case IW_AUTH_RX_UNENCRYPTED_EAPOL:
+            break;
+    	case IW_AUTH_PRIVACY_INVOKED:
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_PRIVACY_INVOKED - param->value = %d!\n", __FUNCTION__, param->value));
+    		break;
+    	case IW_AUTH_DROP_UNENCRYPTED:
+            if (param->value != 0)
+                pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+			else
+			{
+				STA_PORT_SECURED(pAdapter);
+			}
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_WPA_VERSION - param->value = %d!\n", __FUNCTION__, param->value));
+    		break;
+    	case IW_AUTH_80211_AUTH_ALG:
+			if (param->value & IW_AUTH_ALG_SHARED_KEY)
+            {
+				pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeShared;
+			}
+            else if (param->value & IW_AUTH_ALG_OPEN_SYSTEM)
+            {
+				pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+			}
+            else
+				return -EINVAL;
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_80211_AUTH_ALG - param->value = %d!\n", __FUNCTION__, param->value));
+			break;
+    	case IW_AUTH_WPA_ENABLED:
+    		DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_WPA_ENABLED - Driver supports WPA!(param->value = %d)\n", __FUNCTION__, param->value));
+    		break;
+    	default:
+    		return -EOPNOTSUPP;
+}
+
+	return 0;
+}
+
+int rt_ioctl_giwauth(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER   pAdapter = dev->ml_priv;
+	struct iw_param *param = &wrqu->param;
+
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+    }
+
+	switch (param->flags & IW_AUTH_INDEX) {
+	case IW_AUTH_DROP_UNENCRYPTED:
+        param->value = (pAdapter->StaCfg.WepStatus == Ndis802_11WEPDisabled) ? 0 : 1;
+		break;
+
+	case IW_AUTH_80211_AUTH_ALG:
+        param->value = (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeShared) ? IW_AUTH_ALG_SHARED_KEY : IW_AUTH_ALG_OPEN_SYSTEM;
+		break;
+
+	case IW_AUTH_WPA_ENABLED:
+		param->value = (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA) ? 1 : 0;
+		break;
+
+	default:
+		return -EOPNOTSUPP;
+	}
+    DBGPRINT(RT_DEBUG_TRACE, ("rt_ioctl_giwauth::param->value = %d!\n", param->value));
+	return 0;
+}
+
+void fnSetCipherKey(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  INT             keyIdx,
+    IN  UCHAR           CipherAlg,
+    IN  BOOLEAN         bGTK,
+    IN  struct iw_encode_ext *ext)
+{
+    NdisZeroMemory(&pAdapter->SharedKey[BSS0][keyIdx], sizeof(CIPHER_KEY));
+    pAdapter->SharedKey[BSS0][keyIdx].KeyLen = LEN_TKIP_EK;
+    NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].Key, ext->key, LEN_TKIP_EK);
+    NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].TxMic, ext->key + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+    NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].RxMic, ext->key + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+    pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CipherAlg;
+
+    // Update group key information to ASIC Shared Key Table
+	AsicAddSharedKeyEntry(pAdapter,
+						  BSS0,
+						  keyIdx,
+						  pAdapter->SharedKey[BSS0][keyIdx].CipherAlg,
+						  pAdapter->SharedKey[BSS0][keyIdx].Key,
+						  pAdapter->SharedKey[BSS0][keyIdx].TxMic,
+						  pAdapter->SharedKey[BSS0][keyIdx].RxMic);
+
+    if (bGTK)
+        // Update ASIC WCID attribute table and IVEIV table
+    	RTMPAddWcidAttributeEntry(pAdapter,
+    							  BSS0,
+    							  keyIdx,
+    							  pAdapter->SharedKey[BSS0][keyIdx].CipherAlg,
+    							  NULL);
+    else
+        // Update ASIC WCID attribute table and IVEIV table
+    	RTMPAddWcidAttributeEntry(pAdapter,
+    							  BSS0,
+    							  keyIdx,
+    							  pAdapter->SharedKey[BSS0][keyIdx].CipherAlg,
+    							  &pAdapter->MacTab.Content[BSSID_WCID]);
+}
+
+int rt_ioctl_siwencodeext(struct net_device *dev,
+			   struct iw_request_info *info,
+			   union iwreq_data *wrqu,
+			   char *extra)
+			{
+    PRTMP_ADAPTER   pAdapter = dev->ml_priv;
+	struct iw_point *encoding = &wrqu->encoding;
+	struct iw_encode_ext *ext = (struct iw_encode_ext *)extra;
+    int keyIdx, alg = ext->alg;
+
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+    if (encoding->flags & IW_ENCODE_DISABLED)
+	{
+        keyIdx = (encoding->flags & IW_ENCODE_INDEX) - 1;
+        // set BSSID wcid entry of the Pair-wise Key table as no-security mode
+	    AsicRemovePairwiseKeyEntry(pAdapter, BSS0, BSSID_WCID);
+        pAdapter->SharedKey[BSS0][keyIdx].KeyLen = 0;
+		pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_NONE;
+		AsicRemoveSharedKeyEntry(pAdapter, 0, (UCHAR)keyIdx);
+        NdisZeroMemory(&pAdapter->SharedKey[BSS0][keyIdx], sizeof(CIPHER_KEY));
+        DBGPRINT(RT_DEBUG_TRACE, ("%s::Remove all keys!(encoding->flags = %x)\n", __FUNCTION__, encoding->flags));
+    }
+					else
+    {
+        // Get Key Index and convet to our own defined key index
+    	keyIdx = (encoding->flags & IW_ENCODE_INDEX) - 1;
+    	if((keyIdx < 0) || (keyIdx >= NR_WEP_KEYS))
+    		return -EINVAL;
+
+        if (ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY)
+        {
+            pAdapter->StaCfg.DefaultKeyId = keyIdx;
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::DefaultKeyId = %d\n", __FUNCTION__, pAdapter->StaCfg.DefaultKeyId));
+        }
+
+        switch (alg) {
+    		case IW_ENCODE_ALG_NONE:
+                DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_ENCODE_ALG_NONE\n", __FUNCTION__));
+    			break;
+    		case IW_ENCODE_ALG_WEP:
+                DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_ENCODE_ALG_WEP - ext->key_len = %d, keyIdx = %d\n", __FUNCTION__, ext->key_len, keyIdx));
+    			if (ext->key_len == MAX_WEP_KEY_SIZE)
+                {
+        			pAdapter->SharedKey[BSS0][keyIdx].KeyLen = MAX_WEP_KEY_SIZE;
+                    pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_WEP128;
+				}
+        		else if (ext->key_len == MIN_WEP_KEY_SIZE)
+                {
+                    pAdapter->SharedKey[BSS0][keyIdx].KeyLen = MIN_WEP_KEY_SIZE;
+                    pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_WEP64;
+			}
+        		else
+                    return -EINVAL;
+
+                NdisZeroMemory(pAdapter->SharedKey[BSS0][keyIdx].Key,  16);
+			    NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].Key, ext->key, ext->key_len);
+    			break;
+            case IW_ENCODE_ALG_TKIP:
+                DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_ENCODE_ALG_TKIP - keyIdx = %d, ext->key_len = %d\n", __FUNCTION__, keyIdx, ext->key_len));
+                if (ext->key_len == 32)
+                {
+                    if (ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY)
+                    {
+                        fnSetCipherKey(pAdapter, keyIdx, CIPHER_TKIP, FALSE, ext);
+                        if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA2)
+                        {
+                            STA_PORT_SECURED(pAdapter);
+                        }
+		}
+                    else if (ext->ext_flags & IW_ENCODE_EXT_GROUP_KEY)
+                    {
+                        fnSetCipherKey(pAdapter, keyIdx, CIPHER_TKIP, TRUE, ext);
+
+                        // set 802.1x port control
+            	        STA_PORT_SECURED(pAdapter);
+                    }
+                }
+                else
+                    return -EINVAL;
+                break;
+            case IW_ENCODE_ALG_CCMP:
+                if (ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY)
+		{
+                    fnSetCipherKey(pAdapter, keyIdx, CIPHER_AES, FALSE, ext);
+                    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA2)
+                    	STA_PORT_SECURED(pAdapter);
+                }
+                else if (ext->ext_flags & IW_ENCODE_EXT_GROUP_KEY)
+                {
+                    fnSetCipherKey(pAdapter, keyIdx, CIPHER_AES, TRUE, ext);
+
+                    // set 802.1x port control
+        	        STA_PORT_SECURED(pAdapter);
+                }
+                break;
+    		default:
+    			return -EINVAL;
+		}
+    }
+
+    return 0;
+}
+
+int
+rt_ioctl_giwencodeext(struct net_device *dev,
+			  struct iw_request_info *info,
+			  union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER pAd = dev->ml_priv;
+	PCHAR pKey = NULL;
+	struct iw_point *encoding = &wrqu->encoding;
+	struct iw_encode_ext *ext = (struct iw_encode_ext *)extra;
+	int idx, max_key_len;
+
+	DBGPRINT(RT_DEBUG_TRACE ,("===> rt_ioctl_giwencodeext\n"));
+
+	max_key_len = encoding->length - sizeof(*ext);
+	if (max_key_len < 0)
+		return -EINVAL;
+
+	idx = encoding->flags & IW_ENCODE_INDEX;
+	if (idx)
+	{
+		if (idx < 1 || idx > 4)
+			return -EINVAL;
+		idx--;
+
+		if ((pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) ||
+			(pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled))
+		{
+			if (idx != pAd->StaCfg.DefaultKeyId)
+			{
+				ext->key_len = 0;
+				return 0;
+			}
+		}
+	}
+	else
+		idx = pAd->StaCfg.DefaultKeyId;
+
+	encoding->flags = idx + 1;
+	memset(ext, 0, sizeof(*ext));
+
+	ext->key_len = 0;
+	switch(pAd->StaCfg.WepStatus) {
+		case Ndis802_11WEPDisabled:
+			ext->alg = IW_ENCODE_ALG_NONE;
+			encoding->flags |= IW_ENCODE_DISABLED;
+			break;
+		case Ndis802_11WEPEnabled:
+			ext->alg = IW_ENCODE_ALG_WEP;
+			if (pAd->SharedKey[BSS0][idx].KeyLen > max_key_len)
+				return -E2BIG;
+			else
+			{
+				ext->key_len = pAd->SharedKey[BSS0][idx].KeyLen;
+				pKey = &(pAd->SharedKey[BSS0][idx].Key[0]);
+			}
+			break;
+		case Ndis802_11Encryption2Enabled:
+		case Ndis802_11Encryption3Enabled:
+			if (pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled)
+				ext->alg = IW_ENCODE_ALG_TKIP;
+			else
+				ext->alg = IW_ENCODE_ALG_CCMP;
+
+			if (max_key_len < 32)
+				return -E2BIG;
+			else
+			{
+				ext->key_len = 32;
+				pKey = &pAd->StaCfg.PMK[0];
+			}
+			break;
+		default:
+			return -EINVAL;
+	}
+
+	if (ext->key_len && pKey)
+	{
+		encoding->flags |= IW_ENCODE_ENABLED;
+		memcpy(ext->key, pKey, ext->key_len);
+	}
+
+	return 0;
+}
+
+#ifdef SIOCSIWGENIE
+int rt_ioctl_siwgenie(struct net_device *dev,
+			  struct iw_request_info *info,
+			  union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER   pAd = dev->ml_priv;
+
+	if (wrqu->data.length > MAX_LEN_OF_RSNIE ||
+	    (wrqu->data.length && extra == NULL))
+		return -EINVAL;
+
+	if (wrqu->data.length)
+	{
+		pAd->StaCfg.RSNIE_Len = wrqu->data.length;
+		NdisMoveMemory(&pAd->StaCfg.RSN_IE[0], extra, pAd->StaCfg.RSNIE_Len);
+	}
+	else
+	{
+		pAd->StaCfg.RSNIE_Len = 0;
+		NdisZeroMemory(&pAd->StaCfg.RSN_IE[0], MAX_LEN_OF_RSNIE);
+	}
+
+	return 0;
+}
+#endif // SIOCSIWGENIE //
+
+int rt_ioctl_giwgenie(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER   pAd = dev->ml_priv;
+
+	if ((pAd->StaCfg.RSNIE_Len == 0) ||
+		(pAd->StaCfg.AuthMode < Ndis802_11AuthModeWPA))
+	{
+		wrqu->data.length = 0;
+		return 0;
+	}
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+#ifdef SIOCSIWGENIE
+	if (pAd->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_ENABLE)
+	{
+	if (wrqu->data.length < pAd->StaCfg.RSNIE_Len)
+		return -E2BIG;
+
+	wrqu->data.length = pAd->StaCfg.RSNIE_Len;
+	memcpy(extra, &pAd->StaCfg.RSN_IE[0], pAd->StaCfg.RSNIE_Len);
+	}
+	else
+#endif // SIOCSIWGENIE //
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+	{
+		UCHAR RSNIe = IE_WPA;
+
+		if (wrqu->data.length < (pAd->StaCfg.RSNIE_Len + 2)) // ID, Len
+			return -E2BIG;
+		wrqu->data.length = pAd->StaCfg.RSNIE_Len + 2;
+
+		if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK) ||
+            (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2))
+			RSNIe = IE_RSN;
+
+		extra[0] = (char)RSNIe;
+		extra[1] = pAd->StaCfg.RSNIE_Len;
+		memcpy(extra+2, &pAd->StaCfg.RSN_IE[0], pAd->StaCfg.RSNIE_Len);
+	}
+
+	return 0;
+}
+
+int rt_ioctl_siwpmksa(struct net_device *dev,
+			   struct iw_request_info *info,
+			   union iwreq_data *wrqu,
+			   char *extra)
+{
+	PRTMP_ADAPTER   pAd = dev->ml_priv;
+	struct iw_pmksa *pPmksa = (struct iw_pmksa *)wrqu->data.pointer;
+	INT	CachedIdx = 0, idx = 0;
+
+	if (pPmksa == NULL)
+		return -EINVAL;
+
+	DBGPRINT(RT_DEBUG_TRACE ,("===> rt_ioctl_siwpmksa\n"));
+	switch(pPmksa->cmd)
+	{
+		case IW_PMKSA_FLUSH:
+			NdisZeroMemory(pAd->StaCfg.SavedPMK, sizeof(BSSID_INFO)*PMKID_NO);
+			DBGPRINT(RT_DEBUG_TRACE ,("rt_ioctl_siwpmksa - IW_PMKSA_FLUSH\n"));
+			break;
+		case IW_PMKSA_REMOVE:
+			for (CachedIdx = 0; CachedIdx < pAd->StaCfg.SavedPMKNum; CachedIdx++)
+			{
+		        // compare the BSSID
+		        if (NdisEqualMemory(pPmksa->bssid.sa_data, pAd->StaCfg.SavedPMK[CachedIdx].BSSID, MAC_ADDR_LEN))
+		        {
+		        	NdisZeroMemory(pAd->StaCfg.SavedPMK[CachedIdx].BSSID, MAC_ADDR_LEN);
+					NdisZeroMemory(pAd->StaCfg.SavedPMK[CachedIdx].PMKID, 16);
+					for (idx = CachedIdx; idx < (pAd->StaCfg.SavedPMKNum - 1); idx++)
+					{
+						NdisMoveMemory(&pAd->StaCfg.SavedPMK[idx].BSSID[0], &pAd->StaCfg.SavedPMK[idx+1].BSSID[0], MAC_ADDR_LEN);
+						NdisMoveMemory(&pAd->StaCfg.SavedPMK[idx].PMKID[0], &pAd->StaCfg.SavedPMK[idx+1].PMKID[0], 16);
+					}
+					pAd->StaCfg.SavedPMKNum--;
+			        break;
+		        }
+	        }
+
+			DBGPRINT(RT_DEBUG_TRACE ,("rt_ioctl_siwpmksa - IW_PMKSA_REMOVE\n"));
+			break;
+		case IW_PMKSA_ADD:
+			for (CachedIdx = 0; CachedIdx < pAd->StaCfg.SavedPMKNum; CachedIdx++)
+			{
+		        // compare the BSSID
+		        if (NdisEqualMemory(pPmksa->bssid.sa_data, pAd->StaCfg.SavedPMK[CachedIdx].BSSID, MAC_ADDR_LEN))
+			        break;
+	        }
+
+	        // Found, replace it
+	        if (CachedIdx < PMKID_NO)
+	        {
+		        DBGPRINT(RT_DEBUG_OFF, ("Update PMKID, idx = %d\n", CachedIdx));
+		        NdisMoveMemory(&pAd->StaCfg.SavedPMK[CachedIdx].BSSID[0], pPmksa->bssid.sa_data, MAC_ADDR_LEN);
+				NdisMoveMemory(&pAd->StaCfg.SavedPMK[CachedIdx].PMKID[0], pPmksa->pmkid, 16);
+		        pAd->StaCfg.SavedPMKNum++;
+	        }
+	        // Not found, replace the last one
+	        else
+	        {
+		        // Randomly replace one
+		        CachedIdx = (pPmksa->bssid.sa_data[5] % PMKID_NO);
+		        DBGPRINT(RT_DEBUG_OFF, ("Update PMKID, idx = %d\n", CachedIdx));
+		        NdisMoveMemory(&pAd->StaCfg.SavedPMK[CachedIdx].BSSID[0], pPmksa->bssid.sa_data, MAC_ADDR_LEN);
+				NdisMoveMemory(&pAd->StaCfg.SavedPMK[CachedIdx].PMKID[0], pPmksa->pmkid, 16);
+	        }
+
+			DBGPRINT(RT_DEBUG_TRACE ,("rt_ioctl_siwpmksa - IW_PMKSA_ADD\n"));
+			break;
+		default:
+			DBGPRINT(RT_DEBUG_TRACE ,("rt_ioctl_siwpmksa - Unknow Command!!\n"));
+			break;
+	}
+
+	return 0;
+}
+#endif // #if WIRELESS_EXT > 17
+
+#ifdef DBG
+static int
+rt_private_ioctl_bbp(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra)
+			{
+	CHAR				*this_char;
+	CHAR				*value = NULL;
+	UCHAR				regBBP = 0;
+	UINT32				bbpId;
+	UINT32				bbpValue;
+	BOOLEAN				bIsPrintAllBBP = FALSE;
+	INT					Status = 0;
+    PRTMP_ADAPTER       pAdapter = dev->ml_priv;
+
+
+	memset(extra, 0x00, IW_PRIV_SIZE_MASK);
+
+	if (wrq->length > 1) //No parameters.
+				{
+		sprintf(extra, "\n");
+
+		//Parsing Read or Write
+		this_char = wrq->pointer;
+		DBGPRINT(RT_DEBUG_TRACE, ("this_char=%s\n", this_char));
+		if (!*this_char)
+			goto next;
+
+		if ((value = rtstrchr(this_char, '=')) != NULL)
+			*value++ = 0;
+
+		if (!value || !*value)
+		{ //Read
+			DBGPRINT(RT_DEBUG_TRACE, ("this_char=%s, value=%s\n", this_char, value));
+			if (sscanf(this_char, "%d", &(bbpId)) == 1)
+			{
+				if (bbpId <= 136)
+				{
+#ifdef RALINK_ATE
+					if (ATE_ON(pAdapter))
+					{
+						ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+					}
+					else
+#endif // RALINK_ATE //
+					{
+					RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+					}
+					sprintf(extra+strlen(extra), "R%02d[0x%02X]:%02X\n", bbpId, bbpId*2, regBBP);
+                    wrq->length = strlen(extra) + 1; // 1: size of '\0'
+					DBGPRINT(RT_DEBUG_TRACE, ("msg=%s\n", extra));
+				}
+				else
+				{//Invalid parametes, so default printk all bbp
+					bIsPrintAllBBP = TRUE;
+					goto next;
+				}
+			}
+			else
+			{ //Invalid parametes, so default printk all bbp
+				bIsPrintAllBBP = TRUE;
+				goto next;
+			}
+		}
+		else
+		{ //Write
+			if ((sscanf(this_char, "%d", &(bbpId)) == 1) && (sscanf(value, "%x", &(bbpValue)) == 1))
+			{
+				if (bbpId <= 136)
+				{
+#ifdef RALINK_ATE
+					if (ATE_ON(pAdapter))
+					{
+						ATE_BBP_IO_WRITE8_BY_REG_ID(pAdapter, bbpId, bbpValue);
+						//Read it back for showing
+						ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+					}
+					else
+#endif // RALINK_ATE //
+					{
+					    RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, bbpId, bbpValue);
+    					//Read it back for showing
+    					RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+			}
+					sprintf(extra+strlen(extra), "R%02d[0x%02X]:%02X\n", bbpId, bbpId*2, regBBP);
+                    wrq->length = strlen(extra) + 1; // 1: size of '\0'
+					DBGPRINT(RT_DEBUG_TRACE, ("msg=%s\n", extra));
+				}
+				else
+				{//Invalid parametes, so default printk all bbp
+					bIsPrintAllBBP = TRUE;
+					goto next;
+				}
+			}
+			else
+			{ //Invalid parametes, so default printk all bbp
+				bIsPrintAllBBP = TRUE;
+				goto next;
+			}
+		}
+		}
+	else
+		bIsPrintAllBBP = TRUE;
+
+next:
+	if (bIsPrintAllBBP)
+	{
+		memset(extra, 0x00, IW_PRIV_SIZE_MASK);
+		sprintf(extra, "\n");
+		for (bbpId = 0; bbpId <= 136; bbpId++)
+		{
+		    if (strlen(extra) >= (IW_PRIV_SIZE_MASK - 10))
+                break;
+#ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+			}
+			else
+#endif // RALINK_ATE //
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+			sprintf(extra+strlen(extra), "R%02d[0x%02X]:%02X    ", bbpId, bbpId*2, regBBP);
+			if (bbpId%5 == 4)
+				sprintf(extra+strlen(extra), "\n");
+		}
+
+        wrq->length = strlen(extra) + 1; // 1: size of '\0'
+        DBGPRINT(RT_DEBUG_TRACE, ("wrq->length = %d\n", wrq->length));
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<==rt_private_ioctl_bbp\n\n"));
+
+    return Status;
+}
+#endif // DBG //
+
+int rt_ioctl_siwrate(struct net_device *dev,
+			struct iw_request_info *info,
+			union iwreq_data *wrqu, char *extra)
+{
+    PRTMP_ADAPTER   pAd = dev->ml_priv;
+    UINT32          rate = wrqu->bitrate.value, fixed = wrqu->bitrate.fixed;
+
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("rt_ioctl_siwrate::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+    DBGPRINT(RT_DEBUG_TRACE, ("rt_ioctl_siwrate::(rate = %d, fixed = %d)\n", rate, fixed));
+    /* rate = -1 => auto rate
+       rate = X, fixed = 1 => (fixed rate X)
+    */
+    if (rate == -1)
+    {
+		//Auto Rate
+		pAd->StaCfg.DesiredTransmitSetting.field.MCS = MCS_AUTO;
+		pAd->StaCfg.bAutoTxRateSwitch = TRUE;
+		if ((pAd->CommonCfg.PhyMode <= PHY_11G) ||
+		    (pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE <= MODE_OFDM))
+			RTMPSetDesiredRates(pAd, -1);
+
+#ifdef DOT11_N_SUPPORT
+		SetCommonHT(pAd);
+#endif // DOT11_N_SUPPORT //
+    }
+    else
+    {
+        if (fixed)
+        {
+        	pAd->StaCfg.bAutoTxRateSwitch = FALSE;
+            if ((pAd->CommonCfg.PhyMode <= PHY_11G) ||
+                (pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE <= MODE_OFDM))
+                RTMPSetDesiredRates(pAd, rate);
+            else
+            {
+                pAd->StaCfg.DesiredTransmitSetting.field.MCS = MCS_AUTO;
+#ifdef DOT11_N_SUPPORT
+                SetCommonHT(pAd);
+#endif // DOT11_N_SUPPORT //
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("rt_ioctl_siwrate::(HtMcs=%d)\n",pAd->StaCfg.DesiredTransmitSetting.field.MCS));
+        }
+        else
+        {
+            // TODO: rate = X, fixed = 0 => (rates <= X)
+            return -EOPNOTSUPP;
+        }
+    }
+
+    return 0;
+}
+
+int rt_ioctl_giwrate(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+    PRTMP_ADAPTER   pAd = dev->ml_priv;
+    int rate_index = 0, rate_count = 0;
+    HTTRANSMIT_SETTING ht_setting;
+    __s32 ralinkrate[] =
+	{2,  4,   11,  22, // CCK
+	12, 18,   24,  36, 48, 72, 96, 108, // OFDM
+	13, 26,   39,  52,  78, 104, 117, 130, 26,  52,  78, 104, 156, 208, 234, 260, // 20MHz, 800ns GI, MCS: 0 ~ 15
+	39, 78,  117, 156, 234, 312, 351, 390,										  // 20MHz, 800ns GI, MCS: 16 ~ 23
+	27, 54,   81, 108, 162, 216, 243, 270, 54, 108, 162, 216, 324, 432, 486, 540, // 40MHz, 800ns GI, MCS: 0 ~ 15
+	81, 162, 243, 324, 486, 648, 729, 810,										  // 40MHz, 800ns GI, MCS: 16 ~ 23
+	14, 29,   43,  57,  87, 115, 130, 144, 29, 59,   87, 115, 173, 230, 260, 288, // 20MHz, 400ns GI, MCS: 0 ~ 15
+	43, 87,  130, 173, 260, 317, 390, 433,										  // 20MHz, 400ns GI, MCS: 16 ~ 23
+	30, 60,   90, 120, 180, 240, 270, 300, 60, 120, 180, 240, 360, 480, 540, 600, // 40MHz, 400ns GI, MCS: 0 ~ 15
+	90, 180, 270, 360, 540, 720, 810, 900};										  // 40MHz, 400ns GI, MCS: 16 ~ 23
+
+    rate_count = sizeof(ralinkrate)/sizeof(__s32);
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+    if ((pAd->StaCfg.bAutoTxRateSwitch == FALSE) &&
+        (INFRA_ON(pAd)) &&
+        ((pAd->CommonCfg.PhyMode <= PHY_11G) || (pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE <= MODE_OFDM)))
+        ht_setting.word = pAd->StaCfg.HTPhyMode.word;
+    else
+        ht_setting.word = pAd->MacTab.Content[BSSID_WCID].HTPhyMode.word;
+
+#ifdef DOT11_N_SUPPORT
+    if (ht_setting.field.MODE >= MODE_HTMIX)
+    {
+    	rate_index = 12 + ((UCHAR)ht_setting.field.BW *24) + ((UCHAR)ht_setting.field.ShortGI *48) + ((UCHAR)ht_setting.field.MCS);
+    }
+    else
+#endif // DOT11_N_SUPPORT //
+    if (ht_setting.field.MODE == MODE_OFDM)
+    	rate_index = (UCHAR)(ht_setting.field.MCS) + 4;
+    else if (ht_setting.field.MODE == MODE_CCK)
+    	rate_index = (UCHAR)(ht_setting.field.MCS);
+
+    if (rate_index < 0)
+        rate_index = 0;
+
+    if (rate_index > rate_count)
+        rate_index = rate_count;
+
+    wrqu->bitrate.value = ralinkrate[rate_index] * 500000;
+    wrqu->bitrate.disabled = 0;
+
+    return 0;
+}
+
+static const iw_handler rt_handler[] =
+{
+	(iw_handler) NULL,			            /* SIOCSIWCOMMIT */
+	(iw_handler) rt_ioctl_giwname,			/* SIOCGIWNAME   */
+	(iw_handler) NULL,			            /* SIOCSIWNWID   */
+	(iw_handler) NULL,			            /* SIOCGIWNWID   */
+	(iw_handler) rt_ioctl_siwfreq,		    /* SIOCSIWFREQ   */
+	(iw_handler) rt_ioctl_giwfreq,		    /* SIOCGIWFREQ   */
+	(iw_handler) rt_ioctl_siwmode,		    /* SIOCSIWMODE   */
+	(iw_handler) rt_ioctl_giwmode,		    /* SIOCGIWMODE   */
+	(iw_handler) NULL,		                /* SIOCSIWSENS   */
+	(iw_handler) NULL,		                /* SIOCGIWSENS   */
+	(iw_handler) NULL /* not used */,		/* SIOCSIWRANGE  */
+	(iw_handler) rt_ioctl_giwrange,		    /* SIOCGIWRANGE  */
+	(iw_handler) NULL /* not used */,		/* SIOCSIWPRIV   */
+	(iw_handler) NULL /* kernel code */,    /* SIOCGIWPRIV   */
+	(iw_handler) NULL /* not used */,		/* SIOCSIWSTATS  */
+	(iw_handler) rt28xx_get_wireless_stats /* kernel code */,    /* SIOCGIWSTATS  */
+	(iw_handler) NULL,		                /* SIOCSIWSPY    */
+	(iw_handler) NULL,		                /* SIOCGIWSPY    */
+	(iw_handler) NULL,				        /* SIOCSIWTHRSPY */
+	(iw_handler) NULL,				        /* SIOCGIWTHRSPY */
+	(iw_handler) rt_ioctl_siwap,            /* SIOCSIWAP     */
+	(iw_handler) rt_ioctl_giwap,		    /* SIOCGIWAP     */
+#ifdef SIOCSIWMLME
+	(iw_handler) rt_ioctl_siwmlme,	        /* SIOCSIWMLME   */
+#else
+	(iw_handler) NULL,				        /* SIOCSIWMLME */
+#endif // SIOCSIWMLME //
+	(iw_handler) rt_ioctl_iwaplist,		    /* SIOCGIWAPLIST */
+#ifdef SIOCGIWSCAN
+	(iw_handler) rt_ioctl_siwscan,		    /* SIOCSIWSCAN   */
+	(iw_handler) rt_ioctl_giwscan,		    /* SIOCGIWSCAN   */
+#else
+	(iw_handler) NULL,				        /* SIOCSIWSCAN   */
+	(iw_handler) NULL,				        /* SIOCGIWSCAN   */
+#endif /* SIOCGIWSCAN */
+	(iw_handler) rt_ioctl_siwessid,		    /* SIOCSIWESSID  */
+	(iw_handler) rt_ioctl_giwessid,		    /* SIOCGIWESSID  */
+	(iw_handler) rt_ioctl_siwnickn,		    /* SIOCSIWNICKN  */
+	(iw_handler) rt_ioctl_giwnickn,		    /* SIOCGIWNICKN  */
+	(iw_handler) NULL,				        /* -- hole --    */
+	(iw_handler) NULL,				        /* -- hole --    */
+	(iw_handler) rt_ioctl_siwrate,          /* SIOCSIWRATE   */
+	(iw_handler) rt_ioctl_giwrate,          /* SIOCGIWRATE   */
+	(iw_handler) rt_ioctl_siwrts,		    /* SIOCSIWRTS    */
+	(iw_handler) rt_ioctl_giwrts,		    /* SIOCGIWRTS    */
+	(iw_handler) rt_ioctl_siwfrag,		    /* SIOCSIWFRAG   */
+	(iw_handler) rt_ioctl_giwfrag,		    /* SIOCGIWFRAG   */
+	(iw_handler) NULL,		                /* SIOCSIWTXPOW  */
+	(iw_handler) NULL,		                /* SIOCGIWTXPOW  */
+	(iw_handler) NULL,		                /* SIOCSIWRETRY  */
+	(iw_handler) NULL,		                /* SIOCGIWRETRY  */
+	(iw_handler) rt_ioctl_siwencode,		/* SIOCSIWENCODE */
+	(iw_handler) rt_ioctl_giwencode,		/* SIOCGIWENCODE */
+	(iw_handler) NULL,		                /* SIOCSIWPOWER  */
+	(iw_handler) NULL,		                /* SIOCGIWPOWER  */
+	(iw_handler) NULL,						/* -- hole -- */
+	(iw_handler) NULL,						/* -- hole -- */
+#if WIRELESS_EXT > 17
+    (iw_handler) rt_ioctl_siwgenie,         /* SIOCSIWGENIE  */
+	(iw_handler) rt_ioctl_giwgenie,         /* SIOCGIWGENIE  */
+	(iw_handler) rt_ioctl_siwauth,		    /* SIOCSIWAUTH   */
+	(iw_handler) rt_ioctl_giwauth,		    /* SIOCGIWAUTH   */
+	(iw_handler) rt_ioctl_siwencodeext,	    /* SIOCSIWENCODEEXT */
+	(iw_handler) rt_ioctl_giwencodeext,		/* SIOCGIWENCODEEXT */
+	(iw_handler) rt_ioctl_siwpmksa,         /* SIOCSIWPMKSA  */
+#endif
+};
+
+static const iw_handler rt_priv_handlers[] = {
+	(iw_handler) NULL, /* + 0x00 */
+	(iw_handler) NULL, /* + 0x01 */
+#ifndef CONFIG_AP_SUPPORT
+	(iw_handler) rt_ioctl_setparam, /* + 0x02 */
+#else
+	(iw_handler) NULL, /* + 0x02 */
+#endif // CONFIG_AP_SUPPORT //
+#ifdef DBG
+	(iw_handler) rt_private_ioctl_bbp, /* + 0x03 */
+#else
+	(iw_handler) NULL, /* + 0x03 */
+#endif
+	(iw_handler) NULL, /* + 0x04 */
+	(iw_handler) NULL, /* + 0x05 */
+	(iw_handler) NULL, /* + 0x06 */
+	(iw_handler) NULL, /* + 0x07 */
+	(iw_handler) NULL, /* + 0x08 */
+	(iw_handler) rt_private_get_statistics, /* + 0x09 */
+	(iw_handler) NULL, /* + 0x0A */
+	(iw_handler) NULL, /* + 0x0B */
+	(iw_handler) NULL, /* + 0x0C */
+	(iw_handler) NULL, /* + 0x0D */
+	(iw_handler) NULL, /* + 0x0E */
+	(iw_handler) NULL, /* + 0x0F */
+	(iw_handler) NULL, /* + 0x10 */
+	(iw_handler) rt_private_show, /* + 0x11 */
+    (iw_handler) NULL, /* + 0x12 */
+	(iw_handler) NULL, /* + 0x13 */
+	(iw_handler) NULL, /* + 0x15 */
+	(iw_handler) NULL, /* + 0x17 */
+	(iw_handler) NULL, /* + 0x18 */
+};
+
+const struct iw_handler_def rt28xx_iw_handler_def =
+{
+#define	N(a)	(sizeof (a) / sizeof (a[0]))
+	.standard	= (iw_handler *) rt_handler,
+	.num_standard	= sizeof(rt_handler) / sizeof(iw_handler),
+	.private	= (iw_handler *) rt_priv_handlers,
+	.num_private		= N(rt_priv_handlers),
+	.private_args	= (struct iw_priv_args *) privtab,
+	.num_private_args	= N(privtab),
+#if IW_HANDLER_VERSION >= 7
+    .get_wireless_stats = rt28xx_get_wireless_stats,
+#endif
+};
+
+INT RTMPSetInformation(
+    IN  PRTMP_ADAPTER pAdapter,
+    IN  OUT struct ifreq    *rq,
+    IN  INT                 cmd)
+{
+    struct iwreq                        *wrq = (struct iwreq *) rq;
+    NDIS_802_11_SSID                    Ssid;
+    NDIS_802_11_MAC_ADDRESS             Bssid;
+    RT_802_11_PHY_MODE                  PhyMode;
+    RT_802_11_STA_CONFIG                StaConfig;
+    NDIS_802_11_RATES                   aryRates;
+    RT_802_11_PREAMBLE                  Preamble;
+    NDIS_802_11_WEP_STATUS              WepStatus;
+    NDIS_802_11_AUTHENTICATION_MODE     AuthMode = Ndis802_11AuthModeMax;
+    NDIS_802_11_NETWORK_INFRASTRUCTURE  BssType;
+    NDIS_802_11_RTS_THRESHOLD           RtsThresh;
+    NDIS_802_11_FRAGMENTATION_THRESHOLD FragThresh;
+    NDIS_802_11_POWER_MODE              PowerMode;
+    PNDIS_802_11_KEY                    pKey = NULL;
+    PNDIS_802_11_WEP			        pWepKey =NULL;
+    PNDIS_802_11_REMOVE_KEY             pRemoveKey = NULL;
+    NDIS_802_11_CONFIGURATION           Config, *pConfig = NULL;
+    NDIS_802_11_NETWORK_TYPE            NetType;
+    ULONG                               Now;
+    UINT                                KeyIdx = 0;
+    INT                                 Status = NDIS_STATUS_SUCCESS, MaxPhyMode = PHY_11G;
+    ULONG                               PowerTemp;
+    BOOLEAN                             RadioState;
+    BOOLEAN                             StateMachineTouched = FALSE;
+#ifdef DOT11_N_SUPPORT
+	OID_SET_HT_PHYMODE					HT_PhyMode;	//11n ,kathy
+#endif // DOT11_N_SUPPORT //
+#ifdef WPA_SUPPLICANT_SUPPORT
+    PNDIS_802_11_PMKID                  pPmkId = NULL;
+    BOOLEAN				                IEEE8021xState = FALSE;
+    BOOLEAN				                IEEE8021x_required_keys = FALSE;
+    UCHAR                               wpa_supplicant_enable = 0;
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef SNMP_SUPPORT
+	TX_RTY_CFG_STRUC			tx_rty_cfg;
+	ULONG						ShortRetryLimit, LongRetryLimit;
+	UCHAR						ctmp;
+#endif // SNMP_SUPPORT //
+
+
+#ifdef DOT11_N_SUPPORT
+	MaxPhyMode = PHY_11N_5G;
+#endif // DOT11_N_SUPPORT //
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("-->RTMPSetInformation(),	0x%08x\n", cmd&0x7FFF));
+    switch(cmd & 0x7FFF) {
+        case RT_OID_802_11_COUNTRY_REGION:
+            if (wrq->u.data.length < sizeof(UCHAR))
+                Status = -EINVAL;
+			// Only avaliable when EEPROM not programming
+            else if (!(pAdapter->CommonCfg.CountryRegion & 0x80) && !(pAdapter->CommonCfg.CountryRegionForABand & 0x80))
+            {
+                ULONG   Country;
+                UCHAR	TmpPhy;
+
+				Status = copy_from_user(&Country, wrq->u.data.pointer, wrq->u.data.length);
+				pAdapter->CommonCfg.CountryRegion = (UCHAR)(Country & 0x000000FF);
+				pAdapter->CommonCfg.CountryRegionForABand = (UCHAR)((Country >> 8) & 0x000000FF);
+                TmpPhy = pAdapter->CommonCfg.PhyMode;
+				pAdapter->CommonCfg.PhyMode = 0xff;
+				// Build all corresponding channel information
+				RTMPSetPhyMode(pAdapter, TmpPhy);
+#ifdef DOT11_N_SUPPORT
+				SetCommonHT(pAdapter);
+#endif // DOT11_N_SUPPORT //
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_COUNTRY_REGION (A:%d  B/G:%d)\n", pAdapter->CommonCfg.CountryRegionForABand,
+				    pAdapter->CommonCfg.CountryRegion));
+            }
+            break;
+        case OID_802_11_BSSID_LIST_SCAN:
+ #ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+				break;
+			}
+#endif // RALINK_ATE //
+            Now = jiffies;
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_BSSID_LIST_SCAN, TxCnt = %d \n", pAdapter->RalinkCounters.LastOneSecTotalTxCount));
+
+            if (MONITOR_ON(pAdapter))
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! Driver is in Monitor Mode now !!!\n"));
+                break;
+            }
+
+			//Benson add 20080527, when radio off, sta don't need to scan
+			if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_RADIO_OFF))
+				break;
+
+			if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+			{
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! Driver is scanning now !!!\n"));
+				pAdapter->StaCfg.bScanReqIsFromWebUI = TRUE;
+				Status = NDIS_STATUS_SUCCESS;
+                break;
+            }
+
+			if (pAdapter->RalinkCounters.LastOneSecTotalTxCount > 100)
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! Link UP, ignore this set::OID_802_11_BSSID_LIST_SCAN\n"));
+				Status = NDIS_STATUS_SUCCESS;
+				pAdapter->StaCfg.ScanCnt = 99;		// Prevent auto scan triggered by this OID
+				break;
+            }
+
+            if ((OPSTATUS_TEST_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED)) &&
+				((pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
+				(pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+				(pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) ||
+				(pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)) &&
+                (pAdapter->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! Link UP, Port Not Secured! ignore this set::OID_802_11_BSSID_LIST_SCAN\n"));
+				Status = NDIS_STATUS_SUCCESS;
+				pAdapter->StaCfg.ScanCnt = 99;		// Prevent auto scan triggered by this OID
+				break;
+            }
+
+
+            if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+            {
+                RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+            }
+
+            // tell CNTL state machine to call NdisMSetInformationComplete() after completing
+            // this request, because this request is initiated by NDIS.
+            pAdapter->MlmeAux.CurrReqIsFromNdis = FALSE;
+            // Reset allowed scan retries
+            pAdapter->StaCfg.ScanCnt = 0;
+            pAdapter->StaCfg.LastScanTime = Now;
+
+			pAdapter->StaCfg.bScanReqIsFromWebUI = TRUE;
+            RTMP_SET_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS);
+            MlmeEnqueue(pAdapter,
+                        MLME_CNTL_STATE_MACHINE,
+                        OID_802_11_BSSID_LIST_SCAN,
+                        0,
+                        NULL);
+
+            Status = NDIS_STATUS_SUCCESS;
+            StateMachineTouched = TRUE;
+            break;
+        case OID_802_11_SSID:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_SSID))
+                Status = -EINVAL;
+            else
+            {
+            	PCHAR pSsidString = NULL;
+                Status = copy_from_user(&Ssid, wrq->u.data.pointer, wrq->u.data.length);
+
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_SSID (Len=%d,Ssid=%s)\n", Ssid.SsidLength, Ssid.Ssid));
+                if (Ssid.SsidLength > MAX_LEN_OF_SSID)
+                    Status = -EINVAL;
+                else
+                {
+                	if (Ssid.SsidLength == 0)
+                	{
+                		Set_SSID_Proc(pAdapter, "");
+                	}
+					else
+                	{
+	                	pSsidString = (CHAR *) kmalloc(MAX_LEN_OF_SSID+1, MEM_ALLOC_FLAG);
+						if (pSsidString)
+						{
+							NdisZeroMemory(pSsidString, MAX_LEN_OF_SSID+1);
+							NdisMoveMemory(pSsidString, Ssid.Ssid, Ssid.SsidLength);
+	                		Set_SSID_Proc(pAdapter, pSsidString);
+							kfree(pSsidString);
+						}
+						else
+							Status = -ENOMEM;
+                	}
+                }
+            }
+            break;
+        case OID_802_11_BSSID:
+#ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+				break;
+			}
+#endif // RALINK_ATE //
+            if (wrq->u.data.length != sizeof(NDIS_802_11_MAC_ADDRESS))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&Bssid, wrq->u.data.pointer, wrq->u.data.length);
+
+                // tell CNTL state machine to call NdisMSetInformationComplete() after completing
+                // this request, because this request is initiated by NDIS.
+                pAdapter->MlmeAux.CurrReqIsFromNdis = FALSE;
+
+				// Prevent to connect AP again in STAMlmePeriodicExec
+				pAdapter->MlmeAux.AutoReconnectSsidLen= 32;
+
+                // Reset allowed scan retries
+				pAdapter->StaCfg.ScanCnt = 0;
+
+                if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+                {
+                    RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+                    DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+                }
+                MlmeEnqueue(pAdapter,
+                            MLME_CNTL_STATE_MACHINE,
+                            OID_802_11_BSSID,
+                            sizeof(NDIS_802_11_MAC_ADDRESS),
+                            (VOID *)&Bssid);
+                Status = NDIS_STATUS_SUCCESS;
+                StateMachineTouched = TRUE;
+
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_BSSID %02x:%02x:%02x:%02x:%02x:%02x\n",
+                                        Bssid[0], Bssid[1], Bssid[2], Bssid[3], Bssid[4], Bssid[5]));
+            }
+            break;
+        case RT_OID_802_11_RADIO:
+            if (wrq->u.data.length != sizeof(BOOLEAN))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&RadioState, wrq->u.data.pointer, wrq->u.data.length);
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_RADIO (=%d)\n", RadioState));
+                if (pAdapter->StaCfg.bSwRadio != RadioState)
+                {
+                    pAdapter->StaCfg.bSwRadio = RadioState;
+                    if (pAdapter->StaCfg.bRadio != (pAdapter->StaCfg.bHwRadio && pAdapter->StaCfg.bSwRadio))
+                    {
+                        pAdapter->StaCfg.bRadio = (pAdapter->StaCfg.bHwRadio && pAdapter->StaCfg.bSwRadio);
+                        if (pAdapter->StaCfg.bRadio == TRUE)
+                        {
+                            MlmeRadioOn(pAdapter);
+                            // Update extra information
+							pAdapter->ExtraInfo = EXTRA_INFO_CLEAR;
+                        }
+                        else
+                        {
+                            MlmeRadioOff(pAdapter);
+                            // Update extra information
+							pAdapter->ExtraInfo = SW_RADIO_OFF;
+                        }
+                    }
+                }
+            }
+            break;
+        case RT_OID_802_11_PHY_MODE:
+            if (wrq->u.data.length != sizeof(RT_802_11_PHY_MODE))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&PhyMode, wrq->u.data.pointer, wrq->u.data.length);
+				if (PhyMode <= MaxPhyMode)
+				{
+                	RTMPSetPhyMode(pAdapter, PhyMode);
+#ifdef DOT11_N_SUPPORT
+					SetCommonHT(pAdapter);
+#endif // DOT11_N_SUPPORT //
+				}
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_PHY_MODE (=%d)\n", PhyMode));
+            }
+            break;
+        case RT_OID_802_11_STA_CONFIG:
+            if (wrq->u.data.length != sizeof(RT_802_11_STA_CONFIG))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&StaConfig, wrq->u.data.pointer, wrq->u.data.length);
+                pAdapter->CommonCfg.bEnableTxBurst = StaConfig.EnableTxBurst;
+                pAdapter->CommonCfg.UseBGProtection = StaConfig.UseBGProtection;
+                pAdapter->CommonCfg.bUseShortSlotTime = 1; // 2003-10-30 always SHORT SLOT capable
+                if ((pAdapter->CommonCfg.PhyMode != StaConfig.AdhocMode) &&
+					(StaConfig.AdhocMode <= MaxPhyMode))
+                {
+                    // allow dynamic change of "USE OFDM rate or not" in ADHOC mode
+                    // if setting changed, need to reset current TX rate as well as BEACON frame format
+                    if (pAdapter->StaCfg.BssType == BSS_ADHOC)
+                    {
+						pAdapter->CommonCfg.PhyMode = StaConfig.AdhocMode;
+                    	RTMPSetPhyMode(pAdapter, PhyMode);
+                        MlmeUpdateTxRates(pAdapter, FALSE, 0);
+                        MakeIbssBeacon(pAdapter);           // re-build BEACON frame
+                        AsicEnableIbssSync(pAdapter);   // copy to on-chip memory
+                    }
+                }
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_STA_CONFIG (Burst=%d, Protection=%ld,ShortSlot=%d\n",
+                                        pAdapter->CommonCfg.bEnableTxBurst,
+                                        pAdapter->CommonCfg.UseBGProtection,
+                                        pAdapter->CommonCfg.bUseShortSlotTime));
+            }
+            break;
+        case OID_802_11_DESIRED_RATES:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_RATES))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&aryRates, wrq->u.data.pointer, wrq->u.data.length);
+                NdisZeroMemory(pAdapter->CommonCfg.DesireRate, MAX_LEN_OF_SUPPORTED_RATES);
+                NdisMoveMemory(pAdapter->CommonCfg.DesireRate, &aryRates, sizeof(NDIS_802_11_RATES));
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_DESIRED_RATES (%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x)\n",
+                    pAdapter->CommonCfg.DesireRate[0],pAdapter->CommonCfg.DesireRate[1],
+                    pAdapter->CommonCfg.DesireRate[2],pAdapter->CommonCfg.DesireRate[3],
+                    pAdapter->CommonCfg.DesireRate[4],pAdapter->CommonCfg.DesireRate[5],
+                    pAdapter->CommonCfg.DesireRate[6],pAdapter->CommonCfg.DesireRate[7] ));
+                // Changing DesiredRate may affect the MAX TX rate we used to TX frames out
+                MlmeUpdateTxRates(pAdapter, FALSE, 0);
+            }
+            break;
+        case RT_OID_802_11_PREAMBLE:
+            if (wrq->u.data.length != sizeof(RT_802_11_PREAMBLE))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&Preamble, wrq->u.data.pointer, wrq->u.data.length);
+                if (Preamble == Rt802_11PreambleShort)
+                {
+                    pAdapter->CommonCfg.TxPreamble = Preamble;
+                    MlmeSetTxPreamble(pAdapter, Rt802_11PreambleShort);
+                }
+                else if ((Preamble == Rt802_11PreambleLong) || (Preamble == Rt802_11PreambleAuto))
+                {
+                    // if user wants AUTO, initialize to LONG here, then change according to AP's
+                    // capability upon association.
+                    pAdapter->CommonCfg.TxPreamble = Preamble;
+                    MlmeSetTxPreamble(pAdapter, Rt802_11PreambleLong);
+                }
+                else
+                {
+                    Status = -EINVAL;
+                    break;
+                }
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_PREAMBLE (=%d)\n", Preamble));
+            }
+            break;
+        case OID_802_11_WEP_STATUS:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_WEP_STATUS))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&WepStatus, wrq->u.data.pointer, wrq->u.data.length);
+                // Since TKIP, AES, WEP are all supported. It should not have any invalid setting
+                if (WepStatus <= Ndis802_11Encryption3KeyAbsent)
+                {
+                    if (pAdapter->StaCfg.WepStatus != WepStatus)
+                    {
+                        // Config has changed
+                        pAdapter->bConfigChanged = TRUE;
+                    }
+                    pAdapter->StaCfg.WepStatus     = WepStatus;
+                    pAdapter->StaCfg.OrigWepStatus = WepStatus;
+                    pAdapter->StaCfg.PairCipher    = WepStatus;
+                	pAdapter->StaCfg.GroupCipher   = WepStatus;
+                }
+                else
+                {
+                    Status  = -EINVAL;
+                    break;
+                }
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_WEP_STATUS (=%d)\n",WepStatus));
+            }
+            break;
+        case OID_802_11_AUTHENTICATION_MODE:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_AUTHENTICATION_MODE))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&AuthMode, wrq->u.data.pointer, wrq->u.data.length);
+                if (AuthMode > Ndis802_11AuthModeMax)
+                {
+                    Status  = -EINVAL;
+                    break;
+                }
+                else
+                {
+                    if (pAdapter->StaCfg.AuthMode != AuthMode)
+                    {
+                        // Config has changed
+                        pAdapter->bConfigChanged = TRUE;
+                    }
+                    pAdapter->StaCfg.AuthMode = AuthMode;
+                }
+                pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_AUTHENTICATION_MODE (=%d) \n",pAdapter->StaCfg.AuthMode));
+            }
+            break;
+        case OID_802_11_INFRASTRUCTURE_MODE:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_NETWORK_INFRASTRUCTURE))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&BssType, wrq->u.data.pointer, wrq->u.data.length);
+
+				if (BssType == Ndis802_11IBSS)
+					Set_NetworkType_Proc(pAdapter, "Adhoc");
+				else if (BssType == Ndis802_11Infrastructure)
+					Set_NetworkType_Proc(pAdapter, "Infra");
+				else if (BssType == Ndis802_11Monitor)
+					Set_NetworkType_Proc(pAdapter, "Monitor");
+				else
+				{
+					Status  = -EINVAL;
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_INFRASTRUCTURE_MODE (unknown)\n"));
+				}
+			}
+			break;
+	 case OID_802_11_REMOVE_WEP:
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_WEP\n"));
+            if (wrq->u.data.length != sizeof(NDIS_802_11_KEY_INDEX))
+            {
+				Status = -EINVAL;
+            }
+            else
+            {
+				KeyIdx = *(NDIS_802_11_KEY_INDEX *) wrq->u.data.pointer;
+
+				if (KeyIdx & 0x80000000)
+				{
+					// Should never set default bit when remove key
+					Status = -EINVAL;
+				}
+				else
+				{
+					KeyIdx = KeyIdx & 0x0fffffff;
+					if (KeyIdx >= 4){
+						Status = -EINVAL;
+					}
+					else
+					{
+						pAdapter->SharedKey[BSS0][KeyIdx].KeyLen = 0;
+						pAdapter->SharedKey[BSS0][KeyIdx].CipherAlg = CIPHER_NONE;
+						AsicRemoveSharedKeyEntry(pAdapter, 0, (UCHAR)KeyIdx);
+					}
+				}
+            }
+            break;
+        case RT_OID_802_11_RESET_COUNTERS:
+            NdisZeroMemory(&pAdapter->WlanCounters, sizeof(COUNTER_802_11));
+            NdisZeroMemory(&pAdapter->Counters8023, sizeof(COUNTER_802_3));
+            NdisZeroMemory(&pAdapter->RalinkCounters, sizeof(COUNTER_RALINK));
+            pAdapter->Counters8023.RxNoBuffer   = 0;
+			pAdapter->Counters8023.GoodReceives = 0;
+			pAdapter->Counters8023.RxNoBuffer   = 0;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_RESET_COUNTERS \n"));
+            break;
+        case OID_802_11_RTS_THRESHOLD:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_RTS_THRESHOLD))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&RtsThresh, wrq->u.data.pointer, wrq->u.data.length);
+                if (RtsThresh > MAX_RTS_THRESHOLD)
+                    Status  = -EINVAL;
+                else
+                    pAdapter->CommonCfg.RtsThreshold = (USHORT)RtsThresh;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_RTS_THRESHOLD (=%ld)\n",RtsThresh));
+            break;
+        case OID_802_11_FRAGMENTATION_THRESHOLD:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_FRAGMENTATION_THRESHOLD))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&FragThresh, wrq->u.data.pointer, wrq->u.data.length);
+                pAdapter->CommonCfg.bUseZeroToDisableFragment = FALSE;
+                if (FragThresh > MAX_FRAG_THRESHOLD || FragThresh < MIN_FRAG_THRESHOLD)
+                {
+                    if (FragThresh == 0)
+                    {
+                        pAdapter->CommonCfg.FragmentThreshold = MAX_FRAG_THRESHOLD;
+                        pAdapter->CommonCfg.bUseZeroToDisableFragment = TRUE;
+                    }
+                    else
+                        Status  = -EINVAL;
+                }
+                else
+                    pAdapter->CommonCfg.FragmentThreshold = (USHORT)FragThresh;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_FRAGMENTATION_THRESHOLD (=%ld) \n",FragThresh));
+            break;
+        case OID_802_11_POWER_MODE:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_POWER_MODE))
+                Status = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&PowerMode, wrq->u.data.pointer, wrq->u.data.length);
+                if (PowerMode == Ndis802_11PowerModeCAM)
+                	Set_PSMode_Proc(pAdapter, "CAM");
+                else if (PowerMode == Ndis802_11PowerModeMAX_PSP)
+                	Set_PSMode_Proc(pAdapter, "Max_PSP");
+                else if (PowerMode == Ndis802_11PowerModeFast_PSP)
+					Set_PSMode_Proc(pAdapter, "Fast_PSP");
+                else if (PowerMode == Ndis802_11PowerModeLegacy_PSP)
+					Set_PSMode_Proc(pAdapter, "Legacy_PSP");
+                else
+                    Status = -EINVAL;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_POWER_MODE (=%d)\n",PowerMode));
+            break;
+         case RT_OID_802_11_TX_POWER_LEVEL_1:
+			if (wrq->u.data.length  < sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&PowerTemp, wrq->u.data.pointer, wrq->u.data.length);
+				if (PowerTemp > 100)
+					PowerTemp = 0xffffffff;  // AUTO
+				pAdapter->CommonCfg.TxPowerDefault = PowerTemp; //keep current setting.
+					pAdapter->CommonCfg.TxPowerPercentage = pAdapter->CommonCfg.TxPowerDefault;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_TX_POWER_LEVEL_1 (=%ld)\n", pAdapter->CommonCfg.TxPowerPercentage));
+			}
+	        break;
+		case OID_802_11_NETWORK_TYPE_IN_USE:
+			if (wrq->u.data.length != sizeof(NDIS_802_11_NETWORK_TYPE))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&NetType, wrq->u.data.pointer, wrq->u.data.length);
+
+				if (NetType == Ndis802_11DS)
+					RTMPSetPhyMode(pAdapter, PHY_11B);
+				else if (NetType == Ndis802_11OFDM24)
+					RTMPSetPhyMode(pAdapter, PHY_11BG_MIXED);
+				else if (NetType == Ndis802_11OFDM5)
+					RTMPSetPhyMode(pAdapter, PHY_11A);
+				else
+					Status = -EINVAL;
+#ifdef DOT11_N_SUPPORT
+				if (Status == NDIS_STATUS_SUCCESS)
+					SetCommonHT(pAdapter);
+#endif // DOT11_N_SUPPORT //
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_NETWORK_TYPE_IN_USE (=%d)\n",NetType));
+		    }
+			break;
+        // For WPA PSK PMK key
+        case RT_OID_802_11_ADD_WPA:
+            pKey = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+            if(pKey == NULL)
+            {
+                Status = -ENOMEM;
+                break;
+            }
+
+            Status = copy_from_user(pKey, wrq->u.data.pointer, wrq->u.data.length);
+            if (pKey->Length != wrq->u.data.length)
+            {
+                Status  = -EINVAL;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_WPA, Failed!!\n"));
+            }
+            else
+            {
+                if ((pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPAPSK) &&
+				    (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPA2PSK) &&
+				    (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPANone) )
+                {
+                    Status = -EOPNOTSUPP;
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_WPA, Failed!! [AuthMode != WPAPSK/WPA2PSK/WPANONE]\n"));
+                }
+                else if ((pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+						 (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK) ||
+						 (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPANone) )     // Only for WPA PSK mode
+				{
+                    NdisMoveMemory(pAdapter->StaCfg.PMK, &pKey->KeyMaterial, pKey->KeyLength);
+                    // Use RaConfig as PSK agent.
+                    // Start STA supplicant state machine
+                    if (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPANone)
+                        pAdapter->StaCfg.WpaState = SS_START;
+
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_WPA (id=0x%x, Len=%d-byte)\n", pKey->KeyIndex, pKey->KeyLength));
+                }
+                else
+                {
+                    pAdapter->StaCfg.WpaState = SS_NOTUSE;
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_WPA (id=0x%x, Len=%d-byte)\n", pKey->KeyIndex, pKey->KeyLength));
+                }
+            }
+            kfree(pKey);
+            break;
+        case OID_802_11_REMOVE_KEY:
+            pRemoveKey = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+            if(pRemoveKey == NULL)
+            {
+                Status = -ENOMEM;
+                break;
+            }
+
+            Status = copy_from_user(pRemoveKey, wrq->u.data.pointer, wrq->u.data.length);
+            if (pRemoveKey->Length != wrq->u.data.length)
+            {
+                Status  = -EINVAL;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY, Failed!!\n"));
+            }
+            else
+            {
+                if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+                {
+                    RTMPWPARemoveKeyProc(pAdapter, pRemoveKey);
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY, Remove WPA Key!!\n"));
+                }
+                else
+                {
+                    KeyIdx = pRemoveKey->KeyIndex;
+
+                    if (KeyIdx & 0x80000000)
+                    {
+                        // Should never set default bit when remove key
+                        Status  = -EINVAL;
+                        DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY, Failed!!(Should never set default bit when remove key)\n"));
+                    }
+                    else
+                    {
+                        KeyIdx = KeyIdx & 0x0fffffff;
+                        if (KeyIdx > 3)
+                        {
+                            Status  = -EINVAL;
+                            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY, Failed!!(KeyId[%d] out of range)\n", KeyIdx));
+                        }
+                        else
+                        {
+                            pAdapter->SharedKey[BSS0][KeyIdx].KeyLen = 0;
+                            pAdapter->SharedKey[BSS0][KeyIdx].CipherAlg = CIPHER_NONE;
+                            AsicRemoveSharedKeyEntry(pAdapter, 0, (UCHAR)KeyIdx);
+                            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY (id=0x%x, Len=%d-byte)\n", pRemoveKey->KeyIndex, pRemoveKey->Length));
+                        }
+                    }
+                }
+            }
+            kfree(pRemoveKey);
+            break;
+        // New for WPA
+        case OID_802_11_ADD_KEY:
+            pKey = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+            if(pKey == NULL)
+            {
+                Status = -ENOMEM;
+                break;
+            }
+            Status = copy_from_user(pKey, wrq->u.data.pointer, wrq->u.data.length);
+            if (pKey->Length != wrq->u.data.length)
+            {
+                Status  = -EINVAL;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_KEY, Failed!!\n"));
+            }
+            else
+            {
+                RTMPAddKey(pAdapter, pKey);
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_KEY (id=0x%x, Len=%d-byte)\n", pKey->KeyIndex, pKey->KeyLength));
+            }
+            kfree(pKey);
+            break;
+        case OID_802_11_CONFIGURATION:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_CONFIGURATION))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&Config, wrq->u.data.pointer, wrq->u.data.length);
+                pConfig = &Config;
+
+                if ((pConfig->BeaconPeriod >= 20) && (pConfig->BeaconPeriod <=400))
+                     pAdapter->CommonCfg.BeaconPeriod = (USHORT) pConfig->BeaconPeriod;
+
+                pAdapter->StaActive.AtimWin = (USHORT) pConfig->ATIMWindow;
+                MAP_KHZ_TO_CHANNEL_ID(pConfig->DSConfig, pAdapter->CommonCfg.Channel);
+                //
+				// Save the channel on MlmeAux for CntlOidRTBssidProc used.
+				//
+				pAdapter->MlmeAux.Channel = pAdapter->CommonCfg.Channel;
+
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_CONFIGURATION (BeacnPeriod=%ld,AtimW=%ld,Ch=%d)\n",
+                    pConfig->BeaconPeriod, pConfig->ATIMWindow, pAdapter->CommonCfg.Channel));
+                // Config has changed
+                pAdapter->bConfigChanged = TRUE;
+            }
+            break;
+#ifdef DOT11_N_SUPPORT
+		case RT_OID_802_11_SET_HT_PHYMODE:
+			if (wrq->u.data.length	!= sizeof(OID_SET_HT_PHYMODE))
+				Status = -EINVAL;
+			else
+			{
+			    POID_SET_HT_PHYMODE	pHTPhyMode = &HT_PhyMode;
+
+				Status = copy_from_user(&HT_PhyMode, wrq->u.data.pointer, wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::pHTPhyMode	(PhyMode = %d,TransmitNo = %d, HtMode =	%d,	ExtOffset =	%d , MCS = %d, BW =	%d,	STBC = %d, SHORTGI = %d) \n",
+				pHTPhyMode->PhyMode, pHTPhyMode->TransmitNo,pHTPhyMode->HtMode,pHTPhyMode->ExtOffset,
+				pHTPhyMode->MCS, pHTPhyMode->BW, pHTPhyMode->STBC,	pHTPhyMode->SHORTGI));
+				if (pAdapter->CommonCfg.PhyMode	>= PHY_11ABGN_MIXED)
+					RTMPSetHT(pAdapter,	pHTPhyMode);
+			}
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_HT_PHYMODE(MCS=%d,BW=%d,SGI=%d,STBC=%d)\n",
+				pAdapter->StaCfg.HTPhyMode.field.MCS, pAdapter->StaCfg.HTPhyMode.field.BW, pAdapter->StaCfg.HTPhyMode.field.ShortGI,
+				pAdapter->StaCfg.HTPhyMode.field.STBC));
+			break;
+#endif // DOT11_N_SUPPORT //
+		case RT_OID_802_11_SET_APSD_SETTING:
+			if (wrq->u.data.length != sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				ULONG apsd ;
+				Status = copy_from_user(&apsd, wrq->u.data.pointer,	wrq->u.data.length);
+
+				/*-------------------------------------------------------------------
+				|B31~B7	|	B6~B5	 |	 B4	 |	 B3	 |	B2	 |	B1	 |	   B0		|
+				---------------------------------------------------------------------
+				| Rsvd	| Max SP Len | AC_VO | AC_VI | AC_BK | AC_BE | APSD	Capable	|
+				---------------------------------------------------------------------*/
+				pAdapter->CommonCfg.bAPSDCapable = (apsd & 0x00000001) ? TRUE :	FALSE;
+				pAdapter->CommonCfg.bAPSDAC_BE = ((apsd	& 0x00000002) >> 1)	? TRUE : FALSE;
+				pAdapter->CommonCfg.bAPSDAC_BK = ((apsd	& 0x00000004) >> 2)	? TRUE : FALSE;
+				pAdapter->CommonCfg.bAPSDAC_VI = ((apsd	& 0x00000008) >> 3)	? TRUE : FALSE;
+				pAdapter->CommonCfg.bAPSDAC_VO = ((apsd	& 0x00000010) >> 4)	? TRUE : FALSE;
+				pAdapter->CommonCfg.MaxSPLength	= (UCHAR)((apsd	& 0x00000060) >> 5);
+
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_APSD_SETTING (apsd=0x%lx, APSDCap=%d, [BE,BK,VI,VO]=[%d/%d/%d/%d],	MaxSPLen=%d)\n", apsd, pAdapter->CommonCfg.bAPSDCapable,
+					pAdapter->CommonCfg.bAPSDAC_BE,	pAdapter->CommonCfg.bAPSDAC_BK,	pAdapter->CommonCfg.bAPSDAC_VI,	pAdapter->CommonCfg.bAPSDAC_VO,	pAdapter->CommonCfg.MaxSPLength));
+			}
+			break;
+
+		case RT_OID_802_11_SET_APSD_PSM:
+			if (wrq->u.data.length	!= sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				// Driver needs	to notify AP when PSM changes
+				Status = copy_from_user(&pAdapter->CommonCfg.bAPSDForcePowerSave, wrq->u.data.pointer, wrq->u.data.length);
+				if (pAdapter->CommonCfg.bAPSDForcePowerSave	!= pAdapter->StaCfg.Psm)
+				{
+					MlmeSetPsmBit(pAdapter,	pAdapter->CommonCfg.bAPSDForcePowerSave);
+					RTMPSendNullFrame(pAdapter,	pAdapter->CommonCfg.TxRate,	TRUE);
+				}
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_APSD_PSM (bAPSDForcePowerSave:%d)\n",	pAdapter->CommonCfg.bAPSDForcePowerSave));
+			}
+			break;
+#ifdef QOS_DLS_SUPPORT
+		case RT_OID_802_11_SET_DLS:
+			if (wrq->u.data.length != sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				BOOLEAN	oldvalue = pAdapter->CommonCfg.bDLSCapable;
+				Status = copy_from_user(&pAdapter->CommonCfg.bDLSCapable, wrq->u.data.pointer, wrq->u.data.length);
+				if (oldvalue &&	!pAdapter->CommonCfg.bDLSCapable)
+				{
+					int	i;
+					// tear	down local dls table entry
+					for	(i=0; i<MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+					{
+						if (pAdapter->StaCfg.DLSEntry[i].Valid && (pAdapter->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+						{
+							pAdapter->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+							pAdapter->StaCfg.DLSEntry[i].Valid	= FALSE;
+							RTMPSendDLSTearDownFrame(pAdapter, pAdapter->StaCfg.DLSEntry[i].MacAddr);
+						}
+					}
+
+					// tear	down peer dls table	entry
+					for	(i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+					{
+						if (pAdapter->StaCfg.DLSEntry[i].Valid && (pAdapter->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+						{
+							pAdapter->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+							pAdapter->StaCfg.DLSEntry[i].Valid	= FALSE;
+							RTMPSendDLSTearDownFrame(pAdapter, pAdapter->StaCfg.DLSEntry[i].MacAddr);
+						}
+					}
+				}
+
+				DBGPRINT(RT_DEBUG_TRACE,("Set::RT_OID_802_11_SET_DLS (=%d)\n", pAdapter->CommonCfg.bDLSCapable));
+			}
+			break;
+
+		case RT_OID_802_11_SET_DLS_PARAM:
+			if (wrq->u.data.length	!= sizeof(RT_802_11_DLS_UI))
+				Status = -EINVAL;
+			else
+			{
+				RT_802_11_DLS	Dls;
+
+				NdisZeroMemory(&Dls, sizeof(RT_802_11_DLS));
+				RTMPMoveMemory(&Dls, wrq->u.data.pointer, sizeof(RT_802_11_DLS_UI));
+				MlmeEnqueue(pAdapter,
+							MLME_CNTL_STATE_MACHINE,
+							RT_OID_802_11_SET_DLS_PARAM,
+							sizeof(RT_802_11_DLS),
+							&Dls);
+				DBGPRINT(RT_DEBUG_TRACE,("Set::RT_OID_802_11_SET_DLS_PARAM \n"));
+			}
+			break;
+#endif // QOS_DLS_SUPPORT //
+		case RT_OID_802_11_SET_WMM:
+			if (wrq->u.data.length	!= sizeof(BOOLEAN))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&pAdapter->CommonCfg.bWmmCapable, wrq->u.data.pointer, wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_WMM (=%d)	\n", pAdapter->CommonCfg.bWmmCapable));
+			}
+			break;
+
+		case OID_802_11_DISASSOCIATE:
+#ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+				break;
+			}
+#endif // RALINK_ATE //
+			//
+			// Set NdisRadioStateOff to	TRUE, instead of called	MlmeRadioOff.
+			// Later on, NDIS_802_11_BSSID_LIST_EX->NumberOfItems should be	0
+			// when	query OID_802_11_BSSID_LIST.
+			//
+			// TRUE:  NumberOfItems	will set to	0.
+			// FALSE: NumberOfItems	no change.
+			//
+			pAdapter->CommonCfg.NdisRadioStateOff =	TRUE;
+			// Set to immediately send the media disconnect	event
+			pAdapter->MlmeAux.CurrReqIsFromNdis	= TRUE;
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_DISASSOCIATE	\n"));
+
+			if (INFRA_ON(pAdapter))
+			{
+				if (pAdapter->Mlme.CntlMachine.CurrState !=	CNTL_IDLE)
+				{
+					RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+					DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME	busy, reset	MLME state machine !!!\n"));
+				}
+
+				MlmeEnqueue(pAdapter,
+					MLME_CNTL_STATE_MACHINE,
+					OID_802_11_DISASSOCIATE,
+					0,
+					NULL);
+
+				StateMachineTouched	= TRUE;
+			}
+			break;
+
+#ifdef DOT11_N_SUPPORT
+		case RT_OID_802_11_SET_IMME_BA_CAP:
+				if (wrq->u.data.length != sizeof(OID_BACAP_STRUC))
+					Status = -EINVAL;
+				else
+				{
+					OID_BACAP_STRUC Orde ;
+					Status = copy_from_user(&Orde, wrq->u.data.pointer, wrq->u.data.length);
+					if (Orde.Policy > BA_NOTUSE)
+					{
+						Status = NDIS_STATUS_INVALID_DATA;
+					}
+					else if (Orde.Policy == BA_NOTUSE)
+					{
+						pAdapter->CommonCfg.BACapability.field.Policy = BA_NOTUSE;
+						pAdapter->CommonCfg.BACapability.field.MpduDensity = Orde.MpduDensity;
+						pAdapter->CommonCfg.DesiredHtPhy.MpduDensity = Orde.MpduDensity;
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduEnable = Orde.AmsduEnable;
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduSize= Orde.AmsduSize;
+						pAdapter->CommonCfg.DesiredHtPhy.MimoPs= Orde.MMPSmode;
+						pAdapter->CommonCfg.BACapability.field.MMPSmode = Orde.MMPSmode;
+						// UPdata to HT IE
+						pAdapter->CommonCfg.HtCapability.HtCapInfo.MimoPs = Orde.MMPSmode;
+						pAdapter->CommonCfg.HtCapability.HtCapInfo.AMsduSize = Orde.AmsduSize;
+						pAdapter->CommonCfg.HtCapability.HtCapParm.MpduDensity = Orde.MpduDensity;
+					}
+					else
+					{
+                        pAdapter->CommonCfg.BACapability.field.AutoBA = Orde.AutoBA;
+						pAdapter->CommonCfg.BACapability.field.Policy = IMMED_BA; // we only support immediate BA.
+						pAdapter->CommonCfg.BACapability.field.MpduDensity = Orde.MpduDensity;
+						pAdapter->CommonCfg.DesiredHtPhy.MpduDensity = Orde.MpduDensity;
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduEnable = Orde.AmsduEnable;
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduSize= Orde.AmsduSize;
+						pAdapter->CommonCfg.DesiredHtPhy.MimoPs = Orde.MMPSmode;
+						pAdapter->CommonCfg.BACapability.field.MMPSmode = Orde.MMPSmode;
+
+						// UPdata to HT IE
+						pAdapter->CommonCfg.HtCapability.HtCapInfo.MimoPs = Orde.MMPSmode;
+						pAdapter->CommonCfg.HtCapability.HtCapInfo.AMsduSize = Orde.AmsduSize;
+						pAdapter->CommonCfg.HtCapability.HtCapParm.MpduDensity = Orde.MpduDensity;
+
+						if (pAdapter->CommonCfg.BACapability.field.RxBAWinLimit > MAX_RX_REORDERBUF)
+							pAdapter->CommonCfg.BACapability.field.RxBAWinLimit = MAX_RX_REORDERBUF;
+
+					}
+
+					pAdapter->CommonCfg.REGBACapability.word = pAdapter->CommonCfg.BACapability.word;
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::(Orde.AutoBA = %d) (Policy=%d)(ReBAWinLimit=%d)(TxBAWinLimit=%d)(AutoMode=%d)\n",Orde.AutoBA, pAdapter->CommonCfg.BACapability.field.Policy,
+						pAdapter->CommonCfg.BACapability.field.RxBAWinLimit,pAdapter->CommonCfg.BACapability.field.TxBAWinLimit, pAdapter->CommonCfg.BACapability.field.AutoBA));
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::(MimoPs = %d)(AmsduEnable = %d) (AmsduSize=%d)(MpduDensity=%d)\n",pAdapter->CommonCfg.DesiredHtPhy.MimoPs, pAdapter->CommonCfg.DesiredHtPhy.AmsduEnable,
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduSize, pAdapter->CommonCfg.DesiredHtPhy.MpduDensity));
+				}
+
+				break;
+		case RT_OID_802_11_ADD_IMME_BA:
+			DBGPRINT(RT_DEBUG_TRACE, (" Set :: RT_OID_802_11_ADD_IMME_BA \n"));
+			if (wrq->u.data.length != sizeof(OID_ADD_BA_ENTRY))
+					Status = -EINVAL;
+			else
+			{
+				UCHAR		        index;
+				OID_ADD_BA_ENTRY    BA;
+				MAC_TABLE_ENTRY     *pEntry;
+
+				Status = copy_from_user(&BA, wrq->u.data.pointer, wrq->u.data.length);
+				if (BA.TID > 15)
+				{
+					Status = NDIS_STATUS_INVALID_DATA;
+					break;
+				}
+				else
+				{
+					//BATableInsertEntry
+					//As ad-hoc mode, BA pair is not limited to only BSSID. so add via OID.
+					index = BA.TID;
+					// in ad hoc mode, when adding BA pair, we should insert this entry into MACEntry too
+					pEntry = MacTableLookup(pAdapter, BA.MACAddr);
+					if (!pEntry)
+					{
+						DBGPRINT(RT_DEBUG_TRACE, ("RT_OID_802_11_ADD_IMME_BA. break on no connection.----:%x:%x\n", BA.MACAddr[4], BA.MACAddr[5]));
+						break;
+					}
+					if (BA.IsRecipient == FALSE)
+					{
+					    if (pEntry->bIAmBadAtheros == TRUE)
+							pAdapter->CommonCfg.BACapability.field.RxBAWinLimit = 0x10;
+
+						BAOriSessionSetUp(pAdapter, pEntry, index, 0, 100, TRUE);
+					}
+					else
+					{
+						//BATableInsertEntry(pAdapter, pEntry->Aid, BA.MACAddr, 0, 0xffff, BA.TID, BA.nMSDU, BA.IsRecipient);
+					}
+
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_IMME_BA. Rec = %d. Mac = %x:%x:%x:%x:%x:%x . \n",
+						BA.IsRecipient, BA.MACAddr[0], BA.MACAddr[1], BA.MACAddr[2], BA.MACAddr[2]
+						, BA.MACAddr[4], BA.MACAddr[5]));
+				}
+			}
+			break;
+
+		case RT_OID_802_11_TEAR_IMME_BA:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set :: RT_OID_802_11_TEAR_IMME_BA \n"));
+			if (wrq->u.data.length != sizeof(OID_ADD_BA_ENTRY))
+					Status = -EINVAL;
+			else
+			{
+				POID_ADD_BA_ENTRY	pBA;
+				MAC_TABLE_ENTRY *pEntry;
+
+				pBA = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+
+				if (pBA == NULL)
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("Set :: RT_OID_802_11_TEAR_IMME_BA kmalloc() can't allocate enough memory\n"));
+					Status = NDIS_STATUS_FAILURE;
+				}
+				else
+				{
+					Status = copy_from_user(pBA, wrq->u.data.pointer, wrq->u.data.length);
+					DBGPRINT(RT_DEBUG_TRACE, ("Set :: RT_OID_802_11_TEAR_IMME_BA(TID=%d, bAllTid=%d)\n", pBA->TID, pBA->bAllTid));
+
+					if (!pBA->bAllTid && (pBA->TID > NUM_OF_TID))
+					{
+						Status = NDIS_STATUS_INVALID_DATA;
+						break;
+					}
+
+					if (pBA->IsRecipient == FALSE)
+					{
+						pEntry = MacTableLookup(pAdapter, pBA->MACAddr);
+						DBGPRINT(RT_DEBUG_TRACE, (" pBA->IsRecipient == FALSE\n"));
+						if (pEntry)
+						{
+							DBGPRINT(RT_DEBUG_TRACE, (" pBA->pEntry\n"));
+							BAOriSessionTearDown(pAdapter, pEntry->Aid, pBA->TID, FALSE, TRUE);
+						}
+						else
+							DBGPRINT(RT_DEBUG_TRACE, ("Set :: Not found pEntry \n"));
+					}
+					else
+					{
+						pEntry = MacTableLookup(pAdapter, pBA->MACAddr);
+						if (pEntry)
+						{
+							BARecSessionTearDown( pAdapter, (UCHAR)pEntry->Aid, pBA->TID, TRUE);
+						}
+						else
+							DBGPRINT(RT_DEBUG_TRACE, ("Set :: Not found pEntry \n"));
+					}
+					kfree(pBA);
+				}
+            }
+            break;
+#endif // DOT11_N_SUPPORT //
+
+        // For WPA_SUPPLICANT to set static wep key
+    	case OID_802_11_ADD_WEP:
+    	    pWepKey = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+
+    	    if(pWepKey == NULL)
+            {
+                Status = -ENOMEM;
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP, Failed!!\n"));
+                break;
+            }
+            Status = copy_from_user(pWepKey, wrq->u.data.pointer, wrq->u.data.length);
+            if (Status)
+            {
+                Status  = -EINVAL;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP, Failed (length mismatch)!!\n"));
+            }
+            else
+            {
+		        KeyIdx = pWepKey->KeyIndex & 0x0fffffff;
+                // KeyIdx must be 0 ~ 3
+                if (KeyIdx > 4)
+    			{
+                    Status  = -EINVAL;
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP, Failed (KeyIdx must be smaller than 4)!!\n"));
+                }
+                else
+                {
+                    UCHAR CipherAlg = 0;
+                    PUCHAR Key;
+
+                    // set key material and key length
+                    NdisZeroMemory(pAdapter->SharedKey[BSS0][KeyIdx].Key, 16);
+                    pAdapter->SharedKey[BSS0][KeyIdx].KeyLen = (UCHAR) pWepKey->KeyLength;
+                    NdisMoveMemory(pAdapter->SharedKey[BSS0][KeyIdx].Key, &pWepKey->KeyMaterial, pWepKey->KeyLength);
+
+                    switch(pWepKey->KeyLength)
+                    {
+                        case 5:
+                            CipherAlg = CIPHER_WEP64;
+                            break;
+                        case 13:
+                            CipherAlg = CIPHER_WEP128;
+                            break;
+                        default:
+                            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP, only support CIPHER_WEP64(len:5) & CIPHER_WEP128(len:13)!!\n"));
+                            Status = -EINVAL;
+                            break;
+                    }
+                    pAdapter->SharedKey[BSS0][KeyIdx].CipherAlg = CipherAlg;
+
+                    // Default key for tx (shared key)
+                    if (pWepKey->KeyIndex & 0x80000000)
+                    {
+#ifdef WPA_SUPPLICANT_SUPPORT
+                        // set key material and key length
+                        NdisZeroMemory(pAdapter->StaCfg.DesireSharedKey[KeyIdx].Key, 16);
+                        pAdapter->StaCfg.DesireSharedKey[KeyIdx].KeyLen = (UCHAR) pWepKey->KeyLength;
+                        NdisMoveMemory(pAdapter->StaCfg.DesireSharedKey[KeyIdx].Key, &pWepKey->KeyMaterial, pWepKey->KeyLength);
+                        pAdapter->StaCfg.DesireSharedKeyId = KeyIdx;
+                        pAdapter->StaCfg.DesireSharedKey[KeyIdx].CipherAlg = CipherAlg;
+#endif // WPA_SUPPLICANT_SUPPORT //
+                        pAdapter->StaCfg.DefaultKeyId = (UCHAR) KeyIdx;
+                    }
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+                    if (pAdapter->StaCfg.PortSecured == WPA_802_1X_PORT_SECURED)
+#endif // WPA_SUPPLICANT_SUPPORT
+                    {
+                        Key = pAdapter->SharedKey[BSS0][KeyIdx].Key;
+
+                        // Set key material and cipherAlg to Asic
+        				AsicAddSharedKeyEntry(pAdapter, BSS0, KeyIdx, CipherAlg, Key, NULL, NULL);
+
+                        if (pWepKey->KeyIndex & 0x80000000)
+                        {
+                            PMAC_TABLE_ENTRY pEntry = &pAdapter->MacTab.Content[BSSID_WCID];
+                            // Assign group key info
+    						RTMPAddWcidAttributeEntry(pAdapter, BSS0, KeyIdx, CipherAlg, NULL);
+    						// Assign pairwise key info
+    						RTMPAddWcidAttributeEntry(pAdapter, BSS0, KeyIdx, CipherAlg, pEntry);
+                        }
+                    }
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP (id=0x%x, Len=%d-byte), %s\n", pWepKey->KeyIndex, pWepKey->KeyLength, (pAdapter->StaCfg.PortSecured == WPA_802_1X_PORT_SECURED) ? "Port Secured":"Port NOT Secured"));
+				}
+            }
+            kfree(pWepKey);
+            break;
+#ifdef WPA_SUPPLICANT_SUPPORT
+	    case OID_SET_COUNTERMEASURES:
+            if (wrq->u.data.length != sizeof(int))
+                Status  = -EINVAL;
+            else
+            {
+                int enabled = 0;
+                Status = copy_from_user(&enabled, wrq->u.data.pointer, wrq->u.data.length);
+                if (enabled == 1)
+                    pAdapter->StaCfg.bBlockAssoc = TRUE;
+                else
+                    // WPA MIC error should block association attempt for 60 seconds
+                    pAdapter->StaCfg.bBlockAssoc = FALSE;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_SET_COUNTERMEASURES bBlockAssoc=%s\n", pAdapter->StaCfg.bBlockAssoc ? "TRUE":"FALSE"));
+            }
+	        break;
+        case RT_OID_WPA_SUPPLICANT_SUPPORT:
+			if (wrq->u.data.length != sizeof(UCHAR))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&wpa_supplicant_enable, wrq->u.data.pointer, wrq->u.data.length);
+    			pAdapter->StaCfg.WpaSupplicantUP = wpa_supplicant_enable;
+    			DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_WPA_SUPPLICANT_SUPPORT (=%d)\n", pAdapter->StaCfg.WpaSupplicantUP));
+			}
+            break;
+        case OID_802_11_DEAUTHENTICATION:
+            if (wrq->u.data.length != sizeof(MLME_DEAUTH_REQ_STRUCT))
+                Status  = -EINVAL;
+            else
+            {
+                MLME_DEAUTH_REQ_STRUCT      *pInfo;
+				MLME_QUEUE_ELEM *MsgElem = (MLME_QUEUE_ELEM *) kmalloc(sizeof(MLME_QUEUE_ELEM), MEM_ALLOC_FLAG);
+
+                pInfo = (MLME_DEAUTH_REQ_STRUCT *) MsgElem->Msg;
+                Status = copy_from_user(pInfo, wrq->u.data.pointer, wrq->u.data.length);
+                MlmeDeauthReqAction(pAdapter, MsgElem);
+				kfree(MsgElem);
+
+                if (INFRA_ON(pAdapter))
+                {
+                    LinkDown(pAdapter, FALSE);
+                    pAdapter->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+                }
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_DEAUTHENTICATION (Reason=%d)\n", pInfo->Reason));
+            }
+            break;
+        case OID_802_11_DROP_UNENCRYPTED:
+            if (wrq->u.data.length != sizeof(int))
+                Status  = -EINVAL;
+            else
+            {
+                int enabled = 0;
+                Status = copy_from_user(&enabled, wrq->u.data.pointer, wrq->u.data.length);
+                if (enabled == 1)
+                    pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+                else
+                    pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+				NdisAcquireSpinLock(&pAdapter->MacTabLock);
+				pAdapter->MacTab.Content[BSSID_WCID].PortSecured = pAdapter->StaCfg.PortSecured;
+				NdisReleaseSpinLock(&pAdapter->MacTabLock);
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_DROP_UNENCRYPTED (=%d)\n", enabled));
+            }
+            break;
+        case OID_802_11_SET_IEEE8021X:
+            if (wrq->u.data.length != sizeof(BOOLEAN))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&IEEE8021xState, wrq->u.data.pointer, wrq->u.data.length);
+		        pAdapter->StaCfg.IEEE8021X = IEEE8021xState;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_SET_IEEE8021X (=%d)\n", IEEE8021xState));
+            }
+            break;
+        case OID_802_11_SET_IEEE8021X_REQUIRE_KEY:
+			if (wrq->u.data.length != sizeof(BOOLEAN))
+				 Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&IEEE8021x_required_keys, wrq->u.data.pointer, wrq->u.data.length);
+				pAdapter->StaCfg.IEEE8021x_required_keys = IEEE8021x_required_keys;
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_SET_IEEE8021X_REQUIRE_KEY (%d)\n", IEEE8021x_required_keys));
+			}
+			break;
+        case OID_802_11_PMKID:
+	        pPmkId = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+
+	        if(pPmkId == NULL) {
+                Status = -ENOMEM;
+                break;
+            }
+            Status = copy_from_user(pPmkId, wrq->u.data.pointer, wrq->u.data.length);
+
+	        // check the PMKID information
+	        if (pPmkId->BSSIDInfoCount == 0)
+                NdisZeroMemory(pAdapter->StaCfg.SavedPMK, sizeof(BSSID_INFO)*PMKID_NO);
+	        else
+	        {
+		        PBSSID_INFO	pBssIdInfo;
+		        UINT		BssIdx;
+		        UINT		CachedIdx;
+
+		        for (BssIdx = 0; BssIdx < pPmkId->BSSIDInfoCount; BssIdx++)
+		        {
+			        // point to the indexed BSSID_INFO structure
+			        pBssIdInfo = (PBSSID_INFO) ((PUCHAR) pPmkId + 2 * sizeof(UINT) + BssIdx * sizeof(BSSID_INFO));
+			        // Find the entry in the saved data base.
+			        for (CachedIdx = 0; CachedIdx < pAdapter->StaCfg.SavedPMKNum; CachedIdx++)
+			        {
+				        // compare the BSSID
+				        if (NdisEqualMemory(pBssIdInfo->BSSID, pAdapter->StaCfg.SavedPMK[CachedIdx].BSSID, sizeof(NDIS_802_11_MAC_ADDRESS)))
+					        break;
+			        }
+
+			        // Found, replace it
+			        if (CachedIdx < PMKID_NO)
+			        {
+				        DBGPRINT(RT_DEBUG_OFF, ("Update OID_802_11_PMKID, idx = %d\n", CachedIdx));
+				        NdisMoveMemory(&pAdapter->StaCfg.SavedPMK[CachedIdx], pBssIdInfo, sizeof(BSSID_INFO));
+				        pAdapter->StaCfg.SavedPMKNum++;
+			        }
+			        // Not found, replace the last one
+			        else
+			        {
+				        // Randomly replace one
+				        CachedIdx = (pBssIdInfo->BSSID[5] % PMKID_NO);
+				        DBGPRINT(RT_DEBUG_OFF, ("Update OID_802_11_PMKID, idx = %d\n", CachedIdx));
+				        NdisMoveMemory(&pAdapter->StaCfg.SavedPMK[CachedIdx], pBssIdInfo, sizeof(BSSID_INFO));
+			        }
+		        }
+			}
+			if(pPmkId)
+				kfree(pPmkId);
+	        break;
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+
+
+#ifdef SNMP_SUPPORT
+		case OID_802_11_SHORTRETRYLIMIT:
+			if (wrq->u.data.length != sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&ShortRetryLimit, wrq->u.data.pointer, wrq->u.data.length);
+				RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+				tx_rty_cfg.field.ShortRtyLimit = ShortRetryLimit;
+				RTMP_IO_WRITE32(pAdapter, TX_RTY_CFG, tx_rty_cfg.word);
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_SHORTRETRYLIMIT (tx_rty_cfg.field.ShortRetryLimit=%d, ShortRetryLimit=%ld)\n", tx_rty_cfg.field.ShortRtyLimit, ShortRetryLimit));
+			}
+			break;
+
+		case OID_802_11_LONGRETRYLIMIT:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_LONGRETRYLIMIT \n"));
+			if (wrq->u.data.length != sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&LongRetryLimit, wrq->u.data.pointer, wrq->u.data.length);
+				RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+				tx_rty_cfg.field.LongRtyLimit = LongRetryLimit;
+				RTMP_IO_WRITE32(pAdapter, TX_RTY_CFG, tx_rty_cfg.word);
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_LONGRETRYLIMIT (tx_rty_cfg.field.LongRetryLimit= %d,LongRetryLimit=%ld)\n", tx_rty_cfg.field.LongRtyLimit, LongRetryLimit));
+			}
+			break;
+
+		case OID_802_11_WEPDEFAULTKEYVALUE:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_WEPDEFAULTKEYVALUE\n"));
+			pKey = kmalloc(wrq->u.data.length, GFP_KERNEL);
+			Status = copy_from_user(pKey, wrq->u.data.pointer, wrq->u.data.length);
+			//pKey = &WepKey;
+
+			if ( pKey->Length != wrq->u.data.length)
+			{
+				Status = -EINVAL;
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_WEPDEFAULTKEYVALUE, Failed!!\n"));
+			}
+			KeyIdx = pKey->KeyIndex & 0x0fffffff;
+			DBGPRINT(RT_DEBUG_TRACE,("pKey->KeyIndex =%d, pKey->KeyLength=%d\n", pKey->KeyIndex, pKey->KeyLength));
+
+			// it is a shared key
+			if (KeyIdx > 4)
+				Status = -EINVAL;
+			else
+			{
+				pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen = (UCHAR) pKey->KeyLength;
+				NdisMoveMemory(&pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].Key, &pKey->KeyMaterial, pKey->KeyLength);
+				if (pKey->KeyIndex & 0x80000000)
+				{
+					// Default key for tx (shared key)
+					pAdapter->StaCfg.DefaultKeyId = (UCHAR) KeyIdx;
+				}
+				//RestartAPIsRequired = TRUE;
+			}
+			break;
+
+
+		case OID_802_11_WEPDEFAULTKEYID:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_WEPDEFAULTKEYID \n"));
+
+			if (wrq->u.data.length != sizeof(UCHAR))
+				Status = -EINVAL;
+			else
+				Status = copy_from_user(&pAdapter->StaCfg.DefaultKeyId, wrq->u.data.pointer, wrq->u.data.length);
+
+			break;
+
+
+		case OID_802_11_CURRENTCHANNEL:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_CURRENTCHANNEL \n"));
+			if (wrq->u.data.length != sizeof(UCHAR))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&ctmp, wrq->u.data.pointer, wrq->u.data.length);
+				sprintf(&ctmp,"%d", ctmp);
+				Set_Channel_Proc(pAdapter, &ctmp);
+			}
+			break;
+#endif
+
+
+
+        default:
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::unknown IOCTL's subcmd = 0x%08x\n", cmd));
+            Status = -EOPNOTSUPP;
+            break;
+    }
+
+
+    return Status;
+}
+
+INT RTMPQueryInformation(
+    IN  PRTMP_ADAPTER pAdapter,
+    IN  OUT struct ifreq    *rq,
+    IN  INT                 cmd)
+{
+    struct iwreq                        *wrq = (struct iwreq *) rq;
+    NDIS_802_11_BSSID_LIST_EX           *pBssidList = NULL;
+    PNDIS_WLAN_BSSID_EX                 pBss;
+    NDIS_802_11_SSID                    Ssid;
+    NDIS_802_11_CONFIGURATION           *pConfiguration = NULL;
+    RT_802_11_LINK_STATUS               *pLinkStatus = NULL;
+    RT_802_11_STA_CONFIG                *pStaConfig = NULL;
+    NDIS_802_11_STATISTICS              *pStatistics = NULL;
+    NDIS_802_11_RTS_THRESHOLD           RtsThresh;
+    NDIS_802_11_FRAGMENTATION_THRESHOLD FragThresh;
+    NDIS_802_11_POWER_MODE              PowerMode;
+    NDIS_802_11_NETWORK_INFRASTRUCTURE  BssType;
+    RT_802_11_PREAMBLE                  PreamType;
+    NDIS_802_11_AUTHENTICATION_MODE     AuthMode;
+    NDIS_802_11_WEP_STATUS              WepStatus;
+    NDIS_MEDIA_STATE                    MediaState;
+    ULONG                               BssBufSize, ulInfo=0, NetworkTypeList[4], apsd = 0;
+    USHORT                              BssLen = 0;
+    PUCHAR                              pBuf = NULL, pPtr;
+    INT                                 Status = NDIS_STATUS_SUCCESS;
+    UINT                                we_version_compiled;
+    UCHAR                               i, Padding = 0;
+    BOOLEAN                             RadioState;
+	UCHAR	driverVersion[8];
+    OID_SET_HT_PHYMODE			        *pHTPhyMode = NULL;
+
+
+#ifdef SNMP_SUPPORT
+	//for snmp, kathy
+	DefaultKeyIdxValue			*pKeyIdxValue;
+	INT							valueLen;
+	TX_RTY_CFG_STRUC			tx_rty_cfg;
+	ULONG						ShortRetryLimit, LongRetryLimit;
+	UCHAR						tmp[64];
+#endif //SNMP
+
+    switch(cmd)
+    {
+        case RT_OID_DEVICE_NAME:
+            wrq->u.data.length = sizeof(STA_NIC_DEVICE_NAME);
+            Status = copy_to_user(wrq->u.data.pointer, STA_NIC_DEVICE_NAME, wrq->u.data.length);
+            break;
+        case RT_OID_VERSION_INFO:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_VERSION_INFO \n"));
+			wrq->u.data.length = 8*sizeof(UCHAR);
+			sprintf(&driverVersion[0], "%s", STA_DRIVER_VERSION);
+			driverVersion[7] = '\0';
+			if (copy_to_user(wrq->u.data.pointer, &driverVersion, wrq->u.data.length))
+            {
+				Status = -EFAULT;
+            }
+            break;
+#ifdef RALINK_ATE
+		case RT_QUERY_ATE_TXDONE_COUNT:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_QUERY_ATE_TXDONE_COUNT \n"));
+			wrq->u.data.length = sizeof(UINT32);
+			if (copy_to_user(wrq->u.data.pointer, &pAdapter->ate.TxDoneCount, wrq->u.data.length))
+			{
+				Status = -EFAULT;
+			}
+			break;
+#endif // RALINK_ATE //
+        case OID_802_11_BSSID_LIST:
+            if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+            {
+            	/*
+            	 * Still scanning, indicate the caller should try again.
+            	 */
+            	DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_BSSID_LIST (Still scanning)\n"));
+				return -EAGAIN;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_BSSID_LIST (%d BSS returned)\n",pAdapter->ScanTab.BssNr));
+			pAdapter->StaCfg.bScanReqIsFromWebUI = FALSE;
+            // Claculate total buffer size required
+            BssBufSize = sizeof(ULONG);
+
+            for (i = 0; i < pAdapter->ScanTab.BssNr; i++)
+            {
+                // Align pointer to 4 bytes boundary.
+                //Padding = 4 - (pAdapter->ScanTab.BssEntry[i].VarIELen & 0x0003);
+                //if (Padding == 4)
+                //    Padding = 0;
+                BssBufSize += (sizeof(NDIS_WLAN_BSSID_EX) - 1 + sizeof(NDIS_802_11_FIXED_IEs) + pAdapter->ScanTab.BssEntry[i].VarIELen + Padding);
+            }
+
+            // For safety issue, we add 256 bytes just in case
+            BssBufSize += 256;
+            // Allocate the same size as passed from higher layer
+            pBuf = kmalloc(BssBufSize, MEM_ALLOC_FLAG);
+            if(pBuf == NULL)
+            {
+                Status = -ENOMEM;
+                break;
+            }
+            // Init 802_11_BSSID_LIST_EX structure
+            NdisZeroMemory(pBuf, BssBufSize);
+            pBssidList = (PNDIS_802_11_BSSID_LIST_EX) pBuf;
+            pBssidList->NumberOfItems = pAdapter->ScanTab.BssNr;
+
+            // Calculate total buffer length
+            BssLen = 4; // Consist of NumberOfItems
+            // Point to start of NDIS_WLAN_BSSID_EX
+            // pPtr = pBuf + sizeof(ULONG);
+            pPtr = (PUCHAR) &pBssidList->Bssid[0];
+            for (i = 0; i < pAdapter->ScanTab.BssNr; i++)
+            {
+                pBss = (PNDIS_WLAN_BSSID_EX) pPtr;
+                NdisMoveMemory(&pBss->MacAddress, &pAdapter->ScanTab.BssEntry[i].Bssid, MAC_ADDR_LEN);
+                if ((pAdapter->ScanTab.BssEntry[i].Hidden == 1) && (pAdapter->StaCfg.bShowHiddenSSID == FALSE))
+                {
+                    //
+					// We must return this SSID during 4way handshaking, otherwise Aegis will failed to parse WPA infomation
+					// and then failed to send EAPOl farame.
+					//
+					if ((pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA) && (pAdapter->StaCfg.PortSecured != WPA_802_1X_PORT_SECURED))
+					{
+						pBss->Ssid.SsidLength = pAdapter->ScanTab.BssEntry[i].SsidLen;
+						NdisMoveMemory(pBss->Ssid.Ssid, pAdapter->ScanTab.BssEntry[i].Ssid, pAdapter->ScanTab.BssEntry[i].SsidLen);
+					}
+					else
+                    	pBss->Ssid.SsidLength = 0;
+                }
+                else
+                {
+                    pBss->Ssid.SsidLength = pAdapter->ScanTab.BssEntry[i].SsidLen;
+                    NdisMoveMemory(pBss->Ssid.Ssid, pAdapter->ScanTab.BssEntry[i].Ssid, pAdapter->ScanTab.BssEntry[i].SsidLen);
+                }
+                pBss->Privacy = pAdapter->ScanTab.BssEntry[i].Privacy;
+                pBss->Rssi = pAdapter->ScanTab.BssEntry[i].Rssi - pAdapter->BbpRssiToDbmDelta;
+                pBss->NetworkTypeInUse = NetworkTypeInUseSanity(&pAdapter->ScanTab.BssEntry[i]);
+                pBss->Configuration.Length = sizeof(NDIS_802_11_CONFIGURATION);
+                pBss->Configuration.BeaconPeriod = pAdapter->ScanTab.BssEntry[i].BeaconPeriod;
+                pBss->Configuration.ATIMWindow = pAdapter->ScanTab.BssEntry[i].AtimWin;
+
+                MAP_CHANNEL_ID_TO_KHZ(pAdapter->ScanTab.BssEntry[i].Channel, pBss->Configuration.DSConfig);
+
+                if (pAdapter->ScanTab.BssEntry[i].BssType == BSS_INFRA)
+                    pBss->InfrastructureMode = Ndis802_11Infrastructure;
+                else
+                    pBss->InfrastructureMode = Ndis802_11IBSS;
+
+                NdisMoveMemory(pBss->SupportedRates, pAdapter->ScanTab.BssEntry[i].SupRate, pAdapter->ScanTab.BssEntry[i].SupRateLen);
+                NdisMoveMemory(pBss->SupportedRates + pAdapter->ScanTab.BssEntry[i].SupRateLen,
+                               pAdapter->ScanTab.BssEntry[i].ExtRate,
+                               pAdapter->ScanTab.BssEntry[i].ExtRateLen);
+
+                if (pAdapter->ScanTab.BssEntry[i].VarIELen == 0)
+                {
+                    pBss->IELength = sizeof(NDIS_802_11_FIXED_IEs);
+                    NdisMoveMemory(pBss->IEs, &pAdapter->ScanTab.BssEntry[i].FixIEs, sizeof(NDIS_802_11_FIXED_IEs));
+                    pPtr = pPtr + sizeof(NDIS_WLAN_BSSID_EX) - 1 + sizeof(NDIS_802_11_FIXED_IEs);
+                }
+                else
+                {
+                    pBss->IELength = (ULONG)(sizeof(NDIS_802_11_FIXED_IEs) + pAdapter->ScanTab.BssEntry[i].VarIELen);
+                    pPtr = pPtr + sizeof(NDIS_WLAN_BSSID_EX) - 1 + sizeof(NDIS_802_11_FIXED_IEs);
+                    NdisMoveMemory(pBss->IEs, &pAdapter->ScanTab.BssEntry[i].FixIEs, sizeof(NDIS_802_11_FIXED_IEs));
+                    NdisMoveMemory(pBss->IEs + sizeof(NDIS_802_11_FIXED_IEs), pAdapter->ScanTab.BssEntry[i].VarIEs, pAdapter->ScanTab.BssEntry[i].VarIELen);
+                    pPtr += pAdapter->ScanTab.BssEntry[i].VarIELen;
+                }
+                pBss->Length = (ULONG)(sizeof(NDIS_WLAN_BSSID_EX) - 1 + sizeof(NDIS_802_11_FIXED_IEs) + pAdapter->ScanTab.BssEntry[i].VarIELen + Padding);
+
+#if WIRELESS_EXT < 17
+                if ((BssLen + pBss->Length) < wrq->u.data.length)
+                BssLen += pBss->Length;
+                else
+                {
+                    pBssidList->NumberOfItems = i;
+                    break;
+                }
+#else
+                BssLen += pBss->Length;
+#endif
+            }
+
+#if WIRELESS_EXT < 17
+            wrq->u.data.length = BssLen;
+#else
+            if (BssLen > wrq->u.data.length)
+            {
+                kfree(pBssidList);
+                return -E2BIG;
+            }
+            else
+                wrq->u.data.length = BssLen;
+#endif
+            Status = copy_to_user(wrq->u.data.pointer, pBssidList, BssLen);
+            kfree(pBssidList);
+            break;
+        case OID_802_3_CURRENT_ADDRESS:
+            wrq->u.data.length = MAC_ADDR_LEN;
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CurrentAddress, wrq->u.data.length);
+            break;
+        case OID_GEN_MEDIA_CONNECT_STATUS:
+            if (pAdapter->IndicateMediaState == NdisMediaStateConnected)
+                MediaState = NdisMediaStateConnected;
+            else
+                MediaState = NdisMediaStateDisconnected;
+
+            wrq->u.data.length = sizeof(NDIS_MEDIA_STATE);
+            Status = copy_to_user(wrq->u.data.pointer, &MediaState, wrq->u.data.length);
+            break;
+        case OID_802_11_BSSID:
+#ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+				Status = NDIS_STATUS_RESOURCES;
+				break;
+			}
+#endif // RALINK_ATE //
+            if (INFRA_ON(pAdapter) || ADHOC_ON(pAdapter))
+            {
+                Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.Bssid, sizeof(NDIS_802_11_MAC_ADDRESS));
+
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_BSSID(=EMPTY)\n"));
+                Status = -ENOTCONN;
+            }
+            break;
+        case OID_802_11_SSID:
+			NdisZeroMemory(&Ssid, sizeof(NDIS_802_11_SSID));
+			NdisZeroMemory(Ssid.Ssid, MAX_LEN_OF_SSID);
+            Ssid.SsidLength = pAdapter->CommonCfg.SsidLen;
+			memcpy(Ssid.Ssid, pAdapter->CommonCfg.Ssid,	Ssid.SsidLength);
+            wrq->u.data.length = sizeof(NDIS_802_11_SSID);
+            Status = copy_to_user(wrq->u.data.pointer, &Ssid, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_SSID (Len=%d, ssid=%s)\n", Ssid.SsidLength,Ssid.Ssid));
+            break;
+        case RT_OID_802_11_QUERY_LINK_STATUS:
+            pLinkStatus = (RT_802_11_LINK_STATUS *) kmalloc(sizeof(RT_802_11_LINK_STATUS), MEM_ALLOC_FLAG);
+            if (pLinkStatus)
+            {
+                pLinkStatus->CurrTxRate = RateIdTo500Kbps[pAdapter->CommonCfg.TxRate];   // unit : 500 kbps
+                pLinkStatus->ChannelQuality = pAdapter->Mlme.ChannelQuality;
+                pLinkStatus->RxByteCount = pAdapter->RalinkCounters.ReceivedByteCount;
+                pLinkStatus->TxByteCount = pAdapter->RalinkCounters.TransmittedByteCount;
+        		pLinkStatus->CentralChannel = pAdapter->CommonCfg.CentralChannel;
+                wrq->u.data.length = sizeof(RT_802_11_LINK_STATUS);
+                Status = copy_to_user(wrq->u.data.pointer, pLinkStatus, wrq->u.data.length);
+                kfree(pLinkStatus);
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_LINK_STATUS\n"));
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_LINK_STATUS(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case OID_802_11_CONFIGURATION:
+            pConfiguration = (NDIS_802_11_CONFIGURATION *) kmalloc(sizeof(NDIS_802_11_CONFIGURATION), MEM_ALLOC_FLAG);
+            if (pConfiguration)
+            {
+                pConfiguration->Length = sizeof(NDIS_802_11_CONFIGURATION);
+                pConfiguration->BeaconPeriod = pAdapter->CommonCfg.BeaconPeriod;
+                pConfiguration->ATIMWindow = pAdapter->StaActive.AtimWin;
+                MAP_CHANNEL_ID_TO_KHZ(pAdapter->CommonCfg.Channel, pConfiguration->DSConfig);
+                wrq->u.data.length = sizeof(NDIS_802_11_CONFIGURATION);
+                Status = copy_to_user(wrq->u.data.pointer, pConfiguration, wrq->u.data.length);
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_CONFIGURATION(BeaconPeriod=%ld,AtimW=%ld,Channel=%d) \n",
+                                        pConfiguration->BeaconPeriod, pConfiguration->ATIMWindow, pAdapter->CommonCfg.Channel));
+				kfree(pConfiguration);
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_CONFIGURATION(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+		case RT_OID_802_11_SNR_0:
+			if ((pAdapter->StaCfg.LastSNR0 > 0))
+			{
+				ulInfo = ((0xeb	- pAdapter->StaCfg.LastSNR0) * 3) /	16 ;
+				wrq->u.data.length = sizeof(ulInfo);
+				Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_SNR_0(0x=%lx)\n", ulInfo));
+			}
+            else
+			    Status = -EFAULT;
+			break;
+		case RT_OID_802_11_SNR_1:
+			if ((pAdapter->Antenna.field.RxPath	> 1) &&
+                (pAdapter->StaCfg.LastSNR1 > 0))
+			{
+				ulInfo = ((0xeb	- pAdapter->StaCfg.LastSNR1) * 3) /	16 ;
+				wrq->u.data.length = sizeof(ulInfo);
+				Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE,("Query::RT_OID_802_11_SNR_1(0x=%lx)\n",ulInfo));
+			}
+			else
+				Status = -EFAULT;
+            DBGPRINT(RT_DEBUG_TRACE,("Query::RT_OID_802_11_SNR_1(pAdapter->StaCfg.LastSNR1=%d)\n",pAdapter->StaCfg.LastSNR1));
+			break;
+        case OID_802_11_RSSI_TRIGGER:
+            ulInfo = pAdapter->StaCfg.RssiSample.LastRssi0 - pAdapter->BbpRssiToDbmDelta;
+            wrq->u.data.length = sizeof(ulInfo);
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_RSSI_TRIGGER(=%ld)\n", ulInfo));
+            break;
+		case OID_802_11_RSSI:
+        case RT_OID_802_11_RSSI:
+			ulInfo = pAdapter->StaCfg.RssiSample.LastRssi0;
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+			break;
+		case RT_OID_802_11_RSSI_1:
+            ulInfo = pAdapter->StaCfg.RssiSample.LastRssi1;
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+			break;
+        case RT_OID_802_11_RSSI_2:
+            ulInfo = pAdapter->StaCfg.RssiSample.LastRssi2;
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+			break;
+        case OID_802_11_STATISTICS:
+            pStatistics = (NDIS_802_11_STATISTICS *) kmalloc(sizeof(NDIS_802_11_STATISTICS), MEM_ALLOC_FLAG);
+            if (pStatistics)
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_STATISTICS \n"));
+                // add the most up-to-date h/w raw counters into software counters
+			    NICUpdateRawCounters(pAdapter);
+
+                // Sanity check for calculation of sucessful count
+                if (pAdapter->WlanCounters.TransmittedFragmentCount.QuadPart < pAdapter->WlanCounters.RetryCount.QuadPart)
+                    pAdapter->WlanCounters.TransmittedFragmentCount.QuadPart = pAdapter->WlanCounters.RetryCount.QuadPart;
+
+                pStatistics->TransmittedFragmentCount.QuadPart = pAdapter->WlanCounters.TransmittedFragmentCount.QuadPart;
+                pStatistics->MulticastTransmittedFrameCount.QuadPart = pAdapter->WlanCounters.MulticastTransmittedFrameCount.QuadPart;
+                pStatistics->FailedCount.QuadPart = pAdapter->WlanCounters.FailedCount.QuadPart;
+                pStatistics->RetryCount.QuadPart = pAdapter->WlanCounters.RetryCount.QuadPart;
+                pStatistics->MultipleRetryCount.QuadPart = pAdapter->WlanCounters.MultipleRetryCount.QuadPart;
+                pStatistics->RTSSuccessCount.QuadPart = pAdapter->WlanCounters.RTSSuccessCount.QuadPart;
+                pStatistics->RTSFailureCount.QuadPart = pAdapter->WlanCounters.RTSFailureCount.QuadPart;
+                pStatistics->ACKFailureCount.QuadPart = pAdapter->WlanCounters.ACKFailureCount.QuadPart;
+                pStatistics->FrameDuplicateCount.QuadPart = pAdapter->WlanCounters.FrameDuplicateCount.QuadPart;
+                pStatistics->ReceivedFragmentCount.QuadPart = pAdapter->WlanCounters.ReceivedFragmentCount.QuadPart;
+                pStatistics->MulticastReceivedFrameCount.QuadPart = pAdapter->WlanCounters.MulticastReceivedFrameCount.QuadPart;
+#ifdef DBG
+                pStatistics->FCSErrorCount = pAdapter->RalinkCounters.RealFcsErrCount;
+#else
+                pStatistics->FCSErrorCount.QuadPart = pAdapter->WlanCounters.FCSErrorCount.QuadPart;
+                pStatistics->FrameDuplicateCount.u.LowPart = pAdapter->WlanCounters.FrameDuplicateCount.u.LowPart / 100;
+#endif
+                wrq->u.data.length = sizeof(NDIS_802_11_STATISTICS);
+                Status = copy_to_user(wrq->u.data.pointer, pStatistics, wrq->u.data.length);
+                kfree(pStatistics);
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_STATISTICS(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case OID_GEN_RCV_OK:
+            ulInfo = pAdapter->Counters8023.GoodReceives;
+            wrq->u.data.length = sizeof(ulInfo);
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+            break;
+        case OID_GEN_RCV_NO_BUFFER:
+            ulInfo = pAdapter->Counters8023.RxNoBuffer;
+            wrq->u.data.length = sizeof(ulInfo);
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+            break;
+        case RT_OID_802_11_PHY_MODE:
+            ulInfo = (ULONG)pAdapter->CommonCfg.PhyMode;
+            wrq->u.data.length = sizeof(ulInfo);
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_PHY_MODE (=%ld)\n", ulInfo));
+            break;
+        case RT_OID_802_11_STA_CONFIG:
+            pStaConfig = (RT_802_11_STA_CONFIG *) kmalloc(sizeof(RT_802_11_STA_CONFIG), MEM_ALLOC_FLAG);
+            if (pStaConfig)
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_STA_CONFIG\n"));
+                pStaConfig->EnableTxBurst = pAdapter->CommonCfg.bEnableTxBurst;
+                pStaConfig->EnableTurboRate = 0;
+                pStaConfig->UseBGProtection = pAdapter->CommonCfg.UseBGProtection;
+                pStaConfig->UseShortSlotTime = pAdapter->CommonCfg.bUseShortSlotTime;
+                //pStaConfig->AdhocMode = pAdapter->StaCfg.AdhocMode;
+                pStaConfig->HwRadioStatus = (pAdapter->StaCfg.bHwRadio == TRUE) ? 1 : 0;
+                pStaConfig->Rsv1 = 0;
+                pStaConfig->SystemErrorBitmap = pAdapter->SystemErrorBitmap;
+                wrq->u.data.length = sizeof(RT_802_11_STA_CONFIG);
+                Status = copy_to_user(wrq->u.data.pointer, pStaConfig, wrq->u.data.length);
+                kfree(pStaConfig);
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_STA_CONFIG(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case OID_802_11_RTS_THRESHOLD:
+            RtsThresh = pAdapter->CommonCfg.RtsThreshold;
+            wrq->u.data.length = sizeof(RtsThresh);
+            Status = copy_to_user(wrq->u.data.pointer, &RtsThresh, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_RTS_THRESHOLD(=%ld)\n", RtsThresh));
+            break;
+        case OID_802_11_FRAGMENTATION_THRESHOLD:
+            FragThresh = pAdapter->CommonCfg.FragmentThreshold;
+            if (pAdapter->CommonCfg.bUseZeroToDisableFragment == TRUE)
+                FragThresh = 0;
+            wrq->u.data.length = sizeof(FragThresh);
+            Status = copy_to_user(wrq->u.data.pointer, &FragThresh, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_FRAGMENTATION_THRESHOLD(=%ld)\n", FragThresh));
+            break;
+        case OID_802_11_POWER_MODE:
+            PowerMode = pAdapter->StaCfg.WindowsPowerMode;
+            wrq->u.data.length = sizeof(PowerMode);
+            Status = copy_to_user(wrq->u.data.pointer, &PowerMode, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_POWER_MODE(=%d)\n", PowerMode));
+            break;
+        case RT_OID_802_11_RADIO:
+            RadioState = (BOOLEAN) pAdapter->StaCfg.bSwRadio;
+            wrq->u.data.length = sizeof(RadioState);
+            Status = copy_to_user(wrq->u.data.pointer, &RadioState, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_RADIO (=%d)\n", RadioState));
+            break;
+        case OID_802_11_INFRASTRUCTURE_MODE:
+            if (pAdapter->StaCfg.BssType == BSS_ADHOC)
+                BssType = Ndis802_11IBSS;
+            else if (pAdapter->StaCfg.BssType == BSS_INFRA)
+                BssType = Ndis802_11Infrastructure;
+            else if (pAdapter->StaCfg.BssType == BSS_MONITOR)
+                BssType = Ndis802_11Monitor;
+            else
+                BssType = Ndis802_11AutoUnknown;
+
+            wrq->u.data.length = sizeof(BssType);
+            Status = copy_to_user(wrq->u.data.pointer, &BssType, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_INFRASTRUCTURE_MODE(=%d)\n", BssType));
+            break;
+        case RT_OID_802_11_PREAMBLE:
+            PreamType = pAdapter->CommonCfg.TxPreamble;
+            wrq->u.data.length = sizeof(PreamType);
+            Status = copy_to_user(wrq->u.data.pointer, &PreamType, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_PREAMBLE(=%d)\n", PreamType));
+            break;
+        case OID_802_11_AUTHENTICATION_MODE:
+            AuthMode = pAdapter->StaCfg.AuthMode;
+            wrq->u.data.length = sizeof(AuthMode);
+            Status = copy_to_user(wrq->u.data.pointer, &AuthMode, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_AUTHENTICATION_MODE(=%d)\n", AuthMode));
+            break;
+        case OID_802_11_WEP_STATUS:
+            WepStatus = pAdapter->StaCfg.WepStatus;
+            wrq->u.data.length = sizeof(WepStatus);
+            Status = copy_to_user(wrq->u.data.pointer, &WepStatus, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_WEP_STATUS(=%d)\n", WepStatus));
+            break;
+        case OID_802_11_TX_POWER_LEVEL:
+			wrq->u.data.length = sizeof(ULONG);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.TxPower, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_TX_POWER_LEVEL %x\n",pAdapter->CommonCfg.TxPower));
+			break;
+        case RT_OID_802_11_TX_POWER_LEVEL_1:
+            wrq->u.data.length = sizeof(ULONG);
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.TxPowerPercentage, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_TX_POWER_LEVEL_1 (=%ld)\n", pAdapter->CommonCfg.TxPowerPercentage));
+			break;
+        case OID_802_11_NETWORK_TYPES_SUPPORTED:
+			if ((pAdapter->RfIcType	== RFIC_2850) || (pAdapter->RfIcType ==	RFIC_2750))
+			{
+				NetworkTypeList[0] = 3;                 // NumberOfItems = 3
+				NetworkTypeList[1] = Ndis802_11DS;      // NetworkType[1] = 11b
+				NetworkTypeList[2] = Ndis802_11OFDM24;  // NetworkType[2] = 11g
+				NetworkTypeList[3] = Ndis802_11OFDM5;   // NetworkType[3] = 11a
+                wrq->u.data.length = 16;
+				Status = copy_to_user(wrq->u.data.pointer, &NetworkTypeList[0], wrq->u.data.length);
+			}
+			else
+			{
+				NetworkTypeList[0] = 2;                 // NumberOfItems = 2
+				NetworkTypeList[1] = Ndis802_11DS;      // NetworkType[1] = 11b
+				NetworkTypeList[2] = Ndis802_11OFDM24;  // NetworkType[2] = 11g
+			    wrq->u.data.length = 12;
+				Status = copy_to_user(wrq->u.data.pointer, &NetworkTypeList[0], wrq->u.data.length);
+			}
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_NETWORK_TYPES_SUPPORTED\n"));
+				break;
+	    case OID_802_11_NETWORK_TYPE_IN_USE:
+            wrq->u.data.length = sizeof(ULONG);
+			if (pAdapter->CommonCfg.PhyMode == PHY_11A)
+				ulInfo = Ndis802_11OFDM5;
+			else if ((pAdapter->CommonCfg.PhyMode == PHY_11BG_MIXED) || (pAdapter->CommonCfg.PhyMode == PHY_11G))
+				ulInfo = Ndis802_11OFDM24;
+			else
+				ulInfo = Ndis802_11DS;
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+			break;
+        case RT_OID_802_11_QUERY_LAST_RX_RATE:
+            ulInfo = (ULONG)pAdapter->LastRxRate;
+            wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_LAST_RX_RATE (=%ld)\n", ulInfo));
+			break;
+		case RT_OID_802_11_QUERY_LAST_TX_RATE:
+			//ulInfo = (ULONG)pAdapter->LastTxRate;
+			ulInfo = (ULONG)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.word;
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_LAST_TX_RATE (=%lx)\n", ulInfo));
+			break;
+        case RT_OID_802_11_QUERY_EEPROM_VERSION:
+            wrq->u.data.length = sizeof(ULONG);
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->EepromVersion, wrq->u.data.length);
+            break;
+        case RT_OID_802_11_QUERY_FIRMWARE_VERSION:
+            wrq->u.data.length = sizeof(ULONG);
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->FirmwareVersion, wrq->u.data.length);
+			break;
+	    case RT_OID_802_11_QUERY_NOISE_LEVEL:
+			wrq->u.data.length = sizeof(UCHAR);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->BbpWriteLatch[66], wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_NOISE_LEVEL (=%d)\n", pAdapter->BbpWriteLatch[66]));
+			break;
+	    case RT_OID_802_11_EXTRA_INFO:
+			wrq->u.data.length = sizeof(ULONG);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->ExtraInfo, wrq->u.data.length);
+	        DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_EXTRA_INFO (=%ld)\n", pAdapter->ExtraInfo));
+	        break;
+	    case RT_OID_WE_VERSION_COMPILED:
+	        wrq->u.data.length = sizeof(UINT);
+	        we_version_compiled = WIRELESS_EXT;
+	        Status = copy_to_user(wrq->u.data.pointer, &we_version_compiled, wrq->u.data.length);
+	        break;
+		case RT_OID_802_11_QUERY_APSD_SETTING:
+			apsd = (pAdapter->CommonCfg.bAPSDCapable | (pAdapter->CommonCfg.bAPSDAC_BE << 1) | (pAdapter->CommonCfg.bAPSDAC_BK << 2)
+				| (pAdapter->CommonCfg.bAPSDAC_VI << 3)	| (pAdapter->CommonCfg.bAPSDAC_VO << 4)	| (pAdapter->CommonCfg.MaxSPLength << 5));
+
+			wrq->u.data.length = sizeof(ULONG);
+			Status = copy_to_user(wrq->u.data.pointer, &apsd, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_APSD_SETTING (=0x%lx,APSDCap=%d,AC_BE=%d,AC_BK=%d,AC_VI=%d,AC_VO=%d,MAXSPLen=%d)\n",
+				apsd,pAdapter->CommonCfg.bAPSDCapable,pAdapter->CommonCfg.bAPSDAC_BE,pAdapter->CommonCfg.bAPSDAC_BK,pAdapter->CommonCfg.bAPSDAC_VI,pAdapter->CommonCfg.bAPSDAC_VO,pAdapter->CommonCfg.MaxSPLength));
+			break;
+		case RT_OID_802_11_QUERY_APSD_PSM:
+			wrq->u.data.length = sizeof(ULONG);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.bAPSDForcePowerSave, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_APSD_PSM (=%d)\n", pAdapter->CommonCfg.bAPSDForcePowerSave));
+			break;
+		case RT_OID_802_11_QUERY_WMM:
+			wrq->u.data.length = sizeof(BOOLEAN);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.bWmmCapable, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_WMM (=%d)\n",	pAdapter->CommonCfg.bWmmCapable));
+			break;
+#ifdef WPA_SUPPLICANT_SUPPORT
+        case RT_OID_NEW_DRIVER:
+            {
+                UCHAR enabled = 1;
+    	        wrq->u.data.length = sizeof(UCHAR);
+    	        Status = copy_to_user(wrq->u.data.pointer, &enabled, wrq->u.data.length);
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_NEW_DRIVER (=%d)\n", enabled));
+            }
+	        break;
+        case RT_OID_WPA_SUPPLICANT_SUPPORT:
+	        wrq->u.data.length = sizeof(UCHAR);
+	        Status = copy_to_user(wrq->u.data.pointer, &pAdapter->StaCfg.WpaSupplicantUP, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_WPA_SUPPLICANT_SUPPORT (=%d)\n", pAdapter->StaCfg.WpaSupplicantUP));
+	        break;
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+        case RT_OID_DRIVER_DEVICE_NAME:
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_DRIVER_DEVICE_NAME \n"));
+			wrq->u.data.length = 16;
+			if (copy_to_user(wrq->u.data.pointer, pAdapter->StaCfg.dev_name, wrq->u.data.length))
+			{
+				Status = -EFAULT;
+			}
+            break;
+        case RT_OID_802_11_QUERY_HT_PHYMODE:
+            pHTPhyMode = (OID_SET_HT_PHYMODE *) kmalloc(sizeof(OID_SET_HT_PHYMODE), MEM_ALLOC_FLAG);
+            if (pHTPhyMode)
+            {
+                pHTPhyMode->PhyMode = pAdapter->CommonCfg.PhyMode;
+    			pHTPhyMode->HtMode = (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE;
+    			pHTPhyMode->BW = (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.BW;
+    			pHTPhyMode->MCS= (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.MCS;
+    			pHTPhyMode->SHORTGI= (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.ShortGI;
+    			pHTPhyMode->STBC= (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.STBC;
+
+    			pHTPhyMode->ExtOffset = ((pAdapter->CommonCfg.CentralChannel < pAdapter->CommonCfg.Channel) ? (EXTCHA_BELOW) : (EXTCHA_ABOVE));
+                wrq->u.data.length = sizeof(OID_SET_HT_PHYMODE);
+                if (copy_to_user(wrq->u.data.pointer, pHTPhyMode, wrq->u.data.length))
+    			{
+    				Status = -EFAULT;
+    			}
+    			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_HT_PHYMODE (PhyMode = %d, MCS =%d, BW = %d, STBC = %d, ExtOffset=%d)\n",
+    				pHTPhyMode->HtMode, pHTPhyMode->MCS, pHTPhyMode->BW, pHTPhyMode->STBC, pHTPhyMode->ExtOffset));
+    			DBGPRINT(RT_DEBUG_TRACE, (" MlmeUpdateTxRates (.word = %x )\n", pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.word));
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_STA_CONFIG(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case RT_OID_802_11_COUNTRY_REGION:
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_COUNTRY_REGION \n"));
+			wrq->u.data.length = sizeof(ulInfo);
+            ulInfo = pAdapter->CommonCfg.CountryRegionForABand;
+            ulInfo = (ulInfo << 8)|(pAdapter->CommonCfg.CountryRegion);
+			if (copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length))
+            {
+				Status = -EFAULT;
+            }
+            break;
+        case RT_OID_802_11_QUERY_DAT_HT_PHYMODE:
+            pHTPhyMode = (OID_SET_HT_PHYMODE *) kmalloc(sizeof(OID_SET_HT_PHYMODE), MEM_ALLOC_FLAG);
+            if (pHTPhyMode)
+            {
+                pHTPhyMode->PhyMode = pAdapter->CommonCfg.PhyMode;
+    			pHTPhyMode->HtMode = (UCHAR)pAdapter->CommonCfg.RegTransmitSetting.field.HTMODE;
+    			pHTPhyMode->BW = (UCHAR)pAdapter->CommonCfg.RegTransmitSetting.field.BW;
+    			pHTPhyMode->MCS= (UCHAR)pAdapter->StaCfg.DesiredTransmitSetting.field.MCS;
+    			pHTPhyMode->SHORTGI= (UCHAR)pAdapter->CommonCfg.RegTransmitSetting.field.ShortGI;
+    			pHTPhyMode->STBC= (UCHAR)pAdapter->CommonCfg.RegTransmitSetting.field.STBC;
+
+                wrq->u.data.length = sizeof(OID_SET_HT_PHYMODE);
+                if (copy_to_user(wrq->u.data.pointer, pHTPhyMode, wrq->u.data.length))
+    			{
+    				Status = -EFAULT;
+    			}
+    			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_HT_PHYMODE (PhyMode = %d, MCS =%d, BW = %d, STBC = %d, ExtOffset=%d)\n",
+    				pHTPhyMode->HtMode, pHTPhyMode->MCS, pHTPhyMode->BW, pHTPhyMode->STBC, pHTPhyMode->ExtOffset));
+    			DBGPRINT(RT_DEBUG_TRACE, (" MlmeUpdateTxRates (.word = %x )\n", pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.word));
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_STA_CONFIG(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case RT_OID_QUERY_MULTIPLE_CARD_SUPPORT:
+			wrq->u.data.length = sizeof(UCHAR);
+            i = 0;
+#ifdef MULTIPLE_CARD_SUPPORT
+            i = 1;
+#endif // MULTIPLE_CARD_SUPPORT //
+			if (copy_to_user(wrq->u.data.pointer, &i, wrq->u.data.length))
+            {
+				Status = -EFAULT;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_QUERY_MULTIPLE_CARD_SUPPORT(=%d) \n", i));
+            break;
+#ifdef SNMP_SUPPORT
+		case RT_OID_802_11_MAC_ADDRESS:
+            wrq->u.data.length = MAC_ADDR_LEN;
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CurrentAddress, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_MANUFACTUREROUI:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_MANUFACTUREROUI \n"));
+			wrq->u.data.length = ManufacturerOUI_LEN;
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CurrentAddress, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_MANUFACTURERNAME:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_MANUFACTURERNAME \n"));
+			wrq->u.data.length = strlen(ManufacturerNAME);
+			Status = copy_to_user(wrq->u.data.pointer, ManufacturerNAME, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_RESOURCETYPEIDNAME:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_RESOURCETYPEIDNAME \n"));
+			wrq->u.data.length = strlen(ResourceTypeIdName);
+			Status = copy_to_user(wrq->u.data.pointer, ResourceTypeIdName, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_PRIVACYOPTIONIMPLEMENTED:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_PRIVACYOPTIONIMPLEMENTED \n"));
+			ulInfo = 1; // 1 is support wep else 2 is not support.
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_POWERMANAGEMENTMODE:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_POWERMANAGEMENTMODE \n"));
+			if (pAdapter->StaCfg.Psm == PSMP_ACTION)
+				ulInfo = 1; // 1 is power active else 2 is power save.
+			else
+				ulInfo = 2;
+
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+			break;
+
+		case OID_802_11_WEPDEFAULTKEYVALUE:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_WEPDEFAULTKEYVALUE \n"));
+			//KeyIdxValue.KeyIdx = pAd->PortCfg.MBSSID[pAd->IoctlIF].DefaultKeyId;
+			pKeyIdxValue = wrq->u.data.pointer;
+			DBGPRINT(RT_DEBUG_TRACE,("KeyIdxValue.KeyIdx = %d, \n",pKeyIdxValue->KeyIdx));
+			valueLen = pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen;
+			NdisMoveMemory(pKeyIdxValue->Value,
+						   &pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].Key,
+						   valueLen);
+			pKeyIdxValue->Value[valueLen]='\0';
+
+			wrq->u.data.length = sizeof(DefaultKeyIdxValue);
+
+			Status = copy_to_user(wrq->u.data.pointer, pKeyIdxValue, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE,("DefaultKeyId = %d, total len = %d, str len=%d, KeyValue= %02x %02x %02x %02x \n", pAdapter->StaCfg.DefaultKeyId, wrq->u.data.length, pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen,
+			pAdapter->SharedKey[BSS0][0].Key[0],
+			pAdapter->SharedKey[BSS0][1].Key[0],
+			pAdapter->SharedKey[BSS0][2].Key[0],
+			pAdapter->SharedKey[BSS0][3].Key[0]));
+			break;
+
+		case OID_802_11_WEPDEFAULTKEYID:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_WEPDEFAULTKEYID \n"));
+			wrq->u.data.length = sizeof(UCHAR);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->StaCfg.DefaultKeyId, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("DefaultKeyId =%d \n", pAdapter->StaCfg.DefaultKeyId));
+			break;
+
+		case RT_OID_802_11_WEPKEYMAPPINGLENGTH:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_WEPKEYMAPPINGLENGTH \n"));
+			wrq->u.data.length = sizeof(UCHAR);
+			Status = copy_to_user(wrq->u.data.pointer,
+									&pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen,
+									wrq->u.data.length);
+			break;
+
+		case OID_802_11_SHORTRETRYLIMIT:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_SHORTRETRYLIMIT \n"));
+			wrq->u.data.length = sizeof(ULONG);
+			RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+			ShortRetryLimit = tx_rty_cfg.field.ShortRtyLimit;
+			DBGPRINT(RT_DEBUG_TRACE, ("ShortRetryLimit =%ld,  tx_rty_cfg.field.ShortRetryLimit=%d\n", ShortRetryLimit, tx_rty_cfg.field.ShortRtyLimit));
+			Status = copy_to_user(wrq->u.data.pointer, &ShortRetryLimit, wrq->u.data.length);
+			break;
+
+		case OID_802_11_LONGRETRYLIMIT:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_LONGRETRYLIMIT \n"));
+			wrq->u.data.length = sizeof(ULONG);
+			RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+			LongRetryLimit = tx_rty_cfg.field.LongRtyLimit;
+			DBGPRINT(RT_DEBUG_TRACE, ("LongRetryLimit =%ld,  tx_rty_cfg.field.LongRtyLimit=%d\n", LongRetryLimit, tx_rty_cfg.field.LongRtyLimit));
+			Status = copy_to_user(wrq->u.data.pointer, &LongRetryLimit, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_PRODUCTID:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_PRODUCTID \n"));
+
+#ifdef RT2860
+			{
+
+				USHORT  device_id;
+				if (((POS_COOKIE)pAdapter->OS_Cookie)->pci_dev != NULL)
+			    	pci_read_config_word(((POS_COOKIE)pAdapter->OS_Cookie)->pci_dev, PCI_DEVICE_ID, &device_id);
+				else
+					DBGPRINT(RT_DEBUG_TRACE, (" pci_dev = NULL\n"));
+				sprintf(tmp, "%04x %04x\n", NIC_PCI_VENDOR_ID, device_id);
+			}
+#endif // RT2860 //
+			wrq->u.data.length = strlen(tmp);
+			Status = copy_to_user(wrq->u.data.pointer, tmp, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_MANUFACTUREID:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_MANUFACTUREID \n"));
+			wrq->u.data.length = strlen(ManufacturerNAME);
+			Status = copy_to_user(wrq->u.data.pointer, ManufacturerNAME, wrq->u.data.length);
+			break;
+
+		case OID_802_11_CURRENTCHANNEL:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_CURRENTCHANNEL \n"));
+			wrq->u.data.length = sizeof(UCHAR);
+			DBGPRINT(RT_DEBUG_TRACE, ("sizeof UCHAR=%d, channel=%d \n", sizeof(UCHAR), pAdapter->CommonCfg.Channel));
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.Channel, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+			break;
+#endif //SNMP_SUPPORT
+
+		case OID_802_11_BUILD_CHANNEL_EX:
+			{
+				UCHAR value;
+				DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_BUILD_CHANNEL_EX \n"));
+				wrq->u.data.length = sizeof(UCHAR);
+#ifdef EXT_BUILD_CHANNEL_LIST
+				DBGPRINT(RT_DEBUG_TRACE, ("Support EXT_BUILD_CHANNEL_LIST.\n"));
+				value = 1;
+#else
+				DBGPRINT(RT_DEBUG_TRACE, ("Doesn't support EXT_BUILD_CHANNEL_LIST.\n"));
+				value = 0;
+#endif // EXT_BUILD_CHANNEL_LIST //
+				Status = copy_to_user(wrq->u.data.pointer, &value, 1);
+				DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+			}
+			break;
+
+		case OID_802_11_GET_CH_LIST:
+			{
+				PRT_CHANNEL_LIST_INFO pChListBuf;
+
+				DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_GET_CH_LIST \n"));
+				if (pAdapter->ChannelListNum == 0)
+				{
+					wrq->u.data.length = 0;
+					break;
+				}
+
+				pChListBuf = (RT_CHANNEL_LIST_INFO *) kmalloc(sizeof(RT_CHANNEL_LIST_INFO), MEM_ALLOC_FLAG);
+				if (pChListBuf == NULL)
+				{
+					wrq->u.data.length = 0;
+					break;
+				}
+
+				pChListBuf->ChannelListNum = pAdapter->ChannelListNum;
+				for (i = 0; i < pChListBuf->ChannelListNum; i++)
+					pChListBuf->ChannelList[i] = pAdapter->ChannelList[i].Channel;
+
+				wrq->u.data.length = sizeof(RT_CHANNEL_LIST_INFO);
+				Status = copy_to_user(wrq->u.data.pointer, pChListBuf, sizeof(RT_CHANNEL_LIST_INFO));
+				DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+
+				if (pChListBuf)
+					kfree(pChListBuf);
+			}
+			break;
+
+		case OID_802_11_GET_COUNTRY_CODE:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_GET_COUNTRY_CODE \n"));
+			wrq->u.data.length = 2;
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.CountryCode, 2);
+			DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+			break;
+
+		case OID_802_11_GET_CHANNEL_GEOGRAPHY:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_GET_CHANNEL_GEOGRAPHY \n"));
+			wrq->u.data.length = 1;
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.Geography, 1);
+			DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+			break;
+
+
+#ifdef QOS_DLS_SUPPORT
+		case RT_OID_802_11_QUERY_DLS:
+			wrq->u.data.length = sizeof(BOOLEAN);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.bDLSCapable, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_DLS(=%d)\n", pAdapter->CommonCfg.bDLSCapable));
+			break;
+
+		case RT_OID_802_11_QUERY_DLS_PARAM:
+			{
+				PRT_802_11_DLS_INFO	pDlsInfo = kmalloc(sizeof(RT_802_11_DLS_INFO), GFP_ATOMIC);
+				if (pDlsInfo == NULL)
+					break;
+
+				for (i=0; i<MAX_NUM_OF_DLS_ENTRY; i++)
+				{
+					RTMPMoveMemory(&pDlsInfo->Entry[i], &pAdapter->StaCfg.DLSEntry[i], sizeof(RT_802_11_DLS_UI));
+				}
+
+				pDlsInfo->num = MAX_NUM_OF_DLS_ENTRY;
+				wrq->u.data.length = sizeof(RT_802_11_DLS_INFO);
+				Status = copy_to_user(wrq->u.data.pointer, pDlsInfo, wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_DLS_PARAM\n"));
+
+				if (pDlsInfo)
+					kfree(pDlsInfo);
+			}
+			break;
+#endif // QOS_DLS_SUPPORT //
+        default:
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::unknown IOCTL's subcmd = 0x%08x\n", cmd));
+            Status = -EOPNOTSUPP;
+            break;
+    }
+    return Status;
+}
+
+INT rt28xx_sta_ioctl(
+	IN	struct net_device	*net_dev,
+	IN	OUT	struct ifreq	*rq,
+	IN	INT					cmd)
+{
+	POS_COOKIE			pObj;
+	VIRTUAL_ADAPTER		*pVirtualAd = NULL;
+	RTMP_ADAPTER        *pAd = NULL;
+	struct iwreq        *wrq = (struct iwreq *) rq;
+	BOOLEAN				StateMachineTouched = FALSE;
+	INT					Status = NDIS_STATUS_SUCCESS;
+	USHORT				subcmd;
+
+	if (net_dev->priv_flags == INT_MAIN)
+	{
+		pAd = net_dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = net_dev->ml_priv;
+		pAd = pVirtualAd->RtmpDev->ml_priv;
+	}
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	if (pAd == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+    //check if the interface is down
+    if(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+	    if (wrq->u.data.pointer == NULL)
+	    {
+		    return Status;
+	    }
+
+	    if (strstr(wrq->u.data.pointer, "OpMode") == NULL)
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+        {
+            DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+		    return -ENETDOWN;
+        }
+    }
+
+	{	// determine this ioctl command is comming from which interface.
+		pObj->ioctl_if_type = INT_MAIN;
+		pObj->ioctl_if = MAIN_MBSSID;
+	}
+
+	switch(cmd)
+	{
+#ifdef RALINK_ATE
+#ifdef RALINK_28xx_QA
+		case RTPRIV_IOCTL_ATE:
+			{
+				RtmpDoAte(pAd, wrq);
+			}
+			break;
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+        case SIOCGIFHWADDR:
+			DBGPRINT(RT_DEBUG_TRACE, ("IOCTL::SIOCGIFHWADDR\n"));
+			memcpy(wrq->u.name, pAd->CurrentAddress, ETH_ALEN);
+			break;
+		case SIOCGIWNAME:
+        {
+        	char *name=&wrq->u.name[0];
+        	rt_ioctl_giwname(net_dev, NULL, name, NULL);
+			break;
+		}
+		case SIOCGIWESSID:  //Get ESSID
+        {
+        	struct iw_point *essid=&wrq->u.essid;
+        	rt_ioctl_giwessid(net_dev, NULL, essid, essid->pointer);
+			break;
+		}
+		case SIOCSIWESSID:  //Set ESSID
+        {
+        	struct iw_point	*essid=&wrq->u.essid;
+        	rt_ioctl_siwessid(net_dev, NULL, essid, essid->pointer);
+			break;
+		}
+		case SIOCSIWNWID:   // set network id (the cell)
+		case SIOCGIWNWID:   // get network id
+			Status = -EOPNOTSUPP;
+			break;
+		case SIOCSIWFREQ:   //set channel/frequency (Hz)
+        {
+        	struct iw_freq *freq=&wrq->u.freq;
+        	rt_ioctl_siwfreq(net_dev, NULL, freq, NULL);
+			break;
+		}
+		case SIOCGIWFREQ:   // get channel/frequency (Hz)
+        {
+        	struct iw_freq *freq=&wrq->u.freq;
+        	rt_ioctl_giwfreq(net_dev, NULL, freq, NULL);
+			break;
+		}
+		case SIOCSIWNICKN: //set node name/nickname
+        {
+        	struct iw_point *data=&wrq->u.data;
+        	rt_ioctl_siwnickn(net_dev, NULL, data, NULL);
+			break;
+		}
+		case SIOCGIWNICKN: //get node name/nickname
+        {
+        	struct iw_point *data=&wrq->u.data;
+        	rt_ioctl_giwnickn(net_dev, NULL, data, NULL);
+			break;
+		}
+		case SIOCGIWRATE:   //get default bit rate (bps)
+		    rt_ioctl_giwrate(net_dev, NULL, &wrq->u, NULL);
+            break;
+	    case SIOCSIWRATE:  //set default bit rate (bps)
+	        rt_ioctl_siwrate(net_dev, NULL, &wrq->u, NULL);
+            break;
+        case SIOCGIWRTS:  // get RTS/CTS threshold (bytes)
+        {
+        	struct iw_param *rts=&wrq->u.rts;
+        	rt_ioctl_giwrts(net_dev, NULL, rts, NULL);
+			break;
+		}
+        case SIOCSIWRTS:  //set RTS/CTS threshold (bytes)
+        {
+        	struct iw_param *rts=&wrq->u.rts;
+        	rt_ioctl_siwrts(net_dev, NULL, rts, NULL);
+			break;
+		}
+        case SIOCGIWFRAG:  //get fragmentation thr (bytes)
+        {
+        	struct iw_param *frag=&wrq->u.frag;
+        	rt_ioctl_giwfrag(net_dev, NULL, frag, NULL);
+			break;
+		}
+        case SIOCSIWFRAG:  //set fragmentation thr (bytes)
+        {
+        	struct iw_param *frag=&wrq->u.frag;
+        	rt_ioctl_siwfrag(net_dev, NULL, frag, NULL);
+			break;
+		}
+        case SIOCGIWENCODE:  //get encoding token & mode
+        {
+        	struct iw_point *erq=&wrq->u.encoding;
+        	if(erq->pointer)
+        		rt_ioctl_giwencode(net_dev, NULL, erq, erq->pointer);
+			break;
+		}
+        case SIOCSIWENCODE:  //set encoding token & mode
+        {
+        	struct iw_point *erq=&wrq->u.encoding;
+        	if(erq->pointer)
+        		rt_ioctl_siwencode(net_dev, NULL, erq, erq->pointer);
+			break;
+		}
+		case SIOCGIWAP:     //get access point MAC addresses
+        {
+        	struct sockaddr *ap_addr=&wrq->u.ap_addr;
+        	rt_ioctl_giwap(net_dev, NULL, ap_addr, ap_addr->sa_data);
+			break;
+		}
+	    case SIOCSIWAP:  //set access point MAC addresses
+        {
+        	struct sockaddr *ap_addr=&wrq->u.ap_addr;
+        	rt_ioctl_siwap(net_dev, NULL, ap_addr, ap_addr->sa_data);
+			break;
+		}
+		case SIOCGIWMODE:   //get operation mode
+        {
+        	__u32 *mode=&wrq->u.mode;
+        	rt_ioctl_giwmode(net_dev, NULL, mode, NULL);
+			break;
+		}
+		case SIOCSIWMODE:   //set operation mode
+        {
+        	__u32 *mode=&wrq->u.mode;
+        	rt_ioctl_siwmode(net_dev, NULL, mode, NULL);
+			break;
+		}
+		case SIOCGIWSENS:   //get sensitivity (dBm)
+		case SIOCSIWSENS:	//set sensitivity (dBm)
+		case SIOCGIWPOWER:  //get Power Management settings
+		case SIOCSIWPOWER:  //set Power Management settings
+		case SIOCGIWTXPOW:  //get transmit power (dBm)
+		case SIOCSIWTXPOW:  //set transmit power (dBm)
+		case SIOCGIWRANGE:	//Get range of parameters
+		case SIOCGIWRETRY:	//get retry limits and lifetime
+		case SIOCSIWRETRY:	//set retry limits and lifetime
+			Status = -EOPNOTSUPP;
+			break;
+		case RT_PRIV_IOCTL:
+			subcmd = wrq->u.data.flags;
+			if( subcmd & OID_GET_SET_TOGGLE)
+				Status = RTMPSetInformation(pAd, rq, subcmd);
+			else
+				Status = RTMPQueryInformation(pAd, rq, subcmd);
+			break;
+		case SIOCGIWPRIV:
+			if (wrq->u.data.pointer)
+			{
+				if ( access_ok(VERIFY_WRITE, wrq->u.data.pointer, sizeof(privtab)) != TRUE)
+					break;
+				wrq->u.data.length = sizeof(privtab) / sizeof(privtab[0]);
+				if (copy_to_user(wrq->u.data.pointer, privtab, sizeof(privtab)))
+					Status = -EFAULT;
+			}
+			break;
+		case RTPRIV_IOCTL_SET:
+			if(access_ok(VERIFY_READ, wrq->u.data.pointer, wrq->u.data.length) != TRUE)
+				break;
+			rt_ioctl_setparam(net_dev, NULL, NULL, wrq->u.data.pointer);
+			break;
+		case RTPRIV_IOCTL_GSITESURVEY:
+			RTMPIoctlGetSiteSurvey(pAd, wrq);
+		    break;
+#ifdef DBG
+		case RTPRIV_IOCTL_MAC:
+			RTMPIoctlMAC(pAd, wrq);
+			break;
+		case RTPRIV_IOCTL_E2P:
+			RTMPIoctlE2PROM(pAd, wrq);
+			break;
+#endif // DBG //
+        case SIOCETHTOOL:
+                break;
+		default:
+			DBGPRINT(RT_DEBUG_ERROR, ("IOCTL::unknown IOCTL's cmd = 0x%08x\n", cmd));
+			Status = -EOPNOTSUPP;
+			break;
+	}
+
+    if(StateMachineTouched) // Upper layer sent a MLME-related operations
+    	RT28XX_MLME_HANDLER(pAd);
+
+	return Status;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set SSID
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_SSID_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    NDIS_802_11_SSID                    Ssid, *pSsid=NULL;
+    BOOLEAN                             StateMachineTouched = FALSE;
+    int                                 success = TRUE;
+
+    if( strlen(arg) <= MAX_LEN_OF_SSID)
+    {
+        NdisZeroMemory(&Ssid, sizeof(NDIS_802_11_SSID));
+        if (strlen(arg) != 0)
+        {
+            NdisMoveMemory(Ssid.Ssid, arg, strlen(arg));
+            Ssid.SsidLength = strlen(arg);
+        }
+        else   //ANY ssid
+        {
+            Ssid.SsidLength = 0;
+		    memcpy(Ssid.Ssid, "", 0);
+			pAdapter->StaCfg.BssType = BSS_INFRA;
+			pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+	        pAdapter->StaCfg.WepStatus  = Ndis802_11EncryptionDisabled;
+		}
+        pSsid = &Ssid;
+
+        if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+        {
+            RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+            DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+        }
+
+        pAdapter->MlmeAux.CurrReqIsFromNdis = TRUE;
+        pAdapter->StaCfg.bScanReqIsFromWebUI = FALSE;
+		pAdapter->bConfigChanged = TRUE;
+
+        MlmeEnqueue(pAdapter,
+                    MLME_CNTL_STATE_MACHINE,
+                    OID_802_11_SSID,
+                    sizeof(NDIS_802_11_SSID),
+                    (VOID *)pSsid);
+
+        StateMachineTouched = TRUE;
+        DBGPRINT(RT_DEBUG_TRACE, ("Set_SSID_Proc::(Len=%d,Ssid=%s)\n", Ssid.SsidLength, Ssid.Ssid));
+    }
+    else
+        success = FALSE;
+
+    if (StateMachineTouched) // Upper layer sent a MLME-related operations
+    	RT28XX_MLME_HANDLER(pAdapter);
+
+    return success;
+}
+
+#ifdef WMM_SUPPORT
+/*
+    ==========================================================================
+    Description:
+        Set WmmCapable Enable or Disable
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_WmmCapable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	BOOLEAN	bWmmCapable;
+
+	bWmmCapable = simple_strtol(arg, 0, 10);
+
+	if ((bWmmCapable == 1)
+		)
+		pAd->CommonCfg.bWmmCapable = TRUE;
+	else if (bWmmCapable == 0)
+		pAd->CommonCfg.bWmmCapable = FALSE;
+	else
+		return FALSE;  //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_WmmCapable_Proc::(bWmmCapable=%d)\n",
+		pAd->CommonCfg.bWmmCapable));
+
+	return TRUE;
+}
+#endif // WMM_SUPPORT //
+
+/*
+    ==========================================================================
+    Description:
+        Set Network Type(Infrastructure/Adhoc mode)
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_NetworkType_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    UINT32	Value = 0;
+
+    if (strcmp(arg, "Adhoc") == 0)
+	{
+		if (pAdapter->StaCfg.BssType != BSS_ADHOC)
+		{
+			// Config has changed
+			pAdapter->bConfigChanged = TRUE;
+            if (MONITOR_ON(pAdapter))
+            {
+                RTMP_IO_WRITE32(pAdapter, RX_FILTR_CFG, STANORMAL);
+                RTMP_IO_READ32(pAdapter, MAC_SYS_CTRL, &Value);
+				Value &= (~0x80);
+				RTMP_IO_WRITE32(pAdapter, MAC_SYS_CTRL, Value);
+                OPSTATUS_CLEAR_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED);
+                pAdapter->StaCfg.bAutoReconnect = TRUE;
+                LinkDown(pAdapter, FALSE);
+            }
+			if (INFRA_ON(pAdapter))
+			{
+				//BOOLEAN Cancelled;
+				// Set the AutoReconnectSsid to prevent it reconnect to old SSID
+				// Since calling this indicate user don't want to connect to that SSID anymore.
+				pAdapter->MlmeAux.AutoReconnectSsidLen= 32;
+				NdisZeroMemory(pAdapter->MlmeAux.AutoReconnectSsid, pAdapter->MlmeAux.AutoReconnectSsidLen);
+
+				LinkDown(pAdapter, FALSE);
+
+				DBGPRINT(RT_DEBUG_TRACE, ("NDIS_STATUS_MEDIA_DISCONNECT Event BB!\n"));
+			}
+		}
+		pAdapter->StaCfg.BssType = BSS_ADHOC;
+        pAdapter->net_dev->type = pAdapter->StaCfg.OriDevType;
+		DBGPRINT(RT_DEBUG_TRACE, ("===>Set_NetworkType_Proc::(AD-HOC)\n"));
+	}
+    else if (strcmp(arg, "Infra") == 0)
+	{
+		if (pAdapter->StaCfg.BssType != BSS_INFRA)
+		{
+			// Config has changed
+			pAdapter->bConfigChanged = TRUE;
+            if (MONITOR_ON(pAdapter))
+            {
+                RTMP_IO_WRITE32(pAdapter, RX_FILTR_CFG, STANORMAL);
+                RTMP_IO_READ32(pAdapter, MAC_SYS_CTRL, &Value);
+				Value &= (~0x80);
+				RTMP_IO_WRITE32(pAdapter, MAC_SYS_CTRL, Value);
+                OPSTATUS_CLEAR_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED);
+                pAdapter->StaCfg.bAutoReconnect = TRUE;
+                LinkDown(pAdapter, FALSE);
+            }
+			if (ADHOC_ON(pAdapter))
+			{
+				// Set the AutoReconnectSsid to prevent it reconnect to old SSID
+				// Since calling this indicate user don't want to connect to that SSID anymore.
+				pAdapter->MlmeAux.AutoReconnectSsidLen= 32;
+				NdisZeroMemory(pAdapter->MlmeAux.AutoReconnectSsid, pAdapter->MlmeAux.AutoReconnectSsidLen);
+
+				LinkDown(pAdapter, FALSE);
+			}
+		}
+		pAdapter->StaCfg.BssType = BSS_INFRA;
+        pAdapter->net_dev->type = pAdapter->StaCfg.OriDevType;
+		DBGPRINT(RT_DEBUG_TRACE, ("===>Set_NetworkType_Proc::(INFRA)\n"));
+
+        pAdapter->StaCfg.BssType = BSS_INFRA;
+	}
+    else if (strcmp(arg, "Monitor") == 0)
+    {
+		UCHAR	bbpValue = 0;
+		BCN_TIME_CFG_STRUC csr;
+		OPSTATUS_CLEAR_FLAG(pAdapter, fOP_STATUS_INFRA_ON);
+        OPSTATUS_CLEAR_FLAG(pAdapter, fOP_STATUS_ADHOC_ON);
+		OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED);
+		// disable all periodic state machine
+		pAdapter->StaCfg.bAutoReconnect = FALSE;
+		// reset all mlme state machine
+		RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+		DBGPRINT(RT_DEBUG_TRACE, ("fOP_STATUS_MEDIA_STATE_CONNECTED \n"));
+        if (pAdapter->CommonCfg.CentralChannel == 0)
+        {
+#ifdef DOT11_N_SUPPORT
+            if (pAdapter->CommonCfg.PhyMode == PHY_11AN_MIXED)
+                pAdapter->CommonCfg.CentralChannel = 36;
+            else
+#endif // DOT11_N_SUPPORT //
+                pAdapter->CommonCfg.CentralChannel = 6;
+        }
+#ifdef DOT11_N_SUPPORT
+        else
+            N_ChannelCheck(pAdapter);
+#endif // DOT11_N_SUPPORT //
+
+#ifdef DOT11_N_SUPPORT
+	if (pAdapter->CommonCfg.PhyMode >= PHY_11ABGN_MIXED &&
+            pAdapter->CommonCfg.RegTransmitSetting.field.BW == BW_40 &&
+            pAdapter->CommonCfg.RegTransmitSetting.field.EXTCHA == EXTCHA_ABOVE)
+		{
+			// 40MHz ,control channel at lower
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R4, &bbpValue);
+			bbpValue &= (~0x18);
+			bbpValue |= 0x10;
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R4, bbpValue);
+			pAdapter->CommonCfg.BBPCurrentBW = BW_40;
+			//  RX : control channel at lower
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R3, &bbpValue);
+			bbpValue &= (~0x20);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R3, bbpValue);
+
+			RTMP_IO_READ32(pAdapter, TX_BAND_CFG, &Value);
+			Value &= 0xfffffffe;
+			RTMP_IO_WRITE32(pAdapter, TX_BAND_CFG, Value);
+			pAdapter->CommonCfg.CentralChannel = pAdapter->CommonCfg.Channel + 2;
+            AsicSwitchChannel(pAdapter, pAdapter->CommonCfg.CentralChannel, FALSE);
+		    AsicLockChannel(pAdapter, pAdapter->CommonCfg.CentralChannel);
+            DBGPRINT(RT_DEBUG_TRACE, ("BW_40 ,control_channel(%d), CentralChannel(%d) \n",
+                                       pAdapter->CommonCfg.Channel,
+                                       pAdapter->CommonCfg.CentralChannel));
+		}
+		else if (pAdapter->CommonCfg.PhyMode >= PHY_11ABGN_MIXED &&
+                 pAdapter->CommonCfg.RegTransmitSetting.field.BW == BW_40 &&
+                 pAdapter->CommonCfg.RegTransmitSetting.field.EXTCHA == EXTCHA_BELOW)
+		{
+			// 40MHz ,control channel at upper
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R4, &bbpValue);
+			bbpValue &= (~0x18);
+			bbpValue |= 0x10;
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R4, bbpValue);
+			pAdapter->CommonCfg.BBPCurrentBW = BW_40;
+			RTMP_IO_READ32(pAdapter, TX_BAND_CFG, &Value);
+			Value |= 0x1;
+			RTMP_IO_WRITE32(pAdapter, TX_BAND_CFG, Value);
+
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R3, &bbpValue);
+			bbpValue |= (0x20);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R3, bbpValue);
+			pAdapter->CommonCfg.CentralChannel = pAdapter->CommonCfg.Channel - 2;
+            AsicSwitchChannel(pAdapter, pAdapter->CommonCfg.CentralChannel, FALSE);
+		    AsicLockChannel(pAdapter, pAdapter->CommonCfg.CentralChannel);
+            DBGPRINT(RT_DEBUG_TRACE, ("BW_40 ,control_channel(%d), CentralChannel(%d) \n",
+                                       pAdapter->CommonCfg.Channel,
+                                       pAdapter->CommonCfg.CentralChannel));
+		}
+		else
+#endif // DOT11_N_SUPPORT //
+		{
+			// 20MHz
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R4, &bbpValue);
+			bbpValue &= (~0x18);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R4, bbpValue);
+			pAdapter->CommonCfg.BBPCurrentBW = BW_20;
+			AsicSwitchChannel(pAdapter, pAdapter->CommonCfg.Channel, FALSE);
+			AsicLockChannel(pAdapter, pAdapter->CommonCfg.Channel);
+			DBGPRINT(RT_DEBUG_TRACE, ("BW_20, Channel(%d)\n", pAdapter->CommonCfg.Channel));
+		}
+		// Enable Rx with promiscuous reception
+		RTMP_IO_WRITE32(pAdapter, RX_FILTR_CFG, 0x3);
+		// ASIC supporsts sniffer function with replacing RSSI with timestamp.
+		//RTMP_IO_READ32(pAdapter, MAC_SYS_CTRL, &Value);
+		//Value |= (0x80);
+		//RTMP_IO_WRITE32(pAdapter, MAC_SYS_CTRL, Value);
+		// disable sync
+		RTMP_IO_READ32(pAdapter, BCN_TIME_CFG, &csr.word);
+		csr.field.bBeaconGen = 0;
+		csr.field.bTBTTEnable = 0;
+		csr.field.TsfSyncMode = 0;
+		RTMP_IO_WRITE32(pAdapter, BCN_TIME_CFG, csr.word);
+
+		pAdapter->StaCfg.BssType = BSS_MONITOR;
+        pAdapter->net_dev->type = ARPHRD_IEEE80211_PRISM; //ARPHRD_IEEE80211; // IEEE80211
+		DBGPRINT(RT_DEBUG_TRACE, ("===>Set_NetworkType_Proc::(MONITOR)\n"));
+    }
+
+    // Reset Ralink supplicant to not use, it will be set to start when UI set PMK key
+    pAdapter->StaCfg.WpaState = SS_NOTUSE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_NetworkType_Proc::(NetworkType=%d)\n", pAdapter->StaCfg.BssType));
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Authentication mode
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_AuthMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    if ((strcmp(arg, "WEPAUTO") == 0) || (strcmp(arg, "wepauto") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeAutoSwitch;
+    else if ((strcmp(arg, "OPEN") == 0) || (strcmp(arg, "open") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+    else if ((strcmp(arg, "SHARED") == 0) || (strcmp(arg, "shared") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeShared;
+    else if ((strcmp(arg, "WPAPSK") == 0) || (strcmp(arg, "wpapsk") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPAPSK;
+    else if ((strcmp(arg, "WPANONE") == 0) || (strcmp(arg, "wpanone") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPANone;
+    else if ((strcmp(arg, "WPA2PSK") == 0) || (strcmp(arg, "wpa2psk") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA2PSK;
+#ifdef WPA_SUPPLICANT_SUPPORT
+    else if ((strcmp(arg, "WPA") == 0) || (strcmp(arg, "wpa") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA;
+    else if ((strcmp(arg, "WPA2") == 0) || (strcmp(arg, "wpa2") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA2;
+#endif // WPA_SUPPLICANT_SUPPORT //
+    else
+        return FALSE;
+
+    pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_AuthMode_Proc::(AuthMode=%d)\n", pAdapter->StaCfg.AuthMode));
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Encryption Type
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_EncrypType_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    if ((strcmp(arg, "NONE") == 0) || (strcmp(arg, "none") == 0))
+    {
+        if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+            return TRUE;    // do nothing
+
+        pAdapter->StaCfg.WepStatus     = Ndis802_11WEPDisabled;
+        pAdapter->StaCfg.PairCipher    = Ndis802_11WEPDisabled;
+	    pAdapter->StaCfg.GroupCipher   = Ndis802_11WEPDisabled;
+    }
+    else if ((strcmp(arg, "WEP") == 0) || (strcmp(arg, "wep") == 0))
+    {
+        if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+            return TRUE;    // do nothing
+
+        pAdapter->StaCfg.WepStatus     = Ndis802_11WEPEnabled;
+        pAdapter->StaCfg.PairCipher    = Ndis802_11WEPEnabled;
+	    pAdapter->StaCfg.GroupCipher   = Ndis802_11WEPEnabled;
+    }
+    else if ((strcmp(arg, "TKIP") == 0) || (strcmp(arg, "tkip") == 0))
+    {
+        if (pAdapter->StaCfg.AuthMode < Ndis802_11AuthModeWPA)
+            return TRUE;    // do nothing
+
+        pAdapter->StaCfg.WepStatus     = Ndis802_11Encryption2Enabled;
+        pAdapter->StaCfg.PairCipher    = Ndis802_11Encryption2Enabled;
+	    pAdapter->StaCfg.GroupCipher   = Ndis802_11Encryption2Enabled;
+    }
+    else if ((strcmp(arg, "AES") == 0) || (strcmp(arg, "aes") == 0))
+    {
+        if (pAdapter->StaCfg.AuthMode < Ndis802_11AuthModeWPA)
+            return TRUE;    // do nothing
+
+        pAdapter->StaCfg.WepStatus     = Ndis802_11Encryption3Enabled;
+        pAdapter->StaCfg.PairCipher    = Ndis802_11Encryption3Enabled;
+	    pAdapter->StaCfg.GroupCipher   = Ndis802_11Encryption3Enabled;
+    }
+    else
+        return FALSE;
+
+    pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_EncrypType_Proc::(EncrypType=%d)\n", pAdapter->StaCfg.WepStatus));
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Default Key ID
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_DefaultKeyID_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    ULONG                               KeyIdx;
+
+    KeyIdx = simple_strtol(arg, 0, 10);
+    if((KeyIdx >= 1 ) && (KeyIdx <= 4))
+        pAdapter->StaCfg.DefaultKeyId = (UCHAR) (KeyIdx - 1 );
+    else
+        return FALSE;  //Invalid argument
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_DefaultKeyID_Proc::(DefaultKeyID=%d)\n", pAdapter->StaCfg.DefaultKeyId));
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set WEP KEY1
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Key1_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    int                                 KeyLen;
+    int                                 i;
+    UCHAR                               CipherAlg=CIPHER_WEP64;
+
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        return TRUE;    // do nothing
+
+    KeyLen = strlen(arg);
+
+    switch (KeyLen)
+    {
+        case 5: //wep 40 Ascii type
+            pAdapter->SharedKey[BSS0][0].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][0].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::(Key1=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 10: //wep 40 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][0].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][0].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::(Key1=%s and type=%s)\n", arg, "Hex"));
+            break;
+        case 13: //wep 104 Ascii type
+            pAdapter->SharedKey[BSS0][0].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][0].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::(Key1=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 26: //wep 104 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][0].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][0].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::(Key1=%s and type=%s)\n", arg, "Hex"));
+            break;
+        default: //Invalid argument
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::Invalid argument (=%s)\n", arg));
+            return FALSE;
+    }
+
+    pAdapter->SharedKey[BSS0][0].CipherAlg = CipherAlg;
+
+    // Set keys (into ASIC)
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        ;   // not support
+    else    // Old WEP stuff
+    {
+        AsicAddSharedKeyEntry(pAdapter,
+                              0,
+                              0,
+                              pAdapter->SharedKey[BSS0][0].CipherAlg,
+                              pAdapter->SharedKey[BSS0][0].Key,
+                              NULL,
+                              NULL);
+    }
+
+    return TRUE;
+}
+/*
+    ==========================================================================
+
+    Description:
+        Set WEP KEY2
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Key2_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    int                                 KeyLen;
+    int                                 i;
+    UCHAR                               CipherAlg=CIPHER_WEP64;
+
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        return TRUE;    // do nothing
+
+    KeyLen = strlen(arg);
+
+    switch (KeyLen)
+    {
+        case 5: //wep 40 Ascii type
+            pAdapter->SharedKey[BSS0][1].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][1].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::(Key2=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 10: //wep 40 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][1].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][1].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::(Key2=%s and type=%s)\n", arg, "Hex"));
+            break;
+        case 13: //wep 104 Ascii type
+            pAdapter->SharedKey[BSS0][1].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][1].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::(Key2=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 26: //wep 104 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][1].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][1].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::(Key2=%s and type=%s)\n", arg, "Hex"));
+            break;
+        default: //Invalid argument
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::Invalid argument (=%s)\n", arg));
+            return FALSE;
+    }
+    pAdapter->SharedKey[BSS0][1].CipherAlg = CipherAlg;
+
+    // Set keys (into ASIC)
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        ;   // not support
+    else    // Old WEP stuff
+    {
+        AsicAddSharedKeyEntry(pAdapter,
+                              0,
+                              1,
+                              pAdapter->SharedKey[BSS0][1].CipherAlg,
+                              pAdapter->SharedKey[BSS0][1].Key,
+                              NULL,
+                              NULL);
+    }
+
+    return TRUE;
+}
+/*
+    ==========================================================================
+    Description:
+        Set WEP KEY3
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Key3_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    int                                 KeyLen;
+    int                                 i;
+    UCHAR                               CipherAlg=CIPHER_WEP64;
+
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        return TRUE;    // do nothing
+
+    KeyLen = strlen(arg);
+
+    switch (KeyLen)
+    {
+        case 5: //wep 40 Ascii type
+            pAdapter->SharedKey[BSS0][2].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][2].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::(Key3=%s and type=Ascii)\n", arg));
+            break;
+        case 10: //wep 40 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][2].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][2].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::(Key3=%s and type=Hex)\n", arg));
+            break;
+        case 13: //wep 104 Ascii type
+            pAdapter->SharedKey[BSS0][2].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][2].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::(Key3=%s and type=Ascii)\n", arg));
+            break;
+        case 26: //wep 104 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][2].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][2].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::(Key3=%s and type=Hex)\n", arg));
+            break;
+        default: //Invalid argument
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::Invalid argument (=%s)\n", arg));
+            return FALSE;
+    }
+    pAdapter->SharedKey[BSS0][2].CipherAlg = CipherAlg;
+
+    // Set keys (into ASIC)
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        ;   // not support
+    else    // Old WEP stuff
+    {
+        AsicAddSharedKeyEntry(pAdapter,
+                              0,
+                              2,
+                              pAdapter->SharedKey[BSS0][2].CipherAlg,
+                              pAdapter->SharedKey[BSS0][2].Key,
+                              NULL,
+                              NULL);
+    }
+
+    return TRUE;
+}
+/*
+    ==========================================================================
+    Description:
+        Set WEP KEY4
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Key4_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    int                                 KeyLen;
+    int                                 i;
+    UCHAR                               CipherAlg=CIPHER_WEP64;
+
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        return TRUE;    // do nothing
+
+    KeyLen = strlen(arg);
+
+    switch (KeyLen)
+    {
+        case 5: //wep 40 Ascii type
+            pAdapter->SharedKey[BSS0][3].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][3].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::(Key4=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 10: //wep 40 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][3].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][3].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::(Key4=%s and type=%s)\n", arg, "Hex"));
+            break;
+        case 13: //wep 104 Ascii type
+            pAdapter->SharedKey[BSS0][3].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][3].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::(Key4=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 26: //wep 104 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][3].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][3].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::(Key4=%s and type=%s)\n", arg, "Hex"));
+            break;
+        default: //Invalid argument
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::Invalid argument (=%s)\n", arg));
+            return FALSE;
+    }
+    pAdapter->SharedKey[BSS0][3].CipherAlg = CipherAlg;
+
+    // Set keys (into ASIC)
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        ;   // not support
+    else    // Old WEP stuff
+    {
+        AsicAddSharedKeyEntry(pAdapter,
+                              0,
+                              3,
+                              pAdapter->SharedKey[BSS0][3].CipherAlg,
+                              pAdapter->SharedKey[BSS0][3].Key,
+                              NULL,
+                              NULL);
+    }
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set WPA PSK key
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_WPAPSK_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    UCHAR                   keyMaterial[40];
+
+    if ((pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPAPSK) &&
+        (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPA2PSK) &&
+	    (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPANone)
+		)
+        return TRUE;    // do nothing
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_WPAPSK_Proc::(WPAPSK=%s)\n", arg));
+
+    NdisZeroMemory(keyMaterial, 40);
+
+    if ((strlen(arg) < 8) || (strlen(arg) > 64))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("Set failed!!(WPAPSK=%s), WPAPSK key-string required 8 ~ 64 characters \n", arg));
+        return FALSE;
+    }
+
+    if (strlen(arg) == 64)
+    {
+        AtoH(arg, keyMaterial, 32);
+        NdisMoveMemory(pAdapter->StaCfg.PMK, keyMaterial, 32);
+
+    }
+    else
+    {
+        PasswordHash((char *)arg, pAdapter->MlmeAux.Ssid, pAdapter->MlmeAux.SsidLen, keyMaterial);
+        NdisMoveMemory(pAdapter->StaCfg.PMK, keyMaterial, 32);
+    }
+
+
+
+    if(pAdapter->StaCfg.BssType == BSS_ADHOC &&
+       pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+    {
+         pAdapter->StaCfg.WpaState = SS_NOTUSE;
+    }
+    else
+    {
+        // Start STA supplicant state machine
+        pAdapter->StaCfg.WpaState = SS_START;
+    }
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Power Saving mode
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_PSMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    if (pAdapter->StaCfg.BssType == BSS_INFRA)
+    {
+        if ((strcmp(arg, "Max_PSP") == 0) ||
+			(strcmp(arg, "max_psp") == 0) ||
+			(strcmp(arg, "MAX_PSP") == 0))
+        {
+            // do NOT turn on PSM bit here, wait until MlmeCheckForPsmChange()
+            // to exclude certain situations.
+            if (pAdapter->StaCfg.bWindowsACCAMEnable == FALSE)
+                pAdapter->StaCfg.WindowsPowerMode = Ndis802_11PowerModeMAX_PSP;
+            pAdapter->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeMAX_PSP;
+            OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_RECEIVE_DTIM);
+            pAdapter->StaCfg.DefaultListenCount = 5;
+
+        }
+        else if ((strcmp(arg, "Fast_PSP") == 0) ||
+				 (strcmp(arg, "fast_psp") == 0) ||
+                 (strcmp(arg, "FAST_PSP") == 0))
+        {
+            // do NOT turn on PSM bit here, wait until MlmeCheckForPsmChange()
+            // to exclude certain situations.
+            OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_RECEIVE_DTIM);
+            if (pAdapter->StaCfg.bWindowsACCAMEnable == FALSE)
+                pAdapter->StaCfg.WindowsPowerMode = Ndis802_11PowerModeFast_PSP;
+            pAdapter->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeFast_PSP;
+            pAdapter->StaCfg.DefaultListenCount = 3;
+        }
+        else if ((strcmp(arg, "Legacy_PSP") == 0) ||
+                 (strcmp(arg, "legacy_psp") == 0) ||
+                 (strcmp(arg, "LEGACY_PSP") == 0))
+        {
+            // do NOT turn on PSM bit here, wait until MlmeCheckForPsmChange()
+            // to exclude certain situations.
+            OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_RECEIVE_DTIM);
+            if (pAdapter->StaCfg.bWindowsACCAMEnable == FALSE)
+                pAdapter->StaCfg.WindowsPowerMode = Ndis802_11PowerModeLegacy_PSP;
+            pAdapter->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeLegacy_PSP;
+            pAdapter->StaCfg.DefaultListenCount = 3;
+        }
+        else
+        {
+            //Default Ndis802_11PowerModeCAM
+            // clear PSM bit immediately
+            MlmeSetPsmBit(pAdapter, PWR_ACTIVE);
+            OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_RECEIVE_DTIM);
+            if (pAdapter->StaCfg.bWindowsACCAMEnable == FALSE)
+                pAdapter->StaCfg.WindowsPowerMode = Ndis802_11PowerModeCAM;
+            pAdapter->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeCAM;
+        }
+
+        DBGPRINT(RT_DEBUG_TRACE, ("Set_PSMode_Proc::(PSMode=%ld)\n", pAdapter->StaCfg.WindowsPowerMode));
+    }
+    else
+        return FALSE;
+
+
+    return TRUE;
+}
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+/*
+    ==========================================================================
+    Description:
+        Set WpaSupport flag.
+    Value:
+        0: Driver ignore wpa_supplicant.
+        1: wpa_supplicant initiates scanning and AP selection.
+        2: driver takes care of scanning, AP selection, and IEEE 802.11 association parameters.
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Wpa_Support(
+    IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+
+    if ( simple_strtol(arg, 0, 10) == 0)
+        pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_DISABLE;
+    else if ( simple_strtol(arg, 0, 10) == 1)
+        pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_ENABLE;
+    else if ( simple_strtol(arg, 0, 10) == 2)
+        pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_ENABLE_WITH_WEB_UI;
+    else
+        pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_DISABLE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_Wpa_Support::(WpaSupplicantUP=%d)\n", pAd->StaCfg.WpaSupplicantUP));
+
+    return TRUE;
+}
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef DBG
+/*
+    ==========================================================================
+    Description:
+        Read / Write MAC
+    Arguments:
+        pAdapter                    Pointer to our adapter
+        wrq                         Pointer to the ioctl argument
+
+    Return Value:
+        None
+
+    Note:
+        Usage:
+               1.) iwpriv ra0 mac 0        ==> read MAC where Addr=0x0
+               2.) iwpriv ra0 mac 0=12     ==> write MAC where Addr=0x0, value=12
+    ==========================================================================
+*/
+VOID RTMPIoctlMAC(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq)
+{
+	CHAR				*this_char;
+	CHAR				*value;
+	INT					j = 0, k = 0;
+	CHAR				msg[1024];
+	CHAR				arg[255];
+	ULONG				macAddr = 0;
+	UCHAR				temp[16], temp2[16];
+	UINT32				macValue = 0;
+	INT					Status;
+
+
+	memset(msg, 0x00, 1024);
+	if (wrq->u.data.length > 1) //No parameters.
+	{
+	    Status = copy_from_user(arg, wrq->u.data.pointer, (wrq->u.data.length > 255) ? 255 : wrq->u.data.length);
+		sprintf(msg, "\n");
+
+		//Parsing Read or Write
+	    this_char = arg;
+		if (!*this_char)
+			goto next;
+
+		if ((value = rtstrchr(this_char, '=')) != NULL)
+			*value++ = 0;
+
+		if (!value || !*value)
+		{ //Read
+			// Sanity check
+			if(strlen(this_char) > 4)
+				goto next;
+
+			j = strlen(this_char);
+			while(j-- > 0)
+			{
+				if(this_char[j] > 'f' || this_char[j] < '0')
+					return;
+			}
+
+			// Mac Addr
+			k = j = strlen(this_char);
+			while(j-- > 0)
+			{
+				this_char[4-k+j] = this_char[j];
+			}
+
+			while(k < 4)
+				this_char[3-k++]='0';
+			this_char[4]='\0';
+
+			if(strlen(this_char) == 4)
+			{
+				AtoH(this_char, temp, 2);
+				macAddr = *temp*256 + temp[1];
+				if (macAddr < 0xFFFF)
+				{
+					RTMP_IO_READ32(pAdapter, macAddr, &macValue);
+					DBGPRINT(RT_DEBUG_TRACE, ("MacAddr=%lx, MacValue=%x\n", macAddr, macValue));
+					sprintf(msg+strlen(msg), "[0x%08lX]:%08X  ", macAddr , macValue);
+				}
+				else
+				{//Invalid parametes, so default printk all bbp
+					goto next;
+				}
+			}
+		}
+		else
+		{ //Write
+			memcpy(&temp2, value, strlen(value));
+			temp2[strlen(value)] = '\0';
+
+			// Sanity check
+			if((strlen(this_char) > 4) || strlen(temp2) > 8)
+				goto next;
+
+			j = strlen(this_char);
+			while(j-- > 0)
+			{
+				if(this_char[j] > 'f' || this_char[j] < '0')
+					return;
+			}
+
+			j = strlen(temp2);
+			while(j-- > 0)
+			{
+				if(temp2[j] > 'f' || temp2[j] < '0')
+					return;
+			}
+
+			//MAC Addr
+			k = j = strlen(this_char);
+			while(j-- > 0)
+			{
+				this_char[4-k+j] = this_char[j];
+			}
+
+			while(k < 4)
+				this_char[3-k++]='0';
+			this_char[4]='\0';
+
+			//MAC value
+			k = j = strlen(temp2);
+			while(j-- > 0)
+			{
+				temp2[8-k+j] = temp2[j];
+			}
+
+			while(k < 8)
+				temp2[7-k++]='0';
+			temp2[8]='\0';
+
+			{
+				AtoH(this_char, temp, 2);
+				macAddr = *temp*256 + temp[1];
+
+				AtoH(temp2, temp, 4);
+				macValue = *temp*256*256*256 + temp[1]*256*256 + temp[2]*256 + temp[3];
+
+				// debug mode
+				if (macAddr == (HW_DEBUG_SETTING_BASE + 4))
+				{
+					// 0x2bf4: byte0 non-zero: enable R17 tuning, 0: disable R17 tuning
+                    if (macValue & 0x000000ff)
+                    {
+                        pAdapter->BbpTuning.bEnable = TRUE;
+                        DBGPRINT(RT_DEBUG_TRACE,("turn on R17 tuning\n"));
+                    }
+                    else
+                    {
+                        UCHAR R66;
+                        pAdapter->BbpTuning.bEnable = FALSE;
+                        R66 = 0x26 + GET_LNA_GAIN(pAdapter);
+#ifdef RALINK_ATE
+						if (ATE_ON(pAdapter))
+						{
+							ATE_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R66, (0x26 + GET_LNA_GAIN(pAdapter)));
+						}
+						else
+#endif // RALINK_ATE //
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R66, (0x26 + GET_LNA_GAIN(pAdapter)));
+                        DBGPRINT(RT_DEBUG_TRACE,("turn off R17 tuning, restore to 0x%02x\n", R66));
+                    }
+					return;
+				}
+
+				DBGPRINT(RT_DEBUG_TRACE, ("MacAddr=%02lx, MacValue=0x%x\n", macAddr, macValue));
+
+				RTMP_IO_WRITE32(pAdapter, macAddr, macValue);
+				sprintf(msg+strlen(msg), "[0x%08lX]:%08X  ", macAddr, macValue);
+			}
+		}
+	}
+next:
+	if(strlen(msg) == 1)
+		sprintf(msg+strlen(msg), "===>Error command format!");
+
+	// Copy the information into the user buffer
+	wrq->u.data.length = strlen(msg);
+	Status = copy_to_user(wrq->u.data.pointer, msg, wrq->u.data.length);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<==RTMPIoctlMAC\n\n"));
+}
+
+/*
+    ==========================================================================
+    Description:
+        Read / Write E2PROM
+    Arguments:
+        pAdapter                    Pointer to our adapter
+        wrq                         Pointer to the ioctl argument
+
+    Return Value:
+        None
+
+    Note:
+        Usage:
+               1.) iwpriv ra0 e2p 0     	==> read E2PROM where Addr=0x0
+               2.) iwpriv ra0 e2p 0=1234    ==> write E2PROM where Addr=0x0, value=1234
+    ==========================================================================
+*/
+VOID RTMPIoctlE2PROM(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq)
+{
+	CHAR				*this_char;
+	CHAR				*value;
+	INT					j = 0, k = 0;
+	CHAR				msg[1024];
+	CHAR				arg[255];
+	USHORT				eepAddr = 0;
+	UCHAR				temp[16], temp2[16];
+	USHORT				eepValue;
+	int					Status;
+
+
+	memset(msg, 0x00, 1024);
+	if (wrq->u.data.length > 1) //No parameters.
+	{
+	    Status = copy_from_user(arg, wrq->u.data.pointer, (wrq->u.data.length > 255) ? 255 : wrq->u.data.length);
+		sprintf(msg, "\n");
+
+	    //Parsing Read or Write
+		this_char = arg;
+
+
+		if (!*this_char)
+			goto next;
+
+		if ((value = rtstrchr(this_char, '=')) != NULL)
+			*value++ = 0;
+
+		if (!value || !*value)
+		{ //Read
+
+			// Sanity check
+			if(strlen(this_char) > 4)
+				goto next;
+
+			j = strlen(this_char);
+			while(j-- > 0)
+			{
+				if(this_char[j] > 'f' || this_char[j] < '0')
+					return;
+			}
+
+			// E2PROM addr
+			k = j = strlen(this_char);
+			while(j-- > 0)
+			{
+				this_char[4-k+j] = this_char[j];
+			}
+
+			while(k < 4)
+				this_char[3-k++]='0';
+			this_char[4]='\0';
+
+			if(strlen(this_char) == 4)
+			{
+				AtoH(this_char, temp, 2);
+				eepAddr = *temp*256 + temp[1];
+				if (eepAddr < 0xFFFF)
+				{
+					RT28xx_EEPROM_READ16(pAdapter, eepAddr, eepValue);
+					sprintf(msg+strlen(msg), "[0x%04X]:0x%04X  ", eepAddr , eepValue);
+				}
+				else
+				{//Invalid parametes, so default printk all bbp
+					goto next;
+				}
+			}
+		}
+		else
+		{ //Write
+			memcpy(&temp2, value, strlen(value));
+			temp2[strlen(value)] = '\0';
+
+			// Sanity check
+			if((strlen(this_char) > 4) || strlen(temp2) > 8)
+				goto next;
+
+			j = strlen(this_char);
+			while(j-- > 0)
+			{
+				if(this_char[j] > 'f' || this_char[j] < '0')
+					return;
+			}
+			j = strlen(temp2);
+			while(j-- > 0)
+			{
+				if(temp2[j] > 'f' || temp2[j] < '0')
+					return;
+			}
+
+			//MAC Addr
+			k = j = strlen(this_char);
+			while(j-- > 0)
+			{
+				this_char[4-k+j] = this_char[j];
+			}
+
+			while(k < 4)
+				this_char[3-k++]='0';
+			this_char[4]='\0';
+
+			//MAC value
+			k = j = strlen(temp2);
+			while(j-- > 0)
+			{
+				temp2[4-k+j] = temp2[j];
+			}
+
+			while(k < 4)
+				temp2[3-k++]='0';
+			temp2[4]='\0';
+
+			AtoH(this_char, temp, 2);
+			eepAddr = *temp*256 + temp[1];
+
+			AtoH(temp2, temp, 2);
+			eepValue = *temp*256 + temp[1];
+
+			RT28xx_EEPROM_WRITE16(pAdapter, eepAddr, eepValue);
+			sprintf(msg+strlen(msg), "[0x%02X]:%02X  ", eepAddr, eepValue);
+		}
+	}
+next:
+	if(strlen(msg) == 1)
+		sprintf(msg+strlen(msg), "===>Error command format!");
+
+
+	// Copy the information into the user buffer
+	wrq->u.data.length = strlen(msg);
+	Status = copy_to_user(wrq->u.data.pointer, msg, wrq->u.data.length);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<==RTMPIoctlE2PROM\n"));
+}
+#endif // DBG //
+
+
+
+
+INT Set_TGnWifiTest_Proc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          arg)
+{
+    if (simple_strtol(arg, 0, 10) == 0)
+        pAd->StaCfg.bTGnWifiTest = FALSE;
+    else
+        pAd->StaCfg.bTGnWifiTest = TRUE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("IF Set_TGnWifiTest_Proc::(bTGnWifiTest=%d)\n", pAd->StaCfg.bTGnWifiTest));
+	return TRUE;
+}
+
+INT Set_LongRetryLimit_Proc(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PUCHAR			arg)
+{
+	TX_RTY_CFG_STRUC	tx_rty_cfg;
+	UCHAR				LongRetryLimit = (UCHAR)simple_strtol(arg, 0, 10);
+
+	RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+	tx_rty_cfg.field.LongRtyLimit = LongRetryLimit;
+	RTMP_IO_WRITE32(pAdapter, TX_RTY_CFG, tx_rty_cfg.word);
+	DBGPRINT(RT_DEBUG_TRACE, ("IF Set_LongRetryLimit_Proc::(tx_rty_cfg=0x%x)\n", tx_rty_cfg.word));
+	return TRUE;
+}
+
+INT Set_ShortRetryLimit_Proc(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PUCHAR			arg)
+{
+	TX_RTY_CFG_STRUC	tx_rty_cfg;
+	UCHAR				ShortRetryLimit = (UCHAR)simple_strtol(arg, 0, 10);
+
+	RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+	tx_rty_cfg.field.ShortRtyLimit = ShortRetryLimit;
+	RTMP_IO_WRITE32(pAdapter, TX_RTY_CFG, tx_rty_cfg.word);
+	DBGPRINT(RT_DEBUG_TRACE, ("IF Set_ShortRetryLimit_Proc::(tx_rty_cfg=0x%x)\n", tx_rty_cfg.word));
+	return TRUE;
+}
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+INT Set_Ieee80211dClientMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    if (simple_strtol(arg, 0, 10) == 0)
+        pAdapter->StaCfg.IEEE80211dClientMode = Rt802_11_D_None;
+    else if (simple_strtol(arg, 0, 10) == 1)
+        pAdapter->StaCfg.IEEE80211dClientMode = Rt802_11_D_Flexible;
+    else if (simple_strtol(arg, 0, 10) == 2)
+        pAdapter->StaCfg.IEEE80211dClientMode = Rt802_11_D_Strict;
+    else
+        return FALSE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_Ieee802dMode_Proc::(IEEEE0211dMode=%d)\n", pAdapter->StaCfg.IEEE80211dClientMode));
+    return TRUE;
+}
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+#ifdef CARRIER_DETECTION_SUPPORT
+INT Set_CarrierDetect_Proc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          arg)
+{
+    if (simple_strtol(arg, 0, 10) == 0)
+        pAd->CommonCfg.CarrierDetect.Enable = FALSE;
+    else
+        pAd->CommonCfg.CarrierDetect.Enable = TRUE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("IF Set_CarrierDetect_Proc::(CarrierDetect.Enable=%d)\n", pAd->CommonCfg.CarrierDetect.Enable));
+	return TRUE;
+}
+#endif // CARRIER_DETECTION_SUPPORT //
+
diff --git a/drivers/staging/rt2860/wpa.h b/drivers/staging/rt2860/wpa.h
new file mode 100644
index 00000000000..88c7c8bf3fc
--- /dev/null
+++ b/drivers/staging/rt2860/wpa.h
@@ -0,0 +1,356 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	wpa.h
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+*/
+
+#ifndef	__WPA_H__
+#define	__WPA_H__
+
+// EAPOL Key descripter frame format related length
+#define LEN_KEY_DESC_NONCE			32
+#define LEN_KEY_DESC_IV				16
+#define LEN_KEY_DESC_RSC			8
+#define LEN_KEY_DESC_ID				8
+#define LEN_KEY_DESC_REPLAY			8
+#define LEN_KEY_DESC_MIC			16
+
+// The length is the EAPoL-Key frame except key data field.
+// Please refer to 802.11i-2004 ,Figure 43u in p.78
+#define LEN_EAPOL_KEY_MSG			(sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE)
+
+// EAP Code Type.
+#define EAP_CODE_REQUEST	1
+#define EAP_CODE_RESPONSE	2
+#define EAP_CODE_SUCCESS    3
+#define EAP_CODE_FAILURE    4
+
+// EAPOL frame Protocol Version
+#define	EAPOL_VER					1
+#define	EAPOL_VER2					2
+
+// EAPOL-KEY Descriptor Type
+#define	WPA1_KEY_DESC				0xfe
+#define WPA2_KEY_DESC               0x02
+
+// Key Descriptor Version of Key Information
+#define	DESC_TYPE_TKIP				1
+#define	DESC_TYPE_AES				2
+#define DESC_TYPE_MESH				3
+
+#define LEN_MSG1_2WAY               0x7f
+#define MAX_LEN_OF_EAP_HS           256
+
+#define LEN_MASTER_KEY				32
+
+// EAPOL EK, MK
+#define LEN_EAP_EK					16
+#define LEN_EAP_MICK				16
+#define LEN_EAP_KEY					((LEN_EAP_EK)+(LEN_EAP_MICK))
+// TKIP key related
+#define LEN_PMKID					16
+#define LEN_TKIP_EK					16
+#define LEN_TKIP_RXMICK				8
+#define LEN_TKIP_TXMICK				8
+#define LEN_AES_EK					16
+#define LEN_AES_KEY					LEN_AES_EK
+#define LEN_TKIP_KEY				((LEN_TKIP_EK)+(LEN_TKIP_RXMICK)+(LEN_TKIP_TXMICK))
+#define TKIP_AP_TXMICK_OFFSET		((LEN_EAP_KEY)+(LEN_TKIP_EK))
+#define TKIP_AP_RXMICK_OFFSET		(TKIP_AP_TXMICK_OFFSET+LEN_TKIP_TXMICK)
+#define TKIP_GTK_LENGTH				((LEN_TKIP_EK)+(LEN_TKIP_RXMICK)+(LEN_TKIP_TXMICK))
+#define LEN_PTK						((LEN_EAP_KEY)+(LEN_TKIP_KEY))
+
+// RSN IE Length definition
+#define MAX_LEN_OF_RSNIE         	90
+#define MIN_LEN_OF_RSNIE         	8
+
+//EAP Packet Type
+#define	EAPPacket		0
+#define	EAPOLStart		1
+#define	EAPOLLogoff		2
+#define	EAPOLKey		3
+#define	EAPOLASFAlert	4
+#define	EAPTtypeMax		5
+
+#define	EAPOL_MSG_INVALID	0
+#define	EAPOL_PAIR_MSG_1	1
+#define	EAPOL_PAIR_MSG_2	2
+#define	EAPOL_PAIR_MSG_3	3
+#define	EAPOL_PAIR_MSG_4	4
+#define	EAPOL_GROUP_MSG_1	5
+#define	EAPOL_GROUP_MSG_2	6
+
+#define PAIRWISEKEY					1
+#define GROUPKEY					0
+
+// Retry timer counter initial value
+#define PEER_MSG1_RETRY_TIMER_CTR           0
+#define PEER_MSG3_RETRY_TIMER_CTR           10
+#define GROUP_MSG1_RETRY_TIMER_CTR          20
+
+
+#define EAPOL_START_DISABLE					0
+#define EAPOL_START_PSK						1
+#define EAPOL_START_1X						2
+
+#define MIX_CIPHER_WPA_TKIP_ON(x)       (((x) & 0x08) != 0)
+#define MIX_CIPHER_WPA_AES_ON(x)        (((x) & 0x04) != 0)
+#define MIX_CIPHER_WPA2_TKIP_ON(x)      (((x) & 0x02) != 0)
+#define MIX_CIPHER_WPA2_AES_ON(x)       (((x) & 0x01) != 0)
+
+#define ROUND_UP(__x, __y) \
+	(((ULONG)((__x)+((__y)-1))) & ((ULONG)~((__y)-1)))
+
+#define	ADD_ONE_To_64BIT_VAR(_V)		\
+{										\
+	UCHAR	cnt = LEN_KEY_DESC_REPLAY;	\
+	do									\
+	{									\
+		cnt--;							\
+		_V[cnt]++;						\
+		if (cnt == 0)					\
+			break;						\
+	}while (_V[cnt] == 0);				\
+}
+
+#define IS_WPA_CAPABILITY(a)       (((a) >= Ndis802_11AuthModeWPA) && ((a) <= Ndis802_11AuthModeWPA1PSKWPA2PSK))
+
+// EAPOL Key Information definition within Key descriptor format
+typedef	struct PACKED _KEY_INFO
+{
+#ifdef RT_BIG_ENDIAN
+	UCHAR	KeyAck:1;
+    UCHAR	Install:1;
+    UCHAR	KeyIndex:2;
+    UCHAR	KeyType:1;
+    UCHAR	KeyDescVer:3;
+    UCHAR	Rsvd:3;
+    UCHAR	EKD_DL:1;		// EKD for AP; DL for STA
+    UCHAR	Request:1;
+    UCHAR	Error:1;
+    UCHAR	Secure:1;
+    UCHAR	KeyMic:1;
+#else
+	UCHAR	KeyMic:1;
+	UCHAR	Secure:1;
+	UCHAR	Error:1;
+	UCHAR	Request:1;
+	UCHAR	EKD_DL:1;       // EKD for AP; DL for STA
+	UCHAR	Rsvd:3;
+	UCHAR	KeyDescVer:3;
+	UCHAR	KeyType:1;
+	UCHAR	KeyIndex:2;
+	UCHAR	Install:1;
+	UCHAR	KeyAck:1;
+#endif
+}	KEY_INFO, *PKEY_INFO;
+
+// EAPOL Key descriptor format
+typedef	struct PACKED _KEY_DESCRIPTER
+{
+	UCHAR		Type;
+	KEY_INFO	KeyInfo;
+	UCHAR		KeyLength[2];
+	UCHAR		ReplayCounter[LEN_KEY_DESC_REPLAY];
+	UCHAR		KeyNonce[LEN_KEY_DESC_NONCE];
+	UCHAR		KeyIv[LEN_KEY_DESC_IV];
+	UCHAR		KeyRsc[LEN_KEY_DESC_RSC];
+	UCHAR		KeyId[LEN_KEY_DESC_ID];
+	UCHAR		KeyMic[LEN_KEY_DESC_MIC];
+	UCHAR		KeyDataLen[2];
+	UCHAR		KeyData[MAX_LEN_OF_RSNIE];
+}	KEY_DESCRIPTER, *PKEY_DESCRIPTER;
+
+typedef	struct PACKED _EAPOL_PACKET
+{
+	UCHAR	 			ProVer;
+	UCHAR	 			ProType;
+	UCHAR	 			Body_Len[2];
+	KEY_DESCRIPTER		KeyDesc;
+}	EAPOL_PACKET, *PEAPOL_PACKET;
+
+//802.11i D10 page 83
+typedef struct PACKED _GTK_ENCAP
+{
+#ifndef RT_BIG_ENDIAN
+    UCHAR               Kid:2;
+    UCHAR               tx:1;
+    UCHAR               rsv:5;
+    UCHAR               rsv1;
+#else
+    UCHAR               rsv:5;
+    UCHAR               tx:1;
+    UCHAR               Kid:2;
+    UCHAR               rsv1;
+#endif
+    UCHAR               GTK[TKIP_GTK_LENGTH];
+}   GTK_ENCAP, *PGTK_ENCAP;
+
+typedef struct PACKED _KDE_ENCAP
+{
+    UCHAR               Type;
+    UCHAR               Len;
+    UCHAR               OUI[3];
+    UCHAR               DataType;
+    GTK_ENCAP      GTKEncap;
+}   KDE_ENCAP, *PKDE_ENCAP;
+
+// For WPA1
+typedef struct PACKED _RSNIE {
+    UCHAR   oui[4];
+    USHORT  version;
+    UCHAR   mcast[4];
+    USHORT  ucount;
+    struct PACKED {
+        UCHAR oui[4];
+    }ucast[1];
+} RSNIE, *PRSNIE;
+
+// For WPA2
+typedef struct PACKED _RSNIE2 {
+    USHORT  version;
+    UCHAR   mcast[4];
+    USHORT  ucount;
+    struct PACKED {
+        UCHAR oui[4];
+    }ucast[1];
+} RSNIE2, *PRSNIE2;
+
+// AKM Suite
+typedef struct PACKED _RSNIE_AUTH {
+    USHORT acount;
+    struct PACKED {
+        UCHAR oui[4];
+    }auth[1];
+} RSNIE_AUTH,*PRSNIE_AUTH;
+
+typedef	union PACKED _RSN_CAPABILITIES	{
+	struct	PACKED {
+#ifdef RT_BIG_ENDIAN
+        USHORT		Rsvd:10;
+        USHORT		GTKSA_R_Counter:2;
+        USHORT		PTKSA_R_Counter:2;
+        USHORT		No_Pairwise:1;
+		USHORT		PreAuth:1;
+#else
+        USHORT		PreAuth:1;
+		USHORT		No_Pairwise:1;
+		USHORT		PTKSA_R_Counter:2;
+		USHORT		GTKSA_R_Counter:2;
+		USHORT		Rsvd:10;
+#endif
+	}	field;
+	USHORT			word;
+}	RSN_CAPABILITIES, *PRSN_CAPABILITIES;
+
+typedef struct PACKED _EAP_HDR {
+    UCHAR   ProVer;
+    UCHAR   ProType;
+    UCHAR   Body_Len[2];
+    UCHAR   code;
+    UCHAR   identifier;
+    UCHAR   length[2]; // including code and identifier, followed by length-2 octets of data
+} EAP_HDR, *PEAP_HDR;
+
+// For supplicant state machine states. 802.11i Draft 4.1, p. 97
+// We simplified it
+typedef	enum	_WpaState
+{
+	SS_NOTUSE,				// 0
+	SS_START,				// 1
+	SS_WAIT_MSG_3,			// 2
+	SS_WAIT_GROUP,			// 3
+	SS_FINISH,  			// 4
+	SS_KEYUPDATE,			// 5
+}	WPA_STATE;
+
+//
+//	The definition of the cipher combination
+//
+// 	 bit3	bit2  bit1   bit0
+//	+------------+------------+
+// 	|	  WPA	 |	   WPA2   |
+//	+------+-----+------+-----+
+//	| TKIP | AES | TKIP | AES |
+//	|	0  |  1  |   1  |  0  | -> 0x06
+//	|	0  |  1  |   1  |  1  | -> 0x07
+//	|	1  |  0  |   0  |  1  | -> 0x09
+//	|	1  |  0  |   1  |  1  | -> 0x0B
+//	|	1  |  1  |   0  |  1  | -> 0x0D
+//	|	1  |  1  |   1  |  0  | -> 0x0E
+//	|	1  |  1  |   1  |  1  |	-> 0x0F
+//	+------+-----+------+-----+
+//
+typedef	enum	_WpaMixPairCipher
+{
+	MIX_CIPHER_NOTUSE 			= 0x00,
+	WPA_NONE_WPA2_TKIPAES		= 0x03,		// WPA2-TKIPAES
+	WPA_AES_WPA2_TKIP 			= 0x06,
+	WPA_AES_WPA2_TKIPAES		= 0x07,
+	WPA_TKIP_WPA2_AES			= 0x09,
+	WPA_TKIP_WPA2_TKIPAES		= 0x0B,
+	WPA_TKIPAES_WPA2_NONE		= 0x0C,		// WPA-TKIPAES
+	WPA_TKIPAES_WPA2_AES		= 0x0D,
+	WPA_TKIPAES_WPA2_TKIP		= 0x0E,
+	WPA_TKIPAES_WPA2_TKIPAES	= 0x0F,
+}	WPA_MIX_PAIR_CIPHER;
+
+typedef struct PACKED _RSN_IE_HEADER_STRUCT	{
+	UCHAR		Eid;
+	UCHAR		Length;
+	USHORT		Version;	// Little endian format
+}	RSN_IE_HEADER_STRUCT, *PRSN_IE_HEADER_STRUCT;
+
+// Cipher suite selector types
+typedef struct PACKED _CIPHER_SUITE_STRUCT	{
+	UCHAR		Oui[3];
+	UCHAR		Type;
+}	CIPHER_SUITE_STRUCT, *PCIPHER_SUITE_STRUCT;
+
+// Authentication and Key Management suite selector
+typedef struct PACKED _AKM_SUITE_STRUCT	{
+	UCHAR		Oui[3];
+	UCHAR		Type;
+}	AKM_SUITE_STRUCT, *PAKM_SUITE_STRUCT;
+
+// RSN capability
+typedef struct	PACKED _RSN_CAPABILITY	{
+	USHORT		Rsv:10;
+	USHORT		GTKSAReplayCnt:2;
+	USHORT		PTKSAReplayCnt:2;
+	USHORT		NoPairwise:1;
+	USHORT		PreAuth:1;
+}	RSN_CAPABILITY, *PRSN_CAPABILITY;
+
+#endif
diff --git a/drivers/staging/rt2870/2870_main_dev.c b/drivers/staging/rt2870/2870_main_dev.c
new file mode 100644
index 00000000000..91da4e2298a
--- /dev/null
+++ b/drivers/staging/rt2870/2870_main_dev.c
@@ -0,0 +1,1612 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    rtmp_main.c
+
+    Abstract:
+    main initialization routines
+
+    Revision History:
+    Who         When            What
+    --------    ----------      ----------------------------------------------
+    Name        Date            Modification logs
+    Jan Lee		01-10-2005	    modified
+	Sample		Jun/01/07		Merge RT2870 and RT2860 drivers.
+*/
+
+#include "rt_config.h"
+
+
+// Following information will be show when you run 'modinfo'
+// *** If you have a solution for the bug in current version of driver, please mail to me.
+// Otherwise post to forum in ralinktech's web site(www.ralinktech.com) and let all users help you. ***
+MODULE_AUTHOR("Paul Lin <paul_lin@ralinktech.com>");
+MODULE_DESCRIPTION("RT2870 Wireless Lan Linux Driver");
+#ifdef CONFIG_STA_SUPPORT
+MODULE_LICENSE("GPL");
+#ifdef MODULE_VERSION
+MODULE_VERSION(STA_DRIVER_VERSION);
+#endif
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef MULTIPLE_CARD_SUPPORT
+// record whether the card in the card list is used in the card file
+extern UINT8  MC_CardUsed[];
+#endif // MULTIPLE_CARD_SUPPORT //
+
+/* Kernel thread and vars, which handles packets that are completed. Only
+ * packets that have a "complete" function are sent here. This way, the
+ * completion is run out of kernel context, and doesn't block the rest of
+ * the stack. */
+
+extern INT __devinit rt28xx_probe(IN void *_dev_p, IN void *_dev_id_p,
+									IN UINT argc, OUT PRTMP_ADAPTER *ppAd);
+
+
+/* module table */
+struct usb_device_id    rtusb_usb_id[] = RT2870_USB_DEVICES;
+INT const               rtusb_usb_id_len = sizeof(rtusb_usb_id) / sizeof(struct usb_device_id);
+MODULE_DEVICE_TABLE(usb, rtusb_usb_id);
+
+#ifndef PF_NOFREEZE
+#define PF_NOFREEZE  0
+#endif
+
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
+
+/**************************************************************************/
+/**************************************************************************/
+//tested for kernel 2.4 series
+/**************************************************************************/
+/**************************************************************************/
+static void *rtusb_probe(struct usb_device *dev, UINT interface,
+						const struct usb_device_id *id_table);
+static void rtusb_disconnect(struct usb_device *dev, void *ptr);
+
+struct usb_driver rtusb_driver = {
+		name:"rt2870",
+		probe:rtusb_probe,
+		disconnect:rtusb_disconnect,
+		id_table:rtusb_usb_id,
+	};
+
+#else
+
+#ifdef CONFIG_PM
+static int rt2870_suspend(struct usb_interface *intf, pm_message_t state);
+static int rt2870_resume(struct usb_interface *intf);
+#endif // CONFIG_PM //
+
+/**************************************************************************/
+/**************************************************************************/
+//tested for kernel 2.6series
+/**************************************************************************/
+/**************************************************************************/
+static int rtusb_probe (struct usb_interface *intf,
+						const struct usb_device_id *id);
+static void rtusb_disconnect(struct usb_interface *intf);
+
+struct usb_driver rtusb_driver = {
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,15)
+	.owner = THIS_MODULE,
+#endif
+	.name="rt2870",
+	.probe=rtusb_probe,
+	.disconnect=rtusb_disconnect,
+	.id_table=rtusb_usb_id,
+
+#ifdef CONFIG_PM
+	suspend:	rt2870_suspend,
+	resume:		rt2870_resume,
+#endif
+	};
+
+#ifdef CONFIG_PM
+
+VOID RT2860RejectPendingPackets(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	// clear PS packets
+	// clear TxSw packets
+}
+
+static int rt2870_suspend(
+	struct usb_interface *intf,
+	pm_message_t state)
+{
+	struct net_device *net_dev;
+	PRTMP_ADAPTER pAd = usb_get_intfdata(intf);
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("===> rt2870_suspend()\n"));
+	net_dev = pAd->net_dev;
+	netif_device_detach (net_dev);
+
+	pAd->PM_FlgSuspend = 1;
+	if (netif_running(net_dev)) {
+		RTUSBCancelPendingBulkInIRP(pAd);
+		RTUSBCancelPendingBulkOutIRP(pAd);
+	}
+	DBGPRINT(RT_DEBUG_TRACE, ("<=== rt2870_suspend()\n"));
+	return 0;
+}
+
+static int rt2870_resume(
+	struct usb_interface *intf)
+{
+	struct net_device *net_dev;
+	PRTMP_ADAPTER pAd = usb_get_intfdata(intf);
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("===> rt2870_resume()\n"));
+
+	pAd->PM_FlgSuspend = 0;
+	net_dev = pAd->net_dev;
+	netif_device_attach (net_dev);
+	netif_start_queue(net_dev);
+	netif_carrier_on(net_dev);
+	netif_wake_queue(net_dev);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<=== rt2870_resume()\n"));
+	return 0;
+}
+#endif // CONFIG_PM //
+#endif // LINUX_VERSION_CODE //
+
+
+// Init driver module
+INT __init rtusb_init(void)
+{
+	printk("rtusb init --->\n");
+	return usb_register(&rtusb_driver);
+}
+
+// Deinit driver module
+VOID __exit rtusb_exit(void)
+{
+	usb_deregister(&rtusb_driver);
+	printk("<--- rtusb exit\n");
+}
+
+module_init(rtusb_init);
+module_exit(rtusb_exit);
+
+
+
+
+/*---------------------------------------------------------------------	*/
+/* function declarations												*/
+/*---------------------------------------------------------------------	*/
+
+/*
+========================================================================
+Routine Description:
+    MLME kernel thread.
+
+Arguments:
+	*Context			the pAd, driver control block pointer
+
+Return Value:
+    0					close the thread
+
+Note:
+========================================================================
+*/
+INT MlmeThread(
+	IN void *Context)
+{
+	PRTMP_ADAPTER	pAd = (PRTMP_ADAPTER)Context;
+	POS_COOKIE	pObj;
+	int status;
+
+	pObj = (POS_COOKIE)pAd->OS_Cookie;
+
+	rtmp_os_thread_init("rt2870MlmeThread", (PVOID)&(pAd->mlmeComplete));
+
+	while (pAd->mlme_kill == 0)
+	{
+		/* lock the device pointers */
+		//down(&(pAd->mlme_semaphore));
+		status = down_interruptible(&(pAd->mlme_semaphore));
+
+		/* lock the device pointers , need to check if required*/
+		//down(&(pAd->usbdev_semaphore));
+
+		if (!pAd->PM_FlgSuspend)
+		MlmeHandler(pAd);
+
+		/* unlock the device pointers */
+		//up(&(pAd->usbdev_semaphore));
+		if (status != 0)
+		{
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS);
+			break;
+		}
+	}
+
+	/* notify the exit routine that we're actually exiting now
+	 *
+	 * complete()/wait_for_completion() is similar to up()/down(),
+	 * except that complete() is safe in the case where the structure
+	 * is getting deleted in a parallel mode of execution (i.e. just
+	 * after the down() -- that's necessary for the thread-shutdown
+	 * case.
+	 *
+	 * complete_and_exit() goes even further than this -- it is safe in
+	 * the case that the thread of the caller is going away (not just
+	 * the structure) -- this is necessary for the module-remove case.
+	 * This is important in preemption kernels, which transfer the flow
+	 * of execution immediately upon a complete().
+	 */
+	DBGPRINT(RT_DEBUG_TRACE,( "<---%s\n",__FUNCTION__));
+
+	pObj->MLMEThr_pid = THREAD_PID_INIT_VALUE;
+
+	complete_and_exit (&pAd->mlmeComplete, 0);
+	return 0;
+
+}
+
+
+/*
+========================================================================
+Routine Description:
+    USB command kernel thread.
+
+Arguments:
+	*Context			the pAd, driver control block pointer
+
+Return Value:
+    0					close the thread
+
+Note:
+========================================================================
+*/
+INT RTUSBCmdThread(
+	IN void * Context)
+{
+	PRTMP_ADAPTER	pAd = (PRTMP_ADAPTER)Context;
+	POS_COOKIE		pObj;
+	int status;
+
+	pObj = (POS_COOKIE)pAd->OS_Cookie;
+
+	rtmp_os_thread_init("rt2870CmdThread", (PVOID)&(pAd->CmdQComplete));
+
+	NdisAcquireSpinLock(&pAd->CmdQLock);
+	pAd->CmdQ.CmdQState = RT2870_THREAD_RUNNING;
+	NdisReleaseSpinLock(&pAd->CmdQLock);
+
+	while (pAd->CmdQ.CmdQState == RT2870_THREAD_RUNNING)
+	{
+		/* lock the device pointers */
+		//down(&(pAd->RTUSBCmd_semaphore));
+		status = down_interruptible(&(pAd->RTUSBCmd_semaphore));
+
+		if (pAd->CmdQ.CmdQState == RT2870_THREAD_STOPED)
+			break;
+
+		if (status != 0)
+		{
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS);
+			break;
+		}
+		/* lock the device pointers , need to check if required*/
+		//down(&(pAd->usbdev_semaphore));
+
+		if (!pAd->PM_FlgSuspend)
+		CMDHandler(pAd);
+
+		/* unlock the device pointers */
+		//up(&(pAd->usbdev_semaphore));
+	}
+
+	if (!pAd->PM_FlgSuspend)
+	{	// Clear the CmdQElements.
+		CmdQElmt	*pCmdQElmt = NULL;
+
+		NdisAcquireSpinLock(&pAd->CmdQLock);
+		pAd->CmdQ.CmdQState = RT2870_THREAD_STOPED;
+		while(pAd->CmdQ.size)
+		{
+			RTUSBDequeueCmd(&pAd->CmdQ, &pCmdQElmt);
+			if (pCmdQElmt)
+			{
+				if (pCmdQElmt->CmdFromNdis == TRUE)
+				{
+					if (pCmdQElmt->buffer != NULL)
+						NdisFreeMemory(pCmdQElmt->buffer, pCmdQElmt->bufferlength, 0);
+
+					NdisFreeMemory(pCmdQElmt, sizeof(CmdQElmt), 0);
+				}
+				else
+				{
+					if ((pCmdQElmt->buffer != NULL) && (pCmdQElmt->bufferlength != 0))
+						NdisFreeMemory(pCmdQElmt->buffer, pCmdQElmt->bufferlength, 0);
+		            {
+						NdisFreeMemory(pCmdQElmt, sizeof(CmdQElmt), 0);
+					}
+				}
+			}
+		}
+
+		NdisReleaseSpinLock(&pAd->CmdQLock);
+	}
+	/* notify the exit routine that we're actually exiting now
+	 *
+	 * complete()/wait_for_completion() is similar to up()/down(),
+	 * except that complete() is safe in the case where the structure
+	 * is getting deleted in a parallel mode of execution (i.e. just
+	 * after the down() -- that's necessary for the thread-shutdown
+	 * case.
+	 *
+	 * complete_and_exit() goes even further than this -- it is safe in
+	 * the case that the thread of the caller is going away (not just
+	 * the structure) -- this is necessary for the module-remove case.
+	 * This is important in preemption kernels, which transfer the flow
+	 * of execution immediately upon a complete().
+	 */
+	DBGPRINT(RT_DEBUG_TRACE,( "<---RTUSBCmdThread\n"));
+
+	pObj->RTUSBCmdThr_pid = THREAD_PID_INIT_VALUE;
+
+	complete_and_exit (&pAd->CmdQComplete, 0);
+	return 0;
+
+}
+
+
+static void RT2870_TimerQ_Handle(RTMP_ADAPTER *pAd)
+{
+	int status;
+	RALINK_TIMER_STRUCT	*pTimer;
+	RT2870_TIMER_ENTRY	*pEntry;
+	unsigned long	irqFlag;
+
+	while(!pAd->TimerFunc_kill)
+	{
+//		printk("waiting for event!\n");
+		pTimer = NULL;
+
+		status = down_interruptible(&(pAd->RTUSBTimer_semaphore));
+
+		if (pAd->TimerQ.status == RT2870_THREAD_STOPED)
+			break;
+
+		// event happened.
+		while(pAd->TimerQ.pQHead)
+		{
+			RTMP_IRQ_LOCK(&pAd->TimerQLock, irqFlag);
+			pEntry = pAd->TimerQ.pQHead;
+			if (pEntry)
+			{
+				pTimer = pEntry->pRaTimer;
+
+				// update pQHead
+				pAd->TimerQ.pQHead = pEntry->pNext;
+				if (pEntry == pAd->TimerQ.pQTail)
+					pAd->TimerQ.pQTail = NULL;
+
+				// return this queue entry to timerQFreeList.
+				pEntry->pNext = pAd->TimerQ.pQPollFreeList;
+				pAd->TimerQ.pQPollFreeList = pEntry;
+			}
+			RTMP_IRQ_UNLOCK(&pAd->TimerQLock, irqFlag);
+
+			if (pTimer)
+			{
+				if (pTimer->handle != NULL)
+				if (!pAd->PM_FlgSuspend)
+					pTimer->handle(NULL, (PVOID) pTimer->cookie, NULL, pTimer);
+				if ((pTimer->Repeat) && (pTimer->State == FALSE))
+					RTMP_OS_Add_Timer(&pTimer->TimerObj, pTimer->TimerValue);
+			}
+		}
+
+		if (status != 0)
+		{
+			pAd->TimerQ.status = RT2870_THREAD_STOPED;
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS);
+			break;
+		}
+	}
+}
+
+
+INT TimerQThread(
+	IN OUT PVOID Context)
+{
+	PRTMP_ADAPTER	pAd;
+	POS_COOKIE	pObj;
+
+	pAd = (PRTMP_ADAPTER)Context;
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	rtmp_os_thread_init("rt2870TimerQHandle", (PVOID)&(pAd->TimerQComplete));
+
+	RT2870_TimerQ_Handle(pAd);
+
+	/* notify the exit routine that we're actually exiting now
+	 *
+	 * complete()/wait_for_completion() is similar to up()/down(),
+	 * except that complete() is safe in the case where the structure
+	 * is getting deleted in a parallel mode of execution (i.e. just
+	 * after the down() -- that's necessary for the thread-shutdown
+	 * case.
+	 *
+	 * complete_and_exit() goes even further than this -- it is safe in
+	 * the case that the thread of the caller is going away (not just
+	 * the structure) -- this is necessary for the module-remove case.
+	 * This is important in preemption kernels, which transfer the flow
+	 * of execution immediately upon a complete().
+	 */
+	DBGPRINT(RT_DEBUG_TRACE,( "<---%s\n",__FUNCTION__));
+
+	pObj->TimerQThr_pid = THREAD_PID_INIT_VALUE;
+
+	complete_and_exit(&pAd->TimerQComplete, 0);
+	return 0;
+
+}
+
+
+RT2870_TIMER_ENTRY *RT2870_TimerQ_Insert(
+	IN RTMP_ADAPTER *pAd,
+	IN RALINK_TIMER_STRUCT *pTimer)
+{
+	RT2870_TIMER_ENTRY *pQNode = NULL, *pQTail;
+	unsigned long irqFlags;
+
+
+	RTMP_IRQ_LOCK(&pAd->TimerQLock, irqFlags);
+	if (pAd->TimerQ.status & RT2870_THREAD_CAN_DO_INSERT)
+	{
+		if(pAd->TimerQ.pQPollFreeList)
+		{
+			pQNode = pAd->TimerQ.pQPollFreeList;
+			pAd->TimerQ.pQPollFreeList = pQNode->pNext;
+
+			pQNode->pRaTimer = pTimer;
+			pQNode->pNext = NULL;
+
+			pQTail = pAd->TimerQ.pQTail;
+			if (pAd->TimerQ.pQTail != NULL)
+				pQTail->pNext = pQNode;
+			pAd->TimerQ.pQTail = pQNode;
+			if (pAd->TimerQ.pQHead == NULL)
+				pAd->TimerQ.pQHead = pQNode;
+		}
+		RTMP_IRQ_UNLOCK(&pAd->TimerQLock, irqFlags);
+
+		if (pQNode)
+			up(&pAd->RTUSBTimer_semaphore);
+			//wake_up(&timerWaitQ);
+	}
+	else
+	{
+		RTMP_IRQ_UNLOCK(&pAd->TimerQLock, irqFlags);
+	}
+	return pQNode;
+}
+
+
+BOOLEAN RT2870_TimerQ_Remove(
+	IN RTMP_ADAPTER *pAd,
+	IN RALINK_TIMER_STRUCT *pTimer)
+{
+	RT2870_TIMER_ENTRY *pNode, *pPrev = NULL;
+	unsigned long irqFlags;
+
+	RTMP_IRQ_LOCK(&pAd->TimerQLock, irqFlags);
+	if (pAd->TimerQ.status >= RT2870_THREAD_INITED)
+	{
+		pNode = pAd->TimerQ.pQHead;
+		while (pNode)
+		{
+			if (pNode->pRaTimer == pTimer)
+				break;
+			pPrev = pNode;
+			pNode = pNode->pNext;
+		}
+
+		// Now move it to freeList queue.
+		if (pNode)
+		{
+			if (pNode == pAd->TimerQ.pQHead)
+				pAd->TimerQ.pQHead = pNode->pNext;
+			if (pNode == pAd->TimerQ.pQTail)
+				pAd->TimerQ.pQTail = pPrev;
+			if (pPrev != NULL)
+				pPrev->pNext = pNode->pNext;
+
+			// return this queue entry to timerQFreeList.
+			pNode->pNext = pAd->TimerQ.pQPollFreeList;
+			pAd->TimerQ.pQPollFreeList = pNode;
+		}
+	}
+	RTMP_IRQ_UNLOCK(&pAd->TimerQLock, irqFlags);
+
+	return TRUE;
+}
+
+
+void RT2870_TimerQ_Exit(RTMP_ADAPTER *pAd)
+{
+	RT2870_TIMER_ENTRY *pTimerQ;
+	unsigned long irqFlags;
+
+	RTMP_IRQ_LOCK(&pAd->TimerQLock, irqFlags);
+	while (pAd->TimerQ.pQHead)
+	{
+		pTimerQ = pAd->TimerQ.pQHead;
+		pAd->TimerQ.pQHead = pTimerQ->pNext;
+		// remove the timeQ
+	}
+	pAd->TimerQ.pQPollFreeList = NULL;
+	os_free_mem(pAd, pAd->TimerQ.pTimerQPoll);
+	pAd->TimerQ.pQTail = NULL;
+	pAd->TimerQ.pQHead = NULL;
+	pAd->TimerQ.status = RT2870_THREAD_STOPED;
+	RTMP_IRQ_UNLOCK(&pAd->TimerQLock, irqFlags);
+
+}
+
+
+void RT2870_TimerQ_Init(RTMP_ADAPTER *pAd)
+{
+	int 	i;
+	RT2870_TIMER_ENTRY *pQNode, *pEntry;
+	unsigned long irqFlags;
+
+	NdisAllocateSpinLock(&pAd->TimerQLock);
+
+	RTMP_IRQ_LOCK(&pAd->TimerQLock, irqFlags);
+	NdisZeroMemory(&pAd->TimerQ, sizeof(pAd->TimerQ));
+	//InterlockedExchange(&pAd->TimerQ.count, 0);
+
+	/* Initialise the wait q head */
+	//init_waitqueue_head(&timerWaitQ);
+
+	os_alloc_mem(pAd, &pAd->TimerQ.pTimerQPoll, sizeof(RT2870_TIMER_ENTRY) * TIMER_QUEUE_SIZE_MAX);
+	if (pAd->TimerQ.pTimerQPoll)
+	{
+		pEntry = NULL;
+		pQNode = (RT2870_TIMER_ENTRY *)pAd->TimerQ.pTimerQPoll;
+		for (i = 0 ;i <TIMER_QUEUE_SIZE_MAX; i++)
+		{
+			pQNode->pNext = pEntry;
+			pEntry = pQNode;
+			pQNode++;
+		}
+		pAd->TimerQ.pQPollFreeList = pEntry;
+		pAd->TimerQ.pQHead = NULL;
+		pAd->TimerQ.pQTail = NULL;
+		pAd->TimerQ.status = RT2870_THREAD_INITED;
+	}
+	RTMP_IRQ_UNLOCK(&pAd->TimerQLock, irqFlags);
+}
+
+
+VOID RT2870_WatchDog(IN RTMP_ADAPTER *pAd)
+{
+	PHT_TX_CONTEXT		pHTTXContext;
+	int 					idx;
+	ULONG				irqFlags;
+	PURB		   		pUrb;
+	BOOLEAN				needDumpSeq = FALSE;
+	UINT32          	MACValue;
+
+
+	idx = 0;
+	RTMP_IO_READ32(pAd, TXRXQ_PCNT, &MACValue);
+	if ((MACValue & 0xff) !=0 )
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("TX QUEUE 0 Not EMPTY(Value=0x%0x). !!!!!!!!!!!!!!!\n", MACValue));
+		RTMP_IO_WRITE32(pAd, PBF_CFG, 0xf40012);
+		while((MACValue &0xff) != 0 && (idx++ < 10))
+		{
+		        RTMP_IO_READ32(pAd, TXRXQ_PCNT, &MACValue);
+		        NdisMSleep(1);
+		}
+		RTMP_IO_WRITE32(pAd, PBF_CFG, 0xf40006);
+	}
+
+	idx = 0;
+	if ((MACValue & 0xff00) !=0 )
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("TX QUEUE 1 Not EMPTY(Value=0x%0x). !!!!!!!!!!!!!!!\n", MACValue));
+		RTMP_IO_WRITE32(pAd, PBF_CFG, 0xf4000a);
+		while((MACValue &0xff00) != 0 && (idx++ < 10))
+		{
+			RTMP_IO_READ32(pAd, TXRXQ_PCNT, &MACValue);
+			NdisMSleep(1);
+		}
+		RTMP_IO_WRITE32(pAd, PBF_CFG, 0xf40006);
+	}
+
+
+	if (pAd->watchDogRxOverFlowCnt >= 2)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("Maybe the Rx Bulk-In hanged! Cancel the pending Rx bulks request!\n"));
+		if ((!RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_RESET_IN_PROGRESS |
+									fRTMP_ADAPTER_BULKIN_RESET |
+									fRTMP_ADAPTER_HALT_IN_PROGRESS |
+									fRTMP_ADAPTER_NIC_NOT_EXIST))))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("Call CMDTHREAD_RESET_BULK_IN to cancel the pending Rx Bulk!\n"));
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKIN_RESET);
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_IN, NULL, 0);
+			needDumpSeq = TRUE;
+		}
+		pAd->watchDogRxOverFlowCnt = 0;
+	}
+
+
+	for (idx = 0; idx < NUM_OF_TX_RING; idx++)
+	{
+		pUrb = NULL;
+
+		RTMP_IRQ_LOCK(&pAd->BulkOutLock[idx], irqFlags);
+		if ((pAd->BulkOutPending[idx] == TRUE) && pAd->watchDogTxPendingCnt)
+		{
+			pAd->watchDogTxPendingCnt[idx]++;
+
+			if ((pAd->watchDogTxPendingCnt[idx] > 2) &&
+				 (!RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_RESET_IN_PROGRESS | fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST | fRTMP_ADAPTER_BULKOUT_RESET)))
+				)
+			{
+				// FIXME: Following code just support single bulk out. If you wanna support multiple bulk out. Modify it!
+				pHTTXContext = (PHT_TX_CONTEXT)(&pAd->TxContext[idx]);
+				if (pHTTXContext->IRPPending)
+				{	// Check TxContext.
+					pUrb = pHTTXContext->pUrb;
+				}
+				else if (idx == MGMTPIPEIDX)
+				{
+					PTX_CONTEXT pMLMEContext, pNULLContext, pPsPollContext;
+
+					//Check MgmtContext.
+					pMLMEContext = (PTX_CONTEXT)(pAd->MgmtRing.Cell[pAd->MgmtRing.TxDmaIdx].AllocVa);
+					pPsPollContext = (PTX_CONTEXT)(&pAd->PsPollContext);
+					pNULLContext = (PTX_CONTEXT)(&pAd->NullContext);
+
+					if (pMLMEContext->IRPPending)
+					{
+						ASSERT(pMLMEContext->IRPPending);
+						pUrb = pMLMEContext->pUrb;
+					}
+					else if (pNULLContext->IRPPending)
+					{
+						ASSERT(pNULLContext->IRPPending);
+						pUrb = pNULLContext->pUrb;
+					}
+					else if (pPsPollContext->IRPPending)
+					{
+						ASSERT(pPsPollContext->IRPPending);
+						pUrb = pPsPollContext->pUrb;
+					}
+				}
+
+				RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[idx], irqFlags);
+
+				DBGPRINT(RT_DEBUG_TRACE, ("Maybe the Tx Bulk-Out hanged! Cancel the pending Tx bulks request of idx(%d)!\n", idx));
+				if (pUrb)
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("Unlink the pending URB!\n"));
+					// unlink it now
+					RTUSB_UNLINK_URB(pUrb);
+					// Sleep 200 microseconds to give cancellation time to work
+					RTMPusecDelay(200);
+					needDumpSeq = TRUE;
+				}
+				else
+				{
+					DBGPRINT(RT_DEBUG_ERROR, ("Unkonw bulkOut URB maybe hanged!!!!!!!!!!!!\n"));
+				}
+			}
+			else
+			{
+				RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[idx], irqFlags);
+			}
+		}
+		else
+		{
+			RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[idx], irqFlags);
+		}
+	}
+
+#ifdef DOT11_N_SUPPORT
+	// For Sigma debug, dump the ba_reordering sequence.
+	if((needDumpSeq == TRUE) && (pAd->CommonCfg.bDisableReordering == 0))
+	{
+		USHORT				Idx;
+		PBA_REC_ENTRY		pBAEntry = NULL;
+		UCHAR				count = 0;
+		struct reordering_mpdu *mpdu_blk;
+
+		Idx = pAd->MacTab.Content[BSSID_WCID].BARecWcidArray[0];
+
+		pBAEntry = &pAd->BATable.BARecEntry[Idx];
+		if((pBAEntry->list.qlen > 0) && (pBAEntry->list.next != NULL))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("NICUpdateRawCounters():The Queueing pkt in reordering buffer:\n"));
+			NdisAcquireSpinLock(&pBAEntry->RxReRingLock);
+			mpdu_blk = pBAEntry->list.next;
+			while (mpdu_blk)
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("\t%d:Seq-%d, bAMSDU-%d!\n", count, mpdu_blk->Sequence, mpdu_blk->bAMSDU));
+				mpdu_blk = mpdu_blk->next;
+				count++;
+			}
+
+			DBGPRINT(RT_DEBUG_TRACE, ("\npBAEntry->LastIndSeq=%d!\n", pBAEntry->LastIndSeq));
+			NdisReleaseSpinLock(&pBAEntry->RxReRingLock);
+		}
+	}
+#endif // DOT11_N_SUPPORT //
+}
+
+/*
+========================================================================
+Routine Description:
+    Release allocated resources.
+
+Arguments:
+    *dev				Point to the PCI or USB device
+	pAd					driver control block pointer
+
+Return Value:
+    None
+
+Note:
+========================================================================
+*/
+static void _rtusb_disconnect(struct usb_device *dev, PRTMP_ADAPTER pAd)
+{
+	struct net_device	*net_dev = NULL;
+
+
+	DBGPRINT(RT_DEBUG_ERROR, ("rtusb_disconnect: unregister usbnet usb-%s-%s\n",
+				dev->bus->bus_name, dev->devpath));
+	if (!pAd)
+	{
+#ifdef MULTIPLE_CARD_SUPPORT
+		if ((pAd->MC_RowID >= 0) && (pAd->MC_RowID <= MAX_NUM_OF_MULTIPLE_CARD))
+			MC_CardUsed[pAd->MC_RowID] = 0; // not clear MAC address
+#endif // MULTIPLE_CARD_SUPPORT //
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)	/* kernel 2.4 series */
+		while(MOD_IN_USE > 0)
+		{
+			MOD_DEC_USE_COUNT;
+		}
+#else
+		usb_put_dev(dev);
+#endif // LINUX_VERSION_CODE //
+
+		printk("rtusb_disconnect: pAd == NULL!\n");
+		return;
+	}
+	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST);
+
+
+
+	// for debug, wait to show some messages to /proc system
+	udelay(1);
+
+
+
+
+	net_dev = pAd->net_dev;
+	if (pAd->net_dev != NULL)
+	{
+		printk("rtusb_disconnect: unregister_netdev(), dev->name=%s!\n", net_dev->name);
+		unregister_netdev (pAd->net_dev);
+	}
+	udelay(1);
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)	/* kernel 2.4 series */
+#else
+	flush_scheduled_work();
+#endif // LINUX_VERSION_CODE //
+	udelay(1);
+
+	// free net_device memory
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)	/* kernel 2.4 series */
+	kfree(net_dev);
+#else
+	free_netdev(net_dev);
+#endif // LINUX_VERSION_CODE //
+
+	// free adapter memory
+	RTMPFreeAdapter(pAd);
+
+	// release a use of the usb device structure
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)	/* kernel 2.4 series */
+	while(MOD_IN_USE > 0)
+	{
+		MOD_DEC_USE_COUNT;
+	}
+#else
+	usb_put_dev(dev);
+#endif // LINUX_VERSION_CODE //
+	udelay(1);
+
+	DBGPRINT(RT_DEBUG_ERROR, (" RTUSB disconnect successfully\n"));
+}
+
+
+/*
+========================================================================
+Routine Description:
+    Probe RT28XX chipset.
+
+Arguments:
+    *dev				Point to the PCI or USB device
+	interface
+	*id_table			Point to the PCI or USB device ID
+
+Return Value:
+    None
+
+Note:
+========================================================================
+*/
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)	/* kernel 2.4 series */
+static void *rtusb_probe(struct usb_device *dev, UINT interface,
+						const struct usb_device_id *id)
+{
+	PRTMP_ADAPTER pAd;
+	rt28xx_probe((void *)dev, (void *)id, interface, &pAd);
+	return (void *)pAd;
+}
+
+//Disconnect function is called within exit routine
+static void rtusb_disconnect(struct usb_device *dev, void *ptr)
+{
+	_rtusb_disconnect(dev, ((PRTMP_ADAPTER)ptr));
+}
+
+#else	/* kernel 2.6 series */
+static int rtusb_probe (struct usb_interface *intf,
+						const struct usb_device_id *id)
+{
+	PRTMP_ADAPTER pAd;
+	return (int)rt28xx_probe((void *)intf, (void *)id, 0, &pAd);
+}
+
+
+static void rtusb_disconnect(struct usb_interface *intf)
+{
+	struct usb_device   *dev = interface_to_usbdev(intf);
+	PRTMP_ADAPTER       pAd;
+
+
+	pAd = usb_get_intfdata(intf);
+	usb_set_intfdata(intf, NULL);
+
+	_rtusb_disconnect(dev, pAd);
+}
+#endif // LINUX_VERSION_CODE //
+
+
+/*
+========================================================================
+Routine Description:
+    Close kernel threads.
+
+Arguments:
+	*pAd				the raxx interface data pointer
+
+Return Value:
+    NONE
+
+Note:
+========================================================================
+*/
+VOID RT28xxThreadTerminate(
+	IN RTMP_ADAPTER *pAd)
+{
+	POS_COOKIE	pObj = (POS_COOKIE) pAd->OS_Cookie;
+	INT			ret;
+
+
+	// Sleep 50 milliseconds so pending io might finish normally
+	RTMPusecDelay(50000);
+
+	// We want to wait until all pending receives and sends to the
+	// device object. We cancel any
+	// irps. Wait until sends and receives have stopped.
+	RTUSBCancelPendingIRPs(pAd);
+
+	// Terminate Threads
+	CHECK_PID_LEGALITY(pObj->TimerQThr_pid)
+	{
+		POS_COOKIE pObj = (POS_COOKIE)pAd->OS_Cookie;
+
+		printk("Terminate the TimerQThr_pid=%d!\n", GET_PID_NUMBER(pObj->TimerQThr_pid));
+		mb();
+		pAd->TimerFunc_kill = 1;
+		mb();
+		ret = KILL_THREAD_PID(pObj->TimerQThr_pid, SIGTERM, 1);
+		if (ret)
+		{
+			printk(KERN_WARNING "%s: unable to stop TimerQThread, pid=%d, ret=%d!\n",
+					pAd->net_dev->name, GET_PID_NUMBER(pObj->TimerQThr_pid), ret);
+		}
+		else
+		{
+			wait_for_completion(&pAd->TimerQComplete);
+			pObj->TimerQThr_pid = THREAD_PID_INIT_VALUE;
+		}
+	}
+
+	CHECK_PID_LEGALITY(pObj->MLMEThr_pid)
+	{
+		printk("Terminate the MLMEThr_pid=%d!\n", GET_PID_NUMBER(pObj->MLMEThr_pid));
+		mb();
+		pAd->mlme_kill = 1;
+		//RT28XX_MLME_HANDLER(pAd);
+		mb();
+		ret = KILL_THREAD_PID(pObj->MLMEThr_pid, SIGTERM, 1);
+		if (ret)
+		{
+			printk (KERN_WARNING "%s: unable to Mlme thread, pid=%d, ret=%d!\n",
+					pAd->net_dev->name, GET_PID_NUMBER(pObj->MLMEThr_pid), ret);
+		}
+		else
+		{
+			//wait_for_completion (&pAd->notify);
+			wait_for_completion (&pAd->mlmeComplete);
+			pObj->MLMEThr_pid = THREAD_PID_INIT_VALUE;
+		}
+	}
+
+	CHECK_PID_LEGALITY(pObj->RTUSBCmdThr_pid)
+	{
+		printk("Terminate the RTUSBCmdThr_pid=%d!\n", GET_PID_NUMBER(pObj->RTUSBCmdThr_pid));
+		mb();
+		NdisAcquireSpinLock(&pAd->CmdQLock);
+		pAd->CmdQ.CmdQState = RT2870_THREAD_STOPED;
+		NdisReleaseSpinLock(&pAd->CmdQLock);
+		mb();
+		//RTUSBCMDUp(pAd);
+		ret = KILL_THREAD_PID(pObj->RTUSBCmdThr_pid, SIGTERM, 1);
+		if (ret)
+		{
+			printk(KERN_WARNING "%s: unable to RTUSBCmd thread, pid=%d, ret=%d!\n",
+					pAd->net_dev->name, GET_PID_NUMBER(pObj->RTUSBCmdThr_pid), ret);
+		}
+		else
+		{
+			//wait_for_completion (&pAd->notify);
+			wait_for_completion (&pAd->CmdQComplete);
+			pObj->RTUSBCmdThr_pid = THREAD_PID_INIT_VALUE;
+	}
+	}
+
+
+	// Kill tasklets
+	pAd->mlme_kill = 0;
+	pAd->CmdQ.CmdQState = RT2870_THREAD_UNKNOWN;
+	pAd->TimerFunc_kill = 0;
+}
+
+
+void kill_thread_task(IN PRTMP_ADAPTER pAd)
+{
+	POS_COOKIE pObj;
+
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	tasklet_kill(&pObj->rx_done_task);
+	tasklet_kill(&pObj->mgmt_dma_done_task);
+	tasklet_kill(&pObj->ac0_dma_done_task);
+	tasklet_kill(&pObj->ac1_dma_done_task);
+	tasklet_kill(&pObj->ac2_dma_done_task);
+	tasklet_kill(&pObj->ac3_dma_done_task);
+	tasklet_kill(&pObj->hcca_dma_done_task);
+	tasklet_kill(&pObj->tbtt_task);
+
+}
+
+
+/*
+========================================================================
+Routine Description:
+    Check the chipset vendor/product ID.
+
+Arguments:
+    _dev_p				Point to the PCI or USB device
+
+Return Value:
+    TRUE				Check ok
+	FALSE				Check fail
+
+Note:
+========================================================================
+*/
+BOOLEAN RT28XXChipsetCheck(
+	IN void *_dev_p)
+{
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)	/* kernel 2.4 series */
+	struct usb_device *dev_p = (struct usb_device *)_dev_p;
+#else
+	struct usb_interface *intf = (struct usb_interface *)_dev_p;
+	struct usb_device *dev_p = interface_to_usbdev(intf);
+#endif // LINUX_VERSION_CODE //
+	UINT32 i;
+
+
+	for(i=0; i<rtusb_usb_id_len; i++)
+	{
+		if (dev_p->descriptor.idVendor == rtusb_usb_id[i].idVendor &&
+			dev_p->descriptor.idProduct == rtusb_usb_id[i].idProduct)
+		{
+			printk("rt2870: idVendor = 0x%x, idProduct = 0x%x\n",
+					dev_p->descriptor.idVendor, dev_p->descriptor.idProduct);
+			break;
+		}
+	}
+
+	if (i == rtusb_usb_id_len)
+	{
+		printk("rt2870: Error! Device Descriptor not matching!\n");
+		return FALSE;
+	}
+
+	return TRUE;
+}
+
+
+/*
+========================================================================
+Routine Description:
+    Init net device structure.
+
+Arguments:
+    _dev_p				Point to the PCI or USB device
+    *net_dev			Point to the net device
+	*pAd				the raxx interface data pointer
+
+Return Value:
+    TRUE				Init ok
+	FALSE				Init fail
+
+Note:
+========================================================================
+*/
+BOOLEAN RT28XXNetDevInit(
+	IN void 				*_dev_p,
+	IN struct  net_device	*net_dev,
+	IN RTMP_ADAPTER 		*pAd)
+{
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)	/* kernel 2.4 series */
+	struct usb_device *dev_p = (struct usb_device *)_dev_p;
+#else
+	struct usb_interface *intf = (struct usb_interface *)_dev_p;
+	struct usb_device *dev_p = interface_to_usbdev(intf);
+#endif // LINUX_VERSION_CODE //
+
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)	/* kernel 2.4 series */
+	pAd->config = dev_p->config;
+#else
+	pAd->config = &dev_p->config->desc;
+#endif // LINUX_VERSION_CODE //
+	return TRUE;
+}
+
+
+/*
+========================================================================
+Routine Description:
+    Init net device structure.
+
+Arguments:
+    _dev_p				Point to the PCI or USB device
+	*pAd				the raxx interface data pointer
+
+Return Value:
+    TRUE				Config ok
+	FALSE				Config fail
+
+Note:
+========================================================================
+*/
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
+BOOLEAN RT28XXProbePostConfig(
+	IN void 				*_dev_p,
+	IN RTMP_ADAPTER 		*pAd,
+	IN INT32				interface)
+{
+	struct usb_device *dev_p = (struct usb_device *)_dev_p;
+	struct usb_interface *intf;
+	struct usb_interface_descriptor *iface_desc;
+	struct usb_endpoint_descriptor *endpoint;
+	ULONG BulkOutIdx;
+	UINT32 i;
+
+
+	/* get the active interface descriptor */
+	intf = &dev_p->actconfig->interface[interface];
+	iface_desc = &intf->altsetting[0];
+
+	/* get # of enpoints */
+	pAd->NumberOfPipes = iface_desc->bNumEndpoints;
+	DBGPRINT(RT_DEBUG_TRACE, ("NumEndpoints=%d\n", iface_desc->bNumEndpoints));
+
+	/* Configure Pipes */
+	endpoint = &iface_desc->endpoint[0];
+	BulkOutIdx = 0;
+
+	for(i=0; i<pAd->NumberOfPipes; i++)
+	{
+		if ((endpoint[i].bmAttributes == USB_ENDPOINT_XFER_BULK) &&
+			((endpoint[i].bEndpointAddress & USB_ENDPOINT_DIR_MASK) == USB_DIR_IN))
+		{
+			pAd->BulkInEpAddr = endpoint[i].bEndpointAddress;
+			pAd->BulkInMaxPacketSize = endpoint[i].wMaxPacketSize;
+
+			DBGPRINT_RAW(RT_DEBUG_TRACE,
+				("BULK IN MaximumPacketSize = %d\n", pAd->BulkInMaxPacketSize));
+			DBGPRINT_RAW(RT_DEBUG_TRACE,
+				("EP address = 0x%2x  \n", endpoint[i].bEndpointAddress));
+		}
+		else if ((endpoint[i].bmAttributes == USB_ENDPOINT_XFER_BULK) &&
+				((endpoint[i].bEndpointAddress & USB_ENDPOINT_DIR_MASK) == USB_DIR_OUT))
+		{
+			// There are 6 bulk out EP. EP6 highest priority.
+			// EP1-4 is EDCA.  EP5 is HCCA.
+			pAd->BulkOutEpAddr[BulkOutIdx++] = endpoint[i].bEndpointAddress;
+			pAd->BulkOutMaxPacketSize = endpoint[i].wMaxPacketSize;
+
+			DBGPRINT_RAW(RT_DEBUG_TRACE,
+				("BULK OUT MaximumPacketSize = %d\n", pAd->BulkOutMaxPacketSize));
+			DBGPRINT_RAW(RT_DEBUG_TRACE,
+				("EP address = 0x%2x  \n", endpoint[i].bEndpointAddress));
+		}
+	}
+
+	if (!(pAd->BulkInEpAddr && pAd->BulkOutEpAddr[0]))
+	{
+		printk("Could not find both bulk-in and bulk-out endpoints\n");
+		return FALSE;
+	}
+
+	return TRUE;
+}
+
+#else
+BOOLEAN RT28XXProbePostConfig(
+	IN void 				*_dev_p,
+	IN RTMP_ADAPTER 		*pAd,
+	IN INT32				interface)
+{
+	struct usb_interface *intf = (struct usb_interface *)_dev_p;
+	struct usb_host_interface *iface_desc;
+	ULONG BulkOutIdx;
+	UINT32 i;
+
+
+	/* get the active interface descriptor */
+	iface_desc = intf->cur_altsetting;
+
+	/* get # of enpoints  */
+	pAd->NumberOfPipes = iface_desc->desc.bNumEndpoints;
+	DBGPRINT(RT_DEBUG_TRACE,
+			("NumEndpoints=%d\n", iface_desc->desc.bNumEndpoints));
+
+	/* Configure Pipes */
+	BulkOutIdx = 0;
+
+	for(i=0; i<pAd->NumberOfPipes; i++)
+	{
+		if ((iface_desc->endpoint[i].desc.bmAttributes ==
+				USB_ENDPOINT_XFER_BULK) &&
+			((iface_desc->endpoint[i].desc.bEndpointAddress &
+				USB_ENDPOINT_DIR_MASK) == USB_DIR_IN))
+		{
+			pAd->BulkInEpAddr = iface_desc->endpoint[i].desc.bEndpointAddress;
+			pAd->BulkInMaxPacketSize = iface_desc->endpoint[i].desc.wMaxPacketSize;
+
+			DBGPRINT_RAW(RT_DEBUG_TRACE,
+				("BULK IN MaximumPacketSize = %d\n", pAd->BulkInMaxPacketSize));
+			DBGPRINT_RAW(RT_DEBUG_TRACE,
+				("EP address = 0x%2x\n", iface_desc->endpoint[i].desc.bEndpointAddress));
+		}
+		else if ((iface_desc->endpoint[i].desc.bmAttributes ==
+					USB_ENDPOINT_XFER_BULK) &&
+				((iface_desc->endpoint[i].desc.bEndpointAddress &
+					USB_ENDPOINT_DIR_MASK) == USB_DIR_OUT))
+		{
+			// there are 6 bulk out EP. EP6 highest priority.
+			// EP1-4 is EDCA.  EP5 is HCCA.
+			pAd->BulkOutEpAddr[BulkOutIdx++] = iface_desc->endpoint[i].desc.bEndpointAddress;
+			pAd->BulkOutMaxPacketSize = iface_desc->endpoint[i].desc.wMaxPacketSize;
+
+			DBGPRINT_RAW(RT_DEBUG_TRACE,
+				("BULK OUT MaximumPacketSize = %d\n", pAd->BulkOutMaxPacketSize));
+			DBGPRINT_RAW(RT_DEBUG_TRACE,
+				("EP address = 0x%2x  \n", iface_desc->endpoint[i].desc.bEndpointAddress));
+		}
+	}
+
+	if (!(pAd->BulkInEpAddr && pAd->BulkOutEpAddr[0]))
+	{
+		printk("%s: Could not find both bulk-in and bulk-out endpoints\n", __FUNCTION__);
+		return FALSE;
+	}
+
+	return TRUE;
+}
+#endif // LINUX_VERSION_CODE //
+
+
+/*
+========================================================================
+Routine Description:
+    Disable DMA.
+
+Arguments:
+	*pAd				the raxx interface data pointer
+
+Return Value:
+	None
+
+Note:
+========================================================================
+*/
+VOID RT28XXDMADisable(
+	IN RTMP_ADAPTER 		*pAd)
+{
+	// no use
+}
+
+
+
+/*
+========================================================================
+Routine Description:
+    Enable DMA.
+
+Arguments:
+	*pAd				the raxx interface data pointer
+
+Return Value:
+	None
+
+Note:
+========================================================================
+*/
+VOID RT28XXDMAEnable(
+	IN RTMP_ADAPTER 		*pAd)
+{
+	WPDMA_GLO_CFG_STRUC	GloCfg;
+	USB_DMA_CFG_STRUC	UsbCfg;
+	int					i = 0;
+
+
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x4);
+	do
+	{
+		RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &GloCfg.word);
+		if ((GloCfg.field.TxDMABusy == 0)  && (GloCfg.field.RxDMABusy == 0))
+			break;
+
+		DBGPRINT(RT_DEBUG_TRACE, ("==>  DMABusy\n"));
+		RTMPusecDelay(1000);
+		i++;
+	}while ( i <200);
+
+
+	RTMPusecDelay(50);
+	GloCfg.field.EnTXWriteBackDDONE = 1;
+	GloCfg.field.EnableRxDMA = 1;
+	GloCfg.field.EnableTxDMA = 1;
+	DBGPRINT(RT_DEBUG_TRACE, ("<== WRITE DMA offset 0x208 = 0x%x\n", GloCfg.word));
+	RTMP_IO_WRITE32(pAd, WPDMA_GLO_CFG, GloCfg.word);
+
+	UsbCfg.word = 0;
+	UsbCfg.field.phyclear = 0;
+	/* usb version is 1.1,do not use bulk in aggregation */
+	if (pAd->BulkInMaxPacketSize == 512)
+			UsbCfg.field.RxBulkAggEn = 1;
+	/* for last packet, PBF might use more than limited, so minus 2 to prevent from error */
+	UsbCfg.field.RxBulkAggLmt = (MAX_RXBULK_SIZE /1024)-3;
+	UsbCfg.field.RxBulkAggTOut = 0x80; /* 2006-10-18 */
+	UsbCfg.field.RxBulkEn = 1;
+	UsbCfg.field.TxBulkEn = 1;
+
+	RTUSBWriteMACRegister(pAd, USB_DMA_CFG, UsbCfg.word);
+
+}
+
+/*
+========================================================================
+Routine Description:
+    Write Beacon buffer to Asic.
+
+Arguments:
+	*pAd				the raxx interface data pointer
+
+Return Value:
+	None
+
+Note:
+========================================================================
+*/
+VOID RT28xx_UpdateBeaconToAsic(
+	IN RTMP_ADAPTER		*pAd,
+	IN INT				apidx,
+	IN ULONG			FrameLen,
+	IN ULONG			UpdatePos)
+{
+	PUCHAR        	pBeaconFrame = NULL;
+	UCHAR  			*ptr;
+	UINT  			i, padding;
+	BEACON_SYNC_STRUCT	*pBeaconSync = pAd->CommonCfg.pBeaconSync;
+	UINT32			longValue;
+	BOOLEAN			bBcnReq = FALSE;
+	UCHAR			bcn_idx = 0;
+
+
+	if (pBeaconFrame == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("pBeaconFrame is NULL!\n"));
+		return;
+	}
+
+	if (pBeaconSync == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("pBeaconSync is NULL!\n"));
+		return;
+	}
+
+	//if ((pAd->WdsTab.Mode == WDS_BRIDGE_MODE) ||
+	//	((pAd->ApCfg.MBSSID[apidx].MSSIDDev == NULL) || !(pAd->ApCfg.MBSSID[apidx].MSSIDDev->flags & IFF_UP))
+	//	)
+	if (bBcnReq == FALSE)
+	{
+		/* when the ra interface is down, do not send its beacon frame */
+		/* clear all zero */
+		for(i=0; i<TXWI_SIZE; i+=4) {
+			RTMP_IO_WRITE32(pAd, pAd->BeaconOffset[bcn_idx] + i, 0x00);
+		}
+		pBeaconSync->BeaconBitMap &= (~(BEACON_BITMAP_MASK & (1 << bcn_idx)));
+		NdisZeroMemory(pBeaconSync->BeaconTxWI[bcn_idx], TXWI_SIZE);
+	}
+	else
+	{
+		ptr = (PUCHAR)&pAd->BeaconTxWI;
+#ifdef RT_BIG_ENDIAN
+		RTMPWIEndianChange(ptr, TYPE_TXWI);
+#endif
+		if (NdisEqualMemory(pBeaconSync->BeaconTxWI[bcn_idx], &pAd->BeaconTxWI, TXWI_SIZE) == FALSE)
+		{	// If BeaconTxWI changed, we need to rewrite the TxWI for the Beacon frames.
+			pBeaconSync->BeaconBitMap &= (~(BEACON_BITMAP_MASK & (1 << bcn_idx)));
+			NdisMoveMemory(pBeaconSync->BeaconTxWI[bcn_idx], &pAd->BeaconTxWI, TXWI_SIZE);
+		}
+
+		if ((pBeaconSync->BeaconBitMap & (1 << bcn_idx)) != (1 << bcn_idx))
+		{
+			for (i=0; i<TXWI_SIZE; i+=4)  // 16-byte TXWI field
+			{
+				longValue =  *ptr + (*(ptr+1)<<8) + (*(ptr+2)<<16) + (*(ptr+3)<<24);
+				RTMP_IO_WRITE32(pAd, pAd->BeaconOffset[bcn_idx] + i, longValue);
+				ptr += 4;
+			}
+		}
+
+		ptr = pBeaconSync->BeaconBuf[bcn_idx];
+		padding = (FrameLen & 0x01);
+		NdisZeroMemory((PUCHAR)(pBeaconFrame + FrameLen), padding);
+		FrameLen += padding;
+		for (i = 0 ; i < FrameLen /*HW_BEACON_OFFSET*/; i += 2)
+		{
+			if (NdisEqualMemory(ptr, pBeaconFrame, 2) == FALSE)
+			{
+				NdisMoveMemory(ptr, pBeaconFrame, 2);
+				//shortValue = *ptr + (*(ptr+1)<<8);
+				//RTMP_IO_WRITE8(pAd, pAd->BeaconOffset[bcn_idx] + TXWI_SIZE + i, shortValue);
+				RTUSBMultiWrite(pAd, pAd->BeaconOffset[bcn_idx] + TXWI_SIZE + i, ptr, 2);
+			}
+			ptr +=2;
+			pBeaconFrame += 2;
+		}
+
+		pBeaconSync->BeaconBitMap |= (1 << bcn_idx);
+	}
+
+}
+
+
+VOID RT2870_BssBeaconStop(
+	IN RTMP_ADAPTER *pAd)
+{
+	BEACON_SYNC_STRUCT	*pBeaconSync;
+	int i, offset;
+	BOOLEAN	Cancelled = TRUE;
+
+	pBeaconSync = pAd->CommonCfg.pBeaconSync;
+	if (pBeaconSync && pBeaconSync->EnableBeacon)
+	{
+		INT NumOfBcn;
+
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			NumOfBcn = MAX_MESH_NUM;
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+		RTMPCancelTimer(&pAd->CommonCfg.BeaconUpdateTimer, &Cancelled);
+
+		for(i=0; i<NumOfBcn; i++)
+		{
+			NdisZeroMemory(pBeaconSync->BeaconBuf[i], HW_BEACON_OFFSET);
+			NdisZeroMemory(pBeaconSync->BeaconTxWI[i], TXWI_SIZE);
+
+			for (offset=0; offset<HW_BEACON_OFFSET; offset+=4)
+				RTMP_IO_WRITE32(pAd, pAd->BeaconOffset[i] + offset, 0x00);
+
+			pBeaconSync->CapabilityInfoLocationInBeacon[i] = 0;
+			pBeaconSync->TimIELocationInBeacon[i] = 0;
+		}
+		pBeaconSync->BeaconBitMap = 0;
+		pBeaconSync->DtimBitOn = 0;
+	}
+}
+
+
+VOID RT2870_BssBeaconStart(
+	IN RTMP_ADAPTER *pAd)
+{
+	int apidx;
+	BEACON_SYNC_STRUCT	*pBeaconSync;
+//	LARGE_INTEGER 	tsfTime, deltaTime;
+
+	pBeaconSync = pAd->CommonCfg.pBeaconSync;
+	if (pBeaconSync && pBeaconSync->EnableBeacon)
+	{
+		INT NumOfBcn;
+
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			NumOfBcn = MAX_MESH_NUM;
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+		for(apidx=0; apidx<NumOfBcn; apidx++)
+		{
+			UCHAR CapabilityInfoLocationInBeacon = 0;
+			UCHAR TimIELocationInBeacon = 0;
+
+			NdisZeroMemory(pBeaconSync->BeaconBuf[apidx], HW_BEACON_OFFSET);
+			pBeaconSync->CapabilityInfoLocationInBeacon[apidx] = CapabilityInfoLocationInBeacon;
+			pBeaconSync->TimIELocationInBeacon[apidx] = TimIELocationInBeacon;
+			NdisZeroMemory(pBeaconSync->BeaconTxWI[apidx], TXWI_SIZE);
+		}
+		pBeaconSync->BeaconBitMap = 0;
+		pBeaconSync->DtimBitOn = 0;
+		pAd->CommonCfg.BeaconUpdateTimer.Repeat = TRUE;
+
+		pAd->CommonCfg.BeaconAdjust = 0;
+		pAd->CommonCfg.BeaconFactor = 0xffffffff / (pAd->CommonCfg.BeaconPeriod << 10);
+		pAd->CommonCfg.BeaconRemain = (0xffffffff % (pAd->CommonCfg.BeaconPeriod << 10)) + 1;
+		printk("RT2870_BssBeaconStart:BeaconFactor=%d, BeaconRemain=%d!\n", pAd->CommonCfg.BeaconFactor, pAd->CommonCfg.BeaconRemain);
+		RTMPSetTimer(&pAd->CommonCfg.BeaconUpdateTimer, pAd->CommonCfg.BeaconPeriod);
+
+	}
+}
+
+
+VOID RT2870_BssBeaconInit(
+	IN RTMP_ADAPTER *pAd)
+{
+	BEACON_SYNC_STRUCT	*pBeaconSync;
+	int i;
+
+	NdisAllocMemory(pAd->CommonCfg.pBeaconSync, sizeof(BEACON_SYNC_STRUCT), MEM_ALLOC_FLAG);
+	if (pAd->CommonCfg.pBeaconSync)
+	{
+		pBeaconSync = pAd->CommonCfg.pBeaconSync;
+		NdisZeroMemory(pBeaconSync, sizeof(BEACON_SYNC_STRUCT));
+		for(i=0; i < HW_BEACON_MAX_COUNT; i++)
+		{
+			NdisZeroMemory(pBeaconSync->BeaconBuf[i], HW_BEACON_OFFSET);
+			pBeaconSync->CapabilityInfoLocationInBeacon[i] = 0;
+			pBeaconSync->TimIELocationInBeacon[i] = 0;
+			NdisZeroMemory(pBeaconSync->BeaconTxWI[i], TXWI_SIZE);
+		}
+		pBeaconSync->BeaconBitMap = 0;
+
+		//RTMPInitTimer(pAd, &pAd->CommonCfg.BeaconUpdateTimer, GET_TIMER_FUNCTION(BeaconUpdateExec), pAd, TRUE);
+		pBeaconSync->EnableBeacon = TRUE;
+	}
+}
+
+
+VOID RT2870_BssBeaconExit(
+	IN RTMP_ADAPTER *pAd)
+{
+	BEACON_SYNC_STRUCT	*pBeaconSync;
+	BOOLEAN	Cancelled = TRUE;
+	int i;
+
+	if (pAd->CommonCfg.pBeaconSync)
+	{
+		pBeaconSync = pAd->CommonCfg.pBeaconSync;
+		pBeaconSync->EnableBeacon = FALSE;
+		RTMPCancelTimer(&pAd->CommonCfg.BeaconUpdateTimer, &Cancelled);
+		pBeaconSync->BeaconBitMap = 0;
+
+		for(i=0; i<HW_BEACON_MAX_COUNT; i++)
+		{
+			NdisZeroMemory(pBeaconSync->BeaconBuf[i], HW_BEACON_OFFSET);
+			pBeaconSync->CapabilityInfoLocationInBeacon[i] = 0;
+			pBeaconSync->TimIELocationInBeacon[i] = 0;
+			NdisZeroMemory(pBeaconSync->BeaconTxWI[i], TXWI_SIZE);
+		}
+
+		NdisFreeMemory(pAd->CommonCfg.pBeaconSync, HW_BEACON_OFFSET * HW_BEACON_MAX_COUNT, 0);
+		pAd->CommonCfg.pBeaconSync = NULL;
+	}
+}
+
+VOID BeaconUpdateExec(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3)
+{
+	PRTMP_ADAPTER	pAd = (PRTMP_ADAPTER)FunctionContext;
+	LARGE_INTEGER	tsfTime_a;//, tsfTime_b, deltaTime_exp, deltaTime_ab;
+	UINT32			delta, remain, remain_low, remain_high;
+//	BOOLEAN			positive;
+
+	ReSyncBeaconTime(pAd);
+
+
+
+	RTMP_IO_READ32(pAd, TSF_TIMER_DW0, &tsfTime_a.u.LowPart);
+	RTMP_IO_READ32(pAd, TSF_TIMER_DW1, &tsfTime_a.u.HighPart);
+
+
+	//positive=getDeltaTime(tsfTime_a, expectedTime, &deltaTime_exp);
+	remain_high = pAd->CommonCfg.BeaconRemain * tsfTime_a.u.HighPart;
+	remain_low = tsfTime_a.u.LowPart % (pAd->CommonCfg.BeaconPeriod << 10);
+	remain = (remain_high + remain_low)%(pAd->CommonCfg.BeaconPeriod << 10);
+	delta = (pAd->CommonCfg.BeaconPeriod << 10) - remain;
+
+	pAd->CommonCfg.BeaconUpdateTimer.TimerValue = (delta >> 10) + 10;
+
+}
+
diff --git a/drivers/staging/rt2870/Kconfig b/drivers/staging/rt2870/Kconfig
new file mode 100644
index 00000000000..8398d9797e1
--- /dev/null
+++ b/drivers/staging/rt2870/Kconfig
@@ -0,0 +1,6 @@
+config RT2870
+	tristate "Ralink 2870 wireless support"
+	depends on USB && X86 && WLAN_80211
+	---help---
+	  This is an experimental driver for the Ralink 2870 wireless chip.
+
diff --git a/drivers/staging/rt2870/Makefile b/drivers/staging/rt2870/Makefile
new file mode 100644
index 00000000000..1a015f408e6
--- /dev/null
+++ b/drivers/staging/rt2870/Makefile
@@ -0,0 +1,47 @@
+obj-$(CONFIG_RT2870)	+= rt2870sta.o
+
+# TODO: all of these should be removed
+EXTRA_CFLAGS += -DLINUX -DAGGREGATION_SUPPORT -DPIGGYBACK_SUPPORT -DWMM_SUPPORT
+EXTRA_CFLAGS += -DRT2870
+EXTRA_CFLAGS += -DCONFIG_STA_SUPPORT
+EXTRA_CFLAGS += -DDBG
+EXTRA_CFLAGS += -DDOT11_N_SUPPORT
+EXTRA_CFLAGS += -DWPA_SUPPLICANT_SUPPORT
+EXTRA_CFLAGS += -DNATIVE_WPA_SUPPLICANT_SUPPORT
+
+rt2870sta-objs :=		\
+	common/md5.o		\
+	common/mlme.o		\
+	common/rtmp_wep.o	\
+	common/action.o		\
+	common/cmm_data.o	\
+	common/rtmp_init.o	\
+	common/rtmp_tkip.o	\
+	common/cmm_sync.o	\
+	common/eeprom.o		\
+	common/cmm_sanity.o	\
+	common/cmm_info.o	\
+	common/cmm_wpa.o	\
+	common/dfs.o		\
+	common/spectrum.o	\
+	sta/assoc.o		\
+	sta/aironet.o		\
+	sta/auth.o		\
+	sta/auth_rsp.o		\
+	sta/sync.o		\
+	sta/sanity.o		\
+	sta/rtmp_data.o		\
+	sta/connect.o		\
+	sta/wpa.o		\
+	rt_linux.o		\
+	rt_profile.o		\
+	rt_main_dev.o		\
+	sta_ioctl.o		\
+	common/ba_action.o	\
+	2870_main_dev.o		\
+	common/2870_rtmp_init.o	\
+	common/rtusb_io.o	\
+	common/rtusb_bulk.o	\
+	common/rtusb_data.o	\
+	common/cmm_data_2870.o
+
diff --git a/drivers/staging/rt2870/TODO b/drivers/staging/rt2870/TODO
new file mode 100644
index 00000000000..eae1ac47d3f
--- /dev/null
+++ b/drivers/staging/rt2870/TODO
@@ -0,0 +1,10 @@
+TODO:
+	- checkpatch.pl clean
+	- sparse clean
+	- port to in-kernel 80211 stack
+	- remove reading from /etc/ config files
+	- review by the wireless developer community
+
+Please send any patches or complaints about this driver to Greg
+Kroah-Hartman <greg@kroah.com> and don't bother the upstream wireless
+kernel developers about it, they want nothing to do with it.
diff --git a/drivers/staging/rt2870/aironet.h b/drivers/staging/rt2870/aironet.h
new file mode 100644
index 00000000000..1e07b19b8cd
--- /dev/null
+++ b/drivers/staging/rt2870/aironet.h
@@ -0,0 +1,210 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	aironet.h
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+	Paul Lin	04-06-15		Initial
+*/
+
+#ifndef	__AIRONET_H__
+#define	__AIRONET_H__
+
+// Measurement Type definition
+#define	MSRN_TYPE_UNUSED				0
+#define	MSRN_TYPE_CHANNEL_LOAD_REQ		1
+#define	MSRN_TYPE_NOISE_HIST_REQ		2
+#define	MSRN_TYPE_BEACON_REQ			3
+#define	MSRN_TYPE_FRAME_REQ				4
+
+// Scan Mode in Beacon Request
+#define	MSRN_SCAN_MODE_PASSIVE			0
+#define	MSRN_SCAN_MODE_ACTIVE			1
+#define	MSRN_SCAN_MODE_BEACON_TABLE		2
+
+// PHY type definition for Aironet beacon report, CCX 2 table 36-9
+#define	PHY_FH							1
+#define	PHY_DSS							2
+#define	PHY_UNUSED						3
+#define	PHY_OFDM						4
+#define	PHY_HR_DSS						5
+#define	PHY_ERP							6
+
+// RPI table in dBm
+#define	RPI_0			0			//	Power <= -87
+#define	RPI_1			1			//	-87 < Power <= -82
+#define	RPI_2			2			//	-82 < Power <= -77
+#define	RPI_3			3			//	-77 < Power <= -72
+#define	RPI_4			4			//	-72 < Power <= -67
+#define	RPI_5			5			//	-67 < Power <= -62
+#define	RPI_6			6			//	-62 < Power <= -57
+#define	RPI_7			7			//	-57 < Power
+
+// Cisco Aironet IAPP definetions
+#define	AIRONET_IAPP_TYPE					0x32
+#define	AIRONET_IAPP_SUBTYPE_REQUEST		0x01
+#define	AIRONET_IAPP_SUBTYPE_REPORT			0x81
+
+// Measurement Request detail format
+typedef	struct	_MEASUREMENT_REQUEST	{
+	UCHAR	Channel;
+	UCHAR	ScanMode;			// Use only in beacon request, other requests did not use this field
+	USHORT	Duration;
+}	MEASUREMENT_REQUEST, *PMEASUREMENT_REQUEST;
+
+// Beacon Measurement Report
+// All these field might change to UCHAR, because we didn't do anything to these report.
+// We copy all these beacons and report to CCX 2 AP.
+typedef	struct	_BEACON_REPORT	{
+	UCHAR	Channel;
+	UCHAR	Spare;
+	USHORT	Duration;
+	UCHAR	PhyType;			// Definiation is listed above table 36-9
+	UCHAR	RxPower;
+	UCHAR	BSSID[6];
+	UCHAR	ParentTSF[4];
+	UCHAR	TargetTSF[8];
+	USHORT	BeaconInterval;
+	USHORT	CapabilityInfo;
+}	BEACON_REPORT, *PBEACON_REPORT;
+
+// Frame Measurement Report (Optional)
+typedef	struct	_FRAME_REPORT	{
+	UCHAR	Channel;
+	UCHAR	Spare;
+	USHORT	Duration;
+	UCHAR	TA;
+	UCHAR	BSSID[6];
+	UCHAR	RSSI;
+	UCHAR	Count;
+}	FRAME_REPORT, *PFRAME_REPORT;
+
+#pragma pack(1)
+// Channel Load Report
+typedef	struct	_CHANNEL_LOAD_REPORT	{
+	UCHAR	Channel;
+	UCHAR	Spare;
+	USHORT	Duration;
+	UCHAR	CCABusy;
+}	CHANNEL_LOAD_REPORT, *PCHANNEL_LOAD_REPORT;
+#pragma pack()
+
+// Nosie Histogram Report
+typedef	struct	_NOISE_HIST_REPORT	{
+	UCHAR	Channel;
+	UCHAR	Spare;
+	USHORT	Duration;
+	UCHAR	Density[8];
+}	NOISE_HIST_REPORT, *PNOISE_HIST_REPORT;
+
+// Radio Management Capability element
+typedef	struct	_RADIO_MANAGEMENT_CAPABILITY	{
+	UCHAR	Eid;				// TODO: Why the Eid is 1 byte, not normal 2 bytes???
+	UCHAR	Length;
+	UCHAR	AironetOui[3];		// AIronet OUI (00 40 96)
+	UCHAR	Type;				// Type / Version
+	USHORT	Status;				// swap16 required
+}	RADIO_MANAGEMENT_CAPABILITY, *PRADIO_MANAGEMENT_CAPABILITY;
+
+// Measurement Mode Bit definition
+typedef	struct	_MEASUREMENT_MODE	{
+	UCHAR	Rsvd:4;
+	UCHAR	Report:1;
+	UCHAR	NotUsed:1;
+	UCHAR	Enable:1;
+	UCHAR	Parallel:1;
+}	MEASUREMENT_MODE, *PMEASUREMENT_MODE;
+
+// Measurement Request element, This is little endian mode
+typedef	struct	_MEASUREMENT_REQUEST_ELEMENT	{
+	USHORT				Eid;
+	USHORT				Length;				// swap16 required
+	USHORT				Token;				// non-zero unique token
+	UCHAR				Mode;				// Measurement Mode
+	UCHAR				Type;				// Measurement type
+}	MEASUREMENT_REQUEST_ELEMENT, *PMEASUREMENT_REQUEST_ELEMENT;
+
+// Measurement Report element, This is little endian mode
+typedef	struct	_MEASUREMENT_REPORT_ELEMENT	{
+	USHORT				Eid;
+	USHORT				Length;				// swap16 required
+	USHORT				Token;				// non-zero unique token
+	UCHAR				Mode;				// Measurement Mode
+	UCHAR				Type;				// Measurement type
+}	MEASUREMENT_REPORT_ELEMENT, *PMEASUREMENT_REPORT_ELEMENT;
+
+// Cisco Aironet IAPP Frame Header, Network byte order used
+typedef	struct	_AIRONET_IAPP_HEADER {
+	UCHAR	CiscoSnapHeader[8];	// 8 bytes Cisco snap header
+	USHORT	Length;				// IAPP ID & length, remember to swap16 in LE system
+	UCHAR	Type;				// IAPP type
+	UCHAR	SubType;			// IAPP subtype
+	UCHAR	DA[6];				// Destination MAC address
+	UCHAR	SA[6];				// Source MAC address
+	USHORT	Token;				// Dialog token, no need to swap16 since it is for yoken usage only
+}	AIRONET_IAPP_HEADER, *PAIRONET_IAPP_HEADER;
+
+// Radio Measurement Request frame
+typedef	struct	_AIRONET_RM_REQUEST_FRAME	{
+    AIRONET_IAPP_HEADER	IAPP;			// Common header
+	UCHAR				Delay;			// Activation Delay
+	UCHAR				Offset;			// Measurement offset
+}	AIRONET_RM_REQUEST_FRAME, *PAIRONET_RM_REQUEST_FRAME;
+
+// Radio Measurement Report frame
+typedef	struct	_AIRONET_RM_REPORT_FRAME	{
+    AIRONET_IAPP_HEADER	IAPP;			// Common header
+}	AIRONET_RM_REPORT_FRAME, *PAIRONET_RM_REPORT_FRAME;
+
+// Saved element request actions which will saved in StaCfg.
+typedef	struct	_RM_REQUEST_ACTION	{
+	MEASUREMENT_REQUEST_ELEMENT	ReqElem;		// Saved request element
+	MEASUREMENT_REQUEST			Measurement;	// Saved measurement within the request element
+}	RM_REQUEST_ACTION, *PRM_REQUEST_ACTION;
+
+// CCX administration control
+typedef	union	_CCX_CONTROL	{
+	struct	{
+		UINT32		Enable:1;			// Enable CCX2
+		UINT32		LeapEnable:1;		// Enable LEAP at CCX2
+		UINT32		RMEnable:1;			// Radio Measurement Enable
+		UINT32		DCRMEnable:1;		// Non serving channel Radio Measurement enable
+		UINT32		QOSEnable:1;		// Enable QOS for CCX 2.0 support
+		UINT32		FastRoamEnable:1;	// Enable fast roaming
+		UINT32		Rsvd:2;				// Not used
+		UINT32		dBmToRoam:8;		// the condition to roam when receiving Rssi less than this value. It's negative value.
+		UINT32		TuLimit:16;			// Limit for different channel scan
+	}	field;
+	UINT32			word;
+}	CCX_CONTROL, *PCCX_CONTROL;
+
+#endif	// __AIRONET_H__
diff --git a/drivers/staging/rt2870/ap.h b/drivers/staging/rt2870/ap.h
new file mode 100644
index 00000000000..0dc55751679
--- /dev/null
+++ b/drivers/staging/rt2870/ap.h
@@ -0,0 +1,562 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    ap.h
+
+    Abstract:
+    Miniport generic portion header file
+
+    Revision History:
+    Who         When          What
+    --------    ----------    ----------------------------------------------
+    Paul Lin    08-01-2002    created
+    James Tan   09-06-2002    modified (Revise NTCRegTable)
+    John Chang  12-22-2004    modified for RT2561/2661. merge with STA driver
+*/
+#ifndef __AP_H__
+#define __AP_H__
+
+
+
+// ========================= AP RTMP.h ================================
+
+
+
+// =============================================================
+//      Function Prototypes
+// =============================================================
+
+// ap_data.c
+
+BOOLEAN APBridgeToWirelessSta(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          pHeader,
+    IN  UINT            HdrLen,
+    IN  PUCHAR          pData,
+    IN  UINT            DataLen,
+    IN  ULONG           fromwdsidx);
+
+BOOLEAN APHandleRxDoneInterrupt(
+    IN  PRTMP_ADAPTER   pAd);
+
+VOID	APSendPackets(
+	IN	NDIS_HANDLE		MiniportAdapterContext,
+	IN	PPNDIS_PACKET	ppPacketArray,
+	IN	UINT			NumberOfPackets);
+
+NDIS_STATUS APSendPacket(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PNDIS_PACKET    pPacket);
+
+
+NDIS_STATUS APHardTransmit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	UCHAR			QueIdx);
+
+VOID APRxEAPOLFrameIndicate(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID);
+
+NDIS_STATUS APCheckRxError(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PRT28XX_RXD_STRUC		pRxD,
+	IN	UCHAR			Wcid);
+
+BOOLEAN APCheckClass2Class3Error(
+    IN  PRTMP_ADAPTER   pAd,
+	IN ULONG Wcid,
+	IN  PHEADER_802_11  pHeader);
+
+VOID APHandleRxPsPoll(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pAddr,
+	IN	USHORT			Aid,
+    IN	BOOLEAN			isActive);
+
+VOID    RTMPDescriptorEndianChange(
+    IN  PUCHAR          pData,
+    IN  ULONG           DescriptorType);
+
+VOID    RTMPFrameEndianChange(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          pData,
+    IN  ULONG           Dir,
+    IN  BOOLEAN         FromRxDoneInt);
+
+// ap_assoc.c
+
+VOID APAssocStateMachineInit(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  STATE_MACHINE *S,
+    OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID  APPeerAssocReqAction(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  MLME_QUEUE_ELEM *Elem);
+
+VOID  APPeerReassocReqAction(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  MLME_QUEUE_ELEM *Elem);
+
+VOID  APPeerDisassocReqAction(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  MLME_QUEUE_ELEM *Elem);
+
+VOID MbssKickOutStas(
+	IN PRTMP_ADAPTER pAd,
+	IN INT apidx,
+	IN USHORT Reason);
+
+VOID APMlmeKickOutSta(
+    IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pStaAddr,
+	IN UCHAR Wcid,
+	IN USHORT Reason);
+
+VOID APMlmeDisassocReqAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID  APCls3errAction(
+    IN  PRTMP_ADAPTER   pAd,
+	IN 	ULONG Wcid,
+    IN	PHEADER_802_11	pHeader);
+
+
+USHORT APBuildAssociation(
+    IN PRTMP_ADAPTER pAd,
+    IN MAC_TABLE_ENTRY *pEntry,
+    IN USHORT CapabilityInfo,
+    IN UCHAR  MaxSupportedRateIn500Kbps,
+    IN UCHAR  *RSN,
+    IN UCHAR  *pRSNLen,
+    IN BOOLEAN bWmmCapable,
+    IN ULONG  RalinkIe,
+#ifdef DOT11N_DRAFT3
+    IN EXT_CAP_INFO_ELEMENT ExtCapInfo,
+#endif // DOT11N_DRAFT3 //
+	IN HT_CAPABILITY_IE		*pHtCapability,
+	IN UCHAR		 HtCapabilityLen,
+    OUT USHORT *pAid);
+
+/*
+VOID	RTMPAddClientSec(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR	BssIdx,
+	IN UCHAR		 KeyIdx,
+	IN UCHAR		 CipherAlg,
+	IN PUCHAR		 pKey,
+	IN PUCHAR		 pTxMic,
+	IN PUCHAR		 pRxMic,
+	IN MAC_TABLE_ENTRY *pEntry);
+*/
+
+// ap_auth.c
+
+void APAuthStateMachineInit(
+    IN PRTMP_ADAPTER pAd,
+    IN STATE_MACHINE *Sm,
+    OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID APMlmeDeauthReqAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID APCls2errAction(
+    IN PRTMP_ADAPTER pAd,
+	IN 	ULONG Wcid,
+    IN	PHEADER_802_11	pHeader);
+
+// ap_authrsp.c
+
+VOID APAuthRspStateMachineInit(
+    IN PRTMP_ADAPTER pAd,
+    IN PSTATE_MACHINE Sm,
+    IN STATE_MACHINE_FUNC Trans[]);
+
+VOID APPeerAuthAtAuthRspIdleAction(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  MLME_QUEUE_ELEM *Elem);
+
+VOID APPeerDeauthReqAction(
+    IN PRTMP_ADAPTER	pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID APPeerAuthSimpleRspGenAndSend(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PHEADER_802_11 pHdr80211,
+    IN  USHORT Alg,
+    IN  USHORT Seq,
+    IN  USHORT StatusCode);
+
+// ap_connect.c
+
+BOOLEAN BeaconTransmitRequired(
+	IN PRTMP_ADAPTER	pAd,
+	IN INT				apidx);
+
+VOID APMakeBssBeacon(
+    IN  PRTMP_ADAPTER   pAd,
+	IN	INT				apidx);
+
+VOID  APUpdateBeaconFrame(
+    IN  PRTMP_ADAPTER   pAd,
+	IN	INT				apidx);
+
+VOID APMakeAllBssBeacon(
+    IN  PRTMP_ADAPTER   pAd);
+
+VOID  APUpdateAllBeaconFrame(
+    IN  PRTMP_ADAPTER   pAd);
+
+
+// ap_sync.c
+
+VOID APSyncStateMachineInit(
+    IN PRTMP_ADAPTER pAd,
+    IN STATE_MACHINE *Sm,
+    OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID APScanTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID APInvalidStateWhenScan(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID APScanTimeoutAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID APPeerProbeReqAction(
+    IN  PRTMP_ADAPTER pAd,
+    IN  MLME_QUEUE_ELEM *Elem);
+
+VOID APPeerBeaconAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID APMlmeScanReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID APPeerBeaconAtScanAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID APScanCnclAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID ApSiteSurvey(
+	IN PRTMP_ADAPTER pAd);
+
+VOID SupportRate(
+	IN PUCHAR SupRate,
+	IN UCHAR SupRateLen,
+	IN PUCHAR ExtRate,
+	IN UCHAR ExtRateLen,
+	OUT PUCHAR *Rates,
+	OUT PUCHAR RatesLen,
+	OUT PUCHAR pMaxSupportRate);
+
+
+BOOLEAN ApScanRunning(
+	IN PRTMP_ADAPTER pAd);
+
+#ifdef DOT11N_DRAFT3
+VOID APOverlappingBSSScan(
+	IN RTMP_ADAPTER *pAd);
+#endif // DOT11N_DRAFT3 //
+
+// ap_wpa.c
+
+VOID APWpaStateMachineInit(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  STATE_MACHINE *Sm,
+    OUT STATE_MACHINE_FUNC Trans[]);
+
+// ap_mlme.c
+
+VOID APMlmePeriodicExec(
+    IN  PRTMP_ADAPTER   pAd);
+
+VOID APMlmeSelectTxRateTable(
+	IN PRTMP_ADAPTER		pAd,
+	IN PMAC_TABLE_ENTRY		pEntry,
+	IN PUCHAR				*ppTable,
+	IN PUCHAR				pTableSize,
+	IN PUCHAR				pInitTxRateIdx);
+
+VOID APMlmeSetTxRate(
+	IN PRTMP_ADAPTER		pAd,
+	IN PMAC_TABLE_ENTRY		pEntry,
+	IN PRTMP_TX_RATE_SWITCH	pTxRate);
+
+VOID APMlmeDynamicTxRateSwitching(
+    IN PRTMP_ADAPTER pAd);
+
+VOID APQuickResponeForRateUpExec(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3);
+
+BOOLEAN APMsgTypeSubst(
+    IN PRTMP_ADAPTER pAd,
+    IN PFRAME_802_11 pFrame,
+    OUT INT *Machine,
+    OUT INT *MsgType);
+
+VOID APQuickResponeForRateUpExec(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3);
+
+#ifdef RT2870
+VOID BeaconUpdateExec(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3);
+#endif // RT2870 //
+
+VOID RTMPSetPiggyBack(
+	IN PRTMP_ADAPTER	pAd,
+	IN BOOLEAN			bPiggyBack);
+
+VOID APAsicEvaluateRxAnt(
+	IN PRTMP_ADAPTER	pAd);
+
+VOID APAsicRxAntEvalTimeout(
+	IN PRTMP_ADAPTER	pAd);
+
+// ap.c
+
+VOID APSwitchChannel(
+	IN PRTMP_ADAPTER pAd,
+	IN INT Channel);
+
+NDIS_STATUS APInitialize(
+    IN  PRTMP_ADAPTER   pAd);
+
+VOID APShutdown(
+    IN PRTMP_ADAPTER    pAd);
+
+VOID APStartUp(
+    IN  PRTMP_ADAPTER   pAd);
+
+VOID APStop(
+    IN  PRTMP_ADAPTER   pAd);
+
+VOID APCleanupPsQueue(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PQUEUE_HEADER   pQueue);
+
+VOID MacTableReset(
+    IN  PRTMP_ADAPTER   pAd);
+
+MAC_TABLE_ENTRY *MacTableInsertEntry(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          pAddr,
+	IN	UCHAR			apidx,
+	IN BOOLEAN	CleanAll);
+
+BOOLEAN MacTableDeleteEntry(
+    IN  PRTMP_ADAPTER   pAd,
+	IN USHORT wcid,
+    IN  PUCHAR          pAddr);
+
+MAC_TABLE_ENTRY *MacTableLookup(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          pAddr);
+
+VOID MacTableMaintenance(
+    IN PRTMP_ADAPTER pAd);
+
+UINT32 MacTableAssocStaNumGet(
+	IN PRTMP_ADAPTER pAd);
+
+MAC_TABLE_ENTRY *APSsPsInquiry(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          pAddr,
+    OUT SST             *Sst,
+    OUT USHORT          *Aid,
+    OUT UCHAR           *PsMode,
+    OUT UCHAR           *Rate);
+
+BOOLEAN APPsIndicate(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          pAddr,
+	IN ULONG Wcid,
+    IN  UCHAR           Psm);
+
+VOID ApLogEvent(
+    IN PRTMP_ADAPTER    pAd,
+    IN PUCHAR           pAddr,
+    IN USHORT           Event);
+
+#ifdef DOT11_N_SUPPORT
+VOID APUpdateOperationMode(
+    IN PRTMP_ADAPTER pAd);
+#endif // DOT11_N_SUPPORT //
+
+VOID APUpdateCapabilityAndErpIe(
+	IN PRTMP_ADAPTER pAd);
+
+BOOLEAN ApCheckAccessControlList(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR        pAddr,
+	IN UCHAR         Apidx);
+
+VOID ApUpdateAccessControlList(
+    IN PRTMP_ADAPTER pAd,
+    IN UCHAR         Apidx);
+
+VOID ApEnqueueNullFrame(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR        pAddr,
+	IN UCHAR         TxRate,
+	IN UCHAR         PID,
+	IN UCHAR         apidx,
+    IN BOOLEAN       bQosNull,
+    IN BOOLEAN       bEOSP,
+    IN UCHAR         OldUP);
+
+VOID ApSendFrame(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PVOID           pBuffer,
+    IN  ULONG           Length,
+    IN  UCHAR           TxRate,
+    IN  UCHAR           PID);
+
+VOID ApEnqueueAckFrame(
+    IN PRTMP_ADAPTER pAd,
+    IN PUCHAR        pAddr,
+    IN UCHAR         TxRate,
+	IN UCHAR         apidx);
+
+UCHAR APAutoSelectChannel(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN Optimal);
+
+// ap_sanity.c
+
+
+BOOLEAN PeerAssocReqCmmSanity(
+    IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN isRessoc,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT USHORT *pCapabilityInfo,
+    OUT USHORT *pListenInterval,
+    OUT PUCHAR pApAddr,
+    OUT UCHAR *pSsidLen,
+    OUT char *Ssid,
+    OUT UCHAR *pRatesLen,
+    OUT UCHAR Rates[],
+    OUT UCHAR *RSN,
+    OUT UCHAR *pRSNLen,
+    OUT BOOLEAN *pbWmmCapable,
+#ifdef WSC_AP_SUPPORT
+    OUT BOOLEAN *pWscCapable,
+#endif // WSC_AP_SUPPORT //
+    OUT ULONG  *pRalinkIe,
+#ifdef DOT11N_DRAFT3
+    OUT EXT_CAP_INFO_ELEMENT	*pExtCapInfo,
+#endif // DOT11N_DRAFT3 //
+    OUT UCHAR		 *pHtCapabilityLen,
+    OUT HT_CAPABILITY_IE *pHtCapability);
+
+BOOLEAN PeerDisassocReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT USHORT *Reason);
+
+BOOLEAN PeerDeauthReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT USHORT *Reason);
+
+BOOLEAN APPeerAuthSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+	OUT PUCHAR pAddr1,
+    OUT PUCHAR pAddr2,
+    OUT USHORT *Alg,
+    OUT USHORT *Seq,
+    OUT USHORT *Status,
+    CHAR *ChlgText);
+
+BOOLEAN APPeerProbeReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT CHAR Ssid[],
+    OUT UCHAR *SsidLen);
+
+BOOLEAN APPeerBeaconAndProbeRspSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT PUCHAR pBssid,
+    OUT CHAR Ssid[],
+    OUT UCHAR *SsidLen,
+    OUT UCHAR *BssType,
+    OUT USHORT *BeaconPeriod,
+    OUT UCHAR *Channel,
+    OUT LARGE_INTEGER *Timestamp,
+    OUT USHORT *CapabilityInfo,
+    OUT UCHAR Rate[],
+    OUT UCHAR *RateLen,
+    OUT BOOLEAN *ExtendedRateIeExist,
+    OUT UCHAR *Erp);
+
+// ap_info.c
+
+
+
+// ================== end of AP RTMP.h ========================
+
+
+#endif  // __AP_H__
+
diff --git a/drivers/staging/rt2870/chlist.h b/drivers/staging/rt2870/chlist.h
new file mode 100644
index 00000000000..9e15b9daeb8
--- /dev/null
+++ b/drivers/staging/rt2870/chlist.h
@@ -0,0 +1,1296 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	chlist.c
+
+	Abstract:
+
+	Revision History:
+	Who         When          What
+	--------    ----------    ----------------------------------------------
+	Fonchi Wu   2007-12-19    created
+*/
+
+#ifndef __CHLIST_H__
+#define __CHLIST_H__
+
+#include "rtmp_type.h"
+#include "rtmp_def.h"
+
+
+#define ODOR			0
+#define IDOR			1
+#define BOTH			2
+
+#define BAND_5G         0
+#define BAND_24G        1
+#define BAND_BOTH       2
+
+typedef struct _CH_DESP {
+	UCHAR FirstChannel;
+	UCHAR NumOfCh;
+	CHAR MaxTxPwr;			// dBm
+	UCHAR Geography;			// 0:out door, 1:in door, 2:both
+	BOOLEAN DfsReq;			// Dfs require, 0: No, 1: yes.
+} CH_DESP, *PCH_DESP;
+
+typedef struct _CH_REGION {
+	UCHAR CountReg[3];
+	UCHAR DfsType;			// 0: CE, 1: FCC, 2: JAP, 3:JAP_W53, JAP_W56
+	CH_DESP ChDesp[10];
+} CH_REGION, *PCH_REGION;
+
+static CH_REGION ChRegion[] =
+{
+		{	// Antigua and Berbuda
+			"AG",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  23, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100, 11, 30, BOTH, FALSE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Argentina
+			"AR",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 52,  4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149, 4,  30, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Aruba
+			"AW",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  23, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100, 11, 30, BOTH, FALSE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Australia
+			"AU",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149, 5,  30, BOTH, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// Austria
+			"AT",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, IDOR, TRUE},		// 5G, ch 36~48
+				{ 52,  4,  23, IDOR, TRUE},		// 5G, ch 52~64
+				{ 100, 11, 30, BOTH, TRUE},		// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Bahamas
+			"BS",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149, 5,  30, BOTH, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// Barbados
+			"BB",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100, 11, 30, BOTH, FALSE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Bermuda
+			"BM",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100, 11, 30, BOTH, FALSE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Brazil
+			"BR",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100, 11, 24, BOTH, FALSE},	// 5G, ch 100~140
+				{ 149, 5,  30, BOTH, FALSE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Belgium
+			"BE",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  18, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  18, IDOR, FALSE},	// 5G, ch 52~64
+				{ 0},							// end
+			}
+		},
+
+		{	// Bulgaria
+			"BG",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11, 30, ODOR, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Canada
+			"CA",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  23, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149, 5,  30, BOTH, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// Cayman IsLands
+			"KY",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100, 11, 30, BOTH, FALSE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Chile
+			"CL",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  20, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  20, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149, 5,  20, BOTH, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// China
+			"CN",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 149, 4,  27, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Colombia
+			"CO",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  17, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100, 11, 30, BOTH, FALSE},	// 5G, ch 100~140
+				{ 149, 5,  30, BOTH, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// Costa Rica
+			"CR",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  17, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149, 4,  30, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Cyprus
+			"CY",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  24, IDOR, TRUE},		// 5G, ch 52~64
+				{ 100, 11, 30, BOTH, TRUE},		// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Czech_Republic
+			"CZ",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  23, IDOR, TRUE},		// 5G, ch 52~64
+				{ 0},							// end
+			}
+		},
+
+		{	// Denmark
+			"DK",
+			CE,
+			{
+				{ 1,   13, 20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,  23, IDOR, TRUE},		// 5G, ch 52~64
+				{ 100, 11, 30, BOTH, TRUE},		// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Dominican Republic
+			"DO",
+			CE,
+			{
+				{ 1,   0,  20, BOTH, FALSE},	// 2.4 G, ch 0
+				{ 149, 4,  20, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Equador
+			"EC",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 100, 11,  27, BOTH, FALSE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// El Salvador
+			"SV",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,   23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,   30, BOTH, TRUE},	// 5G, ch 52~64
+				{ 149, 4,   36, BOTH, TRUE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// Finland
+			"FI",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,   23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,   23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// France
+			"FR",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,   23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,   23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 0},							// end
+			}
+		},
+
+		{	// Germany
+			"DE",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,   23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,   23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Greece
+			"GR",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,   23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,  4,   23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, ODOR, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Guam
+			"GU",
+			CE,
+			{
+				{ 1,   11,  20, BOTH, FALSE},	// 2.4 G, ch 1~11
+				{ 36,  4,   17, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,   24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, FALSE},	// 5G, ch 100~140
+				{ 149,  5,  30, BOTH, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// Guatemala
+			"GT",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,   17, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,   24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149,  4,  30, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Haiti
+			"HT",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,  4,   17, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,  4,   24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149,  4,  30, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Honduras
+			"HN",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 149,  4,  27, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Hong Kong
+			"HK",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, FALSE},	// 5G, ch 52~64
+				{ 149,  4,  30, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Hungary
+			"HU",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 0},							// end
+			}
+		},
+
+		{	// Iceland
+			"IS",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// India
+			"IN",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 149, 	4,  24, IDOR, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Indonesia
+			"ID",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 149, 	4,  27, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Ireland
+			"IE",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, ODOR, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Israel
+			"IL",
+			CE,
+			{
+				{ 1,    3,  20, IDOR, FALSE},	// 2.4 G, ch 1~3
+				{ 4, 	6,  20, BOTH, FALSE},	// 2.4 G, ch 4~9
+				{ 10, 	4,  20, IDOR, FALSE},	// 2.4 G, ch 10~13
+				{ 0},							// end
+			}
+		},
+
+		{	// Italy
+			"IT",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, ODOR, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Japan
+			"JP",
+			JAP,
+			{
+				{ 1,   14,  20, BOTH, FALSE},	// 2.4 G, ch 1~14
+				{ 36, 	4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 0},							// end
+			}
+		},
+
+		{	// Jordan
+			"JO",
+			CE,
+			{
+				{ 1,   13,  20, IDOR, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 149, 	4,  23, IDOR, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Latvia
+			"LV",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Liechtenstein
+			"LI",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 52, 	4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Lithuania
+			"LT",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Luxemburg
+			"LU",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Malaysia
+			"MY",
+			CE,
+			{
+				{ 36, 	4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  23, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149,  5,  20, BOTH, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// Malta
+			"MT",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Marocco
+			"MA",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  24, IDOR, FALSE},	// 5G, ch 36~48
+				{ 0},							// end
+			}
+		},
+
+		{	// Mexico
+			"MX",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149,  5,  30, IDOR, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// Netherlands
+			"NL",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  24, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// New Zealand
+			"NZ",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  24, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  24, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149,  4,  30, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Norway
+			"NO",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36, 	4,  24, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52, 	4,  24, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Peru
+			"PE",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 149,  4,  27, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Portugal
+			"PT",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Poland
+			"PL",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Romania
+			"RO",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Russia
+			"RU",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 149,  4,  20, IDOR, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Saudi Arabia
+			"SA",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149,  4,  23, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Serbia_and_Montenegro
+			"CS",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 0},							// end
+			}
+		},
+
+		{	// Singapore
+			"SG",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, BOTH, FALSE},	// 5G, ch 52~64
+				{ 149,  4,  20, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Slovakia
+			"SK",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Slovenia
+			"SI",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 0},							// end
+			}
+		},
+
+		{	// South Africa
+			"ZA",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, FALSE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 149,  4,  30, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// South Korea
+			"KR",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  20, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  20, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100,  8,  20, BOTH, FALSE},	// 5G, ch 100~128
+				{ 149,  4,  20, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Spain
+			"ES",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  17, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Sweden
+			"SE",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Switzerland
+			"CH",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~13
+				{ 36,   4,  23, IDOR, TRUE},	// 5G, ch 36~48
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 0},							// end
+			}
+		},
+
+		{	// Taiwan
+			"TW",
+			CE,
+			{
+				{ 1,   11,  30, BOTH, FALSE},	// 2.4 G, ch 1~11
+				{ 52,   4,  23, IDOR, FALSE},	// 5G, ch 52~64
+				{ 0},							// end
+			}
+		},
+
+		{	// Turkey
+			"TR",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~11
+				{ 36,   4,  23, BOTH, FALSE},	// 5G, ch 36~48
+				{ 52,   4,  23, BOTH, FALSE},	// 5G, ch 52~64
+				{ 0},							// end
+			}
+		},
+
+		{	// UK
+			"GB",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~11
+				{ 36,   4,  23, IDOR, FALSE},	// 5G, ch 52~64
+				{ 52,   4,  23, IDOR, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 0},							// end
+			}
+		},
+
+		{	// Ukraine
+			"UA",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~11
+				{ 0},							// end
+			}
+		},
+
+		{	// United_Arab_Emirates
+			"AE",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~11
+				{ 0},							// end
+			}
+		},
+
+		{	// United_States
+			"US",
+			CE,
+			{
+				{ 1,   11,  30, BOTH, FALSE},	// 2.4 G, ch 1~11
+				{ 36,   4,  17, IDOR, FALSE},	// 5G, ch 52~64
+				{ 52,   4,  24, BOTH, TRUE},	// 5G, ch 52~64
+				{ 100, 11,  30, BOTH, TRUE},	// 5G, ch 100~140
+				{ 149,  5,  30, BOTH, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+
+		{	// Venezuela
+			"VE",
+			CE,
+			{
+				{ 1,   13,  20, BOTH, FALSE},	// 2.4 G, ch 1~11
+				{ 149,  4,  27, BOTH, FALSE},	// 5G, ch 149~161
+				{ 0},							// end
+			}
+		},
+
+		{	// Default
+			"",
+			CE,
+			{
+				{ 1,   11,  20, BOTH, FALSE},	// 2.4 G, ch 1~11
+				{ 36,   4,  20, BOTH, FALSE},	// 5G, ch 52~64
+				{ 52,   4,  20, BOTH, FALSE},	// 5G, ch 52~64
+				{ 100, 11,  20, BOTH, FALSE},	// 5G, ch 100~140
+				{ 149,  5,  20, BOTH, FALSE},	// 5G, ch 149~165
+				{ 0},							// end
+			}
+		},
+};
+
+static inline PCH_REGION GetChRegion(
+	IN PUCHAR CntryCode)
+{
+	INT loop = 0;
+	PCH_REGION pChRegion = NULL;
+
+	while (strcmp(ChRegion[loop].CountReg, "") != 0)
+	{
+		if (strncmp(ChRegion[loop].CountReg, CntryCode, 2) == 0)
+		{
+			pChRegion = &ChRegion[loop];
+			break;
+		}
+		loop++;
+	}
+
+	if (pChRegion == NULL)
+		pChRegion = &ChRegion[loop];
+	return pChRegion;
+}
+
+static inline VOID ChBandCheck(
+	IN UCHAR PhyMode,
+	OUT PUCHAR pChType)
+{
+	switch(PhyMode)
+	{
+		case PHY_11A:
+#ifdef DOT11_N_SUPPORT
+		case PHY_11AN_MIXED:
+#endif // DOT11_N_SUPPORT //
+			*pChType = BAND_5G;
+			break;
+		case PHY_11ABG_MIXED:
+#ifdef DOT11_N_SUPPORT
+		case PHY_11AGN_MIXED:
+		case PHY_11ABGN_MIXED:
+#endif // DOT11_N_SUPPORT //
+			*pChType = BAND_BOTH;
+			break;
+
+		default:
+			*pChType = BAND_24G;
+			break;
+	}
+}
+
+static inline UCHAR FillChList(
+	IN PRTMP_ADAPTER pAd,
+	IN PCH_DESP pChDesp,
+	IN UCHAR Offset,
+	IN UCHAR increment)
+{
+	INT i, j, l;
+	UCHAR channel;
+
+	j = Offset;
+	for (i = 0; i < pChDesp->NumOfCh; i++)
+	{
+		channel = pChDesp->FirstChannel + i * increment;
+		for (l=0; l<MAX_NUM_OF_CHANNELS; l++)
+		{
+			if (channel == pAd->TxPower[l].Channel)
+			{
+				pAd->ChannelList[j].Power = pAd->TxPower[l].Power;
+				pAd->ChannelList[j].Power2 = pAd->TxPower[l].Power2;
+				break;
+			}
+		}
+		if (l == MAX_NUM_OF_CHANNELS)
+			continue;
+
+		pAd->ChannelList[j].Channel = pChDesp->FirstChannel + i * increment;
+		pAd->ChannelList[j].MaxTxPwr = pChDesp->MaxTxPwr;
+		pAd->ChannelList[j].DfsReq = pChDesp->DfsReq;
+		j++;
+	}
+	pAd->ChannelListNum = j;
+
+	return j;
+}
+
+static inline VOID CreateChList(
+	IN PRTMP_ADAPTER pAd,
+	IN PCH_REGION pChRegion,
+	IN UCHAR Geography)
+{
+	INT i;
+	UCHAR offset = 0;
+	PCH_DESP pChDesp;
+	UCHAR ChType;
+	UCHAR increment;
+
+	if (pChRegion == NULL)
+		return;
+
+	ChBandCheck(pAd->CommonCfg.PhyMode, &ChType);
+
+	for (i=0; i<10; i++)
+	{
+		pChDesp = &pChRegion->ChDesp[i];
+		if (pChDesp->FirstChannel == 0)
+			break;
+
+		if (ChType == BAND_5G)
+		{
+			if (pChDesp->FirstChannel <= 14)
+				continue;
+		}
+		else if (ChType == BAND_24G)
+		{
+			if (pChDesp->FirstChannel > 14)
+				continue;
+		}
+
+		if ((pChDesp->Geography == BOTH)
+			|| (pChDesp->Geography == Geography))
+        {
+			if (pChDesp->FirstChannel > 14)
+                increment = 4;
+            else
+                increment = 1;
+			offset = FillChList(pAd, pChDesp, offset, increment);
+        }
+	}
+}
+
+static inline VOID BuildChannelListEx(
+	IN PRTMP_ADAPTER pAd)
+{
+	PCH_REGION pChReg;
+
+	pChReg = GetChRegion(pAd->CommonCfg.CountryCode);
+	CreateChList(pAd, pChReg, pAd->CommonCfg.Geography);
+}
+
+static inline VOID BuildBeaconChList(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pBuf,
+	OUT	PULONG pBufLen)
+{
+	INT i;
+	ULONG TmpLen;
+	PCH_REGION pChRegion;
+	PCH_DESP pChDesp;
+	UCHAR ChType;
+
+	pChRegion = GetChRegion(pAd->CommonCfg.CountryCode);
+
+	if (pChRegion == NULL)
+		return;
+
+	ChBandCheck(pAd->CommonCfg.PhyMode, &ChType);
+	*pBufLen = 0;
+
+	for (i=0; i<10; i++)
+	{
+		pChDesp = &pChRegion->ChDesp[i];
+		if (pChDesp->FirstChannel == 0)
+			break;
+
+		if (ChType == BAND_5G)
+		{
+			if (pChDesp->FirstChannel <= 14)
+				continue;
+		}
+		else if (ChType == BAND_24G)
+		{
+			if (pChDesp->FirstChannel > 14)
+				continue;
+		}
+
+		if ((pChDesp->Geography == BOTH)
+			|| (pChDesp->Geography == pAd->CommonCfg.Geography))
+		{
+			MakeOutgoingFrame(pBuf + *pBufLen,		&TmpLen,
+								1,                 	&pChDesp->FirstChannel,
+								1,                 	&pChDesp->NumOfCh,
+								1,                 	&pChDesp->MaxTxPwr,
+								END_OF_ARGS);
+			*pBufLen += TmpLen;
+		}
+	}
+}
+
+
+#ifdef DOT11_N_SUPPORT
+static inline BOOLEAN IsValidChannel(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR channel)
+
+{
+	INT i;
+
+	for (i = 0; i < pAd->ChannelListNum; i++)
+	{
+		if (pAd->ChannelList[i].Channel == channel)
+			break;
+	}
+
+	if (i == pAd->ChannelListNum)
+		return FALSE;
+	else
+		return TRUE;
+}
+
+
+static inline UCHAR GetExtCh(
+	IN UCHAR Channel,
+	IN UCHAR Direction)
+{
+	CHAR ExtCh;
+
+	if (Direction == EXTCHA_ABOVE)
+		ExtCh = Channel + 4;
+	else
+		ExtCh = (Channel - 4) > 0 ? (Channel - 4) : 0;
+
+	return ExtCh;
+}
+
+
+static inline VOID N_ChannelCheck(
+	IN PRTMP_ADAPTER pAd)
+{
+	//UCHAR ChannelNum = pAd->ChannelListNum;
+	UCHAR Channel = pAd->CommonCfg.Channel;
+
+	if ((pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED) && (pAd->CommonCfg.RegTransmitSetting.field.BW  == BW_40))
+	{
+		if (Channel > 14)
+		{
+			if ((Channel == 36) || (Channel == 44) || (Channel == 52) || (Channel == 60) || (Channel == 100) || (Channel == 108) ||
+			    (Channel == 116) || (Channel == 124) || (Channel == 132) || (Channel == 149) || (Channel == 157))
+			{
+				pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_ABOVE;
+			}
+			else if ((Channel == 40) || (Channel == 48) || (Channel == 56) || (Channel == 64) || (Channel == 104) || (Channel == 112) ||
+					(Channel == 120) || (Channel == 128) || (Channel == 136) || (Channel == 153) || (Channel == 161))
+			{
+				pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_BELOW;
+			}
+			else
+			{
+				pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_20;
+			}
+		}
+		else
+		{
+			do
+			{
+				UCHAR ExtCh;
+				UCHAR Dir = pAd->CommonCfg.RegTransmitSetting.field.EXTCHA;
+				ExtCh = GetExtCh(Channel, Dir);
+				if (IsValidChannel(pAd, ExtCh))
+					break;
+
+				Dir = (Dir == EXTCHA_ABOVE) ? EXTCHA_BELOW : EXTCHA_ABOVE;
+				ExtCh = GetExtCh(Channel, Dir);
+				if (IsValidChannel(pAd, ExtCh))
+				{
+					pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = Dir;
+					break;
+				}
+				pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_20;
+			} while(FALSE);
+
+			if (Channel == 14)
+			{
+				pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_20;
+				//pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_NONE;	// We didn't set the ExtCh as NONE due to it'll set in RTMPSetHT()
+			}
+#if 0
+			switch (pAd->CommonCfg.CountryRegion  & 0x7f)
+			{
+				case REGION_0_BG_BAND:	// 1 -11
+				case REGION_1_BG_BAND:	// 1 - 13
+				case REGION_5_BG_BAND:	// 1 - 14
+					if (Channel <= 4)
+					{
+						pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_ABOVE;
+					}
+					else if (Channel >= 8)
+					{
+						if ((ChannelNum - Channel) < 4)
+							pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_BELOW;
+					}
+					break;
+
+				case REGION_2_BG_BAND:	// 10 - 11
+				case REGION_3_BG_BAND:	// 10 - 13
+				case REGION_4_BG_BAND:	// 14
+					pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_20;
+					break;
+
+				case REGION_6_BG_BAND:	// 3 - 9
+					if (Channel <= 5)
+						pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_ABOVE;
+					else if (Channel == 6)
+						pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_20;
+					else if (Channel >= 7)
+						pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_BELOW;
+					break;
+
+				case REGION_7_BG_BAND:  // 5 - 13
+					if (Channel <= 8)
+						pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_ABOVE;
+					else if (Channel >= 10)
+						pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_BELOW;
+					break;
+
+				default:	// Error. should never happen
+					break;
+			}
+#endif
+		}
+	}
+
+
+}
+
+
+static inline VOID N_SetCenCh(
+	IN PRTMP_ADAPTER pAd)
+{
+	if (pAd->CommonCfg.RegTransmitSetting.field.BW == BW_40)
+	{
+		if (pAd->CommonCfg.RegTransmitSetting.field.EXTCHA == EXTCHA_ABOVE)
+		{
+			pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel + 2;
+		}
+		else
+		{
+			if (pAd->CommonCfg.Channel == 14)
+				pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel - 1;
+			else
+				pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel - 2;
+		}
+	}
+	else
+	{
+		pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel;
+	}
+}
+#endif // DOT11_N_SUPPORT //
+
+
+static inline UINT8 GetCuntryMaxTxPwr(
+	IN PRTMP_ADAPTER pAd,
+	IN UINT8 channel)
+{
+	int i;
+	for (i = 0; i < pAd->ChannelListNum; i++)
+	{
+		if (pAd->ChannelList[i].Channel == channel)
+			break;
+	}
+
+	if (i == pAd->ChannelListNum)
+		return 0xff;
+	else
+		return pAd->ChannelList[i].MaxTxPwr;
+}
+#endif // __CHLIST_H__
+
diff --git a/drivers/staging/rt2870/common/2870_rtmp_init.c b/drivers/staging/rt2870/common/2870_rtmp_init.c
new file mode 100644
index 00000000000..5d89a310d1e
--- /dev/null
+++ b/drivers/staging/rt2870/common/2870_rtmp_init.c
@@ -0,0 +1,1778 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	2870_rtmp_init.c
+
+	Abstract:
+	Miniport generic portion header file
+
+	Revision History:
+	Who         When          What
+	--------    ----------    ----------------------------------------------
+	Paul Lin    2002-08-01    created
+    John Chang  2004-08-20    RT2561/2661 use scatter-gather scheme
+    Jan Lee  	2006-09-15    RT2860. Change for 802.11n , EEPROM, Led, BA, HT.
+	Sample Lin	2007-05-31    Merge RT2860 and RT2870 drivers.
+*/
+
+#include "../rt_config.h"
+
+
+static void rx_done_tasklet(unsigned long data);
+static void rt2870_hcca_dma_done_tasklet(unsigned long data);
+static void rt2870_ac3_dma_done_tasklet(unsigned long data);
+static void rt2870_ac2_dma_done_tasklet(unsigned long data);
+static void rt2870_ac1_dma_done_tasklet(unsigned long data);
+static void rt2870_ac0_dma_done_tasklet(unsigned long data);
+static void rt2870_mgmt_dma_done_tasklet(unsigned long data);
+static void rt2870_null_frame_complete_tasklet(unsigned long data);
+static void rt2870_rts_frame_complete_tasklet(unsigned long data);
+static void rt2870_pspoll_frame_complete_tasklet(unsigned long data);
+static void rt2870_dataout_complete_tasklet(unsigned long data);
+
+
+/*
+========================================================================
+Routine Description:
+    Initialize receive data structures.
+
+Arguments:
+    pAd					Pointer to our adapter
+
+Return Value:
+	NDIS_STATUS_SUCCESS
+	NDIS_STATUS_RESOURCES
+
+Note:
+	Initialize all receive releated private buffer, include those define
+	in RTMP_ADAPTER structure and all private data structures. The mahor
+	work is to allocate buffer for each packet and chain buffer to
+	NDIS packet descriptor.
+========================================================================
+*/
+NDIS_STATUS	NICInitRecv(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	UCHAR				i;
+	NDIS_STATUS			Status = NDIS_STATUS_SUCCESS;
+	POS_COOKIE			pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> NICInitRecv\n"));
+	pObj = pObj;
+
+	//InterlockedExchange(&pAd->PendingRx, 0);
+	pAd->PendingRx = 0;
+	pAd->NextRxBulkInReadIndex 	= 0;	// Next Rx Read index
+	pAd->NextRxBulkInIndex		= 0 ; //RX_RING_SIZE -1; // Rx Bulk pointer
+	pAd->NextRxBulkInPosition 	= 0;
+
+	for (i = 0; i < (RX_RING_SIZE); i++)
+	{
+		PRX_CONTEXT  pRxContext = &(pAd->RxContext[i]);
+
+		//Allocate URB
+		pRxContext->pUrb = RTUSB_ALLOC_URB(0);
+		if (pRxContext->pUrb == NULL)
+		{
+			Status = NDIS_STATUS_RESOURCES;
+			goto out1;
+		}
+
+		// Allocate transfer buffer
+		pRxContext->TransferBuffer = RTUSB_URB_ALLOC_BUFFER(pObj->pUsb_Dev, MAX_RXBULK_SIZE, &pRxContext->data_dma);
+		if (pRxContext->TransferBuffer == NULL)
+		{
+			Status = NDIS_STATUS_RESOURCES;
+			goto out1;
+		}
+
+		NdisZeroMemory(pRxContext->TransferBuffer, MAX_RXBULK_SIZE);
+
+		pRxContext->pAd	= pAd;
+		pRxContext->pIrp = NULL;
+		pRxContext->InUse		= FALSE;
+		pRxContext->IRPPending	= FALSE;
+		pRxContext->Readable	= FALSE;
+		//pRxContext->ReorderInUse = FALSE;
+		pRxContext->bRxHandling = FALSE;
+		pRxContext->BulkInOffset = 0;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- NICInitRecv\n"));
+	return Status;
+
+out1:
+	for (i = 0; i < (RX_RING_SIZE); i++)
+	{
+		PRX_CONTEXT  pRxContext = &(pAd->RxContext[i]);
+
+		if (NULL != pRxContext->TransferBuffer)
+		{
+			RTUSB_URB_FREE_BUFFER(pObj->pUsb_Dev, MAX_RXBULK_SIZE,
+								pRxContext->TransferBuffer, pRxContext->data_dma);
+			pRxContext->TransferBuffer = NULL;
+		}
+
+		if (NULL != pRxContext->pUrb)
+		{
+			RTUSB_UNLINK_URB(pRxContext->pUrb);
+			RTUSB_FREE_URB(pRxContext->pUrb);
+			pRxContext->pUrb = NULL;
+		}
+	}
+
+	return Status;
+}
+
+
+/*
+========================================================================
+Routine Description:
+    Initialize transmit data structures.
+
+Arguments:
+    pAd					Pointer to our adapter
+
+Return Value:
+	NDIS_STATUS_SUCCESS
+	NDIS_STATUS_RESOURCES
+
+Note:
+========================================================================
+*/
+NDIS_STATUS	NICInitTransmit(
+	IN	PRTMP_ADAPTER	pAd)
+{
+#define LM_USB_ALLOC(pObj, Context, TB_Type, BufferSize, Status, msg1, err1, msg2, err2)	\
+	Context->pUrb = RTUSB_ALLOC_URB(0);		\
+	if (Context->pUrb == NULL) {			\
+		DBGPRINT(RT_DEBUG_ERROR, msg1);		\
+		Status = NDIS_STATUS_RESOURCES;		\
+		goto err1; }						\
+											\
+	Context->TransferBuffer = 				\
+		(TB_Type)RTUSB_URB_ALLOC_BUFFER(pObj->pUsb_Dev, BufferSize, &Context->data_dma);	\
+	if (Context->TransferBuffer == NULL) {	\
+		DBGPRINT(RT_DEBUG_ERROR, msg2);		\
+		Status = NDIS_STATUS_RESOURCES;		\
+		goto err2; }
+
+#define LM_URB_FREE(pObj, Context, BufferSize)				\
+	if (NULL != Context->pUrb) {							\
+		RTUSB_UNLINK_URB(Context->pUrb);					\
+		RTUSB_FREE_URB(Context->pUrb);						\
+		Context->pUrb = NULL; }								\
+	if (NULL != Context->TransferBuffer) {				\
+		RTUSB_URB_FREE_BUFFER(pObj->pUsb_Dev, BufferSize,	\
+								Context->TransferBuffer,	\
+								Context->data_dma);			\
+		Context->TransferBuffer = NULL; }
+
+	UCHAR			i, acidx;
+	NDIS_STATUS     Status = NDIS_STATUS_SUCCESS;
+	PTX_CONTEXT		pNullContext   = &(pAd->NullContext);
+	PTX_CONTEXT		pPsPollContext = &(pAd->PsPollContext);
+	PTX_CONTEXT		pRTSContext    = &(pAd->RTSContext);
+	PTX_CONTEXT		pMLMEContext = NULL;
+//	PHT_TX_CONTEXT	pHTTXContext = NULL;
+	POS_COOKIE		pObj = (POS_COOKIE) pAd->OS_Cookie;
+	PVOID			RingBaseVa;
+//	RTMP_TX_RING	*pTxRing;
+	RTMP_MGMT_RING  *pMgmtRing;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> NICInitTransmit\n"));
+	pObj = pObj;
+
+	// Init 4 set of Tx parameters
+	for(acidx = 0; acidx < NUM_OF_TX_RING; acidx++)
+	{
+		// Initialize all Transmit releated queues
+		InitializeQueueHeader(&pAd->TxSwQueue[acidx]);
+
+		// Next Local tx ring pointer waiting for buck out
+		pAd->NextBulkOutIndex[acidx] = acidx;
+		pAd->BulkOutPending[acidx] = FALSE; // Buck Out control flag
+		//pAd->DataBulkDoneIdx[acidx] = 0;
+	}
+
+	//pAd->NextMLMEIndex	= 0;
+	//pAd->PushMgmtIndex	= 0;
+	//pAd->PopMgmtIndex	= 0;
+	//InterlockedExchange(&pAd->MgmtQueueSize, 0);
+	//InterlockedExchange(&pAd->TxCount, 0);
+
+	//pAd->PrioRingFirstIndex	= 0;
+	//pAd->PrioRingTxCnt		= 0;
+
+	do
+	{
+		//
+		// TX_RING_SIZE, 4 ACs
+		//
+		for(acidx=0; acidx<4; acidx++)
+		{
+			PHT_TX_CONTEXT	pHTTXContext = &(pAd->TxContext[acidx]);
+
+			NdisZeroMemory(pHTTXContext, sizeof(HT_TX_CONTEXT));
+			//Allocate URB
+			LM_USB_ALLOC(pObj, pHTTXContext, PHTTX_BUFFER, sizeof(HTTX_BUFFER), Status,
+							("<-- ERROR in Alloc TX TxContext[%d] urb!! \n", acidx),
+							done,
+							("<-- ERROR in Alloc TX TxContext[%d] HTTX_BUFFER !! \n", acidx),
+							out1);
+
+			NdisZeroMemory(pHTTXContext->TransferBuffer->Aggregation, 4);
+			pHTTXContext->pAd = pAd;
+			pHTTXContext->pIrp = NULL;
+			pHTTXContext->IRPPending = FALSE;
+			pHTTXContext->NextBulkOutPosition = 0;
+			pHTTXContext->ENextBulkOutPosition = 0;
+			pHTTXContext->CurWritePosition = 0;
+			pHTTXContext->CurWriteRealPos = 0;
+			pHTTXContext->BulkOutSize = 0;
+			pHTTXContext->BulkOutPipeId = acidx;
+			pHTTXContext->bRingEmpty = TRUE;
+			pHTTXContext->bCopySavePad = FALSE;
+
+			pAd->BulkOutPending[acidx] = FALSE;
+		}
+
+
+		//
+		// MGMT_RING_SIZE
+		//
+#if 0
+		for(i=0; i<MGMT_RING_SIZE; i++) // 8
+		{
+			PTX_CONTEXT	pMLMEContext = &(pAd->MLMEContext[i]);
+
+
+			NdisZeroMemory(pMLMEContext, sizeof(TX_CONTEXT));
+
+			//Allocate URB
+			LM_USB_ALLOC(pObj, pMLMEContext, PTX_BUFFER, sizeof(TX_BUFFER), Status,
+							("<-- ERROR in Alloc TX MLMEContext[%d] urb!! \n", i),
+							out2,
+							("<-- ERROR in Alloc TX MLMEContext[%d] TX_BUFFER !! \n", i),
+							out2);
+
+			pMLMEContext->pAd = pAd;
+			pMLMEContext->pIrp = NULL;
+			pMLMEContext->InUse = FALSE;
+			pMLMEContext->IRPPending = FALSE;
+		}
+#else
+		// Allocate MGMT ring descriptor's memory
+		pAd->MgmtDescRing.AllocSize = MGMT_RING_SIZE * sizeof(TX_CONTEXT);
+		RTMPAllocateMemory(&pAd->MgmtDescRing.AllocVa, pAd->MgmtDescRing.AllocSize);
+		if (pAd->MgmtDescRing.AllocVa == NULL)
+		{
+			DBGPRINT_ERR(("Failed to allocate a big buffer for MgmtDescRing!\n"));
+			Status = NDIS_STATUS_RESOURCES;
+			goto out1;
+		}
+		NdisZeroMemory(pAd->MgmtDescRing.AllocVa, pAd->MgmtDescRing.AllocSize);
+		RingBaseVa     = pAd->MgmtDescRing.AllocVa;
+
+		// Initialize MGMT Ring and associated buffer memory
+		pMgmtRing = &pAd->MgmtRing;
+		for (i = 0; i < MGMT_RING_SIZE; i++)
+		{
+			// link the pre-allocated Mgmt buffer to MgmtRing.Cell
+			pMgmtRing->Cell[i].AllocSize = sizeof(TX_CONTEXT);
+			pMgmtRing->Cell[i].AllocVa = RingBaseVa;
+			pMgmtRing->Cell[i].pNdisPacket = NULL;
+			pMgmtRing->Cell[i].pNextNdisPacket = NULL;
+
+			//Allocate URB for MLMEContext
+			pMLMEContext = (PTX_CONTEXT) pAd->MgmtRing.Cell[i].AllocVa;
+			pMLMEContext->pUrb = RTUSB_ALLOC_URB(0);
+			if (pMLMEContext->pUrb == NULL)
+			{
+				DBGPRINT(RT_DEBUG_ERROR, ("<-- ERROR in Alloc TX MLMEContext[%d] urb!! \n", i));
+				Status = NDIS_STATUS_RESOURCES;
+				goto out2;
+			}
+			pMLMEContext->pAd = pAd;
+			pMLMEContext->pIrp = NULL;
+			pMLMEContext->TransferBuffer = NULL;
+			pMLMEContext->InUse = FALSE;
+			pMLMEContext->IRPPending = FALSE;
+			pMLMEContext->bWaitingBulkOut = FALSE;
+			pMLMEContext->BulkOutSize = 0;
+			pMLMEContext->SelfIdx = i;
+
+			// Offset to next ring descriptor address
+			RingBaseVa = (PUCHAR) RingBaseVa + sizeof(TX_CONTEXT);
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("MGMT Ring: total %d entry allocated\n", i));
+
+		//pAd->MgmtRing.TxSwFreeIdx = (MGMT_RING_SIZE - 1);
+		pAd->MgmtRing.TxSwFreeIdx = MGMT_RING_SIZE;
+		pAd->MgmtRing.TxCpuIdx = 0;
+		pAd->MgmtRing.TxDmaIdx = 0;
+#endif
+
+		//
+		// BEACON_RING_SIZE
+		//
+		for(i=0; i<BEACON_RING_SIZE; i++) // 2
+		{
+			PTX_CONTEXT	pBeaconContext = &(pAd->BeaconContext[i]);
+
+
+			NdisZeroMemory(pBeaconContext, sizeof(TX_CONTEXT));
+
+			//Allocate URB
+			LM_USB_ALLOC(pObj, pBeaconContext, PTX_BUFFER, sizeof(TX_BUFFER), Status,
+							("<-- ERROR in Alloc TX BeaconContext[%d] urb!! \n", i),
+							out2,
+							("<-- ERROR in Alloc TX BeaconContext[%d] TX_BUFFER !! \n", i),
+							out3);
+
+			pBeaconContext->pAd = pAd;
+			pBeaconContext->pIrp = NULL;
+			pBeaconContext->InUse = FALSE;
+			pBeaconContext->IRPPending = FALSE;
+		}
+
+		//
+		// NullContext
+		//
+		NdisZeroMemory(pNullContext, sizeof(TX_CONTEXT));
+
+		//Allocate URB
+		LM_USB_ALLOC(pObj, pNullContext, PTX_BUFFER, sizeof(TX_BUFFER), Status,
+						("<-- ERROR in Alloc TX NullContext urb!! \n"),
+						out3,
+						("<-- ERROR in Alloc TX NullContext TX_BUFFER !! \n"),
+						out4);
+
+		pNullContext->pAd = pAd;
+		pNullContext->pIrp = NULL;
+		pNullContext->InUse = FALSE;
+		pNullContext->IRPPending = FALSE;
+
+		//
+		// RTSContext
+		//
+		NdisZeroMemory(pRTSContext, sizeof(TX_CONTEXT));
+
+		//Allocate URB
+		LM_USB_ALLOC(pObj, pRTSContext, PTX_BUFFER, sizeof(TX_BUFFER), Status,
+						("<-- ERROR in Alloc TX RTSContext urb!! \n"),
+						out4,
+						("<-- ERROR in Alloc TX RTSContext TX_BUFFER !! \n"),
+						out5);
+
+		pRTSContext->pAd = pAd;
+		pRTSContext->pIrp = NULL;
+		pRTSContext->InUse = FALSE;
+		pRTSContext->IRPPending = FALSE;
+
+		//
+		// PsPollContext
+		//
+		//NdisZeroMemory(pPsPollContext, sizeof(TX_CONTEXT));
+		//Allocate URB
+		LM_USB_ALLOC(pObj, pPsPollContext, PTX_BUFFER, sizeof(TX_BUFFER), Status,
+						("<-- ERROR in Alloc TX PsPollContext urb!! \n"),
+						out5,
+						("<-- ERROR in Alloc TX PsPollContext TX_BUFFER !! \n"),
+						out6);
+
+		pPsPollContext->pAd = pAd;
+		pPsPollContext->pIrp = NULL;
+		pPsPollContext->InUse = FALSE;
+		pPsPollContext->IRPPending = FALSE;
+		pPsPollContext->bAggregatible = FALSE;
+		pPsPollContext->LastOne = TRUE;
+
+	}   while (FALSE);
+
+
+done:
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- NICInitTransmit\n"));
+
+	return Status;
+
+	/* --------------------------- ERROR HANDLE --------------------------- */
+out6:
+	LM_URB_FREE(pObj, pPsPollContext, sizeof(TX_BUFFER));
+
+out5:
+	LM_URB_FREE(pObj, pRTSContext, sizeof(TX_BUFFER));
+
+out4:
+	LM_URB_FREE(pObj, pNullContext, sizeof(TX_BUFFER));
+
+out3:
+	for(i=0; i<BEACON_RING_SIZE; i++)
+	{
+		PTX_CONTEXT	pBeaconContext = &(pAd->BeaconContext[i]);
+		if (pBeaconContext)
+			LM_URB_FREE(pObj, pBeaconContext, sizeof(TX_BUFFER));
+	}
+
+out2:
+	if (pAd->MgmtDescRing.AllocVa)
+	{
+		pMgmtRing = &pAd->MgmtRing;
+	for(i=0; i<MGMT_RING_SIZE; i++)
+	{
+		pMLMEContext = (PTX_CONTEXT) pAd->MgmtRing.Cell[i].AllocVa;
+		if (pMLMEContext)
+			LM_URB_FREE(pObj, pMLMEContext, sizeof(TX_BUFFER));
+	}
+		NdisFreeMemory(pAd->MgmtDescRing.AllocVa, pAd->MgmtDescRing.AllocSize, 0);
+		pAd->MgmtDescRing.AllocVa = NULL;
+	}
+
+out1:
+	for (acidx = 0; acidx < 4; acidx++)
+	{
+		PHT_TX_CONTEXT pTxContext = &(pAd->TxContext[acidx]);
+		if (pTxContext)
+			LM_URB_FREE(pObj, pTxContext, sizeof(HTTX_BUFFER));
+	}
+
+	// Here we didn't have any pre-allocated memory need to free.
+
+	return Status;
+}
+
+
+/*
+========================================================================
+Routine Description:
+    Allocate DMA memory blocks for send, receive.
+
+Arguments:
+    pAd					Pointer to our adapter
+
+Return Value:
+	NDIS_STATUS_SUCCESS
+	NDIS_STATUS_FAILURE
+	NDIS_STATUS_RESOURCES
+
+Note:
+========================================================================
+*/
+NDIS_STATUS	RTMPAllocTxRxRingMemory(
+	IN	PRTMP_ADAPTER	pAd)
+{
+//	COUNTER_802_11	pCounter = &pAd->WlanCounters;
+	NDIS_STATUS		Status;
+	INT				num;
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> RTMPAllocTxRxRingMemory\n"));
+
+
+	do
+	{
+		// Init the CmdQ and CmdQLock
+		NdisAllocateSpinLock(&pAd->CmdQLock);
+		NdisAcquireSpinLock(&pAd->CmdQLock);
+		RTUSBInitializeCmdQ(&pAd->CmdQ);
+		NdisReleaseSpinLock(&pAd->CmdQLock);
+
+
+		NdisAllocateSpinLock(&pAd->MLMEBulkOutLock);
+		//NdisAllocateSpinLock(&pAd->MLMEWaitQueueLock);
+		NdisAllocateSpinLock(&pAd->BulkOutLock[0]);
+		NdisAllocateSpinLock(&pAd->BulkOutLock[1]);
+		NdisAllocateSpinLock(&pAd->BulkOutLock[2]);
+		NdisAllocateSpinLock(&pAd->BulkOutLock[3]);
+		NdisAllocateSpinLock(&pAd->BulkOutLock[4]);
+		NdisAllocateSpinLock(&pAd->BulkOutLock[5]);
+		NdisAllocateSpinLock(&pAd->BulkInLock);
+
+		for (num = 0; num < NUM_OF_TX_RING; num++)
+		{
+			NdisAllocateSpinLock(&pAd->TxContextQueueLock[num]);
+		}
+
+#ifdef RALINK_ATE
+		NdisAllocateSpinLock(&pAd->GenericLock);
+#endif // RALINK_ATE //
+
+//		NdisAllocateSpinLock(&pAd->MemLock);	// Not used in RT28XX
+
+//		NdisAllocateSpinLock(&pAd->MacTabLock); // init it in UserCfgInit()
+//		NdisAllocateSpinLock(&pAd->BATabLock); // init it in BATableInit()
+
+//		for(num=0; num<MAX_LEN_OF_BA_REC_TABLE; num++)
+//		{
+//			NdisAllocateSpinLock(&pAd->BATable.BARecEntry[num].RxReRingLock);
+//		}
+
+		//
+		// Init Mac Table
+		//
+//		MacTableInitialize(pAd);
+
+		//
+		// Init send data structures and related parameters
+		//
+		Status = NICInitTransmit(pAd);
+		if (Status != NDIS_STATUS_SUCCESS)
+			break;
+
+		//
+		// Init receive data structures and related parameters
+		//
+		Status = NICInitRecv(pAd);
+		if (Status != NDIS_STATUS_SUCCESS)
+			break;
+
+		pAd->PendingIoCount = 1;
+
+	} while (FALSE);
+
+	NdisZeroMemory(&pAd->FragFrame, sizeof(FRAGMENT_FRAME));
+	pAd->FragFrame.pFragPacket =  RTMP_AllocateFragPacketBuffer(pAd, RX_BUFFER_NORMSIZE);
+
+	if (pAd->FragFrame.pFragPacket == NULL)
+	{
+		Status = NDIS_STATUS_RESOURCES;
+	}
+
+	DBGPRINT_S(Status, ("<-- RTMPAllocTxRxRingMemory, Status=%x\n", Status));
+	return Status;
+}
+
+
+/*
+========================================================================
+Routine Description:
+	Calls USB_InterfaceStop and frees memory allocated for the URBs
+    calls NdisMDeregisterDevice and frees the memory
+    allocated in VNetInitialize for the Adapter Object
+
+Arguments:
+	*pAd				the raxx interface data pointer
+
+Return Value:
+	None
+
+Note:
+========================================================================
+*/
+VOID	RTMPFreeTxRxRingMemory(
+	IN	PRTMP_ADAPTER	pAd)
+{
+#define LM_URB_FREE(pObj, Context, BufferSize)				\
+	if (NULL != Context->pUrb) {							\
+		RTUSB_UNLINK_URB(Context->pUrb);					\
+		RTUSB_FREE_URB(Context->pUrb);						\
+		Context->pUrb = NULL; }								\
+	if (NULL != Context->TransferBuffer) {					\
+		RTUSB_URB_FREE_BUFFER(pObj->pUsb_Dev, BufferSize,	\
+								Context->TransferBuffer,	\
+								Context->data_dma);			\
+		Context->TransferBuffer = NULL; }
+
+
+	UINT                i, acidx;
+	PTX_CONTEXT			pNullContext   = &pAd->NullContext;
+	PTX_CONTEXT			pPsPollContext = &pAd->PsPollContext;
+	PTX_CONTEXT			pRTSContext    = &pAd->RTSContext;
+//	PHT_TX_CONTEXT 		pHTTXContext;
+	//PRTMP_REORDERBUF	pReorderBuf;
+	POS_COOKIE			pObj = (POS_COOKIE) pAd->OS_Cookie;
+//	RTMP_TX_RING		*pTxRing;
+
+	DBGPRINT(RT_DEBUG_ERROR, ("---> RTMPFreeTxRxRingMemory\n"));
+	pObj = pObj;
+
+	// Free all resources for the RECEIVE buffer queue.
+	for(i=0; i<(RX_RING_SIZE); i++)
+	{
+		PRX_CONTEXT  pRxContext = &(pAd->RxContext[i]);
+		if (pRxContext)
+			LM_URB_FREE(pObj, pRxContext, MAX_RXBULK_SIZE);
+	}
+
+	// Free PsPoll frame resource
+	LM_URB_FREE(pObj, pPsPollContext, sizeof(TX_BUFFER));
+
+	// Free NULL frame resource
+	LM_URB_FREE(pObj, pNullContext, sizeof(TX_BUFFER));
+
+	// Free RTS frame resource
+	LM_URB_FREE(pObj, pRTSContext, sizeof(TX_BUFFER));
+
+
+	// Free beacon frame resource
+	for(i=0; i<BEACON_RING_SIZE; i++)
+	{
+		PTX_CONTEXT	pBeaconContext = &(pAd->BeaconContext[i]);
+		if (pBeaconContext)
+			LM_URB_FREE(pObj, pBeaconContext, sizeof(TX_BUFFER));
+	}
+
+
+	// Free mgmt frame resource
+	for(i = 0; i < MGMT_RING_SIZE; i++)
+	{
+		PTX_CONTEXT pMLMEContext = (PTX_CONTEXT)pAd->MgmtRing.Cell[i].AllocVa;
+		//LM_URB_FREE(pObj, pMLMEContext, sizeof(TX_BUFFER));
+		if (NULL != pAd->MgmtRing.Cell[i].pNdisPacket)
+		{
+			RTMPFreeNdisPacket(pAd, pAd->MgmtRing.Cell[i].pNdisPacket);
+			pAd->MgmtRing.Cell[i].pNdisPacket = NULL;
+			pMLMEContext->TransferBuffer = NULL;
+		}
+
+		if (pMLMEContext)
+		{
+			if (NULL != pMLMEContext->pUrb)
+			{
+				RTUSB_UNLINK_URB(pMLMEContext->pUrb);
+				RTUSB_FREE_URB(pMLMEContext->pUrb);
+				pMLMEContext->pUrb = NULL;
+			}
+		}
+	}
+	if (pAd->MgmtDescRing.AllocVa)
+		NdisFreeMemory(pAd->MgmtDescRing.AllocVa, pAd->MgmtDescRing.AllocSize, 0);
+
+
+	// Free Tx frame resource
+	for (acidx = 0; acidx < 4; acidx++)
+		{
+		PHT_TX_CONTEXT pHTTXContext = &(pAd->TxContext[acidx]);
+			if (pHTTXContext)
+				LM_URB_FREE(pObj, pHTTXContext, sizeof(HTTX_BUFFER));
+		}
+
+	if (pAd->FragFrame.pFragPacket)
+		RELEASE_NDIS_PACKET(pAd, pAd->FragFrame.pFragPacket, NDIS_STATUS_SUCCESS);
+
+	for(i=0; i<6; i++)
+	{
+		NdisFreeSpinLock(&pAd->BulkOutLock[i]);
+	}
+
+	NdisFreeSpinLock(&pAd->BulkInLock);
+	NdisFreeSpinLock(&pAd->MLMEBulkOutLock);
+
+	NdisFreeSpinLock(&pAd->CmdQLock);
+#ifdef RALINK_ATE
+	NdisFreeSpinLock(&pAd->GenericLock);
+#endif // RALINK_ATE //
+	// Clear all pending bulk-out request flags.
+	RTUSB_CLEAR_BULK_FLAG(pAd, 0xffffffff);
+
+//	NdisFreeSpinLock(&pAd->MacTabLock);
+
+//	for(i=0; i<MAX_LEN_OF_BA_REC_TABLE; i++)
+//	{
+//		NdisFreeSpinLock(&pAd->BATable.BARecEntry[i].RxReRingLock);
+//	}
+
+	DBGPRINT(RT_DEBUG_ERROR, ("<--- ReleaseAdapter\n"));
+}
+
+
+/*
+========================================================================
+Routine Description:
+    Allocate memory for adapter control block.
+
+Arguments:
+    pAd					Pointer to our adapter
+
+Return Value:
+	NDIS_STATUS_SUCCESS
+	NDIS_STATUS_FAILURE
+	NDIS_STATUS_RESOURCES
+
+Note:
+========================================================================
+*/
+NDIS_STATUS AdapterBlockAllocateMemory(
+	IN PVOID	handle,
+	OUT	PVOID	*ppAd)
+{
+	PUSB_DEV	usb_dev;
+	POS_COOKIE	pObj = (POS_COOKIE) handle;
+
+
+	usb_dev = pObj->pUsb_Dev;
+
+	pObj->MLMEThr_pid		= THREAD_PID_INIT_VALUE;
+	pObj->RTUSBCmdThr_pid	= THREAD_PID_INIT_VALUE;
+
+	*ppAd = (PVOID)vmalloc(sizeof(RTMP_ADAPTER));
+
+	if (*ppAd)
+	{
+		NdisZeroMemory(*ppAd, sizeof(RTMP_ADAPTER));
+		((PRTMP_ADAPTER)*ppAd)->OS_Cookie = handle;
+		return (NDIS_STATUS_SUCCESS);
+	}
+	else
+	{
+		return (NDIS_STATUS_FAILURE);
+	}
+}
+
+
+/*
+========================================================================
+Routine Description:
+    Create kernel threads & tasklets.
+
+Arguments:
+    *net_dev			Pointer to wireless net device interface
+
+Return Value:
+	NDIS_STATUS_SUCCESS
+	NDIS_STATUS_FAILURE
+
+Note:
+========================================================================
+*/
+NDIS_STATUS	 CreateThreads(
+	IN	struct net_device *net_dev)
+{
+	PRTMP_ADAPTER pAd = net_dev->ml_priv;
+	POS_COOKIE pObj = (POS_COOKIE) pAd->OS_Cookie;
+	pid_t pid_number = -1;
+
+	//init_MUTEX(&(pAd->usbdev_semaphore));
+
+	init_MUTEX_LOCKED(&(pAd->mlme_semaphore));
+	init_completion (&pAd->mlmeComplete);
+
+	init_MUTEX_LOCKED(&(pAd->RTUSBCmd_semaphore));
+	init_completion (&pAd->CmdQComplete);
+
+	init_MUTEX_LOCKED(&(pAd->RTUSBTimer_semaphore));
+	init_completion (&pAd->TimerQComplete);
+
+	// Creat MLME Thread
+	pObj->MLMEThr_pid= THREAD_PID_INIT_VALUE;
+	pid_number = kernel_thread(MlmeThread, pAd, CLONE_VM);
+	if (pid_number < 0)
+	{
+		printk (KERN_WARNING "%s: unable to start Mlme thread\n",pAd->net_dev->name);
+		return NDIS_STATUS_FAILURE;
+	}
+	pObj->MLMEThr_pid = GET_PID(pid_number);
+	// Wait for the thread to start
+	wait_for_completion(&(pAd->mlmeComplete));
+
+	// Creat Command Thread
+	pObj->RTUSBCmdThr_pid= THREAD_PID_INIT_VALUE;
+	pid_number = kernel_thread(RTUSBCmdThread, pAd, CLONE_VM);
+	if (pid_number < 0)
+	{
+		printk (KERN_WARNING "%s: unable to start RTUSBCmd thread\n",pAd->net_dev->name);
+		return NDIS_STATUS_FAILURE;
+	}
+	pObj->RTUSBCmdThr_pid = GET_PID(pid_number);
+	wait_for_completion(&(pAd->CmdQComplete));
+
+	pObj->TimerQThr_pid= THREAD_PID_INIT_VALUE;
+	pid_number = kernel_thread(TimerQThread, pAd, CLONE_VM);
+	if (pid_number < 0)
+	{
+		printk (KERN_WARNING "%s: unable to start TimerQThread\n",pAd->net_dev->name);
+		return NDIS_STATUS_FAILURE;
+	}
+	pObj->TimerQThr_pid = GET_PID(pid_number);
+	// Wait for the thread to start
+	wait_for_completion(&(pAd->TimerQComplete));
+
+	// Create receive tasklet
+	tasklet_init(&pObj->rx_done_task, rx_done_tasklet, (ULONG)pAd);
+	tasklet_init(&pObj->mgmt_dma_done_task, rt2870_mgmt_dma_done_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->ac0_dma_done_task, rt2870_ac0_dma_done_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->ac1_dma_done_task, rt2870_ac1_dma_done_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->ac2_dma_done_task, rt2870_ac2_dma_done_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->ac3_dma_done_task, rt2870_ac3_dma_done_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->hcca_dma_done_task, rt2870_hcca_dma_done_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->tbtt_task, tbtt_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->null_frame_complete_task, rt2870_null_frame_complete_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->rts_frame_complete_task, rt2870_rts_frame_complete_tasklet, (unsigned long)pAd);
+	tasklet_init(&pObj->pspoll_frame_complete_task, rt2870_pspoll_frame_complete_tasklet, (unsigned long)pAd);
+
+	return NDIS_STATUS_SUCCESS;
+}
+
+
+#ifdef CONFIG_STA_SUPPORT
+/*
+========================================================================
+Routine Description:
+ 	As STA's BSSID is a WC too, it uses shared key table.
+ 	This function write correct unicast TX key to ASIC WCID.
+ 	And we still make a copy in our MacTab.Content[BSSID_WCID].PairwiseKey.
+	Caller guarantee TKIP/AES always has keyidx = 0. (pairwise key)
+	Caller guarantee WEP calls this function when set Txkey,  default key index=0~3.
+
+Arguments:
+	pAd 					Pointer to our adapter
+	pKey					Pointer to the where the key stored
+
+Return Value:
+	NDIS_SUCCESS			Add key successfully
+
+Note:
+========================================================================
+*/
+VOID	RTMPAddBSSIDCipher(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	UCHAR				Aid,
+	IN	PNDIS_802_11_KEY	pKey,
+	IN  UCHAR   			CipherAlg)
+{
+	PUCHAR		pTxMic, pRxMic;
+	BOOLEAN 	bKeyRSC, bAuthenticator; // indicate the receive SC set by KeyRSC value
+//	UCHAR		CipherAlg;
+	UCHAR		i;
+	ULONG		WCIDAttri;
+	USHORT	 	offset;
+	UCHAR		KeyIdx, IVEIV[8];
+	UINT32		Value;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPAddBSSIDCipher==> Aid = %d\n",Aid));
+
+	// Bit 29 of Add-key KeyRSC
+	bKeyRSC 	   = (pKey->KeyIndex & 0x20000000) ? TRUE : FALSE;
+
+	// Bit 28 of Add-key Authenticator
+	bAuthenticator = (pKey->KeyIndex & 0x10000000) ? TRUE : FALSE;
+	KeyIdx = (UCHAR)pKey->KeyIndex&0xff;
+
+	if (KeyIdx > 4)
+		return;
+
+
+	if (pAd->MacTab.Content[Aid].PairwiseKey.CipherAlg == CIPHER_TKIP)
+	{	if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+		{
+			// for WPA-None Tx, Rx MIC is the same
+			pTxMic = (PUCHAR) (&pKey->KeyMaterial) + 16;
+			pRxMic = pTxMic;
+		}
+		else if (bAuthenticator == TRUE)
+		{
+			pTxMic = (PUCHAR) (&pKey->KeyMaterial) + 16;
+			pRxMic = (PUCHAR) (&pKey->KeyMaterial) + 24;
+		}
+		else
+		{
+			pRxMic = (PUCHAR) (&pKey->KeyMaterial) + 16;
+			pTxMic = (PUCHAR) (&pKey->KeyMaterial) + 24;
+		}
+
+		offset = PAIRWISE_KEY_TABLE_BASE + (Aid * HW_KEY_ENTRY_SIZE) + 0x10;
+		for (i=0; i<8; )
+		{
+			Value = *(pTxMic+i);
+			Value += (*(pTxMic+i+1)<<8);
+			Value += (*(pTxMic+i+2)<<16);
+			Value += (*(pTxMic+i+3)<<24);
+			RTUSBWriteMACRegister(pAd, offset+i, Value);
+			i+=4;
+		}
+
+		offset = PAIRWISE_KEY_TABLE_BASE + (Aid * HW_KEY_ENTRY_SIZE) + 0x18;
+		for (i=0; i<8; )
+		{
+			Value = *(pRxMic+i);
+			Value += (*(pRxMic+i+1)<<8);
+			Value += (*(pRxMic+i+2)<<16);
+			Value += (*(pRxMic+i+3)<<24);
+			RTUSBWriteMACRegister(pAd, offset+i, Value);
+			i+=4;
+		}
+
+		// Only Key lenth equal to TKIP key have these
+		NdisMoveMemory(pAd->MacTab.Content[Aid].PairwiseKey.RxMic, pRxMic, 8);
+		NdisMoveMemory(pAd->MacTab.Content[Aid].PairwiseKey.TxMic, pTxMic, 8);
+
+		DBGPRINT(RT_DEBUG_TRACE,
+				("	TxMIC  = %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x \n",
+				pTxMic[0],pTxMic[1],pTxMic[2],pTxMic[3],
+				pTxMic[4],pTxMic[5],pTxMic[6],pTxMic[7]));
+		DBGPRINT(RT_DEBUG_TRACE,
+				("	RxMIC  = %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x \n",
+				pRxMic[0],pRxMic[1],pRxMic[2],pRxMic[3],
+				pRxMic[4],pRxMic[5],pRxMic[6],pRxMic[7]));
+	}
+
+	// 2. Record Security Key.
+	pAd->MacTab.Content[BSSID_WCID].PairwiseKey.KeyLen= (UCHAR)pKey->KeyLength;
+	NdisMoveMemory(pAd->MacTab.Content[Aid].PairwiseKey.Key, &pKey->KeyMaterial, pKey->KeyLength);
+
+	// 3. Check RxTsc. And used to init to ASIC IV.
+	if (bKeyRSC == TRUE)
+		NdisMoveMemory(pAd->MacTab.Content[Aid].PairwiseKey.RxTsc, &pKey->KeyRSC, 6);
+	else
+		NdisZeroMemory(pAd->MacTab.Content[Aid].PairwiseKey.RxTsc, 6);
+
+	// 4. Init TxTsc to one based on WiFi WPA specs
+	pAd->MacTab.Content[Aid].PairwiseKey.TxTsc[0] = 1;
+	pAd->MacTab.Content[Aid].PairwiseKey.TxTsc[1] = 0;
+	pAd->MacTab.Content[Aid].PairwiseKey.TxTsc[2] = 0;
+	pAd->MacTab.Content[Aid].PairwiseKey.TxTsc[3] = 0;
+	pAd->MacTab.Content[Aid].PairwiseKey.TxTsc[4] = 0;
+	pAd->MacTab.Content[Aid].PairwiseKey.TxTsc[5] = 0;
+
+	CipherAlg = pAd->MacTab.Content[Aid].PairwiseKey.CipherAlg;
+
+	offset = PAIRWISE_KEY_TABLE_BASE + (Aid * HW_KEY_ENTRY_SIZE);
+	RTUSBMultiWrite(pAd, (USHORT) offset, pKey->KeyMaterial,
+				((pKey->KeyLength == LEN_TKIP_KEY) ? 16 : (USHORT)pKey->KeyLength));
+
+	offset = SHARED_KEY_TABLE_BASE + (KeyIdx * HW_KEY_ENTRY_SIZE);
+	RTUSBMultiWrite(pAd, (USHORT) offset, pKey->KeyMaterial, (USHORT)pKey->KeyLength);
+
+	offset = PAIRWISE_IVEIV_TABLE_BASE + (Aid * HW_IVEIV_ENTRY_SIZE);
+	NdisZeroMemory(IVEIV, 8);
+
+	// IV/EIV
+	if ((CipherAlg == CIPHER_TKIP) ||
+		(CipherAlg == CIPHER_TKIP_NO_MIC) ||
+		(CipherAlg == CIPHER_AES))
+	{
+		IVEIV[3] = 0x20; // Eiv bit on. keyid always 0 for pairwise key
+	}
+	// default key idx needs to set.
+	// in TKIP/AES KeyIdx = 0 , WEP KeyIdx is default tx key.
+	else
+	{
+		IVEIV[3] |= (KeyIdx<< 6);
+	}
+	RTUSBMultiWrite(pAd, (USHORT) offset, IVEIV, 8);
+
+	// WCID Attribute UDF:3, BSSIdx:3, Alg:3, Keytable:1=PAIRWISE KEY, BSSIdx is 0
+	if ((CipherAlg == CIPHER_TKIP) ||
+		(CipherAlg == CIPHER_TKIP_NO_MIC) ||
+		(CipherAlg == CIPHER_AES))
+	{
+		WCIDAttri = (CipherAlg<<1)|SHAREDKEYTABLE;
+	}
+	else
+		WCIDAttri = (CipherAlg<<1)|SHAREDKEYTABLE;
+
+	offset = MAC_WCID_ATTRIBUTE_BASE + (Aid* HW_WCID_ATTRI_SIZE);
+	RTUSBWriteMACRegister(pAd, offset, WCIDAttri);
+	RTUSBReadMACRegister(pAd, offset, &Value);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("BSSID_WCID : offset = %x, WCIDAttri = %lx\n",
+			offset, WCIDAttri));
+
+	// pAddr
+	// Add Bssid mac address at linkup. not here.  check!
+	/*offset = MAC_WCID_BASE + (BSSID_WCID * HW_WCID_ENTRY_SIZE);
+	*for (i=0; i<MAC_ADDR_LEN; i++)
+	{
+		RTMP_IO_WRITE8(pAd, offset+i, pKey->BSSID[i]);
+	}
+	*/
+
+	DBGPRINT(RT_DEBUG_ERROR, ("AddBSSIDasWCIDEntry: Alg=%s, KeyLength = %d\n",
+			CipherName[CipherAlg], pKey->KeyLength));
+	DBGPRINT(RT_DEBUG_TRACE, ("Key [idx=%x] [KeyLen = %d]\n",
+			pKey->KeyIndex, pKey->KeyLength));
+	for(i=0; i<pKey->KeyLength; i++)
+		DBGPRINT_RAW(RT_DEBUG_TRACE,(" %x:", pKey->KeyMaterial[i]));
+	DBGPRINT(RT_DEBUG_TRACE,("	 \n"));
+}
+#endif // CONFIG_STA_SUPPORT //
+
+/*
+========================================================================
+Routine Description:
+    Get a received packet.
+
+Arguments:
+	pAd					device control block
+	pSaveRxD			receive descriptor information
+	*pbReschedule		need reschedule flag
+	*pRxPending			pending received packet flag
+
+Return Value:
+    the recieved packet
+
+Note:
+========================================================================
+*/
+#define RT2870_RXDMALEN_FIELD_SIZE			4
+PNDIS_PACKET GetPacketFromRxRing(
+	IN		PRTMP_ADAPTER		pAd,
+	OUT		PRT28XX_RXD_STRUC	pSaveRxD,
+	OUT		BOOLEAN				*pbReschedule,
+	IN OUT	UINT32				*pRxPending)
+{
+	PRX_CONTEXT		pRxContext;
+	PNDIS_PACKET	pSkb;
+	PUCHAR			pData;
+	ULONG			ThisFrameLen;
+	ULONG			RxBufferLength;
+	PRXWI_STRUC		pRxWI;
+
+	pRxContext = &pAd->RxContext[pAd->NextRxBulkInReadIndex];
+	if ((pRxContext->Readable == FALSE) || (pRxContext->InUse == TRUE))
+		return NULL;
+
+	RxBufferLength = pRxContext->BulkInOffset - pAd->ReadPosition;
+	if (RxBufferLength < (RT2870_RXDMALEN_FIELD_SIZE + sizeof(RXWI_STRUC) + sizeof(RXINFO_STRUC)))
+	{
+		goto label_null;
+	}
+
+	pData = &pRxContext->TransferBuffer[pAd->ReadPosition]; /* 4KB */
+	// The RXDMA field is 4 bytes, now just use the first 2 bytes. The Length including the (RXWI + MSDU + Padding)
+	ThisFrameLen = *pData + (*(pData+1)<<8);
+    if (ThisFrameLen == 0)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("BIRIdx(%d): RXDMALen is zero.[%ld], BulkInBufLen = %ld)\n",
+								pAd->NextRxBulkInReadIndex, ThisFrameLen, pRxContext->BulkInOffset));
+		goto label_null;
+	}
+	if ((ThisFrameLen&0x3) != 0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("BIRIdx(%d): RXDMALen not multiple of 4.[%ld], BulkInBufLen = %ld)\n",
+								pAd->NextRxBulkInReadIndex, ThisFrameLen, pRxContext->BulkInOffset));
+		goto label_null;
+	}
+
+	if ((ThisFrameLen + 8)> RxBufferLength)	// 8 for (RT2870_RXDMALEN_FIELD_SIZE + sizeof(RXINFO_STRUC))
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("BIRIdx(%d):FrameLen(0x%lx) outranges. BulkInLen=0x%lx, remaining RxBufLen=0x%lx, ReadPos=0x%lx\n",
+						pAd->NextRxBulkInReadIndex, ThisFrameLen, pRxContext->BulkInOffset, RxBufferLength, pAd->ReadPosition));
+
+		// error frame. finish this loop
+		goto label_null;
+	}
+
+	// skip USB frame length field
+	pData += RT2870_RXDMALEN_FIELD_SIZE;
+	pRxWI = (PRXWI_STRUC)pData;
+#ifdef RT_BIG_ENDIAN
+	RTMPWIEndianChange(pData, TYPE_RXWI);
+#endif // RT_BIG_ENDIAN //
+	if (pRxWI->MPDUtotalByteCount > ThisFrameLen)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s():pRxWIMPDUtotalByteCount(%d) large than RxDMALen(%ld)\n",
+									__FUNCTION__, pRxWI->MPDUtotalByteCount, ThisFrameLen));
+		goto label_null;
+	}
+#ifdef RT_BIG_ENDIAN
+	RTMPWIEndianChange(pData, TYPE_RXWI);
+#endif // RT_BIG_ENDIAN //
+
+	// allocate a rx packet
+	pSkb = dev_alloc_skb(ThisFrameLen);
+	if (pSkb == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("%s():Cannot Allocate sk buffer for this Bulk-In buffer!\n", __FUNCTION__));
+		goto label_null;
+	}
+
+	// copy the rx packet
+	memcpy(skb_put(pSkb, ThisFrameLen), pData, ThisFrameLen);
+	RTPKT_TO_OSPKT(pSkb)->dev = get_netdev_from_bssid(pAd, BSS0);
+	RTMP_SET_PACKET_SOURCE(OSPKT_TO_RTPKT(pSkb), PKTSRC_NDIS);
+
+	// copy RxD
+	*pSaveRxD = *(PRXINFO_STRUC)(pData + ThisFrameLen);
+#ifdef RT_BIG_ENDIAN
+	RTMPDescriptorEndianChange((PUCHAR)pSaveRxD, TYPE_RXINFO);
+#endif // RT_BIG_ENDIAN //
+
+	// update next packet read position.
+	pAd->ReadPosition += (ThisFrameLen + RT2870_RXDMALEN_FIELD_SIZE + RXINFO_SIZE);	// 8 for (RT2870_RXDMALEN_FIELD_SIZE + sizeof(RXINFO_STRUC))
+
+	return pSkb;
+
+label_null:
+
+	return NULL;
+}
+
+
+/*
+========================================================================
+Routine Description:
+    Handle received packets.
+
+Arguments:
+	data				- URB information pointer
+
+Return Value:
+    None
+
+Note:
+========================================================================
+*/
+static void rx_done_tasklet(unsigned long data)
+{
+	purbb_t 			pUrb;
+	PRX_CONTEXT			pRxContext;
+	PRTMP_ADAPTER		pAd;
+	NTSTATUS			Status;
+	unsigned int		IrqFlags;
+
+	pUrb		= (purbb_t)data;
+	pRxContext	= (PRX_CONTEXT)pUrb->context;
+	pAd 		= pRxContext->pAd;
+	Status = pUrb->status;
+
+
+	RTMP_IRQ_LOCK(&pAd->BulkInLock, IrqFlags);
+	pRxContext->InUse = FALSE;
+	pRxContext->IRPPending = FALSE;
+	pRxContext->BulkInOffset += pUrb->actual_length;
+	//NdisInterlockedDecrement(&pAd->PendingRx);
+	pAd->PendingRx--;
+
+	if (Status == USB_ST_NOERROR)
+	{
+		pAd->BulkInComplete++;
+		pAd->NextRxBulkInPosition = 0;
+		if (pRxContext->BulkInOffset)	// As jan's comment, it may bulk-in success but size is zero.
+		{
+			pRxContext->Readable = TRUE;
+			INC_RING_INDEX(pAd->NextRxBulkInIndex, RX_RING_SIZE);
+		}
+		RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+	}
+	else	 // STATUS_OTHER
+	{
+		pAd->BulkInCompleteFail++;
+		// Still read this packet although it may comtain wrong bytes.
+		pRxContext->Readable = FALSE;
+		RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+
+		// Parsing all packets. because after reset, the index will reset to all zero.
+		if ((!RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_RESET_IN_PROGRESS |
+									fRTMP_ADAPTER_BULKIN_RESET |
+									fRTMP_ADAPTER_HALT_IN_PROGRESS |
+									fRTMP_ADAPTER_NIC_NOT_EXIST))))
+		{
+
+			DBGPRINT_RAW(RT_DEBUG_ERROR, ("Bulk In Failed. Status=%d, BIIdx=0x%x, BIRIdx=0x%x, actual_length= 0x%x\n",
+							Status, pAd->NextRxBulkInIndex, pAd->NextRxBulkInReadIndex, pRxContext->pUrb->actual_length));
+
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKIN_RESET);
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_IN, NULL, 0);
+		}
+	}
+
+	ASSERT((pRxContext->InUse == pRxContext->IRPPending));
+
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+    {
+		// If the driver is in ATE mode and Rx frame is set into here.
+		if (pAd->ContinBulkIn == TRUE)
+		{
+			RTUSBBulkReceive(pAd);
+		}
+	}
+	else
+#endif // RALINK_ATE //
+	RTUSBBulkReceive(pAd);
+
+	return;
+
+}
+
+
+static void rt2870_mgmt_dma_done_tasklet(unsigned long data)
+{
+	PRTMP_ADAPTER 	pAd;
+	PTX_CONTEXT		pMLMEContext;
+	int				index;
+	PNDIS_PACKET	pPacket;
+	purbb_t			pUrb;
+	NTSTATUS		Status;
+	unsigned long	IrqFlags;
+
+
+	pUrb			= (purbb_t)data;
+	pMLMEContext	= (PTX_CONTEXT)pUrb->context;
+	pAd 			= pMLMEContext->pAd;
+	Status			= pUrb->status;
+	index 			= pMLMEContext->SelfIdx;
+
+	ASSERT((pAd->MgmtRing.TxDmaIdx == index));
+
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[MGMTPIPEIDX], IrqFlags);
+
+
+	if (Status != USB_ST_NOERROR)
+	{
+		//Bulk-Out fail status handle
+		if ((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET)))
+		{
+			DBGPRINT_RAW(RT_DEBUG_ERROR, ("Bulk Out MLME Failed, Status=%d!\n", Status));
+			// TODO: How to handle about the MLMEBulkOut failed issue. Need to resend the mgmt pkt?
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+			pAd->bulkResetPipeid = (MGMTPIPEIDX | BULKOUT_MGMT_RESET_FLAG);
+		}
+	}
+
+	pAd->BulkOutPending[MGMTPIPEIDX] = FALSE;
+	RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[MGMTPIPEIDX], IrqFlags);
+
+	RTMP_IRQ_LOCK(&pAd->MLMEBulkOutLock, IrqFlags);
+	// Reset MLME context flags
+	pMLMEContext->IRPPending = FALSE;
+	pMLMEContext->InUse = FALSE;
+	pMLMEContext->bWaitingBulkOut = FALSE;
+	pMLMEContext->BulkOutSize = 0;
+
+	pPacket = pAd->MgmtRing.Cell[index].pNdisPacket;
+	pAd->MgmtRing.Cell[index].pNdisPacket = NULL;
+
+	// Increase MgmtRing Index
+	INC_RING_INDEX(pAd->MgmtRing.TxDmaIdx, MGMT_RING_SIZE);
+	pAd->MgmtRing.TxSwFreeIdx++;
+	RTMP_IRQ_UNLOCK(&pAd->MLMEBulkOutLock, IrqFlags);
+
+	// No-matter success or fail, we free the mgmt packet.
+	if (pPacket)
+		RTMPFreeNdisPacket(pAd, pPacket);
+
+	if ((RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_RESET_IN_PROGRESS |
+								fRTMP_ADAPTER_HALT_IN_PROGRESS |
+								fRTMP_ADAPTER_NIC_NOT_EXIST))))
+	{
+		// do nothing and return directly.
+	}
+	else
+	{
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET) &&
+			((pAd->bulkResetPipeid & BULKOUT_MGMT_RESET_FLAG) == BULKOUT_MGMT_RESET_FLAG))
+		{	// For Mgmt Bulk-Out failed, ignore it now.
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+		}
+		else
+		{
+
+			// Always call Bulk routine, even reset bulk.
+			// The protectioon of rest bulk should be in BulkOut routine
+			if (pAd->MgmtRing.TxSwFreeIdx < MGMT_RING_SIZE /* pMLMEContext->bWaitingBulkOut == TRUE */)
+			{
+				RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_MLME);
+			}
+				RTUSBKickBulkOut(pAd);
+			}
+		}
+
+}
+
+
+static void rt2870_hcca_dma_done_tasklet(unsigned long data)
+{
+	PRTMP_ADAPTER		pAd;
+	PHT_TX_CONTEXT		pHTTXContext;
+	UCHAR				BulkOutPipeId = 4;
+	purbb_t				pUrb;
+
+
+	DBGPRINT_RAW(RT_DEBUG_ERROR, ("--->hcca_dma_done_tasklet\n"));
+
+
+	pUrb			= (purbb_t)data;
+	pHTTXContext	= (PHT_TX_CONTEXT)pUrb->context;
+	pAd				= pHTTXContext->pAd;
+
+	rt2870_dataout_complete_tasklet((unsigned long)pUrb);
+
+	if ((RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_RESET_IN_PROGRESS |
+								fRTMP_ADAPTER_HALT_IN_PROGRESS |
+								fRTMP_ADAPTER_NIC_NOT_EXIST))))
+	{
+		// do nothing and return directly.
+	}
+	else
+	{
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET))
+		{
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+		}
+		else
+		{	pHTTXContext = &pAd->TxContext[BulkOutPipeId];
+			if ((pAd->TxSwQueue[BulkOutPipeId].Number > 0) &&
+				/*((pHTTXContext->CurWritePosition > (pHTTXContext->NextBulkOutPosition + 0x6000)) || (pHTTXContext->NextBulkOutPosition > pHTTXContext->CurWritePosition + 0x6000)) && */
+				(pAd->DeQueueRunning[BulkOutPipeId] == FALSE) &&
+				(pHTTXContext->bCurWriting == FALSE))
+			{
+				RTMPDeQueuePacket(pAd, FALSE, BulkOutPipeId, MAX_TX_PROCESS);
+			}
+
+			RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL);
+			RTUSBKickBulkOut(pAd);
+		}
+	}
+
+	DBGPRINT_RAW(RT_DEBUG_ERROR, ("<---hcca_dma_done_tasklet\n"));
+
+		return;
+}
+
+
+static void rt2870_ac3_dma_done_tasklet(unsigned long data)
+{
+	PRTMP_ADAPTER		pAd;
+	PHT_TX_CONTEXT		pHTTXContext;
+	UCHAR				BulkOutPipeId = 3;
+	purbb_t				pUrb;
+
+
+	pUrb			= (purbb_t)data;
+	pHTTXContext	= (PHT_TX_CONTEXT)pUrb->context;
+	pAd				= pHTTXContext->pAd;
+
+	rt2870_dataout_complete_tasklet((unsigned long)pUrb);
+
+	if ((RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_RESET_IN_PROGRESS |
+								fRTMP_ADAPTER_HALT_IN_PROGRESS |
+								fRTMP_ADAPTER_NIC_NOT_EXIST))))
+	{
+		// do nothing and return directly.
+	}
+	else
+	{
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET))
+		{
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+		}
+		else
+		{	pHTTXContext = &pAd->TxContext[BulkOutPipeId];
+			if ((pAd->TxSwQueue[BulkOutPipeId].Number > 0) &&
+				/*((pHTTXContext->CurWritePosition > (pHTTXContext->NextBulkOutPosition + 0x6000)) || (pHTTXContext->NextBulkOutPosition > pHTTXContext->CurWritePosition + 0x6000)) && */
+				(pAd->DeQueueRunning[BulkOutPipeId] == FALSE) &&
+				(pHTTXContext->bCurWriting == FALSE))
+			{
+				RTMPDeQueuePacket(pAd, FALSE, BulkOutPipeId, MAX_TX_PROCESS);
+			}
+
+			RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL<<3);
+			RTUSBKickBulkOut(pAd);
+		}
+	}
+
+
+		return;
+}
+
+
+static void rt2870_ac2_dma_done_tasklet(unsigned long data)
+{
+	PRTMP_ADAPTER		pAd;
+	PHT_TX_CONTEXT		pHTTXContext;
+	UCHAR				BulkOutPipeId = 2;
+	purbb_t				pUrb;
+
+
+	pUrb			= (purbb_t)data;
+	pHTTXContext	= (PHT_TX_CONTEXT)pUrb->context;
+	pAd				= pHTTXContext->pAd;
+
+	rt2870_dataout_complete_tasklet((unsigned long)pUrb);
+
+	if ((RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_RESET_IN_PROGRESS |
+								fRTMP_ADAPTER_HALT_IN_PROGRESS |
+								fRTMP_ADAPTER_NIC_NOT_EXIST))))
+	{
+		// do nothing and return directly.
+	}
+	else
+	{
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET))
+		{
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+		}
+		else
+		{	pHTTXContext = &pAd->TxContext[BulkOutPipeId];
+			if ((pAd->TxSwQueue[BulkOutPipeId].Number > 0) &&
+				/*((pHTTXContext->CurWritePosition > (pHTTXContext->NextBulkOutPosition + 0x6000)) || (pHTTXContext->NextBulkOutPosition > pHTTXContext->CurWritePosition + 0x6000)) && */
+				(pAd->DeQueueRunning[BulkOutPipeId] == FALSE) &&
+				(pHTTXContext->bCurWriting == FALSE))
+			{
+				RTMPDeQueuePacket(pAd, FALSE, BulkOutPipeId, MAX_TX_PROCESS);
+			}
+
+			RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL<<2);
+			RTUSBKickBulkOut(pAd);
+		}
+	}
+
+		return;
+}
+
+
+static void rt2870_ac1_dma_done_tasklet(unsigned long data)
+{
+	PRTMP_ADAPTER		pAd;
+	PHT_TX_CONTEXT		pHTTXContext;
+	UCHAR				BulkOutPipeId = 1;
+	purbb_t				pUrb;
+
+
+	pUrb			= (purbb_t)data;
+	pHTTXContext	= (PHT_TX_CONTEXT)pUrb->context;
+	pAd				= pHTTXContext->pAd;
+
+	rt2870_dataout_complete_tasklet((unsigned long)pUrb);
+
+	if ((RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_RESET_IN_PROGRESS |
+								fRTMP_ADAPTER_HALT_IN_PROGRESS |
+								fRTMP_ADAPTER_NIC_NOT_EXIST))))
+	{
+		// do nothing and return directly.
+	}
+	else
+	{
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET))
+		{
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+		}
+		else
+		{	pHTTXContext = &pAd->TxContext[BulkOutPipeId];
+			if ((pAd->TxSwQueue[BulkOutPipeId].Number > 0) &&
+				/*((pHTTXContext->CurWritePosition > (pHTTXContext->NextBulkOutPosition + 0x6000)) || (pHTTXContext->NextBulkOutPosition > pHTTXContext->CurWritePosition + 0x6000)) && */
+				(pAd->DeQueueRunning[BulkOutPipeId] == FALSE) &&
+				(pHTTXContext->bCurWriting == FALSE))
+			{
+				RTMPDeQueuePacket(pAd, FALSE, BulkOutPipeId, MAX_TX_PROCESS);
+			}
+
+			RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL<<1);
+			RTUSBKickBulkOut(pAd);
+		}
+	}
+
+
+	return;
+}
+
+
+static void rt2870_ac0_dma_done_tasklet(unsigned long data)
+{
+	PRTMP_ADAPTER		pAd;
+	PHT_TX_CONTEXT		pHTTXContext;
+	UCHAR				BulkOutPipeId = 0;
+	purbb_t				pUrb;
+
+
+	pUrb			= (purbb_t)data;
+	pHTTXContext	= (PHT_TX_CONTEXT)pUrb->context;
+	pAd				= pHTTXContext->pAd;
+
+	rt2870_dataout_complete_tasklet((unsigned long)pUrb);
+
+	if ((RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_RESET_IN_PROGRESS |
+								fRTMP_ADAPTER_HALT_IN_PROGRESS |
+								fRTMP_ADAPTER_NIC_NOT_EXIST))))
+	{
+		// do nothing and return directly.
+	}
+	else
+	{
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET))
+		{
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+		}
+		else
+		{	pHTTXContext = &pAd->TxContext[BulkOutPipeId];
+			if ((pAd->TxSwQueue[BulkOutPipeId].Number > 0) &&
+				/*  ((pHTTXContext->CurWritePosition > (pHTTXContext->NextBulkOutPosition + 0x6000)) || (pHTTXContext->NextBulkOutPosition > pHTTXContext->CurWritePosition + 0x6000)) && */
+				(pAd->DeQueueRunning[BulkOutPipeId] == FALSE) &&
+				(pHTTXContext->bCurWriting == FALSE))
+			{
+				RTMPDeQueuePacket(pAd, FALSE, BulkOutPipeId, MAX_TX_PROCESS);
+			}
+
+			RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL);
+			RTUSBKickBulkOut(pAd);
+		}
+	}
+
+
+	return;
+
+}
+
+
+static void rt2870_null_frame_complete_tasklet(unsigned long data)
+{
+	PRTMP_ADAPTER	pAd;
+	PTX_CONTEXT		pNullContext;
+	purbb_t			pUrb;
+	NTSTATUS		Status;
+	unsigned long	irqFlag;
+
+
+	pUrb			= (purbb_t)data;
+	pNullContext	= (PTX_CONTEXT)pUrb->context;
+	pAd 			= pNullContext->pAd;
+	Status 			= pUrb->status;
+
+	// Reset Null frame context flags
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[0], irqFlag);
+	pNullContext->IRPPending 	= FALSE;
+	pNullContext->InUse 		= FALSE;
+	pAd->BulkOutPending[0] = FALSE;
+	pAd->watchDogTxPendingCnt[0] = 0;
+
+	if (Status == USB_ST_NOERROR)
+	{
+		RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], irqFlag);
+
+		RTMPDeQueuePacket(pAd, FALSE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+	}
+	else	// STATUS_OTHER
+	{
+		if ((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET)))
+		{
+			DBGPRINT_RAW(RT_DEBUG_ERROR, ("Bulk Out Null Frame Failed, ReasonCode=%d!\n", Status));
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+			pAd->bulkResetPipeid = (MGMTPIPEIDX | BULKOUT_MGMT_RESET_FLAG);
+			RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], irqFlag);
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+		}
+		else
+		{
+			RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], irqFlag);
+		}
+	}
+
+	// Always call Bulk routine, even reset bulk.
+	// The protectioon of rest bulk should be in BulkOut routine
+	RTUSBKickBulkOut(pAd);
+
+}
+
+
+static void rt2870_rts_frame_complete_tasklet(unsigned long data)
+{
+	PRTMP_ADAPTER	pAd;
+	PTX_CONTEXT		pRTSContext;
+	purbb_t			pUrb;
+	NTSTATUS		Status;
+	unsigned long	irqFlag;
+
+
+	pUrb		= (purbb_t)data;
+	pRTSContext	= (PTX_CONTEXT)pUrb->context;
+	pAd			= pRTSContext->pAd;
+	Status		= pUrb->status;
+
+	// Reset RTS frame context flags
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[0], irqFlag);
+	pRTSContext->IRPPending = FALSE;
+	pRTSContext->InUse		= FALSE;
+
+	if (Status == USB_ST_NOERROR)
+	{
+		RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], irqFlag);
+		RTMPDeQueuePacket(pAd, FALSE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+	}
+	else	// STATUS_OTHER
+	{
+		if ((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET)))
+		{
+			DBGPRINT_RAW(RT_DEBUG_ERROR, ("Bulk Out RTS Frame Failed\n"));
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+			pAd->bulkResetPipeid = (MGMTPIPEIDX | BULKOUT_MGMT_RESET_FLAG);
+			RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], irqFlag);
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+		}
+		else
+		{
+			RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], irqFlag);
+		}
+	}
+
+	RTMP_SEM_LOCK(&pAd->BulkOutLock[pRTSContext->BulkOutPipeId]);
+	pAd->BulkOutPending[pRTSContext->BulkOutPipeId] = FALSE;
+	RTMP_SEM_UNLOCK(&pAd->BulkOutLock[pRTSContext->BulkOutPipeId]);
+
+	// Always call Bulk routine, even reset bulk.
+	// The protectioon of rest bulk should be in BulkOut routine
+	RTUSBKickBulkOut(pAd);
+
+}
+
+
+static void rt2870_pspoll_frame_complete_tasklet(unsigned long data)
+{
+	PRTMP_ADAPTER	pAd;
+	PTX_CONTEXT		pPsPollContext;
+	purbb_t			pUrb;
+	NTSTATUS		Status;
+
+
+	pUrb			= (purbb_t)data;
+	pPsPollContext	= (PTX_CONTEXT)pUrb->context;
+	pAd				= pPsPollContext->pAd;
+	Status			= pUrb->status;
+
+	// Reset PsPoll context flags
+	pPsPollContext->IRPPending	= FALSE;
+	pPsPollContext->InUse		= FALSE;
+	pAd->watchDogTxPendingCnt[0] = 0;
+
+	if (Status == USB_ST_NOERROR)
+	{
+		RTMPDeQueuePacket(pAd, FALSE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+	}
+	else // STATUS_OTHER
+	{
+		if ((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET)))
+		{
+			DBGPRINT_RAW(RT_DEBUG_ERROR, ("Bulk Out PSPoll Failed\n"));
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+			pAd->bulkResetPipeid = (MGMTPIPEIDX | BULKOUT_MGMT_RESET_FLAG);
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+		}
+	}
+
+	RTMP_SEM_LOCK(&pAd->BulkOutLock[0]);
+	pAd->BulkOutPending[0] = FALSE;
+	RTMP_SEM_UNLOCK(&pAd->BulkOutLock[0]);
+
+	// Always call Bulk routine, even reset bulk.
+	// The protectioon of rest bulk should be in BulkOut routine
+	RTUSBKickBulkOut(pAd);
+
+}
+
+
+static void rt2870_dataout_complete_tasklet(unsigned long data)
+{
+	PRTMP_ADAPTER		pAd;
+	purbb_t				pUrb;
+	POS_COOKIE			pObj;
+	PHT_TX_CONTEXT		pHTTXContext;
+	UCHAR				BulkOutPipeId;
+	NTSTATUS			Status;
+	unsigned long		IrqFlags;
+
+
+	pUrb			= (purbb_t)data;
+	pHTTXContext	= (PHT_TX_CONTEXT)pUrb->context;
+	pAd				= pHTTXContext->pAd;
+	pObj 			= (POS_COOKIE) pAd->OS_Cookie;
+	Status			= pUrb->status;
+
+	// Store BulkOut PipeId
+	BulkOutPipeId = pHTTXContext->BulkOutPipeId;
+	pAd->BulkOutDataOneSecCount++;
+
+	//DBGPRINT(RT_DEBUG_LOUD, ("Done-B(%d):I=0x%lx, CWPos=%ld, NBPos=%ld, ENBPos=%ld, bCopy=%d!\n", BulkOutPipeId, in_interrupt(), pHTTXContext->CurWritePosition,
+	//		pHTTXContext->NextBulkOutPosition, pHTTXContext->ENextBulkOutPosition, pHTTXContext->bCopySavePad));
+
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+	pAd->BulkOutPending[BulkOutPipeId] = FALSE;
+	pHTTXContext->IRPPending = FALSE;
+	pAd->watchDogTxPendingCnt[BulkOutPipeId] = 0;
+
+	if (Status == USB_ST_NOERROR)
+	{
+		pAd->BulkOutComplete++;
+
+		RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+
+		pAd->Counters8023.GoodTransmits++;
+		//RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+		FREE_HTTX_RING(pAd, BulkOutPipeId, pHTTXContext);
+		//RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+
+
+	}
+	else	// STATUS_OTHER
+	{
+		PUCHAR	pBuf;
+
+		pAd->BulkOutCompleteOther++;
+
+		pBuf = &pHTTXContext->TransferBuffer->field.WirelessPacket[pHTTXContext->NextBulkOutPosition];
+
+		if (!RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_RESET_IN_PROGRESS |
+									fRTMP_ADAPTER_HALT_IN_PROGRESS |
+									fRTMP_ADAPTER_NIC_NOT_EXIST |
+									fRTMP_ADAPTER_BULKOUT_RESET)))
+		{
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+			pAd->bulkResetPipeid = BulkOutPipeId;
+			pAd->bulkResetReq[BulkOutPipeId] = pAd->BulkOutReq;
+		}
+		RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("BulkOutDataPacket failed: ReasonCode=%d!\n", Status));
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("\t>>BulkOut Req=0x%lx, Complete=0x%lx, Other=0x%lx\n", pAd->BulkOutReq, pAd->BulkOutComplete, pAd->BulkOutCompleteOther));
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("\t>>BulkOut Header:%x %x %x %x %x %x %x %x\n", pBuf[0], pBuf[1], pBuf[2], pBuf[3], pBuf[4], pBuf[5], pBuf[6], pBuf[7]));
+		//DBGPRINT_RAW(RT_DEBUG_ERROR, (">>BulkOutCompleteCancel=0x%x, BulkOutCompleteOther=0x%x\n", pAd->BulkOutCompleteCancel, pAd->BulkOutCompleteOther));
+
+	}
+
+	//
+	// bInUse = TRUE, means some process are filling TX data, after that must turn on bWaitingBulkOut
+	// bWaitingBulkOut = TRUE, means the TX data are waiting for bulk out.
+	//
+	//RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+	if ((pHTTXContext->ENextBulkOutPosition != pHTTXContext->CurWritePosition) &&
+		(pHTTXContext->ENextBulkOutPosition != (pHTTXContext->CurWritePosition+8)) &&
+		!RTUSB_TEST_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_FRAG << BulkOutPipeId)))
+	{
+		// Indicate There is data avaliable
+		RTUSB_SET_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_NORMAL << BulkOutPipeId));
+	}
+	//RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+
+	// Always call Bulk routine, even reset bulk.
+	// The protection of rest bulk should be in BulkOut routine
+	RTUSBKickBulkOut(pAd);
+}
+
+/* End of 2870_rtmp_init.c */
diff --git a/drivers/staging/rt2870/common/action.c b/drivers/staging/rt2870/common/action.c
new file mode 100644
index 00000000000..3a48a7f5e2b
--- /dev/null
+++ b/drivers/staging/rt2870/common/action.c
@@ -0,0 +1,1046 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+	action.c
+
+    Abstract:
+    Handle association related requests either from WSTA or from local MLME
+
+    Revision History:
+    Who         When          What
+    --------    ----------    ----------------------------------------------
+	Jan Lee		2006	  	created for rt2860
+ */
+
+#include "../rt_config.h"
+#include "action.h"
+
+
+static VOID ReservedAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+/*
+    ==========================================================================
+    Description:
+        association state machine init, including state transition and timer init
+    Parameters:
+        S - pointer to the association state machine
+    Note:
+        The state machine looks like the following
+
+                                    ASSOC_IDLE
+        MT2_MLME_DISASSOC_REQ    mlme_disassoc_req_action
+        MT2_PEER_DISASSOC_REQ    peer_disassoc_action
+        MT2_PEER_ASSOC_REQ       drop
+        MT2_PEER_REASSOC_REQ     drop
+        MT2_CLS3ERR              cls3err_action
+    ==========================================================================
+ */
+VOID ActionStateMachineInit(
+    IN	PRTMP_ADAPTER	pAd,
+    IN  STATE_MACHINE *S,
+    OUT STATE_MACHINE_FUNC Trans[])
+{
+	StateMachineInit(S, (STATE_MACHINE_FUNC *)Trans, MAX_ACT_STATE, MAX_ACT_MSG, (STATE_MACHINE_FUNC)Drop, ACT_IDLE, ACT_MACHINE_BASE);
+
+	StateMachineSetAction(S, ACT_IDLE, MT2_PEER_SPECTRUM_CATE, (STATE_MACHINE_FUNC)PeerSpectrumAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_PEER_QOS_CATE, (STATE_MACHINE_FUNC)PeerQOSAction);
+
+	StateMachineSetAction(S, ACT_IDLE, MT2_PEER_DLS_CATE, (STATE_MACHINE_FUNC)ReservedAction);
+#ifdef QOS_DLS_SUPPORT
+		StateMachineSetAction(S, ACT_IDLE, MT2_PEER_DLS_CATE, (STATE_MACHINE_FUNC)PeerDLSAction);
+#endif // QOS_DLS_SUPPORT //
+
+#ifdef DOT11_N_SUPPORT
+	StateMachineSetAction(S, ACT_IDLE, MT2_PEER_BA_CATE, (STATE_MACHINE_FUNC)PeerBAAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_PEER_HT_CATE, (STATE_MACHINE_FUNC)PeerHTAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_MLME_ADD_BA_CATE, (STATE_MACHINE_FUNC)MlmeADDBAAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_MLME_ORI_DELBA_CATE, (STATE_MACHINE_FUNC)MlmeDELBAAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_MLME_REC_DELBA_CATE, (STATE_MACHINE_FUNC)MlmeDELBAAction);
+#endif // DOT11_N_SUPPORT //
+
+	StateMachineSetAction(S, ACT_IDLE, MT2_PEER_PUBLIC_CATE, (STATE_MACHINE_FUNC)PeerPublicAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_PEER_RM_CATE, (STATE_MACHINE_FUNC)PeerRMAction);
+
+	StateMachineSetAction(S, ACT_IDLE, MT2_MLME_QOS_CATE, (STATE_MACHINE_FUNC)MlmeQOSAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_MLME_DLS_CATE, (STATE_MACHINE_FUNC)MlmeDLSAction);
+	StateMachineSetAction(S, ACT_IDLE, MT2_ACT_INVALID, (STATE_MACHINE_FUNC)MlmeInvalidAction);
+}
+
+#ifdef DOT11_N_SUPPORT
+VOID MlmeADDBAAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+
+{
+	MLME_ADDBA_REQ_STRUCT *pInfo;
+	UCHAR           Addr[6];
+	PUCHAR         pOutBuffer = NULL;
+	NDIS_STATUS     NStatus;
+	ULONG		Idx;
+	FRAME_ADDBA_REQ  Frame;
+	ULONG		FrameLen;
+	BA_ORI_ENTRY			*pBAEntry = NULL;
+
+	pInfo = (MLME_ADDBA_REQ_STRUCT *)Elem->Msg;
+	NdisZeroMemory(&Frame, sizeof(FRAME_ADDBA_REQ));
+
+	if(MlmeAddBAReqSanity(pAd, Elem->Msg, Elem->MsgLen, Addr))
+	{
+		NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+		if(NStatus != NDIS_STATUS_SUCCESS)
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("BA - MlmeADDBAAction() allocate memory failed \n"));
+			return;
+		}
+		// 1. find entry
+		Idx = pAd->MacTab.Content[pInfo->Wcid].BAOriWcidArray[pInfo->TID];
+		if (Idx == 0)
+		{
+			MlmeFreeMemory(pAd, pOutBuffer);
+			DBGPRINT(RT_DEBUG_ERROR,("BA - MlmeADDBAAction() can't find BAOriEntry \n"));
+			return;
+		}
+		else
+		{
+			pBAEntry =&pAd->BATable.BAOriEntry[Idx];
+		}
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			if (ADHOC_ON(pAd))
+				ActHeaderInit(pAd, &Frame.Hdr, pInfo->pAddr, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+			else
+#ifdef QOS_DLS_SUPPORT
+			if (pAd->MacTab.Content[pInfo->Wcid].ValidAsDls)
+				ActHeaderInit(pAd, &Frame.Hdr, pInfo->pAddr, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+			else
+#endif // QOS_DLS_SUPPORT //
+				ActHeaderInit(pAd, &Frame.Hdr, pAd->CommonCfg.Bssid, pAd->CurrentAddress, pInfo->pAddr);
+
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+		Frame.Category = CATEGORY_BA;
+		Frame.Action = ADDBA_REQ;
+		Frame.BaParm.AMSDUSupported = 0;
+		Frame.BaParm.BAPolicy = IMMED_BA;
+		Frame.BaParm.TID = pInfo->TID;
+		Frame.BaParm.BufSize = pInfo->BaBufSize;
+		Frame.Token = pInfo->Token;
+		Frame.TimeOutValue = pInfo->TimeOutValue;
+		Frame.BaStartSeq.field.FragNum = 0;
+		Frame.BaStartSeq.field.StartSeq = pAd->MacTab.Content[pInfo->Wcid].TxSeq[pInfo->TID];
+
+		*(USHORT *)(&Frame.BaParm) = cpu2le16(*(USHORT *)(&Frame.BaParm));
+		Frame.TimeOutValue = cpu2le16(Frame.TimeOutValue);
+		Frame.BaStartSeq.word = cpu2le16(Frame.BaStartSeq.word);
+
+		MakeOutgoingFrame(pOutBuffer,		   &FrameLen,
+		              sizeof(FRAME_ADDBA_REQ), &Frame,
+		              END_OF_ARGS);
+		MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+		MlmeFreeMemory(pAd, pOutBuffer);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("BA - Send ADDBA request. StartSeq = %x,  FrameLen = %ld. BufSize = %d\n", Frame.BaStartSeq.field.StartSeq, FrameLen, Frame.BaParm.BufSize));
+    }
+}
+
+/*
+    ==========================================================================
+    Description:
+        send DELBA and delete BaEntry if any
+    Parametrs:
+        Elem - MLME message MLME_DELBA_REQ_STRUCT
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID MlmeDELBAAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+	MLME_DELBA_REQ_STRUCT *pInfo;
+	PUCHAR         pOutBuffer = NULL;
+	PUCHAR		   pOutBuffer2 = NULL;
+	NDIS_STATUS     NStatus;
+	ULONG		Idx;
+	FRAME_DELBA_REQ  Frame;
+	ULONG		FrameLen;
+	FRAME_BAR	FrameBar;
+
+	pInfo = (MLME_DELBA_REQ_STRUCT *)Elem->Msg;
+	// must send back DELBA
+	NdisZeroMemory(&Frame, sizeof(FRAME_DELBA_REQ));
+	DBGPRINT(RT_DEBUG_TRACE, ("==> MlmeDELBAAction(), Initiator(%d) \n", pInfo->Initiator));
+
+	if(MlmeDelBAReqSanity(pAd, Elem->Msg, Elem->MsgLen))
+	{
+		NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+		if(NStatus != NDIS_STATUS_SUCCESS)
+		{
+			DBGPRINT(RT_DEBUG_ERROR,("BA - MlmeDELBAAction() allocate memory failed 1. \n"));
+			return;
+		}
+
+		NStatus = MlmeAllocateMemory(pAd, &pOutBuffer2);  //Get an unused nonpaged memory
+		if(NStatus != NDIS_STATUS_SUCCESS)
+		{
+			MlmeFreeMemory(pAd, pOutBuffer);
+			DBGPRINT(RT_DEBUG_ERROR, ("BA - MlmeDELBAAction() allocate memory failed 2. \n"));
+			return;
+		}
+
+		// SEND BAR (Send BAR to refresh peer reordering buffer.)
+		Idx = pAd->MacTab.Content[pInfo->Wcid].BAOriWcidArray[pInfo->TID];
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			BarHeaderInit(pAd, &FrameBar, pAd->MacTab.Content[pInfo->Wcid].Addr, pAd->CurrentAddress);
+#endif // CONFIG_STA_SUPPORT //
+
+		FrameBar.StartingSeq.field.FragNum = 0; // make sure sequence not clear in DEL funciton.
+		FrameBar.StartingSeq.field.StartSeq = pAd->MacTab.Content[pInfo->Wcid].TxSeq[pInfo->TID]; // make sure sequence not clear in DEL funciton.
+		FrameBar.BarControl.TID = pInfo->TID; // make sure sequence not clear in DEL funciton.
+		FrameBar.BarControl.ACKPolicy = IMMED_BA; // make sure sequence not clear in DEL funciton.
+		FrameBar.BarControl.Compressed = 1; // make sure sequence not clear in DEL funciton.
+		FrameBar.BarControl.MTID = 0; // make sure sequence not clear in DEL funciton.
+
+		MakeOutgoingFrame(pOutBuffer2,				&FrameLen,
+					  sizeof(FRAME_BAR),	  &FrameBar,
+					  END_OF_ARGS);
+		MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer2, FrameLen);
+		MlmeFreeMemory(pAd, pOutBuffer2);
+		DBGPRINT(RT_DEBUG_TRACE,("BA - MlmeDELBAAction() . Send BAR to refresh peer reordering buffer \n"));
+
+		// SEND DELBA FRAME
+		FrameLen = 0;
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			if (ADHOC_ON(pAd))
+				ActHeaderInit(pAd, &Frame.Hdr, pAd->MacTab.Content[pInfo->Wcid].Addr, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+			else
+#ifdef QOS_DLS_SUPPORT
+			if (pAd->MacTab.Content[pInfo->Wcid].ValidAsDls)
+				ActHeaderInit(pAd, &Frame.Hdr, pAd->MacTab.Content[pInfo->Wcid].Addr, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+			else
+#endif // QOS_DLS_SUPPORT //
+				ActHeaderInit(pAd, &Frame.Hdr,  pAd->CommonCfg.Bssid, pAd->CurrentAddress, pAd->MacTab.Content[pInfo->Wcid].Addr);
+		}
+#endif // CONFIG_STA_SUPPORT //
+		Frame.Category = CATEGORY_BA;
+		Frame.Action = DELBA;
+		Frame.DelbaParm.Initiator = pInfo->Initiator;
+		Frame.DelbaParm.TID = pInfo->TID;
+		Frame.ReasonCode = 39; // Time Out
+		*(USHORT *)(&Frame.DelbaParm) = cpu2le16(*(USHORT *)(&Frame.DelbaParm));
+		Frame.ReasonCode = cpu2le16(Frame.ReasonCode);
+
+		MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+		              sizeof(FRAME_DELBA_REQ),    &Frame,
+		              END_OF_ARGS);
+		MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+		MlmeFreeMemory(pAd, pOutBuffer);
+		DBGPRINT(RT_DEBUG_TRACE, ("BA - MlmeDELBAAction() . 3 DELBA sent. Initiator(%d)\n", pInfo->Initiator));
+    	}
+}
+#endif // DOT11_N_SUPPORT //
+
+VOID MlmeQOSAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+}
+
+VOID MlmeDLSAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+}
+
+VOID MlmeInvalidAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+	//PUCHAR		   pOutBuffer = NULL;
+	//Return the receiving frame except the MSB of category filed set to 1.  7.3.1.11
+}
+
+VOID PeerQOSAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+}
+
+#ifdef QOS_DLS_SUPPORT
+VOID PeerDLSAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR	Action = Elem->Msg[LENGTH_802_11+1];
+
+	switch(Action)
+	{
+		case ACTION_DLS_REQUEST:
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			PeerDlsReqAction(pAd, Elem);
+#endif // CONFIG_STA_SUPPORT //
+			break;
+
+		case ACTION_DLS_RESPONSE:
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			PeerDlsRspAction(pAd, Elem);
+#endif // CONFIG_STA_SUPPORT //
+			break;
+
+		case ACTION_DLS_TEARDOWN:
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			PeerDlsTearDownAction(pAd, Elem);
+#endif // CONFIG_STA_SUPPORT //
+			break;
+	}
+}
+#endif // QOS_DLS_SUPPORT //
+
+#ifdef DOT11_N_SUPPORT
+VOID PeerBAAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR	Action = Elem->Msg[LENGTH_802_11+1];
+
+	switch(Action)
+	{
+		case ADDBA_REQ:
+			PeerAddBAReqAction(pAd,Elem);
+			break;
+		case ADDBA_RESP:
+			PeerAddBARspAction(pAd,Elem);
+			break;
+		case DELBA:
+			PeerDelBAAction(pAd,Elem);
+			break;
+	}
+}
+
+
+#ifdef DOT11N_DRAFT3
+
+#ifdef CONFIG_STA_SUPPORT
+VOID StaPublicAction(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR Bss2040Coexist)
+{
+	BSS_2040_COEXIST_IE		BssCoexist;
+	MLME_SCAN_REQ_STRUCT			ScanReq;
+
+	BssCoexist.word = Bss2040Coexist;
+	// AP asks Station to return a 20/40 BSS Coexistence mgmt frame.  So we first starts a scan, then send back 20/40 BSS Coexistence mgmt frame
+	if ((BssCoexist.field.InfoReq == 1) && (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_SCAN_2040)))
+	{
+		// Clear record first.  After scan , will update those bit and send back to transmiter.
+		pAd->CommonCfg.BSSCoexist2040.field.InfoReq = 1;
+		pAd->CommonCfg.BSSCoexist2040.field.Intolerant40 = 0;
+		pAd->CommonCfg.BSSCoexist2040.field.BSS20WidthReq = 0;
+		// Fill out stuff for scan request
+		ScanParmFill(pAd, &ScanReq, ZeroSsid, 0, BSS_ANY, SCAN_2040_BSS_COEXIST);
+		MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_SCAN_REQ, sizeof(MLME_SCAN_REQ_STRUCT), &ScanReq);
+		pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_LIST_SCAN;
+	}
+}
+
+
+/*
+Description : Build Intolerant Channel Rerpot from Trigger event table.
+return : how many bytes copied.
+*/
+ULONG BuildIntolerantChannelRep(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    PUCHAR  pDest)
+{
+	ULONG			FrameLen = 0;
+	ULONG			ReadOffset = 0;
+	UCHAR			i;
+	UCHAR			LastRegClass = 0xff;
+	PUCHAR			pLen;
+
+	for ( i = 0;i < MAX_TRIGGER_EVENT;i++)
+	{
+		if (pAd->CommonCfg.TriggerEventTab.EventA[i].bValid == TRUE)
+		{
+			if (pAd->CommonCfg.TriggerEventTab.EventA[i].RegClass == LastRegClass)
+			{
+				*(pDest + ReadOffset) = (UCHAR)pAd->CommonCfg.TriggerEventTab.EventA[i].Channel;
+				*pLen++;
+				ReadOffset++;
+				FrameLen++;
+			}
+			else
+			{
+				*(pDest + ReadOffset) = IE_2040_BSS_INTOLERANT_REPORT;  // IE
+				*(pDest + ReadOffset + 1) = 2;	// Len = RegClass byte + channel byte.
+				pLen = pDest + ReadOffset + 1;
+				LastRegClass = pAd->CommonCfg.TriggerEventTab.EventA[i].RegClass;
+				*(pDest + ReadOffset + 2) = LastRegClass;	// Len = RegClass byte + channel byte.
+				*(pDest + ReadOffset + 3) = (UCHAR)pAd->CommonCfg.TriggerEventTab.EventA[i].Channel;
+				FrameLen += 4;
+				ReadOffset += 4;
+			}
+
+		}
+	}
+	return FrameLen;
+}
+
+
+/*
+Description : Send 20/40 BSS Coexistence Action frame If one trigger event is triggered.
+*/
+VOID Send2040CoexistAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR  Wcid,
+	IN	BOOLEAN	bAddIntolerantCha)
+{
+	PUCHAR			pOutBuffer = NULL;
+	NDIS_STATUS 	NStatus;
+	FRAME_ACTION_HDR	Frame;
+	ULONG			FrameLen;
+	ULONG			IntolerantChaRepLen;
+
+	IntolerantChaRepLen = 0;
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if(NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("ACT - Send2040CoexistAction() allocate memory failed \n"));
+		return;
+	}
+	ActHeaderInit(pAd, &Frame.Hdr, pAd->MacTab.Content[Wcid].Addr, pAd->CommonCfg.Bssid);
+	Frame.Category = CATEGORY_PUBLIC;
+	Frame.Action = ACTION_BSS_2040_COEXIST;
+
+	MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+				  sizeof(FRAME_ACTION_HDR),	  &Frame,
+				  END_OF_ARGS);
+
+	*(pOutBuffer + FrameLen) = pAd->CommonCfg.BSSCoexist2040.word;
+	FrameLen++;
+
+	if (bAddIntolerantCha == TRUE)
+		IntolerantChaRepLen = BuildIntolerantChannelRep(pAd, pOutBuffer + FrameLen);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen + IntolerantChaRepLen);
+	DBGPRINT(RT_DEBUG_ERROR,("ACT - Send2040CoexistAction( BSSCoexist2040 = 0x%x )  \n", pAd->CommonCfg.BSSCoexist2040.word));
+
+}
+
+
+/*
+	==========================================================================
+	Description:
+	After scan, Update 20/40 BSS Coexistence IE and send out.
+	According to 802.11n D3.03 11.14.10
+
+	Parameters:
+	==========================================================================
+ */
+VOID Update2040CoexistFrameAndNotify(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR  Wcid,
+	IN	BOOLEAN	bAddIntolerantCha)
+{
+	BSS_2040_COEXIST_IE	OldValue;
+
+	OldValue.word = pAd->CommonCfg.BSSCoexist2040.word;
+	if ((pAd->CommonCfg.TriggerEventTab.EventANo > 0) || (pAd->CommonCfg.TriggerEventTab.EventBCountDown > 0))
+		pAd->CommonCfg.BSSCoexist2040.field.BSS20WidthReq = 1;
+
+	// Need to check !!!!
+	// How STA will set Intolerant40 if implementation dependent. Now we don't set this bit first.!!!!!
+	// So Only check BSS20WidthReq change.
+	if (OldValue.field.BSS20WidthReq != pAd->CommonCfg.BSSCoexist2040.field.BSS20WidthReq)
+	{
+		Send2040CoexistAction(pAd, Wcid, bAddIntolerantCha);
+	}
+}
+#endif // CONFIG_STA_SUPPORT //
+
+
+BOOLEAN ChannelSwitchSanityCheck(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR  Wcid,
+	IN    UCHAR  NewChannel,
+	IN    UCHAR  Secondary)
+{
+	UCHAR		i;
+
+	if (Wcid >= MAX_LEN_OF_MAC_TABLE)
+		return FALSE;
+
+	if ((NewChannel > 7) && (Secondary == 1))
+		return FALSE;
+
+	if ((NewChannel < 5) && (Secondary == 3))
+		return FALSE;
+
+	// 0. Check if new channel is in the channellist.
+	for (i = 0;i < pAd->ChannelListNum;i++)
+	{
+		if (pAd->ChannelList[i].Channel == NewChannel)
+		{
+			break;
+		}
+	}
+
+	if (i == pAd->ChannelListNum)
+		return FALSE;
+
+	return TRUE;
+}
+
+
+VOID ChannelSwitchAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR  Wcid,
+	IN    UCHAR  NewChannel,
+	IN    UCHAR  Secondary)
+{
+	UCHAR		BBPValue = 0;
+	ULONG		MACValue;
+
+	DBGPRINT(RT_DEBUG_TRACE,("SPECTRUM - ChannelSwitchAction(NewChannel = %d , Secondary = %d)  \n", NewChannel, Secondary));
+
+	if (ChannelSwitchSanityCheck(pAd, Wcid, NewChannel, Secondary) == FALSE)
+		return;
+
+	// 1.  Switches to BW = 20.
+	if (Secondary == 0)
+	{
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+		BBPValue&= (~0x18);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+		if (pAd->MACVersion == 0x28600100)
+		{
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, 0x16);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, 0x08);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R73, 0x11);
+			DBGPRINT(RT_DEBUG_TRACE, ("!!!rt2860C !!! \n" ));
+		}
+		pAd->CommonCfg.BBPCurrentBW = BW_20;
+		pAd->CommonCfg.Channel = NewChannel;
+		pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel;
+		AsicSwitchChannel(pAd, pAd->CommonCfg.Channel,FALSE);
+		AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+		pAd->MacTab.Content[Wcid].HTPhyMode.field.BW = 0;
+		DBGPRINT(RT_DEBUG_TRACE, ("!!!20MHz   !!! \n" ));
+	}
+	// 1.  Switches to BW = 40 And Station supports BW = 40.
+	else if (((Secondary == 1) || (Secondary == 3)) && (pAd->CommonCfg.HtCapability.HtCapInfo.ChannelWidth == 1))
+	{
+		pAd->CommonCfg.Channel = NewChannel;
+
+		if (Secondary == 1)
+		{
+			// Secondary above.
+			pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel + 2;
+			RTMP_IO_READ32(pAd, TX_BAND_CFG, &MACValue);
+			MACValue &= 0xfe;
+			RTMP_IO_WRITE32(pAd, TX_BAND_CFG, MACValue);
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+			BBPValue&= (~0x18);
+			BBPValue|= (0x10);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BBPValue);
+			BBPValue&= (~0x20);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BBPValue);
+			DBGPRINT(RT_DEBUG_TRACE, ("!!!40MHz Lower LINK UP !!! Control Channel at Below. Central = %d \n", pAd->CommonCfg.CentralChannel ));
+		}
+		else
+		{
+			// Secondary below.
+			pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel - 2;
+			RTMP_IO_READ32(pAd, TX_BAND_CFG, &MACValue);
+			MACValue &= 0xfe;
+			MACValue |= 0x1;
+			RTMP_IO_WRITE32(pAd, TX_BAND_CFG, MACValue);
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+			BBPValue&= (~0x18);
+			BBPValue|= (0x10);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BBPValue);
+			BBPValue&= (~0x20);
+			BBPValue|= (0x20);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BBPValue);
+			DBGPRINT(RT_DEBUG_TRACE, ("!!!40MHz Upper LINK UP !!! Control Channel at UpperCentral = %d \n", pAd->CommonCfg.CentralChannel ));
+		}
+		pAd->CommonCfg.BBPCurrentBW = BW_40;
+		AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+		AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+		pAd->MacTab.Content[Wcid].HTPhyMode.field.BW = 1;
+	}
+}
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+
+VOID PeerPublicAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+#ifdef DOT11N_DRAFT3
+	UCHAR	Action = Elem->Msg[LENGTH_802_11+1];
+#endif // DOT11N_DRAFT3 //
+
+	if (Elem->Wcid >= MAX_LEN_OF_MAC_TABLE)
+		return;
+
+#ifdef DOT11N_DRAFT3
+	switch(Action)
+	{
+		case ACTION_BSS_2040_COEXIST:	// Format defined in IEEE 7.4.7a.1 in 11n Draf3.03
+			{
+				//UCHAR	BssCoexist;
+				BSS_2040_COEXIST_ELEMENT		*pCoexistInfo;
+				BSS_2040_COEXIST_IE 			*pBssCoexistIe;
+				BSS_2040_INTOLERANT_CH_REPORT	*pIntolerantReport = NULL;
+
+				if (Elem->MsgLen <= (LENGTH_802_11 + sizeof(BSS_2040_COEXIST_ELEMENT)) )
+				{
+					DBGPRINT(RT_DEBUG_ERROR, ("ACTION - 20/40 BSS Coexistence Management Frame length too short! len = %ld!\n", Elem->MsgLen));
+					break;
+				}
+				DBGPRINT(RT_DEBUG_TRACE, ("ACTION - 20/40 BSS Coexistence Management action----> \n"));
+				hex_dump("CoexistenceMgmtFrame", Elem->Msg, Elem->MsgLen);
+
+
+				pCoexistInfo = (BSS_2040_COEXIST_ELEMENT *) &Elem->Msg[LENGTH_802_11+2];
+				//hex_dump("CoexistInfo", (PUCHAR)pCoexistInfo, sizeof(BSS_2040_COEXIST_ELEMENT));
+				if (Elem->MsgLen >= (LENGTH_802_11 + sizeof(BSS_2040_COEXIST_ELEMENT) + sizeof(BSS_2040_INTOLERANT_CH_REPORT)))
+				{
+					pIntolerantReport = (BSS_2040_INTOLERANT_CH_REPORT *)((PUCHAR)pCoexistInfo + sizeof(BSS_2040_COEXIST_ELEMENT));
+				}
+				//hex_dump("IntolerantReport ", (PUCHAR)pIntolerantReport, sizeof(BSS_2040_INTOLERANT_CH_REPORT));
+
+				pBssCoexistIe = (BSS_2040_COEXIST_IE *)(&pCoexistInfo->BssCoexistIe);
+
+#ifdef CONFIG_STA_SUPPORT
+				IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				{
+					if (INFRA_ON(pAd))
+					{
+						StaPublicAction(pAd, pCoexistInfo);
+					}
+				}
+#endif // CONFIG_STA_SUPPORT //
+
+			}
+			break;
+	}
+
+#endif // DOT11N_DRAFT3 //
+
+}
+
+
+static VOID ReservedAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR Category;
+
+	if (Elem->MsgLen <= LENGTH_802_11)
+	{
+		return;
+	}
+
+	Category = Elem->Msg[LENGTH_802_11];
+	DBGPRINT(RT_DEBUG_TRACE,("Rcv reserved category(%d) Action Frame\n", Category));
+	hex_dump("Reserved Action Frame", &Elem->Msg[0], Elem->MsgLen);
+}
+
+VOID PeerRMAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+
+{
+	return;
+}
+
+#ifdef DOT11_N_SUPPORT
+static VOID respond_ht_information_exchange_action(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	PUCHAR			pOutBuffer = NULL;
+	NDIS_STATUS		NStatus;
+	ULONG			FrameLen;
+	FRAME_HT_INFO	HTINFOframe, *pFrame;
+	UCHAR   		*pAddr;
+
+
+	// 2. Always send back ADDBA Response
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);	 //Get an unused nonpaged memory
+
+	if (NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("ACTION - respond_ht_information_exchange_action() allocate memory failed \n"));
+		return;
+	}
+
+	// get RA
+	pFrame = (FRAME_HT_INFO *) &Elem->Msg[0];
+	pAddr = pFrame->Hdr.Addr2;
+
+	NdisZeroMemory(&HTINFOframe, sizeof(FRAME_HT_INFO));
+	// 2-1. Prepare ADDBA Response frame.
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (ADHOC_ON(pAd))
+			ActHeaderInit(pAd, &HTINFOframe.Hdr, pAddr, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+		else
+			ActHeaderInit(pAd, &HTINFOframe.Hdr, pAd->CommonCfg.Bssid, pAd->CurrentAddress, pAddr);
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	HTINFOframe.Category = CATEGORY_HT;
+	HTINFOframe.Action = HT_INFO_EXCHANGE;
+	HTINFOframe.HT_Info.Request = 0;
+	HTINFOframe.HT_Info.Forty_MHz_Intolerant = pAd->CommonCfg.HtCapability.HtCapInfo.Forty_Mhz_Intolerant;
+	HTINFOframe.HT_Info.STA_Channel_Width	 = pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth;
+
+	MakeOutgoingFrame(pOutBuffer,					&FrameLen,
+					  sizeof(FRAME_HT_INFO),	&HTINFOframe,
+					  END_OF_ARGS);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+}
+
+
+#ifdef DOT11N_DRAFT3
+VOID SendNotifyBWActionFrame(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR  Wcid,
+	IN UCHAR apidx)
+{
+	PUCHAR			pOutBuffer = NULL;
+	NDIS_STATUS 	NStatus;
+	FRAME_ACTION_HDR	Frame;
+	ULONG			FrameLen;
+	PUCHAR			pAddr1;
+
+
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if(NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("ACT - SendNotifyBWAction() allocate memory failed \n"));
+		return;
+	}
+
+	if (Wcid == MCAST_WCID)
+		pAddr1 = &BROADCAST_ADDR[0];
+	else
+		pAddr1 = pAd->MacTab.Content[Wcid].Addr;
+	ActHeaderInit(pAd, &Frame.Hdr, pAddr1, pAd->ApCfg.MBSSID[apidx].Bssid, pAd->ApCfg.MBSSID[apidx].Bssid);
+
+	Frame.Category = CATEGORY_HT;
+	Frame.Action = NOTIFY_BW_ACTION;
+
+	MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+				  sizeof(FRAME_ACTION_HDR),	  &Frame,
+				  END_OF_ARGS);
+
+	*(pOutBuffer + FrameLen) = pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth;
+	FrameLen++;
+
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	DBGPRINT(RT_DEBUG_TRACE,("ACT - SendNotifyBWAction(NotifyBW= %d)!\n", pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth));
+
+}
+#endif // DOT11N_DRAFT3 //
+
+
+VOID PeerHTAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR	Action = Elem->Msg[LENGTH_802_11+1];
+
+	if (Elem->Wcid >= MAX_LEN_OF_MAC_TABLE)
+		return;
+
+	switch(Action)
+	{
+		case NOTIFY_BW_ACTION:
+			DBGPRINT(RT_DEBUG_TRACE,("ACTION - HT Notify Channel bandwidth action----> \n"));
+#ifdef CONFIG_STA_SUPPORT
+			if(pAd->StaActive.SupportedPhyInfo.bHtEnable == FALSE)
+			{
+				// Note, this is to patch DIR-1353 AP. When the AP set to Wep, it will use legacy mode. But AP still keeps
+				// sending BW_Notify Action frame, and cause us to linkup and linkdown.
+				// In legacy mode, don't need to parse HT action frame.
+				DBGPRINT(RT_DEBUG_TRACE,("ACTION -Ignore HT Notify Channel BW when link as legacy mode. BW = %d---> \n",
+								Elem->Msg[LENGTH_802_11+2] ));
+				break;
+			}
+#endif // CONFIG_STA_SUPPORT //
+
+			if (Elem->Msg[LENGTH_802_11+2] == 0)	// 7.4.8.2. if value is 1, keep the same as supported channel bandwidth.
+				pAd->MacTab.Content[Elem->Wcid].HTPhyMode.field.BW = 0;
+
+			break;
+
+		case SMPS_ACTION:
+			// 7.3.1.25
+ 			DBGPRINT(RT_DEBUG_TRACE,("ACTION - SMPS action----> \n"));
+			if (((Elem->Msg[LENGTH_802_11+2]&0x1) == 0))
+			{
+				pAd->MacTab.Content[Elem->Wcid].MmpsMode = MMPS_ENABLE;
+			}
+			else if (((Elem->Msg[LENGTH_802_11+2]&0x2) == 0))
+			{
+				pAd->MacTab.Content[Elem->Wcid].MmpsMode = MMPS_STATIC;
+			}
+			else
+			{
+				pAd->MacTab.Content[Elem->Wcid].MmpsMode = MMPS_DYNAMIC;
+			}
+
+			DBGPRINT(RT_DEBUG_TRACE,("Aid(%d) MIMO PS = %d\n", Elem->Wcid, pAd->MacTab.Content[Elem->Wcid].MmpsMode));
+			// rt2860c : add something for smps change.
+			break;
+
+		case SETPCO_ACTION:
+			break;
+
+		case MIMO_CHA_MEASURE_ACTION:
+			break;
+
+		case HT_INFO_EXCHANGE:
+			{
+				HT_INFORMATION_OCTET	*pHT_info;
+
+				pHT_info = (HT_INFORMATION_OCTET *) &Elem->Msg[LENGTH_802_11+2];
+    				// 7.4.8.10
+    				DBGPRINT(RT_DEBUG_TRACE,("ACTION - HT Information Exchange action----> \n"));
+    				if (pHT_info->Request)
+    				{
+    					respond_ht_information_exchange_action(pAd, Elem);
+    				}
+			}
+    			break;
+	}
+}
+
+
+/*
+	==========================================================================
+	Description:
+		Retry sending ADDBA Reqest.
+
+	IRQL = DISPATCH_LEVEL
+
+	Parametrs:
+	p8023Header: if this is already 802.3 format, p8023Header is NULL
+
+	Return	: TRUE if put into rx reordering buffer, shouldn't indicaterxhere.
+				FALSE , then continue indicaterx at this moment.
+	==========================================================================
+ */
+VOID ORIBATimerTimeout(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	MAC_TABLE_ENTRY	*pEntry;
+	INT			i, total;
+//	FRAME_BAR			FrameBar;
+//	ULONG			FrameLen;
+//	NDIS_STATUS 	NStatus;
+//	PUCHAR			pOutBuffer = NULL;
+//	USHORT			Sequence;
+	UCHAR			TID;
+
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+	total = pAd->MacTab.Size * NUM_OF_TID;
+
+	for (i = 1; ((i <MAX_LEN_OF_BA_ORI_TABLE) && (total > 0)) ; i++)
+	{
+		if  (pAd->BATable.BAOriEntry[i].ORI_BA_Status == Originator_Done)
+		{
+			pEntry = &pAd->MacTab.Content[pAd->BATable.BAOriEntry[i].Wcid];
+			TID = pAd->BATable.BAOriEntry[i].TID;
+
+			ASSERT(pAd->BATable.BAOriEntry[i].Wcid < MAX_LEN_OF_MAC_TABLE);
+		}
+		total --;
+	}
+}
+
+
+VOID SendRefreshBAR(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry)
+{
+	FRAME_BAR		FrameBar;
+	ULONG			FrameLen;
+	NDIS_STATUS 	NStatus;
+	PUCHAR			pOutBuffer = NULL;
+	USHORT			Sequence;
+	UCHAR			i, TID;
+	USHORT			idx;
+	BA_ORI_ENTRY	*pBAEntry;
+
+	for (i = 0; i <NUM_OF_TID; i++)
+	{
+		idx = pEntry->BAOriWcidArray[i];
+		if (idx == 0)
+		{
+			continue;
+		}
+		pBAEntry = &pAd->BATable.BAOriEntry[idx];
+
+		if  (pBAEntry->ORI_BA_Status == Originator_Done)
+		{
+			TID = pBAEntry->TID;
+
+			ASSERT(pBAEntry->Wcid < MAX_LEN_OF_MAC_TABLE);
+
+			NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+			if(NStatus != NDIS_STATUS_SUCCESS)
+			{
+				DBGPRINT(RT_DEBUG_ERROR,("BA - MlmeADDBAAction() allocate memory failed \n"));
+				return;
+			}
+
+			Sequence = pEntry->TxSeq[TID];
+
+
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				BarHeaderInit(pAd, &FrameBar, pEntry->Addr, pAd->CurrentAddress);
+#endif // CONFIG_STA_SUPPORT //
+
+			FrameBar.StartingSeq.field.FragNum = 0; // make sure sequence not clear in DEL function.
+			FrameBar.StartingSeq.field.StartSeq = Sequence; // make sure sequence not clear in DEL funciton.
+			FrameBar.BarControl.TID = TID; // make sure sequence not clear in DEL funciton.
+
+			MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+							  sizeof(FRAME_BAR),	  &FrameBar,
+							  END_OF_ARGS);
+			//if (!(CLIENT_STATUS_TEST_FLAG(pEntry, fCLIENT_STATUS_RALINK_CHIPSET)))
+			if (1)	// Now we always send BAR.
+			{
+				//MiniportMMRequestUnlock(pAd, 0, pOutBuffer, FrameLen);
+				MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+			}
+			MlmeFreeMemory(pAd, pOutBuffer);
+		}
+	}
+}
+#endif // DOT11_N_SUPPORT //
+
+VOID ActHeaderInit(
+    IN	PRTMP_ADAPTER	pAd,
+    IN OUT PHEADER_802_11 pHdr80211,
+    IN PUCHAR Addr1,
+    IN PUCHAR Addr2,
+    IN PUCHAR Addr3)
+{
+    NdisZeroMemory(pHdr80211, sizeof(HEADER_802_11));
+    pHdr80211->FC.Type = BTYPE_MGMT;
+    pHdr80211->FC.SubType = SUBTYPE_ACTION;
+
+    COPY_MAC_ADDR(pHdr80211->Addr1, Addr1);
+	COPY_MAC_ADDR(pHdr80211->Addr2, Addr2);
+    COPY_MAC_ADDR(pHdr80211->Addr3, Addr3);
+}
+
+VOID BarHeaderInit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN OUT PFRAME_BAR pCntlBar,
+	IN PUCHAR pDA,
+	IN PUCHAR pSA)
+{
+//	USHORT	Duration;
+
+	NdisZeroMemory(pCntlBar, sizeof(FRAME_BAR));
+	pCntlBar->FC.Type = BTYPE_CNTL;
+	pCntlBar->FC.SubType = SUBTYPE_BLOCK_ACK_REQ;
+   	pCntlBar->BarControl.MTID = 0;
+	pCntlBar->BarControl.Compressed = 1;
+	pCntlBar->BarControl.ACKPolicy = 0;
+
+
+	pCntlBar->Duration = 16 + RTMPCalcDuration(pAd, RATE_1, sizeof(FRAME_BA));
+
+	COPY_MAC_ADDR(pCntlBar->Addr1, pDA);
+	COPY_MAC_ADDR(pCntlBar->Addr2, pSA);
+}
+
+
+/*
+	==========================================================================
+	Description:
+		Insert Category and action code into the action frame.
+
+	Parametrs:
+		1. frame buffer pointer.
+		2. frame length.
+		3. category code of the frame.
+		4. action code of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID InsertActField(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen,
+	IN UINT8 Category,
+	IN UINT8 ActCode)
+{
+	ULONG TempLen;
+
+	MakeOutgoingFrame(	pFrameBuf,		&TempLen,
+						1,				&Category,
+						1,				&ActCode,
+						END_OF_ARGS);
+
+	*pFrameLen = *pFrameLen + TempLen;
+
+	return;
+}
diff --git a/drivers/staging/rt2870/common/action.h b/drivers/staging/rt2870/common/action.h
new file mode 100644
index 00000000000..ce3877dce81
--- /dev/null
+++ b/drivers/staging/rt2870/common/action.h
@@ -0,0 +1,68 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	aironet.h
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+	Paul Lin	04-06-15		Initial
+*/
+
+#ifndef	__ACTION_H__
+#define	__ACTION_H__
+
+typedef struct PACKED __HT_INFO_OCTET
+{
+#ifdef RT_BIG_ENDIAN
+	UCHAR	Reserved:5;
+	UCHAR 	STA_Channel_Width:1;
+	UCHAR	Forty_MHz_Intolerant:1;
+	UCHAR	Request:1;
+#else
+	UCHAR	Request:1;
+	UCHAR	Forty_MHz_Intolerant:1;
+	UCHAR 	STA_Channel_Width:1;
+	UCHAR	Reserved:5;
+#endif
+} HT_INFORMATION_OCTET;
+
+
+typedef struct PACKED __FRAME_HT_INFO
+{
+	HEADER_802_11   		Hdr;
+	UCHAR					Category;
+	UCHAR					Action;
+	HT_INFORMATION_OCTET	HT_Info;
+}   FRAME_HT_INFO, *PFRAME_HT_INFO;
+
+#endif /* __ACTION_H__ */
+
+
diff --git a/drivers/staging/rt2870/common/ba_action.c b/drivers/staging/rt2870/common/ba_action.c
new file mode 100644
index 00000000000..95addb20bce
--- /dev/null
+++ b/drivers/staging/rt2870/common/ba_action.c
@@ -0,0 +1,1798 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+
+#ifdef DOT11_N_SUPPORT
+
+#include "../rt_config.h"
+
+
+
+#define BA_ORI_INIT_SEQ		(pEntry->TxSeq[TID]) //1			// inital sequence number of BA session
+
+#define ORI_SESSION_MAX_RETRY	8
+#define ORI_BA_SESSION_TIMEOUT	(2000)	// ms
+#define REC_BA_SESSION_IDLE_TIMEOUT	(1000)	// ms
+
+#define REORDERING_PACKET_TIMEOUT		((100 * HZ)/1000)	// system ticks -- 100 ms
+#define MAX_REORDERING_PACKET_TIMEOUT	((3000 * HZ)/1000)	// system ticks -- 100 ms
+
+#define RESET_RCV_SEQ		(0xFFFF)
+
+static void ba_mpdu_blk_free(PRTMP_ADAPTER pAd, struct reordering_mpdu *mpdu_blk);
+
+
+BA_ORI_ENTRY *BATableAllocOriEntry(
+								  IN  PRTMP_ADAPTER   pAd,
+								  OUT USHORT          *Idx);
+
+BA_REC_ENTRY *BATableAllocRecEntry(
+								  IN  PRTMP_ADAPTER   pAd,
+								  OUT USHORT          *Idx);
+
+VOID BAOriSessionSetupTimeout(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3);
+
+VOID BARecSessionIdleTimeout(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3);
+
+
+BUILD_TIMER_FUNCTION(BAOriSessionSetupTimeout);
+BUILD_TIMER_FUNCTION(BARecSessionIdleTimeout);
+
+#define ANNOUNCE_REORDERING_PACKET(_pAd, _mpdu_blk)	\
+			Announce_Reordering_Packet(_pAd, _mpdu_blk);
+
+VOID BA_MaxWinSizeReasign(
+	IN PRTMP_ADAPTER	pAd,
+	IN MAC_TABLE_ENTRY  *pEntryPeer,
+	OUT UCHAR			*pWinSize)
+{
+	UCHAR MaxSize;
+
+
+	if (pAd->MACVersion >= RALINK_2883_VERSION) // 3*3
+	{
+		if (pAd->MACVersion >= RALINK_3070_VERSION)
+		{
+			if (pEntryPeer->WepStatus != Ndis802_11EncryptionDisabled)
+				MaxSize = 7; // for non-open mode
+			else
+				MaxSize = 13;
+		}
+		else
+			MaxSize = 31;
+	}
+	else if (pAd->MACVersion >= RALINK_2880E_VERSION) // 2880 e
+	{
+		if (pEntryPeer->WepStatus != Ndis802_11EncryptionDisabled)
+			MaxSize = 7; // for non-open mode
+		else
+			MaxSize = 13;
+	}
+	else
+		MaxSize = 7;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("ba> Win Size = %d, Max Size = %d\n",
+			*pWinSize, MaxSize));
+
+	if ((*pWinSize) > MaxSize)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("ba> reassign max win size from %d to %d\n",
+				*pWinSize, MaxSize));
+
+		*pWinSize = MaxSize;
+	}
+}
+
+void Announce_Reordering_Packet(IN PRTMP_ADAPTER			pAd,
+								IN struct reordering_mpdu	*mpdu)
+{
+	PNDIS_PACKET    pPacket;
+
+	pPacket = mpdu->pPacket;
+
+	if (mpdu->bAMSDU)
+	{
+		ASSERT(0);
+		BA_Reorder_AMSDU_Annnounce(pAd, pPacket);
+	}
+	else
+	{
+		//
+		// pass this 802.3 packet to upper layer or forward this packet to WM directly
+		//
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			ANNOUNCE_OR_FORWARD_802_3_PACKET(pAd, pPacket, RTMP_GET_PACKET_IF(pPacket));
+#endif // CONFIG_STA_SUPPORT //
+	}
+}
+
+/*
+ * Insert a reordering mpdu into sorted linked list by sequence no.
+ */
+BOOLEAN ba_reordering_mpdu_insertsorted(struct reordering_list *list, struct reordering_mpdu *mpdu)
+{
+
+	struct reordering_mpdu **ppScan = &list->next;
+
+	while (*ppScan != NULL)
+	{
+		if (SEQ_SMALLER((*ppScan)->Sequence, mpdu->Sequence, MAXSEQ))
+		{
+			ppScan = &(*ppScan)->next;
+		}
+		else if ((*ppScan)->Sequence == mpdu->Sequence)
+		{
+			/* give up this duplicated frame */
+			return(FALSE);
+		}
+		else
+		{
+			/* find position */
+			break;
+		}
+	}
+
+	mpdu->next = *ppScan;
+	*ppScan = mpdu;
+	list->qlen++;
+	return TRUE;
+}
+
+
+/*
+ * caller lock critical section if necessary
+ */
+static inline void ba_enqueue(struct reordering_list *list, struct reordering_mpdu *mpdu_blk)
+{
+	list->qlen++;
+	mpdu_blk->next = list->next;
+	list->next = mpdu_blk;
+}
+
+/*
+ * caller lock critical section if necessary
+ */
+static inline struct reordering_mpdu * ba_dequeue(struct reordering_list *list)
+{
+	struct reordering_mpdu *mpdu_blk = NULL;
+
+	ASSERT(list);
+
+		if (list->qlen)
+		{
+			list->qlen--;
+			mpdu_blk = list->next;
+			if (mpdu_blk)
+			{
+				list->next = mpdu_blk->next;
+				mpdu_blk->next = NULL;
+			}
+		}
+	return mpdu_blk;
+}
+
+
+static inline struct reordering_mpdu  *ba_reordering_mpdu_dequeue(struct reordering_list *list)
+{
+	return(ba_dequeue(list));
+}
+
+
+static inline struct reordering_mpdu  *ba_reordering_mpdu_probe(struct reordering_list *list)
+	{
+	ASSERT(list);
+
+		return(list->next);
+	}
+
+
+/*
+ * free all resource for reordering mechanism
+ */
+void ba_reordering_resource_release(PRTMP_ADAPTER pAd)
+{
+	BA_TABLE        *Tab;
+	PBA_REC_ENTRY   pBAEntry;
+	struct reordering_mpdu *mpdu_blk;
+	int i;
+
+	Tab = &pAd->BATable;
+
+	/* I.  release all pending reordering packet */
+	NdisAcquireSpinLock(&pAd->BATabLock);
+	for (i = 0; i < MAX_LEN_OF_BA_REC_TABLE; i++)
+	{
+		pBAEntry = &Tab->BARecEntry[i];
+		if (pBAEntry->REC_BA_Status != Recipient_NONE)
+		{
+			while ((mpdu_blk = ba_reordering_mpdu_dequeue(&pBAEntry->list)))
+			{
+				ASSERT(mpdu_blk->pPacket);
+				RELEASE_NDIS_PACKET(pAd, mpdu_blk->pPacket, NDIS_STATUS_FAILURE);
+				ba_mpdu_blk_free(pAd, mpdu_blk);
+			}
+		}
+	}
+	NdisReleaseSpinLock(&pAd->BATabLock);
+
+	ASSERT(pBAEntry->list.qlen == 0);
+	/* II. free memory of reordering mpdu table */
+	NdisAcquireSpinLock(&pAd->mpdu_blk_pool.lock);
+	os_free_mem(pAd, pAd->mpdu_blk_pool.mem);
+	NdisReleaseSpinLock(&pAd->mpdu_blk_pool.lock);
+}
+
+
+
+/*
+ * Allocate all resource for reordering mechanism
+ */
+BOOLEAN ba_reordering_resource_init(PRTMP_ADAPTER pAd, int num)
+{
+	int     i;
+	PUCHAR  mem;
+	struct reordering_mpdu *mpdu_blk;
+	struct reordering_list *freelist;
+
+	/* allocate spinlock */
+	NdisAllocateSpinLock(&pAd->mpdu_blk_pool.lock);
+
+	/* initialize freelist */
+	freelist = &pAd->mpdu_blk_pool.freelist;
+	freelist->next = NULL;
+	freelist->qlen = 0;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Allocate %d memory for BA reordering\n", (UINT32)(num*sizeof(struct reordering_mpdu))));
+
+	/* allocate number of mpdu_blk memory */
+	os_alloc_mem(pAd, (PUCHAR *)&mem, (num*sizeof(struct reordering_mpdu)));
+
+	pAd->mpdu_blk_pool.mem = mem;
+
+	if (mem == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Can't Allocate Memory for BA Reordering\n"));
+		return(FALSE);
+	}
+
+	/* build mpdu_blk free list */
+	for (i=0; i<num; i++)
+	{
+		/* get mpdu_blk */
+		mpdu_blk = (struct reordering_mpdu *) mem;
+		/* initial mpdu_blk */
+		NdisZeroMemory(mpdu_blk, sizeof(struct reordering_mpdu));
+		/* next mpdu_blk */
+		mem += sizeof(struct reordering_mpdu);
+		/* insert mpdu_blk into freelist */
+		ba_enqueue(freelist, mpdu_blk);
+	}
+
+	return(TRUE);
+}
+
+//static int blk_count=0; // sample take off, no use
+
+static struct reordering_mpdu *ba_mpdu_blk_alloc(PRTMP_ADAPTER pAd)
+{
+	struct reordering_mpdu *mpdu_blk;
+
+	NdisAcquireSpinLock(&pAd->mpdu_blk_pool.lock);
+	mpdu_blk = ba_dequeue(&pAd->mpdu_blk_pool.freelist);
+	if (mpdu_blk)
+	{
+//		blk_count++;
+		/* reset mpdu_blk */
+		NdisZeroMemory(mpdu_blk, sizeof(struct reordering_mpdu));
+	}
+	NdisReleaseSpinLock(&pAd->mpdu_blk_pool.lock);
+	return mpdu_blk;
+}
+
+static void ba_mpdu_blk_free(PRTMP_ADAPTER pAd, struct reordering_mpdu *mpdu_blk)
+{
+	ASSERT(mpdu_blk);
+
+	NdisAcquireSpinLock(&pAd->mpdu_blk_pool.lock);
+//	blk_count--;
+	ba_enqueue(&pAd->mpdu_blk_pool.freelist, mpdu_blk);
+	NdisReleaseSpinLock(&pAd->mpdu_blk_pool.lock);
+}
+
+
+static USHORT ba_indicate_reordering_mpdus_in_order(
+												   IN PRTMP_ADAPTER    pAd,
+												   IN PBA_REC_ENTRY    pBAEntry,
+												   IN USHORT           StartSeq)
+{
+	struct reordering_mpdu *mpdu_blk;
+	USHORT  LastIndSeq = RESET_RCV_SEQ;
+
+	NdisAcquireSpinLock(&pBAEntry->RxReRingLock);
+
+	while ((mpdu_blk = ba_reordering_mpdu_probe(&pBAEntry->list)))
+		{
+			/* find in-order frame */
+		if (!SEQ_STEPONE(mpdu_blk->Sequence, StartSeq, MAXSEQ))
+			{
+				break;
+			}
+			/* dequeue in-order frame from reodering list */
+			mpdu_blk = ba_reordering_mpdu_dequeue(&pBAEntry->list);
+			/* pass this frame up */
+		ANNOUNCE_REORDERING_PACKET(pAd, mpdu_blk);
+		/* move to next sequence */
+			StartSeq = mpdu_blk->Sequence;
+		LastIndSeq = StartSeq;
+		/* free mpdu_blk */
+			ba_mpdu_blk_free(pAd, mpdu_blk);
+	}
+
+	NdisReleaseSpinLock(&pBAEntry->RxReRingLock);
+
+	/* update last indicated sequence */
+	return LastIndSeq;
+}
+
+static void ba_indicate_reordering_mpdus_le_seq(
+											   IN PRTMP_ADAPTER    pAd,
+											   IN PBA_REC_ENTRY    pBAEntry,
+											   IN USHORT           Sequence)
+{
+	struct reordering_mpdu *mpdu_blk;
+
+	NdisAcquireSpinLock(&pBAEntry->RxReRingLock);
+	while ((mpdu_blk = ba_reordering_mpdu_probe(&pBAEntry->list)))
+		{
+			/* find in-order frame */
+		if ((mpdu_blk->Sequence == Sequence) || SEQ_SMALLER(mpdu_blk->Sequence, Sequence, MAXSEQ))
+		{
+			/* dequeue in-order frame from reodering list */
+			mpdu_blk = ba_reordering_mpdu_dequeue(&pBAEntry->list);
+			/* pass this frame up */
+			ANNOUNCE_REORDERING_PACKET(pAd, mpdu_blk);
+			/* free mpdu_blk */
+			ba_mpdu_blk_free(pAd, mpdu_blk);
+		}
+		else
+			{
+				break;
+			}
+	}
+	NdisReleaseSpinLock(&pBAEntry->RxReRingLock);
+}
+
+
+static void ba_refresh_reordering_mpdus(
+									   IN PRTMP_ADAPTER    pAd,
+									   PBA_REC_ENTRY       pBAEntry)
+{
+	struct reordering_mpdu *mpdu_blk;
+
+	NdisAcquireSpinLock(&pBAEntry->RxReRingLock);
+
+			/* dequeue in-order frame from reodering list */
+	while ((mpdu_blk = ba_reordering_mpdu_dequeue(&pBAEntry->list)))
+	{
+			/* pass this frame up */
+		ANNOUNCE_REORDERING_PACKET(pAd, mpdu_blk);
+
+		pBAEntry->LastIndSeq = mpdu_blk->Sequence;
+			ba_mpdu_blk_free(pAd, mpdu_blk);
+
+		/* update last indicated sequence */
+	}
+	ASSERT(pBAEntry->list.qlen == 0);
+	pBAEntry->LastIndSeq = RESET_RCV_SEQ;
+	NdisReleaseSpinLock(&pBAEntry->RxReRingLock);
+}
+
+
+//static
+void ba_flush_reordering_timeout_mpdus(
+									IN PRTMP_ADAPTER    pAd,
+									IN PBA_REC_ENTRY    pBAEntry,
+									IN ULONG            Now32)
+
+{
+	USHORT Sequence;
+
+//	if ((RTMP_TIME_AFTER((unsigned long)Now32, (unsigned long)(pBAEntry->LastIndSeqAtTimer+REORDERING_PACKET_TIMEOUT)) &&
+//		 (pBAEntry->list.qlen > ((pBAEntry->BAWinSize*7)/8))) //||
+//		(RTMP_TIME_AFTER((unsigned long)Now32, (unsigned long)(pBAEntry->LastIndSeqAtTimer+(10*REORDERING_PACKET_TIMEOUT))) &&
+//		 (pBAEntry->list.qlen > (pBAEntry->BAWinSize/8)))
+	if (RTMP_TIME_AFTER((unsigned long)Now32, (unsigned long)(pBAEntry->LastIndSeqAtTimer+(MAX_REORDERING_PACKET_TIMEOUT/6)))
+		 &&(pBAEntry->list.qlen > 1)
+		)
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("timeout[%d] (%08lx-%08lx = %d > %d): %x, flush all!\n ", pBAEntry->list.qlen, Now32, (pBAEntry->LastIndSeqAtTimer),
+			   (int)((long) Now32 - (long)(pBAEntry->LastIndSeqAtTimer)), MAX_REORDERING_PACKET_TIMEOUT,
+			   pBAEntry->LastIndSeq));
+		ba_refresh_reordering_mpdus(pAd, pBAEntry);
+		pBAEntry->LastIndSeqAtTimer = Now32;
+	}
+	else
+	if (RTMP_TIME_AFTER((unsigned long)Now32, (unsigned long)(pBAEntry->LastIndSeqAtTimer+(REORDERING_PACKET_TIMEOUT)))
+		&& (pBAEntry->list.qlen > 0)
+	   )
+		{
+    		//
+		// force LastIndSeq to shift to LastIndSeq+1
+    		//
+    		Sequence = (pBAEntry->LastIndSeq+1) & MAXSEQ;
+    		ba_indicate_reordering_mpdus_le_seq(pAd, pBAEntry, Sequence);
+    		pBAEntry->LastIndSeqAtTimer = Now32;
+			pBAEntry->LastIndSeq = Sequence;
+    		//
+    		// indicate in-order mpdus
+    		//
+    		Sequence = ba_indicate_reordering_mpdus_in_order(pAd, pBAEntry, Sequence);
+    		if (Sequence != RESET_RCV_SEQ)
+    		{
+    			pBAEntry->LastIndSeq = Sequence;
+    		}
+
+	}
+#if 0
+	else if (
+			 (RTMP_TIME_AFTER((unsigned long)Now32, (unsigned long)(pBAEntry->LastIndSeqAtTimer+(MAX_REORDERING_PACKET_TIMEOUT))) &&
+			  (pBAEntry->list.qlen > 1))
+			)
+		{
+		DBGPRINT(RT_DEBUG_TRACE,("timeout[%d] (%lx-%lx = %d > %d): %x\n ", pBAEntry->list.qlen, Now32, (pBAEntry->LastIndSeqAtTimer),
+			   (int)((long) Now32 - (long)(pBAEntry->LastIndSeqAtTimer)), MAX_REORDERING_PACKET_TIMEOUT,
+			   pBAEntry->LastIndSeq));
+		ba_refresh_reordering_mpdus(pAd, pBAEntry);
+			pBAEntry->LastIndSeqAtTimer = Now32;
+				}
+#endif
+}
+
+
+/*
+ * generate ADDBA request to
+ * set up BA agreement
+ */
+VOID BAOriSessionSetUp(
+					  IN PRTMP_ADAPTER    pAd,
+					  IN MAC_TABLE_ENTRY  *pEntry,
+					  IN UCHAR            TID,
+					  IN USHORT           TimeOut,
+					  IN ULONG            DelayTime,
+					  IN BOOLEAN          isForced)
+
+{
+	//MLME_ADDBA_REQ_STRUCT	AddbaReq;
+	BA_ORI_ENTRY            *pBAEntry = NULL;
+	USHORT                  Idx;
+	BOOLEAN                 Cancelled;
+
+	if ((pAd->CommonCfg.BACapability.field.AutoBA != TRUE)  &&  (isForced == FALSE))
+		return;
+
+	// if this entry is limited to use legacy tx mode, it doesn't generate BA.
+	if (RTMPStaFixedTxMode(pAd, pEntry) != FIXED_TXMODE_HT)
+		return;
+
+	if ((pEntry->BADeclineBitmap & (1<<TID)) && (isForced == FALSE))
+	{
+		// try again after 3 secs
+		DelayTime = 3000;
+//		printk("DeCline BA from Peer\n");
+//		return;
+	}
+
+
+	Idx = pEntry->BAOriWcidArray[TID];
+	if (Idx == 0)
+	{
+		// allocate a BA session
+		pBAEntry = BATableAllocOriEntry(pAd, &Idx);
+		if (pBAEntry == NULL)
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("ADDBA - MlmeADDBAAction() allocate BA session failed \n"));
+			return;
+		}
+	}
+	else
+	{
+		pBAEntry =&pAd->BATable.BAOriEntry[Idx];
+	}
+
+	if (pBAEntry->ORI_BA_Status >= Originator_WaitRes)
+	{
+		return;
+	}
+
+	pEntry->BAOriWcidArray[TID] = Idx;
+
+	// Initialize BA session
+	pBAEntry->ORI_BA_Status = Originator_WaitRes;
+	pBAEntry->Wcid = pEntry->Aid;
+	pBAEntry->BAWinSize = pAd->CommonCfg.BACapability.field.RxBAWinLimit;
+	pBAEntry->Sequence = BA_ORI_INIT_SEQ;
+	pBAEntry->Token = 1;	// (2008-01-21) Jan Lee recommends it - this token can't be 0
+	pBAEntry->TID = TID;
+	pBAEntry->TimeOutValue = TimeOut;
+	pBAEntry->pAdapter = pAd;
+
+	if (!(pEntry->TXBAbitmap & (1<<TID)))
+	{
+		RTMPInitTimer(pAd, &pBAEntry->ORIBATimer, GET_TIMER_FUNCTION(BAOriSessionSetupTimeout), pBAEntry, FALSE);
+	}
+	else
+		RTMPCancelTimer(&pBAEntry->ORIBATimer, &Cancelled);
+
+	// set timer to send ADDBA request
+	RTMPSetTimer(&pBAEntry->ORIBATimer, DelayTime);
+}
+
+VOID BAOriSessionAdd(
+			IN PRTMP_ADAPTER    pAd,
+					IN MAC_TABLE_ENTRY  *pEntry,
+			IN PFRAME_ADDBA_RSP pFrame)
+{
+	BA_ORI_ENTRY  *pBAEntry = NULL;
+	BOOLEAN       Cancelled;
+	UCHAR         TID;
+	USHORT        Idx;
+	PUCHAR          pOutBuffer2 = NULL;
+	NDIS_STATUS     NStatus;
+	ULONG           FrameLen;
+	FRAME_BAR       FrameBar;
+
+	TID = pFrame->BaParm.TID;
+	Idx = pEntry->BAOriWcidArray[TID];
+	pBAEntry =&pAd->BATable.BAOriEntry[Idx];
+
+	// Start fill in parameters.
+	if ((Idx !=0) && (pBAEntry->TID == TID) && (pBAEntry->ORI_BA_Status == Originator_WaitRes))
+	{
+		pBAEntry->BAWinSize = min(pBAEntry->BAWinSize, ((UCHAR)pFrame->BaParm.BufSize));
+		BA_MaxWinSizeReasign(pAd, pEntry, &pBAEntry->BAWinSize);
+
+		pBAEntry->TimeOutValue = pFrame->TimeOutValue;
+		pBAEntry->ORI_BA_Status = Originator_Done;
+		// reset sequence number
+		pBAEntry->Sequence = BA_ORI_INIT_SEQ;
+		// Set Bitmap flag.
+		pEntry->TXBAbitmap |= (1<<TID);
+				RTMPCancelTimer(&pBAEntry->ORIBATimer, &Cancelled);
+
+		pBAEntry->ORIBATimer.TimerValue = 0;	//pFrame->TimeOutValue;
+
+		DBGPRINT(RT_DEBUG_TRACE,("%s : TXBAbitmap = %x, BAWinSize = %d, TimeOut = %ld\n", __FUNCTION__, pEntry->TXBAbitmap,
+								 pBAEntry->BAWinSize, pBAEntry->ORIBATimer.TimerValue));
+
+		// SEND BAR ;
+		NStatus = MlmeAllocateMemory(pAd, &pOutBuffer2);  //Get an unused nonpaged memory
+		if (NStatus != NDIS_STATUS_SUCCESS)
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("BA - BAOriSessionAdd() allocate memory failed \n"));
+			return;
+		}
+
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			BarHeaderInit(pAd, &FrameBar, pAd->MacTab.Content[pBAEntry->Wcid].Addr, pAd->CurrentAddress);
+#endif // CONFIG_STA_SUPPORT //
+
+		FrameBar.StartingSeq.field.FragNum = 0;	// make sure sequence not clear in DEL function.
+		FrameBar.StartingSeq.field.StartSeq = pBAEntry->Sequence; // make sure sequence not clear in DEL funciton.
+		FrameBar.BarControl.TID = pBAEntry->TID; // make sure sequence not clear in DEL funciton.
+		MakeOutgoingFrame(pOutBuffer2,              &FrameLen,
+						  sizeof(FRAME_BAR),      &FrameBar,
+					  END_OF_ARGS);
+		MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer2, FrameLen);
+		MlmeFreeMemory(pAd, pOutBuffer2);
+
+
+		if (pBAEntry->ORIBATimer.TimerValue)
+			RTMPSetTimer(&pBAEntry->ORIBATimer, pBAEntry->ORIBATimer.TimerValue); // in mSec
+	}
+}
+
+BOOLEAN BARecSessionAdd(
+					   IN PRTMP_ADAPTER    pAd,
+					   IN MAC_TABLE_ENTRY  *pEntry,
+					   IN PFRAME_ADDBA_REQ pFrame)
+{
+	BA_REC_ENTRY            *pBAEntry = NULL;
+	BOOLEAN                 Status = TRUE;
+	BOOLEAN                 Cancelled;
+	USHORT                  Idx;
+	UCHAR                   TID;
+	UCHAR                   BAWinSize;
+	//UINT32                  Value;
+	//UINT                    offset;
+
+
+	ASSERT(pEntry);
+
+	// find TID
+	TID = pFrame->BaParm.TID;
+
+	BAWinSize = min(((UCHAR)pFrame->BaParm.BufSize), (UCHAR)pAd->CommonCfg.BACapability.field.RxBAWinLimit);
+
+	// Intel patch
+	if (BAWinSize == 0)
+	{
+		BAWinSize = 64;
+	}
+
+	Idx = pEntry->BARecWcidArray[TID];
+
+
+	if (Idx == 0)
+	{
+		pBAEntry = BATableAllocRecEntry(pAd, &Idx);
+	}
+	else
+	{
+		pBAEntry = &pAd->BATable.BARecEntry[Idx];
+		// flush all pending reordering mpdus
+		ba_refresh_reordering_mpdus(pAd, pBAEntry);
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("%s(%ld): Idx = %d, BAWinSize(req %d) = %d\n", __FUNCTION__, pAd->BATable.numAsRecipient, Idx,
+							 pFrame->BaParm.BufSize, BAWinSize));
+
+	// Start fill in parameters.
+	if (pBAEntry != NULL)
+	{
+		ASSERT(pBAEntry->list.qlen == 0);
+
+		pBAEntry->REC_BA_Status = Recipient_HandleRes;
+		pBAEntry->BAWinSize = BAWinSize;
+		pBAEntry->Wcid = pEntry->Aid;
+		pBAEntry->TID = TID;
+		pBAEntry->TimeOutValue = pFrame->TimeOutValue;
+		pBAEntry->REC_BA_Status = Recipient_Accept;
+		// initial sequence number
+		pBAEntry->LastIndSeq = RESET_RCV_SEQ; //pFrame->BaStartSeq.field.StartSeq;
+
+		printk("Start Seq = %08x\n",  pFrame->BaStartSeq.field.StartSeq);
+
+		if (pEntry->RXBAbitmap & (1<<TID))
+		{
+			RTMPCancelTimer(&pBAEntry->RECBATimer, &Cancelled);
+		}
+		else
+		{
+			RTMPInitTimer(pAd, &pBAEntry->RECBATimer, GET_TIMER_FUNCTION(BARecSessionIdleTimeout), pBAEntry, TRUE);
+		}
+
+#if 0	// for debugging
+		RTMPSetTimer(&pBAEntry->RECBATimer, REC_BA_SESSION_IDLE_TIMEOUT);
+#endif
+
+		// Set Bitmap flag.
+		pEntry->RXBAbitmap |= (1<<TID);
+		pEntry->BARecWcidArray[TID] = Idx;
+
+		pEntry->BADeclineBitmap &= ~(1<<TID);
+
+		// Set BA session mask in WCID table.
+		RT28XX_ADD_BA_SESSION_TO_ASIC(pAd, pEntry->Aid, TID);
+
+		DBGPRINT(RT_DEBUG_TRACE,("MACEntry[%d]RXBAbitmap = 0x%x. BARecWcidArray=%d\n",
+				pEntry->Aid, pEntry->RXBAbitmap, pEntry->BARecWcidArray[TID]));
+	}
+	else
+	{
+		Status = FALSE;
+		DBGPRINT(RT_DEBUG_TRACE,("Can't Accept ADDBA for %02x:%02x:%02x:%02x:%02x:%02x TID = %d\n",
+				PRINT_MAC(pEntry->Addr), TID));
+	}
+	return(Status);
+}
+
+
+BA_REC_ENTRY *BATableAllocRecEntry(
+								  IN  PRTMP_ADAPTER   pAd,
+								  OUT USHORT          *Idx)
+{
+	int             i;
+	BA_REC_ENTRY    *pBAEntry = NULL;
+
+
+	NdisAcquireSpinLock(&pAd->BATabLock);
+
+	if (pAd->BATable.numAsRecipient >= MAX_BARECI_SESSION)
+	{
+		printk("BA Recipeint Session (%ld) > %d\n", pAd->BATable.numAsRecipient,
+			MAX_BARECI_SESSION);
+		goto done;
+	}
+
+	// reserve idx 0 to identify BAWcidArray[TID] as empty
+	for (i=1; i < MAX_LEN_OF_BA_REC_TABLE; i++)
+	{
+		pBAEntry =&pAd->BATable.BARecEntry[i];
+		if ((pBAEntry->REC_BA_Status == Recipient_NONE))
+		{
+			// get one
+			pAd->BATable.numAsRecipient++;
+			pBAEntry->REC_BA_Status = Recipient_USED;
+			*Idx = i;
+			break;
+		}
+	}
+
+done:
+	NdisReleaseSpinLock(&pAd->BATabLock);
+	return pBAEntry;
+}
+
+BA_ORI_ENTRY *BATableAllocOriEntry(
+								  IN  PRTMP_ADAPTER   pAd,
+								  OUT USHORT          *Idx)
+{
+	int             i;
+	BA_ORI_ENTRY    *pBAEntry = NULL;
+
+	NdisAcquireSpinLock(&pAd->BATabLock);
+
+	if (pAd->BATable.numAsOriginator >= (MAX_LEN_OF_BA_ORI_TABLE))
+	{
+		goto done;
+	}
+
+	// reserve idx 0 to identify BAWcidArray[TID] as empty
+	for (i=1; i<MAX_LEN_OF_BA_ORI_TABLE; i++)
+	{
+		pBAEntry =&pAd->BATable.BAOriEntry[i];
+		if ((pBAEntry->ORI_BA_Status == Originator_NONE))
+		{
+			// get one
+			pAd->BATable.numAsOriginator++;
+			pBAEntry->ORI_BA_Status = Originator_USED;
+			pBAEntry->pAdapter = pAd;
+			*Idx = i;
+			break;
+		}
+	}
+
+done:
+	NdisReleaseSpinLock(&pAd->BATabLock);
+	return pBAEntry;
+}
+
+
+VOID BATableFreeOriEntry(
+						IN  PRTMP_ADAPTER   pAd,
+						IN  ULONG           Idx)
+{
+	BA_ORI_ENTRY    *pBAEntry = NULL;
+	MAC_TABLE_ENTRY *pEntry;
+
+
+	if ((Idx == 0) || (Idx >= MAX_LEN_OF_BA_ORI_TABLE))
+		return;
+
+	pBAEntry =&pAd->BATable.BAOriEntry[Idx];
+
+	if (pBAEntry->ORI_BA_Status != Originator_NONE)
+	{
+		pEntry = &pAd->MacTab.Content[pBAEntry->Wcid];
+		pEntry->BAOriWcidArray[pBAEntry->TID] = 0;
+
+
+		NdisAcquireSpinLock(&pAd->BATabLock);
+		if (pBAEntry->ORI_BA_Status == Originator_Done)
+		{
+		 	pEntry->TXBAbitmap &= (~(1<<(pBAEntry->TID) ));
+			DBGPRINT(RT_DEBUG_TRACE, ("BATableFreeOriEntry numAsOriginator= %ld\n", pAd->BATable.numAsRecipient));
+			// Erase Bitmap flag.
+		}
+
+		ASSERT(pAd->BATable.numAsOriginator != 0);
+
+		pAd->BATable.numAsOriginator -= 1;
+
+		pBAEntry->ORI_BA_Status = Originator_NONE;
+		pBAEntry->Token = 0;
+		NdisReleaseSpinLock(&pAd->BATabLock);
+	}
+}
+
+
+VOID BATableFreeRecEntry(
+						IN  PRTMP_ADAPTER   pAd,
+						IN  ULONG           Idx)
+{
+	BA_REC_ENTRY    *pBAEntry = NULL;
+	MAC_TABLE_ENTRY *pEntry;
+
+
+	if ((Idx == 0) || (Idx >= MAX_LEN_OF_BA_REC_TABLE))
+		return;
+
+	pBAEntry =&pAd->BATable.BARecEntry[Idx];
+
+	if (pBAEntry->REC_BA_Status != Recipient_NONE)
+	{
+		pEntry = &pAd->MacTab.Content[pBAEntry->Wcid];
+		pEntry->BARecWcidArray[pBAEntry->TID] = 0;
+
+		NdisAcquireSpinLock(&pAd->BATabLock);
+
+		ASSERT(pAd->BATable.numAsRecipient != 0);
+
+		pAd->BATable.numAsRecipient -= 1;
+
+		pBAEntry->REC_BA_Status = Recipient_NONE;
+		NdisReleaseSpinLock(&pAd->BATabLock);
+	}
+}
+
+
+VOID BAOriSessionTearDown(
+						 IN OUT  PRTMP_ADAPTER   pAd,
+						 IN      UCHAR           Wcid,
+						 IN      UCHAR           TID,
+						 IN      BOOLEAN         bPassive,
+						 IN      BOOLEAN         bForceSend)
+{
+	ULONG           Idx = 0;
+	BA_ORI_ENTRY    *pBAEntry;
+	BOOLEAN         Cancelled;
+
+	if (Wcid >= MAX_LEN_OF_MAC_TABLE)
+	{
+		return;
+	}
+
+	//
+	// Locate corresponding BA Originator Entry in BA Table with the (pAddr,TID).
+	//
+	Idx = pAd->MacTab.Content[Wcid].BAOriWcidArray[TID];
+	if ((Idx == 0) || (Idx >= MAX_LEN_OF_BA_ORI_TABLE))
+	{
+		if (bForceSend == TRUE)
+		{
+			// force send specified TID DelBA
+			MLME_DELBA_REQ_STRUCT   DelbaReq;
+			MLME_QUEUE_ELEM *Elem = (MLME_QUEUE_ELEM *) kmalloc(sizeof(MLME_QUEUE_ELEM), MEM_ALLOC_FLAG);
+
+			NdisZeroMemory(&DelbaReq, sizeof(DelbaReq));
+			NdisZeroMemory(Elem, sizeof(MLME_QUEUE_ELEM));
+
+			COPY_MAC_ADDR(DelbaReq.Addr, pAd->MacTab.Content[Wcid].Addr);
+			DelbaReq.Wcid = Wcid;
+			DelbaReq.TID = TID;
+			DelbaReq.Initiator = ORIGINATOR;
+#if 1
+			Elem->MsgLen  = sizeof(DelbaReq);
+			NdisMoveMemory(Elem->Msg, &DelbaReq, sizeof(DelbaReq));
+			MlmeDELBAAction(pAd, Elem);
+			kfree(Elem);
+#else
+			MlmeEnqueue(pAd, ACTION_STATE_MACHINE, MT2_MLME_ORI_DELBA_CATE, sizeof(MLME_DELBA_REQ_STRUCT), (PVOID)&DelbaReq);
+			RT28XX_MLME_HANDLER(pAd);
+#endif
+		}
+
+		return;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("%s===>Wcid=%d.TID=%d \n", __FUNCTION__, Wcid, TID));
+
+	pBAEntry = &pAd->BATable.BAOriEntry[Idx];
+	DBGPRINT(RT_DEBUG_TRACE,("\t===>Idx = %ld, Wcid=%d.TID=%d, ORI_BA_Status = %d \n", Idx, Wcid, TID, pBAEntry->ORI_BA_Status));
+	//
+	// Prepare DelBA action frame and send to the peer.
+	//
+	if ((bPassive == FALSE) && (TID == pBAEntry->TID) && (pBAEntry->ORI_BA_Status == Originator_Done))
+	{
+		MLME_DELBA_REQ_STRUCT   DelbaReq;
+		MLME_QUEUE_ELEM *Elem = (MLME_QUEUE_ELEM *) kmalloc(sizeof(MLME_QUEUE_ELEM), MEM_ALLOC_FLAG);
+
+		NdisZeroMemory(&DelbaReq, sizeof(DelbaReq));
+		NdisZeroMemory(Elem, sizeof(MLME_QUEUE_ELEM));
+
+		COPY_MAC_ADDR(DelbaReq.Addr, pAd->MacTab.Content[Wcid].Addr);
+		DelbaReq.Wcid = Wcid;
+		DelbaReq.TID = pBAEntry->TID;
+		DelbaReq.Initiator = ORIGINATOR;
+#if 1
+		Elem->MsgLen  = sizeof(DelbaReq);
+		NdisMoveMemory(Elem->Msg, &DelbaReq, sizeof(DelbaReq));
+		MlmeDELBAAction(pAd, Elem);
+		kfree(Elem);
+#else
+		MlmeEnqueue(pAd, ACTION_STATE_MACHINE, MT2_MLME_ORI_DELBA_CATE, sizeof(MLME_DELBA_REQ_STRUCT), (PVOID)&DelbaReq);
+		RT28XX_MLME_HANDLER(pAd);
+#endif
+	}
+	RTMPCancelTimer(&pBAEntry->ORIBATimer, &Cancelled);
+	BATableFreeOriEntry(pAd, Idx);
+
+	if (bPassive)
+	{
+		//BAOriSessionSetUp(pAd, &pAd->MacTab.Content[Wcid], TID, 0, 10000, TRUE);
+	}
+}
+
+VOID BARecSessionTearDown(
+						 IN OUT  PRTMP_ADAPTER   pAd,
+						 IN      UCHAR           Wcid,
+						 IN      UCHAR           TID,
+						 IN      BOOLEAN         bPassive)
+{
+	ULONG           Idx = 0;
+	BA_REC_ENTRY    *pBAEntry;
+
+	if (Wcid >= MAX_LEN_OF_MAC_TABLE)
+	{
+		return;
+	}
+
+	//
+	//  Locate corresponding BA Originator Entry in BA Table with the (pAddr,TID).
+	//
+	Idx = pAd->MacTab.Content[Wcid].BARecWcidArray[TID];
+	if (Idx == 0)
+		return;
+
+	DBGPRINT(RT_DEBUG_TRACE,("%s===>Wcid=%d.TID=%d \n", __FUNCTION__, Wcid, TID));
+
+
+	pBAEntry = &pAd->BATable.BARecEntry[Idx];
+	DBGPRINT(RT_DEBUG_TRACE,("\t===>Idx = %ld, Wcid=%d.TID=%d, REC_BA_Status = %d \n", Idx, Wcid, TID, pBAEntry->REC_BA_Status));
+	//
+	// Prepare DelBA action frame and send to the peer.
+	//
+	if ((TID == pBAEntry->TID) && (pBAEntry->REC_BA_Status == Recipient_Accept))
+	{
+		MLME_DELBA_REQ_STRUCT   DelbaReq;
+		BOOLEAN 				Cancelled;
+		MLME_QUEUE_ELEM *Elem = (MLME_QUEUE_ELEM *) kmalloc(sizeof(MLME_QUEUE_ELEM), MEM_ALLOC_FLAG);
+		//ULONG   offset;
+		//UINT32  VALUE;
+
+		RTMPCancelTimer(&pBAEntry->RECBATimer, &Cancelled);
+
+		//
+		// 1. Send DELBA Action Frame
+		//
+		if (bPassive == FALSE)
+		{
+			NdisZeroMemory(&DelbaReq, sizeof(DelbaReq));
+			NdisZeroMemory(Elem, sizeof(MLME_QUEUE_ELEM));
+
+			COPY_MAC_ADDR(DelbaReq.Addr, pAd->MacTab.Content[Wcid].Addr);
+			DelbaReq.Wcid = Wcid;
+			DelbaReq.TID = TID;
+			DelbaReq.Initiator = RECIPIENT;
+#if 1
+			Elem->MsgLen  = sizeof(DelbaReq);
+			NdisMoveMemory(Elem->Msg, &DelbaReq, sizeof(DelbaReq));
+			MlmeDELBAAction(pAd, Elem);
+			kfree(Elem);
+#else
+			MlmeEnqueue(pAd, ACTION_STATE_MACHINE, MT2_MLME_ORI_DELBA_CATE, sizeof(MLME_DELBA_REQ_STRUCT), (PVOID)&DelbaReq);
+			RT28XX_MLME_HANDLER(pAd);
+#endif
+		}
+
+
+		//
+		// 2. Free resource of BA session
+		//
+		// flush all pending reordering mpdus
+		ba_refresh_reordering_mpdus(pAd, pBAEntry);
+
+		NdisAcquireSpinLock(&pAd->BATabLock);
+
+		// Erase Bitmap flag.
+		pBAEntry->LastIndSeq = RESET_RCV_SEQ;
+		pBAEntry->BAWinSize = 0;
+		// Erase Bitmap flag at software mactable
+		pAd->MacTab.Content[Wcid].RXBAbitmap &= (~(1<<(pBAEntry->TID)));
+		pAd->MacTab.Content[Wcid].BARecWcidArray[TID] = 0;
+
+		RT28XX_DEL_BA_SESSION_FROM_ASIC(pAd, Wcid, TID);
+
+		NdisReleaseSpinLock(&pAd->BATabLock);
+
+	}
+
+	BATableFreeRecEntry(pAd, Idx);
+}
+
+VOID BASessionTearDownALL(
+						 IN OUT  PRTMP_ADAPTER pAd,
+						 IN      UCHAR Wcid)
+{
+	int i;
+
+	for (i=0; i<NUM_OF_TID; i++)
+	{
+		BAOriSessionTearDown(pAd, Wcid, i, FALSE, FALSE);
+		BARecSessionTearDown(pAd, Wcid, i, FALSE);
+	}
+}
+
+
+/*
+	==========================================================================
+	Description:
+		Retry sending ADDBA Reqest.
+
+	IRQL = DISPATCH_LEVEL
+
+	Parametrs:
+	p8023Header: if this is already 802.3 format, p8023Header is NULL
+
+	Return	: TRUE if put into rx reordering buffer, shouldn't indicaterxhere.
+				FALSE , then continue indicaterx at this moment.
+	==========================================================================
+ */
+VOID BAOriSessionSetupTimeout(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3)
+{
+	BA_ORI_ENTRY    *pBAEntry = (BA_ORI_ENTRY *)FunctionContext;
+	MAC_TABLE_ENTRY *pEntry;
+	PRTMP_ADAPTER   pAd;
+
+	if (pBAEntry == NULL)
+		return;
+
+	pAd = pBAEntry->pAdapter;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// Do nothing if monitor mode is on
+		if (MONITOR_ON(pAd))
+			return;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef RALINK_ATE
+	// Nothing to do in ATE mode.
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+	pEntry = &pAd->MacTab.Content[pBAEntry->Wcid];
+
+	if ((pBAEntry->ORI_BA_Status == Originator_WaitRes) && (pBAEntry->Token < ORI_SESSION_MAX_RETRY))
+	{
+		MLME_ADDBA_REQ_STRUCT    AddbaReq;
+
+		NdisZeroMemory(&AddbaReq, sizeof(AddbaReq));
+		COPY_MAC_ADDR(AddbaReq.pAddr, pEntry->Addr);
+		AddbaReq.Wcid = (UCHAR)(pEntry->Aid);
+		AddbaReq.TID = pBAEntry->TID;
+		AddbaReq.BaBufSize = pAd->CommonCfg.BACapability.field.RxBAWinLimit;
+		AddbaReq.TimeOutValue = 0;
+		AddbaReq.Token = pBAEntry->Token;
+		MlmeEnqueue(pAd, ACTION_STATE_MACHINE, MT2_MLME_ADD_BA_CATE, sizeof(MLME_ADDBA_REQ_STRUCT), (PVOID)&AddbaReq);
+		RT28XX_MLME_HANDLER(pAd);
+		DBGPRINT(RT_DEBUG_TRACE,("BA Ori Session Timeout(%d) : Send ADD BA again\n", pBAEntry->Token));
+
+		pBAEntry->Token++;
+		RTMPSetTimer(&pBAEntry->ORIBATimer, ORI_BA_SESSION_TIMEOUT);
+	}
+	else
+	{
+		BATableFreeOriEntry(pAd, pEntry->BAOriWcidArray[pBAEntry->TID]);
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+		Retry sending ADDBA Reqest.
+
+	IRQL = DISPATCH_LEVEL
+
+	Parametrs:
+	p8023Header: if this is already 802.3 format, p8023Header is NULL
+
+	Return	: TRUE if put into rx reordering buffer, shouldn't indicaterxhere.
+				FALSE , then continue indicaterx at this moment.
+	==========================================================================
+ */
+VOID BARecSessionIdleTimeout(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3)
+{
+
+	BA_REC_ENTRY    *pBAEntry = (BA_REC_ENTRY *)FunctionContext;
+	PRTMP_ADAPTER   pAd;
+	ULONG           Now32;
+
+	if (pBAEntry == NULL)
+		return;
+
+	if ((pBAEntry->REC_BA_Status == Recipient_Accept))
+	{
+		NdisGetSystemUpTime(&Now32);
+
+		if (RTMP_TIME_AFTER((unsigned long)Now32, (unsigned long)(pBAEntry->LastIndSeqAtTimer + REC_BA_SESSION_IDLE_TIMEOUT)))
+		{
+			pAd = pBAEntry->pAdapter;
+			// flush all pending reordering mpdus
+			ba_refresh_reordering_mpdus(pAd, pBAEntry);
+			printk("%ld: REC BA session Timeout\n", Now32);
+		}
+	}
+}
+
+
+VOID PeerAddBAReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+
+{
+	//	7.4.4.1
+	//ULONG	Idx;
+	UCHAR   Status = 1;
+	UCHAR   pAddr[6];
+	FRAME_ADDBA_RSP ADDframe;
+	PUCHAR         pOutBuffer = NULL;
+	NDIS_STATUS     NStatus;
+	PFRAME_ADDBA_REQ  pAddreqFrame = NULL;
+	//UCHAR		BufSize;
+	ULONG       FrameLen;
+	PULONG      ptemp;
+	PMAC_TABLE_ENTRY	pMacEntry;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s ==> (Wcid = %d)\n", __FUNCTION__, Elem->Wcid));
+
+	//hex_dump("AddBAReq", Elem->Msg, Elem->MsgLen);
+
+	//ADDBA Request from unknown peer, ignore this.
+	if (Elem->Wcid >= MAX_LEN_OF_MAC_TABLE)
+		return;
+
+	pMacEntry = &pAd->MacTab.Content[Elem->Wcid];
+	DBGPRINT(RT_DEBUG_TRACE,("BA - PeerAddBAReqAction----> \n"));
+	ptemp = (PULONG)Elem->Msg;
+	//DBGPRINT_RAW(RT_DEBUG_EMU, ("%08x:: %08x:: %08x:: %08x:: %08x:: %08x:: %08x:: %08x:: %08x\n", *(ptemp), *(ptemp+1), *(ptemp+2), *(ptemp+3), *(ptemp+4), *(ptemp+5), *(ptemp+6), *(ptemp+7), *(ptemp+8)));
+
+	if (PeerAddBAReqActionSanity(pAd, Elem->Msg, Elem->MsgLen, pAddr))
+	{
+
+		if ((pAd->CommonCfg.bBADecline == FALSE) && IS_HT_STA(pMacEntry))
+		{
+			pAddreqFrame = (PFRAME_ADDBA_REQ)(&Elem->Msg[0]);
+			printk("Rcv Wcid(%d) AddBAReq\n", Elem->Wcid);
+			if (BARecSessionAdd(pAd, &pAd->MacTab.Content[Elem->Wcid], pAddreqFrame))
+				Status = 0;
+			else
+				Status = 38; // more parameters have invalid values
+		}
+		else
+		{
+			Status = 37; // the request has been declined.
+		}
+	}
+
+	if (pAd->MacTab.Content[Elem->Wcid].ValidAsCLI)
+		ASSERT(pAd->MacTab.Content[Elem->Wcid].Sst == SST_ASSOC);
+
+	pAddreqFrame = (PFRAME_ADDBA_REQ)(&Elem->Msg[0]);
+	// 2. Always send back ADDBA Response
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);	 //Get an unused nonpaged memory
+	if (NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("ACTION - PeerBAAction() allocate memory failed \n"));
+		return;
+	}
+
+	NdisZeroMemory(&ADDframe, sizeof(FRAME_ADDBA_RSP));
+	// 2-1. Prepare ADDBA Response frame.
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (ADHOC_ON(pAd))
+			ActHeaderInit(pAd, &ADDframe.Hdr, pAddr, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+		else
+#ifdef QOS_DLS_SUPPORT
+		if (pAd->MacTab.Content[Elem->Wcid].ValidAsDls)
+			ActHeaderInit(pAd, &ADDframe.Hdr, pAddr, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+		else
+#endif // QOS_DLS_SUPPORT //
+			ActHeaderInit(pAd, &ADDframe.Hdr, pAd->CommonCfg.Bssid, pAd->CurrentAddress, pAddr);
+	}
+#endif // CONFIG_STA_SUPPORT //
+	ADDframe.Category = CATEGORY_BA;
+	ADDframe.Action = ADDBA_RESP;
+	ADDframe.Token = pAddreqFrame->Token;
+	// What is the Status code??  need to check.
+	ADDframe.StatusCode = Status;
+	ADDframe.BaParm.BAPolicy = IMMED_BA;
+	ADDframe.BaParm.AMSDUSupported = 0;
+	ADDframe.BaParm.TID = pAddreqFrame->BaParm.TID;
+	ADDframe.BaParm.BufSize = min(((UCHAR)pAddreqFrame->BaParm.BufSize), (UCHAR)pAd->CommonCfg.BACapability.field.RxBAWinLimit);
+	if (ADDframe.BaParm.BufSize == 0)
+	{
+		ADDframe.BaParm.BufSize = 64;
+	}
+	ADDframe.TimeOutValue = 0; //pAddreqFrame->TimeOutValue;
+
+	*(USHORT *)(&ADDframe.BaParm) = cpu2le16(*(USHORT *)(&ADDframe.BaParm));
+	ADDframe.StatusCode = cpu2le16(ADDframe.StatusCode);
+	ADDframe.TimeOutValue = cpu2le16(ADDframe.TimeOutValue);
+
+	MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+					  sizeof(FRAME_ADDBA_RSP),  &ADDframe,
+			  END_OF_ARGS);
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s(%d): TID(%d), BufSize(%d) <== \n", __FUNCTION__, Elem->Wcid, ADDframe.BaParm.TID,
+							  ADDframe.BaParm.BufSize));
+}
+
+
+VOID PeerAddBARspAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+
+{
+	//UCHAR		Idx, i;
+	//PUCHAR		   pOutBuffer = NULL;
+	PFRAME_ADDBA_RSP    pFrame = NULL;
+	//PBA_ORI_ENTRY		pBAEntry;
+
+	//ADDBA Response from unknown peer, ignore this.
+	if (Elem->Wcid >= MAX_LEN_OF_MAC_TABLE)
+		return;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s ==> Wcid(%d)\n", __FUNCTION__, Elem->Wcid));
+
+	//hex_dump("PeerAddBARspAction()", Elem->Msg, Elem->MsgLen);
+
+	if (PeerAddBARspActionSanity(pAd, Elem->Msg, Elem->MsgLen))
+	{
+		pFrame = (PFRAME_ADDBA_RSP)(&Elem->Msg[0]);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("\t\t StatusCode = %d\n", pFrame->StatusCode));
+		switch (pFrame->StatusCode)
+		{
+			case 0:
+				// I want a BAsession with this peer as an originator.
+				BAOriSessionAdd(pAd, &pAd->MacTab.Content[Elem->Wcid], pFrame);
+				break;
+			default:
+				// check status == USED ???
+				BAOriSessionTearDown(pAd, Elem->Wcid, pFrame->BaParm.TID, TRUE, FALSE);
+				break;
+		}
+		// Rcv Decline StatusCode
+		if ((pFrame->StatusCode == 37)
+#ifdef CONFIG_STA_SUPPORT
+            || ((pAd->OpMode == OPMODE_STA) && STA_TGN_WIFI_ON(pAd) && (pFrame->StatusCode != 0))
+#endif // CONFIG_STA_SUPPORT //
+            )
+		{
+			pAd->MacTab.Content[Elem->Wcid].BADeclineBitmap |= 1<<pFrame->BaParm.TID;
+		}
+	}
+}
+
+VOID PeerDelBAAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+
+{
+	//UCHAR				Idx;
+	//PUCHAR				pOutBuffer = NULL;
+	PFRAME_DELBA_REQ    pDelFrame = NULL;
+
+	DBGPRINT(RT_DEBUG_TRACE,("%s ==>\n", __FUNCTION__));
+	//DELBA Request from unknown peer, ignore this.
+	if (PeerDelBAActionSanity(pAd, Elem->Wcid, Elem->Msg, Elem->MsgLen))
+	{
+		pDelFrame = (PFRAME_DELBA_REQ)(&Elem->Msg[0]);
+		if (pDelFrame->DelbaParm.Initiator == ORIGINATOR)
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("BA - PeerDelBAAction----> ORIGINATOR\n"));
+			BARecSessionTearDown(pAd, Elem->Wcid, pDelFrame->DelbaParm.TID, TRUE);
+		}
+		else
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("BA - PeerDelBAAction----> RECIPIENT, Reason = %d\n",  pDelFrame->ReasonCode));
+			//hex_dump("DelBA Frame", pDelFrame, Elem->MsgLen);
+			BAOriSessionTearDown(pAd, Elem->Wcid, pDelFrame->DelbaParm.TID, TRUE, FALSE);
+		}
+	}
+}
+
+
+BOOLEAN CntlEnqueueForRecv(
+						  IN PRTMP_ADAPTER		pAd,
+						  IN ULONG				Wcid,
+						  IN ULONG				MsgLen,
+						  IN PFRAME_BA_REQ		pMsg)
+{
+	PFRAME_BA_REQ   pFrame = pMsg;
+	//PRTMP_REORDERBUF	pBuffer;
+	//PRTMP_REORDERBUF	pDmaBuf;
+	PBA_REC_ENTRY pBAEntry;
+	//BOOLEAN 	Result;
+	ULONG   Idx;
+	//UCHAR	NumRxPkt;
+	UCHAR	TID;//, i;
+
+	TID = (UCHAR)pFrame->BARControl.TID;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s(): BAR-Wcid(%ld), Tid (%d)\n", __FUNCTION__, Wcid, TID));
+	//hex_dump("BAR", (PCHAR) pFrame, MsgLen);
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return FALSE;
+
+	// First check the size, it MUST not exceed the mlme queue size
+	if (MsgLen > MGMT_DMA_BUFFER_SIZE)
+	{
+		DBGPRINT_ERR(("CntlEnqueueForRecv: frame too large, size = %ld \n", MsgLen));
+		return FALSE;
+	}
+	else if (MsgLen != sizeof(FRAME_BA_REQ))
+	{
+		DBGPRINT_ERR(("CntlEnqueueForRecv: BlockAck Request frame length size = %ld incorrect\n", MsgLen));
+		return FALSE;
+	}
+	else if (MsgLen != sizeof(FRAME_BA_REQ))
+	{
+		DBGPRINT_ERR(("CntlEnqueueForRecv: BlockAck Request frame length size = %ld incorrect\n", MsgLen));
+		return FALSE;
+	}
+
+	if ((Wcid < MAX_LEN_OF_MAC_TABLE) && (TID < 8))
+		{
+		// if this receiving packet is from SA that is in our OriEntry. Since WCID <9 has direct mapping. no need search.
+		Idx = pAd->MacTab.Content[Wcid].BARecWcidArray[TID];
+		pBAEntry = &pAd->BATable.BARecEntry[Idx];
+		}
+		else
+		{
+		return FALSE;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("BAR(%ld) : Tid (%d) - %04x:%04x\n", Wcid, TID, pFrame->BAStartingSeq.field.StartSeq, pBAEntry->LastIndSeq ));
+
+	if (SEQ_SMALLER(pBAEntry->LastIndSeq, pFrame->BAStartingSeq.field.StartSeq, MAXSEQ))
+	{
+		//printk("BAR Seq = %x, LastIndSeq = %x\n", pFrame->BAStartingSeq.field.StartSeq, pBAEntry->LastIndSeq);
+		ba_indicate_reordering_mpdus_le_seq(pAd, pBAEntry, pFrame->BAStartingSeq.field.StartSeq);
+		pBAEntry->LastIndSeq = (pFrame->BAStartingSeq.field.StartSeq == 0) ? MAXSEQ :(pFrame->BAStartingSeq.field.StartSeq -1);
+	}
+	//ba_refresh_reordering_mpdus(pAd, pBAEntry);
+	return TRUE;
+}
+
+/*
+Description : Send PSMP Action frame If PSMP mode switches.
+*/
+VOID SendPSMPAction(
+				   IN PRTMP_ADAPTER		pAd,
+				   IN UCHAR				Wcid,
+				   IN UCHAR				Psmp)
+{
+	PUCHAR          pOutBuffer = NULL;
+	NDIS_STATUS     NStatus;
+	//ULONG           Idx;
+	FRAME_PSMP_ACTION   Frame;
+	ULONG           FrameLen;
+
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);	 //Get an unused nonpaged memory
+	if (NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("BA - MlmeADDBAAction() allocate memory failed \n"));
+		return;
+	}
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		ActHeaderInit(pAd, &Frame.Hdr, pAd->CommonCfg.Bssid, pAd->CurrentAddress, pAd->MacTab.Content[Wcid].Addr);
+#endif // CONFIG_STA_SUPPORT //
+
+	Frame.Category = CATEGORY_HT;
+	Frame.Action = SMPS_ACTION;
+	switch (Psmp)
+	{
+		case MMPS_ENABLE:
+			Frame.Psmp = 0;
+			break;
+		case MMPS_DYNAMIC:
+			Frame.Psmp = 3;
+			break;
+		case MMPS_STATIC:
+			Frame.Psmp = 1;
+			break;
+	}
+	MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+					  sizeof(FRAME_PSMP_ACTION),      &Frame,
+					  END_OF_ARGS);
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+	DBGPRINT(RT_DEBUG_ERROR,("HT - SendPSMPAction( %d )  \n", Frame.Psmp));
+}
+
+
+#define RADIO_MEASUREMENT_REQUEST_ACTION	0
+
+typedef struct PACKED
+{
+	UCHAR	RegulatoryClass;
+	UCHAR	ChannelNumber;
+	USHORT	RandomInterval;
+	USHORT	MeasurementDuration;
+	UCHAR	MeasurementMode;
+	UCHAR   BSSID[MAC_ADDR_LEN];
+	UCHAR	ReportingCondition;
+	UCHAR	Threshold;
+	UCHAR   SSIDIE[2];			// 2 byte
+} BEACON_REQUEST;
+
+typedef struct PACKED
+{
+	UCHAR	ID;
+	UCHAR	Length;
+	UCHAR	Token;
+	UCHAR	RequestMode;
+	UCHAR	Type;
+} MEASUREMENT_REQ;
+
+
+
+
+void convert_reordering_packet_to_preAMSDU_or_802_3_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN  UCHAR			FromWhichBSSID)
+{
+	PNDIS_PACKET	pRxPkt;
+	UCHAR			Header802_3[LENGTH_802_3];
+
+	// 1. get 802.3 Header
+	// 2. remove LLC
+	// 		a. pointer pRxBlk->pData to payload
+	//      b. modify pRxBlk->DataSize
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		RTMP_802_11_REMOVE_LLC_AND_CONVERT_TO_802_3(pRxBlk, Header802_3);
+#endif // CONFIG_STA_SUPPORT //
+
+	ASSERT(pRxBlk->pRxPacket);
+	pRxPkt = RTPKT_TO_OSPKT(pRxBlk->pRxPacket);
+
+	RTPKT_TO_OSPKT(pRxPkt)->dev = get_netdev_from_bssid(pAd, FromWhichBSSID);
+	RTPKT_TO_OSPKT(pRxPkt)->data = pRxBlk->pData;
+	RTPKT_TO_OSPKT(pRxPkt)->len = pRxBlk->DataSize;
+	RTPKT_TO_OSPKT(pRxPkt)->tail = RTPKT_TO_OSPKT(pRxPkt)->data + RTPKT_TO_OSPKT(pRxPkt)->len;
+
+	//
+	// copy 802.3 header, if necessary
+	//
+	if (!RX_BLK_TEST_FLAG(pRxBlk, fRX_AMSDU))
+	{
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+#ifdef LINUX
+			NdisMoveMemory(skb_push(pRxPkt, LENGTH_802_3), Header802_3, LENGTH_802_3);
+#endif
+#ifdef UCOS
+			NdisMoveMemory(net_pkt_push(pRxPkt, LENGTH_802_3), Header802_3, LENGTH_802_3);
+#endif
+		}
+#endif // CONFIG_STA_SUPPORT //
+	}
+}
+
+
+#define INDICATE_LEGACY_OR_AMSDU(_pAd, _pRxBlk, _fromWhichBSSID)		\
+	do																	\
+	{																	\
+    	if (RX_BLK_TEST_FLAG(_pRxBlk, fRX_AMSDU))						\
+    	{																\
+    		Indicate_AMSDU_Packet(_pAd, _pRxBlk, _fromWhichBSSID);		\
+    	}																\
+		else if (RX_BLK_TEST_FLAG(_pRxBlk, fRX_EAP))					\
+		{																\
+			Indicate_EAPOL_Packet(_pAd, _pRxBlk, _fromWhichBSSID);		\
+		}																\
+    	else															\
+    	{																\
+    		Indicate_Legacy_Packet(_pAd, _pRxBlk, _fromWhichBSSID);		\
+    	}																\
+	} while (0);
+
+
+
+static VOID ba_enqueue_reordering_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PBA_REC_ENTRY	pBAEntry,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+	struct reordering_mpdu *mpdu_blk;
+	UINT16	Sequence = (UINT16) pRxBlk->pHeader->Sequence;
+
+	mpdu_blk = ba_mpdu_blk_alloc(pAd);
+	if (mpdu_blk != NULL)
+	{
+		// Write RxD buffer address & allocated buffer length
+		NdisAcquireSpinLock(&pBAEntry->RxReRingLock);
+
+		mpdu_blk->Sequence = Sequence;
+
+		mpdu_blk->bAMSDU = RX_BLK_TEST_FLAG(pRxBlk, fRX_AMSDU);
+
+		convert_reordering_packet_to_preAMSDU_or_802_3_packet(pAd, pRxBlk, FromWhichBSSID);
+
+		STATS_INC_RX_PACKETS(pAd, FromWhichBSSID);
+
+        //
+		// it is necessary for reordering packet to record
+		// which BSS it come from
+		//
+		RTMP_SET_PACKET_IF(pRxBlk->pRxPacket, FromWhichBSSID);
+
+		mpdu_blk->pPacket = pRxBlk->pRxPacket;
+
+		if (ba_reordering_mpdu_insertsorted(&pBAEntry->list, mpdu_blk) == FALSE)
+		{
+			// had been already within reordering list
+			// don't indicate
+			RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_SUCCESS);
+			ba_mpdu_blk_free(pAd, mpdu_blk);
+		}
+
+		ASSERT((0<= pBAEntry->list.qlen)  && (pBAEntry->list.qlen <= pBAEntry->BAWinSize));
+		NdisReleaseSpinLock(&pBAEntry->RxReRingLock);
+	}
+	else
+	{
+#if 0
+		DBGPRINT(RT_DEBUG_ERROR,  ("!!! (%d:%d) Can't allocate reordering mpdu blk\n",
+								   blk_count, pBAEntry->list.qlen));
+#else
+		DBGPRINT(RT_DEBUG_ERROR,  ("!!! (%d) Can't allocate reordering mpdu blk\n",
+								   pBAEntry->list.qlen));
+#endif
+		/*
+		 * flush all pending reordering mpdus
+		 * and receving mpdu to upper layer
+		 * make tcp/ip to take care reordering mechanism
+		 */
+		//ba_refresh_reordering_mpdus(pAd, pBAEntry);
+		ba_indicate_reordering_mpdus_le_seq(pAd, pBAEntry, Sequence);
+
+		pBAEntry->LastIndSeq = Sequence;
+		INDICATE_LEGACY_OR_AMSDU(pAd, pRxBlk, FromWhichBSSID);
+	}
+}
+
+
+/*
+	==========================================================================
+	Description:
+		Indicate this packet to upper layer or put it into reordering buffer
+
+	Parametrs:
+		pRxBlk         : carry necessary packet info 802.11 format
+		FromWhichBSSID : the packet received from which BSS
+
+	Return	:
+			  none
+
+	Note    :
+	          the packet queued into reordering buffer need to cover to 802.3 format
+			  or pre_AMSDU format
+	==========================================================================
+ */
+
+VOID Indicate_AMPDU_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+	USHORT				Idx;
+	PBA_REC_ENTRY		pBAEntry = NULL;
+	UINT16				Sequence = pRxBlk->pHeader->Sequence;
+	ULONG				Now32;
+	UCHAR				Wcid = pRxBlk->pRxWI->WirelessCliID;
+	UCHAR				TID = pRxBlk->pRxWI->TID;
+
+
+	if (!RX_BLK_TEST_FLAG(pRxBlk, fRX_AMSDU) &&  (pRxBlk->DataSize > MAX_RX_PKT_LEN))
+	{
+#if 0 // sample take off, no use
+		static int err_size;
+
+		err_size++;
+		if (err_size > 20) {
+			 printk("AMPDU DataSize = %d\n", pRxBlk->DataSize);
+			 hex_dump("802.11 Header", (UCHAR *)pRxBlk->pHeader, 24);
+			 hex_dump("Payload", pRxBlk->pData, 64);
+			 err_size = 0;
+		}
+#endif
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+
+
+#if 0 // test
+	/* Rec BA Session had been torn down */
+	INDICATE_LEGACY_OR_AMSDU(pAd, pRxBlk, FromWhichBSSID);
+	return;
+#endif
+
+	if (Wcid < MAX_LEN_OF_MAC_TABLE)
+	{
+		Idx = pAd->MacTab.Content[Wcid].BARecWcidArray[TID];
+		if (Idx == 0)
+		{
+			/* Rec BA Session had been torn down */
+			INDICATE_LEGACY_OR_AMSDU(pAd, pRxBlk, FromWhichBSSID);
+			return;
+		}
+		pBAEntry = &pAd->BATable.BARecEntry[Idx];
+	}
+	else
+	{
+		// impossible !!!
+		ASSERT(0);
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+
+	ASSERT(pBAEntry);
+
+	// update last rx time
+	NdisGetSystemUpTime(&Now32);
+
+	pBAEntry->rcvSeq = Sequence;
+
+
+	ba_flush_reordering_timeout_mpdus(pAd, pBAEntry, Now32);
+	pBAEntry->LastIndSeqAtTimer = Now32;
+
+	//
+	// Reset Last Indicate Sequence
+	//
+	if (pBAEntry->LastIndSeq == RESET_RCV_SEQ)
+	{
+		ASSERT((pBAEntry->list.qlen == 0) && (pBAEntry->list.next == NULL));
+
+		// reset rcv sequence of BA session
+		pBAEntry->LastIndSeq = Sequence;
+		pBAEntry->LastIndSeqAtTimer = Now32;
+		INDICATE_LEGACY_OR_AMSDU(pAd, pRxBlk, FromWhichBSSID);
+		return;
+	}
+
+
+	//
+	// I. Check if in order.
+	//
+	if (SEQ_STEPONE(Sequence, pBAEntry->LastIndSeq, MAXSEQ))
+	{
+		USHORT  LastIndSeq;
+
+		pBAEntry->LastIndSeq = Sequence;
+		INDICATE_LEGACY_OR_AMSDU(pAd, pRxBlk, FromWhichBSSID);
+ 		LastIndSeq = ba_indicate_reordering_mpdus_in_order(pAd, pBAEntry, pBAEntry->LastIndSeq);
+		if (LastIndSeq != RESET_RCV_SEQ)
+		{
+			pBAEntry->LastIndSeq = LastIndSeq;
+		}
+		pBAEntry->LastIndSeqAtTimer = Now32;
+	}
+	//
+	// II. Drop Duplicated Packet
+	//
+	else if (Sequence == pBAEntry->LastIndSeq)
+	{
+
+		// drop and release packet
+		pBAEntry->nDropPacket++;
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+	}
+	//
+	// III. Drop Old Received Packet
+	//
+	else if (SEQ_SMALLER(Sequence, pBAEntry->LastIndSeq, MAXSEQ))
+	{
+
+		// drop and release packet
+		pBAEntry->nDropPacket++;
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+	}
+	//
+	// IV. Receive Sequence within Window Size
+	//
+	else if (SEQ_SMALLER(Sequence, (((pBAEntry->LastIndSeq+pBAEntry->BAWinSize+1)) & MAXSEQ), MAXSEQ))
+	{
+		ba_enqueue_reordering_packet(pAd, pBAEntry, pRxBlk, FromWhichBSSID);
+	}
+	//
+	// V. Receive seq surpasses Win(lastseq + nMSDU). So refresh all reorder buffer
+	//
+	else
+	{
+#if 0
+		ba_refresh_reordering_mpdus(pAd, pBAEntry);
+		INDICATE_LEGACY_OR_AMSDU(pAd, pRxBlk, FromWhichBSSID);
+#else
+		LONG WinStartSeq, TmpSeq;
+
+
+		TmpSeq = Sequence - (pBAEntry->BAWinSize) -1;
+		if (TmpSeq < 0)
+		{
+			TmpSeq = (MAXSEQ+1) + TmpSeq;
+		}
+		WinStartSeq = (TmpSeq+1) & MAXSEQ;
+		ba_indicate_reordering_mpdus_le_seq(pAd, pBAEntry, WinStartSeq);
+		pBAEntry->LastIndSeq = WinStartSeq; //TmpSeq;
+
+		pBAEntry->LastIndSeqAtTimer = Now32;
+
+		ba_enqueue_reordering_packet(pAd, pBAEntry, pRxBlk, FromWhichBSSID);
+
+		TmpSeq = ba_indicate_reordering_mpdus_in_order(pAd, pBAEntry, pBAEntry->LastIndSeq);
+		if (TmpSeq != RESET_RCV_SEQ)
+		{
+			pBAEntry->LastIndSeq = TmpSeq;
+		}
+#endif
+	}
+}
+
+#endif // DOT11_N_SUPPORT //
+
diff --git a/drivers/staging/rt2870/common/cmm_data.c b/drivers/staging/rt2870/common/cmm_data.c
new file mode 100644
index 00000000000..4477a8e64a2
--- /dev/null
+++ b/drivers/staging/rt2870/common/cmm_data.c
@@ -0,0 +1,2734 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+*/
+
+#include "../rt_config.h"
+
+#define MAX_TX_IN_TBTT		(16)
+
+
+UCHAR	SNAP_802_1H[] = {0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00};
+UCHAR	SNAP_BRIDGE_TUNNEL[] = {0xaa, 0xaa, 0x03, 0x00, 0x00, 0xf8};
+// Add Cisco Aironet SNAP heade for CCX2 support
+UCHAR	SNAP_AIRONET[] = {0xaa, 0xaa, 0x03, 0x00, 0x40, 0x96, 0x00, 0x00};
+UCHAR	CKIP_LLC_SNAP[] = {0xaa, 0xaa, 0x03, 0x00, 0x40, 0x96, 0x00, 0x02};
+UCHAR	EAPOL_LLC_SNAP[]= {0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00, 0x88, 0x8e};
+UCHAR	EAPOL[] = {0x88, 0x8e};
+UCHAR   TPID[] = {0x81, 0x00}; /* VLAN related */
+
+UCHAR	IPX[] = {0x81, 0x37};
+UCHAR	APPLE_TALK[] = {0x80, 0xf3};
+UCHAR	RateIdToPlcpSignal[12] = {
+	 0, /* RATE_1 */	1, /* RATE_2 */ 	2, /* RATE_5_5 */	3, /* RATE_11 */	// see BBP spec
+	11, /* RATE_6 */   15, /* RATE_9 */    10, /* RATE_12 */   14, /* RATE_18 */	// see IEEE802.11a-1999 p.14
+	 9, /* RATE_24 */  13, /* RATE_36 */	8, /* RATE_48 */   12  /* RATE_54 */ }; // see IEEE802.11a-1999 p.14
+
+UCHAR	 OfdmSignalToRateId[16] = {
+	RATE_54,  RATE_54,	RATE_54,  RATE_54,	// OFDM PLCP Signal = 0,  1,  2,  3 respectively
+	RATE_54,  RATE_54,	RATE_54,  RATE_54,	// OFDM PLCP Signal = 4,  5,  6,  7 respectively
+	RATE_48,  RATE_24,	RATE_12,  RATE_6,	// OFDM PLCP Signal = 8,  9,  10, 11 respectively
+	RATE_54,  RATE_36,	RATE_18,  RATE_9,	// OFDM PLCP Signal = 12, 13, 14, 15 respectively
+};
+
+UCHAR	 OfdmRateToRxwiMCS[12] = {
+	0,  0,	0,  0,
+	0,  1,	2,  3,	// OFDM rate 6,9,12,18 = rxwi mcs 0,1,2,3
+	4,  5,	6,  7,	// OFDM rate 24,36,48,54 = rxwi mcs 4,5,6,7
+};
+UCHAR	 RxwiMCSToOfdmRate[12] = {
+	RATE_6,  RATE_9,	RATE_12,  RATE_18,
+	RATE_24,  RATE_36,	RATE_48,  RATE_54,	// OFDM rate 6,9,12,18 = rxwi mcs 0,1,2,3
+	4,  5,	6,  7,	// OFDM rate 24,36,48,54 = rxwi mcs 4,5,6,7
+};
+
+char*   MCSToMbps[] = {"1Mbps","2Mbps","5.5Mbps","11Mbps","06Mbps","09Mbps","12Mbps","18Mbps","24Mbps","36Mbps","48Mbps","54Mbps","MM-0","MM-1","MM-2","MM-3","MM-4","MM-5","MM-6","MM-7","MM-8","MM-9","MM-10","MM-11","MM-12","MM-13","MM-14","MM-15","MM-32","ee1","ee2","ee3"};
+
+UCHAR default_cwmin[]={CW_MIN_IN_BITS, CW_MIN_IN_BITS, CW_MIN_IN_BITS-1, CW_MIN_IN_BITS-2};
+//UCHAR default_cwmax[]={CW_MAX_IN_BITS, CW_MAX_IN_BITS, CW_MIN_IN_BITS, CW_MIN_IN_BITS-1};
+UCHAR default_sta_aifsn[]={3,7,2,2};
+
+UCHAR MapUserPriorityToAccessCategory[8] = {QID_AC_BE, QID_AC_BK, QID_AC_BK, QID_AC_BE, QID_AC_VI, QID_AC_VI, QID_AC_VO, QID_AC_VO};
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		API for MLME to transmit management frame to AP (BSS Mode)
+	or station (IBSS Mode)
+
+	Arguments:
+		pAd Pointer to our adapter
+		pData		Pointer to the outgoing 802.11 frame
+		Length		Size of outgoing management frame
+
+	Return Value:
+		NDIS_STATUS_FAILURE
+		NDIS_STATUS_PENDING
+		NDIS_STATUS_SUCCESS
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS MiniportMMRequest(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			QueIdx,
+	IN	PUCHAR			pData,
+	IN	UINT			Length)
+{
+	PNDIS_PACKET	pPacket;
+	NDIS_STATUS  	Status = NDIS_STATUS_SUCCESS;
+	ULONG	 		FreeNum;
+	UCHAR			IrqState;
+	UCHAR			rtmpHwHdr[TXINFO_SIZE + TXWI_SIZE]; //RTMP_HW_HDR_LEN];
+
+	ASSERT(Length <= MGMT_DMA_BUFFER_SIZE);
+
+	QueIdx=3;
+
+	// 2860C use Tx Ring
+
+	IrqState = pAd->irq_disabled;
+
+	do
+	{
+		// Reset is in progress, stop immediately
+		if ( RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS) ||
+			 RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST)||
+			 !RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_START_UP))
+		{
+			Status = NDIS_STATUS_FAILURE;
+			break;
+		}
+
+		// Check Free priority queue
+		// Since we use PBF Queue2 for management frame.  Its corresponding DMA ring should be using TxRing.
+
+		// 2860C use Tx Ring
+		if (pAd->MACVersion == 0x28600100)
+		{
+			FreeNum = GET_TXRING_FREENO(pAd, QueIdx);
+		}
+		else
+		{
+			FreeNum = GET_MGMTRING_FREENO(pAd);
+		}
+
+		if ((FreeNum > 0))
+		{
+			// We need to reserve space for rtmp hardware header. i.e., TxWI for RT2860 and TxInfo+TxWI for RT2870
+			NdisZeroMemory(&rtmpHwHdr, (TXINFO_SIZE + TXWI_SIZE));
+			Status = RTMPAllocateNdisPacket(pAd, &pPacket, (PUCHAR)&rtmpHwHdr, (TXINFO_SIZE + TXWI_SIZE), pData, Length);
+			if (Status != NDIS_STATUS_SUCCESS)
+			{
+				DBGPRINT(RT_DEBUG_WARN, ("MiniportMMRequest (error:: can't allocate NDIS PACKET)\n"));
+				break;
+			}
+
+			//pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_CCK;
+			//pAd->CommonCfg.MlmeRate = RATE_2;
+
+
+			Status = MlmeHardTransmit(pAd, QueIdx, pPacket);
+			if (Status != NDIS_STATUS_SUCCESS)
+				RTMPFreeNdisPacket(pAd, pPacket);
+		}
+		else
+		{
+			pAd->RalinkCounters.MgmtRingFullCount++;
+			DBGPRINT(RT_DEBUG_ERROR, ("Qidx(%d), not enough space in MgmtRing, MgmtRingFullCount=%ld!\n",
+										QueIdx, pAd->RalinkCounters.MgmtRingFullCount));
+		}
+
+	} while (FALSE);
+
+
+	return Status;
+}
+
+
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Copy frame from waiting queue into relative ring buffer and set
+	appropriate ASIC register to kick hardware transmit function
+
+	Arguments:
+		pAd Pointer to our adapter
+		pBuffer 	Pointer to	memory of outgoing frame
+		Length		Size of outgoing management frame
+
+	Return Value:
+		NDIS_STATUS_FAILURE
+		NDIS_STATUS_PENDING
+		NDIS_STATUS_SUCCESS
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS MlmeHardTransmit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			QueIdx,
+	IN	PNDIS_PACKET	pPacket)
+{
+	if ((pAd->CommonCfg.RadarDetect.RDMode != RD_NORMAL_MODE)
+#ifdef CARRIER_DETECTION_SUPPORT
+#endif // CARRIER_DETECTION_SUPPORT //
+		)
+	{
+		return NDIS_STATUS_FAILURE;
+	}
+
+		return MlmeHardTransmitMgmtRing(pAd,QueIdx,pPacket);
+
+}
+
+
+
+NDIS_STATUS MlmeHardTransmitMgmtRing(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR	QueIdx,
+	IN	PNDIS_PACKET	pPacket)
+{
+	PACKET_INFO 	PacketInfo;
+	PUCHAR			pSrcBufVA;
+	UINT			SrcBufLen;
+	PHEADER_802_11	pHeader_802_11;
+	BOOLEAN 		bAckRequired, bInsertTimestamp;
+	UCHAR			MlmeRate;
+	PTXWI_STRUC 	pFirstTxWI;
+	MAC_TABLE_ENTRY	*pMacEntry = NULL;
+
+	RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pSrcBufVA, &SrcBufLen);
+
+	// Make sure MGMT ring resource won't be used by other threads
+// sample, for IRQ LOCK -> SEM LOCK
+//	IrqState = pAd->irq_disabled;
+//	if (!IrqState)
+		RTMP_SEM_LOCK(&pAd->MgmtRingLock);
+
+
+	if (pSrcBufVA == NULL)
+	{
+		// The buffer shouldn't be NULL
+//		if (!IrqState)
+			RTMP_SEM_UNLOCK(&pAd->MgmtRingLock);
+		return NDIS_STATUS_FAILURE;
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// outgoing frame always wakeup PHY to prevent frame lost
+		if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+			AsicForceWakeup(pAd, TRUE);
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	pFirstTxWI = (PTXWI_STRUC)(pSrcBufVA +  TXINFO_SIZE);
+	pHeader_802_11 = (PHEADER_802_11) (pSrcBufVA + TXINFO_SIZE + TXWI_SIZE); //TXWI_SIZE);
+
+	if (pHeader_802_11->Addr1[0] & 0x01)
+	{
+		MlmeRate = pAd->CommonCfg.BasicMlmeRate;
+	}
+	else
+	{
+		MlmeRate = pAd->CommonCfg.MlmeRate;
+	}
+
+	// Verify Mlme rate for a / g bands.
+	if ((pAd->LatchRfRegs.Channel > 14) && (MlmeRate < RATE_6)) // 11A band
+		MlmeRate = RATE_6;
+
+	if ((pHeader_802_11->FC.Type == BTYPE_DATA) &&
+		(pHeader_802_11->FC.SubType == SUBTYPE_QOS_NULL))
+	{
+		pMacEntry = MacTableLookup(pAd, pHeader_802_11->Addr1);
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// Fixed W52 with Activity scan issue in ABG_MIXED and ABGN_MIXED mode.
+		if (pAd->CommonCfg.PhyMode == PHY_11ABG_MIXED
+#ifdef DOT11_N_SUPPORT
+			|| pAd->CommonCfg.PhyMode == PHY_11ABGN_MIXED
+#endif // DOT11_N_SUPPORT //
+		)
+		{
+			if (pAd->LatchRfRegs.Channel > 14)
+				pAd->CommonCfg.MlmeTransmit.field.MODE = 1;
+			else
+				pAd->CommonCfg.MlmeTransmit.field.MODE = 0;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	//
+	// Should not be hard code to set PwrMgmt to 0 (PWR_ACTIVE)
+	// Snice it's been set to 0 while on MgtMacHeaderInit
+	// By the way this will cause frame to be send on PWR_SAVE failed.
+	//
+	// pHeader_802_11->FC.PwrMgmt = 0; // (pAd->StaCfg.Psm == PWR_SAVE);
+	//
+	// In WMM-UAPSD, mlme frame should be set psm as power saving but probe request frame
+#ifdef CONFIG_STA_SUPPORT
+    // Data-Null packets alse pass through MMRequest in RT2860, however, we hope control the psm bit to pass APSD
+	if ((pHeader_802_11->FC.Type != BTYPE_DATA) && (pHeader_802_11->FC.Type != BTYPE_CNTL))
+	{
+		if ((pAd->StaCfg.Psm == PWR_SAVE) &&
+			(pHeader_802_11->FC.SubType == SUBTYPE_ACTION))
+			pHeader_802_11->FC.PwrMgmt = PWR_SAVE;
+		else
+			pHeader_802_11->FC.PwrMgmt = PWR_ACTIVE;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	bInsertTimestamp = FALSE;
+	if (pHeader_802_11->FC.Type == BTYPE_CNTL) // must be PS-POLL
+	{
+#ifdef CONFIG_STA_SUPPORT
+		//Set PM bit in ps-poll, to fix WLK 1.2  PowerSaveMode_ext failure issue.
+		if ((pAd->OpMode == OPMODE_STA) && (pHeader_802_11->FC.SubType == SUBTYPE_PS_POLL))
+		{
+			pHeader_802_11->FC.PwrMgmt = PWR_SAVE;
+		}
+#endif // CONFIG_STA_SUPPORT //
+		bAckRequired = FALSE;
+	}
+	else // BTYPE_MGMT or BTYPE_DATA(must be NULL frame)
+	{
+		//pAd->Sequence++;
+		//pHeader_802_11->Sequence = pAd->Sequence;
+
+		if (pHeader_802_11->Addr1[0] & 0x01) // MULTICAST, BROADCAST
+		{
+			bAckRequired = FALSE;
+			pHeader_802_11->Duration = 0;
+		}
+		else
+		{
+			bAckRequired = TRUE;
+			pHeader_802_11->Duration = RTMPCalcDuration(pAd, MlmeRate, 14);
+			if (pHeader_802_11->FC.SubType == SUBTYPE_PROBE_RSP)
+			{
+				bInsertTimestamp = TRUE;
+			}
+		}
+	}
+
+	pHeader_802_11->Sequence = pAd->Sequence++;
+	if (pAd->Sequence >0xfff)
+		pAd->Sequence = 0;
+
+	// Before radar detection done, mgmt frame can not be sent but probe req
+	// Because we need to use probe req to trigger driver to send probe req in passive scan
+	if ((pHeader_802_11->FC.SubType != SUBTYPE_PROBE_REQ)
+		&& (pAd->CommonCfg.bIEEE80211H == 1)
+		&& (pAd->CommonCfg.RadarDetect.RDMode != RD_NORMAL_MODE))
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("MlmeHardTransmit --> radar detect not in normal mode !!!\n"));
+//		if (!IrqState)
+			RTMP_SEM_UNLOCK(&pAd->MgmtRingLock);
+		return (NDIS_STATUS_FAILURE);
+	}
+
+#ifdef RT_BIG_ENDIAN
+	RTMPFrameEndianChange(pAd, (PUCHAR)pHeader_802_11, DIR_WRITE, FALSE);
+#endif
+
+	//
+	// fill scatter-and-gather buffer list into TXD. Internally created NDIS PACKET
+	// should always has only one ohysical buffer, and the whole frame size equals
+	// to the first scatter buffer size
+	//
+
+	// Initialize TX Descriptor
+	// For inter-frame gap, the number is for this frame and next frame
+	// For MLME rate, we will fix as 2Mb to match other vendor's implement
+//	pAd->CommonCfg.MlmeTransmit.field.MODE = 1;
+
+// management frame doesn't need encryption. so use RESERVED_WCID no matter u are sending to specific wcid or not.
+	if (pMacEntry == NULL)
+	{
+		RTMPWriteTxWI(pAd, pFirstTxWI, FALSE, FALSE, bInsertTimestamp, FALSE, bAckRequired, FALSE,
+		0, RESERVED_WCID, (SrcBufLen - TXINFO_SIZE - TXWI_SIZE), PID_MGMT, 0,  (UCHAR)pAd->CommonCfg.MlmeTransmit.field.MCS, IFS_BACKOFF, FALSE, &pAd->CommonCfg.MlmeTransmit);
+	}
+	else
+	{
+		RTMPWriteTxWI(pAd, pFirstTxWI, FALSE, FALSE,
+					bInsertTimestamp, FALSE, bAckRequired, FALSE,
+					0, pMacEntry->Aid, (SrcBufLen - TXINFO_SIZE - TXWI_SIZE),
+					pMacEntry->MaxHTPhyMode.field.MCS, 0,
+					(UCHAR)pMacEntry->MaxHTPhyMode.field.MCS,
+					IFS_BACKOFF, FALSE, &pMacEntry->MaxHTPhyMode);
+	}
+
+#ifdef RT_BIG_ENDIAN
+	RTMPWIEndianChange((PUCHAR)pFirstTxWI, TYPE_TXWI);
+#endif
+
+	// Now do hardware-depened kick out.
+	HAL_KickOutMgmtTx(pAd, QueIdx, pPacket, pSrcBufVA, SrcBufLen);
+
+	// Make sure to release MGMT ring resource
+//	if (!IrqState)
+		RTMP_SEM_UNLOCK(&pAd->MgmtRingLock);
+	return NDIS_STATUS_SUCCESS;
+}
+
+
+/********************************************************************************
+
+	New DeQueue Procedures.
+
+ ********************************************************************************/
+
+#define DEQUEUE_LOCK(lock, bIntContext, IrqFlags) 				\
+			do{													\
+				if (bIntContext == FALSE)						\
+				RTMP_IRQ_LOCK((lock), IrqFlags);		\
+			}while(0)
+
+#define DEQUEUE_UNLOCK(lock, bIntContext, IrqFlags)				\
+			do{													\
+				if (bIntContext == FALSE)						\
+					RTMP_IRQ_UNLOCK((lock), IrqFlags);	\
+			}while(0)
+
+
+#if 0
+static VOID dumpTxBlk(TX_BLK *pTxBlk)
+{
+	NDIS_PACKET *pPacket;
+	int i, frameNum;
+	PQUEUE_ENTRY	pQEntry;
+
+	printk("Dump TX_BLK Structure:\n");
+	printk("\tTxFrameType=%d!\n", pTxBlk->TxFrameType);
+	printk("\tTotalFrameLen=%d\n", pTxBlk->TotalFrameLen);
+	printk("\tTotalFrameNum=%ld!\n", pTxBlk->TxPacketList.Number);
+	printk("\tTotalFragNum=%d!\n", pTxBlk->TotalFragNum);
+	printk("\tpPacketList=\n");
+
+	frameNum = pTxBlk->TxPacketList.Number;
+
+	for(i=0; i < frameNum; i++)
+	{	int j;
+		UCHAR	*pBuf;
+
+		pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
+		pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
+		if (pPacket)
+		{
+			pBuf = GET_OS_PKT_DATAPTR(pPacket);
+			printk("\t\t[%d]:ptr=0x%x, Len=%d!\n", i, (UINT32)(GET_OS_PKT_DATAPTR(pPacket)), GET_OS_PKT_LEN(pPacket));
+			printk("\t\t");
+			for (j =0 ; j < GET_OS_PKT_LEN(pPacket); j++)
+			{
+				printk("%02x ", (pBuf[j] & 0xff));
+				if (j == 16)
+					break;
+			}
+			InsertTailQueue(&pTxBlk->TxPacketList, PACKET_TO_QUEUE_ENTRY(pPacket));
+		}
+	}
+	printk("\tWcid=%d!\n", pTxBlk->Wcid);
+	printk("\tapidx=%d!\n", pTxBlk->apidx);
+	printk("----EndOfDump\n");
+
+}
+#endif
+
+
+/*
+	========================================================================
+	Tx Path design algorithm:
+		Basically, we divide the packets into four types, Broadcast/Multicast, 11N Rate(AMPDU, AMSDU, Normal), B/G Rate(ARALINK, Normal),
+		Specific Packet Type. Following show the classification rule and policy for each kinds of packets.
+				Classification Rule=>
+					Multicast: (*addr1 & 0x01) == 0x01
+					Specific : bDHCPFrame, bARPFrame, bEAPOLFrame, etc.
+					11N Rate : If peer support HT
+								(1).AMPDU  -- If TXBA is negotiated.
+								(2).AMSDU  -- If AMSDU is capable for both peer and ourself.
+											*). AMSDU can embedded in a AMPDU, but now we didn't support it.
+								(3).Normal -- Other packets which send as 11n rate.
+
+					B/G Rate : If peer is b/g only.
+								(1).ARALINK-- If both of peer/us supprot Ralink proprietary Aggregation and the TxRate is large than RATE_6
+								(2).Normal -- Other packets which send as b/g rate.
+					Fragment:
+								The packet must be unicast, NOT A-RALINK, NOT A-MSDU, NOT 11n, then can consider about fragment.
+
+				Classified Packet Handle Rule=>
+					Multicast:
+								No ACK, 		//pTxBlk->bAckRequired = FALSE;
+								No WMM, 		//pTxBlk->bWMM = FALSE;
+								No piggyback,   //pTxBlk->bPiggyBack = FALSE;
+								Force LowRate,  //pTxBlk->bForceLowRate = TRUE;
+					Specific :	Basically, for specific packet, we should handle it specifically, but now all specific packets are use
+									the same policy to handle it.
+								Force LowRate,  //pTxBlk->bForceLowRate = TRUE;
+
+					11N Rate :
+								No piggyback,	//pTxBlk->bPiggyBack = FALSE;
+
+								(1).AMSDU
+									pTxBlk->bWMM = TRUE;
+								(2).AMPDU
+									pTxBlk->bWMM = TRUE;
+								(3).Normal
+
+					B/G Rate :
+								(1).ARALINK
+
+								(2).Normal
+	========================================================================
+*/
+static UCHAR TxPktClassification(
+	IN RTMP_ADAPTER *pAd,
+	IN PNDIS_PACKET  pPacket)
+{
+	UCHAR			TxFrameType = TX_UNKOWN_FRAME;
+	UCHAR			Wcid;
+	MAC_TABLE_ENTRY	*pMacEntry = NULL;
+#ifdef DOT11_N_SUPPORT
+	BOOLEAN			bHTRate = FALSE;
+#endif // DOT11_N_SUPPORT //
+
+	Wcid = RTMP_GET_PACKET_WCID(pPacket);
+	if (Wcid == MCAST_WCID)
+	{	// Handle for RA is Broadcast/Multicast Address.
+		return TX_MCAST_FRAME;
+	}
+
+	// Handle for unicast packets
+	pMacEntry = &pAd->MacTab.Content[Wcid];
+	if (RTMP_GET_PACKET_LOWRATE(pPacket))
+	{	// It's a specific packet need to force low rate, i.e., bDHCPFrame, bEAPOLFrame, bWAIFrame
+		TxFrameType = TX_LEGACY_FRAME;
+	}
+#ifdef DOT11_N_SUPPORT
+	else if (IS_HT_RATE(pMacEntry))
+	{	// it's a 11n capable packet
+
+		// Depends on HTPhyMode to check if the peer support the HTRate transmission.
+		// 	Currently didn't support A-MSDU embedded in A-MPDU
+		bHTRate = TRUE;
+		if (RTMP_GET_PACKET_MOREDATA(pPacket) || (pMacEntry->PsMode == PWR_SAVE))
+			TxFrameType = TX_LEGACY_FRAME;
+#ifdef UAPSD_AP_SUPPORT
+		else if (RTMP_GET_PACKET_EOSP(pPacket))
+			TxFrameType = TX_LEGACY_FRAME;
+#endif // UAPSD_AP_SUPPORT //
+		else if((pMacEntry->TXBAbitmap & (1<<(RTMP_GET_PACKET_UP(pPacket)))) != 0)
+			return TX_AMPDU_FRAME;
+		else if(CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_AMSDU_INUSED))
+			return TX_AMSDU_FRAME;
+		else
+			TxFrameType = TX_LEGACY_FRAME;
+	}
+#endif // DOT11_N_SUPPORT //
+	else
+	{	// it's a legacy b/g packet.
+		if ((CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_AGGREGATION_CAPABLE) && pAd->CommonCfg.bAggregationCapable) &&
+			(RTMP_GET_PACKET_TXRATE(pPacket) >= RATE_6) &&
+			(!(OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED) && CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_WMM_CAPABLE))))
+		{	// if peer support Ralink Aggregation, we use it.
+			TxFrameType = TX_RALINK_FRAME;
+		}
+		else
+		{
+			TxFrameType = TX_LEGACY_FRAME;
+		}
+	}
+
+	// Currently, our fragment only support when a unicast packet send as NOT-ARALINK, NOT-AMSDU and NOT-AMPDU.
+	if ((RTMP_GET_PACKET_FRAGMENTS(pPacket) > 1) && (TxFrameType == TX_LEGACY_FRAME))
+		TxFrameType = TX_FRAG_FRAME;
+
+	return TxFrameType;
+}
+
+
+BOOLEAN RTMP_FillTxBlkInfo(
+	IN RTMP_ADAPTER *pAd,
+	IN TX_BLK *pTxBlk)
+{
+	PACKET_INFO			PacketInfo;
+	PNDIS_PACKET		pPacket;
+	PMAC_TABLE_ENTRY	pMacEntry = NULL;
+
+	pPacket = pTxBlk->pPacket;
+	RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pTxBlk->pSrcBufHeader, &pTxBlk->SrcBufLen);
+
+	pTxBlk->Wcid	 	 		= RTMP_GET_PACKET_WCID(pPacket);
+	pTxBlk->apidx		 		= RTMP_GET_PACKET_IF(pPacket);
+	pTxBlk->UserPriority 		= RTMP_GET_PACKET_UP(pPacket);
+	pTxBlk->FrameGap = IFS_HTTXOP;		// ASIC determine Frame Gap
+
+	if (RTMP_GET_PACKET_CLEAR_EAP_FRAME(pTxBlk->pPacket))
+		TX_BLK_SET_FLAG(pTxBlk, fTX_bClearEAPFrame);
+	else
+		TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bClearEAPFrame);
+
+	// Default to clear this flag
+	TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bForceNonQoS);
+
+
+	if (pTxBlk->Wcid == MCAST_WCID)
+	{
+		pTxBlk->pMacEntry = NULL;
+		{
+#ifdef MCAST_RATE_SPECIFIC
+			PUCHAR pDA = GET_OS_PKT_DATAPTR(pPacket);
+			if (((*pDA & 0x01) == 0x01) && (*pDA != 0xff))
+				pTxBlk->pTransmit = &pAd->CommonCfg.MCastPhyMode;
+			else
+#endif // MCAST_RATE_SPECIFIC //
+				pTxBlk->pTransmit = &pAd->MacTab.Content[MCAST_WCID].HTPhyMode;
+		}
+
+		TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bAckRequired);	// AckRequired = FALSE, when broadcast packet in Adhoc mode.
+		//TX_BLK_SET_FLAG(pTxBlk, fTX_bForceLowRate);
+		TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bAllowFrag);
+		TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bWMM);
+		if (RTMP_GET_PACKET_MOREDATA(pPacket))
+		{
+			TX_BLK_SET_FLAG(pTxBlk, fTX_bMoreData);
+		}
+
+	}
+	else
+	{
+		pTxBlk->pMacEntry = &pAd->MacTab.Content[pTxBlk->Wcid];
+		pTxBlk->pTransmit = &pTxBlk->pMacEntry->HTPhyMode;
+
+		pMacEntry = pTxBlk->pMacEntry;
+
+
+		// For all unicast packets, need Ack unless the Ack Policy is not set as NORMAL_ACK.
+		if (pAd->CommonCfg.AckPolicy[pTxBlk->QueIdx] != NORMAL_ACK)
+			TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bAckRequired);
+		else
+			TX_BLK_SET_FLAG(pTxBlk, fTX_bAckRequired);
+
+		{
+
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			{
+
+				// If support WMM, enable it.
+				if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED) &&
+					CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_WMM_CAPABLE))
+					TX_BLK_SET_FLAG(pTxBlk, fTX_bWMM);
+			}
+#endif // CONFIG_STA_SUPPORT //
+		}
+
+		if (pTxBlk->TxFrameType == TX_LEGACY_FRAME)
+		{
+			if ( (RTMP_GET_PACKET_LOWRATE(pPacket)) ||
+                ((pAd->OpMode == OPMODE_AP) && (pMacEntry->MaxHTPhyMode.field.MODE == MODE_CCK) && (pMacEntry->MaxHTPhyMode.field.MCS == RATE_1)))
+			{	// Specific packet, i.e., bDHCPFrame, bEAPOLFrame, bWAIFrame, need force low rate.
+				pTxBlk->pTransmit = &pAd->MacTab.Content[MCAST_WCID].HTPhyMode;
+#ifdef DOT11_N_SUPPORT
+				// Modify the WMM bit for ICV issue. If we have a packet with EOSP field need to set as 1, how to handle it???
+				if (IS_HT_STA(pTxBlk->pMacEntry) &&
+					(CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_RALINK_CHIPSET)) &&
+					((pAd->CommonCfg.bRdg == TRUE) && CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_RDG_CAPABLE)))
+				{
+					TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bWMM);
+					TX_BLK_SET_FLAG(pTxBlk, fTX_bForceNonQoS);
+				}
+#endif // DOT11_N_SUPPORT //
+			}
+
+#ifdef DOT11_N_SUPPORT
+			if ( (IS_HT_RATE(pMacEntry) == FALSE) &&
+				(CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_PIGGYBACK_CAPABLE)))
+			{	// Currently piggy-back only support when peer is operate in b/g mode.
+				TX_BLK_SET_FLAG(pTxBlk, fTX_bPiggyBack);
+			}
+#endif // DOT11_N_SUPPORT //
+
+			if (RTMP_GET_PACKET_MOREDATA(pPacket))
+			{
+				TX_BLK_SET_FLAG(pTxBlk, fTX_bMoreData);
+			}
+#ifdef UAPSD_AP_SUPPORT
+			if (RTMP_GET_PACKET_EOSP(pPacket))
+			{
+				TX_BLK_SET_FLAG(pTxBlk, fTX_bWMM_UAPSD_EOSP);
+			}
+#endif // UAPSD_AP_SUPPORT //
+		}
+		else if (pTxBlk->TxFrameType == TX_FRAG_FRAME)
+		{
+			TX_BLK_SET_FLAG(pTxBlk, fTX_bAllowFrag);
+		}
+
+		pMacEntry->DebugTxCount++;
+	}
+
+	return TRUE;
+
+FillTxBlkErr:
+	return FALSE;
+}
+
+
+BOOLEAN CanDoAggregateTransmit(
+	IN RTMP_ADAPTER *pAd,
+	IN NDIS_PACKET *pPacket,
+	IN TX_BLK		*pTxBlk)
+{
+
+	//printk("Check if can do aggregation! TxFrameType=%d!\n", pTxBlk->TxFrameType);
+
+	if (RTMP_GET_PACKET_WCID(pPacket) == MCAST_WCID)
+		return FALSE;
+
+	if (RTMP_GET_PACKET_DHCP(pPacket) ||
+		RTMP_GET_PACKET_EAPOL(pPacket) ||
+		RTMP_GET_PACKET_WAI(pPacket))
+		return FALSE;
+
+	if ((pTxBlk->TxFrameType == TX_AMSDU_FRAME) &&
+		((pTxBlk->TotalFrameLen + GET_OS_PKT_LEN(pPacket))> (RX_BUFFER_AGGRESIZE - 100)))
+	{	// For AMSDU, allow the packets with total length < max-amsdu size
+		return FALSE;
+	}
+
+	if ((pTxBlk->TxFrameType == TX_RALINK_FRAME) &&
+		(pTxBlk->TxPacketList.Number == 2))
+	{	// For RALINK-Aggregation, allow two frames in one batch.
+		return FALSE;
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	if ((INFRA_ON(pAd)) && (pAd->OpMode == OPMODE_STA)) // must be unicast to AP
+		return TRUE;
+	else
+#endif // CONFIG_STA_SUPPORT //
+		return FALSE;
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		To do the enqueue operation and extract the first item of waiting
+		list. If a number of available shared memory segments could meet
+		the request of extracted item, the extracted item will be fragmented
+		into shared memory segments.
+
+	Arguments:
+		pAd Pointer to our adapter
+		pQueue		Pointer to Waiting Queue
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID RTMPDeQueuePacket(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  BOOLEAN         bIntContext,
+	IN  UCHAR			QIdx, /* BulkOutPipeId */
+	IN  UCHAR           Max_Tx_Packets)
+{
+	PQUEUE_ENTRY    pEntry = NULL;
+	PNDIS_PACKET 	pPacket;
+	NDIS_STATUS     Status = NDIS_STATUS_SUCCESS;
+	UCHAR           Count=0;
+	PQUEUE_HEADER   pQueue;
+	ULONG           FreeNumber[NUM_OF_TX_RING];
+	UCHAR			QueIdx, sQIdx, eQIdx;
+	unsigned long	IrqFlags = 0;
+	BOOLEAN			hasTxDesc = FALSE;
+	TX_BLK			TxBlk;
+	TX_BLK			*pTxBlk;
+
+#ifdef DBG_DIAGNOSE
+	BOOLEAN			firstRound;
+	RtmpDiagStruct	*pDiagStruct = &pAd->DiagStruct;
+#endif
+
+
+	if (QIdx == NUM_OF_TX_RING)
+	{
+		sQIdx = 0;
+		eQIdx = 3;	// 4 ACs, start from 0.
+	}
+	else
+	{
+		sQIdx = eQIdx = QIdx;
+	}
+
+	for (QueIdx=sQIdx; QueIdx <= eQIdx; QueIdx++)
+	{
+		Count=0;
+
+		RT28XX_START_DEQUEUE(pAd, QueIdx, IrqFlags);
+
+#ifdef DBG_DIAGNOSE
+		firstRound = ((QueIdx == 0) ? TRUE : FALSE);
+#endif // DBG_DIAGNOSE //
+
+		while (1)
+		{
+			if ((RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS |
+										fRTMP_ADAPTER_RADIO_OFF |
+										fRTMP_ADAPTER_RESET_IN_PROGRESS |
+										fRTMP_ADAPTER_HALT_IN_PROGRESS |
+										fRTMP_ADAPTER_NIC_NOT_EXIST))))
+			{
+				RT28XX_STOP_DEQUEUE(pAd, QueIdx, IrqFlags);
+				return;
+			}
+
+			if (Count >= Max_Tx_Packets)
+				break;
+
+			DEQUEUE_LOCK(&pAd->irq_lock, bIntContext, IrqFlags);
+			if (&pAd->TxSwQueue[QueIdx] == NULL)
+			{
+#ifdef DBG_DIAGNOSE
+				if (firstRound == TRUE)
+					pDiagStruct->TxSWQueCnt[pDiagStruct->ArrayCurIdx][0]++;
+#endif // DBG_DIAGNOSE //
+				DEQUEUE_UNLOCK(&pAd->irq_lock, bIntContext, IrqFlags);
+				break;
+			}
+
+
+			// probe the Queue Head
+			pQueue = &pAd->TxSwQueue[QueIdx];
+			if ((pEntry = pQueue->Head) == NULL)
+			{
+				DEQUEUE_UNLOCK(&pAd->irq_lock, bIntContext, IrqFlags);
+				break;
+			}
+
+			pTxBlk = &TxBlk;
+			NdisZeroMemory((PUCHAR)pTxBlk, sizeof(TX_BLK));
+			//InitializeQueueHeader(&pTxBlk->TxPacketList);		// Didn't need it because we already memzero it.
+			pTxBlk->QueIdx = QueIdx;
+
+			pPacket = QUEUE_ENTRY_TO_PKT(pEntry);
+
+			// Early check to make sure we have enoguh Tx Resource.
+			hasTxDesc = RT28XX_HAS_ENOUGH_FREE_DESC(pAd, pTxBlk, FreeNumber[QueIdx], pPacket);
+			if (!hasTxDesc)
+			{
+				pAd->PrivateInfo.TxRingFullCnt++;
+
+				DEQUEUE_UNLOCK(&pAd->irq_lock, bIntContext, IrqFlags);
+
+				break;
+			}
+
+			pTxBlk->TxFrameType = TxPktClassification(pAd, pPacket);
+			pEntry = RemoveHeadQueue(pQueue);
+			pTxBlk->TotalFrameNum++;
+			pTxBlk->TotalFragNum += RTMP_GET_PACKET_FRAGMENTS(pPacket);	// The real fragment number maybe vary
+			pTxBlk->TotalFrameLen += GET_OS_PKT_LEN(pPacket);
+			pTxBlk->pPacket = pPacket;
+			InsertTailQueue(&pTxBlk->TxPacketList, PACKET_TO_QUEUE_ENTRY(pPacket));
+
+			if (pTxBlk->TxFrameType == TX_RALINK_FRAME || pTxBlk->TxFrameType == TX_AMSDU_FRAME)
+			{
+				// Enhance SW Aggregation Mechanism
+				if (NEED_QUEUE_BACK_FOR_AGG(pAd, QueIdx, FreeNumber[QueIdx], pTxBlk->TxFrameType))
+				{
+					InsertHeadQueue(pQueue, PACKET_TO_QUEUE_ENTRY(pPacket));
+					DEQUEUE_UNLOCK(&pAd->irq_lock, bIntContext, IrqFlags);
+					break;
+				}
+
+				do{
+					if((pEntry = pQueue->Head) == NULL)
+						break;
+
+					// For TX_AMSDU_FRAME/TX_RALINK_FRAME, Need to check if next pakcet can do aggregation.
+					pPacket = QUEUE_ENTRY_TO_PKT(pEntry);
+					FreeNumber[QueIdx] = GET_TXRING_FREENO(pAd, QueIdx);
+					hasTxDesc = RT28XX_HAS_ENOUGH_FREE_DESC(pAd, pTxBlk, FreeNumber[QueIdx], pPacket);
+					if ((hasTxDesc == FALSE) || (CanDoAggregateTransmit(pAd, pPacket, pTxBlk) == FALSE))
+						break;
+
+					//Remove the packet from the TxSwQueue and insert into pTxBlk
+					pEntry = RemoveHeadQueue(pQueue);
+					ASSERT(pEntry);
+					pPacket = QUEUE_ENTRY_TO_PKT(pEntry);
+					pTxBlk->TotalFrameNum++;
+					pTxBlk->TotalFragNum += RTMP_GET_PACKET_FRAGMENTS(pPacket);	// The real fragment number maybe vary
+					pTxBlk->TotalFrameLen += GET_OS_PKT_LEN(pPacket);
+					InsertTailQueue(&pTxBlk->TxPacketList, PACKET_TO_QUEUE_ENTRY(pPacket));
+				}while(1);
+
+				if (pTxBlk->TxPacketList.Number == 1)
+					pTxBlk->TxFrameType = TX_LEGACY_FRAME;
+			}
+
+#ifdef RT2870
+			DEQUEUE_UNLOCK(&pAd->irq_lock, bIntContext, IrqFlags);
+#endif // RT2870 //
+
+			Count += pTxBlk->TxPacketList.Number;
+
+				// Do HardTransmit now.
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				Status = STAHardTransmit(pAd, pTxBlk, QueIdx);
+#endif // CONFIG_STA_SUPPORT //
+
+
+#if 0	// We should not break if HardTransmit failed. Well, at least now we should not!
+			if (Status != NDIS_STATUS_SUCCESS)
+			{
+				DBGPRINT(RT_DEBUG_TRACE /*RT_DEBUG_INFO*/,("RTMPHardTransmit return failed!!!\n"));
+				break;
+			}
+#endif
+		}
+
+		RT28XX_STOP_DEQUEUE(pAd, QueIdx, IrqFlags);
+
+#ifdef RT2870
+		if (!hasTxDesc)
+			RTUSBKickBulkOut(pAd);
+#endif // RT2870 //
+
+#ifdef BLOCK_NET_IF
+		if ((pAd->blockQueueTab[QueIdx].SwTxQueueBlockFlag == TRUE)
+			&& (pAd->TxSwQueue[QueIdx].Number < 1))
+		{
+			releaseNetIf(&pAd->blockQueueTab[QueIdx]);
+		}
+#endif // BLOCK_NET_IF //
+
+	}
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Calculates the duration which is required to transmit out frames
+	with given size and specified rate.
+
+	Arguments:
+		pAd 	Pointer to our adapter
+		Rate			Transmit rate
+		Size			Frame size in units of byte
+
+	Return Value:
+		Duration number in units of usec
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+USHORT	RTMPCalcDuration(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Rate,
+	IN	ULONG			Size)
+{
+	ULONG	Duration = 0;
+
+	if (Rate < RATE_FIRST_OFDM_RATE) // CCK
+	{
+		if ((Rate > RATE_1) && OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED))
+			Duration = 96;	// 72+24 preamble+plcp
+		else
+			Duration = 192; // 144+48 preamble+plcp
+
+		Duration += (USHORT)((Size << 4) / RateIdTo500Kbps[Rate]);
+		if ((Size << 4) % RateIdTo500Kbps[Rate])
+			Duration ++;
+	}
+	else if (Rate <= RATE_LAST_OFDM_RATE)// OFDM rates
+	{
+		Duration = 20 + 6;		// 16+4 preamble+plcp + Signal Extension
+		Duration += 4 * (USHORT)((11 + Size * 4) / RateIdTo500Kbps[Rate]);
+		if ((11 + Size * 4) % RateIdTo500Kbps[Rate])
+			Duration += 4;
+	}
+	else	//mimo rate
+	{
+		Duration = 20 + 6;		// 16+4 preamble+plcp + Signal Extension
+	}
+
+	return (USHORT)Duration;
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Calculates the duration which is required to transmit out frames
+	with given size and specified rate.
+
+	Arguments:
+		pTxWI		Pointer to head of each MPDU to HW.
+		Ack 		Setting for Ack requirement bit
+		Fragment	Setting for Fragment bit
+		RetryMode	Setting for retry mode
+		Ifs 		Setting for IFS gap
+		Rate		Setting for transmit rate
+		Service 	Setting for service
+		Length		Frame length
+		TxPreamble	Short or Long preamble when using CCK rates
+		QueIdx - 0-3, according to 802.11e/d4.4 June/2003
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+    See also : BASmartHardTransmit()    !!!
+
+	========================================================================
+*/
+VOID RTMPWriteTxWI(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTXWI_STRUC 	pOutTxWI,
+	IN	BOOLEAN			FRAG,
+	IN	BOOLEAN			CFACK,
+	IN	BOOLEAN			InsTimestamp,
+	IN	BOOLEAN 		AMPDU,
+	IN	BOOLEAN 		Ack,
+	IN	BOOLEAN 		NSeq,		// HW new a sequence.
+	IN	UCHAR			BASize,
+	IN	UCHAR			WCID,
+	IN	ULONG			Length,
+	IN	UCHAR 			PID,
+	IN	UCHAR			TID,
+	IN	UCHAR			TxRate,
+	IN	UCHAR			Txopmode,
+	IN	BOOLEAN			CfAck,
+	IN	HTTRANSMIT_SETTING	*pTransmit)
+{
+	PMAC_TABLE_ENTRY	pMac = NULL;
+	TXWI_STRUC 		TxWI;
+	PTXWI_STRUC 	pTxWI;
+
+	if (WCID < MAX_LEN_OF_MAC_TABLE)
+		pMac = &pAd->MacTab.Content[WCID];
+
+	//
+	// Always use Long preamble before verifiation short preamble functionality works well.
+	// Todo: remove the following line if short preamble functionality works
+	//
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED);
+	NdisZeroMemory(&TxWI, TXWI_SIZE);
+	pTxWI = &TxWI;
+
+	pTxWI->FRAG= FRAG;
+
+	pTxWI->CFACK = CFACK;
+	pTxWI->TS= InsTimestamp;
+	pTxWI->AMPDU = AMPDU;
+	pTxWI->ACK = Ack;
+	pTxWI->txop= Txopmode;
+
+	pTxWI->NSEQ = NSeq;
+	// John tune the performace with Intel Client in 20 MHz performance
+#ifdef DOT11_N_SUPPORT
+	BASize = pAd->CommonCfg.TxBASize;
+
+	if( BASize >7 )
+		BASize =7;
+	pTxWI->BAWinSize = BASize;
+	pTxWI->ShortGI = pTransmit->field.ShortGI;
+	pTxWI->STBC = pTransmit->field.STBC;
+#endif // DOT11_N_SUPPORT //
+
+	pTxWI->WirelessCliID = WCID;
+	pTxWI->MPDUtotalByteCount = Length;
+	pTxWI->PacketId = PID;
+
+	// If CCK or OFDM, BW must be 20
+	pTxWI->BW = (pTransmit->field.MODE <= MODE_OFDM) ? (BW_20) : (pTransmit->field.BW);
+#ifdef DOT11N_DRAFT3
+	if (pTxWI->BW)
+		pTxWI->BW = (pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth == 0) ? (BW_20) : (pTransmit->field.BW);
+#endif // DOT11N_DRAFT3 //
+
+	pTxWI->MCS = pTransmit->field.MCS;
+	pTxWI->PHYMODE = pTransmit->field.MODE;
+	pTxWI->CFACK = CfAck;
+
+#ifdef DOT11_N_SUPPORT
+	if (pMac)
+	{
+		if (pAd->CommonCfg.bMIMOPSEnable)
+		{
+			if ((pMac->MmpsMode == MMPS_DYNAMIC) && (pTransmit->field.MCS > 7))
+			{
+				// Dynamic MIMO Power Save Mode
+				pTxWI->MIMOps = 1;
+			}
+			else if (pMac->MmpsMode == MMPS_STATIC)
+			{
+				// Static MIMO Power Save Mode
+				if (pTransmit->field.MODE >= MODE_HTMIX && pTransmit->field.MCS > 7)
+				{
+					pTxWI->MCS = 7;
+					pTxWI->MIMOps = 0;
+				}
+			}
+		}
+		//pTxWI->MIMOps = (pMac->PsMode == PWR_MMPS)? 1:0;
+		if (pMac->bIAmBadAtheros && (pMac->WepStatus != Ndis802_11WEPDisabled))
+		{
+			pTxWI->MpduDensity = 7;
+		}
+		else
+		{
+			pTxWI->MpduDensity = pMac->MpduDensity;
+		}
+	}
+#endif // DOT11_N_SUPPORT //
+
+	pTxWI->PacketId = pTxWI->MCS;
+	NdisMoveMemory(pOutTxWI, &TxWI, sizeof(TXWI_STRUC));
+}
+
+
+VOID RTMPWriteTxWI_Data(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	OUT PTXWI_STRUC		pTxWI,
+	IN	TX_BLK				*pTxBlk)
+{
+	HTTRANSMIT_SETTING	*pTransmit;
+	PMAC_TABLE_ENTRY	pMacEntry;
+#ifdef DOT11_N_SUPPORT
+	UCHAR				BASize;
+#endif // DOT11_N_SUPPORT //
+
+
+	ASSERT(pTxWI);
+
+	pTransmit = pTxBlk->pTransmit;
+	pMacEntry = pTxBlk->pMacEntry;
+
+
+	//
+	// Always use Long preamble before verifiation short preamble functionality works well.
+	// Todo: remove the following line if short preamble functionality works
+	//
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED);
+	NdisZeroMemory(pTxWI, TXWI_SIZE);
+
+	pTxWI->FRAG		= TX_BLK_TEST_FLAG(pTxBlk, fTX_bAllowFrag);
+	pTxWI->ACK		= TX_BLK_TEST_FLAG(pTxBlk, fTX_bAckRequired);
+	pTxWI->txop		= pTxBlk->FrameGap;
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+	if (pMacEntry &&
+		(pAd->StaCfg.BssType == BSS_INFRA) &&
+		(pMacEntry->ValidAsDls == TRUE))
+		pTxWI->WirelessCliID = BSSID_WCID;
+	else
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+		pTxWI->WirelessCliID		= pTxBlk->Wcid;
+
+	pTxWI->MPDUtotalByteCount	= pTxBlk->MpduHeaderLen + pTxBlk->SrcBufLen;
+	pTxWI->CFACK				= TX_BLK_TEST_FLAG(pTxBlk, fTX_bPiggyBack);
+
+	// If CCK or OFDM, BW must be 20
+	pTxWI->BW = (pTransmit->field.MODE <= MODE_OFDM) ? (BW_20) : (pTransmit->field.BW);
+#ifdef DOT11_N_SUPPORT
+#ifdef DOT11N_DRAFT3
+	if (pTxWI->BW)
+		pTxWI->BW = (pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth == 0) ? (BW_20) : (pTransmit->field.BW);
+#endif // DOT11N_DRAFT3 //
+	pTxWI->AMPDU	= ((pTxBlk->TxFrameType == TX_AMPDU_FRAME) ? TRUE : FALSE);
+
+	// John tune the performace with Intel Client in 20 MHz performance
+	BASize = pAd->CommonCfg.TxBASize;
+	if((pTxBlk->TxFrameType == TX_AMPDU_FRAME) && (pMacEntry))
+	{
+		UCHAR		RABAOriIdx = 0;	//The RA's BA Originator table index.
+
+		RABAOriIdx = pTxBlk->pMacEntry->BAOriWcidArray[pTxBlk->UserPriority];
+		BASize = pAd->BATable.BAOriEntry[RABAOriIdx].BAWinSize;
+	}
+
+#if 0 // 3*3
+	if (BASize > 7)
+		BASize = 7;
+#endif
+
+	pTxWI->TxBF = pTransmit->field.TxBF;
+	pTxWI->BAWinSize = BASize;
+	pTxWI->ShortGI = pTransmit->field.ShortGI;
+	pTxWI->STBC = pTransmit->field.STBC;
+#endif // DOT11_N_SUPPORT //
+
+	pTxWI->MCS = pTransmit->field.MCS;
+	pTxWI->PHYMODE = pTransmit->field.MODE;
+
+#ifdef DOT11_N_SUPPORT
+	if (pMacEntry)
+	{
+		if ((pMacEntry->MmpsMode == MMPS_DYNAMIC) && (pTransmit->field.MCS > 7))
+		{
+			// Dynamic MIMO Power Save Mode
+			pTxWI->MIMOps = 1;
+		}
+		else if (pMacEntry->MmpsMode == MMPS_STATIC)
+		{
+			// Static MIMO Power Save Mode
+			if (pTransmit->field.MODE >= MODE_HTMIX && pTransmit->field.MCS > 7)
+			{
+				pTxWI->MCS = 7;
+				pTxWI->MIMOps = 0;
+			}
+		}
+
+		if (pMacEntry->bIAmBadAtheros && (pMacEntry->WepStatus != Ndis802_11WEPDisabled))
+		{
+			pTxWI->MpduDensity = 7;
+		}
+		else
+		{
+			pTxWI->MpduDensity = pMacEntry->MpduDensity;
+		}
+	}
+#endif // DOT11_N_SUPPORT //
+
+#ifdef DBG_DIAGNOSE
+		if (pTxBlk->QueIdx== 0)
+		{
+			pAd->DiagStruct.TxDataCnt[pAd->DiagStruct.ArrayCurIdx]++;
+			pAd->DiagStruct.TxMcsCnt[pAd->DiagStruct.ArrayCurIdx][pTxWI->MCS]++;
+		}
+#endif // DBG_DIAGNOSE //
+
+	// for rate adapation
+	pTxWI->PacketId = pTxWI->MCS;
+}
+
+
+VOID RTMPWriteTxWI_Cache(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	OUT PTXWI_STRUC		pTxWI,
+	IN	TX_BLK				*pTxBlk)
+{
+	PHTTRANSMIT_SETTING	/*pTxHTPhyMode,*/ pTransmit;
+	PMAC_TABLE_ENTRY	pMacEntry;
+
+	//
+	// update TXWI
+	//
+	pMacEntry = pTxBlk->pMacEntry;
+	pTransmit = pTxBlk->pTransmit;
+
+	if (pMacEntry->bAutoTxRateSwitch)
+	{
+		pTxWI->txop = IFS_HTTXOP;
+
+		// If CCK or OFDM, BW must be 20
+		pTxWI->BW = (pTransmit->field.MODE <= MODE_OFDM) ? (BW_20) : (pTransmit->field.BW);
+		pTxWI->ShortGI = pTransmit->field.ShortGI;
+		pTxWI->STBC = pTransmit->field.STBC;
+
+		pTxWI->MCS = pTransmit->field.MCS;
+		pTxWI->PHYMODE = pTransmit->field.MODE;
+
+		// set PID for TxRateSwitching
+		pTxWI->PacketId = pTransmit->field.MCS;
+	}
+
+#ifdef DOT11_N_SUPPORT
+	pTxWI->AMPDU = ((pMacEntry->NoBADataCountDown == 0) ? TRUE: FALSE);
+	pTxWI->MIMOps = 0;
+
+#ifdef DOT11N_DRAFT3
+	if (pTxWI->BW)
+		pTxWI->BW = (pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth == 0) ? (BW_20) : (pTransmit->field.BW);
+#endif // DOT11N_DRAFT3 //
+
+	if (pAd->CommonCfg.bMIMOPSEnable)
+	{
+		// MIMO Power Save Mode
+		if ((pMacEntry->MmpsMode == MMPS_DYNAMIC) && (pTransmit->field.MCS > 7))
+		{
+			// Dynamic MIMO Power Save Mode
+			pTxWI->MIMOps = 1;
+		}
+		else if (pMacEntry->MmpsMode == MMPS_STATIC)
+		{
+			// Static MIMO Power Save Mode
+			if ((pTransmit->field.MODE >= MODE_HTMIX) && (pTransmit->field.MCS > 7))
+			{
+				pTxWI->MCS = 7;
+				pTxWI->MIMOps = 0;
+			}
+		}
+	}
+#endif // DOT11_N_SUPPORT //
+
+#ifdef DBG_DIAGNOSE
+	if (pTxBlk->QueIdx== 0)
+	{
+		pAd->DiagStruct.TxDataCnt[pAd->DiagStruct.ArrayCurIdx]++;
+		pAd->DiagStruct.TxMcsCnt[pAd->DiagStruct.ArrayCurIdx][pTxWI->MCS]++;
+	}
+#endif // DBG_DIAGNOSE //
+
+	pTxWI->MPDUtotalByteCount = pTxBlk->MpduHeaderLen + pTxBlk->SrcBufLen;
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Calculates the duration which is required to transmit out frames
+	with given size and specified rate.
+
+	Arguments:
+		pTxD		Pointer to transmit descriptor
+		Ack 		Setting for Ack requirement bit
+		Fragment	Setting for Fragment bit
+		RetryMode	Setting for retry mode
+		Ifs 		Setting for IFS gap
+		Rate		Setting for transmit rate
+		Service 	Setting for service
+		Length		Frame length
+		TxPreamble	Short or Long preamble when using CCK rates
+		QueIdx - 0-3, according to 802.11e/d4.4 June/2003
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	========================================================================
+*/
+VOID RTMPWriteTxDescriptor(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTXD_STRUC		pTxD,
+	IN	BOOLEAN 		bWIV,
+	IN	UCHAR			QueueSEL)
+{
+	//
+	// Always use Long preamble before verifiation short preamble functionality works well.
+	// Todo: remove the following line if short preamble functionality works
+	//
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED);
+
+	pTxD->WIV	= (bWIV) ? 1: 0;
+	pTxD->QSEL= (QueueSEL);
+	//RT2860c??  fixed using EDCA queue for test...  We doubt Queue1 has problem.  2006-09-26 Jan
+	//pTxD->QSEL= FIFO_EDCA;
+	if (pAd->bGenOneHCCA == TRUE)
+		pTxD->QSEL= FIFO_HCCA;
+	pTxD->DMADONE = 0;
+}
+
+
+// should be called only when -
+// 1. MEADIA_CONNECTED
+// 2. AGGREGATION_IN_USED
+// 3. Fragmentation not in used
+// 4. either no previous frame (pPrevAddr1=NULL) .OR. previoud frame is aggregatible
+BOOLEAN TxFrameIsAggregatible(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pPrevAddr1,
+	IN	PUCHAR			p8023hdr)
+{
+
+	// can't aggregate EAPOL (802.1x) frame
+	if ((p8023hdr[12] == 0x88) && (p8023hdr[13] == 0x8e))
+		return FALSE;
+
+	// can't aggregate multicast/broadcast frame
+	if (p8023hdr[0] & 0x01)
+		return FALSE;
+
+	if (INFRA_ON(pAd)) // must be unicast to AP
+		return TRUE;
+	else if ((pPrevAddr1 == NULL) || MAC_ADDR_EQUAL(pPrevAddr1, p8023hdr)) // unicast to same STA
+		return TRUE;
+	else
+		return FALSE;
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+	   Check the MSDU Aggregation policy
+	1.HT aggregation is A-MSDU
+	2.legaacy rate aggregation is software aggregation by Ralink.
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+BOOLEAN PeerIsAggreOn(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	ULONG		   TxRate,
+	IN	PMAC_TABLE_ENTRY pMacEntry)
+{
+	ULONG	AFlags = (fCLIENT_STATUS_AMSDU_INUSED | fCLIENT_STATUS_AGGREGATION_CAPABLE);
+
+	if (pMacEntry != NULL && CLIENT_STATUS_TEST_FLAG(pMacEntry, AFlags))
+	{
+#ifdef DOT11_N_SUPPORT
+		if (pMacEntry->HTPhyMode.field.MODE >= MODE_HTMIX)
+		{
+			return TRUE;
+		}
+#endif // DOT11_N_SUPPORT //
+
+#ifdef AGGREGATION_SUPPORT
+		if (TxRate >= RATE_6 && pAd->CommonCfg.bAggregationCapable && (!(OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED) && CLIENT_STATUS_TEST_FLAG(pMacEntry, fCLIENT_STATUS_WMM_CAPABLE))))
+		{	// legacy  Ralink Aggregation support
+			return TRUE;
+		}
+#endif // AGGREGATION_SUPPORT //
+	}
+
+	return FALSE;
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Check and fine the packet waiting in SW queue with highest priority
+
+	Arguments:
+		pAd Pointer to our adapter
+
+	Return Value:
+		pQueue		Pointer to Waiting Queue
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+PQUEUE_HEADER	RTMPCheckTxSwQueue(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT PUCHAR			pQueIdx)
+{
+
+	ULONG	Number;
+	// 2004-11-15 to be removed. test aggregation only
+//	if ((OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED)) && (*pNumber < 2))
+//		 return NULL;
+
+	Number = pAd->TxSwQueue[QID_AC_BK].Number
+			 + pAd->TxSwQueue[QID_AC_BE].Number
+			 + pAd->TxSwQueue[QID_AC_VI].Number
+			 + pAd->TxSwQueue[QID_AC_VO].Number
+			 + pAd->TxSwQueue[QID_HCCA].Number;
+
+	if (pAd->TxSwQueue[QID_AC_VO].Head != NULL)
+	{
+		*pQueIdx = QID_AC_VO;
+		return (&pAd->TxSwQueue[QID_AC_VO]);
+	}
+	else if (pAd->TxSwQueue[QID_AC_VI].Head != NULL)
+	{
+		*pQueIdx = QID_AC_VI;
+		return (&pAd->TxSwQueue[QID_AC_VI]);
+	}
+	else if (pAd->TxSwQueue[QID_AC_BE].Head != NULL)
+	{
+		*pQueIdx = QID_AC_BE;
+		return (&pAd->TxSwQueue[QID_AC_BE]);
+	}
+	else if (pAd->TxSwQueue[QID_AC_BK].Head != NULL)
+	{
+		*pQueIdx = QID_AC_BK;
+		return (&pAd->TxSwQueue[QID_AC_BK]);
+	}
+	else if (pAd->TxSwQueue[QID_HCCA].Head != NULL)
+	{
+		*pQueIdx = QID_HCCA;
+		return (&pAd->TxSwQueue[QID_HCCA]);
+	}
+
+	// No packet pending in Tx Sw queue
+	*pQueIdx = QID_AC_BK;
+
+	return (NULL);
+}
+
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Suspend MSDU transmission
+
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPSuspendMsduTransmission(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	DBGPRINT(RT_DEBUG_TRACE,("SCANNING, suspend MSDU transmission ...\n"));
+
+
+	//
+	// Before BSS_SCAN_IN_PROGRESS, we need to keep Current R66 value and
+	// use Lowbound as R66 value on ScanNextChannel(...)
+	//
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R66, &pAd->BbpTuning.R66CurrentValue);
+
+	// set BBP_R66 to 0x30/0x40 when scanning (AsicSwitchChannel will set R66 according to channel when scanning)
+	//RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, (0x26 + GET_LNA_GAIN(pAd)));
+	RTMPSetAGCInitValue(pAd, BW_20);
+
+	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS);
+	//RTMP_IO_WRITE32(pAd, TX_CNTL_CSR, 0x000f0000);		// abort all TX rings
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Resume MSDU transmission
+
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID RTMPResumeMsduTransmission(
+	IN	PRTMP_ADAPTER	pAd)
+{
+//    UCHAR			IrqState;
+
+	DBGPRINT(RT_DEBUG_TRACE,("SCAN done, resume MSDU transmission ...\n"));
+
+
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, pAd->BbpTuning.R66CurrentValue);
+
+	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS);
+// sample, for IRQ LOCK to SEM LOCK
+//    IrqState = pAd->irq_disabled;
+//	if (IrqState)
+//		RTMPDeQueuePacket(pAd, TRUE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+//    else
+	RTMPDeQueuePacket(pAd, FALSE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+}
+
+
+UINT deaggregate_AMSDU_announce(
+	IN	PRTMP_ADAPTER	pAd,
+	PNDIS_PACKET		pPacket,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize)
+{
+	USHORT 			PayloadSize;
+	USHORT 			SubFrameSize;
+	PHEADER_802_3 	pAMSDUsubheader;
+	UINT			nMSDU;
+    UCHAR			Header802_3[14];
+
+	PUCHAR			pPayload, pDA, pSA, pRemovedLLCSNAP;
+	PNDIS_PACKET	pClonePacket;
+
+
+
+	nMSDU = 0;
+
+	while (DataSize > LENGTH_802_3)
+	{
+
+		nMSDU++;
+
+		//hex_dump("subheader", pData, 64);
+		pAMSDUsubheader = (PHEADER_802_3)pData;
+		//pData += LENGTH_802_3;
+		PayloadSize = pAMSDUsubheader->Octet[1] + (pAMSDUsubheader->Octet[0]<<8);
+		SubFrameSize = PayloadSize + LENGTH_802_3;
+
+
+		if ((DataSize < SubFrameSize) || (PayloadSize > 1518 ))
+		{
+			break;
+		}
+
+		//printk("%d subframe: Size = %d\n",  nMSDU, PayloadSize);
+
+		pPayload = pData + LENGTH_802_3;
+		pDA = pData;
+		pSA = pData + MAC_ADDR_LEN;
+
+		// convert to 802.3 header
+        CONVERT_TO_802_3(Header802_3, pDA, pSA, pPayload, PayloadSize, pRemovedLLCSNAP);
+
+#ifdef CONFIG_STA_SUPPORT
+		if ((Header802_3[12] == 0x88) && (Header802_3[13] == 0x8E) )
+		{
+		    // avoid local heap overflow, use dyanamic allocation
+		   MLME_QUEUE_ELEM *Elem = (MLME_QUEUE_ELEM *) kmalloc(sizeof(MLME_QUEUE_ELEM), MEM_ALLOC_FLAG);
+		   memmove(Elem->Msg+(LENGTH_802_11 + LENGTH_802_1_H), pPayload, PayloadSize);
+		   Elem->MsgLen = LENGTH_802_11 + LENGTH_802_1_H + PayloadSize;
+		   WpaEAPOLKeyAction(pAd, Elem);
+		   kfree(Elem);
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+	        	if (pRemovedLLCSNAP)
+	        	{
+	    			pPayload -= LENGTH_802_3;
+	    			PayloadSize += LENGTH_802_3;
+	    			NdisMoveMemory(pPayload, &Header802_3[0], LENGTH_802_3);
+	        	}
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+		pClonePacket = ClonePacket(pAd, pPacket, pPayload, PayloadSize);
+		if (pClonePacket)
+		{
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				ANNOUNCE_OR_FORWARD_802_3_PACKET(pAd, pClonePacket, RTMP_GET_PACKET_IF(pPacket));
+#endif // CONFIG_STA_SUPPORT //
+		}
+
+
+		// A-MSDU has padding to multiple of 4 including subframe header.
+		// align SubFrameSize up to multiple of 4
+		SubFrameSize = (SubFrameSize+3)&(~0x3);
+
+
+		if (SubFrameSize > 1528 || SubFrameSize < 32)
+		{
+			break;
+		}
+
+		if (DataSize > SubFrameSize)
+		{
+			pData += SubFrameSize;
+			DataSize -= SubFrameSize;
+		}
+		else
+		{
+			// end of A-MSDU
+			DataSize = 0;
+		}
+	}
+
+	// finally release original rx packet
+	RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_SUCCESS);
+
+	return nMSDU;
+}
+
+
+UINT BA_Reorder_AMSDU_Annnounce(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket)
+{
+	PUCHAR			pData;
+	USHORT			DataSize;
+	UINT			nMSDU = 0;
+
+	pData = (PUCHAR) GET_OS_PKT_DATAPTR(pPacket);
+	DataSize = (USHORT) GET_OS_PKT_LEN(pPacket);
+
+	nMSDU = deaggregate_AMSDU_announce(pAd, pPacket, pData, DataSize);
+
+	return nMSDU;
+}
+
+
+/*
+	==========================================================================
+	Description:
+		Look up the MAC address in the MAC table. Return NULL if not found.
+	Return:
+		pEntry - pointer to the MAC entry; NULL is not found
+	==========================================================================
+*/
+MAC_TABLE_ENTRY *MacTableLookup(
+	IN PRTMP_ADAPTER pAd,
+	PUCHAR pAddr)
+{
+	ULONG HashIdx;
+	MAC_TABLE_ENTRY *pEntry = NULL;
+
+	HashIdx = MAC_ADDR_HASH_INDEX(pAddr);
+	pEntry = pAd->MacTab.Hash[HashIdx];
+
+	while (pEntry && (pEntry->ValidAsCLI || pEntry->ValidAsWDS || pEntry->ValidAsApCli || pEntry->ValidAsMesh))
+	{
+		if (MAC_ADDR_EQUAL(pEntry->Addr, pAddr))
+		{
+			break;
+		}
+		else
+			pEntry = pEntry->pNext;
+	}
+
+	return pEntry;
+}
+
+MAC_TABLE_ENTRY *MacTableInsertEntry(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR			pAddr,
+	IN	UCHAR			apidx,
+	IN BOOLEAN	CleanAll)
+{
+	UCHAR HashIdx;
+	int i, FirstWcid;
+	MAC_TABLE_ENTRY *pEntry = NULL, *pCurrEntry;
+
+	// if FULL, return
+	if (pAd->MacTab.Size >= MAX_LEN_OF_MAC_TABLE)
+		return NULL;
+
+	FirstWcid = 1;
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	if (pAd->StaCfg.BssType == BSS_INFRA)
+		FirstWcid = 2;
+#endif // CONFIG_STA_SUPPORT //
+
+	// allocate one MAC entry
+	NdisAcquireSpinLock(&pAd->MacTabLock);
+	for (i = FirstWcid; i< MAX_LEN_OF_MAC_TABLE; i++)   // skip entry#0 so that "entry index == AID" for fast lookup
+	{
+		// pick up the first available vacancy
+		if ((pAd->MacTab.Content[i].ValidAsCLI == FALSE) &&
+			(pAd->MacTab.Content[i].ValidAsWDS == FALSE) &&
+			(pAd->MacTab.Content[i].ValidAsApCli== FALSE) &&
+			(pAd->MacTab.Content[i].ValidAsMesh == FALSE)
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+			&& (pAd->MacTab.Content[i].ValidAsDls == FALSE)
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+			)
+		{
+			pEntry = &pAd->MacTab.Content[i];
+			if (CleanAll == TRUE)
+			{
+				pEntry->MaxSupportedRate = RATE_11;
+				pEntry->CurrTxRate = RATE_11;
+				NdisZeroMemory(pEntry, sizeof(MAC_TABLE_ENTRY));
+				pEntry->PairwiseKey.KeyLen = 0;
+				pEntry->PairwiseKey.CipherAlg = CIPHER_NONE;
+			}
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+			if (apidx >= MIN_NET_DEVICE_FOR_DLS)
+			{
+				pEntry->ValidAsCLI = FALSE;
+				pEntry->ValidAsWDS = FALSE;
+				pEntry->ValidAsApCli = FALSE;
+				pEntry->ValidAsMesh = FALSE;
+				pEntry->ValidAsDls = TRUE;
+				pEntry->isCached = FALSE;
+			}
+			else
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+			{
+
+#ifdef CONFIG_STA_SUPPORT
+				IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				{
+					pEntry->ValidAsCLI = TRUE;
+					pEntry->ValidAsWDS = FALSE;
+					pEntry->ValidAsApCli = FALSE;
+					pEntry->ValidAsMesh = FALSE;
+					pEntry->ValidAsDls = FALSE;
+				}
+#endif // CONFIG_STA_SUPPORT //
+			}
+
+			pEntry->bIAmBadAtheros = FALSE;
+			pEntry->pAd = pAd;
+			pEntry->CMTimerRunning = FALSE;
+			pEntry->EnqueueEapolStartTimerRunning = EAPOL_START_DISABLE;
+			pEntry->RSNIE_Len = 0;
+			NdisZeroMemory(pEntry->R_Counter, sizeof(pEntry->R_Counter));
+			pEntry->ReTryCounter = PEER_MSG1_RETRY_TIMER_CTR;
+
+			if (pEntry->ValidAsMesh)
+				pEntry->apidx = (apidx - MIN_NET_DEVICE_FOR_MESH);
+			else if (pEntry->ValidAsApCli)
+				pEntry->apidx = (apidx - MIN_NET_DEVICE_FOR_APCLI);
+			else if (pEntry->ValidAsWDS)
+				pEntry->apidx = (apidx - MIN_NET_DEVICE_FOR_WDS);
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+			else if (pEntry->ValidAsDls)
+				pEntry->apidx = (apidx - MIN_NET_DEVICE_FOR_DLS);
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+			else
+				pEntry->apidx = apidx;
+
+			{
+
+#ifdef CONFIG_STA_SUPPORT
+				IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				{
+					pEntry->AuthMode = pAd->StaCfg.AuthMode;
+					pEntry->WepStatus = pAd->StaCfg.WepStatus;
+					pEntry->PrivacyFilter = Ndis802_11PrivFilterAcceptAll;
+				}
+#endif // CONFIG_STA_SUPPORT //
+			}
+
+			pEntry->GTKState = REKEY_NEGOTIATING;
+			pEntry->PairwiseKey.KeyLen = 0;
+			pEntry->PairwiseKey.CipherAlg = CIPHER_NONE;
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+			if (pEntry->ValidAsDls == TRUE)
+				pEntry->PortSecured = WPA_802_1X_PORT_SECURED;
+#endif //QOS_DLS_SUPPORT
+#endif // CONFIG_STA_SUPPORT //
+			pEntry->PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+			pEntry->PMKID_CacheIdx = ENTRY_NOT_FOUND;
+			COPY_MAC_ADDR(pEntry->Addr, pAddr);
+			pEntry->Sst = SST_NOT_AUTH;
+			pEntry->AuthState = AS_NOT_AUTH;
+			pEntry->Aid = (USHORT)i;  //0;
+			pEntry->CapabilityInfo = 0;
+			pEntry->PsMode = PWR_ACTIVE;
+			pEntry->PsQIdleCount = 0;
+			pEntry->NoDataIdleCount = 0;
+			pEntry->ContinueTxFailCnt = 0;
+			InitializeQueueHeader(&pEntry->PsQueue);
+
+
+			pAd->MacTab.Size ++;
+			// Add this entry into ASIC RX WCID search table
+			RT28XX_STA_ENTRY_ADD(pAd, pEntry);
+
+			DBGPRINT(RT_DEBUG_TRACE, ("MacTableInsertEntry - allocate entry #%d, Total= %d\n",i, pAd->MacTab.Size));
+			break;
+		}
+	}
+
+	// add this MAC entry into HASH table
+	if (pEntry)
+	{
+		HashIdx = MAC_ADDR_HASH_INDEX(pAddr);
+		if (pAd->MacTab.Hash[HashIdx] == NULL)
+		{
+			pAd->MacTab.Hash[HashIdx] = pEntry;
+		}
+		else
+		{
+			pCurrEntry = pAd->MacTab.Hash[HashIdx];
+			while (pCurrEntry->pNext != NULL)
+				pCurrEntry = pCurrEntry->pNext;
+			pCurrEntry->pNext = pEntry;
+		}
+	}
+
+	NdisReleaseSpinLock(&pAd->MacTabLock);
+	return pEntry;
+}
+
+/*
+	==========================================================================
+	Description:
+		Delete a specified client from MAC table
+	==========================================================================
+ */
+BOOLEAN MacTableDeleteEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT wcid,
+	IN PUCHAR pAddr)
+{
+	USHORT HashIdx;
+	MAC_TABLE_ENTRY *pEntry, *pPrevEntry, *pProbeEntry;
+	BOOLEAN Cancelled;
+	//USHORT	offset;	// unused variable
+	//UCHAR	j;			// unused variable
+
+	if (wcid >= MAX_LEN_OF_MAC_TABLE)
+		return FALSE;
+
+	NdisAcquireSpinLock(&pAd->MacTabLock);
+
+	HashIdx = MAC_ADDR_HASH_INDEX(pAddr);
+	//pEntry = pAd->MacTab.Hash[HashIdx];
+	pEntry = &pAd->MacTab.Content[wcid];
+
+	if (pEntry && (pEntry->ValidAsCLI || pEntry->ValidAsApCli || pEntry->ValidAsWDS || pEntry->ValidAsMesh
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+ 		|| pEntry->ValidAsDls
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+		))
+	{
+		if (MAC_ADDR_EQUAL(pEntry->Addr, pAddr))
+		{
+
+			// Delete this entry from ASIC on-chip WCID Table
+			RT28XX_STA_ENTRY_MAC_RESET(pAd, wcid);
+
+#ifdef DOT11_N_SUPPORT
+			// free resources of BA
+			BASessionTearDownALL(pAd, pEntry->Aid);
+#endif // DOT11_N_SUPPORT //
+
+
+			pPrevEntry = NULL;
+			pProbeEntry = pAd->MacTab.Hash[HashIdx];
+			ASSERT(pProbeEntry);
+
+			// update Hash list
+			do
+			{
+				if (pProbeEntry == pEntry)
+				{
+					if (pPrevEntry == NULL)
+					{
+						pAd->MacTab.Hash[HashIdx] = pEntry->pNext;
+					}
+					else
+					{
+						pPrevEntry->pNext = pEntry->pNext;
+					}
+					break;
+				}
+
+				pPrevEntry = pProbeEntry;
+				pProbeEntry = pProbeEntry->pNext;
+			} while (pProbeEntry);
+
+			// not found !!!
+			ASSERT(pProbeEntry != NULL);
+
+			RT28XX_STA_ENTRY_KEY_DEL(pAd, BSS0, wcid);
+
+
+		if (pEntry->EnqueueEapolStartTimerRunning != EAPOL_START_DISABLE)
+		{
+			RTMPCancelTimer(&pEntry->EnqueueStartForPSKTimer, &Cancelled);
+			pEntry->EnqueueEapolStartTimerRunning = EAPOL_START_DISABLE;
+		}
+
+
+   			NdisZeroMemory(pEntry, sizeof(MAC_TABLE_ENTRY));
+			pAd->MacTab.Size --;
+			DBGPRINT(RT_DEBUG_TRACE, ("MacTableDeleteEntry1 - Total= %d\n", pAd->MacTab.Size));
+		}
+		else
+		{
+			printk("\n%s: Impossible Wcid = %d !!!!!\n", __FUNCTION__, wcid);
+		}
+	}
+
+	NdisReleaseSpinLock(&pAd->MacTabLock);
+
+	//Reset operating mode when no Sta.
+	if (pAd->MacTab.Size == 0)
+	{
+#ifdef DOT11_N_SUPPORT
+		pAd->CommonCfg.AddHTInfo.AddHtInfo2.OperaionMode = 0;
+#endif // DOT11_N_SUPPORT //
+		AsicUpdateProtect(pAd, 0 /*pAd->CommonCfg.AddHTInfo.AddHtInfo2.OperaionMode*/, (ALLN_SETPROTECT), TRUE, 0 /*pAd->MacTab.fAnyStationNonGF*/);
+	}
+
+	return TRUE;
+}
+
+
+/*
+	==========================================================================
+	Description:
+		This routine reset the entire MAC table. All packets pending in
+		the power-saving queues are freed here.
+	==========================================================================
+ */
+VOID MacTableReset(
+	IN  PRTMP_ADAPTER  pAd)
+{
+	int         i;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("MacTableReset\n"));
+	//NdisAcquireSpinLock(&pAd->MacTabLock);
+
+	for (i=1; i<MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		if (pAd->MacTab.Content[i].ValidAsCLI == TRUE)
+	   {
+
+#ifdef DOT11_N_SUPPORT
+			// free resources of BA
+			BASessionTearDownALL(pAd, i);
+#endif // DOT11_N_SUPPORT //
+
+			pAd->MacTab.Content[i].ValidAsCLI = FALSE;
+
+
+
+#ifdef RT2870
+			NdisZeroMemory(pAd->MacTab.Content[i].Addr, 6);
+			RT28XX_STA_ENTRY_MAC_RESET(pAd, i);
+#endif // RT2870 //
+
+			//AsicDelWcidTab(pAd, i);
+		}
+	}
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID AssocParmFill(
+	IN PRTMP_ADAPTER pAd,
+	IN OUT MLME_ASSOC_REQ_STRUCT *AssocReq,
+	IN PUCHAR                     pAddr,
+	IN USHORT                     CapabilityInfo,
+	IN ULONG                      Timeout,
+	IN USHORT                     ListenIntv)
+{
+	COPY_MAC_ADDR(AssocReq->Addr, pAddr);
+	// Add mask to support 802.11b mode only
+	AssocReq->CapabilityInfo = CapabilityInfo & SUPPORTED_CAPABILITY_INFO; // not cf-pollable, not cf-poll-request
+	AssocReq->Timeout = Timeout;
+	AssocReq->ListenIntv = ListenIntv;
+}
+
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID DisassocParmFill(
+	IN PRTMP_ADAPTER pAd,
+	IN OUT MLME_DISASSOC_REQ_STRUCT *DisassocReq,
+	IN PUCHAR pAddr,
+	IN USHORT Reason)
+{
+	COPY_MAC_ADDR(DisassocReq->Addr, pAddr);
+	DisassocReq->Reason = Reason;
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Check the out going frame, if this is an DHCP or ARP datagram
+	will be duplicate another frame at low data rate transmit.
+
+	Arguments:
+		pAd 		Pointer to our adapter
+		pPacket 	Pointer to outgoing Ndis frame
+
+	Return Value:
+		TRUE		To be duplicate at Low data rate transmit. (1mb)
+		FALSE		Do nothing.
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+		MAC header + IP Header + UDP Header
+		  14 Bytes	  20 Bytes
+
+		UDP Header
+		00|01|02|03|04|05|06|07|08|09|10|11|12|13|14|15|
+						Source Port
+		16|17|18|19|20|21|22|23|24|25|26|27|28|29|30|31|
+					Destination Port
+
+		port 0x43 means Bootstrap Protocol, server.
+		Port 0x44 means Bootstrap Protocol, client.
+
+	========================================================================
+*/
+
+BOOLEAN RTMPCheckDHCPFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket)
+{
+	PACKET_INFO 	PacketInfo;
+	ULONG			NumberOfBytesRead = 0;
+	ULONG			CurrentOffset = 0;
+	PVOID			pVirtualAddress = NULL;
+	UINT			NdisBufferLength;
+	PUCHAR			pSrc;
+	USHORT			Protocol;
+	UCHAR			ByteOffset36 = 0;
+	UCHAR			ByteOffset38 = 0;
+	BOOLEAN 		ReadFirstParm = TRUE;
+
+	RTMP_QueryPacketInfo(pPacket, &PacketInfo, (PUCHAR *)&pVirtualAddress, &NdisBufferLength);
+
+	NumberOfBytesRead += NdisBufferLength;
+	pSrc = (PUCHAR) pVirtualAddress;
+	Protocol = *(pSrc + 12) * 256 + *(pSrc + 13);
+
+	//
+	// Check DHCP & BOOTP protocol
+	//
+	while (NumberOfBytesRead <= PacketInfo.TotalPacketLength)
+	{
+		if ((NumberOfBytesRead >= 35) && (ReadFirstParm == TRUE))
+		{
+			CurrentOffset = 35 - (NumberOfBytesRead - NdisBufferLength);
+			ByteOffset36 = *(pSrc + CurrentOffset);
+			ReadFirstParm = FALSE;
+		}
+
+		if (NumberOfBytesRead >= 37)
+		{
+			CurrentOffset = 37 - (NumberOfBytesRead - NdisBufferLength);
+			ByteOffset38 = *(pSrc + CurrentOffset);
+			//End of Read
+			break;
+		}
+		return FALSE;
+	}
+
+	// Check for DHCP & BOOTP protocol
+	if ((ByteOffset36 != 0x44) || (ByteOffset38 != 0x43))
+		{
+		//
+		// 2054 (hex 0806) for ARP datagrams
+		// if this packet is not ARP datagrams, then do nothing
+		// ARP datagrams will also be duplicate at 1mb broadcast frames
+		//
+		if (Protocol != 0x0806 )
+			return FALSE;
+		}
+
+	return TRUE;
+}
+
+
+BOOLEAN RTMPCheckEtherType(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket)
+{
+	USHORT	TypeLen;
+	UCHAR	Byte0, Byte1;
+	PUCHAR	pSrcBuf;
+	UINT32	pktLen;
+	UINT16 	srcPort, dstPort;
+	BOOLEAN	status = TRUE;
+
+
+	pSrcBuf = GET_OS_PKT_DATAPTR(pPacket);
+	pktLen = GET_OS_PKT_LEN(pPacket);
+
+	ASSERT(pSrcBuf);
+
+	RTMP_SET_PACKET_SPECIFIC(pPacket, 0);
+
+	// get Ethernet protocol field
+	TypeLen = (pSrcBuf[12] << 8) + pSrcBuf[13];
+
+	pSrcBuf += LENGTH_802_3;	// Skip the Ethernet Header.
+
+	if (TypeLen <= 1500)
+	{	// 802.3, 802.3 LLC
+		/*
+			DestMAC(6) + SrcMAC(6) + Lenght(2) +
+			DSAP(1) + SSAP(1) + Control(1) +
+			if the DSAP = 0xAA, SSAP=0xAA, Contorl = 0x03, it has a 5-bytes SNAP header.
+				=> + SNAP (5, OriginationID(3) + etherType(2))
+		*/
+		if (pSrcBuf[0] == 0xAA && pSrcBuf[1] == 0xAA && pSrcBuf[2] == 0x03)
+		{
+			Sniff2BytesFromNdisBuffer(pSrcBuf, 6, &Byte0, &Byte1);
+			RTMP_SET_PACKET_LLCSNAP(pPacket, 1);
+			TypeLen = (USHORT)((Byte0 << 8) + Byte1);
+			pSrcBuf += 8; // Skip this LLC/SNAP header
+		}
+		else
+		{
+			//It just has 3-byte LLC header, maybe a legacy ether type frame. we didn't handle it.
+		}
+	}
+
+	// If it's a VLAN packet, get the real Type/Length field.
+	if (TypeLen == 0x8100)
+	{
+		/* 0x8100 means VLAN packets */
+
+		/* Dest. MAC Address (6-bytes) +
+		   Source MAC Address (6-bytes) +
+		   Length/Type = 802.1Q Tag Type (2-byte) +
+		   Tag Control Information (2-bytes) +
+		   Length / Type (2-bytes) +
+		   data payload (0-n bytes) +
+		   Pad (0-p bytes) +
+		   Frame Check Sequence (4-bytes) */
+
+		RTMP_SET_PACKET_VLAN(pPacket, 1);
+		Sniff2BytesFromNdisBuffer(pSrcBuf, 2, &Byte0, &Byte1);
+		TypeLen = (USHORT)((Byte0 << 8) + Byte1);
+
+		pSrcBuf += 4; // Skip the VLAN Header.
+	}
+
+	switch (TypeLen)
+	{
+		case 0x0800:
+			{
+				ASSERT((pktLen > 34));
+				if (*(pSrcBuf + 9) == 0x11)
+				{	// udp packet
+					ASSERT((pktLen > 34));	// 14 for ethernet header, 20 for IP header
+
+					pSrcBuf += 20;	// Skip the IP header
+					srcPort = OS_NTOHS(*((UINT16 *)pSrcBuf));
+					dstPort = OS_NTOHS(*((UINT16 *)(pSrcBuf +2)));
+
+					if ((srcPort==0x44 && dstPort==0x43) || (srcPort==0x43 && dstPort==0x44))
+					{	//It's a BOOTP/DHCP packet
+						RTMP_SET_PACKET_DHCP(pPacket, 1);
+					}
+				}
+			}
+			break;
+		case 0x0806:
+			{
+				//ARP Packet.
+				RTMP_SET_PACKET_DHCP(pPacket, 1);
+			}
+			break;
+		case 0x888e:
+			{
+				// EAPOL Packet.
+				RTMP_SET_PACKET_EAPOL(pPacket, 1);
+			}
+			break;
+		default:
+			status = FALSE;
+			break;
+	}
+
+	return status;
+
+}
+
+
+
+VOID Update_Rssi_Sample(
+	IN PRTMP_ADAPTER	pAd,
+	IN RSSI_SAMPLE		*pRssi,
+	IN PRXWI_STRUC		pRxWI)
+		{
+	CHAR	rssi0 = pRxWI->RSSI0;
+	CHAR	rssi1 = pRxWI->RSSI1;
+	CHAR	rssi2 = pRxWI->RSSI2;
+
+	if (rssi0 != 0)
+	{
+		pRssi->LastRssi0	= ConvertToRssi(pAd, (CHAR)rssi0, RSSI_0);
+		pRssi->AvgRssi0X8	= (pRssi->AvgRssi0X8 - pRssi->AvgRssi0) + pRssi->LastRssi0;
+		pRssi->AvgRssi0	= pRssi->AvgRssi0X8 >> 3;
+	}
+
+	if (rssi1 != 0)
+	{
+		pRssi->LastRssi1	= ConvertToRssi(pAd, (CHAR)rssi1, RSSI_1);
+		pRssi->AvgRssi1X8	= (pRssi->AvgRssi1X8 - pRssi->AvgRssi1) + pRssi->LastRssi1;
+		pRssi->AvgRssi1	= pRssi->AvgRssi1X8 >> 3;
+	}
+
+	if (rssi2 != 0)
+	{
+		pRssi->LastRssi2	= ConvertToRssi(pAd, (CHAR)rssi2, RSSI_2);
+		pRssi->AvgRssi2X8  = (pRssi->AvgRssi2X8 - pRssi->AvgRssi2) + pRssi->LastRssi2;
+		pRssi->AvgRssi2 = pRssi->AvgRssi2X8 >> 3;
+	}
+}
+
+
+
+// Normal legacy Rx packet indication
+VOID Indicate_Legacy_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+	PNDIS_PACKET	pRxPacket = pRxBlk->pRxPacket;
+	UCHAR			Header802_3[LENGTH_802_3];
+
+	// 1. get 802.3 Header
+	// 2. remove LLC
+	// 		a. pointer pRxBlk->pData to payload
+	//      b. modify pRxBlk->DataSize
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		RTMP_802_11_REMOVE_LLC_AND_CONVERT_TO_802_3(pRxBlk, Header802_3);
+#endif // CONFIG_STA_SUPPORT //
+
+	if (pRxBlk->DataSize > MAX_RX_PKT_LEN)
+	{
+#if 0 // sample take off, for multiple card design
+		static int err_size;
+
+		err_size++;
+		if (err_size > 20)
+		{
+			 printk("Legacy DataSize = %d\n", pRxBlk->DataSize);
+			 hex_dump("802.3 Header", Header802_3, LENGTH_802_3);
+			 hex_dump("Payload", pRxBlk->pData, 64);
+			 err_size = 0;
+		}
+#endif
+
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+
+
+	STATS_INC_RX_PACKETS(pAd, FromWhichBSSID);
+
+#ifdef RT2870
+#ifdef DOT11_N_SUPPORT
+	if (pAd->CommonCfg.bDisableReordering == 0)
+	{
+		PBA_REC_ENTRY		pBAEntry;
+		ULONG				Now32;
+		UCHAR				Wcid = pRxBlk->pRxWI->WirelessCliID;
+		UCHAR				TID = pRxBlk->pRxWI->TID;
+		USHORT				Idx;
+
+#define REORDERING_PACKET_TIMEOUT		((100 * HZ)/1000)	// system ticks -- 100 ms
+
+		if (Wcid < MAX_LEN_OF_MAC_TABLE)
+		{
+			Idx = pAd->MacTab.Content[Wcid].BARecWcidArray[TID];
+			if (Idx != 0)
+			{
+				pBAEntry = &pAd->BATable.BARecEntry[Idx];
+				// update last rx time
+				NdisGetSystemUpTime(&Now32);
+				if ((pBAEntry->list.qlen > 0) &&
+					 RTMP_TIME_AFTER((unsigned long)Now32, (unsigned long)(pBAEntry->LastIndSeqAtTimer+(REORDERING_PACKET_TIMEOUT)))
+	   				)
+				{
+					printk("Indicate_Legacy_Packet():flush reordering_timeout_mpdus! RxWI->Flags=%d, pRxWI.TID=%d, RxD->AMPDU=%d!\n", pRxBlk->Flags, pRxBlk->pRxWI->TID, pRxBlk->RxD.AMPDU);
+					hex_dump("Dump the legacy Packet:", GET_OS_PKT_DATAPTR(pRxBlk->pRxPacket), 64);
+					ba_flush_reordering_timeout_mpdus(pAd, pBAEntry, Now32);
+				}
+			}
+		}
+	}
+#endif // DOT11_N_SUPPORT //
+#endif // RT2870 //
+
+	wlan_802_11_to_802_3_packet(pAd, pRxBlk, Header802_3, FromWhichBSSID);
+
+	//
+	// pass this 802.3 packet to upper layer or forward this packet to WM directly
+	//
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		ANNOUNCE_OR_FORWARD_802_3_PACKET(pAd, pRxPacket, FromWhichBSSID);
+#endif // CONFIG_STA_SUPPORT //
+
+}
+
+
+// Normal, AMPDU or AMSDU
+VOID CmmRxnonRalinkFrameIndicate(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+#ifdef DOT11_N_SUPPORT
+	if (RX_BLK_TEST_FLAG(pRxBlk, fRX_AMPDU) && (pAd->CommonCfg.bDisableReordering == 0))
+	{
+		Indicate_AMPDU_Packet(pAd, pRxBlk, FromWhichBSSID);
+	}
+	else
+#endif // DOT11_N_SUPPORT //
+	{
+#ifdef DOT11_N_SUPPORT
+		if (RX_BLK_TEST_FLAG(pRxBlk, fRX_AMSDU))
+		{
+			// handle A-MSDU
+			Indicate_AMSDU_Packet(pAd, pRxBlk, FromWhichBSSID);
+		}
+		else
+#endif // DOT11_N_SUPPORT //
+		{
+			Indicate_Legacy_Packet(pAd, pRxBlk, FromWhichBSSID);
+		}
+	}
+}
+
+
+VOID CmmRxRalinkFrameIndicate(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+	UCHAR			Header802_3[LENGTH_802_3];
+	UINT16			Msdu2Size;
+	UINT16 			Payload1Size, Payload2Size;
+	PUCHAR 			pData2;
+	PNDIS_PACKET	pPacket2 = NULL;
+
+
+
+	Msdu2Size = *(pRxBlk->pData) + (*(pRxBlk->pData+1) << 8);
+
+	if ((Msdu2Size <= 1536) && (Msdu2Size < pRxBlk->DataSize))
+	{
+		/* skip two byte MSDU2 len */
+		pRxBlk->pData += 2;
+		pRxBlk->DataSize -= 2;
+	}
+	else
+	{
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+
+	// get 802.3 Header and  remove LLC
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		RTMP_802_11_REMOVE_LLC_AND_CONVERT_TO_802_3(pRxBlk, Header802_3);
+#endif // CONFIG_STA_SUPPORT //
+
+
+	ASSERT(pRxBlk->pRxPacket);
+
+	// Ralink Aggregation frame
+	pAd->RalinkCounters.OneSecRxAggregationCount ++;
+	Payload1Size = pRxBlk->DataSize - Msdu2Size;
+	Payload2Size = Msdu2Size - LENGTH_802_3;
+
+	pData2 = pRxBlk->pData + Payload1Size + LENGTH_802_3;
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		pPacket2 = duplicate_pkt(pAd, (pData2-LENGTH_802_3), LENGTH_802_3, pData2, Payload2Size, FromWhichBSSID);
+#endif // CONFIG_STA_SUPPORT //
+
+	if (!pPacket2)
+	{
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+
+	// update payload size of 1st packet
+	pRxBlk->DataSize = Payload1Size;
+	wlan_802_11_to_802_3_packet(pAd, pRxBlk, Header802_3, FromWhichBSSID);
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		ANNOUNCE_OR_FORWARD_802_3_PACKET(pAd, pRxBlk->pRxPacket, FromWhichBSSID);
+#endif // CONFIG_STA_SUPPORT //
+
+	if (pPacket2)
+	{
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			ANNOUNCE_OR_FORWARD_802_3_PACKET(pAd, pPacket2, FromWhichBSSID);
+#endif // CONFIG_STA_SUPPORT //
+	}
+}
+
+
+#define RESET_FRAGFRAME(_fragFrame) \
+	{								\
+		_fragFrame.RxSize = 0;		\
+		_fragFrame.Sequence = 0;	\
+		_fragFrame.LastFrag = 0;	\
+		_fragFrame.Flags = 0;		\
+	}
+
+
+PNDIS_PACKET RTMPDeFragmentDataFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk)
+{
+	PHEADER_802_11	pHeader = pRxBlk->pHeader;
+	PNDIS_PACKET	pRxPacket = pRxBlk->pRxPacket;
+	UCHAR			*pData = pRxBlk->pData;
+	USHORT			DataSize = pRxBlk->DataSize;
+	PNDIS_PACKET	pRetPacket = NULL;
+	UCHAR			*pFragBuffer = NULL;
+	BOOLEAN 		bReassDone = FALSE;
+	UCHAR			HeaderRoom = 0;
+
+
+	ASSERT(pHeader);
+
+	HeaderRoom = pData - (UCHAR *)pHeader;
+
+	// Re-assemble the fragmented packets
+	if (pHeader->Frag == 0)		// Frag. Number is 0 : First frag or only one pkt
+	{
+		// the first pkt of fragment, record it.
+		if (pHeader->FC.MoreFrag)
+		{
+			ASSERT(pAd->FragFrame.pFragPacket);
+			pFragBuffer = GET_OS_PKT_DATAPTR(pAd->FragFrame.pFragPacket);
+			pAd->FragFrame.RxSize   = DataSize + HeaderRoom;
+			NdisMoveMemory(pFragBuffer,	 pHeader, pAd->FragFrame.RxSize);
+			pAd->FragFrame.Sequence = pHeader->Sequence;
+			pAd->FragFrame.LastFrag = pHeader->Frag;	   // Should be 0
+			ASSERT(pAd->FragFrame.LastFrag == 0);
+			goto done;	// end of processing this frame
+		}
+	}
+	else	//Middle & End of fragment
+	{
+		if ((pHeader->Sequence != pAd->FragFrame.Sequence) ||
+			(pHeader->Frag != (pAd->FragFrame.LastFrag + 1)))
+		{
+			// Fragment is not the same sequence or out of fragment number order
+			// Reset Fragment control blk
+			RESET_FRAGFRAME(pAd->FragFrame);
+			DBGPRINT(RT_DEBUG_ERROR, ("Fragment is not the same sequence or out of fragment number order.\n"));
+			goto done; // give up this frame
+		}
+		else if ((pAd->FragFrame.RxSize + DataSize) > MAX_FRAME_SIZE)
+		{
+			// Fragment frame is too large, it exeeds the maximum frame size.
+			// Reset Fragment control blk
+			RESET_FRAGFRAME(pAd->FragFrame);
+			DBGPRINT(RT_DEBUG_ERROR, ("Fragment frame is too large, it exeeds the maximum frame size.\n"));
+			goto done; // give up this frame
+		}
+
+        //
+		// Broadcom AP(BCM94704AGR) will send out LLC in fragment's packet, LLC only can accpet at first fragment.
+		// In this case, we will dropt it.
+		//
+		if (NdisEqualMemory(pData, SNAP_802_1H, sizeof(SNAP_802_1H)))
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("Find another LLC at Middle or End fragment(SN=%d, Frag=%d)\n", pHeader->Sequence, pHeader->Frag));
+			goto done; // give up this frame
+		}
+
+		pFragBuffer = GET_OS_PKT_DATAPTR(pAd->FragFrame.pFragPacket);
+
+		// concatenate this fragment into the re-assembly buffer
+		NdisMoveMemory((pFragBuffer + pAd->FragFrame.RxSize), pData, DataSize);
+		pAd->FragFrame.RxSize  += DataSize;
+		pAd->FragFrame.LastFrag = pHeader->Frag;	   // Update fragment number
+
+		// Last fragment
+		if (pHeader->FC.MoreFrag == FALSE)
+		{
+			bReassDone = TRUE;
+		}
+	}
+
+done:
+	// always release rx fragmented packet
+	RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+
+	// return defragmented packet if packet is reassembled completely
+	// otherwise return NULL
+	if (bReassDone)
+	{
+		PNDIS_PACKET pNewFragPacket;
+
+		// allocate a new packet buffer for fragment
+		pNewFragPacket = RTMP_AllocateFragPacketBuffer(pAd, RX_BUFFER_NORMSIZE);
+		if (pNewFragPacket)
+		{
+			// update RxBlk
+			pRetPacket = pAd->FragFrame.pFragPacket;
+			pAd->FragFrame.pFragPacket = pNewFragPacket;
+			pRxBlk->pHeader = (PHEADER_802_11) GET_OS_PKT_DATAPTR(pRetPacket);
+			pRxBlk->pData = (UCHAR *)pRxBlk->pHeader + HeaderRoom;
+			pRxBlk->DataSize = pAd->FragFrame.RxSize - HeaderRoom;
+			pRxBlk->pRxPacket = pRetPacket;
+		}
+		else
+		{
+			RESET_FRAGFRAME(pAd->FragFrame);
+		}
+	}
+
+	return pRetPacket;
+}
+
+
+VOID Indicate_AMSDU_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+	UINT			nMSDU;
+
+	update_os_packet_info(pAd, pRxBlk, FromWhichBSSID);
+	RTMP_SET_PACKET_IF(pRxBlk->pRxPacket, FromWhichBSSID);
+	nMSDU = deaggregate_AMSDU_announce(pAd, pRxBlk->pRxPacket, pRxBlk->pData, pRxBlk->DataSize);
+}
+
+VOID Indicate_EAPOL_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+	MAC_TABLE_ENTRY *pEntry = NULL;
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		pEntry = &pAd->MacTab.Content[BSSID_WCID];
+		STARxEAPOLFrameIndicate(pAd, pEntry, pRxBlk, FromWhichBSSID);
+		return;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	if (pEntry == NULL)
+	{
+		DBGPRINT(RT_DEBUG_WARN, ("Indicate_EAPOL_Packet: drop and release the invalid packet.\n"));
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+}
+
+#define BCN_TBTT_OFFSET		64	//defer 64 us
+VOID ReSyncBeaconTime(
+	IN  PRTMP_ADAPTER   pAd)
+{
+
+	UINT32  Offset;
+
+
+	Offset = (pAd->TbttTickCount) % (BCN_TBTT_OFFSET);
+
+	pAd->TbttTickCount++;
+
+	//
+	// The updated BeaconInterval Value will affect Beacon Interval after two TBTT
+	// beacasue the original BeaconInterval had been loaded into next TBTT_TIMER
+	//
+	if (Offset == (BCN_TBTT_OFFSET-2))
+	{
+		BCN_TIME_CFG_STRUC csr;
+		RTMP_IO_READ32(pAd, BCN_TIME_CFG, &csr.word);
+		csr.field.BeaconInterval = (pAd->CommonCfg.BeaconPeriod << 4) - 1 ;	// ASIC register in units of 1/16 TU = 64us
+		RTMP_IO_WRITE32(pAd, BCN_TIME_CFG, csr.word);
+	}
+	else
+	{
+		if (Offset == (BCN_TBTT_OFFSET-1))
+		{
+			BCN_TIME_CFG_STRUC csr;
+
+			RTMP_IO_READ32(pAd, BCN_TIME_CFG, &csr.word);
+			csr.field.BeaconInterval = (pAd->CommonCfg.BeaconPeriod) << 4; // ASIC register in units of 1/16 TU
+			RTMP_IO_WRITE32(pAd, BCN_TIME_CFG, csr.word);
+		}
+	}
+}
+
diff --git a/drivers/staging/rt2870/common/cmm_data_2870.c b/drivers/staging/rt2870/common/cmm_data_2870.c
new file mode 100644
index 00000000000..f77000f336a
--- /dev/null
+++ b/drivers/staging/rt2870/common/cmm_data_2870.c
@@ -0,0 +1,963 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+*/
+/*
+   All functions in this file must be USB-depended, or you should out your function
+	in other files.
+
+*/
+#include "../rt_config.h"
+
+
+/*
+	We can do copy the frame into pTxContext when match following conditions.
+		=>
+		=>
+		=>
+*/
+static inline NDIS_STATUS RtmpUSBCanDoWrite(
+	IN RTMP_ADAPTER		*pAd,
+	IN UCHAR			QueIdx,
+	IN HT_TX_CONTEXT 	*pHTTXContext)
+{
+	NDIS_STATUS	canWrite = NDIS_STATUS_RESOURCES;
+
+	if (((pHTTXContext->CurWritePosition) < pHTTXContext->NextBulkOutPosition) && (pHTTXContext->CurWritePosition + LOCAL_TXBUF_SIZE) > pHTTXContext->NextBulkOutPosition)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("RtmpUSBCanDoWrite c1!\n"));
+		RTUSB_SET_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_NORMAL << QueIdx));
+	}
+	else if ((pHTTXContext->CurWritePosition == 8) && (pHTTXContext->NextBulkOutPosition < LOCAL_TXBUF_SIZE))
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("RtmpUSBCanDoWrite c2!\n"));
+		RTUSB_SET_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_NORMAL << QueIdx));
+	}
+	else if (pHTTXContext->bCurWriting == TRUE)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("RtmpUSBCanDoWrite c3!\n"));
+	}
+	else
+	{
+		canWrite = NDIS_STATUS_SUCCESS;
+	}
+
+
+	return canWrite;
+}
+
+
+USHORT RtmpUSB_WriteSubTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	BOOLEAN			bIsLast,
+	OUT	USHORT			*FreeNumber)
+{
+
+	// Dummy function. Should be removed in the future.
+	return 0;
+
+}
+
+USHORT	RtmpUSB_WriteFragTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	UCHAR			fragNum,
+	OUT	USHORT			*FreeNumber)
+{
+	HT_TX_CONTEXT	*pHTTXContext;
+	USHORT			hwHdrLen;	// The hwHdrLen consist of 802.11 header length plus the header padding length.
+	UINT32			fillOffset;
+	TXINFO_STRUC	*pTxInfo;
+	TXWI_STRUC		*pTxWI;
+	PUCHAR			pWirelessPacket = NULL;
+	UCHAR			QueIdx;
+	NDIS_STATUS		Status;
+	unsigned long	IrqFlags;
+	UINT32			USBDMApktLen = 0, DMAHdrLen, padding;
+	BOOLEAN			TxQLastRound = FALSE;
+
+	//
+	// get Tx Ring Resource & Dma Buffer address
+	//
+	QueIdx = pTxBlk->QueIdx;
+	pHTTXContext  = &pAd->TxContext[QueIdx];
+
+	RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+	pHTTXContext  = &pAd->TxContext[QueIdx];
+	fillOffset = pHTTXContext->CurWritePosition;
+
+	if(fragNum == 0)
+	{
+		// Check if we have enough space for this bulk-out batch.
+		Status = RtmpUSBCanDoWrite(pAd, QueIdx, pHTTXContext);
+		if (Status == NDIS_STATUS_SUCCESS)
+		{
+			pHTTXContext->bCurWriting = TRUE;
+
+			// Reserve space for 8 bytes padding.
+			if ((pHTTXContext->ENextBulkOutPosition == pHTTXContext->CurWritePosition))
+			{
+				pHTTXContext->ENextBulkOutPosition += 8;
+				pHTTXContext->CurWritePosition += 8;
+				fillOffset += 8;
+			}
+			pTxBlk->Priv = 0;
+			pHTTXContext->CurWriteRealPos = pHTTXContext->CurWritePosition;
+		}
+		else
+		{
+			RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+			RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
+			return(Status);
+		}
+	}
+	else
+	{
+		// For sub-sequent frames of this bulk-out batch. Just copy it to our bulk-out buffer.
+		Status = ((pHTTXContext->bCurWriting == TRUE) ? NDIS_STATUS_SUCCESS : NDIS_STATUS_FAILURE);
+		if (Status == NDIS_STATUS_SUCCESS)
+		{
+			fillOffset += pTxBlk->Priv;
+		}
+		else
+		{
+			RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+			RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
+			return(Status);
+		}
+	}
+
+	NdisZeroMemory((PUCHAR)(&pTxBlk->HeaderBuf[0]), TXINFO_SIZE);
+	pTxInfo = (PTXINFO_STRUC)(&pTxBlk->HeaderBuf[0]);
+	pTxWI= (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]);
+
+	pWirelessPacket = &pHTTXContext->TransferBuffer->field.WirelessPacket[fillOffset];
+
+	// copy TXWI + WLAN Header + LLC into DMA Header Buffer
+	//hwHdrLen = ROUND_UP(pTxBlk->MpduHeaderLen, 4);
+	hwHdrLen = pTxBlk->MpduHeaderLen + pTxBlk->HdrPadLen;
+
+	// Build our URB for USBD
+	DMAHdrLen = TXWI_SIZE + hwHdrLen;
+	USBDMApktLen = DMAHdrLen + pTxBlk->SrcBufLen;
+	padding = (4 - (USBDMApktLen % 4)) & 0x03;	// round up to 4 byte alignment
+	USBDMApktLen += padding;
+
+	pTxBlk->Priv += (TXINFO_SIZE + USBDMApktLen);
+
+	// For TxInfo, the length of USBDMApktLen = TXWI_SIZE + 802.11 header + payload
+	RTMPWriteTxInfo(pAd, pTxInfo, (USHORT)(USBDMApktLen), FALSE, FIFO_EDCA, FALSE /*NextValid*/,  FALSE);
+
+	if (fragNum == pTxBlk->TotalFragNum)
+	{
+		pTxInfo->USBDMATxburst = 0;
+		if ((pHTTXContext->CurWritePosition + pTxBlk->Priv + 3906)> MAX_TXBULK_LIMIT)
+		{
+			pTxInfo->SwUseLastRound = 1;
+			TxQLastRound = TRUE;
+		}
+	}
+	else
+	{
+		pTxInfo->USBDMATxburst = 1;
+	}
+
+	NdisMoveMemory(pWirelessPacket, pTxBlk->HeaderBuf, TXINFO_SIZE + TXWI_SIZE + hwHdrLen);
+#ifdef RT_BIG_ENDIAN
+	RTMPFrameEndianChange(pAd, (PUCHAR)(pWirelessPacket + TXINFO_SIZE + TXWI_SIZE), DIR_WRITE, FALSE);
+#endif // RT_BIG_ENDIAN //
+	pWirelessPacket += (TXINFO_SIZE + TXWI_SIZE + hwHdrLen);
+	pHTTXContext->CurWriteRealPos += (TXINFO_SIZE + TXWI_SIZE + hwHdrLen);
+
+	RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+	NdisMoveMemory(pWirelessPacket, pTxBlk->pSrcBufData, pTxBlk->SrcBufLen);
+
+	//	Zero the last padding.
+	pWirelessPacket += pTxBlk->SrcBufLen;
+	NdisZeroMemory(pWirelessPacket, padding + 8);
+
+	if (fragNum == pTxBlk->TotalFragNum)
+	{
+		RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+		// Update the pHTTXContext->CurWritePosition. 3906 used to prevent the NextBulkOut is a A-RALINK/A-MSDU Frame.
+		pHTTXContext->CurWritePosition += pTxBlk->Priv;
+		if (TxQLastRound == TRUE)
+			pHTTXContext->CurWritePosition = 8;
+		pHTTXContext->CurWriteRealPos = pHTTXContext->CurWritePosition;
+
+
+		// Finally, set bCurWriting as FALSE
+	pHTTXContext->bCurWriting = FALSE;
+
+		RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+		// succeed and release the skb buffer
+		RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_SUCCESS);
+	}
+
+
+	return(Status);
+
+}
+
+
+USHORT RtmpUSB_WriteSingleTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	BOOLEAN			bIsLast,
+	OUT	USHORT			*FreeNumber)
+{
+	HT_TX_CONTEXT	*pHTTXContext;
+	USHORT			hwHdrLen;
+	UINT32			fillOffset;
+	TXINFO_STRUC	*pTxInfo;
+	TXWI_STRUC		*pTxWI;
+	PUCHAR			pWirelessPacket;
+	UCHAR			QueIdx;
+	unsigned long	IrqFlags;
+	NDIS_STATUS		Status;
+	UINT32			USBDMApktLen = 0, DMAHdrLen, padding;
+	BOOLEAN			bTxQLastRound = FALSE;
+
+	// For USB, didn't need PCI_MAP_SINGLE()
+	//SrcBufPA = PCI_MAP_SINGLE(pAd, (char *) pTxBlk->pSrcBufData, pTxBlk->SrcBufLen, PCI_DMA_TODEVICE);
+
+
+	//
+	// get Tx Ring Resource & Dma Buffer address
+	//
+	QueIdx = pTxBlk->QueIdx;
+
+	RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+	pHTTXContext  = &pAd->TxContext[QueIdx];
+	fillOffset = pHTTXContext->CurWritePosition;
+
+
+
+	// Check ring full.
+	Status = RtmpUSBCanDoWrite(pAd, QueIdx, pHTTXContext);
+	if(Status == NDIS_STATUS_SUCCESS)
+	{
+		pHTTXContext->bCurWriting = TRUE;
+
+		pTxInfo = (PTXINFO_STRUC)(&pTxBlk->HeaderBuf[0]);
+		pTxWI= (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]);
+
+		// Reserve space for 8 bytes padding.
+		if ((pHTTXContext->ENextBulkOutPosition == pHTTXContext->CurWritePosition))
+		{
+			pHTTXContext->ENextBulkOutPosition += 8;
+			pHTTXContext->CurWritePosition += 8;
+			fillOffset += 8;
+		}
+		pHTTXContext->CurWriteRealPos = pHTTXContext->CurWritePosition;
+
+		pWirelessPacket = &pHTTXContext->TransferBuffer->field.WirelessPacket[fillOffset];
+
+		// copy TXWI + WLAN Header + LLC into DMA Header Buffer
+		//hwHdrLen = ROUND_UP(pTxBlk->MpduHeaderLen, 4);
+		hwHdrLen = pTxBlk->MpduHeaderLen + pTxBlk->HdrPadLen;
+
+		// Build our URB for USBD
+		DMAHdrLen = TXWI_SIZE + hwHdrLen;
+		USBDMApktLen = DMAHdrLen + pTxBlk->SrcBufLen;
+		padding = (4 - (USBDMApktLen % 4)) & 0x03;	// round up to 4 byte alignment
+		USBDMApktLen += padding;
+
+		pTxBlk->Priv = (TXINFO_SIZE + USBDMApktLen);
+
+		// For TxInfo, the length of USBDMApktLen = TXWI_SIZE + 802.11 header + payload
+		RTMPWriteTxInfo(pAd, pTxInfo, (USHORT)(USBDMApktLen), FALSE, FIFO_EDCA, FALSE /*NextValid*/,  FALSE);
+
+		if ((pHTTXContext->CurWritePosition + 3906 + pTxBlk->Priv) > MAX_TXBULK_LIMIT)
+		{
+			pTxInfo->SwUseLastRound = 1;
+			bTxQLastRound = TRUE;
+		}
+		NdisMoveMemory(pWirelessPacket, pTxBlk->HeaderBuf, TXINFO_SIZE + TXWI_SIZE + hwHdrLen);
+#ifdef RT_BIG_ENDIAN
+		RTMPFrameEndianChange(pAd, (PUCHAR)(pWirelessPacket + TXINFO_SIZE + TXWI_SIZE), DIR_WRITE, FALSE);
+#endif // RT_BIG_ENDIAN //
+		pWirelessPacket += (TXINFO_SIZE + TXWI_SIZE + hwHdrLen);
+
+		// We unlock it here to prevent the first 8 bytes maybe over-writed issue.
+		//	1. First we got CurWritePosition but the first 8 bytes still not write to the pTxcontext.
+		//	2. An interrupt break our routine and handle bulk-out complete.
+		//	3. In the bulk-out compllete, it need to do another bulk-out,
+		//			if the ENextBulkOutPosition is just the same as CurWritePosition, it will save the first 8 bytes from CurWritePosition,
+		//			but the payload still not copyed. the pTxContext->SavedPad[] will save as allzero. and set the bCopyPad = TRUE.
+		//	4. Interrupt complete.
+		//  5. Our interrupted routine go back and fill the first 8 bytes to pTxContext.
+		//	6. Next time when do bulk-out, it found the bCopyPad==TRUE and will copy the SavedPad[] to pTxContext->NextBulkOutPosition.
+		//		and the packet will wrong.
+		pHTTXContext->CurWriteRealPos += (TXINFO_SIZE + TXWI_SIZE + hwHdrLen);
+		RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+		NdisMoveMemory(pWirelessPacket, pTxBlk->pSrcBufData, pTxBlk->SrcBufLen);
+		pWirelessPacket += pTxBlk->SrcBufLen;
+		NdisZeroMemory(pWirelessPacket, padding + 8);
+
+		RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+		pHTTXContext->CurWritePosition += pTxBlk->Priv;
+		if (bTxQLastRound)
+			pHTTXContext->CurWritePosition = 8;
+		pHTTXContext->CurWriteRealPos = pHTTXContext->CurWritePosition;
+
+	pHTTXContext->bCurWriting = FALSE;
+	}
+
+
+	RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+
+	// succeed and release the skb buffer
+	RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_SUCCESS);
+
+	return(Status);
+
+}
+
+
+USHORT RtmpUSB_WriteMultiTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	UCHAR			frameNum,
+	OUT	USHORT			*FreeNumber)
+{
+	HT_TX_CONTEXT	*pHTTXContext;
+	USHORT			hwHdrLen;	// The hwHdrLen consist of 802.11 header length plus the header padding length.
+	UINT32			fillOffset;
+	TXINFO_STRUC	*pTxInfo;
+	TXWI_STRUC		*pTxWI;
+	PUCHAR			pWirelessPacket = NULL;
+	UCHAR			QueIdx;
+	NDIS_STATUS		Status;
+	unsigned long	IrqFlags;
+	//UINT32			USBDMApktLen = 0, DMAHdrLen, padding;
+
+	//
+	// get Tx Ring Resource & Dma Buffer address
+	//
+	QueIdx = pTxBlk->QueIdx;
+	pHTTXContext  = &pAd->TxContext[QueIdx];
+
+	RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+	if(frameNum == 0)
+	{
+		// Check if we have enough space for this bulk-out batch.
+		Status = RtmpUSBCanDoWrite(pAd, QueIdx, pHTTXContext);
+		if (Status == NDIS_STATUS_SUCCESS)
+		{
+			pHTTXContext->bCurWriting = TRUE;
+
+			pTxInfo = (PTXINFO_STRUC)(&pTxBlk->HeaderBuf[0]);
+			pTxWI= (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]);
+
+
+			// Reserve space for 8 bytes padding.
+			if ((pHTTXContext->ENextBulkOutPosition == pHTTXContext->CurWritePosition))
+			{
+
+				pHTTXContext->CurWritePosition += 8;
+				pHTTXContext->ENextBulkOutPosition += 8;
+			}
+			fillOffset = pHTTXContext->CurWritePosition;
+			pHTTXContext->CurWriteRealPos = pHTTXContext->CurWritePosition;
+
+			pWirelessPacket = &pHTTXContext->TransferBuffer->field.WirelessPacket[fillOffset];
+
+			//
+			// Copy TXINFO + TXWI + WLAN Header + LLC into DMA Header Buffer
+			//
+			if (pTxBlk->TxFrameType == TX_AMSDU_FRAME)
+				//hwHdrLen = ROUND_UP(pTxBlk->MpduHeaderLen-LENGTH_AMSDU_SUBFRAMEHEAD, 4)+LENGTH_AMSDU_SUBFRAMEHEAD;
+				hwHdrLen = pTxBlk->MpduHeaderLen-LENGTH_AMSDU_SUBFRAMEHEAD + pTxBlk->HdrPadLen + LENGTH_AMSDU_SUBFRAMEHEAD;
+			else if (pTxBlk->TxFrameType == TX_RALINK_FRAME)
+				//hwHdrLen = ROUND_UP(pTxBlk->MpduHeaderLen-LENGTH_ARALINK_HEADER_FIELD, 4)+LENGTH_ARALINK_HEADER_FIELD;
+				hwHdrLen = pTxBlk->MpduHeaderLen-LENGTH_ARALINK_HEADER_FIELD + pTxBlk->HdrPadLen + LENGTH_ARALINK_HEADER_FIELD;
+			else
+				//hwHdrLen = ROUND_UP(pTxBlk->MpduHeaderLen, 4);
+				hwHdrLen = pTxBlk->MpduHeaderLen + pTxBlk->HdrPadLen;
+
+			// Update the pTxBlk->Priv.
+			pTxBlk->Priv = TXINFO_SIZE + TXWI_SIZE + hwHdrLen;
+
+			//	pTxInfo->USBDMApktLen now just a temp value and will to correct latter.
+			RTMPWriteTxInfo(pAd, pTxInfo, (USHORT)(pTxBlk->Priv), FALSE, FIFO_EDCA, FALSE /*NextValid*/,  FALSE);
+
+			// Copy it.
+			NdisMoveMemory(pWirelessPacket, pTxBlk->HeaderBuf, pTxBlk->Priv);
+#ifdef RT_BIG_ENDIAN
+			RTMPFrameEndianChange(pAd, (PUCHAR)(pWirelessPacket+ TXINFO_SIZE + TXWI_SIZE), DIR_WRITE, FALSE);
+#endif // RT_BIG_ENDIAN //
+			pHTTXContext->CurWriteRealPos += pTxBlk->Priv;
+			pWirelessPacket += pTxBlk->Priv;
+		}
+	}
+	else
+	{	// For sub-sequent frames of this bulk-out batch. Just copy it to our bulk-out buffer.
+
+		Status = ((pHTTXContext->bCurWriting == TRUE) ? NDIS_STATUS_SUCCESS : NDIS_STATUS_FAILURE);
+		if (Status == NDIS_STATUS_SUCCESS)
+		{
+			fillOffset =  (pHTTXContext->CurWritePosition + pTxBlk->Priv);
+			pWirelessPacket = &pHTTXContext->TransferBuffer->field.WirelessPacket[fillOffset];
+
+			//hwHdrLen = pTxBlk->MpduHeaderLen;
+			NdisMoveMemory(pWirelessPacket, pTxBlk->HeaderBuf, pTxBlk->MpduHeaderLen);
+			pWirelessPacket += (pTxBlk->MpduHeaderLen);
+			pTxBlk->Priv += pTxBlk->MpduHeaderLen;
+		}
+		else
+		{	// It should not happened now unless we are going to shutdown.
+			DBGPRINT(RT_DEBUG_ERROR, ("WriteMultiTxResource():bCurWriting is FALSE when handle sub-sequent frames.\n"));
+			Status = NDIS_STATUS_FAILURE;
+		}
+	}
+
+
+	// We unlock it here to prevent the first 8 bytes maybe over-write issue.
+	//	1. First we got CurWritePosition but the first 8 bytes still not write to the pTxContext.
+	//	2. An interrupt break our routine and handle bulk-out complete.
+	//	3. In the bulk-out compllete, it need to do another bulk-out,
+	//			if the ENextBulkOutPosition is just the same as CurWritePosition, it will save the first 8 bytes from CurWritePosition,
+	//			but the payload still not copyed. the pTxContext->SavedPad[] will save as allzero. and set the bCopyPad = TRUE.
+	//	4. Interrupt complete.
+	//  5. Our interrupted routine go back and fill the first 8 bytes to pTxContext.
+	//	6. Next time when do bulk-out, it found the bCopyPad==TRUE and will copy the SavedPad[] to pTxContext->NextBulkOutPosition.
+	//		and the packet will wrong.
+	RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+	if (Status != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("WriteMultiTxResource: CWPos = %ld, NBOutPos = %ld.\n", pHTTXContext->CurWritePosition, pHTTXContext->NextBulkOutPosition));
+		goto done;
+	}
+
+	// Copy the frame content into DMA buffer and update the pTxBlk->Priv
+	NdisMoveMemory(pWirelessPacket, pTxBlk->pSrcBufData, pTxBlk->SrcBufLen);
+	pWirelessPacket += pTxBlk->SrcBufLen;
+	pTxBlk->Priv += pTxBlk->SrcBufLen;
+
+done:
+	// Release the skb buffer here
+	RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_SUCCESS);
+
+	return(Status);
+
+}
+
+
+VOID RtmpUSB_FinalWriteTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	USHORT			totalMPDUSize,
+	IN	USHORT			TxIdx)
+{
+	UCHAR			QueIdx;
+	HT_TX_CONTEXT	*pHTTXContext;
+	UINT32			fillOffset;
+	TXINFO_STRUC	*pTxInfo;
+	TXWI_STRUC		*pTxWI;
+	UINT32			USBDMApktLen, padding;
+	unsigned long	IrqFlags;
+	PUCHAR			pWirelessPacket;
+
+	QueIdx = pTxBlk->QueIdx;
+	pHTTXContext  = &pAd->TxContext[QueIdx];
+
+	RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+	if (pHTTXContext->bCurWriting == TRUE)
+	{
+		fillOffset = pHTTXContext->CurWritePosition;
+		if (((pHTTXContext->ENextBulkOutPosition == pHTTXContext->CurWritePosition) || ((pHTTXContext->ENextBulkOutPosition-8) == pHTTXContext->CurWritePosition))
+			&& (pHTTXContext->bCopySavePad == TRUE))
+			pWirelessPacket = (PUCHAR)(&pHTTXContext->SavedPad[0]);
+		else
+			pWirelessPacket = (PUCHAR)(&pHTTXContext->TransferBuffer->field.WirelessPacket[fillOffset]);
+
+		//
+		// Update TxInfo->USBDMApktLen ,
+		//		the length = TXWI_SIZE + 802.11_hdr + 802.11_hdr_pad + payload_of_all_batch_frames + Bulk-Out-padding
+		//
+		pTxInfo = (PTXINFO_STRUC)(pWirelessPacket);
+
+		// Calculate the bulk-out padding
+		USBDMApktLen = pTxBlk->Priv - TXINFO_SIZE;
+		padding = (4 - (USBDMApktLen % 4)) & 0x03;	// round up to 4 byte alignment
+		USBDMApktLen += padding;
+
+		pTxInfo->USBDMATxPktLen = USBDMApktLen;
+
+		//
+		// Update TXWI->MPDUtotalByteCount ,
+		//		the length = 802.11 header + payload_of_all_batch_frames
+		pTxWI= (PTXWI_STRUC)(pWirelessPacket + TXINFO_SIZE);
+		pTxWI->MPDUtotalByteCount = totalMPDUSize;
+
+		//
+		// Update the pHTTXContext->CurWritePosition
+		//
+		pHTTXContext->CurWritePosition += (TXINFO_SIZE + USBDMApktLen);
+		if ((pHTTXContext->CurWritePosition + 3906)> MAX_TXBULK_LIMIT)
+		{	// Add 3906 for prevent the NextBulkOut packet size is a A-RALINK/A-MSDU Frame.
+			pHTTXContext->CurWritePosition = 8;
+			pTxInfo->SwUseLastRound = 1;
+		}
+		pHTTXContext->CurWriteRealPos = pHTTXContext->CurWritePosition;
+
+
+		//
+		//	Zero the last padding.
+		//
+		pWirelessPacket = (&pHTTXContext->TransferBuffer->field.WirelessPacket[fillOffset + pTxBlk->Priv]);
+		NdisZeroMemory(pWirelessPacket, padding + 8);
+
+		// Finally, set bCurWriting as FALSE
+		pHTTXContext->bCurWriting = FALSE;
+
+	}
+	else
+	{	// It should not happened now unless we are going to shutdown.
+		DBGPRINT(RT_DEBUG_ERROR, ("FinalWriteTxResource():bCurWriting is FALSE when handle last frames.\n"));
+	}
+
+	RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+
+}
+
+
+VOID RtmpUSBDataLastTxIdx(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			QueIdx,
+	IN	USHORT			TxIdx)
+{
+	// DO nothing for USB.
+}
+
+
+/*
+	When can do bulk-out:
+		1. TxSwFreeIdx < TX_RING_SIZE;
+			It means has at least one Ring entity is ready for bulk-out, kick it out.
+		2. If TxSwFreeIdx == TX_RING_SIZE
+			Check if the CurWriting flag is FALSE, if it's FALSE, we can do kick out.
+
+*/
+VOID RtmpUSBDataKickOut(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	UCHAR			QueIdx)
+{
+	RTUSB_SET_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_NORMAL << QueIdx));
+	RTUSBKickBulkOut(pAd);
+
+}
+
+
+/*
+	Must be run in Interrupt context
+	This function handle RT2870 specific TxDesc and cpu index update and kick the packet out.
+ */
+int RtmpUSBMgmtKickOut(
+	IN RTMP_ADAPTER 	*pAd,
+	IN UCHAR 			QueIdx,
+	IN PNDIS_PACKET		pPacket,
+	IN PUCHAR			pSrcBufVA,
+	IN UINT 			SrcBufLen)
+{
+	PTXINFO_STRUC	pTxInfo;
+	ULONG			BulkOutSize;
+	UCHAR			padLen;
+	PUCHAR			pDest;
+	ULONG			SwIdx = pAd->MgmtRing.TxCpuIdx;
+	PTX_CONTEXT		pMLMEContext = (PTX_CONTEXT)pAd->MgmtRing.Cell[SwIdx].AllocVa;
+	unsigned long	IrqFlags;
+
+
+	pTxInfo = (PTXINFO_STRUC)(pSrcBufVA);
+
+	// Build our URB for USBD
+	BulkOutSize = SrcBufLen;
+	BulkOutSize = (BulkOutSize + 3) & (~3);
+	RTMPWriteTxInfo(pAd, pTxInfo, (USHORT)(BulkOutSize - TXINFO_SIZE), TRUE, EpToQueue[MGMTPIPEIDX], FALSE,  FALSE);
+
+	BulkOutSize += 4; // Always add 4 extra bytes at every packet.
+
+	// If BulkOutSize is multiple of BulkOutMaxPacketSize, add extra 4 bytes again.
+	if ((BulkOutSize % pAd->BulkOutMaxPacketSize) == 0)
+		BulkOutSize += 4;
+
+	padLen = BulkOutSize - SrcBufLen;
+	ASSERT((padLen <= RTMP_PKT_TAIL_PADDING));
+
+	// Now memzero all extra padding bytes.
+	pDest = (PUCHAR)(pSrcBufVA + SrcBufLen);
+	skb_put(GET_OS_PKT_TYPE(pPacket), padLen);
+	NdisZeroMemory(pDest, padLen);
+
+	RTMP_IRQ_LOCK(&pAd->MLMEBulkOutLock, IrqFlags);
+
+	pAd->MgmtRing.Cell[pAd->MgmtRing.TxCpuIdx].pNdisPacket = pPacket;
+	pMLMEContext->TransferBuffer = (PTX_BUFFER)(GET_OS_PKT_DATAPTR(pPacket));
+
+	// Length in TxInfo should be 8 less than bulkout size.
+	pMLMEContext->BulkOutSize = BulkOutSize;
+	pMLMEContext->InUse = TRUE;
+	pMLMEContext->bWaitingBulkOut = TRUE;
+
+
+	//for debug
+	//hex_dump("RtmpUSBMgmtKickOut", &pMLMEContext->TransferBuffer->field.WirelessPacket[0], (pMLMEContext->BulkOutSize > 16 ? 16 : pMLMEContext->BulkOutSize));
+
+	//pAd->RalinkCounters.KickTxCount++;
+	//pAd->RalinkCounters.OneSecTxDoneCount++;
+
+	//if (pAd->MgmtRing.TxSwFreeIdx == MGMT_RING_SIZE)
+	//	needKickOut = TRUE;
+
+	// Decrease the TxSwFreeIdx and Increase the TX_CTX_IDX
+	pAd->MgmtRing.TxSwFreeIdx--;
+	INC_RING_INDEX(pAd->MgmtRing.TxCpuIdx, MGMT_RING_SIZE);
+
+	RTMP_IRQ_UNLOCK(&pAd->MLMEBulkOutLock, IrqFlags);
+
+	RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_MLME);
+	//if (needKickOut)
+	RTUSBKickBulkOut(pAd);
+
+	return 0;
+}
+
+
+VOID RtmpUSBNullFrameKickOut(
+	IN RTMP_ADAPTER *pAd,
+	IN UCHAR		QueIdx,
+	IN UCHAR		*pNullFrame,
+	IN UINT32		frameLen)
+{
+	if (pAd->NullContext.InUse == FALSE)
+	{
+		PTX_CONTEXT		pNullContext;
+		PTXINFO_STRUC	pTxInfo;
+		PTXWI_STRUC		pTxWI;
+		PUCHAR			pWirelessPkt;
+
+		pNullContext = &(pAd->NullContext);
+
+		// Set the in use bit
+		pNullContext->InUse = TRUE;
+		pWirelessPkt = (PUCHAR)&pNullContext->TransferBuffer->field.WirelessPacket[0];
+
+		RTMPZeroMemory(&pWirelessPkt[0], 100);
+		pTxInfo = (PTXINFO_STRUC)&pWirelessPkt[0];
+		RTMPWriteTxInfo(pAd, pTxInfo, (USHORT)(sizeof(HEADER_802_11)+TXWI_SIZE), TRUE, EpToQueue[MGMTPIPEIDX], FALSE,  FALSE);
+		pTxInfo->QSEL = FIFO_EDCA;
+		pTxWI = (PTXWI_STRUC)&pWirelessPkt[TXINFO_SIZE];
+		RTMPWriteTxWI(pAd, pTxWI,  FALSE, FALSE, FALSE, FALSE, TRUE, FALSE, 0, BSSID_WCID, (sizeof(HEADER_802_11)),
+			0, 0, (UCHAR)pAd->CommonCfg.MlmeTransmit.field.MCS, IFS_HTTXOP, FALSE, &pAd->CommonCfg.MlmeTransmit);
+#ifdef RT_BIG_ENDIAN
+		RTMPWIEndianChange((PUCHAR)pTxWI, TYPE_TXWI);
+#endif // RT_BIG_ENDIAN //
+
+		RTMPMoveMemory(&pWirelessPkt[TXWI_SIZE+TXINFO_SIZE], &pAd->NullFrame, sizeof(HEADER_802_11));
+#ifdef RT_BIG_ENDIAN
+		RTMPFrameEndianChange(pAd, (PUCHAR)&pWirelessPkt[TXINFO_SIZE + TXWI_SIZE], DIR_WRITE, FALSE);
+#endif // RT_BIG_ENDIAN //
+		pAd->NullContext.BulkOutSize =  TXINFO_SIZE + TXWI_SIZE + sizeof(pAd->NullFrame) + 4;
+
+		// Fill out frame length information for global Bulk out arbitor
+		//pNullContext->BulkOutSize = TransferBufferLength;
+		DBGPRINT(RT_DEBUG_TRACE, ("SYNC - send NULL Frame @%d Mbps...\n", RateIdToMbps[pAd->CommonCfg.TxRate]));
+		RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NULL);
+
+		// Kick bulk out
+		RTUSBKickBulkOut(pAd);
+	}
+
+}
+
+#ifdef CONFIG_STA_SUPPORT
+/*
+	========================================================================
+
+	Routine	Description:
+		Check Rx descriptor, return NDIS_STATUS_FAILURE if any error dound
+
+	Arguments:
+		pRxD		Pointer	to the Rx descriptor
+
+	Return Value:
+		NDIS_STATUS_SUCCESS		No err
+		NDIS_STATUS_FAILURE		Error
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS	RTMPCheckRxError(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PHEADER_802_11	pHeader,
+	IN	PRXWI_STRUC	pRxWI,
+	IN	PRT28XX_RXD_STRUC	pRxINFO)
+{
+	PCIPHER_KEY pWpaKey;
+	INT	dBm;
+
+	if (pAd->bPromiscuous == TRUE)
+		return(NDIS_STATUS_SUCCESS);
+	if(pRxINFO == NULL)
+		return(NDIS_STATUS_FAILURE);
+
+	// Phy errors & CRC errors
+	if (pRxINFO->Crc)
+	{
+		// Check RSSI for Noise Hist statistic collection.
+		dBm = (INT) (pRxWI->RSSI0) - pAd->BbpRssiToDbmDelta;
+		if (dBm <= -87)
+			pAd->StaCfg.RPIDensity[0] += 1;
+		else if (dBm <= -82)
+			pAd->StaCfg.RPIDensity[1] += 1;
+		else if (dBm <= -77)
+			pAd->StaCfg.RPIDensity[2] += 1;
+		else if (dBm <= -72)
+			pAd->StaCfg.RPIDensity[3] += 1;
+		else if (dBm <= -67)
+			pAd->StaCfg.RPIDensity[4] += 1;
+		else if (dBm <= -62)
+			pAd->StaCfg.RPIDensity[5] += 1;
+		else if (dBm <= -57)
+			pAd->StaCfg.RPIDensity[6] += 1;
+		else if (dBm > -57)
+			pAd->StaCfg.RPIDensity[7] += 1;
+
+		return(NDIS_STATUS_FAILURE);
+	}
+
+	// Add Rx size to channel load counter, we should ignore error counts
+	pAd->StaCfg.CLBusyBytes += (pRxWI->MPDUtotalByteCount+ 14);
+
+	// Drop ToDs promiscous frame, it is opened due to CCX 2 channel load statistics
+	if (pHeader->FC.ToDs)
+	{
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("Err;FC.ToDs\n"));
+		return NDIS_STATUS_FAILURE;
+	}
+
+	// Paul 04-03 for OFDM Rx length issue
+	if (pRxWI->MPDUtotalByteCount > MAX_AGGREGATION_SIZE)
+	{
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("received packet too long\n"));
+		return NDIS_STATUS_FAILURE;
+	}
+
+	// Drop not U2M frames, cant's drop here because we will drop beacon in this case
+	// I am kind of doubting the U2M bit operation
+	// if (pRxD->U2M == 0)
+	//	return(NDIS_STATUS_FAILURE);
+
+	// drop decyption fail frame
+	if (pRxINFO->Decrypted && pRxINFO->CipherErr)
+	{
+
+		//
+		// MIC Error
+		//
+		if ((pRxINFO->CipherErr == 2) && pRxINFO->MyBss)
+		{
+			pWpaKey = &pAd->SharedKey[BSS0][pRxWI->KeyIndex];
+			RTMPReportMicError(pAd, pWpaKey);
+			DBGPRINT_RAW(RT_DEBUG_ERROR,("Rx MIC Value error\n"));
+		}
+
+		if (pRxINFO->Decrypted &&
+			(pAd->SharedKey[BSS0][pRxWI->KeyIndex].CipherAlg == CIPHER_AES) &&
+			(pHeader->Sequence == pAd->FragFrame.Sequence))
+		{
+			//
+			// Acceptable since the First FragFrame no CipherErr problem.
+			//
+			return(NDIS_STATUS_SUCCESS);
+		}
+
+		return(NDIS_STATUS_FAILURE);
+	}
+
+	return(NDIS_STATUS_SUCCESS);
+}
+
+VOID RT28xxUsbStaAsicForceWakeup(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN       bFromTx)
+{
+    AUTO_WAKEUP_STRUC	AutoWakeupCfg;
+
+	AutoWakeupCfg.word = 0;
+	RTMP_IO_WRITE32(pAd, AUTO_WAKEUP_CFG, AutoWakeupCfg.word);
+
+	AsicSendCommandToMcu(pAd, 0x31, 0xff, 0x00, 0x00);
+
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_DOZE);
+}
+
+VOID RT28xxUsbStaAsicSleepThenAutoWakeup(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT TbttNumToNextWakeUp)
+{
+	AUTO_WAKEUP_STRUC	AutoWakeupCfg;
+
+	// we have decided to SLEEP, so at least do it for a BEACON period.
+	if (TbttNumToNextWakeUp == 0)
+		TbttNumToNextWakeUp = 1;
+
+	AutoWakeupCfg.word = 0;
+	RTMP_IO_WRITE32(pAd, AUTO_WAKEUP_CFG, AutoWakeupCfg.word);
+
+	AutoWakeupCfg.field.NumofSleepingTbtt = TbttNumToNextWakeUp - 1;
+	AutoWakeupCfg.field.EnableAutoWakeup = 1;
+	AutoWakeupCfg.field.AutoLeadTime = 5;
+	RTMP_IO_WRITE32(pAd, AUTO_WAKEUP_CFG, AutoWakeupCfg.word);
+
+	AsicSendCommandToMcu(pAd, 0x30, 0xff, 0xff, 0x02);   // send POWER-SAVE command to MCU. Timeout 40us.
+
+	OPSTATUS_SET_FLAG(pAd, fOP_STATUS_DOZE);
+
+}
+#endif // CONFIG_STA_SUPPORT //
+
+VOID RT28xxUsbMlmeRadioOn(
+	IN PRTMP_ADAPTER pAd)
+{
+    DBGPRINT(RT_DEBUG_TRACE,("RT28xxUsbMlmeRadioOn()\n"));
+
+	if (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF))
+		return;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+    	AsicSendCommandToMcu(pAd, 0x31, 0xff, 0x00, 0x00);
+		RTMPusecDelay(10000);
+	}
+#endif // CONFIG_STA_SUPPORT //
+	NICResetFromError(pAd);
+
+	// Enable Tx/Rx
+	RTMPEnableRxTx(pAd);
+
+	// Clear Radio off flag
+	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF);
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		RTUSBBulkReceive(pAd);
+#endif // CONFIG_STA_SUPPORT //
+
+	// Set LED
+	RTMPSetLED(pAd, LED_RADIO_ON);
+}
+
+VOID RT28xxUsbMlmeRadioOFF(
+	IN PRTMP_ADAPTER pAd)
+{
+	WPDMA_GLO_CFG_STRUC	GloCfg;
+	UINT32	Value, i;
+
+	DBGPRINT(RT_DEBUG_TRACE,("RT28xxUsbMlmeRadioOFF()\n"));
+
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF))
+		return;
+
+	// Set LED
+	RTMPSetLED(pAd, LED_RADIO_OFF);
+	// Set Radio off flag
+	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF);
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// Link down first if any association exists
+		if (INFRA_ON(pAd) || ADHOC_ON(pAd))
+			LinkDown(pAd, FALSE);
+		RTMPusecDelay(10000);
+
+		//==========================================
+		// Clean up old bss table
+		BssTableInit(&pAd->ScanTab);
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+
+	// Disable MAC Tx/Rx
+	RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+	Value &= (0xfffffff3);
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+	// MAC_SYS_CTRL => value = 0x0 => 40mA
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0);
+
+	// PWR_PIN_CFG => value = 0x0 => 40mA
+	RTMP_IO_WRITE32(pAd, PWR_PIN_CFG, 0);
+
+	// TX_PIN_CFG => value = 0x0 => 20mA
+	RTMP_IO_WRITE32(pAd, TX_PIN_CFG, 0);
+
+	if (pAd->CommonCfg.BBPCurrentBW == BW_40)
+	{
+		// Must using 40MHz.
+		AsicTurnOffRFClk(pAd, pAd->CommonCfg.CentralChannel);
+	}
+	else
+	{
+		// Must using 20MHz.
+		AsicTurnOffRFClk(pAd, pAd->CommonCfg.Channel);
+	}
+
+	// Waiting for DMA idle
+	i = 0;
+	do
+	{
+		RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &GloCfg.word);
+		if ((GloCfg.field.TxDMABusy == 0) && (GloCfg.field.RxDMABusy == 0))
+			break;
+
+		RTMPusecDelay(1000);
+	}while (i++ < 100);
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		AsicSendCommandToMcu(pAd, 0x30, 0xff, 0xff, 0x02);
+#endif // CONFIG_STA_SUPPORT //
+}
+
diff --git a/drivers/staging/rt2870/common/cmm_info.c b/drivers/staging/rt2870/common/cmm_info.c
new file mode 100644
index 00000000000..40792e16237
--- /dev/null
+++ b/drivers/staging/rt2870/common/cmm_info.c
@@ -0,0 +1,3712 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+*/
+
+#include "../rt_config.h"
+
+INT	Show_SSID_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_WirelessMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_TxBurst_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_TxPreamble_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_TxPower_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_Channel_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_BGProtection_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_RTSThreshold_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_FragThreshold_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+#ifdef DOT11_N_SUPPORT
+INT	Show_HtBw_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtMcs_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtGi_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtOpMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtExtcha_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtMpduDensity_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtBaWinSize_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtRdg_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtAmsdu_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_HtAutoBa_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+#endif // DOT11_N_SUPPORT //
+
+INT	Show_CountryRegion_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_CountryRegionABand_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_CountryCode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+#ifdef AGGREGATION_SUPPORT
+INT	Show_PktAggregate_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+#endif // AGGREGATION_SUPPORT //
+
+#ifdef WMM_SUPPORT
+INT	Show_WmmCapable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+#endif // WMM_SUPPORT //
+
+INT	Show_IEEE80211H_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+#ifdef CONFIG_STA_SUPPORT
+INT	Show_NetworkType_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+#endif // CONFIG_STA_SUPPORT //
+
+INT	Show_AuthMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_EncrypType_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_DefaultKeyID_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_Key1_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_Key2_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_Key3_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_Key4_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+INT	Show_WPAPSK_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf);
+
+static struct {
+	CHAR *name;
+	INT (*show_proc)(PRTMP_ADAPTER pAdapter, PUCHAR arg);
+} *PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC, RTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC[] = {
+	{"SSID",					Show_SSID_Proc},
+	{"WirelessMode",			Show_WirelessMode_Proc},
+	{"TxBurst",					Show_TxBurst_Proc},
+	{"TxPreamble",				Show_TxPreamble_Proc},
+	{"TxPower",					Show_TxPower_Proc},
+	{"Channel",					Show_Channel_Proc},
+	{"BGProtection",			Show_BGProtection_Proc},
+	{"RTSThreshold",			Show_RTSThreshold_Proc},
+	{"FragThreshold",			Show_FragThreshold_Proc},
+#ifdef DOT11_N_SUPPORT
+	{"HtBw",					Show_HtBw_Proc},
+	{"HtMcs",					Show_HtMcs_Proc},
+	{"HtGi",					Show_HtGi_Proc},
+	{"HtOpMode",				Show_HtOpMode_Proc},
+	{"HtExtcha",				Show_HtExtcha_Proc},
+	{"HtMpduDensity",			Show_HtMpduDensity_Proc},
+	{"HtBaWinSize",		        Show_HtBaWinSize_Proc},
+	{"HtRdg",		        	Show_HtRdg_Proc},
+	{"HtAmsdu",		        	Show_HtAmsdu_Proc},
+	{"HtAutoBa",		        Show_HtAutoBa_Proc},
+#endif // DOT11_N_SUPPORT //
+	{"CountryRegion",			Show_CountryRegion_Proc},
+	{"CountryRegionABand",		Show_CountryRegionABand_Proc},
+	{"CountryCode",				Show_CountryCode_Proc},
+#ifdef AGGREGATION_SUPPORT
+	{"PktAggregate",			Show_PktAggregate_Proc},
+#endif
+
+#ifdef WMM_SUPPORT
+	{"WmmCapable",				Show_WmmCapable_Proc},
+#endif
+	{"IEEE80211H",				Show_IEEE80211H_Proc},
+#ifdef CONFIG_STA_SUPPORT
+    {"NetworkType",				Show_NetworkType_Proc},
+#endif // CONFIG_STA_SUPPORT //
+	{"AuthMode",				Show_AuthMode_Proc},
+	{"EncrypType",				Show_EncrypType_Proc},
+	{"DefaultKeyID",			Show_DefaultKeyID_Proc},
+	{"Key1",					Show_Key1_Proc},
+	{"Key2",					Show_Key2_Proc},
+	{"Key3",					Show_Key3_Proc},
+	{"Key4",					Show_Key4_Proc},
+	{"WPAPSK",					Show_WPAPSK_Proc},
+	{NULL, NULL}
+};
+
+/*
+    ==========================================================================
+    Description:
+        Get Driver version.
+
+    Return:
+    ==========================================================================
+*/
+INT Set_DriverVersion_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		DBGPRINT(RT_DEBUG_TRACE, ("Driver version-%s\n", STA_DRIVER_VERSION));
+#endif // CONFIG_STA_SUPPORT //
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Country Region.
+        This command will not work, if the field of CountryRegion in eeprom is programmed.
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_CountryRegion_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG region;
+
+	region = simple_strtol(arg, 0, 10);
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+	return -EOPNOTSUPP;
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+	// Country can be set only when EEPROM not programmed
+	if (pAd->CommonCfg.CountryRegion & 0x80)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Set_CountryRegion_Proc::parameter of CountryRegion in eeprom is programmed \n"));
+		return FALSE;
+	}
+
+	if((region >= 0) && (region <= REGION_MAXIMUM_BG_BAND))
+	{
+		pAd->CommonCfg.CountryRegion = (UCHAR) region;
+	}
+	else if (region == REGION_31_BG_BAND)
+	{
+		pAd->CommonCfg.CountryRegion = (UCHAR) region;
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Set_CountryRegion_Proc::parameters out of range\n"));
+		return FALSE;
+	}
+
+	// if set country region, driver needs to be reset
+	BuildChannelList(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_CountryRegion_Proc::(CountryRegion=%d)\n", pAd->CommonCfg.CountryRegion));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Country Region for A band.
+        This command will not work, if the field of CountryRegion in eeprom is programmed.
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_CountryRegionABand_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG region;
+
+	region = simple_strtol(arg, 0, 10);
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+	return -EOPNOTSUPP;
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+	// Country can be set only when EEPROM not programmed
+	if (pAd->CommonCfg.CountryRegionForABand & 0x80)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Set_CountryRegionABand_Proc::parameter of CountryRegion in eeprom is programmed \n"));
+		return FALSE;
+	}
+
+	if((region >= 0) && (region <= REGION_MAXIMUM_A_BAND))
+	{
+		pAd->CommonCfg.CountryRegionForABand = (UCHAR) region;
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Set_CountryRegionABand_Proc::parameters out of range\n"));
+		return FALSE;
+	}
+
+	// if set country region, driver needs to be reset
+	BuildChannelList(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_CountryRegionABand_Proc::(CountryRegion=%d)\n", pAd->CommonCfg.CountryRegionForABand));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Wireless Mode
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_WirelessMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG	WirelessMode;
+	INT		success = TRUE;
+
+	WirelessMode = simple_strtol(arg, 0, 10);
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		INT MaxPhyMode = PHY_11G;
+
+#ifdef DOT11_N_SUPPORT
+		MaxPhyMode = PHY_11N_5G;
+#endif // DOT11_N_SUPPORT //
+
+		if (WirelessMode <= MaxPhyMode)
+		{
+			RTMPSetPhyMode(pAd, WirelessMode);
+#ifdef DOT11_N_SUPPORT
+			if (WirelessMode >= PHY_11ABGN_MIXED)
+			{
+				pAd->CommonCfg.BACapability.field.AutoBA = TRUE;
+				pAd->CommonCfg.REGBACapability.field.AutoBA = TRUE;
+			}
+			else
+			{
+				pAd->CommonCfg.BACapability.field.AutoBA = FALSE;
+				pAd->CommonCfg.REGBACapability.field.AutoBA = FALSE;
+			}
+#endif // DOT11_N_SUPPORT //
+			// Set AdhocMode rates
+			if (pAd->StaCfg.BssType == BSS_ADHOC)
+			{
+				MlmeUpdateTxRates(pAd, FALSE, 0);
+				MakeIbssBeacon(pAd);           // re-build BEACON frame
+				AsicEnableIbssSync(pAd);       // copy to on-chip memory
+			}
+		}
+		else
+		{
+			success = FALSE;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	// it is needed to set SSID to take effect
+	if (success == TRUE)
+	{
+#ifdef DOT11_N_SUPPORT
+		SetCommonHT(pAd);
+#endif // DOT11_N_SUPPORT //
+		DBGPRINT(RT_DEBUG_TRACE, ("Set_WirelessMode_Proc::(=%ld)\n", WirelessMode));
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Set_WirelessMode_Proc::parameters out of range\n"));
+	}
+
+	return success;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Channel
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_Channel_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+ 	INT		success = TRUE;
+	UCHAR	Channel;
+
+	Channel = (UCHAR) simple_strtol(arg, 0, 10);
+
+	// check if this channel is valid
+	if (ChannelSanity(pAd, Channel) == TRUE)
+	{
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			pAd->CommonCfg.Channel = Channel;
+
+			if (MONITOR_ON(pAd))
+			{
+#ifdef DOT11_N_SUPPORT
+				N_ChannelCheck(pAd);
+				if (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED &&
+					pAd->CommonCfg.RegTransmitSetting.field.BW == BW_40)
+				{
+					N_SetCenCh(pAd);
+					AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+					AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+					DBGPRINT(RT_DEBUG_TRACE, ("BW_40, control_channel(%d), CentralChannel(%d) \n",
+								pAd->CommonCfg.Channel, pAd->CommonCfg.CentralChannel));
+				}
+				else
+#endif // DOT11_N_SUPPORT //
+				{
+					AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+					AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+					DBGPRINT(RT_DEBUG_TRACE, ("BW_20, Channel(%d)\n", pAd->CommonCfg.Channel));
+				}
+			}
+		}
+#endif // CONFIG_STA_SUPPORT //
+		success = TRUE;
+	}
+	else
+	{
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			success = FALSE;
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+
+	if (success == TRUE)
+		DBGPRINT(RT_DEBUG_TRACE, ("Set_Channel_Proc::(Channel=%d)\n", pAd->CommonCfg.Channel));
+
+	return success;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Short Slot Time Enable or Disable
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ShortSlot_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG ShortSlot;
+
+	ShortSlot = simple_strtol(arg, 0, 10);
+
+	if (ShortSlot == 1)
+		pAd->CommonCfg.bUseShortSlotTime = TRUE;
+	else if (ShortSlot == 0)
+		pAd->CommonCfg.bUseShortSlotTime = FALSE;
+	else
+		return FALSE;  //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_ShortSlot_Proc::(ShortSlot=%d)\n", pAd->CommonCfg.bUseShortSlotTime));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Tx power
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_TxPower_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG TxPower;
+	INT   success = FALSE;
+
+	TxPower = (ULONG) simple_strtol(arg, 0, 10);
+	if (TxPower <= 100)
+	{
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			pAd->CommonCfg.TxPowerDefault = TxPower;
+			pAd->CommonCfg.TxPowerPercentage = pAd->CommonCfg.TxPowerDefault;
+		}
+#endif // CONFIG_STA_SUPPORT //
+		success = TRUE;
+	}
+	else
+		success = FALSE;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_TxPower_Proc::(TxPowerPercentage=%ld)\n", pAd->CommonCfg.TxPowerPercentage));
+
+	return success;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set 11B/11G Protection
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_BGProtection_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	switch (simple_strtol(arg, 0, 10))
+	{
+		case 0: //AUTO
+			pAd->CommonCfg.UseBGProtection = 0;
+			break;
+		case 1: //Always On
+			pAd->CommonCfg.UseBGProtection = 1;
+			break;
+		case 2: //Always OFF
+			pAd->CommonCfg.UseBGProtection = 2;
+			break;
+		default:  //Invalid argument
+			return FALSE;
+	}
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_BGProtection_Proc::(BGProtection=%ld)\n", pAd->CommonCfg.UseBGProtection));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set TxPreamble
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_TxPreamble_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	RT_802_11_PREAMBLE	Preamble;
+
+	Preamble = simple_strtol(arg, 0, 10);
+
+
+	switch (Preamble)
+	{
+		case Rt802_11PreambleShort:
+			pAd->CommonCfg.TxPreamble = Preamble;
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				MlmeSetTxPreamble(pAd, Rt802_11PreambleShort);
+#endif // CONFIG_STA_SUPPORT //
+			break;
+		case Rt802_11PreambleLong:
+#ifdef CONFIG_STA_SUPPORT
+		case Rt802_11PreambleAuto:
+			// if user wants AUTO, initialize to LONG here, then change according to AP's
+			// capability upon association.
+#endif // CONFIG_STA_SUPPORT //
+			pAd->CommonCfg.TxPreamble = Preamble;
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				MlmeSetTxPreamble(pAd, Rt802_11PreambleLong);
+#endif // CONFIG_STA_SUPPORT //
+			break;
+		default: //Invalid argument
+			return FALSE;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_TxPreamble_Proc::(TxPreamble=%ld)\n", pAd->CommonCfg.TxPreamble));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set RTS Threshold
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_RTSThreshold_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	 NDIS_802_11_RTS_THRESHOLD           RtsThresh;
+
+	RtsThresh = simple_strtol(arg, 0, 10);
+
+	if((RtsThresh > 0) && (RtsThresh <= MAX_RTS_THRESHOLD))
+		pAd->CommonCfg.RtsThreshold  = (USHORT)RtsThresh;
+#ifdef CONFIG_STA_SUPPORT
+	else if (RtsThresh == 0)
+		pAd->CommonCfg.RtsThreshold = MAX_RTS_THRESHOLD;
+#endif // CONFIG_STA_SUPPORT //
+	else
+		return FALSE; //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_RTSThreshold_Proc::(RTSThreshold=%d)\n", pAd->CommonCfg.RtsThreshold));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Fragment Threshold
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_FragThreshold_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	 NDIS_802_11_FRAGMENTATION_THRESHOLD     FragThresh;
+
+	FragThresh = simple_strtol(arg, 0, 10);
+
+	if (FragThresh > MAX_FRAG_THRESHOLD || FragThresh < MIN_FRAG_THRESHOLD)
+	{
+		//Illegal FragThresh so we set it to default
+		pAd->CommonCfg.FragmentThreshold = MAX_FRAG_THRESHOLD;
+	}
+	else if (FragThresh % 2 == 1)
+	{
+		// The length of each fragment shall always be an even number of octets, except for the last fragment
+		// of an MSDU or MMPDU, which may be either an even or an odd number of octets.
+		pAd->CommonCfg.FragmentThreshold = (USHORT)(FragThresh - 1);
+	}
+	else
+	{
+		pAd->CommonCfg.FragmentThreshold = (USHORT)FragThresh;
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (pAd->CommonCfg.FragmentThreshold == MAX_FRAG_THRESHOLD)
+			pAd->CommonCfg.bUseZeroToDisableFragment = TRUE;
+		else
+			pAd->CommonCfg.bUseZeroToDisableFragment = FALSE;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_FragThreshold_Proc::(FragThreshold=%d)\n", pAd->CommonCfg.FragmentThreshold));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set TxBurst
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_TxBurst_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG TxBurst;
+
+	TxBurst = simple_strtol(arg, 0, 10);
+	if (TxBurst == 1)
+		pAd->CommonCfg.bEnableTxBurst = TRUE;
+	else if (TxBurst == 0)
+		pAd->CommonCfg.bEnableTxBurst = FALSE;
+	else
+		return FALSE;  //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_TxBurst_Proc::(TxBurst=%d)\n", pAd->CommonCfg.bEnableTxBurst));
+
+	return TRUE;
+}
+
+#ifdef AGGREGATION_SUPPORT
+/*
+    ==========================================================================
+    Description:
+        Set TxBurst
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_PktAggregate_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG aggre;
+
+	aggre = simple_strtol(arg, 0, 10);
+
+	if (aggre == 1)
+		pAd->CommonCfg.bAggregationCapable = TRUE;
+	else if (aggre == 0)
+		pAd->CommonCfg.bAggregationCapable = FALSE;
+	else
+		return FALSE;  //Invalid argument
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_PktAggregate_Proc::(AGGRE=%d)\n", pAd->CommonCfg.bAggregationCapable));
+
+	return TRUE;
+}
+#endif
+
+/*
+    ==========================================================================
+    Description:
+        Set IEEE80211H.
+        This parameter is 1 when needs radar detection, otherwise 0
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_IEEE80211H_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+    ULONG ieee80211h;
+
+	ieee80211h = simple_strtol(arg, 0, 10);
+
+	if (ieee80211h == 1)
+		pAd->CommonCfg.bIEEE80211H = TRUE;
+	else if (ieee80211h == 0)
+		pAd->CommonCfg.bIEEE80211H = FALSE;
+	else
+		return FALSE;  //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_IEEE80211H_Proc::(IEEE80211H=%d)\n", pAd->CommonCfg.bIEEE80211H));
+
+	return TRUE;
+}
+
+
+#ifdef DBG
+/*
+    ==========================================================================
+    Description:
+        For Debug information
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_Debug_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	DBGPRINT(RT_DEBUG_TRACE, ("==> Set_Debug_Proc *******************\n"));
+
+    if(simple_strtol(arg, 0, 10) <= RT_DEBUG_LOUD)
+        RTDebugLevel = simple_strtol(arg, 0, 10);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<== Set_Debug_Proc(RTDebugLevel = %ld)\n", RTDebugLevel));
+
+	return TRUE;
+}
+#endif
+
+INT	Show_DescInfo_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Reset statistics counter
+
+    Arguments:
+        pAdapter            Pointer to our adapter
+        arg
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ResetStatCounter_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	//UCHAR           i;
+	//MAC_TABLE_ENTRY *pEntry;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("==>Set_ResetStatCounter_Proc\n"));
+
+	// add the most up-to-date h/w raw counters into software counters
+	NICUpdateRawCounters(pAd);
+
+	NdisZeroMemory(&pAd->WlanCounters, sizeof(COUNTER_802_11));
+	NdisZeroMemory(&pAd->Counters8023, sizeof(COUNTER_802_3));
+	NdisZeroMemory(&pAd->RalinkCounters, sizeof(COUNTER_RALINK));
+
+	// Reset HotSpot counter
+#if 0 // ToDo.
+	for (i = 0; i < MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		pEntry = &pAd->MacTab.Content[i];
+
+		if ((pEntry->Valid == FALSE) || (pEntry->Sst != SST_ASSOC))
+			continue;
+
+		pEntry->HSCounter.LastDataPacketTime = 0;
+		pEntry->HSCounter.TotalRxByteCount= 0;
+		pEntry->HSCounter.TotalTxByteCount= 0;
+	}
+#endif
+
+
+	return TRUE;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Add WPA key process.
+		In Adhoc WPANONE, bPairwise = 0;  KeyIdx = 0;
+
+	Arguments:
+		pAd 					Pointer to our adapter
+		pBuf							Pointer to the where the key stored
+
+	Return Value:
+		NDIS_SUCCESS					Add key successfully
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+#if 0 // remove by AlbertY
+NDIS_STATUS RTMPWPAAddKeyProc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PVOID			pBuf)
+{
+	PNDIS_802_11_KEY	pKey;
+	ULONG				KeyIdx;
+//	NDIS_STATUS 		Status;
+//	ULONG 	offset;	// unused variable, snowpin 2006.07.13
+
+	PUCHAR		pTxMic, pRxMic;
+	BOOLEAN 	bTxKey; 		// Set the key as transmit key
+	BOOLEAN 	bPairwise;		// Indicate the key is pairwise key
+	BOOLEAN 	bKeyRSC;		// indicate the receive  SC set by KeyRSC value.
+								// Otherwise, it will set by the NIC.
+	BOOLEAN 	bAuthenticator; // indicate key is set by authenticator.
+	UCHAR		apidx = BSS0;
+
+	pKey = (PNDIS_802_11_KEY) pBuf;
+	KeyIdx = pKey->KeyIndex & 0xff;
+	// Bit 31 of Add-key, Tx Key
+	bTxKey		   = (pKey->KeyIndex & 0x80000000) ? TRUE : FALSE;
+	// Bit 30 of Add-key PairwiseKey
+	bPairwise	   = (pKey->KeyIndex & 0x40000000) ? TRUE : FALSE;
+	// Bit 29 of Add-key KeyRSC
+	bKeyRSC 	   = (pKey->KeyIndex & 0x20000000) ? TRUE : FALSE;
+	// Bit 28 of Add-key Authenticator
+	bAuthenticator = (pKey->KeyIndex & 0x10000000) ? TRUE : FALSE;
+
+	DBGPRINT(RT_DEBUG_TRACE,("RTMPWPAAddKeyProc==>pKey->KeyIndex = %x. bPairwise= %d\n", pKey->KeyIndex, bPairwise));
+	// 1. Check Group / Pairwise Key
+	if (bPairwise)	// Pairwise Key
+	{
+		// 1. KeyIdx must be 0, otherwise, return NDIS_STATUS_INVALID_DATA
+		if (KeyIdx != 0)
+			return(NDIS_STATUS_INVALID_DATA);
+
+		// 2. Check bTx, it must be true, otherwise, return NDIS_STATUS_INVALID_DATA
+		if (bTxKey == FALSE)
+			return(NDIS_STATUS_INVALID_DATA);
+
+		// 3. If BSSID is all 0xff, return NDIS_STATUS_INVALID_DATA
+		if (MAC_ADDR_EQUAL(pKey->BSSID, BROADCAST_ADDR))
+			return(NDIS_STATUS_INVALID_DATA);
+
+		// 3.1 Check Pairwise key length for TKIP key. For AES, it's always 128 bits
+		//if ((pAdapter->PortCfg.WepStatus == Ndis802_11Encryption2Enabled) && (pKey->KeyLength != LEN_TKIP_KEY))
+		if ((pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled) && (pKey->KeyLength != LEN_TKIP_KEY))
+			return(NDIS_STATUS_INVALID_DATA);
+
+		pAd->SharedKey[apidx][KeyIdx].Type = PAIRWISE_KEY;
+
+		if (pAd->ApCfg.MBSSID[apidx].AuthMode == Ndis802_11AuthModeWPA2)
+		{
+			// Send media specific event to start PMKID caching
+			RTMPIndicateWPA2Status(pAd);
+		}
+	}
+	else
+	{
+		// 1. Check BSSID, if not current BSSID or Bcast, return NDIS_STATUS_INVALID_DATA
+		if ((! MAC_ADDR_EQUAL(pKey->BSSID, BROADCAST_ADDR)) &&
+			(! MAC_ADDR_EQUAL(pKey->BSSID, pAd->ApCfg.MBSSID[apidx].Bssid)))
+			return(NDIS_STATUS_INVALID_DATA);
+
+		// 2. Check Key index for supported Group Key
+		if (KeyIdx >= GROUP_KEY_NUM)
+			return(NDIS_STATUS_INVALID_DATA);
+
+		// 3. Set as default Tx Key if bTxKey is TRUE
+		if (bTxKey == TRUE)
+			pAd->ApCfg.MBSSID[apidx].DefaultKeyId = (UCHAR) KeyIdx;
+
+		pAd->SharedKey[apidx][KeyIdx].Type = GROUP_KEY;
+	}
+
+	// 4. Select RxMic / TxMic based on Supp / Authenticator
+	if (pAd->ApCfg.MBSSID[apidx].AuthMode == Ndis802_11AuthModeWPANone)
+	{
+		// for WPA-None Tx, Rx MIC is the same
+		pTxMic = (PUCHAR) (&pKey->KeyMaterial) + 16;
+		pRxMic = pTxMic;
+	}
+	else if (bAuthenticator == TRUE)
+	{
+		pTxMic = (PUCHAR) (&pKey->KeyMaterial) + 16;
+		pRxMic = (PUCHAR) (&pKey->KeyMaterial) + 24;
+	}
+	else
+	{
+		pRxMic = (PUCHAR) (&pKey->KeyMaterial) + 16;
+		pTxMic = (PUCHAR) (&pKey->KeyMaterial) + 24;
+	}
+
+	// 6. Check RxTsc
+	if (bKeyRSC == TRUE)
+	{
+		NdisMoveMemory(pAd->SharedKey[apidx][KeyIdx].RxTsc, &pKey->KeyRSC, 6);
+		NdisMoveMemory(pAd->MacTab.Content[BSSID_WCID].PairwiseKey.RxTsc, &pKey->KeyRSC, 6);
+	}
+	else
+	{
+		NdisZeroMemory(pAd->SharedKey[apidx][KeyIdx].RxTsc, 6);
+	}
+
+	// 7. Copy information into Pairwise Key structure.
+	// pKey->KeyLength will include TxMic and RxMic, therefore, we use 16 bytes hardcoded.
+	pAd->SharedKey[apidx][KeyIdx].KeyLen = (UCHAR) pKey->KeyLength;
+	pAd->MacTab.Content[BSSID_WCID].PairwiseKey.KeyLen = (UCHAR)pKey->KeyLength;
+	NdisMoveMemory(pAd->SharedKey[BSS0][KeyIdx].Key, &pKey->KeyMaterial, 16);
+	NdisMoveMemory(pAd->MacTab.Content[BSSID_WCID].PairwiseKey.Key, &pKey->KeyMaterial, 16);
+	if (pKey->KeyLength == LEN_TKIP_KEY)
+	{
+		// Only Key lenth equal to TKIP key have these
+		NdisMoveMemory(pAd->SharedKey[apidx][KeyIdx].RxMic, pRxMic, 8);
+		NdisMoveMemory(pAd->SharedKey[apidx][KeyIdx].TxMic, pTxMic, 8);
+		NdisMoveMemory(pAd->MacTab.Content[BSSID_WCID].PairwiseKey.RxMic, pRxMic, 8);
+		NdisMoveMemory(pAd->MacTab.Content[BSSID_WCID].PairwiseKey.TxMic, pTxMic, 8);
+	}
+
+	COPY_MAC_ADDR(pAd->SharedKey[BSS0][KeyIdx].BssId, pKey->BSSID);
+
+	// Init TxTsc to one based on WiFi WPA specs
+	pAd->SharedKey[apidx][KeyIdx].TxTsc[0] = 1;
+	pAd->SharedKey[apidx][KeyIdx].TxTsc[1] = 0;
+	pAd->SharedKey[apidx][KeyIdx].TxTsc[2] = 0;
+	pAd->SharedKey[apidx][KeyIdx].TxTsc[3] = 0;
+	pAd->SharedKey[apidx][KeyIdx].TxTsc[4] = 0;
+	pAd->SharedKey[apidx][KeyIdx].TxTsc[5] = 0;
+	// 4. Init TxTsc to one based on WiFi WPA specs
+	pAd->MacTab.Content[BSSID_WCID].PairwiseKey.TxTsc[0] = 1;
+	pAd->MacTab.Content[BSSID_WCID].PairwiseKey.TxTsc[1] = 0;
+	pAd->MacTab.Content[BSSID_WCID].PairwiseKey.TxTsc[2] = 0;
+	pAd->MacTab.Content[BSSID_WCID].PairwiseKey.TxTsc[3] = 0;
+	pAd->MacTab.Content[BSSID_WCID].PairwiseKey.TxTsc[4] = 0;
+	pAd->MacTab.Content[BSSID_WCID].PairwiseKey.TxTsc[5] = 0;
+
+	if (pAd->ApCfg.MBSSID[apidx].WepStatus == Ndis802_11Encryption3Enabled)
+	{
+		pAd->SharedKey[apidx][KeyIdx].CipherAlg = CIPHER_AES;
+		pAd->MacTab.Content[BSSID_WCID].PairwiseKey.CipherAlg = CIPHER_AES;
+	}
+	else if (pAd->ApCfg.MBSSID[apidx].WepStatus == Ndis802_11Encryption2Enabled)
+	{
+		pAd->SharedKey[apidx][KeyIdx].CipherAlg = CIPHER_TKIP;
+		pAd->MacTab.Content[BSSID_WCID].PairwiseKey.CipherAlg = CIPHER_TKIP;
+	}
+	else if (pAd->ApCfg.MBSSID[apidx].WepStatus == Ndis802_11Encryption1Enabled)
+	{
+		if (pAd->SharedKey[apidx][KeyIdx].KeyLen == 5)
+		{
+			pAd->SharedKey[apidx][KeyIdx].CipherAlg = CIPHER_WEP64;
+			pAd->MacTab.Content[BSSID_WCID].PairwiseKey.CipherAlg = CIPHER_WEP64;
+		}
+		else if (pAd->SharedKey[apidx][KeyIdx].KeyLen == 13)
+		{
+			pAd->SharedKey[apidx][KeyIdx].CipherAlg = CIPHER_WEP128;
+			pAd->MacTab.Content[BSSID_WCID].PairwiseKey.CipherAlg = CIPHER_WEP128;
+		}
+		else
+		{
+			pAd->SharedKey[apidx][KeyIdx].CipherAlg = CIPHER_NONE;
+			pAd->MacTab.Content[BSSID_WCID].PairwiseKey.CipherAlg = CIPHER_NONE;
+		}
+	}
+	else
+	{
+		pAd->SharedKey[apidx][KeyIdx].CipherAlg = CIPHER_NONE;
+		pAd->MacTab.Content[BSSID_WCID].PairwiseKey.CipherAlg = CIPHER_NONE;
+	}
+
+	if ((pAd->OpMode == OPMODE_STA))  // Pairwise Key. Add BSSID to WCTable
+	{
+		pAd->MacTab.Content[BSSID_WCID].PairwiseKey.CipherAlg = pAd->SharedKey[BSS0][KeyIdx].CipherAlg;
+		pAd->MacTab.Content[BSSID_WCID].PairwiseKey.KeyLen = pAd->SharedKey[BSS0][KeyIdx].KeyLen;
+	}
+
+	if ((pAd->ApCfg.MBSSID[apidx].AuthMode == Ndis802_11AuthModeWPA2) ||
+		(pAd->ApCfg.MBSSID[apidx].AuthMode == Ndis802_11AuthModeWPA2PSK))
+	{
+		//
+		// On WPA2, Update Group Key Cipher.
+		//
+		if (!bPairwise)
+		{
+			if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption3Enabled)
+				pAd->SharedKey[apidx][KeyIdx].CipherAlg = CIPHER_AES;
+			else if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption2Enabled)
+				pAd->SharedKey[apidx][KeyIdx].CipherAlg = CIPHER_TKIP;
+		}
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("pAd->SharedKey[%d][%d].CipherAlg = %d\n", apidx, KeyIdx, pAd->SharedKey[apidx][KeyIdx].CipherAlg));
+
+#if 0
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("%s Key #%d", CipherName[pAd->SharedKey[apidx][KeyIdx].CipherAlg],KeyIdx));
+	for (i = 0; i < 16; i++)
+	{
+		DBGPRINT_RAW(RT_DEBUG_TRACE, ("%02x:", pAd->SharedKey[apidx][KeyIdx].Key[i]));
+	}
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("\n	  Rx MIC Key = "));
+	for (i = 0; i < 8; i++)
+	{
+		DBGPRINT_RAW(RT_DEBUG_TRACE, ("%02x:", pAd->SharedKey[apidx][KeyIdx].RxMic[i]));
+	}
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("\n	  Tx MIC Key = "));
+	for (i = 0; i < 8; i++)
+	{
+		DBGPRINT_RAW(RT_DEBUG_TRACE, ("%02x:", pAd->SharedKey[apidx][KeyIdx].TxMic[i]));
+	}
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("\n	  RxTSC = "));
+	for (i = 0; i < 6; i++)
+	{
+		DBGPRINT_RAW(RT_DEBUG_TRACE, ("%02x:", pAd->SharedKey[apidx][KeyIdx].RxTsc[i]));
+	}
+#endif
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("\n pKey-> BSSID:%02x:%02x:%02x:%02x:%02x:%02x \n",
+		pKey->BSSID[0],pKey->BSSID[1],pKey->BSSID[2],pKey->BSSID[3],pKey->BSSID[4],pKey->BSSID[5]));
+
+	if ((bTxKey) && (pAd->OpMode == OPMODE_STA))  // Pairwise Key. Add BSSID to WCTable
+		RTMPAddBSSIDCipher(pAd, BSSID_WCID, pKey, pAd->SharedKey[BSS0][KeyIdx].CipherAlg);
+
+
+	// No matter pairwise key or what leyidx is, always has a copy at on-chip SharedKeytable.
+	AsicAddSharedKeyEntry(pAd,
+						  apidx,
+						  (UCHAR)KeyIdx,
+						  pAd->SharedKey[apidx][KeyIdx].CipherAlg,
+						  pAd->SharedKey[apidx][KeyIdx].Key,
+						  pAd->SharedKey[apidx][KeyIdx].TxMic,
+						  pAd->SharedKey[apidx][KeyIdx].RxMic);
+
+	// The WCID key specified in used at Tx. For STA, always use pairwise key.
+
+	// ad-hoc mode need to specify WAP Group key with WCID index=BSS0Mcast_WCID. Let's always set this key here.
+/*	if (bPairwise == FALSE)
+	{
+		offset = MAC_IVEIV_TABLE_BASE + (BSS0Mcast_WCID * HW_IVEIV_ENTRY_SIZE);
+		NdisZeroMemory(IVEIV, 8);
+		// 1. IV/EIV
+		// Specify key index to find shared key.
+		if ((pAd->SharedKey[BSS0][KeyIdx].CipherAlg==CIPHER_TKIP) ||
+			(pAd->SharedKey[BSS0][KeyIdx].CipherAlg==CIPHER_AES))
+		IVEIV[3] = 0x20;		// Eiv bit on. keyid always 0 for pairwise key
+		IVEIV[3] |= (KeyIdx<< 6);	// groupkey index is not 0
+		for (i=0; i<8; i++)
+		{
+			RTMP_IO_WRITE8(pAd, offset+i, IVEIV[i]);
+		}
+
+		// 2. WCID Attribute UDF:3, BSSIdx:3, Alg:3, Keytable:use share key, BSSIdx is 0
+		WCIDAttri = (pAd->SharedKey[BSS0][KeyIdx].CipherAlg<<1)|PAIRWISEKEYTABLE;
+		offset = MAC_WCID_ATTRIBUTE_BASE + (BSS0Mcast_WCID* HW_WCID_ATTRI_SIZE);
+		RTMP_IO_WRITE32(pAd, offset, WCIDAttri);
+
+	}
+
+*/
+
+   if (pAd->SharedKey[apidx][KeyIdx].Type == GROUP_KEY)
+	{
+		// 802.1x port control
+		pAd->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+		DBGPRINT(RT_DEBUG_TRACE,("!!WPA_802_1X_PORT_SECURED!!\n"));
+
+	}
+
+	return (NDIS_STATUS_SUCCESS);
+}
+#endif
+
+BOOLEAN RTMPCheckStrPrintAble(
+    IN  CHAR *pInPutStr,
+    IN  UCHAR strLen)
+{
+    UCHAR i=0;
+
+    for (i=0; i<strLen; i++)
+    {
+        if ((pInPutStr[i] < 0x21) ||
+            (pInPutStr[i] > 0x7E))
+            return FALSE;
+    }
+
+    return TRUE;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Remove WPA Key process
+
+	Arguments:
+		pAd 					Pointer to our adapter
+		pBuf							Pointer to the where the key stored
+
+	Return Value:
+		NDIS_SUCCESS					Add key successfully
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+#ifdef CONFIG_STA_SUPPORT
+VOID    RTMPSetDesiredRates(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  LONG            Rates)
+{
+    NDIS_802_11_RATES aryRates;
+
+    memset(&aryRates, 0x00, sizeof(NDIS_802_11_RATES));
+    switch (pAdapter->CommonCfg.PhyMode)
+    {
+        case PHY_11A: // A only
+            switch (Rates)
+            {
+                case 6000000: //6M
+                    aryRates[0] = 0x0c; // 6M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_0;
+                    break;
+                case 9000000: //9M
+                    aryRates[0] = 0x12; // 9M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_1;
+                    break;
+                case 12000000: //12M
+                    aryRates[0] = 0x18; // 12M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_2;
+                    break;
+                case 18000000: //18M
+                    aryRates[0] = 0x24; // 18M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_3;
+                    break;
+                case 24000000: //24M
+                    aryRates[0] = 0x30; // 24M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_4;
+                    break;
+                case 36000000: //36M
+                    aryRates[0] = 0x48; // 36M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_5;
+                    break;
+                case 48000000: //48M
+                    aryRates[0] = 0x60; // 48M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_6;
+                    break;
+                case 54000000: //54M
+                    aryRates[0] = 0x6c; // 54M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_7;
+                    break;
+                case -1: //Auto
+                default:
+                    aryRates[0] = 0x6c; // 54Mbps
+                    aryRates[1] = 0x60; // 48Mbps
+                    aryRates[2] = 0x48; // 36Mbps
+                    aryRates[3] = 0x30; // 24Mbps
+                    aryRates[4] = 0x24; // 18M
+                    aryRates[5] = 0x18; // 12M
+                    aryRates[6] = 0x12; // 9M
+                    aryRates[7] = 0x0c; // 6M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_AUTO;
+                    break;
+            }
+            break;
+        case PHY_11BG_MIXED: // B/G Mixed
+        case PHY_11B: // B only
+        case PHY_11ABG_MIXED: // A/B/G Mixed
+        default:
+            switch (Rates)
+            {
+                case 1000000: //1M
+                    aryRates[0] = 0x02;
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_0;
+                    break;
+                case 2000000: //2M
+                    aryRates[0] = 0x04;
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_1;
+                    break;
+                case 5000000: //5.5M
+                    aryRates[0] = 0x0b; // 5.5M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_2;
+                    break;
+                case 11000000: //11M
+                    aryRates[0] = 0x16; // 11M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_3;
+                    break;
+                case 6000000: //6M
+                    aryRates[0] = 0x0c; // 6M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_0;
+                    break;
+                case 9000000: //9M
+                    aryRates[0] = 0x12; // 9M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_1;
+                    break;
+                case 12000000: //12M
+                    aryRates[0] = 0x18; // 12M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_2;
+                    break;
+                case 18000000: //18M
+                    aryRates[0] = 0x24; // 18M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_3;
+                    break;
+                case 24000000: //24M
+                    aryRates[0] = 0x30; // 24M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_4;
+                    break;
+                case 36000000: //36M
+                    aryRates[0] = 0x48; // 36M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_5;
+                    break;
+                case 48000000: //48M
+                    aryRates[0] = 0x60; // 48M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_6;
+                    break;
+                case 54000000: //54M
+                    aryRates[0] = 0x6c; // 54M
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_7;
+                    break;
+                case -1: //Auto
+                default:
+                    if (pAdapter->CommonCfg.PhyMode == PHY_11B)
+                    { //B Only
+                        aryRates[0] = 0x16; // 11Mbps
+                        aryRates[1] = 0x0b; // 5.5Mbps
+                        aryRates[2] = 0x04; // 2Mbps
+                        aryRates[3] = 0x02; // 1Mbps
+                    }
+                    else
+                    { //(B/G) Mixed or (A/B/G) Mixed
+                        aryRates[0] = 0x6c; // 54Mbps
+                        aryRates[1] = 0x60; // 48Mbps
+                        aryRates[2] = 0x48; // 36Mbps
+                        aryRates[3] = 0x30; // 24Mbps
+                        aryRates[4] = 0x16; // 11Mbps
+                        aryRates[5] = 0x0b; // 5.5Mbps
+                        aryRates[6] = 0x04; // 2Mbps
+                        aryRates[7] = 0x02; // 1Mbps
+                    }
+                    pAdapter->StaCfg.DesiredTransmitSetting.field.MCS = MCS_AUTO;
+                    break;
+            }
+            break;
+    }
+
+    NdisZeroMemory(pAdapter->CommonCfg.DesireRate, MAX_LEN_OF_SUPPORTED_RATES);
+    NdisMoveMemory(pAdapter->CommonCfg.DesireRate, &aryRates, sizeof(NDIS_802_11_RATES));
+    DBGPRINT(RT_DEBUG_TRACE, (" RTMPSetDesiredRates (%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x)\n",
+        pAdapter->CommonCfg.DesireRate[0],pAdapter->CommonCfg.DesireRate[1],
+        pAdapter->CommonCfg.DesireRate[2],pAdapter->CommonCfg.DesireRate[3],
+        pAdapter->CommonCfg.DesireRate[4],pAdapter->CommonCfg.DesireRate[5],
+        pAdapter->CommonCfg.DesireRate[6],pAdapter->CommonCfg.DesireRate[7] ));
+    // Changing DesiredRate may affect the MAX TX rate we used to TX frames out
+    MlmeUpdateTxRates(pAdapter, FALSE, 0);
+}
+
+NDIS_STATUS RTMPWPARemoveKeyProc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PVOID			pBuf)
+{
+	PNDIS_802_11_REMOVE_KEY pKey;
+	ULONG					KeyIdx;
+	NDIS_STATUS 			Status = NDIS_STATUS_FAILURE;
+	BOOLEAN 	bTxKey; 		// Set the key as transmit key
+	BOOLEAN 	bPairwise;		// Indicate the key is pairwise key
+	BOOLEAN 	bKeyRSC;		// indicate the receive  SC set by KeyRSC value.
+								// Otherwise, it will set by the NIC.
+	BOOLEAN 	bAuthenticator; // indicate key is set by authenticator.
+	INT 		i;
+
+	DBGPRINT(RT_DEBUG_TRACE,("---> RTMPWPARemoveKeyProc\n"));
+
+	pKey = (PNDIS_802_11_REMOVE_KEY) pBuf;
+	KeyIdx = pKey->KeyIndex & 0xff;
+	// Bit 31 of Add-key, Tx Key
+	bTxKey		   = (pKey->KeyIndex & 0x80000000) ? TRUE : FALSE;
+	// Bit 30 of Add-key PairwiseKey
+	bPairwise	   = (pKey->KeyIndex & 0x40000000) ? TRUE : FALSE;
+	// Bit 29 of Add-key KeyRSC
+	bKeyRSC 	   = (pKey->KeyIndex & 0x20000000) ? TRUE : FALSE;
+	// Bit 28 of Add-key Authenticator
+	bAuthenticator = (pKey->KeyIndex & 0x10000000) ? TRUE : FALSE;
+
+	// 1. If bTx is TRUE, return failure information
+	if (bTxKey == TRUE)
+		return(NDIS_STATUS_INVALID_DATA);
+
+	// 2. Check Pairwise Key
+	if (bPairwise)
+	{
+		// a. If BSSID is broadcast, remove all pairwise keys.
+		// b. If not broadcast, remove the pairwise specified by BSSID
+		for (i = 0; i < SHARE_KEY_NUM; i++)
+		{
+			if (MAC_ADDR_EQUAL(pAd->SharedKey[BSS0][i].BssId, pKey->BSSID))
+			{
+				DBGPRINT(RT_DEBUG_TRACE,("RTMPWPARemoveKeyProc(KeyIdx=%d)\n", i));
+				pAd->SharedKey[BSS0][i].KeyLen = 0;
+				pAd->SharedKey[BSS0][i].CipherAlg = CIPHER_NONE;
+				AsicRemoveSharedKeyEntry(pAd, BSS0, (UCHAR)i);
+				Status = NDIS_STATUS_SUCCESS;
+				break;
+			}
+		}
+	}
+	// 3. Group Key
+	else
+	{
+		// a. If BSSID is broadcast, remove all group keys indexed
+		// b. If BSSID matched, delete the group key indexed.
+		DBGPRINT(RT_DEBUG_TRACE,("RTMPWPARemoveKeyProc(KeyIdx=%ld)\n", KeyIdx));
+		pAd->SharedKey[BSS0][KeyIdx].KeyLen = 0;
+		pAd->SharedKey[BSS0][KeyIdx].CipherAlg = CIPHER_NONE;
+		AsicRemoveSharedKeyEntry(pAd, BSS0, (UCHAR)KeyIdx);
+		Status = NDIS_STATUS_SUCCESS;
+	}
+
+	return (Status);
+}
+#endif // CONFIG_STA_SUPPORT //
+
+
+#ifdef CONFIG_STA_SUPPORT
+/*
+	========================================================================
+
+	Routine Description:
+		Remove All WPA Keys
+
+	Arguments:
+		pAd 					Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPWPARemoveAllKeys(
+	IN	PRTMP_ADAPTER	pAd)
+{
+
+	UCHAR 	i;
+
+	DBGPRINT(RT_DEBUG_TRACE,("RTMPWPARemoveAllKeys(AuthMode=%d, WepStatus=%d)\n", pAd->StaCfg.AuthMode, pAd->StaCfg.WepStatus));
+
+	// For WEP/CKIP, there is no need to remove it, since WinXP won't set it again after
+	// Link up. And it will be replaced if user changed it.
+	if (pAd->StaCfg.AuthMode < Ndis802_11AuthModeWPA)
+		return;
+
+	// For WPA-None, there is no need to remove it, since WinXP won't set it again after
+	// Link up. And it will be replaced if user changed it.
+	if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+		return;
+
+	// set BSSID wcid entry of the Pair-wise Key table as no-security mode
+	AsicRemovePairwiseKeyEntry(pAd, BSS0, BSSID_WCID);
+
+	// set all shared key mode as no-security.
+	for (i = 0; i < SHARE_KEY_NUM; i++)
+    {
+		DBGPRINT(RT_DEBUG_TRACE,("remove %s key #%d\n", CipherName[pAd->SharedKey[BSS0][i].CipherAlg], i));
+		NdisZeroMemory(&pAd->SharedKey[BSS0][i], sizeof(CIPHER_KEY));
+
+		AsicRemoveSharedKeyEntry(pAd, BSS0, i);
+	}
+
+}
+#endif // CONFIG_STA_SUPPORT //
+
+/*
+	========================================================================
+	Routine Description:
+		Change NIC PHY mode. Re-association may be necessary. possible settings
+		include - PHY_11B, PHY_11BG_MIXED, PHY_11A, and PHY_11ABG_MIXED
+
+	Arguments:
+		pAd - Pointer to our adapter
+		phymode  -
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	========================================================================
+*/
+VOID	RTMPSetPhyMode(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	ULONG phymode)
+{
+	INT i;
+	// the selected phymode must be supported by the RF IC encoded in E2PROM
+
+	// if no change, do nothing
+	/* bug fix
+	if (pAd->CommonCfg.PhyMode == phymode)
+		return;
+    */
+	pAd->CommonCfg.PhyMode = (UCHAR)phymode;
+
+	DBGPRINT(RT_DEBUG_TRACE,("RTMPSetPhyMode : PhyMode=%d, channel=%d \n", pAd->CommonCfg.PhyMode, pAd->CommonCfg.Channel));
+#ifdef EXT_BUILD_CHANNEL_LIST
+	BuildChannelListEx(pAd);
+#else
+	BuildChannelList(pAd);
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+	// sanity check user setting
+	for (i = 0; i < pAd->ChannelListNum; i++)
+	{
+		if (pAd->CommonCfg.Channel == pAd->ChannelList[i].Channel)
+			break;
+	}
+
+	if (i == pAd->ChannelListNum)
+	{
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			pAd->CommonCfg.Channel = FirstChannel(pAd);
+#endif // CONFIG_STA_SUPPORT //
+		DBGPRINT(RT_DEBUG_ERROR, ("RTMPSetPhyMode: channel is out of range, use first channel=%d \n", pAd->CommonCfg.Channel));
+	}
+
+	NdisZeroMemory(pAd->CommonCfg.SupRate, MAX_LEN_OF_SUPPORTED_RATES);
+	NdisZeroMemory(pAd->CommonCfg.ExtRate, MAX_LEN_OF_SUPPORTED_RATES);
+	NdisZeroMemory(pAd->CommonCfg.DesireRate, MAX_LEN_OF_SUPPORTED_RATES);
+	switch (phymode) {
+		case PHY_11B:
+			pAd->CommonCfg.SupRate[0]  = 0x82;	  // 1 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[1]  = 0x84;	  // 2 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[2]  = 0x8B;	  // 5.5 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[3]  = 0x96;	  // 11 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRateLen  = 4;
+			pAd->CommonCfg.ExtRateLen  = 0;
+			pAd->CommonCfg.DesireRate[0]  = 2;	   // 1 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[1]  = 4;	   // 2 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[2]  = 11;    // 5.5 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[3]  = 22;    // 11 mbps, in units of 0.5 Mbps
+			//pAd->CommonCfg.HTPhyMode.field.MODE = MODE_CCK; // This MODE is only FYI. not use
+			break;
+
+		case PHY_11G:
+		case PHY_11BG_MIXED:
+		case PHY_11ABG_MIXED:
+#ifdef DOT11_N_SUPPORT
+		case PHY_11N_2_4G:
+		case PHY_11ABGN_MIXED:
+		case PHY_11BGN_MIXED:
+		case PHY_11GN_MIXED:
+#endif // DOT11_N_SUPPORT //
+			pAd->CommonCfg.SupRate[0]  = 0x82;	  // 1 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[1]  = 0x84;	  // 2 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[2]  = 0x8B;	  // 5.5 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[3]  = 0x96;	  // 11 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[4]  = 0x12;	  // 9 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRate[5]  = 0x24;	  // 18 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRate[6]  = 0x48;	  // 36 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRate[7]  = 0x6c;	  // 54 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRateLen  = 8;
+			pAd->CommonCfg.ExtRate[0]  = 0x0C;	  // 6 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.ExtRate[1]  = 0x18;	  // 12 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.ExtRate[2]  = 0x30;	  // 24 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.ExtRate[3]  = 0x60;	  // 48 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.ExtRateLen  = 4;
+			pAd->CommonCfg.DesireRate[0]  = 2;	   // 1 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[1]  = 4;	   // 2 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[2]  = 11;    // 5.5 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[3]  = 22;    // 11 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[4]  = 12;    // 6 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[5]  = 18;    // 9 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[6]  = 24;    // 12 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[7]  = 36;    // 18 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[8]  = 48;    // 24 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[9]  = 72;    // 36 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[10] = 96;    // 48 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[11] = 108;   // 54 mbps, in units of 0.5 Mbps
+			break;
+
+		case PHY_11A:
+#ifdef DOT11_N_SUPPORT
+		case PHY_11AN_MIXED:
+		case PHY_11AGN_MIXED:
+		case PHY_11N_5G:
+#endif // DOT11_N_SUPPORT //
+			pAd->CommonCfg.SupRate[0]  = 0x8C;	  // 6 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[1]  = 0x12;	  // 9 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRate[2]  = 0x98;	  // 12 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[3]  = 0x24;	  // 18 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRate[4]  = 0xb0;	  // 24 mbps, in units of 0.5 Mbps, basic rate
+			pAd->CommonCfg.SupRate[5]  = 0x48;	  // 36 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRate[6]  = 0x60;	  // 48 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRate[7]  = 0x6c;	  // 54 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.SupRateLen  = 8;
+			pAd->CommonCfg.ExtRateLen  = 0;
+			pAd->CommonCfg.DesireRate[0]  = 12;    // 6 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[1]  = 18;    // 9 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[2]  = 24;    // 12 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[3]  = 36;    // 18 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[4]  = 48;    // 24 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[5]  = 72;    // 36 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[6]  = 96;    // 48 mbps, in units of 0.5 Mbps
+			pAd->CommonCfg.DesireRate[7]  = 108;   // 54 mbps, in units of 0.5 Mbps
+			//pAd->CommonCfg.HTPhyMode.field.MODE = MODE_OFDM; // This MODE is only FYI. not use
+			break;
+
+		default:
+			break;
+	}
+
+
+	pAd->CommonCfg.BandState = UNKNOWN_BAND;
+}
+
+
+#ifdef DOT11_N_SUPPORT
+/*
+	========================================================================
+	Routine Description:
+		Caller ensures we has 802.11n support.
+		Calls at setting HT from AP/STASetinformation
+
+	Arguments:
+		pAd - Pointer to our adapter
+		phymode  -
+
+	========================================================================
+*/
+VOID	RTMPSetHT(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	OID_SET_HT_PHYMODE *pHTPhyMode)
+{
+	//ULONG	*pmcs;
+	UINT32	Value = 0;
+	UCHAR	BBPValue = 0;
+	UCHAR	BBP3Value = 0;
+	UCHAR	RxStream = pAd->CommonCfg.RxStream;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPSetHT : HT_mode(%d), ExtOffset(%d), MCS(%d), BW(%d), STBC(%d), SHORTGI(%d)\n",
+										pHTPhyMode->HtMode, pHTPhyMode->ExtOffset,
+										pHTPhyMode->MCS, pHTPhyMode->BW,
+										pHTPhyMode->STBC, pHTPhyMode->SHORTGI));
+
+	// Don't zero supportedHyPhy structure.
+	RTMPZeroMemory(&pAd->CommonCfg.HtCapability, sizeof(pAd->CommonCfg.HtCapability));
+	RTMPZeroMemory(&pAd->CommonCfg.AddHTInfo, sizeof(pAd->CommonCfg.AddHTInfo));
+	RTMPZeroMemory(&pAd->CommonCfg.NewExtChanOffset, sizeof(pAd->CommonCfg.NewExtChanOffset));
+	RTMPZeroMemory(&pAd->CommonCfg.DesiredHtPhy, sizeof(pAd->CommonCfg.DesiredHtPhy));
+
+   	if (pAd->CommonCfg.bRdg)
+	{
+		pAd->CommonCfg.HtCapability.ExtHtCapInfo.PlusHTC = 1;
+		pAd->CommonCfg.HtCapability.ExtHtCapInfo.RDGSupport = 1;
+	}
+	else
+	{
+		pAd->CommonCfg.HtCapability.ExtHtCapInfo.PlusHTC = 0;
+		pAd->CommonCfg.HtCapability.ExtHtCapInfo.RDGSupport = 0;
+	}
+
+	pAd->CommonCfg.HtCapability.HtCapParm.MaxRAmpduFactor = 3;
+	pAd->CommonCfg.DesiredHtPhy.MaxRAmpduFactor = 3;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPSetHT : RxBAWinLimit = %d\n", pAd->CommonCfg.BACapability.field.RxBAWinLimit));
+
+	// Mimo power save, A-MSDU size,
+	pAd->CommonCfg.DesiredHtPhy.AmsduEnable = (USHORT)pAd->CommonCfg.BACapability.field.AmsduEnable;
+	pAd->CommonCfg.DesiredHtPhy.AmsduSize = (UCHAR)pAd->CommonCfg.BACapability.field.AmsduSize;
+	pAd->CommonCfg.DesiredHtPhy.MimoPs = (UCHAR)pAd->CommonCfg.BACapability.field.MMPSmode;
+	pAd->CommonCfg.DesiredHtPhy.MpduDensity = (UCHAR)pAd->CommonCfg.BACapability.field.MpduDensity;
+
+	pAd->CommonCfg.HtCapability.HtCapInfo.AMsduSize = (USHORT)pAd->CommonCfg.BACapability.field.AmsduSize;
+	pAd->CommonCfg.HtCapability.HtCapInfo.MimoPs = (USHORT)pAd->CommonCfg.BACapability.field.MMPSmode;
+	pAd->CommonCfg.HtCapability.HtCapParm.MpduDensity = (UCHAR)pAd->CommonCfg.BACapability.field.MpduDensity;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPSetHT : AMsduSize = %d, MimoPs = %d, MpduDensity = %d, MaxRAmpduFactor = %d\n",
+													pAd->CommonCfg.DesiredHtPhy.AmsduSize,
+													pAd->CommonCfg.DesiredHtPhy.MimoPs,
+													pAd->CommonCfg.DesiredHtPhy.MpduDensity,
+													pAd->CommonCfg.DesiredHtPhy.MaxRAmpduFactor));
+
+	if(pHTPhyMode->HtMode == HTMODE_GF)
+	{
+		pAd->CommonCfg.HtCapability.HtCapInfo.GF = 1;
+		pAd->CommonCfg.DesiredHtPhy.GF = 1;
+	}
+	else
+		pAd->CommonCfg.DesiredHtPhy.GF = 0;
+
+	// Decide Rx MCSSet
+	switch (RxStream)
+	{
+		case 1:
+			pAd->CommonCfg.HtCapability.MCSSet[0] =  0xff;
+			pAd->CommonCfg.HtCapability.MCSSet[1] =  0x00;
+			break;
+
+		case 2:
+			pAd->CommonCfg.HtCapability.MCSSet[0] =  0xff;
+			pAd->CommonCfg.HtCapability.MCSSet[1] =  0xff;
+			break;
+
+		case 3: // 3*3
+			pAd->CommonCfg.HtCapability.MCSSet[0] =  0xff;
+			pAd->CommonCfg.HtCapability.MCSSet[1] =  0xff;
+			pAd->CommonCfg.HtCapability.MCSSet[2] =  0xff;
+			break;
+	}
+
+	if (pAd->CommonCfg.bForty_Mhz_Intolerant && (pAd->CommonCfg.Channel <= 14) && (pHTPhyMode->BW == BW_40) )
+	{
+		pHTPhyMode->BW = BW_20;
+		pAd->CommonCfg.HtCapability.HtCapInfo.Forty_Mhz_Intolerant = 1;
+	}
+
+	if(pHTPhyMode->BW == BW_40)
+	{
+		pAd->CommonCfg.HtCapability.MCSSet[4] = 0x1; // MCS 32
+		pAd->CommonCfg.HtCapability.HtCapInfo.ChannelWidth = 1;
+		if (pAd->CommonCfg.Channel <= 14)
+			pAd->CommonCfg.HtCapability.HtCapInfo.CCKmodein40 = 1;
+
+		pAd->CommonCfg.DesiredHtPhy.ChannelWidth = 1;
+		pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth = 1;
+		pAd->CommonCfg.AddHTInfo.AddHtInfo.ExtChanOffset = (pHTPhyMode->ExtOffset == EXTCHA_BELOW)? (EXTCHA_BELOW): EXTCHA_ABOVE;
+		// Set Regsiter for extension channel position.
+		RTMP_IO_READ32(pAd, TX_BAND_CFG, &Value);
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BBP3Value);
+		if ((pHTPhyMode->ExtOffset == EXTCHA_BELOW))
+		{
+			Value |= 0x1;
+			BBP3Value |= (0x20);
+			RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Value);
+		}
+		else if ((pHTPhyMode->ExtOffset == EXTCHA_ABOVE))
+		{
+			Value &= 0xfe;
+			BBP3Value &= (~0x20);
+			RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Value);
+		}
+
+		// Turn on BBP 40MHz mode now only as AP .
+		// Sta can turn on BBP 40MHz after connection with 40MHz AP. Sta only broadcast 40MHz capability before connection.
+		if ((pAd->OpMode == OPMODE_AP) || INFRA_ON(pAd) || ADHOC_ON(pAd)
+			)
+		{
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+			BBPValue &= (~0x18);
+			BBPValue |= 0x10;
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BBP3Value);
+			pAd->CommonCfg.BBPCurrentBW = BW_40;
+		}
+	}
+	else
+	{
+		pAd->CommonCfg.HtCapability.HtCapInfo.ChannelWidth = 0;
+		pAd->CommonCfg.DesiredHtPhy.ChannelWidth = 0;
+		pAd->CommonCfg.AddHTInfo.AddHtInfo.RecomWidth = 0;
+		pAd->CommonCfg.AddHTInfo.AddHtInfo.ExtChanOffset = EXTCHA_NONE;
+		pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel;
+		// Turn on BBP 20MHz mode by request here.
+		{
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+			BBPValue &= (~0x18);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+			pAd->CommonCfg.BBPCurrentBW = BW_20;
+		}
+	}
+
+	if(pHTPhyMode->STBC == STBC_USE)
+	{
+		pAd->CommonCfg.HtCapability.HtCapInfo.TxSTBC = 1;
+		pAd->CommonCfg.DesiredHtPhy.TxSTBC = 1;
+		pAd->CommonCfg.HtCapability.HtCapInfo.RxSTBC = 1;
+		pAd->CommonCfg.DesiredHtPhy.RxSTBC = 1;
+	}
+	else
+	{
+		pAd->CommonCfg.DesiredHtPhy.TxSTBC = 0;
+		pAd->CommonCfg.DesiredHtPhy.RxSTBC = 0;
+	}
+
+#ifdef RT2870
+	/* Frank recommend ,If not, Tx maybe block in high power. Rx has no problem*/
+	if(IS_RT3070(pAd) && ((pAd->RfIcType == RFIC_3020) || (pAd->RfIcType == RFIC_2020)))
+	{
+		pAd->CommonCfg.HtCapability.HtCapInfo.TxSTBC = 0;
+		pAd->CommonCfg.DesiredHtPhy.TxSTBC = 0;
+	}
+#endif // RT2870 //
+
+	if(pHTPhyMode->SHORTGI == GI_400)
+	{
+		pAd->CommonCfg.HtCapability.HtCapInfo.ShortGIfor20 = 1;
+		pAd->CommonCfg.HtCapability.HtCapInfo.ShortGIfor40 = 1;
+		pAd->CommonCfg.DesiredHtPhy.ShortGIfor20 = 1;
+		pAd->CommonCfg.DesiredHtPhy.ShortGIfor40 = 1;
+	}
+	else
+	{
+		pAd->CommonCfg.HtCapability.HtCapInfo.ShortGIfor20 = 0;
+		pAd->CommonCfg.HtCapability.HtCapInfo.ShortGIfor40 = 0;
+		pAd->CommonCfg.DesiredHtPhy.ShortGIfor20 = 0;
+		pAd->CommonCfg.DesiredHtPhy.ShortGIfor40 = 0;
+	}
+
+	// We support link adaptation for unsolicit MCS feedback, set to 2.
+	pAd->CommonCfg.HtCapability.ExtHtCapInfo.MCSFeedback = MCSFBK_NONE; //MCSFBK_UNSOLICIT;
+	pAd->CommonCfg.AddHTInfo.ControlChan = pAd->CommonCfg.Channel;
+	// 1, the extension channel above the control channel.
+
+	// EDCA parameters used for AP's own transmission
+	if (pAd->CommonCfg.APEdcaParm.bValid == FALSE)
+	{
+		pAd->CommonCfg.APEdcaParm.bValid = TRUE;
+		pAd->CommonCfg.APEdcaParm.Aifsn[0] = 3;
+		pAd->CommonCfg.APEdcaParm.Aifsn[1] = 7;
+		pAd->CommonCfg.APEdcaParm.Aifsn[2] = 1;
+		pAd->CommonCfg.APEdcaParm.Aifsn[3] = 1;
+
+		pAd->CommonCfg.APEdcaParm.Cwmin[0] = 4;
+		pAd->CommonCfg.APEdcaParm.Cwmin[1] = 4;
+		pAd->CommonCfg.APEdcaParm.Cwmin[2] = 3;
+		pAd->CommonCfg.APEdcaParm.Cwmin[3] = 2;
+
+		pAd->CommonCfg.APEdcaParm.Cwmax[0] = 6;
+		pAd->CommonCfg.APEdcaParm.Cwmax[1] = 10;
+		pAd->CommonCfg.APEdcaParm.Cwmax[2] = 4;
+		pAd->CommonCfg.APEdcaParm.Cwmax[3] = 3;
+
+		pAd->CommonCfg.APEdcaParm.Txop[0]  = 0;
+		pAd->CommonCfg.APEdcaParm.Txop[1]  = 0;
+		pAd->CommonCfg.APEdcaParm.Txop[2]  = 94;
+		pAd->CommonCfg.APEdcaParm.Txop[3]  = 47;
+	}
+	AsicSetEdcaParm(pAd, &pAd->CommonCfg.APEdcaParm);
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		RTMPSetIndividualHT(pAd, 0);
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+}
+
+/*
+	========================================================================
+	Routine Description:
+		Caller ensures we has 802.11n support.
+		Calls at setting HT from AP/STASetinformation
+
+	Arguments:
+		pAd - Pointer to our adapter
+		phymode  -
+
+	========================================================================
+*/
+VOID	RTMPSetIndividualHT(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	UCHAR				apidx)
+{
+	PRT_HT_PHY_INFO		pDesired_ht_phy = NULL;
+	UCHAR	TxStream = pAd->CommonCfg.TxStream;
+	UCHAR	DesiredMcs	= MCS_AUTO;
+
+	do
+	{
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			pDesired_ht_phy = &pAd->StaCfg.DesiredHtPhyInfo;
+			DesiredMcs = pAd->StaCfg.DesiredTransmitSetting.field.MCS;
+			//pAd->StaCfg.bAutoTxRateSwitch = (DesiredMcs == MCS_AUTO) ? TRUE : FALSE;
+				break;
+		}
+#endif // CONFIG_STA_SUPPORT //
+	} while (FALSE);
+
+	if (pDesired_ht_phy == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("RTMPSetIndividualHT: invalid apidx(%d)\n", apidx));
+		return;
+	}
+	RTMPZeroMemory(pDesired_ht_phy, sizeof(RT_HT_PHY_INFO));
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPSetIndividualHT : Desired MCS = %d\n", DesiredMcs));
+	// Check the validity of MCS
+	if ((TxStream == 1) && ((DesiredMcs >= MCS_8) && (DesiredMcs <= MCS_15)))
+	{
+		DBGPRINT(RT_DEBUG_WARN, ("RTMPSetIndividualHT: MCS(%d) is invalid in 1S, reset it as MCS_7\n", DesiredMcs));
+		DesiredMcs = MCS_7;
+	}
+
+	if ((pAd->CommonCfg.DesiredHtPhy.ChannelWidth == BW_20) && (DesiredMcs == MCS_32))
+	{
+		DBGPRINT(RT_DEBUG_WARN, ("RTMPSetIndividualHT: MCS_32 is only supported in 40-MHz, reset it as MCS_0\n"));
+		DesiredMcs = MCS_0;
+	}
+
+	pDesired_ht_phy->bHtEnable = TRUE;
+
+	// Decide desired Tx MCS
+	switch (TxStream)
+	{
+		case 1:
+			if (DesiredMcs == MCS_AUTO)
+			{
+				pDesired_ht_phy->MCSSet[0]= 0xff;
+				pDesired_ht_phy->MCSSet[1]= 0x00;
+			}
+			else if (DesiredMcs <= MCS_7)
+			{
+				pDesired_ht_phy->MCSSet[0]= 1<<DesiredMcs;
+				pDesired_ht_phy->MCSSet[1]= 0x00;
+			}
+			break;
+
+		case 2:
+			if (DesiredMcs == MCS_AUTO)
+			{
+				pDesired_ht_phy->MCSSet[0]= 0xff;
+				pDesired_ht_phy->MCSSet[1]= 0xff;
+			}
+			else if (DesiredMcs <= MCS_15)
+			{
+				ULONG mode;
+
+				mode = DesiredMcs / 8;
+				if (mode < 2)
+					pDesired_ht_phy->MCSSet[mode] = (1 << (DesiredMcs - mode * 8));
+			}
+			break;
+
+		case 3: // 3*3
+			if (DesiredMcs == MCS_AUTO)
+			{
+				/* MCS0 ~ MCS23, 3 bytes */
+				pDesired_ht_phy->MCSSet[0]= 0xff;
+				pDesired_ht_phy->MCSSet[1]= 0xff;
+				pDesired_ht_phy->MCSSet[2]= 0xff;
+			}
+			else if (DesiredMcs <= MCS_23)
+			{
+				ULONG mode;
+
+				mode = DesiredMcs / 8;
+				if (mode < 3)
+					pDesired_ht_phy->MCSSet[mode] = (1 << (DesiredMcs - mode * 8));
+			}
+			break;
+	}
+
+	if(pAd->CommonCfg.DesiredHtPhy.ChannelWidth == BW_40)
+	{
+		if (DesiredMcs == MCS_AUTO || DesiredMcs == MCS_32)
+			pDesired_ht_phy->MCSSet[4] = 0x1;
+	}
+
+	// update HT Rate setting
+    if (pAd->OpMode == OPMODE_STA)
+        MlmeUpdateHtTxRates(pAd, BSS0);
+    else
+	    MlmeUpdateHtTxRates(pAd, apidx);
+}
+
+
+/*
+	========================================================================
+	Routine Description:
+		Update HT IE from our capability.
+
+	Arguments:
+		Send all HT IE in beacon/probe rsp/assoc rsp/action frame.
+
+
+	========================================================================
+*/
+VOID	RTMPUpdateHTIE(
+	IN	RT_HT_CAPABILITY	*pRtHt,
+	IN		UCHAR				*pMcsSet,
+	OUT		HT_CAPABILITY_IE *pHtCapability,
+	OUT		ADD_HT_INFO_IE		*pAddHtInfo)
+{
+	RTMPZeroMemory(pHtCapability, sizeof(HT_CAPABILITY_IE));
+	RTMPZeroMemory(pAddHtInfo, sizeof(ADD_HT_INFO_IE));
+
+		pHtCapability->HtCapInfo.ChannelWidth = pRtHt->ChannelWidth;
+		pHtCapability->HtCapInfo.MimoPs = pRtHt->MimoPs;
+		pHtCapability->HtCapInfo.GF = pRtHt->GF;
+		pHtCapability->HtCapInfo.ShortGIfor20 = pRtHt->ShortGIfor20;
+		pHtCapability->HtCapInfo.ShortGIfor40 = pRtHt->ShortGIfor40;
+		pHtCapability->HtCapInfo.TxSTBC = pRtHt->TxSTBC;
+		pHtCapability->HtCapInfo.RxSTBC = pRtHt->RxSTBC;
+		pHtCapability->HtCapInfo.AMsduSize = pRtHt->AmsduSize;
+		pHtCapability->HtCapParm.MaxRAmpduFactor = pRtHt->MaxRAmpduFactor;
+		pHtCapability->HtCapParm.MpduDensity = pRtHt->MpduDensity;
+
+		pAddHtInfo->AddHtInfo.ExtChanOffset = pRtHt->ExtChanOffset ;
+		pAddHtInfo->AddHtInfo.RecomWidth = pRtHt->RecomWidth;
+		pAddHtInfo->AddHtInfo2.OperaionMode = pRtHt->OperaionMode;
+		pAddHtInfo->AddHtInfo2.NonGfPresent = pRtHt->NonGfPresent;
+		RTMPMoveMemory(pAddHtInfo->MCSSet, /*pRtHt->MCSSet*/pMcsSet, 4); // rt2860 only support MCS max=32, no need to copy all 16 uchar.
+
+        DBGPRINT(RT_DEBUG_TRACE,("RTMPUpdateHTIE <== \n"));
+}
+#endif // DOT11_N_SUPPORT //
+
+/*
+	========================================================================
+	Description:
+		Add Client security information into ASIC WCID table and IVEIV table.
+    Return:
+	========================================================================
+*/
+VOID	RTMPAddWcidAttributeEntry(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			BssIdx,
+	IN 	UCHAR		 	KeyIdx,
+	IN 	UCHAR		 	CipherAlg,
+	IN 	MAC_TABLE_ENTRY *pEntry)
+{
+	UINT32		WCIDAttri = 0;
+	USHORT		offset;
+	UCHAR		IVEIV = 0;
+	USHORT		Wcid = 0;
+
+	{
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			if (BssIdx > BSS0)
+			{
+				DBGPRINT(RT_DEBUG_ERROR, ("RTMPAddWcidAttributeEntry: The BSS-index(%d) is out of range for Infra link. \n", BssIdx));
+				return;
+			}
+
+			// 1.	In ADHOC mode, the AID is wcid number. And NO mesh link exists.
+			// 2.	In Infra mode, the AID:1 MUST be wcid of infra STA.
+			//					   the AID:2~ assign to mesh link entry.
+			if (pEntry && ADHOC_ON(pAd))
+				Wcid = pEntry->Aid;
+			else if (pEntry && INFRA_ON(pAd))
+			{
+#ifdef QOS_DLS_SUPPORT
+				if (pEntry->ValidAsDls == TRUE)
+					Wcid = pEntry->Aid;
+				else
+#endif // QOS_DLS_SUPPORT //
+				Wcid = BSSID_WCID;
+			}
+			else
+				Wcid = MCAST_WCID;
+		}
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+	// Update WCID attribute table
+	offset = MAC_WCID_ATTRIBUTE_BASE + (Wcid * HW_WCID_ATTRI_SIZE);
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (pEntry && pEntry->ValidAsMesh)
+			WCIDAttri = (CipherAlg<<1) | PAIRWISEKEYTABLE;
+#ifdef QOS_DLS_SUPPORT
+		else if ((pEntry) && (pEntry->ValidAsDls) &&
+					((CipherAlg == CIPHER_TKIP) ||
+				 	(CipherAlg == CIPHER_TKIP_NO_MIC) ||
+					(CipherAlg == CIPHER_AES) ||
+				 	(CipherAlg == CIPHER_NONE)))
+			WCIDAttri = (CipherAlg<<1) | PAIRWISEKEYTABLE;
+#endif // QOS_DLS_SUPPORT //
+		else
+			WCIDAttri = (CipherAlg<<1) | SHAREDKEYTABLE;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	RTMP_IO_WRITE32(pAd, offset, WCIDAttri);
+
+
+	// Update IV/EIV table
+	offset = MAC_IVEIV_TABLE_BASE + (Wcid * HW_IVEIV_ENTRY_SIZE);
+
+	// WPA mode
+	if ((CipherAlg == CIPHER_TKIP) || (CipherAlg == CIPHER_TKIP_NO_MIC) || (CipherAlg == CIPHER_AES))
+	{
+		// Eiv bit on. keyid always is 0 for pairwise key
+		IVEIV = (KeyIdx <<6) | 0x20;
+	}
+	else
+	{
+		// WEP KeyIdx is default tx key.
+		IVEIV = (KeyIdx << 6);
+	}
+
+	// For key index and ext IV bit, so only need to update the position(offset+3).
+#ifdef RT2870
+	RTUSBMultiWrite_OneByte(pAd, offset+3, &IVEIV);
+#endif // RT2870 //
+
+	DBGPRINT(RT_DEBUG_TRACE,("RTMPAddWcidAttributeEntry: WCID #%d, KeyIndex #%d, Alg=%s\n",Wcid, KeyIdx, CipherName[CipherAlg]));
+	DBGPRINT(RT_DEBUG_TRACE,("	WCIDAttri = 0x%x \n",  WCIDAttri));
+
+}
+
+/*
+    ==========================================================================
+    Description:
+        Parse encryption type
+Arguments:
+    pAdapter                    Pointer to our adapter
+    wrq                         Pointer to the ioctl argument
+
+    Return Value:
+        None
+
+    Note:
+    ==========================================================================
+*/
+CHAR *GetEncryptType(CHAR enc)
+{
+    if(enc == Ndis802_11WEPDisabled)
+        return "NONE";
+    if(enc == Ndis802_11WEPEnabled)
+    	return "WEP";
+    if(enc == Ndis802_11Encryption2Enabled)
+    	return "TKIP";
+    if(enc == Ndis802_11Encryption3Enabled)
+    	return "AES";
+	if(enc == Ndis802_11Encryption4Enabled)
+    	return "TKIPAES";
+    else
+    	return "UNKNOW";
+}
+
+CHAR *GetAuthMode(CHAR auth)
+{
+    if(auth == Ndis802_11AuthModeOpen)
+    	return "OPEN";
+    if(auth == Ndis802_11AuthModeShared)
+    	return "SHARED";
+	if(auth == Ndis802_11AuthModeAutoSwitch)
+    	return "AUTOWEP";
+    if(auth == Ndis802_11AuthModeWPA)
+    	return "WPA";
+    if(auth == Ndis802_11AuthModeWPAPSK)
+    	return "WPAPSK";
+    if(auth == Ndis802_11AuthModeWPANone)
+    	return "WPANONE";
+    if(auth == Ndis802_11AuthModeWPA2)
+    	return "WPA2";
+    if(auth == Ndis802_11AuthModeWPA2PSK)
+    	return "WPA2PSK";
+	if(auth == Ndis802_11AuthModeWPA1WPA2)
+    	return "WPA1WPA2";
+	if(auth == Ndis802_11AuthModeWPA1PSKWPA2PSK)
+    	return "WPA1PSKWPA2PSK";
+
+    	return "UNKNOW";
+}
+
+#if 1 //#ifndef UCOS
+/*
+    ==========================================================================
+    Description:
+        Get site survey results
+	Arguments:
+	    pAdapter                    Pointer to our adapter
+	    wrq                         Pointer to the ioctl argument
+
+    Return Value:
+        None
+
+    Note:
+        Usage:
+        		1.) UI needs to wait 4 seconds after issue a site survey command
+        		2.) iwpriv ra0 get_site_survey
+        		3.) UI needs to prepare at least 4096bytes to get the results
+    ==========================================================================
+*/
+#define	LINE_LEN	(4+33+20+8+10+9+7+3)	// Channel+SSID+Bssid+WepStatus+AuthMode+Signal+WiressMode+NetworkType
+VOID RTMPIoctlGetSiteSurvey(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq)
+{
+	CHAR		*msg;
+	INT 		i=0;
+	INT			WaitCnt;
+	INT 		Status=0;
+	CHAR		Ssid[MAX_LEN_OF_SSID +1];
+    INT         Rssi = 0, max_len = LINE_LEN;
+	UINT        Rssi_Quality = 0;
+	NDIS_802_11_NETWORK_TYPE    wireless_mode;
+
+	os_alloc_mem(NULL, (PUCHAR *)&msg, sizeof(CHAR)*((MAX_LEN_OF_BSS_TABLE)*max_len));
+
+	if (msg == NULL)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("RTMPIoctlGetSiteSurvey - msg memory alloc fail.\n"));
+		return;
+	}
+
+	memset(msg, 0 ,(MAX_LEN_OF_BSS_TABLE)*max_len );
+	memset(Ssid, 0 ,(MAX_LEN_OF_SSID +1));
+	sprintf(msg,"%s","\n");
+	sprintf(msg+strlen(msg),"%-4s%-33s%-20s%-8s%-10s%-9s%-7s%-3s\n",
+	    "Ch", "SSID", "BSSID", "Enc", "Auth", "Siganl(%)", "W-Mode", " NT");
+
+
+	WaitCnt = 0;
+#ifdef CONFIG_STA_SUPPORT
+	pAdapter->StaCfg.bScanReqIsFromWebUI = TRUE;
+	while ((ScanRunning(pAdapter) == TRUE) && (WaitCnt++ < 200))
+		OS_WAIT(500);
+#endif // CONFIG_STA_SUPPORT //
+
+	for(i=0; i<pAdapter->ScanTab.BssNr ;i++)
+	{
+		if( pAdapter->ScanTab.BssEntry[i].Channel==0)
+			break;
+
+		if((strlen(msg)+max_len ) >= IW_SCAN_MAX_DATA)
+			break;
+
+		//Channel
+		sprintf(msg+strlen(msg),"%-4d", pAdapter->ScanTab.BssEntry[i].Channel);
+		//SSID
+		memcpy(Ssid, pAdapter->ScanTab.BssEntry[i].Ssid, pAdapter->ScanTab.BssEntry[i].SsidLen);
+		Ssid[pAdapter->ScanTab.BssEntry[i].SsidLen] = '\0';
+		sprintf(msg+strlen(msg),"%-33s", Ssid);
+		//BSSID
+		sprintf(msg+strlen(msg),"%02x:%02x:%02x:%02x:%02x:%02x   ",
+			pAdapter->ScanTab.BssEntry[i].Bssid[0],
+			pAdapter->ScanTab.BssEntry[i].Bssid[1],
+			pAdapter->ScanTab.BssEntry[i].Bssid[2],
+			pAdapter->ScanTab.BssEntry[i].Bssid[3],
+			pAdapter->ScanTab.BssEntry[i].Bssid[4],
+			pAdapter->ScanTab.BssEntry[i].Bssid[5]);
+		//Encryption Type
+		sprintf(msg+strlen(msg),"%-8s",GetEncryptType(pAdapter->ScanTab.BssEntry[i].WepStatus));
+		//Authentication Mode
+		if (pAdapter->ScanTab.BssEntry[i].WepStatus == Ndis802_11WEPEnabled)
+			sprintf(msg+strlen(msg),"%-10s", "UNKNOW");
+		else
+			sprintf(msg+strlen(msg),"%-10s",GetAuthMode(pAdapter->ScanTab.BssEntry[i].AuthMode));
+		// Rssi
+		Rssi = (INT)pAdapter->ScanTab.BssEntry[i].Rssi;
+		if (Rssi >= -50)
+			Rssi_Quality = 100;
+		else if (Rssi >= -80)    // between -50 ~ -80dbm
+			Rssi_Quality = (UINT)(24 + ((Rssi + 80) * 26)/10);
+		else if (Rssi >= -90)   // between -80 ~ -90dbm
+			Rssi_Quality = (UINT)(((Rssi + 90) * 26)/10);
+		else    // < -84 dbm
+			Rssi_Quality = 0;
+		sprintf(msg+strlen(msg),"%-9d", Rssi_Quality);
+		// Wireless Mode
+		wireless_mode = NetworkTypeInUseSanity(&pAdapter->ScanTab.BssEntry[i]);
+		if (wireless_mode == Ndis802_11FH ||
+			wireless_mode == Ndis802_11DS)
+			sprintf(msg+strlen(msg),"%-7s", "11b");
+		else if (wireless_mode == Ndis802_11OFDM5)
+			sprintf(msg+strlen(msg),"%-7s", "11a");
+		else if (wireless_mode == Ndis802_11OFDM5_N)
+			sprintf(msg+strlen(msg),"%-7s", "11a/n");
+		else if (wireless_mode == Ndis802_11OFDM24)
+			sprintf(msg+strlen(msg),"%-7s", "11b/g");
+		else if (wireless_mode == Ndis802_11OFDM24_N)
+			sprintf(msg+strlen(msg),"%-7s", "11b/g/n");
+		else
+			sprintf(msg+strlen(msg),"%-7s", "unknow");
+		//Network Type
+		if (pAdapter->ScanTab.BssEntry[i].BssType == BSS_ADHOC)
+			sprintf(msg+strlen(msg),"%-3s", " Ad");
+		else
+			sprintf(msg+strlen(msg),"%-3s", " In");
+
+        sprintf(msg+strlen(msg),"\n");
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	pAdapter->StaCfg.bScanReqIsFromWebUI = FALSE;
+#endif // CONFIG_STA_SUPPORT //
+	wrq->u.data.length = strlen(msg);
+	Status = copy_to_user(wrq->u.data.pointer, msg, wrq->u.data.length);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPIoctlGetSiteSurvey - wrq->u.data.length = %d\n", wrq->u.data.length));
+	os_free_mem(NULL, (PUCHAR)msg);
+}
+
+
+#define	MAC_LINE_LEN	(14+4+4+10+10+10+6+6)	// Addr+aid+psm+datatime+rxbyte+txbyte+current tx rate+last tx rate
+VOID RTMPIoctlGetMacTable(
+	IN PRTMP_ADAPTER pAd,
+	IN struct iwreq *wrq)
+{
+	INT i;
+	RT_802_11_MAC_TABLE MacTab;
+	char *msg;
+
+	MacTab.Num = 0;
+	for (i=0; i<MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		if (pAd->MacTab.Content[i].ValidAsCLI && (pAd->MacTab.Content[i].Sst == SST_ASSOC))
+		{
+			COPY_MAC_ADDR(MacTab.Entry[MacTab.Num].Addr, &pAd->MacTab.Content[i].Addr);
+			MacTab.Entry[MacTab.Num].Aid = (UCHAR)pAd->MacTab.Content[i].Aid;
+			MacTab.Entry[MacTab.Num].Psm = pAd->MacTab.Content[i].PsMode;
+#ifdef DOT11_N_SUPPORT
+			MacTab.Entry[MacTab.Num].MimoPs = pAd->MacTab.Content[i].MmpsMode;
+#endif // DOT11_N_SUPPORT //
+
+			// Fill in RSSI per entry
+			MacTab.Entry[MacTab.Num].AvgRssi0 = pAd->MacTab.Content[i].RssiSample.AvgRssi0;
+			MacTab.Entry[MacTab.Num].AvgRssi1 = pAd->MacTab.Content[i].RssiSample.AvgRssi1;
+			MacTab.Entry[MacTab.Num].AvgRssi2 = pAd->MacTab.Content[i].RssiSample.AvgRssi2;
+
+			// the connected time per entry
+			MacTab.Entry[MacTab.Num].ConnectedTime = pAd->MacTab.Content[i].StaConnectTime;
+#if 0 // ToDo
+			MacTab.Entry[MacTab.Num].HSCounter.LastDataPacketTime = pAd->MacTab.Content[i].HSCounter.LastDataPacketTime;
+			MacTab.Entry[MacTab.Num].HSCounter.TotalRxByteCount = pAd->MacTab.Content[i].HSCounter.TotalRxByteCount;
+			MacTab.Entry[MacTab.Num].HSCounter.TotalTxByteCount = pAd->MacTab.Content[i].HSCounter.TotalTxByteCount;
+#endif
+			MacTab.Entry[MacTab.Num].TxRate.field.MCS = pAd->MacTab.Content[i].HTPhyMode.field.MCS;
+			MacTab.Entry[MacTab.Num].TxRate.field.BW = pAd->MacTab.Content[i].HTPhyMode.field.BW;
+			MacTab.Entry[MacTab.Num].TxRate.field.ShortGI = pAd->MacTab.Content[i].HTPhyMode.field.ShortGI;
+			MacTab.Entry[MacTab.Num].TxRate.field.STBC = pAd->MacTab.Content[i].HTPhyMode.field.STBC;
+			MacTab.Entry[MacTab.Num].TxRate.field.rsv = pAd->MacTab.Content[i].HTPhyMode.field.rsv;
+			MacTab.Entry[MacTab.Num].TxRate.field.MODE = pAd->MacTab.Content[i].HTPhyMode.field.MODE;
+			MacTab.Entry[MacTab.Num].TxRate.word = pAd->MacTab.Content[i].HTPhyMode.word;
+
+			MacTab.Num += 1;
+		}
+	}
+	wrq->u.data.length = sizeof(RT_802_11_MAC_TABLE);
+	if (copy_to_user(wrq->u.data.pointer, &MacTab, wrq->u.data.length))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("%s: copy_to_user() fail\n", __FUNCTION__));
+	}
+
+	msg = (CHAR *) kmalloc(sizeof(CHAR)*(MAX_LEN_OF_MAC_TABLE*MAC_LINE_LEN), MEM_ALLOC_FLAG);
+	memset(msg, 0 ,MAX_LEN_OF_MAC_TABLE*MAC_LINE_LEN );
+	sprintf(msg,"%s","\n");
+	sprintf(msg+strlen(msg),"%-14s%-4s%-4s%-10s%-10s%-10s%-6s%-6s\n",
+		"MAC", "AID", "PSM", "LDT", "RxB", "TxB","CTxR", "LTxR");
+
+	for (i=0; i<MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		PMAC_TABLE_ENTRY pEntry = &pAd->MacTab.Content[i];
+		if (pEntry->ValidAsCLI && (pEntry->Sst == SST_ASSOC))
+		{
+			if((strlen(msg)+MAC_LINE_LEN ) >= (MAX_LEN_OF_MAC_TABLE*MAC_LINE_LEN) )
+				break;
+			sprintf(msg+strlen(msg),"%02x%02x%02x%02x%02x%02x  ",
+				pEntry->Addr[0], pEntry->Addr[1], pEntry->Addr[2],
+				pEntry->Addr[3], pEntry->Addr[4], pEntry->Addr[5]);
+			sprintf(msg+strlen(msg),"%-4d", (int)pEntry->Aid);
+			sprintf(msg+strlen(msg),"%-4d", (int)pEntry->PsMode);
+			sprintf(msg+strlen(msg),"%-10d",0/*pAd->MacTab.Content[i].HSCounter.LastDataPacketTime*/); // ToDo
+			sprintf(msg+strlen(msg),"%-10d",0/*pAd->MacTab.Content[i].HSCounter.TotalRxByteCount*/); // ToDo
+			sprintf(msg+strlen(msg),"%-10d",0/*pAd->MacTab.Content[i].HSCounter.TotalTxByteCount*/); // ToDo
+			sprintf(msg+strlen(msg),"%-6d",RateIdToMbps[pAd->MacTab.Content[i].CurrTxRate]);
+			sprintf(msg+strlen(msg),"%-6d\n",0/*RateIdToMbps[pAd->MacTab.Content[i].LastTxRate]*/); // ToDo
+		}
+	}
+	// for compatible with old API just do the printk to console
+	//wrq->u.data.length = strlen(msg);
+	//if (copy_to_user(wrq->u.data.pointer, msg, wrq->u.data.length))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("%s", msg));
+	}
+
+	kfree(msg);
+}
+#endif // UCOS //
+
+#ifdef DOT11_N_SUPPORT
+INT	Set_BASetup_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+    UCHAR mac[6], tid;
+	char *token, sepValue[] = ":", DASH = '-';
+	INT i;
+    MAC_TABLE_ENTRY *pEntry;
+
+/*
+	The BASetup inupt string format should be xx:xx:xx:xx:xx:xx-d,
+		=>The six 2 digit hex-decimal number previous are the Mac address,
+		=>The seventh decimal number is the tid value.
+*/
+	//printk("\n%s\n", arg);
+
+	if(strlen(arg) < 19)  //Mac address acceptable format 01:02:03:04:05:06 length 17 plus the "-" and tid value in decimal format.
+		return FALSE;
+
+	token = strchr(arg, DASH);
+	if ((token != NULL) && (strlen(token)>1))
+	{
+		tid = simple_strtol((token+1), 0, 10);
+		if (tid > 15)
+			return FALSE;
+
+		*token = '\0';
+		for (i = 0, token = rstrtok(arg, &sepValue[0]); token; token = rstrtok(NULL, &sepValue[0]), i++)
+		{
+			if((strlen(token) != 2) || (!isxdigit(*token)) || (!isxdigit(*(token+1))))
+				return FALSE;
+			AtoH(token, (PUCHAR)(&mac[i]), 1);
+		}
+		if(i != 6)
+			return FALSE;
+
+		printk("\n%02x:%02x:%02x:%02x:%02x:%02x-%02x\n", mac[0], mac[1],
+				mac[2], mac[3], mac[4], mac[5], tid);
+
+	    pEntry = MacTableLookup(pAd, mac);
+
+    	if (pEntry) {
+        	printk("\nSetup BA Session: Tid = %d\n", tid);
+	        BAOriSessionSetUp(pAd, pEntry, tid, 0, 100, TRUE);
+    	}
+
+		return TRUE;
+	}
+
+	return FALSE;
+
+}
+
+INT	Set_BADecline_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG bBADecline;
+
+	bBADecline = simple_strtol(arg, 0, 10);
+
+	if (bBADecline == 0)
+	{
+		pAd->CommonCfg.bBADecline = FALSE;
+	}
+	else if (bBADecline == 1)
+	{
+		pAd->CommonCfg.bBADecline = TRUE;
+	}
+	else
+	{
+		return FALSE; //Invalid argument
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_BADecline_Proc::(BADecline=%d)\n", pAd->CommonCfg.bBADecline));
+
+	return TRUE;
+}
+
+INT	Set_BAOriTearDown_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+    UCHAR mac[6], tid;
+	char *token, sepValue[] = ":", DASH = '-';
+	INT i;
+    MAC_TABLE_ENTRY *pEntry;
+
+    //printk("\n%s\n", arg);
+/*
+	The BAOriTearDown inupt string format should be xx:xx:xx:xx:xx:xx-d,
+		=>The six 2 digit hex-decimal number previous are the Mac address,
+		=>The seventh decimal number is the tid value.
+*/
+    if(strlen(arg) < 19)  //Mac address acceptable format 01:02:03:04:05:06 length 17 plus the "-" and tid value in decimal format.
+		return FALSE;
+
+	token = strchr(arg, DASH);
+	if ((token != NULL) && (strlen(token)>1))
+	{
+		tid = simple_strtol((token+1), 0, 10);
+		if (tid > NUM_OF_TID)
+			return FALSE;
+
+		*token = '\0';
+		for (i = 0, token = rstrtok(arg, &sepValue[0]); token; token = rstrtok(NULL, &sepValue[0]), i++)
+		{
+			if((strlen(token) != 2) || (!isxdigit(*token)) || (!isxdigit(*(token+1))))
+				return FALSE;
+			AtoH(token, (PUCHAR)(&mac[i]), 1);
+		}
+		if(i != 6)
+			return FALSE;
+
+	    printk("\n%02x:%02x:%02x:%02x:%02x:%02x-%02x", mac[0], mac[1],
+	           mac[2], mac[3], mac[4], mac[5], tid);
+
+	    pEntry = MacTableLookup(pAd, mac);
+
+	    if (pEntry) {
+	        printk("\nTear down Ori BA Session: Tid = %d\n", tid);
+        BAOriSessionTearDown(pAd, pEntry->Aid, tid, FALSE, TRUE);
+	    }
+
+		return TRUE;
+	}
+
+	return FALSE;
+
+}
+
+INT	Set_BARecTearDown_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+    UCHAR mac[6], tid;
+	char *token, sepValue[] = ":", DASH = '-';
+	INT i;
+    MAC_TABLE_ENTRY *pEntry;
+
+    //printk("\n%s\n", arg);
+/*
+	The BARecTearDown inupt string format should be xx:xx:xx:xx:xx:xx-d,
+		=>The six 2 digit hex-decimal number previous are the Mac address,
+		=>The seventh decimal number is the tid value.
+*/
+    if(strlen(arg) < 19)  //Mac address acceptable format 01:02:03:04:05:06 length 17 plus the "-" and tid value in decimal format.
+		return FALSE;
+
+	token = strchr(arg, DASH);
+	if ((token != NULL) && (strlen(token)>1))
+	{
+		tid = simple_strtol((token+1), 0, 10);
+		if (tid > NUM_OF_TID)
+			return FALSE;
+
+		*token = '\0';
+		for (i = 0, token = rstrtok(arg, &sepValue[0]); token; token = rstrtok(NULL, &sepValue[0]), i++)
+		{
+			if((strlen(token) != 2) || (!isxdigit(*token)) || (!isxdigit(*(token+1))))
+				return FALSE;
+			AtoH(token, (PUCHAR)(&mac[i]), 1);
+		}
+		if(i != 6)
+			return FALSE;
+
+		printk("\n%02x:%02x:%02x:%02x:%02x:%02x-%02x", mac[0], mac[1],
+		       mac[2], mac[3], mac[4], mac[5], tid);
+
+		pEntry = MacTableLookup(pAd, mac);
+
+		if (pEntry) {
+		    printk("\nTear down Rec BA Session: Tid = %d\n", tid);
+		    BARecSessionTearDown(pAd, pEntry->Aid, tid, FALSE);
+		}
+
+		return TRUE;
+	}
+
+	return FALSE;
+
+}
+
+INT	Set_HtBw_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG HtBw;
+
+	HtBw = simple_strtol(arg, 0, 10);
+	if (HtBw == BW_40)
+		pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_40;
+	else if (HtBw == BW_20)
+		pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_20;
+	else
+		return FALSE;  //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtBw_Proc::(HtBw=%d)\n", pAd->CommonCfg.RegTransmitSetting.field.BW));
+
+	return TRUE;
+}
+
+INT	Set_HtMcs_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG HtMcs, Mcs_tmp;
+#ifdef CONFIG_STA_SUPPORT
+    BOOLEAN bAutoRate = FALSE;
+#endif // CONFIG_STA_SUPPORT //
+
+	Mcs_tmp = simple_strtol(arg, 0, 10);
+
+	if (Mcs_tmp <= 15 || Mcs_tmp == 32)
+		HtMcs = Mcs_tmp;
+	else
+		HtMcs = MCS_AUTO;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		pAd->StaCfg.DesiredTransmitSetting.field.MCS = HtMcs;
+		pAd->StaCfg.bAutoTxRateSwitch = (HtMcs == MCS_AUTO) ? TRUE:FALSE;
+		DBGPRINT(RT_DEBUG_TRACE, ("Set_HtMcs_Proc::(HtMcs=%d, bAutoTxRateSwitch = %d)\n",
+						pAd->StaCfg.DesiredTransmitSetting.field.MCS, pAd->StaCfg.bAutoTxRateSwitch));
+
+		if ((pAd->CommonCfg.PhyMode < PHY_11ABGN_MIXED) ||
+			(pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE < MODE_HTMIX))
+		{
+	        if ((pAd->StaCfg.DesiredTransmitSetting.field.MCS != MCS_AUTO) &&
+				(HtMcs >= 0 && HtMcs <= 3) &&
+	            (pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode == FIXED_TXMODE_CCK))
+			{
+				RTMPSetDesiredRates(pAd, (LONG) (RateIdToMbps[HtMcs] * 1000000));
+			}
+	        else if ((pAd->StaCfg.DesiredTransmitSetting.field.MCS != MCS_AUTO) &&
+					(HtMcs >= 0 && HtMcs <= 7) &&
+	            	(pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode == FIXED_TXMODE_OFDM))
+			{
+				RTMPSetDesiredRates(pAd, (LONG) (RateIdToMbps[HtMcs+4] * 1000000));
+			}
+			else
+				bAutoRate = TRUE;
+
+			if (bAutoRate)
+			{
+	            pAd->StaCfg.DesiredTransmitSetting.field.MCS = MCS_AUTO;
+				RTMPSetDesiredRates(pAd, -1);
+			}
+	        DBGPRINT(RT_DEBUG_TRACE, ("Set_HtMcs_Proc::(FixedTxMode=%d)\n",pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode));
+		}
+        if (ADHOC_ON(pAd))
+            return TRUE;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	SetCommonHT(pAd);
+
+	return TRUE;
+}
+
+INT	Set_HtGi_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG HtGi;
+
+	HtGi = simple_strtol(arg, 0, 10);
+
+	if ( HtGi == GI_400)
+		pAd->CommonCfg.RegTransmitSetting.field.ShortGI = GI_400;
+	else if ( HtGi == GI_800 )
+		pAd->CommonCfg.RegTransmitSetting.field.ShortGI = GI_800;
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtGi_Proc::(ShortGI=%d)\n",pAd->CommonCfg.RegTransmitSetting.field.ShortGI));
+
+	return TRUE;
+}
+
+
+INT	Set_HtTxBASize_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UCHAR Size;
+
+	Size = simple_strtol(arg, 0, 10);
+
+	if (Size <=0 || Size >=64)
+	{
+		Size = 8;
+	}
+	pAd->CommonCfg.TxBASize = Size-1;
+	DBGPRINT(RT_DEBUG_ERROR, ("Set_HtTxBASize ::(TxBASize= %d)\n", Size));
+
+	return TRUE;
+}
+
+
+INT	Set_HtOpMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+	if (Value == HTMODE_GF)
+		pAd->CommonCfg.RegTransmitSetting.field.HTMODE  = HTMODE_GF;
+	else if ( Value == HTMODE_MM )
+		pAd->CommonCfg.RegTransmitSetting.field.HTMODE  = HTMODE_MM;
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtOpMode_Proc::(HtOpMode=%d)\n",pAd->CommonCfg.RegTransmitSetting.field.HTMODE));
+
+	return TRUE;
+
+}
+
+INT	Set_HtStbc_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+	if (Value == STBC_USE)
+		pAd->CommonCfg.RegTransmitSetting.field.STBC = STBC_USE;
+	else if ( Value == STBC_NONE )
+		pAd->CommonCfg.RegTransmitSetting.field.STBC = STBC_NONE;
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_Stbc_Proc::(HtStbc=%d)\n",pAd->CommonCfg.RegTransmitSetting.field.STBC));
+
+	return TRUE;
+}
+
+INT	Set_HtHtc_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->HTCEnable = FALSE;
+	else if ( Value ==1 )
+        	pAd->HTCEnable = TRUE;
+	else
+		return FALSE; //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtHtc_Proc::(HtHtc=%d)\n",pAd->HTCEnable));
+
+	return TRUE;
+}
+
+INT	Set_HtExtcha_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+	if (Value == 0)
+		pAd->CommonCfg.RegTransmitSetting.field.EXTCHA  = EXTCHA_BELOW;
+	else if ( Value ==1 )
+        pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_ABOVE;
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtExtcha_Proc::(HtExtcha=%d)\n",pAd->CommonCfg.RegTransmitSetting.field.EXTCHA));
+
+	return TRUE;
+}
+
+INT	Set_HtMpduDensity_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+	if (Value <=7 && Value >= 0)
+		pAd->CommonCfg.BACapability.field.MpduDensity = Value;
+	else
+		pAd->CommonCfg.BACapability.field.MpduDensity = 4;
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtMpduDensity_Proc::(HtMpduDensity=%d)\n",pAd->CommonCfg.BACapability.field.MpduDensity));
+
+	return TRUE;
+}
+
+INT	Set_HtBaWinSize_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+
+	if (Value >=1 && Value <= 64)
+	{
+		pAd->CommonCfg.REGBACapability.field.RxBAWinLimit = Value;
+		pAd->CommonCfg.BACapability.field.RxBAWinLimit = Value;
+	}
+	else
+	{
+        pAd->CommonCfg.REGBACapability.field.RxBAWinLimit = 64;
+		pAd->CommonCfg.BACapability.field.RxBAWinLimit = 64;
+	}
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtBaWinSize_Proc::(HtBaWinSize=%d)\n",pAd->CommonCfg.BACapability.field.RxBAWinLimit));
+
+	return TRUE;
+}
+
+INT	Set_HtRdg_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+	if (Value == 0)
+		pAd->CommonCfg.bRdg = FALSE;
+	else if ( Value ==1 )
+	{
+		pAd->HTCEnable = TRUE;
+        	pAd->CommonCfg.bRdg = TRUE;
+	}
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtRdg_Proc::(HtRdg=%d)\n",pAd->CommonCfg.bRdg));
+
+	return TRUE;
+}
+
+INT	Set_HtLinkAdapt_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->bLinkAdapt = FALSE;
+	else if ( Value ==1 )
+	{
+			pAd->HTCEnable = TRUE;
+			pAd->bLinkAdapt = TRUE;
+	}
+	else
+		return FALSE; //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtLinkAdapt_Proc::(HtLinkAdapt=%d)\n",pAd->bLinkAdapt));
+
+	return TRUE;
+}
+
+INT	Set_HtAmsdu_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->CommonCfg.BACapability.field.AmsduEnable = FALSE;
+	else if ( Value == 1 )
+        pAd->CommonCfg.BACapability.field.AmsduEnable = TRUE;
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtAmsdu_Proc::(HtAmsdu=%d)\n",pAd->CommonCfg.BACapability.field.AmsduEnable));
+
+	return TRUE;
+}
+
+INT	Set_HtAutoBa_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->CommonCfg.BACapability.field.AutoBA = FALSE;
+    else if (Value == 1)
+		pAd->CommonCfg.BACapability.field.AutoBA = TRUE;
+	else
+		return FALSE; //Invalid argument
+
+    pAd->CommonCfg.REGBACapability.field.AutoBA = pAd->CommonCfg.BACapability.field.AutoBA;
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtAutoBa_Proc::(HtAutoBa=%d)\n",pAd->CommonCfg.BACapability.field.AutoBA));
+
+	return TRUE;
+
+}
+
+INT	Set_HtProtect_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->CommonCfg.bHTProtect = FALSE;
+    else if (Value == 1)
+		pAd->CommonCfg.bHTProtect = TRUE;
+	else
+		return FALSE; //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtProtect_Proc::(HtProtect=%d)\n",pAd->CommonCfg.bHTProtect));
+
+	return TRUE;
+}
+
+INT	Set_SendPSMPAction_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+    UCHAR mac[6], mode;
+	char *token, sepValue[] = ":", DASH = '-';
+	INT i;
+    MAC_TABLE_ENTRY *pEntry;
+
+    //printk("\n%s\n", arg);
+/*
+	The BARecTearDown inupt string format should be xx:xx:xx:xx:xx:xx-d,
+		=>The six 2 digit hex-decimal number previous are the Mac address,
+		=>The seventh decimal number is the mode value.
+*/
+    if(strlen(arg) < 19)  //Mac address acceptable format 01:02:03:04:05:06 length 17 plus the "-" and mode value in decimal format.
+		return FALSE;
+
+   	token = strchr(arg, DASH);
+	if ((token != NULL) && (strlen(token)>1))
+	{
+		mode = simple_strtol((token+1), 0, 10);
+		if (mode > MMPS_ENABLE)
+			return FALSE;
+
+		*token = '\0';
+		for (i = 0, token = rstrtok(arg, &sepValue[0]); token; token = rstrtok(NULL, &sepValue[0]), i++)
+		{
+			if((strlen(token) != 2) || (!isxdigit(*token)) || (!isxdigit(*(token+1))))
+				return FALSE;
+			AtoH(token, (PUCHAR)(&mac[i]), 1);
+		}
+		if(i != 6)
+			return FALSE;
+
+		printk("\n%02x:%02x:%02x:%02x:%02x:%02x-%02x", mac[0], mac[1],
+		       mac[2], mac[3], mac[4], mac[5], mode);
+
+		pEntry = MacTableLookup(pAd, mac);
+
+		if (pEntry) {
+		    printk("\nSendPSMPAction MIPS mode = %d\n", mode);
+		    SendPSMPAction(pAd, pEntry->Aid, mode);
+		}
+
+		return TRUE;
+	}
+
+	return FALSE;
+
+
+}
+
+INT	Set_HtMIMOPSmode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+	if (Value <=3 && Value >= 0)
+		pAd->CommonCfg.BACapability.field.MMPSmode = Value;
+	else
+		pAd->CommonCfg.BACapability.field.MMPSmode = 3;
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtMIMOPSmode_Proc::(MIMOPS mode=%d)\n",pAd->CommonCfg.BACapability.field.MMPSmode));
+
+	return TRUE;
+}
+
+
+INT	Set_ForceShortGI_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->WIFItestbed.bShortGI = FALSE;
+	else if (Value == 1)
+		pAd->WIFItestbed.bShortGI = TRUE;
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_ForceShortGI_Proc::(ForceShortGI=%d)\n", pAd->WIFItestbed.bShortGI));
+
+	return TRUE;
+}
+
+
+
+INT	Set_ForceGF_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->WIFItestbed.bGreenField = FALSE;
+	else if (Value == 1)
+		pAd->WIFItestbed.bGreenField = TRUE;
+	else
+		return FALSE; //Invalid argument
+
+	SetCommonHT(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_ForceGF_Proc::(ForceGF=%d)\n", pAd->WIFItestbed.bGreenField));
+
+	return TRUE;
+}
+
+INT	Set_HtMimoPs_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+	if (Value == 0)
+		pAd->CommonCfg.bMIMOPSEnable = FALSE;
+	else if (Value == 1)
+		pAd->CommonCfg.bMIMOPSEnable = TRUE;
+	else
+		return FALSE; //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_HtMimoPs_Proc::(HtMimoPs=%d)\n",pAd->CommonCfg.bMIMOPSEnable));
+
+	return TRUE;
+}
+#endif // DOT11_N_SUPPORT //
+
+
+#ifdef DOT11_N_SUPPORT
+INT	SetCommonHT(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	OID_SET_HT_PHYMODE		SetHT;
+
+	if (pAd->CommonCfg.PhyMode < PHY_11ABGN_MIXED)
+		return FALSE;
+
+	SetHT.PhyMode = pAd->CommonCfg.PhyMode;
+	SetHT.TransmitNo = ((UCHAR)pAd->Antenna.field.TxPath);
+	SetHT.HtMode = (UCHAR)pAd->CommonCfg.RegTransmitSetting.field.HTMODE;
+	SetHT.ExtOffset = (UCHAR)pAd->CommonCfg.RegTransmitSetting.field.EXTCHA;
+	SetHT.MCS = MCS_AUTO;
+	SetHT.BW = (UCHAR)pAd->CommonCfg.RegTransmitSetting.field.BW;
+	SetHT.STBC = (UCHAR)pAd->CommonCfg.RegTransmitSetting.field.STBC;
+	SetHT.SHORTGI = (UCHAR)pAd->CommonCfg.RegTransmitSetting.field.ShortGI;
+
+	RTMPSetHT(pAd, &SetHT);
+
+	return TRUE;
+}
+#endif // DOT11_N_SUPPORT //
+
+INT	Set_FixedTxMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UCHAR	fix_tx_mode = FIXED_TXMODE_HT;
+
+	if (strcmp(arg, "OFDM") == 0 || strcmp(arg, "ofdm") == 0)
+	{
+		fix_tx_mode = FIXED_TXMODE_OFDM;
+	}
+	else if (strcmp(arg, "CCK") == 0 || strcmp(arg, "cck") == 0)
+	{
+        fix_tx_mode = FIXED_TXMODE_CCK;
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode = fix_tx_mode;
+#endif // CONFIG_STA_SUPPORT //
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_FixedTxMode_Proc::(FixedTxMode=%d)\n", fix_tx_mode));
+
+	return TRUE;
+}
+
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+INT	Set_OpMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+#ifdef RT2870
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_START_UP))
+#endif // RT2870 //
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Can not switch operate mode on interface up !! \n"));
+		return FALSE;
+	}
+
+	if (Value == 0)
+		pAd->OpMode = OPMODE_STA;
+	else if (Value == 1)
+		pAd->OpMode = OPMODE_AP;
+	else
+		return FALSE; //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_OpMode_Proc::(OpMode=%s)\n", pAd->OpMode == 1 ? "AP Mode" : "STA Mode"));
+
+	return TRUE;
+}
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+
+
+/////////////////////////////////////////////////////////////////////////
+PCHAR   RTMPGetRalinkAuthModeStr(
+    IN  NDIS_802_11_AUTHENTICATION_MODE authMode)
+{
+	switch(authMode)
+	{
+		case Ndis802_11AuthModeOpen:
+			return "OPEN";
+		case Ndis802_11AuthModeWPAPSK:
+			return "WPAPSK";
+		case Ndis802_11AuthModeShared:
+			return "SHARED";
+		case Ndis802_11AuthModeWPA:
+			return "WPA";
+		case Ndis802_11AuthModeWPA2:
+			return "WPA2";
+		case Ndis802_11AuthModeWPA2PSK:
+			return "WPA2PSK";
+        case Ndis802_11AuthModeWPA1PSKWPA2PSK:
+			return "WPAPSKWPA2PSK";
+        case Ndis802_11AuthModeWPA1WPA2:
+			return "WPA1WPA2";
+		case Ndis802_11AuthModeWPANone:
+			return "WPANONE";
+		default:
+			return "UNKNOW";
+	}
+}
+
+PCHAR   RTMPGetRalinkEncryModeStr(
+    IN  USHORT encryMode)
+{
+	switch(encryMode)
+	{
+		case Ndis802_11WEPDisabled:
+			return "NONE";
+		case Ndis802_11WEPEnabled:
+			return "WEP";
+		case Ndis802_11Encryption2Enabled:
+			return "TKIP";
+		case Ndis802_11Encryption3Enabled:
+			return "AES";
+        case Ndis802_11Encryption4Enabled:
+			return "TKIPAES";
+		default:
+			return "UNKNOW";
+	}
+}
+
+INT RTMPShowCfgValue(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pName,
+	IN	PUCHAR			pBuf)
+{
+	INT	Status = 0;
+
+	for (PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC = RTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC; PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC->name; PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC++)
+	{
+		if (!strcmp(pName, PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC->name))
+		{
+			if(PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC->show_proc(pAd, pBuf))
+				Status = -EINVAL;
+			break;  //Exit for loop.
+		}
+	}
+
+	if(PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC->name == NULL)
+	{
+		sprintf(pBuf, "\n");
+		for (PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC = RTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC; PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC->name; PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC++)
+			sprintf(pBuf, "%s%s\n", pBuf, PRTMP_PRIVATE_STA_SHOW_CFG_VALUE_PROC->name);
+	}
+
+	return Status;
+}
+
+INT	Show_SSID_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		sprintf(pBuf, "\t%s", pAd->CommonCfg.Ssid);
+#endif // CONFIG_STA_SUPPORT //
+	return 0;
+}
+
+INT	Show_WirelessMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	switch(pAd->CommonCfg.PhyMode)
+	{
+		case PHY_11BG_MIXED:
+			sprintf(pBuf, "\t11B/G");
+			break;
+		case PHY_11B:
+			sprintf(pBuf, "\t11B");
+			break;
+		case PHY_11A:
+			sprintf(pBuf, "\t11A");
+			break;
+		case PHY_11ABG_MIXED:
+			sprintf(pBuf, "\t11A/B/G");
+			break;
+		case PHY_11G:
+			sprintf(pBuf, "\t11G");
+			break;
+#ifdef DOT11_N_SUPPORT
+		case PHY_11ABGN_MIXED:
+			sprintf(pBuf, "\t11A/B/G/N");
+			break;
+		case PHY_11N_2_4G:
+			sprintf(pBuf, "\t11N only with 2.4G");
+			break;
+		case PHY_11GN_MIXED:
+			sprintf(pBuf, "\t11G/N");
+			break;
+		case PHY_11AN_MIXED:
+			sprintf(pBuf, "\t11A/N");
+			break;
+		case PHY_11BGN_MIXED:
+			sprintf(pBuf, "\t11B/G/N");
+			break;
+		case PHY_11AGN_MIXED:
+			sprintf(pBuf, "\t11A/G/N");
+			break;
+		case PHY_11N_5G:
+			sprintf(pBuf, "\t11N only with 5G");
+			break;
+#endif // DOT11_N_SUPPORT //
+		default:
+			sprintf(pBuf, "\tUnknow Value(%d)", pAd->CommonCfg.PhyMode);
+			break;
+	}
+	return 0;
+}
+
+
+INT	Show_TxBurst_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%s", pAd->CommonCfg.bEnableTxBurst ? "TRUE":"FALSE");
+	return 0;
+}
+
+INT	Show_TxPreamble_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	switch(pAd->CommonCfg.TxPreamble)
+	{
+		case Rt802_11PreambleShort:
+			sprintf(pBuf, "\tShort");
+			break;
+		case Rt802_11PreambleLong:
+			sprintf(pBuf, "\tLong");
+			break;
+		case Rt802_11PreambleAuto:
+			sprintf(pBuf, "\tAuto");
+			break;
+		default:
+			sprintf(pBuf, "\tUnknow Value(%lu)", pAd->CommonCfg.TxPreamble);
+			break;
+	}
+
+	return 0;
+}
+
+INT	Show_TxPower_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%lu", pAd->CommonCfg.TxPowerPercentage);
+	return 0;
+}
+
+INT	Show_Channel_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%d", pAd->CommonCfg.Channel);
+	return 0;
+}
+
+INT	Show_BGProtection_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	switch(pAd->CommonCfg.UseBGProtection)
+	{
+		case 1: //Always On
+			sprintf(pBuf, "\tON");
+			break;
+		case 2: //Always OFF
+			sprintf(pBuf, "\tOFF");
+			break;
+		case 0: //AUTO
+			sprintf(pBuf, "\tAuto");
+			break;
+		default:
+			sprintf(pBuf, "\tUnknow Value(%lu)", pAd->CommonCfg.UseBGProtection);
+			break;
+	}
+	return 0;
+}
+
+INT	Show_RTSThreshold_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%u", pAd->CommonCfg.RtsThreshold);
+	return 0;
+}
+
+INT	Show_FragThreshold_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%u", pAd->CommonCfg.FragmentThreshold);
+	return 0;
+}
+
+#ifdef DOT11_N_SUPPORT
+INT	Show_HtBw_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	if (pAd->CommonCfg.RegTransmitSetting.field.BW == BW_40)
+	{
+		sprintf(pBuf, "\t40 MHz");
+	}
+	else
+	{
+        sprintf(pBuf, "\t20 MHz");
+	}
+	return 0;
+}
+
+INT	Show_HtMcs_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		sprintf(pBuf, "\t%u", pAd->StaCfg.DesiredTransmitSetting.field.MCS);
+#endif // CONFIG_STA_SUPPORT //
+	return 0;
+}
+
+INT	Show_HtGi_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	switch(pAd->CommonCfg.RegTransmitSetting.field.ShortGI)
+	{
+		case GI_400:
+			sprintf(pBuf, "\tGI_400");
+			break;
+		case GI_800:
+			sprintf(pBuf, "\tGI_800");
+			break;
+		default:
+			sprintf(pBuf, "\tUnknow Value(%u)", pAd->CommonCfg.RegTransmitSetting.field.ShortGI);
+			break;
+	}
+	return 0;
+}
+
+INT	Show_HtOpMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	switch(pAd->CommonCfg.RegTransmitSetting.field.HTMODE)
+	{
+		case HTMODE_GF:
+			sprintf(pBuf, "\tGF");
+			break;
+		case HTMODE_MM:
+			sprintf(pBuf, "\tMM");
+			break;
+		default:
+			sprintf(pBuf, "\tUnknow Value(%u)", pAd->CommonCfg.RegTransmitSetting.field.HTMODE);
+			break;
+	}
+	return 0;
+}
+
+INT	Show_HtExtcha_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	switch(pAd->CommonCfg.RegTransmitSetting.field.EXTCHA)
+	{
+		case EXTCHA_BELOW:
+			sprintf(pBuf, "\tBelow");
+			break;
+		case EXTCHA_ABOVE:
+			sprintf(pBuf, "\tAbove");
+			break;
+		default:
+			sprintf(pBuf, "\tUnknow Value(%u)", pAd->CommonCfg.RegTransmitSetting.field.EXTCHA);
+			break;
+	}
+	return 0;
+}
+
+
+INT	Show_HtMpduDensity_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%u", pAd->CommonCfg.BACapability.field.MpduDensity);
+	return 0;
+}
+
+INT	Show_HtBaWinSize_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%u", pAd->CommonCfg.BACapability.field.RxBAWinLimit);
+	return 0;
+}
+
+INT	Show_HtRdg_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%s", pAd->CommonCfg.bRdg ? "TRUE":"FALSE");
+	return 0;
+}
+
+INT	Show_HtAmsdu_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%s", pAd->CommonCfg.BACapability.field.AmsduEnable ? "TRUE":"FALSE");
+	return 0;
+}
+
+INT	Show_HtAutoBa_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%s", pAd->CommonCfg.BACapability.field.AutoBA ? "TRUE":"FALSE");
+	return 0;
+}
+#endif // DOT11_N_SUPPORT //
+
+INT	Show_CountryRegion_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%d", pAd->CommonCfg.CountryRegion);
+	return 0;
+}
+
+INT	Show_CountryRegionABand_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%d", pAd->CommonCfg.CountryRegionForABand);
+	return 0;
+}
+
+INT	Show_CountryCode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%s", pAd->CommonCfg.CountryCode);
+	return 0;
+}
+
+#ifdef AGGREGATION_SUPPORT
+INT	Show_PktAggregate_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%s", pAd->CommonCfg.bAggregationCapable ? "TRUE":"FALSE");
+	return 0;
+}
+#endif // AGGREGATION_SUPPORT //
+
+#ifdef WMM_SUPPORT
+INT	Show_WmmCapable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		sprintf(pBuf, "\t%s", pAd->CommonCfg.bWmmCapable ? "TRUE":"FALSE");
+#endif // CONFIG_STA_SUPPORT //
+
+	return 0;
+}
+#endif // WMM_SUPPORT //
+
+INT	Show_IEEE80211H_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	sprintf(pBuf, "\t%s", pAd->CommonCfg.bIEEE80211H ? "TRUE":"FALSE");
+	return 0;
+}
+
+#ifdef CONFIG_STA_SUPPORT
+INT	Show_NetworkType_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	switch(pAd->StaCfg.BssType)
+	{
+		case BSS_ADHOC:
+			sprintf(pBuf, "\tAdhoc");
+			break;
+		case BSS_INFRA:
+			sprintf(pBuf, "\tInfra");
+			break;
+		case BSS_ANY:
+			sprintf(pBuf, "\tAny");
+			break;
+		case BSS_MONITOR:
+			sprintf(pBuf, "\tMonitor");
+			break;
+		default:
+			sprintf(pBuf, "\tUnknow Value(%d)", pAd->StaCfg.BssType);
+			break;
+	}
+	return 0;
+}
+#endif // CONFIG_STA_SUPPORT //
+
+INT	Show_AuthMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	NDIS_802_11_AUTHENTICATION_MODE	AuthMode = Ndis802_11AuthModeOpen;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		AuthMode = pAd->StaCfg.AuthMode;
+#endif // CONFIG_STA_SUPPORT //
+
+	if ((AuthMode >= Ndis802_11AuthModeOpen) &&
+		(AuthMode <= Ndis802_11AuthModeWPA1PSKWPA2PSK))
+		sprintf(pBuf, "\t%s", RTMPGetRalinkAuthModeStr(AuthMode));
+	else
+		sprintf(pBuf, "\tUnknow Value(%d)", AuthMode);
+
+	return 0;
+}
+
+INT	Show_EncrypType_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	NDIS_802_11_WEP_STATUS	WepStatus = Ndis802_11WEPDisabled;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		WepStatus = pAd->StaCfg.WepStatus;
+#endif // CONFIG_STA_SUPPORT //
+
+	if ((WepStatus >= Ndis802_11WEPEnabled) &&
+		(WepStatus <= Ndis802_11Encryption4KeyAbsent))
+		sprintf(pBuf, "\t%s", RTMPGetRalinkEncryModeStr(WepStatus));
+	else
+		sprintf(pBuf, "\tUnknow Value(%d)", WepStatus);
+
+	return 0;
+}
+
+INT	Show_DefaultKeyID_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	UCHAR DefaultKeyId = 0;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		DefaultKeyId = pAd->StaCfg.DefaultKeyId;
+#endif // CONFIG_STA_SUPPORT //
+
+	sprintf(pBuf, "\t%d", DefaultKeyId);
+
+	return 0;
+}
+
+INT	Show_WepKey_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN  INT				KeyIdx,
+	OUT	PUCHAR			pBuf)
+{
+	UCHAR   Key[16] = {0}, KeyLength = 0;
+	INT		index = BSS0;
+
+	KeyLength = pAd->SharedKey[index][KeyIdx].KeyLen;
+	NdisMoveMemory(Key, pAd->SharedKey[index][KeyIdx].Key, KeyLength);
+
+	//check key string is ASCII or not
+    if (RTMPCheckStrPrintAble(Key, KeyLength))
+        sprintf(pBuf, "\t%s", Key);
+    else
+    {
+        int idx;
+        sprintf(pBuf, "\t");
+        for (idx = 0; idx < KeyLength; idx++)
+            sprintf(pBuf+strlen(pBuf), "%02X", Key[idx]);
+    }
+	return 0;
+}
+
+INT	Show_Key1_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	Show_WepKey_Proc(pAd, 0, pBuf);
+	return 0;
+}
+
+INT	Show_Key2_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	Show_WepKey_Proc(pAd, 1, pBuf);
+	return 0;
+}
+
+INT	Show_Key3_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	Show_WepKey_Proc(pAd, 2, pBuf);
+	return 0;
+}
+
+INT	Show_Key4_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	Show_WepKey_Proc(pAd, 3, pBuf);
+	return 0;
+}
+
+INT	Show_WPAPSK_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUCHAR			pBuf)
+{
+	INT 	idx;
+	UCHAR	PMK[32] = {0};
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		NdisMoveMemory(PMK, pAd->StaCfg.PMK, 32);
+#endif // CONFIG_STA_SUPPORT //
+
+    sprintf(pBuf, "\tPMK = ");
+    for (idx = 0; idx < 32; idx++)
+        sprintf(pBuf+strlen(pBuf), "%02X", PMK[idx]);
+
+	return 0;
+}
+
diff --git a/drivers/staging/rt2870/common/cmm_sanity.c b/drivers/staging/rt2870/common/cmm_sanity.c
new file mode 100644
index 00000000000..1e24320177b
--- /dev/null
+++ b/drivers/staging/rt2870/common/cmm_sanity.c
@@ -0,0 +1,1663 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	sanity.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John Chang  2004-09-01      add WMM support
+*/
+#include "../rt_config.h"
+
+
+extern UCHAR	CISCO_OUI[];
+
+extern UCHAR	WPA_OUI[];
+extern UCHAR	RSN_OUI[];
+extern UCHAR	WME_INFO_ELEM[];
+extern UCHAR	WME_PARM_ELEM[];
+extern UCHAR	Ccx2QosInfo[];
+extern UCHAR	RALINK_OUI[];
+extern UCHAR	BROADCOM_OUI[];
+extern UCHAR    WPS_OUI[];
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN MlmeAddBAReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2)
+{
+    PMLME_ADDBA_REQ_STRUCT   pInfo;
+
+    pInfo = (MLME_ADDBA_REQ_STRUCT *)Msg;
+
+    if ((MsgLen != sizeof(MLME_ADDBA_REQ_STRUCT)))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("MlmeAddBAReqSanity fail - message lenght not correct.\n"));
+        return FALSE;
+    }
+
+    if ((pInfo->Wcid >= MAX_LEN_OF_MAC_TABLE))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("MlmeAddBAReqSanity fail - The peer Mac is not associated yet.\n"));
+        return FALSE;
+    }
+
+	/*
+    if ((pInfo->BaBufSize > MAX_RX_REORDERBUF) || (pInfo->BaBufSize < 2))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("MlmeAddBAReqSanity fail - Rx Reordering buffer too big or too small\n"));
+        return FALSE;
+    }
+	*/
+
+    if ((pInfo->pAddr[0]&0x01) == 0x01)
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("MlmeAddBAReqSanity fail - broadcast address not support BA\n"));
+        return FALSE;
+    }
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN MlmeDelBAReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen)
+{
+	MLME_DELBA_REQ_STRUCT *pInfo;
+	pInfo = (MLME_DELBA_REQ_STRUCT *)Msg;
+
+    if ((MsgLen != sizeof(MLME_DELBA_REQ_STRUCT)))
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("MlmeDelBAReqSanity fail - message lenght not correct.\n"));
+        return FALSE;
+    }
+
+    if ((pInfo->Wcid >= MAX_LEN_OF_MAC_TABLE))
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("MlmeDelBAReqSanity fail - The peer Mac is not associated yet.\n"));
+        return FALSE;
+    }
+
+    if ((pInfo->TID & 0xf0))
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("MlmeDelBAReqSanity fail - The peer TID is incorrect.\n"));
+        return FALSE;
+    }
+
+	if (NdisEqualMemory(pAd->MacTab.Content[pInfo->Wcid].Addr, pInfo->Addr, MAC_ADDR_LEN) == 0)
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("MlmeDelBAReqSanity fail - the peer addr dosen't exist.\n"));
+        return FALSE;
+    }
+
+    return TRUE;
+}
+
+BOOLEAN PeerAddBAReqActionSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *pMsg,
+    IN ULONG MsgLen,
+	OUT PUCHAR pAddr2)
+{
+	PFRAME_802_11 pFrame = (PFRAME_802_11)pMsg;
+	PFRAME_ADDBA_REQ pAddFrame;
+	pAddFrame = (PFRAME_ADDBA_REQ)(pMsg);
+	if (MsgLen < (sizeof(FRAME_ADDBA_REQ)))
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("PeerAddBAReqActionSanity: ADDBA Request frame length size = %ld incorrect\n", MsgLen));
+		return FALSE;
+	}
+	// we support immediate BA.
+	*(USHORT *)(&pAddFrame->BaParm) = cpu2le16(*(USHORT *)(&pAddFrame->BaParm));
+	pAddFrame->TimeOutValue = cpu2le16(pAddFrame->TimeOutValue);
+	pAddFrame->BaStartSeq.word = cpu2le16(pAddFrame->BaStartSeq.word);
+
+	if (pAddFrame->BaParm.BAPolicy != IMMED_BA)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("PeerAddBAReqActionSanity: ADDBA Request Ba Policy[%d] not support\n", pAddFrame->BaParm.BAPolicy));
+		DBGPRINT(RT_DEBUG_ERROR,("ADDBA Request. tid=%x, Bufsize=%x, AMSDUSupported=%x \n", pAddFrame->BaParm.TID, pAddFrame->BaParm.BufSize, pAddFrame->BaParm.AMSDUSupported));
+		return FALSE;
+	}
+
+	// we support immediate BA.
+	if (pAddFrame->BaParm.TID &0xfff0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("PeerAddBAReqActionSanity: ADDBA Request incorrect TID = %d\n", pAddFrame->BaParm.TID));
+		return FALSE;
+	}
+	COPY_MAC_ADDR(pAddr2, pFrame->Hdr.Addr2);
+	return TRUE;
+}
+
+BOOLEAN PeerAddBARspActionSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *pMsg,
+    IN ULONG MsgLen)
+{
+	//PFRAME_802_11 pFrame = (PFRAME_802_11)pMsg;
+	PFRAME_ADDBA_RSP pAddFrame;
+
+	pAddFrame = (PFRAME_ADDBA_RSP)(pMsg);
+	if (MsgLen < (sizeof(FRAME_ADDBA_RSP)))
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("PeerAddBARspActionSanity: ADDBA Response frame length size = %ld incorrect\n", MsgLen));
+		return FALSE;
+	}
+	// we support immediate BA.
+	*(USHORT *)(&pAddFrame->BaParm) = cpu2le16(*(USHORT *)(&pAddFrame->BaParm));
+	pAddFrame->StatusCode = cpu2le16(pAddFrame->StatusCode);
+	pAddFrame->TimeOutValue = cpu2le16(pAddFrame->TimeOutValue);
+
+	if (pAddFrame->BaParm.BAPolicy != IMMED_BA)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("PeerAddBAReqActionSanity: ADDBA Response Ba Policy[%d] not support\n", pAddFrame->BaParm.BAPolicy));
+		return FALSE;
+	}
+
+	// we support immediate BA.
+	if (pAddFrame->BaParm.TID &0xfff0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("PeerAddBARspActionSanity: ADDBA Response incorrect TID = %d\n", pAddFrame->BaParm.TID));
+		return FALSE;
+	}
+	return TRUE;
+
+}
+
+BOOLEAN PeerDelBAActionSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN UCHAR Wcid,
+    IN VOID *pMsg,
+    IN ULONG MsgLen )
+{
+	//PFRAME_802_11 pFrame = (PFRAME_802_11)pMsg;
+	PFRAME_DELBA_REQ  pDelFrame;
+	if (MsgLen != (sizeof(FRAME_DELBA_REQ)))
+		return FALSE;
+
+	if (Wcid >= MAX_LEN_OF_MAC_TABLE)
+		return FALSE;
+
+	pDelFrame = (PFRAME_DELBA_REQ)(pMsg);
+
+	*(USHORT *)(&pDelFrame->DelbaParm) = cpu2le16(*(USHORT *)(&pDelFrame->DelbaParm));
+	pDelFrame->ReasonCode = cpu2le16(pDelFrame->ReasonCode);
+
+	if (pDelFrame->DelbaParm.TID &0xfff0)
+		return FALSE;
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN PeerBeaconAndProbeRspSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    IN UCHAR  MsgChannel,
+    OUT PUCHAR pAddr2,
+    OUT PUCHAR pBssid,
+    OUT CHAR Ssid[],
+    OUT UCHAR *pSsidLen,
+    OUT UCHAR *pBssType,
+    OUT USHORT *pBeaconPeriod,
+    OUT UCHAR *pChannel,
+    OUT UCHAR *pNewChannel,
+    OUT LARGE_INTEGER *pTimestamp,
+    OUT CF_PARM *pCfParm,
+    OUT USHORT *pAtimWin,
+    OUT USHORT *pCapabilityInfo,
+    OUT UCHAR *pErp,
+    OUT UCHAR *pDtimCount,
+    OUT UCHAR *pDtimPeriod,
+    OUT UCHAR *pBcastFlag,
+    OUT UCHAR *pMessageToMe,
+    OUT UCHAR SupRate[],
+    OUT UCHAR *pSupRateLen,
+    OUT UCHAR ExtRate[],
+    OUT UCHAR *pExtRateLen,
+    OUT	UCHAR *pCkipFlag,
+    OUT	UCHAR *pAironetCellPowerLimit,
+    OUT PEDCA_PARM       pEdcaParm,
+    OUT PQBSS_LOAD_PARM  pQbssLoad,
+    OUT PQOS_CAPABILITY_PARM pQosCapability,
+    OUT ULONG *pRalinkIe,
+    OUT UCHAR		 *pHtCapabilityLen,
+#ifdef CONFIG_STA_SUPPORT
+    OUT UCHAR		 *pPreNHtCapabilityLen,
+#endif // CONFIG_STA_SUPPORT //
+    OUT HT_CAPABILITY_IE *pHtCapability,
+	OUT UCHAR		 *AddHtInfoLen,
+	OUT ADD_HT_INFO_IE *AddHtInfo,
+	OUT UCHAR *NewExtChannelOffset,		// Ht extension channel offset(above or below)
+    OUT USHORT *LengthVIE,
+    OUT	PNDIS_802_11_VARIABLE_IEs pVIE)
+{
+    CHAR				*Ptr;
+#ifdef CONFIG_STA_SUPPORT
+	CHAR 				TimLen;
+#endif // CONFIG_STA_SUPPORT //
+    PFRAME_802_11		pFrame;
+    PEID_STRUCT         pEid;
+    UCHAR				SubType;
+    UCHAR				Sanity;
+    //UCHAR				ECWMin, ECWMax;
+    //MAC_CSR9_STRUC		Csr9;
+    ULONG				Length = 0;
+
+	// For some 11a AP which didn't have DS_IE, we use two conditions to decide the channel
+	//	1. If the AP is 11n enabled, then check the control channel.
+	//	2. If the AP didn't have any info about channel, use the channel we received this frame as the channel. (May inaccuracy!!)
+	UCHAR			CtrlChannel = 0;
+
+    // Add for 3 necessary EID field check
+    Sanity = 0;
+
+    *pAtimWin = 0;
+    *pErp = 0;
+    *pDtimCount = 0;
+    *pDtimPeriod = 0;
+    *pBcastFlag = 0;
+    *pMessageToMe = 0;
+    *pExtRateLen = 0;
+    *pCkipFlag = 0;			        // Default of CkipFlag is 0
+    *pAironetCellPowerLimit = 0xFF;  // Default of AironetCellPowerLimit is 0xFF
+    *LengthVIE = 0;					// Set the length of VIE to init value 0
+    *pHtCapabilityLen = 0;					// Set the length of VIE to init value 0
+#ifdef CONFIG_STA_SUPPORT
+	if (pAd->OpMode == OPMODE_STA)
+		*pPreNHtCapabilityLen = 0;					// Set the length of VIE to init value 0
+#endif // CONFIG_STA_SUPPORT //
+    *AddHtInfoLen = 0;					// Set the length of VIE to init value 0
+    *pRalinkIe = 0;
+    *pNewChannel = 0;
+    *NewExtChannelOffset = 0xff;	//Default 0xff means no such IE
+    pCfParm->bValid = FALSE;        // default: no IE_CF found
+    pQbssLoad->bValid = FALSE;      // default: no IE_QBSS_LOAD found
+    pEdcaParm->bValid = FALSE;      // default: no IE_EDCA_PARAMETER found
+    pQosCapability->bValid = FALSE; // default: no IE_QOS_CAPABILITY found
+
+    pFrame = (PFRAME_802_11)Msg;
+
+    // get subtype from header
+    SubType = (UCHAR)pFrame->Hdr.FC.SubType;
+
+    // get Addr2 and BSSID from header
+    COPY_MAC_ADDR(pAddr2, pFrame->Hdr.Addr2);
+    COPY_MAC_ADDR(pBssid, pFrame->Hdr.Addr3);
+
+//	hex_dump("Beacon", Msg, MsgLen);
+
+    Ptr = pFrame->Octet;
+    Length += LENGTH_802_11;
+
+    // get timestamp from payload and advance the pointer
+    NdisMoveMemory(pTimestamp, Ptr, TIMESTAMP_LEN);
+
+	pTimestamp->u.LowPart = cpu2le32(pTimestamp->u.LowPart);
+	pTimestamp->u.HighPart = cpu2le32(pTimestamp->u.HighPart);
+
+    Ptr += TIMESTAMP_LEN;
+    Length += TIMESTAMP_LEN;
+
+    // get beacon interval from payload and advance the pointer
+    NdisMoveMemory(pBeaconPeriod, Ptr, 2);
+    Ptr += 2;
+    Length += 2;
+
+    // get capability info from payload and advance the pointer
+    NdisMoveMemory(pCapabilityInfo, Ptr, 2);
+    Ptr += 2;
+    Length += 2;
+
+    if (CAP_IS_ESS_ON(*pCapabilityInfo))
+        *pBssType = BSS_INFRA;
+    else
+        *pBssType = BSS_ADHOC;
+
+    pEid = (PEID_STRUCT) Ptr;
+
+    // get variable fields from payload and advance the pointer
+    while ((Length + 2 + pEid->Len) <= MsgLen)
+    {
+        //
+        // Secure copy VIE to VarIE[MAX_VIE_LEN] didn't overflow.
+        //
+        if ((*LengthVIE + pEid->Len + 2) >= MAX_VIE_LEN)
+        {
+            DBGPRINT(RT_DEBUG_WARN, ("PeerBeaconAndProbeRspSanity - Variable IEs out of resource [len(=%d) > MAX_VIE_LEN(=%d)]\n",
+                    (*LengthVIE + pEid->Len + 2), MAX_VIE_LEN));
+            break;
+        }
+
+        switch(pEid->Eid)
+        {
+            case IE_SSID:
+                // Already has one SSID EID in this beacon, ignore the second one
+                if (Sanity & 0x1)
+                    break;
+                if(pEid->Len <= MAX_LEN_OF_SSID)
+                {
+                    NdisMoveMemory(Ssid, pEid->Octet, pEid->Len);
+                    *pSsidLen = pEid->Len;
+                    Sanity |= 0x1;
+                }
+                else
+                {
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAndProbeRspSanity - wrong IE_SSID (len=%d)\n",pEid->Len));
+                    return FALSE;
+                }
+                break;
+
+            case IE_SUPP_RATES:
+                if(pEid->Len <= MAX_LEN_OF_SUPPORTED_RATES)
+                {
+                    Sanity |= 0x2;
+                    NdisMoveMemory(SupRate, pEid->Octet, pEid->Len);
+                    *pSupRateLen = pEid->Len;
+
+                    // TODO: 2004-09-14 not a good design here, cause it exclude extra rates
+                    // from ScanTab. We should report as is. And filter out unsupported
+                    // rates in MlmeAux.
+                    // Check against the supported rates
+                    // RTMPCheckRates(pAd, SupRate, pSupRateLen);
+                }
+                else
+                {
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAndProbeRspSanity - wrong IE_SUPP_RATES (len=%d)\n",pEid->Len));
+                    return FALSE;
+                }
+                break;
+
+            case IE_HT_CAP:
+			if (pEid->Len >= SIZE_HT_CAP_IE)  //Note: allow extension.!!
+			{
+				NdisMoveMemory(pHtCapability, pEid->Octet, sizeof(HT_CAPABILITY_IE));
+				*pHtCapabilityLen = SIZE_HT_CAP_IE;	// Nnow we only support 26 bytes.
+
+				*(USHORT *)(&pHtCapability->HtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->HtCapInfo));
+				*(USHORT *)(&pHtCapability->ExtHtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->ExtHtCapInfo));
+
+#ifdef CONFIG_STA_SUPPORT
+				IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				{
+					*pPreNHtCapabilityLen = 0;	// Nnow we only support 26 bytes.
+
+					Ptr = (PUCHAR) pVIE;
+					NdisMoveMemory(Ptr + *LengthVIE, &pEid->Eid, pEid->Len + 2);
+					*LengthVIE += (pEid->Len + 2);
+				}
+#endif // CONFIG_STA_SUPPORT //
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_WARN, ("PeerBeaconAndProbeRspSanity - wrong IE_HT_CAP. pEid->Len = %d\n", pEid->Len));
+			}
+
+		break;
+            case IE_ADD_HT:
+			if (pEid->Len >= sizeof(ADD_HT_INFO_IE))
+			{
+				// This IE allows extension, but we can ignore extra bytes beyond our knowledge , so only
+				// copy first sizeof(ADD_HT_INFO_IE)
+				NdisMoveMemory(AddHtInfo, pEid->Octet, sizeof(ADD_HT_INFO_IE));
+				*AddHtInfoLen = SIZE_ADD_HT_INFO_IE;
+
+				CtrlChannel = AddHtInfo->ControlChan;
+
+				*(USHORT *)(&AddHtInfo->AddHtInfo2) = cpu2le16(*(USHORT *)(&AddHtInfo->AddHtInfo2));
+				*(USHORT *)(&AddHtInfo->AddHtInfo3) = cpu2le16(*(USHORT *)(&AddHtInfo->AddHtInfo3));
+
+#ifdef CONFIG_STA_SUPPORT
+				IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				{
+			                Ptr = (PUCHAR) pVIE;
+			                NdisMoveMemory(Ptr + *LengthVIE, &pEid->Eid, pEid->Len + 2);
+			                *LengthVIE += (pEid->Len + 2);
+				}
+#endif // CONFIG_STA_SUPPORT //
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_WARN, ("PeerBeaconAndProbeRspSanity - wrong IE_ADD_HT. \n"));
+			}
+
+		break;
+            case IE_SECONDARY_CH_OFFSET:
+			if (pEid->Len == 1)
+			{
+				*NewExtChannelOffset = pEid->Octet[0];
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_WARN, ("PeerBeaconAndProbeRspSanity - wrong IE_SECONDARY_CH_OFFSET. \n"));
+			}
+
+		break;
+            case IE_FH_PARM:
+                DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAndProbeRspSanity(IE_FH_PARM) \n"));
+                break;
+
+            case IE_DS_PARM:
+                if(pEid->Len == 1)
+                {
+                    *pChannel = *pEid->Octet;
+#ifdef CONFIG_STA_SUPPORT
+					IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+					{
+						if (ChannelSanity(pAd, *pChannel) == 0)
+						{
+
+							return FALSE;
+						}
+					}
+#endif // CONFIG_STA_SUPPORT //
+                    Sanity |= 0x4;
+                }
+                else
+                {
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAndProbeRspSanity - wrong IE_DS_PARM (len=%d)\n",pEid->Len));
+                    return FALSE;
+                }
+                break;
+
+            case IE_CF_PARM:
+                if(pEid->Len == 6)
+                {
+                    pCfParm->bValid = TRUE;
+                    pCfParm->CfpCount = pEid->Octet[0];
+                    pCfParm->CfpPeriod = pEid->Octet[1];
+                    pCfParm->CfpMaxDuration = pEid->Octet[2] + 256 * pEid->Octet[3];
+                    pCfParm->CfpDurRemaining = pEid->Octet[4] + 256 * pEid->Octet[5];
+                }
+                else
+                {
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAndProbeRspSanity - wrong IE_CF_PARM\n"));
+                    return FALSE;
+                }
+                break;
+
+            case IE_IBSS_PARM:
+                if(pEid->Len == 2)
+                {
+                    NdisMoveMemory(pAtimWin, pEid->Octet, pEid->Len);
+                }
+                else
+                {
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAndProbeRspSanity - wrong IE_IBSS_PARM\n"));
+                    return FALSE;
+                }
+                break;
+
+#ifdef CONFIG_STA_SUPPORT
+            case IE_TIM:
+                if(INFRA_ON(pAd) && SubType == SUBTYPE_BEACON)
+                {
+                    GetTimBit((PUCHAR)pEid, pAd->StaActive.Aid, &TimLen, pBcastFlag, pDtimCount, pDtimPeriod, pMessageToMe);
+                }
+                break;
+#endif // CONFIG_STA_SUPPORT //
+            case IE_CHANNEL_SWITCH_ANNOUNCEMENT:
+                if(pEid->Len == 3)
+                {
+                	*pNewChannel = pEid->Octet[1];	//extract new channel number
+                }
+                break;
+
+            // New for WPA
+            // CCX v2 has the same IE, we need to parse that too
+            // Wifi WMM use the same IE vale, need to parse that too
+            // case IE_WPA:
+            case IE_VENDOR_SPECIFIC:
+                // Check Broadcom/Atheros 802.11n OUI version, for HT Capability IE.
+                // This HT IE is before IEEE draft set HT IE value.2006-09-28 by Jan.
+                /*if (NdisEqualMemory(pEid->Octet, BROADCOM_OUI, 3) && (pEid->Len >= 4))
+                {
+                	if ((pEid->Octet[3] == OUI_BROADCOM_HT) && (pEid->Len >= 30))
+            		{
+				{
+					NdisMoveMemory(pHtCapability, &pEid->Octet[4], sizeof(HT_CAPABILITY_IE));
+					*pHtCapabilityLen = SIZE_HT_CAP_IE;	// Nnow we only support 26 bytes.
+				}
+         		}
+                	if ((pEid->Octet[3] == OUI_BROADCOM_HT) && (pEid->Len >= 26))
+            		{
+				{
+					NdisMoveMemory(AddHtInfo, &pEid->Octet[4], sizeof(ADD_HT_INFO_IE));
+					*AddHtInfoLen = SIZE_ADD_HT_INFO_IE;	// Nnow we only support 26 bytes.
+				}
+         		}
+                }
+				*/
+                // Check the OUI version, filter out non-standard usage
+                if (NdisEqualMemory(pEid->Octet, RALINK_OUI, 3) && (pEid->Len == 7))
+                {
+                    //*pRalinkIe = pEid->Octet[3];
+                    if (pEid->Octet[3] != 0)
+        				*pRalinkIe = pEid->Octet[3];
+        			else
+        				*pRalinkIe = 0xf0000000; // Set to non-zero value (can't set bit0-2) to represent this is Ralink Chip. So at linkup, we will set ralinkchip flag.
+                }
+#ifdef CONFIG_STA_SUPPORT
+#ifdef DOT11_N_SUPPORT
+		// This HT IE is before IEEE draft set HT IE value.2006-09-28 by Jan.
+
+                // Other vendors had production before IE_HT_CAP value is assigned. To backward support those old-firmware AP,
+                // Check broadcom-defiend pre-802.11nD1.0 OUI for HT related IE, including HT Capatilities IE and HT Information IE
+                else if ((*pHtCapabilityLen == 0) && NdisEqualMemory(pEid->Octet, PRE_N_HT_OUI, 3) && (pEid->Len >= 4) && (pAd->OpMode == OPMODE_STA))
+                {
+                    if ((pEid->Octet[3] == OUI_PREN_HT_CAP) && (pEid->Len >= 30) && (*pHtCapabilityLen == 0))
+                    {
+                        NdisMoveMemory(pHtCapability, &pEid->Octet[4], sizeof(HT_CAPABILITY_IE));
+                        *pPreNHtCapabilityLen = SIZE_HT_CAP_IE;
+                    }
+
+                    if ((pEid->Octet[3] == OUI_PREN_ADD_HT) && (pEid->Len >= 26))
+                    {
+                        NdisMoveMemory(AddHtInfo, &pEid->Octet[4], sizeof(ADD_HT_INFO_IE));
+                        *AddHtInfoLen = SIZE_ADD_HT_INFO_IE;
+                    }
+                }
+#endif // DOT11_N_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+                else if (NdisEqualMemory(pEid->Octet, WPA_OUI, 4))
+                {
+                    // Copy to pVIE which will report to microsoft bssid list.
+                    Ptr = (PUCHAR) pVIE;
+                    NdisMoveMemory(Ptr + *LengthVIE, &pEid->Eid, pEid->Len + 2);
+                    *LengthVIE += (pEid->Len + 2);
+                }
+                else if (NdisEqualMemory(pEid->Octet, WME_PARM_ELEM, 6) && (pEid->Len == 24))
+                {
+                    PUCHAR ptr;
+                    int i;
+
+                    // parsing EDCA parameters
+                    pEdcaParm->bValid          = TRUE;
+                    pEdcaParm->bQAck           = FALSE; // pEid->Octet[0] & 0x10;
+                    pEdcaParm->bQueueRequest   = FALSE; // pEid->Octet[0] & 0x20;
+                    pEdcaParm->bTxopRequest    = FALSE; // pEid->Octet[0] & 0x40;
+                    pEdcaParm->EdcaUpdateCount = pEid->Octet[6] & 0x0f;
+                    pEdcaParm->bAPSDCapable    = (pEid->Octet[6] & 0x80) ? 1 : 0;
+                    ptr = &pEid->Octet[8];
+                    for (i=0; i<4; i++)
+                    {
+                        UCHAR aci = (*ptr & 0x60) >> 5; // b5~6 is AC INDEX
+                        pEdcaParm->bACM[aci]  = (((*ptr) & 0x10) == 0x10);   // b5 is ACM
+                        pEdcaParm->Aifsn[aci] = (*ptr) & 0x0f;               // b0~3 is AIFSN
+                        pEdcaParm->Cwmin[aci] = *(ptr+1) & 0x0f;             // b0~4 is Cwmin
+                        pEdcaParm->Cwmax[aci] = *(ptr+1) >> 4;               // b5~8 is Cwmax
+                        pEdcaParm->Txop[aci]  = *(ptr+2) + 256 * (*(ptr+3)); // in unit of 32-us
+                        ptr += 4; // point to next AC
+                    }
+                }
+                else if (NdisEqualMemory(pEid->Octet, WME_INFO_ELEM, 6) && (pEid->Len == 7))
+                {
+                    // parsing EDCA parameters
+                    pEdcaParm->bValid          = TRUE;
+                    pEdcaParm->bQAck           = FALSE; // pEid->Octet[0] & 0x10;
+                    pEdcaParm->bQueueRequest   = FALSE; // pEid->Octet[0] & 0x20;
+                    pEdcaParm->bTxopRequest    = FALSE; // pEid->Octet[0] & 0x40;
+                    pEdcaParm->EdcaUpdateCount = pEid->Octet[6] & 0x0f;
+                    pEdcaParm->bAPSDCapable    = (pEid->Octet[6] & 0x80) ? 1 : 0;
+
+                    // use default EDCA parameter
+                    pEdcaParm->bACM[QID_AC_BE]  = 0;
+                    pEdcaParm->Aifsn[QID_AC_BE] = 3;
+                    pEdcaParm->Cwmin[QID_AC_BE] = CW_MIN_IN_BITS;
+                    pEdcaParm->Cwmax[QID_AC_BE] = CW_MAX_IN_BITS;
+                    pEdcaParm->Txop[QID_AC_BE]  = 0;
+
+                    pEdcaParm->bACM[QID_AC_BK]  = 0;
+                    pEdcaParm->Aifsn[QID_AC_BK] = 7;
+                    pEdcaParm->Cwmin[QID_AC_BK] = CW_MIN_IN_BITS;
+                    pEdcaParm->Cwmax[QID_AC_BK] = CW_MAX_IN_BITS;
+                    pEdcaParm->Txop[QID_AC_BK]  = 0;
+
+                    pEdcaParm->bACM[QID_AC_VI]  = 0;
+                    pEdcaParm->Aifsn[QID_AC_VI] = 2;
+                    pEdcaParm->Cwmin[QID_AC_VI] = CW_MIN_IN_BITS-1;
+                    pEdcaParm->Cwmax[QID_AC_VI] = CW_MAX_IN_BITS;
+                    pEdcaParm->Txop[QID_AC_VI]  = 96;   // AC_VI: 96*32us ~= 3ms
+
+                    pEdcaParm->bACM[QID_AC_VO]  = 0;
+                    pEdcaParm->Aifsn[QID_AC_VO] = 2;
+                    pEdcaParm->Cwmin[QID_AC_VO] = CW_MIN_IN_BITS-2;
+                    pEdcaParm->Cwmax[QID_AC_VO] = CW_MAX_IN_BITS-1;
+                    pEdcaParm->Txop[QID_AC_VO]  = 48;   // AC_VO: 48*32us ~= 1.5ms
+                }
+                break;
+
+            case IE_EXT_SUPP_RATES:
+                if (pEid->Len <= MAX_LEN_OF_SUPPORTED_RATES)
+                {
+                    NdisMoveMemory(ExtRate, pEid->Octet, pEid->Len);
+                    *pExtRateLen = pEid->Len;
+
+                    // TODO: 2004-09-14 not a good design here, cause it exclude extra rates
+                    // from ScanTab. We should report as is. And filter out unsupported
+                    // rates in MlmeAux.
+                    // Check against the supported rates
+                    // RTMPCheckRates(pAd, ExtRate, pExtRateLen);
+                }
+                break;
+
+            case IE_ERP:
+                if (pEid->Len == 1)
+                {
+                    *pErp = (UCHAR)pEid->Octet[0];
+                }
+                break;
+
+            case IE_AIRONET_CKIP:
+                // 0. Check Aironet IE length, it must be larger or equal to 28
+                // Cisco AP350 used length as 28
+                // Cisco AP12XX used length as 30
+                if (pEid->Len < (CKIP_NEGOTIATION_LENGTH - 2))
+                    break;
+
+                // 1. Copy CKIP flag byte to buffer for process
+                *pCkipFlag = *(pEid->Octet + 8);
+                break;
+
+            case IE_AP_TX_POWER:
+                // AP Control of Client Transmit Power
+                //0. Check Aironet IE length, it must be 6
+                if (pEid->Len != 0x06)
+                    break;
+
+                // Get cell power limit in dBm
+                if (NdisEqualMemory(pEid->Octet, CISCO_OUI, 3) == 1)
+                    *pAironetCellPowerLimit = *(pEid->Octet + 4);
+                break;
+
+            // WPA2 & 802.11i RSN
+            case IE_RSN:
+                // There is no OUI for version anymore, check the group cipher OUI before copying
+                if (RTMPEqualMemory(pEid->Octet + 2, RSN_OUI, 3))
+                {
+                    // Copy to pVIE which will report to microsoft bssid list.
+                    Ptr = (PUCHAR) pVIE;
+                    NdisMoveMemory(Ptr + *LengthVIE, &pEid->Eid, pEid->Len + 2);
+                    *LengthVIE += (pEid->Len + 2);
+                }
+                break;
+#ifdef CONFIG_STA_SUPPORT
+#ifdef EXT_BUILD_CHANNEL_LIST
+			case IE_COUNTRY:
+				Ptr = (PUCHAR) pVIE;
+                NdisMoveMemory(Ptr + *LengthVIE, &pEid->Eid, pEid->Len + 2);
+                *LengthVIE += (pEid->Len + 2);
+				break;
+#endif // EXT_BUILD_CHANNEL_LIST //
+#endif // CONFIG_STA_SUPPORT //
+            default:
+                break;
+        }
+
+        Length = Length + 2 + pEid->Len;  // Eid[1] + Len[1]+ content[Len]
+        pEid = (PEID_STRUCT)((UCHAR*)pEid + 2 + pEid->Len);
+    }
+
+    // For some 11a AP. it did not have the channel EID, patch here
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		UCHAR LatchRfChannel = MsgChannel;
+		if ((pAd->LatchRfRegs.Channel > 14) && ((Sanity & 0x4) == 0))
+		{
+			if (CtrlChannel != 0)
+				*pChannel = CtrlChannel;
+			else
+				*pChannel = LatchRfChannel;
+			Sanity |= 0x4;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	if (Sanity != 0x7)
+	{
+		DBGPRINT(RT_DEBUG_WARN, ("PeerBeaconAndProbeRspSanity - missing field, Sanity=0x%02x\n", Sanity));
+		return FALSE;
+	}
+	else
+	{
+		return TRUE;
+	}
+
+}
+
+#ifdef DOT11N_DRAFT3
+/*
+	==========================================================================
+	Description:
+		MLME message sanity check for some IE addressed  in 802.11n d3.03.
+	Return:
+		TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+BOOLEAN PeerBeaconAndProbeRspSanity2(
+	IN PRTMP_ADAPTER pAd,
+	IN VOID *Msg,
+	IN ULONG MsgLen,
+	OUT UCHAR 	*RegClass)
+{
+	CHAR				*Ptr;
+	PFRAME_802_11		pFrame;
+	PEID_STRUCT			pEid;
+	ULONG				Length = 0;
+
+	pFrame = (PFRAME_802_11)Msg;
+
+	*RegClass = 0;
+	Ptr = pFrame->Octet;
+	Length += LENGTH_802_11;
+
+	// get timestamp from payload and advance the pointer
+	Ptr += TIMESTAMP_LEN;
+	Length += TIMESTAMP_LEN;
+
+	// get beacon interval from payload and advance the pointer
+	Ptr += 2;
+	Length += 2;
+
+	// get capability info from payload and advance the pointer
+	Ptr += 2;
+	Length += 2;
+
+	pEid = (PEID_STRUCT) Ptr;
+
+	// get variable fields from payload and advance the pointer
+	while ((Length + 2 + pEid->Len) <= MsgLen)
+	{
+		switch(pEid->Eid)
+		{
+			case IE_SUPP_REG_CLASS:
+				if(pEid->Len > 0)
+				{
+					*RegClass = *pEid->Octet;
+				}
+				else
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAndProbeRspSanity - wrong IE_SSID (len=%d)\n",pEid->Len));
+					return FALSE;
+				}
+				break;
+		}
+
+		Length = Length + 2 + pEid->Len;  // Eid[1] + Len[1]+ content[Len]
+		pEid = (PEID_STRUCT)((UCHAR*)pEid + 2 + pEid->Len);
+	}
+
+	return TRUE;
+
+}
+#endif // DOT11N_DRAFT3 //
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+ */
+BOOLEAN MlmeScanReqSanity(
+	IN PRTMP_ADAPTER pAd,
+	IN VOID *Msg,
+	IN ULONG MsgLen,
+	OUT UCHAR *pBssType,
+	OUT CHAR Ssid[],
+	OUT UCHAR *pSsidLen,
+	OUT UCHAR *pScanType)
+{
+	MLME_SCAN_REQ_STRUCT *Info;
+
+	Info = (MLME_SCAN_REQ_STRUCT *)(Msg);
+	*pBssType = Info->BssType;
+	*pSsidLen = Info->SsidLen;
+	NdisMoveMemory(Ssid, Info->Ssid, *pSsidLen);
+	*pScanType = Info->ScanType;
+
+	if ((*pBssType == BSS_INFRA || *pBssType == BSS_ADHOC || *pBssType == BSS_ANY)
+		&& (*pScanType == SCAN_ACTIVE || *pScanType == SCAN_PASSIVE
+#ifdef CONFIG_STA_SUPPORT
+		|| *pScanType == SCAN_CISCO_PASSIVE || *pScanType == SCAN_CISCO_ACTIVE
+		|| *pScanType == SCAN_CISCO_CHANNEL_LOAD || *pScanType == SCAN_CISCO_NOISE
+#endif // CONFIG_STA_SUPPORT //
+		))
+	{
+		return TRUE;
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("MlmeScanReqSanity fail - wrong BssType or ScanType\n"));
+		return FALSE;
+	}
+}
+
+// IRQL = DISPATCH_LEVEL
+UCHAR ChannelSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN UCHAR channel)
+{
+    int i;
+
+    for (i = 0; i < pAd->ChannelListNum; i ++)
+    {
+        if (channel == pAd->ChannelList[i].Channel)
+            return 1;
+    }
+    return 0;
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN PeerDeauthSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT USHORT *pReason)
+{
+    PFRAME_802_11 pFrame = (PFRAME_802_11)Msg;
+
+    COPY_MAC_ADDR(pAddr2, pFrame->Hdr.Addr2);
+    NdisMoveMemory(pReason, &pFrame->Octet[0], 2);
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN PeerAuthSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr,
+    OUT USHORT *pAlg,
+    OUT USHORT *pSeq,
+    OUT USHORT *pStatus,
+    CHAR *pChlgText)
+{
+    PFRAME_802_11 pFrame = (PFRAME_802_11)Msg;
+
+    COPY_MAC_ADDR(pAddr,   pFrame->Hdr.Addr2);
+    NdisMoveMemory(pAlg,    &pFrame->Octet[0], 2);
+    NdisMoveMemory(pSeq,    &pFrame->Octet[2], 2);
+    NdisMoveMemory(pStatus, &pFrame->Octet[4], 2);
+
+    if ((*pAlg == Ndis802_11AuthModeOpen)
+#ifdef LEAP_SUPPORT
+      || (*pAlg == CISCO_AuthModeLEAP)
+#endif // LEAP_SUPPORT //
+      )
+    {
+        if (*pSeq == 1 || *pSeq == 2)
+        {
+            return TRUE;
+        }
+        else
+        {
+            DBGPRINT(RT_DEBUG_TRACE, ("PeerAuthSanity fail - wrong Seg#\n"));
+            return FALSE;
+        }
+    }
+    else if (*pAlg == Ndis802_11AuthModeShared)
+    {
+        if (*pSeq == 1 || *pSeq == 4)
+        {
+            return TRUE;
+        }
+        else if (*pSeq == 2 || *pSeq == 3)
+        {
+            NdisMoveMemory(pChlgText, &pFrame->Octet[8], CIPHER_TEXT_LEN);
+            return TRUE;
+        }
+        else
+        {
+            DBGPRINT(RT_DEBUG_TRACE, ("PeerAuthSanity fail - wrong Seg#\n"));
+            return FALSE;
+        }
+    }
+    else
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("PeerAuthSanity fail - wrong algorithm\n"));
+        return FALSE;
+    }
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+ */
+BOOLEAN MlmeAuthReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr,
+    OUT ULONG *pTimeout,
+    OUT USHORT *pAlg)
+{
+    MLME_AUTH_REQ_STRUCT *pInfo;
+
+    pInfo  = (MLME_AUTH_REQ_STRUCT *)Msg;
+    COPY_MAC_ADDR(pAddr, pInfo->Addr);
+    *pTimeout = pInfo->Timeout;
+    *pAlg = pInfo->Alg;
+
+    if (((*pAlg == Ndis802_11AuthModeShared) ||(*pAlg == Ndis802_11AuthModeOpen)
+#ifdef LEAP_SUPPORT
+     || (*pAlg == CISCO_AuthModeLEAP)
+#endif // LEAP_SUPPORT //
+     	) &&
+        ((*pAddr & 0x01) == 0))
+    {
+        return TRUE;
+    }
+    else
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("MlmeAuthReqSanity fail - wrong algorithm\n"));
+        return FALSE;
+    }
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN MlmeAssocReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pApAddr,
+    OUT USHORT *pCapabilityInfo,
+    OUT ULONG *pTimeout,
+    OUT USHORT *pListenIntv)
+{
+    MLME_ASSOC_REQ_STRUCT *pInfo;
+
+    pInfo = (MLME_ASSOC_REQ_STRUCT *)Msg;
+    *pTimeout = pInfo->Timeout;                             // timeout
+    COPY_MAC_ADDR(pApAddr, pInfo->Addr);                   // AP address
+    *pCapabilityInfo = pInfo->CapabilityInfo;               // capability info
+    *pListenIntv = pInfo->ListenIntv;
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN PeerDisassocSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT USHORT *pReason)
+{
+    PFRAME_802_11 pFrame = (PFRAME_802_11)Msg;
+
+    COPY_MAC_ADDR(pAddr2, pFrame->Hdr.Addr2);
+    NdisMoveMemory(pReason, &pFrame->Octet[0], 2);
+
+    return TRUE;
+}
+
+/*
+	========================================================================
+	Routine Description:
+		Sanity check NetworkType (11b, 11g or 11a)
+
+	Arguments:
+		pBss - Pointer to BSS table.
+
+	Return Value:
+        Ndis802_11DS .......(11b)
+        Ndis802_11OFDM24....(11g)
+        Ndis802_11OFDM5.....(11a)
+
+	IRQL = DISPATCH_LEVEL
+
+	========================================================================
+*/
+NDIS_802_11_NETWORK_TYPE NetworkTypeInUseSanity(
+    IN PBSS_ENTRY pBss)
+{
+	NDIS_802_11_NETWORK_TYPE	NetWorkType;
+	UCHAR						rate, i;
+
+	NetWorkType = Ndis802_11DS;
+
+	if (pBss->Channel <= 14)
+	{
+		//
+		// First check support Rate.
+		//
+		for (i = 0; i < pBss->SupRateLen; i++)
+		{
+			rate = pBss->SupRate[i] & 0x7f; // Mask out basic rate set bit
+			if ((rate == 2) || (rate == 4) || (rate == 11) || (rate == 22))
+			{
+				continue;
+			}
+			else
+			{
+				//
+				// Otherwise (even rate > 108) means Ndis802_11OFDM24
+				//
+				NetWorkType = Ndis802_11OFDM24;
+				break;
+			}
+		}
+
+		//
+		// Second check Extend Rate.
+		//
+		if (NetWorkType != Ndis802_11OFDM24)
+		{
+			for (i = 0; i < pBss->ExtRateLen; i++)
+			{
+				rate = pBss->SupRate[i] & 0x7f; // Mask out basic rate set bit
+				if ((rate == 2) || (rate == 4) || (rate == 11) || (rate == 22))
+				{
+					continue;
+				}
+				else
+				{
+					//
+					// Otherwise (even rate > 108) means Ndis802_11OFDM24
+					//
+					NetWorkType = Ndis802_11OFDM24;
+					break;
+				}
+			}
+		}
+	}
+	else
+	{
+		NetWorkType = Ndis802_11OFDM5;
+	}
+
+    if (pBss->HtCapabilityLen != 0)
+    {
+        if (NetWorkType == Ndis802_11OFDM5)
+            NetWorkType = Ndis802_11OFDM5_N;
+        else
+            NetWorkType = Ndis802_11OFDM24_N;
+    }
+
+	return NetWorkType;
+}
+
+/*
+    ==========================================================================
+    Description:
+        WPA message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+ */
+BOOLEAN PeerWpaMessageSanity(
+    IN 	PRTMP_ADAPTER 		pAd,
+    IN 	PEAPOL_PACKET 		pMsg,
+    IN 	ULONG 				MsgLen,
+    IN 	UCHAR				MsgType,
+    IN 	MAC_TABLE_ENTRY  	*pEntry)
+{
+	UCHAR			mic[LEN_KEY_DESC_MIC], digest[80], KEYDATA[MAX_LEN_OF_RSNIE];
+	BOOLEAN			bReplayDiff = FALSE;
+	BOOLEAN			bWPA2 = FALSE;
+	KEY_INFO		EapolKeyInfo;
+	UCHAR			GroupKeyIndex = 0;
+
+
+	NdisZeroMemory(mic, sizeof(mic));
+	NdisZeroMemory(digest, sizeof(digest));
+	NdisZeroMemory(KEYDATA, sizeof(KEYDATA));
+	NdisZeroMemory((PUCHAR)&EapolKeyInfo, sizeof(EapolKeyInfo));
+
+	NdisMoveMemory((PUCHAR)&EapolKeyInfo, (PUCHAR)&pMsg->KeyDesc.KeyInfo, sizeof(KEY_INFO));
+
+	*((USHORT *)&EapolKeyInfo) = cpu2le16(*((USHORT *)&EapolKeyInfo));
+
+	// Choose WPA2 or not
+	if ((pEntry->AuthMode == Ndis802_11AuthModeWPA2) || (pEntry->AuthMode == Ndis802_11AuthModeWPA2PSK))
+		bWPA2 = TRUE;
+
+	// 0. Check MsgType
+	if ((MsgType > EAPOL_GROUP_MSG_2) || (MsgType < EAPOL_PAIR_MSG_1))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("The message type is invalid(%d)! \n", MsgType));
+		return FALSE;
+	}
+
+	// 1. Replay counter check
+ 	if (MsgType == EAPOL_PAIR_MSG_1 || MsgType == EAPOL_PAIR_MSG_3 || MsgType == EAPOL_GROUP_MSG_1)	// For supplicant
+    {
+    	// First validate replay counter, only accept message with larger replay counter.
+		// Let equal pass, some AP start with all zero replay counter
+		UCHAR	ZeroReplay[LEN_KEY_DESC_REPLAY];
+
+        NdisZeroMemory(ZeroReplay, LEN_KEY_DESC_REPLAY);
+		if ((RTMPCompareMemory(pMsg->KeyDesc.ReplayCounter, pEntry->R_Counter, LEN_KEY_DESC_REPLAY) != 1) &&
+			(RTMPCompareMemory(pMsg->KeyDesc.ReplayCounter, ZeroReplay, LEN_KEY_DESC_REPLAY) != 0))
+    	{
+			bReplayDiff = TRUE;
+    	}
+ 	}
+	else if (MsgType == EAPOL_PAIR_MSG_2 || MsgType == EAPOL_PAIR_MSG_4 || MsgType == EAPOL_GROUP_MSG_2)	// For authenticator
+	{
+		// check Replay Counter coresponds to MSG from authenticator, otherwise discard
+    	if (!NdisEqualMemory(pMsg->KeyDesc.ReplayCounter, pEntry->R_Counter, LEN_KEY_DESC_REPLAY))
+    	{
+			bReplayDiff = TRUE;
+    	}
+	}
+
+	// Replay Counter different condition
+	if (bReplayDiff)
+	{
+		// send wireless event - for replay counter different
+		if (pAd->CommonCfg.bWirelessEvent)
+			RTMPSendWirelessEvent(pAd, IW_REPLAY_COUNTER_DIFF_EVENT_FLAG, pEntry->Addr, pEntry->apidx, 0);
+
+		if (MsgType < EAPOL_GROUP_MSG_1)
+		{
+           	DBGPRINT(RT_DEBUG_ERROR, ("Replay Counter Different in pairwise msg %d of 4-way handshake!\n", MsgType));
+		}
+		else
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("Replay Counter Different in group msg %d of 2-way handshake!\n", (MsgType - EAPOL_PAIR_MSG_4)));
+		}
+
+		hex_dump("Receive replay counter ", pMsg->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+		hex_dump("Current replay counter ", pEntry->R_Counter, LEN_KEY_DESC_REPLAY);
+        return FALSE;
+	}
+
+	// 2. Verify MIC except Pairwise Msg1
+	if (MsgType != EAPOL_PAIR_MSG_1)
+	{
+		UCHAR			rcvd_mic[LEN_KEY_DESC_MIC];
+
+		// Record the received MIC for check later
+		NdisMoveMemory(rcvd_mic, pMsg->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+		NdisZeroMemory(pMsg->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+
+        if (pEntry->WepStatus == Ndis802_11Encryption2Enabled)	// TKIP
+        {
+            hmac_md5(pEntry->PTK, LEN_EAP_MICK, (PUCHAR)pMsg, MsgLen, mic);
+        }
+        else if (pEntry->WepStatus == Ndis802_11Encryption3Enabled)	// AES
+        {
+            HMAC_SHA1((PUCHAR)pMsg, MsgLen, pEntry->PTK, LEN_EAP_MICK, digest);
+            NdisMoveMemory(mic, digest, LEN_KEY_DESC_MIC);
+        }
+
+        if (!NdisEqualMemory(rcvd_mic, mic, LEN_KEY_DESC_MIC))
+        {
+			// send wireless event - for MIC different
+			if (pAd->CommonCfg.bWirelessEvent)
+				RTMPSendWirelessEvent(pAd, IW_MIC_DIFF_EVENT_FLAG, pEntry->Addr, pEntry->apidx, 0);
+
+			if (MsgType < EAPOL_GROUP_MSG_1)
+			{
+            	DBGPRINT(RT_DEBUG_ERROR, ("MIC Different in pairwise msg %d of 4-way handshake!\n", MsgType));
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_ERROR, ("MIC Different in group msg %d of 2-way handshake!\n", (MsgType - EAPOL_PAIR_MSG_4)));
+			}
+
+			hex_dump("Received MIC", rcvd_mic, LEN_KEY_DESC_MIC);
+			hex_dump("Desired  MIC", mic, LEN_KEY_DESC_MIC);
+
+			return FALSE;
+        }
+	}
+
+	// Extract the context of the Key Data field if it exist
+	// The field in pairwise_msg_2_WPA1(WPA2) & pairwise_msg_3_WPA1 is un-encrypted.
+	// The field in group_msg_1_WPA1(WPA2) & pairwise_msg_3_WPA2 is encrypted.
+	if (pMsg->KeyDesc.KeyDataLen[1] > 0)
+	{
+		// Decrypt this field
+		if ((MsgType == EAPOL_PAIR_MSG_3 && bWPA2) || (MsgType == EAPOL_GROUP_MSG_1))
+		{
+			if(pEntry->WepStatus == Ndis802_11Encryption3Enabled)
+			{
+				// AES
+				AES_GTK_KEY_UNWRAP(&pEntry->PTK[16], KEYDATA, pMsg->KeyDesc.KeyDataLen[1],pMsg->KeyDesc.KeyData);
+			}
+			else
+			{
+				INT 	i;
+				UCHAR   Key[32];
+				// Decrypt TKIP GTK
+				// Construct 32 bytes RC4 Key
+				NdisMoveMemory(Key, pMsg->KeyDesc.KeyIv, 16);
+				NdisMoveMemory(&Key[16], &pEntry->PTK[16], 16);
+				ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, Key, 32);
+				//discard first 256 bytes
+				for(i = 0; i < 256; i++)
+					ARCFOUR_BYTE(&pAd->PrivateInfo.WEPCONTEXT);
+				// Decrypt GTK. Becareful, there is no ICV to check the result is correct or not
+				ARCFOUR_DECRYPT(&pAd->PrivateInfo.WEPCONTEXT, KEYDATA, pMsg->KeyDesc.KeyData, pMsg->KeyDesc.KeyDataLen[1]);
+			}
+
+			if (!bWPA2 && (MsgType == EAPOL_GROUP_MSG_1))
+				GroupKeyIndex = EapolKeyInfo.KeyIndex;
+
+		}
+		else if ((MsgType == EAPOL_PAIR_MSG_2) || (MsgType == EAPOL_PAIR_MSG_3 && !bWPA2))
+		{
+			NdisMoveMemory(KEYDATA, pMsg->KeyDesc.KeyData, pMsg->KeyDesc.KeyDataLen[1]);
+		}
+		else
+		{
+
+			return TRUE;
+		}
+
+		// Parse Key Data field to
+		// 1. verify RSN IE for pairwise_msg_2_WPA1(WPA2) ,pairwise_msg_3_WPA1(WPA2)
+		// 2. verify KDE format for pairwise_msg_3_WPA2, group_msg_1_WPA2
+		// 3. update shared key for pairwise_msg_3_WPA2, group_msg_1_WPA1(WPA2)
+		if (!RTMPParseEapolKeyData(pAd, KEYDATA, pMsg->KeyDesc.KeyDataLen[1], GroupKeyIndex, MsgType, bWPA2, pEntry))
+		{
+			return FALSE;
+		}
+	}
+
+	return TRUE;
+
+}
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+BOOLEAN MlmeDlsReqSanity(
+	IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PRT_802_11_DLS *pDLS,
+    OUT PUSHORT pReason)
+{
+	MLME_DLS_REQ_STRUCT *pInfo;
+
+    pInfo = (MLME_DLS_REQ_STRUCT *)Msg;
+
+	*pDLS = pInfo->pDLS;
+	*pReason = pInfo->Reason;
+
+	return TRUE;
+}
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef QOS_DLS_SUPPORT
+BOOLEAN PeerDlsReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pDA,
+    OUT PUCHAR pSA,
+    OUT USHORT *pCapabilityInfo,
+    OUT USHORT *pDlsTimeout,
+    OUT UCHAR *pRatesLen,
+    OUT UCHAR Rates[],
+	OUT UCHAR *pHtCapabilityLen,
+    OUT HT_CAPABILITY_IE *pHtCapability)
+{
+	CHAR            *Ptr;
+    PFRAME_802_11	Fr = (PFRAME_802_11)Msg;
+	PEID_STRUCT  eid_ptr;
+
+    // to prevent caller from using garbage output value
+    *pCapabilityInfo	= 0;
+    *pDlsTimeout	= 0;
+	*pHtCapabilityLen = 0;
+
+    Ptr = Fr->Octet;
+
+	// offset to destination MAC address (Category and Action field)
+    Ptr += 2;
+
+    // get DA from payload and advance the pointer
+    NdisMoveMemory(pDA, Ptr, MAC_ADDR_LEN);
+    Ptr += MAC_ADDR_LEN;
+
+    // get SA from payload and advance the pointer
+    NdisMoveMemory(pSA, Ptr, MAC_ADDR_LEN);
+    Ptr += MAC_ADDR_LEN;
+
+    // get capability info from payload and advance the pointer
+    NdisMoveMemory(pCapabilityInfo, Ptr, 2);
+    Ptr += 2;
+
+    // get capability info from payload and advance the pointer
+    NdisMoveMemory(pDlsTimeout, Ptr, 2);
+    Ptr += 2;
+
+	// Category and Action field + DA + SA + capability + Timeout
+	eid_ptr = (PEID_STRUCT) &Fr->Octet[18];
+
+	while (((UCHAR*)eid_ptr + eid_ptr->Len + 1) < ((UCHAR*)Fr + MsgLen))
+	{
+		switch(eid_ptr->Eid)
+		{
+			case IE_SUPP_RATES:
+                if ((eid_ptr->Len <= MAX_LEN_OF_SUPPORTED_RATES) && (eid_ptr->Len > 0))
+                {
+                    NdisMoveMemory(Rates, eid_ptr->Octet, eid_ptr->Len);
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsReqSanity - IE_SUPP_RATES., Len=%d. Rates[0]=%x\n",eid_ptr->Len, Rates[0]));
+                    DBGPRINT(RT_DEBUG_TRACE, ("Rates[1]=%x %x %x %x %x %x %x\n", Rates[1], Rates[2], Rates[3], Rates[4], Rates[5], Rates[6], Rates[7]));
+                    *pRatesLen = eid_ptr->Len;
+                }
+                else
+                {
+                    *pRatesLen = 8;
+					Rates[0] = 0x82;
+					Rates[1] = 0x84;
+					Rates[2] = 0x8b;
+					Rates[3] = 0x96;
+					Rates[4] = 0x12;
+					Rates[5] = 0x24;
+					Rates[6] = 0x48;
+					Rates[7] = 0x6c;
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsReqSanity - wrong IE_SUPP_RATES., Len=%d\n",eid_ptr->Len));
+                }
+				break;
+
+			case IE_EXT_SUPP_RATES:
+                if (eid_ptr->Len + *pRatesLen <= MAX_LEN_OF_SUPPORTED_RATES)
+                {
+                    NdisMoveMemory(&Rates[*pRatesLen], eid_ptr->Octet, eid_ptr->Len);
+                    *pRatesLen = (*pRatesLen) + eid_ptr->Len;
+                }
+                else
+                {
+                    NdisMoveMemory(&Rates[*pRatesLen], eid_ptr->Octet, MAX_LEN_OF_SUPPORTED_RATES - (*pRatesLen));
+                    *pRatesLen = MAX_LEN_OF_SUPPORTED_RATES;
+                }
+				break;
+
+			case IE_HT_CAP:
+				if (eid_ptr->Len >= sizeof(HT_CAPABILITY_IE))
+				{
+					NdisMoveMemory(pHtCapability, eid_ptr->Octet, sizeof(HT_CAPABILITY_IE));
+
+					*(USHORT *)(&pHtCapability->HtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->HtCapInfo));
+					*(USHORT *)(&pHtCapability->ExtHtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->ExtHtCapInfo));
+					*pHtCapabilityLen = sizeof(HT_CAPABILITY_IE);
+
+					DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsReqSanity - IE_HT_CAP\n"));
+				}
+				else
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsReqSanity - wrong IE_HT_CAP.eid_ptr->Len = %d\n", eid_ptr->Len));
+				}
+				break;
+
+			default:
+				break;
+		}
+
+		eid_ptr = (PEID_STRUCT)((UCHAR*)eid_ptr + 2 + eid_ptr->Len);
+	}
+
+    return TRUE;
+}
+
+BOOLEAN PeerDlsRspSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pDA,
+    OUT PUCHAR pSA,
+    OUT USHORT *pCapabilityInfo,
+    OUT USHORT *pStatus,
+    OUT UCHAR *pRatesLen,
+    OUT UCHAR Rates[],
+    OUT UCHAR *pHtCapabilityLen,
+    OUT HT_CAPABILITY_IE *pHtCapability)
+{
+    CHAR            *Ptr;
+    PFRAME_802_11	Fr = (PFRAME_802_11)Msg;
+	PEID_STRUCT  eid_ptr;
+
+    // to prevent caller from using garbage output value
+    *pStatus		= 0;
+    *pCapabilityInfo	= 0;
+	*pHtCapabilityLen = 0;
+
+    Ptr = Fr->Octet;
+
+	// offset to destination MAC address (Category and Action field)
+    Ptr += 2;
+
+	// get status code from payload and advance the pointer
+    NdisMoveMemory(pStatus, Ptr, 2);
+    Ptr += 2;
+
+    // get DA from payload and advance the pointer
+    NdisMoveMemory(pDA, Ptr, MAC_ADDR_LEN);
+    Ptr += MAC_ADDR_LEN;
+
+    // get SA from payload and advance the pointer
+    NdisMoveMemory(pSA, Ptr, MAC_ADDR_LEN);
+    Ptr += MAC_ADDR_LEN;
+
+	if (pStatus == 0)
+	{
+	    // get capability info from payload and advance the pointer
+	    NdisMoveMemory(pCapabilityInfo, Ptr, 2);
+	    Ptr += 2;
+	}
+
+	// Category and Action field + status code + DA + SA + capability
+	eid_ptr = (PEID_STRUCT) &Fr->Octet[18];
+
+	while (((UCHAR*)eid_ptr + eid_ptr->Len + 1) < ((UCHAR*)Fr + MsgLen))
+	{
+		switch(eid_ptr->Eid)
+		{
+			case IE_SUPP_RATES:
+                if ((eid_ptr->Len <= MAX_LEN_OF_SUPPORTED_RATES) && (eid_ptr->Len > 0))
+                {
+                    NdisMoveMemory(Rates, eid_ptr->Octet, eid_ptr->Len);
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsRspSanity - IE_SUPP_RATES., Len=%d. Rates[0]=%x\n",eid_ptr->Len, Rates[0]));
+                    DBGPRINT(RT_DEBUG_TRACE, ("Rates[1]=%x %x %x %x %x %x %x\n", Rates[1], Rates[2], Rates[3], Rates[4], Rates[5], Rates[6], Rates[7]));
+                    *pRatesLen = eid_ptr->Len;
+                }
+                else
+                {
+                    *pRatesLen = 8;
+					Rates[0] = 0x82;
+					Rates[1] = 0x84;
+					Rates[2] = 0x8b;
+					Rates[3] = 0x96;
+					Rates[4] = 0x12;
+					Rates[5] = 0x24;
+					Rates[6] = 0x48;
+					Rates[7] = 0x6c;
+                    DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsRspSanity - wrong IE_SUPP_RATES., Len=%d\n",eid_ptr->Len));
+                }
+				break;
+
+			case IE_EXT_SUPP_RATES:
+                if (eid_ptr->Len + *pRatesLen <= MAX_LEN_OF_SUPPORTED_RATES)
+                {
+                    NdisMoveMemory(&Rates[*pRatesLen], eid_ptr->Octet, eid_ptr->Len);
+                    *pRatesLen = (*pRatesLen) + eid_ptr->Len;
+                }
+                else
+                {
+                    NdisMoveMemory(&Rates[*pRatesLen], eid_ptr->Octet, MAX_LEN_OF_SUPPORTED_RATES - (*pRatesLen));
+                    *pRatesLen = MAX_LEN_OF_SUPPORTED_RATES;
+                }
+				break;
+
+			case IE_HT_CAP:
+				if (eid_ptr->Len >= sizeof(HT_CAPABILITY_IE))
+				{
+					NdisMoveMemory(pHtCapability, eid_ptr->Octet, sizeof(HT_CAPABILITY_IE));
+
+					*(USHORT *)(&pHtCapability->HtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->HtCapInfo));
+					*(USHORT *)(&pHtCapability->ExtHtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->ExtHtCapInfo));
+					*pHtCapabilityLen = sizeof(HT_CAPABILITY_IE);
+
+					DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsRspSanity - IE_HT_CAP\n"));
+				}
+				else
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("PeerDlsRspSanity - wrong IE_HT_CAP.eid_ptr->Len = %d\n", eid_ptr->Len));
+				}
+				break;
+
+			default:
+				break;
+		}
+
+		eid_ptr = (PEID_STRUCT)((UCHAR*)eid_ptr + 2 + eid_ptr->Len);
+	}
+
+    return TRUE;
+}
+
+BOOLEAN PeerDlsTearDownSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pDA,
+    OUT PUCHAR pSA,
+    OUT USHORT *pReason)
+{
+    CHAR            *Ptr;
+    PFRAME_802_11	Fr = (PFRAME_802_11)Msg;
+
+    // to prevent caller from using garbage output value
+    *pReason	= 0;
+
+    Ptr = Fr->Octet;
+
+	// offset to destination MAC address (Category and Action field)
+    Ptr += 2;
+
+    // get DA from payload and advance the pointer
+    NdisMoveMemory(pDA, Ptr, MAC_ADDR_LEN);
+    Ptr += MAC_ADDR_LEN;
+
+    // get SA from payload and advance the pointer
+    NdisMoveMemory(pSA, Ptr, MAC_ADDR_LEN);
+    Ptr += MAC_ADDR_LEN;
+
+	// get reason code from payload and advance the pointer
+    NdisMoveMemory(pReason, Ptr, 2);
+    Ptr += 2;
+
+    return TRUE;
+}
+#endif // QOS_DLS_SUPPORT //
+
diff --git a/drivers/staging/rt2870/common/cmm_sync.c b/drivers/staging/rt2870/common/cmm_sync.c
new file mode 100644
index 00000000000..2be7c77a384
--- /dev/null
+++ b/drivers/staging/rt2870/common/cmm_sync.c
@@ -0,0 +1,711 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	sync.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John Chang	2004-09-01      modified for rt2561/2661
+*/
+#include "../rt_config.h"
+
+// 2.4 Ghz channel plan index in the TxPower arrays.
+#define	BG_BAND_REGION_0_START	0			// 1,2,3,4,5,6,7,8,9,10,11
+#define	BG_BAND_REGION_0_SIZE	11
+#define	BG_BAND_REGION_1_START	0			// 1,2,3,4,5,6,7,8,9,10,11,12,13
+#define	BG_BAND_REGION_1_SIZE	13
+#define	BG_BAND_REGION_2_START	9			// 10,11
+#define	BG_BAND_REGION_2_SIZE	2
+#define	BG_BAND_REGION_3_START	9			// 10,11,12,13
+#define	BG_BAND_REGION_3_SIZE	4
+#define	BG_BAND_REGION_4_START	13			// 14
+#define	BG_BAND_REGION_4_SIZE	1
+#define	BG_BAND_REGION_5_START	0			// 1,2,3,4,5,6,7,8,9,10,11,12,13,14
+#define	BG_BAND_REGION_5_SIZE	14
+#define	BG_BAND_REGION_6_START	2			// 3,4,5,6,7,8,9
+#define	BG_BAND_REGION_6_SIZE	7
+#define	BG_BAND_REGION_7_START	4			// 5,6,7,8,9,10,11,12,13
+#define	BG_BAND_REGION_7_SIZE	9
+#define	BG_BAND_REGION_31_START	0			// 1,2,3,4,5,6,7,8,9,10,11,12,13,14
+#define	BG_BAND_REGION_31_SIZE	14
+
+// 5 Ghz channel plan index in the TxPower arrays.
+UCHAR A_BAND_REGION_0_CHANNEL_LIST[]={36, 40, 44, 48, 52, 56, 60, 64, 149, 153, 157, 161, 165};
+UCHAR A_BAND_REGION_1_CHANNEL_LIST[]={36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140};
+UCHAR A_BAND_REGION_2_CHANNEL_LIST[]={36, 40, 44, 48, 52, 56, 60, 64};
+UCHAR A_BAND_REGION_3_CHANNEL_LIST[]={52, 56, 60, 64, 149, 153, 157, 161};
+UCHAR A_BAND_REGION_4_CHANNEL_LIST[]={149, 153, 157, 161, 165};
+UCHAR A_BAND_REGION_5_CHANNEL_LIST[]={149, 153, 157, 161};
+UCHAR A_BAND_REGION_6_CHANNEL_LIST[]={36, 40, 44, 48};
+UCHAR A_BAND_REGION_7_CHANNEL_LIST[]={36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 149, 153, 157, 161, 165};
+UCHAR A_BAND_REGION_8_CHANNEL_LIST[]={52, 56, 60, 64};
+UCHAR A_BAND_REGION_9_CHANNEL_LIST[]={36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 132, 136, 140, 149, 153, 157, 161, 165};
+UCHAR A_BAND_REGION_10_CHANNEL_LIST[]={36, 40, 44, 48, 149, 153, 157, 161, 165};
+UCHAR A_BAND_REGION_11_CHANNEL_LIST[]={36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 149, 153, 157, 161};
+
+//BaSizeArray follows the 802.11n definition as MaxRxFactor.  2^(13+factor) bytes. When factor =0, it's about Ba buffer size =8.
+UCHAR BaSizeArray[4] = {8,16,32,64};
+
+/*
+	==========================================================================
+	Description:
+		Update StaCfg->ChannelList[] according to 1) Country Region 2) RF IC type,
+		and 3) PHY-mode user selected.
+		The outcome is used by driver when doing site survey.
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID BuildChannelList(
+	IN PRTMP_ADAPTER pAd)
+{
+	UCHAR i, j, index=0, num=0;
+	PUCHAR	pChannelList = NULL;
+
+	NdisZeroMemory(pAd->ChannelList, MAX_NUM_OF_CHANNELS * sizeof(CHANNEL_TX_POWER));
+
+	// if not 11a-only mode, channel list starts from 2.4Ghz band
+	if ((pAd->CommonCfg.PhyMode != PHY_11A)
+#ifdef DOT11_N_SUPPORT
+		&& (pAd->CommonCfg.PhyMode != PHY_11AN_MIXED) && (pAd->CommonCfg.PhyMode != PHY_11N_5G)
+#endif // DOT11_N_SUPPORT //
+	)
+	{
+		switch (pAd->CommonCfg.CountryRegion  & 0x7f)
+		{
+			case REGION_0_BG_BAND:	// 1 -11
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_0_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_0_SIZE);
+				index += BG_BAND_REGION_0_SIZE;
+				break;
+			case REGION_1_BG_BAND:	// 1 - 13
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_1_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_1_SIZE);
+				index += BG_BAND_REGION_1_SIZE;
+				break;
+			case REGION_2_BG_BAND:	// 10 - 11
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_2_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_2_SIZE);
+				index += BG_BAND_REGION_2_SIZE;
+				break;
+			case REGION_3_BG_BAND:	// 10 - 13
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_3_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_3_SIZE);
+				index += BG_BAND_REGION_3_SIZE;
+				break;
+			case REGION_4_BG_BAND:	// 14
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_4_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_4_SIZE);
+				index += BG_BAND_REGION_4_SIZE;
+				break;
+			case REGION_5_BG_BAND:	// 1 - 14
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_5_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_5_SIZE);
+				index += BG_BAND_REGION_5_SIZE;
+				break;
+			case REGION_6_BG_BAND:	// 3 - 9
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_6_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_6_SIZE);
+				index += BG_BAND_REGION_6_SIZE;
+				break;
+			case REGION_7_BG_BAND:  // 5 - 13
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_7_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_7_SIZE);
+				index += BG_BAND_REGION_7_SIZE;
+				break;
+			case REGION_31_BG_BAND:	// 1 - 14
+				NdisMoveMemory(&pAd->ChannelList[index], &pAd->TxPower[BG_BAND_REGION_31_START], sizeof(CHANNEL_TX_POWER) * BG_BAND_REGION_31_SIZE);
+				index += BG_BAND_REGION_31_SIZE;
+				break;
+			default:            // Error. should never happen
+				break;
+		}
+		for (i=0; i<index; i++)
+			pAd->ChannelList[i].MaxTxPwr = 20;
+	}
+
+	if ((pAd->CommonCfg.PhyMode == PHY_11A) || (pAd->CommonCfg.PhyMode == PHY_11ABG_MIXED)
+#ifdef DOT11_N_SUPPORT
+		|| (pAd->CommonCfg.PhyMode == PHY_11ABGN_MIXED) || (pAd->CommonCfg.PhyMode == PHY_11AN_MIXED)
+		|| (pAd->CommonCfg.PhyMode == PHY_11AGN_MIXED) || (pAd->CommonCfg.PhyMode == PHY_11N_5G)
+#endif // DOT11_N_SUPPORT //
+	)
+	{
+		switch (pAd->CommonCfg.CountryRegionForABand & 0x7f)
+		{
+			case REGION_0_A_BAND:
+				num = sizeof(A_BAND_REGION_0_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_0_CHANNEL_LIST;
+				break;
+			case REGION_1_A_BAND:
+				num = sizeof(A_BAND_REGION_1_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_1_CHANNEL_LIST;
+				break;
+			case REGION_2_A_BAND:
+				num = sizeof(A_BAND_REGION_2_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_2_CHANNEL_LIST;
+				break;
+			case REGION_3_A_BAND:
+				num = sizeof(A_BAND_REGION_3_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_3_CHANNEL_LIST;
+				break;
+			case REGION_4_A_BAND:
+				num = sizeof(A_BAND_REGION_4_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_4_CHANNEL_LIST;
+				break;
+			case REGION_5_A_BAND:
+				num = sizeof(A_BAND_REGION_5_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_5_CHANNEL_LIST;
+				break;
+			case REGION_6_A_BAND:
+				num = sizeof(A_BAND_REGION_6_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_6_CHANNEL_LIST;
+				break;
+			case REGION_7_A_BAND:
+				num = sizeof(A_BAND_REGION_7_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_7_CHANNEL_LIST;
+				break;
+			case REGION_8_A_BAND:
+				num = sizeof(A_BAND_REGION_8_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_8_CHANNEL_LIST;
+				break;
+			case REGION_9_A_BAND:
+				num = sizeof(A_BAND_REGION_9_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_9_CHANNEL_LIST;
+				break;
+
+			case REGION_10_A_BAND:
+				num = sizeof(A_BAND_REGION_10_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_10_CHANNEL_LIST;
+				break;
+
+			case REGION_11_A_BAND:
+				num = sizeof(A_BAND_REGION_11_CHANNEL_LIST)/sizeof(UCHAR);
+				pChannelList = A_BAND_REGION_11_CHANNEL_LIST;
+				break;
+
+			default:            // Error. should never happen
+				DBGPRINT(RT_DEBUG_WARN,("countryregion=%d not support", pAd->CommonCfg.CountryRegionForABand));
+				break;
+		}
+
+		if (num != 0)
+		{
+			UCHAR RadarCh[15]={52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140};
+			for (i=0; i<num; i++)
+			{
+				for (j=0; j<MAX_NUM_OF_CHANNELS; j++)
+				{
+					if (pChannelList[i] == pAd->TxPower[j].Channel)
+						NdisMoveMemory(&pAd->ChannelList[index+i], &pAd->TxPower[j], sizeof(CHANNEL_TX_POWER));
+					}
+				for (j=0; j<15; j++)
+				{
+					if (pChannelList[i] == RadarCh[j])
+						pAd->ChannelList[index+i].DfsReq = TRUE;
+				}
+				pAd->ChannelList[index+i].MaxTxPwr = 20;
+			}
+			index += num;
+		}
+	}
+
+	pAd->ChannelListNum = index;
+	DBGPRINT(RT_DEBUG_TRACE,("country code=%d/%d, RFIC=%d, PHY mode=%d, support %d channels\n",
+		pAd->CommonCfg.CountryRegion, pAd->CommonCfg.CountryRegionForABand, pAd->RfIcType, pAd->CommonCfg.PhyMode, pAd->ChannelListNum));
+#ifdef DBG
+	for (i=0;i<pAd->ChannelListNum;i++)
+	{
+		DBGPRINT_RAW(RT_DEBUG_TRACE,("BuildChannel # %d :: Pwr0 = %d, Pwr1 =%d, \n ", pAd->ChannelList[i].Channel, pAd->ChannelList[i].Power, pAd->ChannelList[i].Power2));
+	}
+#endif
+}
+
+/*
+	==========================================================================
+	Description:
+		This routine return the first channel number according to the country
+		code selection and RF IC selection (signal band or dual band). It is called
+		whenever driver need to start a site survey of all supported channels.
+	Return:
+		ch - the first channel number of current country code setting
+
+	IRQL = PASSIVE_LEVEL
+
+	==========================================================================
+ */
+UCHAR FirstChannel(
+	IN PRTMP_ADAPTER pAd)
+{
+	return pAd->ChannelList[0].Channel;
+}
+
+/*
+	==========================================================================
+	Description:
+		This routine returns the next channel number. This routine is called
+		during driver need to start a site survey of all supported channels.
+	Return:
+		next_channel - the next channel number valid in current country code setting.
+	Note:
+		return 0 if no more next channel
+	==========================================================================
+ */
+UCHAR NextChannel(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR channel)
+{
+	int i;
+	UCHAR next_channel = 0;
+
+	for (i = 0; i < (pAd->ChannelListNum - 1); i++)
+		if (channel == pAd->ChannelList[i].Channel)
+		{
+			next_channel = pAd->ChannelList[i+1].Channel;
+			break;
+	}
+	return next_channel;
+}
+
+/*
+	==========================================================================
+	Description:
+		This routine is for Cisco Compatible Extensions 2.X
+		Spec31. AP Control of Client Transmit Power
+	Return:
+		None
+	Note:
+	   Required by Aironet dBm(mW)
+		   0dBm(1mW),   1dBm(5mW), 13dBm(20mW), 15dBm(30mW),
+		  17dBm(50mw), 20dBm(100mW)
+
+	   We supported
+		   3dBm(Lowest), 6dBm(10%), 9dBm(25%), 12dBm(50%),
+		  14dBm(75%),   15dBm(100%)
+
+		The client station's actual transmit power shall be within +/- 5dB of
+		the minimum value or next lower value.
+	==========================================================================
+ */
+VOID ChangeToCellPowerLimit(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR         AironetCellPowerLimit)
+{
+	//valud 0xFF means that hasn't found power limit information
+	//from the AP's Beacon/Probe response.
+	if (AironetCellPowerLimit == 0xFF)
+		return;
+
+	if (AironetCellPowerLimit < 6) //Used Lowest Power Percentage.
+		pAd->CommonCfg.TxPowerPercentage = 6;
+	else if (AironetCellPowerLimit < 9)
+		pAd->CommonCfg.TxPowerPercentage = 10;
+	else if (AironetCellPowerLimit < 12)
+		pAd->CommonCfg.TxPowerPercentage = 25;
+	else if (AironetCellPowerLimit < 14)
+		pAd->CommonCfg.TxPowerPercentage = 50;
+	else if (AironetCellPowerLimit < 15)
+		pAd->CommonCfg.TxPowerPercentage = 75;
+	else
+		pAd->CommonCfg.TxPowerPercentage = 100; //else used maximum
+
+	if (pAd->CommonCfg.TxPowerPercentage > pAd->CommonCfg.TxPowerDefault)
+		pAd->CommonCfg.TxPowerPercentage = pAd->CommonCfg.TxPowerDefault;
+
+}
+
+CHAR	ConvertToRssi(
+	IN PRTMP_ADAPTER pAd,
+	IN	CHAR			Rssi,
+	IN  UCHAR   RssiNumber)
+{
+	UCHAR	RssiOffset, LNAGain;
+
+	// Rssi equals to zero should be an invalid value
+	if (Rssi == 0)
+		return -99;
+
+	LNAGain = GET_LNA_GAIN(pAd);
+    if (pAd->LatchRfRegs.Channel > 14)
+    {
+        if (RssiNumber == 0)
+			RssiOffset = pAd->ARssiOffset0;
+		else if (RssiNumber == 1)
+			RssiOffset = pAd->ARssiOffset1;
+		else
+			RssiOffset = pAd->ARssiOffset2;
+    }
+    else
+    {
+        if (RssiNumber == 0)
+			RssiOffset = pAd->BGRssiOffset0;
+		else if (RssiNumber == 1)
+			RssiOffset = pAd->BGRssiOffset1;
+		else
+			RssiOffset = pAd->BGRssiOffset2;
+    }
+
+    return (-12 - RssiOffset - LNAGain - Rssi);
+}
+
+/*
+	==========================================================================
+	Description:
+		Scan next channel
+	==========================================================================
+ */
+VOID ScanNextChannel(
+	IN PRTMP_ADAPTER pAd)
+{
+	HEADER_802_11   Hdr80211;
+	PUCHAR          pOutBuffer = NULL;
+	NDIS_STATUS     NStatus;
+	ULONG           FrameLen = 0;
+	UCHAR           SsidLen = 0, ScanType = pAd->MlmeAux.ScanType, BBPValue = 0;
+#ifdef CONFIG_STA_SUPPORT
+	USHORT          Status;
+	PHEADER_802_11  pHdr80211;
+#endif // CONFIG_STA_SUPPORT //
+	UINT			ScanTimeIn5gChannel = SHORT_CHANNEL_TIME;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (MONITOR_ON(pAd))
+			return;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef RALINK_ATE
+	// Nothing to do in ATE mode.
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+	if (pAd->MlmeAux.Channel == 0)
+	{
+		if ((pAd->CommonCfg.BBPCurrentBW == BW_40)
+#ifdef CONFIG_STA_SUPPORT
+			&& (INFRA_ON(pAd)
+				|| (pAd->OpMode == OPMODE_AP))
+#endif // CONFIG_STA_SUPPORT //
+			)
+		{
+			AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+			AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+			BBPValue &= (~0x18);
+			BBPValue |= 0x10;
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+			DBGPRINT(RT_DEBUG_TRACE, ("SYNC - End of SCAN, restore to 40MHz channel %d, Total BSS[%02d]\n",pAd->CommonCfg.CentralChannel, pAd->ScanTab.BssNr));
+		}
+		else
+		{
+			AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+			AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+			DBGPRINT(RT_DEBUG_TRACE, ("SYNC - End of SCAN, restore to channel %d, Total BSS[%02d]\n",pAd->CommonCfg.Channel, pAd->ScanTab.BssNr));
+		}
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			//
+			// To prevent data lost.
+			// Send an NULL data with turned PSM bit on to current associated AP before SCAN progress.
+			// Now, we need to send an NULL data with turned PSM bit off to AP, when scan progress done
+			//
+			if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED) && (INFRA_ON(pAd)))
+			{
+				NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);
+				if (NStatus	== NDIS_STATUS_SUCCESS)
+				{
+					pHdr80211 = (PHEADER_802_11) pOutBuffer;
+					MgtMacHeaderInit(pAd, pHdr80211, SUBTYPE_NULL_FUNC, 1, pAd->CommonCfg.Bssid, pAd->CommonCfg.Bssid);
+					pHdr80211->Duration = 0;
+					pHdr80211->FC.Type = BTYPE_DATA;
+					pHdr80211->FC.PwrMgmt = (pAd->StaCfg.Psm == PWR_SAVE);
+
+					// Send using priority queue
+					MiniportMMRequest(pAd, 0, pOutBuffer, sizeof(HEADER_802_11));
+					DBGPRINT(RT_DEBUG_TRACE, ("MlmeScanReqAction -- Send PSM Data frame\n"));
+					MlmeFreeMemory(pAd, pOutBuffer);
+					RTMPusecDelay(5000);
+				}
+			}
+
+			pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+			Status = MLME_SUCCESS;
+			MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_SCAN_CONF, 2, &Status);
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+
+		RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS);
+	}
+#ifdef RT2870
+#ifdef CONFIG_STA_SUPPORT
+	else if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST) && (pAd->OpMode == OPMODE_STA))
+	{
+		pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+		MlmeCntlConfirm(pAd, MT2_SCAN_CONF, MLME_FAIL_NO_RESOURCE);
+	}
+#endif // CONFIG_STA_SUPPORT //
+#endif // RT2870 //
+	else
+	{
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+		// BBP and RF are not accessible in PS mode, we has to wake them up first
+		if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+			AsicForceWakeup(pAd, TRUE);
+
+			// leave PSM during scanning. otherwise we may lost ProbeRsp & BEACON
+			if (pAd->StaCfg.Psm == PWR_SAVE)
+				MlmeSetPsmBit(pAd, PWR_ACTIVE);
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+		AsicSwitchChannel(pAd, pAd->MlmeAux.Channel, TRUE);
+		AsicLockChannel(pAd, pAd->MlmeAux.Channel);
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			if (pAd->MlmeAux.Channel > 14)
+			{
+				if ((pAd->CommonCfg.bIEEE80211H == 1) && RadarChannelCheck(pAd, pAd->MlmeAux.Channel))
+				{
+					ScanType = SCAN_PASSIVE;
+					ScanTimeIn5gChannel = MIN_CHANNEL_TIME;
+				}
+			}
+
+#ifdef CARRIER_DETECTION_SUPPORT // Roger sync Carrier
+			// carrier detection
+			if (pAd->CommonCfg.CarrierDetect.Enable == TRUE)
+			{
+				ScanType = SCAN_PASSIVE;
+				ScanTimeIn5gChannel = MIN_CHANNEL_TIME;
+			}
+#endif // CARRIER_DETECTION_SUPPORT //
+		}
+
+#endif // CONFIG_STA_SUPPORT //
+
+		//Global country domain(ch1-11:active scan, ch12-14 passive scan)
+		if ((pAd->MlmeAux.Channel <= 14) && (pAd->MlmeAux.Channel >= 12) && ((pAd->CommonCfg.CountryRegion & 0x7f) == REGION_31_BG_BAND))
+		{
+			ScanType = SCAN_PASSIVE;
+		}
+
+		// We need to shorten active scan time in order for WZC connect issue
+		// Chnage the channel scan time for CISCO stuff based on its IAPP announcement
+		if (ScanType == FAST_SCAN_ACTIVE)
+			RTMPSetTimer(&pAd->MlmeAux.ScanTimer, FAST_ACTIVE_SCAN_TIME);
+#ifdef CONFIG_STA_SUPPORT
+		else if (((ScanType == SCAN_CISCO_ACTIVE) ||
+				(ScanType == SCAN_CISCO_PASSIVE) ||
+				(ScanType == SCAN_CISCO_CHANNEL_LOAD) ||
+				(ScanType == SCAN_CISCO_NOISE)) && (pAd->OpMode == OPMODE_STA))
+		{
+			if (pAd->StaCfg.CCXScanTime < 25)
+				RTMPSetTimer(&pAd->MlmeAux.ScanTimer, pAd->StaCfg.CCXScanTime * 2);
+			else
+				RTMPSetTimer(&pAd->MlmeAux.ScanTimer, pAd->StaCfg.CCXScanTime);
+		}
+#endif // CONFIG_STA_SUPPORT //
+		else // must be SCAN_PASSIVE or SCAN_ACTIVE
+		{
+			if ((pAd->CommonCfg.PhyMode == PHY_11ABG_MIXED)
+#ifdef DOT11_N_SUPPORT
+				|| (pAd->CommonCfg.PhyMode == PHY_11ABGN_MIXED) || (pAd->CommonCfg.PhyMode == PHY_11AGN_MIXED)
+#endif // DOT11_N_SUPPORT //
+			)
+			{
+				if (pAd->MlmeAux.Channel > 14)
+					RTMPSetTimer(&pAd->MlmeAux.ScanTimer, ScanTimeIn5gChannel);
+				else
+				RTMPSetTimer(&pAd->MlmeAux.ScanTimer, MIN_CHANNEL_TIME);
+			}
+			else
+				RTMPSetTimer(&pAd->MlmeAux.ScanTimer, MAX_CHANNEL_TIME);
+		}
+
+		if ((ScanType == SCAN_ACTIVE) || (ScanType == FAST_SCAN_ACTIVE) ||
+			(ScanType == SCAN_CISCO_ACTIVE))
+		{
+			NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+			if (NStatus != NDIS_STATUS_SUCCESS)
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("SYNC - ScanNextChannel() allocate memory fail\n"));
+#ifdef CONFIG_STA_SUPPORT
+				IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				{
+					pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+					Status = MLME_FAIL_NO_RESOURCE;
+					MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_SCAN_CONF, 2, &Status);
+				}
+#endif // CONFIG_STA_SUPPORT //
+
+				return;
+			}
+
+			// There is no need to send broadcast probe request if active scan is in effect.
+			if ((ScanType == SCAN_ACTIVE) || (ScanType == FAST_SCAN_ACTIVE)
+				)
+				SsidLen = pAd->MlmeAux.SsidLen;
+			else
+				SsidLen = 0;
+
+			MgtMacHeaderInit(pAd, &Hdr80211, SUBTYPE_PROBE_REQ, 0, BROADCAST_ADDR, BROADCAST_ADDR);
+			MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+							  sizeof(HEADER_802_11),    &Hdr80211,
+							  1,                        &SsidIe,
+							  1,                        &SsidLen,
+							  SsidLen,			        pAd->MlmeAux.Ssid,
+							  1,                        &SupRateIe,
+							  1,                        &pAd->CommonCfg.SupRateLen,
+							  pAd->CommonCfg.SupRateLen,  pAd->CommonCfg.SupRate,
+							  END_OF_ARGS);
+
+			if (pAd->CommonCfg.ExtRateLen)
+			{
+				ULONG Tmp;
+				MakeOutgoingFrame(pOutBuffer + FrameLen,            &Tmp,
+								  1,                                &ExtRateIe,
+								  1,                                &pAd->CommonCfg.ExtRateLen,
+								  pAd->CommonCfg.ExtRateLen,          pAd->CommonCfg.ExtRate,
+								  END_OF_ARGS);
+				FrameLen += Tmp;
+			}
+
+#ifdef DOT11_N_SUPPORT
+			if (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED)
+			{
+				ULONG	Tmp;
+				UCHAR	HtLen;
+				UCHAR	BROADCOM[4] = {0x0, 0x90, 0x4c, 0x33};
+#ifdef RT_BIG_ENDIAN
+				HT_CAPABILITY_IE HtCapabilityTmp;
+#endif
+				if (pAd->bBroadComHT == TRUE)
+				{
+					HtLen = pAd->MlmeAux.HtCapabilityLen + 4;
+#ifdef RT_BIG_ENDIAN
+					NdisMoveMemory(&HtCapabilityTmp, &pAd->MlmeAux.HtCapability, SIZE_HT_CAP_IE);
+					*(USHORT *)(&HtCapabilityTmp.HtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.HtCapInfo));
+					*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo));
+
+					MakeOutgoingFrame(pOutBuffer + FrameLen,          &Tmp,
+									1,                                &WpaIe,
+									1,                                &HtLen,
+									4,                                &BROADCOM[0],
+									pAd->MlmeAux.HtCapabilityLen,     &HtCapabilityTmp,
+									END_OF_ARGS);
+#else
+					MakeOutgoingFrame(pOutBuffer + FrameLen,          &Tmp,
+									1,                                &WpaIe,
+									1,                                &HtLen,
+									4,                                &BROADCOM[0],
+									pAd->MlmeAux.HtCapabilityLen,     &pAd->MlmeAux.HtCapability,
+									END_OF_ARGS);
+#endif // RT_BIG_ENDIAN //
+				}
+				else
+				{
+					HtLen = pAd->MlmeAux.HtCapabilityLen;
+#ifdef RT_BIG_ENDIAN
+					NdisMoveMemory(&HtCapabilityTmp, &pAd->CommonCfg.HtCapability, SIZE_HT_CAP_IE);
+					*(USHORT *)(&HtCapabilityTmp.HtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.HtCapInfo));
+					*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo));
+
+					MakeOutgoingFrame(pOutBuffer + FrameLen,          &Tmp,
+									1,                                &HtCapIe,
+									1,                                &HtLen,
+									HtLen,                            &HtCapabilityTmp,
+									END_OF_ARGS);
+#else
+					MakeOutgoingFrame(pOutBuffer + FrameLen,          &Tmp,
+									1,                                &HtCapIe,
+									1,                                &HtLen,
+									HtLen,                            &pAd->CommonCfg.HtCapability,
+									END_OF_ARGS);
+#endif // RT_BIG_ENDIAN //
+				}
+				FrameLen += Tmp;
+
+#ifdef DOT11N_DRAFT3
+				if (pAd->CommonCfg.BACapability.field.b2040CoexistScanSup == 1)
+				{
+					ULONG		Tmp;
+					HtLen = 1;
+					MakeOutgoingFrame(pOutBuffer + FrameLen,            &Tmp,
+									  1,					&ExtHtCapIe,
+									  1,					&HtLen,
+									  1,          			&pAd->CommonCfg.BSSCoexist2040.word,
+									  END_OF_ARGS);
+
+					FrameLen += Tmp;
+				}
+#endif // DOT11N_DRAFT3 //
+			}
+#endif // DOT11_N_SUPPORT //
+
+
+			MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+			MlmeFreeMemory(pAd, pOutBuffer);
+		}
+
+		// For SCAN_CISCO_PASSIVE, do nothing and silently wait for beacon or other probe reponse
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			pAd->Mlme.SyncMachine.CurrState = SCAN_LISTEN;
+#endif // CONFIG_STA_SUPPORT //
+
+	}
+}
+
+VOID MgtProbReqMacHeaderInit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN OUT PHEADER_802_11 pHdr80211,
+	IN UCHAR SubType,
+	IN UCHAR ToDs,
+	IN PUCHAR pDA,
+	IN PUCHAR pBssid)
+{
+	NdisZeroMemory(pHdr80211, sizeof(HEADER_802_11));
+
+	pHdr80211->FC.Type = BTYPE_MGMT;
+	pHdr80211->FC.SubType = SubType;
+	if (SubType == SUBTYPE_ACK)
+		pHdr80211->FC.Type = BTYPE_CNTL;
+	pHdr80211->FC.ToDs = ToDs;
+	COPY_MAC_ADDR(pHdr80211->Addr1, pDA);
+	COPY_MAC_ADDR(pHdr80211->Addr2, pAd->CurrentAddress);
+	COPY_MAC_ADDR(pHdr80211->Addr3, pBssid);
+}
+
+
diff --git a/drivers/staging/rt2870/common/cmm_wpa.c b/drivers/staging/rt2870/common/cmm_wpa.c
new file mode 100644
index 00000000000..d2c24bd49e7
--- /dev/null
+++ b/drivers/staging/rt2870/common/cmm_wpa.c
@@ -0,0 +1,1654 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	wpa.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Jan	Lee		03-07-22		Initial
+	Paul Lin	03-11-28		Modify for supplicant
+*/
+#include "../rt_config.h"
+// WPA OUI
+UCHAR		OUI_WPA_NONE_AKM[4]		= {0x00, 0x50, 0xF2, 0x00};
+UCHAR       OUI_WPA_VERSION[4]      = {0x00, 0x50, 0xF2, 0x01};
+UCHAR       OUI_WPA_WEP40[4]      = {0x00, 0x50, 0xF2, 0x01};
+UCHAR       OUI_WPA_TKIP[4]     = {0x00, 0x50, 0xF2, 0x02};
+UCHAR       OUI_WPA_CCMP[4]     = {0x00, 0x50, 0xF2, 0x04};
+UCHAR       OUI_WPA_WEP104[4]      = {0x00, 0x50, 0xF2, 0x05};
+UCHAR       OUI_WPA_8021X_AKM[4]	= {0x00, 0x50, 0xF2, 0x01};
+UCHAR       OUI_WPA_PSK_AKM[4]      = {0x00, 0x50, 0xF2, 0x02};
+// WPA2 OUI
+UCHAR       OUI_WPA2_WEP40[4]   = {0x00, 0x0F, 0xAC, 0x01};
+UCHAR       OUI_WPA2_TKIP[4]        = {0x00, 0x0F, 0xAC, 0x02};
+UCHAR       OUI_WPA2_CCMP[4]        = {0x00, 0x0F, 0xAC, 0x04};
+UCHAR       OUI_WPA2_8021X_AKM[4]   = {0x00, 0x0F, 0xAC, 0x01};
+UCHAR       OUI_WPA2_PSK_AKM[4]   	= {0x00, 0x0F, 0xAC, 0x02};
+UCHAR       OUI_WPA2_WEP104[4]   = {0x00, 0x0F, 0xAC, 0x05};
+// MSA OUI
+UCHAR   	OUI_MSA_8021X_AKM[4]    = {0x00, 0x0F, 0xAC, 0x05};		// Not yet final - IEEE 802.11s-D1.06
+UCHAR   	OUI_MSA_PSK_AKM[4]   	= {0x00, 0x0F, 0xAC, 0x06};		// Not yet final - IEEE 802.11s-D1.06
+
+/*
+	========================================================================
+
+	Routine Description:
+		The pseudo-random function(PRF) that hashes various inputs to
+		derive a pseudo-random value. To add liveness to the pseudo-random
+		value, a nonce should be one of the inputs.
+
+		It is used to generate PTK, GTK or some specific random value.
+
+	Arguments:
+		UCHAR	*key,		-	the key material for HMAC_SHA1 use
+		INT		key_len		-	the length of key
+		UCHAR	*prefix		-	a prefix label
+		INT		prefix_len	-	the length of the label
+		UCHAR	*data		-	a specific data with variable length
+		INT		data_len	-	the length of a specific data
+		INT		len			-	the output lenght
+
+	Return Value:
+		UCHAR	*output		-	the calculated result
+
+	Note:
+		802.11i-2004	Annex H.3
+
+	========================================================================
+*/
+VOID	PRF(
+	IN	UCHAR	*key,
+	IN	INT		key_len,
+	IN	UCHAR	*prefix,
+	IN	INT		prefix_len,
+	IN	UCHAR	*data,
+	IN	INT		data_len,
+	OUT	UCHAR	*output,
+	IN	INT		len)
+{
+	INT		i;
+    UCHAR   *input;
+	INT		currentindex = 0;
+	INT		total_len;
+
+	// Allocate memory for input
+	os_alloc_mem(NULL, (PUCHAR *)&input, 1024);
+
+    if (input == NULL)
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("!!!PRF: no memory!!!\n"));
+        return;
+    }
+
+	// Generate concatenation input
+	NdisMoveMemory(input, prefix, prefix_len);
+
+	// Concatenate a single octet containing 0
+	input[prefix_len] =	0;
+
+	// Concatenate specific data
+	NdisMoveMemory(&input[prefix_len + 1], data, data_len);
+	total_len =	prefix_len + 1 + data_len;
+
+	// Concatenate a single octet containing 0
+	// This octet shall be update later
+	input[total_len] = 0;
+	total_len++;
+
+	// Iterate to calculate the result by hmac-sha-1
+	// Then concatenate to last result
+	for	(i = 0;	i <	(len + 19) / 20; i++)
+	{
+		HMAC_SHA1(input, total_len,	key, key_len, &output[currentindex]);
+		currentindex +=	20;
+
+		// update the last octet
+		input[total_len - 1]++;
+	}
+    os_free_mem(NULL, input);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		It utilizes PRF-384 or PRF-512 to derive session-specific keys from a PMK.
+		It shall be called by 4-way handshake processing.
+
+	Arguments:
+		pAd 	-	pointer to our pAdapter context
+		PMK		-	pointer to PMK
+		ANonce	-	pointer to ANonce
+		AA		-	pointer to Authenticator Address
+		SNonce	-	pointer to SNonce
+		SA		-	pointer to Supplicant Address
+		len		-	indicate the length of PTK (octet)
+
+	Return Value:
+		Output		pointer to the PTK
+
+	Note:
+		Refer to IEEE 802.11i-2004 8.5.1.2
+
+	========================================================================
+*/
+VOID WpaCountPTK(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR	*PMK,
+	IN	UCHAR	*ANonce,
+	IN	UCHAR	*AA,
+	IN	UCHAR	*SNonce,
+	IN	UCHAR	*SA,
+	OUT	UCHAR	*output,
+	IN	UINT	len)
+{
+	UCHAR	concatenation[76];
+	UINT	CurrPos = 0;
+	UCHAR	temp[32];
+	UCHAR	Prefix[] = {'P', 'a', 'i', 'r', 'w', 'i', 's', 'e', ' ', 'k', 'e', 'y', ' ',
+						'e', 'x', 'p', 'a', 'n', 's', 'i', 'o', 'n'};
+
+	// initiate the concatenation input
+	NdisZeroMemory(temp, sizeof(temp));
+	NdisZeroMemory(concatenation, 76);
+
+	// Get smaller address
+	if (RTMPCompareMemory(SA, AA, 6) == 1)
+		NdisMoveMemory(concatenation, AA, 6);
+	else
+		NdisMoveMemory(concatenation, SA, 6);
+	CurrPos += 6;
+
+	// Get larger address
+	if (RTMPCompareMemory(SA, AA, 6) == 1)
+		NdisMoveMemory(&concatenation[CurrPos], SA, 6);
+	else
+		NdisMoveMemory(&concatenation[CurrPos], AA, 6);
+
+	// store the larger mac address for backward compatible of
+	// ralink proprietary STA-key issue
+	NdisMoveMemory(temp, &concatenation[CurrPos], MAC_ADDR_LEN);
+	CurrPos += 6;
+
+	// Get smaller Nonce
+	if (RTMPCompareMemory(ANonce, SNonce, 32) == 0)
+		NdisMoveMemory(&concatenation[CurrPos], temp, 32);	// patch for ralink proprietary STA-key issue
+	else if (RTMPCompareMemory(ANonce, SNonce, 32) == 1)
+		NdisMoveMemory(&concatenation[CurrPos], SNonce, 32);
+	else
+		NdisMoveMemory(&concatenation[CurrPos], ANonce, 32);
+	CurrPos += 32;
+
+	// Get larger Nonce
+	if (RTMPCompareMemory(ANonce, SNonce, 32) == 0)
+		NdisMoveMemory(&concatenation[CurrPos], temp, 32);	// patch for ralink proprietary STA-key issue
+	else if (RTMPCompareMemory(ANonce, SNonce, 32) == 1)
+		NdisMoveMemory(&concatenation[CurrPos], ANonce, 32);
+	else
+		NdisMoveMemory(&concatenation[CurrPos], SNonce, 32);
+	CurrPos += 32;
+
+	hex_dump("concatenation=", concatenation, 76);
+
+	// Use PRF to generate PTK
+	PRF(PMK, LEN_MASTER_KEY, Prefix, 22, concatenation, 76, output, len);
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Generate random number by software.
+
+	Arguments:
+		pAd		-	pointer to our pAdapter context
+		macAddr	-	pointer to local MAC address
+
+	Return Value:
+
+	Note:
+		802.1ii-2004  Annex H.5
+
+	========================================================================
+*/
+VOID	GenRandom(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			*macAddr,
+	OUT	UCHAR			*random)
+{
+	INT		i, curr;
+	UCHAR	local[80], KeyCounter[32];
+	UCHAR	result[80];
+	ULONG	CurrentTime;
+	UCHAR	prefix[] = {'I', 'n', 'i', 't', ' ', 'C', 'o', 'u', 'n', 't', 'e', 'r'};
+
+	// Zero the related information
+	NdisZeroMemory(result, 80);
+	NdisZeroMemory(local, 80);
+	NdisZeroMemory(KeyCounter, 32);
+
+	for	(i = 0;	i <	32;	i++)
+	{
+		// copy the local MAC address
+		COPY_MAC_ADDR(local, macAddr);
+		curr =	MAC_ADDR_LEN;
+
+		// concatenate the current time
+		NdisGetSystemUpTime(&CurrentTime);
+		NdisMoveMemory(&local[curr],  &CurrentTime,	sizeof(CurrentTime));
+		curr +=	sizeof(CurrentTime);
+
+		// concatenate the last result
+		NdisMoveMemory(&local[curr],  result, 32);
+		curr +=	32;
+
+		// concatenate a variable
+		NdisMoveMemory(&local[curr],  &i,  2);
+		curr +=	2;
+
+		// calculate the result
+		PRF(KeyCounter, 32, prefix,12, local, curr, result, 32);
+	}
+
+	NdisMoveMemory(random, result,	32);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Build cipher suite in RSN-IE.
+		It only shall be called by RTMPMakeRSNIE.
+
+	Arguments:
+		pAd			-	pointer to our pAdapter context
+    	ElementID	-	indicate the WPA1 or WPA2
+    	WepStatus	-	indicate the encryption type
+		bMixCipher	-	a boolean to indicate the pairwise cipher and group
+						cipher are the same or not
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+static VOID RTMPInsertRsnIeCipher(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	UCHAR			ElementID,
+	IN	UINT			WepStatus,
+	IN	BOOLEAN			bMixCipher,
+	IN	UCHAR			FlexibleCipher,
+	OUT	PUCHAR			pRsnIe,
+	OUT	UCHAR			*rsn_len)
+{
+	UCHAR	PairwiseCnt;
+
+	*rsn_len = 0;
+
+	// decide WPA2 or WPA1
+	if (ElementID == Wpa2Ie)
+	{
+		RSNIE2	*pRsnie_cipher = (RSNIE2*)pRsnIe;
+
+		// Assign the verson as 1
+		pRsnie_cipher->version = 1;
+
+        switch (WepStatus)
+        {
+        	// TKIP mode
+            case Ndis802_11Encryption2Enabled:
+                NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA2_TKIP, 4);
+                pRsnie_cipher->ucount = 1;
+                NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA2_TKIP, 4);
+                *rsn_len = sizeof(RSNIE2);
+                break;
+
+			// AES mode
+            case Ndis802_11Encryption3Enabled:
+				if (bMixCipher)
+					NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA2_TKIP, 4);
+				else
+					NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA2_CCMP, 4);
+                pRsnie_cipher->ucount = 1;
+                NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA2_CCMP, 4);
+                *rsn_len = sizeof(RSNIE2);
+                break;
+
+			// TKIP-AES mix mode
+            case Ndis802_11Encryption4Enabled:
+                NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA2_TKIP, 4);
+
+				PairwiseCnt = 1;
+				// Insert WPA2 TKIP as the first pairwise cipher
+				if (MIX_CIPHER_WPA2_TKIP_ON(FlexibleCipher))
+				{
+                	NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA2_TKIP, 4);
+					// Insert WPA2 AES as the secondary pairwise cipher
+					if (MIX_CIPHER_WPA2_AES_ON(FlexibleCipher))
+					{
+                		NdisMoveMemory(pRsnie_cipher->ucast[0].oui + 4, OUI_WPA2_CCMP, 4);
+						PairwiseCnt = 2;
+					}
+				}
+				else
+				{
+					// Insert WPA2 AES as the first pairwise cipher
+					NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA2_CCMP, 4);
+				}
+
+                pRsnie_cipher->ucount = PairwiseCnt;
+                *rsn_len = sizeof(RSNIE2) + (4 * (PairwiseCnt - 1));
+                break;
+        }
+
+#ifdef CONFIG_STA_SUPPORT
+		if ((pAd->OpMode == OPMODE_STA) &&
+			(pAd->StaCfg.GroupCipher != Ndis802_11Encryption2Enabled) &&
+			(pAd->StaCfg.GroupCipher != Ndis802_11Encryption3Enabled))
+		{
+			UINT GroupCipher = pAd->StaCfg.GroupCipher;
+			switch(GroupCipher)
+			{
+				case Ndis802_11GroupWEP40Enabled:
+					NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA2_WEP40, 4);
+					break;
+				case Ndis802_11GroupWEP104Enabled:
+					NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA2_WEP104, 4);
+					break;
+			}
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+		// swap for big-endian platform
+		pRsnie_cipher->version = cpu2le16(pRsnie_cipher->version);
+	    pRsnie_cipher->ucount = cpu2le16(pRsnie_cipher->ucount);
+	}
+	else
+	{
+		RSNIE	*pRsnie_cipher = (RSNIE*)pRsnIe;
+
+		// Assign OUI and version
+		NdisMoveMemory(pRsnie_cipher->oui, OUI_WPA_VERSION, 4);
+        pRsnie_cipher->version = 1;
+
+		switch (WepStatus)
+		{
+			// TKIP mode
+            case Ndis802_11Encryption2Enabled:
+                NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA_TKIP, 4);
+                pRsnie_cipher->ucount = 1;
+                NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA_TKIP, 4);
+                *rsn_len = sizeof(RSNIE);
+                break;
+
+			// AES mode
+            case Ndis802_11Encryption3Enabled:
+				if (bMixCipher)
+					NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA_TKIP, 4);
+				else
+					NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA_CCMP, 4);
+                pRsnie_cipher->ucount = 1;
+                NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA_CCMP, 4);
+                *rsn_len = sizeof(RSNIE);
+                break;
+
+			// TKIP-AES mix mode
+            case Ndis802_11Encryption4Enabled:
+                NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA_TKIP, 4);
+
+				PairwiseCnt = 1;
+				// Insert WPA TKIP as the first pairwise cipher
+				if (MIX_CIPHER_WPA_TKIP_ON(FlexibleCipher))
+				{
+                	NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA_TKIP, 4);
+					// Insert WPA AES as the secondary pairwise cipher
+					if (MIX_CIPHER_WPA_AES_ON(FlexibleCipher))
+					{
+                		NdisMoveMemory(pRsnie_cipher->ucast[0].oui + 4, OUI_WPA_CCMP, 4);
+						PairwiseCnt = 2;
+					}
+				}
+				else
+				{
+					// Insert WPA AES as the first pairwise cipher
+					NdisMoveMemory(pRsnie_cipher->ucast[0].oui, OUI_WPA_CCMP, 4);
+				}
+
+                pRsnie_cipher->ucount = PairwiseCnt;
+                *rsn_len = sizeof(RSNIE) + (4 * (PairwiseCnt - 1));
+                break;
+        }
+
+#ifdef CONFIG_STA_SUPPORT
+		if ((pAd->OpMode == OPMODE_STA) &&
+			(pAd->StaCfg.GroupCipher != Ndis802_11Encryption2Enabled) &&
+			(pAd->StaCfg.GroupCipher != Ndis802_11Encryption3Enabled))
+		{
+			UINT GroupCipher = pAd->StaCfg.GroupCipher;
+			switch(GroupCipher)
+			{
+				case Ndis802_11GroupWEP40Enabled:
+					NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA_WEP40, 4);
+					break;
+				case Ndis802_11GroupWEP104Enabled:
+					NdisMoveMemory(pRsnie_cipher->mcast, OUI_WPA_WEP104, 4);
+					break;
+			}
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+		// swap for big-endian platform
+		pRsnie_cipher->version = cpu2le16(pRsnie_cipher->version);
+	    pRsnie_cipher->ucount = cpu2le16(pRsnie_cipher->ucount);
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Build AKM suite in RSN-IE.
+		It only shall be called by RTMPMakeRSNIE.
+
+	Arguments:
+		pAd			-	pointer to our pAdapter context
+    	ElementID	-	indicate the WPA1 or WPA2
+    	AuthMode	-	indicate the authentication mode
+		apidx		-	indicate the interface index
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+static VOID RTMPInsertRsnIeAKM(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	UCHAR			ElementID,
+	IN	UINT			AuthMode,
+	IN	UCHAR			apidx,
+	OUT	PUCHAR			pRsnIe,
+	OUT	UCHAR			*rsn_len)
+{
+	RSNIE_AUTH		*pRsnie_auth;
+
+	pRsnie_auth = (RSNIE_AUTH*)(pRsnIe + (*rsn_len));
+
+	// decide WPA2 or WPA1
+	if (ElementID == Wpa2Ie)
+	{
+		switch (AuthMode)
+        {
+            case Ndis802_11AuthModeWPA2:
+            case Ndis802_11AuthModeWPA1WPA2:
+                pRsnie_auth->acount = 1;
+                	NdisMoveMemory(pRsnie_auth->auth[0].oui, OUI_WPA2_8021X_AKM, 4);
+                break;
+
+            case Ndis802_11AuthModeWPA2PSK:
+            case Ndis802_11AuthModeWPA1PSKWPA2PSK:
+                pRsnie_auth->acount = 1;
+                	NdisMoveMemory(pRsnie_auth->auth[0].oui, OUI_WPA2_PSK_AKM, 4);
+                break;
+        }
+	}
+	else
+	{
+		switch (AuthMode)
+        {
+            case Ndis802_11AuthModeWPA:
+            case Ndis802_11AuthModeWPA1WPA2:
+                pRsnie_auth->acount = 1;
+                NdisMoveMemory(pRsnie_auth->auth[0].oui, OUI_WPA_8021X_AKM, 4);
+                break;
+
+            case Ndis802_11AuthModeWPAPSK:
+            case Ndis802_11AuthModeWPA1PSKWPA2PSK:
+                pRsnie_auth->acount = 1;
+                NdisMoveMemory(pRsnie_auth->auth[0].oui, OUI_WPA_PSK_AKM, 4);
+                break;
+
+			case Ndis802_11AuthModeWPANone:
+                pRsnie_auth->acount = 1;
+                NdisMoveMemory(pRsnie_auth->auth[0].oui, OUI_WPA_NONE_AKM, 4);
+                break;
+        }
+	}
+
+	pRsnie_auth->acount = cpu2le16(pRsnie_auth->acount);
+
+	(*rsn_len) += sizeof(RSNIE_AUTH);	// update current RSNIE length
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Build capability in RSN-IE.
+		It only shall be called by RTMPMakeRSNIE.
+
+	Arguments:
+		pAd			-	pointer to our pAdapter context
+    	ElementID	-	indicate the WPA1 or WPA2
+		apidx		-	indicate the interface index
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+static VOID RTMPInsertRsnIeCap(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	UCHAR			ElementID,
+	IN	UCHAR			apidx,
+	OUT	PUCHAR			pRsnIe,
+	OUT	UCHAR			*rsn_len)
+{
+	RSN_CAPABILITIES    *pRSN_Cap;
+
+	// it could be ignored in WPA1 mode
+	if (ElementID == WpaIe)
+		return;
+
+	pRSN_Cap = (RSN_CAPABILITIES*)(pRsnIe + (*rsn_len));
+
+
+	pRSN_Cap->word = cpu2le16(pRSN_Cap->word);
+
+	(*rsn_len) += sizeof(RSN_CAPABILITIES);	// update current RSNIE length
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Build RSN IE context. It is not included element-ID and length.
+
+	Arguments:
+		pAd			-	pointer to our pAdapter context
+    	AuthMode	-	indicate the authentication mode
+    	WepStatus	-	indicate the encryption type
+		apidx		-	indicate the interface index
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID RTMPMakeRSNIE(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  UINT            AuthMode,
+    IN  UINT            WepStatus,
+	IN	UCHAR			apidx)
+{
+	PUCHAR		pRsnIe = NULL;			// primary RSNIE
+	UCHAR 		*rsnielen_cur_p = 0;	// the length of the primary RSNIE
+	UCHAR		*rsnielen_ex_cur_p = 0;	// the length of the secondary RSNIE
+	UCHAR		PrimaryRsnie;
+	BOOLEAN		bMixCipher = FALSE;	// indicate the pairwise and group cipher are different
+	UCHAR		p_offset;
+	WPA_MIX_PAIR_CIPHER		FlexibleCipher = MIX_CIPHER_NOTUSE;	// it provide the more flexible cipher combination in WPA-WPA2 and TKIPAES mode
+
+	rsnielen_cur_p = NULL;
+	rsnielen_ex_cur_p = NULL;
+
+	{
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+#ifdef WPA_SUPPLICANT_SUPPORT
+			if (pAd->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_DISABLE)
+			{
+				if (AuthMode < Ndis802_11AuthModeWPA)
+					return;
+			}
+			else
+#endif // WPA_SUPPLICANT_SUPPORT //
+			{
+				// Support WPAPSK or WPA2PSK in STA-Infra mode
+				// Support WPANone in STA-Adhoc mode
+				if ((AuthMode != Ndis802_11AuthModeWPAPSK) &&
+					(AuthMode != Ndis802_11AuthModeWPA2PSK) &&
+					(AuthMode != Ndis802_11AuthModeWPANone)
+					)
+					return;
+			}
+
+			DBGPRINT(RT_DEBUG_TRACE,("==> RTMPMakeRSNIE(STA)\n"));
+
+			// Zero RSNIE context
+			pAd->StaCfg.RSNIE_Len = 0;
+			NdisZeroMemory(pAd->StaCfg.RSN_IE, MAX_LEN_OF_RSNIE);
+
+			// Pointer to RSNIE
+			rsnielen_cur_p = &pAd->StaCfg.RSNIE_Len;
+			pRsnIe = pAd->StaCfg.RSN_IE;
+
+			bMixCipher = pAd->StaCfg.bMixCipher;
+		}
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+	// indicate primary RSNIE as WPA or WPA2
+	if ((AuthMode == Ndis802_11AuthModeWPA) ||
+		(AuthMode == Ndis802_11AuthModeWPAPSK) ||
+		(AuthMode == Ndis802_11AuthModeWPANone) ||
+		(AuthMode == Ndis802_11AuthModeWPA1WPA2) ||
+		(AuthMode == Ndis802_11AuthModeWPA1PSKWPA2PSK))
+		PrimaryRsnie = WpaIe;
+	else
+		PrimaryRsnie = Wpa2Ie;
+
+	{
+		// Build the primary RSNIE
+		// 1. insert cipher suite
+		RTMPInsertRsnIeCipher(pAd, PrimaryRsnie, WepStatus, bMixCipher, FlexibleCipher, pRsnIe, &p_offset);
+
+		// 2. insert AKM
+		RTMPInsertRsnIeAKM(pAd, PrimaryRsnie, AuthMode, apidx, pRsnIe, &p_offset);
+
+		// 3. insert capability
+		RTMPInsertRsnIeCap(pAd, PrimaryRsnie, apidx, pRsnIe, &p_offset);
+	}
+
+	// 4. update the RSNIE length
+	*rsnielen_cur_p = p_offset;
+
+	hex_dump("The primary RSNIE", pRsnIe, (*rsnielen_cur_p));
+
+
+}
+
+/*
+    ==========================================================================
+    Description:
+		Check whether the received frame is EAP frame.
+
+	Arguments:
+		pAd				-	pointer to our pAdapter context
+		pEntry			-	pointer to active entry
+		pData			-	the received frame
+		DataByteCount 	-	the received frame's length
+		FromWhichBSSID	-	indicate the interface index
+
+    Return:
+         TRUE 			-	This frame is EAP frame
+         FALSE 			-	otherwise
+    ==========================================================================
+*/
+BOOLEAN RTMPCheckWPAframe(
+    IN PRTMP_ADAPTER    pAd,
+    IN PMAC_TABLE_ENTRY	pEntry,
+    IN PUCHAR           pData,
+    IN ULONG            DataByteCount,
+	IN UCHAR			FromWhichBSSID)
+{
+	ULONG	Body_len;
+	BOOLEAN Cancelled;
+
+
+    if(DataByteCount < (LENGTH_802_1_H + LENGTH_EAPOL_H))
+        return FALSE;
+
+
+	// Skip LLC header
+    if (NdisEqualMemory(SNAP_802_1H, pData, 6) ||
+        // Cisco 1200 AP may send packet with SNAP_BRIDGE_TUNNEL
+        NdisEqualMemory(SNAP_BRIDGE_TUNNEL, pData, 6))
+    {
+        pData += 6;
+    }
+	// Skip 2-bytes EAPoL type
+    if (NdisEqualMemory(EAPOL, pData, 2))
+    {
+        pData += 2;
+    }
+    else
+        return FALSE;
+
+    switch (*(pData+1))
+    {
+        case EAPPacket:
+			Body_len = (*(pData+2)<<8) | (*(pData+3));
+            DBGPRINT(RT_DEBUG_TRACE, ("Receive EAP-Packet frame, TYPE = 0, Length = %ld\n", Body_len));
+            break;
+        case EAPOLStart:
+            DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL-Start frame, TYPE = 1 \n"));
+			if (pEntry->EnqueueEapolStartTimerRunning != EAPOL_START_DISABLE)
+            {
+            	DBGPRINT(RT_DEBUG_TRACE, ("Cancel the EnqueueEapolStartTimerRunning \n"));
+                RTMPCancelTimer(&pEntry->EnqueueStartForPSKTimer, &Cancelled);
+                pEntry->EnqueueEapolStartTimerRunning = EAPOL_START_DISABLE;
+            }
+            break;
+        case EAPOLLogoff:
+            DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOLLogoff frame, TYPE = 2 \n"));
+            break;
+        case EAPOLKey:
+			Body_len = (*(pData+2)<<8) | (*(pData+3));
+            DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL-Key frame, TYPE = 3, Length = %ld\n", Body_len));
+            break;
+        case EAPOLASFAlert:
+            DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOLASFAlert frame, TYPE = 4 \n"));
+            break;
+        default:
+            return FALSE;
+
+    }
+    return TRUE;
+}
+
+
+/*
+    ==========================================================================
+    Description:
+        ENCRYPT AES GTK before sending in EAPOL frame.
+        AES GTK length = 128 bit,  so fix blocks for aes-key-wrap as 2 in this function.
+        This function references to RFC 3394 for aes key wrap algorithm.
+    Return:
+    ==========================================================================
+*/
+VOID AES_GTK_KEY_WRAP(
+    IN UCHAR    *key,
+    IN UCHAR    *plaintext,
+    IN UCHAR    p_len,
+    OUT UCHAR   *ciphertext)
+{
+    UCHAR       A[8], BIN[16], BOUT[16];
+    UCHAR       R[512];
+    INT         num_blocks = p_len/8;   // unit:64bits
+    INT         i, j;
+    aes_context aesctx;
+    UCHAR       xor;
+
+    rtmp_aes_set_key(&aesctx, key, 128);
+
+    // Init IA
+    for (i = 0; i < 8; i++)
+        A[i] = 0xa6;
+
+    //Input plaintext
+    for (i = 0; i < num_blocks; i++)
+    {
+        for (j = 0 ; j < 8; j++)
+            R[8 * (i + 1) + j] = plaintext[8 * i + j];
+    }
+
+    // Key Mix
+    for (j = 0; j < 6; j++)
+    {
+        for(i = 1; i <= num_blocks; i++)
+        {
+            //phase 1
+            NdisMoveMemory(BIN, A, 8);
+            NdisMoveMemory(&BIN[8], &R[8 * i], 8);
+            rtmp_aes_encrypt(&aesctx, BIN, BOUT);
+
+            NdisMoveMemory(A, &BOUT[0], 8);
+            xor = num_blocks * j + i;
+            A[7] = BOUT[7] ^ xor;
+            NdisMoveMemory(&R[8 * i], &BOUT[8], 8);
+        }
+    }
+
+    // Output ciphertext
+    NdisMoveMemory(ciphertext, A, 8);
+
+    for (i = 1; i <= num_blocks; i++)
+    {
+        for (j = 0 ; j < 8; j++)
+            ciphertext[8 * i + j] = R[8 * i + j];
+    }
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Misc function to decrypt AES body
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+		This function references to	RFC	3394 for aes key unwrap algorithm.
+
+	========================================================================
+*/
+VOID	AES_GTK_KEY_UNWRAP(
+	IN	UCHAR	*key,
+	OUT	UCHAR	*plaintext,
+	IN	UCHAR    c_len,
+	IN	UCHAR	*ciphertext)
+
+{
+	UCHAR       A[8], BIN[16], BOUT[16];
+	UCHAR       xor;
+	INT         i, j;
+	aes_context aesctx;
+	UCHAR       *R;
+	INT         num_blocks = c_len/8;	// unit:64bits
+
+
+	os_alloc_mem(NULL, (PUCHAR *)&R, 512);
+
+	if (R == NULL)
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("!!!AES_GTK_KEY_UNWRAP: no memory!!!\n"));
+        return;
+    } /* End of if */
+
+	// Initialize
+	NdisMoveMemory(A, ciphertext, 8);
+	//Input plaintext
+	for(i = 0; i < (c_len-8); i++)
+	{
+		R[ i] = ciphertext[i + 8];
+	}
+
+	rtmp_aes_set_key(&aesctx, key, 128);
+
+	for(j = 5; j >= 0; j--)
+	{
+		for(i = (num_blocks-1); i > 0; i--)
+		{
+			xor = (num_blocks -1 )* j + i;
+			NdisMoveMemory(BIN, A, 8);
+			BIN[7] = A[7] ^ xor;
+			NdisMoveMemory(&BIN[8], &R[(i-1)*8], 8);
+			rtmp_aes_decrypt(&aesctx, BIN, BOUT);
+			NdisMoveMemory(A, &BOUT[0], 8);
+			NdisMoveMemory(&R[(i-1)*8], &BOUT[8], 8);
+		}
+	}
+
+	// OUTPUT
+	for(i = 0; i < c_len; i++)
+	{
+		plaintext[i] = R[i];
+	}
+
+
+	os_free_mem(NULL, R);
+}
+
+/*
+    ==========================================================================
+    Description:
+		Report the EAP message type
+
+	Arguments:
+		msg		-	EAPOL_PAIR_MSG_1
+					EAPOL_PAIR_MSG_2
+					EAPOL_PAIR_MSG_3
+					EAPOL_PAIR_MSG_4
+					EAPOL_GROUP_MSG_1
+					EAPOL_GROUP_MSG_2
+
+    Return:
+         message type string
+
+    ==========================================================================
+*/
+CHAR *GetEapolMsgType(CHAR msg)
+{
+    if(msg == EAPOL_PAIR_MSG_1)
+        return "Pairwise Message 1";
+    else if(msg == EAPOL_PAIR_MSG_2)
+        return "Pairwise Message 2";
+	else if(msg == EAPOL_PAIR_MSG_3)
+        return "Pairwise Message 3";
+	else if(msg == EAPOL_PAIR_MSG_4)
+        return "Pairwise Message 4";
+	else if(msg == EAPOL_GROUP_MSG_1)
+        return "Group Message 1";
+	else if(msg == EAPOL_GROUP_MSG_2)
+        return "Group Message 2";
+    else
+    	return "Invalid Message";
+}
+
+
+/*
+    ========================================================================
+
+    Routine Description:
+    Check Sanity RSN IE of EAPoL message
+
+    Arguments:
+
+    Return Value:
+
+
+    ========================================================================
+*/
+BOOLEAN RTMPCheckRSNIE(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pData,
+	IN  UCHAR           DataLen,
+	IN  MAC_TABLE_ENTRY *pEntry,
+	OUT	UCHAR			*Offset)
+{
+	PUCHAR              pVIE;
+	UCHAR               len;
+	PEID_STRUCT         pEid;
+	BOOLEAN				result = FALSE;
+
+	pVIE = pData;
+	len	 = DataLen;
+	*Offset = 0;
+
+	while (len > sizeof(RSNIE2))
+	{
+		pEid = (PEID_STRUCT) pVIE;
+		// WPA RSN IE
+		if ((pEid->Eid == IE_WPA) && (NdisEqualMemory(pEid->Octet, WPA_OUI, 4)))
+		{
+			if ((pEntry->AuthMode == Ndis802_11AuthModeWPA || pEntry->AuthMode == Ndis802_11AuthModeWPAPSK) &&
+				(NdisEqualMemory(pVIE, pEntry->RSN_IE, pEntry->RSNIE_Len)) &&
+				(pEntry->RSNIE_Len == (pEid->Len + 2)))
+			{
+					result = TRUE;
+			}
+
+			*Offset += (pEid->Len + 2);
+		}
+		// WPA2 RSN IE
+		else if ((pEid->Eid == IE_RSN) && (NdisEqualMemory(pEid->Octet + 2, RSN_OUI, 3)))
+		{
+			if ((pEntry->AuthMode == Ndis802_11AuthModeWPA2 || pEntry->AuthMode == Ndis802_11AuthModeWPA2PSK) &&
+				(NdisEqualMemory(pVIE, pEntry->RSN_IE, pEntry->RSNIE_Len)) &&
+				(pEntry->RSNIE_Len == (pEid->Len + 2))/* ToDo-AlbertY for mesh*/)
+			{
+					result = TRUE;
+			}
+
+			*Offset += (pEid->Len + 2);
+		}
+		else
+		{
+			break;
+		}
+
+		pVIE += (pEid->Len + 2);
+		len  -= (pEid->Len + 2);
+	}
+
+
+	return result;
+
+}
+
+
+/*
+    ========================================================================
+
+    Routine Description:
+    Parse KEYDATA field.  KEYDATA[] May contain 2 RSN IE and optionally GTK.
+    GTK  is encaptulated in KDE format at  p.83 802.11i D10
+
+    Arguments:
+
+    Return Value:
+
+    Note:
+        802.11i D10
+
+    ========================================================================
+*/
+BOOLEAN RTMPParseEapolKeyData(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pKeyData,
+	IN  UCHAR           KeyDataLen,
+	IN	UCHAR			GroupKeyIndex,
+	IN	UCHAR			MsgType,
+	IN	BOOLEAN			bWPA2,
+	IN  MAC_TABLE_ENTRY *pEntry)
+{
+    PKDE_ENCAP          pKDE = NULL;
+    PUCHAR              pMyKeyData = pKeyData;
+    UCHAR               KeyDataLength = KeyDataLen;
+    UCHAR               GTKLEN = 0;
+	UCHAR				DefaultIdx = 0;
+	UCHAR				skip_offset;
+
+	// Verify The RSN IE contained in pairewise_msg_2 && pairewise_msg_3 and skip it
+	if (MsgType == EAPOL_PAIR_MSG_2 || MsgType == EAPOL_PAIR_MSG_3)
+    {
+		// Check RSN IE whether it is WPA2/WPA2PSK
+		if (!RTMPCheckRSNIE(pAd, pKeyData, KeyDataLen, pEntry, &skip_offset))
+		{
+			// send wireless event - for RSN IE different
+			if (pAd->CommonCfg.bWirelessEvent)
+				RTMPSendWirelessEvent(pAd, IW_RSNIE_DIFF_EVENT_FLAG, pEntry->Addr, pEntry->apidx, 0);
+
+        	DBGPRINT(RT_DEBUG_ERROR, ("RSN_IE Different in msg %d of 4-way handshake!\n", MsgType));
+			hex_dump("Receive RSN_IE ", pKeyData, KeyDataLen);
+			hex_dump("Desired RSN_IE ", pEntry->RSN_IE, pEntry->RSNIE_Len);
+
+			return FALSE;
+    	}
+    	else
+		{
+			if (bWPA2 && MsgType == EAPOL_PAIR_MSG_3)
+			{
+				// skip RSN IE
+				pMyKeyData += skip_offset;
+				KeyDataLength -= skip_offset;
+				DBGPRINT(RT_DEBUG_TRACE, ("RTMPParseEapolKeyData ==> WPA2/WPA2PSK RSN IE matched in Msg 3, Length(%d) \n", skip_offset));
+			}
+			else
+				return TRUE;
+		}
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("RTMPParseEapolKeyData ==> KeyDataLength %d without RSN_IE \n", KeyDataLength));
+
+	// Parse EKD format in pairwise_msg_3_WPA2 && group_msg_1_WPA2
+	if (bWPA2 && (MsgType == EAPOL_PAIR_MSG_3 || MsgType == EAPOL_GROUP_MSG_1))
+	{
+		if (KeyDataLength >= 8)	// KDE format exclude GTK length
+    	{
+        	pKDE = (PKDE_ENCAP) pMyKeyData;
+
+
+			DefaultIdx = pKDE->GTKEncap.Kid;
+
+			// Sanity check - KED length
+			if (KeyDataLength < (pKDE->Len + 2))
+    		{
+        		DBGPRINT(RT_DEBUG_ERROR, ("ERROR: The len from KDE is too short \n"));
+        		return FALSE;
+    		}
+
+			// Get GTK length - refer to IEEE 802.11i-2004 p.82
+			GTKLEN = pKDE->Len -6;
+			if (GTKLEN < LEN_AES_KEY)
+			{
+				DBGPRINT(RT_DEBUG_ERROR, ("ERROR: GTK Key length is too short (%d) \n", GTKLEN));
+        		return FALSE;
+			}
+
+    	}
+		else
+    	{
+			DBGPRINT(RT_DEBUG_ERROR, ("ERROR: KDE format length is too short \n"));
+	        return FALSE;
+    	}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("GTK in KDE format ,DefaultKeyID=%d, KeyLen=%d \n", DefaultIdx, GTKLEN));
+		// skip it
+		pMyKeyData += 8;
+		KeyDataLength -= 8;
+
+	}
+	else if (!bWPA2 && MsgType == EAPOL_GROUP_MSG_1)
+	{
+		DefaultIdx = GroupKeyIndex;
+		DBGPRINT(RT_DEBUG_TRACE, ("GTK DefaultKeyID=%d \n", DefaultIdx));
+	}
+
+	// Sanity check - shared key index must be 1 ~ 3
+	if (DefaultIdx < 1 || DefaultIdx > 3)
+    {
+     	DBGPRINT(RT_DEBUG_ERROR, ("ERROR: GTK Key index(%d) is invalid in %s %s \n", DefaultIdx, ((bWPA2) ? "WPA2" : "WPA"), GetEapolMsgType(MsgType)));
+        return FALSE;
+    }
+
+
+#ifdef CONFIG_STA_SUPPORT
+	// Todo
+#endif // CONFIG_STA_SUPPORT //
+
+	return TRUE;
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Construct EAPoL message for WPA handshaking
+		Its format is below,
+
+		+--------------------+
+		| Protocol Version	 |  1 octet
+		+--------------------+
+		| Protocol Type		 |	1 octet
+		+--------------------+
+		| Body Length		 |  2 octets
+		+--------------------+
+		| Descriptor Type	 |	1 octet
+		+--------------------+
+		| Key Information    |	2 octets
+		+--------------------+
+		| Key Length	     |  1 octet
+		+--------------------+
+		| Key Repaly Counter |	8 octets
+		+--------------------+
+		| Key Nonce		     |  32 octets
+		+--------------------+
+		| Key IV			 |  16 octets
+		+--------------------+
+		| Key RSC			 |  8 octets
+		+--------------------+
+		| Key ID or Reserved |	8 octets
+		+--------------------+
+		| Key MIC			 |	16 octets
+		+--------------------+
+		| Key Data Length	 |	2 octets
+		+--------------------+
+		| Key Data			 |	n octets
+		+--------------------+
+
+
+	Arguments:
+		pAd			Pointer	to our adapter
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	ConstructEapolMsg(
+	IN 	PRTMP_ADAPTER    	pAd,
+    IN 	UCHAR				AuthMode,
+    IN 	UCHAR				WepStatus,
+    IN 	UCHAR				GroupKeyWepStatus,
+    IN 	UCHAR				MsgType,
+    IN	UCHAR				DefaultKeyIdx,
+    IN 	UCHAR				*ReplayCounter,
+	IN 	UCHAR				*KeyNonce,
+	IN	UCHAR				*TxRSC,
+	IN	UCHAR				*PTK,
+	IN	UCHAR				*GTK,
+	IN	UCHAR				*RSNIE,
+	IN	UCHAR				RSNIE_Len,
+    OUT PEAPOL_PACKET       pMsg)
+{
+	BOOLEAN	bWPA2 = FALSE;
+
+	// Choose WPA2 or not
+	if ((AuthMode == Ndis802_11AuthModeWPA2) || (AuthMode == Ndis802_11AuthModeWPA2PSK))
+		bWPA2 = TRUE;
+
+    // Init Packet and Fill header
+    pMsg->ProVer = EAPOL_VER;
+    pMsg->ProType = EAPOLKey;
+
+	// Default 95 bytes, the EAPoL-Key descriptor exclude Key-data field
+	pMsg->Body_Len[1] = LEN_EAPOL_KEY_MSG;
+
+	// Fill in EAPoL descriptor
+	if (bWPA2)
+		pMsg->KeyDesc.Type = WPA2_KEY_DESC;
+	else
+		pMsg->KeyDesc.Type = WPA1_KEY_DESC;
+
+	// Fill in Key information, refer to IEEE Std 802.11i-2004 page 78
+	// When either the pairwise or the group cipher is AES, the DESC_TYPE_AES(2) shall be used.
+	pMsg->KeyDesc.KeyInfo.KeyDescVer =
+        	(((WepStatus == Ndis802_11Encryption3Enabled) || (GroupKeyWepStatus == Ndis802_11Encryption3Enabled)) ? (DESC_TYPE_AES) : (DESC_TYPE_TKIP));
+
+	// Specify Key Type as Group(0) or Pairwise(1)
+	if (MsgType >= EAPOL_GROUP_MSG_1)
+		pMsg->KeyDesc.KeyInfo.KeyType = GROUPKEY;
+	else
+		pMsg->KeyDesc.KeyInfo.KeyType = PAIRWISEKEY;
+
+	// Specify Key Index, only group_msg1_WPA1
+	if (!bWPA2 && (MsgType >= EAPOL_GROUP_MSG_1))
+		pMsg->KeyDesc.KeyInfo.KeyIndex = DefaultKeyIdx;
+
+	if (MsgType == EAPOL_PAIR_MSG_3)
+		pMsg->KeyDesc.KeyInfo.Install = 1;
+
+	if ((MsgType == EAPOL_PAIR_MSG_1) || (MsgType == EAPOL_PAIR_MSG_3) || (MsgType == EAPOL_GROUP_MSG_1))
+		pMsg->KeyDesc.KeyInfo.KeyAck = 1;
+
+	if (MsgType != EAPOL_PAIR_MSG_1)
+		pMsg->KeyDesc.KeyInfo.KeyMic = 1;
+
+	if ((bWPA2 && (MsgType >= EAPOL_PAIR_MSG_3)) || (!bWPA2 && (MsgType >= EAPOL_GROUP_MSG_1)))
+    {
+       	pMsg->KeyDesc.KeyInfo.Secure = 1;
+    }
+
+	if (bWPA2 && ((MsgType == EAPOL_PAIR_MSG_3) || (MsgType == EAPOL_GROUP_MSG_1)))
+    {
+        pMsg->KeyDesc.KeyInfo.EKD_DL = 1;
+    }
+
+	// key Information element has done.
+	*(USHORT *)(&pMsg->KeyDesc.KeyInfo) = cpu2le16(*(USHORT *)(&pMsg->KeyDesc.KeyInfo));
+
+	// Fill in Key Length
+#if 0
+	if (bWPA2)
+	{
+		// In WPA2 mode, the field indicates the length of pairwise key cipher,
+		// so only pairwise_msg_1 and pairwise_msg_3 need to fill.
+		if ((MsgType == EAPOL_PAIR_MSG_1) || (MsgType == EAPOL_PAIR_MSG_3))
+			pMsg->KeyDesc.KeyLength[1] = ((WepStatus == Ndis802_11Encryption2Enabled) ? LEN_TKIP_KEY : LEN_AES_KEY);
+	}
+	else if (!bWPA2)
+#endif
+	{
+		if (MsgType >= EAPOL_GROUP_MSG_1)
+		{
+			// the length of group key cipher
+			pMsg->KeyDesc.KeyLength[1] = ((GroupKeyWepStatus == Ndis802_11Encryption2Enabled) ? TKIP_GTK_LENGTH : LEN_AES_KEY);
+		}
+		else
+		{
+			// the length of pairwise key cipher
+			pMsg->KeyDesc.KeyLength[1] = ((WepStatus == Ndis802_11Encryption2Enabled) ? LEN_TKIP_KEY : LEN_AES_KEY);
+		}
+	}
+
+ 	// Fill in replay counter
+    NdisMoveMemory(pMsg->KeyDesc.ReplayCounter, ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// Fill Key Nonce field
+	// ANonce : pairwise_msg1 & pairwise_msg3
+	// SNonce : pairwise_msg2
+	// GNonce : group_msg1_wpa1
+	if ((MsgType <= EAPOL_PAIR_MSG_3) || ((!bWPA2 && (MsgType == EAPOL_GROUP_MSG_1))))
+    	NdisMoveMemory(pMsg->KeyDesc.KeyNonce, KeyNonce, LEN_KEY_DESC_NONCE);
+
+	// Fill key IV - WPA2 as 0, WPA1 as random
+	if (!bWPA2 && (MsgType == EAPOL_GROUP_MSG_1))
+	{
+		// Suggest IV be random number plus some number,
+		NdisMoveMemory(pMsg->KeyDesc.KeyIv, &KeyNonce[16], LEN_KEY_DESC_IV);
+        pMsg->KeyDesc.KeyIv[15] += 2;
+	}
+
+    // Fill Key RSC field
+    // It contains the RSC for the GTK being installed.
+	if ((MsgType == EAPOL_PAIR_MSG_3 && bWPA2) || (MsgType == EAPOL_GROUP_MSG_1))
+	{
+        NdisMoveMemory(pMsg->KeyDesc.KeyRsc, TxRSC, 6);
+	}
+
+	// Clear Key MIC field for MIC calculation later
+    NdisZeroMemory(pMsg->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+
+	ConstructEapolKeyData(pAd,
+						  AuthMode,
+						  WepStatus,
+						  GroupKeyWepStatus,
+						  MsgType,
+						  DefaultKeyIdx,
+						  bWPA2,
+						  PTK,
+						  GTK,
+						  RSNIE,
+						  RSNIE_Len,
+						  pMsg);
+
+	// Calculate MIC and fill in KeyMic Field except Pairwise Msg 1.
+	if (MsgType != EAPOL_PAIR_MSG_1)
+	{
+		CalculateMIC(pAd, WepStatus, PTK, pMsg);
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("===> ConstructEapolMsg for %s %s\n", ((bWPA2) ? "WPA2" : "WPA"), GetEapolMsgType(MsgType)));
+	DBGPRINT(RT_DEBUG_TRACE, ("	     Body length = %d \n", pMsg->Body_Len[1]));
+	DBGPRINT(RT_DEBUG_TRACE, ("	     Key length  = %d \n", pMsg->KeyDesc.KeyLength[1]));
+
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Construct the Key Data field of EAPoL message
+
+	Arguments:
+		pAd			Pointer	to our adapter
+		Elem		Message body
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	ConstructEapolKeyData(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			AuthMode,
+	IN	UCHAR			WepStatus,
+	IN	UCHAR			GroupKeyWepStatus,
+	IN 	UCHAR			MsgType,
+	IN	UCHAR			DefaultKeyIdx,
+	IN	BOOLEAN			bWPA2Capable,
+	IN	UCHAR			*PTK,
+	IN	UCHAR			*GTK,
+	IN	UCHAR			*RSNIE,
+	IN	UCHAR			RSNIE_LEN,
+	OUT PEAPOL_PACKET   pMsg)
+{
+	UCHAR		*mpool, *Key_Data, *Rc4GTK;
+	UCHAR       ekey[(LEN_KEY_DESC_IV+LEN_EAP_EK)];
+	UCHAR		data_offset;
+
+
+	if (MsgType == EAPOL_PAIR_MSG_1 || MsgType == EAPOL_PAIR_MSG_4 || MsgType == EAPOL_GROUP_MSG_2)
+		return;
+
+	// allocate memory pool
+	os_alloc_mem(pAd, (PUCHAR *)&mpool, 1500);
+
+    if (mpool == NULL)
+		return;
+
+	/* Rc4GTK Len = 512 */
+	Rc4GTK = (UCHAR *) ROUND_UP(mpool, 4);
+	/* Key_Data Len = 512 */
+	Key_Data = (UCHAR *) ROUND_UP(Rc4GTK + 512, 4);
+
+	NdisZeroMemory(Key_Data, 512);
+	pMsg->KeyDesc.KeyDataLen[1] = 0;
+	data_offset = 0;
+
+	// Encapsulate RSNIE in pairwise_msg2 & pairwise_msg3
+	if (RSNIE_LEN && ((MsgType == EAPOL_PAIR_MSG_2) || (MsgType == EAPOL_PAIR_MSG_3)))
+	{
+		if (bWPA2Capable)
+			Key_Data[data_offset + 0] = IE_WPA2;
+		else
+			Key_Data[data_offset + 0] = IE_WPA;
+
+        Key_Data[data_offset + 1] = RSNIE_LEN;
+		NdisMoveMemory(&Key_Data[data_offset + 2], RSNIE, RSNIE_LEN);
+		data_offset += (2 + RSNIE_LEN);
+	}
+
+	// Encapsulate KDE format in pairwise_msg3_WPA2 & group_msg1_WPA2
+	if (bWPA2Capable && ((MsgType == EAPOL_PAIR_MSG_3) || (MsgType == EAPOL_GROUP_MSG_1)))
+	{
+		// Key Data Encapsulation (KDE) format - 802.11i-2004  Figure-43w and Table-20h
+        Key_Data[data_offset + 0] = 0xDD;
+
+		if (GroupKeyWepStatus == Ndis802_11Encryption3Enabled)
+		{
+			Key_Data[data_offset + 1] = 0x16;// 4+2+16(OUI+DataType+DataField)
+		}
+		else
+		{
+			Key_Data[data_offset + 1] = 0x26;// 4+2+32(OUI+DataType+DataField)
+		}
+
+        Key_Data[data_offset + 2] = 0x00;
+        Key_Data[data_offset + 3] = 0x0F;
+        Key_Data[data_offset + 4] = 0xAC;
+        Key_Data[data_offset + 5] = 0x01;
+
+		// GTK KDE format - 802.11i-2004  Figure-43x
+        Key_Data[data_offset + 6] = (DefaultKeyIdx & 0x03);
+        Key_Data[data_offset + 7] = 0x00;	// Reserved Byte
+
+		data_offset += 8;
+	}
+
+
+	// Encapsulate GTK and encrypt the key-data field with KEK.
+	// Only for pairwise_msg3_WPA2 and group_msg1
+	if ((MsgType == EAPOL_PAIR_MSG_3 && bWPA2Capable) || (MsgType == EAPOL_GROUP_MSG_1))
+	{
+		// Fill in GTK
+		if (GroupKeyWepStatus == Ndis802_11Encryption3Enabled)
+		{
+			NdisMoveMemory(&Key_Data[data_offset], GTK, LEN_AES_KEY);
+			data_offset += LEN_AES_KEY;
+		}
+		else
+		{
+			NdisMoveMemory(&Key_Data[data_offset], GTK, TKIP_GTK_LENGTH);
+			data_offset += TKIP_GTK_LENGTH;
+		}
+
+		// Still dont know why, but if not append will occur "GTK not include in MSG3"
+		// Patch for compatibility between zero config and funk
+		if (MsgType == EAPOL_PAIR_MSG_3 && bWPA2Capable)
+		{
+			if (GroupKeyWepStatus == Ndis802_11Encryption3Enabled)
+			{
+				Key_Data[data_offset + 0] = 0xDD;
+				Key_Data[data_offset + 1] = 0;
+				data_offset += 2;
+			}
+			else
+			{
+				Key_Data[data_offset + 0] = 0xDD;
+				Key_Data[data_offset + 1] = 0;
+				Key_Data[data_offset + 2] = 0;
+				Key_Data[data_offset + 3] = 0;
+				Key_Data[data_offset + 4] = 0;
+				Key_Data[data_offset + 5] = 0;
+				data_offset += 6;
+			}
+		}
+
+		// Encrypt the data material in key data field
+		if (WepStatus == Ndis802_11Encryption3Enabled)
+		{
+			AES_GTK_KEY_WRAP(&PTK[16], Key_Data, data_offset, Rc4GTK);
+            // AES wrap function will grow 8 bytes in length
+            data_offset += 8;
+		}
+		else
+		{
+			// PREPARE Encrypted  "Key DATA" field.  (Encrypt GTK with RC4, usinf PTK[16]->[31] as Key, IV-field as IV)
+			// put TxTsc in Key RSC field
+			pAd->PrivateInfo.FCSCRC32 = PPPINITFCS32;   //Init crc32.
+
+			// ekey is the contanetion of IV-field, and PTK[16]->PTK[31]
+			NdisMoveMemory(ekey, pMsg->KeyDesc.KeyIv, LEN_KEY_DESC_IV);
+			NdisMoveMemory(&ekey[LEN_KEY_DESC_IV], &PTK[16], LEN_EAP_EK);
+			ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, ekey, sizeof(ekey));  //INIT SBOX, KEYLEN+3(IV)
+			pAd->PrivateInfo.FCSCRC32 = RTMP_CALC_FCS32(pAd->PrivateInfo.FCSCRC32, Key_Data, data_offset);
+			WPAARCFOUR_ENCRYPT(&pAd->PrivateInfo.WEPCONTEXT, Rc4GTK, Key_Data, data_offset);
+		}
+
+		NdisMoveMemory(pMsg->KeyDesc.KeyData, Rc4GTK, data_offset);
+	}
+	else
+	{
+		NdisMoveMemory(pMsg->KeyDesc.KeyData, Key_Data, data_offset);
+	}
+
+	// set key data length field and total length
+	pMsg->KeyDesc.KeyDataLen[1] = data_offset;
+    pMsg->Body_Len[1] += data_offset;
+
+	os_free_mem(pAd, mpool);
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Calcaulate MIC. It is used during 4-ways handsharking.
+
+	Arguments:
+		pAd				-	pointer to our pAdapter context
+    	PeerWepStatus	-	indicate the encryption type
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID	CalculateMIC(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			PeerWepStatus,
+	IN	UCHAR			*PTK,
+	OUT PEAPOL_PACKET   pMsg)
+{
+    UCHAR   *OutBuffer;
+	ULONG	FrameLen = 0;
+	UCHAR	mic[LEN_KEY_DESC_MIC];
+	UCHAR	digest[80];
+
+	// allocate memory for MIC calculation
+	os_alloc_mem(pAd, (PUCHAR *)&OutBuffer, 512);
+
+    if (OutBuffer == NULL)
+    {
+		DBGPRINT(RT_DEBUG_ERROR, ("!!!CalculateMIC: no memory!!!\n"));
+		return;
+    }
+
+	// make a frame for calculating MIC.
+    MakeOutgoingFrame(OutBuffer,            	&FrameLen,
+                      pMsg->Body_Len[1] + 4,  	pMsg,
+                      END_OF_ARGS);
+
+	NdisZeroMemory(mic, sizeof(mic));
+
+	// Calculate MIC
+    if (PeerWepStatus == Ndis802_11Encryption3Enabled)
+ 	{
+		HMAC_SHA1(OutBuffer,  FrameLen, PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{
+		hmac_md5(PTK,  LEN_EAP_MICK, OutBuffer, FrameLen, mic);
+	}
+
+	// store the calculated MIC
+	NdisMoveMemory(pMsg->KeyDesc.KeyMic, mic, LEN_KEY_DESC_MIC);
+
+	os_free_mem(pAd, OutBuffer);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Some received frames can't decrypt by Asic, so decrypt them by software.
+
+	Arguments:
+		pAd				-	pointer to our pAdapter context
+    	PeerWepStatus	-	indicate the encryption type
+
+	Return Value:
+		NDIS_STATUS_SUCCESS		-	decryption successful
+		NDIS_STATUS_FAILURE		-	decryption failure
+
+	========================================================================
+*/
+NDIS_STATUS	RTMPSoftDecryptBroadCastData(
+	IN	PRTMP_ADAPTER					pAd,
+	IN	RX_BLK							*pRxBlk,
+	IN  NDIS_802_11_ENCRYPTION_STATUS 	GroupCipher,
+	IN  PCIPHER_KEY						pShard_key)
+{
+	PRXWI_STRUC			pRxWI = pRxBlk->pRxWI;
+
+
+
+	// handle WEP decryption
+	if (GroupCipher == Ndis802_11Encryption1Enabled)
+    {
+		if (RTMPSoftDecryptWEP(pAd, pRxBlk->pData, pRxWI->MPDUtotalByteCount, pShard_key))
+		{
+
+			//Minus IV[4] & ICV[4]
+			pRxWI->MPDUtotalByteCount -= 8;
+		}
+		else
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("ERROR : Software decrypt WEP data fails.\n"));
+			// give up this frame
+			return NDIS_STATUS_FAILURE;
+		}
+	}
+	// handle TKIP decryption
+	else if (GroupCipher == Ndis802_11Encryption2Enabled)
+	{
+		if (RTMPSoftDecryptTKIP(pAd, pRxBlk->pData, pRxWI->MPDUtotalByteCount, 0, pShard_key))
+		{
+
+			//Minus 8 bytes MIC, 8 bytes IV/EIV, 4 bytes ICV
+			pRxWI->MPDUtotalByteCount -= 20;
+		}
+        else
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("ERROR : RTMPSoftDecryptTKIP Failed\n"));
+			// give up this frame
+			return NDIS_STATUS_FAILURE;
+        }
+	}
+	// handle AES decryption
+	else if (GroupCipher == Ndis802_11Encryption3Enabled)
+	{
+		if (RTMPSoftDecryptAES(pAd, pRxBlk->pData, pRxWI->MPDUtotalByteCount , pShard_key))
+		{
+
+			//8 bytes MIC, 8 bytes IV/EIV (CCMP Header)
+			pRxWI->MPDUtotalByteCount -= 16;
+		}
+		else
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("ERROR : RTMPSoftDecryptAES Failed\n"));
+			// give up this frame
+			return NDIS_STATUS_FAILURE;
+		}
+	}
+	else
+	{
+		// give up this frame
+		return NDIS_STATUS_FAILURE;
+	}
+
+	return NDIS_STATUS_SUCCESS;
+
+}
+
diff --git a/drivers/staging/rt2870/common/dfs.c b/drivers/staging/rt2870/common/dfs.c
new file mode 100644
index 00000000000..23cf1510e2d
--- /dev/null
+++ b/drivers/staging/rt2870/common/dfs.c
@@ -0,0 +1,453 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    ap_dfs.c
+
+    Abstract:
+    Support DFS function.
+
+    Revision History:
+    Who       When            What
+    --------  ----------      ----------------------------------------------
+    Fonchi    03-12-2007      created
+*/
+
+#include "../rt_config.h"
+
+typedef struct _RADAR_DURATION_TABLE
+{
+	ULONG RDDurRegion;
+	ULONG RadarSignalDuration;
+	ULONG Tolerance;
+} RADAR_DURATION_TABLE, *PRADAR_DURATION_TABLE;
+
+
+static UCHAR RdIdleTimeTable[MAX_RD_REGION][4] =
+{
+	{9, 250, 250, 250},		// CE
+	{4, 250, 250, 250},		// FCC
+	{4, 250, 250, 250},		// JAP
+	{15, 250, 250, 250},	// JAP_W53
+	{4, 250, 250, 250}		// JAP_W56
+};
+
+/*
+	========================================================================
+
+	Routine Description:
+		Bbp Radar detection routine
+
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	Return Value:
+
+	========================================================================
+*/
+VOID BbpRadarDetectionStart(
+	IN PRTMP_ADAPTER pAd)
+{
+	UINT8 RadarPeriod;
+
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, 114, 0x02);
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, 121, 0x20);
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, 122, 0x00);
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, 123, 0x08/*0x80*/);
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, 124, 0x28);
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, 125, 0xff);
+
+#if 0
+	// toggle Rx enable bit for radar detection.
+	// it's Andy's recommand.
+	{
+		UINT32 Value;
+	RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+	Value |= (0x1 << 3);
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+	Value &= ~(0x1 << 3);
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+	}
+#endif
+	RadarPeriod = ((UINT)RdIdleTimeTable[pAd->CommonCfg.RadarDetect.RDDurRegion][0] + (UINT)pAd->CommonCfg.RadarDetect.DfsSessionTime) < 250 ?
+			(RdIdleTimeTable[pAd->CommonCfg.RadarDetect.RDDurRegion][0] + pAd->CommonCfg.RadarDetect.DfsSessionTime) : 250;
+
+	RTMP_IO_WRITE8(pAd, 0x7020, 0x1d);
+	RTMP_IO_WRITE8(pAd, 0x7021, 0x40);
+
+	RadarDetectionStart(pAd, 0, RadarPeriod);
+	return;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Bbp Radar detection routine
+
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	Return Value:
+
+	========================================================================
+*/
+VOID BbpRadarDetectionStop(
+	IN PRTMP_ADAPTER pAd)
+{
+	RTMP_IO_WRITE8(pAd, 0x7020, 0x1d);
+	RTMP_IO_WRITE8(pAd, 0x7021, 0x60);
+
+	RadarDetectionStop(pAd);
+	return;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Radar detection routine
+
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	Return Value:
+
+	========================================================================
+*/
+VOID RadarDetectionStart(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN CTSProtect,
+	IN UINT8 CTSPeriod)
+{
+	UINT8 DfsActiveTime = (pAd->CommonCfg.RadarDetect.DfsSessionTime & 0x1f);
+	UINT8 CtsProtect = (CTSProtect == 1) ? 0x02 : 0x01; // CTS protect.
+
+	if (CTSProtect != 0)
+	{
+		switch(pAd->CommonCfg.RadarDetect.RDDurRegion)
+		{
+		case FCC:
+		case JAP_W56:
+			CtsProtect = 0x03;
+			break;
+
+		case CE:
+		case JAP_W53:
+		default:
+			CtsProtect = 0x02;
+			break;
+		}
+	}
+	else
+		CtsProtect = 0x01;
+
+
+	// send start-RD with CTS protection command to MCU
+	// highbyte [7]		reserve
+	// highbyte [6:5]	0x: stop Carrier/Radar detection
+	// highbyte [10]:	Start Carrier/Radar detection without CTS protection, 11: Start Carrier/Radar detection with CTS protection
+	// highbyte [4:0]	Radar/carrier detection duration. In 1ms.
+
+	// lowbyte [7:0]	Radar/carrier detection period, in 1ms.
+	AsicSendCommandToMcu(pAd, 0x60, 0xff, CTSPeriod, DfsActiveTime | (CtsProtect << 5));
+	//AsicSendCommandToMcu(pAd, 0x63, 0xff, 10, 0);
+
+	return;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Radar detection routine
+
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	Return Value:
+		TRUE	Found radar signal
+		FALSE	Not found radar signal
+
+	========================================================================
+*/
+VOID RadarDetectionStop(
+	IN PRTMP_ADAPTER	pAd)
+{
+	DBGPRINT(RT_DEBUG_TRACE,("RadarDetectionStop.\n"));
+	AsicSendCommandToMcu(pAd, 0x60, 0xff, 0x00, 0x00);	// send start-RD with CTS protection command to MCU
+
+	return;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Radar channel check routine
+
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	Return Value:
+		TRUE	need to do radar detect
+		FALSE	need not to do radar detect
+
+	========================================================================
+*/
+BOOLEAN RadarChannelCheck(
+	IN PRTMP_ADAPTER	pAd,
+	IN UCHAR			Ch)
+{
+#if 1
+	INT		i;
+	BOOLEAN result = FALSE;
+
+	for (i=0; i<pAd->ChannelListNum; i++)
+	{
+		if (Ch == pAd->ChannelList[i].Channel)
+		{
+			result = pAd->ChannelList[i].DfsReq;
+			break;
+		}
+	}
+
+	return result;
+#else
+	INT		i;
+	UCHAR	Channel[15]={52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140};
+
+	for (i=0; i<15; i++)
+	{
+		if (Ch == Channel[i])
+		{
+			break;
+		}
+	}
+
+	if (i != 15)
+		return TRUE;
+	else
+		return FALSE;
+#endif
+}
+
+ULONG JapRadarType(
+	IN PRTMP_ADAPTER pAd)
+{
+	ULONG		i;
+	const UCHAR	Channel[15]={52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140};
+
+	if (pAd->CommonCfg.RadarDetect.RDDurRegion != JAP)
+	{
+		return pAd->CommonCfg.RadarDetect.RDDurRegion;
+	}
+
+	for (i=0; i<15; i++)
+	{
+		if (pAd->CommonCfg.Channel == Channel[i])
+		{
+			break;
+		}
+	}
+
+	if (i < 4)
+		return JAP_W53;
+	else if (i < 15)
+		return JAP_W56;
+	else
+		return JAP; // W52
+
+}
+
+ULONG RTMPBbpReadRadarDuration(
+	IN PRTMP_ADAPTER	pAd)
+{
+	UINT8 byteValue = 0;
+	ULONG result;
+
+	BBP_IO_READ8_BY_REG_ID(pAd, BBP_R115, &byteValue);
+
+	result = 0;
+	switch (byteValue)
+	{
+	case 1: // radar signal detected by pulse mode.
+	case 2: // radar signal detected by width mode.
+		result = RTMPReadRadarDuration(pAd);
+		break;
+
+	case 0: // No radar signal.
+	default:
+
+		result = 0;
+		break;
+	}
+
+	return result;
+}
+
+ULONG RTMPReadRadarDuration(
+	IN PRTMP_ADAPTER	pAd)
+{
+	ULONG result = 0;
+
+#ifdef DFS_SUPPORT
+	UINT8 duration1 = 0, duration2 = 0, duration3 = 0;
+
+	BBP_IO_READ8_BY_REG_ID(pAd, BBP_R116, &duration1);
+	BBP_IO_READ8_BY_REG_ID(pAd, BBP_R117, &duration2);
+	BBP_IO_READ8_BY_REG_ID(pAd, BBP_R118, &duration3);
+	result = (duration1 << 16) + (duration2 << 8) + duration3;
+#endif // DFS_SUPPORT //
+
+	return result;
+
+}
+
+VOID RTMPCleanRadarDuration(
+	IN PRTMP_ADAPTER	pAd)
+{
+	return;
+}
+
+/*
+    ========================================================================
+    Routine Description:
+        Radar wave detection. The API should be invoke each second.
+
+    Arguments:
+        pAd         - Adapter pointer
+
+    Return Value:
+        None
+
+    ========================================================================
+*/
+VOID ApRadarDetectPeriodic(
+	IN PRTMP_ADAPTER pAd)
+{
+	INT	i;
+
+	pAd->CommonCfg.RadarDetect.InServiceMonitorCount++;
+
+	for (i=0; i<pAd->ChannelListNum; i++)
+	{
+		if (pAd->ChannelList[i].RemainingTimeForUse > 0)
+		{
+			pAd->ChannelList[i].RemainingTimeForUse --;
+			if ((pAd->Mlme.PeriodicRound%5) == 0)
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("RadarDetectPeriodic - ch=%d, RemainingTimeForUse=%d\n", pAd->ChannelList[i].Channel, pAd->ChannelList[i].RemainingTimeForUse));
+			}
+		}
+	}
+
+	//radar detect
+	if ((pAd->CommonCfg.Channel > 14)
+		&& (pAd->CommonCfg.bIEEE80211H == 1)
+		&& RadarChannelCheck(pAd, pAd->CommonCfg.Channel))
+	{
+		RadarDetectPeriodic(pAd);
+	}
+
+	return;
+}
+
+// Periodic Radar detection, switch channel will occur in RTMPHandleTBTTInterrupt()
+// Before switch channel, driver needs doing channel switch announcement.
+VOID RadarDetectPeriodic(
+	IN PRTMP_ADAPTER	pAd)
+{
+	// need to check channel availability, after switch channel
+	if (pAd->CommonCfg.RadarDetect.RDMode != RD_SILENCE_MODE)
+			return;
+
+	// channel availability check time is 60sec, use 65 for assurance
+	if (pAd->CommonCfg.RadarDetect.RDCount++ > pAd->CommonCfg.RadarDetect.ChMovingTime)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("Not found radar signal, start send beacon and radar detection in service monitor\n\n"));
+			BbpRadarDetectionStop(pAd);
+		AsicEnableBssSync(pAd);
+		pAd->CommonCfg.RadarDetect.RDMode = RD_NORMAL_MODE;
+
+
+		return;
+	}
+
+	return;
+}
+
+
+/*
+    ==========================================================================
+    Description:
+		change channel moving time for DFS testing.
+
+	Arguments:
+	    pAdapter                    Pointer to our adapter
+	    wrq                         Pointer to the ioctl argument
+
+    Return Value:
+        None
+
+    Note:
+        Usage:
+               1.) iwpriv ra0 set ChMovTime=[value]
+    ==========================================================================
+*/
+INT Set_ChMovingTime_Proc(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR arg)
+{
+	UINT8 Value;
+
+	Value = simple_strtol(arg, 0, 10);
+
+	pAd->CommonCfg.RadarDetect.ChMovingTime = Value;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s:: %d\n", __FUNCTION__,
+		pAd->CommonCfg.RadarDetect.ChMovingTime));
+
+	return TRUE;
+}
+
+INT Set_LongPulseRadarTh_Proc(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR arg)
+{
+	UINT8 Value;
+
+	Value = simple_strtol(arg, 0, 10) > 10 ? 10 : simple_strtol(arg, 0, 10);
+
+	pAd->CommonCfg.RadarDetect.LongPulseRadarTh = Value;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s:: %d\n", __FUNCTION__,
+		pAd->CommonCfg.RadarDetect.LongPulseRadarTh));
+
+	return TRUE;
+}
+
+
diff --git a/drivers/staging/rt2870/common/eeprom.c b/drivers/staging/rt2870/common/eeprom.c
new file mode 100644
index 00000000000..33f16ed9c49
--- /dev/null
+++ b/drivers/staging/rt2870/common/eeprom.c
@@ -0,0 +1,254 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	eeprom.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+*/
+#include "../rt_config.h"
+
+#if 0
+#define EEPROM_SIZE								0x200
+#define NVRAM_OFFSET			0x30000
+#define RF_OFFSET				0x40000
+
+static UCHAR init_flag = 0;
+static PUCHAR nv_ee_start = 0;
+
+static UCHAR EeBuffer[EEPROM_SIZE];
+#endif
+// IRQL = PASSIVE_LEVEL
+VOID RaiseClock(
+    IN	PRTMP_ADAPTER	pAd,
+    IN  UINT32 *x)
+{
+    *x = *x | EESK;
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, *x);
+    RTMPusecDelay(1);				// Max frequency = 1MHz in Spec. definition
+}
+
+// IRQL = PASSIVE_LEVEL
+VOID LowerClock(
+    IN	PRTMP_ADAPTER	pAd,
+    IN  UINT32 *x)
+{
+    *x = *x & ~EESK;
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, *x);
+    RTMPusecDelay(1);
+}
+
+// IRQL = PASSIVE_LEVEL
+USHORT ShiftInBits(
+    IN	PRTMP_ADAPTER	pAd)
+{
+    UINT32		x,i;
+	USHORT      data=0;
+
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+
+    x &= ~( EEDO | EEDI);
+
+    for(i=0; i<16; i++)
+    {
+        data = data << 1;
+        RaiseClock(pAd, &x);
+
+        RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+
+        x &= ~(EEDI);
+        if(x & EEDO)
+            data |= 1;
+
+        LowerClock(pAd, &x);
+    }
+
+    return data;
+}
+
+// IRQL = PASSIVE_LEVEL
+VOID ShiftOutBits(
+    IN	PRTMP_ADAPTER	pAd,
+    IN  USHORT data,
+    IN  USHORT count)
+{
+    UINT32       x,mask;
+
+    mask = 0x01 << (count - 1);
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+
+    x &= ~(EEDO | EEDI);
+
+    do
+    {
+        x &= ~EEDI;
+        if(data & mask)		x |= EEDI;
+
+        RTMP_IO_WRITE32(pAd, E2PROM_CSR, x);
+
+        RaiseClock(pAd, &x);
+        LowerClock(pAd, &x);
+
+        mask = mask >> 1;
+    } while(mask);
+
+    x &= ~EEDI;
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, x);
+}
+
+// IRQL = PASSIVE_LEVEL
+VOID EEpromCleanup(
+    IN	PRTMP_ADAPTER	pAd)
+{
+    UINT32 x;
+
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+
+    x &= ~(EECS | EEDI);
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, x);
+
+    RaiseClock(pAd, &x);
+    LowerClock(pAd, &x);
+}
+
+VOID EWEN(
+	IN	PRTMP_ADAPTER	pAd)
+{
+    UINT32	x;
+
+    // reset bits and set EECS
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+    x &= ~(EEDI | EEDO | EESK);
+    x |= EECS;
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, x);
+
+	// kick a pulse
+	RaiseClock(pAd, &x);
+	LowerClock(pAd, &x);
+
+    // output the read_opcode and six pulse in that order
+    ShiftOutBits(pAd, EEPROM_EWEN_OPCODE, 5);
+    ShiftOutBits(pAd, 0, 6);
+
+    EEpromCleanup(pAd);
+}
+
+VOID EWDS(
+	IN	PRTMP_ADAPTER	pAd)
+{
+    UINT32	x;
+
+    // reset bits and set EECS
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+    x &= ~(EEDI | EEDO | EESK);
+    x |= EECS;
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, x);
+
+	// kick a pulse
+	RaiseClock(pAd, &x);
+	LowerClock(pAd, &x);
+
+    // output the read_opcode and six pulse in that order
+    ShiftOutBits(pAd, EEPROM_EWDS_OPCODE, 5);
+    ShiftOutBits(pAd, 0, 6);
+
+    EEpromCleanup(pAd);
+}
+
+// IRQL = PASSIVE_LEVEL
+USHORT RTMP_EEPROM_READ16(
+    IN	PRTMP_ADAPTER	pAd,
+    IN  USHORT Offset)
+{
+    UINT32		x;
+    USHORT		data;
+
+    Offset /= 2;
+    // reset bits and set EECS
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+    x &= ~(EEDI | EEDO | EESK);
+    x |= EECS;
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, x);
+
+	// kick a pulse
+	RaiseClock(pAd, &x);
+	LowerClock(pAd, &x);
+
+    // output the read_opcode and register number in that order
+    ShiftOutBits(pAd, EEPROM_READ_OPCODE, 3);
+    ShiftOutBits(pAd, Offset, pAd->EEPROMAddressNum);
+
+    // Now read the data (16 bits) in from the selected EEPROM word
+    data = ShiftInBits(pAd);
+
+    EEpromCleanup(pAd);
+
+    return data;
+}	//ReadEEprom
+
+VOID RTMP_EEPROM_WRITE16(
+    IN	PRTMP_ADAPTER	pAd,
+    IN  USHORT Offset,
+    IN  USHORT Data)
+{
+    UINT32 x;
+
+	Offset /= 2;
+
+	EWEN(pAd);
+
+    // reset bits and set EECS
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+    x &= ~(EEDI | EEDO | EESK);
+    x |= EECS;
+    RTMP_IO_WRITE32(pAd, E2PROM_CSR, x);
+
+	// kick a pulse
+	RaiseClock(pAd, &x);
+	LowerClock(pAd, &x);
+
+    // output the read_opcode ,register number and data in that order
+    ShiftOutBits(pAd, EEPROM_WRITE_OPCODE, 3);
+    ShiftOutBits(pAd, Offset, pAd->EEPROMAddressNum);
+	ShiftOutBits(pAd, Data, 16);		// 16-bit access
+
+    // read DO status
+    RTMP_IO_READ32(pAd, E2PROM_CSR, &x);
+
+	EEpromCleanup(pAd);
+
+	RTMPusecDelay(10000);	//delay for twp(MAX)=10ms
+
+	EWDS(pAd);
+
+    EEpromCleanup(pAd);
+}
+
diff --git a/drivers/staging/rt2870/common/firmware.h b/drivers/staging/rt2870/common/firmware.h
new file mode 100644
index 00000000000..a40669895ef
--- /dev/null
+++ b/drivers/staging/rt2870/common/firmware.h
@@ -0,0 +1,558 @@
+/*
+ Copyright (c) 2007, Ralink Technology Corporation
+ All rights reserved.
+
+ Redistribution.  Redistribution and use in binary form, without
+ modification, are permitted provided that the following conditions are
+ met:
+
+ 	* Redistributions must reproduce the above copyright notice and the
+ 	following disclaimer in the documentation and/or other materials
+ 	provided with the distribution.
+ 	* Neither the name of Ralink Technology Corporation nor the names of its
+ 	suppliers may be used to endorse or promote products derived from this
+ 	software without specific prior written permission.
+ 	* No reverse engineering, decompilation, or disassembly of this software
+ 	is permitted.
+
+ Limited patent license. Ralink Technology Corporation grants a world-wide,
+ royalty-free, non-exclusive license under patents it now or hereafter
+ owns or controls to make, have made, use, import, offer to sell and
+ sell ("Utilize") this software, but solely to the extent that any
+ such patent is necessary to Utilize the software alone, or in
+ combination with an operating system licensed under an approved Open
+ Source license as listed by the Open Source Initiative at
+ http://opensource.org/licenses.  The patent license shall not apply to
+ any other combinations which include this software.  No hardware per
+ se is licensed hereunder.
+
+ DISCLAIMER.  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,
+ BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
+ FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
+ TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
+ USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
+ DAMAGE.
+*/
+/* AUTO GEN PLEASE DO NOT MODIFY IT */
+/* AUTO GEN PLEASE DO NOT MODIFY IT */
+
+
+UCHAR FirmwareImage [] = {
+0xff, 0xff, 0xff, 0x02, 0x10, 0x28, 0x02, 0x10, 0x32, 0x02, 0x10, 0x78, 0x02, 0x12, 0x79, 0x02,
+0x12, 0x7a, 0x02, 0x12, 0x99, 0x02, 0x12, 0x9e, 0x12, 0x12, 0x9a, 0x22, 0x02, 0x16, 0x36, 0x02,
+0x17, 0x0c, 0x02, 0x13, 0x89, 0x02, 0x12, 0x9f, 0x30, 0x05, 0x06, 0x20, 0x0d, 0x03, 0x12, 0x17,
+0xae, 0x22, 0x90, 0x01, 0x8c, 0xe0, 0x30, 0xe3, 0x1b, 0xe5, 0x4c, 0x30, 0xe0, 0x04, 0x7f, 0x40,
+0x80, 0x02, 0x7f, 0x00, 0x90, 0x10, 0x2f, 0xef, 0xf0, 0x90, 0x01, 0x8c, 0x74, 0x08, 0xf0, 0xe4,
+0x90, 0x01, 0xa7, 0xf0, 0x90, 0x01, 0x8c, 0xe0, 0x30, 0xe0, 0x1c, 0x90, 0x01, 0x80, 0xe0, 0xb4,
+0x02, 0x15, 0xa3, 0xe0, 0xb4, 0x01, 0x10, 0x90, 0x01, 0x84, 0xe0, 0xb4, 0x81, 0x09, 0x90, 0x01,
+0x8c, 0x74, 0x01, 0xf0, 0x12, 0x0d, 0xc8, 0x22, 0x90, 0x04, 0x14, 0xe0, 0x20, 0xe7, 0x03, 0x02,
+0x12, 0x6e, 0x90, 0x70, 0x12, 0xe0, 0xf5, 0x56, 0x90, 0x04, 0x04, 0xe0, 0x12, 0x0a, 0x9d, 0x10,
+0xb7, 0x31, 0x10, 0xe2, 0x50, 0x11, 0x08, 0x51, 0x11, 0x13, 0x52, 0x11, 0x13, 0x53, 0x11, 0x13,
+0x54, 0x11, 0x54, 0x55, 0x11, 0x79, 0x70, 0x11, 0xa4, 0x71, 0x11, 0xd2, 0x72, 0x12, 0x25, 0x73,
+0x12, 0x46, 0x80, 0x00, 0x00, 0x12, 0x6e, 0x20, 0x02, 0x03, 0x30, 0x03, 0x1d, 0x7d, 0x02, 0xaf,
+0x56, 0x12, 0x0b, 0x91, 0x90, 0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5,
+0x56, 0xf4, 0x70, 0x03, 0x02, 0x12, 0x6e, 0x02, 0x12, 0x67, 0x85, 0x56, 0x41, 0xd2, 0x02, 0x02,
+0x12, 0x6e, 0x90, 0x70, 0x10, 0xe0, 0x54, 0x7f, 0x64, 0x02, 0x60, 0x03, 0x02, 0x12, 0x6e, 0x90,
+0x70, 0x11, 0xe0, 0x64, 0x08, 0x60, 0x08, 0xe0, 0x64, 0x20, 0x60, 0x03, 0x02, 0x12, 0x6e, 0x75,
+0x4e, 0x03, 0x75, 0x4f, 0x20, 0x02, 0x12, 0x6e, 0x90, 0x70, 0x11, 0xe0, 0x24, 0xff, 0x92, 0x47,
+0x02, 0x12, 0x6e, 0x90, 0x04, 0x04, 0xe0, 0x25, 0xe0, 0x24, 0x5d, 0xf5, 0x57, 0x90, 0x70, 0x10,
+0xe0, 0xff, 0x74, 0x47, 0x25, 0x57, 0xf8, 0xc6, 0xef, 0xc6, 0x90, 0x70, 0x11, 0xe0, 0xff, 0x74,
+0x48, 0x25, 0x57, 0xf8, 0xc6, 0xef, 0xc6, 0xe4, 0xfd, 0xaf, 0x56, 0x12, 0x0b, 0x91, 0x90, 0x04,
+0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56, 0xf4, 0x70, 0x03, 0x02, 0x12,
+0x6e, 0x02, 0x12, 0x67, 0xe5, 0x47, 0x64, 0x07, 0x60, 0x0b, 0xe5, 0x47, 0x64, 0x08, 0x60, 0x05,
+0xe5, 0x47, 0xb4, 0x09, 0x08, 0x90, 0x70, 0x11, 0xe0, 0x54, 0x0f, 0xf5, 0x3a, 0xe4, 0xfd, 0xaf,
+0x56, 0x12, 0x0b, 0x91, 0xd2, 0x04, 0x02, 0x12, 0x6e, 0x90, 0x70, 0x10, 0xe0, 0xfe, 0x90, 0x70,
+0x11, 0xe0, 0xfd, 0xed, 0xf8, 0xe6, 0xf5, 0x57, 0xfd, 0xaf, 0x56, 0x12, 0x0b, 0x91, 0x90, 0x04,
+0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56, 0xf4, 0x70, 0x03, 0x02, 0x12,
+0x6e, 0x02, 0x12, 0x67, 0x90, 0x70, 0x10, 0xe0, 0xfe, 0x90, 0x70, 0x11, 0xe0, 0xfd, 0xed, 0xf5,
+0x82, 0x8e, 0x83, 0xe0, 0xf5, 0x57, 0xfd, 0xaf, 0x56, 0x12, 0x0b, 0x91, 0x90, 0x04, 0x14, 0x74,
+0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56, 0xf4, 0x70, 0x03, 0x02, 0x12, 0x6e, 0x02,
+0x12, 0x67, 0x90, 0x10, 0x02, 0xe0, 0xb4, 0x70, 0x1e, 0xa3, 0xe0, 0xb4, 0x30, 0x19, 0x90, 0x05,
+0x08, 0xe0, 0x44, 0x01, 0xf0, 0xfd, 0x90, 0x05, 0x05, 0xe0, 0x54, 0xfb, 0xf0, 0x44, 0x04, 0xf0,
+0xed, 0x54, 0xfe, 0x90, 0x05, 0x08, 0xf0, 0xe4, 0xf5, 0x4e, 0xf5, 0x4f, 0x75, 0x3a, 0xff, 0xad,
+0x57, 0xaf, 0x56, 0x12, 0x0b, 0x91, 0x90, 0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13,
+0xf0, 0xe5, 0x56, 0xf4, 0x60, 0x07, 0x90, 0x70, 0x25, 0xe0, 0x44, 0x01, 0xf0, 0x90, 0x70, 0x40,
+0xe5, 0x3a, 0xf0, 0x80, 0x49, 0x90, 0x70, 0x10, 0xe0, 0x24, 0xff, 0x92, 0x93, 0xe4, 0xfd, 0xaf,
+0x56, 0x12, 0x0b, 0x91, 0x90, 0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5,
+0x56, 0xf4, 0x60, 0x2a, 0x80, 0x21, 0x90, 0x70, 0x10, 0xe0, 0x24, 0xff, 0x92, 0x4a, 0xd2, 0x05,
+0xad, 0x57, 0xaf, 0x56, 0x12, 0x0b, 0x91, 0x90, 0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70,
+0x13, 0xf0, 0xe5, 0x56, 0xf4, 0x60, 0x07, 0x90, 0x70, 0x25, 0xe0, 0x44, 0x01, 0xf0, 0x90, 0x70,
+0x42, 0xe5, 0x3a, 0xf0, 0xa3, 0x74, 0xab, 0xf0, 0x22, 0x22, 0xe5, 0x53, 0x70, 0x1a, 0x30, 0x60,
+0x09, 0xb2, 0x4d, 0x30, 0x4d, 0x04, 0x05, 0x46, 0xc2, 0x04, 0xe5, 0x4f, 0x45, 0x4e, 0x60, 0x08,
+0xe5, 0x4f, 0x15, 0x4f, 0x70, 0x02, 0x15, 0x4e, 0x22, 0x22, 0xc2, 0x42, 0xd3, 0x22, 0x22, 0xc2,
+0x4b, 0xc2, 0x4c, 0xe5, 0x44, 0x12, 0x0a, 0x9d, 0x12, 0xc1, 0x00, 0x13, 0x54, 0x04, 0x13, 0x50,
+0x08, 0x13, 0x2b, 0x10, 0x12, 0xd5, 0x20, 0x12, 0xf5, 0x60, 0x13, 0x06, 0xa0, 0x00, 0x00, 0x13,
+0x56, 0x85, 0x48, 0x43, 0x85, 0x4a, 0x42, 0x85, 0x4c, 0x5e, 0xe5, 0x47, 0x64, 0x06, 0x60, 0x03,
+0x02, 0x13, 0x56, 0x80, 0x1b, 0xe5, 0x48, 0xc4, 0x54, 0x0f, 0xf5, 0x43, 0xe5, 0x4a, 0xc4, 0x54,
+0x0f, 0xf5, 0x42, 0xe5, 0x4c, 0xc4, 0x54, 0x0f, 0xf5, 0x5e, 0xe5, 0x47, 0x64, 0x06, 0x70, 0x66,
+0x53, 0x43, 0x0f, 0x80, 0x61, 0x85, 0x49, 0x43, 0x85, 0x4b, 0x42, 0x85, 0x4d, 0x5e, 0xe5, 0x47,
+0x64, 0x06, 0x70, 0x52, 0x80, 0x1b, 0xe5, 0x49, 0xc4, 0x54, 0x0f, 0xf5, 0x43, 0xe5, 0x4b, 0xc4,
+0x54, 0x0f, 0xf5, 0x42, 0xe5, 0x4d, 0xc4, 0x54, 0x0f, 0xf5, 0x5e, 0xe5, 0x47, 0x64, 0x06, 0x70,
+0x35, 0xe5, 0x43, 0x54, 0x0f, 0x44, 0x10, 0xf5, 0x43, 0x80, 0x2b, 0xe5, 0x47, 0xb4, 0x04, 0x06,
+0x53, 0x5e, 0xfb, 0x75, 0x42, 0x09, 0xe5, 0x47, 0xb4, 0x05, 0x06, 0x43, 0x5e, 0x04, 0x75, 0x42,
+0x09, 0xe5, 0x47, 0xb4, 0x06, 0x10, 0xe5, 0x43, 0x54, 0x0f, 0x44, 0x30, 0xf5, 0x43, 0x80, 0x06,
+0xd2, 0x4b, 0x80, 0x02, 0xd2, 0x4c, 0xe4, 0xf5, 0x25, 0xe5, 0x42, 0xc4, 0x54, 0xf0, 0xff, 0xe5,
+0x43, 0x54, 0x0f, 0x4f, 0xf5, 0x5f, 0x90, 0x70, 0x44, 0xf0, 0xa3, 0xe5, 0x5e, 0xf0, 0xa3, 0xe5,
+0x4a, 0xf0, 0xa3, 0xe5, 0x48, 0xf0, 0xa3, 0xe5, 0x4c, 0xf0, 0xa3, 0xe5, 0x44, 0xf0, 0xa3, 0xe5,
+0x42, 0xf0, 0xa3, 0xe5, 0x43, 0xf0, 0xd2, 0x60, 0x22, 0xe5, 0x47, 0x60, 0x10, 0x24, 0xc0, 0x70,
+0x03, 0x12, 0x16, 0x16, 0x12, 0x13, 0x9e, 0xc2, 0xaf, 0xc2, 0x04, 0xd2, 0xaf, 0x22, 0xc2, 0xaf,
+0x90, 0x04, 0x14, 0xe0, 0x54, 0x0e, 0x60, 0x04, 0xd2, 0x18, 0x80, 0x08, 0xe5, 0x4e, 0x45, 0x4f,
+0x24, 0xff, 0x92, 0x18, 0xd2, 0xaf, 0x90, 0x04, 0x14, 0xe0, 0xa2, 0xe4, 0x92, 0x19, 0x74, 0x1e,
+0xf0, 0xe5, 0x5f, 0x54, 0x0f, 0xf5, 0x2d, 0xe5, 0x25, 0x70, 0x13, 0x30, 0x18, 0x05, 0xe5, 0x5f,
+0x20, 0xe5, 0x0b, 0x30, 0x19, 0x19, 0xe5, 0x5f, 0x54, 0x30, 0xff, 0xbf, 0x30, 0x11, 0xe5, 0x25,
+0x70, 0x05, 0x75, 0x25, 0x0c, 0x80, 0x02, 0x15, 0x25, 0xd2, 0x6c, 0xd2, 0x6d, 0x80, 0x0f, 0xe5,
+0x5f, 0x30, 0xe6, 0x06, 0xc2, 0x6c, 0xd2, 0x6d, 0x80, 0x04, 0xd2, 0x6c, 0xc2, 0x6d, 0xe5, 0x47,
+0x64, 0x03, 0x70, 0x21, 0x30, 0x4b, 0x06, 0xc2, 0x6c, 0xd2, 0x6d, 0x80, 0x18, 0xe5, 0x25, 0x70,
+0x03, 0x30, 0x4c, 0x11, 0xc2, 0x4c, 0xe5, 0x25, 0x70, 0x05, 0x75, 0x25, 0x07, 0x80, 0x02, 0x15,
+0x25, 0xd2, 0x6c, 0xd2, 0x6d, 0xe5, 0x47, 0xb4, 0x09, 0x14, 0xe5, 0x44, 0x20, 0xe3, 0x0b, 0xe5,
+0x3a, 0x64, 0x02, 0x60, 0x05, 0xe5, 0x3a, 0xb4, 0x03, 0x04, 0xc2, 0x6c, 0xd2, 0x6d, 0x90, 0x70,
+0x46, 0xe5, 0x2d, 0xf0, 0x20, 0x69, 0x07, 0xe5, 0x5e, 0x20, 0xe0, 0x02, 0xb2, 0x68, 0x20, 0x6b,
+0x07, 0xe5, 0x5e, 0x20, 0xe1, 0x02, 0xb2, 0x6a, 0x20, 0x6d, 0x07, 0xe5, 0x5e, 0x20, 0xe2, 0x02,
+0xb2, 0x6c, 0x90, 0x70, 0x47, 0xe5, 0x2d, 0xf0, 0x75, 0x2e, 0x40, 0x20, 0x69, 0x04, 0xa2, 0x68,
+0x80, 0x26, 0x30, 0x68, 0x06, 0xe5, 0x46, 0xa2, 0xe2, 0x80, 0x1d, 0xe5, 0x5e, 0x20, 0xe2, 0x04,
+0x7f, 0x01, 0x80, 0x02, 0x7f, 0x00, 0xe5, 0x46, 0x54, 0xf0, 0xfe, 0xbe, 0xf0, 0x04, 0x7e, 0x01,
+0x80, 0x02, 0x7e, 0x00, 0xee, 0x6f, 0x24, 0xff, 0x92, 0x73, 0x92, 0x72, 0x20, 0x6b, 0x04, 0xa2,
+0x6a, 0x80, 0x26, 0x30, 0x6a, 0x06, 0xe5, 0x46, 0xa2, 0xe2, 0x80, 0x1d, 0xe5, 0x5e, 0x20, 0xe0,
+0x04, 0x7f, 0x01, 0x80, 0x02, 0x7f, 0x00, 0xe5, 0x46, 0x54, 0xf0, 0xfe, 0xbe, 0xf0, 0x04, 0x7e,
+0x01, 0x80, 0x02, 0x7e, 0x00, 0xee, 0x6f, 0x24, 0xff, 0x92, 0x75, 0x92, 0x74, 0x20, 0x6d, 0x04,
+0xa2, 0x6c, 0x80, 0x26, 0x30, 0x6c, 0x06, 0xe5, 0x46, 0xa2, 0xe2, 0x80, 0x1d, 0xe5, 0x5e, 0x20,
+0xe1, 0x04, 0x7f, 0x01, 0x80, 0x02, 0x7f, 0x00, 0xe5, 0x46, 0x54, 0xf0, 0xfe, 0xbe, 0xf0, 0x04,
+0x7e, 0x01, 0x80, 0x02, 0x7e, 0x00, 0xee, 0x6f, 0x24, 0xff, 0x92, 0x71, 0x92, 0x70, 0x90, 0x10,
+0x2f, 0xe5, 0x2e, 0xf0, 0xe5, 0x47, 0x64, 0x06, 0x70, 0x4c, 0x90, 0x02, 0x29, 0xe0, 0x54, 0xfe,
+0xf0, 0xe5, 0x43, 0xc4, 0x54, 0x0f, 0x14, 0x60, 0x14, 0x24, 0xfe, 0x60, 0x23, 0x24, 0x03, 0x60,
+0x03, 0x02, 0x16, 0x05, 0x90, 0x02, 0x28, 0xe0, 0x30, 0x47, 0x0f, 0x80, 0x07, 0x90, 0x02, 0x28,
+0xe0, 0x20, 0x47, 0x06, 0x54, 0xfe, 0xf0, 0x02, 0x16, 0x05, 0x44, 0x01, 0xf0, 0x02, 0x16, 0x05,
+0xe5, 0x46, 0x30, 0xe2, 0x04, 0x7f, 0x01, 0x80, 0x02, 0x7f, 0x00, 0x90, 0x02, 0x28, 0xe0, 0x54,
+0xfe, 0x4f, 0xf0, 0x02, 0x16, 0x05, 0xe5, 0x47, 0x64, 0x07, 0x60, 0x0f, 0xe5, 0x47, 0x64, 0x08,
+0x60, 0x09, 0xe5, 0x47, 0x64, 0x09, 0x60, 0x03, 0x02, 0x16, 0x05, 0xe4, 0xf5, 0x27, 0x90, 0x02,
+0x29, 0xe0, 0x54, 0xfc, 0xf0, 0xe5, 0x3a, 0x14, 0x60, 0x2d, 0x14, 0x60, 0x2e, 0x14, 0x60, 0x36,
+0x24, 0xfc, 0x60, 0x5f, 0x24, 0xf9, 0x60, 0x1f, 0x24, 0x0e, 0x70, 0x69, 0xe5, 0x46, 0x13, 0x13,
+0x54, 0x3f, 0x75, 0xf0, 0x01, 0x84, 0xaf, 0xf0, 0x20, 0x47, 0x04, 0x7e, 0x01, 0x80, 0x02, 0x7e,
+0x00, 0xef, 0x6e, 0x24, 0xff, 0x80, 0x45, 0xa2, 0x47, 0x80, 0x41, 0xe5, 0x46, 0x30, 0xe2, 0x03,
+0xd3, 0x80, 0x27, 0xc3, 0x80, 0x24, 0xe5, 0x46, 0x30, 0xe2, 0x0d, 0x54, 0x38, 0xc3, 0x94, 0x30,
+0x50, 0x06, 0x7e, 0x00, 0x7f, 0x01, 0x80, 0x04, 0x7e, 0x00, 0x7f, 0x00, 0x20, 0x47, 0x04, 0x7d,
+0x01, 0x80, 0x02, 0x7d, 0x00, 0xef, 0x6d, 0x4e, 0x24, 0xff, 0x92, 0x38, 0xa2, 0x47, 0xb3, 0x92,
+0x39, 0x80, 0x19, 0xe5, 0x46, 0x30, 0xe2, 0x03, 0xd3, 0x80, 0x01, 0xc3, 0x92, 0x39, 0xa2, 0x47,
+0xb3, 0x92, 0x38, 0x80, 0x07, 0xa2, 0x47, 0xb3, 0x92, 0x38, 0x92, 0x39, 0x90, 0x02, 0x28, 0xe0,
+0x54, 0xfc, 0x45, 0x27, 0xf0, 0x90, 0x70, 0x9c, 0xe5, 0x3a, 0xf0, 0xa3, 0xe5, 0x47, 0xf0, 0x90,
+0x70, 0x41, 0xe5, 0x3a, 0xf0, 0x22, 0xe4, 0x90, 0x02, 0x29, 0xf0, 0x30, 0x47, 0x04, 0xaf, 0x45,
+0x80, 0x04, 0xe5, 0x45, 0xf4, 0xff, 0x90, 0x02, 0x28, 0xef, 0xf0, 0x22, 0x8f, 0x50, 0xd2, 0x59,
+0x22, 0x8f, 0x54, 0xd2, 0x58, 0x22, 0xe4, 0xf5, 0x62, 0xc2, 0xaf, 0xe5, 0x51, 0x14, 0x60, 0x46,
+0x14, 0x60, 0x62, 0x24, 0x02, 0x60, 0x03, 0x02, 0x16, 0xf0, 0xd2, 0x59, 0x75, 0x55, 0x01, 0x90,
+0x02, 0xa2, 0xe0, 0x54, 0x7f, 0xf0, 0xa3, 0xe0, 0x20, 0xe7, 0x22, 0x90, 0x04, 0x34, 0xe0, 0xb4,
+0x02, 0x1b, 0xa3, 0xe0, 0xb4, 0x02, 0x16, 0xa3, 0xe0, 0xb4, 0x02, 0x11, 0x7f, 0x20, 0x12, 0x16,
+0x2c, 0x90, 0x10, 0x04, 0xe0, 0x54, 0xf3, 0xf0, 0x75, 0x51, 0x01, 0x80, 0x73, 0xe5, 0x50, 0x70,
+0x05, 0x75, 0x62, 0x03, 0x80, 0x6a, 0x90, 0x12, 0x00, 0xe0, 0x54, 0x03, 0x70, 0x11, 0x7f, 0x20,
+0x12, 0x16, 0x2c, 0x90, 0x02, 0xa2, 0xe0, 0x54, 0xbf, 0xf0, 0x75, 0x51, 0x02, 0x80, 0x51, 0xe5,
+0x50, 0x70, 0x02, 0x80, 0x46, 0x90, 0x02, 0xa3, 0xe0, 0x20, 0xe6, 0x3b, 0x90, 0x04, 0x37, 0xe0,
+0x64, 0x22, 0x70, 0x33, 0x90, 0x01, 0x8a, 0x74, 0x7e, 0xf0, 0x90, 0x01, 0x96, 0xf0, 0x90, 0x12,
+0x04, 0x74, 0x0a, 0xf0, 0x90, 0x13, 0x28, 0xe0, 0x54, 0xf0, 0xf0, 0xa3, 0xe0, 0x54, 0xf0, 0xf0,
+0xa3, 0xe0, 0x54, 0xfa, 0xf0, 0x90, 0x04, 0x01, 0xe0, 0x54, 0xf9, 0xf0, 0x75, 0x62, 0x01, 0x75,
+0x55, 0x02, 0xe4, 0xf5, 0x51, 0x80, 0x09, 0xe5, 0x50, 0x70, 0x05, 0x75, 0x62, 0x03, 0xf5, 0x51,
+0xe5, 0x62, 0x60, 0x15, 0xc2, 0x01, 0xe4, 0xf5, 0x51, 0xc2, 0x59, 0xad, 0x62, 0xaf, 0x40, 0x12,
+0x17, 0x7a, 0xe5, 0x62, 0xb4, 0x03, 0x02, 0xd2, 0x03, 0xd2, 0xaf, 0x22, 0xc2, 0xaf, 0x30, 0x01,
+0x12, 0xe4, 0x90, 0x01, 0x96, 0xf0, 0xf5, 0x51, 0xc2, 0x59, 0xc2, 0x01, 0x7d, 0x02, 0xaf, 0x40,
+0x12, 0x17, 0x7a, 0xe5, 0x52, 0x14, 0x60, 0x09, 0x04, 0x70, 0x4c, 0x75, 0x52, 0x01, 0x75, 0x55,
+0x03, 0x90, 0x04, 0x01, 0xe0, 0x44, 0x0e, 0xf0, 0x90, 0x13, 0x28, 0xe0, 0x44, 0x0f, 0xf0, 0xa3,
+0xe0, 0x44, 0x0f, 0xf0, 0xa3, 0xe0, 0x44, 0x05, 0xf0, 0x90, 0x12, 0x04, 0x74, 0x03, 0xf0, 0x90,
+0x02, 0xa2, 0xe0, 0x44, 0xc0, 0xf0, 0x90, 0x10, 0x04, 0xe0, 0x44, 0x0c, 0xf0, 0xe4, 0xf5, 0x52,
+0xf5, 0x55, 0x30, 0x02, 0x0b, 0xc2, 0x02, 0x7d, 0x01, 0xaf, 0x41, 0x12, 0x17, 0x7a, 0x80, 0x02,
+0xc2, 0x03, 0xe4, 0x90, 0x01, 0x96, 0xf0, 0xd2, 0xaf, 0x22, 0xef, 0xf4, 0x60, 0x2d, 0xe4, 0xfe,
+0x74, 0x14, 0x2e, 0xf5, 0x82, 0xe4, 0x34, 0x70, 0xf5, 0x83, 0xe0, 0xb4, 0xff, 0x19, 0x74, 0x14,
+0x2e, 0xf5, 0x82, 0xe4, 0x34, 0x70, 0xf5, 0x83, 0xef, 0xf0, 0x74, 0x1c, 0x2e, 0xf5, 0x82, 0xe4,
+0x34, 0x70, 0xf5, 0x83, 0xed, 0xf0, 0x22, 0x0e, 0xbe, 0x04, 0xd5, 0x22, 0x22, 0x22, 0x90, 0x70,
+0x2a, 0xe0, 0x30, 0xe1, 0x4d, 0xc2, 0xaf, 0x90, 0x70, 0x28, 0xe0, 0x90, 0x10, 0x1c, 0xf0, 0x90,
+0x70, 0x29, 0xe0, 0x90, 0x10, 0x1d, 0xf0, 0x90, 0x70, 0x2a, 0xe0, 0x90, 0x10, 0x1e, 0xf0, 0x90,
+0x10, 0x1c, 0xe0, 0xf5, 0x62, 0x90, 0x10, 0x1e, 0xe0, 0x20, 0xe1, 0xf3, 0x90, 0x10, 0x1c, 0xe0,
+0x90, 0x70, 0x28, 0xf0, 0x90, 0x10, 0x1d, 0xe0, 0x90, 0x70, 0x29, 0xf0, 0x90, 0x10, 0x1e, 0xe0,
+0x90, 0x70, 0x2a, 0xf0, 0x30, 0x4a, 0x07, 0x90, 0x70, 0x24, 0xe0, 0x44, 0x01, 0xf0, 0xc2, 0x05,
+0xd2, 0xaf, 0x22, 0x22, 0x22, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0a, 0x69, 0x77,
+0xff, 0xff, 0xff, 0x02, 0x10, 0x28, 0x02, 0x10, 0x32, 0x02, 0x10, 0x78, 0x02, 0x12, 0x25, 0x02,
+0x12, 0x26, 0x02, 0x12, 0x39, 0x02, 0x12, 0x3e, 0x12, 0x12, 0x3a, 0x22, 0x02, 0x15, 0x72, 0x02,
+0x16, 0x48, 0x02, 0x13, 0x29, 0x02, 0x12, 0x3f, 0x30, 0x05, 0x06, 0x20, 0x0d, 0x03, 0x12, 0x16,
+0xea, 0x22, 0x90, 0x01, 0x8c, 0xe0, 0x30, 0xe3, 0x1b, 0xe5, 0x4c, 0x30, 0xe0, 0x04, 0x7f, 0x40,
+0x80, 0x02, 0x7f, 0x00, 0x90, 0x10, 0x2f, 0xef, 0xf0, 0x90, 0x01, 0x8c, 0x74, 0x08, 0xf0, 0xe4,
+0x90, 0x01, 0xa7, 0xf0, 0x90, 0x01, 0x8c, 0xe0, 0x30, 0xe0, 0x1c, 0x90, 0x01, 0x80, 0xe0, 0xb4,
+0x02, 0x15, 0xa3, 0xe0, 0xb4, 0x01, 0x10, 0x90, 0x01, 0x84, 0xe0, 0xb4, 0x81, 0x09, 0x90, 0x01,
+0x8c, 0x74, 0x01, 0xf0, 0x12, 0x0d, 0xdd, 0x22, 0x90, 0x04, 0x14, 0xe0, 0x20, 0xe7, 0x03, 0x02,
+0x12, 0x1a, 0x90, 0x70, 0x12, 0xe0, 0xf5, 0x56, 0x90, 0x04, 0x04, 0xe0, 0x12, 0x0a, 0xb6, 0x10,
+0xb4, 0x31, 0x10, 0xdf, 0x50, 0x11, 0x05, 0x51, 0x11, 0x10, 0x52, 0x11, 0x10, 0x53, 0x11, 0x10,
+0x54, 0x11, 0x51, 0x55, 0x11, 0x70, 0x70, 0x11, 0x9a, 0x71, 0x11, 0xc4, 0x72, 0x11, 0xf2, 0x80,
+0x00, 0x00, 0x12, 0x1a, 0x20, 0x02, 0x03, 0x30, 0x03, 0x1d, 0x7d, 0x02, 0xaf, 0x56, 0x12, 0x0b,
+0xaa, 0x90, 0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56, 0xf4, 0x70,
+0x03, 0x02, 0x12, 0x1a, 0x02, 0x12, 0x13, 0x85, 0x56, 0x41, 0xd2, 0x02, 0x02, 0x12, 0x1a, 0x90,
+0x70, 0x10, 0xe0, 0x54, 0x7f, 0x64, 0x02, 0x60, 0x03, 0x02, 0x12, 0x1a, 0x90, 0x70, 0x11, 0xe0,
+0x64, 0x08, 0x60, 0x08, 0xe0, 0x64, 0x20, 0x60, 0x03, 0x02, 0x12, 0x1a, 0x75, 0x4e, 0x03, 0x75,
+0x4f, 0x20, 0x02, 0x12, 0x1a, 0x90, 0x70, 0x11, 0xe0, 0x24, 0xff, 0x92, 0x47, 0x02, 0x12, 0x1a,
+0x90, 0x04, 0x04, 0xe0, 0x25, 0xe0, 0x24, 0x5d, 0xf5, 0x57, 0x90, 0x70, 0x10, 0xe0, 0xff, 0x74,
+0x47, 0x25, 0x57, 0xf8, 0xc6, 0xef, 0xc6, 0x90, 0x70, 0x11, 0xe0, 0xff, 0x74, 0x48, 0x25, 0x57,
+0xf8, 0xc6, 0xef, 0xc6, 0xe4, 0xfd, 0xaf, 0x56, 0x12, 0x0b, 0xaa, 0x90, 0x04, 0x14, 0x74, 0x80,
+0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56, 0xf4, 0x70, 0x03, 0x02, 0x12, 0x1a, 0x02, 0x12,
+0x13, 0xe5, 0x47, 0x64, 0x07, 0x60, 0x05, 0xe5, 0x47, 0xb4, 0x08, 0x08, 0x90, 0x70, 0x11, 0xe0,
+0x54, 0x07, 0xf5, 0x3a, 0xe4, 0xfd, 0xaf, 0x56, 0x12, 0x0b, 0xaa, 0xd2, 0x04, 0x02, 0x12, 0x1a,
+0x90, 0x70, 0x10, 0xe0, 0xfe, 0x90, 0x70, 0x11, 0xe0, 0xfd, 0xed, 0xf8, 0xe6, 0xf5, 0x57, 0xfd,
+0xaf, 0x56, 0x12, 0x0b, 0xaa, 0x90, 0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0,
+0xe5, 0x56, 0xf4, 0x70, 0x03, 0x02, 0x12, 0x1a, 0x80, 0x79, 0x90, 0x70, 0x10, 0xe0, 0xfe, 0x90,
+0x70, 0x11, 0xe0, 0xfd, 0xed, 0xf5, 0x82, 0x8e, 0x83, 0xe0, 0xf5, 0x57, 0xfd, 0xaf, 0x56, 0x12,
+0x0b, 0xaa, 0x90, 0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56, 0xf4,
+0x60, 0x58, 0x80, 0x4f, 0xe4, 0xf5, 0x4e, 0xf5, 0x4f, 0x75, 0x3a, 0xff, 0xad, 0x57, 0xaf, 0x56,
+0x12, 0x0b, 0xaa, 0x90, 0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56,
+0xf4, 0x60, 0x07, 0x90, 0x70, 0x25, 0xe0, 0x44, 0x01, 0xf0, 0x90, 0x70, 0x40, 0xe5, 0x3a, 0xf0,
+0x80, 0x28, 0x90, 0x70, 0x10, 0xe0, 0x24, 0xff, 0x92, 0x4a, 0xd2, 0x05, 0xad, 0x57, 0xaf, 0x56,
+0x12, 0x0b, 0xaa, 0x90, 0x04, 0x14, 0x74, 0x80, 0xf0, 0xe4, 0x90, 0x70, 0x13, 0xf0, 0xe5, 0x56,
+0xf4, 0x60, 0x07, 0x90, 0x70, 0x25, 0xe0, 0x44, 0x01, 0xf0, 0x90, 0x70, 0x42, 0xe5, 0x3a, 0xf0,
+0xa3, 0x74, 0xab, 0xf0, 0x22, 0x22, 0xe5, 0x53, 0x70, 0x0e, 0xe5, 0x4f, 0x45, 0x4e, 0x60, 0x08,
+0xe5, 0x4f, 0x15, 0x4f, 0x70, 0x02, 0x15, 0x4e, 0x22, 0x22, 0xc2, 0x42, 0xd3, 0x22, 0x22, 0xc2,
+0x4b, 0xc2, 0x4c, 0xe5, 0x44, 0x12, 0x0a, 0xb6, 0x12, 0x61, 0x00, 0x12, 0xf4, 0x04, 0x12, 0xf0,
+0x08, 0x12, 0xcb, 0x10, 0x12, 0x75, 0x20, 0x12, 0x95, 0x60, 0x12, 0xa6, 0xa0, 0x00, 0x00, 0x12,
+0xf6, 0x85, 0x48, 0x43, 0x85, 0x4a, 0x42, 0x85, 0x4c, 0x5e, 0xe5, 0x47, 0x64, 0x06, 0x60, 0x03,
+0x02, 0x12, 0xf6, 0x80, 0x1b, 0xe5, 0x48, 0xc4, 0x54, 0x0f, 0xf5, 0x43, 0xe5, 0x4a, 0xc4, 0x54,
+0x0f, 0xf5, 0x42, 0xe5, 0x4c, 0xc4, 0x54, 0x0f, 0xf5, 0x5e, 0xe5, 0x47, 0x64, 0x06, 0x70, 0x66,
+0x53, 0x43, 0x0f, 0x80, 0x61, 0x85, 0x49, 0x43, 0x85, 0x4b, 0x42, 0x85, 0x4d, 0x5e, 0xe5, 0x47,
+0x64, 0x06, 0x70, 0x52, 0x80, 0x1b, 0xe5, 0x49, 0xc4, 0x54, 0x0f, 0xf5, 0x43, 0xe5, 0x4b, 0xc4,
+0x54, 0x0f, 0xf5, 0x42, 0xe5, 0x4d, 0xc4, 0x54, 0x0f, 0xf5, 0x5e, 0xe5, 0x47, 0x64, 0x06, 0x70,
+0x35, 0xe5, 0x43, 0x54, 0x0f, 0x44, 0x10, 0xf5, 0x43, 0x80, 0x2b, 0xe5, 0x47, 0xb4, 0x04, 0x06,
+0x53, 0x5e, 0xfb, 0x75, 0x42, 0x09, 0xe5, 0x47, 0xb4, 0x05, 0x06, 0x43, 0x5e, 0x04, 0x75, 0x42,
+0x09, 0xe5, 0x47, 0xb4, 0x06, 0x10, 0xe5, 0x43, 0x54, 0x0f, 0x44, 0x30, 0xf5, 0x43, 0x80, 0x06,
+0xd2, 0x4b, 0x80, 0x02, 0xd2, 0x4c, 0xe4, 0xf5, 0x25, 0xe5, 0x42, 0xc4, 0x54, 0xf0, 0xff, 0xe5,
+0x43, 0x54, 0x0f, 0x4f, 0xf5, 0x5f, 0x90, 0x70, 0x44, 0xf0, 0xa3, 0xe5, 0x5e, 0xf0, 0xa3, 0xe5,
+0x4a, 0xf0, 0xa3, 0xe5, 0x48, 0xf0, 0xa3, 0xe5, 0x4c, 0xf0, 0xa3, 0xe5, 0x44, 0xf0, 0xa3, 0xe5,
+0x42, 0xf0, 0xa3, 0xe5, 0x43, 0xf0, 0xd2, 0x60, 0x22, 0xe5, 0x47, 0x60, 0x10, 0x24, 0xc0, 0x70,
+0x03, 0x12, 0x15, 0x52, 0x12, 0x13, 0x3e, 0xc2, 0xaf, 0xc2, 0x04, 0xd2, 0xaf, 0x22, 0xc2, 0xaf,
+0x90, 0x04, 0x14, 0xe0, 0x54, 0x0e, 0x60, 0x04, 0xd2, 0x18, 0x80, 0x08, 0xe5, 0x4e, 0x45, 0x4f,
+0x24, 0xff, 0x92, 0x18, 0xd2, 0xaf, 0x90, 0x04, 0x14, 0xe0, 0xa2, 0xe4, 0x92, 0x19, 0x74, 0x1e,
+0xf0, 0xe5, 0x5f, 0x54, 0x0f, 0xf5, 0x2d, 0xe5, 0x25, 0x70, 0x13, 0x30, 0x18, 0x05, 0xe5, 0x5f,
+0x20, 0xe5, 0x0b, 0x30, 0x19, 0x19, 0xe5, 0x5f, 0x54, 0x30, 0xff, 0xbf, 0x30, 0x11, 0xe5, 0x25,
+0x70, 0x05, 0x75, 0x25, 0x0c, 0x80, 0x02, 0x15, 0x25, 0xd2, 0x6c, 0xd2, 0x6d, 0x80, 0x0f, 0xe5,
+0x5f, 0x30, 0xe6, 0x06, 0xc2, 0x6c, 0xd2, 0x6d, 0x80, 0x04, 0xd2, 0x6c, 0xc2, 0x6d, 0xe5, 0x47,
+0x64, 0x03, 0x70, 0x21, 0x30, 0x4b, 0x06, 0xc2, 0x6c, 0xd2, 0x6d, 0x80, 0x18, 0xe5, 0x25, 0x70,
+0x03, 0x30, 0x4c, 0x11, 0xc2, 0x4c, 0xe5, 0x25, 0x70, 0x05, 0x75, 0x25, 0x07, 0x80, 0x02, 0x15,
+0x25, 0xd2, 0x6c, 0xd2, 0x6d, 0x90, 0x70, 0x46, 0xe5, 0x2d, 0xf0, 0x20, 0x69, 0x07, 0xe5, 0x5e,
+0x20, 0xe0, 0x02, 0xb2, 0x68, 0x20, 0x6b, 0x07, 0xe5, 0x5e, 0x20, 0xe1, 0x02, 0xb2, 0x6a, 0x20,
+0x6d, 0x07, 0xe5, 0x5e, 0x20, 0xe2, 0x02, 0xb2, 0x6c, 0x90, 0x70, 0x47, 0xe5, 0x2d, 0xf0, 0x75,
+0x2e, 0x40, 0x20, 0x69, 0x04, 0xa2, 0x68, 0x80, 0x15, 0x30, 0x68, 0x06, 0xe5, 0x46, 0xa2, 0xe3,
+0x80, 0x0c, 0xe5, 0x46, 0x54, 0xf0, 0xff, 0xbf, 0xf0, 0x03, 0xd3, 0x80, 0x01, 0xc3, 0x92, 0x73,
+0x92, 0x72, 0x20, 0x6b, 0x04, 0xa2, 0x6a, 0x80, 0x15, 0x30, 0x6a, 0x06, 0xe5, 0x46, 0xa2, 0xe3,
+0x80, 0x0c, 0xe5, 0x46, 0x54, 0xf0, 0xff, 0xbf, 0xf0, 0x03, 0xd3, 0x80, 0x01, 0xc3, 0x92, 0x75,
+0x92, 0x74, 0x20, 0x6d, 0x04, 0xa2, 0x6c, 0x80, 0x15, 0x30, 0x6c, 0x06, 0xe5, 0x46, 0xa2, 0xe3,
+0x80, 0x0c, 0xe5, 0x46, 0x54, 0xf0, 0xff, 0xbf, 0xf0, 0x03, 0xd3, 0x80, 0x01, 0xc3, 0x92, 0x71,
+0x92, 0x70, 0x90, 0x10, 0x2f, 0xe5, 0x2e, 0xf0, 0xe5, 0x47, 0x64, 0x06, 0x70, 0x4c, 0x90, 0x02,
+0x29, 0xe0, 0x54, 0xfe, 0xf0, 0xe5, 0x43, 0xc4, 0x54, 0x0f, 0x14, 0x60, 0x14, 0x24, 0xfe, 0x60,
+0x23, 0x24, 0x03, 0x60, 0x03, 0x02, 0x15, 0x41, 0x90, 0x02, 0x28, 0xe0, 0x30, 0x47, 0x0f, 0x80,
+0x07, 0x90, 0x02, 0x28, 0xe0, 0x20, 0x47, 0x06, 0x54, 0xfe, 0xf0, 0x02, 0x15, 0x41, 0x44, 0x01,
+0xf0, 0x02, 0x15, 0x41, 0xe5, 0x46, 0x30, 0xe3, 0x04, 0x7f, 0x01, 0x80, 0x02, 0x7f, 0x00, 0x90,
+0x02, 0x28, 0xe0, 0x54, 0xfe, 0x4f, 0xf0, 0x02, 0x15, 0x41, 0xe5, 0x47, 0x64, 0x07, 0x60, 0x09,
+0xe5, 0x47, 0x64, 0x08, 0x60, 0x03, 0x02, 0x15, 0x41, 0xe4, 0xf5, 0x27, 0x90, 0x02, 0x29, 0xe0,
+0x54, 0xfc, 0xf0, 0xe5, 0x3a, 0x14, 0x60, 0x26, 0x14, 0x60, 0x2e, 0x14, 0x60, 0x36, 0x24, 0x03,
+0x70, 0x5f, 0xe5, 0x46, 0x13, 0x13, 0x13, 0x54, 0x1f, 0x75, 0xf0, 0x03, 0x84, 0xaf, 0xf0, 0x20,
+0x47, 0x04, 0x7e, 0x01, 0x80, 0x02, 0x7e, 0x00, 0xef, 0x6e, 0x24, 0xff, 0x80, 0x02, 0xa2, 0x47,
+0x92, 0x39, 0xa2, 0x47, 0xb3, 0x92, 0x38, 0x80, 0x3f, 0xe5, 0x46, 0x30, 0xe3, 0x03, 0xd3, 0x80,
+0x27, 0xc3, 0x80, 0x24, 0xe5, 0x46, 0x30, 0xe3, 0x0d, 0x54, 0x70, 0xc3, 0x94, 0x60, 0x50, 0x06,
+0x7e, 0x00, 0x7f, 0x01, 0x80, 0x04, 0x7e, 0x00, 0x7f, 0x00, 0x20, 0x47, 0x04, 0x7d, 0x01, 0x80,
+0x02, 0x7d, 0x00, 0xef, 0x6d, 0x4e, 0x24, 0xff, 0x92, 0x38, 0xa2, 0x47, 0xb3, 0x92, 0x39, 0x80,
+0x07, 0xa2, 0x47, 0xb3, 0x92, 0x38, 0x92, 0x39, 0x90, 0x02, 0x28, 0xe0, 0x54, 0xfc, 0x45, 0x27,
+0xf0, 0x90, 0x70, 0x9c, 0xe5, 0x3a, 0xf0, 0xa3, 0xe5, 0x47, 0xf0, 0x90, 0x70, 0x41, 0xe5, 0x3a,
+0xf0, 0x22, 0xe4, 0x90, 0x02, 0x29, 0xf0, 0x30, 0x47, 0x04, 0xaf, 0x45, 0x80, 0x04, 0xe5, 0x45,
+0xf4, 0xff, 0x90, 0x02, 0x28, 0xef, 0xf0, 0x22, 0x8f, 0x50, 0xd2, 0x59, 0x22, 0x8f, 0x54, 0xd2,
+0x58, 0x22, 0xe4, 0xf5, 0x62, 0xc2, 0xaf, 0xe5, 0x51, 0x14, 0x60, 0x46, 0x14, 0x60, 0x62, 0x24,
+0x02, 0x60, 0x03, 0x02, 0x16, 0x2c, 0xd2, 0x59, 0x75, 0x55, 0x01, 0x90, 0x02, 0xa2, 0xe0, 0x54,
+0x7f, 0xf0, 0xa3, 0xe0, 0x20, 0xe7, 0x22, 0x90, 0x04, 0x34, 0xe0, 0xb4, 0x02, 0x1b, 0xa3, 0xe0,
+0xb4, 0x02, 0x16, 0xa3, 0xe0, 0xb4, 0x02, 0x11, 0x7f, 0x20, 0x12, 0x15, 0x68, 0x90, 0x10, 0x04,
+0xe0, 0x54, 0xf3, 0xf0, 0x75, 0x51, 0x01, 0x80, 0x73, 0xe5, 0x50, 0x70, 0x05, 0x75, 0x62, 0x03,
+0x80, 0x6a, 0x90, 0x12, 0x00, 0xe0, 0x54, 0x03, 0x70, 0x11, 0x7f, 0x20, 0x12, 0x15, 0x68, 0x90,
+0x02, 0xa2, 0xe0, 0x54, 0xbf, 0xf0, 0x75, 0x51, 0x02, 0x80, 0x51, 0xe5, 0x50, 0x70, 0x02, 0x80,
+0x46, 0x90, 0x02, 0xa3, 0xe0, 0x20, 0xe6, 0x3b, 0x90, 0x04, 0x37, 0xe0, 0x64, 0x22, 0x70, 0x33,
+0x90, 0x01, 0x8a, 0x74, 0x7e, 0xf0, 0x90, 0x01, 0x96, 0xf0, 0x90, 0x12, 0x04, 0x74, 0x0a, 0xf0,
+0x90, 0x13, 0x28, 0xe0, 0x54, 0xf0, 0xf0, 0xa3, 0xe0, 0x54, 0xf0, 0xf0, 0xa3, 0xe0, 0x54, 0xfa,
+0xf0, 0x90, 0x04, 0x01, 0xe0, 0x54, 0xf9, 0xf0, 0x75, 0x62, 0x01, 0x75, 0x55, 0x02, 0xe4, 0xf5,
+0x51, 0x80, 0x09, 0xe5, 0x50, 0x70, 0x05, 0x75, 0x62, 0x03, 0xf5, 0x51, 0xe5, 0x62, 0x60, 0x15,
+0xc2, 0x01, 0xe4, 0xf5, 0x51, 0xc2, 0x59, 0xad, 0x62, 0xaf, 0x40, 0x12, 0x16, 0xb6, 0xe5, 0x62,
+0xb4, 0x03, 0x02, 0xd2, 0x03, 0xd2, 0xaf, 0x22, 0xc2, 0xaf, 0x30, 0x01, 0x12, 0xe4, 0x90, 0x01,
+0x96, 0xf0, 0xf5, 0x51, 0xc2, 0x59, 0xc2, 0x01, 0x7d, 0x02, 0xaf, 0x40, 0x12, 0x16, 0xb6, 0xe5,
+0x52, 0x14, 0x60, 0x09, 0x04, 0x70, 0x4c, 0x75, 0x52, 0x01, 0x75, 0x55, 0x03, 0x90, 0x04, 0x01,
+0xe0, 0x44, 0x0e, 0xf0, 0x90, 0x13, 0x28, 0xe0, 0x44, 0x0f, 0xf0, 0xa3, 0xe0, 0x44, 0x0f, 0xf0,
+0xa3, 0xe0, 0x44, 0x05, 0xf0, 0x90, 0x12, 0x04, 0x74, 0x03, 0xf0, 0x90, 0x02, 0xa2, 0xe0, 0x44,
+0xc0, 0xf0, 0x90, 0x10, 0x04, 0xe0, 0x44, 0x0c, 0xf0, 0xe4, 0xf5, 0x52, 0xf5, 0x55, 0x30, 0x02,
+0x0b, 0xc2, 0x02, 0x7d, 0x01, 0xaf, 0x41, 0x12, 0x16, 0xb6, 0x80, 0x02, 0xc2, 0x03, 0xe4, 0x90,
+0x01, 0x96, 0xf0, 0xd2, 0xaf, 0x22, 0xef, 0xf4, 0x60, 0x2d, 0xe4, 0xfe, 0x74, 0x14, 0x2e, 0xf5,
+0x82, 0xe4, 0x34, 0x70, 0xf5, 0x83, 0xe0, 0xb4, 0xff, 0x19, 0x74, 0x14, 0x2e, 0xf5, 0x82, 0xe4,
+0x34, 0x70, 0xf5, 0x83, 0xef, 0xf0, 0x74, 0x1c, 0x2e, 0xf5, 0x82, 0xe4, 0x34, 0x70, 0xf5, 0x83,
+0xed, 0xf0, 0x22, 0x0e, 0xbe, 0x04, 0xd5, 0x22, 0x22, 0x22, 0x90, 0x70, 0x2a, 0xe0, 0x30, 0xe1,
+0x4d, 0xc2, 0xaf, 0x90, 0x70, 0x28, 0xe0, 0x90, 0x10, 0x1c, 0xf0, 0x90, 0x70, 0x29, 0xe0, 0x90,
+0x10, 0x1d, 0xf0, 0x90, 0x70, 0x2a, 0xe0, 0x90, 0x10, 0x1e, 0xf0, 0x90, 0x10, 0x1c, 0xe0, 0xf5,
+0x62, 0x90, 0x10, 0x1e, 0xe0, 0x20, 0xe1, 0xf3, 0x90, 0x10, 0x1c, 0xe0, 0x90, 0x70, 0x28, 0xf0,
+0x90, 0x10, 0x1d, 0xe0, 0x90, 0x70, 0x29, 0xf0, 0x90, 0x10, 0x1e, 0xe0, 0x90, 0x70, 0x2a, 0xf0,
+0x30, 0x4a, 0x07, 0x90, 0x70, 0x24, 0xe0, 0x44, 0x01, 0xf0, 0xc2, 0x05, 0xd2, 0xaf, 0x22, 0x22,
+0x22, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xe9, 0x00, } ;
diff --git a/drivers/staging/rt2870/common/md5.c b/drivers/staging/rt2870/common/md5.c
new file mode 100644
index 00000000000..774776b4b8c
--- /dev/null
+++ b/drivers/staging/rt2870/common/md5.c
@@ -0,0 +1,1427 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    md5.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+	jan			10-28-03		Initial
+	Rita    	11-23-04		Modify MD5 and SHA-1
+	Rita		10-14-05		Modify SHA-1 in big-endian platform
+ */
+#include "../rt_config.h"
+
+/**
+ * md5_mac:
+ * @key: pointer to	the	key	used for MAC generation
+ * @key_len: length	of the key in bytes
+ * @data: pointer to the data area for which the MAC is	generated
+ * @data_len: length of	the	data in	bytes
+ * @mac: pointer to	the	buffer holding space for the MAC; the buffer should
+ * have	space for 128-bit (16 bytes) MD5 hash value
+ *
+ * md5_mac() determines	the	message	authentication code	by using secure	hash
+ * MD5(key | data |	key).
+ */
+void md5_mac(u8 *key, size_t key_len, u8 *data, size_t data_len, u8 *mac)
+{
+	MD5_CTX	context;
+
+	MD5Init(&context);
+	MD5Update(&context,	key, key_len);
+	MD5Update(&context,	data, data_len);
+	MD5Update(&context,	key, key_len);
+	MD5Final(mac, &context);
+}
+
+/**
+ * hmac_md5:
+ * @key: pointer to	the	key	used for MAC generation
+ * @key_len: length	of the key in bytes
+ * @data: pointer to the data area for which the MAC is	generated
+ * @data_len: length of	the	data in	bytes
+ * @mac: pointer to	the	buffer holding space for the MAC; the buffer should
+ * have	space for 128-bit (16 bytes) MD5 hash value
+ *
+ * hmac_md5() determines the message authentication	code using HMAC-MD5.
+ * This	implementation is based	on the sample code presented in	RFC	2104.
+ */
+void hmac_md5(u8 *key, size_t key_len, u8 *data, size_t data_len, u8 *mac)
+{
+	MD5_CTX	context;
+    u8 k_ipad[65]; /* inner padding - key XORd with ipad */
+    u8 k_opad[65]; /* outer padding - key XORd with opad */
+    u8 tk[16];
+	int	i;
+
+	//assert(key != NULL && data != NULL && mac != NULL);
+
+	/* if key is longer	than 64	bytes reset	it to key =	MD5(key) */
+	if (key_len	> 64) {
+		MD5_CTX	ttcontext;
+
+		MD5Init(&ttcontext);
+		MD5Update(&ttcontext, key, key_len);
+		MD5Final(tk, &ttcontext);
+		//key=(PUCHAR)ttcontext.buf;
+		key	= tk;
+		key_len	= 16;
+	}
+
+	/* the HMAC_MD5	transform looks	like:
+	 *
+	 * MD5(K XOR opad, MD5(K XOR ipad, text))
+	 *
+	 * where K is an n byte	key
+	 * ipad	is the byte	0x36 repeated 64 times
+	 * opad	is the byte	0x5c repeated 64 times
+	 * and text	is the data	being protected	*/
+
+	/* start out by	storing	key	in pads	*/
+	NdisZeroMemory(k_ipad, sizeof(k_ipad));
+	NdisZeroMemory(k_opad,	sizeof(k_opad));
+	//assert(key_len < sizeof(k_ipad));
+	NdisMoveMemory(k_ipad, key,	key_len);
+	NdisMoveMemory(k_opad, key,	key_len);
+
+	/* XOR key with	ipad and opad values */
+	for	(i = 0;	i <	64;	i++) {
+		k_ipad[i] ^= 0x36;
+		k_opad[i] ^= 0x5c;
+	}
+
+	/* perform inner MD5 */
+	MD5Init(&context);					 /*	init context for 1st pass */
+	MD5Update(&context,	k_ipad,	64);	 /*	start with inner pad */
+	MD5Update(&context,	data, data_len); /*	then text of datagram */
+	MD5Final(mac, &context);			 /*	finish up 1st pass */
+
+	/* perform outer MD5 */
+	MD5Init(&context);					 /*	init context for 2nd pass */
+	MD5Update(&context,	k_opad,	64);	 /*	start with outer pad */
+	MD5Update(&context,	mac, 16);		 /*	then results of	1st	hash */
+	MD5Final(mac, &context);			 /*	finish up 2nd pass */
+}
+
+#ifndef RT_BIG_ENDIAN
+#define byteReverse(buf, len)   /* Nothing */
+#else
+void byteReverse(unsigned char *buf, unsigned longs);
+void byteReverse(unsigned char *buf, unsigned longs)
+{
+    do {
+        *(UINT32 *)buf = SWAP32(*(UINT32 *)buf);
+        buf += 4;
+    } while (--longs);
+}
+#endif
+
+
+/* ==========================  MD5 implementation =========================== */
+// four base functions for MD5
+#define MD5_F1(x, y, z) (((x) & (y)) | ((~x) & (z)))
+#define MD5_F2(x, y, z) (((x) & (z)) | ((y) & (~z)))
+#define MD5_F3(x, y, z) ((x) ^ (y) ^ (z))
+#define MD5_F4(x, y, z) ((y) ^ ((x) | (~z)))
+#define CYCLIC_LEFT_SHIFT(w, s) (((w) << (s)) | ((w) >> (32-(s))))
+
+#define	MD5Step(f, w, x, y,	z, data, t, s)	\
+	( w	+= f(x,	y, z) +	data + t,  w = (CYCLIC_LEFT_SHIFT(w, s)) & 0xffffffff, w +=	x )
+
+
+/*
+ *  Function Description:
+ *      Initiate MD5 Context satisfied in RFC 1321
+ *
+ *  Arguments:
+ *      pCtx        Pointer	to MD5 context
+ *
+ *  Return Value:
+ *      None
+ */
+VOID MD5Init(MD5_CTX *pCtx)
+{
+    pCtx->Buf[0]=0x67452301;
+    pCtx->Buf[1]=0xefcdab89;
+    pCtx->Buf[2]=0x98badcfe;
+    pCtx->Buf[3]=0x10325476;
+
+    pCtx->LenInBitCount[0]=0;
+    pCtx->LenInBitCount[1]=0;
+}
+
+
+/*
+ *  Function Description:
+ *      Update MD5 Context, allow of an arrary of octets as the next portion
+ *      of the message
+ *
+ *  Arguments:
+ *      pCtx		Pointer	to MD5 context
+ * 	    pData       Pointer to input data
+ *      LenInBytes  The length of input data (unit: byte)
+ *
+ *  Return Value:
+ *      None
+ *
+ *  Note:
+ *      Called after MD5Init or MD5Update(itself)
+ */
+VOID MD5Update(MD5_CTX *pCtx, UCHAR *pData, UINT32 LenInBytes)
+{
+
+    UINT32 TfTimes;
+    UINT32 temp;
+	unsigned int i;
+
+    temp = pCtx->LenInBitCount[0];
+
+    pCtx->LenInBitCount[0] = (UINT32) (pCtx->LenInBitCount[0] + (LenInBytes << 3));
+
+    if (pCtx->LenInBitCount[0] < temp)
+        pCtx->LenInBitCount[1]++;   //carry in
+
+    pCtx->LenInBitCount[1] += LenInBytes >> 29;
+
+    // mod 64 bytes
+    temp = (temp >> 3) & 0x3f;
+
+    // process lacks of 64-byte data
+    if (temp)
+    {
+        UCHAR *pAds = (UCHAR *) pCtx->Input + temp;
+
+        if ((temp+LenInBytes) < 64)
+        {
+            NdisMoveMemory(pAds, (UCHAR *)pData, LenInBytes);
+            return;
+        }
+
+        NdisMoveMemory(pAds, (UCHAR *)pData, 64-temp);
+        byteReverse(pCtx->Input, 16);
+        MD5Transform(pCtx->Buf, (UINT32 *)pCtx->Input);
+
+        pData += 64-temp;
+        LenInBytes -= 64-temp;
+    } // end of if (temp)
+
+
+    TfTimes = (LenInBytes >> 6);
+
+    for (i=TfTimes; i>0; i--)
+    {
+        NdisMoveMemory(pCtx->Input, (UCHAR *)pData, 64);
+        byteReverse(pCtx->Input, 16);
+        MD5Transform(pCtx->Buf, (UINT32 *)pCtx->Input);
+        pData += 64;
+        LenInBytes -= 64;
+    } // end of for
+
+    // buffering lacks of 64-byte data
+    if(LenInBytes)
+        NdisMoveMemory(pCtx->Input, (UCHAR *)pData, LenInBytes);
+
+}
+
+
+/*
+ *  Function Description:
+ *      Append padding bits and length of original message in the tail
+ *      The message digest has to be completed in the end
+ *
+ *  Arguments:
+ *      Digest		Output of Digest-Message for MD5
+ *  	pCtx        Pointer	to MD5 context
+ *
+ *  Return Value:
+ *      None
+ *
+ *  Note:
+ *      Called after MD5Update
+ */
+VOID MD5Final(UCHAR Digest[16], MD5_CTX *pCtx)
+{
+    UCHAR Remainder;
+    UCHAR PadLenInBytes;
+    UCHAR *pAppend=0;
+    unsigned int i;
+
+    Remainder = (UCHAR)((pCtx->LenInBitCount[0] >> 3) & 0x3f);
+
+    PadLenInBytes = (Remainder < 56) ? (56-Remainder) : (120-Remainder);
+
+    pAppend = (UCHAR *)pCtx->Input + Remainder;
+
+    // padding bits without crossing block(64-byte based) boundary
+    if (Remainder < 56)
+    {
+        *pAppend = 0x80;
+        PadLenInBytes --;
+
+        NdisZeroMemory((UCHAR *)pCtx->Input + Remainder+1, PadLenInBytes);
+
+		// add data-length field, from low to high
+       	for (i=0; i<4; i++)
+        {
+        	pCtx->Input[56+i] = (UCHAR)((pCtx->LenInBitCount[0] >> (i << 3)) & 0xff);
+        	pCtx->Input[60+i] = (UCHAR)((pCtx->LenInBitCount[1] >> (i << 3)) & 0xff);
+      	}
+
+        byteReverse(pCtx->Input, 16);
+        MD5Transform(pCtx->Buf, (UINT32 *)pCtx->Input);
+    } // end of if
+
+    // padding bits with crossing block(64-byte based) boundary
+    else
+    {
+        // the first block ===
+        *pAppend = 0x80;
+        PadLenInBytes --;
+
+        NdisZeroMemory((UCHAR *)pCtx->Input + Remainder+1, (64-Remainder-1));
+        PadLenInBytes -= (64 - Remainder - 1);
+
+        byteReverse(pCtx->Input, 16);
+        MD5Transform(pCtx->Buf, (UINT32 *)pCtx->Input);
+
+
+        // the second block ===
+        NdisZeroMemory((UCHAR *)pCtx->Input, PadLenInBytes);
+
+        // add data-length field
+        for (i=0; i<4; i++)
+        {
+        	pCtx->Input[56+i] = (UCHAR)((pCtx->LenInBitCount[0] >> (i << 3)) & 0xff);
+        	pCtx->Input[60+i] = (UCHAR)((pCtx->LenInBitCount[1] >> (i << 3)) & 0xff);
+      	}
+
+        byteReverse(pCtx->Input, 16);
+        MD5Transform(pCtx->Buf, (UINT32 *)pCtx->Input);
+    } // end of else
+
+
+    NdisMoveMemory((UCHAR *)Digest, (UINT32 *)pCtx->Buf, 16); // output
+    byteReverse((UCHAR *)Digest, 4);
+    NdisZeroMemory(pCtx, sizeof(pCtx)); // memory free
+}
+
+
+/*
+ *  Function Description:
+ *      The central algorithm of MD5, consists of four rounds and sixteen
+ *  	steps per round
+ *
+ *  Arguments:
+ *      Buf     Buffers of four states (output: 16 bytes)
+ * 	    Mes     Input data (input: 64 bytes)
+ *
+ *  Return Value:
+ *      None
+ *
+ *  Note:
+ *      Called by MD5Update or MD5Final
+ */
+VOID MD5Transform(UINT32 Buf[4], UINT32 Mes[16])
+{
+    UINT32 Reg[4], Temp;
+	unsigned int i;
+
+    static UCHAR LShiftVal[16] =
+    {
+        7, 12, 17, 22,
+		5, 9 , 14, 20,
+		4, 11, 16, 23,
+ 		6, 10, 15, 21,
+ 	};
+
+
+	// [equal to 4294967296*abs(sin(index))]
+    static UINT32 MD5Table[64] =
+	{
+		0xd76aa478,	0xe8c7b756,	0x242070db,	0xc1bdceee,
+		0xf57c0faf,	0x4787c62a,	0xa8304613, 0xfd469501,
+		0x698098d8,	0x8b44f7af,	0xffff5bb1,	0x895cd7be,
+    	0x6b901122,	0xfd987193,	0xa679438e,	0x49b40821,
+
+    	0xf61e2562,	0xc040b340,	0x265e5a51,	0xe9b6c7aa,
+    	0xd62f105d,	0x02441453,	0xd8a1e681,	0xe7d3fbc8,
+    	0x21e1cde6,	0xc33707d6,	0xf4d50d87,	0x455a14ed,
+    	0xa9e3e905,	0xfcefa3f8,	0x676f02d9,	0x8d2a4c8a,
+
+    	0xfffa3942,	0x8771f681,	0x6d9d6122,	0xfde5380c,
+    	0xa4beea44,	0x4bdecfa9,	0xf6bb4b60,	0xbebfbc70,
+    	0x289b7ec6,	0xeaa127fa,	0xd4ef3085,	0x04881d05,
+    	0xd9d4d039,	0xe6db99e5,	0x1fa27cf8,	0xc4ac5665,
+
+    	0xf4292244,	0x432aff97,	0xab9423a7,	0xfc93a039,
+   		0x655b59c3,	0x8f0ccc92,	0xffeff47d,	0x85845dd1,
+    	0x6fa87e4f,	0xfe2ce6e0,	0xa3014314,	0x4e0811a1,
+    	0xf7537e82,	0xbd3af235,	0x2ad7d2bb,	0xeb86d391
+	};
+
+
+    for (i=0; i<4; i++)
+        Reg[i]=Buf[i];
+
+
+    // 64 steps in MD5 algorithm
+    for (i=0; i<16; i++)
+    {
+        MD5Step(MD5_F1, Reg[0], Reg[1], Reg[2], Reg[3], Mes[i],
+                MD5Table[i], LShiftVal[i & 0x3]);
+
+        // one-word right shift
+        Temp   = Reg[3];
+        Reg[3] = Reg[2];
+        Reg[2] = Reg[1];
+        Reg[1] = Reg[0];
+        Reg[0] = Temp;
+    }
+    for (i=16; i<32; i++)
+    {
+        MD5Step(MD5_F2, Reg[0], Reg[1], Reg[2], Reg[3], Mes[(5*(i & 0xf)+1) & 0xf],
+                MD5Table[i], LShiftVal[(0x1 << 2)+(i & 0x3)]);
+
+        // one-word right shift
+        Temp   = Reg[3];
+        Reg[3] = Reg[2];
+        Reg[2] = Reg[1];
+        Reg[1] = Reg[0];
+        Reg[0] = Temp;
+    }
+    for (i=32; i<48; i++)
+    {
+        MD5Step(MD5_F3, Reg[0], Reg[1], Reg[2], Reg[3], Mes[(3*(i & 0xf)+5) & 0xf],
+                MD5Table[i], LShiftVal[(0x1 << 3)+(i & 0x3)]);
+
+        // one-word right shift
+        Temp   = Reg[3];
+        Reg[3] = Reg[2];
+        Reg[2] = Reg[1];
+        Reg[1] = Reg[0];
+        Reg[0] = Temp;
+    }
+    for (i=48; i<64; i++)
+    {
+        MD5Step(MD5_F4, Reg[0], Reg[1], Reg[2], Reg[3], Mes[(7*(i & 0xf)) & 0xf],
+                MD5Table[i], LShiftVal[(0x3 << 2)+(i & 0x3)]);
+
+        // one-word right shift
+        Temp   = Reg[3];
+        Reg[3] = Reg[2];
+        Reg[2] = Reg[1];
+        Reg[1] = Reg[0];
+        Reg[0] = Temp;
+    }
+
+
+    // (temporary)output
+    for (i=0; i<4; i++)
+        Buf[i] += Reg[i];
+
+}
+
+
+
+/* =========================  SHA-1 implementation ========================== */
+// four base functions for SHA-1
+#define SHA1_F1(b, c, d)    (((b) & (c)) | ((~b) & (d)))
+#define SHA1_F2(b, c, d)    ((b) ^ (c) ^ (d))
+#define SHA1_F3(b, c, d)    (((b) & (c)) | ((b) & (d)) | ((c) & (d)))
+
+
+#define SHA1Step(f, a, b, c, d, e, w, k)    \
+    ( e	+= ( f(b, c, d) + w + k + CYCLIC_LEFT_SHIFT(a, 5)) & 0xffffffff, \
+      b = CYCLIC_LEFT_SHIFT(b, 30) )
+
+//Initiate SHA-1 Context satisfied in RFC 3174
+VOID SHAInit(SHA_CTX *pCtx)
+{
+    pCtx->Buf[0]=0x67452301;
+    pCtx->Buf[1]=0xefcdab89;
+    pCtx->Buf[2]=0x98badcfe;
+    pCtx->Buf[3]=0x10325476;
+    pCtx->Buf[4]=0xc3d2e1f0;
+
+    pCtx->LenInBitCount[0]=0;
+    pCtx->LenInBitCount[1]=0;
+}
+
+/*
+ *  Function Description:
+ *      Update SHA-1 Context, allow of an arrary of octets as the next
+ *      portion of the message
+ *
+ *  Arguments:
+ *      pCtx		Pointer	to SHA-1 context
+ * 	    pData       Pointer to input data
+ *      LenInBytes  The length of input data (unit: byte)
+ *
+ *  Return Value:
+ *      error       indicate more than pow(2,64) bits of data
+ *
+ *  Note:
+ *      Called after SHAInit or SHAUpdate(itself)
+ */
+UCHAR SHAUpdate(SHA_CTX *pCtx, UCHAR *pData, UINT32 LenInBytes)
+{
+    UINT32 TfTimes;
+    UINT32 temp1,temp2;
+	unsigned int i;
+	UCHAR err=1;
+
+    temp1 = pCtx->LenInBitCount[0];
+    temp2 = pCtx->LenInBitCount[1];
+
+    pCtx->LenInBitCount[0] = (UINT32) (pCtx->LenInBitCount[0] + (LenInBytes << 3));
+    if (pCtx->LenInBitCount[0] < temp1)
+        pCtx->LenInBitCount[1]++;   //carry in
+
+
+    pCtx->LenInBitCount[1] = (UINT32) (pCtx->LenInBitCount[1] +(LenInBytes >> 29));
+    if (pCtx->LenInBitCount[1] < temp2)
+        return (err);   //check total length of original data
+
+
+    // mod 64 bytes
+    temp1 = (temp1 >> 3) & 0x3f;
+
+    // process lacks of 64-byte data
+    if (temp1)
+    {
+        UCHAR *pAds = (UCHAR *) pCtx->Input + temp1;
+
+        if ((temp1+LenInBytes) < 64)
+        {
+            NdisMoveMemory(pAds, (UCHAR *)pData, LenInBytes);
+            return (0);
+        }
+
+        NdisMoveMemory(pAds, (UCHAR *)pData, 64-temp1);
+        byteReverse((UCHAR *)pCtx->Input, 16);
+
+        NdisZeroMemory((UCHAR *)pCtx->Input + 64, 16);
+        SHATransform(pCtx->Buf, (UINT32 *)pCtx->Input);
+
+        pData += 64-temp1;
+        LenInBytes -= 64-temp1;
+    } // end of if (temp1)
+
+
+    TfTimes = (LenInBytes >> 6);
+
+    for (i=TfTimes; i>0; i--)
+    {
+        NdisMoveMemory(pCtx->Input, (UCHAR *)pData, 64);
+        byteReverse((UCHAR *)pCtx->Input, 16);
+
+        NdisZeroMemory((UCHAR *)pCtx->Input + 64, 16);
+        SHATransform(pCtx->Buf, (UINT32 *)pCtx->Input);
+        pData += 64;
+        LenInBytes -= 64;
+    } // end of for
+
+    // buffering lacks of 64-byte data
+    if(LenInBytes)
+        NdisMoveMemory(pCtx->Input, (UCHAR *)pData, LenInBytes);
+
+	return (0);
+
+}
+
+// Append padding bits and length of original message in the tail
+// The message digest has to be completed in the end
+VOID SHAFinal(SHA_CTX *pCtx, UCHAR Digest[20])
+{
+    UCHAR Remainder;
+    UCHAR PadLenInBytes;
+    UCHAR *pAppend=0;
+    unsigned int i;
+
+    Remainder = (UCHAR)((pCtx->LenInBitCount[0] >> 3) & 0x3f);
+
+    pAppend = (UCHAR *)pCtx->Input + Remainder;
+
+    PadLenInBytes = (Remainder < 56) ? (56-Remainder) : (120-Remainder);
+
+    // padding bits without crossing block(64-byte based) boundary
+    if (Remainder < 56)
+    {
+        *pAppend = 0x80;
+        PadLenInBytes --;
+
+        NdisZeroMemory((UCHAR *)pCtx->Input + Remainder+1, PadLenInBytes);
+
+		// add data-length field, from high to low
+        for (i=0; i<4; i++)
+        {
+        	pCtx->Input[56+i] = (UCHAR)((pCtx->LenInBitCount[1] >> ((3-i) << 3)) & 0xff);
+        	pCtx->Input[60+i] = (UCHAR)((pCtx->LenInBitCount[0] >> ((3-i) << 3)) & 0xff);
+      	}
+
+        byteReverse((UCHAR *)pCtx->Input, 16);
+        NdisZeroMemory((UCHAR *)pCtx->Input + 64, 14);
+        SHATransform(pCtx->Buf, (UINT32 *)pCtx->Input);
+    } // end of if
+
+    // padding bits with crossing block(64-byte based) boundary
+    else
+    {
+        // the first block ===
+        *pAppend = 0x80;
+        PadLenInBytes --;
+
+        NdisZeroMemory((UCHAR *)pCtx->Input + Remainder+1, (64-Remainder-1));
+        PadLenInBytes -= (64 - Remainder - 1);
+
+        byteReverse((UCHAR *)pCtx->Input, 16);
+        NdisZeroMemory((UCHAR *)pCtx->Input + 64, 16);
+        SHATransform(pCtx->Buf, (UINT32 *)pCtx->Input);
+
+
+        // the second block ===
+        NdisZeroMemory((UCHAR *)pCtx->Input, PadLenInBytes);
+
+		// add data-length field
+		for (i=0; i<4; i++)
+        {
+        	pCtx->Input[56+i] = (UCHAR)((pCtx->LenInBitCount[1] >> ((3-i) << 3)) & 0xff);
+        	pCtx->Input[60+i] = (UCHAR)((pCtx->LenInBitCount[0] >> ((3-i) << 3)) & 0xff);
+      	}
+
+        byteReverse((UCHAR *)pCtx->Input, 16);
+        NdisZeroMemory((UCHAR *)pCtx->Input + 64, 16);
+        SHATransform(pCtx->Buf, (UINT32 *)pCtx->Input);
+    } // end of else
+
+
+    //Output, bytereverse
+    for (i=0; i<20; i++)
+    {
+        Digest [i] = (UCHAR)(pCtx->Buf[i>>2] >> 8*(3-(i & 0x3)));
+    }
+
+    NdisZeroMemory(pCtx, sizeof(pCtx)); // memory free
+}
+
+
+// The central algorithm of SHA-1, consists of four rounds and
+// twenty steps per round
+VOID SHATransform(UINT32 Buf[5], UINT32 Mes[20])
+{
+    UINT32 Reg[5],Temp;
+	unsigned int i;
+    UINT32 W[80];
+
+    static UINT32 SHA1Table[4] = { 0x5a827999, 0x6ed9eba1,
+                                  0x8f1bbcdc, 0xca62c1d6 };
+
+    Reg[0]=Buf[0];
+	Reg[1]=Buf[1];
+	Reg[2]=Buf[2];
+	Reg[3]=Buf[3];
+	Reg[4]=Buf[4];
+
+    //the first octet of a word is stored in the 0th element, bytereverse
+	for(i = 0; i < 16; i++)
+    {
+    	W[i]  = (Mes[i] >> 24) & 0xff;
+        W[i] |= (Mes[i] >> 8 ) & 0xff00;
+        W[i] |= (Mes[i] << 8 ) & 0xff0000;
+        W[i] |= (Mes[i] << 24) & 0xff000000;
+    }
+
+
+    for	(i = 0; i < 64; i++)
+	    W[16+i] = CYCLIC_LEFT_SHIFT(W[i] ^ W[2+i] ^ W[8+i] ^ W[13+i], 1);
+
+
+    // 80 steps in SHA-1 algorithm
+    for (i=0; i<80; i++)
+    {
+        if (i<20)
+            SHA1Step(SHA1_F1, Reg[0], Reg[1], Reg[2], Reg[3], Reg[4],
+                     W[i], SHA1Table[0]);
+
+        else if (i>=20 && i<40)
+            SHA1Step(SHA1_F2, Reg[0], Reg[1], Reg[2], Reg[3], Reg[4],
+                     W[i], SHA1Table[1]);
+
+		else if (i>=40 && i<60)
+            SHA1Step(SHA1_F3, Reg[0], Reg[1], Reg[2], Reg[3], Reg[4],
+                      W[i], SHA1Table[2]);
+
+        else
+            SHA1Step(SHA1_F2, Reg[0], Reg[1], Reg[2], Reg[3], Reg[4],
+                     W[i], SHA1Table[3]);
+
+
+       // one-word right shift
+		Temp   = Reg[4];
+        Reg[4] = Reg[3];
+        Reg[3] = Reg[2];
+        Reg[2] = Reg[1];
+        Reg[1] = Reg[0];
+        Reg[0] = Temp;
+
+    } // end of for-loop
+
+
+    // (temporary)output
+    for (i=0; i<5; i++)
+        Buf[i] += Reg[i];
+
+}
+
+
+/* =========================  AES En/Decryption ========================== */
+
+/* forward S-box */
+static uint32 FSb[256] =
+{
+	0x63, 0x7C,	0x77, 0x7B,	0xF2, 0x6B,	0x6F, 0xC5,
+	0x30, 0x01,	0x67, 0x2B,	0xFE, 0xD7,	0xAB, 0x76,
+	0xCA, 0x82,	0xC9, 0x7D,	0xFA, 0x59,	0x47, 0xF0,
+	0xAD, 0xD4,	0xA2, 0xAF,	0x9C, 0xA4,	0x72, 0xC0,
+	0xB7, 0xFD,	0x93, 0x26,	0x36, 0x3F,	0xF7, 0xCC,
+	0x34, 0xA5,	0xE5, 0xF1,	0x71, 0xD8,	0x31, 0x15,
+	0x04, 0xC7,	0x23, 0xC3,	0x18, 0x96,	0x05, 0x9A,
+	0x07, 0x12,	0x80, 0xE2,	0xEB, 0x27,	0xB2, 0x75,
+	0x09, 0x83,	0x2C, 0x1A,	0x1B, 0x6E,	0x5A, 0xA0,
+	0x52, 0x3B,	0xD6, 0xB3,	0x29, 0xE3,	0x2F, 0x84,
+	0x53, 0xD1,	0x00, 0xED,	0x20, 0xFC,	0xB1, 0x5B,
+	0x6A, 0xCB,	0xBE, 0x39,	0x4A, 0x4C,	0x58, 0xCF,
+	0xD0, 0xEF,	0xAA, 0xFB,	0x43, 0x4D,	0x33, 0x85,
+	0x45, 0xF9,	0x02, 0x7F,	0x50, 0x3C,	0x9F, 0xA8,
+	0x51, 0xA3,	0x40, 0x8F,	0x92, 0x9D,	0x38, 0xF5,
+	0xBC, 0xB6,	0xDA, 0x21,	0x10, 0xFF,	0xF3, 0xD2,
+	0xCD, 0x0C,	0x13, 0xEC,	0x5F, 0x97,	0x44, 0x17,
+	0xC4, 0xA7,	0x7E, 0x3D,	0x64, 0x5D,	0x19, 0x73,
+	0x60, 0x81,	0x4F, 0xDC,	0x22, 0x2A,	0x90, 0x88,
+	0x46, 0xEE,	0xB8, 0x14,	0xDE, 0x5E,	0x0B, 0xDB,
+	0xE0, 0x32,	0x3A, 0x0A,	0x49, 0x06,	0x24, 0x5C,
+	0xC2, 0xD3,	0xAC, 0x62,	0x91, 0x95,	0xE4, 0x79,
+	0xE7, 0xC8,	0x37, 0x6D,	0x8D, 0xD5,	0x4E, 0xA9,
+	0x6C, 0x56,	0xF4, 0xEA,	0x65, 0x7A,	0xAE, 0x08,
+	0xBA, 0x78,	0x25, 0x2E,	0x1C, 0xA6,	0xB4, 0xC6,
+	0xE8, 0xDD,	0x74, 0x1F,	0x4B, 0xBD,	0x8B, 0x8A,
+	0x70, 0x3E,	0xB5, 0x66,	0x48, 0x03,	0xF6, 0x0E,
+	0x61, 0x35,	0x57, 0xB9,	0x86, 0xC1,	0x1D, 0x9E,
+	0xE1, 0xF8,	0x98, 0x11,	0x69, 0xD9,	0x8E, 0x94,
+	0x9B, 0x1E,	0x87, 0xE9,	0xCE, 0x55,	0x28, 0xDF,
+	0x8C, 0xA1,	0x89, 0x0D,	0xBF, 0xE6,	0x42, 0x68,
+	0x41, 0x99,	0x2D, 0x0F,	0xB0, 0x54,	0xBB, 0x16
+};
+
+/* forward table */
+#define	FT \
+\
+	V(C6,63,63,A5),	V(F8,7C,7C,84),	V(EE,77,77,99),	V(F6,7B,7B,8D),	\
+	V(FF,F2,F2,0D),	V(D6,6B,6B,BD),	V(DE,6F,6F,B1),	V(91,C5,C5,54),	\
+	V(60,30,30,50),	V(02,01,01,03),	V(CE,67,67,A9),	V(56,2B,2B,7D),	\
+	V(E7,FE,FE,19),	V(B5,D7,D7,62),	V(4D,AB,AB,E6),	V(EC,76,76,9A),	\
+	V(8F,CA,CA,45),	V(1F,82,82,9D),	V(89,C9,C9,40),	V(FA,7D,7D,87),	\
+	V(EF,FA,FA,15),	V(B2,59,59,EB),	V(8E,47,47,C9),	V(FB,F0,F0,0B),	\
+	V(41,AD,AD,EC),	V(B3,D4,D4,67),	V(5F,A2,A2,FD),	V(45,AF,AF,EA),	\
+	V(23,9C,9C,BF),	V(53,A4,A4,F7),	V(E4,72,72,96),	V(9B,C0,C0,5B),	\
+	V(75,B7,B7,C2),	V(E1,FD,FD,1C),	V(3D,93,93,AE),	V(4C,26,26,6A),	\
+	V(6C,36,36,5A),	V(7E,3F,3F,41),	V(F5,F7,F7,02),	V(83,CC,CC,4F),	\
+	V(68,34,34,5C),	V(51,A5,A5,F4),	V(D1,E5,E5,34),	V(F9,F1,F1,08),	\
+	V(E2,71,71,93),	V(AB,D8,D8,73),	V(62,31,31,53),	V(2A,15,15,3F),	\
+	V(08,04,04,0C),	V(95,C7,C7,52),	V(46,23,23,65),	V(9D,C3,C3,5E),	\
+	V(30,18,18,28),	V(37,96,96,A1),	V(0A,05,05,0F),	V(2F,9A,9A,B5),	\
+	V(0E,07,07,09),	V(24,12,12,36),	V(1B,80,80,9B),	V(DF,E2,E2,3D),	\
+	V(CD,EB,EB,26),	V(4E,27,27,69),	V(7F,B2,B2,CD),	V(EA,75,75,9F),	\
+	V(12,09,09,1B),	V(1D,83,83,9E),	V(58,2C,2C,74),	V(34,1A,1A,2E),	\
+	V(36,1B,1B,2D),	V(DC,6E,6E,B2),	V(B4,5A,5A,EE),	V(5B,A0,A0,FB),	\
+	V(A4,52,52,F6),	V(76,3B,3B,4D),	V(B7,D6,D6,61),	V(7D,B3,B3,CE),	\
+	V(52,29,29,7B),	V(DD,E3,E3,3E),	V(5E,2F,2F,71),	V(13,84,84,97),	\
+	V(A6,53,53,F5),	V(B9,D1,D1,68),	V(00,00,00,00),	V(C1,ED,ED,2C),	\
+	V(40,20,20,60),	V(E3,FC,FC,1F),	V(79,B1,B1,C8),	V(B6,5B,5B,ED),	\
+	V(D4,6A,6A,BE),	V(8D,CB,CB,46),	V(67,BE,BE,D9),	V(72,39,39,4B),	\
+	V(94,4A,4A,DE),	V(98,4C,4C,D4),	V(B0,58,58,E8),	V(85,CF,CF,4A),	\
+	V(BB,D0,D0,6B),	V(C5,EF,EF,2A),	V(4F,AA,AA,E5),	V(ED,FB,FB,16),	\
+	V(86,43,43,C5),	V(9A,4D,4D,D7),	V(66,33,33,55),	V(11,85,85,94),	\
+	V(8A,45,45,CF),	V(E9,F9,F9,10),	V(04,02,02,06),	V(FE,7F,7F,81),	\
+	V(A0,50,50,F0),	V(78,3C,3C,44),	V(25,9F,9F,BA),	V(4B,A8,A8,E3),	\
+	V(A2,51,51,F3),	V(5D,A3,A3,FE),	V(80,40,40,C0),	V(05,8F,8F,8A),	\
+	V(3F,92,92,AD),	V(21,9D,9D,BC),	V(70,38,38,48),	V(F1,F5,F5,04),	\
+	V(63,BC,BC,DF),	V(77,B6,B6,C1),	V(AF,DA,DA,75),	V(42,21,21,63),	\
+	V(20,10,10,30),	V(E5,FF,FF,1A),	V(FD,F3,F3,0E),	V(BF,D2,D2,6D),	\
+	V(81,CD,CD,4C),	V(18,0C,0C,14),	V(26,13,13,35),	V(C3,EC,EC,2F),	\
+	V(BE,5F,5F,E1),	V(35,97,97,A2),	V(88,44,44,CC),	V(2E,17,17,39),	\
+	V(93,C4,C4,57),	V(55,A7,A7,F2),	V(FC,7E,7E,82),	V(7A,3D,3D,47),	\
+	V(C8,64,64,AC),	V(BA,5D,5D,E7),	V(32,19,19,2B),	V(E6,73,73,95),	\
+	V(C0,60,60,A0),	V(19,81,81,98),	V(9E,4F,4F,D1),	V(A3,DC,DC,7F),	\
+	V(44,22,22,66),	V(54,2A,2A,7E),	V(3B,90,90,AB),	V(0B,88,88,83),	\
+	V(8C,46,46,CA),	V(C7,EE,EE,29),	V(6B,B8,B8,D3),	V(28,14,14,3C),	\
+	V(A7,DE,DE,79),	V(BC,5E,5E,E2),	V(16,0B,0B,1D),	V(AD,DB,DB,76),	\
+	V(DB,E0,E0,3B),	V(64,32,32,56),	V(74,3A,3A,4E),	V(14,0A,0A,1E),	\
+	V(92,49,49,DB),	V(0C,06,06,0A),	V(48,24,24,6C),	V(B8,5C,5C,E4),	\
+	V(9F,C2,C2,5D),	V(BD,D3,D3,6E),	V(43,AC,AC,EF),	V(C4,62,62,A6),	\
+	V(39,91,91,A8),	V(31,95,95,A4),	V(D3,E4,E4,37),	V(F2,79,79,8B),	\
+	V(D5,E7,E7,32),	V(8B,C8,C8,43),	V(6E,37,37,59),	V(DA,6D,6D,B7),	\
+	V(01,8D,8D,8C),	V(B1,D5,D5,64),	V(9C,4E,4E,D2),	V(49,A9,A9,E0),	\
+	V(D8,6C,6C,B4),	V(AC,56,56,FA),	V(F3,F4,F4,07),	V(CF,EA,EA,25),	\
+	V(CA,65,65,AF),	V(F4,7A,7A,8E),	V(47,AE,AE,E9),	V(10,08,08,18),	\
+	V(6F,BA,BA,D5),	V(F0,78,78,88),	V(4A,25,25,6F),	V(5C,2E,2E,72),	\
+	V(38,1C,1C,24),	V(57,A6,A6,F1),	V(73,B4,B4,C7),	V(97,C6,C6,51),	\
+	V(CB,E8,E8,23),	V(A1,DD,DD,7C),	V(E8,74,74,9C),	V(3E,1F,1F,21),	\
+	V(96,4B,4B,DD),	V(61,BD,BD,DC),	V(0D,8B,8B,86),	V(0F,8A,8A,85),	\
+	V(E0,70,70,90),	V(7C,3E,3E,42),	V(71,B5,B5,C4),	V(CC,66,66,AA),	\
+	V(90,48,48,D8),	V(06,03,03,05),	V(F7,F6,F6,01),	V(1C,0E,0E,12),	\
+	V(C2,61,61,A3),	V(6A,35,35,5F),	V(AE,57,57,F9),	V(69,B9,B9,D0),	\
+	V(17,86,86,91),	V(99,C1,C1,58),	V(3A,1D,1D,27),	V(27,9E,9E,B9),	\
+	V(D9,E1,E1,38),	V(EB,F8,F8,13),	V(2B,98,98,B3),	V(22,11,11,33),	\
+	V(D2,69,69,BB),	V(A9,D9,D9,70),	V(07,8E,8E,89),	V(33,94,94,A7),	\
+	V(2D,9B,9B,B6),	V(3C,1E,1E,22),	V(15,87,87,92),	V(C9,E9,E9,20),	\
+	V(87,CE,CE,49),	V(AA,55,55,FF),	V(50,28,28,78),	V(A5,DF,DF,7A),	\
+	V(03,8C,8C,8F),	V(59,A1,A1,F8),	V(09,89,89,80),	V(1A,0D,0D,17),	\
+	V(65,BF,BF,DA),	V(D7,E6,E6,31),	V(84,42,42,C6),	V(D0,68,68,B8),	\
+	V(82,41,41,C3),	V(29,99,99,B0),	V(5A,2D,2D,77),	V(1E,0F,0F,11),	\
+	V(7B,B0,B0,CB),	V(A8,54,54,FC),	V(6D,BB,BB,D6),	V(2C,16,16,3A)
+
+#define	V(a,b,c,d) 0x##a##b##c##d
+static uint32 FT0[256] = { FT };
+#undef V
+
+#define	V(a,b,c,d) 0x##d##a##b##c
+static uint32 FT1[256] = { FT };
+#undef V
+
+#define	V(a,b,c,d) 0x##c##d##a##b
+static uint32 FT2[256] = { FT };
+#undef V
+
+#define	V(a,b,c,d) 0x##b##c##d##a
+static uint32 FT3[256] = { FT };
+#undef V
+
+#undef FT
+
+/* reverse S-box */
+
+static uint32 RSb[256] =
+{
+	0x52, 0x09,	0x6A, 0xD5,	0x30, 0x36,	0xA5, 0x38,
+	0xBF, 0x40,	0xA3, 0x9E,	0x81, 0xF3,	0xD7, 0xFB,
+	0x7C, 0xE3,	0x39, 0x82,	0x9B, 0x2F,	0xFF, 0x87,
+	0x34, 0x8E,	0x43, 0x44,	0xC4, 0xDE,	0xE9, 0xCB,
+	0x54, 0x7B,	0x94, 0x32,	0xA6, 0xC2,	0x23, 0x3D,
+	0xEE, 0x4C,	0x95, 0x0B,	0x42, 0xFA,	0xC3, 0x4E,
+	0x08, 0x2E,	0xA1, 0x66,	0x28, 0xD9,	0x24, 0xB2,
+	0x76, 0x5B,	0xA2, 0x49,	0x6D, 0x8B,	0xD1, 0x25,
+	0x72, 0xF8,	0xF6, 0x64,	0x86, 0x68,	0x98, 0x16,
+	0xD4, 0xA4,	0x5C, 0xCC,	0x5D, 0x65,	0xB6, 0x92,
+	0x6C, 0x70,	0x48, 0x50,	0xFD, 0xED,	0xB9, 0xDA,
+	0x5E, 0x15,	0x46, 0x57,	0xA7, 0x8D,	0x9D, 0x84,
+	0x90, 0xD8,	0xAB, 0x00,	0x8C, 0xBC,	0xD3, 0x0A,
+	0xF7, 0xE4,	0x58, 0x05,	0xB8, 0xB3,	0x45, 0x06,
+	0xD0, 0x2C,	0x1E, 0x8F,	0xCA, 0x3F,	0x0F, 0x02,
+	0xC1, 0xAF,	0xBD, 0x03,	0x01, 0x13,	0x8A, 0x6B,
+	0x3A, 0x91,	0x11, 0x41,	0x4F, 0x67,	0xDC, 0xEA,
+	0x97, 0xF2,	0xCF, 0xCE,	0xF0, 0xB4,	0xE6, 0x73,
+	0x96, 0xAC,	0x74, 0x22,	0xE7, 0xAD,	0x35, 0x85,
+	0xE2, 0xF9,	0x37, 0xE8,	0x1C, 0x75,	0xDF, 0x6E,
+	0x47, 0xF1,	0x1A, 0x71,	0x1D, 0x29,	0xC5, 0x89,
+	0x6F, 0xB7,	0x62, 0x0E,	0xAA, 0x18,	0xBE, 0x1B,
+	0xFC, 0x56,	0x3E, 0x4B,	0xC6, 0xD2,	0x79, 0x20,
+	0x9A, 0xDB,	0xC0, 0xFE,	0x78, 0xCD,	0x5A, 0xF4,
+	0x1F, 0xDD,	0xA8, 0x33,	0x88, 0x07,	0xC7, 0x31,
+	0xB1, 0x12,	0x10, 0x59,	0x27, 0x80,	0xEC, 0x5F,
+	0x60, 0x51,	0x7F, 0xA9,	0x19, 0xB5,	0x4A, 0x0D,
+	0x2D, 0xE5,	0x7A, 0x9F,	0x93, 0xC9,	0x9C, 0xEF,
+	0xA0, 0xE0,	0x3B, 0x4D,	0xAE, 0x2A,	0xF5, 0xB0,
+	0xC8, 0xEB,	0xBB, 0x3C,	0x83, 0x53,	0x99, 0x61,
+	0x17, 0x2B,	0x04, 0x7E,	0xBA, 0x77,	0xD6, 0x26,
+	0xE1, 0x69,	0x14, 0x63,	0x55, 0x21,	0x0C, 0x7D
+};
+
+/* reverse table */
+
+#define	RT \
+\
+	V(51,F4,A7,50),	V(7E,41,65,53),	V(1A,17,A4,C3),	V(3A,27,5E,96),	\
+	V(3B,AB,6B,CB),	V(1F,9D,45,F1),	V(AC,FA,58,AB),	V(4B,E3,03,93),	\
+	V(20,30,FA,55),	V(AD,76,6D,F6),	V(88,CC,76,91),	V(F5,02,4C,25),	\
+	V(4F,E5,D7,FC),	V(C5,2A,CB,D7),	V(26,35,44,80),	V(B5,62,A3,8F),	\
+	V(DE,B1,5A,49),	V(25,BA,1B,67),	V(45,EA,0E,98),	V(5D,FE,C0,E1),	\
+	V(C3,2F,75,02),	V(81,4C,F0,12),	V(8D,46,97,A3),	V(6B,D3,F9,C6),	\
+	V(03,8F,5F,E7),	V(15,92,9C,95),	V(BF,6D,7A,EB),	V(95,52,59,DA),	\
+	V(D4,BE,83,2D),	V(58,74,21,D3),	V(49,E0,69,29),	V(8E,C9,C8,44),	\
+	V(75,C2,89,6A),	V(F4,8E,79,78),	V(99,58,3E,6B),	V(27,B9,71,DD),	\
+	V(BE,E1,4F,B6),	V(F0,88,AD,17),	V(C9,20,AC,66),	V(7D,CE,3A,B4),	\
+	V(63,DF,4A,18),	V(E5,1A,31,82),	V(97,51,33,60),	V(62,53,7F,45),	\
+	V(B1,64,77,E0),	V(BB,6B,AE,84),	V(FE,81,A0,1C),	V(F9,08,2B,94),	\
+	V(70,48,68,58),	V(8F,45,FD,19),	V(94,DE,6C,87),	V(52,7B,F8,B7),	\
+	V(AB,73,D3,23),	V(72,4B,02,E2),	V(E3,1F,8F,57),	V(66,55,AB,2A),	\
+	V(B2,EB,28,07),	V(2F,B5,C2,03),	V(86,C5,7B,9A),	V(D3,37,08,A5),	\
+	V(30,28,87,F2),	V(23,BF,A5,B2),	V(02,03,6A,BA),	V(ED,16,82,5C),	\
+	V(8A,CF,1C,2B),	V(A7,79,B4,92),	V(F3,07,F2,F0),	V(4E,69,E2,A1),	\
+	V(65,DA,F4,CD),	V(06,05,BE,D5),	V(D1,34,62,1F),	V(C4,A6,FE,8A),	\
+	V(34,2E,53,9D),	V(A2,F3,55,A0),	V(05,8A,E1,32),	V(A4,F6,EB,75),	\
+	V(0B,83,EC,39),	V(40,60,EF,AA),	V(5E,71,9F,06),	V(BD,6E,10,51),	\
+	V(3E,21,8A,F9),	V(96,DD,06,3D),	V(DD,3E,05,AE),	V(4D,E6,BD,46),	\
+	V(91,54,8D,B5),	V(71,C4,5D,05),	V(04,06,D4,6F),	V(60,50,15,FF),	\
+	V(19,98,FB,24),	V(D6,BD,E9,97),	V(89,40,43,CC),	V(67,D9,9E,77),	\
+	V(B0,E8,42,BD),	V(07,89,8B,88),	V(E7,19,5B,38),	V(79,C8,EE,DB),	\
+	V(A1,7C,0A,47),	V(7C,42,0F,E9),	V(F8,84,1E,C9),	V(00,00,00,00),	\
+	V(09,80,86,83),	V(32,2B,ED,48),	V(1E,11,70,AC),	V(6C,5A,72,4E),	\
+	V(FD,0E,FF,FB),	V(0F,85,38,56),	V(3D,AE,D5,1E),	V(36,2D,39,27),	\
+	V(0A,0F,D9,64),	V(68,5C,A6,21),	V(9B,5B,54,D1),	V(24,36,2E,3A),	\
+	V(0C,0A,67,B1),	V(93,57,E7,0F),	V(B4,EE,96,D2),	V(1B,9B,91,9E),	\
+	V(80,C0,C5,4F),	V(61,DC,20,A2),	V(5A,77,4B,69),	V(1C,12,1A,16),	\
+	V(E2,93,BA,0A),	V(C0,A0,2A,E5),	V(3C,22,E0,43),	V(12,1B,17,1D),	\
+	V(0E,09,0D,0B),	V(F2,8B,C7,AD),	V(2D,B6,A8,B9),	V(14,1E,A9,C8),	\
+	V(57,F1,19,85),	V(AF,75,07,4C),	V(EE,99,DD,BB),	V(A3,7F,60,FD),	\
+	V(F7,01,26,9F),	V(5C,72,F5,BC),	V(44,66,3B,C5),	V(5B,FB,7E,34),	\
+	V(8B,43,29,76),	V(CB,23,C6,DC),	V(B6,ED,FC,68),	V(B8,E4,F1,63),	\
+	V(D7,31,DC,CA),	V(42,63,85,10),	V(13,97,22,40),	V(84,C6,11,20),	\
+	V(85,4A,24,7D),	V(D2,BB,3D,F8),	V(AE,F9,32,11),	V(C7,29,A1,6D),	\
+	V(1D,9E,2F,4B),	V(DC,B2,30,F3),	V(0D,86,52,EC),	V(77,C1,E3,D0),	\
+	V(2B,B3,16,6C),	V(A9,70,B9,99),	V(11,94,48,FA),	V(47,E9,64,22),	\
+	V(A8,FC,8C,C4),	V(A0,F0,3F,1A),	V(56,7D,2C,D8),	V(22,33,90,EF),	\
+	V(87,49,4E,C7),	V(D9,38,D1,C1),	V(8C,CA,A2,FE),	V(98,D4,0B,36),	\
+	V(A6,F5,81,CF),	V(A5,7A,DE,28),	V(DA,B7,8E,26),	V(3F,AD,BF,A4),	\
+	V(2C,3A,9D,E4),	V(50,78,92,0D),	V(6A,5F,CC,9B),	V(54,7E,46,62),	\
+	V(F6,8D,13,C2),	V(90,D8,B8,E8),	V(2E,39,F7,5E),	V(82,C3,AF,F5),	\
+	V(9F,5D,80,BE),	V(69,D0,93,7C),	V(6F,D5,2D,A9),	V(CF,25,12,B3),	\
+	V(C8,AC,99,3B),	V(10,18,7D,A7),	V(E8,9C,63,6E),	V(DB,3B,BB,7B),	\
+	V(CD,26,78,09),	V(6E,59,18,F4),	V(EC,9A,B7,01),	V(83,4F,9A,A8),	\
+	V(E6,95,6E,65),	V(AA,FF,E6,7E),	V(21,BC,CF,08),	V(EF,15,E8,E6),	\
+	V(BA,E7,9B,D9),	V(4A,6F,36,CE),	V(EA,9F,09,D4),	V(29,B0,7C,D6),	\
+	V(31,A4,B2,AF),	V(2A,3F,23,31),	V(C6,A5,94,30),	V(35,A2,66,C0),	\
+	V(74,4E,BC,37),	V(FC,82,CA,A6),	V(E0,90,D0,B0),	V(33,A7,D8,15),	\
+	V(F1,04,98,4A),	V(41,EC,DA,F7),	V(7F,CD,50,0E),	V(17,91,F6,2F),	\
+	V(76,4D,D6,8D),	V(43,EF,B0,4D),	V(CC,AA,4D,54),	V(E4,96,04,DF),	\
+	V(9E,D1,B5,E3),	V(4C,6A,88,1B),	V(C1,2C,1F,B8),	V(46,65,51,7F),	\
+	V(9D,5E,EA,04),	V(01,8C,35,5D),	V(FA,87,74,73),	V(FB,0B,41,2E),	\
+	V(B3,67,1D,5A),	V(92,DB,D2,52),	V(E9,10,56,33),	V(6D,D6,47,13),	\
+	V(9A,D7,61,8C),	V(37,A1,0C,7A),	V(59,F8,14,8E),	V(EB,13,3C,89),	\
+	V(CE,A9,27,EE),	V(B7,61,C9,35),	V(E1,1C,E5,ED),	V(7A,47,B1,3C),	\
+	V(9C,D2,DF,59),	V(55,F2,73,3F),	V(18,14,CE,79),	V(73,C7,37,BF),	\
+	V(53,F7,CD,EA),	V(5F,FD,AA,5B),	V(DF,3D,6F,14),	V(78,44,DB,86),	\
+	V(CA,AF,F3,81),	V(B9,68,C4,3E),	V(38,24,34,2C),	V(C2,A3,40,5F),	\
+	V(16,1D,C3,72),	V(BC,E2,25,0C),	V(28,3C,49,8B),	V(FF,0D,95,41),	\
+	V(39,A8,01,71),	V(08,0C,B3,DE),	V(D8,B4,E4,9C),	V(64,56,C1,90),	\
+	V(7B,CB,84,61),	V(D5,32,B6,70),	V(48,6C,5C,74),	V(D0,B8,57,42)
+
+#define	V(a,b,c,d) 0x##a##b##c##d
+static uint32 RT0[256] = { RT };
+#undef V
+
+#define	V(a,b,c,d) 0x##d##a##b##c
+static uint32 RT1[256] = { RT };
+#undef V
+
+#define	V(a,b,c,d) 0x##c##d##a##b
+static uint32 RT2[256] = { RT };
+#undef V
+
+#define	V(a,b,c,d) 0x##b##c##d##a
+static uint32 RT3[256] = { RT };
+#undef V
+
+#undef RT
+
+/* round constants */
+
+static uint32 RCON[10] =
+{
+	0x01000000,	0x02000000,	0x04000000,	0x08000000,
+	0x10000000,	0x20000000,	0x40000000,	0x80000000,
+	0x1B000000,	0x36000000
+};
+
+/* key schedule	tables */
+
+static int KT_init = 1;
+
+static uint32 KT0[256];
+static uint32 KT1[256];
+static uint32 KT2[256];
+static uint32 KT3[256];
+
+/* platform-independant	32-bit integer manipulation	macros */
+
+#define	GET_UINT32(n,b,i)						\
+{												\
+	(n)	= (	(uint32) (b)[(i)	] << 24	)		\
+		| (	(uint32) (b)[(i) + 1] << 16	)		\
+		| (	(uint32) (b)[(i) + 2] <<  8	)		\
+		| (	(uint32) (b)[(i) + 3]		);		\
+}
+
+#define	PUT_UINT32(n,b,i)						\
+{												\
+	(b)[(i)	   ] = (uint8) ( (n) >>	24 );		\
+	(b)[(i)	+ 1] = (uint8) ( (n) >>	16 );		\
+	(b)[(i)	+ 2] = (uint8) ( (n) >>	 8 );		\
+	(b)[(i)	+ 3] = (uint8) ( (n)	   );		\
+}
+
+/* AES key scheduling routine */
+
+int	rtmp_aes_set_key( aes_context *ctx, uint8 *key, int nbits )
+{
+	int	i;
+	uint32 *RK,	*SK;
+
+	switch(	nbits )
+	{
+		case 128: ctx->nr =	10;	break;
+		case 192: ctx->nr =	12;	break;
+		case 256: ctx->nr =	14;	break;
+		default	: return( 1	);
+	}
+
+	RK = ctx->erk;
+
+	for( i = 0;	i <	(nbits >> 5); i++ )
+	{
+		GET_UINT32(	RK[i], key,	i *	4 );
+	}
+
+	/* setup encryption	round keys */
+
+	switch(	nbits )
+	{
+	case 128:
+
+		for( i = 0;	i <	10;	i++, RK	+= 4 )
+		{
+			RK[4]  = RK[0] ^ RCON[i] ^
+						( FSb[ (uint8) ( RK[3] >> 16 ) ] <<	24 ) ^
+						( FSb[ (uint8) ( RK[3] >>  8 ) ] <<	16 ) ^
+						( FSb[ (uint8) ( RK[3]		 ) ] <<	 8 ) ^
+						( FSb[ (uint8) ( RK[3] >> 24 ) ]	   );
+
+			RK[5]  = RK[1] ^ RK[4];
+			RK[6]  = RK[2] ^ RK[5];
+			RK[7]  = RK[3] ^ RK[6];
+		}
+		break;
+
+	case 192:
+
+		for( i = 0;	i <	8; i++,	RK += 6	)
+		{
+			RK[6]  = RK[0] ^ RCON[i] ^
+						( FSb[ (uint8) ( RK[5] >> 16 ) ] <<	24 ) ^
+						( FSb[ (uint8) ( RK[5] >>  8 ) ] <<	16 ) ^
+						( FSb[ (uint8) ( RK[5]		 ) ] <<	 8 ) ^
+						( FSb[ (uint8) ( RK[5] >> 24 ) ]	   );
+
+			RK[7]  = RK[1] ^ RK[6];
+			RK[8]  = RK[2] ^ RK[7];
+			RK[9]  = RK[3] ^ RK[8];
+			RK[10] = RK[4] ^ RK[9];
+			RK[11] = RK[5] ^ RK[10];
+		}
+		break;
+
+	case 256:
+
+		for( i = 0;	i <	7; i++,	RK += 8	)
+		{
+			RK[8]  = RK[0] ^ RCON[i] ^
+						( FSb[ (uint8) ( RK[7] >> 16 ) ] <<	24 ) ^
+						( FSb[ (uint8) ( RK[7] >>  8 ) ] <<	16 ) ^
+						( FSb[ (uint8) ( RK[7]		 ) ] <<	 8 ) ^
+						( FSb[ (uint8) ( RK[7] >> 24 ) ]	   );
+
+			RK[9]  = RK[1] ^ RK[8];
+			RK[10] = RK[2] ^ RK[9];
+			RK[11] = RK[3] ^ RK[10];
+
+			RK[12] = RK[4] ^
+						( FSb[ (uint8) ( RK[11]	>> 24 )	] << 24	) ^
+						( FSb[ (uint8) ( RK[11]	>> 16 )	] << 16	) ^
+						( FSb[ (uint8) ( RK[11]	>>	8 )	] <<  8	) ^
+						( FSb[ (uint8) ( RK[11]		  )	]		);
+
+			RK[13] = RK[5] ^ RK[12];
+			RK[14] = RK[6] ^ RK[13];
+			RK[15] = RK[7] ^ RK[14];
+		}
+		break;
+	}
+
+	/* setup decryption	round keys */
+
+	if(	KT_init	)
+	{
+		for( i = 0;	i <	256; i++ )
+		{
+			KT0[i] = RT0[ FSb[i] ];
+			KT1[i] = RT1[ FSb[i] ];
+			KT2[i] = RT2[ FSb[i] ];
+			KT3[i] = RT3[ FSb[i] ];
+		}
+
+		KT_init	= 0;
+	}
+
+	SK = ctx->drk;
+
+	*SK++ =	*RK++;
+	*SK++ =	*RK++;
+	*SK++ =	*RK++;
+	*SK++ =	*RK++;
+
+	for( i = 1;	i <	ctx->nr; i++ )
+	{
+		RK -= 8;
+
+		*SK++ =	KT0[ (uint8) ( *RK >> 24 ) ] ^
+				KT1[ (uint8) ( *RK >> 16 ) ] ^
+				KT2[ (uint8) ( *RK >>  8 ) ] ^
+				KT3[ (uint8) ( *RK		 ) ]; RK++;
+
+		*SK++ =	KT0[ (uint8) ( *RK >> 24 ) ] ^
+				KT1[ (uint8) ( *RK >> 16 ) ] ^
+				KT2[ (uint8) ( *RK >>  8 ) ] ^
+				KT3[ (uint8) ( *RK		 ) ]; RK++;
+
+		*SK++ =	KT0[ (uint8) ( *RK >> 24 ) ] ^
+				KT1[ (uint8) ( *RK >> 16 ) ] ^
+				KT2[ (uint8) ( *RK >>  8 ) ] ^
+				KT3[ (uint8) ( *RK		 ) ]; RK++;
+
+		*SK++ =	KT0[ (uint8) ( *RK >> 24 ) ] ^
+				KT1[ (uint8) ( *RK >> 16 ) ] ^
+				KT2[ (uint8) ( *RK >>  8 ) ] ^
+				KT3[ (uint8) ( *RK		 ) ]; RK++;
+	}
+
+	RK -= 8;
+
+	*SK++ =	*RK++;
+	*SK++ =	*RK++;
+	*SK++ =	*RK++;
+	*SK++ =	*RK++;
+
+	return(	0 );
+}
+
+/* AES 128-bit block encryption	routine	*/
+
+void rtmp_aes_encrypt(aes_context *ctx, uint8 input[16],	uint8 output[16] )
+{
+	uint32 *RK,	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3;
+
+	RK = ctx->erk;
+	GET_UINT32(	X0,	input,	0 ); X0	^= RK[0];
+	GET_UINT32(	X1,	input,	4 ); X1	^= RK[1];
+	GET_UINT32(	X2,	input,	8 ); X2	^= RK[2];
+	GET_UINT32(	X3,	input, 12 ); X3	^= RK[3];
+
+#define	AES_FROUND(X0,X1,X2,X3,Y0,Y1,Y2,Y3)		\
+{												\
+	RK += 4;									\
+												\
+	X0 = RK[0] ^ FT0[ (uint8) (	Y0 >> 24 ) ] ^	\
+				 FT1[ (uint8) (	Y1 >> 16 ) ] ^	\
+				 FT2[ (uint8) (	Y2 >>  8 ) ] ^	\
+				 FT3[ (uint8) (	Y3		 ) ];	\
+												\
+	X1 = RK[1] ^ FT0[ (uint8) (	Y1 >> 24 ) ] ^	\
+				 FT1[ (uint8) (	Y2 >> 16 ) ] ^	\
+				 FT2[ (uint8) (	Y3 >>  8 ) ] ^	\
+				 FT3[ (uint8) (	Y0		 ) ];	\
+												\
+	X2 = RK[2] ^ FT0[ (uint8) (	Y2 >> 24 ) ] ^	\
+				 FT1[ (uint8) (	Y3 >> 16 ) ] ^	\
+				 FT2[ (uint8) (	Y0 >>  8 ) ] ^	\
+				 FT3[ (uint8) (	Y1		 ) ];	\
+												\
+	X3 = RK[3] ^ FT0[ (uint8) (	Y3 >> 24 ) ] ^	\
+				 FT1[ (uint8) (	Y0 >> 16 ) ] ^	\
+				 FT2[ (uint8) (	Y1 >>  8 ) ] ^	\
+				 FT3[ (uint8) (	Y2		 ) ];	\
+}
+
+	AES_FROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 1 */
+	AES_FROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 2 */
+	AES_FROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 3 */
+	AES_FROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 4 */
+	AES_FROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 5 */
+	AES_FROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 6 */
+	AES_FROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 7 */
+	AES_FROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 8 */
+	AES_FROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 9 */
+
+	if(	ctx->nr	> 10 )
+	{
+		AES_FROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );	/* round 10	*/
+		AES_FROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );	/* round 11	*/
+	}
+
+	if(	ctx->nr	> 12 )
+	{
+		AES_FROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );	/* round 12	*/
+		AES_FROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );	/* round 13	*/
+	}
+
+	/* last	round */
+
+	RK += 4;
+
+	X0 = RK[0] ^ ( FSb[	(uint8)	( Y0 >>	24 ) ] << 24 ) ^
+				 ( FSb[	(uint8)	( Y1 >>	16 ) ] << 16 ) ^
+				 ( FSb[	(uint8)	( Y2 >>	 8 ) ] <<  8 ) ^
+				 ( FSb[	(uint8)	( Y3	   ) ]		 );
+
+	X1 = RK[1] ^ ( FSb[	(uint8)	( Y1 >>	24 ) ] << 24 ) ^
+				 ( FSb[	(uint8)	( Y2 >>	16 ) ] << 16 ) ^
+				 ( FSb[	(uint8)	( Y3 >>	 8 ) ] <<  8 ) ^
+				 ( FSb[	(uint8)	( Y0	   ) ]		 );
+
+	X2 = RK[2] ^ ( FSb[	(uint8)	( Y2 >>	24 ) ] << 24 ) ^
+				 ( FSb[	(uint8)	( Y3 >>	16 ) ] << 16 ) ^
+				 ( FSb[	(uint8)	( Y0 >>	 8 ) ] <<  8 ) ^
+				 ( FSb[	(uint8)	( Y1	   ) ]		 );
+
+	X3 = RK[3] ^ ( FSb[	(uint8)	( Y3 >>	24 ) ] << 24 ) ^
+				 ( FSb[	(uint8)	( Y0 >>	16 ) ] << 16 ) ^
+				 ( FSb[	(uint8)	( Y1 >>	 8 ) ] <<  8 ) ^
+				 ( FSb[	(uint8)	( Y2	   ) ]		 );
+
+	PUT_UINT32(	X0,	output,	 0 );
+	PUT_UINT32(	X1,	output,	 4 );
+	PUT_UINT32(	X2,	output,	 8 );
+	PUT_UINT32(	X3,	output,	12 );
+}
+
+/* AES 128-bit block decryption	routine	*/
+
+void rtmp_aes_decrypt( aes_context *ctx,	uint8 input[16], uint8 output[16] )
+{
+	uint32 *RK,	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3;
+
+	RK = ctx->drk;
+
+	GET_UINT32(	X0,	input,	0 ); X0	^= RK[0];
+	GET_UINT32(	X1,	input,	4 ); X1	^= RK[1];
+	GET_UINT32(	X2,	input,	8 ); X2	^= RK[2];
+	GET_UINT32(	X3,	input, 12 ); X3	^= RK[3];
+
+#define	AES_RROUND(X0,X1,X2,X3,Y0,Y1,Y2,Y3)		\
+{												\
+	RK += 4;									\
+												\
+	X0 = RK[0] ^ RT0[ (uint8) (	Y0 >> 24 ) ] ^	\
+				 RT1[ (uint8) (	Y3 >> 16 ) ] ^	\
+				 RT2[ (uint8) (	Y2 >>  8 ) ] ^	\
+				 RT3[ (uint8) (	Y1		 ) ];	\
+												\
+	X1 = RK[1] ^ RT0[ (uint8) (	Y1 >> 24 ) ] ^	\
+				 RT1[ (uint8) (	Y0 >> 16 ) ] ^	\
+				 RT2[ (uint8) (	Y3 >>  8 ) ] ^	\
+				 RT3[ (uint8) (	Y2		 ) ];	\
+												\
+	X2 = RK[2] ^ RT0[ (uint8) (	Y2 >> 24 ) ] ^	\
+				 RT1[ (uint8) (	Y1 >> 16 ) ] ^	\
+				 RT2[ (uint8) (	Y0 >>  8 ) ] ^	\
+				 RT3[ (uint8) (	Y3		 ) ];	\
+												\
+	X3 = RK[3] ^ RT0[ (uint8) (	Y3 >> 24 ) ] ^	\
+				 RT1[ (uint8) (	Y2 >> 16 ) ] ^	\
+				 RT2[ (uint8) (	Y1 >>  8 ) ] ^	\
+				 RT3[ (uint8) (	Y0		 ) ];	\
+}
+
+	AES_RROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 1 */
+	AES_RROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 2 */
+	AES_RROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 3 */
+	AES_RROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 4 */
+	AES_RROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 5 */
+	AES_RROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 6 */
+	AES_RROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 7 */
+	AES_RROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );		/* round 8 */
+	AES_RROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );		/* round 9 */
+
+	if(	ctx->nr	> 10 )
+	{
+		AES_RROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );	/* round 10	*/
+		AES_RROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );	/* round 11	*/
+	}
+
+	if(	ctx->nr	> 12 )
+	{
+		AES_RROUND(	X0,	X1,	X2,	X3,	Y0,	Y1,	Y2,	Y3 );	/* round 12	*/
+		AES_RROUND(	Y0,	Y1,	Y2,	Y3,	X0,	X1,	X2,	X3 );	/* round 13	*/
+	}
+
+	/* last	round */
+
+	RK += 4;
+
+	X0 = RK[0] ^ ( RSb[	(uint8)	( Y0 >>	24 ) ] << 24 ) ^
+				 ( RSb[	(uint8)	( Y3 >>	16 ) ] << 16 ) ^
+				 ( RSb[	(uint8)	( Y2 >>	 8 ) ] <<  8 ) ^
+				 ( RSb[	(uint8)	( Y1	   ) ]		 );
+
+	X1 = RK[1] ^ ( RSb[	(uint8)	( Y1 >>	24 ) ] << 24 ) ^
+				 ( RSb[	(uint8)	( Y0 >>	16 ) ] << 16 ) ^
+				 ( RSb[	(uint8)	( Y3 >>	 8 ) ] <<  8 ) ^
+				 ( RSb[	(uint8)	( Y2	   ) ]		 );
+
+	X2 = RK[2] ^ ( RSb[	(uint8)	( Y2 >>	24 ) ] << 24 ) ^
+				 ( RSb[	(uint8)	( Y1 >>	16 ) ] << 16 ) ^
+				 ( RSb[	(uint8)	( Y0 >>	 8 ) ] <<  8 ) ^
+				 ( RSb[	(uint8)	( Y3	   ) ]		 );
+
+	X3 = RK[3] ^ ( RSb[	(uint8)	( Y3 >>	24 ) ] << 24 ) ^
+				 ( RSb[	(uint8)	( Y2 >>	16 ) ] << 16 ) ^
+				 ( RSb[	(uint8)	( Y1 >>	 8 ) ] <<  8 ) ^
+				 ( RSb[	(uint8)	( Y0	   ) ]		 );
+
+	PUT_UINT32(	X0,	output,	 0 );
+	PUT_UINT32(	X1,	output,	 4 );
+	PUT_UINT32(	X2,	output,	 8 );
+	PUT_UINT32(	X3,	output,	12 );
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		SHA1 function
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID	HMAC_SHA1(
+	IN	UCHAR	*text,
+	IN	UINT	text_len,
+	IN	UCHAR	*key,
+	IN	UINT	key_len,
+	IN	UCHAR	*digest)
+{
+	SHA_CTX	context;
+	UCHAR	k_ipad[65]; /* inner padding - key XORd with ipad	*/
+	UCHAR	k_opad[65]; /* outer padding - key XORd with opad	*/
+	INT		i;
+
+	// if key is longer	than 64	bytes reset	it to key=SHA1(key)
+	if (key_len	> 64)
+	{
+		SHA_CTX		 tctx;
+		SHAInit(&tctx);
+		SHAUpdate(&tctx, key, key_len);
+		SHAFinal(&tctx,	key);
+		key_len	= 20;
+	}
+	NdisZeroMemory(k_ipad, sizeof(k_ipad));
+	NdisZeroMemory(k_opad, sizeof(k_opad));
+	NdisMoveMemory(k_ipad, key,	key_len);
+	NdisMoveMemory(k_opad, key,	key_len);
+
+	// XOR key with	ipad and opad values
+	for	(i = 0;	i <	64;	i++)
+	{
+		k_ipad[i] ^= 0x36;
+		k_opad[i] ^= 0x5c;
+	}
+
+	// perform inner SHA1
+	SHAInit(&context); 						/* init context for 1st pass */
+	SHAUpdate(&context,	k_ipad,	64);		/*	start with inner pad */
+	SHAUpdate(&context,	text, text_len);	/*	then text of datagram */
+	SHAFinal(&context, digest);				/* finish up 1st pass */
+
+	//perform outer	SHA1
+	SHAInit(&context);					/* init context for 2nd pass */
+	SHAUpdate(&context,	k_opad,	64);	/*	start with outer pad */
+	SHAUpdate(&context,	digest,	20);	/*	then results of	1st	hash */
+	SHAFinal(&context, digest);			/* finish up 2nd pass */
+
+}
+
+/*
+* F(P, S, c, i) = U1 xor U2 xor ... Uc
+* U1 = PRF(P, S || Int(i))
+* U2 = PRF(P, U1)
+* Uc = PRF(P, Uc-1)
+*/
+
+void F(char *password, unsigned char *ssid, int ssidlength, int iterations, int count, unsigned char *output)
+{
+    unsigned char digest[36], digest1[SHA_DIGEST_LEN];
+    int i, j;
+
+    /* U1 = PRF(P, S || int(i)) */
+    memcpy(digest, ssid, ssidlength);
+    digest[ssidlength] = (unsigned char)((count>>24) & 0xff);
+    digest[ssidlength+1] = (unsigned char)((count>>16) & 0xff);
+    digest[ssidlength+2] = (unsigned char)((count>>8) & 0xff);
+    digest[ssidlength+3] = (unsigned char)(count & 0xff);
+    HMAC_SHA1(digest, ssidlength+4, (unsigned char*) password, (int) strlen(password), digest1); // for WPA update
+
+    /* output = U1 */
+    memcpy(output, digest1, SHA_DIGEST_LEN);
+
+    for (i = 1; i < iterations; i++)
+    {
+        /* Un = PRF(P, Un-1) */
+        HMAC_SHA1(digest1, SHA_DIGEST_LEN, (unsigned char*) password, (int) strlen(password), digest); // for WPA update
+        memcpy(digest1, digest, SHA_DIGEST_LEN);
+
+        /* output = output xor Un */
+        for (j = 0; j < SHA_DIGEST_LEN; j++)
+        {
+            output[j] ^= digest[j];
+        }
+    }
+}
+/*
+* password - ascii string up to 63 characters in length
+* ssid - octet string up to 32 octets
+* ssidlength - length of ssid in octets
+* output must be 40 octets in length and outputs 256 bits of key
+*/
+int PasswordHash(char *password, unsigned char *ssid, int ssidlength, unsigned char *output)
+{
+    if ((strlen(password) > 63) || (ssidlength > 32))
+        return 0;
+
+    F(password, ssid, ssidlength, 4096, 1, output);
+    F(password, ssid, ssidlength, 4096, 2, &output[SHA_DIGEST_LEN]);
+    return 1;
+}
+
+
diff --git a/drivers/staging/rt2870/common/mlme.c b/drivers/staging/rt2870/common/mlme.c
new file mode 100644
index 00000000000..8a82cee8bf2
--- /dev/null
+++ b/drivers/staging/rt2870/common/mlme.c
@@ -0,0 +1,8609 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	mlme.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John Chang	2004-08-25		Modify from RT2500 code base
+	John Chang	2004-09-06		modified for RT2600
+*/
+
+#include "../rt_config.h"
+#include <stdarg.h>
+
+UCHAR	CISCO_OUI[] = {0x00, 0x40, 0x96};
+
+UCHAR	WPA_OUI[] = {0x00, 0x50, 0xf2, 0x01};
+UCHAR	RSN_OUI[] = {0x00, 0x0f, 0xac};
+UCHAR	WAPI_OUI[] = {0x00, 0x14, 0x72};
+UCHAR   WME_INFO_ELEM[]  = {0x00, 0x50, 0xf2, 0x02, 0x00, 0x01};
+UCHAR   WME_PARM_ELEM[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01};
+UCHAR	Ccx2QosInfo[] = {0x00, 0x40, 0x96, 0x04};
+UCHAR   RALINK_OUI[]  = {0x00, 0x0c, 0x43};
+UCHAR   BROADCOM_OUI[]  = {0x00, 0x90, 0x4c};
+UCHAR   WPS_OUI[] = {0x00, 0x50, 0xf2, 0x04};
+#ifdef CONFIG_STA_SUPPORT
+#ifdef DOT11_N_SUPPORT
+UCHAR	PRE_N_HT_OUI[]	= {0x00, 0x90, 0x4c};
+#endif // DOT11_N_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+UCHAR RateSwitchTable[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x11, 0x00,  0,  0,  0,						// Initial used item after association
+    0x00, 0x00,  0, 40, 101,
+    0x01, 0x00,  1, 40, 50,
+    0x02, 0x00,  2, 35, 45,
+    0x03, 0x00,  3, 20, 45,
+    0x04, 0x21,  0, 30, 50,
+    0x05, 0x21,  1, 20, 50,
+    0x06, 0x21,  2, 20, 50,
+    0x07, 0x21,  3, 15, 50,
+    0x08, 0x21,  4, 15, 30,
+    0x09, 0x21,  5, 10, 25,
+    0x0a, 0x21,  6,  8, 25,
+    0x0b, 0x21,  7,  8, 25,
+    0x0c, 0x20, 12,  15, 30,
+    0x0d, 0x20, 13,  8, 20,
+    0x0e, 0x20, 14,  8, 20,
+    0x0f, 0x20, 15,  8, 25,
+    0x10, 0x22, 15,  8, 25,
+    0x11, 0x00,  0,  0,  0,
+    0x12, 0x00,  0,  0,  0,
+    0x13, 0x00,  0,  0,  0,
+    0x14, 0x00,  0,  0,  0,
+    0x15, 0x00,  0,  0,  0,
+    0x16, 0x00,  0,  0,  0,
+    0x17, 0x00,  0,  0,  0,
+    0x18, 0x00,  0,  0,  0,
+    0x19, 0x00,  0,  0,  0,
+    0x1a, 0x00,  0,  0,  0,
+    0x1b, 0x00,  0,  0,  0,
+    0x1c, 0x00,  0,  0,  0,
+    0x1d, 0x00,  0,  0,  0,
+    0x1e, 0x00,  0,  0,  0,
+    0x1f, 0x00,  0,  0,  0,
+};
+
+UCHAR RateSwitchTable11B[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x04, 0x03,  0,  0,  0,						// Initial used item after association
+    0x00, 0x00,  0, 40, 101,
+    0x01, 0x00,  1, 40, 50,
+    0x02, 0x00,  2, 35, 45,
+    0x03, 0x00,  3, 20, 45,
+};
+
+UCHAR RateSwitchTable11BG[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0a, 0x00,  0,  0,  0,						// Initial used item after association
+    0x00, 0x00,  0, 40, 101,
+    0x01, 0x00,  1, 40, 50,
+    0x02, 0x00,  2, 35, 45,
+    0x03, 0x00,  3, 20, 45,
+    0x04, 0x10,  2, 20, 35,
+    0x05, 0x10,  3, 16, 35,
+    0x06, 0x10,  4, 10, 25,
+    0x07, 0x10,  5, 16, 25,
+    0x08, 0x10,  6, 10, 25,
+    0x09, 0x10,  7, 10, 13,
+};
+
+UCHAR RateSwitchTable11G[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x08, 0x00,  0,  0,  0,						// Initial used item after association
+    0x00, 0x10,  0, 20, 101,
+    0x01, 0x10,  1, 20, 35,
+    0x02, 0x10,  2, 20, 35,
+    0x03, 0x10,  3, 16, 35,
+    0x04, 0x10,  4, 10, 25,
+    0x05, 0x10,  5, 16, 25,
+    0x06, 0x10,  6, 10, 25,
+    0x07, 0x10,  7, 10, 13,
+};
+
+#ifdef DOT11_N_SUPPORT
+UCHAR RateSwitchTable11N1S[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x09, 0x00,  0,  0,  0,						// Initial used item after association
+    0x00, 0x21,  0, 30, 101,
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x21,  5, 10, 25,
+    0x06, 0x21,  6,  8, 14,
+    0x07, 0x21,  7,  8, 14,
+    0x08, 0x23,  7,  8, 14,
+};
+
+UCHAR RateSwitchTable11N2S[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0a, 0x00,  0,  0,  0,      // Initial used item after association
+    0x00, 0x21,  0, 30, 101,
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x20, 12,  15, 30,
+    0x06, 0x20, 13,  8, 20,
+    0x07, 0x20, 14,  8, 20,
+    0x08, 0x20, 15,  8, 25,
+    0x09, 0x22, 15,  8, 25,
+};
+
+UCHAR RateSwitchTable11N3S[] = {
+// Item No.	Mode	Curr-MCS	TrainUp	TrainDown	// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0a, 0x00,  0,  0,  0,      // Initial used item after association
+    0x00, 0x21,  0, 30, 101,
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x20, 12,  15, 30,
+    0x06, 0x20, 13,  8, 20,
+    0x07, 0x20, 14,  8, 20,
+    0x08, 0x20, 15,  8, 25,
+    0x09, 0x22, 15,  8, 25,
+};
+
+UCHAR RateSwitchTable11N2SForABand[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0b, 0x09,  0,  0,  0,						// Initial used item after association
+    0x00, 0x21,  0, 30, 101,
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x21,  5, 15, 30,
+    0x06, 0x20, 12,  15, 30,
+    0x07, 0x20, 13,  8, 20,
+    0x08, 0x20, 14,  8, 20,
+    0x09, 0x20, 15,  8, 25,
+    0x0a, 0x22, 15,  8, 25,
+};
+
+UCHAR RateSwitchTable11N3SForABand[] = { // 3*3
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0b, 0x09,  0,  0,  0,						// Initial used item after association
+    0x00, 0x21,  0, 30, 101,
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x21,  5, 15, 30,
+    0x06, 0x20, 12,  15, 30,
+    0x07, 0x20, 13,  8, 20,
+    0x08, 0x20, 14,  8, 20,
+    0x09, 0x20, 15,  8, 25,
+    0x0a, 0x22, 15,  8, 25,
+};
+
+UCHAR RateSwitchTable11BGN1S[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0d, 0x00,  0,  0,  0,						// Initial used item after association
+    0x00, 0x00,  0, 40, 101,
+    0x01, 0x00,  1, 40, 50,
+    0x02, 0x00,  2, 35, 45,
+    0x03, 0x00,  3, 20, 45,
+    0x04, 0x21,  0, 30,101,	//50
+    0x05, 0x21,  1, 20, 50,
+    0x06, 0x21,  2, 20, 50,
+    0x07, 0x21,  3, 15, 50,
+    0x08, 0x21,  4, 15, 30,
+    0x09, 0x21,  5, 10, 25,
+    0x0a, 0x21,  6,  8, 14,
+    0x0b, 0x21,  7,  8, 14,
+	0x0c, 0x23,  7,  8, 14,
+};
+
+UCHAR RateSwitchTable11BGN2S[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0a, 0x00,  0,  0,  0,						// Initial used item after association
+    0x00, 0x21,  0, 30,101,	//50
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x20, 12, 15, 30,
+    0x06, 0x20, 13,  8, 20,
+    0x07, 0x20, 14,  8, 20,
+    0x08, 0x20, 15,  8, 25,
+    0x09, 0x22, 15,  8, 25,
+};
+
+UCHAR RateSwitchTable11BGN3S[] = { // 3*3
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0a, 0x00,  0,  0,  0,						// Initial used item after association
+    0x00, 0x21,  0, 30,101,	//50
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 20, 50,
+    0x04, 0x21,  4, 15, 50,
+#if 1
+    0x05, 0x20, 20, 15, 30,
+    0x06, 0x20, 21,  8, 20,
+    0x07, 0x20, 22,  8, 20,
+    0x08, 0x20, 23,  8, 25,
+    0x09, 0x22, 23,  8, 25,
+#else // for RT2860 2*3 test
+    0x05, 0x20, 12, 15, 30,
+    0x06, 0x20, 13,  8, 20,
+    0x07, 0x20, 14,  8, 20,
+    0x08, 0x20, 15,  8, 25,
+    0x09, 0x22, 15,  8, 25,
+#endif
+};
+
+UCHAR RateSwitchTable11BGN2SForABand[] = {
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0b, 0x09,  0,  0,  0,						// Initial used item after association
+    0x00, 0x21,  0, 30,101,	//50
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x21,  5, 15, 30,
+    0x06, 0x20, 12, 15, 30,
+    0x07, 0x20, 13,  8, 20,
+    0x08, 0x20, 14,  8, 20,
+    0x09, 0x20, 15,  8, 25,
+    0x0a, 0x22, 15,  8, 25,
+};
+
+UCHAR RateSwitchTable11BGN3SForABand[] = { // 3*3
+// Item No.   Mode   Curr-MCS   TrainUp   TrainDown		// Mode- Bit0: STBC, Bit1: Short GI, Bit4,5: Mode(0:CCK, 1:OFDM, 2:HT Mix, 3:HT GF)
+    0x0c, 0x09,  0,  0,  0,						// Initial used item after association
+    0x00, 0x21,  0, 30,101,	//50
+    0x01, 0x21,  1, 20, 50,
+    0x02, 0x21,  2, 20, 50,
+    0x03, 0x21,  3, 15, 50,
+    0x04, 0x21,  4, 15, 30,
+    0x05, 0x21,  5, 15, 30,
+    0x06, 0x21, 12, 15, 30,
+    0x07, 0x20, 20, 15, 30,
+    0x08, 0x20, 21,  8, 20,
+    0x09, 0x20, 22,  8, 20,
+    0x0a, 0x20, 23,  8, 25,
+    0x0b, 0x22, 23,  8, 25,
+};
+#endif // DOT11_N_SUPPORT //
+
+PUCHAR ReasonString[] = {
+	/* 0  */	 "Reserved",
+	/* 1  */	 "Unspecified Reason",
+	/* 2  */	 "Previous Auth no longer valid",
+	/* 3  */	 "STA is leaving / has left",
+	/* 4  */	 "DIS-ASSOC due to inactivity",
+	/* 5  */	 "AP unable to hanle all associations",
+	/* 6  */	 "class 2 error",
+	/* 7  */	 "class 3 error",
+	/* 8  */	 "STA is leaving / has left",
+	/* 9  */	 "require auth before assoc/re-assoc",
+	/* 10 */	 "Reserved",
+	/* 11 */	 "Reserved",
+	/* 12 */	 "Reserved",
+	/* 13 */	 "invalid IE",
+	/* 14 */	 "MIC error",
+	/* 15 */	 "4-way handshake timeout",
+	/* 16 */	 "2-way (group key) handshake timeout",
+	/* 17 */	 "4-way handshake IE diff among AssosReq/Rsp/Beacon",
+	/* 18 */
+};
+
+extern UCHAR	 OfdmRateToRxwiMCS[];
+// since RT61 has better RX sensibility, we have to limit TX ACK rate not to exceed our normal data TX rate.
+// otherwise the WLAN peer may not be able to receive the ACK thus downgrade its data TX rate
+ULONG BasicRateMask[12]				= {0xfffff001 /* 1-Mbps */, 0xfffff003 /* 2 Mbps */, 0xfffff007 /* 5.5 */, 0xfffff00f /* 11 */,
+									  0xfffff01f /* 6 */	 , 0xfffff03f /* 9 */	  , 0xfffff07f /* 12 */ , 0xfffff0ff /* 18 */,
+									  0xfffff1ff /* 24 */	 , 0xfffff3ff /* 36 */	  , 0xfffff7ff /* 48 */ , 0xffffffff /* 54 */};
+
+UCHAR MULTICAST_ADDR[MAC_ADDR_LEN] = {0x1,  0x00, 0x00, 0x00, 0x00, 0x00};
+UCHAR BROADCAST_ADDR[MAC_ADDR_LEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+UCHAR ZERO_MAC_ADDR[MAC_ADDR_LEN]  = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+
+// e.g. RssiSafeLevelForTxRate[RATE_36]" means if the current RSSI is greater than
+//		this value, then it's quaranteed capable of operating in 36 mbps TX rate in
+//		clean environment.
+//								  TxRate: 1   2   5.5	11	 6	  9    12	18	 24   36   48	54	 72  100
+CHAR RssiSafeLevelForTxRate[] ={  -92, -91, -90, -87, -88, -86, -85, -83, -81, -78, -72, -71, -40, -40 };
+
+UCHAR  RateIdToMbps[]	 = { 1, 2, 5, 11, 6, 9, 12, 18, 24, 36, 48, 54, 72, 100};
+USHORT RateIdTo500Kbps[] = { 2, 4, 11, 22, 12, 18, 24, 36, 48, 72, 96, 108, 144, 200};
+
+UCHAR  SsidIe	 = IE_SSID;
+UCHAR  SupRateIe = IE_SUPP_RATES;
+UCHAR  ExtRateIe = IE_EXT_SUPP_RATES;
+#ifdef DOT11_N_SUPPORT
+UCHAR  HtCapIe = IE_HT_CAP;
+UCHAR  AddHtInfoIe = IE_ADD_HT;
+UCHAR  NewExtChanIe = IE_SECONDARY_CH_OFFSET;
+#ifdef DOT11N_DRAFT3
+UCHAR  ExtHtCapIe = IE_EXT_CAPABILITY;
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+UCHAR  ErpIe	 = IE_ERP;
+UCHAR  DsIe 	 = IE_DS_PARM;
+UCHAR  TimIe	 = IE_TIM;
+UCHAR  WpaIe	 = IE_WPA;
+UCHAR  Wpa2Ie	 = IE_WPA2;
+UCHAR  IbssIe	 = IE_IBSS_PARM;
+UCHAR  Ccx2Ie	 = IE_CCX_V2;
+UCHAR  WapiIe	 = IE_WAPI;
+
+extern UCHAR	WPA_OUI[];
+
+UCHAR	SES_OUI[] = {0x00, 0x90, 0x4c};
+
+UCHAR	ZeroSsid[32] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+	0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
+
+// Reset the RFIC setting to new series
+RTMP_RF_REGS RF2850RegTable[] = {
+//		ch	 R1 		 R2 		 R3(TX0~4=0) R4
+		{1,  0x98402ecc, 0x984c0786, 0x9816b455, 0x9800510b},
+		{2,  0x98402ecc, 0x984c0786, 0x98168a55, 0x9800519f},
+		{3,  0x98402ecc, 0x984c078a, 0x98168a55, 0x9800518b},
+		{4,  0x98402ecc, 0x984c078a, 0x98168a55, 0x9800519f},
+		{5,  0x98402ecc, 0x984c078e, 0x98168a55, 0x9800518b},
+		{6,  0x98402ecc, 0x984c078e, 0x98168a55, 0x9800519f},
+		{7,  0x98402ecc, 0x984c0792, 0x98168a55, 0x9800518b},
+		{8,  0x98402ecc, 0x984c0792, 0x98168a55, 0x9800519f},
+		{9,  0x98402ecc, 0x984c0796, 0x98168a55, 0x9800518b},
+		{10, 0x98402ecc, 0x984c0796, 0x98168a55, 0x9800519f},
+		{11, 0x98402ecc, 0x984c079a, 0x98168a55, 0x9800518b},
+		{12, 0x98402ecc, 0x984c079a, 0x98168a55, 0x9800519f},
+		{13, 0x98402ecc, 0x984c079e, 0x98168a55, 0x9800518b},
+		{14, 0x98402ecc, 0x984c07a2, 0x98168a55, 0x98005193},
+
+		// 802.11 UNI / HyperLan 2
+		{36, 0x98402ecc, 0x984c099a, 0x98158a55, 0x980ed1a3},
+		{38, 0x98402ecc, 0x984c099e, 0x98158a55, 0x980ed193},
+		{40, 0x98402ec8, 0x984c0682, 0x98158a55, 0x980ed183},
+		{44, 0x98402ec8, 0x984c0682, 0x98158a55, 0x980ed1a3},
+		{46, 0x98402ec8, 0x984c0686, 0x98158a55, 0x980ed18b},
+		{48, 0x98402ec8, 0x984c0686, 0x98158a55, 0x980ed19b},
+		{52, 0x98402ec8, 0x984c068a, 0x98158a55, 0x980ed193},
+		{54, 0x98402ec8, 0x984c068a, 0x98158a55, 0x980ed1a3},
+		{56, 0x98402ec8, 0x984c068e, 0x98158a55, 0x980ed18b},
+		{60, 0x98402ec8, 0x984c0692, 0x98158a55, 0x980ed183},
+		{62, 0x98402ec8, 0x984c0692, 0x98158a55, 0x980ed193},
+		{64, 0x98402ec8, 0x984c0692, 0x98158a55, 0x980ed1a3}, // Plugfest#4, Day4, change RFR3 left4th 9->5.
+
+		// 802.11 HyperLan 2
+		{100, 0x98402ec8, 0x984c06b2, 0x98178a55, 0x980ed783},
+
+		// 2008.04.30 modified
+		// The system team has AN to improve the EVM value
+		// for channel 102 to 108 for the RT2850/RT2750 dual band solution.
+		{102, 0x98402ec8, 0x985c06b2, 0x98578a55, 0x980ed793},
+		{104, 0x98402ec8, 0x985c06b2, 0x98578a55, 0x980ed1a3},
+		{108, 0x98402ecc, 0x985c0a32, 0x98578a55, 0x980ed193},
+
+		{110, 0x98402ecc, 0x984c0a36, 0x98178a55, 0x980ed183},
+		{112, 0x98402ecc, 0x984c0a36, 0x98178a55, 0x980ed19b},
+		{116, 0x98402ecc, 0x984c0a3a, 0x98178a55, 0x980ed1a3},
+		{118, 0x98402ecc, 0x984c0a3e, 0x98178a55, 0x980ed193},
+		{120, 0x98402ec4, 0x984c0382, 0x98178a55, 0x980ed183},
+		{124, 0x98402ec4, 0x984c0382, 0x98178a55, 0x980ed193},
+		{126, 0x98402ec4, 0x984c0382, 0x98178a55, 0x980ed15b}, // 0x980ed1bb->0x980ed15b required by Rory 20070927
+		{128, 0x98402ec4, 0x984c0382, 0x98178a55, 0x980ed1a3},
+		{132, 0x98402ec4, 0x984c0386, 0x98178a55, 0x980ed18b},
+		{134, 0x98402ec4, 0x984c0386, 0x98178a55, 0x980ed193},
+		{136, 0x98402ec4, 0x984c0386, 0x98178a55, 0x980ed19b},
+		{140, 0x98402ec4, 0x984c038a, 0x98178a55, 0x980ed183},
+
+		// 802.11 UNII
+		{149, 0x98402ec4, 0x984c038a, 0x98178a55, 0x980ed1a7},
+		{151, 0x98402ec4, 0x984c038e, 0x98178a55, 0x980ed187},
+		{153, 0x98402ec4, 0x984c038e, 0x98178a55, 0x980ed18f},
+		{157, 0x98402ec4, 0x984c038e, 0x98178a55, 0x980ed19f},
+		{159, 0x98402ec4, 0x984c038e, 0x98178a55, 0x980ed1a7},
+		{161, 0x98402ec4, 0x984c0392, 0x98178a55, 0x980ed187},
+		{165, 0x98402ec4, 0x984c0392, 0x98178a55, 0x980ed197},
+
+		// Japan
+		{184, 0x95002ccc, 0x9500491e, 0x9509be55, 0x950c0a0b},
+		{188, 0x95002ccc, 0x95004922, 0x9509be55, 0x950c0a13},
+		{192, 0x95002ccc, 0x95004926, 0x9509be55, 0x950c0a1b},
+		{196, 0x95002ccc, 0x9500492a, 0x9509be55, 0x950c0a23},
+		{208, 0x95002ccc, 0x9500493a, 0x9509be55, 0x950c0a13},
+		{212, 0x95002ccc, 0x9500493e, 0x9509be55, 0x950c0a1b},
+		{216, 0x95002ccc, 0x95004982, 0x9509be55, 0x950c0a23},
+
+		// still lack of MMAC(Japan) ch 34,38,42,46
+};
+UCHAR	NUM_OF_2850_CHNL = (sizeof(RF2850RegTable) / sizeof(RTMP_RF_REGS));
+
+FREQUENCY_ITEM FreqItems3020[] =
+{
+	/**************************************************/
+	// ISM : 2.4 to 2.483 GHz                         //
+	/**************************************************/
+	// 11g
+	/**************************************************/
+	//-CH---N-------R---K-----------
+	{1,    241,  2,  2},
+	{2,    241,	 2,  7},
+	{3,    242,	 2,  2},
+	{4,    242,	 2,  7},
+	{5,    243,	 2,  2},
+	{6,    243,	 2,  7},
+	{7,    244,	 2,  2},
+	{8,    244,	 2,  7},
+	{9,    245,	 2,  2},
+	{10,   245,	 2,  7},
+	{11,   246,	 2,  2},
+	{12,   246,	 2,  7},
+	{13,   247,	 2,  2},
+	{14,   248,	 2,  4},
+};
+#define	NUM_OF_3020_CHNL	(sizeof(FreqItems3020) / sizeof(FREQUENCY_ITEM))
+
+/*
+	==========================================================================
+	Description:
+		initialize the MLME task and its data structure (queue, spinlock,
+		timer, state machines).
+
+	IRQL = PASSIVE_LEVEL
+
+	Return:
+		always return NDIS_STATUS_SUCCESS
+
+	==========================================================================
+*/
+NDIS_STATUS MlmeInit(
+	IN PRTMP_ADAPTER pAd)
+{
+	NDIS_STATUS Status = NDIS_STATUS_SUCCESS;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> MLME Initialize\n"));
+
+	do
+	{
+		Status = MlmeQueueInit(&pAd->Mlme.Queue);
+		if(Status != NDIS_STATUS_SUCCESS)
+			break;
+
+		pAd->Mlme.bRunning = FALSE;
+		NdisAllocateSpinLock(&pAd->Mlme.TaskLock);
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			BssTableInit(&pAd->ScanTab);
+
+			// init STA state machines
+			AssocStateMachineInit(pAd, &pAd->Mlme.AssocMachine, pAd->Mlme.AssocFunc);
+			AuthStateMachineInit(pAd, &pAd->Mlme.AuthMachine, pAd->Mlme.AuthFunc);
+			AuthRspStateMachineInit(pAd, &pAd->Mlme.AuthRspMachine, pAd->Mlme.AuthRspFunc);
+			SyncStateMachineInit(pAd, &pAd->Mlme.SyncMachine, pAd->Mlme.SyncFunc);
+			WpaPskStateMachineInit(pAd, &pAd->Mlme.WpaPskMachine, pAd->Mlme.WpaPskFunc);
+			AironetStateMachineInit(pAd, &pAd->Mlme.AironetMachine, pAd->Mlme.AironetFunc);
+
+#ifdef QOS_DLS_SUPPORT
+			DlsStateMachineInit(pAd, &pAd->Mlme.DlsMachine, pAd->Mlme.DlsFunc);
+#endif // QOS_DLS_SUPPORT //
+
+
+			// Since we are using switch/case to implement it, the init is different from the above
+			// state machine init
+			MlmeCntlInit(pAd, &pAd->Mlme.CntlMachine, NULL);
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+
+
+		ActionStateMachineInit(pAd, &pAd->Mlme.ActMachine, pAd->Mlme.ActFunc);
+
+		// Init mlme periodic timer
+		RTMPInitTimer(pAd, &pAd->Mlme.PeriodicTimer, GET_TIMER_FUNCTION(MlmePeriodicExec), pAd, TRUE);
+
+		// Set mlme periodic timer
+		RTMPSetTimer(&pAd->Mlme.PeriodicTimer, MLME_TASK_EXEC_INTV);
+
+		// software-based RX Antenna diversity
+		RTMPInitTimer(pAd, &pAd->Mlme.RxAntEvalTimer, GET_TIMER_FUNCTION(AsicRxAntEvalTimeout), pAd, FALSE);
+
+	} while (FALSE);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- MLME Initialize\n"));
+
+	return Status;
+}
+
+/*
+	==========================================================================
+	Description:
+		main loop of the MLME
+	Pre:
+		Mlme has to be initialized, and there are something inside the queue
+	Note:
+		This function is invoked from MPSetInformation and MPReceive;
+		This task guarantee only one MlmeHandler will run.
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID MlmeHandler(
+	IN PRTMP_ADAPTER pAd)
+{
+	MLME_QUEUE_ELEM 	   *Elem = NULL;
+#ifdef APCLI_SUPPORT
+	SHORT apcliIfIndex;
+#endif
+
+	// Only accept MLME and Frame from peer side, no other (control/data) frame should
+	// get into this state machine
+
+	NdisAcquireSpinLock(&pAd->Mlme.TaskLock);
+	if(pAd->Mlme.bRunning)
+	{
+		NdisReleaseSpinLock(&pAd->Mlme.TaskLock);
+		return;
+	}
+	else
+	{
+		pAd->Mlme.bRunning = TRUE;
+	}
+	NdisReleaseSpinLock(&pAd->Mlme.TaskLock);
+
+	while (!MlmeQueueEmpty(&pAd->Mlme.Queue))
+	{
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_MLME_RESET_IN_PROGRESS) ||
+			RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS) ||
+			RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("Device Halted or Removed or MlmeRest, exit MlmeHandler! (queue num = %ld)\n", pAd->Mlme.Queue.Num));
+			break;
+		}
+
+#ifdef RALINK_ATE
+		if(ATE_ON(pAd))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now in MlmeHandler\n"));
+			break;
+		}
+#endif // RALINK_ATE //
+
+		//From message type, determine which state machine I should drive
+		if (MlmeDequeue(&pAd->Mlme.Queue, &Elem))
+		{
+#ifdef RT2870
+			if (Elem->MsgType == MT2_RESET_CONF)
+			{
+				DBGPRINT_RAW(RT_DEBUG_TRACE, ("!!! reset MLME state machine !!!\n"));
+				MlmeRestartStateMachine(pAd);
+				Elem->Occupied = FALSE;
+				Elem->MsgLen = 0;
+				continue;
+			}
+#endif // RT2870 //
+
+			// if dequeue success
+			switch (Elem->Machine)
+			{
+				// STA state machines
+#ifdef	CONFIG_STA_SUPPORT
+				case ASSOC_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.AssocMachine, Elem);
+					break;
+				case AUTH_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.AuthMachine, Elem);
+					break;
+				case AUTH_RSP_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.AuthRspMachine, Elem);
+					break;
+				case SYNC_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.SyncMachine, Elem);
+					break;
+				case MLME_CNTL_STATE_MACHINE:
+					MlmeCntlMachinePerformAction(pAd, &pAd->Mlme.CntlMachine, Elem);
+					break;
+				case WPA_PSK_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.WpaPskMachine, Elem);
+					break;
+#ifdef LEAP_SUPPORT
+				case LEAP_STATE_MACHINE:
+					LeapMachinePerformAction(pAd, &pAd->Mlme.LeapMachine, Elem);
+					break;
+#endif
+				case AIRONET_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.AironetMachine, Elem);
+					break;
+
+#ifdef QOS_DLS_SUPPORT
+				case DLS_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.DlsMachine, Elem);
+					break;
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+				case ACTION_STATE_MACHINE:
+					StateMachinePerformAction(pAd, &pAd->Mlme.ActMachine, Elem);
+					break;
+
+
+
+
+				default:
+					DBGPRINT(RT_DEBUG_TRACE, ("ERROR: Illegal machine %ld in MlmeHandler()\n", Elem->Machine));
+					break;
+			} // end of switch
+
+			// free MLME element
+			Elem->Occupied = FALSE;
+			Elem->MsgLen = 0;
+
+		}
+		else {
+			DBGPRINT_ERR(("MlmeHandler: MlmeQueue empty\n"));
+		}
+	}
+
+	NdisAcquireSpinLock(&pAd->Mlme.TaskLock);
+	pAd->Mlme.bRunning = FALSE;
+	NdisReleaseSpinLock(&pAd->Mlme.TaskLock);
+}
+
+/*
+	==========================================================================
+	Description:
+		Destructor of MLME (Destroy queue, state machine, spin lock and timer)
+	Parameters:
+		Adapter - NIC Adapter pointer
+	Post:
+		The MLME task will no longer work properly
+
+	IRQL = PASSIVE_LEVEL
+
+	==========================================================================
+ */
+VOID MlmeHalt(
+	IN PRTMP_ADAPTER pAd)
+{
+	BOOLEAN 	  Cancelled;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("==> MlmeHalt\n"));
+
+	if (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST))
+	{
+		// disable BEACON generation and other BEACON related hardware timers
+		AsicDisableSync(pAd);
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+#ifdef QOS_DLS_SUPPORT
+		UCHAR		i;
+#endif // QOS_DLS_SUPPORT //
+		// Cancel pending timers
+		RTMPCancelTimer(&pAd->MlmeAux.AssocTimer,		&Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.ReassocTimer,		&Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.DisassocTimer,	&Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.AuthTimer,		&Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.BeaconTimer,		&Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.ScanTimer,		&Cancelled);
+
+#ifdef QOS_DLS_SUPPORT
+		for (i=0; i<MAX_NUM_OF_DLS_ENTRY; i++)
+		{
+			RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &Cancelled);
+		}
+#endif // QOS_DLS_SUPPORT //
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	RTMPCancelTimer(&pAd->Mlme.PeriodicTimer,		&Cancelled);
+	RTMPCancelTimer(&pAd->Mlme.RxAntEvalTimer,		&Cancelled);
+
+
+
+	if (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST))
+	{
+		// Set LED
+		RTMPSetLED(pAd, LED_HALT);
+        RTMPSetSignalLED(pAd, -100);	// Force signal strength Led to be turned off, firmware is not done it.
+#ifdef RT2870
+        {
+            LED_CFG_STRUC LedCfg;
+            RTMP_IO_READ32(pAd, LED_CFG, &LedCfg.word);
+            LedCfg.field.LedPolar = 0;
+            LedCfg.field.RLedMode = 0;
+            LedCfg.field.GLedMode = 0;
+            LedCfg.field.YLedMode = 0;
+            RTMP_IO_WRITE32(pAd, LED_CFG, LedCfg.word);
+        }
+#endif // RT2870 //
+	}
+
+	RTMPusecDelay(5000);    //  5 msec to gurantee Ant Diversity timer canceled
+
+	MlmeQueueDestroy(&pAd->Mlme.Queue);
+	NdisFreeSpinLock(&pAd->Mlme.TaskLock);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<== MlmeHalt\n"));
+}
+
+VOID MlmeResetRalinkCounters(
+	IN  PRTMP_ADAPTER   pAd)
+{
+	pAd->RalinkCounters.LastOneSecRxOkDataCnt = pAd->RalinkCounters.OneSecRxOkDataCnt;
+	// clear all OneSecxxx counters.
+	pAd->RalinkCounters.OneSecBeaconSentCnt = 0;
+	pAd->RalinkCounters.OneSecFalseCCACnt = 0;
+	pAd->RalinkCounters.OneSecRxFcsErrCnt = 0;
+	pAd->RalinkCounters.OneSecRxOkCnt = 0;
+	pAd->RalinkCounters.OneSecTxFailCount = 0;
+	pAd->RalinkCounters.OneSecTxNoRetryOkCount = 0;
+	pAd->RalinkCounters.OneSecTxRetryOkCount = 0;
+	pAd->RalinkCounters.OneSecRxOkDataCnt = 0;
+
+	// TODO: for debug only. to be removed
+	pAd->RalinkCounters.OneSecOsTxCount[QID_AC_BE] = 0;
+	pAd->RalinkCounters.OneSecOsTxCount[QID_AC_BK] = 0;
+	pAd->RalinkCounters.OneSecOsTxCount[QID_AC_VI] = 0;
+	pAd->RalinkCounters.OneSecOsTxCount[QID_AC_VO] = 0;
+	pAd->RalinkCounters.OneSecDmaDoneCount[QID_AC_BE] = 0;
+	pAd->RalinkCounters.OneSecDmaDoneCount[QID_AC_BK] = 0;
+	pAd->RalinkCounters.OneSecDmaDoneCount[QID_AC_VI] = 0;
+	pAd->RalinkCounters.OneSecDmaDoneCount[QID_AC_VO] = 0;
+	pAd->RalinkCounters.OneSecTxDoneCount = 0;
+	pAd->RalinkCounters.OneSecRxCount = 0;
+	pAd->RalinkCounters.OneSecTxAggregationCount = 0;
+	pAd->RalinkCounters.OneSecRxAggregationCount = 0;
+
+	return;
+}
+
+unsigned long rx_AMSDU;
+unsigned long rx_Total;
+
+/*
+	==========================================================================
+	Description:
+		This routine is executed periodically to -
+		1. Decide if it's a right time to turn on PwrMgmt bit of all
+		   outgoiing frames
+		2. Calculate ChannelQuality based on statistics of the last
+		   period, so that TX rate won't toggling very frequently between a
+		   successful TX and a failed TX.
+		3. If the calculated ChannelQuality indicated current connection not
+		   healthy, then a ROAMing attempt is tried here.
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+#define ADHOC_BEACON_LOST_TIME		(8*OS_HZ)  // 8 sec
+VOID MlmePeriodicExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+	ULONG			TxTotalCnt;
+	PRTMP_ADAPTER	pAd = (RTMP_ADAPTER *)FunctionContext;
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if ((RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_HALT_IN_PROGRESS |
+								fRTMP_ADAPTER_RADIO_OFF |
+								fRTMP_ADAPTER_RADIO_MEASUREMENT |
+								fRTMP_ADAPTER_RESET_IN_PROGRESS))))
+		return;
+
+	RT28XX_MLME_PRE_SANITY_CHECK(pAd);
+
+#ifdef RALINK_ATE
+	/* Do not show RSSI until "Normal 1 second Mlme PeriodicExec". */
+	if (ATE_ON(pAd))
+	{
+		if (pAd->Mlme.PeriodicRound % MLME_TASK_EXEC_MULTIPLE != (MLME_TASK_EXEC_MULTIPLE - 1))
+	{
+			pAd->Mlme.PeriodicRound ++;
+			return;
+		}
+	}
+#endif // RALINK_ATE //
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// Do nothing if monitor mode is on
+		if (MONITOR_ON(pAd))
+			return;
+
+		if (pAd->Mlme.PeriodicRound & 0x1)
+		{
+			// This is the fix for wifi 11n extension channel overlapping test case.  for 2860D
+			if (((pAd->MACVersion & 0xffff) == 0x0101) &&
+				(STA_TGN_WIFI_ON(pAd)) &&
+				(pAd->CommonCfg.IOTestParm.bToggle == FALSE))
+
+				{
+					RTMP_IO_WRITE32(pAd, TXOP_CTRL_CFG, 0x24Bf);
+					pAd->CommonCfg.IOTestParm.bToggle = TRUE;
+				}
+				else if ((STA_TGN_WIFI_ON(pAd)) &&
+						((pAd->MACVersion & 0xffff) == 0x0101))
+				{
+					RTMP_IO_WRITE32(pAd, TXOP_CTRL_CFG, 0x243f);
+					pAd->CommonCfg.IOTestParm.bToggle = FALSE;
+				}
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	pAd->bUpdateBcnCntDone = FALSE;
+
+//	RECBATimerTimeout(SystemSpecific1,FunctionContext,SystemSpecific2,SystemSpecific3);
+	pAd->Mlme.PeriodicRound ++;
+
+	// execute every 500ms
+	if ((pAd->Mlme.PeriodicRound % 5 == 0) && RTMPAutoRateSwitchCheck(pAd)/*(OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_TX_RATE_SWITCH_ENABLED))*/)
+	{
+#ifdef CONFIG_STA_SUPPORT
+		// perform dynamic tx rate switching based on past TX history
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			if ((OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED)
+					)
+				&& (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE)))
+				MlmeDynamicTxRateSwitching(pAd);
+		}
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+	// Normal 1 second Mlme PeriodicExec.
+	if (pAd->Mlme.PeriodicRound %MLME_TASK_EXEC_MULTIPLE == 0)
+	{
+                pAd->Mlme.OneSecPeriodicRound ++;
+
+#ifdef RALINK_ATE
+    	if (ATE_ON(pAd))
+    	{
+			/* request from Baron : move this routine from later to here */
+			/* for showing Rx error count in ATE RXFRAME */
+            NICUpdateRawCounters(pAd);
+			if (pAd->ate.bRxFer == 1)
+			{
+				pAd->ate.RxTotalCnt += pAd->ate.RxCntPerSec;
+			    ate_print(KERN_EMERG "MlmePeriodicExec: Rx packet cnt = %d/%d\n", pAd->ate.RxCntPerSec, pAd->ate.RxTotalCnt);
+				pAd->ate.RxCntPerSec = 0;
+
+				if (pAd->ate.RxAntennaSel == 0)
+					ate_print(KERN_EMERG "MlmePeriodicExec: Rx AvgRssi0=%d, AvgRssi1=%d, AvgRssi2=%d\n\n",
+						pAd->ate.AvgRssi0, pAd->ate.AvgRssi1, pAd->ate.AvgRssi2);
+				else
+					ate_print(KERN_EMERG "MlmePeriodicExec: Rx AvgRssi=%d\n\n", pAd->ate.AvgRssi0);
+			}
+			MlmeResetRalinkCounters(pAd);
+			return;
+    	}
+#endif // RALINK_ATE //
+
+
+		if (rx_Total)
+		{
+
+			// reset counters
+			rx_AMSDU = 0;
+			rx_Total = 0;
+		}
+
+		//ORIBATimerTimeout(pAd);
+
+		// Media status changed, report to NDIS
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_MEDIA_STATE_CHANGE))
+		{
+			RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_MEDIA_STATE_CHANGE);
+			if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+			{
+				pAd->IndicateMediaState = NdisMediaStateConnected;
+				RTMP_IndicateMediaState(pAd);
+
+			}
+			else
+			{
+				pAd->IndicateMediaState = NdisMediaStateDisconnected;
+				RTMP_IndicateMediaState(pAd);
+			}
+		}
+
+		NdisGetSystemUpTime(&pAd->Mlme.Now32);
+
+		// add the most up-to-date h/w raw counters into software variable, so that
+		// the dynamic tuning mechanism below are based on most up-to-date information
+		NICUpdateRawCounters(pAd);
+
+#ifdef RT2870
+		RT2870_WatchDog(pAd);
+#endif // RT2870 //
+
+#ifdef DOT11_N_SUPPORT
+   		// Need statistics after read counter. So put after NICUpdateRawCounters
+		ORIBATimerTimeout(pAd);
+#endif // DOT11_N_SUPPORT //
+
+		// if MGMT RING is full more than twice within 1 second, we consider there's
+		// a hardware problem stucking the TX path. In this case, try a hardware reset
+		// to recover the system
+	//	if (pAd->RalinkCounters.MgmtRingFullCount >= 2)
+	//		RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_HARDWARE_ERROR);
+	//	else
+	//		pAd->RalinkCounters.MgmtRingFullCount = 0;
+
+		// The time period for checking antenna is according to traffic
+		if (pAd->Mlme.bEnableAutoAntennaCheck)
+		{
+			TxTotalCnt = pAd->RalinkCounters.OneSecTxNoRetryOkCount +
+							 pAd->RalinkCounters.OneSecTxRetryOkCount +
+							 pAd->RalinkCounters.OneSecTxFailCount;
+
+			if (TxTotalCnt > 50)
+			{
+				if (pAd->Mlme.OneSecPeriodicRound % 10 == 0)
+				{
+					AsicEvaluateRxAnt(pAd);
+				}
+			}
+			else
+			{
+				if (pAd->Mlme.OneSecPeriodicRound % 3 == 0)
+				{
+					AsicEvaluateRxAnt(pAd);
+				}
+			}
+		}
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			STAMlmePeriodicExec(pAd);
+#endif // CONFIG_STA_SUPPORT //
+
+		MlmeResetRalinkCounters(pAd);
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			{
+				// When Adhoc beacon is enabled and RTS/CTS is enabled, there is a chance that hardware MAC FSM will run into a deadlock
+				// and sending CTS-to-self over and over.
+				// Software Patch Solution:
+				// 1. Polling debug state register 0x10F4 every one second.
+				// 2. If in 0x10F4 the ((bit29==1) && (bit7==1)) OR ((bit29==1) && (bit5==1)), it means the deadlock has occurred.
+				// 3. If the deadlock occurred, reset MAC/BBP by setting 0x1004 to 0x0001 for a while then setting it back to 0x000C again.
+
+				UINT32	MacReg = 0;
+
+				RTMP_IO_READ32(pAd, 0x10F4, &MacReg);
+				if (((MacReg & 0x20000000) && (MacReg & 0x80)) || ((MacReg & 0x20000000) && (MacReg & 0x20)))
+				{
+					RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x1);
+					RTMPusecDelay(1);
+					RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0xC);
+
+					DBGPRINT(RT_DEBUG_WARN,("Warning, MAC specific condition occurs \n"));
+				}
+			}
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+		RT28XX_MLME_HANDLER(pAd);
+	}
+
+
+	pAd->bUpdateBcnCntDone = FALSE;
+}
+
+#ifdef CONFIG_STA_SUPPORT
+VOID STAMlmePeriodicExec(
+	PRTMP_ADAPTER pAd)
+{
+	ULONG	TxTotalCnt;
+	int 	i;
+
+//
+// We return here in ATE mode, because the statistics
+// that ATE needs are not collected via this routine.
+//
+#ifdef RALINK_ATE
+	// It is supposed that we will never reach here in ATE mode.
+	ASSERT(!(ATE_ON(pAd)));
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+    if (pAd->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_DISABLE)
+#endif // WPA_SUPPLICANT_SUPPORT //
+    {
+    	// WPA MIC error should block association attempt for 60 seconds
+    	if (pAd->StaCfg.bBlockAssoc && (pAd->StaCfg.LastMicErrorTime + (60 * OS_HZ) < pAd->Mlme.Now32))
+    		pAd->StaCfg.bBlockAssoc = FALSE;
+    }
+
+    if ((pAd->PreMediaState != pAd->IndicateMediaState) && (pAd->CommonCfg.bWirelessEvent))
+	{
+		if (pAd->IndicateMediaState == NdisMediaStateConnected)
+		{
+			RTMPSendWirelessEvent(pAd, IW_STA_LINKUP_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+		}
+		pAd->PreMediaState = pAd->IndicateMediaState;
+	}
+
+
+
+
+   	AsicStaBbpTuning(pAd);
+
+	TxTotalCnt = pAd->RalinkCounters.OneSecTxNoRetryOkCount +
+					 pAd->RalinkCounters.OneSecTxRetryOkCount +
+					 pAd->RalinkCounters.OneSecTxFailCount;
+
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+	{
+		// update channel quality for Roaming and UI LinkQuality display
+		MlmeCalculateChannelQuality(pAd, pAd->Mlme.Now32);
+	}
+
+	// must be AFTER MlmeDynamicTxRateSwitching() because it needs to know if
+	// Radio is currently in noisy environment
+	if (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+		AsicAdjustTxPower(pAd);
+
+	if (INFRA_ON(pAd))
+	{
+#ifdef QOS_DLS_SUPPORT
+		// Check DLS time out, then tear down those session
+		RTMPCheckDLSTimeOut(pAd);
+#endif // QOS_DLS_SUPPORT //
+
+		// Is PSM bit consistent with user power management policy?
+		// This is the only place that will set PSM bit ON.
+		if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+		MlmeCheckPsmChange(pAd, pAd->Mlme.Now32);
+
+		pAd->RalinkCounters.LastOneSecTotalTxCount = TxTotalCnt;
+
+		if ((pAd->StaCfg.LastBeaconRxTime + 1*OS_HZ < pAd->Mlme.Now32) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS)) &&
+			((TxTotalCnt + pAd->RalinkCounters.OneSecRxOkCnt < 600)))
+		{
+			RTMPSetAGCInitValue(pAd, BW_20);
+			DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - No BEACON. restore R66 to the low bound(%d) \n", (0x2E + GET_LNA_GAIN(pAd))));
+		}
+
+        //if ((pAd->RalinkCounters.OneSecTxNoRetryOkCount == 0) &&
+        //    (pAd->RalinkCounters.OneSecTxRetryOkCount == 0))
+        {
+    		if (pAd->CommonCfg.bAPSDCapable && pAd->CommonCfg.APEdcaParm.bAPSDCapable)
+    		{
+    		    // When APSD is enabled, the period changes as 20 sec
+    			if ((pAd->Mlme.OneSecPeriodicRound % 20) == 8)
+    				RTMPSendNullFrame(pAd, pAd->CommonCfg.TxRate, TRUE);
+    		}
+    		else
+    		{
+    		    // Send out a NULL frame every 10 sec to inform AP that STA is still alive (Avoid being age out)
+    			if ((pAd->Mlme.OneSecPeriodicRound % 10) == 8)
+                {
+                    if (pAd->CommonCfg.bWmmCapable)
+    					RTMPSendNullFrame(pAd, pAd->CommonCfg.TxRate, TRUE);
+                    else
+						RTMPSendNullFrame(pAd, pAd->CommonCfg.TxRate, FALSE);
+                }
+    		}
+        }
+
+		if (CQI_IS_DEAD(pAd->Mlme.ChannelQuality))
+			{
+			DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - No BEACON. Dead CQI. Auto Recovery attempt #%ld\n", pAd->RalinkCounters.BadCQIAutoRecoveryCount));
+			pAd->StaCfg.CCXAdjacentAPReportFlag = TRUE;
+			pAd->StaCfg.CCXAdjacentAPLinkDownTime = pAd->StaCfg.LastBeaconRxTime;
+
+			// Lost AP, send disconnect & link down event
+			LinkDown(pAd, FALSE);
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+            if (pAd->StaCfg.WpaSupplicantUP)
+			{
+                union iwreq_data    wrqu;
+                //send disassociate event to wpa_supplicant
+                memset(&wrqu, 0, sizeof(wrqu));
+                wrqu.data.flags = RT_DISASSOC_EVENT_FLAG;
+                wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+            }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+            {
+                union iwreq_data    wrqu;
+                memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+                wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+            }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+
+			// RTMPPatchMacBbpBug(pAd);
+			MlmeAutoReconnectLastSSID(pAd);
+		}
+		else if (CQI_IS_BAD(pAd->Mlme.ChannelQuality))
+		{
+			pAd->RalinkCounters.BadCQIAutoRecoveryCount ++;
+			DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - Bad CQI. Auto Recovery attempt #%ld\n", pAd->RalinkCounters.BadCQIAutoRecoveryCount));
+			MlmeAutoReconnectLastSSID(pAd);
+		}
+
+		// Add auto seamless roaming
+		if (pAd->StaCfg.bFastRoaming)
+		{
+			SHORT	dBmToRoam = (SHORT)pAd->StaCfg.dBmToRoam;
+
+			DBGPRINT(RT_DEBUG_TRACE, ("Rssi=%d, dBmToRoam=%d\n", RTMPMaxRssi(pAd, pAd->StaCfg.RssiSample.LastRssi0, pAd->StaCfg.RssiSample.LastRssi1, pAd->StaCfg.RssiSample.LastRssi2), (CHAR)dBmToRoam));
+
+			if (RTMPMaxRssi(pAd, pAd->StaCfg.RssiSample.LastRssi0, pAd->StaCfg.RssiSample.LastRssi1, pAd->StaCfg.RssiSample.LastRssi2) <= (CHAR)dBmToRoam)
+			{
+				MlmeCheckForFastRoaming(pAd, pAd->Mlme.Now32);
+			}
+		}
+	}
+	else if (ADHOC_ON(pAd))
+	{
+		//radar detect
+		if ((pAd->CommonCfg.Channel > 14)
+			&& (pAd->CommonCfg.bIEEE80211H == 1)
+			&& RadarChannelCheck(pAd, pAd->CommonCfg.Channel))
+		{
+			RadarDetectPeriodic(pAd);
+		}
+
+		// If all peers leave, and this STA becomes the last one in this IBSS, then change MediaState
+		// to DISCONNECTED. But still holding this IBSS (i.e. sending BEACON) so that other STAs can
+		// join later.
+		if ((pAd->StaCfg.LastBeaconRxTime + ADHOC_BEACON_LOST_TIME < pAd->Mlme.Now32) &&
+			OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+		{
+			MLME_START_REQ_STRUCT     StartReq;
+
+			DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - excessive BEACON lost, last STA in this IBSS, MediaState=Disconnected\n"));
+			LinkDown(pAd, FALSE);
+
+			StartParmFill(pAd, &StartReq, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen);
+			MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_START_REQ, sizeof(MLME_START_REQ_STRUCT), &StartReq);
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_START;
+		}
+
+		for (i = 1; i < MAX_LEN_OF_MAC_TABLE; i++)
+		{
+			MAC_TABLE_ENTRY *pEntry = &pAd->MacTab.Content[i];
+
+			if (pEntry->ValidAsCLI == FALSE)
+				continue;
+
+			if (pEntry->LastBeaconRxTime + ADHOC_BEACON_LOST_TIME < pAd->Mlme.Now32)
+				MacTableDeleteEntry(pAd, pEntry->Aid, pEntry->Addr);
+		}
+	}
+	else // no INFRA nor ADHOC connection
+	{
+
+		if (pAd->StaCfg.bScanReqIsFromWebUI &&
+            ((pAd->StaCfg.LastScanTime + 30 * OS_HZ) > pAd->Mlme.Now32))
+			goto SKIP_AUTO_SCAN_CONN;
+        else
+            pAd->StaCfg.bScanReqIsFromWebUI = FALSE;
+
+		if ((pAd->StaCfg.bAutoReconnect == TRUE)
+			&& RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_START_UP)
+			&& (MlmeValidateSSID(pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.AutoReconnectSsidLen) == TRUE))
+		{
+			if ((pAd->ScanTab.BssNr==0) && (pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE))
+			{
+				MLME_SCAN_REQ_STRUCT	   ScanReq;
+
+				if ((pAd->StaCfg.LastScanTime + 10 * OS_HZ) < pAd->Mlme.Now32)
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("STAMlmePeriodicExec():CNTL - ScanTab.BssNr==0, start a new ACTIVE scan SSID[%s]\n", pAd->MlmeAux.AutoReconnectSsid));
+					ScanParmFill(pAd, &ScanReq, pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.AutoReconnectSsidLen, BSS_ANY, SCAN_ACTIVE);
+					MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_SCAN_REQ, sizeof(MLME_SCAN_REQ_STRUCT), &ScanReq);
+					pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_LIST_SCAN;
+					// Reset Missed scan number
+					pAd->StaCfg.LastScanTime = pAd->Mlme.Now32;
+				}
+				else if (pAd->StaCfg.BssType == BSS_ADHOC)	// Quit the forever scan when in a very clean room
+					MlmeAutoReconnectLastSSID(pAd);
+			}
+			else if (pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE)
+			{
+				if ((pAd->Mlme.OneSecPeriodicRound % 7) == 0)
+				{
+					MlmeAutoScan(pAd);
+					pAd->StaCfg.LastScanTime = pAd->Mlme.Now32;
+				}
+				else
+				{
+#ifdef CARRIER_DETECTION_SUPPORT // Roger sync Carrier
+					if (pAd->CommonCfg.CarrierDetect.Enable == TRUE)
+					{
+						if ((pAd->Mlme.OneSecPeriodicRound % 5) == 1)
+							MlmeAutoReconnectLastSSID(pAd);
+					}
+					else
+#endif // CARRIER_DETECTION_SUPPORT //
+						MlmeAutoReconnectLastSSID(pAd);
+				}
+			}
+		}
+	}
+
+SKIP_AUTO_SCAN_CONN:
+
+#ifdef DOT11_N_SUPPORT
+    if ((pAd->MacTab.Content[BSSID_WCID].TXBAbitmap !=0) && (pAd->MacTab.fAnyBASession == FALSE))
+	{
+		pAd->MacTab.fAnyBASession = TRUE;
+		AsicUpdateProtect(pAd, HT_FORCERTSCTS,  ALLN_SETPROTECT, FALSE, FALSE);
+	}
+	else if ((pAd->MacTab.Content[BSSID_WCID].TXBAbitmap ==0) && (pAd->MacTab.fAnyBASession == TRUE))
+	{
+		pAd->MacTab.fAnyBASession = FALSE;
+		AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode,  ALLN_SETPROTECT, FALSE, FALSE);
+	}
+#endif // DOT11_N_SUPPORT //
+
+
+#ifdef DOT11_N_SUPPORT
+#ifdef DOT11N_DRAFT3
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_SCAN_2040))
+		TriEventCounterMaintenance(pAd);
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+
+	return;
+}
+
+// Link down report
+VOID LinkDownExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+
+	RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+
+	pAd->IndicateMediaState = NdisMediaStateDisconnected;
+	RTMP_IndicateMediaState(pAd);
+    pAd->ExtraInfo = GENERAL_LINK_DOWN;
+}
+
+// IRQL = DISPATCH_LEVEL
+VOID MlmeAutoScan(
+	IN PRTMP_ADAPTER pAd)
+{
+	// check CntlMachine.CurrState to avoid collision with NDIS SetOID request
+	if (pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - Driver auto scan\n"));
+		MlmeEnqueue(pAd,
+					MLME_CNTL_STATE_MACHINE,
+					OID_802_11_BSSID_LIST_SCAN,
+					0,
+					NULL);
+		RT28XX_MLME_HANDLER(pAd);
+	}
+}
+
+// IRQL = DISPATCH_LEVEL
+VOID MlmeAutoReconnectLastSSID(
+	IN PRTMP_ADAPTER pAd)
+{
+
+
+	// check CntlMachine.CurrState to avoid collision with NDIS SetOID request
+	if ((pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE) &&
+		(MlmeValidateSSID(pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.AutoReconnectSsidLen) == TRUE))
+	{
+		NDIS_802_11_SSID OidSsid;
+		OidSsid.SsidLength = pAd->MlmeAux.AutoReconnectSsidLen;
+		NdisMoveMemory(OidSsid.Ssid, pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.AutoReconnectSsidLen);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("Driver auto reconnect to last OID_802_11_SSID setting - %s, len - %d\n", pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.AutoReconnectSsidLen));
+		MlmeEnqueue(pAd,
+					MLME_CNTL_STATE_MACHINE,
+					OID_802_11_SSID,
+					sizeof(NDIS_802_11_SSID),
+					&OidSsid);
+		RT28XX_MLME_HANDLER(pAd);
+	}
+}
+#endif // CONFIG_STA_SUPPORT //
+
+/*
+	==========================================================================
+	Validate SSID for connection try and rescan purpose
+	Valid SSID will have visible chars only.
+	The valid length is from 0 to 32.
+	IRQL = DISPATCH_LEVEL
+	==========================================================================
+ */
+BOOLEAN MlmeValidateSSID(
+	IN PUCHAR	pSsid,
+	IN UCHAR	SsidLen)
+{
+	int	index;
+
+	if (SsidLen > MAX_LEN_OF_SSID)
+		return (FALSE);
+
+	// Check each character value
+	for (index = 0; index < SsidLen; index++)
+	{
+		if (pSsid[index] < 0x20)
+			return (FALSE);
+	}
+
+	// All checked
+	return (TRUE);
+}
+
+VOID MlmeSelectTxRateTable(
+	IN PRTMP_ADAPTER		pAd,
+	IN PMAC_TABLE_ENTRY		pEntry,
+	IN PUCHAR				*ppTable,
+	IN PUCHAR				pTableSize,
+	IN PUCHAR				pInitTxRateIdx)
+{
+	do
+	{
+		// decide the rate table for tuning
+		if (pAd->CommonCfg.TxRateTableSize > 0)
+		{
+			*ppTable = RateSwitchTable;
+			*pTableSize = RateSwitchTable[0];
+			*pInitTxRateIdx = RateSwitchTable[1];
+
+			break;
+		}
+
+#ifdef CONFIG_STA_SUPPORT
+		if ((pAd->OpMode == OPMODE_STA) && ADHOC_ON(pAd))
+		{
+#ifdef DOT11_N_SUPPORT
+			if ((pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED) &&
+				(pEntry->HTCapability.MCSSet[0] == 0xff) &&
+				((pEntry->HTCapability.MCSSet[1] == 0x00) || (pAd->Antenna.field.TxPath == 1)))
+			{// 11N 1S Adhoc
+				*ppTable = RateSwitchTable11N1S;
+				*pTableSize = RateSwitchTable11N1S[0];
+				*pInitTxRateIdx = RateSwitchTable11N1S[1];
+
+			}
+			else if ((pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED) &&
+					(pEntry->HTCapability.MCSSet[0] == 0xff) &&
+					(pEntry->HTCapability.MCSSet[1] == 0xff) &&
+					(pAd->Antenna.field.TxPath == 2))
+			{// 11N 2S Adhoc
+				if (pAd->LatchRfRegs.Channel <= 14)
+				{
+					*ppTable = RateSwitchTable11N2S;
+					*pTableSize = RateSwitchTable11N2S[0];
+					*pInitTxRateIdx = RateSwitchTable11N2S[1];
+				}
+				else
+				{
+					*ppTable = RateSwitchTable11N2SForABand;
+					*pTableSize = RateSwitchTable11N2SForABand[0];
+					*pInitTxRateIdx = RateSwitchTable11N2SForABand[1];
+				}
+
+			}
+			else
+#endif // DOT11_N_SUPPORT //
+				if ((pEntry->RateLen == 4)
+#ifdef DOT11_N_SUPPORT
+					&& (pEntry->HTCapability.MCSSet[0] == 0) && (pEntry->HTCapability.MCSSet[1] == 0)
+#endif // DOT11_N_SUPPORT //
+					)
+			{
+				*ppTable = RateSwitchTable11B;
+				*pTableSize = RateSwitchTable11B[0];
+				*pInitTxRateIdx = RateSwitchTable11B[1];
+
+			}
+			else if (pAd->LatchRfRegs.Channel <= 14)
+			{
+				*ppTable = RateSwitchTable11BG;
+				*pTableSize = RateSwitchTable11BG[0];
+				*pInitTxRateIdx = RateSwitchTable11BG[1];
+
+			}
+			else
+			{
+				*ppTable = RateSwitchTable11G;
+				*pTableSize = RateSwitchTable11G[0];
+				*pInitTxRateIdx = RateSwitchTable11G[1];
+
+			}
+			break;
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef DOT11_N_SUPPORT
+		//if ((pAd->StaActive.SupRateLen + pAd->StaActive.ExtRateLen == 12) && (pAd->StaActive.SupportedPhyInfo.MCSSet[0] == 0xff) &&
+		//	((pAd->StaActive.SupportedPhyInfo.MCSSet[1] == 0x00) || (pAd->Antenna.field.TxPath == 1)))
+		if ((pEntry->RateLen == 12) && (pEntry->HTCapability.MCSSet[0] == 0xff) &&
+			((pEntry->HTCapability.MCSSet[1] == 0x00) || (pAd->CommonCfg.TxStream == 1)))
+		{// 11BGN 1S AP
+			*ppTable = RateSwitchTable11BGN1S;
+			*pTableSize = RateSwitchTable11BGN1S[0];
+			*pInitTxRateIdx = RateSwitchTable11BGN1S[1];
+
+			break;
+		}
+
+		//else if ((pAd->StaActive.SupRateLen + pAd->StaActive.ExtRateLen == 12) && (pAd->StaActive.SupportedPhyInfo.MCSSet[0] == 0xff) &&
+		//	(pAd->StaActive.SupportedPhyInfo.MCSSet[1] == 0xff) && (pAd->Antenna.field.TxPath == 2))
+		if ((pEntry->RateLen == 12) && (pEntry->HTCapability.MCSSet[0] == 0xff) &&
+			(pEntry->HTCapability.MCSSet[1] == 0xff) && (pAd->CommonCfg.TxStream == 2))
+		{// 11BGN 2S AP
+			if (pAd->LatchRfRegs.Channel <= 14)
+			{
+				*ppTable = RateSwitchTable11BGN2S;
+				*pTableSize = RateSwitchTable11BGN2S[0];
+				*pInitTxRateIdx = RateSwitchTable11BGN2S[1];
+
+			}
+			else
+			{
+				*ppTable = RateSwitchTable11BGN2SForABand;
+				*pTableSize = RateSwitchTable11BGN2SForABand[0];
+				*pInitTxRateIdx = RateSwitchTable11BGN2SForABand[1];
+
+			}
+			break;
+		}
+
+		//else if ((pAd->StaActive.SupportedPhyInfo.MCSSet[0] == 0xff) && ((pAd->StaActive.SupportedPhyInfo.MCSSet[1] == 0x00) || (pAd->Antenna.field.TxPath == 1)))
+		if ((pEntry->HTCapability.MCSSet[0] == 0xff) && ((pEntry->HTCapability.MCSSet[1] == 0x00) || (pAd->CommonCfg.TxStream == 1)))
+		{// 11N 1S AP
+			*ppTable = RateSwitchTable11N1S;
+			*pTableSize = RateSwitchTable11N1S[0];
+			*pInitTxRateIdx = RateSwitchTable11N1S[1];
+
+			break;
+		}
+
+		//else if ((pAd->StaActive.SupportedPhyInfo.MCSSet[0] == 0xff) && (pAd->StaActive.SupportedPhyInfo.MCSSet[1] == 0xff) && (pAd->Antenna.field.TxPath == 2))
+		if ((pEntry->HTCapability.MCSSet[0] == 0xff) && (pEntry->HTCapability.MCSSet[1] == 0xff) && (pAd->CommonCfg.TxStream == 2))
+		{// 11N 2S AP
+			if (pAd->LatchRfRegs.Channel <= 14)
+			{
+			*ppTable = RateSwitchTable11N2S;
+			*pTableSize = RateSwitchTable11N2S[0];
+			*pInitTxRateIdx = RateSwitchTable11N2S[1];
+            }
+			else
+			{
+				*ppTable = RateSwitchTable11N2SForABand;
+				*pTableSize = RateSwitchTable11N2SForABand[0];
+				*pInitTxRateIdx = RateSwitchTable11N2SForABand[1];
+			}
+
+			break;
+		}
+#endif // DOT11_N_SUPPORT //
+		//else if ((pAd->StaActive.SupRateLen == 4) && (pAd->StaActive.ExtRateLen == 0) && (pAd->StaActive.SupportedPhyInfo.MCSSet[0] == 0) && (pAd->StaActive.SupportedPhyInfo.MCSSet[1] == 0))
+		if ((pEntry->RateLen == 4)
+#ifdef DOT11_N_SUPPORT
+			&& (pEntry->HTCapability.MCSSet[0] == 0) && (pEntry->HTCapability.MCSSet[1] == 0)
+#endif // DOT11_N_SUPPORT //
+			)
+		{// B only AP
+			*ppTable = RateSwitchTable11B;
+			*pTableSize = RateSwitchTable11B[0];
+			*pInitTxRateIdx = RateSwitchTable11B[1];
+
+			break;
+		}
+
+		//else if ((pAd->StaActive.SupRateLen + pAd->StaActive.ExtRateLen > 8) && (pAd->StaActive.SupportedPhyInfo.MCSSet[0] == 0) && (pAd->StaActive.SupportedPhyInfo.MCSSet[1] == 0))
+		if ((pEntry->RateLen > 8)
+#ifdef DOT11_N_SUPPORT
+			&& (pEntry->HTCapability.MCSSet[0] == 0) && (pEntry->HTCapability.MCSSet[1] == 0)
+#endif // DOT11_N_SUPPORT //
+			)
+		{// B/G  mixed AP
+			*ppTable = RateSwitchTable11BG;
+			*pTableSize = RateSwitchTable11BG[0];
+			*pInitTxRateIdx = RateSwitchTable11BG[1];
+
+			break;
+		}
+
+		//else if ((pAd->StaActive.SupRateLen + pAd->StaActive.ExtRateLen == 8) && (pAd->StaActive.SupportedPhyInfo.MCSSet[0] == 0) && (pAd->StaActive.SupportedPhyInfo.MCSSet[1] == 0))
+		if ((pEntry->RateLen == 8)
+#ifdef DOT11_N_SUPPORT
+			&& (pEntry->HTCapability.MCSSet[0] == 0) && (pEntry->HTCapability.MCSSet[1] == 0)
+#endif // DOT11_N_SUPPORT //
+			)
+		{// G only AP
+			*ppTable = RateSwitchTable11G;
+			*pTableSize = RateSwitchTable11G[0];
+			*pInitTxRateIdx = RateSwitchTable11G[1];
+
+			break;
+		}
+#ifdef DOT11_N_SUPPORT
+#endif // DOT11_N_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+#ifdef DOT11_N_SUPPORT
+			//else if ((pAd->StaActive.SupportedPhyInfo.MCSSet[0] == 0) && (pAd->StaActive.SupportedPhyInfo.MCSSet[1] == 0))
+			if ((pEntry->HTCapability.MCSSet[0] == 0) && (pEntry->HTCapability.MCSSet[1] == 0))
+#endif // DOT11_N_SUPPORT //
+			{	// Legacy mode
+				if (pAd->CommonCfg.MaxTxRate <= RATE_11)
+				{
+					*ppTable = RateSwitchTable11B;
+					*pTableSize = RateSwitchTable11B[0];
+					*pInitTxRateIdx = RateSwitchTable11B[1];
+				}
+				else if ((pAd->CommonCfg.MaxTxRate > RATE_11) && (pAd->CommonCfg.MinTxRate > RATE_11))
+				{
+					*ppTable = RateSwitchTable11G;
+					*pTableSize = RateSwitchTable11G[0];
+					*pInitTxRateIdx = RateSwitchTable11G[1];
+
+				}
+				else
+				{
+					*ppTable = RateSwitchTable11BG;
+					*pTableSize = RateSwitchTable11BG[0];
+					*pInitTxRateIdx = RateSwitchTable11BG[1];
+				}
+				break;
+			}
+#ifdef DOT11_N_SUPPORT
+			if (pAd->LatchRfRegs.Channel <= 14)
+			{
+				if (pAd->CommonCfg.TxStream == 1)
+				{
+					*ppTable = RateSwitchTable11N1S;
+					*pTableSize = RateSwitchTable11N1S[0];
+					*pInitTxRateIdx = RateSwitchTable11N1S[1];
+					DBGPRINT_RAW(RT_DEBUG_ERROR,("DRS: unkown mode,default use 11N 1S AP \n"));
+				}
+				else
+				{
+					*ppTable = RateSwitchTable11N2S;
+					*pTableSize = RateSwitchTable11N2S[0];
+					*pInitTxRateIdx = RateSwitchTable11N2S[1];
+					DBGPRINT_RAW(RT_DEBUG_ERROR,("DRS: unkown mode,default use 11N 2S AP \n"));
+				}
+			}
+			else
+			{
+				if (pAd->CommonCfg.TxStream == 1)
+				{
+					*ppTable = RateSwitchTable11N1S;
+					*pTableSize = RateSwitchTable11N1S[0];
+					*pInitTxRateIdx = RateSwitchTable11N1S[1];
+					DBGPRINT_RAW(RT_DEBUG_ERROR,("DRS: unkown mode,default use 11N 1S AP \n"));
+				}
+				else
+				{
+					*ppTable = RateSwitchTable11N2SForABand;
+					*pTableSize = RateSwitchTable11N2SForABand[0];
+					*pInitTxRateIdx = RateSwitchTable11N2SForABand[1];
+					DBGPRINT_RAW(RT_DEBUG_ERROR,("DRS: unkown mode,default use 11N 2S AP \n"));
+				}
+			}
+#endif // DOT11_N_SUPPORT //
+			DBGPRINT_RAW(RT_DEBUG_ERROR,("DRS: unkown mode (SupRateLen=%d, ExtRateLen=%d, MCSSet[0]=0x%x, MCSSet[1]=0x%x)\n",
+				pAd->StaActive.SupRateLen, pAd->StaActive.ExtRateLen, pAd->StaActive.SupportedPhyInfo.MCSSet[0], pAd->StaActive.SupportedPhyInfo.MCSSet[1]));
+		}
+#endif // CONFIG_STA_SUPPORT //
+	} while(FALSE);
+}
+
+#ifdef CONFIG_STA_SUPPORT
+/*
+	==========================================================================
+	Description:
+		This routine checks if there're other APs out there capable for
+		roaming. Caller should call this routine only when Link up in INFRA mode
+		and channel quality is below CQI_GOOD_THRESHOLD.
+
+	IRQL = DISPATCH_LEVEL
+
+	Output:
+	==========================================================================
+ */
+VOID MlmeCheckForRoaming(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG	Now32)
+{
+	USHORT	   i;
+	BSS_TABLE  *pRoamTab = &pAd->MlmeAux.RoamTab;
+	BSS_ENTRY  *pBss;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("==> MlmeCheckForRoaming\n"));
+	// put all roaming candidates into RoamTab, and sort in RSSI order
+	BssTableInit(pRoamTab);
+	for (i = 0; i < pAd->ScanTab.BssNr; i++)
+	{
+		pBss = &pAd->ScanTab.BssEntry[i];
+
+		if ((pBss->LastBeaconRxTime + BEACON_LOST_TIME) < Now32)
+			continue;	 // AP disappear
+		if (pBss->Rssi <= RSSI_THRESHOLD_FOR_ROAMING)
+			continue;	 // RSSI too weak. forget it.
+		if (MAC_ADDR_EQUAL(pBss->Bssid, pAd->CommonCfg.Bssid))
+			continue;	 // skip current AP
+		if (pBss->Rssi < (pAd->StaCfg.RssiSample.LastRssi0 + RSSI_DELTA))
+			continue;	 // only AP with stronger RSSI is eligible for roaming
+
+		// AP passing all above rules is put into roaming candidate table
+		NdisMoveMemory(&pRoamTab->BssEntry[pRoamTab->BssNr], pBss, sizeof(BSS_ENTRY));
+		pRoamTab->BssNr += 1;
+	}
+
+	if (pRoamTab->BssNr > 0)
+	{
+		// check CntlMachine.CurrState to avoid collision with NDIS SetOID request
+		if (pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE)
+		{
+			pAd->RalinkCounters.PoorCQIRoamingCount ++;
+			DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - Roaming attempt #%ld\n", pAd->RalinkCounters.PoorCQIRoamingCount));
+			MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_MLME_ROAMING_REQ, 0, NULL);
+			RT28XX_MLME_HANDLER(pAd);
+		}
+	}
+	DBGPRINT(RT_DEBUG_TRACE, ("<== MlmeCheckForRoaming(# of candidate= %d)\n",pRoamTab->BssNr));
+}
+
+/*
+	==========================================================================
+	Description:
+		This routine checks if there're other APs out there capable for
+		roaming. Caller should call this routine only when link up in INFRA mode
+		and channel quality is below CQI_GOOD_THRESHOLD.
+
+	IRQL = DISPATCH_LEVEL
+
+	Output:
+	==========================================================================
+ */
+VOID MlmeCheckForFastRoaming(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	ULONG			Now)
+{
+	USHORT		i;
+	BSS_TABLE	*pRoamTab = &pAd->MlmeAux.RoamTab;
+	BSS_ENTRY	*pBss;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("==> MlmeCheckForFastRoaming\n"));
+	// put all roaming candidates into RoamTab, and sort in RSSI order
+	BssTableInit(pRoamTab);
+	for (i = 0; i < pAd->ScanTab.BssNr; i++)
+	{
+		pBss = &pAd->ScanTab.BssEntry[i];
+
+        if ((pBss->Rssi <= -50) && (pBss->Channel == pAd->CommonCfg.Channel))
+			continue;	 // RSSI too weak. forget it.
+		if (MAC_ADDR_EQUAL(pBss->Bssid, pAd->CommonCfg.Bssid))
+			continue;	 // skip current AP
+		if (!SSID_EQUAL(pBss->Ssid, pBss->SsidLen, pAd->CommonCfg.Ssid, pAd->CommonCfg.SsidLen))
+			continue;	 // skip different SSID
+        if (pBss->Rssi < (RTMPMaxRssi(pAd, pAd->StaCfg.RssiSample.LastRssi0, pAd->StaCfg.RssiSample.LastRssi1, pAd->StaCfg.RssiSample.LastRssi2) + RSSI_DELTA))
+			continue;	 // skip AP without better RSSI
+
+        DBGPRINT(RT_DEBUG_TRACE, ("LastRssi0 = %d, pBss->Rssi = %d\n", RTMPMaxRssi(pAd, pAd->StaCfg.RssiSample.LastRssi0, pAd->StaCfg.RssiSample.LastRssi1, pAd->StaCfg.RssiSample.LastRssi2), pBss->Rssi));
+		// AP passing all above rules is put into roaming candidate table
+		NdisMoveMemory(&pRoamTab->BssEntry[pRoamTab->BssNr], pBss, sizeof(BSS_ENTRY));
+		pRoamTab->BssNr += 1;
+	}
+
+	if (pRoamTab->BssNr > 0)
+	{
+		// check CntlMachine.CurrState to avoid collision with NDIS SetOID request
+		if (pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE)
+		{
+			pAd->RalinkCounters.PoorCQIRoamingCount ++;
+			DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - Roaming attempt #%ld\n", pAd->RalinkCounters.PoorCQIRoamingCount));
+			MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_MLME_ROAMING_REQ, 0, NULL);
+			RT28XX_MLME_HANDLER(pAd);
+		}
+	}
+	// Maybe site survey required
+	else
+	{
+		if ((pAd->StaCfg.LastScanTime + 10 * 1000) < Now)
+		{
+			// check CntlMachine.CurrState to avoid collision with NDIS SetOID request
+			DBGPRINT(RT_DEBUG_TRACE, ("MMCHK - Roaming, No eligable entry, try new scan!\n"));
+			pAd->StaCfg.ScanCnt = 2;
+			pAd->StaCfg.LastScanTime = Now;
+			MlmeAutoScan(pAd);
+		}
+	}
+
+    DBGPRINT(RT_DEBUG_TRACE, ("<== MlmeCheckForFastRoaming (BssNr=%d)\n", pRoamTab->BssNr));
+}
+
+/*
+	==========================================================================
+	Description:
+		This routine calculates TxPER, RxPER of the past N-sec period. And
+		according to the calculation result, ChannelQuality is calculated here
+		to decide if current AP is still doing the job.
+
+		If ChannelQuality is not good, a ROAMing attempt may be tried later.
+	Output:
+		StaCfg.ChannelQuality - 0..100
+
+	IRQL = DISPATCH_LEVEL
+
+	NOTE: This routine decide channle quality based on RX CRC error ratio.
+		Caller should make sure a function call to NICUpdateRawCounters(pAd)
+		is performed right before this routine, so that this routine can decide
+		channel quality based on the most up-to-date information
+	==========================================================================
+ */
+VOID MlmeCalculateChannelQuality(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG Now32)
+{
+	ULONG TxOkCnt, TxCnt, TxPER, TxPRR;
+	ULONG RxCnt, RxPER;
+	UCHAR NorRssi;
+	CHAR  MaxRssi;
+	ULONG BeaconLostTime = BEACON_LOST_TIME;
+
+#ifdef CARRIER_DETECTION_SUPPORT // Roger sync Carrier
+	// longer beacon lost time when carrier detection enabled
+	if (pAd->CommonCfg.CarrierDetect.Enable == TRUE)
+	{
+		BeaconLostTime = BEACON_LOST_TIME + BEACON_LOST_TIME/2;
+	}
+#endif // CARRIER_DETECTION_SUPPORT //
+
+	MaxRssi = RTMPMaxRssi(pAd, pAd->StaCfg.RssiSample.LastRssi0, pAd->StaCfg.RssiSample.LastRssi1, pAd->StaCfg.RssiSample.LastRssi2);
+
+	//
+	// calculate TX packet error ratio and TX retry ratio - if too few TX samples, skip TX related statistics
+	//
+	TxOkCnt = pAd->RalinkCounters.OneSecTxNoRetryOkCount + pAd->RalinkCounters.OneSecTxRetryOkCount;
+	TxCnt = TxOkCnt + pAd->RalinkCounters.OneSecTxFailCount;
+	if (TxCnt < 5)
+	{
+		TxPER = 0;
+		TxPRR = 0;
+	}
+	else
+	{
+		TxPER = (pAd->RalinkCounters.OneSecTxFailCount * 100) / TxCnt;
+		TxPRR = ((TxCnt - pAd->RalinkCounters.OneSecTxNoRetryOkCount) * 100) / TxCnt;
+	}
+
+	//
+	// calculate RX PER - don't take RxPER into consideration if too few sample
+	//
+	RxCnt = pAd->RalinkCounters.OneSecRxOkCnt + pAd->RalinkCounters.OneSecRxFcsErrCnt;
+	if (RxCnt < 5)
+		RxPER = 0;
+	else
+		RxPER = (pAd->RalinkCounters.OneSecRxFcsErrCnt * 100) / RxCnt;
+
+	//
+	// decide ChannelQuality based on: 1)last BEACON received time, 2)last RSSI, 3)TxPER, and 4)RxPER
+	//
+	if (INFRA_ON(pAd) &&
+		(pAd->RalinkCounters.OneSecTxNoRetryOkCount < 2) && // no heavy traffic
+		(pAd->StaCfg.LastBeaconRxTime + BeaconLostTime < Now32))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("BEACON lost > %ld msec with TxOkCnt=%ld -> CQI=0\n", BeaconLostTime, TxOkCnt));
+		pAd->Mlme.ChannelQuality = 0;
+	}
+	else
+	{
+		// Normalize Rssi
+		if (MaxRssi > -40)
+			NorRssi = 100;
+		else if (MaxRssi < -90)
+			NorRssi = 0;
+		else
+			NorRssi = (MaxRssi + 90) * 2;
+
+		// ChannelQuality = W1*RSSI + W2*TxPRR + W3*RxPER	 (RSSI 0..100), (TxPER 100..0), (RxPER 100..0)
+		pAd->Mlme.ChannelQuality = (RSSI_WEIGHTING * NorRssi +
+								   TX_WEIGHTING * (100 - TxPRR) +
+								   RX_WEIGHTING* (100 - RxPER)) / 100;
+		if (pAd->Mlme.ChannelQuality >= 100)
+			pAd->Mlme.ChannelQuality = 100;
+	}
+
+}
+
+VOID MlmeSetTxRate(
+	IN PRTMP_ADAPTER		pAd,
+	IN PMAC_TABLE_ENTRY		pEntry,
+	IN PRTMP_TX_RATE_SWITCH	pTxRate)
+{
+	UCHAR	MaxMode = MODE_OFDM;
+
+#ifdef DOT11_N_SUPPORT
+	MaxMode = MODE_HTGREENFIELD;
+
+	if (pTxRate->STBC && (pAd->StaCfg.MaxHTPhyMode.field.STBC) && (pAd->Antenna.field.TxPath == 2))
+		pAd->StaCfg.HTPhyMode.field.STBC = STBC_USE;
+	else
+#endif // DOT11_N_SUPPORT //
+		pAd->StaCfg.HTPhyMode.field.STBC = STBC_NONE;
+
+	if (pTxRate->CurrMCS < MCS_AUTO)
+		pAd->StaCfg.HTPhyMode.field.MCS = pTxRate->CurrMCS;
+
+	if (pAd->StaCfg.HTPhyMode.field.MCS > 7)
+		pAd->StaCfg.HTPhyMode.field.STBC = STBC_NONE;
+
+   	if (ADHOC_ON(pAd))
+	{
+		// If peer adhoc is b-only mode, we can't send 11g rate.
+		pAd->StaCfg.HTPhyMode.field.ShortGI = GI_800;
+		pEntry->HTPhyMode.field.STBC	= STBC_NONE;
+
+		//
+		// For Adhoc MODE_CCK, driver will use AdhocBOnlyJoined flag to roll back to B only if necessary
+		//
+		pEntry->HTPhyMode.field.MODE	= pTxRate->Mode;
+		pEntry->HTPhyMode.field.ShortGI	= pAd->StaCfg.HTPhyMode.field.ShortGI;
+		pEntry->HTPhyMode.field.MCS		= pAd->StaCfg.HTPhyMode.field.MCS;
+
+		// Patch speed error in status page
+		pAd->StaCfg.HTPhyMode.field.MODE = pEntry->HTPhyMode.field.MODE;
+	}
+	else
+	{
+		if (pTxRate->Mode <= MaxMode)
+			pAd->StaCfg.HTPhyMode.field.MODE = pTxRate->Mode;
+
+#ifdef DOT11_N_SUPPORT
+		if (pTxRate->ShortGI && (pAd->StaCfg.MaxHTPhyMode.field.ShortGI))
+			pAd->StaCfg.HTPhyMode.field.ShortGI = GI_400;
+		else
+#endif // DOT11_N_SUPPORT //
+			pAd->StaCfg.HTPhyMode.field.ShortGI = GI_800;
+
+#ifdef DOT11_N_SUPPORT
+		// Reexam each bandwidth's SGI support.
+		if (pAd->StaCfg.HTPhyMode.field.ShortGI == GI_400)
+		{
+			if ((pEntry->HTPhyMode.field.BW == BW_20) && (!CLIENT_STATUS_TEST_FLAG(pEntry, fCLIENT_STATUS_SGI20_CAPABLE)))
+				pAd->StaCfg.HTPhyMode.field.ShortGI = GI_800;
+			if ((pEntry->HTPhyMode.field.BW == BW_40) && (!CLIENT_STATUS_TEST_FLAG(pEntry, fCLIENT_STATUS_SGI40_CAPABLE)))
+				pAd->StaCfg.HTPhyMode.field.ShortGI = GI_800;
+		}
+
+		// Turn RTS/CTS rate to 6Mbps.
+		if ((pEntry->HTPhyMode.field.MCS == 0) && (pAd->StaCfg.HTPhyMode.field.MCS != 0))
+		{
+			pEntry->HTPhyMode.field.MCS		= pAd->StaCfg.HTPhyMode.field.MCS;
+			if (pAd->MacTab.fAnyBASession)
+			{
+				AsicUpdateProtect(pAd, HT_FORCERTSCTS, ALLN_SETPROTECT, TRUE, (BOOLEAN)pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent);
+			}
+			else
+			{
+				AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode, ALLN_SETPROTECT, TRUE, (BOOLEAN)pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent);
+			}
+		}
+		else if ((pEntry->HTPhyMode.field.MCS == 8) && (pAd->StaCfg.HTPhyMode.field.MCS != 8))
+		{
+			pEntry->HTPhyMode.field.MCS		= pAd->StaCfg.HTPhyMode.field.MCS;
+			if (pAd->MacTab.fAnyBASession)
+			{
+				AsicUpdateProtect(pAd, HT_FORCERTSCTS, ALLN_SETPROTECT, TRUE, (BOOLEAN)pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent);
+			}
+			else
+			{
+				AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode, ALLN_SETPROTECT, TRUE, (BOOLEAN)pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent);
+			}
+		}
+		else if ((pEntry->HTPhyMode.field.MCS != 0) && (pAd->StaCfg.HTPhyMode.field.MCS == 0))
+		{
+			AsicUpdateProtect(pAd, HT_RTSCTS_6M, ALLN_SETPROTECT, TRUE, (BOOLEAN)pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent);
+
+		}
+		else if ((pEntry->HTPhyMode.field.MCS != 8) && (pAd->StaCfg.HTPhyMode.field.MCS == 8))
+		{
+			AsicUpdateProtect(pAd, HT_RTSCTS_6M, ALLN_SETPROTECT, TRUE, (BOOLEAN)pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent);
+		}
+#endif // DOT11_N_SUPPORT //
+
+		pEntry->HTPhyMode.field.STBC	= pAd->StaCfg.HTPhyMode.field.STBC;
+		pEntry->HTPhyMode.field.ShortGI	= pAd->StaCfg.HTPhyMode.field.ShortGI;
+		pEntry->HTPhyMode.field.MCS		= pAd->StaCfg.HTPhyMode.field.MCS;
+		pEntry->HTPhyMode.field.MODE	= pAd->StaCfg.HTPhyMode.field.MODE;
+#ifdef DOT11_N_SUPPORT
+		if ((pAd->StaCfg.MaxHTPhyMode.field.MODE == MODE_HTGREENFIELD) &&
+		    pAd->WIFItestbed.bGreenField)
+		    pEntry->HTPhyMode.field.MODE = MODE_HTGREENFIELD;
+#endif // DOT11_N_SUPPORT //
+	}
+
+	pAd->LastTxRate = (USHORT)(pEntry->HTPhyMode.word);
+}
+
+/*
+	==========================================================================
+	Description:
+		This routine calculates the acumulated TxPER of eaxh TxRate. And
+		according to the calculation result, change CommonCfg.TxRate which
+		is the stable TX Rate we expect the Radio situation could sustained.
+
+		CommonCfg.TxRate will change dynamically within {RATE_1/RATE_6, MaxTxRate}
+	Output:
+		CommonCfg.TxRate -
+
+	IRQL = DISPATCH_LEVEL
+
+	NOTE:
+		call this routine every second
+	==========================================================================
+ */
+VOID MlmeDynamicTxRateSwitching(
+	IN PRTMP_ADAPTER pAd)
+{
+	UCHAR					UpRateIdx = 0, DownRateIdx = 0, CurrRateIdx;
+	ULONG					i, AccuTxTotalCnt = 0, TxTotalCnt;
+	ULONG					TxErrorRatio = 0;
+	BOOLEAN					bTxRateChanged, bUpgradeQuality = FALSE;
+	PRTMP_TX_RATE_SWITCH	pCurrTxRate, pNextTxRate = NULL;
+	PUCHAR					pTable;
+	UCHAR					TableSize = 0;
+	UCHAR					InitTxRateIdx = 0, TrainUp, TrainDown;
+	CHAR					Rssi, RssiOffset = 0;
+	TX_STA_CNT1_STRUC		StaTx1;
+	TX_STA_CNT0_STRUC		TxStaCnt0;
+	ULONG					TxRetransmit = 0, TxSuccess = 0, TxFailCount = 0;
+	MAC_TABLE_ENTRY			*pEntry;
+
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+	{
+		return;
+	}
+#endif // RALINK_ATE //
+
+	//
+	// walk through MAC table, see if need to change AP's TX rate toward each entry
+	//
+   	for (i = 1; i < MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		pEntry = &pAd->MacTab.Content[i];
+
+		// check if this entry need to switch rate automatically
+		if (RTMPCheckEntryEnableAutoRateSwitch(pAd, pEntry) == FALSE)
+			continue;
+
+		if ((pAd->MacTab.Size == 1) || (pEntry->ValidAsDls))
+		{
+			Rssi = RTMPMaxRssi(pAd,
+							   pAd->StaCfg.RssiSample.AvgRssi0,
+							   pAd->StaCfg.RssiSample.AvgRssi1,
+							   pAd->StaCfg.RssiSample.AvgRssi2);
+
+			// Update statistic counter
+			RTMP_IO_READ32(pAd, TX_STA_CNT0, &TxStaCnt0.word);
+			RTMP_IO_READ32(pAd, TX_STA_CNT1, &StaTx1.word);
+			pAd->bUpdateBcnCntDone = TRUE;
+			TxRetransmit = StaTx1.field.TxRetransmit;
+			TxSuccess = StaTx1.field.TxSuccess;
+			TxFailCount = TxStaCnt0.field.TxFailCount;
+			TxTotalCnt = TxRetransmit + TxSuccess + TxFailCount;
+
+			pAd->RalinkCounters.OneSecTxRetryOkCount += StaTx1.field.TxRetransmit;
+			pAd->RalinkCounters.OneSecTxNoRetryOkCount += StaTx1.field.TxSuccess;
+			pAd->RalinkCounters.OneSecTxFailCount += TxStaCnt0.field.TxFailCount;
+			pAd->WlanCounters.TransmittedFragmentCount.u.LowPart += StaTx1.field.TxSuccess;
+			pAd->WlanCounters.RetryCount.u.LowPart += StaTx1.field.TxRetransmit;
+			pAd->WlanCounters.FailedCount.u.LowPart += TxStaCnt0.field.TxFailCount;
+
+			// if no traffic in the past 1-sec period, don't change TX rate,
+			// but clear all bad history. because the bad history may affect the next
+			// Chariot throughput test
+			AccuTxTotalCnt = pAd->RalinkCounters.OneSecTxNoRetryOkCount +
+						 pAd->RalinkCounters.OneSecTxRetryOkCount +
+						 pAd->RalinkCounters.OneSecTxFailCount;
+
+			if (TxTotalCnt)
+				TxErrorRatio = ((TxRetransmit + TxFailCount) * 100) / TxTotalCnt;
+		}
+		else
+		{
+			if (INFRA_ON(pAd) && (i == 1))
+				Rssi = RTMPMaxRssi(pAd,
+								   pAd->StaCfg.RssiSample.AvgRssi0,
+								   pAd->StaCfg.RssiSample.AvgRssi1,
+								   pAd->StaCfg.RssiSample.AvgRssi2);
+			else
+				Rssi = RTMPMaxRssi(pAd,
+								   pEntry->RssiSample.AvgRssi0,
+								   pEntry->RssiSample.AvgRssi1,
+								   pEntry->RssiSample.AvgRssi2);
+
+			TxTotalCnt = pEntry->OneSecTxNoRetryOkCount +
+				 pEntry->OneSecTxRetryOkCount +
+				 pEntry->OneSecTxFailCount;
+
+			if (TxTotalCnt)
+				TxErrorRatio = ((pEntry->OneSecTxRetryOkCount + pEntry->OneSecTxFailCount) * 100) / TxTotalCnt;
+		}
+
+		CurrRateIdx = pEntry->CurrTxRateIndex;
+
+		MlmeSelectTxRateTable(pAd, pEntry, &pTable, &TableSize, &InitTxRateIdx);
+
+		if (CurrRateIdx >= TableSize)
+		{
+			CurrRateIdx = TableSize - 1;
+		}
+
+		// When switch from Fixed rate -> auto rate, the REAL TX rate might be different from pAd->CommonCfg.TxRateIndex.
+		// So need to sync here.
+		pCurrTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(CurrRateIdx+1)*5];
+		if ((pEntry->HTPhyMode.field.MCS != pCurrTxRate->CurrMCS)
+			//&& (pAd->StaCfg.bAutoTxRateSwitch == TRUE)
+			)
+		{
+
+			// Need to sync Real Tx rate and our record.
+			// Then return for next DRS.
+			pCurrTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(InitTxRateIdx+1)*5];
+			pEntry->CurrTxRateIndex = InitTxRateIdx;
+			MlmeSetTxRate(pAd, pEntry, pCurrTxRate);
+
+			// reset all OneSecTx counters
+			RESET_ONE_SEC_TX_CNT(pEntry);
+			continue;
+		}
+
+		// decide the next upgrade rate and downgrade rate, if any
+		if ((CurrRateIdx > 0) && (CurrRateIdx < (TableSize - 1)))
+		{
+			UpRateIdx = CurrRateIdx + 1;
+			DownRateIdx = CurrRateIdx -1;
+		}
+		else if (CurrRateIdx == 0)
+		{
+			UpRateIdx = CurrRateIdx + 1;
+			DownRateIdx = CurrRateIdx;
+		}
+		else if (CurrRateIdx == (TableSize - 1))
+		{
+			UpRateIdx = CurrRateIdx;
+			DownRateIdx = CurrRateIdx - 1;
+		}
+
+		pCurrTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(CurrRateIdx+1)*5];
+
+#ifdef DOT11_N_SUPPORT
+		if ((Rssi > -65) && (pCurrTxRate->Mode >= MODE_HTMIX))
+		{
+			TrainUp		= (pCurrTxRate->TrainUp + (pCurrTxRate->TrainUp >> 1));
+			TrainDown	= (pCurrTxRate->TrainDown + (pCurrTxRate->TrainDown >> 1));
+		}
+		else
+#endif // DOT11_N_SUPPORT //
+		{
+			TrainUp		= pCurrTxRate->TrainUp;
+			TrainDown	= pCurrTxRate->TrainDown;
+		}
+
+		//pAd->DrsCounters.LastTimeTxRateChangeAction = pAd->DrsCounters.LastSecTxRateChangeAction;
+
+		//
+		// Keep the last time TxRateChangeAction status.
+		//
+		pEntry->LastTimeTxRateChangeAction = pEntry->LastSecTxRateChangeAction;
+
+
+
+		//
+		// CASE 1. when TX samples are fewer than 15, then decide TX rate solely on RSSI
+		//         (criteria copied from RT2500 for Netopia case)
+		//
+		if (TxTotalCnt <= 15)
+		{
+			CHAR	idx = 0;
+			UCHAR	TxRateIdx;
+			//UCHAR	MCS0 = 0, MCS1 = 0, MCS2 = 0, MCS3 = 0, MCS4 = 0, MCS7 = 0, MCS12 = 0, MCS13 = 0, MCS14 = 0, MCS15 = 0;
+			UCHAR	MCS0 = 0, MCS1 = 0, MCS2 = 0, MCS3 = 0, MCS4 = 0,  MCS5 =0, MCS6 = 0, MCS7 = 0;
+	        UCHAR	MCS12 = 0, MCS13 = 0, MCS14 = 0, MCS15 = 0;
+			UCHAR	MCS20 = 0, MCS21 = 0, MCS22 = 0, MCS23 = 0; // 3*3
+
+			// check the existence and index of each needed MCS
+			while (idx < pTable[0])
+			{
+				pCurrTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(idx+1)*5];
+
+				if (pCurrTxRate->CurrMCS == MCS_0)
+				{
+					MCS0 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_1)
+				{
+					MCS1 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_2)
+				{
+					MCS2 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_3)
+				{
+					MCS3 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_4)
+				{
+					MCS4 = idx;
+				}
+	            else if (pCurrTxRate->CurrMCS == MCS_5)
+	            {
+	                MCS5 = idx;
+	            }
+	            else if (pCurrTxRate->CurrMCS == MCS_6)
+	            {
+	                MCS6 = idx;
+	            }
+				//else if (pCurrTxRate->CurrMCS == MCS_7)
+				else if ((pCurrTxRate->CurrMCS == MCS_7) && (pCurrTxRate->ShortGI == GI_800))	// prevent the highest MCS using short GI when 1T and low throughput
+				{
+					MCS7 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_12)
+				{
+					MCS12 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_13)
+				{
+					MCS13 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_14)
+				{
+					MCS14 = idx;
+				}
+				//else if ((pCurrTxRate->CurrMCS == MCS_15)/* && (pCurrTxRate->ShortGI == GI_800)*/)	//we hope to use ShortGI as initial rate
+				else if ((pCurrTxRate->CurrMCS == MCS_15) && (pCurrTxRate->ShortGI == GI_800))	//we hope to use ShortGI as initial rate, however Atheros's chip has bugs when short GI
+				{
+					MCS15 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_20) // 3*3
+				{
+					MCS20 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_21)
+				{
+					MCS21 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_22)
+				{
+					MCS22 = idx;
+				}
+				else if (pCurrTxRate->CurrMCS == MCS_23)
+				{
+					MCS23 = idx;
+				}
+				idx ++;
+			}
+
+			if (pAd->LatchRfRegs.Channel <= 14)
+			{
+				if (pAd->NicConfig2.field.ExternalLNAForG)
+				{
+					RssiOffset = 2;
+				}
+				else
+				{
+					RssiOffset = 5;
+				}
+			}
+			else
+			{
+				if (pAd->NicConfig2.field.ExternalLNAForA)
+				{
+					RssiOffset = 5;
+				}
+				else
+				{
+					RssiOffset = 8;
+				}
+			}
+#ifdef DOT11_N_SUPPORT
+			/*if (MCS15)*/
+			if ((pTable == RateSwitchTable11BGN3S) ||
+				(pTable == RateSwitchTable11N3S) ||
+				(pTable == RateSwitchTable))
+			{// N mode with 3 stream // 3*3
+				if (MCS23 && (Rssi >= -70))
+					TxRateIdx = MCS15;
+				else if (MCS22 && (Rssi >= -72))
+					TxRateIdx = MCS14;
+        	    else if (MCS21 && (Rssi >= -76))
+					TxRateIdx = MCS13;
+				else if (MCS20 && (Rssi >= -78))
+					TxRateIdx = MCS12;
+			else if (MCS4 && (Rssi >= -82))
+				TxRateIdx = MCS4;
+			else if (MCS3 && (Rssi >= -84))
+				TxRateIdx = MCS3;
+			else if (MCS2 && (Rssi >= -86))
+				TxRateIdx = MCS2;
+			else if (MCS1 && (Rssi >= -88))
+				TxRateIdx = MCS1;
+			else
+				TxRateIdx = MCS0;
+		}
+//		else if ((pTable == RateSwitchTable11BGN2S) || (pTable == RateSwitchTable11BGN2SForABand) ||(pTable == RateSwitchTable11N2S) ||(pTable == RateSwitchTable11N2SForABand) || (pTable == RateSwitchTable))
+		else if ((pTable == RateSwitchTable11BGN2S) || (pTable == RateSwitchTable11BGN2SForABand) ||(pTable == RateSwitchTable11N2S) ||(pTable == RateSwitchTable11N2SForABand)) // 3*3
+			{// N mode with 2 stream
+				if (MCS15 && (Rssi >= (-70+RssiOffset)))
+					TxRateIdx = MCS15;
+				else if (MCS14 && (Rssi >= (-72+RssiOffset)))
+					TxRateIdx = MCS14;
+				else if (MCS13 && (Rssi >= (-76+RssiOffset)))
+					TxRateIdx = MCS13;
+				else if (MCS12 && (Rssi >= (-78+RssiOffset)))
+					TxRateIdx = MCS12;
+				else if (MCS4 && (Rssi >= (-82+RssiOffset)))
+					TxRateIdx = MCS4;
+				else if (MCS3 && (Rssi >= (-84+RssiOffset)))
+					TxRateIdx = MCS3;
+				else if (MCS2 && (Rssi >= (-86+RssiOffset)))
+					TxRateIdx = MCS2;
+				else if (MCS1 && (Rssi >= (-88+RssiOffset)))
+					TxRateIdx = MCS1;
+				else
+					TxRateIdx = MCS0;
+			}
+			else if ((pTable == RateSwitchTable11BGN1S) || (pTable == RateSwitchTable11N1S))
+			{// N mode with 1 stream
+				if (MCS7 && (Rssi > (-72+RssiOffset)))
+					TxRateIdx = MCS7;
+				else if (MCS6 && (Rssi > (-74+RssiOffset)))
+					TxRateIdx = MCS6;
+				else if (MCS5 && (Rssi > (-77+RssiOffset)))
+					TxRateIdx = MCS5;
+				else if (MCS4 && (Rssi > (-79+RssiOffset)))
+					TxRateIdx = MCS4;
+				else if (MCS3 && (Rssi > (-81+RssiOffset)))
+					TxRateIdx = MCS3;
+				else if (MCS2 && (Rssi > (-83+RssiOffset)))
+					TxRateIdx = MCS2;
+				else if (MCS1 && (Rssi > (-86+RssiOffset)))
+					TxRateIdx = MCS1;
+				else
+					TxRateIdx = MCS0;
+			}
+			else
+#endif // DOT11_N_SUPPORT //
+			{// Legacy mode
+				if (MCS7 && (Rssi > -70))
+					TxRateIdx = MCS7;
+				else if (MCS6 && (Rssi > -74))
+					TxRateIdx = MCS6;
+				else if (MCS5 && (Rssi > -78))
+					TxRateIdx = MCS5;
+				else if (MCS4 && (Rssi > -82))
+					TxRateIdx = MCS4;
+				else if (MCS4 == 0)	// for B-only mode
+					TxRateIdx = MCS3;
+				else if (MCS3 && (Rssi > -85))
+					TxRateIdx = MCS3;
+				else if (MCS2 && (Rssi > -87))
+					TxRateIdx = MCS2;
+				else if (MCS1 && (Rssi > -90))
+					TxRateIdx = MCS1;
+				else
+					TxRateIdx = MCS0;
+			}
+
+	//		if (TxRateIdx != pAd->CommonCfg.TxRateIndex)
+			{
+				pEntry->CurrTxRateIndex = TxRateIdx;
+				pNextTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(pEntry->CurrTxRateIndex+1)*5];
+				MlmeSetTxRate(pAd, pEntry, pNextTxRate);
+			}
+
+			NdisZeroMemory(pEntry->TxQuality, sizeof(USHORT) * MAX_STEP_OF_TX_RATE_SWITCH);
+			NdisZeroMemory(pEntry->PER, sizeof(UCHAR) * MAX_STEP_OF_TX_RATE_SWITCH);
+			pEntry->fLastSecAccordingRSSI = TRUE;
+			// reset all OneSecTx counters
+			RESET_ONE_SEC_TX_CNT(pEntry);
+
+			continue;
+		}
+
+		if (pEntry->fLastSecAccordingRSSI == TRUE)
+		{
+			pEntry->fLastSecAccordingRSSI = FALSE;
+			pEntry->LastSecTxRateChangeAction = 0;
+			// reset all OneSecTx counters
+			RESET_ONE_SEC_TX_CNT(pEntry);
+
+			continue;
+		}
+
+		do
+		{
+			BOOLEAN	bTrainUpDown = FALSE;
+
+			pEntry->CurrTxRateStableTime ++;
+
+			// downgrade TX quality if PER >= Rate-Down threshold
+			if (TxErrorRatio >= TrainDown)
+			{
+				bTrainUpDown = TRUE;
+				pEntry->TxQuality[CurrRateIdx] = DRS_TX_QUALITY_WORST_BOUND;
+			}
+			// upgrade TX quality if PER <= Rate-Up threshold
+			else if (TxErrorRatio <= TrainUp)
+			{
+				bTrainUpDown = TRUE;
+				bUpgradeQuality = TRUE;
+				if (pEntry->TxQuality[CurrRateIdx])
+					pEntry->TxQuality[CurrRateIdx] --;  // quality very good in CurrRate
+
+				if (pEntry->TxRateUpPenalty)
+					pEntry->TxRateUpPenalty --;
+				else if (pEntry->TxQuality[UpRateIdx])
+					pEntry->TxQuality[UpRateIdx] --;    // may improve next UP rate's quality
+			}
+
+			pEntry->PER[CurrRateIdx] = (UCHAR)TxErrorRatio;
+
+			if (bTrainUpDown)
+			{
+				// perform DRS - consider TxRate Down first, then rate up.
+				if ((CurrRateIdx != DownRateIdx) && (pEntry->TxQuality[CurrRateIdx] >= DRS_TX_QUALITY_WORST_BOUND))
+				{
+					pEntry->CurrTxRateIndex = DownRateIdx;
+				}
+				else if ((CurrRateIdx != UpRateIdx) && (pEntry->TxQuality[UpRateIdx] <= 0))
+				{
+					pEntry->CurrTxRateIndex = UpRateIdx;
+				}
+			}
+		} while (FALSE);
+
+		// if rate-up happen, clear all bad history of all TX rates
+		if (pEntry->CurrTxRateIndex > CurrRateIdx)
+		{
+			pEntry->CurrTxRateStableTime = 0;
+			pEntry->TxRateUpPenalty = 0;
+			pEntry->LastSecTxRateChangeAction = 1; // rate UP
+			NdisZeroMemory(pEntry->TxQuality, sizeof(USHORT) * MAX_STEP_OF_TX_RATE_SWITCH);
+			NdisZeroMemory(pEntry->PER, sizeof(UCHAR) * MAX_STEP_OF_TX_RATE_SWITCH);
+
+			//
+			// For TxRate fast train up
+			//
+			if (!pAd->StaCfg.StaQuickResponeForRateUpTimerRunning)
+			{
+				RTMPSetTimer(&pAd->StaCfg.StaQuickResponeForRateUpTimer, 100);
+
+				pAd->StaCfg.StaQuickResponeForRateUpTimerRunning = TRUE;
+			}
+			bTxRateChanged = TRUE;
+		}
+		// if rate-down happen, only clear DownRate's bad history
+		else if (pEntry->CurrTxRateIndex < CurrRateIdx)
+		{
+			pEntry->CurrTxRateStableTime = 0;
+			pEntry->TxRateUpPenalty = 0;           // no penalty
+			pEntry->LastSecTxRateChangeAction = 2; // rate DOWN
+			pEntry->TxQuality[pEntry->CurrTxRateIndex] = 0;
+			pEntry->PER[pEntry->CurrTxRateIndex] = 0;
+
+			//
+			// For TxRate fast train down
+			//
+			if (!pAd->StaCfg.StaQuickResponeForRateUpTimerRunning)
+			{
+				RTMPSetTimer(&pAd->StaCfg.StaQuickResponeForRateUpTimer, 100);
+
+				pAd->StaCfg.StaQuickResponeForRateUpTimerRunning = TRUE;
+			}
+			bTxRateChanged = TRUE;
+		}
+		else
+		{
+			pEntry->LastSecTxRateChangeAction = 0; // rate no change
+			bTxRateChanged = FALSE;
+		}
+
+		pEntry->LastTxOkCount = TxSuccess;
+
+		// reset all OneSecTx counters
+		RESET_ONE_SEC_TX_CNT(pEntry);
+
+		pNextTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(pEntry->CurrTxRateIndex+1)*5];
+		if (bTxRateChanged && pNextTxRate)
+		{
+			MlmeSetTxRate(pAd, pEntry, pNextTxRate);
+		}
+	}
+}
+
+/*
+	========================================================================
+	Routine Description:
+		Station side, Auto TxRate faster train up timer call back function.
+
+	Arguments:
+		SystemSpecific1			- Not used.
+		FunctionContext			- Pointer to our Adapter context.
+		SystemSpecific2			- Not used.
+		SystemSpecific3			- Not used.
+
+	Return Value:
+		None
+
+	========================================================================
+*/
+VOID StaQuickResponeForRateUpExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+	PRTMP_ADAPTER			pAd = (PRTMP_ADAPTER)FunctionContext;
+	UCHAR					UpRateIdx = 0, DownRateIdx = 0, CurrRateIdx = 0;
+	ULONG					TxTotalCnt;
+	ULONG					TxErrorRatio = 0;
+	BOOLEAN					bTxRateChanged; //, bUpgradeQuality = FALSE;
+	PRTMP_TX_RATE_SWITCH	pCurrTxRate, pNextTxRate = NULL;
+	PUCHAR					pTable;
+	UCHAR					TableSize = 0;
+	UCHAR					InitTxRateIdx = 0, TrainUp, TrainDown;
+	TX_STA_CNT1_STRUC		StaTx1;
+	TX_STA_CNT0_STRUC		TxStaCnt0;
+	CHAR					Rssi, ratio;
+	ULONG					TxRetransmit = 0, TxSuccess = 0, TxFailCount = 0;
+	MAC_TABLE_ENTRY			*pEntry;
+	ULONG					i;
+
+	pAd->StaCfg.StaQuickResponeForRateUpTimerRunning = FALSE;
+
+    //
+    // walk through MAC table, see if need to change AP's TX rate toward each entry
+    //
+	for (i = 1; i < MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		pEntry = &pAd->MacTab.Content[i];
+
+		// check if this entry need to switch rate automatically
+		if (RTMPCheckEntryEnableAutoRateSwitch(pAd, pEntry) == FALSE)
+			continue;
+
+		if (INFRA_ON(pAd) && (i == 1))
+			Rssi = RTMPMaxRssi(pAd,
+							   pAd->StaCfg.RssiSample.AvgRssi0,
+							   pAd->StaCfg.RssiSample.AvgRssi1,
+							   pAd->StaCfg.RssiSample.AvgRssi2);
+		else
+			Rssi = RTMPMaxRssi(pAd,
+							   pEntry->RssiSample.AvgRssi0,
+							   pEntry->RssiSample.AvgRssi1,
+							   pEntry->RssiSample.AvgRssi2);
+
+		CurrRateIdx = pAd->CommonCfg.TxRateIndex;
+
+			MlmeSelectTxRateTable(pAd, pEntry, &pTable, &TableSize, &InitTxRateIdx);
+
+		// decide the next upgrade rate and downgrade rate, if any
+		if ((CurrRateIdx > 0) && (CurrRateIdx < (TableSize - 1)))
+		{
+			UpRateIdx = CurrRateIdx + 1;
+			DownRateIdx = CurrRateIdx -1;
+		}
+		else if (CurrRateIdx == 0)
+		{
+			UpRateIdx = CurrRateIdx + 1;
+			DownRateIdx = CurrRateIdx;
+		}
+		else if (CurrRateIdx == (TableSize - 1))
+		{
+			UpRateIdx = CurrRateIdx;
+			DownRateIdx = CurrRateIdx - 1;
+		}
+
+		pCurrTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(CurrRateIdx+1)*5];
+
+#ifdef DOT11_N_SUPPORT
+		if ((Rssi > -65) && (pCurrTxRate->Mode >= MODE_HTMIX))
+		{
+			TrainUp		= (pCurrTxRate->TrainUp + (pCurrTxRate->TrainUp >> 1));
+			TrainDown	= (pCurrTxRate->TrainDown + (pCurrTxRate->TrainDown >> 1));
+		}
+		else
+#endif // DOT11_N_SUPPORT //
+		{
+			TrainUp		= pCurrTxRate->TrainUp;
+			TrainDown	= pCurrTxRate->TrainDown;
+		}
+
+		if (pAd->MacTab.Size == 1)
+		{
+			// Update statistic counter
+			RTMP_IO_READ32(pAd, TX_STA_CNT0, &TxStaCnt0.word);
+			RTMP_IO_READ32(pAd, TX_STA_CNT1, &StaTx1.word);
+
+			TxRetransmit = StaTx1.field.TxRetransmit;
+			TxSuccess = StaTx1.field.TxSuccess;
+			TxFailCount = TxStaCnt0.field.TxFailCount;
+			TxTotalCnt = TxRetransmit + TxSuccess + TxFailCount;
+
+			pAd->RalinkCounters.OneSecTxRetryOkCount += StaTx1.field.TxRetransmit;
+			pAd->RalinkCounters.OneSecTxNoRetryOkCount += StaTx1.field.TxSuccess;
+			pAd->RalinkCounters.OneSecTxFailCount += TxStaCnt0.field.TxFailCount;
+			pAd->WlanCounters.TransmittedFragmentCount.u.LowPart += StaTx1.field.TxSuccess;
+			pAd->WlanCounters.RetryCount.u.LowPart += StaTx1.field.TxRetransmit;
+			pAd->WlanCounters.FailedCount.u.LowPart += TxStaCnt0.field.TxFailCount;
+
+#if 0 // test by Gary.
+			// if no traffic in the past 1-sec period, don't change TX rate,
+			// but clear all bad history. because the bad history may affect the next
+			// Chariot throughput test
+			TxTotalCnt = pAd->RalinkCounters.OneSecTxNoRetryOkCount +
+						 pAd->RalinkCounters.OneSecTxRetryOkCount +
+						 pAd->RalinkCounters.OneSecTxFailCount;
+#endif
+			if (TxTotalCnt)
+				TxErrorRatio = ((TxRetransmit + TxFailCount) * 100) / TxTotalCnt;
+		}
+		else
+		{
+			TxTotalCnt = pEntry->OneSecTxNoRetryOkCount +
+				 pEntry->OneSecTxRetryOkCount +
+				 pEntry->OneSecTxFailCount;
+
+			if (TxTotalCnt)
+				TxErrorRatio = ((pEntry->OneSecTxRetryOkCount + pEntry->OneSecTxFailCount) * 100) / TxTotalCnt;
+		}
+
+
+		//
+		// CASE 1. when TX samples are fewer than 15, then decide TX rate solely on RSSI
+		//         (criteria copied from RT2500 for Netopia case)
+		//
+		if (TxTotalCnt <= 12)
+		{
+			NdisZeroMemory(pAd->DrsCounters.TxQuality, sizeof(USHORT) * MAX_STEP_OF_TX_RATE_SWITCH);
+			NdisZeroMemory(pAd->DrsCounters.PER, sizeof(UCHAR) * MAX_STEP_OF_TX_RATE_SWITCH);
+
+			if ((pAd->DrsCounters.LastSecTxRateChangeAction == 1) && (CurrRateIdx != DownRateIdx))
+			{
+				pAd->CommonCfg.TxRateIndex = DownRateIdx;
+				pAd->DrsCounters.TxQuality[CurrRateIdx] = DRS_TX_QUALITY_WORST_BOUND;
+			}
+			else if ((pAd->DrsCounters.LastSecTxRateChangeAction == 2) && (CurrRateIdx != UpRateIdx))
+			{
+				pAd->CommonCfg.TxRateIndex = UpRateIdx;
+			}
+
+			DBGPRINT_RAW(RT_DEBUG_TRACE,("QuickDRS: TxTotalCnt <= 15, train back to original rate \n"));
+			return;
+		}
+
+		do
+		{
+			ULONG OneSecTxNoRetryOKRationCount;
+
+			if (pAd->DrsCounters.LastTimeTxRateChangeAction == 0)
+				ratio = 5;
+			else
+				ratio = 4;
+
+			// downgrade TX quality if PER >= Rate-Down threshold
+			if (TxErrorRatio >= TrainDown)
+			{
+				pAd->DrsCounters.TxQuality[CurrRateIdx] = DRS_TX_QUALITY_WORST_BOUND;
+			}
+
+			pAd->DrsCounters.PER[CurrRateIdx] = (UCHAR)TxErrorRatio;
+
+			OneSecTxNoRetryOKRationCount = (TxSuccess * ratio);
+
+			// perform DRS - consider TxRate Down first, then rate up.
+			if ((pAd->DrsCounters.LastSecTxRateChangeAction == 1) && (CurrRateIdx != DownRateIdx))
+			{
+				if ((pAd->DrsCounters.LastTxOkCount + 2) >= OneSecTxNoRetryOKRationCount)
+				{
+					pAd->CommonCfg.TxRateIndex = DownRateIdx;
+					pAd->DrsCounters.TxQuality[CurrRateIdx] = DRS_TX_QUALITY_WORST_BOUND;
+
+				}
+
+			}
+			else if ((pAd->DrsCounters.LastSecTxRateChangeAction == 2) && (CurrRateIdx != UpRateIdx))
+			{
+				if ((TxErrorRatio >= 50) || (TxErrorRatio >= TrainDown))
+				{
+
+				}
+				else if ((pAd->DrsCounters.LastTxOkCount + 2) >= OneSecTxNoRetryOKRationCount)
+				{
+					pAd->CommonCfg.TxRateIndex = UpRateIdx;
+				}
+			}
+		}while (FALSE);
+
+		// if rate-up happen, clear all bad history of all TX rates
+		if (pAd->CommonCfg.TxRateIndex > CurrRateIdx)
+		{
+			pAd->DrsCounters.TxRateUpPenalty = 0;
+			NdisZeroMemory(pAd->DrsCounters.TxQuality, sizeof(USHORT) * MAX_STEP_OF_TX_RATE_SWITCH);
+			NdisZeroMemory(pAd->DrsCounters.PER, sizeof(UCHAR) * MAX_STEP_OF_TX_RATE_SWITCH);
+			bTxRateChanged = TRUE;
+		}
+		// if rate-down happen, only clear DownRate's bad history
+		else if (pAd->CommonCfg.TxRateIndex < CurrRateIdx)
+		{
+			DBGPRINT_RAW(RT_DEBUG_TRACE,("QuickDRS: --TX rate from %d to %d \n", CurrRateIdx, pAd->CommonCfg.TxRateIndex));
+
+			pAd->DrsCounters.TxRateUpPenalty = 0;           // no penalty
+			pAd->DrsCounters.TxQuality[pAd->CommonCfg.TxRateIndex] = 0;
+			pAd->DrsCounters.PER[pAd->CommonCfg.TxRateIndex] = 0;
+			bTxRateChanged = TRUE;
+		}
+		else
+		{
+			bTxRateChanged = FALSE;
+		}
+
+		pNextTxRate = (PRTMP_TX_RATE_SWITCH) &pTable[(pAd->CommonCfg.TxRateIndex+1)*5];
+		if (bTxRateChanged && pNextTxRate)
+		{
+			MlmeSetTxRate(pAd, pEntry, pNextTxRate);
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+		This routine is executed periodically inside MlmePeriodicExec() after
+		association with an AP.
+		It checks if StaCfg.Psm is consistent with user policy (recorded in
+		StaCfg.WindowsPowerMode). If not, enforce user policy. However,
+		there're some conditions to consider:
+		1. we don't support power-saving in ADHOC mode, so Psm=PWR_ACTIVE all
+		   the time when Mibss==TRUE
+		2. When link up in INFRA mode, Psm should not be switch to PWR_SAVE
+		   if outgoing traffic available in TxRing or MgmtRing.
+	Output:
+		1. change pAd->StaCfg.Psm to PWR_SAVE or leave it untouched
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID MlmeCheckPsmChange(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG	Now32)
+{
+	ULONG	PowerMode;
+
+	// condition -
+	// 1. Psm maybe ON only happen in INFRASTRUCTURE mode
+	// 2. user wants either MAX_PSP or FAST_PSP
+	// 3. but current psm is not in PWR_SAVE
+	// 4. CNTL state machine is not doing SCANning
+	// 5. no TX SUCCESS event for the past 1-sec period
+#ifdef NDIS51_MINIPORT
+	if (pAd->StaCfg.WindowsPowerProfile == NdisPowerProfileBattery)
+		PowerMode = pAd->StaCfg.WindowsBatteryPowerMode;
+	else
+#endif
+		PowerMode = pAd->StaCfg.WindowsPowerMode;
+
+	if (INFRA_ON(pAd) &&
+		(PowerMode != Ndis802_11PowerModeCAM) &&
+		(pAd->StaCfg.Psm == PWR_ACTIVE) &&
+//		(! RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+		(pAd->Mlme.CntlMachine.CurrState == CNTL_IDLE) /*&&
+		(pAd->RalinkCounters.OneSecTxNoRetryOkCount == 0) &&
+		(pAd->RalinkCounters.OneSecTxRetryOkCount == 0)*/)
+	{
+		NdisGetSystemUpTime(&pAd->Mlme.LastSendNULLpsmTime);
+		pAd->RalinkCounters.RxCountSinceLastNULL = 0;
+		MlmeSetPsmBit(pAd, PWR_SAVE);
+		if (!(pAd->CommonCfg.bAPSDCapable && pAd->CommonCfg.APEdcaParm.bAPSDCapable))
+		{
+			RTMPSendNullFrame(pAd, pAd->CommonCfg.TxRate, FALSE);
+		}
+		else
+		{
+			RTMPSendNullFrame(pAd, pAd->CommonCfg.TxRate, TRUE);
+		}
+	}
+}
+
+// IRQL = PASSIVE_LEVEL
+// IRQL = DISPATCH_LEVEL
+VOID MlmeSetPsmBit(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT psm)
+{
+	AUTO_RSP_CFG_STRUC csr4;
+
+	pAd->StaCfg.Psm = psm;
+	RTMP_IO_READ32(pAd, AUTO_RSP_CFG, &csr4.word);
+	csr4.field.AckCtsPsmBit = (psm == PWR_SAVE)? 1:0;
+	RTMP_IO_WRITE32(pAd, AUTO_RSP_CFG, csr4.word);
+	DBGPRINT(RT_DEBUG_TRACE, ("MlmeSetPsmBit = %d\n", psm));
+}
+#endif // CONFIG_STA_SUPPORT //
+
+
+// IRQL = DISPATCH_LEVEL
+VOID MlmeSetTxPreamble(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT TxPreamble)
+{
+	AUTO_RSP_CFG_STRUC csr4;
+
+	//
+	// Always use Long preamble before verifiation short preamble functionality works well.
+	// Todo: remove the following line if short preamble functionality works
+	//
+	//TxPreamble = Rt802_11PreambleLong;
+
+	RTMP_IO_READ32(pAd, AUTO_RSP_CFG, &csr4.word);
+	if (TxPreamble == Rt802_11PreambleLong)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("MlmeSetTxPreamble (= LONG PREAMBLE)\n"));
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED);
+		csr4.field.AutoResponderPreamble = 0;
+	}
+	else
+	{
+		// NOTE: 1Mbps should always use long preamble
+		DBGPRINT(RT_DEBUG_TRACE, ("MlmeSetTxPreamble (= SHORT PREAMBLE)\n"));
+		OPSTATUS_SET_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED);
+		csr4.field.AutoResponderPreamble = 1;
+	}
+
+	RTMP_IO_WRITE32(pAd, AUTO_RSP_CFG, csr4.word);
+}
+
+/*
+    ==========================================================================
+    Description:
+        Update basic rate bitmap
+    ==========================================================================
+ */
+
+VOID UpdateBasicRateBitmap(
+    IN  PRTMP_ADAPTER   pAdapter)
+{
+    INT  i, j;
+                  /* 1  2  5.5, 11,  6,  9, 12, 18, 24, 36, 48,  54 */
+    UCHAR rate[] = { 2, 4,  11, 22, 12, 18, 24, 36, 48, 72, 96, 108 };
+    UCHAR *sup_p = pAdapter->CommonCfg.SupRate;
+    UCHAR *ext_p = pAdapter->CommonCfg.ExtRate;
+    ULONG bitmap = pAdapter->CommonCfg.BasicRateBitmap;
+
+
+    /* if A mode, always use fix BasicRateBitMap */
+    //if (pAdapter->CommonCfg.Channel == PHY_11A)
+	if (pAdapter->CommonCfg.Channel > 14)
+        pAdapter->CommonCfg.BasicRateBitmap = 0x150; /* 6, 12, 24M */
+    /* End of if */
+
+    if (pAdapter->CommonCfg.BasicRateBitmap > 4095)
+    {
+        /* (2 ^ MAX_LEN_OF_SUPPORTED_RATES) -1 */
+        return;
+    } /* End of if */
+
+    for(i=0; i<MAX_LEN_OF_SUPPORTED_RATES; i++)
+    {
+        sup_p[i] &= 0x7f;
+        ext_p[i] &= 0x7f;
+    } /* End of for */
+
+    for(i=0; i<MAX_LEN_OF_SUPPORTED_RATES; i++)
+    {
+        if (bitmap & (1 << i))
+        {
+            for(j=0; j<MAX_LEN_OF_SUPPORTED_RATES; j++)
+            {
+                if (sup_p[j] == rate[i])
+                    sup_p[j] |= 0x80;
+                /* End of if */
+            } /* End of for */
+
+            for(j=0; j<MAX_LEN_OF_SUPPORTED_RATES; j++)
+            {
+                if (ext_p[j] == rate[i])
+                    ext_p[j] |= 0x80;
+                /* End of if */
+            } /* End of for */
+        } /* End of if */
+    } /* End of for */
+} /* End of UpdateBasicRateBitmap */
+
+// IRQL = PASSIVE_LEVEL
+// IRQL = DISPATCH_LEVEL
+// bLinkUp is to identify the inital link speed.
+// TRUE indicates the rate update at linkup, we should not try to set the rate at 54Mbps.
+VOID MlmeUpdateTxRates(
+	IN PRTMP_ADAPTER 		pAd,
+	IN 	BOOLEAN		 		bLinkUp,
+	IN	UCHAR				apidx)
+{
+	int i, num;
+	UCHAR Rate = RATE_6, MaxDesire = RATE_1, MaxSupport = RATE_1;
+	UCHAR MinSupport = RATE_54;
+	ULONG BasicRateBitmap = 0;
+	UCHAR CurrBasicRate = RATE_1;
+	UCHAR *pSupRate, SupRateLen, *pExtRate, ExtRateLen;
+	PHTTRANSMIT_SETTING		pHtPhy = NULL;
+	PHTTRANSMIT_SETTING		pMaxHtPhy = NULL;
+	PHTTRANSMIT_SETTING		pMinHtPhy = NULL;
+	BOOLEAN 				*auto_rate_cur_p;
+	UCHAR					HtMcs = MCS_AUTO;
+
+	// find max desired rate
+	UpdateBasicRateBitmap(pAd);
+
+	num = 0;
+	auto_rate_cur_p = NULL;
+	for (i=0; i<MAX_LEN_OF_SUPPORTED_RATES; i++)
+	{
+		switch (pAd->CommonCfg.DesireRate[i] & 0x7f)
+		{
+			case 2:  Rate = RATE_1;   num++;   break;
+			case 4:  Rate = RATE_2;   num++;   break;
+			case 11: Rate = RATE_5_5; num++;   break;
+			case 22: Rate = RATE_11;  num++;   break;
+			case 12: Rate = RATE_6;   num++;   break;
+			case 18: Rate = RATE_9;   num++;   break;
+			case 24: Rate = RATE_12;  num++;   break;
+			case 36: Rate = RATE_18;  num++;   break;
+			case 48: Rate = RATE_24;  num++;   break;
+			case 72: Rate = RATE_36;  num++;   break;
+			case 96: Rate = RATE_48;  num++;   break;
+			case 108: Rate = RATE_54; num++;   break;
+			//default: Rate = RATE_1;   break;
+		}
+		if (MaxDesire < Rate)  MaxDesire = Rate;
+	}
+
+//===========================================================================
+//===========================================================================
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		pHtPhy 		= &pAd->StaCfg.HTPhyMode;
+		pMaxHtPhy	= &pAd->StaCfg.MaxHTPhyMode;
+		pMinHtPhy	= &pAd->StaCfg.MinHTPhyMode;
+
+		auto_rate_cur_p = &pAd->StaCfg.bAutoTxRateSwitch;
+		HtMcs 		= pAd->StaCfg.DesiredTransmitSetting.field.MCS;
+
+		if ((pAd->StaCfg.BssType == BSS_ADHOC) &&
+			(pAd->CommonCfg.PhyMode == PHY_11B) &&
+			(MaxDesire > RATE_11))
+		{
+			MaxDesire = RATE_11;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	pAd->CommonCfg.MaxDesiredRate = MaxDesire;
+	pMinHtPhy->word = 0;
+	pMaxHtPhy->word = 0;
+	pHtPhy->word = 0;
+
+	// Auto rate switching is enabled only if more than one DESIRED RATES are
+	// specified; otherwise disabled
+	if (num <= 1)
+	{
+		//OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_TX_RATE_SWITCH_ENABLED);
+		//pAd->CommonCfg.bAutoTxRateSwitch	= FALSE;
+		*auto_rate_cur_p = FALSE;
+	}
+	else
+	{
+		//OPSTATUS_SET_FLAG(pAd, fOP_STATUS_TX_RATE_SWITCH_ENABLED);
+		//pAd->CommonCfg.bAutoTxRateSwitch	= TRUE;
+		*auto_rate_cur_p = TRUE;
+	}
+
+#if 1
+	if (HtMcs != MCS_AUTO)
+	{
+		//OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_TX_RATE_SWITCH_ENABLED);
+		//pAd->CommonCfg.bAutoTxRateSwitch	= FALSE;
+		*auto_rate_cur_p = FALSE;
+	}
+	else
+	{
+		//OPSTATUS_SET_FLAG(pAd, fOP_STATUS_TX_RATE_SWITCH_ENABLED);
+		//pAd->CommonCfg.bAutoTxRateSwitch	= TRUE;
+		*auto_rate_cur_p = TRUE;
+	}
+#endif
+
+#ifdef CONFIG_STA_SUPPORT
+	if ((ADHOC_ON(pAd) || INFRA_ON(pAd)) && (pAd->OpMode == OPMODE_STA))
+	{
+		pSupRate = &pAd->StaActive.SupRate[0];
+		pExtRate = &pAd->StaActive.ExtRate[0];
+		SupRateLen = pAd->StaActive.SupRateLen;
+		ExtRateLen = pAd->StaActive.ExtRateLen;
+	}
+	else
+#endif // CONFIG_STA_SUPPORT //
+	{
+		pSupRate = &pAd->CommonCfg.SupRate[0];
+		pExtRate = &pAd->CommonCfg.ExtRate[0];
+		SupRateLen = pAd->CommonCfg.SupRateLen;
+		ExtRateLen = pAd->CommonCfg.ExtRateLen;
+	}
+
+	// find max supported rate
+	for (i=0; i<SupRateLen; i++)
+	{
+		switch (pSupRate[i] & 0x7f)
+		{
+			case 2:   Rate = RATE_1;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0001;	 break;
+			case 4:   Rate = RATE_2;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0002;	 break;
+			case 11:  Rate = RATE_5_5;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0004;	 break;
+			case 22:  Rate = RATE_11;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0008;	 break;
+			case 12:  Rate = RATE_6;	/*if (pSupRate[i] & 0x80)*/  BasicRateBitmap |= 0x0010;  break;
+			case 18:  Rate = RATE_9;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0020;	 break;
+			case 24:  Rate = RATE_12;	/*if (pSupRate[i] & 0x80)*/  BasicRateBitmap |= 0x0040;  break;
+			case 36:  Rate = RATE_18;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0080;	 break;
+			case 48:  Rate = RATE_24;	/*if (pSupRate[i] & 0x80)*/  BasicRateBitmap |= 0x0100;  break;
+			case 72:  Rate = RATE_36;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0200;	 break;
+			case 96:  Rate = RATE_48;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0400;	 break;
+			case 108: Rate = RATE_54;	if (pSupRate[i] & 0x80) BasicRateBitmap |= 0x0800;	 break;
+			default:  Rate = RATE_1;	break;
+		}
+		if (MaxSupport < Rate)	MaxSupport = Rate;
+
+		if (MinSupport > Rate) MinSupport = Rate;
+	}
+
+	for (i=0; i<ExtRateLen; i++)
+	{
+		switch (pExtRate[i] & 0x7f)
+		{
+			case 2:   Rate = RATE_1;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0001;	 break;
+			case 4:   Rate = RATE_2;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0002;	 break;
+			case 11:  Rate = RATE_5_5;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0004;	 break;
+			case 22:  Rate = RATE_11;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0008;	 break;
+			case 12:  Rate = RATE_6;	/*if (pExtRate[i] & 0x80)*/  BasicRateBitmap |= 0x0010;  break;
+			case 18:  Rate = RATE_9;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0020;	 break;
+			case 24:  Rate = RATE_12;	/*if (pExtRate[i] & 0x80)*/  BasicRateBitmap |= 0x0040;  break;
+			case 36:  Rate = RATE_18;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0080;	 break;
+			case 48:  Rate = RATE_24;	/*if (pExtRate[i] & 0x80)*/  BasicRateBitmap |= 0x0100;  break;
+			case 72:  Rate = RATE_36;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0200;	 break;
+			case 96:  Rate = RATE_48;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0400;	 break;
+			case 108: Rate = RATE_54;	if (pExtRate[i] & 0x80) BasicRateBitmap |= 0x0800;	 break;
+			default:  Rate = RATE_1;	break;
+		}
+		if (MaxSupport < Rate)	MaxSupport = Rate;
+
+		if (MinSupport > Rate) MinSupport = Rate;
+	}
+
+	RTMP_IO_WRITE32(pAd, LEGACY_BASIC_RATE, BasicRateBitmap);
+
+	// bug fix
+	// pAd->CommonCfg.BasicRateBitmap = BasicRateBitmap;
+
+	// calculate the exptected ACK rate for each TX rate. This info is used to caculate
+	// the DURATION field of outgoing uniicast DATA/MGMT frame
+	for (i=0; i<MAX_LEN_OF_SUPPORTED_RATES; i++)
+	{
+		if (BasicRateBitmap & (0x01 << i))
+			CurrBasicRate = (UCHAR)i;
+		pAd->CommonCfg.ExpectedACKRate[i] = CurrBasicRate;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("MlmeUpdateTxRates[MaxSupport = %d] = MaxDesire %d Mbps\n", RateIdToMbps[MaxSupport], RateIdToMbps[MaxDesire]));
+	// max tx rate = min {max desire rate, max supported rate}
+	if (MaxSupport < MaxDesire)
+		pAd->CommonCfg.MaxTxRate = MaxSupport;
+	else
+		pAd->CommonCfg.MaxTxRate = MaxDesire;
+
+	pAd->CommonCfg.MinTxRate = MinSupport;
+	// 2003-07-31 john - 2500 doesn't have good sensitivity at high OFDM rates. to increase the success
+	// ratio of initial DHCP packet exchange, TX rate starts from a lower rate depending
+	// on average RSSI
+	//	 1. RSSI >= -70db, start at 54 Mbps (short distance)
+	//	 2. -70 > RSSI >= -75, start at 24 Mbps (mid distance)
+	//	 3. -75 > RSSI, start at 11 Mbps (long distance)
+	//if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_TX_RATE_SWITCH_ENABLED)/* &&
+	//	OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED)*/)
+	if (*auto_rate_cur_p)
+	{
+		short dbm = 0;
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			dbm = pAd->StaCfg.RssiSample.AvgRssi0 - pAd->BbpRssiToDbmDelta;
+#endif // CONFIG_STA_SUPPORT //
+		if (bLinkUp == TRUE)
+			pAd->CommonCfg.TxRate = RATE_24;
+		else
+			pAd->CommonCfg.TxRate = pAd->CommonCfg.MaxTxRate;
+
+		if (dbm < -75)
+			pAd->CommonCfg.TxRate = RATE_11;
+		else if (dbm < -70)
+			pAd->CommonCfg.TxRate = RATE_24;
+
+		// should never exceed MaxTxRate (consider 11B-only mode)
+		if (pAd->CommonCfg.TxRate > pAd->CommonCfg.MaxTxRate)
+			pAd->CommonCfg.TxRate = pAd->CommonCfg.MaxTxRate;
+
+		pAd->CommonCfg.TxRateIndex = 0;
+	}
+	else
+	{
+		pAd->CommonCfg.TxRate = pAd->CommonCfg.MaxTxRate;
+		pHtPhy->field.MCS	= (pAd->CommonCfg.MaxTxRate > 3) ? (pAd->CommonCfg.MaxTxRate - 4) : pAd->CommonCfg.MaxTxRate;
+		pHtPhy->field.MODE	= (pAd->CommonCfg.MaxTxRate > 3) ? MODE_OFDM : MODE_CCK;
+
+		pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.STBC	= pHtPhy->field.STBC;
+		pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.ShortGI	= pHtPhy->field.ShortGI;
+		pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MCS		= pHtPhy->field.MCS;
+		pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE	= pHtPhy->field.MODE;
+	}
+
+	if (pAd->CommonCfg.TxRate <= RATE_11)
+	{
+		pMaxHtPhy->field.MODE = MODE_CCK;
+		pMaxHtPhy->field.MCS = pAd->CommonCfg.TxRate;
+		pMinHtPhy->field.MCS = pAd->CommonCfg.MinTxRate;
+	}
+	else
+	{
+		pMaxHtPhy->field.MODE = MODE_OFDM;
+		pMaxHtPhy->field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.TxRate];
+		if (pAd->CommonCfg.MinTxRate >= RATE_6 && (pAd->CommonCfg.MinTxRate <= RATE_54))
+			{pMinHtPhy->field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MinTxRate];}
+		else
+			{pMinHtPhy->field.MCS = pAd->CommonCfg.MinTxRate;}
+	}
+
+	pHtPhy->word = (pMaxHtPhy->word);
+	if (bLinkUp && (pAd->OpMode == OPMODE_STA))
+	{
+			pAd->MacTab.Content[BSSID_WCID].HTPhyMode.word = pHtPhy->word;
+			pAd->MacTab.Content[BSSID_WCID].MaxHTPhyMode.word = pMaxHtPhy->word;
+			pAd->MacTab.Content[BSSID_WCID].MinHTPhyMode.word = pMinHtPhy->word;
+	}
+	else
+	{
+		switch (pAd->CommonCfg.PhyMode)
+		{
+			case PHY_11BG_MIXED:
+			case PHY_11B:
+#ifdef DOT11_N_SUPPORT
+			case PHY_11BGN_MIXED:
+#endif // DOT11_N_SUPPORT //
+				pAd->CommonCfg.MlmeRate = RATE_1;
+				pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_CCK;
+				pAd->CommonCfg.MlmeTransmit.field.MCS = RATE_1;
+
+//#ifdef	WIFI_TEST
+				pAd->CommonCfg.RtsRate = RATE_11;
+//#else
+//				pAd->CommonCfg.RtsRate = RATE_1;
+//#endif
+				break;
+			case PHY_11G:
+			case PHY_11A:
+#ifdef DOT11_N_SUPPORT
+			case PHY_11AGN_MIXED:
+			case PHY_11GN_MIXED:
+			case PHY_11N_2_4G:
+			case PHY_11AN_MIXED:
+			case PHY_11N_5G:
+#endif // DOT11_N_SUPPORT //
+				pAd->CommonCfg.MlmeRate = RATE_6;
+				pAd->CommonCfg.RtsRate = RATE_6;
+				pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_OFDM;
+				pAd->CommonCfg.MlmeTransmit.field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MlmeRate];
+				break;
+			case PHY_11ABG_MIXED:
+#ifdef DOT11_N_SUPPORT
+			case PHY_11ABGN_MIXED:
+#endif // DOT11_N_SUPPORT //
+				if (pAd->CommonCfg.Channel <= 14)
+				{
+					pAd->CommonCfg.MlmeRate = RATE_1;
+					pAd->CommonCfg.RtsRate = RATE_1;
+					pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_CCK;
+					pAd->CommonCfg.MlmeTransmit.field.MCS = RATE_1;
+				}
+				else
+				{
+					pAd->CommonCfg.MlmeRate = RATE_6;
+					pAd->CommonCfg.RtsRate = RATE_6;
+					pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_OFDM;
+					pAd->CommonCfg.MlmeTransmit.field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MlmeRate];
+				}
+				break;
+			default: // error
+				pAd->CommonCfg.MlmeRate = RATE_6;
+                        	pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_OFDM;
+				pAd->CommonCfg.MlmeTransmit.field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MlmeRate];
+				pAd->CommonCfg.RtsRate = RATE_1;
+				break;
+		}
+		//
+		// Keep Basic Mlme Rate.
+		//
+		pAd->MacTab.Content[MCAST_WCID].HTPhyMode.word = pAd->CommonCfg.MlmeTransmit.word;
+		if (pAd->CommonCfg.MlmeTransmit.field.MODE == MODE_OFDM)
+			pAd->MacTab.Content[MCAST_WCID].HTPhyMode.field.MCS = OfdmRateToRxwiMCS[RATE_24];
+		else
+			pAd->MacTab.Content[MCAST_WCID].HTPhyMode.field.MCS = RATE_1;
+		pAd->CommonCfg.BasicMlmeRate = pAd->CommonCfg.MlmeRate;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, (" MlmeUpdateTxRates (MaxDesire=%d, MaxSupport=%d, MaxTxRate=%d, MinRate=%d, Rate Switching =%d)\n",
+			 RateIdToMbps[MaxDesire], RateIdToMbps[MaxSupport], RateIdToMbps[pAd->CommonCfg.MaxTxRate], RateIdToMbps[pAd->CommonCfg.MinTxRate],
+			 /*OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_TX_RATE_SWITCH_ENABLED)*/*auto_rate_cur_p));
+	DBGPRINT(RT_DEBUG_TRACE, (" MlmeUpdateTxRates (TxRate=%d, RtsRate=%d, BasicRateBitmap=0x%04lx)\n",
+			 RateIdToMbps[pAd->CommonCfg.TxRate], RateIdToMbps[pAd->CommonCfg.RtsRate], BasicRateBitmap));
+	DBGPRINT(RT_DEBUG_TRACE, ("MlmeUpdateTxRates (MlmeTransmit=0x%x, MinHTPhyMode=%x, MaxHTPhyMode=0x%x, HTPhyMode=0x%x)\n",
+			 pAd->CommonCfg.MlmeTransmit.word, pAd->MacTab.Content[BSSID_WCID].MinHTPhyMode.word ,pAd->MacTab.Content[BSSID_WCID].MaxHTPhyMode.word ,pAd->MacTab.Content[BSSID_WCID].HTPhyMode.word ));
+}
+
+#ifdef DOT11_N_SUPPORT
+/*
+	==========================================================================
+	Description:
+		This function update HT Rate setting.
+		Input Wcid value is valid for 2 case :
+		1. it's used for Station in infra mode that copy AP rate to Mactable.
+		2. OR Station 	in adhoc mode to copy peer's HT rate to Mactable.
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID MlmeUpdateHtTxRates(
+	IN PRTMP_ADAPTER 		pAd,
+	IN	UCHAR				apidx)
+{
+	UCHAR	StbcMcs; //j, StbcMcs, bitmask;
+	CHAR 	i; // 3*3
+	RT_HT_CAPABILITY 	*pRtHtCap = NULL;
+	RT_HT_PHY_INFO		*pActiveHtPhy = NULL;
+	ULONG		BasicMCS;
+	UCHAR j, bitmask;
+	PRT_HT_PHY_INFO			pDesireHtPhy = NULL;
+	PHTTRANSMIT_SETTING		pHtPhy = NULL;
+	PHTTRANSMIT_SETTING		pMaxHtPhy = NULL;
+	PHTTRANSMIT_SETTING		pMinHtPhy = NULL;
+	BOOLEAN 				*auto_rate_cur_p;
+
+	DBGPRINT(RT_DEBUG_TRACE,("MlmeUpdateHtTxRates===> \n"));
+
+	auto_rate_cur_p = NULL;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		pDesireHtPhy	= &pAd->StaCfg.DesiredHtPhyInfo;
+		pActiveHtPhy	= &pAd->StaCfg.DesiredHtPhyInfo;
+		pHtPhy 		= &pAd->StaCfg.HTPhyMode;
+		pMaxHtPhy	= &pAd->StaCfg.MaxHTPhyMode;
+		pMinHtPhy	= &pAd->StaCfg.MinHTPhyMode;
+
+		auto_rate_cur_p = &pAd->StaCfg.bAutoTxRateSwitch;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+	if ((ADHOC_ON(pAd) || INFRA_ON(pAd)) && (pAd->OpMode == OPMODE_STA))
+	{
+		if (pAd->StaActive.SupportedPhyInfo.bHtEnable == FALSE)
+			return;
+
+		pRtHtCap = &pAd->StaActive.SupportedHtPhy;
+		pActiveHtPhy = &pAd->StaActive.SupportedPhyInfo;
+		StbcMcs = (UCHAR)pAd->MlmeAux.AddHtInfo.AddHtInfo3.StbcMcs;
+		BasicMCS =pAd->MlmeAux.AddHtInfo.MCSSet[0]+(pAd->MlmeAux.AddHtInfo.MCSSet[1]<<8)+(StbcMcs<<16);
+		if ((pAd->CommonCfg.DesiredHtPhy.TxSTBC) && (pRtHtCap->RxSTBC) && (pAd->Antenna.field.TxPath == 2))
+			pMaxHtPhy->field.STBC = STBC_USE;
+		else
+			pMaxHtPhy->field.STBC = STBC_NONE;
+	}
+	else
+#endif // CONFIG_STA_SUPPORT //
+	{
+		if (pDesireHtPhy->bHtEnable == FALSE)
+			return;
+
+		pRtHtCap = &pAd->CommonCfg.DesiredHtPhy;
+		StbcMcs = (UCHAR)pAd->CommonCfg.AddHTInfo.AddHtInfo3.StbcMcs;
+		BasicMCS = pAd->CommonCfg.AddHTInfo.MCSSet[0]+(pAd->CommonCfg.AddHTInfo.MCSSet[1]<<8)+(StbcMcs<<16);
+		if ((pAd->CommonCfg.DesiredHtPhy.TxSTBC) && (pRtHtCap->RxSTBC) && (pAd->Antenna.field.TxPath == 2))
+			pMaxHtPhy->field.STBC = STBC_USE;
+		else
+			pMaxHtPhy->field.STBC = STBC_NONE;
+	}
+
+	// Decide MAX ht rate.
+	if ((pRtHtCap->GF) && (pAd->CommonCfg.DesiredHtPhy.GF))
+		pMaxHtPhy->field.MODE = MODE_HTGREENFIELD;
+	else
+		pMaxHtPhy->field.MODE = MODE_HTMIX;
+
+    if ((pAd->CommonCfg.DesiredHtPhy.ChannelWidth) && (pRtHtCap->ChannelWidth))
+		pMaxHtPhy->field.BW = BW_40;
+	else
+		pMaxHtPhy->field.BW = BW_20;
+
+    if (pMaxHtPhy->field.BW == BW_20)
+		pMaxHtPhy->field.ShortGI = (pAd->CommonCfg.DesiredHtPhy.ShortGIfor20 & pRtHtCap->ShortGIfor20);
+	else
+		pMaxHtPhy->field.ShortGI = (pAd->CommonCfg.DesiredHtPhy.ShortGIfor40 & pRtHtCap->ShortGIfor40);
+
+	for (i=23; i>=0; i--) // 3*3
+	{
+		j = i/8;
+		bitmask = (1<<(i-(j*8)));
+
+		if ((pActiveHtPhy->MCSSet[j] & bitmask) && (pDesireHtPhy->MCSSet[j] & bitmask))
+		{
+			pMaxHtPhy->field.MCS = i;
+			break;
+		}
+
+		if (i==0)
+			break;
+	}
+
+	// Copy MIN ht rate.  rt2860???
+	pMinHtPhy->field.BW = BW_20;
+	pMinHtPhy->field.MCS = 0;
+	pMinHtPhy->field.STBC = 0;
+	pMinHtPhy->field.ShortGI = 0;
+	//If STA assigns fixed rate. update to fixed here.
+#ifdef CONFIG_STA_SUPPORT
+	if ( (pAd->OpMode == OPMODE_STA) && (pDesireHtPhy->MCSSet[0] != 0xff))
+	{
+		if (pDesireHtPhy->MCSSet[4] != 0)
+		{
+			pMaxHtPhy->field.MCS = 32;
+			pMinHtPhy->field.MCS = 32;
+			DBGPRINT(RT_DEBUG_TRACE,("MlmeUpdateHtTxRates<=== Use Fixed MCS = %d\n",pMinHtPhy->field.MCS));
+		}
+
+		for (i=23; (CHAR)i >= 0; i--) // 3*3
+		{
+			j = i/8;
+			bitmask = (1<<(i-(j*8)));
+			if ( (pDesireHtPhy->MCSSet[j] & bitmask) && (pActiveHtPhy->MCSSet[j] & bitmask))
+			{
+				pMaxHtPhy->field.MCS = i;
+				pMinHtPhy->field.MCS = i;
+				break;
+			}
+			if (i==0)
+				break;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+
+	// Decide ht rate
+	pHtPhy->field.STBC = pMaxHtPhy->field.STBC;
+	pHtPhy->field.BW = pMaxHtPhy->field.BW;
+	pHtPhy->field.MODE = pMaxHtPhy->field.MODE;
+	pHtPhy->field.MCS = pMaxHtPhy->field.MCS;
+	pHtPhy->field.ShortGI = pMaxHtPhy->field.ShortGI;
+
+	// use default now. rt2860
+	if (pDesireHtPhy->MCSSet[0] != 0xff)
+		*auto_rate_cur_p = FALSE;
+	else
+		*auto_rate_cur_p = TRUE;
+
+	DBGPRINT(RT_DEBUG_TRACE, (" MlmeUpdateHtTxRates<---.AMsduSize = %d  \n", pAd->CommonCfg.DesiredHtPhy.AmsduSize ));
+	DBGPRINT(RT_DEBUG_TRACE,("TX: MCS[0] = %x (choose %d), BW = %d, ShortGI = %d, MODE = %d,  \n", pActiveHtPhy->MCSSet[0],pHtPhy->field.MCS,
+		pHtPhy->field.BW, pHtPhy->field.ShortGI, pHtPhy->field.MODE));
+	DBGPRINT(RT_DEBUG_TRACE,("MlmeUpdateHtTxRates<=== \n"));
+}
+#endif // DOT11_N_SUPPORT //
+
+// IRQL = DISPATCH_LEVEL
+VOID MlmeRadioOff(
+	IN PRTMP_ADAPTER pAd)
+{
+	RT28XX_MLME_RADIO_OFF(pAd);
+}
+
+// IRQL = DISPATCH_LEVEL
+VOID MlmeRadioOn(
+	IN PRTMP_ADAPTER pAd)
+{
+	RT28XX_MLME_RADIO_ON(pAd);
+}
+
+// ===========================================================================================
+// bss_table.c
+// ===========================================================================================
+
+
+/*! \brief initialize BSS table
+ *	\param p_tab pointer to the table
+ *	\return none
+ *	\pre
+ *	\post
+
+ IRQL = PASSIVE_LEVEL
+ IRQL = DISPATCH_LEVEL
+
+ */
+VOID BssTableInit(
+	IN BSS_TABLE *Tab)
+{
+	int i;
+
+	Tab->BssNr = 0;
+    Tab->BssOverlapNr = 0;
+	for (i = 0; i < MAX_LEN_OF_BSS_TABLE; i++)
+	{
+		NdisZeroMemory(&Tab->BssEntry[i], sizeof(BSS_ENTRY));
+		Tab->BssEntry[i].Rssi = -127;	// initial the rssi as a minimum value
+	}
+}
+
+#ifdef DOT11_N_SUPPORT
+VOID BATableInit(
+	IN PRTMP_ADAPTER pAd,
+    IN BA_TABLE *Tab)
+{
+	int i;
+
+	Tab->numAsOriginator = 0;
+	Tab->numAsRecipient = 0;
+	NdisAllocateSpinLock(&pAd->BATabLock);
+	for (i = 0; i < MAX_LEN_OF_BA_REC_TABLE; i++)
+	{
+		Tab->BARecEntry[i].REC_BA_Status = Recipient_NONE;
+		NdisAllocateSpinLock(&(Tab->BARecEntry[i].RxReRingLock));
+	}
+	for (i = 0; i < MAX_LEN_OF_BA_ORI_TABLE; i++)
+	{
+		Tab->BAOriEntry[i].ORI_BA_Status = Originator_NONE;
+	}
+}
+#endif // DOT11_N_SUPPORT //
+
+/*! \brief search the BSS table by SSID
+ *	\param p_tab pointer to the bss table
+ *	\param ssid SSID string
+ *	\return index of the table, BSS_NOT_FOUND if not in the table
+ *	\pre
+ *	\post
+ *	\note search by sequential search
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+ULONG BssTableSearch(
+	IN BSS_TABLE *Tab,
+	IN PUCHAR	 pBssid,
+	IN UCHAR	 Channel)
+{
+	UCHAR i;
+
+	for (i = 0; i < Tab->BssNr; i++)
+	{
+		//
+		// Some AP that support A/B/G mode that may used the same BSSID on 11A and 11B/G.
+		// We should distinguish this case.
+		//
+		if ((((Tab->BssEntry[i].Channel <= 14) && (Channel <= 14)) ||
+			 ((Tab->BssEntry[i].Channel > 14) && (Channel > 14))) &&
+			MAC_ADDR_EQUAL(Tab->BssEntry[i].Bssid, pBssid))
+		{
+			return i;
+		}
+	}
+	return (ULONG)BSS_NOT_FOUND;
+}
+
+ULONG BssSsidTableSearch(
+	IN BSS_TABLE *Tab,
+	IN PUCHAR	 pBssid,
+	IN PUCHAR	 pSsid,
+	IN UCHAR	 SsidLen,
+	IN UCHAR	 Channel)
+{
+	UCHAR i;
+
+	for (i = 0; i < Tab->BssNr; i++)
+	{
+		//
+		// Some AP that support A/B/G mode that may used the same BSSID on 11A and 11B/G.
+		// We should distinguish this case.
+		//
+		if ((((Tab->BssEntry[i].Channel <= 14) && (Channel <= 14)) ||
+			 ((Tab->BssEntry[i].Channel > 14) && (Channel > 14))) &&
+			MAC_ADDR_EQUAL(Tab->BssEntry[i].Bssid, pBssid) &&
+			SSID_EQUAL(pSsid, SsidLen, Tab->BssEntry[i].Ssid, Tab->BssEntry[i].SsidLen))
+		{
+			return i;
+		}
+	}
+	return (ULONG)BSS_NOT_FOUND;
+}
+
+ULONG BssTableSearchWithSSID(
+	IN BSS_TABLE *Tab,
+	IN PUCHAR	 Bssid,
+	IN PUCHAR	 pSsid,
+	IN UCHAR	 SsidLen,
+	IN UCHAR	 Channel)
+{
+	UCHAR i;
+
+	for (i = 0; i < Tab->BssNr; i++)
+	{
+		if ((((Tab->BssEntry[i].Channel <= 14) && (Channel <= 14)) ||
+			((Tab->BssEntry[i].Channel > 14) && (Channel > 14))) &&
+			MAC_ADDR_EQUAL(&(Tab->BssEntry[i].Bssid), Bssid) &&
+			(SSID_EQUAL(pSsid, SsidLen, Tab->BssEntry[i].Ssid, Tab->BssEntry[i].SsidLen) ||
+			(NdisEqualMemory(pSsid, ZeroSsid, SsidLen)) ||
+			(NdisEqualMemory(Tab->BssEntry[i].Ssid, ZeroSsid, Tab->BssEntry[i].SsidLen))))
+		{
+			return i;
+		}
+	}
+	return (ULONG)BSS_NOT_FOUND;
+}
+
+// IRQL = DISPATCH_LEVEL
+VOID BssTableDeleteEntry(
+	IN OUT	BSS_TABLE *Tab,
+	IN		PUCHAR	  pBssid,
+	IN		UCHAR	  Channel)
+{
+	UCHAR i, j;
+
+	for (i = 0; i < Tab->BssNr; i++)
+	{
+		if ((Tab->BssEntry[i].Channel == Channel) &&
+			(MAC_ADDR_EQUAL(Tab->BssEntry[i].Bssid, pBssid)))
+		{
+			for (j = i; j < Tab->BssNr - 1; j++)
+			{
+				NdisMoveMemory(&(Tab->BssEntry[j]), &(Tab->BssEntry[j + 1]), sizeof(BSS_ENTRY));
+			}
+			NdisZeroMemory(&(Tab->BssEntry[Tab->BssNr - 1]), sizeof(BSS_ENTRY));
+			Tab->BssNr -= 1;
+			return;
+		}
+	}
+}
+
+#ifdef DOT11_N_SUPPORT
+/*
+	========================================================================
+	Routine Description:
+		Delete the Originator Entry in BAtable. Or decrease numAs Originator by 1 if needed.
+
+	Arguments:
+	// IRQL = DISPATCH_LEVEL
+	========================================================================
+*/
+VOID BATableDeleteORIEntry(
+	IN OUT	PRTMP_ADAPTER pAd,
+	IN		BA_ORI_ENTRY	*pBAORIEntry)
+{
+
+	if (pBAORIEntry->ORI_BA_Status != Originator_NONE)
+	{
+		NdisAcquireSpinLock(&pAd->BATabLock);
+		if (pBAORIEntry->ORI_BA_Status == Originator_Done)
+		{
+			pAd->BATable.numAsOriginator -= 1;
+			DBGPRINT(RT_DEBUG_TRACE, ("BATableDeleteORIEntry numAsOriginator= %ld\n", pAd->BATable.numAsRecipient));
+			// Erase Bitmap flag.
+		}
+		pAd->MacTab.Content[pBAORIEntry->Wcid].TXBAbitmap &= (~(1<<(pBAORIEntry->TID) ));	// If STA mode,  erase flag here
+		pAd->MacTab.Content[pBAORIEntry->Wcid].BAOriWcidArray[pBAORIEntry->TID] = 0;	// If STA mode,  erase flag here
+		pBAORIEntry->ORI_BA_Status = Originator_NONE;
+		pBAORIEntry->Token = 1;
+		// Not clear Sequence here.
+		NdisReleaseSpinLock(&pAd->BATabLock);
+	}
+}
+#endif // DOT11_N_SUPPORT //
+
+/*! \brief
+ *	\param
+ *	\return
+ *	\pre
+ *	\post
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+VOID BssEntrySet(
+	IN PRTMP_ADAPTER	pAd,
+	OUT BSS_ENTRY *pBss,
+	IN PUCHAR pBssid,
+	IN CHAR Ssid[],
+	IN UCHAR SsidLen,
+	IN UCHAR BssType,
+	IN USHORT BeaconPeriod,
+	IN PCF_PARM pCfParm,
+	IN USHORT AtimWin,
+	IN USHORT CapabilityInfo,
+	IN UCHAR SupRate[],
+	IN UCHAR SupRateLen,
+	IN UCHAR ExtRate[],
+	IN UCHAR ExtRateLen,
+	IN HT_CAPABILITY_IE *pHtCapability,
+	IN ADD_HT_INFO_IE *pAddHtInfo,	// AP might use this additional ht info IE
+	IN UCHAR			HtCapabilityLen,
+	IN UCHAR			AddHtInfoLen,
+	IN UCHAR			NewExtChanOffset,
+	IN UCHAR Channel,
+	IN CHAR Rssi,
+	IN LARGE_INTEGER TimeStamp,
+	IN UCHAR CkipFlag,
+	IN PEDCA_PARM pEdcaParm,
+	IN PQOS_CAPABILITY_PARM pQosCapability,
+	IN PQBSS_LOAD_PARM pQbssLoad,
+	IN USHORT LengthVIE,
+	IN PNDIS_802_11_VARIABLE_IEs pVIE)
+{
+	COPY_MAC_ADDR(pBss->Bssid, pBssid);
+	// Default Hidden SSID to be TRUE, it will be turned to FALSE after coping SSID
+	pBss->Hidden = 1;
+	if (SsidLen > 0)
+	{
+		// For hidden SSID AP, it might send beacon with SSID len equal to 0
+		// Or send beacon /probe response with SSID len matching real SSID length,
+		// but SSID is all zero. such as "00-00-00-00" with length 4.
+		// We have to prevent this case overwrite correct table
+		if (NdisEqualMemory(Ssid, ZeroSsid, SsidLen) == 0)
+		{
+		    NdisZeroMemory(pBss->Ssid, MAX_LEN_OF_SSID);
+			NdisMoveMemory(pBss->Ssid, Ssid, SsidLen);
+			pBss->SsidLen = SsidLen;
+			pBss->Hidden = 0;
+		}
+	}
+	else
+		pBss->SsidLen = 0;
+	pBss->BssType = BssType;
+	pBss->BeaconPeriod = BeaconPeriod;
+	if (BssType == BSS_INFRA)
+	{
+		if (pCfParm->bValid)
+		{
+			pBss->CfpCount = pCfParm->CfpCount;
+			pBss->CfpPeriod = pCfParm->CfpPeriod;
+			pBss->CfpMaxDuration = pCfParm->CfpMaxDuration;
+			pBss->CfpDurRemaining = pCfParm->CfpDurRemaining;
+		}
+	}
+	else
+	{
+		pBss->AtimWin = AtimWin;
+	}
+
+	pBss->CapabilityInfo = CapabilityInfo;
+	// The privacy bit indicate security is ON, it maight be WEP, TKIP or AES
+	// Combine with AuthMode, they will decide the connection methods.
+	pBss->Privacy = CAP_IS_PRIVACY_ON(pBss->CapabilityInfo);
+	ASSERT(SupRateLen <= MAX_LEN_OF_SUPPORTED_RATES);
+	if (SupRateLen <= MAX_LEN_OF_SUPPORTED_RATES)
+		NdisMoveMemory(pBss->SupRate, SupRate, SupRateLen);
+	else
+		NdisMoveMemory(pBss->SupRate, SupRate, MAX_LEN_OF_SUPPORTED_RATES);
+	pBss->SupRateLen = SupRateLen;
+	ASSERT(ExtRateLen <= MAX_LEN_OF_SUPPORTED_RATES);
+	NdisMoveMemory(pBss->ExtRate, ExtRate, ExtRateLen);
+	NdisMoveMemory(&pBss->HtCapability, pHtCapability, HtCapabilityLen);
+	NdisMoveMemory(&pBss->AddHtInfo, pAddHtInfo, AddHtInfoLen);
+	pBss->NewExtChanOffset = NewExtChanOffset;
+	pBss->ExtRateLen = ExtRateLen;
+	pBss->Channel = Channel;
+	pBss->CentralChannel = Channel;
+	pBss->Rssi = Rssi;
+	// Update CkipFlag. if not exists, the value is 0x0
+	pBss->CkipFlag = CkipFlag;
+
+	// New for microsoft Fixed IEs
+	NdisMoveMemory(pBss->FixIEs.Timestamp, &TimeStamp, 8);
+	pBss->FixIEs.BeaconInterval = BeaconPeriod;
+	pBss->FixIEs.Capabilities = CapabilityInfo;
+
+	// New for microsoft Variable IEs
+	if (LengthVIE != 0)
+	{
+		pBss->VarIELen = LengthVIE;
+		NdisMoveMemory(pBss->VarIEs, pVIE, pBss->VarIELen);
+	}
+	else
+	{
+		pBss->VarIELen = 0;
+	}
+
+	pBss->AddHtInfoLen = 0;
+	pBss->HtCapabilityLen = 0;
+#ifdef DOT11_N_SUPPORT
+	if (HtCapabilityLen> 0)
+	{
+		pBss->HtCapabilityLen = HtCapabilityLen;
+		NdisMoveMemory(&pBss->HtCapability, pHtCapability, HtCapabilityLen);
+		if (AddHtInfoLen > 0)
+		{
+			pBss->AddHtInfoLen = AddHtInfoLen;
+			NdisMoveMemory(&pBss->AddHtInfo, pAddHtInfo, AddHtInfoLen);
+
+	 			if ((pAddHtInfo->ControlChan > 2)&& (pAddHtInfo->AddHtInfo.ExtChanOffset == EXTCHA_BELOW) && (pHtCapability->HtCapInfo.ChannelWidth == BW_40))
+	 			{
+	 				pBss->CentralChannel = pAddHtInfo->ControlChan - 2;
+	 			}
+	 			else if ((pAddHtInfo->AddHtInfo.ExtChanOffset == EXTCHA_ABOVE) && (pHtCapability->HtCapInfo.ChannelWidth == BW_40))
+				{
+		 				pBss->CentralChannel = pAddHtInfo->ControlChan + 2;
+				}
+		}
+	}
+#endif // DOT11_N_SUPPORT //
+
+	BssCipherParse(pBss);
+
+	// new for QOS
+	if (pEdcaParm)
+		NdisMoveMemory(&pBss->EdcaParm, pEdcaParm, sizeof(EDCA_PARM));
+	else
+		pBss->EdcaParm.bValid = FALSE;
+	if (pQosCapability)
+		NdisMoveMemory(&pBss->QosCapability, pQosCapability, sizeof(QOS_CAPABILITY_PARM));
+	else
+		pBss->QosCapability.bValid = FALSE;
+	if (pQbssLoad)
+		NdisMoveMemory(&pBss->QbssLoad, pQbssLoad, sizeof(QBSS_LOAD_PARM));
+	else
+		pBss->QbssLoad.bValid = FALSE;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		PEID_STRUCT     pEid;
+		USHORT          Length = 0;
+
+
+		NdisZeroMemory(&pBss->WpaIE.IE[0], MAX_CUSTOM_LEN);
+		NdisZeroMemory(&pBss->RsnIE.IE[0], MAX_CUSTOM_LEN);
+#ifdef EXT_BUILD_CHANNEL_LIST
+		NdisZeroMemory(&pBss->CountryString[0], 3);
+		pBss->bHasCountryIE = FALSE;
+#endif // EXT_BUILD_CHANNEL_LIST //
+		pEid = (PEID_STRUCT) pVIE;
+		while ((Length + 2 + (USHORT)pEid->Len) <= LengthVIE)
+		{
+			switch(pEid->Eid)
+			{
+				case IE_WPA:
+					if (NdisEqualMemory(pEid->Octet, WPA_OUI, 4))
+					{
+						if ((pEid->Len + 2) > MAX_CUSTOM_LEN)
+						{
+							pBss->WpaIE.IELen = 0;
+							break;
+						}
+						pBss->WpaIE.IELen = pEid->Len + 2;
+						NdisMoveMemory(pBss->WpaIE.IE, pEid, pBss->WpaIE.IELen);
+					}
+					break;
+                case IE_RSN:
+                    if (NdisEqualMemory(pEid->Octet + 2, RSN_OUI, 3))
+					{
+						if ((pEid->Len + 2) > MAX_CUSTOM_LEN)
+						{
+							pBss->RsnIE.IELen = 0;
+							break;
+						}
+						pBss->RsnIE.IELen = pEid->Len + 2;
+						NdisMoveMemory(pBss->RsnIE.IE, pEid, pBss->RsnIE.IELen);
+			}
+				break;
+#ifdef EXT_BUILD_CHANNEL_LIST
+				case IE_COUNTRY:
+					NdisMoveMemory(&pBss->CountryString[0], pEid->Octet, 3);
+					pBss->bHasCountryIE = TRUE;
+					break;
+#endif // EXT_BUILD_CHANNEL_LIST //
+            }
+			Length = Length + 2 + (USHORT)pEid->Len;  // Eid[1] + Len[1]+ content[Len]
+			pEid = (PEID_STRUCT)((UCHAR*)pEid + 2 + pEid->Len);
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+}
+
+/*!
+ *	\brief insert an entry into the bss table
+ *	\param p_tab The BSS table
+ *	\param Bssid BSSID
+ *	\param ssid SSID
+ *	\param ssid_len Length of SSID
+ *	\param bss_type
+ *	\param beacon_period
+ *	\param timestamp
+ *	\param p_cf
+ *	\param atim_win
+ *	\param cap
+ *	\param rates
+ *	\param rates_len
+ *	\param channel_idx
+ *	\return none
+ *	\pre
+ *	\post
+ *	\note If SSID is identical, the old entry will be replaced by the new one
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+ULONG BssTableSetEntry(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT BSS_TABLE *Tab,
+	IN PUCHAR pBssid,
+	IN CHAR Ssid[],
+	IN UCHAR SsidLen,
+	IN UCHAR BssType,
+	IN USHORT BeaconPeriod,
+	IN CF_PARM *CfParm,
+	IN USHORT AtimWin,
+	IN USHORT CapabilityInfo,
+	IN UCHAR SupRate[],
+	IN UCHAR SupRateLen,
+	IN UCHAR ExtRate[],
+	IN UCHAR ExtRateLen,
+	IN HT_CAPABILITY_IE *pHtCapability,
+	IN ADD_HT_INFO_IE *pAddHtInfo,	// AP might use this additional ht info IE
+	IN UCHAR			HtCapabilityLen,
+	IN UCHAR			AddHtInfoLen,
+	IN UCHAR			NewExtChanOffset,
+	IN UCHAR ChannelNo,
+	IN CHAR Rssi,
+	IN LARGE_INTEGER TimeStamp,
+	IN UCHAR CkipFlag,
+	IN PEDCA_PARM pEdcaParm,
+	IN PQOS_CAPABILITY_PARM pQosCapability,
+	IN PQBSS_LOAD_PARM pQbssLoad,
+	IN USHORT LengthVIE,
+	IN PNDIS_802_11_VARIABLE_IEs pVIE)
+{
+	ULONG	Idx;
+
+	Idx = BssTableSearchWithSSID(Tab, pBssid,  Ssid, SsidLen, ChannelNo);
+	if (Idx == BSS_NOT_FOUND)
+	{
+		if (Tab->BssNr >= MAX_LEN_OF_BSS_TABLE)
+	    {
+			//
+			// It may happen when BSS Table was full.
+			// The desired AP will not be added into BSS Table
+			// In this case, if we found the desired AP then overwrite BSS Table.
+			//
+			if(!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+			{
+				if (MAC_ADDR_EQUAL(pAd->MlmeAux.Bssid, pBssid) ||
+					SSID_EQUAL(pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen, Ssid, SsidLen))
+				{
+					Idx = Tab->BssOverlapNr;
+					BssEntrySet(pAd, &Tab->BssEntry[Idx], pBssid, Ssid, SsidLen, BssType, BeaconPeriod, CfParm, AtimWin,
+						CapabilityInfo, SupRate, SupRateLen, ExtRate, ExtRateLen,pHtCapability, pAddHtInfo,HtCapabilityLen, AddHtInfoLen,
+						NewExtChanOffset, ChannelNo, Rssi, TimeStamp, CkipFlag, pEdcaParm, pQosCapability, pQbssLoad, LengthVIE, pVIE);
+                    Tab->BssOverlapNr = (Tab->BssOverlapNr++) % MAX_LEN_OF_BSS_TABLE;
+				}
+				return Idx;
+			}
+			else
+			{
+			return BSS_NOT_FOUND;
+			}
+		}
+		Idx = Tab->BssNr;
+		BssEntrySet(pAd, &Tab->BssEntry[Idx], pBssid, Ssid, SsidLen, BssType, BeaconPeriod, CfParm, AtimWin,
+					CapabilityInfo, SupRate, SupRateLen, ExtRate, ExtRateLen,pHtCapability, pAddHtInfo,HtCapabilityLen, AddHtInfoLen,
+					NewExtChanOffset, ChannelNo, Rssi, TimeStamp, CkipFlag, pEdcaParm, pQosCapability, pQbssLoad, LengthVIE, pVIE);
+		Tab->BssNr++;
+	}
+	else
+	{
+		BssEntrySet(pAd, &Tab->BssEntry[Idx], pBssid, Ssid, SsidLen, BssType, BeaconPeriod,CfParm, AtimWin,
+					CapabilityInfo, SupRate, SupRateLen, ExtRate, ExtRateLen,pHtCapability, pAddHtInfo,HtCapabilityLen, AddHtInfoLen,
+					NewExtChanOffset, ChannelNo, Rssi, TimeStamp, CkipFlag, pEdcaParm, pQosCapability, pQbssLoad, LengthVIE, pVIE);
+	}
+
+	return Idx;
+}
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef DOT11_N_SUPPORT
+#ifdef DOT11N_DRAFT3
+VOID  TriEventInit(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	UCHAR		i;
+
+	for (i = 0;i < MAX_TRIGGER_EVENT;i++)
+		pAd->CommonCfg.TriggerEventTab.EventA[i].bValid = FALSE;
+
+	pAd->CommonCfg.TriggerEventTab.EventANo = 0;
+	pAd->CommonCfg.TriggerEventTab.EventBCountDown = 0;
+}
+
+ULONG TriEventTableSetEntry(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT TRIGGER_EVENT_TAB *Tab,
+	IN PUCHAR pBssid,
+	IN HT_CAPABILITY_IE *pHtCapability,
+	IN UCHAR			HtCapabilityLen,
+	IN UCHAR			RegClass,
+	IN UCHAR ChannelNo)
+{
+	// Event A
+	if (HtCapabilityLen == 0)
+	{
+		if (Tab->EventANo < MAX_TRIGGER_EVENT)
+		{
+			RTMPMoveMemory(Tab->EventA[Tab->EventANo].BSSID, pBssid, 6);
+			Tab->EventA[Tab->EventANo].bValid = TRUE;
+			Tab->EventA[Tab->EventANo].Channel = ChannelNo;
+			Tab->EventA[Tab->EventANo].CDCounter = pAd->CommonCfg.Dot11BssWidthChanTranDelay;
+			if (RegClass != 0)
+			{
+				// Beacon has Regulatory class IE. So use beacon's
+				Tab->EventA[Tab->EventANo].RegClass = RegClass;
+			}
+			else
+			{
+				// Use Station's Regulatory class instead.
+				if (pAd->StaActive.SupportedHtPhy.bHtEnable == TRUE)
+				{
+					if (pAd->CommonCfg.CentralChannel > pAd->CommonCfg.Channel)
+					{
+						Tab->EventA[Tab->EventANo].RegClass = 32;
+					}
+					else if (pAd->CommonCfg.CentralChannel < pAd->CommonCfg.Channel)
+						Tab->EventA[Tab->EventANo].RegClass = 33;
+				}
+				else
+					Tab->EventA[Tab->EventANo].RegClass = ??;
+
+			}
+
+			Tab->EventANo ++;
+		}
+	}
+	else if (pHtCapability->HtCapInfo.Intolerant40)
+	{
+		Tab->EventBCountDown = pAd->CommonCfg.Dot11BssWidthChanTranDelay;
+	}
+
+}
+
+/*
+	========================================================================
+	Routine Description:
+		Trigger Event table Maintainence called once every second.
+
+	Arguments:
+	// IRQL = DISPATCH_LEVEL
+	========================================================================
+*/
+VOID TriEventCounterMaintenance(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	UCHAR		i;
+	BOOLEAN			bNotify = FALSE;
+	for (i = 0;i < MAX_TRIGGER_EVENT;i++)
+	{
+		if (pAd->CommonCfg.TriggerEventTab.EventA[i].bValid && (pAd->CommonCfg.TriggerEventTab.EventA[i].CDCounter > 0))
+		{
+			pAd->CommonCfg.TriggerEventTab.EventA[i].CDCounter--;
+			if (pAd->CommonCfg.TriggerEventTab.EventA[i].CDCounter == 0)
+			{
+				pAd->CommonCfg.TriggerEventTab.EventA[i].bValid = FALSE;
+				pAd->CommonCfg.TriggerEventTab.EventANo --;
+				// Need to send 20/40 Coexistence Notify frame if has status change.
+				bNotify = TRUE;
+			}
+		}
+	}
+	if (pAd->CommonCfg.TriggerEventTab.EventBCountDown > 0)
+	{
+		pAd->CommonCfg.TriggerEventTab.EventBCountDown--;
+		if (pAd->CommonCfg.TriggerEventTab.EventBCountDown == 0)
+			bNotify = TRUE;
+	}
+
+	if (bNotify == TRUE)
+		Update2040CoexistFrameAndNotify(pAd, BSSID_WCID, TRUE);
+}
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+
+// IRQL = DISPATCH_LEVEL
+VOID BssTableSsidSort(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT BSS_TABLE *OutTab,
+	IN	CHAR Ssid[],
+	IN	UCHAR SsidLen)
+{
+	INT i;
+	BssTableInit(OutTab);
+
+	for (i = 0; i < pAd->ScanTab.BssNr; i++)
+	{
+		BSS_ENTRY *pInBss = &pAd->ScanTab.BssEntry[i];
+		BOOLEAN	bIsHiddenApIncluded = FALSE;
+
+		if (((pAd->CommonCfg.bIEEE80211H == 1) &&
+            (pAd->MlmeAux.Channel > 14) &&
+             RadarChannelCheck(pAd, pInBss->Channel))
+#ifdef CARRIER_DETECTION_SUPPORT // Roger sync Carrier
+             || (pAd->CommonCfg.CarrierDetect.Enable == TRUE)
+#endif // CARRIER_DETECTION_SUPPORT //
+            )
+		{
+			if (pInBss->Hidden)
+				bIsHiddenApIncluded = TRUE;
+		}
+
+		if ((pInBss->BssType == pAd->StaCfg.BssType) &&
+			(SSID_EQUAL(Ssid, SsidLen, pInBss->Ssid, pInBss->SsidLen) || bIsHiddenApIncluded))
+		{
+			BSS_ENTRY *pOutBss = &OutTab->BssEntry[OutTab->BssNr];
+
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+			// If no Country IE exists no Connection will be established when IEEE80211dClientMode is strict.
+			if ((pAd->StaCfg.IEEE80211dClientMode == Rt802_11_D_Strict) &&
+				(pInBss->bHasCountryIE == FALSE))
+			{
+				DBGPRINT(RT_DEBUG_TRACE,("StaCfg.IEEE80211dClientMode == Rt802_11_D_Strict, but this AP doesn't have country IE.\n"));
+				continue;
+			}
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+#ifdef DOT11_N_SUPPORT
+			// 2.4G/5G N only mode
+			if ((pInBss->HtCapabilityLen == 0) &&
+				((pAd->CommonCfg.PhyMode == PHY_11N_2_4G) || (pAd->CommonCfg.PhyMode == PHY_11N_5G)))
+			{
+				DBGPRINT(RT_DEBUG_TRACE,("STA is in N-only Mode, this AP don't have Ht capability in Beacon.\n"));
+				continue;
+			}
+#endif // DOT11_N_SUPPORT //
+
+			// New for WPA2
+			// Check the Authmode first
+			if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+			{
+				// Check AuthMode and AuthModeAux for matching, in case AP support dual-mode
+				if ((pAd->StaCfg.AuthMode != pInBss->AuthMode) && (pAd->StaCfg.AuthMode != pInBss->AuthModeAux))
+					// None matched
+					continue;
+
+				// Check cipher suite, AP must have more secured cipher than station setting
+				if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK))
+				{
+					// If it's not mixed mode, we should only let BSS pass with the same encryption
+					if (pInBss->WPA.bMixMode == FALSE)
+						if (pAd->StaCfg.WepStatus != pInBss->WPA.GroupCipher)
+							continue;
+
+					// check group cipher
+					if ((pAd->StaCfg.WepStatus < pInBss->WPA.GroupCipher) &&
+						(pInBss->WPA.GroupCipher != Ndis802_11GroupWEP40Enabled) &&
+						(pInBss->WPA.GroupCipher != Ndis802_11GroupWEP104Enabled))
+						continue;
+
+					// check pairwise cipher, skip if none matched
+					// If profile set to AES, let it pass without question.
+					// If profile set to TKIP, we must find one mateched
+					if ((pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA.PairCipher) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA.PairCipherAux))
+						continue;
+				}
+				else if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK))
+				{
+					// If it's not mixed mode, we should only let BSS pass with the same encryption
+					if (pInBss->WPA2.bMixMode == FALSE)
+						if (pAd->StaCfg.WepStatus != pInBss->WPA2.GroupCipher)
+							continue;
+
+					// check group cipher
+					if ((pAd->StaCfg.WepStatus < pInBss->WPA.GroupCipher) &&
+						(pInBss->WPA2.GroupCipher != Ndis802_11GroupWEP40Enabled) &&
+						(pInBss->WPA2.GroupCipher != Ndis802_11GroupWEP104Enabled))
+						continue;
+
+					// check pairwise cipher, skip if none matched
+					// If profile set to AES, let it pass without question.
+					// If profile set to TKIP, we must find one mateched
+					if ((pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA2.PairCipher) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA2.PairCipherAux))
+						continue;
+				}
+			}
+			// Bss Type matched, SSID matched.
+			// We will check wepstatus for qualification Bss
+			else if (pAd->StaCfg.WepStatus != pInBss->WepStatus)
+			{
+				DBGPRINT(RT_DEBUG_TRACE,("StaCfg.WepStatus=%d, while pInBss->WepStatus=%d\n", pAd->StaCfg.WepStatus, pInBss->WepStatus));
+				//
+				// For the SESv2 case, we will not qualify WepStatus.
+				//
+				if (!pInBss->bSES)
+					continue;
+			}
+
+			// Since the AP is using hidden SSID, and we are trying to connect to ANY
+			// It definitely will fail. So, skip it.
+			// CCX also require not even try to connect it!!
+			if (SsidLen == 0)
+				continue;
+
+#ifdef DOT11_N_SUPPORT
+			// If both station and AP use 40MHz, still need to check if the 40MHZ band's legality in my country region
+			// If this 40MHz wideband is not allowed in my country list, use bandwidth 20MHZ instead,
+			if ((pInBss->CentralChannel != pInBss->Channel) &&
+				(pAd->CommonCfg.RegTransmitSetting.field.BW == BW_40))
+			{
+				if (RTMPCheckChannel(pAd, pInBss->CentralChannel, pInBss->Channel) == FALSE)
+				{
+					pAd->CommonCfg.RegTransmitSetting.field.BW = BW_20;
+					SetCommonHT(pAd);
+					pAd->CommonCfg.RegTransmitSetting.field.BW = BW_40;
+				}
+				else
+				{
+					if (pAd->CommonCfg.DesiredHtPhy.ChannelWidth == BAND_WIDTH_20)
+					{
+						SetCommonHT(pAd);
+					}
+				}
+			}
+#endif // DOT11_N_SUPPORT //
+
+			// copy matching BSS from InTab to OutTab
+			NdisMoveMemory(pOutBss, pInBss, sizeof(BSS_ENTRY));
+
+			OutTab->BssNr++;
+		}
+		else if ((pInBss->BssType == pAd->StaCfg.BssType) && (SsidLen == 0))
+		{
+			BSS_ENTRY *pOutBss = &OutTab->BssEntry[OutTab->BssNr];
+
+
+#ifdef DOT11_N_SUPPORT
+			// 2.4G/5G N only mode
+			if ((pInBss->HtCapabilityLen == 0) &&
+				((pAd->CommonCfg.PhyMode == PHY_11N_2_4G) || (pAd->CommonCfg.PhyMode == PHY_11N_5G)))
+			{
+				DBGPRINT(RT_DEBUG_TRACE,("STA is in N-only Mode, this AP don't have Ht capability in Beacon.\n"));
+				continue;
+			}
+#endif // DOT11_N_SUPPORT //
+
+			// New for WPA2
+			// Check the Authmode first
+			if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+			{
+				// Check AuthMode and AuthModeAux for matching, in case AP support dual-mode
+				if ((pAd->StaCfg.AuthMode != pInBss->AuthMode) && (pAd->StaCfg.AuthMode != pInBss->AuthModeAux))
+					// None matched
+					continue;
+
+				// Check cipher suite, AP must have more secured cipher than station setting
+				if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK))
+				{
+					// If it's not mixed mode, we should only let BSS pass with the same encryption
+					if (pInBss->WPA.bMixMode == FALSE)
+						if (pAd->StaCfg.WepStatus != pInBss->WPA.GroupCipher)
+							continue;
+
+					// check group cipher
+					if (pAd->StaCfg.WepStatus < pInBss->WPA.GroupCipher)
+						continue;
+
+					// check pairwise cipher, skip if none matched
+					// If profile set to AES, let it pass without question.
+					// If profile set to TKIP, we must find one mateched
+					if ((pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA.PairCipher) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA.PairCipherAux))
+						continue;
+				}
+				else if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK))
+				{
+					// If it's not mixed mode, we should only let BSS pass with the same encryption
+					if (pInBss->WPA2.bMixMode == FALSE)
+						if (pAd->StaCfg.WepStatus != pInBss->WPA2.GroupCipher)
+							continue;
+
+					// check group cipher
+					if (pAd->StaCfg.WepStatus < pInBss->WPA2.GroupCipher)
+						continue;
+
+					// check pairwise cipher, skip if none matched
+					// If profile set to AES, let it pass without question.
+					// If profile set to TKIP, we must find one mateched
+					if ((pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA2.PairCipher) &&
+						(pAd->StaCfg.WepStatus != pInBss->WPA2.PairCipherAux))
+						continue;
+				}
+			}
+			// Bss Type matched, SSID matched.
+			// We will check wepstatus for qualification Bss
+			else if (pAd->StaCfg.WepStatus != pInBss->WepStatus)
+					continue;
+
+#ifdef DOT11_N_SUPPORT
+			// If both station and AP use 40MHz, still need to check if the 40MHZ band's legality in my country region
+			// If this 40MHz wideband is not allowed in my country list, use bandwidth 20MHZ instead,
+			if ((pInBss->CentralChannel != pInBss->Channel) &&
+				(pAd->CommonCfg.RegTransmitSetting.field.BW == BW_40))
+			{
+				if (RTMPCheckChannel(pAd, pInBss->CentralChannel, pInBss->Channel) == FALSE)
+				{
+					pAd->CommonCfg.RegTransmitSetting.field.BW = BW_20;
+					SetCommonHT(pAd);
+					pAd->CommonCfg.RegTransmitSetting.field.BW = BW_40;
+				}
+			}
+#endif // DOT11_N_SUPPORT //
+
+			// copy matching BSS from InTab to OutTab
+			NdisMoveMemory(pOutBss, pInBss, sizeof(BSS_ENTRY));
+
+			OutTab->BssNr++;
+		}
+
+		if (OutTab->BssNr >= MAX_LEN_OF_BSS_TABLE)
+			break;
+	}
+
+	BssTableSortByRssi(OutTab);
+}
+
+
+// IRQL = DISPATCH_LEVEL
+VOID BssTableSortByRssi(
+	IN OUT BSS_TABLE *OutTab)
+{
+	INT 	  i, j;
+	BSS_ENTRY TmpBss;
+
+	for (i = 0; i < OutTab->BssNr - 1; i++)
+	{
+		for (j = i+1; j < OutTab->BssNr; j++)
+		{
+			if (OutTab->BssEntry[j].Rssi > OutTab->BssEntry[i].Rssi)
+			{
+				NdisMoveMemory(&TmpBss, &OutTab->BssEntry[j], sizeof(BSS_ENTRY));
+				NdisMoveMemory(&OutTab->BssEntry[j], &OutTab->BssEntry[i], sizeof(BSS_ENTRY));
+				NdisMoveMemory(&OutTab->BssEntry[i], &TmpBss, sizeof(BSS_ENTRY));
+			}
+		}
+	}
+}
+#endif // CONFIG_STA_SUPPORT //
+
+
+VOID BssCipherParse(
+	IN OUT	PBSS_ENTRY	pBss)
+{
+	PEID_STRUCT 		 pEid;
+	PUCHAR				pTmp;
+	PRSN_IE_HEADER_STRUCT			pRsnHeader;
+	PCIPHER_SUITE_STRUCT			pCipher;
+	PAKM_SUITE_STRUCT				pAKM;
+	USHORT							Count;
+	INT								Length;
+	NDIS_802_11_ENCRYPTION_STATUS	TmpCipher;
+
+	//
+	// WepStatus will be reset later, if AP announce TKIP or AES on the beacon frame.
+	//
+	if (pBss->Privacy)
+	{
+		pBss->WepStatus 	= Ndis802_11WEPEnabled;
+	}
+	else
+	{
+		pBss->WepStatus 	= Ndis802_11WEPDisabled;
+	}
+	// Set default to disable & open authentication before parsing variable IE
+	pBss->AuthMode		= Ndis802_11AuthModeOpen;
+	pBss->AuthModeAux	= Ndis802_11AuthModeOpen;
+
+	// Init WPA setting
+	pBss->WPA.PairCipher	= Ndis802_11WEPDisabled;
+	pBss->WPA.PairCipherAux = Ndis802_11WEPDisabled;
+	pBss->WPA.GroupCipher	= Ndis802_11WEPDisabled;
+	pBss->WPA.RsnCapability = 0;
+	pBss->WPA.bMixMode		= FALSE;
+
+	// Init WPA2 setting
+	pBss->WPA2.PairCipher	 = Ndis802_11WEPDisabled;
+	pBss->WPA2.PairCipherAux = Ndis802_11WEPDisabled;
+	pBss->WPA2.GroupCipher	 = Ndis802_11WEPDisabled;
+	pBss->WPA2.RsnCapability = 0;
+	pBss->WPA2.bMixMode 	 = FALSE;
+
+
+	Length = (INT) pBss->VarIELen;
+
+	while (Length > 0)
+	{
+		// Parse cipher suite base on WPA1 & WPA2, they should be parsed differently
+		pTmp = ((PUCHAR) pBss->VarIEs) + pBss->VarIELen - Length;
+		pEid = (PEID_STRUCT) pTmp;
+		switch (pEid->Eid)
+		{
+			case IE_WPA:
+				//Parse Cisco IE_WPA (LEAP, CCKM, etc.)
+				if ( NdisEqualMemory((pTmp+8), CISCO_OUI, 3))
+				{
+					pTmp   += 11;
+					switch (*pTmp)
+					{
+						case 1:
+						case 5:	// Although WEP is not allowed in WPA related auth mode, we parse it anyway
+							pBss->WepStatus = Ndis802_11Encryption1Enabled;
+							pBss->WPA.PairCipher = Ndis802_11Encryption1Enabled;
+							pBss->WPA.GroupCipher = Ndis802_11Encryption1Enabled;
+							break;
+						case 2:
+							pBss->WepStatus = Ndis802_11Encryption2Enabled;
+							pBss->WPA.PairCipher = Ndis802_11Encryption1Enabled;
+							pBss->WPA.GroupCipher = Ndis802_11Encryption1Enabled;
+							break;
+						case 4:
+							pBss->WepStatus = Ndis802_11Encryption3Enabled;
+							pBss->WPA.PairCipher = Ndis802_11Encryption1Enabled;
+							pBss->WPA.GroupCipher = Ndis802_11Encryption1Enabled;
+							break;
+						default:
+							break;
+					}
+
+					// if Cisco IE_WPA, break
+					break;
+				}
+				else if (NdisEqualMemory(pEid->Octet, SES_OUI, 3) && (pEid->Len == 7))
+				{
+					pBss->bSES = TRUE;
+					break;
+				}
+				else if (NdisEqualMemory(pEid->Octet, WPA_OUI, 4) != 1)
+				{
+					// if unsupported vendor specific IE
+					break;
+				}
+				// Skip OUI, version, and multicast suite
+				// This part should be improved in the future when AP supported multiple cipher suite.
+				// For now, it's OK since almost all APs have fixed cipher suite supported.
+				// pTmp = (PUCHAR) pEid->Octet;
+				pTmp   += 11;
+
+				// Cipher Suite Selectors from Spec P802.11i/D3.2 P26.
+				//	Value	   Meaning
+				//	0			None
+				//	1			WEP-40
+				//	2			Tkip
+				//	3			WRAP
+				//	4			AES
+				//	5			WEP-104
+				// Parse group cipher
+				switch (*pTmp)
+				{
+					case 1:
+						pBss->WPA.GroupCipher = Ndis802_11GroupWEP40Enabled;
+						break;
+					case 5:
+						pBss->WPA.GroupCipher = Ndis802_11GroupWEP104Enabled;
+						break;
+					case 2:
+						pBss->WPA.GroupCipher = Ndis802_11Encryption2Enabled;
+						break;
+					case 4:
+						pBss->WPA.GroupCipher = Ndis802_11Encryption3Enabled;
+						break;
+					default:
+						break;
+				}
+				// number of unicast suite
+				pTmp   += 1;
+
+				// skip all unicast cipher suites
+				//Count = *(PUSHORT) pTmp;
+				Count = (pTmp[1]<<8) + pTmp[0];
+				pTmp   += sizeof(USHORT);
+
+				// Parsing all unicast cipher suite
+				while (Count > 0)
+				{
+					// Skip OUI
+					pTmp += 3;
+					TmpCipher = Ndis802_11WEPDisabled;
+					switch (*pTmp)
+					{
+						case 1:
+						case 5: // Although WEP is not allowed in WPA related auth mode, we parse it anyway
+							TmpCipher = Ndis802_11Encryption1Enabled;
+							break;
+						case 2:
+							TmpCipher = Ndis802_11Encryption2Enabled;
+							break;
+						case 4:
+							TmpCipher = Ndis802_11Encryption3Enabled;
+							break;
+						default:
+							break;
+					}
+					if (TmpCipher > pBss->WPA.PairCipher)
+					{
+						// Move the lower cipher suite to PairCipherAux
+						pBss->WPA.PairCipherAux = pBss->WPA.PairCipher;
+						pBss->WPA.PairCipher	= TmpCipher;
+					}
+					else
+					{
+						pBss->WPA.PairCipherAux = TmpCipher;
+					}
+					pTmp++;
+					Count--;
+				}
+
+				// 4. get AKM suite counts
+				//Count	= *(PUSHORT) pTmp;
+				Count = (pTmp[1]<<8) + pTmp[0];
+				pTmp   += sizeof(USHORT);
+				pTmp   += 3;
+
+				switch (*pTmp)
+				{
+					case 1:
+						// Set AP support WPA mode
+						if (pBss->AuthMode == Ndis802_11AuthModeOpen)
+							pBss->AuthMode = Ndis802_11AuthModeWPA;
+						else
+							pBss->AuthModeAux = Ndis802_11AuthModeWPA;
+						break;
+					case 2:
+						// Set AP support WPA mode
+						if (pBss->AuthMode == Ndis802_11AuthModeOpen)
+							pBss->AuthMode = Ndis802_11AuthModeWPAPSK;
+						else
+							pBss->AuthModeAux = Ndis802_11AuthModeWPAPSK;
+						break;
+					default:
+						break;
+				}
+				pTmp   += 1;
+
+				// Fixed for WPA-None
+				if (pBss->BssType == BSS_ADHOC)
+				{
+					pBss->AuthMode	  = Ndis802_11AuthModeWPANone;
+					pBss->AuthModeAux = Ndis802_11AuthModeWPANone;
+					pBss->WepStatus   = pBss->WPA.GroupCipher;
+					// Patched bugs for old driver
+					if (pBss->WPA.PairCipherAux == Ndis802_11WEPDisabled)
+						pBss->WPA.PairCipherAux = pBss->WPA.GroupCipher;
+				}
+				else
+					pBss->WepStatus   = pBss->WPA.PairCipher;
+
+				// Check the Pair & Group, if different, turn on mixed mode flag
+				if (pBss->WPA.GroupCipher != pBss->WPA.PairCipher)
+					pBss->WPA.bMixMode = TRUE;
+
+				break;
+
+			case IE_RSN:
+				pRsnHeader = (PRSN_IE_HEADER_STRUCT) pTmp;
+
+				// 0. Version must be 1
+				if (le2cpu16(pRsnHeader->Version) != 1)
+					break;
+				pTmp   += sizeof(RSN_IE_HEADER_STRUCT);
+
+				// 1. Check group cipher
+				pCipher = (PCIPHER_SUITE_STRUCT) pTmp;
+				if (!RTMPEqualMemory(pTmp, RSN_OUI, 3))
+					break;
+
+				// Parse group cipher
+				switch (pCipher->Type)
+				{
+					case 1:
+						pBss->WPA2.GroupCipher = Ndis802_11GroupWEP40Enabled;
+						break;
+					case 5:
+						pBss->WPA2.GroupCipher = Ndis802_11GroupWEP104Enabled;
+						break;
+					case 2:
+						pBss->WPA2.GroupCipher = Ndis802_11Encryption2Enabled;
+						break;
+					case 4:
+						pBss->WPA2.GroupCipher = Ndis802_11Encryption3Enabled;
+						break;
+					default:
+						break;
+				}
+				// set to correct offset for next parsing
+				pTmp   += sizeof(CIPHER_SUITE_STRUCT);
+
+				// 2. Get pairwise cipher counts
+				//Count = *(PUSHORT) pTmp;
+				Count = (pTmp[1]<<8) + pTmp[0];
+				pTmp   += sizeof(USHORT);
+
+				// 3. Get pairwise cipher
+				// Parsing all unicast cipher suite
+				while (Count > 0)
+				{
+					// Skip OUI
+					pCipher = (PCIPHER_SUITE_STRUCT) pTmp;
+					TmpCipher = Ndis802_11WEPDisabled;
+					switch (pCipher->Type)
+					{
+						case 1:
+						case 5: // Although WEP is not allowed in WPA related auth mode, we parse it anyway
+							TmpCipher = Ndis802_11Encryption1Enabled;
+							break;
+						case 2:
+							TmpCipher = Ndis802_11Encryption2Enabled;
+							break;
+						case 4:
+							TmpCipher = Ndis802_11Encryption3Enabled;
+							break;
+						default:
+							break;
+					}
+					if (TmpCipher > pBss->WPA2.PairCipher)
+					{
+						// Move the lower cipher suite to PairCipherAux
+						pBss->WPA2.PairCipherAux = pBss->WPA2.PairCipher;
+						pBss->WPA2.PairCipher	 = TmpCipher;
+					}
+					else
+					{
+						pBss->WPA2.PairCipherAux = TmpCipher;
+					}
+					pTmp += sizeof(CIPHER_SUITE_STRUCT);
+					Count--;
+				}
+
+				// 4. get AKM suite counts
+				//Count	= *(PUSHORT) pTmp;
+				Count = (pTmp[1]<<8) + pTmp[0];
+				pTmp   += sizeof(USHORT);
+
+				// 5. Get AKM ciphers
+				pAKM = (PAKM_SUITE_STRUCT) pTmp;
+				if (!RTMPEqualMemory(pTmp, RSN_OUI, 3))
+					break;
+
+				switch (pAKM->Type)
+				{
+					case 1:
+						// Set AP support WPA mode
+						if (pBss->AuthMode == Ndis802_11AuthModeOpen)
+							pBss->AuthMode = Ndis802_11AuthModeWPA2;
+						else
+							pBss->AuthModeAux = Ndis802_11AuthModeWPA2;
+						break;
+					case 2:
+						// Set AP support WPA mode
+						if (pBss->AuthMode == Ndis802_11AuthModeOpen)
+							pBss->AuthMode = Ndis802_11AuthModeWPA2PSK;
+						else
+							pBss->AuthModeAux = Ndis802_11AuthModeWPA2PSK;
+						break;
+					default:
+						break;
+				}
+				pTmp   += (Count * sizeof(AKM_SUITE_STRUCT));
+
+				// Fixed for WPA-None
+				if (pBss->BssType == BSS_ADHOC)
+				{
+					pBss->AuthMode = Ndis802_11AuthModeWPANone;
+					pBss->AuthModeAux = Ndis802_11AuthModeWPANone;
+					pBss->WPA.PairCipherAux = pBss->WPA2.PairCipherAux;
+					pBss->WPA.GroupCipher	= pBss->WPA2.GroupCipher;
+					pBss->WepStatus 		= pBss->WPA.GroupCipher;
+					// Patched bugs for old driver
+					if (pBss->WPA.PairCipherAux == Ndis802_11WEPDisabled)
+						pBss->WPA.PairCipherAux = pBss->WPA.GroupCipher;
+				}
+				pBss->WepStatus   = pBss->WPA2.PairCipher;
+
+				// 6. Get RSN capability
+				//pBss->WPA2.RsnCapability = *(PUSHORT) pTmp;
+				pBss->WPA2.RsnCapability = (pTmp[1]<<8) + pTmp[0];
+				pTmp += sizeof(USHORT);
+
+				// Check the Pair & Group, if different, turn on mixed mode flag
+				if (pBss->WPA2.GroupCipher != pBss->WPA2.PairCipher)
+					pBss->WPA2.bMixMode = TRUE;
+
+				break;
+			default:
+				break;
+		}
+		Length -= (pEid->Len + 2);
+	}
+}
+
+// ===========================================================================================
+// mac_table.c
+// ===========================================================================================
+
+/*! \brief generates a random mac address value for IBSS BSSID
+ *	\param Addr the bssid location
+ *	\return none
+ *	\pre
+ *	\post
+ */
+VOID MacAddrRandomBssid(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pAddr)
+{
+	INT i;
+
+	for (i = 0; i < MAC_ADDR_LEN; i++)
+	{
+		pAddr[i] = RandomByte(pAd);
+	}
+
+	pAddr[0] = (pAddr[0] & 0xfe) | 0x02;  // the first 2 bits must be 01xxxxxxxx
+}
+
+/*! \brief init the management mac frame header
+ *	\param p_hdr mac header
+ *	\param subtype subtype of the frame
+ *	\param p_ds destination address, don't care if it is a broadcast address
+ *	\return none
+ *	\pre the station has the following information in the pAd->StaCfg
+ *	 - bssid
+ *	 - station address
+ *	\post
+ *	\note this function initializes the following field
+
+ IRQL = PASSIVE_LEVEL
+ IRQL = DISPATCH_LEVEL
+
+ */
+VOID MgtMacHeaderInit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN OUT PHEADER_802_11 pHdr80211,
+	IN UCHAR SubType,
+	IN UCHAR ToDs,
+	IN PUCHAR pDA,
+	IN PUCHAR pBssid)
+{
+	NdisZeroMemory(pHdr80211, sizeof(HEADER_802_11));
+
+	pHdr80211->FC.Type = BTYPE_MGMT;
+	pHdr80211->FC.SubType = SubType;
+//	if (SubType == SUBTYPE_ACK)	// sample, no use, it will conflict with ACTION frame sub type
+//		pHdr80211->FC.Type = BTYPE_CNTL;
+	pHdr80211->FC.ToDs = ToDs;
+	COPY_MAC_ADDR(pHdr80211->Addr1, pDA);
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		COPY_MAC_ADDR(pHdr80211->Addr2, pAd->CurrentAddress);
+#endif // CONFIG_STA_SUPPORT //
+	COPY_MAC_ADDR(pHdr80211->Addr3, pBssid);
+}
+
+// ===========================================================================================
+// mem_mgmt.c
+// ===========================================================================================
+
+/*!***************************************************************************
+ * This routine build an outgoing frame, and fill all information specified
+ * in argument list to the frame body. The actual frame size is the summation
+ * of all arguments.
+ * input params:
+ *		Buffer - pointer to a pre-allocated memory segment
+ *		args - a list of <int arg_size, arg> pairs.
+ *		NOTE NOTE NOTE!!!! the last argument must be NULL, otherwise this
+ *						   function will FAIL!!!
+ * return:
+ *		Size of the buffer
+ * usage:
+ *		MakeOutgoingFrame(Buffer, output_length, 2, &fc, 2, &dur, 6, p_addr1, 6,p_addr2, END_OF_ARGS);
+
+ IRQL = PASSIVE_LEVEL
+ IRQL = DISPATCH_LEVEL
+
+ ****************************************************************************/
+ULONG MakeOutgoingFrame(
+	OUT CHAR *Buffer,
+	OUT ULONG *FrameLen, ...)
+{
+	CHAR   *p;
+	int 	leng;
+	ULONG	TotLeng;
+	va_list Args;
+
+	// calculates the total length
+	TotLeng = 0;
+	va_start(Args, FrameLen);
+	do
+	{
+		leng = va_arg(Args, int);
+		if (leng == END_OF_ARGS)
+		{
+			break;
+		}
+		p = va_arg(Args, PVOID);
+		NdisMoveMemory(&Buffer[TotLeng], p, leng);
+		TotLeng = TotLeng + leng;
+	} while(TRUE);
+
+	va_end(Args); /* clean up */
+	*FrameLen = TotLeng;
+	return TotLeng;
+}
+
+// ===========================================================================================
+// mlme_queue.c
+// ===========================================================================================
+
+/*! \brief	Initialize The MLME Queue, used by MLME Functions
+ *	\param	*Queue	   The MLME Queue
+ *	\return Always	   Return NDIS_STATE_SUCCESS in this implementation
+ *	\pre
+ *	\post
+ *	\note	Because this is done only once (at the init stage), no need to be locked
+
+ IRQL = PASSIVE_LEVEL
+
+ */
+NDIS_STATUS MlmeQueueInit(
+	IN MLME_QUEUE *Queue)
+{
+	INT i;
+
+	NdisAllocateSpinLock(&Queue->Lock);
+
+	Queue->Num	= 0;
+	Queue->Head = 0;
+	Queue->Tail = 0;
+
+	for (i = 0; i < MAX_LEN_OF_MLME_QUEUE; i++)
+	{
+		Queue->Entry[i].Occupied = FALSE;
+		Queue->Entry[i].MsgLen = 0;
+		NdisZeroMemory(Queue->Entry[i].Msg, MGMT_DMA_BUFFER_SIZE);
+	}
+
+	return NDIS_STATUS_SUCCESS;
+}
+
+/*! \brief	 Enqueue a message for other threads, if they want to send messages to MLME thread
+ *	\param	*Queue	  The MLME Queue
+ *	\param	 Machine  The State Machine Id
+ *	\param	 MsgType  The Message Type
+ *	\param	 MsgLen   The Message length
+ *	\param	*Msg	  The message pointer
+ *	\return  TRUE if enqueue is successful, FALSE if the queue is full
+ *	\pre
+ *	\post
+ *	\note	 The message has to be initialized
+
+ IRQL = PASSIVE_LEVEL
+ IRQL = DISPATCH_LEVEL
+
+ */
+BOOLEAN MlmeEnqueue(
+	IN	PRTMP_ADAPTER	pAd,
+	IN ULONG Machine,
+	IN ULONG MsgType,
+	IN ULONG MsgLen,
+	IN VOID *Msg)
+{
+	INT Tail;
+	MLME_QUEUE	*Queue = (MLME_QUEUE *)&pAd->Mlme.Queue;
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return FALSE;
+
+	// First check the size, it MUST not exceed the mlme queue size
+	if (MsgLen > MGMT_DMA_BUFFER_SIZE)
+	{
+		DBGPRINT_ERR(("MlmeEnqueue: msg too large, size = %ld \n", MsgLen));
+		return FALSE;
+	}
+
+	if (MlmeQueueFull(Queue))
+	{
+		return FALSE;
+	}
+
+	NdisAcquireSpinLock(&(Queue->Lock));
+	Tail = Queue->Tail;
+	Queue->Tail++;
+	Queue->Num++;
+	if (Queue->Tail == MAX_LEN_OF_MLME_QUEUE)
+	{
+		Queue->Tail = 0;
+	}
+
+	Queue->Entry[Tail].Wcid = RESERVED_WCID;
+	Queue->Entry[Tail].Occupied = TRUE;
+	Queue->Entry[Tail].Machine = Machine;
+	Queue->Entry[Tail].MsgType = MsgType;
+	Queue->Entry[Tail].MsgLen  = MsgLen;
+
+	if (Msg != NULL)
+	{
+		NdisMoveMemory(Queue->Entry[Tail].Msg, Msg, MsgLen);
+	}
+
+	NdisReleaseSpinLock(&(Queue->Lock));
+	return TRUE;
+}
+
+/*! \brief	 This function is used when Recv gets a MLME message
+ *	\param	*Queue			 The MLME Queue
+ *	\param	 TimeStampHigh	 The upper 32 bit of timestamp
+ *	\param	 TimeStampLow	 The lower 32 bit of timestamp
+ *	\param	 Rssi			 The receiving RSSI strength
+ *	\param	 MsgLen 		 The length of the message
+ *	\param	*Msg			 The message pointer
+ *	\return  TRUE if everything ok, FALSE otherwise (like Queue Full)
+ *	\pre
+ *	\post
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+BOOLEAN MlmeEnqueueForRecv(
+	IN	PRTMP_ADAPTER	pAd,
+	IN ULONG Wcid,
+	IN ULONG TimeStampHigh,
+	IN ULONG TimeStampLow,
+	IN UCHAR Rssi0,
+	IN UCHAR Rssi1,
+	IN UCHAR Rssi2,
+	IN ULONG MsgLen,
+	IN VOID *Msg,
+	IN UCHAR Signal)
+{
+	INT 		 Tail, Machine;
+	PFRAME_802_11 pFrame = (PFRAME_802_11)Msg;
+	INT		 MsgType;
+	MLME_QUEUE	*Queue = (MLME_QUEUE *)&pAd->Mlme.Queue;
+
+#ifdef RALINK_ATE
+	/* Nothing to do in ATE mode */
+	if(ATE_ON(pAd))
+		return FALSE;
+#endif // RALINK_ATE //
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+	{
+		DBGPRINT_ERR(("MlmeEnqueueForRecv: fRTMP_ADAPTER_HALT_IN_PROGRESS\n"));
+		return FALSE;
+	}
+
+	// First check the size, it MUST not exceed the mlme queue size
+	if (MsgLen > MGMT_DMA_BUFFER_SIZE)
+	{
+		DBGPRINT_ERR(("MlmeEnqueueForRecv: frame too large, size = %ld \n", MsgLen));
+		return FALSE;
+	}
+
+	if (MlmeQueueFull(Queue))
+	{
+		return FALSE;
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (!MsgTypeSubst(pAd, pFrame, &Machine, &MsgType))
+		{
+			DBGPRINT_ERR(("MlmeEnqueueForRecv: un-recongnized mgmt->subtype=%d\n",pFrame->Hdr.FC.SubType));
+			return FALSE;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	// OK, we got all the informations, it is time to put things into queue
+	NdisAcquireSpinLock(&(Queue->Lock));
+	Tail = Queue->Tail;
+	Queue->Tail++;
+	Queue->Num++;
+	if (Queue->Tail == MAX_LEN_OF_MLME_QUEUE)
+	{
+		Queue->Tail = 0;
+	}
+	Queue->Entry[Tail].Occupied = TRUE;
+	Queue->Entry[Tail].Machine = Machine;
+	Queue->Entry[Tail].MsgType = MsgType;
+	Queue->Entry[Tail].MsgLen  = MsgLen;
+	Queue->Entry[Tail].TimeStamp.u.LowPart = TimeStampLow;
+	Queue->Entry[Tail].TimeStamp.u.HighPart = TimeStampHigh;
+	Queue->Entry[Tail].Rssi0 = Rssi0;
+	Queue->Entry[Tail].Rssi1 = Rssi1;
+	Queue->Entry[Tail].Rssi2 = Rssi2;
+	Queue->Entry[Tail].Signal = Signal;
+	Queue->Entry[Tail].Wcid = (UCHAR)Wcid;
+
+	Queue->Entry[Tail].Channel = pAd->LatchRfRegs.Channel;
+
+	if (Msg != NULL)
+	{
+		NdisMoveMemory(Queue->Entry[Tail].Msg, Msg, MsgLen);
+	}
+
+	NdisReleaseSpinLock(&(Queue->Lock));
+
+	RT28XX_MLME_HANDLER(pAd);
+
+	return TRUE;
+}
+
+
+/*! \brief	 Dequeue a message from the MLME Queue
+ *	\param	*Queue	  The MLME Queue
+ *	\param	*Elem	  The message dequeued from MLME Queue
+ *	\return  TRUE if the Elem contains something, FALSE otherwise
+ *	\pre
+ *	\post
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+BOOLEAN MlmeDequeue(
+	IN MLME_QUEUE *Queue,
+	OUT MLME_QUEUE_ELEM **Elem)
+{
+	NdisAcquireSpinLock(&(Queue->Lock));
+	*Elem = &(Queue->Entry[Queue->Head]);
+	Queue->Num--;
+	Queue->Head++;
+	if (Queue->Head == MAX_LEN_OF_MLME_QUEUE)
+	{
+		Queue->Head = 0;
+	}
+	NdisReleaseSpinLock(&(Queue->Lock));
+	return TRUE;
+}
+
+// IRQL = DISPATCH_LEVEL
+VOID	MlmeRestartStateMachine(
+	IN	PRTMP_ADAPTER	pAd)
+{
+#ifdef CONFIG_STA_SUPPORT
+	BOOLEAN				Cancelled;
+#endif // CONFIG_STA_SUPPORT //
+
+	DBGPRINT(RT_DEBUG_TRACE, ("MlmeRestartStateMachine \n"));
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+#ifdef QOS_DLS_SUPPORT
+		UCHAR i;
+#endif // QOS_DLS_SUPPORT //
+		// Cancel all timer events
+		// Be careful to cancel new added timer
+		RTMPCancelTimer(&pAd->MlmeAux.AssocTimer,	  &Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.ReassocTimer,   &Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.DisassocTimer,  &Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.AuthTimer,	   &Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.BeaconTimer,	   &Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.ScanTimer,	   &Cancelled);
+
+#ifdef QOS_DLS_SUPPORT
+		for (i=0; i<MAX_NUM_OF_DLS_ENTRY; i++)
+		{
+			RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &Cancelled);
+		}
+#endif // QOS_DLS_SUPPORT //
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	// Change back to original channel in case of doing scan
+	AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+	AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+
+	// Resume MSDU which is turned off durning scan
+	RTMPResumeMsduTransmission(pAd);
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// Set all state machines back IDLE
+		pAd->Mlme.CntlMachine.CurrState    = CNTL_IDLE;
+		pAd->Mlme.AssocMachine.CurrState   = ASSOC_IDLE;
+		pAd->Mlme.AuthMachine.CurrState    = AUTH_REQ_IDLE;
+		pAd->Mlme.AuthRspMachine.CurrState = AUTH_RSP_IDLE;
+		pAd->Mlme.SyncMachine.CurrState    = SYNC_IDLE;
+		pAd->Mlme.ActMachine.CurrState    = ACT_IDLE;
+#ifdef QOS_DLS_SUPPORT
+		pAd->Mlme.DlsMachine.CurrState    = DLS_IDLE;
+#endif // QOS_DLS_SUPPORT //
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+}
+
+/*! \brief	test if the MLME Queue is empty
+ *	\param	*Queue	  The MLME Queue
+ *	\return TRUE if the Queue is empty, FALSE otherwise
+ *	\pre
+ *	\post
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+BOOLEAN MlmeQueueEmpty(
+	IN MLME_QUEUE *Queue)
+{
+	BOOLEAN Ans;
+
+	NdisAcquireSpinLock(&(Queue->Lock));
+	Ans = (Queue->Num == 0);
+	NdisReleaseSpinLock(&(Queue->Lock));
+
+	return Ans;
+}
+
+/*! \brief	 test if the MLME Queue is full
+ *	\param	 *Queue 	 The MLME Queue
+ *	\return  TRUE if the Queue is empty, FALSE otherwise
+ *	\pre
+ *	\post
+
+ IRQL = PASSIVE_LEVEL
+ IRQL = DISPATCH_LEVEL
+
+ */
+BOOLEAN MlmeQueueFull(
+	IN MLME_QUEUE *Queue)
+{
+	BOOLEAN Ans;
+
+	NdisAcquireSpinLock(&(Queue->Lock));
+	Ans = (Queue->Num == MAX_LEN_OF_MLME_QUEUE || Queue->Entry[Queue->Tail].Occupied);
+	NdisReleaseSpinLock(&(Queue->Lock));
+
+	return Ans;
+}
+
+/*! \brief	 The destructor of MLME Queue
+ *	\param
+ *	\return
+ *	\pre
+ *	\post
+ *	\note	Clear Mlme Queue, Set Queue->Num to Zero.
+
+ IRQL = PASSIVE_LEVEL
+
+ */
+VOID MlmeQueueDestroy(
+	IN MLME_QUEUE *pQueue)
+{
+	NdisAcquireSpinLock(&(pQueue->Lock));
+	pQueue->Num  = 0;
+	pQueue->Head = 0;
+	pQueue->Tail = 0;
+	NdisReleaseSpinLock(&(pQueue->Lock));
+	NdisFreeSpinLock(&(pQueue->Lock));
+}
+
+/*! \brief	 To substitute the message type if the message is coming from external
+ *	\param	pFrame		   The frame received
+ *	\param	*Machine	   The state machine
+ *	\param	*MsgType	   the message type for the state machine
+ *	\return TRUE if the substitution is successful, FALSE otherwise
+ *	\pre
+ *	\post
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+#ifdef CONFIG_STA_SUPPORT
+BOOLEAN MsgTypeSubst(
+	IN PRTMP_ADAPTER  pAd,
+	IN PFRAME_802_11 pFrame,
+	OUT INT *Machine,
+	OUT INT *MsgType)
+{
+	USHORT	Seq;
+	UCHAR	EAPType;
+	PUCHAR	pData;
+
+	// Pointer to start of data frames including SNAP header
+	pData = (PUCHAR) pFrame + LENGTH_802_11;
+
+	// The only data type will pass to this function is EAPOL frame
+	if (pFrame->Hdr.FC.Type == BTYPE_DATA)
+	{
+		if (NdisEqualMemory(SNAP_AIRONET, pData, LENGTH_802_1_H))
+		{
+			// Cisco Aironet SNAP header
+			*Machine = AIRONET_STATE_MACHINE;
+			*MsgType = MT2_AIRONET_MSG;
+			return (TRUE);
+		}
+#ifdef LEAP_SUPPORT
+		if ( pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP ) //LEAP
+		{
+			// LEAP frames
+			*Machine = LEAP_STATE_MACHINE;
+			EAPType = *((UCHAR*)pFrame + LENGTH_802_11 + LENGTH_802_1_H + 1);
+			return (LeapMsgTypeSubst(EAPType, MsgType));
+		}
+		else
+#endif // LEAP_SUPPORT //
+		{
+			*Machine = WPA_PSK_STATE_MACHINE;
+			EAPType = *((UCHAR*)pFrame + LENGTH_802_11 + LENGTH_802_1_H + 1);
+			return(WpaMsgTypeSubst(EAPType, MsgType));
+		}
+	}
+
+	switch (pFrame->Hdr.FC.SubType)
+	{
+		case SUBTYPE_ASSOC_REQ:
+			*Machine = ASSOC_STATE_MACHINE;
+			*MsgType = MT2_PEER_ASSOC_REQ;
+			break;
+		case SUBTYPE_ASSOC_RSP:
+			*Machine = ASSOC_STATE_MACHINE;
+			*MsgType = MT2_PEER_ASSOC_RSP;
+			break;
+		case SUBTYPE_REASSOC_REQ:
+			*Machine = ASSOC_STATE_MACHINE;
+			*MsgType = MT2_PEER_REASSOC_REQ;
+			break;
+		case SUBTYPE_REASSOC_RSP:
+			*Machine = ASSOC_STATE_MACHINE;
+			*MsgType = MT2_PEER_REASSOC_RSP;
+			break;
+		case SUBTYPE_PROBE_REQ:
+			*Machine = SYNC_STATE_MACHINE;
+			*MsgType = MT2_PEER_PROBE_REQ;
+			break;
+		case SUBTYPE_PROBE_RSP:
+			*Machine = SYNC_STATE_MACHINE;
+			*MsgType = MT2_PEER_PROBE_RSP;
+			break;
+		case SUBTYPE_BEACON:
+			*Machine = SYNC_STATE_MACHINE;
+			*MsgType = MT2_PEER_BEACON;
+			break;
+		case SUBTYPE_ATIM:
+			*Machine = SYNC_STATE_MACHINE;
+			*MsgType = MT2_PEER_ATIM;
+			break;
+		case SUBTYPE_DISASSOC:
+			*Machine = ASSOC_STATE_MACHINE;
+			*MsgType = MT2_PEER_DISASSOC_REQ;
+			break;
+		case SUBTYPE_AUTH:
+			// get the sequence number from payload 24 Mac Header + 2 bytes algorithm
+			NdisMoveMemory(&Seq, &pFrame->Octet[2], sizeof(USHORT));
+			if (Seq == 1 || Seq == 3)
+			{
+				*Machine = AUTH_RSP_STATE_MACHINE;
+				*MsgType = MT2_PEER_AUTH_ODD;
+			}
+			else if (Seq == 2 || Seq == 4)
+			{
+				*Machine = AUTH_STATE_MACHINE;
+				*MsgType = MT2_PEER_AUTH_EVEN;
+			}
+			else
+			{
+				return FALSE;
+			}
+			break;
+		case SUBTYPE_DEAUTH:
+			*Machine = AUTH_RSP_STATE_MACHINE;
+			*MsgType = MT2_PEER_DEAUTH;
+			break;
+		case SUBTYPE_ACTION:
+			*Machine = ACTION_STATE_MACHINE;
+			//  Sometimes Sta will return with category bytes with MSB = 1, if they receive catogory out of their support
+			if ((pFrame->Octet[0]&0x7F) > MAX_PEER_CATE_MSG)
+			{
+				*MsgType = MT2_ACT_INVALID;
+			}
+			else
+			{
+				*MsgType = (pFrame->Octet[0]&0x7F);
+			}
+			break;
+		default:
+			return FALSE;
+			break;
+	}
+
+	return TRUE;
+}
+#endif // CONFIG_STA_SUPPORT //
+
+// ===========================================================================================
+// state_machine.c
+// ===========================================================================================
+
+/*! \brief Initialize the state machine.
+ *	\param *S			pointer to the state machine
+ *	\param	Trans		State machine transition function
+ *	\param	StNr		number of states
+ *	\param	MsgNr		number of messages
+ *	\param	DefFunc 	default function, when there is invalid state/message combination
+ *	\param	InitState	initial state of the state machine
+ *	\param	Base		StateMachine base, internal use only
+ *	\pre p_sm should be a legal pointer
+ *	\post
+
+ IRQL = PASSIVE_LEVEL
+
+ */
+VOID StateMachineInit(
+	IN STATE_MACHINE *S,
+	IN STATE_MACHINE_FUNC Trans[],
+	IN ULONG StNr,
+	IN ULONG MsgNr,
+	IN STATE_MACHINE_FUNC DefFunc,
+	IN ULONG InitState,
+	IN ULONG Base)
+{
+	ULONG i, j;
+
+	// set number of states and messages
+	S->NrState = StNr;
+	S->NrMsg   = MsgNr;
+	S->Base    = Base;
+
+	S->TransFunc  = Trans;
+
+	// init all state transition to default function
+	for (i = 0; i < StNr; i++)
+	{
+		for (j = 0; j < MsgNr; j++)
+		{
+			S->TransFunc[i * MsgNr + j] = DefFunc;
+		}
+	}
+
+	// set the starting state
+	S->CurrState = InitState;
+}
+
+/*! \brief This function fills in the function pointer into the cell in the state machine
+ *	\param *S	pointer to the state machine
+ *	\param St	state
+ *	\param Msg	incoming message
+ *	\param f	the function to be executed when (state, message) combination occurs at the state machine
+ *	\pre *S should be a legal pointer to the state machine, st, msg, should be all within the range, Base should be set in the initial state
+ *	\post
+
+ IRQL = PASSIVE_LEVEL
+
+ */
+VOID StateMachineSetAction(
+	IN STATE_MACHINE *S,
+	IN ULONG St,
+	IN ULONG Msg,
+	IN STATE_MACHINE_FUNC Func)
+{
+	ULONG MsgIdx;
+
+	MsgIdx = Msg - S->Base;
+
+	if (St < S->NrState && MsgIdx < S->NrMsg)
+	{
+		// boundary checking before setting the action
+		S->TransFunc[St * S->NrMsg + MsgIdx] = Func;
+	}
+}
+
+/*! \brief	 This function does the state transition
+ *	\param	 *Adapter the NIC adapter pointer
+ *	\param	 *S 	  the state machine
+ *	\param	 *Elem	  the message to be executed
+ *	\return   None
+
+ IRQL = DISPATCH_LEVEL
+
+ */
+VOID StateMachinePerformAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN STATE_MACHINE *S,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	(*(S->TransFunc[S->CurrState * S->NrMsg + Elem->MsgType - S->Base]))(pAd, Elem);
+}
+
+/*
+	==========================================================================
+	Description:
+		The drop function, when machine executes this, the message is simply
+		ignored. This function does nothing, the message is freed in
+		StateMachinePerformAction()
+	==========================================================================
+ */
+VOID Drop(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+}
+
+// ===========================================================================================
+// lfsr.c
+// ===========================================================================================
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = PASSIVE_LEVEL
+
+	==========================================================================
+ */
+VOID LfsrInit(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG Seed)
+{
+	if (Seed == 0)
+		pAd->Mlme.ShiftReg = 1;
+	else
+		pAd->Mlme.ShiftReg = Seed;
+}
+
+/*
+	==========================================================================
+	Description:
+	==========================================================================
+ */
+UCHAR RandomByte(
+	IN PRTMP_ADAPTER pAd)
+{
+	ULONG i;
+	UCHAR R, Result;
+
+	R = 0;
+
+	if (pAd->Mlme.ShiftReg == 0)
+	NdisGetSystemUpTime((ULONG *)&pAd->Mlme.ShiftReg);
+
+	for (i = 0; i < 8; i++)
+	{
+		if (pAd->Mlme.ShiftReg & 0x00000001)
+		{
+			pAd->Mlme.ShiftReg = ((pAd->Mlme.ShiftReg ^ LFSR_MASK) >> 1) | 0x80000000;
+			Result = 1;
+		}
+		else
+		{
+			pAd->Mlme.ShiftReg = pAd->Mlme.ShiftReg >> 1;
+			Result = 0;
+		}
+		R = (R << 1) | Result;
+	}
+
+	return R;
+}
+
+VOID AsicUpdateAutoFallBackTable(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pRateTable)
+{
+	UCHAR					i;
+	HT_FBK_CFG0_STRUC		HtCfg0;
+	HT_FBK_CFG1_STRUC		HtCfg1;
+	LG_FBK_CFG0_STRUC		LgCfg0;
+	LG_FBK_CFG1_STRUC		LgCfg1;
+	PRTMP_TX_RATE_SWITCH	pCurrTxRate, pNextTxRate;
+
+	// set to initial value
+	HtCfg0.word = 0x65432100;
+	HtCfg1.word = 0xedcba988;
+	LgCfg0.word = 0xedcba988;
+	LgCfg1.word = 0x00002100;
+
+	pNextTxRate = (PRTMP_TX_RATE_SWITCH)pRateTable+1;
+	for (i = 1; i < *((PUCHAR) pRateTable); i++)
+	{
+		pCurrTxRate = (PRTMP_TX_RATE_SWITCH)pRateTable+1+i;
+		switch (pCurrTxRate->Mode)
+		{
+			case 0:		//CCK
+				break;
+			case 1:		//OFDM
+				{
+					switch(pCurrTxRate->CurrMCS)
+					{
+						case 0:
+							LgCfg0.field.OFDMMCS0FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+						case 1:
+							LgCfg0.field.OFDMMCS1FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+						case 2:
+							LgCfg0.field.OFDMMCS2FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+						case 3:
+							LgCfg0.field.OFDMMCS3FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+						case 4:
+							LgCfg0.field.OFDMMCS4FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+						case 5:
+							LgCfg0.field.OFDMMCS5FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+						case 6:
+							LgCfg0.field.OFDMMCS6FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+						case 7:
+							LgCfg0.field.OFDMMCS7FBK = (pNextTxRate->Mode == MODE_OFDM) ? (pNextTxRate->CurrMCS+8): pNextTxRate->CurrMCS;
+							break;
+					}
+				}
+				break;
+#ifdef DOT11_N_SUPPORT
+			case 2:		//HT-MIX
+			case 3:		//HT-GF
+				{
+					if ((pNextTxRate->Mode >= MODE_HTMIX) && (pCurrTxRate->CurrMCS != pNextTxRate->CurrMCS))
+					{
+						switch(pCurrTxRate->CurrMCS)
+						{
+							case 0:
+								HtCfg0.field.HTMCS0FBK = pNextTxRate->CurrMCS;
+								break;
+							case 1:
+								HtCfg0.field.HTMCS1FBK = pNextTxRate->CurrMCS;
+								break;
+							case 2:
+								HtCfg0.field.HTMCS2FBK = pNextTxRate->CurrMCS;
+								break;
+							case 3:
+								HtCfg0.field.HTMCS3FBK = pNextTxRate->CurrMCS;
+								break;
+							case 4:
+								HtCfg0.field.HTMCS4FBK = pNextTxRate->CurrMCS;
+								break;
+							case 5:
+								HtCfg0.field.HTMCS5FBK = pNextTxRate->CurrMCS;
+								break;
+							case 6:
+								HtCfg0.field.HTMCS6FBK = pNextTxRate->CurrMCS;
+								break;
+							case 7:
+								HtCfg0.field.HTMCS7FBK = pNextTxRate->CurrMCS;
+								break;
+							case 8:
+								HtCfg1.field.HTMCS8FBK = pNextTxRate->CurrMCS;
+								break;
+							case 9:
+								HtCfg1.field.HTMCS9FBK = pNextTxRate->CurrMCS;
+								break;
+							case 10:
+								HtCfg1.field.HTMCS10FBK = pNextTxRate->CurrMCS;
+								break;
+							case 11:
+								HtCfg1.field.HTMCS11FBK = pNextTxRate->CurrMCS;
+								break;
+							case 12:
+								HtCfg1.field.HTMCS12FBK = pNextTxRate->CurrMCS;
+								break;
+							case 13:
+								HtCfg1.field.HTMCS13FBK = pNextTxRate->CurrMCS;
+								break;
+							case 14:
+								HtCfg1.field.HTMCS14FBK = pNextTxRate->CurrMCS;
+								break;
+							case 15:
+								HtCfg1.field.HTMCS15FBK = pNextTxRate->CurrMCS;
+								break;
+							default:
+								DBGPRINT(RT_DEBUG_ERROR, ("AsicUpdateAutoFallBackTable: not support CurrMCS=%d\n", pCurrTxRate->CurrMCS));
+						}
+					}
+				}
+				break;
+#endif // DOT11_N_SUPPORT //
+		}
+
+		pNextTxRate = pCurrTxRate;
+	}
+
+	RTMP_IO_WRITE32(pAd, HT_FBK_CFG0, HtCfg0.word);
+	RTMP_IO_WRITE32(pAd, HT_FBK_CFG1, HtCfg1.word);
+	RTMP_IO_WRITE32(pAd, LG_FBK_CFG0, LgCfg0.word);
+	RTMP_IO_WRITE32(pAd, LG_FBK_CFG1, LgCfg1.word);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Set MAC register value according operation mode.
+		OperationMode AND bNonGFExist are for MM and GF Proteciton.
+		If MM or GF mask is not set, those passing argument doesn't not take effect.
+
+		Operation mode meaning:
+		= 0 : Pure HT, no preotection.
+		= 0x01; there may be non-HT devices in both the control and extension channel, protection is optional in BSS.
+		= 0x10: No Transmission in 40M is protected.
+		= 0x11: Transmission in both 40M and 20M shall be protected
+		if (bNonGFExist)
+			we should choose not to use GF. But still set correct ASIC registers.
+	========================================================================
+*/
+VOID 	AsicUpdateProtect(
+	IN		PRTMP_ADAPTER	pAd,
+	IN 		USHORT			OperationMode,
+	IN 		UCHAR			SetMask,
+	IN		BOOLEAN			bDisableBGProtect,
+	IN		BOOLEAN			bNonGFExist)
+{
+	PROT_CFG_STRUC	ProtCfg, ProtCfg4;
+	UINT32 Protect[6];
+	USHORT			offset;
+	UCHAR			i;
+	UINT32 MacReg = 0;
+
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+#ifdef DOT11_N_SUPPORT
+	if (!(pAd->CommonCfg.bHTProtect) && (OperationMode != 8))
+	{
+		return;
+	}
+
+	if (pAd->BATable.numAsOriginator)
+	{
+		//
+		// enable the RTS/CTS to avoid channel collision
+		//
+		SetMask = ALLN_SETPROTECT;
+		OperationMode = 8;
+	}
+#endif // DOT11_N_SUPPORT //
+
+	// Config ASIC RTS threshold register
+	RTMP_IO_READ32(pAd, TX_RTS_CFG, &MacReg);
+	MacReg &= 0xFF0000FF;
+#if 0
+	MacReg |= (pAd->CommonCfg.RtsThreshold << 8);
+#else
+	// If the user want disable RtsThreshold and enbale Amsdu/Ralink-Aggregation, set the RtsThreshold as 4096
+        if ((
+#ifdef DOT11_N_SUPPORT
+			(pAd->CommonCfg.BACapability.field.AmsduEnable) ||
+#endif // DOT11_N_SUPPORT //
+			(pAd->CommonCfg.bAggregationCapable == TRUE))
+            && pAd->CommonCfg.RtsThreshold == MAX_RTS_THRESHOLD)
+        {
+			MacReg |= (0x1000 << 8);
+        }
+        else
+        {
+			MacReg |= (pAd->CommonCfg.RtsThreshold << 8);
+        }
+#endif
+
+	RTMP_IO_WRITE32(pAd, TX_RTS_CFG, MacReg);
+
+	// Initial common protection settings
+	RTMPZeroMemory(Protect, sizeof(Protect));
+	ProtCfg4.word = 0;
+	ProtCfg.word = 0;
+	ProtCfg.field.TxopAllowGF40 = 1;
+	ProtCfg.field.TxopAllowGF20 = 1;
+	ProtCfg.field.TxopAllowMM40 = 1;
+	ProtCfg.field.TxopAllowMM20 = 1;
+	ProtCfg.field.TxopAllowOfdm = 1;
+	ProtCfg.field.TxopAllowCck = 1;
+	ProtCfg.field.RTSThEn = 1;
+	ProtCfg.field.ProtectNav = ASIC_SHORTNAV;
+
+	// update PHY mode and rate
+	if (pAd->CommonCfg.Channel > 14)
+		ProtCfg.field.ProtectRate = 0x4000;
+	ProtCfg.field.ProtectRate |= pAd->CommonCfg.RtsRate;
+
+	// Handle legacy(B/G) protection
+	if (bDisableBGProtect)
+	{
+		//ProtCfg.field.ProtectRate = pAd->CommonCfg.RtsRate;
+		ProtCfg.field.ProtectCtrl = 0;
+		Protect[0] = ProtCfg.word;
+		Protect[1] = ProtCfg.word;
+	}
+	else
+	{
+		//ProtCfg.field.ProtectRate = pAd->CommonCfg.RtsRate;
+		ProtCfg.field.ProtectCtrl = 0;			// CCK do not need to be protected
+		Protect[0] = ProtCfg.word;
+		ProtCfg.field.ProtectCtrl = ASIC_CTS;	// OFDM needs using CCK to protect
+		Protect[1] = ProtCfg.word;
+	}
+
+#ifdef DOT11_N_SUPPORT
+	// Decide HT frame protection.
+	if ((SetMask & ALLN_SETPROTECT) != 0)
+	{
+		switch(OperationMode)
+		{
+			case 0x0:
+				// NO PROTECT
+				// 1.All STAs in the BSS are 20/40 MHz HT
+				// 2. in ai 20/40MHz BSS
+				// 3. all STAs are 20MHz in a 20MHz BSS
+				// Pure HT. no protection.
+
+				// MM20_PROT_CFG
+				//	Reserved (31:27)
+				// 	PROT_TXOP(25:20) -- 010111
+				//	PROT_NAV(19:18)  -- 01 (Short NAV protection)
+				//  PROT_CTRL(17:16) -- 00 (None)
+				// 	PROT_RATE(15:0)  -- 0x4004 (OFDM 24M)
+				Protect[2] = 0x01744004;
+
+				// MM40_PROT_CFG
+				//	Reserved (31:27)
+				// 	PROT_TXOP(25:20) -- 111111
+				//	PROT_NAV(19:18)  -- 01 (Short NAV protection)
+				//  PROT_CTRL(17:16) -- 00 (None)
+				// 	PROT_RATE(15:0)  -- 0x4084 (duplicate OFDM 24M)
+				Protect[3] = 0x03f44084;
+
+				// CF20_PROT_CFG
+				//	Reserved (31:27)
+				// 	PROT_TXOP(25:20) -- 010111
+				//	PROT_NAV(19:18)  -- 01 (Short NAV protection)
+				//  PROT_CTRL(17:16) -- 00 (None)
+				// 	PROT_RATE(15:0)  -- 0x4004 (OFDM 24M)
+				Protect[4] = 0x01744004;
+
+				// CF40_PROT_CFG
+				//	Reserved (31:27)
+				// 	PROT_TXOP(25:20) -- 111111
+				//	PROT_NAV(19:18)  -- 01 (Short NAV protection)
+				//  PROT_CTRL(17:16) -- 00 (None)
+				// 	PROT_RATE(15:0)  -- 0x4084 (duplicate OFDM 24M)
+				Protect[5] = 0x03f44084;
+
+				if (bNonGFExist)
+				{
+					// PROT_NAV(19:18)  -- 01 (Short NAV protectiion)
+					// PROT_CTRL(17:16) -- 01 (RTS/CTS)
+					Protect[4] = 0x01754004;
+					Protect[5] = 0x03f54084;
+				}
+				pAd->CommonCfg.IOTestParm.bRTSLongProtOn = FALSE;
+				break;
+
+ 			case 1:
+				// This is "HT non-member protection mode."
+				// If there may be non-HT STAs my BSS
+				ProtCfg.word = 0x01744004;	// PROT_CTRL(17:16) : 0 (None)
+				ProtCfg4.word = 0x03f44084; // duplicaet legacy 24M. BW set 1.
+				if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_BG_PROTECTION_INUSED))
+				{
+					ProtCfg.word = 0x01740003;	//ERP use Protection bit is set, use protection rate at Clause 18..
+					ProtCfg4.word = 0x03f40003; // Don't duplicate RTS/CTS in CCK mode. 0x03f40083;
+				}
+				//Assign Protection method for 20&40 MHz packets
+				ProtCfg.field.ProtectCtrl = ASIC_RTS;
+				ProtCfg.field.ProtectNav = ASIC_SHORTNAV;
+				ProtCfg4.field.ProtectCtrl = ASIC_RTS;
+				ProtCfg4.field.ProtectNav = ASIC_SHORTNAV;
+				Protect[2] = ProtCfg.word;
+				Protect[3] = ProtCfg4.word;
+				Protect[4] = ProtCfg.word;
+				Protect[5] = ProtCfg4.word;
+				pAd->CommonCfg.IOTestParm.bRTSLongProtOn = TRUE;
+				break;
+
+			case 2:
+				// If only HT STAs are in BSS. at least one is 20MHz. Only protect 40MHz packets
+				ProtCfg.word = 0x01744004;  // PROT_CTRL(17:16) : 0 (None)
+				ProtCfg4.word = 0x03f44084; // duplicaet legacy 24M. BW set 1.
+
+				//Assign Protection method for 40MHz packets
+				ProtCfg4.field.ProtectCtrl = ASIC_RTS;
+				ProtCfg4.field.ProtectNav = ASIC_SHORTNAV;
+				Protect[2] = ProtCfg.word;
+				Protect[3] = ProtCfg4.word;
+				if (bNonGFExist)
+				{
+					ProtCfg.field.ProtectCtrl = ASIC_RTS;
+					ProtCfg.field.ProtectNav = ASIC_SHORTNAV;
+				}
+				Protect[4] = ProtCfg.word;
+				Protect[5] = ProtCfg4.word;
+
+				pAd->CommonCfg.IOTestParm.bRTSLongProtOn = FALSE;
+				break;
+
+			case 3:
+				// HT mixed mode.	 PROTECT ALL!
+				// Assign Rate
+				ProtCfg.word = 0x01744004;	//duplicaet legacy 24M. BW set 1.
+				ProtCfg4.word = 0x03f44084;
+				// both 20MHz and 40MHz are protected. Whether use RTS or CTS-to-self depends on the
+				if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_BG_PROTECTION_INUSED))
+				{
+					ProtCfg.word = 0x01740003;	//ERP use Protection bit is set, use protection rate at Clause 18..
+					ProtCfg4.word = 0x03f40003; // Don't duplicate RTS/CTS in CCK mode. 0x03f40083
+				}
+				//Assign Protection method for 20&40 MHz packets
+				ProtCfg.field.ProtectCtrl = ASIC_RTS;
+				ProtCfg.field.ProtectNav = ASIC_SHORTNAV;
+				ProtCfg4.field.ProtectCtrl = ASIC_RTS;
+				ProtCfg4.field.ProtectNav = ASIC_SHORTNAV;
+				Protect[2] = ProtCfg.word;
+				Protect[3] = ProtCfg4.word;
+				Protect[4] = ProtCfg.word;
+				Protect[5] = ProtCfg4.word;
+				pAd->CommonCfg.IOTestParm.bRTSLongProtOn = TRUE;
+				break;
+
+			case 8:
+				// Special on for Atheros problem n chip.
+				Protect[2] = 0x01754004;
+				Protect[3] = 0x03f54084;
+				Protect[4] = 0x01754004;
+				Protect[5] = 0x03f54084;
+				pAd->CommonCfg.IOTestParm.bRTSLongProtOn = TRUE;
+				break;
+		}
+	}
+#endif // DOT11_N_SUPPORT //
+
+	offset = CCK_PROT_CFG;
+	for (i = 0;i < 6;i++)
+	{
+		if ((SetMask & (1<< i)))
+		{
+			RTMP_IO_WRITE32(pAd, offset + i*4, Protect[i]);
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicSwitchChannel(
+					  IN PRTMP_ADAPTER pAd,
+	IN	UCHAR			Channel,
+	IN	BOOLEAN			bScan)
+{
+	ULONG			R2 = 0, R3 = DEFAULT_RF_TX_POWER, R4 = 0;
+	CHAR    TxPwer = 0, TxPwer2 = DEFAULT_RF_TX_POWER; //Bbp94 = BBPR94_DEFAULT, TxPwer2 = DEFAULT_RF_TX_POWER;
+	UCHAR	index;
+	UINT32 	Value = 0; //BbpReg, Value;
+	RTMP_RF_REGS *RFRegTable;
+
+	// Search Tx power value
+	for (index = 0; index < pAd->ChannelListNum; index++)
+	{
+		if (Channel == pAd->ChannelList[index].Channel)
+		{
+			TxPwer = pAd->ChannelList[index].Power;
+			TxPwer2 = pAd->ChannelList[index].Power2;
+			break;
+		}
+	}
+
+	if (index == MAX_NUM_OF_CHANNELS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("AsicSwitchChannel: Cant find the Channel#%d \n", Channel));
+	}
+
+#ifdef RT2870
+	// The RF programming sequence is difference between 3xxx and 2xxx
+	if (IS_RT3070(pAd) && ((pAd->RfIcType == RFIC_3020) || (pAd->RfIcType == RFIC_2020)))
+	{
+		/* modify by WY for Read RF Reg. error */
+		UCHAR RFValue;
+
+		for (index = 0; index < NUM_OF_3020_CHNL; index++)
+		{
+			if (Channel == FreqItems3020[index].Channel)
+			{
+				// Programming channel parameters
+				RT30xxWriteRFRegister(pAd, RF_R02, FreqItems3020[index].N);
+				RT30xxWriteRFRegister(pAd, RF_R03, FreqItems3020[index].K);
+
+				RT30xxReadRFRegister(pAd, RF_R06, (PUCHAR)&RFValue);
+				RFValue = (RFValue & 0xFC) | FreqItems3020[index].R;
+				RT30xxWriteRFRegister(pAd, RF_R06, (UCHAR)RFValue);
+
+				// Set Tx Power
+				RT30xxReadRFRegister(pAd, RF_R12, (PUCHAR)&RFValue);
+				RFValue = (RFValue & 0xE0) | TxPwer;
+				RT30xxWriteRFRegister(pAd, RF_R12, (UCHAR)RFValue);
+
+				// Set RF offset
+				RT30xxReadRFRegister(pAd, RF_R23, (PUCHAR)&RFValue);
+				RFValue = (RFValue & 0x80) | pAd->RfFreqOffset;
+				RT30xxWriteRFRegister(pAd, RF_R23, (UCHAR)RFValue);
+
+				// Set BW
+				if (!bScan && (pAd->CommonCfg.BBPCurrentBW == BW_40))
+				{
+					RFValue = pAd->Mlme.CaliBW40RfR24;
+					//DISABLE_11N_CHECK(pAd);
+				}
+				else
+				{
+					RFValue = pAd->Mlme.CaliBW20RfR24;
+				}
+				RT30xxWriteRFRegister(pAd, RF_R24, (UCHAR)RFValue);
+
+				// Enable RF tuning
+				RT30xxReadRFRegister(pAd, RF_R07, (PUCHAR)&RFValue);
+				RFValue = RFValue | 0x1;
+				RT30xxWriteRFRegister(pAd, RF_R07, (UCHAR)RFValue);
+
+				// latch channel for future usage.
+				pAd->LatchRfRegs.Channel = Channel;
+
+				break;
+			}
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("SwitchChannel#%d(RF=%d, Pwr0=%d, Pwr1=%d, %dT), N=0x%02X, K=0x%02X, R=0x%02X\n",
+			Channel,
+			pAd->RfIcType,
+			TxPwer,
+			TxPwer2,
+			pAd->Antenna.field.TxPath,
+			FreqItems3020[index].N,
+			FreqItems3020[index].K,
+			FreqItems3020[index].R));
+	}
+	else
+#endif // RT2870 //
+	{
+		RFRegTable = RF2850RegTable;
+
+		switch (pAd->RfIcType)
+		{
+			case RFIC_2820:
+			case RFIC_2850:
+			case RFIC_2720:
+			case RFIC_2750:
+
+			for (index = 0; index < NUM_OF_2850_CHNL; index++)
+			{
+				if (Channel == RFRegTable[index].Channel)
+				{
+					R2 = RFRegTable[index].R2;
+					if (pAd->Antenna.field.TxPath == 1)
+					{
+						R2 |= 0x4000;	// If TXpath is 1, bit 14 = 1;
+					}
+
+					if (pAd->Antenna.field.RxPath == 2)
+					{
+						R2 |= 0x40;	// write 1 to off Rxpath.
+					}
+					else if (pAd->Antenna.field.RxPath == 1)
+					{
+						R2 |= 0x20040;	// write 1 to off RxPath
+					}
+
+					if (Channel > 14)
+					{
+						// initialize R3, R4
+						R3 = (RFRegTable[index].R3 & 0xffffc1ff);
+						R4 = (RFRegTable[index].R4 & (~0x001f87c0)) | (pAd->RfFreqOffset << 15);
+
+						// 5G band power range: 0xF9~0X0F, TX0 Reg3 bit9/TX1 Reg4 bit6="0" means the TX power reduce 7dB
+						// R3
+						if ((TxPwer >= -7) && (TxPwer < 0))
+						{
+							TxPwer = (7+TxPwer);
+							TxPwer = (TxPwer > 0xF) ? (0xF) : (TxPwer);
+							R3 |= (TxPwer << 10);
+							DBGPRINT(RT_DEBUG_ERROR, ("AsicSwitchChannel: TxPwer=%d \n", TxPwer));
+						}
+						else
+						{
+							TxPwer = (TxPwer > 0xF) ? (0xF) : (TxPwer);
+							R3 |= (TxPwer << 10) | (1 << 9);
+						}
+
+						// R4
+						if ((TxPwer2 >= -7) && (TxPwer2 < 0))
+						{
+							TxPwer2 = (7+TxPwer2);
+							TxPwer2 = (TxPwer2 > 0xF) ? (0xF) : (TxPwer2);
+							R4 |= (TxPwer2 << 7);
+							DBGPRINT(RT_DEBUG_ERROR, ("AsicSwitchChannel: TxPwer2=%d \n", TxPwer2));
+						}
+						else
+						{
+							TxPwer2 = (TxPwer2 > 0xF) ? (0xF) : (TxPwer2);
+							R4 |= (TxPwer2 << 7) | (1 << 6);
+						}
+					}
+					else
+					{
+						R3 = (RFRegTable[index].R3 & 0xffffc1ff) | (TxPwer << 9); // set TX power0
+					R4 = (RFRegTable[index].R4 & (~0x001f87c0)) | (pAd->RfFreqOffset << 15) | (TxPwer2 <<6);// Set freq Offset & TxPwr1
+					}
+
+					// Based on BBP current mode before changing RF channel.
+					if (!bScan && (pAd->CommonCfg.BBPCurrentBW == BW_40))
+					{
+						R4 |=0x200000;
+					}
+
+					// Update variables
+					pAd->LatchRfRegs.Channel = Channel;
+					pAd->LatchRfRegs.R1 = RFRegTable[index].R1;
+					pAd->LatchRfRegs.R2 = R2;
+					pAd->LatchRfRegs.R3 = R3;
+					pAd->LatchRfRegs.R4 = R4;
+
+					// Set RF value 1's set R3[bit2] = [0]
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R1);
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R2);
+					RTMP_RF_IO_WRITE32(pAd, (pAd->LatchRfRegs.R3 & (~0x04)));
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R4);
+
+					RTMPusecDelay(200);
+
+					// Set RF value 2's set R3[bit2] = [1]
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R1);
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R2);
+					RTMP_RF_IO_WRITE32(pAd, (pAd->LatchRfRegs.R3 | 0x04));
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R4);
+
+					RTMPusecDelay(200);
+
+					// Set RF value 3's set R3[bit2] = [0]
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R1);
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R2);
+					RTMP_RF_IO_WRITE32(pAd, (pAd->LatchRfRegs.R3 & (~0x04)));
+					RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R4);
+
+					break;
+				}
+			}
+			break;
+
+			default:
+			break;
+		}
+	}
+
+	// Change BBP setting during siwtch from a->g, g->a
+	if (Channel <= 14)
+	{
+	    ULONG	TxPinCfg = 0x00050F0A;//Gary 2007/08/09 0x050A0A
+
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R62, (0x37 - GET_LNA_GAIN(pAd)));
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R63, (0x37 - GET_LNA_GAIN(pAd)));
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R64, (0x37 - GET_LNA_GAIN(pAd)));
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R86, 0);//(0x44 - GET_LNA_GAIN(pAd)));	// According the Rory's suggestion to solve the middle range issue.
+		//RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R82, 0x62);
+
+		// Rx High power VGA offset for LNA select
+	    if (pAd->NicConfig2.field.ExternalLNAForG)
+	    {
+	        RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R82, 0x62);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R75, 0x46);
+	    }
+	    else
+	    {
+	        RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R82, 0x84);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R75, 0x50);
+	    }
+
+		// 5G band selection PIN, bit1 and bit2 are complement
+		RTMP_IO_READ32(pAd, TX_BAND_CFG, &Value);
+		Value &= (~0x6);
+		Value |= (0x04);
+		RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Value);
+
+        // Turn off unused PA or LNA when only 1T or 1R
+		if (pAd->Antenna.field.TxPath == 1)
+		{
+			TxPinCfg &= 0xFFFFFFF3;
+		}
+		if (pAd->Antenna.field.RxPath == 1)
+		{
+			TxPinCfg &= 0xFFFFF3FF;
+		}
+
+		RTMP_IO_WRITE32(pAd, TX_PIN_CFG, TxPinCfg);
+	}
+	else
+	{
+	    ULONG	TxPinCfg = 0x00050F05;//Gary 2007/8/9 0x050505
+
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R62, (0x37 - GET_LNA_GAIN(pAd)));
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R63, (0x37 - GET_LNA_GAIN(pAd)));
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R64, (0x37 - GET_LNA_GAIN(pAd)));
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R86, 0);//(0x44 - GET_LNA_GAIN(pAd)));   // According the Rory's suggestion to solve the middle range issue.
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R82, 0xF2);
+
+		// Rx High power VGA offset for LNA select
+		if (pAd->NicConfig2.field.ExternalLNAForA)
+		{
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R75, 0x46);
+		}
+		else
+		{
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R75, 0x50);
+		}
+
+		// 5G band selection PIN, bit1 and bit2 are complement
+		RTMP_IO_READ32(pAd, TX_BAND_CFG, &Value);
+		Value &= (~0x6);
+		Value |= (0x02);
+		RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Value);
+
+        // Turn off unused PA or LNA when only 1T or 1R
+		if (pAd->Antenna.field.TxPath == 1)
+		{
+			TxPinCfg &= 0xFFFFFFF3;
+	}
+		if (pAd->Antenna.field.RxPath == 1)
+		{
+			TxPinCfg &= 0xFFFFF3FF;
+	}
+
+		RTMP_IO_WRITE32(pAd, TX_PIN_CFG, TxPinCfg);
+	}
+
+    // R66 should be set according to Channel and use 20MHz when scanning
+	//RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, (0x2E + GET_LNA_GAIN(pAd)));
+	if (bScan)
+		RTMPSetAGCInitValue(pAd, BW_20);
+	else
+		RTMPSetAGCInitValue(pAd, pAd->CommonCfg.BBPCurrentBW);
+
+	//
+	// On 11A, We should delay and wait RF/BBP to be stable
+	// and the appropriate time should be 1000 micro seconds
+	// 2005/06/05 - On 11G, We also need this delay time. Otherwise it's difficult to pass the WHQL.
+	//
+	RTMPusecDelay(1000);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("SwitchChannel#%d(RF=%d, Pwr0=%lu, Pwr1=%lu, %dT) to , R1=0x%08lx, R2=0x%08lx, R3=0x%08lx, R4=0x%08lx\n",
+							  Channel,
+							  pAd->RfIcType,
+							  (R3 & 0x00003e00) >> 9,
+							  (R4 & 0x000007c0) >> 6,
+							  pAd->Antenna.field.TxPath,
+							  pAd->LatchRfRegs.R1,
+							  pAd->LatchRfRegs.R2,
+							  pAd->LatchRfRegs.R3,
+							  pAd->LatchRfRegs.R4));
+}
+
+/*
+	==========================================================================
+	Description:
+		This function is required for 2421 only, and should not be used during
+		site survey. It's only required after NIC decided to stay at a channel
+		for a longer period.
+		When this function is called, it's always after AsicSwitchChannel().
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicLockChannel(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR Channel)
+{
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID	AsicAntennaSelect(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Channel)
+{
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Antenna miscellaneous setting.
+
+	Arguments:
+		pAd						Pointer to our adapter
+		BandState				Indicate current Band State.
+
+	Return Value:
+		None
+
+	IRQL <= DISPATCH_LEVEL
+
+	Note:
+		1.) Frame End type control
+			only valid for G only (RF_2527 & RF_2529)
+			0: means DPDT, set BBP R4 bit 5 to 1
+			1: means SPDT, set BBP R4 bit 5 to 0
+
+
+	========================================================================
+*/
+VOID	AsicAntennaSetting(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	ABGBAND_STATE	BandState)
+{
+}
+
+VOID AsicRfTuningExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+}
+
+/*
+	==========================================================================
+	Description:
+		Gives CCK TX rate 2 more dB TX power.
+		This routine works only in LINK UP in INFRASTRUCTURE mode.
+
+		calculate desired Tx power in RF R3.Tx0~5,	should consider -
+		0. if current radio is a noisy environment (pAd->DrsCounters.fNoisyEnvironment)
+		1. TxPowerPercentage
+		2. auto calibration based on TSSI feedback
+		3. extra 2 db for CCK
+		4. -10 db upon very-short distance (AvgRSSI >= -40db) to AP
+
+	NOTE: Since this routine requires the value of (pAd->DrsCounters.fNoisyEnvironment),
+		it should be called AFTER MlmeDynamicTxRatSwitching()
+	==========================================================================
+ */
+VOID AsicAdjustTxPower(
+	IN PRTMP_ADAPTER pAd)
+{
+	INT			i, j;
+	CHAR		DeltaPwr = 0;
+	BOOLEAN		bAutoTxAgc = FALSE;
+	UCHAR		TssiRef, *pTssiMinusBoundary, *pTssiPlusBoundary, TxAgcStep;
+	UCHAR		BbpR1 = 0, BbpR49 = 0, idx;
+	PCHAR		pTxAgcCompensate;
+	ULONG		TxPwr[5];
+	CHAR		Value;
+
+	if (pAd->CommonCfg.BBPCurrentBW == BW_40)
+	{
+		if (pAd->CommonCfg.CentralChannel > 14)
+		{
+			TxPwr[0] = pAd->Tx40MPwrCfgABand[0];
+			TxPwr[1] = pAd->Tx40MPwrCfgABand[1];
+			TxPwr[2] = pAd->Tx40MPwrCfgABand[2];
+			TxPwr[3] = pAd->Tx40MPwrCfgABand[3];
+			TxPwr[4] = pAd->Tx40MPwrCfgABand[4];
+		}
+		else
+		{
+			TxPwr[0] = pAd->Tx40MPwrCfgGBand[0];
+			TxPwr[1] = pAd->Tx40MPwrCfgGBand[1];
+			TxPwr[2] = pAd->Tx40MPwrCfgGBand[2];
+			TxPwr[3] = pAd->Tx40MPwrCfgGBand[3];
+			TxPwr[4] = pAd->Tx40MPwrCfgGBand[4];
+		}
+	}
+	else
+	{
+		if (pAd->CommonCfg.Channel > 14)
+		{
+			TxPwr[0] = pAd->Tx20MPwrCfgABand[0];
+			TxPwr[1] = pAd->Tx20MPwrCfgABand[1];
+			TxPwr[2] = pAd->Tx20MPwrCfgABand[2];
+			TxPwr[3] = pAd->Tx20MPwrCfgABand[3];
+			TxPwr[4] = pAd->Tx20MPwrCfgABand[4];
+		}
+		else
+		{
+			TxPwr[0] = pAd->Tx20MPwrCfgGBand[0];
+			TxPwr[1] = pAd->Tx20MPwrCfgGBand[1];
+			TxPwr[2] = pAd->Tx20MPwrCfgGBand[2];
+			TxPwr[3] = pAd->Tx20MPwrCfgGBand[3];
+			TxPwr[4] = pAd->Tx20MPwrCfgGBand[4];
+		}
+	}
+
+	// TX power compensation for temperature variation based on TSSI. try every 4 second
+	if (pAd->Mlme.OneSecPeriodicRound % 4 == 0)
+	{
+		if (pAd->CommonCfg.Channel <= 14)
+		{
+			/* bg channel */
+			bAutoTxAgc         = pAd->bAutoTxAgcG;
+			TssiRef            = pAd->TssiRefG;
+			pTssiMinusBoundary = &pAd->TssiMinusBoundaryG[0];
+			pTssiPlusBoundary  = &pAd->TssiPlusBoundaryG[0];
+			TxAgcStep          = pAd->TxAgcStepG;
+			pTxAgcCompensate   = &pAd->TxAgcCompensateG;
+		}
+		else
+		{
+			/* a channel */
+			bAutoTxAgc         = pAd->bAutoTxAgcA;
+			TssiRef            = pAd->TssiRefA;
+			pTssiMinusBoundary = &pAd->TssiMinusBoundaryA[0];
+			pTssiPlusBoundary  = &pAd->TssiPlusBoundaryA[0];
+			TxAgcStep          = pAd->TxAgcStepA;
+			pTxAgcCompensate   = &pAd->TxAgcCompensateA;
+		}
+
+		if (bAutoTxAgc)
+		{
+			/* BbpR1 is unsigned char */
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R49, &BbpR49);
+
+			/* (p) TssiPlusBoundaryG[0] = 0 = (m) TssiMinusBoundaryG[0] */
+			/* compensate: +4     +3   +2   +1    0   -1   -2   -3   -4 * steps */
+			/* step value is defined in pAd->TxAgcStepG for tx power value */
+
+			/* [4]+1+[4]   p4     p3   p2   p1   o1   m1   m2   m3   m4 */
+			/* ex:         0x00 0x15 0x25 0x45 0x88 0xA0 0xB5 0xD0 0xF0
+			   above value are examined in mass factory production */
+			/*             [4]    [3]  [2]  [1]  [0]  [1]  [2]  [3]  [4] */
+
+			/* plus (+) is 0x00 ~ 0x45, minus (-) is 0xa0 ~ 0xf0 */
+			/* if value is between p1 ~ o1 or o1 ~ s1, no need to adjust tx power */
+			/* if value is 0xa5, tx power will be -= TxAgcStep*(2-1) */
+
+			if (BbpR49 > pTssiMinusBoundary[1])
+			{
+				// Reading is larger than the reference value
+				// check for how large we need to decrease the Tx power
+				for (idx = 1; idx < 5; idx++)
+				{
+					if (BbpR49 <= pTssiMinusBoundary[idx])  // Found the range
+						break;
+				}
+				// The index is the step we should decrease, idx = 0 means there is nothing to compensate
+//				if (R3 > (ULONG) (TxAgcStep * (idx-1)))
+					*pTxAgcCompensate = -(TxAgcStep * (idx-1));
+//				else
+//					*pTxAgcCompensate = -((UCHAR)R3);
+
+				DeltaPwr += (*pTxAgcCompensate);
+				DBGPRINT(RT_DEBUG_TRACE, ("-- Tx Power, BBP R1=%x, TssiRef=%x, TxAgcStep=%x, step = -%d\n",
+					BbpR49, TssiRef, TxAgcStep, idx-1));
+			}
+			else if (BbpR49 < pTssiPlusBoundary[1])
+			{
+				// Reading is smaller than the reference value
+				// check for how large we need to increase the Tx power
+				for (idx = 1; idx < 5; idx++)
+				{
+					if (BbpR49 >= pTssiPlusBoundary[idx])   // Found the range
+						break;
+				}
+				// The index is the step we should increase, idx = 0 means there is nothing to compensate
+				*pTxAgcCompensate = TxAgcStep * (idx-1);
+				DeltaPwr += (*pTxAgcCompensate);
+				DBGPRINT(RT_DEBUG_TRACE, ("++ Tx Power, BBP R1=%x, TssiRef=%x, TxAgcStep=%x, step = +%d\n",
+					BbpR49, TssiRef, TxAgcStep, idx-1));
+			}
+			else
+			{
+				*pTxAgcCompensate = 0;
+				DBGPRINT(RT_DEBUG_TRACE, ("   Tx Power, BBP R49=%x, TssiRef=%x, TxAgcStep=%x, step = +%d\n",
+					BbpR49, TssiRef, TxAgcStep, 0));
+			}
+		}
+	}
+	else
+	{
+		if (pAd->CommonCfg.Channel <= 14)
+		{
+			bAutoTxAgc         = pAd->bAutoTxAgcG;
+			pTxAgcCompensate   = &pAd->TxAgcCompensateG;
+		}
+		else
+		{
+			bAutoTxAgc         = pAd->bAutoTxAgcA;
+			pTxAgcCompensate   = &pAd->TxAgcCompensateA;
+		}
+
+		if (bAutoTxAgc)
+			DeltaPwr += (*pTxAgcCompensate);
+	}
+
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R1, &BbpR1);
+	BbpR1 &= 0xFC;
+
+#ifdef SINGLE_SKU
+	// Handle regulatory max tx power constrain
+	do
+	{
+		UCHAR    TxPwrInEEPROM = 0xFF, CountryTxPwr = 0xFF, criterion;
+		UCHAR    AdjustMaxTxPwr[40];
+
+		if (pAd->CommonCfg.Channel > 14) // 5G band
+			TxPwrInEEPROM = ((pAd->CommonCfg.DefineMaxTxPwr & 0xFF00) >> 8);
+		else // 2.4G band
+			TxPwrInEEPROM = (pAd->CommonCfg.DefineMaxTxPwr & 0x00FF);
+		CountryTxPwr = GetCuntryMaxTxPwr(pAd, pAd->CommonCfg.Channel);
+
+		// error handling, range check
+		if ((TxPwrInEEPROM > 0x50) || (CountryTxPwr > 0x50))
+		{
+			DBGPRINT(RT_DEBUG_ERROR,("AsicAdjustTxPower - Invalid max tx power (=0x%02x), CountryTxPwr=%d\n", TxPwrInEEPROM, CountryTxPwr));
+			break;
+		}
+
+		criterion = *((PUCHAR)TxPwr + 2) & 0xF;        // FAE use OFDM 6M as criterion
+
+		DBGPRINT_RAW(RT_DEBUG_TRACE,("AsicAdjustTxPower (criterion=%d, TxPwrInEEPROM=%d, CountryTxPwr=%d)\n", criterion, TxPwrInEEPROM, CountryTxPwr));
+
+		// Adjust max tx power according to the relationship of tx power in E2PROM
+		for (i=0; i<5; i++)
+		{
+			// CCK will have 4dBm larger than OFDM
+			// Therefore, we should separate to parse the tx power field
+			if (i == 0)
+			{
+				for (j=0; j<8; j++)
+				{
+					Value = (CHAR)((TxPwr[i] >> j*4) & 0x0F);
+
+					if (j < 4)
+					{
+						// CCK will have 4dBm larger than OFDM
+						AdjustMaxTxPwr[i*8+j] = TxPwrInEEPROM + (Value - criterion) + 4;
+					}
+					else
+					{
+						AdjustMaxTxPwr[i*8+j] = TxPwrInEEPROM + (Value - criterion);
+					}
+					DBGPRINT_RAW(RT_DEBUG_TRACE,("AsicAdjustTxPower (i/j=%d/%d, Value=%d, %d)\n", i, j, Value, AdjustMaxTxPwr[i*8+j]));
+				}
+			}
+			else
+			{
+				for (j=0; j<8; j++)
+				{
+					Value = (CHAR)((TxPwr[i] >> j*4) & 0x0F);
+
+					AdjustMaxTxPwr[i*8+j] = TxPwrInEEPROM + (Value - criterion);
+					DBGPRINT_RAW(RT_DEBUG_TRACE,("AsicAdjustTxPower (i/j=%d/%d, Value=%d, %d)\n", i, j, Value, AdjustMaxTxPwr[i*8+j]));
+				}
+			}
+		}
+
+		// Adjust tx power according to the relationship
+		for (i=0; i<5; i++)
+		{
+			if (TxPwr[i] != 0xffffffff)
+			{
+				for (j=0; j<8; j++)
+				{
+					Value = (CHAR)((TxPwr[i] >> j*4) & 0x0F);
+
+					// The system tx power is larger than the regulatory, the power should be restrain
+					if (AdjustMaxTxPwr[i*8+j] > CountryTxPwr)
+					{
+						// decrease to zero and don't need to take care BBPR1
+						if ((Value - (AdjustMaxTxPwr[i*8+j] - CountryTxPwr)) > 0)
+							Value -= (AdjustMaxTxPwr[i*8+j] - CountryTxPwr);
+						else
+							Value = 0;
+
+						DBGPRINT_RAW(RT_DEBUG_TRACE,("AsicAdjustTxPower (i/j=%d/%d, Value=%d, %d)\n", i, j, Value, AdjustMaxTxPwr[i*8+j]));
+					}
+					else
+						DBGPRINT_RAW(RT_DEBUG_TRACE,("AsicAdjustTxPower (i/j=%d/%d, Value=%d, %d, no change)\n", i, j, Value, AdjustMaxTxPwr[i*8+j]));
+
+						TxPwr[i] = (TxPwr[i] & ~(0x0000000F << j*4)) | (Value << j*4);
+				}
+			}
+		}
+	} while (FALSE);
+#endif // SINGLE_SKU //
+
+	/* calculate delta power based on the percentage specified from UI */
+	// E2PROM setting is calibrated for maximum TX power (i.e. 100%)
+	// We lower TX power here according to the percentage specified from UI
+	if (pAd->CommonCfg.TxPowerPercentage == 0xffffffff)       // AUTO TX POWER control
+		;
+	else if (pAd->CommonCfg.TxPowerPercentage > 90)  // 91 ~ 100% & AUTO, treat as 100% in terms of mW
+		;
+	else if (pAd->CommonCfg.TxPowerPercentage > 60)  // 61 ~ 90%, treat as 75% in terms of mW		// DeltaPwr -= 1;
+	{
+		DeltaPwr -= 1;
+	}
+	else if (pAd->CommonCfg.TxPowerPercentage > 30)  // 31 ~ 60%, treat as 50% in terms of mW		// DeltaPwr -= 3;
+	{
+		DeltaPwr -= 3;
+	}
+	else if (pAd->CommonCfg.TxPowerPercentage > 15)  // 16 ~ 30%, treat as 25% in terms of mW		// DeltaPwr -= 6;
+	{
+		BbpR1 |= 0x01;
+	}
+	else if (pAd->CommonCfg.TxPowerPercentage > 9)   // 10 ~ 15%, treat as 12.5% in terms of mW		// DeltaPwr -= 9;
+	{
+		BbpR1 |= 0x01;
+		DeltaPwr -= 3;
+	}
+	else                                           // 0 ~ 9 %, treat as MIN(~3%) in terms of mW		// DeltaPwr -= 12;
+	{
+		BbpR1 |= 0x02;
+	}
+
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R1, BbpR1);
+
+	/* reset different new tx power for different TX rate */
+	for(i=0; i<5; i++)
+	{
+		if (TxPwr[i] != 0xffffffff)
+		{
+			for (j=0; j<8; j++)
+			{
+				Value = (CHAR)((TxPwr[i] >> j*4) & 0x0F); /* 0 ~ 15 */
+
+				if ((Value + DeltaPwr) < 0)
+				{
+					Value = 0; /* min */
+				}
+				else if ((Value + DeltaPwr) > 0xF)
+				{
+					Value = 0xF; /* max */
+				}
+				else
+				{
+					Value += DeltaPwr; /* temperature compensation */
+				}
+
+				/* fill new value to CSR offset */
+				TxPwr[i] = (TxPwr[i] & ~(0x0000000F << j*4)) | (Value << j*4);
+			}
+
+			/* write tx power value to CSR */
+			/* TX_PWR_CFG_0 (8 tx rate) for	TX power for OFDM 12M/18M
+											TX power for OFDM 6M/9M
+											TX power for CCK5.5M/11M
+											TX power for CCK1M/2M */
+			/* TX_PWR_CFG_1 ~ TX_PWR_CFG_4 */
+			RTMP_IO_WRITE32(pAd, TX_PWR_CFG_0 + i*4, TxPwr[i]);
+		}
+	}
+
+}
+
+#ifdef CONFIG_STA_SUPPORT
+/*
+	==========================================================================
+	Description:
+		put PHY to sleep here, and set next wakeup timer. PHY doesn't not wakeup
+		automatically. Instead, MCU will issue a TwakeUpInterrupt to host after
+		the wakeup timer timeout. Driver has to issue a separate command to wake
+		PHY up.
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicSleepThenAutoWakeup(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT TbttNumToNextWakeUp)
+{
+    RT28XX_STA_SLEEP_THEN_AUTO_WAKEUP(pAd, TbttNumToNextWakeUp);
+}
+
+/*
+	==========================================================================
+	Description:
+		AsicForceWakeup() is used whenever manual wakeup is required
+		AsicForceSleep() should only be used when not in INFRA BSS. When
+		in INFRA BSS, we should use AsicSleepThenAutoWakeup() instead.
+	==========================================================================
+ */
+VOID AsicForceSleep(
+	IN PRTMP_ADAPTER pAd)
+{
+
+}
+
+/*
+	==========================================================================
+	Description:
+		AsicForceWakeup() is used whenever Twakeup timer (set via AsicSleepThenAutoWakeup)
+		expired.
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+	==========================================================================
+ */
+VOID AsicForceWakeup(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN    bFromTx)
+{
+    DBGPRINT(RT_DEBUG_TRACE, ("--> AsicForceWakeup \n"));
+    RT28XX_STA_FORCE_WAKEUP(pAd, bFromTx);
+}
+#endif // CONFIG_STA_SUPPORT //
+/*
+	==========================================================================
+	Description:
+		Set My BSSID
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicSetBssid(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pBssid)
+{
+	ULONG		  Addr4;
+	DBGPRINT(RT_DEBUG_TRACE, ("==============> AsicSetBssid %x:%x:%x:%x:%x:%x\n",
+		pBssid[0],pBssid[1],pBssid[2],pBssid[3], pBssid[4],pBssid[5]));
+
+	Addr4 = (ULONG)(pBssid[0])		 |
+			(ULONG)(pBssid[1] << 8)  |
+			(ULONG)(pBssid[2] << 16) |
+			(ULONG)(pBssid[3] << 24);
+	RTMP_IO_WRITE32(pAd, MAC_BSSID_DW0, Addr4);
+
+	Addr4 = 0;
+	// always one BSSID in STA mode
+	Addr4 = (ULONG)(pBssid[4]) | (ULONG)(pBssid[5] << 8);
+
+	RTMP_IO_WRITE32(pAd, MAC_BSSID_DW1, Addr4);
+}
+
+VOID AsicSetMcastWC(
+	IN PRTMP_ADAPTER pAd)
+{
+	MAC_TABLE_ENTRY *pEntry = &pAd->MacTab.Content[MCAST_WCID];
+	USHORT		offset;
+
+	pEntry->Sst        = SST_ASSOC;
+	pEntry->Aid        = MCAST_WCID;	// Softap supports 1 BSSID and use WCID=0 as multicast Wcid index
+	pEntry->PsMode     = PWR_ACTIVE;
+	pEntry->CurrTxRate = pAd->CommonCfg.MlmeRate;
+	offset = MAC_WCID_BASE + BSS0Mcast_WCID * HW_WCID_ENTRY_SIZE;
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicDelWcidTab(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR	Wcid)
+{
+	ULONG		  Addr0 = 0x0, Addr1 = 0x0;
+	ULONG 		offset;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("AsicDelWcidTab==>Wcid = 0x%x\n",Wcid));
+	offset = MAC_WCID_BASE + Wcid * HW_WCID_ENTRY_SIZE;
+	RTMP_IO_WRITE32(pAd, offset, Addr0);
+	offset += 4;
+	RTMP_IO_WRITE32(pAd, offset, Addr1);
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicEnableRDG(
+	IN PRTMP_ADAPTER pAd)
+{
+	TX_LINK_CFG_STRUC	TxLinkCfg;
+	UINT32				Data = 0;
+
+	RTMP_IO_READ32(pAd, TX_LINK_CFG, &TxLinkCfg.word);
+	TxLinkCfg.field.TxRDGEn = 1;
+	RTMP_IO_WRITE32(pAd, TX_LINK_CFG, TxLinkCfg.word);
+
+	RTMP_IO_READ32(pAd, EDCA_AC0_CFG, &Data);
+	Data  &= 0xFFFFFF00;
+	Data  |= 0x80;
+	RTMP_IO_WRITE32(pAd, EDCA_AC0_CFG, Data);
+
+	//OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED);
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicDisableRDG(
+	IN PRTMP_ADAPTER pAd)
+{
+	TX_LINK_CFG_STRUC	TxLinkCfg;
+	UINT32				Data = 0;
+
+
+	RTMP_IO_READ32(pAd, TX_LINK_CFG, &TxLinkCfg.word);
+	TxLinkCfg.field.TxRDGEn = 0;
+	RTMP_IO_WRITE32(pAd, TX_LINK_CFG, TxLinkCfg.word);
+
+	RTMP_IO_READ32(pAd, EDCA_AC0_CFG, &Data);
+
+	Data  &= 0xFFFFFF00;
+	//Data  |= 0x20;
+#ifndef WIFI_TEST
+	//if ( pAd->CommonCfg.bEnableTxBurst )
+	//	Data |= 0x60; // for performance issue not set the TXOP to 0
+#endif
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_DYNAMIC_BE_TXOP_ACTIVE)
+#ifdef DOT11_N_SUPPORT
+		&& (pAd->MacTab.fAnyStationMIMOPSDynamic == FALSE)
+#endif // DOT11_N_SUPPORT //
+	)
+	{
+		// For CWC test, change txop from 0x30 to 0x20 in TxBurst mode
+		if (pAd->CommonCfg.bEnableTxBurst)
+			Data |= 0x20;
+	}
+	RTMP_IO_WRITE32(pAd, EDCA_AC0_CFG, Data);
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicDisableSync(
+	IN PRTMP_ADAPTER pAd)
+{
+	BCN_TIME_CFG_STRUC csr;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--->Disable TSF synchronization\n"));
+
+	// 2003-12-20 disable TSF and TBTT while NIC in power-saving have side effect
+	//			  that NIC will never wakes up because TSF stops and no more
+	//			  TBTT interrupts
+	pAd->TbttTickCount = 0;
+	RTMP_IO_READ32(pAd, BCN_TIME_CFG, &csr.word);
+	csr.field.bBeaconGen = 0;
+	csr.field.bTBTTEnable = 0;
+	csr.field.TsfSyncMode = 0;
+	csr.field.bTsfTicking = 0;
+	RTMP_IO_WRITE32(pAd, BCN_TIME_CFG, csr.word);
+
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicEnableBssSync(
+	IN PRTMP_ADAPTER pAd)
+{
+	BCN_TIME_CFG_STRUC csr;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--->AsicEnableBssSync(INFRA mode)\n"));
+
+	RTMP_IO_READ32(pAd, BCN_TIME_CFG, &csr.word);
+//	RTMP_IO_WRITE32(pAd, BCN_TIME_CFG, 0x00000000);
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		csr.field.BeaconInterval = pAd->CommonCfg.BeaconPeriod << 4; // ASIC register in units of 1/16 TU
+		csr.field.bTsfTicking = 1;
+		csr.field.TsfSyncMode = 1; // sync TSF in INFRASTRUCTURE mode
+		csr.field.bBeaconGen  = 0; // do NOT generate BEACON
+		csr.field.bTBTTEnable = 1;
+	}
+#endif // CONFIG_STA_SUPPORT //
+	RTMP_IO_WRITE32(pAd, BCN_TIME_CFG, csr.word);
+}
+
+/*
+	==========================================================================
+	Description:
+	Note:
+		BEACON frame in shared memory should be built ok before this routine
+		can be called. Otherwise, a garbage frame maybe transmitted out every
+		Beacon period.
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicEnableIbssSync(
+	IN PRTMP_ADAPTER pAd)
+{
+	BCN_TIME_CFG_STRUC csr9;
+	PUCHAR			ptr;
+	UINT i;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--->AsicEnableIbssSync(ADHOC mode. MPDUtotalByteCount = %d)\n", pAd->BeaconTxWI.MPDUtotalByteCount));
+
+	RTMP_IO_READ32(pAd, BCN_TIME_CFG, &csr9.word);
+	csr9.field.bBeaconGen = 0;
+	csr9.field.bTBTTEnable = 0;
+	csr9.field.bTsfTicking = 0;
+	RTMP_IO_WRITE32(pAd, BCN_TIME_CFG, csr9.word);
+
+
+#ifdef RT2870
+	// move BEACON TXD and frame content to on-chip memory
+	ptr = (PUCHAR)&pAd->BeaconTxWI;
+	for (i=0; i<TXWI_SIZE; i+=2)  // 16-byte TXWI field
+	{
+		//UINT32 longptr =  *ptr + (*(ptr+1)<<8) + (*(ptr+2)<<16) + (*(ptr+3)<<24);
+		//RTMP_IO_WRITE32(pAd, HW_BEACON_BASE0 + i, longptr);
+		RTUSBMultiWrite(pAd, HW_BEACON_BASE0 + i, ptr, 2);
+		ptr += 2;
+	}
+
+	// start right after the 16-byte TXWI field
+	ptr = pAd->BeaconBuf;
+	for (i=0; i< pAd->BeaconTxWI.MPDUtotalByteCount; i+=2)
+	{
+		//UINT32 longptr =  *ptr + (*(ptr+1)<<8) + (*(ptr+2)<<16) + (*(ptr+3)<<24);
+		//RTMP_IO_WRITE32(pAd, HW_BEACON_BASE0 + TXWI_SIZE + i, longptr);
+		RTUSBMultiWrite(pAd, HW_BEACON_BASE0 + TXWI_SIZE + i, ptr, 2);
+		ptr +=2;
+	}
+#endif // RT2870 //
+
+	//
+	// For Wi-Fi faily generated beacons between participating stations.
+	// Set TBTT phase adaptive adjustment step to 8us (default 16us)
+	// don't change settings 2006-5- by Jerry
+	//RTMP_IO_WRITE32(pAd, TBTT_SYNC_CFG, 0x00001010);
+
+	// start sending BEACON
+	csr9.field.BeaconInterval = pAd->CommonCfg.BeaconPeriod << 4; // ASIC register in units of 1/16 TU
+	csr9.field.bTsfTicking = 1;
+	csr9.field.TsfSyncMode = 2; // sync TSF in IBSS mode
+	csr9.field.bTBTTEnable = 1;
+	csr9.field.bBeaconGen = 1;
+	RTMP_IO_WRITE32(pAd, BCN_TIME_CFG, csr9.word);
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicSetEdcaParm(
+	IN PRTMP_ADAPTER pAd,
+	IN PEDCA_PARM	 pEdcaParm)
+{
+	EDCA_AC_CFG_STRUC   Ac0Cfg, Ac1Cfg, Ac2Cfg, Ac3Cfg;
+	AC_TXOP_CSR0_STRUC csr0;
+	AC_TXOP_CSR1_STRUC csr1;
+	AIFSN_CSR_STRUC    AifsnCsr;
+	CWMIN_CSR_STRUC    CwminCsr;
+	CWMAX_CSR_STRUC    CwmaxCsr;
+	int i;
+
+	Ac0Cfg.word = 0;
+	Ac1Cfg.word = 0;
+	Ac2Cfg.word = 0;
+	Ac3Cfg.word = 0;
+	if ((pEdcaParm == NULL) || (pEdcaParm->bValid == FALSE))
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("AsicSetEdcaParm\n"));
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_WMM_INUSED);
+		for (i=0; i<MAX_LEN_OF_MAC_TABLE; i++)
+		{
+			if (pAd->MacTab.Content[i].ValidAsCLI || pAd->MacTab.Content[i].ValidAsApCli)
+				CLIENT_STATUS_CLEAR_FLAG(&pAd->MacTab.Content[i], fCLIENT_STATUS_WMM_CAPABLE);
+		}
+
+		//========================================================
+		//      MAC Register has a copy .
+		//========================================================
+//#ifndef WIFI_TEST
+		if( pAd->CommonCfg.bEnableTxBurst )
+		{
+			// For CWC test, change txop from 0x30 to 0x20 in TxBurst mode
+			Ac0Cfg.field.AcTxop = 0x20; // Suggest by John for TxBurst in HT Mode
+		}
+		else
+			Ac0Cfg.field.AcTxop = 0;	// QID_AC_BE
+//#else
+//		Ac0Cfg.field.AcTxop = 0;	// QID_AC_BE
+//#endif
+		Ac0Cfg.field.Cwmin = CW_MIN_IN_BITS;
+		Ac0Cfg.field.Cwmax = CW_MAX_IN_BITS;
+		Ac0Cfg.field.Aifsn = 2;
+		RTMP_IO_WRITE32(pAd, EDCA_AC0_CFG, Ac0Cfg.word);
+
+		Ac1Cfg.field.AcTxop = 0;	// QID_AC_BK
+		Ac1Cfg.field.Cwmin = CW_MIN_IN_BITS;
+		Ac1Cfg.field.Cwmax = CW_MAX_IN_BITS;
+		Ac1Cfg.field.Aifsn = 2;
+		RTMP_IO_WRITE32(pAd, EDCA_AC1_CFG, Ac1Cfg.word);
+
+		if (pAd->CommonCfg.PhyMode == PHY_11B)
+		{
+			Ac2Cfg.field.AcTxop = 192;	// AC_VI: 192*32us ~= 6ms
+			Ac3Cfg.field.AcTxop = 96;	// AC_VO: 96*32us  ~= 3ms
+		}
+		else
+		{
+			Ac2Cfg.field.AcTxop = 96;	// AC_VI: 96*32us ~= 3ms
+			Ac3Cfg.field.AcTxop = 48;	// AC_VO: 48*32us ~= 1.5ms
+		}
+		Ac2Cfg.field.Cwmin = CW_MIN_IN_BITS;
+		Ac2Cfg.field.Cwmax = CW_MAX_IN_BITS;
+		Ac2Cfg.field.Aifsn = 2;
+		RTMP_IO_WRITE32(pAd, EDCA_AC2_CFG, Ac2Cfg.word);
+		Ac3Cfg.field.Cwmin = CW_MIN_IN_BITS;
+		Ac3Cfg.field.Cwmax = CW_MAX_IN_BITS;
+		Ac3Cfg.field.Aifsn = 2;
+		RTMP_IO_WRITE32(pAd, EDCA_AC3_CFG, Ac3Cfg.word);
+
+		//========================================================
+		//      DMA Register has a copy too.
+		//========================================================
+		csr0.field.Ac0Txop = 0;		// QID_AC_BE
+		csr0.field.Ac1Txop = 0;		// QID_AC_BK
+		RTMP_IO_WRITE32(pAd, WMM_TXOP0_CFG, csr0.word);
+		if (pAd->CommonCfg.PhyMode == PHY_11B)
+		{
+			csr1.field.Ac2Txop = 192;		// AC_VI: 192*32us ~= 6ms
+			csr1.field.Ac3Txop = 96;		// AC_VO: 96*32us  ~= 3ms
+		}
+		else
+		{
+			csr1.field.Ac2Txop = 96;		// AC_VI: 96*32us ~= 3ms
+			csr1.field.Ac3Txop = 48;		// AC_VO: 48*32us ~= 1.5ms
+		}
+		RTMP_IO_WRITE32(pAd, WMM_TXOP1_CFG, csr1.word);
+
+		CwminCsr.word = 0;
+		CwminCsr.field.Cwmin0 = CW_MIN_IN_BITS;
+		CwminCsr.field.Cwmin1 = CW_MIN_IN_BITS;
+		CwminCsr.field.Cwmin2 = CW_MIN_IN_BITS;
+		CwminCsr.field.Cwmin3 = CW_MIN_IN_BITS;
+		RTMP_IO_WRITE32(pAd, WMM_CWMIN_CFG, CwminCsr.word);
+
+		CwmaxCsr.word = 0;
+		CwmaxCsr.field.Cwmax0 = CW_MAX_IN_BITS;
+		CwmaxCsr.field.Cwmax1 = CW_MAX_IN_BITS;
+		CwmaxCsr.field.Cwmax2 = CW_MAX_IN_BITS;
+		CwmaxCsr.field.Cwmax3 = CW_MAX_IN_BITS;
+		RTMP_IO_WRITE32(pAd, WMM_CWMAX_CFG, CwmaxCsr.word);
+
+		RTMP_IO_WRITE32(pAd, WMM_AIFSN_CFG, 0x00002222);
+
+		NdisZeroMemory(&pAd->CommonCfg.APEdcaParm, sizeof(EDCA_PARM));
+	}
+	else
+	{
+		OPSTATUS_SET_FLAG(pAd, fOP_STATUS_WMM_INUSED);
+		//========================================================
+		//      MAC Register has a copy.
+		//========================================================
+		//
+		// Modify Cwmin/Cwmax/Txop on queue[QID_AC_VI], Recommend by Jerry 2005/07/27
+		// To degrade our VIDO Queue's throughput for WiFi WMM S3T07 Issue.
+		//
+		//pEdcaParm->Txop[QID_AC_VI] = pEdcaParm->Txop[QID_AC_VI] * 7 / 10; // rt2860c need this
+
+		Ac0Cfg.field.AcTxop =  pEdcaParm->Txop[QID_AC_BE];
+		Ac0Cfg.field.Cwmin= pEdcaParm->Cwmin[QID_AC_BE];
+		Ac0Cfg.field.Cwmax = pEdcaParm->Cwmax[QID_AC_BE];
+		Ac0Cfg.field.Aifsn = pEdcaParm->Aifsn[QID_AC_BE]; //+1;
+
+		Ac1Cfg.field.AcTxop =  pEdcaParm->Txop[QID_AC_BK];
+		Ac1Cfg.field.Cwmin = pEdcaParm->Cwmin[QID_AC_BK]; //+2;
+		Ac1Cfg.field.Cwmax = pEdcaParm->Cwmax[QID_AC_BK];
+		Ac1Cfg.field.Aifsn = pEdcaParm->Aifsn[QID_AC_BK]; //+1;
+
+		Ac2Cfg.field.AcTxop = (pEdcaParm->Txop[QID_AC_VI] * 6) / 10;
+		Ac2Cfg.field.Cwmin = pEdcaParm->Cwmin[QID_AC_VI];
+		Ac2Cfg.field.Cwmax = pEdcaParm->Cwmax[QID_AC_VI];
+		Ac2Cfg.field.Aifsn = pEdcaParm->Aifsn[QID_AC_VI];
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			// Tuning for Wi-Fi WMM S06
+			if (pAd->CommonCfg.bWiFiTest &&
+				pEdcaParm->Aifsn[QID_AC_VI] == 10)
+				Ac2Cfg.field.Aifsn -= 1;
+
+			// Tuning for TGn Wi-Fi 5.2.32
+			// STA TestBed changes in this item: conexant legacy sta ==> broadcom 11n sta
+			if (STA_TGN_WIFI_ON(pAd) &&
+				pEdcaParm->Aifsn[QID_AC_VI] == 10)
+			{
+				Ac0Cfg.field.Aifsn = 3;
+				Ac2Cfg.field.AcTxop = 5;
+			}
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+		Ac3Cfg.field.AcTxop = pEdcaParm->Txop[QID_AC_VO];
+		Ac3Cfg.field.Cwmin = pEdcaParm->Cwmin[QID_AC_VO];
+		Ac3Cfg.field.Cwmax = pEdcaParm->Cwmax[QID_AC_VO];
+		Ac3Cfg.field.Aifsn = pEdcaParm->Aifsn[QID_AC_VO];
+
+//#ifdef WIFI_TEST
+		if (pAd->CommonCfg.bWiFiTest)
+		{
+			if (Ac3Cfg.field.AcTxop == 102)
+			{
+			Ac0Cfg.field.AcTxop = pEdcaParm->Txop[QID_AC_BE] ? pEdcaParm->Txop[QID_AC_BE] : 10;
+				Ac0Cfg.field.Aifsn  = pEdcaParm->Aifsn[QID_AC_BE]-1; /* AIFSN must >= 1 */
+			Ac1Cfg.field.AcTxop = pEdcaParm->Txop[QID_AC_BK];
+				Ac1Cfg.field.Aifsn  = pEdcaParm->Aifsn[QID_AC_BK];
+			Ac2Cfg.field.AcTxop = pEdcaParm->Txop[QID_AC_VI];
+			} /* End of if */
+		}
+//#endif // WIFI_TEST //
+
+		RTMP_IO_WRITE32(pAd, EDCA_AC0_CFG, Ac0Cfg.word);
+		RTMP_IO_WRITE32(pAd, EDCA_AC1_CFG, Ac1Cfg.word);
+		RTMP_IO_WRITE32(pAd, EDCA_AC2_CFG, Ac2Cfg.word);
+		RTMP_IO_WRITE32(pAd, EDCA_AC3_CFG, Ac3Cfg.word);
+
+
+		//========================================================
+		//      DMA Register has a copy too.
+		//========================================================
+		csr0.field.Ac0Txop = Ac0Cfg.field.AcTxop;
+		csr0.field.Ac1Txop = Ac1Cfg.field.AcTxop;
+		RTMP_IO_WRITE32(pAd, WMM_TXOP0_CFG, csr0.word);
+
+		csr1.field.Ac2Txop = Ac2Cfg.field.AcTxop;
+		csr1.field.Ac3Txop = Ac3Cfg.field.AcTxop;
+		RTMP_IO_WRITE32(pAd, WMM_TXOP1_CFG, csr1.word);
+
+		CwminCsr.word = 0;
+		CwminCsr.field.Cwmin0 = pEdcaParm->Cwmin[QID_AC_BE];
+		CwminCsr.field.Cwmin1 = pEdcaParm->Cwmin[QID_AC_BK];
+		CwminCsr.field.Cwmin2 = pEdcaParm->Cwmin[QID_AC_VI];
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			CwminCsr.field.Cwmin3 = pEdcaParm->Cwmin[QID_AC_VO] - 1; //for TGn wifi test
+#endif // CONFIG_STA_SUPPORT //
+		RTMP_IO_WRITE32(pAd, WMM_CWMIN_CFG, CwminCsr.word);
+
+		CwmaxCsr.word = 0;
+		CwmaxCsr.field.Cwmax0 = pEdcaParm->Cwmax[QID_AC_BE];
+		CwmaxCsr.field.Cwmax1 = pEdcaParm->Cwmax[QID_AC_BK];
+		CwmaxCsr.field.Cwmax2 = pEdcaParm->Cwmax[QID_AC_VI];
+		CwmaxCsr.field.Cwmax3 = pEdcaParm->Cwmax[QID_AC_VO];
+		RTMP_IO_WRITE32(pAd, WMM_CWMAX_CFG, CwmaxCsr.word);
+
+		AifsnCsr.word = 0;
+		AifsnCsr.field.Aifsn0 = Ac0Cfg.field.Aifsn; //pEdcaParm->Aifsn[QID_AC_BE];
+		AifsnCsr.field.Aifsn1 = Ac1Cfg.field.Aifsn; //pEdcaParm->Aifsn[QID_AC_BK];
+		AifsnCsr.field.Aifsn2 = Ac2Cfg.field.Aifsn; //pEdcaParm->Aifsn[QID_AC_VI];
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			// Tuning for Wi-Fi WMM S06
+			if (pAd->CommonCfg.bWiFiTest &&
+				pEdcaParm->Aifsn[QID_AC_VI] == 10)
+				AifsnCsr.field.Aifsn2 = Ac2Cfg.field.Aifsn - 4;
+
+			// Tuning for TGn Wi-Fi 5.2.32
+			// STA TestBed changes in this item: conexant legacy sta ==> broadcom 11n sta
+			if (STA_TGN_WIFI_ON(pAd) &&
+				pEdcaParm->Aifsn[QID_AC_VI] == 10)
+			{
+				AifsnCsr.field.Aifsn0 = 3;
+				AifsnCsr.field.Aifsn2 = 7;
+			}
+
+			if (INFRA_ON(pAd))
+				CLIENT_STATUS_SET_FLAG(&pAd->MacTab.Content[BSSID_WCID], fCLIENT_STATUS_WMM_CAPABLE);
+		}
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			AifsnCsr.field.Aifsn3 = Ac3Cfg.field.Aifsn - 1; //pEdcaParm->Aifsn[QID_AC_VO]; //for TGn wifi test
+#endif // CONFIG_STA_SUPPORT //
+		RTMP_IO_WRITE32(pAd, WMM_AIFSN_CFG, AifsnCsr.word);
+
+		NdisMoveMemory(&pAd->CommonCfg.APEdcaParm, pEdcaParm, sizeof(EDCA_PARM));
+		if (!ADHOC_ON(pAd))
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("EDCA [#%d]: AIFSN CWmin CWmax  TXOP(us)  ACM\n", pEdcaParm->EdcaUpdateCount));
+			DBGPRINT(RT_DEBUG_TRACE,("     AC_BE      %2d     %2d     %2d      %4d     %d\n",
+									 pEdcaParm->Aifsn[0],
+									 pEdcaParm->Cwmin[0],
+									 pEdcaParm->Cwmax[0],
+									 pEdcaParm->Txop[0]<<5,
+									 pEdcaParm->bACM[0]));
+			DBGPRINT(RT_DEBUG_TRACE,("     AC_BK      %2d     %2d     %2d      %4d     %d\n",
+									 pEdcaParm->Aifsn[1],
+									 pEdcaParm->Cwmin[1],
+									 pEdcaParm->Cwmax[1],
+									 pEdcaParm->Txop[1]<<5,
+									 pEdcaParm->bACM[1]));
+			DBGPRINT(RT_DEBUG_TRACE,("     AC_VI      %2d     %2d     %2d      %4d     %d\n",
+									 pEdcaParm->Aifsn[2],
+									 pEdcaParm->Cwmin[2],
+									 pEdcaParm->Cwmax[2],
+									 pEdcaParm->Txop[2]<<5,
+									 pEdcaParm->bACM[2]));
+			DBGPRINT(RT_DEBUG_TRACE,("     AC_VO      %2d     %2d     %2d      %4d     %d\n",
+									 pEdcaParm->Aifsn[3],
+									 pEdcaParm->Cwmin[3],
+									 pEdcaParm->Cwmax[3],
+									 pEdcaParm->Txop[3]<<5,
+									 pEdcaParm->bACM[3]));
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID 	AsicSetSlotTime(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN bUseShortSlotTime)
+{
+	ULONG	SlotTime;
+	UINT32	RegValue = 0;
+
+#ifdef CONFIG_STA_SUPPORT
+	if (pAd->CommonCfg.Channel > 14)
+		bUseShortSlotTime = TRUE;
+#endif // CONFIG_STA_SUPPORT //
+
+	if (bUseShortSlotTime)
+		OPSTATUS_SET_FLAG(pAd, fOP_STATUS_SHORT_SLOT_INUSED);
+	else
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SHORT_SLOT_INUSED);
+
+	SlotTime = (bUseShortSlotTime)? 9 : 20;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// force using short SLOT time for FAE to demo performance when TxBurst is ON
+		if (((pAd->StaActive.SupportedPhyInfo.bHtEnable == FALSE) && (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED)))
+#ifdef DOT11_N_SUPPORT
+			|| ((pAd->StaActive.SupportedPhyInfo.bHtEnable == TRUE) && (pAd->CommonCfg.BACapability.field.Policy == BA_NOTUSE))
+#endif // DOT11_N_SUPPORT //
+			)
+		{
+			// In this case, we will think it is doing Wi-Fi test
+			// And we will not set to short slot when bEnableTxBurst is TRUE.
+		}
+		else if (pAd->CommonCfg.bEnableTxBurst)
+			SlotTime = 9;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	//
+	// For some reasons, always set it to short slot time.
+	//
+	// ToDo: Should consider capability with 11B
+	//
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (pAd->StaCfg.BssType == BSS_ADHOC)
+			SlotTime = 20;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	RTMP_IO_READ32(pAd, BKOFF_SLOT_CFG, &RegValue);
+	RegValue = RegValue & 0xFFFFFF00;
+
+	RegValue |= SlotTime;
+
+	RTMP_IO_WRITE32(pAd, BKOFF_SLOT_CFG, RegValue);
+}
+
+/*
+	========================================================================
+	Description:
+		Add Shared key information into ASIC.
+		Update shared key, TxMic and RxMic to Asic Shared key table
+		Update its cipherAlg to Asic Shared key Mode.
+
+    Return:
+	========================================================================
+*/
+VOID AsicAddSharedKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR		 BssIndex,
+	IN UCHAR		 KeyIdx,
+	IN UCHAR		 CipherAlg,
+	IN PUCHAR		 pKey,
+	IN PUCHAR		 pTxMic,
+	IN PUCHAR		 pRxMic)
+{
+	ULONG offset; //, csr0;
+	SHAREDKEY_MODE_STRUC csr1;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("AsicAddSharedKeyEntry BssIndex=%d, KeyIdx=%d\n", BssIndex,KeyIdx));
+//============================================================================================
+
+	DBGPRINT(RT_DEBUG_TRACE,("AsicAddSharedKeyEntry: %s key #%d\n", CipherName[CipherAlg], BssIndex*4 + KeyIdx));
+	DBGPRINT_RAW(RT_DEBUG_TRACE, (" 	Key = %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
+		pKey[0],pKey[1],pKey[2],pKey[3],pKey[4],pKey[5],pKey[6],pKey[7],pKey[8],pKey[9],pKey[10],pKey[11],pKey[12],pKey[13],pKey[14],pKey[15]));
+	if (pRxMic)
+	{
+		DBGPRINT_RAW(RT_DEBUG_TRACE, (" 	Rx MIC Key = %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
+			pRxMic[0],pRxMic[1],pRxMic[2],pRxMic[3],pRxMic[4],pRxMic[5],pRxMic[6],pRxMic[7]));
+	}
+	if (pTxMic)
+	{
+		DBGPRINT_RAW(RT_DEBUG_TRACE, (" 	Tx MIC Key = %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
+			pTxMic[0],pTxMic[1],pTxMic[2],pTxMic[3],pTxMic[4],pTxMic[5],pTxMic[6],pTxMic[7]));
+	}
+//============================================================================================
+	//
+	// fill key material - key + TX MIC + RX MIC
+	//
+
+#ifdef RT2870
+{
+	offset = SHARED_KEY_TABLE_BASE + (4*BssIndex + KeyIdx)*HW_KEY_ENTRY_SIZE;
+	RTUSBMultiWrite(pAd, offset, pKey, MAX_LEN_OF_SHARE_KEY);
+
+	offset += MAX_LEN_OF_SHARE_KEY;
+	if (pTxMic)
+	{
+		RTUSBMultiWrite(pAd, offset, pTxMic, 8);
+	}
+
+	offset += 8;
+	if (pRxMic)
+	{
+		RTUSBMultiWrite(pAd, offset, pRxMic, 8);
+	}
+}
+#endif // RT2870 //
+
+	//
+	// Update cipher algorithm. WSTA always use BSS0
+	//
+	RTMP_IO_READ32(pAd, SHARED_KEY_MODE_BASE+4*(BssIndex/2), &csr1.word);
+	DBGPRINT(RT_DEBUG_TRACE,("Read: SHARED_KEY_MODE_BASE at this Bss[%d] KeyIdx[%d]= 0x%x \n", BssIndex,KeyIdx, csr1.word));
+	if ((BssIndex%2) == 0)
+	{
+		if (KeyIdx == 0)
+			csr1.field.Bss0Key0CipherAlg = CipherAlg;
+		else if (KeyIdx == 1)
+			csr1.field.Bss0Key1CipherAlg = CipherAlg;
+		else if (KeyIdx == 2)
+			csr1.field.Bss0Key2CipherAlg = CipherAlg;
+		else
+			csr1.field.Bss0Key3CipherAlg = CipherAlg;
+	}
+	else
+	{
+		if (KeyIdx == 0)
+			csr1.field.Bss1Key0CipherAlg = CipherAlg;
+		else if (KeyIdx == 1)
+			csr1.field.Bss1Key1CipherAlg = CipherAlg;
+		else if (KeyIdx == 2)
+			csr1.field.Bss1Key2CipherAlg = CipherAlg;
+		else
+			csr1.field.Bss1Key3CipherAlg = CipherAlg;
+	}
+	DBGPRINT(RT_DEBUG_TRACE,("Write: SHARED_KEY_MODE_BASE at this Bss[%d] = 0x%x \n", BssIndex, csr1.word));
+	RTMP_IO_WRITE32(pAd, SHARED_KEY_MODE_BASE+4*(BssIndex/2), csr1.word);
+
+}
+
+//	IRQL = DISPATCH_LEVEL
+VOID AsicRemoveSharedKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR		 BssIndex,
+	IN UCHAR		 KeyIdx)
+{
+	//ULONG SecCsr0;
+	SHAREDKEY_MODE_STRUC csr1;
+
+	DBGPRINT(RT_DEBUG_TRACE,("AsicRemoveSharedKeyEntry: #%d \n", BssIndex*4 + KeyIdx));
+
+	RTMP_IO_READ32(pAd, SHARED_KEY_MODE_BASE+4*(BssIndex/2), &csr1.word);
+	if ((BssIndex%2) == 0)
+	{
+		if (KeyIdx == 0)
+			csr1.field.Bss0Key0CipherAlg = 0;
+		else if (KeyIdx == 1)
+			csr1.field.Bss0Key1CipherAlg = 0;
+		else if (KeyIdx == 2)
+			csr1.field.Bss0Key2CipherAlg = 0;
+		else
+			csr1.field.Bss0Key3CipherAlg = 0;
+	}
+	else
+	{
+		if (KeyIdx == 0)
+			csr1.field.Bss1Key0CipherAlg = 0;
+		else if (KeyIdx == 1)
+			csr1.field.Bss1Key1CipherAlg = 0;
+		else if (KeyIdx == 2)
+			csr1.field.Bss1Key2CipherAlg = 0;
+		else
+			csr1.field.Bss1Key3CipherAlg = 0;
+	}
+	DBGPRINT(RT_DEBUG_TRACE,("Write: SHARED_KEY_MODE_BASE at this Bss[%d] = 0x%x \n", BssIndex, csr1.word));
+	RTMP_IO_WRITE32(pAd, SHARED_KEY_MODE_BASE+4*(BssIndex/2), csr1.word);
+	ASSERT(BssIndex < 4);
+	ASSERT(KeyIdx < 4);
+
+}
+
+
+VOID AsicUpdateWCIDAttribute(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT		WCID,
+	IN UCHAR		BssIndex,
+	IN UCHAR        CipherAlg,
+	IN BOOLEAN		bUsePairewiseKeyTable)
+{
+	ULONG   WCIDAttri = 0, offset;
+
+	//
+	// Update WCID attribute.
+	// Only TxKey could update WCID attribute.
+	//
+	offset = MAC_WCID_ATTRIBUTE_BASE + (WCID * HW_WCID_ATTRI_SIZE);
+	WCIDAttri = (BssIndex << 4) | (CipherAlg << 1) | (bUsePairewiseKeyTable);
+	RTMP_IO_WRITE32(pAd, offset, WCIDAttri);
+}
+
+VOID AsicUpdateWCIDIVEIV(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT		WCID,
+	IN ULONG        uIV,
+	IN ULONG        uEIV)
+{
+	ULONG	offset;
+
+	offset = MAC_IVEIV_TABLE_BASE + (WCID * HW_IVEIV_ENTRY_SIZE);
+
+	RTMP_IO_WRITE32(pAd, offset, uIV);
+	RTMP_IO_WRITE32(pAd, offset + 4, uEIV);
+}
+
+VOID AsicUpdateRxWCIDTable(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT		WCID,
+	IN PUCHAR        pAddr)
+{
+	ULONG offset;
+	ULONG Addr;
+
+	offset = MAC_WCID_BASE + (WCID * HW_WCID_ENTRY_SIZE);
+	Addr = pAddr[0] + (pAddr[1] << 8) +(pAddr[2] << 16) +(pAddr[3] << 24);
+	RTMP_IO_WRITE32(pAd, offset, Addr);
+	Addr = pAddr[4] + (pAddr[5] << 8);
+	RTMP_IO_WRITE32(pAd, offset + 4, Addr);
+}
+
+
+/*
+    ========================================================================
+
+    Routine Description:
+        Set Cipher Key, Cipher algorithm, IV/EIV to Asic
+
+    Arguments:
+        pAd                     Pointer to our adapter
+        WCID                    WCID Entry number.
+        BssIndex                BSSID index, station or none multiple BSSID support
+                                this value should be 0.
+        KeyIdx                  This KeyIdx will set to IV's KeyID if bTxKey enabled
+        pCipherKey              Pointer to Cipher Key.
+        bUsePairewiseKeyTable   TRUE means saved the key in SharedKey table,
+                                otherwise PairewiseKey table
+        bTxKey                  This is the transmit key if enabled.
+
+    Return Value:
+        None
+
+    Note:
+        This routine will set the relative key stuff to Asic including WCID attribute,
+        Cipher Key, Cipher algorithm and IV/EIV.
+
+        IV/EIV will be update if this CipherKey is the transmission key because
+        ASIC will base on IV's KeyID value to select Cipher Key.
+
+        If bTxKey sets to FALSE, this is not the TX key, but it could be
+        RX key
+
+    	For AP mode bTxKey must be always set to TRUE.
+    ========================================================================
+*/
+VOID AsicAddKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT		WCID,
+	IN UCHAR		BssIndex,
+	IN UCHAR		KeyIdx,
+	IN PCIPHER_KEY	pCipherKey,
+	IN BOOLEAN		bUsePairewiseKeyTable,
+	IN BOOLEAN		bTxKey)
+{
+	ULONG	offset;
+//	ULONG   WCIDAttri = 0;
+	UCHAR	IV4 = 0;
+	PUCHAR		pKey = pCipherKey->Key;
+//	ULONG		KeyLen = pCipherKey->KeyLen;
+	PUCHAR		pTxMic = pCipherKey->TxMic;
+	PUCHAR		pRxMic = pCipherKey->RxMic;
+	PUCHAR		pTxtsc = pCipherKey->TxTsc;
+	UCHAR		CipherAlg = pCipherKey->CipherAlg;
+	SHAREDKEY_MODE_STRUC csr1;
+
+//	ASSERT(KeyLen <= MAX_LEN_OF_PEER_KEY);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("==> AsicAddKeyEntry\n"));
+	//
+	// 1.) decide key table offset
+	//
+	if (bUsePairewiseKeyTable)
+		offset = PAIRWISE_KEY_TABLE_BASE + (WCID * HW_KEY_ENTRY_SIZE);
+	else
+		offset = SHARED_KEY_TABLE_BASE + (4 * BssIndex + KeyIdx) * HW_KEY_ENTRY_SIZE;
+
+	//
+	// 2.) Set Key to Asic
+	//
+	//for (i = 0; i < KeyLen; i++)
+
+#ifdef RT2870
+	RTUSBMultiWrite(pAd, offset, pKey, MAX_LEN_OF_PEER_KEY);
+	offset += MAX_LEN_OF_PEER_KEY;
+
+	//
+	// 3.) Set MIC key if available
+	//
+	if (pTxMic)
+	{
+		RTUSBMultiWrite(pAd, offset, pTxMic, 8);
+	}
+	offset += LEN_TKIP_TXMICK;
+
+	if (pRxMic)
+	{
+		RTUSBMultiWrite(pAd, offset, pRxMic, 8);
+	}
+#endif // RT2870 //
+
+	//
+	// 4.) Modify IV/EIV if needs
+	//     This will force Asic to use this key ID by setting IV.
+	//
+	if (bTxKey)
+	{
+
+#ifdef RT2870
+		UINT32 tmpVal;
+
+		//
+		// Write IV
+		//
+		IV4 = (KeyIdx << 6);
+		if ((CipherAlg == CIPHER_TKIP) || (CipherAlg == CIPHER_TKIP_NO_MIC) ||(CipherAlg == CIPHER_AES))
+			IV4 |= 0x20;  // turn on extension bit means EIV existence
+
+		tmpVal = pTxtsc[1] + (((pTxtsc[1] | 0x20) & 0x7f) << 8) + (pTxtsc[0] << 16) + (IV4 << 24);
+		RTMP_IO_WRITE32(pAd, offset, tmpVal);
+
+		//
+		// Write EIV
+		//
+		offset += 4;
+		RTMP_IO_WRITE32(pAd, offset, *(PUINT32)&pCipherKey->TxTsc[2]);
+#endif // RT2870 //
+		AsicUpdateWCIDAttribute(pAd, WCID, BssIndex, CipherAlg, bUsePairewiseKeyTable);
+	}
+
+	if (!bUsePairewiseKeyTable)
+	{
+		//
+		// Only update the shared key security mode
+		//
+		RTMP_IO_READ32(pAd, SHARED_KEY_MODE_BASE + 4 * (BssIndex / 2), &csr1.word);
+		if ((BssIndex % 2) == 0)
+		{
+			if (KeyIdx == 0)
+				csr1.field.Bss0Key0CipherAlg = CipherAlg;
+			else if (KeyIdx == 1)
+				csr1.field.Bss0Key1CipherAlg = CipherAlg;
+			else if (KeyIdx == 2)
+				csr1.field.Bss0Key2CipherAlg = CipherAlg;
+			else
+				csr1.field.Bss0Key3CipherAlg = CipherAlg;
+		}
+		else
+		{
+			if (KeyIdx == 0)
+				csr1.field.Bss1Key0CipherAlg = CipherAlg;
+			else if (KeyIdx == 1)
+				csr1.field.Bss1Key1CipherAlg = CipherAlg;
+			else if (KeyIdx == 2)
+				csr1.field.Bss1Key2CipherAlg = CipherAlg;
+			else
+				csr1.field.Bss1Key3CipherAlg = CipherAlg;
+		}
+		RTMP_IO_WRITE32(pAd, SHARED_KEY_MODE_BASE + 4 * (BssIndex / 2), csr1.word);
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<== AsicAddKeyEntry\n"));
+}
+
+
+/*
+	========================================================================
+	Description:
+		Add Pair-wise key material into ASIC.
+		Update pairwise key, TxMic and RxMic to Asic Pair-wise key table
+
+    Return:
+	========================================================================
+*/
+VOID AsicAddPairwiseKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR        pAddr,
+	IN UCHAR		WCID,
+	IN CIPHER_KEY		 *pCipherKey)
+{
+	INT i;
+	ULONG 		offset;
+	PUCHAR		 pKey = pCipherKey->Key;
+	PUCHAR		 pTxMic = pCipherKey->TxMic;
+	PUCHAR		 pRxMic = pCipherKey->RxMic;
+#ifdef DBG
+	UCHAR		CipherAlg = pCipherKey->CipherAlg;
+#endif // DBG //
+
+	// EKEY
+	offset = PAIRWISE_KEY_TABLE_BASE + (WCID * HW_KEY_ENTRY_SIZE);
+#ifdef RT2870
+	RTUSBMultiWrite(pAd, offset, &pCipherKey->Key[0], MAX_LEN_OF_PEER_KEY);
+#endif // RT2870 //
+	for (i=0; i<MAX_LEN_OF_PEER_KEY; i+=4)
+	{
+		UINT32 Value;
+		RTMP_IO_READ32(pAd, offset + i, &Value);
+	}
+
+	offset += MAX_LEN_OF_PEER_KEY;
+
+	//  MIC KEY
+	if (pTxMic)
+	{
+#ifdef RT2870
+		RTUSBMultiWrite(pAd, offset, &pCipherKey->TxMic[0], 8);
+#endif // RT2870 //
+	}
+	offset += 8;
+	if (pRxMic)
+	{
+#ifdef RT2870
+		RTUSBMultiWrite(pAd, offset, &pCipherKey->RxMic[0], 8);
+#endif // RT2870 //
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("AsicAddPairwiseKeyEntry: WCID #%d Alg=%s\n",WCID, CipherName[CipherAlg]));
+	DBGPRINT(RT_DEBUG_TRACE,("	Key = %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
+		pKey[0],pKey[1],pKey[2],pKey[3],pKey[4],pKey[5],pKey[6],pKey[7],pKey[8],pKey[9],pKey[10],pKey[11],pKey[12],pKey[13],pKey[14],pKey[15]));
+	if (pRxMic)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("	Rx MIC Key = %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
+			pRxMic[0],pRxMic[1],pRxMic[2],pRxMic[3],pRxMic[4],pRxMic[5],pRxMic[6],pRxMic[7]));
+	}
+	if (pTxMic)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("	Tx MIC Key = %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
+			pTxMic[0],pTxMic[1],pTxMic[2],pTxMic[3],pTxMic[4],pTxMic[5],pTxMic[6],pTxMic[7]));
+	}
+}
+/*
+	========================================================================
+	Description:
+		Remove Pair-wise key material from ASIC.
+
+    Return:
+	========================================================================
+*/
+VOID AsicRemovePairwiseKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR		 BssIdx,
+	IN UCHAR		 Wcid)
+{
+	ULONG		WCIDAttri;
+	USHORT		offset;
+
+	// re-set the entry's WCID attribute as OPEN-NONE.
+	offset = MAC_WCID_ATTRIBUTE_BASE + (Wcid * HW_WCID_ATTRI_SIZE);
+	WCIDAttri = (BssIdx<<4) | PAIRWISEKEYTABLE;
+	RTMP_IO_WRITE32(pAd, offset, WCIDAttri);
+}
+
+BOOLEAN AsicSendCommandToMcu(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR		 Command,
+	IN UCHAR		 Token,
+	IN UCHAR		 Arg0,
+	IN UCHAR		 Arg1)
+{
+	HOST_CMD_CSR_STRUC	H2MCmd;
+	H2M_MAILBOX_STRUC	H2MMailbox;
+	ULONG				i = 0;
+	do
+	{
+		RTMP_IO_READ32(pAd, H2M_MAILBOX_CSR, &H2MMailbox.word);
+		if (H2MMailbox.field.Owner == 0)
+			break;
+
+		RTMPusecDelay(2);
+	} while(i++ < 100);
+
+	if (i >= 100)
+	{
+		{
+		DBGPRINT_ERR(("H2M_MAILBOX still hold by MCU. command fail\n"));
+		}
+		return FALSE;
+	}
+
+
+	H2MMailbox.field.Owner	  = 1;	   // pass ownership to MCU
+	H2MMailbox.field.CmdToken = Token;
+	H2MMailbox.field.HighByte = Arg1;
+	H2MMailbox.field.LowByte  = Arg0;
+	RTMP_IO_WRITE32(pAd, H2M_MAILBOX_CSR, H2MMailbox.word);
+
+	H2MCmd.word 			  = 0;
+	H2MCmd.field.HostCommand  = Command;
+	RTMP_IO_WRITE32(pAd, HOST_CMD_CSR, H2MCmd.word);
+
+	if (Command != 0x80)
+	{
+	}
+
+	return TRUE;
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Verify the support rate for different PHY type
+
+	Arguments:
+		pAd 				Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	========================================================================
+*/
+VOID	RTMPCheckRates(
+	IN		PRTMP_ADAPTER	pAd,
+	IN OUT	UCHAR			SupRate[],
+	IN OUT	UCHAR			*SupRateLen)
+{
+	UCHAR	RateIdx, i, j;
+	UCHAR	NewRate[12], NewRateLen;
+
+	NewRateLen = 0;
+
+	if (pAd->CommonCfg.PhyMode == PHY_11B)
+		RateIdx = 4;
+	else
+		RateIdx = 12;
+
+	// Check for support rates exclude basic rate bit
+	for (i = 0; i < *SupRateLen; i++)
+		for (j = 0; j < RateIdx; j++)
+			if ((SupRate[i] & 0x7f) == RateIdTo500Kbps[j])
+				NewRate[NewRateLen++] = SupRate[i];
+
+	*SupRateLen = NewRateLen;
+	NdisMoveMemory(SupRate, NewRate, NewRateLen);
+}
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef DOT11_N_SUPPORT
+BOOLEAN RTMPCheckChannel(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR		CentralChannel,
+	IN UCHAR		Channel)
+{
+	UCHAR		k;
+	UCHAR		UpperChannel = 0, LowerChannel = 0;
+	UCHAR		NoEffectChannelinList = 0;
+
+	// Find upper and lower channel according to 40MHz current operation.
+	if (CentralChannel < Channel)
+	{
+		UpperChannel = Channel;
+		if (CentralChannel > 2)
+			LowerChannel = CentralChannel - 2;
+		else
+			return FALSE;
+	}
+	else if (CentralChannel > Channel)
+	{
+		UpperChannel = CentralChannel + 2;
+		LowerChannel = Channel;
+	}
+
+	for (k = 0;k < pAd->ChannelListNum;k++)
+	{
+		if (pAd->ChannelList[k].Channel == UpperChannel)
+		{
+			NoEffectChannelinList ++;
+		}
+		if (pAd->ChannelList[k].Channel == LowerChannel)
+		{
+			NoEffectChannelinList ++;
+		}
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("Total Channel in Channel List = [%d]\n", NoEffectChannelinList));
+	if (NoEffectChannelinList == 2)
+		return TRUE;
+	else
+		return FALSE;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Verify the support rate for HT phy type
+
+	Arguments:
+		pAd 				Pointer to our adapter
+
+	Return Value:
+		FALSE if pAd->CommonCfg.SupportedHtPhy doesn't accept the pHtCapability.  (AP Mode)
+
+	IRQL = PASSIVE_LEVEL
+
+	========================================================================
+*/
+BOOLEAN 	RTMPCheckHt(
+	IN	PRTMP_ADAPTER			pAd,
+	IN	UCHAR					Wcid,
+	IN 	HT_CAPABILITY_IE		*pHtCapability,
+	IN 	ADD_HT_INFO_IE			*pAddHtInfo)
+{
+	if (Wcid >= MAX_LEN_OF_MAC_TABLE)
+		return FALSE;
+
+	// If use AMSDU, set flag.
+	if (pAd->CommonCfg.DesiredHtPhy.AmsduEnable)
+		CLIENT_STATUS_SET_FLAG(&pAd->MacTab.Content[Wcid], fCLIENT_STATUS_AMSDU_INUSED);
+	// Save Peer Capability
+	if (pHtCapability->HtCapInfo.ShortGIfor20)
+		CLIENT_STATUS_SET_FLAG(&pAd->MacTab.Content[Wcid], fCLIENT_STATUS_SGI20_CAPABLE);
+	if (pHtCapability->HtCapInfo.ShortGIfor40)
+		CLIENT_STATUS_SET_FLAG(&pAd->MacTab.Content[Wcid], fCLIENT_STATUS_SGI40_CAPABLE);
+	if (pHtCapability->HtCapInfo.TxSTBC)
+		CLIENT_STATUS_SET_FLAG(&pAd->MacTab.Content[Wcid], fCLIENT_STATUS_TxSTBC_CAPABLE);
+	if (pHtCapability->HtCapInfo.RxSTBC)
+		CLIENT_STATUS_SET_FLAG(&pAd->MacTab.Content[Wcid], fCLIENT_STATUS_RxSTBC_CAPABLE);
+	if (pAd->CommonCfg.bRdg && pHtCapability->ExtHtCapInfo.RDGSupport)
+	{
+		CLIENT_STATUS_SET_FLAG(&pAd->MacTab.Content[Wcid], fCLIENT_STATUS_RDG_CAPABLE);
+	}
+
+	if (Wcid < MAX_LEN_OF_MAC_TABLE)
+	{
+		pAd->MacTab.Content[Wcid].MpduDensity = pHtCapability->HtCapParm.MpduDensity;
+	}
+
+	// Will check ChannelWidth for MCSSet[4] below
+	pAd->MlmeAux.HtCapability.MCSSet[4] = 0x1;
+    switch (pAd->CommonCfg.RxStream)
+	{
+		case 1:
+			pAd->MlmeAux.HtCapability.MCSSet[0] = 0xff;
+			pAd->MlmeAux.HtCapability.MCSSet[1] = 0x00;
+            pAd->MlmeAux.HtCapability.MCSSet[2] = 0x00;
+            pAd->MlmeAux.HtCapability.MCSSet[3] = 0x00;
+			break;
+		case 2:
+			pAd->MlmeAux.HtCapability.MCSSet[0] = 0xff;
+			pAd->MlmeAux.HtCapability.MCSSet[1] = 0xff;
+            pAd->MlmeAux.HtCapability.MCSSet[2] = 0x00;
+            pAd->MlmeAux.HtCapability.MCSSet[3] = 0x00;
+			break;
+		case 3:
+			pAd->MlmeAux.HtCapability.MCSSet[0] = 0xff;
+			pAd->MlmeAux.HtCapability.MCSSet[1] = 0xff;
+            pAd->MlmeAux.HtCapability.MCSSet[2] = 0xff;
+            pAd->MlmeAux.HtCapability.MCSSet[3] = 0x00;
+			break;
+	}
+
+	pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth = pAddHtInfo->AddHtInfo.RecomWidth & pAd->CommonCfg.DesiredHtPhy.ChannelWidth;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("RTMPCheckHt:: HtCapInfo.ChannelWidth=%d, RecomWidth=%d, DesiredHtPhy.ChannelWidth=%d, BW40MAvailForA/G=%d/%d, PhyMode=%d \n",
+		pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth, pAddHtInfo->AddHtInfo.RecomWidth, pAd->CommonCfg.DesiredHtPhy.ChannelWidth,
+		pAd->NicConfig2.field.BW40MAvailForA, pAd->NicConfig2.field.BW40MAvailForG, pAd->CommonCfg.PhyMode));
+
+	pAd->MlmeAux.HtCapability.HtCapInfo.GF =  pHtCapability->HtCapInfo.GF &pAd->CommonCfg.DesiredHtPhy.GF;
+
+	// Send Assoc Req with my HT capability.
+	pAd->MlmeAux.HtCapability.HtCapInfo.AMsduSize =  pAd->CommonCfg.DesiredHtPhy.AmsduSize;
+	pAd->MlmeAux.HtCapability.HtCapInfo.MimoPs =  pAd->CommonCfg.DesiredHtPhy.MimoPs;
+	pAd->MlmeAux.HtCapability.HtCapInfo.ShortGIfor20 =  (pAd->CommonCfg.DesiredHtPhy.ShortGIfor20) & (pHtCapability->HtCapInfo.ShortGIfor20);
+	pAd->MlmeAux.HtCapability.HtCapInfo.ShortGIfor40 =  (pAd->CommonCfg.DesiredHtPhy.ShortGIfor40) & (pHtCapability->HtCapInfo.ShortGIfor40);
+	pAd->MlmeAux.HtCapability.HtCapInfo.TxSTBC =  (pAd->CommonCfg.DesiredHtPhy.TxSTBC)&(pHtCapability->HtCapInfo.RxSTBC);
+	pAd->MlmeAux.HtCapability.HtCapInfo.RxSTBC =  (pAd->CommonCfg.DesiredHtPhy.RxSTBC)&(pHtCapability->HtCapInfo.TxSTBC);
+	pAd->MlmeAux.HtCapability.HtCapParm.MaxRAmpduFactor = pAd->CommonCfg.DesiredHtPhy.MaxRAmpduFactor;
+    pAd->MlmeAux.HtCapability.HtCapParm.MpduDensity = pAd->CommonCfg.HtCapability.HtCapParm.MpduDensity;
+	pAd->MlmeAux.HtCapability.ExtHtCapInfo.PlusHTC = pHtCapability->ExtHtCapInfo.PlusHTC;
+	pAd->MacTab.Content[Wcid].HTCapability.ExtHtCapInfo.PlusHTC = pHtCapability->ExtHtCapInfo.PlusHTC;
+	if (pAd->CommonCfg.bRdg)
+	{
+		pAd->MlmeAux.HtCapability.ExtHtCapInfo.RDGSupport = pHtCapability->ExtHtCapInfo.RDGSupport;
+        pAd->MlmeAux.HtCapability.ExtHtCapInfo.PlusHTC = 1;
+	}
+
+    if (pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth == BW_20)
+        pAd->MlmeAux.HtCapability.MCSSet[4] = 0x0;  // BW20 can't transmit MCS32
+
+	COPY_AP_HTSETTINGS_FROM_BEACON(pAd, pHtCapability);
+	return TRUE;
+}
+#endif // DOT11_N_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+/*
+	========================================================================
+
+	Routine Description:
+		Verify the support rate for different PHY type
+
+	Arguments:
+		pAd 				Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	========================================================================
+*/
+VOID RTMPUpdateMlmeRate(
+	IN PRTMP_ADAPTER	pAd)
+{
+	UCHAR	MinimumRate;
+	UCHAR	ProperMlmeRate; //= RATE_54;
+	UCHAR	i, j, RateIdx = 12; //1, 2, 5.5, 11, 6, 9, 12, 18, 24, 36, 48, 54
+	BOOLEAN	bMatch = FALSE;
+
+	switch (pAd->CommonCfg.PhyMode)
+	{
+		case PHY_11B:
+			ProperMlmeRate = RATE_11;
+			MinimumRate = RATE_1;
+			break;
+		case PHY_11BG_MIXED:
+#ifdef DOT11_N_SUPPORT
+		case PHY_11ABGN_MIXED:
+		case PHY_11BGN_MIXED:
+#endif // DOT11_N_SUPPORT //
+			if ((pAd->MlmeAux.SupRateLen == 4) &&
+				(pAd->MlmeAux.ExtRateLen == 0))
+				// B only AP
+				ProperMlmeRate = RATE_11;
+			else
+				ProperMlmeRate = RATE_24;
+
+			if (pAd->MlmeAux.Channel <= 14)
+				MinimumRate = RATE_1;
+			else
+				MinimumRate = RATE_6;
+			break;
+		case PHY_11A:
+#ifdef DOT11_N_SUPPORT
+		case PHY_11N_2_4G:	// rt2860 need to check mlmerate for 802.11n
+		case PHY_11GN_MIXED:
+		case PHY_11AGN_MIXED:
+		case PHY_11AN_MIXED:
+		case PHY_11N_5G:
+#endif // DOT11_N_SUPPORT //
+			ProperMlmeRate = RATE_24;
+			MinimumRate = RATE_6;
+			break;
+		case PHY_11ABG_MIXED:
+			ProperMlmeRate = RATE_24;
+			if (pAd->MlmeAux.Channel <= 14)
+			   MinimumRate = RATE_1;
+			else
+				MinimumRate = RATE_6;
+			break;
+		default: // error
+			ProperMlmeRate = RATE_1;
+			MinimumRate = RATE_1;
+			break;
+	}
+
+	for (i = 0; i < pAd->MlmeAux.SupRateLen; i++)
+	{
+		for (j = 0; j < RateIdx; j++)
+		{
+			if ((pAd->MlmeAux.SupRate[i] & 0x7f) == RateIdTo500Kbps[j])
+			{
+				if (j == ProperMlmeRate)
+				{
+					bMatch = TRUE;
+					break;
+				}
+			}
+		}
+
+		if (bMatch)
+			break;
+	}
+
+	if (bMatch == FALSE)
+	{
+		for (i = 0; i < pAd->MlmeAux.ExtRateLen; i++)
+		{
+			for (j = 0; j < RateIdx; j++)
+			{
+				if ((pAd->MlmeAux.ExtRate[i] & 0x7f) == RateIdTo500Kbps[j])
+				{
+					if (j == ProperMlmeRate)
+					{
+						bMatch = TRUE;
+						break;
+					}
+				}
+			}
+
+			if (bMatch)
+				break;
+		}
+	}
+
+	if (bMatch == FALSE)
+	{
+		ProperMlmeRate = MinimumRate;
+	}
+
+	pAd->CommonCfg.MlmeRate = MinimumRate;
+	pAd->CommonCfg.RtsRate = ProperMlmeRate;
+	if (pAd->CommonCfg.MlmeRate >= RATE_6)
+	{
+		pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_OFDM;
+		pAd->CommonCfg.MlmeTransmit.field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MlmeRate];
+		pAd->MacTab.Content[BSS0Mcast_WCID].HTPhyMode.field.MODE = MODE_OFDM;
+		pAd->MacTab.Content[BSS0Mcast_WCID].HTPhyMode.field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MlmeRate];
+	}
+	else
+	{
+		pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_CCK;
+		pAd->CommonCfg.MlmeTransmit.field.MCS = pAd->CommonCfg.MlmeRate;
+		pAd->MacTab.Content[BSS0Mcast_WCID].HTPhyMode.field.MODE = MODE_CCK;
+		pAd->MacTab.Content[BSS0Mcast_WCID].HTPhyMode.field.MCS = pAd->CommonCfg.MlmeRate;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPUpdateMlmeRate ==>   MlmeTransmit = 0x%x  \n" , pAd->CommonCfg.MlmeTransmit.word));
+}
+
+CHAR RTMPMaxRssi(
+	IN PRTMP_ADAPTER	pAd,
+	IN CHAR				Rssi0,
+	IN CHAR				Rssi1,
+	IN CHAR				Rssi2)
+{
+	CHAR	larger = -127;
+
+	if ((pAd->Antenna.field.RxPath == 1) && (Rssi0 != 0))
+	{
+		larger = Rssi0;
+	}
+
+	if ((pAd->Antenna.field.RxPath >= 2) && (Rssi1 != 0))
+	{
+		larger = max(Rssi0, Rssi1);
+	}
+
+	if ((pAd->Antenna.field.RxPath == 3) && (Rssi2 != 0))
+	{
+		larger = max(larger, Rssi2);
+	}
+
+	if (larger == -127)
+		larger = 0;
+
+	return larger;
+}
+
+/*
+    ========================================================================
+    Routine Description:
+        Periodic evaluate antenna link status
+
+    Arguments:
+        pAd         - Adapter pointer
+
+    Return Value:
+        None
+
+    ========================================================================
+*/
+VOID AsicEvaluateRxAnt(
+	IN PRTMP_ADAPTER	pAd)
+{
+	UCHAR	BBPR3 = 0;
+
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS	|
+								fRTMP_ADAPTER_HALT_IN_PROGRESS	|
+								fRTMP_ADAPTER_RADIO_OFF			|
+								fRTMP_ADAPTER_NIC_NOT_EXIST		|
+								fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+			return;
+
+		if (pAd->StaCfg.Psm == PWR_SAVE)
+			return;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BBPR3);
+	BBPR3 &= (~0x18);
+	if(pAd->Antenna.field.RxPath == 3)
+	{
+		BBPR3 |= (0x10);
+	}
+	else if(pAd->Antenna.field.RxPath == 2)
+	{
+		BBPR3 |= (0x8);
+	}
+	else if(pAd->Antenna.field.RxPath == 1)
+	{
+		BBPR3 |= (0x0);
+	}
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BBPR3);
+#ifdef CONFIG_STA_SUPPORT
+#endif // CONFIG_STA_SUPPORT //
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED)
+		)
+	{
+		ULONG	TxTotalCnt = pAd->RalinkCounters.OneSecTxNoRetryOkCount +
+								pAd->RalinkCounters.OneSecTxRetryOkCount +
+								pAd->RalinkCounters.OneSecTxFailCount;
+
+		if (TxTotalCnt > 50)
+		{
+			RTMPSetTimer(&pAd->Mlme.RxAntEvalTimer, 20);
+			pAd->Mlme.bLowThroughput = FALSE;
+		}
+		else
+		{
+			RTMPSetTimer(&pAd->Mlme.RxAntEvalTimer, 300);
+			pAd->Mlme.bLowThroughput = TRUE;
+		}
+	}
+}
+
+/*
+    ========================================================================
+    Routine Description:
+        After evaluation, check antenna link status
+
+    Arguments:
+        pAd         - Adapter pointer
+
+    Return Value:
+        None
+
+    ========================================================================
+*/
+VOID AsicRxAntEvalTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+	RTMP_ADAPTER	*pAd = (RTMP_ADAPTER *)FunctionContext;
+#ifdef CONFIG_STA_SUPPORT
+	UCHAR			BBPR3 = 0;
+	CHAR			larger = -127, rssi0, rssi1, rssi2;
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)	||
+			RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)		||
+			RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF)			||
+			RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST))
+			return;
+
+		if (pAd->StaCfg.Psm == PWR_SAVE)
+			return;
+
+
+		// if the traffic is low, use average rssi as the criteria
+		if (pAd->Mlme.bLowThroughput == TRUE)
+		{
+			rssi0 = pAd->StaCfg.RssiSample.LastRssi0;
+			rssi1 = pAd->StaCfg.RssiSample.LastRssi1;
+			rssi2 = pAd->StaCfg.RssiSample.LastRssi2;
+		}
+		else
+		{
+			rssi0 = pAd->StaCfg.RssiSample.AvgRssi0;
+			rssi1 = pAd->StaCfg.RssiSample.AvgRssi1;
+			rssi2 = pAd->StaCfg.RssiSample.AvgRssi2;
+		}
+
+		if(pAd->Antenna.field.RxPath == 3)
+		{
+			larger = max(rssi0, rssi1);
+
+			if (larger > (rssi2 + 20))
+				pAd->Mlme.RealRxPath = 2;
+			else
+				pAd->Mlme.RealRxPath = 3;
+		}
+		else if(pAd->Antenna.field.RxPath == 2)
+		{
+			if (rssi0 > (rssi1 + 20))
+				pAd->Mlme.RealRxPath = 1;
+			else
+				pAd->Mlme.RealRxPath = 2;
+		}
+
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BBPR3);
+		BBPR3 &= (~0x18);
+		if(pAd->Mlme.RealRxPath == 3)
+		{
+			BBPR3 |= (0x10);
+		}
+		else if(pAd->Mlme.RealRxPath == 2)
+		{
+			BBPR3 |= (0x8);
+		}
+		else if(pAd->Mlme.RealRxPath == 1)
+		{
+			BBPR3 |= (0x0);
+		}
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BBPR3);
+	}
+
+#endif // CONFIG_STA_SUPPORT //
+
+}
+
+
+
+VOID APSDPeriodicExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+	RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+
+	if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+		return;
+
+	pAd->CommonCfg.TriggerTimerCount++;
+
+// Driver should not send trigger frame, it should be send by application layer
+/*
+	if (pAd->CommonCfg.bAPSDCapable && pAd->CommonCfg.APEdcaParm.bAPSDCapable
+		&& (pAd->CommonCfg.bNeedSendTriggerFrame ||
+		(((pAd->CommonCfg.TriggerTimerCount%20) == 19) && (!pAd->CommonCfg.bAPSDAC_BE || !pAd->CommonCfg.bAPSDAC_BK || !pAd->CommonCfg.bAPSDAC_VI || !pAd->CommonCfg.bAPSDAC_VO))))
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("Sending trigger frame and enter service period when support APSD\n"));
+		RTMPSendNullFrame(pAd, pAd->CommonCfg.TxRate, TRUE);
+		pAd->CommonCfg.bNeedSendTriggerFrame = FALSE;
+		pAd->CommonCfg.TriggerTimerCount = 0;
+		pAd->CommonCfg.bInServicePeriod = TRUE;
+	}*/
+}
+
+/*
+    ========================================================================
+    Routine Description:
+        Set/reset MAC registers according to bPiggyBack parameter
+
+    Arguments:
+        pAd         - Adapter pointer
+        bPiggyBack  - Enable / Disable Piggy-Back
+
+    Return Value:
+        None
+
+    ========================================================================
+*/
+VOID RTMPSetPiggyBack(
+    IN PRTMP_ADAPTER    pAd,
+    IN BOOLEAN          bPiggyBack)
+{
+	TX_LINK_CFG_STRUC  TxLinkCfg;
+
+	RTMP_IO_READ32(pAd, TX_LINK_CFG, &TxLinkCfg.word);
+
+	TxLinkCfg.field.TxCFAckEn = bPiggyBack;
+	RTMP_IO_WRITE32(pAd, TX_LINK_CFG, TxLinkCfg.word);
+}
+
+/*
+    ========================================================================
+    Routine Description:
+        check if this entry need to switch rate automatically
+
+    Arguments:
+        pAd
+        pEntry
+
+    Return Value:
+        TURE
+        FALSE
+
+    ========================================================================
+*/
+BOOLEAN RTMPCheckEntryEnableAutoRateSwitch(
+	IN PRTMP_ADAPTER    pAd,
+	IN PMAC_TABLE_ENTRY	pEntry)
+{
+	BOOLEAN		result = TRUE;
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// only associated STA counts
+		if (pEntry && (pEntry->ValidAsCLI) && (pEntry->Sst == SST_ASSOC))
+		{
+			result = pAd->StaCfg.bAutoTxRateSwitch;
+		}
+		else
+			result = FALSE;
+
+#ifdef QOS_DLS_SUPPORT
+		if (pEntry && (pEntry->ValidAsDls))
+			result = pAd->StaCfg.bAutoTxRateSwitch;
+#endif // QOS_DLS_SUPPORT //
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+
+
+	return result;
+}
+
+
+BOOLEAN RTMPAutoRateSwitchCheck(
+	IN PRTMP_ADAPTER    pAd)
+{
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (pAd->StaCfg.bAutoTxRateSwitch)
+			return TRUE;
+	}
+#endif // CONFIG_STA_SUPPORT //
+	return FALSE;
+}
+
+
+/*
+    ========================================================================
+    Routine Description:
+        check if this entry need to fix tx legacy rate
+
+    Arguments:
+        pAd
+        pEntry
+
+    Return Value:
+        TURE
+        FALSE
+
+    ========================================================================
+*/
+UCHAR RTMPStaFixedTxMode(
+	IN PRTMP_ADAPTER    pAd,
+	IN PMAC_TABLE_ENTRY	pEntry)
+{
+	UCHAR	tx_mode = FIXED_TXMODE_HT;
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		tx_mode = (UCHAR)pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	return tx_mode;
+}
+
+/*
+    ========================================================================
+    Routine Description:
+        Overwrite HT Tx Mode by Fixed Legency Tx Mode, if specified.
+
+    Arguments:
+        pAd
+        pEntry
+
+    Return Value:
+        TURE
+        FALSE
+
+    ========================================================================
+*/
+VOID RTMPUpdateLegacyTxSetting(
+		UCHAR				fixed_tx_mode,
+		PMAC_TABLE_ENTRY	pEntry)
+{
+	HTTRANSMIT_SETTING TransmitSetting;
+
+	if (fixed_tx_mode == FIXED_TXMODE_HT)
+		return;
+
+	TransmitSetting.word = 0;
+
+	TransmitSetting.field.MODE = pEntry->HTPhyMode.field.MODE;
+	TransmitSetting.field.MCS = pEntry->HTPhyMode.field.MCS;
+
+	if (fixed_tx_mode == FIXED_TXMODE_CCK)
+	{
+		TransmitSetting.field.MODE = MODE_CCK;
+		// CCK mode allow MCS 0~3
+		if (TransmitSetting.field.MCS > MCS_3)
+			TransmitSetting.field.MCS = MCS_3;
+	}
+	else
+	{
+		TransmitSetting.field.MODE = MODE_OFDM;
+		// OFDM mode allow MCS 0~7
+		if (TransmitSetting.field.MCS > MCS_7)
+			TransmitSetting.field.MCS = MCS_7;
+	}
+
+	if (pEntry->HTPhyMode.field.MODE >= TransmitSetting.field.MODE)
+	{
+		pEntry->HTPhyMode.word = TransmitSetting.word;
+		DBGPRINT(RT_DEBUG_TRACE, ("RTMPUpdateLegacyTxSetting : wcid-%d, MODE=%s, MCS=%d \n",
+				pEntry->Aid, GetPhyMode(pEntry->HTPhyMode.field.MODE), pEntry->HTPhyMode.field.MCS));
+	}
+}
+
+#ifdef CONFIG_STA_SUPPORT
+/*
+	==========================================================================
+	Description:
+		dynamic tune BBP R66 to find a balance between sensibility and
+		noise isolation
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AsicStaBbpTuning(
+	IN PRTMP_ADAPTER pAd)
+{
+	UCHAR	OrigR66Value = 0, R66;//, R66UpperBound = 0x30, R66LowerBound = 0x30;
+	CHAR	Rssi;
+
+	// 2860C did not support Fase CCA, therefore can't tune
+	if (pAd->MACVersion == 0x28600100)
+		return;
+
+	//
+	// work as a STA
+	//
+	if (pAd->Mlme.CntlMachine.CurrState != CNTL_IDLE)  // no R66 tuning when SCANNING
+		return;
+
+	if ((pAd->OpMode == OPMODE_STA)
+		&& (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED)
+			)
+		&& !(OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+		)
+	{
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R66, &OrigR66Value);
+		R66 = OrigR66Value;
+
+		if (pAd->Antenna.field.RxPath > 1)
+			Rssi = (pAd->StaCfg.RssiSample.AvgRssi0 + pAd->StaCfg.RssiSample.AvgRssi1) >> 1;
+		else
+			Rssi = pAd->StaCfg.RssiSample.AvgRssi0;
+
+		if (pAd->LatchRfRegs.Channel <= 14)
+		{	//BG band
+#ifdef RT2870
+			// RT3070 is a no LNA solution, it should have different control regarding to AGC gain control
+			// Otherwise, it will have some throughput side effect when low RSSI
+			if (IS_RT3070(pAd))
+			{
+				if (Rssi > RSSI_FOR_MID_LOW_SENSIBILITY)
+				{
+					R66 = 0x1C + 2*GET_LNA_GAIN(pAd) + 0x20;
+					if (OrigR66Value != R66)
+					{
+						RTUSBWriteBBPRegister(pAd, BBP_R66, R66);
+					}
+				}
+				else
+				{
+					R66 = 0x1C + 2*GET_LNA_GAIN(pAd);
+					if (OrigR66Value != R66)
+					{
+						RTUSBWriteBBPRegister(pAd, BBP_R66, R66);
+					}
+				}
+			}
+			else
+#endif // RT2870 //
+			{
+				if (Rssi > RSSI_FOR_MID_LOW_SENSIBILITY)
+				{
+					R66 = (0x2E + GET_LNA_GAIN(pAd)) + 0x10;
+					if (OrigR66Value != R66)
+					{
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+					}
+				}
+				else
+				{
+					R66 = 0x2E + GET_LNA_GAIN(pAd);
+					if (OrigR66Value != R66)
+					{
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+					}
+				}
+			}
+		}
+		else
+		{	//A band
+			if (pAd->CommonCfg.BBPCurrentBW == BW_20)
+			{
+				if (Rssi > RSSI_FOR_MID_LOW_SENSIBILITY)
+				{
+					R66 = 0x32 + (GET_LNA_GAIN(pAd)*5)/3 + 0x10;
+					if (OrigR66Value != R66)
+					{
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+					}
+				}
+				else
+				{
+					R66 = 0x32 + (GET_LNA_GAIN(pAd)*5)/3;
+					if (OrigR66Value != R66)
+					{
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+					}
+				}
+			}
+			else
+			{
+				if (Rssi > RSSI_FOR_MID_LOW_SENSIBILITY)
+				{
+					R66 = 0x3A + (GET_LNA_GAIN(pAd)*5)/3 + 0x10;
+					if (OrigR66Value != R66)
+					{
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+					}
+				}
+				else
+				{
+					R66 = 0x3A + (GET_LNA_GAIN(pAd)*5)/3;
+					if (OrigR66Value != R66)
+					{
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+					}
+				}
+			}
+		}
+
+
+	}
+}
+#endif // CONFIG_STA_SUPPORT //
+
+VOID RTMPSetAGCInitValue(
+	IN PRTMP_ADAPTER	pAd,
+	IN UCHAR			BandWidth)
+{
+	UCHAR	R66 = 0x30;
+
+	if (pAd->LatchRfRegs.Channel <= 14)
+	{	// BG band
+		R66 = 0x2E + GET_LNA_GAIN(pAd);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+	}
+	else
+	{	//A band
+		if (BandWidth == BW_20)
+		{
+			R66 = (UCHAR)(0x32 + (GET_LNA_GAIN(pAd)*5)/3);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+		}
+#ifdef DOT11_N_SUPPORT
+		else
+		{
+			R66 = (UCHAR)(0x3A + (GET_LNA_GAIN(pAd)*5)/3);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+		}
+#endif // DOT11_N_SUPPORT //
+	}
+
+}
+
+VOID AsicTurnOffRFClk(
+	IN PRTMP_ADAPTER pAd,
+	IN	UCHAR		Channel)
+{
+
+	// RF R2 bit 18 = 0
+	UINT32			R1 = 0, R2 = 0, R3 = 0;
+	UCHAR			index;
+	RTMP_RF_REGS	*RFRegTable;
+
+	RFRegTable = RF2850RegTable;
+
+	switch (pAd->RfIcType)
+	{
+		case RFIC_2820:
+		case RFIC_2850:
+		case RFIC_2720:
+		case RFIC_2750:
+
+			for (index = 0; index < NUM_OF_2850_CHNL; index++)
+			{
+				if (Channel == RFRegTable[index].Channel)
+				{
+					R1 = RFRegTable[index].R1 & 0xffffdfff;
+					R2 = RFRegTable[index].R2 & 0xfffbffff;
+					R3 = RFRegTable[index].R3 & 0xfff3ffff;
+
+					RTMP_RF_IO_WRITE32(pAd, R1);
+					RTMP_RF_IO_WRITE32(pAd, R2);
+
+					// Program R1b13 to 1, R3/b18,19 to 0, R2b18 to 0.
+					// Set RF R2 bit18=0, R3 bit[18:19]=0
+					//if (pAd->StaCfg.bRadio == FALSE)
+					if (1)
+					{
+						RTMP_RF_IO_WRITE32(pAd, R3);
+
+						DBGPRINT(RT_DEBUG_TRACE, ("AsicTurnOffRFClk#%d(RF=%d, ) , R2=0x%08x,  R3 = 0x%08x \n",
+							Channel, pAd->RfIcType, R2, R3));
+					}
+					else
+						DBGPRINT(RT_DEBUG_TRACE, ("AsicTurnOffRFClk#%d(RF=%d, ) , R2=0x%08x \n",
+							Channel, pAd->RfIcType, R2));
+					break;
+				}
+			}
+			break;
+
+		default:
+			break;
+	}
+}
+
+
+VOID AsicTurnOnRFClk(
+	IN PRTMP_ADAPTER pAd,
+	IN	UCHAR			Channel)
+{
+
+	// RF R2 bit 18 = 0
+	UINT32			R1 = 0, R2 = 0, R3 = 0;
+	UCHAR			index;
+	RTMP_RF_REGS	*RFRegTable;
+
+	RFRegTable = RF2850RegTable;
+
+	switch (pAd->RfIcType)
+	{
+		case RFIC_2820:
+		case RFIC_2850:
+		case RFIC_2720:
+		case RFIC_2750:
+
+			for (index = 0; index < NUM_OF_2850_CHNL; index++)
+			{
+				if (Channel == RFRegTable[index].Channel)
+				{
+					R3 = pAd->LatchRfRegs.R3;
+					R3 &= 0xfff3ffff;
+					R3 |= 0x00080000;
+					RTMP_RF_IO_WRITE32(pAd, R3);
+
+					R1 = RFRegTable[index].R1;
+					RTMP_RF_IO_WRITE32(pAd, R1);
+
+					R2 = RFRegTable[index].R2;
+					if (pAd->Antenna.field.TxPath == 1)
+					{
+						R2 |= 0x4000;	// If TXpath is 1, bit 14 = 1;
+					}
+
+					if (pAd->Antenna.field.RxPath == 2)
+					{
+						R2 |= 0x40;	// write 1 to off Rxpath.
+					}
+					else if (pAd->Antenna.field.RxPath == 1)
+					{
+						R2 |= 0x20040;	// write 1 to off RxPath
+					}
+					RTMP_RF_IO_WRITE32(pAd, R2);
+
+					break;
+				}
+			}
+			break;
+
+		default:
+			break;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("AsicTurnOnRFClk#%d(RF=%d, ) , R2=0x%08x\n",
+		Channel,
+		pAd->RfIcType,
+		R2));
+}
+
diff --git a/drivers/staging/rt2870/common/netif_block.c b/drivers/staging/rt2870/common/netif_block.c
new file mode 100644
index 00000000000..d3f7d087e7f
--- /dev/null
+++ b/drivers/staging/rt2870/common/netif_block.c
@@ -0,0 +1,144 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#include "../rt_config.h"
+#include "netif_block.h"
+
+static NETIF_ENTRY freeNetIfEntryPool[FREE_NETIF_POOL_SIZE];
+static LIST_HEADER freeNetIfEntryList;
+
+void initblockQueueTab(
+	IN PRTMP_ADAPTER pAd)
+{
+	int i;
+
+	initList(&freeNetIfEntryList);
+	for (i = 0; i < FREE_NETIF_POOL_SIZE; i++)
+		insertTailList(&freeNetIfEntryList, (PLIST_ENTRY)&freeNetIfEntryPool[i]);
+
+	for (i=0; i < NUM_OF_TX_RING; i++)
+		initList(&pAd->blockQueueTab[i].NetIfList);
+
+	return;
+}
+
+BOOLEAN blockNetIf(
+	IN PBLOCK_QUEUE_ENTRY pBlockQueueEntry,
+	IN PNET_DEV pNetDev)
+{
+	PNETIF_ENTRY pNetIfEntry = NULL;
+
+	if ((pNetIfEntry = (PNETIF_ENTRY)removeHeadList(&freeNetIfEntryList)) != NULL)
+	{
+		netif_stop_queue(pNetDev);
+		pNetIfEntry->pNetDev = pNetDev;
+		insertTailList(&pBlockQueueEntry->NetIfList, (PLIST_ENTRY)pNetIfEntry);
+
+		pBlockQueueEntry->SwTxQueueBlockFlag = TRUE;
+		DBGPRINT(RT_DEBUG_TRACE, ("netif_stop_queue(%s)\n", pNetDev->name));
+	}
+	else
+		return FALSE;
+
+	return TRUE;
+}
+
+VOID releaseNetIf(
+	IN PBLOCK_QUEUE_ENTRY pBlockQueueEntry)
+{
+	PNETIF_ENTRY pNetIfEntry = NULL;
+	PLIST_HEADER pNetIfList = &pBlockQueueEntry->NetIfList;
+
+	while((pNetIfEntry = (PNETIF_ENTRY)removeHeadList(pNetIfList)) !=  NULL)
+	{
+		PNET_DEV pNetDev = pNetIfEntry->pNetDev;
+		netif_wake_queue(pNetDev);
+		insertTailList(&freeNetIfEntryList, (PLIST_ENTRY)pNetIfEntry);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("netif_wake_queue(%s)\n", pNetDev->name));
+	}
+	pBlockQueueEntry->SwTxQueueBlockFlag = FALSE;
+	return;
+}
+
+
+VOID StopNetIfQueue(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR QueIdx,
+	IN PNDIS_PACKET pPacket)
+{
+	PNET_DEV NetDev = NULL;
+	UCHAR IfIdx = 0;
+	BOOLEAN valid = FALSE;
+
+#ifdef APCLI_SUPPORT
+	if (RTMP_GET_PACKET_NET_DEVICE(pPacket) >= MIN_NET_DEVICE_FOR_APCLI)
+	{
+		IfIdx = (RTMP_GET_PACKET_NET_DEVICE(pPacket) - MIN_NET_DEVICE_FOR_APCLI) % MAX_APCLI_NUM;
+		NetDev = pAd->ApCfg.ApCliTab[IfIdx].dev;
+	}
+	else
+#endif // APCLI_SUPPORT //
+#ifdef WDS_SUPPORT
+	if (RTMP_GET_PACKET_NET_DEVICE(pPacket) >= MIN_NET_DEVICE_FOR_WDS)
+	{
+		IfIdx = (RTMP_GET_PACKET_NET_DEVICE(pPacket) - MIN_NET_DEVICE_FOR_WDS) % MAX_WDS_ENTRY;
+		NetDev = pAd->WdsTab.WdsEntry[IfIdx].dev;
+	}
+	else
+#endif // WDS_SUPPORT //
+	{
+#ifdef MBSS_SUPPORT
+		if (pAd->OpMode == OPMODE_AP)
+		{
+			IfIdx = (RTMP_GET_PACKET_NET_DEVICE(pPacket) - MIN_NET_DEVICE_FOR_MBSSID) % MAX_MBSSID_NUM;
+			NetDev = pAd->ApCfg.MBSSID[IfIdx].MSSIDDev;
+		}
+		else
+		{
+			IfIdx = MAIN_MBSSID;
+			NetDev = pAd->net_dev;
+		}
+#else
+		IfIdx = MAIN_MBSSID;
+		NetDev = pAd->net_dev;
+#endif
+	}
+
+	// WMM support 4 software queues.
+	// One software queue full doesn't mean device have no capbility to transmit packet.
+	// So disable block Net-If queue function while WMM enable.
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		valid = (pAd->CommonCfg.bWmmCapable == TRUE) ? FALSE : TRUE;
+#endif // CONFIG_STA_SUPPORT //
+
+	if (valid)
+		blockNetIf(&pAd->blockQueueTab[QueIdx], NetDev);
+	return;
+}
+
diff --git a/drivers/staging/rt2870/common/rtmp_init.c b/drivers/staging/rt2870/common/rtmp_init.c
new file mode 100644
index 00000000000..87028fd60bc
--- /dev/null
+++ b/drivers/staging/rt2870/common/rtmp_init.c
@@ -0,0 +1,4132 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rtmp_init.c
+
+	Abstract:
+	Miniport generic portion header file
+
+	Revision History:
+	Who         When          What
+	--------    ----------    ----------------------------------------------
+	Paul Lin    2002-08-01    created
+    John Chang  2004-08-20    RT2561/2661 use scatter-gather scheme
+    Jan Lee  2006-09-15    RT2860. Change for 802.11n , EEPROM, Led, BA, HT.
+*/
+#include "../rt_config.h"
+#include "firmware.h"
+
+//#define BIN_IN_FILE /* use *.bin firmware */
+
+UCHAR    BIT8[] = {0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80};
+ULONG    BIT32[] = {0x00000001, 0x00000002, 0x00000004, 0x00000008,
+					0x00000010, 0x00000020, 0x00000040, 0x00000080,
+					0x00000100, 0x00000200, 0x00000400, 0x00000800,
+					0x00001000, 0x00002000, 0x00004000, 0x00008000,
+					0x00010000, 0x00020000, 0x00040000, 0x00080000,
+					0x00100000, 0x00200000, 0x00400000, 0x00800000,
+					0x01000000, 0x02000000, 0x04000000, 0x08000000,
+					0x10000000, 0x20000000, 0x40000000, 0x80000000};
+
+char*   CipherName[] = {"none","wep64","wep128","TKIP","AES","CKIP64","CKIP128"};
+
+const unsigned short ccitt_16Table[] = {
+	0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7,
+	0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF,
+	0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6,
+	0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE,
+	0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485,
+	0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D,
+	0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4,
+	0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC,
+	0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823,
+	0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B,
+	0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12,
+	0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A,
+	0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41,
+	0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49,
+	0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70,
+	0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78,
+	0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F,
+	0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067,
+	0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E,
+	0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256,
+	0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D,
+	0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
+	0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C,
+	0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634,
+	0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB,
+	0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3,
+	0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A,
+	0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92,
+	0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9,
+	0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1,
+	0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8,
+	0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0
+};
+#define ByteCRC16(v, crc) \
+	(unsigned short)((crc << 8) ^  ccitt_16Table[((crc >> 8) ^ (v)) & 255])
+
+unsigned char BitReverse(unsigned char x)
+{
+	int i;
+	unsigned char Temp=0;
+	for(i=0; ; i++)
+	{
+		if(x & 0x80)	Temp |= 0x80;
+		if(i==7)		break;
+		x	<<= 1;
+		Temp >>= 1;
+	}
+	return Temp;
+}
+
+//
+// BBP register initialization set
+//
+REG_PAIR   BBPRegTable[] = {
+	{BBP_R65,		0x2C},		// fix rssi issue
+	{BBP_R66,		0x38},	// Also set this default value to pAd->BbpTuning.R66CurrentValue at initial
+	{BBP_R69,		0x12},
+	{BBP_R70,		0xa},	// BBP_R70 will change to 0x8 in ApStartUp and LinkUp for rt2860C, otherwise value is 0xa
+	{BBP_R73,		0x10},
+	{BBP_R81,		0x37},
+	{BBP_R82,		0x62},
+	{BBP_R83,		0x6A},
+	{BBP_R84,		0x99},	// 0x19 is for rt2860E and after. This is for extension channel overlapping IOT. 0x99 is for rt2860D and before
+	{BBP_R86,		0x00},	// middle range issue, Rory @2008-01-28
+	{BBP_R91,		0x04},	// middle range issue, Rory @2008-01-28
+	{BBP_R92,		0x00},	// middle range issue, Rory @2008-01-28
+	{BBP_R103,  	0x00}, 	// near range high-power issue, requested from Gary @2008-0528
+	{BBP_R105,		0x05},	// 0x05 is for rt2860E to turn on FEQ control. It is safe for rt2860D and before, because Bit 7:2 are reserved in rt2860D and before.
+};
+#define	NUM_BBP_REG_PARMS	(sizeof(BBPRegTable) / sizeof(REG_PAIR))
+
+//
+// RF register initialization set
+//
+#ifdef RT2870
+REG_PAIR   RT30xx_RFRegTable[] = {
+        {RF_R04,          0x40},
+        {RF_R05,          0x03},
+        {RF_R06,          0x02},
+        {RF_R07,          0x70},
+        {RF_R09,          0x0F},
+        {RF_R10,          0x71},
+        {RF_R11,          0x21},
+        {RF_R12,          0x7B},
+        {RF_R14,          0x90},
+        {RF_R15,          0x58},
+        {RF_R16,          0xB3},
+        {RF_R17,          0x92},
+        {RF_R18,          0x2C},
+        {RF_R19,          0x02},
+        {RF_R20,          0xBA},
+        {RF_R21,          0xDB},
+        {RF_R24,          0x16},
+        {RF_R25,          0x01},
+        {RF_R27,          0x03},
+        {RF_R29,          0x1F},
+};
+#define	NUM_RF_REG_PARMS	(sizeof(RT30xx_RFRegTable) / sizeof(REG_PAIR))
+#endif // RT2870 //
+
+//
+// ASIC register initialization sets
+//
+
+RTMP_REG_PAIR	MACRegTable[] =	{
+#if defined(HW_BEACON_OFFSET) && (HW_BEACON_OFFSET == 0x200)
+	{BCN_OFFSET0,			0xf8f0e8e0}, /* 0x3800(e0), 0x3A00(e8), 0x3C00(f0), 0x3E00(f8), 512B for each beacon */
+	{BCN_OFFSET1,			0x6f77d0c8}, /* 0x3200(c8), 0x3400(d0), 0x1DC0(77), 0x1BC0(6f), 512B for each beacon */
+#elif defined(HW_BEACON_OFFSET) && (HW_BEACON_OFFSET == 0x100)
+	{BCN_OFFSET0,			0xece8e4e0}, /* 0x3800, 0x3A00, 0x3C00, 0x3E00, 512B for each beacon */
+	{BCN_OFFSET1,			0xfcf8f4f0}, /* 0x3800, 0x3A00, 0x3C00, 0x3E00, 512B for each beacon */
+#else
+    #error You must re-calculate new value for BCN_OFFSET0 & BCN_OFFSET1 in MACRegTable[]!!!
+#endif // HW_BEACON_OFFSET //
+
+	{LEGACY_BASIC_RATE,		0x0000013f}, //  Basic rate set bitmap
+	{HT_BASIC_RATE,		0x00008003}, // Basic HT rate set , 20M, MCS=3, MM. Format is the same as in TXWI.
+	{MAC_SYS_CTRL,		0x00}, // 0x1004, , default Disable RX
+	{RX_FILTR_CFG,		0x17f97}, //0x1400  , RX filter control,
+	{BKOFF_SLOT_CFG,	0x209}, // default set short slot time, CC_DELAY_TIME should be 2
+	{TX_SW_CFG0,		0x0}, 		// Gary,2008-05-21 for CWC test
+	{TX_SW_CFG1,		0x80606}, // Gary,2006-08-23
+	{TX_LINK_CFG,		0x1020},		// Gary,2006-08-23
+	//{TX_TIMEOUT_CFG,	0x00182090},	// CCK has some problem. So increase timieout value. 2006-10-09// MArvek RT
+	{TX_TIMEOUT_CFG,	0x000a2090},	// CCK has some problem. So increase timieout value. 2006-10-09// MArvek RT , Modify for 2860E ,2007-08-01
+	{MAX_LEN_CFG,		MAX_AGGREGATION_SIZE | 0x00001000},	// 0x3018, MAX frame length. Max PSDU = 16kbytes.
+	{LED_CFG,		0x7f031e46}, // Gary, 2006-08-23
+	{PBF_MAX_PCNT,			0x1F3FBF9F}, 	//0x1F3f7f9f},		//Jan, 2006/04/20
+	//{TX_RTY_CFG,			0x6bb80408},	// Jan, 2006/11/16
+	{TX_RTY_CFG,			0x47d01f0f},	// Jan, 2006/11/16, Set TxWI->ACK =0 in Probe Rsp Modify for 2860E ,2007-08-03
+	{AUTO_RSP_CFG,			0x00000013},	// Initial Auto_Responder, because QA will turn off Auto-Responder
+	{CCK_PROT_CFG,			0x05740003 /*0x01740003*/},	// Initial Auto_Responder, because QA will turn off Auto-Responder. And RTS threshold is enabled.
+	{OFDM_PROT_CFG,			0x05740003 /*0x01740003*/},	// Initial Auto_Responder, because QA will turn off Auto-Responder. And RTS threshold is enabled.
+#ifdef RT2870
+	{PBF_CFG, 				0xf40006}, 		// Only enable Queue 2
+	{MM40_PROT_CFG,			0x3F44084},		// Initial Auto_Responder, because QA will turn off Auto-Responder
+	{WPDMA_GLO_CFG,			0x00000030},
+#endif // RT2870 //
+	{GF20_PROT_CFG,			0x01744004},    // set 19:18 --> Short NAV for MIMO PS
+	{GF40_PROT_CFG,			0x03F44084},
+	{MM20_PROT_CFG,			0x01744004},
+	{TXOP_CTRL_CFG,			0x0000583f, /*0x0000243f*/ /*0x000024bf*/},	//Extension channel backoff.
+	{TX_RTS_CFG,			0x00092b20},
+//#ifdef WIFI_TEST
+	{EXP_ACK_TIME,			0x002400ca},	// default value
+//#else
+//	{EXP_ACK_TIME,			0x005400ca},	// suggested by Gray @ 20070323 for 11n intel-sta throughput
+//#endif // end - WIFI_TEST //
+	{TXOP_HLDR_ET, 			0x00000002},
+
+	/* Jerry comments 2008/01/16: we use SIFS = 10us in CCK defaultly, but it seems that 10us
+		is too small for INTEL 2200bg card, so in MBSS mode, the delta time between beacon0
+		and beacon1 is SIFS (10us), so if INTEL 2200bg card connects to BSS0, the ping
+		will always lost. So we change the SIFS of CCK from 10us to 16us. */
+	{XIFS_TIME_CFG,			0x33a41010},
+	{PWR_PIN_CFG,			0x00000003},	// patch for 2880-E
+};
+
+
+#ifdef CONFIG_STA_SUPPORT
+RTMP_REG_PAIR	STAMACRegTable[] =	{
+	{WMM_AIFSN_CFG,		0x00002273},
+	{WMM_CWMIN_CFG,	0x00002344},
+	{WMM_CWMAX_CFG,	0x000034aa},
+};
+#endif // CONFIG_STA_SUPPORT //
+
+#define	NUM_MAC_REG_PARMS		(sizeof(MACRegTable) / sizeof(RTMP_REG_PAIR))
+#ifdef CONFIG_STA_SUPPORT
+#define	NUM_STA_MAC_REG_PARMS	(sizeof(STAMACRegTable) / sizeof(RTMP_REG_PAIR))
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef RT2870
+//
+// RT2870 Firmware Spec only used 1 oct for version expression
+//
+#define FIRMWARE_MINOR_VERSION	7
+
+#endif // RT2870 //
+
+// New 8k byte firmware size for RT3071/RT3072
+#define FIRMWAREIMAGE_MAX_LENGTH	0x2000
+#define FIRMWAREIMAGE_LENGTH		(sizeof (FirmwareImage) / sizeof(UCHAR))
+#define FIRMWARE_MAJOR_VERSION	0
+
+#define FIRMWAREIMAGEV1_LENGTH	0x1000
+#define FIRMWAREIMAGEV2_LENGTH	0x1000
+
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Allocate RTMP_ADAPTER data block and do some initialization
+
+	Arguments:
+		Adapter		Pointer to our adapter
+
+	Return Value:
+		NDIS_STATUS_SUCCESS
+		NDIS_STATUS_FAILURE
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS	RTMPAllocAdapterBlock(
+	IN  PVOID	handle,
+	OUT	PRTMP_ADAPTER	*ppAdapter)
+{
+	PRTMP_ADAPTER	pAd;
+	NDIS_STATUS		Status;
+	INT 			index;
+	UCHAR			*pBeaconBuf = NULL;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> RTMPAllocAdapterBlock\n"));
+
+	*ppAdapter = NULL;
+
+	do
+	{
+		// Allocate RTMP_ADAPTER memory block
+		pBeaconBuf = kmalloc(MAX_BEACON_SIZE, MEM_ALLOC_FLAG);
+		if (pBeaconBuf == NULL)
+		{
+			Status = NDIS_STATUS_FAILURE;
+			DBGPRINT_ERR(("Failed to allocate memory - BeaconBuf!\n"));
+			break;
+		}
+
+		Status = AdapterBlockAllocateMemory(handle, (PVOID *)&pAd);
+		if (Status != NDIS_STATUS_SUCCESS)
+		{
+			DBGPRINT_ERR(("Failed to allocate memory - ADAPTER\n"));
+			break;
+		}
+		pAd->BeaconBuf = pBeaconBuf;
+		printk("\n\n=== pAd = %p, size = %d ===\n\n", pAd, (UINT32)sizeof(RTMP_ADAPTER));
+
+
+		// Init spin locks
+		NdisAllocateSpinLock(&pAd->MgmtRingLock);
+
+		for (index =0 ; index < NUM_OF_TX_RING; index++)
+		{
+			NdisAllocateSpinLock(&pAd->TxSwQueueLock[index]);
+			NdisAllocateSpinLock(&pAd->DeQueueLock[index]);
+			pAd->DeQueueRunning[index] = FALSE;
+		}
+
+		NdisAllocateSpinLock(&pAd->irq_lock);
+
+	} while (FALSE);
+
+	if ((Status != NDIS_STATUS_SUCCESS) && (pBeaconBuf))
+		kfree(pBeaconBuf);
+
+	*ppAdapter = pAd;
+
+	DBGPRINT_S(Status, ("<-- RTMPAllocAdapterBlock, Status=%x\n", Status));
+	return Status;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Read initial Tx power per MCS and BW from EEPROM
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPReadTxPwrPerRate(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	ULONG		data, Adata, Gdata;
+	USHORT		i, value, value2;
+	INT			Apwrdelta, Gpwrdelta;
+	UCHAR		t1,t2,t3,t4;
+	BOOLEAN		bValid, bApwrdeltaMinus = TRUE, bGpwrdeltaMinus = TRUE;
+
+	//
+	// Get power delta for 20MHz and 40MHz.
+	//
+	DBGPRINT(RT_DEBUG_TRACE, ("Txpower per Rate\n"));
+	RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_DELTA, value2);
+	Apwrdelta = 0;
+	Gpwrdelta = 0;
+
+	if ((value2 & 0xff) != 0xff)
+	{
+		if ((value2 & 0x80))
+			Gpwrdelta = (value2&0xf);
+
+		if ((value2 & 0x40))
+			bGpwrdeltaMinus = FALSE;
+		else
+			bGpwrdeltaMinus = TRUE;
+	}
+	if ((value2 & 0xff00) != 0xff00)
+	{
+		if ((value2 & 0x8000))
+			Apwrdelta = ((value2&0xf00)>>8);
+
+		if ((value2 & 0x4000))
+			bApwrdeltaMinus = FALSE;
+		else
+			bApwrdeltaMinus = TRUE;
+	}
+	DBGPRINT(RT_DEBUG_TRACE, ("Gpwrdelta = %x, Apwrdelta = %x .\n", Gpwrdelta, Apwrdelta));
+
+	//
+	// Get Txpower per MCS for 20MHz in 2.4G.
+	//
+	for (i=0; i<5; i++)
+	{
+		RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_20MHZ_2_4G + i*4, value);
+		data = value;
+		if (bApwrdeltaMinus == FALSE)
+		{
+			t1 = (value&0xf)+(Apwrdelta);
+			if (t1 > 0xf)
+				t1 = 0xf;
+			t2 = ((value&0xf0)>>4)+(Apwrdelta);
+			if (t2 > 0xf)
+				t2 = 0xf;
+			t3 = ((value&0xf00)>>8)+(Apwrdelta);
+			if (t3 > 0xf)
+				t3 = 0xf;
+			t4 = ((value&0xf000)>>12)+(Apwrdelta);
+			if (t4 > 0xf)
+				t4 = 0xf;
+		}
+		else
+		{
+			if ((value&0xf) > Apwrdelta)
+				t1 = (value&0xf)-(Apwrdelta);
+			else
+				t1 = 0;
+			if (((value&0xf0)>>4) > Apwrdelta)
+				t2 = ((value&0xf0)>>4)-(Apwrdelta);
+			else
+				t2 = 0;
+			if (((value&0xf00)>>8) > Apwrdelta)
+				t3 = ((value&0xf00)>>8)-(Apwrdelta);
+			else
+				t3 = 0;
+			if (((value&0xf000)>>12) > Apwrdelta)
+				t4 = ((value&0xf000)>>12)-(Apwrdelta);
+			else
+				t4 = 0;
+		}
+		Adata = t1 + (t2<<4) + (t3<<8) + (t4<<12);
+		if (bGpwrdeltaMinus == FALSE)
+		{
+			t1 = (value&0xf)+(Gpwrdelta);
+			if (t1 > 0xf)
+				t1 = 0xf;
+			t2 = ((value&0xf0)>>4)+(Gpwrdelta);
+			if (t2 > 0xf)
+				t2 = 0xf;
+			t3 = ((value&0xf00)>>8)+(Gpwrdelta);
+			if (t3 > 0xf)
+				t3 = 0xf;
+			t4 = ((value&0xf000)>>12)+(Gpwrdelta);
+			if (t4 > 0xf)
+				t4 = 0xf;
+		}
+		else
+		{
+			if ((value&0xf) > Gpwrdelta)
+				t1 = (value&0xf)-(Gpwrdelta);
+			else
+				t1 = 0;
+			if (((value&0xf0)>>4) > Gpwrdelta)
+				t2 = ((value&0xf0)>>4)-(Gpwrdelta);
+			else
+				t2 = 0;
+			if (((value&0xf00)>>8) > Gpwrdelta)
+				t3 = ((value&0xf00)>>8)-(Gpwrdelta);
+			else
+				t3 = 0;
+			if (((value&0xf000)>>12) > Gpwrdelta)
+				t4 = ((value&0xf000)>>12)-(Gpwrdelta);
+			else
+				t4 = 0;
+		}
+		Gdata = t1 + (t2<<4) + (t3<<8) + (t4<<12);
+
+		RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_20MHZ_2_4G + i*4 + 2, value);
+		if (bApwrdeltaMinus == FALSE)
+		{
+			t1 = (value&0xf)+(Apwrdelta);
+			if (t1 > 0xf)
+				t1 = 0xf;
+			t2 = ((value&0xf0)>>4)+(Apwrdelta);
+			if (t2 > 0xf)
+				t2 = 0xf;
+			t3 = ((value&0xf00)>>8)+(Apwrdelta);
+			if (t3 > 0xf)
+				t3 = 0xf;
+			t4 = ((value&0xf000)>>12)+(Apwrdelta);
+			if (t4 > 0xf)
+				t4 = 0xf;
+		}
+		else
+		{
+			if ((value&0xf) > Apwrdelta)
+				t1 = (value&0xf)-(Apwrdelta);
+			else
+				t1 = 0;
+			if (((value&0xf0)>>4) > Apwrdelta)
+				t2 = ((value&0xf0)>>4)-(Apwrdelta);
+			else
+				t2 = 0;
+			if (((value&0xf00)>>8) > Apwrdelta)
+				t3 = ((value&0xf00)>>8)-(Apwrdelta);
+			else
+				t3 = 0;
+			if (((value&0xf000)>>12) > Apwrdelta)
+				t4 = ((value&0xf000)>>12)-(Apwrdelta);
+			else
+				t4 = 0;
+		}
+		Adata |= ((t1<<16) + (t2<<20) + (t3<<24) + (t4<<28));
+		if (bGpwrdeltaMinus == FALSE)
+		{
+			t1 = (value&0xf)+(Gpwrdelta);
+			if (t1 > 0xf)
+				t1 = 0xf;
+			t2 = ((value&0xf0)>>4)+(Gpwrdelta);
+			if (t2 > 0xf)
+				t2 = 0xf;
+			t3 = ((value&0xf00)>>8)+(Gpwrdelta);
+			if (t3 > 0xf)
+				t3 = 0xf;
+			t4 = ((value&0xf000)>>12)+(Gpwrdelta);
+			if (t4 > 0xf)
+				t4 = 0xf;
+		}
+		else
+		{
+			if ((value&0xf) > Gpwrdelta)
+				t1 = (value&0xf)-(Gpwrdelta);
+			else
+				t1 = 0;
+			if (((value&0xf0)>>4) > Gpwrdelta)
+				t2 = ((value&0xf0)>>4)-(Gpwrdelta);
+			else
+				t2 = 0;
+			if (((value&0xf00)>>8) > Gpwrdelta)
+				t3 = ((value&0xf00)>>8)-(Gpwrdelta);
+			else
+				t3 = 0;
+			if (((value&0xf000)>>12) > Gpwrdelta)
+				t4 = ((value&0xf000)>>12)-(Gpwrdelta);
+			else
+				t4 = 0;
+		}
+		Gdata |= ((t1<<16) + (t2<<20) + (t3<<24) + (t4<<28));
+		data |= (value<<16);
+
+		pAd->Tx20MPwrCfgABand[i] = pAd->Tx40MPwrCfgABand[i] = Adata;
+		pAd->Tx20MPwrCfgGBand[i] = pAd->Tx40MPwrCfgGBand[i] = Gdata;
+
+		if (data != 0xffffffff)
+			RTMP_IO_WRITE32(pAd, TX_PWR_CFG_0 + i*4, data);
+		DBGPRINT_RAW(RT_DEBUG_TRACE, ("20MHz BW, 2.4G band-%lx,  Adata = %lx,  Gdata = %lx \n", data, Adata, Gdata));
+	}
+
+	//
+	// Check this block is valid for 40MHz in 2.4G. If invalid, use parameter for 20MHz in 2.4G
+	//
+	bValid = TRUE;
+	for (i=0; i<6; i++)
+	{
+		RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_40MHZ_2_4G + 2 + i*2, value);
+		if (((value & 0x00FF) == 0x00FF) || ((value & 0xFF00) == 0xFF00))
+		{
+			bValid = FALSE;
+			break;
+		}
+	}
+
+	//
+	// Get Txpower per MCS for 40MHz in 2.4G.
+	//
+	if (bValid)
+	{
+		for (i=0; i<4; i++)
+		{
+			RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_40MHZ_2_4G + i*4, value);
+			if (bGpwrdeltaMinus == FALSE)
+			{
+				t1 = (value&0xf)+(Gpwrdelta);
+				if (t1 > 0xf)
+					t1 = 0xf;
+				t2 = ((value&0xf0)>>4)+(Gpwrdelta);
+				if (t2 > 0xf)
+					t2 = 0xf;
+				t3 = ((value&0xf00)>>8)+(Gpwrdelta);
+				if (t3 > 0xf)
+					t3 = 0xf;
+				t4 = ((value&0xf000)>>12)+(Gpwrdelta);
+				if (t4 > 0xf)
+					t4 = 0xf;
+			}
+			else
+			{
+				if ((value&0xf) > Gpwrdelta)
+					t1 = (value&0xf)-(Gpwrdelta);
+				else
+					t1 = 0;
+				if (((value&0xf0)>>4) > Gpwrdelta)
+					t2 = ((value&0xf0)>>4)-(Gpwrdelta);
+				else
+					t2 = 0;
+				if (((value&0xf00)>>8) > Gpwrdelta)
+					t3 = ((value&0xf00)>>8)-(Gpwrdelta);
+				else
+					t3 = 0;
+				if (((value&0xf000)>>12) > Gpwrdelta)
+					t4 = ((value&0xf000)>>12)-(Gpwrdelta);
+				else
+					t4 = 0;
+			}
+			Gdata = t1 + (t2<<4) + (t3<<8) + (t4<<12);
+
+			RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_40MHZ_2_4G + i*4 + 2, value);
+			if (bGpwrdeltaMinus == FALSE)
+			{
+				t1 = (value&0xf)+(Gpwrdelta);
+				if (t1 > 0xf)
+					t1 = 0xf;
+				t2 = ((value&0xf0)>>4)+(Gpwrdelta);
+				if (t2 > 0xf)
+					t2 = 0xf;
+				t3 = ((value&0xf00)>>8)+(Gpwrdelta);
+				if (t3 > 0xf)
+					t3 = 0xf;
+				t4 = ((value&0xf000)>>12)+(Gpwrdelta);
+				if (t4 > 0xf)
+					t4 = 0xf;
+			}
+			else
+			{
+				if ((value&0xf) > Gpwrdelta)
+					t1 = (value&0xf)-(Gpwrdelta);
+				else
+					t1 = 0;
+				if (((value&0xf0)>>4) > Gpwrdelta)
+					t2 = ((value&0xf0)>>4)-(Gpwrdelta);
+				else
+					t2 = 0;
+				if (((value&0xf00)>>8) > Gpwrdelta)
+					t3 = ((value&0xf00)>>8)-(Gpwrdelta);
+				else
+					t3 = 0;
+				if (((value&0xf000)>>12) > Gpwrdelta)
+					t4 = ((value&0xf000)>>12)-(Gpwrdelta);
+				else
+					t4 = 0;
+			}
+			Gdata |= ((t1<<16) + (t2<<20) + (t3<<24) + (t4<<28));
+
+			if (i == 0)
+				pAd->Tx40MPwrCfgGBand[i+1] = (pAd->Tx40MPwrCfgGBand[i+1] & 0x0000FFFF) | (Gdata & 0xFFFF0000);
+			else
+				pAd->Tx40MPwrCfgGBand[i+1] = Gdata;
+
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("40MHz BW, 2.4G band, Gdata = %lx \n", Gdata));
+		}
+	}
+
+	//
+	// Check this block is valid for 20MHz in 5G. If invalid, use parameter for 20MHz in 2.4G
+	//
+	bValid = TRUE;
+	for (i=0; i<8; i++)
+	{
+		RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_20MHZ_5G + 2 + i*2, value);
+		if (((value & 0x00FF) == 0x00FF) || ((value & 0xFF00) == 0xFF00))
+		{
+			bValid = FALSE;
+			break;
+		}
+	}
+
+	//
+	// Get Txpower per MCS for 20MHz in 5G.
+	//
+	if (bValid)
+	{
+		for (i=0; i<5; i++)
+		{
+			RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_20MHZ_5G + i*4, value);
+			if (bApwrdeltaMinus == FALSE)
+			{
+				t1 = (value&0xf)+(Apwrdelta);
+				if (t1 > 0xf)
+					t1 = 0xf;
+				t2 = ((value&0xf0)>>4)+(Apwrdelta);
+				if (t2 > 0xf)
+					t2 = 0xf;
+				t3 = ((value&0xf00)>>8)+(Apwrdelta);
+				if (t3 > 0xf)
+					t3 = 0xf;
+				t4 = ((value&0xf000)>>12)+(Apwrdelta);
+				if (t4 > 0xf)
+					t4 = 0xf;
+			}
+			else
+			{
+				if ((value&0xf) > Apwrdelta)
+					t1 = (value&0xf)-(Apwrdelta);
+				else
+					t1 = 0;
+				if (((value&0xf0)>>4) > Apwrdelta)
+					t2 = ((value&0xf0)>>4)-(Apwrdelta);
+				else
+					t2 = 0;
+				if (((value&0xf00)>>8) > Apwrdelta)
+					t3 = ((value&0xf00)>>8)-(Apwrdelta);
+				else
+					t3 = 0;
+				if (((value&0xf000)>>12) > Apwrdelta)
+					t4 = ((value&0xf000)>>12)-(Apwrdelta);
+				else
+					t4 = 0;
+			}
+			Adata = t1 + (t2<<4) + (t3<<8) + (t4<<12);
+
+			RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_20MHZ_5G + i*4 + 2, value);
+			if (bApwrdeltaMinus == FALSE)
+			{
+				t1 = (value&0xf)+(Apwrdelta);
+				if (t1 > 0xf)
+					t1 = 0xf;
+				t2 = ((value&0xf0)>>4)+(Apwrdelta);
+				if (t2 > 0xf)
+					t2 = 0xf;
+				t3 = ((value&0xf00)>>8)+(Apwrdelta);
+				if (t3 > 0xf)
+					t3 = 0xf;
+				t4 = ((value&0xf000)>>12)+(Apwrdelta);
+				if (t4 > 0xf)
+					t4 = 0xf;
+			}
+			else
+			{
+				if ((value&0xf) > Apwrdelta)
+					t1 = (value&0xf)-(Apwrdelta);
+				else
+					t1 = 0;
+				if (((value&0xf0)>>4) > Apwrdelta)
+					t2 = ((value&0xf0)>>4)-(Apwrdelta);
+				else
+					t2 = 0;
+				if (((value&0xf00)>>8) > Apwrdelta)
+					t3 = ((value&0xf00)>>8)-(Apwrdelta);
+				else
+					t3 = 0;
+				if (((value&0xf000)>>12) > Apwrdelta)
+					t4 = ((value&0xf000)>>12)-(Apwrdelta);
+				else
+					t4 = 0;
+			}
+			Adata |= ((t1<<16) + (t2<<20) + (t3<<24) + (t4<<28));
+
+			if (i == 0)
+				pAd->Tx20MPwrCfgABand[i] = (pAd->Tx20MPwrCfgABand[i] & 0x0000FFFF) | (Adata & 0xFFFF0000);
+			else
+				pAd->Tx20MPwrCfgABand[i] = Adata;
+
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("20MHz BW, 5GHz band, Adata = %lx \n", Adata));
+		}
+	}
+
+	//
+	// Check this block is valid for 40MHz in 5G. If invalid, use parameter for 20MHz in 2.4G
+	//
+	bValid = TRUE;
+	for (i=0; i<6; i++)
+	{
+		RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_40MHZ_5G + 2 + i*2, value);
+		if (((value & 0x00FF) == 0x00FF) || ((value & 0xFF00) == 0xFF00))
+		{
+			bValid = FALSE;
+			break;
+		}
+	}
+
+	//
+	// Get Txpower per MCS for 40MHz in 5G.
+	//
+	if (bValid)
+	{
+		for (i=0; i<4; i++)
+		{
+			RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_40MHZ_5G + i*4, value);
+			if (bApwrdeltaMinus == FALSE)
+			{
+				t1 = (value&0xf)+(Apwrdelta);
+				if (t1 > 0xf)
+					t1 = 0xf;
+				t2 = ((value&0xf0)>>4)+(Apwrdelta);
+				if (t2 > 0xf)
+					t2 = 0xf;
+				t3 = ((value&0xf00)>>8)+(Apwrdelta);
+				if (t3 > 0xf)
+					t3 = 0xf;
+				t4 = ((value&0xf000)>>12)+(Apwrdelta);
+				if (t4 > 0xf)
+					t4 = 0xf;
+			}
+			else
+			{
+				if ((value&0xf) > Apwrdelta)
+					t1 = (value&0xf)-(Apwrdelta);
+				else
+					t1 = 0;
+				if (((value&0xf0)>>4) > Apwrdelta)
+					t2 = ((value&0xf0)>>4)-(Apwrdelta);
+				else
+					t2 = 0;
+				if (((value&0xf00)>>8) > Apwrdelta)
+					t3 = ((value&0xf00)>>8)-(Apwrdelta);
+				else
+					t3 = 0;
+				if (((value&0xf000)>>12) > Apwrdelta)
+					t4 = ((value&0xf000)>>12)-(Apwrdelta);
+				else
+					t4 = 0;
+			}
+			Adata = t1 + (t2<<4) + (t3<<8) + (t4<<12);
+
+			RT28xx_EEPROM_READ16(pAd, EEPROM_TXPOWER_BYRATE_40MHZ_5G + i*4 + 2, value);
+			if (bApwrdeltaMinus == FALSE)
+			{
+				t1 = (value&0xf)+(Apwrdelta);
+				if (t1 > 0xf)
+					t1 = 0xf;
+				t2 = ((value&0xf0)>>4)+(Apwrdelta);
+				if (t2 > 0xf)
+					t2 = 0xf;
+				t3 = ((value&0xf00)>>8)+(Apwrdelta);
+				if (t3 > 0xf)
+					t3 = 0xf;
+				t4 = ((value&0xf000)>>12)+(Apwrdelta);
+				if (t4 > 0xf)
+					t4 = 0xf;
+			}
+			else
+			{
+				if ((value&0xf) > Apwrdelta)
+					t1 = (value&0xf)-(Apwrdelta);
+				else
+					t1 = 0;
+				if (((value&0xf0)>>4) > Apwrdelta)
+					t2 = ((value&0xf0)>>4)-(Apwrdelta);
+				else
+					t2 = 0;
+				if (((value&0xf00)>>8) > Apwrdelta)
+					t3 = ((value&0xf00)>>8)-(Apwrdelta);
+				else
+					t3 = 0;
+				if (((value&0xf000)>>12) > Apwrdelta)
+					t4 = ((value&0xf000)>>12)-(Apwrdelta);
+				else
+					t4 = 0;
+			}
+			Adata |= ((t1<<16) + (t2<<20) + (t3<<24) + (t4<<28));
+
+			if (i == 0)
+				pAd->Tx40MPwrCfgABand[i+1] = (pAd->Tx40MPwrCfgABand[i+1] & 0x0000FFFF) | (Adata & 0xFFFF0000);
+			else
+				pAd->Tx40MPwrCfgABand[i+1] = Adata;
+
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("40MHz BW, 5GHz band, Adata = %lx \n", Adata));
+		}
+	}
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Read initial channel power parameters from EEPROM
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPReadChannelPwr(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	UCHAR				i, choffset;
+	EEPROM_TX_PWR_STRUC	    Power;
+	EEPROM_TX_PWR_STRUC	    Power2;
+
+	// Read Tx power value for all channels
+	// Value from 1 - 0x7f. Default value is 24.
+	// Power value : 2.4G 0x00 (0) ~ 0x1F (31)
+	//             : 5.5G 0xF9 (-7) ~ 0x0F (15)
+
+	// 0. 11b/g, ch1 - ch 14
+	for (i = 0; i < 7; i++)
+	{
+//		Power.word = RTMP_EEPROM_READ16(pAd, EEPROM_G_TX_PWR_OFFSET + i * 2);
+//		Power2.word = RTMP_EEPROM_READ16(pAd, EEPROM_G_TX2_PWR_OFFSET + i * 2);
+		RT28xx_EEPROM_READ16(pAd, EEPROM_G_TX_PWR_OFFSET + i * 2, Power.word);
+		RT28xx_EEPROM_READ16(pAd, EEPROM_G_TX2_PWR_OFFSET + i * 2, Power2.word);
+		pAd->TxPower[i * 2].Channel = i * 2 + 1;
+		pAd->TxPower[i * 2 + 1].Channel = i * 2 + 2;
+
+		if ((Power.field.Byte0 > 31) || (Power.field.Byte0 < 0))
+			pAd->TxPower[i * 2].Power = DEFAULT_RF_TX_POWER;
+		else
+			pAd->TxPower[i * 2].Power = Power.field.Byte0;
+
+		if ((Power.field.Byte1 > 31) || (Power.field.Byte1 < 0))
+			pAd->TxPower[i * 2 + 1].Power = DEFAULT_RF_TX_POWER;
+		else
+			pAd->TxPower[i * 2 + 1].Power = Power.field.Byte1;
+
+		if ((Power2.field.Byte0 > 31) || (Power2.field.Byte0 < 0))
+			pAd->TxPower[i * 2].Power2 = DEFAULT_RF_TX_POWER;
+		else
+			pAd->TxPower[i * 2].Power2 = Power2.field.Byte0;
+
+		if ((Power2.field.Byte1 > 31) || (Power2.field.Byte1 < 0))
+			pAd->TxPower[i * 2 + 1].Power2 = DEFAULT_RF_TX_POWER;
+		else
+			pAd->TxPower[i * 2 + 1].Power2 = Power2.field.Byte1;
+	}
+
+	// 1. U-NII lower/middle band: 36, 38, 40; 44, 46, 48; 52, 54, 56; 60, 62, 64 (including central frequency in BW 40MHz)
+	// 1.1 Fill up channel
+	choffset = 14;
+	for (i = 0; i < 4; i++)
+	{
+		pAd->TxPower[3 * i + choffset + 0].Channel	= 36 + i * 8 + 0;
+		pAd->TxPower[3 * i + choffset + 0].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 0].Power2	= DEFAULT_RF_TX_POWER;
+
+		pAd->TxPower[3 * i + choffset + 1].Channel	= 36 + i * 8 + 2;
+		pAd->TxPower[3 * i + choffset + 1].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 1].Power2	= DEFAULT_RF_TX_POWER;
+
+		pAd->TxPower[3 * i + choffset + 2].Channel	= 36 + i * 8 + 4;
+		pAd->TxPower[3 * i + choffset + 2].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 2].Power2	= DEFAULT_RF_TX_POWER;
+	}
+
+	// 1.2 Fill up power
+	for (i = 0; i < 6; i++)
+	{
+//		Power.word = RTMP_EEPROM_READ16(pAd, EEPROM_A_TX_PWR_OFFSET + i * 2);
+//		Power2.word = RTMP_EEPROM_READ16(pAd, EEPROM_A_TX2_PWR_OFFSET + i * 2);
+		RT28xx_EEPROM_READ16(pAd, EEPROM_A_TX_PWR_OFFSET + i * 2, Power.word);
+		RT28xx_EEPROM_READ16(pAd, EEPROM_A_TX2_PWR_OFFSET + i * 2, Power2.word);
+
+		if ((Power.field.Byte0 < 16) && (Power.field.Byte0 >= -7))
+			pAd->TxPower[i * 2 + choffset + 0].Power = Power.field.Byte0;
+
+		if ((Power.field.Byte1 < 16) && (Power.field.Byte1 >= -7))
+			pAd->TxPower[i * 2 + choffset + 1].Power = Power.field.Byte1;
+
+		if ((Power2.field.Byte0 < 16) && (Power2.field.Byte0 >= -7))
+			pAd->TxPower[i * 2 + choffset + 0].Power2 = Power2.field.Byte0;
+
+		if ((Power2.field.Byte1 < 16) && (Power2.field.Byte1 >= -7))
+			pAd->TxPower[i * 2 + choffset + 1].Power2 = Power2.field.Byte1;
+	}
+
+	// 2. HipperLAN 2 100, 102 ,104; 108, 110, 112; 116, 118, 120; 124, 126, 128; 132, 134, 136; 140 (including central frequency in BW 40MHz)
+	// 2.1 Fill up channel
+	choffset = 14 + 12;
+	for (i = 0; i < 5; i++)
+	{
+		pAd->TxPower[3 * i + choffset + 0].Channel	= 100 + i * 8 + 0;
+		pAd->TxPower[3 * i + choffset + 0].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 0].Power2	= DEFAULT_RF_TX_POWER;
+
+		pAd->TxPower[3 * i + choffset + 1].Channel	= 100 + i * 8 + 2;
+		pAd->TxPower[3 * i + choffset + 1].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 1].Power2	= DEFAULT_RF_TX_POWER;
+
+		pAd->TxPower[3 * i + choffset + 2].Channel	= 100 + i * 8 + 4;
+		pAd->TxPower[3 * i + choffset + 2].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 2].Power2	= DEFAULT_RF_TX_POWER;
+	}
+	pAd->TxPower[3 * 5 + choffset + 0].Channel		= 140;
+	pAd->TxPower[3 * 5 + choffset + 0].Power		= DEFAULT_RF_TX_POWER;
+	pAd->TxPower[3 * 5 + choffset + 0].Power2		= DEFAULT_RF_TX_POWER;
+
+	// 2.2 Fill up power
+	for (i = 0; i < 8; i++)
+	{
+//		Power.word = RTMP_EEPROM_READ16(pAd, EEPROM_A_TX_PWR_OFFSET + (choffset - 14) + i * 2);
+//		Power2.word = RTMP_EEPROM_READ16(pAd, EEPROM_A_TX2_PWR_OFFSET + (choffset - 14) + i * 2);
+		RT28xx_EEPROM_READ16(pAd, EEPROM_A_TX_PWR_OFFSET + (choffset - 14) + i * 2, Power.word);
+		RT28xx_EEPROM_READ16(pAd, EEPROM_A_TX2_PWR_OFFSET + (choffset - 14) + i * 2, Power2.word);
+
+		if ((Power.field.Byte0 < 16) && (Power.field.Byte0 >= -7))
+			pAd->TxPower[i * 2 + choffset + 0].Power = Power.field.Byte0;
+
+		if ((Power.field.Byte1 < 16) && (Power.field.Byte1 >= -7))
+			pAd->TxPower[i * 2 + choffset + 1].Power = Power.field.Byte1;
+
+		if ((Power2.field.Byte0 < 16) && (Power2.field.Byte0 >= -7))
+			pAd->TxPower[i * 2 + choffset + 0].Power2 = Power2.field.Byte0;
+
+		if ((Power2.field.Byte1 < 16) && (Power2.field.Byte1 >= -7))
+			pAd->TxPower[i * 2 + choffset + 1].Power2 = Power2.field.Byte1;
+	}
+
+	// 3. U-NII upper band: 149, 151, 153; 157, 159, 161; 165 (including central frequency in BW 40MHz)
+	// 3.1 Fill up channel
+	choffset = 14 + 12 + 16;
+	for (i = 0; i < 2; i++)
+	{
+		pAd->TxPower[3 * i + choffset + 0].Channel	= 149 + i * 8 + 0;
+		pAd->TxPower[3 * i + choffset + 0].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 0].Power2	= DEFAULT_RF_TX_POWER;
+
+		pAd->TxPower[3 * i + choffset + 1].Channel	= 149 + i * 8 + 2;
+		pAd->TxPower[3 * i + choffset + 1].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 1].Power2	= DEFAULT_RF_TX_POWER;
+
+		pAd->TxPower[3 * i + choffset + 2].Channel	= 149 + i * 8 + 4;
+		pAd->TxPower[3 * i + choffset + 2].Power	= DEFAULT_RF_TX_POWER;
+		pAd->TxPower[3 * i + choffset + 2].Power2	= DEFAULT_RF_TX_POWER;
+	}
+	pAd->TxPower[3 * 2 + choffset + 0].Channel		= 165;
+	pAd->TxPower[3 * 2 + choffset + 0].Power		= DEFAULT_RF_TX_POWER;
+	pAd->TxPower[3 * 2 + choffset + 0].Power2		= DEFAULT_RF_TX_POWER;
+
+	// 3.2 Fill up power
+	for (i = 0; i < 4; i++)
+	{
+//		Power.word = RTMP_EEPROM_READ16(pAd, EEPROM_A_TX_PWR_OFFSET + (choffset - 14) + i * 2);
+//		Power2.word = RTMP_EEPROM_READ16(pAd, EEPROM_A_TX2_PWR_OFFSET + (choffset - 14) + i * 2);
+		RT28xx_EEPROM_READ16(pAd, EEPROM_A_TX_PWR_OFFSET + (choffset - 14) + i * 2, Power.word);
+		RT28xx_EEPROM_READ16(pAd, EEPROM_A_TX2_PWR_OFFSET + (choffset - 14) + i * 2, Power2.word);
+
+		if ((Power.field.Byte0 < 16) && (Power.field.Byte0 >= -7))
+			pAd->TxPower[i * 2 + choffset + 0].Power = Power.field.Byte0;
+
+		if ((Power.field.Byte1 < 16) && (Power.field.Byte1 >= -7))
+			pAd->TxPower[i * 2 + choffset + 1].Power = Power.field.Byte1;
+
+		if ((Power2.field.Byte0 < 16) && (Power2.field.Byte0 >= -7))
+			pAd->TxPower[i * 2 + choffset + 0].Power2 = Power2.field.Byte0;
+
+		if ((Power2.field.Byte1 < 16) && (Power2.field.Byte1 >= -7))
+			pAd->TxPower[i * 2 + choffset + 1].Power2 = Power2.field.Byte1;
+	}
+
+	// 4. Print and Debug
+	choffset = 14 + 12 + 16 + 7;
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Read the following from the registry
+		1. All the parameters
+		2. NetworkAddres
+
+	Arguments:
+		Adapter						Pointer to our adapter
+		WrapperConfigurationContext	For use by NdisOpenConfiguration
+
+	Return Value:
+		NDIS_STATUS_SUCCESS
+		NDIS_STATUS_FAILURE
+		NDIS_STATUS_RESOURCES
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS	NICReadRegParameters(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	NDIS_HANDLE			WrapperConfigurationContext
+	)
+{
+	NDIS_STATUS						Status = NDIS_STATUS_SUCCESS;
+	DBGPRINT_S(Status, ("<-- NICReadRegParameters, Status=%x\n", Status));
+	return Status;
+}
+
+
+#ifdef RT2870
+/*
+	========================================================================
+
+	Routine Description:
+		For RF filter calibration purpose
+
+	Arguments:
+		pAd                          Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	========================================================================
+*/
+VOID RTUSBFilterCalibration(
+	IN PRTMP_ADAPTER pAd)
+{
+	UCHAR	R55x = 0, value, FilterTarget = 0x1E, BBPValue;
+	UINT	loop = 0, count = 0, loopcnt = 0, ReTry = 0;
+	UCHAR	RF_R24_Value = 0;
+
+	// Give bbp filter initial value
+	pAd->Mlme.CaliBW20RfR24 = 0x16;
+	pAd->Mlme.CaliBW40RfR24 = 0x36;  //Bit[5] must be 1 for BW 40
+
+	do
+	{
+		if (loop == 1)	//BandWidth = 40 MHz
+		{
+			// Write 0x27 to RF_R24 to program filter
+			RF_R24_Value = 0x27;
+			RT30xxWriteRFRegister(pAd, RF_R24, RF_R24_Value);
+			FilterTarget = 0x19;
+
+			// when calibrate BW40, BBP mask must set to BW40.
+			RTUSBReadBBPRegister(pAd, BBP_R4, &BBPValue);
+			BBPValue&= (~0x18);
+			BBPValue|= (0x10);
+			RTUSBWriteBBPRegister(pAd, BBP_R4, BBPValue);
+		}
+		else			//BandWidth = 20 MHz
+		{
+			// Write 0x07 to RF_R24 to program filter
+			RF_R24_Value = 0x07;
+			RT30xxWriteRFRegister(pAd, RF_R24, RF_R24_Value);
+			FilterTarget = 0x16;
+		}
+
+		// Write 0x01 to RF_R22 to enable baseband loopback mode
+		RT30xxReadRFRegister(pAd, RF_R22, &value);
+		value |= 0x01;
+		RT30xxWriteRFRegister(pAd, RF_R22, value);
+
+		// Write 0x00 to BBP_R24 to set power & frequency of passband test tone
+		RTUSBWriteBBPRegister(pAd, BBP_R24, 0);
+
+		do
+		{
+			// Write 0x90 to BBP_R25 to transmit test tone
+			RTUSBWriteBBPRegister(pAd, BBP_R25, 0x90);
+
+			RTMPusecDelay(1000);
+			// Read BBP_R55[6:0] for received power, set R55x = BBP_R55[6:0]
+			RTUSBReadBBPRegister(pAd, BBP_R55, &value);
+			R55x = value & 0xFF;
+
+		} while ((ReTry++ < 100) && (R55x == 0));
+
+		// Write 0x06 to BBP_R24 to set power & frequency of stopband test tone
+		RTUSBWriteBBPRegister(pAd, BBP_R24, 0x06);
+
+		while(TRUE)
+		{
+			// Write 0x90 to BBP_R25 to transmit test tone
+			RTUSBWriteBBPRegister(pAd, BBP_R25, 0x90);
+
+			//We need to wait for calibration
+			RTMPusecDelay(1000);
+			RTUSBReadBBPRegister(pAd, BBP_R55, &value);
+			value &= 0xFF;
+			if ((R55x - value) < FilterTarget)
+			{
+				RF_R24_Value ++;
+			}
+			else if ((R55x - value) == FilterTarget)
+			{
+				RF_R24_Value ++;
+				count ++;
+			}
+			else
+			{
+				break;
+			}
+
+			// prevent infinite loop cause driver hang.
+			if (loopcnt++ > 100)
+			{
+				DBGPRINT(RT_DEBUG_ERROR, ("RTUSBFilterCalibration - can't find a valid value, loopcnt=%d stop calibrating", loopcnt));
+				break;
+			}
+
+			// Write RF_R24 to program filter
+			RT30xxWriteRFRegister(pAd, RF_R24, RF_R24_Value);
+		}
+
+		if (count > 0)
+		{
+			RF_R24_Value = RF_R24_Value - ((count) ? (1) : (0));
+		}
+
+		// Store for future usage
+		if (loopcnt < 100)
+		{
+			if (loop++ == 0)
+			{
+				//BandWidth = 20 MHz
+				pAd->Mlme.CaliBW20RfR24 = (UCHAR)RF_R24_Value;
+			}
+			else
+			{
+				//BandWidth = 40 MHz
+				pAd->Mlme.CaliBW40RfR24 = (UCHAR)RF_R24_Value;
+				break;
+			}
+		}
+		else
+			break;
+
+		RT30xxWriteRFRegister(pAd, RF_R24, RF_R24_Value);
+
+		// reset count
+		count = 0;
+	} while(TRUE);
+
+	//
+	// Set back to initial state
+	//
+	RTUSBWriteBBPRegister(pAd, BBP_R24, 0);
+
+	RT30xxReadRFRegister(pAd, RF_R22, &value);
+	value &= ~(0x01);
+	RT30xxWriteRFRegister(pAd, RF_R22, value);
+
+	// set BBP back to BW20
+	RTUSBReadBBPRegister(pAd, BBP_R4, &BBPValue);
+	BBPValue&= (~0x18);
+	RTUSBWriteBBPRegister(pAd, BBP_R4, BBPValue);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTUSBFilterCalibration - CaliBW20RfR24=0x%x, CaliBW40RfR24=0x%x\n", pAd->Mlme.CaliBW20RfR24, pAd->Mlme.CaliBW40RfR24));
+}
+
+
+VOID NICInitRT30xxRFRegisters(IN PRTMP_ADAPTER pAd)
+{
+	INT i;
+	// Driver must read EEPROM to get RfIcType before initial RF registers
+	// Initialize RF register to default value
+        if (IS_RT3070(pAd) && ((pAd->RfIcType == RFIC_3020) ||(pAd->RfIcType == RFIC_2020)))
+        {
+                // Init RF calibration
+                // Driver should toggle RF R30 bit7 before init RF registers
+                ULONG RfReg = 0;
+                RT30xxReadRFRegister(pAd, RF_R30, (PUCHAR)&RfReg);
+                RfReg |= 0x80;
+                RT30xxWriteRFRegister(pAd, RF_R30, (UCHAR)RfReg);
+                RTMPusecDelay(1000);
+                RfReg &= 0x7F;
+                RT30xxWriteRFRegister(pAd, RF_R30, (UCHAR)RfReg);
+
+                // Initialize RF register to default value
+                for (i = 0; i < NUM_RF_REG_PARMS; i++)
+                {
+                        RT30xxWriteRFRegister(pAd, RT30xx_RFRegTable[i].Register, RT30xx_RFRegTable[i].Value);
+                }
+
+                //For RF filter Calibration
+                RTUSBFilterCalibration(pAd);
+        }
+
+}
+#endif // RT2870 //
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Read initial parameters from EEPROM
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	NICReadEEPROMParameters(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			mac_addr)
+{
+	UINT32			data = 0;
+	USHORT			i, value, value2;
+	UCHAR			TmpPhy;
+	EEPROM_TX_PWR_STRUC	    Power;
+	EEPROM_VERSION_STRUC    Version;
+	EEPROM_ANTENNA_STRUC	Antenna;
+	EEPROM_NIC_CONFIG2_STRUC    NicConfig2;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> NICReadEEPROMParameters\n"));
+
+	// Init EEPROM Address Number, before access EEPROM; if 93c46, EEPROMAddressNum=6, else if 93c66, EEPROMAddressNum=8
+	RTMP_IO_READ32(pAd, E2PROM_CSR, &data);
+	DBGPRINT(RT_DEBUG_TRACE, ("--> E2PROM_CSR = 0x%x\n", data));
+
+	if((data & 0x30) == 0)
+		pAd->EEPROMAddressNum = 6;		// 93C46
+	else if((data & 0x30) == 0x10)
+		pAd->EEPROMAddressNum = 8;     // 93C66
+	else
+		pAd->EEPROMAddressNum = 8;     // 93C86
+	DBGPRINT(RT_DEBUG_TRACE, ("--> EEPROMAddressNum = %d\n", pAd->EEPROMAddressNum ));
+
+	// RT2860 MAC no longer auto load MAC address from E2PROM. Driver has to intialize
+	// MAC address registers according to E2PROM setting
+	if (mac_addr == NULL ||
+		strlen(mac_addr) != 17 ||
+		mac_addr[2] != ':'  || mac_addr[5] != ':'  || mac_addr[8] != ':' ||
+		mac_addr[11] != ':' || mac_addr[14] != ':')
+	{
+		USHORT  Addr01,Addr23,Addr45 ;
+
+		RT28xx_EEPROM_READ16(pAd, 0x04, Addr01);
+		RT28xx_EEPROM_READ16(pAd, 0x06, Addr23);
+		RT28xx_EEPROM_READ16(pAd, 0x08, Addr45);
+
+		pAd->PermanentAddress[0] = (UCHAR)(Addr01 & 0xff);
+		pAd->PermanentAddress[1] = (UCHAR)(Addr01 >> 8);
+		pAd->PermanentAddress[2] = (UCHAR)(Addr23 & 0xff);
+		pAd->PermanentAddress[3] = (UCHAR)(Addr23 >> 8);
+		pAd->PermanentAddress[4] = (UCHAR)(Addr45 & 0xff);
+		pAd->PermanentAddress[5] = (UCHAR)(Addr45 >> 8);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("Initialize MAC Address from E2PROM \n"));
+	}
+	else
+	{
+		INT		j;
+		PUCHAR	macptr;
+
+		macptr = mac_addr;
+
+		for (j=0; j<MAC_ADDR_LEN; j++)
+		{
+			AtoH(macptr, &pAd->PermanentAddress[j], 1);
+			macptr=macptr+3;
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("Initialize MAC Address from module parameter \n"));
+	}
+
+
+	{
+#if 0
+		USHORT  Addr01,Addr23,Addr45 ;
+
+		Addr01=RTMP_EEPROM_READ16(pAd, 0x04);
+		Addr23=RTMP_EEPROM_READ16(pAd, 0x06);
+		Addr45=RTMP_EEPROM_READ16(pAd, 0x08);
+
+		pAd->PermanentAddress[0] = (UCHAR)(Addr01 & 0xff);
+		pAd->PermanentAddress[1] = (UCHAR)(Addr01 >> 8);
+		pAd->PermanentAddress[2] = (UCHAR)(Addr23 & 0xff);
+		pAd->PermanentAddress[3] = (UCHAR)(Addr23 >> 8);
+		pAd->PermanentAddress[4] = (UCHAR)(Addr45 & 0xff);
+		pAd->PermanentAddress[5] = (UCHAR)(Addr45 >> 8);
+#endif
+		//more conveninet to test mbssid, so ap's bssid &0xf1
+		if (pAd->PermanentAddress[0] == 0xff)
+			pAd->PermanentAddress[0] = RandomByte(pAd)&0xf8;
+
+		//if (pAd->PermanentAddress[5] == 0xff)
+		//	pAd->PermanentAddress[5] = RandomByte(pAd)&0xf8;
+
+		DBGPRINT_RAW(RT_DEBUG_TRACE,("E2PROM MAC: =%02x:%02x:%02x:%02x:%02x:%02x\n",
+			pAd->PermanentAddress[0], pAd->PermanentAddress[1],
+			pAd->PermanentAddress[2], pAd->PermanentAddress[3],
+			pAd->PermanentAddress[4], pAd->PermanentAddress[5]));
+		if (pAd->bLocalAdminMAC == FALSE)
+		{
+			MAC_DW0_STRUC csr2;
+			MAC_DW1_STRUC csr3;
+			COPY_MAC_ADDR(pAd->CurrentAddress, pAd->PermanentAddress);
+			csr2.field.Byte0 = pAd->CurrentAddress[0];
+			csr2.field.Byte1 = pAd->CurrentAddress[1];
+			csr2.field.Byte2 = pAd->CurrentAddress[2];
+			csr2.field.Byte3 = pAd->CurrentAddress[3];
+			RTMP_IO_WRITE32(pAd, MAC_ADDR_DW0, csr2.word);
+			csr3.word = 0;
+			csr3.field.Byte4 = pAd->CurrentAddress[4];
+			csr3.field.Byte5 = pAd->CurrentAddress[5];
+			csr3.field.U2MeMask = 0xff;
+			RTMP_IO_WRITE32(pAd, MAC_ADDR_DW1, csr3.word);
+			DBGPRINT_RAW(RT_DEBUG_TRACE,("E2PROM MAC: =%02x:%02x:%02x:%02x:%02x:%02x\n",
+				pAd->PermanentAddress[0], pAd->PermanentAddress[1],
+				pAd->PermanentAddress[2], pAd->PermanentAddress[3],
+				pAd->PermanentAddress[4], pAd->PermanentAddress[5]));
+		}
+	}
+
+	// if not return early. cause fail at emulation.
+	// Init the channel number for TX channel power
+	RTMPReadChannelPwr(pAd);
+
+	// if E2PROM version mismatch with driver's expectation, then skip
+	// all subsequent E2RPOM retieval and set a system error bit to notify GUI
+	RT28xx_EEPROM_READ16(pAd, EEPROM_VERSION_OFFSET, Version.word);
+	pAd->EepromVersion = Version.field.Version + Version.field.FaeReleaseNumber * 256;
+	DBGPRINT(RT_DEBUG_TRACE, ("E2PROM: Version = %d, FAE release #%d\n", Version.field.Version, Version.field.FaeReleaseNumber));
+
+	if (Version.field.Version > VALID_EEPROM_VERSION)
+	{
+		DBGPRINT_ERR(("E2PROM: WRONG VERSION 0x%x, should be %d\n",Version.field.Version, VALID_EEPROM_VERSION));
+		/*pAd->SystemErrorBitmap |= 0x00000001;
+
+		// hard-code default value when no proper E2PROM installed
+		pAd->bAutoTxAgcA = FALSE;
+		pAd->bAutoTxAgcG = FALSE;
+
+		// Default the channel power
+		for (i = 0; i < MAX_NUM_OF_CHANNELS; i++)
+			pAd->TxPower[i].Power = DEFAULT_RF_TX_POWER;
+
+		// Default the channel power
+		for (i = 0; i < MAX_NUM_OF_11JCHANNELS; i++)
+			pAd->TxPower11J[i].Power = DEFAULT_RF_TX_POWER;
+
+		for(i = 0; i < NUM_EEPROM_BBP_PARMS; i++)
+			pAd->EEPROMDefaultValue[i] = 0xffff;
+		return;  */
+	}
+
+	// Read BBP default value from EEPROM and store to array(EEPROMDefaultValue) in pAd
+	RT28xx_EEPROM_READ16(pAd, EEPROM_NIC1_OFFSET, value);
+	pAd->EEPROMDefaultValue[0] = value;
+
+	RT28xx_EEPROM_READ16(pAd, EEPROM_NIC2_OFFSET, value);
+	pAd->EEPROMDefaultValue[1] = value;
+
+	RT28xx_EEPROM_READ16(pAd, 0x38, value);	// Country Region
+	pAd->EEPROMDefaultValue[2] = value;
+
+	for(i = 0; i < 8; i++)
+	{
+		RT28xx_EEPROM_READ16(pAd, EEPROM_BBP_BASE_OFFSET + i*2, value);
+		pAd->EEPROMDefaultValue[i+3] = value;
+	}
+
+	// We have to parse NIC configuration 0 at here.
+	// If TSSI did not have preloaded value, it should reset the TxAutoAgc to false
+	// Therefore, we have to read TxAutoAgc control beforehand.
+	// Read Tx AGC control bit
+	Antenna.word = pAd->EEPROMDefaultValue[0];
+	if (Antenna.word == 0xFFFF)
+	{
+		Antenna.word = 0;
+		Antenna.field.RfIcType = RFIC_2820;
+		Antenna.field.TxPath = 1;
+		Antenna.field.RxPath = 2;
+		DBGPRINT(RT_DEBUG_WARN, ("E2PROM error, hard code as 0x%04x\n", Antenna.word));
+	}
+
+	// Choose the desired Tx&Rx stream.
+	if ((pAd->CommonCfg.TxStream == 0) || (pAd->CommonCfg.TxStream > Antenna.field.TxPath))
+		pAd->CommonCfg.TxStream = Antenna.field.TxPath;
+
+	if ((pAd->CommonCfg.RxStream == 0) || (pAd->CommonCfg.RxStream > Antenna.field.RxPath))
+	{
+		pAd->CommonCfg.RxStream = Antenna.field.RxPath;
+
+		if ((pAd->MACVersion < RALINK_2883_VERSION) &&
+			(pAd->CommonCfg.RxStream > 2))
+		{
+			// only 2 Rx streams for RT2860 series
+			pAd->CommonCfg.RxStream = 2;
+		}
+	}
+
+	// 3*3
+	// read value from EEPROM and set them to CSR174 ~ 177 in chain0 ~ chain2
+	// yet implement
+	for(i=0; i<3; i++)
+	{
+	}
+
+	NicConfig2.word = pAd->EEPROMDefaultValue[1];
+
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		NicConfig2.word = 0;
+		if ((NicConfig2.word & 0x00ff) == 0xff)
+		{
+			NicConfig2.word &= 0xff00;
+		}
+
+		if ((NicConfig2.word >> 8) == 0xff)
+		{
+			NicConfig2.word &= 0x00ff;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	if (NicConfig2.field.DynamicTxAgcControl == 1)
+		pAd->bAutoTxAgcA = pAd->bAutoTxAgcG = TRUE;
+	else
+		pAd->bAutoTxAgcA = pAd->bAutoTxAgcG = FALSE;
+
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("NICReadEEPROMParameters: RxPath = %d, TxPath = %d\n", Antenna.field.RxPath, Antenna.field.TxPath));
+
+	// Save the antenna for future use
+	pAd->Antenna.word = Antenna.word;
+
+	//
+	// Reset PhyMode if we don't support 802.11a
+	// Only RFIC_2850 & RFIC_2750 support 802.11a
+	//
+	if ((Antenna.field.RfIcType != RFIC_2850) && (Antenna.field.RfIcType != RFIC_2750))
+	{
+		if ((pAd->CommonCfg.PhyMode == PHY_11ABG_MIXED) ||
+			(pAd->CommonCfg.PhyMode == PHY_11A))
+			pAd->CommonCfg.PhyMode = PHY_11BG_MIXED;
+#ifdef DOT11_N_SUPPORT
+		else if ((pAd->CommonCfg.PhyMode == PHY_11ABGN_MIXED)	||
+				 (pAd->CommonCfg.PhyMode == PHY_11AN_MIXED) 	||
+				 (pAd->CommonCfg.PhyMode == PHY_11AGN_MIXED) 	||
+				 (pAd->CommonCfg.PhyMode == PHY_11N_5G))
+			pAd->CommonCfg.PhyMode = PHY_11BGN_MIXED;
+#endif // DOT11_N_SUPPORT //
+	}
+
+	// Read TSSI reference and TSSI boundary for temperature compensation. This is ugly
+	// 0. 11b/g
+	{
+		/* these are tempature reference value (0x00 ~ 0xFE)
+		   ex: 0x00 0x15 0x25 0x45 0x88 0xA0 0xB5 0xD0 0xF0
+		   TssiPlusBoundaryG [4] [3] [2] [1] [0] (smaller) +
+		   TssiMinusBoundaryG[0] [1] [2] [3] [4] (larger) */
+		RT28xx_EEPROM_READ16(pAd, 0x6E, Power.word);
+		pAd->TssiMinusBoundaryG[4] = Power.field.Byte0;
+		pAd->TssiMinusBoundaryG[3] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0x70, Power.word);
+		pAd->TssiMinusBoundaryG[2] = Power.field.Byte0;
+		pAd->TssiMinusBoundaryG[1] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0x72, Power.word);
+		pAd->TssiRefG   = Power.field.Byte0; /* reference value [0] */
+		pAd->TssiPlusBoundaryG[1] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0x74, Power.word);
+		pAd->TssiPlusBoundaryG[2] = Power.field.Byte0;
+		pAd->TssiPlusBoundaryG[3] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0x76, Power.word);
+		pAd->TssiPlusBoundaryG[4] = Power.field.Byte0;
+		pAd->TxAgcStepG = Power.field.Byte1;
+		pAd->TxAgcCompensateG = 0;
+		pAd->TssiMinusBoundaryG[0] = pAd->TssiRefG;
+		pAd->TssiPlusBoundaryG[0]  = pAd->TssiRefG;
+
+		// Disable TxAgc if the based value is not right
+		if (pAd->TssiRefG == 0xff)
+			pAd->bAutoTxAgcG = FALSE;
+
+		DBGPRINT(RT_DEBUG_TRACE,("E2PROM: G Tssi[-4 .. +4] = %d %d %d %d - %d -%d %d %d %d, step=%d, tuning=%d\n",
+			pAd->TssiMinusBoundaryG[4], pAd->TssiMinusBoundaryG[3], pAd->TssiMinusBoundaryG[2], pAd->TssiMinusBoundaryG[1],
+			pAd->TssiRefG,
+			pAd->TssiPlusBoundaryG[1], pAd->TssiPlusBoundaryG[2], pAd->TssiPlusBoundaryG[3], pAd->TssiPlusBoundaryG[4],
+			pAd->TxAgcStepG, pAd->bAutoTxAgcG));
+	}
+	// 1. 11a
+	{
+		RT28xx_EEPROM_READ16(pAd, 0xD4, Power.word);
+		pAd->TssiMinusBoundaryA[4] = Power.field.Byte0;
+		pAd->TssiMinusBoundaryA[3] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0xD6, Power.word);
+		pAd->TssiMinusBoundaryA[2] = Power.field.Byte0;
+		pAd->TssiMinusBoundaryA[1] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0xD8, Power.word);
+		pAd->TssiRefA   = Power.field.Byte0;
+		pAd->TssiPlusBoundaryA[1] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0xDA, Power.word);
+		pAd->TssiPlusBoundaryA[2] = Power.field.Byte0;
+		pAd->TssiPlusBoundaryA[3] = Power.field.Byte1;
+		RT28xx_EEPROM_READ16(pAd, 0xDC, Power.word);
+		pAd->TssiPlusBoundaryA[4] = Power.field.Byte0;
+		pAd->TxAgcStepA = Power.field.Byte1;
+		pAd->TxAgcCompensateA = 0;
+		pAd->TssiMinusBoundaryA[0] = pAd->TssiRefA;
+		pAd->TssiPlusBoundaryA[0]  = pAd->TssiRefA;
+
+		// Disable TxAgc if the based value is not right
+		if (pAd->TssiRefA == 0xff)
+			pAd->bAutoTxAgcA = FALSE;
+
+		DBGPRINT(RT_DEBUG_TRACE,("E2PROM: A Tssi[-4 .. +4] = %d %d %d %d - %d -%d %d %d %d, step=%d, tuning=%d\n",
+			pAd->TssiMinusBoundaryA[4], pAd->TssiMinusBoundaryA[3], pAd->TssiMinusBoundaryA[2], pAd->TssiMinusBoundaryA[1],
+			pAd->TssiRefA,
+			pAd->TssiPlusBoundaryA[1], pAd->TssiPlusBoundaryA[2], pAd->TssiPlusBoundaryA[3], pAd->TssiPlusBoundaryA[4],
+			pAd->TxAgcStepA, pAd->bAutoTxAgcA));
+	}
+	pAd->BbpRssiToDbmDelta = 0x0;
+
+	// Read frequency offset setting for RF
+	RT28xx_EEPROM_READ16(pAd, EEPROM_FREQ_OFFSET, value);
+	if ((value & 0x00FF) != 0x00FF)
+		pAd->RfFreqOffset = (ULONG) (value & 0x00FF);
+	else
+		pAd->RfFreqOffset = 0;
+	DBGPRINT(RT_DEBUG_TRACE, ("E2PROM: RF FreqOffset=0x%lx \n", pAd->RfFreqOffset));
+
+	//CountryRegion byte offset (38h)
+	value = pAd->EEPROMDefaultValue[2] >> 8;		// 2.4G band
+	value2 = pAd->EEPROMDefaultValue[2] & 0x00FF;	// 5G band
+
+	if ((value <= REGION_MAXIMUM_BG_BAND) && (value2 <= REGION_MAXIMUM_A_BAND))
+	{
+		pAd->CommonCfg.CountryRegion = ((UCHAR) value) | 0x80;
+		pAd->CommonCfg.CountryRegionForABand = ((UCHAR) value2) | 0x80;
+		TmpPhy = pAd->CommonCfg.PhyMode;
+		pAd->CommonCfg.PhyMode = 0xff;
+		RTMPSetPhyMode(pAd, TmpPhy);
+#ifdef DOT11_N_SUPPORT
+		SetCommonHT(pAd);
+#endif // DOT11_N_SUPPORT //
+	}
+
+	//
+	// Get RSSI Offset on EEPROM 0x9Ah & 0x9Ch.
+	// The valid value are (-10 ~ 10)
+	//
+	RT28xx_EEPROM_READ16(pAd, EEPROM_RSSI_BG_OFFSET, value);
+	pAd->BGRssiOffset0 = value & 0x00ff;
+	pAd->BGRssiOffset1 = (value >> 8);
+	RT28xx_EEPROM_READ16(pAd, EEPROM_RSSI_BG_OFFSET+2, value);
+	pAd->BGRssiOffset2 = value & 0x00ff;
+	pAd->ALNAGain1 = (value >> 8);
+	RT28xx_EEPROM_READ16(pAd, EEPROM_LNA_OFFSET, value);
+	pAd->BLNAGain = value & 0x00ff;
+	pAd->ALNAGain0 = (value >> 8);
+
+	// Validate 11b/g RSSI_0 offset.
+	if ((pAd->BGRssiOffset0 < -10) || (pAd->BGRssiOffset0 > 10))
+		pAd->BGRssiOffset0 = 0;
+
+	// Validate 11b/g RSSI_1 offset.
+	if ((pAd->BGRssiOffset1 < -10) || (pAd->BGRssiOffset1 > 10))
+		pAd->BGRssiOffset1 = 0;
+
+	// Validate 11b/g RSSI_2 offset.
+	if ((pAd->BGRssiOffset2 < -10) || (pAd->BGRssiOffset2 > 10))
+		pAd->BGRssiOffset2 = 0;
+
+	RT28xx_EEPROM_READ16(pAd, EEPROM_RSSI_A_OFFSET, value);
+	pAd->ARssiOffset0 = value & 0x00ff;
+	pAd->ARssiOffset1 = (value >> 8);
+	RT28xx_EEPROM_READ16(pAd, (EEPROM_RSSI_A_OFFSET+2), value);
+	pAd->ARssiOffset2 = value & 0x00ff;
+	pAd->ALNAGain2 = (value >> 8);
+
+	if (((UCHAR)pAd->ALNAGain1 == 0xFF) || (pAd->ALNAGain1 == 0x00))
+		pAd->ALNAGain1 = pAd->ALNAGain0;
+	if (((UCHAR)pAd->ALNAGain2 == 0xFF) || (pAd->ALNAGain2 == 0x00))
+		pAd->ALNAGain2 = pAd->ALNAGain0;
+
+	// Validate 11a RSSI_0 offset.
+	if ((pAd->ARssiOffset0 < -10) || (pAd->ARssiOffset0 > 10))
+		pAd->ARssiOffset0 = 0;
+
+	// Validate 11a RSSI_1 offset.
+	if ((pAd->ARssiOffset1 < -10) || (pAd->ARssiOffset1 > 10))
+		pAd->ARssiOffset1 = 0;
+
+	//Validate 11a RSSI_2 offset.
+	if ((pAd->ARssiOffset2 < -10) || (pAd->ARssiOffset2 > 10))
+		pAd->ARssiOffset2 = 0;
+
+	//
+	// Get LED Setting.
+	//
+	RT28xx_EEPROM_READ16(pAd, 0x3a, value);
+	pAd->LedCntl.word = (value&0xff00) >> 8;
+	RT28xx_EEPROM_READ16(pAd, EEPROM_LED1_OFFSET, value);
+	pAd->Led1 = value;
+	RT28xx_EEPROM_READ16(pAd, EEPROM_LED2_OFFSET, value);
+	pAd->Led2 = value;
+	RT28xx_EEPROM_READ16(pAd, EEPROM_LED3_OFFSET, value);
+	pAd->Led3 = value;
+
+	RTMPReadTxPwrPerRate(pAd);
+
+#ifdef SINGLE_SKU
+	//pAd->CommonCfg.DefineMaxTxPwr = RTMP_EEPROM_READ16(pAd, EEPROM_DEFINE_MAX_TXPWR);
+	RT28xx_EEPROM_READ16(pAd, EEPROM_DEFINE_MAX_TXPWR, pAd->CommonCfg.DefineMaxTxPwr);
+#endif // SINGLE_SKU //
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- NICReadEEPROMParameters\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Set default value from EEPROM
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	NICInitAsicFromEEPROM(
+	IN	PRTMP_ADAPTER	pAd)
+{
+#ifdef CONFIG_STA_SUPPORT
+	UINT32					data = 0;
+	UCHAR	BBPR1 = 0;
+#endif // CONFIG_STA_SUPPORT //
+	USHORT					i;
+	EEPROM_ANTENNA_STRUC	Antenna;
+	EEPROM_NIC_CONFIG2_STRUC    NicConfig2;
+	UCHAR	BBPR3 = 0;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> NICInitAsicFromEEPROM\n"));
+	for(i = 3; i < NUM_EEPROM_BBP_PARMS; i++)
+	{
+		UCHAR BbpRegIdx, BbpValue;
+
+		if ((pAd->EEPROMDefaultValue[i] != 0xFFFF) && (pAd->EEPROMDefaultValue[i] != 0))
+		{
+			BbpRegIdx = (UCHAR)(pAd->EEPROMDefaultValue[i] >> 8);
+			BbpValue  = (UCHAR)(pAd->EEPROMDefaultValue[i] & 0xff);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BbpRegIdx, BbpValue);
+		}
+	}
+
+	Antenna.word = pAd->Antenna.word;
+	pAd->Mlme.RealRxPath = (UCHAR) Antenna.field.RxPath;
+	pAd->RfIcType = (UCHAR) Antenna.field.RfIcType;
+
+	NicConfig2.word = pAd->EEPROMDefaultValue[1];
+
+
+	// Save the antenna for future use
+	pAd->NicConfig2.word = NicConfig2.word;
+
+	//
+	// Send LED Setting to MCU.
+	//
+	if (pAd->LedCntl.word == 0xFF)
+	{
+		pAd->LedCntl.word = 0x01;
+		pAd->Led1 = 0x5555;
+		pAd->Led2 = 0x2221;
+
+#ifdef RT2870
+		pAd->Led3 = 0x5627;
+#endif // RT2870 //
+	}
+
+	AsicSendCommandToMcu(pAd, 0x52, 0xff, (UCHAR)pAd->Led1, (UCHAR)(pAd->Led1 >> 8));
+	AsicSendCommandToMcu(pAd, 0x53, 0xff, (UCHAR)pAd->Led2, (UCHAR)(pAd->Led2 >> 8));
+	AsicSendCommandToMcu(pAd, 0x54, 0xff, (UCHAR)pAd->Led3, (UCHAR)(pAd->Led3 >> 8));
+    pAd->LedIndicatorStregth = 0xFF;
+    RTMPSetSignalLED(pAd, -100);	// Force signal strength Led to be turned off, before link up
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// Read Hardware controlled Radio state enable bit
+		if (NicConfig2.field.HardwareRadioControl == 1)
+		{
+			pAd->StaCfg.bHardwareRadio = TRUE;
+
+			// Read GPIO pin2 as Hardware controlled radio state
+			RTMP_IO_READ32(pAd, GPIO_CTRL_CFG, &data);
+			if ((data & 0x04) == 0)
+			{
+				pAd->StaCfg.bHwRadio = FALSE;
+				pAd->StaCfg.bRadio = FALSE;
+//				RTMP_IO_WRITE32(pAd, PWR_PIN_CFG, 0x00001818);
+				RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF);
+			}
+		}
+		else
+			pAd->StaCfg.bHardwareRadio = FALSE;
+
+		if (pAd->StaCfg.bRadio == FALSE)
+		{
+			RTMPSetLED(pAd, LED_RADIO_OFF);
+		}
+		else
+		{
+			RTMPSetLED(pAd, LED_RADIO_ON);
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	// Turn off patching for cardbus controller
+	if (NicConfig2.field.CardbusAcceleration == 1)
+	{
+//		pAd->bTest1 = TRUE;
+	}
+
+	if (NicConfig2.field.DynamicTxAgcControl == 1)
+		pAd->bAutoTxAgcA = pAd->bAutoTxAgcG = TRUE;
+	else
+		pAd->bAutoTxAgcA = pAd->bAutoTxAgcG = FALSE;
+	//
+	// Since BBP has been progamed, to make sure BBP setting will be
+	// upate inside of AsicAntennaSelect, so reset to UNKNOWN_BAND!!
+	//
+	pAd->CommonCfg.BandState = UNKNOWN_BAND;
+
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BBPR3);
+	BBPR3 &= (~0x18);
+	if(pAd->Antenna.field.RxPath == 3)
+	{
+		BBPR3 |= (0x10);
+	}
+	else if(pAd->Antenna.field.RxPath == 2)
+	{
+		BBPR3 |= (0x8);
+	}
+	else if(pAd->Antenna.field.RxPath == 1)
+	{
+		BBPR3 |= (0x0);
+	}
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BBPR3);
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// Handle the difference when 1T
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R1, &BBPR1);
+		if(pAd->Antenna.field.TxPath == 1)
+		{
+		BBPR1 &= (~0x18);
+		}
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R1, BBPR1);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("Use Hw Radio Control Pin=%d; if used Pin=%d;\n", pAd->CommonCfg.bHardwareRadio, pAd->CommonCfg.bHardwareRadio));
+	}
+#endif // CONFIG_STA_SUPPORT //
+	DBGPRINT(RT_DEBUG_TRACE, ("TxPath = %d, RxPath = %d, RFIC=%d, Polar+LED mode=%x\n", pAd->Antenna.field.TxPath, pAd->Antenna.field.RxPath, pAd->RfIcType, pAd->LedCntl.word));
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- NICInitAsicFromEEPROM\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Initialize NIC hardware
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS	NICInitializeAdapter(
+	IN	PRTMP_ADAPTER	pAd,
+	IN   BOOLEAN    bHardReset)
+{
+	NDIS_STATUS     Status = NDIS_STATUS_SUCCESS;
+	WPDMA_GLO_CFG_STRUC	GloCfg;
+//	INT_MASK_CSR_STRUC		IntMask;
+	ULONG	i =0, j=0;
+	AC_TXOP_CSR0_STRUC	csr0;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> NICInitializeAdapter\n"));
+
+	// 3. Set DMA global configuration except TX_DMA_EN and RX_DMA_EN bits:
+retry:
+	i = 0;
+	do
+	{
+		RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &GloCfg.word);
+		if ((GloCfg.field.TxDMABusy == 0)  && (GloCfg.field.RxDMABusy == 0))
+			break;
+
+		RTMPusecDelay(1000);
+		i++;
+	}while ( i<100);
+	DBGPRINT(RT_DEBUG_TRACE, ("<== DMA offset 0x208 = 0x%x\n", GloCfg.word));
+	GloCfg.word &= 0xff0;
+	GloCfg.field.EnTXWriteBackDDONE =1;
+	RTMP_IO_WRITE32(pAd, WPDMA_GLO_CFG, GloCfg.word);
+
+	// Record HW Beacon offset
+	pAd->BeaconOffset[0] = HW_BEACON_BASE0;
+	pAd->BeaconOffset[1] = HW_BEACON_BASE1;
+	pAd->BeaconOffset[2] = HW_BEACON_BASE2;
+	pAd->BeaconOffset[3] = HW_BEACON_BASE3;
+	pAd->BeaconOffset[4] = HW_BEACON_BASE4;
+	pAd->BeaconOffset[5] = HW_BEACON_BASE5;
+	pAd->BeaconOffset[6] = HW_BEACON_BASE6;
+	pAd->BeaconOffset[7] = HW_BEACON_BASE7;
+
+	//
+	// write all shared Ring's base address into ASIC
+	//
+
+	// asic simulation sequence put this ahead before loading firmware.
+	// pbf hardware reset
+
+	// Initialze ASIC for TX & Rx operation
+	if (NICInitializeAsic(pAd , bHardReset) != NDIS_STATUS_SUCCESS)
+	{
+		if (j++ == 0)
+		{
+			NICLoadFirmware(pAd);
+			goto retry;
+		}
+		return NDIS_STATUS_FAILURE;
+	}
+
+
+
+
+	// WMM parameter
+	csr0.word = 0;
+	RTMP_IO_WRITE32(pAd, WMM_TXOP0_CFG, csr0.word);
+	if (pAd->CommonCfg.PhyMode == PHY_11B)
+	{
+		csr0.field.Ac0Txop = 192;	// AC_VI: 192*32us ~= 6ms
+		csr0.field.Ac1Txop = 96;	// AC_VO: 96*32us  ~= 3ms
+	}
+	else
+	{
+		csr0.field.Ac0Txop = 96;	// AC_VI: 96*32us ~= 3ms
+		csr0.field.Ac1Txop = 48;	// AC_VO: 48*32us ~= 1.5ms
+	}
+	RTMP_IO_WRITE32(pAd, WMM_TXOP1_CFG, csr0.word);
+
+
+
+
+	// reset action
+	// Load firmware
+	//  Status = NICLoadFirmware(pAd);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- NICInitializeAdapter\n"));
+	return Status;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Initialize ASIC
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS	NICInitializeAsic(
+	IN	PRTMP_ADAPTER	pAd,
+	IN  BOOLEAN		bHardReset)
+{
+	ULONG			Index = 0;
+	UCHAR			R0 = 0xff;
+	UINT32			MacCsr12 = 0, Counter = 0;
+#ifdef RT2870
+	UINT32			MacCsr0 = 0;
+	NTSTATUS		Status;
+	UCHAR			Value = 0xff;
+#endif // RT2870 //
+	USHORT			KeyIdx;
+	INT				i,apidx;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> NICInitializeAsic\n"));
+
+
+#ifdef RT2870
+	//
+	// Make sure MAC gets ready after NICLoadFirmware().
+	//
+	Index = 0;
+
+	//To avoid hang-on issue when interface up in kernel 2.4,
+	//we use a local variable "MacCsr0" instead of using "pAd->MACVersion" directly.
+	do
+	{
+		RTMP_IO_READ32(pAd, MAC_CSR0, &MacCsr0);
+
+		if ((MacCsr0 != 0x00) && (MacCsr0 != 0xFFFFFFFF))
+			break;
+
+		RTMPusecDelay(10);
+	} while (Index++ < 100);
+
+	pAd->MACVersion = MacCsr0;
+	DBGPRINT(RT_DEBUG_TRACE, ("MAC_CSR0  [ Ver:Rev=0x%08x]\n", pAd->MACVersion));
+	// turn on bit13 (set to zero) after rt2860D. This is to solve high-current issue.
+	RTMP_IO_READ32(pAd, PBF_SYS_CTRL, &MacCsr12);
+	MacCsr12 &= (~0x2000);
+	RTMP_IO_WRITE32(pAd, PBF_SYS_CTRL, MacCsr12);
+
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x3);
+	RTMP_IO_WRITE32(pAd, USB_DMA_CFG, 0x0);
+	Status = RTUSBVenderReset(pAd);
+
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x0);
+
+	// Initialize MAC register to default value
+	for(Index=0; Index<NUM_MAC_REG_PARMS; Index++)
+	{
+		RTMP_IO_WRITE32(pAd, (USHORT)MACRegTable[Index].Register, MACRegTable[Index].Value);
+	}
+
+	if(IS_RT3070(pAd))
+	{
+		// According to Frank Hsu (from Gary Tsao)
+		RTMP_IO_WRITE32(pAd, (USHORT)TX_SW_CFG0, 0x00000400);
+
+		// Initialize RT3070 serial MAC registers which is different from RT2870 serial
+		RTUSBWriteMACRegister(pAd, TX_SW_CFG1, 0);
+		RTUSBWriteMACRegister(pAd, TX_SW_CFG2, 0);
+	}
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		for (Index = 0; Index < NUM_STA_MAC_REG_PARMS; Index++)
+		{
+			RTMP_IO_WRITE32(pAd, (USHORT)STAMACRegTable[Index].Register, STAMACRegTable[Index].Value);
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+#endif // RT2870 //
+
+	//
+	// Before program BBP, we need to wait BBP/RF get wake up.
+	//
+	Index = 0;
+	do
+	{
+		RTMP_IO_READ32(pAd, MAC_STATUS_CFG, &MacCsr12);
+
+		if ((MacCsr12 & 0x03) == 0)	// if BB.RF is stable
+			break;
+
+		DBGPRINT(RT_DEBUG_TRACE, ("Check MAC_STATUS_CFG  = Busy = %x\n", MacCsr12));
+		RTMPusecDelay(1000);
+	} while (Index++ < 100);
+
+    // The commands to firmware should be after these commands, these commands will init firmware
+	// PCI and USB are not the same because PCI driver needs to wait for PCI bus ready
+	RTMP_IO_WRITE32(pAd, H2M_BBP_AGENT, 0);	// initialize BBP R/W access agent
+	RTMP_IO_WRITE32(pAd, H2M_MAILBOX_CSR, 0);
+	RTMPusecDelay(1000);
+
+	// Read BBP register, make sure BBP is up and running before write new data
+	Index = 0;
+	do
+	{
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R0, &R0);
+		DBGPRINT(RT_DEBUG_TRACE, ("BBP version = %x\n", R0));
+	} while ((++Index < 20) && ((R0 == 0xff) || (R0 == 0x00)));
+	//ASSERT(Index < 20); //this will cause BSOD on Check-build driver
+
+	if ((R0 == 0xff) || (R0 == 0x00))
+		return NDIS_STATUS_FAILURE;
+
+	// Initialize BBP register to default value
+	for (Index = 0; Index < NUM_BBP_REG_PARMS; Index++)
+	{
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBPRegTable[Index].Register, BBPRegTable[Index].Value);
+	}
+
+	// for rt2860E and after, init BBP_R84 with 0x19. This is for extension channel overlapping IOT.
+	if ((pAd->MACVersion&0xffff) != 0x0101)
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R84, 0x19);
+
+#ifdef RT2870
+	//write RT3070 BBP wchich different with 2870 after write RT2870 BBP
+	if (IS_RT3070(pAd))
+	{
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, 0x0a);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R84, 0x99);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R105, 0x05);
+	}
+#endif // RT2870 //
+
+	if (pAd->MACVersion == 0x28600100)
+	{
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, 0x16);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R73, 0x12);
+    }
+
+	if (pAd->MACVersion >= RALINK_2880E_VERSION && pAd->MACVersion < RALINK_3070_VERSION) // 3*3
+	{
+		// enlarge MAX_LEN_CFG
+		UINT32 csr;
+		RTMP_IO_READ32(pAd, MAX_LEN_CFG, &csr);
+		csr &= 0xFFF;
+		csr |= 0x2000;
+		RTMP_IO_WRITE32(pAd, MAX_LEN_CFG, csr);
+	}
+
+#ifdef RT2870
+{
+	UCHAR	MAC_Value[]={0xff,0xff,0xff,0xff,0xff,0xff,0xff,0,0};
+
+	//Initialize WCID table
+	Value = 0xff;
+	for(Index =0 ;Index < 254;Index++)
+	{
+		RTUSBMultiWrite(pAd, (USHORT)(MAC_WCID_BASE + Index * 8), MAC_Value, 8);
+	}
+}
+#endif // RT2870 //
+
+	// Add radio off control
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		if (pAd->StaCfg.bRadio == FALSE)
+		{
+//			RTMP_IO_WRITE32(pAd, PWR_PIN_CFG, 0x00001818);
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF);
+			DBGPRINT(RT_DEBUG_TRACE, ("Set Radio Off\n"));
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	// Clear raw counters
+	RTMP_IO_READ32(pAd, RX_STA_CNT0, &Counter);
+	RTMP_IO_READ32(pAd, RX_STA_CNT1, &Counter);
+	RTMP_IO_READ32(pAd, RX_STA_CNT2, &Counter);
+	RTMP_IO_READ32(pAd, TX_STA_CNT0, &Counter);
+	RTMP_IO_READ32(pAd, TX_STA_CNT1, &Counter);
+	RTMP_IO_READ32(pAd, TX_STA_CNT2, &Counter);
+
+	// ASIC will keep garbage value after boot
+	// Clear all seared key table when initial
+	// This routine can be ignored in radio-ON/OFF operation.
+	if (bHardReset)
+	{
+		for (KeyIdx = 0; KeyIdx < 4; KeyIdx++)
+		{
+			RTMP_IO_WRITE32(pAd, SHARED_KEY_MODE_BASE + 4*KeyIdx, 0);
+		}
+
+		// Clear all pairwise key table when initial
+		for (KeyIdx = 0; KeyIdx < 256; KeyIdx++)
+		{
+			RTMP_IO_WRITE32(pAd, MAC_WCID_ATTRIBUTE_BASE + (KeyIdx * HW_WCID_ATTRI_SIZE), 1);
+		}
+	}
+
+	// assert HOST ready bit
+//  RTMP_IO_WRITE32(pAd, MAC_CSR1, 0x0); // 2004-09-14 asked by Mark
+//  RTMP_IO_WRITE32(pAd, MAC_CSR1, 0x4);
+
+	// It isn't necessary to clear this space when not hard reset.
+	if (bHardReset == TRUE)
+	{
+		// clear all on-chip BEACON frame space
+		for (apidx = 0; apidx < HW_BEACON_MAX_COUNT; apidx++)
+		{
+			for (i = 0; i < HW_BEACON_OFFSET>>2; i+=4)
+				RTMP_IO_WRITE32(pAd, pAd->BeaconOffset[apidx] + i, 0x00);
+		}
+	}
+#ifdef RT2870
+	AsicDisableSync(pAd);
+	// Clear raw counters
+	RTMP_IO_READ32(pAd, RX_STA_CNT0, &Counter);
+	RTMP_IO_READ32(pAd, RX_STA_CNT1, &Counter);
+	RTMP_IO_READ32(pAd, RX_STA_CNT2, &Counter);
+	RTMP_IO_READ32(pAd, TX_STA_CNT0, &Counter);
+	RTMP_IO_READ32(pAd, TX_STA_CNT1, &Counter);
+	RTMP_IO_READ32(pAd, TX_STA_CNT2, &Counter);
+	// Default PCI clock cycle per ms is different as default setting, which is based on PCI.
+	RTMP_IO_READ32(pAd, USB_CYC_CFG, &Counter);
+	Counter&=0xffffff00;
+	Counter|=0x000001e;
+	RTMP_IO_WRITE32(pAd, USB_CYC_CFG, Counter);
+#endif // RT2870 //
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// for rt2860E and after, init TXOP_CTRL_CFG with 0x583f. This is for extension channel overlapping IOT.
+		if ((pAd->MACVersion&0xffff) != 0x0101)
+			RTMP_IO_WRITE32(pAd, TXOP_CTRL_CFG, 0x583f);
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- NICInitializeAsic\n"));
+	return NDIS_STATUS_SUCCESS;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Reset NIC Asics
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+		Reset NIC to initial state AS IS system boot up time.
+
+	========================================================================
+*/
+VOID	NICIssueReset(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	UINT32	Value = 0;
+	DBGPRINT(RT_DEBUG_TRACE, ("--> NICIssueReset\n"));
+
+	// Abort Tx, prevent ASIC from writing to Host memory
+	//RTMP_IO_WRITE32(pAd, TX_CNTL_CSR, 0x001f0000);
+
+	// Disable Rx, register value supposed will remain after reset
+	RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+	Value &= (0xfffffff3);
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+	// Issue reset and clear from reset state
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x03); // 2004-09-17 change from 0x01
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x00);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- NICIssueReset\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Check ASIC registers and find any reason the system might hang
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	========================================================================
+*/
+BOOLEAN	NICCheckForHang(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	return (FALSE);
+}
+
+VOID NICUpdateFifoStaCounters(
+	IN PRTMP_ADAPTER pAd)
+{
+	TX_STA_FIFO_STRUC	StaFifo;
+	MAC_TABLE_ENTRY		*pEntry;
+	UCHAR				i = 0;
+	UCHAR			pid = 0, wcid = 0;
+	CHAR				reTry;
+	UCHAR				succMCS;
+
+#ifdef RALINK_ATE
+	/* Nothing to do in ATE mode */
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+		do
+		{
+			RTMP_IO_READ32(pAd, TX_STA_FIFO, &StaFifo.word);
+
+			if (StaFifo.field.bValid == 0)
+				break;
+
+			wcid = (UCHAR)StaFifo.field.wcid;
+
+
+		/* ignore NoACK and MGMT frame use 0xFF as WCID */
+			if ((StaFifo.field.TxAckRequired == 0) || (wcid >= MAX_LEN_OF_MAC_TABLE))
+			{
+				i++;
+				continue;
+			}
+
+			/* PID store Tx MCS Rate */
+			pid = (UCHAR)StaFifo.field.PidType;
+
+			pEntry = &pAd->MacTab.Content[wcid];
+
+			pEntry->DebugFIFOCount++;
+
+#ifdef DOT11_N_SUPPORT
+			if (StaFifo.field.TxBF) // 3*3
+				pEntry->TxBFCount++;
+#endif // DOT11_N_SUPPORT //
+
+#ifdef UAPSD_AP_SUPPORT
+			UAPSD_SP_AUE_Handle(pAd, pEntry, StaFifo.field.TxSuccess);
+#endif // UAPSD_AP_SUPPORT //
+
+			if (!StaFifo.field.TxSuccess)
+			{
+				pEntry->FIFOCount++;
+				pEntry->OneSecTxFailCount++;
+
+				if (pEntry->FIFOCount >= 1)
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("#"));
+#if 0
+					SendRefreshBAR(pAd, pEntry);
+					pEntry->NoBADataCountDown = 64;
+#else
+#ifdef DOT11_N_SUPPORT
+					pEntry->NoBADataCountDown = 64;
+#endif // DOT11_N_SUPPORT //
+
+					if(pEntry->PsMode == PWR_ACTIVE)
+					{
+#ifdef DOT11_N_SUPPORT
+						int tid;
+						for (tid=0; tid<NUM_OF_TID; tid++)
+						{
+							BAOriSessionTearDown(pAd, pEntry->Aid,  tid, FALSE, FALSE);
+						}
+#endif // DOT11_N_SUPPORT //
+
+						// Update the continuous transmission counter except PS mode
+						pEntry->ContinueTxFailCnt++;
+					}
+					else
+					{
+						// Clear the FIFOCount when sta in Power Save mode. Basically we assume
+						//     this tx error happened due to sta just go to sleep.
+						pEntry->FIFOCount = 0;
+						pEntry->ContinueTxFailCnt = 0;
+					}
+#endif
+					//pEntry->FIFOCount = 0;
+				}
+				//pEntry->bSendBAR = TRUE;
+			}
+			else
+			{
+#ifdef DOT11_N_SUPPORT
+				if ((pEntry->PsMode != PWR_SAVE) && (pEntry->NoBADataCountDown > 0))
+				{
+					pEntry->NoBADataCountDown--;
+					if (pEntry->NoBADataCountDown==0)
+					{
+						DBGPRINT(RT_DEBUG_TRACE, ("@\n"));
+					}
+				}
+#endif // DOT11_N_SUPPORT //
+				pEntry->FIFOCount = 0;
+				pEntry->OneSecTxNoRetryOkCount++;
+				// update NoDataIdleCount when sucessful send packet to STA.
+				pEntry->NoDataIdleCount = 0;
+				pEntry->ContinueTxFailCnt = 0;
+			}
+
+			succMCS = StaFifo.field.SuccessRate & 0x7F;
+
+			reTry = pid - succMCS;
+
+			if (StaFifo.field.TxSuccess)
+			{
+				pEntry->TXMCSExpected[pid]++;
+				if (pid == succMCS)
+				{
+					pEntry->TXMCSSuccessful[pid]++;
+				}
+				else
+				{
+					pEntry->TXMCSAutoFallBack[pid][succMCS]++;
+				}
+			}
+			else
+			{
+				pEntry->TXMCSFailed[pid]++;
+			}
+
+			if (reTry > 0)
+			{
+				if ((pid >= 12) && succMCS <=7)
+				{
+					reTry -= 4;
+				}
+				pEntry->OneSecTxRetryOkCount += reTry;
+			}
+
+			i++;
+			// ASIC store 16 stack
+		} while ( i < (2*TX_RING_SIZE) );
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Read statistical counters from hardware registers and record them
+		in software variables for later on query
+
+	Arguments:
+		pAd					Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	========================================================================
+*/
+VOID NICUpdateRawCounters(
+	IN PRTMP_ADAPTER pAd)
+{
+	UINT32	OldValue;
+	RX_STA_CNT0_STRUC	 RxStaCnt0;
+	RX_STA_CNT1_STRUC   RxStaCnt1;
+	RX_STA_CNT2_STRUC   RxStaCnt2;
+	TX_STA_CNT0_STRUC 	 TxStaCnt0;
+	TX_STA_CNT1_STRUC	 StaTx1;
+	TX_STA_CNT2_STRUC	 StaTx2;
+	TX_AGG_CNT_STRUC	TxAggCnt;
+	TX_AGG_CNT0_STRUC	TxAggCnt0;
+	TX_AGG_CNT1_STRUC	TxAggCnt1;
+	TX_AGG_CNT2_STRUC	TxAggCnt2;
+	TX_AGG_CNT3_STRUC	TxAggCnt3;
+	TX_AGG_CNT4_STRUC	TxAggCnt4;
+	TX_AGG_CNT5_STRUC	TxAggCnt5;
+	TX_AGG_CNT6_STRUC	TxAggCnt6;
+	TX_AGG_CNT7_STRUC	TxAggCnt7;
+
+
+	RTMP_IO_READ32(pAd, RX_STA_CNT0, &RxStaCnt0.word);
+	RTMP_IO_READ32(pAd, RX_STA_CNT2, &RxStaCnt2.word);
+
+	{
+		RTMP_IO_READ32(pAd, RX_STA_CNT1, &RxStaCnt1.word);
+	    // Update RX PLCP error counter
+	    pAd->PrivateInfo.PhyRxErrCnt += RxStaCnt1.field.PlcpErr;
+		// Update False CCA counter
+		pAd->RalinkCounters.OneSecFalseCCACnt += RxStaCnt1.field.FalseCca;
+	}
+
+	// Update FCS counters
+	OldValue= pAd->WlanCounters.FCSErrorCount.u.LowPart;
+	pAd->WlanCounters.FCSErrorCount.u.LowPart += (RxStaCnt0.field.CrcErr); // >> 7);
+	if (pAd->WlanCounters.FCSErrorCount.u.LowPart < OldValue)
+		pAd->WlanCounters.FCSErrorCount.u.HighPart++;
+
+	// Add FCS error count to private counters
+	pAd->RalinkCounters.OneSecRxFcsErrCnt += RxStaCnt0.field.CrcErr;
+	OldValue = pAd->RalinkCounters.RealFcsErrCount.u.LowPart;
+	pAd->RalinkCounters.RealFcsErrCount.u.LowPart += RxStaCnt0.field.CrcErr;
+	if (pAd->RalinkCounters.RealFcsErrCount.u.LowPart < OldValue)
+		pAd->RalinkCounters.RealFcsErrCount.u.HighPart++;
+
+	// Update Duplicate Rcv check
+	pAd->RalinkCounters.DuplicateRcv += RxStaCnt2.field.RxDupliCount;
+	pAd->WlanCounters.FrameDuplicateCount.u.LowPart += RxStaCnt2.field.RxDupliCount;
+	// Update RX Overflow counter
+	pAd->Counters8023.RxNoBuffer += (RxStaCnt2.field.RxFifoOverflowCount);
+
+	//pAd->RalinkCounters.RxCount = 0;
+#ifdef RT2870
+	if (pAd->RalinkCounters.RxCount != pAd->watchDogRxCnt)
+	{
+		pAd->watchDogRxCnt = pAd->RalinkCounters.RxCount;
+		pAd->watchDogRxOverFlowCnt = 0;
+	}
+	else
+	{
+		if (RxStaCnt2.field.RxFifoOverflowCount)
+			pAd->watchDogRxOverFlowCnt++;
+		else
+			pAd->watchDogRxOverFlowCnt = 0;
+	}
+#endif // RT2870 //
+
+
+	//if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_TX_RATE_SWITCH_ENABLED) ||
+	//	(OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_TX_RATE_SWITCH_ENABLED) && (pAd->MacTab.Size != 1)))
+	if (!pAd->bUpdateBcnCntDone)
+	{
+	// Update BEACON sent count
+	RTMP_IO_READ32(pAd, TX_STA_CNT0, &TxStaCnt0.word);
+	RTMP_IO_READ32(pAd, TX_STA_CNT1, &StaTx1.word);
+	RTMP_IO_READ32(pAd, TX_STA_CNT2, &StaTx2.word);
+	pAd->RalinkCounters.OneSecBeaconSentCnt += TxStaCnt0.field.TxBeaconCount;
+	pAd->RalinkCounters.OneSecTxRetryOkCount += StaTx1.field.TxRetransmit;
+	pAd->RalinkCounters.OneSecTxNoRetryOkCount += StaTx1.field.TxSuccess;
+	pAd->RalinkCounters.OneSecTxFailCount += TxStaCnt0.field.TxFailCount;
+	pAd->WlanCounters.TransmittedFragmentCount.u.LowPart += StaTx1.field.TxSuccess;
+	pAd->WlanCounters.RetryCount.u.LowPart += StaTx1.field.TxRetransmit;
+	pAd->WlanCounters.FailedCount.u.LowPart += TxStaCnt0.field.TxFailCount;
+	}
+
+#if 0
+	Retry = StaTx1.field.TxRetransmit;
+	Fail = TxStaCnt0.field.TxFailCount;
+	TxErrorRatio = 0;
+	OneSecTransmitCount = pAd->WlanCounters.TransmittedFragmentCount.u.LowPart- pAd->WlanCounters.LastTransmittedFragmentCount.u.LowPart;
+	if ((OneSecTransmitCount+Retry + Fail) > 0)
+		TxErrorRatio = (( Retry + Fail) *100) / (OneSecTransmitCount+Retry + Fail);
+
+	if ((OneSecTransmitCount+Retry + Fail) > 0)
+		TxErrorRatio = (( Retry + Fail) *100) / (OneSecTransmitCount+Retry + Fail);
+	DBGPRINT(RT_DEBUG_INFO, ("TX ERROR Rate = %ld %%, Retry = %ld, Fail = %ld, Total = %ld  \n",TxErrorRatio, Retry, Fail, (OneSecTransmitCount+Retry + Fail)));
+	pAd->WlanCounters.LastTransmittedFragmentCount.u.LowPart = pAd->WlanCounters.TransmittedFragmentCount.u.LowPart;
+#endif
+
+	//if (pAd->bStaFifoTest == TRUE)
+	{
+		RTMP_IO_READ32(pAd, TX_AGG_CNT, &TxAggCnt.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT0, &TxAggCnt0.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT1, &TxAggCnt1.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT2, &TxAggCnt2.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT3, &TxAggCnt3.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT4, &TxAggCnt4.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT5, &TxAggCnt5.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT6, &TxAggCnt6.word);
+		RTMP_IO_READ32(pAd, TX_AGG_CNT7, &TxAggCnt7.word);
+		pAd->RalinkCounters.TxAggCount += TxAggCnt.field.AggTxCount;
+		pAd->RalinkCounters.TxNonAggCount += TxAggCnt.field.NonAggTxCount;
+		pAd->RalinkCounters.TxAgg1MPDUCount += TxAggCnt0.field.AggSize1Count;
+		pAd->RalinkCounters.TxAgg2MPDUCount += TxAggCnt0.field.AggSize2Count;
+
+		pAd->RalinkCounters.TxAgg3MPDUCount += TxAggCnt1.field.AggSize3Count;
+		pAd->RalinkCounters.TxAgg4MPDUCount += TxAggCnt1.field.AggSize4Count;
+		pAd->RalinkCounters.TxAgg5MPDUCount += TxAggCnt2.field.AggSize5Count;
+		pAd->RalinkCounters.TxAgg6MPDUCount += TxAggCnt2.field.AggSize6Count;
+
+		pAd->RalinkCounters.TxAgg7MPDUCount += TxAggCnt3.field.AggSize7Count;
+		pAd->RalinkCounters.TxAgg8MPDUCount += TxAggCnt3.field.AggSize8Count;
+		pAd->RalinkCounters.TxAgg9MPDUCount += TxAggCnt4.field.AggSize9Count;
+		pAd->RalinkCounters.TxAgg10MPDUCount += TxAggCnt4.field.AggSize10Count;
+
+		pAd->RalinkCounters.TxAgg11MPDUCount += TxAggCnt5.field.AggSize11Count;
+		pAd->RalinkCounters.TxAgg12MPDUCount += TxAggCnt5.field.AggSize12Count;
+		pAd->RalinkCounters.TxAgg13MPDUCount += TxAggCnt6.field.AggSize13Count;
+		pAd->RalinkCounters.TxAgg14MPDUCount += TxAggCnt6.field.AggSize14Count;
+
+		pAd->RalinkCounters.TxAgg15MPDUCount += TxAggCnt7.field.AggSize15Count;
+		pAd->RalinkCounters.TxAgg16MPDUCount += TxAggCnt7.field.AggSize16Count;
+
+		// Calculate the transmitted A-MPDU count
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += TxAggCnt0.field.AggSize1Count;
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt0.field.AggSize2Count / 2);
+
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt1.field.AggSize3Count / 3);
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt1.field.AggSize4Count / 4);
+
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt2.field.AggSize5Count / 5);
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt2.field.AggSize6Count / 6);
+
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt3.field.AggSize7Count / 7);
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt3.field.AggSize8Count / 8);
+
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt4.field.AggSize9Count / 9);
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt4.field.AggSize10Count / 10);
+
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt5.field.AggSize11Count / 11);
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt5.field.AggSize12Count / 12);
+
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt6.field.AggSize13Count / 13);
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt6.field.AggSize14Count / 14);
+
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt7.field.AggSize15Count / 15);
+		pAd->RalinkCounters.TransmittedAMPDUCount.u.LowPart += (TxAggCnt7.field.AggSize16Count / 16);
+	}
+
+#ifdef DBG_DIAGNOSE
+	{
+		RtmpDiagStruct	*pDiag;
+		COUNTER_RALINK	*pRalinkCounters;
+		UCHAR			ArrayCurIdx, i;
+
+		pDiag = &pAd->DiagStruct;
+		pRalinkCounters = &pAd->RalinkCounters;
+		ArrayCurIdx = pDiag->ArrayCurIdx;
+
+		if (pDiag->inited == 0)
+		{
+			NdisZeroMemory(pDiag, sizeof(struct _RtmpDiagStrcut_));
+			pDiag->ArrayStartIdx = pDiag->ArrayCurIdx = 0;
+			pDiag->inited = 1;
+		}
+		else
+		{
+			// Tx
+			pDiag->TxFailCnt[ArrayCurIdx] = TxStaCnt0.field.TxFailCount;
+			pDiag->TxAggCnt[ArrayCurIdx] = TxAggCnt.field.AggTxCount;
+			pDiag->TxNonAggCnt[ArrayCurIdx] = TxAggCnt.field.NonAggTxCount;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][0] = TxAggCnt0.field.AggSize1Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][1] = TxAggCnt0.field.AggSize2Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][2] = TxAggCnt1.field.AggSize3Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][3] = TxAggCnt1.field.AggSize4Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][4] = TxAggCnt2.field.AggSize5Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][5] = TxAggCnt2.field.AggSize6Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][6] = TxAggCnt3.field.AggSize7Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][7] = TxAggCnt3.field.AggSize8Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][8] = TxAggCnt4.field.AggSize9Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][9] = TxAggCnt4.field.AggSize10Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][10] = TxAggCnt5.field.AggSize11Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][11] = TxAggCnt5.field.AggSize12Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][12] = TxAggCnt6.field.AggSize13Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][13] = TxAggCnt6.field.AggSize14Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][14] = TxAggCnt7.field.AggSize15Count;
+			pDiag->TxAMPDUCnt[ArrayCurIdx][15] = TxAggCnt7.field.AggSize16Count;
+
+			pDiag->RxCrcErrCnt[ArrayCurIdx] = RxStaCnt0.field.CrcErr;
+
+			INC_RING_INDEX(pDiag->ArrayCurIdx,  DIAGNOSE_TIME);
+			ArrayCurIdx = pDiag->ArrayCurIdx;
+			for (i =0; i < 9; i++)
+			{
+				pDiag->TxDescCnt[ArrayCurIdx][i]= 0;
+				pDiag->TxSWQueCnt[ArrayCurIdx][i] =0;
+				pDiag->TxMcsCnt[ArrayCurIdx][i] = 0;
+				pDiag->RxMcsCnt[ArrayCurIdx][i] = 0;
+			}
+			pDiag->TxDataCnt[ArrayCurIdx] = 0;
+			pDiag->TxFailCnt[ArrayCurIdx] = 0;
+			pDiag->RxDataCnt[ArrayCurIdx] = 0;
+			pDiag->RxCrcErrCnt[ArrayCurIdx]  = 0;
+//			for (i = 9; i < 16; i++)
+			for (i = 9; i < 24; i++) // 3*3
+			{
+				pDiag->TxDescCnt[ArrayCurIdx][i] = 0;
+				pDiag->TxMcsCnt[ArrayCurIdx][i] = 0;
+				pDiag->RxMcsCnt[ArrayCurIdx][i] = 0;
+}
+
+			if (pDiag->ArrayCurIdx == pDiag->ArrayStartIdx)
+				INC_RING_INDEX(pDiag->ArrayStartIdx,  DIAGNOSE_TIME);
+		}
+
+	}
+#endif // DBG_DIAGNOSE //
+
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Reset NIC from error
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+		Reset NIC from error state
+
+	========================================================================
+*/
+VOID	NICResetFromError(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	// Reset BBP (according to alex, reset ASIC will force reset BBP
+	// Therefore, skip the reset BBP
+	// RTMP_IO_WRITE32(pAd, MAC_CSR1, 0x2);
+
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x1);
+	// Remove ASIC from reset state
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x0);
+
+	NICInitializeAdapter(pAd, FALSE);
+	NICInitAsicFromEEPROM(pAd);
+
+	// Switch to current channel, since during reset process, the connection should remains on.
+	AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+	AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		erase 8051 firmware image in MAC ASIC
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	IRQL = PASSIVE_LEVEL
+
+	========================================================================
+*/
+VOID NICEraseFirmware(
+	IN PRTMP_ADAPTER pAd)
+{
+	ULONG i;
+
+	for(i=0; i<MAX_FIRMWARE_IMAGE_SIZE; i+=4)
+		RTMP_IO_WRITE32(pAd, FIRMWARE_IMAGE_BASE + i, 0);
+
+}/* End of NICEraseFirmware */
+
+/*
+	========================================================================
+
+	Routine Description:
+		Load 8051 firmware RT2561.BIN file into MAC ASIC
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		NDIS_STATUS_SUCCESS         firmware image load ok
+		NDIS_STATUS_FAILURE         image not found
+
+	IRQL = PASSIVE_LEVEL
+
+	========================================================================
+*/
+NDIS_STATUS NICLoadFirmware(
+	IN PRTMP_ADAPTER pAd)
+{
+#ifdef BIN_IN_FILE
+#define NICLF_DEFAULT_USE()	\
+	flg_default_firm_use = TRUE; \
+	printk("%s - Use default firmware!\n", __FUNCTION__);
+
+	NDIS_STATUS		Status = NDIS_STATUS_SUCCESS;
+	PUCHAR			src;
+	struct file		*srcf;
+	INT 			retval, orgfsuid, orgfsgid, i;
+   	mm_segment_t	orgfs;
+	PUCHAR			pFirmwareImage;
+	UINT			FileLength = 0;
+	UINT32			MacReg;
+	ULONG			Index;
+	ULONG			firm;
+	BOOLEAN			flg_default_firm_use = FALSE;
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("===> %s\n", __FUNCTION__));
+
+	/* init */
+	pFirmwareImage = NULL;
+	src = RTMP_FIRMWARE_FILE_NAME;
+
+	/* save uid and gid used for filesystem access.
+	   set user and group to 0 (root) */
+	orgfsuid = current->fsuid;
+	orgfsgid = current->fsgid;
+	current->fsuid = current->fsgid = 0;
+    orgfs = get_fs();
+    set_fs(KERNEL_DS);
+
+	pAd->FirmwareVersion = (FIRMWARE_MAJOR_VERSION << 8) + \
+						   FIRMWARE_MINOR_VERSION;
+
+
+	/* allocate firmware buffer */
+    pFirmwareImage = kmalloc(MAX_FIRMWARE_IMAGE_SIZE, MEM_ALLOC_FLAG);
+    if (pFirmwareImage == NULL)
+	{
+		/* allocate fail, use default firmware array in firmware.h */
+		printk("%s - Allocate memory fail!\n", __FUNCTION__);
+		NICLF_DEFAULT_USE();
+    }
+	else
+	{
+		/* allocate ok! zero the firmware buffer */
+		memset(pFirmwareImage, 0x00, MAX_FIRMWARE_IMAGE_SIZE);
+	} /* End of if */
+
+
+	/* if ok, read firmware file from *.bin file */
+	if (flg_default_firm_use == FALSE)
+	{
+		do
+		{
+			/* open the bin file */
+			srcf = filp_open(src, O_RDONLY, 0);
+
+			if (IS_ERR(srcf))
+			{
+				printk("%s - Error %ld opening %s\n",
+					   __FUNCTION__, -PTR_ERR(srcf), src);
+				NICLF_DEFAULT_USE();
+				break;
+			} /* End of if */
+
+			/* the object must have a read method */
+			if ((srcf->f_op == NULL) || (srcf->f_op->read == NULL))
+			{
+				printk("%s - %s does not have a write method\n", __FUNCTION__, src);
+				NICLF_DEFAULT_USE();
+				break;
+			} /* End of if */
+
+			/* read the firmware from the file *.bin */
+			FileLength = srcf->f_op->read(srcf,
+										  pFirmwareImage,
+										  MAX_FIRMWARE_IMAGE_SIZE,
+										  &srcf->f_pos);
+
+			if (FileLength != MAX_FIRMWARE_IMAGE_SIZE)
+			{
+				printk("%s: error file length (=%d) in RT2860AP.BIN\n",
+					   __FUNCTION__, FileLength);
+				NICLF_DEFAULT_USE();
+				break;
+			}
+			else
+			{
+				PUCHAR ptr = pFirmwareImage;
+				USHORT crc = 0xffff;
+
+
+				/* calculate firmware CRC */
+				for(i=0; i<(MAX_FIRMWARE_IMAGE_SIZE-2); i++, ptr++)
+					crc = ByteCRC16(BitReverse(*ptr), crc);
+				/* End of for */
+
+				if ((pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-2] != \
+								(UCHAR)BitReverse((UCHAR)(crc>>8))) ||
+					(pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-1] != \
+								(UCHAR)BitReverse((UCHAR)crc)))
+				{
+					/* CRC fail */
+					printk("%s: CRC = 0x%02x 0x%02x "
+						   "error, should be 0x%02x 0x%02x\n",
+						   __FUNCTION__,
+						   pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-2],
+						   pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-1],
+						   (UCHAR)(crc>>8), (UCHAR)(crc));
+					NICLF_DEFAULT_USE();
+					break;
+				}
+				else
+				{
+					/* firmware is ok */
+					pAd->FirmwareVersion = \
+						(pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-4] << 8) +
+						pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-3];
+
+					/* check if firmware version of the file is too old */
+					if ((pAd->FirmwareVersion) < \
+											((FIRMWARE_MAJOR_VERSION << 8) +
+									  	 	 FIRMWARE_MINOR_VERSION))
+					{
+						printk("%s: firmware version too old!\n", __FUNCTION__);
+						NICLF_DEFAULT_USE();
+						break;
+					} /* End of if */
+				} /* End of if */
+
+				DBGPRINT(RT_DEBUG_TRACE,
+						 ("NICLoadFirmware: CRC ok, ver=%d.%d\n",
+						  pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-4],
+						  pFirmwareImage[MAX_FIRMWARE_IMAGE_SIZE-3]));
+			} /* End of if (FileLength == MAX_FIRMWARE_IMAGE_SIZE) */
+			break;
+		} while(TRUE);
+
+		/* close firmware file */
+		if (IS_ERR(srcf))
+			;
+		else
+		{
+			retval = filp_close(srcf, NULL);
+			if (retval)
+			{
+				DBGPRINT(RT_DEBUG_ERROR,
+						 ("--> Error %d closing %s\n", -retval, src));
+			} /* End of if */
+		} /* End of if */
+	} /* End of if */
+
+
+	/* write firmware to ASIC */
+	if (flg_default_firm_use == TRUE)
+	{
+		/* use default fimeware, free allocated buffer */
+		if (pFirmwareImage != NULL)
+			kfree(pFirmwareImage);
+		/* End of if */
+
+		/* use default *.bin array */
+		pFirmwareImage = FirmwareImage;
+		FileLength = sizeof(FirmwareImage);
+	} /* End of if */
+
+	/* enable Host program ram write selection */
+	RTMP_IO_WRITE32(pAd, PBF_SYS_CTRL, 0x10000);
+
+	for(i=0; i<FileLength; i+=4)
+	{
+		firm = pFirmwareImage[i] +
+			   (pFirmwareImage[i+3] << 24) +
+			   (pFirmwareImage[i+2] << 16) +
+			   (pFirmwareImage[i+1] << 8);
+
+		RTMP_IO_WRITE32(pAd, FIRMWARE_IMAGE_BASE + i, firm);
+	} /* End of for */
+
+	RTMP_IO_WRITE32(pAd, PBF_SYS_CTRL, 0x00000);
+	RTMP_IO_WRITE32(pAd, PBF_SYS_CTRL, 0x00001);
+
+	/* initialize BBP R/W access agent */
+	RTMP_IO_WRITE32(pAd, H2M_BBP_AGENT, 0);
+	RTMP_IO_WRITE32(pAd, H2M_MAILBOX_CSR, 0);
+
+	if (flg_default_firm_use == FALSE)
+	{
+		/* use file firmware, free allocated buffer */
+		if (pFirmwareImage != NULL)
+			kfree(pFirmwareImage);
+		/* End of if */
+	} /* End of if */
+
+	set_fs(orgfs);
+	current->fsuid = orgfsuid;
+	current->fsgid = orgfsgid;
+#else
+
+	NDIS_STATUS		Status = NDIS_STATUS_SUCCESS;
+	PUCHAR			pFirmwareImage;
+	ULONG			FileLength, Index;
+	//ULONG			firm;
+	UINT32			MacReg = 0;
+#ifdef RT2870
+	UINT32			Version = (pAd->MACVersion >> 16);
+#endif // RT2870 //
+
+	pFirmwareImage = FirmwareImage;
+	FileLength = sizeof(FirmwareImage);
+#ifdef RT2870
+	// New 8k byte firmware size for RT3071/RT3072
+	//printk("Usb Chip\n");
+	if (FIRMWAREIMAGE_LENGTH == FIRMWAREIMAGE_MAX_LENGTH)
+	//The firmware image consists of two parts. One is the origianl and the other is the new.
+	//Use Second Part
+	{
+		if ((Version != 0x2860) && (Version != 0x2872) && (Version != 0x3070))
+		{	// Use Firmware V2.
+			//printk("KH:Use New Version,part2\n");
+			pFirmwareImage = (PUCHAR)&FirmwareImage[FIRMWAREIMAGEV1_LENGTH];
+			FileLength = FIRMWAREIMAGEV2_LENGTH;
+		}
+		else
+		{
+			//printk("KH:Use New Version,part1\n");
+			pFirmwareImage = FirmwareImage;
+			FileLength = FIRMWAREIMAGEV1_LENGTH;
+		}
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("KH: bin file should be 8KB.\n"));
+		Status = NDIS_STATUS_FAILURE;
+	}
+
+#endif // RT2870 //
+
+#if 0
+	/* enable Host program ram write selection */
+	RT28XX_FIRMUD_INIT(pAd);
+
+	for(i=0; i<FileLength; i+=4)
+	{
+		firm = pFirmwareImage[i] +
+			   (pFirmwareImage[i+3] << 24) +
+			   (pFirmwareImage[i+2] << 16) +
+			   (pFirmwareImage[i+1] << 8);
+
+		RTMP_IO_WRITE32(pAd, FIRMWARE_IMAGE_BASE + i, firm);
+	} /* End of for */
+
+	RT28XX_FIRMUD_END(pAd);
+#else
+	RT28XX_WRITE_FIRMWARE(pAd, pFirmwareImage, FileLength);
+#endif
+
+#endif
+
+	/* check if MCU is ready */
+	Index = 0;
+	do
+	{
+		RTMP_IO_READ32(pAd, PBF_SYS_CTRL, &MacReg);
+
+		if (MacReg & 0x80)
+			break;
+
+		RTMPusecDelay(1000);
+	} while (Index++ < 1000);
+
+    if (Index >= 1000)
+	{
+		Status = NDIS_STATUS_FAILURE;
+		DBGPRINT(RT_DEBUG_ERROR, ("NICLoadFirmware: MCU is not ready\n\n\n"));
+	} /* End of if */
+
+#if 0
+    DBGPRINT(RT_DEBUG_TRACE,
+			 ("<=== %s (src=%s, status=%d)\n", __FUNCTION__, src, Status));
+#else
+    DBGPRINT(RT_DEBUG_TRACE,
+			 ("<=== %s (status=%d)\n", __FUNCTION__, Status));
+#endif
+    return Status;
+} /* End of NICLoadFirmware */
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Load Tx rate switching parameters
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		NDIS_STATUS_SUCCESS         firmware image load ok
+		NDIS_STATUS_FAILURE         image not found
+
+	IRQL = PASSIVE_LEVEL
+
+	Rate Table Format:
+		1. (B0: Valid Item number) (B1:Initial item from zero)
+		2. Item Number(Dec)      Mode(Hex)     Current MCS(Dec)    TrainUp(Dec)    TrainDown(Dec)
+
+	========================================================================
+*/
+NDIS_STATUS NICLoadRateSwitchingParams(
+	IN PRTMP_ADAPTER pAd)
+{
+#if 0
+	NDIS_STATUS Status;
+
+	NDIS_HANDLE FileHandle;
+	UINT FileLength = 0, i, j;
+	PUCHAR pFirmwareImage;
+	NDIS_STRING FileName;
+	NDIS_PHYSICAL_ADDRESS HighestAcceptableMax = NDIS_PHYSICAL_ADDRESS_CONST(-1, -1);
+
+	DBGPRINT(RT_DEBUG_TRACE,("===> NICLoadRateSwitchingParams \n"));
+	pAd->CommonCfg.TxRateTableSize = 0;
+
+	if ((pAd->DeviceID == NIC2860_PCI_DEVICE_ID) || (pAd->DeviceID == NIC2860_PCIe_DEVICE_ID))
+	{
+		NdisInitializeString(&FileName,"rate.bin");
+		DBGPRINT(RT_DEBUG_TRACE, ("NICLoadRateSwitchingParams: load file - rate.bin for tx rate switch \n"));
+	}
+	else
+	{
+		DBGPRINT_ERR(("NICLoadRateSwitchingParams: wrong DeviceID = 0x%04x, can't find Tx rate switch parameters file\n", pAd->DeviceID));
+		return NDIS_STATUS_SUCCESS;
+	}
+	NdisOpenFile(&Status, &FileHandle, &FileLength, &FileName, HighestAcceptableMax);
+	NdisFreeString(FileName);
+
+	if (Status != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("NICLoadRateSwitchingParams: NdisOpenFile() failed, used RateSwitchTable instead\n"));
+		return NDIS_STATUS_SUCCESS;
+	}
+
+	if ((FileLength == 0) || (FileLength > (MAX_STEP_OF_TX_RATE_SWITCH+1)*16))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("NICLoadRateSwitchingParams: file size is not reasonable, used RateSwitchTable instead\n"));
+
+		NdisCloseFile(FileHandle);
+		return NDIS_STATUS_SUCCESS;
+	}
+	else
+	{
+		//
+		// NDIS_STATUS_SUCCESS means
+		// The handle at FileHandle is valid for a subsequent call to NdisMapFile.
+		//
+		NdisMapFile(&Status, &pFirmwareImage, FileHandle);
+		DBGPRINT(RT_DEBUG_TRACE, ("NdisMapFile FileLength=%d\n", FileLength));
+	}
+
+	for (i=0, j=0; i<FileLength; i++)
+	{
+		if ((i%16) <= 4)	// trim reserved field
+		{
+			if (i%16 == 1)	// deal with DEC and HEX, only row0 is Hex, others are Dec
+			{
+				RateSwitchTable[j] = *(pFirmwareImage + i);
+			}
+			else
+			{
+				RateSwitchTable[j] = (*(pFirmwareImage + i)>>4) * 10 + (*(pFirmwareImage + i) & 0x0F);
+			}
+
+			j++;
+		}
+	}
+
+	pAd->CommonCfg.TxRateTableSize = RateSwitchTable[0];		// backup table size
+
+	if (Status == NDIS_STATUS_SUCCESS)
+	{
+		NdisUnmapFile(FileHandle);
+		NdisCloseFile(FileHandle);
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("<=== NICLoadRateSwitchingParams(Valid TxRateTable item number=%d)\n", pAd->CommonCfg.TxRateTableSize));
+#endif
+	return NDIS_STATUS_SUCCESS;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		if  pSrc1 all zero with length Length, return 0.
+		If not all zero, return 1
+
+	Arguments:
+		pSrc1
+
+	Return Value:
+		1:			not all zero
+		0:			all zero
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+ULONG	RTMPNotAllZero(
+	IN	PVOID	pSrc1,
+	IN	ULONG	Length)
+{
+	PUCHAR	pMem1;
+	ULONG	Index = 0;
+
+	pMem1 = (PUCHAR) pSrc1;
+
+	for (Index = 0; Index < Length; Index++)
+	{
+		if (pMem1[Index] != 0x0)
+		{
+			break;
+		}
+	}
+
+	if (Index == Length)
+	{
+		return (0);
+	}
+	else
+	{
+		return (1);
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Compare two memory block
+
+	Arguments:
+		pSrc1		Pointer to first memory address
+		pSrc2		Pointer to second memory address
+
+	Return Value:
+		0:			memory is equal
+		1:			pSrc1 memory is larger
+		2:			pSrc2 memory is larger
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+ULONG	RTMPCompareMemory(
+	IN	PVOID	pSrc1,
+	IN	PVOID	pSrc2,
+	IN	ULONG	Length)
+{
+	PUCHAR	pMem1;
+	PUCHAR	pMem2;
+	ULONG	Index = 0;
+
+	pMem1 = (PUCHAR) pSrc1;
+	pMem2 = (PUCHAR) pSrc2;
+
+	for (Index = 0; Index < Length; Index++)
+	{
+		if (pMem1[Index] > pMem2[Index])
+			return (1);
+		else if (pMem1[Index] < pMem2[Index])
+			return (2);
+	}
+
+	// Equal
+	return (0);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Zero out memory block
+
+	Arguments:
+		pSrc1		Pointer to memory address
+		Length		Size
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPZeroMemory(
+	IN	PVOID	pSrc,
+	IN	ULONG	Length)
+{
+	PUCHAR	pMem;
+	ULONG	Index = 0;
+
+	pMem = (PUCHAR) pSrc;
+
+	for (Index = 0; Index < Length; Index++)
+	{
+		pMem[Index] = 0x00;
+	}
+}
+
+VOID	RTMPFillMemory(
+	IN	PVOID	pSrc,
+	IN	ULONG	Length,
+	IN	UCHAR	Fill)
+{
+	PUCHAR	pMem;
+	ULONG	Index = 0;
+
+	pMem = (PUCHAR) pSrc;
+
+	for (Index = 0; Index < Length; Index++)
+	{
+		pMem[Index] = Fill;
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Copy data from memory block 1 to memory block 2
+
+	Arguments:
+		pDest		Pointer to destination memory address
+		pSrc		Pointer to source memory address
+		Length		Copy size
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPMoveMemory(
+	OUT	PVOID	pDest,
+	IN	PVOID	pSrc,
+	IN	ULONG	Length)
+{
+	PUCHAR	pMem1;
+	PUCHAR	pMem2;
+	UINT	Index;
+
+	ASSERT((Length==0) || (pDest && pSrc));
+
+	pMem1 = (PUCHAR) pDest;
+	pMem2 = (PUCHAR) pSrc;
+
+	for (Index = 0; Index < Length; Index++)
+	{
+		pMem1[Index] = pMem2[Index];
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Initialize port configuration structure
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	UserCfgInit(
+	IN	PRTMP_ADAPTER pAd)
+{
+//	EDCA_PARM DefaultEdcaParm;
+    UINT key_index, bss_index;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--> UserCfgInit\n"));
+
+	//
+	//  part I. intialize common configuration
+	//
+#ifdef RT2870
+	pAd->BulkOutReq = 0;
+
+	pAd->BulkOutComplete = 0;
+	pAd->BulkOutCompleteOther = 0;
+	pAd->BulkOutCompleteCancel = 0;
+	pAd->BulkInReq = 0;
+	pAd->BulkInComplete = 0;
+	pAd->BulkInCompleteFail = 0;
+
+	//pAd->QuickTimerP = 100;
+	//pAd->TurnAggrBulkInCount = 0;
+	pAd->bUsbTxBulkAggre = 0;
+
+	// init as unsed value to ensure driver will set to MCU once.
+	pAd->LedIndicatorStregth = 0xFF;
+
+	pAd->CommonCfg.MaxPktOneTxBulk = 2;
+	pAd->CommonCfg.TxBulkFactor = 1;
+	pAd->CommonCfg.RxBulkFactor =1;
+
+	pAd->CommonCfg.TxPower = 100; //mW
+
+	NdisZeroMemory(&pAd->CommonCfg.IOTestParm, sizeof(pAd->CommonCfg.IOTestParm));
+#endif // RT2870 //
+
+	for(key_index=0; key_index<SHARE_KEY_NUM; key_index++)
+	{
+		for(bss_index = 0; bss_index < MAX_MBSSID_NUM; bss_index++)
+		{
+			pAd->SharedKey[bss_index][key_index].KeyLen = 0;
+			pAd->SharedKey[bss_index][key_index].CipherAlg = CIPHER_NONE;
+		}
+	}
+
+	pAd->Antenna.word = 0;
+	pAd->CommonCfg.BBPCurrentBW = BW_20;
+
+	pAd->LedCntl.word = 0;
+
+	pAd->bAutoTxAgcA = FALSE;			// Default is OFF
+	pAd->bAutoTxAgcG = FALSE;			// Default is OFF
+	pAd->RfIcType = RFIC_2820;
+
+	// Init timer for reset complete event
+	pAd->CommonCfg.CentralChannel = 1;
+	pAd->bForcePrintTX = FALSE;
+	pAd->bForcePrintRX = FALSE;
+	pAd->bStaFifoTest = FALSE;
+	pAd->bProtectionTest = FALSE;
+	pAd->bHCCATest = FALSE;
+	pAd->bGenOneHCCA = FALSE;
+	pAd->CommonCfg.Dsifs = 10;      // in units of usec
+	pAd->CommonCfg.TxPower = 100; //mW
+	pAd->CommonCfg.TxPowerPercentage = 0xffffffff; // AUTO
+	pAd->CommonCfg.TxPowerDefault = 0xffffffff; // AUTO
+	pAd->CommonCfg.TxPreamble = Rt802_11PreambleAuto; // use Long preamble on TX by defaut
+	pAd->CommonCfg.bUseZeroToDisableFragment = FALSE;
+	pAd->CommonCfg.RtsThreshold = 2347;
+	pAd->CommonCfg.FragmentThreshold = 2346;
+	pAd->CommonCfg.UseBGProtection = 0;    // 0: AUTO
+	pAd->CommonCfg.bEnableTxBurst = TRUE; //0;
+	pAd->CommonCfg.PhyMode = 0xff;     // unknown
+	pAd->CommonCfg.BandState = UNKNOWN_BAND;
+	pAd->CommonCfg.RadarDetect.CSPeriod = 10;
+	pAd->CommonCfg.RadarDetect.CSCount = 0;
+	pAd->CommonCfg.RadarDetect.RDMode = RD_NORMAL_MODE;
+	pAd->CommonCfg.RadarDetect.ChMovingTime = 65;
+	pAd->CommonCfg.RadarDetect.LongPulseRadarTh = 3;
+	pAd->CommonCfg.bAPSDCapable = FALSE;
+	pAd->CommonCfg.bNeedSendTriggerFrame = FALSE;
+	pAd->CommonCfg.TriggerTimerCount = 0;
+	pAd->CommonCfg.bAPSDForcePowerSave = FALSE;
+	pAd->CommonCfg.bCountryFlag = FALSE;
+	pAd->CommonCfg.TxStream = 0;
+	pAd->CommonCfg.RxStream = 0;
+
+	NdisZeroMemory(&pAd->BeaconTxWI, sizeof(pAd->BeaconTxWI));
+
+#ifdef DOT11_N_SUPPORT
+	NdisZeroMemory(&pAd->CommonCfg.HtCapability, sizeof(pAd->CommonCfg.HtCapability));
+	pAd->HTCEnable = FALSE;
+	pAd->bBroadComHT = FALSE;
+	pAd->CommonCfg.bRdg = FALSE;
+
+#ifdef DOT11N_DRAFT3
+	pAd->CommonCfg.Dot11OBssScanPassiveDwell = dot11OBSSScanPassiveDwell;	// Unit : TU. 5~1000
+	pAd->CommonCfg.Dot11OBssScanActiveDwell = dot11OBSSScanActiveDwell;	// Unit : TU. 10~1000
+	pAd->CommonCfg.Dot11BssWidthTriggerScanInt = dot11BSSWidthTriggerScanInterval;	// Unit : Second
+	pAd->CommonCfg.Dot11OBssScanPassiveTotalPerChannel = dot11OBSSScanPassiveTotalPerChannel;	// Unit : TU. 200~10000
+	pAd->CommonCfg.Dot11OBssScanActiveTotalPerChannel = dot11OBSSScanActiveTotalPerChannel;	// Unit : TU. 20~10000
+	pAd->CommonCfg.Dot11BssWidthChanTranDelayFactor = dot11BSSWidthChannelTransactionDelayFactor;
+	pAd->CommonCfg.Dot11OBssScanActivityThre = dot11BSSScanActivityThreshold;	// Unit : percentage
+	pAd->CommonCfg.Dot11BssWidthChanTranDelay = (pAd->CommonCfg.Dot11BssWidthTriggerScanInt * pAd->CommonCfg.Dot11BssWidthChanTranDelayFactor);
+#endif  // DOT11N_DRAFT3 //
+
+	NdisZeroMemory(&pAd->CommonCfg.AddHTInfo, sizeof(pAd->CommonCfg.AddHTInfo));
+	pAd->CommonCfg.BACapability.field.MMPSmode = MMPS_ENABLE;
+	pAd->CommonCfg.BACapability.field.MpduDensity = 0;
+	pAd->CommonCfg.BACapability.field.Policy = IMMED_BA;
+	pAd->CommonCfg.BACapability.field.RxBAWinLimit = 64; //32;
+	pAd->CommonCfg.BACapability.field.TxBAWinLimit = 64; //32;
+	DBGPRINT(RT_DEBUG_TRACE, ("--> UserCfgInit. BACapability = 0x%x\n", pAd->CommonCfg.BACapability.word));
+
+	pAd->CommonCfg.BACapability.field.AutoBA = FALSE;
+	BATableInit(pAd, &pAd->BATable);
+
+	pAd->CommonCfg.bExtChannelSwitchAnnouncement = 1;
+	pAd->CommonCfg.bHTProtect = 1;
+	pAd->CommonCfg.bMIMOPSEnable = TRUE;
+	pAd->CommonCfg.bBADecline = FALSE;
+	pAd->CommonCfg.bDisableReordering = FALSE;
+
+	pAd->CommonCfg.TxBASize = 7;
+
+	pAd->CommonCfg.REGBACapability.word = pAd->CommonCfg.BACapability.word;
+#endif // DOT11_N_SUPPORT //
+
+	//pAd->CommonCfg.HTPhyMode.field.BW = BW_20;
+	//pAd->CommonCfg.HTPhyMode.field.MCS = MCS_AUTO;
+	//pAd->CommonCfg.HTPhyMode.field.ShortGI = GI_800;
+	//pAd->CommonCfg.HTPhyMode.field.STBC = STBC_NONE;
+	pAd->CommonCfg.TxRate = RATE_6;
+
+	pAd->CommonCfg.MlmeTransmit.field.MCS = MCS_RATE_6;
+	pAd->CommonCfg.MlmeTransmit.field.BW = BW_20;
+	pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_OFDM;
+
+	pAd->CommonCfg.BeaconPeriod = 100;     // in mSec
+
+	//
+	// part II. intialize STA specific configuration
+	//
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		RX_FILTER_SET_FLAG(pAd, fRX_FILTER_ACCEPT_DIRECT);
+		RX_FILTER_CLEAR_FLAG(pAd, fRX_FILTER_ACCEPT_MULTICAST);
+		RX_FILTER_SET_FLAG(pAd, fRX_FILTER_ACCEPT_BROADCAST);
+		RX_FILTER_SET_FLAG(pAd, fRX_FILTER_ACCEPT_ALL_MULTICAST);
+
+		pAd->StaCfg.Psm = PWR_ACTIVE;
+
+		pAd->StaCfg.OrigWepStatus = Ndis802_11EncryptionDisabled;
+		pAd->StaCfg.PairCipher = Ndis802_11EncryptionDisabled;
+		pAd->StaCfg.GroupCipher = Ndis802_11EncryptionDisabled;
+		pAd->StaCfg.bMixCipher = FALSE;
+		pAd->StaCfg.DefaultKeyId = 0;
+
+		// 802.1x port control
+		pAd->StaCfg.PrivacyFilter = Ndis802_11PrivFilter8021xWEP;
+		pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+		pAd->StaCfg.LastMicErrorTime = 0;
+		pAd->StaCfg.MicErrCnt        = 0;
+		pAd->StaCfg.bBlockAssoc      = FALSE;
+		pAd->StaCfg.WpaState         = SS_NOTUSE;
+
+		pAd->CommonCfg.NdisRadioStateOff = FALSE;		// New to support microsoft disable radio with OID command
+
+		pAd->StaCfg.RssiTrigger = 0;
+		NdisZeroMemory(&pAd->StaCfg.RssiSample, sizeof(RSSI_SAMPLE));
+		pAd->StaCfg.RssiTriggerMode = RSSI_TRIGGERED_UPON_BELOW_THRESHOLD;
+		pAd->StaCfg.AtimWin = 0;
+		pAd->StaCfg.DefaultListenCount = 3;//default listen count;
+		pAd->StaCfg.BssType = BSS_INFRA;  // BSS_INFRA or BSS_ADHOC or BSS_MONITOR
+		pAd->StaCfg.bScanReqIsFromWebUI = FALSE;
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_DOZE);
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_WAKEUP_NOW);
+
+		pAd->StaCfg.bAutoTxRateSwitch = TRUE;
+		pAd->StaCfg.DesiredTransmitSetting.field.MCS = MCS_AUTO;
+	}
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+	pAd->StaCfg.IEEE80211dClientMode = Rt802_11_D_None;
+#endif // EXT_BUILD_CHANNEL_LIST //
+#endif // CONFIG_STA_SUPPORT //
+
+	// global variables mXXXX used in MAC protocol state machines
+	OPSTATUS_SET_FLAG(pAd, fOP_STATUS_RECEIVE_DTIM);
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_ADHOC_ON);
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_INFRA_ON);
+
+	// PHY specification
+	pAd->CommonCfg.PhyMode = PHY_11BG_MIXED;		// default PHY mode
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED);  // CCK use LONG preamble
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// user desired power mode
+		pAd->StaCfg.WindowsPowerMode = Ndis802_11PowerModeCAM;
+		pAd->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeCAM;
+		pAd->StaCfg.bWindowsACCAMEnable = FALSE;
+
+#ifdef LEAP_SUPPORT
+		// CCX v1.0 releated init value
+		RTMPInitTimer(pAd, &pAd->StaCfg.LeapAuthTimer, GET_TIMER_FUNCTION(LeapAuthTimeout), pAd, FALSE);
+		pAd->StaCfg.LeapAuthMode = CISCO_AuthModeLEAPNone;
+		pAd->StaCfg.bCkipOn = FALSE;
+#endif // LEAP_SUPPORT //
+
+		RTMPInitTimer(pAd, &pAd->StaCfg.StaQuickResponeForRateUpTimer, GET_TIMER_FUNCTION(StaQuickResponeForRateUpExec), pAd, FALSE);
+		pAd->StaCfg.StaQuickResponeForRateUpTimerRunning = FALSE;
+
+		// Patch for Ndtest
+		pAd->StaCfg.ScanCnt = 0;
+
+		// CCX 2.0 control flag init
+		pAd->StaCfg.CCXEnable = FALSE;
+		pAd->StaCfg.CCXReqType = MSRN_TYPE_UNUSED;
+		pAd->StaCfg.CCXQosECWMin	= 4;
+		pAd->StaCfg.CCXQosECWMax	= 10;
+
+		pAd->StaCfg.bHwRadio  = TRUE; // Default Hardware Radio status is On
+		pAd->StaCfg.bSwRadio  = TRUE; // Default Software Radio status is On
+		pAd->StaCfg.bRadio    = TRUE; // bHwRadio && bSwRadio
+		pAd->StaCfg.bHardwareRadio = FALSE;		// Default is OFF
+		pAd->StaCfg.bShowHiddenSSID = FALSE;		// Default no show
+
+		// Nitro mode control
+		pAd->StaCfg.bAutoReconnect = TRUE;
+
+		// Save the init time as last scan time, the system should do scan after 2 seconds.
+		// This patch is for driver wake up from standby mode, system will do scan right away.
+		pAd->StaCfg.LastScanTime = 0;
+		NdisZeroMemory(pAd->nickname, IW_ESSID_MAX_SIZE+1);
+		sprintf(pAd->nickname, "%s", STA_NIC_DEVICE_NAME);
+		RTMPInitTimer(pAd, &pAd->StaCfg.WpaDisassocAndBlockAssocTimer, GET_TIMER_FUNCTION(WpaDisassocApAndBlockAssoc), pAd, FALSE);
+#ifdef WPA_SUPPLICANT_SUPPORT
+		pAd->StaCfg.IEEE8021X = FALSE;
+		pAd->StaCfg.IEEE8021x_required_keys = FALSE;
+		pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_DISABLE;
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+		pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_ENABLE;
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	// Default for extra information is not valid
+	pAd->ExtraInfo = EXTRA_INFO_CLEAR;
+
+	// Default Config change flag
+	pAd->bConfigChanged = FALSE;
+
+	//
+	// part III. AP configurations
+	//
+
+
+	//
+	// part IV. others
+	//
+	// dynamic BBP R66:sensibity tuning to overcome background noise
+	pAd->BbpTuning.bEnable                = TRUE;
+	pAd->BbpTuning.FalseCcaLowerThreshold = 100;
+	pAd->BbpTuning.FalseCcaUpperThreshold = 512;
+	pAd->BbpTuning.R66Delta               = 4;
+	pAd->Mlme.bEnableAutoAntennaCheck = TRUE;
+
+	//
+	// Also initial R66CurrentValue, RTUSBResumeMsduTransmission might use this value.
+	// if not initial this value, the default value will be 0.
+	//
+	pAd->BbpTuning.R66CurrentValue = 0x38;
+
+	pAd->Bbp94 = BBPR94_DEFAULT;
+	pAd->BbpForCCK = FALSE;
+
+	// Default is FALSE for test bit 1
+	//pAd->bTest1 = FALSE;
+
+	// initialize MAC table and allocate spin lock
+	NdisZeroMemory(&pAd->MacTab, sizeof(MAC_TABLE));
+	InitializeQueueHeader(&pAd->MacTab.McastPsQueue);
+	NdisAllocateSpinLock(&pAd->MacTabLock);
+
+	//RTMPInitTimer(pAd, &pAd->RECBATimer, RECBATimerTimeout, pAd, TRUE);
+	//RTMPSetTimer(&pAd->RECBATimer, REORDER_EXEC_INTV);
+
+#ifdef RALINK_ATE
+	NdisZeroMemory(&pAd->ate, sizeof(ATE_INFO));
+	pAd->ate.Mode = ATE_STOP;
+	pAd->ate.TxCount = 200;/* to exceed TX_RING_SIZE ... */
+	pAd->ate.TxLength = 1024;
+	pAd->ate.TxWI.ShortGI = 0;// LONG GI : 800 ns
+	pAd->ate.TxWI.PHYMODE = MODE_CCK;
+	pAd->ate.TxWI.MCS = 3;
+	pAd->ate.TxWI.BW = BW_20;
+	pAd->ate.Channel = 1;
+	pAd->ate.QID = QID_AC_BE;
+	pAd->ate.Addr1[0] = 0x00;
+	pAd->ate.Addr1[1] = 0x11;
+	pAd->ate.Addr1[2] = 0x22;
+	pAd->ate.Addr1[3] = 0xAA;
+	pAd->ate.Addr1[4] = 0xBB;
+	pAd->ate.Addr1[5] = 0xCC;
+	NdisMoveMemory(pAd->ate.Addr2, pAd->ate.Addr1, ETH_LENGTH_OF_ADDRESS);
+	NdisMoveMemory(pAd->ate.Addr3, pAd->ate.Addr1, ETH_LENGTH_OF_ADDRESS);
+	pAd->ate.bRxFer = 0;
+	pAd->ate.bQATxStart = FALSE;
+	pAd->ate.bQARxStart = FALSE;
+#ifdef RALINK_28xx_QA
+	//pAd->ate.Repeat = 0;
+	pAd->ate.TxStatus = 0;
+	pAd->ate.AtePid = THREAD_PID_INIT_VALUE;
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+
+
+	pAd->CommonCfg.bWiFiTest = FALSE;
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<-- UserCfgInit\n"));
+}
+
+// IRQL = PASSIVE_LEVEL
+UCHAR BtoH(char ch)
+{
+	if (ch >= '0' && ch <= '9') return (ch - '0');        // Handle numerals
+	if (ch >= 'A' && ch <= 'F') return (ch - 'A' + 0xA);  // Handle capitol hex digits
+	if (ch >= 'a' && ch <= 'f') return (ch - 'a' + 0xA);  // Handle small hex digits
+	return(255);
+}
+
+//
+//  FUNCTION: AtoH(char *, UCHAR *, int)
+//
+//  PURPOSE:  Converts ascii string to network order hex
+//
+//  PARAMETERS:
+//    src    - pointer to input ascii string
+//    dest   - pointer to output hex
+//    destlen - size of dest
+//
+//  COMMENTS:
+//
+//    2 ascii bytes make a hex byte so must put 1st ascii byte of pair
+//    into upper nibble and 2nd ascii byte of pair into lower nibble.
+//
+// IRQL = PASSIVE_LEVEL
+
+void AtoH(char * src, UCHAR * dest, int destlen)
+{
+	char * srcptr;
+	PUCHAR destTemp;
+
+	srcptr = src;
+	destTemp = (PUCHAR) dest;
+
+	while(destlen--)
+	{
+		*destTemp = BtoH(*srcptr++) << 4;    // Put 1st ascii byte in upper nibble.
+		*destTemp += BtoH(*srcptr++);      // Add 2nd ascii byte to above.
+		destTemp++;
+	}
+}
+
+VOID	RTMPPatchMacBbpBug(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	ULONG	Index;
+
+	// Initialize BBP register to default value
+	for (Index = 0; Index < NUM_BBP_REG_PARMS; Index++)
+	{
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBPRegTable[Index].Register, (UCHAR)BBPRegTable[Index].Value);
+	}
+
+	// Initialize RF register to default value
+	AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+	AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+
+	// Re-init BBP register from EEPROM value
+	NICInitAsicFromEEPROM(pAd);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Init timer objects
+
+	Arguments:
+		pAd			Pointer to our adapter
+		pTimer				Timer structure
+		pTimerFunc			Function to execute when timer expired
+		Repeat				Ture for period timer
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPInitTimer(
+	IN	PRTMP_ADAPTER			pAd,
+	IN	PRALINK_TIMER_STRUCT	pTimer,
+	IN	PVOID					pTimerFunc,
+	IN	PVOID					pData,
+	IN	BOOLEAN					Repeat)
+{
+	//
+	// Set Valid to TRUE for later used.
+	// It will crash if we cancel a timer or set a timer
+	// that we haven't initialize before.
+	//
+	pTimer->Valid      = TRUE;
+
+	pTimer->PeriodicType = Repeat;
+	pTimer->State      = FALSE;
+	pTimer->cookie = (ULONG) pData;
+
+#ifdef RT2870
+	pTimer->pAd = pAd;
+#endif // RT2870 //
+
+	RTMP_OS_Init_Timer(pAd,	&pTimer->TimerObj,	pTimerFunc, (PVOID) pTimer);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Init timer objects
+
+	Arguments:
+		pTimer				Timer structure
+		Value				Timer value in milliseconds
+
+	Return Value:
+		None
+
+	Note:
+		To use this routine, must call RTMPInitTimer before.
+
+	========================================================================
+*/
+VOID	RTMPSetTimer(
+	IN	PRALINK_TIMER_STRUCT	pTimer,
+	IN	ULONG					Value)
+{
+	if (pTimer->Valid)
+	{
+		pTimer->TimerValue = Value;
+		pTimer->State      = FALSE;
+		if (pTimer->PeriodicType == TRUE)
+		{
+			pTimer->Repeat = TRUE;
+			RTMP_SetPeriodicTimer(&pTimer->TimerObj, Value);
+		}
+		else
+		{
+			pTimer->Repeat = FALSE;
+			RTMP_OS_Add_Timer(&pTimer->TimerObj, Value);
+		}
+	}
+	else
+	{
+		DBGPRINT_ERR(("RTMPSetTimer failed, Timer hasn't been initialize!\n"));
+	}
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Init timer objects
+
+	Arguments:
+		pTimer				Timer structure
+		Value				Timer value in milliseconds
+
+	Return Value:
+		None
+
+	Note:
+		To use this routine, must call RTMPInitTimer before.
+
+	========================================================================
+*/
+VOID	RTMPModTimer(
+	IN	PRALINK_TIMER_STRUCT	pTimer,
+	IN	ULONG					Value)
+{
+	BOOLEAN	Cancel;
+
+	if (pTimer->Valid)
+	{
+		pTimer->TimerValue = Value;
+		pTimer->State      = FALSE;
+		if (pTimer->PeriodicType == TRUE)
+		{
+			RTMPCancelTimer(pTimer, &Cancel);
+			RTMPSetTimer(pTimer, Value);
+		}
+		else
+		{
+			RTMP_OS_Mod_Timer(&pTimer->TimerObj, Value);
+		}
+	}
+	else
+	{
+		DBGPRINT_ERR(("RTMPModTimer failed, Timer hasn't been initialize!\n"));
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Cancel timer objects
+
+	Arguments:
+		Adapter						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+		1.) To use this routine, must call RTMPInitTimer before.
+		2.) Reset NIC to initial state AS IS system boot up time.
+
+	========================================================================
+*/
+VOID	RTMPCancelTimer(
+	IN	PRALINK_TIMER_STRUCT	pTimer,
+	OUT	BOOLEAN					*pCancelled)
+{
+	if (pTimer->Valid)
+	{
+		if (pTimer->State == FALSE)
+			pTimer->Repeat = FALSE;
+			RTMP_OS_Del_Timer(&pTimer->TimerObj, pCancelled);
+
+		if (*pCancelled == TRUE)
+			pTimer->State = TRUE;
+
+#ifdef RT2870
+		// We need to go-through the TimerQ to findout this timer handler and remove it if
+		//		it's still waiting for execution.
+
+		RT2870_TimerQ_Remove(pTimer->pAd, pTimer);
+#endif // RT2870 //
+	}
+	else
+	{
+		//
+		// NdisMCancelTimer just canced the timer and not mean release the timer.
+		// And don't set the "Valid" to False. So that we can use this timer again.
+		//
+		DBGPRINT_ERR(("RTMPCancelTimer failed, Timer hasn't been initialize!\n"));
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Set LED Status
+
+	Arguments:
+		pAd						Pointer to our adapter
+		Status					LED Status
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID RTMPSetLED(
+	IN PRTMP_ADAPTER 	pAd,
+	IN UCHAR			Status)
+{
+	//ULONG			data;
+	UCHAR			HighByte = 0;
+	UCHAR			LowByte;
+
+// In ATE mode of RT2860 AP/STA, we have erased 8051 firmware.
+// So LED mode is not supported when ATE is running.
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+	LowByte = pAd->LedCntl.field.LedMode&0x7f;
+	switch (Status)
+	{
+		case LED_LINK_DOWN:
+			HighByte = 0x20;
+			AsicSendCommandToMcu(pAd, 0x50, 0xff, LowByte, HighByte);
+			pAd->LedIndicatorStregth = 0;
+			break;
+		case LED_LINK_UP:
+			if (pAd->CommonCfg.Channel > 14)
+				HighByte = 0xa0;
+			else
+				HighByte = 0x60;
+			AsicSendCommandToMcu(pAd, 0x50, 0xff, LowByte, HighByte);
+			break;
+		case LED_RADIO_ON:
+			HighByte = 0x20;
+			AsicSendCommandToMcu(pAd, 0x50, 0xff, LowByte, HighByte);
+			break;
+		case LED_HALT:
+			LowByte = 0; // Driver sets MAC register and MAC controls LED
+		case LED_RADIO_OFF:
+			HighByte = 0;
+			AsicSendCommandToMcu(pAd, 0x50, 0xff, LowByte, HighByte);
+			break;
+        case LED_WPS:
+			HighByte = 0x10;
+			AsicSendCommandToMcu(pAd, 0x50, 0xff, LowByte, HighByte);
+			break;
+		case LED_ON_SITE_SURVEY:
+			HighByte = 0x08;
+			AsicSendCommandToMcu(pAd, 0x50, 0xff, LowByte, HighByte);
+			break;
+		case LED_POWER_UP:
+			HighByte = 0x04;
+			AsicSendCommandToMcu(pAd, 0x50, 0xff, LowByte, HighByte);
+			break;
+		default:
+			DBGPRINT(RT_DEBUG_WARN, ("RTMPSetLED::Unknown Status %d\n", Status));
+			break;
+	}
+
+    //
+	// Keep LED status for LED SiteSurvey mode.
+	// After SiteSurvey, we will set the LED mode to previous status.
+	//
+	if ((Status != LED_ON_SITE_SURVEY) && (Status != LED_POWER_UP))
+		pAd->LedStatus = Status;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPSetLED::Mode=%d,HighByte=0x%02x,LowByte=0x%02x\n", pAd->LedCntl.field.LedMode, HighByte, LowByte));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Set LED Signal Stregth
+
+	Arguments:
+		pAd						Pointer to our adapter
+		Dbm						Signal Stregth
+
+	Return Value:
+		None
+
+	IRQL = PASSIVE_LEVEL
+
+	Note:
+		Can be run on any IRQL level.
+
+		According to Microsoft Zero Config Wireless Signal Stregth definition as belows.
+		<= -90  No Signal
+		<= -81  Very Low
+		<= -71  Low
+		<= -67  Good
+		<= -57  Very Good
+		 > -57  Excellent
+	========================================================================
+*/
+VOID RTMPSetSignalLED(
+	IN PRTMP_ADAPTER 	pAd,
+	IN NDIS_802_11_RSSI Dbm)
+{
+	UCHAR		nLed = 0;
+
+	//
+	// if not Signal Stregth, then do nothing.
+	//
+	if (pAd->LedCntl.field.LedMode != LED_MODE_SIGNAL_STREGTH)
+	{
+		return;
+	}
+
+	if (Dbm <= -90)
+		nLed = 0;
+	else if (Dbm <= -81)
+		nLed = 1;
+	else if (Dbm <= -71)
+		nLed = 3;
+	else if (Dbm <= -67)
+		nLed = 7;
+	else if (Dbm <= -57)
+		nLed = 15;
+	else
+		nLed = 31;
+
+	//
+	// Update Signal Stregth to firmware if changed.
+	//
+	if (pAd->LedIndicatorStregth != nLed)
+	{
+		AsicSendCommandToMcu(pAd, 0x51, 0xff, nLed, pAd->LedCntl.field.Polarity);
+		pAd->LedIndicatorStregth = nLed;
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Enable RX
+
+	Arguments:
+		pAd						Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL <= DISPATCH_LEVEL
+
+	Note:
+		Before Enable RX, make sure you have enabled Interrupt.
+	========================================================================
+*/
+VOID RTMPEnableRxTx(
+	IN PRTMP_ADAPTER	pAd)
+{
+//	WPDMA_GLO_CFG_STRUC	GloCfg;
+//	ULONG	i = 0;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("==> RTMPEnableRxTx\n"));
+
+#if 0
+	// Enable Rx DMA.
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0x4);
+	do
+	{
+		RTMP_IO_READ32(pAd, WPDMA_GLO_CFG, &GloCfg.word);
+		if ((GloCfg.field.TxDMABusy == 0)  && (GloCfg.field.RxDMABusy == 0))
+			break;
+
+		DBGPRINT(RT_DEBUG_TRACE, ("==>  DMABusy\n"));
+		RTMPusecDelay(1000);
+		i++;
+	}while ( i <200);
+
+	RTMPusecDelay(50);
+	RT28XX_DMA_WRITE_INIT(GloCfg);
+	DBGPRINT(RT_DEBUG_TRACE, ("<== WRITE DMA offset 0x208 = 0x%x\n", GloCfg.word));
+	RTMP_IO_WRITE32(pAd, WPDMA_GLO_CFG, GloCfg.word);
+
+	RT28XX_DMA_POST_WRITE(pAd);
+#else
+	// Enable Rx DMA.
+	RT28XXDMAEnable(pAd);
+#endif
+
+	// enable RX of MAC block
+	if (pAd->OpMode == OPMODE_AP)
+	{
+		UINT32 rx_filter_flag = APNORMAL;
+
+
+		RTMP_IO_WRITE32(pAd, RX_FILTR_CFG, rx_filter_flag);     // enable RX of DMA block
+	}
+	else
+	{
+		RTMP_IO_WRITE32(pAd, RX_FILTR_CFG, STANORMAL);     // Staion not drop control frame will fail WiFi Certification.
+	}
+
+	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, 0xc);
+	DBGPRINT(RT_DEBUG_TRACE, ("<== RTMPEnableRxTx\n"));
+}
+
+
diff --git a/drivers/staging/rt2870/common/rtmp_tkip.c b/drivers/staging/rt2870/common/rtmp_tkip.c
new file mode 100644
index 00000000000..2847409ff08
--- /dev/null
+++ b/drivers/staging/rt2870/common/rtmp_tkip.c
@@ -0,0 +1,1613 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rtmp_tkip.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Paul Wu		02-25-02		Initial
+*/
+
+#include "../rt_config.h"
+
+// Rotation functions on 32 bit values
+#define ROL32( A, n ) \
+	( ((A) << (n)) | ( ((A)>>(32-(n))) & ( (1UL << (n)) - 1 ) ) )
+#define ROR32( A, n ) ROL32( (A), 32-(n) )
+
+UINT Tkip_Sbox_Lower[256] =
+{
+	0xA5,0x84,0x99,0x8D,0x0D,0xBD,0xB1,0x54,
+	0x50,0x03,0xA9,0x7D,0x19,0x62,0xE6,0x9A,
+	0x45,0x9D,0x40,0x87,0x15,0xEB,0xC9,0x0B,
+	0xEC,0x67,0xFD,0xEA,0xBF,0xF7,0x96,0x5B,
+	0xC2,0x1C,0xAE,0x6A,0x5A,0x41,0x02,0x4F,
+	0x5C,0xF4,0x34,0x08,0x93,0x73,0x53,0x3F,
+	0x0C,0x52,0x65,0x5E,0x28,0xA1,0x0F,0xB5,
+	0x09,0x36,0x9B,0x3D,0x26,0x69,0xCD,0x9F,
+	0x1B,0x9E,0x74,0x2E,0x2D,0xB2,0xEE,0xFB,
+	0xF6,0x4D,0x61,0xCE,0x7B,0x3E,0x71,0x97,
+	0xF5,0x68,0x00,0x2C,0x60,0x1F,0xC8,0xED,
+	0xBE,0x46,0xD9,0x4B,0xDE,0xD4,0xE8,0x4A,
+	0x6B,0x2A,0xE5,0x16,0xC5,0xD7,0x55,0x94,
+	0xCF,0x10,0x06,0x81,0xF0,0x44,0xBA,0xE3,
+	0xF3,0xFE,0xC0,0x8A,0xAD,0xBC,0x48,0x04,
+	0xDF,0xC1,0x75,0x63,0x30,0x1A,0x0E,0x6D,
+	0x4C,0x14,0x35,0x2F,0xE1,0xA2,0xCC,0x39,
+	0x57,0xF2,0x82,0x47,0xAC,0xE7,0x2B,0x95,
+	0xA0,0x98,0xD1,0x7F,0x66,0x7E,0xAB,0x83,
+	0xCA,0x29,0xD3,0x3C,0x79,0xE2,0x1D,0x76,
+	0x3B,0x56,0x4E,0x1E,0xDB,0x0A,0x6C,0xE4,
+	0x5D,0x6E,0xEF,0xA6,0xA8,0xA4,0x37,0x8B,
+	0x32,0x43,0x59,0xB7,0x8C,0x64,0xD2,0xE0,
+	0xB4,0xFA,0x07,0x25,0xAF,0x8E,0xE9,0x18,
+	0xD5,0x88,0x6F,0x72,0x24,0xF1,0xC7,0x51,
+	0x23,0x7C,0x9C,0x21,0xDD,0xDC,0x86,0x85,
+	0x90,0x42,0xC4,0xAA,0xD8,0x05,0x01,0x12,
+	0xA3,0x5F,0xF9,0xD0,0x91,0x58,0x27,0xB9,
+	0x38,0x13,0xB3,0x33,0xBB,0x70,0x89,0xA7,
+	0xB6,0x22,0x92,0x20,0x49,0xFF,0x78,0x7A,
+	0x8F,0xF8,0x80,0x17,0xDA,0x31,0xC6,0xB8,
+	0xC3,0xB0,0x77,0x11,0xCB,0xFC,0xD6,0x3A
+};
+
+UINT Tkip_Sbox_Upper[256] =
+{
+	0xC6,0xF8,0xEE,0xF6,0xFF,0xD6,0xDE,0x91,
+	0x60,0x02,0xCE,0x56,0xE7,0xB5,0x4D,0xEC,
+	0x8F,0x1F,0x89,0xFA,0xEF,0xB2,0x8E,0xFB,
+	0x41,0xB3,0x5F,0x45,0x23,0x53,0xE4,0x9B,
+	0x75,0xE1,0x3D,0x4C,0x6C,0x7E,0xF5,0x83,
+	0x68,0x51,0xD1,0xF9,0xE2,0xAB,0x62,0x2A,
+	0x08,0x95,0x46,0x9D,0x30,0x37,0x0A,0x2F,
+	0x0E,0x24,0x1B,0xDF,0xCD,0x4E,0x7F,0xEA,
+	0x12,0x1D,0x58,0x34,0x36,0xDC,0xB4,0x5B,
+	0xA4,0x76,0xB7,0x7D,0x52,0xDD,0x5E,0x13,
+	0xA6,0xB9,0x00,0xC1,0x40,0xE3,0x79,0xB6,
+	0xD4,0x8D,0x67,0x72,0x94,0x98,0xB0,0x85,
+	0xBB,0xC5,0x4F,0xED,0x86,0x9A,0x66,0x11,
+	0x8A,0xE9,0x04,0xFE,0xA0,0x78,0x25,0x4B,
+	0xA2,0x5D,0x80,0x05,0x3F,0x21,0x70,0xF1,
+	0x63,0x77,0xAF,0x42,0x20,0xE5,0xFD,0xBF,
+	0x81,0x18,0x26,0xC3,0xBE,0x35,0x88,0x2E,
+	0x93,0x55,0xFC,0x7A,0xC8,0xBA,0x32,0xE6,
+	0xC0,0x19,0x9E,0xA3,0x44,0x54,0x3B,0x0B,
+	0x8C,0xC7,0x6B,0x28,0xA7,0xBC,0x16,0xAD,
+	0xDB,0x64,0x74,0x14,0x92,0x0C,0x48,0xB8,
+	0x9F,0xBD,0x43,0xC4,0x39,0x31,0xD3,0xF2,
+	0xD5,0x8B,0x6E,0xDA,0x01,0xB1,0x9C,0x49,
+	0xD8,0xAC,0xF3,0xCF,0xCA,0xF4,0x47,0x10,
+	0x6F,0xF0,0x4A,0x5C,0x38,0x57,0x73,0x97,
+	0xCB,0xA1,0xE8,0x3E,0x96,0x61,0x0D,0x0F,
+	0xE0,0x7C,0x71,0xCC,0x90,0x06,0xF7,0x1C,
+	0xC2,0x6A,0xAE,0x69,0x17,0x99,0x3A,0x27,
+	0xD9,0xEB,0x2B,0x22,0xD2,0xA9,0x07,0x33,
+	0x2D,0x3C,0x15,0xC9,0x87,0xAA,0x50,0xA5,
+	0x03,0x59,0x09,0x1A,0x65,0xD7,0x84,0xD0,
+	0x82,0x29,0x5A,0x1E,0x7B,0xA8,0x6D,0x2C
+};
+
+/*****************************/
+/******** SBOX Table *********/
+/*****************************/
+
+UCHAR SboxTable[256] =
+{
+	0x63, 0x7c, 0x77, 0x7b, 0xf2, 0x6b, 0x6f, 0xc5,
+	0x30, 0x01, 0x67, 0x2b, 0xfe, 0xd7, 0xab, 0x76,
+	0xca, 0x82, 0xc9, 0x7d, 0xfa, 0x59, 0x47, 0xf0,
+	0xad, 0xd4, 0xa2, 0xaf, 0x9c, 0xa4, 0x72, 0xc0,
+	0xb7, 0xfd, 0x93, 0x26, 0x36, 0x3f, 0xf7, 0xcc,
+	0x34, 0xa5, 0xe5, 0xf1, 0x71, 0xd8, 0x31, 0x15,
+	0x04, 0xc7, 0x23, 0xc3, 0x18, 0x96, 0x05, 0x9a,
+	0x07, 0x12, 0x80, 0xe2, 0xeb, 0x27, 0xb2, 0x75,
+	0x09, 0x83, 0x2c, 0x1a, 0x1b, 0x6e, 0x5a, 0xa0,
+	0x52, 0x3b, 0xd6, 0xb3, 0x29, 0xe3, 0x2f, 0x84,
+	0x53, 0xd1, 0x00, 0xed, 0x20, 0xfc, 0xb1, 0x5b,
+	0x6a, 0xcb, 0xbe, 0x39, 0x4a, 0x4c, 0x58, 0xcf,
+	0xd0, 0xef, 0xaa, 0xfb, 0x43, 0x4d, 0x33, 0x85,
+	0x45, 0xf9, 0x02, 0x7f, 0x50, 0x3c, 0x9f, 0xa8,
+	0x51, 0xa3, 0x40, 0x8f, 0x92, 0x9d, 0x38, 0xf5,
+	0xbc, 0xb6, 0xda, 0x21, 0x10, 0xff, 0xf3, 0xd2,
+	0xcd, 0x0c, 0x13, 0xec, 0x5f, 0x97, 0x44, 0x17,
+	0xc4, 0xa7, 0x7e, 0x3d, 0x64, 0x5d, 0x19, 0x73,
+	0x60, 0x81, 0x4f, 0xdc, 0x22, 0x2a, 0x90, 0x88,
+	0x46, 0xee, 0xb8, 0x14, 0xde, 0x5e, 0x0b, 0xdb,
+	0xe0, 0x32, 0x3a, 0x0a, 0x49, 0x06, 0x24, 0x5c,
+	0xc2, 0xd3, 0xac, 0x62, 0x91, 0x95, 0xe4, 0x79,
+	0xe7, 0xc8, 0x37, 0x6d, 0x8d, 0xd5, 0x4e, 0xa9,
+	0x6c, 0x56, 0xf4, 0xea, 0x65, 0x7a, 0xae, 0x08,
+	0xba, 0x78, 0x25, 0x2e, 0x1c, 0xa6, 0xb4, 0xc6,
+	0xe8, 0xdd, 0x74, 0x1f, 0x4b, 0xbd, 0x8b, 0x8a,
+	0x70, 0x3e, 0xb5, 0x66, 0x48, 0x03, 0xf6, 0x0e,
+	0x61, 0x35, 0x57, 0xb9, 0x86, 0xc1, 0x1d, 0x9e,
+	0xe1, 0xf8, 0x98, 0x11, 0x69, 0xd9, 0x8e, 0x94,
+	0x9b, 0x1e, 0x87, 0xe9, 0xce, 0x55, 0x28, 0xdf,
+	0x8c, 0xa1, 0x89, 0x0d, 0xbf, 0xe6, 0x42, 0x68,
+	0x41, 0x99, 0x2d, 0x0f, 0xb0, 0x54, 0xbb, 0x16
+};
+
+VOID xor_32(
+	IN  PUCHAR              a,
+	IN  PUCHAR              b,
+	OUT PUCHAR              out);
+
+VOID xor_128(
+	IN  PUCHAR              a,
+	IN  PUCHAR              b,
+	OUT PUCHAR              out);
+
+VOID next_key(
+	IN  PUCHAR              key,
+	IN  INT                 round);
+
+VOID byte_sub(
+	IN  PUCHAR              in,
+	OUT PUCHAR              out);
+
+VOID shift_row(
+	IN  PUCHAR              in,
+	OUT PUCHAR              out);
+
+VOID mix_column(
+	IN  PUCHAR              in,
+	OUT PUCHAR              out);
+
+UCHAR RTMPCkipSbox(
+	IN  UCHAR               a);
+//
+// Expanded IV for TKIP function.
+//
+typedef	struct	PACKED _IV_CONTROL_
+{
+	union PACKED
+	{
+		struct PACKED
+		{
+			UCHAR		rc0;
+			UCHAR		rc1;
+			UCHAR		rc2;
+
+			union PACKED
+			{
+				struct PACKED
+				{
+#ifdef RT_BIG_ENDIAN
+					UCHAR	KeyID:2;
+					UCHAR	ExtIV:1;
+					UCHAR	Rsvd:5;
+#else
+					UCHAR	Rsvd:5;
+					UCHAR	ExtIV:1;
+					UCHAR	KeyID:2;
+#endif
+				}	field;
+				UCHAR		Byte;
+			}	CONTROL;
+		}	field;
+
+		ULONG	word;
+	}	IV16;
+
+	ULONG	IV32;
+}	TKIP_IV, *PTKIP_IV;
+
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Convert from UCHAR[] to ULONG in a portable way
+
+	Arguments:
+      pMICKey		pointer to MIC Key
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+ULONG	RTMPTkipGetUInt32(
+	IN	PUCHAR	pMICKey)
+{
+	ULONG	res = 0;
+	INT		i;
+
+	for (i = 0; i < 4; i++)
+	{
+		res |= (*pMICKey++) << (8 * i);
+	}
+
+	return res;
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Convert from ULONG to UCHAR[] in a portable way
+
+	Arguments:
+      pDst			pointer to destination for convert ULONG to UCHAR[]
+      val			the value for convert
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPTkipPutUInt32(
+	IN OUT	PUCHAR		pDst,
+	IN		ULONG		val)
+{
+	INT i;
+
+	for(i = 0; i < 4; i++)
+	{
+		*pDst++ = (UCHAR) (val & 0xff);
+		val >>= 8;
+	}
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Set the MIC Key.
+
+	Arguments:
+      pAd		Pointer to our adapter
+      pMICKey		pointer to MIC Key
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID RTMPTkipSetMICKey(
+	IN	PTKIP_KEY_INFO	pTkip,
+	IN	PUCHAR			pMICKey)
+{
+	// Set the key
+	pTkip->K0 = RTMPTkipGetUInt32(pMICKey);
+	pTkip->K1 = RTMPTkipGetUInt32(pMICKey + 4);
+	// and reset the message
+	pTkip->L = pTkip->K0;
+	pTkip->R = pTkip->K1;
+	pTkip->nBytesInM = 0;
+	pTkip->M = 0;
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Calculate the MIC Value.
+
+	Arguments:
+      pAd		Pointer to our adapter
+      uChar			Append this uChar
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPTkipAppendByte(
+	IN	PTKIP_KEY_INFO	pTkip,
+	IN	UCHAR 			uChar)
+{
+	// Append the byte to our word-sized buffer
+	pTkip->M |= (uChar << (8* pTkip->nBytesInM));
+	pTkip->nBytesInM++;
+	// Process the word if it is full.
+	if( pTkip->nBytesInM >= 4 )
+	{
+		pTkip->L ^= pTkip->M;
+		pTkip->R ^= ROL32( pTkip->L, 17 );
+		pTkip->L += pTkip->R;
+		pTkip->R ^= ((pTkip->L & 0xff00ff00) >> 8) | ((pTkip->L & 0x00ff00ff) << 8);
+		pTkip->L += pTkip->R;
+		pTkip->R ^= ROL32( pTkip->L, 3 );
+		pTkip->L += pTkip->R;
+		pTkip->R ^= ROR32( pTkip->L, 2 );
+		pTkip->L += pTkip->R;
+		// Clear the buffer
+		pTkip->M = 0;
+		pTkip->nBytesInM = 0;
+	}
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Calculate the MIC Value.
+
+	Arguments:
+      pAd		Pointer to our adapter
+      pSrc			Pointer to source data for Calculate MIC Value
+      Len			Indicate the length of the source data
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPTkipAppend(
+	IN	PTKIP_KEY_INFO	pTkip,
+	IN	PUCHAR			pSrc,
+	IN	UINT			nBytes)
+{
+	// This is simple
+	while(nBytes > 0)
+	{
+		RTMPTkipAppendByte(pTkip, *pSrc++);
+		nBytes--;
+	}
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Get the MIC Value.
+
+	Arguments:
+      pAd		Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+		the MIC Value is store in pAd->PrivateInfo.MIC
+	========================================================================
+*/
+VOID	RTMPTkipGetMIC(
+	IN	PTKIP_KEY_INFO	pTkip)
+{
+	// Append the minimum padding
+	RTMPTkipAppendByte(pTkip, 0x5a );
+	RTMPTkipAppendByte(pTkip, 0 );
+	RTMPTkipAppendByte(pTkip, 0 );
+	RTMPTkipAppendByte(pTkip, 0 );
+	RTMPTkipAppendByte(pTkip, 0 );
+	// and then zeroes until the length is a multiple of 4
+	while( pTkip->nBytesInM != 0 )
+	{
+		RTMPTkipAppendByte(pTkip, 0 );
+	}
+	// The appendByte function has already computed the result.
+	RTMPTkipPutUInt32(pTkip->MIC, pTkip->L);
+	RTMPTkipPutUInt32(pTkip->MIC + 4, pTkip->R);
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Init Tkip function.
+
+	Arguments:
+      pAd		Pointer to our adapter
+		pTKey       Pointer to the Temporal Key (TK), TK shall be 128bits.
+		KeyId		TK Key ID
+		pTA			Pointer to transmitter address
+		pMICKey		pointer to MIC Key
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPInitTkipEngine(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pKey,
+	IN	UCHAR			KeyId,
+	IN	PUCHAR			pTA,
+	IN	PUCHAR			pMICKey,
+	IN	PUCHAR			pTSC,
+	OUT	PULONG			pIV16,
+	OUT	PULONG			pIV32)
+{
+	TKIP_IV	tkipIv;
+
+	// Prepare 8 bytes TKIP encapsulation for MPDU
+	NdisZeroMemory(&tkipIv, sizeof(TKIP_IV));
+	tkipIv.IV16.field.rc0 = *(pTSC + 1);
+	tkipIv.IV16.field.rc1 = (tkipIv.IV16.field.rc0 | 0x20) & 0x7f;
+	tkipIv.IV16.field.rc2 = *pTSC;
+	tkipIv.IV16.field.CONTROL.field.ExtIV = 1;  // 0: non-extended IV, 1: an extended IV
+	tkipIv.IV16.field.CONTROL.field.KeyID = KeyId;
+//	tkipIv.IV32 = *(PULONG)(pTSC + 2);
+	NdisMoveMemory(&tkipIv.IV32, (pTSC + 2), 4);   // Copy IV
+
+	*pIV16 = tkipIv.IV16.word;
+	*pIV32 = tkipIv.IV32;
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Init MIC Value calculation function which include set MIC key &
+		calculate first 16 bytes (DA + SA + priority +  0)
+
+	Arguments:
+      pAd		Pointer to our adapter
+		pTKey       Pointer to the Temporal Key (TK), TK shall be 128bits.
+		pDA			Pointer to DA address
+		pSA			Pointer to SA address
+		pMICKey		pointer to MIC Key
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPInitMICEngine(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pKey,
+	IN	PUCHAR			pDA,
+	IN	PUCHAR			pSA,
+	IN  UCHAR           UserPriority,
+	IN	PUCHAR			pMICKey)
+{
+	ULONG Priority = UserPriority;
+
+	// Init MIC value calculation
+	RTMPTkipSetMICKey(&pAd->PrivateInfo.Tx, pMICKey);
+	// DA
+	RTMPTkipAppend(&pAd->PrivateInfo.Tx, pDA, MAC_ADDR_LEN);
+	// SA
+	RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSA, MAC_ADDR_LEN);
+	// Priority + 3 bytes of 0
+	RTMPTkipAppend(&pAd->PrivateInfo.Tx, (PUCHAR)&Priority, 4);
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Compare MIC value of received MSDU
+
+	Arguments:
+		pAd	Pointer to our adapter
+		pSrc        Pointer to the received Plain text data
+		pDA			Pointer to DA address
+		pSA			Pointer to SA address
+		pMICKey		pointer to MIC Key
+		Len         the length of the received plain text data exclude MIC value
+
+	Return Value:
+		TRUE        MIC value matched
+		FALSE       MIC value mismatched
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+BOOLEAN	RTMPTkipCompareMICValue(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pSrc,
+	IN	PUCHAR			pDA,
+	IN	PUCHAR			pSA,
+	IN	PUCHAR			pMICKey,
+	IN	UCHAR			UserPriority,
+	IN	UINT			Len)
+{
+	UCHAR	OldMic[8];
+	ULONG	Priority = UserPriority;
+
+	// Init MIC value calculation
+	RTMPTkipSetMICKey(&pAd->PrivateInfo.Rx, pMICKey);
+	// DA
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, pDA, MAC_ADDR_LEN);
+	// SA
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSA, MAC_ADDR_LEN);
+	// Priority + 3 bytes of 0
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, (PUCHAR)&Priority, 4);
+
+	// Calculate MIC value from plain text data
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSrc, Len);
+
+	// Get MIC valude from received frame
+	NdisMoveMemory(OldMic, pSrc + Len, 8);
+
+	// Get MIC value from decrypted plain data
+	RTMPTkipGetMIC(&pAd->PrivateInfo.Rx);
+
+	// Move MIC value from MSDU, this steps should move to data path.
+	// Since the MIC value might cross MPDUs.
+	if(!NdisEqualMemory(pAd->PrivateInfo.Rx.MIC, OldMic, 8))
+	{
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("RTMPTkipCompareMICValue(): TKIP MIC Error !\n"));  //MIC error.
+
+
+		return (FALSE);
+	}
+	return (TRUE);
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Compare MIC value of received MSDU
+
+	Arguments:
+		pAd	Pointer to our adapter
+		pLLC		LLC header
+		pSrc        Pointer to the received Plain text data
+		pDA			Pointer to DA address
+		pSA			Pointer to SA address
+		pMICKey		pointer to MIC Key
+		Len         the length of the received plain text data exclude MIC value
+
+	Return Value:
+		TRUE        MIC value matched
+		FALSE       MIC value mismatched
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+BOOLEAN	RTMPTkipCompareMICValueWithLLC(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pLLC,
+	IN	PUCHAR			pSrc,
+	IN	PUCHAR			pDA,
+	IN	PUCHAR			pSA,
+	IN	PUCHAR			pMICKey,
+	IN	UINT			Len)
+{
+	UCHAR	OldMic[8];
+	ULONG	Priority = 0;
+
+	// Init MIC value calculation
+	RTMPTkipSetMICKey(&pAd->PrivateInfo.Rx, pMICKey);
+	// DA
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, pDA, MAC_ADDR_LEN);
+	// SA
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSA, MAC_ADDR_LEN);
+	// Priority + 3 bytes of 0
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, (PUCHAR)&Priority, 4);
+
+	// Start with LLC header
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, pLLC, 8);
+
+	// Calculate MIC value from plain text data
+	RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSrc, Len);
+
+	// Get MIC valude from received frame
+	NdisMoveMemory(OldMic, pSrc + Len, 8);
+
+	// Get MIC value from decrypted plain data
+	RTMPTkipGetMIC(&pAd->PrivateInfo.Rx);
+
+	// Move MIC value from MSDU, this steps should move to data path.
+	// Since the MIC value might cross MPDUs.
+	if(!NdisEqualMemory(pAd->PrivateInfo.Rx.MIC, OldMic, 8))
+	{
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("RTMPTkipCompareMICValueWithLLC(): TKIP MIC Error !\n"));  //MIC error.
+
+
+		return (FALSE);
+	}
+	return (TRUE);
+}
+/*
+	========================================================================
+
+	Routine	Description:
+		Copy frame from waiting queue into relative ring buffer and set
+	appropriate ASIC register to kick hardware transmit function
+
+	Arguments:
+		pAd		Pointer	to our adapter
+		PNDIS_PACKET	Pointer to Ndis Packet for MIC calculation
+		pEncap			Pointer to LLC encap data
+		LenEncap		Total encap length, might be 0 which indicates no encap
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPCalculateMICValue(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	PUCHAR			pEncap,
+	IN	PCIPHER_KEY		pKey,
+	IN	UCHAR			apidx)
+{
+	PACKET_INFO		PacketInfo;
+	PUCHAR			pSrcBufVA;
+	UINT			SrcBufLen;
+	PUCHAR			pSrc;
+    UCHAR           UserPriority;
+	UCHAR			vlan_offset = 0;
+
+	RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pSrcBufVA, &SrcBufLen);
+
+	UserPriority = RTMP_GET_PACKET_UP(pPacket);
+	pSrc = pSrcBufVA;
+
+	// determine if this is a vlan packet
+	if (((*(pSrc + 12) << 8) + *(pSrc + 13)) == 0x8100)
+		vlan_offset = 4;
+
+#ifdef CONFIG_STA_SUPPORT
+#endif // CONFIG_STA_SUPPORT //
+	{
+		RTMPInitMICEngine(
+			pAd,
+			pKey->Key,
+			pSrc,
+			pSrc + 6,
+			UserPriority,
+			pKey->TxMic);
+	}
+
+
+	if (pEncap != NULL)
+	{
+		// LLC encapsulation
+		RTMPTkipAppend(&pAd->PrivateInfo.Tx, pEncap, 6);
+		// Protocol Type
+		RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSrc + 12 + vlan_offset, 2);
+	}
+	SrcBufLen -= (14 + vlan_offset);
+	pSrc += (14 + vlan_offset);
+	do
+	{
+		if (SrcBufLen > 0)
+		{
+			RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSrc, SrcBufLen);
+		}
+
+		break;	// No need handle next packet
+
+	}	while (TRUE);		// End of copying payload
+
+	// Compute the final MIC Value
+	RTMPTkipGetMIC(&pAd->PrivateInfo.Tx);
+}
+
+
+/************************************************************/
+/* tkip_sbox()																*/
+/* Returns a 16 bit value from a 64K entry table. The Table */
+/* is synthesized from two 256 entry byte wide tables.		*/
+/************************************************************/
+
+UINT tkip_sbox(UINT index)
+{
+	UINT index_low;
+	UINT index_high;
+	UINT left, right;
+
+	index_low = (index % 256);
+	index_high = ((index >> 8) % 256);
+
+	left = Tkip_Sbox_Lower[index_low] + (Tkip_Sbox_Upper[index_low] * 256);
+	right = Tkip_Sbox_Upper[index_high] + (Tkip_Sbox_Lower[index_high] * 256);
+
+	return (left ^ right);
+}
+
+UINT rotr1(UINT a)
+{
+	unsigned int b;
+
+	if ((a & 0x01) == 0x01)
+	{
+		b = (a >> 1) | 0x8000;
+	}
+	else
+	{
+		b = (a >> 1) & 0x7fff;
+	}
+	b = b % 65536;
+	return b;
+}
+
+VOID RTMPTkipMixKey(
+	UCHAR *key,
+	UCHAR *ta,
+	ULONG pnl, /* Least significant 16 bits of PN */
+	ULONG pnh, /* Most significant 32 bits of PN */
+	UCHAR *rc4key,
+	UINT *p1k)
+{
+
+	UINT tsc0;
+	UINT tsc1;
+	UINT tsc2;
+
+	UINT ppk0;
+	UINT ppk1;
+	UINT ppk2;
+	UINT ppk3;
+	UINT ppk4;
+	UINT ppk5;
+
+	INT i;
+	INT j;
+
+	tsc0 = (unsigned int)((pnh >> 16) % 65536); /* msb */
+	tsc1 = (unsigned int)(pnh % 65536);
+	tsc2 = (unsigned int)(pnl % 65536); /* lsb */
+
+	/* Phase 1, step 1 */
+	p1k[0] = tsc1;
+	p1k[1] = tsc0;
+	p1k[2] = (UINT)(ta[0] + (ta[1]*256));
+	p1k[3] = (UINT)(ta[2] + (ta[3]*256));
+	p1k[4] = (UINT)(ta[4] + (ta[5]*256));
+
+	/* Phase 1, step 2 */
+	for (i=0; i<8; i++)
+	{
+		j = 2*(i & 1);
+		p1k[0] = (p1k[0] + tkip_sbox( (p1k[4] ^ ((256*key[1+j]) + key[j])) % 65536 )) % 65536;
+		p1k[1] = (p1k[1] + tkip_sbox( (p1k[0] ^ ((256*key[5+j]) + key[4+j])) % 65536 )) % 65536;
+		p1k[2] = (p1k[2] + tkip_sbox( (p1k[1] ^ ((256*key[9+j]) + key[8+j])) % 65536 )) % 65536;
+		p1k[3] = (p1k[3] + tkip_sbox( (p1k[2] ^ ((256*key[13+j]) + key[12+j])) % 65536 )) % 65536;
+		p1k[4] = (p1k[4] + tkip_sbox( (p1k[3] ^ (((256*key[1+j]) + key[j]))) % 65536 )) % 65536;
+		p1k[4] = (p1k[4] + i) % 65536;
+	}
+
+	/* Phase 2, Step 1 */
+	ppk0 = p1k[0];
+	ppk1 = p1k[1];
+	ppk2 = p1k[2];
+	ppk3 = p1k[3];
+	ppk4 = p1k[4];
+	ppk5 = (p1k[4] + tsc2) % 65536;
+
+	/* Phase2, Step 2 */
+	ppk0 = ppk0 + tkip_sbox( (ppk5 ^ ((256*key[1]) + key[0])) % 65536);
+	ppk1 = ppk1 + tkip_sbox( (ppk0 ^ ((256*key[3]) + key[2])) % 65536);
+	ppk2 = ppk2 + tkip_sbox( (ppk1 ^ ((256*key[5]) + key[4])) % 65536);
+	ppk3 = ppk3 + tkip_sbox( (ppk2 ^ ((256*key[7]) + key[6])) % 65536);
+	ppk4 = ppk4 + tkip_sbox( (ppk3 ^ ((256*key[9]) + key[8])) % 65536);
+	ppk5 = ppk5 + tkip_sbox( (ppk4 ^ ((256*key[11]) + key[10])) % 65536);
+
+	ppk0 = ppk0 + rotr1(ppk5 ^ ((256*key[13]) + key[12]));
+	ppk1 = ppk1 + rotr1(ppk0 ^ ((256*key[15]) + key[14]));
+	ppk2 = ppk2 + rotr1(ppk1);
+	ppk3 = ppk3 + rotr1(ppk2);
+	ppk4 = ppk4 + rotr1(ppk3);
+	ppk5 = ppk5 + rotr1(ppk4);
+
+	/* Phase 2, Step 3 */
+    /* Phase 2, Step 3 */
+
+	tsc0 = (unsigned int)((pnh >> 16) % 65536); /* msb */
+	tsc1 = (unsigned int)(pnh % 65536);
+	tsc2 = (unsigned int)(pnl % 65536); /* lsb */
+
+	rc4key[0] = (tsc2 >> 8) % 256;
+	rc4key[1] = (((tsc2 >> 8) % 256) | 0x20) & 0x7f;
+	rc4key[2] = tsc2 % 256;
+	rc4key[3] = ((ppk5 ^ ((256*key[1]) + key[0])) >> 1) % 256;
+
+	rc4key[4] = ppk0 % 256;
+	rc4key[5] = (ppk0 >> 8) % 256;
+
+	rc4key[6] = ppk1 % 256;
+	rc4key[7] = (ppk1 >> 8) % 256;
+
+	rc4key[8] = ppk2 % 256;
+	rc4key[9] = (ppk2 >> 8) % 256;
+
+	rc4key[10] = ppk3 % 256;
+	rc4key[11] = (ppk3 >> 8) % 256;
+
+	rc4key[12] = ppk4 % 256;
+	rc4key[13] = (ppk4 >> 8) % 256;
+
+	rc4key[14] = ppk5 % 256;
+	rc4key[15] = (ppk5 >> 8) % 256;
+}
+
+
+/************************************************/
+/* construct_mic_header1()                      */
+/* Builds the first MIC header block from       */
+/* header fields.                               */
+/************************************************/
+
+void construct_mic_header1(
+	unsigned char *mic_header1,
+	int header_length,
+	unsigned char *mpdu)
+{
+	mic_header1[0] = (unsigned char)((header_length - 2) / 256);
+	mic_header1[1] = (unsigned char)((header_length - 2) % 256);
+	mic_header1[2] = mpdu[0] & 0xcf;    /* Mute CF poll & CF ack bits */
+	mic_header1[3] = mpdu[1] & 0xc7;    /* Mute retry, more data and pwr mgt bits */
+	mic_header1[4] = mpdu[4];       /* A1 */
+	mic_header1[5] = mpdu[5];
+	mic_header1[6] = mpdu[6];
+	mic_header1[7] = mpdu[7];
+	mic_header1[8] = mpdu[8];
+	mic_header1[9] = mpdu[9];
+	mic_header1[10] = mpdu[10];     /* A2 */
+	mic_header1[11] = mpdu[11];
+	mic_header1[12] = mpdu[12];
+	mic_header1[13] = mpdu[13];
+	mic_header1[14] = mpdu[14];
+	mic_header1[15] = mpdu[15];
+}
+
+/************************************************/
+/* construct_mic_header2()                      */
+/* Builds the last MIC header block from        */
+/* header fields.                               */
+/************************************************/
+
+void construct_mic_header2(
+	unsigned char *mic_header2,
+	unsigned char *mpdu,
+	int a4_exists,
+	int qc_exists)
+{
+	int i;
+
+	for (i = 0; i<16; i++) mic_header2[i]=0x00;
+
+	mic_header2[0] = mpdu[16];    /* A3 */
+	mic_header2[1] = mpdu[17];
+	mic_header2[2] = mpdu[18];
+	mic_header2[3] = mpdu[19];
+	mic_header2[4] = mpdu[20];
+	mic_header2[5] = mpdu[21];
+
+	// In Sequence Control field, mute sequence numer bits (12-bit)
+	mic_header2[6] = mpdu[22] & 0x0f;   /* SC */
+	mic_header2[7] = 0x00; /* mpdu[23]; */
+
+	if ((!qc_exists) & a4_exists)
+	{
+		for (i=0;i<6;i++) mic_header2[8+i] = mpdu[24+i];   /* A4 */
+
+	}
+
+	if (qc_exists && (!a4_exists))
+	{
+		mic_header2[8] = mpdu[24] & 0x0f; /* mute bits 15 - 4 */
+		mic_header2[9] = mpdu[25] & 0x00;
+	}
+
+	if (qc_exists && a4_exists)
+	{
+		for (i=0;i<6;i++) mic_header2[8+i] = mpdu[24+i];   /* A4 */
+
+		mic_header2[14] = mpdu[30] & 0x0f;
+		mic_header2[15] = mpdu[31] & 0x00;
+	}
+}
+
+
+/************************************************/
+/* construct_mic_iv()                           */
+/* Builds the MIC IV from header fields and PN  */
+/************************************************/
+
+void construct_mic_iv(
+	unsigned char *mic_iv,
+	int qc_exists,
+	int a4_exists,
+	unsigned char *mpdu,
+	unsigned int payload_length,
+	unsigned char *pn_vector)
+{
+	int i;
+
+	mic_iv[0] = 0x59;
+	if (qc_exists && a4_exists)
+		mic_iv[1] = mpdu[30] & 0x0f;    /* QoS_TC           */
+	if (qc_exists && !a4_exists)
+		mic_iv[1] = mpdu[24] & 0x0f;   /* mute bits 7-4    */
+	if (!qc_exists)
+		mic_iv[1] = 0x00;
+	for (i = 2; i < 8; i++)
+		mic_iv[i] = mpdu[i + 8];                    /* mic_iv[2:7] = A2[0:5] = mpdu[10:15] */
+#ifdef CONSISTENT_PN_ORDER
+		for (i = 8; i < 14; i++)
+			mic_iv[i] = pn_vector[i - 8];           /* mic_iv[8:13] = PN[0:5] */
+#else
+		for (i = 8; i < 14; i++)
+			mic_iv[i] = pn_vector[13 - i];          /* mic_iv[8:13] = PN[5:0] */
+#endif
+	i = (payload_length / 256);
+	i = (payload_length % 256);
+	mic_iv[14] = (unsigned char) (payload_length / 256);
+	mic_iv[15] = (unsigned char) (payload_length % 256);
+
+}
+
+
+
+/************************************/
+/* bitwise_xor()                    */
+/* A 128 bit, bitwise exclusive or  */
+/************************************/
+
+void bitwise_xor(unsigned char *ina, unsigned char *inb, unsigned char *out)
+{
+	int i;
+	for (i=0; i<16; i++)
+	{
+		out[i] = ina[i] ^ inb[i];
+	}
+}
+
+
+void aes128k128d(unsigned char *key, unsigned char *data, unsigned char *ciphertext)
+{
+	int round;
+	int i;
+	unsigned char intermediatea[16];
+	unsigned char intermediateb[16];
+	unsigned char round_key[16];
+
+	for(i=0; i<16; i++) round_key[i] = key[i];
+
+	for (round = 0; round < 11; round++)
+	{
+		if (round == 0)
+		{
+			xor_128(round_key, data, ciphertext);
+			next_key(round_key, round);
+		}
+		else if (round == 10)
+		{
+			byte_sub(ciphertext, intermediatea);
+			shift_row(intermediatea, intermediateb);
+			xor_128(intermediateb, round_key, ciphertext);
+		}
+		else    /* 1 - 9 */
+		{
+			byte_sub(ciphertext, intermediatea);
+			shift_row(intermediatea, intermediateb);
+			mix_column(&intermediateb[0], &intermediatea[0]);
+			mix_column(&intermediateb[4], &intermediatea[4]);
+			mix_column(&intermediateb[8], &intermediatea[8]);
+			mix_column(&intermediateb[12], &intermediatea[12]);
+			xor_128(intermediatea, round_key, ciphertext);
+			next_key(round_key, round);
+		}
+	}
+
+}
+
+void construct_ctr_preload(
+	unsigned char *ctr_preload,
+	int a4_exists,
+	int qc_exists,
+	unsigned char *mpdu,
+	unsigned char *pn_vector,
+	int c)
+{
+
+	int i = 0;
+	for (i=0; i<16; i++) ctr_preload[i] = 0x00;
+	i = 0;
+
+	ctr_preload[0] = 0x01;                                  /* flag */
+	if (qc_exists && a4_exists) ctr_preload[1] = mpdu[30] & 0x0f;   /* QoC_Control  */
+	if (qc_exists && !a4_exists) ctr_preload[1] = mpdu[24] & 0x0f;
+
+	for (i = 2; i < 8; i++)
+		ctr_preload[i] = mpdu[i + 8];                       /* ctr_preload[2:7] = A2[0:5] = mpdu[10:15] */
+#ifdef CONSISTENT_PN_ORDER
+	  for (i = 8; i < 14; i++)
+			ctr_preload[i] =    pn_vector[i - 8];           /* ctr_preload[8:13] = PN[0:5] */
+#else
+	  for (i = 8; i < 14; i++)
+			ctr_preload[i] =    pn_vector[13 - i];          /* ctr_preload[8:13] = PN[5:0] */
+#endif
+	ctr_preload[14] =  (unsigned char) (c / 256); // Ctr
+	ctr_preload[15] =  (unsigned char) (c % 256);
+
+}
+
+
+//
+// TRUE: Success!
+// FALSE: Decrypt Error!
+//
+BOOLEAN RTMPSoftDecryptTKIP(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR	pData,
+	IN ULONG	DataByteCnt,
+	IN UCHAR    UserPriority,
+	IN PCIPHER_KEY	pWpaKey)
+{
+	UCHAR			KeyID;
+	UINT			HeaderLen;
+    UCHAR			fc0;
+	UCHAR			fc1;
+	USHORT			fc;
+	UINT			frame_type;
+	UINT			frame_subtype;
+    UINT			from_ds;
+    UINT			to_ds;
+	INT				a4_exists;
+	INT				qc_exists;
+	USHORT			duration;
+	USHORT			seq_control;
+	USHORT			qos_control;
+	UCHAR			TA[MAC_ADDR_LEN];
+	UCHAR			DA[MAC_ADDR_LEN];
+	UCHAR			SA[MAC_ADDR_LEN];
+	UCHAR			RC4Key[16];
+	UINT			p1k[5]; //for mix_key;
+	ULONG			pnl;/* Least significant 16 bits of PN */
+	ULONG			pnh;/* Most significant 32 bits of PN */
+	UINT			num_blocks;
+	UINT			payload_remainder;
+	ARCFOURCONTEXT 	ArcFourContext;
+	UINT			crc32 = 0;
+	UINT			trailfcs = 0;
+	UCHAR			MIC[8];
+	UCHAR			TrailMIC[8];
+
+#ifdef RT_BIG_ENDIAN
+	RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
+#endif
+
+	fc0 = *pData;
+	fc1 = *(pData + 1);
+
+	fc = *((PUSHORT)pData);
+
+	frame_type = ((fc0 >> 2) & 0x03);
+	frame_subtype = ((fc0 >> 4) & 0x0f);
+
+    from_ds = (fc1 & 0x2) >> 1;
+    to_ds = (fc1 & 0x1);
+
+    a4_exists = (from_ds & to_ds);
+    qc_exists = ((frame_subtype == 0x08) ||    /* Assumed QoS subtypes */
+                  (frame_subtype == 0x09) ||   /* Likely to change.    */
+                  (frame_subtype == 0x0a) ||
+                  (frame_subtype == 0x0b)
+                 );
+
+	HeaderLen = 24;
+	if (a4_exists)
+		HeaderLen += 6;
+
+	KeyID = *((PUCHAR)(pData+ HeaderLen + 3));
+	KeyID = KeyID >> 6;
+
+	if (pWpaKey[KeyID].KeyLen == 0)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptTKIP failed!(KeyID[%d] Length can not be 0)\n", KeyID));
+		return FALSE;
+	}
+
+	duration = *((PUSHORT)(pData+2));
+
+	seq_control = *((PUSHORT)(pData+22));
+
+	if (qc_exists)
+	{
+		if (a4_exists)
+		{
+			qos_control = *((PUSHORT)(pData+30));
+		}
+		else
+		{
+			qos_control = *((PUSHORT)(pData+24));
+		}
+	}
+
+	if (to_ds == 0 && from_ds == 1)
+	{
+		NdisMoveMemory(DA, pData+4, MAC_ADDR_LEN);
+		NdisMoveMemory(SA, pData+16, MAC_ADDR_LEN);
+		NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);  //BSSID
+	}
+	else if (to_ds == 0 && from_ds == 0 )
+	{
+		NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
+		NdisMoveMemory(DA, pData+4, MAC_ADDR_LEN);
+		NdisMoveMemory(SA, pData+10, MAC_ADDR_LEN);
+	}
+	else if (to_ds == 1 && from_ds == 0)
+	{
+		NdisMoveMemory(SA, pData+10, MAC_ADDR_LEN);
+		NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
+		NdisMoveMemory(DA, pData+16, MAC_ADDR_LEN);
+	}
+	else if (to_ds == 1 && from_ds == 1)
+	{
+		NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
+		NdisMoveMemory(DA, pData+16, MAC_ADDR_LEN);
+		NdisMoveMemory(SA, pData+22, MAC_ADDR_LEN);
+	}
+
+	num_blocks = (DataByteCnt - 16) / 16;
+	payload_remainder = (DataByteCnt - 16) % 16;
+
+	pnl = (*(pData + HeaderLen)) * 256 + *(pData + HeaderLen + 2);
+	pnh = *((PULONG)(pData + HeaderLen + 4));
+	pnh = cpu2le32(pnh);
+	RTMPTkipMixKey(pWpaKey[KeyID].Key, TA, pnl, pnh, RC4Key, p1k);
+
+	ARCFOUR_INIT(&ArcFourContext, RC4Key, 16);
+
+	ARCFOUR_DECRYPT(&ArcFourContext, pData + HeaderLen, pData + HeaderLen + 8, DataByteCnt - HeaderLen - 8);
+	NdisMoveMemory(&trailfcs, pData + DataByteCnt - 8 - 4, 4);
+	crc32 = RTMP_CALC_FCS32(PPPINITFCS32, pData + HeaderLen, DataByteCnt - HeaderLen - 8 - 4);  //Skip IV+EIV 8 bytes & Skip last 4 bytes(FCS).
+	crc32 ^= 0xffffffff;             /* complement */
+
+    if(crc32 != cpu2le32(trailfcs))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptTKIP, WEP Data ICV Error !\n"));	 //ICV error.
+
+		return (FALSE);
+	}
+
+	NdisMoveMemory(TrailMIC, pData + DataByteCnt - 8 - 8 - 4, 8);
+	RTMPInitMICEngine(pAd, pWpaKey[KeyID].Key, DA, SA, UserPriority, pWpaKey[KeyID].RxMic);
+	RTMPTkipAppend(&pAd->PrivateInfo.Tx, pData + HeaderLen, DataByteCnt - HeaderLen - 8 - 12);
+	RTMPTkipGetMIC(&pAd->PrivateInfo.Tx);
+	NdisMoveMemory(MIC, pAd->PrivateInfo.Tx.MIC, 8);
+
+	if (!NdisEqualMemory(MIC, TrailMIC, 8))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("RTMPSoftDecryptTKIP, WEP Data MIC Error !\n"));	 //MIC error.
+		//RTMPReportMicError(pAd, &pWpaKey[KeyID]);	// marked by AlbertY @ 20060630
+		return (FALSE);
+	}
+
+#ifdef RT_BIG_ENDIAN
+	RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
+#endif
+	//DBGPRINT(RT_DEBUG_TRACE, "RTMPSoftDecryptTKIP Decript done!!\n");
+	return TRUE;
+}
+
+
+
+
+BOOLEAN RTMPSoftDecryptAES(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR	pData,
+	IN ULONG	DataByteCnt,
+	IN PCIPHER_KEY	pWpaKey)
+{
+	UCHAR			KeyID;
+	UINT			HeaderLen;
+	UCHAR			PN[6];
+	UINT			payload_len;
+	UINT			num_blocks;
+	UINT			payload_remainder;
+	USHORT			fc;
+	UCHAR			fc0;
+	UCHAR			fc1;
+	UINT			frame_type;
+	UINT			frame_subtype;
+	UINT			from_ds;
+	UINT			to_ds;
+	INT				a4_exists;
+	INT				qc_exists;
+	UCHAR			aes_out[16];
+	int 			payload_index;
+	UINT 			i;
+	UCHAR 			ctr_preload[16];
+	UCHAR 			chain_buffer[16];
+	UCHAR 			padded_buffer[16];
+	UCHAR 			mic_iv[16];
+	UCHAR 			mic_header1[16];
+	UCHAR 			mic_header2[16];
+	UCHAR			MIC[8];
+	UCHAR			TrailMIC[8];
+
+#ifdef RT_BIG_ENDIAN
+	RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
+#endif
+
+	fc0 = *pData;
+	fc1 = *(pData + 1);
+
+	fc = *((PUSHORT)pData);
+
+	frame_type = ((fc0 >> 2) & 0x03);
+	frame_subtype = ((fc0 >> 4) & 0x0f);
+
+	from_ds = (fc1 & 0x2) >> 1;
+	to_ds = (fc1 & 0x1);
+
+	a4_exists = (from_ds & to_ds);
+	qc_exists = ((frame_subtype == 0x08) ||    /* Assumed QoS subtypes */
+				  (frame_subtype == 0x09) ||   /* Likely to change.    */
+				  (frame_subtype == 0x0a) ||
+				  (frame_subtype == 0x0b)
+				 );
+
+	HeaderLen = 24;
+	if (a4_exists)
+		HeaderLen += 6;
+
+	KeyID = *((PUCHAR)(pData+ HeaderLen + 3));
+	KeyID = KeyID >> 6;
+
+	if (pWpaKey[KeyID].KeyLen == 0)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptAES failed!(KeyID[%d] Length can not be 0)\n", KeyID));
+		return FALSE;
+	}
+
+	PN[0] = *(pData+ HeaderLen);
+	PN[1] = *(pData+ HeaderLen + 1);
+	PN[2] = *(pData+ HeaderLen + 4);
+	PN[3] = *(pData+ HeaderLen + 5);
+	PN[4] = *(pData+ HeaderLen + 6);
+	PN[5] = *(pData+ HeaderLen + 7);
+
+	payload_len = DataByteCnt - HeaderLen - 8 - 8;	// 8 bytes for CCMP header , 8 bytes for MIC
+	payload_remainder = (payload_len) % 16;
+	num_blocks = (payload_len) / 16;
+
+
+
+	// Find start of payload
+	payload_index = HeaderLen + 8; //IV+EIV
+
+	for (i=0; i< num_blocks; i++)
+	{
+		construct_ctr_preload(ctr_preload,
+								a4_exists,
+								qc_exists,
+								pData,
+								PN,
+								i+1 );
+
+		aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
+
+		bitwise_xor(aes_out, pData + payload_index, chain_buffer);
+		NdisMoveMemory(pData + payload_index - 8, chain_buffer, 16);
+		payload_index += 16;
+	}
+
+	//
+	// If there is a short final block, then pad it
+	// encrypt it and copy the unpadded part back
+	//
+	if (payload_remainder > 0)
+	{
+		construct_ctr_preload(ctr_preload,
+								a4_exists,
+								qc_exists,
+								pData,
+								PN,
+								num_blocks + 1);
+
+		NdisZeroMemory(padded_buffer, 16);
+		NdisMoveMemory(padded_buffer, pData + payload_index, payload_remainder);
+
+		aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
+
+		bitwise_xor(aes_out, padded_buffer, chain_buffer);
+		NdisMoveMemory(pData + payload_index - 8, chain_buffer, payload_remainder);
+		payload_index += payload_remainder;
+	}
+
+	//
+	// Descrypt the MIC
+	//
+	construct_ctr_preload(ctr_preload,
+							a4_exists,
+							qc_exists,
+							pData,
+							PN,
+							0);
+	NdisZeroMemory(padded_buffer, 16);
+	NdisMoveMemory(padded_buffer, pData + payload_index, 8);
+
+	aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
+
+	bitwise_xor(aes_out, padded_buffer, chain_buffer);
+
+	NdisMoveMemory(TrailMIC, chain_buffer, 8);
+
+	//
+	// Calculate MIC
+	//
+
+	//Force the protected frame bit on
+	*(pData + 1) = *(pData + 1) | 0x40;
+
+	// Find start of payload
+	// Because the CCMP header has been removed
+	payload_index = HeaderLen;
+
+	construct_mic_iv(
+					mic_iv,
+					qc_exists,
+					a4_exists,
+					pData,
+					payload_len,
+					PN);
+
+	construct_mic_header1(
+						mic_header1,
+						HeaderLen,
+						pData);
+
+	construct_mic_header2(
+						mic_header2,
+						pData,
+						a4_exists,
+						qc_exists);
+
+	aes128k128d(pWpaKey[KeyID].Key, mic_iv, aes_out);
+	bitwise_xor(aes_out, mic_header1, chain_buffer);
+	aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
+	bitwise_xor(aes_out, mic_header2, chain_buffer);
+	aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
+
+	// iterate through each 16 byte payload block
+	for (i = 0; i < num_blocks; i++)
+	{
+		bitwise_xor(aes_out, pData + payload_index, chain_buffer);
+		payload_index += 16;
+		aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
+	}
+
+	// Add on the final payload block if it needs padding
+	if (payload_remainder > 0)
+	{
+		NdisZeroMemory(padded_buffer, 16);
+		NdisMoveMemory(padded_buffer, pData + payload_index, payload_remainder);
+
+		bitwise_xor(aes_out, padded_buffer, chain_buffer);
+		aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
+	}
+
+	// aes_out contains padded mic, discard most significant
+	// 8 bytes to generate 64 bit MIC
+	for (i = 0 ; i < 8; i++) MIC[i] = aes_out[i];
+
+	if (!NdisEqualMemory(MIC, TrailMIC, 8))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("RTMPSoftDecryptAES, MIC Error !\n"));	 //MIC error.
+		return FALSE;
+	}
+
+#ifdef RT_BIG_ENDIAN
+	RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
+#endif
+
+	return TRUE;
+}
+
+/****************************************/
+/* aes128k128d()                        */
+/* Performs a 128 bit AES encrypt with  */
+/* 128 bit data.                        */
+/****************************************/
+VOID xor_128(
+	IN  PUCHAR  a,
+	IN  PUCHAR  b,
+	OUT PUCHAR  out)
+{
+	INT i;
+
+	for (i=0;i<16; i++)
+	{
+		out[i] = a[i] ^ b[i];
+	}
+}
+
+VOID next_key(
+	IN  PUCHAR  key,
+	IN  INT     round)
+{
+	UCHAR       rcon;
+	UCHAR       sbox_key[4];
+	UCHAR       rcon_table[12] =
+	{
+		0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80,
+		0x1b, 0x36, 0x36, 0x36
+	};
+
+	sbox_key[0] = RTMPCkipSbox(key[13]);
+	sbox_key[1] = RTMPCkipSbox(key[14]);
+	sbox_key[2] = RTMPCkipSbox(key[15]);
+	sbox_key[3] = RTMPCkipSbox(key[12]);
+
+	rcon = rcon_table[round];
+
+	xor_32(&key[0], sbox_key, &key[0]);
+	key[0] = key[0] ^ rcon;
+
+	xor_32(&key[4], &key[0], &key[4]);
+	xor_32(&key[8], &key[4], &key[8]);
+	xor_32(&key[12], &key[8], &key[12]);
+}
+
+VOID xor_32(
+	IN  PUCHAR  a,
+	IN  PUCHAR  b,
+	OUT PUCHAR  out)
+{
+	INT i;
+
+	for (i=0;i<4; i++)
+	{
+		out[i] = a[i] ^ b[i];
+	}
+}
+
+VOID byte_sub(
+	IN  PUCHAR  in,
+	OUT PUCHAR  out)
+{
+	INT i;
+
+	for (i=0; i< 16; i++)
+	{
+		out[i] = RTMPCkipSbox(in[i]);
+	}
+}
+
+UCHAR RTMPCkipSbox(
+	IN  UCHAR   a)
+{
+	return SboxTable[(int)a];
+}
+
+VOID shift_row(
+	IN  PUCHAR  in,
+	OUT PUCHAR  out)
+{
+	out[0] =  in[0];
+	out[1] =  in[5];
+	out[2] =  in[10];
+	out[3] =  in[15];
+	out[4] =  in[4];
+	out[5] =  in[9];
+	out[6] =  in[14];
+	out[7] =  in[3];
+	out[8] =  in[8];
+	out[9] =  in[13];
+	out[10] = in[2];
+	out[11] = in[7];
+	out[12] = in[12];
+	out[13] = in[1];
+	out[14] = in[6];
+	out[15] = in[11];
+}
+
+VOID mix_column(
+	IN  PUCHAR  in,
+	OUT PUCHAR  out)
+{
+	INT         i;
+	UCHAR       add1b[4];
+	UCHAR       add1bf7[4];
+	UCHAR       rotl[4];
+	UCHAR       swap_halfs[4];
+	UCHAR       andf7[4];
+	UCHAR       rotr[4];
+	UCHAR       temp[4];
+	UCHAR       tempb[4];
+
+	for (i=0 ; i<4; i++)
+	{
+		if ((in[i] & 0x80)== 0x80)
+			add1b[i] = 0x1b;
+		else
+			add1b[i] = 0x00;
+	}
+
+	swap_halfs[0] = in[2];    /* Swap halfs */
+	swap_halfs[1] = in[3];
+	swap_halfs[2] = in[0];
+	swap_halfs[3] = in[1];
+
+	rotl[0] = in[3];        /* Rotate left 8 bits */
+	rotl[1] = in[0];
+	rotl[2] = in[1];
+	rotl[3] = in[2];
+
+	andf7[0] = in[0] & 0x7f;
+	andf7[1] = in[1] & 0x7f;
+	andf7[2] = in[2] & 0x7f;
+	andf7[3] = in[3] & 0x7f;
+
+	for (i = 3; i>0; i--)    /* logical shift left 1 bit */
+	{
+		andf7[i] = andf7[i] << 1;
+		if ((andf7[i-1] & 0x80) == 0x80)
+		{
+			andf7[i] = (andf7[i] | 0x01);
+		}
+	}
+	andf7[0] = andf7[0] << 1;
+	andf7[0] = andf7[0] & 0xfe;
+
+	xor_32(add1b, andf7, add1bf7);
+
+	xor_32(in, add1bf7, rotr);
+
+	temp[0] = rotr[0];         /* Rotate right 8 bits */
+	rotr[0] = rotr[1];
+	rotr[1] = rotr[2];
+	rotr[2] = rotr[3];
+	rotr[3] = temp[0];
+
+	xor_32(add1bf7, rotr, temp);
+	xor_32(swap_halfs, rotl,tempb);
+	xor_32(temp, tempb, out);
+}
+
diff --git a/drivers/staging/rt2870/common/rtmp_wep.c b/drivers/staging/rt2870/common/rtmp_wep.c
new file mode 100644
index 00000000000..62f9e58729d
--- /dev/null
+++ b/drivers/staging/rt2870/common/rtmp_wep.c
@@ -0,0 +1,508 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rtmp_wep.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Paul Wu		10-28-02		Initial
+*/
+
+#include "../rt_config.h"
+
+UINT FCSTAB_32[256] =
+{
+	0x00000000, 0x77073096, 0xee0e612c, 0x990951ba,
+	0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3,
+	0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988,
+	0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91,
+	0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de,
+	0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
+	0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec,
+	0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5,
+	0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172,
+	0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b,
+	0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940,
+	0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
+	0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116,
+	0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f,
+	0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924,
+	0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d,
+	0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a,
+	0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
+	0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818,
+	0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01,
+	0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e,
+	0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457,
+	0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c,
+	0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
+	0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2,
+	0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb,
+	0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0,
+	0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9,
+	0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086,
+	0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
+	0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4,
+	0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad,
+	0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a,
+	0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683,
+	0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8,
+	0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
+	0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe,
+	0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7,
+	0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc,
+	0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5,
+	0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252,
+	0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
+	0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60,
+	0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79,
+	0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236,
+	0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f,
+	0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04,
+	0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
+	0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a,
+	0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713,
+	0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38,
+	0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21,
+	0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e,
+	0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
+	0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c,
+	0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45,
+	0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2,
+	0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db,
+	0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0,
+	0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
+	0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6,
+	0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf,
+	0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94,
+	0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d
+};
+
+/*
+UCHAR   WEPKEY[] = {
+		//IV
+		0x00, 0x11, 0x22,
+		//WEP KEY
+		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88, 0x99, 0xAA, 0xBB, 0xCC
+	};
+ */
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Init WEP function.
+
+	Arguments:
+      pAd		Pointer to our adapter
+		pKey        Pointer to the WEP KEY
+		KeyId		   WEP Key ID
+		KeyLen      the length of WEP KEY
+		pDest       Pointer to the destination which Encryption data will store in.
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPInitWepEngine(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pKey,
+	IN	UCHAR			KeyId,
+	IN	UCHAR			KeyLen,
+	IN OUT	PUCHAR		pDest)
+{
+	UINT i;
+	UCHAR   WEPKEY[] = {
+		//IV
+		0x00, 0x11, 0x22,
+		//WEP KEY
+		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88, 0x99, 0xAA, 0xBB, 0xCC
+	};
+
+	pAd->PrivateInfo.FCSCRC32 = PPPINITFCS32;   //Init crc32.
+
+#ifdef CONFIG_STA_SUPPORT
+    if (pAd->StaCfg.bCkipOn && (pAd->StaCfg.CkipFlag & 0x10) && (pAd->OpMode == OPMODE_STA))
+    {
+        ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, pKey, KeyLen);  //INIT SBOX, KEYLEN+3(IV)
+        NdisMoveMemory(pDest, pKey, 3);  //Append Init Vector
+    }
+    else
+#endif // CONFIG_STA_SUPPORT //
+    {
+		NdisMoveMemory(WEPKEY + 3, pKey, KeyLen);
+
+        for(i = 0; i < 3; i++)
+			WEPKEY[i] = RandomByte(pAd);   //Call mlme RandomByte() function.
+		ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, WEPKEY, KeyLen + 3);  //INIT SBOX, KEYLEN+3(IV)
+
+		NdisMoveMemory(pDest, WEPKEY, 3);  //Append Init Vector
+    }
+	*(pDest+3) = (KeyId << 6);       //Append KEYID
+
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Encrypt transimitted data
+
+	Arguments:
+      pAd		Pointer to our adapter
+      pSrc        Pointer to the transimitted source data that will be encrypt
+      pDest       Pointer to the destination where entryption data will be store in.
+      Len			Indicate the length of the source data
+
+	Return Value:
+      None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPEncryptData(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pSrc,
+	IN	PUCHAR			pDest,
+	IN	UINT			Len)
+{
+	pAd->PrivateInfo.FCSCRC32 = RTMP_CALC_FCS32(pAd->PrivateInfo.FCSCRC32, pSrc, Len);
+	ARCFOUR_ENCRYPT(&pAd->PrivateInfo.WEPCONTEXT, pDest, pSrc, Len);
+}
+
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Decrypt received WEP data
+
+	Arguments:
+		pAdapter		Pointer to our adapter
+		pSrc        Pointer to the received data
+		Len         the length of the received data
+
+	Return Value:
+		TRUE        Decrypt WEP data success
+		FALSE       Decrypt WEP data failed
+
+	Note:
+
+	========================================================================
+*/
+BOOLEAN	RTMPSoftDecryptWEP(
+	IN PRTMP_ADAPTER 	pAd,
+	IN PUCHAR			pData,
+	IN ULONG			DataByteCnt,
+	IN PCIPHER_KEY		pGroupKey)
+{
+	UINT	trailfcs;
+	UINT    crc32;
+	UCHAR	KeyIdx;
+	UCHAR   WEPKEY[] = {
+		//IV
+		0x00, 0x11, 0x22,
+		//WEP KEY
+		0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88, 0x99, 0xAA, 0xBB, 0xCC
+	};
+	UCHAR 	*pPayload = (UCHAR *)pData + LENGTH_802_11;
+	ULONG	payload_len = DataByteCnt - LENGTH_802_11;
+
+	NdisMoveMemory(WEPKEY, pPayload, 3);    //Get WEP IV
+
+	KeyIdx = (*(pPayload + 3) & 0xc0) >> 6;
+	if (pGroupKey[KeyIdx].KeyLen == 0)
+		return (FALSE);
+
+	NdisMoveMemory(WEPKEY + 3, pGroupKey[KeyIdx].Key, pGroupKey[KeyIdx].KeyLen);
+	ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, WEPKEY, pGroupKey[KeyIdx].KeyLen + 3);
+	ARCFOUR_DECRYPT(&pAd->PrivateInfo.WEPCONTEXT, pPayload, pPayload + 4, payload_len - 4);
+	NdisMoveMemory(&trailfcs, pPayload + payload_len - 8, 4);
+	crc32 = RTMP_CALC_FCS32(PPPINITFCS32, pPayload, payload_len - 8);  //Skip last 4 bytes(FCS).
+	crc32 ^= 0xffffffff;             /* complement */
+
+    if(crc32 != cpu2le32(trailfcs))
+    {
+		DBGPRINT(RT_DEBUG_TRACE, ("! WEP Data CRC Error !\n"));	 //CRC error.
+		return (FALSE);
+	}
+	return (TRUE);
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		The Stream Cipher Encryption Algorithm "ARCFOUR" initialize
+
+	Arguments:
+	   Ctx         Pointer to ARCFOUR CONTEXT (SBOX)
+		pKey        Pointer to the WEP KEY
+		KeyLen      Indicate the length fo the WEP KEY
+
+	Return Value:
+	   None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	ARCFOUR_INIT(
+	IN	PARCFOURCONTEXT	Ctx,
+	IN	PUCHAR			pKey,
+	IN	UINT			KeyLen)
+{
+	UCHAR	t, u;
+	UINT	keyindex;
+	UINT	stateindex;
+	PUCHAR	state;
+	UINT	counter;
+
+	state = Ctx->STATE;
+	Ctx->X = 0;
+	Ctx->Y = 0;
+	for (counter = 0; counter < 256; counter++)
+		state[counter] = (UCHAR)counter;
+	keyindex = 0;
+	stateindex = 0;
+	for (counter = 0; counter < 256; counter++)
+	{
+		t = state[counter];
+		stateindex = (stateindex + pKey[keyindex] + t) & 0xff;
+		u = state[stateindex];
+		state[stateindex] = t;
+		state[counter] = u;
+		if (++keyindex >= KeyLen)
+			keyindex = 0;
+	}
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Get bytes from ARCFOUR CONTEXT (S-BOX)
+
+	Arguments:
+	   Ctx         Pointer to ARCFOUR CONTEXT (SBOX)
+
+	Return Value:
+	   UCHAR  - the value of the ARCFOUR CONTEXT (S-BOX)
+
+	Note:
+
+	========================================================================
+*/
+UCHAR	ARCFOUR_BYTE(
+	IN	PARCFOURCONTEXT		Ctx)
+{
+  UINT x;
+  UINT y;
+  UCHAR sx, sy;
+  PUCHAR state;
+
+  state = Ctx->STATE;
+  x = (Ctx->X + 1) & 0xff;
+  sx = state[x];
+  y = (sx + Ctx->Y) & 0xff;
+  sy = state[y];
+  Ctx->X = x;
+  Ctx->Y = y;
+  state[y] = sx;
+  state[x] = sy;
+
+  return(state[(sx + sy) & 0xff]);
+
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		The Stream Cipher Decryption Algorithm
+
+	Arguments:
+		Ctx         Pointer to ARCFOUR CONTEXT (SBOX)
+		pDest			Pointer to the Destination
+		pSrc        Pointer to the Source data
+		Len         Indicate the length of the Source data
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	ARCFOUR_DECRYPT(
+	IN	PARCFOURCONTEXT	Ctx,
+	IN	PUCHAR			pDest,
+	IN	PUCHAR			pSrc,
+	IN	UINT			Len)
+{
+	UINT i;
+
+	for (i = 0; i < Len; i++)
+		pDest[i] = pSrc[i] ^ ARCFOUR_BYTE(Ctx);
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		The Stream Cipher Encryption Algorithm
+
+	Arguments:
+		Ctx         Pointer to ARCFOUR CONTEXT (SBOX)
+		pDest			Pointer to the Destination
+		pSrc        Pointer to the Source data
+		Len         Indicate the length of the Source dta
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	ARCFOUR_ENCRYPT(
+	IN	PARCFOURCONTEXT	Ctx,
+	IN	PUCHAR			pDest,
+	IN	PUCHAR			pSrc,
+	IN	UINT			Len)
+{
+	UINT i;
+
+	for (i = 0; i < Len; i++)
+		pDest[i] = pSrc[i] ^ ARCFOUR_BYTE(Ctx);
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		The Stream Cipher Encryption Algorithm which conform to the special requirement to encrypt  GTK.
+
+	Arguments:
+		Ctx         Pointer to ARCFOUR CONTEXT (SBOX)
+		pDest			Pointer to the Destination
+		pSrc        Pointer to the Source data
+		Len         Indicate the length of the Source dta
+
+
+	========================================================================
+*/
+
+VOID	WPAARCFOUR_ENCRYPT(
+	IN	PARCFOURCONTEXT	Ctx,
+	IN	PUCHAR			pDest,
+	IN	PUCHAR			pSrc,
+	IN	UINT			Len)
+{
+	UINT i;
+        //discard first 256 bytes
+	for (i = 0; i < 256; i++)
+            ARCFOUR_BYTE(Ctx);
+
+	for (i = 0; i < Len; i++)
+		pDest[i] = pSrc[i] ^ ARCFOUR_BYTE(Ctx);
+}
+
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Calculate a new FCS given the current FCS and the new data.
+
+	Arguments:
+		Fcs	      the original FCS value
+		Cp          pointer to the data which will be calculate the FCS
+		Len         the length of the data
+
+	Return Value:
+		UINT - FCS 32 bits
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+UINT	RTMP_CALC_FCS32(
+	IN	UINT	Fcs,
+	IN	PUCHAR	Cp,
+	IN	INT		Len)
+{
+	while (Len--)
+	   Fcs = (((Fcs) >> 8) ^ FCSTAB_32[((Fcs) ^ (*Cp++)) & 0xff]);
+
+	return (Fcs);
+}
+
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Get last FCS and encrypt it to the destination
+
+	Arguments:
+		pDest			Pointer to the Destination
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPSetICV(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR	pDest)
+{
+	pAd->PrivateInfo.FCSCRC32 ^= 0xffffffff;             /* complement */
+	pAd->PrivateInfo.FCSCRC32 = cpu2le32(pAd->PrivateInfo.FCSCRC32);
+
+	ARCFOUR_ENCRYPT(&pAd->PrivateInfo.WEPCONTEXT, pDest, (PUCHAR) &pAd->PrivateInfo.FCSCRC32, 4);
+}
+
diff --git a/drivers/staging/rt2870/common/rtusb_bulk.c b/drivers/staging/rt2870/common/rtusb_bulk.c
new file mode 100644
index 00000000000..3c6ba1b9deb
--- /dev/null
+++ b/drivers/staging/rt2870/common/rtusb_bulk.c
@@ -0,0 +1,1981 @@
+ /*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rtusb_bulk.c
+
+	Abstract:
+
+	Revision History:
+	Who			When		What
+	--------	----------	----------------------------------------------
+	Name		Date		Modification logs
+	Paul Lin	06-25-2004	created
+
+*/
+
+#include "../rt_config.h"
+// Match total 6 bulkout endpoint to corresponding queue.
+UCHAR	EpToQueue[6]={FIFO_EDCA, FIFO_EDCA, FIFO_EDCA, FIFO_EDCA, FIFO_EDCA, FIFO_MGMT};
+
+//static BOOLEAN SingleBulkOut = FALSE;
+
+void RTUSB_FILL_BULK_URB (struct urb *pUrb,
+	struct usb_device *pUsb_Dev,
+	unsigned int bulkpipe,
+	void *pTransferBuf,
+	int BufSize,
+	usb_complete_t Complete,
+	void *pContext)
+{
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+	usb_fill_bulk_urb(pUrb, pUsb_Dev, bulkpipe, pTransferBuf, BufSize, (usb_complete_t)Complete, pContext);
+#else
+	FILL_BULK_URB(pUrb, pUsb_Dev, bulkpipe, pTransferBuf, BufSize, Complete, pContext);
+#endif
+
+}
+
+VOID	RTUSBInitTxDesc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTX_CONTEXT		pTxContext,
+	IN	UCHAR			BulkOutPipeId,
+	IN	usb_complete_t	Func)
+{
+	PURB				pUrb;
+	PUCHAR				pSrc = NULL;
+	POS_COOKIE			pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	pUrb = pTxContext->pUrb;
+	ASSERT(pUrb);
+
+	// Store BulkOut PipeId
+	pTxContext->BulkOutPipeId = BulkOutPipeId;
+
+	if (pTxContext->bAggregatible)
+	{
+		pSrc = &pTxContext->TransferBuffer->Aggregation[2];
+	}
+	else
+	{
+		pSrc = (PUCHAR) pTxContext->TransferBuffer->field.WirelessPacket;
+	}
+
+
+	//Initialize a tx bulk urb
+	RTUSB_FILL_BULK_URB(pUrb,
+						pObj->pUsb_Dev,
+						usb_sndbulkpipe(pObj->pUsb_Dev, pAd->BulkOutEpAddr[BulkOutPipeId]),
+						pSrc,
+						pTxContext->BulkOutSize,
+						Func,
+						pTxContext);
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+	if (pTxContext->bAggregatible)
+		pUrb->transfer_dma	= (pTxContext->data_dma + TX_BUFFER_NORMSIZE + 2);
+	else
+		pUrb->transfer_dma	= pTxContext->data_dma;
+
+	pUrb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
+#endif
+
+}
+
+VOID	RTUSBInitHTTxDesc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PHT_TX_CONTEXT	pTxContext,
+	IN	UCHAR			BulkOutPipeId,
+	IN	ULONG			BulkOutSize,
+	IN	usb_complete_t	Func)
+{
+	PURB				pUrb;
+	PUCHAR				pSrc = NULL;
+	POS_COOKIE			pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	pUrb = pTxContext->pUrb;
+	ASSERT(pUrb);
+
+	// Store BulkOut PipeId
+	pTxContext->BulkOutPipeId = BulkOutPipeId;
+
+	pSrc = &pTxContext->TransferBuffer->field.WirelessPacket[pTxContext->NextBulkOutPosition];
+
+
+	//Initialize a tx bulk urb
+	RTUSB_FILL_BULK_URB(pUrb,
+						pObj->pUsb_Dev,
+						usb_sndbulkpipe(pObj->pUsb_Dev, pAd->BulkOutEpAddr[BulkOutPipeId]),
+						pSrc,
+						BulkOutSize,
+						Func,
+						pTxContext);
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+	pUrb->transfer_dma	= (pTxContext->data_dma + pTxContext->NextBulkOutPosition);
+	pUrb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
+#endif
+
+}
+
+VOID	RTUSBInitRxDesc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PRX_CONTEXT		pRxContext)
+{
+	PURB				pUrb;
+	POS_COOKIE			pObj = (POS_COOKIE) pAd->OS_Cookie;
+	ULONG				RX_bulk_size;
+
+
+	pUrb = pRxContext->pUrb;
+	ASSERT(pUrb);
+
+	if ( pAd->BulkInMaxPacketSize == 64)
+		RX_bulk_size = 4096;
+	else
+		RX_bulk_size = MAX_RXBULK_SIZE;
+
+	//Initialize a rx bulk urb
+	RTUSB_FILL_BULK_URB(pUrb,
+						pObj->pUsb_Dev,
+						usb_rcvbulkpipe(pObj->pUsb_Dev, pAd->BulkInEpAddr),
+						&(pRxContext->TransferBuffer[pAd->NextRxBulkInPosition]),
+						RX_bulk_size - (pAd->NextRxBulkInPosition),
+						(usb_complete_t)RTUSBBulkRxComplete,
+						(void *)pRxContext);
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+	pUrb->transfer_dma	= pRxContext->data_dma + pAd->NextRxBulkInPosition;
+	pUrb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
+#endif
+
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+
+#define BULK_OUT_LOCK(pLock, IrqFlags)	\
+		if(1 /*!(in_interrupt() & 0xffff0000)*/)	\
+			RTMP_IRQ_LOCK((pLock), IrqFlags);
+
+#define BULK_OUT_UNLOCK(pLock, IrqFlags)	\
+		if(1 /*!(in_interrupt() & 0xffff0000)*/)	\
+			RTMP_IRQ_UNLOCK((pLock), IrqFlags);
+
+
+VOID	RTUSBBulkOutDataPacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			BulkOutPipeId,
+	IN	UCHAR			Index)
+{
+
+	PHT_TX_CONTEXT	pHTTXContext;
+	PURB			pUrb;
+	int				ret = 0;
+	PTXINFO_STRUC	pTxInfo, pLastTxInfo = NULL;
+	PTXWI_STRUC             pTxWI;
+	ULONG			TmpBulkEndPos, ThisBulkSize;
+	unsigned long	IrqFlags = 0, IrqFlags2 = 0;
+	PUCHAR			pWirelessPkt, pAppendant;
+	BOOLEAN			bTxQLastRound = FALSE;
+	UCHAR			allzero[4]= {0x0,0x0,0x0,0x0};
+
+	BULK_OUT_LOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+	if ((pAd->BulkOutPending[BulkOutPipeId] == TRUE) || RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NEED_STOP_TX))
+	{
+		BULK_OUT_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+		return;
+	}
+	pAd->BulkOutPending[BulkOutPipeId] = TRUE;
+
+	if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED)
+		)
+	{
+		pAd->BulkOutPending[BulkOutPipeId] = FALSE;
+		BULK_OUT_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+		return;
+	}
+	BULK_OUT_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+
+
+	pHTTXContext = &(pAd->TxContext[BulkOutPipeId]);
+
+	BULK_OUT_LOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags2);
+	if ((pHTTXContext->ENextBulkOutPosition == pHTTXContext->CurWritePosition)
+		|| ((pHTTXContext->ENextBulkOutPosition-8) == pHTTXContext->CurWritePosition))
+	{
+		BULK_OUT_UNLOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags2);
+
+		BULK_OUT_LOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+		pAd->BulkOutPending[BulkOutPipeId] = FALSE;
+
+		// Clear Data flag
+		RTUSB_CLEAR_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_FRAG << BulkOutPipeId));
+		RTUSB_CLEAR_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_NORMAL << BulkOutPipeId));
+
+		BULK_OUT_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+		return;
+	}
+
+	// Clear Data flag
+	RTUSB_CLEAR_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_FRAG << BulkOutPipeId));
+	RTUSB_CLEAR_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_NORMAL << BulkOutPipeId));
+
+	//DBGPRINT(RT_DEBUG_TRACE,("BulkOut-B:I=0x%lx, CWPos=%ld, CWRPos=%ld, NBPos=%ld, ENBPos=%ld, bCopy=%d!\n", in_interrupt(),
+	//							pHTTXContext->CurWritePosition, pHTTXContext->CurWriteRealPos, pHTTXContext->NextBulkOutPosition,
+	//							pHTTXContext->ENextBulkOutPosition, pHTTXContext->bCopySavePad));
+	pHTTXContext->NextBulkOutPosition = pHTTXContext->ENextBulkOutPosition;
+	ThisBulkSize = 0;
+	TmpBulkEndPos = pHTTXContext->NextBulkOutPosition;
+	pWirelessPkt = &pHTTXContext->TransferBuffer->field.WirelessPacket[0];
+
+	if ((pHTTXContext->bCopySavePad == TRUE))
+	{
+		if (RTMPEqualMemory(pHTTXContext->SavedPad, allzero,4))
+		{
+			DBGPRINT_RAW(RT_DEBUG_ERROR,("e1, allzero : %x  %x  %x  %x  %x  %x  %x  %x \n",
+				pHTTXContext->SavedPad[0], pHTTXContext->SavedPad[1], pHTTXContext->SavedPad[2],pHTTXContext->SavedPad[3]
+				,pHTTXContext->SavedPad[4], pHTTXContext->SavedPad[5], pHTTXContext->SavedPad[6],pHTTXContext->SavedPad[7]));
+		}
+		NdisMoveMemory(&pWirelessPkt[TmpBulkEndPos], pHTTXContext->SavedPad, 8);
+		pHTTXContext->bCopySavePad = FALSE;
+		if (pAd->bForcePrintTX == TRUE)
+			DBGPRINT(RT_DEBUG_TRACE,("RTUSBBulkOutDataPacket --> COPY PAD. CurWrite = %ld, NextBulk = %ld.   ENextBulk = %ld.\n",   pHTTXContext->CurWritePosition, pHTTXContext->NextBulkOutPosition, pHTTXContext->ENextBulkOutPosition));
+	}
+
+	do
+	{
+		pTxInfo = (PTXINFO_STRUC)&pWirelessPkt[TmpBulkEndPos];
+		pTxWI = (PTXWI_STRUC)&pWirelessPkt[TmpBulkEndPos + TXINFO_SIZE];
+
+		if (pAd->bForcePrintTX == TRUE)
+			DBGPRINT(RT_DEBUG_TRACE, ("RTUSBBulkOutDataPacket AMPDU = %d.\n",   pTxWI->AMPDU));
+
+		// add by Iverson, limit BulkOut size to 4k to pass WMM b mode 2T1R test items
+		//if ((ThisBulkSize != 0)  && (pTxWI->AMPDU == 0))
+		if ((ThisBulkSize != 0) && (pTxWI->PHYMODE == MODE_CCK))
+		{
+			if (((ThisBulkSize&0xffff8000) != 0) || ((ThisBulkSize&0x1000) == 0x1000))
+			{
+				// Limit BulkOut size to about 4k bytes.
+				pHTTXContext->ENextBulkOutPosition = TmpBulkEndPos;
+				break;
+			}
+			else if (((pAd->BulkOutMaxPacketSize < 512) && ((ThisBulkSize&0xfffff800) != 0) ) /*|| ( (ThisBulkSize != 0)  && (pTxWI->AMPDU == 0))*/)
+			{
+				// For USB 1.1 or peer which didn't support AMPDU, limit the BulkOut size.
+				// For performence in b/g mode, now just check for USB 1.1 and didn't care about the APMDU or not! 2008/06/04.
+				pHTTXContext->ENextBulkOutPosition = TmpBulkEndPos;
+				break;
+			}
+		}
+		// end Iverson
+		else
+		{
+			if (((ThisBulkSize&0xffff8000) != 0) || ((ThisBulkSize&0x6000) == 0x6000))
+			{	// Limit BulkOut size to about 24k bytes.
+				pHTTXContext->ENextBulkOutPosition = TmpBulkEndPos;
+				break;
+			}
+			else if (((pAd->BulkOutMaxPacketSize < 512) && ((ThisBulkSize&0xfffff800) != 0) ) /*|| ( (ThisBulkSize != 0)  && (pTxWI->AMPDU == 0))*/)
+			{	// For USB 1.1 or peer which didn't support AMPDU, limit the BulkOut size.
+				// For performence in b/g mode, now just check for USB 1.1 and didn't care about the APMDU or not! 2008/06/04.
+				pHTTXContext->ENextBulkOutPosition = TmpBulkEndPos;
+				break;
+			}
+		}
+
+		if (TmpBulkEndPos == pHTTXContext->CurWritePosition)
+		{
+			pHTTXContext->ENextBulkOutPosition = TmpBulkEndPos;
+			break;
+		}
+
+		if (pTxInfo->QSEL != FIFO_EDCA)
+		{
+			printk("%s(): ====> pTxInfo->QueueSel(%d)!= FIFO_EDCA!!!!\n", __FUNCTION__, pTxInfo->QSEL);
+			printk("\tCWPos=%ld, NBPos=%ld, ENBPos=%ld, bCopy=%d!\n", pHTTXContext->CurWritePosition, pHTTXContext->NextBulkOutPosition, pHTTXContext->ENextBulkOutPosition, pHTTXContext->bCopySavePad);
+			hex_dump("Wrong QSel Pkt:", (PUCHAR)&pWirelessPkt[TmpBulkEndPos], (pHTTXContext->CurWritePosition - pHTTXContext->NextBulkOutPosition));
+		}
+
+		if (pTxInfo->USBDMATxPktLen <= 8)
+		{
+			BULK_OUT_UNLOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags2);
+			DBGPRINT(RT_DEBUG_ERROR /*RT_DEBUG_TRACE*/,("e2, USBDMATxPktLen==0, Size=%ld, bCSPad=%d, CWPos=%ld, NBPos=%ld, CWRPos=%ld!\n",
+					pHTTXContext->BulkOutSize, pHTTXContext->bCopySavePad, pHTTXContext->CurWritePosition, pHTTXContext->NextBulkOutPosition, pHTTXContext->CurWriteRealPos));
+			{
+				DBGPRINT_RAW(RT_DEBUG_ERROR /*RT_DEBUG_TRACE*/,("%x  %x  %x  %x  %x  %x  %x  %x \n",
+					pHTTXContext->SavedPad[0], pHTTXContext->SavedPad[1], pHTTXContext->SavedPad[2],pHTTXContext->SavedPad[3]
+					,pHTTXContext->SavedPad[4], pHTTXContext->SavedPad[5], pHTTXContext->SavedPad[6],pHTTXContext->SavedPad[7]));
+			}
+			pAd->bForcePrintTX = TRUE;
+			BULK_OUT_LOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+			pAd->BulkOutPending[BulkOutPipeId] = FALSE;
+			BULK_OUT_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+			//DBGPRINT(RT_DEBUG_LOUD,("Out:pTxInfo->USBDMATxPktLen=%d!\n", pTxInfo->USBDMATxPktLen));
+			return;
+		}
+
+			// Increase Total transmit byte counter
+		pAd->RalinkCounters.OneSecTransmittedByteCount +=  pTxWI->MPDUtotalByteCount;
+		pAd->RalinkCounters.TransmittedByteCount +=  pTxWI->MPDUtotalByteCount;
+
+		pLastTxInfo = pTxInfo;
+
+		// Make sure we use EDCA QUEUE.
+		pTxInfo->QSEL = FIFO_EDCA;
+		ThisBulkSize += (pTxInfo->USBDMATxPktLen+4);
+		TmpBulkEndPos += (pTxInfo->USBDMATxPktLen+4);
+
+		if (TmpBulkEndPos != pHTTXContext->CurWritePosition)
+			pTxInfo->USBDMANextVLD = 1;
+
+		if (pTxInfo->SwUseLastRound == 1)
+		{
+			if (pHTTXContext->CurWritePosition == 8)
+				pTxInfo->USBDMANextVLD = 0;
+			pTxInfo->SwUseLastRound = 0;
+
+			bTxQLastRound = TRUE;
+			pHTTXContext->ENextBulkOutPosition = 8;
+
+	#ifdef RT_BIG_ENDIAN
+			RTMPDescriptorEndianChange((PUCHAR)pTxInfo, TYPE_TXINFO);
+			RTMPWIEndianChange((PUCHAR)pTxWI, TYPE_TXWI);
+	#endif // RT_BIG_ENDIAN //
+
+			break;
+		}
+
+#ifdef RT_BIG_ENDIAN
+		RTMPDescriptorEndianChange((PUCHAR)pTxInfo, TYPE_TXINFO);
+		RTMPWIEndianChange((PUCHAR)pTxWI, TYPE_TXWI);
+#endif // RT_BIG_ENDIAN //
+
+	}while (TRUE);
+
+	// adjust the pTxInfo->USBDMANextVLD value of last pTxInfo.
+	if (pLastTxInfo)
+	{
+#ifdef RT_BIG_ENDIAN
+		RTMPDescriptorEndianChange((PUCHAR)pLastTxInfo, TYPE_TXINFO);
+#endif // RT_BIG_ENDIAN //
+		pLastTxInfo->USBDMANextVLD = 0;
+#ifdef RT_BIG_ENDIAN
+		RTMPDescriptorEndianChange((PUCHAR)pLastTxInfo, TYPE_TXINFO);
+#endif // RT_BIG_ENDIAN //
+	}
+
+	/*
+		We need to copy SavedPad when following condition matched!
+			1. Not the last round of the TxQueue and
+			2. any match of following cases:
+				(1). The End Position of this bulk out is reach to the Currenct Write position and
+						the TxInfo and related header already write to the CurWritePosition.
+			   		=>(ENextBulkOutPosition == CurWritePosition) && (CurWriteRealPos > CurWritePosition)
+
+				(2). The EndPosition of the bulk out is not reach to the Current Write Position.
+					=>(ENextBulkOutPosition != CurWritePosition)
+	*/
+	if ((bTxQLastRound == FALSE) &&
+		 (((pHTTXContext->ENextBulkOutPosition == pHTTXContext->CurWritePosition) && (pHTTXContext->CurWriteRealPos > pHTTXContext->CurWritePosition)) ||
+		  (pHTTXContext->ENextBulkOutPosition != pHTTXContext->CurWritePosition))
+		)
+	{
+		NdisMoveMemory(pHTTXContext->SavedPad, &pWirelessPkt[pHTTXContext->ENextBulkOutPosition], 8);
+		pHTTXContext->bCopySavePad = TRUE;
+		if (RTMPEqualMemory(pHTTXContext->SavedPad, allzero,4))
+		{
+			PUCHAR	pBuf = &pHTTXContext->SavedPad[0];
+			DBGPRINT_RAW(RT_DEBUG_ERROR,("WARNING-Zero-3:%02x%02x%02x%02x%02x%02x%02x%02x,CWPos=%ld, CWRPos=%ld, bCW=%d, NBPos=%ld, TBPos=%ld, TBSize=%ld\n",
+				pBuf[0], pBuf[1], pBuf[2],pBuf[3],pBuf[4], pBuf[5], pBuf[6],pBuf[7], pHTTXContext->CurWritePosition, pHTTXContext->CurWriteRealPos,
+				pHTTXContext->bCurWriting, pHTTXContext->NextBulkOutPosition, TmpBulkEndPos, ThisBulkSize));
+
+			pBuf = &pWirelessPkt[pHTTXContext->CurWritePosition];
+			DBGPRINT_RAW(RT_DEBUG_ERROR,("\tCWPos=%02x%02x%02x%02x%02x%02x%02x%02x\n", pBuf[0], pBuf[1], pBuf[2],pBuf[3],pBuf[4], pBuf[5], pBuf[6],pBuf[7]));
+		}
+		//DBGPRINT(RT_DEBUG_LOUD,("ENPos==CWPos=%ld, CWRPos=%ld, bCSPad=%d!\n", pHTTXContext->CurWritePosition, pHTTXContext->CurWriteRealPos, pHTTXContext->bCopySavePad));
+	}
+
+	if (pAd->bForcePrintTX == TRUE)
+		DBGPRINT(RT_DEBUG_TRACE,("BulkOut-A:Size=%ld, CWPos=%ld, NBPos=%ld, ENBPos=%ld, bCopy=%d!\n", ThisBulkSize, pHTTXContext->CurWritePosition, pHTTXContext->NextBulkOutPosition, pHTTXContext->ENextBulkOutPosition, pHTTXContext->bCopySavePad));
+	//DBGPRINT(RT_DEBUG_LOUD,("BulkOut-A:Size=%ld, CWPos=%ld, CWRPos=%ld, NBPos=%ld, ENBPos=%ld, bCopy=%d, bLRound=%d!\n", ThisBulkSize, pHTTXContext->CurWritePosition, pHTTXContext->CurWriteRealPos, pHTTXContext->NextBulkOutPosition, pHTTXContext->ENextBulkOutPosition, pHTTXContext->bCopySavePad, bTxQLastRound));
+
+		// USB DMA engine requires to pad extra 4 bytes. This pad doesn't count into real bulkoutsize.
+	pAppendant = &pWirelessPkt[TmpBulkEndPos];
+	NdisZeroMemory(pAppendant, 8);
+		ThisBulkSize += 4;
+		pHTTXContext->LastOne = TRUE;
+		if ((ThisBulkSize % pAd->BulkOutMaxPacketSize) == 0)
+			ThisBulkSize += 4;
+	pHTTXContext->BulkOutSize = ThisBulkSize;
+
+	pAd->watchDogTxPendingCnt[BulkOutPipeId] = 1;
+	BULK_OUT_UNLOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags2);
+
+	// Init Tx context descriptor
+	RTUSBInitHTTxDesc(pAd, pHTTXContext, BulkOutPipeId, ThisBulkSize, (usb_complete_t)RTUSBBulkOutDataPacketComplete);
+
+	pUrb = pHTTXContext->pUrb;
+	if((ret = RTUSB_SUBMIT_URB(pUrb))!=0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("RTUSBBulkOutDataPacket: Submit Tx URB failed %d\n", ret));
+
+		BULK_OUT_LOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+		pAd->BulkOutPending[BulkOutPipeId] = FALSE;
+		pAd->watchDogTxPendingCnt[BulkOutPipeId] = 0;
+		BULK_OUT_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+
+		return;
+	}
+
+	BULK_OUT_LOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+	pHTTXContext->IRPPending = TRUE;
+	BULK_OUT_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+	pAd->BulkOutReq++;
+
+}
+
+
+VOID RTUSBBulkOutDataPacketComplete(purbb_t pUrb, struct pt_regs *pt_regs)
+{
+#if 0 // sample, IRQ LOCK
+	PRTMP_ADAPTER		pAd;
+	POS_COOKIE			pObj;
+	PHT_TX_CONTEXT		pHTTXContext;
+	UCHAR				BulkOutPipeId;
+	NTSTATUS			Status;
+	unsigned long		IrqFlags;
+
+	DBGPRINT_RAW(RT_DEBUG_INFO, ("--->RTUSBBulkOutDataPacketComplete\n"));
+
+	pHTTXContext	= (PHT_TX_CONTEXT)pUrb->context;
+	pAd			= pHTTXContext->pAd;
+	pObj 		= (POS_COOKIE) pAd->OS_Cookie;
+	Status		= pUrb->status;
+
+	// Store BulkOut PipeId
+	BulkOutPipeId = pHTTXContext->BulkOutPipeId;
+	pAd->BulkOutDataOneSecCount++;
+
+	//DBGPRINT(RT_DEBUG_LOUD, ("Done-B(%d):I=0x%lx, CWPos=%ld, NBPos=%ld, ENBPos=%ld, bCopy=%d!\n", BulkOutPipeId, in_interrupt(), pHTTXContext->CurWritePosition,
+	//		pHTTXContext->NextBulkOutPosition, pHTTXContext->ENextBulkOutPosition, pHTTXContext->bCopySavePad));
+
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+	pAd->BulkOutPending[BulkOutPipeId] = FALSE;
+	pHTTXContext->IRPPending = FALSE;
+	RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+
+	if (Status == USB_ST_NOERROR)
+	{
+		pAd->BulkOutComplete++;
+
+		pAd->Counters8023.GoodTransmits++;
+		//RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+		FREE_HTTX_RING(pAd, BulkOutPipeId, pHTTXContext);
+		//RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+
+
+	}
+	else	// STATUS_OTHER
+	{
+		PUCHAR	pBuf;
+
+		pAd->BulkOutCompleteOther++;
+
+		pBuf = &pHTTXContext->TransferBuffer->WirelessPacket[pHTTXContext->NextBulkOutPosition];
+
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("BulkOutDataPacket failed: ReasonCode=%d!\n", Status));
+		DBGPRINT_RAW(RT_DEBUG_ERROR, (">>BulkOut Req=0x%lx, Complete=0x%lx, Other=0x%lx\n", pAd->BulkOutReq, pAd->BulkOutComplete, pAd->BulkOutCompleteOther));
+		DBGPRINT_RAW(RT_DEBUG_ERROR, (">>BulkOut Header:%x %x %x %x %x %x %x %x\n", pBuf[0], pBuf[1], pBuf[2], pBuf[3], pBuf[4], pBuf[5], pBuf[6], pBuf[7]));
+		//DBGPRINT_RAW(RT_DEBUG_ERROR, (">>BulkOutCompleteCancel=0x%x, BulkOutCompleteOther=0x%x\n", pAd->BulkOutCompleteCancel, pAd->BulkOutCompleteOther));
+
+		if (!RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_RESET_IN_PROGRESS |
+									fRTMP_ADAPTER_HALT_IN_PROGRESS |
+									fRTMP_ADAPTER_NIC_NOT_EXIST |
+									fRTMP_ADAPTER_BULKOUT_RESET)))
+		{
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+			pAd->bulkResetPipeid = BulkOutPipeId;
+		}
+	}
+
+	//
+	// bInUse = TRUE, means some process are filling TX data, after that must turn on bWaitingBulkOut
+	// bWaitingBulkOut = TRUE, means the TX data are waiting for bulk out.
+	//
+	//RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+	if ((pHTTXContext->ENextBulkOutPosition != pHTTXContext->CurWritePosition) &&
+		(pHTTXContext->ENextBulkOutPosition != (pHTTXContext->CurWritePosition+8)) &&
+		!RTUSB_TEST_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_FRAG << BulkOutPipeId)))
+	{
+		// Indicate There is data avaliable
+		RTUSB_SET_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_NORMAL << BulkOutPipeId));
+	}
+	//RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+
+	// Always call Bulk routine, even reset bulk.
+	// The protection of rest bulk should be in BulkOut routine
+	RTUSBKickBulkOut(pAd);
+
+
+	//DBGPRINT(RT_DEBUG_LOUD,("Done-A(%d):I=0x%lx, CWPos=%ld, NBPos=%ld, ENBPos=%ld, bCopy=%d\n", BulkOutPipeId, in_interrupt(),
+	//		pHTTXContext->CurWritePosition, pHTTXContext->NextBulkOutPosition, pHTTXContext->ENextBulkOutPosition, pHTTXContext->bCopySavePad));
+
+	switch (BulkOutPipeId)
+	{
+		case 0:
+				pObj->ac0_dma_done_task.data = (unsigned long)pAd;
+				tasklet_hi_schedule(&pObj->ac0_dma_done_task);
+				break;
+		case 1:
+				pObj->ac1_dma_done_task.data = (unsigned long)pAd;
+				tasklet_hi_schedule(&pObj->ac1_dma_done_task);
+				break;
+		case 2:
+				pObj->ac2_dma_done_task.data = (unsigned long)pAd;
+				tasklet_hi_schedule(&pObj->ac2_dma_done_task);
+				break;
+		case 3:
+				pObj->ac3_dma_done_task.data = (unsigned long)pAd;
+				tasklet_hi_schedule(&pObj->ac3_dma_done_task);
+				break;
+		case 4:
+				pObj->hcca_dma_done_task.data = (unsigned long)pAd;
+				tasklet_hi_schedule(&pObj->hcca_dma_done_task);
+				break;
+	}
+#else
+
+{
+	PHT_TX_CONTEXT	pHTTXContext;
+	PRTMP_ADAPTER	pAd;
+	POS_COOKIE 		pObj;
+	UCHAR			BulkOutPipeId;
+
+
+	pHTTXContext	= (PHT_TX_CONTEXT)pUrb->context;
+	pAd 			= pHTTXContext->pAd;
+	pObj 			= (POS_COOKIE) pAd->OS_Cookie;
+
+	// Store BulkOut PipeId
+	BulkOutPipeId	= pHTTXContext->BulkOutPipeId;
+	pAd->BulkOutDataOneSecCount++;
+
+	switch (BulkOutPipeId)
+	{
+		case 0:
+				pObj->ac0_dma_done_task.data = (unsigned long)pUrb;
+				tasklet_hi_schedule(&pObj->ac0_dma_done_task);
+				break;
+		case 1:
+				pObj->ac1_dma_done_task.data = (unsigned long)pUrb;
+				tasklet_hi_schedule(&pObj->ac1_dma_done_task);
+				break;
+		case 2:
+				pObj->ac2_dma_done_task.data = (unsigned long)pUrb;
+				tasklet_hi_schedule(&pObj->ac2_dma_done_task);
+				break;
+		case 3:
+				pObj->ac3_dma_done_task.data = (unsigned long)pUrb;
+				tasklet_hi_schedule(&pObj->ac3_dma_done_task);
+				break;
+		case 4:
+				pObj->hcca_dma_done_task.data = (unsigned long)pUrb;
+				tasklet_hi_schedule(&pObj->hcca_dma_done_task);
+				break;
+	}
+}
+#endif
+
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	Note: NULL frame use BulkOutPipeId = 0
+
+	========================================================================
+*/
+VOID	RTUSBBulkOutNullFrame(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	PTX_CONTEXT		pNullContext = &(pAd->NullContext);
+	PURB			pUrb;
+	int				ret = 0;
+	unsigned long	IrqFlags;
+
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[0], IrqFlags);
+	if ((pAd->BulkOutPending[0] == TRUE) || RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NEED_STOP_TX))
+	{
+		RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], IrqFlags);
+		return;
+	}
+	pAd->BulkOutPending[0] = TRUE;
+	pAd->watchDogTxPendingCnt[0] = 1;
+	pNullContext->IRPPending = TRUE;
+	RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], IrqFlags);
+
+	// Increase Total transmit byte counter
+	pAd->RalinkCounters.TransmittedByteCount +=  pNullContext->BulkOutSize;
+
+
+	// Clear Null frame bulk flag
+	RTUSB_CLEAR_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NULL);
+
+#ifdef RT_BIG_ENDIAN
+	RTMPDescriptorEndianChange((PUCHAR)pNullContext->TransferBuffer, TYPE_TXINFO);
+#endif // RT_BIG_ENDIAN //
+
+	// Init Tx context descriptor
+	RTUSBInitTxDesc(pAd, pNullContext, 0, (usb_complete_t)RTUSBBulkOutNullFrameComplete);
+
+	pUrb = pNullContext->pUrb;
+	if((ret = RTUSB_SUBMIT_URB(pUrb))!=0)
+	{
+		RTMP_IRQ_LOCK(&pAd->BulkOutLock[0], IrqFlags);
+		pAd->BulkOutPending[0] = FALSE;
+		pAd->watchDogTxPendingCnt[0] = 0;
+		pNullContext->IRPPending = FALSE;
+		RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], IrqFlags);
+
+		DBGPRINT(RT_DEBUG_ERROR, ("RTUSBBulkOutNullFrame: Submit Tx URB failed %d\n", ret));
+		return;
+	}
+
+}
+
+// NULL frame use BulkOutPipeId = 0
+VOID RTUSBBulkOutNullFrameComplete(purbb_t pUrb, struct pt_regs *pt_regs)
+{
+	PRTMP_ADAPTER		pAd;
+	PTX_CONTEXT			pNullContext;
+	NTSTATUS			Status;
+#if 0 // sample, IRQ LOCK
+	unsigned long		IrqFlags;
+#endif
+	POS_COOKIE			pObj;
+
+
+	pNullContext	= (PTX_CONTEXT)pUrb->context;
+	pAd 			= pNullContext->pAd;
+	Status 			= pUrb->status;
+
+#if 0 // sample, IRQ LOCK
+	// Reset Null frame context flags
+	pNullContext->IRPPending 	= FALSE;
+	pNullContext->InUse 		= FALSE;
+
+	if (Status == USB_ST_NOERROR)
+	{
+		// Don't worry about the queue is empty or not, this function will check itself
+		//RTMPUSBDeQueuePacket(pAd, 0);
+		RTMPDeQueuePacket(pAd, TRUE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+	}
+	else	// STATUS_OTHER
+	{
+		if ((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET)))
+		{
+			DBGPRINT_RAW(RT_DEBUG_ERROR, ("Bulk Out Null Frame Failed\n"));
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+		}
+	}
+
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[0], IrqFlags);
+	pAd->BulkOutPending[0] = FALSE;
+	RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], IrqFlags);
+
+	// Always call Bulk routine, even reset bulk.
+	// The protectioon of rest bulk should be in BulkOut routine
+	RTUSBKickBulkOut(pAd);
+#else
+
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+	pObj->null_frame_complete_task.data = (unsigned long)pUrb;
+	tasklet_hi_schedule(&pObj->null_frame_complete_task);
+#endif
+
+}
+
+#if 0	// For RT2870, RTS frame not used now, but maybe will use it latter.
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	Note: RTS frame use BulkOutPipeId = 0
+
+	========================================================================
+*/
+VOID	RTUSBBulkOutRTSFrame(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	PTX_CONTEXT		pRTSContext = &(pAd->RTSContext);
+	PURB			pUrb;
+	int				ret = 0;
+	unsigned long	IrqFlags;
+	UCHAR			PipeID=0;
+
+	if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL_4))
+		PipeID= 3;
+	else if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL_3))
+		PipeID= 2;
+	else if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL_2))
+		PipeID= 1;
+	else if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL))
+		PipeID= 0;
+
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[PipeID], IrqFlags);
+	if ((pAd->BulkOutPending[PipeID] == TRUE) || RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NEED_STOP_TX))
+	{
+		RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[PipeID], IrqFlags);
+		return;
+	}
+	pAd->BulkOutPending[PipeID] = TRUE;
+	RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[PipeID], IrqFlags);
+
+	// Increase Total transmit byte counter
+	pAd->RalinkCounters.TransmittedByteCount +=  pRTSContext->BulkOutSize;
+
+	DBGPRINT_RAW(RT_DEBUG_INFO, ("--->RTUSBBulkOutRTSFrame \n"));
+
+	// Clear RTS frame bulk flag
+	RTUSB_CLEAR_BULK_FLAG(pAd, fRTUSB_BULK_OUT_RTS);
+
+#ifdef RT_BIG_ENDIAN
+	RTMPDescriptorEndianChange((PUCHAR)pRTSContext->TransferBuffer, TYPE_TXINFO);
+#endif // RT_BIG_ENDIAN //
+
+	// Init Tx context descriptor
+	RTUSBInitTxDesc(pAd, pRTSContext, PipeID, (usb_complete_t)RTUSBBulkOutRTSFrameComplete);
+	pRTSContext->IRPPending = TRUE;
+
+	pUrb = pRTSContext->pUrb;
+	if((ret = RTUSB_SUBMIT_URB(pUrb))!=0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("RTUSBBulkOutRTSFrame: Submit Tx URB failed %d\n", ret));
+		return;
+	}
+
+	DBGPRINT_RAW(RT_DEBUG_INFO, ("<---RTUSBBulkOutRTSFrame \n"));
+
+}
+
+// RTS frame use BulkOutPipeId = 0
+VOID RTUSBBulkOutRTSFrameComplete(purbb_t pUrb, struct pt_regs *pt_regs)
+{
+	PRTMP_ADAPTER	pAd;
+	PTX_CONTEXT		pRTSContext;
+	NTSTATUS		Status;
+#if 0 // sample, IRQ LOCK
+	unsigned long	IrqFlags;
+#endif
+	POS_COOKIE		pObj;
+
+	DBGPRINT_RAW(RT_DEBUG_INFO, ("--->RTUSBBulkOutRTSFrameComplete\n"));
+
+	pRTSContext	= (PTX_CONTEXT)pUrb->context;
+	pAd			= pRTSContext->pAd;
+	Status		= pUrb->status;
+
+#if 0 // sample, IRQ LOCK
+	// Reset RTS frame context flags
+	pRTSContext->IRPPending = FALSE;
+	pRTSContext->InUse		= FALSE;
+
+	if (Status == USB_ST_NOERROR)
+	{
+		// Don't worry about the queue is empty or not, this function will check itself
+		//RTMPUSBDeQueuePacket(pAd, pRTSContext->BulkOutPipeId);
+		RTMPDeQueuePacket(pAd, TRUE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+	}
+	else	// STATUS_OTHER
+	{
+		if ((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET)))
+		{
+			DBGPRINT_RAW(RT_DEBUG_ERROR, ("Bulk Out RTS Frame Failed\n"));
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+		}
+	}
+
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[pRTSContext->BulkOutPipeId], IrqFlags);
+	pAd->BulkOutPending[pRTSContext->BulkOutPipeId] = FALSE;
+	RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[pRTSContext->BulkOutPipeId], IrqFlags);
+
+	// Always call Bulk routine, even reset bulk.
+	// The protectioon of rest bulk should be in BulkOut routine
+	RTUSBKickBulkOut(pAd);
+#else
+
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+	pObj->rts_frame_complete_task.data = (unsigned long)pUrb;
+	tasklet_hi_schedule(&pObj->rts_frame_complete_task);
+#endif
+
+	DBGPRINT_RAW(RT_DEBUG_INFO, ("<---RTUSBBulkOutRTSFrameComplete\n"));
+
+}
+#endif
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	Note: MLME use BulkOutPipeId = 0
+
+	========================================================================
+*/
+VOID	RTUSBBulkOutMLMEPacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Index)
+{
+	PTX_CONTEXT		pMLMEContext;
+	PURB			pUrb;
+	int				ret = 0;
+	unsigned long	IrqFlags;
+
+	pMLMEContext = (PTX_CONTEXT)pAd->MgmtRing.Cell[pAd->MgmtRing.TxDmaIdx].AllocVa;
+	pUrb = pMLMEContext->pUrb;
+
+	if ((pAd->MgmtRing.TxSwFreeIdx >= MGMT_RING_SIZE) ||
+		(pMLMEContext->InUse == FALSE) ||
+		(pMLMEContext->bWaitingBulkOut == FALSE))
+	{
+
+
+		// Clear MLME bulk flag
+		RTUSB_CLEAR_BULK_FLAG(pAd, fRTUSB_BULK_OUT_MLME);
+
+		return;
+	}
+
+
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[MGMTPIPEIDX], IrqFlags);
+	if ((pAd->BulkOutPending[MGMTPIPEIDX] == TRUE) || RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NEED_STOP_TX))
+	{
+		RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[MGMTPIPEIDX], IrqFlags);
+		return;
+	}
+
+	pAd->BulkOutPending[MGMTPIPEIDX] = TRUE;
+	pAd->watchDogTxPendingCnt[MGMTPIPEIDX] = 1;
+	pMLMEContext->IRPPending = TRUE;
+	pMLMEContext->bWaitingBulkOut = FALSE;
+	RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[MGMTPIPEIDX], IrqFlags);
+
+	// Increase Total transmit byte counter
+	pAd->RalinkCounters.TransmittedByteCount +=  pMLMEContext->BulkOutSize;
+
+	// Clear MLME bulk flag
+	RTUSB_CLEAR_BULK_FLAG(pAd, fRTUSB_BULK_OUT_MLME);
+
+
+	//DBGPRINT_RAW(RT_DEBUG_INFO, ("--->RTUSBBulkOutMLMEPacket\n"));
+#if 0 // for debug
+{
+	printk("MLME-Out, C=%d!, D=%d, F=%d!\n", pAd->MgmtRing.TxCpuIdx, pAd->MgmtRing.TxDmaIdx, pAd->MgmtRing.TxSwFreeIdx);
+
+	//TODO: Need to remove it when formal release
+	PTXINFO_STRUC pTxInfo;
+
+	pTxInfo = (PTXINFO_STRUC)pMLMEContext->TransferBuffer;
+	if (pTxInfo->QSEL != FIFO_EDCA)
+	{
+			printk("%s(): ====> pTxInfo->QueueSel(%d)!= FIFO_EDCA!!!!\n", __FUNCTION__, pTxInfo->QSEL);
+			printk("\tMLME_Index=%d!\n", Index);
+			hex_dump("Wrong QSel Pkt:", (PUCHAR)pMLMEContext->TransferBuffer, pTxInfo->USBDMATxPktLen);
+	}
+}
+#endif
+
+#ifdef RT_BIG_ENDIAN
+	RTMPDescriptorEndianChange((PUCHAR)pMLMEContext->TransferBuffer, TYPE_TXINFO);
+#endif // RT_BIG_ENDIAN //
+
+	// Init Tx context descriptor
+	RTUSBInitTxDesc(pAd, pMLMEContext, MGMTPIPEIDX, (usb_complete_t)RTUSBBulkOutMLMEPacketComplete);
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+	//For mgmt urb buffer, because we use sk_buff, so we need to notify the USB controller do dma mapping.
+	pUrb->transfer_dma	= 0;
+	pUrb->transfer_flags &= (~URB_NO_TRANSFER_DMA_MAP);
+#endif
+
+	pUrb = pMLMEContext->pUrb;
+	if((ret = RTUSB_SUBMIT_URB(pUrb))!=0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("RTUSBBulkOutMLMEPacket: Submit MLME URB failed %d\n", ret));
+		RTMP_IRQ_LOCK(&pAd->BulkOutLock[MGMTPIPEIDX], IrqFlags);
+		pAd->BulkOutPending[MGMTPIPEIDX] = FALSE;
+		pAd->watchDogTxPendingCnt[MGMTPIPEIDX] = 0;
+		pMLMEContext->IRPPending = FALSE;
+		pMLMEContext->bWaitingBulkOut = TRUE;
+		RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[MGMTPIPEIDX], IrqFlags);
+
+		return;
+	}
+
+	//DBGPRINT_RAW(RT_DEBUG_INFO, ("<---RTUSBBulkOutMLMEPacket \n"));
+//	printk("<---RTUSBBulkOutMLMEPacket,Cpu=%d!, Dma=%d, SwIdx=%d!\n", pAd->MgmtRing.TxCpuIdx, pAd->MgmtRing.TxDmaIdx, pAd->MgmtRing.TxSwFreeIdx);
+}
+
+
+VOID RTUSBBulkOutMLMEPacketComplete(purbb_t pUrb, struct pt_regs *pt_regs)
+{
+	PTX_CONTEXT			pMLMEContext;
+	PRTMP_ADAPTER		pAd;
+	NTSTATUS			Status;
+	POS_COOKIE 			pObj;
+	int					index;
+#if 0 // sample, IRQ LOCK
+	unsigned long		IrqFlags;
+	PNDIS_PACKET		pPacket;
+#endif
+
+
+	//DBGPRINT_RAW(RT_DEBUG_INFO, ("--->RTUSBBulkOutMLMEPacketComplete\n"));
+	pMLMEContext	= (PTX_CONTEXT)pUrb->context;
+	pAd 			= pMLMEContext->pAd;
+	pObj 			= (POS_COOKIE)pAd->OS_Cookie;
+	Status			= pUrb->status;
+	index 			= pMLMEContext->SelfIdx;
+
+
+#if 0 // sample, IRQ LOCK
+	ASSERT((pAd->MgmtRing.TxDmaIdx == index));
+	//printk("MLME-Done-B: C=%d, D=%d, F=%d, Self=%d!\n", pAd->MgmtRing.TxCpuIdx, pAd->MgmtRing.TxDmaIdx, pAd->MgmtRing.TxSwFreeIdx, pMLMEContext->SelfIdx);
+
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[MGMTPIPEIDX], IrqFlags);
+
+
+	if (Status != USB_ST_NOERROR)
+	{
+		//Bulk-Out fail status handle
+		if ((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET)))
+		{
+			DBGPRINT_RAW(RT_DEBUG_ERROR, ("Bulk Out MLME Failed, Status=%d!\n", Status));
+			// TODO: How to handle about the MLMEBulkOut failed issue. Need to resend the mgmt pkt?
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+			pAd->bulkResetPipeid = (MGMTPIPEIDX | BULKOUT_MGMT_RESET_FLAG);
+		}
+	}
+	pAd->BulkOutPending[MGMTPIPEIDX] = FALSE;
+	RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[MGMTPIPEIDX], IrqFlags);
+
+	RTMP_IRQ_LOCK(&pAd->MLMEBulkOutLock, IrqFlags);
+	// Reset MLME context flags
+	pMLMEContext->IRPPending = FALSE;
+	pMLMEContext->InUse = FALSE;
+	pMLMEContext->bWaitingBulkOut = FALSE;
+	pMLMEContext->BulkOutSize = 0;
+
+	pPacket = pAd->MgmtRing.Cell[index].pNdisPacket;
+	pAd->MgmtRing.Cell[index].pNdisPacket = NULL;
+
+	// Increase MgmtRing Index
+	INC_RING_INDEX(pAd->MgmtRing.TxDmaIdx, MGMT_RING_SIZE);
+	pAd->MgmtRing.TxSwFreeIdx++;
+
+	RTMP_IRQ_UNLOCK(&pAd->MLMEBulkOutLock, IrqFlags);
+
+	// No-matter success or fail, we free the mgmt packet.
+	if (pPacket)
+		RTMPFreeNdisPacket(pAd, pPacket);
+
+#if 0
+	//Bulk-Out fail status handle
+	if (Status != USB_ST_NOERROR)
+	{
+		if ((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET)))
+		{
+			DBGPRINT_RAW(RT_DEBUG_ERROR, ("Bulk Out MLME Failed, Status=%d!\n", Status));
+			// TODO: How to handle about the MLMEBulkOut failed issue. Need to reset the endpoint?
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+			pAd->bulkResetPipeid = (MGMTPIPEIDX | BULKOUT_MGMT_RESET_FLAG);
+		}
+	}
+#endif
+
+	//printk("MLME-Done-A: C=%d, D=%d, F=%d!\n", pAd->MgmtRing.TxCpuIdx, pAd->MgmtRing.TxDmaIdx, pAd->MgmtRing.TxSwFreeIdx);
+
+	pObj->mgmt_dma_done_task.data = (unsigned long)pAd;
+	tasklet_hi_schedule(&pObj->mgmt_dma_done_task);
+
+	//DBGPRINT_RAW(RT_DEBUG_INFO, ("<---RTUSBBulkOutMLMEPacketComplete\n"));
+//	printk("<---RTUSBBulkOutMLMEPacketComplete, Cpu=%d, Dma=%d, SwIdx=%d!\n",
+//				pAd->MgmtRing.TxCpuIdx, pAd->MgmtRing.TxDmaIdx, pAd->MgmtRing.TxSwFreeIdx);
+
+#else
+
+	pObj->mgmt_dma_done_task.data = (unsigned long)pUrb;
+	tasklet_hi_schedule(&pObj->mgmt_dma_done_task);
+#endif
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	Note: PsPoll use BulkOutPipeId = 0
+
+	========================================================================
+*/
+VOID	RTUSBBulkOutPsPoll(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	PTX_CONTEXT		pPsPollContext = &(pAd->PsPollContext);
+	PURB			pUrb;
+	int				ret = 0;
+	unsigned long	IrqFlags;
+
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[0], IrqFlags);
+	if ((pAd->BulkOutPending[0] == TRUE) || RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NEED_STOP_TX))
+	{
+		RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], IrqFlags);
+		return;
+	}
+	pAd->BulkOutPending[0] = TRUE;
+	pAd->watchDogTxPendingCnt[0] = 1;
+	pPsPollContext->IRPPending = TRUE;
+	RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], IrqFlags);
+
+
+	// Clear PS-Poll bulk flag
+	RTUSB_CLEAR_BULK_FLAG(pAd, fRTUSB_BULK_OUT_PSPOLL);
+
+#ifdef RT_BIG_ENDIAN
+	RTMPDescriptorEndianChange((PUCHAR)pPsPollContext->TransferBuffer, TYPE_TXINFO);
+#endif // RT_BIG_ENDIAN //
+
+	// Init Tx context descriptor
+	RTUSBInitTxDesc(pAd, pPsPollContext, MGMTPIPEIDX, (usb_complete_t)RTUSBBulkOutPsPollComplete);
+
+	pUrb = pPsPollContext->pUrb;
+	if((ret = RTUSB_SUBMIT_URB(pUrb))!=0)
+	{
+		RTMP_IRQ_LOCK(&pAd->BulkOutLock[0], IrqFlags);
+		pAd->BulkOutPending[0] = FALSE;
+		pAd->watchDogTxPendingCnt[0] = 0;
+		pPsPollContext->IRPPending = FALSE;
+		RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], IrqFlags);
+
+		DBGPRINT(RT_DEBUG_ERROR, ("RTUSBBulkOutPsPoll: Submit Tx URB failed %d\n", ret));
+		return;
+	}
+
+}
+
+// PS-Poll frame use BulkOutPipeId = 0
+VOID RTUSBBulkOutPsPollComplete(purbb_t pUrb,struct pt_regs *pt_regs)
+{
+	PRTMP_ADAPTER		pAd;
+	PTX_CONTEXT			pPsPollContext;
+	NTSTATUS			Status;
+#if 0 // sample, IRQ LOCK
+	unsigned long		IrqFlags;
+#endif
+	POS_COOKIE			pObj;
+
+
+	pPsPollContext= (PTX_CONTEXT)pUrb->context;
+	pAd = pPsPollContext->pAd;
+	Status = pUrb->status;
+
+#if 0 // sample, IRQ LOCK
+	// Reset PsPoll context flags
+	pPsPollContext->IRPPending	= FALSE;
+	pPsPollContext->InUse		= FALSE;
+
+	if (Status == USB_ST_NOERROR)
+	{
+		// Don't worry about the queue is empty or not, this function will check itself
+		RTMPDeQueuePacket(pAd, TRUE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+	}
+	else // STATUS_OTHER
+	{
+		if ((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET)))
+		{
+			DBGPRINT_RAW(RT_DEBUG_ERROR, ("Bulk Out PSPoll Failed\n"));
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+		}
+	}
+
+	RTMP_IRQ_LOCK(&pAd->BulkOutLock[0], IrqFlags);
+	pAd->BulkOutPending[0] = FALSE;
+	RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[0], IrqFlags);
+
+	// Always call Bulk routine, even reset bulk.
+	// The protectioon of rest bulk should be in BulkOut routine
+	RTUSBKickBulkOut(pAd);
+#else
+
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+	pObj->pspoll_frame_complete_task.data = (unsigned long)pUrb;
+	tasklet_hi_schedule(&pObj->pspoll_frame_complete_task);
+#endif
+}
+
+
+#if 0
+/*
+	========================================================================
+
+	Routine Description:
+	USB_RxPacket initializes a URB and uses the Rx IRP to submit it
+	to USB. It checks if an Rx Descriptor is available and passes the
+	the coresponding buffer to be filled. If no descriptor is available
+	fails the request. When setting the completion routine we pass our
+	Adapter Object as Context.
+
+	Arguments:
+
+	Return Value:
+		TRUE			found matched tuple cache
+		FALSE			no matched found
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTUSBBulkReceive(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	PRX_CONTEXT		pRxContext;
+	PURB			pUrb;
+	int				ret = 0;
+	unsigned long	IrqFlags;
+
+
+	/* device had been closed */
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_REMOVE_IN_PROGRESS))
+		return;
+
+	RTMP_IRQ_LOCK(&pAd->BulkInLock, IrqFlags);
+
+	// Last is time point between 2 separate URB.
+	if (pAd->NextRxBulkInPosition == 0)
+	{
+		//pAd->NextRxBulkInIndex = (pAd->NextRxBulkInIndex + 1) % (RX_RING_SIZE);
+		INC_RING_INDEX(pAd->NextRxBulkInIndex, RX_RING_SIZE);
+	}
+	else if ((pAd->NextRxBulkInPosition&0x1ff) != 0)
+	{
+		//pAd->NextRxBulkInIndex = (pAd->NextRxBulkInIndex + 1) % (RX_RING_SIZE);
+		INC_RING_INDEX(pAd->NextRxBulkInIndex, RX_RING_SIZE);
+		DBGPRINT_RAW(RT_DEBUG_TRACE, ("pAd->NextRxBulkInPosition = 0x%lx.  End of URB.\n", pAd->NextRxBulkInPosition ));
+		pAd->NextRxBulkInPosition = 0;
+	}
+
+	if (pAd->NextRxBulkInPosition == MAX_RXBULK_SIZE)
+		pAd->NextRxBulkInPosition = 0;
+
+	pRxContext = &(pAd->RxContext[pAd->NextRxBulkInIndex]);
+
+	// TODO: Why need to check if pRxContext->InUsed == TRUE?
+	//if ((pRxContext->InUse == TRUE) || (pRxContext->Readable == TRUE))
+	if ((pRxContext->InUse == FALSE) && (pRxContext->Readable == TRUE))
+	{
+		DBGPRINT_RAW(RT_DEBUG_TRACE, ("pRxContext[%d] InUse = %d.pRxContext->Readable = %d.  Return.\n", pAd->NextRxBulkInIndex,pRxContext->InUse, pRxContext->Readable ));
+		RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+
+		// read RxContext, Since not
+#ifdef CONFIG_STA_SUPPORT
+		STARxDoneInterruptHandle(pAd, TRUE);
+#endif // CONFIG_STA_SUPPORT //
+
+		//return;
+	}
+	pRxContext->InUse = TRUE;
+	pRxContext->IRPPending= TRUE;
+
+	RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+
+	// Init Rx context descriptor
+	NdisZeroMemory(pRxContext->TransferBuffer, BUFFER_SIZE);
+	RTUSBInitRxDesc(pAd, pRxContext);
+
+	pUrb = pRxContext->pUrb;
+	if ((ret = RTUSB_SUBMIT_URB(pUrb))!=0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("RTUSBBulkReceive: Submit Rx URB failed %d\n", ret));
+		return;
+	}
+	else // success
+	{
+		NdisInterlockedIncrement(&pAd->PendingRx);
+		pAd->BulkInReq++;
+	}
+
+	// read RxContext, Since not
+#ifdef CONFIG_STA_SUPPORT
+	STARxDoneInterruptHandle(pAd, FALSE);
+#endif // CONFIG_STA_SUPPORT //
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		This routine process Rx Irp and call rx complete function.
+
+	Arguments:
+		DeviceObject	Pointer to the device object for next lower
+						device. DeviceObject passed in here belongs to
+						the next lower driver in the stack because we
+						were invoked via IoCallDriver in USB_RxPacket
+						AND it is not OUR device object
+	  Irp				Ptr to completed IRP
+	  Context			Ptr to our Adapter object (context specified
+						in IoSetCompletionRoutine
+
+	Return Value:
+		Always returns STATUS_MORE_PROCESSING_REQUIRED
+
+	Note:
+		Always returns STATUS_MORE_PROCESSING_REQUIRED
+	========================================================================
+*/
+VOID RTUSBBulkRxComplete(purbb_t pUrb, struct pt_regs *pt_regs)
+{
+#if 0
+	PRX_CONTEXT			pRxContext;
+	PRTMP_ADAPTER		pAd;
+	NTSTATUS			Status;
+//	POS_COOKIE			pObj;
+
+	pRxContext	= (PRX_CONTEXT)pUrb->context;
+	pAd 		= pRxContext->pAd;
+//	pObj		= (POS_COOKIE) pAd->OS_Cookie;
+
+
+	Status = pUrb->status;
+	//pRxContext->pIrp = NULL;
+
+	pRxContext->InUse = FALSE;
+	pRxContext->IRPPending = FALSE;
+
+	if (Status == USB_ST_NOERROR)
+	{
+		pAd->BulkInComplete++;
+		pRxContext->Readable = TRUE;
+		pAd->NextRxBulkInPosition = 0;
+
+	}
+	else	 // STATUS_OTHER
+	{
+		pAd->BulkInCompleteFail++;
+		// Still read this packet although it may comtain wrong bytes.
+		pRxContext->Readable = FALSE;
+		// Parsing all packets. because after reset, the index will reset to all zero.
+
+		if ((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKIN_RESET)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+		{
+
+			DBGPRINT_RAW(RT_DEBUG_ERROR, ("Bulk In Failed. Status = %d\n", Status));
+			DBGPRINT_RAW(RT_DEBUG_ERROR, ("==>NextRxBulkInIndex=0x%x, NextRxBulkInReadIndex=0x%x, TransferBufferLength= 0x%x\n",
+				pAd->NextRxBulkInIndex, pAd->NextRxBulkInReadIndex, pRxContext->pUrb->actual_length));
+
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKIN_RESET);
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_IN, NULL, 0);
+		}
+		//pUrb = NULL;
+	}
+
+	if ((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) &&
+		(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKIN_RESET)) &&
+//		(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET)) &&
+		(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF)) &&
+		(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
+		(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+	{
+		RTUSBBulkReceive(pAd);
+#if 0
+#if 1
+		STARxDoneInterruptHandle(pAd, FALSE);
+#else
+		pObj->rx_bh.data = (unsigned long)pUrb;
+		tasklet_schedule(&pObj->rx_bh);
+#endif
+#endif
+	}
+
+	// Call RxPacket to process packet and return the status
+	NdisInterlockedDecrement(&pAd->PendingRx);
+#else
+
+
+	// use a receive tasklet to handle received packets;
+	// or sometimes hardware IRQ will be disabled here, so we can not
+	// use spin_lock_bh()/spin_unlock_bh() after IRQ is disabled. :<
+	PRX_CONTEXT		pRxContext;
+	PRTMP_ADAPTER	pAd;
+	POS_COOKIE 		pObj;
+
+
+	pRxContext	= (PRX_CONTEXT)pUrb->context;
+	pAd 		= pRxContext->pAd;
+	pObj 		= (POS_COOKIE) pAd->OS_Cookie;
+
+	pObj->rx_done_task.data = (unsigned long)pUrb;
+	tasklet_hi_schedule(&pObj->rx_done_task);
+#endif
+}
+
+#else
+
+VOID DoBulkIn(IN RTMP_ADAPTER *pAd)
+{
+	PRX_CONTEXT		pRxContext;
+	PURB			pUrb;
+	int				ret = 0;
+	unsigned long	IrqFlags;
+
+	RTMP_IRQ_LOCK(&pAd->BulkInLock, IrqFlags);
+	pRxContext = &(pAd->RxContext[pAd->NextRxBulkInIndex]);
+	if ((pAd->PendingRx > 0) || (pRxContext->Readable == TRUE) || (pRxContext->InUse == TRUE))
+	{
+		RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+		return;
+	}
+	pRxContext->InUse = TRUE;
+	pRxContext->IRPPending = TRUE;
+	pAd->PendingRx++;
+	pAd->BulkInReq++;
+	RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+
+	// Init Rx context descriptor
+	NdisZeroMemory(pRxContext->TransferBuffer, pRxContext->BulkInOffset);
+	RTUSBInitRxDesc(pAd, pRxContext);
+
+	pUrb = pRxContext->pUrb;
+	if ((ret = RTUSB_SUBMIT_URB(pUrb))!=0)
+	{	// fail
+
+		RTMP_IRQ_LOCK(&pAd->BulkInLock, IrqFlags);
+		pRxContext->InUse = FALSE;
+		pRxContext->IRPPending = FALSE;
+		pAd->PendingRx--;
+		pAd->BulkInReq--;
+		RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+		DBGPRINT(RT_DEBUG_ERROR, ("RTUSBBulkReceive: Submit Rx URB failed %d\n", ret));
+	}
+	else
+	{	// success
+#if 0
+		RTMP_IRQ_LOCK(&pAd->BulkInLock, IrqFlags);
+		pRxContext->IRPPending = TRUE;
+		//NdisInterlockedIncrement(&pAd->PendingRx);
+		pAd->PendingRx++;
+		RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+		pAd->BulkInReq++;
+#endif
+		ASSERT((pRxContext->InUse == pRxContext->IRPPending));
+		//printk("BIDone, Pend=%d,BIIdx=%d,BIRIdx=%d!\n", pAd->PendingRx, pAd->NextRxBulkInIndex, pAd->NextRxBulkInReadIndex);
+	}
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+	USB_RxPacket initializes a URB and uses the Rx IRP to submit it
+	to USB. It checks if an Rx Descriptor is available and passes the
+	the coresponding buffer to be filled. If no descriptor is available
+	fails the request. When setting the completion routine we pass our
+	Adapter Object as Context.
+
+	Arguments:
+
+	Return Value:
+		TRUE			found matched tuple cache
+		FALSE			no matched found
+
+	Note:
+
+	========================================================================
+*/
+#define fRTMP_ADAPTER_NEED_STOP_RX		\
+		(fRTMP_ADAPTER_NIC_NOT_EXIST | fRTMP_ADAPTER_HALT_IN_PROGRESS |	\
+		 fRTMP_ADAPTER_RADIO_OFF | fRTMP_ADAPTER_RESET_IN_PROGRESS | \
+		 fRTMP_ADAPTER_REMOVE_IN_PROGRESS | fRTMP_ADAPTER_BULKIN_RESET)
+
+#define fRTMP_ADAPTER_NEED_STOP_HANDLE_RX	\
+		(fRTMP_ADAPTER_NIC_NOT_EXIST | fRTMP_ADAPTER_HALT_IN_PROGRESS |	\
+		 fRTMP_ADAPTER_RADIO_OFF | fRTMP_ADAPTER_RESET_IN_PROGRESS | \
+		 fRTMP_ADAPTER_REMOVE_IN_PROGRESS)
+
+VOID	RTUSBBulkReceive(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	PRX_CONTEXT		pRxContext;
+	unsigned long	IrqFlags;
+
+
+	/* sanity check */
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NEED_STOP_HANDLE_RX))
+		return;
+
+	while(1)
+	{
+
+		RTMP_IRQ_LOCK(&pAd->BulkInLock, IrqFlags);
+		pRxContext = &(pAd->RxContext[pAd->NextRxBulkInReadIndex]);
+		if (((pRxContext->InUse == FALSE) && (pRxContext->Readable == TRUE)) &&
+			(pRxContext->bRxHandling == FALSE))
+		{
+			pRxContext->bRxHandling = TRUE;
+			RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+
+			// read RxContext, Since not
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+				STARxDoneInterruptHandle(pAd, TRUE);
+#endif // CONFIG_STA_SUPPORT //
+
+			// Finish to handle this bulkIn buffer.
+			RTMP_IRQ_LOCK(&pAd->BulkInLock, IrqFlags);
+			pRxContext->BulkInOffset = 0;
+			pRxContext->Readable = FALSE;
+			pRxContext->bRxHandling = FALSE;
+			pAd->ReadPosition = 0;
+			pAd->TransferBufferLength = 0;
+			INC_RING_INDEX(pAd->NextRxBulkInReadIndex, RX_RING_SIZE);
+			RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+
+		}
+		else
+		{
+			RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+			break;
+		}
+	}
+
+	if (!(RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NEED_STOP_RX)))
+		DoBulkIn(pAd);
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		This routine process Rx Irp and call rx complete function.
+
+	Arguments:
+		DeviceObject	Pointer to the device object for next lower
+						device. DeviceObject passed in here belongs to
+						the next lower driver in the stack because we
+						were invoked via IoCallDriver in USB_RxPacket
+						AND it is not OUR device object
+	  Irp				Ptr to completed IRP
+	  Context			Ptr to our Adapter object (context specified
+						in IoSetCompletionRoutine
+
+	Return Value:
+		Always returns STATUS_MORE_PROCESSING_REQUIRED
+
+	Note:
+		Always returns STATUS_MORE_PROCESSING_REQUIRED
+	========================================================================
+*/
+VOID RTUSBBulkRxComplete(purbb_t pUrb, struct pt_regs *pt_regs)
+{
+	// use a receive tasklet to handle received packets;
+	// or sometimes hardware IRQ will be disabled here, so we can not
+	// use spin_lock_bh()/spin_unlock_bh() after IRQ is disabled. :<
+	PRX_CONTEXT		pRxContext;
+	PRTMP_ADAPTER	pAd;
+	POS_COOKIE 		pObj;
+
+
+	pRxContext	= (PRX_CONTEXT)pUrb->context;
+	pAd 		= pRxContext->pAd;
+	pObj 		= (POS_COOKIE) pAd->OS_Cookie;
+
+	pObj->rx_done_task.data = (unsigned long)pUrb;
+	tasklet_hi_schedule(&pObj->rx_done_task);
+
+}
+
+#endif
+
+
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTUSBKickBulkOut(
+	IN	PRTMP_ADAPTER pAd)
+{
+	// BulkIn Reset will reset whole USB PHY. So we need to make sure fRTMP_ADAPTER_BULKIN_RESET not flaged.
+	if (!RTMP_TEST_FLAG(pAd ,fRTMP_ADAPTER_NEED_STOP_TX)
+#ifdef RALINK_ATE
+		&& !(ATE_ON(pAd))
+#endif // RALINK_ATE //
+		)
+	{
+#if 0	// not used now in RT28xx, but may used latter.
+		// 1. Data Fragment has highest priority
+		if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_FRAG))
+		{
+			if (((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS)) ||
+				(!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+				))
+			{
+				RTUSBBulkOutDataPacket(pAd, 0, pAd->NextBulkOutIndex[0]);
+			}
+		}
+		if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_FRAG_2))
+		{
+			if (((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS)) ||
+				(!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+				))
+			{
+				RTUSBBulkOutDataPacket(pAd, 1, pAd->NextBulkOutIndex[1]);
+			}
+		}
+		if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_FRAG_3))
+		{
+			if (((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS)) ||
+				(!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+				))
+			{
+				RTUSBBulkOutDataPacket(pAd, 2, pAd->NextBulkOutIndex[2]);
+			}
+		}
+		if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_FRAG_4))
+		{
+			if (((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS)) ||
+				(!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+				))
+			{
+				RTUSBBulkOutDataPacket(pAd, 3, pAd->NextBulkOutIndex[3]);
+			}
+		}
+#endif
+
+		// 2. PS-Poll frame is next
+		if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_PSPOLL))
+		{
+			RTUSBBulkOutPsPoll(pAd);
+		}
+
+		// 5. Mlme frame is next
+		else if ((RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_MLME)) &&
+				 (pAd->MgmtRing.TxSwFreeIdx < MGMT_RING_SIZE))
+		{
+			RTUSBBulkOutMLMEPacket(pAd, pAd->MgmtRing.TxDmaIdx);
+		}
+
+		// 6. Data frame normal is next
+		if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL))
+		{
+			if (((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS)) ||
+				(!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+				))
+			{
+				RTUSBBulkOutDataPacket(pAd, 0, pAd->NextBulkOutIndex[0]);
+			}
+		}
+		if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL_2))
+		{
+			if (((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS)) ||
+				(!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+				))
+			{
+				RTUSBBulkOutDataPacket(pAd, 1, pAd->NextBulkOutIndex[1]);
+			}
+		}
+		if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL_3))
+		{
+			if (((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS)) ||
+				(!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+				))
+			{
+				RTUSBBulkOutDataPacket(pAd, 2, pAd->NextBulkOutIndex[2]);
+			}
+		}
+		if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NORMAL_4))
+		{
+			if (((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS)) ||
+				(!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+				))
+			{
+				RTUSBBulkOutDataPacket(pAd, 3, pAd->NextBulkOutIndex[3]);
+			}
+		}
+
+		// 7. Null frame is the last
+		else if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_NULL))
+		{
+			if (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+			{
+				RTUSBBulkOutNullFrame(pAd);
+			}
+		}
+
+		// 8. No data avaliable
+		else
+		{
+
+		}
+	}
+#ifdef RALINK_ATE
+	/* If the mode is in ATE mode. */
+	else if((ATE_ON(pAd)) &&
+		!RTMP_TEST_FLAG(pAd ,fRTMP_ADAPTER_NEED_STOP_TX))// PETER : watch out !
+	{
+		if (RTUSB_TEST_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_ATE))
+		{
+			ATE_RTUSBBulkOutDataPacket(pAd, 0);
+		}
+	}
+#endif // RALINK_ATE //
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+	Call from Reset action after BulkOut failed.
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTUSBCleanUpDataBulkOutQueue(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	UCHAR			Idx;
+	PHT_TX_CONTEXT	pTxContext;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("--->CleanUpDataBulkOutQueue\n"));
+
+	for (Idx = 0; Idx < 4; Idx++)
+	{
+		pTxContext = &pAd->TxContext[Idx];
+
+		pTxContext->CurWritePosition = pTxContext->NextBulkOutPosition;
+		pTxContext->LastOne = FALSE;
+		NdisAcquireSpinLock(&pAd->BulkOutLock[Idx]);
+		pAd->BulkOutPending[Idx] = FALSE;
+		NdisReleaseSpinLock(&pAd->BulkOutLock[Idx]);
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<---CleanUpDataBulkOutQueue\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTUSBCleanUpMLMEBulkOutQueue(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	DBGPRINT(RT_DEBUG_TRACE, ("--->CleanUpMLMEBulkOutQueue\n"));
+
+#if 0	// Do nothing!
+	NdisAcquireSpinLock(&pAd->MLMEBulkOutLock);
+	while (pAd->PrioRingTxCnt > 0)
+	{
+		pAd->MLMEContext[pAd->PrioRingFirstIndex].InUse = FALSE;
+
+		pAd->PrioRingFirstIndex++;
+		if (pAd->PrioRingFirstIndex >= MGMT_RING_SIZE)
+		{
+			pAd->PrioRingFirstIndex = 0;
+		}
+
+		pAd->PrioRingTxCnt--;
+	}
+	NdisReleaseSpinLock(&pAd->MLMEBulkOutLock);
+#endif
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<---CleanUpMLMEBulkOutQueue\n"));
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTUSBCancelPendingIRPs(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	RTUSBCancelPendingBulkInIRP(pAd);
+	RTUSBCancelPendingBulkOutIRP(pAd);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTUSBCancelPendingBulkInIRP(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	PRX_CONTEXT		pRxContext;
+	UINT			i;
+
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("--->RTUSBCancelPendingBulkInIRP\n"));
+	for ( i = 0; i < (RX_RING_SIZE); i++)
+	{
+		pRxContext = &(pAd->RxContext[i]);
+		if(pRxContext->IRPPending == TRUE)
+		{
+			RTUSB_UNLINK_URB(pRxContext->pUrb);
+			pRxContext->IRPPending = FALSE;
+			pRxContext->InUse = FALSE;
+			//NdisInterlockedDecrement(&pAd->PendingRx);
+			//pAd->PendingRx--;
+		}
+	}
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("<---RTUSBCancelPendingBulkInIRP\n"));
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTUSBCancelPendingBulkOutIRP(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	PHT_TX_CONTEXT		pHTTXContext;
+	PTX_CONTEXT			pMLMEContext;
+	PTX_CONTEXT			pBeaconContext;
+	PTX_CONTEXT			pNullContext;
+	PTX_CONTEXT			pPsPollContext;
+	PTX_CONTEXT			pRTSContext;
+	UINT				i, Idx;
+//	unsigned int 		IrqFlags;
+//	NDIS_SPIN_LOCK		*pLock;
+//	BOOLEAN				*pPending;
+
+
+//	pLock = &pAd->BulkOutLock[MGMTPIPEIDX];
+//	pPending = &pAd->BulkOutPending[MGMTPIPEIDX];
+
+	for (Idx = 0; Idx < 4; Idx++)
+	{
+		pHTTXContext = &(pAd->TxContext[Idx]);
+
+		if (pHTTXContext->IRPPending == TRUE)
+		{
+
+			// Get the USB_CONTEXT and cancel it's IRP; the completion routine will itself
+			// remove it from the HeadPendingSendList and NULL out HeadPendingSendList
+			//	when the last IRP on the list has been	cancelled; that's how we exit this loop
+			//
+
+			RTUSB_UNLINK_URB(pHTTXContext->pUrb);
+
+			// Sleep 200 microseconds to give cancellation time to work
+			RTMPusecDelay(200);
+		}
+
+#ifdef RALINK_ATE
+		pHTTXContext->bCopySavePad = 0;
+		pHTTXContext->CurWritePosition = 0;
+		pHTTXContext->CurWriteRealPos = 0;
+		pHTTXContext->bCurWriting = FALSE;
+		pHTTXContext->NextBulkOutPosition = 0;
+		pHTTXContext->ENextBulkOutPosition = 0;
+#endif // RALINK_ATE //
+		pAd->BulkOutPending[Idx] = FALSE;
+	}
+
+	//RTMP_IRQ_LOCK(pLock, IrqFlags);
+	for (i = 0; i < MGMT_RING_SIZE; i++)
+	{
+		pMLMEContext = (PTX_CONTEXT)pAd->MgmtRing.Cell[i].AllocVa;
+		if(pMLMEContext && (pMLMEContext->IRPPending == TRUE))
+		{
+
+			// Get the USB_CONTEXT and cancel it's IRP; the completion routine will itself
+			// remove it from the HeadPendingSendList and NULL out HeadPendingSendList
+			//	when the last IRP on the list has been	cancelled; that's how we exit this loop
+			//
+
+			RTUSB_UNLINK_URB(pMLMEContext->pUrb);
+			pMLMEContext->IRPPending = FALSE;
+
+			// Sleep 200 microsecs to give cancellation time to work
+			RTMPusecDelay(200);
+		}
+	}
+	pAd->BulkOutPending[MGMTPIPEIDX] = FALSE;
+	//RTMP_IRQ_UNLOCK(pLock, IrqFlags);
+
+
+	for (i = 0; i < BEACON_RING_SIZE; i++)
+	{
+		pBeaconContext = &(pAd->BeaconContext[i]);
+
+		if(pBeaconContext->IRPPending == TRUE)
+		{
+
+			// Get the USB_CONTEXT and cancel it's IRP; the completion routine will itself
+			// remove it from the HeadPendingSendList and NULL out HeadPendingSendList
+			//	when the last IRP on the list has been	cancelled; that's how we exit this loop
+			//
+
+			RTUSB_UNLINK_URB(pBeaconContext->pUrb);
+
+			// Sleep 200 microsecs to give cancellation time to work
+			RTMPusecDelay(200);
+		}
+	}
+
+	pNullContext = &(pAd->NullContext);
+	if (pNullContext->IRPPending == TRUE)
+		RTUSB_UNLINK_URB(pNullContext->pUrb);
+
+	pRTSContext = &(pAd->RTSContext);
+	if (pRTSContext->IRPPending == TRUE)
+		RTUSB_UNLINK_URB(pRTSContext->pUrb);
+
+	pPsPollContext = &(pAd->PsPollContext);
+	if (pPsPollContext->IRPPending == TRUE)
+		RTUSB_UNLINK_URB(pPsPollContext->pUrb);
+
+	for (Idx = 0; Idx < 4; Idx++)
+	{
+		NdisAcquireSpinLock(&pAd->BulkOutLock[Idx]);
+		pAd->BulkOutPending[Idx] = FALSE;
+		NdisReleaseSpinLock(&pAd->BulkOutLock[Idx]);
+	}
+}
+
diff --git a/drivers/staging/rt2870/common/rtusb_data.c b/drivers/staging/rt2870/common/rtusb_data.c
new file mode 100644
index 00000000000..5a0d78389f4
--- /dev/null
+++ b/drivers/staging/rt2870/common/rtusb_data.c
@@ -0,0 +1,229 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rtusb_data.c
+
+	Abstract:
+	Ralink USB driver Tx/Rx functions.
+
+	Revision History:
+	Who         When          What
+	--------    ----------    ----------------------------------------------
+	Jan            03-25-2006    created
+
+*/
+#include "../rt_config.h"
+
+extern  UCHAR Phy11BGNextRateUpward[]; // defined in mlme.c
+extern UCHAR	EpToQueue[];
+
+VOID REPORT_AMSDU_FRAMES_TO_LLC(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize)
+{
+	PNDIS_PACKET	pPacket;
+	UINT			nMSDU;
+	struct			sk_buff *pSkb;
+
+	nMSDU = 0;
+	/* allocate a rx packet */
+	pSkb = dev_alloc_skb(RX_BUFFER_AGGRESIZE);
+	pPacket = (PNDIS_PACKET)OSPKT_TO_RTPKT(pSkb);
+	if (pSkb)
+	{
+
+		/* convert 802.11 to 802.3 packet */
+		pSkb->dev = get_netdev_from_bssid(pAd, BSS0);
+		RTMP_SET_PACKET_SOURCE(pPacket, PKTSRC_NDIS);
+		deaggregate_AMSDU_announce(pAd, pPacket, pData, DataSize);
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("Can't allocate skb\n"));
+	}
+}
+
+NDIS_STATUS	RTUSBFreeDescriptorRequest(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			BulkOutPipeId,
+	IN	UINT32			NumberRequired)
+{
+//	UCHAR			FreeNumber = 0;
+//	UINT			Index;
+	NDIS_STATUS		Status = NDIS_STATUS_FAILURE;
+	unsigned long   IrqFlags;
+	HT_TX_CONTEXT	*pHTTXContext;
+
+
+	pHTTXContext = &pAd->TxContext[BulkOutPipeId];
+	RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+	if ((pHTTXContext->CurWritePosition < pHTTXContext->NextBulkOutPosition) && ((pHTTXContext->CurWritePosition + NumberRequired + LOCAL_TXBUF_SIZE) > pHTTXContext->NextBulkOutPosition))
+	{
+
+		RTUSB_SET_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_NORMAL << BulkOutPipeId));
+	}
+	else if ((pHTTXContext->CurWritePosition == 8) && (pHTTXContext->NextBulkOutPosition < (NumberRequired + LOCAL_TXBUF_SIZE)))
+	{
+		RTUSB_SET_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_NORMAL << BulkOutPipeId));
+	}
+	else if (pHTTXContext->bCurWriting == TRUE)
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("RTUSBFreeD c3 --> QueIdx=%d, CWPos=%ld, NBOutPos=%ld!\n", BulkOutPipeId, pHTTXContext->CurWritePosition, pHTTXContext->NextBulkOutPosition));
+		RTUSB_SET_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_NORMAL << BulkOutPipeId));
+	}
+	else
+	{
+		Status = NDIS_STATUS_SUCCESS;
+	}
+	RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+
+
+	return (Status);
+}
+
+NDIS_STATUS RTUSBFreeDescriptorRelease(
+	IN RTMP_ADAPTER *pAd,
+	IN UCHAR		BulkOutPipeId)
+{
+	unsigned long   IrqFlags;
+	HT_TX_CONTEXT	*pHTTXContext;
+
+	pHTTXContext = &pAd->TxContext[BulkOutPipeId];
+	RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+	pHTTXContext->bCurWriting = FALSE;
+	RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+
+	return (NDIS_STATUS_SUCCESS);
+}
+
+
+BOOLEAN	RTUSBNeedQueueBackForAgg(
+	IN RTMP_ADAPTER *pAd,
+	IN UCHAR		BulkOutPipeId)
+{
+	unsigned long   IrqFlags;
+	HT_TX_CONTEXT	*pHTTXContext;
+	BOOLEAN			needQueBack = FALSE;
+
+	pHTTXContext = &pAd->TxContext[BulkOutPipeId];
+
+	RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+	if ((pHTTXContext->IRPPending == TRUE)  /*&& (pAd->TxSwQueue[BulkOutPipeId].Number == 0) */)
+	{
+#if 0
+		if ((pHTTXContext->CurWritePosition <= 8) &&
+			(pHTTXContext->NextBulkOutPosition > 8 && (pHTTXContext->NextBulkOutPosition+MAX_AGGREGATION_SIZE) < MAX_TXBULK_LIMIT))
+		{
+			needQueBack = TRUE;
+		}
+		else if ((pHTTXContext->CurWritePosition < pHTTXContext->NextBulkOutPosition) &&
+				 ((pHTTXContext->NextBulkOutPosition + MAX_AGGREGATION_SIZE) < MAX_TXBULK_LIMIT))
+		{
+			needQueBack = TRUE;
+		}
+#else
+		if ((pHTTXContext->CurWritePosition < pHTTXContext->ENextBulkOutPosition) &&
+			(((pHTTXContext->ENextBulkOutPosition+MAX_AGGREGATION_SIZE) < MAX_TXBULK_LIMIT) || (pHTTXContext->CurWritePosition > MAX_AGGREGATION_SIZE)))
+		{
+			needQueBack = TRUE;
+		}
+#endif
+		else if ((pHTTXContext->CurWritePosition > pHTTXContext->ENextBulkOutPosition) &&
+				 ((pHTTXContext->ENextBulkOutPosition + MAX_AGGREGATION_SIZE) < pHTTXContext->CurWritePosition))
+		{
+			needQueBack = TRUE;
+		}
+	}
+	RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[BulkOutPipeId], IrqFlags);
+
+	return needQueBack;
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTUSBRejectPendingPackets(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	UCHAR			Index;
+	PQUEUE_ENTRY	pEntry;
+	PNDIS_PACKET	pPacket;
+	PQUEUE_HEADER	pQueue;
+
+
+	for (Index = 0; Index < 4; Index++)
+	{
+		NdisAcquireSpinLock(&pAd->TxSwQueueLock[Index]);
+		while (pAd->TxSwQueue[Index].Head != NULL)
+		{
+			pQueue = (PQUEUE_HEADER) &(pAd->TxSwQueue[Index]);
+			pEntry = RemoveHeadQueue(pQueue);
+			pPacket = QUEUE_ENTRY_TO_PACKET(pEntry);
+			RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+		}
+		NdisReleaseSpinLock(&pAd->TxSwQueueLock[Index]);
+
+	}
+
+}
+
+VOID RTMPWriteTxInfo(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTXINFO_STRUC 	pTxInfo,
+	IN	  USHORT		USBDMApktLen,
+	IN	  BOOLEAN		bWiv,
+	IN	  UCHAR			QueueSel,
+	IN	  UCHAR			NextValid,
+	IN	  UCHAR			TxBurst)
+{
+	pTxInfo->USBDMATxPktLen = USBDMApktLen;
+	pTxInfo->QSEL = QueueSel;
+	if (QueueSel != FIFO_EDCA)
+		DBGPRINT(RT_DEBUG_TRACE, ("====> QueueSel != FIFO_EDCA<============\n"));
+	pTxInfo->USBDMANextVLD = FALSE; //NextValid;  // Need to check with Jan about this.
+	pTxInfo->USBDMATxburst = TxBurst;
+	pTxInfo->WIV = bWiv;
+	pTxInfo->SwUseLastRound = 0;
+	pTxInfo->rsv = 0;
+	pTxInfo->rsv2 = 0;
+}
+
diff --git a/drivers/staging/rt2870/common/rtusb_io.c b/drivers/staging/rt2870/common/rtusb_io.c
new file mode 100644
index 00000000000..6db443e9268
--- /dev/null
+++ b/drivers/staging/rt2870/common/rtusb_io.c
@@ -0,0 +1,2006 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+ 	Module Name:
+	rtusb_io.c
+
+	Abstract:
+
+	Revision History:
+	Who			When	    What
+	--------	----------  ----------------------------------------------
+	Name		Date	    Modification logs
+	Paul Lin    06-25-2004  created
+*/
+
+#include "../rt_config.h"
+
+
+/*
+	========================================================================
+
+	Routine Description: NIC initialization complete
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+
+NTSTATUS	RTUSBFirmwareRun(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	NTSTATUS	Status;
+
+	Status = RTUSB_VendorRequest(
+		pAd,
+		USBD_TRANSFER_DIRECTION_OUT,
+		DEVICE_VENDOR_REQUEST_OUT,
+		0x01,
+		0x8,
+		0,
+		NULL,
+		0);
+
+	return Status;
+}
+
+
+
+/*
+	========================================================================
+
+	Routine Description: Write Firmware to NIC.
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS RTUSBFirmwareWrite(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR		pFwImage,
+	IN ULONG		FwLen)
+{
+	UINT32		MacReg;
+	NTSTATUS 	Status;
+//	ULONG 		i;
+	USHORT		writeLen;
+
+	Status = RTUSBReadMACRegister(pAd, MAC_CSR0, &MacReg);
+
+
+	writeLen = FwLen;
+	RTUSBMultiWrite(pAd, FIRMWARE_IMAGE_BASE, pFwImage, writeLen);
+
+	Status = RTUSBWriteMACRegister(pAd, 0x7014, 0xffffffff);
+	Status = RTUSBWriteMACRegister(pAd, 0x701c, 0xffffffff);
+	Status = RTUSBFirmwareRun(pAd);
+
+	return Status;
+}
+
+
+/*
+	========================================================================
+
+	Routine Description: Get current firmware operation mode (Return Value)
+
+	Arguments:
+
+	Return Value:
+		0 or 1 = Downloaded by host driver
+		others = Driver doesn't download firmware
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RTUSBFirmwareOpmode(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUINT32			pValue)
+{
+	NTSTATUS	Status;
+
+	Status = RTUSB_VendorRequest(
+		pAd,
+		(USBD_TRANSFER_DIRECTION_IN | USBD_SHORT_TRANSFER_OK),
+		DEVICE_VENDOR_REQUEST_IN,
+		0x1,
+		0x11,
+		0,
+		pValue,
+		4);
+	return Status;
+}
+NTSTATUS	RTUSBVenderReset(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	NTSTATUS	Status;
+	DBGPRINT_RAW(RT_DEBUG_ERROR, ("-->RTUSBVenderReset\n"));
+	Status = RTUSB_VendorRequest(
+		pAd,
+		USBD_TRANSFER_DIRECTION_OUT,
+		DEVICE_VENDOR_REQUEST_OUT,
+		0x01,
+		0x1,
+		0,
+		NULL,
+		0);
+
+	DBGPRINT_RAW(RT_DEBUG_ERROR, ("<--RTUSBVenderReset\n"));
+	return Status;
+}
+/*
+	========================================================================
+
+	Routine Description: Read various length data from RT2573
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RTUSBMultiRead(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Offset,
+	OUT	PUCHAR			pData,
+	IN	USHORT			length)
+{
+	NTSTATUS	Status;
+
+	Status = RTUSB_VendorRequest(
+		pAd,
+		(USBD_TRANSFER_DIRECTION_IN | USBD_SHORT_TRANSFER_OK),
+		DEVICE_VENDOR_REQUEST_IN,
+		0x7,
+		0,
+		Offset,
+		pData,
+		length);
+
+	return Status;
+}
+
+/*
+	========================================================================
+
+	Routine Description: Write various length data to RT2573
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RTUSBMultiWrite_OneByte(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Offset,
+	IN	PUCHAR			pData)
+{
+	NTSTATUS	Status;
+
+	// TODO: In 2870, use this funciton carefully cause it's not stable.
+	Status = RTUSB_VendorRequest(
+		pAd,
+		USBD_TRANSFER_DIRECTION_OUT,
+		DEVICE_VENDOR_REQUEST_OUT,
+		0x6,
+		0,
+		Offset,
+		pData,
+		1);
+
+	return Status;
+}
+
+NTSTATUS	RTUSBMultiWrite(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Offset,
+	IN	PUCHAR			pData,
+	IN	USHORT			length)
+{
+	NTSTATUS	Status;
+
+
+        USHORT          index = 0,Value;
+        PUCHAR          pSrc = pData;
+        USHORT          resude = 0;
+
+        resude = length % 2;
+		length  += resude;
+		do
+		{
+			Value =(USHORT)( *pSrc  | (*(pSrc + 1) << 8));
+		Status = RTUSBSingleWrite(pAd,Offset + index,Value);
+            index +=2;
+            length -= 2;
+            pSrc = pSrc + 2;
+        }while(length > 0);
+
+	return Status;
+}
+
+
+NTSTATUS RTUSBSingleWrite(
+	IN 	RTMP_ADAPTER 	*pAd,
+	IN	USHORT			Offset,
+	IN	USHORT			Value)
+{
+	NTSTATUS	Status;
+
+	Status = RTUSB_VendorRequest(
+		pAd,
+		USBD_TRANSFER_DIRECTION_OUT,
+		DEVICE_VENDOR_REQUEST_OUT,
+		0x2,
+		Value,
+		Offset,
+		NULL,
+		0);
+
+	return Status;
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description: Read 32-bit MAC register
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RTUSBReadMACRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Offset,
+	OUT	PUINT32			pValue)
+{
+	NTSTATUS	Status;
+	UINT32		localVal;
+
+	Status = RTUSB_VendorRequest(
+		pAd,
+		(USBD_TRANSFER_DIRECTION_IN | USBD_SHORT_TRANSFER_OK),
+		DEVICE_VENDOR_REQUEST_IN,
+		0x7,
+		0,
+		Offset,
+		&localVal,
+		4);
+
+	*pValue = le2cpu32(localVal);
+
+
+	if (Status < 0)
+		*pValue = 0xffffffff;
+
+	return Status;
+}
+
+
+/*
+	========================================================================
+
+	Routine Description: Write 32-bit MAC register
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RTUSBWriteMACRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Offset,
+	IN	UINT32			Value)
+{
+	NTSTATUS	Status;
+	UINT32		localVal;
+
+	localVal = Value;
+
+	Status = RTUSBSingleWrite(pAd, Offset, (USHORT)(localVal & 0xffff));
+	Status = RTUSBSingleWrite(pAd, Offset + 2, (USHORT)((localVal & 0xffff0000) >> 16));
+
+	return Status;
+}
+
+
+
+#if 1
+/*
+	========================================================================
+
+	Routine Description: Read 8-bit BBP register
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RTUSBReadBBPRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Id,
+	IN	PUCHAR			pValue)
+{
+	BBP_CSR_CFG_STRUC	BbpCsr;
+	UINT			i = 0;
+	NTSTATUS		status;
+
+	// Verify the busy condition
+	do
+	{
+		status = RTUSBReadMACRegister(pAd, BBP_CSR_CFG, &BbpCsr.word);
+		if(status >= 0)
+		{
+		if (!(BbpCsr.field.Busy == BUSY))
+			break;
+		}
+		printk("RTUSBReadBBPRegister(BBP_CSR_CFG_1):retry count=%d!\n", i);
+		i++;
+	}
+	while ((i < RETRY_LIMIT) && (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)));
+
+	if ((i == RETRY_LIMIT) || (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+	{
+		//
+		// Read failed then Return Default value.
+		//
+		*pValue = pAd->BbpWriteLatch[Id];
+
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("Retry count exhausted or device removed!!!\n"));
+		return STATUS_UNSUCCESSFUL;
+	}
+
+	// Prepare for write material
+	BbpCsr.word 				= 0;
+	BbpCsr.field.fRead			= 1;
+	BbpCsr.field.Busy			= 1;
+	BbpCsr.field.RegNum 		= Id;
+	RTUSBWriteMACRegister(pAd, BBP_CSR_CFG, BbpCsr.word);
+
+	i = 0;
+	// Verify the busy condition
+	do
+	{
+		status = RTUSBReadMACRegister(pAd, BBP_CSR_CFG, &BbpCsr.word);
+		if (status >= 0)
+		{
+		if (!(BbpCsr.field.Busy == BUSY))
+		{
+			*pValue = (UCHAR)BbpCsr.field.Value;
+			break;
+		}
+		}
+		printk("RTUSBReadBBPRegister(BBP_CSR_CFG_2):retry count=%d!\n", i);
+		i++;
+	}
+	while ((i < RETRY_LIMIT) && (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)));
+
+	if ((i == RETRY_LIMIT) || (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+	{
+		//
+		// Read failed then Return Default value.
+		//
+		*pValue = pAd->BbpWriteLatch[Id];
+
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("Retry count exhausted or device removed!!!\n"));
+		return STATUS_UNSUCCESSFUL;
+	}
+
+	return STATUS_SUCCESS;
+}
+#else
+/*
+	========================================================================
+
+	Routine Description: Read 8-bit BBP register via firmware
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RTUSBReadBBPRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Id,
+	IN	PUCHAR			pValue)
+{
+	BBP_CSR_CFG_STRUC	BbpCsr;
+	int					i, k;
+	for (i=0; i<MAX_BUSY_COUNT; i++)
+	{
+		RTUSBReadMACRegister(pAd, H2M_BBP_AGENT, &BbpCsr.word);
+		if (BbpCsr.field.Busy == BUSY)
+		{
+			continue;
+		}
+		BbpCsr.word = 0;
+		BbpCsr.field.fRead = 1;
+		BbpCsr.field.BBP_RW_MODE = 1;
+		BbpCsr.field.Busy = 1;
+		BbpCsr.field.RegNum = Id;
+		RTUSBWriteMACRegister(pAd, H2M_BBP_AGENT, BbpCsr.word);
+		AsicSendCommandToMcu(pAd, 0x80, 0xff, 0x0, 0x0);
+		for (k=0; k<MAX_BUSY_COUNT; k++)
+		{
+			RTUSBReadMACRegister(pAd, H2M_BBP_AGENT, &BbpCsr.word);
+			if (BbpCsr.field.Busy == IDLE)
+				break;
+		}
+		if ((BbpCsr.field.Busy == IDLE) &&
+			(BbpCsr.field.RegNum == Id))
+		{
+			*pValue = (UCHAR)BbpCsr.field.Value;
+			break;
+		}
+	}
+	if (BbpCsr.field.Busy == BUSY)
+	{
+		DBGPRINT_ERR(("BBP read R%d=0x%x fail\n", Id, BbpCsr.word));
+		*pValue = pAd->BbpWriteLatch[Id];
+		return STATUS_UNSUCCESSFUL;
+	}
+	return STATUS_SUCCESS;
+}
+#endif
+
+#if 1
+/*
+	========================================================================
+
+	Routine Description: Write 8-bit BBP register
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RTUSBWriteBBPRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Id,
+	IN	UCHAR			Value)
+{
+	BBP_CSR_CFG_STRUC	BbpCsr;
+	UINT			i = 0;
+	NTSTATUS		status;
+	// Verify the busy condition
+	do
+	{
+		status = RTUSBReadMACRegister(pAd, BBP_CSR_CFG, &BbpCsr.word);
+		if (status >= 0)
+		{
+		if (!(BbpCsr.field.Busy == BUSY))
+			break;
+		}
+		printk("RTUSBWriteBBPRegister(BBP_CSR_CFG):retry count=%d!\n", i);
+		i++;
+	}
+	while ((i < RETRY_LIMIT) && (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)));
+
+	if ((i == RETRY_LIMIT) || (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+	{
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("Retry count exhausted or device removed!!!\n"));
+		return STATUS_UNSUCCESSFUL;
+	}
+
+	// Prepare for write material
+	BbpCsr.word 				= 0;
+	BbpCsr.field.fRead			= 0;
+	BbpCsr.field.Value			= Value;
+	BbpCsr.field.Busy			= 1;
+	BbpCsr.field.RegNum 		= Id;
+	RTUSBWriteMACRegister(pAd, BBP_CSR_CFG, BbpCsr.word);
+
+	pAd->BbpWriteLatch[Id] = Value;
+
+	return STATUS_SUCCESS;
+}
+#else
+/*
+	========================================================================
+
+	Routine Description: Write 8-bit BBP register via firmware
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+
+NTSTATUS	RTUSBWriteBBPRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Id,
+	IN	UCHAR			Value)
+
+{
+	BBP_CSR_CFG_STRUC	BbpCsr;
+	int					BusyCnt;
+	for (BusyCnt=0; BusyCnt<MAX_BUSY_COUNT; BusyCnt++)
+	{
+		RTMP_IO_READ32(pAd, H2M_BBP_AGENT, &BbpCsr.word);
+		if (BbpCsr.field.Busy == BUSY)
+			continue;
+		BbpCsr.word = 0;
+		BbpCsr.field.fRead = 0;
+		BbpCsr.field.BBP_RW_MODE = 1;
+		BbpCsr.field.Busy = 1;
+		BbpCsr.field.Value = Value;
+		BbpCsr.field.RegNum = Id;
+		RTMP_IO_WRITE32(pAd, H2M_BBP_AGENT, BbpCsr.word);
+		AsicSendCommandToMcu(pAd, 0x80, 0xff, 0x0, 0x0);
+		pAd->BbpWriteLatch[Id] = Value;
+		break;
+	}
+	if (BusyCnt == MAX_BUSY_COUNT)
+	{
+		DBGPRINT_ERR(("BBP write R%d=0x%x fail\n", Id, BbpCsr.word));
+		return STATUS_UNSUCCESSFUL;
+	}
+	return STATUS_SUCCESS;
+}
+#endif
+/*
+	========================================================================
+
+	Routine Description: Write RF register through MAC
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RTUSBWriteRFRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UINT32			Value)
+{
+	PHY_CSR4_STRUC	PhyCsr4;
+	UINT			i = 0;
+	NTSTATUS		status;
+
+	NdisZeroMemory(&PhyCsr4, sizeof(PHY_CSR4_STRUC));
+	do
+	{
+		status = RTUSBReadMACRegister(pAd, RF_CSR_CFG0, &PhyCsr4.word);
+		if (status >= 0)
+		{
+		if (!(PhyCsr4.field.Busy))
+			break;
+		}
+		printk("RTUSBWriteRFRegister(RF_CSR_CFG0):retry count=%d!\n", i);
+		i++;
+	}
+	while ((i < RETRY_LIMIT) && (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)));
+
+	if ((i == RETRY_LIMIT) || (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+	{
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("Retry count exhausted or device removed!!!\n"));
+		return STATUS_UNSUCCESSFUL;
+	}
+
+	RTUSBWriteMACRegister(pAd, RF_CSR_CFG0, Value);
+
+	return STATUS_SUCCESS;
+}
+
+/*
+	========================================================================
+
+	Routine Description: Write RT3070 RF register through MAC
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RT30xxWriteRFRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			RegID,
+	IN	UCHAR			Value)
+{
+	RF_CSR_CFG_STRUC	rfcsr;
+	UINT				i = 0;
+
+	do
+	{
+		RTUSBReadMACRegister(pAd, RF_CSR_CFG, &rfcsr.word);
+
+		if (!rfcsr.field.RF_CSR_KICK)
+			break;
+		i++;
+	}
+	while ((i < RETRY_LIMIT) && (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)));
+
+	if ((i == RETRY_LIMIT) || (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+	{
+		DBGPRINT_RAW(RT_DEBUG_ERROR, ("Retry count exhausted or device removed!!!\n"));
+		return STATUS_UNSUCCESSFUL;
+	}
+
+	rfcsr.field.RF_CSR_WR = 1;
+	rfcsr.field.RF_CSR_KICK = 1;
+	rfcsr.field.TESTCSR_RFACC_REGNUM = RegID;
+	rfcsr.field.RF_CSR_DATA = Value;
+
+	RTUSBWriteMACRegister(pAd, RF_CSR_CFG, rfcsr.word);
+
+	return STATUS_SUCCESS;
+}
+
+/*
+	========================================================================
+
+	Routine Description: Read RT3070 RF register through MAC
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RT30xxReadRFRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			RegID,
+	IN	PUCHAR			pValue)
+{
+	RF_CSR_CFG_STRUC	rfcsr;
+	UINT				i=0, k;
+
+	for (i=0; i<MAX_BUSY_COUNT; i++)
+	{
+		RTUSBReadMACRegister(pAd, RF_CSR_CFG, &rfcsr.word);
+
+		if (rfcsr.field.RF_CSR_KICK == BUSY)
+		{
+			continue;
+		}
+		rfcsr.word = 0;
+		rfcsr.field.RF_CSR_WR = 0;
+		rfcsr.field.RF_CSR_KICK = 1;
+		rfcsr.field.TESTCSR_RFACC_REGNUM = RegID;
+		RTUSBWriteMACRegister(pAd, RF_CSR_CFG, rfcsr.word);
+		for (k=0; k<MAX_BUSY_COUNT; k++)
+		{
+			RTUSBReadMACRegister(pAd, RF_CSR_CFG, &rfcsr.word);
+
+			if (rfcsr.field.RF_CSR_KICK == IDLE)
+				break;
+		}
+		if ((rfcsr.field.RF_CSR_KICK == IDLE) &&
+			(rfcsr.field.TESTCSR_RFACC_REGNUM == RegID))
+		{
+			*pValue = (UCHAR)rfcsr.field.RF_CSR_DATA;
+			break;
+		}
+	}
+	if (rfcsr.field.RF_CSR_KICK == BUSY)
+	{
+		DBGPRINT_ERR(("RF read R%d=0x%x fail\n", RegID, rfcsr.word));
+		return STATUS_UNSUCCESSFUL;
+	}
+
+	return STATUS_SUCCESS;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RTUSBReadEEPROM(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Offset,
+	OUT	PUCHAR			pData,
+	IN	USHORT			length)
+{
+	NTSTATUS	Status = STATUS_SUCCESS;
+
+	Status = RTUSB_VendorRequest(
+		pAd,
+		(USBD_TRANSFER_DIRECTION_IN | USBD_SHORT_TRANSFER_OK),
+		DEVICE_VENDOR_REQUEST_IN,
+		0x9,
+		0,
+		Offset,
+		pData,
+		length);
+
+	return Status;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RTUSBWriteEEPROM(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Offset,
+	IN	PUCHAR			pData,
+	IN	USHORT			length)
+{
+	NTSTATUS	Status = STATUS_SUCCESS;
+
+	Status = RTUSB_VendorRequest(
+		pAd,
+		USBD_TRANSFER_DIRECTION_OUT,
+		DEVICE_VENDOR_REQUEST_OUT,
+		0x8,
+		0,
+		Offset,
+		pData,
+		length);
+
+	return Status;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+VOID RTUSBPutToSleep(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	UINT32		value;
+
+	// Timeout 0x40 x 50us
+	value = (SLEEPCID<<16)+(OWNERMCU<<24)+ (0x40<<8)+1;
+	RTUSBWriteMACRegister(pAd, 0x7010, value);
+	RTUSBWriteMACRegister(pAd, 0x404, 0x30);
+	//RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS);
+	DBGPRINT_RAW(RT_DEBUG_ERROR, ("Sleep Mailbox testvalue %x\n", value));
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS RTUSBWakeUp(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	NTSTATUS	Status;
+
+	Status = RTUSB_VendorRequest(
+		pAd,
+		USBD_TRANSFER_DIRECTION_OUT,
+		DEVICE_VENDOR_REQUEST_OUT,
+		0x01,
+		0x09,
+		0,
+		NULL,
+		0);
+
+	return Status;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTUSBInitializeCmdQ(
+	IN	PCmdQ	cmdq)
+{
+	cmdq->head = NULL;
+	cmdq->tail = NULL;
+	cmdq->size = 0;
+	cmdq->CmdQState = RT2870_THREAD_INITED;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS	RTUSBEnqueueCmdFromNdis(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	NDIS_OID		Oid,
+	IN	BOOLEAN			SetInformation,
+	IN	PVOID			pInformationBuffer,
+	IN	UINT32			InformationBufferLength)
+{
+	NDIS_STATUS	status;
+	PCmdQElmt	cmdqelmt = NULL;
+	POS_COOKIE pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+
+	CHECK_PID_LEGALITY(pObj->RTUSBCmdThr_pid)
+		return (NDIS_STATUS_RESOURCES);
+
+	status = RTMPAllocateMemory((PVOID *)&cmdqelmt, sizeof(CmdQElmt));
+	if ((status != NDIS_STATUS_SUCCESS) || (cmdqelmt == NULL))
+		return (NDIS_STATUS_RESOURCES);
+
+		cmdqelmt->buffer = NULL;
+		if (pInformationBuffer != NULL)
+		{
+			status = RTMPAllocateMemory((PVOID *)&cmdqelmt->buffer, InformationBufferLength);
+			if ((status != NDIS_STATUS_SUCCESS) || (cmdqelmt->buffer == NULL))
+			{
+				kfree(cmdqelmt);
+				return (NDIS_STATUS_RESOURCES);
+			}
+			else
+			{
+				NdisMoveMemory(cmdqelmt->buffer, pInformationBuffer, InformationBufferLength);
+				cmdqelmt->bufferlength = InformationBufferLength;
+			}
+		}
+		else
+			cmdqelmt->bufferlength = 0;
+
+	cmdqelmt->command = Oid;
+	cmdqelmt->CmdFromNdis = TRUE;
+	if (SetInformation == TRUE)
+		cmdqelmt->SetOperation = TRUE;
+	else
+		cmdqelmt->SetOperation = FALSE;
+
+	NdisAcquireSpinLock(&pAd->CmdQLock);
+	if (pAd->CmdQ.CmdQState & RT2870_THREAD_CAN_DO_INSERT)
+	{
+		EnqueueCmd((&pAd->CmdQ), cmdqelmt);
+		status = NDIS_STATUS_SUCCESS;
+	}
+	else
+	{
+		status = NDIS_STATUS_FAILURE;
+	}
+	NdisReleaseSpinLock(&pAd->CmdQLock);
+
+	if (status == NDIS_STATUS_FAILURE)
+	{
+		if (cmdqelmt->buffer)
+			NdisFreeMemory(cmdqelmt->buffer, cmdqelmt->bufferlength, 0);
+		NdisFreeMemory(cmdqelmt, sizeof(CmdQElmt), 0);
+	}
+	else
+	RTUSBCMDUp(pAd);
+
+
+    return(NDIS_STATUS_SUCCESS);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS RTUSBEnqueueInternalCmd(
+	IN PRTMP_ADAPTER	pAd,
+	IN NDIS_OID			Oid,
+	IN PVOID			pInformationBuffer,
+	IN UINT32			InformationBufferLength)
+{
+	NDIS_STATUS	status;
+	PCmdQElmt	cmdqelmt = NULL;
+
+
+	status = RTMPAllocateMemory((PVOID *)&cmdqelmt, sizeof(CmdQElmt));
+	if ((status != NDIS_STATUS_SUCCESS) || (cmdqelmt == NULL))
+		return (NDIS_STATUS_RESOURCES);
+	NdisZeroMemory(cmdqelmt, sizeof(CmdQElmt));
+
+	if(InformationBufferLength > 0)
+	{
+		status = RTMPAllocateMemory((PVOID *)&cmdqelmt->buffer, InformationBufferLength);
+		if ((status != NDIS_STATUS_SUCCESS) || (cmdqelmt->buffer == NULL))
+		{
+			NdisFreeMemory(cmdqelmt, sizeof(CmdQElmt), 0);
+			return (NDIS_STATUS_RESOURCES);
+		}
+		else
+		{
+			NdisMoveMemory(cmdqelmt->buffer, pInformationBuffer, InformationBufferLength);
+			cmdqelmt->bufferlength = InformationBufferLength;
+		}
+	}
+	else
+	{
+		cmdqelmt->buffer = NULL;
+		cmdqelmt->bufferlength = 0;
+	}
+
+	cmdqelmt->command = Oid;
+	cmdqelmt->CmdFromNdis = FALSE;
+
+	if (cmdqelmt != NULL)
+	{
+		NdisAcquireSpinLock(&pAd->CmdQLock);
+		if (pAd->CmdQ.CmdQState & RT2870_THREAD_CAN_DO_INSERT)
+		{
+			EnqueueCmd((&pAd->CmdQ), cmdqelmt);
+			status = NDIS_STATUS_SUCCESS;
+		}
+		else
+		{
+			status = NDIS_STATUS_FAILURE;
+		}
+		NdisReleaseSpinLock(&pAd->CmdQLock);
+
+		if (status == NDIS_STATUS_FAILURE)
+		{
+			if (cmdqelmt->buffer)
+				NdisFreeMemory(cmdqelmt->buffer, cmdqelmt->bufferlength, 0);
+			NdisFreeMemory(cmdqelmt, sizeof(CmdQElmt), 0);
+		}
+		else
+		RTUSBCMDUp(pAd);
+	}
+	return(NDIS_STATUS_SUCCESS);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	IRQL =
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTUSBDequeueCmd(
+	IN	PCmdQ		cmdq,
+	OUT	PCmdQElmt	*pcmdqelmt)
+{
+	*pcmdqelmt = cmdq->head;
+
+	if (*pcmdqelmt != NULL)
+	{
+		cmdq->head = cmdq->head->next;
+		cmdq->size--;
+		if (cmdq->size == 0)
+			cmdq->tail = NULL;
+	}
+}
+
+/*
+    ========================================================================
+	  usb_control_msg - Builds a control urb, sends it off and waits for completion
+	  @dev: pointer to the usb device to send the message to
+	  @pipe: endpoint "pipe" to send the message to
+	  @request: USB message request value
+	  @requesttype: USB message request type value
+	  @value: USB message value
+	  @index: USB message index value
+	  @data: pointer to the data to send
+	  @size: length in bytes of the data to send
+	  @timeout: time in jiffies to wait for the message to complete before
+			  timing out (if 0 the wait is forever)
+	  Context: !in_interrupt ()
+
+	  This function sends a simple control message to a specified endpoint
+	  and waits for the message to complete, or timeout.
+	  If successful, it returns the number of bytes transferred, otherwise a negative error number.
+
+	 Don't use this function from within an interrupt context, like a
+	  bottom half handler.	If you need an asynchronous message, or need to send
+	  a message from within interrupt context, use usb_submit_urb()
+	  If a thread in your driver uses this call, make sure your disconnect()
+	  method can wait for it to complete.  Since you don't have a handle on
+	  the URB used, you can't cancel the request.
+
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS    RTUSB_VendorRequest(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UINT32			TransferFlags,
+	IN	UCHAR			RequestType,
+	IN	UCHAR			Request,
+	IN	USHORT			Value,
+	IN	USHORT			Index,
+	IN	PVOID			TransferBuffer,
+	IN	UINT32			TransferBufferLength)
+{
+	int				ret;
+	POS_COOKIE		pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("device disconnected\n"));
+		return -1;
+	}
+	else if (in_interrupt())
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("in_interrupt, RTUSB_VendorRequest Request%02x Value%04x Offset%04x\n",Request,Value,Index));
+
+		return -1;
+	}
+	else
+	{
+#define MAX_RETRY_COUNT  10
+
+		int retryCount = 0;
+		void	*tmpBuf = TransferBuffer;
+
+		// Acquire Control token
+#ifdef INF_AMAZON_SE
+		//Semaphore fix INF_AMAZON_SE hang
+		//pAd->UsbVendorReqBuf is the swap for DEVICE_VENDOR_REQUEST_IN to fix dma bug.
+		ret = down_interruptible(&(pAd->UsbVendorReq_semaphore));
+		if (pAd->UsbVendorReqBuf)
+		{
+			ASSERT(TransferBufferLength <MAX_PARAM_BUFFER_SIZE);
+
+		   	tmpBuf = (void *)pAd->UsbVendorReqBuf;
+		   	NdisZeroMemory(pAd->UsbVendorReqBuf, TransferBufferLength);
+
+		   	if (RequestType == DEVICE_VENDOR_REQUEST_OUT)
+		   	 NdisMoveMemory(tmpBuf, TransferBuffer, TransferBufferLength);
+		}
+#endif // INF_AMAZON_SE //
+		do {
+		if( RequestType == DEVICE_VENDOR_REQUEST_OUT)
+			ret=usb_control_msg(pObj->pUsb_Dev, usb_sndctrlpipe( pObj->pUsb_Dev, 0 ), Request, RequestType, Value,Index, tmpBuf, TransferBufferLength, CONTROL_TIMEOUT_JIFFIES);
+		else if(RequestType == DEVICE_VENDOR_REQUEST_IN)
+			ret=usb_control_msg(pObj->pUsb_Dev, usb_rcvctrlpipe( pObj->pUsb_Dev, 0 ), Request, RequestType, Value,Index, tmpBuf, TransferBufferLength, CONTROL_TIMEOUT_JIFFIES);
+		else
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("vendor request direction is failed\n"));
+			ret = -1;
+		}
+
+			retryCount++;
+			if (ret < 0) {
+				printk("#\n");
+				RTMPusecDelay(5000);
+			}
+		} while((ret < 0) && (retryCount < MAX_RETRY_COUNT));
+
+#ifdef INF_AMAZON_SE
+	  	if ((pAd->UsbVendorReqBuf) && (RequestType == DEVICE_VENDOR_REQUEST_IN))
+			NdisMoveMemory(TransferBuffer, tmpBuf, TransferBufferLength);
+	  	up(&(pAd->UsbVendorReq_semaphore));
+#endif // INF_AMAZON_SE //
+
+        if (ret < 0) {
+//			DBGPRINT(RT_DEBUG_ERROR, ("USBVendorRequest failed ret=%d \n",ret));
+			DBGPRINT(RT_DEBUG_ERROR, ("RTUSB_VendorRequest failed(%d),TxFlags=0x%x, ReqType=%s, Req=0x%x, Index=0x%x\n",
+						ret, TransferFlags, (RequestType == DEVICE_VENDOR_REQUEST_OUT ? "OUT" : "IN"), Request, Index));
+			if (Request == 0x2)
+				DBGPRINT(RT_DEBUG_ERROR, ("\tRequest Value=0x%04x!\n", Value));
+
+			if ((TransferBuffer!= NULL) && (TransferBufferLength > 0))
+				hex_dump("Failed TransferBuffer value", TransferBuffer, TransferBufferLength);
+        }
+
+#if 0
+        // retry
+		if (ret < 0) {
+			int temp_i=0;
+			DBGPRINT(RT_DEBUG_ERROR, ("USBVendorRequest failed ret=%d, \n",ret));
+			ret = 0;
+			do
+			{
+				if( RequestType == DEVICE_VENDOR_REQUEST_OUT)
+					ret=usb_control_msg(pObj->pUsb_Dev, usb_sndctrlpipe( pObj->pUsb_Dev, 0 ), Request, RequestType, Value,Index, TransferBuffer, TransferBufferLength, CONTROL_TIMEOUT_JIFFIES);
+				else if(RequestType == DEVICE_VENDOR_REQUEST_IN)
+					ret=usb_control_msg(pObj->pUsb_Dev, usb_rcvctrlpipe( pObj->pUsb_Dev, 0 ), Request, RequestType, Value,Index, TransferBuffer, TransferBufferLength, CONTROL_TIMEOUT_JIFFIES);
+				temp_i++;
+			} while( (ret < 0) && (temp_i <= 1) );
+
+			if( ret >= 0)
+				return ret;
+
+		}
+#endif
+
+	}
+	return ret;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+	  Creates an IRP to submite an IOCTL_INTERNAL_USB_RESET_PORT
+	  synchronously. Callers of this function must be running at
+	  PASSIVE LEVEL.
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+NTSTATUS	RTUSB_ResetDevice(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	NTSTATUS		Status = TRUE;
+
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("--->USB_ResetDevice\n"));
+	//RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS);
+	return Status;
+}
+
+VOID CMDHandler(
+    IN PRTMP_ADAPTER pAd)
+{
+	PCmdQElmt		cmdqelmt;
+	PUCHAR			pData;
+	NDIS_STATUS		NdisStatus = NDIS_STATUS_SUCCESS;
+//	ULONG			Now = 0;
+	NTSTATUS		ntStatus;
+//	unsigned long	IrqFlags;
+
+	while (pAd->CmdQ.size > 0)
+	{
+		NdisStatus = NDIS_STATUS_SUCCESS;
+
+		NdisAcquireSpinLock(&pAd->CmdQLock);
+		RTUSBDequeueCmd(&pAd->CmdQ, &cmdqelmt);
+		NdisReleaseSpinLock(&pAd->CmdQLock);
+
+		if (cmdqelmt == NULL)
+			break;
+
+		pData = cmdqelmt->buffer;
+
+		if(!(RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST) || RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)))
+		{
+			switch (cmdqelmt->command)
+			{
+				case CMDTHREAD_CHECK_GPIO:
+					{
+#ifdef CONFIG_STA_SUPPORT
+						UINT32 data;
+#endif // CONFIG_STA_SUPPORT //
+#ifdef RALINK_ATE
+       					if(ATE_ON(pAd))
+						{
+							DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+							break;
+						}
+#endif // RALINK_ATE //
+
+#ifdef CONFIG_STA_SUPPORT
+
+
+						IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+						{
+							// Read GPIO pin2 as Hardware controlled radio state
+
+							RTUSBReadMACRegister( pAd, GPIO_CTRL_CFG, &data);
+
+							if (data & 0x04)
+							{
+								pAd->StaCfg.bHwRadio = TRUE;
+							}
+							else
+							{
+								pAd->StaCfg.bHwRadio = FALSE;
+							}
+
+							if(pAd->StaCfg.bRadio != (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio))
+							{
+								pAd->StaCfg.bRadio = (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio);
+								if(pAd->StaCfg.bRadio == TRUE)
+								{
+									DBGPRINT_RAW(RT_DEBUG_ERROR, ("!!! Radio On !!!\n"));
+
+									MlmeRadioOn(pAd);
+									// Update extra information
+									pAd->ExtraInfo = EXTRA_INFO_CLEAR;
+								}
+								else
+								{
+									DBGPRINT_RAW(RT_DEBUG_ERROR, ("!!! Radio Off !!!\n"));
+
+									MlmeRadioOff(pAd);
+									// Update extra information
+									pAd->ExtraInfo = HW_RADIO_OFF;
+								}
+							}
+						}
+#endif // CONFIG_STA_SUPPORT //
+					}
+					break;
+
+#ifdef CONFIG_STA_SUPPORT
+				case CMDTHREAD_QKERIODIC_EXECUT:
+					{
+						StaQuickResponeForRateUpExec(NULL, pAd, NULL, NULL);
+					}
+					break;
+#endif // CONFIG_STA_SUPPORT //
+
+				case CMDTHREAD_RESET_BULK_OUT:
+					{
+						UINT32		MACValue;
+						UCHAR		Index;
+						int			ret=0;
+						PHT_TX_CONTEXT	pHTTXContext;
+//						RTMP_TX_RING *pTxRing;
+						unsigned long IrqFlags;
+#ifdef RALINK_ATE
+						PTX_CONTEXT		pNullContext = &(pAd->NullContext);
+#endif // RALINK_ATE //
+						DBGPRINT_RAW(RT_DEBUG_TRACE, ("CmdThread : CMDTHREAD_RESET_BULK_OUT(ResetPipeid=0x%0x)===>\n", pAd->bulkResetPipeid));
+						// All transfers must be aborted or cancelled before attempting to reset the pipe.
+						//RTUSBCancelPendingBulkOutIRP(pAd);
+						// Wait 10ms to let previous packet that are already in HW FIFO to clear. by MAXLEE 12-25-2007
+						Index = 0;
+						do
+						{
+							RTUSBReadMACRegister(pAd, TXRXQ_PCNT, &MACValue);
+							if ((MACValue & 0xf00000/*0x800000*/) == 0)
+								break;
+							Index++;
+							RTMPusecDelay(10000);
+						}while(Index < 100);
+						MACValue = 0;
+						RTUSBReadMACRegister(pAd, USB_DMA_CFG, &MACValue);
+						// To prevent Read Register error, we 2nd check the validity.
+						if ((MACValue & 0xc00000) == 0)
+							RTUSBReadMACRegister(pAd, USB_DMA_CFG, &MACValue);
+						// To prevent Read Register error, we 3rd check the validity.
+						if ((MACValue & 0xc00000) == 0)
+							RTUSBReadMACRegister(pAd, USB_DMA_CFG, &MACValue);
+						MACValue |= 0x80000;
+						RTUSBWriteMACRegister(pAd, USB_DMA_CFG, MACValue);
+
+						// Wait 1ms to prevent next URB to bulkout before HW reset. by MAXLEE 12-25-2007
+						RTMPusecDelay(1000);
+
+						MACValue &= (~0x80000);
+						RTUSBWriteMACRegister(pAd, USB_DMA_CFG, MACValue);
+						DBGPRINT_RAW(RT_DEBUG_TRACE, ("\tSet 0x2a0 bit19. Clear USB DMA TX path\n"));
+
+						// Wait 5ms to prevent next URB to bulkout before HW reset. by MAXLEE 12-25-2007
+						//RTMPusecDelay(5000);
+
+						if ((pAd->bulkResetPipeid & BULKOUT_MGMT_RESET_FLAG) == BULKOUT_MGMT_RESET_FLAG)
+						{
+							RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+							if (pAd->MgmtRing.TxSwFreeIdx < MGMT_RING_SIZE /* pMLMEContext->bWaitingBulkOut == TRUE */)
+							{
+								RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_MLME);
+							}
+							RTUSBKickBulkOut(pAd);
+
+							DBGPRINT_RAW(RT_DEBUG_TRACE, ("\tTX MGMT RECOVER Done!\n"));
+						}
+						else
+						{
+							pHTTXContext = &(pAd->TxContext[pAd->bulkResetPipeid]);
+							//NdisAcquireSpinLock(&pAd->BulkOutLock[pAd->bulkResetPipeid]);
+							RTMP_INT_LOCK(&pAd->BulkOutLock[pAd->bulkResetPipeid], IrqFlags);
+							if ( pAd->BulkOutPending[pAd->bulkResetPipeid] == FALSE)
+							{
+								pAd->BulkOutPending[pAd->bulkResetPipeid] = TRUE;
+								pHTTXContext->IRPPending = TRUE;
+								pAd->watchDogTxPendingCnt[pAd->bulkResetPipeid] = 1;
+
+								// no matter what, clean the flag
+								RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+
+								//NdisReleaseSpinLock(&pAd->BulkOutLock[pAd->bulkResetPipeid]);
+								RTMP_INT_UNLOCK(&pAd->BulkOutLock[pAd->bulkResetPipeid], IrqFlags);
+/*-----------------------------------------------------------------------------------------------*/
+#ifdef RALINK_ATE
+								if(ATE_ON(pAd))
+							    {
+									pNullContext->IRPPending = TRUE;
+									//
+									// If driver is still in ATE TXFRAME mode,
+									// keep on transmitting ATE frames.
+									//
+									DBGPRINT_RAW(RT_DEBUG_TRACE, ("pAd->ate.Mode == %d\npAd->ContinBulkOut == %d\npAd->BulkOutRemained == %d\n", pAd->ate.Mode, pAd->ContinBulkOut, atomic_read(&pAd->BulkOutRemained)));
+									if((pAd->ate.Mode == ATE_TXFRAME) && ((pAd->ContinBulkOut == TRUE) || (atomic_read(&pAd->BulkOutRemained) > 0)))
+								    {
+										DBGPRINT_RAW(RT_DEBUG_TRACE, ("After CMDTHREAD_RESET_BULK_OUT, continue to bulk out frames !\n"));
+
+										// Init Tx context descriptor
+										RTUSBInitTxDesc(pAd, pNullContext, 0/* pAd->bulkResetPipeid */, (usb_complete_t)ATE_RTUSBBulkOutDataPacketComplete);
+
+										if((ret = RTUSB_SUBMIT_URB(pNullContext->pUrb))!=0)
+										{
+											DBGPRINT(RT_DEBUG_ERROR, ("ATE_RTUSBBulkOutDataPacket: Submit Tx URB failed %d\n", ret));
+										}
+
+										pAd->BulkOutReq++;
+									}
+								}
+								else
+#endif // RALINK_ATE //
+/*-----------------------------------------------------------------------------------------------*/
+								{
+								RTUSBInitHTTxDesc(pAd, pHTTXContext, pAd->bulkResetPipeid, pHTTXContext->BulkOutSize, (usb_complete_t)RTUSBBulkOutDataPacketComplete);
+
+								if((ret = RTUSB_SUBMIT_URB(pHTTXContext->pUrb))!=0)
+								{
+										RTMP_INT_LOCK(&pAd->BulkOutLock[pAd->bulkResetPipeid], IrqFlags);
+									pAd->BulkOutPending[pAd->bulkResetPipeid] = FALSE;
+									pHTTXContext->IRPPending = FALSE;
+										pAd->watchDogTxPendingCnt[pAd->bulkResetPipeid] = 0;
+										RTMP_INT_UNLOCK(&pAd->BulkOutLock[pAd->bulkResetPipeid], IrqFlags);
+
+										DBGPRINT(RT_DEBUG_ERROR, ("CmdThread : CMDTHREAD_RESET_BULK_OUT: Submit Tx URB failed %d\n", ret));
+								}
+									else
+									{
+										RTMP_IRQ_LOCK(&pAd->BulkOutLock[pAd->bulkResetPipeid], IrqFlags);
+										DBGPRINT_RAW(RT_DEBUG_TRACE,("\tCMDTHREAD_RESET_BULK_OUT: TxContext[%d]:CWPos=%ld, NBPos=%ld, ENBPos=%ld, bCopy=%d, pending=%d!\n",
+												pAd->bulkResetPipeid, pHTTXContext->CurWritePosition, pHTTXContext->NextBulkOutPosition,
+															pHTTXContext->ENextBulkOutPosition, pHTTXContext->bCopySavePad, pAd->BulkOutPending[pAd->bulkResetPipeid]));
+										DBGPRINT_RAW(RT_DEBUG_TRACE,("\t\tBulkOut Req=0x%lx, Complete=0x%lx, Other=0x%lx\n",
+															pAd->BulkOutReq, pAd->BulkOutComplete, pAd->BulkOutCompleteOther));
+										RTMP_IRQ_UNLOCK(&pAd->BulkOutLock[pAd->bulkResetPipeid], IrqFlags);
+										DBGPRINT_RAW(RT_DEBUG_TRACE, ("\tCMDTHREAD_RESET_BULK_OUT: Submit Tx DATA URB for failed BulkReq(0x%lx) Done, status=%d!\n", pAd->bulkResetReq[pAd->bulkResetPipeid], pHTTXContext->pUrb->status));
+
+									}
+								}
+							}
+							else
+							{
+								//NdisReleaseSpinLock(&pAd->BulkOutLock[pAd->bulkResetPipeid]);
+								//RTMP_INT_UNLOCK(&pAd->BulkOutLock[pAd->bulkResetPipeid], IrqFlags);
+
+								DBGPRINT_RAW(RT_DEBUG_ERROR, ("CmdThread : TX DATA RECOVER FAIL for BulkReq(0x%lx) because BulkOutPending[%d] is TRUE!\n", pAd->bulkResetReq[pAd->bulkResetPipeid], pAd->bulkResetPipeid));
+								if (pAd->bulkResetPipeid == 0)
+								{
+									UCHAR	pendingContext = 0;
+									PHT_TX_CONTEXT pHTTXContext = (PHT_TX_CONTEXT)(&pAd->TxContext[pAd->bulkResetPipeid ]);
+									PTX_CONTEXT pMLMEContext = (PTX_CONTEXT)(pAd->MgmtRing.Cell[pAd->MgmtRing.TxDmaIdx].AllocVa);
+									PTX_CONTEXT pNULLContext = (PTX_CONTEXT)(&pAd->PsPollContext);
+									PTX_CONTEXT pPsPollContext = (PTX_CONTEXT)(&pAd->NullContext);
+
+									if (pHTTXContext->IRPPending)
+										pendingContext |= 1;
+									else if (pMLMEContext->IRPPending)
+										pendingContext |= 2;
+									else if (pNULLContext->IRPPending)
+										pendingContext |= 4;
+									else if (pPsPollContext->IRPPending)
+										pendingContext |= 8;
+									else
+										pendingContext = 0;
+
+									DBGPRINT_RAW(RT_DEBUG_ERROR, ("\tTX Occupied by %d!\n", pendingContext));
+								}
+
+							// no matter what, clean the flag
+							RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+
+								RTMP_INT_UNLOCK(&pAd->BulkOutLock[pAd->bulkResetPipeid], IrqFlags);
+
+								RTUSB_SET_BULK_FLAG(pAd, (fRTUSB_BULK_OUT_DATA_NORMAL << pAd->bulkResetPipeid));
+							}
+
+							RTMPDeQueuePacket(pAd, FALSE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+							//RTUSBKickBulkOut(pAd);
+						}
+
+					}
+					/*
+						// Don't cancel BULKIN.
+						while ((atomic_read(&pAd->PendingRx) > 0) &&
+								(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+						{
+							if (atomic_read(&pAd->PendingRx) > 0)
+							{
+								DBGPRINT_RAW(RT_DEBUG_ERROR, ("BulkIn IRP Pending!!cancel it!\n"));
+								RTUSBCancelPendingBulkInIRP(pAd);
+							}
+							RTMPusecDelay(100000);
+						}
+
+						if ((atomic_read(&pAd->PendingRx) == 0) && (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)))
+						{
+							UCHAR	i;
+							RTUSBRxPacket(pAd);
+							pAd->NextRxBulkInReadIndex = 0;	// Next Rx Read index
+							pAd->NextRxBulkInIndex		= 0;	// Rx Bulk pointer
+							for (i = 0; i < (RX_RING_SIZE); i++)
+							{
+								PRX_CONTEXT  pRxContext = &(pAd->RxContext[i]);
+
+								pRxContext->pAd	= pAd;
+								pRxContext->InUse		= FALSE;
+								pRxContext->IRPPending	= FALSE;
+								pRxContext->Readable	= FALSE;
+								pRxContext->ReorderInUse = FALSE;
+
+							}
+							RTUSBBulkReceive(pAd);
+							DBGPRINT_RAW(RT_DEBUG_ERROR, ("RTUSBBulkReceive\n"));
+						}*/
+					DBGPRINT_RAW(RT_DEBUG_TRACE, ("CmdThread : CMDTHREAD_RESET_BULK_OUT<===\n"));
+    	   			break;
+
+				case CMDTHREAD_RESET_BULK_IN:
+					DBGPRINT_RAW(RT_DEBUG_TRACE, ("CmdThread : CMDTHREAD_RESET_BULK_IN === >\n"));
+
+					// All transfers must be aborted or cancelled before attempting to reset the pipe.
+					{
+						UINT32		MACValue;
+/*-----------------------------------------------------------------------------------------------*/
+#ifdef RALINK_ATE
+						if (ATE_ON(pAd))
+						{
+							if((pAd->PendingRx > 0) && (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+							{
+								DBGPRINT_RAW(RT_DEBUG_ERROR, ("ATE : BulkIn IRP Pending!!!\n"));
+								ATE_RTUSBCancelPendingBulkInIRP(pAd);
+								RTMPusecDelay(100000);
+								pAd->PendingRx = 0;
+							}
+						}
+						else
+#endif // RALINK_ATE //
+/*-----------------------------------------------------------------------------------------------*/
+						{
+						//while ((atomic_read(&pAd->PendingRx) > 0) && (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+						if((pAd->PendingRx > 0) && (!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+						{
+							DBGPRINT_RAW(RT_DEBUG_ERROR, ("BulkIn IRP Pending!!!\n"));
+							RTUSBCancelPendingBulkInIRP(pAd);
+							RTMPusecDelay(100000);
+							pAd->PendingRx = 0;
+						}
+						}
+
+						// Wait 10ms before reading register.
+						RTMPusecDelay(10000);
+						ntStatus = RTUSBReadMACRegister(pAd, MAC_CSR0, &MACValue);
+
+						if ((NT_SUCCESS(ntStatus) == TRUE) &&
+							(!(RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_RESET_IN_PROGRESS | fRTMP_ADAPTER_RADIO_OFF |
+													fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST)))))
+						{
+							UCHAR	i;
+
+							if (RTMP_TEST_FLAG(pAd, (fRTMP_ADAPTER_RESET_IN_PROGRESS | fRTMP_ADAPTER_RADIO_OFF |
+														fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST)))
+								break;
+							pAd->NextRxBulkInPosition = pAd->RxContext[pAd->NextRxBulkInIndex].BulkInOffset;
+							DBGPRINT(RT_DEBUG_TRACE, ("BULK_IN_RESET: NBIIdx=0x%x,NBIRIdx=0x%x, BIRPos=0x%lx. BIReq=x%lx, BIComplete=0x%lx, BICFail0x%lx\n",
+									pAd->NextRxBulkInIndex,  pAd->NextRxBulkInReadIndex, pAd->NextRxBulkInPosition, pAd->BulkInReq, pAd->BulkInComplete, pAd->BulkInCompleteFail));
+							for (i = 0; i < RX_RING_SIZE; i++)
+							{
+ 								DBGPRINT(RT_DEBUG_TRACE, ("\tRxContext[%d]: IRPPending=%d, InUse=%d, Readable=%d!\n"
+									, i, pAd->RxContext[i].IRPPending, pAd->RxContext[i].InUse, pAd->RxContext[i].Readable));
+							}
+ 							/*
+
+							DBGPRINT_RAW(RT_DEBUG_ERROR, ("==========================================\n"));
+
+							pAd->NextRxBulkInReadIndex = 0;	// Next Rx Read index
+							pAd->NextRxBulkInIndex		= 0;	// Rx Bulk pointer
+							for (i = 0; i < (RX_RING_SIZE); i++)
+							{
+								PRX_CONTEXT  pRxContext = &(pAd->RxContext[i]);
+
+								pRxContext->pAd	= pAd;
+								pRxContext->InUse		= FALSE;
+								pRxContext->IRPPending	= FALSE;
+								pRxContext->Readable	= FALSE;
+								pRxContext->ReorderInUse = FALSE;
+
+							}*/
+							RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BULKIN_RESET);
+							for (i = 0; i < pAd->CommonCfg.NumOfBulkInIRP; i++)
+							{
+								//RTUSBBulkReceive(pAd);
+								PRX_CONTEXT		pRxContext;
+								PURB			pUrb;
+								int				ret = 0;
+								unsigned long	IrqFlags;
+
+
+								RTMP_IRQ_LOCK(&pAd->BulkInLock, IrqFlags);
+								pRxContext = &(pAd->RxContext[pAd->NextRxBulkInIndex]);
+								if ((pAd->PendingRx > 0) || (pRxContext->Readable == TRUE) || (pRxContext->InUse == TRUE))
+								{
+									RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+									break;
+								}
+								pRxContext->InUse = TRUE;
+								pRxContext->IRPPending = TRUE;
+								pAd->PendingRx++;
+								pAd->BulkInReq++;
+								RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+
+								// Init Rx context descriptor
+								RTUSBInitRxDesc(pAd, pRxContext);
+								pUrb = pRxContext->pUrb;
+								if ((ret = RTUSB_SUBMIT_URB(pUrb))!=0)
+								{	// fail
+
+									RTMP_IRQ_LOCK(&pAd->BulkInLock, IrqFlags);
+									pRxContext->InUse = FALSE;
+									pRxContext->IRPPending = FALSE;
+									pAd->PendingRx--;
+									pAd->BulkInReq--;
+									RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+									DBGPRINT(RT_DEBUG_ERROR, ("CMDTHREAD_RESET_BULK_IN: Submit Rx URB failed(%d), status=%d\n", ret, pUrb->status));
+								}
+								else
+								{	// success
+#if 0
+									RTMP_IRQ_LOCK(&pAd->BulkInLock, IrqFlags);
+									pRxContext->IRPPending = TRUE;
+									//NdisInterlockedIncrement(&pAd->PendingRx);
+									pAd->PendingRx++;
+									RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+									pAd->BulkInReq++;
+#endif
+									//printk("BIDone, Pend=%d,BIIdx=%d,BIRIdx=%d!\n", pAd->PendingRx, pAd->NextRxBulkInIndex, pAd->NextRxBulkInReadIndex);
+									DBGPRINT_RAW(RT_DEBUG_TRACE, ("CMDTHREAD_RESET_BULK_IN: Submit Rx URB Done, status=%d!\n", pUrb->status));
+									ASSERT((pRxContext->InUse == pRxContext->IRPPending));
+								}
+							}
+
+						}
+						else
+						{
+							// Card must be removed
+							if (NT_SUCCESS(ntStatus) != TRUE)
+							{
+							RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST);
+								DBGPRINT_RAW(RT_DEBUG_ERROR, ("CMDTHREAD_RESET_BULK_IN: Read Register Failed!Card must be removed!!\n\n"));
+							}
+							else
+							{
+								DBGPRINT_RAW(RT_DEBUG_ERROR, ("CMDTHREAD_RESET_BULK_IN: Cannot do bulk in because flags(0x%lx) on !\n", pAd->Flags));
+						}
+					}
+					}
+					DBGPRINT_RAW(RT_DEBUG_TRACE, ("CmdThread : CMDTHREAD_RESET_BULK_IN <===\n"));
+					break;
+
+				case CMDTHREAD_SET_ASIC_WCID:
+					{
+						RT_SET_ASIC_WCID	SetAsicWcid;
+						USHORT		offset;
+						UINT32		MACValue, MACRValue = 0;
+						SetAsicWcid = *((PRT_SET_ASIC_WCID)(pData));
+
+						if (SetAsicWcid.WCID >= MAX_LEN_OF_MAC_TABLE)
+							return;
+
+						offset = MAC_WCID_BASE + ((UCHAR)SetAsicWcid.WCID)*HW_WCID_ENTRY_SIZE;
+
+						DBGPRINT_RAW(RT_DEBUG_TRACE, ("CmdThread : CMDTHREAD_SET_ASIC_WCID : WCID = %ld, SetTid  = %lx, DeleteTid = %lx.\n", SetAsicWcid.WCID, SetAsicWcid.SetTid, SetAsicWcid.DeleteTid));
+						MACValue = (pAd->MacTab.Content[SetAsicWcid.WCID].Addr[3]<<24)+(pAd->MacTab.Content[SetAsicWcid.WCID].Addr[2]<<16)+(pAd->MacTab.Content[SetAsicWcid.WCID].Addr[1]<<8)+(pAd->MacTab.Content[SetAsicWcid.WCID].Addr[0]);
+						DBGPRINT_RAW(RT_DEBUG_TRACE, ("1-MACValue= %x,\n", MACValue));
+						RTUSBWriteMACRegister(pAd, offset, MACValue);
+						// Read bitmask
+						RTUSBReadMACRegister(pAd, offset+4, &MACRValue);
+						if ( SetAsicWcid.DeleteTid != 0xffffffff)
+							MACRValue &= (~SetAsicWcid.DeleteTid);
+						if (SetAsicWcid.SetTid != 0xffffffff)
+							MACRValue |= (SetAsicWcid.SetTid);
+						MACRValue &= 0xffff0000;
+
+						MACValue = (pAd->MacTab.Content[SetAsicWcid.WCID].Addr[5]<<8)+pAd->MacTab.Content[SetAsicWcid.WCID].Addr[4];
+						MACValue |= MACRValue;
+						RTUSBWriteMACRegister(pAd, offset+4, MACValue);
+
+						DBGPRINT_RAW(RT_DEBUG_TRACE, ("2-MACValue= %x,\n", MACValue));
+					}
+					break;
+
+				case CMDTHREAD_SET_ASIC_WCID_CIPHER:
+					{
+#ifdef CONFIG_STA_SUPPORT
+						RT_SET_ASIC_WCID_ATTRI	SetAsicWcidAttri;
+						USHORT		offset;
+						UINT32		MACRValue = 0;
+						SHAREDKEY_MODE_STRUC csr1;
+						SetAsicWcidAttri = *((PRT_SET_ASIC_WCID_ATTRI)(pData));
+
+						if (SetAsicWcidAttri.WCID >= MAX_LEN_OF_MAC_TABLE)
+							return;
+
+						offset = MAC_WCID_ATTRIBUTE_BASE + ((UCHAR)SetAsicWcidAttri.WCID)*HW_WCID_ATTRI_SIZE;
+
+						DBGPRINT_RAW(RT_DEBUG_TRACE, ("Cmd : CMDTHREAD_SET_ASIC_WCID_CIPHER : WCID = %ld, Cipher = %lx.\n", SetAsicWcidAttri.WCID, SetAsicWcidAttri.Cipher));
+						// Read bitmask
+						RTUSBReadMACRegister(pAd, offset, &MACRValue);
+						MACRValue = 0;
+						MACRValue |= (((UCHAR)SetAsicWcidAttri.Cipher) << 1);
+
+						RTUSBWriteMACRegister(pAd, offset, MACRValue);
+						DBGPRINT_RAW(RT_DEBUG_TRACE, ("2-offset = %x , MACValue= %x,\n", offset, MACRValue));
+
+						offset = PAIRWISE_IVEIV_TABLE_BASE + ((UCHAR)SetAsicWcidAttri.WCID)*HW_IVEIV_ENTRY_SIZE;
+						MACRValue = 0;
+						if ( (SetAsicWcidAttri.Cipher <= CIPHER_WEP128))
+							MACRValue |= ( pAd->StaCfg.DefaultKeyId << 30);
+						else
+							MACRValue |= (0x20000000);
+						RTUSBWriteMACRegister(pAd, offset, MACRValue);
+						DBGPRINT_RAW(RT_DEBUG_TRACE, ("2-offset = %x , MACValue= %x,\n", offset, MACRValue));
+
+						//
+						// Update cipher algorithm. WSTA always use BSS0
+						//
+						// for adhoc mode only ,because wep status slow than add key, when use zero config
+						if (pAd->StaCfg.BssType == BSS_ADHOC )
+						{
+							offset = MAC_WCID_ATTRIBUTE_BASE;
+
+							RTUSBReadMACRegister(pAd, offset, &MACRValue);
+							MACRValue &= (~0xe);
+							MACRValue |= (((UCHAR)SetAsicWcidAttri.Cipher) << 1);
+
+							RTUSBWriteMACRegister(pAd, offset, MACRValue);
+
+							//Update group key cipher,,because wep status slow than add key, when use zero config
+							RTUSBReadMACRegister(pAd, SHARED_KEY_MODE_BASE+4*(0/2), &csr1.word);
+
+							csr1.field.Bss0Key0CipherAlg = SetAsicWcidAttri.Cipher;
+							csr1.field.Bss0Key1CipherAlg = SetAsicWcidAttri.Cipher;
+
+							RTUSBWriteMACRegister(pAd, SHARED_KEY_MODE_BASE+4*(0/2), csr1.word);
+						}
+#endif // CONFIG_STA_SUPPORT //
+					}
+					break;
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+				// avoid in interrupt when write key
+				case RT_CMD_SET_KEY_TABLE: //General call for AsicAddPairwiseKeyEntry()
+					{
+						RT_ADD_PAIRWISE_KEY_ENTRY KeyInfo;
+						KeyInfo  = *((PRT_ADD_PAIRWISE_KEY_ENTRY)(pData));
+						AsicAddPairwiseKeyEntry(pAd,
+												KeyInfo.MacAddr,
+												(UCHAR)KeyInfo.MacTabMatchWCID,
+												&KeyInfo.CipherKey);
+					}
+					break;
+
+				case RT_CMD_SET_RX_WCID_TABLE: //General call for RTMPAddWcidAttributeEntry()
+					{
+						PMAC_TABLE_ENTRY pEntry ;
+						pEntry = (PMAC_TABLE_ENTRY)(pData);
+						RTMPAddWcidAttributeEntry(pAd,
+													BSS0,
+													0,
+													pEntry->PairwiseKey.CipherAlg,
+													pEntry);
+					}
+					break;
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+				case CMDTHREAD_SET_CLIENT_MAC_ENTRY:
+					{
+						MAC_TABLE_ENTRY *pEntry;
+						pEntry = (MAC_TABLE_ENTRY *)pData;
+
+
+#ifdef CONFIG_STA_SUPPORT
+						IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+						{
+							AsicRemovePairwiseKeyEntry(pAd, pEntry->apidx, (UCHAR)pEntry->Aid);
+							if ((pEntry->AuthMode <= Ndis802_11AuthModeAutoSwitch) && (pEntry->WepStatus == Ndis802_11Encryption1Enabled))
+							{
+								UINT32 uIV = 0;
+								PUCHAR  ptr;
+
+								ptr = (PUCHAR) &uIV;
+								*(ptr + 3) = (pAd->StaCfg.DefaultKeyId << 6);
+								AsicUpdateWCIDIVEIV(pAd, pEntry->Aid, uIV, 0);
+								AsicUpdateWCIDAttribute(pAd, pEntry->Aid, BSS0, pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg, FALSE);
+							}
+							else if (pEntry->AuthMode == Ndis802_11AuthModeWPANone)
+							{
+								UINT32 uIV = 0;
+								PUCHAR  ptr;
+
+								ptr = (PUCHAR) &uIV;
+								*(ptr + 3) = (pAd->StaCfg.DefaultKeyId << 6);
+								AsicUpdateWCIDIVEIV(pAd, pEntry->Aid, uIV, 0);
+								AsicUpdateWCIDAttribute(pAd, pEntry->Aid, BSS0, pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg, FALSE);
+							}
+							else
+							{
+								//
+								// Other case, disable engine.
+								// Don't worry WPA key, we will add WPA Key after 4-Way handshaking.
+								//
+								USHORT   offset;
+								offset = MAC_WCID_ATTRIBUTE_BASE + (pEntry->Aid * HW_WCID_ATTRI_SIZE);
+								// RX_PKEY_MODE:0 for no security; RX_KEY_TAB:0 for shared key table; BSS_IDX:0
+								RTUSBWriteMACRegister(pAd, offset, 0);
+							}
+						}
+#endif // CONFIG_STA_SUPPORT //
+
+						AsicUpdateRxWCIDTable(pAd, pEntry->Aid, pEntry->Addr);
+						printk("UpdateRxWCIDTable(): Aid=%d, Addr=%02x:%02x:%02x:%02x:%02x:%02x!\n", pEntry->Aid,
+								pEntry->Addr[0], pEntry->Addr[1], pEntry->Addr[2], pEntry->Addr[3], pEntry->Addr[4], pEntry->Addr[5]);
+					}
+					break;
+
+				case OID_802_11_ADD_WEP:
+					{
+#ifdef CONFIG_STA_SUPPORT
+						UINT	i;
+						UINT32	KeyIdx;
+						PNDIS_802_11_WEP	pWepKey;
+
+						DBGPRINT(RT_DEBUG_TRACE, ("CmdThread::OID_802_11_ADD_WEP  \n"));
+
+						pWepKey = (PNDIS_802_11_WEP)pData;
+						KeyIdx = pWepKey->KeyIndex & 0x0fffffff;
+
+						// it is a shared key
+						if ((KeyIdx >= 4) || ((pWepKey->KeyLength != 5) && (pWepKey->KeyLength != 13)))
+						{
+							NdisStatus = NDIS_STATUS_INVALID_DATA;
+							DBGPRINT(RT_DEBUG_ERROR, ("CmdThread::OID_802_11_ADD_WEP, INVALID_DATA!!\n"));
+						}
+						else
+						{
+							UCHAR CipherAlg;
+							pAd->SharedKey[BSS0][KeyIdx].KeyLen = (UCHAR) pWepKey->KeyLength;
+							NdisMoveMemory(pAd->SharedKey[BSS0][KeyIdx].Key, &pWepKey->KeyMaterial, pWepKey->KeyLength);
+							CipherAlg = (pAd->SharedKey[BSS0][KeyIdx].KeyLen == 5)? CIPHER_WEP64 : CIPHER_WEP128;
+
+							//
+							// Change the WEP cipher to CKIP cipher if CKIP KP on.
+							// Funk UI or Meetinghouse UI will add ckip key from this path.
+							//
+
+							if (pAd->OpMode == OPMODE_STA)
+						 	{
+								pAd->MacTab.Content[BSSID_WCID].PairwiseKey.CipherAlg = pAd->SharedKey[BSS0][KeyIdx].CipherAlg;
+								pAd->MacTab.Content[BSSID_WCID].PairwiseKey.KeyLen = pAd->SharedKey[BSS0][KeyIdx].KeyLen;
+						 	}
+							pAd->SharedKey[BSS0][KeyIdx].CipherAlg = CipherAlg;
+							if (pWepKey->KeyIndex & 0x80000000)
+							{
+								// Default key for tx (shared key)
+								UCHAR	IVEIV[8];
+								UINT32	WCIDAttri, Value;
+								USHORT	offset, offset2;
+								NdisZeroMemory(IVEIV, 8);
+								pAd->StaCfg.DefaultKeyId = (UCHAR) KeyIdx;
+								// Add BSSID to WCTable. because this is Tx wep key.
+								// WCID Attribute UDF:3, BSSIdx:3, Alg:3, Keytable:1=PAIRWISE KEY, BSSIdx is 0
+								WCIDAttri = (CipherAlg<<1)|SHAREDKEYTABLE;
+
+								offset = MAC_WCID_ATTRIBUTE_BASE + (BSSID_WCID* HW_WCID_ATTRI_SIZE);
+								RTUSBWriteMACRegister(pAd, offset, WCIDAttri);
+								// 1. IV/EIV
+								// Specify key index to find shared key.
+								IVEIV[3] = (UCHAR)(KeyIdx<< 6);	//WEP Eiv bit off. groupkey index is not 0
+								offset = PAIRWISE_IVEIV_TABLE_BASE + (BSS0Mcast_WCID * HW_IVEIV_ENTRY_SIZE);
+								offset2 = PAIRWISE_IVEIV_TABLE_BASE + (BSSID_WCID* HW_IVEIV_ENTRY_SIZE);
+								for (i=0; i<8;)
+								{
+									Value = IVEIV[i];
+									Value += (IVEIV[i+1]<<8);
+									Value += (IVEIV[i+2]<<16);
+									Value += (IVEIV[i+3]<<24);
+									RTUSBWriteMACRegister(pAd, offset+i, Value);
+									RTUSBWriteMACRegister(pAd, offset2+i, Value);
+									i+=4;
+								}
+
+								// 2. WCID Attribute UDF:3, BSSIdx:3, Alg:3, Keytable:use share key, BSSIdx is 0
+								WCIDAttri = (pAd->SharedKey[BSS0][KeyIdx].CipherAlg<<1)|SHAREDKEYTABLE;
+								offset = MAC_WCID_ATTRIBUTE_BASE + (BSS0Mcast_WCID* HW_WCID_ATTRI_SIZE);
+							        DBGPRINT(RT_DEBUG_TRACE, ("BSS0Mcast_WCID : offset = %x, WCIDAttri = %x\n", offset, WCIDAttri));
+								RTUSBWriteMACRegister(pAd, offset, WCIDAttri);
+
+							}
+							AsicAddSharedKeyEntry(pAd, BSS0, (UCHAR)KeyIdx, CipherAlg, pWepKey->KeyMaterial, NULL, NULL);
+							DBGPRINT(RT_DEBUG_TRACE, ("CmdThread::OID_802_11_ADD_WEP (KeyIdx=%d, Len=%d-byte)\n", KeyIdx, pWepKey->KeyLength));
+						}
+#endif // CONFIG_STA_SUPPORT //
+					}
+					break;
+
+				case CMDTHREAD_802_11_COUNTER_MEASURE:
+					break;
+				default:
+					DBGPRINT(RT_DEBUG_ERROR, ("--> Control Thread !! ERROR !! Unknown(cmdqelmt->command=0x%x) !! \n", cmdqelmt->command));
+					break;
+			}
+		}
+
+		if (cmdqelmt->CmdFromNdis == TRUE)
+		{
+				if (cmdqelmt->buffer != NULL)
+					NdisFreeMemory(cmdqelmt->buffer, cmdqelmt->bufferlength, 0);
+
+			NdisFreeMemory(cmdqelmt, sizeof(CmdQElmt), 0);
+		}
+		else
+		{
+			if ((cmdqelmt->buffer != NULL) && (cmdqelmt->bufferlength != 0))
+				NdisFreeMemory(cmdqelmt->buffer, cmdqelmt->bufferlength, 0);
+            {
+				NdisFreeMemory(cmdqelmt, sizeof(CmdQElmt), 0);
+			}
+		}
+	}	/* end of while */
+}
+
diff --git a/drivers/staging/rt2870/common/spectrum.c b/drivers/staging/rt2870/common/spectrum.c
new file mode 100644
index 00000000000..abba8405184
--- /dev/null
+++ b/drivers/staging/rt2870/common/spectrum.c
@@ -0,0 +1,1876 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+	action.c
+
+    Abstract:
+    Handle association related requests either from WSTA or from local MLME
+
+    Revision History:
+    Who          When          What
+    ---------    ----------    ----------------------------------------------
+	Fonchi Wu    2008	  	   created for 802.11h
+ */
+
+#include "../rt_config.h"
+#include "action.h"
+
+VOID MeasureReqTabInit(
+	IN PRTMP_ADAPTER pAd)
+{
+	NdisAllocateSpinLock(&pAd->CommonCfg.MeasureReqTabLock);
+
+	pAd->CommonCfg.pMeasureReqTab = kmalloc(sizeof(MEASURE_REQ_TAB), GFP_ATOMIC);
+	if (pAd->CommonCfg.pMeasureReqTab)
+		NdisZeroMemory(pAd->CommonCfg.pMeasureReqTab, sizeof(MEASURE_REQ_TAB));
+	else
+		DBGPRINT(RT_DEBUG_ERROR, ("%s Fail to alloc memory for pAd->CommonCfg.pMeasureReqTab.\n", __FUNCTION__));
+
+	return;
+}
+
+VOID MeasureReqTabExit(
+	IN PRTMP_ADAPTER pAd)
+{
+	NdisFreeSpinLock(pAd->CommonCfg.MeasureReqTabLock);
+
+	if (pAd->CommonCfg.pMeasureReqTab)
+		kfree(pAd->CommonCfg.pMeasureReqTab);
+	pAd->CommonCfg.pMeasureReqTab = NULL;
+
+	return;
+}
+
+static PMEASURE_REQ_ENTRY MeasureReqLookUp(
+	IN PRTMP_ADAPTER	pAd,
+	IN UINT8			DialogToken)
+{
+	UINT HashIdx;
+	PMEASURE_REQ_TAB pTab = pAd->CommonCfg.pMeasureReqTab;
+	PMEASURE_REQ_ENTRY pEntry = NULL;
+	PMEASURE_REQ_ENTRY pPrevEntry = NULL;
+
+	if (pTab == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: pMeasureReqTab doesn't exist.\n", __FUNCTION__));
+		return NULL;
+	}
+
+	RTMP_SEM_LOCK(&pAd->CommonCfg.MeasureReqTabLock);
+
+	HashIdx = MQ_DIALOGTOKEN_HASH_INDEX(DialogToken);
+	pEntry = pTab->Hash[HashIdx];
+
+	while (pEntry)
+	{
+		if (pEntry->DialogToken == DialogToken)
+			break;
+		else
+		{
+			pPrevEntry = pEntry;
+			pEntry = pEntry->pNext;
+		}
+	}
+
+	RTMP_SEM_UNLOCK(&pAd->CommonCfg.MeasureReqTabLock);
+
+	return pEntry;
+}
+
+static PMEASURE_REQ_ENTRY MeasureReqInsert(
+	IN PRTMP_ADAPTER	pAd,
+	IN UINT8			DialogToken)
+{
+	INT i;
+	ULONG HashIdx;
+	PMEASURE_REQ_TAB pTab = pAd->CommonCfg.pMeasureReqTab;
+	PMEASURE_REQ_ENTRY pEntry = NULL, pCurrEntry;
+	ULONG Now;
+
+	if(pTab == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: pMeasureReqTab doesn't exist.\n", __FUNCTION__));
+		return NULL;
+	}
+
+	pEntry = MeasureReqLookUp(pAd, DialogToken);
+	if (pEntry == NULL)
+	{
+		RTMP_SEM_LOCK(&pAd->CommonCfg.MeasureReqTabLock);
+		for (i = 0; i < MAX_MEASURE_REQ_TAB_SIZE; i++)
+		{
+			NdisGetSystemUpTime(&Now);
+			pEntry = &pTab->Content[i];
+
+			if ((pEntry->Valid == TRUE)
+				&& RTMP_TIME_AFTER((unsigned long)Now, (unsigned long)(pEntry->lastTime + MQ_REQ_AGE_OUT)))
+			{
+				PMEASURE_REQ_ENTRY pPrevEntry = NULL;
+				ULONG HashIdx = MQ_DIALOGTOKEN_HASH_INDEX(pEntry->DialogToken);
+				PMEASURE_REQ_ENTRY pProbeEntry = pTab->Hash[HashIdx];
+
+				// update Hash list
+				do
+				{
+					if (pProbeEntry == pEntry)
+					{
+						if (pPrevEntry == NULL)
+						{
+							pTab->Hash[HashIdx] = pEntry->pNext;
+						}
+						else
+						{
+							pPrevEntry->pNext = pEntry->pNext;
+						}
+						break;
+					}
+
+					pPrevEntry = pProbeEntry;
+					pProbeEntry = pProbeEntry->pNext;
+				} while (pProbeEntry);
+
+				NdisZeroMemory(pEntry, sizeof(MEASURE_REQ_ENTRY));
+				pTab->Size--;
+
+				break;
+			}
+
+			if (pEntry->Valid == FALSE)
+				break;
+		}
+
+		if (i < MAX_MEASURE_REQ_TAB_SIZE)
+		{
+			NdisGetSystemUpTime(&Now);
+			pEntry->lastTime = Now;
+			pEntry->Valid = TRUE;
+			pEntry->DialogToken = DialogToken;
+			pTab->Size++;
+		}
+		else
+		{
+			pEntry = NULL;
+			DBGPRINT(RT_DEBUG_ERROR, ("%s: pMeasureReqTab tab full.\n", __FUNCTION__));
+		}
+
+		// add this Neighbor entry into HASH table
+		if (pEntry)
+		{
+			HashIdx = MQ_DIALOGTOKEN_HASH_INDEX(DialogToken);
+			if (pTab->Hash[HashIdx] == NULL)
+			{
+				pTab->Hash[HashIdx] = pEntry;
+			}
+			else
+			{
+				pCurrEntry = pTab->Hash[HashIdx];
+				while (pCurrEntry->pNext != NULL)
+					pCurrEntry = pCurrEntry->pNext;
+				pCurrEntry->pNext = pEntry;
+			}
+		}
+
+		RTMP_SEM_UNLOCK(&pAd->CommonCfg.MeasureReqTabLock);
+	}
+
+	return pEntry;
+}
+
+static VOID MeasureReqDelete(
+	IN PRTMP_ADAPTER	pAd,
+	IN UINT8			DialogToken)
+{
+	PMEASURE_REQ_TAB pTab = pAd->CommonCfg.pMeasureReqTab;
+	PMEASURE_REQ_ENTRY pEntry = NULL;
+
+	if(pTab == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: pMeasureReqTab doesn't exist.\n", __FUNCTION__));
+		return;
+	}
+
+	// if empty, return
+	if (pTab->Size == 0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("pMeasureReqTab empty.\n"));
+		return;
+	}
+
+	pEntry = MeasureReqLookUp(pAd, DialogToken);
+	if (pEntry != NULL)
+	{
+		PMEASURE_REQ_ENTRY pPrevEntry = NULL;
+		ULONG HashIdx = MQ_DIALOGTOKEN_HASH_INDEX(pEntry->DialogToken);
+		PMEASURE_REQ_ENTRY pProbeEntry = pTab->Hash[HashIdx];
+
+		RTMP_SEM_LOCK(&pAd->CommonCfg.MeasureReqTabLock);
+		// update Hash list
+		do
+		{
+			if (pProbeEntry == pEntry)
+			{
+				if (pPrevEntry == NULL)
+				{
+					pTab->Hash[HashIdx] = pEntry->pNext;
+				}
+				else
+				{
+					pPrevEntry->pNext = pEntry->pNext;
+				}
+				break;
+			}
+
+			pPrevEntry = pProbeEntry;
+			pProbeEntry = pProbeEntry->pNext;
+		} while (pProbeEntry);
+
+		NdisZeroMemory(pEntry, sizeof(MEASURE_REQ_ENTRY));
+		pTab->Size--;
+
+		RTMP_SEM_UNLOCK(&pAd->CommonCfg.MeasureReqTabLock);
+	}
+
+	return;
+}
+
+VOID TpcReqTabInit(
+	IN PRTMP_ADAPTER pAd)
+{
+	NdisAllocateSpinLock(&pAd->CommonCfg.TpcReqTabLock);
+
+	pAd->CommonCfg.pTpcReqTab = kmalloc(sizeof(TPC_REQ_TAB), GFP_ATOMIC);
+	if (pAd->CommonCfg.pTpcReqTab)
+		NdisZeroMemory(pAd->CommonCfg.pTpcReqTab, sizeof(TPC_REQ_TAB));
+	else
+		DBGPRINT(RT_DEBUG_ERROR, ("%s Fail to alloc memory for pAd->CommonCfg.pTpcReqTab.\n", __FUNCTION__));
+
+	return;
+}
+
+VOID TpcReqTabExit(
+	IN PRTMP_ADAPTER pAd)
+{
+	NdisFreeSpinLock(pAd->CommonCfg.TpcReqTabLock);
+
+	if (pAd->CommonCfg.pTpcReqTab)
+		kfree(pAd->CommonCfg.pTpcReqTab);
+	pAd->CommonCfg.pTpcReqTab = NULL;
+
+	return;
+}
+
+static PTPC_REQ_ENTRY TpcReqLookUp(
+	IN PRTMP_ADAPTER	pAd,
+	IN UINT8			DialogToken)
+{
+	UINT HashIdx;
+	PTPC_REQ_TAB pTab = pAd->CommonCfg.pTpcReqTab;
+	PTPC_REQ_ENTRY pEntry = NULL;
+	PTPC_REQ_ENTRY pPrevEntry = NULL;
+
+	if (pTab == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: pTpcReqTab doesn't exist.\n", __FUNCTION__));
+		return NULL;
+	}
+
+	RTMP_SEM_LOCK(&pAd->CommonCfg.TpcReqTabLock);
+
+	HashIdx = TPC_DIALOGTOKEN_HASH_INDEX(DialogToken);
+	pEntry = pTab->Hash[HashIdx];
+
+	while (pEntry)
+	{
+		if (pEntry->DialogToken == DialogToken)
+			break;
+		else
+		{
+			pPrevEntry = pEntry;
+			pEntry = pEntry->pNext;
+		}
+	}
+
+	RTMP_SEM_UNLOCK(&pAd->CommonCfg.TpcReqTabLock);
+
+	return pEntry;
+}
+
+
+static PTPC_REQ_ENTRY TpcReqInsert(
+	IN PRTMP_ADAPTER	pAd,
+	IN UINT8			DialogToken)
+{
+	INT i;
+	ULONG HashIdx;
+	PTPC_REQ_TAB pTab = pAd->CommonCfg.pTpcReqTab;
+	PTPC_REQ_ENTRY pEntry = NULL, pCurrEntry;
+	ULONG Now;
+
+	if(pTab == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: pTpcReqTab doesn't exist.\n", __FUNCTION__));
+		return NULL;
+	}
+
+	pEntry = TpcReqLookUp(pAd, DialogToken);
+	if (pEntry == NULL)
+	{
+		RTMP_SEM_LOCK(&pAd->CommonCfg.TpcReqTabLock);
+		for (i = 0; i < MAX_TPC_REQ_TAB_SIZE; i++)
+		{
+			NdisGetSystemUpTime(&Now);
+			pEntry = &pTab->Content[i];
+
+			if ((pEntry->Valid == TRUE)
+				&& RTMP_TIME_AFTER((unsigned long)Now, (unsigned long)(pEntry->lastTime + TPC_REQ_AGE_OUT)))
+			{
+				PTPC_REQ_ENTRY pPrevEntry = NULL;
+				ULONG HashIdx = TPC_DIALOGTOKEN_HASH_INDEX(pEntry->DialogToken);
+				PTPC_REQ_ENTRY pProbeEntry = pTab->Hash[HashIdx];
+
+				// update Hash list
+				do
+				{
+					if (pProbeEntry == pEntry)
+					{
+						if (pPrevEntry == NULL)
+						{
+							pTab->Hash[HashIdx] = pEntry->pNext;
+						}
+						else
+						{
+							pPrevEntry->pNext = pEntry->pNext;
+						}
+						break;
+					}
+
+					pPrevEntry = pProbeEntry;
+					pProbeEntry = pProbeEntry->pNext;
+				} while (pProbeEntry);
+
+				NdisZeroMemory(pEntry, sizeof(TPC_REQ_ENTRY));
+				pTab->Size--;
+
+				break;
+			}
+
+			if (pEntry->Valid == FALSE)
+				break;
+		}
+
+		if (i < MAX_TPC_REQ_TAB_SIZE)
+		{
+			NdisGetSystemUpTime(&Now);
+			pEntry->lastTime = Now;
+			pEntry->Valid = TRUE;
+			pEntry->DialogToken = DialogToken;
+			pTab->Size++;
+		}
+		else
+		{
+			pEntry = NULL;
+			DBGPRINT(RT_DEBUG_ERROR, ("%s: pTpcReqTab tab full.\n", __FUNCTION__));
+		}
+
+		// add this Neighbor entry into HASH table
+		if (pEntry)
+		{
+			HashIdx = TPC_DIALOGTOKEN_HASH_INDEX(DialogToken);
+			if (pTab->Hash[HashIdx] == NULL)
+			{
+				pTab->Hash[HashIdx] = pEntry;
+			}
+			else
+			{
+				pCurrEntry = pTab->Hash[HashIdx];
+				while (pCurrEntry->pNext != NULL)
+					pCurrEntry = pCurrEntry->pNext;
+				pCurrEntry->pNext = pEntry;
+			}
+		}
+
+		RTMP_SEM_UNLOCK(&pAd->CommonCfg.TpcReqTabLock);
+	}
+
+	return pEntry;
+}
+
+static VOID TpcReqDelete(
+	IN PRTMP_ADAPTER	pAd,
+	IN UINT8			DialogToken)
+{
+	PTPC_REQ_TAB pTab = pAd->CommonCfg.pTpcReqTab;
+	PTPC_REQ_ENTRY pEntry = NULL;
+
+	if(pTab == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: pTpcReqTab doesn't exist.\n", __FUNCTION__));
+		return;
+	}
+
+	// if empty, return
+	if (pTab->Size == 0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("pTpcReqTab empty.\n"));
+		return;
+	}
+
+	pEntry = TpcReqLookUp(pAd, DialogToken);
+	if (pEntry != NULL)
+	{
+		PTPC_REQ_ENTRY pPrevEntry = NULL;
+		ULONG HashIdx = TPC_DIALOGTOKEN_HASH_INDEX(pEntry->DialogToken);
+		PTPC_REQ_ENTRY pProbeEntry = pTab->Hash[HashIdx];
+
+		RTMP_SEM_LOCK(&pAd->CommonCfg.TpcReqTabLock);
+		// update Hash list
+		do
+		{
+			if (pProbeEntry == pEntry)
+			{
+				if (pPrevEntry == NULL)
+				{
+					pTab->Hash[HashIdx] = pEntry->pNext;
+				}
+				else
+				{
+					pPrevEntry->pNext = pEntry->pNext;
+				}
+				break;
+			}
+
+			pPrevEntry = pProbeEntry;
+			pProbeEntry = pProbeEntry->pNext;
+		} while (pProbeEntry);
+
+		NdisZeroMemory(pEntry, sizeof(TPC_REQ_ENTRY));
+		pTab->Size--;
+
+		RTMP_SEM_UNLOCK(&pAd->CommonCfg.TpcReqTabLock);
+	}
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Get Current TimeS tamp.
+
+	Parametrs:
+
+	Return	: Current Time Stamp.
+	==========================================================================
+ */
+static UINT64 GetCurrentTimeStamp(
+	IN PRTMP_ADAPTER pAd)
+{
+	// get current time stamp.
+	return 0;
+}
+
+/*
+	==========================================================================
+	Description:
+		Get Current Transmit Power.
+
+	Parametrs:
+
+	Return	: Current Time Stamp.
+	==========================================================================
+ */
+static UINT8 GetCurTxPwr(
+	IN PRTMP_ADAPTER pAd,
+	IN UINT8 Wcid)
+{
+	return 16; /* 16 dBm */
+}
+
+/*
+	==========================================================================
+	Description:
+		Insert Dialog Token into frame.
+
+	Parametrs:
+		1. frame buffer pointer.
+		2. frame length.
+		3. Dialog token.
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID InsertDialogToken(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen,
+	IN UINT8 DialogToken)
+{
+	ULONG TempLen;
+	MakeOutgoingFrame(pFrameBuf,	&TempLen,
+					1,				&DialogToken,
+					END_OF_ARGS);
+
+	*pFrameLen = *pFrameLen + TempLen;
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Insert TPC Request IE into frame.
+
+	Parametrs:
+		1. frame buffer pointer.
+		2. frame length.
+
+	Return	: None.
+	==========================================================================
+ */
+ static VOID InsertTpcReqIE(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen)
+{
+	ULONG TempLen;
+	ULONG Len = 0;
+	UINT8 ElementID = IE_TPC_REQUEST;
+
+	MakeOutgoingFrame(pFrameBuf,					&TempLen,
+						1,							&ElementID,
+						1,							&Len,
+						END_OF_ARGS);
+
+	*pFrameLen = *pFrameLen + TempLen;
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Insert TPC Report IE into frame.
+
+	Parametrs:
+		1. frame buffer pointer.
+		2. frame length.
+		3. Transmit Power.
+		4. Link Margin.
+
+	Return	: None.
+	==========================================================================
+ */
+ static VOID InsertTpcReportIE(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen,
+	IN UINT8 TxPwr,
+	IN UINT8 LinkMargin)
+{
+	ULONG TempLen;
+	ULONG Len = sizeof(TPC_REPORT_INFO);
+	UINT8 ElementID = IE_TPC_REPORT;
+	TPC_REPORT_INFO TpcReportIE;
+
+	TpcReportIE.TxPwr = TxPwr;
+	TpcReportIE.LinkMargin = LinkMargin;
+
+	MakeOutgoingFrame(pFrameBuf,					&TempLen,
+						1,							&ElementID,
+						1,							&Len,
+						Len,						&TpcReportIE,
+						END_OF_ARGS);
+
+	*pFrameLen = *pFrameLen + TempLen;
+
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Insert Channel Switch Announcement IE into frame.
+
+	Parametrs:
+		1. frame buffer pointer.
+		2. frame length.
+		3. channel switch announcement mode.
+		4. new selected channel.
+		5. channel switch announcement count.
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID InsertChSwAnnIE(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen,
+	IN UINT8 ChSwMode,
+	IN UINT8 NewChannel,
+	IN UINT8 ChSwCnt)
+{
+	ULONG TempLen;
+	ULONG Len = sizeof(CH_SW_ANN_INFO);
+	UINT8 ElementID = IE_CHANNEL_SWITCH_ANNOUNCEMENT;
+	CH_SW_ANN_INFO ChSwAnnIE;
+
+	ChSwAnnIE.ChSwMode = ChSwMode;
+	ChSwAnnIE.Channel = NewChannel;
+	ChSwAnnIE.ChSwCnt = ChSwCnt;
+
+	MakeOutgoingFrame(pFrameBuf,				&TempLen,
+						1,						&ElementID,
+						1,						&Len,
+						Len,					&ChSwAnnIE,
+						END_OF_ARGS);
+
+	*pFrameLen = *pFrameLen + TempLen;
+
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Insert Measure Request IE into frame.
+
+	Parametrs:
+		1. frame buffer pointer.
+		2. frame length.
+		3. Measure Token.
+		4. Measure Request Mode.
+		5. Measure Request Type.
+		6. Measure Channel.
+		7. Measure Start time.
+		8. Measure Duration.
+
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID InsertMeasureReqIE(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen,
+	IN PMEASURE_REQ_INFO pMeasureReqIE)
+{
+	ULONG TempLen;
+	UINT8 Len = sizeof(MEASURE_REQ_INFO);
+	UINT8 ElementID = IE_MEASUREMENT_REQUEST;
+
+	MakeOutgoingFrame(pFrameBuf,					&TempLen,
+						1,							&ElementID,
+						1,							&Len,
+						Len,						pMeasureReqIE,
+						END_OF_ARGS);
+
+	*pFrameLen = *pFrameLen + TempLen;
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Insert Measure Report IE into frame.
+
+	Parametrs:
+		1. frame buffer pointer.
+		2. frame length.
+		3. Measure Token.
+		4. Measure Request Mode.
+		5. Measure Request Type.
+		6. Length of Report Infomation
+		7. Pointer of Report Infomation Buffer.
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID InsertMeasureReportIE(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen,
+	IN PMEASURE_REPORT_INFO pMeasureReportIE,
+	IN UINT8 ReportLnfoLen,
+	IN PUINT8 pReportInfo)
+{
+	ULONG TempLen;
+	ULONG Len;
+	UINT8 ElementID = IE_MEASUREMENT_REPORT;
+
+	Len = sizeof(MEASURE_REPORT_INFO) + ReportLnfoLen;
+
+	MakeOutgoingFrame(pFrameBuf,					&TempLen,
+						1,							&ElementID,
+						1,							&Len,
+						Len,						pMeasureReportIE,
+						END_OF_ARGS);
+
+	*pFrameLen = *pFrameLen + TempLen;
+
+	if ((ReportLnfoLen > 0) && (pReportInfo != NULL))
+	{
+		MakeOutgoingFrame(pFrameBuf + *pFrameLen,		&TempLen,
+							ReportLnfoLen,				pReportInfo,
+							END_OF_ARGS);
+
+		*pFrameLen = *pFrameLen + TempLen;
+	}
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Prepare Measurement request action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueMeasurementReq(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UINT8 MeasureToken,
+	IN UINT8 MeasureReqMode,
+	IN UINT8 MeasureReqType,
+	IN UINT8 MeasureCh,
+	IN UINT16 MeasureDuration)
+{
+	PUCHAR pOutBuffer = NULL;
+	NDIS_STATUS NStatus;
+	ULONG FrameLen;
+	HEADER_802_11 ActHdr;
+	MEASURE_REQ_INFO MeasureReqIE;
+	UINT8 RmReqDailogToken = RandomByte(pAd);
+	UINT64 MeasureStartTime = GetCurrentTimeStamp(pAd);
+
+	// build action frame header.
+	MgtMacHeaderInit(pAd, &ActHdr, SUBTYPE_ACTION, 0, pDA,
+						pAd->CurrentAddress);
+
+	NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+	if(NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("%s() allocate memory failed \n", __FUNCTION__));
+		return;
+	}
+	NdisMoveMemory(pOutBuffer, (PCHAR)&ActHdr, sizeof(HEADER_802_11));
+	FrameLen = sizeof(HEADER_802_11);
+
+	InsertActField(pAd, (pOutBuffer + FrameLen), &FrameLen, CATEGORY_SPECTRUM, SPEC_MRQ);
+
+	// fill Dialog Token
+	InsertDialogToken(pAd, (pOutBuffer + FrameLen), &FrameLen, MeasureToken);
+
+	// prepare Measurement IE.
+	NdisZeroMemory(&MeasureReqIE, sizeof(MEASURE_REQ_INFO));
+	MeasureReqIE.Token = RmReqDailogToken;
+	MeasureReqIE.ReqMode.word = MeasureReqMode;
+	MeasureReqIE.ReqType = MeasureReqType;
+	MeasureReqIE.MeasureReq.ChNum = MeasureCh;
+	MeasureReqIE.MeasureReq.MeasureStartTime = cpu2le64(MeasureStartTime);
+	MeasureReqIE.MeasureReq.MeasureDuration = cpu2le16(MeasureDuration);
+	InsertMeasureReqIE(pAd, (pOutBuffer + FrameLen), &FrameLen, &MeasureReqIE);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Prepare Measurement report action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueMeasurementRep(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UINT8 DialogToken,
+	IN UINT8 MeasureToken,
+	IN UINT8 MeasureReqMode,
+	IN UINT8 MeasureReqType,
+	IN UINT8 ReportInfoLen,
+	IN PUINT8 pReportInfo)
+{
+	PUCHAR pOutBuffer = NULL;
+	NDIS_STATUS NStatus;
+	ULONG FrameLen;
+	HEADER_802_11 ActHdr;
+	MEASURE_REPORT_INFO MeasureRepIE;
+
+	// build action frame header.
+	MgtMacHeaderInit(pAd, &ActHdr, SUBTYPE_ACTION, 0, pDA,
+						pAd->CurrentAddress);
+
+	NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+	if(NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("%s() allocate memory failed \n", __FUNCTION__));
+		return;
+	}
+	NdisMoveMemory(pOutBuffer, (PCHAR)&ActHdr, sizeof(HEADER_802_11));
+	FrameLen = sizeof(HEADER_802_11);
+
+	InsertActField(pAd, (pOutBuffer + FrameLen), &FrameLen, CATEGORY_SPECTRUM, SPEC_MRP);
+
+	// fill Dialog Token
+	InsertDialogToken(pAd, (pOutBuffer + FrameLen), &FrameLen, DialogToken);
+
+	// prepare Measurement IE.
+	NdisZeroMemory(&MeasureRepIE, sizeof(MEASURE_REPORT_INFO));
+	MeasureRepIE.Token = MeasureToken;
+	MeasureRepIE.ReportMode.word = MeasureReqMode;
+	MeasureRepIE.ReportType = MeasureReqType;
+	InsertMeasureReportIE(pAd, (pOutBuffer + FrameLen), &FrameLen, &MeasureRepIE, ReportInfoLen, pReportInfo);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Prepare TPC Request action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueTPCReq(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UCHAR DialogToken)
+{
+	PUCHAR pOutBuffer = NULL;
+	NDIS_STATUS NStatus;
+	ULONG FrameLen;
+
+	HEADER_802_11 ActHdr;
+
+	// build action frame header.
+	MgtMacHeaderInit(pAd, &ActHdr, SUBTYPE_ACTION, 0, pDA,
+						pAd->CurrentAddress);
+
+	NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+	if(NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("%s() allocate memory failed \n", __FUNCTION__));
+		return;
+	}
+	NdisMoveMemory(pOutBuffer, (PCHAR)&ActHdr, sizeof(HEADER_802_11));
+	FrameLen = sizeof(HEADER_802_11);
+
+	InsertActField(pAd, (pOutBuffer + FrameLen), &FrameLen, CATEGORY_SPECTRUM, SPEC_TPCRQ);
+
+	// fill Dialog Token
+	InsertDialogToken(pAd, (pOutBuffer + FrameLen), &FrameLen, DialogToken);
+
+	// Insert TPC Request IE.
+	InsertTpcReqIE(pAd, (pOutBuffer + FrameLen), &FrameLen);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Prepare TPC Report action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueTPCRep(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UINT8 DialogToken,
+	IN UINT8 TxPwr,
+	IN UINT8 LinkMargin)
+{
+	PUCHAR pOutBuffer = NULL;
+	NDIS_STATUS NStatus;
+	ULONG FrameLen;
+
+	HEADER_802_11 ActHdr;
+
+	// build action frame header.
+	MgtMacHeaderInit(pAd, &ActHdr, SUBTYPE_ACTION, 0, pDA,
+						pAd->CurrentAddress);
+
+	NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+	if(NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("%s() allocate memory failed \n", __FUNCTION__));
+		return;
+	}
+	NdisMoveMemory(pOutBuffer, (PCHAR)&ActHdr, sizeof(HEADER_802_11));
+	FrameLen = sizeof(HEADER_802_11);
+
+	InsertActField(pAd, (pOutBuffer + FrameLen), &FrameLen, CATEGORY_SPECTRUM, SPEC_TPCRP);
+
+	// fill Dialog Token
+	InsertDialogToken(pAd, (pOutBuffer + FrameLen), &FrameLen, DialogToken);
+
+	// Insert TPC Request IE.
+	InsertTpcReportIE(pAd, (pOutBuffer + FrameLen), &FrameLen, TxPwr, LinkMargin);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Prepare Channel Switch Announcement action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+		2. Channel switch announcement mode.
+		2. a New selected channel.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueChSwAnn(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UINT8 ChSwMode,
+	IN UINT8 NewCh)
+{
+	PUCHAR pOutBuffer = NULL;
+	NDIS_STATUS NStatus;
+	ULONG FrameLen;
+
+	HEADER_802_11 ActHdr;
+
+	// build action frame header.
+	MgtMacHeaderInit(pAd, &ActHdr, SUBTYPE_ACTION, 0, pDA,
+						pAd->CurrentAddress);
+
+	NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+	if(NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("%s() allocate memory failed \n", __FUNCTION__));
+		return;
+	}
+	NdisMoveMemory(pOutBuffer, (PCHAR)&ActHdr, sizeof(HEADER_802_11));
+	FrameLen = sizeof(HEADER_802_11);
+
+	InsertActField(pAd, (pOutBuffer + FrameLen), &FrameLen, CATEGORY_SPECTRUM, SPEC_CHANNEL_SWITCH);
+
+	InsertChSwAnnIE(pAd, (pOutBuffer + FrameLen), &FrameLen, ChSwMode, NewCh, 0);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	return;
+}
+
+static BOOLEAN DfsRequirementCheck(
+	IN PRTMP_ADAPTER pAd,
+	IN UINT8 Channel)
+{
+	BOOLEAN Result = FALSE;
+	INT i;
+
+	do
+	{
+		// check DFS procedure is running.
+		// make sure DFS procedure won't start twice.
+		if (pAd->CommonCfg.RadarDetect.RDMode != RD_NORMAL_MODE)
+		{
+			Result = FALSE;
+			break;
+		}
+
+		// check the new channel carried from Channel Switch Announcemnet is valid.
+		for (i=0; i<pAd->ChannelListNum; i++)
+		{
+			if ((Channel == pAd->ChannelList[i].Channel)
+				&&(pAd->ChannelList[i].RemainingTimeForUse == 0))
+			{
+				// found radar signal in the channel. the channel can't use at least for 30 minutes.
+				pAd->ChannelList[i].RemainingTimeForUse = 1800;//30 min = 1800 sec
+				Result = TRUE;
+				break;
+			}
+		}
+	} while(FALSE);
+
+	return Result;
+}
+
+VOID NotifyChSwAnnToPeerAPs(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pRA,
+	IN PUCHAR pTA,
+	IN UINT8 ChSwMode,
+	IN UINT8 Channel)
+{
+#ifdef WDS_SUPPORT
+	if (!((pRA[0] & 0xff) == 0xff)) // is pRA a broadcase address.
+	{
+		INT i;
+		// info neighbor APs that Radar signal found throgh WDS link.
+		for (i = 0; i < MAX_WDS_ENTRY; i++)
+		{
+			if (ValidWdsEntry(pAd, i))
+			{
+				PUCHAR pDA = pAd->WdsTab.WdsEntry[i].PeerWdsAddr;
+
+				// DA equal to SA. have no necessary orignal AP which found Radar signal.
+				if (MAC_ADDR_EQUAL(pTA, pDA))
+					continue;
+
+				// send Channel Switch Action frame to info Neighbro APs.
+				EnqueueChSwAnn(pAd, pDA, ChSwMode, Channel);
+			}
+		}
+	}
+#endif // WDS_SUPPORT //
+}
+
+static VOID StartDFSProcedure(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR Channel,
+	IN UINT8 ChSwMode)
+{
+	// start DFS procedure
+	pAd->CommonCfg.Channel = Channel;
+#ifdef DOT11_N_SUPPORT
+	N_ChannelCheck(pAd);
+#endif // DOT11_N_SUPPORT //
+	pAd->CommonCfg.RadarDetect.RDMode = RD_SWITCHING_MODE;
+	pAd->CommonCfg.RadarDetect.CSCount = 0;
+}
+
+/*
+	==========================================================================
+	Description:
+		Channel Switch Announcement action frame sanity check.
+
+	Parametrs:
+		1. MLME message containing the received frame
+		2. message length.
+		3. Channel switch announcement infomation buffer.
+
+
+	Return	: None.
+	==========================================================================
+ */
+
+/*
+  Channel Switch Announcement IE.
+  +----+-----+-----------+------------+-----------+
+  | ID | Len |Ch Sw Mode | New Ch Num | Ch Sw Cnt |
+  +----+-----+-----------+------------+-----------+
+    1    1        1           1            1
+*/
+static BOOLEAN PeerChSwAnnSanity(
+	IN PRTMP_ADAPTER pAd,
+	IN VOID *pMsg,
+	IN ULONG MsgLen,
+	OUT PCH_SW_ANN_INFO pChSwAnnInfo)
+{
+	PFRAME_802_11 Fr = (PFRAME_802_11)pMsg;
+	PUCHAR pFramePtr = Fr->Octet;
+	BOOLEAN result = FALSE;
+	PEID_STRUCT eid_ptr;
+
+	// skip 802.11 header.
+	MsgLen -= sizeof(HEADER_802_11);
+
+	// skip category and action code.
+	pFramePtr += 2;
+	MsgLen -= 2;
+
+	if (pChSwAnnInfo == NULL)
+		return result;
+
+	eid_ptr = (PEID_STRUCT)pFramePtr;
+	while (((UCHAR*)eid_ptr + eid_ptr->Len + 1) < ((PUCHAR)pFramePtr + MsgLen))
+	{
+		switch(eid_ptr->Eid)
+		{
+			case IE_CHANNEL_SWITCH_ANNOUNCEMENT:
+				NdisMoveMemory(&pChSwAnnInfo->ChSwMode, eid_ptr->Octet, 1);
+				NdisMoveMemory(&pChSwAnnInfo->Channel, eid_ptr->Octet + 1, 1);
+				NdisMoveMemory(&pChSwAnnInfo->ChSwCnt, eid_ptr->Octet + 2, 1);
+
+				result = TRUE;
+                break;
+
+			default:
+				break;
+		}
+		eid_ptr = (PEID_STRUCT)((UCHAR*)eid_ptr + 2 + eid_ptr->Len);
+	}
+
+	return result;
+}
+
+/*
+	==========================================================================
+	Description:
+		Measurement request action frame sanity check.
+
+	Parametrs:
+		1. MLME message containing the received frame
+		2. message length.
+		3. Measurement request infomation buffer.
+
+	Return	: None.
+	==========================================================================
+ */
+static BOOLEAN PeerMeasureReqSanity(
+	IN PRTMP_ADAPTER pAd,
+	IN VOID *pMsg,
+	IN ULONG MsgLen,
+	OUT PUINT8 pDialogToken,
+	OUT PMEASURE_REQ_INFO pMeasureReqInfo)
+{
+	PFRAME_802_11 Fr = (PFRAME_802_11)pMsg;
+	PUCHAR pFramePtr = Fr->Octet;
+	BOOLEAN result = FALSE;
+	PEID_STRUCT eid_ptr;
+	PUCHAR ptr;
+	UINT64 MeasureStartTime;
+	UINT16 MeasureDuration;
+
+	// skip 802.11 header.
+	MsgLen -= sizeof(HEADER_802_11);
+
+	// skip category and action code.
+	pFramePtr += 2;
+	MsgLen -= 2;
+
+	if (pMeasureReqInfo == NULL)
+		return result;
+
+	NdisMoveMemory(pDialogToken, pFramePtr, 1);
+	pFramePtr += 1;
+	MsgLen -= 1;
+
+	eid_ptr = (PEID_STRUCT)pFramePtr;
+	while (((UCHAR*)eid_ptr + eid_ptr->Len + 1) < ((PUCHAR)pFramePtr + MsgLen))
+	{
+		switch(eid_ptr->Eid)
+		{
+			case IE_MEASUREMENT_REQUEST:
+				NdisMoveMemory(&pMeasureReqInfo->Token, eid_ptr->Octet, 1);
+				NdisMoveMemory(&pMeasureReqInfo->ReqMode.word, eid_ptr->Octet + 1, 1);
+				NdisMoveMemory(&pMeasureReqInfo->ReqType, eid_ptr->Octet + 2, 1);
+				ptr = eid_ptr->Octet + 3;
+				NdisMoveMemory(&pMeasureReqInfo->MeasureReq.ChNum, ptr, 1);
+				NdisMoveMemory(&MeasureStartTime, ptr + 1, 8);
+				pMeasureReqInfo->MeasureReq.MeasureStartTime = SWAP64(MeasureStartTime);
+				NdisMoveMemory(&MeasureDuration, ptr + 9, 2);
+				pMeasureReqInfo->MeasureReq.MeasureDuration = SWAP16(MeasureDuration);
+
+				result = TRUE;
+				break;
+
+			default:
+				break;
+		}
+		eid_ptr = (PEID_STRUCT)((UCHAR*)eid_ptr + 2 + eid_ptr->Len);
+	}
+
+	return result;
+}
+
+/*
+	==========================================================================
+	Description:
+		Measurement report action frame sanity check.
+
+	Parametrs:
+		1. MLME message containing the received frame
+		2. message length.
+		3. Measurement report infomation buffer.
+		4. basic report infomation buffer.
+
+	Return	: None.
+	==========================================================================
+ */
+
+/*
+  Measurement Report IE.
+  +----+-----+-------+-------------+--------------+----------------+
+  | ID | Len | Token | Report Mode | Measure Type | Measure Report |
+  +----+-----+-------+-------------+--------------+----------------+
+    1     1      1          1             1            variable
+
+  Basic Report.
+  +--------+------------+----------+-----+
+  | Ch Num | Start Time | Duration | Map |
+  +--------+------------+----------+-----+
+      1          8           2        1
+
+  Map Field Bit Format.
+  +-----+---------------+---------------------+-------+------------+----------+
+  | Bss | OFDM Preamble | Unidentified signal | Radar | Unmeasured | Reserved |
+  +-----+---------------+---------------------+-------+------------+----------+
+     0          1                  2              3         4          5-7
+*/
+static BOOLEAN PeerMeasureReportSanity(
+	IN PRTMP_ADAPTER pAd,
+	IN VOID *pMsg,
+	IN ULONG MsgLen,
+	OUT PUINT8 pDialogToken,
+	OUT PMEASURE_REPORT_INFO pMeasureReportInfo,
+	OUT PUINT8 pReportBuf)
+{
+	PFRAME_802_11 Fr = (PFRAME_802_11)pMsg;
+	PUCHAR pFramePtr = Fr->Octet;
+	BOOLEAN result = FALSE;
+	PEID_STRUCT eid_ptr;
+	PUCHAR ptr;
+
+	// skip 802.11 header.
+	MsgLen -= sizeof(HEADER_802_11);
+
+	// skip category and action code.
+	pFramePtr += 2;
+	MsgLen -= 2;
+
+	if (pMeasureReportInfo == NULL)
+		return result;
+
+	NdisMoveMemory(pDialogToken, pFramePtr, 1);
+	pFramePtr += 1;
+	MsgLen -= 1;
+
+	eid_ptr = (PEID_STRUCT)pFramePtr;
+	while (((UCHAR*)eid_ptr + eid_ptr->Len + 1) < ((PUCHAR)pFramePtr + MsgLen))
+	{
+		switch(eid_ptr->Eid)
+		{
+			case IE_MEASUREMENT_REPORT:
+				NdisMoveMemory(&pMeasureReportInfo->Token, eid_ptr->Octet, 1);
+				NdisMoveMemory(&pMeasureReportInfo->ReportMode, eid_ptr->Octet + 1, 1);
+				NdisMoveMemory(&pMeasureReportInfo->ReportType, eid_ptr->Octet + 2, 1);
+				if (pMeasureReportInfo->ReportType == RM_BASIC)
+				{
+					PMEASURE_BASIC_REPORT pReport = (PMEASURE_BASIC_REPORT)pReportBuf;
+					ptr = eid_ptr->Octet + 3;
+					NdisMoveMemory(&pReport->ChNum, ptr, 1);
+					NdisMoveMemory(&pReport->MeasureStartTime, ptr + 1, 8);
+					NdisMoveMemory(&pReport->MeasureDuration, ptr + 9, 2);
+					NdisMoveMemory(&pReport->Map, ptr + 11, 1);
+
+				}
+				else if (pMeasureReportInfo->ReportType == RM_CCA)
+				{
+					PMEASURE_CCA_REPORT pReport = (PMEASURE_CCA_REPORT)pReportBuf;
+					ptr = eid_ptr->Octet + 3;
+					NdisMoveMemory(&pReport->ChNum, ptr, 1);
+					NdisMoveMemory(&pReport->MeasureStartTime, ptr + 1, 8);
+					NdisMoveMemory(&pReport->MeasureDuration, ptr + 9, 2);
+					NdisMoveMemory(&pReport->CCA_Busy_Fraction, ptr + 11, 1);
+
+				}
+				else if (pMeasureReportInfo->ReportType == RM_RPI_HISTOGRAM)
+				{
+					PMEASURE_RPI_REPORT pReport = (PMEASURE_RPI_REPORT)pReportBuf;
+					ptr = eid_ptr->Octet + 3;
+					NdisMoveMemory(&pReport->ChNum, ptr, 1);
+					NdisMoveMemory(&pReport->MeasureStartTime, ptr + 1, 8);
+					NdisMoveMemory(&pReport->MeasureDuration, ptr + 9, 2);
+					NdisMoveMemory(&pReport->RPI_Density, ptr + 11, 8);
+				}
+				result = TRUE;
+                break;
+
+			default:
+				break;
+		}
+		eid_ptr = (PEID_STRUCT)((UCHAR*)eid_ptr + 2 + eid_ptr->Len);
+	}
+
+	return result;
+}
+
+/*
+	==========================================================================
+	Description:
+		TPC Request action frame sanity check.
+
+	Parametrs:
+		1. MLME message containing the received frame
+		2. message length.
+		3. Dialog Token.
+
+	Return	: None.
+	==========================================================================
+ */
+static BOOLEAN PeerTpcReqSanity(
+	IN PRTMP_ADAPTER pAd,
+	IN VOID *pMsg,
+	IN ULONG MsgLen,
+	OUT PUINT8 pDialogToken)
+{
+	PFRAME_802_11 Fr = (PFRAME_802_11)pMsg;
+	PUCHAR pFramePtr = Fr->Octet;
+	BOOLEAN result = FALSE;
+	PEID_STRUCT eid_ptr;
+
+	MsgLen -= sizeof(HEADER_802_11);
+
+	// skip category and action code.
+	pFramePtr += 2;
+	MsgLen -= 2;
+
+	if (pDialogToken == NULL)
+		return result;
+
+	NdisMoveMemory(pDialogToken, pFramePtr, 1);
+	pFramePtr += 1;
+	MsgLen -= 1;
+
+	eid_ptr = (PEID_STRUCT)pFramePtr;
+	while (((UCHAR*)eid_ptr + eid_ptr->Len + 1) < ((PUCHAR)pFramePtr + MsgLen))
+	{
+		switch(eid_ptr->Eid)
+		{
+			case IE_TPC_REQUEST:
+				result = TRUE;
+                break;
+
+			default:
+				break;
+		}
+		eid_ptr = (PEID_STRUCT)((UCHAR*)eid_ptr + 2 + eid_ptr->Len);
+	}
+
+	return result;
+}
+
+/*
+	==========================================================================
+	Description:
+		TPC Report action frame sanity check.
+
+	Parametrs:
+		1. MLME message containing the received frame
+		2. message length.
+		3. Dialog Token.
+		4. TPC Report IE.
+
+	Return	: None.
+	==========================================================================
+ */
+static BOOLEAN PeerTpcRepSanity(
+	IN PRTMP_ADAPTER pAd,
+	IN VOID *pMsg,
+	IN ULONG MsgLen,
+	OUT PUINT8 pDialogToken,
+	OUT PTPC_REPORT_INFO pTpcRepInfo)
+{
+	PFRAME_802_11 Fr = (PFRAME_802_11)pMsg;
+	PUCHAR pFramePtr = Fr->Octet;
+	BOOLEAN result = FALSE;
+	PEID_STRUCT eid_ptr;
+
+	MsgLen -= sizeof(HEADER_802_11);
+
+	// skip category and action code.
+	pFramePtr += 2;
+	MsgLen -= 2;
+
+	if (pDialogToken == NULL)
+		return result;
+
+	NdisMoveMemory(pDialogToken, pFramePtr, 1);
+	pFramePtr += 1;
+	MsgLen -= 1;
+
+	eid_ptr = (PEID_STRUCT)pFramePtr;
+	while (((UCHAR*)eid_ptr + eid_ptr->Len + 1) < ((PUCHAR)pFramePtr + MsgLen))
+	{
+		switch(eid_ptr->Eid)
+		{
+			case IE_TPC_REPORT:
+				NdisMoveMemory(&pTpcRepInfo->TxPwr, eid_ptr->Octet, 1);
+				NdisMoveMemory(&pTpcRepInfo->LinkMargin, eid_ptr->Octet + 1, 1);
+				result = TRUE;
+                break;
+
+			default:
+				break;
+		}
+		eid_ptr = (PEID_STRUCT)((UCHAR*)eid_ptr + 2 + eid_ptr->Len);
+	}
+
+	return result;
+}
+
+/*
+	==========================================================================
+	Description:
+		Channel Switch Announcement action frame handler.
+
+	Parametrs:
+		Elme - MLME message containing the received frame
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID PeerChSwAnnAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	CH_SW_ANN_INFO ChSwAnnInfo;
+	PFRAME_802_11 pFr = (PFRAME_802_11)Elem->Msg;
+#ifdef CONFIG_STA_SUPPORT
+	UCHAR index = 0, Channel = 0, NewChannel = 0;
+	ULONG Bssidx = 0;
+#endif // CONFIG_STA_SUPPORT //
+
+	NdisZeroMemory(&ChSwAnnInfo, sizeof(CH_SW_ANN_INFO));
+	if (! PeerChSwAnnSanity(pAd, Elem->Msg, Elem->MsgLen, &ChSwAnnInfo))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("Invalid Channel Switch Action Frame.\n"));
+		return;
+	}
+
+
+#ifdef CONFIG_STA_SUPPORT
+	if (pAd->OpMode == OPMODE_STA)
+	{
+		Bssidx = BssTableSearch(&pAd->ScanTab, pFr->Hdr.Addr3, pAd->CommonCfg.Channel);
+		if (Bssidx == BSS_NOT_FOUND)
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("PeerChSwAnnAction - Bssidx is not found\n"));
+			return;
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("\n****Bssidx is %d, Channel = %d\n", index, pAd->ScanTab.BssEntry[Bssidx].Channel));
+		hex_dump("SSID",pAd->ScanTab.BssEntry[Bssidx].Bssid ,6);
+
+		Channel = pAd->CommonCfg.Channel;
+		NewChannel = ChSwAnnInfo.Channel;
+
+		if ((pAd->CommonCfg.bIEEE80211H == 1) && (NewChannel != 0) && (Channel != NewChannel))
+		{
+			// Switching to channel 1 can prevent from rescanning the current channel immediately (by auto reconnection).
+			// In addition, clear the MLME queue and the scan table to discard the RX packets and previous scanning results.
+			AsicSwitchChannel(pAd, 1, FALSE);
+			AsicLockChannel(pAd, 1);
+		    LinkDown(pAd, FALSE);
+			MlmeQueueInit(&pAd->Mlme.Queue);
+			BssTableInit(&pAd->ScanTab);
+		    RTMPusecDelay(1000000);		// use delay to prevent STA do reassoc
+
+			// channel sanity check
+			for (index = 0 ; index < pAd->ChannelListNum; index++)
+			{
+				if (pAd->ChannelList[index].Channel == NewChannel)
+				{
+					pAd->ScanTab.BssEntry[Bssidx].Channel = NewChannel;
+					pAd->CommonCfg.Channel = NewChannel;
+					AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+					AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+					DBGPRINT(RT_DEBUG_TRACE, ("&&&&&&&&&&&&&&&&PeerChSwAnnAction - STA receive channel switch announcement IE (New Channel =%d)\n", NewChannel));
+					break;
+				}
+			}
+
+			if (index >= pAd->ChannelListNum)
+			{
+				DBGPRINT_ERR(("&&&&&&&&&&&&&&&&&&&&&&&&&&PeerChSwAnnAction(can not find New Channel=%d in ChannelList[%d]\n", pAd->CommonCfg.Channel, pAd->ChannelListNum));
+			}
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	return;
+}
+
+
+/*
+	==========================================================================
+	Description:
+		Measurement Request action frame handler.
+
+	Parametrs:
+		Elme - MLME message containing the received frame
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID PeerMeasureReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	PFRAME_802_11 pFr = (PFRAME_802_11)Elem->Msg;
+	UINT8 DialogToken;
+	MEASURE_REQ_INFO MeasureReqInfo;
+	MEASURE_REPORT_MODE ReportMode;
+
+	if(PeerMeasureReqSanity(pAd, Elem->Msg, Elem->MsgLen, &DialogToken, &MeasureReqInfo))
+	{
+		ReportMode.word = 0;
+		ReportMode.field.Incapable = 1;
+		EnqueueMeasurementRep(pAd, pFr->Hdr.Addr2, DialogToken, MeasureReqInfo.Token, ReportMode.word, MeasureReqInfo.ReqType, 0, NULL);
+	}
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Measurement Report action frame handler.
+
+	Parametrs:
+		Elme - MLME message containing the received frame
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID PeerMeasureReportAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	MEASURE_REPORT_INFO MeasureReportInfo;
+	PFRAME_802_11 pFr = (PFRAME_802_11)Elem->Msg;
+	UINT8 DialogToken;
+	PUINT8 pMeasureReportInfo;
+
+//	if (pAd->CommonCfg.bIEEE80211H != TRUE)
+//		return;
+
+	if ((pMeasureReportInfo = kmalloc(sizeof(MEASURE_RPI_REPORT), GFP_ATOMIC)) == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s unable to alloc memory for measure report buffer (size=%d).\n", __FUNCTION__, sizeof(MEASURE_RPI_REPORT)));
+		return;
+	}
+
+	NdisZeroMemory(&MeasureReportInfo, sizeof(MEASURE_REPORT_INFO));
+	NdisZeroMemory(pMeasureReportInfo, sizeof(MEASURE_RPI_REPORT));
+	if (PeerMeasureReportSanity(pAd, Elem->Msg, Elem->MsgLen, &DialogToken, &MeasureReportInfo, pMeasureReportInfo))
+	{
+		do {
+			PMEASURE_REQ_ENTRY pEntry = NULL;
+
+			// Not a autonomous measure report.
+			// check the dialog token field. drop it if the dialog token doesn't match.
+			if ((DialogToken != 0)
+				&& ((pEntry = MeasureReqLookUp(pAd, DialogToken)) == NULL))
+				break;
+
+			if (pEntry != NULL)
+				MeasureReqDelete(pAd, pEntry->DialogToken);
+
+			if (MeasureReportInfo.ReportType == RM_BASIC)
+			{
+				PMEASURE_BASIC_REPORT pBasicReport = (PMEASURE_BASIC_REPORT)pMeasureReportInfo;
+				if ((pBasicReport->Map.field.Radar)
+					&& (DfsRequirementCheck(pAd, pBasicReport->ChNum) == TRUE))
+				{
+					NotifyChSwAnnToPeerAPs(pAd, pFr->Hdr.Addr1, pFr->Hdr.Addr2, 1, pBasicReport->ChNum);
+					StartDFSProcedure(pAd, pBasicReport->ChNum, 1);
+				}
+			}
+		} while (FALSE);
+	}
+	else
+		DBGPRINT(RT_DEBUG_TRACE, ("Invalid Measurement Report Frame.\n"));
+
+	kfree(pMeasureReportInfo);
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		TPC Request action frame handler.
+
+	Parametrs:
+		Elme - MLME message containing the received frame
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID PeerTpcReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	PFRAME_802_11 pFr = (PFRAME_802_11)Elem->Msg;
+	PUCHAR pFramePtr = pFr->Octet;
+	UINT8 DialogToken;
+	UINT8 TxPwr = GetCurTxPwr(pAd, Elem->Wcid);
+	UINT8 LinkMargin = 0;
+	CHAR RealRssi;
+
+	// link margin: Ratio of the received signal power to the minimum desired by the station (STA). The
+	//				STA may incorporate rate information and channel conditions, including interference, into its computation
+	//				of link margin.
+
+	RealRssi = RTMPMaxRssi(pAd, ConvertToRssi(pAd, Elem->Rssi0, RSSI_0),
+								ConvertToRssi(pAd, Elem->Rssi1, RSSI_1),
+								ConvertToRssi(pAd, Elem->Rssi2, RSSI_2));
+
+	// skip Category and action code.
+	pFramePtr += 2;
+
+	// Dialog token.
+	NdisMoveMemory(&DialogToken, pFramePtr, 1);
+
+	LinkMargin = (RealRssi / MIN_RCV_PWR);
+	if (PeerTpcReqSanity(pAd, Elem->Msg, Elem->MsgLen, &DialogToken))
+		EnqueueTPCRep(pAd, pFr->Hdr.Addr2, DialogToken, TxPwr, LinkMargin);
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		TPC Report action frame handler.
+
+	Parametrs:
+		Elme - MLME message containing the received frame
+
+	Return	: None.
+	==========================================================================
+ */
+static VOID PeerTpcRepAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UINT8 DialogToken;
+	TPC_REPORT_INFO TpcRepInfo;
+	PTPC_REQ_ENTRY pEntry = NULL;
+
+	NdisZeroMemory(&TpcRepInfo, sizeof(TPC_REPORT_INFO));
+	if (PeerTpcRepSanity(pAd, Elem->Msg, Elem->MsgLen, &DialogToken, &TpcRepInfo))
+	{
+		if ((pEntry = TpcReqLookUp(pAd, DialogToken)) != NULL)
+		{
+			TpcReqDelete(pAd, pEntry->DialogToken);
+			DBGPRINT(RT_DEBUG_TRACE, ("%s: DialogToken=%x, TxPwr=%d, LinkMargin=%d\n",
+				__FUNCTION__, DialogToken, TpcRepInfo.TxPwr, TpcRepInfo.LinkMargin));
+		}
+	}
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+		Spectrun action frames Handler such as channel switch annoucement,
+		measurement report, measurement request actions frames.
+
+	Parametrs:
+		Elme - MLME message containing the received frame
+
+	Return	: None.
+	==========================================================================
+ */
+VOID PeerSpectrumAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+
+	UCHAR	Action = Elem->Msg[LENGTH_802_11+1];
+
+	if (pAd->CommonCfg.bIEEE80211H != TRUE)
+		return;
+
+	switch(Action)
+	{
+		case SPEC_MRQ:
+			// current rt2860 unable do such measure specified in Measurement Request.
+			// reject all measurement request.
+			PeerMeasureReqAction(pAd, Elem);
+			break;
+
+		case SPEC_MRP:
+			PeerMeasureReportAction(pAd, Elem);
+			break;
+
+		case SPEC_TPCRQ:
+			PeerTpcReqAction(pAd, Elem);
+			break;
+
+		case SPEC_TPCRP:
+			PeerTpcRepAction(pAd, Elem);
+			break;
+
+		case SPEC_CHANNEL_SWITCH:
+{
+#ifdef DOT11N_DRAFT3
+				SEC_CHA_OFFSET_IE	Secondary;
+				CHA_SWITCH_ANNOUNCE_IE	ChannelSwitch;
+
+				// 802.11h only has Channel Switch Announcement IE.
+				RTMPMoveMemory(&ChannelSwitch, &Elem->Msg[LENGTH_802_11+4], sizeof (CHA_SWITCH_ANNOUNCE_IE));
+
+				// 802.11n D3.03 adds secondary channel offset element in the end.
+				if (Elem->MsgLen ==  (LENGTH_802_11 + 2 + sizeof (CHA_SWITCH_ANNOUNCE_IE) + sizeof (SEC_CHA_OFFSET_IE)))
+				{
+					RTMPMoveMemory(&Secondary, &Elem->Msg[LENGTH_802_11+9], sizeof (SEC_CHA_OFFSET_IE));
+				}
+				else
+				{
+					Secondary.SecondaryChannelOffset = 0;
+				}
+
+				if ((Elem->Msg[LENGTH_802_11+2] == IE_CHANNEL_SWITCH_ANNOUNCEMENT) && (Elem->Msg[LENGTH_802_11+3] == 3))
+				{
+					ChannelSwitchAction(pAd, Elem->Wcid, ChannelSwitch.NewChannel, Secondary.SecondaryChannelOffset);
+				}
+#endif // DOT11N_DRAFT3 //
+}
+			PeerChSwAnnAction(pAd, Elem);
+			break;
+	}
+
+	return;
+}
+
+/*
+	==========================================================================
+	Description:
+
+	Parametrs:
+
+	Return	: None.
+	==========================================================================
+ */
+INT Set_MeasureReq_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UINT Aid = 1;
+	UINT ArgIdx;
+	PUCHAR thisChar;
+
+	MEASURE_REQ_MODE MeasureReqMode;
+	UINT8 MeasureReqToken = RandomByte(pAd);
+	UINT8 MeasureReqType = RM_BASIC;
+	UINT8 MeasureCh = 1;
+
+	ArgIdx = 1;
+	while ((thisChar = strsep((char **)&arg, "-")) != NULL)
+	{
+		switch(ArgIdx)
+		{
+			case 1:	// Aid.
+				Aid = simple_strtol(thisChar, 0, 16);
+				break;
+
+			case 2: // Measurement Request Type.
+				MeasureReqType = simple_strtol(thisChar, 0, 16);
+				if (MeasureReqType > 3)
+				{
+					DBGPRINT(RT_DEBUG_ERROR, ("%s: unknow MeasureReqType(%d)\n", __FUNCTION__, MeasureReqType));
+					return TRUE;
+				}
+				break;
+
+			case 3: // Measurement channel.
+				MeasureCh = simple_strtol(thisChar, 0, 16);
+				break;
+		}
+		ArgIdx++;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s::Aid = %d, MeasureReqType=%d MeasureCh=%d\n", __FUNCTION__, Aid, MeasureReqType, MeasureCh));
+	if (!VALID_WCID(Aid))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: unknow sta of Aid(%d)\n", __FUNCTION__, Aid));
+		return TRUE;
+	}
+
+	MeasureReqMode.word = 0;
+	MeasureReqMode.field.Enable = 1;
+
+	MeasureReqInsert(pAd, MeasureReqToken);
+
+	EnqueueMeasurementReq(pAd, pAd->MacTab.Content[Aid].Addr,
+		MeasureReqToken, MeasureReqMode.word, MeasureReqType, MeasureCh, 2000);
+
+	return TRUE;
+}
+
+INT Set_TpcReq_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UINT Aid;
+
+	UINT8 TpcReqToken = RandomByte(pAd);
+
+	Aid = simple_strtol(arg, 0, 16);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("%s::Aid = %d\n", __FUNCTION__, Aid));
+	if (!VALID_WCID(Aid))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: unknow sta of Aid(%d)\n", __FUNCTION__, Aid));
+		return TRUE;
+	}
+
+	TpcReqInsert(pAd, TpcReqToken);
+
+	EnqueueTPCReq(pAd, pAd->MacTab.Content[Aid].Addr, TpcReqToken);
+
+	return TRUE;
+}
+
diff --git a/drivers/staging/rt2870/dfs.h b/drivers/staging/rt2870/dfs.h
new file mode 100644
index 00000000000..752a6352d9d
--- /dev/null
+++ b/drivers/staging/rt2870/dfs.h
@@ -0,0 +1,100 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    dfs.h
+
+    Abstract:
+    Support DFS function.
+
+    Revision History:
+    Who       When            What
+    --------  ----------      ----------------------------------------------
+    Fonchi    03-12-2007      created
+*/
+
+#define RADAR_PULSE 1
+#define RADAR_WIDTH 2
+
+#define WIDTH_RD_IDLE 0
+#define WIDTH_RD_CHECK 1
+
+
+VOID BbpRadarDetectionStart(
+	IN PRTMP_ADAPTER pAd);
+
+VOID BbpRadarDetectionStop(
+	IN PRTMP_ADAPTER pAd);
+
+VOID RadarDetectionStart(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN CTS_Protect,
+	IN UINT8 CTSPeriod);
+
+VOID RadarDetectionStop(
+	IN PRTMP_ADAPTER	pAd);
+
+VOID RadarDetectPeriodic(
+	IN PRTMP_ADAPTER	pAd);
+
+
+BOOLEAN RadarChannelCheck(
+	IN PRTMP_ADAPTER	pAd,
+	IN UCHAR			Ch);
+
+ULONG JapRadarType(
+	IN PRTMP_ADAPTER pAd);
+
+ULONG RTMPBbpReadRadarDuration(
+	IN PRTMP_ADAPTER	pAd);
+
+ULONG RTMPReadRadarDuration(
+	IN PRTMP_ADAPTER	pAd);
+
+VOID RTMPCleanRadarDuration(
+	IN PRTMP_ADAPTER	pAd);
+
+VOID RTMPPrepareRDCTSFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pDA,
+	IN	ULONG			Duration,
+	IN  UCHAR           RTSRate,
+	IN  ULONG           CTSBaseAddr,
+	IN  UCHAR			FrameGap);
+
+VOID RTMPPrepareRadarDetectParams(
+	IN PRTMP_ADAPTER	pAd);
+
+
+INT Set_ChMovingTime_Proc(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR arg);
+
+INT Set_LongPulseRadarTh_Proc(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR arg);
+
+
diff --git a/drivers/staging/rt2870/leap.h b/drivers/staging/rt2870/leap.h
new file mode 100644
index 00000000000..6818c1ff4d7
--- /dev/null
+++ b/drivers/staging/rt2870/leap.h
@@ -0,0 +1,215 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	leap.h
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+*/
+#ifndef __LEAP_H__
+#define __LEAP_H__
+
+// Messages for Associate state machine
+#define LEAP_MACHINE_BASE                   30
+
+#define LEAP_MSG_REQUEST_IDENTITY           31
+#define LEAP_MSG_REQUEST_LEAP               32
+#define LEAP_MSG_SUCCESS                    33
+#define LEAP_MSG_FAILED                     34
+#define LEAP_MSG_RESPONSE_LEAP              35
+#define LEAP_MSG_EAPOLKEY                   36
+#define LEAP_MSG_UNKNOWN                    37
+#define LEAP_MSG                            38
+//! assoc state-machine states
+#define LEAP_IDLE                           0
+#define LEAP_WAIT_IDENTITY_REQUEST          1
+#define LEAP_WAIT_CHANLLENGE_REQUEST        2
+#define LEAP_WAIT_SUCCESS                   3
+#define LEAP_WAIT_CHANLLENGE_RESPONSE       4
+#define LEAP_WAIT_EAPOLKEY                  5
+
+#define LEAP_REASON_INVALID_AUTH                    0x01
+#define LEAP_REASON_AUTH_TIMEOUT                    0x02
+#define LEAP_REASON_CHALLENGE_FROM_AP_FAILED        0x03
+#define LEAP_REASON_CHALLENGE_TO_AP_FAILED          0x04
+
+#define CISCO_AuthModeLEAP                          0x80
+#define CISCO_AuthModeLEAPNone                      0x00
+#define LEAP_AUTH_TIMEOUT                           30000
+#define LEAP_CHALLENGE_RESPONSE_LENGTH              24
+#define LEAP_CHALLENGE_REQUEST_LENGTH               8
+
+typedef struct _LEAP_EAPOL_HEADER_ {
+    UCHAR       Version;
+    UCHAR       Type;
+    UCHAR       Length[2];
+} LEAP_EAPOL_HEADER, *PLEAP_EAPOL_HEADER;
+
+typedef struct _LEAP_EAPOL_PACKET_ {
+    UCHAR       Code;
+    UCHAR       Identifier;
+    UCHAR       Length[2];
+    UCHAR       Type;
+} LEAP_EAPOL_PACKET, *PLEAP_EAPOL_PACKET;
+
+typedef struct _LEAP_EAP_CONTENTS_ {
+    UCHAR       Version;
+    UCHAR       Reserved;
+    UCHAR       Length;
+} LEAP_EAP_CONTENTS, *PLEAP_EAP_CONTENTS;
+
+/*** EAPOL key ***/
+typedef struct _EAPOL_KEY_HEADER_ {
+    UCHAR       Type;
+    UCHAR       Length[2];
+    UCHAR       Counter[8];
+    UCHAR       IV[16];
+    UCHAR       Index;
+    UCHAR       Signature[16];
+} EAPOL_KEY_HEADER, *PEAPOL_KEY_HEADER;
+
+BOOLEAN LeapMsgTypeSubst(
+    IN  UCHAR   EAPType,
+    OUT ULONG   *MsgType);
+
+VOID LeapMachinePerformAction(
+    IN PRTMP_ADAPTER    pAd,
+    IN STATE_MACHINE    *S,
+    IN MLME_QUEUE_ELEM  *Elem);
+
+VOID LeapMacHeaderInit(
+    IN  PRTMP_ADAPTER       pAd,
+    IN  OUT PHEADER_802_11  pHdr80211,
+    IN  UCHAR               wep,
+    IN  PUCHAR              pAddr3);
+
+VOID LeapStartAction(
+    IN PRTMP_ADAPTER    pAd,
+    IN MLME_QUEUE_ELEM  *Elem);
+
+VOID LeapIdentityAction(
+    IN PRTMP_ADAPTER    pAd,
+    IN MLME_QUEUE_ELEM  *Elem);
+
+VOID LeapPeerChallengeAction(
+    IN PRTMP_ADAPTER    pAd,
+    IN MLME_QUEUE_ELEM  *Elem);
+
+VOID HashPwd(
+    IN  PUCHAR  pwd,
+    IN  INT     pwdlen,
+    OUT PUCHAR  hash);
+
+VOID PeerChallengeResponse(
+    IN  PUCHAR  szChallenge,
+    IN  PUCHAR  smbPasswd,
+    OUT PUCHAR  szResponse);
+
+VOID ParityKey(
+    OUT PUCHAR  szOut,
+    IN  PUCHAR  szIn);
+
+VOID DesKey(
+    OUT ULONG   k[16][2],
+    IN  PUCHAR  key,
+    IN  INT     decrypt);
+
+VOID Des(
+    IN  ULONG   ks[16][2],
+    OUT UCHAR   block[8]);
+
+VOID DesEncrypt(
+    IN  PUCHAR  szClear,
+    IN  PUCHAR  szKey,
+    OUT PUCHAR  szOut);
+
+VOID LeapNetworkChallengeAction(
+    IN PRTMP_ADAPTER    pAd,
+    IN MLME_QUEUE_ELEM  *Elem);
+
+VOID LeapNetworkChallengeResponse(
+    IN PRTMP_ADAPTER    pAd,
+    IN MLME_QUEUE_ELEM  *Elem);
+
+VOID HashpwdHash(
+    IN  PUCHAR  hash,
+    IN  PUCHAR  hashhash);
+
+VOID ProcessSessionKey(
+    OUT PUCHAR  SessionKey,
+    IN  PUCHAR  hash2,
+    IN  PUCHAR  ChallengeToRadius,
+    IN  PUCHAR  ChallengeResponseFromRadius,
+    IN  PUCHAR  ChallengeFromRadius,
+    IN  PUCHAR  ChallengeResponseToRadius);
+
+VOID LeapEapolKeyAction(
+    IN PRTMP_ADAPTER    pAd,
+    IN MLME_QUEUE_ELEM  *Elem);
+
+VOID RogueApTableInit(
+    IN ROGUEAP_TABLE    *Tab);
+
+ULONG RogueApTableSearch(
+    IN ROGUEAP_TABLE    *Tab,
+    IN PUCHAR           pAddr);
+
+VOID RogueApEntrySet(
+    IN  PRTMP_ADAPTER   pAd,
+    OUT ROGUEAP_ENTRY   *pRogueAp,
+    IN PUCHAR           pAddr,
+    IN UCHAR            FaileCode);
+
+ULONG RogueApTableSetEntry(
+    IN  PRTMP_ADAPTER   pAd,
+    OUT ROGUEAP_TABLE  *Tab,
+    IN PUCHAR           pAddr,
+    IN UCHAR            FaileCode);
+
+VOID RogueApTableDeleteEntry(
+    IN OUT ROGUEAP_TABLE *Tab,
+    IN PUCHAR          pAddr);
+
+VOID LeapAuthTimeout(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3);
+
+VOID LeapSendRogueAPReport(
+    IN  PRTMP_ADAPTER   pAd);
+
+BOOLEAN CCKMAssocRspSanity(
+    IN PRTMP_ADAPTER    pAd,
+    IN VOID             *Msg,
+    IN ULONG            MsgLen);
+
+#endif  // __LEAP_H__
diff --git a/drivers/staging/rt2870/link_list.h b/drivers/staging/rt2870/link_list.h
new file mode 100644
index 00000000000..f6521133fd5
--- /dev/null
+++ b/drivers/staging/rt2870/link_list.h
@@ -0,0 +1,134 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#ifndef __LINK_LIST_H__
+#define __LINK_LIST_H__
+
+typedef struct _LIST_ENTRY
+{
+	struct _LIST_ENTRY *pNext;
+} LIST_ENTRY, *PLIST_ENTRY;
+
+typedef struct _LIST_HEADR
+{
+	PLIST_ENTRY pHead;
+	PLIST_ENTRY pTail;
+	UCHAR size;
+} LIST_HEADER, *PLIST_HEADER;
+
+static inline VOID initList(
+	IN PLIST_HEADER pList)
+{
+	pList->pHead = pList->pTail = NULL;
+	pList->size = 0;
+	return;
+}
+
+static inline VOID insertTailList(
+	IN PLIST_HEADER pList,
+	IN PLIST_ENTRY pEntry)
+{
+	pEntry->pNext = NULL;
+	if (pList->pTail)
+		pList->pTail->pNext = pEntry;
+	else
+		pList->pHead = pEntry;
+	pList->pTail = pEntry;
+	pList->size++;
+
+	return;
+}
+
+static inline PLIST_ENTRY removeHeadList(
+	IN PLIST_HEADER pList)
+{
+	PLIST_ENTRY pNext;
+	PLIST_ENTRY pEntry;
+
+	pEntry = pList->pHead;
+	if (pList->pHead != NULL)
+	{
+		pNext = pList->pHead->pNext;
+		pList->pHead = pNext;
+		if (pNext == NULL)
+			pList->pTail = NULL;
+		pList->size--;
+	}
+	return pEntry;
+}
+
+static inline int getListSize(
+	IN PLIST_HEADER pList)
+{
+	return pList->size;
+}
+
+static inline PLIST_ENTRY delEntryList(
+	IN PLIST_HEADER pList,
+	IN PLIST_ENTRY pEntry)
+{
+	PLIST_ENTRY pCurEntry;
+	PLIST_ENTRY pPrvEntry;
+
+	if(pList->pHead == NULL)
+		return NULL;
+
+	if(pEntry == pList->pHead)
+	{
+		pCurEntry = pList->pHead;
+		pList->pHead = pCurEntry->pNext;
+
+		if(pList->pHead == NULL)
+			pList->pTail = NULL;
+
+		pList->size--;
+		return pCurEntry;
+	}
+
+	pPrvEntry = pList->pHead;
+	pCurEntry = pPrvEntry->pNext;
+	while(pCurEntry != NULL)
+	{
+		if (pEntry == pCurEntry)
+		{
+			pPrvEntry->pNext = pCurEntry->pNext;
+
+			if(pEntry == pList->pTail)
+				pList->pTail = pPrvEntry;
+
+			pList->size--;
+			break;
+		}
+		pPrvEntry = pCurEntry;
+		pCurEntry = pPrvEntry->pNext;
+	}
+
+	return pCurEntry;
+}
+
+#endif // ___LINK_LIST_H__ //
+
diff --git a/drivers/staging/rt2870/md4.h b/drivers/staging/rt2870/md4.h
new file mode 100644
index 00000000000..f1e5b526350
--- /dev/null
+++ b/drivers/staging/rt2870/md4.h
@@ -0,0 +1,42 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#ifndef __MD4_H__
+#define __MD4_H__
+
+/* MD4 context. */
+typedef	struct	_MD4_CTX_	{
+	ULONG	state[4];        /* state (ABCD) */
+	ULONG	count[2];        /* number of bits, modulo 2^64 (lsb first) */
+	UCHAR	buffer[64];      /* input buffer */
+}	MD4_CTX;
+
+VOID MD4Init (MD4_CTX *);
+VOID MD4Update (MD4_CTX *, PUCHAR, UINT);
+VOID MD4Final (UCHAR [16], MD4_CTX *);
+
+#endif //__MD4_H__
\ No newline at end of file
diff --git a/drivers/staging/rt2870/md5.h b/drivers/staging/rt2870/md5.h
new file mode 100644
index 00000000000..d85db12170d
--- /dev/null
+++ b/drivers/staging/rt2870/md5.h
@@ -0,0 +1,107 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	md5.h
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+	jan			10-28-03		Initial
+	Rita    	11-23-04		Modify MD5 and SHA-1
+*/
+
+#ifndef	uint8
+#define	uint8  unsigned	char
+#endif
+
+#ifndef	uint32
+#define	uint32 unsigned	long int
+#endif
+
+
+#ifndef	__MD5_H__
+#define	__MD5_H__
+
+#define MD5_MAC_LEN 16
+
+typedef struct _MD5_CTX {
+    UINT32   Buf[4];             // buffers of four states
+	UCHAR   Input[64];          // input message
+	UINT32   LenInBitCount[2];   // length counter for input message, 0 up to 64 bits
+}   MD5_CTX;
+
+VOID MD5Init(MD5_CTX *pCtx);
+VOID MD5Update(MD5_CTX *pCtx, UCHAR *pData, UINT32 LenInBytes);
+VOID MD5Final(UCHAR Digest[16], MD5_CTX *pCtx);
+VOID MD5Transform(UINT32 Buf[4], UINT32 Mes[16]);
+
+void md5_mac(u8 *key, size_t key_len, u8 *data, size_t data_len, u8 *mac);
+void hmac_md5(u8 *key, size_t key_len, u8 *data, size_t data_len, u8 *mac);
+
+//
+// SHA context
+//
+typedef	struct _SHA_CTX
+{
+	UINT32   Buf[5];             // buffers of five states
+	UCHAR   Input[80];          // input message
+	UINT32   LenInBitCount[2];   // length counter for input message, 0 up to 64 bits
+
+}	SHA_CTX;
+
+VOID SHAInit(SHA_CTX *pCtx);
+UCHAR SHAUpdate(SHA_CTX *pCtx, UCHAR *pData, UINT32 LenInBytes);
+VOID SHAFinal(SHA_CTX *pCtx, UCHAR Digest[20]);
+VOID SHATransform(UINT32 Buf[5], UINT32 Mes[20]);
+
+#define SHA_DIGEST_LEN 20
+#endif // __MD5_H__
+
+/******************************************************************************/
+#ifndef	_AES_H
+#define	_AES_H
+
+typedef	struct
+{
+	uint32 erk[64];		/* encryption round	keys */
+	uint32 drk[64];		/* decryption round	keys */
+	int	nr;				/* number of rounds	*/
+}
+aes_context;
+
+int	 rtmp_aes_set_key( aes_context *ctx,	uint8 *key,	int	nbits );
+void rtmp_aes_encrypt( aes_context *ctx,	uint8 input[16], uint8 output[16] );
+void rtmp_aes_decrypt( aes_context *ctx,	uint8 input[16], uint8 output[16] );
+
+void F(char *password, unsigned char *ssid, int ssidlength, int iterations, int count, unsigned char *output);
+int PasswordHash(char *password, unsigned char *ssid, int ssidlength, unsigned char *output);
+
+#endif /* aes.h	*/
+
diff --git a/drivers/staging/rt2870/mlme.h b/drivers/staging/rt2870/mlme.h
new file mode 100644
index 00000000000..52fb8e1c290
--- /dev/null
+++ b/drivers/staging/rt2870/mlme.h
@@ -0,0 +1,1471 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	mlme.h
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John Chang	2003-08-28		Created
+	John Chang  2004-09-06      modified for RT2600
+
+*/
+#ifndef __MLME_H__
+#define __MLME_H__
+
+//extern UCHAR BROADCAST_ADDR[];
+
+// maximum supported capability information -
+// ESS, IBSS, Privacy, Short Preamble, Spectrum mgmt, Short Slot
+#define SUPPORTED_CAPABILITY_INFO   0x0533
+
+#define END_OF_ARGS                 -1
+#define LFSR_MASK                   0x80000057
+#define MLME_TASK_EXEC_INTV         100/*200*/       //
+#define LEAD_TIME                   5
+#define MLME_TASK_EXEC_MULTIPLE       10  /*5*/       // MLME_TASK_EXEC_MULTIPLE * MLME_TASK_EXEC_INTV = 1 sec
+#define REORDER_EXEC_INTV         	100       // 0.1 sec
+//#define TBTT_PRELOAD_TIME         384        // usec. LomgPreamble + 24-byte at 1Mbps
+
+// The definition of Radar detection duration region
+#define CE		0
+#define FCC		1
+#define JAP		2
+#define JAP_W53	3
+#define JAP_W56	4
+#define MAX_RD_REGION 5
+
+#ifdef	NDIS51_MINIPORT
+#define BEACON_LOST_TIME            4000       // 2048 msec = 2 sec
+#else
+#define BEACON_LOST_TIME            4 * OS_HZ    // 2048 msec = 2 sec
+#endif
+
+#define DLS_TIMEOUT                 1200      // unit: msec
+#define AUTH_TIMEOUT                300       // unit: msec
+#define ASSOC_TIMEOUT               300       // unit: msec
+#define JOIN_TIMEOUT                2 * OS_HZ      // unit: msec
+#define SHORT_CHANNEL_TIME          90        // unit: msec
+#define MIN_CHANNEL_TIME            110        // unit: msec, for dual band scan
+#define MAX_CHANNEL_TIME            140       // unit: msec, for single band scan
+#define	FAST_ACTIVE_SCAN_TIME	    30 		  // Active scan waiting for probe response time
+#define CW_MIN_IN_BITS              4         // actual CwMin = 2^CW_MIN_IN_BITS - 1
+
+
+#ifdef CONFIG_STA_SUPPORT
+#ifndef CONFIG_AP_SUPPORT
+#define CW_MAX_IN_BITS              10        // actual CwMax = 2^CW_MAX_IN_BITS - 1
+#endif
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+extern UINT32 CW_MAX_IN_BITS;
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+
+// Note: RSSI_TO_DBM_OFFSET has been changed to variable for new RF (2004-0720).
+// SHould not refer to this constant anymore
+//#define RSSI_TO_DBM_OFFSET          120 // for RT2530 RSSI-115 = dBm
+#define RSSI_FOR_MID_TX_POWER       -55  // -55 db is considered mid-distance
+#define RSSI_FOR_LOW_TX_POWER       -45  // -45 db is considered very short distance and
+                                        // eligible to use a lower TX power
+#define RSSI_FOR_LOWEST_TX_POWER    -30
+//#define MID_TX_POWER_DELTA          0   // 0 db from full TX power upon mid-distance to AP
+#define LOW_TX_POWER_DELTA          6    // -3 db from full TX power upon very short distance. 1 grade is 0.5 db
+#define LOWEST_TX_POWER_DELTA       16   // -8 db from full TX power upon shortest distance. 1 grade is 0.5 db
+
+#define RSSI_TRIGGERED_UPON_BELOW_THRESHOLD     0
+#define RSSI_TRIGGERED_UPON_EXCCEED_THRESHOLD   1
+#define RSSI_THRESHOLD_FOR_ROAMING              25
+#define RSSI_DELTA                              5
+
+// Channel Quality Indication
+#define CQI_IS_GOOD(cqi)            ((cqi) >= 50)
+//#define CQI_IS_FAIR(cqi)          (((cqi) >= 20) && ((cqi) < 50))
+#define CQI_IS_POOR(cqi)            (cqi < 50)  //(((cqi) >= 5) && ((cqi) < 20))
+#define CQI_IS_BAD(cqi)             (cqi < 5)
+#define CQI_IS_DEAD(cqi)            (cqi == 0)
+
+// weighting factor to calculate Channel quality, total should be 100%
+#define RSSI_WEIGHTING                   50
+#define TX_WEIGHTING                     30
+#define RX_WEIGHTING                     20
+
+//#define PEER_KEY_NOT_USED                0
+//#define PEER_KEY_64_BIT                  64
+//#define PEER_KEY_128_BIT                 128
+
+//#define PEER_KEY_64BIT_LEN               8
+//#define PEER_KEY_128BIT_LEN              16
+
+#define BSS_NOT_FOUND                    0xFFFFFFFF
+
+
+#ifdef CONFIG_STA_SUPPORT
+#define MAX_LEN_OF_MLME_QUEUE            40 //10
+#endif // CONFIG_STA_SUPPORT //
+
+#define SCAN_PASSIVE                     18		// scan with no probe request, only wait beacon and probe response
+#define SCAN_ACTIVE                      19		// scan with probe request, and wait beacon and probe response
+#define	SCAN_CISCO_PASSIVE				 20		// Single channel passive scan
+#define	SCAN_CISCO_ACTIVE				 21		// Single channel active scan
+#define	SCAN_CISCO_NOISE				 22		// Single channel passive scan for noise histogram collection
+#define	SCAN_CISCO_CHANNEL_LOAD			 23		// Single channel passive scan for channel load collection
+#define FAST_SCAN_ACTIVE                 24		// scan with probe request, and wait beacon and probe response
+
+#ifdef DOT11N_DRAFT3
+#define SCAN_2040_BSS_COEXIST                  26
+#endif // DOT11N_DRAFT3 //
+
+//#define BSS_TABLE_EMPTY(x)             ((x).BssNr == 0)
+#define MAC_ADDR_IS_GROUP(Addr)       (((Addr[0]) & 0x01))
+#define MAC_ADDR_HASH(Addr)            (Addr[0] ^ Addr[1] ^ Addr[2] ^ Addr[3] ^ Addr[4] ^ Addr[5])
+#define MAC_ADDR_HASH_INDEX(Addr)      (MAC_ADDR_HASH(Addr) % HASH_TABLE_SIZE)
+#define TID_MAC_HASH(Addr,TID)            (TID^Addr[0] ^ Addr[1] ^ Addr[2] ^ Addr[3] ^ Addr[4] ^ Addr[5])
+#define TID_MAC_HASH_INDEX(Addr,TID)      (TID_MAC_HASH(Addr,TID) % HASH_TABLE_SIZE)
+
+// LED Control
+// assoiation ON. one LED ON. another blinking when TX, OFF when idle
+// no association, both LED off
+#define ASIC_LED_ACT_ON(pAd)        RTMP_IO_WRITE32(pAd, MAC_CSR14, 0x00031e46)
+#define ASIC_LED_ACT_OFF(pAd)       RTMP_IO_WRITE32(pAd, MAC_CSR14, 0x00001e46)
+
+// bit definition of the 2-byte pBEACON->Capability field
+#define CAP_IS_ESS_ON(x)                 (((x) & 0x0001) != 0)
+#define CAP_IS_IBSS_ON(x)                (((x) & 0x0002) != 0)
+#define CAP_IS_CF_POLLABLE_ON(x)         (((x) & 0x0004) != 0)
+#define CAP_IS_CF_POLL_REQ_ON(x)         (((x) & 0x0008) != 0)
+#define CAP_IS_PRIVACY_ON(x)             (((x) & 0x0010) != 0)
+#define CAP_IS_SHORT_PREAMBLE_ON(x)      (((x) & 0x0020) != 0)
+#define CAP_IS_PBCC_ON(x)                (((x) & 0x0040) != 0)
+#define CAP_IS_AGILITY_ON(x)             (((x) & 0x0080) != 0)
+#define CAP_IS_SPECTRUM_MGMT(x)          (((x) & 0x0100) != 0)  // 802.11e d9
+#define CAP_IS_QOS(x)                    (((x) & 0x0200) != 0)  // 802.11e d9
+#define CAP_IS_SHORT_SLOT(x)             (((x) & 0x0400) != 0)
+#define CAP_IS_APSD(x)                   (((x) & 0x0800) != 0)  // 802.11e d9
+#define CAP_IS_IMMED_BA(x)               (((x) & 0x1000) != 0)  // 802.11e d9
+#define CAP_IS_DSSS_OFDM(x)              (((x) & 0x2000) != 0)
+#define CAP_IS_DELAY_BA(x)               (((x) & 0x4000) != 0)  // 802.11e d9
+
+#define CAP_GENERATE(ess,ibss,priv,s_pre,s_slot,spectrum)  (((ess) ? 0x0001 : 0x0000) | ((ibss) ? 0x0002 : 0x0000) | ((priv) ? 0x0010 : 0x0000) | ((s_pre) ? 0x0020 : 0x0000) | ((s_slot) ? 0x0400 : 0x0000) | ((spectrum) ? 0x0100 : 0x0000))
+
+//#define STA_QOS_CAPABILITY               0 // 1-byte. see 802.11e d9.0 for bit definition
+
+#define ERP_IS_NON_ERP_PRESENT(x)        (((x) & 0x01) != 0)    // 802.11g
+#define ERP_IS_USE_PROTECTION(x)         (((x) & 0x02) != 0)    // 802.11g
+#define ERP_IS_USE_BARKER_PREAMBLE(x)    (((x) & 0x04) != 0)    // 802.11g
+
+#define DRS_TX_QUALITY_WORST_BOUND       8// 3  // just test by gary
+#define DRS_PENALTY                      8
+
+#define BA_NOTUSE 	2
+//BA Policy subfiled value in ADDBA frame
+#define IMMED_BA 	1
+#define DELAY_BA	0
+
+// BA Initiator subfield in DELBA frame
+#define ORIGINATOR	1
+#define RECIPIENT	0
+
+// ADDBA Status Code
+#define ADDBA_RESULTCODE_SUCCESS					0
+#define ADDBA_RESULTCODE_REFUSED					37
+#define ADDBA_RESULTCODE_INVALID_PARAMETERS			38
+
+// DELBA Reason Code
+#define DELBA_REASONCODE_QSTA_LEAVING				36
+#define DELBA_REASONCODE_END_BA						37
+#define DELBA_REASONCODE_UNKNOWN_BA					38
+#define DELBA_REASONCODE_TIMEOUT					39
+
+// reset all OneSecTx counters
+#define RESET_ONE_SEC_TX_CNT(__pEntry) \
+if (((__pEntry)) != NULL) \
+{ \
+	(__pEntry)->OneSecTxRetryOkCount = 0; \
+	(__pEntry)->OneSecTxFailCount = 0; \
+	(__pEntry)->OneSecTxNoRetryOkCount = 0; \
+}
+
+//
+// 802.11 frame formats
+//
+//  HT Capability INFO field in HT Cap IE .
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+	USHORT	LSIGTxopProSup:1;
+	USHORT	Forty_Mhz_Intolerant:1;
+	USHORT	PSMP:1;
+	USHORT	CCKmodein40:1;
+	USHORT	AMsduSize:1;
+	USHORT	DelayedBA:1;	//rt2860c not support
+	USHORT	RxSTBC:2;
+	USHORT	TxSTBC:1;
+	USHORT	ShortGIfor40:1;	//for40MHz
+	USHORT	ShortGIfor20:1;
+	USHORT	GF:1;	//green field
+	USHORT	MimoPs:2;//momi power safe
+	USHORT	ChannelWidth:1;
+	USHORT	AdvCoding:1;
+#else
+	USHORT	AdvCoding:1;
+	USHORT	ChannelWidth:1;
+	USHORT	MimoPs:2;//momi power safe
+	USHORT	GF:1;	//green field
+	USHORT	ShortGIfor20:1;
+	USHORT	ShortGIfor40:1;	//for40MHz
+	USHORT	TxSTBC:1;
+	USHORT	RxSTBC:2;
+	USHORT	DelayedBA:1;	//rt2860c not support
+	USHORT	AMsduSize:1;	// only support as zero
+	USHORT	CCKmodein40:1;
+	USHORT	PSMP:1;
+	USHORT	Forty_Mhz_Intolerant:1;
+	USHORT	LSIGTxopProSup:1;
+#endif	/* !RT_BIG_ENDIAN */
+} HT_CAP_INFO, *PHT_CAP_INFO;
+
+//  HT Capability INFO field in HT Cap IE .
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+	UCHAR	rsv:3;//momi power safe
+	UCHAR	MpduDensity:3;
+	UCHAR	MaxRAmpduFactor:2;
+#else
+	UCHAR	MaxRAmpduFactor:2;
+	UCHAR	MpduDensity:3;
+	UCHAR	rsv:3;//momi power safe
+#endif /* !RT_BIG_ENDIAN */
+} HT_CAP_PARM, *PHT_CAP_PARM;
+
+//  HT Capability INFO field in HT Cap IE .
+typedef struct PACKED {
+	UCHAR	MCSSet[10];
+	UCHAR	SupRate[2];  // unit : 1Mbps
+#ifdef RT_BIG_ENDIAN
+	UCHAR	rsv:3;
+	UCHAR	MpduDensity:1;
+	UCHAR	TxStream:2;
+	UCHAR	TxRxNotEqual:1;
+	UCHAR	TxMCSSetDefined:1;
+#else
+	UCHAR	TxMCSSetDefined:1;
+	UCHAR	TxRxNotEqual:1;
+	UCHAR	TxStream:2;
+	UCHAR	MpduDensity:1;
+	UCHAR	rsv:3;
+#endif // RT_BIG_ENDIAN //
+	UCHAR	rsv3[3];
+} HT_MCS_SET, *PHT_MCS_SET;
+
+//  HT Capability INFO field in HT Cap IE .
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+	USHORT	rsv2:4;
+	USHORT	RDGSupport:1;	//reverse Direction Grant  support
+	USHORT	PlusHTC:1;	//+HTC control field support
+	USHORT	MCSFeedback:2;	//0:no MCS feedback, 2:unsolicited MCS feedback, 3:Full MCS feedback,  1:rsv.
+	USHORT	rsv:5;//momi power safe
+	USHORT	TranTime:2;
+	USHORT	Pco:1;
+#else
+	USHORT	Pco:1;
+	USHORT	TranTime:2;
+	USHORT	rsv:5;//momi power safe
+	USHORT	MCSFeedback:2;	//0:no MCS feedback, 2:unsolicited MCS feedback, 3:Full MCS feedback,  1:rsv.
+	USHORT	PlusHTC:1;	//+HTC control field support
+	USHORT	RDGSupport:1;	//reverse Direction Grant  support
+	USHORT	rsv2:4;
+#endif /* RT_BIG_ENDIAN */
+} EXT_HT_CAP_INFO, *PEXT_HT_CAP_INFO;
+
+//  HT Beamforming field in HT Cap IE .
+typedef struct PACKED _HT_BF_CAP{
+#ifdef RT_BIG_ENDIAN
+	ULONG	rsv:3;
+	ULONG	ChanEstimation:2;
+	ULONG	CSIRowBFSup:2;
+	ULONG	ComSteerBFAntSup:2;
+	ULONG	NoComSteerBFAntSup:2;
+	ULONG	CSIBFAntSup:2;
+	ULONG	MinGrouping:2;
+	ULONG	ExpComBF:2;
+	ULONG	ExpNoComBF:2;
+	ULONG	ExpCSIFbk:2;
+	ULONG	ExpComSteerCapable:1;
+	ULONG	ExpNoComSteerCapable:1;
+	ULONG	ExpCSICapable:1;
+	ULONG	Calibration:2;
+	ULONG	ImpTxBFCapable:1;
+	ULONG	TxNDPCapable:1;
+	ULONG	RxNDPCapable:1;
+	ULONG	TxSoundCapable:1;
+	ULONG	RxSoundCapable:1;
+	ULONG	TxBFRecCapable:1;
+#else
+	ULONG	TxBFRecCapable:1;
+	ULONG	RxSoundCapable:1;
+	ULONG	TxSoundCapable:1;
+	ULONG	RxNDPCapable:1;
+	ULONG	TxNDPCapable:1;
+	ULONG	ImpTxBFCapable:1;
+	ULONG	Calibration:2;
+	ULONG	ExpCSICapable:1;
+	ULONG	ExpNoComSteerCapable:1;
+	ULONG	ExpComSteerCapable:1;
+	ULONG	ExpCSIFbk:2;
+	ULONG	ExpNoComBF:2;
+	ULONG	ExpComBF:2;
+	ULONG	MinGrouping:2;
+	ULONG	CSIBFAntSup:2;
+	ULONG	NoComSteerBFAntSup:2;
+	ULONG	ComSteerBFAntSup:2;
+	ULONG	CSIRowBFSup:2;
+	ULONG	ChanEstimation:2;
+	ULONG	rsv:3;
+#endif // RT_BIG_ENDIAN //
+} HT_BF_CAP, *PHT_BF_CAP;
+
+//  HT antenna selection field in HT Cap IE .
+typedef struct PACKED _HT_AS_CAP{
+#ifdef RT_BIG_ENDIAN
+	UCHAR	rsv:1;
+	UCHAR	TxSoundPPDU:1;
+	UCHAR	RxASel:1;
+	UCHAR	AntIndFbk:1;
+	UCHAR	ExpCSIFbk:1;
+	UCHAR	AntIndFbkTxASEL:1;
+	UCHAR	ExpCSIFbkTxASEL:1;
+	UCHAR	AntSelect:1;
+#else
+	UCHAR	AntSelect:1;
+	UCHAR	ExpCSIFbkTxASEL:1;
+	UCHAR	AntIndFbkTxASEL:1;
+	UCHAR	ExpCSIFbk:1;
+	UCHAR	AntIndFbk:1;
+	UCHAR	RxASel:1;
+	UCHAR	TxSoundPPDU:1;
+	UCHAR	rsv:1;
+#endif // RT_BIG_ENDIAN //
+} HT_AS_CAP, *PHT_AS_CAP;
+
+// Draft 1.0 set IE length 26, but is extensible..
+#define SIZE_HT_CAP_IE		26
+// The structure for HT Capability IE.
+typedef struct PACKED _HT_CAPABILITY_IE{
+	HT_CAP_INFO		HtCapInfo;
+	HT_CAP_PARM		HtCapParm;
+//	HT_MCS_SET		HtMCSSet;
+	UCHAR			MCSSet[16];
+	EXT_HT_CAP_INFO	ExtHtCapInfo;
+	HT_BF_CAP		TxBFCap;	// beamforming cap. rt2860c not support beamforming.
+	HT_AS_CAP		ASCap;	//antenna selection.
+} HT_CAPABILITY_IE, *PHT_CAPABILITY_IE;
+
+
+// 802.11n draft3 related structure definitions.
+// 7.3.2.60
+#define dot11OBSSScanPassiveDwell							20	// in TU. min amount of time that the STA continously scans each channel when performing an active OBSS scan.
+#define dot11OBSSScanActiveDwell							10	// in TU.min amount of time that the STA continously scans each channel when performing an passive OBSS scan.
+#define dot11BSSWidthTriggerScanInterval					300  // in sec. max interval between scan operations to be performed to detect BSS channel width trigger events.
+#define dot11OBSSScanPassiveTotalPerChannel					200	// in TU. min total amount of time that the STA scans each channel when performing a passive OBSS scan.
+#define dot11OBSSScanActiveTotalPerChannel					20	//in TU. min total amount of time that the STA scans each channel when performing a active OBSS scan
+#define dot11BSSWidthChannelTransactionDelayFactor			5	// min ratio between the delay time in performing a switch from 20MHz BSS to 20/40 BSS operation and the maxima
+																//	interval between overlapping BSS scan operations.
+#define dot11BSSScanActivityThreshold						25	// in %%, max total time that a STA may be active on the medium during a period of
+																//	(dot11BSSWidthChannelTransactionDelayFactor * dot11BSSWidthTriggerScanInterval) seconds without
+																//	being obligated to perform OBSS Scan operations. default is 25(== 0.25%)
+
+typedef struct PACKED _OVERLAP_BSS_SCAN_IE{
+	USHORT		ScanPassiveDwell;
+	USHORT		ScanActiveDwell;
+	USHORT		TriggerScanInt;				// Trigger scan interval
+	USHORT		PassiveTalPerChannel;		// passive total per channel
+	USHORT		ActiveTalPerChannel;		// active total per channel
+	USHORT		DelayFactor;				// BSS width channel transition delay factor
+	USHORT		ScanActThre;				// Scan Activity threshold
+}OVERLAP_BSS_SCAN_IE, *POVERLAP_BSS_SCAN_IE;
+
+
+//  7.3.2.56. 20/40 Coexistence element used in  Element ID = 72 = IE_2040_BSS_COEXIST
+typedef union PACKED _BSS_2040_COEXIST_IE{
+ struct PACKED {
+ #ifdef RT_BIG_ENDIAN
+	UCHAR	rsv:5;
+	UCHAR	BSS20WidthReq:1;
+	UCHAR	Intolerant40:1;
+	UCHAR	InfoReq:1;
+ #else
+	UCHAR	InfoReq:1;
+	UCHAR	Intolerant40:1;			// Inter-BSS. set 1 when prohibits a receiving BSS from operating as a 20/40 Mhz BSS.
+	UCHAR	BSS20WidthReq:1;		// Intra-BSS set 1 when prohibits a receiving AP from operating its BSS as a 20/40MHz BSS.
+	UCHAR	rsv:5;
+#endif // RT_BIG_ENDIAN //
+    } field;
+ UCHAR   word;
+} BSS_2040_COEXIST_IE, *PBSS_2040_COEXIST_IE;
+
+
+typedef struct  _TRIGGER_EVENTA{
+	BOOLEAN			bValid;
+	UCHAR	BSSID[6];
+	UCHAR	RegClass;	// Regulatory Class
+	USHORT	Channel;
+	ULONG	CDCounter;   // Maintain a seperate count down counter for each Event A.
+} TRIGGER_EVENTA, *PTRIGGER_EVENTA;
+
+// 20/40 trigger event table
+// If one Event A delete or created, or if Event B is detected or not detected, STA should send 2040BSSCoexistence to AP.
+#define MAX_TRIGGER_EVENT		64
+typedef struct  _TRIGGER_EVENT_TAB{
+	UCHAR	EventANo;
+	TRIGGER_EVENTA	EventA[MAX_TRIGGER_EVENT];
+	ULONG			EventBCountDown;	// Count down counter for Event B.
+} TRIGGER_EVENT_TAB, *PTRIGGER_EVENT_TAB;
+
+// 7.3.27 20/40 Bss Coexistence Mgmt capability used in extended capabilities information IE( ID = 127 = IE_EXT_CAPABILITY).
+//	This is the first octet and was defined in 802.11n D3.03 and 802.11yD9.0
+typedef struct PACKED _EXT_CAP_INFO_ELEMENT{
+#ifdef RT_BIG_ENDIAN
+	UCHAR	rsv2:5;
+	UCHAR	ExtendChannelSwitch:1;
+	UCHAR	rsv:1;
+	UCHAR	BssCoexistMgmtSupport:1;
+#else
+	UCHAR	BssCoexistMgmtSupport:1;
+	UCHAR	rsv:1;
+	UCHAR	ExtendChannelSwitch:1;
+	UCHAR	rsv2:5;
+#endif // RT_BIG_ENDIAN //
+}EXT_CAP_INFO_ELEMENT, *PEXT_CAP_INFO_ELEMENT;
+
+
+// 802.11n 7.3.2.61
+typedef struct PACKED _BSS_2040_COEXIST_ELEMENT{
+	UCHAR					ElementID;		// ID = IE_2040_BSS_COEXIST = 72
+	UCHAR					Len;
+	BSS_2040_COEXIST_IE		BssCoexistIe;
+}BSS_2040_COEXIST_ELEMENT, *PBSS_2040_COEXIST_ELEMENT;
+
+
+//802.11n 7.3.2.59
+typedef struct PACKED _BSS_2040_INTOLERANT_CH_REPORT{
+	UCHAR				ElementID;		// ID = IE_2040_BSS_INTOLERANT_REPORT = 73
+	UCHAR				Len;
+	UCHAR				RegulatoryClass;
+	UCHAR				ChList[0];
+}BSS_2040_INTOLERANT_CH_REPORT, *PBSS_2040_INTOLERANT_CH_REPORT;
+
+
+// The structure for channel switch annoucement IE. This is in 802.11n D3.03
+typedef struct PACKED _CHA_SWITCH_ANNOUNCE_IE{
+	UCHAR			SwitchMode;	//channel switch mode
+	UCHAR			NewChannel;	//
+	UCHAR			SwitchCount;	//
+} CHA_SWITCH_ANNOUNCE_IE, *PCHA_SWITCH_ANNOUNCE_IE;
+
+
+// The structure for channel switch annoucement IE. This is in 802.11n D3.03
+typedef struct PACKED _SEC_CHA_OFFSET_IE{
+	UCHAR			SecondaryChannelOffset;	 // 1: Secondary above, 3: Secondary below, 0: no Secondary
+} SEC_CHA_OFFSET_IE, *PSEC_CHA_OFFSET_IE;
+
+
+// This structure is extracted from struct RT_HT_CAPABILITY
+typedef struct {
+	BOOLEAN			bHtEnable;	 // If we should use ht rate.
+	BOOLEAN			bPreNHt;	 // If we should use ht rate.
+	//Substract from HT Capability IE
+	UCHAR			MCSSet[16];	//only supoort MCS=0-15,32 ,
+} RT_HT_PHY_INFO, *PRT_HT_PHY_INFO;
+
+//This structure substracts ralink supports from all 802.11n-related features.
+//Features not listed here but contained in 802.11n spec are not supported in rt2860.
+typedef struct {
+#if 0	// move to
+	BOOLEAN			bHtEnable;	 // If we should use ht rate.
+	BOOLEAN			bPreNHt;	 // If we should use ht rate.
+	//Substract from HT Capability IE
+	UCHAR			MCSSet[16];	//only supoort MCS=0-15,32 ,
+#endif
+#ifdef RT_BIG_ENDIAN
+	USHORT	rsv:5;
+	USHORT	AmsduSize:1;	// Max receiving A-MSDU size
+	USHORT	AmsduEnable:1;	// Enable to transmit A-MSDU. Suggest disable. We should use A-MPDU to gain best benifit of 802.11n
+	USHORT	RxSTBC:2;	// 2 bits
+	USHORT	TxSTBC:1;
+	USHORT	ShortGIfor40:1;	//for40MHz
+	USHORT	ShortGIfor20:1;
+	USHORT	GF:1;	//green field
+	USHORT	MimoPs:2;//mimo power safe MMPS_
+	USHORT	ChannelWidth:1;
+#else
+	USHORT	ChannelWidth:1;
+	USHORT	MimoPs:2;//mimo power safe MMPS_
+	USHORT	GF:1;	//green field
+	USHORT	ShortGIfor20:1;
+	USHORT	ShortGIfor40:1;	//for40MHz
+	USHORT	TxSTBC:1;
+	USHORT	RxSTBC:2;	// 2 bits
+	USHORT	AmsduEnable:1;	// Enable to transmit A-MSDU. Suggest disable. We should use A-MPDU to gain best benifit of 802.11n
+	USHORT	AmsduSize:1;	// Max receiving A-MSDU size
+	USHORT	rsv:5;
+#endif
+
+	//Substract from Addiont HT INFO IE
+#ifdef RT_BIG_ENDIAN
+	UCHAR	RecomWidth:1;
+	UCHAR	ExtChanOffset:2;	// Please not the difference with following 	UCHAR	NewExtChannelOffset; from 802.11n
+	UCHAR	MpduDensity:3;
+	UCHAR	MaxRAmpduFactor:2;
+#else
+	UCHAR	MaxRAmpduFactor:2;
+	UCHAR	MpduDensity:3;
+	UCHAR	ExtChanOffset:2;	// Please not the difference with following 	UCHAR	NewExtChannelOffset; from 802.11n
+	UCHAR	RecomWidth:1;
+#endif
+
+#ifdef RT_BIG_ENDIAN
+	USHORT	rsv2:11;
+	USHORT	OBSS_NonHTExist:1;
+	USHORT	rsv3:1;
+	USHORT	NonGfPresent:1;
+	USHORT	OperaionMode:2;
+#else
+	USHORT	OperaionMode:2;
+	USHORT	NonGfPresent:1;
+	USHORT	rsv3:1;
+	USHORT	OBSS_NonHTExist:1;
+	USHORT	rsv2:11;
+#endif
+
+	// New Extension Channel Offset IE
+	UCHAR	NewExtChannelOffset;
+	// Extension Capability IE = 127
+	UCHAR	BSSCoexist2040;
+} RT_HT_CAPABILITY, *PRT_HT_CAPABILITY;
+
+//   field in Addtional HT Information IE .
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+	UCHAR	SerInterGranu:3;
+	UCHAR	S_PSMPSup:1;
+	UCHAR	RifsMode:1;
+	UCHAR	RecomWidth:1;
+	UCHAR	ExtChanOffset:2;
+#else
+	UCHAR	ExtChanOffset:2;
+	UCHAR	RecomWidth:1;
+	UCHAR	RifsMode:1;
+	UCHAR	S_PSMPSup:1;	 //Indicate support for scheduled PSMP
+	UCHAR	SerInterGranu:3;	 //service interval granularity
+#endif
+} ADD_HTINFO, *PADD_HTINFO;
+
+typedef struct PACKED{
+#ifdef RT_BIG_ENDIAN
+	USHORT	rsv2:11;
+	USHORT	OBSS_NonHTExist:1;
+	USHORT	rsv:1;
+	USHORT	NonGfPresent:1;
+	USHORT	OperaionMode:2;
+#else
+	USHORT	OperaionMode:2;
+	USHORT	NonGfPresent:1;
+	USHORT	rsv:1;
+	USHORT	OBSS_NonHTExist:1;
+	USHORT	rsv2:11;
+#endif
+} ADD_HTINFO2, *PADD_HTINFO2;
+
+
+// TODO: Need sync with spec about the definition of StbcMcs. In Draft 3.03, it's reserved.
+typedef struct PACKED{
+#ifdef RT_BIG_ENDIAN
+	USHORT	rsv:4;
+	USHORT	PcoPhase:1;
+	USHORT	PcoActive:1;
+	USHORT	LsigTxopProt:1;
+	USHORT	STBCBeacon:1;
+	USHORT	DualCTSProtect:1;
+	USHORT	DualBeacon:1;
+	USHORT	StbcMcs:6;
+#else
+	USHORT	StbcMcs:6;
+	USHORT	DualBeacon:1;
+	USHORT	DualCTSProtect:1;
+	USHORT	STBCBeacon:1;
+	USHORT	LsigTxopProt:1;	// L-SIG TXOP protection full support
+	USHORT	PcoActive:1;
+	USHORT	PcoPhase:1;
+	USHORT	rsv:4;
+#endif // RT_BIG_ENDIAN //
+} ADD_HTINFO3, *PADD_HTINFO3;
+
+#define SIZE_ADD_HT_INFO_IE		22
+typedef struct  PACKED{
+	UCHAR				ControlChan;
+	ADD_HTINFO			AddHtInfo;
+	ADD_HTINFO2			AddHtInfo2;
+	ADD_HTINFO3			AddHtInfo3;
+	UCHAR				MCSSet[16];		// Basic MCS set
+} ADD_HT_INFO_IE, *PADD_HT_INFO_IE;
+
+typedef struct  PACKED{
+	UCHAR				NewExtChanOffset;
+} NEW_EXT_CHAN_IE, *PNEW_EXT_CHAN_IE;
+
+
+// 4-byte HTC field.  maybe included in any frame except non-QOS data frame.  The Order bit must set 1.
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+    UINT32		RDG:1;	//RDG / More PPDU
+    UINT32		ACConstraint:1;	//feedback request
+    UINT32		rsv:5;  //calibration sequence
+    UINT32		ZLFAnnouce:1;	// ZLF announcement
+    UINT32		CSISTEERING:2;	//CSI/ STEERING
+    UINT32		FBKReq:2;	//feedback request
+    UINT32		CalSeq:2;  //calibration sequence
+    UINT32		CalPos:2;	// calibration position
+    UINT32		MFBorASC:7;	//Link adaptation feedback containing recommended MCS. 0x7f for no feedback or not available
+    UINT32		MFS:3;	//SET to the received value of MRS. 0x111 for unsolicited MFB.
+    UINT32		MRSorASI:3;	// MRQ Sequence identifier. unchanged during entire procedure. 0x000-0x110.
+    UINT32		MRQ:1;	//MCS feedback. Request for a MCS feedback
+    UINT32		TRQ:1;	//sounding request
+    UINT32		MA:1;	//management action payload exist in (QoS Null+HTC)
+#else
+    UINT32		MA:1;	//management action payload exist in (QoS Null+HTC)
+    UINT32		TRQ:1;	//sounding request
+    UINT32		MRQ:1;	//MCS feedback. Request for a MCS feedback
+    UINT32		MRSorASI:3;	// MRQ Sequence identifier. unchanged during entire procedure. 0x000-0x110.
+    UINT32		MFS:3;	//SET to the received value of MRS. 0x111 for unsolicited MFB.
+    UINT32		MFBorASC:7;	//Link adaptation feedback containing recommended MCS. 0x7f for no feedback or not available
+    UINT32		CalPos:2;	// calibration position
+    UINT32		CalSeq:2;  //calibration sequence
+    UINT32		FBKReq:2;	//feedback request
+    UINT32		CSISTEERING:2;	//CSI/ STEERING
+    UINT32		ZLFAnnouce:1;	// ZLF announcement
+    UINT32		rsv:5;  //calibration sequence
+    UINT32		ACConstraint:1;	//feedback request
+    UINT32		RDG:1;	//RDG / More PPDU
+#endif /* !RT_BIG_ENDIAN */
+} HT_CONTROL, *PHT_CONTROL;
+
+// 2-byte QOS CONTROL field
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+    USHORT      Txop_QueueSize:8;
+    USHORT      AMsduPresent:1;
+    USHORT      AckPolicy:2;  //0: normal ACK 1:No ACK 2:scheduled under MTBA/PSMP  3: BA
+    USHORT      EOSP:1;
+    USHORT      TID:4;
+#else
+    USHORT      TID:4;
+    USHORT      EOSP:1;
+    USHORT      AckPolicy:2;  //0: normal ACK 1:No ACK 2:scheduled under MTBA/PSMP  3: BA
+    USHORT      AMsduPresent:1;
+    USHORT      Txop_QueueSize:8;
+#endif /* !RT_BIG_ENDIAN */
+} QOS_CONTROL, *PQOS_CONTROL;
+
+// 2-byte Frame control field
+typedef	struct	PACKED {
+#ifdef RT_BIG_ENDIAN
+	USHORT		Order:1;			// Strict order expected
+	USHORT		Wep:1;				// Wep data
+	USHORT		MoreData:1;			// More data bit
+	USHORT		PwrMgmt:1;			// Power management bit
+	USHORT		Retry:1;			// Retry status bit
+	USHORT		MoreFrag:1;			// More fragment bit
+	USHORT		FrDs:1;				// From DS indication
+	USHORT		ToDs:1;				// To DS indication
+	USHORT		SubType:4;			// MSDU subtype
+	USHORT		Type:2;				// MSDU type
+	USHORT		Ver:2;				// Protocol version
+#else
+	USHORT		Ver:2;				// Protocol version
+	USHORT		Type:2;				// MSDU type
+	USHORT		SubType:4;			// MSDU subtype
+	USHORT		ToDs:1;				// To DS indication
+	USHORT		FrDs:1;				// From DS indication
+	USHORT		MoreFrag:1;			// More fragment bit
+	USHORT		Retry:1;			// Retry status bit
+	USHORT		PwrMgmt:1;			// Power management bit
+	USHORT		MoreData:1;			// More data bit
+	USHORT		Wep:1;				// Wep data
+	USHORT		Order:1;			// Strict order expected
+#endif /* !RT_BIG_ENDIAN */
+} FRAME_CONTROL, *PFRAME_CONTROL;
+
+typedef	struct	PACKED _HEADER_802_11	{
+    FRAME_CONTROL   FC;
+    USHORT          Duration;
+    UCHAR           Addr1[MAC_ADDR_LEN];
+    UCHAR           Addr2[MAC_ADDR_LEN];
+	UCHAR			Addr3[MAC_ADDR_LEN];
+#ifdef RT_BIG_ENDIAN
+	USHORT			Sequence:12;
+	USHORT			Frag:4;
+#else
+	USHORT			Frag:4;
+	USHORT			Sequence:12;
+#endif /* !RT_BIG_ENDIAN */
+	UCHAR			Octet[0];
+}	HEADER_802_11, *PHEADER_802_11;
+
+typedef struct PACKED _FRAME_802_11 {
+    HEADER_802_11   Hdr;
+    UCHAR            Octet[1];
+}   FRAME_802_11, *PFRAME_802_11;
+
+// QoSNull embedding of management action. When HT Control MA field set to 1.
+typedef struct PACKED _MA_BODY {
+    UCHAR            Category;
+    UCHAR            Action;
+    UCHAR            Octet[1];
+}   MA_BODY, *PMA_BODY;
+
+typedef	struct	PACKED _HEADER_802_3	{
+    UCHAR           DAAddr1[MAC_ADDR_LEN];
+    UCHAR           SAAddr2[MAC_ADDR_LEN];
+    UCHAR           Octet[2];
+}	HEADER_802_3, *PHEADER_802_3;
+////Block ACK related format
+// 2-byte BA Parameter  field  in 	DELBA frames to terminate an already set up bA
+typedef struct PACKED{
+#ifdef RT_BIG_ENDIAN
+    USHORT      TID:4;	// value of TC os TS
+    USHORT      Initiator:1;	// 1: originator    0:recipient
+    USHORT      Rsv:11;	// always set to 0
+#else
+    USHORT      Rsv:11;	// always set to 0
+    USHORT      Initiator:1;	// 1: originator    0:recipient
+    USHORT      TID:4;	// value of TC os TS
+#endif /* !RT_BIG_ENDIAN */
+} DELBA_PARM, *PDELBA_PARM;
+
+// 2-byte BA Parameter Set field  in ADDBA frames to signal parm for setting up a BA
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+    USHORT      BufSize:10;	// number of buffe of size 2304 octetsr
+    USHORT      TID:4;	// value of TC os TS
+    USHORT      BAPolicy:1;	// 1: immediately BA    0:delayed BA
+    USHORT      AMSDUSupported:1;	// 0: not permitted		1: permitted
+#else
+    USHORT      AMSDUSupported:1;	// 0: not permitted		1: permitted
+    USHORT      BAPolicy:1;	// 1: immediately BA    0:delayed BA
+    USHORT      TID:4;	// value of TC os TS
+    USHORT      BufSize:10;	// number of buffe of size 2304 octetsr
+#endif /* !RT_BIG_ENDIAN */
+} BA_PARM, *PBA_PARM;
+
+// 2-byte BA Starting Seq CONTROL field
+typedef union PACKED {
+    struct PACKED {
+#ifdef RT_BIG_ENDIAN
+    USHORT      StartSeq:12;   // sequence number of the 1st MSDU for which this BAR is sent
+	USHORT      FragNum:4;	// always set to 0
+#else
+    USHORT      FragNum:4;	// always set to 0
+	USHORT      StartSeq:12;   // sequence number of the 1st MSDU for which this BAR is sent
+#endif /* RT_BIG_ENDIAN */
+    }   field;
+    USHORT           word;
+} BASEQ_CONTROL, *PBASEQ_CONTROL;
+
+//BAControl and BARControl are the same
+// 2-byte BA CONTROL field in BA frame
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+    USHORT      TID:4;
+    USHORT      Rsv:9;
+    USHORT      Compressed:1;
+    USHORT      MTID:1;		//EWC V1.24
+    USHORT      ACKPolicy:1; // only related to N-Delayed BA. But not support in RT2860b. 0:NormalACK  1:No ACK
+#else
+    USHORT      ACKPolicy:1; // only related to N-Delayed BA. But not support in RT2860b. 0:NormalACK  1:No ACK
+    USHORT      MTID:1;		//EWC V1.24
+    USHORT      Compressed:1;
+    USHORT      Rsv:9;
+    USHORT      TID:4;
+#endif /* !RT_BIG_ENDIAN */
+} BA_CONTROL, *PBA_CONTROL;
+
+// 2-byte BAR CONTROL field in BAR frame
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+    USHORT      TID:4;
+    USHORT      Rsv1:9;
+    USHORT      Compressed:1;
+    USHORT      MTID:1;		//if this bit1, use  FRAME_MTBA_REQ,  if 0, use FRAME_BA_REQ
+    USHORT      ACKPolicy:1;
+#else
+    USHORT      ACKPolicy:1; // 0:normal ack,  1:no ack.
+    USHORT      MTID:1;		//if this bit1, use  FRAME_MTBA_REQ,  if 0, use FRAME_BA_REQ
+    USHORT      Compressed:1;
+    USHORT      Rsv1:9;
+    USHORT      TID:4;
+#endif /* !RT_BIG_ENDIAN */
+} BAR_CONTROL, *PBAR_CONTROL;
+
+// BARControl in MTBAR frame
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+    USHORT      NumTID:4;
+    USHORT      Rsv1:9;
+    USHORT      Compressed:1;
+    USHORT      MTID:1;
+    USHORT      ACKPolicy:1;
+#else
+    USHORT      ACKPolicy:1;
+    USHORT      MTID:1;
+    USHORT      Compressed:1;
+    USHORT      Rsv1:9;
+    USHORT      NumTID:4;
+#endif /* !RT_BIG_ENDIAN */
+} MTBAR_CONTROL, *PMTBAR_CONTROL;
+
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+    USHORT      TID:4;
+    USHORT      Rsv1:12;
+#else
+    USHORT      Rsv1:12;
+    USHORT      TID:4;
+#endif /* !RT_BIG_ENDIAN */
+} PER_TID_INFO, *PPER_TID_INFO;
+
+typedef struct {
+	PER_TID_INFO      PerTID;
+	BASEQ_CONTROL 	 BAStartingSeq;
+} EACH_TID, *PEACH_TID;
+
+
+typedef struct PACKED _PSPOLL_FRAME {
+    FRAME_CONTROL   FC;
+    USHORT          Aid;
+    UCHAR           Bssid[MAC_ADDR_LEN];
+    UCHAR           Ta[MAC_ADDR_LEN];
+}   PSPOLL_FRAME, *PPSPOLL_FRAME;
+
+typedef	struct	PACKED _RTS_FRAME	{
+    FRAME_CONTROL   FC;
+    USHORT          Duration;
+    UCHAR           Addr1[MAC_ADDR_LEN];
+    UCHAR           Addr2[MAC_ADDR_LEN];
+}RTS_FRAME, *PRTS_FRAME;
+
+// BAREQ AND MTBAREQ have the same subtype BAR, 802.11n BAR use compressed bitmap.
+typedef struct PACKED _FRAME_BA_REQ {
+	FRAME_CONTROL   FC;
+	USHORT          Duration;
+	UCHAR           Addr1[MAC_ADDR_LEN];
+	UCHAR           Addr2[MAC_ADDR_LEN];
+	BAR_CONTROL  BARControl;
+	BASEQ_CONTROL 	 BAStartingSeq;
+}   FRAME_BA_REQ, *PFRAME_BA_REQ;
+
+typedef struct PACKED _FRAME_MTBA_REQ {
+	FRAME_CONTROL   FC;
+	USHORT          Duration;
+	UCHAR           Addr1[MAC_ADDR_LEN];
+	UCHAR           Addr2[MAC_ADDR_LEN];
+	MTBAR_CONTROL  MTBARControl;
+	PER_TID_INFO	PerTIDInfo;
+	BASEQ_CONTROL 	 BAStartingSeq;
+}   FRAME_MTBA_REQ, *PFRAME_MTBA_REQ;
+
+// Compressed format is mandantory in HT STA
+typedef struct PACKED _FRAME_MTBA {
+	FRAME_CONTROL   FC;
+	USHORT          Duration;
+	UCHAR           Addr1[MAC_ADDR_LEN];
+	UCHAR           Addr2[MAC_ADDR_LEN];
+	BA_CONTROL  BAControl;
+	BASEQ_CONTROL 	 BAStartingSeq;
+	UCHAR		BitMap[8];
+}   FRAME_MTBA, *PFRAME_MTBA;
+
+typedef struct PACKED _FRAME_PSMP_ACTION {
+	HEADER_802_11   Hdr;
+	UCHAR	Category;
+	UCHAR	Action;
+	UCHAR	Psmp;	// 7.3.1.25
+}   FRAME_PSMP_ACTION, *PFRAME_PSMP_ACTION;
+
+typedef struct PACKED _FRAME_ACTION_HDR {
+	HEADER_802_11   Hdr;
+	UCHAR	Category;
+	UCHAR	Action;
+}   FRAME_ACTION_HDR, *PFRAME_ACTION_HDR;
+
+//Action Frame
+//Action Frame  Category:Spectrum,  Action:Channel Switch. 7.3.2.20
+typedef struct PACKED _CHAN_SWITCH_ANNOUNCE {
+	UCHAR					ElementID;	// ID = IE_CHANNEL_SWITCH_ANNOUNCEMENT = 37
+	UCHAR					Len;
+	CHA_SWITCH_ANNOUNCE_IE	CSAnnounceIe;
+}   CHAN_SWITCH_ANNOUNCE, *PCHAN_SWITCH_ANNOUNCE;
+
+
+//802.11n : 7.3.2.20a
+typedef struct PACKED _SECOND_CHAN_OFFSET {
+	UCHAR				ElementID;		// ID = IE_SECONDARY_CH_OFFSET = 62
+	UCHAR				Len;
+	SEC_CHA_OFFSET_IE	SecChOffsetIe;
+}   SECOND_CHAN_OFFSET, *PSECOND_CHAN_OFFSET;
+
+
+typedef struct PACKED _FRAME_SPETRUM_CS {
+	HEADER_802_11   Hdr;
+	UCHAR	Category;
+	UCHAR	Action;
+	CHAN_SWITCH_ANNOUNCE	CSAnnounce;
+	SECOND_CHAN_OFFSET		SecondChannel;
+}   FRAME_SPETRUM_CS, *PFRAME_SPETRUM_CS;
+
+
+typedef struct PACKED _FRAME_ADDBA_REQ {
+	HEADER_802_11   Hdr;
+	UCHAR	Category;
+	UCHAR	Action;
+	UCHAR	Token;	// 1
+	BA_PARM		BaParm;	      //  2 - 10
+	USHORT		TimeOutValue;	// 0 - 0
+	BASEQ_CONTROL	BaStartSeq; // 0-0
+}   FRAME_ADDBA_REQ, *PFRAME_ADDBA_REQ;
+
+typedef struct PACKED _FRAME_ADDBA_RSP {
+	HEADER_802_11   Hdr;
+	UCHAR	Category;
+	UCHAR	Action;
+	UCHAR	Token;
+	USHORT	StatusCode;
+	BA_PARM		BaParm; //0 - 2
+	USHORT		TimeOutValue;
+}   FRAME_ADDBA_RSP, *PFRAME_ADDBA_RSP;
+
+typedef struct PACKED _FRAME_DELBA_REQ {
+	HEADER_802_11   Hdr;
+	UCHAR	Category;
+	UCHAR	Action;
+	DELBA_PARM		DelbaParm;
+	USHORT	ReasonCode;
+}   FRAME_DELBA_REQ, *PFRAME_DELBA_REQ;
+
+
+//7.2.1.7
+typedef struct PACKED _FRAME_BAR {
+	FRAME_CONTROL   FC;
+	USHORT          Duration;
+	UCHAR           Addr1[MAC_ADDR_LEN];
+	UCHAR           Addr2[MAC_ADDR_LEN];
+	BAR_CONTROL		BarControl;
+	BASEQ_CONTROL	StartingSeq;
+}   FRAME_BAR, *PFRAME_BAR;
+
+//7.2.1.7
+typedef struct PACKED _FRAME_BA {
+	FRAME_CONTROL   FC;
+	USHORT          Duration;
+	UCHAR           Addr1[MAC_ADDR_LEN];
+	UCHAR           Addr2[MAC_ADDR_LEN];
+	BAR_CONTROL		BarControl;
+	BASEQ_CONTROL	StartingSeq;
+	UCHAR		bitmask[8];
+}   FRAME_BA, *PFRAME_BA;
+
+
+// Radio Measuement Request Frame Format
+typedef struct PACKED _FRAME_RM_REQ_ACTION {
+	HEADER_802_11   Hdr;
+	UCHAR	Category;
+	UCHAR	Action;
+	UCHAR	Token;
+	USHORT	Repetition;
+	UCHAR   data[0];
+}   FRAME_RM_REQ_ACTION, *PFRAME_RM_REQ_ACTION;
+
+typedef struct PACKED {
+	UCHAR		ID;
+	UCHAR		Length;
+	UCHAR		ChannelSwitchMode;
+	UCHAR		NewRegClass;
+	UCHAR		NewChannelNum;
+	UCHAR		ChannelSwitchCount;
+} HT_EXT_CHANNEL_SWITCH_ANNOUNCEMENT_IE, *PHT_EXT_CHANNEL_SWITCH_ANNOUNCEMENT_IE;
+
+
+//
+// _Limit must be the 2**n - 1
+// _SEQ1 , _SEQ2 must be within 0 ~ _Limit
+//
+#define SEQ_STEPONE(_SEQ1, _SEQ2, _Limit)	((_SEQ1 == ((_SEQ2+1) & _Limit)))
+#define SEQ_SMALLER(_SEQ1, _SEQ2, _Limit)	(((_SEQ1-_SEQ2) & ((_Limit+1)>>1)))
+#define SEQ_LARGER(_SEQ1, _SEQ2, _Limit)	((_SEQ1 != _SEQ2) && !(((_SEQ1-_SEQ2) & ((_Limit+1)>>1))))
+#define SEQ_WITHIN_WIN(_SEQ1, _SEQ2, _WIN, _Limit) (SEQ_LARGER(_SEQ1, _SEQ2, _Limit) &&  \
+												SEQ_SMALLER(_SEQ1, ((_SEQ2+_WIN+1)&_Limit), _Limit))
+
+//
+// Contention-free parameter (without ID and Length)
+//
+typedef struct PACKED {
+    BOOLEAN     bValid;         // 1: variable contains valid value
+    UCHAR       CfpCount;
+    UCHAR       CfpPeriod;
+    USHORT      CfpMaxDuration;
+    USHORT      CfpDurRemaining;
+} CF_PARM, *PCF_PARM;
+
+typedef	struct	_CIPHER_SUITE	{
+	NDIS_802_11_ENCRYPTION_STATUS	PairCipher;		// Unicast cipher 1, this one has more secured cipher suite
+	NDIS_802_11_ENCRYPTION_STATUS	PairCipherAux;	// Unicast cipher 2 if AP announce two unicast cipher suite
+	NDIS_802_11_ENCRYPTION_STATUS	GroupCipher;	// Group cipher
+	USHORT							RsnCapability;	// RSN capability from beacon
+	BOOLEAN							bMixMode;		// Indicate Pair & Group cipher might be different
+}	CIPHER_SUITE, *PCIPHER_SUITE;
+
+// EDCA configuration from AP's BEACON/ProbeRsp
+typedef struct {
+    BOOLEAN     bValid;         // 1: variable contains valid value
+    BOOLEAN     bAdd;         // 1: variable contains valid value
+    BOOLEAN     bQAck;
+    BOOLEAN     bQueueRequest;
+    BOOLEAN     bTxopRequest;
+    BOOLEAN     bAPSDCapable;
+//  BOOLEAN     bMoreDataAck;
+    UCHAR       EdcaUpdateCount;
+    UCHAR       Aifsn[4];       // 0:AC_BK, 1:AC_BE, 2:AC_VI, 3:AC_VO
+    UCHAR       Cwmin[4];
+    UCHAR       Cwmax[4];
+    USHORT      Txop[4];      // in unit of 32-us
+    BOOLEAN     bACM[4];      // 1: Admission Control of AC_BK is mandattory
+} EDCA_PARM, *PEDCA_PARM;
+
+// QBSS LOAD information from QAP's BEACON/ProbeRsp
+typedef struct {
+    BOOLEAN     bValid;                     // 1: variable contains valid value
+    USHORT      StaNum;
+    UCHAR       ChannelUtilization;
+    USHORT      RemainingAdmissionControl;  // in unit of 32-us
+} QBSS_LOAD_PARM, *PQBSS_LOAD_PARM;
+
+// QBSS Info field in QSTA's assoc req
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+	UCHAR		Rsv2:1;
+	UCHAR		MaxSPLength:2;
+	UCHAR		Rsv1:1;
+	UCHAR		UAPSD_AC_BE:1;
+	UCHAR		UAPSD_AC_BK:1;
+	UCHAR		UAPSD_AC_VI:1;
+	UCHAR		UAPSD_AC_VO:1;
+#else
+    UCHAR		UAPSD_AC_VO:1;
+	UCHAR		UAPSD_AC_VI:1;
+	UCHAR		UAPSD_AC_BK:1;
+	UCHAR		UAPSD_AC_BE:1;
+	UCHAR		Rsv1:1;
+	UCHAR		MaxSPLength:2;
+	UCHAR		Rsv2:1;
+#endif /* !RT_BIG_ENDIAN */
+} QBSS_STA_INFO_PARM, *PQBSS_STA_INFO_PARM;
+
+// QBSS Info field in QAP's Beacon/ProbeRsp
+typedef struct PACKED {
+#ifdef RT_BIG_ENDIAN
+	UCHAR		UAPSD:1;
+	UCHAR		Rsv:3;
+    UCHAR		ParamSetCount:4;
+#else
+    UCHAR		ParamSetCount:4;
+	UCHAR		Rsv:3;
+	UCHAR		UAPSD:1;
+#endif /* !RT_BIG_ENDIAN */
+} QBSS_AP_INFO_PARM, *PQBSS_AP_INFO_PARM;
+
+// QOS Capability reported in QAP's BEACON/ProbeRsp
+// QOS Capability sent out in QSTA's AssociateReq/ReAssociateReq
+typedef struct {
+    BOOLEAN     bValid;                     // 1: variable contains valid value
+    BOOLEAN     bQAck;
+    BOOLEAN     bQueueRequest;
+    BOOLEAN     bTxopRequest;
+//  BOOLEAN     bMoreDataAck;
+    UCHAR       EdcaUpdateCount;
+} QOS_CAPABILITY_PARM, *PQOS_CAPABILITY_PARM;
+
+#ifdef CONFIG_STA_SUPPORT
+typedef struct {
+    UCHAR       IELen;
+    UCHAR       IE[MAX_CUSTOM_LEN];
+} WPA_IE_;
+#endif // CONFIG_STA_SUPPORT //
+
+
+typedef struct {
+    UCHAR   Bssid[MAC_ADDR_LEN];
+    UCHAR   Channel;
+	UCHAR   CentralChannel;	//Store the wide-band central channel for 40MHz.  .used in 40MHz AP. Or this is the same as Channel.
+    UCHAR   BssType;
+    USHORT  AtimWin;
+    USHORT  BeaconPeriod;
+
+    UCHAR   SupRate[MAX_LEN_OF_SUPPORTED_RATES];
+    UCHAR   SupRateLen;
+    UCHAR   ExtRate[MAX_LEN_OF_SUPPORTED_RATES];
+    UCHAR   ExtRateLen;
+	HT_CAPABILITY_IE HtCapability;
+	UCHAR			HtCapabilityLen;
+	ADD_HT_INFO_IE AddHtInfo;	// AP might use this additional ht info IE
+	UCHAR			AddHtInfoLen;
+	UCHAR			NewExtChanOffset;
+	CHAR    Rssi;
+    UCHAR   Privacy;			// Indicate security function ON/OFF. Don't mess up with auth mode.
+	UCHAR	Hidden;
+
+    USHORT  DtimPeriod;
+    USHORT  CapabilityInfo;
+
+    USHORT  CfpCount;
+    USHORT  CfpPeriod;
+    USHORT  CfpMaxDuration;
+    USHORT  CfpDurRemaining;
+    UCHAR   SsidLen;
+    CHAR    Ssid[MAX_LEN_OF_SSID];
+
+    ULONG   LastBeaconRxTime; // OS's timestamp
+
+	BOOLEAN	bSES;
+
+	// New for WPA2
+	CIPHER_SUITE					WPA;			// AP announced WPA cipher suite
+	CIPHER_SUITE					WPA2;			// AP announced WPA2 cipher suite
+
+	// New for microsoft WPA support
+	NDIS_802_11_FIXED_IEs	FixIEs;
+	NDIS_802_11_AUTHENTICATION_MODE	AuthModeAux;	// Addition mode for WPA2 / WPA capable AP
+	NDIS_802_11_AUTHENTICATION_MODE	AuthMode;
+	NDIS_802_11_WEP_STATUS	WepStatus;				// Unicast Encryption Algorithm extract from VAR_IE
+	USHORT					VarIELen;				// Length of next VIE include EID & Length
+	UCHAR					VarIEs[MAX_VIE_LEN];
+
+	// CCX Ckip information
+    UCHAR   CkipFlag;
+
+	// CCX 2 TSF
+	UCHAR	PTSF[4];		// Parent TSF
+	UCHAR	TTSF[8];		// Target TSF
+
+    // 802.11e d9, and WMM
+	EDCA_PARM           EdcaParm;
+	QOS_CAPABILITY_PARM QosCapability;
+	QBSS_LOAD_PARM      QbssLoad;
+#ifdef CONFIG_STA_SUPPORT
+    WPA_IE_     WpaIE;
+    WPA_IE_     RsnIE;
+#ifdef EXT_BUILD_CHANNEL_LIST
+	UCHAR		CountryString[3];
+	BOOLEAN		bHasCountryIE;
+#endif // EXT_BUILD_CHANNEL_LIST //
+#endif // CONFIG_STA_SUPPORT //
+} BSS_ENTRY, *PBSS_ENTRY;
+
+typedef struct {
+    UCHAR           BssNr;
+    UCHAR           BssOverlapNr;
+    BSS_ENTRY       BssEntry[MAX_LEN_OF_BSS_TABLE];
+} BSS_TABLE, *PBSS_TABLE;
+
+
+typedef struct _MLME_QUEUE_ELEM {
+    ULONG             Machine;
+    ULONG             MsgType;
+    ULONG             MsgLen;
+    UCHAR             Msg[MGMT_DMA_BUFFER_SIZE];
+    LARGE_INTEGER     TimeStamp;
+    UCHAR             Rssi0;
+    UCHAR             Rssi1;
+    UCHAR             Rssi2;
+    UCHAR             Signal;
+    UCHAR             Channel;
+    UCHAR             Wcid;
+    BOOLEAN           Occupied;
+} MLME_QUEUE_ELEM, *PMLME_QUEUE_ELEM;
+
+typedef struct _MLME_QUEUE {
+    ULONG             Num;
+    ULONG             Head;
+    ULONG             Tail;
+    NDIS_SPIN_LOCK   Lock;
+    MLME_QUEUE_ELEM  Entry[MAX_LEN_OF_MLME_QUEUE];
+} MLME_QUEUE, *PMLME_QUEUE;
+
+typedef VOID (*STATE_MACHINE_FUNC)(VOID *Adaptor, MLME_QUEUE_ELEM *Elem);
+
+typedef struct _STATE_MACHINE {
+    ULONG                           Base;
+    ULONG                           NrState;
+    ULONG                           NrMsg;
+    ULONG                           CurrState;
+    STATE_MACHINE_FUNC             *TransFunc;
+} STATE_MACHINE, *PSTATE_MACHINE;
+
+
+// MLME AUX data structure that hold temporarliy settings during a connection attempt.
+// Once this attemp succeeds, all settings will be copy to pAd->StaActive.
+// A connection attempt (user set OID, roaming, CCX fast roaming,..) consists of
+// several steps (JOIN, AUTH, ASSOC or REASSOC) and may fail at any step. We purposely
+// separate this under-trial settings away from pAd->StaActive so that once
+// this new attempt failed, driver can auto-recover back to the active settings.
+typedef struct _MLME_AUX {
+    UCHAR               BssType;
+    UCHAR               Ssid[MAX_LEN_OF_SSID];
+    UCHAR               SsidLen;
+    UCHAR               Bssid[MAC_ADDR_LEN];
+	UCHAR				AutoReconnectSsid[MAX_LEN_OF_SSID];
+	UCHAR				AutoReconnectSsidLen;
+    USHORT              Alg;
+    UCHAR               ScanType;
+    UCHAR               Channel;
+	UCHAR               CentralChannel;
+    USHORT              Aid;
+    USHORT              CapabilityInfo;
+    USHORT              BeaconPeriod;
+    USHORT              CfpMaxDuration;
+    USHORT              CfpPeriod;
+    USHORT              AtimWin;
+
+	// Copy supported rate from desired AP's beacon. We are trying to match
+	// AP's supported and extended rate settings.
+	UCHAR		        SupRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR		        ExtRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR		        SupRateLen;
+	UCHAR		        ExtRateLen;
+	HT_CAPABILITY_IE		HtCapability;
+	UCHAR		        	HtCapabilityLen;
+	ADD_HT_INFO_IE		AddHtInfo;	// AP might use this additional ht info IE
+	UCHAR			NewExtChannelOffset;
+	//RT_HT_CAPABILITY	SupportedHtPhy;
+
+    // new for QOS
+    QOS_CAPABILITY_PARM APQosCapability;    // QOS capability of the current associated AP
+    EDCA_PARM           APEdcaParm;         // EDCA parameters of the current associated AP
+    QBSS_LOAD_PARM      APQbssLoad;         // QBSS load of the current associated AP
+
+    // new to keep Ralink specific feature
+    ULONG               APRalinkIe;
+
+    BSS_TABLE           SsidBssTab;     // AP list for the same SSID
+    BSS_TABLE           RoamTab;        // AP list eligible for roaming
+    ULONG               BssIdx;
+    ULONG               RoamIdx;
+
+	BOOLEAN				CurrReqIsFromNdis;
+
+    RALINK_TIMER_STRUCT BeaconTimer, ScanTimer;
+    RALINK_TIMER_STRUCT AuthTimer;
+    RALINK_TIMER_STRUCT AssocTimer, ReassocTimer, DisassocTimer;
+} MLME_AUX, *PMLME_AUX;
+
+typedef struct _MLME_ADDBA_REQ_STRUCT{
+	UCHAR   Wcid;	//
+	UCHAR   pAddr[MAC_ADDR_LEN];
+	UCHAR   BaBufSize;
+	USHORT	TimeOutValue;
+	UCHAR   TID;
+	UCHAR   Token;
+	USHORT	BaStartSeq;
+} MLME_ADDBA_REQ_STRUCT, *PMLME_ADDBA_REQ_STRUCT;
+
+
+typedef struct _MLME_DELBA_REQ_STRUCT{
+	UCHAR   Wcid;	//
+	UCHAR     Addr[MAC_ADDR_LEN];
+	UCHAR   TID;
+	UCHAR	Initiator;
+} MLME_DELBA_REQ_STRUCT, *PMLME_DELBA_REQ_STRUCT;
+
+// assoc struct is equal to reassoc
+typedef struct _MLME_ASSOC_REQ_STRUCT{
+    UCHAR     Addr[MAC_ADDR_LEN];
+    USHORT    CapabilityInfo;
+    USHORT    ListenIntv;
+    ULONG     Timeout;
+} MLME_ASSOC_REQ_STRUCT, *PMLME_ASSOC_REQ_STRUCT, MLME_REASSOC_REQ_STRUCT, *PMLME_REASSOC_REQ_STRUCT;
+
+typedef struct _MLME_DISASSOC_REQ_STRUCT{
+    UCHAR     Addr[MAC_ADDR_LEN];
+    USHORT    Reason;
+} MLME_DISASSOC_REQ_STRUCT, *PMLME_DISASSOC_REQ_STRUCT;
+
+typedef struct _MLME_AUTH_REQ_STRUCT {
+    UCHAR        Addr[MAC_ADDR_LEN];
+    USHORT       Alg;
+    ULONG        Timeout;
+} MLME_AUTH_REQ_STRUCT, *PMLME_AUTH_REQ_STRUCT;
+
+typedef struct _MLME_DEAUTH_REQ_STRUCT {
+    UCHAR        Addr[MAC_ADDR_LEN];
+    USHORT       Reason;
+} MLME_DEAUTH_REQ_STRUCT, *PMLME_DEAUTH_REQ_STRUCT;
+
+typedef struct {
+    ULONG      BssIdx;
+} MLME_JOIN_REQ_STRUCT;
+
+typedef struct _MLME_SCAN_REQ_STRUCT {
+    UCHAR      Bssid[MAC_ADDR_LEN];
+    UCHAR      BssType;
+    UCHAR      ScanType;
+    UCHAR      SsidLen;
+    CHAR       Ssid[MAX_LEN_OF_SSID];
+} MLME_SCAN_REQ_STRUCT, *PMLME_SCAN_REQ_STRUCT;
+
+typedef struct _MLME_START_REQ_STRUCT {
+    CHAR        Ssid[MAX_LEN_OF_SSID];
+    UCHAR       SsidLen;
+} MLME_START_REQ_STRUCT, *PMLME_START_REQ_STRUCT;
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+// structure for DLS
+typedef struct _RT_802_11_DLS {
+	USHORT						TimeOut;		// Use to time out while slience, unit: second , set by UI
+	USHORT						CountDownTimer;	// Use to time out while slience,unit: second , used by driver only
+	NDIS_802_11_MAC_ADDRESS		MacAddr;		// set by UI
+	UCHAR						Status;			// 0: none , 1: wait STAkey, 2: finish DLS setup , set by driver only
+	BOOLEAN						Valid;			// 1: valid , 0: invalid , set by UI, use to setup or tear down DLS link
+	RALINK_TIMER_STRUCT			Timer;			// Use to time out while handshake
+	USHORT						Sequence;
+	USHORT						MacTabMatchWCID;	// ASIC
+	BOOLEAN						bHTCap;
+	PVOID						pAd;
+} RT_802_11_DLS, *PRT_802_11_DLS;
+
+typedef struct _MLME_DLS_REQ_STRUCT {
+    PRT_802_11_DLS	pDLS;
+    USHORT			Reason;
+} MLME_DLS_REQ_STRUCT, *PMLME_DLS_REQ_STRUCT;
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+typedef struct PACKED {
+    UCHAR   Eid;
+    UCHAR   Len;
+    CHAR   Octet[1];
+} EID_STRUCT,*PEID_STRUCT, BEACON_EID_STRUCT, *PBEACON_EID_STRUCT;
+
+typedef struct PACKED _RTMP_TX_RATE_SWITCH
+{
+	UCHAR   ItemNo;
+#ifdef RT_BIG_ENDIAN
+	UCHAR	Rsv2:2;
+	UCHAR	Mode:2;
+	UCHAR	Rsv1:1;
+	UCHAR	BW:1;
+	UCHAR	ShortGI:1;
+	UCHAR	STBC:1;
+#else
+	UCHAR	STBC:1;
+	UCHAR	ShortGI:1;
+	UCHAR	BW:1;
+	UCHAR	Rsv1:1;
+	UCHAR	Mode:2;
+	UCHAR	Rsv2:2;
+#endif
+	UCHAR   CurrMCS;
+	UCHAR   TrainUp;
+	UCHAR   TrainDown;
+} RRTMP_TX_RATE_SWITCH, *PRTMP_TX_RATE_SWITCH;
+
+// ========================== AP mlme.h ===============================
+#define TBTT_PRELOAD_TIME       384        // usec. LomgPreamble + 24-byte at 1Mbps
+#define DEFAULT_DTIM_PERIOD     1
+
+// weighting factor to calculate Channel quality, total should be 100%
+//#define RSSI_WEIGHTING                   0
+//#define TX_WEIGHTING                     40
+//#define RX_WEIGHTING                     60
+
+#define MAC_TABLE_AGEOUT_TIME			300			// unit: sec
+#define MAC_TABLE_ASSOC_TIMEOUT			5			// unit: sec
+#define MAC_TABLE_FULL(Tab)				((Tab).size == MAX_LEN_OF_MAC_TABLE)
+
+// AP shall drop the sta if contine Tx fail count reach it.
+#define MAC_ENTRY_LIFE_CHECK_CNT		20			// packet cnt.
+
+// Value domain of pMacEntry->Sst
+typedef enum _Sst {
+    SST_NOT_AUTH,   // 0: equivalent to IEEE 802.11/1999 state 1
+    SST_AUTH,       // 1: equivalent to IEEE 802.11/1999 state 2
+    SST_ASSOC       // 2: equivalent to IEEE 802.11/1999 state 3
+} SST;
+
+// value domain of pMacEntry->AuthState
+typedef enum _AuthState {
+    AS_NOT_AUTH,
+    AS_AUTH_OPEN,       // STA has been authenticated using OPEN SYSTEM
+    AS_AUTH_KEY,        // STA has been authenticated using SHARED KEY
+    AS_AUTHENTICATING   // STA is waiting for AUTH seq#3 using SHARED KEY
+} AUTH_STATE;
+
+//for-wpa value domain of pMacEntry->WpaState  802.1i D3   p.114
+typedef enum _ApWpaState {
+    AS_NOTUSE,              // 0
+    AS_DISCONNECT,          // 1
+    AS_DISCONNECTED,        // 2
+    AS_INITIALIZE,          // 3
+    AS_AUTHENTICATION,      // 4
+    AS_AUTHENTICATION2,     // 5
+    AS_INITPMK,             // 6
+    AS_INITPSK,             // 7
+    AS_PTKSTART,            // 8
+    AS_PTKINIT_NEGOTIATING, // 9
+    AS_PTKINITDONE,         // 10
+    AS_UPDATEKEYS,          // 11
+    AS_INTEGRITY_FAILURE,   // 12
+    AS_KEYUPDATE,           // 13
+} AP_WPA_STATE;
+
+// for-wpa value domain of pMacEntry->WpaState  802.1i D3   p.114
+typedef enum _GTKState {
+    REKEY_NEGOTIATING,
+    REKEY_ESTABLISHED,
+    KEYERROR,
+} GTK_STATE;
+
+//  for-wpa  value domain of pMacEntry->WpaState  802.1i D3   p.114
+typedef enum _WpaGTKState {
+    SETKEYS,
+    SETKEYS_DONE,
+} WPA_GTK_STATE;
+// ====================== end of AP mlme.h ============================
+
+
+#endif	// MLME_H__
diff --git a/drivers/staging/rt2870/netif_block.h b/drivers/staging/rt2870/netif_block.h
new file mode 100644
index 00000000000..6e5151c4109
--- /dev/null
+++ b/drivers/staging/rt2870/netif_block.h
@@ -0,0 +1,58 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#ifndef __NET_IF_BLOCK_H__
+#define __NET_IF_BLOCK_H__
+
+//#include <linux/device.h>
+#include "link_list.h"
+#include "rtmp.h"
+
+#define FREE_NETIF_POOL_SIZE 32
+
+typedef struct _NETIF_ENTRY
+{
+	struct _NETIF_ENTRY *pNext;
+	PNET_DEV pNetDev;
+} NETIF_ENTRY, *PNETIF_ENTRY;
+
+void initblockQueueTab(
+	IN PRTMP_ADAPTER pAd);
+
+BOOLEAN blockNetIf(
+	IN PBLOCK_QUEUE_ENTRY pBlockQueueEntry,
+	IN PNET_DEV pNetDev);
+
+VOID releaseNetIf(
+	IN PBLOCK_QUEUE_ENTRY pBlockQueueEntry);
+
+VOID StopNetIfQueue(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR QueIdx,
+	IN PNDIS_PACKET pPacket);
+#endif // __NET_IF_BLOCK_H__
+
diff --git a/drivers/staging/rt2870/oid.h b/drivers/staging/rt2870/oid.h
new file mode 100644
index 00000000000..d788db6b626
--- /dev/null
+++ b/drivers/staging/rt2870/oid.h
@@ -0,0 +1,1091 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	oid.h
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+*/
+#ifndef _OID_H_
+#define _OID_H_
+
+#define TRUE				1
+#define FALSE				0
+//
+// IEEE 802.11 Structures and definitions
+//
+#define MAX_TX_POWER_LEVEL              100   /* mW */
+#define MAX_RSSI_TRIGGER                -10    /* dBm */
+#define MIN_RSSI_TRIGGER                -200   /* dBm */
+#define MAX_FRAG_THRESHOLD              2346  /* byte count */
+#define MIN_FRAG_THRESHOLD              256   /* byte count */
+#define MAX_RTS_THRESHOLD               2347  /* byte count */
+
+// new types for Media Specific Indications
+// Extension channel offset
+#define EXTCHA_NONE			0
+#define EXTCHA_ABOVE		0x1
+#define EXTCHA_BELOW		0x3
+
+// BW
+#define BAND_WIDTH_20		0
+#define BAND_WIDTH_40		1
+#define BAND_WIDTH_BOTH		2
+#define BAND_WIDTH_10		3	// 802.11j has 10MHz. This definition is for internal usage. doesn't fill in the IE or other field.
+// SHORTGI
+#define GAP_INTERVAL_400	1	// only support in HT mode
+#define GAP_INTERVAL_800	0
+#define GAP_INTERVAL_BOTH	2
+
+#define NdisMediaStateConnected			1
+#define NdisMediaStateDisconnected		0
+
+#define NDIS_802_11_LENGTH_SSID         32
+#define NDIS_802_11_LENGTH_RATES        8
+#define NDIS_802_11_LENGTH_RATES_EX     16
+#define MAC_ADDR_LENGTH                 6
+#define MAX_NUM_OF_CHS					49 // 14 channels @2.4G +  12@UNII + 4 @MMAC + 11 @HiperLAN2 + 7 @Japan + 1 as NULL terminationc
+#define MAX_NUMBER_OF_EVENT				10  // entry # in EVENT table
+#define MAX_NUMBER_OF_MAC				32 // if MAX_MBSSID_NUM is 8, this value can't be larger than 211
+#define MAX_NUMBER_OF_ACL				64
+#define MAX_LENGTH_OF_SUPPORT_RATES		12    // 1, 2, 5.5, 11, 6, 9, 12, 18, 24, 36, 48, 54
+#define MAX_NUMBER_OF_DLS_ENTRY			4
+
+#ifndef UNDER_CE
+// OID definition, since NDIS 5.0 didn't define these, we need to define for our own
+//#if _WIN32_WINNT<=0x0500
+
+#define OID_GEN_MACHINE_NAME               0x0001021A
+
+#ifdef RALINK_ATE
+#define RT_QUERY_ATE_TXDONE_COUNT			0x0401
+#endif // RALINK_ATE //
+#define RT_QUERY_SIGNAL_CONTEXT				0x0402
+#define RT_SET_IAPP_PID                 	0x0404
+#define RT_SET_APD_PID						0x0405
+#define RT_SET_DEL_MAC_ENTRY				0x0406
+
+//
+// IEEE 802.11 OIDs
+//
+#define	OID_GET_SET_TOGGLE			0x8000
+
+#define	OID_802_11_NETWORK_TYPES_SUPPORTED			0x0103
+#define	OID_802_11_NETWORK_TYPE_IN_USE				0x0104
+#define	OID_802_11_RSSI_TRIGGER						0x0107
+#define	RT_OID_802_11_RSSI							0x0108 //rt2860	only , kathy
+#define	RT_OID_802_11_RSSI_1						0x0109 //rt2860	only , kathy
+#define	RT_OID_802_11_RSSI_2						0x010A //rt2860	only , kathy
+#define	OID_802_11_NUMBER_OF_ANTENNAS				0x010B
+#define	OID_802_11_RX_ANTENNA_SELECTED				0x010C
+#define	OID_802_11_TX_ANTENNA_SELECTED				0x010D
+#define	OID_802_11_SUPPORTED_RATES					0x010E
+#define	OID_802_11_ADD_WEP							0x0112
+#define	OID_802_11_REMOVE_WEP						0x0113
+#define	OID_802_11_DISASSOCIATE						0x0114
+#define	OID_802_11_PRIVACY_FILTER					0x0118
+#define	OID_802_11_ASSOCIATION_INFORMATION			0x011E
+#define	OID_802_11_TEST								0x011F
+#define	RT_OID_802_11_COUNTRY_REGION				0x0507
+#define	OID_802_11_BSSID_LIST_SCAN					0x0508
+#define	OID_802_11_SSID								0x0509
+#define	OID_802_11_BSSID							0x050A
+#define	RT_OID_802_11_RADIO							0x050B
+#define	RT_OID_802_11_PHY_MODE						0x050C
+#define	RT_OID_802_11_STA_CONFIG					0x050D
+#define	OID_802_11_DESIRED_RATES					0x050E
+#define	RT_OID_802_11_PREAMBLE						0x050F
+#define	OID_802_11_WEP_STATUS						0x0510
+#define	OID_802_11_AUTHENTICATION_MODE				0x0511
+#define	OID_802_11_INFRASTRUCTURE_MODE				0x0512
+#define	RT_OID_802_11_RESET_COUNTERS				0x0513
+#define	OID_802_11_RTS_THRESHOLD					0x0514
+#define	OID_802_11_FRAGMENTATION_THRESHOLD			0x0515
+#define	OID_802_11_POWER_MODE						0x0516
+#define	OID_802_11_TX_POWER_LEVEL					0x0517
+#define	RT_OID_802_11_ADD_WPA						0x0518
+#define	OID_802_11_REMOVE_KEY						0x0519
+#define	OID_802_11_ADD_KEY							0x0520
+#define	OID_802_11_CONFIGURATION					0x0521
+#define	OID_802_11_TX_PACKET_BURST					0x0522
+#define	RT_OID_802_11_QUERY_NOISE_LEVEL				0x0523
+#define	RT_OID_802_11_EXTRA_INFO					0x0524
+#ifdef	DBG
+#define	RT_OID_802_11_HARDWARE_REGISTER				0x0525
+#endif
+#define OID_802_11_ENCRYPTION_STATUS            OID_802_11_WEP_STATUS
+#define OID_802_11_DEAUTHENTICATION                 0x0526
+#define OID_802_11_DROP_UNENCRYPTED                 0x0527
+#define OID_802_11_MIC_FAILURE_REPORT_FRAME         0x0528
+
+// For 802.1x daemin using to require current driver configuration
+#define OID_802_11_RADIUS_QUERY_SETTING				0x0540
+
+#define	RT_OID_DEVICE_NAME							0x0607
+#define	RT_OID_VERSION_INFO							0x0608
+#define	OID_802_11_BSSID_LIST						0x0609
+#define	OID_802_3_CURRENT_ADDRESS					0x060A
+#define	OID_GEN_MEDIA_CONNECT_STATUS				0x060B
+#define	RT_OID_802_11_QUERY_LINK_STATUS				0x060C
+#define	OID_802_11_RSSI								0x060D
+#define	OID_802_11_STATISTICS						0x060E
+#define	OID_GEN_RCV_OK								0x060F
+#define	OID_GEN_RCV_NO_BUFFER						0x0610
+#define	RT_OID_802_11_QUERY_EEPROM_VERSION			0x0611
+#define	RT_OID_802_11_QUERY_FIRMWARE_VERSION		0x0612
+#define	RT_OID_802_11_QUERY_LAST_RX_RATE			0x0613
+#define	RT_OID_802_11_TX_POWER_LEVEL_1				0x0614
+#define	RT_OID_802_11_QUERY_PIDVID					0x0615
+//for WPA_SUPPLICANT_SUPPORT
+#define OID_SET_COUNTERMEASURES                     0x0616
+#define OID_802_11_SET_IEEE8021X                    0x0617
+#define OID_802_11_SET_IEEE8021X_REQUIRE_KEY        0x0618
+#define OID_802_11_PMKID                            0x0620
+#define RT_OID_WPA_SUPPLICANT_SUPPORT               0x0621
+#define RT_OID_WE_VERSION_COMPILED                  0x0622
+#define RT_OID_NEW_DRIVER                           0x0623
+
+
+//rt2860 , kathy
+#define	RT_OID_802_11_SNR_0							0x0630
+#define	RT_OID_802_11_SNR_1							0x0631
+#define	RT_OID_802_11_QUERY_LAST_TX_RATE			0x0632
+#define	RT_OID_802_11_QUERY_HT_PHYMODE				0x0633
+#define	RT_OID_802_11_SET_HT_PHYMODE				0x0634
+#define	OID_802_11_RELOAD_DEFAULTS					0x0635
+#define	RT_OID_802_11_QUERY_APSD_SETTING			0x0636
+#define	RT_OID_802_11_SET_APSD_SETTING				0x0637
+#define	RT_OID_802_11_QUERY_APSD_PSM				0x0638
+#define	RT_OID_802_11_SET_APSD_PSM					0x0639
+#define	RT_OID_802_11_QUERY_DLS						0x063A
+#define	RT_OID_802_11_SET_DLS						0x063B
+#define	RT_OID_802_11_QUERY_DLS_PARAM				0x063C
+#define	RT_OID_802_11_SET_DLS_PARAM					0x063D
+#define RT_OID_802_11_QUERY_WMM              		0x063E
+#define RT_OID_802_11_SET_WMM      					0x063F
+#define RT_OID_802_11_QUERY_IMME_BA_CAP				0x0640
+#define RT_OID_802_11_SET_IMME_BA_CAP				0x0641
+#define RT_OID_802_11_QUERY_BATABLE					0x0642
+#define RT_OID_802_11_ADD_IMME_BA					0x0643
+#define RT_OID_802_11_TEAR_IMME_BA					0x0644
+#define RT_OID_DRIVER_DEVICE_NAME                   0x0645
+#define RT_OID_802_11_QUERY_DAT_HT_PHYMODE          0x0646
+#define RT_OID_QUERY_MULTIPLE_CARD_SUPPORT          0x0647
+
+// Ralink defined OIDs
+// Dennis Lee move to platform specific
+
+#define	RT_OID_802_11_BSSID					  (OID_GET_SET_TOGGLE |	OID_802_11_BSSID)
+#define	RT_OID_802_11_SSID					  (OID_GET_SET_TOGGLE |	OID_802_11_SSID)
+#define	RT_OID_802_11_INFRASTRUCTURE_MODE	  (OID_GET_SET_TOGGLE |	OID_802_11_INFRASTRUCTURE_MODE)
+#define	RT_OID_802_11_ADD_WEP				  (OID_GET_SET_TOGGLE |	OID_802_11_ADD_WEP)
+#define	RT_OID_802_11_ADD_KEY				  (OID_GET_SET_TOGGLE |	OID_802_11_ADD_KEY)
+#define	RT_OID_802_11_REMOVE_WEP			  (OID_GET_SET_TOGGLE |	OID_802_11_REMOVE_WEP)
+#define	RT_OID_802_11_REMOVE_KEY			  (OID_GET_SET_TOGGLE |	OID_802_11_REMOVE_KEY)
+#define	RT_OID_802_11_DISASSOCIATE			  (OID_GET_SET_TOGGLE |	OID_802_11_DISASSOCIATE)
+#define	RT_OID_802_11_AUTHENTICATION_MODE	  (OID_GET_SET_TOGGLE |	OID_802_11_AUTHENTICATION_MODE)
+#define	RT_OID_802_11_PRIVACY_FILTER		  (OID_GET_SET_TOGGLE |	OID_802_11_PRIVACY_FILTER)
+#define	RT_OID_802_11_BSSID_LIST_SCAN		  (OID_GET_SET_TOGGLE |	OID_802_11_BSSID_LIST_SCAN)
+#define	RT_OID_802_11_WEP_STATUS			  (OID_GET_SET_TOGGLE |	OID_802_11_WEP_STATUS)
+#define	RT_OID_802_11_RELOAD_DEFAULTS		  (OID_GET_SET_TOGGLE |	OID_802_11_RELOAD_DEFAULTS)
+#define	RT_OID_802_11_NETWORK_TYPE_IN_USE	  (OID_GET_SET_TOGGLE |	OID_802_11_NETWORK_TYPE_IN_USE)
+#define	RT_OID_802_11_TX_POWER_LEVEL		  (OID_GET_SET_TOGGLE |	OID_802_11_TX_POWER_LEVEL)
+#define	RT_OID_802_11_RSSI_TRIGGER			  (OID_GET_SET_TOGGLE |	OID_802_11_RSSI_TRIGGER)
+#define	RT_OID_802_11_FRAGMENTATION_THRESHOLD (OID_GET_SET_TOGGLE |	OID_802_11_FRAGMENTATION_THRESHOLD)
+#define	RT_OID_802_11_RTS_THRESHOLD			  (OID_GET_SET_TOGGLE |	OID_802_11_RTS_THRESHOLD)
+#define	RT_OID_802_11_RX_ANTENNA_SELECTED	  (OID_GET_SET_TOGGLE |	OID_802_11_RX_ANTENNA_SELECTED)
+#define	RT_OID_802_11_TX_ANTENNA_SELECTED	  (OID_GET_SET_TOGGLE |	OID_802_11_TX_ANTENNA_SELECTED)
+#define	RT_OID_802_11_SUPPORTED_RATES		  (OID_GET_SET_TOGGLE |	OID_802_11_SUPPORTED_RATES)
+#define	RT_OID_802_11_DESIRED_RATES			  (OID_GET_SET_TOGGLE |	OID_802_11_DESIRED_RATES)
+#define	RT_OID_802_11_CONFIGURATION			  (OID_GET_SET_TOGGLE |	OID_802_11_CONFIGURATION)
+#define	RT_OID_802_11_POWER_MODE			  (OID_GET_SET_TOGGLE |	OID_802_11_POWER_MODE)
+
+typedef enum _NDIS_802_11_STATUS_TYPE
+{
+    Ndis802_11StatusType_Authentication,
+    Ndis802_11StatusType_MediaStreamMode,
+    Ndis802_11StatusType_PMKID_CandidateList,
+    Ndis802_11StatusTypeMax    // not a real type, defined as an upper bound
+} NDIS_802_11_STATUS_TYPE, *PNDIS_802_11_STATUS_TYPE;
+
+typedef UCHAR   NDIS_802_11_MAC_ADDRESS[6];
+
+typedef struct _NDIS_802_11_STATUS_INDICATION
+{
+    NDIS_802_11_STATUS_TYPE StatusType;
+} NDIS_802_11_STATUS_INDICATION, *PNDIS_802_11_STATUS_INDICATION;
+
+// mask for authentication/integrity fields
+#define NDIS_802_11_AUTH_REQUEST_AUTH_FIELDS        0x0f
+
+#define NDIS_802_11_AUTH_REQUEST_REAUTH             0x01
+#define NDIS_802_11_AUTH_REQUEST_KEYUPDATE          0x02
+#define NDIS_802_11_AUTH_REQUEST_PAIRWISE_ERROR     0x06
+#define NDIS_802_11_AUTH_REQUEST_GROUP_ERROR        0x0E
+
+typedef struct _NDIS_802_11_AUTHENTICATION_REQUEST
+{
+    ULONG Length;            // Length of structure
+    NDIS_802_11_MAC_ADDRESS Bssid;
+    ULONG Flags;
+} NDIS_802_11_AUTHENTICATION_REQUEST, *PNDIS_802_11_AUTHENTICATION_REQUEST;
+
+//Added new types for PMKID Candidate lists.
+typedef struct _PMKID_CANDIDATE {
+    NDIS_802_11_MAC_ADDRESS BSSID;
+    ULONG Flags;
+} PMKID_CANDIDATE, *PPMKID_CANDIDATE;
+
+typedef struct _NDIS_802_11_PMKID_CANDIDATE_LIST
+{
+    ULONG Version;       // Version of the structure
+    ULONG NumCandidates; // No. of pmkid candidates
+    PMKID_CANDIDATE CandidateList[1];
+} NDIS_802_11_PMKID_CANDIDATE_LIST, *PNDIS_802_11_PMKID_CANDIDATE_LIST;
+
+//Flags for PMKID Candidate list structure
+#define NDIS_802_11_PMKID_CANDIDATE_PREAUTH_ENABLED	0x01
+
+// Added new types for OFDM 5G and 2.4G
+typedef enum _NDIS_802_11_NETWORK_TYPE
+{
+   Ndis802_11FH,
+   Ndis802_11DS,
+    Ndis802_11OFDM5,
+    Ndis802_11OFDM5_N,
+    Ndis802_11OFDM24,
+    Ndis802_11OFDM24_N,
+   Ndis802_11Automode,
+    Ndis802_11NetworkTypeMax    // not a real type, defined as an upper bound
+} NDIS_802_11_NETWORK_TYPE, *PNDIS_802_11_NETWORK_TYPE;
+
+typedef struct _NDIS_802_11_NETWORK_TYPE_LIST
+{
+    UINT                       NumberOfItems;  // in list below, at least 1
+   NDIS_802_11_NETWORK_TYPE    NetworkType [1];
+} NDIS_802_11_NETWORK_TYPE_LIST, *PNDIS_802_11_NETWORK_TYPE_LIST;
+
+typedef enum _NDIS_802_11_POWER_MODE
+{
+    Ndis802_11PowerModeCAM,
+    Ndis802_11PowerModeMAX_PSP,
+    Ndis802_11PowerModeFast_PSP,
+    Ndis802_11PowerModeLegacy_PSP,
+    Ndis802_11PowerModeMax      // not a real mode, defined as an upper bound
+} NDIS_802_11_POWER_MODE, *PNDIS_802_11_POWER_MODE;
+
+typedef ULONG   NDIS_802_11_TX_POWER_LEVEL; // in milliwatts
+
+//
+// Received Signal Strength Indication
+//
+typedef LONG    NDIS_802_11_RSSI;           // in dBm
+
+typedef struct _NDIS_802_11_CONFIGURATION_FH
+{
+   ULONG           Length;            // Length of structure
+   ULONG           HopPattern;        // As defined by 802.11, MSB set
+   ULONG           HopSet;            // to one if non-802.11
+   ULONG           DwellTime;         // units are Kusec
+} NDIS_802_11_CONFIGURATION_FH, *PNDIS_802_11_CONFIGURATION_FH;
+
+typedef struct _NDIS_802_11_CONFIGURATION
+{
+   ULONG                           Length;             // Length of structure
+   ULONG                           BeaconPeriod;       // units are Kusec
+   ULONG                           ATIMWindow;         // units are Kusec
+   ULONG                           DSConfig;           // Frequency, units are kHz
+   NDIS_802_11_CONFIGURATION_FH    FHConfig;
+} NDIS_802_11_CONFIGURATION, *PNDIS_802_11_CONFIGURATION;
+
+typedef struct _NDIS_802_11_STATISTICS
+{
+   ULONG           Length;             // Length of structure
+   LARGE_INTEGER   TransmittedFragmentCount;
+   LARGE_INTEGER   MulticastTransmittedFrameCount;
+   LARGE_INTEGER   FailedCount;
+   LARGE_INTEGER   RetryCount;
+   LARGE_INTEGER   MultipleRetryCount;
+   LARGE_INTEGER   RTSSuccessCount;
+   LARGE_INTEGER   RTSFailureCount;
+   LARGE_INTEGER   ACKFailureCount;
+   LARGE_INTEGER   FrameDuplicateCount;
+   LARGE_INTEGER   ReceivedFragmentCount;
+   LARGE_INTEGER   MulticastReceivedFrameCount;
+   LARGE_INTEGER   FCSErrorCount;
+   LARGE_INTEGER   TKIPLocalMICFailures;
+   LARGE_INTEGER   TKIPRemoteMICErrors;
+   LARGE_INTEGER   TKIPICVErrors;
+   LARGE_INTEGER   TKIPCounterMeasuresInvoked;
+   LARGE_INTEGER   TKIPReplays;
+   LARGE_INTEGER   CCMPFormatErrors;
+   LARGE_INTEGER   CCMPReplays;
+   LARGE_INTEGER   CCMPDecryptErrors;
+   LARGE_INTEGER   FourWayHandshakeFailures;
+} NDIS_802_11_STATISTICS, *PNDIS_802_11_STATISTICS;
+
+typedef  ULONG  NDIS_802_11_KEY_INDEX;
+typedef ULONGLONG   NDIS_802_11_KEY_RSC;
+
+#define MAX_RADIUS_SRV_NUM			2	  // 802.1x failover number
+
+typedef struct PACKED _RADIUS_SRV_INFO {
+	UINT32			radius_ip;
+	UINT32			radius_port;
+	UCHAR			radius_key[64];
+	UCHAR			radius_key_len;
+} RADIUS_SRV_INFO, *PRADIUS_SRV_INFO;
+
+typedef struct PACKED _RADIUS_KEY_INFO
+{
+	UCHAR			radius_srv_num;
+	RADIUS_SRV_INFO	radius_srv_info[MAX_RADIUS_SRV_NUM];
+	UCHAR			ieee8021xWEP;		 // dynamic WEP
+    UCHAR           key_index;
+    UCHAR           key_length;          // length of key in bytes
+    UCHAR           key_material[13];
+} RADIUS_KEY_INFO, *PRADIUS_KEY_INFO;
+
+// It's used by 802.1x daemon to require relative configuration
+typedef struct PACKED _RADIUS_CONF
+{
+    UINT32          Length;             // Length of this structure
+    UCHAR			mbss_num;			// indicate multiple BSS number
+	UINT32			own_ip_addr;
+	UINT32			retry_interval;
+	UINT32			session_timeout_interval;
+	UCHAR			EAPifname[IFNAMSIZ];
+	UCHAR			EAPifname_len;
+	UCHAR 			PreAuthifname[IFNAMSIZ];
+	UCHAR			PreAuthifname_len;
+	RADIUS_KEY_INFO	RadiusInfo[8/*MAX_MBSSID_NUM*/];
+} RADIUS_CONF, *PRADIUS_CONF;
+
+
+
+#ifdef CONFIG_STA_SUPPORT
+// Key mapping keys require a BSSID
+typedef struct _NDIS_802_11_KEY
+{
+    UINT           Length;             // Length of this structure
+    UINT           KeyIndex;
+    UINT           KeyLength;          // length of key in bytes
+    NDIS_802_11_MAC_ADDRESS BSSID;
+    NDIS_802_11_KEY_RSC KeyRSC;
+    UCHAR           KeyMaterial[1];     // variable length depending on above field
+} NDIS_802_11_KEY, *PNDIS_802_11_KEY;
+#endif // CONFIG_STA_SUPPORT //
+
+typedef struct _NDIS_802_11_REMOVE_KEY
+{
+    UINT           Length;             // Length of this structure
+    UINT           KeyIndex;
+    NDIS_802_11_MAC_ADDRESS BSSID;
+} NDIS_802_11_REMOVE_KEY, *PNDIS_802_11_REMOVE_KEY;
+
+typedef struct _NDIS_802_11_WEP
+{
+   UINT     Length;        // Length of this structure
+   UINT     KeyIndex;           // 0 is the per-client key, 1-N are the
+                                        // global keys
+   UINT     KeyLength;     // length of key in bytes
+   UCHAR     KeyMaterial[1];// variable length depending on above field
+} NDIS_802_11_WEP, *PNDIS_802_11_WEP;
+
+
+typedef enum _NDIS_802_11_NETWORK_INFRASTRUCTURE
+{
+   Ndis802_11IBSS,
+   Ndis802_11Infrastructure,
+   Ndis802_11AutoUnknown,
+   Ndis802_11Monitor,
+   Ndis802_11InfrastructureMax     // Not a real value, defined as upper bound
+} NDIS_802_11_NETWORK_INFRASTRUCTURE, *PNDIS_802_11_NETWORK_INFRASTRUCTURE;
+
+// Add new authentication modes
+typedef enum _NDIS_802_11_AUTHENTICATION_MODE
+{
+   Ndis802_11AuthModeOpen,
+   Ndis802_11AuthModeShared,
+   Ndis802_11AuthModeAutoSwitch,
+    Ndis802_11AuthModeWPA,
+    Ndis802_11AuthModeWPAPSK,
+    Ndis802_11AuthModeWPANone,
+   Ndis802_11AuthModeWPA2,
+   Ndis802_11AuthModeWPA2PSK,
+   	Ndis802_11AuthModeWPA1WPA2,
+	Ndis802_11AuthModeWPA1PSKWPA2PSK,
+   Ndis802_11AuthModeMax           // Not a real mode, defined as upper bound
+} NDIS_802_11_AUTHENTICATION_MODE, *PNDIS_802_11_AUTHENTICATION_MODE;
+
+typedef UCHAR   NDIS_802_11_RATES[NDIS_802_11_LENGTH_RATES];        // Set of 8 data rates
+typedef UCHAR   NDIS_802_11_RATES_EX[NDIS_802_11_LENGTH_RATES_EX];  // Set of 16 data rates
+
+typedef struct PACKED _NDIS_802_11_SSID
+{
+    UINT   SsidLength;         // length of SSID field below, in bytes;
+                                // this can be zero.
+    UCHAR   Ssid[NDIS_802_11_LENGTH_SSID];           // SSID information field
+} NDIS_802_11_SSID, *PNDIS_802_11_SSID;
+
+
+typedef struct PACKED _NDIS_WLAN_BSSID
+{
+   ULONG                               Length;     // Length of this structure
+   NDIS_802_11_MAC_ADDRESS             MacAddress; // BSSID
+   UCHAR                               Reserved[2];
+   NDIS_802_11_SSID                    Ssid;       // SSID
+   ULONG                               Privacy;    // WEP encryption requirement
+   NDIS_802_11_RSSI                    Rssi;       // receive signal strength in dBm
+   NDIS_802_11_NETWORK_TYPE            NetworkTypeInUse;
+   NDIS_802_11_CONFIGURATION           Configuration;
+   NDIS_802_11_NETWORK_INFRASTRUCTURE  InfrastructureMode;
+   NDIS_802_11_RATES                   SupportedRates;
+} NDIS_WLAN_BSSID, *PNDIS_WLAN_BSSID;
+
+typedef struct PACKED _NDIS_802_11_BSSID_LIST
+{
+   UINT           NumberOfItems;      // in list below, at least 1
+   NDIS_WLAN_BSSID Bssid[1];
+} NDIS_802_11_BSSID_LIST, *PNDIS_802_11_BSSID_LIST;
+
+// Added Capabilities, IELength and IEs for each BSSID
+typedef struct PACKED _NDIS_WLAN_BSSID_EX
+{
+    ULONG                               Length;             // Length of this structure
+    NDIS_802_11_MAC_ADDRESS             MacAddress;         // BSSID
+    UCHAR                               Reserved[2];
+    NDIS_802_11_SSID                    Ssid;               // SSID
+    UINT                                Privacy;            // WEP encryption requirement
+    NDIS_802_11_RSSI                    Rssi;               // receive signal
+                                                            // strength in dBm
+    NDIS_802_11_NETWORK_TYPE            NetworkTypeInUse;
+    NDIS_802_11_CONFIGURATION           Configuration;
+    NDIS_802_11_NETWORK_INFRASTRUCTURE  InfrastructureMode;
+    NDIS_802_11_RATES_EX                SupportedRates;
+    ULONG                               IELength;
+    UCHAR                               IEs[1];
+} NDIS_WLAN_BSSID_EX, *PNDIS_WLAN_BSSID_EX;
+
+typedef struct PACKED _NDIS_802_11_BSSID_LIST_EX
+{
+    UINT                   NumberOfItems;      // in list below, at least 1
+    NDIS_WLAN_BSSID_EX      Bssid[1];
+} NDIS_802_11_BSSID_LIST_EX, *PNDIS_802_11_BSSID_LIST_EX;
+
+typedef struct PACKED _NDIS_802_11_FIXED_IEs
+{
+    UCHAR Timestamp[8];
+    USHORT BeaconInterval;
+    USHORT Capabilities;
+} NDIS_802_11_FIXED_IEs, *PNDIS_802_11_FIXED_IEs;
+
+typedef struct _NDIS_802_11_VARIABLE_IEs
+{
+    UCHAR ElementID;
+    UCHAR Length;    // Number of bytes in data field
+    UCHAR data[1];
+} NDIS_802_11_VARIABLE_IEs, *PNDIS_802_11_VARIABLE_IEs;
+
+typedef  ULONG   NDIS_802_11_FRAGMENTATION_THRESHOLD;
+
+typedef  ULONG   NDIS_802_11_RTS_THRESHOLD;
+
+typedef  ULONG   NDIS_802_11_ANTENNA;
+
+typedef enum _NDIS_802_11_PRIVACY_FILTER
+{
+   Ndis802_11PrivFilterAcceptAll,
+   Ndis802_11PrivFilter8021xWEP
+} NDIS_802_11_PRIVACY_FILTER, *PNDIS_802_11_PRIVACY_FILTER;
+
+// Added new encryption types
+// Also aliased typedef to new name
+typedef enum _NDIS_802_11_WEP_STATUS
+{
+   Ndis802_11WEPEnabled,
+    Ndis802_11Encryption1Enabled = Ndis802_11WEPEnabled,
+   Ndis802_11WEPDisabled,
+    Ndis802_11EncryptionDisabled = Ndis802_11WEPDisabled,
+   Ndis802_11WEPKeyAbsent,
+    Ndis802_11Encryption1KeyAbsent = Ndis802_11WEPKeyAbsent,
+   Ndis802_11WEPNotSupported,
+    Ndis802_11EncryptionNotSupported = Ndis802_11WEPNotSupported,
+    Ndis802_11Encryption2Enabled,
+    Ndis802_11Encryption2KeyAbsent,
+    Ndis802_11Encryption3Enabled,
+    Ndis802_11Encryption3KeyAbsent,
+    Ndis802_11Encryption4Enabled,	// TKIP or AES mix
+    Ndis802_11Encryption4KeyAbsent,
+    Ndis802_11GroupWEP40Enabled,
+	Ndis802_11GroupWEP104Enabled,
+} NDIS_802_11_WEP_STATUS, *PNDIS_802_11_WEP_STATUS,
+  NDIS_802_11_ENCRYPTION_STATUS, *PNDIS_802_11_ENCRYPTION_STATUS;
+
+typedef enum _NDIS_802_11_RELOAD_DEFAULTS
+{
+   Ndis802_11ReloadWEPKeys
+} NDIS_802_11_RELOAD_DEFAULTS, *PNDIS_802_11_RELOAD_DEFAULTS;
+
+#define NDIS_802_11_AI_REQFI_CAPABILITIES      1
+#define NDIS_802_11_AI_REQFI_LISTENINTERVAL    2
+#define NDIS_802_11_AI_REQFI_CURRENTAPADDRESS  4
+
+#define NDIS_802_11_AI_RESFI_CAPABILITIES      1
+#define NDIS_802_11_AI_RESFI_STATUSCODE        2
+#define NDIS_802_11_AI_RESFI_ASSOCIATIONID     4
+
+typedef struct _NDIS_802_11_AI_REQFI
+{
+    USHORT Capabilities;
+    USHORT ListenInterval;
+    NDIS_802_11_MAC_ADDRESS  CurrentAPAddress;
+} NDIS_802_11_AI_REQFI, *PNDIS_802_11_AI_REQFI;
+
+typedef struct _NDIS_802_11_AI_RESFI
+{
+    USHORT Capabilities;
+    USHORT StatusCode;
+    USHORT AssociationId;
+} NDIS_802_11_AI_RESFI, *PNDIS_802_11_AI_RESFI;
+
+typedef struct _NDIS_802_11_ASSOCIATION_INFORMATION
+{
+    ULONG                   Length;
+    USHORT                  AvailableRequestFixedIEs;
+    NDIS_802_11_AI_REQFI    RequestFixedIEs;
+    ULONG                   RequestIELength;
+    ULONG                   OffsetRequestIEs;
+    USHORT                  AvailableResponseFixedIEs;
+    NDIS_802_11_AI_RESFI    ResponseFixedIEs;
+    ULONG                   ResponseIELength;
+    ULONG                   OffsetResponseIEs;
+} NDIS_802_11_ASSOCIATION_INFORMATION, *PNDIS_802_11_ASSOCIATION_INFORMATION;
+
+typedef struct _NDIS_802_11_AUTHENTICATION_EVENT
+{
+    NDIS_802_11_STATUS_INDICATION       Status;
+    NDIS_802_11_AUTHENTICATION_REQUEST  Request[1];
+} NDIS_802_11_AUTHENTICATION_EVENT, *PNDIS_802_11_AUTHENTICATION_EVENT;
+
+/*
+typedef struct _NDIS_802_11_TEST
+{
+    ULONG Length;
+    ULONG Type;
+    union
+    {
+        NDIS_802_11_AUTHENTICATION_EVENT AuthenticationEvent;
+        NDIS_802_11_RSSI RssiTrigger;
+    };
+} NDIS_802_11_TEST, *PNDIS_802_11_TEST;
+ */
+
+// 802.11 Media stream constraints, associated with OID_802_11_MEDIA_STREAM_MODE
+typedef enum _NDIS_802_11_MEDIA_STREAM_MODE
+{
+    Ndis802_11MediaStreamOff,
+    Ndis802_11MediaStreamOn,
+} NDIS_802_11_MEDIA_STREAM_MODE, *PNDIS_802_11_MEDIA_STREAM_MODE;
+
+// PMKID Structures
+typedef UCHAR   NDIS_802_11_PMKID_VALUE[16];
+
+#ifdef CONFIG_STA_SUPPORT
+typedef struct _BSSID_INFO
+{
+    NDIS_802_11_MAC_ADDRESS BSSID;
+    NDIS_802_11_PMKID_VALUE PMKID;
+} BSSID_INFO, *PBSSID_INFO;
+
+typedef struct _NDIS_802_11_PMKID
+{
+    UINT    Length;
+    UINT    BSSIDInfoCount;
+    BSSID_INFO BSSIDInfo[1];
+} NDIS_802_11_PMKID, *PNDIS_802_11_PMKID;
+#endif // CONFIG_STA_SUPPORT //
+
+
+typedef struct _NDIS_802_11_AUTHENTICATION_ENCRYPTION
+{
+    NDIS_802_11_AUTHENTICATION_MODE AuthModeSupported;
+    NDIS_802_11_ENCRYPTION_STATUS EncryptStatusSupported;
+} NDIS_802_11_AUTHENTICATION_ENCRYPTION, *PNDIS_802_11_AUTHENTICATION_ENCRYPTION;
+
+typedef struct _NDIS_802_11_CAPABILITY
+{
+     ULONG Length;
+     ULONG Version;
+     ULONG NoOfPMKIDs;
+     ULONG NoOfAuthEncryptPairsSupported;
+     NDIS_802_11_AUTHENTICATION_ENCRYPTION AuthenticationEncryptionSupported[1];
+} NDIS_802_11_CAPABILITY, *PNDIS_802_11_CAPABILITY;
+
+//#endif //of WIN 2k
+#endif //UNDER_CE
+
+#if WIRELESS_EXT <= 11
+#ifndef SIOCDEVPRIVATE
+#define SIOCDEVPRIVATE                              0x8BE0
+#endif
+#define SIOCIWFIRSTPRIV								SIOCDEVPRIVATE
+#endif
+
+#ifdef CONFIG_STA_SUPPORT
+#define RTPRIV_IOCTL_SET							(SIOCIWFIRSTPRIV + 0x02)
+
+#ifdef DBG
+#define RTPRIV_IOCTL_BBP                            (SIOCIWFIRSTPRIV + 0x03)
+#define RTPRIV_IOCTL_MAC                            (SIOCIWFIRSTPRIV + 0x05)
+#define RTPRIV_IOCTL_E2P                            (SIOCIWFIRSTPRIV + 0x07)
+#endif
+
+#ifdef RALINK_ATE
+#ifdef RALINK_28xx_QA
+#define RTPRIV_IOCTL_ATE							(SIOCIWFIRSTPRIV + 0x08)
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+
+#define RTPRIV_IOCTL_STATISTICS                     (SIOCIWFIRSTPRIV + 0x09)
+#define RTPRIV_IOCTL_ADD_PMKID_CACHE                (SIOCIWFIRSTPRIV + 0x0A)
+#define RTPRIV_IOCTL_RADIUS_DATA                    (SIOCIWFIRSTPRIV + 0x0C)
+#define RTPRIV_IOCTL_GSITESURVEY					(SIOCIWFIRSTPRIV + 0x0D)
+#define RT_PRIV_IOCTL								(SIOCIWFIRSTPRIV + 0x0E) // Sync. with RT61 (for wpa_supplicant)
+#define RTPRIV_IOCTL_GET_MAC_TABLE					(SIOCIWFIRSTPRIV + 0x0F)
+
+#define RTPRIV_IOCTL_SHOW							(SIOCIWFIRSTPRIV + 0x11)
+enum {
+    SHOW_CONN_STATUS = 4,
+    SHOW_DRVIER_VERION = 5,
+    SHOW_BA_INFO = 6,
+	SHOW_DESC_INFO = 7,
+#ifdef RT2870
+	SHOW_RXBULK_INFO = 8,
+	SHOW_TXBULK_INFO = 9,
+#endif // RT2870 //
+    RAIO_OFF = 10,
+    RAIO_ON = 11,
+#ifdef QOS_DLS_SUPPORT
+	SHOW_DLS_ENTRY_INFO = 19,
+#endif // QOS_DLS_SUPPORT //
+	SHOW_CFG_VALUE = 20,
+	SHOW_ADHOC_ENTRY_INFO = 21,
+};
+
+
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef SNMP_SUPPORT
+//SNMP ieee 802dot11, kathy , 2008_0220
+// dot11res(3)
+#define RT_OID_802_11_MANUFACTUREROUI			0x0700
+#define RT_OID_802_11_MANUFACTURERNAME			0x0701
+#define RT_OID_802_11_RESOURCETYPEIDNAME		0x0702
+
+// dot11smt(1)
+#define RT_OID_802_11_PRIVACYOPTIONIMPLEMENTED	0x0703
+#define RT_OID_802_11_POWERMANAGEMENTMODE		0x0704
+#define OID_802_11_WEPDEFAULTKEYVALUE			0x0705 // read , write
+#define OID_802_11_WEPDEFAULTKEYID				0x0706
+#define RT_OID_802_11_WEPKEYMAPPINGLENGTH		0x0707
+#define OID_802_11_SHORTRETRYLIMIT				0x0708
+#define OID_802_11_LONGRETRYLIMIT				0x0709
+#define RT_OID_802_11_PRODUCTID					0x0710
+#define RT_OID_802_11_MANUFACTUREID				0x0711
+
+// //dot11Phy(4)
+#define OID_802_11_CURRENTCHANNEL				0x0712
+
+//dot11mac
+#define RT_OID_802_11_MAC_ADDRESS				0x0713
+#endif // SNMP_SUPPORT //
+
+#define OID_802_11_BUILD_CHANNEL_EX				0x0714
+#define OID_802_11_GET_CH_LIST					0x0715
+#define OID_802_11_GET_COUNTRY_CODE				0x0716
+#define OID_802_11_GET_CHANNEL_GEOGRAPHY		0x0717
+
+#ifdef LLTD_SUPPORT
+// for consistency with RT61
+#define RT_OID_GET_PHY_MODE                         0x761
+#endif // LLTD_SUPPORT //
+
+// MIMO Tx parameter, ShortGI, MCS, STBC, etc.  these are fields in TXWI. Don't change this definition!!!
+typedef union  _HTTRANSMIT_SETTING {
+#ifdef RT_BIG_ENDIAN
+	struct	{
+	USHORT		MODE:2;	// Use definition MODE_xxx.
+//	USHORT		rsv:3;
+	USHORT		TxBF:1;
+	USHORT		rsv:2;
+	USHORT		STBC:2;	//SPACE
+	USHORT		ShortGI:1;
+	USHORT		BW:1;	//channel bandwidth 20MHz or 40 MHz
+	USHORT   	MCS:7;                 // MCS
+	}	field;
+#else
+	struct	{
+	USHORT   	MCS:7;                 // MCS
+	USHORT		BW:1;	//channel bandwidth 20MHz or 40 MHz
+	USHORT		ShortGI:1;
+	USHORT		STBC:2;	//SPACE
+//	USHORT		rsv:3;
+	USHORT		rsv:2;
+	USHORT		TxBF:1;
+	USHORT		MODE:2;	// Use definition MODE_xxx.
+	}	field;
+#endif
+	USHORT		word;
+ } HTTRANSMIT_SETTING, *PHTTRANSMIT_SETTING;
+
+typedef enum _RT_802_11_PREAMBLE {
+    Rt802_11PreambleLong,
+    Rt802_11PreambleShort,
+    Rt802_11PreambleAuto
+} RT_802_11_PREAMBLE, *PRT_802_11_PREAMBLE;
+
+// Only for STA, need to sync with AP
+typedef enum _RT_802_11_PHY_MODE {
+	PHY_11BG_MIXED = 0,
+	PHY_11B,
+	PHY_11A,
+	PHY_11ABG_MIXED,
+	PHY_11G,
+#ifdef DOT11_N_SUPPORT
+	PHY_11ABGN_MIXED,	// both band   5
+	PHY_11N_2_4G,		// 11n-only with 2.4G band   	6
+	PHY_11GN_MIXED,	// 2.4G band      7
+	PHY_11AN_MIXED,	// 5G  band       8
+	PHY_11BGN_MIXED,	// if check 802.11b.      9
+	PHY_11AGN_MIXED,	// if check 802.11b.      10
+	PHY_11N_5G,			// 11n-only with 5G band		11
+#endif // DOT11_N_SUPPORT //
+} RT_802_11_PHY_MODE;
+
+// put all proprietery for-query objects here to reduce # of Query_OID
+typedef struct _RT_802_11_LINK_STATUS {
+    ULONG   CurrTxRate;         // in units of 0.5Mbps
+    ULONG   ChannelQuality;     // 0..100 %
+    ULONG   TxByteCount;        // both ok and fail
+    ULONG   RxByteCount;        // both ok and fail
+    ULONG	CentralChannel;		// 40MHz central channel number
+} RT_802_11_LINK_STATUS, *PRT_802_11_LINK_STATUS;
+
+typedef struct _RT_802_11_EVENT_LOG {
+    LARGE_INTEGER   SystemTime;  // timestammp via NdisGetCurrentSystemTime()
+    UCHAR           Addr[MAC_ADDR_LENGTH];
+    USHORT          Event;       // EVENT_xxx
+} RT_802_11_EVENT_LOG, *PRT_802_11_EVENT_LOG;
+
+typedef struct _RT_802_11_EVENT_TABLE {
+    ULONG       Num;
+    ULONG       Rsv;     // to align Log[] at LARGE_INEGER boundary
+    RT_802_11_EVENT_LOG   Log[MAX_NUMBER_OF_EVENT];
+} RT_802_11_EVENT_TABLE, PRT_802_11_EVENT_TABLE;
+
+// MIMO Tx parameter, ShortGI, MCS, STBC, etc.  these are fields in TXWI. Don't change this definition!!!
+typedef union  _MACHTTRANSMIT_SETTING {
+	struct	{
+	USHORT   	MCS:7;                 // MCS
+	USHORT		BW:1;	//channel bandwidth 20MHz or 40 MHz
+	USHORT		ShortGI:1;
+	USHORT		STBC:2;	//SPACE
+	USHORT		rsv:3;
+	USHORT		MODE:2;	// Use definition MODE_xxx.
+	}	field;
+	USHORT		word;
+ } MACHTTRANSMIT_SETTING, *PMACHTTRANSMIT_SETTING;
+
+typedef struct _RT_802_11_MAC_ENTRY {
+    UCHAR       Addr[MAC_ADDR_LENGTH];
+    UCHAR       Aid;
+    UCHAR       Psm;     // 0:PWR_ACTIVE, 1:PWR_SAVE
+    UCHAR		MimoPs;  // 0:MMPS_STATIC, 1:MMPS_DYNAMIC, 3:MMPS_Enabled
+    CHAR		AvgRssi0;
+	CHAR		AvgRssi1;
+	CHAR		AvgRssi2;
+	UINT32		ConnectedTime;
+    MACHTTRANSMIT_SETTING	TxRate;
+} RT_802_11_MAC_ENTRY, *PRT_802_11_MAC_ENTRY;
+
+typedef struct _RT_802_11_MAC_TABLE {
+    ULONG       Num;
+    RT_802_11_MAC_ENTRY Entry[MAX_NUMBER_OF_MAC];
+} RT_802_11_MAC_TABLE, *PRT_802_11_MAC_TABLE;
+
+// structure for query/set hardware register - MAC, BBP, RF register
+typedef struct _RT_802_11_HARDWARE_REGISTER {
+    ULONG   HardwareType;       // 0:MAC, 1:BBP, 2:RF register, 3:EEPROM
+    ULONG   Offset;             // Q/S register offset addr
+    ULONG   Data;               // R/W data buffer
+} RT_802_11_HARDWARE_REGISTER, *PRT_802_11_HARDWARE_REGISTER;
+
+// structure to tune BBP R17 "RX AGC VGC init"
+//typedef struct _RT_802_11_RX_AGC_VGC_TUNING {
+//    UCHAR   FalseCcaLowerThreshold;  // 0-255, def 10
+//    UCHAR   FalseCcaUpperThreshold;  // 0-255, def 100
+//    UCHAR   VgcDelta;                // R17 +-= VgcDelta whenever flase CCA over UpprThreshold
+//                                     // or lower than LowerThresholdupper threshold
+//    UCHAR   VgcUpperBound;           // max value of R17
+//} RT_802_11_RX_AGC_VGC_TUNING, *PRT_802_11_RX_AGC_VGC_TUNING;
+
+typedef struct _RT_802_11_AP_CONFIG {
+    ULONG   EnableTxBurst;      // 0-disable, 1-enable
+    ULONG   EnableTurboRate;    // 0-disable, 1-enable 72/100mbps turbo rate
+    ULONG   IsolateInterStaTraffic;     // 0-disable, 1-enable isolation
+    ULONG   HideSsid;           // 0-disable, 1-enable hiding
+    ULONG   UseBGProtection;    // 0-AUTO, 1-always ON, 2-always OFF
+    ULONG   UseShortSlotTime;   // 0-no use, 1-use 9-us short slot time
+    ULONG   Rsv1;               // must be 0
+    ULONG   SystemErrorBitmap;  // ignore upon SET, return system error upon QUERY
+} RT_802_11_AP_CONFIG, *PRT_802_11_AP_CONFIG;
+
+// structure to query/set STA_CONFIG
+typedef struct _RT_802_11_STA_CONFIG {
+    ULONG   EnableTxBurst;      // 0-disable, 1-enable
+    ULONG   EnableTurboRate;    // 0-disable, 1-enable 72/100mbps turbo rate
+    ULONG   UseBGProtection;    // 0-AUTO, 1-always ON, 2-always OFF
+    ULONG   UseShortSlotTime;   // 0-no use, 1-use 9-us short slot time when applicable
+    ULONG   AdhocMode; 			// 0-11b rates only (WIFI spec), 1 - b/g mixed, 2 - g only
+    ULONG   HwRadioStatus;      // 0-OFF, 1-ON, default is 1, Read-Only
+    ULONG   Rsv1;               // must be 0
+    ULONG   SystemErrorBitmap;  // ignore upon SET, return system error upon QUERY
+} RT_802_11_STA_CONFIG, *PRT_802_11_STA_CONFIG;
+
+//
+//  For OID Query or Set about BA structure
+//
+typedef	struct	_OID_BACAP_STRUC	{
+		UCHAR		RxBAWinLimit;
+		UCHAR		TxBAWinLimit;
+		UCHAR		Policy;	// 0: DELAY_BA 1:IMMED_BA  (//BA Policy subfiled value in ADDBA frame)   2:BA-not use. other value invalid
+		UCHAR		MpduDensity;	// 0: DELAY_BA 1:IMMED_BA  (//BA Policy subfiled value in ADDBA frame)   2:BA-not use. other value invalid
+		UCHAR       	AmsduEnable;	//Enable AMSDU transmisstion
+		UCHAR       	AmsduSize;	// 0:3839, 1:7935 bytes. UINT  MSDUSizeToBytes[]	= { 3839, 7935};
+		UCHAR       	MMPSmode;	// MIMO power save more, 0:static, 1:dynamic, 2:rsv, 3:mimo enable
+		BOOLEAN		AutoBA;	// Auto BA will automatically
+} OID_BACAP_STRUC, *POID_BACAP_STRUC;
+
+typedef struct _RT_802_11_ACL_ENTRY {
+    UCHAR   Addr[MAC_ADDR_LENGTH];
+    USHORT  Rsv;
+} RT_802_11_ACL_ENTRY, *PRT_802_11_ACL_ENTRY;
+
+typedef struct PACKED _RT_802_11_ACL {
+    ULONG   Policy;             // 0-disable, 1-positive list, 2-negative list
+    ULONG   Num;
+    RT_802_11_ACL_ENTRY Entry[MAX_NUMBER_OF_ACL];
+} RT_802_11_ACL, *PRT_802_11_ACL;
+
+typedef struct _RT_802_11_WDS {
+    ULONG						Num;
+    NDIS_802_11_MAC_ADDRESS		Entry[24/*MAX_NUM_OF_WDS_LINK*/];
+	ULONG						KeyLength;
+	UCHAR						KeyMaterial[32];
+} RT_802_11_WDS, *PRT_802_11_WDS;
+
+typedef struct _RT_802_11_TX_RATES_ {
+    UCHAR       SupRateLen;
+    UCHAR       SupRate[MAX_LENGTH_OF_SUPPORT_RATES];
+    UCHAR       ExtRateLen;
+    UCHAR       ExtRate[MAX_LENGTH_OF_SUPPORT_RATES];
+} RT_802_11_TX_RATES, *PRT_802_11_TX_RATES;
+
+
+// Definition of extra information code
+#define	GENERAL_LINK_UP			0x0			// Link is Up
+#define	GENERAL_LINK_DOWN		0x1			// Link is Down
+#define	HW_RADIO_OFF			0x2			// Hardware radio off
+#define	SW_RADIO_OFF			0x3			// Software radio off
+#define	AUTH_FAIL				0x4			// Open authentication fail
+#define	AUTH_FAIL_KEYS			0x5			// Shared authentication fail
+#define	ASSOC_FAIL				0x6			// Association failed
+#define	EAP_MIC_FAILURE			0x7			// Deauthencation because MIC failure
+#define	EAP_4WAY_TIMEOUT		0x8			// Deauthencation on 4-way handshake timeout
+#define	EAP_GROUP_KEY_TIMEOUT	0x9			// Deauthencation on group key handshake timeout
+#define	EAP_SUCCESS				0xa			// EAP succeed
+#define	DETECT_RADAR_SIGNAL		0xb         // Radar signal occur in current channel
+#define EXTRA_INFO_MAX			0xb			// Indicate Last OID
+
+#define EXTRA_INFO_CLEAR		0xffffffff
+
+// This is OID setting structure. So only GF or MM as Mode. This is valid when our wirelss mode has 802.11n in use.
+typedef struct {
+	RT_802_11_PHY_MODE		PhyMode; 	//
+	UCHAR		TransmitNo;
+	UCHAR		HtMode; 	//HTMODE_GF or HTMODE_MM
+	UCHAR		ExtOffset;	//extension channel above or below
+	UCHAR		MCS;
+	UCHAR   	BW;
+	UCHAR		STBC;
+	UCHAR		SHORTGI;
+	UCHAR		rsv;
+} OID_SET_HT_PHYMODE, *POID_SET_HT_PHYMODE;
+
+#ifdef NINTENDO_AP
+#define NINTENDO_MAX_ENTRY 16
+#define NINTENDO_SSID_NAME_LN 8
+#define NINTENDO_SSID_NAME "NWCUSBAP"
+#define NINTENDO_PROBE_REQ_FLAG_MASK 0x03
+#define NINTENDO_PROBE_REQ_ON 0x01
+#define NINTENDO_PROBE_REQ_SIGNAL 0x02
+#define NINTENDO_PROBE_RSP_ON 0x01
+#define NINTENDO_SSID_NICKNAME_LN 20
+
+#define NINTENDO_WEPKEY_LN 13
+
+typedef struct _NINTENDO_SSID
+{
+	UCHAR	NINTENDOFixChar[NINTENDO_SSID_NAME_LN];
+	UCHAR	zero1;
+	UCHAR	registe;
+	UCHAR	ID;
+	UCHAR	zero2;
+	UCHAR	NICKname[NINTENDO_SSID_NICKNAME_LN];
+} RT_NINTENDO_SSID, *PRT_NINTENDO_SSID;
+
+typedef struct _NINTENDO_ENTRY
+{
+	UCHAR	NICKname[NINTENDO_SSID_NICKNAME_LN];
+    UCHAR   DS_Addr[ETH_LENGTH_OF_ADDRESS];
+	UCHAR	registe;
+	UCHAR	UserSpaceAck;
+} RT_NINTENDO_ENTRY, *PRT_NINTENDO_ENTRY;
+
+//RTPRIV_IOCTL_NINTENDO_GET_TABLE
+//RTPRIV_IOCTL_NINTENDO_SET_TABLE
+typedef struct _NINTENDO_TABLE
+{
+	UINT				number;
+	RT_NINTENDO_ENTRY	entry[NINTENDO_MAX_ENTRY];
+} RT_NINTENDO_TABLE, *PRT_NINTENDO_TABLE;
+
+//RTPRIV_IOCTL_NINTENDO_SEED_WEPKEY
+typedef struct _NINTENDO_SEED_WEPKEY
+{
+	UCHAR	seed[NINTENDO_SSID_NICKNAME_LN];
+	UCHAR	wepkey[16];//use 13 for 104 bits wep key
+} RT_NINTENDO_SEED_WEPKEY, *PRT_NINTENDO_SEED_WEPKEY;
+#endif // NINTENDO_AP //
+
+#ifdef LLTD_SUPPORT
+typedef struct _RT_LLTD_ASSOICATION_ENTRY {
+    UCHAR           Addr[ETH_LENGTH_OF_ADDRESS];
+    unsigned short  MOR;        // maximum operational rate
+    UCHAR           phyMode;
+} RT_LLTD_ASSOICATION_ENTRY, *PRT_LLTD_ASSOICATION_ENTRY;
+
+typedef struct _RT_LLTD_ASSOICATION_TABLE {
+    unsigned int                Num;
+    RT_LLTD_ASSOICATION_ENTRY   Entry[MAX_NUMBER_OF_MAC];
+} RT_LLTD_ASSOICATION_TABLE, *PRT_LLTD_ASSOICATION_TABLE;
+#endif // LLTD_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+//rt2860, kathy 2007-0118
+// structure for DLS
+typedef struct _RT_802_11_DLS_UI {
+	USHORT						TimeOut;		// unit: second , set by UI
+	USHORT						CountDownTimer;	// unit: second , used by driver only
+	NDIS_802_11_MAC_ADDRESS		MacAddr;		// set by UI
+	UCHAR						Status;			// 0: none , 1: wait STAkey, 2: finish DLS setup , set by driver only
+	BOOLEAN						Valid;			// 1: valid , 0: invalid , set by UI, use to setup or tear down DLS link
+} RT_802_11_DLS_UI, *PRT_802_11_DLS_UI;
+
+typedef struct _RT_802_11_DLS_INFO {
+	RT_802_11_DLS_UI	Entry[MAX_NUMBER_OF_DLS_ENTRY];
+	UCHAR				num;
+} RT_802_11_DLS_INFO, *PRT_802_11_DLS_INFO;
+
+typedef enum _RT_802_11_DLS_MODE {
+    DLS_NONE,
+    DLS_WAIT_KEY,
+    DLS_FINISH
+} RT_802_11_DLS_MODE;
+#endif // QOS_DLS_SUPPORT //
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+#define	RT_ASSOC_EVENT_FLAG                         0x0101
+#define	RT_DISASSOC_EVENT_FLAG                      0x0102
+#define	RT_REQIE_EVENT_FLAG                         0x0103
+#define	RT_RESPIE_EVENT_FLAG                        0x0104
+#define	RT_ASSOCINFO_EVENT_FLAG                     0x0105
+#define RT_PMKIDCAND_FLAG                           0x0106
+#define RT_INTERFACE_DOWN                           0x0107
+#define RT_INTERFACE_UP                             0x0108
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+
+#define MAX_CUSTOM_LEN 128
+
+#ifdef CONFIG_STA_SUPPORT
+typedef enum _RT_802_11_D_CLIENT_MODE
+{
+   Rt802_11_D_None,
+   Rt802_11_D_Flexible,
+   Rt802_11_D_Strict,
+} RT_802_11_D_CLIENT_MODE, *PRT_802_11_D_CLIENT_MODE;
+#endif // CONFIG_STA_SUPPORT //
+
+typedef struct _RT_CHANNEL_LIST_INFO
+{
+	UCHAR ChannelList[MAX_NUM_OF_CHS];   // list all supported channels for site survey
+	UCHAR ChannelListNum; // number of channel in ChannelList[]
+} RT_CHANNEL_LIST_INFO, *PRT_CHANNEL_LIST_INFO;
+
+// WSC configured credential
+typedef	struct	_WSC_CREDENTIAL
+{
+	NDIS_802_11_SSID	SSID;				// mandatory
+	USHORT				AuthType;			// mandatory, 1: open, 2: wpa-psk, 4: shared, 8:wpa, 0x10: wpa2, 0x20: wpa2-psk
+	USHORT				EncrType;			// mandatory, 1: none, 2: wep, 4: tkip, 8: aes
+	UCHAR				Key[64];			// mandatory, Maximum 64 byte
+	USHORT				KeyLength;
+	UCHAR				MacAddr[6];			// mandatory, AP MAC address
+	UCHAR				KeyIndex;			// optional, default is 1
+	UCHAR				Rsvd[3];			// Make alignment
+}	WSC_CREDENTIAL, *PWSC_CREDENTIAL;
+
+// WSC configured profiles
+typedef	struct	_WSC_PROFILE
+{
+	UINT			ProfileCnt;
+	WSC_CREDENTIAL	Profile[8];				// Support up to 8 profiles
+}	WSC_PROFILE, *PWSC_PROFILE;
+
+
+#endif // _OID_H_
+
diff --git a/drivers/staging/rt2870/rt2870.h b/drivers/staging/rt2870/rt2870.h
new file mode 100644
index 00000000000..30af4b57c3d
--- /dev/null
+++ b/drivers/staging/rt2870/rt2870.h
@@ -0,0 +1,761 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#ifndef __RT2870_H__
+#define __RT2870_H__
+
+//usb header files
+#include <linux/usb.h>
+
+/* rtmp_def.h */
+//
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+#define BULKAGGRE_ZISE          100
+#define RT28XX_DRVDATA_SET(_a)                                             usb_set_intfdata(_a, pAd);
+#define RT28XX_PUT_DEVICE                                                  usb_put_dev
+#define RTUSB_ALLOC_URB(iso)                                               usb_alloc_urb(iso, GFP_ATOMIC)
+#define RTUSB_SUBMIT_URB(pUrb)                                             usb_submit_urb(pUrb, GFP_ATOMIC)
+#define	RTUSB_URB_ALLOC_BUFFER(pUsb_Dev, BufSize, pDma_addr)               usb_buffer_alloc(pUsb_Dev, BufSize, GFP_ATOMIC, pDma_addr)
+#define	RTUSB_URB_FREE_BUFFER(pUsb_Dev, BufSize, pTransferBuf, Dma_addr)   usb_buffer_free(pUsb_Dev, BufSize, pTransferBuf, Dma_addr)
+#else
+#define BULKAGGRE_ZISE          60
+#define RT28XX_DRVDATA_SET(_a)
+#define RT28XX_PUT_DEVICE(dev_p)
+#define RTUSB_ALLOC_URB(iso)                                               usb_alloc_urb(iso)
+#define RTUSB_SUBMIT_URB(pUrb)                                             usb_submit_urb(pUrb)
+#define RTUSB_URB_ALLOC_BUFFER(pUsb_Dev, BufSize, pDma_addr)               kmalloc(BufSize, GFP_ATOMIC)
+#define	RTUSB_URB_FREE_BUFFER(pUsb_Dev, BufSize, pTransferBuf, Dma_addr)   kfree(pTransferBuf)
+#endif
+
+#define RXBULKAGGRE_ZISE        12
+#define MAX_TXBULK_LIMIT        (LOCAL_TXBUF_SIZE*(BULKAGGRE_ZISE-1))
+#define MAX_TXBULK_SIZE         (LOCAL_TXBUF_SIZE*BULKAGGRE_ZISE)
+#define MAX_RXBULK_SIZE         (LOCAL_TXBUF_SIZE*RXBULKAGGRE_ZISE)
+#define MAX_MLME_HANDLER_MEMORY 20
+#define	RETRY_LIMIT             10
+#define BUFFER_SIZE				2400	//2048
+#define	TX_RING					0xa
+#define	PRIO_RING				0xc
+
+
+// Flags for Bulkflags control for bulk out data
+//
+#define	fRTUSB_BULK_OUT_DATA_NULL				0x00000001
+#define fRTUSB_BULK_OUT_RTS						0x00000002
+#define	fRTUSB_BULK_OUT_MLME					0x00000004
+
+#define	fRTUSB_BULK_OUT_DATA_NORMAL				0x00010000
+#define	fRTUSB_BULK_OUT_DATA_NORMAL_2			0x00020000
+#define	fRTUSB_BULK_OUT_DATA_NORMAL_3			0x00040000
+#define	fRTUSB_BULK_OUT_DATA_NORMAL_4			0x00080000
+
+#define	fRTUSB_BULK_OUT_PSPOLL					0x00000020
+#define	fRTUSB_BULK_OUT_DATA_FRAG				0x00000040
+#define	fRTUSB_BULK_OUT_DATA_FRAG_2				0x00000080
+#define	fRTUSB_BULK_OUT_DATA_FRAG_3				0x00000100
+#define	fRTUSB_BULK_OUT_DATA_FRAG_4				0x00000200
+
+#ifdef RALINK_ATE
+#define	fRTUSB_BULK_OUT_DATA_ATE				0x00100000
+#endif // RALINK_ATE //
+
+#define RT2870_USB_DEVICES	\
+{	\
+	{USB_DEVICE(0x148F,0x2770)}, /* Ralink */		\
+	{USB_DEVICE(0x148F,0x2870)}, /* Ralink */		\
+	{USB_DEVICE(0x148F,0x3070)}, /* Ralink */		\
+	{USB_DEVICE(0x0B05,0x1731)}, /* Asus */			\
+	{USB_DEVICE(0x0B05,0x1732)}, /* Asus */			\
+	{USB_DEVICE(0x0B05,0x1742)}, /* Asus */			\
+	{USB_DEVICE(0x0DF6,0x0017)}, /* Sitecom */		\
+	{USB_DEVICE(0x0DF6,0x002B)}, /* Sitecom */		\
+	{USB_DEVICE(0x0DF6,0x002C)}, /* Sitecom */		\
+	{USB_DEVICE(0x0DF6,0x002D)}, /* Sitecom */		\
+	{USB_DEVICE(0x14B2,0x3C06)}, /* Conceptronic */		\
+	{USB_DEVICE(0x14B2,0x3C28)}, /* Conceptronic */		\
+	{USB_DEVICE(0x2019,0xED06)}, /* Planex Communications, Inc. */		\
+	{USB_DEVICE(0x2019,0xAB25)}, /* Planex Communications, Inc. RT3070 */		\
+	{USB_DEVICE(0x07D1,0x3C09)}, /* D-Link */		\
+	{USB_DEVICE(0x07D1,0x3C11)}, /* D-Link */		\
+	{USB_DEVICE(0x14B2,0x3C07)}, /* AL */			\
+	{USB_DEVICE(0x14B2,0x3C12)}, /* AL */           \
+	{USB_DEVICE(0x050D,0x8053)}, /* Belkin */		\
+	{USB_DEVICE(0x14B2,0x3C23)}, /* Airlink */		\
+	{USB_DEVICE(0x14B2,0x3C27)}, /* Airlink */		\
+	{USB_DEVICE(0x07AA,0x002F)}, /* Corega */		\
+	{USB_DEVICE(0x07AA,0x003C)}, /* Corega */		\
+	{USB_DEVICE(0x07AA,0x003F)}, /* Corega */		\
+	{USB_DEVICE(0x18C5,0x0012)}, /* Corega */		\
+	{USB_DEVICE(0x1044,0x800B)}, /* Gigabyte */		\
+	{USB_DEVICE(0x15A9,0x0006)}, /* Sparklan */		\
+	{USB_DEVICE(0x083A,0xB522)}, /* SMC */			\
+	{USB_DEVICE(0x083A,0xA618)}, /* SMC */			\
+	{USB_DEVICE(0x083A,0x7522)}, /* Arcadyan */		\
+	{USB_DEVICE(0x0CDE,0x0022)}, /* ZCOM */			\
+	{USB_DEVICE(0x0586,0x3416)}, /* Zyxel */		\
+	{USB_DEVICE(0x0CDE,0x0025)}, /* Zyxel */		\
+	{USB_DEVICE(0x1740,0x9701)}, /* EnGenius */		\
+	{USB_DEVICE(0x1740,0x9702)}, /* EnGenius */		\
+	{USB_DEVICE(0x0471,0x200f)}, /* Philips */		\
+	{USB_DEVICE(0x14B2,0x3C25)}, /* Draytek */		\
+	{USB_DEVICE(0x13D3,0x3247)}, /* AzureWave */	\
+	{USB_DEVICE(0x083A,0x6618)}, /* Accton */		\
+	{USB_DEVICE(0x15c5,0x0008)}, /* Amit */			\
+	{USB_DEVICE(0x0E66,0x0001)}, /* Hawking */		\
+	{USB_DEVICE(0x0E66,0x0003)}, /* Hawking */		\
+	{USB_DEVICE(0x129B,0x1828)}, /* Siemens */		\
+	{USB_DEVICE(0x157E,0x300E)},	/* U-Media */	\
+	{USB_DEVICE(0x050d,0x805c)},					\
+	{USB_DEVICE(0x1482,0x3C09)}, /* Abocom*/		\
+	{USB_DEVICE(0x14B2,0x3C09)}, /* Alpha */		\
+	{USB_DEVICE(0x04E8,0x2018)}, /* samsung */  	\
+	{USB_DEVICE(0x07B8,0x3070)}, /* AboCom */		\
+	{USB_DEVICE(0x07B8,0x3071)}, /* AboCom */		\
+	{USB_DEVICE(0x07B8,0x2870)}, /* AboCom */		\
+	{USB_DEVICE(0x07B8,0x2770)}, /* AboCom */		\
+	{USB_DEVICE(0x7392,0x7711)}, /* Edimax */		\
+	{USB_DEVICE(0x5A57,0x0280)}, /* Zinwell */		\
+	{USB_DEVICE(0x5A57,0x0282)}, /* Zinwell */		\
+	{USB_DEVICE(0x0789,0x0162)}, /* Logitec */		\
+	{USB_DEVICE(0x0789,0x0163)}, /* Logitec */		\
+	{USB_DEVICE(0x0789,0x0164)}, /* Logitec */		\
+	{ }/* Terminating entry */                      \
+}
+
+#define	FREE_HTTX_RING(_p, _b, _t)			\
+{										\
+	if ((_t)->ENextBulkOutPosition == (_t)->CurWritePosition)				\
+	{																	\
+		(_t)->bRingEmpty = TRUE;			\
+	}																	\
+	/*NdisInterlockedDecrement(&(_p)->TxCount); */\
+}
+
+//
+// RXINFO appends at the end of each rx packet.
+//
+#ifdef RT_BIG_ENDIAN
+typedef	struct	PACKED _RXINFO_STRUC {
+	UINT32		PlcpSignal:12;
+	UINT32		LastAMSDU:1;
+	UINT32		CipherAlg:1;
+	UINT32		PlcpRssil:1;
+	UINT32		Decrypted:1;
+	UINT32		AMPDU:1;		// To be moved
+	UINT32		L2PAD:1;
+	UINT32		RSSI:1;
+	UINT32		HTC:1;
+	UINT32		AMSDU:1;		// rx with 802.3 header, not 802.11 header.
+	UINT32		CipherErr:2;        // 0: decryption okay, 1:ICV error, 2:MIC error, 3:KEY not valid
+	UINT32		Crc:1;              // 1: CRC error
+	UINT32		MyBss:1;  	// 1: this frame belongs to the same BSSID
+	UINT32		Bcast:1;            // 1: this is a broadcast frame
+	UINT32		Mcast:1;            // 1: this is a multicast frame
+	UINT32		U2M:1;              // 1: this RX frame is unicast to me
+	UINT32		FRAG:1;
+	UINT32		NULLDATA:1;
+	UINT32		DATA:1;
+	UINT32		BA:1;
+}	RXINFO_STRUC, *PRXINFO_STRUC, RT28XX_RXD_STRUC, *PRT28XX_RXD_STRUC;
+#else
+typedef	struct	PACKED _RXINFO_STRUC {
+	UINT32		BA:1;
+	UINT32		DATA:1;
+	UINT32		NULLDATA:1;
+	UINT32		FRAG:1;
+	UINT32		U2M:1;              // 1: this RX frame is unicast to me
+	UINT32		Mcast:1;            // 1: this is a multicast frame
+	UINT32		Bcast:1;            // 1: this is a broadcast frame
+	UINT32		MyBss:1;  	// 1: this frame belongs to the same BSSID
+	UINT32		Crc:1;              // 1: CRC error
+	UINT32		CipherErr:2;        // 0: decryption okay, 1:ICV error, 2:MIC error, 3:KEY not valid
+	UINT32		AMSDU:1;		// rx with 802.3 header, not 802.11 header.
+	UINT32		HTC:1;
+	UINT32		RSSI:1;
+	UINT32		L2PAD:1;
+	UINT32		AMPDU:1;		// To be moved
+	UINT32		Decrypted:1;
+	UINT32		PlcpRssil:1;
+	UINT32		CipherAlg:1;
+	UINT32		LastAMSDU:1;
+	UINT32		PlcpSignal:12;
+}	RXINFO_STRUC, *PRXINFO_STRUC, RT28XX_RXD_STRUC, *PRT28XX_RXD_STRUC;
+#endif
+
+
+//
+// TXINFO
+//
+#ifdef RT_BIG_ENDIAN
+typedef	struct	_TXINFO_STRUC {
+	// Word	0
+	UINT32		USBDMATxburst:1;//used ONLY in USB bulk Aggre. Force USB DMA transmit frame from current selected endpoint
+	UINT32		USBDMANextVLD:1;	//used ONLY in USB bulk Aggregation, NextValid
+	UINT32		rsv2:2;  // Software use.
+	UINT32		SwUseLastRound:1; // Software use.
+	UINT32		QSEL:2;	// select on-chip FIFO ID for 2nd-stage output scheduler.0:MGMT, 1:HCCA 2:EDCA
+	UINT32		WIV:1;	// Wireless Info Valid. 1 if Driver already fill WI,  o if DMA needs to copy WI to correctposition
+	UINT32		rsv:8;
+	UINT32		USBDMATxPktLen:16;	//used ONLY in USB bulk Aggregation,  Total byte counts of all sub-frame.
+}	TXINFO_STRUC, *PTXINFO_STRUC;
+#else
+typedef	struct	_TXINFO_STRUC {
+	// Word	0
+	UINT32		USBDMATxPktLen:16;	//used ONLY in USB bulk Aggregation,  Total byte counts of all sub-frame.
+	UINT32		rsv:8;
+	UINT32		WIV:1;	// Wireless Info Valid. 1 if Driver already fill WI,  o if DMA needs to copy WI to correctposition
+	UINT32		QSEL:2;	// select on-chip FIFO ID for 2nd-stage output scheduler.0:MGMT, 1:HCCA 2:EDCA
+	UINT32		SwUseLastRound:1; // Software use.
+	UINT32		rsv2:2;  // Software use.
+	UINT32		USBDMANextVLD:1;	//used ONLY in USB bulk Aggregation, NextValid
+	UINT32		USBDMATxburst:1;//used ONLY in USB bulk Aggre. Force USB DMA transmit frame from current selected endpoint
+}	TXINFO_STRUC, *PTXINFO_STRUC;
+#endif
+
+#define TXINFO_SIZE				4
+#define RXINFO_SIZE				4
+#define TXPADDING_SIZE			11
+
+//
+// Management ring buffer format
+//
+typedef	struct	_MGMT_STRUC	{
+	BOOLEAN		Valid;
+	PUCHAR		pBuffer;
+	ULONG		Length;
+}	MGMT_STRUC, *PMGMT_STRUC;
+
+
+/* ----------------- EEPROM Related MACRO ----------------- */
+#define RT28xx_EEPROM_READ16(pAd, offset, var)					\
+	do {														\
+		RTUSBReadEEPROM(pAd, offset, (PUCHAR)&(var), 2);		\
+		var = le2cpu16(var);									\
+	}while(0)
+
+#define RT28xx_EEPROM_WRITE16(pAd, offset, var)					\
+	do{															\
+		USHORT _tmpVar;											\
+		_tmpVar = cpu2le16(var);								\
+		RTUSBWriteEEPROM(pAd, offset, (PUCHAR)&(_tmpVar), 2);	\
+	}while(0)
+
+/* ----------------- TASK/THREAD Related MACRO ----------------- */
+#define RT28XX_TASK_THREAD_INIT(pAd, Status)		\
+	Status = CreateThreads(net_dev);
+
+
+/* ----------------- Frimware Related MACRO ----------------- */
+#if 0
+#define RT28XX_FIRMUD_INIT(pAd)		\
+	{	UINT32	MacReg;				\
+		RTUSBReadMACRegister(pAd, MAC_CSR0, &MacReg); }
+
+#define RT28XX_FIRMUD_END(pAd)	\
+	RTUSBWriteMACRegister(pAd, 0x7014, 0xffffffff);	\
+	RTUSBWriteMACRegister(pAd, 0x701c, 0xffffffff);	\
+	RTUSBFirmwareRun(pAd);
+#else
+#define RT28XX_WRITE_FIRMWARE(_pAd, _pFwImage, _FwLen)		\
+	RTUSBFirmwareWrite(_pAd, _pFwImage, _FwLen)
+#endif
+
+/* ----------------- TX Related MACRO ----------------- */
+#define RT28XX_START_DEQUEUE(pAd, QueIdx, irqFlags)				\
+			{													\
+				RTMP_IRQ_LOCK(&pAd->DeQueueLock[QueIdx], irqFlags);		\
+				if (pAd->DeQueueRunning[QueIdx])						\
+				{														\
+					RTMP_IRQ_UNLOCK(&pAd->DeQueueLock[QueIdx], irqFlags);\
+					printk("DeQueueRunning[%d]= TRUE!\n", QueIdx);		\
+					continue;											\
+				}														\
+				else													\
+				{														\
+					pAd->DeQueueRunning[QueIdx] = TRUE;					\
+					RTMP_IRQ_UNLOCK(&pAd->DeQueueLock[QueIdx], irqFlags);\
+				}														\
+			}
+#define RT28XX_STOP_DEQUEUE(pAd, QueIdx, irqFlags)						\
+			do{															\
+				RTMP_IRQ_LOCK(&pAd->DeQueueLock[QueIdx], irqFlags);		\
+				pAd->DeQueueRunning[QueIdx] = FALSE;					\
+				RTMP_IRQ_UNLOCK(&pAd->DeQueueLock[QueIdx], irqFlags);	\
+			}while(0)
+
+
+#define	RT28XX_HAS_ENOUGH_FREE_DESC(pAd, pTxBlk, freeNum, pPacket) \
+		(RTUSBFreeDescriptorRequest(pAd, pTxBlk->QueIdx, (pTxBlk->TotalFrameLen + GET_OS_PKT_LEN(pPacket))) == NDIS_STATUS_SUCCESS)
+
+#define RT28XX_RELEASE_DESC_RESOURCE(pAd, QueIdx)			\
+		do{}while(0)
+
+#define NEED_QUEUE_BACK_FOR_AGG(_pAd, _QueIdx, _freeNum, _TxFrameType) 		\
+		((_TxFrameType == TX_RALINK_FRAME) && (RTUSBNeedQueueBackForAgg(_pAd, _QueIdx)))
+
+
+
+#define fRTMP_ADAPTER_NEED_STOP_TX		\
+		(fRTMP_ADAPTER_NIC_NOT_EXIST | fRTMP_ADAPTER_HALT_IN_PROGRESS |	\
+		 fRTMP_ADAPTER_RESET_IN_PROGRESS | fRTMP_ADAPTER_BULKOUT_RESET | \
+		 fRTMP_ADAPTER_RADIO_OFF | fRTMP_ADAPTER_REMOVE_IN_PROGRESS)
+
+
+#define HAL_WriteSubTxResource(pAd, pTxBlk, bIsLast, pFreeNumber)	\
+			RtmpUSB_WriteSubTxResource(pAd, pTxBlk, bIsLast, pFreeNumber)
+
+#define HAL_WriteTxResource(pAd, pTxBlk,bIsLast, pFreeNumber)	\
+			RtmpUSB_WriteSingleTxResource(pAd, pTxBlk,bIsLast, pFreeNumber)
+
+#define HAL_WriteFragTxResource(pAd, pTxBlk, fragNum, pFreeNumber) \
+			RtmpUSB_WriteFragTxResource(pAd, pTxBlk, fragNum, pFreeNumber)
+
+#define HAL_WriteMultiTxResource(pAd, pTxBlk,frameNum, pFreeNumber)	\
+			RtmpUSB_WriteMultiTxResource(pAd, pTxBlk,frameNum, pFreeNumber)
+
+#define HAL_FinalWriteTxResource(pAd, pTxBlk, totalMPDUSize, TxIdx)	\
+			RtmpUSB_FinalWriteTxResource(pAd, pTxBlk, totalMPDUSize, TxIdx)
+
+#define HAL_LastTxIdx(pAd, QueIdx,TxIdx) \
+			/*RtmpUSBDataLastTxIdx(pAd, QueIdx,TxIdx)*/
+
+#define HAL_KickOutTx(pAd, pTxBlk, QueIdx)	\
+			RtmpUSBDataKickOut(pAd, pTxBlk, QueIdx)
+
+
+#define HAL_KickOutMgmtTx(pAd, QueIdx, pPacket, pSrcBufVA, SrcBufLen)	\
+			RtmpUSBMgmtKickOut(pAd, QueIdx, pPacket, pSrcBufVA, SrcBufLen)
+
+#define HAL_KickOutNullFrameTx(_pAd, _QueIdx, _pNullFrame, _frameLen)	\
+			RtmpUSBNullFrameKickOut(_pAd, _QueIdx, _pNullFrame, _frameLen)
+
+#define RTMP_PKT_TAIL_PADDING 	11 // 3(max 4 byte padding) + 4 (last packet padding) + 4 (MaxBulkOutsize align padding)
+
+extern UCHAR EpToQueue[6];
+
+
+#ifdef RT2870
+#define GET_TXRING_FREENO(_pAd, _QueIdx) 	(_QueIdx) //(_pAd->TxRing[_QueIdx].TxSwFreeIdx)
+#define GET_MGMTRING_FREENO(_pAd) 			(_pAd->MgmtRing.TxSwFreeIdx)
+#endif // RT2870 //
+
+
+/* ----------------- RX Related MACRO ----------------- */
+//#define RT28XX_RX_ERROR_CHECK				RTMPCheckRxWI
+
+#if 0
+#define RT28XX_RCV_INIT(pAd)					\
+	pAd->TransferBufferLength = 0;				\
+	pAd->ReadPosition = 0;						\
+	pAd->pCurrRxContext = NULL;
+#endif
+
+#define RT28XX_RV_ALL_BUF_END(bBulkReceive)		\
+	/* We return STATUS_MORE_PROCESSING_REQUIRED so that the completion */	\
+	/* routine (IofCompleteRequest) will stop working on the irp. */		\
+	if (bBulkReceive == TRUE)	RTUSBBulkReceive(pAd);
+
+
+/* ----------------- ASIC Related MACRO ----------------- */
+#if 0
+#define RT28XX_DMA_WRITE_INIT(GloCfg)			\
+	{	GloCfg.field.EnTXWriteBackDDONE = 1;	\
+		GloCfg.field.EnableRxDMA = 1;			\
+		GloCfg.field.EnableTxDMA = 1; }
+
+#define RT28XX_DMA_POST_WRITE(_pAd)				\
+	do{	USB_DMA_CFG_STRUC	UsbCfg;				\
+		UsbCfg.word = 0;						\
+		/* for last packet, PBF might use more than limited, so minus 2 to prevent from error */ \
+		UsbCfg.field.RxBulkAggLmt = (MAX_RXBULK_SIZE /1024)-3;	\
+		UsbCfg.field.phyclear = 0;								\
+		/* usb version is 1.1,do not use bulk in aggregation */	\
+		if (_pAd->BulkInMaxPacketSize == 512)					\
+			UsbCfg.field.RxBulkAggEn = 1;						\
+		UsbCfg.field.RxBulkEn = 1;								\
+		UsbCfg.field.TxBulkEn = 1;								\
+		UsbCfg.field.RxBulkAggTOut = 0x80; /* 2006-10-18 */		\
+		RTUSBWriteMACRegister(_pAd, USB_DMA_CFG, UsbCfg.word); 	\
+	}while(0)
+#endif
+
+// reset MAC of a station entry to 0xFFFFFFFFFFFF
+#define RT28XX_STA_ENTRY_MAC_RESET(pAd, Wcid)					\
+	{	RT_SET_ASIC_WCID	SetAsicWcid;						\
+		SetAsicWcid.WCID = Wcid;								\
+		SetAsicWcid.SetTid = 0xffffffff;						\
+		SetAsicWcid.DeleteTid = 0xffffffff;						\
+		RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_SET_ASIC_WCID, 	\
+				&SetAsicWcid, sizeof(RT_SET_ASIC_WCID));	}
+
+// add this entry into ASIC RX WCID search table
+#define RT28XX_STA_ENTRY_ADD(pAd, pEntry)							\
+	RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_SET_CLIENT_MAC_ENTRY, 	\
+							pEntry, sizeof(MAC_TABLE_ENTRY));
+
+// remove Pair-wise key material from ASIC
+// yet implement
+#define RT28XX_STA_ENTRY_KEY_DEL(pAd, BssIdx, Wcid)
+
+// add Client security information into ASIC WCID table and IVEIV table
+#define RT28XX_STA_SECURITY_INFO_ADD(pAd, apidx, KeyID, pEntry)						\
+	{	RT28XX_STA_ENTRY_MAC_RESET(pAd, pEntry->Aid);								\
+		if (pEntry->Aid >= 1) {														\
+			RT_SET_ASIC_WCID_ATTRI	SetAsicWcidAttri;								\
+			SetAsicWcidAttri.WCID = pEntry->Aid;									\
+			if ((pEntry->AuthMode <= Ndis802_11AuthModeAutoSwitch) &&				\
+				(pEntry->WepStatus == Ndis802_11Encryption1Enabled))				\
+			{																		\
+				SetAsicWcidAttri.Cipher = pAd->SharedKey[apidx][KeyID].CipherAlg;	\
+			}																		\
+			else if (pEntry->AuthMode == Ndis802_11AuthModeWPANone)					\
+			{																		\
+				SetAsicWcidAttri.Cipher = pAd->SharedKey[apidx][KeyID].CipherAlg;	\
+			}																		\
+			else SetAsicWcidAttri.Cipher = 0;										\
+            DBGPRINT(RT_DEBUG_TRACE, ("aid cipher = %ld\n",SetAsicWcidAttri.Cipher));       \
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_SET_ASIC_WCID_CIPHER, 			\
+							&SetAsicWcidAttri, sizeof(RT_SET_ASIC_WCID_ATTRI)); } }
+
+// Insert the BA bitmap to ASIC for the Wcid entry
+#define RT28XX_ADD_BA_SESSION_TO_ASIC(_pAd, _Aid, _TID)					\
+		do{																\
+			RT_SET_ASIC_WCID	SetAsicWcid;							\
+			SetAsicWcid.WCID = (_Aid);									\
+			SetAsicWcid.SetTid = (0x10000<<(_TID));						\
+			SetAsicWcid.DeleteTid = 0xffffffff;							\
+			RTUSBEnqueueInternalCmd((_pAd), CMDTHREAD_SET_ASIC_WCID, &SetAsicWcid, sizeof(RT_SET_ASIC_WCID));	\
+		}while(0)
+
+// Remove the BA bitmap from ASIC for the Wcid entry
+#define RT28XX_DEL_BA_SESSION_FROM_ASIC(_pAd, _Wcid, _TID)				\
+		do{																\
+			RT_SET_ASIC_WCID	SetAsicWcid;							\
+			SetAsicWcid.WCID = (_Wcid);									\
+			SetAsicWcid.SetTid = (0xffffffff);							\
+			SetAsicWcid.DeleteTid = (0x10000<<(_TID) );					\
+			RTUSBEnqueueInternalCmd((_pAd), CMDTHREAD_SET_ASIC_WCID, &SetAsicWcid, sizeof(RT_SET_ASIC_WCID));	\
+		}while(0)
+
+
+/* ----------------- PCI/USB Related MACRO ----------------- */
+#define RT28XX_HANDLE_DEV_ASSIGN(handle, dev_p)			\
+	((POS_COOKIE)handle)->pUsb_Dev = dev_p;
+
+// no use
+#define RT28XX_UNMAP()
+#define RT28XX_IRQ_REQUEST(net_dev)
+#define RT28XX_IRQ_RELEASE(net_dev)
+#define RT28XX_IRQ_INIT(pAd)
+#define RT28XX_IRQ_ENABLE(pAd)
+
+
+/* ----------------- MLME Related MACRO ----------------- */
+#define RT28XX_MLME_HANDLER(pAd)			RTUSBMlmeUp(pAd)
+
+#define RT28XX_MLME_PRE_SANITY_CHECK(pAd)								\
+	{	if ((pAd->CommonCfg.bHardwareRadio == TRUE) && 					\
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)) &&		\
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS))) {	\
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_CHECK_GPIO, NULL, 0); } }
+
+#define RT28XX_MLME_STA_QUICK_RSP_WAKE_UP(pAd)	\
+	{	RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_QKERIODIC_EXECUT, NULL, 0);	\
+		RTUSBMlmeUp(pAd); }
+
+#define RT28XX_MLME_RESET_STATE_MACHINE(pAd)	\
+		        MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_RESET_CONF, 0, NULL);	\
+		        RTUSBMlmeUp(pAd);
+
+#define RT28XX_HANDLE_COUNTER_MEASURE(_pAd, _pEntry)		\
+	{	RTUSBEnqueueInternalCmd(_pAd, CMDTHREAD_802_11_COUNTER_MEASURE, _pEntry, sizeof(MAC_TABLE_ENTRY));	\
+		RTUSBMlmeUp(_pAd);									\
+	}
+
+
+/* ----------------- Power Save Related MACRO ----------------- */
+#define RT28XX_PS_POLL_ENQUEUE(pAd)						\
+	{	RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_PSPOLL);	\
+		RTUSBKickBulkOut(pAd); }
+
+#define RT28xx_CHIP_NAME            "RT2870"
+#define USB_CYC_CFG                 0x02a4
+#define STATUS_SUCCESS				0x00
+#define STATUS_UNSUCCESSFUL 		0x01
+#define NT_SUCCESS(status)			(((status) > 0) ? (1):(0))
+#define InterlockedIncrement 	 	atomic_inc
+#define NdisInterlockedIncrement 	atomic_inc
+#define InterlockedDecrement		atomic_dec
+#define NdisInterlockedDecrement 	atomic_dec
+#define InterlockedExchange			atomic_set
+//#define NdisMSendComplete			RTMP_SendComplete
+#define NdisMCancelTimer			RTMPCancelTimer
+#define NdisAllocMemory(_ptr, _size, _flag)	\
+									do{_ptr = kmalloc((_size),(_flag));}while(0)
+#define NdisFreeMemory(a, b, c) 	kfree((a))
+#define NdisMSleep					RTMPusecDelay		/* unit: microsecond */
+
+
+#define USBD_TRANSFER_DIRECTION_OUT		0
+#define USBD_TRANSFER_DIRECTION_IN		0
+#define USBD_SHORT_TRANSFER_OK			0
+#define PURB			purbb_t
+
+#define RTUSB_FREE_URB(pUrb)	usb_free_urb(pUrb)
+
+//#undef MlmeAllocateMemory
+//#undef MlmeFreeMemory
+
+typedef int				NTSTATUS;
+typedef struct usb_device	* PUSB_DEV;
+
+/* MACRO for linux usb */
+typedef struct urb *purbb_t;
+typedef struct usb_ctrlrequest devctrlrequest;
+#define PIRP		PVOID
+#define PMDL		PVOID
+#define NDIS_OID	UINT
+#ifndef USB_ST_NOERROR
+#define USB_ST_NOERROR     0
+#endif
+
+// vendor-specific control operations
+#define CONTROL_TIMEOUT_JIFFIES ( (100 * HZ) / 1000)
+#define UNLINK_TIMEOUT_MS		3
+
+/* unlink urb	*/
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,7)
+#define RTUSB_UNLINK_URB(pUrb)		usb_kill_urb(pUrb)
+#else
+#define RTUSB_UNLINK_URB(pUrb)		usb_unlink_urb(pUrb)
+#endif
+
+// Prototypes of completion funuc.
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
+#define RTUSBBulkOutDataPacketComplete(purb, pt_regs)    RTUSBBulkOutDataPacketComplete(purb)
+#define RTUSBBulkOutMLMEPacketComplete(pUrb, pt_regs)    RTUSBBulkOutMLMEPacketComplete(pUrb)
+#define RTUSBBulkOutNullFrameComplete(pUrb, pt_regs)     RTUSBBulkOutNullFrameComplete(pUrb)
+#define RTUSBBulkOutRTSFrameComplete(pUrb, pt_regs)      RTUSBBulkOutRTSFrameComplete(pUrb)
+#define RTUSBBulkOutPsPollComplete(pUrb, pt_regs)        RTUSBBulkOutPsPollComplete(pUrb)
+#define RTUSBBulkRxComplete(pUrb, pt_regs)               RTUSBBulkRxComplete(pUrb)
+#endif
+
+
+VOID RTUSBBulkOutDataPacketComplete(purbb_t purb, struct pt_regs *pt_regs);
+VOID RTUSBBulkOutMLMEPacketComplete(purbb_t pUrb, struct pt_regs *pt_regs);
+VOID RTUSBBulkOutNullFrameComplete(purbb_t pUrb, struct pt_regs *pt_regs);
+VOID RTUSBBulkOutRTSFrameComplete(purbb_t pUrb, struct pt_regs *pt_regs);
+VOID RTUSBBulkOutPsPollComplete(purbb_t pUrb, struct pt_regs *pt_regs);
+VOID RTUSBBulkRxComplete(purbb_t pUrb, struct pt_regs *pt_regs);
+
+
+#define RTUSBMlmeUp(pAd)	        \
+{								    \
+	POS_COOKIE pObj = (POS_COOKIE) pAd->OS_Cookie;	\
+	CHECK_PID_LEGALITY(pObj->MLMEThr_pid)		    \
+        up(&(pAd->mlme_semaphore)); \
+}
+
+#define RTUSBCMDUp(pAd)	                \
+{									    \
+	POS_COOKIE pObj = (POS_COOKIE) pAd->OS_Cookie;	\
+	CHECK_PID_LEGALITY(pObj->RTUSBCmdThr_pid)	    \
+	    up(&(pAd->RTUSBCmd_semaphore)); \
+}
+
+
+static inline NDIS_STATUS RTMPAllocateMemory(
+	OUT PVOID *ptr,
+	IN size_t size)
+{
+	*ptr = kmalloc(size, GFP_ATOMIC);
+	if(*ptr)
+		return NDIS_STATUS_SUCCESS;
+	else
+		return NDIS_STATUS_RESOURCES;
+}
+
+/* rtmp.h */
+#define	BEACON_RING_SIZE                2
+#define DEVICE_VENDOR_REQUEST_OUT       0x40
+#define DEVICE_VENDOR_REQUEST_IN        0xc0
+#define INTERFACE_VENDOR_REQUEST_OUT    0x41
+#define INTERFACE_VENDOR_REQUEST_IN     0xc1
+#define MGMTPIPEIDX						0	// EP6 is highest priority
+
+#define BULKOUT_MGMT_RESET_FLAG				0x80
+
+#define RTUSB_SET_BULK_FLAG(_M, _F)				((_M)->BulkFlags |= (_F))
+#define RTUSB_CLEAR_BULK_FLAG(_M, _F)			((_M)->BulkFlags &= ~(_F))
+#define RTUSB_TEST_BULK_FLAG(_M, _F)			(((_M)->BulkFlags & (_F)) != 0)
+
+#define EnqueueCmd(cmdq, cmdqelmt)		\
+{										\
+	if (cmdq->size == 0)				\
+		cmdq->head = cmdqelmt;			\
+	else								\
+		cmdq->tail->next = cmdqelmt;	\
+	cmdq->tail = cmdqelmt;				\
+	cmdqelmt->next = NULL;				\
+	cmdq->size++;						\
+}
+
+typedef struct   _RT_SET_ASIC_WCID {
+	ULONG WCID;          // mechanism for rekeying: 0:disable, 1: time-based, 2: packet-based
+	ULONG SetTid;        // time-based: seconds, packet-based: kilo-packets
+	ULONG DeleteTid;        // time-based: seconds, packet-based: kilo-packets
+	UCHAR Addr[MAC_ADDR_LEN];	// avoid in interrupt when write key
+} RT_SET_ASIC_WCID,*PRT_SET_ASIC_WCID;
+
+typedef struct   _RT_SET_ASIC_WCID_ATTRI {
+	ULONG	WCID;          // mechanism for rekeying: 0:disable, 1: time-based, 2: packet-based
+	ULONG	Cipher;        // ASIC Cipher definition
+	UCHAR	Addr[ETH_LENGTH_OF_ADDRESS];
+} RT_SET_ASIC_WCID_ATTRI,*PRT_SET_ASIC_WCID_ATTRI;
+
+typedef struct _MLME_MEMORY_STRUCT {
+	PVOID                           AllocVa;    //Pointer to the base virtual address of the allocated memory
+	struct _MLME_MEMORY_STRUCT      *Next;      //Pointer to the next virtual address of the allocated memory
+}   MLME_MEMORY_STRUCT, *PMLME_MEMORY_STRUCT;
+
+typedef struct  _MLME_MEMORY_HANDLER {
+	BOOLEAN                 MemRunning;         //The flag of the Mlme memory handler's status
+	UINT                    MemoryCount;        //Total nonpaged system-space memory not size
+	UINT                    InUseCount;         //Nonpaged system-space memory in used counts
+	UINT                    UnUseCount;         //Nonpaged system-space memory available counts
+	INT                    PendingCount;       //Nonpaged system-space memory for free counts
+	PMLME_MEMORY_STRUCT     pInUseHead;         //Pointer to the first nonpaed memory not used
+	PMLME_MEMORY_STRUCT     pInUseTail;         //Pointer to the last nonpaged memory not used
+	PMLME_MEMORY_STRUCT     pUnUseHead;         //Pointer to the first nonpaged memory in used
+	PMLME_MEMORY_STRUCT     pUnUseTail;         //Pointer to the last nonpaged memory in used
+	PULONG                  MemFreePending[MAX_MLME_HANDLER_MEMORY];   //an array to keep pending free-memory's pointer (32bits)
+}   MLME_MEMORY_HANDLER, *PMLME_MEMORY_HANDLER;
+
+typedef	struct _CmdQElmt	{
+	UINT				command;
+	PVOID				buffer;
+	ULONG				bufferlength;
+	BOOLEAN				CmdFromNdis;
+	BOOLEAN				SetOperation;
+	struct _CmdQElmt	*next;
+}	CmdQElmt, *PCmdQElmt;
+
+typedef	struct	_CmdQ	{
+	UINT		size;
+	CmdQElmt	*head;
+	CmdQElmt	*tail;
+	UINT32		CmdQState;
+}CmdQ, *PCmdQ;
+
+//
+// For WPA SUPPLICANT: WIRELESS EXT support wireless events: v14 or newer
+//
+#if WIRELESS_EXT >= 14
+//#define WPA_SUPPLICANT_SUPPORT  1
+#endif
+
+/* oid.h */
+// Cipher suite type for mixed mode group cipher, P802.11i-2004
+typedef enum _RT_802_11_CIPHER_SUITE_TYPE {
+	Cipher_Type_NONE,
+	Cipher_Type_WEP40,
+	Cipher_Type_TKIP,
+	Cipher_Type_RSVD,
+	Cipher_Type_CCMP,
+	Cipher_Type_WEP104
+} RT_802_11_CIPHER_SUITE_TYPE, *PRT_802_11_CIPHER_SUITE_TYPE;
+
+//CMDTHREAD_MULTI_READ_MAC
+//CMDTHREAD_MULTI_WRITE_MAC
+//CMDTHREAD_VENDOR_EEPROM_READ
+//CMDTHREAD_VENDOR_EEPROM_WRITE
+typedef	struct	_CMDHandler_TLV	{
+	USHORT		Offset;
+	USHORT		Length;
+	UCHAR		DataFirst;
+}	CMDHandler_TLV, *PCMDHandler_TLV;
+
+// New for MeetingHouse Api support
+#define CMDTHREAD_VENDOR_RESET                      0x0D730101	// cmd
+#define CMDTHREAD_VENDOR_UNPLUG                     0x0D730102	// cmd
+#define CMDTHREAD_VENDOR_SWITCH_FUNCTION            0x0D730103	// cmd
+#define CMDTHREAD_MULTI_WRITE_MAC                   0x0D730107	// cmd
+#define CMDTHREAD_MULTI_READ_MAC                    0x0D730108	// cmd
+#define CMDTHREAD_VENDOR_EEPROM_WRITE               0x0D73010A	// cmd
+#define CMDTHREAD_VENDOR_EEPROM_READ                0x0D73010B	// cmd
+#define CMDTHREAD_VENDOR_ENTER_TESTMODE             0x0D73010C	// cmd
+#define CMDTHREAD_VENDOR_EXIT_TESTMODE              0x0D73010D	// cmd
+#define CMDTHREAD_VENDOR_WRITE_BBP                  0x0D730119	// cmd
+#define CMDTHREAD_VENDOR_READ_BBP                   0x0D730118	// cmd
+#define CMDTHREAD_VENDOR_WRITE_RF                   0x0D73011A	// cmd
+#define CMDTHREAD_VENDOR_FLIP_IQ                    0x0D73011D	// cmd
+#define CMDTHREAD_RESET_BULK_OUT                    0x0D730210	// cmd
+#define CMDTHREAD_RESET_BULK_IN                     0x0D730211	// cmd
+#define CMDTHREAD_SET_PSM_BIT_SAVE                  0x0D730212	// cmd
+#define CMDTHREAD_SET_RADIO                         0x0D730214	// cmd
+#define CMDTHREAD_UPDATE_TX_RATE                    0x0D730216	// cmd
+#define CMDTHREAD_802_11_ADD_KEY_WEP                0x0D730218	// cmd
+#define CMDTHREAD_RESET_FROM_ERROR                  0x0D73021A	// cmd
+#define CMDTHREAD_LINK_DOWN                         0x0D73021B	// cmd
+#define CMDTHREAD_RESET_FROM_NDIS                   0x0D73021C	// cmd
+#define CMDTHREAD_CHECK_GPIO                        0x0D730215	// cmd
+#define CMDTHREAD_FORCE_WAKE_UP                     0x0D730222	// cmd
+#define CMDTHREAD_SET_BW                            0x0D730225	// cmd
+#define CMDTHREAD_SET_ASIC_WCID                     0x0D730226	// cmd
+#define CMDTHREAD_SET_ASIC_WCID_CIPHER              0x0D730227	// cmd
+#define CMDTHREAD_QKERIODIC_EXECUT                  0x0D73023D	// cmd
+#define RT_CMD_SET_KEY_TABLE                        0x0D730228  // cmd
+#define RT_CMD_SET_RX_WCID_TABLE                    0x0D730229  // cmd
+#define CMDTHREAD_SET_CLIENT_MAC_ENTRY              0x0D73023E	// cmd
+#define CMDTHREAD_802_11_QUERY_HARDWARE_REGISTER    0x0D710105	// cmd
+#define CMDTHREAD_802_11_SET_PHY_MODE               0x0D79010C	// cmd
+#define CMDTHREAD_802_11_SET_STA_CONFIG             0x0D790111	// cmd
+#define CMDTHREAD_802_11_SET_PREAMBLE               0x0D790101	// cmd
+#define CMDTHREAD_802_11_COUNTER_MEASURE			0x0D790102	// cmd
+
+
+#define WPA1AKMBIT	    0x01
+#define WPA2AKMBIT	    0x02
+#define WPA1PSKAKMBIT   0x04
+#define WPA2PSKAKMBIT   0x08
+#define TKIPBIT         0x01
+#define CCMPBIT         0x02
+
+
+#define RT28XX_STA_FORCE_WAKEUP(pAd, bFromTx) \
+    RT28xxUsbStaAsicForceWakeup(pAd, bFromTx);
+
+#define RT28XX_STA_SLEEP_THEN_AUTO_WAKEUP(pAd, TbttNumToNextWakeUp) \
+    RT28xxUsbStaAsicSleepThenAutoWakeup(pAd, TbttNumToNextWakeUp);
+
+#define RT28XX_MLME_RADIO_ON(pAd) \
+    RT28xxUsbMlmeRadioOn(pAd);
+
+#define RT28XX_MLME_RADIO_OFF(pAd) \
+    RT28xxUsbMlmeRadioOFF(pAd);
+
+#endif //__RT2870_H__
diff --git a/drivers/staging/rt2870/rt28xx.h b/drivers/staging/rt2870/rt28xx.h
new file mode 100644
index 00000000000..3927d22f78f
--- /dev/null
+++ b/drivers/staging/rt2870/rt28xx.h
@@ -0,0 +1,2689 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rt28xx.h
+
+	Abstract:
+	RT28xx ASIC related definition & structures
+
+	Revision History:
+	Who			When		  What
+	--------	----------	  ----------------------------------------------
+       Jan Lee           Jan-3-2006     created for RT2860c
+*/
+
+#ifndef	__RT28XX_H__
+#define	__RT28XX_H__
+
+
+//
+// PCI registers - base address 0x0000
+//
+#define PCI_CFG			0x0000
+#define PCI_EECTRL			0x0004
+#define PCI_MCUCTRL			0x0008
+
+//
+// SCH/DMA registers - base address 0x0200
+//
+// INT_SOURCE_CSR: Interrupt source register. Write one to clear corresponding bit
+//
+#define DMA_CSR0      0x200
+#define INT_SOURCE_CSR      0x200
+#ifdef RT_BIG_ENDIAN
+typedef	union	_INT_SOURCE_CSR_STRUC	{
+	struct	{
+		UINT32       	:14;
+		UINT32       	TxCoherent:1;
+		UINT32       	RxCoherent:1;
+		UINT32       	GPTimer:1;
+		UINT32       	AutoWakeup:1;//bit14
+		UINT32       	TXFifoStatusInt:1;//FIFO Statistics is full, sw should read 0x171c
+		UINT32       	PreTBTT:1;
+		UINT32       	TBTTInt:1;
+		UINT32       	RxTxCoherent:1;
+		UINT32       	MCUCommandINT:1;
+		UINT32       	MgmtDmaDone:1;
+		UINT32       	HccaDmaDone:1;
+		UINT32       	Ac3DmaDone:1;
+		UINT32       	Ac2DmaDone:1;
+		UINT32       	Ac1DmaDone:1;
+		UINT32		Ac0DmaDone:1;
+		UINT32		RxDone:1;
+		UINT32		TxDelayINT:1;	//delayed interrupt, not interrupt until several int or time limit hit
+		UINT32		RxDelayINT:1; //dealyed interrupt
+	}	field;
+	UINT32			word;
+}	INT_SOURCE_CSR_STRUC, *PINT_SOURCE_CSR_STRUC;
+#else
+typedef	union	_INT_SOURCE_CSR_STRUC	{
+	struct	{
+		UINT32		RxDelayINT:1;
+		UINT32		TxDelayINT:1;
+		UINT32		RxDone:1;
+		UINT32		Ac0DmaDone:1;//4
+		UINT32       	Ac1DmaDone:1;
+		UINT32       	Ac2DmaDone:1;
+		UINT32       	Ac3DmaDone:1;
+		UINT32       	HccaDmaDone:1; // bit7
+		UINT32       	MgmtDmaDone:1;
+		UINT32       	MCUCommandINT:1;//bit 9
+		UINT32       	RxTxCoherent:1;
+		UINT32       	TBTTInt:1;
+		UINT32       	PreTBTT:1;
+		UINT32       	TXFifoStatusInt:1;//FIFO Statistics is full, sw should read 0x171c
+		UINT32       	AutoWakeup:1;//bit14
+		UINT32       	GPTimer:1;
+		UINT32       	RxCoherent:1;//bit16
+		UINT32       	TxCoherent:1;
+		UINT32       	:14;
+	}	field;
+	UINT32			word;
+} INT_SOURCE_CSR_STRUC, *PINT_SOURCE_CSR_STRUC;
+#endif
+
+//
+// INT_MASK_CSR:   Interrupt MASK register.   1: the interrupt is mask OFF
+//
+#define INT_MASK_CSR        0x204
+#ifdef RT_BIG_ENDIAN
+typedef	union	_INT_MASK_CSR_STRUC	{
+	struct	{
+		UINT32       	TxCoherent:1;
+		UINT32       	RxCoherent:1;
+		UINT32       	:20;
+		UINT32       	MCUCommandINT:1;
+		UINT32       	MgmtDmaDone:1;
+		UINT32       	HccaDmaDone:1;
+		UINT32       	Ac3DmaDone:1;
+		UINT32       	Ac2DmaDone:1;
+		UINT32       	Ac1DmaDone:1;
+		UINT32		Ac0DmaDone:1;
+		UINT32		RxDone:1;
+		UINT32		TxDelay:1;
+		UINT32		RXDelay_INT_MSK:1;
+	}	field;
+	UINT32			word;
+}INT_MASK_CSR_STRUC, *PINT_MASK_CSR_STRUC;
+#else
+typedef	union	_INT_MASK_CSR_STRUC	{
+	struct	{
+		UINT32		RXDelay_INT_MSK:1;
+		UINT32		TxDelay:1;
+		UINT32		RxDone:1;
+		UINT32		Ac0DmaDone:1;
+		UINT32       	Ac1DmaDone:1;
+		UINT32       	Ac2DmaDone:1;
+		UINT32       	Ac3DmaDone:1;
+		UINT32       	HccaDmaDone:1;
+		UINT32       	MgmtDmaDone:1;
+		UINT32       	MCUCommandINT:1;
+		UINT32       	:20;
+		UINT32       	RxCoherent:1;
+		UINT32       	TxCoherent:1;
+	}	field;
+	UINT32			word;
+} INT_MASK_CSR_STRUC, *PINT_MASK_CSR_STRUC;
+#endif
+#define WPDMA_GLO_CFG 	0x208
+#ifdef RT_BIG_ENDIAN
+typedef	union	_WPDMA_GLO_CFG_STRUC	{
+	struct	{
+		UINT32       	HDR_SEG_LEN:16;
+		UINT32       	RXHdrScater:8;
+		UINT32       	BigEndian:1;
+		UINT32       	EnTXWriteBackDDONE:1;
+		UINT32       	WPDMABurstSIZE:2;
+		UINT32		RxDMABusy:1;
+		UINT32		EnableRxDMA:1;
+		UINT32		TxDMABusy:1;
+		UINT32		EnableTxDMA:1;
+	}	field;
+	UINT32			word;
+}WPDMA_GLO_CFG_STRUC, *PWPDMA_GLO_CFG_STRUC;
+#else
+typedef	union	_WPDMA_GLO_CFG_STRUC	{
+	struct	{
+		UINT32		EnableTxDMA:1;
+		UINT32		TxDMABusy:1;
+		UINT32		EnableRxDMA:1;
+		UINT32		RxDMABusy:1;
+		UINT32       	WPDMABurstSIZE:2;
+		UINT32       	EnTXWriteBackDDONE:1;
+		UINT32       	BigEndian:1;
+		UINT32       	RXHdrScater:8;
+		UINT32       	HDR_SEG_LEN:16;
+	}	field;
+	UINT32			word;
+} WPDMA_GLO_CFG_STRUC, *PWPDMA_GLO_CFG_STRUC;
+#endif
+#define WPDMA_RST_IDX 	0x20c
+#ifdef RT_BIG_ENDIAN
+typedef	union	_WPDMA_RST_IDX_STRUC	{
+	struct	{
+		UINT32       	:15;
+		UINT32       	RST_DRX_IDX0:1;
+		UINT32       	rsv:10;
+		UINT32       	RST_DTX_IDX5:1;
+		UINT32       	RST_DTX_IDX4:1;
+		UINT32		RST_DTX_IDX3:1;
+		UINT32		RST_DTX_IDX2:1;
+		UINT32		RST_DTX_IDX1:1;
+		UINT32		RST_DTX_IDX0:1;
+	}	field;
+	UINT32			word;
+}WPDMA_RST_IDX_STRUC, *PWPDMA_RST_IDX_STRUC;
+#else
+typedef	union	_WPDMA_RST_IDX_STRUC	{
+	struct	{
+		UINT32		RST_DTX_IDX0:1;
+		UINT32		RST_DTX_IDX1:1;
+		UINT32		RST_DTX_IDX2:1;
+		UINT32		RST_DTX_IDX3:1;
+		UINT32       	RST_DTX_IDX4:1;
+		UINT32       	RST_DTX_IDX5:1;
+		UINT32       	rsv:10;
+		UINT32       	RST_DRX_IDX0:1;
+		UINT32       	:15;
+	}	field;
+	UINT32			word;
+} WPDMA_RST_IDX_STRUC, *PWPDMA_RST_IDX_STRUC;
+#endif
+#define DELAY_INT_CFG  0x0210
+#ifdef RT_BIG_ENDIAN
+typedef	union	_DELAY_INT_CFG_STRUC	{
+	struct	{
+		UINT32       	TXDLY_INT_EN:1;
+		UINT32       	TXMAX_PINT:7;
+		UINT32       	TXMAX_PTIME:8;
+		UINT32       	RXDLY_INT_EN:1;
+		UINT32       	RXMAX_PINT:7;
+		UINT32		RXMAX_PTIME:8;
+	}	field;
+	UINT32			word;
+}DELAY_INT_CFG_STRUC, *PDELAY_INT_CFG_STRUC;
+#else
+typedef	union	_DELAY_INT_CFG_STRUC	{
+	struct	{
+		UINT32		RXMAX_PTIME:8;
+		UINT32       	RXMAX_PINT:7;
+		UINT32       	RXDLY_INT_EN:1;
+		UINT32       	TXMAX_PTIME:8;
+		UINT32       	TXMAX_PINT:7;
+		UINT32       	TXDLY_INT_EN:1;
+	}	field;
+	UINT32			word;
+} DELAY_INT_CFG_STRUC, *PDELAY_INT_CFG_STRUC;
+#endif
+#define WMM_AIFSN_CFG   0x0214
+#ifdef RT_BIG_ENDIAN
+typedef	union	_AIFSN_CSR_STRUC	{
+	struct	{
+	    UINT32   Rsv:16;
+	    UINT32   Aifsn3:4;       // for AC_VO
+	    UINT32   Aifsn2:4;       // for AC_VI
+	    UINT32   Aifsn1:4;       // for AC_BK
+	    UINT32   Aifsn0:4;       // for AC_BE
+	}	field;
+	UINT32			word;
+}	AIFSN_CSR_STRUC, *PAIFSN_CSR_STRUC;
+#else
+typedef	union	_AIFSN_CSR_STRUC	{
+	struct	{
+	    UINT32   Aifsn0:4;       // for AC_BE
+	    UINT32   Aifsn1:4;       // for AC_BK
+	    UINT32   Aifsn2:4;       // for AC_VI
+	    UINT32   Aifsn3:4;       // for AC_VO
+	    UINT32   Rsv:16;
+	}	field;
+	UINT32			word;
+}	AIFSN_CSR_STRUC, *PAIFSN_CSR_STRUC;
+#endif
+//
+// CWMIN_CSR: CWmin for each EDCA AC
+//
+#define WMM_CWMIN_CFG   0x0218
+#ifdef RT_BIG_ENDIAN
+typedef	union	_CWMIN_CSR_STRUC	{
+	struct	{
+	    UINT32   Rsv:16;
+	    UINT32   Cwmin3:4;       // for AC_VO
+	    UINT32   Cwmin2:4;       // for AC_VI
+	    UINT32   Cwmin1:4;       // for AC_BK
+	    UINT32   Cwmin0:4;       // for AC_BE
+	}	field;
+	UINT32			word;
+}	CWMIN_CSR_STRUC, *PCWMIN_CSR_STRUC;
+#else
+typedef	union	_CWMIN_CSR_STRUC	{
+	struct	{
+	    UINT32   Cwmin0:4;       // for AC_BE
+	    UINT32   Cwmin1:4;       // for AC_BK
+	    UINT32   Cwmin2:4;       // for AC_VI
+	    UINT32   Cwmin3:4;       // for AC_VO
+	    UINT32   Rsv:16;
+	}	field;
+	UINT32			word;
+}	CWMIN_CSR_STRUC, *PCWMIN_CSR_STRUC;
+#endif
+
+//
+// CWMAX_CSR: CWmin for each EDCA AC
+//
+#define WMM_CWMAX_CFG   0x021c
+#ifdef RT_BIG_ENDIAN
+typedef	union	_CWMAX_CSR_STRUC	{
+	struct	{
+	    UINT32   Rsv:16;
+	    UINT32   Cwmax3:4;       // for AC_VO
+	    UINT32   Cwmax2:4;       // for AC_VI
+	    UINT32   Cwmax1:4;       // for AC_BK
+	    UINT32   Cwmax0:4;       // for AC_BE
+	}	field;
+	UINT32			word;
+}	CWMAX_CSR_STRUC, *PCWMAX_CSR_STRUC;
+#else
+typedef	union	_CWMAX_CSR_STRUC	{
+	struct	{
+	    UINT32   Cwmax0:4;       // for AC_BE
+	    UINT32   Cwmax1:4;       // for AC_BK
+	    UINT32   Cwmax2:4;       // for AC_VI
+	    UINT32   Cwmax3:4;       // for AC_VO
+	    UINT32   Rsv:16;
+	}	field;
+	UINT32			word;
+}	CWMAX_CSR_STRUC, *PCWMAX_CSR_STRUC;
+#endif
+
+
+//
+// AC_TXOP_CSR0: AC_BK/AC_BE TXOP register
+//
+#define WMM_TXOP0_CFG    0x0220
+#ifdef RT_BIG_ENDIAN
+typedef	union	_AC_TXOP_CSR0_STRUC	{
+	struct	{
+	    USHORT  Ac1Txop;        // for AC_BE, in unit of 32us
+	    USHORT  Ac0Txop;        // for AC_BK, in unit of 32us
+	}	field;
+	UINT32			word;
+}	AC_TXOP_CSR0_STRUC, *PAC_TXOP_CSR0_STRUC;
+#else
+typedef	union	_AC_TXOP_CSR0_STRUC	{
+	struct	{
+	    USHORT  Ac0Txop;        // for AC_BK, in unit of 32us
+	    USHORT  Ac1Txop;        // for AC_BE, in unit of 32us
+	}	field;
+	UINT32			word;
+}	AC_TXOP_CSR0_STRUC, *PAC_TXOP_CSR0_STRUC;
+#endif
+
+//
+// AC_TXOP_CSR1: AC_VO/AC_VI TXOP register
+//
+#define WMM_TXOP1_CFG    0x0224
+#ifdef RT_BIG_ENDIAN
+typedef	union	_AC_TXOP_CSR1_STRUC	{
+	struct	{
+	    USHORT  Ac3Txop;        // for AC_VO, in unit of 32us
+	    USHORT  Ac2Txop;        // for AC_VI, in unit of 32us
+	}	field;
+	UINT32			word;
+}	AC_TXOP_CSR1_STRUC, *PAC_TXOP_CSR1_STRUC;
+#else
+typedef	union	_AC_TXOP_CSR1_STRUC	{
+	struct	{
+	    USHORT  Ac2Txop;        // for AC_VI, in unit of 32us
+	    USHORT  Ac3Txop;        // for AC_VO, in unit of 32us
+	}	field;
+	UINT32			word;
+}	AC_TXOP_CSR1_STRUC, *PAC_TXOP_CSR1_STRUC;
+#endif
+#define RINGREG_DIFF			0x10
+#define GPIO_CTRL_CFG    0x0228	//MAC_CSR13
+#define MCU_CMD_CFG    0x022c
+#define TX_BASE_PTR0     0x0230	//AC_BK base address
+#define TX_MAX_CNT0      0x0234
+#define TX_CTX_IDX0       0x0238
+#define TX_DTX_IDX0      0x023c
+#define TX_BASE_PTR1     0x0240 	//AC_BE base address
+#define TX_MAX_CNT1      0x0244
+#define TX_CTX_IDX1       0x0248
+#define TX_DTX_IDX1      0x024c
+#define TX_BASE_PTR2     0x0250 	//AC_VI base address
+#define TX_MAX_CNT2      0x0254
+#define TX_CTX_IDX2       0x0258
+#define TX_DTX_IDX2      0x025c
+#define TX_BASE_PTR3     0x0260 	//AC_VO base address
+#define TX_MAX_CNT3      0x0264
+#define TX_CTX_IDX3       0x0268
+#define TX_DTX_IDX3      0x026c
+#define TX_BASE_PTR4     0x0270 	//HCCA base address
+#define TX_MAX_CNT4      0x0274
+#define TX_CTX_IDX4       0x0278
+#define TX_DTX_IDX4      0x027c
+#define TX_BASE_PTR5     0x0280 	//MGMT base address
+#define  TX_MAX_CNT5     0x0284
+#define TX_CTX_IDX5       0x0288
+#define TX_DTX_IDX5      0x028c
+#define TX_MGMTMAX_CNT      TX_MAX_CNT5
+#define TX_MGMTCTX_IDX       TX_CTX_IDX5
+#define TX_MGMTDTX_IDX      TX_DTX_IDX5
+#define RX_BASE_PTR     0x0290 	//RX base address
+#define RX_MAX_CNT      0x0294
+#define RX_CRX_IDX       0x0298
+#define RX_DRX_IDX      0x029c
+#define USB_DMA_CFG      0x02a0
+#ifdef RT_BIG_ENDIAN
+typedef	union	_USB_DMA_CFG_STRUC	{
+	struct	{
+	    UINT32  TxBusy:1;   	//USB DMA TX FSM busy . debug only
+	    UINT32  RxBusy:1;        //USB DMA RX FSM busy . debug only
+	    UINT32  EpoutValid:6;        //OUT endpoint data valid. debug only
+	    UINT32  TxBulkEn:1;        //Enable USB DMA Tx
+	    UINT32  RxBulkEn:1;        //Enable USB DMA Rx
+	    UINT32  RxBulkAggEn:1;        //Enable Rx Bulk Aggregation
+	    UINT32  TxopHalt:1;        //Halt TXOP count down when TX buffer is full.
+	    UINT32  TxClear:1;        //Clear USB DMA TX path
+	    UINT32  rsv:2;
+	    UINT32  phyclear:1;        		//phy watch dog enable. write 1
+	    UINT32  RxBulkAggLmt:8;        //Rx Bulk Aggregation Limit  in unit of 1024 bytes
+	    UINT32  RxBulkAggTOut:8;        //Rx Bulk Aggregation TimeOut  in unit of 33ns
+	}	field;
+	UINT32			word;
+}	USB_DMA_CFG_STRUC, *PUSB_DMA_CFG_STRUC;
+#else
+typedef	union	_USB_DMA_CFG_STRUC	{
+	struct	{
+	    UINT32  RxBulkAggTOut:8;        //Rx Bulk Aggregation TimeOut  in unit of 33ns
+	    UINT32  RxBulkAggLmt:8;        //Rx Bulk Aggregation Limit  in unit of 256 bytes
+	    UINT32  phyclear:1;        		//phy watch dog enable. write 1
+	    UINT32  rsv:2;
+	    UINT32  TxClear:1;        //Clear USB DMA TX path
+	    UINT32  TxopHalt:1;        //Halt TXOP count down when TX buffer is full.
+	    UINT32  RxBulkAggEn:1;        //Enable Rx Bulk Aggregation
+	    UINT32  RxBulkEn:1;        //Enable USB DMA Rx
+	    UINT32  TxBulkEn:1;        //Enable USB DMA Tx
+	    UINT32  EpoutValid:6;        //OUT endpoint data valid
+	    UINT32  RxBusy:1;        //USB DMA RX FSM busy
+	    UINT32  TxBusy:1;   	//USB DMA TX FSM busy
+	}	field;
+	UINT32			word;
+}	USB_DMA_CFG_STRUC, *PUSB_DMA_CFG_STRUC;
+#endif
+
+//
+//  3  PBF  registers
+//
+//
+// Most are for debug. Driver doesn't touch PBF register.
+#define 	PBF_SYS_CTRL 	 0x0400
+#define     PBF_CFG                 0x0408
+#define 	PBF_MAX_PCNT 	 0x040C
+#define 	PBF_CTRL	 	0x0410
+#define 	PBF_INT_STA	 0x0414
+#define 	PBF_INT_ENA	 0x0418
+#define 	TXRXQ_PCNT  	 0x0438
+#define 	PBF_DBG 	 	 0x043c
+#define     PBF_CAP_CTRL     0x0440
+
+//
+//  4  MAC  registers
+//
+//
+//  4.1 MAC SYSTEM  configuration registers (offset:0x1000)
+//
+#define MAC_CSR0            0x1000
+#ifdef RT_BIG_ENDIAN
+typedef	union	_ASIC_VER_ID_STRUC	{
+	struct	{
+	    USHORT  ASICVer;        // version : 2860
+	    USHORT  ASICRev;        // reversion  : 0
+	}	field;
+	UINT32			word;
+}	ASIC_VER_ID_STRUC, *PASIC_VER_ID_STRUC;
+#else
+typedef	union	_ASIC_VER_ID_STRUC	{
+	struct	{
+	    USHORT  ASICRev;        // reversion  : 0
+	    USHORT  ASICVer;        // version : 2860
+	}	field;
+	UINT32			word;
+}	ASIC_VER_ID_STRUC, *PASIC_VER_ID_STRUC;
+#endif
+#define MAC_SYS_CTRL            0x1004		//MAC_CSR1
+#define MAC_ADDR_DW0            		0x1008		// MAC ADDR DW0
+#define MAC_ADDR_DW1           		 0x100c		// MAC ADDR DW1
+//
+// MAC_CSR2: STA MAC register 0
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_MAC_DW0_STRUC	{
+	struct	{
+		UCHAR		Byte3;		// MAC address byte 3
+		UCHAR		Byte2;		// MAC address byte 2
+		UCHAR		Byte1;		// MAC address byte 1
+		UCHAR		Byte0;		// MAC address byte 0
+	}	field;
+	UINT32			word;
+}	MAC_DW0_STRUC, *PMAC_DW0_STRUC;
+#else
+typedef	union	_MAC_DW0_STRUC	{
+	struct	{
+		UCHAR		Byte0;		// MAC address byte 0
+		UCHAR		Byte1;		// MAC address byte 1
+		UCHAR		Byte2;		// MAC address byte 2
+		UCHAR		Byte3;		// MAC address byte 3
+	}	field;
+	UINT32			word;
+}	MAC_DW0_STRUC, *PMAC_DW0_STRUC;
+#endif
+
+//
+// MAC_CSR3: STA MAC register 1
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_MAC_DW1_STRUC	{
+	struct	{
+		UCHAR		Rsvd1;
+		UCHAR		U2MeMask;
+		UCHAR		Byte5;		// MAC address byte 5
+		UCHAR		Byte4;		// MAC address byte 4
+	}	field;
+	UINT32			word;
+}	MAC_DW1_STRUC, *PMAC_DW1_STRUC;
+#else
+typedef	union	_MAC_DW1_STRUC	{
+	struct	{
+		UCHAR		Byte4;		// MAC address byte 4
+		UCHAR		Byte5;		// MAC address byte 5
+		UCHAR		U2MeMask;
+		UCHAR		Rsvd1;
+	}	field;
+	UINT32			word;
+}	MAC_DW1_STRUC, *PMAC_DW1_STRUC;
+#endif
+
+#define MAC_BSSID_DW0            		0x1010		// MAC BSSID DW0
+#define MAC_BSSID_DW1            		0x1014		// MAC BSSID DW1
+
+//
+// MAC_CSR5: BSSID register 1
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_MAC_CSR5_STRUC	{
+	struct	{
+		USHORT		Rsvd:11;
+		USHORT		MBssBcnNum:3;
+		USHORT		BssIdMode:2; // 0: one BSSID, 10: 4 BSSID,  01: 2 BSSID , 11: 8BSSID
+		UCHAR		Byte5;		 // BSSID byte 5
+		UCHAR		Byte4;		 // BSSID byte 4
+	}	field;
+	UINT32			word;
+}	MAC_CSR5_STRUC, *PMAC_CSR5_STRUC;
+#else
+typedef	union	_MAC_CSR5_STRUC	{
+	struct	{
+		UCHAR		Byte4;		 // BSSID byte 4
+		UCHAR		Byte5;		 // BSSID byte 5
+		USHORT      	BssIdMask:2; // 0: one BSSID, 10: 4 BSSID,  01: 2 BSSID , 11: 8BSSID
+		USHORT		MBssBcnNum:3;
+		USHORT		Rsvd:11;
+	}	field;
+	UINT32			word;
+}	MAC_CSR5_STRUC, *PMAC_CSR5_STRUC;
+#endif
+
+#define MAX_LEN_CFG              0x1018		// rt2860b max 16k bytes. bit12:13 Maximum PSDU length (power factor) 0:2^13, 1:2^14, 2:2^15, 3:2^16
+#define BBP_CSR_CFG            		0x101c		//
+//
+// BBP_CSR_CFG: BBP serial control register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_BBP_CSR_CFG_STRUC	{
+	struct	{
+		UINT32		:12;
+		UINT32		BBP_RW_MODE:1;		// 0: use serial mode  1:parallel
+		UINT32		BBP_PAR_DUR:1;		    // 0: 4 MAC clock cycles  1: 8 MAC clock cycles
+		UINT32		Busy:1;				// 1: ASIC is busy execute BBP programming.
+		UINT32		fRead:1;		    // 0: Write	BBP, 1:	Read BBP
+		UINT32		RegNum:8;			// Selected	BBP	register
+		UINT32		Value:8;			// Register	value to program into BBP
+	}	field;
+	UINT32			word;
+}	BBP_CSR_CFG_STRUC, *PBBP_CSR_CFG_STRUC;
+#else
+typedef	union	_BBP_CSR_CFG_STRUC	{
+	struct	{
+		UINT32		Value:8;			// Register	value to program into BBP
+		UINT32		RegNum:8;			// Selected	BBP	register
+		UINT32		fRead:1;		    // 0: Write	BBP, 1:	Read BBP
+		UINT32		Busy:1;				// 1: ASIC is busy execute BBP programming.
+		UINT32		BBP_PAR_DUR:1;		     // 0: 4 MAC clock cycles  1: 8 MAC clock cycles
+		UINT32		BBP_RW_MODE:1;		// 0: use serial mode  1:parallel
+		UINT32		:12;
+	}	field;
+	UINT32			word;
+}	BBP_CSR_CFG_STRUC, *PBBP_CSR_CFG_STRUC;
+#endif
+#define RF_CSR_CFG0            		0x1020
+//
+// RF_CSR_CFG: RF control register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_RF_CSR_CFG0_STRUC	{
+	struct	{
+		UINT32		Busy:1;		    // 0: idle 1: 8busy
+		UINT32		Sel:1;				// 0:RF_LE0 activate  1:RF_LE1 activate
+		UINT32		StandbyMode:1;		    // 0: high when stand by 1:	low when standby
+		UINT32		bitwidth:5;			// Selected	BBP	register
+		UINT32		RegIdAndContent:24;			// Register	value to program into BBP
+	}	field;
+	UINT32			word;
+}	RF_CSR_CFG0_STRUC, *PRF_CSR_CFG0_STRUC;
+#else
+typedef	union	_RF_CSR_CFG0_STRUC	{
+	struct	{
+		UINT32		RegIdAndContent:24;			// Register	value to program into BBP
+		UINT32		bitwidth:5;			// Selected	BBP	register
+		UINT32		StandbyMode:1;		    // 0: high when stand by 1:	low when standby
+		UINT32		Sel:1;				// 0:RF_LE0 activate  1:RF_LE1 activate
+		UINT32		Busy:1;		    // 0: idle 1: 8busy
+	}	field;
+	UINT32			word;
+}	RF_CSR_CFG0_STRUC, *PRF_CSR_CFG0_STRUC;
+#endif
+#define RF_CSR_CFG1           		0x1024
+#ifdef RT_BIG_ENDIAN
+typedef	union	_RF_CSR_CFG1_STRUC	{
+	struct	{
+		UINT32		rsv:7;		    // 0: idle 1: 8busy
+		UINT32		RFGap:5;			// Gap between BB_CONTROL_RF and RF_LE. 0: 3 system clock cycle (37.5usec) 1: 5 system clock cycle (62.5usec)
+		UINT32		RegIdAndContent:24;			// Register	value to program into BBP
+	}	field;
+	UINT32			word;
+}	RF_CSR_CFG1_STRUC, *PRF_CSR_CFG1_STRUC;
+#else
+typedef	union	_RF_CSR_CFG1_STRUC	{
+	struct	{
+		UINT32		RegIdAndContent:24;			// Register	value to program into BBP
+		UINT32		RFGap:5;			// Gap between BB_CONTROL_RF and RF_LE. 0: 3 system clock cycle (37.5usec) 1: 5 system clock cycle (62.5usec)
+		UINT32		rsv:7;		    // 0: idle 1: 8busy
+	}	field;
+	UINT32			word;
+}	RF_CSR_CFG1_STRUC, *PRF_CSR_CFG1_STRUC;
+#endif
+#define RF_CSR_CFG2           		0x1028		//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_RF_CSR_CFG2_STRUC	{
+	struct	{
+		UINT32		rsv:8;		    // 0: idle 1: 8busy
+		UINT32		RegIdAndContent:24;			// Register	value to program into BBP
+	}	field;
+	UINT32			word;
+}	RF_CSR_CFG2_STRUC, *PRF_CSR_CFG2_STRUC;
+#else
+typedef	union	_RF_CSR_CFG2_STRUC	{
+	struct	{
+		UINT32		RegIdAndContent:24;			// Register	value to program into BBP
+		UINT32		rsv:8;		    // 0: idle 1: 8busy
+	}	field;
+	UINT32			word;
+}	RF_CSR_CFG2_STRUC, *PRF_CSR_CFG2_STRUC;
+#endif
+#define LED_CFG           		0x102c		//  MAC_CSR14
+#ifdef RT_BIG_ENDIAN
+typedef	union	_LED_CFG_STRUC	{
+	struct	{
+		UINT32		:1;
+		UINT32		LedPolar:1;			// Led Polarity.  0: active low1: active high
+		UINT32		YLedMode:2;			// yellow Led Mode
+		UINT32		GLedMode:2;			// green Led Mode
+		UINT32		RLedMode:2;			// red Led Mode    0: off1: blinking upon TX2: periodic slow blinking3: always on
+		UINT32		rsv:2;
+		UINT32		SlowBlinkPeriod:6;			// slow blinking period. unit:1ms
+		UINT32		OffPeriod:8;			// blinking off period unit 1ms
+		UINT32		OnPeriod:8;			// blinking on period unit 1ms
+	}	field;
+	UINT32			word;
+}	LED_CFG_STRUC, *PLED_CFG_STRUC;
+#else
+typedef	union	_LED_CFG_STRUC	{
+	struct	{
+		UINT32		OnPeriod:8;			// blinking on period unit 1ms
+		UINT32		OffPeriod:8;			// blinking off period unit 1ms
+		UINT32		SlowBlinkPeriod:6;			// slow blinking period. unit:1ms
+		UINT32		rsv:2;
+		UINT32		RLedMode:2;			// red Led Mode    0: off1: blinking upon TX2: periodic slow blinking3: always on
+		UINT32		GLedMode:2;			// green Led Mode
+		UINT32		YLedMode:2;			// yellow Led Mode
+		UINT32		LedPolar:1;			// Led Polarity.  0: active low1: active high
+		UINT32		:1;
+	}	field;
+	UINT32			word;
+}	LED_CFG_STRUC, *PLED_CFG_STRUC;
+#endif
+//
+//  4.2 MAC TIMING  configuration registers (offset:0x1100)
+//
+#define XIFS_TIME_CFG             0x1100		 // MAC_CSR8  MAC_CSR9
+#ifdef RT_BIG_ENDIAN
+typedef	union	_IFS_SLOT_CFG_STRUC	{
+	struct	{
+	    UINT32  rsv:2;
+	    UINT32  BBRxendEnable:1;        //  reference RXEND signal to begin XIFS defer
+	    UINT32  EIFS:9;        //  unit 1us
+	    UINT32  OfdmXifsTime:4;        //OFDM SIFS. unit 1us. Applied after OFDM RX when MAC doesn't reference BBP signal BBRXEND
+	    UINT32  OfdmSifsTime:8;        //  unit 1us. Applied after OFDM RX/TX
+	    UINT32  CckmSifsTime:8;        //  unit 1us. Applied after CCK RX/TX
+	}	field;
+	UINT32			word;
+}	IFS_SLOT_CFG_STRUC, *PIFS_SLOT_CFG_STRUC;
+#else
+typedef	union	_IFS_SLOT_CFG_STRUC	{
+	struct	{
+	    UINT32  CckmSifsTime:8;        //  unit 1us. Applied after CCK RX/TX
+	    UINT32  OfdmSifsTime:8;        //  unit 1us. Applied after OFDM RX/TX
+	    UINT32  OfdmXifsTime:4;        //OFDM SIFS. unit 1us. Applied after OFDM RX when MAC doesn't reference BBP signal BBRXEND
+	    UINT32  EIFS:9;        //  unit 1us
+	    UINT32  BBRxendEnable:1;        //  reference RXEND signal to begin XIFS defer
+	    UINT32  rsv:2;
+	}	field;
+	UINT32			word;
+}	IFS_SLOT_CFG_STRUC, *PIFS_SLOT_CFG_STRUC;
+#endif
+
+#define BKOFF_SLOT_CFG             0x1104		 //  mac_csr9 last 8 bits
+#define NAV_TIME_CFG             0x1108		 // NAV  (MAC_CSR15)
+#define CH_TIME_CFG             0x110C		 	// Count as channel busy
+#define PBF_LIFE_TIMER             0x1110		 //TX/RX MPDU timestamp timer (free run)Unit: 1us
+#define BCN_TIME_CFG             0x1114		 // TXRX_CSR9
+
+#define BCN_OFFSET0				0x042C
+#define BCN_OFFSET1				0x0430
+
+//
+// BCN_TIME_CFG : Synchronization control register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_BCN_TIME_CFG_STRUC	{
+	struct	{
+		UINT32		TxTimestampCompensate:8;
+        UINT32       :3;
+		UINT32		bBeaconGen:1;		// Enable beacon generator
+        UINT32       bTBTTEnable:1;
+		UINT32		TsfSyncMode:2;		// Enable TSF sync, 00: disable, 01: infra mode, 10: ad-hoc mode
+		UINT32		bTsfTicking:1;		// Enable TSF auto counting
+		UINT32       BeaconInterval:16;  // in unit of 1/16 TU
+	}	field;
+	UINT32			word;
+}	BCN_TIME_CFG_STRUC, *PBCN_TIME_CFG_STRUC;
+#else
+typedef	union	_BCN_TIME_CFG_STRUC	{
+	struct	{
+		UINT32       BeaconInterval:16;  // in unit of 1/16 TU
+		UINT32		bTsfTicking:1;		// Enable TSF auto counting
+		UINT32		TsfSyncMode:2;		// Enable TSF sync, 00: disable, 01: infra mode, 10: ad-hoc mode
+        UINT32       bTBTTEnable:1;
+		UINT32		bBeaconGen:1;		// Enable beacon generator
+        UINT32       :3;
+		UINT32		TxTimestampCompensate:8;
+	}	field;
+	UINT32			word;
+}	BCN_TIME_CFG_STRUC, *PBCN_TIME_CFG_STRUC;
+#endif
+#define TBTT_SYNC_CFG            0x1118  		// txrx_csr10
+#define TSF_TIMER_DW0             0x111C  		// Local TSF timer lsb 32 bits. Read-only
+#define TSF_TIMER_DW1             0x1120  		// msb 32 bits. Read-only.
+#define TBTT_TIMER             	0x1124  		// TImer remains till next TBTT. Read-only.  TXRX_CSR14
+#define INT_TIMER_CFG              	0x1128  		//
+#define INT_TIMER_EN             	0x112c  		//  GP-timer and pre-tbtt Int enable
+#define CH_IDLE_STA              	0x1130  		//  channel idle time
+#define CH_BUSY_STA              	0x1134  		//  channle busy time
+//
+//  4.2 MAC POWER  configuration registers (offset:0x1200)
+//
+#define MAC_STATUS_CFG             0x1200		 // old MAC_CSR12
+#define PWR_PIN_CFG             0x1204		 // old MAC_CSR12
+#define AUTO_WAKEUP_CFG             0x1208		 // old MAC_CSR10
+//
+// AUTO_WAKEUP_CFG: Manual power control / status register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_AUTO_WAKEUP_STRUC	{
+	struct	{
+		UINT32		:16;
+		UINT32		EnableAutoWakeup:1;	// 0:sleep, 1:awake
+		UINT32       NumofSleepingTbtt:7;          // ForceWake has high privilege than PutToSleep when both set
+		UINT32       AutoLeadTime:8;
+	}	field;
+	UINT32			word;
+}	AUTO_WAKEUP_STRUC, *PAUTO_WAKEUP_STRUC;
+#else
+typedef	union	_AUTO_WAKEUP_STRUC	{
+	struct	{
+		UINT32       AutoLeadTime:8;
+		UINT32       NumofSleepingTbtt:7;          // ForceWake has high privilege than PutToSleep when both set
+		UINT32		EnableAutoWakeup:1;	// 0:sleep, 1:awake
+		UINT32		:16;
+	}	field;
+	UINT32			word;
+}	AUTO_WAKEUP_STRUC, *PAUTO_WAKEUP_STRUC;
+#endif
+//
+//  4.3 MAC TX  configuration registers (offset:0x1300)
+//
+
+#define EDCA_AC0_CFG	0x1300		//AC_TXOP_CSR0 0x3474
+#define EDCA_AC1_CFG	0x1304
+#define EDCA_AC2_CFG	0x1308
+#define EDCA_AC3_CFG	0x130c
+#ifdef RT_BIG_ENDIAN
+typedef	union	_EDCA_AC_CFG_STRUC	{
+	struct	{
+	    UINT32  :12;        //
+	    UINT32  Cwmax:4;        //unit power of 2
+	    UINT32  Cwmin:4;        //
+	    UINT32  Aifsn:4;        // # of slot time
+	    UINT32  AcTxop:8;        //  in unit of 32us
+	}	field;
+	UINT32			word;
+}	EDCA_AC_CFG_STRUC, *PEDCA_AC_CFG_STRUC;
+#else
+typedef	union	_EDCA_AC_CFG_STRUC	{
+	struct	{
+	    UINT32  AcTxop:8;        //  in unit of 32us
+	    UINT32  Aifsn:4;        // # of slot time
+	    UINT32  Cwmin:4;        //
+	    UINT32  Cwmax:4;        //unit power of 2
+	    UINT32  :12;       //
+	}	field;
+	UINT32			word;
+}	EDCA_AC_CFG_STRUC, *PEDCA_AC_CFG_STRUC;
+#endif
+
+#define EDCA_TID_AC_MAP	0x1310
+#define TX_PWR_CFG_0	0x1314
+#define TX_PWR_CFG_1	0x1318
+#define TX_PWR_CFG_2	0x131C
+#define TX_PWR_CFG_3	0x1320
+#define TX_PWR_CFG_4	0x1324
+#define TX_PIN_CFG		0x1328
+#define TX_BAND_CFG	0x132c		// 0x1 use upper 20MHz. 0 juse lower 20MHz
+#define TX_SW_CFG0		0x1330
+#define TX_SW_CFG1		0x1334
+#define TX_SW_CFG2		0x1338
+#define TXOP_THRES_CFG		0x133c
+#define TXOP_CTRL_CFG		0x1340
+#define TX_RTS_CFG		0x1344
+
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_RTS_CFG_STRUC	{
+	struct	{
+	    UINT32       rsv:7;
+	    UINT32       RtsFbkEn:1;    // enable rts rate fallback
+	    UINT32       RtsThres:16;    // unit:byte
+	    UINT32       AutoRtsRetryLimit:8;
+	}	field;
+	UINT32			word;
+}	TX_RTS_CFG_STRUC, *PTX_RTS_CFG_STRUC;
+#else
+typedef	union	_TX_RTS_CFG_STRUC	{
+	struct	{
+	    UINT32       AutoRtsRetryLimit:8;
+	    UINT32       RtsThres:16;    // unit:byte
+	    UINT32       RtsFbkEn:1;    // enable rts rate fallback
+	    UINT32       rsv:7;     // 1: HT non-STBC control frame enable
+	}	field;
+	UINT32			word;
+}	TX_RTS_CFG_STRUC, *PTX_RTS_CFG_STRUC;
+#endif
+#define TX_TIMEOUT_CFG	0x1348
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_TIMEOUT_CFG_STRUC	{
+	struct	{
+	    UINT32       rsv2:8;
+	    UINT32       TxopTimeout:8;	//TXOP timeout value for TXOP truncation.  It is recommended that (SLOT_TIME) > (TX_OP_TIMEOUT) > (RX_ACK_TIMEOUT)
+	    UINT32       RxAckTimeout:8;	// unit:slot. Used for TX precedure
+	    UINT32       MpduLifeTime:4;    //  expiration time = 2^(9+MPDU LIFE TIME)  us
+	    UINT32       rsv:4;
+	}	field;
+	UINT32			word;
+}	TX_TIMEOUT_CFG_STRUC, *PTX_TIMEOUT_CFG_STRUC;
+#else
+typedef	union	_TX_TIMEOUT_CFG_STRUC	{
+	struct	{
+	    UINT32       rsv:4;
+	    UINT32       MpduLifeTime:4;    //  expiration time = 2^(9+MPDU LIFE TIME)  us
+	    UINT32       RxAckTimeout:8;	// unit:slot. Used for TX precedure
+	    UINT32       TxopTimeout:8;	//TXOP timeout value for TXOP truncation.  It is recommended that (SLOT_TIME) > (TX_OP_TIMEOUT) > (RX_ACK_TIMEOUT)
+	    UINT32       rsv2:8;     // 1: HT non-STBC control frame enable
+	}	field;
+	UINT32			word;
+}	TX_TIMEOUT_CFG_STRUC, *PTX_TIMEOUT_CFG_STRUC;
+#endif
+#define TX_RTY_CFG	0x134c
+#ifdef RT_BIG_ENDIAN
+typedef	union PACKED _TX_RTY_CFG_STRUC	{
+	struct	{
+	    UINT32       rsv:1;
+	    UINT32       TxautoFBEnable:1;    // Tx retry PHY rate auto fallback enable
+	    UINT32       AggRtyMode:1;	// Aggregate MPDU retry mode.  0:expired by retry limit, 1: expired by mpdu life timer
+	    UINT32       NonAggRtyMode:1;	// Non-Aggregate MPDU retry mode.  0:expired by retry limit, 1: expired by mpdu life timer
+	    UINT32       LongRtyThre:12;	// Long retry threshoold
+	    UINT32       LongRtyLimit:8;	//long retry limit
+	    UINT32       ShortRtyLimit:8;	//  short retry limit
+
+	}	field;
+	UINT32			word;
+}	TX_RTY_CFG_STRUC, *PTX_RTY_CFG_STRUC;
+#else
+typedef	union PACKED _TX_RTY_CFG_STRUC	{
+	struct	{
+	    UINT32       ShortRtyLimit:8;	//  short retry limit
+	    UINT32       LongRtyLimit:8;	//long retry limit
+	    UINT32       LongRtyThre:12;	// Long retry threshoold
+	    UINT32       NonAggRtyMode:1;	// Non-Aggregate MPDU retry mode.  0:expired by retry limit, 1: expired by mpdu life timer
+	    UINT32       AggRtyMode:1;	// Aggregate MPDU retry mode.  0:expired by retry limit, 1: expired by mpdu life timer
+	    UINT32       TxautoFBEnable:1;    // Tx retry PHY rate auto fallback enable
+	    UINT32       rsv:1;     // 1: HT non-STBC control frame enable
+	}	field;
+	UINT32			word;
+}	TX_RTY_CFG_STRUC, *PTX_RTY_CFG_STRUC;
+#endif
+#define TX_LINK_CFG	0x1350
+#ifdef RT_BIG_ENDIAN
+typedef	union	PACKED _TX_LINK_CFG_STRUC	{
+	struct PACKED {
+	    UINT32       RemotMFS:8;	//remote MCS feedback sequence number
+	    UINT32       RemotMFB:8;    //  remote MCS feedback
+	    UINT32       rsv:3;	//
+	    UINT32       TxCFAckEn:1;	//   Piggyback CF-ACK enable
+	    UINT32       TxRDGEn:1;	// RDG TX enable
+	    UINT32       TxMRQEn:1;	//  MCS request TX enable
+	    UINT32       RemoteUMFSEnable:1;	//  remote unsolicit  MFB enable.  0: not apply remote remote unsolicit (MFS=7)
+	    UINT32       MFBEnable:1;	//  TX apply remote MFB 1:enable
+	    UINT32       RemoteMFBLifeTime:8;	//remote MFB life time. unit : 32us
+	}	field;
+	UINT32			word;
+}	TX_LINK_CFG_STRUC, *PTX_LINK_CFG_STRUC;
+#else
+typedef	union	PACKED _TX_LINK_CFG_STRUC	{
+	struct PACKED {
+	    UINT32       RemoteMFBLifeTime:8;	//remote MFB life time. unit : 32us
+	    UINT32       MFBEnable:1;	//  TX apply remote MFB 1:enable
+	    UINT32       RemoteUMFSEnable:1;	//  remote unsolicit  MFB enable.  0: not apply remote remote unsolicit (MFS=7)
+	    UINT32       TxMRQEn:1;	//  MCS request TX enable
+	    UINT32       TxRDGEn:1;	// RDG TX enable
+	    UINT32       TxCFAckEn:1;	//   Piggyback CF-ACK enable
+	    UINT32       rsv:3;	//
+	    UINT32       RemotMFB:8;    //  remote MCS feedback
+	    UINT32       RemotMFS:8;	//remote MCS feedback sequence number
+	}	field;
+	UINT32			word;
+}	TX_LINK_CFG_STRUC, *PTX_LINK_CFG_STRUC;
+#endif
+#define HT_FBK_CFG0	0x1354
+#ifdef RT_BIG_ENDIAN
+typedef	union PACKED _HT_FBK_CFG0_STRUC	{
+	struct	{
+	    UINT32       HTMCS7FBK:4;
+	    UINT32       HTMCS6FBK:4;
+	    UINT32       HTMCS5FBK:4;
+	    UINT32       HTMCS4FBK:4;
+	    UINT32       HTMCS3FBK:4;
+	    UINT32       HTMCS2FBK:4;
+	    UINT32       HTMCS1FBK:4;
+	    UINT32       HTMCS0FBK:4;
+	}	field;
+	UINT32			word;
+}	HT_FBK_CFG0_STRUC, *PHT_FBK_CFG0_STRUC;
+#else
+typedef	union PACKED _HT_FBK_CFG0_STRUC	{
+	struct	{
+	    UINT32       HTMCS0FBK:4;
+	    UINT32       HTMCS1FBK:4;
+	    UINT32       HTMCS2FBK:4;
+	    UINT32       HTMCS3FBK:4;
+	    UINT32       HTMCS4FBK:4;
+	    UINT32       HTMCS5FBK:4;
+	    UINT32       HTMCS6FBK:4;
+	    UINT32       HTMCS7FBK:4;
+	}	field;
+	UINT32			word;
+}	HT_FBK_CFG0_STRUC, *PHT_FBK_CFG0_STRUC;
+#endif
+#define HT_FBK_CFG1	0x1358
+#ifdef RT_BIG_ENDIAN
+typedef	union	_HT_FBK_CFG1_STRUC	{
+	struct	{
+	    UINT32       HTMCS15FBK:4;
+	    UINT32       HTMCS14FBK:4;
+	    UINT32       HTMCS13FBK:4;
+	    UINT32       HTMCS12FBK:4;
+	    UINT32       HTMCS11FBK:4;
+	    UINT32       HTMCS10FBK:4;
+	    UINT32       HTMCS9FBK:4;
+	    UINT32       HTMCS8FBK:4;
+	}	field;
+	UINT32			word;
+}	HT_FBK_CFG1_STRUC, *PHT_FBK_CFG1_STRUC;
+#else
+typedef	union	_HT_FBK_CFG1_STRUC	{
+	struct	{
+	    UINT32       HTMCS8FBK:4;
+	    UINT32       HTMCS9FBK:4;
+	    UINT32       HTMCS10FBK:4;
+	    UINT32       HTMCS11FBK:4;
+	    UINT32       HTMCS12FBK:4;
+	    UINT32       HTMCS13FBK:4;
+	    UINT32       HTMCS14FBK:4;
+	    UINT32       HTMCS15FBK:4;
+	}	field;
+	UINT32			word;
+}	HT_FBK_CFG1_STRUC, *PHT_FBK_CFG1_STRUC;
+#endif
+#define LG_FBK_CFG0	0x135c
+#ifdef RT_BIG_ENDIAN
+typedef	union	_LG_FBK_CFG0_STRUC	{
+	struct	{
+	    UINT32       OFDMMCS7FBK:4;	//initial value is 6
+	    UINT32       OFDMMCS6FBK:4;	//initial value is 5
+	    UINT32       OFDMMCS5FBK:4;	//initial value is 4
+	    UINT32       OFDMMCS4FBK:4;	//initial value is 3
+	    UINT32       OFDMMCS3FBK:4;	//initial value is 2
+	    UINT32       OFDMMCS2FBK:4;	//initial value is 1
+	    UINT32       OFDMMCS1FBK:4;	//initial value is 0
+	    UINT32       OFDMMCS0FBK:4;	//initial value is 0
+	}	field;
+	UINT32			word;
+}	LG_FBK_CFG0_STRUC, *PLG_FBK_CFG0_STRUC;
+#else
+typedef	union	_LG_FBK_CFG0_STRUC	{
+	struct	{
+	    UINT32       OFDMMCS0FBK:4;	//initial value is 0
+	    UINT32       OFDMMCS1FBK:4;	//initial value is 0
+	    UINT32       OFDMMCS2FBK:4;	//initial value is 1
+	    UINT32       OFDMMCS3FBK:4;	//initial value is 2
+	    UINT32       OFDMMCS4FBK:4;	//initial value is 3
+	    UINT32       OFDMMCS5FBK:4;	//initial value is 4
+	    UINT32       OFDMMCS6FBK:4;	//initial value is 5
+	    UINT32       OFDMMCS7FBK:4;	//initial value is 6
+	}	field;
+	UINT32			word;
+}	LG_FBK_CFG0_STRUC, *PLG_FBK_CFG0_STRUC;
+#endif
+#define LG_FBK_CFG1		0x1360
+#ifdef RT_BIG_ENDIAN
+typedef	union	_LG_FBK_CFG1_STRUC	{
+	struct	{
+	    UINT32       rsv:16;
+	    UINT32       CCKMCS3FBK:4;	//initial value is 2
+	    UINT32       CCKMCS2FBK:4;	//initial value is 1
+	    UINT32       CCKMCS1FBK:4;	//initial value is 0
+	    UINT32       CCKMCS0FBK:4;	//initial value is 0
+	}	field;
+	UINT32			word;
+}	LG_FBK_CFG1_STRUC, *PLG_FBK_CFG1_STRUC;
+#else
+typedef	union	_LG_FBK_CFG1_STRUC	{
+	struct	{
+	    UINT32       CCKMCS0FBK:4;	//initial value is 0
+	    UINT32       CCKMCS1FBK:4;	//initial value is 0
+	    UINT32       CCKMCS2FBK:4;	//initial value is 1
+	    UINT32       CCKMCS3FBK:4;	//initial value is 2
+	    UINT32       rsv:16;
+	}	field;
+	UINT32			word;
+}	LG_FBK_CFG1_STRUC, *PLG_FBK_CFG1_STRUC;
+#endif
+
+//=======================================================
+//================ Protection Paramater================================
+//=======================================================
+#define CCK_PROT_CFG	0x1364		//CCK Protection
+#define ASIC_SHORTNAV		1
+#define ASIC_LONGNAV		2
+#define ASIC_RTS		1
+#define ASIC_CTS		2
+#ifdef RT_BIG_ENDIAN
+typedef	union	_PROT_CFG_STRUC	{
+	struct	{
+	    UINT32       rsv:5;
+	    UINT32       RTSThEn:1;	//RTS threshold enable on CCK TX
+	    UINT32       TxopAllowGF40:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       TxopAllowGF20:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       TxopAllowMM40:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       TxopAllowMM20:1;	//CCK TXOP allowance. 0:disallow.
+	    UINT32       TxopAllowOfdm:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       TxopAllowCck:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       ProtectNav:2;	//TXOP protection type for CCK TX. 0:None, 1:ShortNAVprotect,  2:LongNAVProtect, 3:rsv
+	    UINT32       ProtectCtrl:2;	//Protection control frame type for CCK TX. 1:RTS/CTS, 2:CTS-to-self, 0:None, 3:rsv
+	    UINT32       ProtectRate:16;	//Protection control frame rate for CCK TX(RTS/CTS/CFEnd).
+	}	field;
+	UINT32			word;
+}	PROT_CFG_STRUC, *PPROT_CFG_STRUC;
+#else
+typedef	union	_PROT_CFG_STRUC	{
+	struct	{
+	    UINT32       ProtectRate:16;	//Protection control frame rate for CCK TX(RTS/CTS/CFEnd).
+	    UINT32       ProtectCtrl:2;	//Protection control frame type for CCK TX. 1:RTS/CTS, 2:CTS-to-self, 0:None, 3:rsv
+	    UINT32       ProtectNav:2;	//TXOP protection type for CCK TX. 0:None, 1:ShortNAVprotect,  2:LongNAVProtect, 3:rsv
+	    UINT32       TxopAllowCck:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       TxopAllowOfdm:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       TxopAllowMM20:1;	//CCK TXOP allowance. 0:disallow.
+	    UINT32       TxopAllowMM40:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       TxopAllowGF20:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       TxopAllowGF40:1;	//CCK TXOP allowance.0:disallow.
+	    UINT32       RTSThEn:1;	//RTS threshold enable on CCK TX
+	    UINT32       rsv:5;
+	}	field;
+	UINT32			word;
+}	PROT_CFG_STRUC, *PPROT_CFG_STRUC;
+#endif
+
+#define OFDM_PROT_CFG	0x1368		//OFDM Protection
+#define MM20_PROT_CFG	0x136C		//MM20 Protection
+#define MM40_PROT_CFG	0x1370		//MM40 Protection
+#define GF20_PROT_CFG	0x1374		//GF20 Protection
+#define GF40_PROT_CFG	0x1378		//GR40 Protection
+#define EXP_CTS_TIME	0x137C		//
+#define EXP_ACK_TIME	0x1380		//
+
+//
+//  4.4 MAC RX configuration registers (offset:0x1400)
+//
+#define RX_FILTR_CFG	0x1400			//TXRX_CSR0
+#define AUTO_RSP_CFG	0x1404			//TXRX_CSR4
+//
+// TXRX_CSR4: Auto-Responder/
+//
+#ifdef RT_BIG_ENDIAN
+typedef union _AUTO_RSP_CFG_STRUC {
+ struct {
+     UINT32        :24;
+     UINT32       AckCtsPsmBit:1;   // Power bit value in conrtrol frame
+     UINT32       DualCTSEn:1;   // Power bit value in conrtrol frame
+     UINT32       rsv:1;   // Power bit value in conrtrol frame
+     UINT32       AutoResponderPreamble:1;    // 0:long, 1:short preamble
+     UINT32       CTS40MRef:1;  // Response CTS 40MHz duplicate mode
+     UINT32       CTS40MMode:1;  // Response CTS 40MHz duplicate mode
+     UINT32       BACAckPolicyEnable:1;    // 0:long, 1:short preamble
+     UINT32       AutoResponderEnable:1;
+ } field;
+ UINT32   word;
+} AUTO_RSP_CFG_STRUC, *PAUTO_RSP_CFG_STRUC;
+#else
+typedef union _AUTO_RSP_CFG_STRUC {
+ struct {
+     UINT32       AutoResponderEnable:1;
+     UINT32       BACAckPolicyEnable:1;    // 0:long, 1:short preamble
+     UINT32       CTS40MMode:1;  // Response CTS 40MHz duplicate mode
+     UINT32       CTS40MRef:1;  // Response CTS 40MHz duplicate mode
+     UINT32       AutoResponderPreamble:1;    // 0:long, 1:short preamble
+     UINT32       rsv:1;   // Power bit value in conrtrol frame
+     UINT32       DualCTSEn:1;   // Power bit value in conrtrol frame
+     UINT32       AckCtsPsmBit:1;   // Power bit value in conrtrol frame
+     UINT32        :24;
+ } field;
+ UINT32   word;
+} AUTO_RSP_CFG_STRUC, *PAUTO_RSP_CFG_STRUC;
+#endif
+
+#define LEGACY_BASIC_RATE	0x1408	//  TXRX_CSR5           0x3054
+#define HT_BASIC_RATE		0x140c
+#define HT_CTRL_CFG		0x1410
+#define SIFS_COST_CFG		0x1414
+#define RX_PARSER_CFG		0x1418	//Set NAV for all received frames
+
+//
+//  4.5 MAC Security configuration (offset:0x1500)
+//
+#define TX_SEC_CNT0		0x1500		//
+#define RX_SEC_CNT0		0x1504		//
+#define CCMP_FC_MUTE		0x1508		//
+//
+//  4.6 HCCA/PSMP (offset:0x1600)
+//
+#define TXOP_HLDR_ADDR0		0x1600
+#define TXOP_HLDR_ADDR1		0x1604
+#define TXOP_HLDR_ET		0x1608
+#define QOS_CFPOLL_RA_DW0		0x160c
+#define QOS_CFPOLL_A1_DW1		0x1610
+#define QOS_CFPOLL_QC		0x1614
+//
+//  4.7 MAC Statistis registers (offset:0x1700)
+//
+#define RX_STA_CNT0		0x1700		//
+#define RX_STA_CNT1		0x1704		//
+#define RX_STA_CNT2		0x1708		//
+
+//
+// RX_STA_CNT0_STRUC: RX PLCP error count & RX CRC error count
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_RX_STA_CNT0_STRUC	{
+	struct	{
+	    USHORT  PhyErr;
+	    USHORT  CrcErr;
+	}	field;
+	UINT32			word;
+}	RX_STA_CNT0_STRUC, *PRX_STA_CNT0_STRUC;
+#else
+typedef	union	_RX_STA_CNT0_STRUC	{
+	struct	{
+	    USHORT  CrcErr;
+	    USHORT  PhyErr;
+	}	field;
+	UINT32			word;
+}	RX_STA_CNT0_STRUC, *PRX_STA_CNT0_STRUC;
+#endif
+
+//
+// RX_STA_CNT1_STRUC: RX False CCA count & RX LONG frame count
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_RX_STA_CNT1_STRUC	{
+	struct	{
+	    USHORT  PlcpErr;
+	    USHORT  FalseCca;
+	}	field;
+	UINT32			word;
+}	RX_STA_CNT1_STRUC, *PRX_STA_CNT1_STRUC;
+#else
+typedef	union	_RX_STA_CNT1_STRUC	{
+	struct	{
+	    USHORT  FalseCca;
+	    USHORT  PlcpErr;
+	}	field;
+	UINT32			word;
+}	RX_STA_CNT1_STRUC, *PRX_STA_CNT1_STRUC;
+#endif
+
+//
+// RX_STA_CNT2_STRUC:
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_RX_STA_CNT2_STRUC	{
+	struct	{
+	    USHORT  RxFifoOverflowCount;
+	    USHORT  RxDupliCount;
+	}	field;
+	UINT32			word;
+}	RX_STA_CNT2_STRUC, *PRX_STA_CNT2_STRUC;
+#else
+typedef	union	_RX_STA_CNT2_STRUC	{
+	struct	{
+	    USHORT  RxDupliCount;
+	    USHORT  RxFifoOverflowCount;
+	}	field;
+	UINT32			word;
+}	RX_STA_CNT2_STRUC, *PRX_STA_CNT2_STRUC;
+#endif
+#define TX_STA_CNT0		0x170C		//
+//
+// STA_CSR3: TX Beacon count
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_STA_CNT0_STRUC	{
+	struct	{
+	    USHORT  TxBeaconCount;
+	    USHORT  TxFailCount;
+	}	field;
+	UINT32			word;
+}	TX_STA_CNT0_STRUC, *PTX_STA_CNT0_STRUC;
+#else
+typedef	union	_TX_STA_CNT0_STRUC	{
+	struct	{
+	    USHORT  TxFailCount;
+	    USHORT  TxBeaconCount;
+	}	field;
+	UINT32			word;
+}	TX_STA_CNT0_STRUC, *PTX_STA_CNT0_STRUC;
+#endif
+#define TX_STA_CNT1		0x1710		//
+//
+// TX_STA_CNT1: TX tx count
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_STA_CNT1_STRUC	{
+	struct	{
+	    USHORT  TxRetransmit;
+	    USHORT  TxSuccess;
+	}	field;
+	UINT32			word;
+}	TX_STA_CNT1_STRUC, *PTX_STA_CNT1_STRUC;
+#else
+typedef	union	_TX_STA_CNT1_STRUC	{
+	struct	{
+	    USHORT  TxSuccess;
+	    USHORT  TxRetransmit;
+	}	field;
+	UINT32			word;
+}	TX_STA_CNT1_STRUC, *PTX_STA_CNT1_STRUC;
+#endif
+#define TX_STA_CNT2		0x1714		//
+//
+// TX_STA_CNT2: TX tx count
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_STA_CNT2_STRUC	{
+	struct	{
+	    USHORT  TxUnderFlowCount;
+	    USHORT  TxZeroLenCount;
+	}	field;
+	UINT32			word;
+}	TX_STA_CNT2_STRUC, *PTX_STA_CNT2_STRUC;
+#else
+typedef	union	_TX_STA_CNT2_STRUC	{
+	struct	{
+	    USHORT  TxZeroLenCount;
+	    USHORT  TxUnderFlowCount;
+	}	field;
+	UINT32			word;
+}	TX_STA_CNT2_STRUC, *PTX_STA_CNT2_STRUC;
+#endif
+#define TX_STA_FIFO		0x1718		//
+//
+// TX_STA_FIFO_STRUC: TX Result for specific PID status fifo register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union PACKED _TX_STA_FIFO_STRUC	{
+	struct	{
+		UINT32		Reserve:2;
+		UINT32		TxBF:1; // 3*3
+		UINT32		SuccessRate:13;	//include MCS, mode ,shortGI, BW settingSame format as TXWI Word 0 Bit 31-16.
+//		UINT32		SuccessRate:16;	//include MCS, mode ,shortGI, BW settingSame format as TXWI Word 0 Bit 31-16.
+		UINT32		wcid:8;		//wireless client index
+		UINT32       	TxAckRequired:1;    // ack required
+		UINT32       	TxAggre:1;    // Tx is aggregated
+		UINT32       	TxSuccess:1;   // Tx success. whether success or not
+		UINT32       	PidType:4;
+		UINT32       	bValid:1;   // 1:This register contains a valid TX result
+	}	field;
+	UINT32			word;
+}	TX_STA_FIFO_STRUC, *PTX_STA_FIFO_STRUC;
+#else
+typedef	union PACKED _TX_STA_FIFO_STRUC	{
+	struct	{
+		UINT32       	bValid:1;   // 1:This register contains a valid TX result
+		UINT32       	PidType:4;
+		UINT32       	TxSuccess:1;   // Tx No retry success
+		UINT32       	TxAggre:1;    // Tx Retry Success
+		UINT32       	TxAckRequired:1;    // Tx fail
+		UINT32		wcid:8;		//wireless client index
+//		UINT32		SuccessRate:16;	//include MCS, mode ,shortGI, BW settingSame format as TXWI Word 0 Bit 31-16.
+		UINT32		SuccessRate:13;	//include MCS, mode ,shortGI, BW settingSame format as TXWI Word 0 Bit 31-16.
+		UINT32		TxBF:1;
+		UINT32		Reserve:2;
+	}	field;
+	UINT32			word;
+}	TX_STA_FIFO_STRUC, *PTX_STA_FIFO_STRUC;
+#endif
+// Debug counter
+#define TX_AGG_CNT	0x171c
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_AGG_CNT_STRUC	{
+	struct	{
+	    USHORT  AggTxCount;
+	    USHORT  NonAggTxCount;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT_STRUC, *PTX_AGG_CNT_STRUC;
+#else
+typedef	union	_TX_AGG_CNT_STRUC	{
+	struct	{
+	    USHORT  NonAggTxCount;
+	    USHORT  AggTxCount;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT_STRUC, *PTX_AGG_CNT_STRUC;
+#endif
+// Debug counter
+#define TX_AGG_CNT0	0x1720
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_AGG_CNT0_STRUC	{
+	struct	{
+	    USHORT  AggSize2Count;
+	    USHORT  AggSize1Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT0_STRUC, *PTX_AGG_CNT0_STRUC;
+#else
+typedef	union	_TX_AGG_CNT0_STRUC	{
+	struct	{
+	    USHORT  AggSize1Count;
+	    USHORT  AggSize2Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT0_STRUC, *PTX_AGG_CNT0_STRUC;
+#endif
+// Debug counter
+#define TX_AGG_CNT1	0x1724
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_AGG_CNT1_STRUC	{
+	struct	{
+	    USHORT  AggSize4Count;
+	    USHORT  AggSize3Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT1_STRUC, *PTX_AGG_CNT1_STRUC;
+#else
+typedef	union	_TX_AGG_CNT1_STRUC	{
+	struct	{
+	    USHORT  AggSize3Count;
+	    USHORT  AggSize4Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT1_STRUC, *PTX_AGG_CNT1_STRUC;
+#endif
+#define TX_AGG_CNT2	0x1728
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_AGG_CNT2_STRUC	{
+	struct	{
+	    USHORT  AggSize6Count;
+	    USHORT  AggSize5Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT2_STRUC, *PTX_AGG_CNT2_STRUC;
+#else
+typedef	union	_TX_AGG_CNT2_STRUC	{
+	struct	{
+	    USHORT  AggSize5Count;
+	    USHORT  AggSize6Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT2_STRUC, *PTX_AGG_CNT2_STRUC;
+#endif
+// Debug counter
+#define TX_AGG_CNT3	0x172c
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_AGG_CNT3_STRUC	{
+	struct	{
+	    USHORT  AggSize8Count;
+	    USHORT  AggSize7Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT3_STRUC, *PTX_AGG_CNT3_STRUC;
+#else
+typedef	union	_TX_AGG_CNT3_STRUC	{
+	struct	{
+	    USHORT  AggSize7Count;
+	    USHORT  AggSize8Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT3_STRUC, *PTX_AGG_CNT3_STRUC;
+#endif
+// Debug counter
+#define TX_AGG_CNT4	0x1730
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_AGG_CNT4_STRUC	{
+	struct	{
+	    USHORT  AggSize10Count;
+	    USHORT  AggSize9Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT4_STRUC, *PTX_AGG_CNT4_STRUC;
+#else
+typedef	union	_TX_AGG_CNT4_STRUC	{
+	struct	{
+	    USHORT  AggSize9Count;
+	    USHORT  AggSize10Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT4_STRUC, *PTX_AGG_CNT4_STRUC;
+#endif
+#define TX_AGG_CNT5	0x1734
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_AGG_CNT5_STRUC	{
+	struct	{
+	    USHORT  AggSize12Count;
+	    USHORT  AggSize11Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT5_STRUC, *PTX_AGG_CNT5_STRUC;
+#else
+typedef	union	_TX_AGG_CNT5_STRUC	{
+	struct	{
+	    USHORT  AggSize11Count;
+	    USHORT  AggSize12Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT5_STRUC, *PTX_AGG_CNT5_STRUC;
+#endif
+#define TX_AGG_CNT6		0x1738
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_AGG_CNT6_STRUC	{
+	struct	{
+	    USHORT  AggSize14Count;
+	    USHORT  AggSize13Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT6_STRUC, *PTX_AGG_CNT6_STRUC;
+#else
+typedef	union	_TX_AGG_CNT6_STRUC	{
+	struct	{
+	    USHORT  AggSize13Count;
+	    USHORT  AggSize14Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT6_STRUC, *PTX_AGG_CNT6_STRUC;
+#endif
+#define TX_AGG_CNT7		0x173c
+#ifdef RT_BIG_ENDIAN
+typedef	union	_TX_AGG_CNT7_STRUC	{
+	struct	{
+	    USHORT  AggSize16Count;
+	    USHORT  AggSize15Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT7_STRUC, *PTX_AGG_CNT7_STRUC;
+#else
+typedef	union	_TX_AGG_CNT7_STRUC	{
+	struct	{
+	    USHORT  AggSize15Count;
+	    USHORT  AggSize16Count;
+	}	field;
+	UINT32			word;
+}	TX_AGG_CNT7_STRUC, *PTX_AGG_CNT7_STRUC;
+#endif
+#define MPDU_DENSITY_CNT		0x1740
+#ifdef RT_BIG_ENDIAN
+typedef	union	_MPDU_DEN_CNT_STRUC	{
+	struct	{
+	    USHORT  RXZeroDelCount;	//RX zero length delimiter count
+	    USHORT  TXZeroDelCount;	//TX zero length delimiter count
+	}	field;
+	UINT32			word;
+}	MPDU_DEN_CNT_STRUC, *PMPDU_DEN_CNT_STRUC;
+#else
+typedef	union	_MPDU_DEN_CNT_STRUC	{
+	struct	{
+	    USHORT  TXZeroDelCount;	//TX zero length delimiter count
+	    USHORT  RXZeroDelCount;	//RX zero length delimiter count
+	}	field;
+	UINT32			word;
+}	MPDU_DEN_CNT_STRUC, *PMPDU_DEN_CNT_STRUC;
+#endif
+//
+// TXRX control registers - base address 0x3000
+//
+// rt2860b  UNKNOWN reg use R/O Reg Addr 0x77d0 first..
+#define TXRX_CSR1           0x77d0
+
+//
+// Security key table memory, base address = 0x1000
+//
+#define MAC_WCID_BASE		0x1800 //8-bytes(use only 6-bytes) * 256 entry =
+#define HW_WCID_ENTRY_SIZE   8
+#define PAIRWISE_KEY_TABLE_BASE     0x4000      // 32-byte * 256-entry =  -byte
+#define HW_KEY_ENTRY_SIZE           0x20
+#define PAIRWISE_IVEIV_TABLE_BASE     0x6000      // 8-byte * 256-entry =  -byte
+#define MAC_IVEIV_TABLE_BASE     0x6000      // 8-byte * 256-entry =  -byte
+#define HW_IVEIV_ENTRY_SIZE   8
+#define MAC_WCID_ATTRIBUTE_BASE     0x6800      // 4-byte * 256-entry =  -byte
+#define HW_WCID_ATTRI_SIZE   4
+#define WCID_RESERVED          		0x6bfc
+#define SHARED_KEY_TABLE_BASE       0x6c00      // 32-byte * 16-entry = 512-byte
+#define SHARED_KEY_MODE_BASE       0x7000      // 32-byte * 16-entry = 512-byte
+#define HW_SHARED_KEY_MODE_SIZE   4
+#define SHAREDKEYTABLE			0
+#define PAIRWISEKEYTABLE			1
+
+
+#ifdef RT_BIG_ENDIAN
+typedef	union	_SHAREDKEY_MODE_STRUC	{
+	struct	{
+		UINT32       :1;
+		UINT32       Bss1Key3CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss1Key2CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss1Key1CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss1Key0CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss0Key3CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss0Key2CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss0Key1CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss0Key0CipherAlg:3;
+	}	field;
+	UINT32			word;
+}	SHAREDKEY_MODE_STRUC, *PSHAREDKEY_MODE_STRUC;
+#else
+typedef	union	_SHAREDKEY_MODE_STRUC	{
+	struct	{
+		UINT32       Bss0Key0CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss0Key1CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss0Key2CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss0Key3CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss1Key0CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss1Key1CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss1Key2CipherAlg:3;
+		UINT32       :1;
+		UINT32       Bss1Key3CipherAlg:3;
+		UINT32       :1;
+	}	field;
+	UINT32			word;
+}	SHAREDKEY_MODE_STRUC, *PSHAREDKEY_MODE_STRUC;
+#endif
+// 64-entry for pairwise key table
+typedef struct _HW_WCID_ENTRY {  // 8-byte per entry
+    UCHAR   Address[6];
+    UCHAR   Rsv[2];
+} HW_WCID_ENTRY, PHW_WCID_ENTRY;
+
+
+
+//
+// Other on-chip shared memory space, base = 0x2000
+//
+
+// CIS space - base address = 0x2000
+#define HW_CIS_BASE             0x2000
+
+// Carrier-sense CTS frame base address. It's where mac stores carrier-sense frame for carrier-sense function.
+#define HW_CS_CTS_BASE			0x7700
+// DFS CTS frame base address. It's where mac stores CTS frame for DFS.
+#define HW_DFS_CTS_BASE			0x7780
+#define HW_CTS_FRAME_SIZE		0x80
+
+// 2004-11-08 john - since NULL frame won't be that long (256 byte). We steal 16 tail bytes
+// to save debugging settings
+#define HW_DEBUG_SETTING_BASE   0x77f0  // 0x77f0~0x77ff total 16 bytes
+#define HW_DEBUG_SETTING_BASE2   0x7770  // 0x77f0~0x77ff total 16 bytes
+
+#if 0
+// on-chip BEACON frame space - base address = 0x7800
+#define HW_BEACON_MAX_SIZE      0x0800 /* unit: byte */
+#define HW_BEACON_BASE0         0x7800
+#define HW_BEACON_BASE1         0x7900
+#define HW_BEACON_BASE2         0x7a00
+#define HW_BEACON_BASE3         0x7b00
+#define HW_BEACON_BASE4         0x7c00
+#define HW_BEACON_BASE5         0x7d00
+#define HW_BEACON_BASE6         0x7e00
+#define HW_BEACON_BASE7         0x7f00
+/* 1. HW_BEACON_OFFSET/64B must be 0;
+   2. BCN_OFFSET0 must also be changed in NICInitializeAsic();
+   3. max 0x0800 for 8 beacon frames; */
+#else
+// In order to support maximum 8 MBSS and its maximum length is 512 for each beacon
+// Three section discontinue memory segments will be used.
+// 1. The original region for BCN 0~3
+// 2. Extract memory from FCE table for BCN 4~5
+// 3. Extract memory from Pair-wise key table for BCN 6~7
+//	  It occupied those memory of wcid 238~253 for BCN 6
+//						      and wcid 222~237 for BCN 7
+#define HW_BEACON_MAX_SIZE      0x1000 /* unit: byte */
+#define HW_BEACON_BASE0         0x7800
+#define HW_BEACON_BASE1         0x7A00
+#define HW_BEACON_BASE2         0x7C00
+#define HW_BEACON_BASE3         0x7E00
+#define HW_BEACON_BASE4         0x7200
+#define HW_BEACON_BASE5         0x7400
+#define HW_BEACON_BASE6         0x5DC0
+#define HW_BEACON_BASE7         0x5BC0
+#endif
+
+#define HW_BEACON_MAX_COUNT     8
+#define HW_BEACON_OFFSET		0x0200
+#define HW_BEACON_CONTENT_LEN	(HW_BEACON_OFFSET - TXWI_SIZE)
+
+// HOST-MCU shared memory - base address = 0x2100
+#define HOST_CMD_CSR		0x404
+#define H2M_MAILBOX_CSR         0x7010
+#define H2M_MAILBOX_CID         0x7014
+#define H2M_MAILBOX_STATUS      0x701c
+#define H2M_INT_SRC             0x7024
+#define H2M_BBP_AGENT           0x7028
+#define M2H_CMD_DONE_CSR        0x000c
+#define MCU_TXOP_ARRAY_BASE     0x000c   // TODO: to be provided by Albert
+#define MCU_TXOP_ENTRY_SIZE     32       // TODO: to be provided by Albert
+#define MAX_NUM_OF_TXOP_ENTRY   16       // TODO: must be same with 8051 firmware
+#define MCU_MBOX_VERSION        0x01     // TODO: to be confirmed by Albert
+#define MCU_MBOX_VERSION_OFFSET 5        // TODO: to be provided by Albert
+
+//
+// Host DMA registers - base address 0x200 .  TX0-3=EDCAQid0-3, TX4=HCCA, TX5=MGMT,
+//
+//
+//  DMA RING DESCRIPTOR
+//
+#define E2PROM_CSR          0x0004
+#define IO_CNTL_CSR         0x77d0
+
+#ifdef RT2870
+// 8051 firmware image for usb - use last-half base address = 0x3000
+#define FIRMWARE_IMAGE_BASE     0x3000
+#define MAX_FIRMWARE_IMAGE_SIZE 0x1000    // 4kbyte
+#endif // RT2870 //
+
+// TODO: ????? old RT2560 registers. to keep them or remove them?
+//#define MCAST0                  0x0178  // multicast filter register 0
+//#define MCAST1                  0x017c  // multicast filter register 1
+
+
+// ================================================================
+// Tx /	Rx / Mgmt ring descriptor definition
+// ================================================================
+
+// the following PID values are used to mark outgoing frame type in TXD->PID so that
+// proper TX statistics can be collected based on these categories
+// b3-2 of PID field -
+#define PID_MGMT			0x05
+#define PID_BEACON			0x0c
+#define PID_DATA_NORMALUCAST	 	0x02
+#define PID_DATA_AMPDU	 	0x04
+#define PID_DATA_NO_ACK    	0x08
+#define PID_DATA_NOT_NORM_ACK	 	0x03
+#if 0
+#define PTYPE_DATA_REQUIRE_ACK  0x00 // b7-6:00, b5-0: 0~59 is MAC table index (AID?), 60~63 is WDS index
+#define PTYPE_NULL_AT_HIGH_RATE 0x04 // b7-6:01, b5-0: 0~59 is MAC table index (AID?), 60~63 is WDS index
+#define PTYPE_RESERVED          0x08 // b7-6:10
+#define PTYPE_SPECIAL           0x0c // b7-6:11
+
+// when b3-2=11 (PTYPE_SPECIAL), b1-0 coube be ...
+#define PSUBTYPE_DATA_NO_ACK    0x00
+#define PSUBTYPE_MGMT           0x01
+#define PSUBTYPE_OTHER_CNTL     0x02
+#define PSUBTYPE_RTS            0x03
+#endif
+// value domain of pTxD->HostQId (4-bit: 0~15)
+#define QID_AC_BK               1   // meet ACI definition in 802.11e
+#define QID_AC_BE               0   // meet ACI definition in 802.11e
+#define QID_AC_VI               2
+#define QID_AC_VO               3
+#define QID_HCCA                4
+#define NUM_OF_TX_RING          5
+#define QID_MGMT                13
+#define QID_RX                  14
+#define QID_OTHER               15
+
+
+// ------------------------------------------------------
+// BBP & RF	definition
+// ------------------------------------------------------
+#define	BUSY		                1
+#define	IDLE		                0
+
+#define	RF_R00					    0
+#define	RF_R01					    1
+#define	RF_R02					    2
+#define	RF_R03					    3
+#define	RF_R04					    4
+#define	RF_R05					    5
+#define	RF_R06					    6
+#define	RF_R07					    7
+#define	RF_R08					    8
+#define	RF_R09					    9
+#define	RF_R10					    10
+#define	RF_R11					    11
+#define	RF_R12					    12
+#define	RF_R13					    13
+#define	RF_R14					    14
+#define	RF_R15					    15
+#define	RF_R16					    16
+#define	RF_R17					    17
+#define	RF_R18					    18
+#define	RF_R19					    19
+#define	RF_R20					    20
+#define	RF_R21					    21
+#define	RF_R22					    22
+#define	RF_R23					    23
+#define	RF_R24					    24
+#define	RF_R25					    25
+#define	RF_R26					    26
+#define	RF_R27					    27
+#define	RF_R28					    28
+#define	RF_R29					    29
+#define	RF_R30					    30
+#define	RF_R31					    31
+
+#define	BBP_R0					    0  // version
+#define	BBP_R1				        1  // TSSI
+#define	BBP_R2          			2  // TX configure
+#define BBP_R3                      3
+#define BBP_R4                      4
+#define BBP_R5                      5
+#define BBP_R6                      6
+#define	BBP_R14			            14 // RX configure
+#define BBP_R16                     16
+#define BBP_R17                     17 // RX sensibility
+#define BBP_R18                     18
+#define BBP_R21                     21
+#define BBP_R22                     22
+#define BBP_R24                     24
+#define BBP_R25                     25
+#define BBP_R49                     49 //TSSI
+#define BBP_R50                     50
+#define BBP_R51                     51
+#define BBP_R52                     52
+#define BBP_R55                     55
+#define BBP_R62                     62 // Rx SQ0 Threshold HIGH
+#define BBP_R63                     63
+#define BBP_R64                     64
+#define BBP_R65                     65
+#define BBP_R66                     66
+#define BBP_R67                     67
+#define BBP_R68                     68
+#define BBP_R69                     69
+#define BBP_R70                     70 // Rx AGC SQ CCK Xcorr threshold
+#define BBP_R73                     73
+#define BBP_R75						75
+#define BBP_R77                     77
+#define BBP_R81                     81
+#define BBP_R82                     82
+#define BBP_R83                     83
+#define BBP_R84                     84
+#define BBP_R86						86
+#define BBP_R91						91
+#define BBP_R92						92
+#define BBP_R94                     94 // Tx Gain Control
+#define BBP_R103                    103
+#define BBP_R105                    105
+#define BBP_R113                    113
+#define BBP_R114                    114
+#define BBP_R115                    115
+#define BBP_R116                    116
+#define BBP_R117                    117
+#define BBP_R118                    118
+#define BBP_R119                    119
+#define BBP_R120                    120
+#define BBP_R121                    121
+#define BBP_R122                    122
+#define BBP_R123                    123
+
+
+#define BBPR94_DEFAULT              0x06 // Add 1 value will gain 1db
+
+//#define PHY_TR_SWITCH_TIME          5  // usec
+
+//#define BBP_R17_LOW_SENSIBILITY     0x50
+//#define BBP_R17_MID_SENSIBILITY     0x41
+//#define BBP_R17_DYNAMIC_UP_BOUND    0x40
+#define RSSI_FOR_VERY_LOW_SENSIBILITY -35
+#define RSSI_FOR_LOW_SENSIBILITY      -58
+#define RSSI_FOR_MID_LOW_SENSIBILITY  -80
+#define RSSI_FOR_MID_SENSIBILITY      -90
+
+//-------------------------------------------------------------------------
+// EEPROM definition
+//-------------------------------------------------------------------------
+#define EEDO                        0x08
+#define EEDI                        0x04
+#define EECS                        0x02
+#define EESK                        0x01
+#define EERL                        0x80
+
+#define EEPROM_WRITE_OPCODE         0x05
+#define EEPROM_READ_OPCODE          0x06
+#define EEPROM_EWDS_OPCODE          0x10
+#define EEPROM_EWEN_OPCODE          0x13
+
+#define	NUM_EEPROM_BBP_PARMS		19			// Include NIC Config 0, 1, CR, TX ALC step, BBPs
+#define	NUM_EEPROM_TX_G_PARMS		7
+#define	EEPROM_NIC1_OFFSET          0x34		// The address is from NIC config 0, not BBP register ID
+#define	EEPROM_NIC2_OFFSET          0x36		// The address is from NIC config 0, not BBP register ID
+#define	EEPROM_BBP_BASE_OFFSET		0xf0		// The address is from NIC config 0, not BBP register ID
+#define	EEPROM_G_TX_PWR_OFFSET		0x52
+#define	EEPROM_G_TX2_PWR_OFFSET		0x60
+#define EEPROM_LED1_OFFSET			0x3c
+#define EEPROM_LED2_OFFSET			0x3e
+#define EEPROM_LED3_OFFSET			0x40
+#define EEPROM_LNA_OFFSET			0x44
+#define EEPROM_RSSI_BG_OFFSET		0x46
+#define EEPROM_RSSI_A_OFFSET		0x4a
+#define EEPROM_DEFINE_MAX_TXPWR		0x4e
+#define EEPROM_TXPOWER_BYRATE_20MHZ_2_4G	0xde	// 20MHZ 2.4G tx power.
+#define EEPROM_TXPOWER_BYRATE_40MHZ_2_4G	0xee	// 40MHZ 2.4G tx power.
+#define EEPROM_TXPOWER_BYRATE_20MHZ_5G		0xfa	// 20MHZ 5G tx power.
+#define EEPROM_TXPOWER_BYRATE_40MHZ_5G		0x10a	// 40MHZ 5G tx power.
+#define EEPROM_A_TX_PWR_OFFSET      0x78
+#define EEPROM_A_TX2_PWR_OFFSET      0xa6
+//#define EEPROM_Japan_TX_PWR_OFFSET      0x90 // 802.11j
+//#define EEPROM_Japan_TX2_PWR_OFFSET      0xbe
+//#define EEPROM_TSSI_REF_OFFSET	0x54
+//#define EEPROM_TSSI_DELTA_OFFSET	0x24
+//#define EEPROM_CCK_TX_PWR_OFFSET  0x62
+//#define EEPROM_CALIBRATE_OFFSET	0x7c
+#define EEPROM_VERSION_OFFSET       0x02
+#define	EEPROM_FREQ_OFFSET			0x3a
+#define EEPROM_TXPOWER_BYRATE 	0xde	// 20MHZ power.
+#define EEPROM_TXPOWER_DELTA		0x50	// 20MHZ AND 40 MHZ use different power. This is delta in 40MHZ.
+#define VALID_EEPROM_VERSION        1
+
+// PairKeyMode definition
+#define PKMODE_NONE                 0
+#define PKMODE_WEP64                1
+#define PKMODE_WEP128               2
+#define PKMODE_TKIP                 3
+#define PKMODE_AES                  4
+#define PKMODE_CKIP64               5
+#define PKMODE_CKIP128              6
+#define PKMODE_TKIP_NO_MIC          7       // MIC appended by driver: not a valid value in hardware key table
+
+// =================================================================================
+// WCID  format
+// =================================================================================
+//7.1	WCID  ENTRY  format  : 8bytes
+typedef	struct	_WCID_ENTRY_STRUC {
+	UCHAR		RXBABitmap7;    // bit0 for TID8, bit7 for TID 15
+	UCHAR		RXBABitmap0;    // bit0 for TID0, bit7 for TID 7
+	UCHAR		MAC[6];	// 0 for shared key table.  1 for pairwise key table
+}	WCID_ENTRY_STRUC, *PWCID_ENTRY_STRUC;
+
+//8.1.1	SECURITY  KEY  format  : 8DW
+// 32-byte per entry, total 16-entry for shared key table, 64-entry for pairwise key table
+typedef struct _HW_KEY_ENTRY {          // 32-byte per entry
+    UCHAR   Key[16];
+    UCHAR   TxMic[8];
+    UCHAR   RxMic[8];
+} HW_KEY_ENTRY, *PHW_KEY_ENTRY;
+
+//8.1.2	IV/EIV  format  : 2DW
+
+//8.1.3	RX attribute entry format  : 1DW
+#ifdef RT_BIG_ENDIAN
+typedef	struct	_MAC_ATTRIBUTE_STRUC {
+	UINT32		rsv:22;
+	UINT32		RXWIUDF:3;
+	UINT32		BSSIDIdx:3; //multipleBSS index for the WCID
+	UINT32		PairKeyMode:3;
+	UINT32		KeyTab:1;	// 0 for shared key table.  1 for pairwise key table
+}	MAC_ATTRIBUTE_STRUC, *PMAC_ATTRIBUTE_STRUC;
+#else
+typedef	struct	_MAC_ATTRIBUTE_STRUC {
+	UINT32		KeyTab:1;	// 0 for shared key table.  1 for pairwise key table
+	UINT32		PairKeyMode:3;
+	UINT32		BSSIDIdx:3; //multipleBSS index for the WCID
+	UINT32		RXWIUDF:3;
+	UINT32		rsv:22;
+}	MAC_ATTRIBUTE_STRUC, *PMAC_ATTRIBUTE_STRUC;
+#endif
+
+
+// =================================================================================
+// TX / RX ring descriptor format
+// =================================================================================
+
+// the first 24-byte in TXD is called TXINFO and will be DMAed to MAC block through TXFIFO.
+// MAC block use this TXINFO to control the transmission behavior of this frame.
+#define FIFO_MGMT                 0
+#define FIFO_HCCA                 1
+#define FIFO_EDCA                 2
+
+//
+// TX descriptor format, Tx	ring, Mgmt Ring
+//
+#ifdef RT_BIG_ENDIAN
+typedef	struct	PACKED _TXD_STRUC {
+	// Word 0
+	UINT32		SDPtr0;
+	// Word 1
+	UINT32		DMADONE:1;
+	UINT32		LastSec0:1;
+	UINT32		SDLen0:14;
+	UINT32		Burst:1;
+	UINT32		LastSec1:1;
+	UINT32		SDLen1:14;
+	// Word 2
+	UINT32		SDPtr1;
+	// Word 3
+	UINT32		ICO:1;
+	UINT32		UCO:1;
+	UINT32		TCO:1;
+	UINT32		rsv:2;
+	UINT32		QSEL:2;	// select on-chip FIFO ID for 2nd-stage output scheduler.0:MGMT, 1:HCCA 2:EDCA
+	UINT32		WIV:1;	// Wireless Info Valid. 1 if Driver already fill WI,  o if DMA needs to copy WI to correctposition
+	UINT32		rsv2:24;
+}	TXD_STRUC, *PTXD_STRUC;
+#else
+typedef	struct	PACKED _TXD_STRUC {
+	// Word	0
+	UINT32		SDPtr0;
+	// Word	1
+	UINT32		SDLen1:14;
+	UINT32		LastSec1:1;
+	UINT32		Burst:1;
+	UINT32		SDLen0:14;
+	UINT32		LastSec0:1;
+	UINT32		DMADONE:1;
+	//Word2
+	UINT32		SDPtr1;
+	//Word3
+	UINT32		rsv2:24;
+	UINT32		WIV:1;	// Wireless Info Valid. 1 if Driver already fill WI,  o if DMA needs to copy WI to correctposition
+	UINT32		QSEL:2;	// select on-chip FIFO ID for 2nd-stage output scheduler.0:MGMT, 1:HCCA 2:EDCA
+	UINT32		rsv:2;
+	UINT32		TCO:1;	//
+	UINT32		UCO:1;	//
+	UINT32		ICO:1;	//
+}	TXD_STRUC, *PTXD_STRUC;
+#endif
+
+
+//
+// TXD Wireless Information format for Tx ring and Mgmt Ring
+//
+//txop : for txop mode
+// 0:txop for the MPDU frame will be handles by ASIC by register
+// 1/2/3:the MPDU frame is send after PIFS/backoff/SIFS
+#ifdef RT_BIG_ENDIAN
+typedef	struct	PACKED _TXWI_STRUC {
+	// Word 0
+	UINT32		PHYMODE:2;
+	UINT32		TxBF:1;	// 3*3
+	UINT32		rsv2:1;
+//	UINT32		rsv2:2;
+	UINT32		Ifs:1;	//
+	UINT32		STBC:2;	//channel bandwidth 20MHz or 40 MHz
+	UINT32		ShortGI:1;
+	UINT32		BW:1;	//channel bandwidth 20MHz or 40 MHz
+	UINT32		MCS:7;
+
+	UINT32		rsv:6;
+	UINT32		txop:2;	//tx back off mode 0:HT TXOP rule , 1:PIFS TX ,2:Backoff, 3:sifs only when previous frame exchange is successful.
+	UINT32		MpduDensity:3;
+	UINT32		AMPDU:1;
+
+	UINT32		TS:1;
+	UINT32		CFACK:1;
+	UINT32		MIMOps:1;	// the remote peer is in dynamic MIMO-PS mode
+	UINT32		FRAG:1;		// 1 to inform TKIP engine this is a fragment.
+	// Word 1
+	UINT32		PacketId:4;
+	UINT32		MPDUtotalByteCount:12;
+	UINT32		WirelessCliID:8;
+	UINT32		BAWinSize:6;
+	UINT32		NSEQ:1;
+	UINT32		ACK:1;
+	// Word 2
+	UINT32		IV;
+	// Word 3
+	UINT32		EIV;
+}	TXWI_STRUC, *PTXWI_STRUC;
+#else
+typedef	struct	PACKED _TXWI_STRUC {
+	// Word	0
+	UINT32		FRAG:1;		// 1 to inform TKIP engine this is a fragment.
+	UINT32		MIMOps:1;	// the remote peer is in dynamic MIMO-PS mode
+	UINT32		CFACK:1;
+	UINT32		TS:1;
+
+	UINT32		AMPDU:1;
+	UINT32		MpduDensity:3;
+	UINT32		txop:2;	//FOR "THIS" frame. 0:HT TXOP rule , 1:PIFS TX ,2:Backoff, 3:sifs only when previous frame exchange is successful.
+	UINT32		rsv:6;
+
+	UINT32		MCS:7;
+	UINT32		BW:1;	//channel bandwidth 20MHz or 40 MHz
+	UINT32		ShortGI:1;
+	UINT32		STBC:2;	// 1: STBC support MCS =0-7,   2,3 : RESERVE
+	UINT32		Ifs:1;	//
+//	UINT32		rsv2:2;	//channel bandwidth 20MHz or 40 MHz
+	UINT32		rsv2:1;
+	UINT32		TxBF:1;	// 3*3
+	UINT32		PHYMODE:2;
+	// Word	1
+	UINT32		ACK:1;
+	UINT32		NSEQ:1;
+	UINT32		BAWinSize:6;
+	UINT32		WirelessCliID:8;
+	UINT32		MPDUtotalByteCount:12;
+	UINT32		PacketId:4;
+	//Word2
+	UINT32		IV;
+	//Word3
+	UINT32		EIV;
+}	TXWI_STRUC, *PTXWI_STRUC;
+#endif
+//
+// Rx descriptor format, Rx	Ring
+//
+//
+// RXWI wireless information format, in PBF. invisible in driver.
+//
+#ifdef RT_BIG_ENDIAN
+typedef	struct	PACKED _RXWI_STRUC {
+	// Word 0
+	UINT32		TID:4;
+	UINT32		MPDUtotalByteCount:12;
+	UINT32		UDF:3;
+	UINT32		BSSID:3;
+	UINT32		KeyIndex:2;
+	UINT32		WirelessCliID:8;
+	// Word 1
+	UINT32		PHYMODE:2;              // 1: this RX frame is unicast to me
+	UINT32		rsv:3;
+	UINT32		STBC:2;
+	UINT32		ShortGI:1;
+	UINT32		BW:1;
+	UINT32		MCS:7;
+	UINT32		SEQUENCE:12;
+	UINT32		FRAG:4;
+	// Word 2
+	UINT32		rsv1:8;
+	UINT32		RSSI2:8;
+	UINT32		RSSI1:8;
+	UINT32		RSSI0:8;
+	// Word 3
+	UINT32		rsv2:16;
+	UINT32		SNR1:8;
+	UINT32		SNR0:8;
+}	RXWI_STRUC, *PRXWI_STRUC;
+#else
+typedef	struct	PACKED _RXWI_STRUC {
+	// Word	0
+	UINT32		WirelessCliID:8;
+	UINT32		KeyIndex:2;
+	UINT32		BSSID:3;
+	UINT32		UDF:3;
+	UINT32		MPDUtotalByteCount:12;
+	UINT32		TID:4;
+	// Word	1
+	UINT32		FRAG:4;
+	UINT32		SEQUENCE:12;
+	UINT32		MCS:7;
+	UINT32		BW:1;
+	UINT32		ShortGI:1;
+	UINT32		STBC:2;
+	UINT32		rsv:3;
+	UINT32		PHYMODE:2;              // 1: this RX frame is unicast to me
+	//Word2
+	UINT32		RSSI0:8;
+	UINT32		RSSI1:8;
+	UINT32		RSSI2:8;
+	UINT32		rsv1:8;
+	//Word3
+	UINT32		SNR0:8;
+	UINT32		SNR1:8;
+	UINT32		rsv2:16;
+}	RXWI_STRUC, *PRXWI_STRUC;
+#endif
+
+
+// =================================================================================
+// HOST-MCU communication data structure
+// =================================================================================
+
+//
+// H2M_MAILBOX_CSR: Host-to-MCU Mailbox
+//
+#ifdef RT_BIG_ENDIAN
+typedef union  _H2M_MAILBOX_STRUC {
+    struct {
+        UINT32       Owner:8;
+        UINT32       CmdToken:8;    // 0xff tells MCU not to report CmdDoneInt after excuting the command
+        UINT32       HighByte:8;
+        UINT32       LowByte:8;
+    }   field;
+    UINT32           word;
+} H2M_MAILBOX_STRUC, *PH2M_MAILBOX_STRUC;
+#else
+typedef union  _H2M_MAILBOX_STRUC {
+    struct {
+        UINT32       LowByte:8;
+        UINT32       HighByte:8;
+        UINT32       CmdToken:8;
+        UINT32       Owner:8;
+    }   field;
+    UINT32           word;
+} H2M_MAILBOX_STRUC, *PH2M_MAILBOX_STRUC;
+#endif
+
+//
+// M2H_CMD_DONE_CSR: MCU-to-Host command complete indication
+//
+#ifdef RT_BIG_ENDIAN
+typedef union _M2H_CMD_DONE_STRUC {
+    struct  {
+        UINT32       CmdToken3;
+        UINT32       CmdToken2;
+        UINT32       CmdToken1;
+        UINT32       CmdToken0;
+    } field;
+    UINT32           word;
+} M2H_CMD_DONE_STRUC, *PM2H_CMD_DONE_STRUC;
+#else
+typedef union _M2H_CMD_DONE_STRUC {
+    struct  {
+        UINT32       CmdToken0;
+        UINT32       CmdToken1;
+        UINT32       CmdToken2;
+        UINT32       CmdToken3;
+    } field;
+    UINT32           word;
+} M2H_CMD_DONE_STRUC, *PM2H_CMD_DONE_STRUC;
+#endif
+
+
+
+//
+// MCU_LEDCS: MCU LED Control Setting.
+//
+#ifdef RT_BIG_ENDIAN
+typedef union  _MCU_LEDCS_STRUC {
+	struct	{
+		UCHAR		Polarity:1;
+		UCHAR		LedMode:7;
+	} field;
+	UCHAR				word;
+} MCU_LEDCS_STRUC, *PMCU_LEDCS_STRUC;
+#else
+typedef union  _MCU_LEDCS_STRUC {
+	struct	{
+		UCHAR		LedMode:7;
+		UCHAR		Polarity:1;
+	} field;
+	UCHAR			word;
+} MCU_LEDCS_STRUC, *PMCU_LEDCS_STRUC;
+#endif
+// =================================================================================
+// Register format
+// =================================================================================
+
+
+
+//NAV_TIME_CFG :NAV
+#ifdef RT_BIG_ENDIAN
+typedef	union	_NAV_TIME_CFG_STRUC	{
+	struct	{
+		USHORT		rsv:6;
+		USHORT		ZeroSifs:1;               // Applied zero SIFS timer after OFDM RX 0: disable
+		USHORT		Eifs:9;               // in unit of 1-us
+		UCHAR       SlotTime;    // in unit of 1-us
+		UCHAR		Sifs;               // in unit of 1-us
+	}	field;
+	UINT32			word;
+}	NAV_TIME_CFG_STRUC, *PNAV_TIME_CFG_STRUC;
+#else
+typedef	union	_NAV_TIME_CFG_STRUC	{
+	struct	{
+		UCHAR		Sifs;               // in unit of 1-us
+		UCHAR       SlotTime;    // in unit of 1-us
+		USHORT		Eifs:9;               // in unit of 1-us
+		USHORT		ZeroSifs:1;               // Applied zero SIFS timer after OFDM RX 0: disable
+		USHORT		rsv:6;
+	}	field;
+	UINT32			word;
+}	NAV_TIME_CFG_STRUC, *PNAV_TIME_CFG_STRUC;
+#endif
+
+
+
+
+
+//
+// RX_FILTR_CFG:  /RX configuration register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	RX_FILTR_CFG_STRUC	{
+	struct	{
+		UINT32		:15;
+		UINT32       DropRsvCntlType:1;
+
+        	UINT32       	DropBAR:1;       //
+		UINT32		DropBA:1;		//
+		UINT32		DropPsPoll:1;		// Drop Ps-Poll
+		UINT32		DropRts:1;		// Drop Ps-Poll
+
+		UINT32		DropCts:1;		// Drop Ps-Poll
+		UINT32		DropAck:1;		// Drop Ps-Poll
+		UINT32		DropCFEnd:1;		// Drop Ps-Poll
+		UINT32		DropCFEndAck:1;		// Drop Ps-Poll
+
+		UINT32		DropDuplicate:1;		// Drop duplicate frame
+		UINT32		DropBcast:1;		// Drop broadcast frames
+		UINT32		DropMcast:1;		// Drop multicast frames
+		UINT32		DropVerErr:1;	    // Drop version error frame
+
+		UINT32		DropNotMyBSSID:1;			// Drop fram ToDs bit is true
+		UINT32		DropNotToMe:1;		// Drop not to me unicast frame
+		UINT32		DropPhyErr:1;		// Drop physical error
+		UINT32		DropCRCErr:1;		// Drop CRC error
+	}	field;
+	UINT32			word;
+}	RX_FILTR_CFG_STRUC, *PRX_FILTR_CFG_STRUC;
+#else
+typedef	union	_RX_FILTR_CFG_STRUC	{
+	struct	{
+		UINT32		DropCRCErr:1;		// Drop CRC error
+		UINT32		DropPhyErr:1;		// Drop physical error
+		UINT32		DropNotToMe:1;		// Drop not to me unicast frame
+		UINT32		DropNotMyBSSID:1;			// Drop fram ToDs bit is true
+
+		UINT32		DropVerErr:1;	    // Drop version error frame
+		UINT32		DropMcast:1;		// Drop multicast frames
+		UINT32		DropBcast:1;		// Drop broadcast frames
+		UINT32		DropDuplicate:1;		// Drop duplicate frame
+
+		UINT32		DropCFEndAck:1;		// Drop Ps-Poll
+		UINT32		DropCFEnd:1;		// Drop Ps-Poll
+		UINT32		DropAck:1;		// Drop Ps-Poll
+		UINT32		DropCts:1;		// Drop Ps-Poll
+
+		UINT32		DropRts:1;		// Drop Ps-Poll
+		UINT32		DropPsPoll:1;		// Drop Ps-Poll
+		UINT32		DropBA:1;		//
+        	UINT32       	DropBAR:1;       //
+
+		UINT32       	DropRsvCntlType:1;
+		UINT32		:15;
+	}	field;
+	UINT32			word;
+}	RX_FILTR_CFG_STRUC, *PRX_FILTR_CFG_STRUC;
+#endif
+
+
+
+
+//
+// PHY_CSR4: RF serial control register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_PHY_CSR4_STRUC	{
+	struct	{
+		UINT32		Busy:1;				// 1: ASIC is busy execute RF programming.
+		UINT32		PLL_LD:1;			// RF PLL_LD status
+		UINT32		IFSelect:1;			// 1: select IF	to program,	0: select RF to	program
+		UINT32		NumberOfBits:5;		// Number of bits used in RFRegValue (I:20,	RFMD:22)
+		UINT32		RFRegValue:24;		// Register	value (include register	id)	serial out to RF/IF	chip.
+	}	field;
+	UINT32			word;
+}	PHY_CSR4_STRUC, *PPHY_CSR4_STRUC;
+#else
+typedef	union	_PHY_CSR4_STRUC	{
+	struct	{
+		UINT32		RFRegValue:24;		// Register	value (include register	id)	serial out to RF/IF	chip.
+		UINT32		NumberOfBits:5;		// Number of bits used in RFRegValue (I:20,	RFMD:22)
+		UINT32		IFSelect:1;			// 1: select IF	to program,	0: select RF to	program
+		UINT32		PLL_LD:1;			// RF PLL_LD status
+		UINT32		Busy:1;				// 1: ASIC is busy execute RF programming.
+	}	field;
+	UINT32			word;
+}	PHY_CSR4_STRUC, *PPHY_CSR4_STRUC;
+#endif
+
+
+//
+// SEC_CSR5: shared key table security mode register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_SEC_CSR5_STRUC	{
+	struct	{
+        UINT32       :1;
+        UINT32       Bss3Key3CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss3Key2CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss3Key1CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss3Key0CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss2Key3CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss2Key2CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss2Key1CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss2Key0CipherAlg:3;
+	}	field;
+	UINT32			word;
+}	SEC_CSR5_STRUC, *PSEC_CSR5_STRUC;
+#else
+typedef	union	_SEC_CSR5_STRUC	{
+	struct	{
+        UINT32       Bss2Key0CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss2Key1CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss2Key2CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss2Key3CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss3Key0CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss3Key1CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss3Key2CipherAlg:3;
+        UINT32       :1;
+        UINT32       Bss3Key3CipherAlg:3;
+        UINT32       :1;
+	}	field;
+	UINT32			word;
+}	SEC_CSR5_STRUC, *PSEC_CSR5_STRUC;
+#endif
+
+
+//
+// HOST_CMD_CSR: For HOST to interrupt embedded processor
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_HOST_CMD_CSR_STRUC	{
+	struct	{
+	    UINT32   Rsv:24;
+	    UINT32   HostCommand:8;
+	}	field;
+	UINT32			word;
+}	HOST_CMD_CSR_STRUC, *PHOST_CMD_CSR_STRUC;
+#else
+typedef	union	_HOST_CMD_CSR_STRUC	{
+	struct	{
+	    UINT32   HostCommand:8;
+	    UINT32   Rsv:24;
+	}	field;
+	UINT32			word;
+}	HOST_CMD_CSR_STRUC, *PHOST_CMD_CSR_STRUC;
+#endif
+
+
+//
+// AIFSN_CSR: AIFSN for each EDCA AC
+//
+
+
+
+//
+// E2PROM_CSR: EEPROM control register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_E2PROM_CSR_STRUC	{
+	struct	{
+		UINT32		Rsvd:25;
+		UINT32       LoadStatus:1;   // 1:loading, 0:done
+		UINT32		Type:1;			// 1: 93C46, 0:93C66
+		UINT32		EepromDO:1;
+		UINT32		EepromDI:1;
+		UINT32		EepromCS:1;
+		UINT32		EepromSK:1;
+		UINT32		Reload:1;		// Reload EEPROM content, write one to reload, self-cleared.
+	}	field;
+	UINT32			word;
+}	E2PROM_CSR_STRUC, *PE2PROM_CSR_STRUC;
+#else
+typedef	union	_E2PROM_CSR_STRUC	{
+	struct	{
+		UINT32		Reload:1;		// Reload EEPROM content, write one to reload, self-cleared.
+		UINT32		EepromSK:1;
+		UINT32		EepromCS:1;
+		UINT32		EepromDI:1;
+		UINT32		EepromDO:1;
+		UINT32		Type:1;			// 1: 93C46, 0:93C66
+		UINT32       LoadStatus:1;   // 1:loading, 0:done
+		UINT32		Rsvd:25;
+	}	field;
+	UINT32			word;
+}	E2PROM_CSR_STRUC, *PE2PROM_CSR_STRUC;
+#endif
+
+
+// -------------------------------------------------------------------
+//  E2PROM data layout
+// -------------------------------------------------------------------
+
+//
+// EEPROM antenna select format
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_EEPROM_ANTENNA_STRUC	{
+	struct	{
+		USHORT      Rsv:4;
+		USHORT      RfIcType:4;             // see E2PROM document
+		USHORT		TxPath:4;	// 1: 1T, 2: 2T
+		USHORT		RxPath:4;	// 1: 1R, 2: 2R, 3: 3R
+	}	field;
+	USHORT			word;
+}	EEPROM_ANTENNA_STRUC, *PEEPROM_ANTENNA_STRUC;
+#else
+typedef	union	_EEPROM_ANTENNA_STRUC	{
+	struct	{
+		USHORT		RxPath:4;	// 1: 1R, 2: 2R, 3: 3R
+		USHORT		TxPath:4;	// 1: 1T, 2: 2T
+		USHORT      RfIcType:4;             // see E2PROM document
+		USHORT      Rsv:4;
+	}	field;
+	USHORT			word;
+}	EEPROM_ANTENNA_STRUC, *PEEPROM_ANTENNA_STRUC;
+#endif
+
+#ifdef RT_BIG_ENDIAN
+typedef	union _EEPROM_NIC_CINFIG2_STRUC	{
+	struct	{
+        USHORT		Rsv2:6;					// must be 0
+		USHORT		BW40MAvailForA:1;			// 0:enable, 1:disable
+		USHORT		BW40MAvailForG:1;			// 0:enable, 1:disable
+		USHORT		EnableWPSPBC:1;                 // WPS PBC Control bit
+		USHORT		BW40MSidebandForA:1;
+		USHORT		BW40MSidebandForG:1;
+		USHORT		CardbusAcceleration:1;	// !!! NOTE: 0 - enable, 1 - disable
+		USHORT		ExternalLNAForA:1;			// external LNA enable for 5G
+		USHORT		ExternalLNAForG:1;			// external LNA enable for 2.4G
+		USHORT		DynamicTxAgcControl:1;			//
+		USHORT		HardwareRadioControl:1;	// Whether RF is controlled by driver or HW. 1:enable hw control, 0:disable
+	}	field;
+	USHORT			word;
+}	EEPROM_NIC_CONFIG2_STRUC, *PEEPROM_NIC_CONFIG2_STRUC;
+#else
+typedef	union _EEPROM_NIC_CINFIG2_STRUC	{
+	struct {
+		USHORT		HardwareRadioControl:1;	// 1:enable, 0:disable
+		USHORT		DynamicTxAgcControl:1;			//
+		USHORT		ExternalLNAForG:1;				//
+		USHORT		ExternalLNAForA:1;			// external LNA enable for 2.4G
+		USHORT		CardbusAcceleration:1;	// !!! NOTE: 0 - enable, 1 - disable
+		USHORT		BW40MSidebandForG:1;
+		USHORT		BW40MSidebandForA:1;
+		USHORT		EnableWPSPBC:1;                 // WPS PBC Control bit
+		USHORT		BW40MAvailForG:1;			// 0:enable, 1:disable
+		USHORT		BW40MAvailForA:1;			// 0:enable, 1:disable
+		USHORT		Rsv2:6;                 // must be 0
+	}	field;
+	USHORT			word;
+}	EEPROM_NIC_CONFIG2_STRUC, *PEEPROM_NIC_CONFIG2_STRUC;
+#endif
+
+//
+// TX_PWR Value valid range 0xFA(-6) ~ 0x24(36)
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_EEPROM_TX_PWR_STRUC	{
+	struct	{
+		CHAR	Byte1;				// High Byte
+		CHAR	Byte0;				// Low Byte
+	}	field;
+	USHORT	word;
+}	EEPROM_TX_PWR_STRUC, *PEEPROM_TX_PWR_STRUC;
+#else
+typedef	union	_EEPROM_TX_PWR_STRUC	{
+	struct	{
+		CHAR	Byte0;				// Low Byte
+		CHAR	Byte1;				// High Byte
+	}	field;
+	USHORT	word;
+}	EEPROM_TX_PWR_STRUC, *PEEPROM_TX_PWR_STRUC;
+#endif
+
+#ifdef RT_BIG_ENDIAN
+typedef	union	_EEPROM_VERSION_STRUC	{
+	struct	{
+		UCHAR	Version;			// High Byte
+		UCHAR	FaeReleaseNumber;	// Low Byte
+	}	field;
+	USHORT	word;
+}	EEPROM_VERSION_STRUC, *PEEPROM_VERSION_STRUC;
+#else
+typedef	union	_EEPROM_VERSION_STRUC	{
+	struct	{
+		UCHAR	FaeReleaseNumber;	// Low Byte
+		UCHAR	Version;			// High Byte
+	}	field;
+	USHORT	word;
+}	EEPROM_VERSION_STRUC, *PEEPROM_VERSION_STRUC;
+#endif
+
+#ifdef RT_BIG_ENDIAN
+typedef	union	_EEPROM_LED_STRUC	{
+	struct	{
+		USHORT	Rsvd:3;				// Reserved
+		USHORT	LedMode:5;			// Led mode.
+		USHORT	PolarityGPIO_4:1;	// Polarity GPIO#4 setting.
+		USHORT	PolarityGPIO_3:1;	// Polarity GPIO#3 setting.
+		USHORT	PolarityGPIO_2:1;	// Polarity GPIO#2 setting.
+		USHORT	PolarityGPIO_1:1;	// Polarity GPIO#1 setting.
+		USHORT	PolarityGPIO_0:1;	// Polarity GPIO#0 setting.
+		USHORT	PolarityACT:1;		// Polarity ACT setting.
+		USHORT	PolarityRDY_A:1;		// Polarity RDY_A setting.
+		USHORT	PolarityRDY_G:1;		// Polarity RDY_G setting.
+	}	field;
+	USHORT	word;
+}	EEPROM_LED_STRUC, *PEEPROM_LED_STRUC;
+#else
+typedef	union	_EEPROM_LED_STRUC	{
+	struct	{
+		USHORT	PolarityRDY_G:1;		// Polarity RDY_G setting.
+		USHORT	PolarityRDY_A:1;		// Polarity RDY_A setting.
+		USHORT	PolarityACT:1;		// Polarity ACT setting.
+		USHORT	PolarityGPIO_0:1;	// Polarity GPIO#0 setting.
+		USHORT	PolarityGPIO_1:1;	// Polarity GPIO#1 setting.
+		USHORT	PolarityGPIO_2:1;	// Polarity GPIO#2 setting.
+		USHORT	PolarityGPIO_3:1;	// Polarity GPIO#3 setting.
+		USHORT	PolarityGPIO_4:1;	// Polarity GPIO#4 setting.
+		USHORT	LedMode:5;			// Led mode.
+		USHORT	Rsvd:3;				// Reserved
+	}	field;
+	USHORT	word;
+}	EEPROM_LED_STRUC, *PEEPROM_LED_STRUC;
+#endif
+
+#ifdef RT_BIG_ENDIAN
+typedef	union	_EEPROM_TXPOWER_DELTA_STRUC	{
+	struct	{
+		UCHAR	TxPowerEnable:1;// Enable
+		UCHAR	Type:1;			// 1: plus the delta value, 0: minus the delta value
+		UCHAR	DeltaValue:6;	// Tx Power dalta value (MAX=4)
+	}	field;
+	UCHAR	value;
+}	EEPROM_TXPOWER_DELTA_STRUC, *PEEPROM_TXPOWER_DELTA_STRUC;
+#else
+typedef	union	_EEPROM_TXPOWER_DELTA_STRUC	{
+	struct	{
+		UCHAR	DeltaValue:6;	// Tx Power dalta value (MAX=4)
+		UCHAR	Type:1;			// 1: plus the delta value, 0: minus the delta value
+		UCHAR	TxPowerEnable:1;// Enable
+	}	field;
+	UCHAR	value;
+}	EEPROM_TXPOWER_DELTA_STRUC, *PEEPROM_TXPOWER_DELTA_STRUC;
+#endif
+
+//
+// QOS_CSR0: TXOP holder address0 register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_QOS_CSR0_STRUC	{
+	struct	{
+		UCHAR		Byte3;		// MAC address byte 3
+		UCHAR		Byte2;		// MAC address byte 2
+		UCHAR		Byte1;		// MAC address byte 1
+		UCHAR		Byte0;		// MAC address byte 0
+	}	field;
+	UINT32			word;
+}	QOS_CSR0_STRUC, *PQOS_CSR0_STRUC;
+#else
+typedef	union	_QOS_CSR0_STRUC	{
+	struct	{
+		UCHAR		Byte0;		// MAC address byte 0
+		UCHAR		Byte1;		// MAC address byte 1
+		UCHAR		Byte2;		// MAC address byte 2
+		UCHAR		Byte3;		// MAC address byte 3
+	}	field;
+	UINT32			word;
+}	QOS_CSR0_STRUC, *PQOS_CSR0_STRUC;
+#endif
+
+//
+// QOS_CSR1: TXOP holder address1 register
+//
+#ifdef RT_BIG_ENDIAN
+typedef	union	_QOS_CSR1_STRUC	{
+	struct	{
+		UCHAR		Rsvd1;
+		UCHAR		Rsvd0;
+		UCHAR		Byte5;		// MAC address byte 5
+		UCHAR		Byte4;		// MAC address byte 4
+	}	field;
+	UINT32			word;
+}	QOS_CSR1_STRUC, *PQOS_CSR1_STRUC;
+#else
+typedef	union	_QOS_CSR1_STRUC	{
+	struct	{
+		UCHAR		Byte4;		// MAC address byte 4
+		UCHAR		Byte5;		// MAC address byte 5
+		UCHAR		Rsvd0;
+		UCHAR		Rsvd1;
+	}	field;
+	UINT32			word;
+}	QOS_CSR1_STRUC, *PQOS_CSR1_STRUC;
+#endif
+
+#define	RF_CSR_CFG	0x500
+#ifdef RT_BIG_ENDIAN
+typedef	union	_RF_CSR_CFG_STRUC	{
+	struct	{
+		UINT	Rsvd1:14;				// Reserved
+		UINT	RF_CSR_KICK:1;			// kick RF register read/write
+		UINT	RF_CSR_WR:1;			// 0: read  1: write
+		UINT	Rsvd2:3;				// Reserved
+		UINT	TESTCSR_RFACC_REGNUM:5;	// RF register ID
+		UINT	RF_CSR_DATA:8;			// DATA
+	}	field;
+	UINT	word;
+}	RF_CSR_CFG_STRUC, *PRF_CSR_CFG_STRUC;
+#else
+typedef	union	_RF_CSR_CFG_STRUC	{
+	struct	{
+		UINT	RF_CSR_DATA:8;			// DATA
+		UINT	TESTCSR_RFACC_REGNUM:5;	// RF register ID
+		UINT	Rsvd2:3;				// Reserved
+		UINT	RF_CSR_WR:1;			// 0: read  1: write
+		UINT	RF_CSR_KICK:1;			// kick RF register read/write
+		UINT	Rsvd1:14;				// Reserved
+	}	field;
+	UINT	word;
+}	RF_CSR_CFG_STRUC, *PRF_CSR_CFG_STRUC;
+#endif
+
+#endif	// __RT28XX_H__
diff --git a/drivers/staging/rt2870/rt_ate.c b/drivers/staging/rt2870/rt_ate.c
new file mode 100644
index 00000000000..e99b3da0b62
--- /dev/null
+++ b/drivers/staging/rt2870/rt_ate.c
@@ -0,0 +1,6452 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#include "rt_config.h"
+
+#ifdef UCOS
+INT IoctlResponse(PUCHAR payload, PUCHAR msg, INT len);
+#endif // UCOS //
+
+#ifdef RALINK_ATE
+UCHAR TemplateFrame[24] = {0x08/* Data type */,0x00,0x00,0x00,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x00,0xAA,0xBB,0x12,0x34,0x56,0x00,0x11,0x22,0xAA,0xBB,0xCC,0x00,0x00};	// 802.11 MAC Header, Type:Data, Length:24bytes
+extern RTMP_RF_REGS RF2850RegTable[];
+extern UCHAR NUM_OF_2850_CHNL;
+
+#ifdef RT2870
+extern UCHAR EpToQueue[];
+extern VOID	RTUSBRejectPendingPackets(	IN	PRTMP_ADAPTER	pAd);
+#endif // RT2870 //
+
+#ifdef UCOS
+extern INT ConsoleResponse(IN PUCHAR buff);
+extern int (*remote_display)(char *);
+#endif // UCOS //
+
+static CHAR CCKRateTable[] = {0, 1, 2, 3, 8, 9, 10, 11, -1}; /* CCK Mode. */
+static CHAR OFDMRateTable[] = {0, 1, 2, 3, 4, 5, 6, 7, -1}; /* OFDM Mode. */
+static CHAR HTMIXRateTable[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, -1}; /* HT Mix Mode. */
+
+static INT TxDmaBusy(
+	IN PRTMP_ADAPTER pAd);
+
+static INT RxDmaBusy(
+	IN PRTMP_ADAPTER pAd);
+
+static VOID RtmpDmaEnable(
+	IN PRTMP_ADAPTER pAd,
+	IN INT Enable);
+
+static VOID BbpSoftReset(
+	IN PRTMP_ADAPTER pAd);
+
+static VOID RtmpRfIoWrite(
+	IN PRTMP_ADAPTER pAd);
+
+static INT ATESetUpFrame(
+	IN PRTMP_ADAPTER pAd,
+	IN UINT32 TxIdx);
+
+static INT ATETxPwrHandler(
+	IN PRTMP_ADAPTER pAd,
+	IN char index);
+
+static INT ATECmdHandler(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+static int CheckMCSValid(
+	IN UCHAR Mode,
+	IN UCHAR Mcs);
+
+
+#ifdef RT2870
+static VOID ATEWriteTxInfo(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTXINFO_STRUC 	pTxInfo,
+	IN	  USHORT		USBDMApktLen,
+	IN	  BOOLEAN		bWiv,
+	IN	  UCHAR			QueueSel,
+	IN	  UCHAR			NextValid,
+	IN	  UCHAR			TxBurst);
+
+static VOID ATEWriteTxWI(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTXWI_STRUC 	pTxWI,
+	IN	BOOLEAN			FRAG,
+	IN	BOOLEAN			InsTimestamp,
+	IN	BOOLEAN 		AMPDU,
+	IN	BOOLEAN 		Ack,
+	IN	BOOLEAN 		NSeq,		// HW new a sequence.
+	IN	UCHAR			BASize,
+	IN	UCHAR			WCID,
+	IN	ULONG			Length,
+	IN	UCHAR 			PID,
+	IN	UCHAR			MIMOps,
+	IN	UCHAR			Txopmode,
+	IN	BOOLEAN			CfAck,
+	IN	HTTRANSMIT_SETTING	Transmit);
+
+#endif // RT2870 //
+
+static VOID SetJapanFilter(
+	IN	PRTMP_ADAPTER	pAd);
+
+/*=========================end of prototype=========================*/
+
+
+#ifdef RT2870
+static INT TxDmaBusy(
+	IN PRTMP_ADAPTER pAd)
+{
+	INT result;
+	USB_DMA_CFG_STRUC UsbCfg;
+
+	RTMP_IO_READ32(pAd, USB_DMA_CFG, &UsbCfg.word);	// disable DMA
+	if (UsbCfg.field.TxBusy)
+		result = 1;
+	else
+		result = 0;
+
+	return result;
+}
+
+static INT RxDmaBusy(
+	IN PRTMP_ADAPTER pAd)
+{
+	INT result;
+	USB_DMA_CFG_STRUC UsbCfg;
+
+	RTMP_IO_READ32(pAd, USB_DMA_CFG, &UsbCfg.word);	// disable DMA
+	if (UsbCfg.field.RxBusy)
+		result = 1;
+	else
+		result = 0;
+
+	return result;
+}
+
+static VOID RtmpDmaEnable(
+	IN PRTMP_ADAPTER pAd,
+	IN INT Enable)
+{
+	BOOLEAN value;
+	ULONG WaitCnt;
+	USB_DMA_CFG_STRUC UsbCfg;
+
+	value = Enable > 0 ? 1 : 0;
+
+	// check DMA is in busy mode.
+	WaitCnt = 0;
+	while (TxDmaBusy(pAd) || RxDmaBusy(pAd))
+	{
+		RTMPusecDelay(10);
+		if (WaitCnt++ > 100)
+			break;
+	}
+
+	//Why not to clear USB DMA TX path first ???
+	RTMP_IO_READ32(pAd, USB_DMA_CFG, &UsbCfg.word);	// disable DMA
+	UsbCfg.field.TxBulkEn = value;
+	UsbCfg.field.RxBulkEn = value;
+	RTMP_IO_WRITE32(pAd, USB_DMA_CFG, UsbCfg.word);	// abort all TX rings
+	RTMPusecDelay(5000);
+
+	return;
+}
+#endif // RT2870 //
+
+static VOID BbpSoftReset(
+	IN PRTMP_ADAPTER pAd)
+{
+	UCHAR BbpData = 0;
+
+	// Soft reset, set BBP R21 bit0=1->0
+	ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R21, &BbpData);
+	BbpData |= 0x00000001; //set bit0=1
+	ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R21, BbpData);
+
+	ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R21, &BbpData);
+	BbpData &= ~(0x00000001); //set bit0=0
+	ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R21, BbpData);
+
+	return;
+}
+
+static VOID RtmpRfIoWrite(
+	IN PRTMP_ADAPTER pAd)
+{
+	// Set RF value 1's set R3[bit2] = [0]
+	RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R1);
+	RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R2);
+	RTMP_RF_IO_WRITE32(pAd, (pAd->LatchRfRegs.R3 & (~0x04)));
+	RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R4);
+
+	RTMPusecDelay(200);
+
+	// Set RF value 2's set R3[bit2] = [1]
+	RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R1);
+	RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R2);
+	RTMP_RF_IO_WRITE32(pAd, (pAd->LatchRfRegs.R3 | 0x04));
+	RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R4);
+
+	RTMPusecDelay(200);
+
+	// Set RF value 3's set R3[bit2] = [0]
+	RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R1);
+	RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R2);
+	RTMP_RF_IO_WRITE32(pAd, (pAd->LatchRfRegs.R3 & (~0x04)));
+	RTMP_RF_IO_WRITE32(pAd, pAd->LatchRfRegs.R4);
+
+	return;
+}
+
+static int CheckMCSValid(
+	UCHAR Mode,
+	UCHAR Mcs)
+{
+	int i;
+	PCHAR pRateTab;
+
+	switch(Mode)
+	{
+		case 0:
+			pRateTab = CCKRateTable;
+			break;
+		case 1:
+			pRateTab = OFDMRateTable;
+			break;
+		case 2:
+		case 3:
+			pRateTab = HTMIXRateTable;
+			break;
+		default:
+			ATEDBGPRINT(RT_DEBUG_ERROR, ("unrecognizable Tx Mode %d\n", Mode));
+			return -1;
+			break;
+	}
+
+	i = 0;
+	while(pRateTab[i] != -1)
+	{
+		if (pRateTab[i] == Mcs)
+			return 0;
+		i++;
+	}
+
+	return -1;
+}
+
+#if 1
+static INT ATETxPwrHandler(
+	IN PRTMP_ADAPTER pAd,
+	IN char index)
+{
+	ULONG R;
+	CHAR TxPower;
+	UCHAR Bbp94 = 0;
+	BOOLEAN bPowerReduce = FALSE;
+
+#ifdef RALINK_28xx_QA
+	if ((pAd->ate.bQATxStart == TRUE) || (pAd->ate.bQARxStart == TRUE))
+	{
+		/* When QA is used for Tx, pAd->ate.TxPower0/1 and real tx power
+		** are not synchronized.
+		*/
+/*
+		pAd->ate.TxPower0 = pAd->LatchRfRegs.xxx;
+		pAd->ate.TxPower1 = pAd->LatchRfRegs.xxx;
+*/
+		return 0;
+	}
+	else
+#endif // RALINK_28xx_QA //
+	{
+		TxPower = index == 0 ? pAd->ate.TxPower0 : pAd->ate.TxPower1;
+
+		if (pAd->ate.Channel <= 14)
+		{
+			if (TxPower > 31)
+			{
+				//
+				// R3, R4 can't large than 31 (0x24), 31 ~ 36 used by BBP 94
+				//
+				R = 31;
+				if (TxPower <= 36)
+					Bbp94 = BBPR94_DEFAULT + (UCHAR)(TxPower - 31);
+			}
+			else if (TxPower < 0)
+			{
+				//
+				// R3, R4 can't less than 0, -1 ~ -6 used by BBP 94
+				//
+				R = 0;
+				if (TxPower >= -6)
+					Bbp94 = BBPR94_DEFAULT + TxPower;
+			}
+			else
+			{
+				// 0 ~ 31
+				R = (ULONG) TxPower;
+				Bbp94 = BBPR94_DEFAULT;
+			}
+
+			ATEDBGPRINT(RT_DEBUG_TRACE, ("%s (TxPower=%d, R=%ld, BBP_R94=%d)\n", __FUNCTION__, TxPower, R, Bbp94));
+		}
+		else// 5.5 GHz
+		{
+			if (TxPower > 15)
+			{
+				//
+				// R3, R4 can't large than 15 (0x0F)
+				//
+				R = 15;
+			}
+			else if (TxPower < 0)
+			{
+				//
+				// R3, R4 can't less than 0
+				//
+				// -1 ~ -7
+				ASSERT((TxPower >= -7));
+				R = (ULONG)(TxPower + 7);
+				bPowerReduce = TRUE;
+			}
+			else
+			{
+				// 0 ~ 15
+				R = (ULONG) TxPower;
+			}
+
+			ATEDBGPRINT(RT_DEBUG_TRACE, ("%s (TxPower=%d, R=%lu)\n", __FUNCTION__, TxPower, R));
+		}
+
+		if (pAd->ate.Channel <= 14)
+		{
+			if (index == 0)
+			{
+				R = R << 9;		// shift TX power control to correct RF(R3) register bit position
+				R |= (pAd->LatchRfRegs.R3 & 0xffffc1ff);
+				pAd->LatchRfRegs.R3 = R;
+			}
+			else
+			{
+				R = R << 6;		// shift TX power control to correct RF(R4) register bit position
+				R |= (pAd->LatchRfRegs.R4 & 0xfffff83f);
+				pAd->LatchRfRegs.R4 = R;
+			}
+		}
+		else// 5.5GHz
+		{
+			if (bPowerReduce == FALSE)
+			{
+				if (index == 0)
+				{
+					R = (R << 10) | (1 << 9);		// shift TX power control to correct RF(R3) register bit position
+					R |= (pAd->LatchRfRegs.R3 & 0xffffc1ff);
+					pAd->LatchRfRegs.R3 = R;
+				}
+				else
+				{
+					R = (R << 7) | (1 << 6);		// shift TX power control to correct RF(R4) register bit position
+					R |= (pAd->LatchRfRegs.R4 & 0xfffff83f);
+					pAd->LatchRfRegs.R4 = R;
+				}
+			}
+			else
+			{
+				if (index == 0)
+				{
+					R = (R << 10);		// shift TX power control to correct RF(R3) register bit position
+					R |= (pAd->LatchRfRegs.R3 & 0xffffc1ff);
+
+					/* Clear bit 9 of R3 to reduce 7dB. */
+					pAd->LatchRfRegs.R3 = (R & (~(1 << 9)));
+				}
+				else
+				{
+					R = (R << 7);		// shift TX power control to correct RF(R4) register bit position
+					R |= (pAd->LatchRfRegs.R4 & 0xfffff83f);
+
+					/* Clear bit 6 of R4 to reduce 7dB. */
+					pAd->LatchRfRegs.R4 = (R & (~(1 << 6)));
+				}
+			}
+		}
+
+		RtmpRfIoWrite(pAd);
+
+		return 0;
+	}
+}
+#else// 1 //
+static INT ATETxPwrHandler(
+	IN PRTMP_ADAPTER pAd,
+	IN char index)
+{
+	ULONG R;
+	CHAR TxPower;
+	UCHAR Bbp94 = 0;
+
+#ifdef RALINK_28xx_QA
+	if ((pAd->ate.bQATxStart == TRUE) || (pAd->ate.bQARxStart == TRUE))
+	{
+		// TODO: how to get current TxPower0/1 from pAd->LatchRfRegs ?
+		/* When QA is used for Tx, pAd->ate.TxPower0/1 and real tx power
+		** are not synchronized.
+		*/
+/*
+		pAd->ate.TxPower0 = pAd->LatchRfRegs.xxx;
+		pAd->ate.TxPower1 = pAd->LatchRfRegs.xxx;
+*/
+		return 0;
+	}
+	else
+#endif // RALINK_28xx_QA //
+	{
+		TxPower = index == 0 ? pAd->ate.TxPower0 : pAd->ate.TxPower1;
+
+	if (TxPower > 31)
+	{
+		//
+		// R3, R4 can't large than 36 (0x24), 31 ~ 36 used by BBP 94
+		//
+		R = 31;
+		if (TxPower <= 36)
+			Bbp94 = BBPR94_DEFAULT + (UCHAR)(TxPower - 31);
+	}
+	else if (TxPower < 0)
+	{
+		//
+		// R3, R4 can't less than 0, -1 ~ -6 used by BBP 94
+		//
+		R = 0;
+		if (TxPower >= -6)
+			Bbp94 = BBPR94_DEFAULT + TxPower;
+	}
+	else
+	{
+		// 0 ~ 31
+		R = (ULONG) TxPower;
+		Bbp94 = BBPR94_DEFAULT;
+	}
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("%s (TxPower=%d, R3=%ld, BBP_R94=%d)\n", __FUNCTION__, TxPower, R, Bbp94));
+
+		if (pAd->ate.Channel <= 14)
+		{
+	if (index == 0)
+	{
+		R = R << 9;		// shift TX power control to correct RF(R3) register bit position
+		R |= (pAd->LatchRfRegs.R3 & 0xffffc1ff);
+		pAd->LatchRfRegs.R3 = R;
+	}
+	else
+	{
+		R = R << 6;		// shift TX power control to correct RF(R4) register bit position
+		R |= (pAd->LatchRfRegs.R4 & 0xfffff83f);
+		pAd->LatchRfRegs.R4 = R;
+	}
+		}
+		else
+		{
+			if (index == 0)
+			{
+				R = (R << 10) | (1 << 9);		// shift TX power control to correct RF(R3) register bit position
+				R |= (pAd->LatchRfRegs.R3 & 0xffffc1ff);
+				pAd->LatchRfRegs.R3 = R;
+			}
+			else
+			{
+				R = (R << 7) | (1 << 6);		// shift TX power control to correct RF(R4) register bit position
+				R |= (pAd->LatchRfRegs.R4 & 0xfffff83f);
+				pAd->LatchRfRegs.R4 = R;
+			}
+		}
+
+	RtmpRfIoWrite(pAd);
+
+	return 0;
+	}
+}
+#endif // 1 //
+/*
+    ==========================================================================
+    Description:
+        Set ATE operation mode to
+        0. ATESTART  = Start ATE Mode
+        1. ATESTOP   = Stop ATE Mode
+        2. TXCONT    = Continuous Transmit
+        3. TXCARR    = Transmit Carrier
+        4. TXFRAME   = Transmit Frames
+        5. RXFRAME   = Receive Frames
+#ifdef RALINK_28xx_QA
+        6. TXSTOP    = Stop Any Type of Transmition
+        7. RXSTOP    = Stop Receiving Frames
+#endif // RALINK_28xx_QA //
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+/*                                                           */
+/*                                                           */
+/*=======================End of RT2860=======================*/
+
+
+/*======================Start of RT2870======================*/
+/*                                                           */
+/*                                                           */
+
+#ifdef RT2870
+static INT	ATECmdHandler(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UINT32			Value;
+	UCHAR			BbpData;
+	UINT32          MacData;
+	UINT			i=0, atemode;
+	//NDIS_STATUS		Status = NDIS_STATUS_SUCCESS;
+	//PUCHAR			pDest;
+	UINT32 			temp;
+	ULONG			IrqFlags;
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("===> ATECmdHandler()\n"));
+	ATEAsicSwitchChannel(pAd);
+	/* AsicLockChannel() is empty function so far in fact */
+	AsicLockChannel(pAd, pAd->ate.Channel);
+
+	RTMPusecDelay(5000);
+
+	// Default value in BBP R22 is 0x0.
+	BbpData = 0;
+
+	/* Enter ATE mode and set Tx/Rx Idle */
+	if (!strcmp(arg, "ATESTART"))
+	{
+#ifdef CONFIG_STA_SUPPORT
+		BOOLEAN       Cancelled;
+#endif // CONFIG_STA_SUPPORT //
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ATE: ATESTART\n"));
+
+		netif_stop_queue(pAd->net_dev);
+
+		atemode = pAd->ate.Mode;
+		pAd->ate.Mode = ATE_START;
+//		pAd->ate.TxDoneCount = pAd->ate.TxCount;
+		// Disable Rx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 3);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		// Disable auto responder
+		RTMP_IO_READ32(pAd, AUTO_RSP_CFG, &temp);
+		temp = temp & 0xFFFFFFFE;
+		RTMP_IO_WRITE32(pAd, AUTO_RSP_CFG, temp);
+
+		// read MAC_SYS_CTRL and backup MAC_SYS_CTRL value.
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &MacData);
+		// clean bit4 to stop continuous Tx production test.
+		MacData &= 0xFFFFFFEF;
+		// Stop continuous TX production test.
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, MacData);//disable or cancel pending irp first ???
+
+        if (atemode & ATE_TXCARR)
+		{
+			// No Carrier Test set BBP R22 bit7=0, bit6=0, bit[5~0]=0x0
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R22, &BbpData);
+			BbpData &= 0xFFFFFF00; //clear bit7, bit6, bit[5~0]
+		    ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+		}
+		else if (atemode & ATE_TXCARRSUPP)
+		{
+			// No Cont. TX set BBP R22 bit7=0
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R22, &BbpData);
+			BbpData &= ~(1 << 7); //set bit7=0
+			ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+
+			// No Carrier Suppression set BBP R24 bit0=0
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R24, &BbpData);
+			BbpData &= 0xFFFFFFFE; //clear bit0
+		    ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R24, BbpData);
+		}
+		// We should free some resource which allocate when ATE_TXFRAME , ATE_STOP, and ATE_TXCONT.
+		// TODO:Should we free some resource which was allocated when LoopBack and ATE_STOP ?
+		else if ((atemode & ATE_TXFRAME) || (atemode == ATE_STOP))
+		{
+			if (atemode & ATE_TXCONT)
+			{
+				// Not Cont. TX anymore, so set BBP R22 bit7=0
+				ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R22, &BbpData);
+				BbpData &= ~(1 << 7); //set bit7=0
+				ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+			}
+			// Abort Tx, Rx DMA.
+			RtmpDmaEnable(pAd, 0);
+
+			{
+				// It seems nothing to free,
+				// because we didn't allocate any resource when we entered ATE_TXFRAME mode latestly.
+			}
+
+			// Start Tx, RX DMA
+			RtmpDmaEnable(pAd, 1);
+		}
+
+		RTUSBRejectPendingPackets(pAd);
+		RTUSBCleanUpDataBulkOutQueue(pAd);
+
+#ifdef CONFIG_STA_SUPPORT
+		//
+		// It will be called in MlmeSuspend().
+		//
+		// Cancel pending timers
+		RTMPCancelTimer(&pAd->MlmeAux.AssocTimer,     &Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.ReassocTimer,   &Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.DisassocTimer,   &Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.AuthTimer,       &Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.BeaconTimer,     &Cancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.ScanTimer,       &Cancelled);
+#endif // CONFIG_STA_SUPPORT //
+
+		//RTUSBCleanUpMLMEWaitQueue(pAd);	/* not used in RT28xx */
+		RTUSBCleanUpMLMEBulkOutQueue(pAd);
+
+		// Sometimes kernel will hang on, so we avoid calling MlmeSuspend().
+//		MlmeSuspend(pAd, TRUE);
+		//RTMPCancelTimer(&pAd->Mlme.PeriodicTimer, &Cancelled);
+
+		// Disable Rx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 3);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		// Abort Tx, RX DMA.
+		RtmpDmaEnable(pAd, 0);
+
+		// Disable Tx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 2);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		// Make sure there are no pending bulk in/out IRPs before we go on.
+/*=========================================================================*/
+		/* pAd->PendingRx is not of type atomic_t anymore in 28xx */
+//		while ((atomic_read(&pAd->PendingRx) > 0))	//pAd->BulkFlags != 0 wait bulk out finish
+		while ((pAd->PendingRx > 0))	//pAd->BulkFlags != 0 wait bulk out finish
+		{
+#if 1
+			ATE_RTUSBCancelPendingBulkInIRP(pAd);
+#else
+			NdisInterlockedDecrement(&pAd->PendingRx);
+#endif
+			/* delay 0.5 seconds */
+			RTMPusecDelay(500000);
+			pAd->PendingRx = 0;
+		}
+		/* peter : why don't we have to get BulkOutLock first ? */
+		while (((pAd->BulkOutPending[0] == TRUE) ||
+				(pAd->BulkOutPending[1] == TRUE) ||
+				(pAd->BulkOutPending[2] == TRUE) ||
+				(pAd->BulkOutPending[3] == TRUE)) && (pAd->BulkFlags != 0))	//pAd->BulkFlags != 0 wait bulk out finish
+		{
+			do
+			{
+				/* pAd->BulkOutPending[y] will be set to FALSE in RTUSBCancelPendingBulkOutIRP(pAd) */
+				RTUSBCancelPendingBulkOutIRP(pAd);
+			} while (FALSE);
+
+			/* we have enough time delay in RTUSBCancelPendingBulkOutIRP(pAd)
+			** so this is not necessary
+			*/
+//			RTMPusecDelay(500000);
+		}
+
+		/* pAd->PendingRx is not of type atomic_t anymore in 28xx */
+//		ASSERT(atomic_read(&pAd->PendingRx) == 0);
+		ASSERT(pAd->PendingRx == 0);
+/*=========================================================================*/
+
+		// reset Rx statistics.
+		pAd->ate.LastSNR0 = 0;
+		pAd->ate.LastSNR1 = 0;
+		pAd->ate.LastRssi0 = 0;
+		pAd->ate.LastRssi1 = 0;
+		pAd->ate.LastRssi2 = 0;
+		pAd->ate.AvgRssi0 = 0;
+		pAd->ate.AvgRssi1 = 0;
+		pAd->ate.AvgRssi2 = 0;
+		pAd->ate.AvgRssi0X8 = 0;
+		pAd->ate.AvgRssi1X8 = 0;
+		pAd->ate.AvgRssi2X8 = 0;
+		pAd->ate.NumOfAvgRssiSample = 0;
+
+#ifdef RALINK_28xx_QA
+		// Tx frame
+		pAd->ate.bQATxStart = FALSE;
+		pAd->ate.bQARxStart = FALSE;
+		pAd->ate.seq = 0;
+
+		// counters
+		pAd->ate.U2M = 0;
+		pAd->ate.OtherData = 0;
+		pAd->ate.Beacon = 0;
+		pAd->ate.OtherCount = 0;
+		pAd->ate.TxAc0 = 0;
+		pAd->ate.TxAc1 = 0;
+		pAd->ate.TxAc2 = 0;
+		pAd->ate.TxAc3 = 0;
+		pAd->ate.TxHCCA = 0;
+		pAd->ate.TxMgmt = 0;
+		pAd->ate.RSSI0 = 0;
+		pAd->ate.RSSI1 = 0;
+		pAd->ate.RSSI2 = 0;
+		pAd->ate.SNR0 = 0;
+		pAd->ate.SNR1 = 0;
+
+		// control
+		pAd->ate.TxDoneCount = 0;
+		pAd->ate.TxStatus = 0; // task Tx status // 0 --> task is idle, 1 --> task is running
+#endif // RALINK_28xx_QA //
+
+		// Soft reset BBP.
+		BbpSoftReset(pAd);
+
+
+#ifdef CONFIG_STA_SUPPORT
+		AsicDisableSync(pAd);
+
+		/*
+		** If we skip "LinkDown()", we should disable protection
+		** to prevent from sending out RTS or CTS-to-self.
+		*/
+		ATEDisableAsicProtect(pAd);
+		RTMPStationStop(pAd);
+#endif // CONFIG_STA_SUPPORT //
+
+		// Default value in BBP R22 is 0x0.
+		BbpData = 0;
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &MacData);
+
+		// Clean bit4 to stop continuous Tx production test.
+		MacData &= 0xFFFFFFEF;
+	   	ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+	    	RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, MacData);
+		//Clean ATE Bulk in/out counter and continue setup
+		InterlockedExchange(&pAd->BulkOutRemained, 0);
+
+		/* NdisAcquireSpinLock()/NdisReleaseSpinLock() need only one argument in RT28xx */
+		NdisAcquireSpinLock(&pAd->GenericLock);
+		pAd->ContinBulkOut = FALSE;
+		pAd->ContinBulkIn = FALSE;
+		NdisReleaseSpinLock(&pAd->GenericLock);
+
+		RTUSB_CLEAR_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_ATE);
+	}
+	else if (!strcmp(arg, "ATESTOP"))
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ATE : ATESTOP ===>\n"));
+
+		// Default value in BBP R22 is 0x0.
+		BbpData = 0;
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &MacData);//0820
+		// Clean bit4 to stop continuous Tx production test.
+		MacData &= 0xFFFFFFEF;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, MacData); // recover the MAC_SYS_CTRL register back.
+
+		// Disable Rx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 3);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		/*
+		** Abort Tx, RX DMA.
+		** Q   : How to do the following I/O if Tx, Rx DMA is aborted ?
+		** Ans : Bulk endpoints are aborted, while the control endpoint is not.
+		*/
+		RtmpDmaEnable(pAd, 0);
+
+		// Disable Tx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 2);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		/* Make sure there are no pending bulk in/out IRPs before we go on. */
+/*=========================================================================*/
+//		while ((atomic_read(&pAd->PendingRx) > 0))	//pAd->BulkFlags != 0 wait bulk out finish
+		while (pAd->PendingRx > 0)
+		{
+#if 1
+			ATE_RTUSBCancelPendingBulkInIRP(pAd);
+#else
+//			NdisInterlockedDecrement(&pAd->PendingRx);
+			pAd->PendingRx--;
+#endif
+			RTMPusecDelay(500000);
+		}
+
+		while (((pAd->BulkOutPending[0] == TRUE) ||
+				(pAd->BulkOutPending[1] == TRUE) ||
+				(pAd->BulkOutPending[2] == TRUE) ||
+				(pAd->BulkOutPending[3] == TRUE)) && (pAd->BulkFlags != 0))	//pAd->BulkFlags != 0 wait bulk out finish
+		{
+			do
+			{
+				RTUSBCancelPendingBulkOutIRP(pAd);
+			} while (FALSE);
+
+			RTMPusecDelay(500000);
+		}
+
+//		ASSERT(atomic_read(&pAd->PendingRx) == 0);
+		ASSERT(pAd->PendingRx == 0);
+/*=========================================================================*/
+/*      Reset Rx RING                                                      */
+/*=========================================================================*/
+//		InterlockedExchange(&pAd->PendingRx, 0);
+		pAd->PendingRx = 0;
+		pAd->NextRxBulkInReadIndex = 0;	// Next Rx Read index
+		pAd->NextRxBulkInIndex = RX_RING_SIZE - 1;	// Rx Bulk pointer
+		pAd->NextRxBulkInPosition = 0;
+		for (i = 0; i < (RX_RING_SIZE); i++)
+		{
+			PRX_CONTEXT  pRxContext = &(pAd->RxContext[i]);
+			NdisZeroMemory(pRxContext->TransferBuffer, MAX_RXBULK_SIZE);
+			/* peter : why don't we have to get BulkInLock first ? */
+			pRxContext->pAd	= pAd;
+			pRxContext->pIrp = NULL;
+            /* peter debug ++ */
+			pRxContext->BulkInOffset = 0;
+			pRxContext->bRxHandling = FALSE;
+            /* peter debug -- */
+			pRxContext->InUse		= FALSE;
+			pRxContext->IRPPending	= FALSE;
+			pRxContext->Readable	= FALSE;
+//			pRxContext->ReorderInUse = FALSE;
+//			pRxContext->ReadPosOffset = 0;
+		}
+
+/*=========================================================================*/
+/*      Reset Tx RING                                                      */
+/*=========================================================================*/
+		do
+		{
+			RTUSBCancelPendingBulkOutIRP(pAd);
+		} while (FALSE);
+
+/*=========================================================================*/
+		// Enable auto responder.
+		RTMP_IO_READ32(pAd, AUTO_RSP_CFG, &temp);
+		temp = temp | (0x01);
+		RTMP_IO_WRITE32(pAd, AUTO_RSP_CFG, temp);
+
+/*================================================*/
+		AsicEnableBssSync(pAd);
+
+		/* Soft reset BBP.*/
+		/* In 2870 chipset, ATE_BBP_IO_READ8_BY_REG_ID() == RTMP_BBP_IO_READ8_BY_REG_ID() */
+		/* Both rt2870ap and rt2870sta use BbpSoftReset(pAd) to do BBP soft reset */
+		BbpSoftReset(pAd);
+/*================================================*/
+		{
+#ifdef CONFIG_STA_SUPPORT
+			// Set all state machines back IDLE
+			pAd->Mlme.CntlMachine.CurrState    = CNTL_IDLE;
+			pAd->Mlme.AssocMachine.CurrState   = ASSOC_IDLE;
+			pAd->Mlme.AuthMachine.CurrState    = AUTH_REQ_IDLE;
+			pAd->Mlme.AuthRspMachine.CurrState = AUTH_RSP_IDLE;
+			pAd->Mlme.SyncMachine.CurrState    = SYNC_IDLE;
+			pAd->Mlme.ActMachine.CurrState    = ACT_IDLE;
+#endif // CONFIG_STA_SUPPORT //
+
+			//
+			// ===> refer to MlmeRestartStateMachine().
+			// When we entered ATE_START mode, PeriodicTimer was not cancelled.
+			// So we don't have to set it here.
+			//
+			//RTMPSetTimer(pAd, &pAd->Mlme.PeriodicTimer, MLME_TASK_EXEC_INTV);
+
+			ASSERT(pAd->CommonCfg.Channel != 0);
+
+			AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+			AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+
+
+#ifdef CONFIG_STA_SUPPORT
+		    RTMPStationStart(pAd);
+#endif // CONFIG_STA_SUPPORT //
+		}
+//
+// These two steps have been done when entering ATE_STOP mode.
+//
+#if 0
+	    RTUSBWriteBBPRegister(pAd, BBP_R22, BbpData);
+	   	RTUSBWriteMACRegister(pAd, MAC_SYS_CTRL, MacData);
+#endif
+		// Clean ATE Bulk in/out counter and continue setup.
+		InterlockedExchange(&pAd->BulkOutRemained, 0);
+		NdisAcquireSpinLock(&pAd->GenericLock);
+		pAd->ContinBulkOut = FALSE;
+		pAd->ContinBulkIn = FALSE;
+		NdisReleaseSpinLock(&pAd->GenericLock);
+
+		/* Wait 50ms to prevent next URB to bulkout during HW reset. */
+		/* todo : remove this if not necessary */
+		NdisMSleep(50000);
+
+		pAd->ate.Mode = ATE_STOP;
+
+		// Enable Tx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value |= (1 << 2);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+/*=========================================================================*/
+		/* restore RX_FILTR_CFG */
+#ifdef CONFIG_STA_SUPPORT
+		/* restore RX_FILTR_CFG in order that QA maybe set it to 0x3 */
+		RTMP_IO_WRITE32(pAd, RX_FILTR_CFG, STANORMAL);
+#endif // CONFIG_STA_SUPPORT //
+/*=========================================================================*/
+
+		// Enable Tx, RX DMA.
+		RtmpDmaEnable(pAd, 1);
+
+		// Enable Rx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value |= (1 << 3);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		// Wait 10ms to wait all of the bulk-in URBs to complete.
+		/* todo : remove this if not necessary */
+		NdisMSleep(10000);
+
+		// Everything is ready to start normal Tx/Rx.
+		RTUSBBulkReceive(pAd);
+		netif_start_queue(pAd->net_dev);
+
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("<=== ATE : ATESTOP \n"));
+	}
+	else if (!strcmp(arg, "TXCARR"))	// Tx Carrier
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ATE: TXCARR\n"));
+		pAd->ate.Mode |= ATE_TXCARR;
+
+		// Disable Rx
+		// May be we need not to do this, because these have been done in ATE_START mode ???
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 3);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		// QA has done the following steps if it is used.
+		if (pAd->ate.bQATxStart == FALSE)
+		{
+			// Soft reset BBP.
+			BbpSoftReset(pAd);
+
+			// Carrier Test set BBP R22 bit7=1, bit6=1, bit[5~0]=0x01
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R22, &BbpData);
+			BbpData &= 0xFFFFFF00; //clear bit7, bit6, bit[5~0]
+			BbpData |= 0x000000C1; //set bit7=1, bit6=1, bit[5~0]=0x01
+			ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+
+			// set MAC_SYS_CTRL(0x1004) Continuous Tx Production Test (bit4) = 1
+			RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+			Value = Value | 0x00000010;
+			RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+		}
+	}
+	else if (!strcmp(arg, "TXCONT"))	// Tx Continue
+	{
+		if (pAd->ate.bQATxStart == TRUE)
+		{
+			/* set MAC_SYS_CTRL(0x1004) bit4(Continuous Tx Production Test)
+			   and bit2(MAC TX enable) back to zero. */
+			RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &MacData);
+			MacData &= 0xFFFFFFEB;
+			RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, MacData);
+
+			// set BBP R22 bit7=0
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R22, &BbpData);
+			BbpData &= 0xFFFFFF7F; //set bit7=0
+			ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+		}
+
+		/* for TxCont mode.
+		** Step 1: Send 50 packets first then wait for a moment.
+		** Step 2: Send more 50 packet then start continue mode.
+		*/
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ATE: TXCONT\n"));
+		// Step 1: send 50 packets first.
+		pAd->ate.Mode |= ATE_TXCONT;
+		pAd->ate.TxCount = 50;
+		pAd->ate.TxDoneCount = 0;
+
+		// Soft reset BBP.
+		BbpSoftReset(pAd);
+
+		// Abort Tx, RX DMA.
+		RtmpDmaEnable(pAd, 0);
+
+
+		/* Only needed if we have to send some normal frames. */
+		SetJapanFilter(pAd);
+
+		// Setup frame format.
+		ATESetUpFrame(pAd, 0);
+
+		// Enable Tx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value |= (1 << 2);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		// Disable Rx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 3);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		// Start Tx, RX DMA.
+		RtmpDmaEnable(pAd, 1);
+
+		InterlockedExchange(&pAd->BulkOutRemained, pAd->ate.TxCount);
+
+#ifdef	RALINK_28xx_QA
+		if (pAd->ate.bQATxStart == TRUE)
+		{
+			pAd->ate.TxStatus = 1;
+			//pAd->ate.Repeat = 0;
+		}
+#endif	// RALINK_28xx_QA //
+
+		NdisAcquireSpinLock(&pAd->GenericLock);//0820
+		pAd->ContinBulkOut = FALSE;
+		NdisReleaseSpinLock(&pAd->GenericLock);
+
+		RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_ATE);
+
+		// Kick bulk out
+		RTUSBKickBulkOut(pAd);
+
+		/* To make sure all the 50 frames have been bulk out before executing step 2 */
+		while (atomic_read(&pAd->BulkOutRemained) > 0)
+		{
+			RTMPusecDelay(5000);
+		}
+
+		// Step 2: send more 50 packets then start continue mode.
+		// Abort Tx, RX DMA.
+		RtmpDmaEnable(pAd, 0);
+
+		// Cont. TX set BBP R22 bit7=1
+		ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R22, &BbpData);
+		BbpData |= 0x00000080; //set bit7=1
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+
+		pAd->ate.TxCount = 50;
+		pAd->ate.TxDoneCount = 0;
+
+		SetJapanFilter(pAd);
+
+		// Setup frame format.
+		ATESetUpFrame(pAd, 0);
+
+		// Enable Tx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value |= (1 << 2);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		// Disable Rx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 3);
+
+		// Start Tx, RX DMA.
+		RtmpDmaEnable(pAd, 1);
+
+		InterlockedExchange(&pAd->BulkOutRemained, pAd->ate.TxCount);
+
+#ifdef	RALINK_28xx_QA
+		if (pAd->ate.bQATxStart == TRUE)
+		{
+			pAd->ate.TxStatus = 1;
+			//pAd->ate.Repeat = 0;
+		}
+#endif	// RALINK_28xx_QA //
+
+		NdisAcquireSpinLock(&pAd->GenericLock);//0820
+		pAd->ContinBulkOut = FALSE;
+		NdisReleaseSpinLock(&pAd->GenericLock);
+
+		RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_ATE);
+		// Kick bulk out
+		RTUSBKickBulkOut(pAd);
+
+#if 1
+		RTMPusecDelay(500);
+#else
+		while (atomic_read(&pAd->BulkOutRemained) > 0)
+		{
+			RTMPusecDelay(5000);
+		}
+#endif // 1 //
+
+		// Set MAC_SYS_CTRL(0x1004) Continuous Tx Production Test (bit4) = 1.
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &MacData);
+		MacData |= 0x00000010;
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, MacData);
+	}
+	else if (!strcmp(arg, "TXFRAME"))	// Tx Frames
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ATE: TXFRAME(Count=0x%08x)\n", pAd->ate.TxCount));
+		pAd->ate.Mode |= ATE_TXFRAME;
+
+		// Soft reset BBP.
+		BbpSoftReset(pAd);
+
+		// Default value in BBP R22 is 0x0.
+		BbpData = 0;
+
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &MacData);
+
+		// Clean bit4 to stop continuous Tx production test.
+		MacData &= 0xFFFFFFEF;
+
+	   	ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, MacData);
+
+#ifdef RALINK_28xx_QA
+		// add this for LoopBack mode
+		if (pAd->ate.bQARxStart == FALSE)
+		{
+			// Disable Rx
+			RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+			Value &= ~(1 << 3);
+			RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+		}
+
+		if (pAd->ate.bQATxStart == TRUE)
+		{
+			pAd->ate.TxStatus = 1;
+			//pAd->ate.Repeat = 0;
+		}
+#else
+		// Disable Rx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 3);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+#endif // RALINK_28xx_QA //
+
+		// Enable Tx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value |= (1 << 2);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+        SetJapanFilter(pAd);
+
+		// Abort Tx, RX DMA.
+		RtmpDmaEnable(pAd, 0);
+
+		pAd->ate.TxDoneCount = 0;
+
+        // Setup frame format
+		ATESetUpFrame(pAd, 0);
+
+		// Start Tx, RX DMA.
+		RtmpDmaEnable(pAd, 1);
+
+		// Check count is continuous or not yet.
+		//
+		// Due to the type mismatch between "pAd->BulkOutRemained"(atomic_t) and "pAd->ate.TxCount"(UINT32)
+		//
+		if (pAd->ate.TxCount == 0)
+		{
+			InterlockedExchange(&pAd->BulkOutRemained, 0);
+		}
+		else
+		{
+			InterlockedExchange(&pAd->BulkOutRemained, pAd->ate.TxCount);
+		}
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("bulk out count = %d\n", atomic_read(&pAd->BulkOutRemained)));
+		ASSERT((atomic_read(&pAd->BulkOutRemained) >= 0));
+
+		if (atomic_read(&pAd->BulkOutRemained) == 0)
+		{
+			ATEDBGPRINT(RT_DEBUG_TRACE, ("Send packet countinuously\n"));
+
+			/* In 28xx, NdisAcquireSpinLock() == spin_lock_bh() */
+			/* NdisAcquireSpinLock only need one argument in 28xx. */
+			NdisAcquireSpinLock(&pAd->GenericLock);
+			pAd->ContinBulkOut = TRUE;
+			NdisReleaseSpinLock(&pAd->GenericLock);
+
+			/* In 28xx, BULK_OUT_LOCK() == spin_lock_irqsave() */
+			BULK_OUT_LOCK(&pAd->BulkOutLock[0], IrqFlags);// peter : NdisAcquireSpinLock ==> BULK_OUT_LOCK
+			pAd->BulkOutPending[0] = FALSE;
+			BULK_OUT_UNLOCK(&pAd->BulkOutLock[0], IrqFlags);// peter : NdisAcquireSpinLock ==> BULK_OUT_LOCK
+		}
+		else
+		{
+			ATEDBGPRINT(RT_DEBUG_TRACE, ("Send packets depend on counter\n"));
+
+			NdisAcquireSpinLock(&pAd->GenericLock);
+			pAd->ContinBulkOut = FALSE;
+			NdisReleaseSpinLock(&pAd->GenericLock);
+
+			BULK_OUT_LOCK(&pAd->BulkOutLock[0], IrqFlags);
+			pAd->BulkOutPending[0] = FALSE;
+			BULK_OUT_UNLOCK(&pAd->BulkOutLock[0], IrqFlags);
+		}
+
+		RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_ATE);
+
+		// Kick bulk out
+		RTUSBKickBulkOut(pAd);
+	}
+#ifdef RALINK_28xx_QA
+	else if (!strcmp(arg, "TXSTOP")) 		//Enter ATE mode and set Tx/Rx Idle
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ATE: TXSTOP\n"));
+
+		atemode = pAd->ate.Mode;
+		pAd->ate.Mode &= ATE_TXSTOP;
+		pAd->ate.bQATxStart = FALSE;
+//		pAd->ate.TxDoneCount = pAd->ate.TxCount;
+
+/*=========================================================================*/
+		if (atemode & ATE_TXCARR)
+		{
+			// No Carrier Test set BBP R22 bit7=0, bit6=0, bit[5~0]=0x0
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R22, &BbpData);
+			BbpData &= 0xFFFFFF00; //clear bit7, bit6, bit[5~0]
+		    ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+		}
+		else if (atemode & ATE_TXCARRSUPP)
+		{
+			// No Cont. TX set BBP R22 bit7=0
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R22, &BbpData);
+			BbpData &= ~(1 << 7); //set bit7=0
+			ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+
+			// No Carrier Suppression set BBP R24 bit0=0
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R24, &BbpData);
+			BbpData &= 0xFFFFFFFE; //clear bit0
+		    ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R24, BbpData);
+		}
+		else if ((atemode & ATE_TXFRAME) || (atemode == ATE_STOP))
+		{
+			if (atemode & ATE_TXCONT)
+			{
+				// No Cont. TX set BBP R22 bit7=0
+				ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R22, &BbpData);
+				BbpData &= ~(1 << 7); //set bit7=0
+				ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+			}
+		}
+
+/*=========================================================================*/
+		RTUSBRejectPendingPackets(pAd);
+		RTUSBCleanUpDataBulkOutQueue(pAd);
+
+		/* not used in RT28xx */
+		//RTUSBCleanUpMLMEWaitQueue(pAd);
+		/* empty function so far */
+		RTUSBCleanUpMLMEBulkOutQueue(pAd);
+/*=========================================================================*/
+		// Abort Tx, RX DMA.
+		RtmpDmaEnable(pAd, 0);
+/*=========================================================================*/
+
+		/* In 28xx, pAd->PendingRx is not of type atomic_t anymore */
+//		while ((atomic_read(&pAd->PendingRx) > 0))	//pAd->BulkFlags != 0 wait bulk out finish
+		/* peter todo : BulkInLock */
+		while (pAd->PendingRx > 0)
+		{
+#if 1
+			ATE_RTUSBCancelPendingBulkInIRP(pAd);
+#else
+//			NdisInterlockedDecrement(&pAd->PendingRx);
+			pAd->PendingRx--;
+#endif
+			RTMPusecDelay(500000);
+		}
+
+		while (((pAd->BulkOutPending[0] == TRUE) ||
+				(pAd->BulkOutPending[1] == TRUE) ||
+				(pAd->BulkOutPending[2] == TRUE) ||
+				(pAd->BulkOutPending[3] == TRUE)) && (pAd->BulkFlags != 0))	//pAd->BulkFlags != 0 wait bulk out finish
+		{
+			do
+			{
+				RTUSBCancelPendingBulkOutIRP(pAd);
+			} while (FALSE);
+
+			RTMPusecDelay(500000);
+		}
+
+		ASSERT(pAd->PendingRx == 0);
+/*=========================================================================*/
+		// Enable Tx, Rx DMA.
+		RtmpDmaEnable(pAd, 1);
+
+		/* task Tx status : 0 --> task is idle, 1 --> task is running */
+		pAd->ate.TxStatus = 0;
+
+		// Soft reset BBP.
+		BbpSoftReset(pAd);
+
+		// Disable Tx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &MacData);
+		MacData &= (0xfffffffb);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, MacData);
+
+		//Clean ATE Bulk in/out counter and continue setup
+		InterlockedExchange(&pAd->BulkOutRemained, 0);
+
+		pAd->ContinBulkOut = FALSE;
+	}
+	else if (!strcmp(arg, "RXSTOP"))
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ATE: RXSTOP\n"));
+		atemode = pAd->ate.Mode;
+
+		// Disable Rx
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 3);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		pAd->ate.Mode &= ATE_RXSTOP;
+		pAd->ate.bQARxStart = FALSE;
+//		pAd->ate.TxDoneCount = pAd->ate.TxCount;
+
+/*=========================================================================*/
+		RTUSBRejectPendingPackets(pAd);
+		RTUSBCleanUpDataBulkOutQueue(pAd);
+
+		/* not used in RT28xx */
+		//RTUSBCleanUpMLMEWaitQueue(pAd);
+		RTUSBCleanUpMLMEBulkOutQueue(pAd);
+/*=========================================================================*/
+
+		// Abort Tx, RX DMA.
+		RtmpDmaEnable(pAd, 0);
+/*=========================================================================*/
+//		while ((atomic_read(&pAd->PendingRx) > 0))
+		while (pAd->PendingRx > 0)
+		{
+#if 1
+			ATE_RTUSBCancelPendingBulkInIRP(pAd);
+#else
+//			NdisInterlockedDecrement(&pAd->PendingRx);
+			pAd->PendingRx--;
+#endif
+			RTMPusecDelay(500000);
+		}
+
+		while (((pAd->BulkOutPending[0] == TRUE) ||
+				(pAd->BulkOutPending[1] == TRUE) ||
+				(pAd->BulkOutPending[2] == TRUE) ||
+				(pAd->BulkOutPending[3] == TRUE)) && (pAd->BulkFlags != 0))	//pAd->BulkFlags != 0 wait bulk out finish
+		{
+			do
+			{
+				RTUSBCancelPendingBulkOutIRP(pAd);
+			} while (FALSE);
+
+			RTMPusecDelay(500000);
+		}
+
+		ASSERT(pAd->PendingRx == 0);
+/*=========================================================================*/
+
+		// Soft reset BBP.
+		BbpSoftReset(pAd);
+		pAd->ContinBulkIn = FALSE;
+	}
+#endif // RALINK_28xx_QA //
+	else if (!strcmp(arg, "RXFRAME")) // Rx Frames
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ATE: RXFRAME\n"));
+
+		// Disable Rx of MAC block
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 3);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		// Default value in BBP R22 is 0x0.
+		BbpData = 0;
+
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &MacData);
+		// Clean bit4 to stop continuous Tx production test.
+		MacData &= 0xFFFFFFEF;
+
+	   	ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R22, BbpData);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, MacData);
+
+		pAd->ate.Mode |= ATE_RXFRAME;
+
+		// Abort Tx, RX DMA.
+		RtmpDmaEnable(pAd, 0);
+
+		// Disable TX of MAC block
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value &= ~(1 << 2);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+        // Reset Rx RING.
+		for ( i = 0; i < (RX_RING_SIZE); i++)
+		{
+			PRX_CONTEXT  pRxContext = &(pAd->RxContext[i]);
+
+			pRxContext->InUse = FALSE;
+			pRxContext->IRPPending = FALSE;
+			pRxContext->Readable = FALSE;
+
+			//
+			// Get the urb from kernel back to driver.
+			//
+			RTUSB_UNLINK_URB(pRxContext->pUrb);
+
+			/* Sleep 200 microsecs to give cancellation time to work. */
+			NdisMSleep(200);
+			pAd->BulkInReq = 0;
+
+//			InterlockedExchange(&pAd->PendingRx, 0);
+			pAd->PendingRx = 0;
+			pAd->NextRxBulkInReadIndex = 0;	// Next Rx Read index
+			pAd->NextRxBulkInIndex		= RX_RING_SIZE - 1;	// Rx Bulk pointer
+			pAd->NextRxBulkInPosition = 0;
+		}
+
+		// read to clear counters
+		RTUSBReadMACRegister(pAd, RX_STA_CNT0, &temp); //RX PHY & RX CRC count
+		RTUSBReadMACRegister(pAd, RX_STA_CNT1, &temp); //RX PLCP error count & CCA false alarm count
+		RTUSBReadMACRegister(pAd, RX_STA_CNT2, &temp); //RX FIFO overflow frame count & RX duplicated filtered frame count
+
+		pAd->ContinBulkIn = TRUE;
+
+		// Enable Tx, RX DMA.
+		RtmpDmaEnable(pAd, 1);
+
+		// Enable RX of MAC block
+		RTMP_IO_READ32(pAd, MAC_SYS_CTRL, &Value);
+		Value |= (1 << 3);
+		RTMP_IO_WRITE32(pAd, MAC_SYS_CTRL, Value);
+
+		// Kick bulk in
+		RTUSBBulkReceive(pAd);
+	}
+	else
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ATE: Invalid arg!\n"));
+		return FALSE;
+	}
+	RTMPusecDelay(5000);
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("<=== ATECmdHandler()\n"));
+
+	return TRUE;
+}
+#endif // RT2870 //
+
+INT	Set_ATE_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	if (ATECmdHandler(pAd, arg))
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_Proc Success\n"));
+
+
+		return TRUE;
+	}
+	else
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_Proc Failed\n"));
+		return FALSE;
+	}
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE ADDR1=DA for TxFrame(AP  : To DS = 0 ; From DS = 1)
+        or
+        Set ATE ADDR3=DA for TxFrame(STA : To DS = 1 ; From DS = 0)
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_DA_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	CHAR				*value;
+	INT					i;
+
+	if(strlen(arg) != 17)  //Mac address acceptable format 01:02:03:04:05:06 length 17
+		return FALSE;
+
+    for (i=0, value = rstrtok(arg, ":"); value; value = rstrtok(NULL, ":"))
+	{
+		if((strlen(value) != 2) || (!isxdigit(*value)) || (!isxdigit(*(value+1))) )
+			return FALSE;  //Invalid
+
+
+#ifdef CONFIG_STA_SUPPORT
+		AtoH(value, &pAd->ate.Addr3[i++], 1);
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+	if(i != 6)
+		return FALSE;  //Invalid
+
+
+#ifdef CONFIG_STA_SUPPORT
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_DA_Proc (DA = %2X:%2X:%2X:%2X:%2X:%2X)\n", pAd->ate.Addr3[0],
+		pAd->ate.Addr3[1], pAd->ate.Addr3[2], pAd->ate.Addr3[3], pAd->ate.Addr3[4], pAd->ate.Addr3[5]));
+#endif // CONFIG_STA_SUPPORT //
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_DA_Proc Success\n"));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE ADDR3=SA for TxFrame(AP  : To DS = 0 ; From DS = 1)
+        or
+        Set ATE ADDR2=SA for TxFrame(STA : To DS = 1 ; From DS = 0)
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_SA_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	CHAR				*value;
+	INT					i;
+
+	if(strlen(arg) != 17)  //Mac address acceptable format 01:02:03:04:05:06 length 17
+		return FALSE;
+
+    for (i=0, value = rstrtok(arg, ":"); value; value = rstrtok(NULL, ":"))
+	{
+		if((strlen(value) != 2) || (!isxdigit(*value)) || (!isxdigit(*(value+1))) )
+			return FALSE;  //Invalid
+
+
+#ifdef CONFIG_STA_SUPPORT
+		AtoH(value, &pAd->ate.Addr2[i++], 1);
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+	if(i != 6)
+		return FALSE;  //Invalid
+
+
+#ifdef CONFIG_STA_SUPPORT
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_SA_Proc (SA = %2X:%2X:%2X:%2X:%2X:%2X)\n", pAd->ate.Addr2[0],
+		pAd->ate.Addr2[1], pAd->ate.Addr2[2], pAd->ate.Addr2[3], pAd->ate.Addr2[4], pAd->ate.Addr2[5]));
+#endif // CONFIG_STA_SUPPORT //
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_SA_Proc Success\n"));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE ADDR2=BSSID for TxFrame(AP  : To DS = 0 ; From DS = 1)
+        or
+        Set ATE ADDR1=BSSID for TxFrame(STA : To DS = 1 ; From DS = 0)
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_BSSID_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	CHAR				*value;
+	INT					i;
+
+	if(strlen(arg) != 17)  //Mac address acceptable format 01:02:03:04:05:06 length 17
+		return FALSE;
+
+    for (i=0, value = rstrtok(arg, ":"); value; value = rstrtok(NULL, ":"))
+	{
+		if((strlen(value) != 2) || (!isxdigit(*value)) || (!isxdigit(*(value+1))) )
+			return FALSE;  //Invalid
+
+
+#ifdef CONFIG_STA_SUPPORT
+		AtoH(value, &pAd->ate.Addr1[i++], 1);
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+	if(i != 6)
+		return FALSE;  //Invalid
+
+
+#ifdef CONFIG_STA_SUPPORT
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_BSSID_Proc (BSSID = %2X:%2X:%2X:%2X:%2X:%2X)\n",	pAd->ate.Addr1[0],
+		pAd->ate.Addr1[1], pAd->ate.Addr1[2], pAd->ate.Addr1[3], pAd->ate.Addr1[4], pAd->ate.Addr1[5]));
+#endif // CONFIG_STA_SUPPORT //
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_BSSID_Proc Success\n"));
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Tx Channel
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_CHANNEL_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UCHAR channel;
+
+	channel = simple_strtol(arg, 0, 10);
+
+	if ((channel < 1) || (channel > 216))// to allow A band channel : ((channel < 1) || (channel > 14))
+	{
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_CHANNEL_Proc::Out of range, it should be in range of 1~14.\n"));
+		return FALSE;
+	}
+	pAd->ate.Channel = channel;
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_CHANNEL_Proc (ATE Channel = %d)\n", pAd->ate.Channel));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_CHANNEL_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Tx Power0
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_POWER0_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	CHAR TxPower;
+
+	TxPower = simple_strtol(arg, 0, 10);
+
+	if (pAd->ate.Channel <= 14)
+	{
+		if ((TxPower > 31) || (TxPower < 0))
+		{
+			ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_POWER0_Proc::Out of range (Value=%d)\n", TxPower));
+			return FALSE;
+		}
+	}
+	else// 5.5GHz
+	{
+		if ((TxPower > 15) || (TxPower < -7))
+		{
+			ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_POWER0_Proc::Out of range (Value=%d)\n", TxPower));
+			return FALSE;
+		}
+	}
+
+	pAd->ate.TxPower0 = TxPower;
+	ATETxPwrHandler(pAd, 0);
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_POWER0_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Tx Power1
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_POWER1_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	CHAR TxPower;
+
+	TxPower = simple_strtol(arg, 0, 10);
+
+	if (pAd->ate.Channel <= 14)
+	{
+	if ((TxPower > 31) || (TxPower < 0))
+	{
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_POWER1_Proc::Out of range (Value=%d)\n", TxPower));
+		return FALSE;
+	}
+	}
+	else
+	{
+		if ((TxPower > 15) || (TxPower < -7))
+		{
+			ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_POWER1_Proc::Out of range (Value=%d)\n", TxPower));
+			return FALSE;
+		}
+	}
+
+	pAd->ate.TxPower1 = TxPower;
+	ATETxPwrHandler(pAd, 1);
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_POWER1_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Tx Antenna
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_Antenna_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	CHAR value;
+
+	value = simple_strtol(arg, 0, 10);
+
+	if ((value > 2) || (value < 0))
+	{
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_Antenna_Proc::Out of range (Value=%d)\n", value));
+		return FALSE;
+	}
+
+	pAd->ate.TxAntennaSel = value;
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_TX_Antenna_Proc (Antenna = %d)\n", pAd->ate.TxAntennaSel));
+	ATEDBGPRINT(RT_DEBUG_TRACE,("Ralink: Set_ATE_TX_Antenna_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Rx Antenna
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_RX_Antenna_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	CHAR value;
+
+	value = simple_strtol(arg, 0, 10);
+
+	if ((value > 3) || (value < 0))
+	{
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_RX_Antenna_Proc::Out of range (Value=%d)\n", value));
+		return FALSE;
+	}
+
+	pAd->ate.RxAntennaSel = value;
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_RX_Antenna_Proc (Antenna = %d)\n", pAd->ate.RxAntennaSel));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_RX_Antenna_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE RF frequence offset
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_FREQOFFSET_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UCHAR RFFreqOffset;
+	ULONG R4;
+
+	RFFreqOffset = simple_strtol(arg, 0, 10);
+
+	if(RFFreqOffset >= 64)
+	{
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_FREQOFFSET_Proc::Out of range, it should be in range of 0~63.\n"));
+		return FALSE;
+	}
+
+	pAd->ate.RFFreqOffset = RFFreqOffset;
+	R4 = pAd->ate.RFFreqOffset << 15;		// shift TX power control to correct RF register bit position
+	R4 |= (pAd->LatchRfRegs.R4 & ((~0x001f8000)));
+	pAd->LatchRfRegs.R4 = R4;
+
+	RtmpRfIoWrite(pAd);
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_TX_FREQOFFSET_Proc (RFFreqOffset = %d)\n", pAd->ate.RFFreqOffset));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_FREQOFFSET_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE RF BW
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_BW_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	int i;
+	UCHAR value = 0;
+	UCHAR BBPCurrentBW;
+
+	BBPCurrentBW = simple_strtol(arg, 0, 10);
+
+	if(BBPCurrentBW == 0)
+		pAd->ate.TxWI.BW = BW_20;
+	else
+		pAd->ate.TxWI.BW = BW_40;
+
+	if(pAd->ate.TxWI.BW == BW_20)
+	{
+		if(pAd->ate.Channel <= 14)
+		{
+ 		for (i=0; i<5; i++)
+ 		{
+				if (pAd->Tx20MPwrCfgGBand[i] != 0xffffffff)
+				{
+					RTMP_IO_WRITE32(pAd, TX_PWR_CFG_0 + i*4, pAd->Tx20MPwrCfgGBand[i]);
+					RTMPusecDelay(5000);
+				}
+			}
+		}
+		else
+		{
+			for (i=0; i<5; i++)
+			{
+				if (pAd->Tx20MPwrCfgABand[i] != 0xffffffff)
+ 			{
+					RTMP_IO_WRITE32(pAd, TX_PWR_CFG_0 + i*4, pAd->Tx20MPwrCfgABand[i]);
+ 				RTMPusecDelay(5000);
+ 			}
+ 		}
+		}
+
+		//Set BBP R4 bit[4:3]=0:0
+ 		ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &value);
+ 		value &= (~0x18);
+ 		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, value);
+
+  		//Set BBP R66=0x3C
+ 		value = 0x3C;
+ 		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, value);
+		//Set BBP R68=0x0B
+		//to improve Rx sensitivity.
+		value = 0x0B;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R68, value);
+		//Set BBP R69=0x16
+		value = 0x16;
+ 		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, value);
+		//Set BBP R70=0x08
+		value = 0x08;
+ 		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, value);
+		//Set BBP R73=0x11
+		value = 0x11;
+ 		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R73, value);
+
+	    // If Channel=14, Bandwidth=20M and Mode=CCK, Set BBP R4 bit5=1
+	    // (Japan filter coefficients)
+	    // This segment of code will only works when ATETXMODE and ATECHANNEL
+	    // were set to MODE_CCK and 14 respectively before ATETXBW is set to 0.
+	    //=====================================================================
+		if (pAd->ate.Channel == 14)
+		{
+			int TxMode = pAd->ate.TxWI.PHYMODE;
+			if (TxMode == MODE_CCK)
+			{
+				// when Channel==14 && Mode==CCK && BandWidth==20M, BBP R4 bit5=1
+ 				ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &value);
+				value |= 0x20; //set bit5=1
+ 				ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, value);
+			}
+		}
+
+	    //=====================================================================
+		// If bandwidth != 40M, RF Reg4 bit 21 = 0.
+		pAd->LatchRfRegs.R4 &= ~0x00200000;
+		RtmpRfIoWrite(pAd);
+	}
+	else if(pAd->ate.TxWI.BW == BW_40)
+	{
+		if(pAd->ate.Channel <= 14)
+		{
+			for (i=0; i<5; i++)
+			{
+				if (pAd->Tx40MPwrCfgGBand[i] != 0xffffffff)
+				{
+					RTMP_IO_WRITE32(pAd, TX_PWR_CFG_0 + i*4, pAd->Tx40MPwrCfgGBand[i]);
+					RTMPusecDelay(5000);
+				}
+			}
+		}
+		else
+		{
+			for (i=0; i<5; i++)
+			{
+				if (pAd->Tx40MPwrCfgABand[i] != 0xffffffff)
+				{
+					RTMP_IO_WRITE32(pAd, TX_PWR_CFG_0 + i*4, pAd->Tx40MPwrCfgABand[i]);
+					RTMPusecDelay(5000);
+				}
+			}
+#ifdef DOT11_N_SUPPORT
+			if ((pAd->ate.TxWI.PHYMODE >= MODE_HTMIX) && (pAd->ate.TxWI.MCS == 7))
+			{
+    			value = 0x28;
+    			ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R67, value);
+			}
+#endif // DOT11_N_SUPPORT //
+		}
+
+		//Set BBP R4 bit[4:3]=1:0
+		ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &value);
+		value &= (~0x18);
+		value |= 0x10;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, value);
+
+		//Set BBP R66=0x3C
+		value = 0x3C;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, value);
+		//Set BBP R68=0x0C
+		//to improve Rx sensitivity.
+		value = 0x0C;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R68, value);
+		//Set BBP R69=0x1A
+		value = 0x1A;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, value);
+		//Set BBP R70=0x0A
+		value = 0x0A;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, value);
+		//Set BBP R73=0x16
+		value = 0x16;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R73, value);
+
+		// If bandwidth = 40M, set RF Reg4 bit 21 = 1.
+		pAd->LatchRfRegs.R4 |= 0x00200000;
+		RtmpRfIoWrite(pAd);
+	}
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_TX_BW_Proc (BBPCurrentBW = %d)\n", pAd->ate.TxWI.BW));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_BW_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Tx frame length
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_LENGTH_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	pAd->ate.TxLength = simple_strtol(arg, 0, 10);
+
+	if((pAd->ate.TxLength < 24) || (pAd->ate.TxLength > (MAX_FRAME_SIZE - 34/* == 2312 */)))
+	{
+		pAd->ate.TxLength = (MAX_FRAME_SIZE - 34/* == 2312 */);
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_LENGTH_Proc::Out of range, it should be in range of 24~%d.\n", (MAX_FRAME_SIZE - 34/* == 2312 */)));
+		return FALSE;
+	}
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_TX_LENGTH_Proc (TxLength = %d)\n", pAd->ate.TxLength));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_LENGTH_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Tx frame count
+
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_COUNT_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	pAd->ate.TxCount = simple_strtol(arg, 0, 10);
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_TX_COUNT_Proc (TxCount = %d)\n", pAd->ate.TxCount));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_COUNT_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Tx frame MCS
+
+        Return:
+        	TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_MCS_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UCHAR MCS;
+	int result;
+
+	MCS = simple_strtol(arg, 0, 10);
+	result = CheckMCSValid(pAd->ate.TxWI.PHYMODE, MCS);
+
+	if (result != -1)
+	{
+		pAd->ate.TxWI.MCS = (UCHAR)MCS;
+	}
+	else
+	{
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_MCS_Proc::Out of range, refer to rate table.\n"));
+		return FALSE;
+	}
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_TX_MCS_Proc (MCS = %d)\n", pAd->ate.TxWI.MCS));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_MCS_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Tx frame Mode
+        0: MODE_CCK
+        1: MODE_OFDM
+        2: MODE_HTMIX
+        3: MODE_HTGREENFIELD
+
+        Return:
+        	TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_MODE_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	pAd->ate.TxWI.PHYMODE = simple_strtol(arg, 0, 10);
+
+	if(pAd->ate.TxWI.PHYMODE > 3)
+	{
+		pAd->ate.TxWI.PHYMODE = 0;
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_MODE_Proc::Out of range. it should be in range of 0~3\n"));
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("0: CCK, 1: OFDM, 2: HT_MIX, 3: HT_GREEN_FIELD.\n"));
+		return FALSE;
+	}
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_TX_MODE_Proc (TxMode = %d)\n", pAd->ate.TxWI.PHYMODE));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_MODE_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set ATE Tx frame GI
+
+        Return:
+        	TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_ATE_TX_GI_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	pAd->ate.TxWI.ShortGI = simple_strtol(arg, 0, 10);
+
+	if(pAd->ate.TxWI.ShortGI > 1)
+	{
+		pAd->ate.TxWI.ShortGI = 0;
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("Set_ATE_TX_GI_Proc::Out of range\n"));
+		return FALSE;
+	}
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_TX_GI_Proc (GI = %d)\n", pAd->ate.TxWI.ShortGI));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_TX_GI_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+    ==========================================================================
+ */
+INT	Set_ATE_RX_FER_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	pAd->ate.bRxFer = simple_strtol(arg, 0, 10);
+
+	if (pAd->ate.bRxFer == 1)
+	{
+		pAd->ate.RxCntPerSec = 0;
+		pAd->ate.RxTotalCnt = 0;
+	}
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Set_ATE_RX_FER_Proc (bRxFer = %d)\n", pAd->ate.bRxFer));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("Ralink: Set_ATE_RX_FER_Proc Success\n"));
+
+
+	return TRUE;
+}
+
+INT Set_ATE_Read_RF_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ate_print(KERN_EMERG "R1 = %lx\n", pAd->LatchRfRegs.R1);
+	ate_print(KERN_EMERG "R2 = %lx\n", pAd->LatchRfRegs.R2);
+	ate_print(KERN_EMERG "R3 = %lx\n", pAd->LatchRfRegs.R3);
+	ate_print(KERN_EMERG "R4 = %lx\n", pAd->LatchRfRegs.R4);
+
+	return TRUE;
+}
+
+INT Set_ATE_Write_RF1_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UINT32 value = simple_strtol(arg, 0, 16);
+
+	pAd->LatchRfRegs.R1 = value;
+	RtmpRfIoWrite(pAd);
+
+	return TRUE;
+}
+
+INT Set_ATE_Write_RF2_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UINT32 value = simple_strtol(arg, 0, 16);
+
+	pAd->LatchRfRegs.R2 = value;
+	RtmpRfIoWrite(pAd);
+
+	return TRUE;
+}
+
+INT Set_ATE_Write_RF3_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UINT32 value = simple_strtol(arg, 0, 16);
+
+	pAd->LatchRfRegs.R3 = value;
+	RtmpRfIoWrite(pAd);
+
+	return TRUE;
+}
+
+INT Set_ATE_Write_RF4_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UINT32 value = simple_strtol(arg, 0, 16);
+
+	pAd->LatchRfRegs.R4 = value;
+	RtmpRfIoWrite(pAd);
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Load and Write EEPROM from a binary file prepared in advance.
+
+        Return:
+        	TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+#ifndef UCOS
+INT Set_ATE_Load_E2P_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	BOOLEAN		    ret = FALSE;
+	PUCHAR			src = EEPROM_BIN_FILE_NAME;
+	struct file		*srcf;
+	INT32 			retval, orgfsuid, orgfsgid;
+   	mm_segment_t	orgfs;
+	USHORT 			WriteEEPROM[(EEPROM_SIZE/2)];
+	UINT32			FileLength = 0;
+	UINT32 			value = simple_strtol(arg, 0, 10);
+
+	ATEDBGPRINT(RT_DEBUG_ERROR, ("===> %s (value=%d)\n\n", __FUNCTION__, value));
+
+	if (value > 0)
+	{
+		/* zero the e2p buffer */
+		NdisZeroMemory((PUCHAR)WriteEEPROM, EEPROM_SIZE);
+
+		/* save uid and gid used for filesystem access.
+	    ** set user and group to 0 (root)
+	    */
+		orgfsuid = current->fsuid;
+		orgfsgid = current->fsgid;
+		/* as root */
+		current->fsuid = current->fsgid = 0;
+    	orgfs = get_fs();
+    	set_fs(KERNEL_DS);
+
+		do
+		{
+			/* open the bin file */
+			srcf = filp_open(src, O_RDONLY, 0);
+
+			if (IS_ERR(srcf))
+			{
+				ate_print("%s - Error %ld opening %s\n", __FUNCTION__, -PTR_ERR(srcf), src);
+				break;
+			}
+
+			/* the object must have a read method */
+			if ((srcf->f_op == NULL) || (srcf->f_op->read == NULL))
+			{
+				ate_print("%s - %s does not have a read method\n", __FUNCTION__, src);
+				break;
+			}
+
+			/* read the firmware from the file *.bin */
+			FileLength = srcf->f_op->read(srcf,
+										  (PUCHAR)WriteEEPROM,
+										  EEPROM_SIZE,
+										  &srcf->f_pos);
+
+			if (FileLength != EEPROM_SIZE)
+			{
+				ate_print("%s: error file length (=%d) in e2p.bin\n",
+					   __FUNCTION__, FileLength);
+				break;
+			}
+			else
+			{
+				/* write the content of .bin file to EEPROM */
+				rt_ee_write_all(pAd, WriteEEPROM);
+				ret = TRUE;
+			}
+			break;
+		} while(TRUE);
+
+		/* close firmware file */
+		if (IS_ERR(srcf))
+		{
+				;
+		}
+		else
+		{
+			retval = filp_close(srcf, NULL);
+			if (retval)
+			{
+				ATEDBGPRINT(RT_DEBUG_ERROR, ("--> Error %d closing %s\n", -retval, src));
+
+			}
+		}
+
+		/* restore */
+		set_fs(orgfs);
+		current->fsuid = orgfsuid;
+		current->fsgid = orgfsgid;
+	}
+    ATEDBGPRINT(RT_DEBUG_ERROR, ("<=== %s (ret=%d)\n", __FUNCTION__, ret));
+
+    return ret;
+
+}
+#else
+INT Set_ATE_Load_E2P_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	USHORT 			WriteEEPROM[(EEPROM_SIZE/2)];
+	struct iwreq	*wrq = (struct iwreq *)arg;
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("===> %s (wrq->u.data.length = %d)\n\n", __FUNCTION__, wrq->u.data.length));
+
+	if (wrq->u.data.length != EEPROM_SIZE)
+	{
+		ate_print("%s: error length (=%d) from host\n",
+			   __FUNCTION__, wrq->u.data.length);
+		return FALSE;
+	}
+	else/* (wrq->u.data.length == EEPROM_SIZE) */
+	{
+		/* zero the e2p buffer */
+		NdisZeroMemory((PUCHAR)WriteEEPROM, EEPROM_SIZE);
+
+		/* fill the local buffer */
+		NdisMoveMemory((PUCHAR)WriteEEPROM, wrq->u.data.pointer, wrq->u.data.length);
+
+		do
+		{
+				/* write the content of .bin file to EEPROM */
+				rt_ee_write_all(pAd, WriteEEPROM);
+
+		} while(FALSE);
+		}
+
+    ATEDBGPRINT(RT_DEBUG_TRACE, ("<=== %s\n", __FUNCTION__));
+
+    return TRUE;
+
+}
+#endif // !UCOS //
+
+INT Set_ATE_Read_E2P_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	USHORT buffer[EEPROM_SIZE/2];
+	USHORT *p;
+	int i;
+
+	rt_ee_read_all(pAd, (USHORT *)buffer);
+	p = buffer;
+	for (i = 0; i < (EEPROM_SIZE/2); i++)
+	{
+		ate_print("%4.4x ", *p);
+		if (((i+1) % 16) == 0)
+			ate_print("\n");
+		p++;
+	}
+	return TRUE;
+}
+
+INT	Set_ATE_Show_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ate_print("Mode=%d\n", pAd->ate.Mode);
+	ate_print("TxPower0=%d\n", pAd->ate.TxPower0);
+	ate_print("TxPower1=%d\n", pAd->ate.TxPower1);
+	ate_print("TxAntennaSel=%d\n", pAd->ate.TxAntennaSel);
+	ate_print("RxAntennaSel=%d\n", pAd->ate.RxAntennaSel);
+	ate_print("BBPCurrentBW=%d\n", pAd->ate.TxWI.BW);
+	ate_print("GI=%d\n", pAd->ate.TxWI.ShortGI);
+	ate_print("MCS=%d\n", pAd->ate.TxWI.MCS);
+	ate_print("TxMode=%d\n", pAd->ate.TxWI.PHYMODE);
+	ate_print("Addr1=%02x:%02x:%02x:%02x:%02x:%02x\n",
+		pAd->ate.Addr1[0], pAd->ate.Addr1[1], pAd->ate.Addr1[2], pAd->ate.Addr1[3], pAd->ate.Addr1[4], pAd->ate.Addr1[5]);
+	ate_print("Addr2=%02x:%02x:%02x:%02x:%02x:%02x\n",
+		pAd->ate.Addr2[0], pAd->ate.Addr2[1], pAd->ate.Addr2[2], pAd->ate.Addr2[3], pAd->ate.Addr2[4], pAd->ate.Addr2[5]);
+	ate_print("Addr3=%02x:%02x:%02x:%02x:%02x:%02x\n",
+		pAd->ate.Addr3[0], pAd->ate.Addr3[1], pAd->ate.Addr3[2], pAd->ate.Addr3[3], pAd->ate.Addr3[4], pAd->ate.Addr3[5]);
+	ate_print("Channel=%d\n", pAd->ate.Channel);
+	ate_print("TxLength=%d\n", pAd->ate.TxLength);
+	ate_print("TxCount=%u\n", pAd->ate.TxCount);
+	ate_print("RFFreqOffset=%d\n", pAd->ate.RFFreqOffset);
+	ate_print(KERN_EMERG "Set_ATE_Show_Proc Success\n");
+	return TRUE;
+}
+
+INT	Set_ATE_Help_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ate_print("ATE=ATESTART, ATESTOP, TXCONT, TXCARR, TXFRAME, RXFRAME\n");
+	ate_print("ATEDA\n");
+	ate_print("ATESA\n");
+	ate_print("ATEBSSID\n");
+	ate_print("ATECHANNEL, range:0~14(unless A band !)\n");
+	ate_print("ATETXPOW0, set power level of antenna 1.\n");
+	ate_print("ATETXPOW1, set power level of antenna 2.\n");
+	ate_print("ATETXANT, set TX antenna. 0:all, 1:antenna one, 2:antenna two.\n");
+	ate_print("ATERXANT, set RX antenna.0:all, 1:antenna one, 2:antenna two, 3:antenna three.\n");
+	ate_print("ATETXFREQOFFSET, set frequency offset, range 0~63\n");
+	ate_print("ATETXBW, set BandWidth, 0:20MHz, 1:40MHz.\n");
+	ate_print("ATETXLEN, set Frame length, range 24~%d\n", (MAX_FRAME_SIZE - 34/* == 2312 */));
+	ate_print("ATETXCNT, set how many frame going to transmit.\n");
+	ate_print("ATETXMCS, set MCS, reference to rate table.\n");
+	ate_print("ATETXMODE, set Mode 0:CCK, 1:OFDM, 2:HT-Mix, 3:GreenField, reference to rate table.\n");
+	ate_print("ATETXGI, set GI interval, 0:Long, 1:Short\n");
+	ate_print("ATERXFER, 0:disable Rx Frame error rate. 1:enable Rx Frame error rate.\n");
+	ate_print("ATERRF, show all RF registers.\n");
+	ate_print("ATEWRF1, set RF1 register.\n");
+	ate_print("ATEWRF2, set RF2 register.\n");
+	ate_print("ATEWRF3, set RF3 register.\n");
+	ate_print("ATEWRF4, set RF4 register.\n");
+	ate_print("ATELDE2P, load EEPROM from .bin file.\n");
+	ate_print("ATERE2P, display all EEPROM content.\n");
+	ate_print("ATESHOW, display all parameters of ATE.\n");
+	ate_print("ATEHELP, online help.\n");
+
+	return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	AsicSwitchChannel() dedicated for ATE.
+
+    ==========================================================================
+*/
+VOID ATEAsicSwitchChannel(
+    IN PRTMP_ADAPTER pAd)
+{
+	UINT32 R2 = 0, R3 = DEFAULT_RF_TX_POWER, R4 = 0, Value = 0;
+	CHAR TxPwer = 0, TxPwer2 = 0;
+	UCHAR index, BbpValue = 0, R66 = 0x30;
+	RTMP_RF_REGS *RFRegTable;
+	UCHAR Channel;
+
+#ifdef RALINK_28xx_QA
+	if ((pAd->ate.bQATxStart == TRUE) || (pAd->ate.bQARxStart == TRUE))
+	{
+		if (pAd->ate.Channel != pAd->LatchRfRegs.Channel)
+		{
+			pAd->ate.Channel = pAd->LatchRfRegs.Channel;
+		}
+		return;
+	}
+	else
+#endif // RALINK_28xx_QA //
+	Channel = pAd->ate.Channel;
+
+	// Select antenna
+	AsicAntennaSelect(pAd, Channel);
+
+	// fill Tx power value
+	TxPwer = pAd->ate.TxPower0;
+	TxPwer2 = pAd->ate.TxPower1;
+
+	RFRegTable = RF2850RegTable;
+
+	switch (pAd->RfIcType)
+	{
+		/* But only 2850 and 2750 support 5.5GHz band... */
+		case RFIC_2820:
+		case RFIC_2850:
+		case RFIC_2720:
+		case RFIC_2750:
+
+			for (index = 0; index < NUM_OF_2850_CHNL; index++)
+			{
+				if (Channel == RFRegTable[index].Channel)
+				{
+					R2 = RFRegTable[index].R2;
+					if (pAd->Antenna.field.TxPath == 1)
+					{
+						R2 |= 0x4000;	// If TXpath is 1, bit 14 = 1;
+					}
+
+					if (pAd->Antenna.field.RxPath == 2)
+					{
+						switch (pAd->ate.RxAntennaSel)
+						{
+							case 1:
+								R2 |= 0x20040;
+								ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BbpValue);
+								BbpValue &= 0xE4;
+								BbpValue |= 0x00;
+								ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BbpValue);
+								break;
+							case 2:
+								R2 |= 0x10040;
+								ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BbpValue);
+								BbpValue &= 0xE4;
+								BbpValue |= 0x01;
+								ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BbpValue);
+								break;
+							default:
+								R2 |= 0x40;
+								ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BbpValue);
+								BbpValue &= 0xE4;
+								/* Only enable two Antenna to receive. */
+								BbpValue |= 0x08;
+								ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BbpValue);
+								break;
+						}
+					}
+					else if (pAd->Antenna.field.RxPath == 1)
+					{
+						R2 |= 0x20040;	// write 1 to off RxPath
+					}
+
+					if (pAd->Antenna.field.TxPath == 2)
+					{
+						if (pAd->ate.TxAntennaSel == 1)
+						{
+							R2 |= 0x4000;	// If TX Antenna select is 1 , bit 14 = 1; Disable Ant 2
+							ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R1, &BbpValue);
+							BbpValue &= 0xE7;		//11100111B
+							ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R1, BbpValue);
+						}
+						else if (pAd->ate.TxAntennaSel == 2)
+						{
+							R2 |= 0x8000;	// If TX Antenna select is 2 , bit 15 = 1; Disable Ant 1
+							ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R1, &BbpValue);
+							BbpValue &= 0xE7;
+							BbpValue |= 0x08;
+							ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R1, BbpValue);
+						}
+						else
+						{
+							ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R1, &BbpValue);
+							BbpValue &= 0xE7;
+							BbpValue |= 0x10;
+							ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R1, BbpValue);
+						}
+					}
+					if (pAd->Antenna.field.RxPath == 3)
+					{
+						switch (pAd->ate.RxAntennaSel)
+						{
+							case 1:
+								R2 |= 0x20040;
+								ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BbpValue);
+								BbpValue &= 0xE4;
+								BbpValue |= 0x00;
+								ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BbpValue);
+								break;
+							case 2:
+								R2 |= 0x10040;
+								ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BbpValue);
+								BbpValue &= 0xE4;
+								BbpValue |= 0x01;
+								ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BbpValue);
+								break;
+							case 3:
+								R2 |= 0x30000;
+								ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BbpValue);
+								BbpValue &= 0xE4;
+								BbpValue |= 0x02;
+								ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BbpValue);
+								break;
+							default:
+								ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &BbpValue);
+								BbpValue &= 0xE4;
+								BbpValue |= 0x10;
+								ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, BbpValue);
+								break;
+						}
+					}
+
+					if (Channel > 14)
+					{
+						// initialize R3, R4
+						R3 = (RFRegTable[index].R3 & 0xffffc1ff);
+						R4 = (RFRegTable[index].R4 & (~0x001f87c0)) | (pAd->ate.RFFreqOffset << 15);
+
+                        // According the Rory's suggestion to solve the middle range issue.
+						// 5.5G band power range: 0xF9~0X0F, TX0 Reg3 bit9/TX1 Reg4 bit6="0" means the TX power reduce 7dB
+						// R3
+						if ((TxPwer >= -7) && (TxPwer < 0))
+						{
+							TxPwer = (7+TxPwer);
+							TxPwer = (TxPwer > 0xF) ? (0xF) : (TxPwer);
+							R3 |= (TxPwer << 10);
+							ATEDBGPRINT(RT_DEBUG_TRACE, ("ATEAsicSwitchChannel: TxPwer=%d \n", TxPwer));
+						}
+						else
+						{
+							TxPwer = (TxPwer > 0xF) ? (0xF) : (TxPwer);
+							R3 |= (TxPwer << 10) | (1 << 9);
+						}
+
+						// R4
+						if ((TxPwer2 >= -7) && (TxPwer2 < 0))
+						{
+							TxPwer2 = (7+TxPwer2);
+							TxPwer2 = (TxPwer2 > 0xF) ? (0xF) : (TxPwer2);
+							R4 |= (TxPwer2 << 7);
+							ATEDBGPRINT(RT_DEBUG_TRACE, ("ATEAsicSwitchChannel: TxPwer2=%d \n", TxPwer2));
+						}
+						else
+						{
+							TxPwer2 = (TxPwer2 > 0xF) ? (0xF) : (TxPwer2);
+							R4 |= (TxPwer2 << 7) | (1 << 6);
+						}
+					}
+					else
+					{
+						R3 = (RFRegTable[index].R3 & 0xffffc1ff) | (TxPwer << 9); // set TX power0
+						R4 = (RFRegTable[index].R4 & (~0x001f87c0)) | (pAd->ate.RFFreqOffset << 15) | (TxPwer2 <<6);// Set freq offset & TxPwr1
+					}
+
+					// Based on BBP current mode before changing RF channel.
+					if (pAd->ate.TxWI.BW == BW_40)
+					{
+						R4 |=0x200000;
+					}
+
+					// Update variables
+					pAd->LatchRfRegs.Channel = Channel;
+					pAd->LatchRfRegs.R1 = RFRegTable[index].R1;
+					pAd->LatchRfRegs.R2 = R2;
+					pAd->LatchRfRegs.R3 = R3;
+					pAd->LatchRfRegs.R4 = R4;
+
+					RtmpRfIoWrite(pAd);
+
+					break;
+				}
+			}
+			break;
+
+		default:
+			break;
+	}
+
+	// Change BBP setting during switch from a->g, g->a
+	if (Channel <= 14)
+	{
+	    ULONG	TxPinCfg = 0x00050F0A;// 2007.10.09 by Brian : 0x0005050A ==> 0x00050F0A
+
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R62, (0x37 - GET_LNA_GAIN(pAd)));
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R63, (0x37 - GET_LNA_GAIN(pAd)));
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R64, (0x37 - GET_LNA_GAIN(pAd)));
+
+		/* For 1T/2R chip only... */
+	    if (pAd->NicConfig2.field.ExternalLNAForG)
+	    {
+	        ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R82, 0x62);
+	    }
+	    else
+	    {
+	        ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R82, 0x84);
+	    }
+
+        // According the Rory's suggestion to solve the middle range issue.
+		ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R86, &BbpValue);
+		ASSERT((BbpValue == 0x00));
+		if ((BbpValue != 0x00))
+		{
+			ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R86, 0x00);
+		}
+
+		// 5.5GHz band selection PIN, bit1 and bit2 are complement
+		RTMP_IO_READ32(pAd, TX_BAND_CFG, &Value);
+		Value &= (~0x6);
+		Value |= (0x04);
+		RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Value);
+
+        // Turn off unused PA or LNA when only 1T or 1R.
+		if (pAd->Antenna.field.TxPath == 1)
+		{
+			TxPinCfg &= 0xFFFFFFF3;
+		}
+		if (pAd->Antenna.field.RxPath == 1)
+		{
+			TxPinCfg &= 0xFFFFF3FF;
+		}
+
+		RTMP_IO_WRITE32(pAd, TX_PIN_CFG, TxPinCfg);
+	}
+	else
+	{
+	    ULONG	TxPinCfg = 0x00050F05;//2007.10.09 by Brian : 0x00050505 ==> 0x00050F05
+
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R62, (0x37 - GET_LNA_GAIN(pAd)));
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R63, (0x37 - GET_LNA_GAIN(pAd)));
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R64, (0x37 - GET_LNA_GAIN(pAd)));
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R82, 0xF2);
+
+        // According the Rory's suggestion to solve the middle range issue.
+		ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R86, &BbpValue);
+		ASSERT((BbpValue == 0x00));
+		if ((BbpValue != 0x00))
+		{
+			ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R86, 0x00);
+		}
+		ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R91, &BbpValue);
+		ASSERT((BbpValue == 0x04));
+
+		ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R92, &BbpValue);
+		ASSERT((BbpValue == 0x00));
+
+		// 5.5GHz band selection PIN, bit1 and bit2 are complement
+		RTMP_IO_READ32(pAd, TX_BAND_CFG, &Value);
+		Value &= (~0x6);
+		Value |= (0x02);
+		RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Value);
+
+        // Turn off unused PA or LNA when only 1T or 1R.
+		if (pAd->Antenna.field.TxPath == 1)
+		{
+			TxPinCfg &= 0xFFFFFFF3;
+	    }
+		if (pAd->Antenna.field.RxPath == 1)
+		{
+			TxPinCfg &= 0xFFFFF3FF;
+		}
+
+		RTMP_IO_WRITE32(pAd, TX_PIN_CFG, TxPinCfg);
+	}
+
+    // R66 should be set according to Channel and use 20MHz when scanning
+	if (Channel <= 14)
+	{
+		// BG band
+		R66 = 0x2E + GET_LNA_GAIN(pAd);
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+	}
+	else
+	{
+		// 5.5 GHz band
+		if (pAd->ate.TxWI.BW == BW_20)
+		{
+			R66 = (UCHAR)(0x32 + (GET_LNA_GAIN(pAd)*5)/3);
+    		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+		}
+		else
+		{
+			R66 = (UCHAR)(0x3A + (GET_LNA_GAIN(pAd)*5)/3);
+			ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, R66);
+		}
+	}
+
+	//
+	// On 11A, We should delay and wait RF/BBP to be stable
+	// and the appropriate time should be 1000 micro seconds
+	// 2005/06/05 - On 11G, We also need this delay time. Otherwise it's difficult to pass the WHQL.
+	//
+	RTMPusecDelay(1000);
+
+	if (Channel > 14)
+	{
+		// When 5.5GHz band the LSB of TxPwr will be used to reduced 7dB or not.
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("SwitchChannel#%d(RF=%d, %dT) to , R1=0x%08lx, R2=0x%08lx, R3=0x%08lx, R4=0x%08lx\n",
+								  Channel,
+								  pAd->RfIcType,
+								  pAd->Antenna.field.TxPath,
+								  pAd->LatchRfRegs.R1,
+								  pAd->LatchRfRegs.R2,
+								  pAd->LatchRfRegs.R3,
+								  pAd->LatchRfRegs.R4));
+	}
+	else
+	{
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("SwitchChannel#%d(RF=%d, Pwr0=%u, Pwr1=%u, %dT) to , R1=0x%08lx, R2=0x%08lx, R3=0x%08lx, R4=0x%08lx\n",
+								  Channel,
+								  pAd->RfIcType,
+								  (R3 & 0x00003e00) >> 9,
+								  (R4 & 0x000007c0) >> 6,
+								  pAd->Antenna.field.TxPath,
+								  pAd->LatchRfRegs.R1,
+								  pAd->LatchRfRegs.R2,
+								  pAd->LatchRfRegs.R3,
+								  pAd->LatchRfRegs.R4));
+    }
+}
+
+//
+// In fact, no one will call this routine so far !
+//
+/*
+	==========================================================================
+	Description:
+		Gives CCK TX rate 2 more dB TX power.
+		This routine works only in ATE mode.
+
+		calculate desired Tx power in RF R3.Tx0~5,	should consider -
+		0. if current radio is a noisy environment (pAd->DrsCounters.fNoisyEnvironment)
+		1. TxPowerPercentage
+		2. auto calibration based on TSSI feedback
+		3. extra 2 db for CCK
+		4. -10 db upon very-short distance (AvgRSSI >= -40db) to AP
+
+	NOTE: Since this routine requires the value of (pAd->DrsCounters.fNoisyEnvironment),
+		it should be called AFTER MlmeDynamicTxRateSwitching()
+	==========================================================================
+ */
+VOID ATEAsicAdjustTxPower(
+	IN PRTMP_ADAPTER pAd)
+{
+	INT			i, j;
+	CHAR		DeltaPwr = 0;
+	BOOLEAN		bAutoTxAgc = FALSE;
+	UCHAR		TssiRef, *pTssiMinusBoundary, *pTssiPlusBoundary, TxAgcStep;
+	UCHAR		BbpR49 = 0, idx;
+	PCHAR		pTxAgcCompensate;
+	ULONG		TxPwr[5];
+	CHAR		Value;
+
+	/* no one calls this procedure so far */
+	if (pAd->ate.TxWI.BW == BW_40)
+	{
+		if (pAd->ate.Channel > 14)
+		{
+			TxPwr[0] = pAd->Tx40MPwrCfgABand[0];
+			TxPwr[1] = pAd->Tx40MPwrCfgABand[1];
+			TxPwr[2] = pAd->Tx40MPwrCfgABand[2];
+			TxPwr[3] = pAd->Tx40MPwrCfgABand[3];
+			TxPwr[4] = pAd->Tx40MPwrCfgABand[4];
+		}
+		else
+		{
+			TxPwr[0] = pAd->Tx40MPwrCfgGBand[0];
+			TxPwr[1] = pAd->Tx40MPwrCfgGBand[1];
+			TxPwr[2] = pAd->Tx40MPwrCfgGBand[2];
+			TxPwr[3] = pAd->Tx40MPwrCfgGBand[3];
+			TxPwr[4] = pAd->Tx40MPwrCfgGBand[4];
+		}
+	}
+	else
+	{
+		if (pAd->ate.Channel > 14)
+		{
+			TxPwr[0] = pAd->Tx20MPwrCfgABand[0];
+			TxPwr[1] = pAd->Tx20MPwrCfgABand[1];
+			TxPwr[2] = pAd->Tx20MPwrCfgABand[2];
+			TxPwr[3] = pAd->Tx20MPwrCfgABand[3];
+			TxPwr[4] = pAd->Tx20MPwrCfgABand[4];
+		}
+		else
+		{
+			TxPwr[0] = pAd->Tx20MPwrCfgGBand[0];
+			TxPwr[1] = pAd->Tx20MPwrCfgGBand[1];
+			TxPwr[2] = pAd->Tx20MPwrCfgGBand[2];
+			TxPwr[3] = pAd->Tx20MPwrCfgGBand[3];
+			TxPwr[4] = pAd->Tx20MPwrCfgGBand[4];
+		}
+	}
+
+	// TX power compensation for temperature variation based on TSSI.
+	// Do it per 4 seconds.
+	if (pAd->Mlme.OneSecPeriodicRound % 4 == 0)
+	{
+		if (pAd->ate.Channel <= 14)
+		{
+			/* bg channel */
+			bAutoTxAgc         = pAd->bAutoTxAgcG;
+			TssiRef            = pAd->TssiRefG;
+			pTssiMinusBoundary = &pAd->TssiMinusBoundaryG[0];
+			pTssiPlusBoundary  = &pAd->TssiPlusBoundaryG[0];
+			TxAgcStep          = pAd->TxAgcStepG;
+			pTxAgcCompensate   = &pAd->TxAgcCompensateG;
+		}
+		else
+		{
+			/* a channel */
+			bAutoTxAgc         = pAd->bAutoTxAgcA;
+			TssiRef            = pAd->TssiRefA;
+			pTssiMinusBoundary = &pAd->TssiMinusBoundaryA[0];
+			pTssiPlusBoundary  = &pAd->TssiPlusBoundaryA[0];
+			TxAgcStep          = pAd->TxAgcStepA;
+			pTxAgcCompensate   = &pAd->TxAgcCompensateA;
+		}
+
+		if (bAutoTxAgc)
+		{
+			/* BbpR49 is unsigned char */
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R49, &BbpR49);
+
+			/* (p) TssiPlusBoundaryG[0] = 0 = (m) TssiMinusBoundaryG[0] */
+			/* compensate: +4     +3   +2   +1    0   -1   -2   -3   -4 * steps */
+			/* step value is defined in pAd->TxAgcStepG for tx power value */
+
+			/* [4]+1+[4]   p4     p3   p2   p1   o1   m1   m2   m3   m4 */
+			/* ex:         0x00 0x15 0x25 0x45 0x88 0xA0 0xB5 0xD0 0xF0
+			   above value are examined in mass factory production */
+			/*             [4]    [3]  [2]  [1]  [0]  [1]  [2]  [3]  [4] */
+
+			/* plus is 0x10 ~ 0x40, minus is 0x60 ~ 0x90 */
+			/* if value is between p1 ~ o1 or o1 ~ s1, no need to adjust tx power */
+			/* if value is 0x65, tx power will be -= TxAgcStep*(2-1) */
+
+			if (BbpR49 > pTssiMinusBoundary[1])
+			{
+				// Reading is larger than the reference value.
+				// Check for how large we need to decrease the Tx power.
+				for (idx = 1; idx < 5; idx++)
+				{
+					if (BbpR49 <= pTssiMinusBoundary[idx])  // Found the range
+						break;
+				}
+				// The index is the step we should decrease, idx = 0 means there is nothing to compensate
+//				if (R3 > (ULONG) (TxAgcStep * (idx-1)))
+					*pTxAgcCompensate = -(TxAgcStep * (idx-1));
+//				else
+//					*pTxAgcCompensate = -((UCHAR)R3);
+
+				DeltaPwr += (*pTxAgcCompensate);
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("-- Tx Power, BBP R1=%x, TssiRef=%x, TxAgcStep=%x, step = -%d\n",
+					BbpR49, TssiRef, TxAgcStep, idx-1));
+			}
+			else if (BbpR49 < pTssiPlusBoundary[1])
+			{
+				// Reading is smaller than the reference value
+				// check for how large we need to increase the Tx power
+				for (idx = 1; idx < 5; idx++)
+				{
+					if (BbpR49 >= pTssiPlusBoundary[idx])   // Found the range
+						break;
+				}
+				// The index is the step we should increase, idx = 0 means there is nothing to compensate
+				*pTxAgcCompensate = TxAgcStep * (idx-1);
+				DeltaPwr += (*pTxAgcCompensate);
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("++ Tx Power, BBP R1=%x, TssiRef=%x, TxAgcStep=%x, step = +%d\n",
+					BbpR49, TssiRef, TxAgcStep, idx-1));
+			}
+			else
+			{
+				*pTxAgcCompensate = 0;
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("   Tx Power, BBP R1=%x, TssiRef=%x, TxAgcStep=%x, step = +%d\n",
+					BbpR49, TssiRef, TxAgcStep, 0));
+			}
+		}
+	}
+	else
+	{
+		if (pAd->ate.Channel <= 14)
+		{
+			bAutoTxAgc         = pAd->bAutoTxAgcG;
+			pTxAgcCompensate   = &pAd->TxAgcCompensateG;
+		}
+		else
+		{
+			bAutoTxAgc         = pAd->bAutoTxAgcA;
+			pTxAgcCompensate   = &pAd->TxAgcCompensateA;
+		}
+
+		if (bAutoTxAgc)
+			DeltaPwr += (*pTxAgcCompensate);
+	}
+
+	/* calculate delta power based on the percentage specified from UI */
+	// E2PROM setting is calibrated for maximum TX power (i.e. 100%)
+	// We lower TX power here according to the percentage specified from UI
+	if (pAd->CommonCfg.TxPowerPercentage == 0xffffffff)       // AUTO TX POWER control
+		;
+	else if (pAd->CommonCfg.TxPowerPercentage > 90)  // 91 ~ 100% & AUTO, treat as 100% in terms of mW
+		;
+	else if (pAd->CommonCfg.TxPowerPercentage > 60)  // 61 ~ 90%, treat as 75% in terms of mW
+	{
+		DeltaPwr -= 1;
+	}
+	else if (pAd->CommonCfg.TxPowerPercentage > 30)  // 31 ~ 60%, treat as 50% in terms of mW
+	{
+		DeltaPwr -= 3;
+	}
+	else if (pAd->CommonCfg.TxPowerPercentage > 15)  // 16 ~ 30%, treat as 25% in terms of mW
+	{
+		DeltaPwr -= 6;
+	}
+	else if (pAd->CommonCfg.TxPowerPercentage > 9)   // 10 ~ 15%, treat as 12.5% in terms of mW
+	{
+		DeltaPwr -= 9;
+	}
+	else                                           // 0 ~ 9 %, treat as MIN(~3%) in terms of mW
+	{
+		DeltaPwr -= 12;
+	}
+
+	/* reset different new tx power for different TX rate */
+	for(i=0; i<5; i++)
+	{
+		if (TxPwr[i] != 0xffffffff)
+		{
+			for (j=0; j<8; j++)
+			{
+				Value = (CHAR)((TxPwr[i] >> j*4) & 0x0F); /* 0 ~ 15 */
+
+				if ((Value + DeltaPwr) < 0)
+				{
+					Value = 0; /* min */
+				}
+				else if ((Value + DeltaPwr) > 0xF)
+				{
+					Value = 0xF; /* max */
+				}
+				else
+				{
+					Value += DeltaPwr; /* temperature compensation */
+				}
+
+				/* fill new value to CSR offset */
+				TxPwr[i] = (TxPwr[i] & ~(0x0000000F << j*4)) | (Value << j*4);
+			}
+
+			/* write tx power value to CSR */
+			/* TX_PWR_CFG_0 (8 tx rate) for	TX power for OFDM 12M/18M
+											TX power for OFDM 6M/9M
+											TX power for CCK5.5M/11M
+											TX power for CCK1M/2M */
+			/* TX_PWR_CFG_1 ~ TX_PWR_CFG_4 */
+			RTMP_IO_WRITE32(pAd, TX_PWR_CFG_0 + i*4, TxPwr[i]);
+
+
+		}
+	}
+
+}
+
+/*
+	========================================================================
+	Routine Description:
+		Write TxWI for ATE mode.
+
+	Return Value:
+		None
+	========================================================================
+*/
+
+#ifdef RT2870
+static VOID ATEWriteTxWI(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTXWI_STRUC 	pTxWI,
+	IN	BOOLEAN			FRAG,
+	IN	BOOLEAN			InsTimestamp,
+	IN	BOOLEAN 		AMPDU,
+	IN	BOOLEAN 		Ack,
+	IN	BOOLEAN 		NSeq,		// HW new a sequence.
+	IN	UCHAR			BASize,
+	IN	UCHAR			WCID,
+	IN	ULONG			Length,
+	IN	UCHAR 			PID,
+	IN	UCHAR			MIMOps,
+	IN	UCHAR			Txopmode,
+	IN	BOOLEAN			CfAck,
+	IN	HTTRANSMIT_SETTING	Transmit)
+{
+	//
+	// Always use Long preamble before verifiation short preamble functionality works well.
+	// Todo: remove the following line if short preamble functionality works
+	//
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED);
+	pTxWI->FRAG= FRAG;
+	pTxWI->TS= InsTimestamp;
+	pTxWI->AMPDU = AMPDU;
+
+	pTxWI->MIMOps = PWR_ACTIVE;
+	pTxWI->MpduDensity = 4;
+	pTxWI->ACK = Ack;
+	pTxWI->txop = Txopmode;
+	pTxWI->NSEQ = NSeq;
+	pTxWI->BAWinSize = BASize;
+
+	pTxWI->WirelessCliID = WCID;
+	pTxWI->MPDUtotalByteCount = Length;
+	pTxWI->PacketId = PID;
+
+	pTxWI->BW = Transmit.field.BW;
+	pTxWI->ShortGI = Transmit.field.ShortGI;
+	pTxWI->STBC= Transmit.field.STBC;
+
+	pTxWI->MCS = Transmit.field.MCS;
+	pTxWI->PHYMODE= Transmit.field.MODE;
+
+#ifdef DOT11_N_SUPPORT
+	//
+	// MMPS is 802.11n features. Because TxWI->MCS > 7 must be HT mode,
+	// so need not check if it's HT rate.
+	//
+	if ((MIMOps == MMPS_STATIC) && (pTxWI->MCS > 7))
+		pTxWI->MCS = 7;
+
+	if ((MIMOps == MMPS_DYNAMIC) && (pTxWI->MCS > 7)) // SMPS protect 2 spatial.
+		pTxWI->MIMOps = 1;
+#endif // DOT11_N_SUPPORT //
+
+	pTxWI->CFACK = CfAck;
+
+	return;
+}
+#endif // RT2870 //
+/*
+	========================================================================
+
+	Routine Description:
+		Disable protection for ATE.
+	========================================================================
+*/
+VOID ATEDisableAsicProtect(
+	IN		PRTMP_ADAPTER	pAd)
+{
+	PROT_CFG_STRUC	ProtCfg, ProtCfg4;
+	UINT32 Protect[6];
+	USHORT			offset;
+	UCHAR			i;
+	UINT32 MacReg = 0;
+
+	// Config ASIC RTS threshold register
+	RTMP_IO_READ32(pAd, TX_RTS_CFG, &MacReg);
+	MacReg &= 0xFF0000FF;
+	MacReg |= (pAd->CommonCfg.RtsThreshold << 8);
+	RTMP_IO_WRITE32(pAd, TX_RTS_CFG, MacReg);
+
+	// Initial common protection settings
+	RTMPZeroMemory(Protect, sizeof(Protect));
+	ProtCfg4.word = 0;
+	ProtCfg.word = 0;
+	ProtCfg.field.TxopAllowGF40 = 1;
+	ProtCfg.field.TxopAllowGF20 = 1;
+	ProtCfg.field.TxopAllowMM40 = 1;
+	ProtCfg.field.TxopAllowMM20 = 1;
+	ProtCfg.field.TxopAllowOfdm = 1;
+	ProtCfg.field.TxopAllowCck = 1;
+	ProtCfg.field.RTSThEn = 1;
+	ProtCfg.field.ProtectNav = ASIC_SHORTNAV;
+
+	// Handle legacy(B/G) protection
+	ProtCfg.field.ProtectRate = pAd->CommonCfg.RtsRate;
+	ProtCfg.field.ProtectCtrl = 0;
+	Protect[0] = ProtCfg.word;
+	Protect[1] = ProtCfg.word;
+
+	// NO PROTECT
+	// 1.All STAs in the BSS are 20/40 MHz HT
+	// 2. in ai 20/40MHz BSS
+	// 3. all STAs are 20MHz in a 20MHz BSS
+	// Pure HT. no protection.
+
+	// MM20_PROT_CFG
+	//	Reserved (31:27)
+	// 	PROT_TXOP(25:20) -- 010111
+	//	PROT_NAV(19:18)  -- 01 (Short NAV protection)
+	//  PROT_CTRL(17:16) -- 00 (None)
+	// 	PROT_RATE(15:0)  -- 0x4004 (OFDM 24M)
+	Protect[2] = 0x01744004;
+
+	// MM40_PROT_CFG
+	//	Reserved (31:27)
+	// 	PROT_TXOP(25:20) -- 111111
+	//	PROT_NAV(19:18)  -- 01 (Short NAV protection)
+	//  PROT_CTRL(17:16) -- 00 (None)
+	// 	PROT_RATE(15:0)  -- 0x4084 (duplicate OFDM 24M)
+	Protect[3] = 0x03f44084;
+
+	// CF20_PROT_CFG
+	//	Reserved (31:27)
+	// 	PROT_TXOP(25:20) -- 010111
+	//	PROT_NAV(19:18)  -- 01 (Short NAV protection)
+	//  PROT_CTRL(17:16) -- 00 (None)
+	// 	PROT_RATE(15:0)  -- 0x4004 (OFDM 24M)
+	Protect[4] = 0x01744004;
+
+	// CF40_PROT_CFG
+	//	Reserved (31:27)
+	// 	PROT_TXOP(25:20) -- 111111
+	//	PROT_NAV(19:18)  -- 01 (Short NAV protection)
+	//  PROT_CTRL(17:16) -- 00 (None)
+	// 	PROT_RATE(15:0)  -- 0x4084 (duplicate OFDM 24M)
+	Protect[5] = 0x03f44084;
+
+	pAd->CommonCfg.IOTestParm.bRTSLongProtOn = FALSE;
+
+	offset = CCK_PROT_CFG;
+	for (i = 0;i < 6;i++)
+		RTMP_IO_WRITE32(pAd, offset + i*4, Protect[i]);
+
+}
+
+#ifdef RT2870
+/*
+	========================================================================
+	Routine	Description:
+		Write TxInfo for ATE mode.
+
+	Return Value:
+		None
+	========================================================================
+*/
+static VOID ATEWriteTxInfo(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTXINFO_STRUC 	pTxInfo,
+	IN	USHORT		USBDMApktLen,
+	IN	BOOLEAN		bWiv,
+	IN	UCHAR			QueueSel,
+	IN	UCHAR			NextValid,
+	IN	UCHAR			TxBurst)
+{
+	pTxInfo->USBDMATxPktLen = USBDMApktLen;
+	pTxInfo->QSEL = QueueSel;
+
+	if (QueueSel != FIFO_EDCA)
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("=======> QueueSel != FIFO_EDCA<=======\n"));
+
+	pTxInfo->USBDMANextVLD = NextValid;
+	pTxInfo->USBDMATxburst = TxBurst;
+	pTxInfo->WIV = bWiv;
+	pTxInfo->SwUseLastRound = 0;
+	pTxInfo->rsv = 0;
+	pTxInfo->rsv2 = 0;
+
+	return;
+}
+#endif // RT2870 //
+
+/* There are two ways to convert Rssi */
+#if 1
+//
+// The way used with GET_LNA_GAIN().
+//
+CHAR ATEConvertToRssi(
+	IN PRTMP_ADAPTER pAd,
+	IN	CHAR	Rssi,
+	IN  UCHAR   RssiNumber)
+{
+	UCHAR	RssiOffset, LNAGain;
+
+	// Rssi equals to zero should be an invalid value
+	if (Rssi == 0)
+		return -99;
+
+	LNAGain = GET_LNA_GAIN(pAd);
+	if (pAd->LatchRfRegs.Channel > 14)
+	{
+		if (RssiNumber == 0)
+			RssiOffset = pAd->ARssiOffset0;
+		else if (RssiNumber == 1)
+			RssiOffset = pAd->ARssiOffset1;
+		else
+			RssiOffset = pAd->ARssiOffset2;
+	}
+	else
+	{
+		if (RssiNumber == 0)
+			RssiOffset = pAd->BGRssiOffset0;
+		else if (RssiNumber == 1)
+			RssiOffset = pAd->BGRssiOffset1;
+		else
+			RssiOffset = pAd->BGRssiOffset2;
+	}
+
+	return (-12 - RssiOffset - LNAGain - Rssi);
+}
+#else
+//
+// The way originally used in ATE of rt2860ap.
+//
+CHAR ATEConvertToRssi(
+	IN PRTMP_ADAPTER pAd,
+	IN	CHAR			Rssi,
+	IN  UCHAR   RssiNumber)
+{
+	UCHAR	RssiOffset, LNAGain;
+
+	// Rssi equals to zero should be an invalid value
+	if (Rssi == 0)
+		return -99;
+
+    if (pAd->LatchRfRegs.Channel > 14)
+    {
+        LNAGain = pAd->ALNAGain;
+        if (RssiNumber == 0)
+			RssiOffset = pAd->ARssiOffset0;
+		else if (RssiNumber == 1)
+			RssiOffset = pAd->ARssiOffset1;
+		else
+			RssiOffset = pAd->ARssiOffset2;
+    }
+    else
+    {
+        LNAGain = pAd->BLNAGain;
+        if (RssiNumber == 0)
+			RssiOffset = pAd->BGRssiOffset0;
+		else if (RssiNumber == 1)
+			RssiOffset = pAd->BGRssiOffset1;
+		else
+			RssiOffset = pAd->BGRssiOffset2;
+    }
+
+    return (-32 - RssiOffset + LNAGain - Rssi);
+}
+#endif /* end of #if 1 */
+
+/*
+	========================================================================
+
+	Routine Description:
+		Set Japan filter coefficients if needed.
+	Note:
+		This routine should only be called when
+		entering TXFRAME mode or TXCONT mode.
+
+	========================================================================
+*/
+static VOID SetJapanFilter(
+	IN		PRTMP_ADAPTER	pAd)
+{
+	UCHAR			BbpData = 0;
+
+	//
+	// If Channel=14 and Bandwidth=20M and Mode=CCK, set BBP R4 bit5=1
+	// (Japan Tx filter coefficients)when (TXFRAME or TXCONT).
+	//
+	ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BbpData);
+
+    if ((pAd->ate.TxWI.PHYMODE == MODE_CCK) && (pAd->ate.Channel == 14) && (pAd->ate.TxWI.BW == BW_20))
+    {
+        BbpData |= 0x20;    // turn on
+        ATEDBGPRINT(RT_DEBUG_TRACE, ("SetJapanFilter!!!\n"));
+    }
+    else
+    {
+		BbpData &= 0xdf;    // turn off
+		ATEDBGPRINT(RT_DEBUG_TRACE, ("ClearJapanFilter!!!\n"));
+    }
+
+	ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BbpData);
+}
+
+VOID ATESampleRssi(
+	IN PRTMP_ADAPTER	pAd,
+	IN PRXWI_STRUC		pRxWI)
+{
+	/* There are two ways to collect RSSI. */
+#if 1
+	//pAd->LastRxRate = (USHORT)((pRxWI->MCS) + (pRxWI->BW <<7) + (pRxWI->ShortGI <<8)+ (pRxWI->PHYMODE <<14)) ;
+	if (pRxWI->RSSI0 != 0)
+	{
+		pAd->ate.LastRssi0	= ATEConvertToRssi(pAd, (CHAR) pRxWI->RSSI0, RSSI_0);
+		pAd->ate.AvgRssi0X8	= (pAd->ate.AvgRssi0X8 - pAd->ate.AvgRssi0) + pAd->ate.LastRssi0;
+		pAd->ate.AvgRssi0  	= pAd->ate.AvgRssi0X8 >> 3;
+	}
+	if (pRxWI->RSSI1 != 0)
+	{
+		pAd->ate.LastRssi1	= ATEConvertToRssi(pAd, (CHAR) pRxWI->RSSI1, RSSI_1);
+		pAd->ate.AvgRssi1X8	= (pAd->ate.AvgRssi1X8 - pAd->ate.AvgRssi1) + pAd->ate.LastRssi1;
+		pAd->ate.AvgRssi1	= pAd->ate.AvgRssi1X8 >> 3;
+	}
+	if (pRxWI->RSSI2 != 0)
+	{
+		pAd->ate.LastRssi2	= ATEConvertToRssi(pAd, (CHAR) pRxWI->RSSI2, RSSI_2);
+		pAd->ate.AvgRssi2X8	= (pAd->ate.AvgRssi2X8 - pAd->ate.AvgRssi2) + pAd->ate.LastRssi2;
+		pAd->ate.AvgRssi2	= pAd->ate.AvgRssi2X8 >> 3;
+	}
+
+	pAd->ate.LastSNR0 = (CHAR)(pRxWI->SNR0);// CHAR ==> UCHAR ?
+	pAd->ate.LastSNR1 = (CHAR)(pRxWI->SNR1);// CHAR ==> UCHAR ?
+
+	pAd->ate.NumOfAvgRssiSample ++;
+#else
+	pAd->ate.LastSNR0 = (CHAR)(pRxWI->SNR0);
+	pAd->ate.LastSNR1 = (CHAR)(pRxWI->SNR1);
+	pAd->ate.RxCntPerSec++;
+	pAd->ate.LastRssi0 = ATEConvertToRssi(pAd, (CHAR) pRxWI->RSSI0, RSSI_0);
+	pAd->ate.LastRssi1 = ATEConvertToRssi(pAd, (CHAR) pRxWI->RSSI1, RSSI_1);
+	pAd->ate.LastRssi2 = ATEConvertToRssi(pAd, (CHAR) pRxWI->RSSI2, RSSI_2);
+	pAd->ate.AvgRssi0X8 = (pAd->ate.AvgRssi0X8 - pAd->ate.AvgRssi0) + pAd->ate.LastRssi0;
+	pAd->ate.AvgRssi0 = pAd->ate.AvgRssi0X8 >> 3;
+	pAd->ate.AvgRssi1X8 = (pAd->ate.AvgRssi1X8 - pAd->ate.AvgRssi1) + pAd->ate.LastRssi1;
+	pAd->ate.AvgRssi1 = pAd->ate.AvgRssi1X8 >> 3;
+	pAd->ate.AvgRssi2X8 = (pAd->ate.AvgRssi2X8 - pAd->ate.AvgRssi2) + pAd->ate.LastRssi2;
+	pAd->ate.AvgRssi2 = pAd->ate.AvgRssi2X8 >> 3;
+	pAd->ate.NumOfAvgRssiSample ++;
+#endif
+}
+
+#ifdef CONFIG_STA_SUPPORT
+VOID RTMPStationStop(
+    IN  PRTMP_ADAPTER   pAd)
+{
+//	BOOLEAN       Cancelled;
+
+    ATEDBGPRINT(RT_DEBUG_TRACE, ("==> RTMPStationStop\n"));
+
+#if 0
+	RTMPCancelTimer(&pAd->MlmeAux.AssocTimer,      &Cancelled);
+	RTMPCancelTimer(&pAd->MlmeAux.ReassocTimer,    &Cancelled);
+	RTMPCancelTimer(&pAd->MlmeAux.DisassocTimer,   &Cancelled);
+	RTMPCancelTimer(&pAd->MlmeAux.AuthTimer,       &Cancelled);
+	RTMPCancelTimer(&pAd->MlmeAux.BeaconTimer,     &Cancelled);
+	RTMPCancelTimer(&pAd->MlmeAux.ScanTimer,       &Cancelled);
+#endif
+	// For rx statistics, we need to keep this timer running.
+//	RTMPCancelTimer(&pAd->Mlme.PeriodicTimer,      &Cancelled);
+
+    ATEDBGPRINT(RT_DEBUG_TRACE, ("<== RTMPStationStop\n"));
+}
+
+VOID RTMPStationStart(
+    IN  PRTMP_ADAPTER   pAd)
+{
+    ATEDBGPRINT(RT_DEBUG_TRACE, ("==> RTMPStationStart\n"));
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("<== RTMPStationStart\n"));
+}
+#endif // CONFIG_STA_SUPPORT //
+
+/*
+	==========================================================================
+	Description:
+		Setup Frame format.
+	NOTE:
+		This routine should only be used in ATE mode.
+	==========================================================================
+ */
+
+#ifdef RT2870
+/*======================Start of RT2870======================*/
+/*                                                           */
+/*                                                           */
+static INT ATESetUpFrame(
+	IN PRTMP_ADAPTER pAd,
+	IN UINT32 TxIdx)
+{
+	UINT j;
+	PTX_CONTEXT	pNullContext;
+	PUCHAR			pDest;
+	HTTRANSMIT_SETTING	TxHTPhyMode;
+	PTXWI_STRUC		pTxWI;
+	PTXINFO_STRUC		pTxInfo;
+	UINT32			TransferBufferLength, OrgBufferLength = 0;
+	UCHAR			padLen = 0;
+#ifdef RALINK_28xx_QA
+	PHEADER_802_11	pHeader80211 = NULL;
+#endif // RALINK_28xx_QA //
+
+	if ((RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) ||
+		(RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET)) ||
+		(RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) ||
+		(RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+	{
+		return -1;
+	}
+
+	/* We always use QID_AC_BE and FIFO_EDCA in ATE mode. */
+
+	pNullContext = &(pAd->NullContext);
+	ASSERT(pNullContext != NULL);
+
+	if (pNullContext->InUse == FALSE)
+	{
+		// Set the in use bit
+		pNullContext->InUse = TRUE;
+		NdisZeroMemory(&(pAd->NullFrame), sizeof(HEADER_802_11));
+
+		// Fill 802.11 header.
+#ifdef RALINK_28xx_QA
+		if (pAd->ate.bQATxStart == TRUE)
+		{
+			pHeader80211 = NdisMoveMemory(&(pAd->NullFrame), pAd->ate.Header, pAd->ate.HLen);
+//			pDest = NdisMoveMemory(&(pAd->NullFrame), pAd->ate.Header, pAd->ate.HLen);
+//			pHeader80211 = (PHEADER_802_11)pDest;
+		}
+		else
+#endif // RALINK_28xx_QA //
+		{
+			// Fill 802.11 header.
+			NdisMoveMemory(&(pAd->NullFrame), TemplateFrame, sizeof(HEADER_802_11));
+		}
+#ifdef RT_BIG_ENDIAN
+		RTMPFrameEndianChange(pAd, (PUCHAR)&(pAd->NullFrame), DIR_READ, FALSE);
+#endif // RT_BIG_ENDIAN //
+
+#ifdef RALINK_28xx_QA
+		if (pAd->ate.bQATxStart == TRUE)
+		{
+			/* modify sequence number.... */
+			if (pAd->ate.TxDoneCount == 0)
+			{
+				pAd->ate.seq = pHeader80211->Sequence;
+			}
+			else
+			{
+				pHeader80211->Sequence = ++pAd->ate.seq;
+			}
+			/* We already got all the addr. fields from QA GUI. */
+		}
+		else
+#endif // RALINK_28xx_QA //
+		{
+			COPY_MAC_ADDR(pAd->NullFrame.Addr1, pAd->ate.Addr1);
+			COPY_MAC_ADDR(pAd->NullFrame.Addr2, pAd->ate.Addr2);
+			COPY_MAC_ADDR(pAd->NullFrame.Addr3, pAd->ate.Addr3);
+		}
+
+		RTMPZeroMemory(&pAd->NullContext.TransferBuffer->field.WirelessPacket[0], TX_BUFFER_NORMSIZE);//???
+		pTxInfo = (PTXINFO_STRUC)&pAd->NullContext.TransferBuffer->field.WirelessPacket[0];
+
+#ifdef RALINK_28xx_QA
+		if (pAd->ate.bQATxStart == TRUE)
+		{
+			// Avoid to exceed the range of WirelessPacket[].
+			ASSERT(pAd->ate.TxInfo.USBDMATxPktLen <= (MAX_FRAME_SIZE - 34/* == 2312 */));
+			NdisMoveMemory(pTxInfo, &(pAd->ate.TxInfo), sizeof(pAd->ate.TxInfo));
+		}
+		else
+#endif // RALINK_28xx_QA //
+		{
+			// Avoid to exceed the range of WirelessPacket[].
+			ASSERT(pAd->ate.TxLength <= (MAX_FRAME_SIZE - 34/* == 2312 */));
+
+			// pTxInfo->USBDMATxPktLen will be updated to include padding later.
+			ATEWriteTxInfo(pAd, pTxInfo, (USHORT)(TXWI_SIZE + pAd->ate.TxLength), TRUE, EpToQueue[MGMTPIPEIDX], FALSE,  FALSE);
+			pTxInfo->QSEL = FIFO_EDCA;
+		}
+
+		pTxWI = (PTXWI_STRUC)&pAd->NullContext.TransferBuffer->field.WirelessPacket[TXINFO_SIZE];
+
+		// Fill TxWI.
+		if (pAd->ate.bQATxStart == TRUE)
+		{
+			TxHTPhyMode.field.BW = pAd->ate.TxWI.BW;
+			TxHTPhyMode.field.ShortGI = pAd->ate.TxWI.ShortGI;
+			TxHTPhyMode.field.STBC = pAd->ate.TxWI.STBC;
+			TxHTPhyMode.field.MCS = pAd->ate.TxWI.MCS;
+			TxHTPhyMode.field.MODE = pAd->ate.TxWI.PHYMODE;
+			ATEWriteTxWI(pAd, pTxWI, pAd->ate.TxWI.FRAG, pAd->ate.TxWI.TS, pAd->ate.TxWI.AMPDU, pAd->ate.TxWI.ACK, pAd->ate.TxWI.NSEQ,
+				pAd->ate.TxWI.BAWinSize, BSSID_WCID, pAd->ate.TxWI.MPDUtotalByteCount/* include 802.11 header */, pAd->ate.TxWI.PacketId, 0, pAd->ate.TxWI.txop/*IFS_HTTXOP*/, pAd->ate.TxWI.CFACK/*FALSE*/, TxHTPhyMode);
+		}
+		else
+		{
+			TxHTPhyMode.field.BW = pAd->ate.TxWI.BW;
+			TxHTPhyMode.field.ShortGI = pAd->ate.TxWI.ShortGI;
+			TxHTPhyMode.field.STBC = 0;
+			TxHTPhyMode.field.MCS = pAd->ate.TxWI.MCS;
+			TxHTPhyMode.field.MODE = pAd->ate.TxWI.PHYMODE;
+
+			ATEWriteTxWI(pAd, pTxWI,  FALSE, FALSE, FALSE, FALSE/* No ack required. */, FALSE, 0, BSSID_WCID, pAd->ate.TxLength,
+				0, 0, IFS_HTTXOP, FALSE, TxHTPhyMode);// "MMPS_STATIC" instead of "MMPS_DYNAMIC" ???
+		}
+
+		RTMPMoveMemory(&pAd->NullContext.TransferBuffer->field.WirelessPacket[TXINFO_SIZE+TXWI_SIZE], &pAd->NullFrame, sizeof(HEADER_802_11));
+
+		pDest = &(pAd->NullContext.TransferBuffer->field.WirelessPacket[TXINFO_SIZE+TXWI_SIZE+sizeof(HEADER_802_11)]);
+
+		// Prepare frame payload
+#ifdef RALINK_28xx_QA
+		if (pAd->ate.bQATxStart == TRUE)
+		{
+			// copy pattern
+			if ((pAd->ate.PLen != 0))
+			{
+				for (j = 0; j < pAd->ate.DLen; j+=pAd->ate.PLen)
+				{
+					RTMPMoveMemory(pDest, pAd->ate.Pattern, pAd->ate.PLen);
+					pDest += pAd->ate.PLen;
+				}
+			}
+			TransferBufferLength = TXINFO_SIZE + TXWI_SIZE + pAd->ate.TxWI.MPDUtotalByteCount;
+		}
+		else
+#endif // RALINK_28xx_QA //
+		{
+		    for (j = 0; j < (pAd->ate.TxLength - sizeof(HEADER_802_11)); j++)
+		    {
+				*pDest = 0xA5;
+				pDest += 1;
+		    }
+			TransferBufferLength = TXINFO_SIZE + TXWI_SIZE + pAd->ate.TxLength;
+		}
+
+#if 1
+		OrgBufferLength = TransferBufferLength;
+		TransferBufferLength = (TransferBufferLength + 3) & (~3);
+
+		// Always add 4 extra bytes at every packet.
+		padLen = TransferBufferLength - OrgBufferLength + 4;/* 4 == last packet padding */
+		ASSERT((padLen <= (RTMP_PKT_TAIL_PADDING - 4/* 4 == MaxBulkOutsize alignment padding */)));
+
+		/* Now memzero all extra padding bytes. */
+		NdisZeroMemory(pDest, padLen);
+		pDest += padLen;
+#else
+		if ((TransferBufferLength % 4) == 1)
+		{
+			NdisZeroMemory(pDest, 7);
+			pDest += 7;
+			TransferBufferLength  += 3;
+		}
+		else if ((TransferBufferLength % 4) == 2)
+		{
+			NdisZeroMemory(pDest, 6);
+			pDest += 6;
+			TransferBufferLength  += 2;
+		}
+		else if ((TransferBufferLength % 4) == 3)
+		{
+			NdisZeroMemory(pDest, 5);
+			pDest += 5;
+			TransferBufferLength  += 1;
+		}
+#endif // 1 //
+
+		// Update pTxInfo->USBDMATxPktLen to include padding.
+		pTxInfo->USBDMATxPktLen = TransferBufferLength - TXINFO_SIZE;
+
+		TransferBufferLength += 4;
+
+		// If TransferBufferLength is multiple of 64, add extra 4 bytes again.
+		if ((TransferBufferLength % pAd->BulkOutMaxPacketSize) == 0)
+		{
+			NdisZeroMemory(pDest, 4);
+			TransferBufferLength += 4;
+		}
+
+		// Fill out frame length information for global Bulk out arbitor
+		pAd->NullContext.BulkOutSize = TransferBufferLength;
+	}
+#ifdef RT_BIG_ENDIAN
+	RTMPWIEndianChange((PUCHAR)pTxWI, TYPE_TXWI);
+	RTMPFrameEndianChange(pAd, (((PUCHAR)pTxInfo)+TXWI_SIZE+TXINFO_SIZE), DIR_WRITE, FALSE);
+    RTMPDescriptorEndianChange((PUCHAR)pTxInfo, TYPE_TXINFO);
+#endif // RT_BIG_ENDIAN //
+	return 0;
+}
+
+VOID ATE_RTUSBBulkOutDataPacketComplete(purbb_t pUrb, struct pt_regs *pt_regs)
+{
+	PRTMP_ADAPTER		pAd;
+	PTX_CONTEXT		    pNullContext;
+	UCHAR				BulkOutPipeId;
+	NTSTATUS			Status;
+	unsigned long		IrqFlags;
+	ULONG			    OldValue;
+
+	pNullContext = (PTX_CONTEXT)pUrb->context;
+	pAd = pNullContext->pAd;
+
+
+	// Reset Null frame context flags
+	pNullContext->IRPPending = FALSE;
+	pNullContext->InUse = FALSE;
+	Status = pUrb->status;
+
+	// Store BulkOut PipeId
+	BulkOutPipeId = pNullContext->BulkOutPipeId;
+	pAd->BulkOutDataOneSecCount++;
+
+	if (Status == USB_ST_NOERROR)
+	{
+#ifdef RALINK_28xx_QA
+		if ((ATE_ON(pAd)) && (pAd->ate.bQATxStart == TRUE))
+		{
+			if (pAd->ate.QID == BulkOutPipeId)
+			{
+				// Let Rx can have a chance to break in during Tx process,
+				// especially for loopback mode in QA ATE.
+				// To trade off between tx performance and loopback mode integrity.
+				/* Q   : Now Rx is handled by tasklet, do we still need this delay ? */
+				/* Ans : Even tasklet is used, Rx/Tx < 1 if we do not delay for a while right here. */
+				RTMPusecDelay(500);
+				pAd->ate.TxDoneCount++;
+				pAd->RalinkCounters.KickTxCount++;
+				ASSERT(pAd->ate.QID == 0);
+				pAd->ate.TxAc0++;
+			}
+		}
+#endif // RALINK_28xx_QA //
+		pAd->BulkOutComplete++;
+
+		pAd->Counters8023.GoodTransmits++;
+
+		/* Don't worry about the queue is empty or not. This function will check itself. */
+		RTMPDeQueuePacket(pAd, TRUE, BulkOutPipeId, MAX_TX_PROCESS);
+
+		/* In 28xx, SendTxWaitQueue ==> TxSwQueue  */
+/*
+		if (pAd->SendTxWaitQueue[BulkOutPipeId].Number > 0)
+		{
+			RTMPDeQueuePacket(pAd, BulkOutPipeId);
+		}
+*/
+	}
+	else	// STATUS_OTHER
+	{
+		pAd->BulkOutCompleteOther++;
+
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("BulkOutDataPacket Failed STATUS_OTHER = 0x%x . \n", Status));
+		ATEDBGPRINT(RT_DEBUG_ERROR, (">>BulkOutReq=0x%lx, BulkOutComplete=0x%lx\n", pAd->BulkOutReq, pAd->BulkOutComplete));
+
+		if ((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET)))
+		{
+			RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_BULKOUT_RESET);
+			/* In 28xx, RT_OID_USB_RESET_BULK_OUT ==> CMDTHREAD_RESET_BULK_OUT */
+			RTUSBEnqueueInternalCmd(pAd, CMDTHREAD_RESET_BULK_OUT, NULL, 0);
+			// Check
+			BULK_OUT_LOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+			pAd->BulkOutPending[BulkOutPipeId] = FALSE;
+			pAd->bulkResetPipeid = BulkOutPipeId;
+			BULK_OUT_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+			return;
+		}
+	}
+
+
+
+	if (atomic_read(&pAd->BulkOutRemained) > 0)
+	{
+		atomic_dec(&pAd->BulkOutRemained);
+	}
+
+	// 1st - Transmit Success
+	OldValue = pAd->WlanCounters.TransmittedFragmentCount.u.LowPart;
+	pAd->WlanCounters.TransmittedFragmentCount.u.LowPart++;
+
+	if (pAd->WlanCounters.TransmittedFragmentCount.u.LowPart < OldValue)
+	{
+		pAd->WlanCounters.TransmittedFragmentCount.u.HighPart++;
+	}
+
+	if(((pAd->ContinBulkOut == TRUE ) ||(atomic_read(&pAd->BulkOutRemained) > 0)) && (pAd->ate.Mode & ATE_TXFRAME))
+	{
+		RTUSB_SET_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_ATE);
+	}
+	else
+	{
+		RTUSB_CLEAR_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_ATE);
+#ifdef RALINK_28xx_QA
+		pAd->ate.TxStatus = 0;
+#endif // RALINK_28xx_QA //
+	}
+
+	BULK_OUT_LOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+	pAd->BulkOutPending[BulkOutPipeId] = FALSE;
+	BULK_OUT_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+
+	// Always call Bulk routine, even reset bulk.
+	// The protection of rest bulk should be in BulkOut routine.
+	RTUSBKickBulkOut(pAd);
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID	ATE_RTUSBBulkOutDataPacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			BulkOutPipeId)
+{
+	PTX_CONTEXT		pNullContext = &(pAd->NullContext);
+	PURB			pUrb;
+	int				ret = 0;
+	unsigned long	IrqFlags;
+
+
+	ASSERT(BulkOutPipeId == 0);
+
+	/* Build up the frame first. */
+//	ATESetUpFrame(pAd, 0);
+
+	BULK_OUT_LOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+
+	if (pAd->BulkOutPending[BulkOutPipeId] == TRUE)
+	{
+		BULK_OUT_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+		return;
+	}
+
+	pAd->BulkOutPending[BulkOutPipeId] = TRUE;
+	BULK_OUT_UNLOCK(&pAd->BulkOutLock[BulkOutPipeId], IrqFlags);
+
+	// Increase Total transmit byte counter
+	pAd->RalinkCounters.OneSecTransmittedByteCount +=  pNullContext->BulkOutSize;
+	pAd->RalinkCounters.TransmittedByteCount +=  pNullContext->BulkOutSize;
+
+	// Clear ATE frame bulk out flag
+	RTUSB_CLEAR_BULK_FLAG(pAd, fRTUSB_BULK_OUT_DATA_ATE);
+
+	// Init Tx context descriptor
+	pNullContext->IRPPending = TRUE;
+	RTUSBInitTxDesc(pAd, pNullContext, BulkOutPipeId, (usb_complete_t)ATE_RTUSBBulkOutDataPacketComplete);
+	pUrb = pNullContext->pUrb;
+
+	if((ret = RTUSB_SUBMIT_URB(pUrb))!=0)
+	{
+		ATEDBGPRINT(RT_DEBUG_ERROR, ("ATE_RTUSBBulkOutDataPacket: Submit Tx URB failed %d\n", ret));
+		return;
+	}
+
+	pAd->BulkOutReq++;
+	return;
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID	ATE_RTUSBCancelPendingBulkInIRP(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	PRX_CONTEXT		pRxContext;
+	UINT			i;
+
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("--->ATE_RTUSBCancelPendingBulkInIRP\n"));
+#if 1
+	for ( i = 0; i < (RX_RING_SIZE); i++)
+	{
+		pRxContext = &(pAd->RxContext[i]);
+		if(pRxContext->IRPPending == TRUE)
+		{
+			RTUSB_UNLINK_URB(pRxContext->pUrb);
+			pRxContext->IRPPending = FALSE;
+			pRxContext->InUse = FALSE;
+			//NdisInterlockedDecrement(&pAd->PendingRx);
+			//pAd->PendingRx--;
+		}
+	}
+#else
+	for ( i = 0; i < (RX_RING_SIZE); i++)
+	{
+		pRxContext = &(pAd->RxContext[i]);
+		if(atomic_read(&pRxContext->IrpLock) == IRPLOCK_CANCELABLE)
+		{
+			RTUSB_UNLINK_URB(pRxContext->pUrb);
+		}
+		InterlockedExchange(&pRxContext->IrpLock, IRPLOCK_CANCE_START);
+	}
+#endif // 1 //
+	ATEDBGPRINT(RT_DEBUG_TRACE, ("<---ATE_RTUSBCancelPendingBulkInIRP\n"));
+	return;
+}
+#endif // RT2870 //
+
+VOID rt_ee_read_all(PRTMP_ADAPTER pAd, USHORT *Data)
+{
+	USHORT i;
+	USHORT value;
+
+	for (i = 0 ; i < EEPROM_SIZE/2 ; )
+	{
+		/* "value" is expecially for some compilers... */
+		RT28xx_EEPROM_READ16(pAd, i*2, value);
+		Data[i] = value;
+		i++;
+	}
+}
+
+VOID rt_ee_write_all(PRTMP_ADAPTER pAd, USHORT *Data)
+{
+	USHORT i;
+	USHORT value;
+
+	for (i = 0 ; i < EEPROM_SIZE/2 ; )
+	{
+		/* "value" is expecially for some compilers... */
+		value = Data[i];
+		RT28xx_EEPROM_WRITE16(pAd, i*2, value);
+		i ++;
+	}
+}
+#ifdef RALINK_28xx_QA
+VOID ATE_QA_Statistics(
+	IN PRTMP_ADAPTER			pAd,
+	IN PRXWI_STRUC				pRxWI,
+	IN PRT28XX_RXD_STRUC		pRxD,
+	IN PHEADER_802_11			pHeader)
+{
+	// update counter first
+	if (pHeader != NULL)
+	{
+		if (pHeader->FC.Type == BTYPE_DATA)
+		{
+			if (pRxD->U2M)
+				pAd->ate.U2M++;
+			else
+				pAd->ate.OtherData++;
+		}
+		else if (pHeader->FC.Type == BTYPE_MGMT)
+		{
+			if (pHeader->FC.SubType == SUBTYPE_BEACON)
+				pAd->ate.Beacon++;
+			else
+				pAd->ate.OtherCount++;
+		}
+		else if (pHeader->FC.Type == BTYPE_CNTL)
+		{
+			pAd->ate.OtherCount++;
+		}
+	}
+	pAd->ate.RSSI0 = pRxWI->RSSI0;
+	pAd->ate.RSSI1 = pRxWI->RSSI1;
+	pAd->ate.RSSI2 = pRxWI->RSSI2;
+	pAd->ate.SNR0 = pRxWI->SNR0;
+	pAd->ate.SNR1 = pRxWI->SNR1;
+}
+
+/* command id with Cmd Type == 0x0008(for 28xx)/0x0005(for iNIC) */
+#define RACFG_CMD_RF_WRITE_ALL		0x0000
+#define RACFG_CMD_E2PROM_READ16		0x0001
+#define RACFG_CMD_E2PROM_WRITE16	0x0002
+#define RACFG_CMD_E2PROM_READ_ALL	0x0003
+#define RACFG_CMD_E2PROM_WRITE_ALL	0x0004
+#define RACFG_CMD_IO_READ			0x0005
+#define RACFG_CMD_IO_WRITE			0x0006
+#define RACFG_CMD_IO_READ_BULK		0x0007
+#define RACFG_CMD_BBP_READ8			0x0008
+#define RACFG_CMD_BBP_WRITE8		0x0009
+#define RACFG_CMD_BBP_READ_ALL		0x000a
+#define RACFG_CMD_GET_COUNTER		0x000b
+#define RACFG_CMD_CLEAR_COUNTER		0x000c
+
+#define RACFG_CMD_RSV1				0x000d
+#define RACFG_CMD_RSV2				0x000e
+#define RACFG_CMD_RSV3				0x000f
+
+#define RACFG_CMD_TX_START			0x0010
+#define RACFG_CMD_GET_TX_STATUS		0x0011
+#define RACFG_CMD_TX_STOP			0x0012
+#define RACFG_CMD_RX_START			0x0013
+#define RACFG_CMD_RX_STOP			0x0014
+#define RACFG_CMD_GET_NOISE_LEVEL	0x0015
+
+#define RACFG_CMD_ATE_START			0x0080
+#define RACFG_CMD_ATE_STOP			0x0081
+
+#define RACFG_CMD_ATE_START_TX_CARRIER		0x0100
+#define RACFG_CMD_ATE_START_TX_CONT			0x0101
+#define RACFG_CMD_ATE_START_TX_FRAME		0x0102
+#define RACFG_CMD_ATE_SET_BW	            0x0103
+#define RACFG_CMD_ATE_SET_TX_POWER0	        0x0104
+#define RACFG_CMD_ATE_SET_TX_POWER1			0x0105
+#define RACFG_CMD_ATE_SET_FREQ_OFFSET		0x0106
+#define RACFG_CMD_ATE_GET_STATISTICS		0x0107
+#define RACFG_CMD_ATE_RESET_COUNTER			0x0108
+#define RACFG_CMD_ATE_SEL_TX_ANTENNA		0x0109
+#define RACFG_CMD_ATE_SEL_RX_ANTENNA		0x010a
+#define RACFG_CMD_ATE_SET_PREAMBLE			0x010b
+#define RACFG_CMD_ATE_SET_CHANNEL			0x010c
+#define RACFG_CMD_ATE_SET_ADDR1				0x010d
+#define RACFG_CMD_ATE_SET_ADDR2				0x010e
+#define RACFG_CMD_ATE_SET_ADDR3				0x010f
+#define RACFG_CMD_ATE_SET_RATE				0x0110
+#define RACFG_CMD_ATE_SET_TX_FRAME_LEN		0x0111
+#define RACFG_CMD_ATE_SET_TX_FRAME_COUNT	0x0112
+#define RACFG_CMD_ATE_START_RX_FRAME		0x0113
+#define RACFG_CMD_ATE_E2PROM_READ_BULK	0x0114
+#define RACFG_CMD_ATE_E2PROM_WRITE_BULK	0x0115
+#define RACFG_CMD_ATE_IO_WRITE_BULK		0x0116
+#define RACFG_CMD_ATE_BBP_READ_BULK		0x0117
+#define RACFG_CMD_ATE_BBP_WRITE_BULK	0x0118
+#define RACFG_CMD_ATE_RF_READ_BULK		0x0119
+#define RACFG_CMD_ATE_RF_WRITE_BULK		0x011a
+
+
+
+#define A2Hex(_X, _p) 				\
+{									\
+	UCHAR *p;						\
+	_X = 0;							\
+	p = _p;							\
+	while (((*p >= 'a') && (*p <= 'f')) || ((*p >= 'A') && (*p <= 'F')) || ((*p >= '0') && (*p <= '9')))		\
+	{												\
+		if ((*p >= 'a') && (*p <= 'f'))				\
+			_X = _X * 16 + *p - 87;					\
+		else if ((*p >= 'A') && (*p <= 'F'))		\
+			_X = _X * 16 + *p - 55;					\
+		else if ((*p >= '0') && (*p <= '9'))		\
+			_X = _X * 16 + *p - 48;					\
+		p++;										\
+	}												\
+}
+
+
+static VOID memcpy_exl(PRTMP_ADAPTER pAd, UCHAR *dst, UCHAR *src, ULONG len);
+static VOID memcpy_exs(PRTMP_ADAPTER pAd, UCHAR *dst, UCHAR *src, ULONG len);
+static VOID RTMP_IO_READ_BULK(PRTMP_ADAPTER pAd, UCHAR *dst, UCHAR *src, UINT32 len);
+
+#ifdef UCOS
+int ate_copy_to_user(
+	IN PUCHAR payload,
+	IN PUCHAR msg,
+	IN INT    len)
+{
+	memmove(payload, msg, len);
+	return 0;
+}
+
+#undef	copy_to_user
+#define copy_to_user(x,y,z) ate_copy_to_user((PUCHAR)x, (PUCHAR)y, z)
+#endif // UCOS //
+
+#define	LEN_OF_ARG 16
+
+VOID RtmpDoAte(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq)
+{
+	unsigned short Command_Id;
+	struct ate_racfghdr *pRaCfg;
+	INT	Status = NDIS_STATUS_SUCCESS;
+
+
+
+	if((pRaCfg = kmalloc(sizeof(struct ate_racfghdr), GFP_KERNEL)) == NULL)
+	{
+		Status = -EINVAL;
+		return;
+	}
+
+	NdisZeroMemory(pRaCfg, sizeof(struct ate_racfghdr));
+
+    if (copy_from_user((PUCHAR)pRaCfg, wrq->u.data.pointer, wrq->u.data.length))
+	{
+		Status = -EFAULT;
+		kfree(pRaCfg);
+		return;
+	}
+
+
+	Command_Id = ntohs(pRaCfg->command_id);
+
+	ATEDBGPRINT(RT_DEBUG_TRACE,("\n%s: Command_Id = 0x%04x !\n", __FUNCTION__, Command_Id));
+
+	switch (Command_Id)
+	{
+ 		// We will get this command when QA starts.
+		case RACFG_CMD_ATE_START:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_START\n"));
+
+				// prepare feedback as soon as we can to avoid QA timeout.
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("wrq->u.data.length = %d\n", wrq->u.data.length));
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_TRACE, ("copy_to_user() fail in case RACFG_CMD_ATE_START\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_START is done !\n"));
+				}
+				Set_ATE_Proc(pAdapter, "ATESTART");
+			}
+			break;
+
+ 		// We will get this command either QA is closed or ated is killed by user.
+		case RACFG_CMD_ATE_STOP:
+			{
+#ifndef UCOS
+				INT32 ret;
+#endif // !UCOS //
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_STOP\n"));
+
+				// Distinguish this command came from QA(via ated)
+				// or ate daemon according to the existence of pid in payload.
+				// No need to prepare feedback if this cmd came directly from ate daemon.
+				pRaCfg->length = ntohs(pRaCfg->length);
+
+				if (pRaCfg->length == sizeof(pAdapter->ate.AtePid))
+				{
+					// This command came from QA.
+					// Get the pid of ATE daemon.
+					memcpy((UCHAR *)&pAdapter->ate.AtePid,
+						(&pRaCfg->data[0]) - 2/* == &(pRaCfg->status) */,
+						sizeof(pAdapter->ate.AtePid));
+
+					// prepare feedback as soon as we can to avoid QA timeout.
+					pRaCfg->length = htons(2);
+					pRaCfg->status = htons(0);
+
+					wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+										+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+										+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+					ATEDBGPRINT(RT_DEBUG_TRACE, ("wrq->u.data.length = %d\n", wrq->u.data.length));
+
+	            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+	            	{
+	            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_STOP\n"));
+	                    Status = -EFAULT;
+	            	}
+
+					//
+					// kill ATE daemon when leaving ATE mode.
+					// We must kill ATE daemon first before setting ATESTOP,
+					// or Microsoft will report sth. wrong.
+#ifndef UCOS
+					ret = KILL_THREAD_PID(pAdapter->ate.AtePid, SIGTERM, 1);
+					if (ret)
+					{
+						ATEDBGPRINT(RT_DEBUG_ERROR, ("%s: unable to signal thread\n", pAdapter->net_dev->name));
+					}
+#endif // !UCOS //
+				}
+
+				// AP might have in ATE_STOP mode due to cmd from QA.
+				if (ATE_ON(pAdapter))
+				{
+					// Someone has killed ate daemon while QA GUI is still open.
+					Set_ATE_Proc(pAdapter, "ATESTOP");
+					ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_AP_START is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_RF_WRITE_ALL:
+			{
+				UINT32 R1, R2, R3, R4;
+				USHORT channel;
+
+				memcpy(&R1, pRaCfg->data-2, 4);
+				memcpy(&R2, pRaCfg->data+2, 4);
+				memcpy(&R3, pRaCfg->data+6, 4);
+				memcpy(&R4, pRaCfg->data+10, 4);
+				memcpy(&channel, pRaCfg->data+14, 2);
+
+				pAdapter->LatchRfRegs.R1 = ntohl(R1);
+				pAdapter->LatchRfRegs.R2 = ntohl(R2);
+				pAdapter->LatchRfRegs.R3 = ntohl(R3);
+				pAdapter->LatchRfRegs.R4 = ntohl(R4);
+				pAdapter->LatchRfRegs.Channel = ntohs(channel);
+
+				RTMP_RF_IO_WRITE32(pAdapter, pAdapter->LatchRfRegs.R1);
+				RTMP_RF_IO_WRITE32(pAdapter, pAdapter->LatchRfRegs.R2);
+				RTMP_RF_IO_WRITE32(pAdapter, pAdapter->LatchRfRegs.R3);
+				RTMP_RF_IO_WRITE32(pAdapter, pAdapter->LatchRfRegs.R4);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("wrq->u.data.length = %d\n", wrq->u.data.length));
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_RF_WRITE_ALL\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_RF_WRITE_ALL is done !\n"));
+				}
+			}
+            break;
+
+		case RACFG_CMD_E2PROM_READ16:
+			{
+				USHORT	offset, value, tmp;
+
+				offset = ntohs(pRaCfg->status);
+				/* "tmp" is expecially for some compilers... */
+				RT28xx_EEPROM_READ16(pAdapter, offset, tmp);
+				value = tmp;
+				value = htons(value);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("EEPROM Read offset = 0x%04x, value = 0x%04x\n", offset, value));
+
+				// prepare feedback
+				pRaCfg->length = htons(4);
+				pRaCfg->status = htons(0);
+				memcpy(pRaCfg->data, &value, 2);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("sizeof(struct ate_racfghdr) = %d\n", sizeof(struct ate_racfghdr)));
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("wrq->u.data.length = %d\n", wrq->u.data.length));
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_E2PROM_READ16\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_E2PROM_READ16 is done !\n"));
+				}
+           	}
+			break;
+
+		case RACFG_CMD_E2PROM_WRITE16:
+			{
+				USHORT	offset, value;
+
+				offset = ntohs(pRaCfg->status);
+				memcpy(&value, pRaCfg->data, 2);
+				value = ntohs(value);
+				RT28xx_EEPROM_WRITE16(pAdapter, offset, value);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_E2PROM_WRITE16\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_E2PROM_WRITE16 is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_E2PROM_READ_ALL:
+			{
+				USHORT buffer[EEPROM_SIZE/2];
+
+				rt_ee_read_all(pAdapter,(USHORT *)buffer);
+				memcpy_exs(pAdapter, pRaCfg->data, (UCHAR *)buffer, EEPROM_SIZE);
+
+				// prepare feedback
+				pRaCfg->length = htons(2+EEPROM_SIZE);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_E2PROM_READ_ALL\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_E2PROM_READ_ALL is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_E2PROM_WRITE_ALL:
+			{
+				USHORT buffer[EEPROM_SIZE/2];
+
+				NdisZeroMemory((UCHAR *)buffer, EEPROM_SIZE);
+				memcpy_exs(pAdapter, (UCHAR *)buffer, (UCHAR *)&pRaCfg->status, EEPROM_SIZE);
+				rt_ee_write_all(pAdapter,(USHORT *)buffer);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_E2PROM_WRITE_ALL\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_ERROR, ("RACFG_CMD_E2PROM_WRITE_ALL is done !\n"));
+				}
+
+			}
+			break;
+
+		case RACFG_CMD_IO_READ:
+			{
+				UINT32	offset;
+				UINT32	value;
+
+				memcpy(&offset, &pRaCfg->status, 4);
+				offset = ntohl(offset);
+
+				// We do not need the base address.
+				// So just extract the offset out.
+				offset &= 0x0000FFFF;
+				RTMP_IO_READ32(pAdapter, offset, &value);
+				value = htonl(value);
+
+				// prepare feedback
+				pRaCfg->length = htons(6);
+				pRaCfg->status = htons(0);
+				memcpy(pRaCfg->data, &value, 4);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_IO_READ\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_IO_READ is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_IO_WRITE:
+			{
+				UINT32	offset, value;
+
+				memcpy(&offset, pRaCfg->data-2, 4);
+				memcpy(&value, pRaCfg->data+2, 4);
+
+				offset = ntohl(offset);
+
+				// We do not need the base address.
+				// So just extract out the offset.
+				offset &= 0x0000FFFF;
+				value = ntohl(value);
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_IO_WRITE: offset = %x, value = %x\n", offset, value));
+				RTMP_IO_WRITE32(pAdapter, offset, value);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_IO_WRITE\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_IO_WRITE is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_IO_READ_BULK:
+			{
+				UINT32	offset;
+				USHORT	len;
+
+				memcpy(&offset, &pRaCfg->status, 4);
+				offset = ntohl(offset);
+
+				// We do not need the base address.
+				// So just extract the offset.
+				offset &= 0x0000FFFF;
+				memcpy(&len, pRaCfg->data+2, 2);
+				len = ntohs(len);
+
+				if (len > 371)
+				{
+					ATEDBGPRINT(RT_DEBUG_TRACE,("len is too large, make it smaller\n"));
+					pRaCfg->length = htons(2);
+					pRaCfg->status = htons(1);
+					break;
+				}
+
+				RTMP_IO_READ_BULK(pAdapter, pRaCfg->data, (UCHAR *)offset, len*4);// unit in four bytes
+
+				// prepare feedback
+				pRaCfg->length = htons(2+len*4);// unit in four bytes
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_IO_READ_BULK\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_IO_READ_BULK is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_BBP_READ8:
+			{
+				USHORT	offset;
+				UCHAR	value;
+
+				value = 0;
+				offset = ntohs(pRaCfg->status);
+
+				if (ATE_ON(pAdapter))
+				{
+					ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, offset,  &value);
+				}
+				else
+				{
+					RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, offset,  &value);
+				}
+				// prepare feedback
+				pRaCfg->length = htons(3);
+				pRaCfg->status = htons(0);
+				pRaCfg->data[0] = value;
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("BBP value = %x\n", value));
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_BBP_READ8\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_BBP_READ8 is done !\n"));
+				}
+			}
+			break;
+		case RACFG_CMD_BBP_WRITE8:
+			{
+				USHORT	offset;
+				UCHAR	value;
+
+				offset = ntohs(pRaCfg->status);
+				memcpy(&value, pRaCfg->data, 1);
+
+				if (ATE_ON(pAdapter))
+				{
+					ATE_BBP_IO_WRITE8_BY_REG_ID(pAdapter, offset,  value);
+				}
+				else
+				{
+					RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, offset,  value);
+				}
+
+				if ((offset == BBP_R1) || (offset == BBP_R3))
+				{
+					SyncTxRxConfig(pAdapter, offset, value);
+				}
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_BBP_WRITE8\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_BBP_WRITE8 is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_BBP_READ_ALL:
+			{
+				USHORT j;
+
+				for (j = 0; j < 137; j++)
+				{
+					pRaCfg->data[j] = 0;
+
+					if (ATE_ON(pAdapter))
+					{
+						ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, j,  &pRaCfg->data[j]);
+					}
+					else
+					{
+						RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, j,  &pRaCfg->data[j]);
+					}
+				}
+
+				// prepare feedback
+				pRaCfg->length = htons(2+137);
+				pRaCfg->status = htons(0);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_BBP_READ_ALL\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_BBP_READ_ALL is done !\n"));
+				}
+			}
+
+			break;
+
+		case RACFG_CMD_ATE_E2PROM_READ_BULK:
+		{
+			USHORT offset;
+			USHORT len;
+			USHORT buffer[EEPROM_SIZE/2];
+
+			offset = ntohs(pRaCfg->status);
+			memcpy(&len, pRaCfg->data, 2);
+			len = ntohs(len);
+
+			rt_ee_read_all(pAdapter,(USHORT *)buffer);
+			if (offset + len <= EEPROM_SIZE)
+				memcpy_exs(pAdapter, pRaCfg->data, (UCHAR *)buffer+offset, len);
+			else
+				ATEDBGPRINT(RT_DEBUG_ERROR, ("exceed EEPROM size\n"));
+
+			// prepare feedback
+			pRaCfg->length = htons(2+len);
+			pRaCfg->status = htons(0);
+			wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            {
+            	ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_E2PROM_READ_BULK\n"));
+                Status = -EFAULT;
+            }
+			else
+			{
+               	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_E2PROM_READ_BULK is done !\n"));
+			}
+
+		}
+			break;
+
+		case RACFG_CMD_ATE_E2PROM_WRITE_BULK:
+		{
+			USHORT offset;
+			USHORT len;
+			USHORT buffer[EEPROM_SIZE/2];
+
+			offset = ntohs(pRaCfg->status);
+			memcpy(&len, pRaCfg->data, 2);
+			len = ntohs(len);
+
+			rt_ee_read_all(pAdapter,(USHORT *)buffer);
+			memcpy_exs(pAdapter, (UCHAR *)buffer + offset, (UCHAR *)pRaCfg->data + 2, len);
+			rt_ee_write_all(pAdapter,(USHORT *)buffer);
+
+			// prepare feedback
+			pRaCfg->length = htons(2);
+			pRaCfg->status = htons(0);
+			wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+								+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+								+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+            if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            {
+            	ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_E2PROM_WRITE_BULK\n"));
+                   Status = -EFAULT;
+            }
+			else
+			{
+               	ATEDBGPRINT(RT_DEBUG_ERROR, ("RACFG_CMD_ATE_E2PROM_WRITE_BULK is done !\n"));
+			}
+
+		}
+			break;
+
+		case RACFG_CMD_ATE_IO_WRITE_BULK:
+		{
+			UINT32 offset, i, value;
+			USHORT len;
+
+			memcpy(&offset, &pRaCfg->status, 4);
+			offset = ntohl(offset);
+			memcpy(&len, pRaCfg->data+2, 2);
+			len = ntohs(len);
+
+			for (i = 0; i < len; i += 4)
+			{
+				memcpy_exl(pAdapter, (UCHAR *)&value, pRaCfg->data+4+i, 4);
+				printk("Write %x %x\n", offset + i, value);
+				RTMP_IO_WRITE32(pAdapter, (offset +i) & 0xffff, value);
+			}
+
+			// prepare feedback
+			pRaCfg->length = htons(2);
+			pRaCfg->status = htons(0);
+			wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+								+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+								+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+            if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            {
+            	ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_IO_WRITE_BULK\n"));
+                   Status = -EFAULT;
+            }
+			else
+			{
+               	ATEDBGPRINT(RT_DEBUG_ERROR, ("RACFG_CMD_ATE_IO_WRITE_BULK is done !\n"));
+			}
+
+		}
+			break;
+
+		case RACFG_CMD_ATE_BBP_READ_BULK:
+		{
+			USHORT offset;
+			USHORT len;
+			USHORT j;
+
+			offset = ntohs(pRaCfg->status);
+			memcpy(&len, pRaCfg->data, 2);
+			len = ntohs(len);
+
+
+			for (j = offset; j < (offset+len); j++)
+			{
+				pRaCfg->data[j - offset] = 0;
+
+				if (pAdapter->ate.Mode == ATE_STOP)
+				{
+					RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, j,  &pRaCfg->data[j - offset]);
+				}
+				else
+				{
+					ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, j,  &pRaCfg->data[j - offset]);
+				}
+			}
+
+			// prepare feedback
+			pRaCfg->length = htons(2+len);
+			pRaCfg->status = htons(0);
+			wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+								+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+								+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            {
+            	ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_BBP_READ_BULK\n"));
+                   Status = -EFAULT;
+            }
+			else
+			{
+               	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_BBP_READ_BULK is done !\n"));
+			}
+
+		}
+			break;
+
+		case RACFG_CMD_ATE_BBP_WRITE_BULK:
+		{
+			USHORT offset;
+			USHORT len;
+			USHORT j;
+			UCHAR *value;
+
+			offset = ntohs(pRaCfg->status);
+			memcpy(&len, pRaCfg->data, 2);
+			len = ntohs(len);
+
+			for (j = offset; j < (offset+len); j++)
+			{
+				value = pRaCfg->data + 2 + (j - offset);
+				if (pAdapter->ate.Mode == ATE_STOP)
+				{
+					RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, j,  *value);
+				}
+				else
+				{
+					ATE_BBP_IO_WRITE8_BY_REG_ID(pAdapter, j,  *value);
+				}
+			}
+
+			// prepare feedback
+			pRaCfg->length = htons(2);
+			pRaCfg->status = htons(0);
+			wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+								+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+								+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            {
+            	ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_BBP_WRITE_BULK\n"));
+                   Status = -EFAULT;
+            }
+			else
+			{
+               	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_BBP_WRITE_BULK is done !\n"));
+			}
+		}
+			break;
+
+#ifdef CONFIG_RALINK_RT3052
+		case RACFG_CMD_ATE_RF_READ_BULK:
+		{
+			USHORT offset;
+			USHORT len;
+			USHORT j;
+
+			offset = ntohs(pRaCfg->status);
+			memcpy(&len, pRaCfg->data, 2);
+			len = ntohs(len);
+
+			for (j = offset; j < (offset+len); j++)
+			{
+				pRaCfg->data[j - offset] = 0;
+				RT30xxReadRFRegister(pAdapter, j,  &pRaCfg->data[j - offset]);
+			}
+
+			// prepare feedback
+			pRaCfg->length = htons(2+len);
+			pRaCfg->status = htons(0);
+			wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+								+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+								+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            {
+            	ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_RF_READ_BULK\n"));
+                   Status = -EFAULT;
+            }
+			else
+			{
+               	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_RF_READ_BULK is done !\n"));
+			}
+
+		}
+			break;
+
+		case RACFG_CMD_ATE_RF_WRITE_BULK:
+		{
+			USHORT offset;
+			USHORT len;
+			USHORT j;
+			UCHAR *value;
+
+			offset = ntohs(pRaCfg->status);
+			memcpy(&len, pRaCfg->data, 2);
+			len = ntohs(len);
+
+			for (j = offset; j < (offset+len); j++)
+			{
+				value = pRaCfg->data + 2 + (j - offset);
+				RT30xxWriteRFRegister(pAdapter, j,  *value);
+			}
+
+			// prepare feedback
+			pRaCfg->length = htons(2);
+			pRaCfg->status = htons(0);
+			wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+								+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+								+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            {
+            	ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_RF_WRITE_BULK\n"));
+                   Status = -EFAULT;
+            }
+			else
+			{
+               	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_RF_WRITE_BULK is done !\n"));
+			}
+
+		}
+			break;
+#endif
+
+
+		case RACFG_CMD_GET_NOISE_LEVEL:
+			{
+				UCHAR	channel;
+				INT32   buffer[3][10];/* 3 : RxPath ; 10 : no. of per rssi samples */
+
+				channel = (ntohs(pRaCfg->status) & 0x00FF);
+				CalNoiseLevel(pAdapter, channel, buffer);
+				memcpy_exl(pAdapter, (UCHAR *)pRaCfg->data, (UCHAR *)&(buffer[0][0]), (sizeof(INT32)*3*10));
+
+				// prepare feedback
+				pRaCfg->length = htons(2 + (sizeof(INT32)*3*10));
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_GET_NOISE_LEVEL\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_GET_NOISE_LEVEL is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_GET_COUNTER:
+			{
+				memcpy_exl(pAdapter, &pRaCfg->data[0], (UCHAR *)&pAdapter->ate.U2M, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[4], (UCHAR *)&pAdapter->ate.OtherData, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[8], (UCHAR *)&pAdapter->ate.Beacon, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[12], (UCHAR *)&pAdapter->ate.OtherCount, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[16], (UCHAR *)&pAdapter->ate.TxAc0, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[20], (UCHAR *)&pAdapter->ate.TxAc1, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[24], (UCHAR *)&pAdapter->ate.TxAc2, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[28], (UCHAR *)&pAdapter->ate.TxAc3, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[32], (UCHAR *)&pAdapter->ate.TxHCCA, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[36], (UCHAR *)&pAdapter->ate.TxMgmt, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[40], (UCHAR *)&pAdapter->ate.RSSI0, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[44], (UCHAR *)&pAdapter->ate.RSSI1, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[48], (UCHAR *)&pAdapter->ate.RSSI2, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[52], (UCHAR *)&pAdapter->ate.SNR0, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[56], (UCHAR *)&pAdapter->ate.SNR1, 4);
+
+				pRaCfg->length = htons(2+60);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_GET_COUNTER\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_GET_COUNTER is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_CLEAR_COUNTER:
+			{
+				pAdapter->ate.U2M = 0;
+				pAdapter->ate.OtherData = 0;
+				pAdapter->ate.Beacon = 0;
+				pAdapter->ate.OtherCount = 0;
+				pAdapter->ate.TxAc0 = 0;
+				pAdapter->ate.TxAc1 = 0;
+				pAdapter->ate.TxAc2 = 0;
+				pAdapter->ate.TxAc3 = 0;
+				pAdapter->ate.TxHCCA = 0;
+				pAdapter->ate.TxMgmt = 0;
+				pAdapter->ate.TxDoneCount = 0;
+
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_CLEAR_COUNTER\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_CLEAR_COUNTER is done !\n"));
+				}
+			}
+
+			break;
+
+		case RACFG_CMD_TX_START:
+			{
+				USHORT *p;
+				USHORT	err = 1;
+				UCHAR   Bbp22Value = 0, Bbp24Value = 0;
+
+				if ((pAdapter->ate.TxStatus != 0) && (pAdapter->ate.Mode & ATE_TXFRAME))
+				{
+					ATEDBGPRINT(RT_DEBUG_TRACE,("Ate Tx is already running, to run next Tx, you must stop it first\n"));
+					err = 2;
+					goto TX_START_ERROR;
+				}
+				else if ((pAdapter->ate.TxStatus != 0) && !(pAdapter->ate.Mode & ATE_TXFRAME))
+				{
+					int i = 0;
+
+					while ((i++ < 10) && (pAdapter->ate.TxStatus != 0))
+					{
+						RTMPusecDelay(5000);
+					}
+
+					// force it to stop
+					pAdapter->ate.TxStatus = 0;
+					pAdapter->ate.TxDoneCount = 0;
+					//pAdapter->ate.Repeat = 0;
+					pAdapter->ate.bQATxStart = FALSE;
+				}
+
+				// If pRaCfg->length == 0, this "RACFG_CMD_TX_START" is for Carrier test or Carrier Suppression.
+				if (ntohs(pRaCfg->length) != 0)
+				{
+					// Get frame info
+#ifdef RT2870
+					NdisMoveMemory(&pAdapter->ate.TxInfo, pRaCfg->data - 2, 4);
+#ifdef RT_BIG_ENDIAN
+					RTMPDescriptorEndianChange((PUCHAR) &pAdapter->ate.TxInfo, TYPE_TXINFO);
+#endif // RT_BIG_ENDIAN //
+#endif // RT2870 //
+
+					NdisMoveMemory(&pAdapter->ate.TxWI, pRaCfg->data + 2, 16);
+#ifdef RT_BIG_ENDIAN
+					RTMPWIEndianChange((PUCHAR)&pAdapter->ate.TxWI, TYPE_TXWI);
+#endif // RT_BIG_ENDIAN //
+
+					NdisMoveMemory(&pAdapter->ate.TxCount, pRaCfg->data + 18, 4);
+					pAdapter->ate.TxCount = ntohl(pAdapter->ate.TxCount);
+
+					p = (USHORT *)(&pRaCfg->data[22]);
+					//p = pRaCfg->data + 22;
+					// always use QID_AC_BE
+					pAdapter->ate.QID = 0;
+					p = (USHORT *)(&pRaCfg->data[24]);
+					//p = pRaCfg->data + 24;
+					pAdapter->ate.HLen = ntohs(*p);
+
+					if (pAdapter->ate.HLen > 32)
+					{
+						ATEDBGPRINT(RT_DEBUG_ERROR,("pAdapter->ate.HLen > 32\n"));
+						err = 3;
+						goto TX_START_ERROR;
+					}
+
+					NdisMoveMemory(&pAdapter->ate.Header, pRaCfg->data + 26, pAdapter->ate.HLen);
+
+
+					pAdapter->ate.PLen = ntohs(pRaCfg->length) - (pAdapter->ate.HLen + 28);
+
+					if (pAdapter->ate.PLen > 32)
+					{
+						ATEDBGPRINT(RT_DEBUG_ERROR,("pAdapter->ate.PLen > 32\n"));
+						err = 4;
+						goto TX_START_ERROR;
+					}
+
+					NdisMoveMemory(&pAdapter->ate.Pattern, pRaCfg->data + 26 + pAdapter->ate.HLen, pAdapter->ate.PLen);
+					pAdapter->ate.DLen = pAdapter->ate.TxWI.MPDUtotalByteCount - pAdapter->ate.HLen;
+				}
+
+				ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R22, &Bbp22Value);
+
+				switch (Bbp22Value)
+				{
+					case BBP22_TXFRAME:
+						{
+							if (pAdapter->ate.TxCount == 0)
+							{
+							}
+							ATEDBGPRINT(RT_DEBUG_TRACE,("START TXFRAME\n"));
+							pAdapter->ate.bQATxStart = TRUE;
+							Set_ATE_Proc(pAdapter, "TXFRAME");
+						}
+						break;
+
+					case BBP22_TXCONT_OR_CARRSUPP:
+						{
+							ATEDBGPRINT(RT_DEBUG_TRACE,("BBP22_TXCONT_OR_CARRSUPP\n"));
+							ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, 24, &Bbp24Value);
+
+							switch (Bbp24Value)
+							{
+								case BBP24_TXCONT:
+									{
+										ATEDBGPRINT(RT_DEBUG_TRACE,("START TXCONT\n"));
+										pAdapter->ate.bQATxStart = TRUE;
+										Set_ATE_Proc(pAdapter, "TXCONT");
+									}
+									break;
+
+								case BBP24_CARRSUPP:
+									{
+										ATEDBGPRINT(RT_DEBUG_TRACE,("START TXCARRSUPP\n"));
+										pAdapter->ate.bQATxStart = TRUE;
+										pAdapter->ate.Mode |= ATE_TXCARRSUPP;
+									}
+									break;
+
+								default:
+									{
+										ATEDBGPRINT(RT_DEBUG_ERROR,("Unknown Start TX subtype !"));
+									}
+									break;
+							}
+						}
+						break;
+
+					case BBP22_TXCARR:
+						{
+							ATEDBGPRINT(RT_DEBUG_TRACE,("START TXCARR\n"));
+							pAdapter->ate.bQATxStart = TRUE;
+							Set_ATE_Proc(pAdapter, "TXCARR");
+						}
+						break;
+
+					default:
+						{
+							ATEDBGPRINT(RT_DEBUG_ERROR,("Unknown Start TX subtype !"));
+						}
+						break;
+				}
+
+				if (pAdapter->ate.bQATxStart == TRUE)
+				{
+					// prepare feedback
+					pRaCfg->length = htons(2);
+					pRaCfg->status = htons(0);
+
+					wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+										+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+										+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+	            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+	            	{
+	            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() was failed in case RACFG_CMD_TX_START\n"));
+	                    Status = -EFAULT;
+	            	}
+					else
+					{
+	                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_TX_START is done !\n"));
+					}
+					break;
+				}
+
+TX_START_ERROR:
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(err);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_TX_START\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("feedback of TX_START_ERROR is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_GET_TX_STATUS:
+			{
+				UINT32 count;
+
+				// prepare feedback
+				pRaCfg->length = htons(6);
+				pRaCfg->status = htons(0);
+				count = htonl(pAdapter->ate.TxDoneCount);
+				NdisMoveMemory(pRaCfg->data, &count, 4);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_GET_TX_STATUS\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_GET_TX_STATUS is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_TX_STOP:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_TX_STOP\n"));
+
+				Set_ATE_Proc(pAdapter, "TXSTOP");
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_TRACE, ("copy_to_user() fail in case RACFG_CMD_TX_STOP\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_TX_STOP is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_RX_START:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_RX_START\n"));
+
+				pAdapter->ate.bQARxStart = TRUE;
+				Set_ATE_Proc(pAdapter, "RXFRAME");
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_RX_START\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_RX_START is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_RX_STOP:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_RX_STOP\n"));
+
+				Set_ATE_Proc(pAdapter, "RXSTOP");
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_RX_STOP\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_RX_STOP is done !\n"));
+				}
+			}
+			break;
+
+		/* The following cases are for new ATE GUI(not QA). */
+		/*==================================================*/
+		case RACFG_CMD_ATE_START_TX_CARRIER:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_START_TX_CARRIER\n"));
+
+				Set_ATE_Proc(pAdapter, "TXCARR");
+
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("wrq->u.data.length = %d\n", wrq->u.data.length));
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_START_TX_CARRIER\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_START_TX_CARRIER is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_START_TX_CONT:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_START_TX_CONT\n"));
+
+				Set_ATE_Proc(pAdapter, "TXCONT");
+
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("wrq->u.data.length = %d\n", wrq->u.data.length));
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_START_TX_CONT\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_START_TX_CONT is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_START_TX_FRAME:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_START_TX_FRAME\n"));
+
+				Set_ATE_Proc(pAdapter, "TXFRAME");
+
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE, ("wrq->u.data.length = %d\n", wrq->u.data.length));
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_START_TX_FRAME\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_START_TX_FRAME is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_BW:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_BW\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+
+				Set_ATE_TX_BW_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_BW\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_BW is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_TX_POWER0:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_TX_POWER0\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+				Set_ATE_TX_POWER0_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_TX_POWER0\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_TX_POWER0 is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_TX_POWER1:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_TX_POWER1\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+				Set_ATE_TX_POWER1_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_TX_POWER1\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_TX_POWER1 is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_FREQ_OFFSET:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_FREQ_OFFSET\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+				Set_ATE_TX_FREQOFFSET_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_FREQ_OFFSET\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_FREQ_OFFSET is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_GET_STATISTICS:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_GET_STATISTICS\n"));
+
+				memcpy_exl(pAdapter, &pRaCfg->data[0], (UCHAR *)&pAdapter->ate.TxDoneCount, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[4], (UCHAR *)&pAdapter->WlanCounters.RetryCount.u.LowPart, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[8], (UCHAR *)&pAdapter->WlanCounters.FailedCount.u.LowPart, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[12], (UCHAR *)&pAdapter->WlanCounters.RTSSuccessCount.u.LowPart, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[16], (UCHAR *)&pAdapter->WlanCounters.RTSFailureCount.u.LowPart, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[20], (UCHAR *)&pAdapter->WlanCounters.ReceivedFragmentCount.QuadPart, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[24], (UCHAR *)&pAdapter->WlanCounters.FCSErrorCount.u.LowPart, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[28], (UCHAR *)&pAdapter->Counters8023.RxNoBuffer, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[32], (UCHAR *)&pAdapter->WlanCounters.FrameDuplicateCount.u.LowPart, 4);
+				memcpy_exl(pAdapter, &pRaCfg->data[36], (UCHAR *)&pAdapter->RalinkCounters.OneSecFalseCCACnt, 4);
+
+				if (pAdapter->ate.RxAntennaSel == 0)
+				{
+					INT32 RSSI0 = 0;
+					INT32 RSSI1 = 0;
+					INT32 RSSI2 = 0;
+
+					RSSI0 = (INT32)(pAdapter->ate.LastRssi0 - pAdapter->BbpRssiToDbmDelta);
+					RSSI1 = (INT32)(pAdapter->ate.LastRssi1 - pAdapter->BbpRssiToDbmDelta);
+					RSSI2 = (INT32)(pAdapter->ate.LastRssi2 - pAdapter->BbpRssiToDbmDelta);
+					memcpy_exl(pAdapter, &pRaCfg->data[40], (UCHAR *)&RSSI0, 4);
+					memcpy_exl(pAdapter, &pRaCfg->data[44], (UCHAR *)&RSSI1, 4);
+					memcpy_exl(pAdapter, &pRaCfg->data[48], (UCHAR *)&RSSI2, 4);
+					pRaCfg->length = htons(2+52);
+				}
+				else
+				{
+					INT32 RSSI0 = 0;
+
+					RSSI0 = (INT32)(pAdapter->ate.LastRssi0 - pAdapter->BbpRssiToDbmDelta);
+					memcpy_exl(pAdapter, &pRaCfg->data[40], (UCHAR *)&RSSI0, 4);
+					pRaCfg->length = htons(2+44);
+				}
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_GET_STATISTICS\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_GET_STATISTICS is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_RESET_COUNTER:
+			{
+				SHORT    value = 1;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_RESET_COUNTER\n"));
+
+				sprintf((PCHAR)str, "%d", value);
+				Set_ResetStatCounter_Proc(pAdapter, str);
+
+				pAdapter->ate.TxDoneCount = 0;
+
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_RESET_COUNTER\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_RESET_COUNTER is done !\n"));
+				}
+			}
+
+			break;
+
+		case RACFG_CMD_ATE_SEL_TX_ANTENNA:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SEL_TX_ANTENNA\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+				Set_ATE_TX_Antenna_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SEL_TX_ANTENNA\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SEL_TX_ANTENNA is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SEL_RX_ANTENNA:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SEL_RX_ANTENNA\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+				Set_ATE_RX_Antenna_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SEL_RX_ANTENNA\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SEL_RX_ANTENNA is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_PREAMBLE:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_PREAMBLE\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+				Set_ATE_TX_MODE_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_PREAMBLE\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_PREAMBLE is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_CHANNEL:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_CHANNEL\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+				Set_ATE_CHANNEL_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_CHANNEL\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_CHANNEL is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_ADDR1:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_ADDR1\n"));
+
+				// Addr is an array of UCHAR,
+				// so no need to perform endian swap.
+				memcpy(pAdapter->ate.Addr1, (PUCHAR)(pRaCfg->data - 2), MAC_ADDR_LEN);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_ADDR1\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+					ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_ADDR1 is done !\n (ADDR1 = %2X:%2X:%2X:%2X:%2X:%2X)\n", pAdapter->ate.Addr1[0],
+						pAdapter->ate.Addr1[1], pAdapter->ate.Addr1[2], pAdapter->ate.Addr1[3], pAdapter->ate.Addr1[4], pAdapter->ate.Addr1[5]));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_ADDR2:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_ADDR2\n"));
+
+				// Addr is an array of UCHAR,
+				// so no need to perform endian swap.
+				memcpy(pAdapter->ate.Addr2, (PUCHAR)(pRaCfg->data - 2), MAC_ADDR_LEN);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_ADDR2\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+					ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_ADDR2 is done !\n (ADDR2 = %2X:%2X:%2X:%2X:%2X:%2X)\n", pAdapter->ate.Addr2[0],
+						pAdapter->ate.Addr2[1], pAdapter->ate.Addr2[2], pAdapter->ate.Addr2[3], pAdapter->ate.Addr2[4], pAdapter->ate.Addr2[5]));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_ADDR3:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_ADDR3\n"));
+
+				// Addr is an array of UCHAR,
+				// so no need to perform endian swap.
+				memcpy(pAdapter->ate.Addr3, (PUCHAR)(pRaCfg->data - 2), MAC_ADDR_LEN);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_ADDR3\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+					ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_ADDR3 is done !\n (ADDR3 = %2X:%2X:%2X:%2X:%2X:%2X)\n", pAdapter->ate.Addr3[0],
+						pAdapter->ate.Addr3[1], pAdapter->ate.Addr3[2], pAdapter->ate.Addr3[3], pAdapter->ate.Addr3[4], pAdapter->ate.Addr3[5]));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_RATE:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_RATE\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+				Set_ATE_TX_MCS_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_RATE\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_RATE is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_TX_FRAME_LEN:
+			{
+				SHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_TX_FRAME_LEN\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				sprintf((PCHAR)str, "%d", value);
+				Set_ATE_TX_LENGTH_Proc(pAdapter, str);
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_TX_FRAME_LEN\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_TX_FRAME_LEN is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_SET_TX_FRAME_COUNT:
+			{
+				USHORT    value = 0;
+				UCHAR    str[LEN_OF_ARG];
+
+				NdisZeroMemory(str, LEN_OF_ARG);
+
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_ATE_SET_TX_FRAME_COUNT\n"));
+
+				memcpy((PUCHAR)&value, (PUCHAR)&(pRaCfg->status), 2);
+				value = ntohs(value);
+				{
+					sprintf((PCHAR)str, "%d", value);
+					Set_ATE_TX_COUNT_Proc(pAdapter, str);
+				}
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_ATE_SET_TX_FRAME_COUNT\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_ATE_SET_TX_FRAME_COUNT is done !\n"));
+				}
+			}
+			break;
+
+		case RACFG_CMD_ATE_START_RX_FRAME:
+			{
+				ATEDBGPRINT(RT_DEBUG_TRACE,("RACFG_CMD_RX_START\n"));
+
+				Set_ATE_Proc(pAdapter, "RXFRAME");
+
+				// prepare feedback
+				pRaCfg->length = htons(2);
+				pRaCfg->status = htons(0);
+				wrq->u.data.length = sizeof(pRaCfg->magic_no) + sizeof(pRaCfg->command_type)
+									+ sizeof(pRaCfg->command_id) + sizeof(pRaCfg->length)
+									+ sizeof(pRaCfg->sequence) + ntohs(pRaCfg->length);
+
+            	if (copy_to_user(wrq->u.data.pointer, pRaCfg, wrq->u.data.length))
+            	{
+            		ATEDBGPRINT(RT_DEBUG_ERROR, ("copy_to_user() fail in case RACFG_CMD_RX_START\n"));
+                    Status = -EFAULT;
+            	}
+				else
+				{
+                	ATEDBGPRINT(RT_DEBUG_TRACE, ("RACFG_CMD_RX_START is done !\n"));
+				}
+			}
+			break;
+		default:
+			break;
+	}
+    ASSERT(pRaCfg != NULL);
+    if (pRaCfg != NULL)
+    {
+    kfree(pRaCfg);
+    }
+	return;
+}
+
+VOID BubbleSort(INT32 n, INT32 a[])
+{
+	INT32 k, j, temp;
+
+	for (k = n-1;  k>0;  k--)
+	{
+		for (j = 0; j<k; j++)
+		{
+			if(a[j] > a[j+1])
+			{
+				temp = a[j];
+				a[j]=a[j+1];
+				a[j+1]=temp;
+			}
+		}
+	}
+}
+
+VOID CalNoiseLevel(PRTMP_ADAPTER pAd, UCHAR channel, INT32 RSSI[3][10])
+{
+	INT32		RSSI0, RSSI1, RSSI2;
+ 	CHAR		Rssi0Offset, Rssi1Offset, Rssi2Offset;
+	UCHAR		BbpR50Rssi0 = 0, BbpR51Rssi1 = 0, BbpR52Rssi2 = 0;
+	UCHAR		Org_BBP66value = 0, Org_BBP69value = 0, Org_BBP70value = 0, data = 0;
+	USHORT		LNA_Gain = 0;
+	INT32       j = 0;
+	UCHAR		Org_Channel = pAd->ate.Channel;
+	USHORT	    GainValue = 0, OffsetValue = 0;
+
+	ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R66, &Org_BBP66value);
+	ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R69, &Org_BBP69value);
+	ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R70, &Org_BBP70value);
+
+	//**********************************************************************
+	// Read the value of LNA gain and Rssi offset
+	//**********************************************************************
+	RT28xx_EEPROM_READ16(pAd, EEPROM_LNA_OFFSET, GainValue);
+
+	// for Noise Level
+	if (channel <= 14)
+	{
+		LNA_Gain = GainValue & 0x00FF;
+
+		RT28xx_EEPROM_READ16(pAd, EEPROM_RSSI_BG_OFFSET, OffsetValue);
+		Rssi0Offset = OffsetValue & 0x00FF;
+		Rssi1Offset = (OffsetValue & 0xFF00) >> 8;
+		RT28xx_EEPROM_READ16(pAd, (EEPROM_RSSI_BG_OFFSET + 2)/* 0x48 */, OffsetValue);
+		Rssi2Offset = OffsetValue & 0x00FF;
+	}
+	else
+	{
+		LNA_Gain = (GainValue & 0xFF00) >> 8;
+
+		RT28xx_EEPROM_READ16(pAd, EEPROM_RSSI_A_OFFSET, OffsetValue);
+		Rssi0Offset = OffsetValue & 0x00FF;
+		Rssi1Offset = (OffsetValue & 0xFF00) >> 8;
+		RT28xx_EEPROM_READ16(pAd, (EEPROM_RSSI_A_OFFSET + 2)/* 0x4C */, OffsetValue);
+		Rssi2Offset = OffsetValue & 0x00FF;
+	}
+	//**********************************************************************
+	{
+		pAd->ate.Channel = channel;
+		ATEAsicSwitchChannel(pAd);
+		mdelay(5);
+
+		data = 0x10;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, data);
+		data = 0x40;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, data);
+		data = 0x40;
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, data);
+		mdelay(5);
+
+		// Start Rx
+		pAd->ate.bQARxStart = TRUE;
+		Set_ATE_Proc(pAd, "RXFRAME");
+
+		mdelay(5);
+
+		for (j = 0; j < 10; j++)
+		{
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R50, &BbpR50Rssi0);
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R51, &BbpR51Rssi1);
+			ATE_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R52, &BbpR52Rssi2);
+
+			mdelay(10);
+
+			// Calculate RSSI 0
+			if (BbpR50Rssi0 == 0)
+			{
+				RSSI0 = -100;
+			}
+			else
+			{
+				RSSI0 = (INT32)(-12 - BbpR50Rssi0 - LNA_Gain - Rssi0Offset);
+			}
+			RSSI[0][j] = RSSI0;
+
+			if ( pAd->Antenna.field.RxPath >= 2 ) // 2R
+			{
+				// Calculate RSSI 1
+				if (BbpR51Rssi1 == 0)
+				{
+					RSSI1 = -100;
+				}
+				else
+				{
+					RSSI1 = (INT32)(-12 - BbpR51Rssi1 - LNA_Gain - Rssi1Offset);
+				}
+				RSSI[1][j] = RSSI1;
+			}
+
+			if ( pAd->Antenna.field.RxPath >= 3 ) // 3R
+			{
+				// Calculate RSSI 2
+				if (BbpR52Rssi2 == 0)
+					RSSI2 = -100;
+				else
+					RSSI2 = (INT32)(-12 - BbpR52Rssi2 - LNA_Gain - Rssi2Offset);
+
+				RSSI[2][j] = RSSI2;
+			}
+		}
+
+		// Stop Rx
+		Set_ATE_Proc(pAd, "RXSTOP");
+
+		mdelay(5);
+
+#if 0// Debug Message................
+		ate_print("\n**********************************************************\n");
+		ate_print("Noise Level: Channel %d\n", channel);
+		ate_print("RSSI0 = %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n",
+			RSSI[0][0], RSSI[0][1], RSSI[0][2],
+			RSSI[0][3], RSSI[0][4], RSSI[0][5],
+			RSSI[0][6], RSSI[0][7], RSSI[0][8],
+			RSSI[0][9]);
+		if ( pAd->Antenna.field.RxPath >= 2 ) // 2R
+		{
+			ate_print("RSSI1 = %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n",
+				RSSI[1][0], RSSI[1][1], RSSI[1][2],
+				RSSI[1][3], RSSI[1][4], RSSI[1][5],
+				RSSI[1][6], RSSI[1][7], RSSI[1][8],
+				RSSI[1][9]);
+		}
+		if ( pAd->Antenna.field.RxPath >= 3 ) // 3R
+		{
+			ate_print("RSSI2 = %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n",
+				RSSI[2][0], RSSI[2][1], RSSI[2][2],
+				RSSI[2][3], RSSI[2][4], RSSI[2][5],
+				RSSI[2][6], RSSI[2][7], RSSI[2][8],
+				RSSI[2][9]);
+		}
+#endif // 0 //
+		BubbleSort(10, RSSI[0]);	// 1R
+
+		if ( pAd->Antenna.field.RxPath >= 2 ) // 2R
+		{
+			BubbleSort(10, RSSI[1]);
+		}
+
+		if ( pAd->Antenna.field.RxPath >= 3 ) // 3R
+		{
+			BubbleSort(10, RSSI[2]);
+		}
+
+#if 0// Debug Message................
+		ate_print("\nAfter Sorting....Channel %d\n", channel);
+		ate_print("RSSI0 = %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n",
+			RSSI[0][0], RSSI[0][1], RSSI[0][2],
+			RSSI[0][3], RSSI[0][4], RSSI[0][5],
+			RSSI[0][6], RSSI[0][7], RSSI[0][8],
+			RSSI[0][9]);
+		if ( pAd->Antenna.field.RxPath >= 2 ) // 2R
+		{
+			ate_print("RSSI1 = %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n",
+				RSSI[1][0], RSSI[1][1], RSSI[1][2],
+				RSSI[1][3], RSSI[1][4], RSSI[1][5],
+				RSSI[1][6], RSSI[1][7], RSSI[1][8],
+				RSSI[1][9]);
+		}
+		if ( pAd->Antenna.field.RxPath >= 3 ) // 3R
+		{
+			ate_print("RSSI2 = %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n",
+				RSSI[2][0], RSSI[2][1], RSSI[2][2],
+				RSSI[2][3], RSSI[2][4], RSSI[2][5],
+				RSSI[2][6], RSSI[2][7], RSSI[2][8],
+				RSSI[2][9]);
+		}
+		ate_print("**********************************************************\n");
+#endif // 0 //
+	}
+
+	pAd->ate.Channel = Org_Channel;
+	ATEAsicSwitchChannel(pAd);
+
+	// Restore original value
+    ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R66, Org_BBP66value);
+    ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, Org_BBP69value);
+    ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, Org_BBP70value);
+
+	return;
+}
+
+BOOLEAN SyncTxRxConfig(PRTMP_ADAPTER pAd, USHORT offset, UCHAR value)
+{
+	UCHAR tmp = 0, bbp_data = 0;
+
+	if (ATE_ON(pAd))
+	{
+		ATE_BBP_IO_READ8_BY_REG_ID(pAd, offset, &bbp_data);
+	}
+	else
+	{
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, offset, &bbp_data);
+	}
+
+	/* confirm again */
+	ASSERT(bbp_data == value);
+
+	switch(offset)
+	{
+		case BBP_R1:
+			/* Need to sync. tx configuration with legacy ATE. */
+			tmp = (bbp_data & ((1 << 4) | (1 << 3))/* 0x18 */) >> 3;
+		    switch(tmp)
+		    {
+				/* The BBP R1 bit[4:3] = 2 :: Both DACs will be used by QA. */
+		        case 2:
+					/* All */
+					pAd->ate.TxAntennaSel = 0;
+		            break;
+				/* The BBP R1 bit[4:3] = 0 :: DAC 0 will be used by QA. */
+		        case 0:
+					/* Antenna one */
+					pAd->ate.TxAntennaSel = 1;
+		            break;
+				/* The BBP R1 bit[4:3] = 1 :: DAC 1 will be used by QA. */
+		        case 1:
+					/* Antenna two */
+					pAd->ate.TxAntennaSel = 2;
+		            break;
+		        default:
+		            DBGPRINT(RT_DEBUG_TRACE, ("%s -- Sth. wrong!  : return FALSE; \n", __FUNCTION__));
+		            return FALSE;
+		    }
+			break;/* case BBP_R1 */
+
+		case BBP_R3:
+			/* Need to sync. rx configuration with legacy ATE. */
+			tmp = (bbp_data & ((1 << 1) | (1 << 0))/* 0x03 */);
+		    switch(tmp)
+		    {
+				/* The BBP R3 bit[1:0] = 3 :: All ADCs will be used by QA. */
+		        case 3:
+					/* All */
+					pAd->ate.RxAntennaSel = 0;
+		            break;
+				/* The BBP R3 bit[1:0] = 0 :: ADC 0 will be used by QA, */
+				/* unless the BBP R3 bit[4:3] = 2 */
+		        case 0:
+					/* Antenna one */
+					pAd->ate.RxAntennaSel = 1;
+					tmp = ((bbp_data & ((1 << 4) | (1 << 3))/* 0x03 */) >> 3);
+					if (tmp == 2)// 3R
+					{
+						/* Default : All ADCs will be used by QA */
+						pAd->ate.RxAntennaSel = 0;
+					}
+		            break;
+				/* The BBP R3 bit[1:0] = 1 :: ADC 1 will be used by QA. */
+		        case 1:
+					/* Antenna two */
+					pAd->ate.RxAntennaSel = 2;
+		            break;
+				/* The BBP R3 bit[1:0] = 2 :: ADC 2 will be used by QA. */
+		        case 2:
+					/* Antenna three */
+					pAd->ate.RxAntennaSel = 3;
+		            break;
+		        default:
+		            DBGPRINT(RT_DEBUG_ERROR, ("%s -- Impossible!  : return FALSE; \n", __FUNCTION__));
+		            return FALSE;
+		    }
+			break;/* case BBP_R3 */
+
+        default:
+            DBGPRINT(RT_DEBUG_ERROR, ("%s -- Sth. wrong!  : return FALSE; \n", __FUNCTION__));
+            return FALSE;
+
+	}
+	return TRUE;
+}
+
+static VOID memcpy_exl(PRTMP_ADAPTER pAd, UCHAR *dst, UCHAR *src, ULONG len)
+{
+	ULONG i, Value = 0;
+	ULONG *pDst, *pSrc;
+	UCHAR *p8;
+
+	p8 = src;
+	pDst = (ULONG *) dst;
+	pSrc = (ULONG *) src;
+
+	for (i = 0 ; i < (len/4); i++)
+	{
+		/* For alignment issue, we need a variable "Value". */
+		memmove(&Value, pSrc, 4);
+		Value = htonl(Value);
+		memmove(pDst, &Value, 4);
+		pDst++;
+		pSrc++;
+	}
+	if ((len % 4) != 0)
+	{
+		/* wish that it will never reach here */
+		memmove(&Value, pSrc, (len % 4));
+		Value = htonl(Value);
+		memmove(pDst, &Value, (len % 4));
+	}
+}
+
+static VOID memcpy_exs(PRTMP_ADAPTER pAd, UCHAR *dst, UCHAR *src, ULONG len)
+{
+	ULONG i;
+	UCHAR *pDst, *pSrc;
+
+	pDst = dst;
+	pSrc = src;
+
+	for (i = 0; i < (len/2); i++)
+	{
+		memmove(pDst, pSrc, 2);
+		*((USHORT *)pDst) = htons(*((USHORT *)pDst));
+		pDst+=2;
+		pSrc+=2;
+	}
+
+	if ((len % 2) != 0)
+	{
+		memmove(pDst, pSrc, 1);
+	}
+}
+
+static VOID RTMP_IO_READ_BULK(PRTMP_ADAPTER pAd, UCHAR *dst, UCHAR *src, UINT32 len)
+{
+	UINT32 i, Value;
+	UINT32 *pDst, *pSrc;
+
+	pDst = (UINT32 *) dst;
+	pSrc = (UINT32 *) src;
+
+	for (i = 0 ; i < (len/4); i++)
+	{
+		RTMP_IO_READ32(pAd, (ULONG)pSrc, &Value);
+		Value = htonl(Value);
+		memmove(pDst, &Value, 4);
+		pDst++;
+		pSrc++;
+	}
+	return;
+}
+
+// TODO:
+#if 0
+/* These work only when RALINK_ATE is defined */
+INT Set_TxStart_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ULONG value = simple_strtol(arg, 0, 10);
+	UCHAR buffer[26] = {0x88, 0x02, 0x2c, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x11, 0x22, 0x33, 0x44, 0x55, 0x00, 0x55, 0x44, 0x33, 0x22, 0x11, 0xc0, 0x22, 0x00, 0x00};
+	POS_COOKIE pObj;
+
+	if (pAd->ate.TxStatus != 0)
+		return FALSE;
+
+	pAd->ate.TxInfo = 0x04000000;
+	bzero(&pAd->ate.TxWI, sizeof(TXWI_STRUC));
+	pAd->ate.TxWI.PHYMODE = 0;// MODE_CCK
+	pAd->ate.TxWI.MPDUtotalByteCount = 1226;
+	pAd->ate.TxWI.MCS = 3;
+	//pAd->ate.Mode = ATE_START;
+	pAd->ate.Mode |= ATE_TXFRAME;
+	pAd->ate.TxCount = value;
+	pAd->ate.QID = 0;
+	pAd->ate.HLen = 26;
+	pAd->ate.PLen = 0;
+	pAd->ate.DLen = 1200;
+	memcpy(pAd->ate.Header, buffer, 26);
+	pAd->ate.bQATxStart = TRUE;
+	//pObj = (POS_COOKIE) pAd->OS_Cookie;
+	//tasklet_hi_schedule(&pObj->AteTxTask);
+	return TRUE;
+}
+#endif  /* end of #if 0 */
+
+INT Set_TxStop_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ATEDBGPRINT(RT_DEBUG_TRACE,("Set_TxStop_Proc\n"));
+
+	if (Set_ATE_Proc(pAd, "TXSTOP"))
+	{
+	return TRUE;
+}
+	else
+	{
+		return FALSE;
+	}
+}
+
+INT Set_RxStop_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	ATEDBGPRINT(RT_DEBUG_TRACE,("Set_RxStop_Proc\n"));
+
+	if (Set_ATE_Proc(pAd, "RXSTOP"))
+	{
+	return TRUE;
+}
+	else
+	{
+		return FALSE;
+	}
+}
+
+#if 0
+INT Set_EEWrite_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	USHORT offset = 0, value;
+	PUCHAR p2 = arg;
+
+	while((*p2 != ':') && (*p2 != '\0'))
+	{
+		p2++;
+	}
+
+	if (*p2 == ':')
+	{
+		A2Hex(offset, arg);
+		A2Hex(value, p2+ 1);
+	}
+	else
+	{
+		A2Hex(value, arg);
+	}
+
+	if (offset >= EEPROM_SIZE)
+	{
+		ate_print("Offset can not exceed EEPROM_SIZE( == 0x%04x)\n", EEPROM_SIZE);
+		return FALSE;
+	}
+
+	RTMP_EEPROM_WRITE16(pAd, offset, value);
+
+	return TRUE;
+}
+
+INT Set_BBPRead_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UCHAR value = 0, offset;
+
+	A2Hex(offset, arg);
+
+	if (ATE_ON(pAd))
+	{
+		ATE_BBP_IO_READ8_BY_REG_ID(pAd, offset,  &value);
+	}
+	else
+	{
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, offset,  &value);
+	}
+
+	ate_print("%x\n", value);
+
+	return TRUE;
+}
+
+
+INT Set_BBPWrite_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	USHORT offset = 0;
+	PUCHAR p2 = arg;
+	UCHAR value;
+
+	while((*p2 != ':') && (*p2 != '\0'))
+	{
+		p2++;
+	}
+
+	if (*p2 == ':')
+	{
+		A2Hex(offset, arg);
+		A2Hex(value, p2+ 1);
+	}
+	else
+	{
+		A2Hex(value, arg);
+	}
+
+	if (ATE_ON(pAd))
+	{
+		ATE_BBP_IO_WRITE8_BY_REG_ID(pAd, offset,  value);
+	}
+	else
+	{
+		RTNP_BBP_IO_WRITE8_BY_REG_ID(pAd, offset,  value);
+	}
+
+	return TRUE;
+}
+
+INT Set_RFWrite_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	PUCHAR p2, p3, p4;
+	ULONG R1, R2, R3, R4;
+
+	p2 = arg;
+
+	while((*p2 != ':') && (*p2 != '\0'))
+	{
+		p2++;
+	}
+
+	if (*p2 != ':')
+		return FALSE;
+
+	p3 = p2 + 1;
+
+	while((*p3 != ':') && (*p3 != '\0'))
+	{
+		p3++;
+	}
+
+	if (*p3 != ':')
+		return FALSE;
+
+	p4 = p3 + 1;
+
+	while((*p4 != ':') && (*p4 != '\0'))
+	{
+		p4++;
+	}
+
+	if (*p4 != ':')
+		return FALSE;
+
+
+	A2Hex(R1, arg);
+	A2Hex(R2, p2 + 1);
+	A2Hex(R3, p3 + 1);
+	A2Hex(R4, p4 + 1);
+
+	RTMP_RF_IO_WRITE32(pAd, R1);
+	RTMP_RF_IO_WRITE32(pAd, R2);
+	RTMP_RF_IO_WRITE32(pAd, R3);
+	RTMP_RF_IO_WRITE32(pAd, R4);
+
+	return TRUE;
+}
+#endif  // end of #if 0 //
+#endif	// RALINK_28xx_QA //
+
+#endif	// RALINK_ATE //
+
diff --git a/drivers/staging/rt2870/rt_ate.h b/drivers/staging/rt2870/rt_ate.h
new file mode 100644
index 00000000000..b618ce3599a
--- /dev/null
+++ b/drivers/staging/rt2870/rt_ate.h
@@ -0,0 +1,315 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#ifndef __ATE_H__
+#define __ATE_H__
+
+#ifndef UCOS
+#define ate_print printk
+#define ATEDBGPRINT DBGPRINT
+
+#ifdef RT2870
+#define EEPROM_SIZE								0x400
+#ifdef CONFIG_STA_SUPPORT
+#define EEPROM_BIN_FILE_NAME  "/etc/Wireless/RT2870STA/e2p.bin"
+#endif // CONFIG_STA_SUPPORT //
+#endif // RT2870 //
+#else // !UCOS //
+#define fATE_LOAD_EEPROM						0x0C43
+#ifdef CONFIG_PRINTK
+extern INT ConsoleResponse(IN PUCHAR buff);
+extern int (*remote_display)(char *);
+extern void puts (const char *s);
+
+/* specificly defined to redirect and show ate-related messages to host. */
+/* Try to define ate_print as a macro. */
+#define ate_print(fmt, args...)                 \
+do{   int (*org_remote_display)(char *) = NULL;   \
+	org_remote_display = remote_display;\
+	/* Save original "remote_display" */\
+	remote_display = (int (*)(char *))ConsoleResponse;           \
+	printk(fmt, ## args);                       \
+	/* Restore the remote_display function pointer */        \
+	remote_display = org_remote_display; }while(0)
+
+#define ATEDBGPRINT(Level, Fmt)    	\
+{                                   \
+    if ((Level) <= RTDebugLevel)      \
+    {                               \
+        ate_print Fmt;					\
+    }                               \
+}
+#endif // CONFIG_PRINTK //
+#endif // !UCOS //
+
+#define ATE_ON(_p)              (((_p)->ate.Mode) != ATE_STOP)
+
+/* RT2880_iNIC will define "RT2860". */
+
+/* RT2880_iNIC will define RT2860. */
+
+#ifdef RT2870
+#define EEPROM_SIZE								0x400
+#ifdef CONFIG_STA_SUPPORT
+#define EEPROM_BIN_FILE_NAME  "/etc/Wireless/RT2870STA/e2p.bin"
+#endif // CONFIG_STA_SUPPORT //
+#endif // RT2870 //
+
+#ifdef RT2870
+#define ATE_BBP_IO_READ8_BY_REG_ID(_A, _I, _pV)    RTMP_BBP_IO_READ8_BY_REG_ID(_A, _I, _pV)
+#define ATE_BBP_IO_WRITE8_BY_REG_ID(_A, _I, _V)    RTMP_BBP_IO_WRITE8_BY_REG_ID(_A, _I, _V)
+
+#define BULK_OUT_LOCK(pLock, IrqFlags)	\
+		if(1 /*!(in_interrupt() & 0xffff0000)*/)	\
+			RTMP_IRQ_LOCK((pLock), IrqFlags);
+
+#define BULK_OUT_UNLOCK(pLock, IrqFlags)	\
+		if(1 /*!(in_interrupt() & 0xffff0000)*/)	\
+			RTMP_IRQ_UNLOCK((pLock), IrqFlags);
+
+// Prototypes of completion funuc.
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
+#define ATE_RTUSBBulkOutDataPacketComplete(purb, pt_regs)    ATE_RTUSBBulkOutDataPacketComplete(purb)
+#endif
+
+VOID ATE_RTUSBBulkOutDataPacketComplete(
+	IN purbb_t purb,
+	OUT struct pt_regs *pt_regs);
+
+VOID ATE_RTUSBBulkOutDataPacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			BulkOutPipeId);
+
+VOID ATE_RTUSBCancelPendingBulkInIRP(
+	IN	PRTMP_ADAPTER	pAd);
+#endif // RT2870 //
+
+VOID rt_ee_read_all(
+	IN  PRTMP_ADAPTER   pAd,
+	OUT USHORT *Data);
+
+
+VOID rt_ee_write_all(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  USHORT *Data);
+
+INT Set_ATE_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_DA_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_SA_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_BSSID_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_CHANNEL_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_POWER0_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_POWER1_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_Antenna_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_RX_Antenna_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_FREQOFFSET_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_BW_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_LENGTH_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_COUNT_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_MCS_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_MODE_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_TX_GI_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+
+INT	Set_ATE_RX_FER_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_ATE_Read_RF_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_ATE_Write_RF1_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_ATE_Write_RF2_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_ATE_Write_RF3_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_ATE_Write_RF4_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_ATE_Load_E2P_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_ATE_Read_E2P_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_Show_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ATE_Help_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+#ifdef RALINK_ATE
+#ifdef RALINK_28xx_QA
+VOID ATE_QA_Statistics(
+	IN PRTMP_ADAPTER		pAd,
+	IN PRXWI_STRUC			pRxWI,
+	IN PRT28XX_RXD_STRUC    p28xxRxD,
+	IN PHEADER_802_11		pHeader);
+
+VOID RtmpDoAte(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq);
+
+VOID BubbleSort(
+	IN  INT32 n,
+	IN  INT32 a[]);
+
+VOID CalNoiseLevel(
+	IN  PRTMP_ADAPTER   pAdapter,
+	IN  UCHAR           channel,
+	OUT INT32           buffer[3][10]);
+
+BOOLEAN SyncTxRxConfig(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	USHORT			offset,
+	IN	UCHAR			value);
+
+#if 0
+INT Set_TxStart_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif  // 0 //
+
+INT Set_TxStop_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_RxStop_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+#if 0
+INT Set_EERead_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_EEWrite_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_BBPRead_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_BBPWrite_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_RFWrite_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif // end of #if 0 //
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+
+VOID ATEAsicSwitchChannel(
+	IN PRTMP_ADAPTER pAd);
+
+VOID ATEAsicAdjustTxPower(
+	IN PRTMP_ADAPTER pAd);
+
+VOID ATEDisableAsicProtect(
+	IN		PRTMP_ADAPTER	pAd);
+
+CHAR ATEConvertToRssi(
+	IN PRTMP_ADAPTER  pAd,
+	IN CHAR				Rssi,
+	IN UCHAR    RssiNumber);
+
+VOID ATESampleRssi(
+	IN PRTMP_ADAPTER	pAd,
+	IN PRXWI_STRUC		pRxWI);
+
+
+#ifdef CONFIG_STA_SUPPORT
+VOID RTMPStationStop(
+    IN  PRTMP_ADAPTER   pAd);
+
+VOID RTMPStationStart(
+    IN  PRTMP_ADAPTER   pAd);
+#endif // CONFIG_STA_SUPPORT //
+#endif // __ATE_H__ //
diff --git a/drivers/staging/rt2870/rt_config.h b/drivers/staging/rt2870/rt_config.h
new file mode 100644
index 00000000000..e3fe2642633
--- /dev/null
+++ b/drivers/staging/rt2870/rt_config.h
@@ -0,0 +1,104 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rt_config.h
+
+	Abstract:
+	Central header file to maintain all include files for all NDIS
+	miniport driver routines.
+
+	Revision History:
+	Who         When          What
+	--------    ----------    ----------------------------------------------
+	Paul Lin    08-01-2002    created
+
+*/
+#ifndef	__RT_CONFIG_H__
+#define	__RT_CONFIG_H__
+
+#include    "rtmp_type.h"
+#ifdef UCOS
+#include "includes.h"
+#include <stdio.h>
+#include 	"rt_ucos.h"
+#endif
+
+#ifdef LINUX
+#include	"rt_linux.h"
+#endif
+#include    "rtmp_def.h"
+#include    "rt28xx.h"
+
+
+#ifdef RT2870
+#include	"rt2870.h"
+#endif // RT2870 //
+
+#include    "oid.h"
+#include    "mlme.h"
+#include    "wpa.h"
+#include    "md5.h"
+#include    "rtmp.h"
+#include	"ap.h"
+#include	"dfs.h"
+#include	"chlist.h"
+#include	"spectrum.h"
+
+
+#ifdef LEAP_SUPPORT
+#include    "leap.h"
+#endif // LEAP_SUPPORT //
+
+#ifdef BLOCK_NET_IF
+#include "netif_block.h"
+#endif // BLOCK_NET_IF //
+
+#ifdef IGMP_SNOOP_SUPPORT
+#include "igmp_snoop.h"
+#endif // IGMP_SNOOP_SUPPORT //
+
+#ifdef RALINK_ATE
+#include "rt_ate.h"
+#endif // RALINK_ATE //
+
+
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+#ifndef WPA_SUPPLICANT_SUPPORT
+#error "Build for being controlled by NetworkManager or wext, please set HAS_WPA_SUPPLICANT=y and HAS_NATIVE_WPA_SUPPLICANT_SUPPORT=y"
+#endif // WPA_SUPPLICANT_SUPPORT //
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef IKANOS_VX_1X0
+#include	"vr_ikans.h"
+#endif // IKANOS_VX_1X0 //
+
+#endif	// __RT_CONFIG_H__
+
diff --git a/drivers/staging/rt2870/rt_linux.c b/drivers/staging/rt2870/rt_linux.c
new file mode 100644
index 00000000000..f2ea8ebcd04
--- /dev/null
+++ b/drivers/staging/rt2870/rt_linux.c
@@ -0,0 +1,1095 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#include "rt_config.h"
+
+ULONG	RTDebugLevel = RT_DEBUG_ERROR;
+
+BUILD_TIMER_FUNCTION(MlmePeriodicExec);
+//BUILD_TIMER_FUNCTION(MlmeRssiReportExec);
+BUILD_TIMER_FUNCTION(AsicRxAntEvalTimeout);
+BUILD_TIMER_FUNCTION(APSDPeriodicExec);
+BUILD_TIMER_FUNCTION(AsicRfTuningExec);
+#ifdef RT2870
+BUILD_TIMER_FUNCTION(BeaconUpdateExec);
+#endif // RT2870 //
+
+
+#ifdef CONFIG_STA_SUPPORT
+BUILD_TIMER_FUNCTION(BeaconTimeout);
+BUILD_TIMER_FUNCTION(ScanTimeout);
+BUILD_TIMER_FUNCTION(AuthTimeout);
+BUILD_TIMER_FUNCTION(AssocTimeout);
+BUILD_TIMER_FUNCTION(ReassocTimeout);
+BUILD_TIMER_FUNCTION(DisassocTimeout);
+BUILD_TIMER_FUNCTION(LinkDownExec);
+#ifdef LEAP_SUPPORT
+BUILD_TIMER_FUNCTION(LeapAuthTimeout);
+#endif
+BUILD_TIMER_FUNCTION(StaQuickResponeForRateUpExec);
+BUILD_TIMER_FUNCTION(WpaDisassocApAndBlockAssoc);
+#ifdef QOS_DLS_SUPPORT
+BUILD_TIMER_FUNCTION(DlsTimeoutAction);
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+// for wireless system event message
+char const *pWirelessSysEventText[IW_SYS_EVENT_TYPE_NUM] = {
+	// system status event
+    "had associated successfully",							/* IW_ASSOC_EVENT_FLAG */
+    "had disassociated",									/* IW_DISASSOC_EVENT_FLAG */
+    "had deauthenticated",									/* IW_DEAUTH_EVENT_FLAG */
+    "had been aged-out and disassociated",					/* IW_AGEOUT_EVENT_FLAG */
+    "occurred CounterMeasures attack",						/* IW_COUNTER_MEASURES_EVENT_FLAG */
+    "occurred replay counter different in Key Handshaking",	/* IW_REPLAY_COUNTER_DIFF_EVENT_FLAG */
+    "occurred RSNIE different in Key Handshaking",			/* IW_RSNIE_DIFF_EVENT_FLAG */
+    "occurred MIC different in Key Handshaking",			/* IW_MIC_DIFF_EVENT_FLAG */
+    "occurred ICV error in RX",								/* IW_ICV_ERROR_EVENT_FLAG */
+    "occurred MIC error in RX",								/* IW_MIC_ERROR_EVENT_FLAG */
+	"Group Key Handshaking timeout",						/* IW_GROUP_HS_TIMEOUT_EVENT_FLAG */
+	"Pairwise Key Handshaking timeout",						/* IW_PAIRWISE_HS_TIMEOUT_EVENT_FLAG */
+	"RSN IE sanity check failure",							/* IW_RSNIE_SANITY_FAIL_EVENT_FLAG */
+	"set key done in WPA/WPAPSK",							/* IW_SET_KEY_DONE_WPA1_EVENT_FLAG */
+	"set key done in WPA2/WPA2PSK",                         /* IW_SET_KEY_DONE_WPA2_EVENT_FLAG */
+	"connects with our wireless client",                    /* IW_STA_LINKUP_EVENT_FLAG */
+	"disconnects with our wireless client",                 /* IW_STA_LINKDOWN_EVENT_FLAG */
+	"scan completed"										/* IW_SCAN_COMPLETED_EVENT_FLAG */
+	"scan terminate!! Busy!! Enqueue fail!!"				/* IW_SCAN_ENQUEUE_FAIL_EVENT_FLAG */
+	};
+
+// for wireless IDS_spoof_attack event message
+char const *pWirelessSpoofEventText[IW_SPOOF_EVENT_TYPE_NUM] = {
+    "detected conflict SSID",								/* IW_CONFLICT_SSID_EVENT_FLAG */
+    "detected spoofed association response",				/* IW_SPOOF_ASSOC_RESP_EVENT_FLAG */
+    "detected spoofed reassociation responses",				/* IW_SPOOF_REASSOC_RESP_EVENT_FLAG */
+    "detected spoofed probe response",						/* IW_SPOOF_PROBE_RESP_EVENT_FLAG */
+    "detected spoofed beacon",								/* IW_SPOOF_BEACON_EVENT_FLAG */
+    "detected spoofed disassociation",						/* IW_SPOOF_DISASSOC_EVENT_FLAG */
+    "detected spoofed authentication",						/* IW_SPOOF_AUTH_EVENT_FLAG */
+    "detected spoofed deauthentication",					/* IW_SPOOF_DEAUTH_EVENT_FLAG */
+    "detected spoofed unknown management frame",			/* IW_SPOOF_UNKNOWN_MGMT_EVENT_FLAG */
+	"detected replay attack"								/* IW_REPLAY_ATTACK_EVENT_FLAG */
+	};
+
+// for wireless IDS_flooding_attack event message
+char const *pWirelessFloodEventText[IW_FLOOD_EVENT_TYPE_NUM] = {
+	"detected authentication flooding",						/* IW_FLOOD_AUTH_EVENT_FLAG */
+    "detected association request flooding",				/* IW_FLOOD_ASSOC_REQ_EVENT_FLAG */
+    "detected reassociation request flooding",				/* IW_FLOOD_REASSOC_REQ_EVENT_FLAG */
+    "detected probe request flooding",						/* IW_FLOOD_PROBE_REQ_EVENT_FLAG */
+    "detected disassociation flooding",						/* IW_FLOOD_DISASSOC_EVENT_FLAG */
+    "detected deauthentication flooding",					/* IW_FLOOD_DEAUTH_EVENT_FLAG */
+    "detected 802.1x eap-request flooding"					/* IW_FLOOD_EAP_REQ_EVENT_FLAG */
+	};
+
+/* timeout -- ms */
+VOID RTMP_SetPeriodicTimer(
+	IN	NDIS_MINIPORT_TIMER *pTimer,
+	IN	unsigned long timeout)
+{
+	timeout = ((timeout*HZ) / 1000);
+	pTimer->expires = jiffies + timeout;
+	add_timer(pTimer);
+}
+
+/* convert NdisMInitializeTimer --> RTMP_OS_Init_Timer */
+VOID RTMP_OS_Init_Timer(
+	IN	PRTMP_ADAPTER pAd,
+	IN	NDIS_MINIPORT_TIMER *pTimer,
+	IN	TIMER_FUNCTION function,
+	IN	PVOID data)
+{
+	init_timer(pTimer);
+    pTimer->data = (unsigned long)data;
+    pTimer->function = function;
+}
+
+
+VOID RTMP_OS_Add_Timer(
+	IN	NDIS_MINIPORT_TIMER		*pTimer,
+	IN	unsigned long timeout)
+{
+	if (timer_pending(pTimer))
+		return;
+
+	timeout = ((timeout*HZ) / 1000);
+	pTimer->expires = jiffies + timeout;
+	add_timer(pTimer);
+}
+
+VOID RTMP_OS_Mod_Timer(
+	IN	NDIS_MINIPORT_TIMER		*pTimer,
+	IN	unsigned long timeout)
+{
+	timeout = ((timeout*HZ) / 1000);
+	mod_timer(pTimer, jiffies + timeout);
+}
+
+VOID RTMP_OS_Del_Timer(
+	IN	NDIS_MINIPORT_TIMER		*pTimer,
+	OUT	BOOLEAN					*pCancelled)
+{
+	if (timer_pending(pTimer))
+	{
+		*pCancelled = del_timer_sync(pTimer);
+	}
+	else
+	{
+		*pCancelled = TRUE;
+	}
+
+}
+
+VOID RTMP_OS_Release_Packet(
+	IN	PRTMP_ADAPTER pAd,
+	IN	PQUEUE_ENTRY  pEntry)
+{
+	//RTMPFreeNdisPacket(pAd, (struct sk_buff *)pEntry);
+}
+
+// Unify all delay routine by using udelay
+VOID RTMPusecDelay(
+	IN	ULONG	usec)
+{
+	ULONG	i;
+
+	for (i = 0; i < (usec / 50); i++)
+		udelay(50);
+
+	if (usec % 50)
+		udelay(usec % 50);
+}
+
+void RTMP_GetCurrentSystemTime(LARGE_INTEGER *time)
+{
+	time->u.LowPart = jiffies;
+}
+
+// pAd MUST allow to be NULL
+NDIS_STATUS os_alloc_mem(
+	IN	PRTMP_ADAPTER pAd,
+	OUT	PUCHAR *mem,
+	IN	ULONG  size)
+{
+	*mem = (PUCHAR) kmalloc(size, GFP_ATOMIC);
+	if (*mem)
+		return (NDIS_STATUS_SUCCESS);
+	else
+		return (NDIS_STATUS_FAILURE);
+}
+
+// pAd MUST allow to be NULL
+NDIS_STATUS os_free_mem(
+	IN	PRTMP_ADAPTER pAd,
+	IN	PUCHAR mem)
+{
+
+	ASSERT(mem);
+	kfree(mem);
+	return (NDIS_STATUS_SUCCESS);
+}
+
+
+PNDIS_PACKET RTMP_AllocateFragPacketBuffer(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length)
+{
+	struct sk_buff *pkt;
+
+	pkt = dev_alloc_skb(Length);
+
+	if (pkt == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("can't allocate frag rx %ld size packet\n",Length));
+	}
+
+	if (pkt)
+	{
+		RTMP_SET_PACKET_SOURCE(OSPKT_TO_RTPKT(pkt), PKTSRC_NDIS);
+	}
+
+	return (PNDIS_PACKET) pkt;
+}
+
+
+PNDIS_PACKET RTMP_AllocateTxPacketBuffer(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress)
+{
+	struct sk_buff *pkt;
+
+	pkt = dev_alloc_skb(Length);
+
+	if (pkt == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("can't allocate tx %ld size packet\n",Length));
+	}
+
+	if (pkt)
+	{
+		RTMP_SET_PACKET_SOURCE(OSPKT_TO_RTPKT(pkt), PKTSRC_NDIS);
+		*VirtualAddress = (PVOID) pkt->data;
+	}
+	else
+	{
+		*VirtualAddress = (PVOID) NULL;
+	}
+
+	return (PNDIS_PACKET) pkt;
+}
+
+
+VOID build_tx_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	PUCHAR	pFrame,
+	IN	ULONG	FrameLen)
+{
+
+	struct sk_buff	*pTxPkt;
+
+	ASSERT(pPacket);
+	pTxPkt = RTPKT_TO_OSPKT(pPacket);
+
+	NdisMoveMemory(skb_put(pTxPkt, FrameLen), pFrame, FrameLen);
+}
+
+VOID	RTMPFreeAdapter(
+	IN	PRTMP_ADAPTER	pAd)
+{
+    POS_COOKIE os_cookie;
+	int index;
+
+	os_cookie=(POS_COOKIE)pAd->OS_Cookie;
+
+	kfree(pAd->BeaconBuf);
+
+
+	NdisFreeSpinLock(&pAd->MgmtRingLock);
+
+
+	for (index =0 ; index < NUM_OF_TX_RING; index++)
+	{
+    	NdisFreeSpinLock(&pAd->TxSwQueueLock[index]);
+		NdisFreeSpinLock(&pAd->DeQueueLock[index]);
+		pAd->DeQueueRunning[index] = FALSE;
+	}
+
+	NdisFreeSpinLock(&pAd->irq_lock);
+
+
+	vfree(pAd); // pci_free_consistent(os_cookie->pci_dev,sizeof(RTMP_ADAPTER),pAd,os_cookie->pAd_pa);
+	kfree(os_cookie);
+}
+
+BOOLEAN OS_Need_Clone_Packet(void)
+{
+	return (FALSE);
+}
+
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		clone an input NDIS PACKET to another one. The new internally created NDIS PACKET
+		must have only one NDIS BUFFER
+		return - byte copied. 0 means can't create NDIS PACKET
+		NOTE: internally created NDIS_PACKET should be destroyed by RTMPFreeNdisPacket
+
+	Arguments:
+		pAd 	Pointer to our adapter
+		pInsAMSDUHdr	EWC A-MSDU format has extra 14-bytes header. if TRUE, insert this 14-byte hdr in front of MSDU.
+		*pSrcTotalLen			return total packet length. This lenght is calculated with 802.3 format packet.
+
+	Return Value:
+		NDIS_STATUS_SUCCESS
+		NDIS_STATUS_FAILURE
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS RTMPCloneNdisPacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	BOOLEAN			pInsAMSDUHdr,
+	IN	PNDIS_PACKET	pInPacket,
+	OUT PNDIS_PACKET   *ppOutPacket)
+{
+
+	struct sk_buff *pkt;
+
+	ASSERT(pInPacket);
+	ASSERT(ppOutPacket);
+
+	// 1. Allocate a packet
+	pkt = dev_alloc_skb(2048);
+
+	if (pkt == NULL)
+	{
+		return NDIS_STATUS_FAILURE;
+	}
+
+ 	skb_put(pkt, GET_OS_PKT_LEN(pInPacket));
+	NdisMoveMemory(pkt->data, GET_OS_PKT_DATAPTR(pInPacket), GET_OS_PKT_LEN(pInPacket));
+	*ppOutPacket = OSPKT_TO_RTPKT(pkt);
+
+
+	RTMP_SET_PACKET_SOURCE(OSPKT_TO_RTPKT(pkt), PKTSRC_NDIS);
+
+	printk("###Clone###\n");
+
+	return NDIS_STATUS_SUCCESS;
+}
+
+
+// the allocated NDIS PACKET must be freed via RTMPFreeNdisPacket()
+NDIS_STATUS RTMPAllocateNdisPacket(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT PNDIS_PACKET   *ppPacket,
+	IN	PUCHAR			pHeader,
+	IN	UINT			HeaderLen,
+	IN	PUCHAR			pData,
+	IN	UINT			DataLen)
+{
+	PNDIS_PACKET	pPacket;
+	ASSERT(pData);
+	ASSERT(DataLen);
+
+	// 1. Allocate a packet
+	pPacket = (PNDIS_PACKET *) dev_alloc_skb(HeaderLen + DataLen + TXPADDING_SIZE);
+	if (pPacket == NULL)
+ 	{
+		*ppPacket = NULL;
+#ifdef DEBUG
+		printk("RTMPAllocateNdisPacket Fail\n\n");
+#endif
+		return NDIS_STATUS_FAILURE;
+	}
+
+	// 2. clone the frame content
+	if (HeaderLen > 0)
+		NdisMoveMemory(GET_OS_PKT_DATAPTR(pPacket), pHeader, HeaderLen);
+	if (DataLen > 0)
+		NdisMoveMemory(GET_OS_PKT_DATAPTR(pPacket) + HeaderLen, pData, DataLen);
+
+	// 3. update length of packet
+ 	skb_put(GET_OS_PKT_TYPE(pPacket), HeaderLen+DataLen);
+
+	RTMP_SET_PACKET_SOURCE(pPacket, PKTSRC_NDIS);
+//	printk("%s : pPacket = %p, len = %d\n", __FUNCTION__, pPacket, GET_OS_PKT_LEN(pPacket));
+	*ppPacket = pPacket;
+	return NDIS_STATUS_SUCCESS;
+}
+
+/*
+  ========================================================================
+  Description:
+	This routine frees a miniport internally allocated NDIS_PACKET and its
+	corresponding NDIS_BUFFER and allocated memory.
+  ========================================================================
+*/
+VOID RTMPFreeNdisPacket(
+	IN PRTMP_ADAPTER pAd,
+	IN PNDIS_PACKET  pPacket)
+{
+	dev_kfree_skb_any(RTPKT_TO_OSPKT(pPacket));
+}
+
+
+// IRQL = DISPATCH_LEVEL
+// NOTE: we do have an assumption here, that Byte0 and Byte1 always reasid at the same
+//			 scatter gather buffer
+NDIS_STATUS Sniff2BytesFromNdisBuffer(
+	IN	PNDIS_BUFFER	pFirstBuffer,
+	IN	UCHAR			DesiredOffset,
+	OUT PUCHAR			pByte0,
+	OUT PUCHAR			pByte1)
+{
+    *pByte0 = *(PUCHAR)(pFirstBuffer + DesiredOffset);
+    *pByte1 = *(PUCHAR)(pFirstBuffer + DesiredOffset + 1);
+
+	return NDIS_STATUS_SUCCESS;
+}
+
+
+void RTMP_QueryPacketInfo(
+	IN  PNDIS_PACKET pPacket,
+	OUT PACKET_INFO  *pPacketInfo,
+	OUT PUCHAR		 *pSrcBufVA,
+	OUT	UINT		 *pSrcBufLen)
+{
+	pPacketInfo->BufferCount = 1;
+	pPacketInfo->pFirstBuffer = GET_OS_PKT_DATAPTR(pPacket);
+	pPacketInfo->PhysicalBufferCount = 1;
+	pPacketInfo->TotalPacketLength = GET_OS_PKT_LEN(pPacket);
+
+	*pSrcBufVA = GET_OS_PKT_DATAPTR(pPacket);
+	*pSrcBufLen = GET_OS_PKT_LEN(pPacket);
+}
+
+void RTMP_QueryNextPacketInfo(
+	IN  PNDIS_PACKET *ppPacket,
+	OUT PACKET_INFO  *pPacketInfo,
+	OUT PUCHAR		 *pSrcBufVA,
+	OUT	UINT		 *pSrcBufLen)
+{
+	PNDIS_PACKET pPacket = NULL;
+
+	if (*ppPacket)
+		pPacket = GET_OS_PKT_NEXT(*ppPacket);
+
+	if (pPacket)
+	{
+		pPacketInfo->BufferCount = 1;
+		pPacketInfo->pFirstBuffer = GET_OS_PKT_DATAPTR(pPacket);
+		pPacketInfo->PhysicalBufferCount = 1;
+		pPacketInfo->TotalPacketLength = GET_OS_PKT_LEN(pPacket);
+
+		*pSrcBufVA = GET_OS_PKT_DATAPTR(pPacket);
+		*pSrcBufLen = GET_OS_PKT_LEN(pPacket);
+		*ppPacket = GET_OS_PKT_NEXT(pPacket);
+	}
+	else
+	{
+		pPacketInfo->BufferCount = 0;
+		pPacketInfo->pFirstBuffer = NULL;
+		pPacketInfo->PhysicalBufferCount = 0;
+		pPacketInfo->TotalPacketLength = 0;
+
+		*pSrcBufVA = NULL;
+		*pSrcBufLen = 0;
+		*ppPacket = NULL;
+	}
+}
+
+// not yet support MBSS
+PNET_DEV get_netdev_from_bssid(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			FromWhichBSSID)
+{
+    PNET_DEV dev_p = NULL;
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		dev_p = pAd->net_dev;
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	ASSERT(dev_p);
+	return dev_p; /* return one of MBSS */
+}
+
+PNDIS_PACKET DuplicatePacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	UCHAR			FromWhichBSSID)
+{
+	struct sk_buff	*skb;
+	PNDIS_PACKET	pRetPacket = NULL;
+	USHORT			DataSize;
+	UCHAR			*pData;
+
+	DataSize = (USHORT) GET_OS_PKT_LEN(pPacket);
+	pData = (PUCHAR) GET_OS_PKT_DATAPTR(pPacket);
+
+
+	skb = skb_clone(RTPKT_TO_OSPKT(pPacket), MEM_ALLOC_FLAG);
+	if (skb)
+	{
+		skb->dev = get_netdev_from_bssid(pAd, FromWhichBSSID);
+		pRetPacket = OSPKT_TO_RTPKT(skb);
+	}
+
+#if 0
+	if ((skb = __dev_alloc_skb(DataSize + 2+32, MEM_ALLOC_FLAG)) != NULL)
+	{
+		skb_reserve(skb, 2+32);
+		NdisMoveMemory(skb->tail, pData, DataSize);
+		skb_put(skb, DataSize);
+		skb->dev = get_netdev_from_bssid(pAd, FromWhichBSSID);
+		pRetPacket = OSPKT_TO_RTPKT(skb);
+	}
+#endif
+
+	return pRetPacket;
+
+}
+
+PNDIS_PACKET duplicate_pkt(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pHeader802_3,
+    IN  UINT            HdrLen,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize,
+	IN	UCHAR			FromWhichBSSID)
+{
+	struct sk_buff	*skb;
+	PNDIS_PACKET	pPacket = NULL;
+
+
+	if ((skb = __dev_alloc_skb(HdrLen + DataSize + 2, MEM_ALLOC_FLAG)) != NULL)
+	{
+		skb_reserve(skb, 2);
+		NdisMoveMemory(skb->tail, pHeader802_3, HdrLen);
+		skb_put(skb, HdrLen);
+		NdisMoveMemory(skb->tail, pData, DataSize);
+		skb_put(skb, DataSize);
+		skb->dev = get_netdev_from_bssid(pAd, FromWhichBSSID);
+		pPacket = OSPKT_TO_RTPKT(skb);
+	}
+
+	return pPacket;
+}
+
+
+#define TKIP_TX_MIC_SIZE		8
+PNDIS_PACKET duplicate_pkt_with_TKIP_MIC(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket)
+{
+	struct sk_buff	*skb, *newskb;
+
+
+	skb = RTPKT_TO_OSPKT(pPacket);
+	if (skb_tailroom(skb) < TKIP_TX_MIC_SIZE)
+	{
+		// alloc a new skb and copy the packet
+		newskb = skb_copy_expand(skb, skb_headroom(skb), TKIP_TX_MIC_SIZE, GFP_ATOMIC);
+		dev_kfree_skb_any(skb);
+		if (newskb == NULL)
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("Extend Tx.MIC for packet failed!, dropping packet!\n"));
+			return NULL;
+		}
+		skb = newskb;
+	}
+
+	return OSPKT_TO_RTPKT(skb);
+
+#if 0
+	if ((data = skb_put(skb, TKIP_TX_MIC_SIZE)) != NULL)
+	{	// If we can extend it, well, copy it first.
+		NdisMoveMemory(data, pAd->PrivateInfo.Tx.MIC, TKIP_TX_MIC_SIZE);
+	}
+	else
+	{
+		// Otherwise, copy the packet.
+		newskb = skb_copy_expand(skb, skb_headroom(skb), TKIP_TX_MIC_SIZE, GFP_ATOMIC);
+		dev_kfree_skb_any(skb);
+		if (newskb == NULL)
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("Extend Tx.MIC to packet failed!, dropping packet\n"));
+			return NULL;
+		}
+		skb = newskb;
+
+		NdisMoveMemory(skb->tail, pAd->PrivateInfo.Tx.MIC, TKIP_TX_MIC_SIZE);
+		skb_put(skb, TKIP_TX_MIC_SIZE);
+	}
+
+	return OSPKT_TO_RTPKT(skb);
+#endif
+
+}
+
+
+
+
+PNDIS_PACKET ClonePacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize)
+{
+	struct sk_buff	*pRxPkt;
+	struct sk_buff	*pClonedPkt;
+
+	ASSERT(pPacket);
+	pRxPkt = RTPKT_TO_OSPKT(pPacket);
+
+	// clone the packet
+	pClonedPkt = skb_clone(pRxPkt, MEM_ALLOC_FLAG);
+
+	if (pClonedPkt)
+	{
+    	// set the correct dataptr and data len
+    	pClonedPkt->dev = pRxPkt->dev;
+    	pClonedPkt->data = pData;
+    	pClonedPkt->len = DataSize;
+    	pClonedPkt->tail = pClonedPkt->data + pClonedPkt->len;
+		ASSERT(DataSize < 1530);
+	}
+	return pClonedPkt;
+}
+
+//
+// change OS packet DataPtr and DataLen
+//
+void  update_os_packet_info(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN  UCHAR			FromWhichBSSID)
+{
+	struct sk_buff	*pOSPkt;
+
+	ASSERT(pRxBlk->pRxPacket);
+	pOSPkt = RTPKT_TO_OSPKT(pRxBlk->pRxPacket);
+
+	pOSPkt->dev = get_netdev_from_bssid(pAd, FromWhichBSSID);
+	pOSPkt->data = pRxBlk->pData;
+	pOSPkt->len = pRxBlk->DataSize;
+	pOSPkt->tail = pOSPkt->data + pOSPkt->len;
+}
+
+
+void wlan_802_11_to_802_3_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	PUCHAR			pHeader802_3,
+	IN  UCHAR			FromWhichBSSID)
+{
+	struct sk_buff	*pOSPkt;
+
+	ASSERT(pRxBlk->pRxPacket);
+	ASSERT(pHeader802_3);
+
+	pOSPkt = RTPKT_TO_OSPKT(pRxBlk->pRxPacket);
+
+	pOSPkt->dev = get_netdev_from_bssid(pAd, FromWhichBSSID);
+	pOSPkt->data = pRxBlk->pData;
+	pOSPkt->len = pRxBlk->DataSize;
+	pOSPkt->tail = pOSPkt->data + pOSPkt->len;
+
+	//
+	// copy 802.3 header
+	//
+	//
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		NdisMoveMemory(skb_push(pOSPkt, LENGTH_802_3), pHeader802_3, LENGTH_802_3);
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+
+
+void announce_802_3_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket)
+{
+
+	struct sk_buff	*pRxPkt;
+
+	ASSERT(pPacket);
+
+	pRxPkt = RTPKT_TO_OSPKT(pPacket);
+
+#ifdef CONFIG_STA_SUPPORT
+#endif // CONFIG_STA_SUPPORT //
+
+    /* Push up the protocol stack */
+#ifdef IKANOS_VX_1X0
+	IKANOS_DataFrameRx(pAd, pRxPkt->dev, pRxPkt, pRxPkt->len);
+#else
+	pRxPkt->protocol = eth_type_trans(pRxPkt, pRxPkt->dev);
+
+//#ifdef CONFIG_5VT_ENHANCE
+//	*(int*)(pRxPkt->cb) = BRIDGE_TAG;
+//#endif
+	netif_rx(pRxPkt);
+#endif // IKANOS_VX_1X0 //
+}
+
+
+PRTMP_SCATTER_GATHER_LIST
+rt_get_sg_list_from_packet(PNDIS_PACKET pPacket, RTMP_SCATTER_GATHER_LIST *sg)
+{
+	sg->NumberOfElements = 1;
+	sg->Elements[0].Address =  GET_OS_PKT_DATAPTR(pPacket);
+	sg->Elements[0].Length = GET_OS_PKT_LEN(pPacket);
+	return (sg);
+}
+
+void hex_dump(char *str, unsigned char *pSrcBufVA, unsigned int SrcBufLen)
+{
+	unsigned char *pt;
+	int x;
+
+	if (RTDebugLevel < RT_DEBUG_TRACE)
+		return;
+
+	pt = pSrcBufVA;
+	printk("%s: %p, len = %d\n",str,  pSrcBufVA, SrcBufLen);
+	for (x=0; x<SrcBufLen; x++)
+	{
+		if (x % 16 == 0)
+			printk("0x%04x : ", x);
+		printk("%02x ", ((unsigned char)pt[x]));
+		if (x%16 == 15) printk("\n");
+	}
+	printk("\n");
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Send log message through wireless event
+
+		Support standard iw_event with IWEVCUSTOM. It is used below.
+
+		iwreq_data.data.flags is used to store event_flag that is defined by user.
+		iwreq_data.data.length is the length of the event log.
+
+		The format of the event log is composed of the entry's MAC address and
+		the desired log message (refer to pWirelessEventText).
+
+			ex: 11:22:33:44:55:66 has associated successfully
+
+		p.s. The requirement of Wireless Extension is v15 or newer.
+
+	========================================================================
+*/
+VOID RTMPSendWirelessEvent(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Event_flag,
+	IN	PUCHAR 			pAddr,
+	IN	UCHAR			BssIdx,
+	IN	CHAR			Rssi)
+{
+#if WIRELESS_EXT >= 15
+
+	union 	iwreq_data      wrqu;
+	PUCHAR 	pBuf = NULL, pBufPtr = NULL;
+	USHORT	event, type, BufLen;
+	UCHAR	event_table_len = 0;
+
+	type = Event_flag & 0xFF00;
+	event = Event_flag & 0x00FF;
+
+	switch (type)
+	{
+		case IW_SYS_EVENT_FLAG_START:
+			event_table_len = IW_SYS_EVENT_TYPE_NUM;
+			break;
+
+		case IW_SPOOF_EVENT_FLAG_START:
+			event_table_len = IW_SPOOF_EVENT_TYPE_NUM;
+			break;
+
+		case IW_FLOOD_EVENT_FLAG_START:
+			event_table_len = IW_FLOOD_EVENT_TYPE_NUM;
+			break;
+	}
+
+	if (event_table_len == 0)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s : The type(%0x02x) is not valid.\n", __FUNCTION__, type));
+		return;
+	}
+
+	if (event >= event_table_len)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("%s : The event(%0x02x) is not valid.\n", __FUNCTION__, event));
+		return;
+	}
+
+	//Allocate memory and copy the msg.
+	if((pBuf = kmalloc(IW_CUSTOM_MAX_LEN, GFP_ATOMIC)) != NULL)
+	{
+		//Prepare the payload
+		memset(pBuf, 0, IW_CUSTOM_MAX_LEN);
+
+		pBufPtr = pBuf;
+
+		if (pAddr)
+			pBufPtr += sprintf(pBufPtr, "(RT2860) STA(%02x:%02x:%02x:%02x:%02x:%02x) ", PRINT_MAC(pAddr));
+		else if (BssIdx < MAX_MBSSID_NUM)
+			pBufPtr += sprintf(pBufPtr, "(RT2860) BSS(ra%d) ", BssIdx);
+		else
+			pBufPtr += sprintf(pBufPtr, "(RT2860) ");
+
+		if (type == IW_SYS_EVENT_FLAG_START)
+			pBufPtr += sprintf(pBufPtr, "%s", pWirelessSysEventText[event]);
+		else if (type == IW_SPOOF_EVENT_FLAG_START)
+			pBufPtr += sprintf(pBufPtr, "%s (RSSI=%d)", pWirelessSpoofEventText[event], Rssi);
+		else if (type == IW_FLOOD_EVENT_FLAG_START)
+			pBufPtr += sprintf(pBufPtr, "%s", pWirelessFloodEventText[event]);
+		else
+			pBufPtr += sprintf(pBufPtr, "%s", "unknown event");
+
+		pBufPtr[pBufPtr - pBuf] = '\0';
+		BufLen = pBufPtr - pBuf;
+
+		memset(&wrqu, 0, sizeof(wrqu));
+	    wrqu.data.flags = Event_flag;
+		wrqu.data.length = BufLen;
+
+		//send wireless event
+	    wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, pBuf);
+
+		//DBGPRINT(RT_DEBUG_TRACE, ("%s : %s\n", __FUNCTION__, pBuf));
+
+		kfree(pBuf);
+	}
+	else
+		DBGPRINT(RT_DEBUG_ERROR, ("%s : Can't allocate memory for wireless event.\n", __FUNCTION__));
+#else
+	DBGPRINT(RT_DEBUG_ERROR, ("%s : The Wireless Extension MUST be v15 or newer.\n", __FUNCTION__));
+#endif  /* WIRELESS_EXT >= 15 */
+}
+
+
+#ifdef CONFIG_STA_SUPPORT
+void send_monitor_packets(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk)
+{
+    struct sk_buff	*pOSPkt;
+    wlan_ng_prism2_header *ph;
+    int rate_index = 0;
+    USHORT header_len = 0;
+    UCHAR temp_header[40] = {0};
+
+    u_int32_t ralinkrate[256] = {2,4,11,22, 12,18,24,36,48,72,96,  108,   109, 110, 111, 112, 13, 26, 39, 52,78,104, 117, 130, 26, 52, 78,104, 156, 208, 234, 260, 27, 54,81,108,162, 216, 243, 270, // Last 38
+	54, 108, 162, 216, 324, 432, 486, 540,  14, 29, 43, 57, 87, 115, 130, 144, 29, 59,87,115, 173, 230,260, 288, 30, 60,90,120,180,240,270,300,60,120,180,240,360,480,540,600, 0,1,2,3,4,5,6,7,8,9,10,
+	11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30,31,32,33,34,35,36,37,38,39,40,41,42,43,44,45,46,47,48,49,50,51,52,53,54,55,56,57,58,59,60,61,62,63,64,65,66,67,68,69,70,71,72,73,74,75,76,77,78,79,80};
+
+
+    ASSERT(pRxBlk->pRxPacket);
+    if (pRxBlk->DataSize < 10)
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("%s : Size is too small! (%d)\n", __FUNCTION__, pRxBlk->DataSize));
+		goto err_free_sk_buff;
+    }
+
+    if (pRxBlk->DataSize + sizeof(wlan_ng_prism2_header) > RX_BUFFER_AGGRESIZE)
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("%s : Size is too large! (%d)\n", __FUNCTION__, pRxBlk->DataSize + sizeof(wlan_ng_prism2_header)));
+		goto err_free_sk_buff;
+    }
+
+    pOSPkt = RTPKT_TO_OSPKT(pRxBlk->pRxPacket);
+	pOSPkt->dev = get_netdev_from_bssid(pAd, BSS0);
+    if (pRxBlk->pHeader->FC.Type == BTYPE_DATA)
+    {
+        pRxBlk->DataSize -= LENGTH_802_11;
+        if ((pRxBlk->pHeader->FC.ToDs == 1) &&
+            (pRxBlk->pHeader->FC.FrDs == 1))
+            header_len = LENGTH_802_11_WITH_ADDR4;
+        else
+            header_len = LENGTH_802_11;
+
+        // QOS
+    	if (pRxBlk->pHeader->FC.SubType & 0x08)
+    	{
+    	    header_len += 2;
+    		// Data skip QOS contorl field
+    		pRxBlk->DataSize -=2;
+    	}
+
+    	// Order bit: A-Ralink or HTC+
+    	if (pRxBlk->pHeader->FC.Order)
+    	{
+    	    header_len += 4;
+			// Data skip HTC contorl field
+			pRxBlk->DataSize -= 4;
+    	}
+
+        // Copy Header
+        if (header_len <= 40)
+            NdisMoveMemory(temp_header, pRxBlk->pData, header_len);
+
+        // skip HW padding
+    	if (pRxBlk->RxD.L2PAD)
+    	    pRxBlk->pData += (header_len + 2);
+        else
+            pRxBlk->pData += header_len;
+    } //end if
+
+
+	if (pRxBlk->DataSize < pOSPkt->len) {
+        skb_trim(pOSPkt,pRxBlk->DataSize);
+    } else {
+        skb_put(pOSPkt,(pRxBlk->DataSize - pOSPkt->len));
+    } //end if
+
+    if ((pRxBlk->pData - pOSPkt->data) > 0) {
+	    skb_put(pOSPkt,(pRxBlk->pData - pOSPkt->data));
+	    skb_pull(pOSPkt,(pRxBlk->pData - pOSPkt->data));
+    } //end if
+
+    if (skb_headroom(pOSPkt) < (sizeof(wlan_ng_prism2_header)+ header_len)) {
+        if (pskb_expand_head(pOSPkt, (sizeof(wlan_ng_prism2_header) + header_len), 0, GFP_ATOMIC)) {
+	        DBGPRINT(RT_DEBUG_ERROR, ("%s : Reallocate header size of sk_buff fail!\n", __FUNCTION__));
+			goto err_free_sk_buff;
+	    } //end if
+    } //end if
+
+    if (header_len > 0)
+        NdisMoveMemory(skb_push(pOSPkt, header_len), temp_header, header_len);
+
+    ph = (wlan_ng_prism2_header *) skb_push(pOSPkt, sizeof(wlan_ng_prism2_header));
+	NdisZeroMemory(ph, sizeof(wlan_ng_prism2_header));
+
+    ph->msgcode		    = DIDmsg_lnxind_wlansniffrm;
+	ph->msglen		    = sizeof(wlan_ng_prism2_header);
+	strcpy(ph->devname, pAd->net_dev->name);
+
+    ph->hosttime.did = DIDmsg_lnxind_wlansniffrm_hosttime;
+	ph->hosttime.status = 0;
+	ph->hosttime.len = 4;
+	ph->hosttime.data = jiffies;
+
+	ph->mactime.did = DIDmsg_lnxind_wlansniffrm_mactime;
+	ph->mactime.status = 0;
+	ph->mactime.len = 0;
+	ph->mactime.data = 0;
+
+    ph->istx.did = DIDmsg_lnxind_wlansniffrm_istx;
+	ph->istx.status = 0;
+	ph->istx.len = 0;
+	ph->istx.data = 0;
+
+    ph->channel.did = DIDmsg_lnxind_wlansniffrm_channel;
+	ph->channel.status = 0;
+	ph->channel.len = 4;
+
+    ph->channel.data = (u_int32_t)pAd->CommonCfg.Channel;
+
+    ph->rssi.did = DIDmsg_lnxind_wlansniffrm_rssi;
+	ph->rssi.status = 0;
+	ph->rssi.len = 4;
+    ph->rssi.data = (u_int32_t)RTMPMaxRssi(pAd, ConvertToRssi(pAd, pRxBlk->pRxWI->RSSI0, RSSI_0), ConvertToRssi(pAd, pRxBlk->pRxWI->RSSI1, RSSI_1), ConvertToRssi(pAd, pRxBlk->pRxWI->RSSI2, RSSI_2));;
+
+	ph->signal.did = DIDmsg_lnxind_wlansniffrm_signal;
+	ph->signal.status = 0;
+	ph->signal.len = 4;
+	ph->signal.data = 0; //rssi + noise;
+
+	ph->noise.did = DIDmsg_lnxind_wlansniffrm_noise;
+	ph->noise.status = 0;
+	ph->noise.len = 4;
+	ph->noise.data = 0;
+
+#ifdef DOT11_N_SUPPORT
+    if (pRxBlk->pRxWI->PHYMODE >= MODE_HTMIX)
+    {
+    	rate_index = 16 + ((UCHAR)pRxBlk->pRxWI->BW *16) + ((UCHAR)pRxBlk->pRxWI->ShortGI *32) + ((UCHAR)pRxBlk->pRxWI->MCS);
+    }
+    else
+#endif // DOT11_N_SUPPORT //
+	if (pRxBlk->pRxWI->PHYMODE == MODE_OFDM)
+    	rate_index = (UCHAR)(pRxBlk->pRxWI->MCS) + 4;
+    else
+    	rate_index = (UCHAR)(pRxBlk->pRxWI->MCS);
+    if (rate_index < 0)
+        rate_index = 0;
+    if (rate_index > 255)
+        rate_index = 255;
+
+	ph->rate.did = DIDmsg_lnxind_wlansniffrm_rate;
+	ph->rate.status = 0;
+	ph->rate.len = 4;
+    ph->rate.data = ralinkrate[rate_index];
+
+	ph->frmlen.did = DIDmsg_lnxind_wlansniffrm_frmlen;
+    ph->frmlen.status = 0;
+	ph->frmlen.len = 4;
+	ph->frmlen.data	= (u_int32_t)pRxBlk->DataSize;
+
+
+    pOSPkt->pkt_type = PACKET_OTHERHOST;
+    pOSPkt->protocol = eth_type_trans(pOSPkt, pOSPkt->dev);
+    pOSPkt->ip_summed = CHECKSUM_NONE;
+    netif_rx(pOSPkt);
+
+    return;
+
+err_free_sk_buff:
+	RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+	return;
+
+}
+#endif // CONFIG_STA_SUPPORT //
+
+
+void rtmp_os_thread_init(PUCHAR pThreadName, PVOID pNotify)
+{
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+	daemonize(pThreadName /*"%s",pAd->net_dev->name*/);
+
+	allow_signal(SIGTERM);
+	allow_signal(SIGKILL);
+	current->flags |= PF_NOFREEZE;
+#else
+	unsigned long flags;
+
+	daemonize();
+	reparent_to_init();
+	strcpy(current->comm, pThreadName);
+
+	siginitsetinv(&current->blocked, sigmask(SIGTERM) | sigmask(SIGKILL));
+
+	/* Allow interception of SIGKILL only
+	 * Don't allow other signals to interrupt the transmission */
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,4,22)
+	spin_lock_irqsave(&current->sigmask_lock, flags);
+	flush_signals(current);
+	recalc_sigpending(current);
+	spin_unlock_irqrestore(&current->sigmask_lock, flags);
+#endif
+#endif
+
+    /* signal that we've started the thread */
+	complete(pNotify);
+
+}
+
+void RTMP_IndicateMediaState(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	if (pAd->CommonCfg.bWirelessEvent)
+	{
+		if (pAd->IndicateMediaState == NdisMediaStateConnected)
+		{
+			RTMPSendWirelessEvent(pAd, IW_STA_LINKUP_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+		}
+		else
+		{
+			RTMPSendWirelessEvent(pAd, IW_STA_LINKDOWN_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+		}
+	}
+}
+
diff --git a/drivers/staging/rt2870/rt_linux.h b/drivers/staging/rt2870/rt_linux.h
new file mode 100644
index 00000000000..81429757726
--- /dev/null
+++ b/drivers/staging/rt2870/rt_linux.h
@@ -0,0 +1,908 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+/***********************************************************************/
+/*                                                                     */
+/*   Program:    rt_linux.c                                            */
+/*   Created:    4/21/2006 1:17:38 PM                                  */
+/*   Author:     Wu Xi-Kun                                             */
+/*   Comments:   `description`                                         */
+/*                                                                     */
+/*---------------------------------------------------------------------*/
+/*                                                                     */
+/* History:                                                            */
+/*    Revision 1.1 4/21/2006 1:17:38 PM  xsikun                        */
+/*    Initial revision                                                 */
+/*                                                                     */
+/***********************************************************************/
+
+#include "rtmp_type.h"
+#include <linux/module.h>
+#include <linux/version.h>
+#include <linux/kernel.h>
+
+#include <linux/spinlock.h>
+#include <linux/init.h>
+#include <linux/string.h>
+#include <linux/timer.h>
+#include <linux/errno.h>
+#include <linux/slab.h>
+#include <linux/interrupt.h>
+#include <linux/pci.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/skbuff.h>
+#include <linux/ethtool.h>
+#include <linux/wireless.h>
+#include <linux/proc_fs.h>
+#include <linux/delay.h>
+#include <linux/if_arp.h>
+#include <linux/ctype.h>
+#include <linux/vmalloc.h>
+
+
+#include <linux/wireless.h>
+#include <net/iw_handler.h>
+
+// load firmware
+#define __KERNEL_SYSCALLS__
+#include <linux/unistd.h>
+#include <asm/uaccess.h>
+
+
+#define MEM_ALLOC_FLAG      (GFP_ATOMIC) //(GFP_DMA | GFP_ATOMIC)
+
+#ifndef IFNAMSIZ
+#define IFNAMSIZ 16
+#endif
+
+//#define CONFIG_CKIP_SUPPORT
+
+#undef __inline
+#define __inline	   static inline
+
+typedef int (*HARD_START_XMIT_FUNC)(struct sk_buff *skb, struct net_device *net_dev);
+
+// add by kathy
+
+#ifdef CONFIG_STA_SUPPORT
+
+#ifdef RT2870
+#define STA_PROFILE_PATH			"/etc/Wireless/RT2870STA/RT2870STA.dat"
+#define STA_RT2870_IMAGE_FILE_NAME  "/etc/Wireless/RT2870STA/rt2870.bin"
+#define STA_NIC_DEVICE_NAME			"RT2870STA"
+#define STA_DRIVER_VERSION			"1.4.0.0"
+#ifdef MULTIPLE_CARD_SUPPORT
+#define CARD_INFO_PATH			"/etc/Wireless/RT2870STA/RT2870STACard.dat"
+#endif // MULTIPLE_CARD_SUPPORT //
+#endif // RT2870 //
+
+#endif // CONFIG_STA_SUPPORT //
+
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+
+#define RTMP_TIME_AFTER(a,b)		\
+	(typecheck(unsigned long, (unsigned long)a) && \
+	 typecheck(unsigned long, (unsigned long)b) && \
+	 ((long)(b) - (long)(a) < 0))
+
+#define RTMP_TIME_AFTER_EQ(a,b)	\
+	(typecheck(unsigned long, (unsigned long)a) && \
+	 typecheck(unsigned long, (unsigned long)b) && \
+	 ((long)(a) - (long)(b) >= 0))
+#define RTMP_TIME_BEFORE(a,b)	RTMP_TIME_AFTER_EQ(b,a)
+#else
+#define RTMP_TIME_AFTER(a,b) time_after(a, b)
+#endif
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+#define RT_MOD_INC_USE_COUNT() \
+	if (!try_module_get(THIS_MODULE)) \
+	{ \
+		DBGPRINT(RT_DEBUG_ERROR, ("%s: cannot reserve module\n", __FUNCTION__)); \
+		return -1; \
+	}
+
+#define RT_MOD_DEC_USE_COUNT() module_put(THIS_MODULE);
+#else
+#define RT_MOD_INC_USE_COUNT()	MOD_INC_USE_COUNT;
+#define RT_MOD_DEC_USE_COUNT() MOD_DEC_USE_COUNT;
+#endif
+
+#define OS_HZ			HZ
+
+#define ETH_LENGTH_OF_ADDRESS	6
+
+#define IN
+#define OUT
+
+#define NDIS_STATUS                             INT
+#define NDIS_STATUS_SUCCESS                     0x00
+#define NDIS_STATUS_FAILURE                     0x01
+#define NDIS_STATUS_INVALID_DATA				0x02
+#define NDIS_STATUS_RESOURCES                   0x03
+
+#define MIN_NET_DEVICE_FOR_AID			0x00		//0x00~0x3f
+#define MIN_NET_DEVICE_FOR_MBSSID		0x00		//0x00,0x10,0x20,0x30
+#define MIN_NET_DEVICE_FOR_WDS			0x10		//0x40,0x50,0x60,0x70
+#define MIN_NET_DEVICE_FOR_APCLI		0x20
+#define MIN_NET_DEVICE_FOR_MESH			0x30
+#ifdef CONFIG_STA_SUPPORT
+#define MIN_NET_DEVICE_FOR_DLS			0x40
+#endif // CONFIG_STA_SUPPORT //
+
+
+#ifdef CONFIG_STA_SUPPORT
+#define NDIS_PACKET_TYPE_DIRECTED		0
+#define NDIS_PACKET_TYPE_MULTICAST		1
+#define NDIS_PACKET_TYPE_BROADCAST		2
+#define NDIS_PACKET_TYPE_ALL_MULTICAST	3
+#endif // CONFIG_STA_SUPPORT //
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
+typedef	struct pid *	THREAD_PID;
+#define	THREAD_PID_INIT_VALUE	NULL
+#define	GET_PID(_v)	find_get_pid(_v)
+#define	GET_PID_NUMBER(_v)	pid_nr(_v)
+#define CHECK_PID_LEGALITY(_pid)	if (pid_nr(_pid) >= 0)
+#define KILL_THREAD_PID(_A, _B, _C)	kill_pid(_A, _B, _C)
+#else
+typedef	pid_t	THREAD_PID;
+#define	THREAD_PID_INIT_VALUE	-1
+#define	GET_PID(_v)	_v
+#define	GET_PID_NUMBER(_v)	_v
+#define CHECK_PID_LEGALITY(_pid)	if (_pid >= 0)
+#define KILL_THREAD_PID(_A, _B, _C)	kill_proc(_A, _B, _C)
+#endif
+
+struct os_lock  {
+	spinlock_t		lock;
+	unsigned long  	flags;
+};
+
+
+struct os_cookie {
+
+#ifdef RT2870
+	struct usb_device		*pUsb_Dev;
+
+	THREAD_PID				MLMEThr_pid;
+	THREAD_PID				RTUSBCmdThr_pid;
+	THREAD_PID				TimerQThr_pid;
+#endif // RT2870 //
+
+	struct tasklet_struct 	rx_done_task;
+	struct tasklet_struct 	mgmt_dma_done_task;
+	struct tasklet_struct 	ac0_dma_done_task;
+	struct tasklet_struct 	ac1_dma_done_task;
+	struct tasklet_struct 	ac2_dma_done_task;
+	struct tasklet_struct 	ac3_dma_done_task;
+	struct tasklet_struct 	hcca_dma_done_task;
+	struct tasklet_struct	tbtt_task;
+#ifdef RT2870
+	struct tasklet_struct	null_frame_complete_task;
+	struct tasklet_struct	rts_frame_complete_task;
+	struct tasklet_struct	pspoll_frame_complete_task;
+#endif // RT2870 //
+
+
+	unsigned long			apd_pid; //802.1x daemon pid
+	INT						ioctl_if_type;
+	INT 					ioctl_if;
+};
+
+typedef struct _VIRTUAL_ADAPTER
+{
+	struct net_device		*RtmpDev;
+	struct net_device		*VirtualDev;
+} VIRTUAL_ADAPTER, PVIRTUAL_ADAPTER;
+
+#undef  ASSERT
+#define ASSERT(x)                                                               \
+{                                                                               \
+    if (!(x))                                                                   \
+    {                                                                           \
+        printk(KERN_WARNING __FILE__ ":%d assert " #x "failed\n", __LINE__);    \
+    }                                                                           \
+}
+
+typedef struct os_cookie	* POS_COOKIE;
+typedef struct pci_dev 		* PPCI_DEV;
+typedef struct net_device	* PNET_DEV;
+typedef void				* PNDIS_PACKET;
+typedef char				NDIS_PACKET;
+typedef PNDIS_PACKET		* PPNDIS_PACKET;
+typedef	dma_addr_t			NDIS_PHYSICAL_ADDRESS;
+typedef	dma_addr_t			* PNDIS_PHYSICAL_ADDRESS;
+//typedef struct timer_list	RALINK_TIMER_STRUCT;
+//typedef struct timer_list	* PRALINK_TIMER_STRUCT;
+//typedef struct os_lock		NDIS_SPIN_LOCK;
+typedef spinlock_t			NDIS_SPIN_LOCK;
+typedef struct timer_list	NDIS_MINIPORT_TIMER;
+typedef void				* NDIS_HANDLE;
+typedef char 				* PNDIS_BUFFER;
+
+
+
+void hex_dump(char *str, unsigned char *pSrcBufVA, unsigned int SrcBufLen);
+
+dma_addr_t linux_pci_map_single(void *handle, void *ptr, size_t size, int sd_idx, int direction);
+void linux_pci_unmap_single(void *handle, dma_addr_t dma_addr, size_t size, int direction);
+
+
+////////////////////////////////////////
+// MOVE TO rtmp.h ?
+/////////////////////////////////////////
+#define PKTSRC_NDIS             0x7f
+#define PKTSRC_DRIVER           0x0f
+#define PRINT_MAC(addr)	\
+	addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]
+
+
+#define RT2860_PCI_DEVICE_ID		0x0601
+
+
+#ifdef RT2870
+#define PCI_MAP_SINGLE(_handle, _ptr, _size, _dir) (ULONG)0
+
+#define PCI_UNMAP_SINGLE(_handle, _ptr, _size, _dir)
+#endif // RT2870 //
+
+
+#define BEACON_FRAME_DMA_CACHE_WBACK(_ptr, _size)	\
+	dma_cache_wback(_ptr, _size)
+
+
+//////////////////////////////////////////
+//
+//////////////////////////////////////////
+
+
+#define NdisMIndicateStatus(_w, _x, _y, _z)
+
+typedef struct timer_list	RTMP_OS_TIMER;
+
+#ifdef RT2870
+/* ----------------- Timer Related MARCO ---------------*/
+// In RT2870, we have a lot of timer functions and will read/write register, it's
+//	not allowed in Linux USB sub-system to do it ( because of sleep issue when submit
+//  to ctrl pipe). So we need a wrapper function to take care it.
+
+typedef VOID (*RT2870_TIMER_HANDLE)(
+	IN  PVOID   SystemSpecific1,
+	IN  PVOID   FunctionContext,
+	IN  PVOID   SystemSpecific2,
+	IN  PVOID   SystemSpecific3);
+#endif // RT2870 //
+
+
+typedef struct  _RALINK_TIMER_STRUCT    {
+    RTMP_OS_TIMER		TimerObj;       // Ndis Timer object
+	BOOLEAN				Valid;			// Set to True when call RTMPInitTimer
+    BOOLEAN             State;          // True if timer cancelled
+    BOOLEAN	      		PeriodicType;	// True if timer is periodic timer
+    BOOLEAN             Repeat;         // True if periodic timer
+    ULONG               TimerValue;     // Timer value in milliseconds
+	ULONG				cookie;			// os specific object
+#ifdef RT2870
+	RT2870_TIMER_HANDLE	handle;
+	void				*pAd;
+#endif // RT2870 //
+}   RALINK_TIMER_STRUCT, *PRALINK_TIMER_STRUCT;
+
+
+#ifdef RT2870
+
+typedef enum _RT2870_KERNEL_THREAD_STATUS_
+{
+	RT2870_THREAD_UNKNOWN = 0,
+	RT2870_THREAD_INITED = 1,
+	RT2870_THREAD_RUNNING = 2,
+	RT2870_THREAD_STOPED = 4,
+}RT2870_KERNEL_THREAD_STATUS;
+
+#define RT2870_THREAD_CAN_DO_INSERT		(RT2870_THREAD_INITED |RT2870_THREAD_RUNNING)
+
+typedef struct _RT2870_TIMER_ENTRY_
+{
+	RALINK_TIMER_STRUCT 			*pRaTimer;
+	struct _RT2870_TIMER_ENTRY_ 	*pNext;
+}RT2870_TIMER_ENTRY;
+
+
+#define TIMER_QUEUE_SIZE_MAX	128
+typedef struct _RT2870_TIMER_QUEUE_
+{
+	unsigned int		status;
+	//wait_queue_head_t 	timerWaitQ;
+	//atomic_t			count;
+	UCHAR				*pTimerQPoll;
+	RT2870_TIMER_ENTRY	*pQPollFreeList;
+	RT2870_TIMER_ENTRY 	*pQHead;
+	RT2870_TIMER_ENTRY 	*pQTail;
+}RT2870_TIMER_QUEUE;
+#endif // RT2870 //
+
+
+//#define DBG	1
+
+//
+//  MACRO for debugging information
+//
+
+#ifdef DBG
+extern ULONG    RTDebugLevel;
+
+#define DBGPRINT_RAW(Level, Fmt)    \
+{                                   \
+    if (Level <= RTDebugLevel)      \
+    {                               \
+        printk Fmt;               \
+    }                               \
+}
+
+#define DBGPRINT(Level, Fmt)    DBGPRINT_RAW(Level, Fmt)
+
+
+#define DBGPRINT_ERR(Fmt)           \
+{                                   \
+    printk("ERROR!!! ");          \
+    printk Fmt;                  \
+}
+
+#define DBGPRINT_S(Status, Fmt)		\
+{									\
+	printk Fmt;					\
+}
+
+
+#else
+#define DBGPRINT(Level, Fmt)
+#define DBGPRINT_RAW(Level, Fmt)
+#define DBGPRINT_S(Status, Fmt)
+#define DBGPRINT_ERR(Fmt)
+#endif
+
+
+//
+//  spin_lock enhanced for Nested spin lock
+//
+#define NdisAllocateSpinLock(__lock)      \
+{                                       \
+    spin_lock_init((spinlock_t *)(__lock));               \
+}
+
+#define NdisFreeSpinLock(lock)          \
+{                                       \
+}
+
+
+#define RTMP_SEM_LOCK(__lock)					\
+{												\
+	spin_lock_bh((spinlock_t *)(__lock));				\
+}
+
+#define RTMP_SEM_UNLOCK(__lock)					\
+{												\
+	spin_unlock_bh((spinlock_t *)(__lock));				\
+}
+
+#if 0 // sample, IRQ LOCK
+#define RTMP_IRQ_LOCK(__lock, __irqflags)					\
+{													\
+	spin_lock_irqsave((spinlock_t *)__lock, __irqflags);	\
+	pAd->irq_disabled |= 1; \
+}
+
+#define RTMP_IRQ_UNLOCK(__lock, __irqflag)						\
+{														\
+	pAd->irq_disabled &= 0; \
+	spin_unlock_irqrestore((spinlock_t *)(__lock), ((unsigned long)__irqflag));	\
+}
+#else
+
+// sample, use semaphore lock to replace IRQ lock, 2007/11/15
+#define RTMP_IRQ_LOCK(__lock, __irqflags)			\
+{													\
+	__irqflags = 0;									\
+	spin_lock_bh((spinlock_t *)(__lock));			\
+	pAd->irq_disabled |= 1; \
+}
+
+#define RTMP_IRQ_UNLOCK(__lock, __irqflag)			\
+{													\
+	pAd->irq_disabled &= 0; \
+	spin_unlock_bh((spinlock_t *)(__lock));			\
+}
+
+#define RTMP_INT_LOCK(__lock, __irqflags)			\
+{													\
+	spin_lock_irqsave((spinlock_t *)__lock, __irqflags);	\
+}
+
+#define RTMP_INT_UNLOCK(__lock, __irqflag)			\
+{													\
+	spin_unlock_irqrestore((spinlock_t *)(__lock), ((unsigned long)__irqflag));	\
+}
+#endif
+
+
+
+#ifdef RT2870
+//Patch for ASIC turst read/write bug, needs to remove after metel fix
+#define RTMP_IO_READ32(_A, _R, _pV)								\
+	RTUSBReadMACRegister(_A, _R, _pV)
+
+#define RTMP_IO_READ8(_A, _R, _pV)								\
+{																\
+}
+
+#define RTMP_IO_WRITE32(_A, _R, _V)								\
+	RTUSBWriteMACRegister(_A, _R, _V)
+
+
+#define RTMP_IO_WRITE8(_A, _R, _V)								\
+{																\
+	USHORT	_Val = _V;											\
+	RTUSBSingleWrite(_A, _R, _Val);								\
+}
+
+
+#define RTMP_IO_WRITE16(_A, _R, _V)								\
+{																\
+	RTUSBSingleWrite(_A, _R, _V);								\
+}
+#endif // RT2870 //
+
+#ifndef wait_event_interruptible_timeout
+#define __wait_event_interruptible_timeout(wq, condition, ret) \
+do { \
+        wait_queue_t __wait; \
+        init_waitqueue_entry(&__wait, current); \
+        add_wait_queue(&wq, &__wait); \
+        for (;;) { \
+                set_current_state(TASK_INTERRUPTIBLE); \
+                if (condition) \
+                        break; \
+                if (!signal_pending(current)) { \
+                        ret = schedule_timeout(ret); \
+                        if (!ret) \
+                                break; \
+                        continue; \
+                } \
+                ret = -ERESTARTSYS; \
+                break; \
+        } \
+        current->state = TASK_RUNNING; \
+        remove_wait_queue(&wq, &__wait); \
+} while (0)
+
+#define wait_event_interruptible_timeout(wq, condition, timeout) \
+({ \
+        long __ret = timeout; \
+        if (!(condition)) \
+                __wait_event_interruptible_timeout(wq, condition, __ret); \
+        __ret; \
+})
+#endif
+#define ONE_TICK 1
+#define OS_WAIT(_time) \
+{	int _i; \
+	long _loop = ((_time)/(1000/OS_HZ)) > 0 ? ((_time)/(1000/OS_HZ)) : 1;\
+	wait_queue_head_t _wait; \
+	init_waitqueue_head(&_wait); \
+	for (_i=0; _i<(_loop); _i++) \
+		wait_event_interruptible_timeout(_wait, 0, ONE_TICK); }
+
+
+typedef void (*TIMER_FUNCTION)(unsigned long);
+
+#define COPY_MAC_ADDR(Addr1, Addr2)             memcpy((Addr1), (Addr2), MAC_ADDR_LEN)
+
+#define MlmeAllocateMemory(_pAd, _ppVA) os_alloc_mem(_pAd, _ppVA, MGMT_DMA_BUFFER_SIZE)
+#define MlmeFreeMemory(_pAd, _pVA)     os_free_mem(_pAd, _pVA)
+
+
+#ifdef RT2870
+#define BUILD_TIMER_FUNCTION(_func)													\
+void linux_##_func(unsigned long data)												\
+{																					\
+	PRALINK_TIMER_STRUCT	_pTimer = (PRALINK_TIMER_STRUCT)data;					\
+	RT2870_TIMER_ENTRY		*_pQNode;												\
+	RTMP_ADAPTER			*_pAd;													\
+																				\
+	_pTimer->handle = _func;															\
+	_pAd = (RTMP_ADAPTER *)_pTimer->pAd;												\
+	_pQNode = RT2870_TimerQ_Insert(_pAd, _pTimer); 									\
+	if ((_pQNode == NULL) && (_pAd->TimerQ.status & RT2870_THREAD_CAN_DO_INSERT))	\
+		RTMP_OS_Add_Timer(&_pTimer->TimerObj, HZ);               					\
+}
+#endif // RT2870 //
+
+
+#define DECLARE_TIMER_FUNCTION(_func)			\
+void linux_##_func(unsigned long data)
+
+#define GET_TIMER_FUNCTION(_func)				\
+		linux_##_func
+
+DECLARE_TIMER_FUNCTION(MlmePeriodicExec);
+DECLARE_TIMER_FUNCTION(MlmeRssiReportExec);
+DECLARE_TIMER_FUNCTION(AsicRxAntEvalTimeout);
+DECLARE_TIMER_FUNCTION(APSDPeriodicExec);
+DECLARE_TIMER_FUNCTION(AsicRfTuningExec);
+#ifdef RT2870
+DECLARE_TIMER_FUNCTION(BeaconUpdateExec);
+#endif // RT2870 //
+
+
+#ifdef CONFIG_STA_SUPPORT
+DECLARE_TIMER_FUNCTION(BeaconTimeout);
+DECLARE_TIMER_FUNCTION(ScanTimeout);
+DECLARE_TIMER_FUNCTION(AuthTimeout);
+DECLARE_TIMER_FUNCTION(AssocTimeout);
+DECLARE_TIMER_FUNCTION(ReassocTimeout);
+DECLARE_TIMER_FUNCTION(DisassocTimeout);
+DECLARE_TIMER_FUNCTION(LinkDownExec);
+#ifdef LEAP_SUPPORT
+DECLARE_TIMER_FUNCTION(LeapAuthTimeout);
+#endif
+DECLARE_TIMER_FUNCTION(StaQuickResponeForRateUpExec);
+DECLARE_TIMER_FUNCTION(WpaDisassocApAndBlockAssoc);
+DECLARE_TIMER_FUNCTION(PsPollWakeExec);
+DECLARE_TIMER_FUNCTION(RadioOnExec);
+
+#ifdef QOS_DLS_SUPPORT
+DECLARE_TIMER_FUNCTION(DlsTimeoutAction);
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+void RTMP_GetCurrentSystemTime(LARGE_INTEGER *time);
+
+
+/*
+ * packet helper
+ * 	- convert internal rt packet to os packet or
+ *             os packet to rt packet
+ */
+#define RTPKT_TO_OSPKT(_p)		((struct sk_buff *)(_p))
+#define OSPKT_TO_RTPKT(_p)		((PNDIS_PACKET)(_p))
+
+#define GET_OS_PKT_DATAPTR(_pkt) \
+		(RTPKT_TO_OSPKT(_pkt)->data)
+
+#define GET_OS_PKT_LEN(_pkt) \
+		(RTPKT_TO_OSPKT(_pkt)->len)
+
+#define GET_OS_PKT_DATATAIL(_pkt) \
+		(RTPKT_TO_OSPKT(_pkt)->tail)
+
+#define GET_OS_PKT_HEAD(_pkt) \
+		(RTPKT_TO_OSPKT(_pkt)->head)
+
+#define GET_OS_PKT_END(_pkt) \
+		(RTPKT_TO_OSPKT(_pkt)->end)
+
+#define GET_OS_PKT_NETDEV(_pkt) \
+		(RTPKT_TO_OSPKT(_pkt)->dev)
+
+#define GET_OS_PKT_TYPE(_pkt) \
+		(RTPKT_TO_OSPKT(_pkt))
+
+#define GET_OS_PKT_NEXT(_pkt) \
+		(RTPKT_TO_OSPKT(_pkt)->next)
+
+
+#define OS_NTOHS(_Val) \
+		(ntohs(_Val))
+#define OS_HTONS(_Val) \
+		(htons(_Val))
+#define OS_NTOHL(_Val) \
+		(ntohl(_Val))
+#define OS_HTONL(_Val) \
+		(htonl(_Val))
+
+/* statistics counter */
+#define STATS_INC_RX_PACKETS(_pAd, _dev)
+#define STATS_INC_TX_PACKETS(_pAd, _dev)
+
+#define STATS_INC_RX_BYTESS(_pAd, _dev, len)
+#define STATS_INC_TX_BYTESS(_pAd, _dev, len)
+
+#define STATS_INC_RX_ERRORS(_pAd, _dev)
+#define STATS_INC_TX_ERRORS(_pAd, _dev)
+
+#define STATS_INC_RX_DROPPED(_pAd, _dev)
+#define STATS_INC_TX_DROPPED(_pAd, _dev)
+
+
+#define CB_OFF  10
+
+
+//   check DDK NDIS_PACKET data structure and find out only MiniportReservedEx[0..7] can be used by our driver without
+//   ambiguity. Fields after pPacket->MiniportReservedEx[8] may be used by other wrapper layer thus crashes the driver
+//
+//#define RTMP_GET_PACKET_MR(_p)			(RTPKT_TO_OSPKT(_p))
+
+// User Priority
+#define RTMP_SET_PACKET_UP(_p, _prio)			(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+0] = _prio)
+#define RTMP_GET_PACKET_UP(_p)					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+0])
+
+// Fragment #
+#define RTMP_SET_PACKET_FRAGMENTS(_p, _num)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+1] = _num)
+#define RTMP_GET_PACKET_FRAGMENTS(_p)			(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+1])
+
+// 0x0 ~0x7f: TX to AP's own BSS which has the specified AID. if AID>127, set bit 7 in RTMP_SET_PACKET_EMACTAB too.
+//(this value also as MAC(on-chip WCID) table index)
+// 0x80~0xff: TX to a WDS link. b0~6: WDS index
+#define RTMP_SET_PACKET_WCID(_p, _wdsidx)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+2] = _wdsidx)
+#define RTMP_GET_PACKET_WCID(_p)          		((UCHAR)(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+2]))
+
+// 0xff: PKTSRC_NDIS, others: local TX buffer index. This value affects how to a packet
+#define RTMP_SET_PACKET_SOURCE(_p, _pktsrc)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+3] = _pktsrc)
+#define RTMP_GET_PACKET_SOURCE(_p)       		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+3])
+
+// RTS/CTS-to-self protection method
+#define RTMP_SET_PACKET_RTS(_p, _num)      		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+4] = _num)
+#define RTMP_GET_PACKET_RTS(_p)          		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+4])
+// see RTMP_S(G)ET_PACKET_EMACTAB
+
+// TX rate index
+#define RTMP_SET_PACKET_TXRATE(_p, _rate)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+5] = _rate)
+#define RTMP_GET_PACKET_TXRATE(_p)		  		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+5])
+
+// From which Interface
+#define RTMP_SET_PACKET_IF(_p, _ifdx)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+6] = _ifdx)
+#define RTMP_GET_PACKET_IF(_p)		  		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+6])
+#define RTMP_SET_PACKET_NET_DEVICE_MBSSID(_p, _bss)		RTMP_SET_PACKET_IF((_p), (_bss))
+#define RTMP_SET_PACKET_NET_DEVICE_WDS(_p, _bss)		RTMP_SET_PACKET_IF((_p), ((_bss) + MIN_NET_DEVICE_FOR_WDS))
+#define RTMP_SET_PACKET_NET_DEVICE_APCLI(_p, _idx)   	RTMP_SET_PACKET_IF((_p), ((_idx) + MIN_NET_DEVICE_FOR_APCLI))
+#define RTMP_SET_PACKET_NET_DEVICE_MESH(_p, _idx)   	RTMP_SET_PACKET_IF((_p), ((_idx) + MIN_NET_DEVICE_FOR_MESH))
+#define RTMP_GET_PACKET_NET_DEVICE_MBSSID(_p)			RTMP_GET_PACKET_IF((_p))
+#define RTMP_GET_PACKET_NET_DEVICE(_p)					RTMP_GET_PACKET_IF((_p))
+
+#define RTMP_SET_PACKET_MOREDATA(_p, _morebit)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+7] = _morebit)
+#define RTMP_GET_PACKET_MOREDATA(_p)				(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+7])
+
+//#define RTMP_SET_PACKET_NET_DEVICE_MBSSID(_p, _bss)	(RTPKT_TO_OSPKT(_p)->cb[8] = _bss)
+//#define RTMP_GET_PACKET_NET_DEVICE_MBSSID(_p)		(RTPKT_TO_OSPKT(_p)->cb[8])
+
+
+
+
+#if 0
+//#define RTMP_SET_PACKET_DHCP(_p, _flg)   	(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11] = _flg)
+//#define RTMP_GET_PACKET_DHCP(_p)         	(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11])
+#else
+//
+//	Sepcific Pakcet Type definition
+//
+#define RTMP_PACKET_SPECIFIC_CB_OFFSET	11
+
+#define RTMP_PACKET_SPECIFIC_DHCP		0x01
+#define RTMP_PACKET_SPECIFIC_EAPOL		0x02
+#define RTMP_PACKET_SPECIFIC_IPV4		0x04
+#define RTMP_PACKET_SPECIFIC_WAI		0x08
+#define RTMP_PACKET_SPECIFIC_VLAN		0x10
+#define RTMP_PACKET_SPECIFIC_LLCSNAP	0x20
+
+//Specific
+#define RTMP_SET_PACKET_SPECIFIC(_p, _flg)	   	(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11] = _flg)
+
+//DHCP
+#define RTMP_SET_PACKET_DHCP(_p, _flg)   													\
+			do{																				\
+				if (_flg)																	\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) |= (RTMP_PACKET_SPECIFIC_DHCP);		\
+				else																		\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) &= (!RTMP_PACKET_SPECIFIC_DHCP);	\
+			}while(0)
+#define RTMP_GET_PACKET_DHCP(_p)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11] & RTMP_PACKET_SPECIFIC_DHCP)
+
+//EAPOL
+#define RTMP_SET_PACKET_EAPOL(_p, _flg)   													\
+			do{																				\
+				if (_flg)																	\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) |= (RTMP_PACKET_SPECIFIC_EAPOL);		\
+				else																		\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) &= (!RTMP_PACKET_SPECIFIC_EAPOL);	\
+			}while(0)
+#define RTMP_GET_PACKET_EAPOL(_p)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11] & RTMP_PACKET_SPECIFIC_EAPOL)
+
+//WAI
+#define RTMP_SET_PACKET_WAI(_p, _flg)   													\
+			do{																				\
+				if (_flg)																	\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) |= (RTMP_PACKET_SPECIFIC_WAI);		\
+				else																		\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) &= (!RTMP_PACKET_SPECIFIC_WAI);	\
+			}while(0)
+#define RTMP_GET_PACKET_WAI(_p)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11] & RTMP_PACKET_SPECIFIC_WAI)
+
+#define RTMP_GET_PACKET_LOWRATE(_p)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11] & (RTMP_PACKET_SPECIFIC_EAPOL | RTMP_PACKET_SPECIFIC_DHCP | RTMP_PACKET_SPECIFIC_WAI))
+
+//VLAN
+#define RTMP_SET_PACKET_VLAN(_p, _flg)   													\
+			do{																				\
+				if (_flg)																	\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) |= (RTMP_PACKET_SPECIFIC_VLAN);		\
+				else																		\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) &= (!RTMP_PACKET_SPECIFIC_VLAN);	\
+			}while(0)
+#define RTMP_GET_PACKET_VLAN(_p)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11] & RTMP_PACKET_SPECIFIC_VLAN)
+
+//LLC/SNAP
+#define RTMP_SET_PACKET_LLCSNAP(_p, _flg)   													\
+			do{																				\
+				if (_flg)																	\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) |= (RTMP_PACKET_SPECIFIC_LLCSNAP);		\
+				else																		\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) &= (!RTMP_PACKET_SPECIFIC_LLCSNAP);		\
+			}while(0)
+
+#define RTMP_GET_PACKET_LLCSNAP(_p)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11] & RTMP_PACKET_SPECIFIC_LLCSNAP)
+
+// IP
+#define RTMP_SET_PACKET_IPV4(_p, _flg)														\
+			do{																				\
+				if (_flg)																	\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) |= (RTMP_PACKET_SPECIFIC_IPV4);		\
+				else																		\
+					(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11]) &= (!RTMP_PACKET_SPECIFIC_IPV4);	\
+			}while(0)
+
+#define RTMP_GET_PACKET_IPV4(_p)		(RTPKT_TO_OSPKT(_p)->cb[CB_OFF+11] & RTMP_PACKET_SPECIFIC_IPV4)
+
+#endif
+
+
+// If this flag is set, it indicates that this EAPoL frame MUST be clear.
+#define RTMP_SET_PACKET_CLEAR_EAP_FRAME(_p, _flg)   (RTPKT_TO_OSPKT(_p)->cb[CB_OFF+12] = _flg)
+#define RTMP_GET_PACKET_CLEAR_EAP_FRAME(_p)         (RTPKT_TO_OSPKT(_p)->cb[CB_OFF+12])
+
+#define RTMP_SET_PACKET_5VT(_p, _flg)   (RTPKT_TO_OSPKT(_p)->cb[CB_OFF+22] = _flg)
+#define RTMP_GET_PACKET_5VT(_p)         (RTPKT_TO_OSPKT(_p)->cb[CB_OFF+22])
+
+
+#ifdef CONFIG_5VT_ENHANCE
+#define BRIDGE_TAG 0x35564252    // depends on 5VT define in br_input.c
+#endif
+
+
+#define NDIS_SET_PACKET_STATUS(_p, _status)
+
+
+#define GET_SG_LIST_FROM_PACKET(_p, _sc)	\
+    rt_get_sg_list_from_packet(_p, _sc)
+
+
+#define NdisMoveMemory(Destination, Source, Length) memmove(Destination, Source, Length)
+#define NdisZeroMemory(Destination, Length)         memset(Destination, 0, Length)
+#define NdisFillMemory(Destination, Length, Fill)   memset(Destination, Fill, Length)
+#define NdisEqualMemory(Source1, Source2, Length)   (!memcmp(Source1, Source2, Length))
+#define RTMPEqualMemory(Source1, Source2, Length)	(!memcmp(Source1, Source2, Length))
+
+
+#define RTMP_INC_REF(_A)		0
+#define RTMP_DEC_REF(_A)		0
+#define RTMP_GET_REF(_A)		0
+
+
+
+/*
+ * ULONG
+ * RTMP_GetPhysicalAddressLow(
+ *   IN NDIS_PHYSICAL_ADDRESS  PhysicalAddress);
+ */
+#define RTMP_GetPhysicalAddressLow(PhysicalAddress)		(PhysicalAddress)
+
+/*
+ * ULONG
+ * RTMP_GetPhysicalAddressHigh(
+ *   IN NDIS_PHYSICAL_ADDRESS  PhysicalAddress);
+ */
+#define RTMP_GetPhysicalAddressHigh(PhysicalAddress)		(0)
+
+/*
+ * VOID
+ * RTMP_SetPhysicalAddressLow(
+ *   IN NDIS_PHYSICAL_ADDRESS  PhysicalAddress,
+ *   IN ULONG  Value);
+ */
+#define RTMP_SetPhysicalAddressLow(PhysicalAddress, Value)	\
+			PhysicalAddress = Value;
+
+/*
+ * VOID
+ * RTMP_SetPhysicalAddressHigh(
+ *   IN NDIS_PHYSICAL_ADDRESS  PhysicalAddress,
+ *   IN ULONG  Value);
+ */
+#define RTMP_SetPhysicalAddressHigh(PhysicalAddress, Value)
+
+
+//CONTAINING_RECORD(pEntry, NDIS_PACKET, MiniportReservedEx);
+#define QUEUE_ENTRY_TO_PACKET(pEntry) \
+	(PNDIS_PACKET)(pEntry)
+
+#define PACKET_TO_QUEUE_ENTRY(pPacket) \
+	(PQUEUE_ENTRY)(pPacket)
+
+
+#ifndef CONTAINING_RECORD
+#define CONTAINING_RECORD(address, type, field)			\
+((type *)((PCHAR)(address) - offsetof(type, field)))
+#endif
+
+
+#define RELEASE_NDIS_PACKET(_pAd, _pPacket, _Status)                    \
+{                                                                       \
+        RTMPFreeNdisPacket(_pAd, _pPacket);                             \
+}
+
+
+#define SWITCH_PhyAB(_pAA, _pBB)    \
+{                                                                           \
+    ULONG	AABasePaHigh;                           \
+    ULONG	AABasePaLow;                           \
+    ULONG	BBBasePaHigh;                           \
+    ULONG	BBBasePaLow;                           \
+    BBBasePaHigh = RTMP_GetPhysicalAddressHigh(_pBB);                                                 \
+    BBBasePaLow = RTMP_GetPhysicalAddressLow(_pBB);                                                 \
+    AABasePaHigh = RTMP_GetPhysicalAddressHigh(_pAA);                                                 \
+    AABasePaLow = RTMP_GetPhysicalAddressLow(_pAA);                                                 \
+    RTMP_SetPhysicalAddressHigh(_pAA, BBBasePaHigh);                                                 \
+    RTMP_SetPhysicalAddressLow(_pAA, BBBasePaLow);                                                 \
+    RTMP_SetPhysicalAddressHigh(_pBB, AABasePaHigh);                                                 \
+    RTMP_SetPhysicalAddressLow(_pBB, AABasePaLow);                                                 \
+}
+
+
+#define NdisWriteErrorLogEntry(_a, _b, _c, _d)
+#define NdisMAllocateMapRegisters(_a, _b, _c, _d, _e)		NDIS_STATUS_SUCCESS
+
+
+#define NdisAcquireSpinLock		RTMP_SEM_LOCK
+#define NdisReleaseSpinLock		RTMP_SEM_UNLOCK
+
+static inline void NdisGetSystemUpTime(ULONG *time)
+{
+	*time = jiffies;
+}
+
+//pPacket = CONTAINING_RECORD(pEntry, NDIS_PACKET, MiniportReservedEx);
+#define QUEUE_ENTRY_TO_PKT(pEntry) \
+		((PNDIS_PACKET) (pEntry))
+
+int rt28xx_packet_xmit(struct sk_buff *skb);
+
+
+
+void rtmp_os_thread_init(PUCHAR pThreadName, PVOID pNotify);
+
+
diff --git a/drivers/staging/rt2870/rt_main_dev.c b/drivers/staging/rt2870/rt_main_dev.c
new file mode 100644
index 00000000000..b990f8a6164
--- /dev/null
+++ b/drivers/staging/rt2870/rt_main_dev.c
@@ -0,0 +1,1863 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    rt_main_dev.c
+
+    Abstract:
+    Create and register network interface.
+
+    Revision History:
+    Who         When            What
+    --------    ----------      ----------------------------------------------
+	Sample		Mar/21/07		Merge RT2870 and RT2860 drivers.
+*/
+
+#include "rt_config.h"
+
+#define FORTY_MHZ_INTOLERANT_INTERVAL	(60*1000) // 1 min
+
+#ifdef MULTIPLE_CARD_SUPPORT
+// record whether the card in the card list is used in the card file
+UINT8  MC_CardUsed[MAX_NUM_OF_MULTIPLE_CARD];
+// record used card mac address in the card list
+static UINT8  MC_CardMac[MAX_NUM_OF_MULTIPLE_CARD][6];
+#endif // MULTIPLE_CARD_SUPPORT //
+
+/*---------------------------------------------------------------------*/
+/* Private Variables Used                                              */
+/*---------------------------------------------------------------------*/
+//static RALINK_TIMER_STRUCT     PeriodicTimer;
+
+char *mac = "";		   // default 00:00:00:00:00:00
+char *hostname = "";
+#if LINUX_VERSION_CODE <= KERNEL_VERSION(2,6,12)
+MODULE_PARM (mac, "s");
+#else
+module_param (mac, charp, 0);
+#endif
+MODULE_PARM_DESC (mac, "rt28xx: wireless mac addr");
+
+
+/*---------------------------------------------------------------------*/
+/* Prototypes of Functions Used                                        */
+/*---------------------------------------------------------------------*/
+#ifdef DOT11_N_SUPPORT
+extern BOOLEAN ba_reordering_resource_init(PRTMP_ADAPTER pAd, int num);
+extern void ba_reordering_resource_release(PRTMP_ADAPTER pAd);
+#endif // DOT11_N_SUPPORT //
+extern NDIS_STATUS NICLoadRateSwitchingParams(IN PRTMP_ADAPTER pAd);
+
+
+// public function prototype
+INT __devinit rt28xx_probe(IN void *_dev_p, IN void *_dev_id_p,
+							IN UINT argc, OUT PRTMP_ADAPTER *ppAd);
+
+// private function prototype
+static int rt28xx_init(IN struct net_device *net_dev);
+INT rt28xx_send_packets(IN struct sk_buff *skb_p, IN struct net_device *net_dev);
+
+#if LINUX_VERSION_CODE <= 0x20402	// Red Hat 7.1
+struct net_device *alloc_netdev(
+	int sizeof_priv,
+	const char *mask,
+	void (*setup)(struct net_device *));
+#endif // LINUX_VERSION_CODE //
+
+static void CfgInitHook(PRTMP_ADAPTER pAd);
+//static BOOLEAN RT28XXAvailRANameAssign(IN CHAR *name_p);
+
+#ifdef CONFIG_STA_SUPPORT
+extern	const struct iw_handler_def rt28xx_iw_handler_def;
+#endif // CONFIG_STA_SUPPORT //
+
+#if WIRELESS_EXT >= 12
+// This function will be called when query /proc
+struct iw_statistics *rt28xx_get_wireless_stats(
+    IN struct net_device *net_dev);
+#endif
+
+struct net_device_stats *RT28xx_get_ether_stats(
+    IN  struct net_device *net_dev);
+
+/*
+========================================================================
+Routine Description:
+    Close raxx interface.
+
+Arguments:
+	*net_dev			the raxx interface pointer
+
+Return Value:
+    0					Open OK
+	otherwise			Open Fail
+
+Note:
+	1. if open fail, kernel will not call the close function.
+	2. Free memory for
+		(1) Mlme Memory Handler:		MlmeHalt()
+		(2) TX & RX:					RTMPFreeTxRxRingMemory()
+		(3) BA Reordering: 				ba_reordering_resource_release()
+========================================================================
+*/
+int MainVirtualIF_close(IN struct net_device *net_dev)
+{
+    RTMP_ADAPTER *pAd = net_dev->ml_priv;
+
+	// Sanity check for pAd
+	if (pAd == NULL)
+		return 0; // close ok
+
+	netif_carrier_off(pAd->net_dev);
+	netif_stop_queue(pAd->net_dev);
+
+
+
+	VIRTUAL_IF_DOWN(pAd);
+
+	RT_MOD_DEC_USE_COUNT();
+
+	return 0; // close ok
+}
+
+/*
+========================================================================
+Routine Description:
+    Open raxx interface.
+
+Arguments:
+	*net_dev			the raxx interface pointer
+
+Return Value:
+    0					Open OK
+	otherwise			Open Fail
+
+Note:
+	1. if open fail, kernel will not call the close function.
+	2. Free memory for
+		(1) Mlme Memory Handler:		MlmeHalt()
+		(2) TX & RX:					RTMPFreeTxRxRingMemory()
+		(3) BA Reordering: 				ba_reordering_resource_release()
+========================================================================
+*/
+int MainVirtualIF_open(IN struct net_device *net_dev)
+{
+    RTMP_ADAPTER *pAd = net_dev->ml_priv;
+
+	// Sanity check for pAd
+	if (pAd == NULL)
+		return 0; // close ok
+
+	if (VIRTUAL_IF_UP(pAd) != 0)
+		return -1;
+
+	// increase MODULE use count
+	RT_MOD_INC_USE_COUNT();
+
+	netif_start_queue(net_dev);
+	netif_carrier_on(net_dev);
+	netif_wake_queue(net_dev);
+
+	return 0;
+}
+
+/*
+========================================================================
+Routine Description:
+    Close raxx interface.
+
+Arguments:
+	*net_dev			the raxx interface pointer
+
+Return Value:
+    0					Open OK
+	otherwise			Open Fail
+
+Note:
+	1. if open fail, kernel will not call the close function.
+	2. Free memory for
+		(1) Mlme Memory Handler:		MlmeHalt()
+		(2) TX & RX:					RTMPFreeTxRxRingMemory()
+		(3) BA Reordering: 				ba_reordering_resource_release()
+========================================================================
+*/
+int rt28xx_close(IN PNET_DEV dev)
+{
+	struct net_device * net_dev = (struct net_device *)dev;
+    RTMP_ADAPTER	*pAd = net_dev->ml_priv;
+	BOOLEAN 		Cancelled = FALSE;
+	UINT32			i = 0;
+#ifdef RT2870
+	DECLARE_WAIT_QUEUE_HEAD(unlink_wakeup);
+	DECLARE_WAITQUEUE(wait, current);
+
+	//RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_REMOVE_IN_PROGRESS);
+#endif // RT2870 //
+
+
+    DBGPRINT(RT_DEBUG_TRACE, ("===> rt28xx_close\n"));
+
+	// Sanity check for pAd
+	if (pAd == NULL)
+		return 0; // close ok
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+
+		// If dirver doesn't wake up firmware here,
+		// NICLoadFirmware will hang forever when interface is up again.
+		if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+        {
+		    AsicForceWakeup(pAd, TRUE);
+        }
+
+#ifdef QOS_DLS_SUPPORT
+		// send DLS-TEAR_DOWN message,
+		if (pAd->CommonCfg.bDLSCapable)
+		{
+			UCHAR i;
+
+			// tear down local dls table entry
+			for (i=0; i<MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+			{
+				if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+				{
+					RTMPSendDLSTearDownFrame(pAd, pAd->StaCfg.DLSEntry[i].MacAddr);
+					pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+					pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+				}
+			}
+
+			// tear down peer dls table entry
+			for (i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+			{
+				if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+				{
+					RTMPSendDLSTearDownFrame(pAd, pAd->StaCfg.DLSEntry[i].MacAddr);
+					pAd->StaCfg.DLSEntry[i].Status = DLS_NONE;
+					pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+				}
+			}
+			RT28XX_MLME_HANDLER(pAd);
+		}
+#endif // QOS_DLS_SUPPORT //
+
+		if (INFRA_ON(pAd) &&
+			(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_NIC_NOT_EXIST)))
+		{
+			MLME_DISASSOC_REQ_STRUCT	DisReq;
+			MLME_QUEUE_ELEM *MsgElem = (MLME_QUEUE_ELEM *) kmalloc(sizeof(MLME_QUEUE_ELEM), MEM_ALLOC_FLAG);
+
+			COPY_MAC_ADDR(DisReq.Addr, pAd->CommonCfg.Bssid);
+			DisReq.Reason =  REASON_DEAUTH_STA_LEAVING;
+
+			MsgElem->Machine = ASSOC_STATE_MACHINE;
+			MsgElem->MsgType = MT2_MLME_DISASSOC_REQ;
+			MsgElem->MsgLen = sizeof(MLME_DISASSOC_REQ_STRUCT);
+			NdisMoveMemory(MsgElem->Msg, &DisReq, sizeof(MLME_DISASSOC_REQ_STRUCT));
+
+			// Prevent to connect AP again in STAMlmePeriodicExec
+			pAd->MlmeAux.AutoReconnectSsidLen= 32;
+			NdisZeroMemory(pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.AutoReconnectSsidLen);
+
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_DISASSOC;
+			MlmeDisassocReqAction(pAd, MsgElem);
+			kfree(MsgElem);
+
+			RTMPusecDelay(1000);
+		}
+
+#ifdef RT2870
+	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_REMOVE_IN_PROGRESS);
+#endif // RT2870 //
+
+#ifdef CCX_SUPPORT
+		RTMPCancelTimer(&pAd->StaCfg.LeapAuthTimer, &Cancelled);
+#endif
+
+		RTMPCancelTimer(&pAd->StaCfg.StaQuickResponeForRateUpTimer, &Cancelled);
+		RTMPCancelTimer(&pAd->StaCfg.WpaDisassocAndBlockAssocTimer, &Cancelled);
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+		{
+			union iwreq_data    wrqu;
+			// send wireless event to wpa_supplicant for infroming interface down.
+			memset(&wrqu, 0, sizeof(wrqu));
+			wrqu.data.flags = RT_INTERFACE_DOWN;
+			wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+		}
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+		MlmeRadioOff(pAd);
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS);
+
+	for (i = 0 ; i < NUM_OF_TX_RING; i++)
+	{
+		while (pAd->DeQueueRunning[i] == TRUE)
+		{
+			printk("Waiting for TxQueue[%d] done..........\n", i);
+			RTMPusecDelay(1000);
+		}
+	}
+
+#ifdef RT2870
+	// ensure there are no more active urbs.
+	add_wait_queue (&unlink_wakeup, &wait);
+	pAd->wait = &unlink_wakeup;
+
+	// maybe wait for deletions to finish.
+	i = 0;
+	//while((i < 25) && atomic_read(&pAd->PendingRx) > 0)
+	while(i < 25)
+	{
+		unsigned long IrqFlags;
+
+		RTMP_IRQ_LOCK(&pAd->BulkInLock, IrqFlags);
+		if (pAd->PendingRx == 0)
+		{
+			RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+			break;
+		}
+		RTMP_IRQ_UNLOCK(&pAd->BulkInLock, IrqFlags);
+
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,9)
+		msleep(UNLINK_TIMEOUT_MS);	//Time in millisecond
+#else
+		RTMPusecDelay(UNLINK_TIMEOUT_MS*1000);	//Time in microsecond
+#endif
+		i++;
+	}
+	pAd->wait = NULL;
+	remove_wait_queue (&unlink_wakeup, &wait);
+#endif // RT2870 //
+
+	//RTUSBCleanUpMLMEWaitQueue(pAd);	/*not used in RT28xx*/
+
+
+#ifdef RT2870
+	// We need clear timerQ related structure before exits of the timer thread.
+	RT2870_TimerQ_Exit(pAd);
+	// Close kernel threads or tasklets
+	RT28xxThreadTerminate(pAd);
+#endif // RT2870 //
+
+	// Stop Mlme state machine
+	MlmeHalt(pAd);
+
+	// Close kernel threads or tasklets
+	kill_thread_task(pAd);
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		MacTableReset(pAd);
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+
+	MeasureReqTabExit(pAd);
+	TpcReqTabExit(pAd);
+
+
+
+
+	// Free Ring or USB buffers
+	RTMPFreeTxRxRingMemory(pAd);
+
+	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS);
+
+#ifdef DOT11_N_SUPPORT
+	// Free BA reorder resource
+	ba_reordering_resource_release(pAd);
+#endif // DOT11_N_SUPPORT //
+
+#ifdef RT2870
+#ifdef INF_AMAZON_SE
+	if (pAd->UsbVendorReqBuf)
+		os_free_mem(pAd, pAd->UsbVendorReqBuf);
+#endif // INF_AMAZON_SE //
+#endif // RT2870 //
+
+	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_START_UP);
+
+	return 0; // close ok
+} /* End of rt28xx_close */
+
+static int rt28xx_init(IN struct net_device *net_dev)
+{
+	PRTMP_ADAPTER 			pAd = net_dev->ml_priv;
+	UINT					index;
+	UCHAR					TmpPhy;
+	NDIS_STATUS				Status;
+	UINT32 		MacCsr0 = 0;
+
+#ifdef RT2870
+#ifdef INF_AMAZON_SE
+	init_MUTEX(&(pAd->UsbVendorReq_semaphore));
+	os_alloc_mem(pAd, (PUCHAR)&pAd->UsbVendorReqBuf, MAX_PARAM_BUFFER_SIZE - 1);
+	if (pAd->UsbVendorReqBuf == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Allocate vendor request temp buffer failed!\n"));
+		goto err0;
+	}
+#endif // INF_AMAZON_SE //
+#endif // RT2870 //
+
+#ifdef DOT11_N_SUPPORT
+	// Allocate BA Reordering memory
+	ba_reordering_resource_init(pAd, MAX_REORDERING_MPDU_NUM);
+#endif // DOT11_N_SUPPORT //
+
+	// Make sure MAC gets ready.
+	index = 0;
+	do
+	{
+		RTMP_IO_READ32(pAd, MAC_CSR0, &MacCsr0);
+		pAd->MACVersion = MacCsr0;
+
+		if ((pAd->MACVersion != 0x00) && (pAd->MACVersion != 0xFFFFFFFF))
+			break;
+
+		RTMPusecDelay(10);
+	} while (index++ < 100);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("MAC_CSR0  [ Ver:Rev=0x%08x]\n", pAd->MACVersion));
+
+	// Disable DMA
+	RT28XXDMADisable(pAd);
+
+
+	// Load 8051 firmware
+	Status = NICLoadFirmware(pAd);
+	if (Status != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT_ERR(("NICLoadFirmware failed, Status[=0x%08x]\n", Status));
+		goto err1;
+	}
+
+	NICLoadRateSwitchingParams(pAd);
+
+	// Disable interrupts here which is as soon as possible
+	// This statement should never be true. We might consider to remove it later
+
+	Status = RTMPAllocTxRxRingMemory(pAd);
+	if (Status != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT_ERR(("RTMPAllocDMAMemory failed, Status[=0x%08x]\n", Status));
+		goto err1;
+	}
+
+	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE);
+
+	// initialize MLME
+	//
+
+	Status = MlmeInit(pAd);
+	if (Status != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT_ERR(("MlmeInit failed, Status[=0x%08x]\n", Status));
+		goto err2;
+	}
+
+	// Initialize pAd->StaCfg, pAd->ApCfg, pAd->CommonCfg to manufacture default
+	//
+	UserCfgInit(pAd);
+
+#ifdef RT2870
+	// We need init timerQ related structure before create the timer thread.
+	RT2870_TimerQ_Init(pAd);
+#endif // RT2870 //
+
+	RT28XX_TASK_THREAD_INIT(pAd, Status);
+	if (Status != NDIS_STATUS_SUCCESS)
+		goto err1;
+
+//	COPY_MAC_ADDR(pAd->ApCfg.MBSSID[apidx].Bssid, netif->hwaddr);
+//	pAd->bForcePrintTX = TRUE;
+
+	CfgInitHook(pAd);
+
+
+#ifdef BLOCK_NET_IF
+	initblockQueueTab(pAd);
+#endif // BLOCK_NET_IF //
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		NdisAllocateSpinLock(&pAd->MacTabLock);
+#endif // CONFIG_STA_SUPPORT //
+
+	MeasureReqTabInit(pAd);
+	TpcReqTabInit(pAd);
+
+	//
+	// Init the hardware, we need to init asic before read registry, otherwise mac register will be reset
+	//
+	Status = NICInitializeAdapter(pAd, TRUE);
+	if (Status != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT_ERR(("NICInitializeAdapter failed, Status[=0x%08x]\n", Status));
+		if (Status != NDIS_STATUS_SUCCESS)
+		goto err3;
+	}
+
+	// Read parameters from Config File
+	Status = RTMPReadParametersHook(pAd);
+
+	printk("1. Phy Mode = %d\n", pAd->CommonCfg.PhyMode);
+	if (Status != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT_ERR(("NICReadRegParameters failed, Status[=0x%08x]\n",Status));
+		goto err4;
+	}
+
+#ifdef RT2870
+	pAd->CommonCfg.bMultipleIRP = FALSE;
+
+	if (pAd->CommonCfg.bMultipleIRP)
+		pAd->CommonCfg.NumOfBulkInIRP = RX_RING_SIZE;
+	else
+		pAd->CommonCfg.NumOfBulkInIRP = 1;
+#endif // RT2870 //
+
+
+   	//Init Ba Capability parameters.
+//	RT28XX_BA_INIT(pAd);
+#ifdef DOT11_N_SUPPORT
+	pAd->CommonCfg.DesiredHtPhy.MpduDensity = (UCHAR)pAd->CommonCfg.BACapability.field.MpduDensity;
+	pAd->CommonCfg.DesiredHtPhy.AmsduEnable = (USHORT)pAd->CommonCfg.BACapability.field.AmsduEnable;
+	pAd->CommonCfg.DesiredHtPhy.AmsduSize = (USHORT)pAd->CommonCfg.BACapability.field.AmsduSize;
+	pAd->CommonCfg.DesiredHtPhy.MimoPs = (USHORT)pAd->CommonCfg.BACapability.field.MMPSmode;
+	// UPdata to HT IE
+	pAd->CommonCfg.HtCapability.HtCapInfo.MimoPs = (USHORT)pAd->CommonCfg.BACapability.field.MMPSmode;
+	pAd->CommonCfg.HtCapability.HtCapInfo.AMsduSize = (USHORT)pAd->CommonCfg.BACapability.field.AmsduSize;
+	pAd->CommonCfg.HtCapability.HtCapParm.MpduDensity = (UCHAR)pAd->CommonCfg.BACapability.field.MpduDensity;
+#endif // DOT11_N_SUPPORT //
+
+	// after reading Registry, we now know if in AP mode or STA mode
+
+	// Load 8051 firmware; crash when FW image not existent
+	// Status = NICLoadFirmware(pAd);
+	// if (Status != NDIS_STATUS_SUCCESS)
+	//    break;
+
+	printk("2. Phy Mode = %d\n", pAd->CommonCfg.PhyMode);
+
+	// We should read EEPROM for all cases.  rt2860b
+	NICReadEEPROMParameters(pAd, mac);
+#ifdef CONFIG_STA_SUPPORT
+#endif // CONFIG_STA_SUPPORT //
+
+	printk("3. Phy Mode = %d\n", pAd->CommonCfg.PhyMode);
+
+	NICInitAsicFromEEPROM(pAd); //rt2860b
+
+	// Set PHY to appropriate mode
+	TmpPhy = pAd->CommonCfg.PhyMode;
+	pAd->CommonCfg.PhyMode = 0xff;
+	RTMPSetPhyMode(pAd, TmpPhy);
+#ifdef DOT11_N_SUPPORT
+	SetCommonHT(pAd);
+#endif // DOT11_N_SUPPORT //
+
+	// No valid channels.
+	if (pAd->ChannelListNum == 0)
+	{
+		printk("Wrong configuration. No valid channel found. Check \"ContryCode\" and \"ChannelGeography\" setting.\n");
+		goto err4;
+	}
+
+#ifdef DOT11_N_SUPPORT
+	printk("MCS Set = %02x %02x %02x %02x %02x\n", pAd->CommonCfg.HtCapability.MCSSet[0],
+           pAd->CommonCfg.HtCapability.MCSSet[1], pAd->CommonCfg.HtCapability.MCSSet[2],
+           pAd->CommonCfg.HtCapability.MCSSet[3], pAd->CommonCfg.HtCapability.MCSSet[4]);
+#endif // DOT11_N_SUPPORT //
+
+#ifdef RT2870
+    //Init RT30xx RFRegisters after read RFIC type from EEPROM
+	NICInitRT30xxRFRegisters(pAd);
+#endif // RT2870 //
+
+#if 0
+	// Patch cardbus controller if EEPROM said so.
+	if (pAd->bTest1 == FALSE)
+		RTMPPatchCardBus(pAd);
+#endif
+
+
+//		APInitialize(pAd);
+
+#ifdef IKANOS_VX_1X0
+	VR_IKANOS_FP_Init(pAd->ApCfg.BssidNum, pAd->PermanentAddress);
+#endif // IKANOS_VX_1X0 //
+
+		//
+	// Initialize RF register to default value
+	//
+	AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+	AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+
+	// 8051 firmware require the signal during booting time.
+	AsicSendCommandToMcu(pAd, 0x72, 0xFF, 0x00, 0x00);
+
+	if (pAd && (Status != NDIS_STATUS_SUCCESS))
+	{
+		//
+		// Undo everything if it failed
+		//
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+		{
+//			NdisMDeregisterInterrupt(&pAd->Interrupt);
+			RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE);
+		}
+//		RTMPFreeAdapter(pAd); // we will free it in disconnect()
+	}
+	else if (pAd)
+	{
+		// Microsoft HCT require driver send a disconnect event after driver initialization.
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED);
+//		pAd->IndicateMediaState = NdisMediaStateDisconnected;
+		RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_MEDIA_STATE_CHANGE);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("NDIS_STATUS_MEDIA_DISCONNECT Event B!\n"));
+
+
+#ifdef RT2870
+		RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS);
+		RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_REMOVE_IN_PROGRESS);
+
+		//
+		// Support multiple BulkIn IRP,
+		// the value on pAd->CommonCfg.NumOfBulkInIRP may be large than 1.
+		//
+		for(index=0; index<pAd->CommonCfg.NumOfBulkInIRP; index++)
+		{
+			RTUSBBulkReceive(pAd);
+			DBGPRINT(RT_DEBUG_TRACE, ("RTUSBBulkReceive!\n" ));
+		}
+#endif // RT2870 //
+	}// end of else
+
+
+	DBGPRINT_S(Status, ("<==== RTMPInitialize, Status=%x\n", Status));
+
+	return TRUE;
+
+
+err4:
+err3:
+	MlmeHalt(pAd);
+err2:
+	RTMPFreeTxRxRingMemory(pAd);
+//	RTMPFreeAdapter(pAd);
+err1:
+
+#ifdef DOT11_N_SUPPORT
+	os_free_mem(pAd, pAd->mpdu_blk_pool.mem); // free BA pool
+#endif // DOT11_N_SUPPORT //
+	RT28XX_IRQ_RELEASE(net_dev);
+
+	// shall not set ml_priv to NULL here because the ml_priv didn't been free yet.
+	//net_dev->ml_priv = 0;
+#ifdef INF_AMAZON_SE
+err0:
+#endif // INF_AMAZON_SE //
+	printk("!!! %s Initialized fail !!!\n", RT28xx_CHIP_NAME);
+	return FALSE;
+} /* End of rt28xx_init */
+
+
+/*
+========================================================================
+Routine Description:
+    Open raxx interface.
+
+Arguments:
+	*net_dev			the raxx interface pointer
+
+Return Value:
+    0					Open OK
+	otherwise			Open Fail
+
+Note:
+========================================================================
+*/
+int rt28xx_open(IN PNET_DEV dev)
+{
+	struct net_device * net_dev = (struct net_device *)dev;
+	PRTMP_ADAPTER pAd = net_dev->ml_priv;
+	int retval = 0;
+ 	POS_COOKIE pObj;
+
+
+	// Sanity check for pAd
+	if (pAd == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -1;
+	}
+
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+	if (pAd->OpMode == OPMODE_AP)
+	{
+		CW_MAX_IN_BITS = 6;
+	}
+	else if (pAd->OpMode == OPMODE_STA)
+	{
+		CW_MAX_IN_BITS = 10;
+	}
+
+#if WIRELESS_EXT >= 12
+	if (net_dev->priv_flags == INT_MAIN)
+	{
+		if (pAd->OpMode == OPMODE_AP)
+			net_dev->wireless_handlers = (struct iw_handler_def *) &rt28xx_ap_iw_handler_def;
+		else if (pAd->OpMode == OPMODE_STA)
+			net_dev->wireless_handlers = (struct iw_handler_def *) &rt28xx_iw_handler_def;
+	}
+#endif // WIRELESS_EXT >= 12 //
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+#endif // CONFIG_STA_SUPPORT //
+
+	// Init
+ 	pObj = (POS_COOKIE)pAd->OS_Cookie;
+
+	// reset Adapter flags
+	RTMP_CLEAR_FLAGS(pAd);
+
+	// Request interrupt service routine for PCI device
+	// register the interrupt routine with the os
+	RT28XX_IRQ_REQUEST(net_dev);
+
+
+	// Init BssTab & ChannelInfo tabbles for auto channel select.
+
+
+	// Chip & other init
+	if (rt28xx_init(net_dev) == FALSE)
+		goto err;
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		NdisZeroMemory(pAd->StaCfg.dev_name, 16);
+		NdisMoveMemory(pAd->StaCfg.dev_name, net_dev->name, strlen(net_dev->name));
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	// Set up the Mac address
+	NdisMoveMemory(net_dev->dev_addr, (void *) pAd->CurrentAddress, 6);
+
+	// Init IRQ parameters
+	RT28XX_IRQ_INIT(pAd);
+
+	// Various AP function init
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+		{
+			union iwreq_data    wrqu;
+			// send wireless event to wpa_supplicant for infroming interface down.
+			memset(&wrqu, 0, sizeof(wrqu));
+			wrqu.data.flags = RT_INTERFACE_UP;
+			wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+		}
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	// Enable Interrupt
+	RT28XX_IRQ_ENABLE(pAd);
+
+	// Now Enable RxTx
+	RTMPEnableRxTx(pAd);
+	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_START_UP);
+
+	{
+	UINT32 reg = 0;
+	RTMP_IO_READ32(pAd, 0x1300, &reg);  // clear garbage interrupts
+	printk("0x1300 = %08x\n", reg);
+	}
+
+	{
+//	u32 reg;
+//	u8  byte;
+//	u16 tmp;
+
+//	RTMP_IO_READ32(pAd, XIFS_TIME_CFG, &reg);
+
+//	tmp = 0x0805;
+//	reg  = (reg & 0xffff0000) | tmp;
+//	RTMP_IO_WRITE32(pAd, XIFS_TIME_CFG, reg);
+
+	}
+
+#if 0
+	/*
+	 * debugging helper
+	 * 		show the size of main table in Adapter structure
+	 *		MacTab  -- 185K
+	 *		BATable -- 137K
+	 * 		Total 	-- 385K  !!!!! (5/26/2006)
+	 */
+	printk("sizeof(pAd->MacTab) = %ld\n", sizeof(pAd->MacTab));
+	printk("sizeof(pAd->AccessControlList) = %ld\n", sizeof(pAd->AccessControlList));
+	printk("sizeof(pAd->ApCfg) = %ld\n", sizeof(pAd->ApCfg));
+	printk("sizeof(pAd->BATable) = %ld\n", sizeof(pAd->BATable));
+	BUG();
+#endif
+
+#ifdef CONFIG_STA_SUPPORT
+#endif // CONFIG_STA_SUPPORT //
+
+	return (retval);
+
+err:
+	return (-1);
+} /* End of rt28xx_open */
+
+
+/* Must not be called for mdev and apdev */
+static NDIS_STATUS rt_ieee80211_if_setup(struct net_device *dev, PRTMP_ADAPTER pAd)
+{
+	NDIS_STATUS Status;
+	INT     i=0;
+	CHAR    slot_name[IFNAMSIZ];
+	struct net_device   *device;
+
+
+	//ether_setup(dev);
+	dev->hard_start_xmit = rt28xx_send_packets;
+
+#ifdef IKANOS_VX_1X0
+	dev->hard_start_xmit = IKANOS_DataFramesTx;
+#endif // IKANOS_VX_1X0 //
+
+//	dev->set_multicast_list = ieee80211_set_multicast_list;
+//	dev->change_mtu = ieee80211_change_mtu;
+#ifdef CONFIG_STA_SUPPORT
+#if WIRELESS_EXT >= 12
+	if (pAd->OpMode == OPMODE_STA)
+	{
+		dev->wireless_handlers = &rt28xx_iw_handler_def;
+	}
+#endif //WIRELESS_EXT >= 12
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+#if WIRELESS_EXT >= 12
+	if (pAd->OpMode == OPMODE_AP)
+	{
+		dev->wireless_handlers = &rt28xx_ap_iw_handler_def;
+	}
+#endif //WIRELESS_EXT >= 12
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+
+#if WIRELESS_EXT < 21
+		dev->get_wireless_stats = rt28xx_get_wireless_stats;
+#endif
+	dev->get_stats = RT28xx_get_ether_stats;
+	dev->open = MainVirtualIF_open; //rt28xx_open;
+	dev->stop = MainVirtualIF_close; //rt28xx_close;
+//	dev->uninit = ieee80211_if_reinit;
+//	dev->destructor = ieee80211_if_free;
+	dev->priv_flags = INT_MAIN;
+	dev->do_ioctl = rt28xx_ioctl;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,24)
+    dev->validate_addr = NULL;
+#endif
+	// find available device name
+	for (i = 0; i < 8; i++)
+	{
+#ifdef MULTIPLE_CARD_SUPPORT
+		if (pAd->MC_RowID >= 0)
+			sprintf(slot_name, "ra%02d_%d", pAd->MC_RowID, i);
+		else
+#endif // MULTIPLE_CARD_SUPPORT //
+		sprintf(slot_name, "ra%d", i);
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,24)
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,26)
+        device = dev_get_by_name(dev_net(dev), slot_name);
+#else
+        device = dev_get_by_name(dev->nd_net, slot_name);
+#endif
+#else
+		device = dev_get_by_name(slot_name);
+#endif
+		if (device != NULL) dev_put(device);
+#else
+		for (device = dev_base; device != NULL; device = device->next)
+		{
+			if (strncmp(device->name, slot_name, 4) == 0)
+				break;
+		}
+#endif
+		if(device == NULL)
+			break;
+	}
+
+	if(i == 8)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("No available slot name\n"));
+		Status = NDIS_STATUS_FAILURE;
+	}
+	else
+	{
+#ifdef MULTIPLE_CARD_SUPPORT
+		if (pAd->MC_RowID >= 0)
+	        sprintf(dev->name, "ra%02d_%d", pAd->MC_RowID, i);
+		else
+#endif // MULTIPLE_CARD_SUPPORT //
+		sprintf(dev->name, "ra%d", i);
+		Status = NDIS_STATUS_SUCCESS;
+	}
+
+	return Status;
+
+}
+
+
+#ifdef MULTIPLE_CARD_SUPPORT
+/*
+========================================================================
+Routine Description:
+    Get card profile path.
+
+Arguments:
+    pAd
+
+Return Value:
+    TRUE		- Find a card profile
+	FALSE		- use default profile
+
+Note:
+========================================================================
+*/
+extern INT RTMPGetKeyParameter(
+    IN  PCHAR   key,
+    OUT PCHAR   dest,
+    IN  INT     destsize,
+    IN  PCHAR   buffer);
+
+BOOLEAN RTMP_CardInfoRead(
+	IN	PRTMP_ADAPTER pAd)
+{
+#define MC_SELECT_CARDID		0	/* use CARD ID (0 ~ 31) to identify different cards */
+#define MC_SELECT_MAC			1	/* use CARD MAC to identify different cards */
+#define MC_SELECT_CARDTYPE		2	/* use CARD type (abgn or bgn) to identify different cards */
+
+#define LETTER_CASE_TRANSLATE(txt_p, card_id)			\
+	{	UINT32 _len; char _char;						\
+		for(_len=0; _len<strlen(card_id); _len++) {		\
+			_char = *(txt_p + _len);					\
+			if (('A' <= _char) && (_char <= 'Z'))		\
+				*(txt_p+_len) = 'a'+(_char-'A');		\
+		} }
+
+	struct file *srcf;
+	INT retval, orgfsuid, orgfsgid;
+   	mm_segment_t orgfs;
+	CHAR *buffer, *tmpbuf, card_id_buf[30], RFIC_word[30];
+	BOOLEAN flg_match_ok = FALSE;
+	INT32 card_select_method;
+	INT32 card_free_id, card_nouse_id, card_same_mac_id, card_match_id;
+	EEPROM_ANTENNA_STRUC antenna;
+	USHORT addr01, addr23, addr45;
+	UINT8 mac[6];
+	UINT32 data, card_index;
+	UCHAR *start_ptr;
+
+
+	// init
+	buffer = kmalloc(MAX_INI_BUFFER_SIZE, MEM_ALLOC_FLAG);
+	if (buffer == NULL)
+        return FALSE;
+
+	tmpbuf = kmalloc(MAX_PARAM_BUFFER_SIZE, MEM_ALLOC_FLAG);
+	if(tmpbuf == NULL)
+	{
+		kfree(buffer);
+        return NDIS_STATUS_FAILURE;
+	}
+
+	orgfsuid = current->fsuid;
+	orgfsgid = current->fsgid;
+	current->fsuid = current->fsgid = 0;
+    orgfs = get_fs();
+    set_fs(KERNEL_DS);
+
+	// get RF IC type
+	RTMP_IO_READ32(pAd, E2PROM_CSR, &data);
+
+	if ((data & 0x30) == 0)
+		pAd->EEPROMAddressNum = 6;	// 93C46
+	else if ((data & 0x30) == 0x10)
+		pAd->EEPROMAddressNum = 8;	// 93C66
+	else
+		pAd->EEPROMAddressNum = 8;	// 93C86
+
+	//antenna.word = RTMP_EEPROM_READ16(pAd, EEPROM_NIC1_OFFSET);
+	RT28xx_EEPROM_READ16(pAd, EEPROM_NIC1_OFFSET, antenna.word);
+
+	if ((antenna.field.RfIcType == RFIC_2850) ||
+		(antenna.field.RfIcType == RFIC_2750))
+	{
+		/* ABGN card */
+		strcpy(RFIC_word, "abgn");
+	}
+	else
+	{
+		/* BGN card */
+		strcpy(RFIC_word, "bgn");
+	}
+
+	// get MAC address
+	//addr01 = RTMP_EEPROM_READ16(pAd, 0x04);
+	//addr23 = RTMP_EEPROM_READ16(pAd, 0x06);
+	//addr45 = RTMP_EEPROM_READ16(pAd, 0x08);
+	RT28xx_EEPROM_READ16(pAd, 0x04, addr01);
+	RT28xx_EEPROM_READ16(pAd, 0x06, addr23);
+	RT28xx_EEPROM_READ16(pAd, 0x08, addr45);
+
+	mac[0] = (UCHAR)(addr01 & 0xff);
+	mac[1] = (UCHAR)(addr01 >> 8);
+	mac[2] = (UCHAR)(addr23 & 0xff);
+	mac[3] = (UCHAR)(addr23 >> 8);
+	mac[4] = (UCHAR)(addr45 & 0xff);
+	mac[5] = (UCHAR)(addr45 >> 8);
+
+	// open card information file
+	srcf = filp_open(CARD_INFO_PATH, O_RDONLY, 0);
+	if (IS_ERR(srcf))
+	{
+		/* card information file does not exist */
+			DBGPRINT(RT_DEBUG_TRACE,
+				("--> Error %ld opening %s\n", -PTR_ERR(srcf), CARD_INFO_PATH));
+		return FALSE;
+	}
+
+	if (srcf->f_op && srcf->f_op->read)
+	{
+		/* card information file exists so reading the card information */
+		memset(buffer, 0x00, MAX_INI_BUFFER_SIZE);
+		retval = srcf->f_op->read(srcf, buffer, MAX_INI_BUFFER_SIZE, &srcf->f_pos);
+		if (retval < 0)
+		{
+			/* read fail */
+				DBGPRINT(RT_DEBUG_TRACE,
+					("--> Read %s error %d\n", CARD_INFO_PATH, -retval));
+		}
+		else
+		{
+			/* get card selection method */
+			memset(tmpbuf, 0x00, MAX_PARAM_BUFFER_SIZE);
+			card_select_method = MC_SELECT_CARDTYPE; // default
+
+			if (RTMPGetKeyParameter("SELECT", tmpbuf, 256, buffer))
+			{
+				if (strcmp(tmpbuf, "CARDID") == 0)
+					card_select_method = MC_SELECT_CARDID;
+				else if (strcmp(tmpbuf, "MAC") == 0)
+					card_select_method = MC_SELECT_MAC;
+				else if (strcmp(tmpbuf, "CARDTYPE") == 0)
+					card_select_method = MC_SELECT_CARDTYPE;
+			}
+
+			DBGPRINT(RT_DEBUG_TRACE,
+					("MC> Card Selection = %d\n", card_select_method));
+
+			// init
+			card_free_id = -1;
+			card_nouse_id = -1;
+			card_same_mac_id = -1;
+			card_match_id = -1;
+
+			// search current card information records
+			for(card_index=0;
+				card_index<MAX_NUM_OF_MULTIPLE_CARD;
+				card_index++)
+			{
+				if ((*(UINT32 *)&MC_CardMac[card_index][0] == 0) &&
+					(*(UINT16 *)&MC_CardMac[card_index][4] == 0))
+				{
+					// MAC is all-0 so the entry is available
+					MC_CardUsed[card_index] = 0;
+
+					if (card_free_id < 0)
+						card_free_id = card_index; // 1st free entry
+				}
+				else
+				{
+					if (memcmp(MC_CardMac[card_index], mac, 6) == 0)
+					{
+						// we find the entry with same MAC
+						if (card_same_mac_id < 0)
+							card_same_mac_id = card_index; // 1st same entry
+					}
+					else
+					{
+						// MAC is not all-0 but used flag == 0
+						if ((MC_CardUsed[card_index] == 0) &&
+							(card_nouse_id < 0))
+						{
+							card_nouse_id = card_index; // 1st available entry
+						}
+					}
+				}
+			}
+
+			DBGPRINT(RT_DEBUG_TRACE,
+					("MC> Free = %d, Same = %d, NOUSE = %d\n",
+					card_free_id, card_same_mac_id, card_nouse_id));
+
+			if ((card_same_mac_id >= 0) &&
+				((card_select_method == MC_SELECT_CARDID) ||
+				(card_select_method == MC_SELECT_CARDTYPE)))
+			{
+				// same MAC entry is found
+				card_match_id = card_same_mac_id;
+
+				if (card_select_method == MC_SELECT_CARDTYPE)
+				{
+					// for CARDTYPE
+					sprintf(card_id_buf, "%02dCARDTYPE%s",
+							card_match_id, RFIC_word);
+
+					if ((start_ptr=rtstrstruncasecmp(buffer, card_id_buf)) != NULL)
+					{
+						// we found the card ID
+						LETTER_CASE_TRANSLATE(start_ptr, card_id_buf);
+					}
+				}
+			}
+			else
+			{
+				// the card is 1st plug-in, try to find the match card profile
+				switch(card_select_method)
+				{
+					case MC_SELECT_CARDID: // CARDID
+					default:
+						if (card_free_id >= 0)
+							card_match_id = card_free_id;
+						else
+							card_match_id = card_nouse_id;
+						break;
+
+					case MC_SELECT_MAC: // MAC
+						sprintf(card_id_buf, "MAC%02x:%02x:%02x:%02x:%02x:%02x",
+								mac[0], mac[1], mac[2],
+								mac[3], mac[4], mac[5]);
+
+						/* try to find the key word in the card file */
+						if ((start_ptr=rtstrstruncasecmp(buffer, card_id_buf)) != NULL)
+						{
+							LETTER_CASE_TRANSLATE(start_ptr, card_id_buf);
+
+							/* get the row ID (2 ASCII characters) */
+							start_ptr -= 2;
+							card_id_buf[0] = *(start_ptr);
+							card_id_buf[1] = *(start_ptr+1);
+							card_id_buf[2] = 0x00;
+
+							card_match_id = simple_strtol(card_id_buf, 0, 10);
+						}
+						break;
+
+					case MC_SELECT_CARDTYPE: // CARDTYPE
+						card_nouse_id = -1;
+
+						for(card_index=0;
+							card_index<MAX_NUM_OF_MULTIPLE_CARD;
+							card_index++)
+						{
+							sprintf(card_id_buf, "%02dCARDTYPE%s",
+									card_index, RFIC_word);
+
+							if ((start_ptr=rtstrstruncasecmp(buffer,
+														card_id_buf)) != NULL)
+							{
+								LETTER_CASE_TRANSLATE(start_ptr, card_id_buf);
+
+								if (MC_CardUsed[card_index] == 0)
+								{
+									/* current the card profile is not used */
+									if ((*(UINT32 *)&MC_CardMac[card_index][0] == 0) &&
+										(*(UINT16 *)&MC_CardMac[card_index][4] == 0))
+									{
+										// find it and no previous card use it
+										card_match_id = card_index;
+										break;
+									}
+									else
+									{
+										// ever a card use it
+										if (card_nouse_id < 0)
+											card_nouse_id = card_index;
+									}
+								}
+							}
+						}
+
+						// if not find a free one, use the available one
+						if (card_match_id < 0)
+							card_match_id = card_nouse_id;
+						break;
+				}
+			}
+
+			if (card_match_id >= 0)
+			{
+				// make up search keyword
+				switch(card_select_method)
+				{
+					case MC_SELECT_CARDID: // CARDID
+						sprintf(card_id_buf, "%02dCARDID", card_match_id);
+						break;
+
+					case MC_SELECT_MAC: // MAC
+						sprintf(card_id_buf,
+								"%02dmac%02x:%02x:%02x:%02x:%02x:%02x",
+								card_match_id,
+								mac[0], mac[1], mac[2],
+								mac[3], mac[4], mac[5]);
+						break;
+
+					case MC_SELECT_CARDTYPE: // CARDTYPE
+					default:
+						sprintf(card_id_buf, "%02dcardtype%s",
+								card_match_id, RFIC_word);
+						break;
+				}
+
+				DBGPRINT(RT_DEBUG_TRACE, ("Search Keyword = %s\n", card_id_buf));
+
+				// read card file path
+				if (RTMPGetKeyParameter(card_id_buf, tmpbuf, 256, buffer))
+				{
+					if (strlen(tmpbuf) < sizeof(pAd->MC_FileName))
+					{
+						// backup card information
+						pAd->MC_RowID = card_match_id; /* base 0 */
+						MC_CardUsed[card_match_id] = 1;
+						memcpy(MC_CardMac[card_match_id], mac, sizeof(mac));
+
+						// backup card file path
+						NdisMoveMemory(pAd->MC_FileName, tmpbuf , strlen(tmpbuf));
+						pAd->MC_FileName[strlen(tmpbuf)] = '\0';
+						flg_match_ok = TRUE;
+
+						DBGPRINT(RT_DEBUG_TRACE,
+								("Card Profile Name = %s\n", pAd->MC_FileName));
+					}
+					else
+					{
+						DBGPRINT(RT_DEBUG_ERROR,
+								("Card Profile Name length too large!\n"));
+					}
+				}
+				else
+				{
+					DBGPRINT(RT_DEBUG_ERROR,
+							("Can not find search key word in card.dat!\n"));
+				}
+
+				if ((flg_match_ok != TRUE) &&
+					(card_match_id < MAX_NUM_OF_MULTIPLE_CARD))
+				{
+					MC_CardUsed[card_match_id] = 0;
+					memset(MC_CardMac[card_match_id], 0, sizeof(mac));
+				}
+			} // if (card_match_id >= 0)
+		}
+	}
+
+	// close file
+	retval = filp_close(srcf, NULL);
+	set_fs(orgfs);
+	current->fsuid = orgfsuid;
+	current->fsgid = orgfsgid;
+	kfree(buffer);
+	kfree(tmpbuf);
+	return flg_match_ok;
+}
+#endif // MULTIPLE_CARD_SUPPORT //
+
+
+/*
+========================================================================
+Routine Description:
+    Probe RT28XX chipset.
+
+Arguments:
+    _dev_p				Point to the PCI or USB device
+	_dev_id_p			Point to the PCI or USB device ID
+
+Return Value:
+    0					Probe OK
+	-ENODEV				Probe Fail
+
+Note:
+========================================================================
+*/
+INT __devinit   rt28xx_probe(
+    IN  void *_dev_p,
+    IN  void *_dev_id_p,
+	IN  UINT argc,
+	OUT PRTMP_ADAPTER *ppAd)
+{
+    struct  net_device	*net_dev;
+    PRTMP_ADAPTER       pAd = (PRTMP_ADAPTER) NULL;
+    INT                 status;
+	PVOID				handle;
+#ifdef RT2870
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)	/* kernel 2.4 series */
+	struct usb_device *dev_p = (struct usb_device *)_dev_p;
+#else
+	struct usb_interface *intf = (struct usb_interface *)_dev_p;
+	struct usb_device *dev_p = interface_to_usbdev(intf);
+
+	dev_p = usb_get_dev(dev_p);
+#endif // LINUX_VERSION_CODE //
+#endif // RT2870 //
+
+
+#ifdef CONFIG_STA_SUPPORT
+    DBGPRINT(RT_DEBUG_TRACE, ("STA Driver version-%s\n", STA_DRIVER_VERSION));
+#endif // CONFIG_STA_SUPPORT //
+
+	// Check chipset vendor/product ID
+//	if (RT28XXChipsetCheck(_dev_p) == FALSE)
+//		goto err_out;
+
+#if LINUX_VERSION_CODE <= 0x20402       // Red Hat 7.1
+    net_dev = alloc_netdev(sizeof(PRTMP_ADAPTER), "eth%d", ether_setup);
+#else
+    net_dev = alloc_etherdev(sizeof(PRTMP_ADAPTER));
+#endif
+    if (net_dev == NULL)
+    {
+        printk("alloc_netdev failed\n");
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,15)
+		module_put(THIS_MODULE);
+#endif //LINUX_VERSION_CODE < KERNEL_VERSION(2,6,15)
+#else
+		MOD_DEC_USE_COUNT;
+#endif
+        goto err_out;
+    }
+
+// sample
+//	if (rt_ieee80211_if_setup(net_dev) != NDIS_STATUS_SUCCESS)
+//		goto err_out;
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,24)
+    SET_MODULE_OWNER(net_dev);
+#endif
+
+	netif_stop_queue(net_dev);
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+/* for supporting Network Manager */
+/* Set the sysfs physical device reference for the network logical device
+ * if set prior to registration will cause a symlink during initialization.
+ */
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0))
+    SET_NETDEV_DEV(net_dev, &(dev_p->dev));
+#endif
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+
+	// Allocate RTMP_ADAPTER miniport adapter structure
+	handle = kmalloc(sizeof(struct os_cookie), GFP_KERNEL);
+	RT28XX_HANDLE_DEV_ASSIGN(handle, dev_p);
+
+	status = RTMPAllocAdapterBlock(handle, &pAd);
+	if (status != NDIS_STATUS_SUCCESS)
+		goto err_out_free_netdev;
+
+	net_dev->ml_priv = (PVOID)pAd;
+    pAd->net_dev = net_dev; // must be before RT28XXNetDevInit()
+
+	RT28XXNetDevInit(_dev_p, net_dev, pAd);
+
+#ifdef CONFIG_STA_SUPPORT
+    pAd->StaCfg.OriDevType = net_dev->type;
+#endif // CONFIG_STA_SUPPORT //
+
+	// Find and assign a free interface name, raxx
+//	RT28XXAvailRANameAssign(net_dev->name);
+
+	// Post config
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
+	if (RT28XXProbePostConfig(_dev_p, pAd, argc) == FALSE)
+		goto err_out_unmap;
+#else
+	if (RT28XXProbePostConfig(_dev_p, pAd, 0) == FALSE)
+		goto err_out_unmap;
+#endif // LINUX_VERSION_CODE //
+
+#ifdef CONFIG_STA_SUPPORT
+	pAd->OpMode = OPMODE_STA;
+#endif // CONFIG_STA_SUPPORT //
+
+
+#ifdef MULTIPLE_CARD_SUPPORT
+	// find its profile path
+	pAd->MC_RowID = -1; // use default profile path
+	RTMP_CardInfoRead(pAd);
+
+	if (pAd->MC_RowID == -1)
+#ifdef CONFIG_STA_SUPPORT
+		strcpy(pAd->MC_FileName, STA_PROFILE_PATH);
+#endif // CONFIG_STA_SUPPORT //
+
+	DBGPRINT(RT_DEBUG_TRACE,
+			("MC> ROW = %d, PATH = %s\n", pAd->MC_RowID, pAd->MC_FileName));
+#endif // MULTIPLE_CARD_SUPPORT //
+
+	// sample move
+	if (rt_ieee80211_if_setup(net_dev, pAd) != NDIS_STATUS_SUCCESS)
+		goto err_out_unmap;
+
+    // Register this device
+    status = register_netdev(net_dev);
+    if (status)
+        goto err_out_unmap;
+
+    // Set driver data
+	RT28XX_DRVDATA_SET(_dev_p);
+
+
+
+	*ppAd = pAd;
+    return 0; // probe ok
+
+
+	/* --------------------------- ERROR HANDLE --------------------------- */
+err_out_unmap:
+	RTMPFreeAdapter(pAd);
+	RT28XX_UNMAP();
+
+err_out_free_netdev:
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0)
+    free_netdev(net_dev);
+#else
+	kfree(net_dev);
+#endif
+
+err_out:
+	RT28XX_PUT_DEVICE(dev_p);
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
+	return (LONG)NULL;
+#else
+    return -ENODEV; /* probe fail */
+#endif // LINUX_VERSION_CODE //
+} /* End of rt28xx_probe */
+
+
+/*
+========================================================================
+Routine Description:
+    The entry point for Linux kernel sent packet to our driver.
+
+Arguments:
+    sk_buff *skb		the pointer refer to a sk_buffer.
+
+Return Value:
+    0
+
+Note:
+	This function is the entry point of Tx Path for Os delivery packet to
+	our driver. You only can put OS-depened & STA/AP common handle procedures
+	in here.
+========================================================================
+*/
+int rt28xx_packet_xmit(struct sk_buff *skb)
+{
+	struct net_device *net_dev = skb->dev;
+	PRTMP_ADAPTER pAd = net_dev->ml_priv;
+	int status = 0;
+	PNDIS_PACKET pPacket = (PNDIS_PACKET) skb;
+
+	/* RT2870STA does this in RTMPSendPackets() */
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+	{
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_RESOURCES);
+		return 0;
+	}
+#endif // RALINK_ATE //
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		// Drop send request since we are in monitor mode
+		if (MONITOR_ON(pAd))
+		{
+			RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+			goto done;
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+        // EapolStart size is 18
+	if (skb->len < 14)
+	{
+		//printk("bad packet size: %d\n", pkt->len);
+		hex_dump("bad packet", skb->data, skb->len);
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+		goto done;
+	}
+
+#if 0
+//	if ((pkt->data[0] & 0x1) == 0)
+	{
+		//hex_dump(__FUNCTION__, pkt->data, pkt->len);
+		printk("pPacket = %x\n", pPacket);
+	}
+#endif
+
+	RTMP_SET_PACKET_5VT(pPacket, 0);
+//	MiniportMMRequest(pAd, pkt->data, pkt->len);
+#ifdef CONFIG_5VT_ENHANCE
+    if (*(int*)(skb->cb) == BRIDGE_TAG) {
+		RTMP_SET_PACKET_5VT(pPacket, 1);
+    }
+#endif
+
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+
+		STASendPackets((NDIS_HANDLE)pAd, (PPNDIS_PACKET) &pPacket, 1);
+	}
+
+#endif // CONFIG_STA_SUPPORT //
+
+	status = 0;
+done:
+
+	return status;
+}
+
+
+/*
+========================================================================
+Routine Description:
+    Send a packet to WLAN.
+
+Arguments:
+    skb_p           points to our adapter
+    dev_p           which WLAN network interface
+
+Return Value:
+    0: transmit successfully
+    otherwise: transmit fail
+
+Note:
+========================================================================
+*/
+INT rt28xx_send_packets(
+	IN struct sk_buff 		*skb_p,
+	IN struct net_device 	*net_dev)
+{
+    RTMP_ADAPTER *pAd = net_dev->ml_priv;
+
+	if (!(net_dev->flags & IFF_UP))
+	{
+		RELEASE_NDIS_PACKET(pAd, (PNDIS_PACKET)skb_p, NDIS_STATUS_FAILURE);
+		return 0;
+	}
+
+	NdisZeroMemory((PUCHAR)&skb_p->cb[CB_OFF], 15);
+	RTMP_SET_PACKET_NET_DEVICE_MBSSID(skb_p, MAIN_MBSSID);
+
+	return rt28xx_packet_xmit(skb_p);
+} /* End of MBSS_VirtualIF_PacketSend */
+
+
+
+
+#if LINUX_VERSION_CODE <= 0x20402	// Red Hat 7.1
+//static struct net_device *alloc_netdev(int sizeof_priv, const char *mask, void (*setup)(struct net_device *)) //sample
+struct net_device *alloc_netdev(
+	int sizeof_priv,
+	const char *mask,
+	void (*setup)(struct net_device *))
+{
+    struct net_device	*dev;
+    INT					alloc_size;
+
+
+    /* ensure 32-byte alignment of the private area */
+    alloc_size = sizeof (*dev) + sizeof_priv + 31;
+
+    dev = (struct net_device *) kmalloc(alloc_size, GFP_KERNEL);
+    if (dev == NULL)
+    {
+        DBGPRINT(RT_DEBUG_ERROR,
+				("alloc_netdev: Unable to allocate device memory.\n"));
+        return NULL;
+    }
+
+    memset(dev, 0, alloc_size);
+
+    if (sizeof_priv)
+        dev->priv = (void *) (((long)(dev + 1) + 31) & ~31);
+
+    setup(dev);
+    strcpy(dev->name, mask);
+
+    return dev;
+}
+#endif // LINUX_VERSION_CODE //
+
+
+void CfgInitHook(PRTMP_ADAPTER pAd)
+{
+	pAd->bBroadComHT = TRUE;
+} /* End of CfgInitHook */
+
+
+#if 0	// Not used now, should keep it in our source tree??
+/*
+========================================================================
+Routine Description:
+    Find and assign a free interface name (raxx).
+
+Arguments:
+    *name_p				the interface name pointer
+
+Return Value:
+	TRUE				OK
+	FALSE				FAIL
+
+Note:
+========================================================================
+*/
+static BOOLEAN RT28XXAvailRANameAssign(
+	IN CHAR			*name_p)
+{
+    CHAR				slot_name[IFNAMSIZ];
+    struct net_device	*device;
+	UINT32				if_id;
+
+
+    for(if_id=0; if_id<8; if_id++)
+    {
+        sprintf(slot_name, "ra%d", if_id);
+
+        for(device=dev_base; device!=NULL; device=device->next)
+        {
+            if (strncmp(device->name, slot_name, 4) == 0)
+                break;
+        }
+
+        if (device == NULL)
+			break;
+    }
+
+    if (if_id == 8)
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("No available slot name\n"));
+        return FALSE;
+    }
+
+    sprintf(name_p, "ra%d", if_id);
+	return TRUE;
+} /* End of RT28XXAvailRANameAssign */
+#endif
+
+#if WIRELESS_EXT >= 12
+// This function will be called when query /proc
+struct iw_statistics *rt28xx_get_wireless_stats(
+    IN struct net_device *net_dev)
+{
+	PRTMP_ADAPTER pAd = net_dev->ml_priv;
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("rt28xx_get_wireless_stats --->\n"));
+
+	pAd->iw_stats.status = 0; // Status - device dependent for now
+
+	// link quality
+	pAd->iw_stats.qual.qual = ((pAd->Mlme.ChannelQuality * 12)/10 + 10);
+	if(pAd->iw_stats.qual.qual > 100)
+		pAd->iw_stats.qual.qual = 100;
+
+#ifdef CONFIG_STA_SUPPORT
+	if (pAd->OpMode == OPMODE_STA)
+		pAd->iw_stats.qual.level = RTMPMaxRssi(pAd, pAd->StaCfg.RssiSample.LastRssi0, pAd->StaCfg.RssiSample.LastRssi1, pAd->StaCfg.RssiSample.LastRssi2);
+#endif // CONFIG_STA_SUPPORT //
+
+	pAd->iw_stats.qual.noise = pAd->BbpWriteLatch[66]; // noise level (dBm)
+
+	pAd->iw_stats.qual.noise += 256 - 143;
+	pAd->iw_stats.qual.updated = 1;     // Flags to know if updated
+#ifdef IW_QUAL_DBM
+	pAd->iw_stats.qual.updated |= IW_QUAL_DBM;	// Level + Noise are dBm
+#endif // IW_QUAL_DBM //
+
+	pAd->iw_stats.discard.nwid = 0;     // Rx : Wrong nwid/essid
+	pAd->iw_stats.miss.beacon = 0;      // Missed beacons/superframe
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<--- rt28xx_get_wireless_stats\n"));
+	return &pAd->iw_stats;
+} /* End of rt28xx_get_wireless_stats */
+#endif // WIRELESS_EXT //
+
+
+
+void tbtt_tasklet(unsigned long data)
+{
+#define MAX_TX_IN_TBTT		(16)
+
+}
+
+INT rt28xx_ioctl(
+	IN	struct net_device	*net_dev,
+	IN	OUT	struct ifreq	*rq,
+	IN	INT					cmd)
+{
+	VIRTUAL_ADAPTER	*pVirtualAd = NULL;
+	RTMP_ADAPTER	*pAd = NULL;
+	INT				ret = 0;
+
+	if (net_dev->priv_flags == INT_MAIN)
+	{
+		pAd = net_dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = net_dev->ml_priv;
+		pAd = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAd == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+	{
+		ret = rt28xx_sta_ioctl(net_dev, rq, cmd);
+	}
+#endif // CONFIG_STA_SUPPORT //
+
+	return ret;
+}
+
+/*
+    ========================================================================
+
+    Routine Description:
+        return ethernet statistics counter
+
+    Arguments:
+        net_dev                     Pointer to net_device
+
+    Return Value:
+        net_device_stats*
+
+    Note:
+
+    ========================================================================
+*/
+struct net_device_stats *RT28xx_get_ether_stats(
+    IN  struct net_device *net_dev)
+{
+    RTMP_ADAPTER *pAd = NULL;
+
+	if (net_dev)
+		pAd = net_dev->ml_priv;
+
+	if (pAd)
+	{
+
+		pAd->stats.rx_packets = pAd->WlanCounters.ReceivedFragmentCount.QuadPart;
+		pAd->stats.tx_packets = pAd->WlanCounters.TransmittedFragmentCount.QuadPart;
+
+		pAd->stats.rx_bytes = pAd->RalinkCounters.ReceivedByteCount;
+		pAd->stats.tx_bytes = pAd->RalinkCounters.TransmittedByteCount;
+
+		pAd->stats.rx_errors = pAd->Counters8023.RxErrors;
+		pAd->stats.tx_errors = pAd->Counters8023.TxErrors;
+
+		pAd->stats.rx_dropped = 0;
+		pAd->stats.tx_dropped = 0;
+
+	    pAd->stats.multicast = pAd->WlanCounters.MulticastReceivedFrameCount.QuadPart;   // multicast packets received
+	    pAd->stats.collisions = pAd->Counters8023.OneCollision + pAd->Counters8023.MoreCollisions;  // Collision packets
+
+	    pAd->stats.rx_length_errors = 0;
+	    pAd->stats.rx_over_errors = pAd->Counters8023.RxNoBuffer;                   // receiver ring buff overflow
+	    pAd->stats.rx_crc_errors = 0;//pAd->WlanCounters.FCSErrorCount;     // recved pkt with crc error
+	    pAd->stats.rx_frame_errors = pAd->Counters8023.RcvAlignmentErrors;          // recv'd frame alignment error
+	    pAd->stats.rx_fifo_errors = pAd->Counters8023.RxNoBuffer;                   // recv'r fifo overrun
+	    pAd->stats.rx_missed_errors = 0;                                            // receiver missed packet
+
+	    // detailed tx_errors
+	    pAd->stats.tx_aborted_errors = 0;
+	    pAd->stats.tx_carrier_errors = 0;
+	    pAd->stats.tx_fifo_errors = 0;
+	    pAd->stats.tx_heartbeat_errors = 0;
+	    pAd->stats.tx_window_errors = 0;
+
+	    // for cslip etc
+	    pAd->stats.rx_compressed = 0;
+	    pAd->stats.tx_compressed = 0;
+
+		return &pAd->stats;
+	}
+	else
+    	return NULL;
+}
+
diff --git a/drivers/staging/rt2870/rt_profile.c b/drivers/staging/rt2870/rt_profile.c
new file mode 100644
index 00000000000..c4474a6428a
--- /dev/null
+++ b/drivers/staging/rt2870/rt_profile.c
@@ -0,0 +1,2020 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#include "rt_config.h"
+
+#ifdef DOT11_N_SUPPORT
+static void HTParametersHook(
+	IN	PRTMP_ADAPTER pAd,
+	IN	CHAR		  *pValueStr,
+	IN	CHAR		  *pInput);
+#endif // DOT11_N_SUPPORT //
+
+#define ETH_MAC_ADDR_STR_LEN 17  // in format of xx:xx:xx:xx:xx:xx
+
+// We assume the s1 is a sting, s2 is a memory space with 6 bytes. and content of s1 will be changed.
+BOOLEAN rtstrmactohex(char *s1, char *s2)
+{
+	int i = 0;
+	char *ptokS = s1, *ptokE = s1;
+
+	if (strlen(s1) != ETH_MAC_ADDR_STR_LEN)
+		return FALSE;
+
+	while((*ptokS) != '\0')
+	{
+		if((ptokE = strchr(ptokS, ':')) != NULL)
+			*ptokE++ = '\0';
+		if ((strlen(ptokS) != 2) || (!isxdigit(*ptokS)) || (!isxdigit(*(ptokS+1))))
+			break; // fail
+		AtoH(ptokS, &s2[i++], 1);
+		ptokS = ptokE;
+		if (i == 6)
+			break; // parsing finished
+	}
+
+	return ( i == 6 ? TRUE : FALSE);
+
+}
+
+
+// we assume the s1 and s2 both are strings.
+BOOLEAN rtstrcasecmp(char *s1, char *s2)
+{
+	char *p1 = s1, *p2 = s2;
+
+	if (strlen(s1) != strlen(s2))
+		return FALSE;
+
+	while(*p1 != '\0')
+	{
+		if((*p1 != *p2) && ((*p1 ^ *p2) != 0x20))
+			return FALSE;
+		p1++;
+		p2++;
+	}
+
+	return TRUE;
+}
+
+// we assume the s1 (buffer) and s2 (key) both are strings.
+char * rtstrstruncasecmp(char * s1, char * s2)
+{
+	INT l1, l2, i;
+	char temp1, temp2;
+
+	l2 = strlen(s2);
+	if (!l2)
+		return (char *) s1;
+
+	l1 = strlen(s1);
+
+	while (l1 >= l2)
+	{
+		l1--;
+
+		for(i=0; i<l2; i++)
+		{
+			temp1 = *(s1+i);
+			temp2 = *(s2+i);
+
+			if (('a' <= temp1) && (temp1 <= 'z'))
+				temp1 = 'A'+(temp1-'a');
+			if (('a' <= temp2) && (temp2 <= 'z'))
+				temp2 = 'A'+(temp2-'a');
+
+			if (temp1 != temp2)
+				break;
+		}
+
+		if (i == l2)
+			return (char *) s1;
+
+		s1++;
+	}
+
+	return NULL; // not found
+}
+
+//add by kathy
+
+ /**
+  * strstr - Find the first substring in a %NUL terminated string
+  * @s1: The string to be searched
+  * @s2: The string to search for
+  */
+char * rtstrstr(const char * s1,const char * s2)
+{
+	INT l1, l2;
+
+	l2 = strlen(s2);
+	if (!l2)
+		return (char *) s1;
+
+	l1 = strlen(s1);
+
+	while (l1 >= l2)
+	{
+		l1--;
+		if (!memcmp(s1,s2,l2))
+			return (char *) s1;
+		s1++;
+	}
+
+	return NULL;
+}
+
+/**
+ * rstrtok - Split a string into tokens
+ * @s: The string to be searched
+ * @ct: The characters to search for
+ * * WARNING: strtok is deprecated, use strsep instead. However strsep is not compatible with old architecture.
+ */
+char * __rstrtok;
+char * rstrtok(char * s,const char * ct)
+{
+	char *sbegin, *send;
+
+	sbegin  = s ? s : __rstrtok;
+	if (!sbegin)
+	{
+		return NULL;
+	}
+
+	sbegin += strspn(sbegin,ct);
+	if (*sbegin == '\0')
+	{
+		__rstrtok = NULL;
+		return( NULL );
+	}
+
+	send = strpbrk( sbegin, ct);
+	if (send && *send != '\0')
+		*send++ = '\0';
+
+	__rstrtok = send;
+
+	return (sbegin);
+}
+
+/**
+ * delimitcnt - return the count of a given delimiter in a given string.
+ * @s: The string to be searched.
+ * @ct: The delimiter to search for.
+ * Notice : We suppose the delimiter is a single-char string(for example : ";").
+ */
+INT delimitcnt(char * s,const char * ct)
+{
+	INT count = 0;
+	/* point to the beginning of the line */
+	const char *token = s;
+
+	for ( ;; )
+	{
+		token = strpbrk(token, ct); /* search for delimiters */
+
+        if ( token == NULL )
+		{
+			/* advanced to the terminating null character */
+			break;
+		}
+		/* skip the delimiter */
+	    ++token;
+
+		/*
+		 * Print the found text: use len with %.*s to specify field width.
+		 */
+
+		/* accumulate delimiter count */
+	    ++count;
+	}
+    return count;
+}
+
+/*
+  * converts the Internet host address from the standard numbers-and-dots notation
+  * into binary data.
+  * returns nonzero if the address is valid, zero if not.
+  */
+int rtinet_aton(const char *cp, unsigned int *addr)
+{
+	unsigned int 	val;
+	int         	base, n;
+	char        	c;
+	unsigned int    parts[4];
+	unsigned int    *pp = parts;
+
+	for (;;)
+    {
+         /*
+          * Collect number up to ``.''.
+          * Values are specified as for C:
+          *	0x=hex, 0=octal, other=decimal.
+          */
+         val = 0;
+         base = 10;
+         if (*cp == '0')
+         {
+             if (*++cp == 'x' || *cp == 'X')
+                 base = 16, cp++;
+             else
+                 base = 8;
+         }
+         while ((c = *cp) != '\0')
+         {
+             if (isdigit((unsigned char) c))
+             {
+                 val = (val * base) + (c - '0');
+                 cp++;
+                 continue;
+             }
+             if (base == 16 && isxdigit((unsigned char) c))
+             {
+                 val = (val << 4) +
+                     (c + 10 - (islower((unsigned char) c) ? 'a' : 'A'));
+                 cp++;
+                 continue;
+             }
+             break;
+         }
+         if (*cp == '.')
+         {
+             /*
+              * Internet format: a.b.c.d a.b.c   (with c treated as 16-bits)
+              * a.b     (with b treated as 24 bits)
+              */
+             if (pp >= parts + 3 || val > 0xff)
+                 return 0;
+             *pp++ = val, cp++;
+         }
+         else
+             break;
+     }
+
+     /*
+      * Check for trailing junk.
+      */
+     while (*cp)
+         if (!isspace((unsigned char) *cp++))
+             return 0;
+
+     /*
+      * Concoct the address according to the number of parts specified.
+      */
+     n = pp - parts + 1;
+     switch (n)
+     {
+
+         case 1:         /* a -- 32 bits */
+             break;
+
+         case 2:         /* a.b -- 8.24 bits */
+             if (val > 0xffffff)
+                 return 0;
+             val |= parts[0] << 24;
+             break;
+
+         case 3:         /* a.b.c -- 8.8.16 bits */
+             if (val > 0xffff)
+                 return 0;
+             val |= (parts[0] << 24) | (parts[1] << 16);
+             break;
+
+         case 4:         /* a.b.c.d -- 8.8.8.8 bits */
+             if (val > 0xff)
+                 return 0;
+             val |= (parts[0] << 24) | (parts[1] << 16) | (parts[2] << 8);
+             break;
+     }
+
+     *addr = htonl(val);
+     return 1;
+
+}
+
+/*
+    ========================================================================
+
+    Routine Description:
+        Find key section for Get key parameter.
+
+    Arguments:
+        buffer                      Pointer to the buffer to start find the key section
+        section                     the key of the secion to be find
+
+    Return Value:
+        NULL                        Fail
+        Others                      Success
+    ========================================================================
+*/
+PUCHAR  RTMPFindSection(
+    IN  PCHAR   buffer)
+{
+    CHAR temp_buf[32];
+    PUCHAR  ptr;
+
+    strcpy(temp_buf, "Default");
+
+    if((ptr = rtstrstr(buffer, temp_buf)) != NULL)
+            return (ptr+strlen("\n"));
+        else
+            return NULL;
+}
+
+/*
+    ========================================================================
+
+    Routine Description:
+        Get key parameter.
+
+    Arguments:
+        key                         Pointer to key string
+        dest                        Pointer to destination
+        destsize                    The datasize of the destination
+        buffer                      Pointer to the buffer to start find the key
+
+    Return Value:
+        TRUE                        Success
+        FALSE                       Fail
+
+    Note:
+        This routine get the value with the matched key (case case-sensitive)
+    ========================================================================
+*/
+INT RTMPGetKeyParameter(
+    IN  PCHAR   key,
+    OUT PCHAR   dest,
+    IN  INT     destsize,
+    IN  PCHAR   buffer)
+{
+    UCHAR *temp_buf1 = NULL;
+    UCHAR *temp_buf2 = NULL;
+    CHAR *start_ptr;
+    CHAR *end_ptr;
+    CHAR *ptr;
+    CHAR *offset = 0;
+    INT  len;
+
+	//temp_buf1 = kmalloc(MAX_PARAM_BUFFER_SIZE, MEM_ALLOC_FLAG);
+	os_alloc_mem(NULL, &temp_buf1, MAX_PARAM_BUFFER_SIZE);
+
+	if(temp_buf1 == NULL)
+        return (FALSE);
+
+	//temp_buf2 = kmalloc(MAX_PARAM_BUFFER_SIZE, MEM_ALLOC_FLAG);
+	os_alloc_mem(NULL, &temp_buf2, MAX_PARAM_BUFFER_SIZE);
+	if(temp_buf2 == NULL)
+	{
+		os_free_mem(NULL, temp_buf1);
+        return (FALSE);
+	}
+
+    //find section
+    if((offset = RTMPFindSection(buffer)) == NULL)
+    {
+    	os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    strcpy(temp_buf1, "\n");
+    strcat(temp_buf1, key);
+    strcat(temp_buf1, "=");
+
+    //search key
+    if((start_ptr=rtstrstr(offset, temp_buf1))==NULL)
+    {
+		os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    start_ptr+=strlen("\n");
+    if((end_ptr=rtstrstr(start_ptr, "\n"))==NULL)
+       end_ptr=start_ptr+strlen(start_ptr);
+
+    if (end_ptr<start_ptr)
+    {
+		os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    NdisMoveMemory(temp_buf2, start_ptr, end_ptr-start_ptr);
+    temp_buf2[end_ptr-start_ptr]='\0';
+    len = strlen(temp_buf2);
+    strcpy(temp_buf1, temp_buf2);
+    if((start_ptr=rtstrstr(temp_buf1, "=")) == NULL)
+    {
+		os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    strcpy(temp_buf2, start_ptr+1);
+    ptr = temp_buf2;
+    //trim space or tab
+    while(*ptr != 0x00)
+    {
+        if( (*ptr == ' ') || (*ptr == '\t') )
+            ptr++;
+        else
+           break;
+    }
+
+    len = strlen(ptr);
+    memset(dest, 0x00, destsize);
+    strncpy(dest, ptr, len >= destsize ?  destsize: len);
+
+	os_free_mem(NULL, temp_buf1);
+    os_free_mem(NULL, temp_buf2);
+    return TRUE;
+}
+
+/*
+    ========================================================================
+
+    Routine Description:
+        Get key parameter.
+
+    Arguments:
+        key                         Pointer to key string
+        dest                        Pointer to destination
+        destsize                    The datasize of the destination
+        buffer                      Pointer to the buffer to start find the key
+
+    Return Value:
+        TRUE                        Success
+        FALSE                       Fail
+
+    Note:
+        This routine get the value with the matched key (case case-sensitive).
+        It is called for parsing SSID and any key string.
+    ========================================================================
+*/
+INT RTMPGetCriticalParameter(
+    IN  PCHAR   key,
+    OUT PCHAR   dest,
+    IN  INT     destsize,
+    IN  PCHAR   buffer)
+{
+    UCHAR *temp_buf1 = NULL;
+    UCHAR *temp_buf2 = NULL;
+    CHAR *start_ptr;
+    CHAR *end_ptr;
+    CHAR *ptr;
+    CHAR *offset = 0;
+    INT  len;
+
+	//temp_buf1 = kmalloc(MAX_PARAM_BUFFER_SIZE, MEM_ALLOC_FLAG);
+	os_alloc_mem(NULL, &temp_buf1, MAX_PARAM_BUFFER_SIZE);
+
+	if(temp_buf1 == NULL)
+        return (FALSE);
+
+	//temp_buf2 = kmalloc(MAX_PARAM_BUFFER_SIZE, MEM_ALLOC_FLAG);
+	os_alloc_mem(NULL, &temp_buf2, MAX_PARAM_BUFFER_SIZE);
+	if(temp_buf2 == NULL)
+	{
+		os_free_mem(NULL, temp_buf1);
+        return (FALSE);
+	}
+
+    //find section
+    if((offset = RTMPFindSection(buffer)) == NULL)
+    {
+    	os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    strcpy(temp_buf1, "\n");
+    strcat(temp_buf1, key);
+    strcat(temp_buf1, "=");
+
+    //search key
+    if((start_ptr=rtstrstr(offset, temp_buf1))==NULL)
+    {
+		os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    start_ptr+=strlen("\n");
+    if((end_ptr=rtstrstr(start_ptr, "\n"))==NULL)
+       end_ptr=start_ptr+strlen(start_ptr);
+
+    if (end_ptr<start_ptr)
+    {
+		os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    NdisMoveMemory(temp_buf2, start_ptr, end_ptr-start_ptr);
+    temp_buf2[end_ptr-start_ptr]='\0';
+    len = strlen(temp_buf2);
+    strcpy(temp_buf1, temp_buf2);
+    if((start_ptr=rtstrstr(temp_buf1, "=")) == NULL)
+    {
+		os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    strcpy(temp_buf2, start_ptr+1);
+    ptr = temp_buf2;
+
+    //trim tab
+    /* We cannot trim space(' ') for SSID and key string. */
+    while(*ptr != 0x00)
+    {
+        //if( (*ptr == ' ') || (*ptr == '\t') )
+        if( (*ptr == '\t') )
+            ptr++;
+        else
+           break;
+    }
+
+    len = strlen(ptr);
+    memset(dest, 0x00, destsize);
+    strncpy(dest, ptr, len >= destsize ?  destsize: len);
+
+	os_free_mem(NULL, temp_buf1);
+    os_free_mem(NULL, temp_buf2);
+    return TRUE;
+}
+
+/*
+    ========================================================================
+
+    Routine Description:
+        Get multiple key parameter.
+
+    Arguments:
+        key                         Pointer to key string
+        dest                        Pointer to destination
+        destsize                    The datasize of the destination
+        buffer                      Pointer to the buffer to start find the key
+
+    Return Value:
+        TRUE                        Success
+        FALSE                       Fail
+
+    Note:
+        This routine get the value with the matched key (case case-sensitive)
+    ========================================================================
+*/
+INT RTMPGetKeyParameterWithOffset(
+    IN  PCHAR   key,
+    OUT PCHAR   dest,
+    OUT	USHORT	*end_offset,
+    IN  INT     destsize,
+    IN  PCHAR   buffer,
+    IN	BOOLEAN	bTrimSpace)
+{
+    UCHAR *temp_buf1 = NULL;
+    UCHAR *temp_buf2 = NULL;
+    CHAR *start_ptr;
+    CHAR *end_ptr;
+    CHAR *ptr;
+    CHAR *offset = 0;
+    INT  len;
+
+	if (*end_offset >= MAX_INI_BUFFER_SIZE)
+		return (FALSE);
+
+	os_alloc_mem(NULL, &temp_buf1, MAX_PARAM_BUFFER_SIZE);
+
+	if(temp_buf1 == NULL)
+        return (FALSE);
+
+	os_alloc_mem(NULL, &temp_buf2, MAX_PARAM_BUFFER_SIZE);
+	if(temp_buf2 == NULL)
+	{
+		os_free_mem(NULL, temp_buf1);
+        return (FALSE);
+	}
+
+    //find section
+	if(*end_offset == 0)
+    {
+		if ((offset = RTMPFindSection(buffer)) == NULL)
+		{
+			os_free_mem(NULL, temp_buf1);
+	    	os_free_mem(NULL, temp_buf2);
+    	    return (FALSE);
+		}
+    }
+	else
+		offset = buffer + (*end_offset);
+
+    strcpy(temp_buf1, "\n");
+    strcat(temp_buf1, key);
+    strcat(temp_buf1, "=");
+
+    //search key
+    if((start_ptr=rtstrstr(offset, temp_buf1))==NULL)
+    {
+		os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    start_ptr+=strlen("\n");
+    if((end_ptr=rtstrstr(start_ptr, "\n"))==NULL)
+       end_ptr=start_ptr+strlen(start_ptr);
+
+    if (end_ptr<start_ptr)
+    {
+		os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+	*end_offset = end_ptr - buffer;
+
+    NdisMoveMemory(temp_buf2, start_ptr, end_ptr-start_ptr);
+    temp_buf2[end_ptr-start_ptr]='\0';
+    len = strlen(temp_buf2);
+    strcpy(temp_buf1, temp_buf2);
+    if((start_ptr=rtstrstr(temp_buf1, "=")) == NULL)
+    {
+		os_free_mem(NULL, temp_buf1);
+    	os_free_mem(NULL, temp_buf2);
+        return (FALSE);
+    }
+
+    strcpy(temp_buf2, start_ptr+1);
+    ptr = temp_buf2;
+    //trim space or tab
+    while(*ptr != 0x00)
+    {
+        if((bTrimSpace && (*ptr == ' ')) || (*ptr == '\t') )
+            ptr++;
+        else
+           break;
+    }
+
+    len = strlen(ptr);
+    memset(dest, 0x00, destsize);
+    strncpy(dest, ptr, len >= destsize ?  destsize: len);
+
+	os_free_mem(NULL, temp_buf1);
+    os_free_mem(NULL, temp_buf2);
+    return TRUE;
+}
+
+
+static int rtmp_parse_key_buffer_from_file(IN  PRTMP_ADAPTER pAd,IN  char *buffer,IN  ULONG KeyType,IN  INT BSSIdx,IN  INT KeyIdx)
+{
+	PUCHAR		keybuff;
+	INT			i = BSSIdx, idx = KeyIdx;
+	ULONG		KeyLen;
+	UCHAR		CipherAlg = CIPHER_WEP64;
+
+	keybuff = buffer;
+	KeyLen = strlen(keybuff);
+
+	if (KeyType == 1)
+	{//Ascii
+		if( (KeyLen == 5) || (KeyLen == 13))
+		{
+			pAd->SharedKey[i][idx].KeyLen = KeyLen;
+			NdisMoveMemory(pAd->SharedKey[i][idx].Key, keybuff, KeyLen);
+			if (KeyLen == 5)
+				CipherAlg = CIPHER_WEP64;
+			else
+				CipherAlg = CIPHER_WEP128;
+			pAd->SharedKey[i][idx].CipherAlg = CipherAlg;
+
+			DBGPRINT(RT_DEBUG_TRACE, ("I/F(ra%d) Key%dStr=%s and type=%s\n", i, idx+1, keybuff, (KeyType == 0) ? "Hex":"Ascii"));
+			return 1;
+		}
+		else
+		{//Invalid key length
+			DBGPRINT(RT_DEBUG_ERROR, ("Key%dStr is Invalid key length! KeyLen = %ld!\n", idx+1, KeyLen));
+			return 0;
+		}
+	}
+	else
+	{//Hex type
+		if( (KeyLen == 10) || (KeyLen == 26))
+		{
+			pAd->SharedKey[i][idx].KeyLen = KeyLen / 2;
+			AtoH(keybuff, pAd->SharedKey[i][idx].Key, KeyLen / 2);
+			if (KeyLen == 10)
+				CipherAlg = CIPHER_WEP64;
+			else
+				CipherAlg = CIPHER_WEP128;
+			pAd->SharedKey[i][idx].CipherAlg = CipherAlg;
+
+			DBGPRINT(RT_DEBUG_TRACE, ("I/F(ra%d) Key%dStr=%s and type=%s\n", i, idx+1, keybuff, (KeyType == 0) ? "Hex":"Ascii"));
+			return 1;
+		}
+		else
+		{//Invalid key length
+			DBGPRINT(RT_DEBUG_ERROR, ("I/F(ra%d) Key%dStr is Invalid key length! KeyLen = %ld!\n", i, idx+1, KeyLen));
+			return 0;
+		}
+	}
+}
+static void rtmp_read_key_parms_from_file(IN  PRTMP_ADAPTER pAd, char *tmpbuf, char *buffer)
+{
+	char		tok_str[16];
+	PUCHAR		macptr;
+	INT			i = 0, idx;
+	ULONG		KeyType[MAX_MBSSID_NUM];
+	ULONG		KeyIdx;
+
+	NdisZeroMemory(KeyType, MAX_MBSSID_NUM);
+
+	//DefaultKeyID
+	if(RTMPGetKeyParameter("DefaultKeyID", tmpbuf, 25, buffer))
+	{
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			KeyIdx = simple_strtol(tmpbuf, 0, 10);
+			if((KeyIdx >= 1 ) && (KeyIdx <= 4))
+				pAd->StaCfg.DefaultKeyId = (UCHAR) (KeyIdx - 1);
+			else
+				pAd->StaCfg.DefaultKeyId = 0;
+
+			DBGPRINT(RT_DEBUG_TRACE, ("DefaultKeyID(0~3)=%d\n", pAd->StaCfg.DefaultKeyId));
+		}
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+
+	for (idx = 0; idx < 4; idx++)
+	{
+		sprintf(tok_str, "Key%dType", idx + 1);
+		//Key1Type
+		if (RTMPGetKeyParameter(tok_str, tmpbuf, 128, buffer))
+		{
+		    for (i = 0, macptr = rstrtok(tmpbuf,";"); macptr; macptr = rstrtok(NULL,";"), i++)
+		    {
+			    KeyType[i] = simple_strtol(macptr, 0, 10);
+		    }
+
+#ifdef CONFIG_STA_SUPPORT
+			IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+			{
+				sprintf(tok_str, "Key%dStr", idx + 1);
+				if (RTMPGetCriticalParameter(tok_str, tmpbuf, 128, buffer))
+				{
+					rtmp_parse_key_buffer_from_file(pAd, tmpbuf, KeyType[BSS0], BSS0, idx);
+				}
+			}
+#endif // CONFIG_STA_SUPPORT //
+		}
+	}
+}
+
+
+#ifdef CONFIG_STA_SUPPORT
+static void rtmp_read_sta_wmm_parms_from_file(IN  PRTMP_ADAPTER pAd, char *tmpbuf, char *buffer)
+{
+	PUCHAR					macptr;
+	INT						i=0;
+	BOOLEAN					bWmmEnable = FALSE;
+
+	//WmmCapable
+	if(RTMPGetKeyParameter("WmmCapable", tmpbuf, 32, buffer))
+	{
+		if(simple_strtol(tmpbuf, 0, 10) != 0) //Enable
+		{
+			pAd->CommonCfg.bWmmCapable = TRUE;
+			bWmmEnable = TRUE;
+		}
+		else //Disable
+		{
+			pAd->CommonCfg.bWmmCapable = FALSE;
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("WmmCapable=%d\n", pAd->CommonCfg.bWmmCapable));
+	}
+
+#ifdef QOS_DLS_SUPPORT
+	//DLSCapable
+	if(RTMPGetKeyParameter("DLSCapable", tmpbuf, 32, buffer))
+	{
+		if(simple_strtol(tmpbuf, 0, 10) != 0)  //Enable
+		{
+			pAd->CommonCfg.bDLSCapable = TRUE;
+		}
+		else //Disable
+		{
+			pAd->CommonCfg.bDLSCapable = FALSE;
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("bDLSCapable=%d\n", pAd->CommonCfg.bDLSCapable));
+	}
+#endif // QOS_DLS_SUPPORT //
+
+	//AckPolicy for AC_BK, AC_BE, AC_VI, AC_VO
+	if(RTMPGetKeyParameter("AckPolicy", tmpbuf, 32, buffer))
+	{
+		for (i = 0, macptr = rstrtok(tmpbuf,";"); macptr; macptr = rstrtok(NULL,";"), i++)
+		{
+			pAd->CommonCfg.AckPolicy[i] = (UCHAR)simple_strtol(macptr, 0, 10);
+
+			DBGPRINT(RT_DEBUG_TRACE, ("AckPolicy[%d]=%d\n", i, pAd->CommonCfg.AckPolicy[i]));
+		}
+	}
+
+	if (bWmmEnable)
+	{
+		//APSDCapable
+		if(RTMPGetKeyParameter("APSDCapable", tmpbuf, 10, buffer))
+		{
+			if(simple_strtol(tmpbuf, 0, 10) != 0)  //Enable
+				pAd->CommonCfg.bAPSDCapable = TRUE;
+			else
+				pAd->CommonCfg.bAPSDCapable = FALSE;
+
+			DBGPRINT(RT_DEBUG_TRACE, ("APSDCapable=%d\n", pAd->CommonCfg.bAPSDCapable));
+		}
+
+		//APSDAC for AC_BE, AC_BK, AC_VI, AC_VO
+		if(RTMPGetKeyParameter("APSDAC", tmpbuf, 32, buffer))
+		{
+			BOOLEAN apsd_ac[4];
+
+			for (i = 0, macptr = rstrtok(tmpbuf,";"); macptr; macptr = rstrtok(NULL,";"), i++)
+			{
+				apsd_ac[i] = (BOOLEAN)simple_strtol(macptr, 0, 10);
+
+				DBGPRINT(RT_DEBUG_TRACE, ("APSDAC%d  %d\n", i,  apsd_ac[i]));
+			}
+
+			pAd->CommonCfg.bAPSDAC_BE = apsd_ac[0];
+			pAd->CommonCfg.bAPSDAC_BK = apsd_ac[1];
+			pAd->CommonCfg.bAPSDAC_VI = apsd_ac[2];
+			pAd->CommonCfg.bAPSDAC_VO = apsd_ac[3];
+		}
+	}
+
+}
+#endif // CONFIG_STA_SUPPORT //
+
+
+NDIS_STATUS	RTMPReadParametersHook(
+	IN	PRTMP_ADAPTER pAd)
+{
+	PUCHAR					src = NULL;
+	struct file				*srcf;
+	INT 					retval, orgfsuid, orgfsgid;
+   	mm_segment_t			orgfs;
+	CHAR					*buffer;
+	CHAR					*tmpbuf;
+	ULONG					RtsThresh;
+	ULONG					FragThresh;
+#ifdef CONFIG_STA_SUPPORT
+	UCHAR	                keyMaterial[40];
+#endif // CONFIG_STA_SUPPORT //
+
+
+	PUCHAR					macptr;
+	INT						i = 0;
+
+	buffer = kmalloc(MAX_INI_BUFFER_SIZE, MEM_ALLOC_FLAG);
+	if(buffer == NULL)
+        return NDIS_STATUS_FAILURE;
+
+	tmpbuf = kmalloc(MAX_PARAM_BUFFER_SIZE, MEM_ALLOC_FLAG);
+	if(tmpbuf == NULL)
+	{
+		kfree(buffer);
+        return NDIS_STATUS_FAILURE;
+	}
+
+#ifdef CONFIG_STA_SUPPORT
+	IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		src = STA_PROFILE_PATH;
+#endif // CONFIG_STA_SUPPORT //
+#ifdef MULTIPLE_CARD_SUPPORT
+	src = pAd->MC_FileName;
+#endif // MULTIPLE_CARD_SUPPORT //
+
+	// Save uid and gid used for filesystem access.
+	// Set user and group to 0 (root)
+	orgfsuid = current_fsuid();
+	orgfsgid = current_fsgid();
+	/* Hm, can't really do this nicely anymore, so rely on these files
+	 * being set to the proper permission to read them... */
+	/* current->cred->fsuid = current->cred->fsgid = 0; */
+    orgfs = get_fs();
+    set_fs(KERNEL_DS);
+
+	if (src && *src)
+	{
+		srcf = filp_open(src, O_RDONLY, 0);
+		if (IS_ERR(srcf))
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("--> Error %ld opening %s\n", -PTR_ERR(srcf),src));
+		}
+		else
+		{
+			// The object must have a read method
+			if (srcf->f_op && srcf->f_op->read)
+			{
+				memset(buffer, 0x00, MAX_INI_BUFFER_SIZE);
+				retval=srcf->f_op->read(srcf, buffer, MAX_INI_BUFFER_SIZE, &srcf->f_pos);
+				if (retval < 0)
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("--> Read %s error %d\n", src, -retval));
+				}
+				else
+				{
+					// set file parameter to portcfg
+					//CountryRegion
+					if(RTMPGetKeyParameter("CountryRegion", tmpbuf, 25, buffer))
+					{
+						pAd->CommonCfg.CountryRegion = (UCHAR) simple_strtol(tmpbuf, 0, 10);
+						DBGPRINT(RT_DEBUG_TRACE, ("CountryRegion=%d\n", pAd->CommonCfg.CountryRegion));
+					}
+					//CountryRegionABand
+					if(RTMPGetKeyParameter("CountryRegionABand", tmpbuf, 25, buffer))
+					{
+						pAd->CommonCfg.CountryRegionForABand= (UCHAR) simple_strtol(tmpbuf, 0, 10);
+						DBGPRINT(RT_DEBUG_TRACE, ("CountryRegionABand=%d\n", pAd->CommonCfg.CountryRegionForABand));
+					}
+					//CountryCode
+					if(RTMPGetKeyParameter("CountryCode", tmpbuf, 25, buffer))
+					{
+						NdisMoveMemory(pAd->CommonCfg.CountryCode, tmpbuf , 2);
+#ifdef CONFIG_STA_SUPPORT
+#ifdef EXT_BUILD_CHANNEL_LIST
+						IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+							NdisMoveMemory(pAd->StaCfg.StaOriCountryCode, tmpbuf , 2);
+#endif // EXT_BUILD_CHANNEL_LIST //
+#endif // CONFIG_STA_SUPPORT //
+						if (strlen(pAd->CommonCfg.CountryCode) != 0)
+						{
+							pAd->CommonCfg.bCountryFlag = TRUE;
+						}
+						DBGPRINT(RT_DEBUG_TRACE, ("CountryCode=%s\n", pAd->CommonCfg.CountryCode));
+					}
+					//ChannelGeography
+					if(RTMPGetKeyParameter("ChannelGeography", tmpbuf, 25, buffer))
+					{
+						UCHAR Geography = (UCHAR) simple_strtol(tmpbuf, 0, 10);
+						if (Geography <= BOTH)
+						{
+							pAd->CommonCfg.Geography = Geography;
+							pAd->CommonCfg.CountryCode[2] =
+								(pAd->CommonCfg.Geography == BOTH) ? ' ' : ((pAd->CommonCfg.Geography == IDOR) ? 'I' : 'O');
+#ifdef CONFIG_STA_SUPPORT
+#ifdef EXT_BUILD_CHANNEL_LIST
+						IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+							pAd->StaCfg.StaOriGeography = pAd->CommonCfg.Geography;
+#endif // EXT_BUILD_CHANNEL_LIST //
+#endif // CONFIG_STA_SUPPORT //
+							DBGPRINT(RT_DEBUG_TRACE, ("ChannelGeography=%d\n", pAd->CommonCfg.Geography));
+						}
+					}
+					else
+					{
+						pAd->CommonCfg.Geography = BOTH;
+						pAd->CommonCfg.CountryCode[2] = ' ';
+					}
+
+
+#ifdef CONFIG_STA_SUPPORT
+					IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+					{
+						//SSID
+						if (RTMPGetCriticalParameter("SSID", tmpbuf, 256, buffer))
+						{
+							if (strlen(tmpbuf) <= 32)
+							{
+			 					pAd->CommonCfg.SsidLen = (UCHAR) strlen(tmpbuf);
+								NdisZeroMemory(pAd->CommonCfg.Ssid, NDIS_802_11_LENGTH_SSID);
+								NdisMoveMemory(pAd->CommonCfg.Ssid, tmpbuf, pAd->CommonCfg.SsidLen);
+								pAd->MlmeAux.AutoReconnectSsidLen = pAd->CommonCfg.SsidLen;
+								NdisZeroMemory(pAd->MlmeAux.AutoReconnectSsid, NDIS_802_11_LENGTH_SSID);
+								NdisMoveMemory(pAd->MlmeAux.AutoReconnectSsid, tmpbuf, pAd->MlmeAux.AutoReconnectSsidLen);
+								pAd->MlmeAux.SsidLen = pAd->CommonCfg.SsidLen;
+								NdisZeroMemory(pAd->MlmeAux.Ssid, NDIS_802_11_LENGTH_SSID);
+								NdisMoveMemory(pAd->MlmeAux.Ssid, tmpbuf, pAd->MlmeAux.SsidLen);
+								DBGPRINT(RT_DEBUG_TRACE, ("%s::(SSID=%s)\n", __FUNCTION__, tmpbuf));
+							}
+						}
+					}
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+					IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+					{
+						//NetworkType
+						if (RTMPGetKeyParameter("NetworkType", tmpbuf, 25, buffer))
+						{
+							pAd->bConfigChanged = TRUE;
+							if (strcmp(tmpbuf, "Adhoc") == 0)
+								pAd->StaCfg.BssType = BSS_ADHOC;
+							else //Default Infrastructure mode
+								pAd->StaCfg.BssType = BSS_INFRA;
+							// Reset Ralink supplicant to not use, it will be set to start when UI set PMK key
+							pAd->StaCfg.WpaState = SS_NOTUSE;
+							DBGPRINT(RT_DEBUG_TRACE, ("%s::(NetworkType=%d)\n", __FUNCTION__, pAd->StaCfg.BssType));
+						}
+					}
+#endif // CONFIG_STA_SUPPORT //
+					//Channel
+					if(RTMPGetKeyParameter("Channel", tmpbuf, 10, buffer))
+					{
+						pAd->CommonCfg.Channel = (UCHAR) simple_strtol(tmpbuf, 0, 10);
+						DBGPRINT(RT_DEBUG_TRACE, ("Channel=%d\n", pAd->CommonCfg.Channel));
+					}
+					//WirelessMode
+					if(RTMPGetKeyParameter("WirelessMode", tmpbuf, 10, buffer))
+					{
+						int value  = 0, maxPhyMode = PHY_11G;
+
+#ifdef DOT11_N_SUPPORT
+						maxPhyMode = PHY_11N_5G;
+#endif // DOT11_N_SUPPORT //
+
+						value = simple_strtol(tmpbuf, 0, 10);
+
+						if (value <= maxPhyMode)
+						{
+							pAd->CommonCfg.PhyMode = value;
+						}
+						DBGPRINT(RT_DEBUG_TRACE, ("PhyMode=%d\n", pAd->CommonCfg.PhyMode));
+					}
+                    //BasicRate
+					if(RTMPGetKeyParameter("BasicRate", tmpbuf, 10, buffer))
+					{
+						pAd->CommonCfg.BasicRateBitmap = (ULONG) simple_strtol(tmpbuf, 0, 10);
+						DBGPRINT(RT_DEBUG_TRACE, ("BasicRate=%ld\n", pAd->CommonCfg.BasicRateBitmap));
+					}
+					//BeaconPeriod
+					if(RTMPGetKeyParameter("BeaconPeriod", tmpbuf, 10, buffer))
+					{
+						pAd->CommonCfg.BeaconPeriod = (USHORT) simple_strtol(tmpbuf, 0, 10);
+						DBGPRINT(RT_DEBUG_TRACE, ("BeaconPeriod=%d\n", pAd->CommonCfg.BeaconPeriod));
+					}
+                    //TxPower
+					if(RTMPGetKeyParameter("TxPower", tmpbuf, 10, buffer))
+					{
+						pAd->CommonCfg.TxPowerPercentage = (ULONG) simple_strtol(tmpbuf, 0, 10);
+#ifdef CONFIG_STA_SUPPORT
+						IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+							pAd->CommonCfg.TxPowerDefault = pAd->CommonCfg.TxPowerPercentage;
+#endif // CONFIG_STA_SUPPORT //
+						DBGPRINT(RT_DEBUG_TRACE, ("TxPower=%ld\n", pAd->CommonCfg.TxPowerPercentage));
+					}
+					//BGProtection
+					if(RTMPGetKeyParameter("BGProtection", tmpbuf, 10, buffer))
+					{
+						switch (simple_strtol(tmpbuf, 0, 10))
+						{
+							case 1: //Always On
+								pAd->CommonCfg.UseBGProtection = 1;
+								break;
+							case 2: //Always OFF
+								pAd->CommonCfg.UseBGProtection = 2;
+								break;
+							case 0: //AUTO
+							default:
+								pAd->CommonCfg.UseBGProtection = 0;
+								break;
+						}
+						DBGPRINT(RT_DEBUG_TRACE, ("BGProtection=%ld\n", pAd->CommonCfg.UseBGProtection));
+					}
+					//OLBCDetection
+					if(RTMPGetKeyParameter("DisableOLBC", tmpbuf, 10, buffer))
+					{
+						switch (simple_strtol(tmpbuf, 0, 10))
+						{
+							case 1: //disable OLBC Detection
+								pAd->CommonCfg.DisableOLBCDetect = 1;
+								break;
+							case 0: //enable OLBC Detection
+								pAd->CommonCfg.DisableOLBCDetect = 0;
+								break;
+							default:
+								pAd->CommonCfg.DisableOLBCDetect= 0;
+								break;
+						}
+						DBGPRINT(RT_DEBUG_TRACE, ("OLBCDetection=%ld\n", pAd->CommonCfg.DisableOLBCDetect));
+					}
+					//TxPreamble
+					if(RTMPGetKeyParameter("TxPreamble", tmpbuf, 10, buffer))
+					{
+						switch (simple_strtol(tmpbuf, 0, 10))
+						{
+							case Rt802_11PreambleShort:
+								pAd->CommonCfg.TxPreamble = Rt802_11PreambleShort;
+								break;
+							case Rt802_11PreambleLong:
+							default:
+								pAd->CommonCfg.TxPreamble = Rt802_11PreambleLong;
+								break;
+						}
+						DBGPRINT(RT_DEBUG_TRACE, ("TxPreamble=%ld\n", pAd->CommonCfg.TxPreamble));
+					}
+					//RTSThreshold
+					if(RTMPGetKeyParameter("RTSThreshold", tmpbuf, 10, buffer))
+					{
+						RtsThresh = simple_strtol(tmpbuf, 0, 10);
+						if( (RtsThresh >= 1) && (RtsThresh <= MAX_RTS_THRESHOLD) )
+							pAd->CommonCfg.RtsThreshold  = (USHORT)RtsThresh;
+						else
+							pAd->CommonCfg.RtsThreshold = MAX_RTS_THRESHOLD;
+
+						DBGPRINT(RT_DEBUG_TRACE, ("RTSThreshold=%d\n", pAd->CommonCfg.RtsThreshold));
+					}
+					//FragThreshold
+					if(RTMPGetKeyParameter("FragThreshold", tmpbuf, 10, buffer))
+					{
+						FragThresh = simple_strtol(tmpbuf, 0, 10);
+						pAd->CommonCfg.bUseZeroToDisableFragment = FALSE;
+
+						if (FragThresh > MAX_FRAG_THRESHOLD || FragThresh < MIN_FRAG_THRESHOLD)
+						{ //illegal FragThresh so we set it to default
+							pAd->CommonCfg.FragmentThreshold = MAX_FRAG_THRESHOLD;
+							pAd->CommonCfg.bUseZeroToDisableFragment = TRUE;
+						}
+						else if (FragThresh % 2 == 1)
+						{
+							// The length of each fragment shall always be an even number of octets, except for the last fragment
+							// of an MSDU or MMPDU, which may be either an even or an odd number of octets.
+							pAd->CommonCfg.FragmentThreshold = (USHORT)(FragThresh - 1);
+						}
+						else
+						{
+							pAd->CommonCfg.FragmentThreshold = (USHORT)FragThresh;
+						}
+						//pAd->CommonCfg.AllowFragSize = (pAd->CommonCfg.FragmentThreshold) - LENGTH_802_11 - LENGTH_CRC;
+						DBGPRINT(RT_DEBUG_TRACE, ("FragThreshold=%d\n", pAd->CommonCfg.FragmentThreshold));
+					}
+					//TxBurst
+					if(RTMPGetKeyParameter("TxBurst", tmpbuf, 10, buffer))
+					{
+//#ifdef WIFI_TEST
+//						pAd->CommonCfg.bEnableTxBurst = FALSE;
+//#else
+						if(simple_strtol(tmpbuf, 0, 10) != 0)  //Enable
+							pAd->CommonCfg.bEnableTxBurst = TRUE;
+						else //Disable
+							pAd->CommonCfg.bEnableTxBurst = FALSE;
+//#endif
+						DBGPRINT(RT_DEBUG_TRACE, ("TxBurst=%d\n", pAd->CommonCfg.bEnableTxBurst));
+					}
+
+#ifdef AGGREGATION_SUPPORT
+					//PktAggregate
+					if(RTMPGetKeyParameter("PktAggregate", tmpbuf, 10, buffer))
+					{
+						if(simple_strtol(tmpbuf, 0, 10) != 0)  //Enable
+							pAd->CommonCfg.bAggregationCapable = TRUE;
+						else //Disable
+							pAd->CommonCfg.bAggregationCapable = FALSE;
+#ifdef PIGGYBACK_SUPPORT
+						pAd->CommonCfg.bPiggyBackCapable = pAd->CommonCfg.bAggregationCapable;
+#endif // PIGGYBACK_SUPPORT //
+						DBGPRINT(RT_DEBUG_TRACE, ("PktAggregate=%d\n", pAd->CommonCfg.bAggregationCapable));
+					}
+#else
+					pAd->CommonCfg.bAggregationCapable = FALSE;
+					pAd->CommonCfg.bPiggyBackCapable = FALSE;
+#endif // AGGREGATION_SUPPORT //
+
+					// WmmCapable
+
+#ifdef CONFIG_STA_SUPPORT
+					IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+						rtmp_read_sta_wmm_parms_from_file(pAd, tmpbuf, buffer);
+#endif // CONFIG_STA_SUPPORT //
+
+					//ShortSlot
+					if(RTMPGetKeyParameter("ShortSlot", tmpbuf, 10, buffer))
+					{
+						if(simple_strtol(tmpbuf, 0, 10) != 0)  //Enable
+							pAd->CommonCfg.bUseShortSlotTime = TRUE;
+						else //Disable
+							pAd->CommonCfg.bUseShortSlotTime = FALSE;
+
+						DBGPRINT(RT_DEBUG_TRACE, ("ShortSlot=%d\n", pAd->CommonCfg.bUseShortSlotTime));
+					}
+					//IEEE80211H
+					if(RTMPGetKeyParameter("IEEE80211H", tmpbuf, 10, buffer))
+					{
+					    for (i = 0, macptr = rstrtok(tmpbuf,";"); macptr; macptr = rstrtok(NULL,";"), i++)
+					    {
+    						if(simple_strtol(macptr, 0, 10) != 0)  //Enable
+    							pAd->CommonCfg.bIEEE80211H = TRUE;
+    						else //Disable
+    							pAd->CommonCfg.bIEEE80211H = FALSE;
+
+    						DBGPRINT(RT_DEBUG_TRACE, ("IEEE80211H=%d\n", pAd->CommonCfg.bIEEE80211H));
+					    }
+					}
+					//CSPeriod
+					if(RTMPGetKeyParameter("CSPeriod", tmpbuf, 10, buffer))
+					{
+					    if(simple_strtol(tmpbuf, 0, 10) != 0)
+							pAd->CommonCfg.RadarDetect.CSPeriod = simple_strtol(tmpbuf, 0, 10);
+						else
+							pAd->CommonCfg.RadarDetect.CSPeriod = 0;
+
+   						DBGPRINT(RT_DEBUG_TRACE, ("CSPeriod=%d\n", pAd->CommonCfg.RadarDetect.CSPeriod));
+					}
+
+					//RDRegion
+					if(RTMPGetKeyParameter("RDRegion", tmpbuf, 128, buffer))
+					{
+						if ((strncmp(tmpbuf, "JAP_W53", 7) == 0) || (strncmp(tmpbuf, "jap_w53", 7) == 0))
+						{
+							pAd->CommonCfg.RadarDetect.RDDurRegion = JAP_W53;
+							pAd->CommonCfg.RadarDetect.DfsSessionTime = 15;
+						}
+						else if ((strncmp(tmpbuf, "JAP_W56", 7) == 0) || (strncmp(tmpbuf, "jap_w56", 7) == 0))
+						{
+							pAd->CommonCfg.RadarDetect.RDDurRegion = JAP_W56;
+							pAd->CommonCfg.RadarDetect.DfsSessionTime = 13;
+						}
+						else if ((strncmp(tmpbuf, "JAP", 3) == 0) || (strncmp(tmpbuf, "jap", 3) == 0))
+						{
+							pAd->CommonCfg.RadarDetect.RDDurRegion = JAP;
+							pAd->CommonCfg.RadarDetect.DfsSessionTime = 5;
+						}
+						else  if ((strncmp(tmpbuf, "FCC", 3) == 0) || (strncmp(tmpbuf, "fcc", 3) == 0))
+						{
+							pAd->CommonCfg.RadarDetect.RDDurRegion = FCC;
+							pAd->CommonCfg.RadarDetect.DfsSessionTime = 5;
+						}
+						else if ((strncmp(tmpbuf, "CE", 2) == 0) || (strncmp(tmpbuf, "ce", 2) == 0))
+						{
+							pAd->CommonCfg.RadarDetect.RDDurRegion = CE;
+							pAd->CommonCfg.RadarDetect.DfsSessionTime = 13;
+						}
+						else
+						{
+							pAd->CommonCfg.RadarDetect.RDDurRegion = CE;
+							pAd->CommonCfg.RadarDetect.DfsSessionTime = 13;
+						}
+
+						DBGPRINT(RT_DEBUG_TRACE, ("RDRegion=%d\n", pAd->CommonCfg.RadarDetect.RDDurRegion));
+					}
+					else
+					{
+						pAd->CommonCfg.RadarDetect.RDDurRegion = CE;
+						pAd->CommonCfg.RadarDetect.DfsSessionTime = 13;
+					}
+
+					//WirelessEvent
+					if(RTMPGetKeyParameter("WirelessEvent", tmpbuf, 10, buffer))
+					{
+#if WIRELESS_EXT >= 15
+					    if(simple_strtol(tmpbuf, 0, 10) != 0)
+							pAd->CommonCfg.bWirelessEvent = simple_strtol(tmpbuf, 0, 10);
+						else
+							pAd->CommonCfg.bWirelessEvent = 0;	// disable
+#else
+						pAd->CommonCfg.bWirelessEvent = 0;	// disable
+#endif
+   						DBGPRINT(RT_DEBUG_TRACE, ("WirelessEvent=%d\n", pAd->CommonCfg.bWirelessEvent));
+					}
+					if(RTMPGetKeyParameter("WiFiTest", tmpbuf, 10, buffer))
+					{
+					    if(simple_strtol(tmpbuf, 0, 10) != 0)
+							pAd->CommonCfg.bWiFiTest= simple_strtol(tmpbuf, 0, 10);
+						else
+							pAd->CommonCfg.bWiFiTest = 0;	// disable
+
+   						DBGPRINT(RT_DEBUG_TRACE, ("WiFiTest=%d\n", pAd->CommonCfg.bWiFiTest));
+					}
+					//AuthMode
+					if(RTMPGetKeyParameter("AuthMode", tmpbuf, 128, buffer))
+					{
+#ifdef CONFIG_STA_SUPPORT
+						IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+						{
+							if ((strcmp(tmpbuf, "WEPAUTO") == 0) || (strcmp(tmpbuf, "wepauto") == 0))
+							    pAd->StaCfg.AuthMode = Ndis802_11AuthModeAutoSwitch;
+							else if ((strcmp(tmpbuf, "SHARED") == 0) || (strcmp(tmpbuf, "shared") == 0))
+							    pAd->StaCfg.AuthMode = Ndis802_11AuthModeShared;
+							else if ((strcmp(tmpbuf, "WPAPSK") == 0) || (strcmp(tmpbuf, "wpapsk") == 0))
+							    pAd->StaCfg.AuthMode = Ndis802_11AuthModeWPAPSK;
+							else if ((strcmp(tmpbuf, "WPANONE") == 0) || (strcmp(tmpbuf, "wpanone") == 0))
+							    pAd->StaCfg.AuthMode = Ndis802_11AuthModeWPANone;
+							else if ((strcmp(tmpbuf, "WPA2PSK") == 0) || (strcmp(tmpbuf, "wpa2psk") == 0))
+							    pAd->StaCfg.AuthMode = Ndis802_11AuthModeWPA2PSK;
+#ifdef WPA_SUPPLICANT_SUPPORT
+							else if ((strcmp(tmpbuf, "WPA") == 0) || (strcmp(tmpbuf, "wpa") == 0))
+							    pAd->StaCfg.AuthMode = Ndis802_11AuthModeWPA;
+							else if ((strcmp(tmpbuf, "WPA2") == 0) || (strcmp(tmpbuf, "wpa2") == 0))
+							    pAd->StaCfg.AuthMode = Ndis802_11AuthModeWPA2;
+#endif // WPA_SUPPLICANT_SUPPORT //
+				                        else
+				                            pAd->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+
+				                        pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+
+							DBGPRINT(RT_DEBUG_TRACE, ("%s::(EncrypType=%d)\n", __FUNCTION__, pAd->StaCfg.WepStatus));
+						}
+#endif // CONFIG_STA_SUPPORT //
+					}
+					//EncrypType
+					if(RTMPGetKeyParameter("EncrypType", tmpbuf, 128, buffer))
+					{
+
+#ifdef CONFIG_STA_SUPPORT
+						IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+						{
+							if ((strcmp(tmpbuf, "WEP") == 0) || (strcmp(tmpbuf, "wep") == 0))
+								pAd->StaCfg.WepStatus	= Ndis802_11WEPEnabled;
+							else if ((strcmp(tmpbuf, "TKIP") == 0) || (strcmp(tmpbuf, "tkip") == 0))
+								pAd->StaCfg.WepStatus	= Ndis802_11Encryption2Enabled;
+							else if ((strcmp(tmpbuf, "AES") == 0) || (strcmp(tmpbuf, "aes") == 0))
+								pAd->StaCfg.WepStatus	= Ndis802_11Encryption3Enabled;
+							else
+								pAd->StaCfg.WepStatus	= Ndis802_11WEPDisabled;
+
+							// Update all wepstatus related
+							pAd->StaCfg.PairCipher		= pAd->StaCfg.WepStatus;
+							pAd->StaCfg.GroupCipher 	= pAd->StaCfg.WepStatus;
+							pAd->StaCfg.OrigWepStatus 	= pAd->StaCfg.WepStatus;
+							pAd->StaCfg.bMixCipher 		= FALSE;
+
+							//RTMPMakeRSNIE(pAd, pAd->StaCfg.AuthMode, pAd->StaCfg.WepStatus, 0);
+							DBGPRINT(RT_DEBUG_TRACE, ("%s::(EncrypType=%d)\n", __FUNCTION__, pAd->StaCfg.WepStatus));
+						}
+#endif // CONFIG_STA_SUPPORT //
+					}
+
+
+
+#ifdef CONFIG_STA_SUPPORT
+					IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+					{
+						if(RTMPGetCriticalParameter("WPAPSK", tmpbuf, 512, buffer))
+						{
+							int     err=0;
+
+							tmpbuf[strlen(tmpbuf)] = '\0'; // make STA can process .$^& for WPAPSK input
+
+							if ((pAd->StaCfg.AuthMode != Ndis802_11AuthModeWPAPSK) &&
+								(pAd->StaCfg.AuthMode != Ndis802_11AuthModeWPA2PSK) &&
+								(pAd->StaCfg.AuthMode != Ndis802_11AuthModeWPANone)
+								)
+							{
+								err = 1;
+							}
+							else if ((strlen(tmpbuf) >= 8) && (strlen(tmpbuf) < 64))
+							{
+								PasswordHash((char *)tmpbuf, pAd->CommonCfg.Ssid, pAd->CommonCfg.SsidLen, keyMaterial);
+								NdisMoveMemory(pAd->StaCfg.PMK, keyMaterial, 32);
+
+							}
+							else if (strlen(tmpbuf) == 64)
+							{
+								AtoH(tmpbuf, keyMaterial, 32);
+								NdisMoveMemory(pAd->StaCfg.PMK, keyMaterial, 32);
+							}
+							else
+							{
+								err = 1;
+								DBGPRINT(RT_DEBUG_ERROR, ("%s::(WPAPSK key-string required 8 ~ 64 characters!)\n", __FUNCTION__));
+							}
+
+							if (err == 0)
+	                        			{
+	                        				if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+									(pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK))
+								{
+									// Start STA supplicant state machine
+									pAd->StaCfg.WpaState = SS_START;
+								}
+								else if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+								{
+	/*
+									NdisZeroMemory(&pAd->SharedKey[BSS0][0], sizeof(CIPHER_KEY));
+									pAd->SharedKey[BSS0][0].KeyLen = LEN_TKIP_EK;
+									NdisMoveMemory(pAd->SharedKey[BSS0][0].Key, pAd->StaCfg.PMK, LEN_TKIP_EK);
+									NdisMoveMemory(pAd->SharedKey[BSS0][0].RxMic, &pAd->StaCfg.PMK[16], LEN_TKIP_RXMICK);
+									NdisMoveMemory(pAd->SharedKey[BSS0][0].TxMic, &pAd->StaCfg.PMK[16], LEN_TKIP_TXMICK);
+
+									// Decide its ChiperAlg
+									if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
+										pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_TKIP;
+									else if (pAd->StaCfg.PairCipher == Ndis802_11Encryption3Enabled)
+										pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_AES;
+									else
+										pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_NONE;
+	*/
+									pAd->StaCfg.WpaState = SS_NOTUSE;
+								}
+
+								DBGPRINT(RT_DEBUG_TRACE, ("%s::(WPAPSK=%s)\n", __FUNCTION__, tmpbuf));
+							}
+						}
+					}
+#endif // CONFIG_STA_SUPPORT //
+
+					//DefaultKeyID, KeyType, KeyStr
+					rtmp_read_key_parms_from_file(pAd, tmpbuf, buffer);
+
+
+					//HSCounter
+					/*if(RTMPGetKeyParameter("HSCounter", tmpbuf, 10, buffer))
+					{
+						switch (simple_strtol(tmpbuf, 0, 10))
+						{
+							case 1: //Enable
+								pAd->CommonCfg.bEnableHSCounter = TRUE;
+								break;
+							case 0: //Disable
+							default:
+								pAd->CommonCfg.bEnableHSCounter = FALSE;
+								break;
+						}
+						DBGPRINT(RT_DEBUG_TRACE, "HSCounter=%d\n", pAd->CommonCfg.bEnableHSCounter);
+					}*/
+
+#ifdef DOT11_N_SUPPORT
+					HTParametersHook(pAd, tmpbuf, buffer);
+#endif // DOT11_N_SUPPORT //
+
+
+#ifdef CARRIER_DETECTION_SUPPORT
+						//CarrierDetect
+						if(RTMPGetKeyParameter("CarrierDetect", tmpbuf, 128, buffer))
+						{
+							if ((strncmp(tmpbuf, "0", 1) == 0))
+								pAd->CommonCfg.CarrierDetect.Enable = FALSE;
+							else if ((strncmp(tmpbuf, "1", 1) == 0))
+								pAd->CommonCfg.CarrierDetect.Enable = TRUE;
+							else
+								pAd->CommonCfg.CarrierDetect.Enable = FALSE;
+
+							DBGPRINT(RT_DEBUG_TRACE, ("CarrierDetect.Enable=%d\n", pAd->CommonCfg.CarrierDetect.Enable));
+						}
+						else
+							pAd->CommonCfg.CarrierDetect.Enable = FALSE;
+#endif // CARRIER_DETECTION_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+					IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+					{
+						//PSMode
+						if (RTMPGetKeyParameter("PSMode", tmpbuf, 10, buffer))
+						{
+							if (pAd->StaCfg.BssType == BSS_INFRA)
+							{
+								if ((strcmp(tmpbuf, "MAX_PSP") == 0) || (strcmp(tmpbuf, "max_psp") == 0))
+								{
+									// do NOT turn on PSM bit here, wait until MlmeCheckForPsmChange()
+									// to exclude certain situations.
+									//	   MlmeSetPsm(pAd, PWR_SAVE);
+									OPSTATUS_SET_FLAG(pAd, fOP_STATUS_RECEIVE_DTIM);
+									if (pAd->StaCfg.bWindowsACCAMEnable == FALSE)
+										pAd->StaCfg.WindowsPowerMode = Ndis802_11PowerModeMAX_PSP;
+									pAd->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeMAX_PSP;
+									pAd->StaCfg.DefaultListenCount = 5;
+								}
+								else if ((strcmp(tmpbuf, "Fast_PSP") == 0) || (strcmp(tmpbuf, "fast_psp") == 0)
+									|| (strcmp(tmpbuf, "FAST_PSP") == 0))
+								{
+									// do NOT turn on PSM bit here, wait until MlmeCheckForPsmChange()
+									// to exclude certain situations.
+									//	   MlmeSetPsmBit(pAd, PWR_SAVE);
+									OPSTATUS_SET_FLAG(pAd, fOP_STATUS_RECEIVE_DTIM);
+									if (pAd->StaCfg.bWindowsACCAMEnable == FALSE)
+										pAd->StaCfg.WindowsPowerMode = Ndis802_11PowerModeFast_PSP;
+									pAd->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeFast_PSP;
+									pAd->StaCfg.DefaultListenCount = 3;
+								}
+								else if ((strcmp(tmpbuf, "Legacy_PSP") == 0) || (strcmp(tmpbuf, "legacy_psp") == 0)
+									|| (strcmp(tmpbuf, "LEGACY_PSP") == 0))
+								{
+									// do NOT turn on PSM bit here, wait until MlmeCheckForPsmChange()
+									// to exclude certain situations.
+									//	   MlmeSetPsmBit(pAd, PWR_SAVE);
+									OPSTATUS_SET_FLAG(pAd, fOP_STATUS_RECEIVE_DTIM);
+									if (pAd->StaCfg.bWindowsACCAMEnable == FALSE)
+										pAd->StaCfg.WindowsPowerMode = Ndis802_11PowerModeLegacy_PSP;
+									pAd->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeLegacy_PSP;
+									pAd->StaCfg.DefaultListenCount = 3;
+								}
+								else
+								{ //Default Ndis802_11PowerModeCAM
+									// clear PSM bit immediately
+									MlmeSetPsmBit(pAd, PWR_ACTIVE);
+									OPSTATUS_SET_FLAG(pAd, fOP_STATUS_RECEIVE_DTIM);
+									if (pAd->StaCfg.bWindowsACCAMEnable == FALSE)
+										pAd->StaCfg.WindowsPowerMode = Ndis802_11PowerModeCAM;
+									pAd->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeCAM;
+								}
+								DBGPRINT(RT_DEBUG_TRACE, ("PSMode=%ld\n", pAd->StaCfg.WindowsPowerMode));
+							}
+						}
+						// FastRoaming
+						if (RTMPGetKeyParameter("FastRoaming", tmpbuf, 32, buffer))
+						{
+							if (simple_strtol(tmpbuf, 0, 10) == 0)
+								pAd->StaCfg.bFastRoaming = FALSE;
+							else
+								pAd->StaCfg.bFastRoaming = TRUE;
+
+							DBGPRINT(RT_DEBUG_TRACE, ("FastRoaming=%d\n", pAd->StaCfg.bFastRoaming));
+						}
+						// RoamThreshold
+						if (RTMPGetKeyParameter("RoamThreshold", tmpbuf, 32, buffer))
+						{
+							long lInfo = simple_strtol(tmpbuf, 0, 10);
+
+							if (lInfo > 90 || lInfo < 60)
+								pAd->StaCfg.dBmToRoam = -70;
+							else
+								pAd->StaCfg.dBmToRoam = (CHAR)(-1)*lInfo;
+
+							DBGPRINT(RT_DEBUG_TRACE, ("RoamThreshold=%d  dBm\n", pAd->StaCfg.dBmToRoam));
+						}
+
+						if(RTMPGetKeyParameter("TGnWifiTest", tmpbuf, 10, buffer))
+						{
+							if(simple_strtol(tmpbuf, 0, 10) == 0)
+								pAd->StaCfg.bTGnWifiTest = FALSE;
+							else
+								pAd->StaCfg.bTGnWifiTest = TRUE;
+								DBGPRINT(RT_DEBUG_TRACE, ("TGnWifiTest=%d\n", pAd->StaCfg.bTGnWifiTest));
+						}
+					}
+#endif // CONFIG_STA_SUPPORT //
+
+
+
+				}
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("--> %s does not have a write method\n", src));
+			}
+
+			retval=filp_close(srcf,NULL);
+
+			if (retval)
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("--> Error %d closing %s\n", -retval, src));
+			}
+		}
+	}
+
+	set_fs(orgfs);
+#if 0
+	current->fsuid = orgfsuid;
+	current->fsgid = orgfsgid;
+#endif
+
+	kfree(buffer);
+	kfree(tmpbuf);
+
+	return (NDIS_STATUS_SUCCESS);
+}
+
+#ifdef DOT11_N_SUPPORT
+static void	HTParametersHook(
+	IN	PRTMP_ADAPTER pAd,
+	IN	CHAR		  *pValueStr,
+	IN	CHAR		  *pInput)
+{
+
+	INT Value;
+
+    if (RTMPGetKeyParameter("HT_PROTECT", pValueStr, 25, pInput))
+    {
+        Value = simple_strtol(pValueStr, 0, 10);
+        if (Value == 0)
+        {
+            pAd->CommonCfg.bHTProtect = FALSE;
+        }
+        else
+        {
+            pAd->CommonCfg.bHTProtect = TRUE;
+        }
+        DBGPRINT(RT_DEBUG_TRACE, ("HT: Protection  = %s\n", (Value==0) ? "Disable" : "Enable"));
+    }
+
+    if (RTMPGetKeyParameter("HT_MIMOPSEnable", pValueStr, 25, pInput))
+    {
+        Value = simple_strtol(pValueStr, 0, 10);
+        if (Value == 0)
+        {
+            pAd->CommonCfg.bMIMOPSEnable = FALSE;
+        }
+        else
+        {
+            pAd->CommonCfg.bMIMOPSEnable = TRUE;
+        }
+        DBGPRINT(RT_DEBUG_TRACE, ("HT: MIMOPSEnable  = %s\n", (Value==0) ? "Disable" : "Enable"));
+    }
+
+
+    if (RTMPGetKeyParameter("HT_MIMOPSMode", pValueStr, 25, pInput))
+    {
+        Value = simple_strtol(pValueStr, 0, 10);
+        if (Value > MMPS_ENABLE)
+        {
+			pAd->CommonCfg.BACapability.field.MMPSmode = MMPS_ENABLE;
+        }
+        else
+        {
+            //TODO: add mimo power saving mechanism
+            pAd->CommonCfg.BACapability.field.MMPSmode = MMPS_ENABLE;
+			//pAd->CommonCfg.BACapability.field.MMPSmode = Value;
+        }
+        DBGPRINT(RT_DEBUG_TRACE, ("HT: MIMOPS Mode  = %d\n", Value));
+    }
+
+    if (RTMPGetKeyParameter("HT_BADecline", pValueStr, 25, pInput))
+    {
+        Value = simple_strtol(pValueStr, 0, 10);
+        if (Value == 0)
+        {
+            pAd->CommonCfg.bBADecline = FALSE;
+        }
+        else
+        {
+            pAd->CommonCfg.bBADecline = TRUE;
+        }
+        DBGPRINT(RT_DEBUG_TRACE, ("HT: BA Decline  = %s\n", (Value==0) ? "Disable" : "Enable"));
+    }
+
+
+    if (RTMPGetKeyParameter("HT_DisableReordering", pValueStr, 25, pInput))
+    {
+        Value = simple_strtol(pValueStr, 0, 10);
+        if (Value == 0)
+        {
+            pAd->CommonCfg.bDisableReordering = FALSE;
+        }
+        else
+        {
+            pAd->CommonCfg.bDisableReordering = TRUE;
+        }
+        DBGPRINT(RT_DEBUG_TRACE, ("HT: DisableReordering  = %s\n", (Value==0) ? "Disable" : "Enable"));
+    }
+
+    if (RTMPGetKeyParameter("HT_AutoBA", pValueStr, 25, pInput))
+    {
+        Value = simple_strtol(pValueStr, 0, 10);
+        if (Value == 0)
+        {
+            pAd->CommonCfg.BACapability.field.AutoBA = FALSE;
+        }
+        else
+        {
+            pAd->CommonCfg.BACapability.field.AutoBA = TRUE;
+        }
+        pAd->CommonCfg.REGBACapability.field.AutoBA = pAd->CommonCfg.BACapability.field.AutoBA;
+        DBGPRINT(RT_DEBUG_TRACE, ("HT: Auto BA  = %s\n", (Value==0) ? "Disable" : "Enable"));
+    }
+
+	// Tx_+HTC frame
+    if (RTMPGetKeyParameter("HT_HTC", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+		if (Value == 0)
+		{
+			pAd->HTCEnable = FALSE;
+		}
+		else
+		{
+            		pAd->HTCEnable = TRUE;
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: Tx +HTC frame = %s\n", (Value==0) ? "Disable" : "Enable"));
+	}
+
+	// Enable HT Link Adaptation Control
+	if (RTMPGetKeyParameter("HT_LinkAdapt", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+		if (Value == 0)
+		{
+			pAd->bLinkAdapt = FALSE;
+		}
+		else
+		{
+			pAd->HTCEnable = TRUE;
+			pAd->bLinkAdapt = TRUE;
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: Link Adaptation Control = %s\n", (Value==0) ? "Disable" : "Enable(+HTC)"));
+	}
+
+	// Reverse Direction Mechanism
+    if (RTMPGetKeyParameter("HT_RDG", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+		if (Value == 0)
+		{
+			pAd->CommonCfg.bRdg = FALSE;
+		}
+		else
+		{
+			pAd->HTCEnable = TRUE;
+            pAd->CommonCfg.bRdg = TRUE;
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: RDG = %s\n", (Value==0) ? "Disable" : "Enable(+HTC)"));
+	}
+
+
+
+
+	// Tx A-MSUD ?
+    if (RTMPGetKeyParameter("HT_AMSDU", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+		if (Value == 0)
+		{
+			pAd->CommonCfg.BACapability.field.AmsduEnable = FALSE;
+		}
+		else
+		{
+            pAd->CommonCfg.BACapability.field.AmsduEnable = TRUE;
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: Tx A-MSDU = %s\n", (Value==0) ? "Disable" : "Enable"));
+	}
+
+	// MPDU Density
+    if (RTMPGetKeyParameter("HT_MpduDensity", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+		if (Value <=7 && Value >= 0)
+		{
+			pAd->CommonCfg.BACapability.field.MpduDensity = Value;
+			DBGPRINT(RT_DEBUG_TRACE, ("HT: MPDU Density = %d\n", Value));
+		}
+		else
+		{
+			pAd->CommonCfg.BACapability.field.MpduDensity = 4;
+			DBGPRINT(RT_DEBUG_TRACE, ("HT: MPDU Density = %d (Default)\n", 4));
+		}
+	}
+
+	// Max Rx BA Window Size
+    if (RTMPGetKeyParameter("HT_BAWinSize", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+
+		if (Value >=1 && Value <= 64)
+		{
+			pAd->CommonCfg.REGBACapability.field.RxBAWinLimit = Value;
+			pAd->CommonCfg.BACapability.field.RxBAWinLimit = Value;
+			DBGPRINT(RT_DEBUG_TRACE, ("HT: BA Windw Size = %d\n", Value));
+		}
+		else
+		{
+            pAd->CommonCfg.REGBACapability.field.RxBAWinLimit = 64;
+			pAd->CommonCfg.BACapability.field.RxBAWinLimit = 64;
+			DBGPRINT(RT_DEBUG_TRACE, ("HT: BA Windw Size = 64 (Defualt)\n"));
+		}
+
+	}
+
+	// Guard Interval
+	if (RTMPGetKeyParameter("HT_GI", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+
+		if (Value == GI_400)
+		{
+			pAd->CommonCfg.RegTransmitSetting.field.ShortGI = GI_400;
+		}
+		else
+		{
+			pAd->CommonCfg.RegTransmitSetting.field.ShortGI = GI_800;
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: Guard Interval = %s\n", (Value==GI_400) ? "400" : "800" ));
+	}
+
+	// HT Operation Mode : Mixed Mode , Green Field
+	if (RTMPGetKeyParameter("HT_OpMode", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+
+		if (Value == HTMODE_GF)
+		{
+
+			pAd->CommonCfg.RegTransmitSetting.field.HTMODE  = HTMODE_GF;
+		}
+		else
+		{
+			pAd->CommonCfg.RegTransmitSetting.field.HTMODE  = HTMODE_MM;
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: Operate Mode = %s\n", (Value==HTMODE_GF) ? "Green Field" : "Mixed Mode" ));
+	}
+
+	// Fixed Tx mode : CCK, OFDM
+	if (RTMPGetKeyParameter("FixedTxMode", pValueStr, 25, pInput))
+	{
+		UCHAR	fix_tx_mode;
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			fix_tx_mode = FIXED_TXMODE_HT;
+
+			if (strcmp(pValueStr, "OFDM") == 0 || strcmp(pValueStr, "ofdm") == 0)
+			{
+				fix_tx_mode = FIXED_TXMODE_OFDM;
+			}
+			else if (strcmp(pValueStr, "CCK") == 0 || strcmp(pValueStr, "cck") == 0)
+			{
+		        fix_tx_mode = FIXED_TXMODE_CCK;
+			}
+			else if (strcmp(pValueStr, "HT") == 0 || strcmp(pValueStr, "ht") == 0)
+			{
+		        fix_tx_mode = FIXED_TXMODE_HT;
+		}
+		else
+		{
+				Value = simple_strtol(pValueStr, 0, 10);
+				// 1 : CCK
+				// 2 : OFDM
+				// otherwise : HT
+				if (Value == FIXED_TXMODE_CCK || Value == FIXED_TXMODE_OFDM)
+					fix_tx_mode = Value;
+				else
+					fix_tx_mode = FIXED_TXMODE_HT;
+		}
+
+			pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode = fix_tx_mode;
+			DBGPRINT(RT_DEBUG_TRACE, ("Fixed Tx Mode = %d\n", fix_tx_mode));
+
+		}
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+
+	// Channel Width
+	if (RTMPGetKeyParameter("HT_BW", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+
+		if (Value == BW_40)
+		{
+			pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_40;
+		}
+		else
+		{
+            pAd->CommonCfg.RegTransmitSetting.field.BW  = BW_20;
+		}
+
+#ifdef MCAST_RATE_SPECIFIC
+		pAd->CommonCfg.MCastPhyMode.field.BW = pAd->CommonCfg.RegTransmitSetting.field.BW;
+#endif // MCAST_RATE_SPECIFIC //
+
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: Channel Width = %s\n", (Value==BW_40) ? "40 MHz" : "20 MHz" ));
+	}
+
+	if (RTMPGetKeyParameter("HT_EXTCHA", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+
+		if (Value == 0)
+		{
+
+			pAd->CommonCfg.RegTransmitSetting.field.EXTCHA  = EXTCHA_BELOW;
+		}
+		else
+		{
+            pAd->CommonCfg.RegTransmitSetting.field.EXTCHA = EXTCHA_ABOVE;
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: Ext Channel = %s\n", (Value==0) ? "BELOW" : "ABOVE" ));
+	}
+
+	// MSC
+	if (RTMPGetKeyParameter("HT_MCS", pValueStr, 50, pInput))
+	{
+
+#ifdef CONFIG_STA_SUPPORT
+		IF_DEV_CONFIG_OPMODE_ON_STA(pAd)
+		{
+			Value = simple_strtol(pValueStr, 0, 10);
+
+//			if ((Value >= 0 && Value <= 15) || (Value == 32))
+			if ((Value >= 0 && Value <= 23) || (Value == 32)) // 3*3
+		{
+				pAd->StaCfg.DesiredTransmitSetting.field.MCS  = Value;
+				pAd->StaCfg.bAutoTxRateSwitch = FALSE;
+				DBGPRINT(RT_DEBUG_TRACE, ("HT: MCS = %d\n", pAd->StaCfg.DesiredTransmitSetting.field.MCS));
+		}
+		else
+		{
+				pAd->StaCfg.DesiredTransmitSetting.field.MCS  = MCS_AUTO;
+				pAd->StaCfg.bAutoTxRateSwitch = TRUE;
+				DBGPRINT(RT_DEBUG_TRACE, ("HT: MCS = AUTO\n"));
+		}
+	}
+#endif // CONFIG_STA_SUPPORT //
+	}
+
+	// STBC
+    if (RTMPGetKeyParameter("HT_STBC", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+		if (Value == STBC_USE)
+		{
+			pAd->CommonCfg.RegTransmitSetting.field.STBC = STBC_USE;
+		}
+		else
+		{
+			pAd->CommonCfg.RegTransmitSetting.field.STBC = STBC_NONE;
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: STBC = %d\n", pAd->CommonCfg.RegTransmitSetting.field.STBC));
+	}
+
+	// 40_Mhz_Intolerant
+	if (RTMPGetKeyParameter("HT_40MHZ_INTOLERANT", pValueStr, 25, pInput))
+	{
+		Value = simple_strtol(pValueStr, 0, 10);
+		if (Value == 0)
+		{
+			pAd->CommonCfg.bForty_Mhz_Intolerant = FALSE;
+		}
+		else
+		{
+			pAd->CommonCfg.bForty_Mhz_Intolerant = TRUE;
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: 40MHZ INTOLERANT = %d\n", pAd->CommonCfg.bForty_Mhz_Intolerant));
+	}
+	//HT_TxStream
+	if(RTMPGetKeyParameter("HT_TxStream", pValueStr, 10, pInput))
+	{
+		switch (simple_strtol(pValueStr, 0, 10))
+		{
+			case 1:
+				pAd->CommonCfg.TxStream = 1;
+				break;
+			case 2:
+				pAd->CommonCfg.TxStream = 2;
+				break;
+			case 3: // 3*3
+			default:
+				pAd->CommonCfg.TxStream = 3;
+
+				if (pAd->MACVersion < RALINK_2883_VERSION)
+					pAd->CommonCfg.TxStream = 2; // only 2 tx streams for RT2860 series
+				break;
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: Tx Stream = %d\n", pAd->CommonCfg.TxStream));
+	}
+	//HT_RxStream
+	if(RTMPGetKeyParameter("HT_RxStream", pValueStr, 10, pInput))
+	{
+		switch (simple_strtol(pValueStr, 0, 10))
+		{
+			case 1:
+				pAd->CommonCfg.RxStream = 1;
+				break;
+			case 2:
+				pAd->CommonCfg.RxStream = 2;
+				break;
+			case 3:
+			default:
+				pAd->CommonCfg.RxStream = 3;
+
+				if (pAd->MACVersion < RALINK_2883_VERSION)
+					pAd->CommonCfg.RxStream = 2; // only 2 rx streams for RT2860 series
+				break;
+		}
+		DBGPRINT(RT_DEBUG_TRACE, ("HT: Rx Stream = %d\n", pAd->CommonCfg.RxStream));
+	}
+
+}
+#endif // DOT11_N_SUPPORT //
+
diff --git a/drivers/staging/rt2870/rtmp.h b/drivers/staging/rt2870/rtmp.h
new file mode 100644
index 00000000000..c2a4784ec33
--- /dev/null
+++ b/drivers/staging/rt2870/rtmp.h
@@ -0,0 +1,7586 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    rtmp.h
+
+    Abstract:
+    Miniport generic portion header file
+
+    Revision History:
+    Who         When          What
+    --------    ----------    ----------------------------------------------
+    Paul Lin    2002-08-01    created
+    James Tan   2002-09-06    modified (Revise NTCRegTable)
+    John Chang  2004-09-06    modified for RT2600
+*/
+#ifndef __RTMP_H__
+#define __RTMP_H__
+
+#include "link_list.h"
+#include "spectrum_def.h"
+
+
+#ifdef CONFIG_STA_SUPPORT
+#include "aironet.h"
+#endif // CONFIG_STA_SUPPORT //
+
+//#define DBG		1
+
+//#define DBG_DIAGNOSE		1
+
+#if defined(CONFIG_AP_SUPPORT) && defined(CONFIG_STA_SUPPORT)
+#define IF_DEV_CONFIG_OPMODE_ON_AP(_pAd)	if(_pAd->OpMode == OPMODE_AP)
+#define IF_DEV_CONFIG_OPMODE_ON_STA(_pAd)	if(_pAd->OpMode == OPMODE_STA)
+#else
+#define IF_DEV_CONFIG_OPMODE_ON_AP(_pAd)
+#define IF_DEV_CONFIG_OPMODE_ON_STA(_pAd)
+#endif
+
+#define VIRTUAL_IF_INC(__pAd) ((__pAd)->VirtualIfCnt++)
+#define VIRTUAL_IF_DEC(__pAd) ((__pAd)->VirtualIfCnt--)
+#define VIRTUAL_IF_NUM(__pAd) ((__pAd)->VirtualIfCnt)
+
+#ifdef RT2870
+////////////////////////////////////////////////////////////////////////////
+// The TX_BUFFER structure forms the transmitted USB packet to the device
+////////////////////////////////////////////////////////////////////////////
+typedef struct __TX_BUFFER{
+	union	{
+		UCHAR			WirelessPacket[TX_BUFFER_NORMSIZE];
+		HEADER_802_11	NullFrame;
+		PSPOLL_FRAME	PsPollPacket;
+		RTS_FRAME		RTSFrame;
+	}field;
+	UCHAR			Aggregation[4];  //Buffer for save Aggregation size.
+} TX_BUFFER, *PTX_BUFFER;
+
+typedef struct __HTTX_BUFFER{
+	union	{
+		UCHAR			WirelessPacket[MAX_TXBULK_SIZE];
+		HEADER_802_11	NullFrame;
+		PSPOLL_FRAME	PsPollPacket;
+		RTS_FRAME		RTSFrame;
+	}field;
+	UCHAR			Aggregation[4];  //Buffer for save Aggregation size.
+} HTTX_BUFFER, *PHTTX_BUFFER;
+
+
+// used to track driver-generated write irps
+typedef struct _TX_CONTEXT
+{
+	PVOID			pAd;		//Initialized in MiniportInitialize
+	PURB			pUrb;			//Initialized in MiniportInitialize
+	PIRP			pIrp;			//used to cancel pending bulk out.
+									//Initialized in MiniportInitialize
+	PTX_BUFFER		TransferBuffer;	//Initialized in MiniportInitialize
+	ULONG			BulkOutSize;
+	UCHAR			BulkOutPipeId;
+	UCHAR			SelfIdx;
+	BOOLEAN			InUse;
+	BOOLEAN			bWaitingBulkOut; // at least one packet is in this TxContext, ready for making IRP anytime.
+	BOOLEAN			bFullForBulkOut; // all tx buffer are full , so waiting for tx bulkout.
+	BOOLEAN			IRPPending;
+	BOOLEAN			LastOne;
+	BOOLEAN			bAggregatible;
+	UCHAR			Header_802_3[LENGTH_802_3];
+	UCHAR			Rsv[2];
+	ULONG			DataOffset;
+	UINT			TxRate;
+	dma_addr_t		data_dma;		// urb dma on linux
+
+}	TX_CONTEXT, *PTX_CONTEXT, **PPTX_CONTEXT;
+
+
+// used to track driver-generated write irps
+typedef struct _HT_TX_CONTEXT
+{
+	PVOID			pAd;		//Initialized in MiniportInitialize
+	PURB			pUrb;			//Initialized in MiniportInitialize
+	PIRP			pIrp;			//used to cancel pending bulk out.
+									//Initialized in MiniportInitialize
+	PHTTX_BUFFER	TransferBuffer;	//Initialized in MiniportInitialize
+	ULONG			BulkOutSize;	// Indicate the total bulk-out size in bytes in one bulk-transmission
+	UCHAR			BulkOutPipeId;
+	BOOLEAN			IRPPending;
+	BOOLEAN			LastOne;
+	BOOLEAN			bCurWriting;
+	BOOLEAN			bRingEmpty;
+	BOOLEAN			bCopySavePad;
+	UCHAR			SavedPad[8];
+	UCHAR			Header_802_3[LENGTH_802_3];
+	ULONG			CurWritePosition;		// Indicate the buffer offset which packet will be inserted start from.
+	ULONG			CurWriteRealPos;		// Indicate the buffer offset which packet now are writing to.
+	ULONG			NextBulkOutPosition;	// Indicate the buffer start offset of a bulk-transmission
+	ULONG			ENextBulkOutPosition;	// Indicate the buffer end offset of a bulk-transmission
+	UINT			TxRate;
+	dma_addr_t		data_dma;		// urb dma on linux
+}	HT_TX_CONTEXT, *PHT_TX_CONTEXT, **PPHT_TX_CONTEXT;
+
+
+//
+// Structure to keep track of receive packets and buffers to indicate
+// receive data to the protocol.
+//
+typedef struct _RX_CONTEXT
+{
+	PUCHAR				TransferBuffer;
+	PVOID				pAd;
+	PIRP				pIrp;//used to cancel pending bulk in.
+	PURB				pUrb;
+	//These 2 Boolean shouldn't both be 1 at the same time.
+	ULONG				BulkInOffset;	// number of packets waiting for reordering .
+//	BOOLEAN				ReorderInUse;	// At least one packet in this buffer are in reordering buffer and wait for receive indication
+	BOOLEAN				bRxHandling;	// Notify this packet is being process now.
+	BOOLEAN				InUse;			// USB Hardware Occupied. Wait for USB HW to put packet.
+	BOOLEAN				Readable;		// Receive Complete back. OK for driver to indicate receiving packet.
+	BOOLEAN				IRPPending;		// TODO: To be removed
+	atomic_t			IrpLock;
+	NDIS_SPIN_LOCK		RxContextLock;
+	dma_addr_t			data_dma;		// urb dma on linux
+}	RX_CONTEXT, *PRX_CONTEXT;
+#endif // RT2870 //
+
+
+//
+//  NDIS Version definitions
+//
+#ifdef  NDIS50_MINIPORT
+#define RTMP_NDIS_MAJOR_VERSION     5
+#define RTMP_NDIS_MINOR_VERSION     0
+#endif
+
+#ifdef  NDIS51_MINIPORT
+#define RTMP_NDIS_MAJOR_VERSION     5
+#define RTMP_NDIS_MINOR_VERSION     1
+#endif
+
+extern  char    NIC_VENDOR_DESC[];
+extern  int     NIC_VENDOR_DESC_LEN;
+
+extern  unsigned char   SNAP_AIRONET[];
+extern  unsigned char   CipherSuiteCiscoCCKM[];
+extern  unsigned char   CipherSuiteCiscoCCKMLen;
+extern	unsigned char	CipherSuiteCiscoCCKM24[];
+extern	unsigned char	CipherSuiteCiscoCCKM24Len;
+extern  unsigned char   CipherSuiteCCXTkip[];
+extern  unsigned char   CipherSuiteCCXTkipLen;
+extern  unsigned char   CISCO_OUI[];
+extern  UCHAR	BaSizeArray[4];
+
+extern UCHAR BROADCAST_ADDR[MAC_ADDR_LEN];
+extern UCHAR MULTICAST_ADDR[MAC_ADDR_LEN];
+extern UCHAR ZERO_MAC_ADDR[MAC_ADDR_LEN];
+extern ULONG BIT32[32];
+extern UCHAR BIT8[8];
+extern char* CipherName[];
+extern char* MCSToMbps[];
+extern UCHAR	 RxwiMCSToOfdmRate[12];
+extern UCHAR SNAP_802_1H[6];
+extern UCHAR SNAP_BRIDGE_TUNNEL[6];
+extern UCHAR SNAP_AIRONET[8];
+extern UCHAR CKIP_LLC_SNAP[8];
+extern UCHAR EAPOL_LLC_SNAP[8];
+extern UCHAR EAPOL[2];
+extern UCHAR IPX[2];
+extern UCHAR APPLE_TALK[2];
+extern UCHAR RateIdToPlcpSignal[12]; // see IEEE802.11a-1999 p.14
+extern UCHAR	 OfdmRateToRxwiMCS[];
+extern UCHAR OfdmSignalToRateId[16] ;
+extern UCHAR default_cwmin[4];
+extern UCHAR default_cwmax[4];
+extern UCHAR default_sta_aifsn[4];
+extern UCHAR MapUserPriorityToAccessCategory[8];
+
+extern USHORT RateUpPER[];
+extern USHORT RateDownPER[];
+extern UCHAR  Phy11BNextRateDownward[];
+extern UCHAR  Phy11BNextRateUpward[];
+extern UCHAR  Phy11BGNextRateDownward[];
+extern UCHAR  Phy11BGNextRateUpward[];
+extern UCHAR  Phy11ANextRateDownward[];
+extern UCHAR  Phy11ANextRateUpward[];
+extern CHAR   RssiSafeLevelForTxRate[];
+extern UCHAR  RateIdToMbps[];
+extern USHORT RateIdTo500Kbps[];
+
+extern UCHAR  CipherSuiteWpaNoneTkip[];
+extern UCHAR  CipherSuiteWpaNoneTkipLen;
+
+extern UCHAR  CipherSuiteWpaNoneAes[];
+extern UCHAR  CipherSuiteWpaNoneAesLen;
+
+extern UCHAR  SsidIe;
+extern UCHAR  SupRateIe;
+extern UCHAR  ExtRateIe;
+
+#ifdef DOT11_N_SUPPORT
+extern UCHAR  HtCapIe;
+extern UCHAR  AddHtInfoIe;
+extern UCHAR  NewExtChanIe;
+#ifdef DOT11N_DRAFT3
+extern UCHAR  ExtHtCapIe;
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+
+extern UCHAR  ErpIe;
+extern UCHAR  DsIe;
+extern UCHAR  TimIe;
+extern UCHAR  WpaIe;
+extern UCHAR  Wpa2Ie;
+extern UCHAR  IbssIe;
+extern UCHAR  Ccx2Ie;
+
+extern UCHAR  WPA_OUI[];
+extern UCHAR  RSN_OUI[];
+extern UCHAR  WME_INFO_ELEM[];
+extern UCHAR  WME_PARM_ELEM[];
+extern UCHAR  Ccx2QosInfo[];
+extern UCHAR  Ccx2IeInfo[];
+extern UCHAR  RALINK_OUI[];
+extern UCHAR  PowerConstraintIE[];
+
+
+extern UCHAR  RateSwitchTable[];
+extern UCHAR  RateSwitchTable11B[];
+extern UCHAR  RateSwitchTable11G[];
+extern UCHAR  RateSwitchTable11BG[];
+
+#ifdef DOT11_N_SUPPORT
+extern UCHAR  RateSwitchTable11BGN1S[];
+extern UCHAR  RateSwitchTable11BGN2S[];
+extern UCHAR  RateSwitchTable11BGN2SForABand[];
+extern UCHAR  RateSwitchTable11N1S[];
+extern UCHAR  RateSwitchTable11N2S[];
+extern UCHAR  RateSwitchTable11N2SForABand[];
+
+#ifdef CONFIG_STA_SUPPORT
+extern UCHAR  PRE_N_HT_OUI[];
+#endif // CONFIG_STA_SUPPORT //
+#endif // DOT11_N_SUPPORT //
+
+#define	MAXSEQ		(0xFFF)
+
+#ifdef RALINK_ATE
+typedef	struct _ATE_INFO {
+	UCHAR	Mode;
+	CHAR	TxPower0;
+	CHAR	TxPower1;
+	CHAR    TxAntennaSel;
+	CHAR    RxAntennaSel;
+	TXWI_STRUC  TxWI; 	  // TXWI
+	USHORT	QID;
+	UCHAR	Addr1[MAC_ADDR_LEN];
+	UCHAR	Addr2[MAC_ADDR_LEN];
+	UCHAR	Addr3[MAC_ADDR_LEN];
+	UCHAR	Channel;
+	UINT32	TxLength;
+	UINT32	TxCount;
+	UINT32	TxDoneCount; // Tx DMA Done
+	UINT32	RFFreqOffset;
+	BOOLEAN	bRxFer;
+	BOOLEAN	bQATxStart; // Have compiled QA in and use it to ATE tx.
+	BOOLEAN	bQARxStart;	// Have compiled QA in and use it to ATE rx.
+	UINT32	RxTotalCnt;
+	UINT32	RxCntPerSec;
+
+	CHAR	LastSNR0;             // last received SNR
+	CHAR    LastSNR1;             // last received SNR for 2nd  antenna
+	CHAR    LastRssi0;            // last received RSSI
+	CHAR    LastRssi1;            // last received RSSI for 2nd  antenna
+	CHAR    LastRssi2;            // last received RSSI for 3rd  antenna
+	CHAR    AvgRssi0;             // last 8 frames' average RSSI
+	CHAR    AvgRssi1;             // last 8 frames' average RSSI
+	CHAR    AvgRssi2;             // last 8 frames' average RSSI
+	SHORT   AvgRssi0X8;           // sum of last 8 frames' RSSI
+	SHORT   AvgRssi1X8;           // sum of last 8 frames' RSSI
+	SHORT   AvgRssi2X8;           // sum of last 8 frames' RSSI
+
+	UINT32	NumOfAvgRssiSample;
+
+#ifdef RALINK_28xx_QA
+	// Tx frame
+#ifdef RT2870
+	/* not used in RT2860 */
+	TXINFO_STRUC		TxInfo; // TxInfo
+#endif // RT2870 //
+	USHORT		HLen; // Header Length
+	USHORT		PLen; // Pattern Length
+	UCHAR 		Header[32]; // Header buffer
+	UCHAR		Pattern[32]; // Pattern buffer
+	USHORT		DLen; // Data Length
+	USHORT		seq;
+	UINT32		CID;
+	THREAD_PID 		AtePid;
+	// counters
+	UINT32		U2M;
+	UINT32		OtherData;
+	UINT32		Beacon;
+	UINT32		OtherCount;
+	UINT32		TxAc0;
+	UINT32		TxAc1;
+	UINT32		TxAc2;
+	UINT32		TxAc3;
+	UINT32		TxHCCA;
+	UINT32		TxMgmt;
+	UINT32		RSSI0;
+	UINT32		RSSI1;
+	UINT32		RSSI2;
+	UINT32		SNR0;
+	UINT32		SNR1;
+	// control
+	//UINT32		Repeat; // Tx Cpu count
+	UCHAR		TxStatus; // task Tx status // 0 --> task is idle, 1 --> task is running
+#endif // RALINK_28xx_QA //
+}	ATE_INFO, *PATE_INFO;
+
+#ifdef RALINK_28xx_QA
+struct ate_racfghdr {
+ 	UINT32		magic_no;
+	USHORT		command_type;
+	USHORT		command_id;
+	USHORT		length;
+	USHORT		sequence;
+	USHORT		status;
+	UCHAR		data[2046];
+}  __attribute__((packed));
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+
+#ifdef DOT11_N_SUPPORT
+struct reordering_mpdu
+{
+	struct reordering_mpdu	*next;
+	PNDIS_PACKET			pPacket;		/* coverted to 802.3 frame */
+	int						Sequence;		/* sequence number of MPDU */
+	BOOLEAN					bAMSDU;
+};
+
+struct reordering_list
+{
+	struct reordering_mpdu *next;
+	int 	qlen;
+};
+
+struct reordering_mpdu_pool
+{
+	PVOID					mem;
+	NDIS_SPIN_LOCK			lock;
+	struct reordering_list 	freelist;
+};
+#endif // DOT11_N_SUPPORT //
+
+typedef struct 	_RSSI_SAMPLE {
+	CHAR			LastRssi0;             // last received RSSI
+	CHAR			LastRssi1;             // last received RSSI
+	CHAR			LastRssi2;             // last received RSSI
+	CHAR			AvgRssi0;
+	CHAR			AvgRssi1;
+	CHAR			AvgRssi2;
+	SHORT			AvgRssi0X8;
+	SHORT			AvgRssi1X8;
+	SHORT			AvgRssi2X8;
+} RSSI_SAMPLE;
+
+//
+//  Queue structure and macros
+//
+typedef struct  _QUEUE_ENTRY    {
+	struct _QUEUE_ENTRY     *Next;
+}   QUEUE_ENTRY, *PQUEUE_ENTRY;
+
+// Queue structure
+typedef struct  _QUEUE_HEADER   {
+	PQUEUE_ENTRY    Head;
+	PQUEUE_ENTRY    Tail;
+	ULONG           Number;
+}   QUEUE_HEADER, *PQUEUE_HEADER;
+
+#define InitializeQueueHeader(QueueHeader)              \
+{                                                       \
+	(QueueHeader)->Head = (QueueHeader)->Tail = NULL;   \
+	(QueueHeader)->Number = 0;                          \
+}
+
+#define RemoveHeadQueue(QueueHeader)                \
+(QueueHeader)->Head;                                \
+{                                                   \
+	PQUEUE_ENTRY pNext;                             \
+	if ((QueueHeader)->Head != NULL)				\
+	{												\
+		pNext = (QueueHeader)->Head->Next;          \
+		(QueueHeader)->Head = pNext;                \
+		if (pNext == NULL)                          \
+			(QueueHeader)->Tail = NULL;             \
+		(QueueHeader)->Number--;                    \
+	}												\
+}
+
+#define InsertHeadQueue(QueueHeader, QueueEntry)            \
+{                                                           \
+		((PQUEUE_ENTRY)QueueEntry)->Next = (QueueHeader)->Head; \
+		(QueueHeader)->Head = (PQUEUE_ENTRY)(QueueEntry);       \
+		if ((QueueHeader)->Tail == NULL)                        \
+			(QueueHeader)->Tail = (PQUEUE_ENTRY)(QueueEntry);   \
+		(QueueHeader)->Number++;                                \
+}
+
+#define InsertTailQueue(QueueHeader, QueueEntry)                \
+{                                                               \
+	((PQUEUE_ENTRY)QueueEntry)->Next = NULL;                    \
+	if ((QueueHeader)->Tail)                                    \
+		(QueueHeader)->Tail->Next = (PQUEUE_ENTRY)(QueueEntry); \
+	else                                                        \
+		(QueueHeader)->Head = (PQUEUE_ENTRY)(QueueEntry);       \
+	(QueueHeader)->Tail = (PQUEUE_ENTRY)(QueueEntry);           \
+	(QueueHeader)->Number++;                                    \
+}
+
+//
+//  Macros for flag and ref count operations
+//
+#define RTMP_SET_FLAG(_M, _F)       ((_M)->Flags |= (_F))
+#define RTMP_CLEAR_FLAG(_M, _F)     ((_M)->Flags &= ~(_F))
+#define RTMP_CLEAR_FLAGS(_M)        ((_M)->Flags = 0)
+#define RTMP_TEST_FLAG(_M, _F)      (((_M)->Flags & (_F)) != 0)
+#define RTMP_TEST_FLAGS(_M, _F)     (((_M)->Flags & (_F)) == (_F))
+
+#define OPSTATUS_SET_FLAG(_pAd, _F)     ((_pAd)->CommonCfg.OpStatusFlags |= (_F))
+#define OPSTATUS_CLEAR_FLAG(_pAd, _F)   ((_pAd)->CommonCfg.OpStatusFlags &= ~(_F))
+#define OPSTATUS_TEST_FLAG(_pAd, _F)    (((_pAd)->CommonCfg.OpStatusFlags & (_F)) != 0)
+
+#define CLIENT_STATUS_SET_FLAG(_pEntry,_F)      ((_pEntry)->ClientStatusFlags |= (_F))
+#define CLIENT_STATUS_CLEAR_FLAG(_pEntry,_F)    ((_pEntry)->ClientStatusFlags &= ~(_F))
+#define CLIENT_STATUS_TEST_FLAG(_pEntry,_F)     (((_pEntry)->ClientStatusFlags & (_F)) != 0)
+
+#define RX_FILTER_SET_FLAG(_pAd, _F)    ((_pAd)->CommonCfg.PacketFilter |= (_F))
+#define RX_FILTER_CLEAR_FLAG(_pAd, _F)  ((_pAd)->CommonCfg.PacketFilter &= ~(_F))
+#define RX_FILTER_TEST_FLAG(_pAd, _F)   (((_pAd)->CommonCfg.PacketFilter & (_F)) != 0)
+
+#ifdef CONFIG_STA_SUPPORT
+#define STA_NO_SECURITY_ON(_p)          (_p->StaCfg.WepStatus == Ndis802_11EncryptionDisabled)
+#define STA_WEP_ON(_p)                  (_p->StaCfg.WepStatus == Ndis802_11Encryption1Enabled)
+#define STA_TKIP_ON(_p)                 (_p->StaCfg.WepStatus == Ndis802_11Encryption2Enabled)
+#define STA_AES_ON(_p)                  (_p->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+
+#define STA_TGN_WIFI_ON(_p)             (_p->StaCfg.bTGnWifiTest == TRUE)
+#endif // CONFIG_STA_SUPPORT //
+
+#define CKIP_KP_ON(_p)				((((_p)->StaCfg.CkipFlag) & 0x10) && ((_p)->StaCfg.bCkipCmicOn == TRUE))
+#define CKIP_CMIC_ON(_p)			((((_p)->StaCfg.CkipFlag) & 0x08) && ((_p)->StaCfg.bCkipCmicOn == TRUE))
+
+
+#define INC_RING_INDEX(_idx, _RingSize)    \
+{                                          \
+    (_idx) = (_idx+1) % (_RingSize);       \
+}
+
+#define IS_RT3070(_pAd)				(((_pAd)->MACVersion & 0xffff0000) == 0x30700000)
+
+#define RING_PACKET_INIT(_TxRing, _idx)    \
+{                                          \
+    _TxRing->Cell[_idx].pNdisPacket = NULL;                              \
+    _TxRing->Cell[_idx].pNextNdisPacket = NULL;                              \
+}
+
+#define TXDT_INIT(_TxD)    \
+{                                          \
+	NdisZeroMemory(_TxD, TXD_SIZE);	\
+	_TxD->DMADONE = 1;                              \
+}
+
+//Set last data segment
+#define RING_SET_LASTDS(_TxD, _IsSD0)    \
+{                                          \
+    if (_IsSD0) {_TxD->LastSec0 = 1;}     \
+    else {_TxD->LastSec1 = 1;}     \
+}
+
+// Increase TxTsc value for next transmission
+// TODO:
+// When i==6, means TSC has done one full cycle, do re-keying stuff follow specs
+// Should send a special event microsoft defined to request re-key
+#define INC_TX_TSC(_tsc)                                \
+{                                                       \
+    int i=0;                                            \
+    while (++_tsc[i] == 0x0)                            \
+    {                                                   \
+        i++;                                            \
+        if (i == 6)                                     \
+            break;                                      \
+    }                                                   \
+}
+
+#ifdef DOT11_N_SUPPORT
+// StaActive.SupportedHtPhy.MCSSet is copied from AP beacon.  Don't need to update here.
+#define COPY_HTSETTINGS_FROM_MLME_AUX_TO_ACTIVE_CFG(_pAd)                                 \
+{                                                                                       \
+	_pAd->StaActive.SupportedHtPhy.ChannelWidth = _pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth;      \
+	_pAd->StaActive.SupportedHtPhy.MimoPs = _pAd->MlmeAux.HtCapability.HtCapInfo.MimoPs;      \
+	_pAd->StaActive.SupportedHtPhy.GF = _pAd->MlmeAux.HtCapability.HtCapInfo.GF;      \
+	_pAd->StaActive.SupportedHtPhy.ShortGIfor20 = _pAd->MlmeAux.HtCapability.HtCapInfo.ShortGIfor20;      \
+	_pAd->StaActive.SupportedHtPhy.ShortGIfor40 = _pAd->MlmeAux.HtCapability.HtCapInfo.ShortGIfor40;      \
+	_pAd->StaActive.SupportedHtPhy.TxSTBC = _pAd->MlmeAux.HtCapability.HtCapInfo.TxSTBC;      \
+	_pAd->StaActive.SupportedHtPhy.RxSTBC = _pAd->MlmeAux.HtCapability.HtCapInfo.RxSTBC;      \
+	_pAd->StaActive.SupportedHtPhy.ExtChanOffset = _pAd->MlmeAux.AddHtInfo.AddHtInfo.ExtChanOffset;      \
+	_pAd->StaActive.SupportedHtPhy.RecomWidth = _pAd->MlmeAux.AddHtInfo.AddHtInfo.RecomWidth;      \
+	_pAd->StaActive.SupportedHtPhy.OperaionMode = _pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode;      \
+	_pAd->StaActive.SupportedHtPhy.NonGfPresent = _pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent;      \
+	NdisMoveMemory((_pAd)->MacTab.Content[BSSID_WCID].HTCapability.MCSSet, (_pAd)->StaActive.SupportedPhyInfo.MCSSet, sizeof(UCHAR) * 16);\
+}
+
+#define COPY_AP_HTSETTINGS_FROM_BEACON(_pAd, _pHtCapability)                                 \
+{                                                                                       \
+	_pAd->MacTab.Content[BSSID_WCID].AMsduSize = (UCHAR)(_pHtCapability->HtCapInfo.AMsduSize);	\
+	_pAd->MacTab.Content[BSSID_WCID].MmpsMode= (UCHAR)(_pHtCapability->HtCapInfo.MimoPs);	\
+	_pAd->MacTab.Content[BSSID_WCID].MaxRAmpduFactor = (UCHAR)(_pHtCapability->HtCapParm.MaxRAmpduFactor);	\
+}
+#endif // DOT11_N_SUPPORT //
+
+//
+// BBP & RF are using indirect access. Before write any value into it.
+// We have to make sure there is no outstanding command pending via checking busy bit.
+//
+#define MAX_BUSY_COUNT  100         // Number of retry before failing access BBP & RF indirect register
+//
+
+#ifdef RT2870
+#define RTMP_RF_IO_WRITE32(_A, _V)                 RTUSBWriteRFRegister(_A, _V)
+#define RTMP_BBP_IO_READ8_BY_REG_ID(_A, _I, _pV)   RTUSBReadBBPRegister(_A, _I, _pV)
+#define RTMP_BBP_IO_WRITE8_BY_REG_ID(_A, _I, _V)   RTUSBWriteBBPRegister(_A, _I, _V)
+
+#define BBP_IO_WRITE8_BY_REG_ID(_A, _I, _V)			RTUSBWriteBBPRegister(_A, _I, _V)
+#define BBP_IO_READ8_BY_REG_ID(_A, _I, _pV)   		RTUSBReadBBPRegister(_A, _I, _pV)
+#endif // RT2870 //
+
+#define     MAP_CHANNEL_ID_TO_KHZ(ch, khz)  {               \
+                switch (ch)                                 \
+                {                                           \
+                    case 1:     khz = 2412000;   break;     \
+                    case 2:     khz = 2417000;   break;     \
+                    case 3:     khz = 2422000;   break;     \
+                    case 4:     khz = 2427000;   break;     \
+                    case 5:     khz = 2432000;   break;     \
+                    case 6:     khz = 2437000;   break;     \
+                    case 7:     khz = 2442000;   break;     \
+                    case 8:     khz = 2447000;   break;     \
+                    case 9:     khz = 2452000;   break;     \
+                    case 10:    khz = 2457000;   break;     \
+                    case 11:    khz = 2462000;   break;     \
+                    case 12:    khz = 2467000;   break;     \
+                    case 13:    khz = 2472000;   break;     \
+                    case 14:    khz = 2484000;   break;     \
+                    case 36:  /* UNII */  khz = 5180000;   break;     \
+                    case 40:  /* UNII */  khz = 5200000;   break;     \
+                    case 44:  /* UNII */  khz = 5220000;   break;     \
+                    case 48:  /* UNII */  khz = 5240000;   break;     \
+                    case 52:  /* UNII */  khz = 5260000;   break;     \
+                    case 56:  /* UNII */  khz = 5280000;   break;     \
+                    case 60:  /* UNII */  khz = 5300000;   break;     \
+                    case 64:  /* UNII */  khz = 5320000;   break;     \
+                    case 149: /* UNII */  khz = 5745000;   break;     \
+                    case 153: /* UNII */  khz = 5765000;   break;     \
+                    case 157: /* UNII */  khz = 5785000;   break;     \
+                    case 161: /* UNII */  khz = 5805000;   break;     \
+                    case 165: /* UNII */  khz = 5825000;   break;     \
+                    case 100: /* HiperLAN2 */  khz = 5500000;   break;     \
+                    case 104: /* HiperLAN2 */  khz = 5520000;   break;     \
+                    case 108: /* HiperLAN2 */  khz = 5540000;   break;     \
+                    case 112: /* HiperLAN2 */  khz = 5560000;   break;     \
+                    case 116: /* HiperLAN2 */  khz = 5580000;   break;     \
+                    case 120: /* HiperLAN2 */  khz = 5600000;   break;     \
+                    case 124: /* HiperLAN2 */  khz = 5620000;   break;     \
+                    case 128: /* HiperLAN2 */  khz = 5640000;   break;     \
+                    case 132: /* HiperLAN2 */  khz = 5660000;   break;     \
+                    case 136: /* HiperLAN2 */  khz = 5680000;   break;     \
+                    case 140: /* HiperLAN2 */  khz = 5700000;   break;     \
+                    case 34:  /* Japan MMAC */   khz = 5170000;   break;   \
+                    case 38:  /* Japan MMAC */   khz = 5190000;   break;   \
+                    case 42:  /* Japan MMAC */   khz = 5210000;   break;   \
+                    case 46:  /* Japan MMAC */   khz = 5230000;   break;   \
+                    case 184: /* Japan */   khz = 4920000;   break;   \
+                    case 188: /* Japan */   khz = 4940000;   break;   \
+                    case 192: /* Japan */   khz = 4960000;   break;   \
+                    case 196: /* Japan */   khz = 4980000;   break;   \
+                    case 208: /* Japan, means J08 */   khz = 5040000;   break;   \
+                    case 212: /* Japan, means J12 */   khz = 5060000;   break;   \
+                    case 216: /* Japan, means J16 */   khz = 5080000;   break;   \
+                    default:    khz = 2412000;   break;     \
+                }                                           \
+            }
+
+#define     MAP_KHZ_TO_CHANNEL_ID(khz, ch)  {               \
+                switch (khz)                                \
+                {                                           \
+                    case 2412000:    ch = 1;     break;     \
+                    case 2417000:    ch = 2;     break;     \
+                    case 2422000:    ch = 3;     break;     \
+                    case 2427000:    ch = 4;     break;     \
+                    case 2432000:    ch = 5;     break;     \
+                    case 2437000:    ch = 6;     break;     \
+                    case 2442000:    ch = 7;     break;     \
+                    case 2447000:    ch = 8;     break;     \
+                    case 2452000:    ch = 9;     break;     \
+                    case 2457000:    ch = 10;    break;     \
+                    case 2462000:    ch = 11;    break;     \
+                    case 2467000:    ch = 12;    break;     \
+                    case 2472000:    ch = 13;    break;     \
+                    case 2484000:    ch = 14;    break;     \
+                    case 5180000:    ch = 36;  /* UNII */  break;     \
+                    case 5200000:    ch = 40;  /* UNII */  break;     \
+                    case 5220000:    ch = 44;  /* UNII */  break;     \
+                    case 5240000:    ch = 48;  /* UNII */  break;     \
+                    case 5260000:    ch = 52;  /* UNII */  break;     \
+                    case 5280000:    ch = 56;  /* UNII */  break;     \
+                    case 5300000:    ch = 60;  /* UNII */  break;     \
+                    case 5320000:    ch = 64;  /* UNII */  break;     \
+                    case 5745000:    ch = 149; /* UNII */  break;     \
+                    case 5765000:    ch = 153; /* UNII */  break;     \
+                    case 5785000:    ch = 157; /* UNII */  break;     \
+                    case 5805000:    ch = 161; /* UNII */  break;     \
+                    case 5825000:    ch = 165; /* UNII */  break;     \
+                    case 5500000:    ch = 100; /* HiperLAN2 */  break;     \
+                    case 5520000:    ch = 104; /* HiperLAN2 */  break;     \
+                    case 5540000:    ch = 108; /* HiperLAN2 */  break;     \
+                    case 5560000:    ch = 112; /* HiperLAN2 */  break;     \
+                    case 5580000:    ch = 116; /* HiperLAN2 */  break;     \
+                    case 5600000:    ch = 120; /* HiperLAN2 */  break;     \
+                    case 5620000:    ch = 124; /* HiperLAN2 */  break;     \
+                    case 5640000:    ch = 128; /* HiperLAN2 */  break;     \
+                    case 5660000:    ch = 132; /* HiperLAN2 */  break;     \
+                    case 5680000:    ch = 136; /* HiperLAN2 */  break;     \
+                    case 5700000:    ch = 140; /* HiperLAN2 */  break;     \
+                    case 5170000:    ch = 34;  /* Japan MMAC */   break;   \
+                    case 5190000:    ch = 38;  /* Japan MMAC */   break;   \
+                    case 5210000:    ch = 42;  /* Japan MMAC */   break;   \
+                    case 5230000:    ch = 46;  /* Japan MMAC */   break;   \
+                    case 4920000:    ch = 184; /* Japan */  break;   \
+                    case 4940000:    ch = 188; /* Japan */  break;   \
+                    case 4960000:    ch = 192; /* Japan */  break;   \
+                    case 4980000:    ch = 196; /* Japan */  break;   \
+                    case 5040000:    ch = 208; /* Japan, means J08 */  break;   \
+                    case 5060000:    ch = 212; /* Japan, means J12 */  break;   \
+                    case 5080000:    ch = 216; /* Japan, means J16 */  break;   \
+                    default:         ch = 1;     break;     \
+                }                                           \
+            }
+
+//
+// Common fragment list structure -  Identical to the scatter gather frag list structure
+//
+//#define RTMP_SCATTER_GATHER_ELEMENT         SCATTER_GATHER_ELEMENT
+//#define PRTMP_SCATTER_GATHER_ELEMENT        PSCATTER_GATHER_ELEMENT
+#define NIC_MAX_PHYS_BUF_COUNT              8
+
+typedef struct _RTMP_SCATTER_GATHER_ELEMENT {
+    PVOID		Address;
+    ULONG		Length;
+    PULONG		Reserved;
+} RTMP_SCATTER_GATHER_ELEMENT, *PRTMP_SCATTER_GATHER_ELEMENT;
+
+
+typedef struct _RTMP_SCATTER_GATHER_LIST {
+    ULONG  NumberOfElements;
+    PULONG Reserved;
+    RTMP_SCATTER_GATHER_ELEMENT Elements[NIC_MAX_PHYS_BUF_COUNT];
+} RTMP_SCATTER_GATHER_LIST, *PRTMP_SCATTER_GATHER_LIST;
+
+//
+//  Some utility macros
+//
+#ifndef min
+#define min(_a, _b)     (((_a) < (_b)) ? (_a) : (_b))
+#endif
+
+#ifndef max
+#define max(_a, _b)     (((_a) > (_b)) ? (_a) : (_b))
+#endif
+
+#define GET_LNA_GAIN(_pAd)	((_pAd->LatchRfRegs.Channel <= 14) ? (_pAd->BLNAGain) : ((_pAd->LatchRfRegs.Channel <= 64) ? (_pAd->ALNAGain0) : ((_pAd->LatchRfRegs.Channel <= 128) ? (_pAd->ALNAGain1) : (_pAd->ALNAGain2))))
+
+#define INC_COUNTER64(Val)          (Val.QuadPart++)
+
+#define INFRA_ON(_p)                (OPSTATUS_TEST_FLAG(_p, fOP_STATUS_INFRA_ON))
+#define ADHOC_ON(_p)                (OPSTATUS_TEST_FLAG(_p, fOP_STATUS_ADHOC_ON))
+#define MONITOR_ON(_p)              (((_p)->StaCfg.BssType) == BSS_MONITOR)
+#define IDLE_ON(_p)                 (!INFRA_ON(_p) && !ADHOC_ON(_p))
+
+// Check LEAP & CCKM flags
+#define LEAP_ON(_p)                 (((_p)->StaCfg.LeapAuthMode) == CISCO_AuthModeLEAP)
+#define LEAP_CCKM_ON(_p)            ((((_p)->StaCfg.LeapAuthMode) == CISCO_AuthModeLEAP) && ((_p)->StaCfg.LeapAuthInfo.CCKM == TRUE))
+
+// if orginal Ethernet frame contains no LLC/SNAP, then an extra LLC/SNAP encap is required
+#define EXTRA_LLCSNAP_ENCAP_FROM_PKT_START(_pBufVA, _pExtraLlcSnapEncap)		\
+{																\
+	if (((*(_pBufVA + 12) << 8) + *(_pBufVA + 13)) > 1500)		\
+	{															\
+		_pExtraLlcSnapEncap = SNAP_802_1H;						\
+		if (NdisEqualMemory(IPX, _pBufVA + 12, 2) || 			\
+			NdisEqualMemory(APPLE_TALK, _pBufVA + 12, 2))		\
+		{														\
+			_pExtraLlcSnapEncap = SNAP_BRIDGE_TUNNEL;			\
+		}														\
+	}															\
+	else														\
+	{															\
+		_pExtraLlcSnapEncap = NULL;								\
+	}															\
+}
+
+// New Define for new Tx Path.
+#define EXTRA_LLCSNAP_ENCAP_FROM_PKT_OFFSET(_pBufVA, _pExtraLlcSnapEncap)	\
+{																\
+	if (((*(_pBufVA) << 8) + *(_pBufVA + 1)) > 1500)			\
+	{															\
+		_pExtraLlcSnapEncap = SNAP_802_1H;						\
+		if (NdisEqualMemory(IPX, _pBufVA, 2) || 				\
+			NdisEqualMemory(APPLE_TALK, _pBufVA, 2))			\
+		{														\
+			_pExtraLlcSnapEncap = SNAP_BRIDGE_TUNNEL;			\
+		}														\
+	}															\
+	else														\
+	{															\
+		_pExtraLlcSnapEncap = NULL;								\
+	}															\
+}
+
+
+#define MAKE_802_3_HEADER(_p, _pMac1, _pMac2, _pType)                   \
+{                                                                       \
+    NdisMoveMemory(_p, _pMac1, MAC_ADDR_LEN);                           \
+    NdisMoveMemory((_p + MAC_ADDR_LEN), _pMac2, MAC_ADDR_LEN);          \
+    NdisMoveMemory((_p + MAC_ADDR_LEN * 2), _pType, LENGTH_802_3_TYPE); \
+}
+
+// if pData has no LLC/SNAP (neither RFC1042 nor Bridge tunnel), keep it that way.
+// else if the received frame is LLC/SNAP-encaped IPX or APPLETALK, preserve the LLC/SNAP field
+// else remove the LLC/SNAP field from the result Ethernet frame
+// Patch for WHQL only, which did not turn on Netbios but use IPX within its payload
+// Note:
+//     _pData & _DataSize may be altered (remove 8-byte LLC/SNAP) by this MACRO
+//     _pRemovedLLCSNAP: pointer to removed LLC/SNAP; NULL is not removed
+#define CONVERT_TO_802_3(_p8023hdr, _pDA, _pSA, _pData, _DataSize, _pRemovedLLCSNAP)      \
+{                                                                       \
+    char LLC_Len[2];                                                    \
+                                                                        \
+    _pRemovedLLCSNAP = NULL;                                            \
+    if (NdisEqualMemory(SNAP_802_1H, _pData, 6)  ||                     \
+        NdisEqualMemory(SNAP_BRIDGE_TUNNEL, _pData, 6))                 \
+    {                                                                   \
+        PUCHAR pProto = _pData + 6;                                     \
+                                                                        \
+        if ((NdisEqualMemory(IPX, pProto, 2) || NdisEqualMemory(APPLE_TALK, pProto, 2)) &&  \
+            NdisEqualMemory(SNAP_802_1H, _pData, 6))                    \
+        {                                                               \
+            LLC_Len[0] = (UCHAR)(_DataSize / 256);                      \
+            LLC_Len[1] = (UCHAR)(_DataSize % 256);                      \
+            MAKE_802_3_HEADER(_p8023hdr, _pDA, _pSA, LLC_Len);          \
+        }                                                               \
+        else                                                            \
+        {                                                               \
+            MAKE_802_3_HEADER(_p8023hdr, _pDA, _pSA, pProto);           \
+            _pRemovedLLCSNAP = _pData;                                  \
+            _DataSize -= LENGTH_802_1_H;                                \
+            _pData += LENGTH_802_1_H;                                   \
+        }                                                               \
+    }                                                                   \
+    else                                                                \
+    {                                                                   \
+        LLC_Len[0] = (UCHAR)(_DataSize / 256);                          \
+        LLC_Len[1] = (UCHAR)(_DataSize % 256);                          \
+        MAKE_802_3_HEADER(_p8023hdr, _pDA, _pSA, LLC_Len);              \
+    }                                                                   \
+}
+
+#define SWITCH_AB( _pAA, _pBB)    \
+{                                                                           \
+    PVOID pCC;                                                          \
+    pCC = _pBB;                                                 \
+    _pBB = _pAA;                                                 \
+    _pAA = pCC;                                                 \
+}
+
+// Enqueue this frame to MLME engine
+// We need to enqueue the whole frame because MLME need to pass data type
+// information from 802.11 header
+#ifdef RT2870
+#define REPORT_MGMT_FRAME_TO_MLME(_pAd, Wcid, _pFrame, _FrameSize, _Rssi0, _Rssi1, _Rssi2, _PlcpSignal)        \
+{                                                                                       \
+    UINT32 High32TSF=0, Low32TSF=0;                                                          \
+    MlmeEnqueueForRecv(_pAd, Wcid, High32TSF, Low32TSF, (UCHAR)_Rssi0, (UCHAR)_Rssi1,(UCHAR)_Rssi2,_FrameSize, _pFrame, (UCHAR)_PlcpSignal);   \
+}
+#endif // RT2870 //
+
+#define NDIS_QUERY_BUFFER(_NdisBuf, _ppVA, _pBufLen)                    \
+    NdisQueryBuffer(_NdisBuf, _ppVA, _pBufLen)
+
+#define MAC_ADDR_EQUAL(pAddr1,pAddr2)           RTMPEqualMemory((PVOID)(pAddr1), (PVOID)(pAddr2), MAC_ADDR_LEN)
+#define SSID_EQUAL(ssid1, len1, ssid2, len2)    ((len1==len2) && (RTMPEqualMemory(ssid1, ssid2, len1)))
+
+//
+// Check if it is Japan W53(ch52,56,60,64) channel.
+//
+#define JapanChannelCheck(channel)  ((channel == 52) || (channel == 56) || (channel == 60) || (channel == 64))
+
+#ifdef CONFIG_STA_SUPPORT
+#define STA_PORT_SECURED(_pAd) \
+{ \
+	_pAd->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED; \
+	NdisAcquireSpinLock(&_pAd->MacTabLock); \
+	_pAd->MacTab.Content[BSSID_WCID].PortSecured = _pAd->StaCfg.PortSecured; \
+	NdisReleaseSpinLock(&_pAd->MacTabLock); \
+}
+#endif // CONFIG_STA_SUPPORT //
+
+
+//
+// Register set pair for initialzation register set definition
+//
+typedef struct  _RTMP_REG_PAIR
+{
+	ULONG   Register;
+	ULONG   Value;
+} RTMP_REG_PAIR, *PRTMP_REG_PAIR;
+
+typedef struct  _REG_PAIR
+{
+	UCHAR   Register;
+	UCHAR   Value;
+} REG_PAIR, *PREG_PAIR;
+
+//
+// Register set pair for initialzation register set definition
+//
+typedef struct  _RTMP_RF_REGS
+{
+	UCHAR   Channel;
+	ULONG   R1;
+	ULONG   R2;
+	ULONG   R3;
+	ULONG   R4;
+} RTMP_RF_REGS, *PRTMP_RF_REGS;
+
+typedef struct _FREQUENCY_ITEM {
+	UCHAR	Channel;
+	UCHAR	N;
+	UCHAR	R;
+	UCHAR	K;
+} FREQUENCY_ITEM, *PFREQUENCY_ITEM;
+
+//
+//  Data buffer for DMA operation, the buffer must be contiguous physical memory
+//  Both DMA to / from CPU use the same structure.
+//
+typedef struct  _RTMP_DMABUF
+{
+	ULONG                   AllocSize;
+	PVOID                   AllocVa;            // TxBuf virtual address
+	NDIS_PHYSICAL_ADDRESS   AllocPa;            // TxBuf physical address
+} RTMP_DMABUF, *PRTMP_DMABUF;
+
+
+typedef	union	_HEADER_802_11_SEQ{
+#ifdef RT_BIG_ENDIAN
+    struct {
+   	USHORT			Sequence:12;
+	USHORT			Frag:4;
+    }   field;
+#else
+    struct {
+	USHORT			Frag:4;
+	USHORT			Sequence:12;
+    }   field;
+#endif
+    USHORT           value;
+}	HEADER_802_11_SEQ, *PHEADER_802_11_SEQ;
+
+//
+//  Data buffer for DMA operation, the buffer must be contiguous physical memory
+//  Both DMA to / from CPU use the same structure.
+//
+typedef struct  _RTMP_REORDERBUF
+{
+	BOOLEAN			IsFull;
+	PVOID                   AllocVa;            // TxBuf virtual address
+	UCHAR			Header802_3[14];
+	HEADER_802_11_SEQ			Sequence;	//support compressed bitmap BA, so no consider fragment in BA
+	UCHAR 		DataOffset;
+	USHORT 		Datasize;
+	ULONG                   AllocSize;
+#ifdef RT2870
+	PUCHAR					AllocPa;
+#endif // RT2870 //
+}   RTMP_REORDERBUF, *PRTMP_REORDERBUF;
+
+//
+// Control block (Descriptor) for all ring descriptor DMA operation, buffer must be
+// contiguous physical memory. NDIS_PACKET stored the binding Rx packet descriptor
+// which won't be released, driver has to wait until upper layer return the packet
+// before giveing up this rx ring descriptor to ASIC. NDIS_BUFFER is assocaited pair
+// to describe the packet buffer. For Tx, NDIS_PACKET stored the tx packet descriptor
+// which driver should ACK upper layer when the tx is physically done or failed.
+//
+typedef struct _RTMP_DMACB
+{
+	ULONG                   AllocSize;          // Control block size
+	PVOID                   AllocVa;            // Control block virtual address
+	NDIS_PHYSICAL_ADDRESS   AllocPa;            // Control block physical address
+	PNDIS_PACKET pNdisPacket;
+	PNDIS_PACKET pNextNdisPacket;
+
+	RTMP_DMABUF             DmaBuf;             // Associated DMA buffer structure
+} RTMP_DMACB, *PRTMP_DMACB;
+
+typedef struct _RTMP_TX_BUF
+{
+	PQUEUE_ENTRY    Next;
+	UCHAR           Index;
+	ULONG                   AllocSize;          // Control block size
+	PVOID                   AllocVa;            // Control block virtual address
+	NDIS_PHYSICAL_ADDRESS   AllocPa;            // Control block physical address
+} RTMP_TXBUF, *PRTMP_TXBUF;
+
+typedef struct _RTMP_RX_BUF
+{
+	BOOLEAN           InUse;
+	ULONG           	ByBaRecIndex;
+	RTMP_REORDERBUF	MAP_RXBuf[MAX_RX_REORDERBUF];
+} RTMP_RXBUF, *PRTMP_RXBUF;
+typedef struct _RTMP_TX_RING
+{
+	RTMP_DMACB  Cell[TX_RING_SIZE];
+	UINT32		TxCpuIdx;
+	UINT32		TxDmaIdx;
+	UINT32		TxSwFreeIdx; 	// software next free tx index
+} RTMP_TX_RING, *PRTMP_TX_RING;
+
+typedef struct _RTMP_RX_RING
+{
+	RTMP_DMACB  Cell[RX_RING_SIZE];
+	UINT32		RxCpuIdx;
+	UINT32		RxDmaIdx;
+	INT32		RxSwReadIdx; 	// software next read index
+} RTMP_RX_RING, *PRTMP_RX_RING;
+
+typedef struct _RTMP_MGMT_RING
+{
+	RTMP_DMACB  Cell[MGMT_RING_SIZE];
+	UINT32		TxCpuIdx;
+	UINT32		TxDmaIdx;
+	UINT32		TxSwFreeIdx; // software next free tx index
+} RTMP_MGMT_RING, *PRTMP_MGMT_RING;
+
+//
+//  Statistic counter structure
+//
+typedef struct _COUNTER_802_3
+{
+	// General Stats
+	ULONG       GoodTransmits;
+	ULONG       GoodReceives;
+	ULONG       TxErrors;
+	ULONG       RxErrors;
+	ULONG       RxNoBuffer;
+
+	// Ethernet Stats
+	ULONG       RcvAlignmentErrors;
+	ULONG       OneCollision;
+	ULONG       MoreCollisions;
+
+} COUNTER_802_3, *PCOUNTER_802_3;
+
+typedef struct _COUNTER_802_11 {
+	ULONG           Length;
+	LARGE_INTEGER   LastTransmittedFragmentCount;
+	LARGE_INTEGER   TransmittedFragmentCount;
+	LARGE_INTEGER   MulticastTransmittedFrameCount;
+	LARGE_INTEGER   FailedCount;
+	LARGE_INTEGER   RetryCount;
+	LARGE_INTEGER   MultipleRetryCount;
+	LARGE_INTEGER   RTSSuccessCount;
+	LARGE_INTEGER   RTSFailureCount;
+	LARGE_INTEGER   ACKFailureCount;
+	LARGE_INTEGER   FrameDuplicateCount;
+	LARGE_INTEGER   ReceivedFragmentCount;
+	LARGE_INTEGER   MulticastReceivedFrameCount;
+	LARGE_INTEGER   FCSErrorCount;
+} COUNTER_802_11, *PCOUNTER_802_11;
+
+typedef struct _COUNTER_RALINK {
+	ULONG           TransmittedByteCount;   // both successful and failure, used to calculate TX throughput
+	ULONG           ReceivedByteCount;      // both CRC okay and CRC error, used to calculate RX throughput
+	ULONG           BeenDisassociatedCount;
+	ULONG           BadCQIAutoRecoveryCount;
+	ULONG           PoorCQIRoamingCount;
+	ULONG           MgmtRingFullCount;
+	ULONG           RxCountSinceLastNULL;
+	ULONG           RxCount;
+	ULONG           RxRingErrCount;
+	ULONG           KickTxCount;
+	ULONG           TxRingErrCount;
+	LARGE_INTEGER   RealFcsErrCount;
+	ULONG           PendingNdisPacketCount;
+
+	ULONG           OneSecOsTxCount[NUM_OF_TX_RING];
+	ULONG           OneSecDmaDoneCount[NUM_OF_TX_RING];
+	UINT32          OneSecTxDoneCount;
+	ULONG           OneSecRxCount;
+	UINT32          OneSecTxAggregationCount;
+	UINT32          OneSecRxAggregationCount;
+
+	UINT32   		OneSecFrameDuplicateCount;
+
+#ifdef RT2870
+	ULONG           OneSecTransmittedByteCount;   // both successful and failure, used to calculate TX throughput
+#endif // RT2870 //
+
+	UINT32          OneSecTxNoRetryOkCount;
+	UINT32          OneSecTxRetryOkCount;
+	UINT32          OneSecTxFailCount;
+	UINT32          OneSecFalseCCACnt;      // CCA error count, for debug purpose, might move to global counter
+	UINT32          OneSecRxOkCnt;          // RX without error
+	UINT32          OneSecRxOkDataCnt;      // unicast-to-me DATA frame count
+	UINT32          OneSecRxFcsErrCnt;      // CRC error
+	UINT32          OneSecBeaconSentCnt;
+	UINT32          LastOneSecTotalTxCount; // OneSecTxNoRetryOkCount + OneSecTxRetryOkCount + OneSecTxFailCount
+	UINT32          LastOneSecRxOkDataCnt;  // OneSecRxOkDataCnt
+	ULONG		DuplicateRcv;
+	ULONG		TxAggCount;
+	ULONG		TxNonAggCount;
+	ULONG		TxAgg1MPDUCount;
+	ULONG		TxAgg2MPDUCount;
+	ULONG		TxAgg3MPDUCount;
+	ULONG		TxAgg4MPDUCount;
+	ULONG		TxAgg5MPDUCount;
+	ULONG		TxAgg6MPDUCount;
+	ULONG		TxAgg7MPDUCount;
+	ULONG		TxAgg8MPDUCount;
+	ULONG		TxAgg9MPDUCount;
+	ULONG		TxAgg10MPDUCount;
+	ULONG		TxAgg11MPDUCount;
+	ULONG		TxAgg12MPDUCount;
+	ULONG		TxAgg13MPDUCount;
+	ULONG		TxAgg14MPDUCount;
+	ULONG		TxAgg15MPDUCount;
+	ULONG		TxAgg16MPDUCount;
+
+	LARGE_INTEGER       TransmittedOctetsInAMSDU;
+	LARGE_INTEGER       TransmittedAMSDUCount;
+	LARGE_INTEGER       ReceivedOctesInAMSDUCount;
+	LARGE_INTEGER       ReceivedAMSDUCount;
+	LARGE_INTEGER       TransmittedAMPDUCount;
+	LARGE_INTEGER       TransmittedMPDUsInAMPDUCount;
+	LARGE_INTEGER       TransmittedOctetsInAMPDUCount;
+	LARGE_INTEGER       MPDUInReceivedAMPDUCount;
+} COUNTER_RALINK, *PCOUNTER_RALINK;
+
+typedef struct _PID_COUNTER {
+	ULONG           TxAckRequiredCount;      // CRC error
+	ULONG           TxAggreCount;
+	ULONG           TxSuccessCount; // OneSecTxNoRetryOkCount + OneSecTxRetryOkCount + OneSecTxFailCount
+	ULONG		LastSuccessRate;
+} PID_COUNTER, *PPID_COUNTER;
+
+typedef struct _COUNTER_DRS {
+	// to record the each TX rate's quality. 0 is best, the bigger the worse.
+	USHORT          TxQuality[MAX_STEP_OF_TX_RATE_SWITCH];
+	UCHAR           PER[MAX_STEP_OF_TX_RATE_SWITCH];
+	UCHAR           TxRateUpPenalty;      // extra # of second penalty due to last unstable condition
+	ULONG           CurrTxRateStableTime; // # of second in current TX rate
+	BOOLEAN         fNoisyEnvironment;
+	BOOLEAN         fLastSecAccordingRSSI;
+	UCHAR           LastSecTxRateChangeAction; // 0: no change, 1:rate UP, 2:rate down
+	UCHAR			LastTimeTxRateChangeAction; //Keep last time value of LastSecTxRateChangeAction
+	ULONG			LastTxOkCount;
+} COUNTER_DRS, *PCOUNTER_DRS;
+
+//
+//  Arcfour Structure Added by PaulWu
+//
+typedef struct  _ARCFOUR
+{
+	UINT            X;
+	UINT            Y;
+	UCHAR           STATE[256];
+} ARCFOURCONTEXT, *PARCFOURCONTEXT;
+
+// MIMO Tx parameter, ShortGI, MCS, STBC, etc.  these are fields in TXWI too. just copy to TXWI.
+typedef struct  _RECEIVE_SETTING {
+#ifdef RT_BIG_ENDIAN
+	USHORT		MIMO:1;
+	USHORT		OFDM:1;
+	USHORT		rsv:3;
+	USHORT		STBC:2;	//SPACE
+	USHORT		ShortGI:1;
+	USHORT		Mode:2;	//channel bandwidth 20MHz or 40 MHz
+	USHORT   	NumOfRX:2;                 // MIMO. WE HAVE 3R
+#else
+	USHORT   	NumOfRX:2;                 // MIMO. WE HAVE 3R
+	USHORT		Mode:2;	//channel bandwidth 20MHz or 40 MHz
+	USHORT		ShortGI:1;
+	USHORT		STBC:2;	//SPACE
+	USHORT		rsv:3;
+	USHORT		OFDM:1;
+	USHORT		MIMO:1;
+#endif
+ } RECEIVE_SETTING, *PRECEIVE_SETTING;
+
+// Shared key data structure
+typedef struct  _WEP_KEY {
+	UCHAR   KeyLen;                     // Key length for each key, 0: entry is invalid
+	UCHAR   Key[MAX_LEN_OF_KEY];        // right now we implement 4 keys, 128 bits max
+} WEP_KEY, *PWEP_KEY;
+
+typedef struct _CIPHER_KEY {
+	UCHAR   Key[16];            // right now we implement 4 keys, 128 bits max
+	UCHAR   RxMic[8];			// make alignment
+	UCHAR   TxMic[8];
+	UCHAR   TxTsc[6];           // 48bit TSC value
+	UCHAR   RxTsc[6];           // 48bit TSC value
+	UCHAR   CipherAlg;          // 0-none, 1:WEP64, 2:WEP128, 3:TKIP, 4:AES, 5:CKIP64, 6:CKIP128
+	UCHAR   KeyLen;
+#ifdef CONFIG_STA_SUPPORT
+	UCHAR   BssId[6];
+#endif // CONFIG_STA_SUPPORT //
+            // Key length for each key, 0: entry is invalid
+	UCHAR   Type;               // Indicate Pairwise/Group when reporting MIC error
+} CIPHER_KEY, *PCIPHER_KEY;
+
+typedef struct _BBP_TUNING_STRUCT {
+	BOOLEAN     Enable;
+	UCHAR       FalseCcaCountUpperBound;  // 100 per sec
+	UCHAR       FalseCcaCountLowerBound;  // 10 per sec
+	UCHAR       R17LowerBound;            // specified in E2PROM
+	UCHAR       R17UpperBound;            // 0x68 according to David Tung
+	UCHAR       CurrentR17Value;
+} BBP_TUNING, *PBBP_TUNING;
+
+typedef struct _SOFT_RX_ANT_DIVERSITY_STRUCT {
+	UCHAR     EvaluatePeriod;		 // 0:not evalute status, 1: evaluate status, 2: switching status
+	UCHAR     Pair1PrimaryRxAnt;     // 0:Ant-E1, 1:Ant-E2
+	UCHAR     Pair1SecondaryRxAnt;   // 0:Ant-E1, 1:Ant-E2
+	UCHAR     Pair2PrimaryRxAnt;     // 0:Ant-E3, 1:Ant-E4
+	UCHAR     Pair2SecondaryRxAnt;   // 0:Ant-E3, 1:Ant-E4
+	SHORT     Pair1AvgRssi[2];       // AvgRssi[0]:E1, AvgRssi[1]:E2
+	SHORT     Pair2AvgRssi[2];       // AvgRssi[0]:E3, AvgRssi[1]:E4
+	SHORT     Pair1LastAvgRssi;      //
+	SHORT     Pair2LastAvgRssi;      //
+	ULONG     RcvPktNumWhenEvaluate;
+	BOOLEAN   FirstPktArrivedWhenEvaluate;
+	RALINK_TIMER_STRUCT    RxAntDiversityTimer;
+} SOFT_RX_ANT_DIVERSITY, *PSOFT_RX_ANT_DIVERSITY;
+
+typedef struct _LEAP_AUTH_INFO {
+	BOOLEAN         Enabled;        //Ture: Enable LEAP Authentication
+	BOOLEAN         CCKM;           //Ture: Use Fast Reauthentication with CCKM
+	UCHAR           Reserve[2];
+	UCHAR           UserName[256];  //LEAP, User name
+	ULONG           UserNameLen;
+	UCHAR           Password[256];  //LEAP, User Password
+	ULONG           PasswordLen;
+} LEAP_AUTH_INFO, *PLEAP_AUTH_INFO;
+
+typedef struct {
+	UCHAR        Addr[MAC_ADDR_LEN];
+	UCHAR        ErrorCode[2];  //00 01-Invalid authentication type
+								//00 02-Authentication timeout
+								//00 03-Challenge from AP failed
+								//00 04-Challenge to AP failed
+	BOOLEAN      Reported;
+} ROGUEAP_ENTRY, *PROGUEAP_ENTRY;
+
+typedef struct {
+	UCHAR               RogueApNr;
+	ROGUEAP_ENTRY       RogueApEntry[MAX_LEN_OF_BSS_TABLE];
+} ROGUEAP_TABLE, *PROGUEAP_TABLE;
+
+typedef struct {
+	BOOLEAN     Enable;
+	UCHAR       Delta;
+	BOOLEAN     PlusSign;
+} CCK_TX_POWER_CALIBRATE, *PCCK_TX_POWER_CALIBRATE;
+
+//
+// Receive Tuple Cache Format
+//
+typedef struct  _TUPLE_CACHE    {
+	BOOLEAN         Valid;
+	UCHAR           MacAddress[MAC_ADDR_LEN];
+	USHORT          Sequence;
+	USHORT          Frag;
+} TUPLE_CACHE, *PTUPLE_CACHE;
+
+//
+// Fragment Frame structure
+//
+typedef struct  _FRAGMENT_FRAME {
+	PNDIS_PACKET    pFragPacket;
+	ULONG       RxSize;
+	USHORT      Sequence;
+	USHORT      LastFrag;
+	ULONG       Flags;          // Some extra frame information. bit 0: LLC presented
+} FRAGMENT_FRAME, *PFRAGMENT_FRAME;
+
+
+//
+// Packet information for NdisQueryPacket
+//
+typedef struct  _PACKET_INFO    {
+	UINT            PhysicalBufferCount;    // Physical breaks of buffer descripor chained
+	UINT            BufferCount ;           // Number of Buffer descriptor chained
+	UINT            TotalPacketLength ;     // Self explained
+	PNDIS_BUFFER    pFirstBuffer;           // Pointer to first buffer descriptor
+} PACKET_INFO, *PPACKET_INFO;
+
+//
+// Tkip Key structure which RC4 key & MIC calculation
+//
+typedef struct  _TKIP_KEY_INFO  {
+	UINT        nBytesInM;  // # bytes in M for MICKEY
+	ULONG       IV16;
+	ULONG       IV32;
+	ULONG       K0;         // for MICKEY Low
+	ULONG       K1;         // for MICKEY Hig
+	ULONG       L;          // Current state for MICKEY
+	ULONG       R;          // Current state for MICKEY
+	ULONG       M;          // Message accumulator for MICKEY
+	UCHAR       RC4KEY[16];
+	UCHAR       MIC[8];
+} TKIP_KEY_INFO, *PTKIP_KEY_INFO;
+
+//
+// Private / Misc data, counters for driver internal use
+//
+typedef struct  __PRIVATE_STRUC {
+	UINT       SystemResetCnt;         // System reset counter
+	UINT       TxRingFullCnt;          // Tx ring full occurrance number
+	UINT       PhyRxErrCnt;            // PHY Rx error count, for debug purpose, might move to global counter
+	// Variables for WEP encryption / decryption in rtmp_wep.c
+	UINT       FCSCRC32;
+	ARCFOURCONTEXT  WEPCONTEXT;
+	// Tkip stuff
+	TKIP_KEY_INFO   Tx;
+	TKIP_KEY_INFO   Rx;
+} PRIVATE_STRUC, *PPRIVATE_STRUC;
+
+// structure to tune BBP R66 (BBP TUNING)
+typedef struct _BBP_R66_TUNING {
+	BOOLEAN     bEnable;
+	USHORT      FalseCcaLowerThreshold;  // default 100
+	USHORT      FalseCcaUpperThreshold;  // default 512
+	UCHAR       R66Delta;
+	UCHAR       R66CurrentValue;
+	BOOLEAN		R66LowerUpperSelect; //Before LinkUp, Used LowerBound or UpperBound as R66 value.
+} BBP_R66_TUNING, *PBBP_R66_TUNING;
+
+// structure to store channel TX power
+typedef struct _CHANNEL_TX_POWER {
+	USHORT     RemainingTimeForUse;		//unit: sec
+	UCHAR      Channel;
+#ifdef DOT11N_DRAFT3
+	BOOLEAN       bEffectedChannel;	// For BW 40 operating in 2.4GHz , the "effected channel" is the channel that is covered in 40Mhz.
+#endif // DOT11N_DRAFT3 //
+	CHAR       Power;
+	CHAR       Power2;
+	UCHAR      MaxTxPwr;
+	UCHAR      DfsReq;
+} CHANNEL_TX_POWER, *PCHANNEL_TX_POWER;
+
+// structure to store 802.11j channel TX power
+typedef struct _CHANNEL_11J_TX_POWER {
+	UCHAR      Channel;
+	UCHAR      BW;	// BW_10 or BW_20
+	CHAR       Power;
+	CHAR       Power2;
+	USHORT     RemainingTimeForUse;		//unit: sec
+} CHANNEL_11J_TX_POWER, *PCHANNEL_11J_TX_POWER;
+
+typedef enum _ABGBAND_STATE_ {
+	UNKNOWN_BAND,
+	BG_BAND,
+	A_BAND,
+} ABGBAND_STATE;
+
+typedef struct _MLME_STRUCT {
+#ifdef CONFIG_STA_SUPPORT
+	// STA state machines
+	STATE_MACHINE           CntlMachine;
+	STATE_MACHINE           AssocMachine;
+	STATE_MACHINE           AuthMachine;
+	STATE_MACHINE           AuthRspMachine;
+	STATE_MACHINE           SyncMachine;
+	STATE_MACHINE           WpaPskMachine;
+	STATE_MACHINE           LeapMachine;
+	STATE_MACHINE           AironetMachine;
+	STATE_MACHINE_FUNC      AssocFunc[ASSOC_FUNC_SIZE];
+	STATE_MACHINE_FUNC      AuthFunc[AUTH_FUNC_SIZE];
+	STATE_MACHINE_FUNC      AuthRspFunc[AUTH_RSP_FUNC_SIZE];
+	STATE_MACHINE_FUNC      SyncFunc[SYNC_FUNC_SIZE];
+	STATE_MACHINE_FUNC      WpaPskFunc[WPA_PSK_FUNC_SIZE];
+	STATE_MACHINE_FUNC      AironetFunc[AIRONET_FUNC_SIZE];
+#endif // CONFIG_STA_SUPPORT //
+	STATE_MACHINE_FUNC      ActFunc[ACT_FUNC_SIZE];
+	// Action
+	STATE_MACHINE           ActMachine;
+
+
+#ifdef QOS_DLS_SUPPORT
+	STATE_MACHINE			DlsMachine;
+	STATE_MACHINE_FUNC      DlsFunc[DLS_FUNC_SIZE];
+#endif // QOS_DLS_SUPPORT //
+
+
+
+
+	ULONG                   ChannelQuality;  // 0..100, Channel Quality Indication for Roaming
+	ULONG                   Now32;           // latch the value of NdisGetSystemUpTime()
+	ULONG                   LastSendNULLpsmTime;
+
+	BOOLEAN                 bRunning;
+	NDIS_SPIN_LOCK          TaskLock;
+	MLME_QUEUE              Queue;
+
+	UINT                    ShiftReg;
+
+	RALINK_TIMER_STRUCT     PeriodicTimer;
+	RALINK_TIMER_STRUCT     APSDPeriodicTimer;
+	RALINK_TIMER_STRUCT     LinkDownTimer;
+	RALINK_TIMER_STRUCT     LinkUpTimer;
+	ULONG                   PeriodicRound;
+	ULONG                   OneSecPeriodicRound;
+
+	UCHAR					RealRxPath;
+	BOOLEAN					bLowThroughput;
+	BOOLEAN					bEnableAutoAntennaCheck;
+	RALINK_TIMER_STRUCT		RxAntEvalTimer;
+
+#ifdef RT2870
+	UCHAR CaliBW40RfR24;
+	UCHAR CaliBW20RfR24;
+#endif // RT2870 //
+
+} MLME_STRUCT, *PMLME_STRUCT;
+
+// structure for radar detection and channel switch
+typedef struct _RADAR_DETECT_STRUCT {
+    //BOOLEAN		IEEE80211H;			// 0: disable, 1: enable IEEE802.11h
+	UCHAR		CSCount;			//Channel switch counter
+	UCHAR		CSPeriod;			//Channel switch period (beacon count)
+	UCHAR		RDCount;			//Radar detection counter
+	UCHAR		RDMode;				//Radar Detection mode
+	UCHAR		RDDurRegion;		//Radar detection duration region
+	UCHAR		BBPR16;
+	UCHAR		BBPR17;
+	UCHAR		BBPR18;
+	UCHAR		BBPR21;
+	UCHAR		BBPR22;
+	UCHAR		BBPR64;
+	ULONG		InServiceMonitorCount; // unit: sec
+	UINT8		DfsSessionTime;
+	BOOLEAN		bFastDfs;
+	UINT8		ChMovingTime;
+	UINT8		LongPulseRadarTh;
+} RADAR_DETECT_STRUCT, *PRADAR_DETECT_STRUCT;
+
+#ifdef CARRIER_DETECTION_SUPPORT
+typedef enum CD_STATE_n
+{
+	CD_NORMAL,
+	CD_SILENCE,
+	CD_MAX_STATE
+} CD_STATE;
+
+typedef struct CARRIER_DETECTION_s
+{
+	BOOLEAN					Enable;
+	UINT8					CDSessionTime;
+	UINT8					CDPeriod;
+	CD_STATE				CD_State;
+} CARRIER_DETECTION, *PCARRIER_DETECTION;
+#endif // CARRIER_DETECTION_SUPPORT //
+
+typedef enum _REC_BLOCKACK_STATUS
+{
+    Recipient_NONE=0,
+	Recipient_USED,
+	Recipient_HandleRes,
+    Recipient_Accept
+} REC_BLOCKACK_STATUS, *PREC_BLOCKACK_STATUS;
+
+typedef enum _ORI_BLOCKACK_STATUS
+{
+    Originator_NONE=0,
+	Originator_USED,
+    Originator_WaitRes,
+    Originator_Done
+} ORI_BLOCKACK_STATUS, *PORI_BLOCKACK_STATUS;
+
+#ifdef DOT11_N_SUPPORT
+typedef struct _BA_ORI_ENTRY{
+	UCHAR   Wcid;
+	UCHAR   TID;
+	UCHAR   BAWinSize;
+	UCHAR   Token;
+// Sequence is to fill every outgoing QoS DATA frame's sequence field in 802.11 header.
+	USHORT	Sequence;
+	USHORT	TimeOutValue;
+	ORI_BLOCKACK_STATUS  ORI_BA_Status;
+	RALINK_TIMER_STRUCT ORIBATimer;
+	PVOID	pAdapter;
+} BA_ORI_ENTRY, *PBA_ORI_ENTRY;
+
+typedef struct _BA_REC_ENTRY {
+	UCHAR   Wcid;
+	UCHAR   TID;
+	UCHAR   BAWinSize;	// 7.3.1.14. each buffer is capable of holding a max AMSDU or MSDU.
+	//UCHAR	NumOfRxPkt;
+	//UCHAR    Curindidx; // the head in the RX reordering buffer
+	USHORT		LastIndSeq;
+//	USHORT		LastIndSeqAtTimer;
+	USHORT		TimeOutValue;
+	RALINK_TIMER_STRUCT RECBATimer;
+	ULONG		LastIndSeqAtTimer;
+	ULONG		nDropPacket;
+	ULONG		rcvSeq;
+	REC_BLOCKACK_STATUS  REC_BA_Status;
+//	UCHAR	RxBufIdxUsed;
+	// corresponding virtual address for RX reordering packet storage.
+	//RTMP_REORDERDMABUF MAP_RXBuf[MAX_RX_REORDERBUF];
+	NDIS_SPIN_LOCK          RxReRingLock;                 // Rx Ring spinlock
+//	struct _BA_REC_ENTRY *pNext;
+	PVOID	pAdapter;
+	struct reordering_list	list;
+} BA_REC_ENTRY, *PBA_REC_ENTRY;
+
+
+typedef struct {
+	ULONG		numAsRecipient;		// I am recipient of numAsRecipient clients. These client are in the BARecEntry[]
+	ULONG		numAsOriginator;	// I am originator of 	numAsOriginator clients. These clients are in the BAOriEntry[]
+	BA_ORI_ENTRY       BAOriEntry[MAX_LEN_OF_BA_ORI_TABLE];
+	BA_REC_ENTRY       BARecEntry[MAX_LEN_OF_BA_REC_TABLE];
+} BA_TABLE, *PBA_TABLE;
+
+//For QureyBATableOID use;
+typedef struct  PACKED _OID_BA_REC_ENTRY{
+	UCHAR   MACAddr[MAC_ADDR_LEN];
+	UCHAR   BaBitmap;   // if (BaBitmap&(1<<TID)), this session with{MACAddr, TID}exists, so read BufSize[TID] for BufferSize
+	UCHAR   rsv;
+	UCHAR   BufSize[8];
+	REC_BLOCKACK_STATUS	REC_BA_Status[8];
+} OID_BA_REC_ENTRY, *POID_BA_REC_ENTRY;
+
+//For QureyBATableOID use;
+typedef struct  PACKED _OID_BA_ORI_ENTRY{
+	UCHAR   MACAddr[MAC_ADDR_LEN];
+	UCHAR   BaBitmap;  // if (BaBitmap&(1<<TID)), this session with{MACAddr, TID}exists, so read BufSize[TID] for BufferSize, read ORI_BA_Status[TID] for status
+	UCHAR   rsv;
+	UCHAR   BufSize[8];
+	ORI_BLOCKACK_STATUS  ORI_BA_Status[8];
+} OID_BA_ORI_ENTRY, *POID_BA_ORI_ENTRY;
+
+typedef struct _QUERYBA_TABLE{
+	OID_BA_ORI_ENTRY       BAOriEntry[32];
+	OID_BA_REC_ENTRY       BARecEntry[32];
+	UCHAR   OriNum;// Number of below BAOriEntry
+	UCHAR   RecNum;// Number of below BARecEntry
+} QUERYBA_TABLE, *PQUERYBA_TABLE;
+
+typedef	union	_BACAP_STRUC	{
+#ifdef RT_BIG_ENDIAN
+	struct	{
+		UINT32     :4;
+		UINT32     b2040CoexistScanSup:1;		//As Sta, support do 2040 coexistence scan for AP. As Ap, support monitor trigger event to check if can use BW 40MHz.
+		UINT32     bHtAdhoc:1;			// adhoc can use ht rate.
+		UINT32     MMPSmode:2;	// MIMO power save more, 0:static, 1:dynamic, 2:rsv, 3:mimo enable
+		UINT32     AmsduSize:1;	// 0:3839, 1:7935 bytes. UINT  MSDUSizeToBytes[]	= { 3839, 7935};
+		UINT32     AmsduEnable:1;	//Enable AMSDU transmisstion
+		UINT32		MpduDensity:3;
+		UINT32		Policy:2;	// 0: DELAY_BA 1:IMMED_BA  (//BA Policy subfiled value in ADDBA frame)   2:BA-not use
+		UINT32		AutoBA:1;	// automatically BA
+		UINT32		TxBAWinLimit:8;
+		UINT32		RxBAWinLimit:8;
+	}	field;
+#else
+	struct	{
+		UINT32		RxBAWinLimit:8;
+		UINT32		TxBAWinLimit:8;
+		UINT32		AutoBA:1;	// automatically BA
+		UINT32		Policy:2;	// 0: DELAY_BA 1:IMMED_BA  (//BA Policy subfiled value in ADDBA frame)   2:BA-not use
+		UINT32		MpduDensity:3;
+		UINT32       	AmsduEnable:1;	//Enable AMSDU transmisstion
+		UINT32       	AmsduSize:1;	// 0:3839, 1:7935 bytes. UINT  MSDUSizeToBytes[]	= { 3839, 7935};
+		UINT32       	MMPSmode:2;	// MIMO power save more, 0:static, 1:dynamic, 2:rsv, 3:mimo enable
+		UINT32       	bHtAdhoc:1;			// adhoc can use ht rate.
+		UINT32       	b2040CoexistScanSup:1;		//As Sta, support do 2040 coexistence scan for AP. As Ap, support monitor trigger event to check if can use BW 40MHz.
+		UINT32       	:4;
+	}	field;
+#endif
+	UINT32			word;
+} BACAP_STRUC, *PBACAP_STRUC;
+#endif // DOT11_N_SUPPORT //
+
+//This structure is for all 802.11n card InterOptibilityTest action. Reset all Num every n second.  (Details see MLMEPeriodic)
+typedef	struct	_IOT_STRUC	{
+	UCHAR			Threshold[2];
+	UCHAR			ReorderTimeOutNum[MAX_LEN_OF_BA_REC_TABLE];	// compare with threshold[0]
+	UCHAR			RefreshNum[MAX_LEN_OF_BA_REC_TABLE];	// compare with threshold[1]
+	ULONG			OneSecInWindowCount;
+	ULONG			OneSecFrameDuplicateCount;
+	ULONG			OneSecOutWindowCount;
+	UCHAR			DelOriAct;
+	UCHAR			DelRecAct;
+	UCHAR			RTSShortProt;
+	UCHAR			RTSLongProt;
+	BOOLEAN			bRTSLongProtOn;
+#ifdef CONFIG_STA_SUPPORT
+	BOOLEAN			bLastAtheros;
+    BOOLEAN			bCurrentAtheros;
+    BOOLEAN         bNowAtherosBurstOn;
+	BOOLEAN			bNextDisableRxBA;
+    BOOLEAN			bToggle;
+#endif // CONFIG_STA_SUPPORT //
+} IOT_STRUC, *PIOT_STRUC;
+
+// This is the registry setting for 802.11n transmit setting.  Used in advanced page.
+typedef union _REG_TRANSMIT_SETTING {
+#ifdef RT_BIG_ENDIAN
+ struct {
+         UINT32  rsv:13;
+		 UINT32  EXTCHA:2;
+		 UINT32  HTMODE:1;
+		 UINT32  TRANSNO:2;
+		 UINT32  STBC:1; //SPACE
+		 UINT32  ShortGI:1;
+		 UINT32  BW:1; //channel bandwidth 20MHz or 40 MHz
+		 UINT32  TxBF:1; // 3*3
+		 UINT32  rsv0:10;
+		 //UINT32  MCS:7;                 // MCS
+         //UINT32  PhyMode:4;
+    } field;
+#else
+ struct {
+         //UINT32  PhyMode:4;
+         //UINT32  MCS:7;                 // MCS
+		 UINT32  rsv0:10;
+		 UINT32  TxBF:1;
+         UINT32  BW:1; //channel bandwidth 20MHz or 40 MHz
+         UINT32  ShortGI:1;
+         UINT32  STBC:1; //SPACE
+         UINT32  TRANSNO:2;
+         UINT32  HTMODE:1;
+         UINT32  EXTCHA:2;
+         UINT32  rsv:13;
+    } field;
+#endif
+ UINT32   word;
+} REG_TRANSMIT_SETTING, *PREG_TRANSMIT_SETTING;
+
+typedef union  _DESIRED_TRANSMIT_SETTING {
+#ifdef RT_BIG_ENDIAN
+	struct	{
+			USHORT		rsv:3;
+			USHORT		FixedTxMode:2;			// If MCS isn't AUTO, fix rate in CCK, OFDM or HT mode.
+			USHORT		PhyMode:4;
+			USHORT   	MCS:7;                 // MCS
+	}	field;
+#else
+	struct	{
+			USHORT   	MCS:7;                 	// MCS
+			USHORT		PhyMode:4;
+			USHORT	 	FixedTxMode:2;			// If MCS isn't AUTO, fix rate in CCK, OFDM or HT mode.
+			USHORT		rsv:3;
+	}	field;
+#endif
+	USHORT		word;
+ } DESIRED_TRANSMIT_SETTING, *PDESIRED_TRANSMIT_SETTING;
+
+typedef struct {
+	BOOLEAN		IsRecipient;
+	UCHAR   MACAddr[MAC_ADDR_LEN];
+	UCHAR   TID;
+	UCHAR   nMSDU;
+	USHORT   TimeOut;
+	BOOLEAN bAllTid;  // If True, delete all TID for BA sessions with this MACaddr.
+} OID_ADD_BA_ENTRY, *POID_ADD_BA_ENTRY;
+
+//
+// Multiple SSID structure
+//
+#define WLAN_MAX_NUM_OF_TIM			((MAX_LEN_OF_MAC_TABLE >> 3) + 1) /* /8 + 1 */
+#define WLAN_CT_TIM_BCMC_OFFSET		0 /* unit: 32B */
+
+/* clear bcmc TIM bit */
+#define WLAN_MR_TIM_BCMC_CLEAR(apidx) \
+	pAd->ApCfg.MBSSID[apidx].TimBitmaps[WLAN_CT_TIM_BCMC_OFFSET] &= ~BIT8[0];
+
+/* set bcmc TIM bit */
+#define WLAN_MR_TIM_BCMC_SET(apidx) \
+	pAd->ApCfg.MBSSID[apidx].TimBitmaps[WLAN_CT_TIM_BCMC_OFFSET] |= BIT8[0];
+
+/* clear a station PS TIM bit */
+#define WLAN_MR_TIM_BIT_CLEAR(ad_p, apidx, wcid) \
+	{	UCHAR tim_offset = wcid >> 3; \
+		UCHAR bit_offset = wcid & 0x7; \
+		ad_p->ApCfg.MBSSID[apidx].TimBitmaps[tim_offset] &= (~BIT8[bit_offset]); }
+
+/* set a station PS TIM bit */
+#define WLAN_MR_TIM_BIT_SET(ad_p, apidx, wcid) \
+	{	UCHAR tim_offset = wcid >> 3; \
+		UCHAR bit_offset = wcid & 0x7; \
+		ad_p->ApCfg.MBSSID[apidx].TimBitmaps[tim_offset] |= BIT8[bit_offset]; }
+
+#ifdef RT2870
+#define BEACON_BITMAP_MASK		0xff
+typedef struct _BEACON_SYNC_STRUCT_
+{
+	UCHAR        			BeaconBuf[HW_BEACON_MAX_COUNT][HW_BEACON_OFFSET];
+	UCHAR					BeaconTxWI[HW_BEACON_MAX_COUNT][TXWI_SIZE];
+	ULONG 					TimIELocationInBeacon[HW_BEACON_MAX_COUNT];
+	ULONG					CapabilityInfoLocationInBeacon[HW_BEACON_MAX_COUNT];
+	BOOLEAN					EnableBeacon;		// trigger to enable beacon transmission.
+	UCHAR					BeaconBitMap;		// NOTE: If the MAX_MBSSID_NUM is larger than 8, this parameter need to change.
+	UCHAR					DtimBitOn;			// NOTE: If the MAX_MBSSID_NUM is larger than 8, this parameter need to change.
+}BEACON_SYNC_STRUCT;
+#endif // RT2870 //
+
+typedef struct _MULTISSID_STRUCT {
+	UCHAR								Bssid[MAC_ADDR_LEN];
+    UCHAR                               SsidLen;
+    CHAR                                Ssid[MAX_LEN_OF_SSID];
+    USHORT                              CapabilityInfo;
+
+    PNET_DEV                   			MSSIDDev;
+
+	NDIS_802_11_AUTHENTICATION_MODE     AuthMode;
+	NDIS_802_11_WEP_STATUS              WepStatus;
+	NDIS_802_11_WEP_STATUS				GroupKeyWepStatus;
+	WPA_MIX_PAIR_CIPHER					WpaMixPairCipher;
+
+	ULONG								TxCount;
+	ULONG								RxCount;
+	ULONG								ReceivedByteCount;
+	ULONG								TransmittedByteCount;
+	ULONG								RxErrorCount;
+	ULONG								RxDropCount;
+
+	HTTRANSMIT_SETTING					HTPhyMode, MaxHTPhyMode, MinHTPhyMode;// For transmit phy setting in TXWI.
+	RT_HT_PHY_INFO						DesiredHtPhyInfo;
+	DESIRED_TRANSMIT_SETTING        	DesiredTransmitSetting; // Desired transmit setting. this is for reading registry setting only. not useful.
+	BOOLEAN								bAutoTxRateSwitch;
+
+	//CIPHER_KEY                          SharedKey[SHARE_KEY_NUM]; // ref pAd->SharedKey[BSS][4]
+	UCHAR                               DefaultKeyId;
+
+	UCHAR								TxRate;       // RATE_1, RATE_2, RATE_5_5, RATE_11, ...
+	UCHAR     							DesiredRates[MAX_LEN_OF_SUPPORTED_RATES];// OID_802_11_DESIRED_RATES
+	UCHAR								DesiredRatesIndex;
+	UCHAR     							MaxTxRate;            // RATE_1, RATE_2, RATE_5_5, RATE_11
+
+//	ULONG           					TimBitmap;      // bit0 for broadcast, 1 for AID1, 2 for AID2, ...so on
+//    ULONG           					TimBitmap2;     // b0 for AID32, b1 for AID33, ... and so on
+	UCHAR								TimBitmaps[WLAN_MAX_NUM_OF_TIM];
+
+    // WPA
+    UCHAR                               GMK[32];
+    UCHAR                               PMK[32];
+	UCHAR								GTK[32];
+    BOOLEAN                             IEEE8021X;
+    BOOLEAN                             PreAuth;
+    UCHAR                               GNonce[32];
+    UCHAR                               PortSecured;
+    NDIS_802_11_PRIVACY_FILTER          PrivacyFilter;
+    UCHAR                               BANClass3Data;
+    ULONG                               IsolateInterStaTraffic;
+
+    UCHAR                               RSNIE_Len[2];
+    UCHAR                               RSN_IE[2][MAX_LEN_OF_RSNIE];
+
+
+    UCHAR                   			TimIELocationInBeacon;
+    UCHAR                   			CapabilityInfoLocationInBeacon;
+    // outgoing BEACON frame buffer and corresponding TXWI
+	// PTXWI_STRUC                           BeaconTxWI; //
+    CHAR                                BeaconBuf[MAX_BEACON_SIZE]; // NOTE: BeaconBuf should be 4-byte aligned
+
+    BOOLEAN                             bHideSsid;
+	UINT16								StationKeepAliveTime; // unit: second
+
+    USHORT                              VLAN_VID;
+    USHORT                              VLAN_Priority;
+
+    RT_802_11_ACL						AccessControlList;
+
+	// EDCA Qos
+    BOOLEAN								bWmmCapable;	// 0:disable WMM, 1:enable WMM
+    BOOLEAN								bDLSCapable;	// 0:disable DLS, 1:enable DLS
+
+	UCHAR           					DlsPTK[64];		// Due to windows dirver count on meetinghouse to handle 4-way shake
+
+	// For 802.1x daemon setting per BSS
+	UCHAR								radius_srv_num;
+	RADIUS_SRV_INFO						radius_srv_info[MAX_RADIUS_SRV_NUM];
+
+#ifdef RTL865X_SOC
+	unsigned int						mylinkid;
+#endif
+
+
+	UINT32					RcvdConflictSsidCount;
+	UINT32					RcvdSpoofedAssocRespCount;
+	UINT32					RcvdSpoofedReassocRespCount;
+	UINT32					RcvdSpoofedProbeRespCount;
+	UINT32					RcvdSpoofedBeaconCount;
+	UINT32					RcvdSpoofedDisassocCount;
+	UINT32					RcvdSpoofedAuthCount;
+	UINT32					RcvdSpoofedDeauthCount;
+	UINT32					RcvdSpoofedUnknownMgmtCount;
+	UINT32					RcvdReplayAttackCount;
+
+	CHAR					RssiOfRcvdConflictSsid;
+	CHAR					RssiOfRcvdSpoofedAssocResp;
+	CHAR					RssiOfRcvdSpoofedReassocResp;
+	CHAR					RssiOfRcvdSpoofedProbeResp;
+	CHAR					RssiOfRcvdSpoofedBeacon;
+	CHAR					RssiOfRcvdSpoofedDisassoc;
+	CHAR					RssiOfRcvdSpoofedAuth;
+	CHAR					RssiOfRcvdSpoofedDeauth;
+	CHAR					RssiOfRcvdSpoofedUnknownMgmt;
+	CHAR					RssiOfRcvdReplayAttack;
+
+	BOOLEAN					bBcnSntReq;
+	UCHAR					BcnBufIdx;
+} MULTISSID_STRUCT, *PMULTISSID_STRUCT;
+
+
+
+#ifdef DOT11N_DRAFT3
+typedef enum _BSS2040COEXIST_FLAG{
+	BSS_2040_COEXIST_DISABLE = 0,
+	BSS_2040_COEXIST_TIMER_FIRED  = 1,
+	BSS_2040_COEXIST_INFO_SYNC = 2,
+	BSS_2040_COEXIST_INFO_NOTIFY = 4,
+}BSS2040COEXIST_FLAG;
+#endif // DOT11N_DRAFT3 //
+
+// configuration common to OPMODE_AP as well as OPMODE_STA
+typedef struct _COMMON_CONFIG {
+
+	BOOLEAN		bCountryFlag;
+	UCHAR		CountryCode[3];
+	UCHAR		Geography;
+	UCHAR       CountryRegion;      // Enum of country region, 0:FCC, 1:IC, 2:ETSI, 3:SPAIN, 4:France, 5:MKK, 6:MKK1, 7:Israel
+	UCHAR       CountryRegionForABand;	// Enum of country region for A band
+	UCHAR       PhyMode;            // PHY_11A, PHY_11B, PHY_11BG_MIXED, PHY_ABG_MIXED
+	USHORT      Dsifs;              // in units of usec
+	ULONG       PacketFilter;       // Packet filter for receiving
+
+	CHAR        Ssid[MAX_LEN_OF_SSID]; // NOT NULL-terminated
+	UCHAR       SsidLen;               // the actual ssid length in used
+	UCHAR       LastSsidLen;               // the actual ssid length in used
+	CHAR        LastSsid[MAX_LEN_OF_SSID]; // NOT NULL-terminated
+	UCHAR		LastBssid[MAC_ADDR_LEN];
+
+	UCHAR       Bssid[MAC_ADDR_LEN];
+	USHORT      BeaconPeriod;
+	UCHAR       Channel;
+	UCHAR       CentralChannel;    	// Central Channel when using 40MHz is indicating. not real channel.
+
+#if 0	// move to STA_ADMIN_CONFIG
+	UCHAR       DefaultKeyId;
+
+	NDIS_802_11_PRIVACY_FILTER          PrivacyFilter;  // PrivacyFilter enum for 802.1X
+	NDIS_802_11_AUTHENTICATION_MODE     AuthMode;       // This should match to whatever microsoft defined
+	NDIS_802_11_WEP_STATUS              WepStatus;
+	NDIS_802_11_WEP_STATUS				OrigWepStatus;	// Original wep status set from OID
+
+	// Add to support different cipher suite for WPA2/WPA mode
+	NDIS_802_11_ENCRYPTION_STATUS		GroupCipher;		// Multicast cipher suite
+	NDIS_802_11_ENCRYPTION_STATUS		PairCipher;			// Unicast cipher suite
+	BOOLEAN								bMixCipher;			// Indicate current Pair & Group use different cipher suites
+	USHORT								RsnCapability;
+
+	NDIS_802_11_WEP_STATUS              GroupKeyWepStatus;
+#endif
+
+	UCHAR       SupRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR       SupRateLen;
+	UCHAR       ExtRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR       ExtRateLen;
+	UCHAR       DesireRate[MAX_LEN_OF_SUPPORTED_RATES];      // OID_802_11_DESIRED_RATES
+	UCHAR       MaxDesiredRate;
+	UCHAR       ExpectedACKRate[MAX_LEN_OF_SUPPORTED_RATES];
+
+	ULONG       BasicRateBitmap;        // backup basic ratebitmap
+
+	BOOLEAN		bAPSDCapable;
+	BOOLEAN		bInServicePeriod;
+	BOOLEAN		bAPSDAC_BE;
+	BOOLEAN		bAPSDAC_BK;
+	BOOLEAN		bAPSDAC_VI;
+	BOOLEAN		bAPSDAC_VO;
+	BOOLEAN		bNeedSendTriggerFrame;
+	BOOLEAN		bAPSDForcePowerSave;	// Force power save mode, should only use in APSD-STAUT
+	ULONG		TriggerTimerCount;
+	UCHAR		MaxSPLength;
+	UCHAR		BBPCurrentBW;	// BW_10, 	BW_20, BW_40
+	// move to MULTISSID_STRUCT for MBSS
+	//HTTRANSMIT_SETTING	HTPhyMode, MaxHTPhyMode, MinHTPhyMode;// For transmit phy setting in TXWI.
+	REG_TRANSMIT_SETTING        RegTransmitSetting; //registry transmit setting. this is for reading registry setting only. not useful.
+	//UCHAR       FixedTxMode;              // Fixed Tx Mode (CCK, OFDM), for HT fixed tx mode (GF, MIX) , refer to RegTransmitSetting.field.HTMode
+	UCHAR       TxRate;                 // Same value to fill in TXD. TxRate is 6-bit
+	UCHAR       MaxTxRate;              // RATE_1, RATE_2, RATE_5_5, RATE_11
+	UCHAR       TxRateIndex;            // Tx rate index in RateSwitchTable
+	UCHAR       TxRateTableSize;        // Valid Tx rate table size in RateSwitchTable
+	//BOOLEAN		bAutoTxRateSwitch;
+	UCHAR       MinTxRate;              // RATE_1, RATE_2, RATE_5_5, RATE_11
+	UCHAR       RtsRate;                // RATE_xxx
+	HTTRANSMIT_SETTING	MlmeTransmit;   // MGMT frame PHY rate setting when operatin at Ht rate.
+	UCHAR       MlmeRate;               // RATE_xxx, used to send MLME frames
+	UCHAR       BasicMlmeRate;          // Default Rate for sending MLME frames
+
+	USHORT      RtsThreshold;           // in unit of BYTE
+	USHORT      FragmentThreshold;      // in unit of BYTE
+
+	UCHAR       TxPower;                // in unit of mW
+	ULONG       TxPowerPercentage;      // 0~100 %
+	ULONG       TxPowerDefault;         // keep for TxPowerPercentage
+
+#ifdef DOT11_N_SUPPORT
+	BACAP_STRUC        BACapability; //   NO USE = 0XFF  ;  IMMED_BA =1  ;  DELAY_BA=0
+	BACAP_STRUC        REGBACapability; //   NO USE = 0XFF  ;  IMMED_BA =1  ;  DELAY_BA=0
+#endif // DOT11_N_SUPPORT //
+	IOT_STRUC		IOTestParm;	// 802.11n InterOpbility Test Parameter;
+	ULONG       TxPreamble;             // Rt802_11PreambleLong, Rt802_11PreambleShort, Rt802_11PreambleAuto
+	BOOLEAN     bUseZeroToDisableFragment;     // Microsoft use 0 as disable
+	ULONG       UseBGProtection;        // 0: auto, 1: always use, 2: always not use
+	BOOLEAN     bUseShortSlotTime;      // 0: disable, 1 - use short slot (9us)
+	BOOLEAN     bEnableTxBurst;         // 1: enble TX PACKET BURST, 0: disable TX PACKET BURST
+	BOOLEAN     bAggregationCapable;      // 1: enable TX aggregation when the peer supports it
+	BOOLEAN     bPiggyBackCapable;		// 1: enable TX piggy-back according MAC's version
+	BOOLEAN     bIEEE80211H;			// 1: enable IEEE802.11h spec.
+	ULONG		DisableOLBCDetect;		// 0: enable OLBC detect; 1 disable OLBC detect
+
+#ifdef DOT11_N_SUPPORT
+	BOOLEAN				bRdg;
+#endif // DOT11_N_SUPPORT //
+	BOOLEAN             bWmmCapable;        // 0:disable WMM, 1:enable WMM
+	QOS_CAPABILITY_PARM APQosCapability;    // QOS capability of the current associated AP
+	EDCA_PARM           APEdcaParm;         // EDCA parameters of the current associated AP
+	QBSS_LOAD_PARM      APQbssLoad;         // QBSS load of the current associated AP
+	UCHAR               AckPolicy[4];       // ACK policy of the specified AC. see ACK_xxx
+#ifdef CONFIG_STA_SUPPORT
+	BOOLEAN				bDLSCapable;		// 0:disable DLS, 1:enable DLS
+#endif // CONFIG_STA_SUPPORT //
+	// a bitmap of BOOLEAN flags. each bit represent an operation status of a particular
+	// BOOLEAN control, either ON or OFF. These flags should always be accessed via
+	// OPSTATUS_TEST_FLAG(), OPSTATUS_SET_FLAG(), OP_STATUS_CLEAR_FLAG() macros.
+	// see fOP_STATUS_xxx in RTMP_DEF.C for detail bit definition
+	ULONG               OpStatusFlags;
+
+	BOOLEAN				NdisRadioStateOff; //For HCT 12.0, set this flag to TRUE instead of called MlmeRadioOff.
+	ABGBAND_STATE		BandState;		// For setting BBP used on B/G or A mode.
+
+	// IEEE802.11H--DFS.
+	RADAR_DETECT_STRUCT	RadarDetect;
+
+#ifdef CARRIER_DETECTION_SUPPORT
+	CARRIER_DETECTION		CarrierDetect;
+#endif // CARRIER_DETECTION_SUPPORT //
+
+#ifdef DOT11_N_SUPPORT
+	// HT
+	UCHAR			BASize;		// USer desired BAWindowSize. Should not exceed our max capability
+	//RT_HT_CAPABILITY	SupportedHtPhy;
+	RT_HT_CAPABILITY	DesiredHtPhy;
+	HT_CAPABILITY_IE		HtCapability;
+	ADD_HT_INFO_IE		AddHTInfo;	// Useful as AP.
+	//This IE is used with channel switch announcement element when changing to a new 40MHz.
+	//This IE is included in channel switch ammouncement frames 7.4.1.5, beacons, probe Rsp.
+	NEW_EXT_CHAN_IE	NewExtChanOffset;	//7.3.2.20A, 1 if extension channel is above the control channel, 3 if below, 0 if not present
+
+#ifdef DOT11N_DRAFT3
+	UCHAR					Bss2040CoexistFlag;		// bit 0: bBssCoexistTimerRunning, bit 1: NeedSyncAddHtInfo.
+	RALINK_TIMER_STRUCT	Bss2040CoexistTimer;
+
+	//This IE is used for 20/40 BSS Coexistence.
+	BSS_2040_COEXIST_IE		BSS2040CoexistInfo;
+	// ====== 11n D3.0 =======================>
+	USHORT					Dot11OBssScanPassiveDwell;				// Unit : TU. 5~1000
+	USHORT					Dot11OBssScanActiveDwell;				// Unit : TU. 10~1000
+	USHORT					Dot11BssWidthTriggerScanInt;			// Unit : Second
+	USHORT					Dot11OBssScanPassiveTotalPerChannel;	// Unit : TU. 200~10000
+	USHORT					Dot11OBssScanActiveTotalPerChannel;	// Unit : TU. 20~10000
+	USHORT					Dot11BssWidthChanTranDelayFactor;
+	USHORT					Dot11OBssScanActivityThre;				// Unit : percentage
+
+	ULONG					Dot11BssWidthChanTranDelay;			// multiple of (Dot11BssWidthTriggerScanInt * Dot11BssWidthChanTranDelayFactor)
+	ULONG					CountDownCtr;	// CountDown Counter from (Dot11BssWidthTriggerScanInt * Dot11BssWidthChanTranDelayFactor)
+
+	NDIS_SPIN_LOCK          TriggerEventTabLock;
+	BSS_2040_COEXIST_IE		LastBSSCoexist2040;
+	BSS_2040_COEXIST_IE		BSSCoexist2040;
+	TRIGGER_EVENT_TAB		TriggerEventTab;
+	UCHAR					ChannelListIdx;
+	// <====== 11n D3.0 =======================
+	BOOLEAN					bOverlapScanning;
+#endif // DOT11N_DRAFT3 //
+
+    BOOLEAN                 bHTProtect;
+    BOOLEAN                 bMIMOPSEnable;
+    BOOLEAN					bBADecline;
+	BOOLEAN					bDisableReordering;
+	BOOLEAN					bForty_Mhz_Intolerant;
+	BOOLEAN					bExtChannelSwitchAnnouncement;
+	BOOLEAN					bRcvBSSWidthTriggerEvents;
+	ULONG					LastRcvBSSWidthTriggerEventsTime;
+
+	UCHAR					TxBASize;
+#endif // DOT11_N_SUPPORT //
+
+	// Enable wireless event
+	BOOLEAN				bWirelessEvent;
+	BOOLEAN				bWiFiTest;				// Enable this parameter for WiFi test
+
+	// Tx & Rx Stream number selection
+	UCHAR				TxStream;
+	UCHAR				RxStream;
+
+	// transmit phy mode, trasmit rate for Multicast.
+#ifdef MCAST_RATE_SPECIFIC
+	UCHAR				McastTransmitMcs;
+	UCHAR				McastTransmitPhyMode;
+#endif // MCAST_RATE_SPECIFIC //
+
+	BOOLEAN     		bHardwareRadio;     // Hardware controlled Radio enabled
+
+#ifdef RT2870
+	BOOLEAN     		bMultipleIRP;       // Multiple Bulk IN flag
+	UCHAR       		NumOfBulkInIRP;     // if bMultipleIRP == TRUE, NumOfBulkInIRP will be 4 otherwise be 1
+ 	RT_HT_CAPABILITY	SupportedHtPhy;
+	ULONG				MaxPktOneTxBulk;
+	UCHAR				TxBulkFactor;
+	UCHAR				RxBulkFactor;
+
+	BEACON_SYNC_STRUCT	*pBeaconSync;
+	RALINK_TIMER_STRUCT	BeaconUpdateTimer;
+	UINT32				BeaconAdjust;
+	UINT32				BeaconFactor;
+	UINT32				BeaconRemain;
+#endif // RT2870 //
+
+
+ 	NDIS_SPIN_LOCK			MeasureReqTabLock;
+	PMEASURE_REQ_TAB		pMeasureReqTab;
+
+	NDIS_SPIN_LOCK			TpcReqTabLock;
+	PTPC_REQ_TAB			pTpcReqTab;
+
+	// transmit phy mode, trasmit rate for Multicast.
+#ifdef MCAST_RATE_SPECIFIC
+	HTTRANSMIT_SETTING		MCastPhyMode;
+#endif // MCAST_RATE_SPECIFIC //
+
+#ifdef SINGLE_SKU
+	UINT16					DefineMaxTxPwr;
+#endif // SINGLE_SKU //
+
+
+} COMMON_CONFIG, *PCOMMON_CONFIG;
+
+
+#ifdef CONFIG_STA_SUPPORT
+/* Modified by Wu Xi-Kun 4/21/2006 */
+// STA configuration and status
+typedef struct _STA_ADMIN_CONFIG {
+	// GROUP 1 -
+	//   User configuration loaded from Registry, E2PROM or OID_xxx. These settings describe
+	//   the user intended configuration, but not necessary fully equal to the final
+	//   settings in ACTIVE BSS after negotiation/compromize with the BSS holder (either
+	//   AP or IBSS holder).
+	//   Once initialized, user configuration can only be changed via OID_xxx
+	UCHAR       BssType;              // BSS_INFRA or BSS_ADHOC
+	USHORT      AtimWin;          // used when starting a new IBSS
+
+	// GROUP 2 -
+	//   User configuration loaded from Registry, E2PROM or OID_xxx. These settings describe
+	//   the user intended configuration, and should be always applied to the final
+	//   settings in ACTIVE BSS without compromising with the BSS holder.
+	//   Once initialized, user configuration can only be changed via OID_xxx
+	UCHAR       RssiTrigger;
+	UCHAR       RssiTriggerMode;      // RSSI_TRIGGERED_UPON_BELOW_THRESHOLD or RSSI_TRIGGERED_UPON_EXCCEED_THRESHOLD
+	USHORT      DefaultListenCount;   // default listen count;
+	ULONG       WindowsPowerMode;           // Power mode for AC power
+	ULONG       WindowsBatteryPowerMode;    // Power mode for battery if exists
+	BOOLEAN     bWindowsACCAMEnable;        // Enable CAM power mode when AC on
+	BOOLEAN     bAutoReconnect;         // Set to TRUE when setting OID_802_11_SSID with no matching BSSID
+	ULONG       WindowsPowerProfile;    // Windows power profile, for NDIS5.1 PnP
+
+	// MIB:ieee802dot11.dot11smt(1).dot11StationConfigTable(1)
+	USHORT      Psm;                  // power management mode   (PWR_ACTIVE|PWR_SAVE)
+	USHORT      DisassocReason;
+	UCHAR       DisassocSta[MAC_ADDR_LEN];
+	USHORT      DeauthReason;
+	UCHAR       DeauthSta[MAC_ADDR_LEN];
+	USHORT      AuthFailReason;
+	UCHAR       AuthFailSta[MAC_ADDR_LEN];
+
+	NDIS_802_11_PRIVACY_FILTER          PrivacyFilter;  // PrivacyFilter enum for 802.1X
+	NDIS_802_11_AUTHENTICATION_MODE     AuthMode;       // This should match to whatever microsoft defined
+	NDIS_802_11_WEP_STATUS              WepStatus;
+	NDIS_802_11_WEP_STATUS				OrigWepStatus;	// Original wep status set from OID
+
+	// Add to support different cipher suite for WPA2/WPA mode
+	NDIS_802_11_ENCRYPTION_STATUS		GroupCipher;		// Multicast cipher suite
+	NDIS_802_11_ENCRYPTION_STATUS		PairCipher;			// Unicast cipher suite
+	BOOLEAN								bMixCipher;			// Indicate current Pair & Group use different cipher suites
+	USHORT								RsnCapability;
+
+	NDIS_802_11_WEP_STATUS              GroupKeyWepStatus;
+
+	UCHAR		PMK[32];                // WPA PSK mode PMK
+	UCHAR       PTK[64];                // WPA PSK mode PTK
+	UCHAR		GTK[32];				// GTK from authenticator
+	BSSID_INFO	SavedPMK[PMKID_NO];
+	UINT		SavedPMKNum;			// Saved PMKID number
+
+	UCHAR		DefaultKeyId;
+
+
+	// WPA 802.1x port control, WPA_802_1X_PORT_SECURED, WPA_802_1X_PORT_NOT_SECURED
+	UCHAR       PortSecured;
+
+	// For WPA countermeasures
+	ULONG       LastMicErrorTime;   // record last MIC error time
+	ULONG       MicErrCnt;          // Should be 0, 1, 2, then reset to zero (after disassoiciation).
+	BOOLEAN     bBlockAssoc;        // Block associate attempt for 60 seconds after counter measure occurred.
+	// For WPA-PSK supplicant state
+	WPA_STATE   WpaState;           // Default is SS_NOTUSE and handled by microsoft 802.1x
+	UCHAR       ReplayCounter[8];
+	UCHAR       ANonce[32];         // ANonce for WPA-PSK from aurhenticator
+	UCHAR       SNonce[32];         // SNonce for WPA-PSK
+
+	UCHAR       LastSNR0;             // last received BEACON's SNR
+	UCHAR       LastSNR1;            // last received BEACON's SNR for 2nd  antenna
+	RSSI_SAMPLE RssiSample;
+	ULONG       NumOfAvgRssiSample;
+
+	ULONG       LastBeaconRxTime;     // OS's timestamp of the last BEACON RX time
+	ULONG       Last11bBeaconRxTime;  // OS's timestamp of the last 11B BEACON RX time
+	ULONG		Last11gBeaconRxTime;	// OS's timestamp of the last 11G BEACON RX time
+	ULONG		Last20NBeaconRxTime;	// OS's timestamp of the last 20MHz N BEACON RX time
+
+	ULONG       LastScanTime;       // Record last scan time for issue BSSID_SCAN_LIST
+	ULONG       ScanCnt;            // Scan counts since most recent SSID, BSSID, SCAN OID request
+	BOOLEAN     bSwRadio;           // Software controlled Radio On/Off, TRUE: On
+	BOOLEAN     bHwRadio;           // Hardware controlled Radio On/Off, TRUE: On
+	BOOLEAN     bRadio;             // Radio state, And of Sw & Hw radio state
+	BOOLEAN     bHardwareRadio;     // Hardware controlled Radio enabled
+	BOOLEAN     bShowHiddenSSID;    // Show all known SSID in SSID list get operation
+
+
+	// New for WPA, windows want us to to keep association information and
+	// Fixed IEs from last association response
+	NDIS_802_11_ASSOCIATION_INFORMATION     AssocInfo;
+	USHORT       ReqVarIELen;                // Length of next VIE include EID & Length
+	UCHAR       ReqVarIEs[MAX_VIE_LEN];		// The content saved here should be little-endian format.
+	USHORT       ResVarIELen;                // Length of next VIE include EID & Length
+	UCHAR       ResVarIEs[MAX_VIE_LEN];
+
+	UCHAR       RSNIE_Len;
+	UCHAR       RSN_IE[MAX_LEN_OF_RSNIE];	// The content saved here should be little-endian format.
+
+	// New variables used for CCX 1.0
+	BOOLEAN             bCkipOn;
+	BOOLEAN             bCkipCmicOn;
+	UCHAR               CkipFlag;
+	UCHAR               GIV[3];  //for CCX iv
+	UCHAR               RxSEQ[4];
+	UCHAR               TxSEQ[4];
+	UCHAR               CKIPMIC[4];
+	UCHAR               LeapAuthMode;
+	LEAP_AUTH_INFO      LeapAuthInfo;
+	UCHAR               HashPwd[16];
+	UCHAR               NetworkChallenge[8];
+	UCHAR               NetworkChallengeResponse[24];
+	UCHAR               PeerChallenge[8];
+
+	UCHAR               PeerChallengeResponse[24];
+	UCHAR               SessionKey[16]; //Network session keys (NSK)
+	RALINK_TIMER_STRUCT LeapAuthTimer;
+	ROGUEAP_TABLE       RogueApTab;   //Cisco CCX1 Rogue AP Detection
+
+	// New control flags for CCX
+	CCX_CONTROL         CCXControl;                 // Master administration state
+	BOOLEAN             CCXEnable;                  // Actual CCX state
+	UCHAR               CCXScanChannel;             // Selected channel for CCX beacon request
+	USHORT              CCXScanTime;                // Time out to wait for beacon and probe response
+	UCHAR               CCXReqType;                 // Current processing CCX request type
+	BSS_TABLE           CCXBssTab;                  // BSS Table
+	UCHAR               FrameReportBuf[2048];       // Buffer for creating frame report
+	USHORT              FrameReportLen;             // Current Frame report length
+	ULONG               CLBusyBytes;                // Save the total bytes received durning channel load scan time
+	USHORT              RPIDensity[8];              // Array for RPI density collection
+	// Start address of each BSS table within FrameReportBuf
+	// It's important to update the RxPower of the corresponding Bss
+	USHORT              BssReportOffset[MAX_LEN_OF_BSS_TABLE];
+	USHORT              BeaconToken;                // Token for beacon report
+	ULONG               LastBssIndex;               // Most current reported Bss index
+	RM_REQUEST_ACTION   MeasurementRequest[16];     // Saved measurement request
+	UCHAR               RMReqCnt;                   // Number of measurement request saved.
+	UCHAR               CurrentRMReqIdx;            // Number of measurement request saved.
+	BOOLEAN             ParallelReq;                // Parallel measurement, only one request performed,
+													// It must be the same channel with maximum duration
+	USHORT              ParallelDuration;           // Maximum duration for parallel measurement
+	UCHAR               ParallelChannel;            // Only one channel with parallel measurement
+	USHORT              IAPPToken;                  // IAPP dialog token
+	UCHAR               CCXQosECWMin;               // Cisco QOS ECWMin for AC 0
+	UCHAR               CCXQosECWMax;               // Cisco QOS ECWMax for AC 0
+	// Hack for channel load and noise histogram parameters
+	UCHAR               NHFactor;                   // Parameter for Noise histogram
+	UCHAR               CLFactor;                   // Parameter for channel load
+
+	UCHAR               KRK[16];        //Key Refresh Key.
+	UCHAR               BTK[32];        //Base Transient Key
+	BOOLEAN             CCKMLinkUpFlag;
+	ULONG               CCKMRN;    //(Re)Association request number.
+	LARGE_INTEGER       CCKMBeaconAtJoinTimeStamp;  //TSF timer for Re-assocaite to the new AP
+	UCHAR               AironetCellPowerLimit;      //in dBm
+	UCHAR               AironetIPAddress[4];        //eg. 192.168.1.1
+	BOOLEAN             CCXAdjacentAPReportFlag;    //flag for determining report Assoc Lost time
+	CHAR                CCXAdjacentAPSsid[MAX_LEN_OF_SSID]; //Adjacent AP's SSID report
+	UCHAR               CCXAdjacentAPSsidLen;               // the actual ssid length in used
+	UCHAR               CCXAdjacentAPBssid[MAC_ADDR_LEN];         //Adjacent AP's BSSID report
+	USHORT              CCXAdjacentAPChannel;
+	ULONG               CCXAdjacentAPLinkDownTime;  //for Spec S32.
+
+	RALINK_TIMER_STRUCT	StaQuickResponeForRateUpTimer;
+	BOOLEAN				StaQuickResponeForRateUpTimerRunning;
+
+	UCHAR           	DtimCount;      // 0.. DtimPeriod-1
+	UCHAR           	DtimPeriod;     // default = 3
+
+#ifdef QOS_DLS_SUPPORT
+	RT_802_11_DLS		DLSEntry[MAX_NUM_OF_DLS_ENTRY];
+	UCHAR				DlsReplayCounter[8];
+#endif // QOS_DLS_SUPPORT //
+	////////////////////////////////////////////////////////////////////////////////////////
+	// This is only for WHQL test.
+	BOOLEAN				WhqlTest;
+	////////////////////////////////////////////////////////////////////////////////////////
+
+    RALINK_TIMER_STRUCT WpaDisassocAndBlockAssocTimer;
+    // Fast Roaming
+	BOOLEAN		        bFastRoaming;       // 0:disable fast roaming, 1:enable fast roaming
+	CHAR		        dBmToRoam;          // the condition to roam when receiving Rssi less than this value. It's negative value.
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+    BOOLEAN             IEEE8021X;
+    BOOLEAN             IEEE8021x_required_keys;
+    CIPHER_KEY	        DesireSharedKey[4];	// Record user desired WEP keys
+    UCHAR               DesireSharedKeyId;
+
+    // 0: driver ignores wpa_supplicant
+    // 1: wpa_supplicant initiates scanning and AP selection
+    // 2: driver takes care of scanning, AP selection, and IEEE 802.11 association parameters
+    UCHAR               WpaSupplicantUP;
+	UCHAR				WpaSupplicantScanCount;
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+    CHAR                dev_name[16];
+    USHORT              OriDevType;
+
+    BOOLEAN             bTGnWifiTest;
+	BOOLEAN			    bScanReqIsFromWebUI;
+
+	HTTRANSMIT_SETTING				HTPhyMode, MaxHTPhyMode, MinHTPhyMode;// For transmit phy setting in TXWI.
+	DESIRED_TRANSMIT_SETTING       	DesiredTransmitSetting;
+	RT_HT_PHY_INFO					DesiredHtPhyInfo;
+	BOOLEAN							bAutoTxRateSwitch;
+
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+	UCHAR				IEEE80211dClientMode;
+	UCHAR				StaOriCountryCode[3];
+	UCHAR				StaOriGeography;
+#endif // EXT_BUILD_CHANNEL_LIST //
+} STA_ADMIN_CONFIG, *PSTA_ADMIN_CONFIG;
+
+// This data structure keep the current active BSS/IBSS's configuration that this STA
+// had agreed upon joining the network. Which means these parameters are usually decided
+// by the BSS/IBSS creator instead of user configuration. Data in this data structurre
+// is valid only when either ADHOC_ON(pAd) or INFRA_ON(pAd) is TRUE.
+// Normally, after SCAN or failed roaming attempts, we need to recover back to
+// the current active settings.
+typedef struct _STA_ACTIVE_CONFIG {
+	USHORT      Aid;
+	USHORT      AtimWin;                // in kusec; IBSS parameter set element
+	USHORT      CapabilityInfo;
+	USHORT      CfpMaxDuration;
+	USHORT      CfpPeriod;
+
+	// Copy supported rate from desired AP's beacon. We are trying to match
+	// AP's supported and extended rate settings.
+	UCHAR       SupRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR       ExtRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR       SupRateLen;
+	UCHAR       ExtRateLen;
+	// Copy supported ht from desired AP's beacon. We are trying to match
+	RT_HT_PHY_INFO		SupportedPhyInfo;
+	RT_HT_CAPABILITY	SupportedHtPhy;
+} STA_ACTIVE_CONFIG, *PSTA_ACTIVE_CONFIG;
+
+#ifdef RT2870
+// for USB interface, avoid in interrupt when write key
+typedef struct   RT_ADD_PAIRWISE_KEY_ENTRY {
+        NDIS_802_11_MAC_ADDRESS         MacAddr;
+        USHORT                          MacTabMatchWCID;        // ASIC
+        CIPHER_KEY                      CipherKey;
+} RT_ADD_PAIRWISE_KEY_ENTRY,*PRT_ADD_PAIRWISE_KEY_ENTRY;
+#endif // RT2870 //
+#endif // CONFIG_STA_SUPPORT //
+
+// ----------- start of AP --------------------------
+// AUTH-RSP State Machine Aux data structure
+typedef struct _AP_MLME_AUX {
+	UCHAR               Addr[MAC_ADDR_LEN];
+	USHORT              Alg;
+	CHAR                Challenge[CIPHER_TEXT_LEN];
+} AP_MLME_AUX, *PAP_MLME_AUX;
+
+// structure to define WPA Group Key Rekey Interval
+typedef struct PACKED _RT_802_11_WPA_REKEY {
+	ULONG ReKeyMethod;          // mechanism for rekeying: 0:disable, 1: time-based, 2: packet-based
+	ULONG ReKeyInterval;        // time-based: seconds, packet-based: kilo-packets
+} RT_WPA_REKEY,*PRT_WPA_REKEY, RT_802_11_WPA_REKEY, *PRT_802_11_WPA_REKEY;
+
+typedef struct _MAC_TABLE_ENTRY {
+	//Choose 1 from ValidAsWDS and ValidAsCLI  to validize.
+	BOOLEAN		ValidAsCLI;		// Sta mode, set this TRUE after Linkup,too.
+	BOOLEAN		ValidAsWDS;	// This is WDS Entry. only for AP mode.
+	BOOLEAN		ValidAsApCli;   //This is a AP-Client entry, only for AP mode which enable AP-Client functions.
+	BOOLEAN		ValidAsMesh;
+	BOOLEAN		ValidAsDls;	// This is DLS Entry. only for STA mode.
+	BOOLEAN		isCached;
+	BOOLEAN		bIAmBadAtheros;	// Flag if this is Atheros chip that has IOT problem.  We need to turn on RTS/CTS protection.
+
+	UCHAR         	EnqueueEapolStartTimerRunning;  // Enqueue EAPoL-Start for triggering EAP SM
+	//jan for wpa
+	// record which entry revoke MIC Failure , if it leaves the BSS itself, AP won't update aMICFailTime MIB
+	UCHAR           CMTimerRunning;
+	UCHAR           apidx;			// MBSS number
+	UCHAR           RSNIE_Len;
+	UCHAR           RSN_IE[MAX_LEN_OF_RSNIE];
+	UCHAR           ANonce[LEN_KEY_DESC_NONCE];
+	UCHAR           R_Counter[LEN_KEY_DESC_REPLAY];
+	UCHAR           PTK[64];
+	UCHAR           ReTryCounter;
+	RALINK_TIMER_STRUCT                 RetryTimer;
+	RALINK_TIMER_STRUCT					EnqueueStartForPSKTimer;	// A timer which enqueue EAPoL-Start for triggering PSK SM
+	NDIS_802_11_AUTHENTICATION_MODE     AuthMode;   // This should match to whatever microsoft defined
+	NDIS_802_11_WEP_STATUS              WepStatus;
+	AP_WPA_STATE    WpaState;
+	GTK_STATE       GTKState;
+	USHORT          PortSecured;
+	NDIS_802_11_PRIVACY_FILTER  PrivacyFilter;      // PrivacyFilter enum for 802.1X
+	CIPHER_KEY      PairwiseKey;
+	PVOID           pAd;
+    INT				PMKID_CacheIdx;
+    UCHAR			PMKID[LEN_PMKID];
+
+
+	UCHAR           Addr[MAC_ADDR_LEN];
+	UCHAR           PsMode;
+	SST             Sst;
+	AUTH_STATE      AuthState; // for SHARED KEY authentication state machine used only
+	BOOLEAN			IsReassocSta;	// Indicate whether this is a reassociation procedure
+	USHORT          Aid;
+	USHORT          CapabilityInfo;
+	UCHAR           LastRssi;
+	ULONG           NoDataIdleCount;
+	UINT16			StationKeepAliveCount; // unit: second
+	ULONG           PsQIdleCount;
+	QUEUE_HEADER    PsQueue;
+
+	UINT32			StaConnectTime;		// the live time of this station since associated with AP
+
+
+#ifdef DOT11_N_SUPPORT
+	BOOLEAN			bSendBAR;
+	USHORT			NoBADataCountDown;
+
+	UINT32   		CachedBuf[16];		// UINT (4 bytes) for alignment
+	UINT			TxBFCount; // 3*3
+#endif // DOT11_N_SUPPORT //
+	UINT			FIFOCount;
+	UINT			DebugFIFOCount;
+	UINT			DebugTxCount;
+    BOOLEAN			bDlsInit;
+
+
+//====================================================
+//WDS entry needs these
+// rt2860 add this. if ValidAsWDS==TRUE, MatchWDSTabIdx is the index in WdsTab.MacTab
+	UINT			MatchWDSTabIdx;
+	UCHAR           MaxSupportedRate;
+	UCHAR           CurrTxRate;
+	UCHAR           CurrTxRateIndex;
+	// to record the each TX rate's quality. 0 is best, the bigger the worse.
+	USHORT          TxQuality[MAX_STEP_OF_TX_RATE_SWITCH];
+//	USHORT          OneSecTxOkCount;
+	UINT32			OneSecTxNoRetryOkCount;
+	UINT32          OneSecTxRetryOkCount;
+	UINT32          OneSecTxFailCount;
+	UINT32			ContinueTxFailCnt;
+	UINT32          CurrTxRateStableTime; // # of second in current TX rate
+	UCHAR           TxRateUpPenalty;      // extra # of second penalty due to last unstable condition
+//====================================================
+
+
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+	UINT			MatchDlsEntryIdx; // indicate the index in pAd->StaCfg.DLSEntry
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+	BOOLEAN         fNoisyEnvironment;
+	BOOLEAN			fLastSecAccordingRSSI;
+	UCHAR           LastSecTxRateChangeAction; // 0: no change, 1:rate UP, 2:rate down
+	CHAR			LastTimeTxRateChangeAction; //Keep last time value of LastSecTxRateChangeAction
+	ULONG			LastTxOkCount;
+	UCHAR           PER[MAX_STEP_OF_TX_RATE_SWITCH];
+
+	// a bitmap of BOOLEAN flags. each bit represent an operation status of a particular
+	// BOOLEAN control, either ON or OFF. These flags should always be accessed via
+	// CLIENT_STATUS_TEST_FLAG(), CLIENT_STATUS_SET_FLAG(), CLIENT_STATUS_CLEAR_FLAG() macros.
+	// see fOP_STATUS_xxx in RTMP_DEF.C for detail bit definition. fCLIENT_STATUS_AMSDU_INUSED
+	ULONG           ClientStatusFlags;
+
+	// TODO: Shall we move that to DOT11_N_SUPPORT???
+	HTTRANSMIT_SETTING	HTPhyMode, MaxHTPhyMode, MinHTPhyMode;// For transmit phy setting in TXWI.
+
+#ifdef DOT11_N_SUPPORT
+	// HT EWC MIMO-N used parameters
+	USHORT		RXBAbitmap;	// fill to on-chip  RXWI_BA_BITMASK in 8.1.3RX attribute entry format
+	USHORT		TXBAbitmap;	// This bitmap as originator, only keep in software used to mark AMPDU bit in TXWI
+	USHORT		TXAutoBAbitmap;
+	USHORT		BADeclineBitmap;
+	USHORT		BARecWcidArray[NUM_OF_TID];	// The mapping wcid of recipient session. if RXBAbitmap bit is masked
+	USHORT		BAOriWcidArray[NUM_OF_TID]; // The mapping wcid of originator session. if TXBAbitmap bit is masked
+	USHORT		BAOriSequence[NUM_OF_TID]; // The mapping wcid of originator session. if TXBAbitmap bit is masked
+
+	// 802.11n features.
+	UCHAR		MpduDensity;
+	UCHAR		MaxRAmpduFactor;
+	UCHAR		AMsduSize;
+	UCHAR		MmpsMode;	// MIMO power save more.
+
+	HT_CAPABILITY_IE		HTCapability;
+
+#ifdef DOT11N_DRAFT3
+	UCHAR		BSS2040CoexistenceMgmtSupport;
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+
+	BOOLEAN		bAutoTxRateSwitch;
+
+	UCHAR       RateLen;
+	struct _MAC_TABLE_ENTRY *pNext;
+    USHORT      TxSeq[NUM_OF_TID];
+	USHORT		NonQosDataSeq;
+
+	RSSI_SAMPLE	RssiSample;
+
+	UINT32			TXMCSExpected[16];
+	UINT32			TXMCSSuccessful[16];
+	UINT32			TXMCSFailed[16];
+	UINT32			TXMCSAutoFallBack[16][16];
+
+#ifdef CONFIG_STA_SUPPORT
+	ULONG   		LastBeaconRxTime;
+#endif // CONFIG_STA_SUPPORT //
+} MAC_TABLE_ENTRY, *PMAC_TABLE_ENTRY;
+
+typedef struct _MAC_TABLE {
+	USHORT			Size;
+	MAC_TABLE_ENTRY *Hash[HASH_TABLE_SIZE];
+	MAC_TABLE_ENTRY Content[MAX_LEN_OF_MAC_TABLE];
+	QUEUE_HEADER    McastPsQueue;
+	ULONG           PsQIdleCount;
+	BOOLEAN         fAnyStationInPsm;
+	BOOLEAN         fAnyStationBadAtheros;	// Check if any Station is atheros 802.11n Chip.  We need to use RTS/CTS with Atheros 802,.11n chip.
+	BOOLEAN			fAnyTxOPForceDisable;	// Check if it is necessary to disable BE TxOP
+	BOOLEAN			fAllStationAsRalink; 	// Check if all stations are ralink-chipset
+#ifdef DOT11_N_SUPPORT
+	BOOLEAN         fAnyStationIsLegacy;	// Check if I use legacy rate to transmit to my BSS Station/
+	BOOLEAN         fAnyStationNonGF;		// Check if any Station can't support GF.
+	BOOLEAN         fAnyStation20Only;		// Check if any Station can't support GF.
+	BOOLEAN			fAnyStationMIMOPSDynamic; // Check if any Station is MIMO Dynamic
+	BOOLEAN         fAnyBASession;   // Check if there is BA session.  Force turn on RTS/CTS
+#endif // DOT11_N_SUPPORT //
+} MAC_TABLE, *PMAC_TABLE;
+
+#ifdef DOT11_N_SUPPORT
+#define IS_HT_STA(_pMacEntry)	\
+	(_pMacEntry->MaxHTPhyMode.field.MODE >= MODE_HTMIX)
+
+#define IS_HT_RATE(_pMacEntry)	\
+	(_pMacEntry->HTPhyMode.field.MODE >= MODE_HTMIX)
+
+#define PEER_IS_HT_RATE(_pMacEntry)	\
+	(_pMacEntry->HTPhyMode.field.MODE >= MODE_HTMIX)
+#endif // DOT11_N_SUPPORT //
+
+typedef struct _WDS_ENTRY {
+	BOOLEAN         Valid;
+	UCHAR           Addr[MAC_ADDR_LEN];
+	ULONG           NoDataIdleCount;
+	struct _WDS_ENTRY *pNext;
+} WDS_ENTRY, *PWDS_ENTRY;
+
+typedef struct  _WDS_TABLE_ENTRY {
+	USHORT			Size;
+	UCHAR           WdsAddr[MAC_ADDR_LEN];
+	WDS_ENTRY       *Hash[HASH_TABLE_SIZE];
+	WDS_ENTRY       Content[MAX_LEN_OF_MAC_TABLE];
+	UCHAR           MaxSupportedRate;
+	UCHAR           CurrTxRate;
+	USHORT          TxQuality[MAX_LEN_OF_SUPPORTED_RATES];
+	USHORT          OneSecTxOkCount;
+	USHORT          OneSecTxRetryOkCount;
+	USHORT          OneSecTxFailCount;
+	ULONG           CurrTxRateStableTime; // # of second in current TX rate
+	UCHAR           TxRateUpPenalty;      // extra # of second penalty due to last unstable condition
+} WDS_TABLE_ENTRY, *PWDS_TABLE_ENTRY;
+
+typedef struct _RT_802_11_WDS_ENTRY {
+	PNET_DEV			dev;
+	UCHAR				Valid;
+	UCHAR				PhyMode;
+	UCHAR				PeerWdsAddr[MAC_ADDR_LEN];
+	UCHAR				MacTabMatchWCID;	// ASIC
+	NDIS_802_11_WEP_STATUS  WepStatus;
+	UCHAR					KeyIdx;
+	CIPHER_KEY          	WdsKey;
+	HTTRANSMIT_SETTING				HTPhyMode, MaxHTPhyMode, MinHTPhyMode;
+	RT_HT_PHY_INFO					DesiredHtPhyInfo;
+	BOOLEAN							bAutoTxRateSwitch;
+	DESIRED_TRANSMIT_SETTING       	DesiredTransmitSetting; // Desired transmit setting.
+} RT_802_11_WDS_ENTRY, *PRT_802_11_WDS_ENTRY;
+
+typedef struct _WDS_TABLE {
+	UCHAR               Mode;
+	ULONG               Size;
+	RT_802_11_WDS_ENTRY	WdsEntry[MAX_WDS_ENTRY];
+} WDS_TABLE, *PWDS_TABLE;
+
+typedef struct _APCLI_STRUCT {
+	PNET_DEV				dev;
+#ifdef RTL865X_SOC
+	unsigned int            mylinkid;
+#endif
+	BOOLEAN                 Enable;	// Set it as 1 if the apcli interface was configured to "1"  or by iwpriv cmd "ApCliEnable"
+	BOOLEAN                 Valid;	// Set it as 1 if the apcli interface associated success to remote AP.
+	UCHAR					MacTabWCID;	//WCID value, which point to the entry of ASIC Mac table.
+	UCHAR                   SsidLen;
+	CHAR                    Ssid[MAX_LEN_OF_SSID];
+
+	UCHAR                   CfgSsidLen;
+	CHAR                    CfgSsid[MAX_LEN_OF_SSID];
+	UCHAR                   CfgApCliBssid[ETH_LENGTH_OF_ADDRESS];
+	UCHAR                   CurrentAddress[ETH_LENGTH_OF_ADDRESS];
+
+	ULONG                   ApCliRcvBeaconTime;
+
+	ULONG                   CtrlCurrState;
+	ULONG                   SyncCurrState;
+	ULONG                   AuthCurrState;
+	ULONG                   AssocCurrState;
+	ULONG					WpaPskCurrState;
+
+	USHORT                  AuthReqCnt;
+	USHORT                  AssocReqCnt;
+
+	ULONG                   ClientStatusFlags;
+	UCHAR                   MpduDensity;
+
+	NDIS_802_11_AUTHENTICATION_MODE     AuthMode;   // This should match to whatever microsoft defined
+	NDIS_802_11_WEP_STATUS              WepStatus;
+
+	// Add to support different cipher suite for WPA2/WPA mode
+	NDIS_802_11_ENCRYPTION_STATUS		GroupCipher;		// Multicast cipher suite
+	NDIS_802_11_ENCRYPTION_STATUS		PairCipher;			// Unicast cipher suite
+	BOOLEAN								bMixCipher;			// Indicate current Pair & Group use different cipher suites
+	USHORT								RsnCapability;
+
+	UCHAR		PSK[100];				// reserve PSK key material
+	UCHAR       PSKLen;
+	UCHAR       PMK[32];                // WPA PSK mode PMK
+	//UCHAR       PTK[64];                // WPA PSK mode PTK
+	UCHAR		GTK[32];				// GTK from authenticator
+
+	//CIPHER_KEY		PairwiseKey;
+	CIPHER_KEY      SharedKey[SHARE_KEY_NUM];
+	UCHAR           DefaultKeyId;
+
+	// WPA 802.1x port control, WPA_802_1X_PORT_SECURED, WPA_802_1X_PORT_NOT_SECURED
+	//UCHAR       PortSecured;
+
+	// store RSN_IE built by driver
+	UCHAR		RSN_IE[MAX_LEN_OF_RSNIE];  // The content saved here should be convert to little-endian format.
+	UCHAR		RSNIE_Len;
+
+	// For WPA countermeasures
+	ULONG       LastMicErrorTime;   // record last MIC error time
+	//ULONG       MicErrCnt;          // Should be 0, 1, 2, then reset to zero (after disassoiciation).
+	BOOLEAN                 bBlockAssoc; // Block associate attempt for 60 seconds after counter measure occurred.
+
+	// For WPA-PSK supplicant state
+	//WPA_STATE   	WpaState;           // Default is SS_NOTUSE
+	//UCHAR       	ReplayCounter[8];
+	//UCHAR       	ANonce[32];         // ANonce for WPA-PSK from authenticator
+	UCHAR       	SNonce[32];         // SNonce for WPA-PSK
+	UCHAR			GNonce[32];			// GNonce for WPA-PSK from authenticator
+
+	HTTRANSMIT_SETTING				HTPhyMode, MaxHTPhyMode, MinHTPhyMode;
+	RT_HT_PHY_INFO					DesiredHtPhyInfo;
+	BOOLEAN							bAutoTxRateSwitch;
+	DESIRED_TRANSMIT_SETTING       	DesiredTransmitSetting; // Desired transmit setting.
+} APCLI_STRUCT, *PAPCLI_STRUCT;
+
+// ----------- end of AP ----------------------------
+
+#ifdef BLOCK_NET_IF
+typedef struct _BLOCK_QUEUE_ENTRY
+{
+	BOOLEAN SwTxQueueBlockFlag;
+	LIST_HEADER NetIfList;
+} BLOCK_QUEUE_ENTRY, *PBLOCK_QUEUE_ENTRY;
+#endif // BLOCK_NET_IF //
+
+struct wificonf
+{
+	BOOLEAN	bShortGI;
+	BOOLEAN bGreenField;
+};
+
+
+
+typedef struct _INF_PCI_CONFIG
+{
+	PUCHAR                  CSRBaseAddress;     // PCI MMIO Base Address, all access will use
+}INF_PCI_CONFIG;
+
+typedef struct _INF_USB_CONFIG
+{
+	UINT                BulkInEpAddr;		// bulk-in endpoint address
+	UINT                BulkOutEpAddr[6];	// bulk-out endpoint address
+
+}INF_USB_CONFIG;
+
+#ifdef IKANOS_VX_1X0
+	typedef void (*IkanosWlanTxCbFuncP)(void *, void *);
+
+	struct IKANOS_TX_INFO
+	{
+		struct net_device *netdev;
+		IkanosWlanTxCbFuncP *fp;
+	};
+#endif // IKANOS_VX_1X0 //
+
+#ifdef NINTENDO_AP
+typedef struct _NINDO_CTRL_BLOCK {
+
+	RT_NINTENDO_TABLE	DS_TABLE;
+
+#ifdef CHIP25XX
+	spinlock_t			NINTENDO_TABLE_Lock;
+#else
+	NDIS_SPIN_LOCK		NINTENDO_TABLE_Lock;
+#endif // CHIP25XX //
+
+	UCHAR				NINTENDO_UP_BUFFER[512];
+	UCHAR				Local_KeyIdx;
+	CIPHER_KEY			Local_SharedKey;
+	UCHAR				Local_bHideSsid;
+	UCHAR				Local_AuthMode;
+	UCHAR				Local_WepStatus;
+	USHORT				Local_CapabilityInfo;
+} NINDO_CTRL_BLOCK;
+#endif // NINTENDO_AP //
+
+
+#ifdef DBG_DIAGNOSE
+#define DIAGNOSE_TIME	10   // 10 sec
+typedef struct _RtmpDiagStrcut_
+{	// Diagnosis Related element
+	unsigned char		inited;
+	unsigned char 	qIdx;
+	unsigned char 	ArrayStartIdx;
+	unsigned char		ArrayCurIdx;
+	// Tx Related Count
+	USHORT			TxDataCnt[DIAGNOSE_TIME];
+	USHORT			TxFailCnt[DIAGNOSE_TIME];
+//	USHORT			TxDescCnt[DIAGNOSE_TIME][16];		// TxDesc queue length in scale of 0~14, >=15
+	USHORT			TxDescCnt[DIAGNOSE_TIME][24]; // 3*3	// TxDesc queue length in scale of 0~14, >=15
+//	USHORT			TxMcsCnt[DIAGNOSE_TIME][16];			// TxDate MCS Count in range from 0 to 15, step in 1.
+	USHORT			TxMcsCnt[DIAGNOSE_TIME][24]; // 3*3
+	USHORT			TxSWQueCnt[DIAGNOSE_TIME][9];		// TxSwQueue length in scale of 0, 1, 2, 3, 4, 5, 6, 7, >=8
+
+	USHORT			TxAggCnt[DIAGNOSE_TIME];
+	USHORT			TxNonAggCnt[DIAGNOSE_TIME];
+//	USHORT			TxAMPDUCnt[DIAGNOSE_TIME][16];		// 10 sec, TxDMA APMDU Aggregation count in range from 0 to 15, in setp of 1.
+	USHORT			TxAMPDUCnt[DIAGNOSE_TIME][24]; // 3*3 // 10 sec, TxDMA APMDU Aggregation count in range from 0 to 15, in setp of 1.
+	USHORT			TxRalinkCnt[DIAGNOSE_TIME];			// TxRalink Aggregation Count in 1 sec scale.
+	USHORT			TxAMSDUCnt[DIAGNOSE_TIME];			// TxAMSUD Aggregation Count in 1 sec scale.
+
+	// Rx Related Count
+	USHORT			RxDataCnt[DIAGNOSE_TIME];			// Rx Total Data count.
+	USHORT			RxCrcErrCnt[DIAGNOSE_TIME];
+//	USHORT			RxMcsCnt[DIAGNOSE_TIME][16];		// Rx MCS Count in range from 0 to 15, step in 1.
+	USHORT			RxMcsCnt[DIAGNOSE_TIME][24]; // 3*3
+}RtmpDiagStruct;
+#endif // DBG_DIAGNOSE //
+
+
+//
+//  The miniport adapter structure
+//
+typedef struct _RTMP_ADAPTER
+{
+	PVOID					OS_Cookie;	// save specific structure relative to OS
+	PNET_DEV				net_dev;
+	ULONG					VirtualIfCnt;
+
+
+
+	NDIS_SPIN_LOCK          irq_lock;
+	UCHAR                   irq_disabled;
+
+#ifdef RT2870
+/*****************************************************************************************/
+/*      USB related parameters                                                           */
+/*****************************************************************************************/
+	struct usb_config_descriptor		*config;
+	UINT								BulkInEpAddr;		// bulk-in endpoint address
+	UINT								BulkOutEpAddr[6];	// bulk-out endpoint address
+
+	UINT								NumberOfPipes;
+	USHORT								BulkOutMaxPacketSize;
+	USHORT								BulkInMaxPacketSize;
+
+	//======Control Flags
+	LONG                    	PendingIoCount;
+	ULONG						BulkFlags;
+	BOOLEAN                     bUsbTxBulkAggre;	// Flags for bulk out data priority
+
+
+	//======Timer Thread
+	RT2870_TIMER_QUEUE		TimerQ;
+	NDIS_SPIN_LOCK			TimerQLock;
+
+
+	//======Cmd Thread
+	CmdQ					CmdQ;
+	NDIS_SPIN_LOCK			CmdQLock;				// CmdQLock spinlock
+
+	BOOLEAN					TimerFunc_kill;
+	BOOLEAN					mlme_kill;
+
+
+	//======Semaphores (event)
+	struct semaphore			mlme_semaphore;			/* to sleep thread on	*/
+	struct semaphore			RTUSBCmd_semaphore;		/* to sleep thread on	*/
+	struct semaphore			RTUSBTimer_semaphore;
+#ifdef INF_AMAZON_SE
+	struct semaphore			UsbVendorReq_semaphore;
+	PVOID						UsbVendorReqBuf;
+#endif // INF_AMAZON_SE //
+	struct completion			TimerQComplete;
+	struct completion			mlmeComplete;
+	struct completion			CmdQComplete;
+	wait_queue_head_t			*wait;
+
+	//======Lock for 2870 ATE
+#ifdef RALINK_ATE
+	NDIS_SPIN_LOCK			GenericLock;		// ATE Tx/Rx generic spinlock
+#endif // RALINK_ATE //
+
+#endif // RT2870 //
+
+
+/*****************************************************************************************/
+	/*      Both PCI/USB related parameters                                                  */
+/*****************************************************************************************/
+
+
+/*****************************************************************************************/
+/*      Tx related parameters                                                           */
+/*****************************************************************************************/
+	BOOLEAN                 DeQueueRunning[NUM_OF_TX_RING];  // for ensuring RTUSBDeQueuePacket get call once
+	NDIS_SPIN_LOCK          DeQueueLock[NUM_OF_TX_RING];
+
+#ifdef RT2870
+	// Data related context and AC specified, 4 AC supported
+	NDIS_SPIN_LOCK			BulkOutLock[6];			// BulkOut spinlock for 4 ACs
+	NDIS_SPIN_LOCK			MLMEBulkOutLock;	// MLME BulkOut lock
+
+	HT_TX_CONTEXT			TxContext[NUM_OF_TX_RING];
+	NDIS_SPIN_LOCK			TxContextQueueLock[NUM_OF_TX_RING];		// TxContextQueue spinlock
+
+	// 4 sets of Bulk Out index and pending flag
+	UCHAR					NextBulkOutIndex[4];	// only used for 4 EDCA bulkout pipe
+
+	BOOLEAN					BulkOutPending[6];	// used for total 6 bulkout pipe
+	UCHAR					bulkResetPipeid;
+	BOOLEAN					MgmtBulkPending;
+	ULONG					bulkResetReq[6];
+#endif // RT2870 //
+
+	// resource for software backlog queues
+	QUEUE_HEADER            TxSwQueue[NUM_OF_TX_RING];  // 4 AC + 1 HCCA
+	NDIS_SPIN_LOCK          TxSwQueueLock[NUM_OF_TX_RING];	// TxSwQueue spinlock
+
+	RTMP_DMABUF             MgmtDescRing;               	// Shared memory for MGMT descriptors
+	RTMP_MGMT_RING          MgmtRing;
+	NDIS_SPIN_LOCK          MgmtRingLock;               	// Prio Ring spinlock
+
+
+/*****************************************************************************************/
+/*      Rx related parameters                                                           */
+/*****************************************************************************************/
+
+
+#ifdef RT2870
+	RX_CONTEXT				RxContext[RX_RING_SIZE];  // 1 for redundant multiple IRP bulk in.
+	NDIS_SPIN_LOCK			BulkInLock;				// BulkIn spinlock for 4 ACs
+	UCHAR					PendingRx;				// The Maxima pending Rx value should be 	RX_RING_SIZE.
+	UCHAR					NextRxBulkInIndex;		// Indicate the current RxContext Index which hold by Host controller.
+	UCHAR					NextRxBulkInReadIndex;	// Indicate the current RxContext Index which driver can read & process it.
+	ULONG					NextRxBulkInPosition;   // Want to contatenate 2 URB buffer while 1st is bulkin failed URB. This Position is 1st URB TransferLength.
+	ULONG					TransferBufferLength;	// current length of the packet buffer
+	ULONG					ReadPosition;			// current read position in a packet buffer
+#endif // RT2870 //
+
+
+/*****************************************************************************************/
+/*      ASIC related parameters                                                          */
+/*****************************************************************************************/
+	UINT32               	MACVersion;      	// MAC version. Record rt2860C(0x28600100) or rt2860D (0x28600101)..
+
+	// ---------------------------
+	// E2PROM
+	// ---------------------------
+	ULONG                   EepromVersion;          // byte 0: version, byte 1: revision, byte 2~3: unused
+	UCHAR                   EEPROMAddressNum;       // 93c46=6  93c66=8
+	USHORT                  EEPROMDefaultValue[NUM_EEPROM_BBP_PARMS];
+	ULONG                   FirmwareVersion;        // byte 0: Minor version, byte 1: Major version, otherwise unused.
+
+	// ---------------------------
+	// BBP Control
+	// ---------------------------
+	UCHAR                   BbpWriteLatch[140];     // record last BBP register value written via BBP_IO_WRITE/BBP_IO_WRITE_VY_REG_ID
+	UCHAR                   BbpRssiToDbmDelta;
+	BBP_R66_TUNING          BbpTuning;
+
+	// ----------------------------
+	// RFIC control
+	// ----------------------------
+	UCHAR                   RfIcType;       // RFIC_xxx
+	ULONG                   RfFreqOffset;   // Frequency offset for channel switching
+	RTMP_RF_REGS            LatchRfRegs;    // latch th latest RF programming value since RF IC doesn't support READ
+
+	EEPROM_ANTENNA_STRUC    Antenna;                            // Since ANtenna definition is different for a & g. We need to save it for future reference.
+	EEPROM_NIC_CONFIG2_STRUC    NicConfig2;
+
+	// This soft Rx Antenna Diversity mechanism is used only when user set
+	// RX Antenna = DIVERSITY ON
+	SOFT_RX_ANT_DIVERSITY   RxAnt;
+
+	UCHAR                   RFProgSeq;
+	CHANNEL_TX_POWER        TxPower[MAX_NUM_OF_CHANNELS];       // Store Tx power value for all channels.
+	CHANNEL_TX_POWER        ChannelList[MAX_NUM_OF_CHANNELS];   // list all supported channels for site survey
+	CHANNEL_11J_TX_POWER    TxPower11J[MAX_NUM_OF_11JCHANNELS];       // 802.11j channel and bw
+	CHANNEL_11J_TX_POWER    ChannelList11J[MAX_NUM_OF_11JCHANNELS];   // list all supported channels for site survey
+
+	UCHAR                   ChannelListNum;                     // number of channel in ChannelList[]
+	UCHAR					Bbp94;
+	BOOLEAN					BbpForCCK;
+	ULONG		Tx20MPwrCfgABand[5];
+	ULONG		Tx20MPwrCfgGBand[5];
+	ULONG		Tx40MPwrCfgABand[5];
+	ULONG		Tx40MPwrCfgGBand[5];
+
+	BOOLEAN     bAutoTxAgcA;                // Enable driver auto Tx Agc control
+	UCHAR	    TssiRefA;					// Store Tssi reference value as 25 temperature.
+	UCHAR	    TssiPlusBoundaryA[5];		// Tssi boundary for increase Tx power to compensate.
+	UCHAR	    TssiMinusBoundaryA[5];		// Tssi boundary for decrease Tx power to compensate.
+	UCHAR	    TxAgcStepA;					// Store Tx TSSI delta increment / decrement value
+	CHAR		TxAgcCompensateA;			// Store the compensation (TxAgcStep * (idx-1))
+
+	BOOLEAN     bAutoTxAgcG;                // Enable driver auto Tx Agc control
+	UCHAR	    TssiRefG;					// Store Tssi reference value as 25 temperature.
+	UCHAR	    TssiPlusBoundaryG[5];		// Tssi boundary for increase Tx power to compensate.
+	UCHAR	    TssiMinusBoundaryG[5];		// Tssi boundary for decrease Tx power to compensate.
+	UCHAR	    TxAgcStepG;					// Store Tx TSSI delta increment / decrement value
+	CHAR		TxAgcCompensateG;			// Store the compensation (TxAgcStep * (idx-1))
+
+	//+++For RT2870, the parameteres is start from BGRssiOffset1 ~ BGRssiOffset3
+	CHAR		BGRssiOffset0;				// Store B/G RSSI#0 Offset value on EEPROM 0x46h
+	CHAR		BGRssiOffset1;				// Store B/G RSSI#1 Offset value
+	CHAR		BGRssiOffset2;				// Store B/G RSSI#2 Offset value
+	//---
+
+	//+++For RT2870, the parameteres is start from ARssiOffset1 ~ ARssiOffset3
+	CHAR		ARssiOffset0;				// Store A RSSI#0 Offset value on EEPROM 0x4Ah
+	CHAR		ARssiOffset1;				// Store A RSSI#1 Offset value
+	CHAR		ARssiOffset2;				// Store A RSSI#2 Offset value
+	//---
+
+	CHAR		BLNAGain;					// Store B/G external LNA#0 value on EEPROM 0x44h
+	CHAR		ALNAGain0;					// Store A external LNA#0 value for ch36~64
+	CHAR		ALNAGain1;					// Store A external LNA#1 value for ch100~128
+	CHAR		ALNAGain2;					// Store A external LNA#2 value for ch132~165
+
+	// ----------------------------
+	// LED control
+	// ----------------------------
+	MCU_LEDCS_STRUC		LedCntl;
+	USHORT				Led1;	// read from EEPROM 0x3c
+	USHORT				Led2;	// EEPROM 0x3e
+	USHORT				Led3;	// EEPROM 0x40
+	UCHAR				LedIndicatorStregth;
+	UCHAR				RssiSingalstrengthOffet;
+    BOOLEAN				bLedOnScanning;
+	UCHAR				LedStatus;
+
+/*****************************************************************************************/
+/*      802.11 related parameters                                                        */
+/*****************************************************************************************/
+	// outgoing BEACON frame buffer and corresponding TXD
+	TXWI_STRUC              	BeaconTxWI;
+	PUCHAR						BeaconBuf;
+	USHORT						BeaconOffset[HW_BEACON_MAX_COUNT];
+
+	// pre-build PS-POLL and NULL frame upon link up. for efficiency purpose.
+	PSPOLL_FRAME            	PsPollFrame;
+	HEADER_802_11           	NullFrame;
+
+#ifdef RT2870
+	TX_CONTEXT				BeaconContext[BEACON_RING_SIZE];
+	TX_CONTEXT				NullContext;
+	TX_CONTEXT				PsPollContext;
+	TX_CONTEXT				RTSContext;
+#endif // RT2870 //
+
+
+
+//=========AP===========
+
+
+//=======STA===========
+#ifdef CONFIG_STA_SUPPORT
+/* Modified by Wu Xi-Kun 4/21/2006 */
+	// -----------------------------------------------
+	// STA specific configuration & operation status
+	// used only when pAd->OpMode == OPMODE_STA
+	// -----------------------------------------------
+	STA_ADMIN_CONFIG        StaCfg;           // user desired settings
+	STA_ACTIVE_CONFIG       StaActive;         // valid only when ADHOC_ON(pAd) || INFRA_ON(pAd)
+	CHAR                    nickname[IW_ESSID_MAX_SIZE+1]; // nickname, only used in the iwconfig i/f
+	NDIS_MEDIA_STATE        PreMediaState;
+#endif // CONFIG_STA_SUPPORT //
+
+//=======Common===========
+	// OP mode: either AP or STA
+	UCHAR                   OpMode;                     // OPMODE_STA, OPMODE_AP
+
+	NDIS_MEDIA_STATE        IndicateMediaState;			// Base on Indication state, default is NdisMediaStateDisConnected
+
+
+	// configuration: read from Registry & E2PROM
+	BOOLEAN                 bLocalAdminMAC;             // Use user changed MAC
+	UCHAR                   PermanentAddress[MAC_ADDR_LEN];    // Factory default MAC address
+	UCHAR                   CurrentAddress[MAC_ADDR_LEN];      // User changed MAC address
+
+	// ------------------------------------------------------
+	// common configuration to both OPMODE_STA and OPMODE_AP
+	// ------------------------------------------------------
+	COMMON_CONFIG           CommonCfg;
+	MLME_STRUCT             Mlme;
+
+	// AP needs those vaiables for site survey feature.
+	MLME_AUX                MlmeAux;           // temporary settings used during MLME state machine
+	BSS_TABLE               ScanTab;           // store the latest SCAN result
+
+	//About MacTab, the sta driver will use #0 and #1 for multicast and AP.
+	MAC_TABLE                 MacTab;     // ASIC on-chip WCID entry table.  At TX, ASIC always use key according to this on-chip table.
+	NDIS_SPIN_LOCK          MacTabLock;
+
+#ifdef DOT11_N_SUPPORT
+	BA_TABLE			BATable;
+#endif // DOT11_N_SUPPORT //
+	NDIS_SPIN_LOCK          BATabLock;
+	RALINK_TIMER_STRUCT RECBATimer;
+
+	// encryption/decryption KEY tables
+	CIPHER_KEY              SharedKey[MAX_MBSSID_NUM][4]; // STA always use SharedKey[BSS0][0..3]
+
+		// RX re-assembly buffer for fragmentation
+	FRAGMENT_FRAME          FragFrame;                  // Frame storage for fragment frame
+
+	// various Counters
+	COUNTER_802_3           Counters8023;               // 802.3 counters
+	COUNTER_802_11          WlanCounters;               // 802.11 MIB counters
+	COUNTER_RALINK          RalinkCounters;             // Ralink propriety counters
+	COUNTER_DRS             DrsCounters;                // counters for Dynamic TX Rate Switching
+	PRIVATE_STRUC           PrivateInfo;                // Private information & counters
+
+	// flags, see fRTMP_ADAPTER_xxx flags
+	ULONG                   Flags;                      // Represent current device status
+
+	// current TX sequence #
+	USHORT                  Sequence;
+
+	// Control disconnect / connect event generation
+	//+++Didn't used anymore
+	ULONG                   LinkDownTime;
+	//---
+	ULONG                   LastRxRate;
+	ULONG                   LastTxRate;
+	//+++Used only for Station
+	BOOLEAN                 bConfigChanged;         // Config Change flag for the same SSID setting
+	//---
+
+	ULONG                   ExtraInfo;              // Extra information for displaying status
+	ULONG                   SystemErrorBitmap;      // b0: E2PROM version error
+
+	//+++Didn't used anymore
+	ULONG                   MacIcVersion;           // MAC/BBP serial interface issue solved after ver.D
+	//---
+
+	// ---------------------------
+	// System event log
+	// ---------------------------
+	RT_802_11_EVENT_TABLE   EventTab;
+
+
+	BOOLEAN		HTCEnable;
+
+	/*****************************************************************************************/
+	/*      Statistic related parameters                                                     */
+	/*****************************************************************************************/
+#ifdef RT2870
+	ULONG						BulkOutDataOneSecCount;
+	ULONG						BulkInDataOneSecCount;
+	ULONG						BulkLastOneSecCount; // BulkOutDataOneSecCount + BulkInDataOneSecCount
+	ULONG						watchDogRxCnt;
+	ULONG						watchDogRxOverFlowCnt;
+	ULONG						watchDogTxPendingCnt[NUM_OF_TX_RING];
+#endif // RT2870 //
+
+	BOOLEAN						bUpdateBcnCntDone;
+	ULONG						watchDogMacDeadlock;	// prevent MAC/BBP into deadlock condition
+	// ----------------------------
+	// DEBUG paramerts
+	// ----------------------------
+	//ULONG		DebugSetting[4];
+	BOOLEAN		bBanAllBaSetup;
+	BOOLEAN		bPromiscuous;
+
+	// ----------------------------
+	// rt2860c emulation-use Parameters
+	// ----------------------------
+	ULONG		rtsaccu[30];
+	ULONG		ctsaccu[30];
+	ULONG		cfendaccu[30];
+	ULONG		bacontent[16];
+	ULONG		rxint[RX_RING_SIZE+1];
+	UCHAR		rcvba[60];
+	BOOLEAN		bLinkAdapt;
+	BOOLEAN		bForcePrintTX;
+	BOOLEAN		bForcePrintRX;
+	BOOLEAN		bDisablescanning;		//defined in RT2870 USB
+	BOOLEAN		bStaFifoTest;
+	BOOLEAN		bProtectionTest;
+	BOOLEAN		bHCCATest;
+	BOOLEAN		bGenOneHCCA;
+	BOOLEAN		bBroadComHT;
+	//+++Following add from RT2870 USB.
+	ULONG		BulkOutReq;
+	ULONG		BulkOutComplete;
+	ULONG		BulkOutCompleteOther;
+	ULONG		BulkOutCompleteCancel;	// seems not use now?
+	ULONG		BulkInReq;
+	ULONG		BulkInComplete;
+	ULONG		BulkInCompleteFail;
+	//---
+
+    struct wificonf			WIFItestbed;
+
+#ifdef RALINK_ATE
+	ATE_INFO				ate;
+#ifdef RT2870
+	BOOLEAN					ContinBulkOut;		//ATE bulk out control
+	BOOLEAN					ContinBulkIn;		//ATE bulk in control
+	atomic_t				BulkOutRemained;
+	atomic_t				BulkInRemained;
+#endif // RT2870 //
+#endif // RALINK_ATE //
+
+#ifdef DOT11_N_SUPPORT
+	struct reordering_mpdu_pool mpdu_blk_pool;
+#endif // DOT11_N_SUPPORT //
+
+	ULONG					OneSecondnonBEpackets;		// record non BE packets per second
+
+#if WIRELESS_EXT >= 12
+    struct iw_statistics    iw_stats;
+#endif
+
+	struct net_device_stats	stats;
+
+#ifdef BLOCK_NET_IF
+	BLOCK_QUEUE_ENTRY		blockQueueTab[NUM_OF_TX_RING];
+#endif // BLOCK_NET_IF //
+
+
+
+#ifdef MULTIPLE_CARD_SUPPORT
+	INT32					MC_RowID;
+	UCHAR					MC_FileName[256];
+#endif // MULTIPLE_CARD_SUPPORT //
+
+	ULONG					TbttTickCount;
+#ifdef PCI_MSI_SUPPORT
+	BOOLEAN					HaveMsi;
+#endif // PCI_MSI_SUPPORT //
+
+
+	UCHAR					is_on;
+
+#define TIME_BASE			(1000000/OS_HZ)
+#define TIME_ONE_SECOND		(1000000/TIME_BASE)
+	UCHAR					flg_be_adjust;
+	ULONG					be_adjust_last_time;
+
+
+#ifdef IKANOS_VX_1X0
+	struct IKANOS_TX_INFO	IkanosTxInfo;
+	struct IKANOS_TX_INFO	IkanosRxInfo[MAX_MBSSID_NUM + MAX_WDS_ENTRY + MAX_APCLI_NUM + MAX_MESH_NUM];
+#endif // IKANOS_VX_1X0 //
+
+
+#ifdef DBG_DIAGNOSE
+	RtmpDiagStruct	DiagStruct;
+#endif // DBG_DIAGNOSE //
+
+
+	UINT8					PM_FlgSuspend;
+} RTMP_ADAPTER, *PRTMP_ADAPTER;
+
+//
+// Cisco IAPP format
+//
+typedef struct  _CISCO_IAPP_CONTENT_
+{
+	USHORT     Length;        //IAPP Length
+	UCHAR      MessageType;      //IAPP type
+	UCHAR      FunctionCode;     //IAPP function type
+	UCHAR      DestinaionMAC[MAC_ADDR_LEN];
+	UCHAR      SourceMAC[MAC_ADDR_LEN];
+	USHORT     Tag;           //Tag(element IE) - Adjacent AP report
+	USHORT     TagLength;     //Length of element not including 4 byte header
+	UCHAR      OUI[4];           //0x00, 0x40, 0x96, 0x00
+	UCHAR      PreviousAP[MAC_ADDR_LEN];       //MAC Address of access point
+	USHORT     Channel;
+	USHORT     SsidLen;
+	UCHAR      Ssid[MAX_LEN_OF_SSID];
+	USHORT     Seconds;          //Seconds that the client has been disassociated.
+} CISCO_IAPP_CONTENT, *PCISCO_IAPP_CONTENT;
+
+#define DELAYINTMASK		0x0003fffb
+#define INTMASK				0x0003fffb
+#define IndMask				0x0003fffc
+#define RxINT				0x00000005	// Delayed Rx or indivi rx
+#define TxDataInt			0x000000fa	// Delayed Tx or indivi tx
+#define TxMgmtInt			0x00000102	// Delayed Tx or indivi tx
+#define TxCoherent			0x00020000	// tx coherent
+#define RxCoherent			0x00010000	// rx coherent
+#define McuCommand			0x00000200	// mcu
+#define PreTBTTInt			0x00001000	// Pre-TBTT interrupt
+#define TBTTInt				0x00000800		// TBTT interrupt
+#define GPTimeOutInt			0x00008000		// GPtimeout interrupt
+#define AutoWakeupInt		0x00004000		// AutoWakeupInt interrupt
+#define FifoStaFullInt			0x00002000	//  fifo statistics full interrupt
+
+
+typedef struct _RX_BLK_
+{
+//	RXD_STRUC		RxD; // sample
+	RT28XX_RXD_STRUC	RxD;
+	PRXWI_STRUC			pRxWI;
+	PHEADER_802_11		pHeader;
+	PNDIS_PACKET		pRxPacket;
+	UCHAR				*pData;
+	USHORT				DataSize;
+	USHORT				Flags;
+	UCHAR				UserPriority;	// for calculate TKIP MIC using
+} RX_BLK;
+
+
+#define RX_BLK_SET_FLAG(_pRxBlk, _flag)		(_pRxBlk->Flags |= _flag)
+#define RX_BLK_TEST_FLAG(_pRxBlk, _flag)	(_pRxBlk->Flags & _flag)
+#define RX_BLK_CLEAR_FLAG(_pRxBlk, _flag)	(_pRxBlk->Flags &= ~(_flag))
+
+
+#define fRX_WDS			0x0001
+#define fRX_AMSDU       0x0002
+#define fRX_ARALINK     0x0004
+#define fRX_HTC         0x0008
+#define fRX_PAD         0x0010
+#define fRX_AMPDU       0x0020
+#define fRX_QOS			0x0040
+#define fRX_INFRA		0x0080
+#define fRX_EAP			0x0100
+#define fRX_MESH		0x0200
+#define fRX_APCLI		0x0400
+#define fRX_DLS			0x0800
+#define fRX_WPI			0x1000
+
+#define LENGTH_AMSDU_SUBFRAMEHEAD	14
+#define LENGTH_ARALINK_SUBFRAMEHEAD	14
+#define LENGTH_ARALINK_HEADER_FIELD	 2
+
+#define TX_UNKOWN_FRAME			0x00
+#define TX_MCAST_FRAME			0x01
+#define TX_LEGACY_FRAME			0x02
+#define TX_AMPDU_FRAME			0x04
+#define TX_AMSDU_FRAME			0x08
+#define TX_RALINK_FRAME			0x10
+#define TX_FRAG_FRAME			0x20
+
+
+//	Currently the sizeof(TX_BLK) is 148 bytes.
+typedef struct _TX_BLK_
+{
+	UCHAR				QueIdx;
+	UCHAR				TxFrameType;				// Indicate the Transmission type of the all frames in one batch
+	UCHAR				TotalFrameNum;				// Total frame number want to send-out in one batch
+	USHORT				TotalFragNum;				// Total frame fragments required in one batch
+	USHORT				TotalFrameLen;				// Total length of all frames want to send-out in one batch
+
+	QUEUE_HEADER		TxPacketList;
+	MAC_TABLE_ENTRY		*pMacEntry;					// NULL: packet with 802.11 RA field is multicast/broadcast address
+	HTTRANSMIT_SETTING	*pTransmit;
+
+	// Following structure used for the characteristics of a specific packet.
+	PNDIS_PACKET		pPacket;
+	PUCHAR				pSrcBufHeader;				// Reference to the head of sk_buff->data
+	PUCHAR				pSrcBufData;				// Reference to the sk_buff->data, will changed depends on hanlding progresss
+	UINT				SrcBufLen;					// Length of packet payload which not including Layer 2 header
+	PUCHAR				pExtraLlcSnapEncap;			// NULL means no extra LLC/SNAP is required
+	UCHAR				HeaderBuf[80];				// TempBuffer for TX_INFO + TX_WI + 802.11 Header + padding + AMSDU SubHeader + LLC/SNAP
+	UCHAR				MpduHeaderLen;				// 802.11 header length NOT including the padding
+	UCHAR				HdrPadLen;					// recording Header Padding Length;
+	UCHAR				apidx;						// The interface associated to this packet
+	UCHAR				Wcid;						// The MAC entry associated to this packet
+	UCHAR				UserPriority;				// priority class of packet
+	UCHAR				FrameGap;					// what kind of IFS this packet use
+	UCHAR				MpduReqNum;					// number of fragments of this frame
+	UCHAR				TxRate;						// TODO: Obsoleted? Should change to MCS?
+	UCHAR				CipherAlg;					// cipher alogrithm
+	PCIPHER_KEY			pKey;
+
+
+
+	USHORT				Flags;						//See following definitions for detail.
+
+	//YOU SHOULD NOT TOUCH IT! Following parameters are used for hardware-depended layer.
+	ULONG				Priv;						// Hardware specific value saved in here.
+} TX_BLK, *PTX_BLK;
+
+
+#define fTX_bRtsRequired		0x0001	// Indicate if need send RTS frame for protection. Not used in RT2860/RT2870.
+#define fTX_bAckRequired       	0x0002	// the packet need ack response
+#define fTX_bPiggyBack     		0x0004	// Legacy device use Piggback or not
+#define fTX_bHTRate         	0x0008	// allow to use HT rate
+//#define fTX_bForceLowRate       0x0010	// force to use Low Rate
+#define fTX_bForceNonQoS       	0x0010	// force to transmit frame without WMM-QoS in HT mode
+#define fTX_bAllowFrag       	0x0020	// allow to fragment the packet, A-MPDU, A-MSDU, A-Ralink is not allowed to fragment
+#define fTX_bMoreData			0x0040	// there are more data packets in PowerSave Queue
+#define fTX_bWMM				0x0080	// QOS Data
+
+#define fTX_bClearEAPFrame		0x0100
+
+
+#ifdef CONFIG_STA_SUPPORT
+#endif // CONFIG_STA_SUPPORT //
+
+
+
+#define TX_BLK_ASSIGN_FLAG(_pTxBlk, _flag, value)	\
+		do {										\
+			if (value) 								\
+				(_pTxBlk->Flags |= _flag) 			\
+			else 									\
+				(_pTxBlk->Flags &= ~(_flag))		\
+		}while(0)
+
+#define TX_BLK_SET_FLAG(_pTxBlk, _flag)		(_pTxBlk->Flags |= _flag)
+#define TX_BLK_TEST_FLAG(_pTxBlk, _flag)	(((_pTxBlk->Flags & _flag) == _flag) ? 1 : 0)
+#define TX_BLK_CLEAR_FLAG(_pTxBlk, _flag)	(_pTxBlk->Flags &= ~(_flag))
+
+
+
+
+
+//------------------------------------------------------------------------------------------
+
+
+
+#ifdef RT_BIG_ENDIAN
+static inline VOID	WriteBackToDescriptor(
+	IN  PUCHAR			Dest,
+ 	IN	PUCHAR			Src,
+    IN  BOOLEAN			DoEncrypt,
+	IN  ULONG           DescriptorType)
+{
+	UINT32 *p1, *p2;
+
+	p1 = ((UINT32 *)Dest);
+	p2 = ((UINT32 *)Src);
+
+	*p1 = *p2;
+	*(p1+2) = *(p2+2);
+	*(p1+3) = *(p2+3);
+	*(p1+1) = *(p2+1); // Word 1; this must be written back last
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Endian conversion of Tx/Rx descriptor .
+
+	Arguments:
+		pAd 	Pointer to our adapter
+		pData			Pointer to Tx/Rx descriptor
+		DescriptorType	Direction of the frame
+
+	Return Value:
+		None
+
+	Note:
+		Call this function when read or update descriptor
+	========================================================================
+*/
+static inline VOID	RTMPWIEndianChange(
+	IN	PUCHAR			pData,
+	IN	ULONG			DescriptorType)
+{
+	int size;
+	int i;
+
+	size = ((DescriptorType == TYPE_TXWI) ? TXWI_SIZE : RXWI_SIZE);
+
+	if(DescriptorType == TYPE_TXWI)
+	{
+		*((UINT32 *)(pData)) = SWAP32(*((UINT32 *)(pData)));		// Byte 0~3
+		*((UINT32 *)(pData + 4)) = SWAP32(*((UINT32 *)(pData+4)));	// Byte 4~7
+	}
+	else
+	{
+		for(i=0; i < size/4 ; i++)
+			*(((UINT32 *)pData) +i) = SWAP32(*(((UINT32 *)pData)+i));
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Endian conversion of Tx/Rx descriptor .
+
+	Arguments:
+		pAd 	Pointer to our adapter
+		pData			Pointer to Tx/Rx descriptor
+		DescriptorType	Direction of the frame
+
+	Return Value:
+		None
+
+	Note:
+		Call this function when read or update descriptor
+	========================================================================
+*/
+
+#ifdef RT2870
+static inline VOID	RTMPDescriptorEndianChange(
+	IN	PUCHAR			pData,
+	IN	ULONG			DescriptorType)
+{
+	*((UINT32 *)(pData)) = SWAP32(*((UINT32 *)(pData)));
+}
+#endif // RT2870 //
+/*
+	========================================================================
+
+	Routine Description:
+		Endian conversion of all kinds of 802.11 frames .
+
+	Arguments:
+		pAd 	Pointer to our adapter
+		pData			Pointer to the 802.11 frame structure
+		Dir 			Direction of the frame
+		FromRxDoneInt	Caller is from RxDone interrupt
+
+	Return Value:
+		None
+
+	Note:
+		Call this function when read or update buffer data
+	========================================================================
+*/
+static inline VOID	RTMPFrameEndianChange(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pData,
+	IN	ULONG			Dir,
+	IN	BOOLEAN 		FromRxDoneInt)
+{
+	PHEADER_802_11 pFrame;
+	PUCHAR	pMacHdr;
+
+	// swab 16 bit fields - Frame Control field
+	if(Dir == DIR_READ)
+	{
+		*(USHORT *)pData = SWAP16(*(USHORT *)pData);
+	}
+
+	pFrame = (PHEADER_802_11) pData;
+	pMacHdr = (PUCHAR) pFrame;
+
+	// swab 16 bit fields - Duration/ID field
+	*(USHORT *)(pMacHdr + 2) = SWAP16(*(USHORT *)(pMacHdr + 2));
+
+	// swab 16 bit fields - Sequence Control field
+	*(USHORT *)(pMacHdr + 22) = SWAP16(*(USHORT *)(pMacHdr + 22));
+
+	if(pFrame->FC.Type == BTYPE_MGMT)
+	{
+		switch(pFrame->FC.SubType)
+		{
+			case SUBTYPE_ASSOC_REQ:
+			case SUBTYPE_REASSOC_REQ:
+				// swab 16 bit fields - CapabilityInfo field
+				pMacHdr += sizeof(HEADER_802_11);
+				*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+
+				// swab 16 bit fields - Listen Interval field
+				pMacHdr += 2;
+				*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+				break;
+
+			case SUBTYPE_ASSOC_RSP:
+			case SUBTYPE_REASSOC_RSP:
+				// swab 16 bit fields - CapabilityInfo field
+				pMacHdr += sizeof(HEADER_802_11);
+				*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+
+				// swab 16 bit fields - Status Code field
+				pMacHdr += 2;
+				*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+
+				// swab 16 bit fields - AID field
+				pMacHdr += 2;
+				*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+				break;
+
+			case SUBTYPE_AUTH:
+				// If from APHandleRxDoneInterrupt routine, it is still a encrypt format.
+				// The convertion is delayed to RTMPHandleDecryptionDoneInterrupt.
+				if(!FromRxDoneInt && pFrame->FC.Wep == 1)
+					break;
+				else
+				{
+					// swab 16 bit fields - Auth Alg No. field
+					pMacHdr += sizeof(HEADER_802_11);
+					*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+
+					// swab 16 bit fields - Auth Seq No. field
+					pMacHdr += 2;
+					*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+
+					// swab 16 bit fields - Status Code field
+					pMacHdr += 2;
+					*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+				}
+				break;
+
+			case SUBTYPE_BEACON:
+			case SUBTYPE_PROBE_RSP:
+				// swab 16 bit fields - BeaconInterval field
+				pMacHdr += (sizeof(HEADER_802_11) + TIMESTAMP_LEN);
+				*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+
+				// swab 16 bit fields - CapabilityInfo field
+				pMacHdr += sizeof(USHORT);
+				*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+				break;
+
+			case SUBTYPE_DEAUTH:
+			case SUBTYPE_DISASSOC:
+				// swab 16 bit fields - Reason code field
+				pMacHdr += sizeof(HEADER_802_11);
+				*(USHORT *)pMacHdr = SWAP16(*(USHORT *)pMacHdr);
+				break;
+		}
+	}
+	else if( pFrame->FC.Type == BTYPE_DATA )
+	{
+	}
+	else if(pFrame->FC.Type == BTYPE_CNTL)
+	{
+		switch(pFrame->FC.SubType)
+		{
+			case SUBTYPE_BLOCK_ACK_REQ:
+				{
+					PFRAME_BA_REQ pBAReq = (PFRAME_BA_REQ)pFrame;
+					*(USHORT *)(&pBAReq->BARControl) = SWAP16(*(USHORT *)(&pBAReq->BARControl));
+					pBAReq->BAStartingSeq.word = SWAP16(pBAReq->BAStartingSeq.word);
+				}
+				break;
+			case SUBTYPE_BLOCK_ACK:
+				// For Block Ack packet, the HT_CONTROL field is in the same offset with Addr3
+				*(UINT32 *)(&pFrame->Addr3[0]) = SWAP32(*(UINT32 *)(&pFrame->Addr3[0]));
+				break;
+
+			case SUBTYPE_ACK:
+				//For ACK packet, the HT_CONTROL field is in the same offset with Addr2
+				*(UINT32 *)(&pFrame->Addr2[0])=	SWAP32(*(UINT32 *)(&pFrame->Addr2[0]));
+				break;
+		}
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("Invalid Frame Type!!!\n"));
+	}
+
+	// swab 16 bit fields - Frame Control
+	if(Dir == DIR_WRITE)
+	{
+		*(USHORT *)pData = SWAP16(*(USHORT *)pData);
+	}
+}
+#endif // RT_BIG_ENDIAN //
+
+
+static inline VOID ConvertMulticastIP2MAC(
+	IN PUCHAR pIpAddr,
+	IN PUCHAR *ppMacAddr,
+	IN UINT16 ProtoType)
+{
+	if (pIpAddr == NULL)
+		return;
+
+	if (ppMacAddr == NULL || *ppMacAddr == NULL)
+		return;
+
+	switch (ProtoType)
+	{
+		case ETH_P_IPV6:
+//			memset(*ppMacAddr, 0, ETH_LENGTH_OF_ADDRESS);
+			*(*ppMacAddr) = 0x33;
+			*(*ppMacAddr + 1) = 0x33;
+			*(*ppMacAddr + 2) = pIpAddr[12];
+			*(*ppMacAddr + 3) = pIpAddr[13];
+			*(*ppMacAddr + 4) = pIpAddr[14];
+			*(*ppMacAddr + 5) = pIpAddr[15];
+			break;
+
+		case ETH_P_IP:
+		default:
+//			memset(*ppMacAddr, 0, ETH_LENGTH_OF_ADDRESS);
+			*(*ppMacAddr) = 0x01;
+			*(*ppMacAddr + 1) = 0x00;
+			*(*ppMacAddr + 2) = 0x5e;
+			*(*ppMacAddr + 3) = pIpAddr[1] & 0x7f;
+			*(*ppMacAddr + 4) = pIpAddr[2];
+			*(*ppMacAddr + 5) = pIpAddr[3];
+			break;
+	}
+
+	return;
+}
+
+BOOLEAN RTMPCheckForHang(
+	IN  NDIS_HANDLE MiniportAdapterContext
+	);
+
+VOID  RTMPHalt(
+	IN  NDIS_HANDLE MiniportAdapterContext
+	);
+
+//
+//  Private routines in rtmp_init.c
+//
+NDIS_STATUS RTMPAllocAdapterBlock(
+	IN PVOID			handle,
+	OUT PRTMP_ADAPTER   *ppAdapter
+	);
+
+NDIS_STATUS RTMPAllocTxRxRingMemory(
+	IN  PRTMP_ADAPTER   pAd
+	);
+
+NDIS_STATUS RTMPFindAdapter(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  NDIS_HANDLE     WrapperConfigurationContext
+	);
+
+NDIS_STATUS	RTMPReadParametersHook(
+	IN	PRTMP_ADAPTER pAd
+	);
+
+VOID RTMPFreeAdapter(
+	IN  PRTMP_ADAPTER   pAd
+	);
+
+NDIS_STATUS NICReadRegParameters(
+	IN  PRTMP_ADAPTER       pAd,
+	IN  NDIS_HANDLE         WrapperConfigurationContext
+	);
+
+#ifdef RT2870
+VOID NICInitRT30xxRFRegisters(
+	IN PRTMP_ADAPTER pAd);
+#endif // RT2870 //
+
+VOID NICReadEEPROMParameters(
+	IN  PRTMP_ADAPTER       pAd,
+	IN	PUCHAR				mac_addr);
+
+VOID NICInitAsicFromEEPROM(
+	IN  PRTMP_ADAPTER       pAd);
+
+VOID NICInitTxRxRingAndBacklogQueue(
+	IN  PRTMP_ADAPTER   pAd);
+
+NDIS_STATUS NICInitializeAdapter(
+	IN  PRTMP_ADAPTER   pAd,
+	IN   BOOLEAN    bHardReset);
+
+NDIS_STATUS NICInitializeAsic(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  BOOLEAN		bHardReset);
+
+VOID NICIssueReset(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID RTMPRingCleanUp(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           RingType);
+
+VOID RxTest(
+	IN  PRTMP_ADAPTER   pAd);
+
+NDIS_STATUS DbgSendPacket(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PNDIS_PACKET    pPacket);
+
+VOID UserCfgInit(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID NICResetFromError(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID NICEraseFirmware(
+	IN PRTMP_ADAPTER pAd);
+
+NDIS_STATUS NICLoadFirmware(
+	IN  PRTMP_ADAPTER   pAd);
+
+NDIS_STATUS NICLoadRateSwitchingParams(
+	IN PRTMP_ADAPTER pAd);
+
+BOOLEAN NICCheckForHang(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID NICUpdateFifoStaCounters(
+	IN PRTMP_ADAPTER pAd);
+
+VOID NICUpdateRawCounters(
+	IN  PRTMP_ADAPTER   pAd);
+
+#if 0
+ULONG RTMPEqualMemory(
+	IN  PVOID   pSrc1,
+	IN  PVOID   pSrc2,
+	IN  ULONG   Length);
+#endif
+
+ULONG	RTMPNotAllZero(
+	IN	PVOID	pSrc1,
+	IN	ULONG	Length);
+
+VOID RTMPZeroMemory(
+	IN  PVOID   pSrc,
+	IN  ULONG   Length);
+
+ULONG RTMPCompareMemory(
+	IN  PVOID   pSrc1,
+	IN  PVOID   pSrc2,
+	IN  ULONG   Length);
+
+VOID RTMPMoveMemory(
+	OUT PVOID   pDest,
+	IN  PVOID   pSrc,
+	IN  ULONG   Length);
+
+VOID AtoH(
+	char	*src,
+	UCHAR	*dest,
+	int		destlen);
+
+UCHAR BtoH(
+	char ch);
+
+VOID RTMPPatchMacBbpBug(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID RTMPPatchCardBus(
+	IN	PRTMP_ADAPTER	pAdapter);
+
+VOID RTMPPatchRalinkCardBus(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	ULONG			Bus);
+
+ULONG RTMPReadCBConfig(
+	IN	ULONG	Bus,
+	IN	ULONG	Slot,
+	IN	ULONG	Func,
+	IN	ULONG	Offset);
+
+VOID RTMPWriteCBConfig(
+	IN	ULONG	Bus,
+	IN	ULONG	Slot,
+	IN	ULONG	Func,
+	IN	ULONG	Offset,
+	IN	ULONG	Value);
+
+VOID RTMPInitTimer(
+	IN  PRTMP_ADAPTER           pAd,
+	IN  PRALINK_TIMER_STRUCT    pTimer,
+	IN  PVOID                   pTimerFunc,
+	IN	PVOID					pData,
+	IN  BOOLEAN                 Repeat);
+
+VOID RTMPSetTimer(
+	IN  PRALINK_TIMER_STRUCT    pTimer,
+	IN  ULONG                   Value);
+
+
+VOID RTMPModTimer(
+	IN	PRALINK_TIMER_STRUCT	pTimer,
+	IN	ULONG					Value);
+
+VOID RTMPCancelTimer(
+	IN  PRALINK_TIMER_STRUCT    pTimer,
+	OUT BOOLEAN                 *pCancelled);
+
+VOID RTMPSetLED(
+	IN PRTMP_ADAPTER 	pAd,
+	IN UCHAR			Status);
+
+VOID RTMPSetSignalLED(
+	IN PRTMP_ADAPTER 	pAd,
+	IN NDIS_802_11_RSSI Dbm);
+
+VOID RTMPEnableRxTx(
+	IN PRTMP_ADAPTER	pAd);
+
+//
+// prototype in action.c
+//
+VOID ActionStateMachineInit(
+    IN	PRTMP_ADAPTER	pAd,
+    IN  STATE_MACHINE *S,
+    OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID MlmeADDBAAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID MlmeDELBAAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID MlmeDLSAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID MlmeInvalidAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID MlmeQOSAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+#ifdef DOT11_N_SUPPORT
+VOID PeerAddBAReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID PeerAddBARspAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID PeerDelBAAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID PeerBAAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+#endif // DOT11_N_SUPPORT //
+
+VOID SendPSMPAction(
+	IN PRTMP_ADAPTER	pAd,
+	IN UCHAR			Wcid,
+	IN UCHAR			Psmp);
+
+
+#ifdef DOT11N_DRAFT3
+VOID SendBSS2040CoexistMgmtAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR	Wcid,
+	IN	UCHAR	apidx,
+	IN	UCHAR	InfoReq);
+
+VOID SendNotifyBWActionFrame(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR  Wcid,
+	IN UCHAR apidx);
+
+BOOLEAN ChannelSwitchSanityCheck(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR  Wcid,
+	IN    UCHAR  NewChannel,
+	IN    UCHAR  Secondary);
+
+VOID ChannelSwitchAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR  Wcid,
+	IN    UCHAR  Channel,
+	IN    UCHAR  Secondary);
+
+ULONG BuildIntolerantChannelRep(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    PUCHAR  pDest);
+
+VOID Update2040CoexistFrameAndNotify(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR  Wcid,
+	IN	BOOLEAN	bAddIntolerantCha);
+
+VOID Send2040CoexistAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR  Wcid,
+	IN	BOOLEAN	bAddIntolerantCha);
+#endif // DOT11N_DRAFT3 //
+
+VOID PeerRMAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID PeerPublicAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+#ifdef CONFIG_STA_SUPPORT
+VOID StaPublicAction(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR Bss2040Coexist);
+#endif // CONFIG_STA_SUPPORT //
+
+
+VOID PeerBSSTranAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+#ifdef DOT11_N_SUPPORT
+VOID PeerHTAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+#endif // DOT11_N_SUPPORT //
+
+VOID PeerQOSAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+#ifdef QOS_DLS_SUPPORT
+VOID PeerDLSAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+#endif // QOS_DLS_SUPPORT //
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+VOID DlsParmFill(
+	IN PRTMP_ADAPTER pAd,
+	IN OUT MLME_DLS_REQ_STRUCT *pDlsReq,
+	IN PRT_802_11_DLS pDls,
+	IN USHORT reason);
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef DOT11_N_SUPPORT
+VOID RECBATimerTimeout(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3);
+
+VOID ORIBATimerTimeout(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID SendRefreshBAR(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry);
+#endif // DOT11_N_SUPPORT //
+
+VOID ActHeaderInit(
+    IN	PRTMP_ADAPTER	pAd,
+    IN OUT PHEADER_802_11 pHdr80211,
+    IN PUCHAR Addr1,
+    IN PUCHAR Addr2,
+    IN PUCHAR Addr3);
+
+VOID BarHeaderInit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN OUT PFRAME_BAR pCntlBar,
+	IN PUCHAR pDA,
+	IN PUCHAR pSA);
+
+VOID InsertActField(
+	IN PRTMP_ADAPTER pAd,
+	OUT PUCHAR pFrameBuf,
+	OUT PULONG pFrameLen,
+	IN UINT8 Category,
+	IN UINT8 ActCode);
+
+BOOLEAN QosBADataParse(
+	IN PRTMP_ADAPTER	pAd,
+	IN BOOLEAN bAMSDU,
+	IN PUCHAR p8023Header,
+	IN UCHAR	WCID,
+	IN UCHAR	TID,
+	IN USHORT Sequence,
+	IN UCHAR DataOffset,
+	IN USHORT Datasize,
+	IN UINT   CurRxIndex);
+
+#ifdef DOT11_N_SUPPORT
+BOOLEAN CntlEnqueueForRecv(
+    IN	PRTMP_ADAPTER	pAd,
+	IN ULONG Wcid,
+    IN ULONG MsgLen,
+	IN PFRAME_BA_REQ pMsg);
+
+VOID BaAutoManSwitch(
+	IN	PRTMP_ADAPTER	pAd);
+#endif // DOT11_N_SUPPORT //
+
+VOID HTIOTCheck(
+	IN	PRTMP_ADAPTER	pAd,
+	IN    UCHAR     BatRecIdx);
+
+//
+// Private routines in rtmp_data.c
+//
+BOOLEAN RTMPHandleRxDoneInterrupt(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID RTMPHandleTxDoneInterrupt(
+	IN  PRTMP_ADAPTER   pAd);
+
+BOOLEAN RTMPHandleTxRingDmaDoneInterrupt(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  INT_SOURCE_CSR_STRUC TxRingBitmap);
+
+VOID RTMPHandleMgmtRingDmaDoneInterrupt(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID RTMPHandleTBTTInterrupt(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID RTMPHandlePreTBTTInterrupt(
+	IN  PRTMP_ADAPTER   pAd);
+
+void RTMPHandleTwakeupInterrupt(
+	IN PRTMP_ADAPTER pAd);
+
+VOID	RTMPHandleRxCoherentInterrupt(
+	IN	PRTMP_ADAPTER	pAd);
+
+BOOLEAN TxFrameIsAggregatible(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pPrevAddr1,
+	IN  PUCHAR          p8023hdr);
+
+BOOLEAN PeerIsAggreOn(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  ULONG          TxRate,
+    IN  PMAC_TABLE_ENTRY pMacEntry);
+
+#if 0	// It's not be used
+HTTRANSMIT_SETTING  *GetTxMode(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk);
+#endif
+
+NDIS_STATUS Sniff2BytesFromNdisBuffer(
+	IN  PNDIS_BUFFER    pFirstBuffer,
+	IN  UCHAR           DesiredOffset,
+	OUT PUCHAR          pByte0,
+	OUT PUCHAR          pByte1);
+
+NDIS_STATUS STASendPacket(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PNDIS_PACKET    pPacket);
+
+VOID STASendPackets(
+	IN  NDIS_HANDLE     MiniportAdapterContext,
+	IN  PPNDIS_PACKET   ppPacketArray,
+	IN  UINT            NumberOfPackets);
+
+VOID RTMPDeQueuePacket(
+	IN  PRTMP_ADAPTER   pAd,
+   	IN	BOOLEAN			bIntContext,
+	IN  UCHAR			QueIdx,
+	IN	UCHAR			Max_Tx_Packets);
+
+NDIS_STATUS	RTMPHardTransmit(
+	IN PRTMP_ADAPTER	pAd,
+	IN PNDIS_PACKET		pPacket,
+	IN  UCHAR			QueIdx,
+	OUT	PULONG			pFreeTXDLeft);
+
+NDIS_STATUS	STAHardTransmit(
+	IN PRTMP_ADAPTER	pAd,
+	IN TX_BLK			*pTxBlk,
+	IN  UCHAR			QueIdx);
+
+VOID STARxEAPOLFrameIndicate(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID);
+
+NDIS_STATUS RTMPFreeTXDRequest(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           RingType,
+	IN  UCHAR           NumberRequired,
+	IN 	PUCHAR          FreeNumberIs);
+
+NDIS_STATUS MlmeHardTransmit(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR	QueIdx,
+	IN  PNDIS_PACKET    pPacket);
+
+NDIS_STATUS MlmeHardTransmitMgmtRing(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR	QueIdx,
+	IN  PNDIS_PACKET    pPacket);
+
+NDIS_STATUS MlmeHardTransmitTxRing(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR	QueIdx,
+	IN  PNDIS_PACKET    pPacket);
+
+USHORT  RTMPCalcDuration(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           Rate,
+	IN  ULONG           Size);
+
+VOID RTMPWriteTxWI(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTXWI_STRUC		pTxWI,
+	IN  BOOLEAN    		FRAG,
+	IN  BOOLEAN    		CFACK,
+	IN  BOOLEAN    		InsTimestamp,
+	IN	BOOLEAN			AMPDU,
+	IN	BOOLEAN			Ack,
+	IN	BOOLEAN			NSeq,		// HW new a sequence.
+	IN	UCHAR			BASize,
+	IN	UCHAR			WCID,
+	IN	ULONG			Length,
+	IN  UCHAR      		PID,
+	IN	UCHAR			TID,
+	IN	UCHAR			TxRate,
+	IN	UCHAR			Txopmode,
+	IN	BOOLEAN			CfAck,
+	IN	HTTRANSMIT_SETTING	*pTransmit);
+
+
+VOID RTMPWriteTxWI_Data(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	OUT PTXWI_STRUC		pTxWI,
+	IN	TX_BLK				*pTxBlk);
+
+
+VOID RTMPWriteTxWI_Cache(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	OUT PTXWI_STRUC		pTxWI,
+	IN	TX_BLK				*pTxBlk);
+
+VOID RTMPWriteTxDescriptor(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTXD_STRUC		pTxD,
+	IN	BOOLEAN			bWIV,
+	IN	UCHAR			QSEL);
+
+VOID RTMPSuspendMsduTransmission(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID RTMPResumeMsduTransmission(
+	IN  PRTMP_ADAPTER   pAd);
+
+NDIS_STATUS MiniportMMRequest(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	UCHAR			QueIdx,
+	IN	PUCHAR			pData,
+	IN  UINT            Length);
+
+NDIS_STATUS MiniportDataMMRequest(
+	 IN  PRTMP_ADAPTER   pAd,
+	 IN  UCHAR           QueIdx,
+	 IN  PUCHAR          pData,
+	 IN  UINT            Length);
+
+VOID RTMPSendNullFrame(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           TxRate,
+	IN	BOOLEAN			bQosNull);
+
+VOID RTMPSendDisassociationFrame(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID RTMPSendRTSFrame(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pDA,
+	IN	unsigned int	NextMpduSize,
+	IN  UCHAR           TxRate,
+	IN  UCHAR           RTSRate,
+	IN  USHORT          AckDuration,
+	IN  UCHAR           QueIdx,
+	IN  UCHAR			FrameGap);
+
+
+NDIS_STATUS RTMPApplyPacketFilter(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PRT28XX_RXD_STRUC      pRxD,
+	IN  PHEADER_802_11  pHeader);
+
+PQUEUE_HEADER   RTMPCheckTxSwQueue(
+	IN  PRTMP_ADAPTER   pAd,
+	OUT UCHAR           *QueIdx);
+
+#ifdef CONFIG_STA_SUPPORT
+VOID RTMPReportMicError(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PCIPHER_KEY     pWpaKey);
+
+VOID	WpaMicFailureReportFrame(
+	IN  PRTMP_ADAPTER    pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID    WpaDisassocApAndBlockAssoc(
+    IN  PVOID SystemSpecific1,
+    IN  PVOID FunctionContext,
+    IN  PVOID SystemSpecific2,
+    IN  PVOID SystemSpecific3);
+#endif // CONFIG_STA_SUPPORT //
+
+NDIS_STATUS RTMPCloneNdisPacket(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	BOOLEAN    pInsAMSDUHdr,
+	IN  PNDIS_PACKET    pInPacket,
+	OUT PNDIS_PACKET   *ppOutPacket);
+
+NDIS_STATUS RTMPAllocateNdisPacket(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PNDIS_PACKET    *pPacket,
+	IN  PUCHAR          pHeader,
+	IN  UINT            HeaderLen,
+	IN  PUCHAR          pData,
+	IN  UINT            DataLen);
+
+VOID RTMPFreeNdisPacket(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PNDIS_PACKET    pPacket);
+
+BOOLEAN RTMPFreeTXDUponTxDmaDone(
+	IN PRTMP_ADAPTER    pAd,
+	IN UCHAR            QueIdx);
+
+BOOLEAN RTMPCheckDHCPFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket);
+
+
+BOOLEAN RTMPCheckEtherType(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket);
+
+
+VOID RTMPCckBbpTuning(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UINT			TxRate);
+
+//
+// Private routines in rtmp_wep.c
+//
+VOID RTMPInitWepEngine(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pKey,
+	IN  UCHAR           KeyId,
+	IN  UCHAR           KeyLen,
+	IN  PUCHAR          pDest);
+
+VOID RTMPEncryptData(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pSrc,
+	IN  PUCHAR          pDest,
+	IN  UINT            Len);
+
+BOOLEAN	RTMPDecryptData(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PUCHAR			pSrc,
+	IN	UINT			Len,
+	IN	UINT			idx);
+
+BOOLEAN	RTMPSoftDecryptWEP(
+	IN PRTMP_ADAPTER 	pAd,
+	IN PUCHAR			pData,
+	IN ULONG			DataByteCnt,
+	IN PCIPHER_KEY		pGroupKey);
+
+VOID RTMPSetICV(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pDest);
+
+VOID ARCFOUR_INIT(
+	IN  PARCFOURCONTEXT Ctx,
+	IN  PUCHAR          pKey,
+	IN  UINT            KeyLen);
+
+UCHAR   ARCFOUR_BYTE(
+	IN  PARCFOURCONTEXT     Ctx);
+
+VOID ARCFOUR_DECRYPT(
+	IN  PARCFOURCONTEXT Ctx,
+	IN  PUCHAR          pDest,
+	IN  PUCHAR          pSrc,
+	IN  UINT            Len);
+
+VOID ARCFOUR_ENCRYPT(
+	IN  PARCFOURCONTEXT Ctx,
+	IN  PUCHAR          pDest,
+	IN  PUCHAR          pSrc,
+	IN  UINT            Len);
+
+VOID WPAARCFOUR_ENCRYPT(
+	IN  PARCFOURCONTEXT Ctx,
+	IN  PUCHAR          pDest,
+	IN  PUCHAR          pSrc,
+	IN  UINT            Len);
+
+UINT RTMP_CALC_FCS32(
+	IN  UINT   Fcs,
+	IN  PUCHAR  Cp,
+	IN  INT     Len);
+
+//
+// MLME routines
+//
+
+// Asic/RF/BBP related functions
+
+VOID AsicAdjustTxPower(
+	IN PRTMP_ADAPTER pAd);
+
+VOID 	AsicUpdateProtect(
+	IN		PRTMP_ADAPTER	pAd,
+	IN 		USHORT			OperaionMode,
+	IN 		UCHAR			SetMask,
+	IN		BOOLEAN			bDisableBGProtect,
+	IN		BOOLEAN			bNonGFExist);
+
+VOID AsicSwitchChannel(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	UCHAR			Channel,
+	IN	BOOLEAN			bScan);
+
+VOID AsicLockChannel(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR Channel) ;
+
+VOID AsicAntennaSelect(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           Channel);
+
+VOID AsicAntennaSetting(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	ABGBAND_STATE	BandState);
+
+VOID AsicRfTuningExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+#ifdef CONFIG_STA_SUPPORT
+VOID AsicSleepThenAutoWakeup(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  USHORT TbttNumToNextWakeUp);
+
+VOID AsicForceSleep(
+	IN PRTMP_ADAPTER pAd);
+
+VOID AsicForceWakeup(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN    bFromTx);
+#endif // CONFIG_STA_SUPPORT //
+
+VOID AsicSetBssid(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR pBssid);
+
+VOID AsicSetMcastWC(
+	IN PRTMP_ADAPTER pAd);
+
+#if 0	// removed by AlbertY
+VOID AsicSetBssidWC(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pBssid);
+#endif
+
+VOID AsicDelWcidTab(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR	Wcid);
+
+VOID AsicEnableRDG(
+	IN PRTMP_ADAPTER pAd);
+
+VOID AsicDisableRDG(
+	IN PRTMP_ADAPTER pAd);
+
+VOID AsicDisableSync(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID AsicEnableBssSync(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID AsicEnableIbssSync(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID AsicSetEdcaParm(
+	IN PRTMP_ADAPTER pAd,
+	IN PEDCA_PARM    pEdcaParm);
+
+VOID AsicSetSlotTime(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN bUseShortSlotTime);
+
+#if 0
+VOID AsicAddWcidCipherEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR		 WCID,
+	IN UCHAR		 BssIndex,
+	IN UCHAR		 KeyTable,
+	IN UCHAR		 CipherAlg,
+	IN PUCHAR		 pAddr,
+	IN CIPHER_KEY		 *pCipherKey);
+#endif
+
+VOID AsicAddSharedKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR         BssIndex,
+	IN UCHAR         KeyIdx,
+	IN UCHAR         CipherAlg,
+	IN PUCHAR        pKey,
+	IN PUCHAR        pTxMic,
+	IN PUCHAR        pRxMic);
+
+VOID AsicRemoveSharedKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR         BssIndex,
+	IN UCHAR         KeyIdx);
+
+VOID AsicUpdateWCIDAttribute(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT		WCID,
+	IN UCHAR		BssIndex,
+	IN UCHAR        CipherAlg,
+	IN BOOLEAN		bUsePairewiseKeyTable);
+
+VOID AsicUpdateWCIDIVEIV(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT		WCID,
+	IN ULONG        uIV,
+	IN ULONG        uEIV);
+
+VOID AsicUpdateRxWCIDTable(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT		WCID,
+	IN PUCHAR        pAddr);
+
+VOID AsicAddKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT		WCID,
+	IN UCHAR		BssIndex,
+	IN UCHAR		KeyIdx,
+	IN PCIPHER_KEY	pCipherKey,
+	IN BOOLEAN		bUsePairewiseKeyTable,
+	IN BOOLEAN		bTxKey);
+
+VOID AsicAddPairwiseKeyEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR        pAddr,
+	IN UCHAR		WCID,
+	IN CIPHER_KEY		 *pCipherKey);
+
+VOID AsicRemovePairwiseKeyEntry(
+	IN PRTMP_ADAPTER  pAd,
+	IN UCHAR		 BssIdx,
+	IN UCHAR		 Wcid);
+
+BOOLEAN AsicSendCommandToMcu(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR         Command,
+	IN UCHAR         Token,
+	IN UCHAR         Arg0,
+	IN UCHAR         Arg1);
+
+
+VOID MacAddrRandomBssid(
+	IN  PRTMP_ADAPTER   pAd,
+	OUT PUCHAR pAddr);
+
+VOID MgtMacHeaderInit(
+	IN  PRTMP_ADAPTER     pAd,
+	IN OUT PHEADER_802_11 pHdr80211,
+	IN UCHAR SubType,
+	IN UCHAR ToDs,
+	IN PUCHAR pDA,
+	IN PUCHAR pBssid);
+
+VOID MlmeRadioOff(
+	IN PRTMP_ADAPTER pAd);
+
+VOID MlmeRadioOn(
+	IN PRTMP_ADAPTER pAd);
+
+
+VOID BssTableInit(
+	IN BSS_TABLE *Tab);
+
+#ifdef DOT11_N_SUPPORT
+VOID BATableInit(
+	IN PRTMP_ADAPTER pAd,
+    IN BA_TABLE *Tab);
+#endif // DOT11_N_SUPPORT //
+
+ULONG BssTableSearch(
+	IN BSS_TABLE *Tab,
+	IN PUCHAR pBssid,
+	IN UCHAR Channel);
+
+ULONG BssSsidTableSearch(
+	IN BSS_TABLE *Tab,
+	IN PUCHAR    pBssid,
+	IN PUCHAR    pSsid,
+	IN UCHAR     SsidLen,
+	IN UCHAR     Channel);
+
+ULONG BssTableSearchWithSSID(
+	IN BSS_TABLE *Tab,
+	IN PUCHAR    Bssid,
+	IN PUCHAR    pSsid,
+	IN UCHAR     SsidLen,
+	IN UCHAR     Channel);
+
+VOID BssTableDeleteEntry(
+	IN OUT  PBSS_TABLE pTab,
+	IN      PUCHAR pBssid,
+	IN      UCHAR Channel);
+
+#ifdef DOT11_N_SUPPORT
+VOID BATableDeleteORIEntry(
+	IN OUT	PRTMP_ADAPTER pAd,
+	IN		BA_ORI_ENTRY	*pBAORIEntry);
+
+VOID BATableDeleteRECEntry(
+	IN OUT	PRTMP_ADAPTER pAd,
+	IN		BA_REC_ENTRY	*pBARECEntry);
+
+VOID BATableTearORIEntry(
+	IN OUT	PRTMP_ADAPTER pAd,
+	IN		UCHAR TID,
+	IN		UCHAR Wcid,
+	IN		BOOLEAN bForceDelete,
+	IN		BOOLEAN ALL);
+
+VOID BATableTearRECEntry(
+	IN OUT	PRTMP_ADAPTER pAd,
+	IN		UCHAR TID,
+	IN		UCHAR WCID,
+	IN		BOOLEAN ALL);
+#endif // DOT11_N_SUPPORT //
+
+VOID  BssEntrySet(
+	IN  PRTMP_ADAPTER   pAd,
+	OUT PBSS_ENTRY pBss,
+	IN PUCHAR pBssid,
+	IN CHAR Ssid[],
+	IN UCHAR SsidLen,
+	IN UCHAR BssType,
+	IN USHORT BeaconPeriod,
+	IN PCF_PARM CfParm,
+	IN USHORT AtimWin,
+	IN USHORT CapabilityInfo,
+	IN UCHAR SupRate[],
+	IN UCHAR SupRateLen,
+	IN UCHAR ExtRate[],
+	IN UCHAR ExtRateLen,
+	IN HT_CAPABILITY_IE *pHtCapability,
+	IN ADD_HT_INFO_IE *pAddHtInfo,	// AP might use this additional ht info IE
+	IN UCHAR			HtCapabilityLen,
+	IN UCHAR			AddHtInfoLen,
+	IN UCHAR			NewExtChanOffset,
+	IN UCHAR Channel,
+	IN CHAR Rssi,
+	IN LARGE_INTEGER TimeStamp,
+	IN UCHAR CkipFlag,
+	IN PEDCA_PARM pEdcaParm,
+	IN PQOS_CAPABILITY_PARM pQosCapability,
+	IN PQBSS_LOAD_PARM pQbssLoad,
+	IN USHORT LengthVIE,
+	IN PNDIS_802_11_VARIABLE_IEs pVIE);
+
+ULONG  BssTableSetEntry(
+	IN  PRTMP_ADAPTER   pAd,
+	OUT PBSS_TABLE pTab,
+	IN PUCHAR pBssid,
+	IN CHAR Ssid[],
+	IN UCHAR SsidLen,
+	IN UCHAR BssType,
+	IN USHORT BeaconPeriod,
+	IN CF_PARM *CfParm,
+	IN USHORT AtimWin,
+	IN USHORT CapabilityInfo,
+	IN UCHAR SupRate[],
+	IN UCHAR SupRateLen,
+	IN UCHAR ExtRate[],
+	IN UCHAR ExtRateLen,
+	IN HT_CAPABILITY_IE *pHtCapability,
+	IN ADD_HT_INFO_IE *pAddHtInfo,	// AP might use this additional ht info IE
+	IN UCHAR			HtCapabilityLen,
+	IN UCHAR			AddHtInfoLen,
+	IN UCHAR			NewExtChanOffset,
+	IN UCHAR Channel,
+	IN CHAR Rssi,
+	IN LARGE_INTEGER TimeStamp,
+	IN UCHAR CkipFlag,
+	IN PEDCA_PARM pEdcaParm,
+	IN PQOS_CAPABILITY_PARM pQosCapability,
+	IN PQBSS_LOAD_PARM pQbssLoad,
+	IN USHORT LengthVIE,
+	IN PNDIS_802_11_VARIABLE_IEs pVIE);
+
+#ifdef DOT11_N_SUPPORT
+VOID BATableInsertEntry(
+    IN	PRTMP_ADAPTER	pAd,
+	IN USHORT Aid,
+    IN USHORT		TimeOutValue,
+	IN USHORT		StartingSeq,
+    IN UCHAR TID,
+	IN UCHAR BAWinSize,
+	IN UCHAR OriginatorStatus,
+    IN BOOLEAN IsRecipient);
+
+#ifdef DOT11N_DRAFT3
+VOID Bss2040CoexistTimeOut(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+
+VOID  TriEventInit(
+	IN	PRTMP_ADAPTER	pAd);
+
+ULONG TriEventTableSetEntry(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT TRIGGER_EVENT_TAB *Tab,
+	IN PUCHAR pBssid,
+	IN HT_CAPABILITY_IE *pHtCapability,
+	IN UCHAR			HtCapabilityLen,
+	IN UCHAR			RegClass,
+	IN UCHAR ChannelNo);
+
+VOID TriEventCounterMaintenance(
+	IN	PRTMP_ADAPTER	pAd);
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+
+VOID BssTableSsidSort(
+	IN  PRTMP_ADAPTER   pAd,
+	OUT BSS_TABLE *OutTab,
+	IN  CHAR Ssid[],
+	IN  UCHAR SsidLen);
+
+VOID  BssTableSortByRssi(
+	IN OUT BSS_TABLE *OutTab);
+
+VOID BssCipherParse(
+	IN OUT  PBSS_ENTRY  pBss);
+
+NDIS_STATUS  MlmeQueueInit(
+	IN MLME_QUEUE *Queue);
+
+VOID  MlmeQueueDestroy(
+	IN MLME_QUEUE *Queue);
+
+BOOLEAN MlmeEnqueue(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG Machine,
+	IN ULONG MsgType,
+	IN ULONG MsgLen,
+	IN VOID *Msg);
+
+BOOLEAN MlmeEnqueueForRecv(
+	IN  PRTMP_ADAPTER   pAd,
+	IN ULONG Wcid,
+	IN ULONG TimeStampHigh,
+	IN ULONG TimeStampLow,
+	IN UCHAR Rssi0,
+	IN UCHAR Rssi1,
+	IN UCHAR Rssi2,
+	IN ULONG MsgLen,
+	IN PVOID Msg,
+	IN UCHAR Signal);
+
+
+BOOLEAN MlmeDequeue(
+	IN MLME_QUEUE *Queue,
+	OUT MLME_QUEUE_ELEM **Elem);
+
+VOID    MlmeRestartStateMachine(
+	IN  PRTMP_ADAPTER   pAd);
+
+BOOLEAN  MlmeQueueEmpty(
+	IN MLME_QUEUE *Queue);
+
+BOOLEAN  MlmeQueueFull(
+	IN MLME_QUEUE *Queue);
+
+BOOLEAN  MsgTypeSubst(
+	IN PRTMP_ADAPTER pAd,
+	IN PFRAME_802_11 pFrame,
+	OUT INT *Machine,
+	OUT INT *MsgType);
+
+VOID StateMachineInit(
+	IN STATE_MACHINE *Sm,
+	IN STATE_MACHINE_FUNC Trans[],
+	IN ULONG StNr,
+	IN ULONG MsgNr,
+	IN STATE_MACHINE_FUNC DefFunc,
+	IN ULONG InitState,
+	IN ULONG Base);
+
+VOID StateMachineSetAction(
+	IN STATE_MACHINE *S,
+	IN ULONG St,
+	ULONG Msg,
+	IN STATE_MACHINE_FUNC F);
+
+VOID StateMachinePerformAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN STATE_MACHINE *S,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID Drop(
+	IN  PRTMP_ADAPTER   pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID AssocStateMachineInit(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  STATE_MACHINE *Sm,
+	OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID ReassocTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID AssocTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID DisassocTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+//----------------------------------------------
+VOID MlmeDisassocReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID MlmeAssocReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID MlmeReassocReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID MlmeDisassocReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerAssocRspAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerReassocRspAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerDisassocAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID DisassocTimeoutAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID AssocTimeoutAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID  ReassocTimeoutAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID  Cls3errAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR pAddr);
+
+VOID SwitchBetweenWepAndCkip(
+	IN PRTMP_ADAPTER pAd);
+
+VOID  InvalidStateWhenAssoc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID  InvalidStateWhenReassoc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID InvalidStateWhenDisassociate(
+	IN  PRTMP_ADAPTER pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+#ifdef RT2870
+VOID MlmeCntlConfirm(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG MsgType,
+	IN USHORT Msg);
+#endif // RT2870 //
+
+VOID  ComposePsPoll(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID  ComposeNullFrame(
+	IN  PRTMP_ADAPTER pAd);
+
+VOID  AssocPostProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR pAddr2,
+	IN  USHORT CapabilityInfo,
+	IN  USHORT Aid,
+	IN  UCHAR SupRate[],
+	IN  UCHAR SupRateLen,
+	IN  UCHAR ExtRate[],
+	IN  UCHAR ExtRateLen,
+	IN PEDCA_PARM pEdcaParm,
+	IN HT_CAPABILITY_IE		*pHtCapability,
+	IN  UCHAR HtCapabilityLen,
+	IN ADD_HT_INFO_IE		*pAddHtInfo);
+
+VOID AuthStateMachineInit(
+	IN  PRTMP_ADAPTER   pAd,
+	IN PSTATE_MACHINE sm,
+	OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID AuthTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID MlmeAuthReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerAuthRspAtSeq2Action(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerAuthRspAtSeq4Action(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID AuthTimeoutAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID Cls2errAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR pAddr);
+
+VOID MlmeDeauthReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID InvalidStateWhenAuth(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+//=============================================
+
+VOID AuthRspStateMachineInit(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PSTATE_MACHINE Sm,
+	IN  STATE_MACHINE_FUNC Trans[]);
+
+VOID PeerDeauthAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID PeerAuthSimpleRspGenAndSend(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PHEADER_802_11  pHdr80211,
+	IN  USHORT Alg,
+	IN  USHORT Seq,
+	IN  USHORT Reason,
+	IN  USHORT Status);
+
+//
+// Private routines in dls.c
+//
+
+#ifdef CONFIG_STA_SUPPORT
+#ifdef QOS_DLS_SUPPORT
+void DlsStateMachineInit(
+    IN PRTMP_ADAPTER pAd,
+    IN STATE_MACHINE *Sm,
+    OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID MlmeDlsReqAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID PeerDlsReqAction(
+    IN PRTMP_ADAPTER	pAd,
+    IN MLME_QUEUE_ELEM	*Elem);
+
+VOID PeerDlsRspAction(
+    IN PRTMP_ADAPTER	pAd,
+    IN MLME_QUEUE_ELEM	*Elem);
+
+VOID MlmeDlsTearDownAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+VOID PeerDlsTearDownAction(
+    IN PRTMP_ADAPTER	pAd,
+    IN MLME_QUEUE_ELEM	*Elem);
+
+VOID RTMPCheckDLSTimeOut(
+	IN PRTMP_ADAPTER	pAd);
+
+BOOLEAN RTMPRcvFrameDLSCheck(
+	IN PRTMP_ADAPTER	pAd,
+	IN PHEADER_802_11	pHeader,
+	IN ULONG			Len,
+	IN PRT28XX_RXD_STRUC	pRxD);
+
+INT	RTMPCheckDLSFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN  PUCHAR          pDA);
+
+VOID RTMPSendDLSTearDownFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN  PUCHAR          pDA);
+
+NDIS_STATUS RTMPSendSTAKeyRequest(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pDA);
+
+NDIS_STATUS RTMPSendSTAKeyHandShake(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pDA);
+
+VOID DlsTimeoutAction(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+BOOLEAN MlmeDlsReqSanity(
+	IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PRT_802_11_DLS *pDLS,
+    OUT PUSHORT pReason);
+
+INT Set_DlsEntryInfo_Display_Proc(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR arg);
+
+MAC_TABLE_ENTRY *MacTableInsertDlsEntry(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR	pAddr,
+	IN  UINT	DlsEntryIdx);
+
+BOOLEAN MacTableDeleteDlsEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT wcid,
+	IN PUCHAR pAddr);
+
+MAC_TABLE_ENTRY *DlsEntryTableLookup(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR	pAddr,
+	IN BOOLEAN	bResetIdelCount);
+
+MAC_TABLE_ENTRY *DlsEntryTableLookupByWcid(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR	wcid,
+	IN PUCHAR	pAddr,
+	IN BOOLEAN	bResetIdelCount);
+
+INT	Set_DlsAddEntry_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_DlsTearDownEntry_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif // QOS_DLS_SUPPORT //
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef QOS_DLS_SUPPORT
+BOOLEAN PeerDlsReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pDA,
+    OUT PUCHAR pSA,
+    OUT USHORT *pCapabilityInfo,
+    OUT USHORT *pDlsTimeout,
+    OUT UCHAR *pRatesLen,
+    OUT UCHAR Rates[],
+    OUT UCHAR *pHtCapabilityLen,
+    OUT HT_CAPABILITY_IE *pHtCapability);
+
+BOOLEAN PeerDlsRspSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pDA,
+    OUT PUCHAR pSA,
+    OUT USHORT *pCapabilityInfo,
+    OUT USHORT *pStatus,
+    OUT UCHAR *pRatesLen,
+    OUT UCHAR Rates[],
+    OUT UCHAR *pHtCapabilityLen,
+    OUT HT_CAPABILITY_IE *pHtCapability);
+
+BOOLEAN PeerDlsTearDownSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pDA,
+    OUT PUCHAR pSA,
+    OUT USHORT *pReason);
+#endif // QOS_DLS_SUPPORT //
+
+//========================================
+
+VOID SyncStateMachineInit(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  STATE_MACHINE *Sm,
+	OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID BeaconTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID ScanTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID MlmeScanReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID InvalidStateWhenScan(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID InvalidStateWhenJoin(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID InvalidStateWhenStart(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerBeacon(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID EnqueueProbeRequest(
+	IN PRTMP_ADAPTER pAd);
+
+BOOLEAN ScanRunning(
+		IN PRTMP_ADAPTER pAd);
+//=========================================
+
+VOID MlmeCntlInit(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  STATE_MACHINE *S,
+	OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID MlmeCntlMachinePerformAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  STATE_MACHINE *S,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID CntlIdleProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID CntlOidScanProc(
+	IN  PRTMP_ADAPTER pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID CntlOidSsidProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM * Elem);
+
+VOID CntlOidRTBssidProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM * Elem);
+
+VOID CntlMlmeRoamingProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM * Elem);
+
+VOID CntlWaitDisassocProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID CntlWaitJoinProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID CntlWaitReassocProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID CntlWaitStartProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID CntlWaitAuthProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID CntlWaitAuthProc2(
+	IN  PRTMP_ADAPTER pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID CntlWaitAssocProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+#ifdef QOS_DLS_SUPPORT
+VOID CntlOidDLSSetupProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+#endif // QOS_DLS_SUPPORT //
+
+VOID LinkUp(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR BssType);
+
+VOID LinkDown(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  BOOLEAN         IsReqFromAP);
+
+VOID IterateOnBssTab(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID IterateOnBssTab2(
+	IN  PRTMP_ADAPTER   pAd);;
+
+VOID JoinParmFill(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  OUT MLME_JOIN_REQ_STRUCT *JoinReq,
+	IN  ULONG BssIdx);
+
+VOID AssocParmFill(
+	IN  PRTMP_ADAPTER   pAd,
+	IN OUT MLME_ASSOC_REQ_STRUCT *AssocReq,
+	IN  PUCHAR pAddr,
+	IN  USHORT CapabilityInfo,
+	IN  ULONG Timeout,
+	IN  USHORT ListenIntv);
+
+VOID ScanParmFill(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  OUT MLME_SCAN_REQ_STRUCT *ScanReq,
+	IN  CHAR Ssid[],
+	IN  UCHAR SsidLen,
+	IN  UCHAR BssType,
+	IN  UCHAR ScanType);
+
+VOID DisassocParmFill(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  OUT MLME_DISASSOC_REQ_STRUCT *DisassocReq,
+	IN  PUCHAR pAddr,
+	IN  USHORT Reason);
+
+VOID StartParmFill(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  OUT MLME_START_REQ_STRUCT *StartReq,
+	IN  CHAR Ssid[],
+	IN  UCHAR SsidLen);
+
+VOID AuthParmFill(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  OUT MLME_AUTH_REQ_STRUCT *AuthReq,
+	IN  PUCHAR pAddr,
+	IN  USHORT Alg);
+
+VOID EnqueuePsPoll(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID EnqueueBeaconFrame(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID MlmeJoinReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID MlmeScanReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID MlmeStartReqAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID ScanTimeoutAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID BeaconTimeoutAtJoinAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerBeaconAtScanAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerBeaconAtJoinAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerBeacon(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID PeerProbeReqAction(
+	IN  PRTMP_ADAPTER pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID ScanNextChannel(
+	IN  PRTMP_ADAPTER   pAd);
+
+ULONG MakeIbssBeacon(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID CCXAdjacentAPReport(
+	IN  PRTMP_ADAPTER   pAd);
+
+BOOLEAN MlmeScanReqSanity(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  VOID *Msg,
+	IN  ULONG MsgLen,
+	OUT UCHAR *BssType,
+	OUT CHAR ssid[],
+	OUT UCHAR *SsidLen,
+	OUT UCHAR *ScanType);
+
+BOOLEAN PeerBeaconAndProbeRspSanity(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  VOID *Msg,
+	IN  ULONG MsgLen,
+	IN  UCHAR MsgChannel,
+	OUT PUCHAR pAddr2,
+	OUT PUCHAR pBssid,
+	OUT CHAR Ssid[],
+	OUT UCHAR *pSsidLen,
+	OUT UCHAR *pBssType,
+	OUT USHORT *pBeaconPeriod,
+	OUT UCHAR *pChannel,
+	OUT UCHAR *pNewChannel,
+	OUT LARGE_INTEGER *pTimestamp,
+	OUT CF_PARM *pCfParm,
+	OUT USHORT *pAtimWin,
+	OUT USHORT *pCapabilityInfo,
+	OUT UCHAR *pErp,
+	OUT UCHAR *pDtimCount,
+	OUT UCHAR *pDtimPeriod,
+	OUT UCHAR *pBcastFlag,
+	OUT UCHAR *pMessageToMe,
+	OUT UCHAR SupRate[],
+	OUT UCHAR *pSupRateLen,
+	OUT UCHAR ExtRate[],
+	OUT UCHAR *pExtRateLen,
+	OUT	UCHAR *pCkipFlag,
+	OUT	UCHAR *pAironetCellPowerLimit,
+	OUT PEDCA_PARM       pEdcaParm,
+	OUT PQBSS_LOAD_PARM  pQbssLoad,
+	OUT PQOS_CAPABILITY_PARM pQosCapability,
+	OUT ULONG *pRalinkIe,
+	OUT UCHAR		 *pHtCapabilityLen,
+#ifdef CONFIG_STA_SUPPORT
+	OUT UCHAR		 *pPreNHtCapabilityLen,
+#endif // CONFIG_STA_SUPPORT //
+	OUT HT_CAPABILITY_IE *pHtCapability,
+	OUT UCHAR		 *AddHtInfoLen,
+	OUT ADD_HT_INFO_IE *AddHtInfo,
+	OUT UCHAR *NewExtChannel,
+	OUT USHORT *LengthVIE,
+	OUT PNDIS_802_11_VARIABLE_IEs pVIE);
+
+BOOLEAN PeerAddBAReqActionSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *pMsg,
+    IN ULONG MsgLen,
+	OUT PUCHAR pAddr2);
+
+BOOLEAN PeerAddBARspActionSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *pMsg,
+    IN ULONG MsgLen);
+
+BOOLEAN PeerDelBAActionSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN UCHAR Wcid,
+    IN VOID *pMsg,
+    IN ULONG MsgLen);
+
+BOOLEAN MlmeAssocReqSanity(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  VOID *Msg,
+	IN  ULONG MsgLen,
+	OUT PUCHAR pApAddr,
+	OUT USHORT *CapabilityInfo,
+	OUT ULONG *Timeout,
+	OUT USHORT *ListenIntv);
+
+BOOLEAN MlmeAuthReqSanity(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  VOID *Msg,
+	IN  ULONG MsgLen,
+	OUT PUCHAR pAddr,
+	OUT ULONG *Timeout,
+	OUT USHORT *Alg);
+
+BOOLEAN MlmeStartReqSanity(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  VOID *Msg,
+	IN  ULONG MsgLen,
+	OUT CHAR Ssid[],
+	OUT UCHAR *Ssidlen);
+
+BOOLEAN PeerAuthSanity(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  VOID *Msg,
+	IN  ULONG MsgLen,
+	OUT PUCHAR pAddr,
+	OUT USHORT *Alg,
+	OUT USHORT *Seq,
+	OUT USHORT *Status,
+	OUT CHAR ChlgText[]);
+
+BOOLEAN PeerAssocRspSanity(
+	IN  PRTMP_ADAPTER   pAd,
+    IN VOID *pMsg,
+	IN  ULONG MsgLen,
+	OUT PUCHAR pAddr2,
+	OUT USHORT *pCapabilityInfo,
+	OUT USHORT *pStatus,
+	OUT USHORT *pAid,
+	OUT UCHAR SupRate[],
+	OUT UCHAR *pSupRateLen,
+	OUT UCHAR ExtRate[],
+	OUT UCHAR *pExtRateLen,
+    OUT HT_CAPABILITY_IE		*pHtCapability,
+    OUT ADD_HT_INFO_IE		*pAddHtInfo,	// AP might use this additional ht info IE
+    OUT UCHAR			*pHtCapabilityLen,
+    OUT UCHAR			*pAddHtInfoLen,
+    OUT UCHAR			*pNewExtChannelOffset,
+	OUT PEDCA_PARM pEdcaParm,
+	OUT UCHAR *pCkipFlag);
+
+BOOLEAN PeerDisassocSanity(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  VOID *Msg,
+	IN  ULONG MsgLen,
+	OUT PUCHAR pAddr2,
+	OUT USHORT *Reason);
+
+BOOLEAN PeerWpaMessageSanity(
+    IN 	PRTMP_ADAPTER 		pAd,
+    IN 	PEAPOL_PACKET 		pMsg,
+    IN 	ULONG 				MsgLen,
+    IN 	UCHAR				MsgType,
+    IN 	MAC_TABLE_ENTRY  	*pEntry);
+
+BOOLEAN PeerDeauthSanity(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  VOID *Msg,
+	IN  ULONG MsgLen,
+	OUT PUCHAR pAddr2,
+	OUT USHORT *Reason);
+
+BOOLEAN PeerProbeReqSanity(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  VOID *Msg,
+	IN  ULONG MsgLen,
+	OUT PUCHAR pAddr2,
+	OUT CHAR Ssid[],
+	OUT UCHAR *pSsidLen);
+
+BOOLEAN GetTimBit(
+	IN  CHAR *Ptr,
+	IN  USHORT Aid,
+	OUT UCHAR *TimLen,
+	OUT UCHAR *BcastFlag,
+	OUT UCHAR *DtimCount,
+	OUT UCHAR *DtimPeriod,
+	OUT UCHAR *MessageToMe);
+
+UCHAR ChannelSanity(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR channel);
+
+NDIS_802_11_NETWORK_TYPE NetworkTypeInUseSanity(
+	IN PBSS_ENTRY pBss);
+
+#if 0	// It's omitted
+NDIS_STATUS	RTMPWepKeySanity(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PVOID			pBuf);
+#endif
+
+BOOLEAN MlmeDelBAReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen);
+
+BOOLEAN MlmeAddBAReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2);
+
+ULONG MakeOutgoingFrame(
+	OUT CHAR *Buffer,
+	OUT ULONG *Length, ...);
+
+VOID  LfsrInit(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  ULONG Seed);
+
+UCHAR RandomByte(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID AsicUpdateAutoFallBackTable(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pTxRate);
+
+VOID  MlmePeriodicExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID LinkDownExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID LinkUpExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID STAMlmePeriodicExec(
+	PRTMP_ADAPTER pAd);
+
+VOID MlmeAutoScan(
+	IN PRTMP_ADAPTER pAd);
+
+VOID MlmeAutoReconnectLastSSID(
+	IN PRTMP_ADAPTER pAd);
+
+BOOLEAN MlmeValidateSSID(
+	IN PUCHAR pSsid,
+	IN UCHAR  SsidLen);
+
+VOID MlmeCheckForRoaming(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG    Now32);
+
+VOID MlmeCheckForFastRoaming(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  ULONG           Now);
+
+VOID MlmeDynamicTxRateSwitching(
+	IN PRTMP_ADAPTER pAd);
+
+VOID MlmeSetTxRate(
+	IN PRTMP_ADAPTER		pAd,
+	IN PMAC_TABLE_ENTRY		pEntry,
+	IN PRTMP_TX_RATE_SWITCH	pTxRate);
+
+VOID MlmeSelectTxRateTable(
+	IN PRTMP_ADAPTER		pAd,
+	IN PMAC_TABLE_ENTRY		pEntry,
+	IN PUCHAR				*ppTable,
+	IN PUCHAR				pTableSize,
+	IN PUCHAR				pInitTxRateIdx);
+
+VOID MlmeCalculateChannelQuality(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG Now);
+
+VOID MlmeCheckPsmChange(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG    Now32);
+
+VOID MlmeSetPsmBit(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT psm);
+
+VOID MlmeSetTxPreamble(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT TxPreamble);
+
+VOID UpdateBasicRateBitmap(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID MlmeUpdateTxRates(
+	IN PRTMP_ADAPTER 	pAd,
+	IN 	BOOLEAN		 	bLinkUp,
+	IN	UCHAR			apidx);
+
+#ifdef DOT11_N_SUPPORT
+VOID MlmeUpdateHtTxRates(
+	IN PRTMP_ADAPTER 		pAd,
+	IN	UCHAR				apidx);
+#endif // DOT11_N_SUPPORT //
+
+VOID    RTMPCheckRates(
+	IN      PRTMP_ADAPTER   pAd,
+	IN OUT  UCHAR           SupRate[],
+	IN OUT  UCHAR           *SupRateLen);
+
+#ifdef CONFIG_STA_SUPPORT
+BOOLEAN RTMPCheckChannel(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR		CentralChannel,
+	IN UCHAR		Channel);
+#endif // CONFIG_STA_SUPPORT //
+
+BOOLEAN 	RTMPCheckHt(
+	IN		PRTMP_ADAPTER	pAd,
+	IN		UCHAR	Wcid,
+	IN OUT	HT_CAPABILITY_IE			*pHtCapability,
+	IN OUT	ADD_HT_INFO_IE			*pAddHtInfo);
+
+VOID StaQuickResponeForRateUpExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID AsicBbpTuning1(
+	IN PRTMP_ADAPTER pAd);
+
+VOID AsicBbpTuning2(
+	IN PRTMP_ADAPTER pAd);
+
+VOID RTMPUpdateMlmeRate(
+	IN PRTMP_ADAPTER	pAd);
+
+CHAR RTMPMaxRssi(
+	IN PRTMP_ADAPTER	pAd,
+	IN CHAR				Rssi0,
+	IN CHAR				Rssi1,
+	IN CHAR				Rssi2);
+
+VOID AsicEvaluateRxAnt(
+	IN PRTMP_ADAPTER	pAd);
+
+VOID AsicRxAntEvalTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID APSDPeriodicExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+BOOLEAN RTMPCheckEntryEnableAutoRateSwitch(
+	IN PRTMP_ADAPTER    pAd,
+	IN PMAC_TABLE_ENTRY	pEntry);
+
+UCHAR RTMPStaFixedTxMode(
+	IN PRTMP_ADAPTER    pAd,
+	IN PMAC_TABLE_ENTRY	pEntry);
+
+VOID RTMPUpdateLegacyTxSetting(
+		UCHAR				fixed_tx_mode,
+		PMAC_TABLE_ENTRY	pEntry);
+
+BOOLEAN RTMPAutoRateSwitchCheck(
+	IN PRTMP_ADAPTER    pAd);
+
+NDIS_STATUS MlmeInit(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID MlmeHandler(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID MlmeHalt(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID MlmeResetRalinkCounters(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID BuildChannelList(
+	IN PRTMP_ADAPTER pAd);
+
+UCHAR FirstChannel(
+	IN  PRTMP_ADAPTER   pAd);
+
+UCHAR NextChannel(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR channel);
+
+VOID ChangeToCellPowerLimit(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR         AironetCellPowerLimit);
+
+VOID RaiseClock(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UINT32 *x);
+
+VOID LowerClock(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UINT32 *x);
+
+USHORT ShiftInBits(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID ShiftOutBits(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  USHORT data,
+	IN  USHORT count);
+
+VOID EEpromCleanup(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID EWDS(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID EWEN(
+	IN  PRTMP_ADAPTER   pAd);
+
+USHORT RTMP_EEPROM_READ16(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  USHORT Offset);
+
+VOID RTMP_EEPROM_WRITE16(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  USHORT Offset,
+	IN  USHORT Data);
+
+//
+// Prototypes of function definition in rtmp_tkip.c
+//
+VOID    RTMPInitTkipEngine(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pTKey,
+	IN  UCHAR           KeyId,
+	IN  PUCHAR          pTA,
+	IN  PUCHAR          pMICKey,
+	IN  PUCHAR          pTSC,
+	OUT PULONG          pIV16,
+	OUT PULONG          pIV32);
+
+VOID    RTMPInitMICEngine(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pKey,
+	IN  PUCHAR          pDA,
+	IN  PUCHAR          pSA,
+	IN  UCHAR           UserPriority,
+	IN  PUCHAR          pMICKey);
+
+BOOLEAN RTMPTkipCompareMICValue(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pSrc,
+	IN  PUCHAR          pDA,
+	IN  PUCHAR          pSA,
+	IN  PUCHAR          pMICKey,
+	IN	UCHAR			UserPriority,
+	IN  UINT            Len);
+
+VOID    RTMPCalculateMICValue(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PNDIS_PACKET    pPacket,
+	IN  PUCHAR          pEncap,
+	IN  PCIPHER_KEY     pKey,
+	IN	UCHAR			apidx);
+
+BOOLEAN RTMPTkipCompareMICValueWithLLC(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pLLC,
+	IN  PUCHAR          pSrc,
+	IN  PUCHAR          pDA,
+	IN  PUCHAR          pSA,
+	IN  PUCHAR          pMICKey,
+	IN  UINT            Len);
+
+VOID    RTMPTkipAppendByte(
+	IN  PTKIP_KEY_INFO  pTkip,
+	IN  UCHAR           uChar);
+
+VOID    RTMPTkipAppend(
+	IN  PTKIP_KEY_INFO  pTkip,
+	IN  PUCHAR          pSrc,
+	IN  UINT            nBytes);
+
+VOID    RTMPTkipGetMIC(
+	IN  PTKIP_KEY_INFO  pTkip);
+
+BOOLEAN RTMPSoftDecryptTKIP(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR	pData,
+	IN ULONG	DataByteCnt,
+	IN UCHAR    UserPriority,
+	IN PCIPHER_KEY	pWpaKey);
+
+BOOLEAN RTMPSoftDecryptAES(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR	pData,
+	IN ULONG	DataByteCnt,
+	IN PCIPHER_KEY	pWpaKey);
+
+#if 0	// removed by AlbertY
+NDIS_STATUS RTMPWPAAddKeyProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PVOID           pBuf);
+#endif
+
+//
+// Prototypes of function definition in cmm_info.c
+//
+NDIS_STATUS RTMPWPARemoveKeyProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PVOID           pBuf);
+
+VOID    RTMPWPARemoveAllKeys(
+	IN  PRTMP_ADAPTER   pAd);
+
+BOOLEAN RTMPCheckStrPrintAble(
+    IN  CHAR *pInPutStr,
+    IN  UCHAR strLen);
+
+VOID    RTMPSetPhyMode(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  ULONG phymode);
+
+VOID	RTMPUpdateHTIE(
+	IN	RT_HT_CAPABILITY	*pRtHt,
+	IN		UCHAR				*pMcsSet,
+	OUT		HT_CAPABILITY_IE *pHtCapability,
+	OUT		ADD_HT_INFO_IE		*pAddHtInfo);
+
+VOID	RTMPAddWcidAttributeEntry(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			BssIdx,
+	IN 	UCHAR		 	KeyIdx,
+	IN 	UCHAR		 	CipherAlg,
+	IN 	MAC_TABLE_ENTRY *pEntry);
+
+CHAR *GetEncryptType(
+	CHAR enc);
+
+CHAR *GetAuthMode(
+	CHAR auth);
+
+VOID RTMPIoctlGetSiteSurvey(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq);
+
+VOID RTMPIoctlGetMacTable(
+	IN PRTMP_ADAPTER pAd,
+	IN struct iwreq *wrq);
+
+VOID RTMPIndicateWPA2Status(
+	IN  PRTMP_ADAPTER  pAdapter);
+
+VOID	RTMPOPModeSwitching(
+	IN	PRTMP_ADAPTER	pAd);
+
+#ifdef CONFIG_STA_SUPPORT
+VOID    RTMPAddBSSIDCipher(
+    IN  PRTMP_ADAPTER   pAd,
+	IN	UCHAR	Aid,
+    IN  PNDIS_802_11_KEY    pKey,
+    IN  UCHAR   CipherAlg);
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef DOT11_N_SUPPORT
+VOID	RTMPSetHT(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	OID_SET_HT_PHYMODE *pHTPhyMode);
+
+VOID	RTMPSetIndividualHT(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	UCHAR				apidx);
+#endif // DOT11_N_SUPPORT //
+
+VOID RTMPSendWirelessEvent(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Event_flag,
+	IN	PUCHAR 			pAddr,
+	IN  UCHAR			BssIdx,
+	IN	CHAR			Rssi);
+
+VOID	NICUpdateCntlCounters(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PHEADER_802_11	pHeader,
+	IN    UCHAR			SubType,
+	IN	PRXWI_STRUC 	pRxWI);
+//
+// prototype in wpa.c
+//
+BOOLEAN WpaMsgTypeSubst(
+	IN  UCHAR   EAPType,
+	OUT INT		*MsgType);
+
+VOID WpaPskStateMachineInit(
+	IN  PRTMP_ADAPTER       pAd,
+	IN  STATE_MACHINE       *S,
+	OUT STATE_MACHINE_FUNC Trans[]);
+
+VOID WpaEAPOLKeyAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID    WpaPairMsg1Action(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID    WpaPairMsg3Action(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID    WpaGroupMsg1Action(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID    WpaMacHeaderInit(
+	IN      PRTMP_ADAPTER   pAd,
+	IN OUT  PHEADER_802_11  pHdr80211,
+	IN      UCHAR           wep,
+	IN      PUCHAR          pAddr1);
+
+VOID    Wpa2PairMsg1Action(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  MLME_QUEUE_ELEM *Elem);
+
+VOID    Wpa2PairMsg3Action(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  MLME_QUEUE_ELEM *Elem);
+
+BOOLEAN ParseKeyData(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          pKeyData,
+    IN  UCHAR           KeyDataLen,
+	IN	UCHAR			bPairewise);
+
+VOID    RTMPToWirelessSta(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pHeader802_3,
+    IN  UINT            HdrLen,
+	IN  PUCHAR          pData,
+    IN  UINT            DataLen,
+    IN	BOOLEAN			is4wayFrame);
+
+VOID    HMAC_SHA1(
+	IN  UCHAR   *text,
+	IN  UINT    text_len,
+	IN  UCHAR   *key,
+	IN  UINT    key_len,
+	IN  UCHAR   *digest);
+
+VOID    PRF(
+	IN  UCHAR   *key,
+	IN  INT     key_len,
+	IN  UCHAR   *prefix,
+	IN  INT     prefix_len,
+	IN  UCHAR   *data,
+	IN  INT     data_len,
+	OUT UCHAR   *output,
+	IN  INT     len);
+
+VOID    CCKMPRF(
+	IN  UCHAR   *key,
+	IN  INT     key_len,
+	IN  UCHAR   *data,
+	IN  INT     data_len,
+	OUT UCHAR   *output,
+	IN  INT     len);
+
+VOID WpaCountPTK(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR   *PMK,
+	IN  UCHAR   *ANonce,
+	IN  UCHAR   *AA,
+	IN  UCHAR   *SNonce,
+	IN  UCHAR   *SA,
+	OUT UCHAR   *output,
+	IN  UINT    len);
+
+VOID    GenRandom(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	UCHAR			*macAddr,
+	OUT	UCHAR			*random);
+
+//
+// prototype in aironet.c
+//
+VOID    AironetStateMachineInit(
+	IN  PRTMP_ADAPTER       pAd,
+	IN  STATE_MACHINE       *S,
+	OUT STATE_MACHINE_FUNC  Trans[]);
+
+VOID    AironetMsgAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID    AironetRequestAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID    ChannelLoadRequestAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           Index);
+
+VOID    NoiseHistRequestAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           Index);
+
+VOID    BeaconRequestAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           Index);
+
+VOID    AironetReportAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem);
+
+VOID    ChannelLoadReportAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           Index);
+
+VOID    NoiseHistReportAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           Index);
+
+VOID    AironetFinalReportAction(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID    BeaconReportAction(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UCHAR           Index);
+
+VOID    AironetAddBeaconReport(
+	IN  PRTMP_ADAPTER       pAd,
+	IN  ULONG               Index,
+	IN  PMLME_QUEUE_ELEM    pElem);
+
+VOID    AironetCreateBeaconReportFromBssTable(
+	IN  PRTMP_ADAPTER       pAd);
+
+VOID    DBGPRINT_TX_RING(
+	IN PRTMP_ADAPTER  pAd,
+	IN UCHAR          QueIdx);
+
+VOID DBGPRINT_RX_RING(
+	IN PRTMP_ADAPTER  pAd);
+
+CHAR    ConvertToRssi(
+	IN PRTMP_ADAPTER  pAd,
+	IN CHAR				Rssi,
+	IN UCHAR    RssiNumber);
+
+
+#ifdef DOT11N_DRAFT3
+VOID BuildEffectedChannelList(
+	IN PRTMP_ADAPTER pAd);
+#endif // DOT11N_DRAFT3 //
+
+
+VOID APAsicEvaluateRxAnt(
+	IN PRTMP_ADAPTER	pAd);
+
+
+VOID APAsicRxAntEvalTimeout(
+	IN PRTMP_ADAPTER	pAd);
+
+//
+// function prototype in cmm_wpa.c
+//
+BOOLEAN RTMPCheckWPAframe(
+	IN PRTMP_ADAPTER pAd,
+	IN PMAC_TABLE_ENTRY	pEntry,
+	IN PUCHAR 			pData,
+	IN ULONG 			DataByteCount,
+	IN UCHAR			FromWhichBSSID);
+
+VOID AES_GTK_KEY_UNWRAP(
+	IN  UCHAR   *key,
+	OUT UCHAR   *plaintext,
+	IN	UCHAR	c_len,
+	IN  UCHAR   *ciphertext);
+
+BOOLEAN RTMPCheckRSNIE(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pData,
+	IN  UCHAR           DataLen,
+	IN  MAC_TABLE_ENTRY *pEntry,
+	OUT	UCHAR			*Offset);
+
+BOOLEAN RTMPParseEapolKeyData(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pKeyData,
+	IN  UCHAR           KeyDataLen,
+	IN	UCHAR			GroupKeyIndex,
+	IN	UCHAR			MsgType,
+	IN	BOOLEAN			bWPA2,
+	IN  MAC_TABLE_ENTRY *pEntry);
+
+VOID	ConstructEapolMsg(
+	IN 	PRTMP_ADAPTER    	pAd,
+    IN 	UCHAR				PeerAuthMode,
+    IN 	UCHAR				PeerWepStatus,
+    IN 	UCHAR				MyGroupKeyWepStatus,
+    IN 	UCHAR				MsgType,
+    IN	UCHAR				DefaultKeyIdx,
+    IN 	UCHAR				*ReplayCounter,
+	IN 	UCHAR				*KeyNonce,
+	IN	UCHAR				*TxRSC,
+	IN	UCHAR				*PTK,
+	IN	UCHAR				*GTK,
+	IN	UCHAR				*RSNIE,
+	IN	UCHAR				RSNIE_Len,
+    OUT PEAPOL_PACKET       pMsg);
+
+VOID	CalculateMIC(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			PeerWepStatus,
+	IN	UCHAR			*PTK,
+	OUT PEAPOL_PACKET   pMsg);
+
+NDIS_STATUS	RTMPSoftDecryptBroadCastData(
+	IN	PRTMP_ADAPTER					pAd,
+	IN	RX_BLK							*pRxBlk,
+	IN  NDIS_802_11_ENCRYPTION_STATUS 	GroupCipher,
+	IN  PCIPHER_KEY						pShard_key);
+
+VOID	ConstructEapolKeyData(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			PeerAuthMode,
+	IN	UCHAR			PeerWepStatus,
+	IN	UCHAR			GroupKeyWepStatus,
+	IN 	UCHAR			MsgType,
+	IN	UCHAR			DefaultKeyIdx,
+	IN	BOOLEAN			bWPA2Capable,
+	IN	UCHAR			*PTK,
+	IN	UCHAR			*GTK,
+	IN	UCHAR			*RSNIE,
+	IN	UCHAR			RSNIE_LEN,
+	OUT PEAPOL_PACKET   pMsg);
+
+VOID RTMPMakeRSNIE(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  UINT            AuthMode,
+	IN  UINT            WepStatus,
+	IN	UCHAR			apidx);
+
+//
+// function prototype in ap_wpa.c
+//
+
+BOOLEAN APWpaMsgTypeSubst(
+	IN UCHAR    EAPType,
+	OUT INT *MsgType) ;
+
+MAC_TABLE_ENTRY *PACInquiry(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  ULONG           Wcid);
+
+BOOLEAN RTMPCheckMcast(
+	IN PRTMP_ADAPTER pAd,
+	IN PEID_STRUCT      eid_ptr,
+	IN MAC_TABLE_ENTRY  *pEntry);
+
+BOOLEAN RTMPCheckUcast(
+	IN PRTMP_ADAPTER pAd,
+	IN PEID_STRUCT      eid_ptr,
+	IN MAC_TABLE_ENTRY  *pEntry);
+
+BOOLEAN RTMPCheckAUTH(
+	IN PRTMP_ADAPTER pAd,
+	IN PEID_STRUCT      eid_ptr,
+	IN MAC_TABLE_ENTRY  *pEntry);
+
+VOID WPAStart4WayHS(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MAC_TABLE_ENTRY *pEntry,
+	IN	ULONG			TimeInterval);
+
+VOID WPAStart2WayGroupHS(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MAC_TABLE_ENTRY *pEntry);
+
+VOID APWpaEAPPacketAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID APWpaEAPOLStartAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID APWpaEAPOLLogoffAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID APWpaEAPOLKeyAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID APWpaEAPOLASFAlertAction(
+	IN  PRTMP_ADAPTER    pAd,
+	IN  MLME_QUEUE_ELEM  *Elem);
+
+VOID HandleCounterMeasure(
+	IN PRTMP_ADAPTER pAd,
+	IN MAC_TABLE_ENTRY  *pEntry);
+
+VOID PeerPairMsg2Action(
+	IN PRTMP_ADAPTER pAd,
+	IN MAC_TABLE_ENTRY  *pEntry,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID PeerPairMsg4Action(
+	IN PRTMP_ADAPTER pAd,
+	IN MAC_TABLE_ENTRY  *pEntry,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID CMTimerExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID WPARetryExec(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3);
+
+VOID EnqueueStartForPSKExec(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3);
+
+VOID RTMPHandleSTAKey(
+    IN PRTMP_ADAPTER    pAdapter,
+    IN MAC_TABLE_ENTRY  *pEntry,
+    IN MLME_QUEUE_ELEM  *Elem);
+
+#if 0 // merge into PeerPairMsg4Action
+VOID Wpa1PeerPairMsg4Action(
+	IN PRTMP_ADAPTER pAd,
+	IN MAC_TABLE_ENTRY  *pEntry,
+	IN MLME_QUEUE_ELEM *Elem);
+
+VOID Wpa2PeerPairMsg4Action(
+	IN  PRTMP_ADAPTER    pAd,
+	IN  PMAC_TABLE_ENTRY pEntry,
+	IN  MLME_QUEUE_ELEM  *Elem);
+#endif // 0 //
+
+VOID PeerGroupMsg2Action(
+	IN  PRTMP_ADAPTER    pAd,
+	IN  PMAC_TABLE_ENTRY pEntry,
+	IN  VOID             *Msg,
+	IN  UINT             MsgLen);
+
+#if 0	// replaced by WPAStart2WayGroupHS
+NDIS_STATUS APWpaHardTransmit(
+	IN  PRTMP_ADAPTER    pAd,
+	IN  PMAC_TABLE_ENTRY pEntry);
+#endif // 0 //
+
+VOID PairDisAssocAction(
+	IN  PRTMP_ADAPTER    pAd,
+	IN  PMAC_TABLE_ENTRY pEntry,
+	IN  USHORT           Reason);
+
+VOID MlmeDeAuthAction(
+	IN  PRTMP_ADAPTER    pAd,
+	IN  PMAC_TABLE_ENTRY pEntry,
+	IN  USHORT           Reason);
+
+VOID GREKEYPeriodicExec(
+	IN  PVOID   SystemSpecific1,
+	IN  PVOID   FunctionContext,
+	IN  PVOID   SystemSpecific2,
+	IN  PVOID   SystemSpecific3);
+
+VOID CountGTK(
+	IN  UCHAR   *PMK,
+	IN  UCHAR   *GNonce,
+	IN  UCHAR   *AA,
+	OUT UCHAR   *output,
+	IN  UINT    len);
+
+VOID    GetSmall(
+	IN  PVOID   pSrc1,
+	IN  PVOID   pSrc2,
+	OUT PUCHAR  out,
+	IN  ULONG   Length);
+
+VOID    GetLarge(
+	IN  PVOID   pSrc1,
+	IN  PVOID   pSrc2,
+	OUT PUCHAR  out,
+	IN  ULONG   Length);
+
+VOID APGenRandom(
+	IN PRTMP_ADAPTER pAd,
+	OUT UCHAR       *random);
+
+VOID AES_GTK_KEY_WRAP(
+	IN UCHAR *key,
+	IN UCHAR *plaintext,
+	IN UCHAR p_len,
+	OUT UCHAR *ciphertext);
+
+VOID    WpaSend(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          pPacket,
+    IN  ULONG           Len);
+
+VOID    APToWirelessSta(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MAC_TABLE_ENTRY *pEntry,
+	IN  PUCHAR          pHeader802_3,
+	IN  UINT            HdrLen,
+	IN  PUCHAR          pData,
+	IN  UINT            DataLen,
+    IN	BOOLEAN			bClearFrame);
+
+VOID RTMPAddPMKIDCache(
+	IN  PRTMP_ADAPTER   		pAd,
+	IN	INT						apidx,
+	IN	PUCHAR				pAddr,
+	IN	UCHAR					*PMKID,
+	IN	UCHAR					*PMK);
+
+INT RTMPSearchPMKIDCache(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	INT				apidx,
+	IN	PUCHAR		pAddr);
+
+VOID RTMPDeletePMKIDCache(
+	IN  PRTMP_ADAPTER   pAd,
+	IN	INT				apidx,
+	IN  INT				idx);
+
+VOID RTMPMaintainPMKIDCache(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID	RTMPSendTriggerFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PVOID			pBuffer,
+	IN	ULONG			Length,
+	IN  UCHAR           TxRate,
+	IN	BOOLEAN			bQosNull);
+
+
+//typedef void (*TIMER_FUNCTION)(unsigned long);
+
+
+/* timeout -- ms */
+VOID RTMP_SetPeriodicTimer(
+	IN	NDIS_MINIPORT_TIMER *pTimer,
+	IN	unsigned long timeout);
+
+VOID RTMP_OS_Init_Timer(
+	IN	PRTMP_ADAPTER pAd,
+	IN	NDIS_MINIPORT_TIMER *pTimer,
+	IN	TIMER_FUNCTION function,
+	IN	PVOID data);
+
+VOID RTMP_OS_Add_Timer(
+	IN	NDIS_MINIPORT_TIMER	*pTimer,
+	IN	unsigned long timeout);
+
+VOID RTMP_OS_Mod_Timer(
+	IN	NDIS_MINIPORT_TIMER	*pTimer,
+	IN	unsigned long timeout);
+
+
+VOID RTMP_OS_Del_Timer(
+	IN	NDIS_MINIPORT_TIMER	*pTimer,
+	OUT	BOOLEAN				 *pCancelled);
+
+
+VOID RTMP_OS_Release_Packet(
+	IN	PRTMP_ADAPTER pAd,
+	IN	PQUEUE_ENTRY  pEntry);
+
+VOID RTMPusecDelay(
+	IN	ULONG	usec);
+
+NDIS_STATUS os_alloc_mem(
+	IN	PRTMP_ADAPTER pAd,
+	OUT	PUCHAR *mem,
+	IN	ULONG  size);
+
+NDIS_STATUS os_free_mem(
+	IN	PRTMP_ADAPTER pAd,
+	IN	PUCHAR mem);
+
+
+void RTMP_AllocateSharedMemory(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress,
+	OUT	PNDIS_PHYSICAL_ADDRESS PhysicalAddress);
+
+VOID RTMPFreeTxRxRingMemory(
+    IN  PRTMP_ADAPTER   pAd);
+
+NDIS_STATUS AdapterBlockAllocateMemory(
+	IN PVOID	handle,
+	OUT	PVOID	*ppAd);
+
+void RTMP_AllocateTxDescMemory(
+	IN	PRTMP_ADAPTER pAd,
+	IN	UINT	Index,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress,
+	OUT	PNDIS_PHYSICAL_ADDRESS PhysicalAddress);
+
+void RTMP_AllocateFirstTxBuffer(
+	IN	PRTMP_ADAPTER pAd,
+	IN	UINT	Index,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress,
+	OUT	PNDIS_PHYSICAL_ADDRESS PhysicalAddress);
+
+void RTMP_AllocateMgmtDescMemory(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress,
+	OUT	PNDIS_PHYSICAL_ADDRESS PhysicalAddress);
+
+void RTMP_AllocateRxDescMemory(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress,
+	OUT	PNDIS_PHYSICAL_ADDRESS PhysicalAddress);
+
+PNDIS_PACKET RTMP_AllocateRxPacketBuffer(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress,
+	OUT	PNDIS_PHYSICAL_ADDRESS PhysicalAddress);
+
+PNDIS_PACKET RTMP_AllocateTxPacketBuffer(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length,
+	IN	BOOLEAN	Cached,
+	OUT	PVOID	*VirtualAddress);
+
+PNDIS_PACKET RTMP_AllocateFragPacketBuffer(
+	IN	PRTMP_ADAPTER pAd,
+	IN	ULONG	Length);
+
+void RTMP_QueryPacketInfo(
+	IN  PNDIS_PACKET pPacket,
+	OUT PACKET_INFO  *pPacketInfo,
+	OUT PUCHAR		 *pSrcBufVA,
+	OUT	UINT		 *pSrcBufLen);
+
+void RTMP_QueryNextPacketInfo(
+	IN  PNDIS_PACKET *ppPacket,
+	OUT PACKET_INFO  *pPacketInfo,
+	OUT PUCHAR		 *pSrcBufVA,
+	OUT	UINT		 *pSrcBufLen);
+
+
+BOOLEAN RTMP_FillTxBlkInfo(
+	IN RTMP_ADAPTER *pAd,
+	IN TX_BLK *pTxBlk);
+
+
+PRTMP_SCATTER_GATHER_LIST
+rt_get_sg_list_from_packet(PNDIS_PACKET pPacket, RTMP_SCATTER_GATHER_LIST *sg);
+
+
+ void announce_802_3_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket);
+
+
+UINT BA_Reorder_AMSDU_Annnounce(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket);
+
+
+UINT Handle_AMSDU_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize,
+	IN  UCHAR			FromWhichBSSID);
+
+
+void convert_802_11_to_802_3_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	PUCHAR			p8023hdr,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize,
+	IN  UCHAR			FromWhichBSSID);
+
+
+PNET_DEV get_netdev_from_bssid(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			FromWhichBSSID);
+
+
+PNDIS_PACKET duplicate_pkt(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pHeader802_3,
+    IN  UINT            HdrLen,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize,
+	IN	UCHAR			FromWhichBSSID);
+
+
+PNDIS_PACKET duplicate_pkt_with_TKIP_MIC(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pOldPkt);
+
+PNDIS_PACKET duplicate_pkt_with_VLAN(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pHeader802_3,
+    IN  UINT            HdrLen,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize,
+	IN	UCHAR			FromWhichBSSID);
+
+PNDIS_PACKET duplicate_pkt_with_WPI(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	UINT32			ext_head_len,
+	IN	UINT32			ext_tail_len);
+
+UCHAR VLAN_8023_Header_Copy(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pHeader802_3,
+	IN	UINT            HdrLen,
+	OUT PUCHAR			pData,
+	IN	UCHAR			FromWhichBSSID);
+
+#ifdef DOT11_N_SUPPORT
+void ba_flush_reordering_timeout_mpdus(
+	IN PRTMP_ADAPTER	pAd,
+	IN PBA_REC_ENTRY	pBAEntry,
+	IN ULONG			Now32);
+
+
+VOID BAOriSessionSetUp(
+			IN PRTMP_ADAPTER    pAd,
+			IN MAC_TABLE_ENTRY	*pEntry,
+			IN UCHAR			TID,
+			IN USHORT			TimeOut,
+			IN ULONG			DelayTime,
+			IN BOOLEAN		isForced);
+
+VOID BASessionTearDownALL(
+	IN OUT	PRTMP_ADAPTER pAd,
+	IN		UCHAR Wcid);
+#endif // DOT11_N_SUPPORT //
+
+BOOLEAN OS_Need_Clone_Packet(void);
+
+
+VOID build_tx_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	PUCHAR	pFrame,
+	IN	ULONG	FrameLen);
+
+
+VOID BAOriSessionTearDown(
+	IN OUT	PRTMP_ADAPTER	pAd,
+	IN		UCHAR			Wcid,
+	IN		UCHAR			TID,
+	IN		BOOLEAN			bPassive,
+	IN		BOOLEAN			bForceSend);
+
+VOID BARecSessionTearDown(
+	IN OUT	PRTMP_ADAPTER	pAd,
+	IN		UCHAR			Wcid,
+	IN		UCHAR			TID,
+	IN		BOOLEAN			bPassive);
+
+BOOLEAN ba_reordering_resource_init(PRTMP_ADAPTER pAd, int num);
+void ba_reordering_resource_release(PRTMP_ADAPTER pAd);
+
+ULONG AutoChBssInsertEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pBssid,
+	IN CHAR Ssid[],
+	IN UCHAR SsidLen,
+	IN UCHAR ChannelNo,
+	IN CHAR Rssi);
+
+void AutoChBssTableInit(
+	IN PRTMP_ADAPTER pAd);
+
+void ChannelInfoInit(
+	IN PRTMP_ADAPTER pAd);
+
+void AutoChBssTableDestroy(
+	IN PRTMP_ADAPTER pAd);
+
+void ChannelInfoDestroy(
+	IN PRTMP_ADAPTER pAd);
+
+UCHAR New_ApAutoSelectChannel(
+	IN PRTMP_ADAPTER pAd);
+
+BOOLEAN rtstrmactohex(
+	IN char *s1,
+	IN char *s2);
+
+BOOLEAN rtstrcasecmp(
+	IN char *s1,
+	IN char *s2);
+
+char *rtstrstruncasecmp(
+	IN char *s1,
+	IN char *s2);
+
+char    *rtstrstr(
+	IN	const char * s1,
+	IN	const char * s2);
+
+char *rstrtok(
+	IN char * s,
+	IN const char * ct);
+
+int rtinet_aton(
+	const char *cp,
+	unsigned int *addr);
+
+////////// common ioctl functions //////////
+INT Set_DriverVersion_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_CountryRegion_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_CountryRegionABand_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_WirelessMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_Channel_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ShortSlot_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_TxPower_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_BGProtection_Proc(
+	IN  PRTMP_ADAPTER		pAd,
+	IN  PUCHAR			arg);
+
+INT Set_TxPreamble_Proc(
+	IN  PRTMP_ADAPTER		pAd,
+	IN  PUCHAR			arg);
+
+INT Set_RTSThreshold_Proc(
+	IN  PRTMP_ADAPTER		pAd,
+	IN  PUCHAR			arg);
+
+INT Set_FragThreshold_Proc(
+	IN  PRTMP_ADAPTER		pAd,
+	IN  PUCHAR			arg);
+
+INT Set_TxBurst_Proc(
+	IN  PRTMP_ADAPTER		pAd,
+	IN  PUCHAR			arg);
+
+#ifdef AGGREGATION_SUPPORT
+INT	Set_PktAggregate_Proc(
+	IN  PRTMP_ADAPTER		pAd,
+	IN  PUCHAR			arg);
+#endif
+
+INT	Set_IEEE80211H_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+#ifdef DBG
+INT	Set_Debug_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif
+
+INT	Show_DescInfo_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ResetStatCounter_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+#ifdef DOT11_N_SUPPORT
+INT	Set_BASetup_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_BADecline_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_BAOriTearDown_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_BARecTearDown_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtBw_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtMcs_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtGi_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtOpMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtStbc_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtHtc_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtExtcha_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtMpduDensity_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtBaWinSize_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtRdg_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtLinkAdapt_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtAmsdu_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtAutoBa_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtProtect_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtMimoPs_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+
+INT	Set_ForceShortGI_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_ForceGF_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	SetCommonHT(
+	IN	PRTMP_ADAPTER	pAd);
+
+INT	Set_SendPSMPAction_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT	Set_HtMIMOPSmode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+
+INT	Set_HtTxBASize_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif // DOT11_N_SUPPORT //
+
+
+
+#ifdef CONFIG_STA_SUPPORT
+//Dls ,	kathy
+VOID RTMPSendDLSTearDownFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pDA);
+
+#ifdef DOT11_N_SUPPORT
+//Block ACK
+VOID QueryBATABLE(
+	IN  PRTMP_ADAPTER pAd,
+	OUT PQUERYBA_TABLE pBAT);
+#endif // DOT11_N_SUPPORT //
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+INT	    WpaCheckEapCode(
+	IN  PRTMP_ADAPTER   	pAd,
+	IN  PUCHAR				pFrame,
+	IN  USHORT				FrameLen,
+	IN  USHORT				OffSet);
+
+VOID    WpaSendMicFailureToWpaSupplicant(
+    IN  PRTMP_ADAPTER       pAd,
+    IN  BOOLEAN             bUnicast);
+
+VOID    SendAssocIEsToWpaSupplicant(
+    IN  PRTMP_ADAPTER       pAd);
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+int wext_notify_event_assoc(
+	IN  RTMP_ADAPTER *pAd);
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+
+#endif // CONFIG_STA_SUPPORT //
+
+
+
+#ifdef DOT11_N_SUPPORT
+VOID Handle_BSS_Width_Trigger_Events(
+	IN PRTMP_ADAPTER pAd);
+
+void build_ext_channel_switch_ie(
+	IN PRTMP_ADAPTER pAd,
+	IN HT_EXT_CHANNEL_SWITCH_ANNOUNCEMENT_IE *pIE);
+#endif // DOT11_N_SUPPORT //
+
+
+BOOLEAN APRxDoneInterruptHandle(
+	IN	PRTMP_ADAPTER	pAd);
+
+BOOLEAN STARxDoneInterruptHandle(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	BOOLEAN			argc);
+
+#ifdef DOT11_N_SUPPORT
+// AMPDU packet indication
+VOID Indicate_AMPDU_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID);
+
+// AMSDU packet indication
+VOID Indicate_AMSDU_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID);
+#endif // DOT11_N_SUPPORT //
+
+// Normal legacy Rx packet indication
+VOID Indicate_Legacy_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID);
+
+VOID Indicate_EAPOL_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID);
+
+void  update_os_packet_info(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID);
+
+void wlan_802_11_to_802_3_packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	PUCHAR			pHeader802_3,
+	IN  UCHAR			FromWhichBSSID);
+
+UINT deaggregate_AMSDU_announce(
+	IN	PRTMP_ADAPTER	pAd,
+	PNDIS_PACKET		pPacket,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize);
+
+
+#ifdef CONFIG_STA_SUPPORT
+// remove LLC and get 802_3 Header
+#define  RTMP_802_11_REMOVE_LLC_AND_CONVERT_TO_802_3(_pRxBlk, _pHeader802_3)	\
+{																				\
+	PUCHAR _pRemovedLLCSNAP = NULL, _pDA, _pSA;                                 \
+																				\
+	if (RX_BLK_TEST_FLAG(_pRxBlk, fRX_MESH))                                    \
+	{                                                                           \
+		_pDA = _pRxBlk->pHeader->Addr3;                                         \
+		_pSA = (PUCHAR)_pRxBlk->pHeader + sizeof(HEADER_802_11);                \
+	}                                                                           \
+	else                                                                        \
+	{                                                                           \
+		if (RX_BLK_TEST_FLAG(_pRxBlk, fRX_INFRA))                              	\
+		{                                                                       \
+			_pDA = _pRxBlk->pHeader->Addr1;                                     \
+		if (RX_BLK_TEST_FLAG(_pRxBlk, fRX_DLS))									\
+			_pSA = _pRxBlk->pHeader->Addr2;										\
+		else																	\
+			_pSA = _pRxBlk->pHeader->Addr3;                                     \
+		}                                                                       \
+		else                                                                    \
+		{                                                                       \
+			_pDA = _pRxBlk->pHeader->Addr1;                                     \
+			_pSA = _pRxBlk->pHeader->Addr2;                                     \
+		}                                                                       \
+	}                                                                           \
+																				\
+	CONVERT_TO_802_3(_pHeader802_3, _pDA, _pSA, _pRxBlk->pData, 				\
+		_pRxBlk->DataSize, _pRemovedLLCSNAP);                                   \
+}
+#endif // CONFIG_STA_SUPPORT //
+
+
+BOOLEAN APFowardWirelessStaToWirelessSta(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	ULONG			FromWhichBSSID);
+
+VOID Announce_or_Forward_802_3_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	UCHAR			FromWhichBSSID);
+
+VOID Sta_Announce_or_Forward_802_3_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	UCHAR			FromWhichBSSID);
+
+
+#ifdef CONFIG_STA_SUPPORT
+#define ANNOUNCE_OR_FORWARD_802_3_PACKET(_pAd, _pPacket, _FromWhichBSS)\
+			Sta_Announce_or_Forward_802_3_Packet(_pAd, _pPacket, _FromWhichBSS);
+			//announce_802_3_packet(_pAd, _pPacket);
+#endif // CONFIG_STA_SUPPORT //
+
+
+PNDIS_PACKET DuplicatePacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	UCHAR			FromWhichBSSID);
+
+
+PNDIS_PACKET ClonePacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize);
+
+
+// Normal, AMPDU or AMSDU
+VOID CmmRxnonRalinkFrameIndicate(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID);
+
+VOID CmmRxRalinkFrameIndicate(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID);
+
+VOID Update_Rssi_Sample(
+	IN PRTMP_ADAPTER	pAd,
+	IN RSSI_SAMPLE		*pRssi,
+	IN PRXWI_STRUC		pRxWI);
+
+PNDIS_PACKET GetPacketFromRxRing(
+	IN		PRTMP_ADAPTER	pAd,
+	OUT		PRT28XX_RXD_STRUC		pSaveRxD,
+	OUT		BOOLEAN			*pbReschedule,
+	IN OUT	UINT32			*pRxPending);
+
+PNDIS_PACKET RTMPDeFragmentDataFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk);
+
+////////////////////////////////////////
+
+
+
+
+
+#ifdef SNMP_SUPPORT
+//for snmp , kathy
+typedef struct _DefaultKeyIdxValue
+{
+	UCHAR	KeyIdx;
+	UCHAR	Value[16];
+} DefaultKeyIdxValue, *PDefaultKeyIdxValue;
+#endif
+
+
+#ifdef CONFIG_STA_SUPPORT
+enum {
+	DIDmsg_lnxind_wlansniffrm		= 0x00000044,
+	DIDmsg_lnxind_wlansniffrm_hosttime	= 0x00010044,
+	DIDmsg_lnxind_wlansniffrm_mactime	= 0x00020044,
+	DIDmsg_lnxind_wlansniffrm_channel	= 0x00030044,
+	DIDmsg_lnxind_wlansniffrm_rssi		= 0x00040044,
+	DIDmsg_lnxind_wlansniffrm_sq		= 0x00050044,
+	DIDmsg_lnxind_wlansniffrm_signal	= 0x00060044,
+	DIDmsg_lnxind_wlansniffrm_noise		= 0x00070044,
+	DIDmsg_lnxind_wlansniffrm_rate		= 0x00080044,
+	DIDmsg_lnxind_wlansniffrm_istx		= 0x00090044,
+	DIDmsg_lnxind_wlansniffrm_frmlen	= 0x000A0044
+};
+enum {
+	P80211ENUM_msgitem_status_no_value	= 0x00
+};
+enum {
+	P80211ENUM_truth_false			= 0x00,
+	P80211ENUM_truth_true			= 0x01
+};
+
+/* Definition from madwifi */
+typedef struct {
+        UINT32 did;
+        UINT16 status;
+        UINT16 len;
+        UINT32 data;
+} p80211item_uint32_t;
+
+typedef struct {
+        UINT32 msgcode;
+        UINT32 msglen;
+#define WLAN_DEVNAMELEN_MAX 16
+        UINT8 devname[WLAN_DEVNAMELEN_MAX];
+        p80211item_uint32_t hosttime;
+        p80211item_uint32_t mactime;
+        p80211item_uint32_t channel;
+        p80211item_uint32_t rssi;
+        p80211item_uint32_t sq;
+        p80211item_uint32_t signal;
+        p80211item_uint32_t noise;
+        p80211item_uint32_t rate;
+        p80211item_uint32_t istx;
+        p80211item_uint32_t frmlen;
+} wlan_ng_prism2_header;
+
+/* The radio capture header precedes the 802.11 header. */
+typedef struct PACKED _ieee80211_radiotap_header {
+    UINT8	it_version;	/* Version 0. Only increases
+				 * for drastic changes,
+				 * introduction of compatible
+				 * new fields does not count.
+				 */
+    UINT8	it_pad;
+    UINT16     it_len;         /* length of the whole
+				 * header in bytes, including
+				 * it_version, it_pad,
+				 * it_len, and data fields.
+				 */
+    UINT32   it_present;	/* A bitmap telling which
+					 * fields are present. Set bit 31
+					 * (0x80000000) to extend the
+					 * bitmap by another 32 bits.
+					 * Additional extensions are made
+					 * by setting bit 31.
+					 */
+}ieee80211_radiotap_header ;
+
+enum ieee80211_radiotap_type {
+    IEEE80211_RADIOTAP_TSFT = 0,
+    IEEE80211_RADIOTAP_FLAGS = 1,
+    IEEE80211_RADIOTAP_RATE = 2,
+    IEEE80211_RADIOTAP_CHANNEL = 3,
+    IEEE80211_RADIOTAP_FHSS = 4,
+    IEEE80211_RADIOTAP_DBM_ANTSIGNAL = 5,
+    IEEE80211_RADIOTAP_DBM_ANTNOISE = 6,
+    IEEE80211_RADIOTAP_LOCK_QUALITY = 7,
+    IEEE80211_RADIOTAP_TX_ATTENUATION = 8,
+    IEEE80211_RADIOTAP_DB_TX_ATTENUATION = 9,
+    IEEE80211_RADIOTAP_DBM_TX_POWER = 10,
+    IEEE80211_RADIOTAP_ANTENNA = 11,
+    IEEE80211_RADIOTAP_DB_ANTSIGNAL = 12,
+    IEEE80211_RADIOTAP_DB_ANTNOISE = 13
+};
+
+#define WLAN_RADIOTAP_PRESENT (			\
+	(1 << IEEE80211_RADIOTAP_TSFT)	|	\
+	(1 << IEEE80211_RADIOTAP_FLAGS) |	\
+	(1 << IEEE80211_RADIOTAP_RATE)  | 	\
+	 0)
+
+typedef struct _wlan_radiotap_header {
+	ieee80211_radiotap_header wt_ihdr;
+	INT64 wt_tsft;
+	UINT8 wt_flags;
+	UINT8 wt_rate;
+} wlan_radiotap_header;
+/* Definition from madwifi */
+
+void send_monitor_packets(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk);
+
+#if WIRELESS_EXT >= 12
+// This function will be called when query /proc
+struct iw_statistics *rt28xx_get_wireless_stats(
+    IN struct net_device *net_dev);
+#endif
+
+VOID    RTMPSetDesiredRates(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  LONG            Rates);
+#endif // CONFIG_STA_SUPPORT //
+
+INT	Set_FixedTxMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+INT	Set_OpMode_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+
+static inline char* GetPhyMode(
+	int Mode)
+{
+	switch(Mode)
+	{
+		case MODE_CCK:
+			return "CCK";
+
+		case MODE_OFDM:
+			return "OFDM";
+#ifdef DOT11_N_SUPPORT
+		case MODE_HTMIX:
+			return "HTMIX";
+
+		case MODE_HTGREENFIELD:
+			return "GREEN";
+#endif // DOT11_N_SUPPORT //
+		default:
+			return "N/A";
+	}
+}
+
+
+static inline char* GetBW(
+	int BW)
+{
+	switch(BW)
+	{
+		case BW_10:
+			return "10M";
+
+		case BW_20:
+			return "20M";
+#ifdef DOT11_N_SUPPORT
+		case BW_40:
+			return "40M";
+#endif // DOT11_N_SUPPORT //
+		default:
+			return "N/A";
+	}
+}
+
+
+VOID RT28xxThreadTerminate(
+	IN RTMP_ADAPTER *pAd);
+
+BOOLEAN RT28XXChipsetCheck(
+	IN void *_dev_p);
+
+BOOLEAN RT28XXNetDevInit(
+	IN void 				*_dev_p,
+	IN struct  net_device	*net_dev,
+	IN RTMP_ADAPTER 		*pAd);
+
+BOOLEAN RT28XXProbePostConfig(
+	IN void 				*_dev_p,
+	IN RTMP_ADAPTER 		*pAd,
+	IN INT32				argc);
+
+VOID RT28XXDMADisable(
+	IN RTMP_ADAPTER 		*pAd);
+
+VOID RT28XXDMAEnable(
+	IN RTMP_ADAPTER 		*pAd);
+
+VOID RT28xx_UpdateBeaconToAsic(
+	IN RTMP_ADAPTER * pAd,
+	IN INT apidx,
+	IN ULONG BeaconLen,
+	IN ULONG UpdatePos);
+
+INT rt28xx_ioctl(
+	IN	struct net_device	*net_dev,
+	IN	OUT	struct ifreq	*rq,
+	IN	INT			cmd);
+
+
+#ifdef CONFIG_STA_SUPPORT
+INT rt28xx_sta_ioctl(
+	IN	struct net_device	*net_dev,
+	IN	OUT	struct ifreq	*rq,
+	IN	INT			cmd);
+#endif // CONFIG_STA_SUPPORT //
+
+BOOLEAN RT28XXSecurityKeyAdd(
+	IN		PRTMP_ADAPTER		pAd,
+	IN		ULONG				apidx,
+	IN		ULONG				KeyIdx,
+	IN		MAC_TABLE_ENTRY 	*pEntry);
+
+////////////////////////////////////////
+PNDIS_PACKET GetPacketFromRxRing(
+	IN		PRTMP_ADAPTER	pAd,
+	OUT		PRT28XX_RXD_STRUC	pSaveRxD,
+	OUT		BOOLEAN			*pbReschedule,
+	IN OUT	UINT32			*pRxPending);
+
+
+void kill_thread_task(PRTMP_ADAPTER pAd);
+
+void tbtt_tasklet(unsigned long data);
+
+
+VOID AsicTurnOffRFClk(
+	IN PRTMP_ADAPTER    pAd,
+	IN	UCHAR           Channel);
+
+VOID AsicTurnOnRFClk(
+	IN PRTMP_ADAPTER 	pAd,
+	IN	UCHAR			Channel);
+
+#ifdef RT2870
+//
+// Function Prototype in rtusb_bulk.c
+//
+VOID	RTUSBInitTxDesc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTX_CONTEXT		pTxContext,
+	IN	UCHAR			BulkOutPipeId,
+	IN	usb_complete_t	Func);
+
+VOID	RTUSBInitHTTxDesc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PHT_TX_CONTEXT	pTxContext,
+	IN	UCHAR			BulkOutPipeId,
+	IN	ULONG			BulkOutSize,
+	IN	usb_complete_t	Func);
+
+VOID	RTUSBInitRxDesc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PRX_CONTEXT		pRxContext);
+
+VOID RTUSBCleanUpDataBulkOutQueue(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID RTUSBCancelPendingBulkOutIRP(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID RTUSBBulkOutDataPacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			BulkOutPipeId,
+	IN	UCHAR			Index);
+
+VOID RTUSBBulkOutNullFrame(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID RTUSBBulkOutRTSFrame(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID RTUSBCancelPendingBulkInIRP(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID RTUSBCancelPendingIRPs(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID RTUSBBulkOutMLMEPacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Index);
+
+VOID RTUSBBulkOutPsPoll(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID RTUSBCleanUpMLMEBulkOutQueue(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID RTUSBKickBulkOut(
+	IN	PRTMP_ADAPTER pAd);
+
+VOID	RTUSBBulkReceive(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID DoBulkIn(
+	IN RTMP_ADAPTER *pAd);
+
+VOID RTUSBInitRxDesc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN  PRX_CONTEXT		pRxContext);
+
+VOID RTUSBBulkRxHandle(
+	IN unsigned long data);
+
+//
+// Function Prototype in rtusb_io.c
+//
+NTSTATUS RTUSBMultiRead(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Offset,
+	OUT	PUCHAR			pData,
+	IN	USHORT			length);
+
+NTSTATUS RTUSBMultiWrite(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Offset,
+	IN	PUCHAR			pData,
+	IN	USHORT			length);
+
+NTSTATUS RTUSBMultiWrite_OneByte(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Offset,
+	IN	PUCHAR			pData);
+
+NTSTATUS RTUSBReadBBPRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Id,
+	IN	PUCHAR			pValue);
+
+NTSTATUS RTUSBWriteBBPRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Id,
+	IN	UCHAR			Value);
+
+NTSTATUS RTUSBWriteRFRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UINT32			Value);
+
+NTSTATUS	RT30xxWriteRFRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			RegID,
+	IN	UCHAR			Value);
+
+NTSTATUS	RT30xxReadRFRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			RegID,
+	IN	PUCHAR			pValue);
+
+NTSTATUS RTUSB_VendorRequest(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UINT32			TransferFlags,
+	IN	UCHAR			ReservedBits,
+	IN	UCHAR			Request,
+	IN	USHORT			Value,
+	IN	USHORT			Index,
+	IN	PVOID			TransferBuffer,
+	IN	UINT32			TransferBufferLength);
+
+NTSTATUS RTUSBReadEEPROM(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Offset,
+	OUT	PUCHAR			pData,
+	IN	USHORT			length);
+
+NTSTATUS RTUSBWriteEEPROM(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Offset,
+	IN	PUCHAR			pData,
+	IN	USHORT			length);
+
+VOID RTUSBPutToSleep(
+	IN	PRTMP_ADAPTER	pAd);
+
+NTSTATUS RTUSBWakeUp(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID RTUSBInitializeCmdQ(
+	IN	PCmdQ	cmdq);
+
+NDIS_STATUS	RTUSBEnqueueCmdFromNdis(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	NDIS_OID		Oid,
+	IN	BOOLEAN			SetInformation,
+	IN	PVOID			pInformationBuffer,
+	IN	UINT32			InformationBufferLength);
+
+NDIS_STATUS RTUSBEnqueueInternalCmd(
+	IN	PRTMP_ADAPTER	pAd,
+	IN NDIS_OID			Oid,
+	IN PVOID			pInformationBuffer,
+	IN UINT32			InformationBufferLength);
+
+VOID RTUSBDequeueCmd(
+	IN	PCmdQ		cmdq,
+	OUT	PCmdQElmt	*pcmdqelmt);
+
+INT RTUSBCmdThread(
+	IN OUT PVOID Context);
+
+INT TimerQThread(
+	IN OUT PVOID Context);
+
+RT2870_TIMER_ENTRY *RT2870_TimerQ_Insert(
+	IN RTMP_ADAPTER *pAd,
+	IN RALINK_TIMER_STRUCT *pTimer);
+
+BOOLEAN RT2870_TimerQ_Remove(
+	IN RTMP_ADAPTER *pAd,
+	IN RALINK_TIMER_STRUCT *pTimer);
+
+void RT2870_TimerQ_Exit(
+	IN RTMP_ADAPTER *pAd);
+
+void RT2870_TimerQ_Init(
+	IN RTMP_ADAPTER *pAd);
+
+VOID RT2870_BssBeaconExit(
+	IN RTMP_ADAPTER *pAd);
+
+VOID RT2870_BssBeaconStop(
+	IN RTMP_ADAPTER *pAd);
+
+VOID RT2870_BssBeaconStart(
+	IN RTMP_ADAPTER * pAd);
+
+VOID RT2870_BssBeaconInit(
+	IN RTMP_ADAPTER *pAd);
+
+VOID RT2870_WatchDog(
+	IN RTMP_ADAPTER *pAd);
+
+NTSTATUS RTUSBWriteMACRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Offset,
+	IN	UINT32			Value);
+
+NTSTATUS RTUSBReadMACRegister(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	USHORT			Offset,
+	OUT	PUINT32			pValue);
+
+NTSTATUS RTUSBSingleWrite(
+	IN 	RTMP_ADAPTER 	*pAd,
+	IN	USHORT			Offset,
+	IN	USHORT			Value);
+
+NTSTATUS RTUSBFirmwareRun(
+	IN	PRTMP_ADAPTER	pAd);
+
+NTSTATUS RTUSBFirmwareWrite(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR		pFwImage,
+	IN ULONG		FwLen);
+
+NTSTATUS	RTUSBFirmwareOpmode(
+	IN	PRTMP_ADAPTER	pAd,
+	OUT	PUINT32			pValue);
+
+NTSTATUS	RTUSBVenderReset(
+	IN	PRTMP_ADAPTER	pAd);
+
+NDIS_STATUS RTUSBSetHardWareRegister(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PVOID			pBuf);
+
+NDIS_STATUS RTUSBQueryHardWareRegister(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PVOID			pBuf);
+
+VOID CMDHandler(
+    IN PRTMP_ADAPTER pAd);
+
+
+NDIS_STATUS	 CreateThreads(
+	IN	struct net_device *net_dev );
+
+
+VOID MacTableInitialize(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID MlmeSetPsm(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT psm);
+
+NDIS_STATUS RTMPWPAAddKeyProc(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PVOID           pBuf);
+
+VOID AsicRxAntEvalAction(
+	IN PRTMP_ADAPTER pAd);
+
+#if 0 // Mark because not used in RT28xx.
+NTSTATUS RTUSBRxPacket(
+	IN	PRTMP_ADAPTER  pAd,
+	IN    BOOLEAN          bBulkReceive);
+
+VOID RTUSBDequeueMLMEPacket(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID RTUSBCleanUpMLMEWaitQueue(
+	IN	PRTMP_ADAPTER	pAd);
+#endif
+
+void append_pkt(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pHeader802_3,
+    IN  UINT            HdrLen,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize,
+	OUT  PNDIS_PACKET	*ppPacket);
+
+UINT deaggregate_AMSDU_announce(
+	IN	PRTMP_ADAPTER	pAd,
+	PNDIS_PACKET		pPacket,
+	IN	PUCHAR			pData,
+	IN	ULONG			DataSize);
+
+NDIS_STATUS	RTMPCheckRxError(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PHEADER_802_11	pHeader,
+	IN	PRXWI_STRUC	pRxWI,
+	IN	PRT28XX_RXD_STRUC	pRxINFO);
+
+
+VOID RTUSBMlmeHardTransmit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PMGMT_STRUC		pMgmt);
+
+INT MlmeThread(
+	IN PVOID Context);
+
+#if 0
+VOID    RTUSBResumeMsduTransmission(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID    RTUSBSuspendMsduTransmission(
+	IN	PRTMP_ADAPTER	pAd);
+#endif
+
+//
+// Function Prototype in rtusb_data.c
+//
+NDIS_STATUS	RTUSBFreeDescriptorRequest(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			BulkOutPipeId,
+	IN	UINT32			NumberRequired);
+
+
+BOOLEAN	RTUSBNeedQueueBackForAgg(
+	IN RTMP_ADAPTER *pAd,
+	IN UCHAR		BulkOutPipeId);
+
+
+VOID RTMPWriteTxInfo(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PTXINFO_STRUC 	pTxInfo,
+	IN	  USHORT		USBDMApktLen,
+	IN	  BOOLEAN		bWiv,
+	IN	  UCHAR			QueueSel,
+	IN	  UCHAR			NextValid,
+	IN	  UCHAR			TxBurst);
+
+//
+// Function Prototype in cmm_data_2870.c
+//
+USHORT RtmpUSB_WriteSubTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	BOOLEAN			bIsLast,
+	OUT	USHORT			*FreeNumber);
+
+USHORT RtmpUSB_WriteSingleTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	BOOLEAN			bIsLast,
+	OUT	USHORT			*FreeNumber);
+
+USHORT	RtmpUSB_WriteFragTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	UCHAR			fragNum,
+	OUT	USHORT			*FreeNumber);
+
+USHORT RtmpUSB_WriteMultiTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	UCHAR			frameNum,
+	OUT	USHORT			*FreeNumber);
+
+VOID RtmpUSB_FinalWriteTxResource(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	USHORT			totalMPDUSize,
+	IN	USHORT			TxIdx);
+
+VOID RtmpUSBDataLastTxIdx(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			QueIdx,
+	IN	USHORT			TxIdx);
+
+VOID RtmpUSBDataKickOut(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk,
+	IN	UCHAR			QueIdx);
+
+
+int RtmpUSBMgmtKickOut(
+	IN RTMP_ADAPTER 	*pAd,
+	IN UCHAR 			QueIdx,
+	IN PNDIS_PACKET		pPacket,
+	IN PUCHAR			pSrcBufVA,
+	IN UINT 			SrcBufLen);
+
+VOID RtmpUSBNullFrameKickOut(
+	IN RTMP_ADAPTER *pAd,
+	IN UCHAR		QueIdx,
+	IN UCHAR		*pNullFrame,
+	IN UINT32		frameLen);
+
+VOID RT28xxUsbStaAsicForceWakeup(
+	IN PRTMP_ADAPTER pAd,
+	IN BOOLEAN       bFromTx);
+
+VOID RT28xxUsbStaAsicSleepThenAutoWakeup(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT TbttNumToNextWakeUp);
+
+VOID RT28xxUsbMlmeRadioOn(
+	IN PRTMP_ADAPTER pAd);
+
+VOID RT28xxUsbMlmeRadioOFF(
+	IN PRTMP_ADAPTER pAd);
+#endif // RT2870 //
+
+////////////////////////////////////////
+
+VOID QBSS_LoadInit(
+ 	IN		RTMP_ADAPTER	*pAd);
+
+UINT32 QBSS_LoadElementAppend(
+ 	IN		RTMP_ADAPTER	*pAd,
+	OUT		UINT8			*buf_p);
+
+VOID QBSS_LoadUpdate(
+ 	IN		RTMP_ADAPTER	*pAd);
+
+///////////////////////////////////////
+INT RTMPShowCfgValue(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pName,
+	IN	PUCHAR			pBuf);
+
+PCHAR   RTMPGetRalinkAuthModeStr(
+    IN  NDIS_802_11_AUTHENTICATION_MODE authMode);
+
+PCHAR   RTMPGetRalinkEncryModeStr(
+    IN  USHORT encryMode);
+//////////////////////////////////////
+
+#ifdef CONFIG_STA_SUPPORT
+VOID AsicStaBbpTuning(
+	IN PRTMP_ADAPTER pAd);
+
+BOOLEAN StaAddMacTableEntry(
+	IN  PRTMP_ADAPTER		pAd,
+	IN  PMAC_TABLE_ENTRY	pEntry,
+	IN  UCHAR				MaxSupportedRateIn500Kbps,
+	IN  HT_CAPABILITY_IE	*pHtCapability,
+	IN  UCHAR				HtCapabilityLen,
+	IN  USHORT        		CapabilityInfo);
+#endif // CONFIG_STA_SUPPORT //
+
+void RTMP_IndicateMediaState(
+	IN	PRTMP_ADAPTER	pAd);
+
+VOID ReSyncBeaconTime(
+	IN  PRTMP_ADAPTER   pAd);
+
+VOID RTMPSetAGCInitValue(
+	IN PRTMP_ADAPTER	pAd,
+	IN UCHAR			BandWidth);
+
+int rt28xx_close(IN PNET_DEV dev);
+int rt28xx_open(IN PNET_DEV dev);
+
+__inline INT VIRTUAL_IF_UP(PRTMP_ADAPTER pAd)
+{
+extern VOID MeshMakeBeacon(IN PRTMP_ADAPTER pAd, IN UCHAR idx);
+extern VOID MeshUpdateBeaconFrame(IN PRTMP_ADAPTER pAd, IN UCHAR idx);
+
+	if (VIRTUAL_IF_NUM(pAd) == 0)
+	{
+		if (rt28xx_open(pAd->net_dev) != 0)
+			return -1;
+	}
+	else
+	{
+	}
+	VIRTUAL_IF_INC(pAd);
+	return 0;
+}
+
+__inline VOID VIRTUAL_IF_DOWN(PRTMP_ADAPTER pAd)
+{
+	VIRTUAL_IF_DEC(pAd);
+	if (VIRTUAL_IF_NUM(pAd) == 0)
+		rt28xx_close(pAd->net_dev);
+	return;
+}
+
+
+#endif  // __RTMP_H__
+
diff --git a/drivers/staging/rt2870/rtmp_ckipmic.h b/drivers/staging/rt2870/rtmp_ckipmic.h
new file mode 100644
index 00000000000..a3d949a39d3
--- /dev/null
+++ b/drivers/staging/rt2870/rtmp_ckipmic.h
@@ -0,0 +1,113 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rtmp_ckipmic.h
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+*/
+#ifndef	__RTMP_CKIPMIC_H__
+#define	__RTMP_CKIPMIC_H__
+
+typedef	struct	_MIC_CONTEXT	{
+	/* --- MMH context                            */
+	UCHAR		CK[16];				/* the key                                    */
+	UCHAR		coefficient[16];	/* current aes counter mode coefficients      */
+	ULONGLONG	accum;				/* accumulated mic, reduced to u32 in final() */
+	UINT		position;			/* current position (byte offset) in message  */
+	UCHAR		part[4];			/* for conversion of message to u32 for mmh   */
+}	MIC_CONTEXT, *PMIC_CONTEXT;
+
+VOID	CKIP_key_permute(
+	OUT	UCHAR	*PK,			/* output permuted key */
+	IN	UCHAR	*CK,			/* input CKIP key */
+	IN	UCHAR	toDsFromDs,		/* input toDs/FromDs bits */
+	IN	UCHAR	*piv);			/* input pointer to IV */
+
+VOID	RTMPCkipMicInit(
+	IN	PMIC_CONTEXT		pContext,
+	IN	PUCHAR				CK);
+
+VOID RTMPMicUpdate(
+    IN  PMIC_CONTEXT        pContext,
+    IN  PUCHAR              pOctets,
+    IN  INT                 len);
+
+ULONG RTMPMicGetCoefficient(
+    IN  PMIC_CONTEXT         pContext);
+
+VOID xor_128(
+    IN  PUCHAR              a,
+    IN  PUCHAR              b,
+    OUT PUCHAR              out);
+
+UCHAR RTMPCkipSbox(
+    IN  UCHAR               a);
+
+VOID xor_32(
+    IN  PUCHAR              a,
+    IN  PUCHAR              b,
+    OUT PUCHAR              out);
+
+VOID next_key(
+    IN  PUCHAR              key,
+    IN  INT                 round);
+
+VOID byte_sub(
+    IN  PUCHAR              in,
+    OUT PUCHAR              out);
+
+VOID shift_row(
+    IN  PUCHAR              in,
+    OUT PUCHAR              out);
+
+VOID mix_column(
+    IN  PUCHAR              in,
+    OUT PUCHAR              out);
+
+VOID RTMPAesEncrypt(
+    IN  PUCHAR              key,
+    IN  PUCHAR              data,
+    IN  PUCHAR              ciphertext);
+
+VOID RTMPMicFinal(
+    IN  PMIC_CONTEXT        pContext,
+    OUT UCHAR               digest[4]);
+
+VOID RTMPCkipInsertCMIC(
+    IN  PRTMP_ADAPTER   pAd,
+    OUT PUCHAR          pMIC,
+    IN  PUCHAR          p80211hdr,
+    IN  PNDIS_PACKET    pPacket,
+    IN  PCIPHER_KEY     pKey,
+    IN  PUCHAR          mic_snap);
+
+#endif //__RTMP_CKIPMIC_H__
diff --git a/drivers/staging/rt2870/rtmp_def.h b/drivers/staging/rt2870/rtmp_def.h
new file mode 100644
index 00000000000..9b86325b4c5
--- /dev/null
+++ b/drivers/staging/rt2870/rtmp_def.h
@@ -0,0 +1,1622 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    rtmp_def.h
+
+    Abstract:
+    Miniport related definition header
+
+    Revision History:
+    Who         When          What
+    --------    ----------    ----------------------------------------------
+    Paul Lin    08-01-2002    created
+    John Chang  08-05-2003    add definition for 11g & other drafts
+*/
+#ifndef __RTMP_DEF_H__
+#define __RTMP_DEF_H__
+
+#include "oid.h"
+
+//
+//  Debug information verbosity: lower values indicate higher urgency
+//
+#define RT_DEBUG_OFF        0
+#define RT_DEBUG_ERROR      1
+#define RT_DEBUG_WARN       2
+#define RT_DEBUG_TRACE      3
+#define RT_DEBUG_INFO       4
+#define RT_DEBUG_LOUD       5
+
+#define NIC_TAG             ((ULONG)'0682')
+#define NIC_DBG_STRING      ("**RT28xx**")
+
+#ifdef SNMP_SUPPORT
+// for snmp
+// to get manufacturer OUI, kathy, 2008_0220
+#define ManufacturerOUI_LEN			3
+#define ManufacturerNAME			("Ralink Technology Company.")
+#define	ResourceTypeIdName			("Ralink_ID")
+#endif
+
+
+//#define PACKED
+
+#define RALINK_2883_VERSION		((UINT32)0x28830300)
+#define RALINK_2880E_VERSION	((UINT32)0x28720200)
+#define RALINK_3070_VERSION		((UINT32)0x30700200)
+
+//
+// NDIS version in use by the NIC driver.
+// The high byte is the major version. The low byte is the minor version.
+//
+#ifdef  NDIS51_MINIPORT
+#define NIC_DRIVER_VERSION      0x0501
+#else
+#define NIC_DRIVER_VERSION      0x0500
+#endif
+
+//
+// NDIS media type, current is ethernet, change if native wireless supported
+//
+#define NIC_MEDIA_TYPE          NdisMedium802_3
+#define NIC_PCI_HDR_LENGTH      0xe2
+#define NIC_MAX_PACKET_SIZE     2304
+#define NIC_HEADER_SIZE         14
+#define MAX_MAP_REGISTERS_NEEDED 32
+#define MIN_MAP_REGISTERS_NEEDED 2   //Todo: should consider fragment issue.
+
+//
+// interface type, we use PCI
+//
+#define NIC_INTERFACE_TYPE      NdisInterfacePci
+#define NIC_INTERRUPT_MODE      NdisInterruptLevelSensitive
+
+//
+// buffer size passed in NdisMQueryAdapterResources
+// We should only need three adapter resources (IO, interrupt and memory),
+// Some devices get extra resources, so have room for 10 resources
+//                    UF_SIZE   (sizeof(NDIS_RESOURCE_LIST) + (10*sizeof(CM_PARTIAL_RESOURCE_DESCRIPTOR)))
+
+
+#define NIC_RESOURCE_B//
+// IO space length
+//
+#define NIC_MAP_IOSPACE_LENGTH  sizeof(CSR_STRUC)
+
+#define MAX_RX_PKT_LEN	1520
+
+//
+// Entry number for each DMA descriptor ring
+//
+
+
+#ifdef RT2870
+#define TX_RING_SIZE            8 // 1
+#define PRIO_RING_SIZE          8
+#define MGMT_RING_SIZE       32 // PRIO_RING_SIZE
+#define RX_RING_SIZE            8
+#define MAX_TX_PROCESS          4
+#define LOCAL_TXBUF_SIZE        2048
+#endif // RT2870 //
+
+#ifdef MULTIPLE_CARD_SUPPORT
+// MC: Multple Cards
+#define MAX_NUM_OF_MULTIPLE_CARD		32
+#endif // MULTIPLE_CARD_SUPPORT //
+
+#define MAX_RX_PROCESS          128 //64 //32
+#define NUM_OF_LOCAL_TXBUF      2
+#define TXD_SIZE                16
+#define TXWI_SIZE               16
+#define RXD_SIZE               	16
+#define RXWI_SIZE             	16
+// TXINFO_SIZE + TXWI_SIZE + 802.11 Header Size + AMSDU sub frame header
+#define TX_DMA_1ST_BUFFER_SIZE  96    // only the 1st physical buffer is pre-allocated
+#define MGMT_DMA_BUFFER_SIZE    1536 //2048
+#define RX_BUFFER_AGGRESIZE     3840 //3904 //3968 //4096 //2048 //4096
+#define RX_BUFFER_NORMSIZE      3840 //3904 //3968 //4096 //2048 //4096
+#define TX_BUFFER_NORMSIZE		RX_BUFFER_NORMSIZE
+#define MAX_FRAME_SIZE          2346                    // Maximum 802.11 frame size
+#define MAX_AGGREGATION_SIZE    3840 //3904 //3968 //4096
+#define MAX_NUM_OF_TUPLE_CACHE  2
+#define MAX_MCAST_LIST_SIZE     32
+#define MAX_LEN_OF_VENDOR_DESC  64
+//#define MAX_SIZE_OF_MCAST_PSQ   (NUM_OF_LOCAL_TXBUF >> 2) // AP won't spend more than 1/4 of total buffers on M/BCAST PSQ
+#define MAX_SIZE_OF_MCAST_PSQ               32
+
+#define MAX_RX_PROCESS_CNT	(RX_RING_SIZE)
+
+
+#define MAX_PACKETS_IN_QUEUE				(512) //(512)    // to pass WMM A5-WPAPSK
+#define MAX_PACKETS_IN_MCAST_PS_QUEUE		32
+#define MAX_PACKETS_IN_PS_QUEUE				128	//32
+#define WMM_NUM_OF_AC                       4  /* AC0, AC1, AC2, and AC3 */
+
+
+
+// RxFilter
+#define STANORMAL	 0x17f97
+#define APNORMAL	 0x15f97
+//
+//  RTMP_ADAPTER flags
+//
+#define fRTMP_ADAPTER_MAP_REGISTER          0x00000001
+#define fRTMP_ADAPTER_INTERRUPT_IN_USE      0x00000002
+#define fRTMP_ADAPTER_HARDWARE_ERROR        0x00000004
+#define fRTMP_ADAPTER_SCATTER_GATHER        0x00000008
+#define fRTMP_ADAPTER_SEND_PACKET_ERROR     0x00000010
+#define fRTMP_ADAPTER_MLME_RESET_IN_PROGRESS 0x00000020
+#define fRTMP_ADAPTER_HALT_IN_PROGRESS      0x00000040
+#define fRTMP_ADAPTER_RESET_IN_PROGRESS     0x00000080
+#define fRTMP_ADAPTER_NIC_NOT_EXIST         0x00000100
+#define fRTMP_ADAPTER_TX_RING_ALLOCATED     0x00000200
+#define fRTMP_ADAPTER_REMOVE_IN_PROGRESS    0x00000400
+#define fRTMP_ADAPTER_MIMORATE_INUSED       0x00000800
+#define fRTMP_ADAPTER_RX_RING_ALLOCATED     0x00001000
+#define fRTMP_ADAPTER_INTERRUPT_ACTIVE      0x00002000
+#define fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS  0x00004000
+#define	fRTMP_ADAPTER_REASSOC_IN_PROGRESS	0x00008000
+#define	fRTMP_ADAPTER_MEDIA_STATE_PENDING	0x00010000
+#define	fRTMP_ADAPTER_RADIO_OFF				0x00020000
+#define fRTMP_ADAPTER_BULKOUT_RESET			0x00040000
+#define	fRTMP_ADAPTER_BULKIN_RESET			0x00080000
+#define fRTMP_ADAPTER_RDG_ACTIVE			0x00100000
+#define fRTMP_ADAPTER_DYNAMIC_BE_TXOP_ACTIVE 0x00200000
+#define fRTMP_ADAPTER_SCAN_2040 			0x04000000
+#define	fRTMP_ADAPTER_RADIO_MEASUREMENT		0x08000000
+
+#define fRTMP_ADAPTER_START_UP         		0x10000000	//Devive already initialized and enabled Tx/Rx.
+#define fRTMP_ADAPTER_MEDIA_STATE_CHANGE    0x20000000
+#define fRTMP_ADAPTER_IDLE_RADIO_OFF        0x40000000
+
+// Lock bit for accessing different ring buffers
+//#define fRTMP_ADAPTER_TX_RING_BUSY        0x80000000
+//#define fRTMP_ADAPTER_MGMT_RING_BUSY      0x40000000
+//#define fRTMP_ADAPTER_ATIM_RING_BUSY      0x20000000
+//#define fRTMP_ADAPTER_RX_RING_BUSY        0x10000000
+
+// Lock bit for accessing different queue
+//#define   fRTMP_ADAPTER_TX_QUEUE_BUSY     0x08000000
+//#define   fRTMP_ADAPTER_MGMT_QUEUE_BUSY   0x04000000
+
+//
+//  STA operation status flags
+//
+#define fOP_STATUS_INFRA_ON                 0x00000001
+#define fOP_STATUS_ADHOC_ON                 0x00000002
+#define fOP_STATUS_BG_PROTECTION_INUSED     0x00000004
+#define fOP_STATUS_SHORT_SLOT_INUSED        0x00000008
+#define fOP_STATUS_SHORT_PREAMBLE_INUSED    0x00000010
+#define fOP_STATUS_RECEIVE_DTIM             0x00000020
+//#define fOP_STATUS_TX_RATE_SWITCH_ENABLED   0x00000040
+#define fOP_STATUS_MEDIA_STATE_CONNECTED    0x00000080
+#define fOP_STATUS_WMM_INUSED               0x00000100
+#define fOP_STATUS_AGGREGATION_INUSED       0x00000200
+#define fOP_STATUS_DOZE                     0x00000400  // debug purpose
+#define fOP_STATUS_PIGGYBACK_INUSED         0x00000800  // piggy-back, and aggregation
+#define fOP_STATUS_APSD_INUSED				0x00001000
+#define fOP_STATUS_TX_AMSDU_INUSED			0x00002000
+#define fOP_STATUS_MAX_RETRY_ENABLED		0x00004000
+#define fOP_STATUS_WAKEUP_NOW               0x00008000
+#define fOP_STATUS_ADVANCE_POWER_SAVE_PCIE_DEVICE       0x00020000
+
+#ifdef DOT11N_DRAFT3
+#define fOP_STATUS_SCAN_2040               	    0x00040000
+#endif // DOT11N_DRAFT3 //
+
+#define CCKSETPROTECT		0x1
+#define OFDMSETPROTECT		0x2
+#define MM20SETPROTECT		0x4
+#define MM40SETPROTECT		0x8
+#define GF20SETPROTECT		0x10
+#define GR40SETPROTECT		0x20
+#define ALLN_SETPROTECT		(GR40SETPROTECT | GF20SETPROTECT | MM40SETPROTECT | MM20SETPROTECT)
+
+//
+//  AP's client table operation status flags
+//
+#define fCLIENT_STATUS_WMM_CAPABLE          0x00000001  // CLIENT can parse QOS DATA frame
+#define fCLIENT_STATUS_AGGREGATION_CAPABLE  0x00000002  // CLIENT can receive Ralink's proprietary TX aggregation frame
+#define fCLIENT_STATUS_PIGGYBACK_CAPABLE    0x00000004  // CLIENT support piggy-back
+#define fCLIENT_STATUS_AMSDU_INUSED			0x00000008
+#define fCLIENT_STATUS_SGI20_CAPABLE		0x00000010
+#define fCLIENT_STATUS_SGI40_CAPABLE		0x00000020
+#define fCLIENT_STATUS_TxSTBC_CAPABLE		0x00000040
+#define fCLIENT_STATUS_RxSTBC_CAPABLE		0x00000080
+#define fCLIENT_STATUS_HTC_CAPABLE			0x00000100
+#define fCLIENT_STATUS_RDG_CAPABLE			0x00000200
+#define fCLIENT_STATUS_MCSFEEDBACK_CAPABLE  0x00000400
+#define fCLIENT_STATUS_APSD_CAPABLE         0x00000800  /* UAPSD STATION */
+
+#ifdef DOT11N_DRAFT3
+#define fCLIENT_STATUS_BSSCOEXIST_CAPABLE	0x00001000
+#endif // DOT11N_DRAFT3 //
+
+#define fCLIENT_STATUS_RALINK_CHIPSET		0x00100000
+//
+//  STA configuration flags
+//
+//#define fSTA_CFG_ENABLE_TX_BURST          0x00000001
+
+// 802.11n Operating Mode Definition. 0-3 also used in ASICUPdateProtect switch case
+#define HT_NO_PROTECT	0
+#define HT_LEGACY_PROTECT	1
+#define HT_40_PROTECT	2
+#define HT_2040_PROTECT	3
+#define HT_RTSCTS_6M	7
+//following is our own definition in order to turn on our ASIC protection register in INFRASTRUCTURE.
+#define HT_ATHEROS	8	// rt2860c has problem with atheros chip. we need to turn on RTS/CTS .
+#define HT_FORCERTSCTS	9	// Force turn on RTS/CTS first. then go to evaluate if this force RTS is necessary.
+
+//
+// RX Packet Filter control flags. Apply on pAd->PacketFilter
+//
+#define fRX_FILTER_ACCEPT_DIRECT            NDIS_PACKET_TYPE_DIRECTED
+#define fRX_FILTER_ACCEPT_MULTICAST         NDIS_PACKET_TYPE_MULTICAST
+#define fRX_FILTER_ACCEPT_BROADCAST         NDIS_PACKET_TYPE_BROADCAST
+#define fRX_FILTER_ACCEPT_ALL_MULTICAST     NDIS_PACKET_TYPE_ALL_MULTICAST
+
+//
+// Error code section
+//
+// NDIS_ERROR_CODE_ADAPTER_NOT_FOUND
+#define ERRLOG_READ_PCI_SLOT_FAILED     0x00000101L
+#define ERRLOG_WRITE_PCI_SLOT_FAILED    0x00000102L
+#define ERRLOG_VENDOR_DEVICE_NOMATCH    0x00000103L
+
+// NDIS_ERROR_CODE_ADAPTER_DISABLED
+#define ERRLOG_BUS_MASTER_DISABLED      0x00000201L
+
+// NDIS_ERROR_CODE_UNSUPPORTED_CONFIGURATION
+#define ERRLOG_INVALID_SPEED_DUPLEX     0x00000301L
+#define ERRLOG_SET_SECONDARY_FAILED     0x00000302L
+
+// NDIS_ERROR_CODE_OUT_OF_RESOURCES
+#define ERRLOG_OUT_OF_MEMORY            0x00000401L
+#define ERRLOG_OUT_OF_SHARED_MEMORY     0x00000402L
+#define ERRLOG_OUT_OF_MAP_REGISTERS     0x00000403L
+#define ERRLOG_OUT_OF_BUFFER_POOL       0x00000404L
+#define ERRLOG_OUT_OF_NDIS_BUFFER       0x00000405L
+#define ERRLOG_OUT_OF_PACKET_POOL       0x00000406L
+#define ERRLOG_OUT_OF_NDIS_PACKET       0x00000407L
+#define ERRLOG_OUT_OF_LOOKASIDE_MEMORY  0x00000408L
+
+// NDIS_ERROR_CODE_HARDWARE_FAILURE
+#define ERRLOG_SELFTEST_FAILED          0x00000501L
+#define ERRLOG_INITIALIZE_ADAPTER       0x00000502L
+#define ERRLOG_REMOVE_MINIPORT          0x00000503L
+
+// NDIS_ERROR_CODE_RESOURCE_CONFLICT
+#define ERRLOG_MAP_IO_SPACE             0x00000601L
+#define ERRLOG_QUERY_ADAPTER_RESOURCES  0x00000602L
+#define ERRLOG_NO_IO_RESOURCE           0x00000603L
+#define ERRLOG_NO_INTERRUPT_RESOURCE    0x00000604L
+#define ERRLOG_NO_MEMORY_RESOURCE       0x00000605L
+
+
+// WDS definition
+#define	MAX_WDS_ENTRY               4
+#define WDS_PAIRWISE_KEY_OFFSET     60    // WDS links uses pairwise key#60 ~ 63 in ASIC pairwise key table
+
+#define	WDS_DISABLE_MODE            0
+#define	WDS_RESTRICT_MODE           1
+#define	WDS_BRIDGE_MODE             2
+#define	WDS_REPEATER_MODE           3
+#define	WDS_LAZY_MODE               4
+
+
+#define MAX_MESH_NUM				0
+
+#define MAX_APCLI_NUM				0
+#ifdef APCLI_SUPPORT
+#undef	MAX_APCLI_NUM
+#define MAX_APCLI_NUM				1
+#endif // APCLI_SUPPORT //
+
+#define MAX_MBSSID_NUM				1
+
+/* sanity check for apidx */
+#define MBSS_MR_APIDX_SANITY_CHECK(apidx) \
+    { if (apidx > MAX_MBSSID_NUM) { \
+          printk("%s> Error! apidx = %d > MAX_MBSSID_NUM!\n", __FUNCTION__, apidx); \
+	  apidx = MAIN_MBSSID; } }
+
+#define VALID_WCID(_wcid)	((_wcid) > 0 && (_wcid) < MAX_LEN_OF_MAC_TABLE )
+
+#define MAIN_MBSSID                 0
+#define FIRST_MBSSID                1
+
+
+#define MAX_BEACON_SIZE				512
+// If the MAX_MBSSID_NUM is larger than 6,
+// it shall reserve some WCID space(wcid 222~253) for beacon frames.
+// -	these wcid 238~253 are reserved for beacon#6(ra6).
+// -	these wcid 222~237 are reserved for beacon#7(ra7).
+#if defined(MAX_MBSSID_NUM) && (MAX_MBSSID_NUM == 8)
+#define HW_RESERVED_WCID	222
+#elif defined(MAX_MBSSID_NUM) && (MAX_MBSSID_NUM == 7)
+#define HW_RESERVED_WCID	238
+#else
+#define HW_RESERVED_WCID	255
+#endif
+
+// Then dedicate wcid of DFS and Carrier-Sense.
+#define DFS_CTS_WCID 		(HW_RESERVED_WCID - 1)
+#define CS_CTS_WCID 		(HW_RESERVED_WCID - 2)
+#define LAST_SPECIFIC_WCID	(HW_RESERVED_WCID - 2)
+
+// If MAX_MBSSID_NUM is 8, the maximum available wcid for the associated STA is 211.
+// If MAX_MBSSID_NUM is 7, the maximum available wcid for the associated STA is 228.
+#define MAX_AVAILABLE_CLIENT_WCID	(LAST_SPECIFIC_WCID - MAX_MBSSID_NUM - 1)
+
+// TX need WCID to find Cipher Key
+// these wcid 212 ~ 219 are reserved for bc/mc packets if MAX_MBSSID_NUM is 8.
+#define GET_GroupKey_WCID(__wcid, __bssidx) \
+	{										\
+		__wcid = LAST_SPECIFIC_WCID - (MAX_MBSSID_NUM) + __bssidx;	\
+	}
+
+#define IsGroupKeyWCID(__wcid) (((__wcid) < LAST_SPECIFIC_WCID) && ((__wcid) >= (LAST_SPECIFIC_WCID - (MAX_MBSSID_NUM))))
+
+
+// definition to support multiple BSSID
+#define BSS0                            0
+#define BSS1                            1
+#define BSS2                            2
+#define BSS3                            3
+#define BSS4                            4
+#define BSS5                            5
+#define BSS6                            6
+#define BSS7                            7
+
+
+//============================================================
+// Length definitions
+#define PEER_KEY_NO                     2
+#define MAC_ADDR_LEN                    6
+#define TIMESTAMP_LEN                   8
+#define MAX_LEN_OF_SUPPORTED_RATES      MAX_LENGTH_OF_SUPPORT_RATES // 1, 2, 5.5, 11, 6, 9, 12, 18, 24, 36, 48, 54
+#define MAX_LEN_OF_KEY                  32      // 32 octets == 256 bits, Redefine for WPA
+#define MAX_NUM_OF_CHANNELS             MAX_NUM_OF_CHS      // 14 channels @2.4G +  12@UNII + 4 @MMAC + 11 @HiperLAN2 + 7 @Japan + 1 as NULL termination
+#define MAX_NUM_OF_11JCHANNELS             20      // 14 channels @2.4G +  12@UNII + 4 @MMAC + 11 @HiperLAN2 + 7 @Japan + 1 as NULL termination
+#define MAX_LEN_OF_SSID                 32
+#define CIPHER_TEXT_LEN                 128
+#define HASH_TABLE_SIZE                 256
+#define MAX_VIE_LEN                     1024   // New for WPA cipher suite variable IE sizes.
+#define MAX_SUPPORT_MCS             32
+
+//============================================================
+// ASIC WCID Table definition.
+//============================================================
+#define BSSID_WCID		1	// in infra mode, always put bssid with this WCID
+#define MCAST_WCID	0x0
+#define BSS0Mcast_WCID	0x0
+#define BSS1Mcast_WCID	0xf8
+#define BSS2Mcast_WCID	0xf9
+#define BSS3Mcast_WCID	0xfa
+#define BSS4Mcast_WCID	0xfb
+#define BSS5Mcast_WCID	0xfc
+#define BSS6Mcast_WCID	0xfd
+#define BSS7Mcast_WCID	0xfe
+#define RESERVED_WCID		0xff
+
+#define MAX_NUM_OF_ACL_LIST				MAX_NUMBER_OF_ACL
+
+#define MAX_LEN_OF_MAC_TABLE            MAX_NUMBER_OF_MAC // if MAX_MBSSID_NUM is 8, this value can't be larger than 211
+
+#if MAX_LEN_OF_MAC_TABLE>MAX_AVAILABLE_CLIENT_WCID
+#error MAX_LEN_OF_MAC_TABLE can not be larger than MAX_AVAILABLE_CLIENT_WCID!!!!
+#endif
+
+#define MAX_NUM_OF_WDS_LINK_PERBSSID	            3
+#define MAX_NUM_OF_WDS_LINK	            (MAX_NUM_OF_WDS_LINK_PERBSSID*MAX_MBSSID_NUM)
+#define MAX_NUM_OF_EVENT                MAX_NUMBER_OF_EVENT
+#define WDS_LINK_START_WCID				(MAX_LEN_OF_MAC_TABLE-1)
+
+#define NUM_OF_TID			8
+#define MAX_AID_BA                    4
+#define MAX_LEN_OF_BA_REC_TABLE          ((NUM_OF_TID * MAX_LEN_OF_MAC_TABLE)/2)//   (NUM_OF_TID*MAX_AID_BA + 32)	 //Block ACK recipient
+#define MAX_LEN_OF_BA_ORI_TABLE          ((NUM_OF_TID * MAX_LEN_OF_MAC_TABLE)/2)//   (NUM_OF_TID*MAX_AID_BA + 32)   // Block ACK originator
+#define MAX_LEN_OF_BSS_TABLE             64
+#define MAX_REORDERING_MPDU_NUM			 512
+
+// key related definitions
+#define SHARE_KEY_NUM                   4
+#define MAX_LEN_OF_SHARE_KEY            16    // byte count
+#define MAX_LEN_OF_PEER_KEY             16    // byte count
+#define PAIRWISE_KEY_NUM                64    // in MAC ASIC pairwise key table
+#define GROUP_KEY_NUM                   4
+#define PMK_LEN                         32
+#define WDS_PAIRWISE_KEY_OFFSET         60    // WDS links uses pairwise key#60 ~ 63 in ASIC pairwise key table
+#define	PMKID_NO                        4     // Number of PMKID saved supported
+#define MAX_LEN_OF_MLME_BUFFER          2048
+
+// power status related definitions
+#define PWR_ACTIVE                      0
+#define PWR_SAVE                        1
+#define PWR_MMPS                        2			//MIMO power save
+//#define PWR_UNKNOWN                   2
+
+// Auth and Assoc mode related definitions
+#define AUTH_MODE_OPEN                  0x00
+#define AUTH_MODE_KEY                   0x01
+//#define AUTH_MODE_AUTO_SWITCH         0x03
+//#define AUTH_MODE_DEAUTH              0x04
+//#define AUTH_MODE_UPLAYER             0x05 // reserved for 802.11i use
+
+// BSS Type definitions
+#define BSS_ADHOC                       0  // = Ndis802_11IBSS
+#define BSS_INFRA                       1  // = Ndis802_11Infrastructure
+#define BSS_ANY                         2  // = Ndis802_11AutoUnknown
+#define BSS_MONITOR			            3  // = Ndis802_11Monitor
+
+
+// Reason code definitions
+#define REASON_RESERVED                 0
+#define REASON_UNSPECIFY                1
+#define REASON_NO_LONGER_VALID          2
+#define REASON_DEAUTH_STA_LEAVING       3
+#define REASON_DISASSOC_INACTIVE        4
+#define REASON_DISASSPC_AP_UNABLE       5
+#define REASON_CLS2ERR                  6
+#define REASON_CLS3ERR                  7
+#define REASON_DISASSOC_STA_LEAVING     8
+#define REASON_STA_REQ_ASSOC_NOT_AUTH   9
+#define REASON_INVALID_IE               13
+#define REASON_MIC_FAILURE              14
+#define REASON_4_WAY_TIMEOUT            15
+#define REASON_GROUP_KEY_HS_TIMEOUT     16
+#define REASON_IE_DIFFERENT             17
+#define REASON_MCIPHER_NOT_VALID        18
+#define REASON_UCIPHER_NOT_VALID        19
+#define REASON_AKMP_NOT_VALID           20
+#define REASON_UNSUPPORT_RSNE_VER       21
+#define REASON_INVALID_RSNE_CAP         22
+#define REASON_8021X_AUTH_FAIL          23
+#define REASON_CIPHER_SUITE_REJECTED    24
+#define REASON_DECLINED                 37
+
+#define REASON_QOS_UNSPECIFY              32
+#define REASON_QOS_LACK_BANDWIDTH         33
+#define REASON_POOR_CHANNEL_CONDITION     34
+#define REASON_QOS_OUTSIDE_TXOP_LIMITION  35
+#define REASON_QOS_QSTA_LEAVING_QBSS      36
+#define REASON_QOS_UNWANTED_MECHANISM     37
+#define REASON_QOS_MECH_SETUP_REQUIRED    38
+#define REASON_QOS_REQUEST_TIMEOUT        39
+#define REASON_QOS_CIPHER_NOT_SUPPORT     45
+
+// Status code definitions
+#define MLME_SUCCESS                    0
+#define MLME_UNSPECIFY_FAIL             1
+#define MLME_CANNOT_SUPPORT_CAP         10
+#define MLME_REASSOC_DENY_ASSOC_EXIST   11
+#define MLME_ASSOC_DENY_OUT_SCOPE       12
+#define MLME_ALG_NOT_SUPPORT            13
+#define MLME_SEQ_NR_OUT_OF_SEQUENCE     14
+#define MLME_REJ_CHALLENGE_FAILURE      15
+#define MLME_REJ_TIMEOUT                  16
+#define MLME_ASSOC_REJ_UNABLE_HANDLE_STA  17
+#define MLME_ASSOC_REJ_DATA_RATE          18
+
+#define MLME_ASSOC_REJ_NO_EXT_RATE        22
+#define MLME_ASSOC_REJ_NO_EXT_RATE_PBCC   23
+#define MLME_ASSOC_REJ_NO_CCK_OFDM        24
+
+#define MLME_QOS_UNSPECIFY                32
+#define MLME_REQUEST_DECLINED             37
+#define MLME_REQUEST_WITH_INVALID_PARAM   38
+#define MLME_DLS_NOT_ALLOW_IN_QBSS        48
+#define MLME_DEST_STA_NOT_IN_QBSS         49
+#define MLME_DEST_STA_IS_NOT_A_QSTA       50
+
+#define MLME_INVALID_FORMAT             0x51
+#define MLME_FAIL_NO_RESOURCE           0x52
+#define MLME_STATE_MACHINE_REJECT       0x53
+#define MLME_MAC_TABLE_FAIL             0x54
+
+// IE code
+#define IE_SSID                         0
+#define IE_SUPP_RATES                   1
+#define IE_FH_PARM                      2
+#define IE_DS_PARM                      3
+#define IE_CF_PARM                      4
+#define IE_TIM                          5
+#define IE_IBSS_PARM                    6
+#define IE_COUNTRY                      7     // 802.11d
+#define IE_802_11D_REQUEST              10    // 802.11d
+#define IE_QBSS_LOAD                    11    // 802.11e d9
+#define IE_EDCA_PARAMETER               12    // 802.11e d9
+#define IE_TSPEC                        13    // 802.11e d9
+#define IE_TCLAS                        14    // 802.11e d9
+#define IE_SCHEDULE                     15    // 802.11e d9
+#define IE_CHALLENGE_TEXT               16
+#define IE_POWER_CONSTRAINT             32    // 802.11h d3.3
+#define IE_POWER_CAPABILITY             33    // 802.11h d3.3
+#define IE_TPC_REQUEST                  34    // 802.11h d3.3
+#define IE_TPC_REPORT                   35    // 802.11h d3.3
+#define IE_SUPP_CHANNELS                36    // 802.11h d3.3
+#define IE_CHANNEL_SWITCH_ANNOUNCEMENT  37    // 802.11h d3.3
+#define IE_MEASUREMENT_REQUEST          38    // 802.11h d3.3
+#define IE_MEASUREMENT_REPORT           39    // 802.11h d3.3
+#define IE_QUIET                        40    // 802.11h d3.3
+#define IE_IBSS_DFS                     41    // 802.11h d3.3
+#define IE_ERP                          42    // 802.11g
+#define IE_TS_DELAY                     43    // 802.11e d9
+#define IE_TCLAS_PROCESSING             44    // 802.11e d9
+#define IE_QOS_CAPABILITY               46    // 802.11e d6
+#define IE_HT_CAP                       45    // 802.11n d1. HT CAPABILITY. ELEMENT ID TBD
+#define IE_AP_CHANNEL_REPORT			51    // 802.11k d6
+#define IE_HT_CAP2                         52    // 802.11n d1. HT CAPABILITY. ELEMENT ID TBD
+#define IE_RSN                          48    // 802.11i d3.0
+#define IE_WPA2                         48    // WPA2
+#define IE_EXT_SUPP_RATES               50    // 802.11g
+#define IE_SUPP_REG_CLASS               59    // 802.11y. Supported regulatory classes.
+#define IE_EXT_CHANNEL_SWITCH_ANNOUNCEMENT	60	// 802.11n
+#define IE_ADD_HT                         61    // 802.11n d1. ADDITIONAL HT CAPABILITY. ELEMENT ID TBD
+#define IE_ADD_HT2                        53    // 802.11n d1. ADDITIONAL HT CAPABILITY. ELEMENT ID TBD
+
+
+// For 802.11n D3.03
+//#define IE_NEW_EXT_CHA_OFFSET             62    // 802.11n d1. New extension channel offset elemet
+#define IE_SECONDARY_CH_OFFSET		62	// 802.11n D3.03	Secondary Channel Offset element
+#define IE_WAPI							68		// WAPI information element
+#define IE_2040_BSS_COEXIST               72    // 802.11n D3.0.3
+#define IE_2040_BSS_INTOLERANT_REPORT     73    // 802.11n D3.03
+#define IE_OVERLAPBSS_SCAN_PARM           74    // 802.11n D3.03
+#define IE_EXT_CAPABILITY                127   // 802.11n D3.03
+
+
+#define IE_WPA                          221   // WPA
+#define IE_VENDOR_SPECIFIC              221   // Wifi WMM (WME)
+
+#define OUI_BROADCOM_HT              51   //
+#define OUI_BROADCOM_HTADD              52   //
+#define OUI_PREN_HT_CAP              51   //
+#define OUI_PREN_ADD_HT              52   //
+
+// CCX information
+#define IE_AIRONET_CKIP                 133   // CCX1.0 ID 85H for CKIP
+#define IE_AP_TX_POWER                  150   // CCX 2.0 for AP transmit power
+#define IE_MEASUREMENT_CAPABILITY       221   // CCX 2.0
+#define IE_CCX_V2                       221
+#define IE_AIRONET_IPADDRESS            149   // CCX ID 95H for IP Address
+#define IE_AIRONET_CCKMREASSOC          156   // CCX ID 9CH for CCKM Reassociation Request element
+#define CKIP_NEGOTIATION_LENGTH         30
+#define AIRONET_IPADDRESS_LENGTH        10
+#define AIRONET_CCKMREASSOC_LENGTH      24
+
+// ========================================================
+// MLME state machine definition
+// ========================================================
+
+// STA MLME state mahcines
+#define ASSOC_STATE_MACHINE             1
+#define AUTH_STATE_MACHINE              2
+#define AUTH_RSP_STATE_MACHINE          3
+#define SYNC_STATE_MACHINE              4
+#define MLME_CNTL_STATE_MACHINE         5
+#define WPA_PSK_STATE_MACHINE           6
+#define LEAP_STATE_MACHINE              7
+#define AIRONET_STATE_MACHINE           8
+#define ACTION_STATE_MACHINE           9
+
+// AP MLME state machines
+#define AP_ASSOC_STATE_MACHINE          11
+#define AP_AUTH_STATE_MACHINE           12
+#define AP_AUTH_RSP_STATE_MACHINE       13
+#define AP_SYNC_STATE_MACHINE           14
+#define AP_CNTL_STATE_MACHINE           15
+#define AP_WPA_STATE_MACHINE            16
+
+#ifdef QOS_DLS_SUPPORT
+#define DLS_STATE_MACHINE               26
+#endif // QOS_DLS_SUPPORT //
+
+//
+// STA's CONTROL/CONNECT state machine: states, events, total function #
+//
+#define CNTL_IDLE                       0
+#define CNTL_WAIT_DISASSOC              1
+#define CNTL_WAIT_JOIN                  2
+#define CNTL_WAIT_REASSOC               3
+#define CNTL_WAIT_START                 4
+#define CNTL_WAIT_AUTH                  5
+#define CNTL_WAIT_ASSOC                 6
+#define CNTL_WAIT_AUTH2                 7
+#define CNTL_WAIT_OID_LIST_SCAN         8
+#define CNTL_WAIT_OID_DISASSOC          9
+#ifdef RT2870
+#define CNTL_WAIT_SCAN_FOR_CONNECT      10
+#endif // RT2870 //
+
+#define MT2_ASSOC_CONF                  34
+#define MT2_AUTH_CONF                   35
+#define MT2_DEAUTH_CONF                 36
+#define MT2_DISASSOC_CONF               37
+#define MT2_REASSOC_CONF                38
+#define MT2_PWR_MGMT_CONF               39
+#define MT2_JOIN_CONF                   40
+#define MT2_SCAN_CONF                   41
+#define MT2_START_CONF                  42
+#define MT2_GET_CONF                    43
+#define MT2_SET_CONF                    44
+#define MT2_RESET_CONF                  45
+#define MT2_MLME_ROAMING_REQ            52
+
+#define CNTL_FUNC_SIZE                  1
+
+//
+// STA's ASSOC state machine: states, events, total function #
+//
+#define ASSOC_IDLE                      0
+#define ASSOC_WAIT_RSP                  1
+#define REASSOC_WAIT_RSP                2
+#define DISASSOC_WAIT_RSP               3
+#define MAX_ASSOC_STATE                 4
+
+#define ASSOC_MACHINE_BASE              0
+#define MT2_MLME_ASSOC_REQ              0
+#define MT2_MLME_REASSOC_REQ            1
+#define MT2_MLME_DISASSOC_REQ           2
+#define MT2_PEER_DISASSOC_REQ           3
+#define MT2_PEER_ASSOC_REQ              4
+#define MT2_PEER_ASSOC_RSP              5
+#define MT2_PEER_REASSOC_REQ            6
+#define MT2_PEER_REASSOC_RSP            7
+#define MT2_DISASSOC_TIMEOUT            8
+#define MT2_ASSOC_TIMEOUT               9
+#define MT2_REASSOC_TIMEOUT             10
+#define MAX_ASSOC_MSG                   11
+
+#define ASSOC_FUNC_SIZE                 (MAX_ASSOC_STATE * MAX_ASSOC_MSG)
+
+//
+// ACT state machine: states, events, total function #
+//
+#define ACT_IDLE                      0
+#define MAX_ACT_STATE                 1
+
+#define ACT_MACHINE_BASE              0
+
+//Those PEER_xx_CATE number is based on real Categary value in IEEE spec. Please don'es modify it by your self.
+//Category
+#define MT2_PEER_SPECTRUM_CATE              0
+#define MT2_PEER_QOS_CATE              1
+#define MT2_PEER_DLS_CATE             2
+#define MT2_PEER_BA_CATE             3
+#define MT2_PEER_PUBLIC_CATE             4
+#define MT2_PEER_RM_CATE             5
+#define MT2_PEER_HT_CATE             7	//	7.4.7
+#define MAX_PEER_CATE_MSG                   7
+#define MT2_MLME_ADD_BA_CATE             8
+#define MT2_MLME_ORI_DELBA_CATE             9
+#define MT2_MLME_REC_DELBA_CATE             10
+#define MT2_MLME_QOS_CATE              11
+#define MT2_MLME_DLS_CATE             12
+#define MT2_ACT_INVALID             13
+#define MAX_ACT_MSG                   14
+
+//Category field
+#define CATEGORY_SPECTRUM		0
+#define CATEGORY_QOS			1
+#define CATEGORY_DLS			2
+#define CATEGORY_BA			3
+#define CATEGORY_PUBLIC		4
+#define CATEGORY_RM			5
+#define CATEGORY_HT			7
+
+
+// DLS Action frame definition
+#define ACTION_DLS_REQUEST			0
+#define ACTION_DLS_RESPONSE			1
+#define ACTION_DLS_TEARDOWN			2
+
+//Spectrum  Action field value 802.11h 7.4.1
+#define SPEC_MRQ	0	// Request
+#define SPEC_MRP	1	//Report
+#define SPEC_TPCRQ	2
+#define SPEC_TPCRP	3
+#define SPEC_CHANNEL_SWITCH	4
+
+
+//BA  Action field value
+#define ADDBA_REQ	0
+#define ADDBA_RESP	1
+#define DELBA   2
+
+//Public's  Action field value in Public Category.  Some in 802.11y and some in 11n
+#define ACTION_BSS_2040_COEXIST				0	// 11n
+#define ACTION_DSE_ENABLEMENT					1	// 11y D9.0
+#define ACTION_DSE_DEENABLEMENT				2	// 11y D9.0
+#define ACTION_DSE_REG_LOCATION_ANNOUNCE	3	// 11y D9.0
+#define ACTION_EXT_CH_SWITCH_ANNOUNCE		4	// 11y D9.0
+#define ACTION_DSE_MEASUREMENT_REQ			5	// 11y D9.0
+#define ACTION_DSE_MEASUREMENT_REPORT		6	// 11y D9.0
+#define ACTION_MEASUREMENT_PILOT_ACTION		7  	// 11y D9.0
+#define ACTION_DSE_POWER_CONSTRAINT			8	// 11y D9.0
+
+
+//HT  Action field value
+#define NOTIFY_BW_ACTION				0
+#define SMPS_ACTION						1
+#define PSMP_ACTION   					2
+#define SETPCO_ACTION					3
+#define MIMO_CHA_MEASURE_ACTION			4
+#define MIMO_N_BEACONFORM				5
+#define MIMO_BEACONFORM					6
+#define ANTENNA_SELECT					7
+#define HT_INFO_EXCHANGE				8
+
+#define ACT_FUNC_SIZE                 (MAX_ACT_STATE * MAX_ACT_MSG)
+//
+// STA's AUTHENTICATION state machine: states, evvents, total function #
+//
+#define AUTH_REQ_IDLE                   0
+#define AUTH_WAIT_SEQ2                  1
+#define AUTH_WAIT_SEQ4                  2
+#define MAX_AUTH_STATE                  3
+
+#define AUTH_MACHINE_BASE               0
+#define MT2_MLME_AUTH_REQ               0
+#define MT2_PEER_AUTH_EVEN              1
+#define MT2_AUTH_TIMEOUT                2
+#define MAX_AUTH_MSG                    3
+
+#define AUTH_FUNC_SIZE                  (MAX_AUTH_STATE * MAX_AUTH_MSG)
+
+//
+// STA's AUTH_RSP state machine: states, events, total function #
+//
+#define AUTH_RSP_IDLE                   0
+#define AUTH_RSP_WAIT_CHAL              1
+#define MAX_AUTH_RSP_STATE              2
+
+#define AUTH_RSP_MACHINE_BASE           0
+#define MT2_AUTH_CHALLENGE_TIMEOUT      0
+#define MT2_PEER_AUTH_ODD               1
+#define MT2_PEER_DEAUTH                 2
+#define MAX_AUTH_RSP_MSG                3
+
+#define AUTH_RSP_FUNC_SIZE              (MAX_AUTH_RSP_STATE * MAX_AUTH_RSP_MSG)
+
+//
+// STA's SYNC state machine: states, events, total function #
+//
+#define SYNC_IDLE                       0  // merge NO_BSS,IBSS_IDLE,IBSS_ACTIVE and BSS in to 1 state
+#define JOIN_WAIT_BEACON                1
+#define SCAN_LISTEN                     2
+#define MAX_SYNC_STATE                  3
+
+#define SYNC_MACHINE_BASE               0
+#define MT2_MLME_SCAN_REQ               0
+#define MT2_MLME_JOIN_REQ               1
+#define MT2_MLME_START_REQ              2
+#define MT2_PEER_BEACON                 3
+#define MT2_PEER_PROBE_RSP              4
+#define MT2_PEER_ATIM                   5
+#define MT2_SCAN_TIMEOUT                6
+#define MT2_BEACON_TIMEOUT              7
+#define MT2_ATIM_TIMEOUT                8
+#define MT2_PEER_PROBE_REQ              9
+#define MAX_SYNC_MSG                    10
+
+#define SYNC_FUNC_SIZE                  (MAX_SYNC_STATE * MAX_SYNC_MSG)
+
+//Messages for the DLS state machine
+#define DLS_IDLE						0
+#define MAX_DLS_STATE					1
+
+#define DLS_MACHINE_BASE				0
+#define MT2_MLME_DLS_REQ			    0
+#define MT2_PEER_DLS_REQ			    1
+#define MT2_PEER_DLS_RSP			    2
+#define MT2_MLME_DLS_TEAR_DOWN		    3
+#define MT2_PEER_DLS_TEAR_DOWN		    4
+#define MAX_DLS_MSG				        5
+
+#define DLS_FUNC_SIZE					(MAX_DLS_STATE * MAX_DLS_MSG)
+
+//
+// STA's WPA-PSK State machine: states, events, total function #
+//
+#define WPA_PSK_IDLE					0
+#define MAX_WPA_PSK_STATE				1
+
+#define WPA_MACHINE_BASE                0
+#define MT2_EAPPacket                   0
+#define MT2_EAPOLStart                  1
+#define MT2_EAPOLLogoff                 2
+#define MT2_EAPOLKey                    3
+#define MT2_EAPOLASFAlert               4
+#define MAX_WPA_PSK_MSG                 5
+
+#define	WPA_PSK_FUNC_SIZE				(MAX_WPA_PSK_STATE * MAX_WPA_PSK_MSG)
+
+//
+// STA's CISCO-AIRONET State machine: states, events, total function #
+//
+#define AIRONET_IDLE					0
+#define	AIRONET_SCANNING				1
+#define MAX_AIRONET_STATE				2
+
+#define AIRONET_MACHINE_BASE		    0
+#define MT2_AIRONET_MSG				    0
+#define MT2_AIRONET_SCAN_REQ		    1
+#define MT2_AIRONET_SCAN_DONE		    2
+#define MAX_AIRONET_MSG				    3
+
+#define	AIRONET_FUNC_SIZE				(MAX_AIRONET_STATE * MAX_AIRONET_MSG)
+
+//
+// AP's CONTROL/CONNECT state machine: states, events, total function #
+//
+#define AP_CNTL_FUNC_SIZE               1
+
+//
+// AP's ASSOC state machine: states, events, total function #
+//
+#define AP_ASSOC_IDLE                   0
+#define AP_MAX_ASSOC_STATE              1
+
+#define AP_ASSOC_MACHINE_BASE           0
+#define APMT2_MLME_DISASSOC_REQ         0
+#define APMT2_PEER_DISASSOC_REQ         1
+#define APMT2_PEER_ASSOC_REQ            2
+#define APMT2_PEER_REASSOC_REQ          3
+#define APMT2_CLS3ERR                   4
+#define AP_MAX_ASSOC_MSG                5
+
+#define AP_ASSOC_FUNC_SIZE              (AP_MAX_ASSOC_STATE * AP_MAX_ASSOC_MSG)
+
+//
+// AP's AUTHENTICATION state machine: states, events, total function #
+//
+#define AP_AUTH_REQ_IDLE                0
+#define AP_MAX_AUTH_STATE               1
+
+#define AP_AUTH_MACHINE_BASE            0
+#define APMT2_MLME_DEAUTH_REQ           0
+#define APMT2_CLS2ERR                   1
+#define AP_MAX_AUTH_MSG                 2
+
+#define AP_AUTH_FUNC_SIZE               (AP_MAX_AUTH_STATE * AP_MAX_AUTH_MSG)
+
+//
+// AP's AUTH-RSP state machine: states, events, total function #
+//
+#define AP_AUTH_RSP_IDLE                0
+#define AP_MAX_AUTH_RSP_STATE           1
+
+#define AP_AUTH_RSP_MACHINE_BASE        0
+#define APMT2_AUTH_CHALLENGE_TIMEOUT    0
+#define APMT2_PEER_AUTH_ODD             1
+#define APMT2_PEER_DEAUTH               2
+#define AP_MAX_AUTH_RSP_MSG             3
+
+#define AP_AUTH_RSP_FUNC_SIZE           (AP_MAX_AUTH_RSP_STATE * AP_MAX_AUTH_RSP_MSG)
+
+//
+// AP's SYNC state machine: states, events, total function #
+//
+#define AP_SYNC_IDLE                    0
+#define AP_SCAN_LISTEN					1
+#define AP_MAX_SYNC_STATE               2
+
+#define AP_SYNC_MACHINE_BASE            0
+#define APMT2_PEER_PROBE_REQ            0
+#define APMT2_PEER_BEACON               1
+#define APMT2_MLME_SCAN_REQ				2
+#define APMT2_PEER_PROBE_RSP			3
+#define APMT2_SCAN_TIMEOUT				4
+#define APMT2_MLME_SCAN_CNCL			5
+#define AP_MAX_SYNC_MSG                 6
+
+#define AP_SYNC_FUNC_SIZE               (AP_MAX_SYNC_STATE * AP_MAX_SYNC_MSG)
+
+//
+// AP's WPA state machine: states, events, total function #
+//
+#define AP_WPA_PTK                      0
+#define AP_MAX_WPA_PTK_STATE            1
+
+#define AP_WPA_MACHINE_BASE             0
+#define APMT2_EAPPacket                 0
+#define APMT2_EAPOLStart                1
+#define APMT2_EAPOLLogoff               2
+#define APMT2_EAPOLKey                  3
+#define APMT2_EAPOLASFAlert             4
+#define AP_MAX_WPA_MSG                  5
+
+#define AP_WPA_FUNC_SIZE                (AP_MAX_WPA_PTK_STATE * AP_MAX_WPA_MSG)
+
+#ifdef APCLI_SUPPORT
+//ApCli authentication state machine
+#define APCLI_AUTH_REQ_IDLE                0
+#define APCLI_AUTH_WAIT_SEQ2               1
+#define APCLI_AUTH_WAIT_SEQ4               2
+#define APCLI_MAX_AUTH_STATE               3
+
+#define APCLI_AUTH_MACHINE_BASE            0
+#define APCLI_MT2_MLME_AUTH_REQ            0
+#define APCLI_MT2_MLME_DEAUTH_REQ          1
+#define APCLI_MT2_PEER_AUTH_EVEN           2
+#define APCLI_MT2_PEER_DEAUTH              3
+#define APCLI_MT2_AUTH_TIMEOUT             4
+#define APCLI_MAX_AUTH_MSG                 5
+
+#define APCLI_AUTH_FUNC_SIZE               (APCLI_MAX_AUTH_STATE * APCLI_MAX_AUTH_MSG)
+
+//ApCli association state machine
+#define APCLI_ASSOC_IDLE                   0
+#define APCLI_ASSOC_WAIT_RSP               1
+#define APCLI_MAX_ASSOC_STATE              2
+
+#define APCLI_ASSOC_MACHINE_BASE           0
+#define APCLI_MT2_MLME_ASSOC_REQ           0
+#define APCLI_MT2_MLME_DISASSOC_REQ        1
+#define APCLI_MT2_PEER_DISASSOC_REQ        2
+#define APCLI_MT2_PEER_ASSOC_RSP           3
+#define APCLI_MT2_ASSOC_TIMEOUT            4
+#define APCLI_MAX_ASSOC_MSG                5
+
+#define APCLI_ASSOC_FUNC_SIZE              (APCLI_MAX_ASSOC_STATE * APCLI_MAX_ASSOC_MSG)
+
+//ApCli sync state machine
+#define APCLI_SYNC_IDLE                   0  // merge NO_BSS,IBSS_IDLE,IBSS_ACTIVE and BSS in to 1 state
+#define APCLI_JOIN_WAIT_PROBE_RSP         1
+#define APCLI_MAX_SYNC_STATE              2
+
+#define APCLI_SYNC_MACHINE_BASE           0
+#define APCLI_MT2_MLME_PROBE_REQ          0
+#define APCLI_MT2_PEER_PROBE_RSP          1
+#define APCLI_MT2_PROBE_TIMEOUT           2
+#define APCLI_MAX_SYNC_MSG                3
+
+#define APCLI_SYNC_FUNC_SIZE              (APCLI_MAX_SYNC_STATE * APCLI_MAX_SYNC_MSG)
+
+//ApCli ctrl state machine
+#define APCLI_CTRL_DISCONNECTED           0  // merge NO_BSS,IBSS_IDLE,IBSS_ACTIVE and BSS in to 1 state
+#define APCLI_CTRL_PROBE                  1
+#define APCLI_CTRL_AUTH                   2
+#define APCLI_CTRL_AUTH_2                 3
+#define APCLI_CTRL_ASSOC                  4
+#define APCLI_CTRL_DEASSOC                5
+#define APCLI_CTRL_CONNECTED              6
+#define APCLI_MAX_CTRL_STATE              7
+
+#define APCLI_CTRL_MACHINE_BASE           0
+#define APCLI_CTRL_JOIN_REQ               0
+#define APCLI_CTRL_PROBE_RSP              1
+#define APCLI_CTRL_AUTH_RSP               2
+#define APCLI_CTRL_DISCONNECT_REQ         3
+#define APCLI_CTRL_PEER_DISCONNECT_REQ    4
+#define APCLI_CTRL_ASSOC_RSP              5
+#define APCLI_CTRL_DEASSOC_RSP            6
+#define APCLI_CTRL_JOIN_REQ_TIMEOUT       7
+#define APCLI_CTRL_AUTH_REQ_TIMEOUT       8
+#define APCLI_CTRL_ASSOC_REQ_TIMEOUT      9
+#define APCLI_MAX_CTRL_MSG                10
+
+#define APCLI_CTRL_FUNC_SIZE              (APCLI_MAX_CTRL_STATE * APCLI_MAX_CTRL_MSG)
+
+#if 0	// remove those variables by AlbertY
+// ApCli WPA state machine
+#define APCLI_WPA_PSK_IDLE				0
+#define APCLI_MAX_WPA_PSK_STATE			1
+
+// ApCli WPA MSG Type
+#define APCLI_WPA_MACHINE_BASE			0
+#define APCLI_MT2_EAPPacket				0
+#define APCLI_MT2_EAPOLStart			1
+#define APCLI_MT2_EAPOLLogoff			2
+#define APCLI_MT2_EAPOLKey				3
+#define APCLI_MT2_EAPOLASFAlert			4
+#define APCLI_MAX_WPA_PSK_MSG			5
+
+#define	APCLI_WPA_PSK_FUNC_SIZE			(APCLI_MAX_WPA_PSK_STATE * APCLI_MAX_WPA_PSK_MSG)
+#endif // end - 0 //
+
+#endif	// APCLI_SUPPORT //
+
+
+// =============================================================================
+
+// value domain of 802.11 header FC.Tyte, which is b3..b2 of the 1st-byte of MAC header
+#define BTYPE_MGMT                  0
+#define BTYPE_CNTL                  1
+#define BTYPE_DATA                  2
+
+// value domain of 802.11 MGMT frame's FC.subtype, which is b7..4 of the 1st-byte of MAC header
+#define SUBTYPE_ASSOC_REQ           0
+#define SUBTYPE_ASSOC_RSP           1
+#define SUBTYPE_REASSOC_REQ         2
+#define SUBTYPE_REASSOC_RSP         3
+#define SUBTYPE_PROBE_REQ           4
+#define SUBTYPE_PROBE_RSP           5
+#define SUBTYPE_BEACON              8
+#define SUBTYPE_ATIM                9
+#define SUBTYPE_DISASSOC            10
+#define SUBTYPE_AUTH                11
+#define SUBTYPE_DEAUTH              12
+#define SUBTYPE_ACTION              13
+#define SUBTYPE_ACTION_NO_ACK              14
+
+// value domain of 802.11 CNTL frame's FC.subtype, which is b7..4 of the 1st-byte of MAC header
+#define SUBTYPE_WRAPPER       	7
+#define SUBTYPE_BLOCK_ACK_REQ       8
+#define SUBTYPE_BLOCK_ACK           9
+#define SUBTYPE_PS_POLL             10
+#define SUBTYPE_RTS                 11
+#define SUBTYPE_CTS                 12
+#define SUBTYPE_ACK                 13
+#define SUBTYPE_CFEND               14
+#define SUBTYPE_CFEND_CFACK         15
+
+// value domain of 802.11 DATA frame's FC.subtype, which is b7..4 of the 1st-byte of MAC header
+#define SUBTYPE_DATA                0
+#define SUBTYPE_DATA_CFACK          1
+#define SUBTYPE_DATA_CFPOLL         2
+#define SUBTYPE_DATA_CFACK_CFPOLL   3
+#define SUBTYPE_NULL_FUNC           4
+#define SUBTYPE_CFACK               5
+#define SUBTYPE_CFPOLL              6
+#define SUBTYPE_CFACK_CFPOLL        7
+#define SUBTYPE_QDATA               8
+#define SUBTYPE_QDATA_CFACK         9
+#define SUBTYPE_QDATA_CFPOLL        10
+#define SUBTYPE_QDATA_CFACK_CFPOLL  11
+#define SUBTYPE_QOS_NULL            12
+#define SUBTYPE_QOS_CFACK           13
+#define SUBTYPE_QOS_CFPOLL          14
+#define SUBTYPE_QOS_CFACK_CFPOLL    15
+
+// ACK policy of QOS Control field bit 6:5
+#define NORMAL_ACK                  0x00  // b6:5 = 00
+#define NO_ACK                      0x20  // b6:5 = 01
+#define NO_EXPLICIT_ACK             0x40  // b6:5 = 10
+#define BLOCK_ACK                   0x60  // b6:5 = 11
+
+//
+// rtmp_data.c use these definition
+//
+#define LENGTH_802_11               24
+#define LENGTH_802_11_AND_H         30
+#define LENGTH_802_11_CRC_H         34
+#define LENGTH_802_11_CRC           28
+#define LENGTH_802_11_WITH_ADDR4    30
+#define LENGTH_802_3                14
+#define LENGTH_802_3_TYPE           2
+#define LENGTH_802_1_H              8
+#define LENGTH_EAPOL_H              4
+#define LENGTH_WMMQOS_H				2
+#define LENGTH_CRC                  4
+#define MAX_SEQ_NUMBER              0x0fff
+#define LENGTH_802_3_NO_TYPE		12
+#define LENGTH_802_1Q				4 /* VLAN related */
+
+// STA_CSR4.field.TxResult
+#define TX_RESULT_SUCCESS           0
+#define TX_RESULT_ZERO_LENGTH       1
+#define TX_RESULT_UNDER_RUN         2
+#define TX_RESULT_OHY_ERROR         4
+#define TX_RESULT_RETRY_FAIL        6
+
+// All PHY rate summary in TXD
+// Preamble MODE in TxD
+#define MODE_CCK	0
+#define MODE_OFDM   1
+#ifdef DOT11_N_SUPPORT
+#define MODE_HTMIX	2
+#define MODE_HTGREENFIELD	3
+#endif // DOT11_N_SUPPORT //
+// MCS for CCK.  BW.SGI.STBC are reserved
+#define MCS_LONGP_RATE_1                      0	 // long preamble CCK 1Mbps
+#define MCS_LONGP_RATE_2                      1	// long preamble CCK 1Mbps
+#define MCS_LONGP_RATE_5_5                    2
+#define MCS_LONGP_RATE_11                     3
+#define MCS_SHORTP_RATE_1                      4	 // long preamble CCK 1Mbps. short is forbidden in 1Mbps
+#define MCS_SHORTP_RATE_2                      5	// short preamble CCK 2Mbps
+#define MCS_SHORTP_RATE_5_5                    6
+#define MCS_SHORTP_RATE_11                     7
+// To send duplicate legacy OFDM. set BW=BW_40.  SGI.STBC are reserved
+#define MCS_RATE_6                      0   // legacy OFDM
+#define MCS_RATE_9                      1   // OFDM
+#define MCS_RATE_12                     2   // OFDM
+#define MCS_RATE_18                     3   // OFDM
+#define MCS_RATE_24                     4  // OFDM
+#define MCS_RATE_36                     5   // OFDM
+#define MCS_RATE_48                     6  // OFDM
+#define MCS_RATE_54                     7 // OFDM
+// HT
+#define MCS_0		0	// 1S
+#define MCS_1		1
+#define MCS_2		2
+#define MCS_3		3
+#define MCS_4		4
+#define MCS_5		5
+#define MCS_6		6
+#define MCS_7		7
+#define MCS_8		8	// 2S
+#define MCS_9		9
+#define MCS_10		10
+#define MCS_11		11
+#define MCS_12		12
+#define MCS_13		13
+#define MCS_14		14
+#define MCS_15		15
+#define MCS_16		16	// 3*3
+#define MCS_17		17
+#define MCS_18		18
+#define MCS_19		19
+#define MCS_20		20
+#define MCS_21		21
+#define MCS_22		22
+#define MCS_23		23
+#define MCS_32		32
+#define MCS_AUTO		33
+
+#ifdef DOT11_N_SUPPORT
+// OID_HTPHYMODE
+// MODE
+#define HTMODE_MM	0
+#define HTMODE_GF	1
+#endif // DOT11_N_SUPPORT //
+
+// Fixed Tx MODE - HT, CCK or OFDM
+#define FIXED_TXMODE_HT		0
+#define FIXED_TXMODE_CCK	1
+#define FIXED_TXMODE_OFDM 	2
+// BW
+#define BW_20		BAND_WIDTH_20
+#define BW_40		BAND_WIDTH_40
+#define BW_BOTH		BAND_WIDTH_BOTH
+#define BW_10		BAND_WIDTH_10	// 802.11j has 10MHz. This definition is for internal usage. doesn't fill in the IE or other field.
+
+#ifdef DOT11_N_SUPPORT
+// SHORTGI
+#define GI_400		GAP_INTERVAL_400	// only support in HT mode
+#define GI_BOTH		GAP_INTERVAL_BOTH
+#endif // DOT11_N_SUPPORT //
+#define GI_800		GAP_INTERVAL_800
+// STBC
+#define STBC_NONE	0
+#ifdef DOT11_N_SUPPORT
+#define STBC_USE	1	// limited use in rt2860b phy
+#define RXSTBC_ONE	1	// rx support of one spatial stream
+#define RXSTBC_TWO	2	// rx support of 1 and 2 spatial stream
+#define RXSTBC_THR	3	// rx support of 1~3 spatial stream
+// MCS FEEDBACK
+#define MCSFBK_NONE	0  // not support mcs feedback /
+#define MCSFBK_RSV	1	// reserved
+#define MCSFBK_UNSOLICIT	2	// only support unsolict mcs feedback
+#define MCSFBK_MRQ	3	// response to both MRQ and unsolict mcs feedback
+
+// MIMO power safe
+#define	MMPS_STATIC	0
+#define	MMPS_DYNAMIC		1
+#define   MMPS_RSV		2
+#define MMPS_ENABLE		3
+
+
+// A-MSDU size
+#define	AMSDU_0	0
+#define	AMSDU_1		1
+
+#endif // DOT11_N_SUPPORT //
+
+// MCS use 7 bits
+#define TXRATEMIMO		0x80
+#define TXRATEMCS		0x7F
+#define TXRATEOFDM		0x7F
+#define RATE_1                      0
+#define RATE_2                      1
+#define RATE_5_5                    2
+#define RATE_11                     3
+#define RATE_6                      4   // OFDM
+#define RATE_9                      5   // OFDM
+#define RATE_12                     6   // OFDM
+#define RATE_18                     7   // OFDM
+#define RATE_24                     8   // OFDM
+#define RATE_36                     9   // OFDM
+#define RATE_48                     10  // OFDM
+#define RATE_54                     11  // OFDM
+#define RATE_FIRST_OFDM_RATE        RATE_6
+#define RATE_LAST_OFDM_RATE        	RATE_54
+#define RATE_6_5                    12  // HT mix
+#define RATE_13                     13  // HT mix
+#define RATE_19_5                   14  // HT mix
+#define RATE_26                     15  // HT mix
+#define RATE_39                     16  // HT mix
+#define RATE_52                     17  // HT mix
+#define RATE_58_5                   18  // HT mix
+#define RATE_65                     19  // HT mix
+#define RATE_78                     20  // HT mix
+#define RATE_104                    21  // HT mix
+#define RATE_117                    22  // HT mix
+#define RATE_130                    23  // HT mix
+//#define RATE_AUTO_SWITCH            255 // for StaCfg.FixedTxRate only
+#define HTRATE_0                      12
+#define RATE_FIRST_MM_RATE        HTRATE_0
+#define RATE_FIRST_HT_RATE        HTRATE_0
+#define RATE_LAST_HT_RATE        HTRATE_0
+
+// pTxWI->txop
+#define IFS_HTTXOP                 0	// The txop will be handles by ASIC.
+#define IFS_PIFS                    1
+#define IFS_SIFS                    2
+#define IFS_BACKOFF                 3
+
+// pTxD->RetryMode
+#define LONG_RETRY                  1
+#define SHORT_RETRY                 0
+
+// Country Region definition
+#define REGION_MINIMUM_BG_BAND            0
+#define REGION_0_BG_BAND                  0       // 1-11
+#define REGION_1_BG_BAND                  1       // 1-13
+#define REGION_2_BG_BAND                  2       // 10-11
+#define REGION_3_BG_BAND                  3       // 10-13
+#define REGION_4_BG_BAND                  4       // 14
+#define REGION_5_BG_BAND                  5       // 1-14
+#define REGION_6_BG_BAND                  6       // 3-9
+#define REGION_7_BG_BAND                  7       // 5-13
+#define REGION_31_BG_BAND                 31       // 5-13
+#define REGION_MAXIMUM_BG_BAND            7
+
+#define REGION_MINIMUM_A_BAND             0
+#define REGION_0_A_BAND                   0       // 36, 40, 44, 48, 52, 56, 60, 64, 149, 153, 157, 161, 165
+#define REGION_1_A_BAND                   1       // 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140
+#define REGION_2_A_BAND                   2       // 36, 40, 44, 48, 52, 56, 60, 64
+#define REGION_3_A_BAND                   3       // 52, 56, 60, 64, 149, 153, 157, 161
+#define REGION_4_A_BAND                   4       // 149, 153, 157, 161, 165
+#define REGION_5_A_BAND                   5       // 149, 153, 157, 161
+#define REGION_6_A_BAND                   6       // 36, 40, 44, 48
+#define REGION_7_A_BAND                   7       // 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140, 149, 153, 157, 161, 165
+#define REGION_8_A_BAND                   8       // 52, 56, 60, 64
+#define REGION_9_A_BAND                   9       // 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 132, 136, 140, 149, 153, 157, 161, 165
+#define REGION_10_A_BAND                  10	  // 36, 40, 44, 48, 149, 153, 157, 161, 165
+#define REGION_11_A_BAND                  11	  // 36, 40, 44, 48, 52, 56, 60, 64, 100, 104, 108, 112, 116, 120, 149, 153, 157, 161
+#define REGION_MAXIMUM_A_BAND             11
+
+// pTxD->CipherAlg
+#define CIPHER_NONE                 0
+#define CIPHER_WEP64                1
+#define CIPHER_WEP128               2
+#define CIPHER_TKIP                 3
+#define CIPHER_AES                  4
+#define CIPHER_CKIP64               5
+#define CIPHER_CKIP128              6
+#define CIPHER_TKIP_NO_MIC          7       // MIC appended by driver: not a valid value in hardware key table
+#define CIPHER_SMS4					8
+
+// value domain of pAd->RfIcType
+#define RFIC_2820                   1       // 2.4G 2T3R
+#define RFIC_2850                   2       // 2.4G/5G 2T3R
+#define RFIC_2720                   3       // 2.4G 1T2R
+#define RFIC_2750                   4       // 2.4G/5G 1T2R
+#define RFIC_3020                   5       // 2.4G 1T1R
+#define RFIC_2020                   6       // 2.4G B/G
+
+// LED Status.
+#define LED_LINK_DOWN               0
+#define LED_LINK_UP                 1
+#define LED_RADIO_OFF               2
+#define LED_RADIO_ON                3
+#define LED_HALT                    4
+#define LED_WPS                     5
+#define LED_ON_SITE_SURVEY          6
+#define LED_POWER_UP                7
+
+// value domain of pAd->LedCntl.LedMode and E2PROM
+#define LED_MODE_DEFAULT            0
+#define LED_MODE_TWO_LED			1
+#define LED_MODE_SIGNAL_STREGTH		8  // EEPROM define =8
+
+// RC4 init value, used fro WEP & TKIP
+#define PPPINITFCS32                0xffffffff   /* Initial FCS value */
+
+// value domain of pAd->StaCfg.PortSecured. 802.1X controlled port definition
+#define WPA_802_1X_PORT_SECURED     1
+#define WPA_802_1X_PORT_NOT_SECURED 2
+
+#define PAIRWISE_KEY                1
+#define GROUP_KEY                   2
+
+//definition of DRS
+#define MAX_STEP_OF_TX_RATE_SWITCH	32
+
+
+// pre-allocated free NDIS PACKET/BUFFER poll for internal usage
+#define MAX_NUM_OF_FREE_NDIS_PACKET 128
+
+//Block ACK
+#define MAX_TX_REORDERBUF   64
+#define MAX_RX_REORDERBUF   64
+#define DEFAULT_TX_TIMEOUT   30
+#define DEFAULT_RX_TIMEOUT   30
+
+// definition of Recipient or Originator
+#define I_RECIPIENT                  TRUE
+#define I_ORIGINATOR                   FALSE
+
+#define DEFAULT_BBP_TX_POWER        0
+#define DEFAULT_RF_TX_POWER         5
+
+#define MAX_INI_BUFFER_SIZE			4096
+#define MAX_PARAM_BUFFER_SIZE		(2048) // enough for ACL (18*64)
+											//18 : the length of Mac address acceptable format "01:02:03:04:05:06;")
+											//64 : MAX_NUM_OF_ACL_LIST
+// definition of pAd->OpMode
+#define OPMODE_STA                  0
+#define OPMODE_AP                   1
+//#define OPMODE_L3_BRG               2       // as AP and STA at the same time
+
+#ifdef RT_BIG_ENDIAN
+#define DIR_READ                    0
+#define DIR_WRITE                   1
+#define TYPE_TXD                    0
+#define TYPE_RXD                    1
+#define TYPE_TXINFO					0
+#define TYPE_RXINFO					1
+#define TYPE_TXWI					0
+#define TYPE_RXWI					1
+#endif
+
+// ========================= AP rtmp_def.h ===========================
+// value domain for pAd->EventTab.Log[].Event
+#define EVENT_RESET_ACCESS_POINT    0 // Log = "hh:mm:ss   Restart Access Point"
+#define EVENT_ASSOCIATED            1 // Log = "hh:mm:ss   STA 00:01:02:03:04:05 associated"
+#define EVENT_DISASSOCIATED         2 // Log = "hh:mm:ss   STA 00:01:02:03:04:05 left this BSS"
+#define EVENT_AGED_OUT              3 // Log = "hh:mm:ss   STA 00:01:02:03:04:05 was aged-out and removed from this BSS"
+#define EVENT_COUNTER_M             4
+#define EVENT_INVALID_PSK           5
+#define EVENT_MAX_EVENT_TYPE        6
+// ==== end of AP rtmp_def.h ============
+
+// definition RSSI Number
+#define RSSI_0					0
+#define RSSI_1					1
+#define RSSI_2					2
+
+// definition of radar detection
+#define RD_NORMAL_MODE				0	// Not found radar signal
+#define RD_SWITCHING_MODE			1	// Found radar signal, and doing channel switch
+#define RD_SILENCE_MODE				2	// After channel switch, need to be silence a while to ensure radar not found
+
+//Driver defined cid for mapping status and command.
+#define  SLEEPCID	0x11
+#define  WAKECID	0x22
+#define  QUERYPOWERCID	0x33
+#define  OWNERMCU	0x1
+#define  OWNERCPU	0x0
+
+// MBSSID definition
+#define ENTRY_NOT_FOUND             0xFF
+
+
+/* After Linux 2.6.9,
+ * VLAN module use Private (from user) interface flags (netdevice->priv_flags).
+ * #define IFF_802_1Q_VLAN 0x1         --    802.1Q VLAN device.  in if.h
+ * ref to ip_sabotage_out() [ out->priv_flags & IFF_802_1Q_VLAN ] in br_netfilter.c
+ *
+ * For this reason, we MUST use EVEN value in priv_flags
+ */
+#define INT_MAIN                    0x0100
+#define INT_MBSSID                  0x0200
+#define INT_WDS                     0x0300
+#define INT_APCLI                   0x0400
+#define INT_MESH                   	0x0500
+
+// Use bitmap to allow coexist of ATE_TXFRAME and ATE_RXFRAME(i.e.,to support LoopBack mode)
+#ifdef RALINK_ATE
+#define	ATE_START                   0x00   // Start ATE
+#define	ATE_STOP                    0x80   // Stop ATE
+#define	ATE_TXCONT                  0x05   // Continuous Transmit
+#define	ATE_TXCARR                  0x09   // Transmit Carrier
+#define	ATE_TXCARRSUPP              0x11   // Transmit Carrier Suppression
+#define	ATE_TXFRAME                 0x01   // Transmit Frames
+#define	ATE_RXFRAME                 0x02   // Receive Frames
+#ifdef RALINK_28xx_QA
+#define ATE_TXSTOP                  0xe2   // Stop Transmition(i.e., TXCONT, TXCARR, TXCARRSUPP, and TXFRAME)
+#define ATE_RXSTOP					0xfd   // Stop receiving Frames
+#define	BBP22_TXFRAME     			0x00   // Transmit Frames
+#define	BBP22_TXCONT_OR_CARRSUPP    0x80   // Continuous Transmit or Carrier Suppression
+#define	BBP22_TXCARR                0xc1   // Transmit Carrier
+#define	BBP24_TXCONT                0x00   // Continuous Transmit
+#define	BBP24_CARRSUPP              0x01   // Carrier Suppression
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+
+// WEP Key TYPE
+#define WEP_HEXADECIMAL_TYPE    0
+#define WEP_ASCII_TYPE          1
+
+
+
+// WIRELESS EVENTS definition
+/* Max number of char in custom event, refer to wireless_tools.28/wireless.20.h */
+#define IW_CUSTOM_MAX_LEN				  			255	/* In bytes */
+
+// For system event - start
+#define	IW_SYS_EVENT_FLAG_START                     0x0200
+#define	IW_ASSOC_EVENT_FLAG                         0x0200
+#define	IW_DISASSOC_EVENT_FLAG                      0x0201
+#define	IW_DEAUTH_EVENT_FLAG                      	0x0202
+#define	IW_AGEOUT_EVENT_FLAG                      	0x0203
+#define	IW_COUNTER_MEASURES_EVENT_FLAG              0x0204
+#define	IW_REPLAY_COUNTER_DIFF_EVENT_FLAG           0x0205
+#define	IW_RSNIE_DIFF_EVENT_FLAG           			0x0206
+#define	IW_MIC_DIFF_EVENT_FLAG           			0x0207
+#define IW_ICV_ERROR_EVENT_FLAG						0x0208
+#define IW_MIC_ERROR_EVENT_FLAG						0x0209
+#define IW_GROUP_HS_TIMEOUT_EVENT_FLAG				0x020A
+#define	IW_PAIRWISE_HS_TIMEOUT_EVENT_FLAG			0x020B
+#define IW_RSNIE_SANITY_FAIL_EVENT_FLAG				0x020C
+#define IW_SET_KEY_DONE_WPA1_EVENT_FLAG				0x020D
+#define IW_SET_KEY_DONE_WPA2_EVENT_FLAG				0x020E
+#define IW_STA_LINKUP_EVENT_FLAG					0x020F
+#define IW_STA_LINKDOWN_EVENT_FLAG					0x0210
+#define IW_SCAN_COMPLETED_EVENT_FLAG				0x0211
+#define IW_SCAN_ENQUEUE_FAIL_EVENT_FLAG				0x0212
+// if add new system event flag, please upadte the IW_SYS_EVENT_FLAG_END
+#define	IW_SYS_EVENT_FLAG_END                       0x0212
+#define	IW_SYS_EVENT_TYPE_NUM						(IW_SYS_EVENT_FLAG_END - IW_SYS_EVENT_FLAG_START + 1)
+// For system event - end
+
+// For spoof attack event - start
+#define	IW_SPOOF_EVENT_FLAG_START                   0x0300
+#define IW_CONFLICT_SSID_EVENT_FLAG					0x0300
+#define IW_SPOOF_ASSOC_RESP_EVENT_FLAG				0x0301
+#define IW_SPOOF_REASSOC_RESP_EVENT_FLAG			0x0302
+#define IW_SPOOF_PROBE_RESP_EVENT_FLAG				0x0303
+#define IW_SPOOF_BEACON_EVENT_FLAG					0x0304
+#define IW_SPOOF_DISASSOC_EVENT_FLAG				0x0305
+#define IW_SPOOF_AUTH_EVENT_FLAG					0x0306
+#define IW_SPOOF_DEAUTH_EVENT_FLAG					0x0307
+#define IW_SPOOF_UNKNOWN_MGMT_EVENT_FLAG			0x0308
+#define IW_REPLAY_ATTACK_EVENT_FLAG					0x0309
+// if add new spoof attack event flag, please upadte the IW_SPOOF_EVENT_FLAG_END
+#define	IW_SPOOF_EVENT_FLAG_END                     0x0309
+#define	IW_SPOOF_EVENT_TYPE_NUM						(IW_SPOOF_EVENT_FLAG_END - IW_SPOOF_EVENT_FLAG_START + 1)
+// For spoof attack event - end
+
+// For flooding attack event - start
+#define	IW_FLOOD_EVENT_FLAG_START                   0x0400
+#define IW_FLOOD_AUTH_EVENT_FLAG					0x0400
+#define IW_FLOOD_ASSOC_REQ_EVENT_FLAG				0x0401
+#define IW_FLOOD_REASSOC_REQ_EVENT_FLAG				0x0402
+#define IW_FLOOD_PROBE_REQ_EVENT_FLAG				0x0403
+#define IW_FLOOD_DISASSOC_EVENT_FLAG				0x0404
+#define IW_FLOOD_DEAUTH_EVENT_FLAG					0x0405
+#define IW_FLOOD_EAP_REQ_EVENT_FLAG					0x0406
+// if add new flooding attack event flag, please upadte the IW_FLOOD_EVENT_FLAG_END
+#define	IW_FLOOD_EVENT_FLAG_END                   	0x0406
+#define	IW_FLOOD_EVENT_TYPE_NUM						(IW_FLOOD_EVENT_FLAG_END - IW_FLOOD_EVENT_FLAG_START + 1)
+// For flooding attack - end
+
+// End - WIRELESS EVENTS definition
+
+#ifdef CONFIG_STA_SUPPORT
+// definition for DLS, kathy
+#define	MAX_NUM_OF_INIT_DLS_ENTRY   1
+#define	MAX_NUM_OF_DLS_ENTRY        MAX_NUMBER_OF_DLS_ENTRY
+
+//Block ACK , rt2860, kathy
+#define MAX_TX_REORDERBUF		64
+#define MAX_RX_REORDERBUF		64
+#define DEFAULT_TX_TIMEOUT		30
+#define DEFAULT_RX_TIMEOUT		30
+#ifndef CONFIG_AP_SUPPORT
+#define MAX_BARECI_SESSION		8
+#endif
+
+#ifndef IW_ESSID_MAX_SIZE
+/* Maximum size of the ESSID and pAd->nickname strings */
+#define IW_ESSID_MAX_SIZE   		32
+#endif
+#endif // CONFIG_STA_SUPPORT //
+
+#ifdef MCAST_RATE_SPECIFIC
+#define MCAST_DISABLE	0
+#define MCAST_CCK		1
+#define MCAST_OFDM		2
+#define MCAST_HTMIX		3
+#endif // MCAST_RATE_SPECIFIC //
+
+// For AsicRadioOff/AsicRadioOn function
+#define DOT11POWERSAVE		0
+#define GUIRADIO_OFF		1
+#define RTMP_HALT		    2
+#define GUI_IDLE_POWER_SAVE		3
+// --
+
+
+// definition for WpaSupport flag
+#define WPA_SUPPLICANT_DISABLE				0
+#define WPA_SUPPLICANT_ENABLE				1
+#define	WPA_SUPPLICANT_ENABLE_WITH_WEB_UI	2
+
+// Endian byte swapping codes
+#define SWAP16(x) \
+    ((UINT16)( \
+    (((UINT16)(x) & (UINT16) 0x00ffU) << 8) | \
+    (((UINT16)(x) & (UINT16) 0xff00U) >> 8) ))
+
+#define SWAP32(x) \
+    ((UINT32)( \
+    (((UINT32)(x) & (UINT32) 0x000000ffUL) << 24) | \
+    (((UINT32)(x) & (UINT32) 0x0000ff00UL) <<  8) | \
+    (((UINT32)(x) & (UINT32) 0x00ff0000UL) >>  8) | \
+    (((UINT32)(x) & (UINT32) 0xff000000UL) >> 24) ))
+
+#define SWAP64(x) \
+    ((UINT64)( \
+    (UINT64)(((UINT64)(x) & (UINT64) 0x00000000000000ffULL) << 56) | \
+    (UINT64)(((UINT64)(x) & (UINT64) 0x000000000000ff00ULL) << 40) | \
+    (UINT64)(((UINT64)(x) & (UINT64) 0x0000000000ff0000ULL) << 24) | \
+    (UINT64)(((UINT64)(x) & (UINT64) 0x00000000ff000000ULL) <<  8) | \
+    (UINT64)(((UINT64)(x) & (UINT64) 0x000000ff00000000ULL) >>  8) | \
+    (UINT64)(((UINT64)(x) & (UINT64) 0x0000ff0000000000ULL) >> 24) | \
+    (UINT64)(((UINT64)(x) & (UINT64) 0x00ff000000000000ULL) >> 40) | \
+    (UINT64)(((UINT64)(x) & (UINT64) 0xff00000000000000ULL) >> 56) ))
+
+#ifdef RT_BIG_ENDIAN
+
+#define cpu2le64(x) SWAP64((x))
+#define le2cpu64(x) SWAP64((x))
+#define cpu2le32(x) SWAP32((x))
+#define le2cpu32(x) SWAP32((x))
+#define cpu2le16(x) SWAP16((x))
+#define le2cpu16(x) SWAP16((x))
+#define cpu2be64(x) ((UINT64)(x))
+#define be2cpu64(x) ((UINT64)(x))
+#define cpu2be32(x) ((UINT32)(x))
+#define be2cpu32(x) ((UINT32)(x))
+#define cpu2be16(x) ((UINT16)(x))
+#define be2cpu16(x) ((UINT16)(x))
+
+#else   // Little_Endian
+
+#define cpu2le64(x) ((UINT64)(x))
+#define le2cpu64(x) ((UINT64)(x))
+#define cpu2le32(x) ((UINT32)(x))
+#define le2cpu32(x) ((UINT32)(x))
+#define cpu2le16(x) ((UINT16)(x))
+#define le2cpu16(x) ((UINT16)(x))
+#define cpu2be64(x) SWAP64((x))
+#define be2cpu64(x) SWAP64((x))
+#define cpu2be32(x) SWAP32((x))
+#define be2cpu32(x) SWAP32((x))
+#define cpu2be16(x) SWAP16((x))
+#define be2cpu16(x) SWAP16((x))
+
+#endif  // RT_BIG_ENDIAN
+
+#endif  // __RTMP_DEF_H__
+
+
diff --git a/drivers/staging/rt2870/rtmp_type.h b/drivers/staging/rt2870/rtmp_type.h
new file mode 100644
index 00000000000..1fd7df1e179
--- /dev/null
+++ b/drivers/staging/rt2870/rtmp_type.h
@@ -0,0 +1,94 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    rtmp_type.h
+
+    Abstract:
+
+    Revision History:
+    Who         When            What
+    --------    ----------      ----------------------------------------------
+    Name        Date            Modification logs
+    Paul Lin    1-2-2004
+*/
+#ifndef __RTMP_TYPE_H__
+#define __RTMP_TYPE_H__
+
+#define PACKED  __attribute__ ((packed))
+
+// Put platform dependent declaration here
+// For example, linux type definition
+typedef unsigned char		UINT8;
+typedef unsigned short		UINT16;
+typedef unsigned int		UINT32;
+typedef unsigned long long	UINT64;
+typedef int					INT32;
+typedef long long 			INT64;
+
+typedef unsigned char *			PUINT8;
+typedef unsigned short *		PUINT16;
+typedef unsigned int *			PUINT32;
+typedef unsigned long long *	PUINT64;
+typedef int	*					PINT32;
+typedef long long * 			PINT64;
+
+typedef signed char			CHAR;
+typedef signed short		SHORT;
+typedef signed int			INT;
+typedef signed long			LONG;
+typedef signed long long	LONGLONG;
+
+
+typedef unsigned char		UCHAR;
+typedef unsigned short		USHORT;
+typedef unsigned int		UINT;
+typedef unsigned long		ULONG;
+typedef unsigned long long	ULONGLONG;
+
+typedef unsigned char		BOOLEAN;
+typedef void				VOID;
+
+typedef VOID *				PVOID;
+typedef CHAR *				PCHAR;
+typedef UCHAR * 			PUCHAR;
+typedef USHORT *			PUSHORT;
+typedef LONG *				PLONG;
+typedef ULONG *				PULONG;
+typedef UINT *				PUINT;
+
+typedef unsigned int	NDIS_MEDIA_STATE;
+
+typedef union _LARGE_INTEGER {
+    struct {
+        UINT LowPart;
+        INT32 HighPart;
+    } u;
+    INT64 QuadPart;
+} LARGE_INTEGER;
+
+#endif  // __RTMP_TYPE_H__
+
diff --git a/drivers/staging/rt2870/spectrum.h b/drivers/staging/rt2870/spectrum.h
new file mode 100644
index 00000000000..94cfa5b174f
--- /dev/null
+++ b/drivers/staging/rt2870/spectrum.h
@@ -0,0 +1,322 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+ */
+
+#ifndef __SPECTRUM_H__
+#define __SPECTRUM_H__
+
+#include "rtmp_type.h"
+#include "spectrum_def.h"
+
+typedef struct PACKED _TPC_REPORT_INFO
+{
+	UINT8 TxPwr;
+	UINT8 LinkMargin;
+} TPC_REPORT_INFO, *PTPC_REPORT_INFO;
+
+typedef struct PACKED _CH_SW_ANN_INFO
+{
+	UINT8 ChSwMode;
+	UINT8 Channel;
+	UINT8 ChSwCnt;
+} CH_SW_ANN_INFO, *PCH_SW_ANN_INFO;
+
+typedef union PACKED _MEASURE_REQ_MODE
+{
+#ifdef RT_BIG_ENDIAN
+	struct PACKED
+	{
+		UINT8 Rev1:4;
+		UINT8 Report:1;
+		UINT8 Request:1;
+		UINT8 Enable:1;
+		UINT8 Rev0:1;
+	} field;
+#else
+	struct PACKED
+	{
+		UINT8 Rev0:1;
+		UINT8 Enable:1;
+		UINT8 Request:1;
+		UINT8 Report:1;
+		UINT8 Rev1:4;
+	} field;
+#endif // RT_BIG_ENDIAN //
+	UINT8 word;
+} MEASURE_REQ_MODE, *PMEASURE_REQ_MODE;
+
+typedef struct PACKED _MEASURE_REQ
+{
+	UINT8 ChNum;
+	UINT64 MeasureStartTime;
+	UINT16 MeasureDuration;
+} MEASURE_REQ, *PMEASURE_REQ;
+
+typedef struct PACKED _MEASURE_REQ_INFO
+{
+	UINT8 Token;
+	MEASURE_REQ_MODE ReqMode;
+	UINT8 ReqType;
+	MEASURE_REQ MeasureReq;
+} MEASURE_REQ_INFO, *PMEASURE_REQ_INFO;
+
+typedef union PACKED _MEASURE_BASIC_REPORT_MAP
+{
+#ifdef RT_BIG_ENDIAN
+	struct PACKED
+	{
+		UINT8 Rev:3;
+		UINT8 Unmeasure:1;
+		UINT8 Radar:1;
+		UINT8 UnidentifiedSignal:1;
+		UINT8 OfdmPreamble:1;
+		UINT8 BSS:1;
+	} field;
+#else
+	struct PACKED
+	{
+		UINT8 BSS:1;
+		UINT8 OfdmPreamble:1;
+		UINT8 UnidentifiedSignal:1;
+		UINT8 Radar:1;
+		UINT8 Unmeasure:1;
+		UINT8 Rev:3;
+	} field;
+#endif // RT_BIG_ENDIAN //
+	UINT8 word;
+} MEASURE_BASIC_REPORT_MAP, *PMEASURE_BASIC_REPORT_MAP;
+
+typedef struct PACKED _MEASURE_BASIC_REPORT
+{
+	UINT8 ChNum;
+	UINT64 MeasureStartTime;
+	UINT16 MeasureDuration;
+	MEASURE_BASIC_REPORT_MAP Map;
+} MEASURE_BASIC_REPORT, *PMEASURE_BASIC_REPORT;
+
+typedef struct PACKED _MEASURE_CCA_REPORT
+{
+	UINT8 ChNum;
+	UINT64 MeasureStartTime;
+	UINT16 MeasureDuration;
+	UINT8 CCA_Busy_Fraction;
+} MEASURE_CCA_REPORT, *PMEASURE_CCA_REPORT;
+
+typedef struct PACKED _MEASURE_RPI_REPORT
+{
+	UINT8 ChNum;
+	UINT64 MeasureStartTime;
+	UINT16 MeasureDuration;
+	UINT8 RPI_Density[8];
+} MEASURE_RPI_REPORT, *PMEASURE_RPI_REPORT;
+
+typedef union PACKED _MEASURE_REPORT_MODE
+{
+	struct PACKED
+	{
+#ifdef RT_BIG_ENDIAN
+		UINT8 Rev:5;
+		UINT8 Refused:1;
+		UINT8 Incapable:1;
+		UINT8 Late:1;
+#else
+		UINT8 Late:1;
+		UINT8 Incapable:1;
+		UINT8 Refused:1;
+		UINT8 Rev:5;
+#endif // RT_BIG_ENDIAN //
+	} field;
+	UINT8 word;
+} MEASURE_REPORT_MODE, *PMEASURE_REPORT_MODE;
+
+typedef struct PACKED _MEASURE_REPORT_INFO
+{
+	UINT8 Token;
+	MEASURE_REPORT_MODE ReportMode;
+	UINT8 ReportType;
+	UINT8 Octect[0];
+} MEASURE_REPORT_INFO, *PMEASURE_REPORT_INFO;
+
+typedef struct PACKED _QUIET_INFO
+{
+	UINT8 QuietCnt;
+	UINT8 QuietPeriod;
+	UINT8 QuietDuration;
+	UINT8 QuietOffset;
+} QUIET_INFO, *PQUIET_INFO;
+
+/*
+	==========================================================================
+	Description:
+		Prepare Measurement request action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueMeasurementReq(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UINT8 MeasureToken,
+	IN UINT8 MeasureReqMode,
+	IN UINT8 MeasureReqType,
+	IN UINT8 MeasureCh,
+	IN UINT16 MeasureDuration);
+
+/*
+	==========================================================================
+	Description:
+		Prepare Measurement report action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueMeasurementRep(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UINT8 DialogToken,
+	IN UINT8 MeasureToken,
+	IN UINT8 MeasureReqMode,
+	IN UINT8 MeasureReqType,
+	IN UINT8 ReportInfoLen,
+	IN PUINT8 pReportInfo);
+
+/*
+	==========================================================================
+	Description:
+		Prepare TPC Request action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueTPCReq(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UCHAR DialogToken);
+
+/*
+	==========================================================================
+	Description:
+		Prepare TPC Report action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueTPCRep(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UINT8 DialogToken,
+	IN UINT8 TxPwr,
+	IN UINT8 LinkMargin);
+
+/*
+	==========================================================================
+	Description:
+		Prepare Channel Switch Announcement action frame and enqueue it into
+		management queue waiting for transmition.
+
+	Parametrs:
+		1. the destination mac address of the frame.
+		2. Channel switch announcement mode.
+		2. a New selected channel.
+
+	Return	: None.
+	==========================================================================
+ */
+VOID EnqueueChSwAnn(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pDA,
+	IN UINT8 ChSwMode,
+	IN UINT8 NewCh);
+
+/*
+	==========================================================================
+	Description:
+		Spectrun action frames Handler such as channel switch annoucement,
+		measurement report, measurement request actions frames.
+
+	Parametrs:
+		Elme - MLME message containing the received frame
+
+	Return	: None.
+	==========================================================================
+ */
+VOID PeerSpectrumAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem);
+
+/*
+	==========================================================================
+	Description:
+
+	Parametrs:
+
+	Return	: None.
+	==========================================================================
+ */
+INT Set_MeasureReq_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+INT Set_TpcReq_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+
+VOID MeasureReqTabInit(
+	IN PRTMP_ADAPTER pAd);
+
+VOID MeasureReqTabExit(
+	IN PRTMP_ADAPTER pAd);
+
+VOID TpcReqTabInit(
+	IN PRTMP_ADAPTER pAd);
+
+VOID TpcReqTabExit(
+	IN PRTMP_ADAPTER pAd);
+
+VOID NotifyChSwAnnToPeerAPs(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pRA,
+	IN PUCHAR pTA,
+	IN UINT8 ChSwMode,
+	IN UINT8 Channel);
+#endif // __SPECTRUM_H__ //
+
diff --git a/drivers/staging/rt2870/spectrum_def.h b/drivers/staging/rt2870/spectrum_def.h
new file mode 100644
index 00000000000..4ca4817bba0
--- /dev/null
+++ b/drivers/staging/rt2870/spectrum_def.h
@@ -0,0 +1,95 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+	spectrum_def.h
+
+    Abstract:
+    Handle association related requests either from WSTA or from local MLME
+
+    Revision History:
+    Who          When          What
+    ---------    ----------    ----------------------------------------------
+	Fonchi Wu    2008	  	   created for 802.11h
+ */
+
+#ifndef __SPECTRUM_DEF_H__
+#define __SPECTRUM_DEF_H__
+
+#define MAX_MEASURE_REQ_TAB_SIZE		3
+#define MAX_HASH_MEASURE_REQ_TAB_SIZE	MAX_MEASURE_REQ_TAB_SIZE
+
+#define MAX_TPC_REQ_TAB_SIZE			3
+#define MAX_HASH_TPC_REQ_TAB_SIZE		MAX_TPC_REQ_TAB_SIZE
+
+#define MIN_RCV_PWR				100		/* Negative value ((dBm) */
+
+#define RM_TPC_REQ				0
+#define RM_MEASURE_REQ			1
+
+#define RM_BASIC				0
+#define RM_CCA					1
+#define RM_RPI_HISTOGRAM		2
+
+#define TPC_REQ_AGE_OUT			500		/* ms */
+#define MQ_REQ_AGE_OUT			500		/* ms */
+
+#define TPC_DIALOGTOKEN_HASH_INDEX(_DialogToken)	((_DialogToken) % MAX_HASH_TPC_REQ_TAB_SIZE)
+#define MQ_DIALOGTOKEN_HASH_INDEX(_DialogToken)		((_DialogToken) % MAX_MEASURE_REQ_TAB_SIZE)
+
+typedef struct _MEASURE_REQ_ENTRY
+{
+	struct _MEASURE_REQ_ENTRY *pNext;
+	ULONG lastTime;
+	BOOLEAN	Valid;
+	UINT8 DialogToken;
+	UINT8 MeasureDialogToken[3];	// 0:basic measure, 1: CCA measure, 2: RPI_Histogram measure.
+} MEASURE_REQ_ENTRY, *PMEASURE_REQ_ENTRY;
+
+typedef struct _MEASURE_REQ_TAB
+{
+	UCHAR Size;
+	PMEASURE_REQ_ENTRY Hash[MAX_HASH_MEASURE_REQ_TAB_SIZE];
+	MEASURE_REQ_ENTRY Content[MAX_MEASURE_REQ_TAB_SIZE];
+} MEASURE_REQ_TAB, *PMEASURE_REQ_TAB;
+
+typedef struct _TPC_REQ_ENTRY
+{
+	struct _TPC_REQ_ENTRY *pNext;
+	ULONG lastTime;
+	BOOLEAN Valid;
+	UINT8 DialogToken;
+} TPC_REQ_ENTRY, *PTPC_REQ_ENTRY;
+
+typedef struct _TPC_REQ_TAB
+{
+	UCHAR Size;
+	PTPC_REQ_ENTRY Hash[MAX_HASH_TPC_REQ_TAB_SIZE];
+	TPC_REQ_ENTRY Content[MAX_TPC_REQ_TAB_SIZE];
+} TPC_REQ_TAB, *PTPC_REQ_TAB;
+
+#endif // __SPECTRUM_DEF_H__ //
+
diff --git a/drivers/staging/rt2870/sta/aironet.c b/drivers/staging/rt2870/sta/aironet.c
new file mode 100644
index 00000000000..4af4a190618
--- /dev/null
+++ b/drivers/staging/rt2870/sta/aironet.c
@@ -0,0 +1,1312 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	aironet.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Paul Lin	04-06-15		Initial
+*/
+#include "../rt_config.h"
+
+/*
+	==========================================================================
+	Description:
+		association	state machine init,	including state	transition and timer init
+	Parameters:
+		S -	pointer	to the association state machine
+	==========================================================================
+ */
+VOID	AironetStateMachineInit(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	STATE_MACHINE		*S,
+	OUT	STATE_MACHINE_FUNC	Trans[])
+{
+	StateMachineInit(S,	Trans, MAX_AIRONET_STATE, MAX_AIRONET_MSG, (STATE_MACHINE_FUNC)Drop, AIRONET_IDLE, AIRONET_MACHINE_BASE);
+	StateMachineSetAction(S, AIRONET_IDLE, MT2_AIRONET_MSG, (STATE_MACHINE_FUNC)AironetMsgAction);
+	StateMachineSetAction(S, AIRONET_IDLE, MT2_AIRONET_SCAN_REQ, (STATE_MACHINE_FUNC)AironetRequestAction);
+	StateMachineSetAction(S, AIRONET_SCANNING, MT2_AIRONET_SCAN_DONE, (STATE_MACHINE_FUNC)AironetReportAction);
+}
+
+/*
+	==========================================================================
+	Description:
+		This is	state machine function.
+		When receiving EAPOL packets which is  for 802.1x key management.
+		Use	both in	WPA, and WPAPSK	case.
+		In this	function, further dispatch to different	functions according	to the received	packet.	 3 categories are :
+		  1.  normal 4-way pairwisekey and 2-way groupkey handshake
+		  2.  MIC error	(Countermeasures attack)  report packet	from STA.
+		  3.  Request for pairwise/group key update	from STA
+	Return:
+	==========================================================================
+*/
+VOID	AironetMsgAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MLME_QUEUE_ELEM	*Elem)
+{
+	USHORT							Length;
+	UCHAR							Index, i;
+	PUCHAR							pData;
+	PAIRONET_RM_REQUEST_FRAME		pRMReq;
+	PRM_REQUEST_ACTION				pReqElem;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("-----> AironetMsgAction\n"));
+
+	// 0. Get Aironet IAPP header first
+	pRMReq = (PAIRONET_RM_REQUEST_FRAME) &Elem->Msg[LENGTH_802_11];
+	pData  = (PUCHAR) &Elem->Msg[LENGTH_802_11];
+
+	// 1. Change endian format form network to little endian
+	Length = be2cpu16(pRMReq->IAPP.Length);
+
+	// 2.0 Sanity check, this should only happen when CCX 2.0 support is enabled
+	if (pAd->StaCfg.CCXEnable != TRUE)
+		return;
+
+	// 2.1 Radio measurement must be on
+	if (pAd->StaCfg.CCXControl.field.RMEnable != 1)
+		return;
+
+	// 2.2. Debug print all bit information
+	DBGPRINT(RT_DEBUG_TRACE, ("IAPP ID & Length %d\n", Length));
+	DBGPRINT(RT_DEBUG_TRACE, ("IAPP Type %x\n", pRMReq->IAPP.Type));
+	DBGPRINT(RT_DEBUG_TRACE, ("IAPP SubType %x\n", pRMReq->IAPP.SubType));
+	DBGPRINT(RT_DEBUG_TRACE, ("IAPP Dialog Token %x\n", pRMReq->IAPP.Token));
+	DBGPRINT(RT_DEBUG_TRACE, ("IAPP Activation Delay %x\n", pRMReq->Delay));
+	DBGPRINT(RT_DEBUG_TRACE, ("IAPP Measurement Offset %x\n", pRMReq->Offset));
+
+	// 3. Check IAPP frame type, it must be 0x32 for Cisco Aironet extension
+	if (pRMReq->IAPP.Type != AIRONET_IAPP_TYPE)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Wrong IAPP type for Cisco Aironet extension\n"));
+		return;
+	}
+
+	// 4. Check IAPP frame subtype, it must be 0x01 for Cisco Aironet extension request.
+	//    Since we are acting as client only, we will disregards reply subtype.
+	if (pRMReq->IAPP.SubType != AIRONET_IAPP_SUBTYPE_REQUEST)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Wrong IAPP subtype for Cisco Aironet extension\n"));
+		return;
+	}
+
+	// 5. Verify Destination MAC and Source MAC, both should be all zeros.
+	if (! MAC_ADDR_EQUAL(pRMReq->IAPP.DA, ZERO_MAC_ADDR))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Wrong IAPP DA for Cisco Aironet extension, it's not Zero\n"));
+		return;
+	}
+
+	if (! MAC_ADDR_EQUAL(pRMReq->IAPP.SA, ZERO_MAC_ADDR))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("Wrong IAPP SA for Cisco Aironet extension, it's not Zero\n"));
+		return;
+	}
+
+	// 6. Reinit all report related fields
+	NdisZeroMemory(pAd->StaCfg.FrameReportBuf, 2048);
+	NdisZeroMemory(pAd->StaCfg.BssReportOffset, sizeof(USHORT) * MAX_LEN_OF_BSS_TABLE);
+	NdisZeroMemory(pAd->StaCfg.MeasurementRequest, sizeof(RM_REQUEST_ACTION) * 4);
+
+	// 7. Point to the start of first element report element
+	pAd->StaCfg.FrameReportLen   = LENGTH_802_11 + sizeof(AIRONET_IAPP_HEADER);
+	DBGPRINT(RT_DEBUG_TRACE, ("FR len = %d\n", pAd->StaCfg.FrameReportLen));
+	pAd->StaCfg.LastBssIndex     = 0xff;
+	pAd->StaCfg.RMReqCnt         = 0;
+	pAd->StaCfg.ParallelReq      = FALSE;
+	pAd->StaCfg.ParallelDuration = 0;
+	pAd->StaCfg.ParallelChannel  = 0;
+	pAd->StaCfg.IAPPToken        = pRMReq->IAPP.Token;
+	pAd->StaCfg.CurrentRMReqIdx  = 0;
+	pAd->StaCfg.CLBusyBytes      = 0;
+	// Reset the statistics
+	for (i = 0; i < 8; i++)
+		pAd->StaCfg.RPIDensity[i] = 0;
+
+	Index = 0;
+
+	// 8. Save dialog token for report
+	pAd->StaCfg.IAPPToken = pRMReq->IAPP.Token;
+
+	// Save Activation delay & measurement offset, Not really needed
+
+	// 9. Point to the first request element
+	pData += sizeof(AIRONET_RM_REQUEST_FRAME);
+	//    Length should exclude the CISCO Aironet SNAP header
+	Length -= (sizeof(AIRONET_RM_REQUEST_FRAME) - LENGTH_802_1_H);
+
+	// 10. Start Parsing the Measurement elements.
+	//    Be careful about multiple MR elements within one frames.
+	while (Length > 0)
+	{
+		pReqElem = (PRM_REQUEST_ACTION) pData;
+		switch (pReqElem->ReqElem.Eid)
+		{
+			case IE_MEASUREMENT_REQUEST:
+				// From the example, it seems we only need to support one request in one frame
+				// There is no multiple request in one frame.
+				// Besides, looks like we need to take care the measurement request only.
+				// The measurement request is always 4 bytes.
+
+				// Start parsing this type of request.
+				// 0. Eid is IE_MEASUREMENT_REQUEST
+				// 1. Length didn't include Eid and Length field, it always be 8.
+				// 2. Measurement Token, we nned to save it for the corresponding report.
+				// 3. Measurement Mode, Although there are definitions, but we din't see value other than
+				//    0 from test specs examples.
+				// 4. Measurement Type, this is what we need to do.
+				switch (pReqElem->ReqElem.Type)
+				{
+					case MSRN_TYPE_CHANNEL_LOAD_REQ:
+					case MSRN_TYPE_NOISE_HIST_REQ:
+					case MSRN_TYPE_BEACON_REQ:
+						// Check the Enable non-serving channel measurement control
+						if (pAd->StaCfg.CCXControl.field.DCRMEnable == 0)
+						{
+							// Check channel before enqueue the action
+							if (pReqElem->Measurement.Channel != pAd->CommonCfg.Channel)
+								break;
+						}
+						else
+						{
+							// If off channel measurement, check the TU duration limit
+							if (pReqElem->Measurement.Channel != pAd->CommonCfg.Channel)
+								if (pReqElem->Measurement.Duration > pAd->StaCfg.CCXControl.field.TuLimit)
+									break;
+						}
+
+						// Save requests and execute actions later
+						NdisMoveMemory(&pAd->StaCfg.MeasurementRequest[Index], pReqElem, sizeof(RM_REQUEST_ACTION));
+						Index += 1;
+						break;
+
+					case MSRN_TYPE_FRAME_REQ:
+						// Since it's option, we will support later
+						// FrameRequestAction(pAd, pData);
+						break;
+
+					default:
+						break;
+				}
+
+				// Point to next Measurement request
+				pData  += sizeof(RM_REQUEST_ACTION);
+				Length -= sizeof(RM_REQUEST_ACTION);
+				break;
+
+			// We accept request only, all others are dropped
+			case IE_MEASUREMENT_REPORT:
+			case IE_AP_TX_POWER:
+			case IE_MEASUREMENT_CAPABILITY:
+			default:
+				return;
+		}
+	}
+
+	// 11. Update some flags and index
+	pAd->StaCfg.RMReqCnt = Index;
+
+	if (Index)
+	{
+		MlmeEnqueue(pAd, AIRONET_STATE_MACHINE, MT2_AIRONET_SCAN_REQ, 0, NULL);
+		RT28XX_MLME_HANDLER(pAd);
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<----- AironetMsgAction\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	AironetRequestAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MLME_QUEUE_ELEM	*Elem)
+{
+	PRM_REQUEST_ACTION	pReq;
+
+	// 1. Point to next request element
+	pReq = (PRM_REQUEST_ACTION) &pAd->StaCfg.MeasurementRequest[pAd->StaCfg.CurrentRMReqIdx];
+
+	// 2. Parse measurement type and call appropriate functions
+	if (pReq->ReqElem.Type == MSRN_TYPE_CHANNEL_LOAD_REQ)
+		// Channel Load measurement request
+		ChannelLoadRequestAction(pAd, pAd->StaCfg.CurrentRMReqIdx);
+	else if (pReq->ReqElem.Type == MSRN_TYPE_NOISE_HIST_REQ)
+		// Noise Histogram measurement request
+		NoiseHistRequestAction(pAd, pAd->StaCfg.CurrentRMReqIdx);
+	else if (pReq->ReqElem.Type == MSRN_TYPE_BEACON_REQ)
+		// Beacon measurement request
+		BeaconRequestAction(pAd, pAd->StaCfg.CurrentRMReqIdx);
+	else
+		// Unknown. Do nothing and return, this should never happen
+		return;
+
+	// 3. Peek into the next request, if it's parallel, we will update the scan time to the largest one
+	if ((pAd->StaCfg.CurrentRMReqIdx + 1) < pAd->StaCfg.RMReqCnt)
+	{
+		pReq = (PRM_REQUEST_ACTION) &pAd->StaCfg.MeasurementRequest[pAd->StaCfg.CurrentRMReqIdx + 1];
+		// Check for parallel bit
+		if ((pReq->ReqElem.Mode & 0x01) && (pReq->Measurement.Channel == pAd->StaCfg.CCXScanChannel))
+		{
+			// Update parallel mode request information
+			pAd->StaCfg.ParallelReq = TRUE;
+			pAd->StaCfg.CCXScanTime = ((pReq->Measurement.Duration > pAd->StaCfg.CCXScanTime) ?
+			(pReq->Measurement.Duration) : (pAd->StaCfg.CCXScanTime));
+		}
+	}
+
+	// 4. Call RT28XX_MLME_HANDLER to execute the request mlme commands, Scan request is the only one used
+	RT28XX_MLME_HANDLER(pAd);
+
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Prepare channel load report action, special scan operation added
+		to support
+
+	Arguments:
+		pAd	Pointer	to our adapter
+		pData		Start from element ID
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	ChannelLoadRequestAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Index)
+{
+	PRM_REQUEST_ACTION				pReq;
+	MLME_SCAN_REQ_STRUCT			ScanReq;
+	UCHAR							ZeroSsid[32];
+	NDIS_STATUS						NStatus;
+	PUCHAR							pOutBuffer = NULL;
+	PHEADER_802_11					pNullFrame;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("ChannelLoadRequestAction ----->\n"));
+
+	pReq = (PRM_REQUEST_ACTION) &pAd->StaCfg.MeasurementRequest[Index];
+	NdisZeroMemory(ZeroSsid, 32);
+
+	// Prepare for special scan request
+	// The scan definition is different with our Active, Passive scan definition.
+	// For CCX2, Active means send out probe request with broadcast BSSID.
+	// Passive means no probe request sent, only listen to the beacons.
+	// The channel scanned is fixed as specified, no need to scan all channels.
+	// The scan wait time is specified in the request too.
+	// Passive scan Mode
+
+	// Control state machine is not idle, reject the request
+	if ((pAd->Mlme.CntlMachine.CurrState != CNTL_IDLE) && (Index == 0))
+		return;
+
+	// Fill out stuff for scan request
+	ScanParmFill(pAd, &ScanReq, ZeroSsid, 0, BSS_ANY, SCAN_CISCO_CHANNEL_LOAD);
+	MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_SCAN_REQ, sizeof(MLME_SCAN_REQ_STRUCT), &ScanReq);
+	pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_LIST_SCAN;
+
+	// Reset some internal control flags to make sure this scan works.
+	BssTableInit(&pAd->StaCfg.CCXBssTab);
+	pAd->StaCfg.ScanCnt        = 0;
+	pAd->StaCfg.CCXScanChannel = pReq->Measurement.Channel;
+	pAd->StaCfg.CCXScanTime    = pReq->Measurement.Duration;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Duration %d, Channel %d!\n", pReq->Measurement.Duration, pReq->Measurement.Channel));
+
+	// If it's non serving channel scan, send out a null frame with PSM bit on.
+	if (pAd->StaCfg.CCXScanChannel != pAd->CommonCfg.Channel)
+	{
+		// Use MLME enqueue method
+		NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+		if (NStatus	!= NDIS_STATUS_SUCCESS)
+			return;
+
+		pNullFrame = (PHEADER_802_11) pOutBuffer;;
+		// Make the power save Null frame with PSM bit on
+		MgtMacHeaderInit(pAd, pNullFrame, SUBTYPE_NULL_FUNC, 1, pAd->CommonCfg.Bssid, pAd->CommonCfg.Bssid);
+		pNullFrame->Duration 	= 0;
+		pNullFrame->FC.Type 	= BTYPE_DATA;
+		pNullFrame->FC.PwrMgmt	= PWR_SAVE;
+
+		// Send using priority queue
+		MiniportMMRequest(pAd, 0, pOutBuffer, sizeof(HEADER_802_11));
+		MlmeFreeMemory(pAd, pOutBuffer);
+		DBGPRINT(RT_DEBUG_TRACE, ("Send PSM Data frame for off channel RM\n"));
+		RTMPusecDelay(5000);
+	}
+
+	pAd->StaCfg.CCXReqType     = MSRN_TYPE_CHANNEL_LOAD_REQ;
+	pAd->StaCfg.CLBusyBytes    = 0;
+	// Enable Rx with promiscuous reception
+	RTMP_IO_WRITE32(pAd, RX_FILTR_CFG, 0x1010);
+
+	// Set channel load measurement flag
+	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_RADIO_MEASUREMENT);
+
+	pAd->Mlme.AironetMachine.CurrState = AIRONET_SCANNING;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("ChannelLoadRequestAction <-----\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Prepare noise histogram report action, special scan operation added
+		to support
+
+	Arguments:
+		pAd	Pointer	to our adapter
+		pData		Start from element ID
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	NoiseHistRequestAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Index)
+{
+	PRM_REQUEST_ACTION				pReq;
+	MLME_SCAN_REQ_STRUCT			ScanReq;
+	UCHAR							ZeroSsid[32], i;
+	NDIS_STATUS						NStatus;
+	PUCHAR							pOutBuffer = NULL;
+	PHEADER_802_11					pNullFrame;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("NoiseHistRequestAction ----->\n"));
+
+	pReq = (PRM_REQUEST_ACTION) &pAd->StaCfg.MeasurementRequest[Index];
+	NdisZeroMemory(ZeroSsid, 32);
+
+	// Prepare for special scan request
+	// The scan definition is different with our Active, Passive scan definition.
+	// For CCX2, Active means send out probe request with broadcast BSSID.
+	// Passive means no probe request sent, only listen to the beacons.
+	// The channel scanned is fixed as specified, no need to scan all channels.
+	// The scan wait time is specified in the request too.
+	// Passive scan Mode
+
+	// Control state machine is not idle, reject the request
+	if ((pAd->Mlme.CntlMachine.CurrState != CNTL_IDLE) && (Index == 0))
+		return;
+
+	// Fill out stuff for scan request
+	ScanParmFill(pAd, &ScanReq, ZeroSsid, 0, BSS_ANY, SCAN_CISCO_NOISE);
+	MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_SCAN_REQ, sizeof(MLME_SCAN_REQ_STRUCT), &ScanReq);
+	pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_LIST_SCAN;
+
+	// Reset some internal control flags to make sure this scan works.
+	BssTableInit(&pAd->StaCfg.CCXBssTab);
+	pAd->StaCfg.ScanCnt        = 0;
+	pAd->StaCfg.CCXScanChannel = pReq->Measurement.Channel;
+	pAd->StaCfg.CCXScanTime    = pReq->Measurement.Duration;
+	pAd->StaCfg.CCXReqType     = MSRN_TYPE_NOISE_HIST_REQ;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Duration %d, Channel %d!\n", pReq->Measurement.Duration, pReq->Measurement.Channel));
+
+	// If it's non serving channel scan, send out a null frame with PSM bit on.
+	if (pAd->StaCfg.CCXScanChannel != pAd->CommonCfg.Channel)
+	{
+		// Use MLME enqueue method
+		NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+		if (NStatus	!= NDIS_STATUS_SUCCESS)
+			return;
+
+		pNullFrame = (PHEADER_802_11) pOutBuffer;
+		// Make the power save Null frame with PSM bit on
+		MgtMacHeaderInit(pAd, pNullFrame, SUBTYPE_NULL_FUNC, 1, pAd->CommonCfg.Bssid, pAd->CommonCfg.Bssid);
+		pNullFrame->Duration 	= 0;
+		pNullFrame->FC.Type  	= BTYPE_DATA;
+		pNullFrame->FC.PwrMgmt	= PWR_SAVE;
+
+		// Send using priority queue
+		MiniportMMRequest(pAd, 0, pOutBuffer, sizeof(HEADER_802_11));
+		MlmeFreeMemory(pAd, pOutBuffer);
+		DBGPRINT(RT_DEBUG_TRACE, ("Send PSM Data frame for off channel RM\n"));
+		RTMPusecDelay(5000);
+	}
+
+	// Reset the statistics
+	for (i = 0; i < 8; i++)
+		pAd->StaCfg.RPIDensity[i] = 0;
+
+	// Enable Rx with promiscuous reception
+	RTMP_IO_WRITE32(pAd, RX_FILTR_CFG, 0x1010);
+
+	// Set channel load measurement flag
+	RTMP_SET_FLAG(pAd, fRTMP_ADAPTER_RADIO_MEASUREMENT);
+
+	pAd->Mlme.AironetMachine.CurrState = AIRONET_SCANNING;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("NoiseHistRequestAction <-----\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Prepare Beacon report action, special scan operation added
+		to support
+
+	Arguments:
+		pAd	Pointer	to our adapter
+		pData		Start from element ID
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	BeaconRequestAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Index)
+{
+	PRM_REQUEST_ACTION				pReq;
+	NDIS_STATUS						NStatus;
+	PUCHAR							pOutBuffer = NULL;
+	PHEADER_802_11					pNullFrame;
+	MLME_SCAN_REQ_STRUCT			ScanReq;
+	UCHAR							ZeroSsid[32];
+
+	DBGPRINT(RT_DEBUG_TRACE, ("BeaconRequestAction ----->\n"));
+
+	pReq = (PRM_REQUEST_ACTION) &pAd->StaCfg.MeasurementRequest[Index];
+	NdisZeroMemory(ZeroSsid, 32);
+
+	// Prepare for special scan request
+	// The scan definition is different with our Active, Passive scan definition.
+	// For CCX2, Active means send out probe request with broadcast BSSID.
+	// Passive means no probe request sent, only listen to the beacons.
+	// The channel scanned is fixed as specified, no need to scan all channels.
+	// The scan wait time is specified in the request too.
+	if (pReq->Measurement.ScanMode == MSRN_SCAN_MODE_PASSIVE)
+	{
+		// Passive scan Mode
+		DBGPRINT(RT_DEBUG_TRACE, ("Passive Scan Mode!\n"));
+
+		// Control state machine is not idle, reject the request
+		if ((pAd->Mlme.CntlMachine.CurrState != CNTL_IDLE) && (Index == 0))
+			return;
+
+		// Fill out stuff for scan request
+		ScanParmFill(pAd, &ScanReq, ZeroSsid, 0, BSS_ANY, SCAN_CISCO_PASSIVE);
+		MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_SCAN_REQ, sizeof(MLME_SCAN_REQ_STRUCT), &ScanReq);
+		pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_LIST_SCAN;
+
+		// Reset some internal control flags to make sure this scan works.
+		BssTableInit(&pAd->StaCfg.CCXBssTab);
+		pAd->StaCfg.ScanCnt        = 0;
+		pAd->StaCfg.CCXScanChannel = pReq->Measurement.Channel;
+		pAd->StaCfg.CCXScanTime    = pReq->Measurement.Duration;
+		pAd->StaCfg.CCXReqType     = MSRN_TYPE_BEACON_REQ;
+		DBGPRINT(RT_DEBUG_TRACE, ("Duration %d!\n", pReq->Measurement.Duration));
+
+		// If it's non serving channel scan, send out a null frame with PSM bit on.
+		if (pAd->StaCfg.CCXScanChannel != pAd->CommonCfg.Channel)
+		{
+			// Use MLME enqueue method
+			NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+			if (NStatus	!= NDIS_STATUS_SUCCESS)
+				return;
+
+			pNullFrame = (PHEADER_802_11) pOutBuffer;
+			// Make the power save Null frame with PSM bit on
+			MgtMacHeaderInit(pAd, pNullFrame, SUBTYPE_NULL_FUNC, 1, pAd->CommonCfg.Bssid, pAd->CommonCfg.Bssid);
+			pNullFrame->Duration 	= 0;
+			pNullFrame->FC.Type     = BTYPE_DATA;
+			pNullFrame->FC.PwrMgmt  = PWR_SAVE;
+
+			// Send using priority queue
+			MiniportMMRequest(pAd, 0, pOutBuffer, sizeof(HEADER_802_11));
+			MlmeFreeMemory(pAd, pOutBuffer);
+			DBGPRINT(RT_DEBUG_TRACE, ("Send PSM Data frame for off channel RM\n"));
+			RTMPusecDelay(5000);
+		}
+
+		pAd->Mlme.AironetMachine.CurrState = AIRONET_SCANNING;
+	}
+	else if (pReq->Measurement.ScanMode == MSRN_SCAN_MODE_ACTIVE)
+	{
+		// Active scan Mode
+		DBGPRINT(RT_DEBUG_TRACE, ("Active Scan Mode!\n"));
+
+		// Control state machine is not idle, reject the request
+		if (pAd->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+			return;
+
+		// Fill out stuff for scan request
+		ScanParmFill(pAd, &ScanReq, ZeroSsid, 0, BSS_ANY, SCAN_CISCO_ACTIVE);
+		MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_SCAN_REQ, sizeof(MLME_SCAN_REQ_STRUCT), &ScanReq);
+		pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_LIST_SCAN;
+
+		// Reset some internal control flags to make sure this scan works.
+		BssTableInit(&pAd->StaCfg.CCXBssTab);
+		pAd->StaCfg.ScanCnt        = 0;
+		pAd->StaCfg.CCXScanChannel = pReq->Measurement.Channel;
+		pAd->StaCfg.CCXScanTime    = pReq->Measurement.Duration;
+		pAd->StaCfg.CCXReqType     = MSRN_TYPE_BEACON_REQ;
+		DBGPRINT(RT_DEBUG_TRACE, ("Duration %d!\n", pReq->Measurement.Duration));
+
+		// If it's non serving channel scan, send out a null frame with PSM bit on.
+		if (pAd->StaCfg.CCXScanChannel != pAd->CommonCfg.Channel)
+		{
+			// Use MLME enqueue method
+			NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);  //Get an unused nonpaged memory
+			if (NStatus	!= NDIS_STATUS_SUCCESS)
+				return;
+
+			pNullFrame = (PHEADER_802_11) pOutBuffer;
+			// Make the power save Null frame with PSM bit on
+			MgtMacHeaderInit(pAd, pNullFrame, SUBTYPE_NULL_FUNC, 1, pAd->CommonCfg.Bssid, pAd->CommonCfg.Bssid);
+			pNullFrame->Duration 	= 0;
+			pNullFrame->FC.Type     = BTYPE_DATA;
+			pNullFrame->FC.PwrMgmt  = PWR_SAVE;
+
+			// Send using priority queue
+			MiniportMMRequest(pAd, 0, pOutBuffer, sizeof(HEADER_802_11));
+			MlmeFreeMemory(pAd, pOutBuffer);
+			DBGPRINT(RT_DEBUG_TRACE, ("Send PSM Data frame for off channel RM\n"));
+			RTMPusecDelay(5000);
+		}
+
+		pAd->Mlme.AironetMachine.CurrState = AIRONET_SCANNING;
+	}
+	else if (pReq->Measurement.ScanMode == MSRN_SCAN_MODE_BEACON_TABLE)
+	{
+		// Beacon report Mode, report all the APS in current bss table
+		DBGPRINT(RT_DEBUG_TRACE, ("Beacon Report Mode!\n"));
+
+		// Copy current BSS table to CCX table, we can omit this step later on.
+		NdisMoveMemory(&pAd->StaCfg.CCXBssTab, &pAd->ScanTab, sizeof(BSS_TABLE));
+
+		// Create beacon report from Bss table
+		AironetCreateBeaconReportFromBssTable(pAd);
+
+		// Set state to scanning
+		pAd->Mlme.AironetMachine.CurrState = AIRONET_SCANNING;
+
+		// Enqueue report request
+		// Cisco scan request is finished, prepare beacon report
+		MlmeEnqueue(pAd, AIRONET_STATE_MACHINE, MT2_AIRONET_SCAN_DONE, 0, NULL);
+	}
+	else
+	{
+		// Wrong scan Mode
+		DBGPRINT(RT_DEBUG_TRACE, ("Wrong Scan Mode!\n"));
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("BeaconRequestAction <-----\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	AironetReportAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MLME_QUEUE_ELEM	*Elem)
+{
+	PRM_REQUEST_ACTION	pReq;
+	ULONG				Now32;
+
+    NdisGetSystemUpTime(&Now32);
+	pAd->StaCfg.LastBeaconRxTime = Now32;
+
+	pReq = (PRM_REQUEST_ACTION) &pAd->StaCfg.MeasurementRequest[pAd->StaCfg.CurrentRMReqIdx];
+
+	DBGPRINT(RT_DEBUG_TRACE, ("AironetReportAction ----->\n"));
+
+	// 1. Parse measurement type and call appropriate functions
+	if (pReq->ReqElem.Type == MSRN_TYPE_CHANNEL_LOAD_REQ)
+		// Channel Load measurement request
+		ChannelLoadReportAction(pAd, pAd->StaCfg.CurrentRMReqIdx);
+	else if (pReq->ReqElem.Type == MSRN_TYPE_NOISE_HIST_REQ)
+		// Noise Histogram measurement request
+		NoiseHistReportAction(pAd, pAd->StaCfg.CurrentRMReqIdx);
+	else if (pReq->ReqElem.Type == MSRN_TYPE_BEACON_REQ)
+		// Beacon measurement request
+		BeaconReportAction(pAd, pAd->StaCfg.CurrentRMReqIdx);
+	else
+		// Unknown. Do nothing and return
+		;
+
+	// 2. Point to the correct index of action element, start from 0
+	pAd->StaCfg.CurrentRMReqIdx++;
+
+	// 3. Check for parallel actions
+	if (pAd->StaCfg.ParallelReq == TRUE)
+	{
+		pReq = (PRM_REQUEST_ACTION) &pAd->StaCfg.MeasurementRequest[pAd->StaCfg.CurrentRMReqIdx];
+
+		// Process next action right away
+		if (pReq->ReqElem.Type == MSRN_TYPE_CHANNEL_LOAD_REQ)
+			// Channel Load measurement request
+			ChannelLoadReportAction(pAd, pAd->StaCfg.CurrentRMReqIdx);
+		else if (pReq->ReqElem.Type == MSRN_TYPE_NOISE_HIST_REQ)
+			// Noise Histogram measurement request
+			NoiseHistReportAction(pAd, pAd->StaCfg.CurrentRMReqIdx);
+
+		pAd->StaCfg.ParallelReq = FALSE;
+		pAd->StaCfg.CurrentRMReqIdx++;
+	}
+
+	if (pAd->StaCfg.CurrentRMReqIdx >= pAd->StaCfg.RMReqCnt)
+	{
+		// 4. There is no more unprocessed measurement request, go for transmit this report
+		AironetFinalReportAction(pAd);
+		pAd->Mlme.AironetMachine.CurrState = AIRONET_IDLE;
+	}
+	else
+	{
+		pReq = (PRM_REQUEST_ACTION) &pAd->StaCfg.MeasurementRequest[pAd->StaCfg.CurrentRMReqIdx];
+
+		if (pReq->Measurement.Channel != pAd->CommonCfg.Channel)
+		{
+			RTMPusecDelay(100000);
+		}
+
+		// 5. There are more requests to be measure
+		MlmeEnqueue(pAd, AIRONET_STATE_MACHINE, MT2_AIRONET_SCAN_REQ, 0, NULL);
+		RT28XX_MLME_HANDLER(pAd);
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("AironetReportAction <-----\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	AironetFinalReportAction(
+	IN	PRTMP_ADAPTER	pAd)
+{
+	PUCHAR					pDest;
+	PAIRONET_IAPP_HEADER	pIAPP;
+	PHEADER_802_11			pHeader;
+	UCHAR					AckRate = RATE_2;
+	USHORT					AckDuration = 0;
+	NDIS_STATUS				NStatus;
+	PUCHAR					pOutBuffer = NULL;
+	ULONG					FrameLen = 0;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("AironetFinalReportAction ----->\n"));
+
+	// 0. Set up the frame pointer, Frame was inited at the end of message action
+	pDest = &pAd->StaCfg.FrameReportBuf[LENGTH_802_11];
+
+	// 1. Update report IAPP fields
+	pIAPP = (PAIRONET_IAPP_HEADER) pDest;
+
+	// 2. Copy Cisco SNAP header
+	NdisMoveMemory(pIAPP->CiscoSnapHeader, SNAP_AIRONET, LENGTH_802_1_H);
+
+	// 3. network order for this 16bit length
+	pIAPP->Length  = cpu2be16(pAd->StaCfg.FrameReportLen - LENGTH_802_11 - LENGTH_802_1_H);
+
+	// 3.1 sanity check the report length, ignore it if there is nothing to report
+	if (be2cpu16(pIAPP->Length) <= 18)
+		return;
+
+	// 4. Type must be 0x32
+	pIAPP->Type    = AIRONET_IAPP_TYPE;
+
+	// 5. SubType for report must be 0x81
+	pIAPP->SubType = AIRONET_IAPP_SUBTYPE_REPORT;
+
+	// 6. DA is not used and must be zero, although the whole frame was cleared at the start of function
+	//    We will do it again here. We can use BSSID instead
+	COPY_MAC_ADDR(pIAPP->DA, pAd->CommonCfg.Bssid);
+
+	// 7. SA is the client reporting which must be our MAC
+	COPY_MAC_ADDR(pIAPP->SA, pAd->CurrentAddress);
+
+	// 8. Copy the saved dialog token
+	pIAPP->Token = pAd->StaCfg.IAPPToken;
+
+	// 9. Make the Report frame 802.11 header
+	//    Reuse function in wpa.c
+	pHeader = (PHEADER_802_11) pAd->StaCfg.FrameReportBuf;
+	pAd->Sequence ++;
+	WpaMacHeaderInit(pAd, pHeader, 0, pAd->CommonCfg.Bssid);
+
+	// ACK size	is 14 include CRC, and its rate	is based on real time information
+	AckRate     = pAd->CommonCfg.ExpectedACKRate[pAd->CommonCfg.MlmeRate];
+	AckDuration = RTMPCalcDuration(pAd, AckRate, 14);
+	pHeader->Duration = pAd->CommonCfg.Dsifs + AckDuration;
+
+	// Use MLME enqueue method
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if (NStatus	!= NDIS_STATUS_SUCCESS)
+		return;
+
+	// 10. Prepare report frame with dynamic outbuffer. Just simply copy everything.
+	MakeOutgoingFrame(pOutBuffer,                       &FrameLen,
+	                  pAd->StaCfg.FrameReportLen, pAd->StaCfg.FrameReportBuf,
+		              END_OF_ARGS);
+
+	// 11. Send using priority queue
+	MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	pAd->StaCfg.CCXReqType = MSRN_TYPE_UNUSED;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("AironetFinalReportAction <-----\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	ChannelLoadReportAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Index)
+{
+	PMEASUREMENT_REPORT_ELEMENT	pReport;
+	PCHANNEL_LOAD_REPORT		pLoad;
+	PUCHAR						pDest;
+	UCHAR						CCABusyFraction;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("ChannelLoadReportAction ----->\n"));
+
+	// Disable Rx with promiscuous reception, make it back to normal
+	RTMP_IO_WRITE32(pAd, RX_FILTR_CFG, STANORMAL); // Staion not drop control frame will fail WiFi Certification.
+
+	// 0. Setup pointer for processing beacon & probe response
+	pDest = (PUCHAR) &pAd->StaCfg.FrameReportBuf[pAd->StaCfg.FrameReportLen];
+	pReport = (PMEASUREMENT_REPORT_ELEMENT) pDest;
+
+	// 1. Fill Measurement report element field.
+	pReport->Eid    = IE_MEASUREMENT_REPORT;
+	// Fixed Length at 9, not include Eid and length fields
+	pReport->Length = 9;
+	pReport->Token  = pAd->StaCfg.MeasurementRequest[Index].ReqElem.Token;
+	pReport->Mode   = pAd->StaCfg.MeasurementRequest[Index].ReqElem.Mode;
+	pReport->Type   = MSRN_TYPE_CHANNEL_LOAD_REQ;
+
+	// 2. Fill channel report measurement data
+	pDest += sizeof(MEASUREMENT_REPORT_ELEMENT);
+	pLoad  = (PCHANNEL_LOAD_REPORT) pDest;
+	pLoad->Channel  = pAd->StaCfg.MeasurementRequest[Index].Measurement.Channel;
+	pLoad->Spare    = 0;
+	pLoad->Duration = pAd->StaCfg.MeasurementRequest[Index].Measurement.Duration;
+
+	// 3. Calculate the CCA Busy Fraction
+	//    (Bytes + ACK size) * 8 / Tx speed * 255 / 1000 / measurement duration, use 24 us Tx speed
+	//     =  (Bytes + ACK) / 12 / duration
+	//     9 is the good value for pAd->StaCfg.CLFactor
+	// CCABusyFraction = (UCHAR) (pAd->StaCfg.CLBusyBytes / 9 / pLoad->Duration);
+	CCABusyFraction = (UCHAR) (pAd->StaCfg.CLBusyBytes / pAd->StaCfg.CLFactor / pLoad->Duration);
+	if (CCABusyFraction < 10)
+			CCABusyFraction = (UCHAR) (pAd->StaCfg.CLBusyBytes / 3 / pLoad->Duration) + 1;
+
+	pLoad->CCABusy = CCABusyFraction;
+	DBGPRINT(RT_DEBUG_TRACE, ("CLBusyByte %ld, Duration %d, Result, %d\n", pAd->StaCfg.CLBusyBytes, pLoad->Duration, CCABusyFraction));
+
+	DBGPRINT(RT_DEBUG_TRACE, ("FrameReportLen %d\n", pAd->StaCfg.FrameReportLen));
+	pAd->StaCfg.FrameReportLen += (sizeof(MEASUREMENT_REPORT_ELEMENT) + sizeof(CHANNEL_LOAD_REPORT));
+	DBGPRINT(RT_DEBUG_TRACE, ("FrameReportLen %d\n", pAd->StaCfg.FrameReportLen));
+
+	// 4. Clear channel load measurement flag
+	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_RADIO_MEASUREMENT);
+
+	// 5. reset to idle state
+	pAd->Mlme.AironetMachine.CurrState = AIRONET_IDLE;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("ChannelLoadReportAction <-----\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	NoiseHistReportAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Index)
+{
+	PMEASUREMENT_REPORT_ELEMENT	pReport;
+	PNOISE_HIST_REPORT			pNoise;
+	PUCHAR						pDest;
+	UCHAR						i,NoiseCnt;
+	USHORT						TotalRPICnt, TotalRPISum;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("NoiseHistReportAction ----->\n"));
+
+	// 0. Disable Rx with promiscuous reception, make it back to normal
+	RTMP_IO_WRITE32(pAd, RX_FILTR_CFG, STANORMAL); // Staion not drop control frame will fail WiFi Certification.
+	// 1. Setup pointer for processing beacon & probe response
+	pDest = (PUCHAR) &pAd->StaCfg.FrameReportBuf[pAd->StaCfg.FrameReportLen];
+	pReport = (PMEASUREMENT_REPORT_ELEMENT) pDest;
+
+	// 2. Fill Measurement report element field.
+	pReport->Eid    = IE_MEASUREMENT_REPORT;
+	// Fixed Length at 16, not include Eid and length fields
+	pReport->Length = 16;
+	pReport->Token  = pAd->StaCfg.MeasurementRequest[Index].ReqElem.Token;
+	pReport->Mode   = pAd->StaCfg.MeasurementRequest[Index].ReqElem.Mode;
+	pReport->Type   = MSRN_TYPE_NOISE_HIST_REQ;
+
+	// 3. Fill noise histogram report measurement data
+	pDest += sizeof(MEASUREMENT_REPORT_ELEMENT);
+	pNoise  = (PNOISE_HIST_REPORT) pDest;
+	pNoise->Channel  = pAd->StaCfg.MeasurementRequest[Index].Measurement.Channel;
+	pNoise->Spare    = 0;
+	pNoise->Duration = pAd->StaCfg.MeasurementRequest[Index].Measurement.Duration;
+	// 4. Fill Noise histogram, the total RPI counts should be 0.4 * TU
+	//    We estimate 4000 normal packets received durning 10 seconds test.
+	//    Adjust it if required.
+	// 3 is a good value for pAd->StaCfg.NHFactor
+	// TotalRPICnt = pNoise->Duration * 3 / 10;
+	TotalRPICnt = pNoise->Duration * pAd->StaCfg.NHFactor / 10;
+	TotalRPISum = 0;
+
+	for (i = 0; i < 8; i++)
+	{
+		TotalRPISum += pAd->StaCfg.RPIDensity[i];
+		DBGPRINT(RT_DEBUG_TRACE, ("RPI %d Conuts %d\n", i, pAd->StaCfg.RPIDensity[i]));
+	}
+
+	// Double check if the counter is larger than our expectation.
+	// We will replace it with the total number plus a fraction.
+	if (TotalRPISum > TotalRPICnt)
+		TotalRPICnt = TotalRPISum + pNoise->Duration / 20;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Total RPI Conuts %d\n", TotalRPICnt));
+
+	// 5. Initialize noise count for the total summation of 0xff
+	NoiseCnt = 0;
+	for (i = 1; i < 8; i++)
+	{
+		pNoise->Density[i] = (UCHAR) (pAd->StaCfg.RPIDensity[i] * 255 / TotalRPICnt);
+		if ((pNoise->Density[i] == 0) && (pAd->StaCfg.RPIDensity[i] != 0))
+			pNoise->Density[i]++;
+		NoiseCnt += pNoise->Density[i];
+		DBGPRINT(RT_DEBUG_TRACE, ("Reported RPI[%d]  = 0x%02x\n", i, pNoise->Density[i]));
+	}
+
+	// 6. RPI[0] represents the rest of counts
+	pNoise->Density[0] = 0xff - NoiseCnt;
+	DBGPRINT(RT_DEBUG_TRACE, ("Reported RPI[0]  = 0x%02x\n", pNoise->Density[0]));
+
+	pAd->StaCfg.FrameReportLen += (sizeof(MEASUREMENT_REPORT_ELEMENT) + sizeof(NOISE_HIST_REPORT));
+
+	// 7. Clear channel load measurement flag
+	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_RADIO_MEASUREMENT);
+
+	// 8. reset to idle state
+	pAd->Mlme.AironetMachine.CurrState = AIRONET_IDLE;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("NoiseHistReportAction <-----\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Prepare Beacon report action,
+
+	Arguments:
+		pAd	Pointer	to our adapter
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	BeaconReportAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			Index)
+{
+	DBGPRINT(RT_DEBUG_TRACE, ("BeaconReportAction ----->\n"));
+
+	// Looks like we don't have anything thing need to do here.
+	// All measurement report already finished in AddBeaconReport
+	// The length is in the FrameReportLen
+
+	// reset Beacon index for next beacon request
+	pAd->StaCfg.LastBssIndex = 0xff;
+
+	// reset to idle state
+	pAd->Mlme.AironetMachine.CurrState = AIRONET_IDLE;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("BeaconReportAction <-----\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+		Index		Current BSSID in CCXBsstab entry index
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID	AironetAddBeaconReport(
+	IN	PRTMP_ADAPTER		pAd,
+	IN	ULONG				Index,
+	IN	PMLME_QUEUE_ELEM	pElem)
+{
+	PVOID						pMsg;
+	PUCHAR						pSrc, pDest;
+	UCHAR						ReqIdx;
+	ULONG						MsgLen;
+	USHORT						Length;
+	PFRAME_802_11				pFrame;
+	PMEASUREMENT_REPORT_ELEMENT	pReport;
+	PEID_STRUCT			        pEid;
+	PBEACON_REPORT				pBeaconReport;
+	PBSS_ENTRY					pBss;
+
+	// 0. Setup pointer for processing beacon & probe response
+	pMsg   = pElem->Msg;
+	MsgLen = pElem->MsgLen;
+	pFrame = (PFRAME_802_11) pMsg;
+	pSrc   = pFrame->Octet;				// Start from AP TSF
+	pBss   = (PBSS_ENTRY) &pAd->StaCfg.CCXBssTab.BssEntry[Index];
+	ReqIdx = pAd->StaCfg.CurrentRMReqIdx;
+
+	// 1 Check the Index, if we already create this entry, only update the average RSSI
+	if ((Index <= pAd->StaCfg.LastBssIndex) && (pAd->StaCfg.LastBssIndex != 0xff))
+	{
+		pDest  = (PUCHAR) &pAd->StaCfg.FrameReportBuf[pAd->StaCfg.BssReportOffset[Index]];
+		// Point to bss report information
+		pDest += sizeof(MEASUREMENT_REPORT_ELEMENT);
+		pBeaconReport = (PBEACON_REPORT) pDest;
+
+		// Update Rx power, in dBm
+		// Get the original RSSI readback from BBP
+		pBeaconReport->RxPower += pAd->BbpRssiToDbmDelta;
+		// Average the Rssi reading
+		pBeaconReport->RxPower  = (pBeaconReport->RxPower + pBss->Rssi) / 2;
+		// Get to dBm format
+		pBeaconReport->RxPower -= pAd->BbpRssiToDbmDelta;
+
+		DBGPRINT(RT_DEBUG_TRACE, ("Bssid %02x:%02x:%02x:%02x:%02x:%02x ",
+			pBss->Bssid[0], pBss->Bssid[1], pBss->Bssid[2],
+			pBss->Bssid[3], pBss->Bssid[4], pBss->Bssid[5]));
+		DBGPRINT(RT_DEBUG_TRACE, ("RxPower[%ld] Rssi %d, Avg Rssi %d\n", Index, (pBss->Rssi - pAd->BbpRssiToDbmDelta), pBeaconReport->RxPower - 256));
+		DBGPRINT(RT_DEBUG_TRACE, ("FrameReportLen = %d\n", pAd->StaCfg.BssReportOffset[Index]));
+
+		// Update other information here
+
+		// Done
+		return;
+	}
+
+	// 2. Update reported Index
+	pAd->StaCfg.LastBssIndex = Index;
+
+	// 3. Setup the buffer address for copying this BSSID into reporting frame
+	//    The offset should start after 802.11 header and report frame header.
+	pDest = (PUCHAR) &pAd->StaCfg.FrameReportBuf[pAd->StaCfg.FrameReportLen];
+
+	// 4. Save the start offset of each Bss in report frame
+	pAd->StaCfg.BssReportOffset[Index] = pAd->StaCfg.FrameReportLen;
+
+	// 5. Fill Measurement report fields
+	pReport = (PMEASUREMENT_REPORT_ELEMENT) pDest;
+	pReport->Eid = IE_MEASUREMENT_REPORT;
+	pReport->Length = 0;
+	pReport->Token  = pAd->StaCfg.MeasurementRequest[ReqIdx].ReqElem.Token;
+	pReport->Mode   = pAd->StaCfg.MeasurementRequest[ReqIdx].ReqElem.Mode;
+	pReport->Type   = MSRN_TYPE_BEACON_REQ;
+	Length          = sizeof(MEASUREMENT_REPORT_ELEMENT);
+	pDest          += sizeof(MEASUREMENT_REPORT_ELEMENT);
+
+	// 6. Start thebeacon report format
+	pBeaconReport = (PBEACON_REPORT) pDest;
+	pDest        += sizeof(BEACON_REPORT);
+	Length       += sizeof(BEACON_REPORT);
+
+	// 7. Copy Channel number
+	pBeaconReport->Channel        = pBss->Channel;
+	pBeaconReport->Spare          = 0;
+	pBeaconReport->Duration       = pAd->StaCfg.MeasurementRequest[ReqIdx].Measurement.Duration;
+	pBeaconReport->PhyType        = ((pBss->SupRateLen+pBss->ExtRateLen > 4) ? PHY_ERP : PHY_DSS);
+	// 8. Rx power, in dBm
+	pBeaconReport->RxPower        = pBss->Rssi - pAd->BbpRssiToDbmDelta;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Bssid %02x:%02x:%02x:%02x:%02x:%02x ",
+		pBss->Bssid[0], pBss->Bssid[1], pBss->Bssid[2],
+		pBss->Bssid[3], pBss->Bssid[4], pBss->Bssid[5]));
+	DBGPRINT(RT_DEBUG_TRACE, ("RxPower[%ld], Rssi %d\n", Index, pBeaconReport->RxPower - 256));
+	DBGPRINT(RT_DEBUG_TRACE, ("FrameReportLen = %d\n", pAd->StaCfg.FrameReportLen));
+
+	pBeaconReport->BeaconInterval = pBss->BeaconPeriod;
+	COPY_MAC_ADDR(pBeaconReport->BSSID, pFrame->Hdr.Addr3);
+	NdisMoveMemory(pBeaconReport->ParentTSF, pSrc, 4);
+	NdisMoveMemory(pBeaconReport->TargetTSF, &pElem->TimeStamp.u.LowPart, 4);
+	NdisMoveMemory(&pBeaconReport->TargetTSF[4], &pElem->TimeStamp.u.HighPart, 4);
+
+	// 9. Skip the beacon frame and offset to start of capabilityinfo since we already processed capabilityinfo
+	pSrc += (TIMESTAMP_LEN + 2);
+	pBeaconReport->CapabilityInfo = *(USHORT *)pSrc;
+
+	// 10. Point to start of element ID
+	pSrc += 2;
+	pEid = (PEID_STRUCT) pSrc;
+
+	// 11. Start process all variable Eid oayload and add the appropriate to the frame report
+	while (((PUCHAR) pEid + pEid->Len + 1) < ((PUCHAR) pFrame + MsgLen))
+	{
+		// Only limited EID are required to report for CCX 2. It includes SSID, Supported rate,
+		// FH paramenter set, DS parameter set, CF parameter set, IBSS parameter set,
+		// TIM (report first 4 bytes only, radio measurement capability
+		switch (pEid->Eid)
+		{
+			case IE_SSID:
+			case IE_SUPP_RATES:
+			case IE_FH_PARM:
+			case IE_DS_PARM:
+			case IE_CF_PARM:
+			case IE_IBSS_PARM:
+				NdisMoveMemory(pDest, pEid, pEid->Len + 2);
+				pDest  += (pEid->Len + 2);
+				Length += (pEid->Len + 2);
+				break;
+
+			case IE_MEASUREMENT_CAPABILITY:
+				// Since this IE is duplicated with WPA security IE, we has to do sanity check before
+				// recognize it.
+				// 1. It also has fixed 6 bytes IE length.
+				if (pEid->Len != 6)
+					break;
+				// 2. Check the Cisco Aironet OUI
+				if (NdisEqualMemory(CISCO_OUI, (pSrc + 2), 3))
+				{
+					// Matched, this is what we want
+					NdisMoveMemory(pDest, pEid, pEid->Len + 2);
+					pDest  += (pEid->Len + 2);
+					Length += (pEid->Len + 2);
+				}
+				break;
+
+			case IE_TIM:
+				if (pEid->Len > 4)
+				{
+					// May truncate and report the first 4 bytes only, with the eid & len, total should be 6
+					NdisMoveMemory(pDest, pEid, 6);
+					pDest  += 6;
+					Length += 6;
+				}
+				else
+				{
+					NdisMoveMemory(pDest, pEid, pEid->Len + 2);
+					pDest  += (pEid->Len + 2);
+					Length += (pEid->Len + 2);
+				}
+				break;
+
+			default:
+				break;
+		}
+		// 12. Move to next element ID
+		pSrc += (2 + pEid->Len);
+		pEid = (PEID_STRUCT) pSrc;
+	}
+
+	// 13. Update the length in the header, not include EID and length
+	pReport->Length = Length - 4;
+
+	// 14. Update the frame report buffer data length
+	pAd->StaCfg.FrameReportLen += Length;
+	DBGPRINT(RT_DEBUG_TRACE, ("FR len = %d\n", pAd->StaCfg.FrameReportLen));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+
+	Arguments:
+		Index		Current BSSID in CCXBsstab entry index
+
+	Return Value:
+
+	Note:
+
+	========================================================================
+*/
+VOID	AironetCreateBeaconReportFromBssTable(
+	IN	PRTMP_ADAPTER		pAd)
+{
+	PMEASUREMENT_REPORT_ELEMENT	pReport;
+	PBEACON_REPORT				pBeaconReport;
+	UCHAR						Index, ReqIdx;
+	USHORT						Length;
+	PUCHAR						pDest;
+	PBSS_ENTRY					pBss;
+
+	// 0. setup base pointer
+	ReqIdx = pAd->StaCfg.CurrentRMReqIdx;
+
+	for (Index = 0; Index < pAd->StaCfg.CCXBssTab.BssNr; Index++)
+	{
+		// 1. Setup the buffer address for copying this BSSID into reporting frame
+		//    The offset should start after 802.11 header and report frame header.
+		pDest  = (PUCHAR) &pAd->StaCfg.FrameReportBuf[pAd->StaCfg.FrameReportLen];
+		pBss   = (PBSS_ENTRY) &pAd->StaCfg.CCXBssTab.BssEntry[Index];
+		Length = 0;
+
+		// 2. Fill Measurement report fields
+		pReport         = (PMEASUREMENT_REPORT_ELEMENT) pDest;
+		pReport->Eid    = IE_MEASUREMENT_REPORT;
+		pReport->Length = 0;
+		pReport->Token  = pAd->StaCfg.MeasurementRequest[ReqIdx].ReqElem.Token;
+		pReport->Mode   = pAd->StaCfg.MeasurementRequest[ReqIdx].ReqElem.Mode;
+		pReport->Type   = MSRN_TYPE_BEACON_REQ;
+		Length          = sizeof(MEASUREMENT_REPORT_ELEMENT);
+		pDest          += sizeof(MEASUREMENT_REPORT_ELEMENT);
+
+		// 3. Start the beacon report format
+		pBeaconReport = (PBEACON_REPORT) pDest;
+		pDest        += sizeof(BEACON_REPORT);
+		Length       += sizeof(BEACON_REPORT);
+
+		// 4. Copy Channel number
+		pBeaconReport->Channel        = pBss->Channel;
+		pBeaconReport->Spare          = 0;
+		pBeaconReport->Duration       = pAd->StaCfg.MeasurementRequest[ReqIdx].Measurement.Duration;
+		pBeaconReport->PhyType        = ((pBss->SupRateLen+pBss->ExtRateLen > 4) ? PHY_ERP : PHY_DSS);
+		pBeaconReport->RxPower        = pBss->Rssi - pAd->BbpRssiToDbmDelta;
+		pBeaconReport->BeaconInterval = pBss->BeaconPeriod;
+		pBeaconReport->CapabilityInfo = pBss->CapabilityInfo;
+		COPY_MAC_ADDR(pBeaconReport->BSSID, pBss->Bssid);
+		NdisMoveMemory(pBeaconReport->ParentTSF, pBss->PTSF, 4);
+		NdisMoveMemory(pBeaconReport->TargetTSF, pBss->TTSF, 8);
+
+		// 5. Create SSID
+		*pDest++ = 0x00;
+		*pDest++ = pBss->SsidLen;
+		NdisMoveMemory(pDest, pBss->Ssid, pBss->SsidLen);
+		pDest  += pBss->SsidLen;
+		Length += (2 + pBss->SsidLen);
+
+		// 6. Create SupportRates
+		*pDest++ = 0x01;
+		*pDest++ = pBss->SupRateLen;
+		NdisMoveMemory(pDest, pBss->SupRate, pBss->SupRateLen);
+		pDest  += pBss->SupRateLen;
+		Length += (2 + pBss->SupRateLen);
+
+		// 7. DS Parameter
+		*pDest++ = 0x03;
+		*pDest++ = 1;
+		*pDest++ = pBss->Channel;
+		Length  += 3;
+
+		// 8. IBSS parameter if presents
+		if (pBss->BssType == BSS_ADHOC)
+		{
+			*pDest++ = 0x06;
+			*pDest++ = 2;
+			*(PUSHORT) pDest = pBss->AtimWin;
+			pDest   += 2;
+			Length  += 4;
+		}
+
+		// 9. Update length field, not include EID and length
+		pReport->Length = Length - 4;
+
+		// 10. Update total frame size
+		pAd->StaCfg.FrameReportLen += Length;
+	}
+}
diff --git a/drivers/staging/rt2870/sta/assoc.c b/drivers/staging/rt2870/sta/assoc.c
new file mode 100644
index 00000000000..a76dab500bc
--- /dev/null
+++ b/drivers/staging/rt2870/sta/assoc.c
@@ -0,0 +1,2039 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	assoc.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John		2004-9-3		porting from RT2500
+*/
+#include "../rt_config.h"
+
+UCHAR	CipherWpaTemplate[] = {
+		0xdd, 					// WPA IE
+		0x16,					// Length
+		0x00, 0x50, 0xf2, 0x01,	// oui
+		0x01, 0x00,				// Version
+		0x00, 0x50, 0xf2, 0x02,	// Multicast
+		0x01, 0x00,				// Number of unicast
+		0x00, 0x50, 0xf2, 0x02,	// unicast
+		0x01, 0x00,				// number of authentication method
+		0x00, 0x50, 0xf2, 0x01	// authentication
+		};
+
+UCHAR	CipherWpa2Template[] = {
+		0x30,					// RSN IE
+		0x14,					// Length
+		0x01, 0x00,				// Version
+		0x00, 0x0f, 0xac, 0x02,	// group cipher, TKIP
+		0x01, 0x00,				// number of pairwise
+		0x00, 0x0f, 0xac, 0x02,	// unicast
+		0x01, 0x00,				// number of authentication method
+		0x00, 0x0f, 0xac, 0x02,	// authentication
+		0x00, 0x00,				// RSN capability
+		};
+
+UCHAR	Ccx2IeInfo[] = { 0x00, 0x40, 0x96, 0x03, 0x02};
+
+/*
+	==========================================================================
+	Description:
+		association state machine init, including state transition and timer init
+	Parameters:
+		S - pointer to the association state machine
+
+	IRQL = PASSIVE_LEVEL
+
+	==========================================================================
+ */
+VOID AssocStateMachineInit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN  STATE_MACHINE *S,
+	OUT STATE_MACHINE_FUNC Trans[])
+{
+	StateMachineInit(S, Trans, MAX_ASSOC_STATE, MAX_ASSOC_MSG, (STATE_MACHINE_FUNC)Drop, ASSOC_IDLE, ASSOC_MACHINE_BASE);
+
+	// first column
+	StateMachineSetAction(S, ASSOC_IDLE, MT2_MLME_ASSOC_REQ, (STATE_MACHINE_FUNC)MlmeAssocReqAction);
+	StateMachineSetAction(S, ASSOC_IDLE, MT2_MLME_REASSOC_REQ, (STATE_MACHINE_FUNC)MlmeReassocReqAction);
+	StateMachineSetAction(S, ASSOC_IDLE, MT2_MLME_DISASSOC_REQ, (STATE_MACHINE_FUNC)MlmeDisassocReqAction);
+	StateMachineSetAction(S, ASSOC_IDLE, MT2_PEER_DISASSOC_REQ, (STATE_MACHINE_FUNC)PeerDisassocAction);
+
+	// second column
+	StateMachineSetAction(S, ASSOC_WAIT_RSP, MT2_MLME_ASSOC_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenAssoc);
+	StateMachineSetAction(S, ASSOC_WAIT_RSP, MT2_MLME_REASSOC_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenReassoc);
+	StateMachineSetAction(S, ASSOC_WAIT_RSP, MT2_MLME_DISASSOC_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenDisassociate);
+	StateMachineSetAction(S, ASSOC_WAIT_RSP, MT2_PEER_DISASSOC_REQ, (STATE_MACHINE_FUNC)PeerDisassocAction);
+	StateMachineSetAction(S, ASSOC_WAIT_RSP, MT2_PEER_ASSOC_RSP, (STATE_MACHINE_FUNC)PeerAssocRspAction);
+	//
+	// Patch 3Com AP MOde:3CRWE454G72
+	// We send Assoc request frame to this AP, it always send Reassoc Rsp not Associate Rsp.
+	//
+	StateMachineSetAction(S, ASSOC_WAIT_RSP, MT2_PEER_REASSOC_RSP, (STATE_MACHINE_FUNC)PeerAssocRspAction);
+	StateMachineSetAction(S, ASSOC_WAIT_RSP, MT2_ASSOC_TIMEOUT, (STATE_MACHINE_FUNC)AssocTimeoutAction);
+
+	// third column
+	StateMachineSetAction(S, REASSOC_WAIT_RSP, MT2_MLME_ASSOC_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenAssoc);
+	StateMachineSetAction(S, REASSOC_WAIT_RSP, MT2_MLME_REASSOC_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenReassoc);
+	StateMachineSetAction(S, REASSOC_WAIT_RSP, MT2_MLME_DISASSOC_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenDisassociate);
+	StateMachineSetAction(S, REASSOC_WAIT_RSP, MT2_PEER_DISASSOC_REQ, (STATE_MACHINE_FUNC)PeerDisassocAction);
+	StateMachineSetAction(S, REASSOC_WAIT_RSP, MT2_PEER_REASSOC_RSP, (STATE_MACHINE_FUNC)PeerReassocRspAction);
+	//
+	// Patch, AP doesn't send Reassociate Rsp frame to Station.
+	//
+	StateMachineSetAction(S, REASSOC_WAIT_RSP, MT2_PEER_ASSOC_RSP, (STATE_MACHINE_FUNC)PeerReassocRspAction);
+	StateMachineSetAction(S, REASSOC_WAIT_RSP, MT2_REASSOC_TIMEOUT, (STATE_MACHINE_FUNC)ReassocTimeoutAction);
+
+	// fourth column
+	StateMachineSetAction(S, DISASSOC_WAIT_RSP, MT2_MLME_ASSOC_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenAssoc);
+	StateMachineSetAction(S, DISASSOC_WAIT_RSP, MT2_MLME_REASSOC_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenReassoc);
+	StateMachineSetAction(S, DISASSOC_WAIT_RSP, MT2_MLME_DISASSOC_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenDisassociate);
+	StateMachineSetAction(S, DISASSOC_WAIT_RSP, MT2_PEER_DISASSOC_REQ, (STATE_MACHINE_FUNC)PeerDisassocAction);
+	StateMachineSetAction(S, DISASSOC_WAIT_RSP, MT2_DISASSOC_TIMEOUT, (STATE_MACHINE_FUNC)DisassocTimeoutAction);
+
+	// initialize the timer
+	RTMPInitTimer(pAd, &pAd->MlmeAux.AssocTimer, GET_TIMER_FUNCTION(AssocTimeout), pAd, FALSE);
+	RTMPInitTimer(pAd, &pAd->MlmeAux.ReassocTimer, GET_TIMER_FUNCTION(ReassocTimeout), pAd, FALSE);
+	RTMPInitTimer(pAd, &pAd->MlmeAux.DisassocTimer, GET_TIMER_FUNCTION(DisassocTimeout), pAd, FALSE);
+}
+
+/*
+	==========================================================================
+	Description:
+		Association timeout procedure. After association timeout, this function
+		will be called and it will put a message into the MLME queue
+	Parameters:
+		Standard timer parameters
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AssocTimeout(IN PVOID SystemSpecific1,
+				 IN PVOID FunctionContext,
+				 IN PVOID SystemSpecific2,
+				 IN PVOID SystemSpecific3)
+{
+	RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return;
+
+	MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_ASSOC_TIMEOUT, 0, NULL);
+	RT28XX_MLME_HANDLER(pAd);
+}
+
+/*
+	==========================================================================
+	Description:
+		Reassociation timeout procedure. After reassociation timeout, this
+		function will be called and put a message into the MLME queue
+	Parameters:
+		Standard timer parameters
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID ReassocTimeout(IN PVOID SystemSpecific1,
+					IN PVOID FunctionContext,
+					IN PVOID SystemSpecific2,
+					IN PVOID SystemSpecific3)
+{
+	RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return;
+
+	MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_REASSOC_TIMEOUT, 0, NULL);
+	RT28XX_MLME_HANDLER(pAd);
+}
+
+/*
+	==========================================================================
+	Description:
+		Disassociation timeout procedure. After disassociation timeout, this
+		function will be called and put a message into the MLME queue
+	Parameters:
+		Standard timer parameters
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID DisassocTimeout(IN PVOID SystemSpecific1,
+					IN PVOID FunctionContext,
+					IN PVOID SystemSpecific2,
+					IN PVOID SystemSpecific3)
+{
+	RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return;
+
+	MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_DISASSOC_TIMEOUT, 0, NULL);
+	RT28XX_MLME_HANDLER(pAd);
+}
+
+/*
+	==========================================================================
+	Description:
+		mlme assoc req handling procedure
+	Parameters:
+		Adapter - Adapter pointer
+		Elem - MLME Queue Element
+	Pre:
+		the station has been authenticated and the following information is stored in the config
+			-# SSID
+			-# supported rates and their length
+			-# listen interval (Adapter->StaCfg.default_listen_count)
+			-# Transmit power  (Adapter->StaCfg.tx_power)
+	Post  :
+		-# An association request frame is generated and sent to the air
+		-# Association timer starts
+		-# Association state -> ASSOC_WAIT_RSP
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID MlmeAssocReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR			ApAddr[6];
+	HEADER_802_11	AssocHdr;
+	UCHAR			Ccx2Len = 5;
+	UCHAR			WmeIe[9] = {IE_VENDOR_SPECIFIC, 0x07, 0x00, 0x50, 0xf2, 0x02, 0x00, 0x01, 0x00};
+	USHORT			ListenIntv;
+	ULONG			Timeout;
+	USHORT			CapabilityInfo;
+	BOOLEAN			TimerCancelled;
+	PUCHAR			pOutBuffer = NULL;
+	NDIS_STATUS		NStatus;
+	ULONG			FrameLen = 0;
+	ULONG			tmp;
+	USHORT			VarIesOffset;
+	UCHAR			CkipFlag;
+	UCHAR			CkipNegotiationBuffer[CKIP_NEGOTIATION_LENGTH];
+	UCHAR			AironetCkipIe = IE_AIRONET_CKIP;
+	UCHAR			AironetCkipLen = CKIP_NEGOTIATION_LENGTH;
+	UCHAR			AironetIPAddressIE = IE_AIRONET_IPADDRESS;
+	UCHAR			AironetIPAddressLen = AIRONET_IPADDRESS_LENGTH;
+	UCHAR			AironetIPAddressBuffer[AIRONET_IPADDRESS_LENGTH] = {0x00, 0x40, 0x96, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00};
+	USHORT			Status;
+
+	// Block all authentication request durning WPA block period
+	if (pAd->StaCfg.bBlockAssoc == TRUE)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - Block Assoc request durning WPA block period!\n"));
+		pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+		Status = MLME_STATE_MACHINE_REJECT;
+		MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_ASSOC_CONF, 2, &Status);
+	}
+	// check sanity first
+	else if (MlmeAssocReqSanity(pAd, Elem->Msg, Elem->MsgLen, ApAddr, &CapabilityInfo, &Timeout, &ListenIntv))
+	{
+		RTMPCancelTimer(&pAd->MlmeAux.AssocTimer, &TimerCancelled);
+		COPY_MAC_ADDR(pAd->MlmeAux.Bssid, ApAddr);
+
+		// Get an unused nonpaged memory
+		NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);
+		if (NStatus != NDIS_STATUS_SUCCESS)
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("ASSOC - MlmeAssocReqAction() allocate memory failed \n"));
+			pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+			Status = MLME_FAIL_NO_RESOURCE;
+			MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_ASSOC_CONF, 2, &Status);
+			return;
+		}
+
+		// Add by James 03/06/27
+		pAd->StaCfg.AssocInfo.Length = sizeof(NDIS_802_11_ASSOCIATION_INFORMATION);
+		// Association don't need to report MAC address
+		pAd->StaCfg.AssocInfo.AvailableRequestFixedIEs =
+			NDIS_802_11_AI_REQFI_CAPABILITIES | NDIS_802_11_AI_REQFI_LISTENINTERVAL;
+		pAd->StaCfg.AssocInfo.RequestFixedIEs.Capabilities = CapabilityInfo;
+		pAd->StaCfg.AssocInfo.RequestFixedIEs.ListenInterval = ListenIntv;
+		// Only reassociate need this
+		//COPY_MAC_ADDR(pAd->StaCfg.AssocInfo.RequestFixedIEs.CurrentAPAddress, ApAddr);
+		pAd->StaCfg.AssocInfo.OffsetRequestIEs = sizeof(NDIS_802_11_ASSOCIATION_INFORMATION);
+
+        NdisZeroMemory(pAd->StaCfg.ReqVarIEs, MAX_VIE_LEN);
+		// First add SSID
+		VarIesOffset = 0;
+		NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, &SsidIe, 1);
+		VarIesOffset += 1;
+		NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, &pAd->MlmeAux.SsidLen, 1);
+		VarIesOffset += 1;
+		NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen);
+		VarIesOffset += pAd->MlmeAux.SsidLen;
+
+		// Second add Supported rates
+		NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, &SupRateIe, 1);
+		VarIesOffset += 1;
+		NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, &pAd->MlmeAux.SupRateLen, 1);
+		VarIesOffset += 1;
+		NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, pAd->MlmeAux.SupRate, pAd->MlmeAux.SupRateLen);
+		VarIesOffset += pAd->MlmeAux.SupRateLen;
+		// End Add by James
+
+        if ((pAd->CommonCfg.Channel > 14) &&
+            (pAd->CommonCfg.bIEEE80211H == TRUE))
+            CapabilityInfo |= 0x0100;
+
+		DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - Send ASSOC request...\n"));
+		MgtMacHeaderInit(pAd, &AssocHdr, SUBTYPE_ASSOC_REQ, 0, ApAddr, ApAddr);
+
+		// Build basic frame first
+		MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+						  sizeof(HEADER_802_11),	&AssocHdr,
+						  2,						&CapabilityInfo,
+						  2,						&ListenIntv,
+						  1,						&SsidIe,
+						  1,						&pAd->MlmeAux.SsidLen,
+						  pAd->MlmeAux.SsidLen, 	pAd->MlmeAux.Ssid,
+						  1,						&SupRateIe,
+						  1,						&pAd->MlmeAux.SupRateLen,
+						  pAd->MlmeAux.SupRateLen,  pAd->MlmeAux.SupRate,
+						  END_OF_ARGS);
+
+		if (pAd->MlmeAux.ExtRateLen != 0)
+		{
+			MakeOutgoingFrame(pOutBuffer + FrameLen,    &tmp,
+							  1,                        &ExtRateIe,
+							  1,                        &pAd->MlmeAux.ExtRateLen,
+							  pAd->MlmeAux.ExtRateLen,  pAd->MlmeAux.ExtRate,
+							  END_OF_ARGS);
+			FrameLen += tmp;
+		}
+
+#ifdef DOT11_N_SUPPORT
+		// HT
+		if ((pAd->MlmeAux.HtCapabilityLen > 0) && (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+		{
+			ULONG TmpLen;
+			UCHAR HtLen;
+			UCHAR BROADCOM[4] = {0x0, 0x90, 0x4c, 0x33};
+			if (pAd->StaActive.SupportedPhyInfo.bPreNHt == TRUE)
+			{
+				HtLen = SIZE_HT_CAP_IE + 4;
+				MakeOutgoingFrame(pOutBuffer + FrameLen,            &TmpLen,
+							  1,                                &WpaIe,
+							  1,                                &HtLen,
+							  4,                                &BROADCOM[0],
+							 pAd->MlmeAux.HtCapabilityLen,          &pAd->MlmeAux.HtCapability,
+							  END_OF_ARGS);
+			}
+			else
+			{
+#ifdef RT_BIG_ENDIAN
+		        HT_CAPABILITY_IE HtCapabilityTmp;
+#endif
+
+#ifndef RT_BIG_ENDIAN
+				MakeOutgoingFrame(pOutBuffer + FrameLen,            &TmpLen,
+							  1,                                &HtCapIe,
+							  1,                                &pAd->MlmeAux.HtCapabilityLen,
+							 pAd->MlmeAux.HtCapabilityLen,          &pAd->MlmeAux.HtCapability,
+							  END_OF_ARGS);
+#else
+                NdisZeroMemory(&HtCapabilityTmp, sizeof(HT_CAPABILITY_IE));
+                NdisMoveMemory(&HtCapabilityTmp, &pAd->MlmeAux.HtCapability, pAd->MlmeAux.HtCapabilityLen);
+        		*(USHORT *)(&HtCapabilityTmp.HtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.HtCapInfo));
+        		*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo));
+
+        		MakeOutgoingFrame(pOutBuffer + FrameLen,         &TmpLen,
+        							1,                           &HtCapIe,
+        							1,                           &pAd->MlmeAux.HtCapabilityLen,
+        							pAd->MlmeAux.HtCapabilityLen,&HtCapabilityTmp,
+        							END_OF_ARGS);
+#endif
+			}
+			FrameLen += TmpLen;
+		}
+#endif // DOT11_N_SUPPORT //
+
+		// add Ralink proprietary IE to inform AP this STA is going to use AGGREGATION or PIGGY-BACK+AGGREGATION
+		// Case I: (Aggregation + Piggy-Back)
+		// 1. user enable aggregation, AND
+		// 2. Mac support piggy-back
+		// 3. AP annouces it's PIGGY-BACK+AGGREGATION-capable in BEACON
+		// Case II: (Aggregation)
+		// 1. user enable aggregation, AND
+		// 2. AP annouces it's AGGREGATION-capable in BEACON
+		if (pAd->CommonCfg.bAggregationCapable)
+		{
+			if ((pAd->CommonCfg.bPiggyBackCapable) && ((pAd->MlmeAux.APRalinkIe & 0x00000003) == 3))
+			{
+				ULONG TmpLen;
+				UCHAR RalinkIe[9] = {IE_VENDOR_SPECIFIC, 7, 0x00, 0x0c, 0x43, 0x03, 0x00, 0x00, 0x00};
+				MakeOutgoingFrame(pOutBuffer+FrameLen,           &TmpLen,
+								  9,                             RalinkIe,
+								  END_OF_ARGS);
+				FrameLen += TmpLen;
+			}
+			else if (pAd->MlmeAux.APRalinkIe & 0x00000001)
+			{
+				ULONG TmpLen;
+				UCHAR RalinkIe[9] = {IE_VENDOR_SPECIFIC, 7, 0x00, 0x0c, 0x43, 0x01, 0x00, 0x00, 0x00};
+				MakeOutgoingFrame(pOutBuffer+FrameLen,           &TmpLen,
+								  9,                             RalinkIe,
+								  END_OF_ARGS);
+				FrameLen += TmpLen;
+			}
+		}
+		else
+		{
+			ULONG TmpLen;
+			UCHAR RalinkIe[9] = {IE_VENDOR_SPECIFIC, 7, 0x00, 0x0c, 0x43, 0x06, 0x00, 0x00, 0x00};
+			MakeOutgoingFrame(pOutBuffer+FrameLen,		 &TmpLen,
+							  9,						 RalinkIe,
+							  END_OF_ARGS);
+			FrameLen += TmpLen;
+		}
+
+		if (pAd->MlmeAux.APEdcaParm.bValid)
+		{
+			if (pAd->CommonCfg.bAPSDCapable && pAd->MlmeAux.APEdcaParm.bAPSDCapable)
+			{
+				QBSS_STA_INFO_PARM QosInfo;
+
+				NdisZeroMemory(&QosInfo, sizeof(QBSS_STA_INFO_PARM));
+				QosInfo.UAPSD_AC_BE = pAd->CommonCfg.bAPSDAC_BE;
+				QosInfo.UAPSD_AC_BK = pAd->CommonCfg.bAPSDAC_BK;
+				QosInfo.UAPSD_AC_VI = pAd->CommonCfg.bAPSDAC_VI;
+				QosInfo.UAPSD_AC_VO = pAd->CommonCfg.bAPSDAC_VO;
+				QosInfo.MaxSPLength = pAd->CommonCfg.MaxSPLength;
+				WmeIe[8] |= *(PUCHAR)&QosInfo;
+			}
+			else
+			{
+                // The Parameter Set Count is set to ¡§0¡¨ in the association request frames
+                // WmeIe[8] |= (pAd->MlmeAux.APEdcaParm.EdcaUpdateCount & 0x0f);
+			}
+
+			MakeOutgoingFrame(pOutBuffer + FrameLen,    &tmp,
+							  9,                        &WmeIe[0],
+							  END_OF_ARGS);
+			FrameLen += tmp;
+		}
+
+		//
+		// Let WPA(#221) Element ID on the end of this association frame.
+		// Otherwise some AP will fail on parsing Element ID and set status fail on Assoc Rsp.
+		// For example: Put Vendor Specific IE on the front of WPA IE.
+		// This happens on AP (Model No:Linksys WRK54G)
+		//
+		if (((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+            (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK) ||
+            (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
+            (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2)
+			)
+            )
+		{
+			UCHAR RSNIe = IE_WPA;
+
+			if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK) ||
+                (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2))
+			{
+				RSNIe = IE_WPA2;
+			}
+
+            	RTMPMakeRSNIE(pAd, pAd->StaCfg.AuthMode, pAd->StaCfg.WepStatus, BSS0);
+
+            // Check for WPA PMK cache list
+			if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2)
+			{
+			    INT     idx;
+                BOOLEAN FoundPMK = FALSE;
+				// Search chched PMKID, append it if existed
+				for (idx = 0; idx < PMKID_NO; idx++)
+				{
+					if (NdisEqualMemory(ApAddr, &pAd->StaCfg.SavedPMK[idx].BSSID, 6))
+					{
+						FoundPMK = TRUE;
+						break;
+					}
+				}
+
+				if (FoundPMK)
+				{
+					// Set PMK number
+					*(PUSHORT) &pAd->StaCfg.RSN_IE[pAd->StaCfg.RSNIE_Len] = 1;
+					NdisMoveMemory(&pAd->StaCfg.RSN_IE[pAd->StaCfg.RSNIE_Len + 2], &pAd->StaCfg.SavedPMK[idx].PMKID, 16);
+                    pAd->StaCfg.RSNIE_Len += 18;
+				}
+			}
+
+			{
+				MakeOutgoingFrame(pOutBuffer + FrameLen,    		&tmp,
+				              		1,                              &RSNIe,
+		                        	1,                              &pAd->StaCfg.RSNIE_Len,
+		                        	pAd->StaCfg.RSNIE_Len,			pAd->StaCfg.RSN_IE,
+		                        	END_OF_ARGS);
+			}
+
+			FrameLen += tmp;
+
+			{
+	            // Append Variable IE
+	            NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, &RSNIe, 1);
+	            VarIesOffset += 1;
+	            NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, &pAd->StaCfg.RSNIE_Len, 1);
+	            VarIesOffset += 1;
+			}
+			NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, pAd->StaCfg.RSN_IE, pAd->StaCfg.RSNIE_Len);
+			VarIesOffset += pAd->StaCfg.RSNIE_Len;
+
+			// Set Variable IEs Length
+			pAd->StaCfg.ReqVarIELen = VarIesOffset;
+		}
+
+		// We have update that at PeerBeaconAtJoinRequest()
+		CkipFlag = pAd->StaCfg.CkipFlag;
+		if (CkipFlag != 0)
+		{
+			NdisZeroMemory(CkipNegotiationBuffer, CKIP_NEGOTIATION_LENGTH);
+			CkipNegotiationBuffer[2] = 0x66;
+			// Make it try KP & MIC, since we have to follow the result from AssocRsp
+			CkipNegotiationBuffer[8] = 0x18;
+			CkipNegotiationBuffer[CKIP_NEGOTIATION_LENGTH - 1] = 0x22;
+			CkipFlag = 0x18;
+
+			MakeOutgoingFrame(pOutBuffer + FrameLen, 	&tmp,
+						1,						  		&AironetCkipIe,
+						1,						  		&AironetCkipLen,
+						AironetCkipLen, 		  		CkipNegotiationBuffer,
+						END_OF_ARGS);
+			FrameLen += tmp;
+		}
+
+		// Add CCX v2 request if CCX2 admin state is on
+		if (pAd->StaCfg.CCXControl.field.Enable == 1)
+		{
+
+			//
+			// Add AironetIPAddressIE for Cisco CCX 2.X
+			// Add CCX Version
+			//
+			MakeOutgoingFrame(pOutBuffer + FrameLen, &tmp,
+						1,							&AironetIPAddressIE,
+						1,							&AironetIPAddressLen,
+						AironetIPAddressLen,		AironetIPAddressBuffer,
+						1,							&Ccx2Ie,
+						1,							&Ccx2Len,
+						Ccx2Len,				    Ccx2IeInfo,
+						END_OF_ARGS);
+			FrameLen += tmp;
+
+			//
+			// Add CipherSuite CCKM or LeapTkip if setting.
+			//
+#ifdef LEAP_SUPPORT
+			if (LEAP_CCKM_ON(pAd))
+			{
+				MakeOutgoingFrame(pOutBuffer + FrameLen,	&tmp,
+						CipherSuiteCiscoCCKMLen,		CipherSuiteCiscoCCKM,
+						END_OF_ARGS);
+				FrameLen += tmp;
+
+				// Third add RSN
+				NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, CipherSuiteCiscoCCKM, CipherSuiteCiscoCCKMLen); //Save CipherSuite
+				VarIesOffset += CipherSuiteCiscoCCKMLen;
+			}
+			else if ((pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP) && (pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled))
+			{
+				MakeOutgoingFrame(pOutBuffer + FrameLen, &tmp,
+						CipherSuiteCCXTkipLen,	    CipherSuiteCCXTkip,
+						END_OF_ARGS);
+				FrameLen += tmp;
+
+				// Third add RSN
+				NdisMoveMemory(pAd->StaCfg.ReqVarIEs + VarIesOffset, CipherSuiteCCXTkip, CipherSuiteCCXTkipLen);
+				VarIesOffset += CipherSuiteCCXTkipLen;
+			}
+#endif // LEAP_SUPPORT //
+
+			// Add by James 03/06/27
+			// Set Variable IEs Length
+			pAd->StaCfg.ReqVarIELen = VarIesOffset;
+			pAd->StaCfg.AssocInfo.RequestIELength = VarIesOffset;
+
+			// OffsetResponseIEs follow ReqVarIE
+			pAd->StaCfg.AssocInfo.OffsetResponseIEs = sizeof(NDIS_802_11_ASSOCIATION_INFORMATION) + pAd->StaCfg.ReqVarIELen;
+			// End Add by James
+		}
+
+
+		MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+		MlmeFreeMemory(pAd, pOutBuffer);
+
+		RTMPSetTimer(&pAd->MlmeAux.AssocTimer, Timeout);
+		pAd->Mlme.AssocMachine.CurrState = ASSOC_WAIT_RSP;
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("ASSOC - MlmeAssocReqAction() sanity check failed. BUG!!!!!! \n"));
+		pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+		Status = MLME_INVALID_FORMAT;
+		MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_ASSOC_CONF, 2, &Status);
+	}
+
+}
+
+/*
+	==========================================================================
+	Description:
+		mlme reassoc req handling procedure
+	Parameters:
+		Elem -
+	Pre:
+		-# SSID  (Adapter->StaCfg.ssid[])
+		-# BSSID (AP address, Adapter->StaCfg.bssid)
+		-# Supported rates (Adapter->StaCfg.supported_rates[])
+		-# Supported rates length (Adapter->StaCfg.supported_rates_len)
+		-# Tx power (Adapter->StaCfg.tx_power)
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID MlmeReassocReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR			ApAddr[6];
+	HEADER_802_11	ReassocHdr;
+	UCHAR			Ccx2Len = 5;
+	UCHAR			WmeIe[9] = {IE_VENDOR_SPECIFIC, 0x07, 0x00, 0x50, 0xf2, 0x02, 0x00, 0x01, 0x00};
+	USHORT			CapabilityInfo, ListenIntv;
+	ULONG			Timeout;
+	ULONG			FrameLen = 0;
+	BOOLEAN			TimerCancelled;
+	NDIS_STATUS		NStatus;
+	ULONG			tmp;
+	PUCHAR			pOutBuffer = NULL;
+//CCX 2.X
+#ifdef LEAP_SUPPORT
+	UCHAR			CkipFlag;
+	UCHAR			CkipNegotiationBuffer[CKIP_NEGOTIATION_LENGTH];
+	UCHAR			AironetCkipIe = IE_AIRONET_CKIP;
+	UCHAR			AironetCkipLen = CKIP_NEGOTIATION_LENGTH;
+	UCHAR			AironetIPAddressIE = IE_AIRONET_IPADDRESS;
+	UCHAR			AironetIPAddressLen = AIRONET_IPADDRESS_LENGTH;
+	UCHAR			AironetIPAddressBuffer[AIRONET_IPADDRESS_LENGTH] = {0x00, 0x40, 0x96, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00};
+	UCHAR			AironetCCKMReassocIE = IE_AIRONET_CCKMREASSOC;
+	UCHAR			AironetCCKMReassocLen = AIRONET_CCKMREASSOC_LENGTH;
+	UCHAR			AironetCCKMReassocBuffer[AIRONET_CCKMREASSOC_LENGTH];
+	UCHAR			AironetOUI[] = {0x00, 0x40, 0x96, 0x00};
+	UCHAR			MICMN[16];
+	UCHAR			CalcMicBuffer[80];
+	ULONG			CalcMicBufferLen = 0;
+#endif // LEAP_SUPPORT //
+	USHORT			Status;
+
+	// Block all authentication request durning WPA block period
+	if (pAd->StaCfg.bBlockAssoc == TRUE)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - Block ReAssoc request durning WPA block period!\n"));
+		pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+		Status = MLME_STATE_MACHINE_REJECT;
+		MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_REASSOC_CONF, 2, &Status);
+	}
+	// the parameters are the same as the association
+	else if(MlmeAssocReqSanity(pAd, Elem->Msg, Elem->MsgLen, ApAddr, &CapabilityInfo, &Timeout, &ListenIntv))
+	{
+		RTMPCancelTimer(&pAd->MlmeAux.ReassocTimer, &TimerCancelled);
+
+		NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+		if(NStatus != NDIS_STATUS_SUCCESS)
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("ASSOC - MlmeReassocReqAction() allocate memory failed \n"));
+			pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+			Status = MLME_FAIL_NO_RESOURCE;
+			MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_REASSOC_CONF, 2, &Status);
+			return;
+		}
+
+		COPY_MAC_ADDR(pAd->MlmeAux.Bssid, ApAddr);
+
+		// make frame, use bssid as the AP address??
+		DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - Send RE-ASSOC request...\n"));
+		MgtMacHeaderInit(pAd, &ReassocHdr, SUBTYPE_REASSOC_REQ, 0, ApAddr, ApAddr);
+		MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+						  sizeof(HEADER_802_11),    &ReassocHdr,
+						  2,                        &CapabilityInfo,
+						  2,                        &ListenIntv,
+						  MAC_ADDR_LEN,             ApAddr,
+						  1,                        &SsidIe,
+						  1,                        &pAd->MlmeAux.SsidLen,
+						  pAd->MlmeAux.SsidLen,     pAd->MlmeAux.Ssid,
+						  1,                        &SupRateIe,
+						  1,						&pAd->MlmeAux.SupRateLen,
+						  pAd->MlmeAux.SupRateLen,  pAd->MlmeAux.SupRate,
+						  END_OF_ARGS);
+
+		if (pAd->MlmeAux.ExtRateLen != 0)
+		{
+			MakeOutgoingFrame(pOutBuffer + FrameLen,        &tmp,
+							  1,                            &ExtRateIe,
+							  1,                            &pAd->MlmeAux.ExtRateLen,
+							  pAd->MlmeAux.ExtRateLen,	    pAd->MlmeAux.ExtRate,
+							  END_OF_ARGS);
+			FrameLen += tmp;
+		}
+
+		if (pAd->MlmeAux.APEdcaParm.bValid)
+		{
+			if (pAd->CommonCfg.bAPSDCapable && pAd->MlmeAux.APEdcaParm.bAPSDCapable)
+			{
+				QBSS_STA_INFO_PARM QosInfo;
+
+				NdisZeroMemory(&QosInfo, sizeof(QBSS_STA_INFO_PARM));
+				QosInfo.UAPSD_AC_BE = pAd->CommonCfg.bAPSDAC_BE;
+				QosInfo.UAPSD_AC_BK = pAd->CommonCfg.bAPSDAC_BK;
+				QosInfo.UAPSD_AC_VI = pAd->CommonCfg.bAPSDAC_VI;
+				QosInfo.UAPSD_AC_VO = pAd->CommonCfg.bAPSDAC_VO;
+				QosInfo.MaxSPLength = pAd->CommonCfg.MaxSPLength;
+				WmeIe[8] |= *(PUCHAR)&QosInfo;
+			}
+
+			MakeOutgoingFrame(pOutBuffer + FrameLen,    &tmp,
+							  9,                        &WmeIe[0],
+							  END_OF_ARGS);
+			FrameLen += tmp;
+		}
+
+#ifdef DOT11_N_SUPPORT
+		// HT
+		if ((pAd->MlmeAux.HtCapabilityLen > 0) && (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+		{
+			ULONG TmpLen;
+			UCHAR HtLen;
+			UCHAR BROADCOM[4] = {0x0, 0x90, 0x4c, 0x33};
+			if (pAd->StaActive.SupportedPhyInfo.bPreNHt == TRUE)
+			{
+				HtLen = SIZE_HT_CAP_IE + 4;
+				MakeOutgoingFrame(pOutBuffer + FrameLen,            &TmpLen,
+							  1,                                &WpaIe,
+							  1,                                &HtLen,
+							  4,                                &BROADCOM[0],
+							 pAd->MlmeAux.HtCapabilityLen,          &pAd->MlmeAux.HtCapability,
+							  END_OF_ARGS);
+			}
+			else
+			{
+				MakeOutgoingFrame(pOutBuffer + FrameLen,            &TmpLen,
+							  1,                                &HtCapIe,
+							  1,                                &pAd->MlmeAux.HtCapabilityLen,
+							 pAd->MlmeAux.HtCapabilityLen,          &pAd->MlmeAux.HtCapability,
+							  END_OF_ARGS);
+			}
+			FrameLen += TmpLen;
+		}
+#endif // DOT11_N_SUPPORT //
+
+		// add Ralink proprietary IE to inform AP this STA is going to use AGGREGATION or PIGGY-BACK+AGGREGATION
+		// Case I: (Aggregation + Piggy-Back)
+		// 1. user enable aggregation, AND
+		// 2. Mac support piggy-back
+		// 3. AP annouces it's PIGGY-BACK+AGGREGATION-capable in BEACON
+		// Case II: (Aggregation)
+		// 1. user enable aggregation, AND
+		// 2. AP annouces it's AGGREGATION-capable in BEACON
+		if (pAd->CommonCfg.bAggregationCapable)
+		{
+			if ((pAd->CommonCfg.bPiggyBackCapable) && ((pAd->MlmeAux.APRalinkIe & 0x00000003) == 3))
+			{
+				ULONG TmpLen;
+				UCHAR RalinkIe[9] = {IE_VENDOR_SPECIFIC, 7, 0x00, 0x0c, 0x43, 0x03, 0x00, 0x00, 0x00};
+				MakeOutgoingFrame(pOutBuffer+FrameLen,           &TmpLen,
+								  9,                             RalinkIe,
+								  END_OF_ARGS);
+				FrameLen += TmpLen;
+			}
+			else if (pAd->MlmeAux.APRalinkIe & 0x00000001)
+			{
+				ULONG TmpLen;
+				UCHAR RalinkIe[9] = {IE_VENDOR_SPECIFIC, 7, 0x00, 0x0c, 0x43, 0x01, 0x00, 0x00, 0x00};
+				MakeOutgoingFrame(pOutBuffer+FrameLen,           &TmpLen,
+								  9,                             RalinkIe,
+								  END_OF_ARGS);
+				FrameLen += TmpLen;
+			}
+		}
+		else
+		{
+			ULONG TmpLen;
+			UCHAR RalinkIe[9] = {IE_VENDOR_SPECIFIC, 7, 0x00, 0x0c, 0x43, 0x04, 0x00, 0x00, 0x00};
+			MakeOutgoingFrame(pOutBuffer+FrameLen,		 &TmpLen,
+							  9,						 RalinkIe,
+							  END_OF_ARGS);
+			FrameLen += TmpLen;
+		}
+#ifdef LEAP_SUPPORT
+		if (LEAP_CCKM_ON(pAd) && (pAd->StaCfg.CCKMLinkUpFlag == TRUE))
+		{
+			CkipFlag = pAd->StaCfg.CkipFlag;	// We have update that at PeerBeaconAtJoinRequest()
+			if (CkipFlag != 0)
+			{
+				NdisZeroMemory(CkipNegotiationBuffer, CKIP_NEGOTIATION_LENGTH);
+				CkipNegotiationBuffer[2] = 0x66;
+				// Make it try KP & MIC, since we have to follow the result from AssocRsp
+				CkipNegotiationBuffer[8] = 0x18;
+				CkipNegotiationBuffer[CKIP_NEGOTIATION_LENGTH - 1] = 0x22;
+
+				MakeOutgoingFrame(pOutBuffer + FrameLen,            &tmp,
+									1,                              &AironetCkipIe,
+									1,                              &AironetCkipLen,
+									AironetCkipLen,                 CkipNegotiationBuffer,
+									END_OF_ARGS);
+				FrameLen += tmp;
+			}
+
+			MakeOutgoingFrame(pOutBuffer + FrameLen, &tmp,
+							1,                              &AironetIPAddressIE,
+							1,                              &AironetIPAddressLen,
+							AironetIPAddressLen,            AironetIPAddressBuffer,
+							END_OF_ARGS);
+			FrameLen += tmp;
+
+			//
+			// The RN is incremented before each reassociation request.
+			//
+			pAd->StaCfg.CCKMRN++;
+			//
+			// Calculate MIC = hmac-md5(krk, STA-ID|BSSID|RSNIE|TSF|RN);
+			//
+			COPY_MAC_ADDR(CalcMicBuffer, pAd->CurrentAddress);
+			CalcMicBufferLen = MAC_ADDR_LEN;
+			COPY_MAC_ADDR(CalcMicBuffer + CalcMicBufferLen, pAd->MlmeAux.Bssid);
+			CalcMicBufferLen += MAC_ADDR_LEN;
+			NdisMoveMemory(CalcMicBuffer + CalcMicBufferLen, CipherSuiteCiscoCCKM, CipherSuiteCiscoCCKMLen);
+			CalcMicBufferLen += CipherSuiteCiscoCCKMLen;
+			NdisMoveMemory(CalcMicBuffer + CalcMicBufferLen, (PUCHAR) &pAd->StaCfg.CCKMBeaconAtJoinTimeStamp, sizeof(pAd->StaCfg.CCKMBeaconAtJoinTimeStamp));
+			CalcMicBufferLen += sizeof(pAd->StaCfg.CCKMBeaconAtJoinTimeStamp);
+			NdisMoveMemory(CalcMicBuffer + CalcMicBufferLen, (PUCHAR)&pAd->StaCfg.CCKMRN, sizeof(pAd->StaCfg.CCKMRN));
+			CalcMicBufferLen += sizeof(pAd->StaCfg.CCKMRN);
+			hmac_md5(pAd->StaCfg.KRK, LEN_EAP_MICK, CalcMicBuffer, CalcMicBufferLen, MICMN);
+
+			//
+			// fill up CCKM reassociation request element
+			//
+			NdisMoveMemory(AironetCCKMReassocBuffer, AironetOUI, 4);
+			NdisMoveMemory(AironetCCKMReassocBuffer + 4, (PUCHAR)&pAd->StaCfg.CCKMBeaconAtJoinTimeStamp, 8);
+			NdisMoveMemory(AironetCCKMReassocBuffer + 12, (PUCHAR) &pAd->StaCfg.CCKMRN, 4);
+			NdisMoveMemory(AironetCCKMReassocBuffer +16, MICMN, 8);
+
+			MakeOutgoingFrame(pOutBuffer + FrameLen, &tmp,
+							1,                      &AironetCCKMReassocIE,
+							1,                      &AironetCCKMReassocLen,
+							AironetCCKMReassocLen,  AironetCCKMReassocBuffer,
+							END_OF_ARGS);
+			FrameLen += tmp;
+
+			MakeOutgoingFrame(pOutBuffer + FrameLen, &tmp,
+							CipherSuiteCiscoCCKMLen,CipherSuiteCiscoCCKM,
+							END_OF_ARGS);
+			FrameLen += tmp;
+		}
+#endif // LEAP_SUPPORT //
+
+		// Add CCX v2 request if CCX2 admin state is on
+		if (pAd->StaCfg.CCXControl.field.Enable == 1)
+		{
+			//
+			// Add CCX Version
+			//
+			MakeOutgoingFrame(pOutBuffer + FrameLen, &tmp,
+						1,							&Ccx2Ie,
+						1,							&Ccx2Len,
+						Ccx2Len,				    Ccx2IeInfo,
+						END_OF_ARGS);
+			FrameLen += tmp;
+		}
+
+		MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+		MlmeFreeMemory(pAd, pOutBuffer);
+
+		RTMPSetTimer(&pAd->MlmeAux.ReassocTimer, Timeout); /* in mSec */
+		pAd->Mlme.AssocMachine.CurrState = REASSOC_WAIT_RSP;
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("ASSOC - MlmeReassocReqAction() sanity check failed. BUG!!!! \n"));
+		pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+		Status = MLME_INVALID_FORMAT;
+		MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_REASSOC_CONF, 2, &Status);
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+		Upper layer issues disassoc request
+	Parameters:
+		Elem -
+
+	IRQL = PASSIVE_LEVEL
+
+	==========================================================================
+ */
+VOID MlmeDisassocReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	PMLME_DISASSOC_REQ_STRUCT pDisassocReq;
+	HEADER_802_11         DisassocHdr;
+	PHEADER_802_11        pDisassocHdr;
+	PUCHAR                pOutBuffer = NULL;
+	ULONG                 FrameLen = 0;
+	NDIS_STATUS           NStatus;
+	BOOLEAN               TimerCancelled;
+	ULONG                 Timeout = 0;
+	USHORT                Status;
+
+#ifdef QOS_DLS_SUPPORT
+	// send DLS-TEAR_DOWN message,
+	if (pAd->CommonCfg.bDLSCapable)
+	{
+		UCHAR i;
+
+		// tear down local dls table entry
+		for (i=0; i<MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+		{
+			if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+			{
+				RTMPSendDLSTearDownFrame(pAd, pAd->StaCfg.DLSEntry[i].MacAddr);
+				pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+				pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+			}
+		}
+
+		// tear down peer dls table entry
+		for (i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+		{
+			if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+			{
+				RTMPSendDLSTearDownFrame(pAd, pAd->StaCfg.DLSEntry[i].MacAddr);
+				pAd->StaCfg.DLSEntry[i].Status = DLS_NONE;
+				pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+			}
+		}
+	}
+#endif // QOS_DLS_SUPPORT //
+
+	// skip sanity check
+	pDisassocReq = (PMLME_DISASSOC_REQ_STRUCT)(Elem->Msg);
+
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if (NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - MlmeDisassocReqAction() allocate memory failed\n"));
+		pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+		Status = MLME_FAIL_NO_RESOURCE;
+		MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_DISASSOC_CONF, 2, &Status);
+		return;
+	}
+
+
+
+	RTMPCancelTimer(&pAd->MlmeAux.DisassocTimer, &TimerCancelled);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - Send DISASSOC request[BSSID::%02x:%02x:%02x:%02x:%02x:%02x (Reason=%d)\n",
+				pDisassocReq->Addr[0], pDisassocReq->Addr[1], pDisassocReq->Addr[2],
+				pDisassocReq->Addr[3], pDisassocReq->Addr[4], pDisassocReq->Addr[5], pDisassocReq->Reason));
+	MgtMacHeaderInit(pAd, &DisassocHdr, SUBTYPE_DISASSOC, 0, pDisassocReq->Addr, pDisassocReq->Addr);	// patch peap ttls switching issue
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+					  sizeof(HEADER_802_11),&DisassocHdr,
+					  2,                    &pDisassocReq->Reason,
+					  END_OF_ARGS);
+	MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+
+	// To patch Instance and Buffalo(N) AP
+	// Driver has to send deauth to Instance AP, but Buffalo(N) needs to send disassoc to reset Authenticator's state machine
+	// Therefore, we send both of them.
+	pDisassocHdr = (PHEADER_802_11)pOutBuffer;
+	pDisassocHdr->FC.SubType = SUBTYPE_DEAUTH;
+	MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	pAd->StaCfg.DisassocReason = REASON_DISASSOC_STA_LEAVING;
+	COPY_MAC_ADDR(pAd->StaCfg.DisassocSta, pDisassocReq->Addr);
+
+	RTMPSetTimer(&pAd->MlmeAux.DisassocTimer, Timeout); /* in mSec */
+	pAd->Mlme.AssocMachine.CurrState = DISASSOC_WAIT_RSP;
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+    if (pAd->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_DISABLE)
+	{
+        union iwreq_data    wrqu;
+        //send disassociate event to wpa_supplicant
+        memset(&wrqu, 0, sizeof(wrqu));
+        wrqu.data.flags = RT_DISASSOC_EVENT_FLAG;
+        wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+    }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+    {
+        union iwreq_data    wrqu;
+        memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+        wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+    }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+
+}
+
+/*
+	==========================================================================
+	Description:
+		peer sends assoc rsp back
+	Parameters:
+		Elme - MLME message containing the received frame
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID PeerAssocRspAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT        CapabilityInfo, Status, Aid;
+	UCHAR         SupRate[MAX_LEN_OF_SUPPORTED_RATES], SupRateLen;
+	UCHAR         ExtRate[MAX_LEN_OF_SUPPORTED_RATES], ExtRateLen;
+	UCHAR         Addr2[MAC_ADDR_LEN];
+	BOOLEAN       TimerCancelled;
+	UCHAR         CkipFlag;
+	EDCA_PARM     EdcaParm;
+	HT_CAPABILITY_IE		HtCapability;
+	ADD_HT_INFO_IE		AddHtInfo;	// AP might use this additional ht info IE
+	UCHAR			HtCapabilityLen;
+	UCHAR			AddHtInfoLen;
+	UCHAR			NewExtChannelOffset = 0xff;
+
+	if (PeerAssocRspSanity(pAd, Elem->Msg, Elem->MsgLen, Addr2, &CapabilityInfo, &Status, &Aid, SupRate, &SupRateLen, ExtRate, &ExtRateLen,
+		&HtCapability,&AddHtInfo, &HtCapabilityLen,&AddHtInfoLen,&NewExtChannelOffset, &EdcaParm, &CkipFlag))
+	{
+		// The frame is for me ?
+		if(MAC_ADDR_EQUAL(Addr2, pAd->MlmeAux.Bssid))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("PeerAssocRspAction():ASSOC - receive ASSOC_RSP to me (status=%d)\n", Status));
+#ifdef DOT11_N_SUPPORT
+			DBGPRINT(RT_DEBUG_TRACE, ("PeerAssocRspAction():MacTable [%d].AMsduSize = %d. ClientStatusFlags = 0x%lx \n",Elem->Wcid, pAd->MacTab.Content[BSSID_WCID].AMsduSize, pAd->MacTab.Content[BSSID_WCID].ClientStatusFlags));
+#endif // DOT11_N_SUPPORT //
+			RTMPCancelTimer(&pAd->MlmeAux.AssocTimer, &TimerCancelled);
+			if(Status == MLME_SUCCESS)
+			{
+				UCHAR			MaxSupportedRateIn500Kbps = 0;
+				UCHAR			idx;
+
+				// supported rates array may not be sorted. sort it and find the maximum rate
+			    for (idx=0; idx<SupRateLen; idx++)
+			    {
+			        if (MaxSupportedRateIn500Kbps < (SupRate[idx] & 0x7f))
+			            MaxSupportedRateIn500Kbps = SupRate[idx] & 0x7f;
+			    }
+
+				for (idx=0; idx<ExtRateLen; idx++)
+			    {
+			        if (MaxSupportedRateIn500Kbps < (ExtRate[idx] & 0x7f))
+			            MaxSupportedRateIn500Kbps = ExtRate[idx] & 0x7f;
+			    }
+				// go to procedure listed on page 376
+				AssocPostProc(pAd, Addr2, CapabilityInfo, Aid, SupRate, SupRateLen, ExtRate, ExtRateLen,
+					&EdcaParm, &HtCapability, HtCapabilityLen, &AddHtInfo);
+
+				StaAddMacTableEntry(pAd, &pAd->MacTab.Content[BSSID_WCID], MaxSupportedRateIn500Kbps, &HtCapability, HtCapabilityLen, CapabilityInfo);
+
+				pAd->StaCfg.CkipFlag = CkipFlag;
+				if (CkipFlag & 0x18)
+				{
+					NdisZeroMemory(pAd->StaCfg.TxSEQ, 4);
+					NdisZeroMemory(pAd->StaCfg.RxSEQ, 4);
+					NdisZeroMemory(pAd->StaCfg.CKIPMIC, 4);
+					pAd->StaCfg.GIV[0] = RandomByte(pAd);
+					pAd->StaCfg.GIV[1] = RandomByte(pAd);
+					pAd->StaCfg.GIV[2] = RandomByte(pAd);
+					pAd->StaCfg.bCkipOn = TRUE;
+					DBGPRINT(RT_DEBUG_TRACE, ("<CCX> pAd->StaCfg.CkipFlag = 0x%02x\n", pAd->StaCfg.CkipFlag));
+				}
+			}
+			else
+			{
+				// Faile on Association, we need to check the status code
+				// Is that a Rogue AP?
+#ifdef LEAP_SUPPORT
+				if ((pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP) && (Status == MLME_ALG_NOT_SUPPORT))
+				{ //Possibly Rogue AP
+					RogueApTableSetEntry(pAd, &pAd->StaCfg.RogueApTab, pAd->MlmeAux.Bssid, LEAP_REASON_INVALID_AUTH);
+				}
+#endif // LEAP_SUPPORT //
+			}
+			pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+			MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_ASSOC_CONF, 2, &Status);
+		}
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - PeerAssocRspAction() sanity check fail\n"));
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+		peer sends reassoc rsp
+	Parametrs:
+		Elem - MLME message cntaining the received frame
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID PeerReassocRspAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT      CapabilityInfo;
+	USHORT      Status;
+	USHORT      Aid;
+	UCHAR       SupRate[MAX_LEN_OF_SUPPORTED_RATES], SupRateLen;
+	UCHAR       ExtRate[MAX_LEN_OF_SUPPORTED_RATES], ExtRateLen;
+	UCHAR       Addr2[MAC_ADDR_LEN];
+	UCHAR       CkipFlag;
+	BOOLEAN     TimerCancelled;
+	EDCA_PARM   EdcaParm;
+	HT_CAPABILITY_IE		HtCapability;
+	ADD_HT_INFO_IE		AddHtInfo;	// AP might use this additional ht info IE
+	UCHAR			HtCapabilityLen;
+	UCHAR			AddHtInfoLen;
+	UCHAR			NewExtChannelOffset = 0xff;
+
+	if(PeerAssocRspSanity(pAd, Elem->Msg, Elem->MsgLen, Addr2, &CapabilityInfo, &Status, &Aid, SupRate, &SupRateLen, ExtRate, &ExtRateLen,
+								&HtCapability,	&AddHtInfo, &HtCapabilityLen, &AddHtInfoLen,&NewExtChannelOffset, &EdcaParm, &CkipFlag))
+	{
+		if(MAC_ADDR_EQUAL(Addr2, pAd->MlmeAux.Bssid)) // The frame is for me ?
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - receive REASSOC_RSP to me (status=%d)\n", Status));
+			RTMPCancelTimer(&pAd->MlmeAux.ReassocTimer, &TimerCancelled);
+
+			if(Status == MLME_SUCCESS)
+			{
+				// go to procedure listed on page 376
+				AssocPostProc(pAd, Addr2, CapabilityInfo, Aid, SupRate, SupRateLen, ExtRate, ExtRateLen,
+					 &EdcaParm, &HtCapability, HtCapabilityLen, &AddHtInfo);
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+                if (pAd->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_DISABLE)
+                {
+                    union iwreq_data    wrqu;
+
+                    SendAssocIEsToWpaSupplicant(pAd);
+                    memset(&wrqu, 0, sizeof(wrqu));
+                    wrqu.data.flags = RT_ASSOC_EVENT_FLAG;
+                    wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+                }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+                {
+                    union iwreq_data    wrqu;
+                    wext_notify_event_assoc(pAd);
+
+                    memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+                    memcpy(wrqu.ap_addr.sa_data, pAd->MlmeAux.Bssid, MAC_ADDR_LEN);
+                    wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+
+                }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+
+			}
+
+			//
+			// Cisco Leap CCKM supported Re-association.
+			//
+#ifdef LEAP_SUPPORT
+			if (LEAP_CCKM_ON(pAd) && (pAd->StaCfg.CCKMLinkUpFlag == TRUE))
+			{
+				if (CCKMAssocRspSanity(pAd, Elem->Msg, Elem->MsgLen) == TRUE)
+				{
+					pAd->StaCfg.CkipFlag = CkipFlag;
+					if (CkipFlag & 0x18)
+					{
+						NdisZeroMemory(pAd->StaCfg.TxSEQ, 4);
+						NdisZeroMemory(pAd->StaCfg.RxSEQ, 4);
+						NdisZeroMemory(pAd->StaCfg.CKIPMIC, 4);
+						pAd->StaCfg.GIV[0] = RandomByte(pAd);
+						pAd->StaCfg.GIV[1] = RandomByte(pAd);
+						pAd->StaCfg.GIV[2] = RandomByte(pAd);
+						pAd->StaCfg.bCkipOn = TRUE;
+						DBGPRINT(RT_DEBUG_TRACE, ("<CCX> pAd->StaCfg.CkipFlag = 0x%02x\n", pAd->StaCfg.CkipFlag));
+					}
+
+					pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+					MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_REASSOC_CONF, 2, &Status);
+				}
+				else
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - CCKMAssocRspSanity() sanity check fail\n"));
+				}
+			}
+			else
+#endif // LEAP_SUPPORT //
+			{
+				// CkipFlag is no use for reassociate
+				pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+				MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_REASSOC_CONF, 2, &Status);
+			}
+		}
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - PeerReassocRspAction() sanity check fail\n"));
+	}
+
+}
+
+/*
+	==========================================================================
+	Description:
+		procedures on IEEE 802.11/1999 p.376
+	Parametrs:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AssocPostProc(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR pAddr2,
+	IN USHORT CapabilityInfo,
+	IN USHORT Aid,
+	IN UCHAR SupRate[],
+	IN UCHAR SupRateLen,
+	IN UCHAR ExtRate[],
+	IN UCHAR ExtRateLen,
+	IN PEDCA_PARM pEdcaParm,
+	IN HT_CAPABILITY_IE		*pHtCapability,
+	IN UCHAR HtCapabilityLen,
+	IN ADD_HT_INFO_IE		*pAddHtInfo)	// AP might use this additional ht info IE
+{
+	ULONG Idx;
+
+	pAd->MlmeAux.BssType = BSS_INFRA;
+	COPY_MAC_ADDR(pAd->MlmeAux.Bssid, pAddr2);
+	pAd->MlmeAux.Aid = Aid;
+	pAd->MlmeAux.CapabilityInfo = CapabilityInfo & SUPPORTED_CAPABILITY_INFO;
+#ifdef DOT11_N_SUPPORT
+	// Some HT AP might lost WMM IE. We add WMM ourselves. beacuase HT requires QoS on.
+	if ((HtCapabilityLen > 0) && (pEdcaParm->bValid == FALSE))
+	{
+		pEdcaParm->bValid = TRUE;
+		pEdcaParm->Aifsn[0] = 3;
+		pEdcaParm->Aifsn[1] = 7;
+		pEdcaParm->Aifsn[2] = 2;
+		pEdcaParm->Aifsn[3] = 2;
+
+		pEdcaParm->Cwmin[0] = 4;
+		pEdcaParm->Cwmin[1] = 4;
+		pEdcaParm->Cwmin[2] = 3;
+		pEdcaParm->Cwmin[3] = 2;
+
+		pEdcaParm->Cwmax[0] = 10;
+		pEdcaParm->Cwmax[1] = 10;
+		pEdcaParm->Cwmax[2] = 4;
+		pEdcaParm->Cwmax[3] = 3;
+
+		pEdcaParm->Txop[0]  = 0;
+		pEdcaParm->Txop[1]  = 0;
+		pEdcaParm->Txop[2]  = 96;
+		pEdcaParm->Txop[3]  = 48;
+
+	}
+#endif // DOT11_N_SUPPORT //
+
+	NdisMoveMemory(&pAd->MlmeAux.APEdcaParm, pEdcaParm, sizeof(EDCA_PARM));
+
+	// filter out un-supported rates
+	pAd->MlmeAux.SupRateLen = SupRateLen;
+	NdisMoveMemory(pAd->MlmeAux.SupRate, SupRate, SupRateLen);
+	RTMPCheckRates(pAd, pAd->MlmeAux.SupRate, &pAd->MlmeAux.SupRateLen);
+
+	// filter out un-supported rates
+	pAd->MlmeAux.ExtRateLen = ExtRateLen;
+	NdisMoveMemory(pAd->MlmeAux.ExtRate, ExtRate, ExtRateLen);
+	RTMPCheckRates(pAd, pAd->MlmeAux.ExtRate, &pAd->MlmeAux.ExtRateLen);
+
+#ifdef DOT11_N_SUPPORT
+	if (HtCapabilityLen > 0)
+	{
+		RTMPCheckHt(pAd, BSSID_WCID, pHtCapability, pAddHtInfo);
+	}
+	DBGPRINT(RT_DEBUG_TRACE, ("AssocPostProc===>  AP.AMsduSize = %d. ClientStatusFlags = 0x%lx \n", pAd->MacTab.Content[BSSID_WCID].AMsduSize, pAd->MacTab.Content[BSSID_WCID].ClientStatusFlags));
+
+	DBGPRINT(RT_DEBUG_TRACE, ("AssocPostProc===>    (Mmps=%d, AmsduSize=%d, )\n",
+		pAd->MacTab.Content[BSSID_WCID].MmpsMode, pAd->MacTab.Content[BSSID_WCID].AMsduSize));
+#endif // DOT11_N_SUPPORT //
+
+	// Set New WPA information
+	Idx = BssTableSearch(&pAd->ScanTab, pAddr2, pAd->MlmeAux.Channel);
+	if (Idx == BSS_NOT_FOUND)
+	{
+		DBGPRINT_ERR(("ASSOC - Can't find BSS after receiving Assoc response\n"));
+	}
+	else
+	{
+		// Init variable
+		pAd->MacTab.Content[BSSID_WCID].RSNIE_Len = 0;
+		NdisZeroMemory(pAd->MacTab.Content[BSSID_WCID].RSN_IE, MAX_LEN_OF_RSNIE);
+
+		// Store appropriate RSN_IE for WPA SM negotiation later
+		if ((pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA) && (pAd->ScanTab.BssEntry[Idx].VarIELen != 0))
+		{
+			PUCHAR              pVIE;
+			USHORT              len;
+			PEID_STRUCT         pEid;
+
+			pVIE = pAd->ScanTab.BssEntry[Idx].VarIEs;
+			len	 = pAd->ScanTab.BssEntry[Idx].VarIELen;
+
+			while (len > 0)
+			{
+				pEid = (PEID_STRUCT) pVIE;
+				// For WPA/WPAPSK
+				if ((pEid->Eid == IE_WPA) && (NdisEqualMemory(pEid->Octet, WPA_OUI, 4))
+					&& (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA || pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK))
+				{
+					NdisMoveMemory(pAd->MacTab.Content[BSSID_WCID].RSN_IE, pVIE, (pEid->Len + 2));
+					pAd->MacTab.Content[BSSID_WCID].RSNIE_Len = (pEid->Len + 2);
+					DBGPRINT(RT_DEBUG_TRACE, ("AssocPostProc===> Store RSN_IE for WPA SM negotiation \n"));
+				}
+				// For WPA2/WPA2PSK
+				else if ((pEid->Eid == IE_RSN) && (NdisEqualMemory(pEid->Octet + 2, RSN_OUI, 3))
+					&& (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2 || pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK))
+				{
+					NdisMoveMemory(pAd->MacTab.Content[BSSID_WCID].RSN_IE, pVIE, (pEid->Len + 2));
+					pAd->MacTab.Content[BSSID_WCID].RSNIE_Len = (pEid->Len + 2);
+					DBGPRINT(RT_DEBUG_TRACE, ("AssocPostProc===> Store RSN_IE for WPA2 SM negotiation \n"));
+				}
+
+				pVIE += (pEid->Len + 2);
+				len  -= (pEid->Len + 2);
+			}
+		}
+
+		if (pAd->MacTab.Content[BSSID_WCID].RSNIE_Len == 0)
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("AssocPostProc===> no RSN_IE \n"));
+		}
+		else
+		{
+			hex_dump("RSN_IE", pAd->MacTab.Content[BSSID_WCID].RSN_IE, pAd->MacTab.Content[BSSID_WCID].RSNIE_Len);
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+		left part of IEEE 802.11/1999 p.374
+	Parameters:
+		Elem - MLME message containing the received frame
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID PeerDisassocAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR         Addr2[MAC_ADDR_LEN];
+	USHORT        Reason;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - PeerDisassocAction()\n"));
+	if(PeerDisassocSanity(pAd, Elem->Msg, Elem->MsgLen, Addr2, &Reason))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - PeerDisassocAction() Reason = %d\n", Reason));
+		if (INFRA_ON(pAd) && MAC_ADDR_EQUAL(pAd->CommonCfg.Bssid, Addr2))
+		{
+
+			if (pAd->CommonCfg.bWirelessEvent)
+			{
+				RTMPSendWirelessEvent(pAd, IW_DISASSOC_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+			}
+
+
+#ifdef LEAP_SUPPORT
+			if (pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP)
+			{
+				// Cisco_LEAP has start a timer
+				// We should cancel it if using LEAP
+				RTMPCancelTimer(&pAd->StaCfg.LeapAuthTimer, &TimerCancelled);
+				//Check is it mach the LEAP Authentication failed as possible a Rogue AP
+				//on it's PortSecured not equal to WPA_802_1X_PORT_SECURED while process the Association.
+				if ((pAd->Mlme.LeapMachine.CurrState != LEAP_IDLE) && (pAd->StaCfg.PortSecured != WPA_802_1X_PORT_SECURED))
+				{
+					RogueApTableSetEntry(pAd, &pAd->StaCfg.RogueApTab, Addr2, LEAP_REASON_AUTH_TIMEOUT);
+				}
+			}
+#endif	// LEAP_SUPPORT //
+			//
+			// Get Current System time and Turn on AdjacentAPReport
+			//
+			NdisGetSystemUpTime(&pAd->StaCfg.CCXAdjacentAPLinkDownTime);
+			pAd->StaCfg.CCXAdjacentAPReportFlag = TRUE;
+			LinkDown(pAd, TRUE);
+			pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+            if (pAd->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_DISABLE)
+			{
+                union iwreq_data    wrqu;
+                //send disassociate event to wpa_supplicant
+                memset(&wrqu, 0, sizeof(wrqu));
+                wrqu.data.flags = RT_DISASSOC_EVENT_FLAG;
+                wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+            }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+            {
+                union iwreq_data    wrqu;
+                memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+                wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+            }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+		}
+	}
+	else
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - PeerDisassocAction() sanity check fail\n"));
+	}
+
+}
+
+/*
+	==========================================================================
+	Description:
+		what the state machine will do after assoc timeout
+	Parameters:
+		Elme -
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID AssocTimeoutAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT  Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - AssocTimeoutAction\n"));
+	pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+	Status = MLME_REJ_TIMEOUT;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_ASSOC_CONF, 2, &Status);
+}
+
+/*
+	==========================================================================
+	Description:
+		what the state machine will do after reassoc timeout
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID ReassocTimeoutAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT  Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - ReassocTimeoutAction\n"));
+	pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+	Status = MLME_REJ_TIMEOUT;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_REASSOC_CONF, 2, &Status);
+}
+
+/*
+	==========================================================================
+	Description:
+		what the state machine will do after disassoc timeout
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID DisassocTimeoutAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT  Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - DisassocTimeoutAction\n"));
+	pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+	Status = MLME_SUCCESS;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_DISASSOC_CONF, 2, &Status);
+}
+
+VOID InvalidStateWhenAssoc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT  Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - InvalidStateWhenAssoc(state=%ld), reset ASSOC state machine\n",
+		pAd->Mlme.AssocMachine.CurrState));
+	pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+	Status = MLME_STATE_MACHINE_REJECT;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_ASSOC_CONF, 2, &Status);
+}
+
+VOID InvalidStateWhenReassoc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - InvalidStateWhenReassoc(state=%ld), reset ASSOC state machine\n",
+		pAd->Mlme.AssocMachine.CurrState));
+	pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+	Status = MLME_STATE_MACHINE_REJECT;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_REASSOC_CONF, 2, &Status);
+}
+
+VOID InvalidStateWhenDisassociate(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - InvalidStateWhenDisassoc(state=%ld), reset ASSOC state machine\n",
+		pAd->Mlme.AssocMachine.CurrState));
+	pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+	Status = MLME_STATE_MACHINE_REJECT;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_DISASSOC_CONF, 2, &Status);
+}
+
+/*
+	==========================================================================
+	Description:
+		right part of IEEE 802.11/1999 page 374
+	Note:
+		This event should never cause ASSOC state machine perform state
+		transition, and has no relationship with CNTL machine. So we separate
+		this routine as a service outside of ASSOC state transition table.
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID Cls3errAction(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR        pAddr)
+{
+	HEADER_802_11         DisassocHdr;
+	PHEADER_802_11        pDisassocHdr;
+	PUCHAR                pOutBuffer = NULL;
+	ULONG                 FrameLen = 0;
+	NDIS_STATUS           NStatus;
+	USHORT                Reason = REASON_CLS3ERR;
+
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if (NStatus != NDIS_STATUS_SUCCESS)
+		return;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("ASSOC - Class 3 Error, Send DISASSOC frame\n"));
+	MgtMacHeaderInit(pAd, &DisassocHdr, SUBTYPE_DISASSOC, 0, pAddr, pAd->CommonCfg.Bssid);	// patch peap ttls switching issue
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+					  sizeof(HEADER_802_11),&DisassocHdr,
+					  2,                    &Reason,
+					  END_OF_ARGS);
+	MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+
+	// To patch Instance and Buffalo(N) AP
+	// Driver has to send deauth to Instance AP, but Buffalo(N) needs to send disassoc to reset Authenticator's state machine
+	// Therefore, we send both of them.
+	pDisassocHdr = (PHEADER_802_11)pOutBuffer;
+	pDisassocHdr->FC.SubType = SUBTYPE_DEAUTH;
+	MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	pAd->StaCfg.DisassocReason = REASON_CLS3ERR;
+	COPY_MAC_ADDR(pAd->StaCfg.DisassocSta, pAddr);
+}
+
+ /*
+	 ==========================================================================
+	 Description:
+		 Switch between WEP and CKIP upon new association up.
+	 Parameters:
+
+	 IRQL = DISPATCH_LEVEL
+
+	 ==========================================================================
+  */
+VOID SwitchBetweenWepAndCkip(
+	IN PRTMP_ADAPTER pAd)
+{
+	int            i;
+	SHAREDKEY_MODE_STRUC  csr1;
+
+	// if KP is required. change the CipherAlg in hardware shard key table from WEP
+	// to CKIP. else remain as WEP
+	if (pAd->StaCfg.bCkipOn && (pAd->StaCfg.CkipFlag & 0x10))
+	{
+		// modify hardware key table so that MAC use correct algorithm to decrypt RX
+		RTMP_IO_READ32(pAd, SHARED_KEY_MODE_BASE, &csr1.word);
+		if (csr1.field.Bss0Key0CipherAlg == CIPHER_WEP64)
+			csr1.field.Bss0Key0CipherAlg = CIPHER_CKIP64;
+		else if (csr1.field.Bss0Key0CipherAlg == CIPHER_WEP128)
+			csr1.field.Bss0Key0CipherAlg = CIPHER_CKIP128;
+
+		if (csr1.field.Bss0Key1CipherAlg == CIPHER_WEP64)
+			csr1.field.Bss0Key1CipherAlg = CIPHER_CKIP64;
+		else if (csr1.field.Bss0Key1CipherAlg == CIPHER_WEP128)
+			csr1.field.Bss0Key1CipherAlg = CIPHER_CKIP128;
+
+		if (csr1.field.Bss0Key2CipherAlg == CIPHER_WEP64)
+			csr1.field.Bss0Key2CipherAlg = CIPHER_CKIP64;
+		else if (csr1.field.Bss0Key2CipherAlg == CIPHER_WEP128)
+			csr1.field.Bss0Key2CipherAlg = CIPHER_CKIP128;
+
+		if (csr1.field.Bss0Key3CipherAlg == CIPHER_WEP64)
+			csr1.field.Bss0Key3CipherAlg = CIPHER_CKIP64;
+		else if (csr1.field.Bss0Key3CipherAlg == CIPHER_WEP128)
+			csr1.field.Bss0Key3CipherAlg = CIPHER_CKIP128;
+		RTMP_IO_WRITE32(pAd, SHARED_KEY_MODE_BASE, csr1.word);
+		DBGPRINT(RT_DEBUG_TRACE, ("SwitchBetweenWepAndCkip: modify BSS0 cipher to %s\n", CipherName[csr1.field.Bss0Key0CipherAlg]));
+
+		// modify software key table so that driver can specify correct algorithm in TXD upon TX
+		for (i=0; i<SHARE_KEY_NUM; i++)
+		{
+			if (pAd->SharedKey[BSS0][i].CipherAlg == CIPHER_WEP64)
+				pAd->SharedKey[BSS0][i].CipherAlg = CIPHER_CKIP64;
+			else if (pAd->SharedKey[BSS0][i].CipherAlg == CIPHER_WEP128)
+				pAd->SharedKey[BSS0][i].CipherAlg = CIPHER_CKIP128;
+		}
+	}
+
+	// else if KP NOT inused. change the CipherAlg in hardware shard key table from CKIP
+	// to WEP.
+	else
+	{
+		// modify hardware key table so that MAC use correct algorithm to decrypt RX
+		RTMP_IO_READ32(pAd, SHARED_KEY_MODE_BASE, &csr1.word);
+		if (csr1.field.Bss0Key0CipherAlg == CIPHER_CKIP64)
+			csr1.field.Bss0Key0CipherAlg = CIPHER_WEP64;
+		else if (csr1.field.Bss0Key0CipherAlg == CIPHER_CKIP128)
+			csr1.field.Bss0Key0CipherAlg = CIPHER_WEP128;
+
+		if (csr1.field.Bss0Key1CipherAlg == CIPHER_CKIP64)
+			csr1.field.Bss0Key1CipherAlg = CIPHER_WEP64;
+		else if (csr1.field.Bss0Key1CipherAlg == CIPHER_CKIP128)
+			csr1.field.Bss0Key1CipherAlg = CIPHER_WEP128;
+
+		if (csr1.field.Bss0Key2CipherAlg == CIPHER_CKIP64)
+			csr1.field.Bss0Key2CipherAlg = CIPHER_WEP64;
+		else if (csr1.field.Bss0Key2CipherAlg == CIPHER_CKIP128)
+			csr1.field.Bss0Key2CipherAlg = CIPHER_WEP128;
+
+		if (csr1.field.Bss0Key3CipherAlg == CIPHER_CKIP64)
+			csr1.field.Bss0Key3CipherAlg = CIPHER_WEP64;
+		else if (csr1.field.Bss0Key3CipherAlg == CIPHER_CKIP128)
+			csr1.field.Bss0Key3CipherAlg = CIPHER_WEP128;
+
+		// modify software key table so that driver can specify correct algorithm in TXD upon TX
+		for (i=0; i<SHARE_KEY_NUM; i++)
+		{
+			if (pAd->SharedKey[BSS0][i].CipherAlg == CIPHER_CKIP64)
+				pAd->SharedKey[BSS0][i].CipherAlg = CIPHER_WEP64;
+			else if (pAd->SharedKey[BSS0][i].CipherAlg == CIPHER_CKIP128)
+				pAd->SharedKey[BSS0][i].CipherAlg = CIPHER_WEP128;
+		}
+
+		//
+		// On WPA-NONE, must update CipherAlg.
+		// Because the OID_802_11_WEP_STATUS was been set after OID_802_11_ADD_KEY
+		// and CipherAlg will be CIPHER_NONE by Windows ZeroConfig.
+		// So we need to update CipherAlg after connect.
+		//
+		if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+		{
+			for (i = 0; i < SHARE_KEY_NUM; i++)
+			{
+				if (pAd->SharedKey[BSS0][i].KeyLen != 0)
+				{
+					if (pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled)
+					{
+						pAd->SharedKey[BSS0][i].CipherAlg = CIPHER_TKIP;
+					}
+					else if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+					{
+						pAd->SharedKey[BSS0][i].CipherAlg = CIPHER_AES;
+					}
+				}
+				else
+				{
+					pAd->SharedKey[BSS0][i].CipherAlg = CIPHER_NONE;
+				}
+			}
+
+			csr1.field.Bss0Key0CipherAlg = pAd->SharedKey[BSS0][0].CipherAlg;
+			csr1.field.Bss0Key1CipherAlg = pAd->SharedKey[BSS0][1].CipherAlg;
+			csr1.field.Bss0Key2CipherAlg = pAd->SharedKey[BSS0][2].CipherAlg;
+			csr1.field.Bss0Key3CipherAlg = pAd->SharedKey[BSS0][3].CipherAlg;
+		}
+		RTMP_IO_WRITE32(pAd, SHARED_KEY_MODE_BASE, csr1.word);
+		DBGPRINT(RT_DEBUG_TRACE, ("SwitchBetweenWepAndCkip: modify BSS0 cipher to %s\n", CipherName[csr1.field.Bss0Key0CipherAlg]));
+	}
+}
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+VOID    SendAssocIEsToWpaSupplicant(
+    IN  PRTMP_ADAPTER pAd)
+{
+    union iwreq_data    wrqu;
+    unsigned char custom[IW_CUSTOM_MAX] = {0};
+
+    if ((pAd->StaCfg.ReqVarIELen + 17) <= IW_CUSTOM_MAX)
+    {
+        sprintf(custom, "ASSOCINFO_ReqIEs=");
+	    NdisMoveMemory(custom+17, pAd->StaCfg.ReqVarIEs, pAd->StaCfg.ReqVarIELen);
+	    memset(&wrqu, 0, sizeof(wrqu));
+        wrqu.data.length = pAd->StaCfg.ReqVarIELen + 17;
+        wrqu.data.flags = RT_REQIE_EVENT_FLAG;
+        wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, custom);
+
+        memset(&wrqu, 0, sizeof(wrqu));
+        wrqu.data.flags = RT_ASSOCINFO_EVENT_FLAG;
+        wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+    }
+    else
+        DBGPRINT(RT_DEBUG_TRACE, ("pAd->StaCfg.ReqVarIELen + 17 > MAX_CUSTOM_LEN\n"));
+
+    return;
+}
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+int wext_notify_event_assoc(
+	IN  RTMP_ADAPTER *pAd)
+{
+    union iwreq_data    wrqu;
+    char custom[IW_CUSTOM_MAX] = {0};
+
+#if WIRELESS_EXT > 17
+    if (pAd->StaCfg.ReqVarIELen <= IW_CUSTOM_MAX)
+    {
+        wrqu.data.length = pAd->StaCfg.ReqVarIELen;
+        memcpy(custom, pAd->StaCfg.ReqVarIEs, pAd->StaCfg.ReqVarIELen);
+        wireless_send_event(pAd->net_dev, IWEVASSOCREQIE, &wrqu, custom);
+    }
+    else
+        DBGPRINT(RT_DEBUG_TRACE, ("pAd->StaCfg.ReqVarIELen > MAX_CUSTOM_LEN\n"));
+#else
+    if (((pAd->StaCfg.ReqVarIELen*2) + 17) <= IW_CUSTOM_MAX)
+    {
+        UCHAR   idx;
+        wrqu.data.length = (pAd->StaCfg.ReqVarIELen*2) + 17;
+        sprintf(custom, "ASSOCINFO(ReqIEs=");
+        for (idx=0; idx<pAd->StaCfg.ReqVarIELen; idx++)
+                sprintf(custom, "%s%02x", custom, pAd->StaCfg.ReqVarIEs[idx]);
+        wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, custom);
+    }
+    else
+        DBGPRINT(RT_DEBUG_TRACE, ("(pAd->StaCfg.ReqVarIELen*2) + 17 > MAX_CUSTOM_LEN\n"));
+#endif
+
+	return 0;
+
+}
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+
+
+BOOLEAN StaAddMacTableEntry(
+	IN  PRTMP_ADAPTER		pAd,
+	IN  PMAC_TABLE_ENTRY	pEntry,
+	IN  UCHAR				MaxSupportedRateIn500Kbps,
+	IN  HT_CAPABILITY_IE	*pHtCapability,
+	IN  UCHAR				HtCapabilityLen,
+	IN  USHORT        		CapabilityInfo)
+{
+	UCHAR            MaxSupportedRate = RATE_11;
+
+	if (ADHOC_ON(pAd))
+		CLIENT_STATUS_CLEAR_FLAG(pEntry, fCLIENT_STATUS_WMM_CAPABLE);
+
+	switch (MaxSupportedRateIn500Kbps)
+    {
+        case 108: MaxSupportedRate = RATE_54;   break;
+        case 96:  MaxSupportedRate = RATE_48;   break;
+        case 72:  MaxSupportedRate = RATE_36;   break;
+        case 48:  MaxSupportedRate = RATE_24;   break;
+        case 36:  MaxSupportedRate = RATE_18;   break;
+        case 24:  MaxSupportedRate = RATE_12;   break;
+        case 18:  MaxSupportedRate = RATE_9;    break;
+        case 12:  MaxSupportedRate = RATE_6;    break;
+        case 22:  MaxSupportedRate = RATE_11;   break;
+        case 11:  MaxSupportedRate = RATE_5_5;  break;
+        case 4:   MaxSupportedRate = RATE_2;    break;
+        case 2:   MaxSupportedRate = RATE_1;    break;
+        default:  MaxSupportedRate = RATE_11;   break;
+    }
+
+    if ((pAd->CommonCfg.PhyMode == PHY_11G) && (MaxSupportedRate < RATE_FIRST_OFDM_RATE))
+        return FALSE;
+
+#ifdef DOT11_N_SUPPORT
+	// 11n only
+	if (((pAd->CommonCfg.PhyMode == PHY_11N_2_4G) || (pAd->CommonCfg.PhyMode == PHY_11N_5G))&& (HtCapabilityLen == 0))
+		return FALSE;
+#endif // DOT11_N_SUPPORT //
+
+	if (!pEntry)
+        return FALSE;
+
+	NdisAcquireSpinLock(&pAd->MacTabLock);
+	if (pEntry)
+	{
+		pEntry->PortSecured = WPA_802_1X_PORT_SECURED;
+		if ((MaxSupportedRate < RATE_FIRST_OFDM_RATE) ||
+			(pAd->CommonCfg.PhyMode == PHY_11B))
+		{
+			pEntry->RateLen = 4;
+			if (MaxSupportedRate >= RATE_FIRST_OFDM_RATE)
+				MaxSupportedRate = RATE_11;
+		}
+		else
+			pEntry->RateLen = 12;
+
+		pEntry->MaxHTPhyMode.word = 0;
+		pEntry->MinHTPhyMode.word = 0;
+		pEntry->HTPhyMode.word = 0;
+		pEntry->MaxSupportedRate = MaxSupportedRate;
+		if (pEntry->MaxSupportedRate < RATE_FIRST_OFDM_RATE)
+		{
+			pEntry->MaxHTPhyMode.field.MODE = MODE_CCK;
+			pEntry->MaxHTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+			pEntry->MinHTPhyMode.field.MODE = MODE_CCK;
+			pEntry->MinHTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+			pEntry->HTPhyMode.field.MODE = MODE_CCK;
+			pEntry->HTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+		}
+		else
+		{
+			pEntry->MaxHTPhyMode.field.MODE = MODE_OFDM;
+			pEntry->MaxHTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+			pEntry->MinHTPhyMode.field.MODE = MODE_OFDM;
+			pEntry->MinHTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+			pEntry->HTPhyMode.field.MODE = MODE_OFDM;
+			pEntry->HTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+		}
+		pEntry->CapabilityInfo = CapabilityInfo;
+		CLIENT_STATUS_CLEAR_FLAG(pEntry, fCLIENT_STATUS_AGGREGATION_CAPABLE);
+		CLIENT_STATUS_CLEAR_FLAG(pEntry, fCLIENT_STATUS_PIGGYBACK_CAPABLE);
+	}
+
+#ifdef DOT11_N_SUPPORT
+	// If this Entry supports 802.11n, upgrade to HT rate.
+	if ((HtCapabilityLen != 0) && (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+	{
+		UCHAR	j, bitmask; //k,bitmask;
+		CHAR    i;
+
+		if (ADHOC_ON(pAd))
+			CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_WMM_CAPABLE);
+		if ((pHtCapability->HtCapInfo.GF) && (pAd->CommonCfg.DesiredHtPhy.GF))
+		{
+			pEntry->MaxHTPhyMode.field.MODE = MODE_HTGREENFIELD;
+		}
+		else
+		{
+			pEntry->MaxHTPhyMode.field.MODE = MODE_HTMIX;
+			pAd->MacTab.fAnyStationNonGF = TRUE;
+			pAd->CommonCfg.AddHTInfo.AddHtInfo2.NonGfPresent = 1;
+		}
+
+		if ((pHtCapability->HtCapInfo.ChannelWidth) && (pAd->CommonCfg.DesiredHtPhy.ChannelWidth))
+		{
+			pEntry->MaxHTPhyMode.field.BW= BW_40;
+			pEntry->MaxHTPhyMode.field.ShortGI = ((pAd->CommonCfg.DesiredHtPhy.ShortGIfor40)&(pHtCapability->HtCapInfo.ShortGIfor40));
+		}
+		else
+		{
+			pEntry->MaxHTPhyMode.field.BW = BW_20;
+			pEntry->MaxHTPhyMode.field.ShortGI = ((pAd->CommonCfg.DesiredHtPhy.ShortGIfor20)&(pHtCapability->HtCapInfo.ShortGIfor20));
+			pAd->MacTab.fAnyStation20Only = TRUE;
+		}
+
+		// 3*3
+		if (pAd->MACVersion >= RALINK_2883_VERSION && pAd->MACVersion < RALINK_3070_VERSION)
+			pEntry->MaxHTPhyMode.field.TxBF = pAd->CommonCfg.RegTransmitSetting.field.TxBF;
+
+		// find max fixed rate
+		for (i=23; i>=0; i--) // 3*3
+		{
+			j = i/8;
+			bitmask = (1<<(i-(j*8)));
+			if ((pAd->StaCfg.DesiredHtPhyInfo.MCSSet[j] & bitmask) && (pHtCapability->MCSSet[j] & bitmask))
+			{
+				pEntry->MaxHTPhyMode.field.MCS = i;
+				break;
+			}
+			if (i==0)
+				break;
+		}
+
+
+		if (pAd->StaCfg.DesiredTransmitSetting.field.MCS != MCS_AUTO)
+		{
+			if (pAd->StaCfg.DesiredTransmitSetting.field.MCS == 32)
+			{
+				// Fix MCS as HT Duplicated Mode
+				pEntry->MaxHTPhyMode.field.BW = 1;
+				pEntry->MaxHTPhyMode.field.MODE = MODE_HTMIX;
+				pEntry->MaxHTPhyMode.field.STBC = 0;
+				pEntry->MaxHTPhyMode.field.ShortGI = 0;
+				pEntry->MaxHTPhyMode.field.MCS = 32;
+			}
+			else if (pEntry->MaxHTPhyMode.field.MCS > pAd->StaCfg.HTPhyMode.field.MCS)
+			{
+				// STA supports fixed MCS
+				pEntry->MaxHTPhyMode.field.MCS = pAd->StaCfg.HTPhyMode.field.MCS;
+			}
+		}
+
+		pEntry->MaxHTPhyMode.field.STBC = (pHtCapability->HtCapInfo.RxSTBC & (pAd->CommonCfg.DesiredHtPhy.TxSTBC));
+		pEntry->MpduDensity = pHtCapability->HtCapParm.MpduDensity;
+		pEntry->MaxRAmpduFactor = pHtCapability->HtCapParm.MaxRAmpduFactor;
+		pEntry->MmpsMode = (UCHAR)pHtCapability->HtCapInfo.MimoPs;
+		pEntry->AMsduSize = (UCHAR)pHtCapability->HtCapInfo.AMsduSize;
+		pEntry->HTPhyMode.word = pEntry->MaxHTPhyMode.word;
+
+		if (pAd->CommonCfg.DesiredHtPhy.AmsduEnable && (pAd->CommonCfg.REGBACapability.field.AutoBA == FALSE))
+			CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_AMSDU_INUSED);
+		if (pHtCapability->HtCapInfo.ShortGIfor20)
+			CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_SGI20_CAPABLE);
+		if (pHtCapability->HtCapInfo.ShortGIfor40)
+			CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_SGI40_CAPABLE);
+		if (pHtCapability->HtCapInfo.TxSTBC)
+			CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_TxSTBC_CAPABLE);
+		if (pHtCapability->HtCapInfo.RxSTBC)
+			CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_RxSTBC_CAPABLE);
+		if (pHtCapability->ExtHtCapInfo.PlusHTC)
+			CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_HTC_CAPABLE);
+		if (pAd->CommonCfg.bRdg && pHtCapability->ExtHtCapInfo.RDGSupport)
+			CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_RDG_CAPABLE);
+		if (pHtCapability->ExtHtCapInfo.MCSFeedback == 0x03)
+			CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_MCSFEEDBACK_CAPABLE);
+	}
+	else
+	{
+		pAd->MacTab.fAnyStationIsLegacy = TRUE;
+	}
+
+	NdisMoveMemory(&pEntry->HTCapability, pHtCapability, sizeof(HT_CAPABILITY_IE));
+#endif // DOT11_N_SUPPORT //
+
+	pEntry->HTPhyMode.word = pEntry->MaxHTPhyMode.word;
+	pEntry->CurrTxRate = pEntry->MaxSupportedRate;
+
+	// Set asic auto fall back
+	if (pAd->StaCfg.bAutoTxRateSwitch == TRUE)
+	{
+		PUCHAR					pTable;
+		UCHAR					TableSize = 0;
+
+		MlmeSelectTxRateTable(pAd, pEntry, &pTable, &TableSize, &pEntry->CurrTxRateIndex);
+		pEntry->bAutoTxRateSwitch = TRUE;
+	}
+	else
+	{
+		pEntry->HTPhyMode.field.MODE	= pAd->StaCfg.HTPhyMode.field.MODE;
+		pEntry->HTPhyMode.field.MCS	= pAd->StaCfg.HTPhyMode.field.MCS;
+		pEntry->bAutoTxRateSwitch = FALSE;
+
+		// If the legacy mode is set, overwrite the transmit setting of this entry.
+		RTMPUpdateLegacyTxSetting((UCHAR)pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode, pEntry);
+	}
+
+	pEntry->PortSecured = WPA_802_1X_PORT_SECURED;
+	pEntry->Sst = SST_ASSOC;
+	pEntry->AuthState = AS_AUTH_OPEN;
+	pEntry->AuthMode = pAd->StaCfg.AuthMode;
+	pEntry->WepStatus = pAd->StaCfg.WepStatus;
+
+	NdisReleaseSpinLock(&pAd->MacTabLock);
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+    if (pAd->StaCfg.WpaSupplicantUP)
+    {
+        union iwreq_data    wrqu;
+
+        SendAssocIEsToWpaSupplicant(pAd);
+        memset(&wrqu, 0, sizeof(wrqu));
+        wrqu.data.flags = RT_ASSOC_EVENT_FLAG;
+        wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+    }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+    {
+        union iwreq_data    wrqu;
+        wext_notify_event_assoc(pAd);
+
+        memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+        memcpy(wrqu.ap_addr.sa_data, pAd->MlmeAux.Bssid, MAC_ADDR_LEN);
+        wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+
+    }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+	return TRUE;
+}
+
+
diff --git a/drivers/staging/rt2870/sta/auth.c b/drivers/staging/rt2870/sta/auth.c
new file mode 100644
index 00000000000..73fb8d6ea76
--- /dev/null
+++ b/drivers/staging/rt2870/sta/auth.c
@@ -0,0 +1,474 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	auth.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John		2004-9-3		porting from RT2500
+*/
+#include "../rt_config.h"
+
+/*
+    ==========================================================================
+    Description:
+        authenticate state machine init, including state transition and timer init
+    Parameters:
+        Sm - pointer to the auth state machine
+    Note:
+        The state machine looks like this
+
+                        AUTH_REQ_IDLE           AUTH_WAIT_SEQ2                   AUTH_WAIT_SEQ4
+    MT2_MLME_AUTH_REQ   mlme_auth_req_action    invalid_state_when_auth          invalid_state_when_auth
+    MT2_PEER_AUTH_EVEN  drop                    peer_auth_even_at_seq2_action    peer_auth_even_at_seq4_action
+    MT2_AUTH_TIMEOUT    Drop                    auth_timeout_action              auth_timeout_action
+
+	IRQL = PASSIVE_LEVEL
+
+    ==========================================================================
+ */
+
+void AuthStateMachineInit(
+    IN PRTMP_ADAPTER pAd,
+    IN STATE_MACHINE *Sm,
+    OUT STATE_MACHINE_FUNC Trans[])
+{
+    StateMachineInit(Sm, Trans, MAX_AUTH_STATE, MAX_AUTH_MSG, (STATE_MACHINE_FUNC)Drop, AUTH_REQ_IDLE, AUTH_MACHINE_BASE);
+
+    // the first column
+    StateMachineSetAction(Sm, AUTH_REQ_IDLE, MT2_MLME_AUTH_REQ, (STATE_MACHINE_FUNC)MlmeAuthReqAction);
+
+    // the second column
+    StateMachineSetAction(Sm, AUTH_WAIT_SEQ2, MT2_MLME_AUTH_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenAuth);
+    StateMachineSetAction(Sm, AUTH_WAIT_SEQ2, MT2_PEER_AUTH_EVEN, (STATE_MACHINE_FUNC)PeerAuthRspAtSeq2Action);
+    StateMachineSetAction(Sm, AUTH_WAIT_SEQ2, MT2_AUTH_TIMEOUT, (STATE_MACHINE_FUNC)AuthTimeoutAction);
+
+    // the third column
+    StateMachineSetAction(Sm, AUTH_WAIT_SEQ4, MT2_MLME_AUTH_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenAuth);
+    StateMachineSetAction(Sm, AUTH_WAIT_SEQ4, MT2_PEER_AUTH_EVEN, (STATE_MACHINE_FUNC)PeerAuthRspAtSeq4Action);
+    StateMachineSetAction(Sm, AUTH_WAIT_SEQ4, MT2_AUTH_TIMEOUT, (STATE_MACHINE_FUNC)AuthTimeoutAction);
+
+	RTMPInitTimer(pAd, &pAd->MlmeAux.AuthTimer, GET_TIMER_FUNCTION(AuthTimeout), pAd, FALSE);
+}
+
+/*
+    ==========================================================================
+    Description:
+        function to be executed at timer thread when auth timer expires
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID AuthTimeout(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3)
+{
+    RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+
+    DBGPRINT(RT_DEBUG_TRACE,("AUTH - AuthTimeout\n"));
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS | fRTMP_ADAPTER_NIC_NOT_EXIST))
+		return;
+
+	// send a de-auth to reset AP's state machine (Patch AP-Dir635)
+	if (pAd->Mlme.AuthMachine.CurrState == AUTH_WAIT_SEQ2)
+		Cls2errAction(pAd, pAd->MlmeAux.Bssid);
+
+
+    MlmeEnqueue(pAd, AUTH_STATE_MACHINE, MT2_AUTH_TIMEOUT, 0, NULL);
+    RT28XX_MLME_HANDLER(pAd);
+}
+
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID MlmeAuthReqAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+    UCHAR              Addr[6];
+    USHORT             Alg, Seq, Status;
+    ULONG              Timeout;
+    HEADER_802_11      AuthHdr;
+    BOOLEAN            TimerCancelled;
+    NDIS_STATUS        NStatus;
+    PUCHAR             pOutBuffer = NULL;
+    ULONG              FrameLen = 0;
+
+	// Block all authentication request durning WPA block period
+	if (pAd->StaCfg.bBlockAssoc == TRUE)
+	{
+        DBGPRINT(RT_DEBUG_TRACE, ("AUTH - Block Auth request durning WPA block period!\n"));
+        pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+        Status = MLME_STATE_MACHINE_REJECT;
+        MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_AUTH_CONF, 2, &Status);
+	}
+    else if(MlmeAuthReqSanity(pAd, Elem->Msg, Elem->MsgLen, Addr, &Timeout, &Alg))
+    {
+        // reset timer
+        RTMPCancelTimer(&pAd->MlmeAux.AuthTimer, &TimerCancelled);
+        COPY_MAC_ADDR(pAd->MlmeAux.Bssid, Addr);
+        pAd->MlmeAux.Alg  = Alg;
+        Seq = 1;
+        Status = MLME_SUCCESS;
+
+        NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+        if(NStatus != NDIS_STATUS_SUCCESS)
+        {
+            DBGPRINT(RT_DEBUG_TRACE, ("AUTH - MlmeAuthReqAction(Alg:%d) allocate memory failed\n", Alg));
+            pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+            Status = MLME_FAIL_NO_RESOURCE;
+            MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_AUTH_CONF, 2, &Status);
+            return;
+        }
+
+        DBGPRINT(RT_DEBUG_TRACE, ("AUTH - Send AUTH request seq#1 (Alg=%d)...\n", Alg));
+        MgtMacHeaderInit(pAd, &AuthHdr, SUBTYPE_AUTH, 0, Addr, pAd->MlmeAux.Bssid);
+        MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+                          sizeof(HEADER_802_11),&AuthHdr,
+                          2,                    &Alg,
+                          2,                    &Seq,
+                          2,                    &Status,
+                          END_OF_ARGS);
+        MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+    	MlmeFreeMemory(pAd, pOutBuffer);
+
+        RTMPSetTimer(&pAd->MlmeAux.AuthTimer, Timeout);
+        pAd->Mlme.AuthMachine.CurrState = AUTH_WAIT_SEQ2;
+    }
+    else
+    {
+        DBGPRINT_ERR(("AUTH - MlmeAuthReqAction() sanity check failed\n"));
+        pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+        Status = MLME_INVALID_FORMAT;
+        MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_AUTH_CONF, 2, &Status);
+    }
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID PeerAuthRspAtSeq2Action(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+    UCHAR         Addr2[MAC_ADDR_LEN];
+    USHORT        Seq, Status, RemoteStatus, Alg;
+    UCHAR         ChlgText[CIPHER_TEXT_LEN];
+    UCHAR         CyperChlgText[CIPHER_TEXT_LEN + 8 + 8];
+    UCHAR         Element[2];
+    HEADER_802_11 AuthHdr;
+    BOOLEAN       TimerCancelled;
+    PUCHAR        pOutBuffer = NULL;
+    NDIS_STATUS   NStatus;
+    ULONG         FrameLen = 0;
+    USHORT        Status2;
+
+    if (PeerAuthSanity(pAd, Elem->Msg, Elem->MsgLen, Addr2, &Alg, &Seq, &Status, ChlgText))
+    {
+        if (MAC_ADDR_EQUAL(pAd->MlmeAux.Bssid, Addr2) && Seq == 2)
+        {
+            DBGPRINT(RT_DEBUG_TRACE, ("AUTH - Receive AUTH_RSP seq#2 to me (Alg=%d, Status=%d)\n", Alg, Status));
+            RTMPCancelTimer(&pAd->MlmeAux.AuthTimer, &TimerCancelled);
+
+            if (Status == MLME_SUCCESS)
+            {
+                // Authentication Mode "LEAP" has allow for CCX 1.X
+                if ((pAd->MlmeAux.Alg == Ndis802_11AuthModeOpen)
+#ifdef LEAP_SUPPORT
+					|| (pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP)
+#endif // LEAP_SUPPORT //
+				)
+                {
+                    pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+#ifdef LEAP_SUPPORT
+                    pAd->Mlme.LeapMachine.CurrState = LEAP_IDLE;
+#endif // LEAP_SUPPORT //
+                    MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_AUTH_CONF, 2, &Status);
+                }
+                else
+                {
+                    // 2. shared key, need to be challenged
+                    Seq++;
+                    RemoteStatus = MLME_SUCCESS;
+
+					// Get an unused nonpaged memory
+                    NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);
+                    if(NStatus != NDIS_STATUS_SUCCESS)
+                    {
+                        DBGPRINT(RT_DEBUG_TRACE, ("AUTH - PeerAuthRspAtSeq2Action() allocate memory fail\n"));
+                        pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+                        Status2 = MLME_FAIL_NO_RESOURCE;
+                        MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_AUTH_CONF, 2, &Status2);
+                        return;
+                    }
+
+                    DBGPRINT(RT_DEBUG_TRACE, ("AUTH - Send AUTH request seq#3...\n"));
+                    MgtMacHeaderInit(pAd, &AuthHdr, SUBTYPE_AUTH, 0, Addr2, pAd->MlmeAux.Bssid);
+                    AuthHdr.FC.Wep = 1;
+                    // Encrypt challenge text & auth information
+                    RTMPInitWepEngine(
+                    	pAd,
+                    	pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key,
+                    	pAd->StaCfg.DefaultKeyId,
+                    	pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].KeyLen,
+                    	CyperChlgText);
+
+					Alg = cpu2le16(*(USHORT *)&Alg);
+					Seq = cpu2le16(*(USHORT *)&Seq);
+					RemoteStatus= cpu2le16(*(USHORT *)&RemoteStatus);
+
+					RTMPEncryptData(pAd, (PUCHAR) &Alg, CyperChlgText + 4, 2);
+					RTMPEncryptData(pAd, (PUCHAR) &Seq, CyperChlgText + 6, 2);
+					RTMPEncryptData(pAd, (PUCHAR) &RemoteStatus, CyperChlgText + 8, 2);
+					Element[0] = 16;
+					Element[1] = 128;
+					RTMPEncryptData(pAd, Element, CyperChlgText + 10, 2);
+					RTMPEncryptData(pAd, ChlgText, CyperChlgText + 12, 128);
+					RTMPSetICV(pAd, CyperChlgText + 140);
+                    MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+                                      sizeof(HEADER_802_11),    &AuthHdr,
+                                      CIPHER_TEXT_LEN + 16,     CyperChlgText,
+                                      END_OF_ARGS);
+                    MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+                	MlmeFreeMemory(pAd, pOutBuffer);
+
+                    RTMPSetTimer(&pAd->MlmeAux.AuthTimer, AUTH_TIMEOUT);
+                    pAd->Mlme.AuthMachine.CurrState = AUTH_WAIT_SEQ4;
+                }
+            }
+            else
+            {
+#ifdef LEAP_SUPPORT
+                if (pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP)
+                {
+                    //Invalid Authentication possible rogue AP
+                    //Add this Ap to Rogue AP.
+                    RogueApTableSetEntry(pAd, &pAd->StaCfg.RogueApTab, Addr2, LEAP_REASON_INVALID_AUTH);
+				}
+#endif // LEAP_SUPPORT //
+                pAd->StaCfg.AuthFailReason = Status;
+                COPY_MAC_ADDR(pAd->StaCfg.AuthFailSta, Addr2);
+                pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+                MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_AUTH_CONF, 2, &Status);
+            }
+        }
+    }
+    else
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("AUTH - PeerAuthSanity() sanity check fail\n"));
+    }
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID PeerAuthRspAtSeq4Action(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+    UCHAR         Addr2[MAC_ADDR_LEN];
+    USHORT        Alg, Seq, Status;
+    CHAR          ChlgText[CIPHER_TEXT_LEN];
+    BOOLEAN       TimerCancelled;
+
+    if(PeerAuthSanity(pAd, Elem->Msg, Elem->MsgLen, Addr2, &Alg, &Seq, &Status, ChlgText))
+    {
+        if(MAC_ADDR_EQUAL(pAd->MlmeAux.Bssid, Addr2) && Seq == 4)
+        {
+            DBGPRINT(RT_DEBUG_TRACE, ("AUTH - Receive AUTH_RSP seq#4 to me\n"));
+            RTMPCancelTimer(&pAd->MlmeAux.AuthTimer, &TimerCancelled);
+
+            if (Status != MLME_SUCCESS)
+            {
+                pAd->StaCfg.AuthFailReason = Status;
+                COPY_MAC_ADDR(pAd->StaCfg.AuthFailSta, Addr2);
+            }
+
+            pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+            MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_AUTH_CONF, 2, &Status);
+        }
+    }
+    else
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("AUTH - PeerAuthRspAtSeq4Action() sanity check fail\n"));
+    }
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID MlmeDeauthReqAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+    MLME_DEAUTH_REQ_STRUCT *pInfo;
+    HEADER_802_11 DeauthHdr;
+    PUCHAR        pOutBuffer = NULL;
+    NDIS_STATUS   NStatus;
+    ULONG         FrameLen = 0;
+    USHORT        Status;
+
+    pInfo = (MLME_DEAUTH_REQ_STRUCT *)Elem->Msg;
+
+    NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+    if (NStatus != NDIS_STATUS_SUCCESS)
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("AUTH - MlmeDeauthReqAction() allocate memory fail\n"));
+        pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+        Status = MLME_FAIL_NO_RESOURCE;
+        MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_DEAUTH_CONF, 2, &Status);
+        return;
+    }
+
+    DBGPRINT(RT_DEBUG_TRACE, ("AUTH - Send DE-AUTH request (Reason=%d)...\n", pInfo->Reason));
+    MgtMacHeaderInit(pAd, &DeauthHdr, SUBTYPE_DEAUTH, 0, pInfo->Addr, pAd->MlmeAux.Bssid);
+    MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+                      sizeof(HEADER_802_11),&DeauthHdr,
+                      2,                    &pInfo->Reason,
+                      END_OF_ARGS);
+    MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+    pAd->StaCfg.DeauthReason = pInfo->Reason;
+    COPY_MAC_ADDR(pAd->StaCfg.DeauthSta, pInfo->Addr);
+    pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+    Status = MLME_SUCCESS;
+    MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_DEAUTH_CONF, 2, &Status);
+
+	// send wireless event - for deauthentication
+	if (pAd->CommonCfg.bWirelessEvent)
+		RTMPSendWirelessEvent(pAd, IW_DEAUTH_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID AuthTimeoutAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+    USHORT Status;
+    DBGPRINT(RT_DEBUG_TRACE, ("AUTH - AuthTimeoutAction\n"));
+    pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+    Status = MLME_REJ_TIMEOUT;
+    MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_AUTH_CONF, 2, &Status);
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID InvalidStateWhenAuth(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+    USHORT Status;
+    DBGPRINT(RT_DEBUG_TRACE, ("AUTH - InvalidStateWhenAuth (state=%ld), reset AUTH state machine\n", pAd->Mlme.AuthMachine.CurrState));
+    pAd->Mlme.AuthMachine.CurrState = AUTH_REQ_IDLE;
+    Status = MLME_STATE_MACHINE_REJECT;
+    MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_AUTH_CONF, 2, &Status);
+}
+
+/*
+    ==========================================================================
+    Description:
+        Some STA/AP
+    Note:
+        This action should never trigger AUTH state transition, therefore we
+        separate it from AUTH state machine, and make it as a standalone service
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID Cls2errAction(
+    IN PRTMP_ADAPTER pAd,
+    IN PUCHAR pAddr)
+{
+    HEADER_802_11 DeauthHdr;
+    PUCHAR        pOutBuffer = NULL;
+    NDIS_STATUS   NStatus;
+    ULONG         FrameLen = 0;
+    USHORT        Reason = REASON_CLS2ERR;
+
+    NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+    if (NStatus != NDIS_STATUS_SUCCESS)
+        return;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("AUTH - Class 2 error, Send DEAUTH frame...\n"));
+    MgtMacHeaderInit(pAd, &DeauthHdr, SUBTYPE_DEAUTH, 0, pAddr, pAd->MlmeAux.Bssid);
+    MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+                      sizeof(HEADER_802_11),&DeauthHdr,
+                      2,                    &Reason,
+                      END_OF_ARGS);
+    MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+    pAd->StaCfg.DeauthReason = Reason;
+    COPY_MAC_ADDR(pAd->StaCfg.DeauthSta, pAddr);
+}
+
+
diff --git a/drivers/staging/rt2870/sta/auth_rsp.c b/drivers/staging/rt2870/sta/auth_rsp.c
new file mode 100644
index 00000000000..6e3c2d24cda
--- /dev/null
+++ b/drivers/staging/rt2870/sta/auth_rsp.c
@@ -0,0 +1,166 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	auth_rsp.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John		2004-10-1		copy from RT2560
+*/
+#include "../rt_config.h"
+
+/*
+    ==========================================================================
+    Description:
+        authentication state machine init procedure
+    Parameters:
+        Sm - the state machine
+
+	IRQL = PASSIVE_LEVEL
+
+    ==========================================================================
+ */
+VOID AuthRspStateMachineInit(
+    IN PRTMP_ADAPTER pAd,
+    IN PSTATE_MACHINE Sm,
+    IN STATE_MACHINE_FUNC Trans[])
+{
+    StateMachineInit(Sm, Trans, MAX_AUTH_RSP_STATE, MAX_AUTH_RSP_MSG, (STATE_MACHINE_FUNC)Drop, AUTH_RSP_IDLE, AUTH_RSP_MACHINE_BASE);
+
+    // column 1
+    StateMachineSetAction(Sm, AUTH_RSP_IDLE, MT2_PEER_DEAUTH, (STATE_MACHINE_FUNC)PeerDeauthAction);
+
+    // column 2
+    StateMachineSetAction(Sm, AUTH_RSP_WAIT_CHAL, MT2_PEER_DEAUTH, (STATE_MACHINE_FUNC)PeerDeauthAction);
+
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+*/
+VOID PeerAuthSimpleRspGenAndSend(
+    IN PRTMP_ADAPTER pAd,
+    IN PHEADER_802_11 pHdr80211,
+    IN USHORT Alg,
+    IN USHORT Seq,
+    IN USHORT Reason,
+    IN USHORT Status)
+{
+    HEADER_802_11     AuthHdr;
+    ULONG             FrameLen = 0;
+    PUCHAR            pOutBuffer = NULL;
+    NDIS_STATUS       NStatus;
+
+    if (Reason != MLME_SUCCESS)
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("Peer AUTH fail...\n"));
+        return;
+    }
+
+	//Get an unused nonpaged memory
+    NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);
+    if (NStatus != NDIS_STATUS_SUCCESS)
+        return;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Send AUTH response (seq#2)...\n"));
+    MgtMacHeaderInit(pAd, &AuthHdr, SUBTYPE_AUTH, 0, pHdr80211->Addr2, pAd->MlmeAux.Bssid);
+    MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+                      sizeof(HEADER_802_11),    &AuthHdr,
+                      2,                        &Alg,
+                      2,                        &Seq,
+                      2,                        &Reason,
+                      END_OF_ARGS);
+    MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+*/
+VOID PeerDeauthAction(
+    IN PRTMP_ADAPTER pAd,
+    IN PMLME_QUEUE_ELEM Elem)
+{
+    UCHAR       Addr2[MAC_ADDR_LEN];
+    USHORT      Reason;
+
+    if (PeerDeauthSanity(pAd, Elem->Msg, Elem->MsgLen, Addr2, &Reason))
+    {
+        if (INFRA_ON(pAd) && MAC_ADDR_EQUAL(Addr2, pAd->CommonCfg.Bssid))
+        {
+            DBGPRINT(RT_DEBUG_TRACE,("AUTH_RSP - receive DE-AUTH from our AP (Reason=%d)\n", Reason));
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+            {
+                union iwreq_data    wrqu;
+                memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+                wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+            }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+
+
+			// send wireless event - for deauthentication
+			if (pAd->CommonCfg.bWirelessEvent)
+				RTMPSendWirelessEvent(pAd, IW_DEAUTH_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+
+            LinkDown(pAd, TRUE);
+
+            // Authentication Mode Cisco_LEAP has start a timer
+            // We should cancel it if using LEAP
+#ifdef LEAP_SUPPORT
+            if (pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP)
+            {
+                RTMPCancelTimer(&pAd->StaCfg.LeapAuthTimer, &TimerCancelled);
+                //Check is it mach the LEAP Authentication failed as possible a Rogue AP
+                //on it's PortSecured not equal to WPA_802_1X_PORT_SECURED while process the Authenticaton.
+                if ((pAd->StaCfg.PortSecured != WPA_802_1X_PORT_SECURED) && (pAd->Mlme.LeapMachine.CurrState != LEAP_IDLE))
+                {
+                    RogueApTableSetEntry(pAd, &pAd->StaCfg.RogueApTab, Addr2, LEAP_REASON_AUTH_TIMEOUT);
+                }
+            }
+#endif // LEAP_SUPPORT //
+        }
+    }
+    else
+    {
+        DBGPRINT(RT_DEBUG_TRACE,("AUTH_RSP - PeerDeauthAction() sanity check fail\n"));
+    }
+}
+
diff --git a/drivers/staging/rt2870/sta/connect.c b/drivers/staging/rt2870/sta/connect.c
new file mode 100644
index 00000000000..c93140a8caa
--- /dev/null
+++ b/drivers/staging/rt2870/sta/connect.c
@@ -0,0 +1,2822 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	connect.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John			2004-08-08			Major modification from RT2560
+*/
+#include "../rt_config.h"
+
+UCHAR	CipherSuiteWpaNoneTkip[] = {
+		0x00, 0x50, 0xf2, 0x01,	// oui
+		0x01, 0x00,				// Version
+		0x00, 0x50, 0xf2, 0x02,	// Multicast
+		0x01, 0x00,				// Number of unicast
+		0x00, 0x50, 0xf2, 0x02,	// unicast
+		0x01, 0x00,				// number of authentication method
+		0x00, 0x50, 0xf2, 0x00	// authentication
+		};
+UCHAR	CipherSuiteWpaNoneTkipLen = (sizeof(CipherSuiteWpaNoneTkip) / sizeof(UCHAR));
+
+UCHAR	CipherSuiteWpaNoneAes[] = {
+		0x00, 0x50, 0xf2, 0x01,	// oui
+		0x01, 0x00,				// Version
+		0x00, 0x50, 0xf2, 0x04,	// Multicast
+		0x01, 0x00,				// Number of unicast
+		0x00, 0x50, 0xf2, 0x04,	// unicast
+		0x01, 0x00,				// number of authentication method
+		0x00, 0x50, 0xf2, 0x00	// authentication
+		};
+UCHAR	CipherSuiteWpaNoneAesLen = (sizeof(CipherSuiteWpaNoneAes) / sizeof(UCHAR));
+
+// The following MACRO is called after 1. starting an new IBSS, 2. succesfully JOIN an IBSS,
+// or 3. succesfully ASSOCIATE to a BSS, 4. successfully RE_ASSOCIATE to a BSS
+// All settings successfuly negotiated furing MLME state machines become final settings
+// and are copied to pAd->StaActive
+#define COPY_SETTINGS_FROM_MLME_AUX_TO_ACTIVE_CFG(_pAd)                                 \
+{                                                                                       \
+	(_pAd)->CommonCfg.SsidLen = (_pAd)->MlmeAux.SsidLen;                                \
+	NdisMoveMemory((_pAd)->CommonCfg.Ssid, (_pAd)->MlmeAux.Ssid, (_pAd)->MlmeAux.SsidLen); \
+	COPY_MAC_ADDR((_pAd)->CommonCfg.Bssid, (_pAd)->MlmeAux.Bssid);                      \
+	(_pAd)->CommonCfg.Channel = (_pAd)->MlmeAux.Channel;                                \
+	(_pAd)->CommonCfg.CentralChannel = (_pAd)->MlmeAux.CentralChannel;                  \
+	(_pAd)->StaActive.Aid = (_pAd)->MlmeAux.Aid;                                        \
+	(_pAd)->StaActive.AtimWin = (_pAd)->MlmeAux.AtimWin;                                \
+	(_pAd)->StaActive.CapabilityInfo = (_pAd)->MlmeAux.CapabilityInfo;                  \
+	(_pAd)->CommonCfg.BeaconPeriod = (_pAd)->MlmeAux.BeaconPeriod;                      \
+	(_pAd)->StaActive.CfpMaxDuration = (_pAd)->MlmeAux.CfpMaxDuration;                  \
+	(_pAd)->StaActive.CfpPeriod = (_pAd)->MlmeAux.CfpPeriod;                            \
+	(_pAd)->StaActive.SupRateLen = (_pAd)->MlmeAux.SupRateLen;                          \
+	NdisMoveMemory((_pAd)->StaActive.SupRate, (_pAd)->MlmeAux.SupRate, (_pAd)->MlmeAux.SupRateLen);\
+	(_pAd)->StaActive.ExtRateLen = (_pAd)->MlmeAux.ExtRateLen;                          \
+	NdisMoveMemory((_pAd)->StaActive.ExtRate, (_pAd)->MlmeAux.ExtRate, (_pAd)->MlmeAux.ExtRateLen);\
+	NdisMoveMemory(&(_pAd)->CommonCfg.APEdcaParm, &(_pAd)->MlmeAux.APEdcaParm, sizeof(EDCA_PARM));\
+	NdisMoveMemory(&(_pAd)->CommonCfg.APQosCapability, &(_pAd)->MlmeAux.APQosCapability, sizeof(QOS_CAPABILITY_PARM));\
+	NdisMoveMemory(&(_pAd)->CommonCfg.APQbssLoad, &(_pAd)->MlmeAux.APQbssLoad, sizeof(QBSS_LOAD_PARM));\
+	COPY_MAC_ADDR((_pAd)->MacTab.Content[BSSID_WCID].Addr, (_pAd)->MlmeAux.Bssid);      \
+	(_pAd)->MacTab.Content[BSSID_WCID].Aid = (_pAd)->MlmeAux.Aid;                       \
+	(_pAd)->MacTab.Content[BSSID_WCID].PairwiseKey.CipherAlg = (_pAd)->StaCfg.PairCipher;\
+	COPY_MAC_ADDR((_pAd)->MacTab.Content[BSSID_WCID].PairwiseKey.BssId, (_pAd)->MlmeAux.Bssid);\
+	(_pAd)->MacTab.Content[BSSID_WCID].RateLen = (_pAd)->StaActive.SupRateLen + (_pAd)->StaActive.ExtRateLen;\
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = PASSIVE_LEVEL
+
+	==========================================================================
+*/
+VOID MlmeCntlInit(
+	IN PRTMP_ADAPTER pAd,
+	IN STATE_MACHINE *S,
+	OUT STATE_MACHINE_FUNC Trans[])
+{
+	// Control state machine differs from other state machines, the interface
+	// follows the standard interface
+	pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID MlmeCntlMachinePerformAction(
+	IN PRTMP_ADAPTER pAd,
+	IN STATE_MACHINE *S,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	switch(pAd->Mlme.CntlMachine.CurrState)
+	{
+		case CNTL_IDLE:
+				CntlIdleProc(pAd, Elem);
+			break;
+		case CNTL_WAIT_DISASSOC:
+			CntlWaitDisassocProc(pAd, Elem);
+			break;
+		case CNTL_WAIT_JOIN:
+			CntlWaitJoinProc(pAd, Elem);
+			break;
+
+		// CNTL_WAIT_REASSOC is the only state in CNTL machine that does
+		// not triggered directly or indirectly by "RTMPSetInformation(OID_xxx)".
+		// Therefore not protected by NDIS's "only one outstanding OID request"
+		// rule. Which means NDIS may SET OID in the middle of ROAMing attempts.
+		// Current approach is to block new SET request at RTMPSetInformation()
+		// when CntlMachine.CurrState is not CNTL_IDLE
+		case CNTL_WAIT_REASSOC:
+			CntlWaitReassocProc(pAd, Elem);
+			break;
+
+		case CNTL_WAIT_START:
+			CntlWaitStartProc(pAd, Elem);
+			break;
+		case CNTL_WAIT_AUTH:
+			CntlWaitAuthProc(pAd, Elem);
+			break;
+		case CNTL_WAIT_AUTH2:
+			CntlWaitAuthProc2(pAd, Elem);
+			break;
+		case CNTL_WAIT_ASSOC:
+			CntlWaitAssocProc(pAd, Elem);
+			break;
+
+		case CNTL_WAIT_OID_LIST_SCAN:
+			if(Elem->MsgType == MT2_SCAN_CONF)
+			{
+				// Resume TxRing after SCANING complete. We hope the out-of-service time
+				// won't be too long to let upper layer time-out the waiting frames
+				RTMPResumeMsduTransmission(pAd);
+				if (pAd->StaCfg.CCXReqType != MSRN_TYPE_UNUSED)
+				{
+					// Cisco scan request is finished, prepare beacon report
+					MlmeEnqueue(pAd, AIRONET_STATE_MACHINE, MT2_AIRONET_SCAN_DONE, 0, NULL);
+				}
+				pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+
+                //
+				// Set LED status to previous status.
+				//
+				if (pAd->bLedOnScanning)
+				{
+					pAd->bLedOnScanning = FALSE;
+					RTMPSetLED(pAd, pAd->LedStatus);
+				}
+#ifdef DOT11N_DRAFT3
+				// AP sent a 2040Coexistence mgmt frame, then station perform a scan, and then send back the respone.
+				if (pAd->CommonCfg.BSSCoexist2040.field.InfoReq == 1)
+				{
+					Update2040CoexistFrameAndNotify(pAd, BSSID_WCID, TRUE);
+				}
+#endif // DOT11N_DRAFT3 //
+			}
+			break;
+
+		case CNTL_WAIT_OID_DISASSOC:
+			if (Elem->MsgType == MT2_DISASSOC_CONF)
+			{
+				LinkDown(pAd, FALSE);
+				pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+			}
+			break;
+#ifdef RT2870
+		//
+		// This state is for that we want to connect to an AP but
+		// it didn't find on BSS List table. So we need to scan the air first,
+		// after that we can try to connect to the desired AP if available.
+		//
+		case CNTL_WAIT_SCAN_FOR_CONNECT:
+			if(Elem->MsgType == MT2_SCAN_CONF)
+			{
+				// Resume TxRing after SCANING complete. We hope the out-of-service time
+				// won't be too long to let upper layer time-out the waiting frames
+				RTMPResumeMsduTransmission(pAd);
+#ifdef CCX_SUPPORT
+				if (pAd->StaCfg.CCXReqType != MSRN_TYPE_UNUSED)
+				{
+					// Cisco scan request is finished, prepare beacon report
+					MlmeEnqueue(pAd, AIRONET_STATE_MACHINE, MT2_AIRONET_SCAN_DONE, 0, NULL);
+				}
+#endif // CCX_SUPPORT //
+				pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+
+				//
+				// Check if we can connect to.
+				//
+				BssTableSsidSort(pAd, &pAd->MlmeAux.SsidBssTab, pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.AutoReconnectSsidLen);
+				if (pAd->MlmeAux.SsidBssTab.BssNr > 0)
+				{
+					MlmeAutoReconnectLastSSID(pAd);
+				}
+			}
+			break;
+#endif // RT2870 //
+		default:
+			DBGPRINT_ERR(("!ERROR! CNTL - Illegal message type(=%ld)", Elem->MsgType));
+			break;
+	}
+}
+
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlIdleProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	MLME_DISASSOC_REQ_STRUCT   DisassocReq;
+
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF))
+		return;
+
+	switch(Elem->MsgType)
+	{
+		case OID_802_11_SSID:
+			CntlOidSsidProc(pAd, Elem);
+			break;
+
+		case OID_802_11_BSSID:
+			CntlOidRTBssidProc(pAd,Elem);
+			break;
+
+		case OID_802_11_BSSID_LIST_SCAN:
+			CntlOidScanProc(pAd,Elem);
+			break;
+
+		case OID_802_11_DISASSOCIATE:
+#ifdef RALINK_ATE
+			if(ATE_ON(pAd))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+				break;
+			}
+#endif // RALINK_ATE //
+			DisassocParmFill(pAd, &DisassocReq, pAd->CommonCfg.Bssid, REASON_DISASSOC_STA_LEAVING);
+			MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_DISASSOC_REQ, sizeof(MLME_DISASSOC_REQ_STRUCT), &DisassocReq);
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_DISASSOC;
+#ifdef WPA_SUPPLICANT_SUPPORT
+            if (pAd->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_ENABLE_WITH_WEB_UI)
+#endif // WPA_SUPPLICANT_SUPPORT //
+            {
+    			// Set the AutoReconnectSsid to prevent it reconnect to old SSID
+    			// Since calling this indicate user don't want to connect to that SSID anymore.
+    			pAd->MlmeAux.AutoReconnectSsidLen= 32;
+    			NdisZeroMemory(pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.AutoReconnectSsidLen);
+            }
+			break;
+
+		case MT2_MLME_ROAMING_REQ:
+			CntlMlmeRoamingProc(pAd, Elem);
+			break;
+
+        case OID_802_11_MIC_FAILURE_REPORT_FRAME:
+            WpaMicFailureReportFrame(pAd, Elem);
+            break;
+
+#ifdef QOS_DLS_SUPPORT
+		case RT_OID_802_11_SET_DLS_PARAM:
+			CntlOidDLSSetupProc(pAd, Elem);
+			break;
+#endif // QOS_DLS_SUPPORT //
+
+		default:
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - Illegal message in CntlIdleProc(MsgType=%ld)\n",Elem->MsgType));
+			break;
+	}
+}
+
+VOID CntlOidScanProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	MLME_SCAN_REQ_STRUCT       ScanReq;
+	ULONG                      BssIdx = BSS_NOT_FOUND;
+	BSS_ENTRY                  CurrBss;
+
+#ifdef RALINK_ATE
+/* Disable scanning when ATE is running. */
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+
+	// record current BSS if network is connected.
+	// 2003-2-13 do not include current IBSS if this is the only STA in this IBSS.
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+	{
+		BssIdx = BssSsidTableSearch(&pAd->ScanTab, pAd->CommonCfg.Bssid, pAd->CommonCfg.Ssid, pAd->CommonCfg.SsidLen, pAd->CommonCfg.Channel);
+		if (BssIdx != BSS_NOT_FOUND)
+		{
+			NdisMoveMemory(&CurrBss, &pAd->ScanTab.BssEntry[BssIdx], sizeof(BSS_ENTRY));
+		}
+	}
+
+	// clean up previous SCAN result, add current BSS back to table if any
+	BssTableInit(&pAd->ScanTab);
+	if (BssIdx != BSS_NOT_FOUND)
+	{
+		// DDK Note: If the NIC is associated with a particular BSSID and SSID
+		//    that are not contained in the list of BSSIDs generated by this scan, the
+		//    BSSID description of the currently associated BSSID and SSID should be
+		//    appended to the list of BSSIDs in the NIC's database.
+		// To ensure this, we append this BSS as the first entry in SCAN result
+		NdisMoveMemory(&pAd->ScanTab.BssEntry[0], &CurrBss, sizeof(BSS_ENTRY));
+		pAd->ScanTab.BssNr = 1;
+	}
+
+	ScanParmFill(pAd, &ScanReq, "", 0, BSS_ANY, SCAN_ACTIVE);
+	MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_SCAN_REQ,
+		sizeof(MLME_SCAN_REQ_STRUCT), &ScanReq);
+	pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_LIST_SCAN;
+}
+
+/*
+	==========================================================================
+	Description:
+		Before calling this routine, user desired SSID should already been
+		recorded in CommonCfg.Ssid[]
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlOidSsidProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM * Elem)
+{
+	PNDIS_802_11_SSID          pOidSsid = (NDIS_802_11_SSID *)Elem->Msg;
+	MLME_DISASSOC_REQ_STRUCT   DisassocReq;
+	ULONG					   Now;
+
+	// Step 1. record the desired user settings to MlmeAux
+	NdisZeroMemory(pAd->MlmeAux.Ssid, MAX_LEN_OF_SSID);
+	NdisMoveMemory(pAd->MlmeAux.Ssid, pOidSsid->Ssid, pOidSsid->SsidLength);
+	pAd->MlmeAux.SsidLen = (UCHAR)pOidSsid->SsidLength;
+	NdisZeroMemory(pAd->MlmeAux.Bssid, MAC_ADDR_LEN);
+	pAd->MlmeAux.BssType = pAd->StaCfg.BssType;
+
+
+	//
+	// Update Reconnect Ssid, that user desired to connect.
+	//
+	NdisZeroMemory(pAd->MlmeAux.AutoReconnectSsid, MAX_LEN_OF_SSID);
+	NdisMoveMemory(pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen);
+	pAd->MlmeAux.AutoReconnectSsidLen = pAd->MlmeAux.SsidLen;
+
+	// step 2. find all matching BSS in the lastest SCAN result (inBssTab)
+	//    & log them into MlmeAux.SsidBssTab for later-on iteration. Sort by RSSI order
+	BssTableSsidSort(pAd, &pAd->MlmeAux.SsidBssTab, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("CntlOidSsidProc():CNTL - %d BSS of %d BSS match the desire (%d)SSID - %s\n",
+			pAd->MlmeAux.SsidBssTab.BssNr, pAd->ScanTab.BssNr, pAd->MlmeAux.SsidLen, pAd->MlmeAux.Ssid));
+	NdisGetSystemUpTime(&Now);
+
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED) &&
+		(pAd->CommonCfg.SsidLen == pAd->MlmeAux.SsidBssTab.BssEntry[0].SsidLen) &&
+		NdisEqualMemory(pAd->CommonCfg.Ssid, pAd->MlmeAux.SsidBssTab.BssEntry[0].Ssid, pAd->CommonCfg.SsidLen) &&
+		MAC_ADDR_EQUAL(pAd->CommonCfg.Bssid, pAd->MlmeAux.SsidBssTab.BssEntry[0].Bssid))
+	{
+		// Case 1. already connected with an AP who has the desired SSID
+		//         with highest RSSI
+
+		// Add checking Mode "LEAP" for CCX 1.0
+		if (((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
+			 (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+			 (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) ||
+			 (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
+#ifdef LEAP_SUPPORT
+			 || (pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP)
+#endif // LEAP_SUPPORT //
+			 ) &&
+			(pAd->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
+		{
+			// case 1.1 For WPA, WPA-PSK, if the 1x port is not secured, we have to redo
+			//          connection process
+			DBGPRINT(RT_DEBUG_TRACE, ("CntlOidSsidProc():CNTL - disassociate with current AP...\n"));
+			DisassocParmFill(pAd, &DisassocReq, pAd->CommonCfg.Bssid, REASON_DISASSOC_STA_LEAVING);
+			MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_DISASSOC_REQ,
+						sizeof(MLME_DISASSOC_REQ_STRUCT), &DisassocReq);
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_DISASSOC;
+		}
+		else if (pAd->bConfigChanged == TRUE)
+		{
+			// case 1.2 Important Config has changed, we have to reconnect to the same AP
+			DBGPRINT(RT_DEBUG_TRACE, ("CntlOidSsidProc():CNTL - disassociate with current AP Because config changed...\n"));
+			DisassocParmFill(pAd, &DisassocReq, pAd->CommonCfg.Bssid, REASON_DISASSOC_STA_LEAVING);
+			MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_DISASSOC_REQ,
+						sizeof(MLME_DISASSOC_REQ_STRUCT), &DisassocReq);
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_DISASSOC;
+		}
+		else
+		{
+			// case 1.3. already connected to the SSID with highest RSSI.
+			DBGPRINT(RT_DEBUG_TRACE, ("CntlOidSsidProc():CNTL - already with this BSSID. ignore this SET_SSID request\n"));
+			//
+			// (HCT 12.1) 1c_wlan_mediaevents required
+			// media connect events are indicated when associating with the same AP
+			//
+			if (INFRA_ON(pAd))
+			{
+				//
+				// Since MediaState already is NdisMediaStateConnected
+				// We just indicate the connect event again to meet the WHQL required.
+				//
+				pAd->IndicateMediaState = NdisMediaStateConnected;
+				RTMP_IndicateMediaState(pAd);
+                pAd->ExtraInfo = GENERAL_LINK_UP;   // Update extra information to link is up
+			}
+
+			pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+            {
+                union iwreq_data    wrqu;
+
+                memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+                memcpy(wrqu.ap_addr.sa_data, pAd->MlmeAux.Bssid, MAC_ADDR_LEN);
+                wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+
+            }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+		}
+	}
+	else if (INFRA_ON(pAd))
+	{
+		//
+		// For RT61
+		// [88888] OID_802_11_SSID should have returned NDTEST_WEP_AP2(Returned: )
+		// RT61 may lost SSID, and not connect to NDTEST_WEP_AP2 and will connect to NDTEST_WEP_AP2 by Autoreconnect
+		// But media status is connected, so the SSID not report correctly.
+		//
+		if (!SSID_EQUAL(pAd->CommonCfg.Ssid, pAd->CommonCfg.SsidLen, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen))
+		{
+			//
+			// Different SSID means not Roaming case, so we let LinkDown() to Indicate a disconnect event.
+			//
+			pAd->MlmeAux.CurrReqIsFromNdis = TRUE;
+		}
+		// case 2. active INFRA association existent
+		//    roaming is done within miniport driver, nothing to do with configuration
+		//    utility. so upon a new SET(OID_802_11_SSID) is received, we just
+		//    disassociate with the current associated AP,
+		//    then perform a new association with this new SSID, no matter the
+		//    new/old SSID are the same or not.
+		DBGPRINT(RT_DEBUG_TRACE, ("CntlOidSsidProc():CNTL - disassociate with current AP...\n"));
+		DisassocParmFill(pAd, &DisassocReq, pAd->CommonCfg.Bssid, REASON_DISASSOC_STA_LEAVING);
+		MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_DISASSOC_REQ,
+					sizeof(MLME_DISASSOC_REQ_STRUCT), &DisassocReq);
+		pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_DISASSOC;
+	}
+	else
+	{
+		if (ADHOC_ON(pAd))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("CntlOidSsidProc():CNTL - drop current ADHOC\n"));
+			LinkDown(pAd, FALSE);
+			OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED);
+			pAd->IndicateMediaState = NdisMediaStateDisconnected;
+			RTMP_IndicateMediaState(pAd);
+            pAd->ExtraInfo = GENERAL_LINK_DOWN;
+			DBGPRINT(RT_DEBUG_TRACE, ("CntlOidSsidProc():NDIS_STATUS_MEDIA_DISCONNECT Event C!\n"));
+		}
+
+		if ((pAd->MlmeAux.SsidBssTab.BssNr == 0) &&
+			(pAd->StaCfg.bAutoReconnect == TRUE) &&
+			(pAd->MlmeAux.BssType == BSS_INFRA) &&
+			(MlmeValidateSSID(pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen) == TRUE)
+			)
+		{
+			MLME_SCAN_REQ_STRUCT       ScanReq;
+
+			DBGPRINT(RT_DEBUG_TRACE, ("CntlOidSsidProc():CNTL - No matching BSS, start a new scan\n"));
+			ScanParmFill(pAd, &ScanReq, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen, BSS_ANY, SCAN_ACTIVE);
+			MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_SCAN_REQ, sizeof(MLME_SCAN_REQ_STRUCT), &ScanReq);
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_LIST_SCAN;
+			// Reset Missed scan number
+			pAd->StaCfg.LastScanTime = Now;
+		}
+		else
+		{
+			pAd->MlmeAux.BssIdx = 0;
+			IterateOnBssTab(pAd);
+		}
+	}
+}
+
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlOidRTBssidProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM * Elem)
+{
+	ULONG       BssIdx;
+	PUCHAR      pOidBssid = (PUCHAR)Elem->Msg;
+	MLME_DISASSOC_REQ_STRUCT    DisassocReq;
+	MLME_JOIN_REQ_STRUCT        JoinReq;
+
+#ifdef RALINK_ATE
+/* No need to perform this routine when ATE is running. */
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+	// record user desired settings
+	COPY_MAC_ADDR(pAd->MlmeAux.Bssid, pOidBssid);
+	pAd->MlmeAux.BssType = pAd->StaCfg.BssType;
+
+	//
+	// Update Reconnect Ssid, that user desired to connect.
+	//
+	NdisZeroMemory(pAd->MlmeAux.AutoReconnectSsid, MAX_LEN_OF_SSID);
+	pAd->MlmeAux.AutoReconnectSsidLen = pAd->MlmeAux.SsidLen;
+	NdisMoveMemory(pAd->MlmeAux.AutoReconnectSsid, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen);
+
+	// find the desired BSS in the latest SCAN result table
+	BssIdx = BssTableSearch(&pAd->ScanTab, pOidBssid, pAd->MlmeAux.Channel);
+	if (BssIdx == BSS_NOT_FOUND)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("CNTL - BSSID not found. reply NDIS_STATUS_NOT_ACCEPTED\n"));
+		pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+		return;
+	}
+
+	// copy the matched BSS entry from ScanTab to MlmeAux.SsidBssTab. Why?
+	// Because we need this entry to become the JOIN target in later on SYNC state machine
+	pAd->MlmeAux.BssIdx = 0;
+	pAd->MlmeAux.SsidBssTab.BssNr = 1;
+	NdisMoveMemory(&pAd->MlmeAux.SsidBssTab.BssEntry[0], &pAd->ScanTab.BssEntry[BssIdx], sizeof(BSS_ENTRY));
+
+	//pAd->MlmeAux.AutoReconnectSsidLen = pAd->ScanTab.BssEntry[BssIdx].SsidLen;
+	//NdisMoveMemory(pAd->MlmeAux.AutoReconnectSsid, pAd->ScanTab.BssEntry[BssIdx].Ssid, pAd->ScanTab.BssEntry[BssIdx].SsidLen);
+
+	// Add SSID into MlmeAux for site surey joining hidden SSID
+	//pAd->MlmeAux.SsidLen = pAd->ScanTab.BssEntry[BssIdx].SsidLen;
+	//NdisMoveMemory(pAd->MlmeAux.Ssid, pAd->ScanTab.BssEntry[BssIdx].Ssid, pAd->MlmeAux.SsidLen);
+
+	// 2002-11-26 skip the following checking. i.e. if user wants to re-connect to same AP
+	//   we just follow normal procedure. The reason of user doing this may because he/she changed
+	//   AP to another channel, but we still received BEACON from it thus don't claim Link Down.
+	//   Since user knows he's changed AP channel, he'll re-connect again. By skipping the following
+	//   checking, we'll disassociate then re-do normal association with this AP at the new channel.
+	// 2003-1-6 Re-enable this feature based on microsoft requirement which prefer not to re-do
+	//   connection when setting the same BSSID.
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED) &&
+		MAC_ADDR_EQUAL(pAd->CommonCfg.Bssid, pOidBssid))
+	{
+		// already connected to the same BSSID, go back to idle state directly
+		DBGPRINT(RT_DEBUG_TRACE, ("CNTL - already in this BSSID. ignore this SET_BSSID request\n"));
+		pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+            {
+                union iwreq_data    wrqu;
+
+                memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+                memcpy(wrqu.ap_addr.sa_data, pAd->MlmeAux.Bssid, MAC_ADDR_LEN);
+                wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+
+            }
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+	}
+	else
+	{
+		if (INFRA_ON(pAd))
+		{
+			// disassoc from current AP first
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - disassociate with current AP ...\n"));
+			DisassocParmFill(pAd, &DisassocReq, pAd->CommonCfg.Bssid, REASON_DISASSOC_STA_LEAVING);
+			MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_DISASSOC_REQ,
+						sizeof(MLME_DISASSOC_REQ_STRUCT), &DisassocReq);
+
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_DISASSOC;
+		}
+		else
+		{
+			if (ADHOC_ON(pAd))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("CNTL - drop current ADHOC\n"));
+				LinkDown(pAd, FALSE);
+				OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED);
+				pAd->IndicateMediaState = NdisMediaStateDisconnected;
+				RTMP_IndicateMediaState(pAd);
+                pAd->ExtraInfo = GENERAL_LINK_DOWN;
+				DBGPRINT(RT_DEBUG_TRACE, ("NDIS_STATUS_MEDIA_DISCONNECT Event C!\n"));
+			}
+
+			// Change the wepstatus to original wepstatus
+			pAd->StaCfg.WepStatus   = pAd->StaCfg.OrigWepStatus;
+			pAd->StaCfg.PairCipher  = pAd->StaCfg.OrigWepStatus;
+			pAd->StaCfg.GroupCipher = pAd->StaCfg.OrigWepStatus;
+
+			// Check cipher suite, AP must have more secured cipher than station setting
+			// Set the Pairwise and Group cipher to match the intended AP setting
+			// We can only connect to AP with less secured cipher setting
+			if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK))
+			{
+				pAd->StaCfg.GroupCipher = pAd->ScanTab.BssEntry[BssIdx].WPA.GroupCipher;
+
+				if (pAd->StaCfg.WepStatus == pAd->ScanTab.BssEntry[BssIdx].WPA.PairCipher)
+					pAd->StaCfg.PairCipher = pAd->ScanTab.BssEntry[BssIdx].WPA.PairCipher;
+				else if (pAd->ScanTab.BssEntry[BssIdx].WPA.PairCipherAux != Ndis802_11WEPDisabled)
+					pAd->StaCfg.PairCipher = pAd->ScanTab.BssEntry[BssIdx].WPA.PairCipherAux;
+				else	// There is no PairCipher Aux, downgrade our capability to TKIP
+					pAd->StaCfg.PairCipher = Ndis802_11Encryption2Enabled;
+			}
+			else if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK))
+			{
+				pAd->StaCfg.GroupCipher = pAd->ScanTab.BssEntry[BssIdx].WPA2.GroupCipher;
+
+				if (pAd->StaCfg.WepStatus == pAd->ScanTab.BssEntry[BssIdx].WPA2.PairCipher)
+					pAd->StaCfg.PairCipher = pAd->ScanTab.BssEntry[BssIdx].WPA2.PairCipher;
+				else if (pAd->ScanTab.BssEntry[BssIdx].WPA2.PairCipherAux != Ndis802_11WEPDisabled)
+					pAd->StaCfg.PairCipher = pAd->ScanTab.BssEntry[BssIdx].WPA2.PairCipherAux;
+				else	// There is no PairCipher Aux, downgrade our capability to TKIP
+					pAd->StaCfg.PairCipher = Ndis802_11Encryption2Enabled;
+
+				// RSN capability
+				pAd->StaCfg.RsnCapability = pAd->ScanTab.BssEntry[BssIdx].WPA2.RsnCapability;
+			}
+
+			// Set Mix cipher flag
+			pAd->StaCfg.bMixCipher = (pAd->StaCfg.PairCipher == pAd->StaCfg.GroupCipher) ? FALSE : TRUE;
+			if (pAd->StaCfg.bMixCipher == TRUE)
+			{
+				// If mix cipher, re-build RSNIE
+				RTMPMakeRSNIE(pAd, pAd->StaCfg.AuthMode, pAd->StaCfg.WepStatus, 0);
+			}
+			// No active association, join the BSS immediately
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - joining %02x:%02x:%02x:%02x:%02x:%02x ...\n",
+				pOidBssid[0],pOidBssid[1],pOidBssid[2],pOidBssid[3],pOidBssid[4],pOidBssid[5]));
+
+			JoinParmFill(pAd, &JoinReq, pAd->MlmeAux.BssIdx);
+			MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_JOIN_REQ, sizeof(MLME_JOIN_REQ_STRUCT), &JoinReq);
+
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_JOIN;
+		}
+	}
+}
+
+// Roaming is the only external request triggering CNTL state machine
+// despite of other "SET OID" operation. All "SET OID" related oerations
+// happen in sequence, because no other SET OID will be sent to this device
+// until the the previous SET operation is complete (successful o failed).
+// So, how do we quarantee this ROAMING request won't corrupt other "SET OID"?
+// or been corrupted by other "SET OID"?
+//
+// IRQL = DISPATCH_LEVEL
+VOID CntlMlmeRoamingProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	// TODO:
+	// AP in different channel may show lower RSSI than actual value??
+	// should we add a weighting factor to compensate it?
+	DBGPRINT(RT_DEBUG_TRACE,("CNTL - Roaming in MlmeAux.RoamTab...\n"));
+
+	NdisMoveMemory(&pAd->MlmeAux.SsidBssTab, &pAd->MlmeAux.RoamTab, sizeof(pAd->MlmeAux.RoamTab));
+	pAd->MlmeAux.SsidBssTab.BssNr = pAd->MlmeAux.RoamTab.BssNr;
+
+	BssTableSortByRssi(&pAd->MlmeAux.SsidBssTab);
+	pAd->MlmeAux.BssIdx = 0;
+	IterateOnBssTab(pAd);
+}
+
+#ifdef QOS_DLS_SUPPORT
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlOidDLSSetupProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	PRT_802_11_DLS		pDLS = (PRT_802_11_DLS)Elem->Msg;
+	MLME_DLS_REQ_STRUCT	MlmeDlsReq;
+	INT					i;
+	USHORT				reason = REASON_UNSPECIFY;
+
+	DBGPRINT(RT_DEBUG_TRACE,("CNTL - (OID set %02x:%02x:%02x:%02x:%02x:%02x with Valid=%d, Status=%d, TimeOut=%d, CountDownTimer=%d)\n",
+		pDLS->MacAddr[0], pDLS->MacAddr[1], pDLS->MacAddr[2], pDLS->MacAddr[3], pDLS->MacAddr[4], pDLS->MacAddr[5],
+		pDLS->Valid, pDLS->Status, pDLS->TimeOut, pDLS->CountDownTimer));
+
+	if (!pAd->CommonCfg.bDLSCapable)
+		return;
+
+	// DLS will not be supported when Adhoc mode
+	if (INFRA_ON(pAd))
+	{
+		for (i = 0; i < MAX_NUM_OF_DLS_ENTRY; i++)
+		{
+			if (pDLS->Valid && pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH) &&
+				(pDLS->TimeOut == pAd->StaCfg.DLSEntry[i].TimeOut) && MAC_ADDR_EQUAL(pDLS->MacAddr, pAd->StaCfg.DLSEntry[i].MacAddr))
+			{
+				// 1. Same setting, just drop it
+				DBGPRINT(RT_DEBUG_TRACE,("CNTL - setting unchanged\n"));
+				break;
+			}
+			else if (!pDLS->Valid && pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH) &&
+				MAC_ADDR_EQUAL(pDLS->MacAddr, pAd->StaCfg.DLSEntry[i].MacAddr))
+			{
+				// 2. Disable DLS link case, just tear down DLS link
+				reason = REASON_QOS_UNWANTED_MECHANISM;
+				pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+				pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+				DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+				MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_TEAR_DOWN, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+				DBGPRINT(RT_DEBUG_TRACE,("CNTL - start tear down procedure\n"));
+				break;
+			}
+			else if ((i < MAX_NUM_OF_DLS_ENTRY) && pDLS->Valid && !pAd->StaCfg.DLSEntry[i].Valid)
+			{
+				// 3. Enable case, start DLS setup procedure
+				NdisMoveMemory(&pAd->StaCfg.DLSEntry[i], pDLS, sizeof(RT_802_11_DLS_UI));
+
+				//Update countdown timer
+				pAd->StaCfg.DLSEntry[i].CountDownTimer = pAd->StaCfg.DLSEntry[i].TimeOut;
+				DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+				MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_REQ, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+				DBGPRINT(RT_DEBUG_TRACE,("CNTL - DLS setup case\n"));
+				break;
+			}
+			else if ((i < MAX_NUM_OF_DLS_ENTRY) && pDLS->Valid && pAd->StaCfg.DLSEntry[i].Valid &&
+				(pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH) && !MAC_ADDR_EQUAL(pDLS->MacAddr, pAd->StaCfg.DLSEntry[i].MacAddr))
+			{
+				// 4. update mac case, tear down old DLS and setup new DLS
+				reason = REASON_QOS_UNWANTED_MECHANISM;
+				pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+				pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+				DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+				MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_TEAR_DOWN, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+				NdisMoveMemory(&pAd->StaCfg.DLSEntry[i], pDLS, sizeof(RT_802_11_DLS_UI));
+				DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+				MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_REQ, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+				DBGPRINT(RT_DEBUG_TRACE,("CNTL - DLS tear down and restart case\n"));
+				break;
+			}
+			else if (pDLS->Valid && pAd->StaCfg.DLSEntry[i].Valid &&
+				MAC_ADDR_EQUAL(pDLS->MacAddr, pAd->StaCfg.DLSEntry[i].MacAddr) && (pAd->StaCfg.DLSEntry[i].TimeOut != pDLS->TimeOut))
+			{
+				// 5. update timeout case, start DLS setup procedure (no tear down)
+				pAd->StaCfg.DLSEntry[i].TimeOut	= pDLS->TimeOut;
+				//Update countdown timer
+				pAd->StaCfg.DLSEntry[i].CountDownTimer = pAd->StaCfg.DLSEntry[i].TimeOut;
+				DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+				MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_REQ, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+				DBGPRINT(RT_DEBUG_TRACE,("CNTL - DLS update timeout case\n"));
+				break;
+			}
+			else if (pDLS->Valid && pAd->StaCfg.DLSEntry[i].Valid &&
+				(pAd->StaCfg.DLSEntry[i].Status != DLS_FINISH) && MAC_ADDR_EQUAL(pDLS->MacAddr, pAd->StaCfg.DLSEntry[i].MacAddr))
+			{
+				// 6. re-setup case, start DLS setup procedure (no tear down)
+				DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+				MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_REQ, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+				DBGPRINT(RT_DEBUG_TRACE,("CNTL - DLS retry setup procedure\n"));
+				break;
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_WARN,("CNTL - DLS not changed in entry - %d - Valid=%d, Status=%d, TimeOut=%d\n",
+					i, pAd->StaCfg.DLSEntry[i].Valid, pAd->StaCfg.DLSEntry[i].Status, pAd->StaCfg.DLSEntry[i].TimeOut));
+			}
+		}
+	}
+}
+#endif // QOS_DLS_SUPPORT //
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlWaitDisassocProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	MLME_START_REQ_STRUCT     StartReq;
+
+	if (Elem->MsgType == MT2_DISASSOC_CONF)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("CNTL - Dis-associate successful\n"));
+
+	    if (pAd->CommonCfg.bWirelessEvent)
+		{
+			RTMPSendWirelessEvent(pAd, IW_DISASSOC_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+		}
+
+		LinkDown(pAd, FALSE);
+
+		// case 1. no matching BSS, and user wants ADHOC, so we just start a new one
+		if ((pAd->MlmeAux.SsidBssTab.BssNr==0) && (pAd->StaCfg.BssType == BSS_ADHOC))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - No matching BSS, start a new ADHOC (Ssid=%s)...\n",pAd->MlmeAux.Ssid));
+			StartParmFill(pAd, &StartReq, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen);
+			MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_START_REQ, sizeof(MLME_START_REQ_STRUCT), &StartReq);
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_START;
+		}
+		// case 2. try each matched BSS
+		else
+		{
+			pAd->MlmeAux.BssIdx = 0;
+
+			IterateOnBssTab(pAd);
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlWaitJoinProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT                      Reason;
+	MLME_AUTH_REQ_STRUCT        AuthReq;
+
+	if (Elem->MsgType == MT2_JOIN_CONF)
+	{
+		NdisMoveMemory(&Reason, Elem->Msg, sizeof(USHORT));
+		if (Reason == MLME_SUCCESS)
+		{
+			// 1. joined an IBSS, we are pretty much done here
+			if (pAd->MlmeAux.BssType == BSS_ADHOC)
+			{
+			    //
+				// 5G bands rules of Japan:
+				// Ad hoc must be disabled in W53(ch52,56,60,64) channels.
+				//
+				if ( (pAd->CommonCfg.bIEEE80211H == 1) &&
+                      RadarChannelCheck(pAd, pAd->CommonCfg.Channel)
+				   )
+				{
+					pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+					DBGPRINT(RT_DEBUG_TRACE, ("CNTL - Channel=%d, Join adhoc on W53(52,56,60,64) Channels are not accepted\n", pAd->CommonCfg.Channel));
+					return;
+				}
+
+				LinkUp(pAd, BSS_ADHOC);
+				pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+				DBGPRINT(RT_DEBUG_TRACE, ("CNTL - join the IBSS = %02x:%02x:%02x:%02x:%02x:%02x ...\n",
+				pAd->CommonCfg.Bssid[0],pAd->CommonCfg.Bssid[1],pAd->CommonCfg.Bssid[2],
+				pAd->CommonCfg.Bssid[3],pAd->CommonCfg.Bssid[4],pAd->CommonCfg.Bssid[5]));
+
+                pAd->IndicateMediaState = NdisMediaStateConnected;
+                pAd->ExtraInfo = GENERAL_LINK_UP;
+			}
+			// 2. joined a new INFRA network, start from authentication
+			else
+			{
+#ifdef LEAP_SUPPORT
+				// Add AuthMode "LEAP" for CCX 1.X
+				if (pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP)
+				{
+					AuthParmFill(pAd, &AuthReq, pAd->MlmeAux.Bssid, CISCO_AuthModeLEAP);
+				}
+				else
+#endif // LEAP_SUPPORT //
+				{
+					// either Ndis802_11AuthModeShared or Ndis802_11AuthModeAutoSwitch, try shared key first
+					if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeShared) ||
+						(pAd->StaCfg.AuthMode == Ndis802_11AuthModeAutoSwitch))
+					{
+						AuthParmFill(pAd, &AuthReq, pAd->MlmeAux.Bssid, Ndis802_11AuthModeShared);
+					}
+					else
+					{
+						AuthParmFill(pAd, &AuthReq, pAd->MlmeAux.Bssid, Ndis802_11AuthModeOpen);
+					}
+				}
+				MlmeEnqueue(pAd, AUTH_STATE_MACHINE, MT2_MLME_AUTH_REQ,
+							sizeof(MLME_AUTH_REQ_STRUCT), &AuthReq);
+
+				pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_AUTH;
+			}
+		}
+		else
+		{
+			// 3. failed, try next BSS
+			pAd->MlmeAux.BssIdx++;
+			IterateOnBssTab(pAd);
+		}
+	}
+}
+
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlWaitStartProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT      Result;
+
+	if (Elem->MsgType == MT2_START_CONF)
+	{
+		NdisMoveMemory(&Result, Elem->Msg, sizeof(USHORT));
+		if (Result == MLME_SUCCESS)
+		{
+		    //
+			// 5G bands rules of Japan:
+			// Ad hoc must be disabled in W53(ch52,56,60,64) channels.
+			//
+			if ( (pAd->CommonCfg.bIEEE80211H == 1) &&
+                  RadarChannelCheck(pAd, pAd->CommonCfg.Channel)
+			   )
+			{
+				pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+				DBGPRINT(RT_DEBUG_TRACE, ("CNTL - Channel=%d, Start adhoc on W53(52,56,60,64) Channels are not accepted\n", pAd->CommonCfg.Channel));
+				return;
+			}
+#ifdef DOT11_N_SUPPORT
+			if (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED)
+			{
+				N_ChannelCheck(pAd);
+				SetCommonHT(pAd);
+				NdisMoveMemory(&pAd->MlmeAux.AddHtInfo, &pAd->CommonCfg.AddHTInfo, sizeof(ADD_HT_INFO_IE));
+				RTMPCheckHt(pAd, BSSID_WCID, &pAd->CommonCfg.HtCapability, &pAd->CommonCfg.AddHTInfo);
+				pAd->StaActive.SupportedPhyInfo.bHtEnable = TRUE;
+				NdisZeroMemory(&pAd->StaActive.SupportedPhyInfo.MCSSet[0], 16);
+				NdisMoveMemory(&pAd->StaActive.SupportedPhyInfo.MCSSet[0], &pAd->CommonCfg.HtCapability.MCSSet[0], 16);
+				COPY_HTSETTINGS_FROM_MLME_AUX_TO_ACTIVE_CFG(pAd);
+
+				if ((pAd->CommonCfg.HtCapability.HtCapInfo.ChannelWidth  == BW_40) &&
+					(pAd->CommonCfg.AddHTInfo.AddHtInfo.ExtChanOffset == EXTCHA_ABOVE))
+				{
+					pAd->MlmeAux.CentralChannel = pAd->CommonCfg.Channel + 2;
+				}
+				else if ((pAd->CommonCfg.HtCapability.HtCapInfo.ChannelWidth  == BW_40) &&
+						 (pAd->CommonCfg.AddHTInfo.AddHtInfo.ExtChanOffset == EXTCHA_BELOW))
+				{
+					pAd->MlmeAux.CentralChannel = pAd->CommonCfg.Channel - 2;
+				}
+			}
+			else
+#endif // DOT11_N_SUPPORT //
+			{
+				pAd->StaActive.SupportedPhyInfo.bHtEnable = FALSE;
+			}
+			LinkUp(pAd, BSS_ADHOC);
+			pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+			// Before send beacon, driver need do radar detection
+			if ((pAd->CommonCfg.Channel > 14 )
+				&& (pAd->CommonCfg.bIEEE80211H == 1)
+				&& RadarChannelCheck(pAd, pAd->CommonCfg.Channel))
+			{
+				pAd->CommonCfg.RadarDetect.RDMode = RD_SILENCE_MODE;
+				pAd->CommonCfg.RadarDetect.RDCount = 0;
+#ifdef DFS_SUPPORT
+				BbpRadarDetectionStart(pAd);
+#endif // DFS_SUPPORT //
+			}
+
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - start a new IBSS = %02x:%02x:%02x:%02x:%02x:%02x ...\n",
+				pAd->CommonCfg.Bssid[0],pAd->CommonCfg.Bssid[1],pAd->CommonCfg.Bssid[2],
+				pAd->CommonCfg.Bssid[3],pAd->CommonCfg.Bssid[4],pAd->CommonCfg.Bssid[5]));
+		}
+		else
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - Start IBSS fail. BUG!!!!!\n"));
+			pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlWaitAuthProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT                       Reason;
+	MLME_ASSOC_REQ_STRUCT        AssocReq;
+	MLME_AUTH_REQ_STRUCT         AuthReq;
+
+	if (Elem->MsgType == MT2_AUTH_CONF)
+	{
+		NdisMoveMemory(&Reason, Elem->Msg, sizeof(USHORT));
+		if (Reason == MLME_SUCCESS)
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - AUTH OK\n"));
+			AssocParmFill(pAd, &AssocReq, pAd->MlmeAux.Bssid, pAd->MlmeAux.CapabilityInfo,
+						  ASSOC_TIMEOUT, pAd->StaCfg.DefaultListenCount);
+
+#ifdef LEAP_SUPPORT
+			//
+			// Cisco Leap CCKM supported Re-association.
+			//
+			if (LEAP_CCKM_ON(pAd) && (pAd->StaCfg.CCKMLinkUpFlag == TRUE))
+			{
+				//if CCKM is turn on , that's mean Fast Reauthentication
+				//Use CCKM Reassociation instead of normal association for Fast Roaming.
+				MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_REASSOC_REQ,
+							sizeof(MLME_ASSOC_REQ_STRUCT), &AssocReq);
+
+				pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_REASSOC;
+			}
+			else
+#endif // LEAP_SUPPORT //
+			{
+				MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_ASSOC_REQ,
+							sizeof(MLME_ASSOC_REQ_STRUCT), &AssocReq);
+
+				pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_ASSOC;
+			}
+		}
+		else
+		{
+			// This fail may because of the AP already keep us in its MAC table without
+			// ageing-out. The previous authentication attempt must have let it remove us.
+			// so try Authentication again may help. For D-Link DWL-900AP+ compatibility.
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - AUTH FAIL, try again...\n"));
+#ifdef LEAP_SUPPORT
+			//Add AuthMode "LEAP" for CCX 1.X
+			if (pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP)
+			{
+				AuthParmFill(pAd, &AuthReq, pAd->MlmeAux.Bssid, CISCO_AuthModeLEAP);
+			}
+			else
+#endif // LEAP_SUPPORT //
+			{
+				if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeShared) ||
+					(pAd->StaCfg.AuthMode == Ndis802_11AuthModeAutoSwitch))
+				{
+					// either Ndis802_11AuthModeShared or Ndis802_11AuthModeAutoSwitch, try shared key first
+					AuthParmFill(pAd, &AuthReq, pAd->MlmeAux.Bssid, Ndis802_11AuthModeShared);
+				}
+				else
+				{
+					AuthParmFill(pAd, &AuthReq, pAd->MlmeAux.Bssid, Ndis802_11AuthModeOpen);
+				}
+			}
+			MlmeEnqueue(pAd, AUTH_STATE_MACHINE, MT2_MLME_AUTH_REQ,
+						sizeof(MLME_AUTH_REQ_STRUCT), &AuthReq);
+
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_AUTH2;
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlWaitAuthProc2(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT                       Reason;
+	MLME_ASSOC_REQ_STRUCT        AssocReq;
+	MLME_AUTH_REQ_STRUCT         AuthReq;
+
+	if (Elem->MsgType == MT2_AUTH_CONF)
+	{
+		NdisMoveMemory(&Reason, Elem->Msg, sizeof(USHORT));
+		if (Reason == MLME_SUCCESS)
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - AUTH OK\n"));
+			AssocParmFill(pAd, &AssocReq, pAd->MlmeAux.Bssid, pAd->MlmeAux.CapabilityInfo,
+						  ASSOC_TIMEOUT, pAd->StaCfg.DefaultListenCount);
+			MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_ASSOC_REQ,
+						sizeof(MLME_ASSOC_REQ_STRUCT), &AssocReq);
+
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_ASSOC;
+		}
+		else
+		{
+#ifdef LEAP_SUPPORT
+			// Process LEAP first, since it use different control variable
+			// We don't want to affect other poven operation
+			if (pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP)
+			{
+				// LEAP Auth not success, try next BSS
+				DBGPRINT(RT_DEBUG_TRACE, ("CNTL - *LEAP* AUTH FAIL, give up; try next BSS\n"));
+				DBGPRINT(RT_DEBUG_TRACE, ("Total match BSSID [=%d]\n", pAd->MlmeAux.SsidBssTab.BssNr));
+				pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+				pAd->MlmeAux.BssIdx++;
+				IterateOnBssTab(pAd);
+			}
+			else
+#endif // LEAP_SUPPORT //
+			if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeAutoSwitch) &&
+				 (pAd->MlmeAux.Alg == Ndis802_11AuthModeShared))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("CNTL - AUTH FAIL, try OPEN system...\n"));
+				AuthParmFill(pAd, &AuthReq, pAd->MlmeAux.Bssid, Ndis802_11AuthModeOpen);
+				MlmeEnqueue(pAd, AUTH_STATE_MACHINE, MT2_MLME_AUTH_REQ,
+							sizeof(MLME_AUTH_REQ_STRUCT), &AuthReq);
+
+				pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_AUTH2;
+			}
+			else
+			{
+				// not success, try next BSS
+				DBGPRINT(RT_DEBUG_TRACE, ("CNTL - AUTH FAIL, give up; try next BSS\n"));
+				pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE; //???????
+				pAd->MlmeAux.BssIdx++;
+				IterateOnBssTab(pAd);
+			}
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlWaitAssocProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT      Reason;
+
+	if (Elem->MsgType == MT2_ASSOC_CONF)
+	{
+		NdisMoveMemory(&Reason, Elem->Msg, sizeof(USHORT));
+		if (Reason == MLME_SUCCESS)
+		{
+			LinkUp(pAd, BSS_INFRA);
+			pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - Association successful on BSS #%ld\n",pAd->MlmeAux.BssIdx));
+
+			if (pAd->CommonCfg.bWirelessEvent)
+			{
+				RTMPSendWirelessEvent(pAd, IW_ASSOC_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+			}
+		}
+		else
+		{
+			// not success, try next BSS
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - Association fails on BSS #%ld\n",pAd->MlmeAux.BssIdx));
+			pAd->MlmeAux.BssIdx++;
+			IterateOnBssTab(pAd);
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID CntlWaitReassocProc(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT      Result;
+
+	if (Elem->MsgType == MT2_REASSOC_CONF)
+	{
+		NdisMoveMemory(&Result, Elem->Msg, sizeof(USHORT));
+		if (Result == MLME_SUCCESS)
+		{
+			//
+			// NDIS requires a new Link UP indication but no Link Down for RE-ASSOC
+			//
+			LinkUp(pAd, BSS_INFRA);
+
+			// send wireless event - for association
+			if (pAd->CommonCfg.bWirelessEvent)
+				RTMPSendWirelessEvent(pAd, IW_ASSOC_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+
+
+#ifdef LEAP_SUPPORT
+			if (LEAP_CCKM_ON(pAd))
+			{
+				STA_PORT_SECURED(pAd);
+				pAd->StaCfg.WpaState = SS_FINISH;
+			}
+#endif // LEAP_SUPPORT //
+			pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - Re-assocition successful on BSS #%ld\n", pAd->MlmeAux.RoamIdx));
+		}
+		else
+		{
+			// reassoc failed, try to pick next BSS in the BSS Table
+			DBGPRINT(RT_DEBUG_TRACE, ("CNTL - Re-assocition fails on BSS #%ld\n", pAd->MlmeAux.RoamIdx));
+			pAd->MlmeAux.RoamIdx++;
+			IterateOnBssTab2(pAd);
+		}
+	}
+}
+
+
+VOID	AdhocTurnOnQos(
+	IN  PRTMP_ADAPTER pAd)
+{
+#define AC0_DEF_TXOP		0
+#define AC1_DEF_TXOP		0
+#define AC2_DEF_TXOP		94
+#define AC3_DEF_TXOP		47
+
+	// Turn on QOs if use HT rate.
+	if (pAd->CommonCfg.APEdcaParm.bValid == FALSE)
+	{
+		pAd->CommonCfg.APEdcaParm.bValid = TRUE;
+		pAd->CommonCfg.APEdcaParm.Aifsn[0] = 3;
+		pAd->CommonCfg.APEdcaParm.Aifsn[1] = 7;
+		pAd->CommonCfg.APEdcaParm.Aifsn[2] = 1;
+		pAd->CommonCfg.APEdcaParm.Aifsn[3] = 1;
+
+		pAd->CommonCfg.APEdcaParm.Cwmin[0] = 4;
+		pAd->CommonCfg.APEdcaParm.Cwmin[1] = 4;
+		pAd->CommonCfg.APEdcaParm.Cwmin[2] = 3;
+		pAd->CommonCfg.APEdcaParm.Cwmin[3] = 2;
+
+		pAd->CommonCfg.APEdcaParm.Cwmax[0] = 10;
+		pAd->CommonCfg.APEdcaParm.Cwmax[1] = 6;
+		pAd->CommonCfg.APEdcaParm.Cwmax[2] = 4;
+		pAd->CommonCfg.APEdcaParm.Cwmax[3] = 3;
+
+		pAd->CommonCfg.APEdcaParm.Txop[0]  = 0;
+		pAd->CommonCfg.APEdcaParm.Txop[1]  = 0;
+		pAd->CommonCfg.APEdcaParm.Txop[2]  = AC2_DEF_TXOP;
+		pAd->CommonCfg.APEdcaParm.Txop[3]  = AC3_DEF_TXOP;
+	}
+	AsicSetEdcaParm(pAd, &pAd->CommonCfg.APEdcaParm);
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID LinkUp(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR BssType)
+{
+	ULONG	Now;
+	UINT32	Data;
+	BOOLEAN	Cancelled;
+	UCHAR	Value = 0, idx;
+	MAC_TABLE_ENTRY *pEntry = NULL, *pCurrEntry;
+
+	pEntry = &pAd->MacTab.Content[BSSID_WCID];
+
+	//
+	// ASSOC - DisassocTimeoutAction
+	// CNTL - Dis-associate successful
+	// !!! LINK DOWN !!!
+	// [88888] OID_802_11_SSID should have returned NDTEST_WEP_AP2(Returned: )
+	//
+	// To prevent DisassocTimeoutAction to call Link down after we link up,
+	// cancel the DisassocTimer no matter what it start or not.
+	//
+	RTMPCancelTimer(&pAd->MlmeAux.DisassocTimer,  &Cancelled);
+
+	COPY_SETTINGS_FROM_MLME_AUX_TO_ACTIVE_CFG(pAd);
+
+#ifdef DOT11_N_SUPPORT
+	COPY_HTSETTINGS_FROM_MLME_AUX_TO_ACTIVE_CFG(pAd);
+#endif // DOT11_N_SUPPORT //
+	// It's quite difficult to tell if a newly added KEY is WEP or CKIP until a new BSS
+	// is formed (either ASSOC/RE-ASSOC done or IBSS started. LinkUP should be a safe place
+	// to examine if cipher algorithm switching is required.
+	//rt2860b. Don't know why need this
+	SwitchBetweenWepAndCkip(pAd);
+
+
+	if (BssType == BSS_ADHOC)
+	{
+		OPSTATUS_SET_FLAG(pAd, fOP_STATUS_ADHOC_ON);
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_INFRA_ON);
+
+#ifdef CARRIER_DETECTION_SUPPORT // Roger sync Carrier
+		// No carrier detection when adhoc
+		// CarrierDetectionStop(pAd);
+		pAd->CommonCfg.CarrierDetect.CD_State = CD_NORMAL;
+#endif // CARRIER_DETECTION_SUPPORT //
+
+#ifdef DOT11_N_SUPPORT
+		if (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED)
+			AdhocTurnOnQos(pAd);
+#endif // DOT11_N_SUPPORT //
+
+		DBGPRINT(RT_DEBUG_TRACE, ("!!!Adhoc LINK UP !!! \n" ));
+	}
+	else
+	{
+		OPSTATUS_SET_FLAG(pAd, fOP_STATUS_INFRA_ON);
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_ADHOC_ON);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("!!!Infra LINK UP !!! \n" ));
+	}
+
+	// 3*3
+	// reset Tx beamforming bit
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &Value);
+	Value &= (~0x01);
+	Value |= pAd->CommonCfg.RegTransmitSetting.field.TxBF;
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, Value);
+
+#ifdef DOT11_N_SUPPORT
+	// Change to AP channel
+    if ((pAd->CommonCfg.CentralChannel > pAd->CommonCfg.Channel) && (pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth == BW_40))
+	{
+		// Must using 40MHz.
+		pAd->CommonCfg.BBPCurrentBW = BW_40;
+		AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+		AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &Value);
+		Value &= (~0x18);
+		Value |= 0x10;
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, Value);
+
+		//  RX : control channel at lower
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &Value);
+		Value &= (~0x20);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, Value);
+
+		RTMP_IO_READ32(pAd, TX_BAND_CFG, &Data);
+		Data &= 0xfffffffe;
+		RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Data);
+
+		if (pAd->MACVersion == 0x28600100)
+		{
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, 0x1A);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, 0x0A);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R73, 0x16);
+            DBGPRINT(RT_DEBUG_TRACE, ("!!!rt2860C !!! \n" ));
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("!!!40MHz Lower LINK UP !!! Control Channel at Below. Central = %d \n", pAd->CommonCfg.CentralChannel ));
+	}
+	else if ((pAd->CommonCfg.CentralChannel < pAd->CommonCfg.Channel) && (pAd->MlmeAux.HtCapability.HtCapInfo.ChannelWidth == BW_40))
+    {
+	    // Must using 40MHz.
+		pAd->CommonCfg.BBPCurrentBW = BW_40;
+		AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+	    AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &Value);
+		Value &= (~0x18);
+		Value |= 0x10;
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, Value);
+
+		RTMP_IO_READ32(pAd, TX_BAND_CFG, &Data);
+		Data |= 0x1;
+		RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Data);
+
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &Value);
+	    Value |= (0x20);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, Value);
+
+		if (pAd->MACVersion == 0x28600100)
+		{
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, 0x1A);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, 0x0A);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R73, 0x16);
+			    DBGPRINT(RT_DEBUG_TRACE, ("!!!rt2860C !!! \n" ));
+		}
+
+	    DBGPRINT(RT_DEBUG_TRACE, ("!!! 40MHz Upper LINK UP !!! Control Channel at UpperCentral = %d \n", pAd->CommonCfg.CentralChannel ));
+    }
+    else
+#endif // DOT11_N_SUPPORT //
+    {
+	    pAd->CommonCfg.BBPCurrentBW = BW_20;
+		pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel;
+		AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+		AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &Value);
+		Value &= (~0x18);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, Value);
+
+		RTMP_IO_READ32(pAd, TX_BAND_CFG, &Data);
+		Data &= 0xfffffffe;
+		RTMP_IO_WRITE32(pAd, TX_BAND_CFG, Data);
+
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R3, &Value);
+		Value &= (~0x20);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R3, Value);
+
+		if (pAd->MACVersion == 0x28600100)
+		{
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R69, 0x16);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R70, 0x08);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R73, 0x11);
+            DBGPRINT(RT_DEBUG_TRACE, ("!!!rt2860C !!! \n" ));
+		}
+
+	    DBGPRINT(RT_DEBUG_TRACE, ("!!! 20MHz LINK UP !!! \n" ));
+    }
+
+	RTMPSetAGCInitValue(pAd, pAd->CommonCfg.BBPCurrentBW);
+	//
+	// Save BBP_R66 value, it will be used in RTUSBResumeMsduTransmission
+	//
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R66, &pAd->BbpTuning.R66CurrentValue);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("!!! LINK UP !!! (BssType=%d, AID=%d, ssid=%s, Channel=%d, CentralChannel = %d)\n",
+		BssType, pAd->StaActive.Aid, pAd->CommonCfg.Ssid, pAd->CommonCfg.Channel, pAd->CommonCfg.CentralChannel));
+
+#ifdef DOT11_N_SUPPORT
+	DBGPRINT(RT_DEBUG_TRACE, ("!!! LINK UP !!! (Density =%d, )\n", pAd->MacTab.Content[BSSID_WCID].MpduDensity));
+#endif // DOT11_N_SUPPORT //
+
+		AsicSetBssid(pAd, pAd->CommonCfg.Bssid);
+
+	AsicSetSlotTime(pAd, TRUE);
+	AsicSetEdcaParm(pAd, &pAd->CommonCfg.APEdcaParm);
+
+	// Call this for RTS protectionfor legacy rate, we will always enable RTS threshold, but normally it will not hit
+	AsicUpdateProtect(pAd, 0, (OFDMSETPROTECT | CCKSETPROTECT), TRUE, FALSE);
+
+#ifdef DOT11_N_SUPPORT
+	if ((pAd->StaActive.SupportedPhyInfo.bHtEnable == TRUE))
+	{
+		// Update HT protectionfor based on AP's operating mode.
+    	if (pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent == 1)
+    	{
+    		AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode,  ALLN_SETPROTECT, FALSE, TRUE);
+    	}
+    	else
+   			AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode,  ALLN_SETPROTECT, FALSE, FALSE);
+	}
+#endif // DOT11_N_SUPPORT //
+
+	NdisZeroMemory(&pAd->DrsCounters, sizeof(COUNTER_DRS));
+
+	NdisGetSystemUpTime(&Now);
+	pAd->StaCfg.LastBeaconRxTime = Now;   // last RX timestamp
+
+	if ((pAd->CommonCfg.TxPreamble != Rt802_11PreambleLong) &&
+		CAP_IS_SHORT_PREAMBLE_ON(pAd->StaActive.CapabilityInfo))
+	{
+		MlmeSetTxPreamble(pAd, Rt802_11PreambleShort);
+	}
+
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED);
+
+	if (pAd->CommonCfg.RadarDetect.RDMode == RD_SILENCE_MODE)
+	{
+#ifdef DFS_SUPPORT
+		RadarDetectionStop(pAd);
+#endif // DFS_SUPPORT //
+	}
+	pAd->CommonCfg.RadarDetect.RDMode = RD_NORMAL_MODE;
+
+	if (BssType == BSS_ADHOC)
+	{
+		MakeIbssBeacon(pAd);
+		if ((pAd->CommonCfg.Channel > 14)
+			&& (pAd->CommonCfg.bIEEE80211H == 1)
+			&& RadarChannelCheck(pAd, pAd->CommonCfg.Channel))
+		{
+			; //Do nothing
+		}
+		else
+		{
+			AsicEnableIbssSync(pAd);
+		}
+
+		// In ad hoc mode, use MAC table from index 1.
+		// p.s ASIC use all 0xff as termination of WCID table search.To prevent it's 0xff-ff-ff-ff-ff-ff, Write 0 here.
+		RTMP_IO_WRITE32(pAd, MAC_WCID_BASE, 0x00);
+		RTMP_IO_WRITE32(pAd, 0x1808, 0x00);
+
+		// If WEP is enabled, add key material and cipherAlg into Asic
+		// Fill in Shared Key Table(offset: 0x6c00) and Shared Key Mode(offset: 0x7000)
+
+		if (pAd->StaCfg.WepStatus == Ndis802_11WEPEnabled)
+		{
+			PUCHAR	Key;
+			UCHAR 	CipherAlg;
+
+			for (idx=0; idx < SHARE_KEY_NUM; idx++)
+        	{
+				CipherAlg = pAd->SharedKey[BSS0][idx].CipherAlg;
+    			Key = pAd->SharedKey[BSS0][idx].Key;
+
+				if (pAd->SharedKey[BSS0][idx].KeyLen > 0)
+				{
+					// Set key material and cipherAlg to Asic
+    				AsicAddSharedKeyEntry(pAd, BSS0, idx, CipherAlg, Key, NULL, NULL);
+
+                    if (idx == pAd->StaCfg.DefaultKeyId)
+					{
+						// Update WCID attribute table and IVEIV table for this group key table
+						RTMPAddWcidAttributeEntry(pAd, BSS0, idx, CipherAlg, NULL);
+					}
+				}
+
+
+			}
+		}
+		// If WPANone is enabled, add key material and cipherAlg into Asic
+		// Fill in Shared Key Table(offset: 0x6c00) and Shared Key Mode(offset: 0x7000)
+		else if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+		{
+			pAd->StaCfg.DefaultKeyId = 0;	// always be zero
+
+            NdisZeroMemory(&pAd->SharedKey[BSS0][0], sizeof(CIPHER_KEY));
+							pAd->SharedKey[BSS0][0].KeyLen = LEN_TKIP_EK;
+			NdisMoveMemory(pAd->SharedKey[BSS0][0].Key, pAd->StaCfg.PMK, LEN_TKIP_EK);
+
+            if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
+            {
+    			NdisMoveMemory(pAd->SharedKey[BSS0][0].RxMic, &pAd->StaCfg.PMK[16], LEN_TKIP_RXMICK);
+    			NdisMoveMemory(pAd->SharedKey[BSS0][0].TxMic, &pAd->StaCfg.PMK[16], LEN_TKIP_TXMICK);
+            }
+
+			// Decide its ChiperAlg
+			if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
+				pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_TKIP;
+			else if (pAd->StaCfg.PairCipher == Ndis802_11Encryption3Enabled)
+				pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_AES;
+			else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Unknow Cipher (=%d), set Cipher to AES\n", pAd->StaCfg.PairCipher));
+				pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_AES;
+            }
+
+			// Set key material and cipherAlg to Asic
+			AsicAddSharedKeyEntry(pAd,
+								  BSS0,
+								  0,
+								  pAd->SharedKey[BSS0][0].CipherAlg,
+								  pAd->SharedKey[BSS0][0].Key,
+								  pAd->SharedKey[BSS0][0].TxMic,
+								  pAd->SharedKey[BSS0][0].RxMic);
+
+            // Update WCID attribute table and IVEIV table for this group key table
+			RTMPAddWcidAttributeEntry(pAd, BSS0, 0, pAd->SharedKey[BSS0][0].CipherAlg, NULL);
+
+		}
+
+	}
+	else // BSS_INFRA
+	{
+		// Check the new SSID with last SSID
+		while (Cancelled == TRUE)
+		{
+			if (pAd->CommonCfg.LastSsidLen == pAd->CommonCfg.SsidLen)
+			{
+				if (RTMPCompareMemory(pAd->CommonCfg.LastSsid, pAd->CommonCfg.Ssid, pAd->CommonCfg.LastSsidLen) == 0)
+				{
+					// Link to the old one no linkdown is required.
+					break;
+				}
+			}
+			// Send link down event before set to link up
+			pAd->IndicateMediaState = NdisMediaStateDisconnected;
+			RTMP_IndicateMediaState(pAd);
+            pAd->ExtraInfo = GENERAL_LINK_DOWN;
+			DBGPRINT(RT_DEBUG_TRACE, ("NDIS_STATUS_MEDIA_DISCONNECT Event AA!\n"));
+			break;
+		}
+
+		//
+		// On WPA mode, Remove All Keys if not connect to the last BSSID
+		// Key will be set after 4-way handshake.
+		//
+		if ((pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA))
+		{
+			ULONG 		IV;
+
+			// Remove all WPA keys
+			RTMPWPARemoveAllKeys(pAd);
+			pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+			pAd->StaCfg.PrivacyFilter = Ndis802_11PrivFilter8021xWEP;
+
+			// Fixed connection failed with Range Maximizer - 515 AP (Marvell Chip) when security is WPAPSK/TKIP
+			// If IV related values are too large in GroupMsg2, AP would ignore this message.
+			IV = 0;
+			IV |= (pAd->StaCfg.DefaultKeyId << 30);
+			AsicUpdateWCIDIVEIV(pAd, BSSID_WCID, IV, 0);
+		}
+		// NOTE:
+		// the decision of using "short slot time" or not may change dynamically due to
+		// new STA association to the AP. so we have to decide that upon parsing BEACON, not here
+
+		// NOTE:
+		// the decision to use "RTC/CTS" or "CTS-to-self" protection or not may change dynamically
+		// due to new STA association to the AP. so we have to decide that upon parsing BEACON, not here
+
+		ComposePsPoll(pAd);
+		ComposeNullFrame(pAd);
+
+			AsicEnableBssSync(pAd);
+
+		// Add BSSID to WCID search table
+		AsicUpdateRxWCIDTable(pAd, BSSID_WCID, pAd->CommonCfg.Bssid);
+
+		NdisAcquireSpinLock(&pAd->MacTabLock);
+		// add this BSSID entry into HASH table
+		{
+			UCHAR HashIdx;
+
+			//pEntry = &pAd->MacTab.Content[BSSID_WCID];
+			HashIdx = MAC_ADDR_HASH_INDEX(pAd->CommonCfg.Bssid);
+			if (pAd->MacTab.Hash[HashIdx] == NULL)
+			{
+				pAd->MacTab.Hash[HashIdx] = pEntry;
+			}
+			else
+			{
+				pCurrEntry = pAd->MacTab.Hash[HashIdx];
+				while (pCurrEntry->pNext != NULL)
+					pCurrEntry = pCurrEntry->pNext;
+				pCurrEntry->pNext = pEntry;
+			}
+		}
+		NdisReleaseSpinLock(&pAd->MacTabLock);
+
+
+		// If WEP is enabled, add paiewise and shared key
+#ifdef WPA_SUPPLICANT_SUPPORT
+        if (((pAd->StaCfg.WpaSupplicantUP)&&
+             (pAd->StaCfg.WepStatus == Ndis802_11WEPEnabled)&&
+             (pAd->StaCfg.PortSecured == WPA_802_1X_PORT_SECURED)) ||
+            ((pAd->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_DISABLE)&&
+              (pAd->StaCfg.WepStatus == Ndis802_11WEPEnabled)))
+#else
+		if (pAd->StaCfg.WepStatus == Ndis802_11WEPEnabled)
+#endif // WPA_SUPPLICANT_SUPPORT //
+		{
+			PUCHAR	Key;
+			UCHAR 	CipherAlg;
+
+			for (idx=0; idx < SHARE_KEY_NUM; idx++)
+        	{
+				CipherAlg = pAd->SharedKey[BSS0][idx].CipherAlg;
+    			Key = pAd->SharedKey[BSS0][idx].Key;
+
+				if (pAd->SharedKey[BSS0][idx].KeyLen > 0)
+				{
+					// Set key material and cipherAlg to Asic
+    				AsicAddSharedKeyEntry(pAd, BSS0, idx, CipherAlg, Key, NULL, NULL);
+
+					if (idx == pAd->StaCfg.DefaultKeyId)
+					{
+						// Assign group key info
+						RTMPAddWcidAttributeEntry(pAd, BSS0, idx, CipherAlg, NULL);
+
+						// Assign pairwise key info
+						RTMPAddWcidAttributeEntry(pAd, BSS0, idx, CipherAlg, pEntry);
+					}
+				}
+			}
+		}
+
+		// only INFRASTRUCTURE mode need to indicate connectivity immediately; ADHOC mode
+		// should wait until at least 2 active nodes in this BSSID.
+		OPSTATUS_SET_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED);
+
+        // For GUI ++
+		if (pAd->StaCfg.AuthMode < Ndis802_11AuthModeWPA)
+		{
+			pAd->IndicateMediaState = NdisMediaStateConnected;
+			pAd->ExtraInfo = GENERAL_LINK_UP;
+			RTMP_IndicateMediaState(pAd);
+		}
+        // --
+
+		// Add BSSID in my MAC Table.
+        NdisAcquireSpinLock(&pAd->MacTabLock);
+		RTMPMoveMemory(pAd->MacTab.Content[BSSID_WCID].Addr, pAd->CommonCfg.Bssid, MAC_ADDR_LEN);
+		pAd->MacTab.Content[BSSID_WCID].Aid = BSSID_WCID;
+		pAd->MacTab.Content[BSSID_WCID].pAd = pAd;
+		pAd->MacTab.Content[BSSID_WCID].ValidAsCLI = TRUE;	//Although this is bssid..still set ValidAsCl
+		pAd->MacTab.Size = 1;	// infra mode always set MACtab size =1.
+		pAd->MacTab.Content[BSSID_WCID].Sst = SST_ASSOC;
+		pAd->MacTab.Content[BSSID_WCID].AuthState = SST_ASSOC;
+		pAd->MacTab.Content[BSSID_WCID].WepStatus = pAd->StaCfg.WepStatus;
+        NdisReleaseSpinLock(&pAd->MacTabLock);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("!!! LINK UP !!!  ClientStatusFlags=%lx)\n",
+			pAd->MacTab.Content[BSSID_WCID].ClientStatusFlags));
+
+		MlmeUpdateTxRates(pAd, TRUE, BSS0);
+#ifdef DOT11_N_SUPPORT
+		MlmeUpdateHtTxRates(pAd, BSS0);
+		DBGPRINT(RT_DEBUG_TRACE, ("!!! LINK UP !! (StaActive.bHtEnable =%d, )\n", pAd->StaActive.SupportedPhyInfo.bHtEnable));
+#endif // DOT11_N_SUPPORT //
+
+		//
+		// Report Adjacent AP report.
+		//
+#ifdef LEAP_SUPPORT
+		CCXAdjacentAPReport(pAd);
+#endif // LEAP_SUPPORT //
+
+		if (pAd->CommonCfg.bAggregationCapable)
+		{
+			if ((pAd->CommonCfg.bPiggyBackCapable) && (pAd->MlmeAux.APRalinkIe & 0x00000003) == 3)
+			{
+
+				OPSTATUS_SET_FLAG(pAd, fOP_STATUS_PIGGYBACK_INUSED);
+				OPSTATUS_SET_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED);
+                RTMPSetPiggyBack(pAd, TRUE);
+				DBGPRINT(RT_DEBUG_TRACE, ("Turn on Piggy-Back\n"));
+			}
+			else if (pAd->MlmeAux.APRalinkIe & 0x00000001)
+			{
+				OPSTATUS_SET_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED);
+			}
+		}
+
+		if (pAd->MlmeAux.APRalinkIe != 0x0)
+		{
+#ifdef DOT11_N_SUPPORT
+			if (CLIENT_STATUS_TEST_FLAG(&pAd->MacTab.Content[BSSID_WCID], fCLIENT_STATUS_RDG_CAPABLE))
+			{
+				AsicEnableRDG(pAd);
+			}
+#endif // DOT11_N_SUPPORT //
+			OPSTATUS_SET_FLAG(pAd, fCLIENT_STATUS_RALINK_CHIPSET);
+			CLIENT_STATUS_SET_FLAG(&pAd->MacTab.Content[BSSID_WCID], fCLIENT_STATUS_RALINK_CHIPSET);
+		}
+		else
+		{
+			OPSTATUS_CLEAR_FLAG(pAd, fCLIENT_STATUS_RALINK_CHIPSET);
+			CLIENT_STATUS_CLEAR_FLAG(&pAd->MacTab.Content[BSSID_WCID], fCLIENT_STATUS_RALINK_CHIPSET);
+		}
+	}
+
+#ifdef DOT11_N_SUPPORT
+	DBGPRINT(RT_DEBUG_TRACE, ("NDIS_STATUS_MEDIA_CONNECT Event B!.BACapability = %x. ClientStatusFlags = %lx\n", pAd->CommonCfg.BACapability.word, pAd->MacTab.Content[BSSID_WCID].ClientStatusFlags));
+#endif // DOT11_N_SUPPORT //
+
+	// Set LED
+	RTMPSetLED(pAd, LED_LINK_UP);
+
+	pAd->Mlme.PeriodicRound = 0;
+	pAd->Mlme.OneSecPeriodicRound = 0;
+	pAd->bConfigChanged = FALSE;        // Reset config flag
+	pAd->ExtraInfo = GENERAL_LINK_UP;   // Update extra information to link is up
+
+	// Set asic auto fall back
+	{
+		PUCHAR					pTable;
+		UCHAR					TableSize = 0;
+
+		MlmeSelectTxRateTable(pAd, &pAd->MacTab.Content[BSSID_WCID], &pTable, &TableSize, &pAd->CommonCfg.TxRateIndex);
+		AsicUpdateAutoFallBackTable(pAd, pTable);
+	}
+
+	NdisAcquireSpinLock(&pAd->MacTabLock);
+    pEntry->HTPhyMode.word = pAd->StaCfg.HTPhyMode.word;
+    pEntry->MaxHTPhyMode.word = pAd->StaCfg.HTPhyMode.word;
+	if (pAd->StaCfg.bAutoTxRateSwitch == FALSE)
+	{
+		pEntry->bAutoTxRateSwitch = FALSE;
+#ifdef DOT11_N_SUPPORT
+		if (pEntry->HTPhyMode.field.MCS == 32)
+			pEntry->HTPhyMode.field.ShortGI = GI_800;
+
+		if ((pEntry->HTPhyMode.field.MCS > MCS_7) || (pEntry->HTPhyMode.field.MCS == 32))
+			pEntry->HTPhyMode.field.STBC = STBC_NONE;
+#endif // DOT11_N_SUPPORT //
+		// If the legacy mode is set, overwrite the transmit setting of this entry.
+		if (pEntry->HTPhyMode.field.MODE <= MODE_OFDM)
+			RTMPUpdateLegacyTxSetting((UCHAR)pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode, pEntry);
+	}
+	else
+		pEntry->bAutoTxRateSwitch = TRUE;
+	NdisReleaseSpinLock(&pAd->MacTabLock);
+
+	//  Let Link Status Page display first initial rate.
+	pAd->LastTxRate = (USHORT)(pEntry->HTPhyMode.word);
+	// Select DAC according to HT or Legacy
+	if (pAd->StaActive.SupportedPhyInfo.MCSSet[0] != 0x00)
+	{
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R1, &Value);
+		Value &= (~0x18);
+		if (pAd->Antenna.field.TxPath == 2)
+		{
+		    Value |= 0x10;
+		}
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R1, Value);
+	}
+	else
+	{
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R1, &Value);
+		Value &= (~0x18);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R1, Value);
+	}
+
+#ifdef DOT11_N_SUPPORT
+	if (pAd->StaActive.SupportedPhyInfo.bHtEnable == FALSE)
+	{
+	}
+	else if (pEntry->MaxRAmpduFactor == 0)
+	{
+	    // If HT AP doesn't support MaxRAmpduFactor = 1, we need to set max PSDU to 0.
+	    // Because our Init value is 1 at MACRegTable.
+		RTMP_IO_WRITE32(pAd, MAX_LEN_CFG, 0x0fff);
+	}
+#endif // DOT11_N_SUPPORT //
+
+	// Patch for Marvel AP to gain high throughput
+	// Need to set as following,
+	// 1. Set txop in register-EDCA_AC0_CFG as 0x60
+	// 2. Set EnTXWriteBackDDONE in register-WPDMA_GLO_CFG as zero
+	// 3. PBF_MAX_PCNT as 0x1F3FBF9F
+	// 4. kick per two packets when dequeue
+	//
+	// Txop can only be modified when RDG is off, WMM is disable and TxBurst is enable
+	//
+	// if 1. Legacy AP WMM on,  or 2. 11n AP, AMPDU disable.  Force turn off burst no matter what bEnableTxBurst is.
+#ifdef DOT11_N_SUPPORT
+	if (((pAd->StaActive.SupportedPhyInfo.bHtEnable == FALSE) && (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED)))
+		|| ((pAd->StaActive.SupportedPhyInfo.bHtEnable == TRUE) && (pAd->CommonCfg.BACapability.field.Policy == BA_NOTUSE)))
+	{
+		RTMP_IO_READ32(pAd, EDCA_AC0_CFG, &Data);
+		Data  &= 0xFFFFFF00;
+		RTMP_IO_WRITE32(pAd, EDCA_AC0_CFG, Data);
+
+		RTMP_IO_WRITE32(pAd, PBF_MAX_PCNT, 0x1F3F7F9F);
+		DBGPRINT(RT_DEBUG_TRACE, ("Txburst 1\n"));
+	}
+	else
+#endif // DOT11_N_SUPPORT //
+	if (pAd->CommonCfg.bEnableTxBurst)
+	{
+		RTMP_IO_READ32(pAd, EDCA_AC0_CFG, &Data);
+		Data  &= 0xFFFFFF00;
+		Data  |= 0x60;
+		RTMP_IO_WRITE32(pAd, EDCA_AC0_CFG, Data);
+		pAd->CommonCfg.IOTestParm.bNowAtherosBurstOn = TRUE;
+
+		RTMP_IO_WRITE32(pAd, PBF_MAX_PCNT, 0x1F3FBF9F);
+		DBGPRINT(RT_DEBUG_TRACE, ("Txburst 2\n"));
+	}
+	else
+	{
+		RTMP_IO_READ32(pAd, EDCA_AC0_CFG, &Data);
+		Data  &= 0xFFFFFF00;
+		RTMP_IO_WRITE32(pAd, EDCA_AC0_CFG, Data);
+
+		RTMP_IO_WRITE32(pAd, PBF_MAX_PCNT, 0x1F3F7F9F);
+		DBGPRINT(RT_DEBUG_TRACE, ("Txburst 3\n"));
+	}
+
+#ifdef DOT11_N_SUPPORT
+	// Re-check to turn on TX burst or not.
+	if ((pAd->CommonCfg.IOTestParm.bLastAtheros == TRUE) && ((STA_WEP_ON(pAd))||(STA_TKIP_ON(pAd))))
+	{
+		pAd->CommonCfg.IOTestParm.bNextDisableRxBA = TRUE;
+		if (pAd->CommonCfg.bEnableTxBurst)
+		{
+		    UINT32 MACValue = 0;
+			// Force disable  TXOP value in this case. The same action in MLMEUpdateProtect too.
+			// I didn't change PBF_MAX_PCNT setting.
+			RTMP_IO_READ32(pAd, EDCA_AC0_CFG, &MACValue);
+			MACValue  &= 0xFFFFFF00;
+			RTMP_IO_WRITE32(pAd, EDCA_AC0_CFG, MACValue);
+			pAd->CommonCfg.IOTestParm.bNowAtherosBurstOn = FALSE;
+		}
+	}
+	else
+	{
+		pAd->CommonCfg.IOTestParm.bNextDisableRxBA = FALSE;
+	}
+#endif // DOT11_N_SUPPORT //
+
+	pAd->CommonCfg.IOTestParm.bLastAtheros = FALSE;
+	COPY_MAC_ADDR(pAd->CommonCfg.LastBssid, pAd->CommonCfg.Bssid);
+	DBGPRINT(RT_DEBUG_TRACE, ("!!!pAd->bNextDisableRxBA= %d \n", pAd->CommonCfg.IOTestParm.bNextDisableRxBA));
+	// BSSID add in one MAC entry too.  Because in Tx, ASIC need to check Cipher and IV/EIV, BAbitmap
+	// Pther information in MACTab.Content[BSSID_WCID] is not necessary for driver.
+	// Note: As STA, The MACTab.Content[BSSID_WCID]. PairwiseKey and Shared Key for BSS0 are the same.
+
+    if (pAd->StaCfg.WepStatus <= Ndis802_11WEPDisabled)
+    {
+        pAd->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+		pAd->StaCfg.PrivacyFilter = Ndis802_11PrivFilterAcceptAll;
+	}
+
+	NdisAcquireSpinLock(&pAd->MacTabLock);
+	pEntry->PortSecured = pAd->StaCfg.PortSecured;
+	NdisReleaseSpinLock(&pAd->MacTabLock);
+
+    //
+	// Patch Atheros AP TX will breakdown issue.
+	// AP Model: DLink DWL-8200AP
+	//
+	if (INFRA_ON(pAd) && OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED) && STA_TKIP_ON(pAd))
+	{
+		RTMP_IO_WRITE32(pAd, RX_PARSER_CFG, 0x01);
+	}
+	else
+	{
+		RTMP_IO_WRITE32(pAd, RX_PARSER_CFG, 0x00);
+	}
+
+	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS);
+
+
+#ifdef DOT11_N_SUPPORT
+#ifdef DOT11N_DRAFT3
+	if ((pAd->CommonCfg.BACapability.field.b2040CoexistScanSup) && (pAd->CommonCfg.Channel <= 11))
+	{
+		OPSTATUS_SET_FLAG(pAd, fOP_STATUS_SCAN_2040);
+		BuildEffectedChannelList(pAd);
+	}
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+}
+
+/*
+	==========================================================================
+
+	Routine	Description:
+		Disconnect current BSSID
+
+	Arguments:
+		pAd				- Pointer to our adapter
+		IsReqFromAP		- Request from AP
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+		We need more information to know it's this requst from AP.
+		If yes! we need to do extra handling, for example, remove the WPA key.
+		Otherwise on 4-way handshaking will faied, since the WPA key didn't be
+		remove while auto reconnect.
+		Disconnect request from AP, it means we will start afresh 4-way handshaking
+		on WPA mode.
+
+	==========================================================================
+*/
+VOID LinkDown(
+	IN PRTMP_ADAPTER pAd,
+	IN  BOOLEAN      IsReqFromAP)
+{
+	UCHAR			    i, ByteValue = 0;
+
+	// Do nothing if monitor mode is on
+	if (MONITOR_ON(pAd))
+		return;
+
+#ifdef RALINK_ATE
+	// Nothing to do in ATE mode.
+	if (ATE_ON(pAd))
+		return;
+#endif // RALINK_ATE //
+
+    if (pAd->CommonCfg.bWirelessEvent)
+	{
+		RTMPSendWirelessEvent(pAd, IW_STA_LINKDOWN_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("!!! LINK DOWN !!!\n"));
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED);
+
+	if (ADHOC_ON(pAd))		// Adhoc mode link down
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("!!! LINK DOWN 1!!!\n"));
+
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_ADHOC_ON);
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED);
+		pAd->IndicateMediaState = NdisMediaStateDisconnected;
+		RTMP_IndicateMediaState(pAd);
+        pAd->ExtraInfo = GENERAL_LINK_DOWN;
+		BssTableDeleteEntry(&pAd->ScanTab, pAd->CommonCfg.Bssid, pAd->CommonCfg.Channel);
+		DBGPRINT(RT_DEBUG_TRACE, ("!!! MacTab.Size=%d !!!\n", pAd->MacTab.Size));
+	}
+	else					// Infra structure mode
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("!!! LINK DOWN 2!!!\n"));
+
+#ifdef QOS_DLS_SUPPORT
+		// DLS tear down frame must be sent before link down
+		// send DLS-TEAR_DOWN message
+		if (pAd->CommonCfg.bDLSCapable)
+		{
+			// tear down local dls table entry
+			for (i=0; i<MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+			{
+				if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+				{
+					pAd->StaCfg.DLSEntry[i].Status = DLS_NONE;
+					RTMPSendDLSTearDownFrame(pAd, pAd->StaCfg.DLSEntry[i].MacAddr);
+				}
+			}
+
+			// tear down peer dls table entry
+			for (i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+			{
+				if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status ==  DLS_FINISH))
+				{
+					pAd->StaCfg.DLSEntry[i].Status = DLS_NONE;
+					RTMPSendDLSTearDownFrame(pAd, pAd->StaCfg.DLSEntry[i].MacAddr);
+				}
+			}
+		}
+#endif // QOS_DLS_SUPPORT //
+
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_INFRA_ON);
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED);
+
+		// Saved last SSID for linkup comparison
+		pAd->CommonCfg.LastSsidLen = pAd->CommonCfg.SsidLen;
+		NdisMoveMemory(pAd->CommonCfg.LastSsid, pAd->CommonCfg.Ssid, pAd->CommonCfg.LastSsidLen);
+		COPY_MAC_ADDR(pAd->CommonCfg.LastBssid, pAd->CommonCfg.Bssid);
+		if (pAd->MlmeAux.CurrReqIsFromNdis == TRUE)
+		{
+			pAd->IndicateMediaState = NdisMediaStateDisconnected;
+			RTMP_IndicateMediaState(pAd);
+            pAd->ExtraInfo = GENERAL_LINK_DOWN;
+			DBGPRINT(RT_DEBUG_TRACE, ("NDIS_STATUS_MEDIA_DISCONNECT Event A!\n"));
+			pAd->MlmeAux.CurrReqIsFromNdis = FALSE;
+		}
+		else
+		{
+            //
+			// If disassociation request is from NDIS, then we don't need to delete BSSID from entry.
+			// Otherwise lost beacon or receive De-Authentication from AP,
+			// then we should delete BSSID from BssTable.
+			// If we don't delete from entry, roaming will fail.
+			//
+			BssTableDeleteEntry(&pAd->ScanTab, pAd->CommonCfg.Bssid, pAd->CommonCfg.Channel);
+		}
+
+		// restore back to -
+		//      1. long slot (20 us) or short slot (9 us) time
+		//      2. turn on/off RTS/CTS and/or CTS-to-self protection
+		//      3. short preamble
+		OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_BG_PROTECTION_INUSED);
+
+		if (pAd->StaCfg.CCXAdjacentAPReportFlag == TRUE)
+		{
+			//
+			// Record current AP's information.
+			// for later used reporting Adjacent AP report.
+			//
+			pAd->StaCfg.CCXAdjacentAPChannel = pAd->CommonCfg.Channel;
+			pAd->StaCfg.CCXAdjacentAPSsidLen = pAd->CommonCfg.SsidLen;
+			NdisMoveMemory(pAd->StaCfg.CCXAdjacentAPSsid, pAd->CommonCfg.Ssid, pAd->StaCfg.CCXAdjacentAPSsidLen);
+			COPY_MAC_ADDR(pAd->StaCfg.CCXAdjacentAPBssid, pAd->CommonCfg.Bssid);
+		}
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+		// Country IE of the AP will be evaluated and will be used.
+		if (pAd->StaCfg.IEEE80211dClientMode != Rt802_11_D_None)
+		{
+			NdisMoveMemory(&pAd->CommonCfg.CountryCode[0], &pAd->StaCfg.StaOriCountryCode[0], 2);
+			pAd->CommonCfg.Geography = pAd->StaCfg.StaOriGeography;
+			BuildChannelListEx(pAd);
+		}
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+	}
+
+	for (i=1; i<MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		if (pAd->MacTab.Content[i].ValidAsCLI == TRUE)
+			MacTableDeleteEntry(pAd, pAd->MacTab.Content[i].Aid, pAd->MacTab.Content[i].Addr);
+	}
+
+	pAd->StaCfg.CCXQosECWMin	= 4;
+	pAd->StaCfg.CCXQosECWMax	= 10;
+
+	AsicSetSlotTime(pAd, TRUE); //FALSE);
+	AsicSetEdcaParm(pAd, NULL);
+
+	// Set LED
+	RTMPSetLED(pAd, LED_LINK_DOWN);
+    pAd->LedIndicatorStregth = 0xF0;
+    RTMPSetSignalLED(pAd, -100);	// Force signal strength Led to be turned off, firmware is not done it.
+
+		AsicDisableSync(pAd);
+
+	pAd->Mlme.PeriodicRound = 0;
+	pAd->Mlme.OneSecPeriodicRound = 0;
+
+	if (pAd->StaCfg.BssType == BSS_INFRA)
+	{
+		// Remove StaCfg Information after link down
+		NdisZeroMemory(pAd->CommonCfg.Bssid, MAC_ADDR_LEN);
+		NdisZeroMemory(pAd->CommonCfg.Ssid, MAX_LEN_OF_SSID);
+		pAd->CommonCfg.SsidLen = 0;
+	}
+#ifdef DOT11_N_SUPPORT
+	NdisZeroMemory(&pAd->MlmeAux.HtCapability, sizeof(HT_CAPABILITY_IE));
+	NdisZeroMemory(&pAd->MlmeAux.AddHtInfo, sizeof(ADD_HT_INFO_IE));
+	pAd->MlmeAux.HtCapabilityLen = 0;
+	pAd->MlmeAux.NewExtChannelOffset = 0xff;
+#endif // DOT11_N_SUPPORT //
+
+	// Reset WPA-PSK state. Only reset when supplicant enabled
+	if (pAd->StaCfg.WpaState != SS_NOTUSE)
+	{
+		pAd->StaCfg.WpaState = SS_START;
+		// Clear Replay counter
+		NdisZeroMemory(pAd->StaCfg.ReplayCounter, 8);
+
+#ifdef QOS_DLS_SUPPORT
+		if (pAd->CommonCfg.bDLSCapable)
+			NdisZeroMemory(pAd->StaCfg.DlsReplayCounter, 8);
+#endif // QOS_DLS_SUPPORT //
+	}
+
+
+	//
+	// if link down come from AP, we need to remove all WPA keys on WPA mode.
+	// otherwise will cause 4-way handshaking failed, since the WPA key not empty.
+	//
+	if ((IsReqFromAP) && (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA))
+	{
+		// Remove all WPA keys
+		RTMPWPARemoveAllKeys(pAd);
+	}
+
+	// 802.1x port control
+#ifdef WPA_SUPPLICANT_SUPPORT
+	// Prevent clear PortSecured here with static WEP
+	// NetworkManger set security policy first then set SSID to connect AP.
+	if (pAd->StaCfg.WpaSupplicantUP &&
+		(pAd->StaCfg.WepStatus == Ndis802_11WEPEnabled) &&
+		(pAd->StaCfg.IEEE8021X == FALSE))
+	{
+		pAd->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+	}
+	else
+#endif // WPA_SUPPLICANT_SUPPORT //
+	{
+		pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+		pAd->StaCfg.PrivacyFilter = Ndis802_11PrivFilter8021xWEP;
+	}
+
+	NdisAcquireSpinLock(&pAd->MacTabLock);
+	pAd->MacTab.Content[BSSID_WCID].PortSecured = pAd->StaCfg.PortSecured;
+	NdisReleaseSpinLock(&pAd->MacTabLock);
+
+	pAd->StaCfg.MicErrCnt = 0;
+
+	// Turn off Ckip control flag
+	pAd->StaCfg.bCkipOn = FALSE;
+	pAd->StaCfg.CCXEnable = FALSE;
+
+    pAd->IndicateMediaState = NdisMediaStateDisconnected;
+	// Update extra information to link is up
+	pAd->ExtraInfo = GENERAL_LINK_DOWN;
+
+    //pAd->StaCfg.AdhocBOnlyJoined = FALSE;
+	//pAd->StaCfg.AdhocBGJoined = FALSE;
+	//pAd->StaCfg.Adhoc20NJoined = FALSE;
+    pAd->StaActive.SupportedPhyInfo.bHtEnable = FALSE;
+
+	// Reset the Current AP's IP address
+	NdisZeroMemory(pAd->StaCfg.AironetIPAddress, 4);
+#ifdef RT2870
+	pAd->bUsbTxBulkAggre = FALSE;
+#endif // RT2870 //
+
+	// Clean association information
+	NdisZeroMemory(&pAd->StaCfg.AssocInfo, sizeof(NDIS_802_11_ASSOCIATION_INFORMATION));
+	pAd->StaCfg.AssocInfo.Length = sizeof(NDIS_802_11_ASSOCIATION_INFORMATION);
+	pAd->StaCfg.ReqVarIELen = 0;
+	pAd->StaCfg.ResVarIELen = 0;
+
+	//
+	// Reset RSSI value after link down
+	//
+	pAd->StaCfg.RssiSample.AvgRssi0 = 0;
+	pAd->StaCfg.RssiSample.AvgRssi0X8 = 0;
+	pAd->StaCfg.RssiSample.AvgRssi1 = 0;
+	pAd->StaCfg.RssiSample.AvgRssi1X8 = 0;
+	pAd->StaCfg.RssiSample.AvgRssi2 = 0;
+	pAd->StaCfg.RssiSample.AvgRssi2X8 = 0;
+
+	// Restore MlmeRate
+	pAd->CommonCfg.MlmeRate = pAd->CommonCfg.BasicMlmeRate;
+	pAd->CommonCfg.RtsRate = pAd->CommonCfg.BasicMlmeRate;
+
+#ifdef DOT11_N_SUPPORT
+	//
+	// After Link down, reset piggy-back setting in ASIC. Disable RDG.
+	//
+	if (pAd->CommonCfg.BBPCurrentBW == BW_40)
+	{
+		pAd->CommonCfg.BBPCurrentBW = BW_20;
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &ByteValue);
+		ByteValue &= (~0x18);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, ByteValue);
+	}
+#endif // DOT11_N_SUPPORT //
+	// Reset DAC
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R1, &ByteValue);
+	ByteValue &= (~0x18);
+	if (pAd->Antenna.field.TxPath == 2)
+	{
+		ByteValue |= 0x10;
+	}
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R1, ByteValue);
+
+	RTMPSetPiggyBack(pAd,FALSE);
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_PIGGYBACK_INUSED);
+
+#ifdef DOT11_N_SUPPORT
+	pAd->CommonCfg.BACapability.word = pAd->CommonCfg.REGBACapability.word;
+#endif // DOT11_N_SUPPORT //
+
+	// Restore all settings in the following.
+	AsicUpdateProtect(pAd, 0, (ALLN_SETPROTECT|CCKSETPROTECT|OFDMSETPROTECT), TRUE, FALSE);
+	AsicDisableRDG(pAd);
+	pAd->CommonCfg.IOTestParm.bCurrentAtheros = FALSE;
+	pAd->CommonCfg.IOTestParm.bNowAtherosBurstOn = FALSE;
+
+#ifdef DOT11_N_SUPPORT
+#ifdef DOT11N_DRAFT3
+	OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_SCAN_2040);
+	pAd->CommonCfg.BSSCoexist2040.word = 0;
+	TriEventInit(pAd);
+	for (i = 0; i < (pAd->ChannelListNum - 1); i++)
+	{
+		pAd->ChannelList[i].bEffectedChannel = FALSE;
+	}
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+
+	RTMP_IO_WRITE32(pAd, MAX_LEN_CFG, 0x1fff);
+	RTMP_CLEAR_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS);
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#ifndef NATIVE_WPA_SUPPLICANT_SUPPORT
+	if (pAd->StaCfg.WpaSupplicantUP) {
+		union iwreq_data    wrqu;
+		//send disassociate event to wpa_supplicant
+		memset(&wrqu, 0, sizeof(wrqu));
+		wrqu.data.flags = RT_DISASSOC_EVENT_FLAG;
+		wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, NULL);
+	}
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+	{
+		union iwreq_data    wrqu;
+		memset(wrqu.ap_addr.sa_data, 0, MAC_ADDR_LEN);
+		wireless_send_event(pAd->net_dev, SIOCGIWAP, &wrqu, NULL);
+	}
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID IterateOnBssTab(
+	IN PRTMP_ADAPTER pAd)
+{
+	MLME_START_REQ_STRUCT   StartReq;
+	MLME_JOIN_REQ_STRUCT    JoinReq;
+	ULONG                   BssIdx;
+
+	// Change the wepstatus to original wepstatus
+	pAd->StaCfg.WepStatus   = pAd->StaCfg.OrigWepStatus;
+	pAd->StaCfg.PairCipher  = pAd->StaCfg.OrigWepStatus;
+	pAd->StaCfg.GroupCipher = pAd->StaCfg.OrigWepStatus;
+
+	BssIdx = pAd->MlmeAux.BssIdx;
+	if (BssIdx < pAd->MlmeAux.SsidBssTab.BssNr)
+	{
+		// Check cipher suite, AP must have more secured cipher than station setting
+		// Set the Pairwise and Group cipher to match the intended AP setting
+		// We can only connect to AP with less secured cipher setting
+		if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK))
+		{
+			pAd->StaCfg.GroupCipher = pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA.GroupCipher;
+
+			if (pAd->StaCfg.WepStatus == pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA.PairCipher)
+				pAd->StaCfg.PairCipher = pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA.PairCipher;
+			else if (pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA.PairCipherAux != Ndis802_11WEPDisabled)
+				pAd->StaCfg.PairCipher = pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA.PairCipherAux;
+			else	// There is no PairCipher Aux, downgrade our capability to TKIP
+				pAd->StaCfg.PairCipher = Ndis802_11Encryption2Enabled;
+		}
+		else if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK))
+		{
+			pAd->StaCfg.GroupCipher = pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA2.GroupCipher;
+
+			if (pAd->StaCfg.WepStatus == pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA2.PairCipher)
+				pAd->StaCfg.PairCipher = pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA2.PairCipher;
+			else if (pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA2.PairCipherAux != Ndis802_11WEPDisabled)
+				pAd->StaCfg.PairCipher = pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA2.PairCipherAux;
+			else	// There is no PairCipher Aux, downgrade our capability to TKIP
+				pAd->StaCfg.PairCipher = Ndis802_11Encryption2Enabled;
+
+			// RSN capability
+			pAd->StaCfg.RsnCapability = pAd->MlmeAux.SsidBssTab.BssEntry[BssIdx].WPA2.RsnCapability;
+		}
+
+		// Set Mix cipher flag
+		pAd->StaCfg.bMixCipher = (pAd->StaCfg.PairCipher == pAd->StaCfg.GroupCipher) ? FALSE : TRUE;
+		if (pAd->StaCfg.bMixCipher == TRUE)
+		{
+			// If mix cipher, re-build RSNIE
+			RTMPMakeRSNIE(pAd, pAd->StaCfg.AuthMode, pAd->StaCfg.WepStatus, 0);
+		}
+
+		DBGPRINT(RT_DEBUG_TRACE, ("CNTL - iterate BSS %ld of %d\n", BssIdx, pAd->MlmeAux.SsidBssTab.BssNr));
+		JoinParmFill(pAd, &JoinReq, BssIdx);
+		MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_JOIN_REQ, sizeof(MLME_JOIN_REQ_STRUCT),
+					&JoinReq);
+		pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_JOIN;
+	}
+	else if (pAd->StaCfg.BssType == BSS_ADHOC)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("CNTL - All BSS fail; start a new ADHOC (Ssid=%s)...\n",pAd->MlmeAux.Ssid));
+		StartParmFill(pAd, &StartReq, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen);
+		MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_MLME_START_REQ, sizeof(MLME_START_REQ_STRUCT), &StartReq);
+		pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_START;
+	}
+	else // no more BSS
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("CNTL - All roaming failed, stay @ ch #%d\n", pAd->CommonCfg.Channel));
+		AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+		AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+		pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+	}
+}
+
+// for re-association only
+// IRQL = DISPATCH_LEVEL
+VOID IterateOnBssTab2(
+	IN PRTMP_ADAPTER pAd)
+{
+	MLME_REASSOC_REQ_STRUCT ReassocReq;
+	ULONG                   BssIdx;
+	BSS_ENTRY               *pBss;
+
+	BssIdx = pAd->MlmeAux.RoamIdx;
+	pBss = &pAd->MlmeAux.RoamTab.BssEntry[BssIdx];
+
+	if (BssIdx < pAd->MlmeAux.RoamTab.BssNr)
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("CNTL - iterate BSS %ld of %d\n", BssIdx, pAd->MlmeAux.RoamTab.BssNr));
+
+		AsicSwitchChannel(pAd, pBss->Channel, FALSE);
+		AsicLockChannel(pAd, pBss->Channel);
+
+		// reassociate message has the same structure as associate message
+		AssocParmFill(pAd, &ReassocReq, pBss->Bssid, pBss->CapabilityInfo,
+					  ASSOC_TIMEOUT, pAd->StaCfg.DefaultListenCount);
+		MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_REASSOC_REQ,
+					sizeof(MLME_REASSOC_REQ_STRUCT), &ReassocReq);
+
+		pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_REASSOC;
+	}
+	else // no more BSS
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("CNTL - All fast roaming failed, back to ch #%d\n",pAd->CommonCfg.Channel));
+		AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+		AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+		pAd->Mlme.CntlMachine.CurrState = CNTL_IDLE;
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID JoinParmFill(
+	IN PRTMP_ADAPTER pAd,
+	IN OUT MLME_JOIN_REQ_STRUCT *JoinReq,
+	IN ULONG BssIdx)
+{
+	JoinReq->BssIdx = BssIdx;
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID ScanParmFill(
+	IN PRTMP_ADAPTER pAd,
+	IN OUT MLME_SCAN_REQ_STRUCT *ScanReq,
+	IN CHAR Ssid[],
+	IN UCHAR SsidLen,
+	IN UCHAR BssType,
+	IN UCHAR ScanType)
+{
+    NdisZeroMemory(ScanReq->Ssid, MAX_LEN_OF_SSID);
+	ScanReq->SsidLen = SsidLen;
+	NdisMoveMemory(ScanReq->Ssid, Ssid, SsidLen);
+	ScanReq->BssType = BssType;
+	ScanReq->ScanType = ScanType;
+}
+
+#ifdef QOS_DLS_SUPPORT
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID DlsParmFill(
+	IN PRTMP_ADAPTER pAd,
+	IN OUT MLME_DLS_REQ_STRUCT *pDlsReq,
+	IN PRT_802_11_DLS pDls,
+	IN USHORT reason)
+{
+	pDlsReq->pDLS = pDls;
+	pDlsReq->Reason = reason;
+}
+#endif // QOS_DLS_SUPPORT //
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID StartParmFill(
+	IN PRTMP_ADAPTER pAd,
+	IN OUT MLME_START_REQ_STRUCT *StartReq,
+	IN CHAR Ssid[],
+	IN UCHAR SsidLen)
+{
+	ASSERT(SsidLen <= MAX_LEN_OF_SSID);
+	NdisMoveMemory(StartReq->Ssid, Ssid, SsidLen);
+	StartReq->SsidLen = SsidLen;
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+VOID AuthParmFill(
+	IN PRTMP_ADAPTER pAd,
+	IN OUT MLME_AUTH_REQ_STRUCT *AuthReq,
+	IN PUCHAR pAddr,
+	IN USHORT Alg)
+{
+	COPY_MAC_ADDR(AuthReq->Addr, pAddr);
+	AuthReq->Alg = Alg;
+	AuthReq->Timeout = AUTH_TIMEOUT;
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+
+
+#ifdef RT2870
+
+VOID MlmeCntlConfirm(
+	IN PRTMP_ADAPTER pAd,
+	IN ULONG MsgType,
+	IN USHORT Msg)
+{
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MsgType, sizeof(USHORT), &Msg);
+}
+
+VOID ComposePsPoll(
+	IN PRTMP_ADAPTER pAd)
+{
+	PTXINFO_STRUC		pTxInfo;
+	PTXWI_STRUC		pTxWI;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("ComposePsPoll\n"));
+	NdisZeroMemory(&pAd->PsPollFrame, sizeof(PSPOLL_FRAME));
+
+	pAd->PsPollFrame.FC.PwrMgmt = 0;
+	pAd->PsPollFrame.FC.Type = BTYPE_CNTL;
+	pAd->PsPollFrame.FC.SubType = SUBTYPE_PS_POLL;
+	pAd->PsPollFrame.Aid = pAd->StaActive.Aid | 0xC000;
+	COPY_MAC_ADDR(pAd->PsPollFrame.Bssid, pAd->CommonCfg.Bssid);
+	COPY_MAC_ADDR(pAd->PsPollFrame.Ta, pAd->CurrentAddress);
+
+	RTMPZeroMemory(&pAd->PsPollContext.TransferBuffer->field.WirelessPacket[0], 100);
+	pTxInfo = (PTXINFO_STRUC)&pAd->PsPollContext.TransferBuffer->field.WirelessPacket[0];
+	RTMPWriteTxInfo(pAd, pTxInfo, (USHORT)(sizeof(PSPOLL_FRAME)+TXWI_SIZE), TRUE, EpToQueue[MGMTPIPEIDX], FALSE,  FALSE);
+	pTxWI = (PTXWI_STRUC)&pAd->PsPollContext.TransferBuffer->field.WirelessPacket[TXINFO_SIZE];
+	RTMPWriteTxWI(pAd, pTxWI, FALSE, FALSE, FALSE, FALSE, TRUE, FALSE, 0, BSSID_WCID, (sizeof(PSPOLL_FRAME)),
+		0,  0, (UCHAR)pAd->CommonCfg.MlmeTransmit.field.MCS, IFS_BACKOFF, FALSE, &pAd->CommonCfg.MlmeTransmit);
+	RTMPMoveMemory(&pAd->PsPollContext.TransferBuffer->field.WirelessPacket[TXWI_SIZE+TXINFO_SIZE], &pAd->PsPollFrame, sizeof(PSPOLL_FRAME));
+	// Append 4 extra zero bytes.
+	pAd->PsPollContext.BulkOutSize =  TXINFO_SIZE + TXWI_SIZE + sizeof(PSPOLL_FRAME) + 4;
+}
+
+// IRQL = DISPATCH_LEVEL
+VOID ComposeNullFrame(
+	IN PRTMP_ADAPTER pAd)
+{
+	PTXINFO_STRUC		pTxInfo;
+	PTXWI_STRUC		pTxWI;
+
+	NdisZeroMemory(&pAd->NullFrame, sizeof(HEADER_802_11));
+	pAd->NullFrame.FC.Type = BTYPE_DATA;
+	pAd->NullFrame.FC.SubType = SUBTYPE_NULL_FUNC;
+	pAd->NullFrame.FC.ToDs = 1;
+	COPY_MAC_ADDR(pAd->NullFrame.Addr1, pAd->CommonCfg.Bssid);
+	COPY_MAC_ADDR(pAd->NullFrame.Addr2, pAd->CurrentAddress);
+	COPY_MAC_ADDR(pAd->NullFrame.Addr3, pAd->CommonCfg.Bssid);
+	RTMPZeroMemory(&pAd->NullContext.TransferBuffer->field.WirelessPacket[0], 100);
+	pTxInfo = (PTXINFO_STRUC)&pAd->NullContext.TransferBuffer->field.WirelessPacket[0];
+	RTMPWriteTxInfo(pAd, pTxInfo, (USHORT)(sizeof(HEADER_802_11)+TXWI_SIZE), TRUE, EpToQueue[MGMTPIPEIDX], FALSE,  FALSE);
+	pTxWI = (PTXWI_STRUC)&pAd->NullContext.TransferBuffer->field.WirelessPacket[TXINFO_SIZE];
+	RTMPWriteTxWI(pAd, pTxWI, FALSE, FALSE, FALSE, FALSE, TRUE, FALSE, 0, BSSID_WCID, (sizeof(HEADER_802_11)),
+		0, 0, (UCHAR)pAd->CommonCfg.MlmeTransmit.field.MCS, IFS_BACKOFF, FALSE, &pAd->CommonCfg.MlmeTransmit);
+	RTMPMoveMemory(&pAd->NullContext.TransferBuffer->field.WirelessPacket[TXWI_SIZE+TXINFO_SIZE], &pAd->NullFrame, sizeof(HEADER_802_11));
+	pAd->NullContext.BulkOutSize =  TXINFO_SIZE + TXWI_SIZE + sizeof(pAd->NullFrame) + 4;
+}
+#endif // RT2870 //
+
+
+/*
+	==========================================================================
+	Description:
+		Pre-build a BEACON frame in the shared memory
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+*/
+ULONG MakeIbssBeacon(
+	IN PRTMP_ADAPTER pAd)
+{
+	UCHAR         DsLen = 1, IbssLen = 2;
+	UCHAR         LocalErpIe[3] = {IE_ERP, 1, 0x04};
+	HEADER_802_11 BcnHdr;
+	USHORT        CapabilityInfo;
+	LARGE_INTEGER FakeTimestamp;
+	ULONG         FrameLen = 0;
+	PTXWI_STRUC	  pTxWI = &pAd->BeaconTxWI;
+	CHAR         *pBeaconFrame = pAd->BeaconBuf;
+	BOOLEAN       Privacy;
+	UCHAR         SupRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR         SupRateLen = 0;
+	UCHAR         ExtRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR         ExtRateLen = 0;
+	UCHAR         RSNIe = IE_WPA;
+
+	if ((pAd->CommonCfg.PhyMode == PHY_11B) && (pAd->CommonCfg.Channel <= 14))
+	{
+		SupRate[0] = 0x82; // 1 mbps
+		SupRate[1] = 0x84; // 2 mbps
+		SupRate[2] = 0x8b; // 5.5 mbps
+		SupRate[3] = 0x96; // 11 mbps
+		SupRateLen = 4;
+		ExtRateLen = 0;
+	}
+	else if (pAd->CommonCfg.Channel > 14)
+	{
+		SupRate[0]  = 0x8C;    // 6 mbps, in units of 0.5 Mbps, basic rate
+		SupRate[1]  = 0x12;    // 9 mbps, in units of 0.5 Mbps
+		SupRate[2]  = 0x98;    // 12 mbps, in units of 0.5 Mbps, basic rate
+		SupRate[3]  = 0x24;    // 18 mbps, in units of 0.5 Mbps
+		SupRate[4]  = 0xb0;    // 24 mbps, in units of 0.5 Mbps, basic rate
+		SupRate[5]  = 0x48;    // 36 mbps, in units of 0.5 Mbps
+		SupRate[6]  = 0x60;    // 48 mbps, in units of 0.5 Mbps
+		SupRate[7]  = 0x6c;    // 54 mbps, in units of 0.5 Mbps
+		SupRateLen  = 8;
+		ExtRateLen  = 0;
+
+		//
+		// Also Update MlmeRate & RtsRate for G only & A only
+		//
+		pAd->CommonCfg.MlmeRate = RATE_6;
+		pAd->CommonCfg.RtsRate = RATE_6;
+		pAd->CommonCfg.MlmeTransmit.field.MODE = MODE_OFDM;
+		pAd->CommonCfg.MlmeTransmit.field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MlmeRate];
+		pAd->MacTab.Content[BSS0Mcast_WCID].HTPhyMode.field.MODE = MODE_OFDM;
+		pAd->MacTab.Content[BSS0Mcast_WCID].HTPhyMode.field.MCS = OfdmRateToRxwiMCS[pAd->CommonCfg.MlmeRate];
+	}
+	else
+	{
+		SupRate[0] = 0x82; // 1 mbps
+		SupRate[1] = 0x84; // 2 mbps
+		SupRate[2] = 0x8b; // 5.5 mbps
+		SupRate[3] = 0x96; // 11 mbps
+		SupRateLen = 4;
+
+		ExtRate[0]  = 0x0C;    // 6 mbps, in units of 0.5 Mbps,
+		ExtRate[1]  = 0x12;    // 9 mbps, in units of 0.5 Mbps
+		ExtRate[2]  = 0x18;    // 12 mbps, in units of 0.5 Mbps,
+		ExtRate[3]  = 0x24;    // 18 mbps, in units of 0.5 Mbps
+		ExtRate[4]  = 0x30;    // 24 mbps, in units of 0.5 Mbps,
+		ExtRate[5]  = 0x48;    // 36 mbps, in units of 0.5 Mbps
+		ExtRate[6]  = 0x60;    // 48 mbps, in units of 0.5 Mbps
+		ExtRate[7]  = 0x6c;    // 54 mbps, in units of 0.5 Mbps
+		ExtRateLen  = 8;
+	}
+
+	pAd->StaActive.SupRateLen = SupRateLen;
+	NdisMoveMemory(pAd->StaActive.SupRate, SupRate, SupRateLen);
+	pAd->StaActive.ExtRateLen = ExtRateLen;
+	NdisMoveMemory(pAd->StaActive.ExtRate, ExtRate, ExtRateLen);
+
+	// compose IBSS beacon frame
+	MgtMacHeaderInit(pAd, &BcnHdr, SUBTYPE_BEACON, 0, BROADCAST_ADDR, pAd->CommonCfg.Bssid);
+	Privacy = (pAd->StaCfg.WepStatus == Ndis802_11Encryption1Enabled) ||
+			  (pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) ||
+			  (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled);
+	CapabilityInfo = CAP_GENERATE(0, 1, Privacy, (pAd->CommonCfg.TxPreamble == Rt802_11PreambleShort), 0, 0);
+
+	MakeOutgoingFrame(pBeaconFrame,                &FrameLen,
+					  sizeof(HEADER_802_11),           &BcnHdr,
+					  TIMESTAMP_LEN,                   &FakeTimestamp,
+					  2,                               &pAd->CommonCfg.BeaconPeriod,
+					  2,                               &CapabilityInfo,
+					  1,                               &SsidIe,
+					  1,                               &pAd->CommonCfg.SsidLen,
+					  pAd->CommonCfg.SsidLen,          pAd->CommonCfg.Ssid,
+					  1,                               &SupRateIe,
+					  1,                               &SupRateLen,
+					  SupRateLen,                      SupRate,
+					  1,                               &DsIe,
+					  1,                               &DsLen,
+					  1,                               &pAd->CommonCfg.Channel,
+					  1,                               &IbssIe,
+					  1,                               &IbssLen,
+					  2,                               &pAd->StaActive.AtimWin,
+					  END_OF_ARGS);
+
+	// add ERP_IE and EXT_RAE IE of in 802.11g
+	if (ExtRateLen)
+	{
+		ULONG	tmp;
+
+		MakeOutgoingFrame(pBeaconFrame + FrameLen,         &tmp,
+						  3,                               LocalErpIe,
+						  1,                               &ExtRateIe,
+						  1,                               &ExtRateLen,
+						  ExtRateLen,                      ExtRate,
+						  END_OF_ARGS);
+		FrameLen += tmp;
+	}
+
+	// If adhoc secruity is set for WPA-None, append the cipher suite IE
+	if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+	{
+		ULONG tmp;
+        RTMPMakeRSNIE(pAd, pAd->StaCfg.AuthMode, pAd->StaCfg.WepStatus, BSS0);
+
+		MakeOutgoingFrame(pBeaconFrame + FrameLen,        	&tmp,
+						  1,                              	&RSNIe,
+						  1,                            	&pAd->StaCfg.RSNIE_Len,
+						  pAd->StaCfg.RSNIE_Len,      		pAd->StaCfg.RSN_IE,
+						  END_OF_ARGS);
+		FrameLen += tmp;
+	}
+
+#ifdef DOT11_N_SUPPORT
+	if ((pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+	{
+		ULONG TmpLen;
+		UCHAR HtLen, HtLen1;
+
+#ifdef RT_BIG_ENDIAN
+		HT_CAPABILITY_IE HtCapabilityTmp;
+		ADD_HT_INFO_IE	addHTInfoTmp;
+		USHORT	b2lTmp, b2lTmp2;
+#endif
+
+		// add HT Capability IE
+		HtLen = sizeof(pAd->CommonCfg.HtCapability);
+		HtLen1 = sizeof(pAd->CommonCfg.AddHTInfo);
+#ifndef RT_BIG_ENDIAN
+		MakeOutgoingFrame(pBeaconFrame+FrameLen,	&TmpLen,
+						  1,						&HtCapIe,
+						  1,						&HtLen,
+						  HtLen,					&pAd->CommonCfg.HtCapability,
+						  1,						&AddHtInfoIe,
+						  1,						&HtLen1,
+						  HtLen1,					&pAd->CommonCfg.AddHTInfo,
+						  END_OF_ARGS);
+#else
+		NdisMoveMemory(&HtCapabilityTmp, &pAd->CommonCfg.HtCapability, HtLen);
+		*(USHORT *)(&HtCapabilityTmp.HtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.HtCapInfo));
+		*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo));
+
+		NdisMoveMemory(&addHTInfoTmp, &pAd->CommonCfg.AddHTInfo, HtLen1);
+		*(USHORT *)(&addHTInfoTmp.AddHtInfo2) = SWAP16(*(USHORT *)(&addHTInfoTmp.AddHtInfo2));
+		*(USHORT *)(&addHTInfoTmp.AddHtInfo3) = SWAP16(*(USHORT *)(&addHTInfoTmp.AddHtInfo3));
+
+		MakeOutgoingFrame(pBeaconFrame+FrameLen,	&TmpLen,
+						  1,						&HtCapIe,
+						  1,						&HtLen,
+						  HtLen,					&HtCapabilityTmp,
+						  1,						&AddHtInfoIe,
+						  1,						&HtLen1,
+						  HtLen1,					&addHTInfoTmp,
+						  END_OF_ARGS);
+#endif
+		FrameLen += TmpLen;
+	}
+#endif // DOT11_N_SUPPORT //
+
+	//beacon use reserved WCID 0xff
+    if (pAd->CommonCfg.Channel > 14)
+    {
+		RTMPWriteTxWI(pAd, pTxWI, FALSE, FALSE,  TRUE, FALSE, FALSE, TRUE, 0, 0xff, FrameLen,
+			PID_MGMT, PID_BEACON, RATE_1, IFS_HTTXOP, FALSE, &pAd->CommonCfg.MlmeTransmit);
+    }
+    else
+    {
+        // Set to use 1Mbps for Adhoc beacon.
+		HTTRANSMIT_SETTING Transmit;
+        Transmit.word = 0;
+        RTMPWriteTxWI(pAd, pTxWI, FALSE, FALSE,  TRUE, FALSE, FALSE, TRUE, 0, 0xff, FrameLen,
+    		PID_MGMT, PID_BEACON, RATE_1, IFS_HTTXOP, FALSE, &Transmit);
+    }
+
+#ifdef RT_BIG_ENDIAN
+	RTMPFrameEndianChange(pAd, pBeaconFrame, DIR_WRITE, FALSE);
+	RTMPWIEndianChange((PUCHAR)pTxWI, TYPE_TXWI);
+#endif
+
+    DBGPRINT(RT_DEBUG_TRACE, ("MakeIbssBeacon (len=%ld), SupRateLen=%d, ExtRateLen=%d, Channel=%d, PhyMode=%d\n",
+					FrameLen, SupRateLen, ExtRateLen, pAd->CommonCfg.Channel, pAd->CommonCfg.PhyMode));
+	return FrameLen;
+}
+
+
diff --git a/drivers/staging/rt2870/sta/dls.c b/drivers/staging/rt2870/sta/dls.c
new file mode 100644
index 00000000000..56bfbc33d6f
--- /dev/null
+++ b/drivers/staging/rt2870/sta/dls.c
@@ -0,0 +1,2210 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    dls.c
+
+    Abstract:
+    Handle WMM-DLS state machine
+
+    Revision History:
+    Who         When          What
+    --------    ----------    ----------------------------------------------
+    Rory Chen   02-14-2006
+	Arvin Tai	06-03-2008	  Modified for RT28xx
+ */
+
+#include "../rt_config.h"
+
+/*
+    ==========================================================================
+    Description:
+        dls state machine init, including state transition and timer init
+    Parameters:
+        Sm - pointer to the dls state machine
+    Note:
+        The state machine looks like this
+
+                            DLS_IDLE
+    MT2_MLME_DLS_REQUEST   MlmeDlsReqAction
+    MT2_PEER_DLS_REQUEST   PeerDlsReqAction
+    MT2_PEER_DLS_RESPONSE  PeerDlsRspAction
+    MT2_MLME_DLS_TEARDOWN  MlmeTearDownAction
+    MT2_PEER_DLS_TEARDOWN  PeerTearDownAction
+
+	IRQL = PASSIVE_LEVEL
+
+    ==========================================================================
+ */
+void DlsStateMachineInit(
+    IN PRTMP_ADAPTER pAd,
+    IN STATE_MACHINE *Sm,
+    OUT STATE_MACHINE_FUNC Trans[])
+{
+	UCHAR	i;
+
+    StateMachineInit(Sm, (STATE_MACHINE_FUNC*)Trans, MAX_DLS_STATE, MAX_DLS_MSG, (STATE_MACHINE_FUNC)Drop, DLS_IDLE, DLS_MACHINE_BASE);
+
+    // the first column
+    StateMachineSetAction(Sm, DLS_IDLE, MT2_MLME_DLS_REQ, (STATE_MACHINE_FUNC)MlmeDlsReqAction);
+    StateMachineSetAction(Sm, DLS_IDLE, MT2_PEER_DLS_REQ, (STATE_MACHINE_FUNC)PeerDlsReqAction);
+    StateMachineSetAction(Sm, DLS_IDLE, MT2_PEER_DLS_RSP, (STATE_MACHINE_FUNC)PeerDlsRspAction);
+    StateMachineSetAction(Sm, DLS_IDLE, MT2_MLME_DLS_TEAR_DOWN, (STATE_MACHINE_FUNC)MlmeDlsTearDownAction);
+    StateMachineSetAction(Sm, DLS_IDLE, MT2_PEER_DLS_TEAR_DOWN, (STATE_MACHINE_FUNC)PeerDlsTearDownAction);
+
+	for (i=0; i<MAX_NUM_OF_DLS_ENTRY; i++)
+	{
+		pAd->StaCfg.DLSEntry[i].pAd = pAd;
+		RTMPInitTimer(pAd, &pAd->StaCfg.DLSEntry[i].Timer, GET_TIMER_FUNCTION(DlsTimeoutAction), pAd, FALSE);
+	}
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID MlmeDlsReqAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+	PUCHAR			pOutBuffer = NULL;
+	NDIS_STATUS		NStatus;
+	ULONG			FrameLen = 0;
+	HEADER_802_11	DlsReqHdr;
+	PRT_802_11_DLS	pDLS = NULL;
+	UCHAR			Category = CATEGORY_DLS;
+	UCHAR			Action = ACTION_DLS_REQUEST;
+	ULONG			tmp;
+	USHORT			reason;
+	ULONG			Timeout;
+	BOOLEAN			TimerCancelled;
+
+	if(!MlmeDlsReqSanity(pAd, Elem->Msg, Elem->MsgLen, &pDLS, &reason))
+		return;
+
+	DBGPRINT(RT_DEBUG_TRACE,("DLS - MlmeDlsReqAction() \n"));
+
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if (NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("DLS - MlmeDlsReqAction() allocate memory failed \n"));
+		return;
+	}
+
+	ActHeaderInit(pAd, &DlsReqHdr, pAd->CommonCfg.Bssid, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+
+	// Build basic frame first
+	MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+					sizeof(HEADER_802_11),		&DlsReqHdr,
+					1,							&Category,
+					1,							&Action,
+					6,							&pDLS->MacAddr,
+					6,							pAd->CurrentAddress,
+					2,							&pAd->StaActive.CapabilityInfo,
+					2,							&pDLS->TimeOut,
+					1,							&SupRateIe,
+					1,							&pAd->MlmeAux.SupRateLen,
+					pAd->MlmeAux.SupRateLen,	pAd->MlmeAux.SupRate,
+					END_OF_ARGS);
+
+	if (pAd->MlmeAux.ExtRateLen != 0)
+	{
+		MakeOutgoingFrame(pOutBuffer + FrameLen,	&tmp,
+						  1,						&ExtRateIe,
+						  1,						&pAd->MlmeAux.ExtRateLen,
+						  pAd->MlmeAux.ExtRateLen,	pAd->MlmeAux.ExtRate,
+						  END_OF_ARGS);
+		FrameLen += tmp;
+	}
+
+#ifdef DOT11_N_SUPPORT
+	if ((pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+	{
+		UCHAR HtLen;
+
+#ifdef RT_BIG_ENDIAN
+		HT_CAPABILITY_IE HtCapabilityTmp;
+#endif
+
+		// add HT Capability IE
+		HtLen = sizeof(HT_CAPABILITY_IE);
+#ifndef RT_BIG_ENDIAN
+		MakeOutgoingFrame(pOutBuffer + FrameLen,	&tmp,
+							1,						&HtCapIe,
+							1,						&HtLen,
+							HtLen,					&pAd->CommonCfg.HtCapability,
+							END_OF_ARGS);
+#else
+		NdisMoveMemory(&HtCapabilityTmp, &pAd->CommonCfg.HtCapability, HtLen);
+							*(USHORT *)(&HtCapabilityTmp.HtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.HtCapInfo));
+							*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo));
+
+		MakeOutgoingFrame(pOutBuffer + FrameLen,	&tmp,
+							1,						&HtCapIe,
+							1,						&HtLen,
+							HtLen,					&HtCapabilityTmp,
+							END_OF_ARGS);
+#endif
+		FrameLen = FrameLen + tmp;
+	}
+#endif // DOT11_N_SUPPORT //
+
+	RTMPCancelTimer(&pDLS->Timer, &TimerCancelled);
+	Timeout = DLS_TIMEOUT;
+	RTMPSetTimer(&pDLS->Timer, Timeout);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID PeerDlsReqAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+	PUCHAR			pOutBuffer = NULL;
+	NDIS_STATUS		NStatus;
+	ULONG			FrameLen = 0;
+	USHORT			StatusCode = MLME_SUCCESS;
+	HEADER_802_11	DlsRspHdr;
+	UCHAR			Category = CATEGORY_DLS;
+	UCHAR			Action = ACTION_DLS_RESPONSE;
+	ULONG			tmp;
+	USHORT			CapabilityInfo;
+	UCHAR			DA[MAC_ADDR_LEN], SA[MAC_ADDR_LEN];
+	USHORT			DLSTimeOut;
+	SHORT			i;
+	ULONG			Timeout;
+	BOOLEAN			TimerCancelled;
+	PRT_802_11_DLS	pDLS = NULL;
+	UCHAR			MaxSupportedRateIn500Kbps = 0;
+    UCHAR			SupportedRatesLen;
+    UCHAR			SupportedRates[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR			HtCapabilityLen;
+	HT_CAPABILITY_IE	HtCapability;
+
+	if (!PeerDlsReqSanity(pAd, Elem->Msg, Elem->MsgLen, DA, SA, &CapabilityInfo, &DLSTimeOut,
+							&SupportedRatesLen, &SupportedRates[0], &HtCapabilityLen, &HtCapability))
+		return;
+
+    // supported rates array may not be sorted. sort it and find the maximum rate
+    for (i = 0; i < SupportedRatesLen; i++)
+    {
+        if (MaxSupportedRateIn500Kbps < (SupportedRates[i] & 0x7f))
+            MaxSupportedRateIn500Kbps = SupportedRates[i] & 0x7f;
+    }
+
+	DBGPRINT(RT_DEBUG_TRACE,("DLS - PeerDlsReqAction() from %02x:%02x:%02x:%02x:%02x:%02x\n", SA[0], SA[1], SA[2], SA[3], SA[4], SA[5]));
+
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if (NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("DLS - PeerDlsReqAction() allocate memory failed \n"));
+		return;
+	}
+
+	if (!INFRA_ON(pAd))
+	{
+		StatusCode = MLME_REQUEST_DECLINED;
+	}
+	else if (!pAd->CommonCfg.bWmmCapable)
+	{
+		StatusCode = MLME_DEST_STA_IS_NOT_A_QSTA;
+	}
+	else if (!pAd->CommonCfg.bDLSCapable)
+	{
+		StatusCode = MLME_REQUEST_DECLINED;
+	}
+	else
+	{
+		// find table to update parameters
+		for (i = (MAX_NUM_OF_DLS_ENTRY-1); i >= 0; i--)
+		{
+			if (pAd->StaCfg.DLSEntry[i].Valid && MAC_ADDR_EQUAL(SA, pAd->StaCfg.DLSEntry[i].MacAddr))
+			{
+				if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+					pAd->StaCfg.DLSEntry[i].Status = DLS_WAIT_KEY;
+				else
+				{
+					RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+					pAd->StaCfg.DLSEntry[i].Status = DLS_FINISH;
+				}
+
+				pAd->StaCfg.DLSEntry[i].Sequence = 0;
+				pAd->StaCfg.DLSEntry[i].TimeOut = DLSTimeOut;
+				pAd->StaCfg.DLSEntry[i].CountDownTimer = DLSTimeOut;
+				if (HtCapabilityLen != 0)
+					pAd->StaCfg.DLSEntry[i].bHTCap = TRUE;
+				else
+					pAd->StaCfg.DLSEntry[i].bHTCap = FALSE;
+				pDLS = &pAd->StaCfg.DLSEntry[i];
+				break;
+			}
+		}
+
+		// can not find in table, create a new one
+		if (i < 0)
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("DLS - PeerDlsReqAction() can not find same entry \n"));
+			for (i=(MAX_NUM_OF_DLS_ENTRY - 1); i >= MAX_NUM_OF_INIT_DLS_ENTRY; i--)
+			{
+				if (!pAd->StaCfg.DLSEntry[i].Valid)
+				{
+					MAC_TABLE_ENTRY *pEntry;
+					UCHAR MaxSupportedRate = RATE_11;
+
+					if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+					{
+						pAd->StaCfg.DLSEntry[i].Status = DLS_WAIT_KEY;
+					}
+					else
+					{
+						RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+						pAd->StaCfg.DLSEntry[i].Status = DLS_FINISH;
+					}
+
+					pAd->StaCfg.DLSEntry[i].Sequence = 0;
+					pAd->StaCfg.DLSEntry[i].Valid = TRUE;
+					pAd->StaCfg.DLSEntry[i].TimeOut = DLSTimeOut;
+					pAd->StaCfg.DLSEntry[i].CountDownTimer = DLSTimeOut;
+					NdisMoveMemory(pAd->StaCfg.DLSEntry[i].MacAddr, SA, MAC_ADDR_LEN);
+					if (HtCapabilityLen != 0)
+						pAd->StaCfg.DLSEntry[i].bHTCap = TRUE;
+					else
+						pAd->StaCfg.DLSEntry[i].bHTCap = FALSE;
+					pDLS = &pAd->StaCfg.DLSEntry[i];
+					pEntry = MacTableInsertDlsEntry(pAd, SA, i);
+
+					switch (MaxSupportedRateIn500Kbps)
+					{
+						case 108: MaxSupportedRate = RATE_54;   break;
+						case 96:  MaxSupportedRate = RATE_48;   break;
+						case 72:  MaxSupportedRate = RATE_36;   break;
+						case 48:  MaxSupportedRate = RATE_24;   break;
+						case 36:  MaxSupportedRate = RATE_18;   break;
+						case 24:  MaxSupportedRate = RATE_12;   break;
+						case 18:  MaxSupportedRate = RATE_9;    break;
+						case 12:  MaxSupportedRate = RATE_6;    break;
+						case 22:  MaxSupportedRate = RATE_11;   break;
+						case 11:  MaxSupportedRate = RATE_5_5;  break;
+						case 4:   MaxSupportedRate = RATE_2;    break;
+						case 2:   MaxSupportedRate = RATE_1;    break;
+						default:  MaxSupportedRate = RATE_11;   break;
+					}
+
+					pEntry->MaxSupportedRate = min(pAd->CommonCfg.MaxTxRate, MaxSupportedRate);
+
+					if (pEntry->MaxSupportedRate < RATE_FIRST_OFDM_RATE)
+					{
+						pEntry->MaxHTPhyMode.field.MODE = MODE_CCK;
+						pEntry->MaxHTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+						pEntry->MinHTPhyMode.field.MODE = MODE_CCK;
+						pEntry->MinHTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+						pEntry->HTPhyMode.field.MODE = MODE_CCK;
+						pEntry->HTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+					}
+					else
+					{
+						pEntry->MaxHTPhyMode.field.MODE = MODE_OFDM;
+						pEntry->MaxHTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+						pEntry->MinHTPhyMode.field.MODE = MODE_OFDM;
+						pEntry->MinHTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+						pEntry->HTPhyMode.field.MODE = MODE_OFDM;
+						pEntry->HTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+					}
+
+					pEntry->MaxHTPhyMode.field.BW = BW_20;
+					pEntry->MinHTPhyMode.field.BW = BW_20;
+
+#ifdef DOT11_N_SUPPORT
+					pEntry->HTCapability.MCSSet[0] = 0;
+					pEntry->HTCapability.MCSSet[1] = 0;
+
+					// If this Entry supports 802.11n, upgrade to HT rate.
+					if ((HtCapabilityLen != 0) && (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+					{
+						UCHAR	j, bitmask; //k,bitmask;
+						CHAR    ii;
+
+						DBGPRINT(RT_DEBUG_OFF, ("DLS - PeerDlsReqAction() Receive Peer HT Capable STA from %02x:%02x:%02x:%02x:%02x:%02x\n",
+									SA[0], SA[1], SA[2], SA[3], SA[4], SA[5]));
+
+						if ((HtCapability.HtCapInfo.GF) && (pAd->CommonCfg.DesiredHtPhy.GF))
+						{
+							pEntry->MaxHTPhyMode.field.MODE = MODE_HTGREENFIELD;
+						}
+						else
+						{
+							pEntry->MaxHTPhyMode.field.MODE = MODE_HTMIX;
+							pAd->MacTab.fAnyStationNonGF = TRUE;
+							pAd->CommonCfg.AddHTInfo.AddHtInfo2.NonGfPresent = 1;
+						}
+
+						if ((HtCapability.HtCapInfo.ChannelWidth) && (pAd->CommonCfg.DesiredHtPhy.ChannelWidth))
+						{
+							pEntry->MaxHTPhyMode.field.BW= BW_40;
+							pEntry->MaxHTPhyMode.field.ShortGI = ((pAd->CommonCfg.DesiredHtPhy.ShortGIfor40)&(HtCapability.HtCapInfo.ShortGIfor40));
+						}
+						else
+						{
+							pEntry->MaxHTPhyMode.field.BW = BW_20;
+							pEntry->MaxHTPhyMode.field.ShortGI = ((pAd->CommonCfg.DesiredHtPhy.ShortGIfor20)&(HtCapability.HtCapInfo.ShortGIfor20));
+							pAd->MacTab.fAnyStation20Only = TRUE;
+						}
+
+						// find max fixed rate
+						for (ii=15; ii>=0; ii--)
+						{
+							j = ii/8;
+							bitmask = (1<<(ii-(j*8)));
+							if ( (pAd->StaCfg.DesiredHtPhyInfo.MCSSet[j]&bitmask) && (HtCapability.MCSSet[j]&bitmask))
+							{
+								pEntry->MaxHTPhyMode.field.MCS = ii;
+								break;
+							}
+							if (ii==0)
+								break;
+						}
+
+
+						if (pAd->StaCfg.DesiredTransmitSetting.field.MCS != MCS_AUTO)
+						{
+
+							printk("@@@ pAd->CommonCfg.RegTransmitSetting.field.MCS = %d\n",
+								pAd->StaCfg.DesiredTransmitSetting.field.MCS);
+							if (pAd->StaCfg.DesiredTransmitSetting.field.MCS == 32)
+							{
+								// Fix MCS as HT Duplicated Mode
+								pEntry->MaxHTPhyMode.field.BW = 1;
+								pEntry->MaxHTPhyMode.field.MODE = MODE_HTMIX;
+								pEntry->MaxHTPhyMode.field.STBC = 0;
+								pEntry->MaxHTPhyMode.field.ShortGI = 0;
+								pEntry->MaxHTPhyMode.field.MCS = 32;
+							}
+							else if (pEntry->MaxHTPhyMode.field.MCS > pAd->StaCfg.HTPhyMode.field.MCS)
+							{
+								// STA supports fixed MCS
+								pEntry->MaxHTPhyMode.field.MCS = pAd->StaCfg.HTPhyMode.field.MCS;
+							}
+						}
+
+						pEntry->MaxHTPhyMode.field.STBC = (HtCapability.HtCapInfo.RxSTBC & (pAd->CommonCfg.DesiredHtPhy.TxSTBC));
+						pEntry->MpduDensity = HtCapability.HtCapParm.MpduDensity;
+						pEntry->MaxRAmpduFactor = HtCapability.HtCapParm.MaxRAmpduFactor;
+						pEntry->MmpsMode = (UCHAR)HtCapability.HtCapInfo.MimoPs;
+						pEntry->AMsduSize = (UCHAR)HtCapability.HtCapInfo.AMsduSize;
+						pEntry->HTPhyMode.word = pEntry->MaxHTPhyMode.word;
+
+						if (HtCapability.HtCapInfo.ShortGIfor20)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_SGI20_CAPABLE);
+						if (HtCapability.HtCapInfo.ShortGIfor40)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_SGI40_CAPABLE);
+						if (HtCapability.HtCapInfo.TxSTBC)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_TxSTBC_CAPABLE);
+						if (HtCapability.HtCapInfo.RxSTBC)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_RxSTBC_CAPABLE);
+						if (HtCapability.ExtHtCapInfo.PlusHTC)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_HTC_CAPABLE);
+						if (pAd->CommonCfg.bRdg && HtCapability.ExtHtCapInfo.RDGSupport)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_RDG_CAPABLE);
+						if (HtCapability.ExtHtCapInfo.MCSFeedback == 0x03)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_MCSFEEDBACK_CAPABLE);
+
+						NdisMoveMemory(&pEntry->HTCapability, &HtCapability, sizeof(HT_CAPABILITY_IE));
+					}
+#endif // DOT11_N_SUPPORT //
+
+					pEntry->HTPhyMode.word = pEntry->MaxHTPhyMode.word;
+					pEntry->CurrTxRate = pEntry->MaxSupportedRate;
+					CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_WMM_CAPABLE);
+
+					if (pAd->StaCfg.bAutoTxRateSwitch == TRUE)
+					{
+						PUCHAR pTable;
+						UCHAR TableSize = 0;
+
+						MlmeSelectTxRateTable(pAd, pEntry, &pTable, &TableSize, &pEntry->CurrTxRateIndex);
+						pEntry->bAutoTxRateSwitch = TRUE;
+					}
+					else
+					{
+						pEntry->HTPhyMode.field.MODE	= pAd->StaCfg.HTPhyMode.field.MODE;
+						pEntry->HTPhyMode.field.MCS	= pAd->StaCfg.HTPhyMode.field.MCS;
+						pEntry->bAutoTxRateSwitch = FALSE;
+
+						RTMPUpdateLegacyTxSetting((UCHAR)pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode, pEntry);
+					}
+					pEntry->RateLen = SupportedRatesLen;
+
+					break;
+				}
+			}
+		}
+		StatusCode = MLME_SUCCESS;
+
+		// can not find in table, create a new one
+		if (i < 0)
+		{
+			StatusCode = MLME_QOS_UNSPECIFY;
+			DBGPRINT(RT_DEBUG_ERROR,("DLS - PeerDlsReqAction() DLSEntry table full(only can support %d DLS session) \n", MAX_NUM_OF_DLS_ENTRY - MAX_NUM_OF_INIT_DLS_ENTRY));
+		}
+		else
+		{
+			DBGPRINT(RT_DEBUG_TRACE,("DLS - PeerDlsReqAction() use entry(%d) %02x:%02x:%02x:%02x:%02x:%02x\n",
+				i, SA[0], SA[1], SA[2], SA[3], SA[4], SA[5]));
+		}
+	}
+
+	ActHeaderInit(pAd, &DlsRspHdr, pAd->CommonCfg.Bssid, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+
+	// Build basic frame first
+	if (StatusCode == MLME_SUCCESS)
+	{
+		MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+						sizeof(HEADER_802_11),		&DlsRspHdr,
+						1,							&Category,
+						1,							&Action,
+						2,							&StatusCode,
+						6,							SA,
+						6,							pAd->CurrentAddress,
+						2,							&pAd->StaActive.CapabilityInfo,
+						1,							&SupRateIe,
+						1,							&pAd->MlmeAux.SupRateLen,
+						pAd->MlmeAux.SupRateLen,	pAd->MlmeAux.SupRate,
+						END_OF_ARGS);
+
+		if (pAd->MlmeAux.ExtRateLen != 0)
+		{
+			MakeOutgoingFrame(pOutBuffer + FrameLen,	&tmp,
+							  1,						&ExtRateIe,
+							  1,						&pAd->MlmeAux.ExtRateLen,
+							  pAd->MlmeAux.ExtRateLen, 	pAd->MlmeAux.ExtRate,
+							  END_OF_ARGS);
+			FrameLen += tmp;
+		}
+
+#ifdef DOT11_N_SUPPORT
+		if ((pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+		{
+			UCHAR HtLen;
+
+#ifdef RT_BIG_ENDIAN
+			HT_CAPABILITY_IE HtCapabilityTmp;
+#endif
+
+			// add HT Capability IE
+			HtLen = sizeof(HT_CAPABILITY_IE);
+#ifndef RT_BIG_ENDIAN
+			MakeOutgoingFrame(pOutBuffer + FrameLen,	&tmp,
+								1,						&HtCapIe,
+								1,						&HtLen,
+								HtLen,					&pAd->CommonCfg.HtCapability,
+								END_OF_ARGS);
+#else
+			NdisMoveMemory(&HtCapabilityTmp, &pAd->CommonCfg.HtCapability, HtLen);
+								*(USHORT *)(&HtCapabilityTmp.HtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.HtCapInfo));
+								*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo) = SWAP16(*(USHORT *)(&HtCapabilityTmp.ExtHtCapInfo));
+
+			MakeOutgoingFrame(pOutBuffer + FrameLen,	&tmp,
+								1,						&HtCapIe,
+								1,						&HtLen,
+								HtLen,					&HtCapabilityTmp,
+								END_OF_ARGS);
+#endif
+			FrameLen = FrameLen + tmp;
+		}
+#endif // DOT11_N_SUPPORT //
+
+		if (pDLS && (pDLS->Status != DLS_FINISH))
+		{
+			RTMPCancelTimer(&pDLS->Timer, &TimerCancelled);
+			Timeout = DLS_TIMEOUT;
+			RTMPSetTimer(&pDLS->Timer, Timeout);
+		}
+	}
+	else
+	{
+		MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+						sizeof(HEADER_802_11),		&DlsRspHdr,
+						1,							&Category,
+						1,							&Action,
+						2,							&StatusCode,
+						6,							SA,
+						6,							pAd->CurrentAddress,
+						END_OF_ARGS);
+	}
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID PeerDlsRspAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT		CapabilityInfo;
+	UCHAR		DA[MAC_ADDR_LEN], SA[MAC_ADDR_LEN];
+	USHORT		StatusCode;
+	SHORT		i;
+	BOOLEAN		TimerCancelled;
+	UCHAR		MaxSupportedRateIn500Kbps = 0;
+    UCHAR		SupportedRatesLen;
+    UCHAR		SupportedRates[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR		HtCapabilityLen;
+	HT_CAPABILITY_IE	HtCapability;
+
+	if (!pAd->CommonCfg.bDLSCapable)
+		return;
+
+	if (!INFRA_ON(pAd))
+		return;
+
+	if (!PeerDlsRspSanity(pAd, Elem->Msg, Elem->MsgLen, DA, SA, &CapabilityInfo, &StatusCode,
+							&SupportedRatesLen, &SupportedRates[0], &HtCapabilityLen, &HtCapability))
+		return;
+
+    // supported rates array may not be sorted. sort it and find the maximum rate
+    for (i=0; i<SupportedRatesLen; i++)
+    {
+        if (MaxSupportedRateIn500Kbps < (SupportedRates[i] & 0x7f))
+            MaxSupportedRateIn500Kbps = SupportedRates[i] & 0x7f;
+    }
+
+	DBGPRINT(RT_DEBUG_TRACE,("DLS - PeerDlsRspAction() from %02x:%02x:%02x:%02x:%02x:%02x with StatusCode=%d, CapabilityInfo=0x%x\n",
+		SA[0], SA[1], SA[2], SA[3], SA[4], SA[5], StatusCode, CapabilityInfo));
+
+	for (i = 0; i < MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+	{
+		if (pAd->StaCfg.DLSEntry[i].Valid && MAC_ADDR_EQUAL(SA, pAd->StaCfg.DLSEntry[i].MacAddr))
+		{
+			if (StatusCode == MLME_SUCCESS)
+			{
+				MAC_TABLE_ENTRY *pEntry;
+				UCHAR MaxSupportedRate = RATE_11;
+
+				pEntry = MacTableInsertDlsEntry(pAd, SA, i);
+
+				switch (MaxSupportedRateIn500Kbps)
+				{
+					case 108: MaxSupportedRate = RATE_54;   break;
+					case 96:  MaxSupportedRate = RATE_48;   break;
+					case 72:  MaxSupportedRate = RATE_36;   break;
+					case 48:  MaxSupportedRate = RATE_24;   break;
+					case 36:  MaxSupportedRate = RATE_18;   break;
+					case 24:  MaxSupportedRate = RATE_12;   break;
+					case 18:  MaxSupportedRate = RATE_9;    break;
+					case 12:  MaxSupportedRate = RATE_6;    break;
+					case 22:  MaxSupportedRate = RATE_11;   break;
+					case 11:  MaxSupportedRate = RATE_5_5;  break;
+					case 4:   MaxSupportedRate = RATE_2;    break;
+					case 2:   MaxSupportedRate = RATE_1;    break;
+					default:  MaxSupportedRate = RATE_11;   break;
+				}
+
+				pEntry->MaxSupportedRate = min(pAd->CommonCfg.MaxTxRate, MaxSupportedRate);
+
+				if (pEntry->MaxSupportedRate < RATE_FIRST_OFDM_RATE)
+				{
+					pEntry->MaxHTPhyMode.field.MODE = MODE_CCK;
+					pEntry->MaxHTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+					pEntry->MinHTPhyMode.field.MODE = MODE_CCK;
+					pEntry->MinHTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+					pEntry->HTPhyMode.field.MODE = MODE_CCK;
+					pEntry->HTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+				}
+				else
+				{
+					pEntry->MaxHTPhyMode.field.MODE = MODE_OFDM;
+					pEntry->MaxHTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+					pEntry->MinHTPhyMode.field.MODE = MODE_OFDM;
+					pEntry->MinHTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+					pEntry->HTPhyMode.field.MODE = MODE_OFDM;
+					pEntry->HTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+				}
+
+				pEntry->MaxHTPhyMode.field.BW = BW_20;
+				pEntry->MinHTPhyMode.field.BW = BW_20;
+
+#ifdef DOT11_N_SUPPORT
+				pEntry->HTCapability.MCSSet[0] = 0;
+				pEntry->HTCapability.MCSSet[1] = 0;
+
+				// If this Entry supports 802.11n, upgrade to HT rate.
+				if ((HtCapabilityLen != 0) && (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+				{
+					UCHAR	j, bitmask; //k,bitmask;
+					CHAR    ii;
+
+					DBGPRINT(RT_DEBUG_OFF, ("DLS - PeerDlsRspAction Receive Peer HT Capable STA from %02x:%02x:%02x:%02x:%02x:%02x\n",
+								SA[0], SA[1], SA[2], SA[3], SA[4], SA[5]));
+
+					if ((HtCapability.HtCapInfo.GF) && (pAd->CommonCfg.DesiredHtPhy.GF))
+					{
+						pEntry->MaxHTPhyMode.field.MODE = MODE_HTGREENFIELD;
+					}
+					else
+					{
+						pEntry->MaxHTPhyMode.field.MODE = MODE_HTMIX;
+						pAd->MacTab.fAnyStationNonGF = TRUE;
+						pAd->CommonCfg.AddHTInfo.AddHtInfo2.NonGfPresent = 1;
+					}
+
+					if ((HtCapability.HtCapInfo.ChannelWidth) && (pAd->CommonCfg.DesiredHtPhy.ChannelWidth))
+					{
+						pEntry->MaxHTPhyMode.field.BW= BW_40;
+						pEntry->MaxHTPhyMode.field.ShortGI = ((pAd->CommonCfg.DesiredHtPhy.ShortGIfor40)&(HtCapability.HtCapInfo.ShortGIfor40));
+					}
+					else
+					{
+						pEntry->MaxHTPhyMode.field.BW = BW_20;
+						pEntry->MaxHTPhyMode.field.ShortGI = ((pAd->CommonCfg.DesiredHtPhy.ShortGIfor20)&(HtCapability.HtCapInfo.ShortGIfor20));
+						pAd->MacTab.fAnyStation20Only = TRUE;
+					}
+
+					// find max fixed rate
+					for (ii=15; ii>=0; ii--)
+					{
+						j = ii/8;
+						bitmask = (1<<(ii-(j*8)));
+						if ( (pAd->StaCfg.DesiredHtPhyInfo.MCSSet[j]&bitmask) && (HtCapability.MCSSet[j]&bitmask))
+						{
+							pEntry->MaxHTPhyMode.field.MCS = ii;
+							break;
+						}
+						if (ii==0)
+							break;
+					}
+
+					if (pAd->StaCfg.DesiredTransmitSetting.field.MCS != MCS_AUTO)
+					{
+						if (pAd->StaCfg.DesiredTransmitSetting.field.MCS == 32)
+						{
+							// Fix MCS as HT Duplicated Mode
+							pEntry->MaxHTPhyMode.field.BW = 1;
+							pEntry->MaxHTPhyMode.field.MODE = MODE_HTMIX;
+							pEntry->MaxHTPhyMode.field.STBC = 0;
+							pEntry->MaxHTPhyMode.field.ShortGI = 0;
+							pEntry->MaxHTPhyMode.field.MCS = 32;
+						}
+						else if (pEntry->MaxHTPhyMode.field.MCS > pAd->StaCfg.HTPhyMode.field.MCS)
+						{
+							// STA supports fixed MCS
+							pEntry->MaxHTPhyMode.field.MCS = pAd->StaCfg.HTPhyMode.field.MCS;
+						}
+					}
+
+					pEntry->MaxHTPhyMode.field.STBC = (HtCapability.HtCapInfo.RxSTBC & (pAd->CommonCfg.DesiredHtPhy.TxSTBC));
+					pEntry->MpduDensity = HtCapability.HtCapParm.MpduDensity;
+					pEntry->MaxRAmpduFactor = HtCapability.HtCapParm.MaxRAmpduFactor;
+					pEntry->MmpsMode = (UCHAR)HtCapability.HtCapInfo.MimoPs;
+					pEntry->AMsduSize = (UCHAR)HtCapability.HtCapInfo.AMsduSize;
+					pEntry->HTPhyMode.word = pEntry->MaxHTPhyMode.word;
+
+					if (HtCapability.HtCapInfo.ShortGIfor20)
+						CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_SGI20_CAPABLE);
+					if (HtCapability.HtCapInfo.ShortGIfor40)
+						CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_SGI40_CAPABLE);
+					if (HtCapability.HtCapInfo.TxSTBC)
+						CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_TxSTBC_CAPABLE);
+					if (HtCapability.HtCapInfo.RxSTBC)
+						CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_RxSTBC_CAPABLE);
+					if (HtCapability.ExtHtCapInfo.PlusHTC)
+						CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_HTC_CAPABLE);
+					if (pAd->CommonCfg.bRdg && HtCapability.ExtHtCapInfo.RDGSupport)
+						CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_RDG_CAPABLE);
+					if (HtCapability.ExtHtCapInfo.MCSFeedback == 0x03)
+						CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_MCSFEEDBACK_CAPABLE);
+
+					NdisMoveMemory(&pEntry->HTCapability, &HtCapability, sizeof(HT_CAPABILITY_IE));
+				}
+#endif // DOT11_N_SUPPORT //
+				pEntry->HTPhyMode.word = pEntry->MaxHTPhyMode.word;
+				pEntry->CurrTxRate = pEntry->MaxSupportedRate;
+				CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_WMM_CAPABLE);
+
+				if (pAd->StaCfg.bAutoTxRateSwitch == TRUE)
+				{
+					PUCHAR pTable;
+					UCHAR TableSize = 0;
+
+					MlmeSelectTxRateTable(pAd, pEntry, &pTable, &TableSize, &pEntry->CurrTxRateIndex);
+					pEntry->bAutoTxRateSwitch = TRUE;
+				}
+				else
+				{
+					pEntry->HTPhyMode.field.MODE	= pAd->StaCfg.HTPhyMode.field.MODE;
+					pEntry->HTPhyMode.field.MCS	= pAd->StaCfg.HTPhyMode.field.MCS;
+					pEntry->bAutoTxRateSwitch = FALSE;
+
+					RTMPUpdateLegacyTxSetting((UCHAR)pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode, pEntry);
+				}
+				pEntry->RateLen = SupportedRatesLen;
+
+				if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+				{
+					// If support WPA or WPA2, start STAKey hand shake,
+					// If failed hand shake, just tear down peer DLS
+					if (RTMPSendSTAKeyRequest(pAd, pAd->StaCfg.DLSEntry[i].MacAddr) != NDIS_STATUS_SUCCESS)
+					{
+						MLME_DLS_REQ_STRUCT	MlmeDlsReq;
+						USHORT				reason = REASON_QOS_CIPHER_NOT_SUPPORT;
+
+						DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+						MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_TEAR_DOWN, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+						pAd->StaCfg.DLSEntry[i].Status = DLS_NONE;
+						pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+						DBGPRINT(RT_DEBUG_ERROR,("DLS - PeerDlsRspAction failed when call RTMPSendSTAKeyRequest \n"));
+					}
+					else
+					{
+						pAd->StaCfg.DLSEntry[i].Status = DLS_WAIT_KEY;
+						DBGPRINT(RT_DEBUG_TRACE,("DLS - waiting for STAKey handshake procedure\n"));
+					}
+				}
+				else
+				{
+					RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+					pAd->StaCfg.DLSEntry[i].Status = DLS_FINISH;
+					DBGPRINT(RT_DEBUG_TRACE,("DLS - PeerDlsRspAction() from %02x:%02x:%02x:%02x:%02x:%02x Succeed with WEP or no security\n", SA[0], SA[1], SA[2], SA[3], SA[4], SA[5]));
+				}
+
+				//initialize seq no for DLS frames.
+				pAd->StaCfg.DLSEntry[i].Sequence = 0;
+				if (HtCapabilityLen != 0)
+					pAd->StaCfg.DLSEntry[i].bHTCap = TRUE;
+				else
+					pAd->StaCfg.DLSEntry[i].bHTCap = FALSE;
+			}
+			else
+			{
+				// DLS setup procedure failed.
+				pAd->StaCfg.DLSEntry[i].Status = DLS_NONE;
+				pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+				RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+				DBGPRINT(RT_DEBUG_ERROR,("DLS - PeerDlsRspAction failed with StatusCode=%d \n", StatusCode));
+			}
+		}
+	}
+
+	if (i >= MAX_NUM_OF_INIT_DLS_ENTRY)
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("DLS - PeerDlsRspAction() update timeout value \n"));
+		for (i=(MAX_NUM_OF_DLS_ENTRY-1); i>=MAX_NUM_OF_INIT_DLS_ENTRY; i--)
+		{
+			if (pAd->StaCfg.DLSEntry[i].Valid && MAC_ADDR_EQUAL(SA, pAd->StaCfg.DLSEntry[i].MacAddr))
+			{
+				if (StatusCode == MLME_SUCCESS)
+				{
+					MAC_TABLE_ENTRY *pEntry;
+					UCHAR MaxSupportedRate = RATE_11;
+
+					pEntry = MacTableInsertDlsEntry(pAd, SA, i);
+
+					switch (MaxSupportedRateIn500Kbps)
+					{
+						case 108: MaxSupportedRate = RATE_54;   break;
+						case 96:  MaxSupportedRate = RATE_48;   break;
+						case 72:  MaxSupportedRate = RATE_36;   break;
+						case 48:  MaxSupportedRate = RATE_24;   break;
+						case 36:  MaxSupportedRate = RATE_18;   break;
+						case 24:  MaxSupportedRate = RATE_12;   break;
+						case 18:  MaxSupportedRate = RATE_9;    break;
+						case 12:  MaxSupportedRate = RATE_6;    break;
+						case 22:  MaxSupportedRate = RATE_11;   break;
+						case 11:  MaxSupportedRate = RATE_5_5;  break;
+						case 4:   MaxSupportedRate = RATE_2;    break;
+						case 2:   MaxSupportedRate = RATE_1;    break;
+						default:  MaxSupportedRate = RATE_11;   break;
+					}
+
+					pEntry->MaxSupportedRate = min(pAd->CommonCfg.MaxTxRate, MaxSupportedRate);
+
+					if (pEntry->MaxSupportedRate < RATE_FIRST_OFDM_RATE)
+					{
+						pEntry->MaxHTPhyMode.field.MODE = MODE_CCK;
+						pEntry->MaxHTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+						pEntry->MinHTPhyMode.field.MODE = MODE_CCK;
+						pEntry->MinHTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+						pEntry->HTPhyMode.field.MODE = MODE_CCK;
+						pEntry->HTPhyMode.field.MCS = pEntry->MaxSupportedRate;
+					}
+					else
+					{
+						pEntry->MaxHTPhyMode.field.MODE = MODE_OFDM;
+						pEntry->MaxHTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+						pEntry->MinHTPhyMode.field.MODE = MODE_OFDM;
+						pEntry->MinHTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+						pEntry->HTPhyMode.field.MODE = MODE_OFDM;
+						pEntry->HTPhyMode.field.MCS = OfdmRateToRxwiMCS[pEntry->MaxSupportedRate];
+					}
+
+					pEntry->MaxHTPhyMode.field.BW = BW_20;
+					pEntry->MinHTPhyMode.field.BW = BW_20;
+
+#ifdef DOT11_N_SUPPORT
+					pEntry->HTCapability.MCSSet[0] = 0;
+					pEntry->HTCapability.MCSSet[1] = 0;
+
+					// If this Entry supports 802.11n, upgrade to HT rate.
+					if ((HtCapabilityLen != 0) && (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+					{
+						UCHAR	j, bitmask; //k,bitmask;
+						CHAR    ii;
+
+						DBGPRINT(RT_DEBUG_OFF, ("DLS - PeerDlsRspAction Receive Peer HT Capable STA from %02x:%02x:%02x:%02x:%02x:%02x\n",
+									SA[0], SA[1], SA[2], SA[3], SA[4], SA[5]));
+
+						if ((HtCapability.HtCapInfo.GF) && (pAd->CommonCfg.DesiredHtPhy.GF))
+						{
+							pEntry->MaxHTPhyMode.field.MODE = MODE_HTGREENFIELD;
+						}
+						else
+						{
+							pEntry->MaxHTPhyMode.field.MODE = MODE_HTMIX;
+							pAd->MacTab.fAnyStationNonGF = TRUE;
+							pAd->CommonCfg.AddHTInfo.AddHtInfo2.NonGfPresent = 1;
+						}
+
+						if ((HtCapability.HtCapInfo.ChannelWidth) && (pAd->CommonCfg.DesiredHtPhy.ChannelWidth))
+						{
+							pEntry->MaxHTPhyMode.field.BW= BW_40;
+							pEntry->MaxHTPhyMode.field.ShortGI = ((pAd->CommonCfg.DesiredHtPhy.ShortGIfor40)&(HtCapability.HtCapInfo.ShortGIfor40));
+						}
+						else
+						{
+							pEntry->MaxHTPhyMode.field.BW = BW_20;
+							pEntry->MaxHTPhyMode.field.ShortGI = ((pAd->CommonCfg.DesiredHtPhy.ShortGIfor20)&(HtCapability.HtCapInfo.ShortGIfor20));
+							pAd->MacTab.fAnyStation20Only = TRUE;
+						}
+
+						// find max fixed rate
+						for (ii=15; ii>=0; ii--)
+						{
+							j = ii/8;
+							bitmask = (1<<(ii-(j*8)));
+							if ( (pAd->StaCfg.DesiredHtPhyInfo.MCSSet[j]&bitmask) && (HtCapability.MCSSet[j]&bitmask))
+							{
+								pEntry->MaxHTPhyMode.field.MCS = ii;
+								break;
+							}
+							if (ii==0)
+								break;
+						}
+
+						if (pAd->StaCfg.DesiredTransmitSetting.field.MCS != MCS_AUTO)
+						{
+							printk("@@@ pAd->CommonCfg.RegTransmitSetting.field.MCS = %d\n",
+								pAd->StaCfg.DesiredTransmitSetting.field.MCS);
+							if (pAd->StaCfg.DesiredTransmitSetting.field.MCS == 32)
+							{
+								// Fix MCS as HT Duplicated Mode
+								pEntry->MaxHTPhyMode.field.BW = 1;
+								pEntry->MaxHTPhyMode.field.MODE = MODE_HTMIX;
+								pEntry->MaxHTPhyMode.field.STBC = 0;
+								pEntry->MaxHTPhyMode.field.ShortGI = 0;
+								pEntry->MaxHTPhyMode.field.MCS = 32;
+							}
+							else if (pEntry->MaxHTPhyMode.field.MCS > pAd->StaCfg.HTPhyMode.field.MCS)
+							{
+								// STA supports fixed MCS
+								pEntry->MaxHTPhyMode.field.MCS = pAd->StaCfg.HTPhyMode.field.MCS;
+							}
+						}
+
+						pEntry->MaxHTPhyMode.field.STBC = (HtCapability.HtCapInfo.RxSTBC & (pAd->CommonCfg.DesiredHtPhy.TxSTBC));
+						pEntry->MpduDensity = HtCapability.HtCapParm.MpduDensity;
+						pEntry->MaxRAmpduFactor = HtCapability.HtCapParm.MaxRAmpduFactor;
+						pEntry->MmpsMode = (UCHAR)HtCapability.HtCapInfo.MimoPs;
+						pEntry->AMsduSize = (UCHAR)HtCapability.HtCapInfo.AMsduSize;
+						pEntry->HTPhyMode.word = pEntry->MaxHTPhyMode.word;
+
+						if (HtCapability.HtCapInfo.ShortGIfor20)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_SGI20_CAPABLE);
+						if (HtCapability.HtCapInfo.ShortGIfor40)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_SGI40_CAPABLE);
+						if (HtCapability.HtCapInfo.TxSTBC)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_TxSTBC_CAPABLE);
+						if (HtCapability.HtCapInfo.RxSTBC)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_RxSTBC_CAPABLE);
+						if (HtCapability.ExtHtCapInfo.PlusHTC)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_HTC_CAPABLE);
+						if (pAd->CommonCfg.bRdg && HtCapability.ExtHtCapInfo.RDGSupport)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_RDG_CAPABLE);
+						if (HtCapability.ExtHtCapInfo.MCSFeedback == 0x03)
+							CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_MCSFEEDBACK_CAPABLE);
+
+						NdisMoveMemory(&pEntry->HTCapability, &HtCapability, sizeof(HT_CAPABILITY_IE));
+					}
+#endif // DOT11_N_SUPPORT //
+
+					pEntry->HTPhyMode.word = pEntry->MaxHTPhyMode.word;
+					pEntry->CurrTxRate = pEntry->MaxSupportedRate;
+					CLIENT_STATUS_SET_FLAG(pEntry, fCLIENT_STATUS_WMM_CAPABLE);
+
+					if (pAd->StaCfg.bAutoTxRateSwitch == TRUE)
+					{
+						PUCHAR pTable;
+						UCHAR TableSize = 0;
+
+						MlmeSelectTxRateTable(pAd, pEntry, &pTable, &TableSize, &pEntry->CurrTxRateIndex);
+						pEntry->bAutoTxRateSwitch = TRUE;
+					}
+					else
+					{
+						pEntry->HTPhyMode.field.MODE	= pAd->StaCfg.HTPhyMode.field.MODE;
+						pEntry->HTPhyMode.field.MCS	= pAd->StaCfg.HTPhyMode.field.MCS;
+						pEntry->bAutoTxRateSwitch = FALSE;
+
+						RTMPUpdateLegacyTxSetting((UCHAR)pAd->StaCfg.DesiredTransmitSetting.field.FixedTxMode, pEntry);
+					}
+					pEntry->RateLen = SupportedRatesLen;
+
+					if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+					{
+						// If support WPA or WPA2, start STAKey hand shake,
+						// If failed hand shake, just tear down peer DLS
+						if (RTMPSendSTAKeyRequest(pAd, pAd->StaCfg.DLSEntry[i].MacAddr) != NDIS_STATUS_SUCCESS)
+						{
+							MLME_DLS_REQ_STRUCT	MlmeDlsReq;
+							USHORT				reason = REASON_QOS_CIPHER_NOT_SUPPORT;
+
+							DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+							MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_TEAR_DOWN, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+							pAd->StaCfg.DLSEntry[i].Status = DLS_NONE;
+							pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+							DBGPRINT(RT_DEBUG_ERROR,("DLS - PeerDlsRspAction failed when call RTMPSendSTAKeyRequest \n"));
+						}
+						else
+						{
+							pAd->StaCfg.DLSEntry[i].Status = DLS_WAIT_KEY;
+							DBGPRINT(RT_DEBUG_TRACE,("DLS - waiting for STAKey handshake procedure\n"));
+						}
+					}
+					else
+					{
+						RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+						pAd->StaCfg.DLSEntry[i].Status = DLS_FINISH;
+						DBGPRINT(RT_DEBUG_TRACE,("DLS - PeerDlsRspAction() from %02x:%02x:%02x:%02x:%02x:%02x Succeed with WEP or no security\n", SA[0], SA[1], SA[2], SA[3], SA[4], SA[5]));
+					}
+					pAd->StaCfg.DLSEntry[i].Sequence = 0;
+					if (HtCapabilityLen != 0)
+						pAd->StaCfg.DLSEntry[i].bHTCap = TRUE;
+					else
+						pAd->StaCfg.DLSEntry[i].bHTCap = FALSE;
+				}
+				else
+				{
+					// DLS setup procedure failed.
+					pAd->StaCfg.DLSEntry[i].Status = DLS_NONE;
+					pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+					RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+					DBGPRINT(RT_DEBUG_ERROR,("DLS - PeerDlsRspAction failed with StatusCode=%d \n", StatusCode));
+				}
+			}
+		}
+	}
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID MlmeDlsTearDownAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+	PUCHAR			pOutBuffer = NULL;
+	NDIS_STATUS		NStatus;
+	ULONG			FrameLen = 0;
+	UCHAR			Category = CATEGORY_DLS;
+	UCHAR			Action = ACTION_DLS_TEARDOWN;
+	USHORT			ReasonCode = REASON_QOS_UNSPECIFY;
+	HEADER_802_11	DlsTearDownHdr;
+	PRT_802_11_DLS	pDLS;
+	BOOLEAN			TimerCancelled;
+	UCHAR			i;
+
+	if(!MlmeDlsReqSanity(pAd, Elem->Msg, Elem->MsgLen, &pDLS, &ReasonCode))
+		return;
+
+	DBGPRINT(RT_DEBUG_TRACE,("DLS - MlmeDlsTearDownAction() with ReasonCode=%d \n", ReasonCode));
+
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if (NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("DLS - MlmeDlsTearDownAction() allocate memory failed \n"));
+		return;
+	}
+
+	ActHeaderInit(pAd, &DlsTearDownHdr, pAd->CommonCfg.Bssid, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+
+	// Build basic frame first
+	MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+					sizeof(HEADER_802_11),		&DlsTearDownHdr,
+					1,							&Category,
+					1,							&Action,
+					6,							&pDLS->MacAddr,
+					6,							pAd->CurrentAddress,
+					2,							&ReasonCode,
+					END_OF_ARGS);
+
+	MiniportMMRequest(pAd, QID_AC_BE, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+	RTMPCancelTimer(&pDLS->Timer, &TimerCancelled);
+
+	// Remove key in local dls table entry
+	for (i = 0; i < MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+	{
+		if (MAC_ADDR_EQUAL(pDLS->MacAddr, pAd->StaCfg.DLSEntry[i].MacAddr))
+		{
+			MacTableDeleteDlsEntry(pAd, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID, pAd->StaCfg.DLSEntry[i].MacAddr);
+		}
+	}
+
+	// clear peer dls table entry
+	for (i = MAX_NUM_OF_INIT_DLS_ENTRY; i < MAX_NUM_OF_DLS_ENTRY; i++)
+	{
+		if (MAC_ADDR_EQUAL(pDLS->MacAddr, pAd->StaCfg.DLSEntry[i].MacAddr))
+		{
+			pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+			pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+			RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+			MacTableDeleteDlsEntry(pAd, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID, pAd->StaCfg.DLSEntry[i].MacAddr);
+		}
+	}
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID PeerDlsTearDownAction(
+    IN PRTMP_ADAPTER pAd,
+    IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR			DA[MAC_ADDR_LEN], SA[MAC_ADDR_LEN];
+	USHORT			ReasonCode;
+	UINT			i;
+	BOOLEAN			TimerCancelled;
+
+	if (!pAd->CommonCfg.bDLSCapable)
+		return;
+
+	if (!INFRA_ON(pAd))
+		return;
+
+	if (!PeerDlsTearDownSanity(pAd, Elem->Msg, Elem->MsgLen, DA, SA, &ReasonCode))
+		return;
+
+	DBGPRINT(RT_DEBUG_TRACE,("DLS - PeerDlsTearDownAction() from %02x:%02x:%02x:%02x:%02x:%02x with ReasonCode=%d\n", SA[0], SA[1], SA[2], SA[3], SA[4], SA[5], ReasonCode));
+
+	// clear local dls table entry
+	for (i=0; i<MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+	{
+		if (pAd->StaCfg.DLSEntry[i].Valid && MAC_ADDR_EQUAL(SA, pAd->StaCfg.DLSEntry[i].MacAddr))
+		{
+			pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+			pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+			RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+			//AsicDelWcidTab(pAd, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID);
+			//AsicRemovePairwiseKeyEntry(pAd, BSS0, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID);
+			MacTableDeleteDlsEntry(pAd, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID, pAd->StaCfg.DLSEntry[i].MacAddr);
+		}
+	}
+
+	// clear peer dls table entry
+	for (i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+	{
+		if (pAd->StaCfg.DLSEntry[i].Valid && MAC_ADDR_EQUAL(SA, pAd->StaCfg.DLSEntry[i].MacAddr))
+		{
+			pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+			pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+			RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+			//AsicDelWcidTab(pAd, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID);
+			//AsicRemovePairwiseKeyEntry(pAd, BSS0, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID);
+			MacTableDeleteDlsEntry(pAd, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID, pAd->StaCfg.DLSEntry[i].MacAddr);
+		}
+	}
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID RTMPCheckDLSTimeOut(
+	IN PRTMP_ADAPTER	pAd)
+{
+	ULONG				i;
+	MLME_DLS_REQ_STRUCT	MlmeDlsReq;
+	USHORT				reason = REASON_QOS_UNSPECIFY;
+
+	if (! pAd->CommonCfg.bDLSCapable)
+		return;
+
+	if (! INFRA_ON(pAd))
+		return;
+
+	// If timeout value is equaled to zero, it means always not be timeout.
+
+	// update local dls table entry
+	for (i=0; i<MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+	{
+		if ((pAd->StaCfg.DLSEntry[i].Valid) && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH)
+			&& (pAd->StaCfg.DLSEntry[i].TimeOut != 0))
+		{
+			pAd->StaCfg.DLSEntry[i].CountDownTimer --;
+
+			if (pAd->StaCfg.DLSEntry[i].CountDownTimer == 0)
+			{
+				reason = REASON_QOS_REQUEST_TIMEOUT;
+				pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+				pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+				DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+				MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_TEAR_DOWN, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+			}
+		}
+	}
+
+	// update peer dls table entry
+	for (i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+	{
+		if ((pAd->StaCfg.DLSEntry[i].Valid) && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH)
+			&& (pAd->StaCfg.DLSEntry[i].TimeOut != 0))
+		{
+			pAd->StaCfg.DLSEntry[i].CountDownTimer --;
+
+			if (pAd->StaCfg.DLSEntry[i].CountDownTimer == 0)
+			{
+				reason = REASON_QOS_REQUEST_TIMEOUT;
+				pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+				pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+				DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+				MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_TEAR_DOWN, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+			}
+		}
+	}
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN RTMPRcvFrameDLSCheck(
+	IN PRTMP_ADAPTER	pAd,
+	IN PHEADER_802_11	pHeader,
+	IN ULONG			Len,
+	IN PRT28XX_RXD_STRUC	pRxD)
+{
+	ULONG			i;
+	BOOLEAN			bFindEntry = FALSE;
+	BOOLEAN			bSTAKeyFrame = FALSE;
+	PEAPOL_PACKET	pEap;
+	PUCHAR			pProto, pAddr = NULL;
+	PUCHAR			pSTAKey = NULL;
+	UCHAR			ZeroReplay[LEN_KEY_DESC_REPLAY];
+	UCHAR			Mic[16], OldMic[16];
+	UCHAR			digest[80];
+	UCHAR			DlsPTK[80];
+	UCHAR			temp[64];
+	BOOLEAN			TimerCancelled;
+	CIPHER_KEY		PairwiseKey;
+
+
+	if (! pAd->CommonCfg.bDLSCapable)
+		return bSTAKeyFrame;
+
+	if (! INFRA_ON(pAd))
+		return bSTAKeyFrame;
+
+	if (! (pHeader->FC.SubType & 0x08))
+		return bSTAKeyFrame;
+
+	if (Len < LENGTH_802_11 + 6 + 2 + 2)
+		return bSTAKeyFrame;
+
+	pProto	= (PUCHAR)pHeader + LENGTH_802_11 + 2 + 6;	// QOS Control field , 0xAA 0xAA 0xAA 0x00 0x00 0x00
+	pAddr	= pHeader->Addr2;
+
+	// L2PAD bit on will pad 2 bytes at LLC
+	if (pRxD->L2PAD)
+	{
+		pProto += 2;
+	}
+
+	if (RTMPEqualMemory(EAPOL, pProto, 2) && (pAd->StaCfg.AuthMode >=  Ndis802_11AuthModeWPA))
+	{
+		pEap = (PEAPOL_PACKET) (pProto + 2);
+
+		DBGPRINT(RT_DEBUG_TRACE,("DLS - Sniff Len=%ld, DataLen=%d, KeyMic=%d, Install=%d, KeyAck=%d, Secure=%d, EKD_DL=%d, Error=%d, Request=%d\n", Len,
+			                                                             (LENGTH_802_11 + 6 + 2 + 2 + sizeof(EAPOL_PACKET) - MAX_LEN_OF_RSNIE + 16),
+			                                                             pEap->KeyDesc.KeyInfo.KeyMic,
+			                                                             pEap->KeyDesc.KeyInfo.Install,
+			                                                             pEap->KeyDesc.KeyInfo.KeyAck,
+			                                                             pEap->KeyDesc.KeyInfo.Secure,
+			                                                             pEap->KeyDesc.KeyInfo.EKD_DL,
+			                                                             pEap->KeyDesc.KeyInfo.Error,
+			                                                             pEap->KeyDesc.KeyInfo.Request));
+
+		if ((Len >= (LENGTH_802_11 + 6 + 2 + 2 + sizeof(EAPOL_PACKET) - MAX_LEN_OF_RSNIE + 16)) && pEap->KeyDesc.KeyInfo.KeyMic
+			&& pEap->KeyDesc.KeyInfo.Install && pEap->KeyDesc.KeyInfo.KeyAck && pEap->KeyDesc.KeyInfo.Secure
+			&& pEap->KeyDesc.KeyInfo.EKD_DL && !pEap->KeyDesc.KeyInfo.Error && !pEap->KeyDesc.KeyInfo.Request)
+		{
+			// First validate replay counter, only accept message with larger replay counter
+			// Let equal pass, some AP start with all zero replay counter
+			NdisZeroMemory(ZeroReplay, LEN_KEY_DESC_REPLAY);
+			if ((RTMPCompareMemory(pEap->KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY) != 1) &&
+				(RTMPCompareMemory(pEap->KeyDesc.ReplayCounter, ZeroReplay, LEN_KEY_DESC_REPLAY) != 0))
+				return bSTAKeyFrame;
+
+			//RTMPMoveMemory(pAd->StaCfg.ReplayCounter, pEap->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+			RTMPMoveMemory(pAd->StaCfg.DlsReplayCounter, pEap->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+			DBGPRINT(RT_DEBUG_TRACE,("DLS - Sniff replay counter (%02x-%02x-%02x-%02x-%02x-%02x-%02x-%02x) Len=%ld, KeyDataLen=%d\n",
+				pAd->StaCfg.ReplayCounter[0], pAd->StaCfg.ReplayCounter[1], pAd->StaCfg.ReplayCounter[2],
+				pAd->StaCfg.ReplayCounter[3], pAd->StaCfg.ReplayCounter[4],	pAd->StaCfg.ReplayCounter[5],
+				pAd->StaCfg.ReplayCounter[6], pAd->StaCfg.ReplayCounter[7], Len, pEap->KeyDesc.KeyData[1]));
+
+			// put these code segment to get the replay counter
+			if (pAd->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED)
+				return bSTAKeyFrame;
+
+			// Check MIC value
+			// Save the MIC and replace with zero
+			// use proprietary PTK
+			NdisZeroMemory(temp, 64);
+			NdisMoveMemory(temp, "IEEE802.11 WIRELESS ACCESS POINT", 32);
+			WpaCountPTK(pAd, temp, temp, pAd->CommonCfg.Bssid, temp, pAd->CurrentAddress, DlsPTK, LEN_PTK);
+
+			NdisMoveMemory(OldMic, pEap->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+			NdisZeroMemory(pEap->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+			if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+			{
+				// AES
+				HMAC_SHA1((PUCHAR) pEap, pEap->Body_Len[1] + 4, DlsPTK, LEN_EAP_MICK, digest);
+				NdisMoveMemory(Mic,	digest,	LEN_KEY_DESC_MIC);
+			}
+			else
+			{
+				hmac_md5(DlsPTK, LEN_EAP_MICK, (PUCHAR) pEap, pEap->Body_Len[1] + 4, Mic);
+			}
+
+			if (!NdisEqualMemory(OldMic, Mic, LEN_KEY_DESC_MIC))
+			{
+				DBGPRINT(RT_DEBUG_ERROR, ("MIC Different in Msg1 of STAKey handshake! \n"));
+				return bSTAKeyFrame;
+			}
+			else
+				DBGPRINT(RT_DEBUG_TRACE, ("MIC VALID in Msg1 of STAKey handshake! \n"));
+#if 1
+			if ((pEap->KeyDesc.KeyData[0] == 0xDD) && (pEap->KeyDesc.KeyData[2] == 0x00) && (pEap->KeyDesc.KeyData[3] == 0x0C)
+				&& (pEap->KeyDesc.KeyData[4] == 0x43) && (pEap->KeyDesc.KeyData[5] == 0x02))
+			{
+				pAddr			= pEap->KeyDesc.KeyData + 8;		// Tpe(1), Len(1), OUI(3), DataType(1), Reserved(2)
+				pSTAKey			= pEap->KeyDesc.KeyData + 14;	// Tpe(1), Len(1), OUI(3), DataType(1), Reserved(2), STAKey_Mac_Addr(6)
+
+				DBGPRINT(RT_DEBUG_TRACE,("DLS - Receive STAKey Message-1 from %02x:%02x:%02x:%02x:%02x:%02x Len=%ld, KeyDataLen=%d\n",
+					pAddr[0], pAddr[1], pAddr[2], pAddr[3], pAddr[4], pAddr[5], Len, pEap->KeyDesc.KeyData[1]));
+
+				bSTAKeyFrame = TRUE;
+			}
+#else
+			if ((pEap->KeyDesc.KeyData[0] == 0xDD) && (pEap->KeyDesc.KeyData[2] == 0x00) && (pEap->KeyDesc.KeyData[3] == 0x0F)
+				&& (pEap->KeyDesc.KeyData[4] == 0xAC) && (pEap->KeyDesc.KeyData[5] == 0x02))
+			{
+				pAddr			= pEap->KeyDesc.KeyData + 8;		// Tpe(1), Len(1), OUI(3), DataType(1), Reserved(2)
+				pSTAKey			= pEap->KeyDesc.KeyData + 14;	// Tpe(1), Len(1), OUI(3), DataType(1), Reserved(2), STAKey_Mac_Addr(6)
+
+				DBGPRINT(RT_DEBUG_TRACE,("DLS - Receive STAKey Message-1 from %02x:%02x:%02x:%02x:%02x:%02x Len=%d, KeyDataLen=%d\n",
+					pAddr[0], pAddr[1], pAddr[2], pAddr[3], pAddr[4], pAddr[5], Len, pEap->KeyDesc.KeyData[1]));
+
+				bSTAKeyFrame = TRUE;
+			}
+#endif
+
+		}
+		else if (Len >= (LENGTH_802_11 + 6 + 2 + 2 + sizeof(EAPOL_PACKET) - MAX_LEN_OF_RSNIE))
+		{
+#if 0
+			RTMPMoveMemory(pAd->StaCfg.ReplayCounter, pEap->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+#endif
+			RTMPMoveMemory(pAd->StaCfg.DlsReplayCounter, pEap->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+			DBGPRINT(RT_DEBUG_TRACE,("DLS - Sniff replay counter 2(%02x-%02x-%02x-%02x-%02x-%02x-%02x-%02x) Len=%ld, KeyDataLen=%d\n",
+				pAd->StaCfg.ReplayCounter[0], pAd->StaCfg.ReplayCounter[1], pAd->StaCfg.ReplayCounter[2],
+				pAd->StaCfg.ReplayCounter[3], pAd->StaCfg.ReplayCounter[4],	pAd->StaCfg.ReplayCounter[5],
+				pAd->StaCfg.ReplayCounter[6], pAd->StaCfg.ReplayCounter[7], Len, pEap->KeyDesc.KeyData[1]));
+
+		}
+	}
+
+	// If timeout value is equaled to zero, it means always not be timeout.
+	// update local dls table entry
+	for (i= 0; i < MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+	{
+		if (pAd->StaCfg.DLSEntry[i].Valid && MAC_ADDR_EQUAL(pAddr, pAd->StaCfg.DLSEntry[i].MacAddr))
+		{
+			if (bSTAKeyFrame)
+			{
+				PMAC_TABLE_ENTRY pEntry;
+
+				// STAKey frame, add pairwise key table
+				pAd->StaCfg.DLSEntry[i].Status = DLS_FINISH;
+				RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+
+				PairwiseKey.KeyLen = LEN_TKIP_EK;
+				NdisMoveMemory(PairwiseKey.Key, &pSTAKey[0], LEN_TKIP_EK);
+				NdisMoveMemory(PairwiseKey.TxMic, &pSTAKey[16], LEN_TKIP_RXMICK);
+				NdisMoveMemory(PairwiseKey.RxMic, &pSTAKey[24], LEN_TKIP_TXMICK);
+
+				PairwiseKey.CipherAlg = pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg;
+
+				pEntry = DlsEntryTableLookup(pAd, pAd->StaCfg.DLSEntry[i].MacAddr, TRUE);
+				//AsicAddKeyEntry(pAd, (USHORT)(i + 2), BSS0, 0, &PairwiseKey, TRUE, TRUE);	// reserve 0 for multicast, 1 for unicast
+				//AsicUpdateRxWCIDTable(pAd, (USHORT)(i + 2), pAddr);
+				// Add Pair-wise key to Asic
+#ifdef RT2870
+				{
+					RT_ADD_PAIRWISE_KEY_ENTRY KeyInfo;
+					COPY_MAC_ADDR(KeyInfo.MacAddr,pAd->StaCfg.DLSEntry[i].MacAddr);
+					KeyInfo.MacTabMatchWCID=pAd->StaCfg.DLSEntry[i].MacTabMatchWCID;
+					NdisMoveMemory(&KeyInfo.CipherKey,  &PairwiseKey,sizeof(CIPHER_KEY));
+					RTUSBEnqueueInternalCmd(pAd, RT_CMD_SET_KEY_TABLE, &KeyInfo, sizeof(RT_ADD_PAIRWISE_KEY_ENTRY));
+				}
+				{
+					PMAC_TABLE_ENTRY pDLSEntry;
+					pDLSEntry = DlsEntryTableLookup(pAd, pAd->StaCfg.DLSEntry[i].MacAddr, TRUE);
+					pDLSEntry->PairwiseKey.CipherAlg=PairwiseKey.CipherAlg;
+					RTUSBEnqueueInternalCmd(pAd, RT_CMD_SET_RX_WCID_TABLE, pDLSEntry, sizeof(MAC_TABLE_ENTRY));
+				}
+#endif // RT2870 //
+				NdisMoveMemory(&pEntry->PairwiseKey, &PairwiseKey, sizeof(CIPHER_KEY));
+				DBGPRINT(RT_DEBUG_TRACE,("DLS - Receive STAKey Message-1 (Peer STA MAC Address STAKey) \n"));
+
+				RTMPSendSTAKeyHandShake(pAd, pAd->StaCfg.DLSEntry[i].MacAddr);
+
+				DBGPRINT(RT_DEBUG_TRACE,("DLS - Finish STAKey handshake procedure (Initiator side)\n"));
+			}
+			else
+			{
+				// Data frame, update timeout value
+				if (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH)
+				{
+					pAd->StaCfg.DLSEntry[i].CountDownTimer = pAd->StaCfg.DLSEntry[i].TimeOut;
+					//AsicUpdateRxWCIDTable(pAd, (USHORT)(i + 2), pAddr);
+				}
+			}
+
+			bFindEntry = TRUE;
+		}
+	}
+
+	// update peer dls table entry
+	for (i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+	{
+		if (pAd->StaCfg.DLSEntry[i].Valid && MAC_ADDR_EQUAL(pAddr, pAd->StaCfg.DLSEntry[i].MacAddr))
+		{
+			if (bSTAKeyFrame)
+			{
+				PMAC_TABLE_ENTRY pEntry = NULL;
+
+				// STAKey frame, add pairwise key table, and send STAkey Msg-2
+				pAd->StaCfg.DLSEntry[i].Status = DLS_FINISH;
+				RTMPCancelTimer(&pAd->StaCfg.DLSEntry[i].Timer, &TimerCancelled);
+
+				PairwiseKey.KeyLen = LEN_TKIP_EK;
+				NdisMoveMemory(PairwiseKey.Key, &pSTAKey[0], LEN_TKIP_EK);
+				NdisMoveMemory(PairwiseKey.TxMic, &pSTAKey[16], LEN_TKIP_RXMICK);
+				NdisMoveMemory(PairwiseKey.RxMic, &pSTAKey[24], LEN_TKIP_TXMICK);
+
+				PairwiseKey.CipherAlg = pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg;
+
+				pEntry = DlsEntryTableLookup(pAd, pAd->StaCfg.DLSEntry[i].MacAddr, TRUE);
+				//AsicAddKeyEntry(pAd, (USHORT)(i + 2), BSS0, 0, &PairwiseKey, TRUE, TRUE);	// reserve 0 for multicast, 1 for unicast
+				//AsicUpdateRxWCIDTable(pAd, (USHORT)(i + 2), pAddr);
+				// Add Pair-wise key to Asic
+#ifdef RT2870
+				{
+					RT_ADD_PAIRWISE_KEY_ENTRY KeyInfo;
+					COPY_MAC_ADDR(KeyInfo.MacAddr,pAd->StaCfg.DLSEntry[i].MacAddr);
+					KeyInfo.MacTabMatchWCID=pAd->StaCfg.DLSEntry[i].MacTabMatchWCID;
+					NdisMoveMemory(&KeyInfo.CipherKey,  &PairwiseKey,sizeof(CIPHER_KEY));
+					RTUSBEnqueueInternalCmd(pAd, RT_CMD_SET_KEY_TABLE, &KeyInfo, sizeof(RT_ADD_PAIRWISE_KEY_ENTRY));
+				}
+				{
+					PMAC_TABLE_ENTRY pDLSEntry;
+					pDLSEntry = DlsEntryTableLookup(pAd, pAd->StaCfg.DLSEntry[i].MacAddr, TRUE);
+					pDLSEntry->PairwiseKey.CipherAlg=PairwiseKey.CipherAlg;
+					RTUSBEnqueueInternalCmd(pAd, RT_CMD_SET_RX_WCID_TABLE, pDLSEntry, sizeof(MAC_TABLE_ENTRY));
+				}
+#endif // RT2870 //
+				NdisMoveMemory(&pEntry->PairwiseKey, &PairwiseKey, sizeof(CIPHER_KEY));
+				DBGPRINT(RT_DEBUG_TRACE,("DLS - Receive STAKey Message-1 (Initiator STA MAC Address STAKey)\n"));
+
+				// If support WPA or WPA2, start STAKey hand shake,
+				// If failed hand shake, just tear down peer DLS
+				if (RTMPSendSTAKeyHandShake(pAd, pAddr) != NDIS_STATUS_SUCCESS)
+				{
+					MLME_DLS_REQ_STRUCT	MlmeDlsReq;
+					USHORT				reason = REASON_QOS_CIPHER_NOT_SUPPORT;
+
+					pAd->StaCfg.DLSEntry[i].Valid	= FALSE;
+					pAd->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+					DlsParmFill(pAd, &MlmeDlsReq, &pAd->StaCfg.DLSEntry[i], reason);
+					MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_TEAR_DOWN, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+				}
+				else
+				{
+					DBGPRINT(RT_DEBUG_TRACE,("DLS - Finish STAKey handshake procedure (Peer side)\n"));
+				}
+			}
+			else
+			{
+				// Data frame, update timeout value
+				if (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH)
+				{
+					pAd->StaCfg.DLSEntry[i].CountDownTimer = pAd->StaCfg.DLSEntry[i].TimeOut;
+				}
+			}
+
+			bFindEntry = TRUE;
+		}
+	}
+
+
+	return bSTAKeyFrame;
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Check if the frame can be sent through DLS direct link interface
+
+	Arguments:
+		pAd		Pointer	to adapter
+
+	Return Value:
+		DLS entry index
+
+	Note:
+
+	========================================================================
+*/
+INT	RTMPCheckDLSFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN  PUCHAR          pDA)
+{
+	INT rval = -1;
+	INT	i;
+
+	if (!pAd->CommonCfg.bDLSCapable)
+		return rval;
+
+	if (!INFRA_ON(pAd))
+		return rval;
+
+	do{
+		// check local dls table entry
+		for (i=0; i<MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+		{
+			if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH) &&
+				MAC_ADDR_EQUAL(pDA, pAd->StaCfg.DLSEntry[i].MacAddr))
+			{
+				rval = i;
+				break;
+			}
+		}
+
+		// check peer dls table entry
+		for (i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+		{
+			if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH) &&
+				MAC_ADDR_EQUAL(pDA, pAd->StaCfg.DLSEntry[i].MacAddr))
+			{
+				rval = i;
+				break;
+			}
+		}
+	} while (FALSE);
+
+	return rval;
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+VOID RTMPSendDLSTearDownFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN  PUCHAR          pDA)
+{
+	PUCHAR			pOutBuffer = NULL;
+	NDIS_STATUS		NStatus;
+	HEADER_802_11	DlsTearDownHdr;
+	ULONG			FrameLen = 0;
+	USHORT			Reason = REASON_QOS_QSTA_LEAVING_QBSS;
+	UCHAR			Category = CATEGORY_DLS;
+	UCHAR			Action = ACTION_DLS_TEARDOWN;
+	UCHAR			i = 0;
+
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS) ||
+		RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS))
+		return;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Send DLS TearDown Frame \n"));
+
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if (NStatus != NDIS_STATUS_SUCCESS)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("ASSOC - RTMPSendDLSTearDownFrame() allocate memory failed \n"));
+		return;
+	}
+
+	ActHeaderInit(pAd, &DlsTearDownHdr, pAd->CommonCfg.Bssid, pAd->CurrentAddress, pAd->CommonCfg.Bssid);
+	MakeOutgoingFrame(pOutBuffer,				&FrameLen,
+					sizeof(HEADER_802_11),		&DlsTearDownHdr,
+					1,							&Category,
+					1,							&Action,
+					6,							pDA,
+					6,							pAd->CurrentAddress,
+					2,							&Reason,
+					END_OF_ARGS);
+
+	MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	// Remove key in local dls table entry
+	for (i = 0; i < MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+	{
+		if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH)
+			&& MAC_ADDR_EQUAL(pDA, pAd->StaCfg.DLSEntry[i].MacAddr))
+		{
+			MacTableDeleteDlsEntry(pAd, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID, pAd->StaCfg.DLSEntry[i].MacAddr);
+		}
+	}
+
+	// Remove key in peer dls table entry
+	for (i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+	{
+		if (pAd->StaCfg.DLSEntry[i].Valid && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH)
+			&& MAC_ADDR_EQUAL(pDA, pAd->StaCfg.DLSEntry[i].MacAddr))
+		{
+			MacTableDeleteDlsEntry(pAd, pAd->StaCfg.DLSEntry[i].MacTabMatchWCID, pAd->StaCfg.DLSEntry[i].MacAddr);
+		}
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Send DLS TearDown Frame and remove key in (i=%d) \n", i));
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+NDIS_STATUS RTMPSendSTAKeyRequest(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pDA)
+{
+	UCHAR				Header802_3[14];
+	NDIS_STATUS			NStatus;
+	ULONG				FrameLen = 0;
+	EAPOL_PACKET		Packet;
+	UCHAR				Mic[16];
+	UCHAR				digest[80];
+	PUCHAR				pOutBuffer = NULL;
+	PNDIS_PACKET		pNdisPacket;
+	UCHAR				temp[64];
+	UCHAR				DlsPTK[80];
+
+	DBGPRINT(RT_DEBUG_TRACE,("DLS - RTMPSendSTAKeyRequest() to %02x:%02x:%02x:%02x:%02x:%02x\n", pDA[0], pDA[1], pDA[2], pDA[3], pDA[4], pDA[5]));
+
+	pAd->Sequence ++;
+	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
+
+	// Zero message body
+	NdisZeroMemory(&Packet, sizeof(Packet));
+	Packet.ProVer = EAPOL_VER;
+	Packet.ProType    = EAPOLKey;
+	Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE + 6 + MAC_ADDR_LEN;		// data field contain KDE andPeer MAC address
+
+	// STAKey Message is as EAPOL-Key(1,1,0,0,G/0,0,0, MIC, 0,Peer MAC KDE)
+	if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK))
+    {
+        Packet.KeyDesc.Type = WPA1_KEY_DESC;
+    }
+    else if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK))
+    {
+        Packet.KeyDesc.Type = WPA2_KEY_DESC;
+    }
+
+	// Key descriptor version
+	Packet.KeyDesc.KeyInfo.KeyDescVer =
+		(((pAd->StaCfg.PairCipher == Ndis802_11Encryption3Enabled) || (pAd->StaCfg.GroupCipher == Ndis802_11Encryption3Enabled)) ? (DESC_TYPE_AES) : (DESC_TYPE_TKIP));
+
+	Packet.KeyDesc.KeyInfo.KeyMic	= 1;
+	Packet.KeyDesc.KeyInfo.Secure	= 1;
+	Packet.KeyDesc.KeyInfo.Request	= 1;
+
+	Packet.KeyDesc.KeyDataLen[1]	= 12;
+
+	// use our own OUI to distinguish proprietary with standard.
+	Packet.KeyDesc.KeyData[0]		= 0xDD;
+	Packet.KeyDesc.KeyData[1]		= 0x0A;
+	Packet.KeyDesc.KeyData[2]		= 0x00;
+	Packet.KeyDesc.KeyData[3]		= 0x0C;
+	Packet.KeyDesc.KeyData[4]		= 0x43;
+	Packet.KeyDesc.KeyData[5]		= 0x03;
+	NdisMoveMemory(&Packet.KeyDesc.KeyData[6], pDA, MAC_ADDR_LEN);
+
+	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pAd->StaCfg.DlsReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// Allocate buffer for transmitting message
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);
+	if (NStatus	!= NDIS_STATUS_SUCCESS)
+		return NStatus;
+
+	// Prepare EAPOL frame for MIC calculation
+	// Be careful, only EAPOL frame is counted for MIC calculation
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+		              Packet.Body_Len[1] + 4,    &Packet,
+		              END_OF_ARGS);
+
+	// use proprietary PTK
+	NdisZeroMemory(temp, 64);
+	NdisMoveMemory(temp, "IEEE802.11 WIRELESS ACCESS POINT", 32);
+	WpaCountPTK(pAd, temp, temp, pAd->CommonCfg.Bssid, temp, pAd->CurrentAddress, DlsPTK, LEN_PTK);
+
+	// calculate MIC
+	if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+	{
+		// AES
+		NdisZeroMemory(digest,	sizeof(digest));
+		HMAC_SHA1(pOutBuffer, FrameLen, DlsPTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Packet.KeyDesc.KeyMic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{
+		NdisZeroMemory(Mic,	sizeof(Mic));
+		hmac_md5(DlsPTK, LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
+		NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
+	}
+
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+	                  sizeof(Header802_3),	Header802_3,
+		              Packet.Body_Len[1] + 4,	&Packet,
+		              END_OF_ARGS);
+
+	NStatus = RTMPAllocateNdisPacket(pAd, &pNdisPacket, NULL, 0, pOutBuffer, FrameLen);
+	if (NStatus == NDIS_STATUS_SUCCESS)
+	{
+		RTMP_SET_PACKET_WCID(pNdisPacket, BSSID_WCID);
+		STASendPacket(pAd, pNdisPacket);
+		RTMPDeQueuePacket(pAd, FALSE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+	}
+
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPSendSTAKeyRequest- Send STAKey request (NStatus=%x, FrameLen=%ld)\n", NStatus, FrameLen));
+
+	return NStatus;
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+NDIS_STATUS RTMPSendSTAKeyHandShake(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pDA)
+{
+	UCHAR				Header802_3[14];
+	NDIS_STATUS			NStatus;
+	ULONG				FrameLen = 0;
+	EAPOL_PACKET		Packet;
+	UCHAR				Mic[16];
+	UCHAR				digest[80];
+	PUCHAR				pOutBuffer = NULL;
+	PNDIS_PACKET		pNdisPacket;
+	UCHAR				temp[64];
+	UCHAR				DlsPTK[80];			// Due to dirver can not get PTK, use proprietary PTK
+
+	DBGPRINT(RT_DEBUG_TRACE,("DLS - RTMPSendSTAKeyHandShake() to %02x:%02x:%02x:%02x:%02x:%02x\n", pDA[0], pDA[1], pDA[2], pDA[3], pDA[4], pDA[5]));
+
+	pAd->Sequence ++;
+	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
+
+	// Zero message body
+	NdisZeroMemory(&Packet, sizeof(Packet));
+	Packet.ProVer = EAPOL_VER;
+	Packet.ProType    = EAPOLKey;
+	Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE + 6 + MAC_ADDR_LEN;		// data field contain KDE and Peer MAC address
+
+	// STAKey Message is as EAPOL-Key(1,1,0,0,G/0,0,0, MIC, 0,Peer MAC KDE)
+	if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK))
+    {
+        Packet.KeyDesc.Type = WPA1_KEY_DESC;
+    }
+    else if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) || (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK))
+    {
+        Packet.KeyDesc.Type = WPA2_KEY_DESC;
+    }
+
+	// Key descriptor version
+	Packet.KeyDesc.KeyInfo.KeyDescVer =
+		(((pAd->StaCfg.PairCipher == Ndis802_11Encryption3Enabled) || (pAd->StaCfg.GroupCipher == Ndis802_11Encryption3Enabled)) ? (DESC_TYPE_AES) : (DESC_TYPE_TKIP));
+
+	Packet.KeyDesc.KeyInfo.KeyMic	= 1;
+	Packet.KeyDesc.KeyInfo.Secure	= 1;
+
+	Packet.KeyDesc.KeyDataLen[1]	= 12;
+
+	// use our own OUI to distinguish proprietary with standard.
+	Packet.KeyDesc.KeyData[0]		= 0xDD;
+	Packet.KeyDesc.KeyData[1]		= 0x0A;
+	Packet.KeyDesc.KeyData[2]		= 0x00;
+	Packet.KeyDesc.KeyData[3]		= 0x0C;
+	Packet.KeyDesc.KeyData[4]		= 0x43;
+	Packet.KeyDesc.KeyData[5]		= 0x03;
+	NdisMoveMemory(&Packet.KeyDesc.KeyData[6], pDA, MAC_ADDR_LEN);
+
+	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pAd->StaCfg.DlsReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// Allocate buffer for transmitting message
+	NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);
+	if (NStatus	!= NDIS_STATUS_SUCCESS)
+		return NStatus;
+
+	// Prepare EAPOL frame for MIC calculation
+	// Be careful, only EAPOL frame is counted for MIC calculation
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+		              Packet.Body_Len[1] + 4,    &Packet,
+		              END_OF_ARGS);
+
+	// use proprietary PTK
+	NdisZeroMemory(temp, 64);
+	NdisMoveMemory(temp, "IEEE802.11 WIRELESS ACCESS POINT", 32);
+	WpaCountPTK(pAd, temp, temp, pAd->CommonCfg.Bssid, temp, pAd->CurrentAddress, DlsPTK, LEN_PTK);
+
+	// calculate MIC
+	if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+	{
+		// AES
+		NdisZeroMemory(digest,	sizeof(digest));
+		HMAC_SHA1(pOutBuffer, FrameLen, DlsPTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Packet.KeyDesc.KeyMic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{
+		NdisZeroMemory(Mic,	sizeof(Mic));
+		hmac_md5(DlsPTK, LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
+		NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
+	}
+
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+	                  sizeof(Header802_3),	Header802_3,
+		              Packet.Body_Len[1] + 4,	&Packet,
+		              END_OF_ARGS);
+
+	NStatus = RTMPAllocateNdisPacket(pAd, &pNdisPacket, NULL, 0, pOutBuffer, FrameLen);
+	if (NStatus == NDIS_STATUS_SUCCESS)
+	{
+		RTMP_SET_PACKET_WCID(pNdisPacket, BSSID_WCID);
+		STASendPacket(pAd, pNdisPacket);
+		RTMPDeQueuePacket(pAd, FALSE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+	}
+
+	MlmeFreeMemory(pAd, pOutBuffer);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPSendSTAKeyHandShake- Send STAKey Message-2 (NStatus=%x, FrameLen=%ld)\n", NStatus, FrameLen));
+
+	return NStatus;
+}
+
+VOID DlsTimeoutAction(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+	MLME_DLS_REQ_STRUCT		MlmeDlsReq;
+	USHORT					reason;
+	PRT_802_11_DLS			pDLS = (PRT_802_11_DLS)FunctionContext;
+	PRTMP_ADAPTER			pAd = pDLS->pAd;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("DlsTimeout - Tear down DLS links (%02x:%02x:%02x:%02x:%02x:%02x)\n",
+		pDLS->MacAddr[0], pDLS->MacAddr[1], pDLS->MacAddr[2], pDLS->MacAddr[3], pDLS->MacAddr[4], pDLS->MacAddr[5]));
+
+	if ((pDLS) && (pDLS->Valid))
+	{
+		reason			= REASON_QOS_REQUEST_TIMEOUT;
+		pDLS->Valid		= FALSE;
+		pDLS->Status	= DLS_NONE;
+		DlsParmFill(pAd, &MlmeDlsReq, pDLS, reason);
+		MlmeEnqueue(pAd, DLS_STATE_MACHINE, MT2_MLME_DLS_TEAR_DOWN, sizeof(MLME_DLS_REQ_STRUCT), &MlmeDlsReq);
+		RT28XX_MLME_HANDLER(pAd);
+	}
+}
+
+/*
+================================================================
+Description : because DLS and CLI share the same WCID table in ASIC.
+Mesh entry also insert to pAd->MacTab.content[].  Such is marked as ValidAsDls = TRUE.
+Also fills the pairwise key.
+Because front MAX_AID_BA entries have direct mapping to BAEntry, which is only used as CLI. So we insert Dls
+from index MAX_AID_BA.
+================================================================
+*/
+MAC_TABLE_ENTRY *MacTableInsertDlsEntry(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR	pAddr,
+	IN  UINT	DlsEntryIdx)
+{
+	PMAC_TABLE_ENTRY pEntry = NULL;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("====> MacTableInsertDlsEntry\n"));
+	// if FULL, return
+	if (pAd->MacTab.Size >= MAX_LEN_OF_MAC_TABLE)
+		return NULL;
+
+	do
+	{
+		if((pEntry = DlsEntryTableLookup(pAd, pAddr, TRUE)) != NULL)
+			break;
+
+		// allocate one MAC entry
+		pEntry = MacTableInsertEntry(pAd, pAddr, DlsEntryIdx + MIN_NET_DEVICE_FOR_DLS, TRUE);
+		if (pEntry)
+		{
+			pAd->StaCfg.DLSEntry[DlsEntryIdx].MacTabMatchWCID = pEntry->Aid;
+			pEntry->MatchDlsEntryIdx = DlsEntryIdx;
+			pEntry->AuthMode = pAd->StaCfg.AuthMode;
+			pEntry->WepStatus = pAd->StaCfg.WepStatus;
+			pEntry->PortSecured = WPA_802_1X_PORT_SECURED;
+
+			DBGPRINT(RT_DEBUG_TRACE, ("MacTableInsertDlsEntry - allocate entry #%d, Total= %d\n",pEntry->Aid, pAd->MacTab.Size));
+
+			// If legacy WEP is used, set pair-wise cipherAlg into WCID attribute table for this entry
+			if ((pEntry->ValidAsDls) && (pEntry->WepStatus == Ndis802_11WEPEnabled))
+			{
+				UCHAR KeyIdx = 0;
+				UCHAR CipherAlg = 0;
+
+				KeyIdx	= pAd->StaCfg.DefaultKeyId;
+
+				CipherAlg 	= pAd->SharedKey[BSS0][KeyIdx].CipherAlg;
+
+				RTMPAddWcidAttributeEntry(pAd,
+											BSS0,
+											pAd->StaCfg.DefaultKeyId,
+											pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
+											pEntry);
+			}
+
+			break;
+		}
+	} while(FALSE);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<==== MacTableInsertDlsEntry\n"));
+
+	return pEntry;
+}
+
+
+/*
+	==========================================================================
+	Description:
+		Delete all Mesh Entry in pAd->MacTab
+	==========================================================================
+ */
+BOOLEAN MacTableDeleteDlsEntry(
+	IN PRTMP_ADAPTER pAd,
+	IN USHORT wcid,
+	IN PUCHAR pAddr)
+{
+	DBGPRINT(RT_DEBUG_TRACE, ("====> MacTableDeleteDlsEntry\n"));
+
+	if (!VALID_WCID(wcid))
+		return FALSE;
+
+	MacTableDeleteEntry(pAd, wcid, pAddr);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<==== MacTableDeleteDlsEntry\n"));
+
+	return TRUE;
+}
+
+MAC_TABLE_ENTRY *DlsEntryTableLookup(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR	pAddr,
+	IN BOOLEAN	bResetIdelCount)
+{
+	ULONG HashIdx;
+	MAC_TABLE_ENTRY *pEntry = NULL;
+
+	RTMP_SEM_LOCK(&pAd->MacTabLock);
+	HashIdx = MAC_ADDR_HASH_INDEX(pAddr);
+	pEntry = pAd->MacTab.Hash[HashIdx];
+
+	while (pEntry)
+	{
+		if ((pEntry->ValidAsDls == TRUE)
+			&& MAC_ADDR_EQUAL(pEntry->Addr, pAddr))
+		{
+			if(bResetIdelCount)
+				pEntry->NoDataIdleCount = 0;
+			break;
+		}
+		else
+			pEntry = pEntry->pNext;
+	}
+
+	RTMP_SEM_UNLOCK(&pAd->MacTabLock);
+	return pEntry;
+}
+
+MAC_TABLE_ENTRY *DlsEntryTableLookupByWcid(
+	IN PRTMP_ADAPTER pAd,
+	IN UCHAR	wcid,
+	IN PUCHAR	pAddr,
+	IN BOOLEAN	bResetIdelCount)
+{
+	ULONG DLsIndex;
+	PMAC_TABLE_ENTRY pCurEntry = NULL;
+	PMAC_TABLE_ENTRY pEntry = NULL;
+
+	if (!VALID_WCID(wcid))
+		return NULL;
+
+	RTMP_SEM_LOCK(&pAd->MacTabLock);
+
+	do
+	{
+		pCurEntry = &pAd->MacTab.Content[wcid];
+
+		DLsIndex = 0xff;
+		if ((pCurEntry) && (pCurEntry->ValidAsDls== TRUE))
+		{
+			DLsIndex = pCurEntry->MatchDlsEntryIdx;
+		}
+
+		if (DLsIndex == 0xff)
+			break;
+
+		if (MAC_ADDR_EQUAL(pCurEntry->Addr, pAddr))
+		{
+			if(bResetIdelCount)
+				pCurEntry->NoDataIdleCount = 0;
+			pEntry = pCurEntry;
+			break;
+		}
+	} while(FALSE);
+
+	RTMP_SEM_UNLOCK(&pAd->MacTabLock);
+
+	return pEntry;
+}
+
+INT Set_DlsEntryInfo_Display_Proc(
+	IN PRTMP_ADAPTER pAd,
+	IN PUCHAR arg)
+{
+	INT i;
+
+	printk("\n%-19s%-8s\n", "MAC", "TIMEOUT\n");
+	for (i=0; i<MAX_NUM_OF_DLS_ENTRY; i++)
+	{
+		if ((pAd->StaCfg.DLSEntry[i].Valid) && (pAd->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+		{
+			PMAC_TABLE_ENTRY pEntry = &pAd->MacTab.Content[pAd->StaCfg.DLSEntry[i].MacTabMatchWCID];
+
+			printk("%02x:%02x:%02x:%02x:%02x:%02x  ",
+				pAd->StaCfg.DLSEntry[i].MacAddr[0], pAd->StaCfg.DLSEntry[i].MacAddr[1], pAd->StaCfg.DLSEntry[i].MacAddr[2],
+				pAd->StaCfg.DLSEntry[i].MacAddr[3], pAd->StaCfg.DLSEntry[i].MacAddr[4], pAd->StaCfg.DLSEntry[i].MacAddr[5]);
+			printk("%-8d\n", pAd->StaCfg.DLSEntry[i].TimeOut);
+
+			printk("\n");
+			printk("\n%-19s%-4s%-4s%-4s%-4s%-7s%-7s%-7s","MAC", "AID", "BSS", "PSM", "WMM", "RSSI0", "RSSI1", "RSSI2");
+#ifdef DOT11_N_SUPPORT
+			printk("%-8s%-10s%-6s%-6s%-6s%-6s", "MIMOPS", "PhMd", "BW", "MCS", "SGI", "STBC");
+#endif // DOT11_N_SUPPORT //
+			printk("\n%02X:%02X:%02X:%02X:%02X:%02X  ",
+				pEntry->Addr[0], pEntry->Addr[1], pEntry->Addr[2],
+				pEntry->Addr[3], pEntry->Addr[4], pEntry->Addr[5]);
+			printk("%-4d", (int)pEntry->Aid);
+			printk("%-4d", (int)pEntry->apidx);
+			printk("%-4d", (int)pEntry->PsMode);
+			printk("%-4d", (int)CLIENT_STATUS_TEST_FLAG(pEntry, fCLIENT_STATUS_WMM_CAPABLE));
+			printk("%-7d", pEntry->RssiSample.AvgRssi0);
+			printk("%-7d", pEntry->RssiSample.AvgRssi1);
+			printk("%-7d", pEntry->RssiSample.AvgRssi2);
+#ifdef DOT11_N_SUPPORT
+			printk("%-8d", (int)pEntry->MmpsMode);
+			printk("%-10s", GetPhyMode(pEntry->HTPhyMode.field.MODE));
+			printk("%-6s", GetBW(pEntry->HTPhyMode.field.BW));
+			printk("%-6d", pEntry->HTPhyMode.field.MCS);
+			printk("%-6d", pEntry->HTPhyMode.field.ShortGI);
+			printk("%-6d", pEntry->HTPhyMode.field.STBC);
+#endif // DOT11_N_SUPPORT //
+			printk("%-10d, %d, %d%%\n", pEntry->DebugFIFOCount, pEntry->DebugTxCount,
+						(pEntry->DebugTxCount) ? ((pEntry->DebugTxCount-pEntry->DebugFIFOCount)*100/pEntry->DebugTxCount) : 0);
+			printk("\n");
+
+		}
+	}
+
+	return TRUE;
+}
+
+INT	Set_DlsAddEntry_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+    UCHAR	mac[MAC_ADDR_LEN];
+	USHORT	Timeout;
+	char *token, sepValue[] = ":", DASH = '-';
+	INT i;
+    RT_802_11_DLS	Dls;
+
+    if(strlen(arg) < 19)  //Mac address acceptable format 01:02:03:04:05:06 length 17 plus the "-" and timeout value in decimal format.
+		return FALSE;
+
+	token = strchr(arg, DASH);
+	if ((token != NULL) && (strlen(token)>1))
+	{
+		Timeout = simple_strtol((token+1), 0, 10);
+
+		*token = '\0';
+		for (i = 0, token = rstrtok(arg, &sepValue[0]); token; token = rstrtok(NULL, &sepValue[0]), i++)
+		{
+			if((strlen(token) != 2) || (!isxdigit(*token)) || (!isxdigit(*(token+1))))
+				return FALSE;
+			AtoH(token, (PUCHAR)(&mac[i]), 1);
+		}
+		if(i != 6)
+			return FALSE;
+
+	    printk("\n%02x:%02x:%02x:%02x:%02x:%02x-%d", mac[0], mac[1],
+	           mac[2], mac[3], mac[4], mac[5], (int)Timeout);
+
+		NdisZeroMemory(&Dls, sizeof(RT_802_11_DLS));
+		Dls.TimeOut = Timeout;
+		COPY_MAC_ADDR(Dls.MacAddr, mac);
+		Dls.Valid = 1;
+
+		MlmeEnqueue(pAd,
+					MLME_CNTL_STATE_MACHINE,
+					RT_OID_802_11_SET_DLS_PARAM,
+					sizeof(RT_802_11_DLS),
+					&Dls);
+
+		return TRUE;
+	}
+
+	return FALSE;
+
+}
+
+INT	Set_DlsTearDownEntry_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	UCHAR			macAddr[MAC_ADDR_LEN];
+	CHAR			*value;
+	INT				i;
+	RT_802_11_DLS	Dls;
+
+	if(strlen(arg) != 17)  //Mac address acceptable format 01:02:03:04:05:06 length 17
+		return FALSE;
+
+	for (i=0, value = rstrtok(arg,":"); value; value = rstrtok(NULL,":"))
+	{
+		if((strlen(value) != 2) || (!isxdigit(*value)) || (!isxdigit(*(value+1))) )
+			return FALSE;  //Invalid
+
+		AtoH(value, &macAddr[i++], 2);
+	}
+
+	printk("\n%02x:%02x:%02x:%02x:%02x:%02x", macAddr[0], macAddr[1],
+	           macAddr[2], macAddr[3], macAddr[4], macAddr[5]);
+
+	NdisZeroMemory(&Dls, sizeof(RT_802_11_DLS));
+	COPY_MAC_ADDR(Dls.MacAddr, macAddr);
+	Dls.Valid = 0;
+
+	MlmeEnqueue(pAd,
+				MLME_CNTL_STATE_MACHINE,
+				RT_OID_802_11_SET_DLS_PARAM,
+				sizeof(RT_802_11_DLS),
+				&Dls);
+
+	return TRUE;
+}
+
diff --git a/drivers/staging/rt2870/sta/rtmp_data.c b/drivers/staging/rt2870/sta/rtmp_data.c
new file mode 100644
index 00000000000..9942ecaf5e2
--- /dev/null
+++ b/drivers/staging/rt2870/sta/rtmp_data.c
@@ -0,0 +1,2619 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	rtmp_data.c
+
+	Abstract:
+	Data path subroutines
+
+	Revision History:
+	Who 		When			What
+	--------	----------		----------------------------------------------
+	John		      Aug/17/04		major modification for RT2561/2661
+	Jan Lee	      Mar/17/06		major modification for RT2860 New Ring Design
+*/
+#include "../rt_config.h"
+
+
+VOID STARxEAPOLFrameIndicate(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+	PRT28XX_RXD_STRUC	pRxD = &(pRxBlk->RxD);
+	PRXWI_STRUC		pRxWI = pRxBlk->pRxWI;
+	UCHAR			*pTmpBuf;
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+	if (pAd->StaCfg.WpaSupplicantUP)
+	{
+		// All EAPoL frames have to pass to upper layer (ex. WPA_SUPPLICANT daemon)
+		// TBD : process fragmented EAPol frames
+		{
+			// In 802.1x mode, if the received frame is EAP-SUCCESS packet, turn on the PortSecured variable
+			if ( pAd->StaCfg.IEEE8021X == TRUE &&
+				 (EAP_CODE_SUCCESS == WpaCheckEapCode(pAd, pRxBlk->pData, pRxBlk->DataSize, LENGTH_802_1_H)))
+			{
+				PUCHAR	Key;
+				UCHAR 	CipherAlg;
+				int     idx = 0;
+
+				DBGPRINT_RAW(RT_DEBUG_TRACE, ("Receive EAP-SUCCESS Packet\n"));
+				//pAd->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+				STA_PORT_SECURED(pAd);
+
+                if (pAd->StaCfg.IEEE8021x_required_keys == FALSE)
+                {
+                    idx = pAd->StaCfg.DesireSharedKeyId;
+                    CipherAlg = pAd->StaCfg.DesireSharedKey[idx].CipherAlg;
+					Key = pAd->StaCfg.DesireSharedKey[idx].Key;
+
+                    if (pAd->StaCfg.DesireSharedKey[idx].KeyLen > 0)
+    				{
+#ifdef RT2870
+						union
+						{
+							char buf[sizeof(NDIS_802_11_WEP)+MAX_LEN_OF_KEY- 1];
+							NDIS_802_11_WEP keyinfo;
+						}  WepKey;
+						int len;
+
+
+						NdisZeroMemory(&WepKey, sizeof(WepKey));
+						len =pAd->StaCfg.DesireSharedKey[idx].KeyLen;
+
+						NdisMoveMemory(WepKey.keyinfo.KeyMaterial,
+							pAd->StaCfg.DesireSharedKey[idx].Key,
+							pAd->StaCfg.DesireSharedKey[idx].KeyLen);
+
+						WepKey.keyinfo.KeyIndex = 0x80000000 + idx;
+						WepKey.keyinfo.KeyLength = len;
+						pAd->SharedKey[BSS0][idx].KeyLen =(UCHAR) (len <= 5 ? 5 : 13);
+
+						pAd->IndicateMediaState = NdisMediaStateConnected;
+						pAd->ExtraInfo = GENERAL_LINK_UP;
+						// need to enqueue cmd to thread
+						RTUSBEnqueueCmdFromNdis(pAd, OID_802_11_ADD_WEP, TRUE, &WepKey, sizeof(WepKey.keyinfo) + len - 1);
+#endif // RT2870 //
+						// For Preventing ShardKey Table is cleared by remove key procedure.
+    					pAd->SharedKey[BSS0][idx].CipherAlg = CipherAlg;
+						pAd->SharedKey[BSS0][idx].KeyLen = pAd->StaCfg.DesireSharedKey[idx].KeyLen;
+						NdisMoveMemory(pAd->SharedKey[BSS0][idx].Key,
+									   pAd->StaCfg.DesireSharedKey[idx].Key,
+									   pAd->StaCfg.DesireSharedKey[idx].KeyLen);
+    				}
+				}
+			}
+
+			Indicate_Legacy_Packet(pAd, pRxBlk, FromWhichBSSID);
+			return;
+		}
+	}
+	else
+#endif // WPA_SUPPLICANT_SUPPORT //
+	{
+		// Special DATA frame that has to pass to MLME
+		//	 1. Cisco Aironet frames for CCX2. We need pass it to MLME for special process
+		//	 2. EAPOL handshaking frames when driver supplicant enabled, pass to MLME for special process
+		{
+			pTmpBuf = pRxBlk->pData - LENGTH_802_11;
+			NdisMoveMemory(pTmpBuf, pRxBlk->pHeader, LENGTH_802_11);
+			REPORT_MGMT_FRAME_TO_MLME(pAd, pRxWI->WirelessCliID, pTmpBuf, pRxBlk->DataSize + LENGTH_802_11, pRxWI->RSSI0, pRxWI->RSSI1, pRxWI->RSSI2, pRxD->PlcpSignal);
+			DBGPRINT_RAW(RT_DEBUG_TRACE, ("!!! report EAPOL/AIRONET DATA to MLME (len=%d) !!!\n", pRxBlk->DataSize));
+		}
+	}
+
+	RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+	return;
+
+}
+
+VOID STARxDataFrameAnnounce(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry,
+	IN	RX_BLK			*pRxBlk,
+	IN	UCHAR			FromWhichBSSID)
+{
+
+	// non-EAP frame
+	if (!RTMPCheckWPAframe(pAd, pEntry, pRxBlk->pData, pRxBlk->DataSize, FromWhichBSSID))
+	{
+		{
+			// drop all non-EAP DATA frame before
+			// this client's Port-Access-Control is secured
+			if (pRxBlk->pHeader->FC.Wep)
+			{
+				// unsupported cipher suite
+				if (pAd->StaCfg.WepStatus == Ndis802_11EncryptionDisabled)
+				{
+					// release packet
+					RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+					return;
+				}
+			}
+			else
+			{
+				// encryption in-use but receive a non-EAPOL clear text frame, drop it
+				if ((pAd->StaCfg.WepStatus != Ndis802_11EncryptionDisabled) &&
+					(pAd->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
+				{
+					// release packet
+					RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+					return;
+				}
+			}
+		}
+		RX_BLK_CLEAR_FLAG(pRxBlk, fRX_EAP);
+		if (!RX_BLK_TEST_FLAG(pRxBlk, fRX_ARALINK))
+		{
+			// Normal legacy, AMPDU or AMSDU
+			CmmRxnonRalinkFrameIndicate(pAd, pRxBlk, FromWhichBSSID);
+
+		}
+		else
+		{
+			// ARALINK
+			CmmRxRalinkFrameIndicate(pAd, pEntry, pRxBlk, FromWhichBSSID);
+		}
+#ifdef QOS_DLS_SUPPORT
+		RX_BLK_CLEAR_FLAG(pRxBlk, fRX_DLS);
+#endif // QOS_DLS_SUPPORT //
+	}
+	else
+	{
+		RX_BLK_SET_FLAG(pRxBlk, fRX_EAP);
+#ifdef DOT11_N_SUPPORT
+		if (RX_BLK_TEST_FLAG(pRxBlk, fRX_AMPDU) && (pAd->CommonCfg.bDisableReordering == 0))
+		{
+			Indicate_AMPDU_Packet(pAd, pRxBlk, FromWhichBSSID);
+		}
+		else
+#endif // DOT11_N_SUPPORT //
+		{
+			// Determin the destination of the EAP frame
+			//  to WPA state machine or upper layer
+			STARxEAPOLFrameIndicate(pAd, pEntry, pRxBlk, FromWhichBSSID);
+		}
+	}
+}
+
+
+// For TKIP frame, calculate the MIC value
+BOOLEAN STACheckTkipMICValue(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MAC_TABLE_ENTRY	*pEntry,
+	IN	RX_BLK			*pRxBlk)
+{
+	PHEADER_802_11	pHeader = pRxBlk->pHeader;
+	UCHAR			*pData = pRxBlk->pData;
+	USHORT			DataSize = pRxBlk->DataSize;
+	UCHAR			UserPriority = pRxBlk->UserPriority;
+	PCIPHER_KEY		pWpaKey;
+	UCHAR			*pDA, *pSA;
+
+	pWpaKey = &pAd->SharedKey[BSS0][pRxBlk->pRxWI->KeyIndex];
+
+	pDA = pHeader->Addr1;
+	if (RX_BLK_TEST_FLAG(pRxBlk, fRX_INFRA))
+	{
+		pSA = pHeader->Addr3;
+	}
+	else
+	{
+		pSA = pHeader->Addr2;
+	}
+
+	if (RTMPTkipCompareMICValue(pAd,
+								pData,
+								pDA,
+								pSA,
+								pWpaKey->RxMic,
+								UserPriority,
+								DataSize) == FALSE)
+	{
+		DBGPRINT_RAW(RT_DEBUG_ERROR,("Rx MIC Value error 2\n"));
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+		if (pAd->StaCfg.WpaSupplicantUP)
+		{
+			WpaSendMicFailureToWpaSupplicant(pAd, (pWpaKey->Type == PAIRWISEKEY) ? TRUE : FALSE);
+		}
+		else
+#endif // WPA_SUPPLICANT_SUPPORT //
+		{
+			RTMPReportMicError(pAd, pWpaKey);
+		}
+
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pRxBlk->pRxPacket, NDIS_STATUS_FAILURE);
+		return FALSE;
+	}
+
+	return TRUE;
+}
+
+
+//
+// All Rx routines use RX_BLK structure to hande rx events
+// It is very important to build pRxBlk attributes
+//  1. pHeader pointer to 802.11 Header
+//  2. pData pointer to payload including LLC (just skip Header)
+//  3. set payload size including LLC to DataSize
+//  4. set some flags with RX_BLK_SET_FLAG()
+//
+VOID STAHandleRxDataFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk)
+{
+	PRT28XX_RXD_STRUC				pRxD = &(pRxBlk->RxD);
+	PRXWI_STRUC						pRxWI = pRxBlk->pRxWI;
+	PHEADER_802_11					pHeader = pRxBlk->pHeader;
+	PNDIS_PACKET					pRxPacket = pRxBlk->pRxPacket;
+	BOOLEAN 						bFragment = FALSE;
+	MAC_TABLE_ENTRY	    			*pEntry = NULL;
+	UCHAR							FromWhichBSSID = BSS0;
+	UCHAR                           UserPriority = 0;
+
+	{
+		// before LINK UP, all DATA frames are rejected
+		if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+		{
+			// release packet
+			RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+			return;
+		}
+
+#ifdef QOS_DLS_SUPPORT
+		//if ((pHeader->FC.FrDs == 0) && (pHeader->FC.ToDs == 0))
+		if (RTMPRcvFrameDLSCheck(pAd, pHeader, pRxWI->MPDUtotalByteCount, pRxD))
+		{
+			return;
+		}
+#endif // QOS_DLS_SUPPORT //
+
+		// Drop not my BSS frames
+		if (pRxD->MyBss == 0)
+		{
+			{
+				// release packet
+				RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+				return;
+			}
+		}
+
+		pAd->RalinkCounters.RxCountSinceLastNULL++;
+		if (pAd->CommonCfg.bAPSDCapable && pAd->CommonCfg.APEdcaParm.bAPSDCapable && (pHeader->FC.SubType & 0x08))
+		{
+			UCHAR *pData;
+			DBGPRINT(RT_DEBUG_TRACE,("bAPSDCapable\n"));
+
+			// Qos bit 4
+			pData = (PUCHAR)pHeader + LENGTH_802_11;
+			if ((*pData >> 4) & 0x01)
+			{
+				DBGPRINT(RT_DEBUG_TRACE,("RxDone- Rcv EOSP frame, driver may fall into sleep\n"));
+				pAd->CommonCfg.bInServicePeriod = FALSE;
+
+				// Force driver to fall into sleep mode when rcv EOSP frame
+				if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+				{
+					USHORT  TbttNumToNextWakeUp;
+					USHORT  NextDtim = pAd->StaCfg.DtimPeriod;
+					ULONG   Now;
+
+					NdisGetSystemUpTime(&Now);
+					NextDtim -= (USHORT)(Now - pAd->StaCfg.LastBeaconRxTime)/pAd->CommonCfg.BeaconPeriod;
+
+					TbttNumToNextWakeUp = pAd->StaCfg.DefaultListenCount;
+					if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_RECEIVE_DTIM) && (TbttNumToNextWakeUp > NextDtim))
+						TbttNumToNextWakeUp = NextDtim;
+
+					MlmeSetPsmBit(pAd, PWR_SAVE);
+					// if WMM-APSD is failed, try to disable following line
+					AsicSleepThenAutoWakeup(pAd, TbttNumToNextWakeUp);
+				}
+			}
+
+			if ((pHeader->FC.MoreData) && (pAd->CommonCfg.bInServicePeriod))
+			{
+				DBGPRINT(RT_DEBUG_TRACE,("Sending another trigger frame when More Data bit is set to 1\n"));
+			}
+		}
+
+		// Drop NULL, CF-ACK(no data), CF-POLL(no data), and CF-ACK+CF-POLL(no data) data frame
+		if ((pHeader->FC.SubType & 0x04)) // bit 2 : no DATA
+		{
+			// release packet
+			RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+			return;
+		}
+
+	    // Drop not my BSS frame (we can not only check the MyBss bit in RxD)
+#ifdef QOS_DLS_SUPPORT
+	    if (!pAd->CommonCfg.bDLSCapable)
+	    {
+#endif // QOS_DLS_SUPPORT //
+		if (INFRA_ON(pAd))
+		{
+			// Infrastructure mode, check address 2 for BSSID
+			if (!RTMPEqualMemory(&pHeader->Addr2, &pAd->CommonCfg.Bssid, 6))
+			{
+				// Receive frame not my BSSID
+	            // release packet
+	            RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+				return;
+			}
+		}
+		else	// Ad-Hoc mode or Not associated
+		{
+			// Ad-Hoc mode, check address 3 for BSSID
+			if (!RTMPEqualMemory(&pHeader->Addr3, &pAd->CommonCfg.Bssid, 6))
+			{
+				// Receive frame not my BSSID
+	            // release packet
+	            RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+				return;
+			}
+		}
+#ifdef QOS_DLS_SUPPORT
+	    }
+#endif // QOS_DLS_SUPPORT //
+
+		//
+		// find pEntry
+		//
+		if (pRxWI->WirelessCliID < MAX_LEN_OF_MAC_TABLE)
+		{
+			pEntry = &pAd->MacTab.Content[pRxWI->WirelessCliID];
+		}
+		else
+		{
+			// 1. release packet if infra mode
+			// 2. new a pEntry if ad-hoc mode
+			RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+			return;
+		}
+
+		// infra or ad-hoc
+		if (INFRA_ON(pAd))
+		{
+			RX_BLK_SET_FLAG(pRxBlk, fRX_INFRA);
+#ifdef QOS_DLS_SUPPORT
+			if ((pHeader->FC.FrDs == 0) && (pHeader->FC.ToDs == 0))
+				RX_BLK_SET_FLAG(pRxBlk, fRX_DLS);
+			else
+#endif // QOS_DLS_SUPPORT //
+			ASSERT(pRxWI->WirelessCliID == BSSID_WCID);
+		}
+
+		// check Atheros Client
+		if ((pEntry->bIAmBadAtheros == FALSE) &&  (pRxD->AMPDU == 1) && (pHeader->FC.Retry ))
+		{
+			pEntry->bIAmBadAtheros = TRUE;
+			pAd->CommonCfg.IOTestParm.bCurrentAtheros = TRUE;
+			pAd->CommonCfg.IOTestParm.bLastAtheros = TRUE;
+			if (!STA_AES_ON(pAd))
+			{
+				AsicUpdateProtect(pAd, 8, ALLN_SETPROTECT, TRUE, FALSE);
+			}
+		}
+	}
+
+	pRxBlk->pData = (UCHAR *)pHeader;
+
+	//
+	// update RxBlk->pData, DataSize
+	// 802.11 Header, QOS, HTC, Hw Padding
+	//
+
+	// 1. skip 802.11 HEADER
+	{
+		pRxBlk->pData += LENGTH_802_11;
+		pRxBlk->DataSize -= LENGTH_802_11;
+	}
+
+	// 2. QOS
+	if (pHeader->FC.SubType & 0x08)
+	{
+		RX_BLK_SET_FLAG(pRxBlk, fRX_QOS);
+		UserPriority = *(pRxBlk->pData) & 0x0f;
+		// bit 7 in QoS Control field signals the HT A-MSDU format
+		if ((*pRxBlk->pData) & 0x80)
+		{
+			RX_BLK_SET_FLAG(pRxBlk, fRX_AMSDU);
+		}
+
+		// skip QOS contorl field
+		pRxBlk->pData += 2;
+		pRxBlk->DataSize -=2;
+	}
+	pRxBlk->UserPriority = UserPriority;
+
+	// 3. Order bit: A-Ralink or HTC+
+	if (pHeader->FC.Order)
+	{
+#ifdef AGGREGATION_SUPPORT
+		if ((pRxWI->PHYMODE <= MODE_OFDM) && (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED)))
+		{
+			RX_BLK_SET_FLAG(pRxBlk, fRX_ARALINK);
+		}
+		else
+#endif
+		{
+#ifdef DOT11_N_SUPPORT
+			RX_BLK_SET_FLAG(pRxBlk, fRX_HTC);
+			// skip HTC contorl field
+			pRxBlk->pData += 4;
+			pRxBlk->DataSize -= 4;
+#endif // DOT11_N_SUPPORT //
+		}
+	}
+
+	// 4. skip HW padding
+	if (pRxD->L2PAD)
+	{
+		// just move pData pointer
+		// because DataSize excluding HW padding
+		RX_BLK_SET_FLAG(pRxBlk, fRX_PAD);
+		pRxBlk->pData += 2;
+	}
+
+#ifdef DOT11_N_SUPPORT
+	if (pRxD->BA)
+	{
+		RX_BLK_SET_FLAG(pRxBlk, fRX_AMPDU);
+	}
+#endif // DOT11_N_SUPPORT //
+
+
+	//
+	// Case I  Process Broadcast & Multicast data frame
+	//
+	if (pRxD->Bcast || pRxD->Mcast)
+	{
+		INC_COUNTER64(pAd->WlanCounters.MulticastReceivedFrameCount);
+
+		// Drop Mcast/Bcast frame with fragment bit on
+		if (pHeader->FC.MoreFrag)
+		{
+			// release packet
+			RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+			return;
+		}
+
+		// Filter out Bcast frame which AP relayed for us
+		if (pHeader->FC.FrDs && MAC_ADDR_EQUAL(pHeader->Addr3, pAd->CurrentAddress))
+		{
+			// release packet
+			RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+			return;
+		}
+
+		Indicate_Legacy_Packet(pAd, pRxBlk, FromWhichBSSID);
+		return;
+	}
+	else if (pRxD->U2M)
+	{
+		pAd->LastRxRate = (USHORT)((pRxWI->MCS) + (pRxWI->BW <<7) + (pRxWI->ShortGI <<8)+ (pRxWI->PHYMODE <<14)) ;
+
+
+#ifdef QOS_DLS_SUPPORT
+        if (RX_BLK_TEST_FLAG(pRxBlk, fRX_DLS))
+		{
+			MAC_TABLE_ENTRY *pDlsEntry = NULL;
+
+			pDlsEntry = DlsEntryTableLookupByWcid(pAd, pRxWI->WirelessCliID, pHeader->Addr2, TRUE);
+										                        if(pDlsEntry)
+			Update_Rssi_Sample(pAd, &pDlsEntry->RssiSample, pRxWI);
+		}
+		else
+#endif // QOS_DLS_SUPPORT //
+		if (ADHOC_ON(pAd))
+		{
+			pEntry = MacTableLookup(pAd, pHeader->Addr2);
+			if (pEntry)
+				Update_Rssi_Sample(pAd, &pEntry->RssiSample, pRxWI);
+		}
+
+
+		Update_Rssi_Sample(pAd, &pAd->StaCfg.RssiSample, pRxWI);
+
+		pAd->StaCfg.LastSNR0 = (UCHAR)(pRxWI->SNR0);
+		pAd->StaCfg.LastSNR1 = (UCHAR)(pRxWI->SNR1);
+
+		pAd->RalinkCounters.OneSecRxOkDataCnt++;
+
+
+    	if (!((pHeader->Frag == 0) && (pHeader->FC.MoreFrag == 0)))
+    	{
+    		// re-assemble the fragmented packets
+    		// return complete frame (pRxPacket) or NULL
+    		bFragment = TRUE;
+    		pRxPacket = RTMPDeFragmentDataFrame(pAd, pRxBlk);
+    	}
+
+    	if (pRxPacket)
+    	{
+			pEntry = &pAd->MacTab.Content[pRxWI->WirelessCliID];
+
+    		// process complete frame
+    		if (bFragment && (pRxD->Decrypted) && (pEntry->WepStatus == Ndis802_11Encryption2Enabled))
+    		{
+				// Minus MIC length
+				pRxBlk->DataSize -= 8;
+
+    			// For TKIP frame, calculate the MIC value
+    			if (STACheckTkipMICValue(pAd, pEntry, pRxBlk) == FALSE)
+    			{
+    				return;
+    			}
+    		}
+
+    		STARxDataFrameAnnounce(pAd, pEntry, pRxBlk, FromWhichBSSID);
+			return;
+    	}
+    	else
+    	{
+    		// just return
+    		// because RTMPDeFragmentDataFrame() will release rx packet,
+    		// if packet is fragmented
+    		return;
+    	}
+	}
+
+	ASSERT(0);
+	// release packet
+	RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+}
+
+VOID STAHandleRxMgmtFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk)
+{
+	PRT28XX_RXD_STRUC	pRxD = &(pRxBlk->RxD);
+	PRXWI_STRUC		pRxWI = pRxBlk->pRxWI;
+	PHEADER_802_11	pHeader = pRxBlk->pHeader;
+	PNDIS_PACKET	pRxPacket = pRxBlk->pRxPacket;
+
+	do
+	{
+
+		// We should collect RSSI not only U2M data but also my beacon
+		if ((pHeader->FC.SubType == SUBTYPE_BEACON) && (MAC_ADDR_EQUAL(&pAd->CommonCfg.Bssid, &pHeader->Addr2)))
+		{
+			Update_Rssi_Sample(pAd, &pAd->StaCfg.RssiSample, pRxWI);
+
+			pAd->StaCfg.LastSNR0 = (UCHAR)(pRxWI->SNR0);
+			pAd->StaCfg.LastSNR1 = (UCHAR)(pRxWI->SNR1);
+		}
+
+		// First check the size, it MUST not exceed the mlme queue size
+		if (pRxWI->MPDUtotalByteCount > MGMT_DMA_BUFFER_SIZE)
+		{
+			DBGPRINT_ERR(("STAHandleRxMgmtFrame: frame too large, size = %d \n", pRxWI->MPDUtotalByteCount));
+			break;
+		}
+
+		REPORT_MGMT_FRAME_TO_MLME(pAd, pRxWI->WirelessCliID, pHeader, pRxWI->MPDUtotalByteCount,
+									pRxWI->RSSI0, pRxWI->RSSI1, pRxWI->RSSI2, pRxD->PlcpSignal);
+	} while (FALSE);
+
+	RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_SUCCESS);
+}
+
+VOID STAHandleRxControlFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	RX_BLK			*pRxBlk)
+{
+#ifdef DOT11_N_SUPPORT
+	PRXWI_STRUC		pRxWI = pRxBlk->pRxWI;
+#endif // DOT11_N_SUPPORT //
+	PHEADER_802_11	pHeader = pRxBlk->pHeader;
+	PNDIS_PACKET	pRxPacket = pRxBlk->pRxPacket;
+
+	switch (pHeader->FC.SubType)
+	{
+		case SUBTYPE_BLOCK_ACK_REQ:
+#ifdef DOT11_N_SUPPORT
+			{
+				CntlEnqueueForRecv(pAd, pRxWI->WirelessCliID, (pRxWI->MPDUtotalByteCount), (PFRAME_BA_REQ)pHeader);
+			}
+			break;
+#endif // DOT11_N_SUPPORT //
+		case SUBTYPE_BLOCK_ACK:
+		case SUBTYPE_ACK:
+		default:
+			break;
+	}
+
+	RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Process RxDone interrupt, running in DPC level
+
+	Arguments:
+		pAd Pointer to our adapter
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+		This routine has to maintain Rx ring read pointer.
+		Need to consider QOS DATA format when converting to 802.3
+	========================================================================
+*/
+BOOLEAN STARxDoneInterruptHandle(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	BOOLEAN			argc)
+{
+	NDIS_STATUS			Status;
+	UINT32			RxProcessed, RxPending;
+	BOOLEAN			bReschedule = FALSE;
+	RT28XX_RXD_STRUC	*pRxD;
+	UCHAR			*pData;
+	PRXWI_STRUC		pRxWI;
+	PNDIS_PACKET	pRxPacket;
+	PHEADER_802_11	pHeader;
+	RX_BLK			RxCell;
+
+	RxProcessed = RxPending = 0;
+
+	// process whole rx ring
+	while (1)
+	{
+
+		if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF |
+								fRTMP_ADAPTER_RESET_IN_PROGRESS |
+								fRTMP_ADAPTER_HALT_IN_PROGRESS |
+								fRTMP_ADAPTER_NIC_NOT_EXIST) ||
+			!RTMP_TEST_FLAG(pAd,fRTMP_ADAPTER_START_UP))
+		{
+			break;
+		}
+
+
+		RxProcessed ++; // test
+
+		// 1. allocate a new data packet into rx ring to replace received packet
+		//    then processing the received packet
+		// 2. the callee must take charge of release of packet
+		// 3. As far as driver is concerned ,
+		//    the rx packet must
+		//      a. be indicated to upper layer or
+		//      b. be released if it is discarded
+		pRxPacket = GetPacketFromRxRing(pAd, &(RxCell.RxD), &bReschedule, &RxPending);
+		if (pRxPacket == NULL)
+		{
+			// no more packet to process
+			break;
+		}
+
+		// get rx ring descriptor
+		pRxD = &(RxCell.RxD);
+		// get rx data buffer
+		pData	= GET_OS_PKT_DATAPTR(pRxPacket);
+		pRxWI	= (PRXWI_STRUC) pData;
+		pHeader = (PHEADER_802_11) (pData+RXWI_SIZE) ;
+
+#ifdef RT_BIG_ENDIAN
+	    RTMPFrameEndianChange(pAd, (PUCHAR)pHeader, DIR_READ, TRUE);
+		RTMPWIEndianChange((PUCHAR)pRxWI, TYPE_RXWI);
+#endif
+
+		// build RxCell
+		RxCell.pRxWI = pRxWI;
+		RxCell.pHeader = pHeader;
+		RxCell.pRxPacket = pRxPacket;
+		RxCell.pData = (UCHAR *) pHeader;
+		RxCell.DataSize = pRxWI->MPDUtotalByteCount;
+		RxCell.Flags = 0;
+
+		// Increase Total receive byte counter after real data received no mater any error or not
+		pAd->RalinkCounters.ReceivedByteCount +=  pRxWI->MPDUtotalByteCount;
+		pAd->RalinkCounters.RxCount ++;
+
+		INC_COUNTER64(pAd->WlanCounters.ReceivedFragmentCount);
+
+		if (pRxWI->MPDUtotalByteCount < 14)
+			Status = NDIS_STATUS_FAILURE;
+
+        if (MONITOR_ON(pAd))
+		{
+            send_monitor_packets(pAd, &RxCell);
+			break;
+		}
+		/* RT2870 invokes STARxDoneInterruptHandle() in rtusb_bulk.c */
+#ifdef RALINK_ATE
+		if (ATE_ON(pAd))
+		{
+			pAd->ate.RxCntPerSec++;
+			ATESampleRssi(pAd, pRxWI);
+#ifdef RALINK_28xx_QA
+			if (pAd->ate.bQARxStart == TRUE)
+			{
+				/* (*pRxD) has been swapped in GetPacketFromRxRing() */
+				ATE_QA_Statistics(pAd, pRxWI, pRxD,	pHeader);
+			}
+#endif // RALINK_28xx_QA //
+			RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_SUCCESS);
+			continue;
+		}
+#endif // RALINK_ATE //
+
+		// Check for all RxD errors
+		Status = RTMPCheckRxError(pAd, pHeader, pRxWI, pRxD);
+
+		// Handle the received frame
+		if (Status == NDIS_STATUS_SUCCESS)
+		{
+			switch (pHeader->FC.Type)
+			{
+				// CASE I, receive a DATA frame
+				case BTYPE_DATA:
+				{
+					// process DATA frame
+					STAHandleRxDataFrame(pAd, &RxCell);
+				}
+				break;
+				// CASE II, receive a MGMT frame
+				case BTYPE_MGMT:
+				{
+					STAHandleRxMgmtFrame(pAd, &RxCell);
+				}
+				break;
+				// CASE III. receive a CNTL frame
+				case BTYPE_CNTL:
+				{
+					STAHandleRxControlFrame(pAd, &RxCell);
+				}
+				break;
+				// discard other type
+				default:
+					RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+					break;
+			}
+		}
+		else
+		{
+			pAd->Counters8023.RxErrors++;
+			// discard this frame
+			RELEASE_NDIS_PACKET(pAd, pRxPacket, NDIS_STATUS_FAILURE);
+		}
+	}
+
+	return bReschedule;
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+	Arguments:
+		pAd 	Pointer to our adapter
+
+	IRQL = DISPATCH_LEVEL
+
+	========================================================================
+*/
+VOID	RTMPHandleTwakeupInterrupt(
+	IN PRTMP_ADAPTER pAd)
+{
+	AsicForceWakeup(pAd, FALSE);
+}
+
+/*
+========================================================================
+Routine Description:
+    Early checking and OS-depened parsing for Tx packet send to our STA driver.
+
+Arguments:
+    NDIS_HANDLE 	MiniportAdapterContext	Pointer refer to the device handle, i.e., the pAd.
+	PPNDIS_PACKET	ppPacketArray			The packet array need to do transmission.
+	UINT			NumberOfPackets			Number of packet in packet array.
+
+Return Value:
+	NONE
+
+Note:
+	This function do early checking and classification for send-out packet.
+	You only can put OS-depened & STA related code in here.
+========================================================================
+*/
+VOID STASendPackets(
+	IN	NDIS_HANDLE		MiniportAdapterContext,
+	IN	PPNDIS_PACKET	ppPacketArray,
+	IN	UINT			NumberOfPackets)
+{
+	UINT			Index;
+	PRTMP_ADAPTER	pAd = (PRTMP_ADAPTER) MiniportAdapterContext;
+	PNDIS_PACKET	pPacket;
+	BOOLEAN			allowToSend = FALSE;
+
+
+	for (Index = 0; Index < NumberOfPackets; Index++)
+	{
+		pPacket = ppPacketArray[Index];
+
+		do
+		{
+			if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS) ||
+				RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS) ||
+				RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RADIO_OFF))
+			{
+				// Drop send request since hardware is in reset state
+					break;
+			}
+			else if (!INFRA_ON(pAd) && !ADHOC_ON(pAd))
+			{
+				// Drop send request since there are no physical connection yet
+					break;
+			}
+			else
+			{
+				// Record that orignal packet source is from NDIS layer,so that
+				// later on driver knows how to release this NDIS PACKET
+#ifdef QOS_DLS_SUPPORT
+				MAC_TABLE_ENTRY *pEntry;
+				PUCHAR pSrcBufVA = GET_OS_PKT_DATAPTR(pPacket);
+
+				pEntry = MacTableLookup(pAd, pSrcBufVA);
+				if (pEntry && (pEntry->ValidAsDls == TRUE))
+				{
+					RTMP_SET_PACKET_WCID(pPacket, pEntry->Aid);
+				}
+				else
+#endif // QOS_DLS_SUPPORT //
+				RTMP_SET_PACKET_WCID(pPacket, 0); // this field is useless when in STA mode
+				RTMP_SET_PACKET_SOURCE(pPacket, PKTSRC_NDIS);
+				NDIS_SET_PACKET_STATUS(pPacket, NDIS_STATUS_PENDING);
+				pAd->RalinkCounters.PendingNdisPacketCount++;
+
+				allowToSend = TRUE;
+			}
+		} while(FALSE);
+
+		if (allowToSend == TRUE)
+			STASendPacket(pAd, pPacket);
+		else
+			RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+	}
+
+	// Dequeue outgoing frames from TxSwQueue[] and process it
+	RTMPDeQueuePacket(pAd, FALSE, NUM_OF_TX_RING, MAX_TX_PROCESS);
+
+}
+
+
+/*
+========================================================================
+Routine Description:
+	This routine is used to do packet parsing and classification for Tx packet
+	to STA device, and it will en-queue packets to our TxSwQueue depends on AC
+	class.
+
+Arguments:
+	pAd    		Pointer to our adapter
+	pPacket 	Pointer to send packet
+
+Return Value:
+	NDIS_STATUS_SUCCESS			If succes to queue the packet into TxSwQueue.
+	NDIS_STATUS_FAILURE			If failed to do en-queue.
+
+Note:
+	You only can put OS-indepened & STA related code in here.
+========================================================================
+*/
+NDIS_STATUS STASendPacket(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket)
+{
+	PACKET_INFO 	PacketInfo;
+	PUCHAR			pSrcBufVA;
+	UINT			SrcBufLen;
+	UINT			AllowFragSize;
+	UCHAR			NumberOfFrag;
+//	UCHAR			RTSRequired;
+	UCHAR			QueIdx, UserPriority;
+	MAC_TABLE_ENTRY *pEntry = NULL;
+	unsigned int 	IrqFlags;
+	UCHAR			FlgIsIP = 0;
+	UCHAR			Rate;
+
+	// Prepare packet information structure for buffer descriptor
+	// chained within a single NDIS packet.
+	RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pSrcBufVA, &SrcBufLen);
+
+	if (pSrcBufVA == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("STASendPacket --> pSrcBufVA == NULL !!!SrcBufLen=%x\n",SrcBufLen));
+		// Resourece is low, system did not allocate virtual address
+		// return NDIS_STATUS_FAILURE directly to upper layer
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+		return NDIS_STATUS_FAILURE;
+	}
+
+
+	if (SrcBufLen < 14)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("STASendPacket --> Ndis Packet buffer error !!!\n"));
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+		return (NDIS_STATUS_FAILURE);
+	}
+
+	// In HT rate adhoc mode, A-MPDU is often used. So need to lookup BA Table and MAC Entry.
+	// Note multicast packets in adhoc also use BSSID_WCID index.
+	{
+		if(INFRA_ON(pAd))
+		{
+#ifdef QOS_DLS_SUPPORT
+			USHORT	tmpWcid;
+
+			tmpWcid = RTMP_GET_PACKET_WCID(pPacket);
+			if (VALID_WCID(tmpWcid) &&
+				(pAd->MacTab.Content[tmpWcid].ValidAsDls== TRUE))
+			{
+				pEntry = &pAd->MacTab.Content[tmpWcid];
+				Rate = pAd->MacTab.Content[tmpWcid].CurrTxRate;
+			}
+			else
+#endif // QOS_DLS_SUPPORT //
+			{
+			pEntry = &pAd->MacTab.Content[BSSID_WCID];
+			RTMP_SET_PACKET_WCID(pPacket, BSSID_WCID);
+			Rate = pAd->CommonCfg.TxRate;
+		}
+		}
+		else if (ADHOC_ON(pAd))
+		{
+			if (*pSrcBufVA & 0x01)
+			{
+				RTMP_SET_PACKET_WCID(pPacket, MCAST_WCID);
+				pEntry = &pAd->MacTab.Content[MCAST_WCID];
+			}
+			else
+			{
+				pEntry = MacTableLookup(pAd, pSrcBufVA);
+			}
+			Rate = pAd->CommonCfg.TxRate;
+		}
+	}
+
+	if (!pEntry)
+	{
+		DBGPRINT(RT_DEBUG_ERROR,("STASendPacket->Cannot find pEntry(%2x:%2x:%2x:%2x:%2x:%2x) in MacTab!\n", PRINT_MAC(pSrcBufVA)));
+		// Resourece is low, system did not allocate virtual address
+		// return NDIS_STATUS_FAILURE directly to upper layer
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+		return NDIS_STATUS_FAILURE;
+	}
+
+	if (ADHOC_ON(pAd)
+		)
+	{
+		RTMP_SET_PACKET_WCID(pPacket, (UCHAR)pEntry->Aid);
+	}
+
+	//
+	// Check the Ethernet Frame type of this packet, and set the RTMP_SET_PACKET_SPECIFIC flags.
+	//		Here we set the PACKET_SPECIFIC flags(LLC, VLAN, DHCP/ARP, EAPOL).
+	RTMPCheckEtherType(pAd, pPacket);
+
+
+
+	//
+	// WPA 802.1x secured port control - drop all non-802.1x frame before port secured
+	//
+	if (((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
+		 (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+		 (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) ||
+		 (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
+#ifdef WPA_SUPPLICANT_SUPPORT
+		  || (pAd->StaCfg.IEEE8021X == TRUE)
+#endif // WPA_SUPPLICANT_SUPPORT //
+#ifdef LEAP_SUPPORT
+		  || (pAd->StaCfg.LeapAuthMode == CISCO_AuthModeLEAP)
+#endif // LEAP_SUPPORT //
+		  )
+		  && ((pAd->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED) || (pAd->StaCfg.MicErrCnt >= 2))
+		  && (RTMP_GET_PACKET_EAPOL(pPacket)== FALSE)
+		  )
+	{
+		DBGPRINT(RT_DEBUG_TRACE,("STASendPacket --> Drop packet before port secured !!!\n"));
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+
+		return (NDIS_STATUS_FAILURE);
+	}
+
+
+	// STEP 1. Decide number of fragments required to deliver this MSDU.
+	//	   The estimation here is not very accurate because difficult to
+	//	   take encryption overhead into consideration here. The result
+	//	   "NumberOfFrag" is then just used to pre-check if enough free
+	//	   TXD are available to hold this MSDU.
+
+
+	if (*pSrcBufVA & 0x01)	// fragmentation not allowed on multicast & broadcast
+		NumberOfFrag = 1;
+	else if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_AGGREGATION_INUSED))
+		NumberOfFrag = 1;	// Aggregation overwhelms fragmentation
+	else if (CLIENT_STATUS_TEST_FLAG(pEntry, fCLIENT_STATUS_AMSDU_INUSED))
+		NumberOfFrag = 1;	// Aggregation overwhelms fragmentation
+#ifdef DOT11_N_SUPPORT
+	else if ((pAd->StaCfg.HTPhyMode.field.MODE == MODE_HTMIX) || (pAd->StaCfg.HTPhyMode.field.MODE == MODE_HTGREENFIELD))
+		NumberOfFrag = 1;	// MIMO RATE overwhelms fragmentation
+#endif // DOT11_N_SUPPORT //
+	else
+	{
+		// The calculated "NumberOfFrag" is a rough estimation because of various
+		// encryption/encapsulation overhead not taken into consideration. This number is just
+		// used to make sure enough free TXD are available before fragmentation takes place.
+		// In case the actual required number of fragments of an NDIS packet
+		// excceeds "NumberOfFrag"caculated here and not enough free TXD available, the
+		// last fragment (i.e. last MPDU) will be dropped in RTMPHardTransmit() due to out of
+		// resource, and the NDIS packet will be indicated NDIS_STATUS_FAILURE. This should
+		// rarely happen and the penalty is just like a TX RETRY fail. Affordable.
+
+		AllowFragSize = (pAd->CommonCfg.FragmentThreshold) - LENGTH_802_11 - LENGTH_CRC;
+		NumberOfFrag = ((PacketInfo.TotalPacketLength - LENGTH_802_3 + LENGTH_802_1_H) / AllowFragSize) + 1;
+		// To get accurate number of fragmentation, Minus 1 if the size just match to allowable fragment size
+		if (((PacketInfo.TotalPacketLength - LENGTH_802_3 + LENGTH_802_1_H) % AllowFragSize) == 0)
+		{
+			NumberOfFrag--;
+		}
+	}
+
+	// Save fragment number to Ndis packet reserved field
+	RTMP_SET_PACKET_FRAGMENTS(pPacket, NumberOfFrag);
+
+
+	// STEP 2. Check the requirement of RTS:
+	//	   If multiple fragment required, RTS is required only for the first fragment
+	//	   if the fragment size large than RTS threshold
+	//     For RT28xx, Let ASIC send RTS/CTS
+	RTMP_SET_PACKET_RTS(pPacket, 0);
+	RTMP_SET_PACKET_TXRATE(pPacket, pAd->CommonCfg.TxRate);
+
+	//
+	// STEP 3. Traffic classification. outcome = <UserPriority, QueIdx>
+	//
+	UserPriority = 0;
+	QueIdx		 = QID_AC_BE;
+	if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED) &&
+		CLIENT_STATUS_TEST_FLAG(pEntry, fCLIENT_STATUS_WMM_CAPABLE))
+	{
+		USHORT Protocol;
+		UCHAR  LlcSnapLen = 0, Byte0, Byte1;
+		do
+		{
+			// get Ethernet protocol field
+			Protocol = (USHORT)((pSrcBufVA[12] << 8) + pSrcBufVA[13]);
+			if (Protocol <= 1500)
+			{
+				// get Ethernet protocol field from LLC/SNAP
+				if (Sniff2BytesFromNdisBuffer(PacketInfo.pFirstBuffer, LENGTH_802_3 + 6, &Byte0, &Byte1) != NDIS_STATUS_SUCCESS)
+					break;
+
+				Protocol = (USHORT)((Byte0 << 8) + Byte1);
+				LlcSnapLen = 8;
+			}
+
+			// always AC_BE for non-IP packet
+			if (Protocol != 0x0800)
+				break;
+
+			// get IP header
+			if (Sniff2BytesFromNdisBuffer(PacketInfo.pFirstBuffer, LENGTH_802_3 + LlcSnapLen, &Byte0, &Byte1) != NDIS_STATUS_SUCCESS)
+				break;
+
+			// return AC_BE if packet is not IPv4
+			if ((Byte0 & 0xf0) != 0x40)
+				break;
+
+			FlgIsIP = 1;
+			UserPriority = (Byte1 & 0xe0) >> 5;
+			QueIdx = MapUserPriorityToAccessCategory[UserPriority];
+
+			// TODO: have to check ACM bit. apply TSPEC if ACM is ON
+			// TODO: downgrade UP & QueIdx before passing ACM
+			if (pAd->CommonCfg.APEdcaParm.bACM[QueIdx])
+			{
+				UserPriority = 0;
+				QueIdx		 = QID_AC_BE;
+			}
+		} while (FALSE);
+	}
+
+	RTMP_SET_PACKET_UP(pPacket, UserPriority);
+
+
+
+	// Make sure SendTxWait queue resource won't be used by other threads
+	RTMP_IRQ_LOCK(&pAd->irq_lock, IrqFlags);
+	if (pAd->TxSwQueue[QueIdx].Number >= MAX_PACKETS_IN_QUEUE)
+	{
+		RTMP_IRQ_UNLOCK(&pAd->irq_lock, IrqFlags);
+#ifdef BLOCK_NET_IF
+		StopNetIfQueue(pAd, QueIdx, pPacket);
+#endif // BLOCK_NET_IF //
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+
+		return NDIS_STATUS_FAILURE;
+	}
+	else
+	{
+		InsertTailQueue(&pAd->TxSwQueue[QueIdx], PACKET_TO_QUEUE_ENTRY(pPacket));
+	}
+	RTMP_IRQ_UNLOCK(&pAd->irq_lock, IrqFlags);
+
+#ifdef DOT11_N_SUPPORT
+    if ((pAd->CommonCfg.BACapability.field.AutoBA == TRUE)&&
+        IS_HT_STA(pEntry))
+	{
+	    //PMAC_TABLE_ENTRY pMacEntry = &pAd->MacTab.Content[BSSID_WCID];
+		if (((pEntry->TXBAbitmap & (1<<UserPriority)) == 0) &&
+            ((pEntry->BADeclineBitmap & (1<<UserPriority)) == 0) &&
+            (pEntry->PortSecured == WPA_802_1X_PORT_SECURED)
+			 // For IOT compatibility, if
+			 // 1. It is Ralink chip or
+			 // 2. It is OPEN or AES mode,
+			 // then BA session can be bulit.
+			 && ((pEntry->ValidAsCLI && pAd->MlmeAux.APRalinkIe != 0x0) ||
+			 	 (pEntry->WepStatus == Ndis802_11WEPDisabled || pEntry->WepStatus == Ndis802_11Encryption3Enabled))
+			)
+		{
+			BAOriSessionSetUp(pAd, pEntry, 0, 0, 10, FALSE);
+		}
+	}
+#endif // DOT11_N_SUPPORT //
+
+	pAd->RalinkCounters.OneSecOsTxCount[QueIdx]++; // TODO: for debug only. to be removed
+	return NDIS_STATUS_SUCCESS;
+}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		This subroutine will scan through releative ring descriptor to find
+		out avaliable free ring descriptor and compare with request size.
+
+	Arguments:
+		pAd Pointer to our adapter
+		QueIdx		Selected TX Ring
+
+	Return Value:
+		NDIS_STATUS_FAILURE 	Not enough free descriptor
+		NDIS_STATUS_SUCCESS 	Enough free descriptor
+
+	IRQL = PASSIVE_LEVEL
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+
+#ifdef RT2870
+/*
+	Actually, this function used to check if the TxHardware Queue still has frame need to send.
+	If no frame need to send, go to sleep, else, still wake up.
+*/
+NDIS_STATUS RTMPFreeTXDRequest(
+	IN		PRTMP_ADAPTER	pAd,
+	IN		UCHAR			QueIdx,
+	IN		UCHAR			NumberRequired,
+	IN		PUCHAR			FreeNumberIs)
+{
+	//ULONG		FreeNumber = 0;
+	NDIS_STATUS 	Status = NDIS_STATUS_FAILURE;
+	unsigned long   IrqFlags;
+	HT_TX_CONTEXT	*pHTTXContext;
+
+	switch (QueIdx)
+	{
+		case QID_AC_BK:
+		case QID_AC_BE:
+		case QID_AC_VI:
+		case QID_AC_VO:
+		case QID_HCCA:
+			{
+				pHTTXContext = &pAd->TxContext[QueIdx];
+				RTMP_IRQ_LOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+				if ((pHTTXContext->CurWritePosition != pHTTXContext->ENextBulkOutPosition) ||
+					(pHTTXContext->IRPPending == TRUE))
+				{
+					Status = NDIS_STATUS_FAILURE;
+				}
+				else
+				{
+					Status = NDIS_STATUS_SUCCESS;
+				}
+				RTMP_IRQ_UNLOCK(&pAd->TxContextQueueLock[QueIdx], IrqFlags);
+			}
+			break;
+
+		case QID_MGMT:
+			if (pAd->MgmtRing.TxSwFreeIdx != MGMT_RING_SIZE)
+				Status = NDIS_STATUS_FAILURE;
+			else
+				Status = NDIS_STATUS_SUCCESS;
+			break;
+
+		default:
+			DBGPRINT(RT_DEBUG_ERROR,("RTMPFreeTXDRequest::Invalid QueIdx(=%d)\n", QueIdx));
+			break;
+	}
+
+	return (Status);
+
+}
+#endif // RT2870 //
+
+
+VOID RTMPSendDisassociationFrame(
+	IN	PRTMP_ADAPTER	pAd)
+{
+}
+
+VOID	RTMPSendNullFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	UCHAR			TxRate,
+	IN	BOOLEAN 		bQosNull)
+{
+	UCHAR	NullFrame[48];
+	ULONG	Length;
+	PHEADER_802_11	pHeader_802_11;
+
+
+#ifdef RALINK_ATE
+	if(ATE_ON(pAd))
+	{
+		return;
+	}
+#endif // RALINK_ATE //
+
+    // WPA 802.1x secured port control
+    if (((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
+         (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+         (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) ||
+         (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
+#ifdef WPA_SUPPLICANT_SUPPORT
+			  || (pAd->StaCfg.IEEE8021X == TRUE)
+#endif
+        ) &&
+       (pAd->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
+	{
+		return;
+	}
+
+	NdisZeroMemory(NullFrame, 48);
+	Length = sizeof(HEADER_802_11);
+
+	pHeader_802_11 = (PHEADER_802_11) NullFrame;
+
+	pHeader_802_11->FC.Type = BTYPE_DATA;
+	pHeader_802_11->FC.SubType = SUBTYPE_NULL_FUNC;
+	pHeader_802_11->FC.ToDs = 1;
+	COPY_MAC_ADDR(pHeader_802_11->Addr1, pAd->CommonCfg.Bssid);
+	COPY_MAC_ADDR(pHeader_802_11->Addr2, pAd->CurrentAddress);
+	COPY_MAC_ADDR(pHeader_802_11->Addr3, pAd->CommonCfg.Bssid);
+
+	if (pAd->CommonCfg.bAPSDForcePowerSave)
+	{
+		pHeader_802_11->FC.PwrMgmt = PWR_SAVE;
+	}
+	else
+	{
+		pHeader_802_11->FC.PwrMgmt = (pAd->StaCfg.Psm == PWR_SAVE) ? 1: 0;
+	}
+	pHeader_802_11->Duration = pAd->CommonCfg.Dsifs + RTMPCalcDuration(pAd, TxRate, 14);
+
+	pAd->Sequence++;
+	pHeader_802_11->Sequence = pAd->Sequence;
+
+	// Prepare QosNull function frame
+	if (bQosNull)
+	{
+		pHeader_802_11->FC.SubType = SUBTYPE_QOS_NULL;
+
+		// copy QOS control bytes
+		NullFrame[Length]	=  0;
+		NullFrame[Length+1] =  0;
+		Length += 2;// if pad with 2 bytes for alignment, APSD will fail
+	}
+
+	HAL_KickOutNullFrameTx(pAd, 0, NullFrame, Length);
+
+}
+
+// IRQL = DISPATCH_LEVEL
+VOID	RTMPSendRTSFrame(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pDA,
+	IN	unsigned int	NextMpduSize,
+	IN	UCHAR			TxRate,
+	IN	UCHAR			RTSRate,
+	IN	USHORT			AckDuration,
+	IN	UCHAR			QueIdx,
+	IN	UCHAR			FrameGap)
+{
+}
+
+
+
+// --------------------------------------------------------
+//  FIND ENCRYPT KEY AND DECIDE CIPHER ALGORITHM
+//		Find the WPA key, either Group or Pairwise Key
+//		LEAP + TKIP also use WPA key.
+// --------------------------------------------------------
+// Decide WEP bit and cipher suite to be used. Same cipher suite should be used for whole fragment burst
+// In Cisco CCX 2.0 Leap Authentication
+//		   WepStatus is Ndis802_11Encryption1Enabled but the key will use PairwiseKey
+//		   Instead of the SharedKey, SharedKey Length may be Zero.
+VOID STAFindCipherAlgorithm(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk)
+{
+	NDIS_802_11_ENCRYPTION_STATUS	Cipher;				// To indicate cipher used for this packet
+	UCHAR							CipherAlg = CIPHER_NONE;		// cipher alogrithm
+	UCHAR							KeyIdx = 0xff;
+	PUCHAR							pSrcBufVA;
+	PCIPHER_KEY						pKey = NULL;
+
+	pSrcBufVA = GET_OS_PKT_DATAPTR(pTxBlk->pPacket);
+
+	{
+	    // Select Cipher
+	    if ((*pSrcBufVA & 0x01) && (ADHOC_ON(pAd)))
+	        Cipher = pAd->StaCfg.GroupCipher; // Cipher for Multicast or Broadcast
+	    else
+	        Cipher = pAd->StaCfg.PairCipher; // Cipher for Unicast
+
+		if (RTMP_GET_PACKET_EAPOL(pTxBlk->pPacket))
+		{
+			ASSERT(pAd->SharedKey[BSS0][0].CipherAlg <= CIPHER_CKIP128);
+
+			// 4-way handshaking frame must be clear
+			if (!(TX_BLK_TEST_FLAG(pTxBlk, fTX_bClearEAPFrame)) && (pAd->SharedKey[BSS0][0].CipherAlg) &&
+				(pAd->SharedKey[BSS0][0].KeyLen))
+			{
+				CipherAlg = pAd->SharedKey[BSS0][0].CipherAlg;
+				KeyIdx = 0;
+			}
+		}
+		else if (Cipher == Ndis802_11Encryption1Enabled)
+		{
+#ifdef LEAP_SUPPORT
+			if (pAd->StaCfg.CkipFlag & 0x10) // Cisco CKIP KP is on
+			{
+				if (LEAP_CCKM_ON(pAd))
+				{
+					if (((*pSrcBufVA & 0x01) && (ADHOC_ON(pAd))))
+						KeyIdx = 1;
+					else
+						KeyIdx = 0;
+				}
+				else
+					KeyIdx = pAd->StaCfg.DefaultKeyId;
+			}
+			else if (pAd->StaCfg.CkipFlag & 0x08) // only CKIP CMIC
+				KeyIdx = pAd->StaCfg.DefaultKeyId;
+			else if (LEAP_CCKM_ON(pAd))
+			{
+				if ((*pSrcBufVA & 0x01) && (ADHOC_ON(pAd)))
+					KeyIdx = 1;
+				else
+					KeyIdx = 0;
+			}
+			else	// standard WEP64 or WEP128
+#endif // LEAP_SUPPORT //
+				KeyIdx = pAd->StaCfg.DefaultKeyId;
+		}
+		else if ((Cipher == Ndis802_11Encryption2Enabled) ||
+				 (Cipher == Ndis802_11Encryption3Enabled))
+		{
+			if ((*pSrcBufVA & 0x01) && (ADHOC_ON(pAd))) // multicast
+				KeyIdx = pAd->StaCfg.DefaultKeyId;
+			else if (pAd->SharedKey[BSS0][0].KeyLen)
+				KeyIdx = 0;
+			else
+				KeyIdx = pAd->StaCfg.DefaultKeyId;
+		}
+
+		if (KeyIdx == 0xff)
+			CipherAlg = CIPHER_NONE;
+		else if ((Cipher == Ndis802_11EncryptionDisabled) || (pAd->SharedKey[BSS0][KeyIdx].KeyLen == 0))
+			CipherAlg = CIPHER_NONE;
+#ifdef WPA_SUPPLICANT_SUPPORT
+	    else if ( pAd->StaCfg.WpaSupplicantUP &&
+	             (Cipher == Ndis802_11Encryption1Enabled) &&
+	             (pAd->StaCfg.IEEE8021X == TRUE) &&
+	             (pAd->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
+	        CipherAlg = CIPHER_NONE;
+#endif // WPA_SUPPLICANT_SUPPORT //
+		else
+		{
+			//Header_802_11.FC.Wep = 1;
+			CipherAlg = pAd->SharedKey[BSS0][KeyIdx].CipherAlg;
+			pKey = &pAd->SharedKey[BSS0][KeyIdx];
+		}
+	}
+
+	pTxBlk->CipherAlg = CipherAlg;
+	pTxBlk->pKey = pKey;
+}
+
+
+VOID STABuildCommon802_11Header(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  TX_BLK          *pTxBlk)
+{
+
+	HEADER_802_11	*pHeader_802_11;
+#ifdef QOS_DLS_SUPPORT
+	BOOLEAN	bDLSFrame = FALSE;
+	INT	DlsEntryIndex = 0;
+#endif // QOS_DLS_SUPPORT //
+
+	//
+	// MAKE A COMMON 802.11 HEADER
+	//
+
+	// normal wlan header size : 24 octets
+	pTxBlk->MpduHeaderLen = sizeof(HEADER_802_11);
+
+	pHeader_802_11 = (HEADER_802_11 *) &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
+
+	NdisZeroMemory(pHeader_802_11, sizeof(HEADER_802_11));
+
+	pHeader_802_11->FC.FrDs = 0;
+	pHeader_802_11->FC.Type = BTYPE_DATA;
+	pHeader_802_11->FC.SubType = ((TX_BLK_TEST_FLAG(pTxBlk, fTX_bWMM)) ? SUBTYPE_QDATA : SUBTYPE_DATA);
+
+#ifdef QOS_DLS_SUPPORT
+	if (INFRA_ON(pAd))
+	{
+		// Check if the frame can be sent through DLS direct link interface
+		// If packet can be sent through DLS, then force aggregation disable. (Hard to determine peer STA's capability)
+		DlsEntryIndex = RTMPCheckDLSFrame(pAd, pTxBlk->pSrcBufHeader);
+		if (DlsEntryIndex >= 0)
+			bDLSFrame = TRUE;
+		else
+			bDLSFrame = FALSE;
+	}
+#endif // QOS_DLS_SUPPORT //
+
+    if (pTxBlk->pMacEntry)
+	{
+		if (TX_BLK_TEST_FLAG(pTxBlk, fTX_bForceNonQoS))
+		{
+			pHeader_802_11->Sequence = pTxBlk->pMacEntry->NonQosDataSeq;
+			pTxBlk->pMacEntry->NonQosDataSeq = (pTxBlk->pMacEntry->NonQosDataSeq+1) & MAXSEQ;
+		}
+		else
+		{
+    	    pHeader_802_11->Sequence = pTxBlk->pMacEntry->TxSeq[pTxBlk->UserPriority];
+    	    pTxBlk->pMacEntry->TxSeq[pTxBlk->UserPriority] = (pTxBlk->pMacEntry->TxSeq[pTxBlk->UserPriority]+1) & MAXSEQ;
+    	}
+	}
+	else
+	{
+		pHeader_802_11->Sequence = pAd->Sequence;
+		pAd->Sequence = (pAd->Sequence+1) & MAXSEQ; // next sequence
+	}
+
+	pHeader_802_11->Frag = 0;
+
+	pHeader_802_11->FC.MoreData = TX_BLK_TEST_FLAG(pTxBlk, fTX_bMoreData);
+
+	{
+		if (INFRA_ON(pAd))
+		{
+#ifdef QOS_DLS_SUPPORT
+			if (bDLSFrame)
+			{
+				COPY_MAC_ADDR(pHeader_802_11->Addr1, pTxBlk->pSrcBufHeader);
+				COPY_MAC_ADDR(pHeader_802_11->Addr2, pAd->CurrentAddress);
+				COPY_MAC_ADDR(pHeader_802_11->Addr3, pAd->CommonCfg.Bssid);
+				pHeader_802_11->FC.ToDs = 0;
+			}
+			else
+#endif // QOS_DLS_SUPPORT //
+			{
+			COPY_MAC_ADDR(pHeader_802_11->Addr1, pAd->CommonCfg.Bssid);
+			COPY_MAC_ADDR(pHeader_802_11->Addr2, pAd->CurrentAddress);
+			COPY_MAC_ADDR(pHeader_802_11->Addr3, pTxBlk->pSrcBufHeader);
+			pHeader_802_11->FC.ToDs = 1;
+		}
+		}
+		else if (ADHOC_ON(pAd))
+		{
+			COPY_MAC_ADDR(pHeader_802_11->Addr1, pTxBlk->pSrcBufHeader);
+			COPY_MAC_ADDR(pHeader_802_11->Addr2, pAd->CurrentAddress);
+			COPY_MAC_ADDR(pHeader_802_11->Addr3, pAd->CommonCfg.Bssid);
+			pHeader_802_11->FC.ToDs = 0;
+		}
+	}
+
+	if (pTxBlk->CipherAlg != CIPHER_NONE)
+		pHeader_802_11->FC.Wep = 1;
+
+	// -----------------------------------------------------------------
+	// STEP 2. MAKE A COMMON 802.11 HEADER SHARED BY ENTIRE FRAGMENT BURST. Fill sequence later.
+	// -----------------------------------------------------------------
+	if (pAd->CommonCfg.bAPSDForcePowerSave)
+    	pHeader_802_11->FC.PwrMgmt = PWR_SAVE;
+	else
+    	pHeader_802_11->FC.PwrMgmt = (pAd->StaCfg.Psm == PWR_SAVE);
+}
+
+#ifdef DOT11_N_SUPPORT
+VOID STABuildCache802_11Header(
+	IN RTMP_ADAPTER		*pAd,
+	IN TX_BLK			*pTxBlk,
+	IN UCHAR			*pHeader)
+{
+	MAC_TABLE_ENTRY	*pMacEntry;
+	PHEADER_802_11	pHeader80211;
+
+	pHeader80211 = (PHEADER_802_11)pHeader;
+	pMacEntry = pTxBlk->pMacEntry;
+
+	//
+	// Update the cached 802.11 HEADER
+	//
+
+	// normal wlan header size : 24 octets
+	pTxBlk->MpduHeaderLen = sizeof(HEADER_802_11);
+
+	// More Bit
+	pHeader80211->FC.MoreData = TX_BLK_TEST_FLAG(pTxBlk, fTX_bMoreData);
+
+	// Sequence
+	pHeader80211->Sequence = pMacEntry->TxSeq[pTxBlk->UserPriority];
+    pMacEntry->TxSeq[pTxBlk->UserPriority] = (pMacEntry->TxSeq[pTxBlk->UserPriority]+1) & MAXSEQ;
+
+	{
+		// Check if the frame can be sent through DLS direct link interface
+		// If packet can be sent through DLS, then force aggregation disable. (Hard to determine peer STA's capability)
+#ifdef QOS_DLS_SUPPORT
+		BOOLEAN	bDLSFrame = FALSE;
+		INT	DlsEntryIndex = 0;
+
+		DlsEntryIndex = RTMPCheckDLSFrame(pAd, pTxBlk->pSrcBufHeader);
+		if (DlsEntryIndex >= 0)
+			bDLSFrame = TRUE;
+		else
+			bDLSFrame = FALSE;
+#endif // QOS_DLS_SUPPORT //
+
+		// The addr3 of normal packet send from DS is Dest Mac address.
+#ifdef QOS_DLS_SUPPORT
+		if (bDLSFrame)
+		{
+			COPY_MAC_ADDR(pHeader80211->Addr1, pTxBlk->pSrcBufHeader);
+			COPY_MAC_ADDR(pHeader80211->Addr3, pAd->CommonCfg.Bssid);
+			pHeader80211->FC.ToDs = 0;
+		}
+		else
+#endif // QOS_DLS_SUPPORT //
+		if (ADHOC_ON(pAd))
+			COPY_MAC_ADDR(pHeader80211->Addr3, pAd->CommonCfg.Bssid);
+		else
+			COPY_MAC_ADDR(pHeader80211->Addr3, pTxBlk->pSrcBufHeader);
+	}
+
+	// -----------------------------------------------------------------
+	// STEP 2. MAKE A COMMON 802.11 HEADER SHARED BY ENTIRE FRAGMENT BURST. Fill sequence later.
+	// -----------------------------------------------------------------
+	if (pAd->CommonCfg.bAPSDForcePowerSave)
+    	pHeader80211->FC.PwrMgmt = PWR_SAVE;
+	else
+    	pHeader80211->FC.PwrMgmt = (pAd->StaCfg.Psm == PWR_SAVE);
+}
+#endif // DOT11_N_SUPPORT //
+
+static inline PUCHAR STA_Build_ARalink_Frame_Header(
+	IN RTMP_ADAPTER *pAd,
+	IN TX_BLK		*pTxBlk)
+{
+	PUCHAR			pHeaderBufPtr;
+	HEADER_802_11	*pHeader_802_11;
+	PNDIS_PACKET	pNextPacket;
+	UINT32			nextBufLen;
+	PQUEUE_ENTRY	pQEntry;
+
+	STAFindCipherAlgorithm(pAd, pTxBlk);
+	STABuildCommon802_11Header(pAd, pTxBlk);
+
+
+	pHeaderBufPtr = &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
+	pHeader_802_11 = (HEADER_802_11 *) pHeaderBufPtr;
+
+	// steal "order" bit to mark "aggregation"
+	pHeader_802_11->FC.Order = 1;
+
+	// skip common header
+	pHeaderBufPtr += pTxBlk->MpduHeaderLen;
+
+	if (TX_BLK_TEST_FLAG(pTxBlk, fTX_bWMM))
+	{
+		//
+		// build QOS Control bytes
+		//
+		*pHeaderBufPtr = (pTxBlk->UserPriority & 0x0F);
+
+		*(pHeaderBufPtr+1) = 0;
+		pHeaderBufPtr +=2;
+		pTxBlk->MpduHeaderLen += 2;
+	}
+
+	// padding at front of LLC header. LLC header should at 4-bytes aligment.
+	pTxBlk->HdrPadLen = (ULONG)pHeaderBufPtr;
+	pHeaderBufPtr = (PCHAR)ROUND_UP(pHeaderBufPtr, 4);
+	pTxBlk->HdrPadLen = (ULONG)(pHeaderBufPtr - pTxBlk->HdrPadLen);
+
+	// For RA Aggregation,
+	// put the 2nd MSDU length(extra 2-byte field) after QOS_CONTROL in little endian format
+	pQEntry = pTxBlk->TxPacketList.Head;
+	pNextPacket = QUEUE_ENTRY_TO_PKT(pQEntry);
+	nextBufLen = GET_OS_PKT_LEN(pNextPacket);
+	if (RTMP_GET_PACKET_VLAN(pNextPacket))
+		nextBufLen -= LENGTH_802_1Q;
+
+	*pHeaderBufPtr = (UCHAR)nextBufLen & 0xff;
+	*(pHeaderBufPtr+1) = (UCHAR)(nextBufLen >> 8);
+
+	pHeaderBufPtr += 2;
+	pTxBlk->MpduHeaderLen += 2;
+
+	return pHeaderBufPtr;
+
+}
+
+#ifdef DOT11_N_SUPPORT
+static inline PUCHAR STA_Build_AMSDU_Frame_Header(
+	IN RTMP_ADAPTER *pAd,
+	IN TX_BLK		*pTxBlk)
+{
+	PUCHAR			pHeaderBufPtr;//, pSaveBufPtr;
+	HEADER_802_11	*pHeader_802_11;
+
+
+	STAFindCipherAlgorithm(pAd, pTxBlk);
+	STABuildCommon802_11Header(pAd, pTxBlk);
+
+	pHeaderBufPtr = &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
+	pHeader_802_11 = (HEADER_802_11 *) pHeaderBufPtr;
+
+	// skip common header
+	pHeaderBufPtr += pTxBlk->MpduHeaderLen;
+
+	//
+	// build QOS Control bytes
+	//
+	*pHeaderBufPtr = (pTxBlk->UserPriority & 0x0F);
+
+	//
+	// A-MSDU packet
+	//
+	*pHeaderBufPtr |= 0x80;
+
+	*(pHeaderBufPtr+1) = 0;
+	pHeaderBufPtr +=2;
+	pTxBlk->MpduHeaderLen += 2;
+
+	//pSaveBufPtr = pHeaderBufPtr;
+
+	//
+	// padding at front of LLC header
+	// LLC header should locate at 4-octets aligment
+	//
+	// @@@ MpduHeaderLen excluding padding @@@
+	//
+	pTxBlk->HdrPadLen = (ULONG)pHeaderBufPtr;
+	pHeaderBufPtr = (PCHAR) ROUND_UP(pHeaderBufPtr, 4);
+	pTxBlk->HdrPadLen = (ULONG)(pHeaderBufPtr - pTxBlk->HdrPadLen);
+
+	return pHeaderBufPtr;
+
+}
+
+
+VOID STA_AMPDU_Frame_Tx(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk)
+{
+	HEADER_802_11	*pHeader_802_11;
+	PUCHAR			pHeaderBufPtr;
+	USHORT			FreeNumber;
+	MAC_TABLE_ENTRY	*pMacEntry;
+	BOOLEAN			bVLANPkt;
+	PQUEUE_ENTRY	pQEntry;
+
+	ASSERT(pTxBlk);
+
+	while(pTxBlk->TxPacketList.Head)
+	{
+		pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
+		pTxBlk->pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
+		if ( RTMP_FillTxBlkInfo(pAd, pTxBlk) != TRUE)
+		{
+			RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
+			continue;
+		}
+
+		bVLANPkt = (RTMP_GET_PACKET_VLAN(pTxBlk->pPacket) ? TRUE : FALSE);
+
+		pMacEntry = pTxBlk->pMacEntry;
+		if (pMacEntry->isCached)
+		{
+			// NOTE: Please make sure the size of pMacEntry->CachedBuf[] is smaller than pTxBlk->HeaderBuf[]!!!!
+			NdisMoveMemory((PUCHAR)&pTxBlk->HeaderBuf[TXINFO_SIZE], (PUCHAR)&pMacEntry->CachedBuf[0], TXWI_SIZE + sizeof(HEADER_802_11));
+			pHeaderBufPtr = (PUCHAR)(&pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE]);
+			STABuildCache802_11Header(pAd, pTxBlk, pHeaderBufPtr);
+		}
+		else
+		{
+			STAFindCipherAlgorithm(pAd, pTxBlk);
+			STABuildCommon802_11Header(pAd, pTxBlk);
+
+			pHeaderBufPtr = &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
+		}
+
+
+		pHeader_802_11 = (HEADER_802_11 *) pHeaderBufPtr;
+
+		// skip common header
+		pHeaderBufPtr += pTxBlk->MpduHeaderLen;
+
+		//
+		// build QOS Control bytes
+		//
+		*pHeaderBufPtr = (pTxBlk->UserPriority & 0x0F);
+		*(pHeaderBufPtr+1) = 0;
+		pHeaderBufPtr +=2;
+		pTxBlk->MpduHeaderLen += 2;
+
+		//
+		// build HTC+
+		// HTC control filed following QoS field
+		//
+		if ((pAd->CommonCfg.bRdg == TRUE) && CLIENT_STATUS_TEST_FLAG(pTxBlk->pMacEntry, fCLIENT_STATUS_RDG_CAPABLE))
+		{
+			if (pMacEntry->isCached == FALSE)
+			{
+				// mark HTC bit
+				pHeader_802_11->FC.Order = 1;
+
+				NdisZeroMemory(pHeaderBufPtr, 4);
+				*(pHeaderBufPtr+3) |= 0x80;
+			}
+			pHeaderBufPtr += 4;
+			pTxBlk->MpduHeaderLen += 4;
+		}
+
+		//pTxBlk->MpduHeaderLen = pHeaderBufPtr - pTxBlk->HeaderBuf - TXWI_SIZE - TXINFO_SIZE;
+		ASSERT(pTxBlk->MpduHeaderLen >= 24);
+
+		// skip 802.3 header
+		pTxBlk->pSrcBufData = pTxBlk->pSrcBufHeader + LENGTH_802_3;
+		pTxBlk->SrcBufLen  -= LENGTH_802_3;
+
+		// skip vlan tag
+		if (bVLANPkt)
+		{
+			pTxBlk->pSrcBufData	+= LENGTH_802_1Q;
+			pTxBlk->SrcBufLen	-= LENGTH_802_1Q;
+		}
+
+		//
+		// padding at front of LLC header
+		// LLC header should locate at 4-octets aligment
+		//
+		// @@@ MpduHeaderLen excluding padding @@@
+		//
+		pTxBlk->HdrPadLen = (ULONG)pHeaderBufPtr;
+		pHeaderBufPtr = (PCHAR) ROUND_UP(pHeaderBufPtr, 4);
+		pTxBlk->HdrPadLen = (ULONG)(pHeaderBufPtr - pTxBlk->HdrPadLen);
+
+		{
+
+			//
+			// Insert LLC-SNAP encapsulation - 8 octets
+			//
+			EXTRA_LLCSNAP_ENCAP_FROM_PKT_OFFSET(pTxBlk->pSrcBufData-2, pTxBlk->pExtraLlcSnapEncap);
+			if (pTxBlk->pExtraLlcSnapEncap)
+			{
+				NdisMoveMemory(pHeaderBufPtr, pTxBlk->pExtraLlcSnapEncap, 6);
+				pHeaderBufPtr += 6;
+				// get 2 octets (TypeofLen)
+				NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufData-2, 2);
+				pHeaderBufPtr += 2;
+				pTxBlk->MpduHeaderLen += LENGTH_802_1_H;
+			}
+
+		}
+
+		if (pMacEntry->isCached)
+		{
+            RTMPWriteTxWI_Cache(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
+		}
+		else
+		{
+			RTMPWriteTxWI_Data(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
+
+			NdisZeroMemory((PUCHAR)(&pMacEntry->CachedBuf[0]), sizeof(pMacEntry->CachedBuf));
+			NdisMoveMemory((PUCHAR)(&pMacEntry->CachedBuf[0]), (PUCHAR)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), (pHeaderBufPtr - (PUCHAR)(&pTxBlk->HeaderBuf[TXINFO_SIZE])));
+			pMacEntry->isCached = TRUE;
+		}
+
+		// calculate Transmitted AMPDU count and ByteCount
+		{
+			pAd->RalinkCounters.TransmittedMPDUsInAMPDUCount.u.LowPart ++;
+			pAd->RalinkCounters.TransmittedOctetsInAMPDUCount.QuadPart += pTxBlk->SrcBufLen;
+		}
+
+		//FreeNumber = GET_TXRING_FREENO(pAd, QueIdx);
+
+		HAL_WriteTxResource(pAd, pTxBlk, TRUE, &FreeNumber);
+
+		//
+		// Kick out Tx
+		//
+		HAL_KickOutTx(pAd, pTxBlk, pTxBlk->QueIdx);
+
+		pAd->RalinkCounters.KickTxCount++;
+		pAd->RalinkCounters.OneSecTxDoneCount++;
+	}
+
+}
+
+
+VOID STA_AMSDU_Frame_Tx(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk)
+{
+	PUCHAR			pHeaderBufPtr;
+	USHORT			FreeNumber;
+	USHORT			subFramePayloadLen = 0;	// AMSDU Subframe length without AMSDU-Header / Padding.
+	USHORT			totalMPDUSize=0;
+	UCHAR			*subFrameHeader;
+	UCHAR			padding = 0;
+	USHORT			FirstTx = 0, LastTxIdx = 0;
+	BOOLEAN			bVLANPkt;
+	int 			frameNum = 0;
+	PQUEUE_ENTRY	pQEntry;
+
+
+	ASSERT(pTxBlk);
+
+	ASSERT((pTxBlk->TxPacketList.Number > 1));
+
+	while(pTxBlk->TxPacketList.Head)
+	{
+		pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
+		pTxBlk->pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
+		if (RTMP_FillTxBlkInfo(pAd, pTxBlk) != TRUE)
+		{
+			RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
+			continue;
+		}
+
+		bVLANPkt = (RTMP_GET_PACKET_VLAN(pTxBlk->pPacket) ? TRUE : FALSE);
+
+		// skip 802.3 header
+		pTxBlk->pSrcBufData = pTxBlk->pSrcBufHeader + LENGTH_802_3;
+		pTxBlk->SrcBufLen  -= LENGTH_802_3;
+
+		// skip vlan tag
+		if (bVLANPkt)
+		{
+			pTxBlk->pSrcBufData	+= LENGTH_802_1Q;
+			pTxBlk->SrcBufLen	-= LENGTH_802_1Q;
+		}
+
+		if (frameNum == 0)
+		{
+			pHeaderBufPtr = STA_Build_AMSDU_Frame_Header(pAd, pTxBlk);
+
+			// NOTE: TxWI->MPDUtotalByteCount will be updated after final frame was handled.
+			RTMPWriteTxWI_Data(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
+		}
+		else
+		{
+			pHeaderBufPtr = &pTxBlk->HeaderBuf[0];
+			padding = ROUND_UP(LENGTH_AMSDU_SUBFRAMEHEAD + subFramePayloadLen, 4) - (LENGTH_AMSDU_SUBFRAMEHEAD + subFramePayloadLen);
+			NdisZeroMemory(pHeaderBufPtr, padding + LENGTH_AMSDU_SUBFRAMEHEAD);
+			pHeaderBufPtr += padding;
+			pTxBlk->MpduHeaderLen = padding;
+		}
+
+		//
+		// A-MSDU subframe
+		//   DA(6)+SA(6)+Length(2) + LLC/SNAP Encap
+		//
+		subFrameHeader = pHeaderBufPtr;
+		subFramePayloadLen = pTxBlk->SrcBufLen;
+
+		NdisMoveMemory(subFrameHeader, pTxBlk->pSrcBufHeader, 12);
+
+
+		pHeaderBufPtr += LENGTH_AMSDU_SUBFRAMEHEAD;
+		pTxBlk->MpduHeaderLen += LENGTH_AMSDU_SUBFRAMEHEAD;
+
+
+		//
+		// Insert LLC-SNAP encapsulation - 8 octets
+		//
+		EXTRA_LLCSNAP_ENCAP_FROM_PKT_OFFSET(pTxBlk->pSrcBufData-2, pTxBlk->pExtraLlcSnapEncap);
+
+		subFramePayloadLen = pTxBlk->SrcBufLen;
+
+		if (pTxBlk->pExtraLlcSnapEncap)
+		{
+			NdisMoveMemory(pHeaderBufPtr, pTxBlk->pExtraLlcSnapEncap, 6);
+			pHeaderBufPtr += 6;
+			// get 2 octets (TypeofLen)
+			NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufData-2, 2);
+			pHeaderBufPtr += 2;
+			pTxBlk->MpduHeaderLen += LENGTH_802_1_H;
+			subFramePayloadLen += LENGTH_802_1_H;
+		}
+
+		// update subFrame Length field
+		subFrameHeader[12] = (subFramePayloadLen & 0xFF00) >> 8;
+		subFrameHeader[13] = subFramePayloadLen & 0xFF;
+
+		totalMPDUSize += pTxBlk->MpduHeaderLen + pTxBlk->SrcBufLen;
+
+		if (frameNum ==0)
+			FirstTx = HAL_WriteMultiTxResource(pAd, pTxBlk, frameNum, &FreeNumber);
+		else
+			LastTxIdx = HAL_WriteMultiTxResource(pAd, pTxBlk, frameNum, &FreeNumber);
+
+		frameNum++;
+
+		pAd->RalinkCounters.KickTxCount++;
+		pAd->RalinkCounters.OneSecTxDoneCount++;
+
+		// calculate Transmitted AMSDU Count and ByteCount
+		{
+			pAd->RalinkCounters.TransmittedAMSDUCount.u.LowPart ++;
+			pAd->RalinkCounters.TransmittedOctetsInAMSDU.QuadPart += totalMPDUSize;
+		}
+
+	}
+
+	HAL_FinalWriteTxResource(pAd, pTxBlk, totalMPDUSize, FirstTx);
+	HAL_LastTxIdx(pAd, pTxBlk->QueIdx, LastTxIdx);
+
+	//
+	// Kick out Tx
+	//
+	HAL_KickOutTx(pAd, pTxBlk, pTxBlk->QueIdx);
+}
+#endif // DOT11_N_SUPPORT //
+
+VOID STA_Legacy_Frame_Tx(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk)
+{
+	HEADER_802_11	*pHeader_802_11;
+	PUCHAR			pHeaderBufPtr;
+	USHORT			FreeNumber;
+	BOOLEAN			bVLANPkt;
+	PQUEUE_ENTRY	pQEntry;
+
+	ASSERT(pTxBlk);
+
+
+	pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
+	pTxBlk->pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
+	if (RTMP_FillTxBlkInfo(pAd, pTxBlk) != TRUE)
+	{
+		RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+
+	if (pTxBlk->TxFrameType == TX_MCAST_FRAME)
+	{
+		INC_COUNTER64(pAd->WlanCounters.MulticastTransmittedFrameCount);
+	}
+
+	if (RTMP_GET_PACKET_RTS(pTxBlk->pPacket))
+		TX_BLK_SET_FLAG(pTxBlk, fTX_bRtsRequired);
+	else
+		TX_BLK_CLEAR_FLAG(pTxBlk, fTX_bRtsRequired);
+
+	bVLANPkt = (RTMP_GET_PACKET_VLAN(pTxBlk->pPacket) ? TRUE : FALSE);
+
+	if (pTxBlk->TxRate < pAd->CommonCfg.MinTxRate)
+		pTxBlk->TxRate = pAd->CommonCfg.MinTxRate;
+
+	STAFindCipherAlgorithm(pAd, pTxBlk);
+	STABuildCommon802_11Header(pAd, pTxBlk);
+
+
+	// skip 802.3 header
+	pTxBlk->pSrcBufData = pTxBlk->pSrcBufHeader + LENGTH_802_3;
+	pTxBlk->SrcBufLen  -= LENGTH_802_3;
+
+	// skip vlan tag
+	if (bVLANPkt)
+	{
+		pTxBlk->pSrcBufData	+= LENGTH_802_1Q;
+		pTxBlk->SrcBufLen	-= LENGTH_802_1Q;
+	}
+
+	pHeaderBufPtr = &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
+	pHeader_802_11 = (HEADER_802_11 *) pHeaderBufPtr;
+
+	// skip common header
+	pHeaderBufPtr += pTxBlk->MpduHeaderLen;
+
+	if (TX_BLK_TEST_FLAG(pTxBlk, fTX_bWMM))
+	{
+		//
+		// build QOS Control bytes
+		//
+		*pHeaderBufPtr = (pTxBlk->UserPriority & 0x0F);
+		*(pHeaderBufPtr+1) = 0;
+		pHeaderBufPtr +=2;
+		pTxBlk->MpduHeaderLen += 2;
+	}
+
+	// The remaining content of MPDU header should locate at 4-octets aligment
+	pTxBlk->HdrPadLen = (ULONG)pHeaderBufPtr;
+	pHeaderBufPtr = (PCHAR) ROUND_UP(pHeaderBufPtr, 4);
+	pTxBlk->HdrPadLen = (ULONG)(pHeaderBufPtr - pTxBlk->HdrPadLen);
+
+	{
+
+		//
+		// Insert LLC-SNAP encapsulation - 8 octets
+		//
+		//
+   		// if original Ethernet frame contains no LLC/SNAP,
+		// then an extra LLC/SNAP encap is required
+		//
+		EXTRA_LLCSNAP_ENCAP_FROM_PKT_START(pTxBlk->pSrcBufHeader, pTxBlk->pExtraLlcSnapEncap);
+		if (pTxBlk->pExtraLlcSnapEncap)
+		{
+			UCHAR vlan_size;
+
+			NdisMoveMemory(pHeaderBufPtr, pTxBlk->pExtraLlcSnapEncap, 6);
+			pHeaderBufPtr += 6;
+			// skip vlan tag
+			vlan_size =  (bVLANPkt) ? LENGTH_802_1Q : 0;
+			// get 2 octets (TypeofLen)
+			NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufHeader+12+vlan_size, 2);
+			pHeaderBufPtr += 2;
+			pTxBlk->MpduHeaderLen += LENGTH_802_1_H;
+		}
+
+	}
+
+	//
+	// prepare for TXWI
+	// use Wcid as Key Index
+	//
+
+	RTMPWriteTxWI_Data(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
+
+	//FreeNumber = GET_TXRING_FREENO(pAd, QueIdx);
+
+	HAL_WriteTxResource(pAd, pTxBlk, TRUE, &FreeNumber);
+
+	pAd->RalinkCounters.KickTxCount++;
+	pAd->RalinkCounters.OneSecTxDoneCount++;
+
+	//
+	// Kick out Tx
+	//
+	HAL_KickOutTx(pAd, pTxBlk, pTxBlk->QueIdx);
+}
+
+
+VOID STA_ARalink_Frame_Tx(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	TX_BLK			*pTxBlk)
+{
+	PUCHAR			pHeaderBufPtr;
+	USHORT			FreeNumber;
+	USHORT			totalMPDUSize=0;
+	USHORT			FirstTx, LastTxIdx;
+	int 			frameNum = 0;
+	BOOLEAN			bVLANPkt;
+	PQUEUE_ENTRY	pQEntry;
+
+
+	ASSERT(pTxBlk);
+
+	ASSERT((pTxBlk->TxPacketList.Number== 2));
+
+
+	FirstTx = LastTxIdx = 0;  // Is it ok init they as 0?
+	while(pTxBlk->TxPacketList.Head)
+	{
+		pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
+		pTxBlk->pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
+
+		if (RTMP_FillTxBlkInfo(pAd, pTxBlk) != TRUE)
+		{
+			RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
+			continue;
+		}
+
+		bVLANPkt = (RTMP_GET_PACKET_VLAN(pTxBlk->pPacket) ? TRUE : FALSE);
+
+		// skip 802.3 header
+		pTxBlk->pSrcBufData = pTxBlk->pSrcBufHeader + LENGTH_802_3;
+		pTxBlk->SrcBufLen  -= LENGTH_802_3;
+
+		// skip vlan tag
+		if (bVLANPkt)
+		{
+			pTxBlk->pSrcBufData	+= LENGTH_802_1Q;
+			pTxBlk->SrcBufLen	-= LENGTH_802_1Q;
+		}
+
+		if (frameNum == 0)
+		{	// For first frame, we need to create the 802.11 header + padding(optional) + RA-AGG-LEN + SNAP Header
+
+			pHeaderBufPtr = STA_Build_ARalink_Frame_Header(pAd, pTxBlk);
+
+			// It's ok write the TxWI here, because the TxWI->MPDUtotalByteCount
+			//	will be updated after final frame was handled.
+			RTMPWriteTxWI_Data(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
+
+
+			//
+			// Insert LLC-SNAP encapsulation - 8 octets
+			//
+			EXTRA_LLCSNAP_ENCAP_FROM_PKT_OFFSET(pTxBlk->pSrcBufData-2, pTxBlk->pExtraLlcSnapEncap);
+
+			if (pTxBlk->pExtraLlcSnapEncap)
+			{
+				NdisMoveMemory(pHeaderBufPtr, pTxBlk->pExtraLlcSnapEncap, 6);
+				pHeaderBufPtr += 6;
+				// get 2 octets (TypeofLen)
+				NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufData-2, 2);
+				pHeaderBufPtr += 2;
+				pTxBlk->MpduHeaderLen += LENGTH_802_1_H;
+			}
+		}
+		else
+		{	// For second aggregated frame, we need create the 802.3 header to headerBuf, because PCI will copy it to SDPtr0.
+
+			pHeaderBufPtr = &pTxBlk->HeaderBuf[0];
+			pTxBlk->MpduHeaderLen = 0;
+
+			// A-Ralink sub-sequent frame header is the same as 802.3 header.
+			//   DA(6)+SA(6)+FrameType(2)
+			NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufHeader, 12);
+			pHeaderBufPtr += 12;
+			// get 2 octets (TypeofLen)
+			NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufData-2, 2);
+			pHeaderBufPtr += 2;
+			pTxBlk->MpduHeaderLen = LENGTH_ARALINK_SUBFRAMEHEAD;
+		}
+
+		totalMPDUSize += pTxBlk->MpduHeaderLen + pTxBlk->SrcBufLen;
+
+		//FreeNumber = GET_TXRING_FREENO(pAd, QueIdx);
+		if (frameNum ==0)
+			FirstTx = HAL_WriteMultiTxResource(pAd, pTxBlk, frameNum, &FreeNumber);
+		else
+			LastTxIdx = HAL_WriteMultiTxResource(pAd, pTxBlk, frameNum, &FreeNumber);
+
+		frameNum++;
+
+		pAd->RalinkCounters.OneSecTxAggregationCount++;
+		pAd->RalinkCounters.KickTxCount++;
+		pAd->RalinkCounters.OneSecTxDoneCount++;
+
+	}
+
+	HAL_FinalWriteTxResource(pAd, pTxBlk, totalMPDUSize, FirstTx);
+	HAL_LastTxIdx(pAd, pTxBlk->QueIdx, LastTxIdx);
+
+	//
+	// Kick out Tx
+	//
+	HAL_KickOutTx(pAd, pTxBlk, pTxBlk->QueIdx);
+
+}
+
+
+VOID STA_Fragment_Frame_Tx(
+	IN RTMP_ADAPTER *pAd,
+	IN TX_BLK		*pTxBlk)
+{
+	HEADER_802_11	*pHeader_802_11;
+	PUCHAR			pHeaderBufPtr;
+	USHORT			FreeNumber;
+	UCHAR 			fragNum = 0;
+	PACKET_INFO		PacketInfo;
+	USHORT			EncryptionOverhead = 0;
+	UINT32			FreeMpduSize, SrcRemainingBytes;
+	USHORT			AckDuration;
+	UINT 			NextMpduSize;
+	BOOLEAN			bVLANPkt;
+	PQUEUE_ENTRY	pQEntry;
+
+
+	ASSERT(pTxBlk);
+
+	pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
+	pTxBlk->pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
+	if (RTMP_FillTxBlkInfo(pAd, pTxBlk) != TRUE)
+	{
+		RELEASE_NDIS_PACKET(pAd, pTxBlk->pPacket, NDIS_STATUS_FAILURE);
+		return;
+	}
+
+	ASSERT(TX_BLK_TEST_FLAG(pTxBlk, fTX_bAllowFrag));
+	bVLANPkt = (RTMP_GET_PACKET_VLAN(pTxBlk->pPacket) ? TRUE : FALSE);
+
+	STAFindCipherAlgorithm(pAd, pTxBlk);
+	STABuildCommon802_11Header(pAd, pTxBlk);
+
+	if (pTxBlk->CipherAlg == CIPHER_TKIP)
+	{
+		pTxBlk->pPacket = duplicate_pkt_with_TKIP_MIC(pAd, pTxBlk->pPacket);
+		if (pTxBlk->pPacket == NULL)
+			return;
+		RTMP_QueryPacketInfo(pTxBlk->pPacket, &PacketInfo, &pTxBlk->pSrcBufHeader, &pTxBlk->SrcBufLen);
+	}
+
+	// skip 802.3 header
+	pTxBlk->pSrcBufData = pTxBlk->pSrcBufHeader + LENGTH_802_3;
+	pTxBlk->SrcBufLen  -= LENGTH_802_3;
+
+
+	// skip vlan tag
+	if (bVLANPkt)
+	{
+		pTxBlk->pSrcBufData	+= LENGTH_802_1Q;
+		pTxBlk->SrcBufLen	-= LENGTH_802_1Q;
+	}
+
+	pHeaderBufPtr = &pTxBlk->HeaderBuf[TXINFO_SIZE + TXWI_SIZE];
+	pHeader_802_11 = (HEADER_802_11 *)pHeaderBufPtr;
+
+
+	// skip common header
+	pHeaderBufPtr += pTxBlk->MpduHeaderLen;
+
+	if (TX_BLK_TEST_FLAG(pTxBlk, fTX_bWMM))
+	{
+		//
+		// build QOS Control bytes
+		//
+		*pHeaderBufPtr = (pTxBlk->UserPriority & 0x0F);
+
+		*(pHeaderBufPtr+1) = 0;
+		pHeaderBufPtr +=2;
+		pTxBlk->MpduHeaderLen += 2;
+	}
+
+	//
+	// padding at front of LLC header
+	// LLC header should locate at 4-octets aligment
+	//
+	pTxBlk->HdrPadLen = (ULONG)pHeaderBufPtr;
+	pHeaderBufPtr = (PCHAR) ROUND_UP(pHeaderBufPtr, 4);
+	pTxBlk->HdrPadLen = (ULONG)(pHeaderBufPtr - pTxBlk->HdrPadLen);
+
+
+
+	//
+	// Insert LLC-SNAP encapsulation - 8 octets
+	//
+	//
+   	// if original Ethernet frame contains no LLC/SNAP,
+	// then an extra LLC/SNAP encap is required
+	//
+	EXTRA_LLCSNAP_ENCAP_FROM_PKT_START(pTxBlk->pSrcBufHeader, pTxBlk->pExtraLlcSnapEncap);
+	if (pTxBlk->pExtraLlcSnapEncap)
+	{
+		UCHAR vlan_size;
+
+		NdisMoveMemory(pHeaderBufPtr, pTxBlk->pExtraLlcSnapEncap, 6);
+		pHeaderBufPtr += 6;
+		// skip vlan tag
+		vlan_size =  (bVLANPkt) ? LENGTH_802_1Q : 0;
+		// get 2 octets (TypeofLen)
+		NdisMoveMemory(pHeaderBufPtr, pTxBlk->pSrcBufHeader+12+vlan_size, 2);
+		pHeaderBufPtr += 2;
+		pTxBlk->MpduHeaderLen += LENGTH_802_1_H;
+	}
+
+
+	// If TKIP is used and fragmentation is required. Driver has to
+	//	append TKIP MIC at tail of the scatter buffer
+	//	MAC ASIC will only perform IV/EIV/ICV insertion but no TKIP MIC
+	if (pTxBlk->CipherAlg == CIPHER_TKIP)
+	{
+
+		// NOTE: DON'T refer the skb->len directly after following copy. Becasue the length is not adjust
+		//			to correct lenght, refer to pTxBlk->SrcBufLen for the packet length in following progress.
+		NdisMoveMemory(pTxBlk->pSrcBufData + pTxBlk->SrcBufLen, &pAd->PrivateInfo.Tx.MIC[0], 8);
+		//skb_put((RTPKT_TO_OSPKT(pTxBlk->pPacket))->tail, 8);
+		pTxBlk->SrcBufLen += 8;
+		pTxBlk->TotalFrameLen += 8;
+		pTxBlk->CipherAlg = CIPHER_TKIP_NO_MIC;
+	}
+
+	//
+	// calcuate the overhead bytes that encryption algorithm may add. This
+	// affects the calculate of "duration" field
+	//
+	if ((pTxBlk->CipherAlg == CIPHER_WEP64) || (pTxBlk->CipherAlg == CIPHER_WEP128))
+		EncryptionOverhead = 8; //WEP: IV[4] + ICV[4];
+	else if (pTxBlk->CipherAlg == CIPHER_TKIP_NO_MIC)
+		EncryptionOverhead = 12;//TKIP: IV[4] + EIV[4] + ICV[4], MIC will be added to TotalPacketLength
+	else if (pTxBlk->CipherAlg == CIPHER_TKIP)
+		EncryptionOverhead = 20;//TKIP: IV[4] + EIV[4] + ICV[4] + MIC[8]
+	else if (pTxBlk->CipherAlg == CIPHER_AES)
+		EncryptionOverhead = 16;	// AES: IV[4] + EIV[4] + MIC[8]
+	else
+		EncryptionOverhead = 0;
+
+	// decide how much time an ACK/CTS frame will consume in the air
+	AckDuration = RTMPCalcDuration(pAd, pAd->CommonCfg.ExpectedACKRate[pTxBlk->TxRate], 14);
+
+	// Init the total payload length of this frame.
+	SrcRemainingBytes = pTxBlk->SrcBufLen;
+
+	pTxBlk->TotalFragNum = 0xff;
+
+	do {
+
+		FreeMpduSize = pAd->CommonCfg.FragmentThreshold - LENGTH_CRC;
+
+		FreeMpduSize -= pTxBlk->MpduHeaderLen;
+
+		if (SrcRemainingBytes <= FreeMpduSize)
+		{	// this is the last or only fragment
+
+			pTxBlk->SrcBufLen = SrcRemainingBytes;
+
+			pHeader_802_11->FC.MoreFrag = 0;
+			pHeader_802_11->Duration = pAd->CommonCfg.Dsifs + AckDuration;
+
+			// Indicate the lower layer that this's the last fragment.
+			pTxBlk->TotalFragNum = fragNum;
+		}
+		else
+		{	// more fragment is required
+
+			pTxBlk->SrcBufLen = FreeMpduSize;
+
+			NextMpduSize = min(((UINT)SrcRemainingBytes - pTxBlk->SrcBufLen), ((UINT)pAd->CommonCfg.FragmentThreshold));
+			pHeader_802_11->FC.MoreFrag = 1;
+			pHeader_802_11->Duration = (3 * pAd->CommonCfg.Dsifs) + (2 * AckDuration) + RTMPCalcDuration(pAd, pTxBlk->TxRate, NextMpduSize + EncryptionOverhead);
+		}
+
+		if (fragNum == 0)
+			pTxBlk->FrameGap = IFS_HTTXOP;
+		else
+			pTxBlk->FrameGap = IFS_SIFS;
+
+		RTMPWriteTxWI_Data(pAd, (PTXWI_STRUC)(&pTxBlk->HeaderBuf[TXINFO_SIZE]), pTxBlk);
+
+		HAL_WriteFragTxResource(pAd, pTxBlk, fragNum, &FreeNumber);
+
+		pAd->RalinkCounters.KickTxCount++;
+		pAd->RalinkCounters.OneSecTxDoneCount++;
+
+		// Update the frame number, remaining size of the NDIS packet payload.
+
+		// space for 802.11 header.
+		if (fragNum == 0 && pTxBlk->pExtraLlcSnapEncap)
+			pTxBlk->MpduHeaderLen -= LENGTH_802_1_H;
+
+		fragNum++;
+		SrcRemainingBytes -= pTxBlk->SrcBufLen;
+		pTxBlk->pSrcBufData += pTxBlk->SrcBufLen;
+
+		pHeader_802_11->Frag++;	 // increase Frag #
+
+	}while(SrcRemainingBytes > 0);
+
+	//
+	// Kick out Tx
+	//
+	HAL_KickOutTx(pAd, pTxBlk, pTxBlk->QueIdx);
+}
+
+
+#define RELEASE_FRAMES_OF_TXBLK(_pAd, _pTxBlk, _pQEntry, _Status) 										\
+		while(_pTxBlk->TxPacketList.Head)														\
+		{																						\
+			_pQEntry = RemoveHeadQueue(&_pTxBlk->TxPacketList);									\
+			RELEASE_NDIS_PACKET(_pAd, QUEUE_ENTRY_TO_PACKET(_pQEntry), _Status);	\
+		}
+
+
+/*
+	========================================================================
+
+	Routine Description:
+		Copy frame from waiting queue into relative ring buffer and set
+	appropriate ASIC register to kick hardware encryption before really
+	sent out to air.
+
+	Arguments:
+		pAd 	Pointer to our adapter
+		PNDIS_PACKET	Pointer to outgoing Ndis frame
+		NumberOfFrag	Number of fragment required
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+NDIS_STATUS STAHardTransmit(
+	IN PRTMP_ADAPTER	pAd,
+	IN TX_BLK 			*pTxBlk,
+	IN	UCHAR			QueIdx)
+{
+	NDIS_PACKET		*pPacket;
+	PQUEUE_ENTRY	pQEntry;
+
+	// ---------------------------------------------
+	// STEP 0. DO SANITY CHECK AND SOME EARLY PREPARATION.
+	// ---------------------------------------------
+	//
+	ASSERT(pTxBlk->TxPacketList.Number);
+	if (pTxBlk->TxPacketList.Head == NULL)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("pTxBlk->TotalFrameNum == %ld!\n", pTxBlk->TxPacketList.Number));
+		return NDIS_STATUS_FAILURE;
+	}
+
+	pPacket = QUEUE_ENTRY_TO_PACKET(pTxBlk->TxPacketList.Head);
+
+#if 0 //def CARRIER_DETECTION_SUPPORT // Roger sync Carrier
+		if ((pAd->CommonCfg.CarrierDetect.Enable == TRUE) && (isCarrierDetectExist(pAd) == TRUE))
+	{
+		DBGPRINT(RT_DEBUG_INFO,("STAHardTransmit --> radar detect not in normal mode !!!\n"));
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+		return (NDIS_STATUS_FAILURE);
+	}
+#endif // CARRIER_DETECTION_SUPPORT //
+
+	// ------------------------------------------------------------------
+	// STEP 1. WAKE UP PHY
+	//		outgoing frame always wakeup PHY to prevent frame lost and
+	//		turn off PSM bit to improve performance
+	// ------------------------------------------------------------------
+	// not to change PSM bit, just send this frame out?
+	if ((pAd->StaCfg.Psm == PWR_SAVE) && OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+	{
+	    DBGPRINT_RAW(RT_DEBUG_TRACE, ("AsicForceWakeup At HardTx\n"));
+		AsicForceWakeup(pAd, TRUE);
+	}
+
+	// It should not change PSM bit, when APSD turn on.
+	if ((!(pAd->CommonCfg.bAPSDCapable && pAd->CommonCfg.APEdcaParm.bAPSDCapable) && (pAd->CommonCfg.bAPSDForcePowerSave == FALSE))
+		|| (RTMP_GET_PACKET_EAPOL(pTxBlk->pPacket))
+		|| (RTMP_GET_PACKET_WAI(pTxBlk->pPacket)))
+	{
+		if ((pAd->StaCfg.Psm == PWR_SAVE) &&
+            (pAd->StaCfg.WindowsPowerMode == Ndis802_11PowerModeFast_PSP))
+			MlmeSetPsmBit(pAd, PWR_ACTIVE);
+	}
+
+	switch (pTxBlk->TxFrameType)
+	{
+#ifdef DOT11_N_SUPPORT
+		case TX_AMPDU_FRAME:
+				STA_AMPDU_Frame_Tx(pAd, pTxBlk);
+			break;
+		case TX_AMSDU_FRAME:
+				STA_AMSDU_Frame_Tx(pAd, pTxBlk);
+			break;
+#endif // DOT11_N_SUPPORT //
+		case TX_LEGACY_FRAME:
+				STA_Legacy_Frame_Tx(pAd, pTxBlk);
+			break;
+		case TX_MCAST_FRAME:
+				STA_Legacy_Frame_Tx(pAd, pTxBlk);
+			break;
+		case TX_RALINK_FRAME:
+				STA_ARalink_Frame_Tx(pAd, pTxBlk);
+			break;
+		case TX_FRAG_FRAME:
+				STA_Fragment_Frame_Tx(pAd, pTxBlk);
+			break;
+		default:
+			{
+				// It should not happened!
+				DBGPRINT(RT_DEBUG_ERROR, ("Send a pacekt was not classified!! It should not happen!\n"));
+				while(pTxBlk->TxPacketList.Number)
+				{
+					pQEntry = RemoveHeadQueue(&pTxBlk->TxPacketList);
+					pPacket = QUEUE_ENTRY_TO_PACKET(pQEntry);
+					if (pPacket)
+						RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+				}
+			}
+			break;
+	}
+
+	return (NDIS_STATUS_SUCCESS);
+
+}
+
+ULONG  HashBytesPolynomial(UCHAR *value, unsigned int len)
+{
+   unsigned char *word = value;
+   unsigned int ret = 0;
+   unsigned int i;
+
+   for(i=0; i < len; i++)
+   {
+	  int mod = i % 32;
+	  ret ^=(unsigned int) (word[i]) << mod;
+	  ret ^=(unsigned int) (word[i]) >> (32 - mod);
+   }
+   return ret;
+}
+
+VOID Sta_Announce_or_Forward_802_3_Packet(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PNDIS_PACKET	pPacket,
+	IN	UCHAR			FromWhichBSSID)
+{
+	if (TRUE
+		)
+	{
+		announce_802_3_packet(pAd, pPacket);
+	}
+	else
+	{
+		// release packet
+		RELEASE_NDIS_PACKET(pAd, pPacket, NDIS_STATUS_FAILURE);
+	}
+}
+
diff --git a/drivers/staging/rt2870/sta/sanity.c b/drivers/staging/rt2870/sta/sanity.c
new file mode 100644
index 00000000000..239872464be
--- /dev/null
+++ b/drivers/staging/rt2870/sta/sanity.c
@@ -0,0 +1,420 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	sanity.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John Chang  2004-09-01      add WMM support
+*/
+#include "../rt_config.h"
+
+extern UCHAR	CISCO_OUI[];
+
+extern UCHAR	WPA_OUI[];
+extern UCHAR	RSN_OUI[];
+extern UCHAR	WME_INFO_ELEM[];
+extern UCHAR	WME_PARM_ELEM[];
+extern UCHAR	Ccx2QosInfo[];
+extern UCHAR	RALINK_OUI[];
+extern UCHAR	BROADCOM_OUI[];
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+ */
+BOOLEAN MlmeStartReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT CHAR Ssid[],
+    OUT UCHAR *pSsidLen)
+{
+    MLME_START_REQ_STRUCT *Info;
+
+    Info = (MLME_START_REQ_STRUCT *)(Msg);
+
+    if (Info->SsidLen > MAX_LEN_OF_SSID)
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("MlmeStartReqSanity fail - wrong SSID length\n"));
+        return FALSE;
+    }
+
+    *pSsidLen = Info->SsidLen;
+    NdisMoveMemory(Ssid, Info->Ssid, *pSsidLen);
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+    IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN PeerAssocRspSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *pMsg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT USHORT *pCapabilityInfo,
+    OUT USHORT *pStatus,
+    OUT USHORT *pAid,
+    OUT UCHAR SupRate[],
+    OUT UCHAR *pSupRateLen,
+    OUT UCHAR ExtRate[],
+    OUT UCHAR *pExtRateLen,
+    OUT HT_CAPABILITY_IE		*pHtCapability,
+    OUT ADD_HT_INFO_IE		*pAddHtInfo,	// AP might use this additional ht info IE
+    OUT UCHAR			*pHtCapabilityLen,
+    OUT UCHAR			*pAddHtInfoLen,
+    OUT UCHAR			*pNewExtChannelOffset,
+    OUT PEDCA_PARM pEdcaParm,
+    OUT UCHAR *pCkipFlag)
+{
+    CHAR          IeType, *Ptr;
+    PFRAME_802_11 pFrame = (PFRAME_802_11)pMsg;
+    PEID_STRUCT   pEid;
+    ULONG         Length = 0;
+
+	*pNewExtChannelOffset = 0xff;
+	*pHtCapabilityLen = 0;
+	*pAddHtInfoLen = 0;
+    COPY_MAC_ADDR(pAddr2, pFrame->Hdr.Addr2);
+    Ptr = pFrame->Octet;
+    Length += LENGTH_802_11;
+
+    NdisMoveMemory(pCapabilityInfo, &pFrame->Octet[0], 2);
+    Length += 2;
+    NdisMoveMemory(pStatus,         &pFrame->Octet[2], 2);
+    Length += 2;
+    *pCkipFlag = 0;
+    *pExtRateLen = 0;
+    pEdcaParm->bValid = FALSE;
+
+    if (*pStatus != MLME_SUCCESS)
+        return TRUE;
+
+    NdisMoveMemory(pAid, &pFrame->Octet[4], 2);
+    Length += 2;
+
+    // Aid already swaped byte order in RTMPFrameEndianChange() for big endian platform
+    *pAid = (*pAid) & 0x3fff; // AID is low 14-bit
+
+    // -- get supported rates from payload and advance the pointer
+    IeType = pFrame->Octet[6];
+    *pSupRateLen = pFrame->Octet[7];
+    if ((IeType != IE_SUPP_RATES) || (*pSupRateLen > MAX_LEN_OF_SUPPORTED_RATES))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("PeerAssocRspSanity fail - wrong SupportedRates IE\n"));
+        return FALSE;
+    }
+    else
+        NdisMoveMemory(SupRate, &pFrame->Octet[8], *pSupRateLen);
+
+    Length = Length + 2 + *pSupRateLen;
+
+    // many AP implement proprietary IEs in non-standard order, we'd better
+    // tolerate mis-ordered IEs to get best compatibility
+    pEid = (PEID_STRUCT) &pFrame->Octet[8 + (*pSupRateLen)];
+
+    // get variable fields from payload and advance the pointer
+    while ((Length + 2 + pEid->Len) <= MsgLen)
+    {
+        switch (pEid->Eid)
+        {
+            case IE_EXT_SUPP_RATES:
+                if (pEid->Len <= MAX_LEN_OF_SUPPORTED_RATES)
+                {
+                    NdisMoveMemory(ExtRate, pEid->Octet, pEid->Len);
+                    *pExtRateLen = pEid->Len;
+                }
+                break;
+
+             case IE_HT_CAP:
+            case IE_HT_CAP2:
+			if (pEid->Len >= SIZE_HT_CAP_IE)  //Note: allow extension.!!
+			{
+				NdisMoveMemory(pHtCapability, pEid->Octet, SIZE_HT_CAP_IE);
+
+				*(USHORT *)(&pHtCapability->HtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->HtCapInfo));
+				*(USHORT *)(&pHtCapability->ExtHtCapInfo) = cpu2le16(*(USHORT *)(&pHtCapability->ExtHtCapInfo));
+
+				*pHtCapabilityLen = SIZE_HT_CAP_IE;
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_WARN, ("PeerAssocRspSanity - wrong IE_HT_CAP. \n"));
+			}
+
+		break;
+#ifdef DOT11_N_SUPPORT
+            case IE_ADD_HT:
+            case IE_ADD_HT2:
+			if (pEid->Len >= sizeof(ADD_HT_INFO_IE))
+			{
+				// This IE allows extension, but we can ignore extra bytes beyond our knowledge , so only
+				// copy first sizeof(ADD_HT_INFO_IE)
+				NdisMoveMemory(pAddHtInfo, pEid->Octet, sizeof(ADD_HT_INFO_IE));
+
+				*(USHORT *)(&pAddHtInfo->AddHtInfo2) = cpu2le16(*(USHORT *)(&pAddHtInfo->AddHtInfo2));
+				*(USHORT *)(&pAddHtInfo->AddHtInfo3) = cpu2le16(*(USHORT *)(&pAddHtInfo->AddHtInfo3));
+
+				*pAddHtInfoLen = SIZE_ADD_HT_INFO_IE;
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_WARN, ("PeerAssocRspSanity - wrong IE_ADD_HT. \n"));
+			}
+
+		break;
+            case IE_SECONDARY_CH_OFFSET:
+			if (pEid->Len == 1)
+			{
+				*pNewExtChannelOffset = pEid->Octet[0];
+			}
+			else
+			{
+				DBGPRINT(RT_DEBUG_WARN, ("PeerAssocRspSanity - wrong IE_SECONDARY_CH_OFFSET. \n"));
+			}
+#endif // DOT11_N_SUPPORT //
+		break;
+            case IE_AIRONET_CKIP:
+                // 0. Check Aironet IE length, it must be larger or equal to 28
+                //    Cisco's AP VxWork version(will not be supported) used this IE length as 28
+                //    Cisco's AP IOS version used this IE length as 30
+                if (pEid->Len < (CKIP_NEGOTIATION_LENGTH - 2))
+                break;
+
+                // 1. Copy CKIP flag byte to buffer for process
+                *pCkipFlag = *(pEid->Octet + 8);
+                break;
+
+            case IE_AIRONET_IPADDRESS:
+                if (pEid->Len != 0x0A)
+                break;
+
+                // Get Cisco Aironet IP information
+                if (NdisEqualMemory(pEid->Octet, CISCO_OUI, 3) == 1)
+                    NdisMoveMemory(pAd->StaCfg.AironetIPAddress, pEid->Octet + 4, 4);
+                break;
+
+            // CCX2, WMM use the same IE value
+            // case IE_CCX_V2:
+            case IE_VENDOR_SPECIFIC:
+                // handle WME PARAMTER ELEMENT
+                if (NdisEqualMemory(pEid->Octet, WME_PARM_ELEM, 6) && (pEid->Len == 24))
+                {
+                    PUCHAR ptr;
+                    int i;
+
+                    // parsing EDCA parameters
+                    pEdcaParm->bValid          = TRUE;
+                    pEdcaParm->bQAck           = FALSE; // pEid->Octet[0] & 0x10;
+                    pEdcaParm->bQueueRequest   = FALSE; // pEid->Octet[0] & 0x20;
+                    pEdcaParm->bTxopRequest    = FALSE; // pEid->Octet[0] & 0x40;
+                    //pEdcaParm->bMoreDataAck    = FALSE; // pEid->Octet[0] & 0x80;
+                    pEdcaParm->EdcaUpdateCount = pEid->Octet[6] & 0x0f;
+                    pEdcaParm->bAPSDCapable    = (pEid->Octet[6] & 0x80) ? 1 : 0;
+                    ptr = &pEid->Octet[8];
+                    for (i=0; i<4; i++)
+                    {
+                        UCHAR aci = (*ptr & 0x60) >> 5; // b5~6 is AC INDEX
+                        pEdcaParm->bACM[aci]  = (((*ptr) & 0x10) == 0x10);   // b5 is ACM
+                        pEdcaParm->Aifsn[aci] = (*ptr) & 0x0f;               // b0~3 is AIFSN
+                        pEdcaParm->Cwmin[aci] = *(ptr+1) & 0x0f;             // b0~4 is Cwmin
+                        pEdcaParm->Cwmax[aci] = *(ptr+1) >> 4;               // b5~8 is Cwmax
+                        pEdcaParm->Txop[aci]  = *(ptr+2) + 256 * (*(ptr+3)); // in unit of 32-us
+                        ptr += 4; // point to next AC
+                    }
+                }
+
+                // handle CCX IE
+                else
+                {
+                    // 0. Check the size and CCX admin control
+                    if (pAd->StaCfg.CCXControl.field.Enable == 0)
+                        break;
+                    if (pEid->Len != 5)
+                        break;
+
+                    // Turn CCX2 if matched
+                    if (NdisEqualMemory(pEid->Octet, Ccx2IeInfo, 5) == 1)
+                        pAd->StaCfg.CCXEnable = TRUE;
+                    break;
+                }
+                break;
+
+            default:
+                DBGPRINT(RT_DEBUG_TRACE, ("PeerAssocRspSanity - ignore unrecognized EID = %d\n", pEid->Eid));
+                break;
+        }
+
+        Length = Length + 2 + pEid->Len;
+        pEid = (PEID_STRUCT)((UCHAR*)pEid + 2 + pEid->Len);
+    }
+
+    // Force CCX2 enable to TRUE for those AP didn't replay CCX v2 IE, we still force it to be on
+    if (pAd->StaCfg.CCXControl.field.Enable == 1)
+        pAd->StaCfg.CCXEnable = TRUE;
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        MLME message sanity check
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN PeerProbeReqSanity(
+    IN PRTMP_ADAPTER pAd,
+    IN VOID *Msg,
+    IN ULONG MsgLen,
+    OUT PUCHAR pAddr2,
+    OUT CHAR Ssid[],
+    OUT UCHAR *pSsidLen)
+{
+    UCHAR         Idx;
+    UCHAR	      RateLen;
+    CHAR          IeType;
+    PFRAME_802_11 pFrame = (PFRAME_802_11)Msg;
+
+    COPY_MAC_ADDR(pAddr2, pFrame->Hdr.Addr2);
+
+    if ((pFrame->Octet[0] != IE_SSID) || (pFrame->Octet[1] > MAX_LEN_OF_SSID))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("PeerProbeReqSanity fail - wrong SSID IE(Type=%d,Len=%d)\n",pFrame->Octet[0],pFrame->Octet[1]));
+        return FALSE;
+    }
+
+    *pSsidLen = pFrame->Octet[1];
+    NdisMoveMemory(Ssid, &pFrame->Octet[2], *pSsidLen);
+
+    Idx = *pSsidLen + 2;
+
+    // -- get supported rates from payload and advance the pointer
+    IeType = pFrame->Octet[Idx];
+    RateLen = pFrame->Octet[Idx + 1];
+    if (IeType != IE_SUPP_RATES)
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("PeerProbeReqSanity fail - wrong SupportRates IE(Type=%d,Len=%d)\n",pFrame->Octet[Idx],pFrame->Octet[Idx+1]));
+        return FALSE;
+    }
+    else
+    {
+        if ((pAd->CommonCfg.PhyMode == PHY_11G) && (RateLen < 8))
+            return (FALSE);
+    }
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+
+	IRQL = DISPATCH_LEVEL
+
+    ==========================================================================
+ */
+BOOLEAN GetTimBit(
+    IN CHAR *Ptr,
+    IN USHORT Aid,
+    OUT UCHAR *TimLen,
+    OUT UCHAR *BcastFlag,
+    OUT UCHAR *DtimCount,
+    OUT UCHAR *DtimPeriod,
+    OUT UCHAR *MessageToMe)
+{
+    UCHAR          BitCntl, N1, N2, MyByte, MyBit;
+    CHAR          *IdxPtr;
+
+    IdxPtr = Ptr;
+
+    IdxPtr ++;
+    *TimLen = *IdxPtr;
+
+    // get DTIM Count from TIM element
+    IdxPtr ++;
+    *DtimCount = *IdxPtr;
+
+    // get DTIM Period from TIM element
+    IdxPtr++;
+    *DtimPeriod = *IdxPtr;
+
+    // get Bitmap Control from TIM element
+    IdxPtr++;
+    BitCntl = *IdxPtr;
+
+    if ((*DtimCount == 0) && (BitCntl & 0x01))
+        *BcastFlag = TRUE;
+    else
+        *BcastFlag = FALSE;
+
+    // Parse Partial Virtual Bitmap from TIM element
+    N1 = BitCntl & 0xfe;    // N1 is the first bitmap byte#
+    N2 = *TimLen - 4 + N1;  // N2 is the last bitmap byte#
+
+    if ((Aid < (N1 << 3)) || (Aid >= ((N2 + 1) << 3)))
+        *MessageToMe = FALSE;
+    else
+    {
+        MyByte = (Aid >> 3) - N1;                       // my byte position in the bitmap byte-stream
+        MyBit = Aid % 16 - ((MyByte & 0x01)? 8:0);
+
+        IdxPtr += (MyByte + 1);
+
+        //if (*IdxPtr)
+        //    DBGPRINT(RT_DEBUG_WARN, ("TIM bitmap = 0x%02x\n", *IdxPtr));
+
+        if (*IdxPtr & (0x01 << MyBit))
+            *MessageToMe = TRUE;
+        else
+            *MessageToMe = FALSE;
+    }
+
+    return TRUE;
+}
+
diff --git a/drivers/staging/rt2870/sta/sync.c b/drivers/staging/rt2870/sta/sync.c
new file mode 100644
index 00000000000..a48975566e0
--- /dev/null
+++ b/drivers/staging/rt2870/sta/sync.c
@@ -0,0 +1,1753 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	sync.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	John Chang	2004-09-01      modified for rt2561/2661
+	Jan Lee		2006-08-01      modified for rt2860 for 802.11n
+*/
+#include "../rt_config.h"
+
+#define ADHOC_ENTRY_BEACON_LOST_TIME	(2*OS_HZ)	// 2 sec
+
+/*
+	==========================================================================
+	Description:
+		The sync state machine,
+	Parameters:
+		Sm - pointer to the state machine
+	Note:
+		the state machine looks like the following
+
+	==========================================================================
+ */
+VOID SyncStateMachineInit(
+	IN PRTMP_ADAPTER pAd,
+	IN STATE_MACHINE *Sm,
+	OUT STATE_MACHINE_FUNC Trans[])
+{
+	StateMachineInit(Sm, Trans, MAX_SYNC_STATE, MAX_SYNC_MSG, (STATE_MACHINE_FUNC)Drop, SYNC_IDLE, SYNC_MACHINE_BASE);
+
+	// column 1
+	StateMachineSetAction(Sm, SYNC_IDLE, MT2_MLME_SCAN_REQ, (STATE_MACHINE_FUNC)MlmeScanReqAction);
+	StateMachineSetAction(Sm, SYNC_IDLE, MT2_MLME_JOIN_REQ, (STATE_MACHINE_FUNC)MlmeJoinReqAction);
+	StateMachineSetAction(Sm, SYNC_IDLE, MT2_MLME_START_REQ, (STATE_MACHINE_FUNC)MlmeStartReqAction);
+	StateMachineSetAction(Sm, SYNC_IDLE, MT2_PEER_BEACON, (STATE_MACHINE_FUNC)PeerBeacon);
+	StateMachineSetAction(Sm, SYNC_IDLE, MT2_PEER_PROBE_REQ, (STATE_MACHINE_FUNC)PeerProbeReqAction);
+
+	//column 2
+	StateMachineSetAction(Sm, JOIN_WAIT_BEACON, MT2_MLME_SCAN_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenScan);
+	StateMachineSetAction(Sm, JOIN_WAIT_BEACON, MT2_MLME_JOIN_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenJoin);
+	StateMachineSetAction(Sm, JOIN_WAIT_BEACON, MT2_MLME_START_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenStart);
+	StateMachineSetAction(Sm, JOIN_WAIT_BEACON, MT2_PEER_BEACON, (STATE_MACHINE_FUNC)PeerBeaconAtJoinAction);
+	StateMachineSetAction(Sm, JOIN_WAIT_BEACON, MT2_BEACON_TIMEOUT, (STATE_MACHINE_FUNC)BeaconTimeoutAtJoinAction);
+
+	// column 3
+	StateMachineSetAction(Sm, SCAN_LISTEN, MT2_MLME_SCAN_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenScan);
+	StateMachineSetAction(Sm, SCAN_LISTEN, MT2_MLME_JOIN_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenJoin);
+	StateMachineSetAction(Sm, SCAN_LISTEN, MT2_MLME_START_REQ, (STATE_MACHINE_FUNC)InvalidStateWhenStart);
+	StateMachineSetAction(Sm, SCAN_LISTEN, MT2_PEER_BEACON, (STATE_MACHINE_FUNC)PeerBeaconAtScanAction);
+	StateMachineSetAction(Sm, SCAN_LISTEN, MT2_PEER_PROBE_RSP, (STATE_MACHINE_FUNC)PeerBeaconAtScanAction);
+	StateMachineSetAction(Sm, SCAN_LISTEN, MT2_SCAN_TIMEOUT, (STATE_MACHINE_FUNC)ScanTimeoutAction);
+
+	// timer init
+	RTMPInitTimer(pAd, &pAd->MlmeAux.BeaconTimer, GET_TIMER_FUNCTION(BeaconTimeout), pAd, FALSE);
+	RTMPInitTimer(pAd, &pAd->MlmeAux.ScanTimer, GET_TIMER_FUNCTION(ScanTimeout), pAd, FALSE);
+}
+
+/*
+	==========================================================================
+	Description:
+		Beacon timeout handler, executed in timer thread
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID BeaconTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+	RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+
+	DBGPRINT(RT_DEBUG_TRACE,("SYNC - BeaconTimeout\n"));
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS))
+		return;
+
+#ifdef DOT11_N_SUPPORT
+	if ((pAd->CommonCfg.BBPCurrentBW == BW_40)
+		)
+	{
+		UCHAR        BBPValue = 0;
+		AsicSwitchChannel(pAd, pAd->CommonCfg.CentralChannel, FALSE);
+		AsicLockChannel(pAd, pAd->CommonCfg.CentralChannel);
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+		BBPValue &= (~0x18);
+		BBPValue |= 0x10;
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+		DBGPRINT(RT_DEBUG_TRACE, ("SYNC - End of SCAN, restore to 40MHz channel %d, Total BSS[%02d]\n",pAd->CommonCfg.CentralChannel, pAd->ScanTab.BssNr));
+	}
+#endif // DOT11_N_SUPPORT //
+
+	MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_BEACON_TIMEOUT, 0, NULL);
+	RT28XX_MLME_HANDLER(pAd);
+}
+
+/*
+	==========================================================================
+	Description:
+		Scan timeout handler, executed in timer thread
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID ScanTimeout(
+	IN PVOID SystemSpecific1,
+	IN PVOID FunctionContext,
+	IN PVOID SystemSpecific2,
+	IN PVOID SystemSpecific3)
+{
+	RTMP_ADAPTER *pAd = (RTMP_ADAPTER *)FunctionContext;
+
+
+	// Do nothing if the driver is starting halt state.
+	// This might happen when timer already been fired before cancel timer with mlmehalt
+	if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_HALT_IN_PROGRESS))
+		return;
+
+	if (MlmeEnqueue(pAd, SYNC_STATE_MACHINE, MT2_SCAN_TIMEOUT, 0, NULL))
+	{
+		RT28XX_MLME_HANDLER(pAd);
+	}
+	else
+	{
+		// To prevent SyncMachine.CurrState is SCAN_LISTEN forever.
+		pAd->MlmeAux.Channel = 0;
+		ScanNextChannel(pAd);
+		if (pAd->CommonCfg.bWirelessEvent)
+		{
+			RTMPSendWirelessEvent(pAd, IW_SCAN_ENQUEUE_FAIL_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+		}
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+		MLME SCAN req state machine procedure
+	==========================================================================
+ */
+VOID MlmeScanReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR          Ssid[MAX_LEN_OF_SSID], SsidLen, ScanType, BssType, BBPValue = 0;
+	BOOLEAN        TimerCancelled;
+	ULONG		   Now;
+	USHORT         Status;
+	PHEADER_802_11 pHdr80211;
+	PUCHAR         pOutBuffer = NULL;
+	NDIS_STATUS    NStatus;
+
+	// Check the total scan tries for one single OID command
+	// If this is the CCX 2.0 Case, skip that!
+	if ( !RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_START_UP))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("SYNC - MlmeScanReqAction before Startup\n"));
+		return;
+	}
+
+	// Increase the scan retry counters.
+	pAd->StaCfg.ScanCnt++;
+
+
+	// first check the parameter sanity
+	if (MlmeScanReqSanity(pAd,
+						  Elem->Msg,
+						  Elem->MsgLen,
+						  &BssType,
+						  Ssid,
+						  &SsidLen,
+						  &ScanType))
+	{
+
+		// Check for channel load and noise hist request
+		// Suspend MSDU only at scan request, not the last two mentioned
+		if ((ScanType == SCAN_CISCO_NOISE) || (ScanType == SCAN_CISCO_CHANNEL_LOAD))
+		{
+			if (pAd->StaCfg.CCXScanChannel != pAd->CommonCfg.Channel)
+				RTMPSuspendMsduTransmission(pAd);			// Suspend MSDU transmission here
+		}
+		else
+		{
+			// Suspend MSDU transmission here
+			RTMPSuspendMsduTransmission(pAd);
+		}
+
+		//
+		// To prevent data lost.
+		// Send an NULL data with turned PSM bit on to current associated AP before SCAN progress.
+		// And should send an NULL data with turned PSM bit off to AP, when scan progress done
+		//
+		if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED) && (INFRA_ON(pAd)))
+		{
+			NStatus = MlmeAllocateMemory(pAd, (PVOID)&pOutBuffer);
+			if (NStatus	== NDIS_STATUS_SUCCESS)
+			{
+				pHdr80211 = (PHEADER_802_11) pOutBuffer;
+				MgtMacHeaderInit(pAd, pHdr80211, SUBTYPE_NULL_FUNC, 1, pAd->CommonCfg.Bssid, pAd->CommonCfg.Bssid);
+				pHdr80211->Duration = 0;
+				pHdr80211->FC.Type = BTYPE_DATA;
+				pHdr80211->FC.PwrMgmt = PWR_SAVE;
+
+				// Send using priority queue
+				MiniportMMRequest(pAd, 0, pOutBuffer, sizeof(HEADER_802_11));
+				DBGPRINT(RT_DEBUG_TRACE, ("MlmeScanReqAction -- Send PSM Data frame for off channel RM\n"));
+				MlmeFreeMemory(pAd, pOutBuffer);
+				RTMPusecDelay(5000);
+			}
+		}
+
+		NdisGetSystemUpTime(&Now);
+		pAd->StaCfg.LastScanTime = Now;
+		// reset all the timers
+		RTMPCancelTimer(&pAd->MlmeAux.BeaconTimer, &TimerCancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.ScanTimer, &TimerCancelled);
+
+		// record desired BSS parameters
+		pAd->MlmeAux.BssType = BssType;
+		pAd->MlmeAux.ScanType = ScanType;
+		pAd->MlmeAux.SsidLen = SsidLen;
+        NdisZeroMemory(pAd->MlmeAux.Ssid, MAX_LEN_OF_SSID);
+		NdisMoveMemory(pAd->MlmeAux.Ssid, Ssid, SsidLen);
+
+		// start from the first channel
+		pAd->MlmeAux.Channel = FirstChannel(pAd);
+
+		// Change the scan channel when dealing with CCX beacon report
+		if ((ScanType == SCAN_CISCO_PASSIVE) || (ScanType == SCAN_CISCO_ACTIVE) ||
+			(ScanType == SCAN_CISCO_CHANNEL_LOAD) || (ScanType == SCAN_CISCO_NOISE))
+			pAd->MlmeAux.Channel = pAd->StaCfg.CCXScanChannel;
+
+		// Let BBP register at 20MHz to do scan
+		RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+		BBPValue &= (~0x18);
+		RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+		DBGPRINT(RT_DEBUG_TRACE, ("SYNC - BBP R4 to 20MHz.l\n"));
+		ScanNextChannel(pAd);
+	}
+	else
+	{
+		DBGPRINT_ERR(("SYNC - MlmeScanReqAction() sanity check fail\n"));
+		pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+		Status = MLME_INVALID_FORMAT;
+		MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_SCAN_CONF, 2, &Status);
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+		MLME JOIN req state machine procedure
+	==========================================================================
+ */
+VOID MlmeJoinReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR        BBPValue = 0;
+	BSS_ENTRY    *pBss;
+	BOOLEAN       TimerCancelled;
+	HEADER_802_11 Hdr80211;
+	NDIS_STATUS   NStatus;
+	ULONG         FrameLen = 0;
+	PUCHAR        pOutBuffer = NULL;
+	PUCHAR        pSupRate = NULL;
+	UCHAR         SupRateLen;
+	PUCHAR        pExtRate = NULL;
+	UCHAR         ExtRateLen;
+	UCHAR         ASupRate[] = {0x8C, 0x12, 0x98, 0x24, 0xb0, 0x48, 0x60, 0x6C};
+	UCHAR         ASupRateLen = sizeof(ASupRate)/sizeof(UCHAR);
+	MLME_JOIN_REQ_STRUCT *pInfo = (MLME_JOIN_REQ_STRUCT *)(Elem->Msg);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("SYNC - MlmeJoinReqAction(BSS #%ld)\n", pInfo->BssIdx));
+
+
+	// reset all the timers
+	RTMPCancelTimer(&pAd->MlmeAux.ScanTimer, &TimerCancelled);
+	RTMPCancelTimer(&pAd->MlmeAux.BeaconTimer, &TimerCancelled);
+
+	pBss = &pAd->MlmeAux.SsidBssTab.BssEntry[pInfo->BssIdx];
+
+	// record the desired SSID & BSSID we're waiting for
+	COPY_MAC_ADDR(pAd->MlmeAux.Bssid, pBss->Bssid);
+
+	// If AP's SSID is not hidden, it is OK for updating ssid to MlmeAux again.
+	if (pBss->Hidden == 0)
+	{
+		NdisMoveMemory(pAd->MlmeAux.Ssid, pBss->Ssid, pBss->SsidLen);
+		pAd->MlmeAux.SsidLen = pBss->SsidLen;
+	}
+
+	pAd->MlmeAux.BssType = pBss->BssType;
+	pAd->MlmeAux.Channel = pBss->Channel;
+	pAd->MlmeAux.CentralChannel = pBss->CentralChannel;
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+	// Country IE of the AP will be evaluated and will be used.
+	if ((pAd->StaCfg.IEEE80211dClientMode != Rt802_11_D_None) &&
+		(pBss->bHasCountryIE == TRUE))
+	{
+		NdisMoveMemory(&pAd->CommonCfg.CountryCode[0], &pBss->CountryString[0], 2);
+		if (pBss->CountryString[2] == 'I')
+			pAd->CommonCfg.Geography = IDOR;
+		else if (pBss->CountryString[2] == 'O')
+			pAd->CommonCfg.Geography = ODOR;
+		else
+			pAd->CommonCfg.Geography = BOTH;
+		BuildChannelListEx(pAd);
+	}
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+	// Let BBP register at 20MHz to do scan
+	RTMP_BBP_IO_READ8_BY_REG_ID(pAd, BBP_R4, &BBPValue);
+	BBPValue &= (~0x18);
+	RTMP_BBP_IO_WRITE8_BY_REG_ID(pAd, BBP_R4, BBPValue);
+	DBGPRINT(RT_DEBUG_TRACE, ("SYNC - BBP R4 to 20MHz.l\n"));
+
+	// switch channel and waiting for beacon timer
+	AsicSwitchChannel(pAd, pAd->MlmeAux.Channel, FALSE);
+	AsicLockChannel(pAd, pAd->MlmeAux.Channel);
+	RTMPSetTimer(&pAd->MlmeAux.BeaconTimer, JOIN_TIMEOUT);
+
+    do
+	{
+		if (((pAd->CommonCfg.bIEEE80211H == 1) &&
+            (pAd->MlmeAux.Channel > 14) &&
+             RadarChannelCheck(pAd, pAd->MlmeAux.Channel))
+#ifdef CARRIER_DETECTION_SUPPORT // Roger sync Carrier
+             || (pAd->CommonCfg.CarrierDetect.Enable == TRUE)
+#endif // CARRIER_DETECTION_SUPPORT //
+            )
+		{
+			//
+			// We can't send any Probe request frame to meet 802.11h.
+			//
+			if (pBss->Hidden == 0)
+				break;
+		}
+
+		//
+		// send probe request
+		//
+		NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);
+		if (NStatus == NDIS_STATUS_SUCCESS)
+		{
+			if (pAd->MlmeAux.Channel <= 14)
+			{
+				pSupRate = pAd->CommonCfg.SupRate;
+				SupRateLen = pAd->CommonCfg.SupRateLen;
+				pExtRate = pAd->CommonCfg.ExtRate;
+				ExtRateLen = pAd->CommonCfg.ExtRateLen;
+			}
+			else
+			{
+				//
+				// Overwrite Support Rate, CCK rate are not allowed
+				//
+				pSupRate = ASupRate;
+				SupRateLen = ASupRateLen;
+				ExtRateLen = 0;
+			}
+
+			if (pAd->MlmeAux.BssType == BSS_INFRA)
+				MgtMacHeaderInit(pAd, &Hdr80211, SUBTYPE_PROBE_REQ, 0, pAd->MlmeAux.Bssid, pAd->MlmeAux.Bssid);
+			else
+				MgtMacHeaderInit(pAd, &Hdr80211, SUBTYPE_PROBE_REQ, 0, BROADCAST_ADDR, BROADCAST_ADDR);
+
+			MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+							  sizeof(HEADER_802_11),    &Hdr80211,
+							  1,                        &SsidIe,
+							  1,                        &pAd->MlmeAux.SsidLen,
+							  pAd->MlmeAux.SsidLen,	    pAd->MlmeAux.Ssid,
+							  1,                        &SupRateIe,
+							  1,                        &SupRateLen,
+							  SupRateLen,               pSupRate,
+							  END_OF_ARGS);
+
+			if (ExtRateLen)
+			{
+				ULONG Tmp;
+				MakeOutgoingFrame(pOutBuffer + FrameLen,            &Tmp,
+								  1,                                &ExtRateIe,
+								  1,                                &ExtRateLen,
+								  ExtRateLen,                       pExtRate,
+								  END_OF_ARGS);
+				FrameLen += Tmp;
+			}
+
+
+			MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+			MlmeFreeMemory(pAd, pOutBuffer);
+		}
+    } while (FALSE);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("SYNC - Switch to ch %d, Wait BEACON from %02x:%02x:%02x:%02x:%02x:%02x\n",
+		pBss->Channel, pBss->Bssid[0], pBss->Bssid[1], pBss->Bssid[2], pBss->Bssid[3], pBss->Bssid[4], pBss->Bssid[5]));
+
+	pAd->Mlme.SyncMachine.CurrState = JOIN_WAIT_BEACON;
+}
+
+/*
+	==========================================================================
+	Description:
+		MLME START Request state machine procedure, starting an IBSS
+	==========================================================================
+ */
+VOID MlmeStartReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR         Ssid[MAX_LEN_OF_SSID], SsidLen;
+	BOOLEAN       TimerCancelled;
+
+	// New for WPA security suites
+	UCHAR						VarIE[MAX_VIE_LEN]; 	// Total VIE length = MAX_VIE_LEN - -5
+	NDIS_802_11_VARIABLE_IEs	*pVIE = NULL;
+	LARGE_INTEGER				TimeStamp;
+	BOOLEAN Privacy;
+	USHORT Status;
+
+	// Init Variable IE structure
+	pVIE = (PNDIS_802_11_VARIABLE_IEs) VarIE;
+	pVIE->Length = 0;
+	TimeStamp.u.LowPart  = 0;
+	TimeStamp.u.HighPart = 0;
+
+	if (MlmeStartReqSanity(pAd, Elem->Msg, Elem->MsgLen, Ssid, &SsidLen))
+	{
+		// reset all the timers
+		RTMPCancelTimer(&pAd->MlmeAux.ScanTimer, &TimerCancelled);
+		RTMPCancelTimer(&pAd->MlmeAux.BeaconTimer, &TimerCancelled);
+
+		//
+		// Start a new IBSS. All IBSS parameters are decided now....
+		//
+		DBGPRINT(RT_DEBUG_TRACE, ("MlmeStartReqAction - Start a new IBSS. All IBSS parameters are decided now.... \n"));
+		pAd->MlmeAux.BssType           = BSS_ADHOC;
+		NdisMoveMemory(pAd->MlmeAux.Ssid, Ssid, SsidLen);
+		pAd->MlmeAux.SsidLen           = SsidLen;
+
+		// generate a radom number as BSSID
+		MacAddrRandomBssid(pAd, pAd->MlmeAux.Bssid);
+		DBGPRINT(RT_DEBUG_TRACE, ("MlmeStartReqAction - generate a radom number as BSSID \n"));
+
+		Privacy = (pAd->StaCfg.WepStatus == Ndis802_11Encryption1Enabled) ||
+				  (pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) ||
+				  (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled);
+		pAd->MlmeAux.CapabilityInfo    = CAP_GENERATE(0,1,Privacy, (pAd->CommonCfg.TxPreamble == Rt802_11PreambleShort), 1, 0);
+		pAd->MlmeAux.BeaconPeriod      = pAd->CommonCfg.BeaconPeriod;
+		pAd->MlmeAux.AtimWin           = pAd->StaCfg.AtimWin;
+		pAd->MlmeAux.Channel           = pAd->CommonCfg.Channel;
+
+		pAd->CommonCfg.CentralChannel  = pAd->CommonCfg.Channel;
+		pAd->MlmeAux.CentralChannel    = pAd->CommonCfg.CentralChannel;
+
+		pAd->MlmeAux.SupRateLen= pAd->CommonCfg.SupRateLen;
+		NdisMoveMemory(pAd->MlmeAux.SupRate, pAd->CommonCfg.SupRate, MAX_LEN_OF_SUPPORTED_RATES);
+		RTMPCheckRates(pAd, pAd->MlmeAux.SupRate, &pAd->MlmeAux.SupRateLen);
+		pAd->MlmeAux.ExtRateLen = pAd->CommonCfg.ExtRateLen;
+		NdisMoveMemory(pAd->MlmeAux.ExtRate, pAd->CommonCfg.ExtRate, MAX_LEN_OF_SUPPORTED_RATES);
+		RTMPCheckRates(pAd, pAd->MlmeAux.ExtRate, &pAd->MlmeAux.ExtRateLen);
+#ifdef DOT11_N_SUPPORT
+		if (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED)
+		{
+			RTMPUpdateHTIE(&pAd->CommonCfg.DesiredHtPhy, &pAd->StaCfg.DesiredHtPhyInfo.MCSSet[0], &pAd->MlmeAux.HtCapability, &pAd->MlmeAux.AddHtInfo);
+			pAd->MlmeAux.HtCapabilityLen = sizeof(HT_CAPABILITY_IE);
+			// Not turn pAd->StaActive.SupportedHtPhy.bHtEnable = TRUE here.
+			DBGPRINT(RT_DEBUG_TRACE, ("SYNC -pAd->StaActive.SupportedHtPhy.bHtEnable = TRUE\n"));
+		}
+		else
+#endif // DOT11_N_SUPPORT //
+		{
+			pAd->MlmeAux.HtCapabilityLen = 0;
+			pAd->StaActive.SupportedPhyInfo.bHtEnable = FALSE;
+		}
+		// temporarily not support QOS in IBSS
+		NdisZeroMemory(&pAd->MlmeAux.APEdcaParm, sizeof(EDCA_PARM));
+		NdisZeroMemory(&pAd->MlmeAux.APQbssLoad, sizeof(QBSS_LOAD_PARM));
+		NdisZeroMemory(&pAd->MlmeAux.APQosCapability, sizeof(QOS_CAPABILITY_PARM));
+
+		AsicSwitchChannel(pAd, pAd->MlmeAux.Channel, FALSE);
+		AsicLockChannel(pAd, pAd->MlmeAux.Channel);
+
+		DBGPRINT(RT_DEBUG_TRACE, ("SYNC - MlmeStartReqAction(ch= %d,sup rates= %d, ext rates=%d)\n",
+			pAd->MlmeAux.Channel, pAd->MlmeAux.SupRateLen, pAd->MlmeAux.ExtRateLen));
+
+		pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+		Status = MLME_SUCCESS;
+		MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_START_CONF, 2, &Status);
+	}
+	else
+	{
+		DBGPRINT_ERR(("SYNC - MlmeStartReqAction() sanity check fail.\n"));
+		pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+		Status = MLME_INVALID_FORMAT;
+		MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_START_CONF, 2, &Status);
+	}
+}
+
+/*
+	==========================================================================
+	Description:
+		peer sends beacon back when scanning
+	==========================================================================
+ */
+VOID PeerBeaconAtScanAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR           Bssid[MAC_ADDR_LEN], Addr2[MAC_ADDR_LEN];
+	UCHAR           Ssid[MAX_LEN_OF_SSID], BssType, Channel, NewChannel,
+					SsidLen, DtimCount, DtimPeriod, BcastFlag, MessageToMe;
+	CF_PARM         CfParm;
+	USHORT          BeaconPeriod, AtimWin, CapabilityInfo;
+	PFRAME_802_11   pFrame;
+	LARGE_INTEGER   TimeStamp;
+	UCHAR           Erp;
+	UCHAR         	SupRate[MAX_LEN_OF_SUPPORTED_RATES], ExtRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR		  	SupRateLen, ExtRateLen;
+	USHORT 			LenVIE;
+	UCHAR			CkipFlag;
+	UCHAR			AironetCellPowerLimit;
+	EDCA_PARM       EdcaParm;
+	QBSS_LOAD_PARM  QbssLoad;
+	QOS_CAPABILITY_PARM QosCapability;
+	ULONG						RalinkIe;
+	UCHAR						VarIE[MAX_VIE_LEN];		// Total VIE length = MAX_VIE_LEN - -5
+	NDIS_802_11_VARIABLE_IEs	*pVIE = NULL;
+	HT_CAPABILITY_IE		HtCapability;
+	ADD_HT_INFO_IE		AddHtInfo;	// AP might use this additional ht info IE
+	UCHAR			HtCapabilityLen = 0, PreNHtCapabilityLen = 0;
+	UCHAR			AddHtInfoLen;
+	UCHAR			NewExtChannelOffset = 0xff;
+
+
+	// NdisFillMemory(Ssid, MAX_LEN_OF_SSID, 0x00);
+	pFrame = (PFRAME_802_11) Elem->Msg;
+	// Init Variable IE structure
+	pVIE = (PNDIS_802_11_VARIABLE_IEs) VarIE;
+	pVIE->Length = 0;
+#ifdef DOT11_N_SUPPORT
+    RTMPZeroMemory(&HtCapability, sizeof(HtCapability));
+	RTMPZeroMemory(&AddHtInfo, sizeof(ADD_HT_INFO_IE));
+#endif // DOT11_N_SUPPORT //
+
+	if (PeerBeaconAndProbeRspSanity(pAd,
+								Elem->Msg,
+								Elem->MsgLen,
+								Elem->Channel,
+								Addr2,
+								Bssid,
+								Ssid,
+								&SsidLen,
+								&BssType,
+								&BeaconPeriod,
+								&Channel,
+								&NewChannel,
+								&TimeStamp,
+								&CfParm,
+								&AtimWin,
+								&CapabilityInfo,
+								&Erp,
+								&DtimCount,
+								&DtimPeriod,
+								&BcastFlag,
+								&MessageToMe,
+								SupRate,
+								&SupRateLen,
+								ExtRate,
+								&ExtRateLen,
+								&CkipFlag,
+								&AironetCellPowerLimit,
+								&EdcaParm,
+								&QbssLoad,
+								&QosCapability,
+								&RalinkIe,
+								&HtCapabilityLen,
+								&PreNHtCapabilityLen,
+								&HtCapability,
+								&AddHtInfoLen,
+								&AddHtInfo,
+								&NewExtChannelOffset,
+								&LenVIE,
+								pVIE))
+	{
+		ULONG Idx;
+		CHAR Rssi = 0;
+
+		Idx = BssTableSearch(&pAd->ScanTab, Bssid, Channel);
+		if (Idx != BSS_NOT_FOUND)
+			Rssi = pAd->ScanTab.BssEntry[Idx].Rssi;
+
+		Rssi = RTMPMaxRssi(pAd, ConvertToRssi(pAd, Elem->Rssi0, RSSI_0), ConvertToRssi(pAd, Elem->Rssi1, RSSI_1), ConvertToRssi(pAd, Elem->Rssi2, RSSI_2));
+
+
+#ifdef DOT11_N_SUPPORT
+		if ((HtCapabilityLen > 0) || (PreNHtCapabilityLen > 0))
+			HtCapabilityLen = SIZE_HT_CAP_IE;
+#endif // DOT11_N_SUPPORT //
+		if ((pAd->StaCfg.CCXReqType != MSRN_TYPE_UNUSED) && (Channel == pAd->StaCfg.CCXScanChannel))
+		{
+			Idx = BssTableSetEntry(pAd, &pAd->StaCfg.CCXBssTab, Bssid, Ssid, SsidLen, BssType, BeaconPeriod,
+						 &CfParm, AtimWin, CapabilityInfo, SupRate, SupRateLen,ExtRate, ExtRateLen, &HtCapability,
+						 &AddHtInfo, HtCapabilityLen, AddHtInfoLen, NewExtChannelOffset, Channel, Rssi, TimeStamp, CkipFlag,
+						 &EdcaParm, &QosCapability, &QbssLoad, LenVIE, pVIE);
+			if (Idx != BSS_NOT_FOUND)
+			{
+				NdisMoveMemory(pAd->StaCfg.CCXBssTab.BssEntry[Idx].PTSF, &Elem->Msg[24], 4);
+				NdisMoveMemory(&pAd->StaCfg.CCXBssTab.BssEntry[Idx].TTSF[0], &Elem->TimeStamp.u.LowPart, 4);
+				NdisMoveMemory(&pAd->StaCfg.CCXBssTab.BssEntry[Idx].TTSF[4], &Elem->TimeStamp.u.LowPart, 4);
+				if (pAd->StaCfg.CCXReqType == MSRN_TYPE_BEACON_REQ)
+					AironetAddBeaconReport(pAd, Idx, Elem);
+			}
+		}
+		else
+		{
+			Idx = BssTableSetEntry(pAd, &pAd->ScanTab, Bssid, Ssid, SsidLen, BssType, BeaconPeriod,
+						  &CfParm, AtimWin, CapabilityInfo, SupRate, SupRateLen, ExtRate, ExtRateLen,  &HtCapability,
+						 &AddHtInfo, HtCapabilityLen, AddHtInfoLen, NewExtChannelOffset, Channel, Rssi, TimeStamp, CkipFlag,
+						 &EdcaParm, &QosCapability, &QbssLoad, LenVIE, pVIE);
+#ifdef DOT11_N_SUPPORT
+#ifdef DOT11N_DRAFT3
+			if (pAd->ChannelList[pAd->CommonCfg.ChannelListIdx].bEffectedChannel == TRUE)
+			{
+				UCHAR		RegClass;
+				PeerBeaconAndProbeRspSanity2(pAd, Elem->Msg, Elem->MsgLen, &RegClass);
+				TriEventTableSetEntry(pAd, &pAd->CommonCfg.TriggerEventTab, Bssid, &HtCapability, HtCapabilityLen, RegClass, Channel);
+			}
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+			if (Idx != BSS_NOT_FOUND)
+			{
+				NdisMoveMemory(pAd->ScanTab.BssEntry[Idx].PTSF, &Elem->Msg[24], 4);
+				NdisMoveMemory(&pAd->ScanTab.BssEntry[Idx].TTSF[0], &Elem->TimeStamp.u.LowPart, 4);
+				NdisMoveMemory(&pAd->ScanTab.BssEntry[Idx].TTSF[4], &Elem->TimeStamp.u.LowPart, 4);
+			}
+		}
+	}
+	// sanity check fail, ignored
+}
+
+/*
+	==========================================================================
+	Description:
+		When waiting joining the (I)BSS, beacon received from external
+	==========================================================================
+ */
+VOID PeerBeaconAtJoinAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR         Bssid[MAC_ADDR_LEN], Addr2[MAC_ADDR_LEN];
+	UCHAR         Ssid[MAX_LEN_OF_SSID], SsidLen, BssType, Channel, MessageToMe,
+				  DtimCount, DtimPeriod, BcastFlag, NewChannel;
+	LARGE_INTEGER TimeStamp;
+	USHORT        BeaconPeriod, AtimWin, CapabilityInfo;
+	CF_PARM       Cf;
+	BOOLEAN       TimerCancelled;
+	UCHAR         Erp;
+	UCHAR         SupRate[MAX_LEN_OF_SUPPORTED_RATES], ExtRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR		  SupRateLen, ExtRateLen;
+	UCHAR         CkipFlag;
+	USHORT 		  LenVIE;
+	UCHAR		  AironetCellPowerLimit;
+	EDCA_PARM       EdcaParm;
+	QBSS_LOAD_PARM  QbssLoad;
+	QOS_CAPABILITY_PARM QosCapability;
+	USHORT        Status;
+	UCHAR						VarIE[MAX_VIE_LEN];		// Total VIE length = MAX_VIE_LEN - -5
+	NDIS_802_11_VARIABLE_IEs	*pVIE = NULL;
+	ULONG           RalinkIe;
+	ULONG         Idx;
+	HT_CAPABILITY_IE		HtCapability;
+	ADD_HT_INFO_IE		AddHtInfo;	// AP might use this additional ht info IE
+	UCHAR				HtCapabilityLen = 0, PreNHtCapabilityLen = 0;
+	UCHAR			AddHtInfoLen;
+	UCHAR			NewExtChannelOffset = 0xff;
+#ifdef DOT11_N_SUPPORT
+	UCHAR			CentralChannel;
+#endif // DOT11_N_SUPPORT //
+
+	// Init Variable IE structure
+	pVIE = (PNDIS_802_11_VARIABLE_IEs) VarIE;
+	pVIE->Length = 0;
+    RTMPZeroMemory(&HtCapability, sizeof(HtCapability));
+	RTMPZeroMemory(&AddHtInfo, sizeof(ADD_HT_INFO_IE));
+
+
+	if (PeerBeaconAndProbeRspSanity(pAd,
+								Elem->Msg,
+								Elem->MsgLen,
+								Elem->Channel,
+								Addr2,
+								Bssid,
+								Ssid,
+								&SsidLen,
+								&BssType,
+								&BeaconPeriod,
+								&Channel,
+								&NewChannel,
+								&TimeStamp,
+								&Cf,
+								&AtimWin,
+								&CapabilityInfo,
+								&Erp,
+								&DtimCount,
+								&DtimPeriod,
+								&BcastFlag,
+								&MessageToMe,
+								SupRate,
+								&SupRateLen,
+								ExtRate,
+								&ExtRateLen,
+								&CkipFlag,
+								&AironetCellPowerLimit,
+								&EdcaParm,
+								&QbssLoad,
+								&QosCapability,
+								&RalinkIe,
+								&HtCapabilityLen,
+								&PreNHtCapabilityLen,
+								&HtCapability,
+								&AddHtInfoLen,
+								&AddHtInfo,
+								&NewExtChannelOffset,
+								&LenVIE,
+								pVIE))
+	{
+		// Disqualify 11b only adhoc when we are in 11g only adhoc mode
+		if ((BssType == BSS_ADHOC) && (pAd->CommonCfg.PhyMode == PHY_11G) && ((SupRateLen+ExtRateLen)< 12))
+			return;
+
+		// BEACON from desired BSS/IBSS found. We should be able to decide most
+		// BSS parameters here.
+		// Q. But what happen if this JOIN doesn't conclude a successful ASSOCIATEION?
+		//    Do we need to receover back all parameters belonging to previous BSS?
+		// A. Should be not. There's no back-door recover to previous AP. It still need
+		//    a new JOIN-AUTH-ASSOC sequence.
+		if (MAC_ADDR_EQUAL(pAd->MlmeAux.Bssid, Bssid))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("SYNC - receive desired BEACON at JoinWaitBeacon... Channel = %d\n", Channel));
+			RTMPCancelTimer(&pAd->MlmeAux.BeaconTimer, &TimerCancelled);
+
+			// Update RSSI to prevent No signal display when cards first initialized
+			pAd->StaCfg.RssiSample.LastRssi0	= ConvertToRssi(pAd, Elem->Rssi0, RSSI_0);
+			pAd->StaCfg.RssiSample.LastRssi1	= ConvertToRssi(pAd, Elem->Rssi1, RSSI_1);
+			pAd->StaCfg.RssiSample.LastRssi2	= ConvertToRssi(pAd, Elem->Rssi2, RSSI_2);
+			pAd->StaCfg.RssiSample.AvgRssi0	= pAd->StaCfg.RssiSample.LastRssi0;
+			pAd->StaCfg.RssiSample.AvgRssi0X8	= pAd->StaCfg.RssiSample.AvgRssi0 << 3;
+			pAd->StaCfg.RssiSample.AvgRssi1	= pAd->StaCfg.RssiSample.LastRssi1;
+			pAd->StaCfg.RssiSample.AvgRssi1X8	= pAd->StaCfg.RssiSample.AvgRssi1 << 3;
+			pAd->StaCfg.RssiSample.AvgRssi2	= pAd->StaCfg.RssiSample.LastRssi2;
+			pAd->StaCfg.RssiSample.AvgRssi2X8	= pAd->StaCfg.RssiSample.AvgRssi2 << 3;
+
+			//
+			// We need to check if SSID only set to any, then we can record the current SSID.
+			// Otherwise will cause hidden SSID association failed.
+			//
+			if (pAd->MlmeAux.SsidLen == 0)
+			{
+				NdisMoveMemory(pAd->MlmeAux.Ssid, Ssid, SsidLen);
+				pAd->MlmeAux.SsidLen = SsidLen;
+			}
+			else
+			{
+				Idx = BssSsidTableSearch(&pAd->ScanTab, Bssid, pAd->MlmeAux.Ssid, pAd->MlmeAux.SsidLen, Channel);
+
+				if (Idx != BSS_NOT_FOUND)
+				{
+					//
+					// Multiple SSID case, used correct CapabilityInfo
+					//
+					CapabilityInfo = pAd->ScanTab.BssEntry[Idx].CapabilityInfo;
+				}
+			}
+			NdisMoveMemory(pAd->MlmeAux.Bssid, Bssid, MAC_ADDR_LEN);
+			pAd->MlmeAux.CapabilityInfo = CapabilityInfo & SUPPORTED_CAPABILITY_INFO;
+			pAd->MlmeAux.BssType = BssType;
+			pAd->MlmeAux.BeaconPeriod = BeaconPeriod;
+			pAd->MlmeAux.Channel = Channel;
+			pAd->MlmeAux.AtimWin = AtimWin;
+			pAd->MlmeAux.CfpPeriod = Cf.CfpPeriod;
+			pAd->MlmeAux.CfpMaxDuration = Cf.CfpMaxDuration;
+			pAd->MlmeAux.APRalinkIe = RalinkIe;
+
+			// Copy AP's supported rate to MlmeAux for creating assoication request
+			// Also filter out not supported rate
+			pAd->MlmeAux.SupRateLen = SupRateLen;
+			NdisMoveMemory(pAd->MlmeAux.SupRate, SupRate, SupRateLen);
+			RTMPCheckRates(pAd, pAd->MlmeAux.SupRate, &pAd->MlmeAux.SupRateLen);
+			pAd->MlmeAux.ExtRateLen = ExtRateLen;
+			NdisMoveMemory(pAd->MlmeAux.ExtRate, ExtRate, ExtRateLen);
+			RTMPCheckRates(pAd, pAd->MlmeAux.ExtRate, &pAd->MlmeAux.ExtRateLen);
+
+            NdisZeroMemory(pAd->StaActive.SupportedPhyInfo.MCSSet, 16);
+#ifdef DOT11_N_SUPPORT
+			pAd->MlmeAux.NewExtChannelOffset = NewExtChannelOffset;
+			pAd->MlmeAux.HtCapabilityLen = HtCapabilityLen;
+
+			// filter out un-supported ht rates
+			if (((HtCapabilityLen > 0) || (PreNHtCapabilityLen > 0)) && (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED))
+			{
+				RTMPZeroMemory(&pAd->MlmeAux.HtCapability, SIZE_HT_CAP_IE);
+   				RTMPMoveMemory(&pAd->MlmeAux.AddHtInfo, &AddHtInfo, SIZE_ADD_HT_INFO_IE);
+
+				// StaActive.SupportedHtPhy.MCSSet stores Peer AP's 11n Rx capability
+				NdisMoveMemory(pAd->StaActive.SupportedPhyInfo.MCSSet, HtCapability.MCSSet, 16);
+				pAd->MlmeAux.NewExtChannelOffset = NewExtChannelOffset;
+				pAd->MlmeAux.HtCapabilityLen = SIZE_HT_CAP_IE;
+				pAd->StaActive.SupportedPhyInfo.bHtEnable = TRUE;
+				if (PreNHtCapabilityLen > 0)
+					pAd->StaActive.SupportedPhyInfo.bPreNHt = TRUE;
+				RTMPCheckHt(pAd, BSSID_WCID, &HtCapability, &AddHtInfo);
+				// Copy AP Parameter to StaActive.  This is also in LinkUp.
+				DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAtJoinAction! (MpduDensity=%d, MaxRAmpduFactor=%d, BW=%d)\n",
+					pAd->StaActive.SupportedHtPhy.MpduDensity, pAd->StaActive.SupportedHtPhy.MaxRAmpduFactor, HtCapability.HtCapInfo.ChannelWidth));
+
+				if (AddHtInfoLen > 0)
+				{
+					CentralChannel = AddHtInfo.ControlChan;
+		 			// Check again the Bandwidth capability of this AP.
+		 			if ((AddHtInfo.ControlChan > 2)&& (AddHtInfo.AddHtInfo.ExtChanOffset == EXTCHA_BELOW) && (HtCapability.HtCapInfo.ChannelWidth == BW_40))
+		 			{
+		 				CentralChannel = AddHtInfo.ControlChan - 2;
+		 			}
+		 			else if ((AddHtInfo.AddHtInfo.ExtChanOffset == EXTCHA_ABOVE) && (HtCapability.HtCapInfo.ChannelWidth == BW_40))
+		 			{
+		 				CentralChannel = AddHtInfo.ControlChan + 2;
+		 			}
+
+					// Check Error .
+					if (pAd->MlmeAux.CentralChannel != CentralChannel)
+		 				DBGPRINT(RT_DEBUG_ERROR, ("PeerBeaconAtJoinAction HT===>Beacon Central Channel = %d, Control Channel = %d. Mlmeaux CentralChannel = %d\n", CentralChannel, AddHtInfo.ControlChan, pAd->MlmeAux.CentralChannel));
+
+		 			DBGPRINT(RT_DEBUG_TRACE, ("PeerBeaconAtJoinAction HT===>Central Channel = %d, Control Channel = %d,  .\n", CentralChannel, AddHtInfo.ControlChan));
+
+				}
+
+			}
+			else
+#endif // DOT11_N_SUPPORT //
+			{
+   				// To prevent error, let legacy AP must have same CentralChannel and Channel.
+				if ((HtCapabilityLen == 0) && (PreNHtCapabilityLen == 0))
+					pAd->MlmeAux.CentralChannel = pAd->MlmeAux.Channel;
+
+				pAd->StaActive.SupportedPhyInfo.bHtEnable = FALSE;
+				RTMPZeroMemory(&pAd->MlmeAux.HtCapability, SIZE_HT_CAP_IE);
+				RTMPZeroMemory(&pAd->MlmeAux.AddHtInfo, SIZE_ADD_HT_INFO_IE);
+			}
+
+			RTMPUpdateMlmeRate(pAd);
+
+			// copy QOS related information
+			if ((pAd->CommonCfg.bWmmCapable)
+#ifdef DOT11_N_SUPPORT
+				 || (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED)
+#endif // DOT11_N_SUPPORT //
+				)
+			{
+				NdisMoveMemory(&pAd->MlmeAux.APEdcaParm, &EdcaParm, sizeof(EDCA_PARM));
+				NdisMoveMemory(&pAd->MlmeAux.APQbssLoad, &QbssLoad, sizeof(QBSS_LOAD_PARM));
+				NdisMoveMemory(&pAd->MlmeAux.APQosCapability, &QosCapability, sizeof(QOS_CAPABILITY_PARM));
+			}
+			else
+			{
+				NdisZeroMemory(&pAd->MlmeAux.APEdcaParm, sizeof(EDCA_PARM));
+				NdisZeroMemory(&pAd->MlmeAux.APQbssLoad, sizeof(QBSS_LOAD_PARM));
+				NdisZeroMemory(&pAd->MlmeAux.APQosCapability, sizeof(QOS_CAPABILITY_PARM));
+			}
+
+			DBGPRINT(RT_DEBUG_TRACE, ("SYNC - after JOIN, SupRateLen=%d, ExtRateLen=%d\n",
+										pAd->MlmeAux.SupRateLen, pAd->MlmeAux.ExtRateLen));
+
+#ifdef LEAP_SUPPORT
+			// Update CkipFlag
+			pAd->StaCfg.CkipFlag = CkipFlag;
+
+			// Keep TimeStamp for Re-Association used.
+			if (LEAP_CCKM_ON(pAd) && (pAd->StaCfg.CCKMLinkUpFlag == TRUE))
+				pAd->StaCfg.CCKMBeaconAtJoinTimeStamp = TimeStamp;
+#endif // LEAP_SUPPORT //
+
+			if (AironetCellPowerLimit != 0xFF)
+			{
+				//We need to change our TxPower for CCX 2.0 AP Control of Client Transmit Power
+				ChangeToCellPowerLimit(pAd, AironetCellPowerLimit);
+			}
+			else  //Used the default TX Power Percentage.
+				pAd->CommonCfg.TxPowerPercentage = pAd->CommonCfg.TxPowerDefault;
+
+			pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+			Status = MLME_SUCCESS;
+			MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_JOIN_CONF, 2, &Status);
+		}
+		// not to me BEACON, ignored
+	}
+	// sanity check fail, ignore this frame
+}
+
+/*
+	==========================================================================
+	Description:
+		receive BEACON from peer
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID PeerBeacon(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR         Bssid[MAC_ADDR_LEN], Addr2[MAC_ADDR_LEN];
+	CHAR          Ssid[MAX_LEN_OF_SSID];
+	CF_PARM       CfParm;
+	UCHAR         SsidLen, MessageToMe=0, BssType, Channel, NewChannel, index=0;
+	UCHAR         DtimCount=0, DtimPeriod=0, BcastFlag=0;
+	USHORT        CapabilityInfo, AtimWin, BeaconPeriod;
+	LARGE_INTEGER TimeStamp;
+	USHORT        TbttNumToNextWakeUp;
+	UCHAR         Erp;
+	UCHAR         SupRate[MAX_LEN_OF_SUPPORTED_RATES], ExtRate[MAX_LEN_OF_SUPPORTED_RATES];
+	UCHAR		  SupRateLen, ExtRateLen;
+	UCHAR		  CkipFlag;
+	USHORT        LenVIE;
+	UCHAR		  AironetCellPowerLimit;
+	EDCA_PARM       EdcaParm;
+	QBSS_LOAD_PARM  QbssLoad;
+	QOS_CAPABILITY_PARM QosCapability;
+	ULONG           RalinkIe;
+	// New for WPA security suites
+	UCHAR						VarIE[MAX_VIE_LEN];		// Total VIE length = MAX_VIE_LEN - -5
+	NDIS_802_11_VARIABLE_IEs	*pVIE = NULL;
+	HT_CAPABILITY_IE		HtCapability;
+	ADD_HT_INFO_IE		AddHtInfo;	// AP might use this additional ht info IE
+	UCHAR			HtCapabilityLen, PreNHtCapabilityLen;
+	UCHAR			AddHtInfoLen;
+	UCHAR			NewExtChannelOffset = 0xff;
+
+
+#ifdef RALINK_ATE
+    if (ATE_ON(pAd))
+    {
+		return;
+    }
+#endif // RALINK_ATE //
+
+	if (!(INFRA_ON(pAd) || ADHOC_ON(pAd)
+		))
+		return;
+
+	// Init Variable IE structure
+	pVIE = (PNDIS_802_11_VARIABLE_IEs) VarIE;
+	pVIE->Length = 0;
+    RTMPZeroMemory(&HtCapability, sizeof(HtCapability));
+	RTMPZeroMemory(&AddHtInfo, sizeof(ADD_HT_INFO_IE));
+
+	if (PeerBeaconAndProbeRspSanity(pAd,
+								Elem->Msg,
+								Elem->MsgLen,
+								Elem->Channel,
+								Addr2,
+								Bssid,
+								Ssid,
+								&SsidLen,
+								&BssType,
+								&BeaconPeriod,
+								&Channel,
+								&NewChannel,
+								&TimeStamp,
+								&CfParm,
+								&AtimWin,
+								&CapabilityInfo,
+								&Erp,
+								&DtimCount,
+								&DtimPeriod,
+								&BcastFlag,
+								&MessageToMe,
+								SupRate,
+								&SupRateLen,
+								ExtRate,
+								&ExtRateLen,
+								&CkipFlag,
+								&AironetCellPowerLimit,
+								&EdcaParm,
+								&QbssLoad,
+								&QosCapability,
+								&RalinkIe,
+								&HtCapabilityLen,
+								&PreNHtCapabilityLen,
+								&HtCapability,
+								&AddHtInfoLen,
+								&AddHtInfo,
+								&NewExtChannelOffset,
+								&LenVIE,
+								pVIE))
+	{
+		BOOLEAN is_my_bssid, is_my_ssid;
+		ULONG   Bssidx, Now;
+		BSS_ENTRY *pBss;
+		CHAR		RealRssi = RTMPMaxRssi(pAd, ConvertToRssi(pAd, Elem->Rssi0, RSSI_0), ConvertToRssi(pAd, Elem->Rssi1, RSSI_1), ConvertToRssi(pAd, Elem->Rssi2, RSSI_2));
+
+		is_my_bssid = MAC_ADDR_EQUAL(Bssid, pAd->CommonCfg.Bssid)? TRUE : FALSE;
+		is_my_ssid = SSID_EQUAL(Ssid, SsidLen, pAd->CommonCfg.Ssid, pAd->CommonCfg.SsidLen)? TRUE:FALSE;
+
+
+		// ignore BEACON not for my SSID
+		if ((! is_my_ssid) && (! is_my_bssid))
+			return;
+
+		// It means STA waits disassoc completely from this AP, ignores this beacon.
+		if (pAd->Mlme.CntlMachine.CurrState == CNTL_WAIT_DISASSOC)
+			return;
+
+#ifdef DOT11_N_SUPPORT
+		// Copy Control channel for this BSSID.
+		if (AddHtInfoLen != 0)
+			Channel = AddHtInfo.ControlChan;
+
+		if ((HtCapabilityLen > 0) || (PreNHtCapabilityLen > 0))
+			HtCapabilityLen = SIZE_HT_CAP_IE;
+#endif // DOT11_N_SUPPORT //
+
+		//
+		// Housekeeping "SsidBssTab" table for later-on ROAMing usage.
+		//
+		Bssidx = BssTableSearch(&pAd->ScanTab, Bssid, Channel);
+		if (Bssidx == BSS_NOT_FOUND)
+		{
+			// discover new AP of this network, create BSS entry
+			Bssidx = BssTableSetEntry(pAd, &pAd->ScanTab, Bssid, Ssid, SsidLen, BssType, BeaconPeriod,
+						 &CfParm, AtimWin, CapabilityInfo, SupRate, SupRateLen, ExtRate, ExtRateLen,
+						&HtCapability, &AddHtInfo,HtCapabilityLen,AddHtInfoLen,NewExtChannelOffset, Channel,
+						RealRssi, TimeStamp, CkipFlag, &EdcaParm, &QosCapability,
+						&QbssLoad, LenVIE, pVIE);
+			if (Bssidx == BSS_NOT_FOUND) // return if BSS table full
+				return;
+
+			NdisMoveMemory(pAd->ScanTab.BssEntry[Bssidx].PTSF, &Elem->Msg[24], 4);
+			NdisMoveMemory(&pAd->ScanTab.BssEntry[Bssidx].TTSF[0], &Elem->TimeStamp.u.LowPart, 4);
+			NdisMoveMemory(&pAd->ScanTab.BssEntry[Bssidx].TTSF[4], &Elem->TimeStamp.u.LowPart, 4);
+
+
+
+		}
+
+		if ((pAd->CommonCfg.bIEEE80211H == 1) && (NewChannel != 0) && (Channel != NewChannel))
+		{
+			// Switching to channel 1 can prevent from rescanning the current channel immediately (by auto reconnection).
+			// In addition, clear the MLME queue and the scan table to discard the RX packets and previous scanning results.
+			AsicSwitchChannel(pAd, 1, FALSE);
+			AsicLockChannel(pAd, 1);
+		    LinkDown(pAd, FALSE);
+			MlmeQueueInit(&pAd->Mlme.Queue);
+			BssTableInit(&pAd->ScanTab);
+		    RTMPusecDelay(1000000);		// use delay to prevent STA do reassoc
+
+			// channel sanity check
+			for (index = 0 ; index < pAd->ChannelListNum; index++)
+			{
+				if (pAd->ChannelList[index].Channel == NewChannel)
+				{
+					pAd->ScanTab.BssEntry[Bssidx].Channel = NewChannel;
+					pAd->CommonCfg.Channel = NewChannel;
+					AsicSwitchChannel(pAd, pAd->CommonCfg.Channel, FALSE);
+					AsicLockChannel(pAd, pAd->CommonCfg.Channel);
+					DBGPRINT(RT_DEBUG_TRACE, ("PeerBeacon - STA receive channel switch announcement IE (New Channel =%d)\n", NewChannel));
+					break;
+				}
+			}
+
+			if (index >= pAd->ChannelListNum)
+			{
+				DBGPRINT_ERR(("PeerBeacon(can not find New Channel=%d in ChannelList[%d]\n", pAd->CommonCfg.Channel, pAd->ChannelListNum));
+			}
+		}
+
+		// if the ssid matched & bssid unmatched, we should select the bssid with large value.
+		// This might happened when two STA start at the same time
+		if ((! is_my_bssid) && ADHOC_ON(pAd))
+		{
+			INT	i;
+
+			// Add the safeguard against the mismatch of adhoc wep status
+			if (pAd->StaCfg.WepStatus != pAd->ScanTab.BssEntry[Bssidx].WepStatus)
+			{
+				return;
+			}
+
+			// collapse into the ADHOC network which has bigger BSSID value.
+			for (i = 0; i < 6; i++)
+			{
+				if (Bssid[i] > pAd->CommonCfg.Bssid[i])
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("SYNC - merge to the IBSS with bigger BSSID=%02x:%02x:%02x:%02x:%02x:%02x\n",
+						Bssid[0], Bssid[1], Bssid[2], Bssid[3], Bssid[4], Bssid[5]));
+					AsicDisableSync(pAd);
+					COPY_MAC_ADDR(pAd->CommonCfg.Bssid, Bssid);
+					AsicSetBssid(pAd, pAd->CommonCfg.Bssid);
+					MakeIbssBeacon(pAd);        // re-build BEACON frame
+					AsicEnableIbssSync(pAd);    // copy BEACON frame to on-chip memory
+					is_my_bssid = TRUE;
+					break;
+				}
+				else if (Bssid[i] < pAd->CommonCfg.Bssid[i])
+					break;
+			}
+		}
+
+
+		NdisGetSystemUpTime(&Now);
+		pBss = &pAd->ScanTab.BssEntry[Bssidx];
+		pBss->Rssi = RealRssi;       // lastest RSSI
+		pBss->LastBeaconRxTime = Now;   // last RX timestamp
+
+		//
+		// BEACON from my BSSID - either IBSS or INFRA network
+		//
+		if (is_my_bssid)
+		{
+			RXWI_STRUC	RxWI;
+
+			pAd->StaCfg.DtimCount = DtimCount;
+			pAd->StaCfg.DtimPeriod = DtimPeriod;
+			pAd->StaCfg.LastBeaconRxTime = Now;
+
+
+			RxWI.RSSI0 = Elem->Rssi0;
+			RxWI.RSSI1 = Elem->Rssi1;
+			RxWI.RSSI2 = Elem->Rssi2;
+
+			Update_Rssi_Sample(pAd, &pAd->StaCfg.RssiSample, &RxWI);
+			if (AironetCellPowerLimit != 0xFF)
+			{
+				//
+				// We get the Cisco (ccx) "TxPower Limit" required
+				// Changed to appropriate TxPower Limit for Ciso Compatible Extensions
+				//
+				ChangeToCellPowerLimit(pAd, AironetCellPowerLimit);
+			}
+			else
+			{
+				//
+				// AironetCellPowerLimit equal to 0xFF means the Cisco (ccx) "TxPower Limit" not exist.
+				// Used the default TX Power Percentage, that set from UI.
+				//
+				pAd->CommonCfg.TxPowerPercentage = pAd->CommonCfg.TxPowerDefault;
+			}
+
+			if (ADHOC_ON(pAd) && (CAP_IS_IBSS_ON(CapabilityInfo)))
+			{
+				UCHAR			MaxSupportedRateIn500Kbps = 0;
+				UCHAR			idx;
+				MAC_TABLE_ENTRY *pEntry;
+
+				// supported rates array may not be sorted. sort it and find the maximum rate
+			    for (idx=0; idx<SupRateLen; idx++)
+			    {
+			        if (MaxSupportedRateIn500Kbps < (SupRate[idx] & 0x7f))
+			            MaxSupportedRateIn500Kbps = SupRate[idx] & 0x7f;
+			    }
+
+				for (idx=0; idx<ExtRateLen; idx++)
+			    {
+			        if (MaxSupportedRateIn500Kbps < (ExtRate[idx] & 0x7f))
+			            MaxSupportedRateIn500Kbps = ExtRate[idx] & 0x7f;
+			    }
+
+				// look up the existing table
+				pEntry = MacTableLookup(pAd, Addr2);
+
+				// Ad-hoc mode is using MAC address as BA session. So we need to continuously find newly joined adhoc station by receiving beacon.
+				// To prevent always check this, we use wcid == RESERVED_WCID to recognize it as newly joined adhoc station.
+				if ((ADHOC_ON(pAd) && (Elem->Wcid == RESERVED_WCID)) ||
+					(pEntry && ((pEntry->LastBeaconRxTime + ADHOC_ENTRY_BEACON_LOST_TIME) < Now)))
+				{
+					if (pEntry == NULL)
+						// Another adhoc joining, add to our MAC table.
+						pEntry = MacTableInsertEntry(pAd, Addr2, BSS0, FALSE);
+
+					if (StaAddMacTableEntry(pAd, pEntry, MaxSupportedRateIn500Kbps, &HtCapability, HtCapabilityLen, CapabilityInfo) == FALSE)
+					{
+						DBGPRINT(RT_DEBUG_TRACE, ("ADHOC - Add Entry failed.\n"));
+						return;
+					}
+
+					if (pEntry &&
+						(Elem->Wcid == RESERVED_WCID))
+					{
+						idx = pAd->StaCfg.DefaultKeyId;
+						RT28XX_STA_SECURITY_INFO_ADD(pAd, BSS0, idx, pEntry);
+					}
+				}
+
+				if (pEntry && pEntry->ValidAsCLI)
+					pEntry->LastBeaconRxTime = Now;
+
+				// At least another peer in this IBSS, declare MediaState as CONNECTED
+				if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED))
+				{
+					OPSTATUS_SET_FLAG(pAd, fOP_STATUS_MEDIA_STATE_CONNECTED);
+
+					pAd->IndicateMediaState = NdisMediaStateConnected;
+					RTMP_IndicateMediaState(pAd);
+	                pAd->ExtraInfo = GENERAL_LINK_UP;
+					AsicSetBssid(pAd, pAd->CommonCfg.Bssid);
+
+					// 2003/03/12 - john
+					// Make sure this entry in "ScanTab" table, thus complies to Microsoft's policy that
+					// "site survey" result should always include the current connected network.
+					//
+					Bssidx = BssTableSearch(&pAd->ScanTab, Bssid, Channel);
+					if (Bssidx == BSS_NOT_FOUND)
+					{
+						Bssidx = BssTableSetEntry(pAd, &pAd->ScanTab, Bssid, Ssid, SsidLen, BssType, BeaconPeriod,
+									&CfParm, AtimWin, CapabilityInfo, SupRate, SupRateLen, ExtRate, ExtRateLen, &HtCapability,
+									&AddHtInfo, HtCapabilityLen, AddHtInfoLen, NewExtChannelOffset, Channel, RealRssi, TimeStamp, 0,
+									&EdcaParm, &QosCapability, &QbssLoad, LenVIE, pVIE);
+					}
+					DBGPRINT(RT_DEBUG_TRACE, ("ADHOC  fOP_STATUS_MEDIA_STATE_CONNECTED.\n"));
+				}
+			}
+
+			if (INFRA_ON(pAd))
+			{
+				BOOLEAN bUseShortSlot, bUseBGProtection;
+
+				// decide to use/change to -
+				//      1. long slot (20 us) or short slot (9 us) time
+				//      2. turn on/off RTS/CTS and/or CTS-to-self protection
+				//      3. short preamble
+
+				//bUseShortSlot = pAd->CommonCfg.bUseShortSlotTime && CAP_IS_SHORT_SLOT(CapabilityInfo);
+				bUseShortSlot = CAP_IS_SHORT_SLOT(CapabilityInfo);
+				if (bUseShortSlot != OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_SHORT_SLOT_INUSED))
+					AsicSetSlotTime(pAd, bUseShortSlot);
+
+				bUseBGProtection = (pAd->CommonCfg.UseBGProtection == 1) ||    // always use
+								   ((pAd->CommonCfg.UseBGProtection == 0) && ERP_IS_USE_PROTECTION(Erp));
+
+				if (pAd->CommonCfg.Channel > 14) // always no BG protection in A-band. falsely happened when switching A/G band to a dual-band AP
+					bUseBGProtection = FALSE;
+
+				if (bUseBGProtection != OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_BG_PROTECTION_INUSED))
+				{
+					if (bUseBGProtection)
+					{
+						OPSTATUS_SET_FLAG(pAd, fOP_STATUS_BG_PROTECTION_INUSED);
+						AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode, (OFDMSETPROTECT|CCKSETPROTECT|ALLN_SETPROTECT),FALSE,(pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent == 1));
+					}
+					else
+					{
+						OPSTATUS_CLEAR_FLAG(pAd, fOP_STATUS_BG_PROTECTION_INUSED);
+						AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode, (OFDMSETPROTECT|CCKSETPROTECT|ALLN_SETPROTECT),TRUE,(pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent == 1));
+					}
+
+					DBGPRINT(RT_DEBUG_WARN, ("SYNC - AP changed B/G protection to %d\n", bUseBGProtection));
+				}
+
+#ifdef DOT11_N_SUPPORT
+				// check Ht protection mode. and adhere to the Non-GF device indication by AP.
+				if ((AddHtInfoLen != 0) &&
+					((AddHtInfo.AddHtInfo2.OperaionMode != pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode) ||
+					(AddHtInfo.AddHtInfo2.NonGfPresent != pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent)))
+				{
+					pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent = AddHtInfo.AddHtInfo2.NonGfPresent;
+					pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode = AddHtInfo.AddHtInfo2.OperaionMode;
+					if (pAd->MlmeAux.AddHtInfo.AddHtInfo2.NonGfPresent == 1)
+				{
+						AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode, ALLN_SETPROTECT, FALSE, TRUE);
+					}
+					else
+						AsicUpdateProtect(pAd, pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode, ALLN_SETPROTECT, FALSE, FALSE);
+
+					DBGPRINT(RT_DEBUG_TRACE, ("SYNC - AP changed N OperaionMode to %d\n", pAd->MlmeAux.AddHtInfo.AddHtInfo2.OperaionMode));
+				}
+#endif // DOT11_N_SUPPORT //
+
+				if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_SHORT_PREAMBLE_INUSED) &&
+					ERP_IS_USE_BARKER_PREAMBLE(Erp))
+				{
+					MlmeSetTxPreamble(pAd, Rt802_11PreambleLong);
+					DBGPRINT(RT_DEBUG_TRACE, ("SYNC - AP forced to use LONG preamble\n"));
+				}
+
+				if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_WMM_INUSED)    &&
+					(EdcaParm.bValid == TRUE)                          &&
+					(EdcaParm.EdcaUpdateCount != pAd->CommonCfg.APEdcaParm.EdcaUpdateCount))
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("SYNC - AP change EDCA parameters(from %d to %d)\n",
+						pAd->CommonCfg.APEdcaParm.EdcaUpdateCount,
+						EdcaParm.EdcaUpdateCount));
+					AsicSetEdcaParm(pAd, &EdcaParm);
+				}
+
+				// copy QOS related information
+				NdisMoveMemory(&pAd->CommonCfg.APQbssLoad, &QbssLoad, sizeof(QBSS_LOAD_PARM));
+				NdisMoveMemory(&pAd->CommonCfg.APQosCapability, &QosCapability, sizeof(QOS_CAPABILITY_PARM));
+			}
+
+			// only INFRASTRUCTURE mode support power-saving feature
+			if ((INFRA_ON(pAd) && (pAd->StaCfg.Psm == PWR_SAVE)) || (pAd->CommonCfg.bAPSDForcePowerSave))
+			{
+				UCHAR FreeNumber;
+				//  1. AP has backlogged unicast-to-me frame, stay AWAKE, send PSPOLL
+				//  2. AP has backlogged broadcast/multicast frame and we want those frames, stay AWAKE
+				//  3. we have outgoing frames in TxRing or MgmtRing, better stay AWAKE
+				//  4. Psm change to PWR_SAVE, but AP not been informed yet, we better stay AWAKE
+				//  5. otherwise, put PHY back to sleep to save battery.
+				if (MessageToMe)
+				{
+					if (pAd->CommonCfg.bAPSDCapable && pAd->CommonCfg.APEdcaParm.bAPSDCapable &&
+						pAd->CommonCfg.bAPSDAC_BE && pAd->CommonCfg.bAPSDAC_BK && pAd->CommonCfg.bAPSDAC_VI && pAd->CommonCfg.bAPSDAC_VO)
+					{
+						pAd->CommonCfg.bNeedSendTriggerFrame = TRUE;
+					}
+					else
+						RT28XX_PS_POLL_ENQUEUE(pAd);
+				}
+				else if (BcastFlag && (DtimCount == 0) && OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_RECEIVE_DTIM))
+				{
+				}
+				else if ((pAd->TxSwQueue[QID_AC_BK].Number != 0)													||
+						(pAd->TxSwQueue[QID_AC_BE].Number != 0)														||
+						(pAd->TxSwQueue[QID_AC_VI].Number != 0)														||
+						(pAd->TxSwQueue[QID_AC_VO].Number != 0)														||
+						(RTMPFreeTXDRequest(pAd, QID_AC_BK, TX_RING_SIZE - 1, &FreeNumber) != NDIS_STATUS_SUCCESS)	||
+						(RTMPFreeTXDRequest(pAd, QID_AC_BE, TX_RING_SIZE - 1, &FreeNumber) != NDIS_STATUS_SUCCESS)	||
+						(RTMPFreeTXDRequest(pAd, QID_AC_VI, TX_RING_SIZE - 1, &FreeNumber) != NDIS_STATUS_SUCCESS)	||
+						(RTMPFreeTXDRequest(pAd, QID_AC_VO, TX_RING_SIZE - 1, &FreeNumber) != NDIS_STATUS_SUCCESS)	||
+						(RTMPFreeTXDRequest(pAd, QID_MGMT, MGMT_RING_SIZE - 1, &FreeNumber) != NDIS_STATUS_SUCCESS))
+				{
+					// TODO: consider scheduled HCCA. might not be proper to use traditional DTIM-based power-saving scheme
+					// can we cheat here (i.e. just check MGMT & AC_BE) for better performance?
+				}
+				else
+				{
+					USHORT NextDtim = DtimCount;
+
+					if (NextDtim == 0)
+						NextDtim = DtimPeriod;
+
+					TbttNumToNextWakeUp = pAd->StaCfg.DefaultListenCount;
+					if (OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_RECEIVE_DTIM) && (TbttNumToNextWakeUp > NextDtim))
+						TbttNumToNextWakeUp = NextDtim;
+
+					if (!OPSTATUS_TEST_FLAG(pAd, fOP_STATUS_DOZE))
+					{
+						AsicSleepThenAutoWakeup(pAd, TbttNumToNextWakeUp);
+					}
+				}
+			}
+		}
+		// not my BSSID, ignore it
+	}
+	// sanity check fail, ignore this frame
+}
+
+/*
+	==========================================================================
+	Description:
+		Receive PROBE REQ from remote peer when operating in IBSS mode
+	==========================================================================
+ */
+VOID PeerProbeReqAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	UCHAR         Addr2[MAC_ADDR_LEN];
+	CHAR          Ssid[MAX_LEN_OF_SSID];
+	UCHAR         SsidLen;
+#ifdef DOT11_N_SUPPORT
+	UCHAR		  HtLen, AddHtLen, NewExtLen;
+#endif // DOT11_N_SUPPORT //
+	HEADER_802_11 ProbeRspHdr;
+	NDIS_STATUS   NStatus;
+	PUCHAR        pOutBuffer = NULL;
+	ULONG         FrameLen = 0;
+	LARGE_INTEGER FakeTimestamp;
+	UCHAR         DsLen = 1, IbssLen = 2;
+	UCHAR         LocalErpIe[3] = {IE_ERP, 1, 0};
+	BOOLEAN       Privacy;
+	USHORT        CapabilityInfo;
+	UCHAR		  RSNIe = IE_WPA;
+
+	if (! ADHOC_ON(pAd))
+		return;
+
+	if (PeerProbeReqSanity(pAd, Elem->Msg, Elem->MsgLen, Addr2, Ssid, &SsidLen))
+	{
+		if ((SsidLen == 0) || SSID_EQUAL(Ssid, SsidLen, pAd->CommonCfg.Ssid, pAd->CommonCfg.SsidLen))
+		{
+			// allocate and send out ProbeRsp frame
+			NStatus = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+			if (NStatus != NDIS_STATUS_SUCCESS)
+				return;
+
+			//pAd->StaCfg.AtimWin = 0;  // ??????
+
+			Privacy = (pAd->StaCfg.WepStatus == Ndis802_11Encryption1Enabled) ||
+					  (pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) ||
+					  (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled);
+			CapabilityInfo = CAP_GENERATE(0, 1, Privacy, (pAd->CommonCfg.TxPreamble == Rt802_11PreambleShort), 0, 0);
+
+			MakeOutgoingFrame(pOutBuffer,                   &FrameLen,
+							  sizeof(HEADER_802_11),        &ProbeRspHdr,
+							  TIMESTAMP_LEN,                &FakeTimestamp,
+							  2,                            &pAd->CommonCfg.BeaconPeriod,
+							  2,                            &CapabilityInfo,
+							  1,                            &SsidIe,
+							  1,                            &pAd->CommonCfg.SsidLen,
+							  pAd->CommonCfg.SsidLen,       pAd->CommonCfg.Ssid,
+							  1,                            &SupRateIe,
+							  1,                            &pAd->StaActive.SupRateLen,
+							  pAd->StaActive.SupRateLen,    pAd->StaActive.SupRate,
+							  1,                            &DsIe,
+							  1,                            &DsLen,
+							  1,                            &pAd->CommonCfg.Channel,
+							  1,                            &IbssIe,
+							  1,                            &IbssLen,
+							  2,                            &pAd->StaActive.AtimWin,
+							  END_OF_ARGS);
+
+			if (pAd->StaActive.ExtRateLen)
+			{
+				ULONG tmp;
+				MakeOutgoingFrame(pOutBuffer + FrameLen,        &tmp,
+								  3,                            LocalErpIe,
+								  1,                            &ExtRateIe,
+								  1,                            &pAd->StaActive.ExtRateLen,
+								  pAd->StaActive.ExtRateLen,    &pAd->StaActive.ExtRate,
+								  END_OF_ARGS);
+				FrameLen += tmp;
+			}
+
+			// If adhoc secruity is set for WPA-None, append the cipher suite IE
+			if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+			{
+				ULONG tmp;
+				MakeOutgoingFrame(pOutBuffer + FrameLen,        	&tmp,
+						  			1,                              &RSNIe,
+						  			1,                            	&pAd->StaCfg.RSNIE_Len,
+						  			pAd->StaCfg.RSNIE_Len,      	pAd->StaCfg.RSN_IE,
+						  			END_OF_ARGS);
+				FrameLen += tmp;
+			}
+#ifdef DOT11_N_SUPPORT
+			if (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED)
+			{
+				ULONG TmpLen;
+				UCHAR	BROADCOM[4] = {0x0, 0x90, 0x4c, 0x33};
+				HtLen = sizeof(pAd->CommonCfg.HtCapability);
+				AddHtLen = sizeof(pAd->CommonCfg.AddHTInfo);
+				NewExtLen = 1;
+				//New extension channel offset IE is included in Beacon, Probe Rsp or channel Switch Announcement Frame
+				if (pAd->bBroadComHT == TRUE)
+				{
+					MakeOutgoingFrame(pOutBuffer + FrameLen,            &TmpLen,
+								  1,                                &WpaIe,
+								  4,                                &BROADCOM[0],
+								 pAd->MlmeAux.HtCapabilityLen,          &pAd->MlmeAux.HtCapability,
+								  END_OF_ARGS);
+				}
+				else
+				{
+				MakeOutgoingFrame(pOutBuffer + FrameLen,            &TmpLen,
+								  1,                                &HtCapIe,
+								  1,                                &HtLen,
+								 sizeof(HT_CAPABILITY_IE),          &pAd->CommonCfg.HtCapability,
+								  1,                                &AddHtInfoIe,
+								  1,                                &AddHtLen,
+								 sizeof(ADD_HT_INFO_IE),          &pAd->CommonCfg.AddHTInfo,
+								  1,                                &NewExtChanIe,
+								  1,                                &NewExtLen,
+								 sizeof(NEW_EXT_CHAN_IE),          &pAd->CommonCfg.NewExtChanOffset,
+								  END_OF_ARGS);
+				}
+				FrameLen += TmpLen;
+			}
+#endif // DOT11_N_SUPPORT //
+			MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+			MlmeFreeMemory(pAd, pOutBuffer);
+		}
+	}
+}
+
+VOID BeaconTimeoutAtJoinAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("SYNC - BeaconTimeoutAtJoinAction\n"));
+	pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+	Status = MLME_REJ_TIMEOUT;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_JOIN_CONF, 2, &Status);
+}
+
+/*
+	==========================================================================
+	Description:
+		Scan timeout procedure. basically add channel index by 1 and rescan
+	==========================================================================
+ */
+VOID ScanTimeoutAction(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	pAd->MlmeAux.Channel = NextChannel(pAd, pAd->MlmeAux.Channel);
+
+	// Only one channel scanned for CISCO beacon request
+	if ((pAd->MlmeAux.ScanType == SCAN_CISCO_ACTIVE) ||
+		(pAd->MlmeAux.ScanType == SCAN_CISCO_PASSIVE) ||
+		(pAd->MlmeAux.ScanType == SCAN_CISCO_NOISE) ||
+		(pAd->MlmeAux.ScanType == SCAN_CISCO_CHANNEL_LOAD))
+		pAd->MlmeAux.Channel = 0;
+
+	// this routine will stop if pAd->MlmeAux.Channel == 0
+	ScanNextChannel(pAd);
+}
+
+/*
+	==========================================================================
+	Description:
+	==========================================================================
+ */
+VOID InvalidStateWhenScan(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("AYNC - InvalidStateWhenScan(state=%ld). Reset SYNC machine\n", pAd->Mlme.SyncMachine.CurrState));
+	pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+	Status = MLME_STATE_MACHINE_REJECT;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_SCAN_CONF, 2, &Status);
+}
+
+/*
+	==========================================================================
+	Description:
+	==========================================================================
+ */
+VOID InvalidStateWhenJoin(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("InvalidStateWhenJoin(state=%ld). Reset SYNC machine\n", pAd->Mlme.SyncMachine.CurrState));
+	pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+	Status = MLME_STATE_MACHINE_REJECT;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_JOIN_CONF, 2, &Status);
+}
+
+/*
+	==========================================================================
+	Description:
+	==========================================================================
+ */
+VOID InvalidStateWhenStart(
+	IN PRTMP_ADAPTER pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	USHORT Status;
+	DBGPRINT(RT_DEBUG_TRACE, ("InvalidStateWhenStart(state=%ld). Reset SYNC machine\n", pAd->Mlme.SyncMachine.CurrState));
+	pAd->Mlme.SyncMachine.CurrState = SYNC_IDLE;
+	Status = MLME_STATE_MACHINE_REJECT;
+	MlmeEnqueue(pAd, MLME_CNTL_STATE_MACHINE, MT2_START_CONF, 2, &Status);
+}
+
+/*
+	==========================================================================
+	Description:
+
+	IRQL = DISPATCH_LEVEL
+
+	==========================================================================
+ */
+VOID EnqueuePsPoll(
+	IN PRTMP_ADAPTER pAd)
+{
+#ifdef RALINK_ATE
+    if (ATE_ON(pAd))
+    {
+		return;
+    }
+#endif // RALINK_ATE //
+
+
+	if (pAd->StaCfg.WindowsPowerMode == Ndis802_11PowerModeLegacy_PSP)
+    	pAd->PsPollFrame.FC.PwrMgmt = PWR_SAVE;
+	MiniportMMRequest(pAd, 0, (PUCHAR)&pAd->PsPollFrame, sizeof(PSPOLL_FRAME));
+}
+
+
+/*
+	==========================================================================
+	Description:
+	==========================================================================
+ */
+VOID EnqueueProbeRequest(
+	IN PRTMP_ADAPTER pAd)
+{
+	NDIS_STATUS     NState;
+	PUCHAR          pOutBuffer;
+	ULONG           FrameLen = 0;
+	HEADER_802_11   Hdr80211;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("force out a ProbeRequest ...\n"));
+
+	NState = MlmeAllocateMemory(pAd, &pOutBuffer);  //Get an unused nonpaged memory
+	if (NState == NDIS_STATUS_SUCCESS)
+	{
+		MgtMacHeaderInit(pAd, &Hdr80211, SUBTYPE_PROBE_REQ, 0, BROADCAST_ADDR, BROADCAST_ADDR);
+
+		// this ProbeRequest explicitly specify SSID to reduce unwanted ProbeResponse
+		MakeOutgoingFrame(pOutBuffer,                     &FrameLen,
+						  sizeof(HEADER_802_11),          &Hdr80211,
+						  1,                              &SsidIe,
+						  1,                              &pAd->CommonCfg.SsidLen,
+						  pAd->CommonCfg.SsidLen,		  pAd->CommonCfg.Ssid,
+						  1,                              &SupRateIe,
+						  1,                              &pAd->StaActive.SupRateLen,
+						  pAd->StaActive.SupRateLen,      pAd->StaActive.SupRate,
+						  END_OF_ARGS);
+		MiniportMMRequest(pAd, 0, pOutBuffer, FrameLen);
+		MlmeFreeMemory(pAd, pOutBuffer);
+	}
+
+}
+
+#ifdef DOT11_N_SUPPORT
+#ifdef DOT11N_DRAFT3
+VOID BuildEffectedChannelList(
+	IN PRTMP_ADAPTER pAd)
+{
+	UCHAR		EChannel[11];
+	UCHAR		i, j, k;
+	UCHAR		UpperChannel = 0, LowerChannel = 0;
+
+	RTMPZeroMemory(EChannel, 11);
+	i = 0;
+	// Find upper channel and lower channel.
+	if (pAd->CommonCfg.CentralChannel < pAd->CommonCfg.Channel)
+	{
+		UpperChannel = pAd->CommonCfg.Channel;
+		LowerChannel = pAd->CommonCfg.CentralChannel;
+	}
+	else if (pAd->CommonCfg.CentralChannel > pAd->CommonCfg.Channel)
+	{
+		UpperChannel = pAd->CommonCfg.CentralChannel;
+		LowerChannel = pAd->CommonCfg.Channel;
+	}
+	else
+	{
+		return;
+	}
+
+	// Record channels that is below lower channel..
+	if (LowerChannel > 1)
+	{
+		EChannel[0] = LowerChannel - 1;
+		i = 1;
+		if (LowerChannel > 2)
+		{
+			EChannel[1] = LowerChannel - 2;
+			i = 2;
+			if (LowerChannel > 3)
+			{
+				EChannel[2] = LowerChannel - 3;
+				i = 3;
+			}
+		}
+	}
+	// Record channels that is between  lower channel and upper channel.
+	for (k = LowerChannel;k < UpperChannel;k++)
+	{
+		EChannel[i] = k;
+		i++;
+	}
+	// Record channels that is above upper channel..
+	if (LowerChannel < 11)
+	{
+		EChannel[i] = UpperChannel + 1;
+		i++;
+		if (LowerChannel < 10)
+		{
+			EChannel[i] = LowerChannel + 2;
+			i++;
+			if (LowerChannel < 9)
+			{
+				EChannel[i] = LowerChannel + 3;
+				i++;
+			}
+		}
+	}
+	//
+	for (j = 0;j < i;j++)
+	{
+		for (k = 0;k < pAd->ChannelListNum;k++)
+		{
+			if (pAd->ChannelList[k].Channel == EChannel[j])
+			{
+				pAd->ChannelList[k].bEffectedChannel = TRUE;
+				DBGPRINT(RT_DEBUG_TRACE,(" EffectedChannel( =%d)\n", EChannel[j]));
+				break;
+			}
+		}
+	}
+}
+#endif // DOT11N_DRAFT3 //
+#endif // DOT11_N_SUPPORT //
+
+BOOLEAN ScanRunning(
+		IN PRTMP_ADAPTER pAd)
+{
+	return (pAd->Mlme.SyncMachine.CurrState == SCAN_LISTEN) ? TRUE : FALSE;
+}
+
diff --git a/drivers/staging/rt2870/sta/wpa.c b/drivers/staging/rt2870/sta/wpa.c
new file mode 100644
index 00000000000..8626dcde605
--- /dev/null
+++ b/drivers/staging/rt2870/sta/wpa.c
@@ -0,0 +1,2107 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	wpa.c
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Jan	Lee		03-07-22		Initial
+	Paul Lin	03-11-28		Modify for supplicant
+*/
+#include "../rt_config.h"
+
+#define		WPARSNIE	0xdd
+#define		WPA2RSNIE	0x30
+
+//extern UCHAR BIT8[];
+UCHAR	CipherWpaPskTkip[] = {
+		0xDD, 0x16,				// RSN IE
+		0x00, 0x50, 0xf2, 0x01,	// oui
+		0x01, 0x00,				// Version
+		0x00, 0x50, 0xf2, 0x02,	// Multicast
+		0x01, 0x00,				// Number of unicast
+		0x00, 0x50, 0xf2, 0x02,	// unicast
+		0x01, 0x00,				// number of authentication method
+		0x00, 0x50, 0xf2, 0x02	// authentication
+		};
+UCHAR	CipherWpaPskTkipLen = (sizeof(CipherWpaPskTkip) / sizeof(UCHAR));
+
+UCHAR	CipherWpaPskAes[] = {
+		0xDD, 0x16, 			// RSN IE
+		0x00, 0x50, 0xf2, 0x01,	// oui
+		0x01, 0x00,				// Version
+		0x00, 0x50, 0xf2, 0x04,	// Multicast
+		0x01, 0x00,				// Number of unicast
+		0x00, 0x50, 0xf2, 0x04,	// unicast
+		0x01, 0x00,				// number of authentication method
+		0x00, 0x50, 0xf2, 0x02	// authentication
+		};
+UCHAR	CipherWpaPskAesLen = (sizeof(CipherWpaPskAes) / sizeof(UCHAR));
+
+UCHAR	CipherSuiteCiscoCCKM[] = {
+		0xDD, 0x16,				// RSN IE
+		0x00, 0x50, 0xf2, 0x01, // oui
+		0x01, 0x00,				// Version
+		0x00, 0x40, 0x96, 0x01, // Multicast
+		0x01, 0x00,				// Number of uicast
+		0x00, 0x40, 0x96, 0x01, // unicast
+		0x01, 0x00,				// number of authentication method
+		0x00, 0x40, 0x96, 0x00  // Authentication
+		};
+UCHAR	CipherSuiteCiscoCCKMLen = (sizeof(CipherSuiteCiscoCCKM) / sizeof(UCHAR));
+
+UCHAR	CipherSuiteCiscoCCKM24[] = {
+		0xDD, 0x18,				// RSN IE
+		0x00, 0x50, 0xf2, 0x01, // oui
+		0x01, 0x00,				// Version
+		0x00, 0x40, 0x96, 0x01, // Multicast
+		0x01, 0x00,				// Number of uicast
+		0x00, 0x40, 0x96, 0x01, // unicast
+		0x01, 0x00,				// number of authentication method
+		0x00, 0x40, 0x96, 0x00,
+		0x28, 0x00// Authentication
+		};
+
+UCHAR	CipherSuiteCiscoCCKM24Len = (sizeof(CipherSuiteCiscoCCKM24) / sizeof(UCHAR));
+
+UCHAR	CipherSuiteCCXTkip[] = {
+		0xDD, 0x16,				// RSN IE
+		0x00, 0x50, 0xf2, 0x01,	// oui
+		0x01, 0x00,				// Version
+		0x00, 0x50, 0xf2, 0x02,	// Multicast
+		0x01, 0x00,				// Number of unicast
+		0x00, 0x50, 0xf2, 0x02,	// unicast
+		0x01, 0x00,				// number of authentication method
+		0x00, 0x50, 0xf2, 0x01	// authentication
+		};
+UCHAR	CipherSuiteCCXTkipLen = (sizeof(CipherSuiteCCXTkip) / sizeof(UCHAR));
+
+UCHAR	CCX_LLC_HDR[] = {0xAA, 0xAA, 0x03, 0x00, 0x40, 0x96, 0x00, 0x02};
+UCHAR	LLC_NORMAL[] = {0xAA, 0xAA, 0x03, 0x00, 0x00, 0x00};
+
+UCHAR	EAPOL_FRAME[] = {0x88, 0x8E};
+
+BOOLEAN CheckRSNIE(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pData,
+	IN  UCHAR           DataLen,
+	OUT	UCHAR			*Offset);
+
+void inc_byte_array(UCHAR *counter, int len);
+
+/*
+	========================================================================
+
+	Routine Description:
+		Classify WPA EAP message type
+
+	Arguments:
+		EAPType		Value of EAP message type
+		MsgType		Internal Message definition for MLME state machine
+
+	Return Value:
+		TRUE		Found appropriate message type
+		FALSE		No appropriate message type
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+		All these constants are defined in wpa.h
+		For supplicant, there is only EAPOL Key message avaliable
+
+	========================================================================
+*/
+BOOLEAN	WpaMsgTypeSubst(
+	IN	UCHAR	EAPType,
+	OUT	INT		*MsgType)
+{
+	switch (EAPType)
+	{
+		case EAPPacket:
+			*MsgType = MT2_EAPPacket;
+			break;
+		case EAPOLStart:
+			*MsgType = MT2_EAPOLStart;
+			break;
+		case EAPOLLogoff:
+			*MsgType = MT2_EAPOLLogoff;
+			break;
+		case EAPOLKey:
+			*MsgType = MT2_EAPOLKey;
+			break;
+		case EAPOLASFAlert:
+			*MsgType = MT2_EAPOLASFAlert;
+			break;
+		default:
+			return FALSE;
+	}
+	return TRUE;
+}
+
+/*
+	==========================================================================
+	Description:
+		association	state machine init,	including state	transition and timer init
+	Parameters:
+		S -	pointer	to the association state machine
+	==========================================================================
+ */
+VOID WpaPskStateMachineInit(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	STATE_MACHINE *S,
+	OUT	STATE_MACHINE_FUNC Trans[])
+{
+	StateMachineInit(S,	Trans, MAX_WPA_PSK_STATE, MAX_WPA_PSK_MSG, (STATE_MACHINE_FUNC)Drop, WPA_PSK_IDLE, WPA_MACHINE_BASE);
+	StateMachineSetAction(S, WPA_PSK_IDLE, MT2_EAPOLKey, (STATE_MACHINE_FUNC)WpaEAPOLKeyAction);
+}
+
+/*
+	==========================================================================
+	Description:
+		This is	state machine function.
+		When receiving EAPOL packets which is  for 802.1x key management.
+		Use	both in	WPA, and WPAPSK	case.
+		In this	function, further dispatch to different	functions according	to the received	packet.	 3 categories are :
+		  1.  normal 4-way pairwisekey and 2-way groupkey handshake
+		  2.  MIC error	(Countermeasures attack)  report packet	from STA.
+		  3.  Request for pairwise/group key update	from STA
+	Return:
+	==========================================================================
+*/
+VOID WpaEAPOLKeyAction(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MLME_QUEUE_ELEM	*Elem)
+
+{
+	INT             MsgType = EAPOL_MSG_INVALID;
+	PKEY_DESCRIPTER pKeyDesc;
+	PHEADER_802_11  pHeader; //red
+	UCHAR           ZeroReplay[LEN_KEY_DESC_REPLAY];
+	UCHAR EapolVr;
+	KEY_INFO		peerKeyInfo;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("-----> WpaEAPOLKeyAction\n"));
+
+	// Get 802.11 header first
+	pHeader = (PHEADER_802_11) Elem->Msg;
+
+	// Get EAPoL-Key Descriptor
+	pKeyDesc = (PKEY_DESCRIPTER) &Elem->Msg[(LENGTH_802_11 + LENGTH_802_1_H + LENGTH_EAPOL_H)];
+
+	NdisZeroMemory((PUCHAR)&peerKeyInfo, sizeof(peerKeyInfo));
+	NdisMoveMemory((PUCHAR)&peerKeyInfo, (PUCHAR)&pKeyDesc->KeyInfo, sizeof(KEY_INFO));
+
+	*((USHORT *)&peerKeyInfo) = cpu2le16(*((USHORT *)&peerKeyInfo));
+
+
+	// 1. Check EAPOL frame version and type
+	EapolVr	= (UCHAR) Elem->Msg[LENGTH_802_11+LENGTH_802_1_H];
+
+    if (((EapolVr != EAPOL_VER) && (EapolVr != EAPOL_VER2)) || ((pKeyDesc->Type != WPA1_KEY_DESC) && (pKeyDesc->Type != WPA2_KEY_DESC)))
+	{
+        DBGPRINT(RT_DEBUG_ERROR, ("Key descripter does not match with WPA rule\n"));
+			return;
+	}
+
+	// First validate replay counter, only accept message with larger replay counter
+	// Let equal pass, some AP start with all zero replay counter
+	NdisZeroMemory(ZeroReplay, LEN_KEY_DESC_REPLAY);
+
+	if((RTMPCompareMemory(pKeyDesc->ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY) != 1) &&
+		(RTMPCompareMemory(pKeyDesc->ReplayCounter, ZeroReplay, LEN_KEY_DESC_REPLAY) != 0))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("   ReplayCounter not match   \n"));
+		return;
+	}
+
+	// Process WPA2PSK frame
+	if(pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
+	{
+		if((peerKeyInfo.KeyType == PAIRWISEKEY) &&
+			(peerKeyInfo.EKD_DL == 0) &&
+			(peerKeyInfo.KeyAck == 1) &&
+			(peerKeyInfo.KeyMic == 0) &&
+			(peerKeyInfo.Secure == 0) &&
+			(peerKeyInfo.Error == 0) &&
+			(peerKeyInfo.Request == 0))
+		{
+			MsgType = EAPOL_PAIR_MSG_1;
+			DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL Key Pairwise Message 1\n"));
+		} else if((peerKeyInfo.KeyType == PAIRWISEKEY) &&
+			(peerKeyInfo.EKD_DL  == 1) &&
+			(peerKeyInfo.KeyAck == 1) &&
+			(peerKeyInfo.KeyMic == 1) &&
+			(peerKeyInfo.Secure == 1) &&
+			(peerKeyInfo.Error == 0) &&
+			(peerKeyInfo.Request == 0))
+		{
+			MsgType = EAPOL_PAIR_MSG_3;
+			DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL Key Pairwise Message 3\n"));
+		} else if((peerKeyInfo.KeyType == GROUPKEY) &&
+			(peerKeyInfo.EKD_DL == 1) &&
+			(peerKeyInfo.KeyAck == 1) &&
+			(peerKeyInfo.KeyMic == 1) &&
+			(peerKeyInfo.Secure == 1) &&
+			(peerKeyInfo.Error == 0) &&
+			(peerKeyInfo.Request == 0))
+		{
+			MsgType = EAPOL_GROUP_MSG_1;
+			DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL Key Group Message 1\n"));
+		}
+
+		// We will assume link is up (assoc suceess and port not secured).
+		// All state has to be able to process message from previous state
+		switch(pAd->StaCfg.WpaState)
+		{
+		case SS_START:
+			if(MsgType == EAPOL_PAIR_MSG_1)
+			{
+				Wpa2PairMsg1Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_MSG_3;
+			}
+			break;
+
+		case SS_WAIT_MSG_3:
+			if(MsgType == EAPOL_PAIR_MSG_1)
+			{
+				Wpa2PairMsg1Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_MSG_3;
+			}
+			else if(MsgType == EAPOL_PAIR_MSG_3)
+			{
+				Wpa2PairMsg3Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_GROUP;
+			}
+			break;
+
+		case SS_WAIT_GROUP:		// When doing group key exchange
+		case SS_FINISH:			// This happened when update group key
+			if(MsgType == EAPOL_PAIR_MSG_1)
+			{
+			    // Reset port secured variable
+				pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+				Wpa2PairMsg1Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_MSG_3;
+			}
+			else if(MsgType == EAPOL_PAIR_MSG_3)
+			{
+			    // Reset port secured variable
+				pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+				Wpa2PairMsg3Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_GROUP;
+			}
+			else if(MsgType == EAPOL_GROUP_MSG_1)
+			{
+				WpaGroupMsg1Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_FINISH;
+			}
+			break;
+
+		default:
+			break;
+		}
+	}
+	// Process WPAPSK Frame
+	// Classify message Type, either pairwise message 1, 3, or group message 1 for supplicant
+	else if(pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK)
+	{
+		if((peerKeyInfo.KeyType == PAIRWISEKEY) &&
+			(peerKeyInfo.KeyIndex == 0) &&
+			(peerKeyInfo.KeyAck == 1) &&
+			(peerKeyInfo.KeyMic == 0) &&
+			(peerKeyInfo.Secure == 0) &&
+			(peerKeyInfo.Error == 0) &&
+			(peerKeyInfo.Request == 0))
+		{
+			MsgType = EAPOL_PAIR_MSG_1;
+			DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL Key Pairwise Message 1\n"));
+		}
+		else if((peerKeyInfo.KeyType == PAIRWISEKEY) &&
+			(peerKeyInfo.KeyIndex == 0) &&
+			(peerKeyInfo.KeyAck == 1) &&
+			(peerKeyInfo.KeyMic == 1) &&
+			(peerKeyInfo.Secure == 0) &&
+			(peerKeyInfo.Error == 0) &&
+			(peerKeyInfo.Request == 0))
+		{
+			MsgType = EAPOL_PAIR_MSG_3;
+			DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL Key Pairwise Message 3\n"));
+		}
+		else if((peerKeyInfo.KeyType == GROUPKEY) &&
+			(peerKeyInfo.KeyIndex != 0) &&
+			(peerKeyInfo.KeyAck == 1) &&
+			(peerKeyInfo.KeyMic == 1) &&
+			(peerKeyInfo.Secure == 1) &&
+			(peerKeyInfo.Error == 0) &&
+			(peerKeyInfo.Request == 0))
+		{
+			MsgType = EAPOL_GROUP_MSG_1;
+			DBGPRINT(RT_DEBUG_TRACE, ("Receive EAPOL Key Group Message 1\n"));
+		}
+
+		// We will assume link is up (assoc suceess and port not secured).
+		// All state has to be able to process message from previous state
+		switch(pAd->StaCfg.WpaState)
+		{
+		case SS_START:
+			if(MsgType == EAPOL_PAIR_MSG_1)
+			{
+				WpaPairMsg1Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_MSG_3;
+			}
+			break;
+
+		case SS_WAIT_MSG_3:
+			if(MsgType == EAPOL_PAIR_MSG_1)
+			{
+				WpaPairMsg1Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_MSG_3;
+			}
+			else if(MsgType == EAPOL_PAIR_MSG_3)
+			{
+				WpaPairMsg3Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_GROUP;
+			}
+			break;
+
+		case SS_WAIT_GROUP:		// When doing group key exchange
+		case SS_FINISH:			// This happened when update group key
+			if(MsgType == EAPOL_PAIR_MSG_1)
+			{
+				WpaPairMsg1Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_MSG_3;
+				// Reset port secured variable
+				pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+			}
+			else if(MsgType == EAPOL_PAIR_MSG_3)
+			{
+				WpaPairMsg3Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_WAIT_GROUP;
+				// Reset port secured variable
+				pAd->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+			}
+			else if(MsgType == EAPOL_GROUP_MSG_1)
+			{
+				WpaGroupMsg1Action(pAd, Elem);
+				pAd->StaCfg.WpaState = SS_FINISH;
+			}
+			break;
+
+		default:
+			break;
+		}
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<----- WpaEAPOLKeyAction\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Process Pairwise key 4-way handshaking
+
+	Arguments:
+		pAd	Pointer	to our adapter
+		Elem		Message body
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	WpaPairMsg1Action(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem)
+{
+	PHEADER_802_11      pHeader;
+	UCHAR				*mpool, *PTK, *digest;
+	PUCHAR              pOutBuffer = NULL;
+	UCHAR               Header802_3[14];
+	ULONG               FrameLen = 0;
+	PEAPOL_PACKET       pMsg1;
+	EAPOL_PACKET        Packet;
+	UCHAR               Mic[16];
+
+	DBGPRINT(RT_DEBUG_TRACE, ("WpaPairMsg1Action ----->\n"));
+
+	// allocate memory pool
+	os_alloc_mem(pAd, (PUCHAR *)&mpool, 256);
+
+	if (mpool == NULL)
+		return;
+
+	// PTK Len = 80.
+	PTK = (UCHAR *) ROUND_UP(mpool, 4);
+	// digest Len = 80.
+	digest = (UCHAR *) ROUND_UP(PTK + 80, 4);
+
+	pHeader = (PHEADER_802_11) Elem->Msg;
+
+	// Process message 1 from authenticator
+	pMsg1 = (PEAPOL_PACKET) &Elem->Msg[LENGTH_802_11 + LENGTH_802_1_H];
+
+	// 1. Save Replay counter, it will use to verify message 3 and construct message 2
+	NdisMoveMemory(pAd->StaCfg.ReplayCounter, pMsg1->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// 2. Save ANonce
+	NdisMoveMemory(pAd->StaCfg.ANonce, pMsg1->KeyDesc.KeyNonce, LEN_KEY_DESC_NONCE);
+
+	// Generate random SNonce
+	GenRandom(pAd, pAd->CurrentAddress, pAd->StaCfg.SNonce);
+
+	// Calc PTK(ANonce, SNonce)
+	WpaCountPTK(pAd,
+		pAd->StaCfg.PMK,
+		pAd->StaCfg.ANonce,
+		pAd->CommonCfg.Bssid,
+		pAd->StaCfg.SNonce,
+		pAd->CurrentAddress,
+		PTK,
+		LEN_PTK);
+
+	// Save key to PTK entry
+	NdisMoveMemory(pAd->StaCfg.PTK, PTK, LEN_PTK);
+
+	// init 802.3 header and Fill Packet
+	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
+
+	// Zero Message 2 body
+	NdisZeroMemory(&Packet, sizeof(Packet));
+	Packet.ProVer	= EAPOL_VER;
+	Packet.ProType	= EAPOLKey;
+	//
+	// Message 2 as  EAPOL-Key(0,1,0,0,0,P,0,SNonce,MIC,RSN IE)
+	//
+	Packet.KeyDesc.Type = WPA1_KEY_DESC;
+	// 1. Key descriptor version and appropriate RSN IE
+	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
+	{
+		Packet.KeyDesc.KeyInfo.KeyDescVer = 2;
+	}
+	else	  // TKIP
+	{
+		Packet.KeyDesc.KeyInfo.KeyDescVer = 1;
+	}
+
+	// fill in Data Material and its length
+	Packet.KeyDesc.KeyData[0] = IE_WPA;
+	Packet.KeyDesc.KeyData[1] = pAd->StaCfg.RSNIE_Len;
+	Packet.KeyDesc.KeyDataLen[1] = pAd->StaCfg.RSNIE_Len + 2;
+	NdisMoveMemory(&Packet.KeyDesc.KeyData[2], pAd->StaCfg.RSN_IE, pAd->StaCfg.RSNIE_Len);
+
+	// Update packet length after decide Key data payload
+	Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE + Packet.KeyDesc.KeyDataLen[1];
+
+	// Update Key length
+	Packet.KeyDesc.KeyLength[0] = pMsg1->KeyDesc.KeyLength[0];
+	Packet.KeyDesc.KeyLength[1] = pMsg1->KeyDesc.KeyLength[1];
+	// 2. Key Type PeerKey
+	Packet.KeyDesc.KeyInfo.KeyType = PAIRWISEKEY;
+
+	// 3. KeyMic field presented
+	Packet.KeyDesc.KeyInfo.KeyMic  = 1;
+
+	//Convert to little-endian format.
+	*((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));
+
+
+	// 4. Fill SNonce
+	NdisMoveMemory(Packet.KeyDesc.KeyNonce, pAd->StaCfg.SNonce, LEN_KEY_DESC_NONCE);
+
+	// 5. Key Replay Count
+	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// Send EAPOL(0, 1, 0, 0, 0, P, 0, SNonce, MIC, RSN_IE)
+	// Out buffer for transmitting message 2
+	MlmeAllocateMemory(pAd, (PUCHAR *)&pOutBuffer);  // allocate memory
+	if(pOutBuffer == NULL)
+	{
+		os_free_mem(pAd, mpool);
+		return;
+	}
+	// Prepare EAPOL frame for MIC calculation
+	// Be careful, only EAPOL frame is counted for MIC calculation
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+		Packet.Body_Len[1] + 4,    &Packet,
+		END_OF_ARGS);
+
+	// 6. Prepare and Fill MIC value
+	NdisZeroMemory(Mic, sizeof(Mic));
+	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
+	{	// AES
+
+		HMAC_SHA1(pOutBuffer, FrameLen, PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{	// TKIP
+		hmac_md5(PTK,  LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
+	}
+	NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
+
+	//hex_dump("MIC", Mic, LEN_KEY_DESC_MIC);
+
+		MakeOutgoingFrame(pOutBuffer,           	&FrameLen,
+			  			LENGTH_802_3,     			&Header802_3,
+						Packet.Body_Len[1] + 4,    &Packet,
+						END_OF_ARGS);
+
+
+	// 5. Copy frame to Tx ring and send Msg 2 to authenticator
+	RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, TRUE);
+
+	MlmeFreeMemory(pAd, (PUCHAR)pOutBuffer);
+	os_free_mem(pAd, (PUCHAR)mpool);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("WpaPairMsg1Action <-----\n"));
+}
+
+VOID Wpa2PairMsg1Action(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem)
+{
+	PHEADER_802_11      pHeader;
+	UCHAR				*mpool, *PTK, *digest;
+	PUCHAR              pOutBuffer = NULL;
+	UCHAR               Header802_3[14];
+	ULONG               FrameLen = 0;
+	PEAPOL_PACKET       pMsg1;
+	EAPOL_PACKET        Packet;
+	UCHAR               Mic[16];
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Wpa2PairMsg1Action ----->\n"));
+
+	// allocate memory pool
+	os_alloc_mem(pAd, (PUCHAR *)&mpool, 256);
+
+	if (mpool == NULL)
+		return;
+
+	// PTK Len = 80.
+	PTK = (UCHAR *) ROUND_UP(mpool, 4);
+	// digest Len = 80.
+	digest = (UCHAR *) ROUND_UP(PTK + 80, 4);
+
+	pHeader = (PHEADER_802_11) Elem->Msg;
+
+	// Process message 1 from authenticator
+		pMsg1 = (PEAPOL_PACKET)	&Elem->Msg[LENGTH_802_11 + LENGTH_802_1_H];
+
+	// 1. Save Replay counter, it will use to verify message 3 and construct message 2
+	NdisMoveMemory(pAd->StaCfg.ReplayCounter, pMsg1->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// 2. Save ANonce
+	NdisMoveMemory(pAd->StaCfg.ANonce, pMsg1->KeyDesc.KeyNonce, LEN_KEY_DESC_NONCE);
+
+	// Generate random SNonce
+	GenRandom(pAd, pAd->CurrentAddress, pAd->StaCfg.SNonce);
+
+	if(pMsg1->KeyDesc.KeyDataLen[1] > 0 )
+	{
+		// cached PMKID
+	}
+
+	// Calc PTK(ANonce, SNonce)
+	WpaCountPTK(pAd,
+		pAd->StaCfg.PMK,
+		pAd->StaCfg.ANonce,
+		pAd->CommonCfg.Bssid,
+		pAd->StaCfg.SNonce,
+		pAd->CurrentAddress,
+		PTK,
+		LEN_PTK);
+
+	// Save key to PTK entry
+	NdisMoveMemory(pAd->StaCfg.PTK, PTK, LEN_PTK);
+
+	// init 802.3 header and Fill Packet
+	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
+
+	// Zero message 2 body
+	NdisZeroMemory(&Packet, sizeof(Packet));
+	Packet.ProVer	= EAPOL_VER;
+	Packet.ProType	= EAPOLKey;
+	//
+	// Message 2 as  EAPOL-Key(0,1,0,0,0,P,0,SNonce,MIC,RSN IE)
+	//
+	Packet.KeyDesc.Type = WPA2_KEY_DESC;
+
+	// 1. Key descriptor version and appropriate RSN IE
+	if(pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+	{
+		Packet.KeyDesc.KeyInfo.KeyDescVer = 2;
+	}
+	else	  // TKIP
+	{
+		Packet.KeyDesc.KeyInfo.KeyDescVer = 1;
+	}
+
+	// fill in Data Material and its length
+	Packet.KeyDesc.KeyData[0] = IE_WPA2;
+	Packet.KeyDesc.KeyData[1] = pAd->StaCfg.RSNIE_Len;
+	Packet.KeyDesc.KeyDataLen[1] = pAd->StaCfg.RSNIE_Len + 2;
+	NdisMoveMemory(&Packet.KeyDesc.KeyData[2], pAd->StaCfg.RSN_IE, pAd->StaCfg.RSNIE_Len);
+
+	// Update packet length after decide Key data payload
+	Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE + Packet.KeyDesc.KeyDataLen[1];
+
+	// 2. Key Type PeerKey
+	Packet.KeyDesc.KeyInfo.KeyType = PAIRWISEKEY;
+
+	// 3. KeyMic field presented
+	Packet.KeyDesc.KeyInfo.KeyMic  = 1;
+
+	// Update Key Length
+	Packet.KeyDesc.KeyLength[0] = 0;
+	Packet.KeyDesc.KeyLength[1] = pMsg1->KeyDesc.KeyLength[1];
+
+	// 4. Fill SNonce
+	NdisMoveMemory(Packet.KeyDesc.KeyNonce, pAd->StaCfg.SNonce, LEN_KEY_DESC_NONCE);
+
+	// 5. Key Replay Count
+	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// Convert to little-endian format.
+	*((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));
+
+	// Send EAPOL-Key(0,1,0,0,0,P,0,SNonce,MIC,RSN IE)
+	// Out buffer for transmitting message 2
+	MlmeAllocateMemory(pAd,  (PUCHAR *)&pOutBuffer);  // allocate memory
+	if(pOutBuffer == NULL)
+	{
+		os_free_mem(pAd, mpool);
+		return;
+	}
+
+	// Prepare EAPOL frame for MIC calculation
+	// Be careful, only EAPOL frame is counted for MIC calculation
+	MakeOutgoingFrame(pOutBuffer,        &FrameLen,
+		Packet.Body_Len[1] + 4, &Packet,
+		END_OF_ARGS);
+
+	// 6. Prepare and Fill MIC value
+	NdisZeroMemory(Mic, sizeof(Mic));
+	if(pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+	{
+		// AES
+		HMAC_SHA1(pOutBuffer, FrameLen, PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{
+		hmac_md5(PTK,  LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
+	}
+	NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
+
+
+	// Make  Transmitting frame
+	MakeOutgoingFrame(pOutBuffer,           	&FrameLen,
+			  			LENGTH_802_3,     		&Header802_3,
+						Packet.Body_Len[1] + 4, &Packet,
+						END_OF_ARGS);
+
+
+	// 5. Copy frame to Tx ring
+	RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, TRUE);
+
+	MlmeFreeMemory(pAd, pOutBuffer);
+	os_free_mem(pAd, mpool);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Wpa2PairMsg1Action <-----\n"));
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Process Pairwise key 4-way handshaking
+
+	Arguments:
+		pAd	Pointer	to our adapter
+		Elem		Message body
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	WpaPairMsg3Action(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MLME_QUEUE_ELEM	*Elem)
+
+{
+	PHEADER_802_11      pHeader;
+	PUCHAR          	pOutBuffer = NULL;
+	UCHAR               Header802_3[14];
+	ULONG           	FrameLen = 0;
+	EAPOL_PACKET        Packet;
+	PEAPOL_PACKET       pMsg3;
+	UCHAR           	Mic[16], OldMic[16];
+	MAC_TABLE_ENTRY 	*pEntry = NULL;
+	UCHAR				skip_offset;
+	KEY_INFO			peerKeyInfo;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("WpaPairMsg3Action ----->\n"));
+
+	// Record 802.11 header & the received EAPOL packet Msg3
+	pHeader = (PHEADER_802_11) Elem->Msg;
+	pMsg3 = (PEAPOL_PACKET)	&Elem->Msg[LENGTH_802_11 + LENGTH_802_1_H];
+
+	NdisZeroMemory((PUCHAR)&peerKeyInfo, sizeof(peerKeyInfo));
+	NdisMoveMemory((PUCHAR)&peerKeyInfo, (PUCHAR)&pMsg3->KeyDesc.KeyInfo, sizeof(KEY_INFO));
+
+	*((USHORT*)&peerKeyInfo) = cpu2le16(*((USHORT*)&peerKeyInfo));
+
+
+	// 1. Verify cipher type match
+	if (pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled && (peerKeyInfo.KeyDescVer != 2))
+	{
+		return;
+	}
+	else if(pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled && (peerKeyInfo.KeyDescVer != 1))
+	{
+		return;
+	}
+
+	// Verify RSN IE
+	//if (!RTMPEqualMemory(pMsg3->KeyDesc.KeyData, pAd->MacTab.Content[BSSID_WCID].RSN_IE, pAd->MacTab.Content[BSSID_WCID].RSNIE_Len))
+	if (!CheckRSNIE(pAd, pMsg3->KeyDesc.KeyData, pMsg3->KeyDesc.KeyDataLen[1], &skip_offset))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("RSN_IE Different in Msg 3 of WPA1 4-way handshake!! \n"));
+		hex_dump("The original RSN_IE", pAd->MacTab.Content[BSSID_WCID].RSN_IE, pAd->MacTab.Content[BSSID_WCID].RSNIE_Len);
+		hex_dump("The received RSN_IE", pMsg3->KeyDesc.KeyData, pMsg3->KeyDesc.KeyDataLen[1]);
+		return;
+	}
+	else
+		DBGPRINT(RT_DEBUG_TRACE, ("RSN_IE VALID in Msg 3 of WPA1 4-way handshake!! \n"));
+
+
+	// 2. Check MIC value
+	// Save the MIC and replace with zero
+	NdisMoveMemory(OldMic, pMsg3->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+	NdisZeroMemory(pMsg3->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
+	{
+		// AES
+		UCHAR digest[80];
+
+		HMAC_SHA1((PUCHAR) pMsg3, pMsg3->Body_Len[1] + 4, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else	// TKIP
+	{
+		hmac_md5(pAd->StaCfg.PTK, LEN_EAP_MICK, (PUCHAR) pMsg3, pMsg3->Body_Len[1] + 4, Mic);
+	}
+
+	if(!NdisEqualMemory(OldMic, Mic, LEN_KEY_DESC_MIC))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, (" MIC Different in msg 3 of 4-way handshake!!!!!!!!!! \n"));
+		return;
+	}
+	else
+		DBGPRINT(RT_DEBUG_TRACE, (" MIC VALID in msg 3 of 4-way handshake!!!!!!!!!! \n"));
+
+	// 3. Check Replay Counter, it has to be larger than last one. No need to be exact one larger
+	if(RTMPCompareMemory(pMsg3->KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY) != 1)
+		return;
+
+	// Update new replay counter
+	NdisMoveMemory(pAd->StaCfg.ReplayCounter, pMsg3->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// 4. Double check ANonce
+	if(!NdisEqualMemory(pAd->StaCfg.ANonce, pMsg3->KeyDesc.KeyNonce, LEN_KEY_DESC_NONCE))
+		return;
+
+	// init 802.3 header and Fill Packet
+	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
+
+	// Zero Message 4 body
+	NdisZeroMemory(&Packet, sizeof(Packet));
+	Packet.ProVer	= EAPOL_VER;
+	Packet.ProType	= EAPOLKey;
+	Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE;		// No data field
+
+	//
+	// Message 4 as  EAPOL-Key(0,1,0,0,0,P,0,0,MIC,0)
+	//
+	Packet.KeyDesc.Type = WPA1_KEY_DESC;
+
+	// Key descriptor version and appropriate RSN IE
+	Packet.KeyDesc.KeyInfo.KeyDescVer = peerKeyInfo.KeyDescVer;
+
+	// Update Key Length
+	Packet.KeyDesc.KeyLength[0] = pMsg3->KeyDesc.KeyLength[0];
+	Packet.KeyDesc.KeyLength[1] = pMsg3->KeyDesc.KeyLength[1];
+
+	// Key Type PeerKey
+	Packet.KeyDesc.KeyInfo.KeyType = PAIRWISEKEY;
+
+	// KeyMic field presented
+	Packet.KeyDesc.KeyInfo.KeyMic  = 1;
+
+	// In Msg3,  KeyInfo.secure =0 if Group Key HS to come. 1 if no group key HS
+	// Station sends Msg4  KeyInfo.secure should be the same as that in Msg.3
+	Packet.KeyDesc.KeyInfo.Secure= peerKeyInfo.Secure;
+
+	// Convert to little-endian format.
+	*((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));
+
+	// Key Replay count
+	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pMsg3->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// Out buffer for transmitting message 4
+	MlmeAllocateMemory(pAd, (PUCHAR *)&pOutBuffer);  // allocate memory
+	if(pOutBuffer == NULL)
+		return;
+
+	// Prepare EAPOL frame for MIC calculation
+	// Be careful, only EAPOL frame is counted for MIC calculation
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+		Packet.Body_Len[1] + 4,    &Packet,
+		END_OF_ARGS);
+
+	// Prepare and Fill MIC value
+	NdisZeroMemory(Mic, sizeof(Mic));
+	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
+	{
+		// AES
+		UCHAR digest[80];
+
+		HMAC_SHA1(pOutBuffer, FrameLen, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{
+		hmac_md5(pAd->StaCfg.PTK, LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
+	}
+	NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
+
+	// Update PTK
+	// Prepare pair-wise key information into shared key table
+	NdisZeroMemory(&pAd->SharedKey[BSS0][0], sizeof(CIPHER_KEY));
+	pAd->SharedKey[BSS0][0].KeyLen = LEN_TKIP_EK;
+    NdisMoveMemory(pAd->SharedKey[BSS0][0].Key, &pAd->StaCfg.PTK[32], LEN_TKIP_EK);
+	NdisMoveMemory(pAd->SharedKey[BSS0][0].RxMic, &pAd->StaCfg.PTK[48], LEN_TKIP_RXMICK);
+	NdisMoveMemory(pAd->SharedKey[BSS0][0].TxMic, &pAd->StaCfg.PTK[48+LEN_TKIP_RXMICK], LEN_TKIP_TXMICK);
+
+	// Decide its ChiperAlg
+	if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
+		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_TKIP;
+	else if (pAd->StaCfg.PairCipher == Ndis802_11Encryption3Enabled)
+		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_AES;
+	else
+		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_NONE;
+
+	// Update these related information to MAC_TABLE_ENTRY
+	pEntry = &pAd->MacTab.Content[BSSID_WCID];
+	NdisMoveMemory(pEntry->PairwiseKey.Key, &pAd->StaCfg.PTK[32], LEN_TKIP_EK);
+	NdisMoveMemory(pEntry->PairwiseKey.RxMic, &pAd->StaCfg.PTK[48], LEN_TKIP_RXMICK);
+	NdisMoveMemory(pEntry->PairwiseKey.TxMic, &pAd->StaCfg.PTK[48+LEN_TKIP_RXMICK], LEN_TKIP_TXMICK);
+	pEntry->PairwiseKey.CipherAlg = pAd->SharedKey[BSS0][0].CipherAlg;
+
+	// Update pairwise key information to ASIC Shared Key Table
+	AsicAddSharedKeyEntry(pAd,
+						  BSS0,
+						  0,
+						  pAd->SharedKey[BSS0][0].CipherAlg,
+						  pAd->SharedKey[BSS0][0].Key,
+						  pAd->SharedKey[BSS0][0].TxMic,
+						  pAd->SharedKey[BSS0][0].RxMic);
+
+	// Update ASIC WCID attribute table and IVEIV table
+	RTMPAddWcidAttributeEntry(pAd,
+							  BSS0,
+							  0,
+							  pAd->SharedKey[BSS0][0].CipherAlg,
+							  pEntry);
+
+	// Make transmitting frame
+	MakeOutgoingFrame(pOutBuffer,           	&FrameLen,
+			  			LENGTH_802_3,     		&Header802_3,
+						Packet.Body_Len[1] + 4, &Packet,
+						END_OF_ARGS);
+
+
+	// Copy frame to Tx ring and Send Message 4 to authenticator
+	RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, TRUE);
+
+	MlmeFreeMemory(pAd, (PUCHAR)pOutBuffer);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("WpaPairMsg3Action <-----\n"));
+}
+
+VOID    Wpa2PairMsg3Action(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  MLME_QUEUE_ELEM *Elem)
+
+{
+	PHEADER_802_11      pHeader;
+	PUCHAR              pOutBuffer = NULL;
+	UCHAR               Header802_3[14];
+	ULONG               FrameLen = 0;
+	EAPOL_PACKET        Packet;
+	PEAPOL_PACKET       pMsg3;
+	UCHAR               Mic[16], OldMic[16];
+	UCHAR               *mpool, *KEYDATA, *digest;
+	UCHAR               Key[32];
+	MAC_TABLE_ENTRY 	*pEntry = NULL;
+	KEY_INFO			peerKeyInfo;
+
+	// allocate memory
+	os_alloc_mem(pAd, (PUCHAR *)&mpool, 1024);
+
+	if(mpool == NULL)
+		return;
+
+	// KEYDATA Len = 512.
+	KEYDATA = (UCHAR *) ROUND_UP(mpool, 4);
+	// digest Len = 80.
+	digest = (UCHAR *) ROUND_UP(KEYDATA + 512, 4);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Wpa2PairMsg3Action ----->\n"));
+
+	pHeader = (PHEADER_802_11) Elem->Msg;
+
+	// Process message 3 frame.
+	pMsg3 = (PEAPOL_PACKET)	&Elem->Msg[LENGTH_802_11 + LENGTH_802_1_H];
+
+	NdisZeroMemory((PUCHAR)&peerKeyInfo, sizeof(peerKeyInfo));
+	NdisMoveMemory((PUCHAR)&peerKeyInfo, (PUCHAR)&pMsg3->KeyDesc.KeyInfo, sizeof(KEY_INFO));
+
+	*((USHORT*)&peerKeyInfo) = cpu2le16(*((USHORT*)&peerKeyInfo));
+
+	// 1. Verify cipher type match
+	if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled && (peerKeyInfo.KeyDescVer!= 2))
+	{
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+	else if(pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled && (peerKeyInfo.KeyDescVer != 1))
+	{
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+
+	// 2. Check MIC value
+	// Save the MIC and replace with zero
+	NdisMoveMemory(OldMic, pMsg3->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+	NdisZeroMemory(pMsg3->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+	if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+	{
+		// AES
+		HMAC_SHA1((PUCHAR) pMsg3, pMsg3->Body_Len[1] + 4, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{
+		hmac_md5(pAd->StaCfg.PTK, LEN_EAP_MICK, (PUCHAR) pMsg3, pMsg3->Body_Len[1] + 4, Mic);
+	}
+
+	if(!NdisEqualMemory(OldMic, Mic, LEN_KEY_DESC_MIC))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, (" MIC Different in msg 3 of 4-way handshake!!!!!!!!!! \n"));
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+	else
+		DBGPRINT(RT_DEBUG_TRACE, (" MIC VALID in msg 3 of 4-way handshake!!!!!!!!!! \n"));
+
+	// 3. Check Replay Counter, it has to be larger than last one. No need to be exact one larger
+	if(RTMPCompareMemory(pMsg3->KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY) != 1)
+	{
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+
+	// Update new replay counter
+	NdisMoveMemory(pAd->StaCfg.ReplayCounter, pMsg3->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// 4. Double check ANonce
+	if(!NdisEqualMemory(pAd->StaCfg.ANonce, pMsg3->KeyDesc.KeyNonce, LEN_KEY_DESC_NONCE))
+	{
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+
+	// Obtain GTK
+	// 5. Decrypt GTK from Key Data
+	DBGPRINT_RAW(RT_DEBUG_TRACE, ("EKD = %d\n", peerKeyInfo.EKD_DL));
+	if(pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+	{
+		// Decrypt AES GTK
+		AES_GTK_KEY_UNWRAP(&pAd->StaCfg.PTK[16], KEYDATA, pMsg3->KeyDesc.KeyDataLen[1],pMsg3->KeyDesc.KeyData);
+	}
+	else	  // TKIP
+	{
+		INT i;
+		// Decrypt TKIP GTK
+		// Construct 32 bytes RC4 Key
+		NdisMoveMemory(Key, pMsg3->KeyDesc.KeyIv, 16);
+		NdisMoveMemory(&Key[16], &pAd->StaCfg.PTK[16], 16);
+		ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, Key, 32);
+		//discard first 256 bytes
+		for(i = 0; i < 256; i++)
+			ARCFOUR_BYTE(&pAd->PrivateInfo.WEPCONTEXT);
+		// Decrypt GTK. Becareful, there is no ICV to check the result is correct or not
+		ARCFOUR_DECRYPT(&pAd->PrivateInfo.WEPCONTEXT, KEYDATA, pMsg3->KeyDesc.KeyData, pMsg3->KeyDesc.KeyDataLen[1]);
+	}
+
+	if (!ParseKeyData(pAd, KEYDATA, pMsg3->KeyDesc.KeyDataLen[1], 1))
+	{
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+
+	// Update GTK to ASIC
+	// Update group key information to ASIC Shared Key Table
+	AsicAddSharedKeyEntry(pAd,
+						  BSS0,
+						  pAd->StaCfg.DefaultKeyId,
+						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
+						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key,
+						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic,
+						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic);
+
+	// Update ASIC WCID attribute table and IVEIV table
+	RTMPAddWcidAttributeEntry(pAd,
+							  BSS0,
+							  pAd->StaCfg.DefaultKeyId,
+							  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
+							  NULL);
+
+	// init 802.3 header and Fill Packet
+	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
+
+	// Zero message 4 body
+	NdisZeroMemory(&Packet, sizeof(Packet));
+	Packet.ProVer	= EAPOL_VER;
+	Packet.ProType	= EAPOLKey;
+	Packet.Body_Len[1]  	= sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE;		// No data field
+
+	//
+	// Message 4 as  EAPOL-Key(0,1,0,0,0,P,0,0,MIC,0)
+	//
+	Packet.KeyDesc.Type = WPA2_KEY_DESC;
+
+	// Key descriptor version and appropriate RSN IE
+	Packet.KeyDesc.KeyInfo.KeyDescVer = peerKeyInfo.KeyDescVer;
+
+	// Update Key Length
+	Packet.KeyDesc.KeyLength[0] = pMsg3->KeyDesc.KeyLength[0];
+	Packet.KeyDesc.KeyLength[1] = pMsg3->KeyDesc.KeyLength[1];
+
+	// Key Type PeerKey
+	Packet.KeyDesc.KeyInfo.KeyType = PAIRWISEKEY;
+
+	// KeyMic field presented
+	Packet.KeyDesc.KeyInfo.KeyMic  = 1;
+	Packet.KeyDesc.KeyInfo.Secure = 1;
+
+	// Convert to little-endian format.
+	*((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));
+
+	// Key Replay count
+	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pMsg3->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// Out buffer for transmitting message 4
+	MlmeAllocateMemory(pAd, (PUCHAR *)&pOutBuffer);  // allocate memory
+	if(pOutBuffer == NULL)
+	{
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+
+	// Prepare EAPOL frame for MIC calculation
+	// Be careful, only EAPOL frame is counted for MIC calculation
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+		Packet.Body_Len[1] + 4,    &Packet,
+		END_OF_ARGS);
+
+	// Prepare and Fill MIC value
+	NdisZeroMemory(Mic, sizeof(Mic));
+	if(pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+	{
+		// AES
+		HMAC_SHA1(pOutBuffer, FrameLen, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{
+		hmac_md5(pAd->StaCfg.PTK, LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
+	}
+	NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
+
+	// Update PTK
+	// Prepare pair-wise key information into shared key table
+	NdisZeroMemory(&pAd->SharedKey[BSS0][0], sizeof(CIPHER_KEY));
+	pAd->SharedKey[BSS0][0].KeyLen = LEN_TKIP_EK;
+    NdisMoveMemory(pAd->SharedKey[BSS0][0].Key, &pAd->StaCfg.PTK[32], LEN_TKIP_EK);
+	NdisMoveMemory(pAd->SharedKey[BSS0][0].RxMic, &pAd->StaCfg.PTK[48], LEN_TKIP_RXMICK);
+	NdisMoveMemory(pAd->SharedKey[BSS0][0].TxMic, &pAd->StaCfg.PTK[48+LEN_TKIP_RXMICK], LEN_TKIP_TXMICK);
+
+	// Decide its ChiperAlg
+	if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
+		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_TKIP;
+	else if (pAd->StaCfg.PairCipher == Ndis802_11Encryption3Enabled)
+		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_AES;
+	else
+		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_NONE;
+
+	// Update these related information to MAC_TABLE_ENTRY
+	pEntry = &pAd->MacTab.Content[BSSID_WCID];
+	NdisMoveMemory(&pEntry->PairwiseKey.Key, &pAd->StaCfg.PTK[32], LEN_TKIP_EK);
+	NdisMoveMemory(&pEntry->PairwiseKey.RxMic, &pAd->StaCfg.PTK[48], LEN_TKIP_RXMICK);
+	NdisMoveMemory(&pEntry->PairwiseKey.TxMic, &pAd->StaCfg.PTK[48+LEN_TKIP_RXMICK], LEN_TKIP_TXMICK);
+	pEntry->PairwiseKey.CipherAlg = pAd->SharedKey[BSS0][0].CipherAlg;
+
+	// Update pairwise key information to ASIC Shared Key Table
+	AsicAddSharedKeyEntry(pAd,
+						  BSS0,
+						  0,
+						  pAd->SharedKey[BSS0][0].CipherAlg,
+						  pAd->SharedKey[BSS0][0].Key,
+						  pAd->SharedKey[BSS0][0].TxMic,
+						  pAd->SharedKey[BSS0][0].RxMic);
+
+	// Update ASIC WCID attribute table and IVEIV table
+	RTMPAddWcidAttributeEntry(pAd,
+							  BSS0,
+							  0,
+							  pAd->SharedKey[BSS0][0].CipherAlg,
+							  pEntry);
+
+	// Make  Transmitting frame
+	MakeOutgoingFrame(pOutBuffer,           	&FrameLen,
+			  			LENGTH_802_3,     		&Header802_3,
+						Packet.Body_Len[1] + 4, &Packet,
+						END_OF_ARGS);
+
+
+	// Copy frame to Tx ring and Send Message 4 to authenticator
+	RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, TRUE);
+
+	// set 802.1x port control
+	//pAd->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+	STA_PORT_SECURED(pAd);
+
+    // Indicate Connected for GUI
+    pAd->IndicateMediaState = NdisMediaStateConnected;
+
+	MlmeFreeMemory(pAd, (PUCHAR)pOutBuffer);
+	os_free_mem(pAd, (PUCHAR)mpool);
+
+
+	// send wireless event - for set key done WPA2
+	if (pAd->CommonCfg.bWirelessEvent)
+		RTMPSendWirelessEvent(pAd, IW_SET_KEY_DONE_WPA2_EVENT_FLAG, pEntry->Addr, BSS0, 0);
+
+	DBGPRINT(RT_DEBUG_ERROR, ("Wpa2PairMsg3Action <-----\n"));
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Process Group key 2-way handshaking
+
+	Arguments:
+		pAd	Pointer	to our adapter
+		Elem		Message body
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	WpaGroupMsg1Action(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	MLME_QUEUE_ELEM	*Elem)
+
+{
+	PUCHAR              pOutBuffer = NULL;
+	UCHAR               Header802_3[14];
+	ULONG               FrameLen = 0;
+	EAPOL_PACKET        Packet;
+	PEAPOL_PACKET       pGroup;
+	UCHAR               *mpool, *digest, *KEYDATA;
+	UCHAR               Mic[16], OldMic[16];
+	UCHAR               GTK[32], Key[32];
+	KEY_INFO			peerKeyInfo;
+
+	// allocate memory
+	os_alloc_mem(pAd, (PUCHAR *)&mpool, 1024);
+
+	if(mpool == NULL)
+		return;
+
+	// digest Len = 80.
+	digest = (UCHAR *) ROUND_UP(mpool, 4);
+	// KEYDATA Len = 512.
+	KEYDATA = (UCHAR *) ROUND_UP(digest + 80, 4);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("WpaGroupMsg1Action ----->\n"));
+
+	// Process Group Message 1 frame. skip 802.11 header(24) & LLC_SNAP header(8)
+	pGroup = (PEAPOL_PACKET) &Elem->Msg[LENGTH_802_11 + LENGTH_802_1_H];
+
+	NdisZeroMemory((PUCHAR)&peerKeyInfo, sizeof(peerKeyInfo));
+	NdisMoveMemory((PUCHAR)&peerKeyInfo, (PUCHAR)&pGroup->KeyDesc.KeyInfo, sizeof(KEY_INFO));
+
+	*((USHORT*)&peerKeyInfo) = cpu2le16(*((USHORT*)&peerKeyInfo));
+
+	// 0. Check cipher type match
+	if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled && (peerKeyInfo.KeyDescVer != 2))
+	{
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+	else if (pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled && (peerKeyInfo.KeyDescVer != 1))
+	{
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+
+	// 1. Verify Replay counter
+	//    Check Replay Counter, it has to be larger than last one. No need to be exact one larger
+	if(RTMPCompareMemory(pGroup->KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY) != 1)
+	{
+		os_free_mem(pAd, (PUCHAR)mpool);
+		return;
+	}
+
+	// Update new replay counter
+	NdisMoveMemory(pAd->StaCfg.ReplayCounter, pGroup->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// 2. Verify MIC is valid
+	// Save the MIC and replace with zero
+	NdisMoveMemory(OldMic, pGroup->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+	NdisZeroMemory(pGroup->KeyDesc.KeyMic, LEN_KEY_DESC_MIC);
+
+	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
+	{	// AES
+		HMAC_SHA1((PUCHAR) pGroup, pGroup->Body_Len[1] + 4, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{	// TKIP
+		hmac_md5(pAd->StaCfg.PTK, LEN_EAP_MICK, (PUCHAR) pGroup, pGroup->Body_Len[1] + 4, Mic);
+	}
+
+	if(!NdisEqualMemory(OldMic, Mic, LEN_KEY_DESC_MIC))
+	{
+		DBGPRINT(RT_DEBUG_ERROR, (" MIC Different in group msg 1 of 2-way handshake!!!!!!!!!! \n"));
+		MlmeFreeMemory(pAd, (PUCHAR)mpool);
+		return;
+	}
+	else
+		DBGPRINT(RT_DEBUG_TRACE, (" MIC VALID in group msg 1 of 2-way handshake!!!!!!!!!! \n"));
+
+
+	// 3. Decrypt GTK from Key Data
+	if (pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled)
+	{
+		// Decrypt AES GTK
+		AES_GTK_KEY_UNWRAP(&pAd->StaCfg.PTK[16], KEYDATA,  pGroup->KeyDesc.KeyDataLen[1], pGroup->KeyDesc.KeyData);
+	}
+	else	// TKIP
+	{
+		INT i;
+
+		// Decrypt TKIP GTK
+		// Construct 32 bytes RC4 Key
+		NdisMoveMemory(Key, pGroup->KeyDesc.KeyIv, 16);
+		NdisMoveMemory(&Key[16], &pAd->StaCfg.PTK[16], 16);
+		ARCFOUR_INIT(&pAd->PrivateInfo.WEPCONTEXT, Key, 32);
+		//discard first 256 bytes
+		for(i = 0; i < 256; i++)
+			ARCFOUR_BYTE(&pAd->PrivateInfo.WEPCONTEXT);
+		// Decrypt GTK. Becareful, there is no ICV to check the result is correct or not
+		ARCFOUR_DECRYPT(&pAd->PrivateInfo.WEPCONTEXT, KEYDATA, pGroup->KeyDesc.KeyData, pGroup->KeyDesc.KeyDataLen[1]);
+	}
+
+	// Process decrypted key data material
+	// Parse keyData to handle KDE format for WPA2PSK
+	if (peerKeyInfo.EKD_DL)
+	{
+		if (!ParseKeyData(pAd, KEYDATA, pGroup->KeyDesc.KeyDataLen[1], 0))
+		{
+			os_free_mem(pAd, (PUCHAR)mpool);
+			return;
+		}
+	}
+	else	// WPAPSK
+	{
+		// set key material, TxMic and RxMic for WPAPSK
+		NdisMoveMemory(GTK, KEYDATA, 32);
+		NdisMoveMemory(pAd->StaCfg.GTK, GTK, 32);
+		pAd->StaCfg.DefaultKeyId = peerKeyInfo.KeyIndex;
+
+		// Prepare pair-wise key information into shared key table
+		NdisZeroMemory(&pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId], sizeof(CIPHER_KEY));
+		pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].KeyLen = LEN_TKIP_EK;
+		NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key, GTK, LEN_TKIP_EK);
+		NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic, &GTK[16], LEN_TKIP_RXMICK);
+		NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic, &GTK[24], LEN_TKIP_TXMICK);
+
+		// Update Shared Key CipherAlg
+		pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_NONE;
+		if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption2Enabled)
+			pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_TKIP;
+		else if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption3Enabled)
+			pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_AES;
+		else if (pAd->StaCfg.GroupCipher == Ndis802_11GroupWEP40Enabled)
+			pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_WEP64;
+		else if (pAd->StaCfg.GroupCipher == Ndis802_11GroupWEP104Enabled)
+			pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_WEP128;
+
+    	//hex_dump("Group Key :", pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key, LEN_TKIP_EK);
+	}
+
+	// Update group key information to ASIC Shared Key Table
+	AsicAddSharedKeyEntry(pAd,
+						  BSS0,
+						  pAd->StaCfg.DefaultKeyId,
+						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
+						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key,
+						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic,
+						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic);
+
+	// Update ASIC WCID attribute table and IVEIV table
+	RTMPAddWcidAttributeEntry(pAd,
+							  BSS0,
+							  pAd->StaCfg.DefaultKeyId,
+							  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
+							  NULL);
+
+	// set 802.1x port control
+	//pAd->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+	STA_PORT_SECURED(pAd);
+
+    // Indicate Connected for GUI
+    pAd->IndicateMediaState = NdisMediaStateConnected;
+
+	// init header and Fill Packet
+	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
+
+	// Zero Group message 1 body
+	NdisZeroMemory(&Packet, sizeof(Packet));
+	Packet.ProVer	= EAPOL_VER;
+	Packet.ProType	= EAPOLKey;
+	Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE;		// No data field
+
+	//
+	// Group Message 2 as  EAPOL-Key(1,0,0,0,G,0,0,MIC,0)
+	//
+	if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
+	{
+		Packet.KeyDesc.Type = WPA2_KEY_DESC;
+	}
+	else
+	{
+		Packet.KeyDesc.Type = WPA1_KEY_DESC;
+	}
+
+	// Key descriptor version and appropriate RSN IE
+	Packet.KeyDesc.KeyInfo.KeyDescVer = peerKeyInfo.KeyDescVer;
+
+	// Update Key Length
+	Packet.KeyDesc.KeyLength[0] = pGroup->KeyDesc.KeyLength[0];
+	Packet.KeyDesc.KeyLength[1] = pGroup->KeyDesc.KeyLength[1];
+
+	// Key Index as G-Msg 1
+	if(pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK)
+		Packet.KeyDesc.KeyInfo.KeyIndex = peerKeyInfo.KeyIndex;
+
+	// Key Type Group key
+	Packet.KeyDesc.KeyInfo.KeyType = GROUPKEY;
+
+	// KeyMic field presented
+	Packet.KeyDesc.KeyInfo.KeyMic  = 1;
+
+	// Secure bit
+	Packet.KeyDesc.KeyInfo.Secure  = 1;
+
+	// Convert to little-endian format.
+	*((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));
+
+	// Key Replay count
+	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pGroup->KeyDesc.ReplayCounter, LEN_KEY_DESC_REPLAY);
+
+	// Out buffer for transmitting group message 2
+	MlmeAllocateMemory(pAd, (PUCHAR *)&pOutBuffer);  // allocate memory
+	if(pOutBuffer == NULL)
+	{
+		MlmeFreeMemory(pAd, (PUCHAR)mpool);
+		return;
+	}
+
+	// Prepare EAPOL frame for MIC calculation
+	// Be careful, only EAPOL frame is counted for MIC calculation
+	MakeOutgoingFrame(pOutBuffer,           &FrameLen,
+		Packet.Body_Len[1] + 4,    &Packet,
+		END_OF_ARGS);
+
+	// Prepare and Fill MIC value
+	NdisZeroMemory(Mic, sizeof(Mic));
+	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
+	{
+		// AES
+		HMAC_SHA1(pOutBuffer, FrameLen, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{
+		hmac_md5(pAd->StaCfg.PTK, LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
+	}
+	NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
+
+
+	MakeOutgoingFrame(pOutBuffer,       		&FrameLen,
+						LENGTH_802_3,     		&Header802_3,
+						Packet.Body_Len[1] + 4, &Packet,
+						END_OF_ARGS);
+
+
+	// 5. Copy frame to Tx ring and prepare for encryption
+	RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, FALSE);
+
+	// 6 Free allocated memory
+	MlmeFreeMemory(pAd, (PUCHAR)pOutBuffer);
+	os_free_mem(pAd, (PUCHAR)mpool);
+
+	// send wireless event - for set key done WPA2
+	if (pAd->CommonCfg.bWirelessEvent)
+		RTMPSendWirelessEvent(pAd, IW_SET_KEY_DONE_WPA2_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("WpaGroupMsg1Action <-----\n"));
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Init WPA MAC header
+
+	Arguments:
+		pAd	Pointer	to our adapter
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID	WpaMacHeaderInit(
+	IN		PRTMP_ADAPTER	pAd,
+	IN OUT	PHEADER_802_11	pHdr80211,
+	IN		UCHAR			wep,
+	IN		PUCHAR		    pAddr1)
+{
+	NdisZeroMemory(pHdr80211, sizeof(HEADER_802_11));
+	pHdr80211->FC.Type	= BTYPE_DATA;
+	pHdr80211->FC.ToDs	= 1;
+	if (wep	== 1)
+		pHdr80211->FC.Wep = 1;
+
+	 //	Addr1: BSSID, Addr2: SA, Addr3:	DA
+	COPY_MAC_ADDR(pHdr80211->Addr1, pAddr1);
+	COPY_MAC_ADDR(pHdr80211->Addr2, pAd->CurrentAddress);
+	COPY_MAC_ADDR(pHdr80211->Addr3, pAd->CommonCfg.Bssid);
+	pHdr80211->Sequence =	pAd->Sequence;
+}
+
+/*
+	========================================================================
+
+	Routine	Description:
+		Copy frame from waiting queue into relative ring buffer and set
+	appropriate ASIC register to kick hardware encryption before really
+	sent out to air.
+
+	Arguments:
+		pAd		Pointer	to our adapter
+		PNDIS_PACKET	Pointer to outgoing Ndis frame
+		NumberOfFrag	Number of fragment required
+
+	Return Value:
+		None
+
+	Note:
+
+	========================================================================
+*/
+VOID    RTMPToWirelessSta(
+	IN	PRTMP_ADAPTER	pAd,
+	IN  PUCHAR          pHeader802_3,
+    IN  UINT            HdrLen,
+	IN  PUCHAR          pData,
+    IN  UINT            DataLen,
+    IN	BOOLEAN			is4wayFrame)
+
+{
+	NDIS_STATUS     Status;
+	PNDIS_PACKET    pPacket;
+	UCHAR   Index;
+
+	do
+	{
+		// 1. build a NDIS packet and call RTMPSendPacket();
+		//    be careful about how/when to release this internal allocated NDIS PACKET buffer
+		Status = RTMPAllocateNdisPacket(pAd, &pPacket, pHeader802_3, HdrLen, pData, DataLen);
+		if (Status != NDIS_STATUS_SUCCESS)
+			break;
+
+		if (is4wayFrame)
+			RTMP_SET_PACKET_CLEAR_EAP_FRAME(pPacket, 1);
+		else
+			RTMP_SET_PACKET_CLEAR_EAP_FRAME(pPacket, 0);
+
+		// 2. send out the packet
+		Status = STASendPacket(pAd, pPacket);
+		if(Status == NDIS_STATUS_SUCCESS)
+		{
+			// Dequeue one frame from TxSwQueue0..3 queue and process it
+			// There are three place calling dequeue for TX ring.
+			// 1. Here, right after queueing the frame.
+			// 2. At the end of TxRingTxDone service routine.
+			// 3. Upon NDIS call RTMPSendPackets
+			if((!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS)) &&
+				(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_RESET_IN_PROGRESS)))
+			{
+				for(Index = 0; Index < 5; Index ++)
+					if(pAd->TxSwQueue[Index].Number > 0)
+						RTMPDeQueuePacket(pAd, FALSE, Index, MAX_TX_PROCESS);
+			}
+		}
+	} while(FALSE);
+
+}
+
+/*
+    ========================================================================
+
+    Routine Description:
+    Check Sanity RSN IE form AP
+
+    Arguments:
+
+    Return Value:
+
+
+    ========================================================================
+*/
+BOOLEAN CheckRSNIE(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pData,
+	IN  UCHAR           DataLen,
+	OUT	UCHAR			*Offset)
+{
+	PUCHAR              pVIE;
+	UCHAR               len;
+	PEID_STRUCT         pEid;
+	BOOLEAN				result = FALSE;
+
+	pVIE = pData;
+	len	 = DataLen;
+	*Offset = 0;
+
+	while (len > sizeof(RSNIE2))
+	{
+		pEid = (PEID_STRUCT) pVIE;
+		// WPA RSN IE
+		if ((pEid->Eid == IE_WPA) && (NdisEqualMemory(pEid->Octet, WPA_OUI, 4)))
+		{
+			if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA || pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) &&
+				(NdisEqualMemory(pVIE, pAd->MacTab.Content[BSSID_WCID].RSN_IE, pAd->MacTab.Content[BSSID_WCID].RSNIE_Len)) &&
+				(pAd->MacTab.Content[BSSID_WCID].RSNIE_Len == (pEid->Len + 2)))
+			{
+					DBGPRINT(RT_DEBUG_TRACE, ("CheckRSNIE ==> WPA/WPAPSK RSN IE matched in Msg 3, Length(%d) \n", (pEid->Len + 2)));
+					result = TRUE;
+			}
+
+			*Offset += (pEid->Len + 2);
+		}
+		// WPA2 RSN IE
+		else if ((pEid->Eid == IE_RSN) && (NdisEqualMemory(pEid->Octet + 2, RSN_OUI, 3)))
+		{
+			if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2 || pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK) &&
+				(NdisEqualMemory(pVIE, pAd->MacTab.Content[BSSID_WCID].RSN_IE, pAd->MacTab.Content[BSSID_WCID].RSNIE_Len)) &&
+				(pAd->MacTab.Content[BSSID_WCID].RSNIE_Len == (pEid->Len + 2)))
+			{
+					DBGPRINT(RT_DEBUG_TRACE, ("CheckRSNIE ==> WPA2/WPA2PSK RSN IE matched in Msg 3, Length(%d) \n", (pEid->Len + 2)));
+					result = TRUE;
+			}
+
+			*Offset += (pEid->Len + 2);
+		}
+		else
+		{
+			break;
+		}
+
+		pVIE += (pEid->Len + 2);
+		len  -= (pEid->Len + 2);
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("CheckRSNIE ==> skip_offset(%d) \n", *Offset));
+
+	return result;
+
+}
+
+
+/*
+    ========================================================================
+
+    Routine Description:
+    Parse KEYDATA field.  KEYDATA[] May contain 2 RSN IE and optionally GTK.
+    GTK  is encaptulated in KDE format at  p.83 802.11i D10
+
+    Arguments:
+
+    Return Value:
+
+    Note:
+        802.11i D10
+
+    ========================================================================
+*/
+BOOLEAN ParseKeyData(
+	IN  PRTMP_ADAPTER   pAd,
+	IN  PUCHAR          pKeyData,
+	IN  UCHAR           KeyDataLen,
+	IN	UCHAR			bPairewise)
+{
+    PKDE_ENCAP          pKDE = NULL;
+    PUCHAR              pMyKeyData = pKeyData;
+    UCHAR               KeyDataLength = KeyDataLen;
+    UCHAR               GTKLEN;
+	UCHAR				skip_offset;
+
+	// Verify The RSN IE contained in Pairewise-Msg 3 and skip it
+	if (bPairewise)
+    {
+		// Check RSN IE whether it is WPA2/WPA2PSK
+		if (!CheckRSNIE(pAd, pKeyData, KeyDataLen, &skip_offset))
+		{
+			DBGPRINT(RT_DEBUG_ERROR, ("ParseKeyData ==> WPA2/WPA2PSK RSN IE mismatched \n"));
+			hex_dump("Get KEYDATA :", pKeyData, KeyDataLen);
+			return FALSE;
+    	}
+    	else
+		{
+			// skip RSN IE
+			pMyKeyData += skip_offset;
+			KeyDataLength -= skip_offset;
+
+			//DBGPRINT(RT_DEBUG_TRACE, ("ParseKeyData ==> WPA2/WPA2PSK RSN IE matched in Msg 3, Length(%d) \n", skip_offset));
+		}
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE,("ParseKeyData ==> KeyDataLength %d without RSN_IE \n", KeyDataLength));
+
+	// Parse EKD format
+	if (KeyDataLength >= 8)
+    {
+        pKDE = (PKDE_ENCAP) pMyKeyData;
+    }
+	else
+    {
+		DBGPRINT(RT_DEBUG_ERROR, ("ERROR: KeyDataLength is too short \n"));
+        return FALSE;
+    }
+
+
+	// Sanity check - shared key index should not be 0
+	if (pKDE->GTKEncap.Kid == 0)
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("ERROR: GTK Key index zero \n"));
+        return FALSE;
+    }
+
+	// Sanity check - KED length
+	if (KeyDataLength < (pKDE->Len + 2))
+    {
+        DBGPRINT(RT_DEBUG_ERROR, ("ERROR: The len from KDE is too short \n"));
+        return FALSE;
+    }
+
+	// Get GTK length - refer to IEEE 802.11i-2004 p.82
+	GTKLEN = pKDE->Len -6;
+
+	if (GTKLEN < MIN_LEN_OF_GTK)
+	{
+		DBGPRINT(RT_DEBUG_ERROR, ("ERROR: GTK Key length is too short (%d) \n", GTKLEN));
+        return FALSE;
+	}
+	else
+		DBGPRINT(RT_DEBUG_TRACE, ("GTK Key with KDE formet got index=%d, len=%d \n", pKDE->GTKEncap.Kid, GTKLEN));
+
+	// Update GTK
+	// set key material, TxMic and RxMic for WPAPSK
+	NdisMoveMemory(pAd->StaCfg.GTK, pKDE->GTKEncap.GTK, 32);
+	pAd->StaCfg.DefaultKeyId = pKDE->GTKEncap.Kid;
+
+	// Update shared key table
+	NdisZeroMemory(&pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId], sizeof(CIPHER_KEY));
+	pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].KeyLen = LEN_TKIP_EK;
+	NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key, pKDE->GTKEncap.GTK, LEN_TKIP_EK);
+	NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic, &pKDE->GTKEncap.GTK[16], LEN_TKIP_RXMICK);
+	NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic, &pKDE->GTKEncap.GTK[24], LEN_TKIP_TXMICK);
+
+	// Update Shared Key CipherAlg
+	pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_NONE;
+	if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption2Enabled)
+		pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_TKIP;
+	else if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption3Enabled)
+		pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_AES;
+	else if (pAd->StaCfg.GroupCipher == Ndis802_11GroupWEP40Enabled)
+		pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_WEP64;
+	else if (pAd->StaCfg.GroupCipher == Ndis802_11GroupWEP104Enabled)
+		pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_WEP128;
+
+	return TRUE;
+
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Cisco CCKM PRF function
+
+	Arguments:
+		key				Cisco Base Transient Key (BTK)
+		key_len			The key length of the BTK
+		data			Ruquest Number(RN) + BSSID
+		data_len		The length of the data
+		output			Store for PTK(Pairwise transient keys)
+		len				The length of the output
+	Return Value:
+		None
+
+	Note:
+		802.1i	Annex F.9
+
+	========================================================================
+*/
+VOID CCKMPRF(
+	IN	UCHAR	*key,
+	IN	INT		key_len,
+	IN	UCHAR	*data,
+	IN	INT		data_len,
+	OUT	UCHAR	*output,
+	IN	INT		len)
+{
+	INT		i;
+	UCHAR	input[1024];
+	INT		currentindex = 0;
+	INT		total_len;
+
+	NdisMoveMemory(input, data, data_len);
+	total_len = data_len;
+	input[total_len] = 0;
+	total_len++;
+	for	(i = 0;	i <	(len + 19) / 20; i++)
+	{
+		HMAC_SHA1(input, total_len,	key, key_len, &output[currentindex]);
+		currentindex +=	20;
+		input[total_len - 1]++;
+	}
+}
+
+/*
+	========================================================================
+
+	Routine Description:
+		Process MIC error indication and record MIC error timer.
+
+	Arguments:
+		pAd 	Pointer to our adapter
+		pWpaKey 		Pointer to the WPA key structure
+
+	Return Value:
+		None
+
+	IRQL = DISPATCH_LEVEL
+
+	Note:
+
+	========================================================================
+*/
+VOID	RTMPReportMicError(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PCIPHER_KEY 	pWpaKey)
+{
+	ULONG	Now;
+    UCHAR   unicastKey = (pWpaKey->Type == PAIRWISE_KEY ? 1:0);
+
+	// Record Last MIC error time and count
+	Now = jiffies;
+	if (pAd->StaCfg.MicErrCnt == 0)
+	{
+		pAd->StaCfg.MicErrCnt++;
+		pAd->StaCfg.LastMicErrorTime = Now;
+        NdisZeroMemory(pAd->StaCfg.ReplayCounter, 8);
+	}
+	else if (pAd->StaCfg.MicErrCnt == 1)
+	{
+		if ((pAd->StaCfg.LastMicErrorTime + (60 * OS_HZ)) < Now)
+		{
+			// Update Last MIC error time, this did not violate two MIC errors within 60 seconds
+			pAd->StaCfg.LastMicErrorTime = Now;
+		}
+		else
+		{
+
+			if (pAd->CommonCfg.bWirelessEvent)
+				RTMPSendWirelessEvent(pAd, IW_COUNTER_MEASURES_EVENT_FLAG, pAd->MacTab.Content[BSSID_WCID].Addr, BSS0, 0);
+
+			pAd->StaCfg.LastMicErrorTime = Now;
+			// Violate MIC error counts, MIC countermeasures kicks in
+			pAd->StaCfg.MicErrCnt++;
+			// We shall block all reception
+			// We shall clean all Tx ring and disassoicate from AP after next EAPOL frame
+			//
+			// No necessary to clean all Tx ring, on RTMPHardTransmit will stop sending non-802.1X EAPOL packets
+			// if pAd->StaCfg.MicErrCnt greater than 2.
+			//
+			// RTMPRingCleanUp(pAd, QID_AC_BK);
+			// RTMPRingCleanUp(pAd, QID_AC_BE);
+			// RTMPRingCleanUp(pAd, QID_AC_VI);
+			// RTMPRingCleanUp(pAd, QID_AC_VO);
+			// RTMPRingCleanUp(pAd, QID_HCCA);
+		}
+	}
+	else
+	{
+		// MIC error count >= 2
+		// This should not happen
+		;
+	}
+    MlmeEnqueue(pAd,
+				MLME_CNTL_STATE_MACHINE,
+				OID_802_11_MIC_FAILURE_REPORT_FRAME,
+				1,
+				&unicastKey);
+
+    if (pAd->StaCfg.MicErrCnt == 2)
+    {
+        RTMPSetTimer(&pAd->StaCfg.WpaDisassocAndBlockAssocTimer, 100);
+    }
+}
+
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+#define	LENGTH_EAP_H    4
+// If the received frame is EAP-Packet ,find out its EAP-Code (Request(0x01), Response(0x02), Success(0x03), Failure(0x04)).
+INT	    WpaCheckEapCode(
+	IN  PRTMP_ADAPTER   		pAd,
+	IN  PUCHAR				pFrame,
+	IN  USHORT				FrameLen,
+	IN  USHORT				OffSet)
+{
+
+	PUCHAR	pData;
+	INT	result = 0;
+
+	if( FrameLen < OffSet + LENGTH_EAPOL_H + LENGTH_EAP_H )
+		return result;
+
+	pData = pFrame + OffSet; // skip offset bytes
+
+	if(*(pData+1) == EAPPacket) 	// 802.1x header - Packet Type
+	{
+		 result = *(pData+4);		// EAP header - Code
+	}
+
+	return result;
+}
+
+VOID    WpaSendMicFailureToWpaSupplicant(
+    IN  PRTMP_ADAPTER    pAd,
+    IN  BOOLEAN          bUnicast)
+{
+    union iwreq_data    wrqu;
+    char custom[IW_CUSTOM_MAX] = {0};
+
+    sprintf(custom, "MLME-MICHAELMICFAILURE.indication");
+    if (bUnicast)
+        sprintf(custom, "%s unicast", custom);
+    wrqu.data.length = strlen(custom);
+    wireless_send_event(pAd->net_dev, IWEVCUSTOM, &wrqu, custom);
+
+    return;
+}
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+VOID	WpaMicFailureReportFrame(
+	IN  PRTMP_ADAPTER   pAd,
+	IN MLME_QUEUE_ELEM *Elem)
+{
+	PUCHAR              pOutBuffer = NULL;
+	UCHAR               Header802_3[14];
+	ULONG               FrameLen = 0;
+	EAPOL_PACKET        Packet;
+	UCHAR               Mic[16];
+    BOOLEAN             bUnicast;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("WpaMicFailureReportFrame ----->\n"));
+
+    bUnicast = (Elem->Msg[0] == 1 ? TRUE:FALSE);
+	pAd->Sequence = ((pAd->Sequence) + 1) & (MAX_SEQ_NUMBER);
+
+	// init 802.3 header and Fill Packet
+	MAKE_802_3_HEADER(Header802_3, pAd->CommonCfg.Bssid, pAd->CurrentAddress, EAPOL);
+
+	NdisZeroMemory(&Packet, sizeof(Packet));
+	Packet.ProVer	= EAPOL_VER;
+	Packet.ProType	= EAPOLKey;
+
+	Packet.KeyDesc.Type = WPA1_KEY_DESC;
+
+    // Request field presented
+    Packet.KeyDesc.KeyInfo.Request = 1;
+
+	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
+	{
+		Packet.KeyDesc.KeyInfo.KeyDescVer = 2;
+	}
+	else	  // TKIP
+	{
+		Packet.KeyDesc.KeyInfo.KeyDescVer = 1;
+	}
+
+    Packet.KeyDesc.KeyInfo.KeyType = (bUnicast ? PAIRWISEKEY : GROUPKEY);
+
+	// KeyMic field presented
+	Packet.KeyDesc.KeyInfo.KeyMic  = 1;
+
+    // Error field presented
+	Packet.KeyDesc.KeyInfo.Error  = 1;
+
+	// Update packet length after decide Key data payload
+	Packet.Body_Len[1]  = sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE;
+
+	// Key Replay Count
+	NdisMoveMemory(Packet.KeyDesc.ReplayCounter, pAd->StaCfg.ReplayCounter, LEN_KEY_DESC_REPLAY);
+    inc_byte_array(pAd->StaCfg.ReplayCounter, 8);
+
+	// Convert to little-endian format.
+	*((USHORT *)&Packet.KeyDesc.KeyInfo) = cpu2le16(*((USHORT *)&Packet.KeyDesc.KeyInfo));
+
+
+	MlmeAllocateMemory(pAd, (PUCHAR *)&pOutBuffer);  // allocate memory
+	if(pOutBuffer == NULL)
+	{
+		return;
+	}
+
+	// Prepare EAPOL frame for MIC calculation
+	// Be careful, only EAPOL frame is counted for MIC calculation
+	MakeOutgoingFrame(pOutBuffer,               &FrameLen,
+		              Packet.Body_Len[1] + 4,   &Packet,
+		              END_OF_ARGS);
+
+	// Prepare and Fill MIC value
+	NdisZeroMemory(Mic, sizeof(Mic));
+	if(pAd->StaCfg.WepStatus  == Ndis802_11Encryption3Enabled)
+	{	// AES
+        UCHAR digest[20] = {0};
+		HMAC_SHA1(pOutBuffer, FrameLen, pAd->StaCfg.PTK, LEN_EAP_MICK, digest);
+		NdisMoveMemory(Mic, digest, LEN_KEY_DESC_MIC);
+	}
+	else
+	{	// TKIP
+		hmac_md5(pAd->StaCfg.PTK,  LEN_EAP_MICK, pOutBuffer, FrameLen, Mic);
+	}
+	NdisMoveMemory(Packet.KeyDesc.KeyMic, Mic, LEN_KEY_DESC_MIC);
+
+    MakeOutgoingFrame(pOutBuffer,           	&FrameLen,
+    	  			LENGTH_802_3,     			&Header802_3,
+    				Packet.Body_Len[1] + 4,     &Packet,
+    				END_OF_ARGS);
+
+	// opy frame to Tx ring and send MIC failure report frame to authenticator
+	RTMPToWirelessSta(pAd, Header802_3, LENGTH_802_3, (PUCHAR)&Packet, Packet.Body_Len[1] + 4, FALSE);
+
+	MlmeFreeMemory(pAd, (PUCHAR)pOutBuffer);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("WpaMicFailureReportFrame <-----\n"));
+}
+
+/** from wpa_supplicant
+ * inc_byte_array - Increment arbitrary length byte array by one
+ * @counter: Pointer to byte array
+ * @len: Length of the counter in bytes
+ *
+ * This function increments the last byte of the counter by one and continues
+ * rolling over to more significant bytes if the byte was incremented from
+ * 0xff to 0x00.
+ */
+void inc_byte_array(UCHAR *counter, int len)
+{
+	int pos = len - 1;
+	while (pos >= 0) {
+		counter[pos]++;
+		if (counter[pos] != 0)
+			break;
+		pos--;
+	}
+}
+
+VOID WpaDisassocApAndBlockAssoc(
+    IN PVOID SystemSpecific1,
+    IN PVOID FunctionContext,
+    IN PVOID SystemSpecific2,
+    IN PVOID SystemSpecific3)
+{
+    RTMP_ADAPTER                *pAd = (PRTMP_ADAPTER)FunctionContext;
+    MLME_DISASSOC_REQ_STRUCT    DisassocReq;
+
+	// disassoc from current AP first
+	DBGPRINT(RT_DEBUG_TRACE, ("RTMPReportMicError - disassociate with current AP after sending second continuous EAPOL frame\n"));
+	DisassocParmFill(pAd, &DisassocReq, pAd->CommonCfg.Bssid, REASON_MIC_FAILURE);
+	MlmeEnqueue(pAd, ASSOC_STATE_MACHINE, MT2_MLME_DISASSOC_REQ, sizeof(MLME_DISASSOC_REQ_STRUCT), &DisassocReq);
+
+	pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_DISASSOC;
+	pAd->StaCfg.bBlockAssoc = TRUE;
+}
+
diff --git a/drivers/staging/rt2870/sta_ioctl.c b/drivers/staging/rt2870/sta_ioctl.c
new file mode 100644
index 00000000000..422f39f9004
--- /dev/null
+++ b/drivers/staging/rt2870/sta_ioctl.c
@@ -0,0 +1,7068 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    sta_ioctl.c
+
+    Abstract:
+    IOCTL related subroutines
+
+    Revision History:
+    Who         When          What
+    --------    ----------    ----------------------------------------------
+    Rory Chen   01-03-2003    created
+	Rory Chen   02-14-2005    modify to support RT61
+*/
+
+#include	"rt_config.h"
+
+#ifdef DBG
+extern ULONG    RTDebugLevel;
+#endif
+
+#define NR_WEP_KEYS 				4
+#define WEP_SMALL_KEY_LEN 			(40/8)
+#define WEP_LARGE_KEY_LEN 			(104/8)
+
+#define GROUP_KEY_NO                4
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
+#define IWE_STREAM_ADD_EVENT(_A, _B, _C, _D, _E)		iwe_stream_add_event(_A, _B, _C, _D, _E)
+#define IWE_STREAM_ADD_POINT(_A, _B, _C, _D, _E)		iwe_stream_add_point(_A, _B, _C, _D, _E)
+#define IWE_STREAM_ADD_VALUE(_A, _B, _C, _D, _E, _F)	iwe_stream_add_value(_A, _B, _C, _D, _E, _F)
+#else
+#define IWE_STREAM_ADD_EVENT(_A, _B, _C, _D, _E)		iwe_stream_add_event(_B, _C, _D, _E)
+#define IWE_STREAM_ADD_POINT(_A, _B, _C, _D, _E)		iwe_stream_add_point(_B, _C, _D, _E)
+#define IWE_STREAM_ADD_VALUE(_A, _B, _C, _D, _E, _F)	iwe_stream_add_value(_B, _C, _D, _E, _F)
+#endif
+
+extern UCHAR    CipherWpa2Template[];
+extern UCHAR    CipherWpaPskTkip[];
+extern UCHAR    CipherWpaPskTkipLen;
+
+typedef struct PACKED _RT_VERSION_INFO{
+    UCHAR       DriverVersionW;
+    UCHAR       DriverVersionX;
+    UCHAR       DriverVersionY;
+    UCHAR       DriverVersionZ;
+    UINT        DriverBuildYear;
+    UINT        DriverBuildMonth;
+    UINT        DriverBuildDay;
+} RT_VERSION_INFO, *PRT_VERSION_INFO;
+
+struct iw_priv_args privtab[] = {
+{ RTPRIV_IOCTL_SET,
+  IW_PRIV_TYPE_CHAR | 1024, 0,
+  "set"},
+
+{ RTPRIV_IOCTL_SHOW, 0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK,
+  ""},
+{ RTPRIV_IOCTL_SHOW, IW_PRIV_TYPE_CHAR | 1024, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK,
+  ""},
+/* --- sub-ioctls definitions --- */
+    { SHOW_CONN_STATUS,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "connStatus" },
+	{ SHOW_DRVIER_VERION,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "driverVer" },
+    { SHOW_BA_INFO,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "bainfo" },
+	{ SHOW_DESC_INFO,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "descinfo" },
+    { RAIO_OFF,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "radio_off" },
+	{ RAIO_ON,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "radio_on" },
+#ifdef QOS_DLS_SUPPORT
+	{ SHOW_DLS_ENTRY_INFO,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "dlsentryinfo" },
+#endif // QOS_DLS_SUPPORT //
+	{ SHOW_CFG_VALUE,
+	  IW_PRIV_TYPE_CHAR | 1024, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "show" },
+	{ SHOW_ADHOC_ENTRY_INFO,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "adhocEntry" },
+
+/* --- sub-ioctls relations --- */
+
+#ifdef DBG
+{ RTPRIV_IOCTL_BBP,
+  IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK,
+  "bbp"},
+{ RTPRIV_IOCTL_MAC,
+  IW_PRIV_TYPE_CHAR | 1024, IW_PRIV_TYPE_CHAR | 1024,
+  "mac"},
+{ RTPRIV_IOCTL_E2P,
+  IW_PRIV_TYPE_CHAR | 1024, IW_PRIV_TYPE_CHAR | 1024,
+  "e2p"},
+#endif  /* DBG */
+
+{ RTPRIV_IOCTL_STATISTICS,
+  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK,
+  "stat"},
+{ RTPRIV_IOCTL_GSITESURVEY,
+  0, IW_PRIV_TYPE_CHAR | 1024,
+  "get_site_survey"},
+};
+
+INT Set_SSID_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+#ifdef WMM_SUPPORT
+INT	Set_WmmCapable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif
+
+INT Set_NetworkType_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_AuthMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_EncrypType_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_DefaultKeyID_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_Key1_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_Key2_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_Key3_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_Key4_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_WPAPSK_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+
+INT Set_PSMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+INT Set_Wpa_Support(
+    IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef DBG
+VOID RTMPIoctlBBP(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq);
+
+VOID RTMPIoctlMAC(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq);
+
+VOID RTMPIoctlE2PROM(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  struct iwreq    *wrq);
+#endif // DBG //
+
+
+NDIS_STATUS RTMPWPANoneAddKeyProc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN	PVOID			pBuf);
+
+INT Set_FragTest_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+#ifdef DOT11_N_SUPPORT
+INT Set_TGnWifiTest_Proc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          arg);
+#endif // DOT11_N_SUPPORT //
+
+INT Set_LongRetryLimit_Proc(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PUCHAR			arg);
+
+INT Set_ShortRetryLimit_Proc(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PUCHAR			arg);
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+INT Set_Ieee80211dClientMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+#ifdef CARRIER_DETECTION_SUPPORT
+INT Set_CarrierDetect_Proc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          arg);
+#endif // CARRIER_DETECTION_SUPPORT //
+
+INT	Show_Adhoc_MacTable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PCHAR			extra);
+
+static struct {
+	CHAR *name;
+	INT (*set_proc)(PRTMP_ADAPTER pAdapter, PUCHAR arg);
+} *PRTMP_PRIVATE_SET_PROC, RTMP_PRIVATE_SUPPORT_PROC[] = {
+	{"DriverVersion",				Set_DriverVersion_Proc},
+	{"CountryRegion",				Set_CountryRegion_Proc},
+	{"CountryRegionABand",			Set_CountryRegionABand_Proc},
+	{"SSID",						Set_SSID_Proc},
+	{"WirelessMode",				Set_WirelessMode_Proc},
+	{"TxBurst",					Set_TxBurst_Proc},
+	{"TxPreamble",				Set_TxPreamble_Proc},
+	{"TxPower",					Set_TxPower_Proc},
+	{"Channel",					Set_Channel_Proc},
+	{"BGProtection",				Set_BGProtection_Proc},
+	{"RTSThreshold",				Set_RTSThreshold_Proc},
+	{"FragThreshold",				Set_FragThreshold_Proc},
+#ifdef DOT11_N_SUPPORT
+	{"HtBw",		                Set_HtBw_Proc},
+	{"HtMcs",		                Set_HtMcs_Proc},
+	{"HtGi",		                Set_HtGi_Proc},
+	{"HtOpMode",		            Set_HtOpMode_Proc},
+	{"HtExtcha",		            Set_HtExtcha_Proc},
+	{"HtMpduDensity",		        Set_HtMpduDensity_Proc},
+	{"HtBaWinSize",		        	Set_HtBaWinSize_Proc},
+	{"HtRdg",		        		Set_HtRdg_Proc},
+	{"HtAmsdu",		        		Set_HtAmsdu_Proc},
+	{"HtAutoBa",		        	Set_HtAutoBa_Proc},
+	{"HtBaDecline",					Set_BADecline_Proc},
+	{"HtProtect",		        	Set_HtProtect_Proc},
+	{"HtMimoPs",		        	Set_HtMimoPs_Proc},
+#endif // DOT11_N_SUPPORT //
+
+#ifdef AGGREGATION_SUPPORT
+	{"PktAggregate",				Set_PktAggregate_Proc},
+#endif
+
+#ifdef WMM_SUPPORT
+	{"WmmCapable",					Set_WmmCapable_Proc},
+#endif
+	{"IEEE80211H",					Set_IEEE80211H_Proc},
+    {"NetworkType",                 Set_NetworkType_Proc},
+	{"AuthMode",					Set_AuthMode_Proc},
+	{"EncrypType",					Set_EncrypType_Proc},
+	{"DefaultKeyID",				Set_DefaultKeyID_Proc},
+	{"Key1",						Set_Key1_Proc},
+	{"Key2",						Set_Key2_Proc},
+	{"Key3",						Set_Key3_Proc},
+	{"Key4",						Set_Key4_Proc},
+	{"WPAPSK",						Set_WPAPSK_Proc},
+	{"ResetCounter",				Set_ResetStatCounter_Proc},
+	{"PSMode",                      Set_PSMode_Proc},
+#ifdef DBG
+	{"Debug",						Set_Debug_Proc},
+#endif
+
+#ifdef RALINK_ATE
+	{"ATE",							Set_ATE_Proc},
+	{"ATEDA",						Set_ATE_DA_Proc},
+	{"ATESA",						Set_ATE_SA_Proc},
+	{"ATEBSSID",					Set_ATE_BSSID_Proc},
+	{"ATECHANNEL",					Set_ATE_CHANNEL_Proc},
+	{"ATETXPOW0",					Set_ATE_TX_POWER0_Proc},
+	{"ATETXPOW1",					Set_ATE_TX_POWER1_Proc},
+	{"ATETXANT",					Set_ATE_TX_Antenna_Proc},
+	{"ATERXANT",					Set_ATE_RX_Antenna_Proc},
+	{"ATETXFREQOFFSET",				Set_ATE_TX_FREQOFFSET_Proc},
+	{"ATETXBW",						Set_ATE_TX_BW_Proc},
+	{"ATETXLEN",					Set_ATE_TX_LENGTH_Proc},
+	{"ATETXCNT",					Set_ATE_TX_COUNT_Proc},
+	{"ATETXMCS",					Set_ATE_TX_MCS_Proc},
+	{"ATETXMODE",					Set_ATE_TX_MODE_Proc},
+	{"ATETXGI",						Set_ATE_TX_GI_Proc},
+	{"ATERXFER",					Set_ATE_RX_FER_Proc},
+	{"ATERRF",						Set_ATE_Read_RF_Proc},
+	{"ATEWRF1",						Set_ATE_Write_RF1_Proc},
+	{"ATEWRF2",						Set_ATE_Write_RF2_Proc},
+	{"ATEWRF3",						Set_ATE_Write_RF3_Proc},
+	{"ATEWRF4",						Set_ATE_Write_RF4_Proc},
+	{"ATELDE2P",				    Set_ATE_Load_E2P_Proc},
+	{"ATERE2P",						Set_ATE_Read_E2P_Proc},
+	{"ATESHOW",						Set_ATE_Show_Proc},
+	{"ATEHELP",						Set_ATE_Help_Proc},
+
+#ifdef RALINK_28xx_QA
+	{"TxStop",						Set_TxStop_Proc},
+	{"RxStop",						Set_RxStop_Proc},
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+    {"WpaSupport",                  Set_Wpa_Support},
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+
+
+	{"FixedTxMode",                 Set_FixedTxMode_Proc},
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+	{"OpMode",						Set_OpMode_Proc},
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+#ifdef DOT11_N_SUPPORT
+    {"TGnWifiTest",                 Set_TGnWifiTest_Proc},
+    {"ForceGF",		        		Set_ForceGF_Proc},
+#endif // DOT11_N_SUPPORT //
+#ifdef QOS_DLS_SUPPORT
+	{"DlsAddEntry",					Set_DlsAddEntry_Proc},
+	{"DlsTearDownEntry",			Set_DlsTearDownEntry_Proc},
+#endif // QOS_DLS_SUPPORT //
+	{"LongRetry",	        		Set_LongRetryLimit_Proc},
+	{"ShortRetry",	        		Set_ShortRetryLimit_Proc},
+#ifdef EXT_BUILD_CHANNEL_LIST
+	{"11dClientMode",				Set_Ieee80211dClientMode_Proc},
+#endif // EXT_BUILD_CHANNEL_LIST //
+#ifdef CARRIER_DETECTION_SUPPORT
+	{"CarrierDetect",				Set_CarrierDetect_Proc},
+#endif // CARRIER_DETECTION_SUPPORT //
+
+	{NULL,}
+};
+
+
+VOID RTMPAddKey(
+	IN	PRTMP_ADAPTER	    pAd,
+	IN	PNDIS_802_11_KEY    pKey)
+{
+	ULONG				KeyIdx;
+	MAC_TABLE_ENTRY  	*pEntry;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("RTMPAddKey ------>\n"));
+
+	if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+	{
+		if (pKey->KeyIndex & 0x80000000)
+		{
+		    if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+            {
+                NdisZeroMemory(pAd->StaCfg.PMK, 32);
+                NdisMoveMemory(pAd->StaCfg.PMK, pKey->KeyMaterial, pKey->KeyLength);
+                goto end;
+            }
+		    // Update PTK
+		    NdisZeroMemory(&pAd->SharedKey[BSS0][0], sizeof(CIPHER_KEY));
+            pAd->SharedKey[BSS0][0].KeyLen = LEN_TKIP_EK;
+            NdisMoveMemory(pAd->SharedKey[BSS0][0].Key, pKey->KeyMaterial, LEN_TKIP_EK);
+#ifdef WPA_SUPPLICANT_SUPPORT
+            if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
+            {
+                NdisMoveMemory(pAd->SharedKey[BSS0][0].RxMic, pKey->KeyMaterial + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+                NdisMoveMemory(pAd->SharedKey[BSS0][0].TxMic, pKey->KeyMaterial + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+            }
+            else
+#endif // WPA_SUPPLICANT_SUPPORT //
+            {
+            	NdisMoveMemory(pAd->SharedKey[BSS0][0].TxMic, pKey->KeyMaterial + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+                NdisMoveMemory(pAd->SharedKey[BSS0][0].RxMic, pKey->KeyMaterial + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+            }
+
+            // Decide its ChiperAlg
+        	if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
+        		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_TKIP;
+        	else if (pAd->StaCfg.PairCipher == Ndis802_11Encryption3Enabled)
+        		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_AES;
+        	else
+        		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_NONE;
+
+            // Update these related information to MAC_TABLE_ENTRY
+        	pEntry = &pAd->MacTab.Content[BSSID_WCID];
+            NdisMoveMemory(pEntry->PairwiseKey.Key, pAd->SharedKey[BSS0][0].Key, LEN_TKIP_EK);
+        	NdisMoveMemory(pEntry->PairwiseKey.RxMic, pAd->SharedKey[BSS0][0].RxMic, LEN_TKIP_RXMICK);
+        	NdisMoveMemory(pEntry->PairwiseKey.TxMic, pAd->SharedKey[BSS0][0].TxMic, LEN_TKIP_TXMICK);
+        	pEntry->PairwiseKey.CipherAlg = pAd->SharedKey[BSS0][0].CipherAlg;
+
+        	// Update pairwise key information to ASIC Shared Key Table
+        	AsicAddSharedKeyEntry(pAd,
+        						  BSS0,
+        						  0,
+        						  pAd->SharedKey[BSS0][0].CipherAlg,
+        						  pAd->SharedKey[BSS0][0].Key,
+        						  pAd->SharedKey[BSS0][0].TxMic,
+        						  pAd->SharedKey[BSS0][0].RxMic);
+
+        	// Update ASIC WCID attribute table and IVEIV table
+        	RTMPAddWcidAttributeEntry(pAd,
+        							  BSS0,
+        							  0,
+        							  pAd->SharedKey[BSS0][0].CipherAlg,
+        							  pEntry);
+
+            if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA2)
+            {
+                // set 802.1x port control
+	            //pAd->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+				STA_PORT_SECURED(pAd);
+
+                // Indicate Connected for GUI
+                pAd->IndicateMediaState = NdisMediaStateConnected;
+            }
+		}
+        else
+        {
+            // Update GTK
+            pAd->StaCfg.DefaultKeyId = (pKey->KeyIndex & 0xFF);
+            NdisZeroMemory(&pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId], sizeof(CIPHER_KEY));
+            pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].KeyLen = LEN_TKIP_EK;
+            NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key, pKey->KeyMaterial, LEN_TKIP_EK);
+#ifdef WPA_SUPPLICANT_SUPPORT
+            if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption2Enabled)
+            {
+                NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic, pKey->KeyMaterial + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+                NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic, pKey->KeyMaterial + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+            }
+            else
+#endif // WPA_SUPPLICANT_SUPPORT //
+            {
+            	NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic, pKey->KeyMaterial + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+                NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic, pKey->KeyMaterial + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+            }
+
+            // Update Shared Key CipherAlg
+    		pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_NONE;
+    		if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption2Enabled)
+    			pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_TKIP;
+    		else if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption3Enabled)
+    			pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_AES;
+
+            // Update group key information to ASIC Shared Key Table
+        	AsicAddSharedKeyEntry(pAd,
+        						  BSS0,
+        						  pAd->StaCfg.DefaultKeyId,
+        						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
+        						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key,
+        						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic,
+        						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic);
+
+        	// Update ASIC WCID attribute table and IVEIV table
+        	RTMPAddWcidAttributeEntry(pAd,
+        							  BSS0,
+        							  pAd->StaCfg.DefaultKeyId,
+        							  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
+        							  NULL);
+
+            // set 802.1x port control
+	        //pAd->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+			STA_PORT_SECURED(pAd);
+
+            // Indicate Connected for GUI
+            pAd->IndicateMediaState = NdisMediaStateConnected;
+        }
+	}
+	else	// dynamic WEP from wpa_supplicant
+	{
+		UCHAR	CipherAlg;
+    	PUCHAR	Key;
+
+		if(pKey->KeyLength == 32)
+			goto end;
+
+		KeyIdx = pKey->KeyIndex & 0x0fffffff;
+
+		if (KeyIdx < 4)
+		{
+			// it is a default shared key, for Pairwise key setting
+			if (pKey->KeyIndex & 0x80000000)
+			{
+				pEntry = MacTableLookup(pAd, pKey->BSSID);
+
+				if (pEntry)
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("RTMPAddKey: Set Pair-wise Key\n"));
+
+					// set key material and key length
+ 					pEntry->PairwiseKey.KeyLen = (UCHAR)pKey->KeyLength;
+					NdisMoveMemory(pEntry->PairwiseKey.Key, &pKey->KeyMaterial, pKey->KeyLength);
+
+					// set Cipher type
+					if (pKey->KeyLength == 5)
+						pEntry->PairwiseKey.CipherAlg = CIPHER_WEP64;
+					else
+						pEntry->PairwiseKey.CipherAlg = CIPHER_WEP128;
+
+					// Add Pair-wise key to Asic
+					AsicAddPairwiseKeyEntry(
+						pAd,
+						pEntry->Addr,
+						(UCHAR)pEntry->Aid,
+                		&pEntry->PairwiseKey);
+
+					// update WCID attribute table and IVEIV table for this entry
+					RTMPAddWcidAttributeEntry(
+						pAd,
+						BSS0,
+						KeyIdx, // The value may be not zero
+						pEntry->PairwiseKey.CipherAlg,
+						pEntry);
+
+				}
+			}
+			else
+            {
+				// Default key for tx (shared key)
+				pAd->StaCfg.DefaultKeyId = (UCHAR) KeyIdx;
+
+				// set key material and key length
+				pAd->SharedKey[BSS0][KeyIdx].KeyLen = (UCHAR) pKey->KeyLength;
+				NdisMoveMemory(pAd->SharedKey[BSS0][KeyIdx].Key, &pKey->KeyMaterial, pKey->KeyLength);
+
+				// Set Ciper type
+				if (pKey->KeyLength == 5)
+					pAd->SharedKey[BSS0][KeyIdx].CipherAlg = CIPHER_WEP64;
+				else
+					pAd->SharedKey[BSS0][KeyIdx].CipherAlg = CIPHER_WEP128;
+
+    			CipherAlg = pAd->SharedKey[BSS0][KeyIdx].CipherAlg;
+    			Key = pAd->SharedKey[BSS0][KeyIdx].Key;
+
+				// Set Group key material to Asic
+    			AsicAddSharedKeyEntry(pAd, BSS0, KeyIdx, CipherAlg, Key, NULL, NULL);
+
+				// Update WCID attribute table and IVEIV table for this group key table
+				RTMPAddWcidAttributeEntry(pAd, BSS0, KeyIdx, CipherAlg, NULL);
+
+			}
+		}
+	}
+end:
+	return;
+}
+
+char * rtstrchr(const char * s, int c)
+{
+    for(; *s != (char) c; ++s)
+        if (*s == '\0')
+            return NULL;
+    return (char *) s;
+}
+
+/*
+This is required for LinEX2004/kernel2.6.7 to provide iwlist scanning function
+*/
+
+int
+rt_ioctl_giwname(struct net_device *dev,
+		   struct iw_request_info *info,
+		   char *name, char *extra)
+{
+//	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+
+#ifdef RT2870
+	strncpy(name, "RT2870 Wireless", IFNAMSIZ);
+#endif // RT2870 //
+	return 0;
+}
+
+int rt_ioctl_siwfreq(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_freq *freq, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+	int 	chan = -1;
+
+    //check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        return -ENETDOWN;
+    }
+
+
+	if (freq->e > 1)
+		return -EINVAL;
+
+	if((freq->e == 0) && (freq->m <= 1000))
+		chan = freq->m;	// Setting by channel number
+	else
+		MAP_KHZ_TO_CHANNEL_ID( (freq->m /100) , chan); // Setting by frequency - search the table , like 2.412G, 2.422G,
+
+    if (ChannelSanity(pAdapter, chan) == TRUE)
+    {
+	pAdapter->CommonCfg.Channel = chan;
+	DBGPRINT(RT_DEBUG_ERROR, ("==>rt_ioctl_siwfreq::SIOCSIWFREQ[cmd=0x%x] (Channel=%d)\n", SIOCSIWFREQ, pAdapter->CommonCfg.Channel));
+    }
+    else
+        return -EINVAL;
+
+	return 0;
+}
+int rt_ioctl_giwfreq(struct net_device *dev,
+		   struct iw_request_info *info,
+		   struct iw_freq *freq, char *extra)
+{
+    VIRTUAL_ADAPTER *pVirtualAd = NULL;
+	PRTMP_ADAPTER pAdapter = NULL;
+	UCHAR ch;
+	ULONG	m;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+		ch = pAdapter->CommonCfg.Channel;
+
+	DBGPRINT(RT_DEBUG_TRACE,("==>rt_ioctl_giwfreq  %d\n", ch));
+
+    MAP_CHANNEL_ID_TO_KHZ(ch, m);
+	freq->m = m * 100;
+	freq->e = 1;
+	return 0;
+}
+
+int rt_ioctl_siwmode(struct net_device *dev,
+		   struct iw_request_info *info,
+		   __u32 *mode, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+
+	//check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+    	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+       	return -ENETDOWN;
+    }
+
+	switch (*mode)
+	{
+		case IW_MODE_ADHOC:
+			Set_NetworkType_Proc(pAdapter, "Adhoc");
+			break;
+		case IW_MODE_INFRA:
+			Set_NetworkType_Proc(pAdapter, "Infra");
+			break;
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,4,20))
+        case IW_MODE_MONITOR:
+			Set_NetworkType_Proc(pAdapter, "Monitor");
+			break;
+#endif
+		default:
+			DBGPRINT(RT_DEBUG_TRACE, ("===>rt_ioctl_siwmode::SIOCSIWMODE (unknown %d)\n", *mode));
+			return -EINVAL;
+	}
+
+	// Reset Ralink supplicant to not use, it will be set to start when UI set PMK key
+	pAdapter->StaCfg.WpaState = SS_NOTUSE;
+
+	return 0;
+}
+
+int rt_ioctl_giwmode(struct net_device *dev,
+		   struct iw_request_info *info,
+		   __u32 *mode, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	if (ADHOC_ON(pAdapter))
+		*mode = IW_MODE_ADHOC;
+    else if (INFRA_ON(pAdapter))
+		*mode = IW_MODE_INFRA;
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,4,20))
+    else if (MONITOR_ON(pAdapter))
+    {
+        *mode = IW_MODE_MONITOR;
+    }
+#endif
+    else
+        *mode = IW_MODE_AUTO;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("==>rt_ioctl_giwmode(mode=%d)\n", *mode));
+	return 0;
+}
+
+int rt_ioctl_siwsens(struct net_device *dev,
+		   struct iw_request_info *info,
+		   char *name, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+
+	//check if the interface is down
+    	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    	{
+        	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        	return -ENETDOWN;
+    	}
+
+	return 0;
+}
+
+int rt_ioctl_giwsens(struct net_device *dev,
+		   struct iw_request_info *info,
+		   char *name, char *extra)
+{
+	return 0;
+}
+
+int rt_ioctl_giwrange(struct net_device *dev,
+		   struct iw_request_info *info,
+		   struct iw_point *data, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+	struct iw_range *range = (struct iw_range *) extra;
+	u16 val;
+	int i;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE ,("===>rt_ioctl_giwrange\n"));
+	data->length = sizeof(struct iw_range);
+	memset(range, 0, sizeof(struct iw_range));
+
+	range->txpower_capa = IW_TXPOW_DBM;
+
+	if (INFRA_ON(pAdapter)||ADHOC_ON(pAdapter))
+	{
+		range->min_pmp = 1 * 1024;
+		range->max_pmp = 65535 * 1024;
+		range->min_pmt = 1 * 1024;
+		range->max_pmt = 1000 * 1024;
+		range->pmp_flags = IW_POWER_PERIOD;
+		range->pmt_flags = IW_POWER_TIMEOUT;
+		range->pm_capa = IW_POWER_PERIOD | IW_POWER_TIMEOUT |
+			IW_POWER_UNICAST_R | IW_POWER_ALL_R;
+	}
+
+	range->we_version_compiled = WIRELESS_EXT;
+	range->we_version_source = 14;
+
+	range->retry_capa = IW_RETRY_LIMIT;
+	range->retry_flags = IW_RETRY_LIMIT;
+	range->min_retry = 0;
+	range->max_retry = 255;
+
+	range->num_channels =  pAdapter->ChannelListNum;
+
+	val = 0;
+	for (i = 1; i <= range->num_channels; i++)
+	{
+		u32 m;
+		range->freq[val].i = pAdapter->ChannelList[i-1].Channel;
+		MAP_CHANNEL_ID_TO_KHZ(pAdapter->ChannelList[i-1].Channel, m);
+		range->freq[val].m = m * 100; /* HZ */
+
+		range->freq[val].e = 1;
+		val++;
+		if (val == IW_MAX_FREQUENCIES)
+			break;
+	}
+	range->num_frequency = val;
+
+	range->max_qual.qual = 100; /* what is correct max? This was not
+					* documented exactly. At least
+					* 69 has been observed. */
+	range->max_qual.level = 0; /* dB */
+	range->max_qual.noise = 0; /* dB */
+
+	/* What would be suitable values for "average/typical" qual? */
+	range->avg_qual.qual = 20;
+	range->avg_qual.level = -60;
+	range->avg_qual.noise = -95;
+	range->sensitivity = 3;
+
+	range->max_encoding_tokens = NR_WEP_KEYS;
+	range->num_encoding_sizes = 2;
+	range->encoding_size[0] = 5;
+	range->encoding_size[1] = 13;
+
+	range->min_rts = 0;
+	range->max_rts = 2347;
+	range->min_frag = 256;
+	range->max_frag = 2346;
+
+#if WIRELESS_EXT > 17
+	/* IW_ENC_CAPA_* bit field */
+	range->enc_capa = IW_ENC_CAPA_WPA | IW_ENC_CAPA_WPA2 |
+					IW_ENC_CAPA_CIPHER_TKIP | IW_ENC_CAPA_CIPHER_CCMP;
+#endif
+
+	return 0;
+}
+
+int rt_ioctl_siwap(struct net_device *dev,
+		      struct iw_request_info *info,
+		      struct sockaddr *ap_addr, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+    NDIS_802_11_MAC_ADDRESS Bssid;
+
+	//check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+       	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+       	return -ENETDOWN;
+    }
+
+	if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+    {
+        RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+        DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+    }
+
+    // tell CNTL state machine to call NdisMSetInformationComplete() after completing
+    // this request, because this request is initiated by NDIS.
+    pAdapter->MlmeAux.CurrReqIsFromNdis = FALSE;
+	// Prevent to connect AP again in STAMlmePeriodicExec
+	pAdapter->MlmeAux.AutoReconnectSsidLen= 32;
+
+    memset(Bssid, 0, MAC_ADDR_LEN);
+    memcpy(Bssid, ap_addr->sa_data, MAC_ADDR_LEN);
+    MlmeEnqueue(pAdapter,
+                MLME_CNTL_STATE_MACHINE,
+                OID_802_11_BSSID,
+                sizeof(NDIS_802_11_MAC_ADDRESS),
+                (VOID *)&Bssid);
+
+    DBGPRINT(RT_DEBUG_TRACE, ("IOCTL::SIOCSIWAP %02x:%02x:%02x:%02x:%02x:%02x\n",
+        Bssid[0], Bssid[1], Bssid[2], Bssid[3], Bssid[4], Bssid[5]));
+
+	return 0;
+}
+
+int rt_ioctl_giwap(struct net_device *dev,
+		      struct iw_request_info *info,
+		      struct sockaddr *ap_addr, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	if (INFRA_ON(pAdapter) || ADHOC_ON(pAdapter))
+	{
+		ap_addr->sa_family = ARPHRD_ETHER;
+		memcpy(ap_addr->sa_data, &pAdapter->CommonCfg.Bssid, ETH_ALEN);
+	}
+#ifdef WPA_SUPPLICANT_SUPPORT
+    // Add for RT2870
+    else if (pAdapter->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_DISABLE)
+    {
+        ap_addr->sa_family = ARPHRD_ETHER;
+        memcpy(ap_addr->sa_data, &pAdapter->MlmeAux.Bssid, ETH_ALEN);
+    }
+#endif // WPA_SUPPLICANT_SUPPORT //
+	else
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("IOCTL::SIOCGIWAP(=EMPTY)\n"));
+		return -ENOTCONN;
+	}
+
+	return 0;
+}
+
+/*
+ * Units are in db above the noise floor. That means the
+ * rssi values reported in the tx/rx descriptors in the
+ * driver are the SNR expressed in db.
+ *
+ * If you assume that the noise floor is -95, which is an
+ * excellent assumption 99.5 % of the time, then you can
+ * derive the absolute signal level (i.e. -95 + rssi).
+ * There are some other slight factors to take into account
+ * depending on whether the rssi measurement is from 11b,
+ * 11g, or 11a.   These differences are at most 2db and
+ * can be documented.
+ *
+ * NB: various calculations are based on the orinoco/wavelan
+ *     drivers for compatibility
+ */
+static void set_quality(PRTMP_ADAPTER pAdapter,
+                        struct iw_quality *iq,
+                        signed char rssi)
+{
+	__u8 ChannelQuality;
+
+	// Normalize Rssi
+	if (rssi >= -50)
+		ChannelQuality = 100;
+	else if (rssi >= -80) // between -50 ~ -80dbm
+		ChannelQuality = (__u8)(24 + ((rssi + 80) * 26)/10);
+	else if (rssi >= -90)   // between -80 ~ -90dbm
+        ChannelQuality = (__u8)((rssi + 90) * 26)/10;
+	else
+		ChannelQuality = 0;
+
+    iq->qual = (__u8)ChannelQuality;
+
+    iq->level = (__u8)(rssi);
+    iq->noise = (pAdapter->BbpWriteLatch[66] > pAdapter->BbpTuning.FalseCcaUpperThreshold) ? ((__u8)pAdapter->BbpTuning.FalseCcaUpperThreshold) : ((__u8) pAdapter->BbpWriteLatch[66]); 	// noise level (dBm)
+    iq->noise += 256 - 143;
+    iq->updated = pAdapter->iw_stats.qual.updated;
+}
+
+int rt_ioctl_iwaplist(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_point *data, char *extra)
+{
+ 	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+
+	struct sockaddr addr[IW_MAX_AP];
+	struct iw_quality qual[IW_MAX_AP];
+	int i;
+
+   	//check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+       	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+		data->length = 0;
+		return 0;
+        //return -ENETDOWN;
+	}
+
+	for (i = 0; i <IW_MAX_AP ; i++)
+	{
+		if (i >=  pAdapter->ScanTab.BssNr)
+			break;
+		addr[i].sa_family = ARPHRD_ETHER;
+			memcpy(addr[i].sa_data, &pAdapter->ScanTab.BssEntry[i].Bssid, MAC_ADDR_LEN);
+		set_quality(pAdapter, &qual[i], pAdapter->ScanTab.BssEntry[i].Rssi);
+	}
+	data->length = i;
+	memcpy(extra, &addr, i*sizeof(addr[0]));
+	data->flags = 1;		/* signal quality present (sort of) */
+	memcpy(extra + i*sizeof(addr[0]), &qual, i*sizeof(qual[i]));
+
+	return 0;
+}
+
+#ifdef SIOCGIWSCAN
+int rt_ioctl_siwscan(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_point *data, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+
+	ULONG								Now;
+	int Status = NDIS_STATUS_SUCCESS;
+
+	//check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+		return -ENETDOWN;
+	}
+
+	if (MONITOR_ON(pAdapter))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("!!! Driver is in Monitor Mode now !!!\n"));
+        return -EINVAL;
+    }
+
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+	if (pAdapter->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_ENABLE)
+	{
+		pAdapter->StaCfg.WpaSupplicantScanCount++;
+	}
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+    pAdapter->StaCfg.bScanReqIsFromWebUI = TRUE;
+	if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+		return 0;
+	do{
+		Now = jiffies;
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+		if ((pAdapter->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_ENABLE) &&
+			(pAdapter->StaCfg.WpaSupplicantScanCount > 3))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("!!! WpaSupplicantScanCount > 3\n"));
+			Status = NDIS_STATUS_SUCCESS;
+			break;
+		}
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+		if ((OPSTATUS_TEST_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED)) &&
+			((pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
+			(pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK)) &&
+			(pAdapter->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("!!! Link UP, Port Not Secured! ignore this set::OID_802_11_BSSID_LIST_SCAN\n"));
+			Status = NDIS_STATUS_SUCCESS;
+			break;
+		}
+
+		if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+		{
+			RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+			DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+		}
+
+		// tell CNTL state machine to call NdisMSetInformationComplete() after completing
+		// this request, because this request is initiated by NDIS.
+		pAdapter->MlmeAux.CurrReqIsFromNdis = FALSE;
+		// Reset allowed scan retries
+		pAdapter->StaCfg.ScanCnt = 0;
+		pAdapter->StaCfg.LastScanTime = Now;
+
+		MlmeEnqueue(pAdapter,
+			MLME_CNTL_STATE_MACHINE,
+			OID_802_11_BSSID_LIST_SCAN,
+			0,
+			NULL);
+
+		Status = NDIS_STATUS_SUCCESS;
+		RT28XX_MLME_HANDLER(pAdapter);
+	}while(0);
+	return 0;
+}
+
+int rt_ioctl_giwscan(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_point *data, char *extra)
+{
+
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+	int i=0;
+	char *current_ev = extra, *previous_ev = extra;
+	char *end_buf;
+	char *current_val, custom[MAX_CUSTOM_LEN] = {0};
+#ifndef IWEVGENIE
+	char idx;
+#endif // IWEVGENIE //
+	struct iw_event iwe;
+
+	if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+    {
+		/*
+		 * Still scanning, indicate the caller should try again.
+		 */
+		return -EAGAIN;
+	}
+
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+	if (pAdapter->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_ENABLE)
+	{
+		pAdapter->StaCfg.WpaSupplicantScanCount = 0;
+	}
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+	if (pAdapter->ScanTab.BssNr == 0)
+	{
+		data->length = 0;
+		return 0;
+	}
+
+#if WIRELESS_EXT >= 17
+    if (data->length > 0)
+        end_buf = extra + data->length;
+    else
+        end_buf = extra + IW_SCAN_MAX_DATA;
+#else
+    end_buf = extra + IW_SCAN_MAX_DATA;
+#endif
+
+	for (i = 0; i < pAdapter->ScanTab.BssNr; i++)
+	{
+		if (current_ev >= end_buf)
+        {
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+        }
+
+		//MAC address
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWAP;
+		iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
+				memcpy(iwe.u.ap_addr.sa_data, &pAdapter->ScanTab.BssEntry[i].Bssid, ETH_ALEN);
+
+        previous_ev = current_ev;
+		current_ev = IWE_STREAM_ADD_EVENT(info, current_ev,end_buf, &iwe, IW_EV_ADDR_LEN);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//ESSID
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWESSID;
+		iwe.u.data.length = pAdapter->ScanTab.BssEntry[i].SsidLen;
+		iwe.u.data.flags = 1;
+
+        previous_ev = current_ev;
+		current_ev = IWE_STREAM_ADD_POINT(info, current_ev,end_buf, &iwe, pAdapter->ScanTab.BssEntry[i].Ssid);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//Network Type
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWMODE;
+		if (pAdapter->ScanTab.BssEntry[i].BssType == Ndis802_11IBSS)
+		{
+			iwe.u.mode = IW_MODE_ADHOC;
+		}
+		else if (pAdapter->ScanTab.BssEntry[i].BssType == Ndis802_11Infrastructure)
+		{
+			iwe.u.mode = IW_MODE_INFRA;
+		}
+		else
+		{
+			iwe.u.mode = IW_MODE_AUTO;
+		}
+		iwe.len = IW_EV_UINT_LEN;
+
+        previous_ev = current_ev;
+		current_ev = IWE_STREAM_ADD_EVENT(info, current_ev, end_buf, &iwe,  IW_EV_UINT_LEN);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//Channel and Frequency
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWFREQ;
+		if (INFRA_ON(pAdapter) || ADHOC_ON(pAdapter))
+			iwe.u.freq.m = pAdapter->ScanTab.BssEntry[i].Channel;
+		else
+			iwe.u.freq.m = pAdapter->ScanTab.BssEntry[i].Channel;
+		iwe.u.freq.e = 0;
+		iwe.u.freq.i = 0;
+
+		previous_ev = current_ev;
+		current_ev = IWE_STREAM_ADD_EVENT(info, current_ev,end_buf, &iwe, IW_EV_FREQ_LEN);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+        //Add quality statistics
+        //================================
+        memset(&iwe, 0, sizeof(iwe));
+    	iwe.cmd = IWEVQUAL;
+    	iwe.u.qual.level = 0;
+    	iwe.u.qual.noise = 0;
+        set_quality(pAdapter, &iwe.u.qual, pAdapter->ScanTab.BssEntry[i].Rssi);
+    	current_ev = IWE_STREAM_ADD_EVENT(info, current_ev, end_buf, &iwe, IW_EV_QUAL_LEN);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//Encyption key
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWENCODE;
+		if (CAP_IS_PRIVACY_ON (pAdapter->ScanTab.BssEntry[i].CapabilityInfo ))
+			iwe.u.data.flags =IW_ENCODE_ENABLED | IW_ENCODE_NOKEY;
+		else
+			iwe.u.data.flags = IW_ENCODE_DISABLED;
+
+        previous_ev = current_ev;
+        current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf,&iwe, (char *)pAdapter->SharedKey[BSS0][(iwe.u.data.flags & IW_ENCODE_INDEX)-1].Key);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//Bit Rate
+		//================================
+		if (pAdapter->ScanTab.BssEntry[i].SupRateLen)
+        {
+            UCHAR tmpRate = pAdapter->ScanTab.BssEntry[i].SupRate[pAdapter->ScanTab.BssEntry[i].SupRateLen-1];
+			memset(&iwe, 0, sizeof(iwe));
+			iwe.cmd = SIOCGIWRATE;
+    		current_val = current_ev + IW_EV_LCP_LEN;
+            if (tmpRate == 0x82)
+                iwe.u.bitrate.value =  1 * 1000000;
+            else if (tmpRate == 0x84)
+                iwe.u.bitrate.value =  2 * 1000000;
+            else if (tmpRate == 0x8B)
+                iwe.u.bitrate.value =  5.5 * 1000000;
+            else if (tmpRate == 0x96)
+                iwe.u.bitrate.value =  11 * 1000000;
+            else
+    		    iwe.u.bitrate.value =  (tmpRate/2) * 1000000;
+
+			iwe.u.bitrate.disabled = 0;
+			current_val = IWE_STREAM_ADD_VALUE(info, current_ev,
+				current_val, end_buf, &iwe,
+    			IW_EV_PARAM_LEN);
+
+        	if((current_val-current_ev)>IW_EV_LCP_LEN)
+            	current_ev = current_val;
+        	else
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+        }
+
+#ifdef IWEVGENIE
+		//WPA IE
+		if (pAdapter->ScanTab.BssEntry[i].WpaIE.IELen > 0)
+		{
+			memset(&iwe, 0, sizeof(iwe));
+			memset(&custom[0], 0, MAX_CUSTOM_LEN);
+			memcpy(custom, &(pAdapter->ScanTab.BssEntry[i].WpaIE.IE[0]),
+						   pAdapter->ScanTab.BssEntry[i].WpaIE.IELen);
+			iwe.cmd = IWEVGENIE;
+			iwe.u.data.length = pAdapter->ScanTab.BssEntry[i].WpaIE.IELen;
+			current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf, &iwe, custom);
+			if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+		}
+
+		//WPA2 IE
+        if (pAdapter->ScanTab.BssEntry[i].RsnIE.IELen > 0)
+        {
+        	memset(&iwe, 0, sizeof(iwe));
+			memset(&custom[0], 0, MAX_CUSTOM_LEN);
+			memcpy(custom, &(pAdapter->ScanTab.BssEntry[i].RsnIE.IE[0]),
+						   pAdapter->ScanTab.BssEntry[i].RsnIE.IELen);
+			iwe.cmd = IWEVGENIE;
+			iwe.u.data.length = pAdapter->ScanTab.BssEntry[i].RsnIE.IELen;
+			current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf, &iwe, custom);
+			if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+        }
+#else
+        //WPA IE
+		//================================
+        if (pAdapter->ScanTab.BssEntry[i].WpaIE.IELen > 0)
+        {
+    		NdisZeroMemory(&iwe, sizeof(iwe));
+			memset(&custom[0], 0, MAX_CUSTOM_LEN);
+    		iwe.cmd = IWEVCUSTOM;
+            iwe.u.data.length = (pAdapter->ScanTab.BssEntry[i].WpaIE.IELen * 2) + 7;
+            NdisMoveMemory(custom, "wpa_ie=", 7);
+            for (idx = 0; idx < pAdapter->ScanTab.BssEntry[i].WpaIE.IELen; idx++)
+                sprintf(custom, "%s%02x", custom, pAdapter->ScanTab.BssEntry[i].WpaIE.IE[idx]);
+            previous_ev = current_ev;
+    		current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf, &iwe,  custom);
+            if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+        }
+
+        //WPA2 IE
+        if (pAdapter->ScanTab.BssEntry[i].RsnIE.IELen > 0)
+        {
+    		NdisZeroMemory(&iwe, sizeof(iwe));
+			memset(&custom[0], 0, MAX_CUSTOM_LEN);
+    		iwe.cmd = IWEVCUSTOM;
+            iwe.u.data.length = (pAdapter->ScanTab.BssEntry[i].RsnIE.IELen * 2) + 7;
+            NdisMoveMemory(custom, "rsn_ie=", 7);
+			for (idx = 0; idx < pAdapter->ScanTab.BssEntry[i].RsnIE.IELen; idx++)
+                sprintf(custom, "%s%02x", custom, pAdapter->ScanTab.BssEntry[i].RsnIE.IE[idx]);
+            previous_ev = current_ev;
+    		current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf, &iwe,  custom);
+            if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+        }
+#endif // IWEVGENIE //
+	}
+
+	data->length = current_ev - extra;
+    pAdapter->StaCfg.bScanReqIsFromWebUI = FALSE;
+	DBGPRINT(RT_DEBUG_ERROR ,("===>rt_ioctl_giwscan. %d(%d) BSS returned, data->length = %d\n",i , pAdapter->ScanTab.BssNr, data->length));
+	return 0;
+}
+#endif
+
+int rt_ioctl_siwessid(struct net_device *dev,
+			 struct iw_request_info *info,
+			 struct iw_point *data, char *essid)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+
+	//check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+       	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+       	return -ENETDOWN;
+    }
+
+	if (data->flags)
+	{
+		PCHAR	pSsidString = NULL;
+
+		// Includes null character.
+		if (data->length > (IW_ESSID_MAX_SIZE + 1))
+			return -E2BIG;
+
+		pSsidString = (CHAR *) kmalloc(MAX_LEN_OF_SSID+1, MEM_ALLOC_FLAG);
+		if (pSsidString)
+		{
+			NdisZeroMemory(pSsidString, MAX_LEN_OF_SSID+1);
+			NdisMoveMemory(pSsidString, essid, data->length);
+			if (Set_SSID_Proc(pAdapter, pSsidString) == FALSE)
+				return -EINVAL;
+		}
+		else
+			return -ENOMEM;
+	}
+	else
+	{
+		// ANY ssid
+		if (Set_SSID_Proc(pAdapter, "") == FALSE)
+			return -EINVAL;
+    }
+	return 0;
+}
+
+int rt_ioctl_giwessid(struct net_device *dev,
+			 struct iw_request_info *info,
+			 struct iw_point *data, char *essid)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	data->flags = 1;
+    if (MONITOR_ON(pAdapter))
+    {
+        data->length  = 0;
+        return 0;
+    }
+
+	if (OPSTATUS_TEST_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED))
+	{
+		DBGPRINT(RT_DEBUG_TRACE ,("MediaState is connected\n"));
+		data->length = pAdapter->CommonCfg.SsidLen;
+		memcpy(essid, pAdapter->CommonCfg.Ssid, pAdapter->CommonCfg.SsidLen);
+	}
+#ifdef RT2870
+#ifdef WPA_SUPPLICANT_SUPPORT
+    // Add for RT2870
+    else if (pAdapter->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_DISABLE)
+    {
+        data->length = pAdapter->CommonCfg.SsidLen;
+		memcpy(essid, pAdapter->CommonCfg.Ssid, pAdapter->CommonCfg.SsidLen);
+	}
+#endif // WPA_SUPPLICANT_SUPPORT //
+#endif // RT2870 //
+	else
+	{//the ANY ssid was specified
+		data->length  = 0;
+		DBGPRINT(RT_DEBUG_TRACE ,("MediaState is not connected, ess\n"));
+	}
+
+	return 0;
+
+}
+
+int rt_ioctl_siwnickn(struct net_device *dev,
+			 struct iw_request_info *info,
+			 struct iw_point *data, char *nickname)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+
+    //check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+        DBGPRINT(RT_DEBUG_TRACE ,("INFO::Network is down!\n"));
+        return -ENETDOWN;
+    }
+
+	if (data->length > IW_ESSID_MAX_SIZE)
+		return -EINVAL;
+
+	memset(pAdapter->nickname, 0, IW_ESSID_MAX_SIZE + 1);
+	memcpy(pAdapter->nickname, nickname, data->length);
+
+
+	return 0;
+}
+
+int rt_ioctl_giwnickn(struct net_device *dev,
+			 struct iw_request_info *info,
+			 struct iw_point *data, char *nickname)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	if (data->length > strlen(pAdapter->nickname) + 1)
+		data->length = strlen(pAdapter->nickname) + 1;
+	if (data->length > 0) {
+		memcpy(nickname, pAdapter->nickname, data->length-1);
+		nickname[data->length-1] = '\0';
+	}
+	return 0;
+}
+
+int rt_ioctl_siwrts(struct net_device *dev,
+		       struct iw_request_info *info,
+		       struct iw_param *rts, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+	u16 val;
+
+    //check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        return -ENETDOWN;
+    }
+
+	if (rts->disabled)
+		val = MAX_RTS_THRESHOLD;
+	else if (rts->value < 0 || rts->value > MAX_RTS_THRESHOLD)
+		return -EINVAL;
+	else if (rts->value == 0)
+	    val = MAX_RTS_THRESHOLD;
+	else
+		val = rts->value;
+
+	if (val != pAdapter->CommonCfg.RtsThreshold)
+		pAdapter->CommonCfg.RtsThreshold = val;
+
+	return 0;
+}
+
+int rt_ioctl_giwrts(struct net_device *dev,
+		       struct iw_request_info *info,
+		       struct iw_param *rts, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	//check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+	rts->value = pAdapter->CommonCfg.RtsThreshold;
+	rts->disabled = (rts->value == MAX_RTS_THRESHOLD);
+	rts->fixed = 1;
+
+	return 0;
+}
+
+int rt_ioctl_siwfrag(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_param *frag, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+	u16 val;
+
+	//check if the interface is down
+    	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    	{
+      		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        	return -ENETDOWN;
+    	}
+
+	if (frag->disabled)
+		val = MAX_FRAG_THRESHOLD;
+	else if (frag->value >= MIN_FRAG_THRESHOLD || frag->value <= MAX_FRAG_THRESHOLD)
+        val = __cpu_to_le16(frag->value & ~0x1); /* even numbers only */
+	else if (frag->value == 0)
+	    val = MAX_FRAG_THRESHOLD;
+	else
+		return -EINVAL;
+
+	pAdapter->CommonCfg.FragmentThreshold = val;
+	return 0;
+}
+
+int rt_ioctl_giwfrag(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_param *frag, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	//check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+	frag->value = pAdapter->CommonCfg.FragmentThreshold;
+	frag->disabled = (frag->value == MAX_FRAG_THRESHOLD);
+	frag->fixed = 1;
+
+	return 0;
+}
+
+#define MAX_WEP_KEY_SIZE 13
+#define MIN_WEP_KEY_SIZE 5
+int rt_ioctl_siwencode(struct net_device *dev,
+			  struct iw_request_info *info,
+			  struct iw_point *erq, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = dev->ml_priv;
+
+	//check if the interface is down
+    	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    	{
+      		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        	return -ENETDOWN;
+    	}
+
+	if ((erq->length == 0) &&
+        (erq->flags & IW_ENCODE_DISABLED))
+	{
+		pAdapter->StaCfg.PairCipher = Ndis802_11WEPDisabled;
+		pAdapter->StaCfg.GroupCipher = Ndis802_11WEPDisabled;
+		pAdapter->StaCfg.WepStatus = Ndis802_11WEPDisabled;
+        pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+        goto done;
+	}
+	else if ((erq->length == 0) &&
+             (erq->flags & IW_ENCODE_RESTRICTED || erq->flags & IW_ENCODE_OPEN))
+	{
+	    //pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+		STA_PORT_SECURED(pAdapter);
+		pAdapter->StaCfg.PairCipher = Ndis802_11WEPEnabled;
+		pAdapter->StaCfg.GroupCipher = Ndis802_11WEPEnabled;
+		pAdapter->StaCfg.WepStatus = Ndis802_11WEPEnabled;
+        pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+		if (erq->flags & IW_ENCODE_RESTRICTED)
+			pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeShared;
+    	else
+			pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+        goto done;
+	}
+
+    if (erq->length > 0)
+	{
+		int keyIdx = (erq->flags & IW_ENCODE_INDEX) - 1;
+		/* Check the size of the key */
+		if (erq->length > MAX_WEP_KEY_SIZE) {
+			return -EINVAL;
+		}
+		/* Check key index */
+		if ((keyIdx < 0) || (keyIdx >= NR_WEP_KEYS))
+        {
+            DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::Wrong keyIdx=%d! Using default key instead (%d)\n",
+                                        keyIdx, pAdapter->StaCfg.DefaultKeyId));
+
+            //Using default key
+			keyIdx = pAdapter->StaCfg.DefaultKeyId;
+        }
+
+        NdisZeroMemory(pAdapter->SharedKey[BSS0][keyIdx].Key,  16);
+
+		if (erq->length == MAX_WEP_KEY_SIZE)
+        {
+			pAdapter->SharedKey[BSS0][keyIdx].KeyLen = MAX_WEP_KEY_SIZE;
+            pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_WEP128;
+		}
+		else if (erq->length == MIN_WEP_KEY_SIZE)
+        {
+            pAdapter->SharedKey[BSS0][keyIdx].KeyLen = MIN_WEP_KEY_SIZE;
+            pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_WEP64;
+		}
+		else
+			/* Disable the key */
+			pAdapter->SharedKey[BSS0][keyIdx].KeyLen = 0;
+
+		/* Check if the key is not marked as invalid */
+		if(!(erq->flags & IW_ENCODE_NOKEY)) {
+			/* Copy the key in the driver */
+			NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].Key, extra, erq->length);
+        }
+	}
+    else
+			{
+		/* Do we want to just set the transmit key index ? */
+		int index = (erq->flags & IW_ENCODE_INDEX) - 1;
+		if ((index >= 0) && (index < 4))
+        {
+			pAdapter->StaCfg.DefaultKeyId = index;
+            }
+        else
+			/* Don't complain if only change the mode */
+			if(!erq->flags & IW_ENCODE_MODE) {
+				return -EINVAL;
+		}
+	}
+
+done:
+    DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::erq->flags=%x\n",erq->flags));
+	DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::AuthMode=%x\n",pAdapter->StaCfg.AuthMode));
+	DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::DefaultKeyId=%x, KeyLen = %d\n",pAdapter->StaCfg.DefaultKeyId , pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen));
+	DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::WepStatus=%x\n",pAdapter->StaCfg.WepStatus));
+	return 0;
+}
+
+int
+rt_ioctl_giwencode(struct net_device *dev,
+			  struct iw_request_info *info,
+			  struct iw_point *erq, char *key)
+{
+	int kid;
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	//check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+	kid = erq->flags & IW_ENCODE_INDEX;
+	DBGPRINT(RT_DEBUG_TRACE, ("===>rt_ioctl_giwencode %d\n", erq->flags & IW_ENCODE_INDEX));
+
+	if (pAdapter->StaCfg.WepStatus == Ndis802_11WEPDisabled)
+	{
+		erq->length = 0;
+		erq->flags = IW_ENCODE_DISABLED;
+	}
+	else if ((kid > 0) && (kid <=4))
+	{
+		// copy wep key
+		erq->flags = kid ;			/* NB: base 1 */
+		if (erq->length > pAdapter->SharedKey[BSS0][kid-1].KeyLen)
+			erq->length = pAdapter->SharedKey[BSS0][kid-1].KeyLen;
+		memcpy(key, pAdapter->SharedKey[BSS0][kid-1].Key, erq->length);
+		//if ((kid == pAdapter->PortCfg.DefaultKeyId))
+		//erq->flags |= IW_ENCODE_ENABLED;	/* XXX */
+		if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeShared)
+			erq->flags |= IW_ENCODE_RESTRICTED;		/* XXX */
+		else
+			erq->flags |= IW_ENCODE_OPEN;		/* XXX */
+
+	}
+	else if (kid == 0)
+	{
+		if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeShared)
+			erq->flags |= IW_ENCODE_RESTRICTED;		/* XXX */
+		else
+			erq->flags |= IW_ENCODE_OPEN;		/* XXX */
+		erq->length = pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen;
+		memcpy(key, pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].Key, erq->length);
+		// copy default key ID
+		if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeShared)
+			erq->flags |= IW_ENCODE_RESTRICTED;		/* XXX */
+		else
+			erq->flags |= IW_ENCODE_OPEN;		/* XXX */
+		erq->flags = pAdapter->StaCfg.DefaultKeyId + 1;			/* NB: base 1 */
+		erq->flags |= IW_ENCODE_ENABLED;	/* XXX */
+	}
+
+	return 0;
+
+}
+
+static int
+rt_ioctl_setparam(struct net_device *dev, struct iw_request_info *info,
+			 void *w, char *extra)
+{
+    VIRTUAL_ADAPTER	*pVirtualAd = NULL;
+	PRTMP_ADAPTER pAdapter;
+	POS_COOKIE pObj;
+	char *this_char = extra;
+	char *value;
+	int  Status=0;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		pAdapter = pVirtualAd->RtmpDev->ml_priv;
+	}
+	pObj = (POS_COOKIE) pAdapter->OS_Cookie;
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	{
+		pObj->ioctl_if_type = INT_MAIN;
+        pObj->ioctl_if = MAIN_MBSSID;
+	}
+
+	//check if the interface is down
+    	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    	{
+      		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+			return -ENETDOWN;
+    	}
+
+	if (!*this_char)
+		return -EINVAL;
+
+	if ((value = rtstrchr(this_char, '=')) != NULL)
+	    *value++ = 0;
+
+	if (!value)
+	    return -EINVAL;
+
+	// reject setting nothing besides ANY ssid(ssidLen=0)
+    if (!*value && (strcmp(this_char, "SSID") != 0))
+        return -EINVAL;
+
+	for (PRTMP_PRIVATE_SET_PROC = RTMP_PRIVATE_SUPPORT_PROC; PRTMP_PRIVATE_SET_PROC->name; PRTMP_PRIVATE_SET_PROC++)
+	{
+	    if (strcmp(this_char, PRTMP_PRIVATE_SET_PROC->name) == 0)
+	    {
+	        if(!PRTMP_PRIVATE_SET_PROC->set_proc(pAdapter, value))
+	        {	//FALSE:Set private failed then return Invalid argument
+			    Status = -EINVAL;
+	        }
+		    break;	//Exit for loop.
+	    }
+	}
+
+	if(PRTMP_PRIVATE_SET_PROC->name == NULL)
+	{  //Not found argument
+	    Status = -EINVAL;
+	    DBGPRINT(RT_DEBUG_TRACE, ("===>rt_ioctl_setparam:: (iwpriv) Not Support Set Command [%s=%s]\n", this_char, value));
+	}
+
+    return Status;
+}
+
+
+static int
+rt_private_get_statistics(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra)
+{
+	INT				Status = 0;
+    PRTMP_ADAPTER   pAd = dev->ml_priv;
+
+    if (extra == NULL)
+    {
+        wrq->length = 0;
+        return -EIO;
+    }
+
+    memset(extra, 0x00, IW_PRIV_SIZE_MASK);
+    sprintf(extra, "\n\n");
+
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+	{
+	    sprintf(extra+strlen(extra), "Tx success                      = %ld\n", (ULONG)pAd->ate.TxDoneCount);
+	    //sprintf(extra+strlen(extra), "Tx success without retry        = %ld\n", (ULONG)pAd->ate.TxDoneCount);
+	}
+	else
+#endif // RALINK_ATE //
+	{
+    sprintf(extra+strlen(extra), "Tx success                      = %ld\n", (ULONG)pAd->WlanCounters.TransmittedFragmentCount.QuadPart);
+    sprintf(extra+strlen(extra), "Tx success without retry        = %ld\n", (ULONG)pAd->WlanCounters.TransmittedFragmentCount.QuadPart - (ULONG)pAd->WlanCounters.RetryCount.QuadPart);
+	}
+    sprintf(extra+strlen(extra), "Tx success after retry          = %ld\n", (ULONG)pAd->WlanCounters.RetryCount.QuadPart);
+    sprintf(extra+strlen(extra), "Tx fail to Rcv ACK after retry  = %ld\n", (ULONG)pAd->WlanCounters.FailedCount.QuadPart);
+    sprintf(extra+strlen(extra), "RTS Success Rcv CTS             = %ld\n", (ULONG)pAd->WlanCounters.RTSSuccessCount.QuadPart);
+    sprintf(extra+strlen(extra), "RTS Fail Rcv CTS                = %ld\n", (ULONG)pAd->WlanCounters.RTSFailureCount.QuadPart);
+
+    sprintf(extra+strlen(extra), "Rx success                      = %ld\n", (ULONG)pAd->WlanCounters.ReceivedFragmentCount.QuadPart);
+    sprintf(extra+strlen(extra), "Rx with CRC                     = %ld\n", (ULONG)pAd->WlanCounters.FCSErrorCount.QuadPart);
+    sprintf(extra+strlen(extra), "Rx drop due to out of resource  = %ld\n", (ULONG)pAd->Counters8023.RxNoBuffer);
+    sprintf(extra+strlen(extra), "Rx duplicate frame              = %ld\n", (ULONG)pAd->WlanCounters.FrameDuplicateCount.QuadPart);
+
+    sprintf(extra+strlen(extra), "False CCA (one second)          = %ld\n", (ULONG)pAd->RalinkCounters.OneSecFalseCCACnt);
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+	{
+		if (pAd->ate.RxAntennaSel == 0)
+		{
+    		sprintf(extra+strlen(extra), "RSSI-A                          = %ld\n", (LONG)(pAd->ate.LastRssi0 - pAd->BbpRssiToDbmDelta));
+			sprintf(extra+strlen(extra), "RSSI-B (if available)           = %ld\n", (LONG)(pAd->ate.LastRssi1 - pAd->BbpRssiToDbmDelta));
+			sprintf(extra+strlen(extra), "RSSI-C (if available)           = %ld\n\n", (LONG)(pAd->ate.LastRssi2 - pAd->BbpRssiToDbmDelta));
+		}
+		else
+		{
+    		sprintf(extra+strlen(extra), "RSSI                            = %ld\n", (LONG)(pAd->ate.LastRssi0 - pAd->BbpRssiToDbmDelta));
+		}
+	}
+	else
+#endif // RALINK_ATE //
+	{
+    	sprintf(extra+strlen(extra), "RSSI-A                          = %ld\n", (LONG)(pAd->StaCfg.RssiSample.LastRssi0 - pAd->BbpRssiToDbmDelta));
+        sprintf(extra+strlen(extra), "RSSI-B (if available)           = %ld\n", (LONG)(pAd->StaCfg.RssiSample.LastRssi1 - pAd->BbpRssiToDbmDelta));
+        sprintf(extra+strlen(extra), "RSSI-C (if available)           = %ld\n\n", (LONG)(pAd->StaCfg.RssiSample.LastRssi2 - pAd->BbpRssiToDbmDelta));
+	}
+#ifdef WPA_SUPPLICANT_SUPPORT
+    sprintf(extra+strlen(extra), "WpaSupplicantUP                 = %d\n\n", pAd->StaCfg.WpaSupplicantUP);
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+
+    wrq->length = strlen(extra) + 1; // 1: size of '\0'
+    DBGPRINT(RT_DEBUG_TRACE, ("<== rt_private_get_statistics, wrq->length = %d\n", wrq->length));
+
+    return Status;
+}
+
+#ifdef DOT11_N_SUPPORT
+void	getBaInfo(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pOutBuf)
+{
+	INT i, j;
+	BA_ORI_ENTRY *pOriBAEntry;
+	BA_REC_ENTRY *pRecBAEntry;
+
+	for (i=0; i<MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		PMAC_TABLE_ENTRY pEntry = &pAd->MacTab.Content[i];
+		if (((pEntry->ValidAsCLI || pEntry->ValidAsApCli) && (pEntry->Sst == SST_ASSOC))
+			|| (pEntry->ValidAsWDS) || (pEntry->ValidAsMesh))
+		{
+			sprintf(pOutBuf, "%s\n%02X:%02X:%02X:%02X:%02X:%02X (Aid = %d) (AP) -\n",
+                pOutBuf,
+				pEntry->Addr[0], pEntry->Addr[1], pEntry->Addr[2],
+				pEntry->Addr[3], pEntry->Addr[4], pEntry->Addr[5], pEntry->Aid);
+
+			sprintf(pOutBuf, "%s[Recipient]\n", pOutBuf);
+			for (j=0; j < NUM_OF_TID; j++)
+			{
+				if (pEntry->BARecWcidArray[j] != 0)
+				{
+					pRecBAEntry =&pAd->BATable.BARecEntry[pEntry->BARecWcidArray[j]];
+					sprintf(pOutBuf, "%sTID=%d, BAWinSize=%d, LastIndSeq=%d, ReorderingPkts=%d\n", pOutBuf, j, pRecBAEntry->BAWinSize, pRecBAEntry->LastIndSeq, pRecBAEntry->list.qlen);
+				}
+			}
+			sprintf(pOutBuf, "%s\n", pOutBuf);
+
+			sprintf(pOutBuf, "%s[Originator]\n", pOutBuf);
+			for (j=0; j < NUM_OF_TID; j++)
+			{
+				if (pEntry->BAOriWcidArray[j] != 0)
+				{
+					pOriBAEntry =&pAd->BATable.BAOriEntry[pEntry->BAOriWcidArray[j]];
+					sprintf(pOutBuf, "%sTID=%d, BAWinSize=%d, StartSeq=%d, CurTxSeq=%d\n", pOutBuf, j, pOriBAEntry->BAWinSize, pOriBAEntry->Sequence, pEntry->TxSeq[j]);
+				}
+			}
+			sprintf(pOutBuf, "%s\n\n", pOutBuf);
+		}
+        if (strlen(pOutBuf) > (IW_PRIV_SIZE_MASK - 30))
+                break;
+	}
+
+	return;
+}
+#endif // DOT11_N_SUPPORT //
+
+static int
+rt_private_show(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra)
+{
+    INT				Status = 0;
+    VIRTUAL_ADAPTER	*pVirtualAd = NULL;
+    PRTMP_ADAPTER   pAd;
+	POS_COOKIE		pObj;
+    u32             subcmd = wrq->flags;
+
+	if (dev->priv_flags == INT_MAIN)
+		pAd = dev->ml_priv;
+	else
+	{
+		pVirtualAd = dev->ml_priv;
+		pAd = pVirtualAd->RtmpDev->ml_priv;
+	}
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	if (pAd == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+    if (extra == NULL)
+    {
+        wrq->length = 0;
+        return -EIO;
+    }
+    memset(extra, 0x00, IW_PRIV_SIZE_MASK);
+
+	{
+		pObj->ioctl_if_type = INT_MAIN;
+        pObj->ioctl_if = MAIN_MBSSID;
+	}
+
+    switch(subcmd)
+    {
+
+        case SHOW_CONN_STATUS:
+            if (MONITOR_ON(pAd))
+            {
+#ifdef DOT11_N_SUPPORT
+                if (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED &&
+                    pAd->CommonCfg.RegTransmitSetting.field.BW)
+                    sprintf(extra, "Monitor Mode(CentralChannel %d)\n", pAd->CommonCfg.CentralChannel);
+                else
+#endif // DOT11_N_SUPPORT //
+                    sprintf(extra, "Monitor Mode(Channel %d)\n", pAd->CommonCfg.Channel);
+            }
+            else
+            {
+                if (pAd->IndicateMediaState == NdisMediaStateConnected)
+            	{
+            	    if (INFRA_ON(pAd))
+                    {
+                    sprintf(extra, "Connected(AP: %s[%02X:%02X:%02X:%02X:%02X:%02X])\n",
+                                    pAd->CommonCfg.Ssid,
+                                    pAd->CommonCfg.Bssid[0],
+                                    pAd->CommonCfg.Bssid[1],
+                                    pAd->CommonCfg.Bssid[2],
+                                    pAd->CommonCfg.Bssid[3],
+                                    pAd->CommonCfg.Bssid[4],
+                                    pAd->CommonCfg.Bssid[5]);
+            		DBGPRINT(RT_DEBUG_TRACE ,("Ssid=%s ,Ssidlen = %d\n",pAd->CommonCfg.Ssid, pAd->CommonCfg.SsidLen));
+            	}
+                    else if (ADHOC_ON(pAd))
+                        sprintf(extra, "Connected\n");
+            	}
+            	else
+            	{
+            	    sprintf(extra, "Disconnected\n");
+            		DBGPRINT(RT_DEBUG_TRACE ,("ConnStatus is not connected\n"));
+            	}
+            }
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+        case SHOW_DRVIER_VERION:
+            sprintf(extra, "Driver version-%s, %s %s\n", STA_DRIVER_VERSION, __DATE__, __TIME__ );
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+#ifdef DOT11_N_SUPPORT
+        case SHOW_BA_INFO:
+            getBaInfo(pAd, extra);
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+#endif // DOT11_N_SUPPORT //
+		case SHOW_DESC_INFO:
+			{
+				Show_DescInfo_Proc(pAd, NULL);
+				wrq->length = 0; // 1: size of '\0'
+			}
+			break;
+        case RAIO_OFF:
+            if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+            {
+                sprintf(extra, "Scanning\n");
+                wrq->length = strlen(extra) + 1; // 1: size of '\0'
+                break;
+            }
+            pAd->StaCfg.bSwRadio = FALSE;
+            if (pAd->StaCfg.bRadio != (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio))
+            {
+                pAd->StaCfg.bRadio = (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio);
+                if (pAd->StaCfg.bRadio == FALSE)
+                {
+                    MlmeRadioOff(pAd);
+                    // Update extra information
+					pAd->ExtraInfo = SW_RADIO_OFF;
+                }
+            }
+            sprintf(extra, "Radio Off\n");
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+        case RAIO_ON:
+            if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+            {
+                sprintf(extra, "Scanning\n");
+                wrq->length = strlen(extra) + 1; // 1: size of '\0'
+                break;
+            }
+            pAd->StaCfg.bSwRadio = TRUE;
+            //if (pAd->StaCfg.bRadio != (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio))
+            {
+                pAd->StaCfg.bRadio = (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio);
+                if (pAd->StaCfg.bRadio == TRUE)
+                {
+                    MlmeRadioOn(pAd);
+                    // Update extra information
+					pAd->ExtraInfo = EXTRA_INFO_CLEAR;
+                }
+            }
+            sprintf(extra, "Radio On\n");
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+
+
+#ifdef QOS_DLS_SUPPORT
+		case SHOW_DLS_ENTRY_INFO:
+			{
+				Set_DlsEntryInfo_Display_Proc(pAd, NULL);
+				wrq->length = 0; // 1: size of '\0'
+			}
+			break;
+#endif // QOS_DLS_SUPPORT //
+
+		case SHOW_CFG_VALUE:
+			{
+				Status = RTMPShowCfgValue(pAd, wrq->pointer, extra);
+				if (Status == 0)
+					wrq->length = strlen(extra) + 1; // 1: size of '\0'
+			}
+			break;
+		case SHOW_ADHOC_ENTRY_INFO:
+			Show_Adhoc_MacTable_Proc(pAd, extra);
+			wrq->length = strlen(extra) + 1; // 1: size of '\0'
+			break;
+        default:
+            DBGPRINT(RT_DEBUG_TRACE, ("%s - unknow subcmd = %d\n", __FUNCTION__, subcmd));
+            break;
+    }
+
+    return Status;
+}
+
+#ifdef SIOCSIWMLME
+int rt_ioctl_siwmlme(struct net_device *dev,
+			   struct iw_request_info *info,
+			   union iwreq_data *wrqu,
+			   char *extra)
+{
+	PRTMP_ADAPTER   pAd = dev->ml_priv;
+	struct iw_mlme *pMlme = (struct iw_mlme *)wrqu->data.pointer;
+	MLME_QUEUE_ELEM				MsgElem;
+	MLME_DISASSOC_REQ_STRUCT	DisAssocReq;
+	MLME_DEAUTH_REQ_STRUCT      DeAuthReq;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("====> %s\n", __FUNCTION__));
+
+	if (pMlme == NULL)
+		return -EINVAL;
+
+	switch(pMlme->cmd)
+	{
+#ifdef IW_MLME_DEAUTH
+		case IW_MLME_DEAUTH:
+			DBGPRINT(RT_DEBUG_TRACE, ("====> %s - IW_MLME_DEAUTH\n", __FUNCTION__));
+			COPY_MAC_ADDR(DeAuthReq.Addr, pAd->CommonCfg.Bssid);
+			DeAuthReq.Reason = pMlme->reason_code;
+			MsgElem.MsgLen = sizeof(MLME_DEAUTH_REQ_STRUCT);
+			NdisMoveMemory(MsgElem.Msg, &DeAuthReq, sizeof(MLME_DEAUTH_REQ_STRUCT));
+			MlmeDeauthReqAction(pAd, &MsgElem);
+			if (INFRA_ON(pAd))
+			{
+			    LinkDown(pAd, FALSE);
+			    pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+			}
+			break;
+#endif // IW_MLME_DEAUTH //
+#ifdef IW_MLME_DISASSOC
+		case IW_MLME_DISASSOC:
+			DBGPRINT(RT_DEBUG_TRACE, ("====> %s - IW_MLME_DISASSOC\n", __FUNCTION__));
+			COPY_MAC_ADDR(DisAssocReq.Addr, pAd->CommonCfg.Bssid);
+			DisAssocReq.Reason =  pMlme->reason_code;
+
+			MsgElem.Machine = ASSOC_STATE_MACHINE;
+			MsgElem.MsgType = MT2_MLME_DISASSOC_REQ;
+			MsgElem.MsgLen = sizeof(MLME_DISASSOC_REQ_STRUCT);
+			NdisMoveMemory(MsgElem.Msg, &DisAssocReq, sizeof(MLME_DISASSOC_REQ_STRUCT));
+
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_DISASSOC;
+			MlmeDisassocReqAction(pAd, &MsgElem);
+			break;
+#endif // IW_MLME_DISASSOC //
+		default:
+			DBGPRINT(RT_DEBUG_TRACE, ("====> %s - Unknow Command\n", __FUNCTION__));
+			break;
+	}
+
+	return 0;
+}
+#endif // SIOCSIWMLME //
+
+#if WIRELESS_EXT > 17
+int rt_ioctl_siwauth(struct net_device *dev,
+			  struct iw_request_info *info,
+			  union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER   pAdapter = dev->ml_priv;
+	struct iw_param *param = &wrqu->param;
+
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+	switch (param->flags & IW_AUTH_INDEX) {
+    	case IW_AUTH_WPA_VERSION:
+            if (param->value == IW_AUTH_WPA_VERSION_WPA)
+            {
+                pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPAPSK;
+				if (pAdapter->StaCfg.BssType == BSS_ADHOC)
+					pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPANone;
+            }
+            else if (param->value == IW_AUTH_WPA_VERSION_WPA2)
+                pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA2PSK;
+
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_WPA_VERSION - param->value = %d!\n", __FUNCTION__, param->value));
+            break;
+    	case IW_AUTH_CIPHER_PAIRWISE:
+            if (param->value == IW_AUTH_CIPHER_NONE)
+            {
+                pAdapter->StaCfg.WepStatus = Ndis802_11WEPDisabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11WEPDisabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_WEP40 ||
+                     param->value == IW_AUTH_CIPHER_WEP104)
+            {
+                pAdapter->StaCfg.WepStatus = Ndis802_11WEPEnabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11WEPEnabled;
+#ifdef WPA_SUPPLICANT_SUPPORT
+                pAdapter->StaCfg.IEEE8021X = FALSE;
+#endif // WPA_SUPPLICANT_SUPPORT //
+            }
+            else if (param->value == IW_AUTH_CIPHER_TKIP)
+            {
+                pAdapter->StaCfg.WepStatus = Ndis802_11Encryption2Enabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11Encryption2Enabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_CCMP)
+            {
+                pAdapter->StaCfg.WepStatus = Ndis802_11Encryption3Enabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11Encryption3Enabled;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_CIPHER_PAIRWISE - param->value = %d!\n", __FUNCTION__, param->value));
+            break;
+    	case IW_AUTH_CIPHER_GROUP:
+            if (param->value == IW_AUTH_CIPHER_NONE)
+            {
+                pAdapter->StaCfg.GroupCipher = Ndis802_11WEPDisabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_WEP40 ||
+                     param->value == IW_AUTH_CIPHER_WEP104)
+            {
+                pAdapter->StaCfg.GroupCipher = Ndis802_11WEPEnabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_TKIP)
+            {
+                pAdapter->StaCfg.GroupCipher = Ndis802_11Encryption2Enabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_CCMP)
+            {
+                pAdapter->StaCfg.GroupCipher = Ndis802_11Encryption3Enabled;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_CIPHER_GROUP - param->value = %d!\n", __FUNCTION__, param->value));
+            break;
+    	case IW_AUTH_KEY_MGMT:
+            if (param->value == IW_AUTH_KEY_MGMT_802_1X)
+            {
+                if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK)
+                {
+                    pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA;
+#ifdef WPA_SUPPLICANT_SUPPORT
+                    pAdapter->StaCfg.IEEE8021X = FALSE;
+#endif // WPA_SUPPLICANT_SUPPORT //
+                }
+                else if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
+                {
+                    pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA2;
+#ifdef WPA_SUPPLICANT_SUPPORT
+                    pAdapter->StaCfg.IEEE8021X = FALSE;
+#endif // WPA_SUPPLICANT_SUPPORT //
+                }
+#ifdef WPA_SUPPLICANT_SUPPORT
+                else
+                    // WEP 1x
+                    pAdapter->StaCfg.IEEE8021X = TRUE;
+#endif // WPA_SUPPLICANT_SUPPORT //
+            }
+            else if (param->value == 0)
+            {
+                //pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+				STA_PORT_SECURED(pAdapter);
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_KEY_MGMT - param->value = %d!\n", __FUNCTION__, param->value));
+            break;
+    	case IW_AUTH_RX_UNENCRYPTED_EAPOL:
+            break;
+    	case IW_AUTH_PRIVACY_INVOKED:
+            /*if (param->value == 0)
+			{
+                pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+                pAdapter->StaCfg.WepStatus = Ndis802_11WEPDisabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11WEPDisabled;
+        	    pAdapter->StaCfg.GroupCipher = Ndis802_11WEPDisabled;
+            }*/
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_PRIVACY_INVOKED - param->value = %d!\n", __FUNCTION__, param->value));
+    		break;
+    	case IW_AUTH_DROP_UNENCRYPTED:
+            if (param->value != 0)
+                pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+			else
+			{
+                //pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+				STA_PORT_SECURED(pAdapter);
+			}
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_WPA_VERSION - param->value = %d!\n", __FUNCTION__, param->value));
+    		break;
+    	case IW_AUTH_80211_AUTH_ALG:
+			if (param->value & IW_AUTH_ALG_SHARED_KEY)
+            {
+				pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeShared;
+			}
+            else if (param->value & IW_AUTH_ALG_OPEN_SYSTEM)
+            {
+				pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+			}
+            else
+				return -EINVAL;
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_80211_AUTH_ALG - param->value = %d!\n", __FUNCTION__, param->value));
+			break;
+    	case IW_AUTH_WPA_ENABLED:
+    		DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_WPA_ENABLED - Driver supports WPA!(param->value = %d)\n", __FUNCTION__, param->value));
+    		break;
+    	default:
+    		return -EOPNOTSUPP;
+}
+
+	return 0;
+}
+
+int rt_ioctl_giwauth(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER   pAdapter = dev->ml_priv;
+	struct iw_param *param = &wrqu->param;
+
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+    }
+
+	switch (param->flags & IW_AUTH_INDEX) {
+	case IW_AUTH_DROP_UNENCRYPTED:
+        param->value = (pAdapter->StaCfg.WepStatus == Ndis802_11WEPDisabled) ? 0 : 1;
+		break;
+
+	case IW_AUTH_80211_AUTH_ALG:
+        param->value = (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeShared) ? IW_AUTH_ALG_SHARED_KEY : IW_AUTH_ALG_OPEN_SYSTEM;
+		break;
+
+	case IW_AUTH_WPA_ENABLED:
+		param->value = (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA) ? 1 : 0;
+		break;
+
+	default:
+		return -EOPNOTSUPP;
+	}
+    DBGPRINT(RT_DEBUG_TRACE, ("rt_ioctl_giwauth::param->value = %d!\n", param->value));
+	return 0;
+}
+
+void fnSetCipherKey(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  INT             keyIdx,
+    IN  UCHAR           CipherAlg,
+    IN  BOOLEAN         bGTK,
+    IN  struct iw_encode_ext *ext)
+{
+    NdisZeroMemory(&pAdapter->SharedKey[BSS0][keyIdx], sizeof(CIPHER_KEY));
+    pAdapter->SharedKey[BSS0][keyIdx].KeyLen = LEN_TKIP_EK;
+    NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].Key, ext->key, LEN_TKIP_EK);
+    NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].TxMic, ext->key + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+    NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].RxMic, ext->key + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+    pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CipherAlg;
+
+    // Update group key information to ASIC Shared Key Table
+	AsicAddSharedKeyEntry(pAdapter,
+						  BSS0,
+						  keyIdx,
+						  pAdapter->SharedKey[BSS0][keyIdx].CipherAlg,
+						  pAdapter->SharedKey[BSS0][keyIdx].Key,
+						  pAdapter->SharedKey[BSS0][keyIdx].TxMic,
+						  pAdapter->SharedKey[BSS0][keyIdx].RxMic);
+
+    if (bGTK)
+        // Update ASIC WCID attribute table and IVEIV table
+    	RTMPAddWcidAttributeEntry(pAdapter,
+    							  BSS0,
+    							  keyIdx,
+    							  pAdapter->SharedKey[BSS0][keyIdx].CipherAlg,
+    							  NULL);
+    else
+        // Update ASIC WCID attribute table and IVEIV table
+    	RTMPAddWcidAttributeEntry(pAdapter,
+    							  BSS0,
+    							  keyIdx,
+    							  pAdapter->SharedKey[BSS0][keyIdx].CipherAlg,
+    							  &pAdapter->MacTab.Content[BSSID_WCID]);
+}
+
+int rt_ioctl_siwencodeext(struct net_device *dev,
+			   struct iw_request_info *info,
+			   union iwreq_data *wrqu,
+			   char *extra)
+			{
+    PRTMP_ADAPTER   pAdapter = dev->ml_priv;
+	struct iw_point *encoding = &wrqu->encoding;
+	struct iw_encode_ext *ext = (struct iw_encode_ext *)extra;
+    int keyIdx, alg = ext->alg;
+
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+    if (encoding->flags & IW_ENCODE_DISABLED)
+	{
+        keyIdx = (encoding->flags & IW_ENCODE_INDEX) - 1;
+        // set BSSID wcid entry of the Pair-wise Key table as no-security mode
+	    AsicRemovePairwiseKeyEntry(pAdapter, BSS0, BSSID_WCID);
+        pAdapter->SharedKey[BSS0][keyIdx].KeyLen = 0;
+		pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_NONE;
+		AsicRemoveSharedKeyEntry(pAdapter, 0, (UCHAR)keyIdx);
+        NdisZeroMemory(&pAdapter->SharedKey[BSS0][keyIdx], sizeof(CIPHER_KEY));
+        DBGPRINT(RT_DEBUG_TRACE, ("%s::Remove all keys!(encoding->flags = %x)\n", __FUNCTION__, encoding->flags));
+    }
+					else
+    {
+        // Get Key Index and convet to our own defined key index
+    	keyIdx = (encoding->flags & IW_ENCODE_INDEX) - 1;
+    	if((keyIdx < 0) || (keyIdx >= NR_WEP_KEYS))
+    		return -EINVAL;
+
+        if (ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY)
+        {
+            pAdapter->StaCfg.DefaultKeyId = keyIdx;
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::DefaultKeyId = %d\n", __FUNCTION__, pAdapter->StaCfg.DefaultKeyId));
+        }
+
+        switch (alg) {
+    		case IW_ENCODE_ALG_NONE:
+                DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_ENCODE_ALG_NONE\n", __FUNCTION__));
+    			break;
+    		case IW_ENCODE_ALG_WEP:
+                DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_ENCODE_ALG_WEP - ext->key_len = %d, keyIdx = %d\n", __FUNCTION__, ext->key_len, keyIdx));
+    			if (ext->key_len == MAX_WEP_KEY_SIZE)
+                {
+        			pAdapter->SharedKey[BSS0][keyIdx].KeyLen = MAX_WEP_KEY_SIZE;
+                    pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_WEP128;
+				}
+        		else if (ext->key_len == MIN_WEP_KEY_SIZE)
+                {
+                    pAdapter->SharedKey[BSS0][keyIdx].KeyLen = MIN_WEP_KEY_SIZE;
+                    pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_WEP64;
+				}
+        		else
+                    return -EINVAL;
+
+                NdisZeroMemory(pAdapter->SharedKey[BSS0][keyIdx].Key,  16);
+			    NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].Key, ext->key, ext->key_len);
+
+				if (pAdapter->StaCfg.GroupCipher == Ndis802_11GroupWEP40Enabled ||
+					pAdapter->StaCfg.GroupCipher == Ndis802_11GroupWEP104Enabled)
+				{
+					// Set Group key material to Asic
+					AsicAddSharedKeyEntry(pAdapter, BSS0, keyIdx, pAdapter->SharedKey[BSS0][keyIdx].CipherAlg, pAdapter->SharedKey[BSS0][keyIdx].Key, NULL, NULL);
+
+					// Update WCID attribute table and IVEIV table for this group key table
+					RTMPAddWcidAttributeEntry(pAdapter, BSS0, keyIdx, pAdapter->SharedKey[BSS0][keyIdx].CipherAlg, NULL);
+
+					STA_PORT_SECURED(pAdapter);
+
+    				// Indicate Connected for GUI
+    				pAdapter->IndicateMediaState = NdisMediaStateConnected;
+				}
+    			break;
+            case IW_ENCODE_ALG_TKIP:
+                DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_ENCODE_ALG_TKIP - keyIdx = %d, ext->key_len = %d\n", __FUNCTION__, keyIdx, ext->key_len));
+                if (ext->key_len == 32)
+                {
+                    if (ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY)
+                    {
+                        fnSetCipherKey(pAdapter, keyIdx, CIPHER_TKIP, FALSE, ext);
+                        if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA2)
+                        {
+                            //pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+                            STA_PORT_SECURED(pAdapter);
+                        }
+		}
+                    else if (ext->ext_flags & IW_ENCODE_EXT_GROUP_KEY)
+                    {
+                        fnSetCipherKey(pAdapter, keyIdx, CIPHER_TKIP, TRUE, ext);
+
+                        // set 802.1x port control
+            	        //pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+            	        STA_PORT_SECURED(pAdapter);
+                    }
+                }
+                else
+                    return -EINVAL;
+                break;
+            case IW_ENCODE_ALG_CCMP:
+                if (ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY)
+		{
+                    fnSetCipherKey(pAdapter, keyIdx, CIPHER_AES, FALSE, ext);
+                    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA2)
+                    	//pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+                    	STA_PORT_SECURED(pAdapter);
+                }
+                else if (ext->ext_flags & IW_ENCODE_EXT_GROUP_KEY)
+                {
+                    fnSetCipherKey(pAdapter, keyIdx, CIPHER_AES, TRUE, ext);
+
+                    // set 802.1x port control
+        	        //pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+        	        STA_PORT_SECURED(pAdapter);
+                }
+                break;
+    		default:
+    			return -EINVAL;
+		}
+    }
+
+    return 0;
+}
+
+int
+rt_ioctl_giwencodeext(struct net_device *dev,
+			  struct iw_request_info *info,
+			  union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER pAd = dev->ml_priv;
+	PCHAR pKey = NULL;
+	struct iw_point *encoding = &wrqu->encoding;
+	struct iw_encode_ext *ext = (struct iw_encode_ext *)extra;
+	int idx, max_key_len;
+
+	DBGPRINT(RT_DEBUG_TRACE ,("===> rt_ioctl_giwencodeext\n"));
+
+	max_key_len = encoding->length - sizeof(*ext);
+	if (max_key_len < 0)
+		return -EINVAL;
+
+	idx = encoding->flags & IW_ENCODE_INDEX;
+	if (idx)
+	{
+		if (idx < 1 || idx > 4)
+			return -EINVAL;
+		idx--;
+
+		if ((pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) ||
+			(pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled))
+		{
+			if (idx != pAd->StaCfg.DefaultKeyId)
+			{
+				ext->key_len = 0;
+				return 0;
+			}
+		}
+	}
+	else
+		idx = pAd->StaCfg.DefaultKeyId;
+
+	encoding->flags = idx + 1;
+	memset(ext, 0, sizeof(*ext));
+
+	ext->key_len = 0;
+	switch(pAd->StaCfg.WepStatus) {
+		case Ndis802_11WEPDisabled:
+			ext->alg = IW_ENCODE_ALG_NONE;
+			encoding->flags |= IW_ENCODE_DISABLED;
+			break;
+		case Ndis802_11WEPEnabled:
+			ext->alg = IW_ENCODE_ALG_WEP;
+			if (pAd->SharedKey[BSS0][idx].KeyLen > max_key_len)
+				return -E2BIG;
+			else
+			{
+				ext->key_len = pAd->SharedKey[BSS0][idx].KeyLen;
+				pKey = &(pAd->SharedKey[BSS0][idx].Key[0]);
+			}
+			break;
+		case Ndis802_11Encryption2Enabled:
+		case Ndis802_11Encryption3Enabled:
+			if (pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled)
+				ext->alg = IW_ENCODE_ALG_TKIP;
+			else
+				ext->alg = IW_ENCODE_ALG_CCMP;
+
+			if (max_key_len < 32)
+				return -E2BIG;
+			else
+			{
+				ext->key_len = 32;
+				pKey = &pAd->StaCfg.PMK[0];
+			}
+			break;
+		default:
+			return -EINVAL;
+	}
+
+	if (ext->key_len && pKey)
+	{
+		encoding->flags |= IW_ENCODE_ENABLED;
+		memcpy(ext->key, pKey, ext->key_len);
+	}
+
+	return 0;
+}
+
+#ifdef SIOCSIWGENIE
+int rt_ioctl_siwgenie(struct net_device *dev,
+			  struct iw_request_info *info,
+			  union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER   pAd = dev->ml_priv;
+
+	if (wrqu->data.length > MAX_LEN_OF_RSNIE ||
+	    (wrqu->data.length && extra == NULL))
+		return -EINVAL;
+
+	if (wrqu->data.length)
+	{
+		pAd->StaCfg.RSNIE_Len = wrqu->data.length;
+		NdisMoveMemory(&pAd->StaCfg.RSN_IE[0], extra, pAd->StaCfg.RSNIE_Len);
+	}
+	else
+	{
+		pAd->StaCfg.RSNIE_Len = 0;
+		NdisZeroMemory(&pAd->StaCfg.RSN_IE[0], MAX_LEN_OF_RSNIE);
+	}
+
+	return 0;
+}
+#endif // SIOCSIWGENIE //
+
+int rt_ioctl_giwgenie(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER   pAd = dev->ml_priv;
+
+	if ((pAd->StaCfg.RSNIE_Len == 0) ||
+		(pAd->StaCfg.AuthMode < Ndis802_11AuthModeWPA))
+	{
+		wrqu->data.length = 0;
+		return 0;
+	}
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+#ifdef SIOCSIWGENIE
+	if (pAd->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_ENABLE)
+	{
+	if (wrqu->data.length < pAd->StaCfg.RSNIE_Len)
+		return -E2BIG;
+
+	wrqu->data.length = pAd->StaCfg.RSNIE_Len;
+	memcpy(extra, &pAd->StaCfg.RSN_IE[0], pAd->StaCfg.RSNIE_Len);
+	}
+	else
+#endif // SIOCSIWGENIE //
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+	{
+		UCHAR RSNIe = IE_WPA;
+
+		if (wrqu->data.length < (pAd->StaCfg.RSNIE_Len + 2)) // ID, Len
+			return -E2BIG;
+		wrqu->data.length = pAd->StaCfg.RSNIE_Len + 2;
+
+		if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK) ||
+            (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2))
+			RSNIe = IE_RSN;
+
+		extra[0] = (char)RSNIe;
+		extra[1] = pAd->StaCfg.RSNIE_Len;
+		memcpy(extra+2, &pAd->StaCfg.RSN_IE[0], pAd->StaCfg.RSNIE_Len);
+	}
+
+	return 0;
+}
+
+int rt_ioctl_siwpmksa(struct net_device *dev,
+			   struct iw_request_info *info,
+			   union iwreq_data *wrqu,
+			   char *extra)
+{
+	PRTMP_ADAPTER   pAd = dev->ml_priv;
+	struct iw_pmksa *pPmksa = (struct iw_pmksa *)wrqu->data.pointer;
+	INT	CachedIdx = 0, idx = 0;
+
+	if (pPmksa == NULL)
+		return -EINVAL;
+
+	DBGPRINT(RT_DEBUG_TRACE ,("===> rt_ioctl_siwpmksa\n"));
+	switch(pPmksa->cmd)
+	{
+		case IW_PMKSA_FLUSH:
+			NdisZeroMemory(pAd->StaCfg.SavedPMK, sizeof(BSSID_INFO)*PMKID_NO);
+			DBGPRINT(RT_DEBUG_TRACE ,("rt_ioctl_siwpmksa - IW_PMKSA_FLUSH\n"));
+			break;
+		case IW_PMKSA_REMOVE:
+			for (CachedIdx = 0; CachedIdx < pAd->StaCfg.SavedPMKNum; CachedIdx++)
+			{
+		        // compare the BSSID
+		        if (NdisEqualMemory(pPmksa->bssid.sa_data, pAd->StaCfg.SavedPMK[CachedIdx].BSSID, MAC_ADDR_LEN))
+		        {
+		        	NdisZeroMemory(pAd->StaCfg.SavedPMK[CachedIdx].BSSID, MAC_ADDR_LEN);
+					NdisZeroMemory(pAd->StaCfg.SavedPMK[CachedIdx].PMKID, 16);
+					for (idx = CachedIdx; idx < (pAd->StaCfg.SavedPMKNum - 1); idx++)
+					{
+						NdisMoveMemory(&pAd->StaCfg.SavedPMK[idx].BSSID[0], &pAd->StaCfg.SavedPMK[idx+1].BSSID[0], MAC_ADDR_LEN);
+						NdisMoveMemory(&pAd->StaCfg.SavedPMK[idx].PMKID[0], &pAd->StaCfg.SavedPMK[idx+1].PMKID[0], 16);
+					}
+					pAd->StaCfg.SavedPMKNum--;
+			        break;
+		        }
+	        }
+
+			DBGPRINT(RT_DEBUG_TRACE ,("rt_ioctl_siwpmksa - IW_PMKSA_REMOVE\n"));
+			break;
+		case IW_PMKSA_ADD:
+			for (CachedIdx = 0; CachedIdx < pAd->StaCfg.SavedPMKNum; CachedIdx++)
+			{
+		        // compare the BSSID
+		        if (NdisEqualMemory(pPmksa->bssid.sa_data, pAd->StaCfg.SavedPMK[CachedIdx].BSSID, MAC_ADDR_LEN))
+			        break;
+	        }
+
+	        // Found, replace it
+	        if (CachedIdx < PMKID_NO)
+	        {
+		        DBGPRINT(RT_DEBUG_OFF, ("Update PMKID, idx = %d\n", CachedIdx));
+		        NdisMoveMemory(&pAd->StaCfg.SavedPMK[CachedIdx].BSSID[0], pPmksa->bssid.sa_data, MAC_ADDR_LEN);
+				NdisMoveMemory(&pAd->StaCfg.SavedPMK[CachedIdx].PMKID[0], pPmksa->pmkid, 16);
+		        pAd->StaCfg.SavedPMKNum++;
+	        }
+	        // Not found, replace the last one
+	        else
+	        {
+		        // Randomly replace one
+		        CachedIdx = (pPmksa->bssid.sa_data[5] % PMKID_NO);
+		        DBGPRINT(RT_DEBUG_OFF, ("Update PMKID, idx = %d\n", CachedIdx));
+		        NdisMoveMemory(&pAd->StaCfg.SavedPMK[CachedIdx].BSSID[0], pPmksa->bssid.sa_data, MAC_ADDR_LEN);
+				NdisMoveMemory(&pAd->StaCfg.SavedPMK[CachedIdx].PMKID[0], pPmksa->pmkid, 16);
+	        }
+
+			DBGPRINT(RT_DEBUG_TRACE ,("rt_ioctl_siwpmksa - IW_PMKSA_ADD\n"));
+			break;
+		default:
+			DBGPRINT(RT_DEBUG_TRACE ,("rt_ioctl_siwpmksa - Unknow Command!!\n"));
+			break;
+	}
+
+	return 0;
+}
+#endif // #if WIRELESS_EXT > 17
+
+#ifdef DBG
+static int
+rt_private_ioctl_bbp(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra)
+			{
+	CHAR				*this_char;
+	CHAR				*value = NULL;
+	UCHAR				regBBP = 0;
+//	CHAR				arg[255]={0};
+	UINT32				bbpId;
+	UINT32				bbpValue;
+	BOOLEAN				bIsPrintAllBBP = FALSE;
+	INT					Status = 0;
+    PRTMP_ADAPTER       pAdapter = dev->ml_priv;
+
+
+	memset(extra, 0x00, IW_PRIV_SIZE_MASK);
+
+	if (wrq->length > 1) //No parameters.
+				{
+		sprintf(extra, "\n");
+
+		//Parsing Read or Write
+		this_char = wrq->pointer;
+		DBGPRINT(RT_DEBUG_TRACE, ("this_char=%s\n", this_char));
+		if (!*this_char)
+			goto next;
+
+		if ((value = rtstrchr(this_char, '=')) != NULL)
+			*value++ = 0;
+
+		if (!value || !*value)
+		{ //Read
+			DBGPRINT(RT_DEBUG_TRACE, ("this_char=%s, value=%s\n", this_char, value));
+			if (sscanf(this_char, "%d", &(bbpId)) == 1)
+			{
+				if (bbpId <= 136)
+				{
+#ifdef RALINK_ATE
+					if (ATE_ON(pAdapter))
+					{
+						ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+					}
+					else
+#endif // RALINK_ATE //
+					{
+					RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+					}
+					sprintf(extra+strlen(extra), "R%02d[0x%02X]:%02X\n", bbpId, bbpId*2, regBBP);
+                    wrq->length = strlen(extra) + 1; // 1: size of '\0'
+					DBGPRINT(RT_DEBUG_TRACE, ("msg=%s\n", extra));
+				}
+				else
+				{//Invalid parametes, so default printk all bbp
+					bIsPrintAllBBP = TRUE;
+					goto next;
+				}
+			}
+			else
+			{ //Invalid parametes, so default printk all bbp
+				bIsPrintAllBBP = TRUE;
+				goto next;
+			}
+		}
+		else
+		{ //Write
+			if ((sscanf(this_char, "%d", &(bbpId)) == 1) && (sscanf(value, "%x", &(bbpValue)) == 1))
+			{
+				if (bbpId <= 136)
+				{
+#ifdef RALINK_ATE
+					if (ATE_ON(pAdapter))
+					{
+						ATE_BBP_IO_WRITE8_BY_REG_ID(pAdapter, bbpId, bbpValue);
+						//Read it back for showing
+						ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+					}
+					else
+#endif // RALINK_ATE //
+					{
+					    RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, bbpId, bbpValue);
+    					//Read it back for showing
+    					RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+			}
+					sprintf(extra+strlen(extra), "R%02d[0x%02X]:%02X\n", bbpId, bbpId*2, regBBP);
+                    wrq->length = strlen(extra) + 1; // 1: size of '\0'
+					DBGPRINT(RT_DEBUG_TRACE, ("msg=%s\n", extra));
+				}
+				else
+				{//Invalid parametes, so default printk all bbp
+					bIsPrintAllBBP = TRUE;
+					goto next;
+				}
+			}
+			else
+			{ //Invalid parametes, so default printk all bbp
+				bIsPrintAllBBP = TRUE;
+				goto next;
+			}
+		}
+		}
+	else
+		bIsPrintAllBBP = TRUE;
+
+next:
+	if (bIsPrintAllBBP)
+	{
+		memset(extra, 0x00, IW_PRIV_SIZE_MASK);
+		sprintf(extra, "\n");
+		for (bbpId = 0; bbpId <= 136; bbpId++)
+		{
+		    if (strlen(extra) >= (IW_PRIV_SIZE_MASK - 10))
+                break;
+#ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+			}
+			else
+#endif // RALINK_ATE //
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+			sprintf(extra+strlen(extra), "R%02d[0x%02X]:%02X    ", bbpId, bbpId*2, regBBP);
+			if (bbpId%5 == 4)
+				sprintf(extra+strlen(extra), "\n");
+		}
+
+        wrq->length = strlen(extra) + 1; // 1: size of '\0'
+        DBGPRINT(RT_DEBUG_TRACE, ("wrq->length = %d\n", wrq->length));
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<==rt_private_ioctl_bbp\n\n"));
+
+    return Status;
+}
+#endif // DBG //
+
+int rt_ioctl_siwrate(struct net_device *dev,
+			struct iw_request_info *info,
+			union iwreq_data *wrqu, char *extra)
+{
+    PRTMP_ADAPTER   pAd = dev->ml_priv;
+    UINT32          rate = wrqu->bitrate.value, fixed = wrqu->bitrate.fixed;
+
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("rt_ioctl_siwrate::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+    DBGPRINT(RT_DEBUG_TRACE, ("rt_ioctl_siwrate::(rate = %d, fixed = %d)\n", rate, fixed));
+    /* rate = -1 => auto rate
+       rate = X, fixed = 1 => (fixed rate X)
+    */
+    if (rate == -1)
+    {
+		//Auto Rate
+		pAd->StaCfg.DesiredTransmitSetting.field.MCS = MCS_AUTO;
+		pAd->StaCfg.bAutoTxRateSwitch = TRUE;
+		if ((pAd->CommonCfg.PhyMode <= PHY_11G) ||
+		    (pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE <= MODE_OFDM))
+			RTMPSetDesiredRates(pAd, -1);
+
+#ifdef DOT11_N_SUPPORT
+		SetCommonHT(pAd);
+#endif // DOT11_N_SUPPORT //
+    }
+    else
+    {
+        if (fixed)
+        {
+        	pAd->StaCfg.bAutoTxRateSwitch = FALSE;
+            if ((pAd->CommonCfg.PhyMode <= PHY_11G) ||
+                (pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE <= MODE_OFDM))
+                RTMPSetDesiredRates(pAd, rate);
+            else
+            {
+                pAd->StaCfg.DesiredTransmitSetting.field.MCS = MCS_AUTO;
+#ifdef DOT11_N_SUPPORT
+                SetCommonHT(pAd);
+#endif // DOT11_N_SUPPORT //
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("rt_ioctl_siwrate::(HtMcs=%d)\n",pAd->StaCfg.DesiredTransmitSetting.field.MCS));
+        }
+        else
+        {
+            // TODO: rate = X, fixed = 0 => (rates <= X)
+            return -EOPNOTSUPP;
+        }
+    }
+
+    return 0;
+}
+
+int rt_ioctl_giwrate(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+    PRTMP_ADAPTER   pAd = dev->ml_priv;
+    int rate_index = 0, rate_count = 0;
+    HTTRANSMIT_SETTING ht_setting;
+    __s32 ralinkrate[] =
+	{2,  4,   11,  22, // CCK
+	12, 18,   24,  36, 48, 72, 96, 108, // OFDM
+	13, 26,   39,  52,  78, 104, 117, 130, 26,  52,  78, 104, 156, 208, 234, 260, // 20MHz, 800ns GI, MCS: 0 ~ 15
+	39, 78,  117, 156, 234, 312, 351, 390,										  // 20MHz, 800ns GI, MCS: 16 ~ 23
+	27, 54,   81, 108, 162, 216, 243, 270, 54, 108, 162, 216, 324, 432, 486, 540, // 40MHz, 800ns GI, MCS: 0 ~ 15
+	81, 162, 243, 324, 486, 648, 729, 810,										  // 40MHz, 800ns GI, MCS: 16 ~ 23
+	14, 29,   43,  57,  87, 115, 130, 144, 29, 59,   87, 115, 173, 230, 260, 288, // 20MHz, 400ns GI, MCS: 0 ~ 15
+	43, 87,  130, 173, 260, 317, 390, 433,										  // 20MHz, 400ns GI, MCS: 16 ~ 23
+	30, 60,   90, 120, 180, 240, 270, 300, 60, 120, 180, 240, 360, 480, 540, 600, // 40MHz, 400ns GI, MCS: 0 ~ 15
+	90, 180, 270, 360, 540, 720, 810, 900};										  // 40MHz, 400ns GI, MCS: 16 ~ 23
+
+    rate_count = sizeof(ralinkrate)/sizeof(__s32);
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+    if ((pAd->StaCfg.bAutoTxRateSwitch == FALSE) &&
+        (INFRA_ON(pAd)) &&
+        ((pAd->CommonCfg.PhyMode <= PHY_11G) || (pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE <= MODE_OFDM)))
+        ht_setting.word = pAd->StaCfg.HTPhyMode.word;
+    else
+        ht_setting.word = pAd->MacTab.Content[BSSID_WCID].HTPhyMode.word;
+
+#ifdef DOT11_N_SUPPORT
+    if (ht_setting.field.MODE >= MODE_HTMIX)
+    {
+//    	rate_index = 12 + ((UCHAR)ht_setting.field.BW *16) + ((UCHAR)ht_setting.field.ShortGI *32) + ((UCHAR)ht_setting.field.MCS);
+    	rate_index = 12 + ((UCHAR)ht_setting.field.BW *24) + ((UCHAR)ht_setting.field.ShortGI *48) + ((UCHAR)ht_setting.field.MCS);
+    }
+    else
+#endif // DOT11_N_SUPPORT //
+    if (ht_setting.field.MODE == MODE_OFDM)
+    	rate_index = (UCHAR)(ht_setting.field.MCS) + 4;
+    else if (ht_setting.field.MODE == MODE_CCK)
+    	rate_index = (UCHAR)(ht_setting.field.MCS);
+
+    if (rate_index < 0)
+        rate_index = 0;
+
+    if (rate_index > rate_count)
+        rate_index = rate_count;
+
+    wrqu->bitrate.value = ralinkrate[rate_index] * 500000;
+    wrqu->bitrate.disabled = 0;
+
+    return 0;
+}
+
+static const iw_handler rt_handler[] =
+{
+	(iw_handler) NULL,			            /* SIOCSIWCOMMIT */
+	(iw_handler) rt_ioctl_giwname,			/* SIOCGIWNAME   */
+	(iw_handler) NULL,			            /* SIOCSIWNWID   */
+	(iw_handler) NULL,			            /* SIOCGIWNWID   */
+	(iw_handler) rt_ioctl_siwfreq,		    /* SIOCSIWFREQ   */
+	(iw_handler) rt_ioctl_giwfreq,		    /* SIOCGIWFREQ   */
+	(iw_handler) rt_ioctl_siwmode,		    /* SIOCSIWMODE   */
+	(iw_handler) rt_ioctl_giwmode,		    /* SIOCGIWMODE   */
+	(iw_handler) NULL,		                /* SIOCSIWSENS   */
+	(iw_handler) NULL,		                /* SIOCGIWSENS   */
+	(iw_handler) NULL /* not used */,		/* SIOCSIWRANGE  */
+	(iw_handler) rt_ioctl_giwrange,		    /* SIOCGIWRANGE  */
+	(iw_handler) NULL /* not used */,		/* SIOCSIWPRIV   */
+	(iw_handler) NULL /* kernel code */,    /* SIOCGIWPRIV   */
+	(iw_handler) NULL /* not used */,		/* SIOCSIWSTATS  */
+	(iw_handler) rt28xx_get_wireless_stats /* kernel code */,    /* SIOCGIWSTATS  */
+	(iw_handler) NULL,		                /* SIOCSIWSPY    */
+	(iw_handler) NULL,		                /* SIOCGIWSPY    */
+	(iw_handler) NULL,				        /* SIOCSIWTHRSPY */
+	(iw_handler) NULL,				        /* SIOCGIWTHRSPY */
+	(iw_handler) rt_ioctl_siwap,            /* SIOCSIWAP     */
+	(iw_handler) rt_ioctl_giwap,		    /* SIOCGIWAP     */
+#ifdef SIOCSIWMLME
+	(iw_handler) rt_ioctl_siwmlme,	        /* SIOCSIWMLME   */
+#else
+	(iw_handler) NULL,				        /* SIOCSIWMLME */
+#endif // SIOCSIWMLME //
+	(iw_handler) rt_ioctl_iwaplist,		    /* SIOCGIWAPLIST */
+#ifdef SIOCGIWSCAN
+	(iw_handler) rt_ioctl_siwscan,		    /* SIOCSIWSCAN   */
+	(iw_handler) rt_ioctl_giwscan,		    /* SIOCGIWSCAN   */
+#else
+	(iw_handler) NULL,				        /* SIOCSIWSCAN   */
+	(iw_handler) NULL,				        /* SIOCGIWSCAN   */
+#endif /* SIOCGIWSCAN */
+	(iw_handler) rt_ioctl_siwessid,		    /* SIOCSIWESSID  */
+	(iw_handler) rt_ioctl_giwessid,		    /* SIOCGIWESSID  */
+	(iw_handler) rt_ioctl_siwnickn,		    /* SIOCSIWNICKN  */
+	(iw_handler) rt_ioctl_giwnickn,		    /* SIOCGIWNICKN  */
+	(iw_handler) NULL,				        /* -- hole --    */
+	(iw_handler) NULL,				        /* -- hole --    */
+	(iw_handler) rt_ioctl_siwrate,          /* SIOCSIWRATE   */
+	(iw_handler) rt_ioctl_giwrate,          /* SIOCGIWRATE   */
+	(iw_handler) rt_ioctl_siwrts,		    /* SIOCSIWRTS    */
+	(iw_handler) rt_ioctl_giwrts,		    /* SIOCGIWRTS    */
+	(iw_handler) rt_ioctl_siwfrag,		    /* SIOCSIWFRAG   */
+	(iw_handler) rt_ioctl_giwfrag,		    /* SIOCGIWFRAG   */
+	(iw_handler) NULL,		                /* SIOCSIWTXPOW  */
+	(iw_handler) NULL,		                /* SIOCGIWTXPOW  */
+	(iw_handler) NULL,		                /* SIOCSIWRETRY  */
+	(iw_handler) NULL,		                /* SIOCGIWRETRY  */
+	(iw_handler) rt_ioctl_siwencode,		/* SIOCSIWENCODE */
+	(iw_handler) rt_ioctl_giwencode,		/* SIOCGIWENCODE */
+	(iw_handler) NULL,		                /* SIOCSIWPOWER  */
+	(iw_handler) NULL,		                /* SIOCGIWPOWER  */
+	(iw_handler) NULL,						/* -- hole -- */
+	(iw_handler) NULL,						/* -- hole -- */
+#if WIRELESS_EXT > 17
+    (iw_handler) rt_ioctl_siwgenie,         /* SIOCSIWGENIE  */
+	(iw_handler) rt_ioctl_giwgenie,         /* SIOCGIWGENIE  */
+	(iw_handler) rt_ioctl_siwauth,		    /* SIOCSIWAUTH   */
+	(iw_handler) rt_ioctl_giwauth,		    /* SIOCGIWAUTH   */
+	(iw_handler) rt_ioctl_siwencodeext,	    /* SIOCSIWENCODEEXT */
+	(iw_handler) rt_ioctl_giwencodeext,		/* SIOCGIWENCODEEXT */
+	(iw_handler) rt_ioctl_siwpmksa,         /* SIOCSIWPMKSA  */
+#endif
+};
+
+static const iw_handler rt_priv_handlers[] = {
+	(iw_handler) NULL, /* + 0x00 */
+	(iw_handler) NULL, /* + 0x01 */
+#ifndef CONFIG_AP_SUPPORT
+	(iw_handler) rt_ioctl_setparam, /* + 0x02 */
+#else
+	(iw_handler) NULL, /* + 0x02 */
+#endif // CONFIG_AP_SUPPORT //
+#ifdef DBG
+	(iw_handler) rt_private_ioctl_bbp, /* + 0x03 */
+#else
+	(iw_handler) NULL, /* + 0x03 */
+#endif
+	(iw_handler) NULL, /* + 0x04 */
+	(iw_handler) NULL, /* + 0x05 */
+	(iw_handler) NULL, /* + 0x06 */
+	(iw_handler) NULL, /* + 0x07 */
+	(iw_handler) NULL, /* + 0x08 */
+	(iw_handler) rt_private_get_statistics, /* + 0x09 */
+	(iw_handler) NULL, /* + 0x0A */
+	(iw_handler) NULL, /* + 0x0B */
+	(iw_handler) NULL, /* + 0x0C */
+	(iw_handler) NULL, /* + 0x0D */
+	(iw_handler) NULL, /* + 0x0E */
+	(iw_handler) NULL, /* + 0x0F */
+	(iw_handler) NULL, /* + 0x10 */
+	(iw_handler) rt_private_show, /* + 0x11 */
+    (iw_handler) NULL, /* + 0x12 */
+	(iw_handler) NULL, /* + 0x13 */
+	(iw_handler) NULL, /* + 0x15 */
+	(iw_handler) NULL, /* + 0x17 */
+	(iw_handler) NULL, /* + 0x18 */
+};
+
+const struct iw_handler_def rt28xx_iw_handler_def =
+{
+#define	N(a)	(sizeof (a) / sizeof (a[0]))
+	.standard	= (iw_handler *) rt_handler,
+	.num_standard	= sizeof(rt_handler) / sizeof(iw_handler),
+	.private	= (iw_handler *) rt_priv_handlers,
+	.num_private		= N(rt_priv_handlers),
+	.private_args	= (struct iw_priv_args *) privtab,
+	.num_private_args	= N(privtab),
+#if IW_HANDLER_VERSION >= 7
+    .get_wireless_stats = rt28xx_get_wireless_stats,
+#endif
+};
+
+INT RTMPSetInformation(
+    IN  PRTMP_ADAPTER pAdapter,
+    IN  OUT struct ifreq    *rq,
+    IN  INT                 cmd)
+{
+    struct iwreq                        *wrq = (struct iwreq *) rq;
+    NDIS_802_11_SSID                    Ssid;
+    NDIS_802_11_MAC_ADDRESS             Bssid;
+    RT_802_11_PHY_MODE                  PhyMode;
+    RT_802_11_STA_CONFIG                StaConfig;
+    NDIS_802_11_RATES                   aryRates;
+    RT_802_11_PREAMBLE                  Preamble;
+    NDIS_802_11_WEP_STATUS              WepStatus;
+    NDIS_802_11_AUTHENTICATION_MODE     AuthMode = Ndis802_11AuthModeMax;
+    NDIS_802_11_NETWORK_INFRASTRUCTURE  BssType;
+    NDIS_802_11_RTS_THRESHOLD           RtsThresh;
+    NDIS_802_11_FRAGMENTATION_THRESHOLD FragThresh;
+    NDIS_802_11_POWER_MODE              PowerMode;
+    PNDIS_802_11_KEY                    pKey = NULL;
+    PNDIS_802_11_WEP			        pWepKey =NULL;
+    PNDIS_802_11_REMOVE_KEY             pRemoveKey = NULL;
+    NDIS_802_11_CONFIGURATION           Config, *pConfig = NULL;
+    NDIS_802_11_NETWORK_TYPE            NetType;
+    ULONG                               Now;
+    UINT                                KeyIdx = 0;
+    INT                                 Status = NDIS_STATUS_SUCCESS, MaxPhyMode = PHY_11G;
+    ULONG                               PowerTemp;
+    BOOLEAN                             RadioState;
+    BOOLEAN                             StateMachineTouched = FALSE;
+#ifdef DOT11_N_SUPPORT
+	OID_SET_HT_PHYMODE					HT_PhyMode;	//11n ,kathy
+#endif // DOT11_N_SUPPORT //
+#ifdef WPA_SUPPLICANT_SUPPORT
+    PNDIS_802_11_PMKID                  pPmkId = NULL;
+    BOOLEAN				                IEEE8021xState = FALSE;
+    BOOLEAN				                IEEE8021x_required_keys = FALSE;
+    UCHAR                               wpa_supplicant_enable = 0;
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef SNMP_SUPPORT
+	TX_RTY_CFG_STRUC			tx_rty_cfg;
+	ULONG						ShortRetryLimit, LongRetryLimit;
+	UCHAR						ctmp;
+#endif // SNMP_SUPPORT //
+
+
+
+#ifdef DOT11_N_SUPPORT
+	MaxPhyMode = PHY_11N_5G;
+#endif // DOT11_N_SUPPORT //
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("-->RTMPSetInformation(),	0x%08x\n", cmd&0x7FFF));
+    switch(cmd & 0x7FFF) {
+        case RT_OID_802_11_COUNTRY_REGION:
+            if (wrq->u.data.length < sizeof(UCHAR))
+                Status = -EINVAL;
+			// Only avaliable when EEPROM not programming
+            else if (!(pAdapter->CommonCfg.CountryRegion & 0x80) && !(pAdapter->CommonCfg.CountryRegionForABand & 0x80))
+            {
+                ULONG   Country;
+                UCHAR	TmpPhy;
+
+				Status = copy_from_user(&Country, wrq->u.data.pointer, wrq->u.data.length);
+				pAdapter->CommonCfg.CountryRegion = (UCHAR)(Country & 0x000000FF);
+				pAdapter->CommonCfg.CountryRegionForABand = (UCHAR)((Country >> 8) & 0x000000FF);
+                TmpPhy = pAdapter->CommonCfg.PhyMode;
+				pAdapter->CommonCfg.PhyMode = 0xff;
+				// Build all corresponding channel information
+				RTMPSetPhyMode(pAdapter, TmpPhy);
+#ifdef DOT11_N_SUPPORT
+				SetCommonHT(pAdapter);
+#endif // DOT11_N_SUPPORT //
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_COUNTRY_REGION (A:%d  B/G:%d)\n", pAdapter->CommonCfg.CountryRegionForABand,
+				    pAdapter->CommonCfg.CountryRegion));
+            }
+            break;
+        case OID_802_11_BSSID_LIST_SCAN:
+ #ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+				break;
+			}
+#endif // RALINK_ATE //
+            Now = jiffies;
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_BSSID_LIST_SCAN, TxCnt = %d \n", pAdapter->RalinkCounters.LastOneSecTotalTxCount));
+
+            if (MONITOR_ON(pAdapter))
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! Driver is in Monitor Mode now !!!\n"));
+                break;
+            }
+
+			//Benson add 20080527, when radio off, sta don't need to scan
+			if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_RADIO_OFF))
+				break;
+
+			if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+			{
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! Driver is scanning now !!!\n"));
+				pAdapter->StaCfg.bScanReqIsFromWebUI = TRUE;
+				Status = NDIS_STATUS_SUCCESS;
+                break;
+            }
+
+			if (pAdapter->RalinkCounters.LastOneSecTotalTxCount > 100)
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! Link UP, ignore this set::OID_802_11_BSSID_LIST_SCAN\n"));
+				Status = NDIS_STATUS_SUCCESS;
+				pAdapter->StaCfg.ScanCnt = 99;		// Prevent auto scan triggered by this OID
+				break;
+            }
+
+            if ((OPSTATUS_TEST_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED)) &&
+				((pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
+				(pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+				(pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) ||
+				(pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)) &&
+                (pAdapter->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! Link UP, Port Not Secured! ignore this set::OID_802_11_BSSID_LIST_SCAN\n"));
+				Status = NDIS_STATUS_SUCCESS;
+				pAdapter->StaCfg.ScanCnt = 99;		// Prevent auto scan triggered by this OID
+				break;
+            }
+
+
+            if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+            {
+                RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+            }
+
+            // tell CNTL state machine to call NdisMSetInformationComplete() after completing
+            // this request, because this request is initiated by NDIS.
+            pAdapter->MlmeAux.CurrReqIsFromNdis = FALSE;
+            // Reset allowed scan retries
+            pAdapter->StaCfg.ScanCnt = 0;
+            pAdapter->StaCfg.LastScanTime = Now;
+
+			pAdapter->StaCfg.bScanReqIsFromWebUI = TRUE;
+            RTMP_SET_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS);
+            MlmeEnqueue(pAdapter,
+                        MLME_CNTL_STATE_MACHINE,
+                        OID_802_11_BSSID_LIST_SCAN,
+                        0,
+                        NULL);
+
+            Status = NDIS_STATUS_SUCCESS;
+            StateMachineTouched = TRUE;
+            break;
+        case OID_802_11_SSID:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_SSID))
+                Status = -EINVAL;
+            else
+            {
+            	PCHAR pSsidString = NULL;
+                Status = copy_from_user(&Ssid, wrq->u.data.pointer, wrq->u.data.length);
+
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_SSID (Len=%d,Ssid=%s)\n", Ssid.SsidLength, Ssid.Ssid));
+                if (Ssid.SsidLength > MAX_LEN_OF_SSID)
+                    Status = -EINVAL;
+                else
+                {
+                	if (Ssid.SsidLength == 0)
+                	{
+                		Set_SSID_Proc(pAdapter, "");
+                	}
+					else
+                	{
+	                	pSsidString = (CHAR *) kmalloc(MAX_LEN_OF_SSID+1, MEM_ALLOC_FLAG);
+						if (pSsidString)
+						{
+							NdisZeroMemory(pSsidString, MAX_LEN_OF_SSID+1);
+							NdisMoveMemory(pSsidString, Ssid.Ssid, Ssid.SsidLength);
+	                		Set_SSID_Proc(pAdapter, pSsidString);
+							kfree(pSsidString);
+						}
+						else
+							Status = -ENOMEM;
+                	}
+                }
+            }
+            break;
+        case OID_802_11_BSSID:
+#ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+				break;
+			}
+#endif // RALINK_ATE //
+            if (wrq->u.data.length != sizeof(NDIS_802_11_MAC_ADDRESS))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&Bssid, wrq->u.data.pointer, wrq->u.data.length);
+
+                // tell CNTL state machine to call NdisMSetInformationComplete() after completing
+                // this request, because this request is initiated by NDIS.
+                pAdapter->MlmeAux.CurrReqIsFromNdis = FALSE;
+
+				// Prevent to connect AP again in STAMlmePeriodicExec
+				pAdapter->MlmeAux.AutoReconnectSsidLen= 32;
+
+                // Reset allowed scan retries
+				pAdapter->StaCfg.ScanCnt = 0;
+
+                if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+                {
+                    RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+                    DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+                }
+                MlmeEnqueue(pAdapter,
+                            MLME_CNTL_STATE_MACHINE,
+                            OID_802_11_BSSID,
+                            sizeof(NDIS_802_11_MAC_ADDRESS),
+                            (VOID *)&Bssid);
+                Status = NDIS_STATUS_SUCCESS;
+                StateMachineTouched = TRUE;
+
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_BSSID %02x:%02x:%02x:%02x:%02x:%02x\n",
+                                        Bssid[0], Bssid[1], Bssid[2], Bssid[3], Bssid[4], Bssid[5]));
+            }
+            break;
+        case RT_OID_802_11_RADIO:
+            if (wrq->u.data.length != sizeof(BOOLEAN))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&RadioState, wrq->u.data.pointer, wrq->u.data.length);
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_RADIO (=%d)\n", RadioState));
+                if (pAdapter->StaCfg.bSwRadio != RadioState)
+                {
+                    pAdapter->StaCfg.bSwRadio = RadioState;
+                    if (pAdapter->StaCfg.bRadio != (pAdapter->StaCfg.bHwRadio && pAdapter->StaCfg.bSwRadio))
+                    {
+                        pAdapter->StaCfg.bRadio = (pAdapter->StaCfg.bHwRadio && pAdapter->StaCfg.bSwRadio);
+                        if (pAdapter->StaCfg.bRadio == TRUE)
+                        {
+                            MlmeRadioOn(pAdapter);
+                            // Update extra information
+							pAdapter->ExtraInfo = EXTRA_INFO_CLEAR;
+                        }
+                        else
+                        {
+                            MlmeRadioOff(pAdapter);
+                            // Update extra information
+							pAdapter->ExtraInfo = SW_RADIO_OFF;
+                        }
+                    }
+                }
+            }
+            break;
+        case RT_OID_802_11_PHY_MODE:
+            if (wrq->u.data.length != sizeof(RT_802_11_PHY_MODE))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&PhyMode, wrq->u.data.pointer, wrq->u.data.length);
+				if (PhyMode <= MaxPhyMode)
+				{
+                	RTMPSetPhyMode(pAdapter, PhyMode);
+#ifdef DOT11_N_SUPPORT
+					SetCommonHT(pAdapter);
+#endif // DOT11_N_SUPPORT //
+				}
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_PHY_MODE (=%d)\n", PhyMode));
+            }
+            break;
+        case RT_OID_802_11_STA_CONFIG:
+            if (wrq->u.data.length != sizeof(RT_802_11_STA_CONFIG))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&StaConfig, wrq->u.data.pointer, wrq->u.data.length);
+                pAdapter->CommonCfg.bEnableTxBurst = StaConfig.EnableTxBurst;
+                pAdapter->CommonCfg.UseBGProtection = StaConfig.UseBGProtection;
+                pAdapter->CommonCfg.bUseShortSlotTime = 1; // 2003-10-30 always SHORT SLOT capable
+                if ((pAdapter->CommonCfg.PhyMode != StaConfig.AdhocMode) &&
+					(StaConfig.AdhocMode <= MaxPhyMode))
+                {
+                    // allow dynamic change of "USE OFDM rate or not" in ADHOC mode
+                    // if setting changed, need to reset current TX rate as well as BEACON frame format
+                    if (pAdapter->StaCfg.BssType == BSS_ADHOC)
+                    {
+						pAdapter->CommonCfg.PhyMode = StaConfig.AdhocMode;
+                    	RTMPSetPhyMode(pAdapter, PhyMode);
+                        MlmeUpdateTxRates(pAdapter, FALSE, 0);
+                        MakeIbssBeacon(pAdapter);           // re-build BEACON frame
+                        AsicEnableIbssSync(pAdapter);   // copy to on-chip memory
+                    }
+                }
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_STA_CONFIG (Burst=%d, Protection=%ld,ShortSlot=%d\n",
+                                        pAdapter->CommonCfg.bEnableTxBurst,
+                                        pAdapter->CommonCfg.UseBGProtection,
+                                        pAdapter->CommonCfg.bUseShortSlotTime));
+            }
+            break;
+        case OID_802_11_DESIRED_RATES:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_RATES))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&aryRates, wrq->u.data.pointer, wrq->u.data.length);
+                NdisZeroMemory(pAdapter->CommonCfg.DesireRate, MAX_LEN_OF_SUPPORTED_RATES);
+                NdisMoveMemory(pAdapter->CommonCfg.DesireRate, &aryRates, sizeof(NDIS_802_11_RATES));
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_DESIRED_RATES (%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x)\n",
+                    pAdapter->CommonCfg.DesireRate[0],pAdapter->CommonCfg.DesireRate[1],
+                    pAdapter->CommonCfg.DesireRate[2],pAdapter->CommonCfg.DesireRate[3],
+                    pAdapter->CommonCfg.DesireRate[4],pAdapter->CommonCfg.DesireRate[5],
+                    pAdapter->CommonCfg.DesireRate[6],pAdapter->CommonCfg.DesireRate[7] ));
+                // Changing DesiredRate may affect the MAX TX rate we used to TX frames out
+                MlmeUpdateTxRates(pAdapter, FALSE, 0);
+            }
+            break;
+        case RT_OID_802_11_PREAMBLE:
+            if (wrq->u.data.length != sizeof(RT_802_11_PREAMBLE))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&Preamble, wrq->u.data.pointer, wrq->u.data.length);
+                if (Preamble == Rt802_11PreambleShort)
+                {
+                    pAdapter->CommonCfg.TxPreamble = Preamble;
+                    MlmeSetTxPreamble(pAdapter, Rt802_11PreambleShort);
+                }
+                else if ((Preamble == Rt802_11PreambleLong) || (Preamble == Rt802_11PreambleAuto))
+                {
+                    // if user wants AUTO, initialize to LONG here, then change according to AP's
+                    // capability upon association.
+                    pAdapter->CommonCfg.TxPreamble = Preamble;
+                    MlmeSetTxPreamble(pAdapter, Rt802_11PreambleLong);
+                }
+                else
+                {
+                    Status = -EINVAL;
+                    break;
+                }
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_PREAMBLE (=%d)\n", Preamble));
+            }
+            break;
+        case OID_802_11_WEP_STATUS:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_WEP_STATUS))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&WepStatus, wrq->u.data.pointer, wrq->u.data.length);
+                // Since TKIP, AES, WEP are all supported. It should not have any invalid setting
+                if (WepStatus <= Ndis802_11Encryption3KeyAbsent)
+                {
+                    if (pAdapter->StaCfg.WepStatus != WepStatus)
+                    {
+                        // Config has changed
+                        pAdapter->bConfigChanged = TRUE;
+                    }
+                    pAdapter->StaCfg.WepStatus     = WepStatus;
+                    pAdapter->StaCfg.OrigWepStatus = WepStatus;
+                    pAdapter->StaCfg.PairCipher    = WepStatus;
+                	pAdapter->StaCfg.GroupCipher   = WepStatus;
+                }
+                else
+                {
+                    Status  = -EINVAL;
+                    break;
+                }
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_WEP_STATUS (=%d)\n",WepStatus));
+            }
+            break;
+        case OID_802_11_AUTHENTICATION_MODE:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_AUTHENTICATION_MODE))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&AuthMode, wrq->u.data.pointer, wrq->u.data.length);
+                if (AuthMode > Ndis802_11AuthModeMax)
+                {
+                    Status  = -EINVAL;
+                    break;
+                }
+                else
+                {
+                    if (pAdapter->StaCfg.AuthMode != AuthMode)
+                    {
+                        // Config has changed
+                        pAdapter->bConfigChanged = TRUE;
+                    }
+                    pAdapter->StaCfg.AuthMode = AuthMode;
+                }
+                pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_AUTHENTICATION_MODE (=%d) \n",pAdapter->StaCfg.AuthMode));
+            }
+            break;
+        case OID_802_11_INFRASTRUCTURE_MODE:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_NETWORK_INFRASTRUCTURE))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&BssType, wrq->u.data.pointer, wrq->u.data.length);
+
+				if (BssType == Ndis802_11IBSS)
+					Set_NetworkType_Proc(pAdapter, "Adhoc");
+				else if (BssType == Ndis802_11Infrastructure)
+					Set_NetworkType_Proc(pAdapter, "Infra");
+				else if (BssType == Ndis802_11Monitor)
+					Set_NetworkType_Proc(pAdapter, "Monitor");
+				else
+				{
+					Status  = -EINVAL;
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_INFRASTRUCTURE_MODE (unknown)\n"));
+				}
+			}
+			break;
+	 case OID_802_11_REMOVE_WEP:
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_WEP\n"));
+            if (wrq->u.data.length != sizeof(NDIS_802_11_KEY_INDEX))
+            {
+				Status = -EINVAL;
+            }
+            else
+            {
+				KeyIdx = *(NDIS_802_11_KEY_INDEX *) wrq->u.data.pointer;
+
+				if (KeyIdx & 0x80000000)
+				{
+					// Should never set default bit when remove key
+					Status = -EINVAL;
+				}
+				else
+				{
+					KeyIdx = KeyIdx & 0x0fffffff;
+					if (KeyIdx >= 4){
+						Status = -EINVAL;
+					}
+					else
+					{
+						pAdapter->SharedKey[BSS0][KeyIdx].KeyLen = 0;
+						pAdapter->SharedKey[BSS0][KeyIdx].CipherAlg = CIPHER_NONE;
+						AsicRemoveSharedKeyEntry(pAdapter, 0, (UCHAR)KeyIdx);
+					}
+				}
+            }
+            break;
+        case RT_OID_802_11_RESET_COUNTERS:
+            NdisZeroMemory(&pAdapter->WlanCounters, sizeof(COUNTER_802_11));
+            NdisZeroMemory(&pAdapter->Counters8023, sizeof(COUNTER_802_3));
+            NdisZeroMemory(&pAdapter->RalinkCounters, sizeof(COUNTER_RALINK));
+            pAdapter->Counters8023.RxNoBuffer   = 0;
+			pAdapter->Counters8023.GoodReceives = 0;
+			pAdapter->Counters8023.RxNoBuffer   = 0;
+#ifdef RT2870
+			pAdapter->BulkOutComplete	= 0;
+			pAdapter->BulkOutCompleteOther= 0;
+			pAdapter->BulkOutCompleteCancel = 0;
+			pAdapter->BulkOutReq = 0;
+			pAdapter->BulkInReq= 0;
+			pAdapter->BulkInComplete = 0;
+			pAdapter->BulkInCompleteFail = 0;
+#endif // RT2870 //
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_RESET_COUNTERS \n"));
+            break;
+        case OID_802_11_RTS_THRESHOLD:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_RTS_THRESHOLD))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&RtsThresh, wrq->u.data.pointer, wrq->u.data.length);
+                if (RtsThresh > MAX_RTS_THRESHOLD)
+                    Status  = -EINVAL;
+                else
+                    pAdapter->CommonCfg.RtsThreshold = (USHORT)RtsThresh;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_RTS_THRESHOLD (=%ld)\n",RtsThresh));
+            break;
+        case OID_802_11_FRAGMENTATION_THRESHOLD:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_FRAGMENTATION_THRESHOLD))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&FragThresh, wrq->u.data.pointer, wrq->u.data.length);
+                pAdapter->CommonCfg.bUseZeroToDisableFragment = FALSE;
+                if (FragThresh > MAX_FRAG_THRESHOLD || FragThresh < MIN_FRAG_THRESHOLD)
+                {
+                    if (FragThresh == 0)
+                    {
+                        pAdapter->CommonCfg.FragmentThreshold = MAX_FRAG_THRESHOLD;
+                        pAdapter->CommonCfg.bUseZeroToDisableFragment = TRUE;
+                    }
+                    else
+                        Status  = -EINVAL;
+                }
+                else
+                    pAdapter->CommonCfg.FragmentThreshold = (USHORT)FragThresh;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_FRAGMENTATION_THRESHOLD (=%ld) \n",FragThresh));
+            break;
+        case OID_802_11_POWER_MODE:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_POWER_MODE))
+                Status = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&PowerMode, wrq->u.data.pointer, wrq->u.data.length);
+                if (PowerMode == Ndis802_11PowerModeCAM)
+                	Set_PSMode_Proc(pAdapter, "CAM");
+                else if (PowerMode == Ndis802_11PowerModeMAX_PSP)
+                	Set_PSMode_Proc(pAdapter, "Max_PSP");
+                else if (PowerMode == Ndis802_11PowerModeFast_PSP)
+					Set_PSMode_Proc(pAdapter, "Fast_PSP");
+                else if (PowerMode == Ndis802_11PowerModeLegacy_PSP)
+					Set_PSMode_Proc(pAdapter, "Legacy_PSP");
+                else
+                    Status = -EINVAL;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_POWER_MODE (=%d)\n",PowerMode));
+            break;
+         case RT_OID_802_11_TX_POWER_LEVEL_1:
+			if (wrq->u.data.length  < sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&PowerTemp, wrq->u.data.pointer, wrq->u.data.length);
+				if (PowerTemp > 100)
+					PowerTemp = 0xffffffff;  // AUTO
+				pAdapter->CommonCfg.TxPowerDefault = PowerTemp; //keep current setting.
+					pAdapter->CommonCfg.TxPowerPercentage = pAdapter->CommonCfg.TxPowerDefault;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_TX_POWER_LEVEL_1 (=%ld)\n", pAdapter->CommonCfg.TxPowerPercentage));
+			}
+	        break;
+		case OID_802_11_NETWORK_TYPE_IN_USE:
+			if (wrq->u.data.length != sizeof(NDIS_802_11_NETWORK_TYPE))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&NetType, wrq->u.data.pointer, wrq->u.data.length);
+
+				if (NetType == Ndis802_11DS)
+					RTMPSetPhyMode(pAdapter, PHY_11B);
+				else if (NetType == Ndis802_11OFDM24)
+					RTMPSetPhyMode(pAdapter, PHY_11BG_MIXED);
+				else if (NetType == Ndis802_11OFDM5)
+					RTMPSetPhyMode(pAdapter, PHY_11A);
+				else
+					Status = -EINVAL;
+#ifdef DOT11_N_SUPPORT
+				if (Status == NDIS_STATUS_SUCCESS)
+					SetCommonHT(pAdapter);
+#endif // DOT11_N_SUPPORT //
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_NETWORK_TYPE_IN_USE (=%d)\n",NetType));
+		    }
+			break;
+        // For WPA PSK PMK key
+        case RT_OID_802_11_ADD_WPA:
+            pKey = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+            if(pKey == NULL)
+            {
+                Status = -ENOMEM;
+                break;
+            }
+
+            Status = copy_from_user(pKey, wrq->u.data.pointer, wrq->u.data.length);
+            if (pKey->Length != wrq->u.data.length)
+            {
+                Status  = -EINVAL;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_WPA, Failed!!\n"));
+            }
+            else
+            {
+                if ((pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPAPSK) &&
+				    (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPA2PSK) &&
+				    (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPANone) )
+                {
+                    Status = -EOPNOTSUPP;
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_WPA, Failed!! [AuthMode != WPAPSK/WPA2PSK/WPANONE]\n"));
+                }
+                else if ((pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+						 (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK) ||
+						 (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPANone) )     // Only for WPA PSK mode
+				{
+                    NdisMoveMemory(pAdapter->StaCfg.PMK, &pKey->KeyMaterial, pKey->KeyLength);
+                    // Use RaConfig as PSK agent.
+                    // Start STA supplicant state machine
+                    if (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPANone)
+                        pAdapter->StaCfg.WpaState = SS_START;
+
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_WPA (id=0x%x, Len=%d-byte)\n", pKey->KeyIndex, pKey->KeyLength));
+                }
+                else
+                {
+                    pAdapter->StaCfg.WpaState = SS_NOTUSE;
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_WPA (id=0x%x, Len=%d-byte)\n", pKey->KeyIndex, pKey->KeyLength));
+                }
+            }
+            kfree(pKey);
+            break;
+        case OID_802_11_REMOVE_KEY:
+            pRemoveKey = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+            if(pRemoveKey == NULL)
+            {
+                Status = -ENOMEM;
+                break;
+            }
+
+            Status = copy_from_user(pRemoveKey, wrq->u.data.pointer, wrq->u.data.length);
+            if (pRemoveKey->Length != wrq->u.data.length)
+            {
+                Status  = -EINVAL;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY, Failed!!\n"));
+            }
+            else
+            {
+                if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+                {
+                    RTMPWPARemoveKeyProc(pAdapter, pRemoveKey);
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY, Remove WPA Key!!\n"));
+                }
+                else
+                {
+                    KeyIdx = pRemoveKey->KeyIndex;
+
+                    if (KeyIdx & 0x80000000)
+                    {
+                        // Should never set default bit when remove key
+                        Status  = -EINVAL;
+                        DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY, Failed!!(Should never set default bit when remove key)\n"));
+                    }
+                    else
+                    {
+                        KeyIdx = KeyIdx & 0x0fffffff;
+                        if (KeyIdx > 3)
+                        {
+                            Status  = -EINVAL;
+                            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY, Failed!!(KeyId[%d] out of range)\n", KeyIdx));
+                        }
+                        else
+                        {
+                            pAdapter->SharedKey[BSS0][KeyIdx].KeyLen = 0;
+                            pAdapter->SharedKey[BSS0][KeyIdx].CipherAlg = CIPHER_NONE;
+                            AsicRemoveSharedKeyEntry(pAdapter, 0, (UCHAR)KeyIdx);
+                            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY (id=0x%x, Len=%d-byte)\n", pRemoveKey->KeyIndex, pRemoveKey->Length));
+                        }
+                    }
+                }
+            }
+            kfree(pRemoveKey);
+            break;
+        // New for WPA
+        case OID_802_11_ADD_KEY:
+            pKey = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+            if(pKey == NULL)
+            {
+                Status = -ENOMEM;
+                break;
+            }
+            Status = copy_from_user(pKey, wrq->u.data.pointer, wrq->u.data.length);
+            if (pKey->Length != wrq->u.data.length)
+            {
+                Status  = -EINVAL;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_KEY, Failed!!\n"));
+            }
+            else
+            {
+                RTMPAddKey(pAdapter, pKey);
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_KEY (id=0x%x, Len=%d-byte)\n", pKey->KeyIndex, pKey->KeyLength));
+            }
+            kfree(pKey);
+            break;
+        case OID_802_11_CONFIGURATION:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_CONFIGURATION))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&Config, wrq->u.data.pointer, wrq->u.data.length);
+                pConfig = &Config;
+
+                if ((pConfig->BeaconPeriod >= 20) && (pConfig->BeaconPeriod <=400))
+                     pAdapter->CommonCfg.BeaconPeriod = (USHORT) pConfig->BeaconPeriod;
+
+                pAdapter->StaActive.AtimWin = (USHORT) pConfig->ATIMWindow;
+                MAP_KHZ_TO_CHANNEL_ID(pConfig->DSConfig, pAdapter->CommonCfg.Channel);
+                //
+				// Save the channel on MlmeAux for CntlOidRTBssidProc used.
+				//
+				pAdapter->MlmeAux.Channel = pAdapter->CommonCfg.Channel;
+
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_CONFIGURATION (BeacnPeriod=%ld,AtimW=%ld,Ch=%d)\n",
+                    pConfig->BeaconPeriod, pConfig->ATIMWindow, pAdapter->CommonCfg.Channel));
+                // Config has changed
+                pAdapter->bConfigChanged = TRUE;
+            }
+            break;
+#ifdef DOT11_N_SUPPORT
+		case RT_OID_802_11_SET_HT_PHYMODE:
+			if (wrq->u.data.length	!= sizeof(OID_SET_HT_PHYMODE))
+				Status = -EINVAL;
+			else
+			{
+			    POID_SET_HT_PHYMODE	pHTPhyMode = &HT_PhyMode;
+
+				Status = copy_from_user(&HT_PhyMode, wrq->u.data.pointer, wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::pHTPhyMode	(PhyMode = %d,TransmitNo = %d, HtMode =	%d,	ExtOffset =	%d , MCS = %d, BW =	%d,	STBC = %d, SHORTGI = %d) \n",
+				pHTPhyMode->PhyMode, pHTPhyMode->TransmitNo,pHTPhyMode->HtMode,pHTPhyMode->ExtOffset,
+				pHTPhyMode->MCS, pHTPhyMode->BW, pHTPhyMode->STBC,	pHTPhyMode->SHORTGI));
+				if (pAdapter->CommonCfg.PhyMode	>= PHY_11ABGN_MIXED)
+					RTMPSetHT(pAdapter,	pHTPhyMode);
+			}
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_HT_PHYMODE(MCS=%d,BW=%d,SGI=%d,STBC=%d)\n",
+				pAdapter->StaCfg.HTPhyMode.field.MCS, pAdapter->StaCfg.HTPhyMode.field.BW, pAdapter->StaCfg.HTPhyMode.field.ShortGI,
+				pAdapter->StaCfg.HTPhyMode.field.STBC));
+			break;
+#endif // DOT11_N_SUPPORT //
+		case RT_OID_802_11_SET_APSD_SETTING:
+			if (wrq->u.data.length != sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				ULONG apsd ;
+				Status = copy_from_user(&apsd, wrq->u.data.pointer,	wrq->u.data.length);
+
+				/*-------------------------------------------------------------------
+				|B31~B7	|	B6~B5	 |	 B4	 |	 B3	 |	B2	 |	B1	 |	   B0		|
+				---------------------------------------------------------------------
+				| Rsvd	| Max SP Len | AC_VO | AC_VI | AC_BK | AC_BE | APSD	Capable	|
+				---------------------------------------------------------------------*/
+				pAdapter->CommonCfg.bAPSDCapable = (apsd & 0x00000001) ? TRUE :	FALSE;
+				pAdapter->CommonCfg.bAPSDAC_BE = ((apsd	& 0x00000002) >> 1)	? TRUE : FALSE;
+				pAdapter->CommonCfg.bAPSDAC_BK = ((apsd	& 0x00000004) >> 2)	? TRUE : FALSE;
+				pAdapter->CommonCfg.bAPSDAC_VI = ((apsd	& 0x00000008) >> 3)	? TRUE : FALSE;
+				pAdapter->CommonCfg.bAPSDAC_VO = ((apsd	& 0x00000010) >> 4)	? TRUE : FALSE;
+				pAdapter->CommonCfg.MaxSPLength	= (UCHAR)((apsd	& 0x00000060) >> 5);
+
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_APSD_SETTING (apsd=0x%lx, APSDCap=%d, [BE,BK,VI,VO]=[%d/%d/%d/%d],	MaxSPLen=%d)\n", apsd, pAdapter->CommonCfg.bAPSDCapable,
+					pAdapter->CommonCfg.bAPSDAC_BE,	pAdapter->CommonCfg.bAPSDAC_BK,	pAdapter->CommonCfg.bAPSDAC_VI,	pAdapter->CommonCfg.bAPSDAC_VO,	pAdapter->CommonCfg.MaxSPLength));
+			}
+			break;
+
+		case RT_OID_802_11_SET_APSD_PSM:
+			if (wrq->u.data.length	!= sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				// Driver needs	to notify AP when PSM changes
+				Status = copy_from_user(&pAdapter->CommonCfg.bAPSDForcePowerSave, wrq->u.data.pointer, wrq->u.data.length);
+				if (pAdapter->CommonCfg.bAPSDForcePowerSave	!= pAdapter->StaCfg.Psm)
+				{
+					MlmeSetPsmBit(pAdapter,	pAdapter->CommonCfg.bAPSDForcePowerSave);
+					RTMPSendNullFrame(pAdapter,	pAdapter->CommonCfg.TxRate,	TRUE);
+				}
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_APSD_PSM (bAPSDForcePowerSave:%d)\n",	pAdapter->CommonCfg.bAPSDForcePowerSave));
+			}
+			break;
+#ifdef QOS_DLS_SUPPORT
+		case RT_OID_802_11_SET_DLS:
+			if (wrq->u.data.length != sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				BOOLEAN	oldvalue = pAdapter->CommonCfg.bDLSCapable;
+				Status = copy_from_user(&pAdapter->CommonCfg.bDLSCapable, wrq->u.data.pointer, wrq->u.data.length);
+				if (oldvalue &&	!pAdapter->CommonCfg.bDLSCapable)
+				{
+					int	i;
+					// tear	down local dls table entry
+					for	(i=0; i<MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+					{
+						if (pAdapter->StaCfg.DLSEntry[i].Valid && (pAdapter->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+						{
+							pAdapter->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+							pAdapter->StaCfg.DLSEntry[i].Valid	= FALSE;
+							RTMPSendDLSTearDownFrame(pAdapter, pAdapter->StaCfg.DLSEntry[i].MacAddr);
+						}
+					}
+
+					// tear	down peer dls table	entry
+					for	(i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+					{
+						if (pAdapter->StaCfg.DLSEntry[i].Valid && (pAdapter->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+						{
+							pAdapter->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+							pAdapter->StaCfg.DLSEntry[i].Valid	= FALSE;
+							RTMPSendDLSTearDownFrame(pAdapter, pAdapter->StaCfg.DLSEntry[i].MacAddr);
+						}
+					}
+				}
+
+				DBGPRINT(RT_DEBUG_TRACE,("Set::RT_OID_802_11_SET_DLS (=%d)\n", pAdapter->CommonCfg.bDLSCapable));
+			}
+			break;
+
+		case RT_OID_802_11_SET_DLS_PARAM:
+			if (wrq->u.data.length	!= sizeof(RT_802_11_DLS_UI))
+				Status = -EINVAL;
+			else
+			{
+				RT_802_11_DLS	Dls;
+
+				NdisZeroMemory(&Dls, sizeof(RT_802_11_DLS));
+				RTMPMoveMemory(&Dls, wrq->u.data.pointer, sizeof(RT_802_11_DLS_UI));
+				MlmeEnqueue(pAdapter,
+							MLME_CNTL_STATE_MACHINE,
+							RT_OID_802_11_SET_DLS_PARAM,
+							sizeof(RT_802_11_DLS),
+							&Dls);
+				DBGPRINT(RT_DEBUG_TRACE,("Set::RT_OID_802_11_SET_DLS_PARAM \n"));
+			}
+			break;
+#endif // QOS_DLS_SUPPORT //
+		case RT_OID_802_11_SET_WMM:
+			if (wrq->u.data.length	!= sizeof(BOOLEAN))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&pAdapter->CommonCfg.bWmmCapable, wrq->u.data.pointer, wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_WMM (=%d)	\n", pAdapter->CommonCfg.bWmmCapable));
+			}
+			break;
+
+		case OID_802_11_DISASSOCIATE:
+#ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+				break;
+			}
+#endif // RALINK_ATE //
+			//
+			// Set NdisRadioStateOff to	TRUE, instead of called	MlmeRadioOff.
+			// Later on, NDIS_802_11_BSSID_LIST_EX->NumberOfItems should be	0
+			// when	query OID_802_11_BSSID_LIST.
+			//
+			// TRUE:  NumberOfItems	will set to	0.
+			// FALSE: NumberOfItems	no change.
+			//
+			pAdapter->CommonCfg.NdisRadioStateOff =	TRUE;
+			// Set to immediately send the media disconnect	event
+			pAdapter->MlmeAux.CurrReqIsFromNdis	= TRUE;
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_DISASSOCIATE	\n"));
+
+			if (INFRA_ON(pAdapter))
+			{
+				if (pAdapter->Mlme.CntlMachine.CurrState !=	CNTL_IDLE)
+				{
+					RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+					DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME	busy, reset	MLME state machine !!!\n"));
+				}
+
+				MlmeEnqueue(pAdapter,
+					MLME_CNTL_STATE_MACHINE,
+					OID_802_11_DISASSOCIATE,
+					0,
+					NULL);
+
+				StateMachineTouched	= TRUE;
+			}
+			break;
+
+#ifdef DOT11_N_SUPPORT
+		case RT_OID_802_11_SET_IMME_BA_CAP:
+				if (wrq->u.data.length != sizeof(OID_BACAP_STRUC))
+					Status = -EINVAL;
+				else
+				{
+					OID_BACAP_STRUC Orde ;
+					Status = copy_from_user(&Orde, wrq->u.data.pointer, wrq->u.data.length);
+					if (Orde.Policy > BA_NOTUSE)
+					{
+						Status = NDIS_STATUS_INVALID_DATA;
+					}
+					else if (Orde.Policy == BA_NOTUSE)
+					{
+						pAdapter->CommonCfg.BACapability.field.Policy = BA_NOTUSE;
+						pAdapter->CommonCfg.BACapability.field.MpduDensity = Orde.MpduDensity;
+						pAdapter->CommonCfg.DesiredHtPhy.MpduDensity = Orde.MpduDensity;
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduEnable = Orde.AmsduEnable;
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduSize= Orde.AmsduSize;
+						pAdapter->CommonCfg.DesiredHtPhy.MimoPs= Orde.MMPSmode;
+						pAdapter->CommonCfg.BACapability.field.MMPSmode = Orde.MMPSmode;
+						// UPdata to HT IE
+						pAdapter->CommonCfg.HtCapability.HtCapInfo.MimoPs = Orde.MMPSmode;
+						pAdapter->CommonCfg.HtCapability.HtCapInfo.AMsduSize = Orde.AmsduSize;
+						pAdapter->CommonCfg.HtCapability.HtCapParm.MpduDensity = Orde.MpduDensity;
+					}
+					else
+					{
+                        pAdapter->CommonCfg.BACapability.field.AutoBA = Orde.AutoBA;
+						pAdapter->CommonCfg.BACapability.field.Policy = IMMED_BA; // we only support immediate BA.
+						pAdapter->CommonCfg.BACapability.field.MpduDensity = Orde.MpduDensity;
+						pAdapter->CommonCfg.DesiredHtPhy.MpduDensity = Orde.MpduDensity;
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduEnable = Orde.AmsduEnable;
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduSize= Orde.AmsduSize;
+						pAdapter->CommonCfg.DesiredHtPhy.MimoPs = Orde.MMPSmode;
+						pAdapter->CommonCfg.BACapability.field.MMPSmode = Orde.MMPSmode;
+
+						// UPdata to HT IE
+						pAdapter->CommonCfg.HtCapability.HtCapInfo.MimoPs = Orde.MMPSmode;
+						pAdapter->CommonCfg.HtCapability.HtCapInfo.AMsduSize = Orde.AmsduSize;
+						pAdapter->CommonCfg.HtCapability.HtCapParm.MpduDensity = Orde.MpduDensity;
+
+						if (pAdapter->CommonCfg.BACapability.field.RxBAWinLimit > MAX_RX_REORDERBUF)
+							pAdapter->CommonCfg.BACapability.field.RxBAWinLimit = MAX_RX_REORDERBUF;
+
+					}
+
+					pAdapter->CommonCfg.REGBACapability.word = pAdapter->CommonCfg.BACapability.word;
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::(Orde.AutoBA = %d) (Policy=%d)(ReBAWinLimit=%d)(TxBAWinLimit=%d)(AutoMode=%d)\n",Orde.AutoBA, pAdapter->CommonCfg.BACapability.field.Policy,
+						pAdapter->CommonCfg.BACapability.field.RxBAWinLimit,pAdapter->CommonCfg.BACapability.field.TxBAWinLimit, pAdapter->CommonCfg.BACapability.field.AutoBA));
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::(MimoPs = %d)(AmsduEnable = %d) (AmsduSize=%d)(MpduDensity=%d)\n",pAdapter->CommonCfg.DesiredHtPhy.MimoPs, pAdapter->CommonCfg.DesiredHtPhy.AmsduEnable,
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduSize, pAdapter->CommonCfg.DesiredHtPhy.MpduDensity));
+				}
+
+				break;
+		case RT_OID_802_11_ADD_IMME_BA:
+			DBGPRINT(RT_DEBUG_TRACE, (" Set :: RT_OID_802_11_ADD_IMME_BA \n"));
+			if (wrq->u.data.length != sizeof(OID_ADD_BA_ENTRY))
+					Status = -EINVAL;
+			else
+			{
+				UCHAR		        index;
+				OID_ADD_BA_ENTRY    BA;
+				MAC_TABLE_ENTRY     *pEntry;
+
+				Status = copy_from_user(&BA, wrq->u.data.pointer, wrq->u.data.length);
+				if (BA.TID > 15)
+				{
+					Status = NDIS_STATUS_INVALID_DATA;
+					break;
+				}
+				else
+				{
+					//BATableInsertEntry
+					//As ad-hoc mode, BA pair is not limited to only BSSID. so add via OID.
+					index = BA.TID;
+					// in ad hoc mode, when adding BA pair, we should insert this entry into MACEntry too
+					pEntry = MacTableLookup(pAdapter, BA.MACAddr);
+					if (!pEntry)
+					{
+						DBGPRINT(RT_DEBUG_TRACE, ("RT_OID_802_11_ADD_IMME_BA. break on no connection.----:%x:%x\n", BA.MACAddr[4], BA.MACAddr[5]));
+						break;
+					}
+					if (BA.IsRecipient == FALSE)
+					{
+					    if (pEntry->bIAmBadAtheros == TRUE)
+							pAdapter->CommonCfg.BACapability.field.RxBAWinLimit = 0x10;
+
+						BAOriSessionSetUp(pAdapter, pEntry, index, 0, 100, TRUE);
+					}
+					else
+					{
+						//BATableInsertEntry(pAdapter, pEntry->Aid, BA.MACAddr, 0, 0xffff, BA.TID, BA.nMSDU, BA.IsRecipient);
+					}
+
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_IMME_BA. Rec = %d. Mac = %x:%x:%x:%x:%x:%x . \n",
+						BA.IsRecipient, BA.MACAddr[0], BA.MACAddr[1], BA.MACAddr[2], BA.MACAddr[2]
+						, BA.MACAddr[4], BA.MACAddr[5]));
+				}
+			}
+			break;
+
+		case RT_OID_802_11_TEAR_IMME_BA:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set :: RT_OID_802_11_TEAR_IMME_BA \n"));
+			if (wrq->u.data.length != sizeof(OID_ADD_BA_ENTRY))
+					Status = -EINVAL;
+			else
+			{
+				POID_ADD_BA_ENTRY	pBA;
+				MAC_TABLE_ENTRY *pEntry;
+
+				pBA = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+
+				if (pBA == NULL)
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("Set :: RT_OID_802_11_TEAR_IMME_BA kmalloc() can't allocate enough memory\n"));
+					Status = NDIS_STATUS_FAILURE;
+				}
+				else
+				{
+					Status = copy_from_user(pBA, wrq->u.data.pointer, wrq->u.data.length);
+					DBGPRINT(RT_DEBUG_TRACE, ("Set :: RT_OID_802_11_TEAR_IMME_BA(TID=%d, bAllTid=%d)\n", pBA->TID, pBA->bAllTid));
+
+					if (!pBA->bAllTid && (pBA->TID > NUM_OF_TID))
+					{
+						Status = NDIS_STATUS_INVALID_DATA;
+						break;
+					}
+
+					if (pBA->IsRecipient == FALSE)
+					{
+						pEntry = MacTableLookup(pAdapter, pBA->MACAddr);
+						DBGPRINT(RT_DEBUG_TRACE, (" pBA->IsRecipient == FALSE\n"));
+						if (pEntry)
+						{
+							DBGPRINT(RT_DEBUG_TRACE, (" pBA->pEntry\n"));
+							BAOriSessionTearDown(pAdapter, pEntry->Aid, pBA->TID, FALSE, TRUE);
+						}
+						else
+							DBGPRINT(RT_DEBUG_TRACE, ("Set :: Not found pEntry \n"));
+					}
+					else
+					{
+						pEntry = MacTableLookup(pAdapter, pBA->MACAddr);
+						if (pEntry)
+						{
+							BARecSessionTearDown( pAdapter, (UCHAR)pEntry->Aid, pBA->TID, TRUE);
+						}
+						else
+							DBGPRINT(RT_DEBUG_TRACE, ("Set :: Not found pEntry \n"));
+					}
+					kfree(pBA);
+				}
+            }
+            break;
+#endif // DOT11_N_SUPPORT //
+
+        // For WPA_SUPPLICANT to set static wep key
+    	case OID_802_11_ADD_WEP:
+    	    pWepKey = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+
+    	    if(pWepKey == NULL)
+            {
+                Status = -ENOMEM;
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP, Failed!!\n"));
+                break;
+            }
+            Status = copy_from_user(pWepKey, wrq->u.data.pointer, wrq->u.data.length);
+            if (Status)
+            {
+                Status  = -EINVAL;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP, Failed (length mismatch)!!\n"));
+            }
+            else
+            {
+		        KeyIdx = pWepKey->KeyIndex & 0x0fffffff;
+                // KeyIdx must be 0 ~ 3
+                if (KeyIdx > 4)
+    			{
+                    Status  = -EINVAL;
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP, Failed (KeyIdx must be smaller than 4)!!\n"));
+                }
+                else
+                {
+                    UCHAR CipherAlg = 0;
+                    PUCHAR Key;
+
+                    // set key material and key length
+                    NdisZeroMemory(pAdapter->SharedKey[BSS0][KeyIdx].Key, 16);
+                    pAdapter->SharedKey[BSS0][KeyIdx].KeyLen = (UCHAR) pWepKey->KeyLength;
+                    NdisMoveMemory(pAdapter->SharedKey[BSS0][KeyIdx].Key, &pWepKey->KeyMaterial, pWepKey->KeyLength);
+
+                    switch(pWepKey->KeyLength)
+                    {
+                        case 5:
+                            CipherAlg = CIPHER_WEP64;
+                            break;
+                        case 13:
+                            CipherAlg = CIPHER_WEP128;
+                            break;
+                        default:
+                            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP, only support CIPHER_WEP64(len:5) & CIPHER_WEP128(len:13)!!\n"));
+                            Status = -EINVAL;
+                            break;
+                    }
+                    pAdapter->SharedKey[BSS0][KeyIdx].CipherAlg = CipherAlg;
+
+                    // Default key for tx (shared key)
+                    if (pWepKey->KeyIndex & 0x80000000)
+                    {
+#ifdef WPA_SUPPLICANT_SUPPORT
+                        // set key material and key length
+                        NdisZeroMemory(pAdapter->StaCfg.DesireSharedKey[KeyIdx].Key, 16);
+                        pAdapter->StaCfg.DesireSharedKey[KeyIdx].KeyLen = (UCHAR) pWepKey->KeyLength;
+                        NdisMoveMemory(pAdapter->StaCfg.DesireSharedKey[KeyIdx].Key, &pWepKey->KeyMaterial, pWepKey->KeyLength);
+                        pAdapter->StaCfg.DesireSharedKeyId = KeyIdx;
+                        pAdapter->StaCfg.DesireSharedKey[KeyIdx].CipherAlg = CipherAlg;
+#endif // WPA_SUPPLICANT_SUPPORT //
+                        pAdapter->StaCfg.DefaultKeyId = (UCHAR) KeyIdx;
+                    }
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+					if ((pAdapter->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_DISABLE) &&
+						(pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA))
+					{
+						Key = pWepKey->KeyMaterial;
+
+						// Set Group key material to Asic
+    					AsicAddSharedKeyEntry(pAdapter, BSS0, KeyIdx, CipherAlg, Key, NULL, NULL);
+
+						// Update WCID attribute table and IVEIV table for this group key table
+						RTMPAddWcidAttributeEntry(pAdapter, BSS0, KeyIdx, CipherAlg, NULL);
+
+						STA_PORT_SECURED(pAdapter);
+
+        				// Indicate Connected for GUI
+        				pAdapter->IndicateMediaState = NdisMediaStateConnected;
+					}
+                    else if (pAdapter->StaCfg.PortSecured == WPA_802_1X_PORT_SECURED)
+#endif // WPA_SUPPLICANT_SUPPORT
+                    {
+                        Key = pAdapter->SharedKey[BSS0][KeyIdx].Key;
+
+                        // Set key material and cipherAlg to Asic
+        				AsicAddSharedKeyEntry(pAdapter, BSS0, KeyIdx, CipherAlg, Key, NULL, NULL);
+
+                        if (pWepKey->KeyIndex & 0x80000000)
+                        {
+                            PMAC_TABLE_ENTRY pEntry = &pAdapter->MacTab.Content[BSSID_WCID];
+                            // Assign group key info
+    						RTMPAddWcidAttributeEntry(pAdapter, BSS0, KeyIdx, CipherAlg, NULL);
+    						// Assign pairwise key info
+    						RTMPAddWcidAttributeEntry(pAdapter, BSS0, KeyIdx, CipherAlg, pEntry);
+                        }
+                    }
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP (id=0x%x, Len=%d-byte), %s\n", pWepKey->KeyIndex, pWepKey->KeyLength, (pAdapter->StaCfg.PortSecured == WPA_802_1X_PORT_SECURED) ? "Port Secured":"Port NOT Secured"));
+				}
+            }
+            kfree(pWepKey);
+            break;
+#ifdef WPA_SUPPLICANT_SUPPORT
+	    case OID_SET_COUNTERMEASURES:
+            if (wrq->u.data.length != sizeof(int))
+                Status  = -EINVAL;
+            else
+            {
+                int enabled = 0;
+                Status = copy_from_user(&enabled, wrq->u.data.pointer, wrq->u.data.length);
+                if (enabled == 1)
+                    pAdapter->StaCfg.bBlockAssoc = TRUE;
+                else
+                    // WPA MIC error should block association attempt for 60 seconds
+                    pAdapter->StaCfg.bBlockAssoc = FALSE;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_SET_COUNTERMEASURES bBlockAssoc=%s\n", pAdapter->StaCfg.bBlockAssoc ? "TRUE":"FALSE"));
+            }
+	        break;
+        case RT_OID_WPA_SUPPLICANT_SUPPORT:
+			if (wrq->u.data.length != sizeof(UCHAR))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&wpa_supplicant_enable, wrq->u.data.pointer, wrq->u.data.length);
+    			pAdapter->StaCfg.WpaSupplicantUP = wpa_supplicant_enable;
+    			DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_WPA_SUPPLICANT_SUPPORT (=%d)\n", pAdapter->StaCfg.WpaSupplicantUP));
+			}
+            break;
+        case OID_802_11_DEAUTHENTICATION:
+            if (wrq->u.data.length != sizeof(MLME_DEAUTH_REQ_STRUCT))
+                Status  = -EINVAL;
+            else
+            {
+                MLME_DEAUTH_REQ_STRUCT      *pInfo;
+				MLME_QUEUE_ELEM *MsgElem = (MLME_QUEUE_ELEM *) kmalloc(sizeof(MLME_QUEUE_ELEM), MEM_ALLOC_FLAG);
+
+                pInfo = (MLME_DEAUTH_REQ_STRUCT *) MsgElem->Msg;
+                Status = copy_from_user(pInfo, wrq->u.data.pointer, wrq->u.data.length);
+                MlmeDeauthReqAction(pAdapter, MsgElem);
+				kfree(MsgElem);
+
+                if (INFRA_ON(pAdapter))
+                {
+                    LinkDown(pAdapter, FALSE);
+                    pAdapter->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+                }
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_DEAUTHENTICATION (Reason=%d)\n", pInfo->Reason));
+            }
+            break;
+        case OID_802_11_DROP_UNENCRYPTED:
+            if (wrq->u.data.length != sizeof(int))
+                Status  = -EINVAL;
+            else
+            {
+                int enabled = 0;
+                Status = copy_from_user(&enabled, wrq->u.data.pointer, wrq->u.data.length);
+                if (enabled == 1)
+                    pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+                else
+                    pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+				NdisAcquireSpinLock(&pAdapter->MacTabLock);
+				pAdapter->MacTab.Content[BSSID_WCID].PortSecured = pAdapter->StaCfg.PortSecured;
+				NdisReleaseSpinLock(&pAdapter->MacTabLock);
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_DROP_UNENCRYPTED (=%d)\n", enabled));
+            }
+            break;
+        case OID_802_11_SET_IEEE8021X:
+            if (wrq->u.data.length != sizeof(BOOLEAN))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&IEEE8021xState, wrq->u.data.pointer, wrq->u.data.length);
+		        pAdapter->StaCfg.IEEE8021X = IEEE8021xState;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_SET_IEEE8021X (=%d)\n", IEEE8021xState));
+            }
+            break;
+        case OID_802_11_SET_IEEE8021X_REQUIRE_KEY:
+			if (wrq->u.data.length != sizeof(BOOLEAN))
+				 Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&IEEE8021x_required_keys, wrq->u.data.pointer, wrq->u.data.length);
+				pAdapter->StaCfg.IEEE8021x_required_keys = IEEE8021x_required_keys;
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_SET_IEEE8021X_REQUIRE_KEY (%d)\n", IEEE8021x_required_keys));
+			}
+			break;
+        case OID_802_11_PMKID:
+	        pPmkId = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+
+	        if(pPmkId == NULL) {
+                Status = -ENOMEM;
+                break;
+            }
+            Status = copy_from_user(pPmkId, wrq->u.data.pointer, wrq->u.data.length);
+
+	        // check the PMKID information
+	        if (pPmkId->BSSIDInfoCount == 0)
+                NdisZeroMemory(pAdapter->StaCfg.SavedPMK, sizeof(BSSID_INFO)*PMKID_NO);
+	        else
+	        {
+		        PBSSID_INFO	pBssIdInfo;
+		        UINT		BssIdx;
+		        UINT		CachedIdx;
+
+		        for (BssIdx = 0; BssIdx < pPmkId->BSSIDInfoCount; BssIdx++)
+		        {
+			        // point to the indexed BSSID_INFO structure
+			        pBssIdInfo = (PBSSID_INFO) ((PUCHAR) pPmkId + 2 * sizeof(UINT) + BssIdx * sizeof(BSSID_INFO));
+			        // Find the entry in the saved data base.
+			        for (CachedIdx = 0; CachedIdx < pAdapter->StaCfg.SavedPMKNum; CachedIdx++)
+			        {
+				        // compare the BSSID
+				        if (NdisEqualMemory(pBssIdInfo->BSSID, pAdapter->StaCfg.SavedPMK[CachedIdx].BSSID, sizeof(NDIS_802_11_MAC_ADDRESS)))
+					        break;
+			        }
+
+			        // Found, replace it
+			        if (CachedIdx < PMKID_NO)
+			        {
+				        DBGPRINT(RT_DEBUG_OFF, ("Update OID_802_11_PMKID, idx = %d\n", CachedIdx));
+				        NdisMoveMemory(&pAdapter->StaCfg.SavedPMK[CachedIdx], pBssIdInfo, sizeof(BSSID_INFO));
+				        pAdapter->StaCfg.SavedPMKNum++;
+			        }
+			        // Not found, replace the last one
+			        else
+			        {
+				        // Randomly replace one
+				        CachedIdx = (pBssIdInfo->BSSID[5] % PMKID_NO);
+				        DBGPRINT(RT_DEBUG_OFF, ("Update OID_802_11_PMKID, idx = %d\n", CachedIdx));
+				        NdisMoveMemory(&pAdapter->StaCfg.SavedPMK[CachedIdx], pBssIdInfo, sizeof(BSSID_INFO));
+			        }
+		        }
+			}
+			if(pPmkId)
+				kfree(pPmkId);
+	        break;
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+
+
+#ifdef SNMP_SUPPORT
+		case OID_802_11_SHORTRETRYLIMIT:
+			if (wrq->u.data.length != sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&ShortRetryLimit, wrq->u.data.pointer, wrq->u.data.length);
+				RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+				tx_rty_cfg.field.ShortRtyLimit = ShortRetryLimit;
+				RTMP_IO_WRITE32(pAdapter, TX_RTY_CFG, tx_rty_cfg.word);
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_SHORTRETRYLIMIT (tx_rty_cfg.field.ShortRetryLimit=%d, ShortRetryLimit=%ld)\n", tx_rty_cfg.field.ShortRtyLimit, ShortRetryLimit));
+			}
+			break;
+
+		case OID_802_11_LONGRETRYLIMIT:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_LONGRETRYLIMIT \n"));
+			if (wrq->u.data.length != sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&LongRetryLimit, wrq->u.data.pointer, wrq->u.data.length);
+				RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+				tx_rty_cfg.field.LongRtyLimit = LongRetryLimit;
+				RTMP_IO_WRITE32(pAdapter, TX_RTY_CFG, tx_rty_cfg.word);
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_LONGRETRYLIMIT (tx_rty_cfg.field.LongRetryLimit= %d,LongRetryLimit=%ld)\n", tx_rty_cfg.field.LongRtyLimit, LongRetryLimit));
+			}
+			break;
+
+		case OID_802_11_WEPDEFAULTKEYVALUE:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_WEPDEFAULTKEYVALUE\n"));
+			pKey = kmalloc(wrq->u.data.length, GFP_KERNEL);
+			Status = copy_from_user(pKey, wrq->u.data.pointer, wrq->u.data.length);
+			//pKey = &WepKey;
+
+			if ( pKey->Length != wrq->u.data.length)
+			{
+				Status = -EINVAL;
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_WEPDEFAULTKEYVALUE, Failed!!\n"));
+			}
+			KeyIdx = pKey->KeyIndex & 0x0fffffff;
+			DBGPRINT(RT_DEBUG_TRACE,("pKey->KeyIndex =%d, pKey->KeyLength=%d\n", pKey->KeyIndex, pKey->KeyLength));
+
+			// it is a shared key
+			if (KeyIdx > 4)
+				Status = -EINVAL;
+			else
+			{
+				pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen = (UCHAR) pKey->KeyLength;
+				NdisMoveMemory(&pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].Key, &pKey->KeyMaterial, pKey->KeyLength);
+				if (pKey->KeyIndex & 0x80000000)
+				{
+					// Default key for tx (shared key)
+					pAdapter->StaCfg.DefaultKeyId = (UCHAR) KeyIdx;
+				}
+				//RestartAPIsRequired = TRUE;
+			}
+			break;
+
+
+		case OID_802_11_WEPDEFAULTKEYID:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_WEPDEFAULTKEYID \n"));
+
+			if (wrq->u.data.length != sizeof(UCHAR))
+				Status = -EINVAL;
+			else
+				Status = copy_from_user(&pAdapter->StaCfg.DefaultKeyId, wrq->u.data.pointer, wrq->u.data.length);
+
+			break;
+
+
+		case OID_802_11_CURRENTCHANNEL:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_CURRENTCHANNEL \n"));
+			if (wrq->u.data.length != sizeof(UCHAR))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&ctmp, wrq->u.data.pointer, wrq->u.data.length);
+				sprintf(&ctmp,"%d", ctmp);
+				Set_Channel_Proc(pAdapter, &ctmp);
+			}
+			break;
+#endif
+
+
+
+        default:
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::unknown IOCTL's subcmd = 0x%08x\n", cmd));
+            Status = -EOPNOTSUPP;
+            break;
+    }
+
+
+    return Status;
+}
+
+INT RTMPQueryInformation(
+    IN  PRTMP_ADAPTER pAdapter,
+    IN  OUT struct ifreq    *rq,
+    IN  INT                 cmd)
+{
+    struct iwreq                        *wrq = (struct iwreq *) rq;
+    NDIS_802_11_BSSID_LIST_EX           *pBssidList = NULL;
+    PNDIS_WLAN_BSSID_EX                 pBss;
+    NDIS_802_11_SSID                    Ssid;
+    NDIS_802_11_CONFIGURATION           *pConfiguration = NULL;
+    RT_802_11_LINK_STATUS               *pLinkStatus = NULL;
+    RT_802_11_STA_CONFIG                *pStaConfig = NULL;
+    NDIS_802_11_STATISTICS              *pStatistics = NULL;
+    NDIS_802_11_RTS_THRESHOLD           RtsThresh;
+    NDIS_802_11_FRAGMENTATION_THRESHOLD FragThresh;
+    NDIS_802_11_POWER_MODE              PowerMode;
+    NDIS_802_11_NETWORK_INFRASTRUCTURE  BssType;
+    RT_802_11_PREAMBLE                  PreamType;
+    NDIS_802_11_AUTHENTICATION_MODE     AuthMode;
+    NDIS_802_11_WEP_STATUS              WepStatus;
+    NDIS_MEDIA_STATE                    MediaState;
+    ULONG                               BssBufSize, ulInfo=0, NetworkTypeList[4], apsd = 0;
+    USHORT                              BssLen = 0;
+    PUCHAR                              pBuf = NULL, pPtr;
+    INT                                 Status = NDIS_STATUS_SUCCESS;
+    UINT                                we_version_compiled;
+    UCHAR                               i, Padding = 0;
+    BOOLEAN                             RadioState;
+	UCHAR	driverVersion[8];
+    OID_SET_HT_PHYMODE			        *pHTPhyMode = NULL;
+
+
+#ifdef SNMP_SUPPORT
+	//for snmp, kathy
+	DefaultKeyIdxValue			*pKeyIdxValue;
+	INT							valueLen;
+	TX_RTY_CFG_STRUC			tx_rty_cfg;
+	ULONG						ShortRetryLimit, LongRetryLimit;
+	UCHAR						tmp[64];
+#endif //SNMP
+
+    switch(cmd)
+    {
+        case RT_OID_DEVICE_NAME:
+            wrq->u.data.length = sizeof(STA_NIC_DEVICE_NAME);
+            Status = copy_to_user(wrq->u.data.pointer, STA_NIC_DEVICE_NAME, wrq->u.data.length);
+            break;
+        case RT_OID_VERSION_INFO:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_VERSION_INFO \n"));
+			wrq->u.data.length = 8*sizeof(UCHAR);
+			sprintf(&driverVersion[0], "%s", STA_DRIVER_VERSION);
+			driverVersion[7] = '\0';
+			if (copy_to_user(wrq->u.data.pointer, &driverVersion, wrq->u.data.length))
+            {
+				Status = -EFAULT;
+            }
+            break;
+#ifdef RALINK_ATE
+		case RT_QUERY_ATE_TXDONE_COUNT:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_QUERY_ATE_TXDONE_COUNT \n"));
+			wrq->u.data.length = sizeof(UINT32);
+			if (copy_to_user(wrq->u.data.pointer, &pAdapter->ate.TxDoneCount, wrq->u.data.length))
+			{
+				Status = -EFAULT;
+			}
+			break;
+#endif // RALINK_ATE //
+        case OID_802_11_BSSID_LIST:
+            if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+            {
+            	/*
+            	 * Still scanning, indicate the caller should try again.
+            	 */
+            	DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_BSSID_LIST (Still scanning)\n"));
+				return -EAGAIN;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_BSSID_LIST (%d BSS returned)\n",pAdapter->ScanTab.BssNr));
+			pAdapter->StaCfg.bScanReqIsFromWebUI = FALSE;
+            // Claculate total buffer size required
+            BssBufSize = sizeof(ULONG);
+
+            for (i = 0; i < pAdapter->ScanTab.BssNr; i++)
+            {
+                // Align pointer to 4 bytes boundary.
+                //Padding = 4 - (pAdapter->ScanTab.BssEntry[i].VarIELen & 0x0003);
+                //if (Padding == 4)
+                //    Padding = 0;
+                BssBufSize += (sizeof(NDIS_WLAN_BSSID_EX) - 1 + sizeof(NDIS_802_11_FIXED_IEs) + pAdapter->ScanTab.BssEntry[i].VarIELen + Padding);
+            }
+
+            // For safety issue, we add 256 bytes just in case
+            BssBufSize += 256;
+            // Allocate the same size as passed from higher layer
+            pBuf = kmalloc(BssBufSize, MEM_ALLOC_FLAG);
+            if(pBuf == NULL)
+            {
+                Status = -ENOMEM;
+                break;
+            }
+            // Init 802_11_BSSID_LIST_EX structure
+            NdisZeroMemory(pBuf, BssBufSize);
+            pBssidList = (PNDIS_802_11_BSSID_LIST_EX) pBuf;
+            pBssidList->NumberOfItems = pAdapter->ScanTab.BssNr;
+
+            // Calculate total buffer length
+            BssLen = 4; // Consist of NumberOfItems
+            // Point to start of NDIS_WLAN_BSSID_EX
+            // pPtr = pBuf + sizeof(ULONG);
+            pPtr = (PUCHAR) &pBssidList->Bssid[0];
+            for (i = 0; i < pAdapter->ScanTab.BssNr; i++)
+            {
+                pBss = (PNDIS_WLAN_BSSID_EX) pPtr;
+                NdisMoveMemory(&pBss->MacAddress, &pAdapter->ScanTab.BssEntry[i].Bssid, MAC_ADDR_LEN);
+                if ((pAdapter->ScanTab.BssEntry[i].Hidden == 1) && (pAdapter->StaCfg.bShowHiddenSSID == FALSE))
+                {
+                    //
+					// We must return this SSID during 4way handshaking, otherwise Aegis will failed to parse WPA infomation
+					// and then failed to send EAPOl farame.
+					//
+					if ((pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA) && (pAdapter->StaCfg.PortSecured != WPA_802_1X_PORT_SECURED))
+					{
+						pBss->Ssid.SsidLength = pAdapter->ScanTab.BssEntry[i].SsidLen;
+						NdisMoveMemory(pBss->Ssid.Ssid, pAdapter->ScanTab.BssEntry[i].Ssid, pAdapter->ScanTab.BssEntry[i].SsidLen);
+					}
+					else
+                    	pBss->Ssid.SsidLength = 0;
+                }
+                else
+                {
+                    pBss->Ssid.SsidLength = pAdapter->ScanTab.BssEntry[i].SsidLen;
+                    NdisMoveMemory(pBss->Ssid.Ssid, pAdapter->ScanTab.BssEntry[i].Ssid, pAdapter->ScanTab.BssEntry[i].SsidLen);
+                }
+                pBss->Privacy = pAdapter->ScanTab.BssEntry[i].Privacy;
+                pBss->Rssi = pAdapter->ScanTab.BssEntry[i].Rssi - pAdapter->BbpRssiToDbmDelta;
+                pBss->NetworkTypeInUse = NetworkTypeInUseSanity(&pAdapter->ScanTab.BssEntry[i]);
+                pBss->Configuration.Length = sizeof(NDIS_802_11_CONFIGURATION);
+                pBss->Configuration.BeaconPeriod = pAdapter->ScanTab.BssEntry[i].BeaconPeriod;
+                pBss->Configuration.ATIMWindow = pAdapter->ScanTab.BssEntry[i].AtimWin;
+
+                MAP_CHANNEL_ID_TO_KHZ(pAdapter->ScanTab.BssEntry[i].Channel, pBss->Configuration.DSConfig);
+
+                if (pAdapter->ScanTab.BssEntry[i].BssType == BSS_INFRA)
+                    pBss->InfrastructureMode = Ndis802_11Infrastructure;
+                else
+                    pBss->InfrastructureMode = Ndis802_11IBSS;
+
+                NdisMoveMemory(pBss->SupportedRates, pAdapter->ScanTab.BssEntry[i].SupRate, pAdapter->ScanTab.BssEntry[i].SupRateLen);
+                NdisMoveMemory(pBss->SupportedRates + pAdapter->ScanTab.BssEntry[i].SupRateLen,
+                               pAdapter->ScanTab.BssEntry[i].ExtRate,
+                               pAdapter->ScanTab.BssEntry[i].ExtRateLen);
+
+                if (pAdapter->ScanTab.BssEntry[i].VarIELen == 0)
+                {
+                    pBss->IELength = sizeof(NDIS_802_11_FIXED_IEs);
+                    NdisMoveMemory(pBss->IEs, &pAdapter->ScanTab.BssEntry[i].FixIEs, sizeof(NDIS_802_11_FIXED_IEs));
+                    pPtr = pPtr + sizeof(NDIS_WLAN_BSSID_EX) - 1 + sizeof(NDIS_802_11_FIXED_IEs);
+                }
+                else
+                {
+                    pBss->IELength = (ULONG)(sizeof(NDIS_802_11_FIXED_IEs) + pAdapter->ScanTab.BssEntry[i].VarIELen);
+                    pPtr = pPtr + sizeof(NDIS_WLAN_BSSID_EX) - 1 + sizeof(NDIS_802_11_FIXED_IEs);
+                    NdisMoveMemory(pBss->IEs, &pAdapter->ScanTab.BssEntry[i].FixIEs, sizeof(NDIS_802_11_FIXED_IEs));
+                    NdisMoveMemory(pBss->IEs + sizeof(NDIS_802_11_FIXED_IEs), pAdapter->ScanTab.BssEntry[i].VarIEs, pAdapter->ScanTab.BssEntry[i].VarIELen);
+                    pPtr += pAdapter->ScanTab.BssEntry[i].VarIELen;
+                }
+                pBss->Length = (ULONG)(sizeof(NDIS_WLAN_BSSID_EX) - 1 + sizeof(NDIS_802_11_FIXED_IEs) + pAdapter->ScanTab.BssEntry[i].VarIELen + Padding);
+
+#if WIRELESS_EXT < 17
+                if ((BssLen + pBss->Length) < wrq->u.data.length)
+                BssLen += pBss->Length;
+                else
+                {
+                    pBssidList->NumberOfItems = i;
+                    break;
+                }
+#else
+                BssLen += pBss->Length;
+#endif
+            }
+
+#if WIRELESS_EXT < 17
+            wrq->u.data.length = BssLen;
+#else
+            if (BssLen > wrq->u.data.length)
+            {
+                kfree(pBssidList);
+                return -E2BIG;
+            }
+            else
+                wrq->u.data.length = BssLen;
+#endif
+            Status = copy_to_user(wrq->u.data.pointer, pBssidList, BssLen);
+            kfree(pBssidList);
+            break;
+        case OID_802_3_CURRENT_ADDRESS:
+            wrq->u.data.length = MAC_ADDR_LEN;
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CurrentAddress, wrq->u.data.length);
+            break;
+        case OID_GEN_MEDIA_CONNECT_STATUS:
+            if (pAdapter->IndicateMediaState == NdisMediaStateConnected)
+                MediaState = NdisMediaStateConnected;
+            else
+                MediaState = NdisMediaStateDisconnected;
+
+            wrq->u.data.length = sizeof(NDIS_MEDIA_STATE);
+            Status = copy_to_user(wrq->u.data.pointer, &MediaState, wrq->u.data.length);
+            break;
+        case OID_802_11_BSSID:
+#ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+				Status = NDIS_STATUS_RESOURCES;
+				break;
+			}
+#endif // RALINK_ATE //
+            if (INFRA_ON(pAdapter) || ADHOC_ON(pAdapter))
+            {
+                Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.Bssid, sizeof(NDIS_802_11_MAC_ADDRESS));
+
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_BSSID(=EMPTY)\n"));
+                Status = -ENOTCONN;
+            }
+            break;
+        case OID_802_11_SSID:
+			NdisZeroMemory(&Ssid, sizeof(NDIS_802_11_SSID));
+			NdisZeroMemory(Ssid.Ssid, MAX_LEN_OF_SSID);
+            Ssid.SsidLength = pAdapter->CommonCfg.SsidLen;
+			memcpy(Ssid.Ssid, pAdapter->CommonCfg.Ssid,	Ssid.SsidLength);
+            wrq->u.data.length = sizeof(NDIS_802_11_SSID);
+            Status = copy_to_user(wrq->u.data.pointer, &Ssid, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_SSID (Len=%d, ssid=%s)\n", Ssid.SsidLength,Ssid.Ssid));
+            break;
+        case RT_OID_802_11_QUERY_LINK_STATUS:
+            pLinkStatus = (RT_802_11_LINK_STATUS *) kmalloc(sizeof(RT_802_11_LINK_STATUS), MEM_ALLOC_FLAG);
+            if (pLinkStatus)
+            {
+                pLinkStatus->CurrTxRate = RateIdTo500Kbps[pAdapter->CommonCfg.TxRate];   // unit : 500 kbps
+                pLinkStatus->ChannelQuality = pAdapter->Mlme.ChannelQuality;
+                pLinkStatus->RxByteCount = pAdapter->RalinkCounters.ReceivedByteCount;
+                pLinkStatus->TxByteCount = pAdapter->RalinkCounters.TransmittedByteCount;
+        		pLinkStatus->CentralChannel = pAdapter->CommonCfg.CentralChannel;
+                wrq->u.data.length = sizeof(RT_802_11_LINK_STATUS);
+                Status = copy_to_user(wrq->u.data.pointer, pLinkStatus, wrq->u.data.length);
+                kfree(pLinkStatus);
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_LINK_STATUS\n"));
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_LINK_STATUS(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case OID_802_11_CONFIGURATION:
+            pConfiguration = (NDIS_802_11_CONFIGURATION *) kmalloc(sizeof(NDIS_802_11_CONFIGURATION), MEM_ALLOC_FLAG);
+            if (pConfiguration)
+            {
+                pConfiguration->Length = sizeof(NDIS_802_11_CONFIGURATION);
+                pConfiguration->BeaconPeriod = pAdapter->CommonCfg.BeaconPeriod;
+                pConfiguration->ATIMWindow = pAdapter->StaActive.AtimWin;
+                MAP_CHANNEL_ID_TO_KHZ(pAdapter->CommonCfg.Channel, pConfiguration->DSConfig);
+                wrq->u.data.length = sizeof(NDIS_802_11_CONFIGURATION);
+                Status = copy_to_user(wrq->u.data.pointer, pConfiguration, wrq->u.data.length);
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_CONFIGURATION(BeaconPeriod=%ld,AtimW=%ld,Channel=%d) \n",
+                                        pConfiguration->BeaconPeriod, pConfiguration->ATIMWindow, pAdapter->CommonCfg.Channel));
+				kfree(pConfiguration);
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_CONFIGURATION(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+		case RT_OID_802_11_SNR_0:
+			if ((pAdapter->StaCfg.LastSNR0 > 0))
+			{
+				ulInfo = ((0xeb	- pAdapter->StaCfg.LastSNR0) * 3) /	16 ;
+				wrq->u.data.length = sizeof(ulInfo);
+				Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_SNR_0(0x=%lx)\n", ulInfo));
+			}
+            else
+			    Status = -EFAULT;
+			break;
+		case RT_OID_802_11_SNR_1:
+			if ((pAdapter->Antenna.field.RxPath	> 1) &&
+                (pAdapter->StaCfg.LastSNR1 > 0))
+			{
+				ulInfo = ((0xeb	- pAdapter->StaCfg.LastSNR1) * 3) /	16 ;
+				wrq->u.data.length = sizeof(ulInfo);
+				Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE,("Query::RT_OID_802_11_SNR_1(0x=%lx)\n",ulInfo));
+			}
+			else
+				Status = -EFAULT;
+            DBGPRINT(RT_DEBUG_TRACE,("Query::RT_OID_802_11_SNR_1(pAdapter->StaCfg.LastSNR1=%d)\n",pAdapter->StaCfg.LastSNR1));
+			break;
+        case OID_802_11_RSSI_TRIGGER:
+            ulInfo = pAdapter->StaCfg.RssiSample.LastRssi0 - pAdapter->BbpRssiToDbmDelta;
+            wrq->u.data.length = sizeof(ulInfo);
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_RSSI_TRIGGER(=%ld)\n", ulInfo));
+            break;
+		case OID_802_11_RSSI:
+        case RT_OID_802_11_RSSI:
+			ulInfo = pAdapter->StaCfg.RssiSample.LastRssi0;
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+			break;
+		case RT_OID_802_11_RSSI_1:
+            ulInfo = pAdapter->StaCfg.RssiSample.LastRssi1;
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+			break;
+        case RT_OID_802_11_RSSI_2:
+            ulInfo = pAdapter->StaCfg.RssiSample.LastRssi2;
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+			break;
+        case OID_802_11_STATISTICS:
+            pStatistics = (NDIS_802_11_STATISTICS *) kmalloc(sizeof(NDIS_802_11_STATISTICS), MEM_ALLOC_FLAG);
+            if (pStatistics)
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_STATISTICS \n"));
+                // add the most up-to-date h/w raw counters into software counters
+			    NICUpdateRawCounters(pAdapter);
+
+                // Sanity check for calculation of sucessful count
+                if (pAdapter->WlanCounters.TransmittedFragmentCount.QuadPart < pAdapter->WlanCounters.RetryCount.QuadPart)
+                    pAdapter->WlanCounters.TransmittedFragmentCount.QuadPart = pAdapter->WlanCounters.RetryCount.QuadPart;
+
+                pStatistics->TransmittedFragmentCount.QuadPart = pAdapter->WlanCounters.TransmittedFragmentCount.QuadPart;
+                pStatistics->MulticastTransmittedFrameCount.QuadPart = pAdapter->WlanCounters.MulticastTransmittedFrameCount.QuadPart;
+                pStatistics->FailedCount.QuadPart = pAdapter->WlanCounters.FailedCount.QuadPart;
+                pStatistics->RetryCount.QuadPart = pAdapter->WlanCounters.RetryCount.QuadPart;
+                pStatistics->MultipleRetryCount.QuadPart = pAdapter->WlanCounters.MultipleRetryCount.QuadPart;
+                pStatistics->RTSSuccessCount.QuadPart = pAdapter->WlanCounters.RTSSuccessCount.QuadPart;
+                pStatistics->RTSFailureCount.QuadPart = pAdapter->WlanCounters.RTSFailureCount.QuadPart;
+                pStatistics->ACKFailureCount.QuadPart = pAdapter->WlanCounters.ACKFailureCount.QuadPart;
+                pStatistics->FrameDuplicateCount.QuadPart = pAdapter->WlanCounters.FrameDuplicateCount.QuadPart;
+                pStatistics->ReceivedFragmentCount.QuadPart = pAdapter->WlanCounters.ReceivedFragmentCount.QuadPart;
+                pStatistics->MulticastReceivedFrameCount.QuadPart = pAdapter->WlanCounters.MulticastReceivedFrameCount.QuadPart;
+#ifdef DBG
+                pStatistics->FCSErrorCount = pAdapter->RalinkCounters.RealFcsErrCount;
+#else
+                pStatistics->FCSErrorCount.QuadPart = pAdapter->WlanCounters.FCSErrorCount.QuadPart;
+                pStatistics->FrameDuplicateCount.u.LowPart = pAdapter->WlanCounters.FrameDuplicateCount.u.LowPart / 100;
+#endif
+                wrq->u.data.length = sizeof(NDIS_802_11_STATISTICS);
+                Status = copy_to_user(wrq->u.data.pointer, pStatistics, wrq->u.data.length);
+                kfree(pStatistics);
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_STATISTICS(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case OID_GEN_RCV_OK:
+            ulInfo = pAdapter->Counters8023.GoodReceives;
+            wrq->u.data.length = sizeof(ulInfo);
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+            break;
+        case OID_GEN_RCV_NO_BUFFER:
+            ulInfo = pAdapter->Counters8023.RxNoBuffer;
+            wrq->u.data.length = sizeof(ulInfo);
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+            break;
+        case RT_OID_802_11_PHY_MODE:
+            ulInfo = (ULONG)pAdapter->CommonCfg.PhyMode;
+            wrq->u.data.length = sizeof(ulInfo);
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_PHY_MODE (=%ld)\n", ulInfo));
+            break;
+        case RT_OID_802_11_STA_CONFIG:
+            pStaConfig = (RT_802_11_STA_CONFIG *) kmalloc(sizeof(RT_802_11_STA_CONFIG), MEM_ALLOC_FLAG);
+            if (pStaConfig)
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_STA_CONFIG\n"));
+                pStaConfig->EnableTxBurst = pAdapter->CommonCfg.bEnableTxBurst;
+                pStaConfig->EnableTurboRate = 0;
+                pStaConfig->UseBGProtection = pAdapter->CommonCfg.UseBGProtection;
+                pStaConfig->UseShortSlotTime = pAdapter->CommonCfg.bUseShortSlotTime;
+                //pStaConfig->AdhocMode = pAdapter->StaCfg.AdhocMode;
+                pStaConfig->HwRadioStatus = (pAdapter->StaCfg.bHwRadio == TRUE) ? 1 : 0;
+                pStaConfig->Rsv1 = 0;
+                pStaConfig->SystemErrorBitmap = pAdapter->SystemErrorBitmap;
+                wrq->u.data.length = sizeof(RT_802_11_STA_CONFIG);
+                Status = copy_to_user(wrq->u.data.pointer, pStaConfig, wrq->u.data.length);
+                kfree(pStaConfig);
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_STA_CONFIG(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case OID_802_11_RTS_THRESHOLD:
+            RtsThresh = pAdapter->CommonCfg.RtsThreshold;
+            wrq->u.data.length = sizeof(RtsThresh);
+            Status = copy_to_user(wrq->u.data.pointer, &RtsThresh, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_RTS_THRESHOLD(=%ld)\n", RtsThresh));
+            break;
+        case OID_802_11_FRAGMENTATION_THRESHOLD:
+            FragThresh = pAdapter->CommonCfg.FragmentThreshold;
+            if (pAdapter->CommonCfg.bUseZeroToDisableFragment == TRUE)
+                FragThresh = 0;
+            wrq->u.data.length = sizeof(FragThresh);
+            Status = copy_to_user(wrq->u.data.pointer, &FragThresh, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_FRAGMENTATION_THRESHOLD(=%ld)\n", FragThresh));
+            break;
+        case OID_802_11_POWER_MODE:
+            PowerMode = pAdapter->StaCfg.WindowsPowerMode;
+            wrq->u.data.length = sizeof(PowerMode);
+            Status = copy_to_user(wrq->u.data.pointer, &PowerMode, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_POWER_MODE(=%d)\n", PowerMode));
+            break;
+        case RT_OID_802_11_RADIO:
+            RadioState = (BOOLEAN) pAdapter->StaCfg.bSwRadio;
+            wrq->u.data.length = sizeof(RadioState);
+            Status = copy_to_user(wrq->u.data.pointer, &RadioState, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_RADIO (=%d)\n", RadioState));
+            break;
+        case OID_802_11_INFRASTRUCTURE_MODE:
+            if (pAdapter->StaCfg.BssType == BSS_ADHOC)
+                BssType = Ndis802_11IBSS;
+            else if (pAdapter->StaCfg.BssType == BSS_INFRA)
+                BssType = Ndis802_11Infrastructure;
+            else if (pAdapter->StaCfg.BssType == BSS_MONITOR)
+                BssType = Ndis802_11Monitor;
+            else
+                BssType = Ndis802_11AutoUnknown;
+
+            wrq->u.data.length = sizeof(BssType);
+            Status = copy_to_user(wrq->u.data.pointer, &BssType, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_INFRASTRUCTURE_MODE(=%d)\n", BssType));
+            break;
+        case RT_OID_802_11_PREAMBLE:
+            PreamType = pAdapter->CommonCfg.TxPreamble;
+            wrq->u.data.length = sizeof(PreamType);
+            Status = copy_to_user(wrq->u.data.pointer, &PreamType, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_PREAMBLE(=%d)\n", PreamType));
+            break;
+        case OID_802_11_AUTHENTICATION_MODE:
+            AuthMode = pAdapter->StaCfg.AuthMode;
+            wrq->u.data.length = sizeof(AuthMode);
+            Status = copy_to_user(wrq->u.data.pointer, &AuthMode, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_AUTHENTICATION_MODE(=%d)\n", AuthMode));
+            break;
+        case OID_802_11_WEP_STATUS:
+            WepStatus = pAdapter->StaCfg.WepStatus;
+            wrq->u.data.length = sizeof(WepStatus);
+            Status = copy_to_user(wrq->u.data.pointer, &WepStatus, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_WEP_STATUS(=%d)\n", WepStatus));
+            break;
+        case OID_802_11_TX_POWER_LEVEL:
+			wrq->u.data.length = sizeof(ULONG);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.TxPower, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_TX_POWER_LEVEL %x\n",pAdapter->CommonCfg.TxPower));
+			break;
+        case RT_OID_802_11_TX_POWER_LEVEL_1:
+            wrq->u.data.length = sizeof(ULONG);
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.TxPowerPercentage, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_TX_POWER_LEVEL_1 (=%ld)\n", pAdapter->CommonCfg.TxPowerPercentage));
+			break;
+        case OID_802_11_NETWORK_TYPES_SUPPORTED:
+			if ((pAdapter->RfIcType	== RFIC_2850) || (pAdapter->RfIcType ==	RFIC_2750))
+			{
+				NetworkTypeList[0] = 3;                 // NumberOfItems = 3
+				NetworkTypeList[1] = Ndis802_11DS;      // NetworkType[1] = 11b
+				NetworkTypeList[2] = Ndis802_11OFDM24;  // NetworkType[2] = 11g
+				NetworkTypeList[3] = Ndis802_11OFDM5;   // NetworkType[3] = 11a
+                wrq->u.data.length = 16;
+				Status = copy_to_user(wrq->u.data.pointer, &NetworkTypeList[0], wrq->u.data.length);
+			}
+			else
+			{
+				NetworkTypeList[0] = 2;                 // NumberOfItems = 2
+				NetworkTypeList[1] = Ndis802_11DS;      // NetworkType[1] = 11b
+				NetworkTypeList[2] = Ndis802_11OFDM24;  // NetworkType[2] = 11g
+			    wrq->u.data.length = 12;
+				Status = copy_to_user(wrq->u.data.pointer, &NetworkTypeList[0], wrq->u.data.length);
+			}
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_NETWORK_TYPES_SUPPORTED\n"));
+				break;
+	    case OID_802_11_NETWORK_TYPE_IN_USE:
+            wrq->u.data.length = sizeof(ULONG);
+			if (pAdapter->CommonCfg.PhyMode == PHY_11A)
+				ulInfo = Ndis802_11OFDM5;
+			else if ((pAdapter->CommonCfg.PhyMode == PHY_11BG_MIXED) || (pAdapter->CommonCfg.PhyMode == PHY_11G))
+				ulInfo = Ndis802_11OFDM24;
+			else
+				ulInfo = Ndis802_11DS;
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+			break;
+        case RT_OID_802_11_QUERY_LAST_RX_RATE:
+            ulInfo = (ULONG)pAdapter->LastRxRate;
+            wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_LAST_RX_RATE (=%ld)\n", ulInfo));
+			break;
+		case RT_OID_802_11_QUERY_LAST_TX_RATE:
+			//ulInfo = (ULONG)pAdapter->LastTxRate;
+			ulInfo = (ULONG)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.word;
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_LAST_TX_RATE (=%lx)\n", ulInfo));
+			break;
+        case RT_OID_802_11_QUERY_EEPROM_VERSION:
+            wrq->u.data.length = sizeof(ULONG);
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->EepromVersion, wrq->u.data.length);
+            break;
+        case RT_OID_802_11_QUERY_FIRMWARE_VERSION:
+            wrq->u.data.length = sizeof(ULONG);
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->FirmwareVersion, wrq->u.data.length);
+			break;
+	    case RT_OID_802_11_QUERY_NOISE_LEVEL:
+			wrq->u.data.length = sizeof(UCHAR);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->BbpWriteLatch[66], wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_NOISE_LEVEL (=%d)\n", pAdapter->BbpWriteLatch[66]));
+			break;
+	    case RT_OID_802_11_EXTRA_INFO:
+			wrq->u.data.length = sizeof(ULONG);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->ExtraInfo, wrq->u.data.length);
+	        DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_EXTRA_INFO (=%ld)\n", pAdapter->ExtraInfo));
+	        break;
+	    case RT_OID_WE_VERSION_COMPILED:
+	        wrq->u.data.length = sizeof(UINT);
+	        we_version_compiled = WIRELESS_EXT;
+	        Status = copy_to_user(wrq->u.data.pointer, &we_version_compiled, wrq->u.data.length);
+	        break;
+		case RT_OID_802_11_QUERY_APSD_SETTING:
+			apsd = (pAdapter->CommonCfg.bAPSDCapable | (pAdapter->CommonCfg.bAPSDAC_BE << 1) | (pAdapter->CommonCfg.bAPSDAC_BK << 2)
+				| (pAdapter->CommonCfg.bAPSDAC_VI << 3)	| (pAdapter->CommonCfg.bAPSDAC_VO << 4)	| (pAdapter->CommonCfg.MaxSPLength << 5));
+
+			wrq->u.data.length = sizeof(ULONG);
+			Status = copy_to_user(wrq->u.data.pointer, &apsd, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_APSD_SETTING (=0x%lx,APSDCap=%d,AC_BE=%d,AC_BK=%d,AC_VI=%d,AC_VO=%d,MAXSPLen=%d)\n",
+				apsd,pAdapter->CommonCfg.bAPSDCapable,pAdapter->CommonCfg.bAPSDAC_BE,pAdapter->CommonCfg.bAPSDAC_BK,pAdapter->CommonCfg.bAPSDAC_VI,pAdapter->CommonCfg.bAPSDAC_VO,pAdapter->CommonCfg.MaxSPLength));
+			break;
+		case RT_OID_802_11_QUERY_APSD_PSM:
+			wrq->u.data.length = sizeof(ULONG);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.bAPSDForcePowerSave, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_APSD_PSM (=%d)\n", pAdapter->CommonCfg.bAPSDForcePowerSave));
+			break;
+		case RT_OID_802_11_QUERY_WMM:
+			wrq->u.data.length = sizeof(BOOLEAN);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.bWmmCapable, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_WMM (=%d)\n",	pAdapter->CommonCfg.bWmmCapable));
+			break;
+#ifdef WPA_SUPPLICANT_SUPPORT
+        case RT_OID_NEW_DRIVER:
+            {
+                UCHAR enabled = 1;
+    	        wrq->u.data.length = sizeof(UCHAR);
+    	        Status = copy_to_user(wrq->u.data.pointer, &enabled, wrq->u.data.length);
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_NEW_DRIVER (=%d)\n", enabled));
+            }
+	        break;
+        case RT_OID_WPA_SUPPLICANT_SUPPORT:
+	        wrq->u.data.length = sizeof(UCHAR);
+	        Status = copy_to_user(wrq->u.data.pointer, &pAdapter->StaCfg.WpaSupplicantUP, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_WPA_SUPPLICANT_SUPPORT (=%d)\n", pAdapter->StaCfg.WpaSupplicantUP));
+	        break;
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+        case RT_OID_DRIVER_DEVICE_NAME:
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_DRIVER_DEVICE_NAME \n"));
+			wrq->u.data.length = 16;
+			if (copy_to_user(wrq->u.data.pointer, pAdapter->StaCfg.dev_name, wrq->u.data.length))
+			{
+				Status = -EFAULT;
+			}
+            break;
+        case RT_OID_802_11_QUERY_HT_PHYMODE:
+            pHTPhyMode = (OID_SET_HT_PHYMODE *) kmalloc(sizeof(OID_SET_HT_PHYMODE), MEM_ALLOC_FLAG);
+            if (pHTPhyMode)
+            {
+                pHTPhyMode->PhyMode = pAdapter->CommonCfg.PhyMode;
+    			pHTPhyMode->HtMode = (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE;
+    			pHTPhyMode->BW = (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.BW;
+    			pHTPhyMode->MCS= (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.MCS;
+    			pHTPhyMode->SHORTGI= (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.ShortGI;
+    			pHTPhyMode->STBC= (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.STBC;
+
+    			pHTPhyMode->ExtOffset = ((pAdapter->CommonCfg.CentralChannel < pAdapter->CommonCfg.Channel) ? (EXTCHA_BELOW) : (EXTCHA_ABOVE));
+                wrq->u.data.length = sizeof(OID_SET_HT_PHYMODE);
+                if (copy_to_user(wrq->u.data.pointer, pHTPhyMode, wrq->u.data.length))
+    			{
+    				Status = -EFAULT;
+    			}
+    			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_HT_PHYMODE (PhyMode = %d, MCS =%d, BW = %d, STBC = %d, ExtOffset=%d)\n",
+    				pHTPhyMode->HtMode, pHTPhyMode->MCS, pHTPhyMode->BW, pHTPhyMode->STBC, pHTPhyMode->ExtOffset));
+    			DBGPRINT(RT_DEBUG_TRACE, (" MlmeUpdateTxRates (.word = %x )\n", pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.word));
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_STA_CONFIG(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case RT_OID_802_11_COUNTRY_REGION:
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_COUNTRY_REGION \n"));
+			wrq->u.data.length = sizeof(ulInfo);
+            ulInfo = pAdapter->CommonCfg.CountryRegionForABand;
+            ulInfo = (ulInfo << 8)|(pAdapter->CommonCfg.CountryRegion);
+			if (copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length))
+            {
+				Status = -EFAULT;
+            }
+            break;
+        case RT_OID_802_11_QUERY_DAT_HT_PHYMODE:
+            pHTPhyMode = (OID_SET_HT_PHYMODE *) kmalloc(sizeof(OID_SET_HT_PHYMODE), MEM_ALLOC_FLAG);
+            if (pHTPhyMode)
+            {
+                pHTPhyMode->PhyMode = pAdapter->CommonCfg.PhyMode;
+    			pHTPhyMode->HtMode = (UCHAR)pAdapter->CommonCfg.RegTransmitSetting.field.HTMODE;
+    			pHTPhyMode->BW = (UCHAR)pAdapter->CommonCfg.RegTransmitSetting.field.BW;
+    			pHTPhyMode->MCS= (UCHAR)pAdapter->StaCfg.DesiredTransmitSetting.field.MCS;
+    			pHTPhyMode->SHORTGI= (UCHAR)pAdapter->CommonCfg.RegTransmitSetting.field.ShortGI;
+    			pHTPhyMode->STBC= (UCHAR)pAdapter->CommonCfg.RegTransmitSetting.field.STBC;
+
+                wrq->u.data.length = sizeof(OID_SET_HT_PHYMODE);
+                if (copy_to_user(wrq->u.data.pointer, pHTPhyMode, wrq->u.data.length))
+    			{
+    				Status = -EFAULT;
+    			}
+    			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_HT_PHYMODE (PhyMode = %d, MCS =%d, BW = %d, STBC = %d, ExtOffset=%d)\n",
+    				pHTPhyMode->HtMode, pHTPhyMode->MCS, pHTPhyMode->BW, pHTPhyMode->STBC, pHTPhyMode->ExtOffset));
+    			DBGPRINT(RT_DEBUG_TRACE, (" MlmeUpdateTxRates (.word = %x )\n", pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.word));
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_STA_CONFIG(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case RT_OID_QUERY_MULTIPLE_CARD_SUPPORT:
+			wrq->u.data.length = sizeof(UCHAR);
+            i = 0;
+#ifdef MULTIPLE_CARD_SUPPORT
+            i = 1;
+#endif // MULTIPLE_CARD_SUPPORT //
+			if (copy_to_user(wrq->u.data.pointer, &i, wrq->u.data.length))
+            {
+				Status = -EFAULT;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_QUERY_MULTIPLE_CARD_SUPPORT(=%d) \n", i));
+            break;
+#ifdef SNMP_SUPPORT
+		case RT_OID_802_11_MAC_ADDRESS:
+            wrq->u.data.length = MAC_ADDR_LEN;
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CurrentAddress, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_MANUFACTUREROUI:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_MANUFACTUREROUI \n"));
+			wrq->u.data.length = ManufacturerOUI_LEN;
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CurrentAddress, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_MANUFACTURERNAME:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_MANUFACTURERNAME \n"));
+			wrq->u.data.length = strlen(ManufacturerNAME);
+			Status = copy_to_user(wrq->u.data.pointer, ManufacturerNAME, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_RESOURCETYPEIDNAME:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_RESOURCETYPEIDNAME \n"));
+			wrq->u.data.length = strlen(ResourceTypeIdName);
+			Status = copy_to_user(wrq->u.data.pointer, ResourceTypeIdName, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_PRIVACYOPTIONIMPLEMENTED:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_PRIVACYOPTIONIMPLEMENTED \n"));
+			ulInfo = 1; // 1 is support wep else 2 is not support.
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_POWERMANAGEMENTMODE:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_POWERMANAGEMENTMODE \n"));
+			if (pAdapter->StaCfg.Psm == PSMP_ACTION)
+				ulInfo = 1; // 1 is power active else 2 is power save.
+			else
+				ulInfo = 2;
+
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+			break;
+
+		case OID_802_11_WEPDEFAULTKEYVALUE:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_WEPDEFAULTKEYVALUE \n"));
+			//KeyIdxValue.KeyIdx = pAd->PortCfg.MBSSID[pAd->IoctlIF].DefaultKeyId;
+			pKeyIdxValue = wrq->u.data.pointer;
+			DBGPRINT(RT_DEBUG_TRACE,("KeyIdxValue.KeyIdx = %d, \n",pKeyIdxValue->KeyIdx));
+			valueLen = pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen;
+			NdisMoveMemory(pKeyIdxValue->Value,
+						   &pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].Key,
+						   valueLen);
+			pKeyIdxValue->Value[valueLen]='\0';
+
+			wrq->u.data.length = sizeof(DefaultKeyIdxValue);
+
+			Status = copy_to_user(wrq->u.data.pointer, pKeyIdxValue, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE,("DefaultKeyId = %d, total len = %d, str len=%d, KeyValue= %02x %02x %02x %02x \n", pAdapter->StaCfg.DefaultKeyId, wrq->u.data.length, pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen,
+			pAdapter->SharedKey[BSS0][0].Key[0],
+			pAdapter->SharedKey[BSS0][1].Key[0],
+			pAdapter->SharedKey[BSS0][2].Key[0],
+			pAdapter->SharedKey[BSS0][3].Key[0]));
+			break;
+
+		case OID_802_11_WEPDEFAULTKEYID:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_WEPDEFAULTKEYID \n"));
+			wrq->u.data.length = sizeof(UCHAR);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->StaCfg.DefaultKeyId, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("DefaultKeyId =%d \n", pAdapter->StaCfg.DefaultKeyId));
+			break;
+
+		case RT_OID_802_11_WEPKEYMAPPINGLENGTH:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_WEPKEYMAPPINGLENGTH \n"));
+			wrq->u.data.length = sizeof(UCHAR);
+			Status = copy_to_user(wrq->u.data.pointer,
+									&pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen,
+									wrq->u.data.length);
+			break;
+
+		case OID_802_11_SHORTRETRYLIMIT:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_SHORTRETRYLIMIT \n"));
+			wrq->u.data.length = sizeof(ULONG);
+			RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+			ShortRetryLimit = tx_rty_cfg.field.ShortRtyLimit;
+			DBGPRINT(RT_DEBUG_TRACE, ("ShortRetryLimit =%ld,  tx_rty_cfg.field.ShortRetryLimit=%d\n", ShortRetryLimit, tx_rty_cfg.field.ShortRtyLimit));
+			Status = copy_to_user(wrq->u.data.pointer, &ShortRetryLimit, wrq->u.data.length);
+			break;
+
+		case OID_802_11_LONGRETRYLIMIT:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_LONGRETRYLIMIT \n"));
+			wrq->u.data.length = sizeof(ULONG);
+			RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+			LongRetryLimit = tx_rty_cfg.field.LongRtyLimit;
+			DBGPRINT(RT_DEBUG_TRACE, ("LongRetryLimit =%ld,  tx_rty_cfg.field.LongRtyLimit=%d\n", LongRetryLimit, tx_rty_cfg.field.LongRtyLimit));
+			Status = copy_to_user(wrq->u.data.pointer, &LongRetryLimit, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_PRODUCTID:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_PRODUCTID \n"));
+
+#ifdef RT2870
+			sprintf(tmp, "%04x %04x\n", ((POS_COOKIE)pAdapter->OS_Cookie)->pUsb_Dev->descriptor.idVendor ,((POS_COOKIE)pAdapter->OS_Cookie)->pUsb_Dev->descriptor.idProduct);
+
+#endif // RT2870 //
+			wrq->u.data.length = strlen(tmp);
+			Status = copy_to_user(wrq->u.data.pointer, tmp, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_MANUFACTUREID:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_MANUFACTUREID \n"));
+			wrq->u.data.length = strlen(ManufacturerNAME);
+			Status = copy_to_user(wrq->u.data.pointer, ManufacturerNAME, wrq->u.data.length);
+			break;
+
+		case OID_802_11_CURRENTCHANNEL:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_CURRENTCHANNEL \n"));
+			wrq->u.data.length = sizeof(UCHAR);
+			DBGPRINT(RT_DEBUG_TRACE, ("sizeof UCHAR=%d, channel=%d \n", sizeof(UCHAR), pAdapter->CommonCfg.Channel));
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.Channel, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+			break;
+#endif //SNMP_SUPPORT
+
+		case OID_802_11_BUILD_CHANNEL_EX:
+			{
+				UCHAR value;
+				DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_BUILD_CHANNEL_EX \n"));
+				wrq->u.data.length = sizeof(UCHAR);
+#ifdef EXT_BUILD_CHANNEL_LIST
+				DBGPRINT(RT_DEBUG_TRACE, ("Support EXT_BUILD_CHANNEL_LIST.\n"));
+				value = 1;
+#else
+				DBGPRINT(RT_DEBUG_TRACE, ("Doesn't support EXT_BUILD_CHANNEL_LIST.\n"));
+				value = 0;
+#endif // EXT_BUILD_CHANNEL_LIST //
+				Status = copy_to_user(wrq->u.data.pointer, &value, 1);
+				DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+			}
+			break;
+
+		case OID_802_11_GET_CH_LIST:
+			{
+				PRT_CHANNEL_LIST_INFO pChListBuf;
+
+				DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_GET_CH_LIST \n"));
+				if (pAdapter->ChannelListNum == 0)
+				{
+					wrq->u.data.length = 0;
+					break;
+				}
+
+				pChListBuf = (RT_CHANNEL_LIST_INFO *) kmalloc(sizeof(RT_CHANNEL_LIST_INFO), MEM_ALLOC_FLAG);
+				if (pChListBuf == NULL)
+				{
+					wrq->u.data.length = 0;
+					break;
+				}
+
+				pChListBuf->ChannelListNum = pAdapter->ChannelListNum;
+				for (i = 0; i < pChListBuf->ChannelListNum; i++)
+					pChListBuf->ChannelList[i] = pAdapter->ChannelList[i].Channel;
+
+				wrq->u.data.length = sizeof(RT_CHANNEL_LIST_INFO);
+				Status = copy_to_user(wrq->u.data.pointer, pChListBuf, sizeof(RT_CHANNEL_LIST_INFO));
+				DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+
+				if (pChListBuf)
+					kfree(pChListBuf);
+			}
+			break;
+
+		case OID_802_11_GET_COUNTRY_CODE:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_GET_COUNTRY_CODE \n"));
+			wrq->u.data.length = 2;
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.CountryCode, 2);
+			DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+			break;
+
+		case OID_802_11_GET_CHANNEL_GEOGRAPHY:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_GET_CHANNEL_GEOGRAPHY \n"));
+			wrq->u.data.length = 1;
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.Geography, 1);
+			DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+			break;
+
+
+#ifdef QOS_DLS_SUPPORT
+		case RT_OID_802_11_QUERY_DLS:
+			wrq->u.data.length = sizeof(BOOLEAN);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.bDLSCapable, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_DLS(=%d)\n", pAdapter->CommonCfg.bDLSCapable));
+			break;
+
+		case RT_OID_802_11_QUERY_DLS_PARAM:
+			{
+				PRT_802_11_DLS_INFO	pDlsInfo = kmalloc(sizeof(RT_802_11_DLS_INFO), GFP_ATOMIC);
+				if (pDlsInfo == NULL)
+					break;
+
+				for (i=0; i<MAX_NUM_OF_DLS_ENTRY; i++)
+				{
+					RTMPMoveMemory(&pDlsInfo->Entry[i], &pAdapter->StaCfg.DLSEntry[i], sizeof(RT_802_11_DLS_UI));
+				}
+
+				pDlsInfo->num = MAX_NUM_OF_DLS_ENTRY;
+				wrq->u.data.length = sizeof(RT_802_11_DLS_INFO);
+				Status = copy_to_user(wrq->u.data.pointer, pDlsInfo, wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_DLS_PARAM\n"));
+
+				if (pDlsInfo)
+					kfree(pDlsInfo);
+			}
+			break;
+#endif // QOS_DLS_SUPPORT //
+        default:
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::unknown IOCTL's subcmd = 0x%08x\n", cmd));
+            Status = -EOPNOTSUPP;
+            break;
+    }
+    return Status;
+}
+
+INT rt28xx_sta_ioctl(
+	IN	struct net_device	*net_dev,
+	IN	OUT	struct ifreq	*rq,
+	IN	INT					cmd)
+{
+	POS_COOKIE			pObj;
+	VIRTUAL_ADAPTER		*pVirtualAd = NULL;
+	RTMP_ADAPTER        *pAd = NULL;
+	struct iwreq        *wrq = (struct iwreq *) rq;
+	BOOLEAN				StateMachineTouched = FALSE;
+	INT					Status = NDIS_STATUS_SUCCESS;
+	USHORT				subcmd;
+
+	if (net_dev->priv_flags == INT_MAIN)
+	{
+		pAd = net_dev->ml_priv;
+	}
+	else
+	{
+		pVirtualAd = net_dev->ml_priv;
+		pAd = pVirtualAd->RtmpDev->ml_priv;
+	}
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	if (pAd == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->ml_priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+    //check if the interface is down
+    if(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+	    if (wrq->u.data.pointer == NULL)
+	    {
+		    return Status;
+	    }
+
+	    if (strstr(wrq->u.data.pointer, "OpMode") == NULL)
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+        {
+            DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+		    return -ENETDOWN;
+        }
+    }
+
+	{	// determine this ioctl command is comming from which interface.
+		pObj->ioctl_if_type = INT_MAIN;
+		pObj->ioctl_if = MAIN_MBSSID;
+	}
+
+	switch(cmd)
+	{
+#ifdef RALINK_ATE
+#ifdef RALINK_28xx_QA
+		case RTPRIV_IOCTL_ATE:
+			{
+				RtmpDoAte(pAd, wrq);
+			}
+			break;
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+        case SIOCGIFHWADDR:
+			DBGPRINT(RT_DEBUG_TRACE, ("IOCTL::SIOCGIFHWADDR\n"));
+			memcpy(wrq->u.name, pAd->CurrentAddress, ETH_ALEN);
+			break;
+		case SIOCGIWNAME:
+        {
+        	char *name=&wrq->u.name[0];
+        	rt_ioctl_giwname(net_dev, NULL, name, NULL);
+			break;
+		}
+		case SIOCGIWESSID:  //Get ESSID
+        {
+        	struct iw_point *essid=&wrq->u.essid;
+        	rt_ioctl_giwessid(net_dev, NULL, essid, essid->pointer);
+			break;
+		}
+		case SIOCSIWESSID:  //Set ESSID
+        {
+        	struct iw_point	*essid=&wrq->u.essid;
+        	rt_ioctl_siwessid(net_dev, NULL, essid, essid->pointer);
+			break;
+		}
+		case SIOCSIWNWID:   // set network id (the cell)
+		case SIOCGIWNWID:   // get network id
+			Status = -EOPNOTSUPP;
+			break;
+		case SIOCSIWFREQ:   //set channel/frequency (Hz)
+        {
+        	struct iw_freq *freq=&wrq->u.freq;
+        	rt_ioctl_siwfreq(net_dev, NULL, freq, NULL);
+			break;
+		}
+		case SIOCGIWFREQ:   // get channel/frequency (Hz)
+        {
+        	struct iw_freq *freq=&wrq->u.freq;
+        	rt_ioctl_giwfreq(net_dev, NULL, freq, NULL);
+			break;
+		}
+		case SIOCSIWNICKN: //set node name/nickname
+        {
+        	struct iw_point *data=&wrq->u.data;
+        	rt_ioctl_siwnickn(net_dev, NULL, data, NULL);
+			break;
+		}
+		case SIOCGIWNICKN: //get node name/nickname
+        {
+        	struct iw_point *data=&wrq->u.data;
+        	rt_ioctl_giwnickn(net_dev, NULL, data, NULL);
+			break;
+		}
+		case SIOCGIWRATE:   //get default bit rate (bps)
+		    rt_ioctl_giwrate(net_dev, NULL, &wrq->u, NULL);
+            break;
+	    case SIOCSIWRATE:  //set default bit rate (bps)
+	        rt_ioctl_siwrate(net_dev, NULL, &wrq->u, NULL);
+            break;
+        case SIOCGIWRTS:  // get RTS/CTS threshold (bytes)
+        {
+        	struct iw_param *rts=&wrq->u.rts;
+        	rt_ioctl_giwrts(net_dev, NULL, rts, NULL);
+			break;
+		}
+        case SIOCSIWRTS:  //set RTS/CTS threshold (bytes)
+        {
+        	struct iw_param *rts=&wrq->u.rts;
+        	rt_ioctl_siwrts(net_dev, NULL, rts, NULL);
+			break;
+		}
+        case SIOCGIWFRAG:  //get fragmentation thr (bytes)
+        {
+        	struct iw_param *frag=&wrq->u.frag;
+        	rt_ioctl_giwfrag(net_dev, NULL, frag, NULL);
+			break;
+		}
+        case SIOCSIWFRAG:  //set fragmentation thr (bytes)
+        {
+        	struct iw_param *frag=&wrq->u.frag;
+        	rt_ioctl_siwfrag(net_dev, NULL, frag, NULL);
+			break;
+		}
+        case SIOCGIWENCODE:  //get encoding token & mode
+        {
+        	struct iw_point *erq=&wrq->u.encoding;
+        	if(erq->pointer)
+        		rt_ioctl_giwencode(net_dev, NULL, erq, erq->pointer);
+			break;
+		}
+        case SIOCSIWENCODE:  //set encoding token & mode
+        {
+        	struct iw_point *erq=&wrq->u.encoding;
+        	if(erq->pointer)
+        		rt_ioctl_siwencode(net_dev, NULL, erq, erq->pointer);
+			break;
+		}
+		case SIOCGIWAP:     //get access point MAC addresses
+        {
+        	struct sockaddr *ap_addr=&wrq->u.ap_addr;
+        	rt_ioctl_giwap(net_dev, NULL, ap_addr, ap_addr->sa_data);
+			break;
+		}
+	    case SIOCSIWAP:  //set access point MAC addresses
+        {
+        	struct sockaddr *ap_addr=&wrq->u.ap_addr;
+        	rt_ioctl_siwap(net_dev, NULL, ap_addr, ap_addr->sa_data);
+			break;
+		}
+		case SIOCGIWMODE:   //get operation mode
+        {
+        	__u32 *mode=&wrq->u.mode;
+        	rt_ioctl_giwmode(net_dev, NULL, mode, NULL);
+			break;
+		}
+		case SIOCSIWMODE:   //set operation mode
+        {
+        	__u32 *mode=&wrq->u.mode;
+        	rt_ioctl_siwmode(net_dev, NULL, mode, NULL);
+			break;
+		}
+		case SIOCGIWSENS:   //get sensitivity (dBm)
+		case SIOCSIWSENS:	//set sensitivity (dBm)
+		case SIOCGIWPOWER:  //get Power Management settings
+		case SIOCSIWPOWER:  //set Power Management settings
+		case SIOCGIWTXPOW:  //get transmit power (dBm)
+		case SIOCSIWTXPOW:  //set transmit power (dBm)
+		case SIOCGIWRANGE:	//Get range of parameters
+		case SIOCGIWRETRY:	//get retry limits and lifetime
+		case SIOCSIWRETRY:	//set retry limits and lifetime
+			Status = -EOPNOTSUPP;
+			break;
+		case RT_PRIV_IOCTL:
+			subcmd = wrq->u.data.flags;
+			if( subcmd & OID_GET_SET_TOGGLE)
+				Status = RTMPSetInformation(pAd, rq, subcmd);
+			else
+				Status = RTMPQueryInformation(pAd, rq, subcmd);
+			break;
+		case SIOCGIWPRIV:
+			if (wrq->u.data.pointer)
+			{
+				if ( access_ok(VERIFY_WRITE, wrq->u.data.pointer, sizeof(privtab)) != TRUE)
+					break;
+				wrq->u.data.length = sizeof(privtab) / sizeof(privtab[0]);
+				if (copy_to_user(wrq->u.data.pointer, privtab, sizeof(privtab)))
+					Status = -EFAULT;
+			}
+			break;
+		case RTPRIV_IOCTL_SET:
+			if(access_ok(VERIFY_READ, wrq->u.data.pointer, wrq->u.data.length) != TRUE)
+				break;
+			rt_ioctl_setparam(net_dev, NULL, NULL, wrq->u.data.pointer);
+			break;
+		case RTPRIV_IOCTL_GSITESURVEY:
+			RTMPIoctlGetSiteSurvey(pAd, wrq);
+		    break;
+#ifdef DBG
+		case RTPRIV_IOCTL_MAC:
+			RTMPIoctlMAC(pAd, wrq);
+			break;
+		case RTPRIV_IOCTL_E2P:
+			RTMPIoctlE2PROM(pAd, wrq);
+			break;
+#endif // DBG //
+        case SIOCETHTOOL:
+                break;
+		default:
+			DBGPRINT(RT_DEBUG_ERROR, ("IOCTL::unknown IOCTL's cmd = 0x%08x\n", cmd));
+			Status = -EOPNOTSUPP;
+			break;
+	}
+
+    if(StateMachineTouched) // Upper layer sent a MLME-related operations
+    	RT28XX_MLME_HANDLER(pAd);
+
+	return Status;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set SSID
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_SSID_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    NDIS_802_11_SSID                    Ssid, *pSsid=NULL;
+    BOOLEAN                             StateMachineTouched = FALSE;
+    int                                 success = TRUE;
+
+    if( strlen(arg) <= MAX_LEN_OF_SSID)
+    {
+        NdisZeroMemory(&Ssid, sizeof(NDIS_802_11_SSID));
+        if (strlen(arg) != 0)
+        {
+            NdisMoveMemory(Ssid.Ssid, arg, strlen(arg));
+            Ssid.SsidLength = strlen(arg);
+        }
+        else   //ANY ssid
+        {
+            Ssid.SsidLength = 0;
+		    memcpy(Ssid.Ssid, "", 0);
+			pAdapter->StaCfg.BssType = BSS_INFRA;
+			pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+	        pAdapter->StaCfg.WepStatus  = Ndis802_11EncryptionDisabled;
+		}
+        pSsid = &Ssid;
+
+        if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+        {
+            RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+            DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+        }
+
+        pAdapter->MlmeAux.CurrReqIsFromNdis = TRUE;
+        pAdapter->StaCfg.bScanReqIsFromWebUI = FALSE;
+		pAdapter->bConfigChanged = TRUE;
+
+        MlmeEnqueue(pAdapter,
+                    MLME_CNTL_STATE_MACHINE,
+                    OID_802_11_SSID,
+                    sizeof(NDIS_802_11_SSID),
+                    (VOID *)pSsid);
+
+        StateMachineTouched = TRUE;
+        DBGPRINT(RT_DEBUG_TRACE, ("Set_SSID_Proc::(Len=%d,Ssid=%s)\n", Ssid.SsidLength, Ssid.Ssid));
+    }
+    else
+        success = FALSE;
+
+    if (StateMachineTouched) // Upper layer sent a MLME-related operations
+    	RT28XX_MLME_HANDLER(pAdapter);
+
+    return success;
+}
+
+#ifdef WMM_SUPPORT
+/*
+    ==========================================================================
+    Description:
+        Set WmmCapable Enable or Disable
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_WmmCapable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	BOOLEAN	bWmmCapable;
+
+	bWmmCapable = simple_strtol(arg, 0, 10);
+
+	if ((bWmmCapable == 1)
+#ifdef RT2870
+		&& (pAd->NumberOfPipes >= 5)
+#endif // RT2870 //
+		)
+		pAd->CommonCfg.bWmmCapable = TRUE;
+	else if (bWmmCapable == 0)
+		pAd->CommonCfg.bWmmCapable = FALSE;
+	else
+		return FALSE;  //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_WmmCapable_Proc::(bWmmCapable=%d)\n",
+		pAd->CommonCfg.bWmmCapable));
+
+	return TRUE;
+}
+#endif // WMM_SUPPORT //
+
+/*
+    ==========================================================================
+    Description:
+        Set Network Type(Infrastructure/Adhoc mode)
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_NetworkType_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    UINT32	Value = 0;
+
+    if (strcmp(arg, "Adhoc") == 0)
+	{
+		if (pAdapter->StaCfg.BssType != BSS_ADHOC)
+		{
+			// Config has changed
+			pAdapter->bConfigChanged = TRUE;
+            if (MONITOR_ON(pAdapter))
+            {
+                RTMP_IO_WRITE32(pAdapter, RX_FILTR_CFG, STANORMAL);
+                RTMP_IO_READ32(pAdapter, MAC_SYS_CTRL, &Value);
+				Value &= (~0x80);
+				RTMP_IO_WRITE32(pAdapter, MAC_SYS_CTRL, Value);
+                OPSTATUS_CLEAR_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED);
+                pAdapter->StaCfg.bAutoReconnect = TRUE;
+                LinkDown(pAdapter, FALSE);
+            }
+			if (INFRA_ON(pAdapter))
+			{
+				//BOOLEAN Cancelled;
+				// Set the AutoReconnectSsid to prevent it reconnect to old SSID
+				// Since calling this indicate user don't want to connect to that SSID anymore.
+				pAdapter->MlmeAux.AutoReconnectSsidLen= 32;
+				NdisZeroMemory(pAdapter->MlmeAux.AutoReconnectSsid, pAdapter->MlmeAux.AutoReconnectSsidLen);
+
+				LinkDown(pAdapter, FALSE);
+
+				DBGPRINT(RT_DEBUG_TRACE, ("NDIS_STATUS_MEDIA_DISCONNECT Event BB!\n"));
+			}
+		}
+		pAdapter->StaCfg.BssType = BSS_ADHOC;
+        pAdapter->net_dev->type = pAdapter->StaCfg.OriDevType;
+		DBGPRINT(RT_DEBUG_TRACE, ("===>Set_NetworkType_Proc::(AD-HOC)\n"));
+	}
+    else if (strcmp(arg, "Infra") == 0)
+	{
+		if (pAdapter->StaCfg.BssType != BSS_INFRA)
+		{
+			// Config has changed
+			pAdapter->bConfigChanged = TRUE;
+            if (MONITOR_ON(pAdapter))
+            {
+                RTMP_IO_WRITE32(pAdapter, RX_FILTR_CFG, STANORMAL);
+                RTMP_IO_READ32(pAdapter, MAC_SYS_CTRL, &Value);
+				Value &= (~0x80);
+				RTMP_IO_WRITE32(pAdapter, MAC_SYS_CTRL, Value);
+                OPSTATUS_CLEAR_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED);
+                pAdapter->StaCfg.bAutoReconnect = TRUE;
+                LinkDown(pAdapter, FALSE);
+            }
+			if (ADHOC_ON(pAdapter))
+			{
+				// Set the AutoReconnectSsid to prevent it reconnect to old SSID
+				// Since calling this indicate user don't want to connect to that SSID anymore.
+				pAdapter->MlmeAux.AutoReconnectSsidLen= 32;
+				NdisZeroMemory(pAdapter->MlmeAux.AutoReconnectSsid, pAdapter->MlmeAux.AutoReconnectSsidLen);
+
+				LinkDown(pAdapter, FALSE);
+			}
+		}
+		pAdapter->StaCfg.BssType = BSS_INFRA;
+        pAdapter->net_dev->type = pAdapter->StaCfg.OriDevType;
+		DBGPRINT(RT_DEBUG_TRACE, ("===>Set_NetworkType_Proc::(INFRA)\n"));
+
+        pAdapter->StaCfg.BssType = BSS_INFRA;
+	}
+    else if (strcmp(arg, "Monitor") == 0)
+    {
+		UCHAR	bbpValue = 0;
+		BCN_TIME_CFG_STRUC csr;
+		OPSTATUS_CLEAR_FLAG(pAdapter, fOP_STATUS_INFRA_ON);
+        OPSTATUS_CLEAR_FLAG(pAdapter, fOP_STATUS_ADHOC_ON);
+		OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED);
+		// disable all periodic state machine
+		pAdapter->StaCfg.bAutoReconnect = FALSE;
+		// reset all mlme state machine
+		RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+		DBGPRINT(RT_DEBUG_TRACE, ("fOP_STATUS_MEDIA_STATE_CONNECTED \n"));
+        if (pAdapter->CommonCfg.CentralChannel == 0)
+        {
+#ifdef DOT11_N_SUPPORT
+            if (pAdapter->CommonCfg.PhyMode == PHY_11AN_MIXED)
+                pAdapter->CommonCfg.CentralChannel = 36;
+            else
+#endif // DOT11_N_SUPPORT //
+                pAdapter->CommonCfg.CentralChannel = 6;
+        }
+#ifdef DOT11_N_SUPPORT
+        else
+            N_ChannelCheck(pAdapter);
+#endif // DOT11_N_SUPPORT //
+
+#ifdef DOT11_N_SUPPORT
+	if (pAdapter->CommonCfg.PhyMode >= PHY_11ABGN_MIXED &&
+            pAdapter->CommonCfg.RegTransmitSetting.field.BW == BW_40 &&
+            pAdapter->CommonCfg.RegTransmitSetting.field.EXTCHA == EXTCHA_ABOVE)
+		{
+			// 40MHz ,control channel at lower
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R4, &bbpValue);
+			bbpValue &= (~0x18);
+			bbpValue |= 0x10;
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R4, bbpValue);
+			pAdapter->CommonCfg.BBPCurrentBW = BW_40;
+			//  RX : control channel at lower
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R3, &bbpValue);
+			bbpValue &= (~0x20);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R3, bbpValue);
+
+			RTMP_IO_READ32(pAdapter, TX_BAND_CFG, &Value);
+			Value &= 0xfffffffe;
+			RTMP_IO_WRITE32(pAdapter, TX_BAND_CFG, Value);
+			pAdapter->CommonCfg.CentralChannel = pAdapter->CommonCfg.Channel + 2;
+            AsicSwitchChannel(pAdapter, pAdapter->CommonCfg.CentralChannel, FALSE);
+		    AsicLockChannel(pAdapter, pAdapter->CommonCfg.CentralChannel);
+            DBGPRINT(RT_DEBUG_TRACE, ("BW_40 ,control_channel(%d), CentralChannel(%d) \n",
+                                       pAdapter->CommonCfg.Channel,
+                                       pAdapter->CommonCfg.CentralChannel));
+		}
+		else if (pAdapter->CommonCfg.PhyMode >= PHY_11ABGN_MIXED &&
+                 pAdapter->CommonCfg.RegTransmitSetting.field.BW == BW_40 &&
+                 pAdapter->CommonCfg.RegTransmitSetting.field.EXTCHA == EXTCHA_BELOW)
+		{
+			// 40MHz ,control channel at upper
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R4, &bbpValue);
+			bbpValue &= (~0x18);
+			bbpValue |= 0x10;
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R4, bbpValue);
+			pAdapter->CommonCfg.BBPCurrentBW = BW_40;
+			RTMP_IO_READ32(pAdapter, TX_BAND_CFG, &Value);
+			Value |= 0x1;
+			RTMP_IO_WRITE32(pAdapter, TX_BAND_CFG, Value);
+
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R3, &bbpValue);
+			bbpValue |= (0x20);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R3, bbpValue);
+			pAdapter->CommonCfg.CentralChannel = pAdapter->CommonCfg.Channel - 2;
+            AsicSwitchChannel(pAdapter, pAdapter->CommonCfg.CentralChannel, FALSE);
+		    AsicLockChannel(pAdapter, pAdapter->CommonCfg.CentralChannel);
+            DBGPRINT(RT_DEBUG_TRACE, ("BW_40 ,control_channel(%d), CentralChannel(%d) \n",
+                                       pAdapter->CommonCfg.Channel,
+                                       pAdapter->CommonCfg.CentralChannel));
+		}
+		else
+#endif // DOT11_N_SUPPORT //
+		{
+			// 20MHz
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R4, &bbpValue);
+			bbpValue &= (~0x18);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R4, bbpValue);
+			pAdapter->CommonCfg.BBPCurrentBW = BW_20;
+			AsicSwitchChannel(pAdapter, pAdapter->CommonCfg.Channel, FALSE);
+			AsicLockChannel(pAdapter, pAdapter->CommonCfg.Channel);
+			DBGPRINT(RT_DEBUG_TRACE, ("BW_20, Channel(%d)\n", pAdapter->CommonCfg.Channel));
+		}
+		// Enable Rx with promiscuous reception
+		RTMP_IO_WRITE32(pAdapter, RX_FILTR_CFG, 0x3);
+		// ASIC supporsts sniffer function with replacing RSSI with timestamp.
+		//RTMP_IO_READ32(pAdapter, MAC_SYS_CTRL, &Value);
+		//Value |= (0x80);
+		//RTMP_IO_WRITE32(pAdapter, MAC_SYS_CTRL, Value);
+		// disable sync
+		RTMP_IO_READ32(pAdapter, BCN_TIME_CFG, &csr.word);
+		csr.field.bBeaconGen = 0;
+		csr.field.bTBTTEnable = 0;
+		csr.field.TsfSyncMode = 0;
+		RTMP_IO_WRITE32(pAdapter, BCN_TIME_CFG, csr.word);
+
+		pAdapter->StaCfg.BssType = BSS_MONITOR;
+        pAdapter->net_dev->type = ARPHRD_IEEE80211_PRISM; //ARPHRD_IEEE80211; // IEEE80211
+		DBGPRINT(RT_DEBUG_TRACE, ("===>Set_NetworkType_Proc::(MONITOR)\n"));
+    }
+
+    // Reset Ralink supplicant to not use, it will be set to start when UI set PMK key
+    pAdapter->StaCfg.WpaState = SS_NOTUSE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_NetworkType_Proc::(NetworkType=%d)\n", pAdapter->StaCfg.BssType));
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Authentication mode
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_AuthMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    if ((strcmp(arg, "WEPAUTO") == 0) || (strcmp(arg, "wepauto") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeAutoSwitch;
+    else if ((strcmp(arg, "OPEN") == 0) || (strcmp(arg, "open") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+    else if ((strcmp(arg, "SHARED") == 0) || (strcmp(arg, "shared") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeShared;
+    else if ((strcmp(arg, "WPAPSK") == 0) || (strcmp(arg, "wpapsk") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPAPSK;
+    else if ((strcmp(arg, "WPANONE") == 0) || (strcmp(arg, "wpanone") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPANone;
+    else if ((strcmp(arg, "WPA2PSK") == 0) || (strcmp(arg, "wpa2psk") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA2PSK;
+#ifdef WPA_SUPPLICANT_SUPPORT
+    else if ((strcmp(arg, "WPA") == 0) || (strcmp(arg, "wpa") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA;
+    else if ((strcmp(arg, "WPA2") == 0) || (strcmp(arg, "wpa2") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA2;
+#endif // WPA_SUPPLICANT_SUPPORT //
+    else
+        return FALSE;
+
+    pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_AuthMode_Proc::(AuthMode=%d)\n", pAdapter->StaCfg.AuthMode));
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Encryption Type
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_EncrypType_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    if ((strcmp(arg, "NONE") == 0) || (strcmp(arg, "none") == 0))
+    {
+        if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+            return TRUE;    // do nothing
+
+        pAdapter->StaCfg.WepStatus     = Ndis802_11WEPDisabled;
+        pAdapter->StaCfg.PairCipher    = Ndis802_11WEPDisabled;
+	    pAdapter->StaCfg.GroupCipher   = Ndis802_11WEPDisabled;
+    }
+    else if ((strcmp(arg, "WEP") == 0) || (strcmp(arg, "wep") == 0))
+    {
+        if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+            return TRUE;    // do nothing
+
+        pAdapter->StaCfg.WepStatus     = Ndis802_11WEPEnabled;
+        pAdapter->StaCfg.PairCipher    = Ndis802_11WEPEnabled;
+	    pAdapter->StaCfg.GroupCipher   = Ndis802_11WEPEnabled;
+    }
+    else if ((strcmp(arg, "TKIP") == 0) || (strcmp(arg, "tkip") == 0))
+    {
+        if (pAdapter->StaCfg.AuthMode < Ndis802_11AuthModeWPA)
+            return TRUE;    // do nothing
+
+        pAdapter->StaCfg.WepStatus     = Ndis802_11Encryption2Enabled;
+        pAdapter->StaCfg.PairCipher    = Ndis802_11Encryption2Enabled;
+	    pAdapter->StaCfg.GroupCipher   = Ndis802_11Encryption2Enabled;
+    }
+    else if ((strcmp(arg, "AES") == 0) || (strcmp(arg, "aes") == 0))
+    {
+        if (pAdapter->StaCfg.AuthMode < Ndis802_11AuthModeWPA)
+            return TRUE;    // do nothing
+
+        pAdapter->StaCfg.WepStatus     = Ndis802_11Encryption3Enabled;
+        pAdapter->StaCfg.PairCipher    = Ndis802_11Encryption3Enabled;
+	    pAdapter->StaCfg.GroupCipher   = Ndis802_11Encryption3Enabled;
+    }
+    else
+        return FALSE;
+
+    pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_EncrypType_Proc::(EncrypType=%d)\n", pAdapter->StaCfg.WepStatus));
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Default Key ID
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_DefaultKeyID_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    ULONG                               KeyIdx;
+
+    KeyIdx = simple_strtol(arg, 0, 10);
+    if((KeyIdx >= 1 ) && (KeyIdx <= 4))
+        pAdapter->StaCfg.DefaultKeyId = (UCHAR) (KeyIdx - 1 );
+    else
+        return FALSE;  //Invalid argument
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_DefaultKeyID_Proc::(DefaultKeyID=%d)\n", pAdapter->StaCfg.DefaultKeyId));
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set WEP KEY1
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Key1_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    int                                 KeyLen;
+    int                                 i;
+    UCHAR                               CipherAlg=CIPHER_WEP64;
+
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        return TRUE;    // do nothing
+
+    KeyLen = strlen(arg);
+
+    switch (KeyLen)
+    {
+        case 5: //wep 40 Ascii type
+            pAdapter->SharedKey[BSS0][0].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][0].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::(Key1=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 10: //wep 40 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][0].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][0].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::(Key1=%s and type=%s)\n", arg, "Hex"));
+            break;
+        case 13: //wep 104 Ascii type
+            pAdapter->SharedKey[BSS0][0].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][0].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::(Key1=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 26: //wep 104 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][0].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][0].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::(Key1=%s and type=%s)\n", arg, "Hex"));
+            break;
+        default: //Invalid argument
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::Invalid argument (=%s)\n", arg));
+            return FALSE;
+    }
+
+    pAdapter->SharedKey[BSS0][0].CipherAlg = CipherAlg;
+
+    // Set keys (into ASIC)
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        ;   // not support
+    else    // Old WEP stuff
+    {
+        AsicAddSharedKeyEntry(pAdapter,
+                              0,
+                              0,
+                              pAdapter->SharedKey[BSS0][0].CipherAlg,
+                              pAdapter->SharedKey[BSS0][0].Key,
+                              NULL,
+                              NULL);
+    }
+
+    return TRUE;
+}
+/*
+    ==========================================================================
+
+    Description:
+        Set WEP KEY2
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Key2_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    int                                 KeyLen;
+    int                                 i;
+    UCHAR                               CipherAlg=CIPHER_WEP64;
+
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        return TRUE;    // do nothing
+
+    KeyLen = strlen(arg);
+
+    switch (KeyLen)
+    {
+        case 5: //wep 40 Ascii type
+            pAdapter->SharedKey[BSS0][1].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][1].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::(Key2=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 10: //wep 40 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][1].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][1].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::(Key2=%s and type=%s)\n", arg, "Hex"));
+            break;
+        case 13: //wep 104 Ascii type
+            pAdapter->SharedKey[BSS0][1].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][1].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::(Key2=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 26: //wep 104 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][1].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][1].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::(Key2=%s and type=%s)\n", arg, "Hex"));
+            break;
+        default: //Invalid argument
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::Invalid argument (=%s)\n", arg));
+            return FALSE;
+    }
+    pAdapter->SharedKey[BSS0][1].CipherAlg = CipherAlg;
+
+    // Set keys (into ASIC)
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        ;   // not support
+    else    // Old WEP stuff
+    {
+        AsicAddSharedKeyEntry(pAdapter,
+                              0,
+                              1,
+                              pAdapter->SharedKey[BSS0][1].CipherAlg,
+                              pAdapter->SharedKey[BSS0][1].Key,
+                              NULL,
+                              NULL);
+    }
+
+    return TRUE;
+}
+/*
+    ==========================================================================
+    Description:
+        Set WEP KEY3
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Key3_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    int                                 KeyLen;
+    int                                 i;
+    UCHAR                               CipherAlg=CIPHER_WEP64;
+
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        return TRUE;    // do nothing
+
+    KeyLen = strlen(arg);
+
+    switch (KeyLen)
+    {
+        case 5: //wep 40 Ascii type
+            pAdapter->SharedKey[BSS0][2].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][2].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::(Key3=%s and type=Ascii)\n", arg));
+            break;
+        case 10: //wep 40 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][2].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][2].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::(Key3=%s and type=Hex)\n", arg));
+            break;
+        case 13: //wep 104 Ascii type
+            pAdapter->SharedKey[BSS0][2].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][2].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::(Key3=%s and type=Ascii)\n", arg));
+            break;
+        case 26: //wep 104 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][2].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][2].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::(Key3=%s and type=Hex)\n", arg));
+            break;
+        default: //Invalid argument
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::Invalid argument (=%s)\n", arg));
+            return FALSE;
+    }
+    pAdapter->SharedKey[BSS0][2].CipherAlg = CipherAlg;
+
+    // Set keys (into ASIC)
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        ;   // not support
+    else    // Old WEP stuff
+    {
+        AsicAddSharedKeyEntry(pAdapter,
+                              0,
+                              2,
+                              pAdapter->SharedKey[BSS0][2].CipherAlg,
+                              pAdapter->SharedKey[BSS0][2].Key,
+                              NULL,
+                              NULL);
+    }
+
+    return TRUE;
+}
+/*
+    ==========================================================================
+    Description:
+        Set WEP KEY4
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Key4_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    int                                 KeyLen;
+    int                                 i;
+    UCHAR                               CipherAlg=CIPHER_WEP64;
+
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        return TRUE;    // do nothing
+
+    KeyLen = strlen(arg);
+
+    switch (KeyLen)
+    {
+        case 5: //wep 40 Ascii type
+            pAdapter->SharedKey[BSS0][3].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][3].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::(Key4=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 10: //wep 40 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][3].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][3].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::(Key4=%s and type=%s)\n", arg, "Hex"));
+            break;
+        case 13: //wep 104 Ascii type
+            pAdapter->SharedKey[BSS0][3].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][3].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::(Key4=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 26: //wep 104 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][3].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][3].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::(Key4=%s and type=%s)\n", arg, "Hex"));
+            break;
+        default: //Invalid argument
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::Invalid argument (=%s)\n", arg));
+            return FALSE;
+    }
+    pAdapter->SharedKey[BSS0][3].CipherAlg = CipherAlg;
+
+    // Set keys (into ASIC)
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        ;   // not support
+    else    // Old WEP stuff
+    {
+        AsicAddSharedKeyEntry(pAdapter,
+                              0,
+                              3,
+                              pAdapter->SharedKey[BSS0][3].CipherAlg,
+                              pAdapter->SharedKey[BSS0][3].Key,
+                              NULL,
+                              NULL);
+    }
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set WPA PSK key
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_WPAPSK_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    UCHAR                   keyMaterial[40];
+
+    if ((pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPAPSK) &&
+        (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPA2PSK) &&
+	    (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPANone)
+		)
+        return TRUE;    // do nothing
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_WPAPSK_Proc::(WPAPSK=%s)\n", arg));
+
+    NdisZeroMemory(keyMaterial, 40);
+
+    if ((strlen(arg) < 8) || (strlen(arg) > 64))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("Set failed!!(WPAPSK=%s), WPAPSK key-string required 8 ~ 64 characters \n", arg));
+        return FALSE;
+    }
+
+    if (strlen(arg) == 64)
+    {
+        AtoH(arg, keyMaterial, 32);
+        NdisMoveMemory(pAdapter->StaCfg.PMK, keyMaterial, 32);
+
+    }
+    else
+    {
+        PasswordHash((char *)arg, pAdapter->MlmeAux.Ssid, pAdapter->MlmeAux.SsidLen, keyMaterial);
+        NdisMoveMemory(pAdapter->StaCfg.PMK, keyMaterial, 32);
+    }
+
+
+
+    if(pAdapter->StaCfg.BssType == BSS_ADHOC &&
+       pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+    {
+         pAdapter->StaCfg.WpaState = SS_NOTUSE;
+    }
+    else
+    {
+        // Start STA supplicant state machine
+        pAdapter->StaCfg.WpaState = SS_START;
+    }
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Power Saving mode
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_PSMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    if (pAdapter->StaCfg.BssType == BSS_INFRA)
+    {
+        if ((strcmp(arg, "Max_PSP") == 0) ||
+			(strcmp(arg, "max_psp") == 0) ||
+			(strcmp(arg, "MAX_PSP") == 0))
+        {
+            // do NOT turn on PSM bit here, wait until MlmeCheckForPsmChange()
+            // to exclude certain situations.
+            if (pAdapter->StaCfg.bWindowsACCAMEnable == FALSE)
+                pAdapter->StaCfg.WindowsPowerMode = Ndis802_11PowerModeMAX_PSP;
+            pAdapter->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeMAX_PSP;
+            OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_RECEIVE_DTIM);
+            pAdapter->StaCfg.DefaultListenCount = 5;
+
+        }
+        else if ((strcmp(arg, "Fast_PSP") == 0) ||
+				 (strcmp(arg, "fast_psp") == 0) ||
+                 (strcmp(arg, "FAST_PSP") == 0))
+        {
+            // do NOT turn on PSM bit here, wait until MlmeCheckForPsmChange()
+            // to exclude certain situations.
+            OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_RECEIVE_DTIM);
+            if (pAdapter->StaCfg.bWindowsACCAMEnable == FALSE)
+                pAdapter->StaCfg.WindowsPowerMode = Ndis802_11PowerModeFast_PSP;
+            pAdapter->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeFast_PSP;
+            pAdapter->StaCfg.DefaultListenCount = 3;
+        }
+        else if ((strcmp(arg, "Legacy_PSP") == 0) ||
+                 (strcmp(arg, "legacy_psp") == 0) ||
+                 (strcmp(arg, "LEGACY_PSP") == 0))
+        {
+            // do NOT turn on PSM bit here, wait until MlmeCheckForPsmChange()
+            // to exclude certain situations.
+            OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_RECEIVE_DTIM);
+            if (pAdapter->StaCfg.bWindowsACCAMEnable == FALSE)
+                pAdapter->StaCfg.WindowsPowerMode = Ndis802_11PowerModeLegacy_PSP;
+            pAdapter->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeLegacy_PSP;
+            pAdapter->StaCfg.DefaultListenCount = 3;
+        }
+        else
+        {
+            //Default Ndis802_11PowerModeCAM
+            // clear PSM bit immediately
+            MlmeSetPsmBit(pAdapter, PWR_ACTIVE);
+            OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_RECEIVE_DTIM);
+            if (pAdapter->StaCfg.bWindowsACCAMEnable == FALSE)
+                pAdapter->StaCfg.WindowsPowerMode = Ndis802_11PowerModeCAM;
+            pAdapter->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeCAM;
+        }
+
+        DBGPRINT(RT_DEBUG_TRACE, ("Set_PSMode_Proc::(PSMode=%ld)\n", pAdapter->StaCfg.WindowsPowerMode));
+    }
+    else
+        return FALSE;
+
+
+    return TRUE;
+}
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+/*
+    ==========================================================================
+    Description:
+        Set WpaSupport flag.
+    Value:
+        0: Driver ignore wpa_supplicant.
+        1: wpa_supplicant initiates scanning and AP selection.
+        2: driver takes care of scanning, AP selection, and IEEE 802.11 association parameters.
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Wpa_Support(
+    IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+
+    if ( simple_strtol(arg, 0, 10) == 0)
+        pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_DISABLE;
+    else if ( simple_strtol(arg, 0, 10) == 1)
+        pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_ENABLE;
+    else if ( simple_strtol(arg, 0, 10) == 2)
+        pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_ENABLE_WITH_WEB_UI;
+    else
+        pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_DISABLE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_Wpa_Support::(WpaSupplicantUP=%d)\n", pAd->StaCfg.WpaSupplicantUP));
+
+    return TRUE;
+}
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef DBG
+/*
+    ==========================================================================
+    Description:
+        Read / Write MAC
+    Arguments:
+        pAdapter                    Pointer to our adapter
+        wrq                         Pointer to the ioctl argument
+
+    Return Value:
+        None
+
+    Note:
+        Usage:
+               1.) iwpriv ra0 mac 0        ==> read MAC where Addr=0x0
+               2.) iwpriv ra0 mac 0=12     ==> write MAC where Addr=0x0, value=12
+    ==========================================================================
+*/
+VOID RTMPIoctlMAC(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq)
+{
+	CHAR				*this_char;
+	CHAR				*value;
+	INT					j = 0, k = 0;
+	CHAR				msg[1024];
+	CHAR				arg[255];
+	ULONG				macAddr = 0;
+	UCHAR				temp[16], temp2[16];
+	UINT32				macValue = 0;
+	INT					Status;
+
+
+	memset(msg, 0x00, 1024);
+	if (wrq->u.data.length > 1) //No parameters.
+	{
+	    Status = copy_from_user(arg, wrq->u.data.pointer, (wrq->u.data.length > 255) ? 255 : wrq->u.data.length);
+		sprintf(msg, "\n");
+
+		//Parsing Read or Write
+	    this_char = arg;
+		if (!*this_char)
+			goto next;
+
+		if ((value = rtstrchr(this_char, '=')) != NULL)
+			*value++ = 0;
+
+		if (!value || !*value)
+		{ //Read
+			// Sanity check
+			if(strlen(this_char) > 4)
+				goto next;
+
+			j = strlen(this_char);
+			while(j-- > 0)
+			{
+				if(this_char[j] > 'f' || this_char[j] < '0')
+					return;
+			}
+
+			// Mac Addr
+			k = j = strlen(this_char);
+			while(j-- > 0)
+			{
+				this_char[4-k+j] = this_char[j];
+			}
+
+			while(k < 4)
+				this_char[3-k++]='0';
+			this_char[4]='\0';
+
+			if(strlen(this_char) == 4)
+			{
+				AtoH(this_char, temp, 2);
+				macAddr = *temp*256 + temp[1];
+				if (macAddr < 0xFFFF)
+				{
+					RTMP_IO_READ32(pAdapter, macAddr, &macValue);
+					DBGPRINT(RT_DEBUG_TRACE, ("MacAddr=%lx, MacValue=%x\n", macAddr, macValue));
+					sprintf(msg+strlen(msg), "[0x%08lX]:%08X  ", macAddr , macValue);
+				}
+				else
+				{//Invalid parametes, so default printk all bbp
+					goto next;
+				}
+			}
+		}
+		else
+		{ //Write
+			memcpy(&temp2, value, strlen(value));
+			temp2[strlen(value)] = '\0';
+
+			// Sanity check
+			if((strlen(this_char) > 4) || strlen(temp2) > 8)
+				goto next;
+
+			j = strlen(this_char);
+			while(j-- > 0)
+			{
+				if(this_char[j] > 'f' || this_char[j] < '0')
+					return;
+			}
+
+			j = strlen(temp2);
+			while(j-- > 0)
+			{
+				if(temp2[j] > 'f' || temp2[j] < '0')
+					return;
+			}
+
+			//MAC Addr
+			k = j = strlen(this_char);
+			while(j-- > 0)
+			{
+				this_char[4-k+j] = this_char[j];
+			}
+
+			while(k < 4)
+				this_char[3-k++]='0';
+			this_char[4]='\0';
+
+			//MAC value
+			k = j = strlen(temp2);
+			while(j-- > 0)
+			{
+				temp2[8-k+j] = temp2[j];
+			}
+
+			while(k < 8)
+				temp2[7-k++]='0';
+			temp2[8]='\0';
+
+			{
+				AtoH(this_char, temp, 2);
+				macAddr = *temp*256 + temp[1];
+
+				AtoH(temp2, temp, 4);
+				macValue = *temp*256*256*256 + temp[1]*256*256 + temp[2]*256 + temp[3];
+
+				// debug mode
+				if (macAddr == (HW_DEBUG_SETTING_BASE + 4))
+				{
+					// 0x2bf4: byte0 non-zero: enable R17 tuning, 0: disable R17 tuning
+                    if (macValue & 0x000000ff)
+                    {
+                        pAdapter->BbpTuning.bEnable = TRUE;
+                        DBGPRINT(RT_DEBUG_TRACE,("turn on R17 tuning\n"));
+                    }
+                    else
+                    {
+                        UCHAR R66;
+                        pAdapter->BbpTuning.bEnable = FALSE;
+                        R66 = 0x26 + GET_LNA_GAIN(pAdapter);
+#ifdef RALINK_ATE
+						if (ATE_ON(pAdapter))
+						{
+							ATE_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R66, (0x26 + GET_LNA_GAIN(pAdapter)));
+						}
+						else
+#endif // RALINK_ATE //
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R66, (0x26 + GET_LNA_GAIN(pAdapter)));
+                        DBGPRINT(RT_DEBUG_TRACE,("turn off R17 tuning, restore to 0x%02x\n", R66));
+                    }
+					return;
+				}
+
+				DBGPRINT(RT_DEBUG_TRACE, ("MacAddr=%02lx, MacValue=0x%x\n", macAddr, macValue));
+
+				RTMP_IO_WRITE32(pAdapter, macAddr, macValue);
+				sprintf(msg+strlen(msg), "[0x%08lX]:%08X  ", macAddr, macValue);
+			}
+		}
+	}
+next:
+	if(strlen(msg) == 1)
+		sprintf(msg+strlen(msg), "===>Error command format!");
+
+	// Copy the information into the user buffer
+	wrq->u.data.length = strlen(msg);
+	Status = copy_to_user(wrq->u.data.pointer, msg, wrq->u.data.length);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<==RTMPIoctlMAC\n\n"));
+}
+
+/*
+    ==========================================================================
+    Description:
+        Read / Write E2PROM
+    Arguments:
+        pAdapter                    Pointer to our adapter
+        wrq                         Pointer to the ioctl argument
+
+    Return Value:
+        None
+
+    Note:
+        Usage:
+               1.) iwpriv ra0 e2p 0     	==> read E2PROM where Addr=0x0
+               2.) iwpriv ra0 e2p 0=1234    ==> write E2PROM where Addr=0x0, value=1234
+    ==========================================================================
+*/
+VOID RTMPIoctlE2PROM(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq)
+{
+	CHAR				*this_char;
+	CHAR				*value;
+	INT					j = 0, k = 0;
+	CHAR				msg[1024];
+	CHAR				arg[255];
+	USHORT				eepAddr = 0;
+	UCHAR				temp[16], temp2[16];
+	USHORT				eepValue;
+	int					Status;
+
+
+	memset(msg, 0x00, 1024);
+	if (wrq->u.data.length > 1) //No parameters.
+	{
+	    Status = copy_from_user(arg, wrq->u.data.pointer, (wrq->u.data.length > 255) ? 255 : wrq->u.data.length);
+		sprintf(msg, "\n");
+
+	    //Parsing Read or Write
+		this_char = arg;
+
+
+		if (!*this_char)
+			goto next;
+
+		if ((value = rtstrchr(this_char, '=')) != NULL)
+			*value++ = 0;
+
+		if (!value || !*value)
+		{ //Read
+
+			// Sanity check
+			if(strlen(this_char) > 4)
+				goto next;
+
+			j = strlen(this_char);
+			while(j-- > 0)
+			{
+				if(this_char[j] > 'f' || this_char[j] < '0')
+					return;
+			}
+
+			// E2PROM addr
+			k = j = strlen(this_char);
+			while(j-- > 0)
+			{
+				this_char[4-k+j] = this_char[j];
+			}
+
+			while(k < 4)
+				this_char[3-k++]='0';
+			this_char[4]='\0';
+
+			if(strlen(this_char) == 4)
+			{
+				AtoH(this_char, temp, 2);
+				eepAddr = *temp*256 + temp[1];
+				if (eepAddr < 0xFFFF)
+				{
+					RT28xx_EEPROM_READ16(pAdapter, eepAddr, eepValue);
+					sprintf(msg+strlen(msg), "[0x%04X]:0x%04X  ", eepAddr , eepValue);
+				}
+				else
+				{//Invalid parametes, so default printk all bbp
+					goto next;
+				}
+			}
+		}
+		else
+		{ //Write
+			memcpy(&temp2, value, strlen(value));
+			temp2[strlen(value)] = '\0';
+
+			// Sanity check
+			if((strlen(this_char) > 4) || strlen(temp2) > 8)
+				goto next;
+
+			j = strlen(this_char);
+			while(j-- > 0)
+			{
+				if(this_char[j] > 'f' || this_char[j] < '0')
+					return;
+			}
+			j = strlen(temp2);
+			while(j-- > 0)
+			{
+				if(temp2[j] > 'f' || temp2[j] < '0')
+					return;
+			}
+
+			//MAC Addr
+			k = j = strlen(this_char);
+			while(j-- > 0)
+			{
+				this_char[4-k+j] = this_char[j];
+			}
+
+			while(k < 4)
+				this_char[3-k++]='0';
+			this_char[4]='\0';
+
+			//MAC value
+			k = j = strlen(temp2);
+			while(j-- > 0)
+			{
+				temp2[4-k+j] = temp2[j];
+			}
+
+			while(k < 4)
+				temp2[3-k++]='0';
+			temp2[4]='\0';
+
+			AtoH(this_char, temp, 2);
+			eepAddr = *temp*256 + temp[1];
+
+			AtoH(temp2, temp, 2);
+			eepValue = *temp*256 + temp[1];
+
+			RT28xx_EEPROM_WRITE16(pAdapter, eepAddr, eepValue);
+			sprintf(msg+strlen(msg), "[0x%02X]:%02X  ", eepAddr, eepValue);
+		}
+	}
+next:
+	if(strlen(msg) == 1)
+		sprintf(msg+strlen(msg), "===>Error command format!");
+
+
+	// Copy the information into the user buffer
+	wrq->u.data.length = strlen(msg);
+	Status = copy_to_user(wrq->u.data.pointer, msg, wrq->u.data.length);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<==RTMPIoctlE2PROM\n"));
+}
+#endif // DBG //
+
+
+
+
+INT Set_TGnWifiTest_Proc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          arg)
+{
+    if (simple_strtol(arg, 0, 10) == 0)
+        pAd->StaCfg.bTGnWifiTest = FALSE;
+    else
+        pAd->StaCfg.bTGnWifiTest = TRUE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("IF Set_TGnWifiTest_Proc::(bTGnWifiTest=%d)\n", pAd->StaCfg.bTGnWifiTest));
+	return TRUE;
+}
+
+INT Set_LongRetryLimit_Proc(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PUCHAR			arg)
+{
+	TX_RTY_CFG_STRUC	tx_rty_cfg;
+	UCHAR				LongRetryLimit = (UCHAR)simple_strtol(arg, 0, 10);
+
+	RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+	tx_rty_cfg.field.LongRtyLimit = LongRetryLimit;
+	RTMP_IO_WRITE32(pAdapter, TX_RTY_CFG, tx_rty_cfg.word);
+	DBGPRINT(RT_DEBUG_TRACE, ("IF Set_LongRetryLimit_Proc::(tx_rty_cfg=0x%x)\n", tx_rty_cfg.word));
+	return TRUE;
+}
+
+INT Set_ShortRetryLimit_Proc(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PUCHAR			arg)
+{
+	TX_RTY_CFG_STRUC	tx_rty_cfg;
+	UCHAR				ShortRetryLimit = (UCHAR)simple_strtol(arg, 0, 10);
+
+	RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+	tx_rty_cfg.field.ShortRtyLimit = ShortRetryLimit;
+	RTMP_IO_WRITE32(pAdapter, TX_RTY_CFG, tx_rty_cfg.word);
+	DBGPRINT(RT_DEBUG_TRACE, ("IF Set_ShortRetryLimit_Proc::(tx_rty_cfg=0x%x)\n", tx_rty_cfg.word));
+	return TRUE;
+}
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+INT Set_Ieee80211dClientMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    if (simple_strtol(arg, 0, 10) == 0)
+        pAdapter->StaCfg.IEEE80211dClientMode = Rt802_11_D_None;
+    else if (simple_strtol(arg, 0, 10) == 1)
+        pAdapter->StaCfg.IEEE80211dClientMode = Rt802_11_D_Flexible;
+    else if (simple_strtol(arg, 0, 10) == 2)
+        pAdapter->StaCfg.IEEE80211dClientMode = Rt802_11_D_Strict;
+    else
+        return FALSE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_Ieee802dMode_Proc::(IEEEE0211dMode=%d)\n", pAdapter->StaCfg.IEEE80211dClientMode));
+    return TRUE;
+}
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+#ifdef CARRIER_DETECTION_SUPPORT
+INT Set_CarrierDetect_Proc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          arg)
+{
+    if (simple_strtol(arg, 0, 10) == 0)
+        pAd->CommonCfg.CarrierDetect.Enable = FALSE;
+    else
+        pAd->CommonCfg.CarrierDetect.Enable = TRUE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("IF Set_CarrierDetect_Proc::(CarrierDetect.Enable=%d)\n", pAd->CommonCfg.CarrierDetect.Enable));
+	return TRUE;
+}
+#endif // CARRIER_DETECTION_SUPPORT //
+
+
+INT	Show_Adhoc_MacTable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PCHAR			extra)
+{
+	INT i;
+
+	sprintf(extra, "\n");
+
+#ifdef DOT11_N_SUPPORT
+	sprintf(extra, "%sHT Operating Mode : %d\n", extra, pAd->CommonCfg.AddHTInfo.AddHtInfo2.OperaionMode);
+#endif // DOT11_N_SUPPORT //
+
+	sprintf(extra, "%s\n%-19s%-4s%-4s%-7s%-7s%-7s%-10s%-6s%-6s%-6s%-6s\n", extra,
+			"MAC", "AID", "BSS", "RSSI0", "RSSI1", "RSSI2", "PhMd", "BW", "MCS", "SGI", "STBC");
+
+	for (i=1; i<MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		PMAC_TABLE_ENTRY pEntry = &pAd->MacTab.Content[i];
+
+		if (strlen(extra) > (IW_PRIV_SIZE_MASK - 30))
+		    break;
+		if ((pEntry->ValidAsCLI || pEntry->ValidAsApCli) && (pEntry->Sst == SST_ASSOC))
+		{
+			sprintf(extra, "%s%02X:%02X:%02X:%02X:%02X:%02X  ", extra,
+				pEntry->Addr[0], pEntry->Addr[1], pEntry->Addr[2],
+				pEntry->Addr[3], pEntry->Addr[4], pEntry->Addr[5]);
+			sprintf(extra, "%s%-4d", extra, (int)pEntry->Aid);
+			sprintf(extra, "%s%-4d", extra, (int)pEntry->apidx);
+			sprintf(extra, "%s%-7d", extra, pEntry->RssiSample.AvgRssi0);
+			sprintf(extra, "%s%-7d", extra, pEntry->RssiSample.AvgRssi1);
+			sprintf(extra, "%s%-7d", extra, pEntry->RssiSample.AvgRssi2);
+			sprintf(extra, "%s%-10s", extra, GetPhyMode(pEntry->HTPhyMode.field.MODE));
+			sprintf(extra, "%s%-6s", extra, GetBW(pEntry->HTPhyMode.field.BW));
+			sprintf(extra, "%s%-6d", extra, pEntry->HTPhyMode.field.MCS);
+			sprintf(extra, "%s%-6d", extra, pEntry->HTPhyMode.field.ShortGI);
+			sprintf(extra, "%s%-6d", extra, pEntry->HTPhyMode.field.STBC);
+			sprintf(extra, "%s%-10d, %d, %d%%\n", extra, pEntry->DebugFIFOCount, pEntry->DebugTxCount,
+						(pEntry->DebugTxCount) ? ((pEntry->DebugTxCount-pEntry->DebugFIFOCount)*100/pEntry->DebugTxCount) : 0);
+			sprintf(extra, "%s\n", extra);
+		}
+	}
+
+	return TRUE;
+}
+
+
diff --git a/drivers/staging/rt2870/sta_ioctl.c.patch b/drivers/staging/rt2870/sta_ioctl.c.patch
new file mode 100644
index 00000000000..8672b147a76
--- /dev/null
+++ b/drivers/staging/rt2870/sta_ioctl.c.patch
@@ -0,0 +1,18 @@
+--- sta_ioctl.c	2008-09-19 14:37:52.000000000 +0800
++++ sta_ioctl.c.fc9	2008-09-19 14:38:20.000000000 +0800
+@@ -49,15 +49,9 @@
+
+ #define GROUP_KEY_NO                4
+
+-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
+ #define IWE_STREAM_ADD_EVENT(_A, _B, _C, _D, _E)		iwe_stream_add_event(_A, _B, _C, _D, _E)
+ #define IWE_STREAM_ADD_POINT(_A, _B, _C, _D, _E)		iwe_stream_add_point(_A, _B, _C, _D, _E)
+ #define IWE_STREAM_ADD_VALUE(_A, _B, _C, _D, _E, _F)	iwe_stream_add_value(_A, _B, _C, _D, _E, _F)
+-#else
+-#define IWE_STREAM_ADD_EVENT(_A, _B, _C, _D, _E)		iwe_stream_add_event(_B, _C, _D, _E)
+-#define IWE_STREAM_ADD_POINT(_A, _B, _C, _D, _E)		iwe_stream_add_point(_B, _C, _D, _E)
+-#define IWE_STREAM_ADD_VALUE(_A, _B, _C, _D, _E, _F)	iwe_stream_add_value(_B, _C, _D, _E, _F)
+-#endif
+
+ extern UCHAR    CipherWpa2Template[];
+ extern UCHAR    CipherWpaPskTkip[];
diff --git a/drivers/staging/rt2870/tmp60 b/drivers/staging/rt2870/tmp60
new file mode 100644
index 00000000000..992096a248c
--- /dev/null
+++ b/drivers/staging/rt2870/tmp60
@@ -0,0 +1,7037 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    sta_ioctl.c
+
+    Abstract:
+    IOCTL related subroutines
+
+    Revision History:
+    Who         When          What
+    --------    ----------    ----------------------------------------------
+    Rory Chen   01-03-2003    created
+	Rory Chen   02-14-2005    modify to support RT61
+*/
+
+#include	"rt_config.h"
+
+#ifdef DBG
+extern ULONG    RTDebugLevel;
+#endif
+
+#define NR_WEP_KEYS 				4
+#define WEP_SMALL_KEY_LEN 			(40/8)
+#define WEP_LARGE_KEY_LEN 			(104/8)
+
+#define GROUP_KEY_NO                4
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
+#define IWE_STREAM_ADD_EVENT(_A, _B, _C, _D, _E)		iwe_stream_add_event(_A, _B, _C, _D, _E)
+#define IWE_STREAM_ADD_POINT(_A, _B, _C, _D, _E)		iwe_stream_add_point(_A, _B, _C, _D, _E)
+#define IWE_STREAM_ADD_VALUE(_A, _B, _C, _D, _E, _F)	iwe_stream_add_value(_A, _B, _C, _D, _E, _F)
+#else
+#define IWE_STREAM_ADD_EVENT(_A, _B, _C, _D, _E)		iwe_stream_add_event(_B, _C, _D, _E)
+#define IWE_STREAM_ADD_POINT(_A, _B, _C, _D, _E)		iwe_stream_add_point(_B, _C, _D, _E)
+#define IWE_STREAM_ADD_VALUE(_A, _B, _C, _D, _E, _F)	iwe_stream_add_value(_B, _C, _D, _E, _F)
+#endif
+
+extern UCHAR    CipherWpa2Template[];
+extern UCHAR    CipherWpaPskTkip[];
+extern UCHAR    CipherWpaPskTkipLen;
+
+typedef struct PACKED _RT_VERSION_INFO{
+    UCHAR       DriverVersionW;
+    UCHAR       DriverVersionX;
+    UCHAR       DriverVersionY;
+    UCHAR       DriverVersionZ;
+    UINT        DriverBuildYear;
+    UINT        DriverBuildMonth;
+    UINT        DriverBuildDay;
+} RT_VERSION_INFO, *PRT_VERSION_INFO;
+
+struct iw_priv_args privtab[] = {
+{ RTPRIV_IOCTL_SET,
+  IW_PRIV_TYPE_CHAR | 1024, 0,
+  "set"},
+
+{ RTPRIV_IOCTL_SHOW, 0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK,
+  ""},
+{ RTPRIV_IOCTL_SHOW, IW_PRIV_TYPE_CHAR | 1024, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK,
+  ""},
+/* --- sub-ioctls definitions --- */
+    { SHOW_CONN_STATUS,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "connStatus" },
+	{ SHOW_DRVIER_VERION,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "driverVer" },
+    { SHOW_BA_INFO,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "bainfo" },
+	{ SHOW_DESC_INFO,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "descinfo" },
+    { RAIO_OFF,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "radio_off" },
+	{ RAIO_ON,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "radio_on" },
+#ifdef QOS_DLS_SUPPORT
+	{ SHOW_DLS_ENTRY_INFO,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "dlsentryinfo" },
+#endif // QOS_DLS_SUPPORT //
+	{ SHOW_CFG_VALUE,
+	  IW_PRIV_TYPE_CHAR | 1024, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "show" },
+	{ SHOW_ADHOC_ENTRY_INFO,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "adhocEntry" },
+
+/* --- sub-ioctls relations --- */
+
+#ifdef DBG
+{ RTPRIV_IOCTL_BBP,
+  IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK,
+  "bbp"},
+{ RTPRIV_IOCTL_MAC,
+  IW_PRIV_TYPE_CHAR | 1024, IW_PRIV_TYPE_CHAR | 1024,
+  "mac"},
+{ RTPRIV_IOCTL_E2P,
+  IW_PRIV_TYPE_CHAR | 1024, IW_PRIV_TYPE_CHAR | 1024,
+  "e2p"},
+#endif  /* DBG */
+
+{ RTPRIV_IOCTL_STATISTICS,
+  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK,
+  "stat"},
+{ RTPRIV_IOCTL_GSITESURVEY,
+  0, IW_PRIV_TYPE_CHAR | 1024,
+  "get_site_survey"},
+};
+
+INT Set_SSID_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+#ifdef WMM_SUPPORT
+INT	Set_WmmCapable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif
+
+INT Set_NetworkType_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_AuthMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_EncrypType_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_DefaultKeyID_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_Key1_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_Key2_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_Key3_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_Key4_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_WPAPSK_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+
+INT Set_PSMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+INT Set_Wpa_Support(
+    IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef DBG
+VOID RTMPIoctlBBP(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq);
+
+VOID RTMPIoctlMAC(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq);
+
+VOID RTMPIoctlE2PROM(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  struct iwreq    *wrq);
+#endif // DBG //
+
+
+NDIS_STATUS RTMPWPANoneAddKeyProc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN	PVOID			pBuf);
+
+INT Set_FragTest_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+#ifdef DOT11_N_SUPPORT
+INT Set_TGnWifiTest_Proc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          arg);
+#endif // DOT11_N_SUPPORT //
+
+INT Set_LongRetryLimit_Proc(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PUCHAR			arg);
+
+INT Set_ShortRetryLimit_Proc(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PUCHAR			arg);
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+INT Set_Ieee80211dClientMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+#ifdef CARRIER_DETECTION_SUPPORT
+INT Set_CarrierDetect_Proc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          arg);
+#endif // CARRIER_DETECTION_SUPPORT //
+
+INT	Show_Adhoc_MacTable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PCHAR			extra);
+
+static struct {
+	CHAR *name;
+	INT (*set_proc)(PRTMP_ADAPTER pAdapter, PUCHAR arg);
+} *PRTMP_PRIVATE_SET_PROC, RTMP_PRIVATE_SUPPORT_PROC[] = {
+	{"DriverVersion",				Set_DriverVersion_Proc},
+	{"CountryRegion",				Set_CountryRegion_Proc},
+	{"CountryRegionABand",			Set_CountryRegionABand_Proc},
+	{"SSID",						Set_SSID_Proc},
+	{"WirelessMode",				Set_WirelessMode_Proc},
+	{"TxBurst",					Set_TxBurst_Proc},
+	{"TxPreamble",				Set_TxPreamble_Proc},
+	{"TxPower",					Set_TxPower_Proc},
+	{"Channel",					Set_Channel_Proc},
+	{"BGProtection",				Set_BGProtection_Proc},
+	{"RTSThreshold",				Set_RTSThreshold_Proc},
+	{"FragThreshold",				Set_FragThreshold_Proc},
+#ifdef DOT11_N_SUPPORT
+	{"HtBw",		                Set_HtBw_Proc},
+	{"HtMcs",		                Set_HtMcs_Proc},
+	{"HtGi",		                Set_HtGi_Proc},
+	{"HtOpMode",		            Set_HtOpMode_Proc},
+	{"HtExtcha",		            Set_HtExtcha_Proc},
+	{"HtMpduDensity",		        Set_HtMpduDensity_Proc},
+	{"HtBaWinSize",		        	Set_HtBaWinSize_Proc},
+	{"HtRdg",		        		Set_HtRdg_Proc},
+	{"HtAmsdu",		        		Set_HtAmsdu_Proc},
+	{"HtAutoBa",		        	Set_HtAutoBa_Proc},
+	{"HtBaDecline",					Set_BADecline_Proc},
+	{"HtProtect",		        	Set_HtProtect_Proc},
+	{"HtMimoPs",		        	Set_HtMimoPs_Proc},
+#endif // DOT11_N_SUPPORT //
+
+#ifdef AGGREGATION_SUPPORT
+	{"PktAggregate",				Set_PktAggregate_Proc},
+#endif
+
+#ifdef WMM_SUPPORT
+	{"WmmCapable",					Set_WmmCapable_Proc},
+#endif
+	{"IEEE80211H",					Set_IEEE80211H_Proc},
+    {"NetworkType",                 Set_NetworkType_Proc},
+	{"AuthMode",					Set_AuthMode_Proc},
+	{"EncrypType",					Set_EncrypType_Proc},
+	{"DefaultKeyID",				Set_DefaultKeyID_Proc},
+	{"Key1",						Set_Key1_Proc},
+	{"Key2",						Set_Key2_Proc},
+	{"Key3",						Set_Key3_Proc},
+	{"Key4",						Set_Key4_Proc},
+	{"WPAPSK",						Set_WPAPSK_Proc},
+	{"ResetCounter",				Set_ResetStatCounter_Proc},
+	{"PSMode",                      Set_PSMode_Proc},
+#ifdef DBG
+	{"Debug",						Set_Debug_Proc},
+#endif
+
+#ifdef RALINK_ATE
+	{"ATE",							Set_ATE_Proc},
+	{"ATEDA",						Set_ATE_DA_Proc},
+	{"ATESA",						Set_ATE_SA_Proc},
+	{"ATEBSSID",					Set_ATE_BSSID_Proc},
+	{"ATECHANNEL",					Set_ATE_CHANNEL_Proc},
+	{"ATETXPOW0",					Set_ATE_TX_POWER0_Proc},
+	{"ATETXPOW1",					Set_ATE_TX_POWER1_Proc},
+	{"ATETXANT",					Set_ATE_TX_Antenna_Proc},
+	{"ATERXANT",					Set_ATE_RX_Antenna_Proc},
+	{"ATETXFREQOFFSET",				Set_ATE_TX_FREQOFFSET_Proc},
+	{"ATETXBW",						Set_ATE_TX_BW_Proc},
+	{"ATETXLEN",					Set_ATE_TX_LENGTH_Proc},
+	{"ATETXCNT",					Set_ATE_TX_COUNT_Proc},
+	{"ATETXMCS",					Set_ATE_TX_MCS_Proc},
+	{"ATETXMODE",					Set_ATE_TX_MODE_Proc},
+	{"ATETXGI",						Set_ATE_TX_GI_Proc},
+	{"ATERXFER",					Set_ATE_RX_FER_Proc},
+	{"ATERRF",						Set_ATE_Read_RF_Proc},
+	{"ATEWRF1",						Set_ATE_Write_RF1_Proc},
+	{"ATEWRF2",						Set_ATE_Write_RF2_Proc},
+	{"ATEWRF3",						Set_ATE_Write_RF3_Proc},
+	{"ATEWRF4",						Set_ATE_Write_RF4_Proc},
+	{"ATELDE2P",				    Set_ATE_Load_E2P_Proc},
+	{"ATERE2P",						Set_ATE_Read_E2P_Proc},
+	{"ATESHOW",						Set_ATE_Show_Proc},
+	{"ATEHELP",						Set_ATE_Help_Proc},
+
+#ifdef RALINK_28xx_QA
+	{"TxStop",						Set_TxStop_Proc},
+	{"RxStop",						Set_RxStop_Proc},
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+    {"WpaSupport",                  Set_Wpa_Support},
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+
+
+	{"FixedTxMode",                 Set_FixedTxMode_Proc},
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+	{"OpMode",						Set_OpMode_Proc},
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+#ifdef DOT11_N_SUPPORT
+    {"TGnWifiTest",                 Set_TGnWifiTest_Proc},
+    {"ForceGF",		        		Set_ForceGF_Proc},
+#endif // DOT11_N_SUPPORT //
+#ifdef QOS_DLS_SUPPORT
+	{"DlsAddEntry",					Set_DlsAddEntry_Proc},
+	{"DlsTearDownEntry",			Set_DlsTearDownEntry_Proc},
+#endif // QOS_DLS_SUPPORT //
+	{"LongRetry",	        		Set_LongRetryLimit_Proc},
+	{"ShortRetry",	        		Set_ShortRetryLimit_Proc},
+#ifdef EXT_BUILD_CHANNEL_LIST
+	{"11dClientMode",				Set_Ieee80211dClientMode_Proc},
+#endif // EXT_BUILD_CHANNEL_LIST //
+#ifdef CARRIER_DETECTION_SUPPORT
+	{"CarrierDetect",				Set_CarrierDetect_Proc},
+#endif // CARRIER_DETECTION_SUPPORT //
+
+	{NULL,}
+};
+
+
+VOID RTMPAddKey(
+	IN	PRTMP_ADAPTER	    pAd,
+	IN	PNDIS_802_11_KEY    pKey)
+{
+	ULONG				KeyIdx;
+	MAC_TABLE_ENTRY  	*pEntry;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("RTMPAddKey ------>\n"));
+
+	if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+	{
+		if (pKey->KeyIndex & 0x80000000)
+		{
+		    if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+            {
+                NdisZeroMemory(pAd->StaCfg.PMK, 32);
+                NdisMoveMemory(pAd->StaCfg.PMK, pKey->KeyMaterial, pKey->KeyLength);
+                goto end;
+            }
+		    // Update PTK
+		    NdisZeroMemory(&pAd->SharedKey[BSS0][0], sizeof(CIPHER_KEY));
+            pAd->SharedKey[BSS0][0].KeyLen = LEN_TKIP_EK;
+            NdisMoveMemory(pAd->SharedKey[BSS0][0].Key, pKey->KeyMaterial, LEN_TKIP_EK);
+#ifdef WPA_SUPPLICANT_SUPPORT
+            if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
+            {
+                NdisMoveMemory(pAd->SharedKey[BSS0][0].RxMic, pKey->KeyMaterial + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+                NdisMoveMemory(pAd->SharedKey[BSS0][0].TxMic, pKey->KeyMaterial + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+            }
+            else
+#endif // WPA_SUPPLICANT_SUPPORT //
+            {
+            	NdisMoveMemory(pAd->SharedKey[BSS0][0].TxMic, pKey->KeyMaterial + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+                NdisMoveMemory(pAd->SharedKey[BSS0][0].RxMic, pKey->KeyMaterial + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+            }
+
+            // Decide its ChiperAlg
+        	if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
+        		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_TKIP;
+        	else if (pAd->StaCfg.PairCipher == Ndis802_11Encryption3Enabled)
+        		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_AES;
+        	else
+        		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_NONE;
+
+            // Update these related information to MAC_TABLE_ENTRY
+        	pEntry = &pAd->MacTab.Content[BSSID_WCID];
+            NdisMoveMemory(pEntry->PairwiseKey.Key, pAd->SharedKey[BSS0][0].Key, LEN_TKIP_EK);
+        	NdisMoveMemory(pEntry->PairwiseKey.RxMic, pAd->SharedKey[BSS0][0].RxMic, LEN_TKIP_RXMICK);
+        	NdisMoveMemory(pEntry->PairwiseKey.TxMic, pAd->SharedKey[BSS0][0].TxMic, LEN_TKIP_TXMICK);
+        	pEntry->PairwiseKey.CipherAlg = pAd->SharedKey[BSS0][0].CipherAlg;
+
+        	// Update pairwise key information to ASIC Shared Key Table
+        	AsicAddSharedKeyEntry(pAd,
+        						  BSS0,
+        						  0,
+        						  pAd->SharedKey[BSS0][0].CipherAlg,
+        						  pAd->SharedKey[BSS0][0].Key,
+        						  pAd->SharedKey[BSS0][0].TxMic,
+        						  pAd->SharedKey[BSS0][0].RxMic);
+
+        	// Update ASIC WCID attribute table and IVEIV table
+        	RTMPAddWcidAttributeEntry(pAd,
+        							  BSS0,
+        							  0,
+        							  pAd->SharedKey[BSS0][0].CipherAlg,
+        							  pEntry);
+
+            if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA2)
+            {
+                // set 802.1x port control
+	            //pAd->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+				STA_PORT_SECURED(pAd);
+
+                // Indicate Connected for GUI
+                pAd->IndicateMediaState = NdisMediaStateConnected;
+            }
+		}
+        else
+        {
+            // Update GTK
+            pAd->StaCfg.DefaultKeyId = (pKey->KeyIndex & 0xFF);
+            NdisZeroMemory(&pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId], sizeof(CIPHER_KEY));
+            pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].KeyLen = LEN_TKIP_EK;
+            NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key, pKey->KeyMaterial, LEN_TKIP_EK);
+#ifdef WPA_SUPPLICANT_SUPPORT
+            if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption2Enabled)
+            {
+                NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic, pKey->KeyMaterial + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+                NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic, pKey->KeyMaterial + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+            }
+            else
+#endif // WPA_SUPPLICANT_SUPPORT //
+            {
+            	NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic, pKey->KeyMaterial + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+                NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic, pKey->KeyMaterial + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+            }
+
+            // Update Shared Key CipherAlg
+    		pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_NONE;
+    		if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption2Enabled)
+    			pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_TKIP;
+    		else if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption3Enabled)
+    			pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_AES;
+
+            // Update group key information to ASIC Shared Key Table
+        	AsicAddSharedKeyEntry(pAd,
+        						  BSS0,
+        						  pAd->StaCfg.DefaultKeyId,
+        						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
+        						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key,
+        						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic,
+        						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic);
+
+        	// Update ASIC WCID attribute table and IVEIV table
+        	RTMPAddWcidAttributeEntry(pAd,
+        							  BSS0,
+        							  pAd->StaCfg.DefaultKeyId,
+        							  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
+        							  NULL);
+
+            // set 802.1x port control
+	        //pAd->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+			STA_PORT_SECURED(pAd);
+
+            // Indicate Connected for GUI
+            pAd->IndicateMediaState = NdisMediaStateConnected;
+        }
+	}
+	else	// dynamic WEP from wpa_supplicant
+	{
+		UCHAR	CipherAlg;
+    	PUCHAR	Key;
+
+		if(pKey->KeyLength == 32)
+			goto end;
+
+		KeyIdx = pKey->KeyIndex & 0x0fffffff;
+
+		if (KeyIdx < 4)
+		{
+			// it is a default shared key, for Pairwise key setting
+			if (pKey->KeyIndex & 0x80000000)
+			{
+				pEntry = MacTableLookup(pAd, pKey->BSSID);
+
+				if (pEntry)
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("RTMPAddKey: Set Pair-wise Key\n"));
+
+					// set key material and key length
+ 					pEntry->PairwiseKey.KeyLen = (UCHAR)pKey->KeyLength;
+					NdisMoveMemory(pEntry->PairwiseKey.Key, &pKey->KeyMaterial, pKey->KeyLength);
+
+					// set Cipher type
+					if (pKey->KeyLength == 5)
+						pEntry->PairwiseKey.CipherAlg = CIPHER_WEP64;
+					else
+						pEntry->PairwiseKey.CipherAlg = CIPHER_WEP128;
+
+					// Add Pair-wise key to Asic
+					AsicAddPairwiseKeyEntry(
+						pAd,
+						pEntry->Addr,
+						(UCHAR)pEntry->Aid,
+                		&pEntry->PairwiseKey);
+
+					// update WCID attribute table and IVEIV table for this entry
+					RTMPAddWcidAttributeEntry(
+						pAd,
+						BSS0,
+						KeyIdx, // The value may be not zero
+						pEntry->PairwiseKey.CipherAlg,
+						pEntry);
+
+				}
+			}
+			else
+            {
+				// Default key for tx (shared key)
+				pAd->StaCfg.DefaultKeyId = (UCHAR) KeyIdx;
+
+				// set key material and key length
+				pAd->SharedKey[BSS0][KeyIdx].KeyLen = (UCHAR) pKey->KeyLength;
+				NdisMoveMemory(pAd->SharedKey[BSS0][KeyIdx].Key, &pKey->KeyMaterial, pKey->KeyLength);
+
+				// Set Ciper type
+				if (pKey->KeyLength == 5)
+					pAd->SharedKey[BSS0][KeyIdx].CipherAlg = CIPHER_WEP64;
+				else
+					pAd->SharedKey[BSS0][KeyIdx].CipherAlg = CIPHER_WEP128;
+
+    			CipherAlg = pAd->SharedKey[BSS0][KeyIdx].CipherAlg;
+    			Key = pAd->SharedKey[BSS0][KeyIdx].Key;
+
+				// Set Group key material to Asic
+    			AsicAddSharedKeyEntry(pAd, BSS0, KeyIdx, CipherAlg, Key, NULL, NULL);
+
+				// Update WCID attribute table and IVEIV table for this group key table
+				RTMPAddWcidAttributeEntry(pAd, BSS0, KeyIdx, CipherAlg, NULL);
+
+			}
+		}
+	}
+end:
+	return;
+}
+
+char * rtstrchr(const char * s, int c)
+{
+    for(; *s != (char) c; ++s)
+        if (*s == '\0')
+            return NULL;
+    return (char *) s;
+}
+
+/*
+This is required for LinEX2004/kernel2.6.7 to provide iwlist scanning function
+*/
+
+int
+rt_ioctl_giwname(struct net_device *dev,
+		   struct iw_request_info *info,
+		   char *name, char *extra)
+{
+//	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+
+#ifdef RT2870
+	strncpy(name, "RT2870 Wireless", IFNAMSIZ);
+#endif // RT2870 //
+	return 0;
+}
+
+int rt_ioctl_siwfreq(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_freq *freq, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+	int 	chan = -1;
+
+    //check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        return -ENETDOWN;
+    }
+
+
+	if (freq->e > 1)
+		return -EINVAL;
+
+	if((freq->e == 0) && (freq->m <= 1000))
+		chan = freq->m;	// Setting by channel number
+	else
+		MAP_KHZ_TO_CHANNEL_ID( (freq->m /100) , chan); // Setting by frequency - search the table , like 2.412G, 2.422G,
+
+    if (ChannelSanity(pAdapter, chan) == TRUE)
+    {
+	pAdapter->CommonCfg.Channel = chan;
+	DBGPRINT(RT_DEBUG_ERROR, ("==>rt_ioctl_siwfreq::SIOCSIWFREQ[cmd=0x%x] (Channel=%d)\n", SIOCSIWFREQ, pAdapter->CommonCfg.Channel));
+    }
+    else
+        return -EINVAL;
+
+	return 0;
+}
+int rt_ioctl_giwfreq(struct net_device *dev,
+		   struct iw_request_info *info,
+		   struct iw_freq *freq, char *extra)
+{
+    VIRTUAL_ADAPTER *pVirtualAd = NULL;
+	PRTMP_ADAPTER pAdapter = NULL;
+	UCHAR ch;
+	ULONG	m;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+		ch = pAdapter->CommonCfg.Channel;
+
+	DBGPRINT(RT_DEBUG_TRACE,("==>rt_ioctl_giwfreq  %d\n", ch));
+
+    MAP_CHANNEL_ID_TO_KHZ(ch, m);
+	freq->m = m * 100;
+	freq->e = 1;
+	return 0;
+}
+
+int rt_ioctl_siwmode(struct net_device *dev,
+		   struct iw_request_info *info,
+		   __u32 *mode, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+
+	//check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+    	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+       	return -ENETDOWN;
+    }
+
+	switch (*mode)
+	{
+		case IW_MODE_ADHOC:
+			Set_NetworkType_Proc(pAdapter, "Adhoc");
+			break;
+		case IW_MODE_INFRA:
+			Set_NetworkType_Proc(pAdapter, "Infra");
+			break;
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,4,20))
+        case IW_MODE_MONITOR:
+			Set_NetworkType_Proc(pAdapter, "Monitor");
+			break;
+#endif
+		default:
+			DBGPRINT(RT_DEBUG_TRACE, ("===>rt_ioctl_siwmode::SIOCSIWMODE (unknown %d)\n", *mode));
+			return -EINVAL;
+	}
+
+	// Reset Ralink supplicant to not use, it will be set to start when UI set PMK key
+	pAdapter->StaCfg.WpaState = SS_NOTUSE;
+
+	return 0;
+}
+
+int rt_ioctl_giwmode(struct net_device *dev,
+		   struct iw_request_info *info,
+		   __u32 *mode, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	if (ADHOC_ON(pAdapter))
+		*mode = IW_MODE_ADHOC;
+    else if (INFRA_ON(pAdapter))
+		*mode = IW_MODE_INFRA;
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,4,20))
+    else if (MONITOR_ON(pAdapter))
+    {
+        *mode = IW_MODE_MONITOR;
+    }
+#endif
+    else
+        *mode = IW_MODE_AUTO;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("==>rt_ioctl_giwmode(mode=%d)\n", *mode));
+	return 0;
+}
+
+int rt_ioctl_siwsens(struct net_device *dev,
+		   struct iw_request_info *info,
+		   char *name, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+
+	//check if the interface is down
+    	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    	{
+        	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        	return -ENETDOWN;
+    	}
+
+	return 0;
+}
+
+int rt_ioctl_giwsens(struct net_device *dev,
+		   struct iw_request_info *info,
+		   char *name, char *extra)
+{
+	return 0;
+}
+
+int rt_ioctl_giwrange(struct net_device *dev,
+		   struct iw_request_info *info,
+		   struct iw_point *data, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+	struct iw_range *range = (struct iw_range *) extra;
+	u16 val;
+	int i;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE ,("===>rt_ioctl_giwrange\n"));
+	data->length = sizeof(struct iw_range);
+	memset(range, 0, sizeof(struct iw_range));
+
+	range->txpower_capa = IW_TXPOW_DBM;
+
+	if (INFRA_ON(pAdapter)||ADHOC_ON(pAdapter))
+	{
+		range->min_pmp = 1 * 1024;
+		range->max_pmp = 65535 * 1024;
+		range->min_pmt = 1 * 1024;
+		range->max_pmt = 1000 * 1024;
+		range->pmp_flags = IW_POWER_PERIOD;
+		range->pmt_flags = IW_POWER_TIMEOUT;
+		range->pm_capa = IW_POWER_PERIOD | IW_POWER_TIMEOUT |
+			IW_POWER_UNICAST_R | IW_POWER_ALL_R;
+	}
+
+	range->we_version_compiled = WIRELESS_EXT;
+	range->we_version_source = 14;
+
+	range->retry_capa = IW_RETRY_LIMIT;
+	range->retry_flags = IW_RETRY_LIMIT;
+	range->min_retry = 0;
+	range->max_retry = 255;
+
+	range->num_channels =  pAdapter->ChannelListNum;
+
+	val = 0;
+	for (i = 1; i <= range->num_channels; i++)
+	{
+		u32 m;
+		range->freq[val].i = pAdapter->ChannelList[i-1].Channel;
+		MAP_CHANNEL_ID_TO_KHZ(pAdapter->ChannelList[i-1].Channel, m);
+		range->freq[val].m = m * 100; /* HZ */
+
+		range->freq[val].e = 1;
+		val++;
+		if (val == IW_MAX_FREQUENCIES)
+			break;
+	}
+	range->num_frequency = val;
+
+	range->max_qual.qual = 100; /* what is correct max? This was not
+					* documented exactly. At least
+					* 69 has been observed. */
+	range->max_qual.level = 0; /* dB */
+	range->max_qual.noise = 0; /* dB */
+
+	/* What would be suitable values for "average/typical" qual? */
+	range->avg_qual.qual = 20;
+	range->avg_qual.level = -60;
+	range->avg_qual.noise = -95;
+	range->sensitivity = 3;
+
+	range->max_encoding_tokens = NR_WEP_KEYS;
+	range->num_encoding_sizes = 2;
+	range->encoding_size[0] = 5;
+	range->encoding_size[1] = 13;
+
+	range->min_rts = 0;
+	range->max_rts = 2347;
+	range->min_frag = 256;
+	range->max_frag = 2346;
+
+#if WIRELESS_EXT > 17
+	/* IW_ENC_CAPA_* bit field */
+	range->enc_capa = IW_ENC_CAPA_WPA | IW_ENC_CAPA_WPA2 |
+					IW_ENC_CAPA_CIPHER_TKIP | IW_ENC_CAPA_CIPHER_CCMP;
+#endif
+
+	return 0;
+}
+
+int rt_ioctl_siwap(struct net_device *dev,
+		      struct iw_request_info *info,
+		      struct sockaddr *ap_addr, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+    NDIS_802_11_MAC_ADDRESS Bssid;
+
+	//check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+       	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+       	return -ENETDOWN;
+    }
+
+	if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+    {
+        RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+        DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+    }
+
+    // tell CNTL state machine to call NdisMSetInformationComplete() after completing
+    // this request, because this request is initiated by NDIS.
+    pAdapter->MlmeAux.CurrReqIsFromNdis = FALSE;
+	// Prevent to connect AP again in STAMlmePeriodicExec
+	pAdapter->MlmeAux.AutoReconnectSsidLen= 32;
+
+    memset(Bssid, 0, MAC_ADDR_LEN);
+    memcpy(Bssid, ap_addr->sa_data, MAC_ADDR_LEN);
+    MlmeEnqueue(pAdapter,
+                MLME_CNTL_STATE_MACHINE,
+                OID_802_11_BSSID,
+                sizeof(NDIS_802_11_MAC_ADDRESS),
+                (VOID *)&Bssid);
+
+    DBGPRINT(RT_DEBUG_TRACE, ("IOCTL::SIOCSIWAP %02x:%02x:%02x:%02x:%02x:%02x\n",
+        Bssid[0], Bssid[1], Bssid[2], Bssid[3], Bssid[4], Bssid[5]));
+
+	return 0;
+}
+
+int rt_ioctl_giwap(struct net_device *dev,
+		      struct iw_request_info *info,
+		      struct sockaddr *ap_addr, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	if (INFRA_ON(pAdapter) || ADHOC_ON(pAdapter))
+	{
+		ap_addr->sa_family = ARPHRD_ETHER;
+		memcpy(ap_addr->sa_data, &pAdapter->CommonCfg.Bssid, ETH_ALEN);
+	}
+#ifdef WPA_SUPPLICANT_SUPPORT
+    // Add for RT2870
+    else if (pAdapter->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_DISABLE)
+    {
+        ap_addr->sa_family = ARPHRD_ETHER;
+        memcpy(ap_addr->sa_data, &pAdapter->MlmeAux.Bssid, ETH_ALEN);
+    }
+#endif // WPA_SUPPLICANT_SUPPORT //
+	else
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("IOCTL::SIOCGIWAP(=EMPTY)\n"));
+		return -ENOTCONN;
+	}
+
+	return 0;
+}
+
+/*
+ * Units are in db above the noise floor. That means the
+ * rssi values reported in the tx/rx descriptors in the
+ * driver are the SNR expressed in db.
+ *
+ * If you assume that the noise floor is -95, which is an
+ * excellent assumption 99.5 % of the time, then you can
+ * derive the absolute signal level (i.e. -95 + rssi).
+ * There are some other slight factors to take into account
+ * depending on whether the rssi measurement is from 11b,
+ * 11g, or 11a.   These differences are at most 2db and
+ * can be documented.
+ *
+ * NB: various calculations are based on the orinoco/wavelan
+ *     drivers for compatibility
+ */
+static void set_quality(PRTMP_ADAPTER pAdapter,
+                        struct iw_quality *iq,
+                        signed char rssi)
+{
+	__u8 ChannelQuality;
+
+	// Normalize Rssi
+	if (rssi >= -50)
+		ChannelQuality = 100;
+	else if (rssi >= -80) // between -50 ~ -80dbm
+		ChannelQuality = (__u8)(24 + ((rssi + 80) * 26)/10);
+	else if (rssi >= -90)   // between -80 ~ -90dbm
+        ChannelQuality = (__u8)((rssi + 90) * 26)/10;
+	else
+		ChannelQuality = 0;
+
+    iq->qual = (__u8)ChannelQuality;
+
+    iq->level = (__u8)(rssi);
+    iq->noise = (pAdapter->BbpWriteLatch[66] > pAdapter->BbpTuning.FalseCcaUpperThreshold) ? ((__u8)pAdapter->BbpTuning.FalseCcaUpperThreshold) : ((__u8) pAdapter->BbpWriteLatch[66]); 	// noise level (dBm)
+    iq->noise += 256 - 143;
+    iq->updated = pAdapter->iw_stats.qual.updated;
+}
+
+int rt_ioctl_iwaplist(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_point *data, char *extra)
+{
+ 	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+
+	struct sockaddr addr[IW_MAX_AP];
+	struct iw_quality qual[IW_MAX_AP];
+	int i;
+
+   	//check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+       	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+		data->length = 0;
+		return 0;
+        //return -ENETDOWN;
+	}
+
+	for (i = 0; i <IW_MAX_AP ; i++)
+	{
+		if (i >=  pAdapter->ScanTab.BssNr)
+			break;
+		addr[i].sa_family = ARPHRD_ETHER;
+			memcpy(addr[i].sa_data, &pAdapter->ScanTab.BssEntry[i].Bssid, MAC_ADDR_LEN);
+		set_quality(pAdapter, &qual[i], pAdapter->ScanTab.BssEntry[i].Rssi);
+	}
+	data->length = i;
+	memcpy(extra, &addr, i*sizeof(addr[0]));
+	data->flags = 1;		/* signal quality present (sort of) */
+	memcpy(extra + i*sizeof(addr[0]), &qual, i*sizeof(qual[i]));
+
+	return 0;
+}
+
+#ifdef SIOCGIWSCAN
+int rt_ioctl_siwscan(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_point *data, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+
+	ULONG								Now;
+	int Status = NDIS_STATUS_SUCCESS;
+
+	//check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+		return -ENETDOWN;
+	}
+
+	if (MONITOR_ON(pAdapter))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("!!! Driver is in Monitor Mode now !!!\n"));
+        return -EINVAL;
+    }
+
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+	if (pAdapter->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_ENABLE)
+	{
+		pAdapter->StaCfg.WpaSupplicantScanCount++;
+	}
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+    pAdapter->StaCfg.bScanReqIsFromWebUI = TRUE;
+	if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+		return 0;
+	do{
+		Now = jiffies;
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+		if ((pAdapter->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_ENABLE) &&
+			(pAdapter->StaCfg.WpaSupplicantScanCount > 3))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("!!! WpaSupplicantScanCount > 3\n"));
+			Status = NDIS_STATUS_SUCCESS;
+			break;
+		}
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+		if ((OPSTATUS_TEST_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED)) &&
+			((pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
+			(pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK)) &&
+			(pAdapter->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("!!! Link UP, Port Not Secured! ignore this set::OID_802_11_BSSID_LIST_SCAN\n"));
+			Status = NDIS_STATUS_SUCCESS;
+			break;
+		}
+
+		if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+		{
+			RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+			DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+		}
+
+		// tell CNTL state machine to call NdisMSetInformationComplete() after completing
+		// this request, because this request is initiated by NDIS.
+		pAdapter->MlmeAux.CurrReqIsFromNdis = FALSE;
+		// Reset allowed scan retries
+		pAdapter->StaCfg.ScanCnt = 0;
+		pAdapter->StaCfg.LastScanTime = Now;
+
+		MlmeEnqueue(pAdapter,
+			MLME_CNTL_STATE_MACHINE,
+			OID_802_11_BSSID_LIST_SCAN,
+			0,
+			NULL);
+
+		Status = NDIS_STATUS_SUCCESS;
+		RT28XX_MLME_HANDLER(pAdapter);
+	}while(0);
+	return 0;
+}
+
+int rt_ioctl_giwscan(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_point *data, char *extra)
+{
+
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+	int i=0;
+	char *current_ev = extra, *previous_ev = extra;
+	char *end_buf;
+	char *current_val, custom[MAX_CUSTOM_LEN] = {0};
+#ifndef IWEVGENIE
+	char idx;
+#endif // IWEVGENIE //
+	struct iw_event iwe;
+
+	if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+    {
+		/*
+		 * Still scanning, indicate the caller should try again.
+		 */
+		return -EAGAIN;
+	}
+
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+	if (pAdapter->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_ENABLE)
+	{
+		pAdapter->StaCfg.WpaSupplicantScanCount = 0;
+	}
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+	if (pAdapter->ScanTab.BssNr == 0)
+	{
+		data->length = 0;
+		return 0;
+	}
+
+#if WIRELESS_EXT >= 17
+    if (data->length > 0)
+        end_buf = extra + data->length;
+    else
+        end_buf = extra + IW_SCAN_MAX_DATA;
+#else
+    end_buf = extra + IW_SCAN_MAX_DATA;
+#endif
+
+	for (i = 0; i < pAdapter->ScanTab.BssNr; i++)
+	{
+		if (current_ev >= end_buf)
+        {
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+        }
+
+		//MAC address
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWAP;
+		iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
+				memcpy(iwe.u.ap_addr.sa_data, &pAdapter->ScanTab.BssEntry[i].Bssid, ETH_ALEN);
+
+        previous_ev = current_ev;
+		current_ev = IWE_STREAM_ADD_EVENT(info, current_ev,end_buf, &iwe, IW_EV_ADDR_LEN);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//ESSID
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWESSID;
+		iwe.u.data.length = pAdapter->ScanTab.BssEntry[i].SsidLen;
+		iwe.u.data.flags = 1;
+
+        previous_ev = current_ev;
+		current_ev = IWE_STREAM_ADD_POINT(info, current_ev,end_buf, &iwe, pAdapter->ScanTab.BssEntry[i].Ssid);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//Network Type
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWMODE;
+		if (pAdapter->ScanTab.BssEntry[i].BssType == Ndis802_11IBSS)
+		{
+			iwe.u.mode = IW_MODE_ADHOC;
+		}
+		else if (pAdapter->ScanTab.BssEntry[i].BssType == Ndis802_11Infrastructure)
+		{
+			iwe.u.mode = IW_MODE_INFRA;
+		}
+		else
+		{
+			iwe.u.mode = IW_MODE_AUTO;
+		}
+		iwe.len = IW_EV_UINT_LEN;
+
+        previous_ev = current_ev;
+		current_ev = IWE_STREAM_ADD_EVENT(info, current_ev, end_buf, &iwe,  IW_EV_UINT_LEN);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//Channel and Frequency
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWFREQ;
+		if (INFRA_ON(pAdapter) || ADHOC_ON(pAdapter))
+			iwe.u.freq.m = pAdapter->ScanTab.BssEntry[i].Channel;
+		else
+			iwe.u.freq.m = pAdapter->ScanTab.BssEntry[i].Channel;
+		iwe.u.freq.e = 0;
+		iwe.u.freq.i = 0;
+
+		previous_ev = current_ev;
+		current_ev = IWE_STREAM_ADD_EVENT(info, current_ev,end_buf, &iwe, IW_EV_FREQ_LEN);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+        //Add quality statistics
+        //================================
+        memset(&iwe, 0, sizeof(iwe));
+    	iwe.cmd = IWEVQUAL;
+    	iwe.u.qual.level = 0;
+    	iwe.u.qual.noise = 0;
+        set_quality(pAdapter, &iwe.u.qual, pAdapter->ScanTab.BssEntry[i].Rssi);
+    	current_ev = IWE_STREAM_ADD_EVENT(info, current_ev, end_buf, &iwe, IW_EV_QUAL_LEN);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//Encyption key
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWENCODE;
+		if (CAP_IS_PRIVACY_ON (pAdapter->ScanTab.BssEntry[i].CapabilityInfo ))
+			iwe.u.data.flags =IW_ENCODE_ENABLED | IW_ENCODE_NOKEY;
+		else
+			iwe.u.data.flags = IW_ENCODE_DISABLED;
+
+        previous_ev = current_ev;
+        current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf,&iwe, (char *)pAdapter->SharedKey[BSS0][(iwe.u.data.flags & IW_ENCODE_INDEX)-1].Key);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//Bit Rate
+		//================================
+		if (pAdapter->ScanTab.BssEntry[i].SupRateLen)
+        {
+            UCHAR tmpRate = pAdapter->ScanTab.BssEntry[i].SupRate[pAdapter->ScanTab.BssEntry[i].SupRateLen-1];
+			memset(&iwe, 0, sizeof(iwe));
+			iwe.cmd = SIOCGIWRATE;
+    		current_val = current_ev + IW_EV_LCP_LEN;
+            if (tmpRate == 0x82)
+                iwe.u.bitrate.value =  1 * 1000000;
+            else if (tmpRate == 0x84)
+                iwe.u.bitrate.value =  2 * 1000000;
+            else if (tmpRate == 0x8B)
+                iwe.u.bitrate.value =  5.5 * 1000000;
+            else if (tmpRate == 0x96)
+                iwe.u.bitrate.value =  11 * 1000000;
+            else
+    		    iwe.u.bitrate.value =  (tmpRate/2) * 1000000;
+
+			iwe.u.bitrate.disabled = 0;
+			current_val = IWE_STREAM_ADD_VALUE(info, current_ev,
+				current_val, end_buf, &iwe,
+    			IW_EV_PARAM_LEN);
+
+        	if((current_val-current_ev)>IW_EV_LCP_LEN)
+            	current_ev = current_val;
+        	else
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+        }
+
+#ifdef IWEVGENIE
+		//WPA IE
+		if (pAdapter->ScanTab.BssEntry[i].WpaIE.IELen > 0)
+		{
+			memset(&iwe, 0, sizeof(iwe));
+			memset(&custom[0], 0, MAX_CUSTOM_LEN);
+			memcpy(custom, &(pAdapter->ScanTab.BssEntry[i].WpaIE.IE[0]),
+						   pAdapter->ScanTab.BssEntry[i].WpaIE.IELen);
+			iwe.cmd = IWEVGENIE;
+			iwe.u.data.length = pAdapter->ScanTab.BssEntry[i].WpaIE.IELen;
+			current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf, &iwe, custom);
+			if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+		}
+
+		//WPA2 IE
+        if (pAdapter->ScanTab.BssEntry[i].RsnIE.IELen > 0)
+        {
+        	memset(&iwe, 0, sizeof(iwe));
+			memset(&custom[0], 0, MAX_CUSTOM_LEN);
+			memcpy(custom, &(pAdapter->ScanTab.BssEntry[i].RsnIE.IE[0]),
+						   pAdapter->ScanTab.BssEntry[i].RsnIE.IELen);
+			iwe.cmd = IWEVGENIE;
+			iwe.u.data.length = pAdapter->ScanTab.BssEntry[i].RsnIE.IELen;
+			current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf, &iwe, custom);
+			if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+        }
+#else
+        //WPA IE
+		//================================
+        if (pAdapter->ScanTab.BssEntry[i].WpaIE.IELen > 0)
+        {
+    		NdisZeroMemory(&iwe, sizeof(iwe));
+			memset(&custom[0], 0, MAX_CUSTOM_LEN);
+    		iwe.cmd = IWEVCUSTOM;
+            iwe.u.data.length = (pAdapter->ScanTab.BssEntry[i].WpaIE.IELen * 2) + 7;
+            NdisMoveMemory(custom, "wpa_ie=", 7);
+            for (idx = 0; idx < pAdapter->ScanTab.BssEntry[i].WpaIE.IELen; idx++)
+                sprintf(custom, "%s%02x", custom, pAdapter->ScanTab.BssEntry[i].WpaIE.IE[idx]);
+            previous_ev = current_ev;
+    		current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf, &iwe,  custom);
+            if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+        }
+
+        //WPA2 IE
+        if (pAdapter->ScanTab.BssEntry[i].RsnIE.IELen > 0)
+        {
+    		NdisZeroMemory(&iwe, sizeof(iwe));
+			memset(&custom[0], 0, MAX_CUSTOM_LEN);
+    		iwe.cmd = IWEVCUSTOM;
+            iwe.u.data.length = (pAdapter->ScanTab.BssEntry[i].RsnIE.IELen * 2) + 7;
+            NdisMoveMemory(custom, "rsn_ie=", 7);
+			for (idx = 0; idx < pAdapter->ScanTab.BssEntry[i].RsnIE.IELen; idx++)
+                sprintf(custom, "%s%02x", custom, pAdapter->ScanTab.BssEntry[i].RsnIE.IE[idx]);
+            previous_ev = current_ev;
+    		current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf, &iwe,  custom);
+            if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+        }
+#endif // IWEVGENIE //
+	}
+
+	data->length = current_ev - extra;
+    pAdapter->StaCfg.bScanReqIsFromWebUI = FALSE;
+	DBGPRINT(RT_DEBUG_ERROR ,("===>rt_ioctl_giwscan. %d(%d) BSS returned, data->length = %d\n",i , pAdapter->ScanTab.BssNr, data->length));
+	return 0;
+}
+#endif
+
+int rt_ioctl_siwessid(struct net_device *dev,
+			 struct iw_request_info *info,
+			 struct iw_point *data, char *essid)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+
+	//check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+       	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+       	return -ENETDOWN;
+    }
+
+	if (data->flags)
+	{
+		PCHAR	pSsidString = NULL;
+
+		// Includes null character.
+		if (data->length > (IW_ESSID_MAX_SIZE + 1))
+			return -E2BIG;
+
+		pSsidString = (CHAR *) kmalloc(MAX_LEN_OF_SSID+1, MEM_ALLOC_FLAG);
+		if (pSsidString)
+		{
+			NdisZeroMemory(pSsidString, MAX_LEN_OF_SSID+1);
+			NdisMoveMemory(pSsidString, essid, data->length);
+			if (Set_SSID_Proc(pAdapter, pSsidString) == FALSE)
+				return -EINVAL;
+		}
+		else
+			return -ENOMEM;
+	}
+	else
+	{
+		// ANY ssid
+		if (Set_SSID_Proc(pAdapter, "") == FALSE)
+			return -EINVAL;
+    }
+	return 0;
+}
+
+int rt_ioctl_giwessid(struct net_device *dev,
+			 struct iw_request_info *info,
+			 struct iw_point *data, char *essid)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	data->flags = 1;
+    if (MONITOR_ON(pAdapter))
+    {
+        data->length  = 0;
+        return 0;
+    }
+
+	if (OPSTATUS_TEST_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED))
+	{
+		DBGPRINT(RT_DEBUG_TRACE ,("MediaState is connected\n"));
+		data->length = pAdapter->CommonCfg.SsidLen;
+		memcpy(essid, pAdapter->CommonCfg.Ssid, pAdapter->CommonCfg.SsidLen);
+	}
+#ifdef RT2870
+#ifdef WPA_SUPPLICANT_SUPPORT
+    // Add for RT2870
+    else if (pAdapter->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_DISABLE)
+    {
+        data->length = pAdapter->CommonCfg.SsidLen;
+		memcpy(essid, pAdapter->CommonCfg.Ssid, pAdapter->CommonCfg.SsidLen);
+	}
+#endif // WPA_SUPPLICANT_SUPPORT //
+#endif // RT2870 //
+	else
+	{//the ANY ssid was specified
+		data->length  = 0;
+		DBGPRINT(RT_DEBUG_TRACE ,("MediaState is not connected, ess\n"));
+	}
+
+	return 0;
+
+}
+
+int rt_ioctl_siwnickn(struct net_device *dev,
+			 struct iw_request_info *info,
+			 struct iw_point *data, char *nickname)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+
+    //check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+        DBGPRINT(RT_DEBUG_TRACE ,("INFO::Network is down!\n"));
+        return -ENETDOWN;
+    }
+
+	if (data->length > IW_ESSID_MAX_SIZE)
+		return -EINVAL;
+
+	memset(pAdapter->nickname, 0, IW_ESSID_MAX_SIZE + 1);
+	memcpy(pAdapter->nickname, nickname, data->length);
+
+
+	return 0;
+}
+
+int rt_ioctl_giwnickn(struct net_device *dev,
+			 struct iw_request_info *info,
+			 struct iw_point *data, char *nickname)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	if (data->length > strlen(pAdapter->nickname) + 1)
+		data->length = strlen(pAdapter->nickname) + 1;
+	if (data->length > 0) {
+		memcpy(nickname, pAdapter->nickname, data->length-1);
+		nickname[data->length-1] = '\0';
+	}
+	return 0;
+}
+
+int rt_ioctl_siwrts(struct net_device *dev,
+		       struct iw_request_info *info,
+		       struct iw_param *rts, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+	u16 val;
+
+    //check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        return -ENETDOWN;
+    }
+
+	if (rts->disabled)
+		val = MAX_RTS_THRESHOLD;
+	else if (rts->value < 0 || rts->value > MAX_RTS_THRESHOLD)
+		return -EINVAL;
+	else if (rts->value == 0)
+	    val = MAX_RTS_THRESHOLD;
+	else
+		val = rts->value;
+
+	if (val != pAdapter->CommonCfg.RtsThreshold)
+		pAdapter->CommonCfg.RtsThreshold = val;
+
+	return 0;
+}
+
+int rt_ioctl_giwrts(struct net_device *dev,
+		       struct iw_request_info *info,
+		       struct iw_param *rts, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	//check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+	rts->value = pAdapter->CommonCfg.RtsThreshold;
+	rts->disabled = (rts->value == MAX_RTS_THRESHOLD);
+	rts->fixed = 1;
+
+	return 0;
+}
+
+int rt_ioctl_siwfrag(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_param *frag, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+	u16 val;
+
+	//check if the interface is down
+    	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    	{
+      		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        	return -ENETDOWN;
+    	}
+
+	if (frag->disabled)
+		val = MAX_FRAG_THRESHOLD;
+	else if (frag->value >= MIN_FRAG_THRESHOLD || frag->value <= MAX_FRAG_THRESHOLD)
+        val = __cpu_to_le16(frag->value & ~0x1); /* even numbers only */
+	else if (frag->value == 0)
+	    val = MAX_FRAG_THRESHOLD;
+	else
+		return -EINVAL;
+
+	pAdapter->CommonCfg.FragmentThreshold = val;
+	return 0;
+}
+
+int rt_ioctl_giwfrag(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_param *frag, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	//check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+	frag->value = pAdapter->CommonCfg.FragmentThreshold;
+	frag->disabled = (frag->value == MAX_FRAG_THRESHOLD);
+	frag->fixed = 1;
+
+	return 0;
+}
+
+#define MAX_WEP_KEY_SIZE 13
+#define MIN_WEP_KEY_SIZE 5
+int rt_ioctl_siwencode(struct net_device *dev,
+			  struct iw_request_info *info,
+			  struct iw_point *erq, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+
+	//check if the interface is down
+    	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    	{
+      		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        	return -ENETDOWN;
+    	}
+
+	if ((erq->length == 0) &&
+        (erq->flags & IW_ENCODE_DISABLED))
+	{
+		pAdapter->StaCfg.PairCipher = Ndis802_11WEPDisabled;
+		pAdapter->StaCfg.GroupCipher = Ndis802_11WEPDisabled;
+		pAdapter->StaCfg.WepStatus = Ndis802_11WEPDisabled;
+        pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+        goto done;
+	}
+	else if ((erq->length == 0) &&
+             (erq->flags & IW_ENCODE_RESTRICTED || erq->flags & IW_ENCODE_OPEN))
+	{
+	    //pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+		STA_PORT_SECURED(pAdapter);
+		pAdapter->StaCfg.PairCipher = Ndis802_11WEPEnabled;
+		pAdapter->StaCfg.GroupCipher = Ndis802_11WEPEnabled;
+		pAdapter->StaCfg.WepStatus = Ndis802_11WEPEnabled;
+        pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+		if (erq->flags & IW_ENCODE_RESTRICTED)
+			pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeShared;
+    	else
+			pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+        goto done;
+	}
+
+    if (erq->length > 0)
+	{
+		int keyIdx = (erq->flags & IW_ENCODE_INDEX) - 1;
+		/* Check the size of the key */
+		if (erq->length > MAX_WEP_KEY_SIZE) {
+			return -EINVAL;
+		}
+		/* Check key index */
+		if ((keyIdx < 0) || (keyIdx >= NR_WEP_KEYS))
+        {
+            DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::Wrong keyIdx=%d! Using default key instead (%d)\n",
+                                        keyIdx, pAdapter->StaCfg.DefaultKeyId));
+
+            //Using default key
+			keyIdx = pAdapter->StaCfg.DefaultKeyId;
+        }
+
+        NdisZeroMemory(pAdapter->SharedKey[BSS0][keyIdx].Key,  16);
+
+		if (erq->length == MAX_WEP_KEY_SIZE)
+        {
+			pAdapter->SharedKey[BSS0][keyIdx].KeyLen = MAX_WEP_KEY_SIZE;
+            pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_WEP128;
+		}
+		else if (erq->length == MIN_WEP_KEY_SIZE)
+        {
+            pAdapter->SharedKey[BSS0][keyIdx].KeyLen = MIN_WEP_KEY_SIZE;
+            pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_WEP64;
+		}
+		else
+			/* Disable the key */
+			pAdapter->SharedKey[BSS0][keyIdx].KeyLen = 0;
+
+		/* Check if the key is not marked as invalid */
+		if(!(erq->flags & IW_ENCODE_NOKEY)) {
+			/* Copy the key in the driver */
+			NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].Key, extra, erq->length);
+        }
+	}
+    else
+			{
+		/* Do we want to just set the transmit key index ? */
+		int index = (erq->flags & IW_ENCODE_INDEX) - 1;
+		if ((index >= 0) && (index < 4))
+        {
+			pAdapter->StaCfg.DefaultKeyId = index;
+            }
+        else
+			/* Don't complain if only change the mode */
+			if(!erq->flags & IW_ENCODE_MODE) {
+				return -EINVAL;
+		}
+	}
+
+done:
+    DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::erq->flags=%x\n",erq->flags));
+	DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::AuthMode=%x\n",pAdapter->StaCfg.AuthMode));
+	DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::DefaultKeyId=%x, KeyLen = %d\n",pAdapter->StaCfg.DefaultKeyId , pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen));
+	DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::WepStatus=%x\n",pAdapter->StaCfg.WepStatus));
+	return 0;
+}
+
+int
+rt_ioctl_giwencode(struct net_device *dev,
+			  struct iw_request_info *info,
+			  struct iw_point *erq, char *key)
+{
+	int kid;
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	//check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+	kid = erq->flags & IW_ENCODE_INDEX;
+	DBGPRINT(RT_DEBUG_TRACE, ("===>rt_ioctl_giwencode %d\n", erq->flags & IW_ENCODE_INDEX));
+
+	if (pAdapter->StaCfg.WepStatus == Ndis802_11WEPDisabled)
+	{
+		erq->length = 0;
+		erq->flags = IW_ENCODE_DISABLED;
+	}
+	else if ((kid > 0) && (kid <=4))
+	{
+		// copy wep key
+		erq->flags = kid ;			/* NB: base 1 */
+		if (erq->length > pAdapter->SharedKey[BSS0][kid-1].KeyLen)
+			erq->length = pAdapter->SharedKey[BSS0][kid-1].KeyLen;
+		memcpy(key, pAdapter->SharedKey[BSS0][kid-1].Key, erq->length);
+		//if ((kid == pAdapter->PortCfg.DefaultKeyId))
+		//erq->flags |= IW_ENCODE_ENABLED;	/* XXX */
+		if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeShared)
+			erq->flags |= IW_ENCODE_RESTRICTED;		/* XXX */
+		else
+			erq->flags |= IW_ENCODE_OPEN;		/* XXX */
+
+	}
+	else if (kid == 0)
+	{
+		if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeShared)
+			erq->flags |= IW_ENCODE_RESTRICTED;		/* XXX */
+		else
+			erq->flags |= IW_ENCODE_OPEN;		/* XXX */
+		erq->length = pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen;
+		memcpy(key, pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].Key, erq->length);
+		// copy default key ID
+		if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeShared)
+			erq->flags |= IW_ENCODE_RESTRICTED;		/* XXX */
+		else
+			erq->flags |= IW_ENCODE_OPEN;		/* XXX */
+		erq->flags = pAdapter->StaCfg.DefaultKeyId + 1;			/* NB: base 1 */
+		erq->flags |= IW_ENCODE_ENABLED;	/* XXX */
+	}
+
+	return 0;
+
+}
+
+static int
+rt_ioctl_setparam(struct net_device *dev, struct iw_request_info *info,
+			 void *w, char *extra)
+{
+    VIRTUAL_ADAPTER	*pVirtualAd = NULL;
+	PRTMP_ADAPTER pAdapter;
+	POS_COOKIE pObj;
+	char *this_char = extra;
+	char *value;
+	int  Status=0;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+	pObj = (POS_COOKIE) pAdapter->OS_Cookie;
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	{
+		pObj->ioctl_if_type = INT_MAIN;
+        pObj->ioctl_if = MAIN_MBSSID;
+	}
+
+	//check if the interface is down
+    	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    	{
+      		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+			return -ENETDOWN;
+    	}
+
+	if (!*this_char)
+		return -EINVAL;
+
+	if ((value = rtstrchr(this_char, '=')) != NULL)
+	    *value++ = 0;
+
+	if (!value)
+	    return -EINVAL;
+
+	// reject setting nothing besides ANY ssid(ssidLen=0)
+    if (!*value && (strcmp(this_char, "SSID") != 0))
+        return -EINVAL;
+
+	for (PRTMP_PRIVATE_SET_PROC = RTMP_PRIVATE_SUPPORT_PROC; PRTMP_PRIVATE_SET_PROC->name; PRTMP_PRIVATE_SET_PROC++)
+	{
+	    if (strcmp(this_char, PRTMP_PRIVATE_SET_PROC->name) == 0)
+	    {
+	        if(!PRTMP_PRIVATE_SET_PROC->set_proc(pAdapter, value))
+	        {	//FALSE:Set private failed then return Invalid argument
+			    Status = -EINVAL;
+	        }
+		    break;	//Exit for loop.
+	    }
+	}
+
+	if(PRTMP_PRIVATE_SET_PROC->name == NULL)
+	{  //Not found argument
+	    Status = -EINVAL;
+	    DBGPRINT(RT_DEBUG_TRACE, ("===>rt_ioctl_setparam:: (iwpriv) Not Support Set Command [%s=%s]\n", this_char, value));
+	}
+
+    return Status;
+}
+
+
+static int
+rt_private_get_statistics(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra)
+{
+	INT				Status = 0;
+    PRTMP_ADAPTER   pAd = (PRTMP_ADAPTER) dev->priv;
+
+    if (extra == NULL)
+    {
+        wrq->length = 0;
+        return -EIO;
+    }
+
+    memset(extra, 0x00, IW_PRIV_SIZE_MASK);
+    sprintf(extra, "\n\n");
+
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+	{
+	    sprintf(extra+strlen(extra), "Tx success                      = %ld\n", (ULONG)pAd->ate.TxDoneCount);
+	    //sprintf(extra+strlen(extra), "Tx success without retry        = %ld\n", (ULONG)pAd->ate.TxDoneCount);
+	}
+	else
+#endif // RALINK_ATE //
+	{
+    sprintf(extra+strlen(extra), "Tx success                      = %ld\n", (ULONG)pAd->WlanCounters.TransmittedFragmentCount.QuadPart);
+    sprintf(extra+strlen(extra), "Tx success without retry        = %ld\n", (ULONG)pAd->WlanCounters.TransmittedFragmentCount.QuadPart - (ULONG)pAd->WlanCounters.RetryCount.QuadPart);
+	}
+    sprintf(extra+strlen(extra), "Tx success after retry          = %ld\n", (ULONG)pAd->WlanCounters.RetryCount.QuadPart);
+    sprintf(extra+strlen(extra), "Tx fail to Rcv ACK after retry  = %ld\n", (ULONG)pAd->WlanCounters.FailedCount.QuadPart);
+    sprintf(extra+strlen(extra), "RTS Success Rcv CTS             = %ld\n", (ULONG)pAd->WlanCounters.RTSSuccessCount.QuadPart);
+    sprintf(extra+strlen(extra), "RTS Fail Rcv CTS                = %ld\n", (ULONG)pAd->WlanCounters.RTSFailureCount.QuadPart);
+
+    sprintf(extra+strlen(extra), "Rx success                      = %ld\n", (ULONG)pAd->WlanCounters.ReceivedFragmentCount.QuadPart);
+    sprintf(extra+strlen(extra), "Rx with CRC                     = %ld\n", (ULONG)pAd->WlanCounters.FCSErrorCount.QuadPart);
+    sprintf(extra+strlen(extra), "Rx drop due to out of resource  = %ld\n", (ULONG)pAd->Counters8023.RxNoBuffer);
+    sprintf(extra+strlen(extra), "Rx duplicate frame              = %ld\n", (ULONG)pAd->WlanCounters.FrameDuplicateCount.QuadPart);
+
+    sprintf(extra+strlen(extra), "False CCA (one second)          = %ld\n", (ULONG)pAd->RalinkCounters.OneSecFalseCCACnt);
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+	{
+		if (pAd->ate.RxAntennaSel == 0)
+		{
+    		sprintf(extra+strlen(extra), "RSSI-A                          = %ld\n", (LONG)(pAd->ate.LastRssi0 - pAd->BbpRssiToDbmDelta));
+			sprintf(extra+strlen(extra), "RSSI-B (if available)           = %ld\n", (LONG)(pAd->ate.LastRssi1 - pAd->BbpRssiToDbmDelta));
+			sprintf(extra+strlen(extra), "RSSI-C (if available)           = %ld\n\n", (LONG)(pAd->ate.LastRssi2 - pAd->BbpRssiToDbmDelta));
+		}
+		else
+		{
+    		sprintf(extra+strlen(extra), "RSSI                            = %ld\n", (LONG)(pAd->ate.LastRssi0 - pAd->BbpRssiToDbmDelta));
+		}
+	}
+	else
+#endif // RALINK_ATE //
+	{
+    	sprintf(extra+strlen(extra), "RSSI-A                          = %ld\n", (LONG)(pAd->StaCfg.RssiSample.LastRssi0 - pAd->BbpRssiToDbmDelta));
+        sprintf(extra+strlen(extra), "RSSI-B (if available)           = %ld\n", (LONG)(pAd->StaCfg.RssiSample.LastRssi1 - pAd->BbpRssiToDbmDelta));
+        sprintf(extra+strlen(extra), "RSSI-C (if available)           = %ld\n\n", (LONG)(pAd->StaCfg.RssiSample.LastRssi2 - pAd->BbpRssiToDbmDelta));
+	}
+#ifdef WPA_SUPPLICANT_SUPPORT
+    sprintf(extra+strlen(extra), "WpaSupplicantUP                 = %d\n\n", pAd->StaCfg.WpaSupplicantUP);
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+
+    wrq->length = strlen(extra) + 1; // 1: size of '\0'
+    DBGPRINT(RT_DEBUG_TRACE, ("<== rt_private_get_statistics, wrq->length = %d\n", wrq->length));
+
+    return Status;
+}
+
+#ifdef DOT11_N_SUPPORT
+void	getBaInfo(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pOutBuf)
+{
+	INT i, j;
+	BA_ORI_ENTRY *pOriBAEntry;
+	BA_REC_ENTRY *pRecBAEntry;
+
+	for (i=0; i<MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		PMAC_TABLE_ENTRY pEntry = &pAd->MacTab.Content[i];
+		if (((pEntry->ValidAsCLI || pEntry->ValidAsApCli) && (pEntry->Sst == SST_ASSOC))
+			|| (pEntry->ValidAsWDS) || (pEntry->ValidAsMesh))
+		{
+			sprintf(pOutBuf, "%s\n%02X:%02X:%02X:%02X:%02X:%02X (Aid = %d) (AP) -\n",
+                pOutBuf,
+				pEntry->Addr[0], pEntry->Addr[1], pEntry->Addr[2],
+				pEntry->Addr[3], pEntry->Addr[4], pEntry->Addr[5], pEntry->Aid);
+
+			sprintf(pOutBuf, "%s[Recipient]\n", pOutBuf);
+			for (j=0; j < NUM_OF_TID; j++)
+			{
+				if (pEntry->BARecWcidArray[j] != 0)
+				{
+					pRecBAEntry =&pAd->BATable.BARecEntry[pEntry->BARecWcidArray[j]];
+					sprintf(pOutBuf, "%sTID=%d, BAWinSize=%d, LastIndSeq=%d, ReorderingPkts=%d\n", pOutBuf, j, pRecBAEntry->BAWinSize, pRecBAEntry->LastIndSeq, pRecBAEntry->list.qlen);
+				}
+			}
+			sprintf(pOutBuf, "%s\n", pOutBuf);
+
+			sprintf(pOutBuf, "%s[Originator]\n", pOutBuf);
+			for (j=0; j < NUM_OF_TID; j++)
+			{
+				if (pEntry->BAOriWcidArray[j] != 0)
+				{
+					pOriBAEntry =&pAd->BATable.BAOriEntry[pEntry->BAOriWcidArray[j]];
+					sprintf(pOutBuf, "%sTID=%d, BAWinSize=%d, StartSeq=%d, CurTxSeq=%d\n", pOutBuf, j, pOriBAEntry->BAWinSize, pOriBAEntry->Sequence, pEntry->TxSeq[j]);
+				}
+			}
+			sprintf(pOutBuf, "%s\n\n", pOutBuf);
+		}
+        if (strlen(pOutBuf) > (IW_PRIV_SIZE_MASK - 30))
+                break;
+	}
+
+	return;
+}
+#endif // DOT11_N_SUPPORT //
+
+static int
+rt_private_show(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra)
+{
+    INT				Status = 0;
+    VIRTUAL_ADAPTER	*pVirtualAd = NULL;
+    PRTMP_ADAPTER   pAd;
+	POS_COOKIE		pObj;
+    u32             subcmd = wrq->flags;
+
+	if (dev->priv_flags == INT_MAIN)
+		pAd = dev->priv;
+	else
+	{
+		pVirtualAd = dev->priv;
+		pAd = pVirtualAd->RtmpDev->priv;
+	}
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	if (pAd == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+    if (extra == NULL)
+    {
+        wrq->length = 0;
+        return -EIO;
+    }
+    memset(extra, 0x00, IW_PRIV_SIZE_MASK);
+
+	{
+		pObj->ioctl_if_type = INT_MAIN;
+        pObj->ioctl_if = MAIN_MBSSID;
+	}
+
+    switch(subcmd)
+    {
+
+        case SHOW_CONN_STATUS:
+            if (MONITOR_ON(pAd))
+            {
+#ifdef DOT11_N_SUPPORT
+                if (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED &&
+                    pAd->CommonCfg.RegTransmitSetting.field.BW)
+                    sprintf(extra, "Monitor Mode(CentralChannel %d)\n", pAd->CommonCfg.CentralChannel);
+                else
+#endif // DOT11_N_SUPPORT //
+                    sprintf(extra, "Monitor Mode(Channel %d)\n", pAd->CommonCfg.Channel);
+            }
+            else
+            {
+                if (pAd->IndicateMediaState == NdisMediaStateConnected)
+            	{
+            	    if (INFRA_ON(pAd))
+                    {
+                    sprintf(extra, "Connected(AP: %s[%02X:%02X:%02X:%02X:%02X:%02X])\n",
+                                    pAd->CommonCfg.Ssid,
+                                    pAd->CommonCfg.Bssid[0],
+                                    pAd->CommonCfg.Bssid[1],
+                                    pAd->CommonCfg.Bssid[2],
+                                    pAd->CommonCfg.Bssid[3],
+                                    pAd->CommonCfg.Bssid[4],
+                                    pAd->CommonCfg.Bssid[5]);
+            		DBGPRINT(RT_DEBUG_TRACE ,("Ssid=%s ,Ssidlen = %d\n",pAd->CommonCfg.Ssid, pAd->CommonCfg.SsidLen));
+            	}
+                    else if (ADHOC_ON(pAd))
+                        sprintf(extra, "Connected\n");
+            	}
+            	else
+            	{
+            	    sprintf(extra, "Disconnected\n");
+            		DBGPRINT(RT_DEBUG_TRACE ,("ConnStatus is not connected\n"));
+            	}
+            }
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+        case SHOW_DRVIER_VERION:
+            sprintf(extra, "Driver version-%s, %s %s\n", STA_DRIVER_VERSION, __DATE__, __TIME__ );
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+#ifdef DOT11_N_SUPPORT
+        case SHOW_BA_INFO:
+            getBaInfo(pAd, extra);
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+#endif // DOT11_N_SUPPORT //
+		case SHOW_DESC_INFO:
+			{
+				Show_DescInfo_Proc(pAd, NULL);
+				wrq->length = 0; // 1: size of '\0'
+			}
+			break;
+        case RAIO_OFF:
+            if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+            {
+                sprintf(extra, "Scanning\n");
+                wrq->length = strlen(extra) + 1; // 1: size of '\0'
+                break;
+            }
+            pAd->StaCfg.bSwRadio = FALSE;
+            if (pAd->StaCfg.bRadio != (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio))
+            {
+                pAd->StaCfg.bRadio = (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio);
+                if (pAd->StaCfg.bRadio == FALSE)
+                {
+                    MlmeRadioOff(pAd);
+                    // Update extra information
+					pAd->ExtraInfo = SW_RADIO_OFF;
+                }
+            }
+            sprintf(extra, "Radio Off\n");
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+        case RAIO_ON:
+            if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+            {
+                sprintf(extra, "Scanning\n");
+                wrq->length = strlen(extra) + 1; // 1: size of '\0'
+                break;
+            }
+            pAd->StaCfg.bSwRadio = TRUE;
+            //if (pAd->StaCfg.bRadio != (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio))
+            {
+                pAd->StaCfg.bRadio = (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio);
+                if (pAd->StaCfg.bRadio == TRUE)
+                {
+                    MlmeRadioOn(pAd);
+                    // Update extra information
+					pAd->ExtraInfo = EXTRA_INFO_CLEAR;
+                }
+            }
+            sprintf(extra, "Radio On\n");
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+
+
+#ifdef QOS_DLS_SUPPORT
+		case SHOW_DLS_ENTRY_INFO:
+			{
+				Set_DlsEntryInfo_Display_Proc(pAd, NULL);
+				wrq->length = 0; // 1: size of '\0'
+			}
+			break;
+#endif // QOS_DLS_SUPPORT //
+
+		case SHOW_CFG_VALUE:
+			{
+				Status = RTMPShowCfgValue(pAd, wrq->pointer, extra);
+				if (Status == 0)
+					wrq->length = strlen(extra) + 1; // 1: size of '\0'
+			}
+			break;
+		case SHOW_ADHOC_ENTRY_INFO:
+			Show_Adhoc_MacTable_Proc(pAd, extra);
+			wrq->length = strlen(extra) + 1; // 1: size of '\0'
+			break;
+        default:
+            DBGPRINT(RT_DEBUG_TRACE, ("%s - unknow subcmd = %d\n", __FUNCTION__, subcmd));
+            break;
+    }
+
+    return Status;
+}
+
+#ifdef SIOCSIWMLME
+int rt_ioctl_siwmlme(struct net_device *dev,
+			   struct iw_request_info *info,
+			   union iwreq_data *wrqu,
+			   char *extra)
+{
+	PRTMP_ADAPTER   pAd = (PRTMP_ADAPTER) dev->priv;
+	struct iw_mlme *pMlme = (struct iw_mlme *)wrqu->data.pointer;
+	MLME_QUEUE_ELEM				MsgElem;
+	MLME_DISASSOC_REQ_STRUCT	DisAssocReq;
+	MLME_DEAUTH_REQ_STRUCT      DeAuthReq;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("====> %s\n", __FUNCTION__));
+
+	if (pMlme == NULL)
+		return -EINVAL;
+
+	switch(pMlme->cmd)
+	{
+#ifdef IW_MLME_DEAUTH
+		case IW_MLME_DEAUTH:
+			DBGPRINT(RT_DEBUG_TRACE, ("====> %s - IW_MLME_DEAUTH\n", __FUNCTION__));
+			COPY_MAC_ADDR(DeAuthReq.Addr, pAd->CommonCfg.Bssid);
+			DeAuthReq.Reason = pMlme->reason_code;
+			MsgElem.MsgLen = sizeof(MLME_DEAUTH_REQ_STRUCT);
+			NdisMoveMemory(MsgElem.Msg, &DeAuthReq, sizeof(MLME_DEAUTH_REQ_STRUCT));
+			MlmeDeauthReqAction(pAd, &MsgElem);
+			if (INFRA_ON(pAd))
+			{
+			    LinkDown(pAd, FALSE);
+			    pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+			}
+			break;
+#endif // IW_MLME_DEAUTH //
+#ifdef IW_MLME_DISASSOC
+		case IW_MLME_DISASSOC:
+			DBGPRINT(RT_DEBUG_TRACE, ("====> %s - IW_MLME_DISASSOC\n", __FUNCTION__));
+			COPY_MAC_ADDR(DisAssocReq.Addr, pAd->CommonCfg.Bssid);
+			DisAssocReq.Reason =  pMlme->reason_code;
+
+			MsgElem.Machine = ASSOC_STATE_MACHINE;
+			MsgElem.MsgType = MT2_MLME_DISASSOC_REQ;
+			MsgElem.MsgLen = sizeof(MLME_DISASSOC_REQ_STRUCT);
+			NdisMoveMemory(MsgElem.Msg, &DisAssocReq, sizeof(MLME_DISASSOC_REQ_STRUCT));
+
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_DISASSOC;
+			MlmeDisassocReqAction(pAd, &MsgElem);
+			break;
+#endif // IW_MLME_DISASSOC //
+		default:
+			DBGPRINT(RT_DEBUG_TRACE, ("====> %s - Unknow Command\n", __FUNCTION__));
+			break;
+	}
+
+	return 0;
+}
+#endif // SIOCSIWMLME //
+
+#if WIRELESS_EXT > 17
+int rt_ioctl_siwauth(struct net_device *dev,
+			  struct iw_request_info *info,
+			  union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER   pAdapter = (PRTMP_ADAPTER) dev->priv;
+	struct iw_param *param = &wrqu->param;
+
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+	switch (param->flags & IW_AUTH_INDEX) {
+    	case IW_AUTH_WPA_VERSION:
+            if (param->value == IW_AUTH_WPA_VERSION_WPA)
+            {
+                pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPAPSK;
+				if (pAdapter->StaCfg.BssType == BSS_ADHOC)
+					pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPANone;
+            }
+            else if (param->value == IW_AUTH_WPA_VERSION_WPA2)
+                pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA2PSK;
+
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_WPA_VERSION - param->value = %d!\n", __FUNCTION__, param->value));
+            break;
+    	case IW_AUTH_CIPHER_PAIRWISE:
+            if (param->value == IW_AUTH_CIPHER_NONE)
+            {
+                pAdapter->StaCfg.WepStatus = Ndis802_11WEPDisabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11WEPDisabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_WEP40 ||
+                     param->value == IW_AUTH_CIPHER_WEP104)
+            {
+                pAdapter->StaCfg.WepStatus = Ndis802_11WEPEnabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11WEPEnabled;
+#ifdef WPA_SUPPLICANT_SUPPORT
+                pAdapter->StaCfg.IEEE8021X = FALSE;
+#endif // WPA_SUPPLICANT_SUPPORT //
+            }
+            else if (param->value == IW_AUTH_CIPHER_TKIP)
+            {
+                pAdapter->StaCfg.WepStatus = Ndis802_11Encryption2Enabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11Encryption2Enabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_CCMP)
+            {
+                pAdapter->StaCfg.WepStatus = Ndis802_11Encryption3Enabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11Encryption3Enabled;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_CIPHER_PAIRWISE - param->value = %d!\n", __FUNCTION__, param->value));
+            break;
+    	case IW_AUTH_CIPHER_GROUP:
+            if (param->value == IW_AUTH_CIPHER_NONE)
+            {
+                pAdapter->StaCfg.GroupCipher = Ndis802_11WEPDisabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_WEP40 ||
+                     param->value == IW_AUTH_CIPHER_WEP104)
+            {
+                pAdapter->StaCfg.GroupCipher = Ndis802_11WEPEnabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_TKIP)
+            {
+                pAdapter->StaCfg.GroupCipher = Ndis802_11Encryption2Enabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_CCMP)
+            {
+                pAdapter->StaCfg.GroupCipher = Ndis802_11Encryption3Enabled;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_CIPHER_GROUP - param->value = %d!\n", __FUNCTION__, param->value));
+            break;
+    	case IW_AUTH_KEY_MGMT:
+            if (param->value == IW_AUTH_KEY_MGMT_802_1X)
+            {
+                if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK)
+                {
+                    pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA;
+#ifdef WPA_SUPPLICANT_SUPPORT
+                    pAdapter->StaCfg.IEEE8021X = FALSE;
+#endif // WPA_SUPPLICANT_SUPPORT //
+                }
+                else if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
+                {
+                    pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA2;
+#ifdef WPA_SUPPLICANT_SUPPORT
+                    pAdapter->StaCfg.IEEE8021X = FALSE;
+#endif // WPA_SUPPLICANT_SUPPORT //
+                }
+#ifdef WPA_SUPPLICANT_SUPPORT
+                else
+                    // WEP 1x
+                    pAdapter->StaCfg.IEEE8021X = TRUE;
+#endif // WPA_SUPPLICANT_SUPPORT //
+            }
+            else if (param->value == 0)
+            {
+                //pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+				STA_PORT_SECURED(pAdapter);
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_KEY_MGMT - param->value = %d!\n", __FUNCTION__, param->value));
+            break;
+    	case IW_AUTH_RX_UNENCRYPTED_EAPOL:
+            break;
+    	case IW_AUTH_PRIVACY_INVOKED:
+            /*if (param->value == 0)
+			{
+                pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+                pAdapter->StaCfg.WepStatus = Ndis802_11WEPDisabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11WEPDisabled;
+        	    pAdapter->StaCfg.GroupCipher = Ndis802_11WEPDisabled;
+            }*/
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_PRIVACY_INVOKED - param->value = %d!\n", __FUNCTION__, param->value));
+    		break;
+    	case IW_AUTH_DROP_UNENCRYPTED:
+            if (param->value != 0)
+                pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+			else
+			{
+                //pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+				STA_PORT_SECURED(pAdapter);
+			}
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_WPA_VERSION - param->value = %d!\n", __FUNCTION__, param->value));
+    		break;
+    	case IW_AUTH_80211_AUTH_ALG:
+			if (param->value & IW_AUTH_ALG_SHARED_KEY)
+            {
+				pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeShared;
+			}
+            else if (param->value & IW_AUTH_ALG_OPEN_SYSTEM)
+            {
+				pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+			}
+            else
+				return -EINVAL;
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_80211_AUTH_ALG - param->value = %d!\n", __FUNCTION__, param->value));
+			break;
+    	case IW_AUTH_WPA_ENABLED:
+    		DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_WPA_ENABLED - Driver supports WPA!(param->value = %d)\n", __FUNCTION__, param->value));
+    		break;
+    	default:
+    		return -EOPNOTSUPP;
+}
+
+	return 0;
+}
+
+int rt_ioctl_giwauth(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER   pAdapter = (PRTMP_ADAPTER) dev->priv;
+	struct iw_param *param = &wrqu->param;
+
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+    }
+
+	switch (param->flags & IW_AUTH_INDEX) {
+	case IW_AUTH_DROP_UNENCRYPTED:
+        param->value = (pAdapter->StaCfg.WepStatus == Ndis802_11WEPDisabled) ? 0 : 1;
+		break;
+
+	case IW_AUTH_80211_AUTH_ALG:
+        param->value = (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeShared) ? IW_AUTH_ALG_SHARED_KEY : IW_AUTH_ALG_OPEN_SYSTEM;
+		break;
+
+	case IW_AUTH_WPA_ENABLED:
+		param->value = (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA) ? 1 : 0;
+		break;
+
+	default:
+		return -EOPNOTSUPP;
+	}
+    DBGPRINT(RT_DEBUG_TRACE, ("rt_ioctl_giwauth::param->value = %d!\n", param->value));
+	return 0;
+}
+
+void fnSetCipherKey(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  INT             keyIdx,
+    IN  UCHAR           CipherAlg,
+    IN  BOOLEAN         bGTK,
+    IN  struct iw_encode_ext *ext)
+{
+    NdisZeroMemory(&pAdapter->SharedKey[BSS0][keyIdx], sizeof(CIPHER_KEY));
+    pAdapter->SharedKey[BSS0][keyIdx].KeyLen = LEN_TKIP_EK;
+    NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].Key, ext->key, LEN_TKIP_EK);
+    NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].TxMic, ext->key + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+    NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].RxMic, ext->key + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+    pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CipherAlg;
+
+    // Update group key information to ASIC Shared Key Table
+	AsicAddSharedKeyEntry(pAdapter,
+						  BSS0,
+						  keyIdx,
+						  pAdapter->SharedKey[BSS0][keyIdx].CipherAlg,
+						  pAdapter->SharedKey[BSS0][keyIdx].Key,
+						  pAdapter->SharedKey[BSS0][keyIdx].TxMic,
+						  pAdapter->SharedKey[BSS0][keyIdx].RxMic);
+
+    if (bGTK)
+        // Update ASIC WCID attribute table and IVEIV table
+    	RTMPAddWcidAttributeEntry(pAdapter,
+    							  BSS0,
+    							  keyIdx,
+    							  pAdapter->SharedKey[BSS0][keyIdx].CipherAlg,
+    							  NULL);
+    else
+        // Update ASIC WCID attribute table and IVEIV table
+    	RTMPAddWcidAttributeEntry(pAdapter,
+    							  BSS0,
+    							  keyIdx,
+    							  pAdapter->SharedKey[BSS0][keyIdx].CipherAlg,
+    							  &pAdapter->MacTab.Content[BSSID_WCID]);
+}
+
+int rt_ioctl_siwencodeext(struct net_device *dev,
+			   struct iw_request_info *info,
+			   union iwreq_data *wrqu,
+			   char *extra)
+			{
+    PRTMP_ADAPTER   pAdapter = (PRTMP_ADAPTER) dev->priv;
+	struct iw_point *encoding = &wrqu->encoding;
+	struct iw_encode_ext *ext = (struct iw_encode_ext *)extra;
+    int keyIdx, alg = ext->alg;
+
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+    if (encoding->flags & IW_ENCODE_DISABLED)
+	{
+        keyIdx = (encoding->flags & IW_ENCODE_INDEX) - 1;
+        // set BSSID wcid entry of the Pair-wise Key table as no-security mode
+	    AsicRemovePairwiseKeyEntry(pAdapter, BSS0, BSSID_WCID);
+        pAdapter->SharedKey[BSS0][keyIdx].KeyLen = 0;
+		pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_NONE;
+		AsicRemoveSharedKeyEntry(pAdapter, 0, (UCHAR)keyIdx);
+        NdisZeroMemory(&pAdapter->SharedKey[BSS0][keyIdx], sizeof(CIPHER_KEY));
+        DBGPRINT(RT_DEBUG_TRACE, ("%s::Remove all keys!(encoding->flags = %x)\n", __FUNCTION__, encoding->flags));
+    }
+					else
+    {
+        // Get Key Index and convet to our own defined key index
+    	keyIdx = (encoding->flags & IW_ENCODE_INDEX) - 1;
+    	if((keyIdx < 0) || (keyIdx >= NR_WEP_KEYS))
+    		return -EINVAL;
+
+        if (ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY)
+        {
+            pAdapter->StaCfg.DefaultKeyId = keyIdx;
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::DefaultKeyId = %d\n", __FUNCTION__, pAdapter->StaCfg.DefaultKeyId));
+        }
+
+        switch (alg) {
+    		case IW_ENCODE_ALG_NONE:
+                DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_ENCODE_ALG_NONE\n", __FUNCTION__));
+    			break;
+    		case IW_ENCODE_ALG_WEP:
+                DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_ENCODE_ALG_WEP - ext->key_len = %d, keyIdx = %d\n", __FUNCTION__, ext->key_len, keyIdx));
+    			if (ext->key_len == MAX_WEP_KEY_SIZE)
+                {
+        			pAdapter->SharedKey[BSS0][keyIdx].KeyLen = MAX_WEP_KEY_SIZE;
+                    pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_WEP128;
+				}
+        		else if (ext->key_len == MIN_WEP_KEY_SIZE)
+                {
+                    pAdapter->SharedKey[BSS0][keyIdx].KeyLen = MIN_WEP_KEY_SIZE;
+                    pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_WEP64;
+			}
+        		else
+                    return -EINVAL;
+
+                NdisZeroMemory(pAdapter->SharedKey[BSS0][keyIdx].Key,  16);
+			    NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].Key, ext->key, ext->key_len);
+    			break;
+            case IW_ENCODE_ALG_TKIP:
+                DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_ENCODE_ALG_TKIP - keyIdx = %d, ext->key_len = %d\n", __FUNCTION__, keyIdx, ext->key_len));
+                if (ext->key_len == 32)
+                {
+                    if (ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY)
+                    {
+                        fnSetCipherKey(pAdapter, keyIdx, CIPHER_TKIP, FALSE, ext);
+                        if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA2)
+                        {
+                            //pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+                            STA_PORT_SECURED(pAdapter);
+                        }
+		}
+                    else if (ext->ext_flags & IW_ENCODE_EXT_GROUP_KEY)
+                    {
+                        fnSetCipherKey(pAdapter, keyIdx, CIPHER_TKIP, TRUE, ext);
+
+                        // set 802.1x port control
+            	        //pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+            	        STA_PORT_SECURED(pAdapter);
+                    }
+                }
+                else
+                    return -EINVAL;
+                break;
+            case IW_ENCODE_ALG_CCMP:
+                if (ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY)
+		{
+                    fnSetCipherKey(pAdapter, keyIdx, CIPHER_AES, FALSE, ext);
+                    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA2)
+                    	//pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+                    	STA_PORT_SECURED(pAdapter);
+                }
+                else if (ext->ext_flags & IW_ENCODE_EXT_GROUP_KEY)
+                {
+                    fnSetCipherKey(pAdapter, keyIdx, CIPHER_AES, TRUE, ext);
+
+                    // set 802.1x port control
+        	        //pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+        	        STA_PORT_SECURED(pAdapter);
+                }
+                break;
+    		default:
+    			return -EINVAL;
+		}
+    }
+
+    return 0;
+}
+
+int
+rt_ioctl_giwencodeext(struct net_device *dev,
+			  struct iw_request_info *info,
+			  union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER pAd = (PRTMP_ADAPTER) dev->priv;
+	PCHAR pKey = NULL;
+	struct iw_point *encoding = &wrqu->encoding;
+	struct iw_encode_ext *ext = (struct iw_encode_ext *)extra;
+	int idx, max_key_len;
+
+	DBGPRINT(RT_DEBUG_TRACE ,("===> rt_ioctl_giwencodeext\n"));
+
+	max_key_len = encoding->length - sizeof(*ext);
+	if (max_key_len < 0)
+		return -EINVAL;
+
+	idx = encoding->flags & IW_ENCODE_INDEX;
+	if (idx)
+	{
+		if (idx < 1 || idx > 4)
+			return -EINVAL;
+		idx--;
+
+		if ((pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) ||
+			(pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled))
+		{
+			if (idx != pAd->StaCfg.DefaultKeyId)
+			{
+				ext->key_len = 0;
+				return 0;
+			}
+		}
+	}
+	else
+		idx = pAd->StaCfg.DefaultKeyId;
+
+	encoding->flags = idx + 1;
+	memset(ext, 0, sizeof(*ext));
+
+	ext->key_len = 0;
+	switch(pAd->StaCfg.WepStatus) {
+		case Ndis802_11WEPDisabled:
+			ext->alg = IW_ENCODE_ALG_NONE;
+			encoding->flags |= IW_ENCODE_DISABLED;
+			break;
+		case Ndis802_11WEPEnabled:
+			ext->alg = IW_ENCODE_ALG_WEP;
+			if (pAd->SharedKey[BSS0][idx].KeyLen > max_key_len)
+				return -E2BIG;
+			else
+			{
+				ext->key_len = pAd->SharedKey[BSS0][idx].KeyLen;
+				pKey = &(pAd->SharedKey[BSS0][idx].Key[0]);
+			}
+			break;
+		case Ndis802_11Encryption2Enabled:
+		case Ndis802_11Encryption3Enabled:
+			if (pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled)
+				ext->alg = IW_ENCODE_ALG_TKIP;
+			else
+				ext->alg = IW_ENCODE_ALG_CCMP;
+
+			if (max_key_len < 32)
+				return -E2BIG;
+			else
+			{
+				ext->key_len = 32;
+				pKey = &pAd->StaCfg.PMK[0];
+			}
+			break;
+		default:
+			return -EINVAL;
+	}
+
+	if (ext->key_len && pKey)
+	{
+		encoding->flags |= IW_ENCODE_ENABLED;
+		memcpy(ext->key, pKey, ext->key_len);
+	}
+
+	return 0;
+}
+
+#ifdef SIOCSIWGENIE
+int rt_ioctl_siwgenie(struct net_device *dev,
+			  struct iw_request_info *info,
+			  union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER   pAd = (PRTMP_ADAPTER) dev->priv;
+
+	if (wrqu->data.length > MAX_LEN_OF_RSNIE ||
+	    (wrqu->data.length && extra == NULL))
+		return -EINVAL;
+
+	if (wrqu->data.length)
+	{
+		pAd->StaCfg.RSNIE_Len = wrqu->data.length;
+		NdisMoveMemory(&pAd->StaCfg.RSN_IE[0], extra, pAd->StaCfg.RSNIE_Len);
+	}
+	else
+	{
+		pAd->StaCfg.RSNIE_Len = 0;
+		NdisZeroMemory(&pAd->StaCfg.RSN_IE[0], MAX_LEN_OF_RSNIE);
+	}
+
+	return 0;
+}
+#endif // SIOCSIWGENIE //
+
+int rt_ioctl_giwgenie(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER   pAd = (PRTMP_ADAPTER) dev->priv;
+
+	if ((pAd->StaCfg.RSNIE_Len == 0) ||
+		(pAd->StaCfg.AuthMode < Ndis802_11AuthModeWPA))
+	{
+		wrqu->data.length = 0;
+		return 0;
+	}
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+#ifdef SIOCSIWGENIE
+	if (pAd->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_ENABLE)
+	{
+	if (wrqu->data.length < pAd->StaCfg.RSNIE_Len)
+		return -E2BIG;
+
+	wrqu->data.length = pAd->StaCfg.RSNIE_Len;
+	memcpy(extra, &pAd->StaCfg.RSN_IE[0], pAd->StaCfg.RSNIE_Len);
+	}
+	else
+#endif // SIOCSIWGENIE //
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+	{
+		UCHAR RSNIe = IE_WPA;
+
+		if (wrqu->data.length < (pAd->StaCfg.RSNIE_Len + 2)) // ID, Len
+			return -E2BIG;
+		wrqu->data.length = pAd->StaCfg.RSNIE_Len + 2;
+
+		if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK) ||
+            (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2))
+			RSNIe = IE_RSN;
+
+		extra[0] = (char)RSNIe;
+		extra[1] = pAd->StaCfg.RSNIE_Len;
+		memcpy(extra+2, &pAd->StaCfg.RSN_IE[0], pAd->StaCfg.RSNIE_Len);
+	}
+
+	return 0;
+}
+
+int rt_ioctl_siwpmksa(struct net_device *dev,
+			   struct iw_request_info *info,
+			   union iwreq_data *wrqu,
+			   char *extra)
+{
+	PRTMP_ADAPTER   pAd = (PRTMP_ADAPTER) dev->priv;
+	struct iw_pmksa *pPmksa = (struct iw_pmksa *)wrqu->data.pointer;
+	INT	CachedIdx = 0, idx = 0;
+
+	if (pPmksa == NULL)
+		return -EINVAL;
+
+	DBGPRINT(RT_DEBUG_TRACE ,("===> rt_ioctl_siwpmksa\n"));
+	switch(pPmksa->cmd)
+	{
+		case IW_PMKSA_FLUSH:
+			NdisZeroMemory(pAd->StaCfg.SavedPMK, sizeof(BSSID_INFO)*PMKID_NO);
+			DBGPRINT(RT_DEBUG_TRACE ,("rt_ioctl_siwpmksa - IW_PMKSA_FLUSH\n"));
+			break;
+		case IW_PMKSA_REMOVE:
+			for (CachedIdx = 0; CachedIdx < pAd->StaCfg.SavedPMKNum; CachedIdx++)
+			{
+		        // compare the BSSID
+		        if (NdisEqualMemory(pPmksa->bssid.sa_data, pAd->StaCfg.SavedPMK[CachedIdx].BSSID, MAC_ADDR_LEN))
+		        {
+		        	NdisZeroMemory(pAd->StaCfg.SavedPMK[CachedIdx].BSSID, MAC_ADDR_LEN);
+					NdisZeroMemory(pAd->StaCfg.SavedPMK[CachedIdx].PMKID, 16);
+					for (idx = CachedIdx; idx < (pAd->StaCfg.SavedPMKNum - 1); idx++)
+					{
+						NdisMoveMemory(&pAd->StaCfg.SavedPMK[idx].BSSID[0], &pAd->StaCfg.SavedPMK[idx+1].BSSID[0], MAC_ADDR_LEN);
+						NdisMoveMemory(&pAd->StaCfg.SavedPMK[idx].PMKID[0], &pAd->StaCfg.SavedPMK[idx+1].PMKID[0], 16);
+					}
+					pAd->StaCfg.SavedPMKNum--;
+			        break;
+		        }
+	        }
+
+			DBGPRINT(RT_DEBUG_TRACE ,("rt_ioctl_siwpmksa - IW_PMKSA_REMOVE\n"));
+			break;
+		case IW_PMKSA_ADD:
+			for (CachedIdx = 0; CachedIdx < pAd->StaCfg.SavedPMKNum; CachedIdx++)
+			{
+		        // compare the BSSID
+		        if (NdisEqualMemory(pPmksa->bssid.sa_data, pAd->StaCfg.SavedPMK[CachedIdx].BSSID, MAC_ADDR_LEN))
+			        break;
+	        }
+
+	        // Found, replace it
+	        if (CachedIdx < PMKID_NO)
+	        {
+		        DBGPRINT(RT_DEBUG_OFF, ("Update PMKID, idx = %d\n", CachedIdx));
+		        NdisMoveMemory(&pAd->StaCfg.SavedPMK[CachedIdx].BSSID[0], pPmksa->bssid.sa_data, MAC_ADDR_LEN);
+				NdisMoveMemory(&pAd->StaCfg.SavedPMK[CachedIdx].PMKID[0], pPmksa->pmkid, 16);
+		        pAd->StaCfg.SavedPMKNum++;
+	        }
+	        // Not found, replace the last one
+	        else
+	        {
+		        // Randomly replace one
+		        CachedIdx = (pPmksa->bssid.sa_data[5] % PMKID_NO);
+		        DBGPRINT(RT_DEBUG_OFF, ("Update PMKID, idx = %d\n", CachedIdx));
+		        NdisMoveMemory(&pAd->StaCfg.SavedPMK[CachedIdx].BSSID[0], pPmksa->bssid.sa_data, MAC_ADDR_LEN);
+				NdisMoveMemory(&pAd->StaCfg.SavedPMK[CachedIdx].PMKID[0], pPmksa->pmkid, 16);
+	        }
+
+			DBGPRINT(RT_DEBUG_TRACE ,("rt_ioctl_siwpmksa - IW_PMKSA_ADD\n"));
+			break;
+		default:
+			DBGPRINT(RT_DEBUG_TRACE ,("rt_ioctl_siwpmksa - Unknow Command!!\n"));
+			break;
+	}
+
+	return 0;
+}
+#endif // #if WIRELESS_EXT > 17
+
+#ifdef DBG
+static int
+rt_private_ioctl_bbp(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra)
+			{
+	CHAR				*this_char;
+	CHAR				*value = NULL;
+	UCHAR				regBBP = 0;
+//	CHAR				arg[255]={0};
+	UINT32				bbpId;
+	UINT32				bbpValue;
+	BOOLEAN				bIsPrintAllBBP = FALSE;
+	INT					Status = 0;
+    PRTMP_ADAPTER       pAdapter = (PRTMP_ADAPTER) dev->priv;
+
+
+	memset(extra, 0x00, IW_PRIV_SIZE_MASK);
+
+	if (wrq->length > 1) //No parameters.
+				{
+		sprintf(extra, "\n");
+
+		//Parsing Read or Write
+		this_char = wrq->pointer;
+		DBGPRINT(RT_DEBUG_TRACE, ("this_char=%s\n", this_char));
+		if (!*this_char)
+			goto next;
+
+		if ((value = rtstrchr(this_char, '=')) != NULL)
+			*value++ = 0;
+
+		if (!value || !*value)
+		{ //Read
+			DBGPRINT(RT_DEBUG_TRACE, ("this_char=%s, value=%s\n", this_char, value));
+			if (sscanf(this_char, "%d", &(bbpId)) == 1)
+			{
+				if (bbpId <= 136)
+				{
+#ifdef RALINK_ATE
+					if (ATE_ON(pAdapter))
+					{
+						ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+					}
+					else
+#endif // RALINK_ATE //
+					{
+					RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+					}
+					sprintf(extra+strlen(extra), "R%02d[0x%02X]:%02X\n", bbpId, bbpId*2, regBBP);
+                    wrq->length = strlen(extra) + 1; // 1: size of '\0'
+					DBGPRINT(RT_DEBUG_TRACE, ("msg=%s\n", extra));
+				}
+				else
+				{//Invalid parametes, so default printk all bbp
+					bIsPrintAllBBP = TRUE;
+					goto next;
+				}
+			}
+			else
+			{ //Invalid parametes, so default printk all bbp
+				bIsPrintAllBBP = TRUE;
+				goto next;
+			}
+		}
+		else
+		{ //Write
+			if ((sscanf(this_char, "%d", &(bbpId)) == 1) && (sscanf(value, "%x", &(bbpValue)) == 1))
+			{
+				if (bbpId <= 136)
+				{
+#ifdef RALINK_ATE
+					if (ATE_ON(pAdapter))
+					{
+						ATE_BBP_IO_WRITE8_BY_REG_ID(pAdapter, bbpId, bbpValue);
+						//Read it back for showing
+						ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+					}
+					else
+#endif // RALINK_ATE //
+					{
+					    RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, bbpId, bbpValue);
+    					//Read it back for showing
+    					RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+			}
+					sprintf(extra+strlen(extra), "R%02d[0x%02X]:%02X\n", bbpId, bbpId*2, regBBP);
+                    wrq->length = strlen(extra) + 1; // 1: size of '\0'
+					DBGPRINT(RT_DEBUG_TRACE, ("msg=%s\n", extra));
+				}
+				else
+				{//Invalid parametes, so default printk all bbp
+					bIsPrintAllBBP = TRUE;
+					goto next;
+				}
+			}
+			else
+			{ //Invalid parametes, so default printk all bbp
+				bIsPrintAllBBP = TRUE;
+				goto next;
+			}
+		}
+		}
+	else
+		bIsPrintAllBBP = TRUE;
+
+next:
+	if (bIsPrintAllBBP)
+	{
+		memset(extra, 0x00, IW_PRIV_SIZE_MASK);
+		sprintf(extra, "\n");
+		for (bbpId = 0; bbpId <= 136; bbpId++)
+		{
+		    if (strlen(extra) >= (IW_PRIV_SIZE_MASK - 10))
+                break;
+#ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+			}
+			else
+#endif // RALINK_ATE //
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+			sprintf(extra+strlen(extra), "R%02d[0x%02X]:%02X    ", bbpId, bbpId*2, regBBP);
+			if (bbpId%5 == 4)
+				sprintf(extra+strlen(extra), "\n");
+		}
+
+        wrq->length = strlen(extra) + 1; // 1: size of '\0'
+        DBGPRINT(RT_DEBUG_TRACE, ("wrq->length = %d\n", wrq->length));
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<==rt_private_ioctl_bbp\n\n"));
+
+    return Status;
+}
+#endif // DBG //
+
+int rt_ioctl_siwrate(struct net_device *dev,
+			struct iw_request_info *info,
+			union iwreq_data *wrqu, char *extra)
+{
+    PRTMP_ADAPTER   pAd = (PRTMP_ADAPTER) dev->priv;
+    UINT32          rate = wrqu->bitrate.value, fixed = wrqu->bitrate.fixed;
+
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("rt_ioctl_siwrate::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+    DBGPRINT(RT_DEBUG_TRACE, ("rt_ioctl_siwrate::(rate = %d, fixed = %d)\n", rate, fixed));
+    /* rate = -1 => auto rate
+       rate = X, fixed = 1 => (fixed rate X)
+    */
+    if (rate == -1)
+    {
+		//Auto Rate
+		pAd->StaCfg.DesiredTransmitSetting.field.MCS = MCS_AUTO;
+		pAd->StaCfg.bAutoTxRateSwitch = TRUE;
+		if ((pAd->CommonCfg.PhyMode <= PHY_11G) ||
+		    (pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE <= MODE_OFDM))
+			RTMPSetDesiredRates(pAd, -1);
+
+#ifdef DOT11_N_SUPPORT
+		SetCommonHT(pAd);
+#endif // DOT11_N_SUPPORT //
+    }
+    else
+    {
+        if (fixed)
+        {
+        	pAd->StaCfg.bAutoTxRateSwitch = FALSE;
+            if ((pAd->CommonCfg.PhyMode <= PHY_11G) ||
+                (pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE <= MODE_OFDM))
+                RTMPSetDesiredRates(pAd, rate);
+            else
+            {
+                pAd->StaCfg.DesiredTransmitSetting.field.MCS = MCS_AUTO;
+#ifdef DOT11_N_SUPPORT
+                SetCommonHT(pAd);
+#endif // DOT11_N_SUPPORT //
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("rt_ioctl_siwrate::(HtMcs=%d)\n",pAd->StaCfg.DesiredTransmitSetting.field.MCS));
+        }
+        else
+        {
+            // TODO: rate = X, fixed = 0 => (rates <= X)
+            return -EOPNOTSUPP;
+        }
+    }
+
+    return 0;
+}
+
+int rt_ioctl_giwrate(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+    PRTMP_ADAPTER   pAd = (PRTMP_ADAPTER) dev->priv;
+    int rate_index = 0, rate_count = 0;
+    HTTRANSMIT_SETTING ht_setting;
+    __s32 ralinkrate[] =
+	{2,  4,   11,  22, // CCK
+	12, 18,   24,  36, 48, 72, 96, 108, // OFDM
+	13, 26,   39,  52,  78, 104, 117, 130, 26,  52,  78, 104, 156, 208, 234, 260, // 20MHz, 800ns GI, MCS: 0 ~ 15
+	39, 78,  117, 156, 234, 312, 351, 390,										  // 20MHz, 800ns GI, MCS: 16 ~ 23
+	27, 54,   81, 108, 162, 216, 243, 270, 54, 108, 162, 216, 324, 432, 486, 540, // 40MHz, 800ns GI, MCS: 0 ~ 15
+	81, 162, 243, 324, 486, 648, 729, 810,										  // 40MHz, 800ns GI, MCS: 16 ~ 23
+	14, 29,   43,  57,  87, 115, 130, 144, 29, 59,   87, 115, 173, 230, 260, 288, // 20MHz, 400ns GI, MCS: 0 ~ 15
+	43, 87,  130, 173, 260, 317, 390, 433,										  // 20MHz, 400ns GI, MCS: 16 ~ 23
+	30, 60,   90, 120, 180, 240, 270, 300, 60, 120, 180, 240, 360, 480, 540, 600, // 40MHz, 400ns GI, MCS: 0 ~ 15
+	90, 180, 270, 360, 540, 720, 810, 900};										  // 40MHz, 400ns GI, MCS: 16 ~ 23
+
+    rate_count = sizeof(ralinkrate)/sizeof(__s32);
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+    if ((pAd->StaCfg.bAutoTxRateSwitch == FALSE) &&
+        (INFRA_ON(pAd)) &&
+        ((pAd->CommonCfg.PhyMode <= PHY_11G) || (pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE <= MODE_OFDM)))
+        ht_setting.word = pAd->StaCfg.HTPhyMode.word;
+    else
+        ht_setting.word = pAd->MacTab.Content[BSSID_WCID].HTPhyMode.word;
+
+#ifdef DOT11_N_SUPPORT
+    if (ht_setting.field.MODE >= MODE_HTMIX)
+    {
+//    	rate_index = 12 + ((UCHAR)ht_setting.field.BW *16) + ((UCHAR)ht_setting.field.ShortGI *32) + ((UCHAR)ht_setting.field.MCS);
+    	rate_index = 12 + ((UCHAR)ht_setting.field.BW *24) + ((UCHAR)ht_setting.field.ShortGI *48) + ((UCHAR)ht_setting.field.MCS);
+    }
+    else
+#endif // DOT11_N_SUPPORT //
+    if (ht_setting.field.MODE == MODE_OFDM)
+    	rate_index = (UCHAR)(ht_setting.field.MCS) + 4;
+    else if (ht_setting.field.MODE == MODE_CCK)
+    	rate_index = (UCHAR)(ht_setting.field.MCS);
+
+    if (rate_index < 0)
+        rate_index = 0;
+
+    if (rate_index > rate_count)
+        rate_index = rate_count;
+
+    wrqu->bitrate.value = ralinkrate[rate_index] * 500000;
+    wrqu->bitrate.disabled = 0;
+
+    return 0;
+}
+
+static const iw_handler rt_handler[] =
+{
+	(iw_handler) NULL,			            /* SIOCSIWCOMMIT */
+	(iw_handler) rt_ioctl_giwname,			/* SIOCGIWNAME   */
+	(iw_handler) NULL,			            /* SIOCSIWNWID   */
+	(iw_handler) NULL,			            /* SIOCGIWNWID   */
+	(iw_handler) rt_ioctl_siwfreq,		    /* SIOCSIWFREQ   */
+	(iw_handler) rt_ioctl_giwfreq,		    /* SIOCGIWFREQ   */
+	(iw_handler) rt_ioctl_siwmode,		    /* SIOCSIWMODE   */
+	(iw_handler) rt_ioctl_giwmode,		    /* SIOCGIWMODE   */
+	(iw_handler) NULL,		                /* SIOCSIWSENS   */
+	(iw_handler) NULL,		                /* SIOCGIWSENS   */
+	(iw_handler) NULL /* not used */,		/* SIOCSIWRANGE  */
+	(iw_handler) rt_ioctl_giwrange,		    /* SIOCGIWRANGE  */
+	(iw_handler) NULL /* not used */,		/* SIOCSIWPRIV   */
+	(iw_handler) NULL /* kernel code */,    /* SIOCGIWPRIV   */
+	(iw_handler) NULL /* not used */,		/* SIOCSIWSTATS  */
+	(iw_handler) rt28xx_get_wireless_stats /* kernel code */,    /* SIOCGIWSTATS  */
+	(iw_handler) NULL,		                /* SIOCSIWSPY    */
+	(iw_handler) NULL,		                /* SIOCGIWSPY    */
+	(iw_handler) NULL,				        /* SIOCSIWTHRSPY */
+	(iw_handler) NULL,				        /* SIOCGIWTHRSPY */
+	(iw_handler) rt_ioctl_siwap,            /* SIOCSIWAP     */
+	(iw_handler) rt_ioctl_giwap,		    /* SIOCGIWAP     */
+#ifdef SIOCSIWMLME
+	(iw_handler) rt_ioctl_siwmlme,	        /* SIOCSIWMLME   */
+#else
+	(iw_handler) NULL,				        /* SIOCSIWMLME */
+#endif // SIOCSIWMLME //
+	(iw_handler) rt_ioctl_iwaplist,		    /* SIOCGIWAPLIST */
+#ifdef SIOCGIWSCAN
+	(iw_handler) rt_ioctl_siwscan,		    /* SIOCSIWSCAN   */
+	(iw_handler) rt_ioctl_giwscan,		    /* SIOCGIWSCAN   */
+#else
+	(iw_handler) NULL,				        /* SIOCSIWSCAN   */
+	(iw_handler) NULL,				        /* SIOCGIWSCAN   */
+#endif /* SIOCGIWSCAN */
+	(iw_handler) rt_ioctl_siwessid,		    /* SIOCSIWESSID  */
+	(iw_handler) rt_ioctl_giwessid,		    /* SIOCGIWESSID  */
+	(iw_handler) rt_ioctl_siwnickn,		    /* SIOCSIWNICKN  */
+	(iw_handler) rt_ioctl_giwnickn,		    /* SIOCGIWNICKN  */
+	(iw_handler) NULL,				        /* -- hole --    */
+	(iw_handler) NULL,				        /* -- hole --    */
+	(iw_handler) rt_ioctl_siwrate,          /* SIOCSIWRATE   */
+	(iw_handler) rt_ioctl_giwrate,          /* SIOCGIWRATE   */
+	(iw_handler) rt_ioctl_siwrts,		    /* SIOCSIWRTS    */
+	(iw_handler) rt_ioctl_giwrts,		    /* SIOCGIWRTS    */
+	(iw_handler) rt_ioctl_siwfrag,		    /* SIOCSIWFRAG   */
+	(iw_handler) rt_ioctl_giwfrag,		    /* SIOCGIWFRAG   */
+	(iw_handler) NULL,		                /* SIOCSIWTXPOW  */
+	(iw_handler) NULL,		                /* SIOCGIWTXPOW  */
+	(iw_handler) NULL,		                /* SIOCSIWRETRY  */
+	(iw_handler) NULL,		                /* SIOCGIWRETRY  */
+	(iw_handler) rt_ioctl_siwencode,		/* SIOCSIWENCODE */
+	(iw_handler) rt_ioctl_giwencode,		/* SIOCGIWENCODE */
+	(iw_handler) NULL,		                /* SIOCSIWPOWER  */
+	(iw_handler) NULL,		                /* SIOCGIWPOWER  */
+	(iw_handler) NULL,						/* -- hole -- */
+	(iw_handler) NULL,						/* -- hole -- */
+#if WIRELESS_EXT > 17
+    (iw_handler) rt_ioctl_siwgenie,         /* SIOCSIWGENIE  */
+	(iw_handler) rt_ioctl_giwgenie,         /* SIOCGIWGENIE  */
+	(iw_handler) rt_ioctl_siwauth,		    /* SIOCSIWAUTH   */
+	(iw_handler) rt_ioctl_giwauth,		    /* SIOCGIWAUTH   */
+	(iw_handler) rt_ioctl_siwencodeext,	    /* SIOCSIWENCODEEXT */
+	(iw_handler) rt_ioctl_giwencodeext,		/* SIOCGIWENCODEEXT */
+	(iw_handler) rt_ioctl_siwpmksa,         /* SIOCSIWPMKSA  */
+#endif
+};
+
+static const iw_handler rt_priv_handlers[] = {
+	(iw_handler) NULL, /* + 0x00 */
+	(iw_handler) NULL, /* + 0x01 */
+#ifndef CONFIG_AP_SUPPORT
+	(iw_handler) rt_ioctl_setparam, /* + 0x02 */
+#else
+	(iw_handler) NULL, /* + 0x02 */
+#endif // CONFIG_AP_SUPPORT //
+#ifdef DBG
+	(iw_handler) rt_private_ioctl_bbp, /* + 0x03 */
+#else
+	(iw_handler) NULL, /* + 0x03 */
+#endif
+	(iw_handler) NULL, /* + 0x04 */
+	(iw_handler) NULL, /* + 0x05 */
+	(iw_handler) NULL, /* + 0x06 */
+	(iw_handler) NULL, /* + 0x07 */
+	(iw_handler) NULL, /* + 0x08 */
+	(iw_handler) rt_private_get_statistics, /* + 0x09 */
+	(iw_handler) NULL, /* + 0x0A */
+	(iw_handler) NULL, /* + 0x0B */
+	(iw_handler) NULL, /* + 0x0C */
+	(iw_handler) NULL, /* + 0x0D */
+	(iw_handler) NULL, /* + 0x0E */
+	(iw_handler) NULL, /* + 0x0F */
+	(iw_handler) NULL, /* + 0x10 */
+	(iw_handler) rt_private_show, /* + 0x11 */
+    (iw_handler) NULL, /* + 0x12 */
+	(iw_handler) NULL, /* + 0x13 */
+	(iw_handler) NULL, /* + 0x15 */
+	(iw_handler) NULL, /* + 0x17 */
+	(iw_handler) NULL, /* + 0x18 */
+};
+
+const struct iw_handler_def rt28xx_iw_handler_def =
+{
+#define	N(a)	(sizeof (a) / sizeof (a[0]))
+	.standard	= (iw_handler *) rt_handler,
+	.num_standard	= sizeof(rt_handler) / sizeof(iw_handler),
+	.private	= (iw_handler *) rt_priv_handlers,
+	.num_private		= N(rt_priv_handlers),
+	.private_args	= (struct iw_priv_args *) privtab,
+	.num_private_args	= N(privtab),
+#if IW_HANDLER_VERSION >= 7
+    .get_wireless_stats = rt28xx_get_wireless_stats,
+#endif
+};
+
+INT RTMPSetInformation(
+    IN  PRTMP_ADAPTER pAdapter,
+    IN  OUT struct ifreq    *rq,
+    IN  INT                 cmd)
+{
+    struct iwreq                        *wrq = (struct iwreq *) rq;
+    NDIS_802_11_SSID                    Ssid;
+    NDIS_802_11_MAC_ADDRESS             Bssid;
+    RT_802_11_PHY_MODE                  PhyMode;
+    RT_802_11_STA_CONFIG                StaConfig;
+    NDIS_802_11_RATES                   aryRates;
+    RT_802_11_PREAMBLE                  Preamble;
+    NDIS_802_11_WEP_STATUS              WepStatus;
+    NDIS_802_11_AUTHENTICATION_MODE     AuthMode = Ndis802_11AuthModeMax;
+    NDIS_802_11_NETWORK_INFRASTRUCTURE  BssType;
+    NDIS_802_11_RTS_THRESHOLD           RtsThresh;
+    NDIS_802_11_FRAGMENTATION_THRESHOLD FragThresh;
+    NDIS_802_11_POWER_MODE              PowerMode;
+    PNDIS_802_11_KEY                    pKey = NULL;
+    PNDIS_802_11_WEP			        pWepKey =NULL;
+    PNDIS_802_11_REMOVE_KEY             pRemoveKey = NULL;
+    NDIS_802_11_CONFIGURATION           Config, *pConfig = NULL;
+    NDIS_802_11_NETWORK_TYPE            NetType;
+    ULONG                               Now;
+    UINT                                KeyIdx = 0;
+    INT                                 Status = NDIS_STATUS_SUCCESS, MaxPhyMode = PHY_11G;
+    ULONG                               PowerTemp;
+    BOOLEAN                             RadioState;
+    BOOLEAN                             StateMachineTouched = FALSE;
+#ifdef DOT11_N_SUPPORT
+	OID_SET_HT_PHYMODE					HT_PhyMode;	//11n ,kathy
+#endif // DOT11_N_SUPPORT //
+#ifdef WPA_SUPPLICANT_SUPPORT
+    PNDIS_802_11_PMKID                  pPmkId = NULL;
+    BOOLEAN				                IEEE8021xState = FALSE;
+    BOOLEAN				                IEEE8021x_required_keys = FALSE;
+    UCHAR                               wpa_supplicant_enable = 0;
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef SNMP_SUPPORT
+	TX_RTY_CFG_STRUC			tx_rty_cfg;
+	ULONG						ShortRetryLimit, LongRetryLimit;
+	UCHAR						ctmp;
+#endif // SNMP_SUPPORT //
+
+
+
+#ifdef DOT11_N_SUPPORT
+	MaxPhyMode = PHY_11N_5G;
+#endif // DOT11_N_SUPPORT //
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("-->RTMPSetInformation(),	0x%08x\n", cmd&0x7FFF));
+    switch(cmd & 0x7FFF) {
+        case RT_OID_802_11_COUNTRY_REGION:
+            if (wrq->u.data.length < sizeof(UCHAR))
+                Status = -EINVAL;
+			// Only avaliable when EEPROM not programming
+            else if (!(pAdapter->CommonCfg.CountryRegion & 0x80) && !(pAdapter->CommonCfg.CountryRegionForABand & 0x80))
+            {
+                ULONG   Country;
+                UCHAR	TmpPhy;
+
+				Status = copy_from_user(&Country, wrq->u.data.pointer, wrq->u.data.length);
+				pAdapter->CommonCfg.CountryRegion = (UCHAR)(Country & 0x000000FF);
+				pAdapter->CommonCfg.CountryRegionForABand = (UCHAR)((Country >> 8) & 0x000000FF);
+                TmpPhy = pAdapter->CommonCfg.PhyMode;
+				pAdapter->CommonCfg.PhyMode = 0xff;
+				// Build all corresponding channel information
+				RTMPSetPhyMode(pAdapter, TmpPhy);
+#ifdef DOT11_N_SUPPORT
+				SetCommonHT(pAdapter);
+#endif // DOT11_N_SUPPORT //
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_COUNTRY_REGION (A:%d  B/G:%d)\n", pAdapter->CommonCfg.CountryRegionForABand,
+				    pAdapter->CommonCfg.CountryRegion));
+            }
+            break;
+        case OID_802_11_BSSID_LIST_SCAN:
+ #ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+				break;
+			}
+#endif // RALINK_ATE //
+            Now = jiffies;
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_BSSID_LIST_SCAN, TxCnt = %d \n", pAdapter->RalinkCounters.LastOneSecTotalTxCount));
+
+            if (MONITOR_ON(pAdapter))
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! Driver is in Monitor Mode now !!!\n"));
+                break;
+            }
+
+			//Benson add 20080527, when radio off, sta don't need to scan
+			if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_RADIO_OFF))
+				break;
+
+			if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+			{
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! Driver is scanning now !!!\n"));
+				pAdapter->StaCfg.bScanReqIsFromWebUI = TRUE;
+				Status = NDIS_STATUS_SUCCESS;
+                break;
+            }
+
+			if (pAdapter->RalinkCounters.LastOneSecTotalTxCount > 100)
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! Link UP, ignore this set::OID_802_11_BSSID_LIST_SCAN\n"));
+				Status = NDIS_STATUS_SUCCESS;
+				pAdapter->StaCfg.ScanCnt = 99;		// Prevent auto scan triggered by this OID
+				break;
+            }
+
+            if ((OPSTATUS_TEST_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED)) &&
+				((pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
+				(pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+				(pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) ||
+				(pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)) &&
+                (pAdapter->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! Link UP, Port Not Secured! ignore this set::OID_802_11_BSSID_LIST_SCAN\n"));
+				Status = NDIS_STATUS_SUCCESS;
+				pAdapter->StaCfg.ScanCnt = 99;		// Prevent auto scan triggered by this OID
+				break;
+            }
+
+
+            if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+            {
+                RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+            }
+
+            // tell CNTL state machine to call NdisMSetInformationComplete() after completing
+            // this request, because this request is initiated by NDIS.
+            pAdapter->MlmeAux.CurrReqIsFromNdis = FALSE;
+            // Reset allowed scan retries
+            pAdapter->StaCfg.ScanCnt = 0;
+            pAdapter->StaCfg.LastScanTime = Now;
+
+			pAdapter->StaCfg.bScanReqIsFromWebUI = TRUE;
+            RTMP_SET_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS);
+            MlmeEnqueue(pAdapter,
+                        MLME_CNTL_STATE_MACHINE,
+                        OID_802_11_BSSID_LIST_SCAN,
+                        0,
+                        NULL);
+
+            Status = NDIS_STATUS_SUCCESS;
+            StateMachineTouched = TRUE;
+            break;
+        case OID_802_11_SSID:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_SSID))
+                Status = -EINVAL;
+            else
+            {
+            	PCHAR pSsidString = NULL;
+                Status = copy_from_user(&Ssid, wrq->u.data.pointer, wrq->u.data.length);
+
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_SSID (Len=%d,Ssid=%s)\n", Ssid.SsidLength, Ssid.Ssid));
+                if (Ssid.SsidLength > MAX_LEN_OF_SSID)
+                    Status = -EINVAL;
+                else
+                {
+                	if (Ssid.SsidLength == 0)
+                	{
+                		Set_SSID_Proc(pAdapter, "");
+                	}
+					else
+                	{
+	                	pSsidString = (CHAR *) kmalloc(MAX_LEN_OF_SSID+1, MEM_ALLOC_FLAG);
+						if (pSsidString)
+						{
+							NdisZeroMemory(pSsidString, MAX_LEN_OF_SSID+1);
+							NdisMoveMemory(pSsidString, Ssid.Ssid, Ssid.SsidLength);
+	                		Set_SSID_Proc(pAdapter, pSsidString);
+							kfree(pSsidString);
+						}
+						else
+							Status = -ENOMEM;
+                	}
+                }
+            }
+            break;
+        case OID_802_11_BSSID:
+#ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+				break;
+			}
+#endif // RALINK_ATE //
+            if (wrq->u.data.length != sizeof(NDIS_802_11_MAC_ADDRESS))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&Bssid, wrq->u.data.pointer, wrq->u.data.length);
+
+                // tell CNTL state machine to call NdisMSetInformationComplete() after completing
+                // this request, because this request is initiated by NDIS.
+                pAdapter->MlmeAux.CurrReqIsFromNdis = FALSE;
+
+				// Prevent to connect AP again in STAMlmePeriodicExec
+				pAdapter->MlmeAux.AutoReconnectSsidLen= 32;
+
+                // Reset allowed scan retries
+				pAdapter->StaCfg.ScanCnt = 0;
+
+                if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+                {
+                    RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+                    DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+                }
+                MlmeEnqueue(pAdapter,
+                            MLME_CNTL_STATE_MACHINE,
+                            OID_802_11_BSSID,
+                            sizeof(NDIS_802_11_MAC_ADDRESS),
+                            (VOID *)&Bssid);
+                Status = NDIS_STATUS_SUCCESS;
+                StateMachineTouched = TRUE;
+
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_BSSID %02x:%02x:%02x:%02x:%02x:%02x\n",
+                                        Bssid[0], Bssid[1], Bssid[2], Bssid[3], Bssid[4], Bssid[5]));
+            }
+            break;
+        case RT_OID_802_11_RADIO:
+            if (wrq->u.data.length != sizeof(BOOLEAN))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&RadioState, wrq->u.data.pointer, wrq->u.data.length);
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_RADIO (=%d)\n", RadioState));
+                if (pAdapter->StaCfg.bSwRadio != RadioState)
+                {
+                    pAdapter->StaCfg.bSwRadio = RadioState;
+                    if (pAdapter->StaCfg.bRadio != (pAdapter->StaCfg.bHwRadio && pAdapter->StaCfg.bSwRadio))
+                    {
+                        pAdapter->StaCfg.bRadio = (pAdapter->StaCfg.bHwRadio && pAdapter->StaCfg.bSwRadio);
+                        if (pAdapter->StaCfg.bRadio == TRUE)
+                        {
+                            MlmeRadioOn(pAdapter);
+                            // Update extra information
+							pAdapter->ExtraInfo = EXTRA_INFO_CLEAR;
+                        }
+                        else
+                        {
+                            MlmeRadioOff(pAdapter);
+                            // Update extra information
+							pAdapter->ExtraInfo = SW_RADIO_OFF;
+                        }
+                    }
+                }
+            }
+            break;
+        case RT_OID_802_11_PHY_MODE:
+            if (wrq->u.data.length != sizeof(RT_802_11_PHY_MODE))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&PhyMode, wrq->u.data.pointer, wrq->u.data.length);
+				if (PhyMode <= MaxPhyMode)
+				{
+                	RTMPSetPhyMode(pAdapter, PhyMode);
+#ifdef DOT11_N_SUPPORT
+					SetCommonHT(pAdapter);
+#endif // DOT11_N_SUPPORT //
+				}
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_PHY_MODE (=%d)\n", PhyMode));
+            }
+            break;
+        case RT_OID_802_11_STA_CONFIG:
+            if (wrq->u.data.length != sizeof(RT_802_11_STA_CONFIG))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&StaConfig, wrq->u.data.pointer, wrq->u.data.length);
+                pAdapter->CommonCfg.bEnableTxBurst = StaConfig.EnableTxBurst;
+                pAdapter->CommonCfg.UseBGProtection = StaConfig.UseBGProtection;
+                pAdapter->CommonCfg.bUseShortSlotTime = 1; // 2003-10-30 always SHORT SLOT capable
+                if ((pAdapter->CommonCfg.PhyMode != StaConfig.AdhocMode) &&
+					(StaConfig.AdhocMode <= MaxPhyMode))
+                {
+                    // allow dynamic change of "USE OFDM rate or not" in ADHOC mode
+                    // if setting changed, need to reset current TX rate as well as BEACON frame format
+                    if (pAdapter->StaCfg.BssType == BSS_ADHOC)
+                    {
+						pAdapter->CommonCfg.PhyMode = StaConfig.AdhocMode;
+                    	RTMPSetPhyMode(pAdapter, PhyMode);
+                        MlmeUpdateTxRates(pAdapter, FALSE, 0);
+                        MakeIbssBeacon(pAdapter);           // re-build BEACON frame
+                        AsicEnableIbssSync(pAdapter);   // copy to on-chip memory
+                    }
+                }
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_STA_CONFIG (Burst=%d, Protection=%ld,ShortSlot=%d\n",
+                                        pAdapter->CommonCfg.bEnableTxBurst,
+                                        pAdapter->CommonCfg.UseBGProtection,
+                                        pAdapter->CommonCfg.bUseShortSlotTime));
+            }
+            break;
+        case OID_802_11_DESIRED_RATES:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_RATES))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&aryRates, wrq->u.data.pointer, wrq->u.data.length);
+                NdisZeroMemory(pAdapter->CommonCfg.DesireRate, MAX_LEN_OF_SUPPORTED_RATES);
+                NdisMoveMemory(pAdapter->CommonCfg.DesireRate, &aryRates, sizeof(NDIS_802_11_RATES));
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_DESIRED_RATES (%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x)\n",
+                    pAdapter->CommonCfg.DesireRate[0],pAdapter->CommonCfg.DesireRate[1],
+                    pAdapter->CommonCfg.DesireRate[2],pAdapter->CommonCfg.DesireRate[3],
+                    pAdapter->CommonCfg.DesireRate[4],pAdapter->CommonCfg.DesireRate[5],
+                    pAdapter->CommonCfg.DesireRate[6],pAdapter->CommonCfg.DesireRate[7] ));
+                // Changing DesiredRate may affect the MAX TX rate we used to TX frames out
+                MlmeUpdateTxRates(pAdapter, FALSE, 0);
+            }
+            break;
+        case RT_OID_802_11_PREAMBLE:
+            if (wrq->u.data.length != sizeof(RT_802_11_PREAMBLE))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&Preamble, wrq->u.data.pointer, wrq->u.data.length);
+                if (Preamble == Rt802_11PreambleShort)
+                {
+                    pAdapter->CommonCfg.TxPreamble = Preamble;
+                    MlmeSetTxPreamble(pAdapter, Rt802_11PreambleShort);
+                }
+                else if ((Preamble == Rt802_11PreambleLong) || (Preamble == Rt802_11PreambleAuto))
+                {
+                    // if user wants AUTO, initialize to LONG here, then change according to AP's
+                    // capability upon association.
+                    pAdapter->CommonCfg.TxPreamble = Preamble;
+                    MlmeSetTxPreamble(pAdapter, Rt802_11PreambleLong);
+                }
+                else
+                {
+                    Status = -EINVAL;
+                    break;
+                }
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_PREAMBLE (=%d)\n", Preamble));
+            }
+            break;
+        case OID_802_11_WEP_STATUS:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_WEP_STATUS))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&WepStatus, wrq->u.data.pointer, wrq->u.data.length);
+                // Since TKIP, AES, WEP are all supported. It should not have any invalid setting
+                if (WepStatus <= Ndis802_11Encryption3KeyAbsent)
+                {
+                    if (pAdapter->StaCfg.WepStatus != WepStatus)
+                    {
+                        // Config has changed
+                        pAdapter->bConfigChanged = TRUE;
+                    }
+                    pAdapter->StaCfg.WepStatus     = WepStatus;
+                    pAdapter->StaCfg.OrigWepStatus = WepStatus;
+                    pAdapter->StaCfg.PairCipher    = WepStatus;
+                	pAdapter->StaCfg.GroupCipher   = WepStatus;
+                }
+                else
+                {
+                    Status  = -EINVAL;
+                    break;
+                }
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_WEP_STATUS (=%d)\n",WepStatus));
+            }
+            break;
+        case OID_802_11_AUTHENTICATION_MODE:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_AUTHENTICATION_MODE))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&AuthMode, wrq->u.data.pointer, wrq->u.data.length);
+                if (AuthMode > Ndis802_11AuthModeMax)
+                {
+                    Status  = -EINVAL;
+                    break;
+                }
+                else
+                {
+                    if (pAdapter->StaCfg.AuthMode != AuthMode)
+                    {
+                        // Config has changed
+                        pAdapter->bConfigChanged = TRUE;
+                    }
+                    pAdapter->StaCfg.AuthMode = AuthMode;
+                }
+                pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_AUTHENTICATION_MODE (=%d) \n",pAdapter->StaCfg.AuthMode));
+            }
+            break;
+        case OID_802_11_INFRASTRUCTURE_MODE:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_NETWORK_INFRASTRUCTURE))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&BssType, wrq->u.data.pointer, wrq->u.data.length);
+
+				if (BssType == Ndis802_11IBSS)
+					Set_NetworkType_Proc(pAdapter, "Adhoc");
+				else if (BssType == Ndis802_11Infrastructure)
+					Set_NetworkType_Proc(pAdapter, "Infra");
+				else if (BssType == Ndis802_11Monitor)
+					Set_NetworkType_Proc(pAdapter, "Monitor");
+				else
+				{
+					Status  = -EINVAL;
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_INFRASTRUCTURE_MODE (unknown)\n"));
+				}
+			}
+			break;
+	 case OID_802_11_REMOVE_WEP:
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_WEP\n"));
+            if (wrq->u.data.length != sizeof(NDIS_802_11_KEY_INDEX))
+            {
+				Status = -EINVAL;
+            }
+            else
+            {
+				KeyIdx = *(NDIS_802_11_KEY_INDEX *) wrq->u.data.pointer;
+
+				if (KeyIdx & 0x80000000)
+				{
+					// Should never set default bit when remove key
+					Status = -EINVAL;
+				}
+				else
+				{
+					KeyIdx = KeyIdx & 0x0fffffff;
+					if (KeyIdx >= 4){
+						Status = -EINVAL;
+					}
+					else
+					{
+						pAdapter->SharedKey[BSS0][KeyIdx].KeyLen = 0;
+						pAdapter->SharedKey[BSS0][KeyIdx].CipherAlg = CIPHER_NONE;
+						AsicRemoveSharedKeyEntry(pAdapter, 0, (UCHAR)KeyIdx);
+					}
+				}
+            }
+            break;
+        case RT_OID_802_11_RESET_COUNTERS:
+            NdisZeroMemory(&pAdapter->WlanCounters, sizeof(COUNTER_802_11));
+            NdisZeroMemory(&pAdapter->Counters8023, sizeof(COUNTER_802_3));
+            NdisZeroMemory(&pAdapter->RalinkCounters, sizeof(COUNTER_RALINK));
+            pAdapter->Counters8023.RxNoBuffer   = 0;
+			pAdapter->Counters8023.GoodReceives = 0;
+			pAdapter->Counters8023.RxNoBuffer   = 0;
+#ifdef RT2870
+			pAdapter->BulkOutComplete	= 0;
+			pAdapter->BulkOutCompleteOther= 0;
+			pAdapter->BulkOutCompleteCancel = 0;
+			pAdapter->BulkOutReq = 0;
+			pAdapter->BulkInReq= 0;
+			pAdapter->BulkInComplete = 0;
+			pAdapter->BulkInCompleteFail = 0;
+#endif // RT2870 //
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_RESET_COUNTERS \n"));
+            break;
+        case OID_802_11_RTS_THRESHOLD:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_RTS_THRESHOLD))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&RtsThresh, wrq->u.data.pointer, wrq->u.data.length);
+                if (RtsThresh > MAX_RTS_THRESHOLD)
+                    Status  = -EINVAL;
+                else
+                    pAdapter->CommonCfg.RtsThreshold = (USHORT)RtsThresh;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_RTS_THRESHOLD (=%ld)\n",RtsThresh));
+            break;
+        case OID_802_11_FRAGMENTATION_THRESHOLD:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_FRAGMENTATION_THRESHOLD))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&FragThresh, wrq->u.data.pointer, wrq->u.data.length);
+                pAdapter->CommonCfg.bUseZeroToDisableFragment = FALSE;
+                if (FragThresh > MAX_FRAG_THRESHOLD || FragThresh < MIN_FRAG_THRESHOLD)
+                {
+                    if (FragThresh == 0)
+                    {
+                        pAdapter->CommonCfg.FragmentThreshold = MAX_FRAG_THRESHOLD;
+                        pAdapter->CommonCfg.bUseZeroToDisableFragment = TRUE;
+                    }
+                    else
+                        Status  = -EINVAL;
+                }
+                else
+                    pAdapter->CommonCfg.FragmentThreshold = (USHORT)FragThresh;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_FRAGMENTATION_THRESHOLD (=%ld) \n",FragThresh));
+            break;
+        case OID_802_11_POWER_MODE:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_POWER_MODE))
+                Status = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&PowerMode, wrq->u.data.pointer, wrq->u.data.length);
+                if (PowerMode == Ndis802_11PowerModeCAM)
+                	Set_PSMode_Proc(pAdapter, "CAM");
+                else if (PowerMode == Ndis802_11PowerModeMAX_PSP)
+                	Set_PSMode_Proc(pAdapter, "Max_PSP");
+                else if (PowerMode == Ndis802_11PowerModeFast_PSP)
+					Set_PSMode_Proc(pAdapter, "Fast_PSP");
+                else if (PowerMode == Ndis802_11PowerModeLegacy_PSP)
+					Set_PSMode_Proc(pAdapter, "Legacy_PSP");
+                else
+                    Status = -EINVAL;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_POWER_MODE (=%d)\n",PowerMode));
+            break;
+         case RT_OID_802_11_TX_POWER_LEVEL_1:
+			if (wrq->u.data.length  < sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&PowerTemp, wrq->u.data.pointer, wrq->u.data.length);
+				if (PowerTemp > 100)
+					PowerTemp = 0xffffffff;  // AUTO
+				pAdapter->CommonCfg.TxPowerDefault = PowerTemp; //keep current setting.
+					pAdapter->CommonCfg.TxPowerPercentage = pAdapter->CommonCfg.TxPowerDefault;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_TX_POWER_LEVEL_1 (=%ld)\n", pAdapter->CommonCfg.TxPowerPercentage));
+			}
+	        break;
+		case OID_802_11_NETWORK_TYPE_IN_USE:
+			if (wrq->u.data.length != sizeof(NDIS_802_11_NETWORK_TYPE))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&NetType, wrq->u.data.pointer, wrq->u.data.length);
+
+				if (NetType == Ndis802_11DS)
+					RTMPSetPhyMode(pAdapter, PHY_11B);
+				else if (NetType == Ndis802_11OFDM24)
+					RTMPSetPhyMode(pAdapter, PHY_11BG_MIXED);
+				else if (NetType == Ndis802_11OFDM5)
+					RTMPSetPhyMode(pAdapter, PHY_11A);
+				else
+					Status = -EINVAL;
+#ifdef DOT11_N_SUPPORT
+				if (Status == NDIS_STATUS_SUCCESS)
+					SetCommonHT(pAdapter);
+#endif // DOT11_N_SUPPORT //
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_NETWORK_TYPE_IN_USE (=%d)\n",NetType));
+		    }
+			break;
+        // For WPA PSK PMK key
+        case RT_OID_802_11_ADD_WPA:
+            pKey = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+            if(pKey == NULL)
+            {
+                Status = -ENOMEM;
+                break;
+            }
+
+            Status = copy_from_user(pKey, wrq->u.data.pointer, wrq->u.data.length);
+            if (pKey->Length != wrq->u.data.length)
+            {
+                Status  = -EINVAL;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_WPA, Failed!!\n"));
+            }
+            else
+            {
+                if ((pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPAPSK) &&
+				    (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPA2PSK) &&
+				    (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPANone) )
+                {
+                    Status = -EOPNOTSUPP;
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_WPA, Failed!! [AuthMode != WPAPSK/WPA2PSK/WPANONE]\n"));
+                }
+                else if ((pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+						 (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK) ||
+						 (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPANone) )     // Only for WPA PSK mode
+				{
+                    NdisMoveMemory(pAdapter->StaCfg.PMK, &pKey->KeyMaterial, pKey->KeyLength);
+                    // Use RaConfig as PSK agent.
+                    // Start STA supplicant state machine
+                    if (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPANone)
+                        pAdapter->StaCfg.WpaState = SS_START;
+
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_WPA (id=0x%x, Len=%d-byte)\n", pKey->KeyIndex, pKey->KeyLength));
+                }
+                else
+                {
+                    pAdapter->StaCfg.WpaState = SS_NOTUSE;
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_WPA (id=0x%x, Len=%d-byte)\n", pKey->KeyIndex, pKey->KeyLength));
+                }
+            }
+            kfree(pKey);
+            break;
+        case OID_802_11_REMOVE_KEY:
+            pRemoveKey = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+            if(pRemoveKey == NULL)
+            {
+                Status = -ENOMEM;
+                break;
+            }
+
+            Status = copy_from_user(pRemoveKey, wrq->u.data.pointer, wrq->u.data.length);
+            if (pRemoveKey->Length != wrq->u.data.length)
+            {
+                Status  = -EINVAL;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY, Failed!!\n"));
+            }
+            else
+            {
+                if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+                {
+                    RTMPWPARemoveKeyProc(pAdapter, pRemoveKey);
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY, Remove WPA Key!!\n"));
+                }
+                else
+                {
+                    KeyIdx = pRemoveKey->KeyIndex;
+
+                    if (KeyIdx & 0x80000000)
+                    {
+                        // Should never set default bit when remove key
+                        Status  = -EINVAL;
+                        DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY, Failed!!(Should never set default bit when remove key)\n"));
+                    }
+                    else
+                    {
+                        KeyIdx = KeyIdx & 0x0fffffff;
+                        if (KeyIdx > 3)
+                        {
+                            Status  = -EINVAL;
+                            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY, Failed!!(KeyId[%d] out of range)\n", KeyIdx));
+                        }
+                        else
+                        {
+                            pAdapter->SharedKey[BSS0][KeyIdx].KeyLen = 0;
+                            pAdapter->SharedKey[BSS0][KeyIdx].CipherAlg = CIPHER_NONE;
+                            AsicRemoveSharedKeyEntry(pAdapter, 0, (UCHAR)KeyIdx);
+                            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY (id=0x%x, Len=%d-byte)\n", pRemoveKey->KeyIndex, pRemoveKey->Length));
+                        }
+                    }
+                }
+            }
+            kfree(pRemoveKey);
+            break;
+        // New for WPA
+        case OID_802_11_ADD_KEY:
+            pKey = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+            if(pKey == NULL)
+            {
+                Status = -ENOMEM;
+                break;
+            }
+            Status = copy_from_user(pKey, wrq->u.data.pointer, wrq->u.data.length);
+            if (pKey->Length != wrq->u.data.length)
+            {
+                Status  = -EINVAL;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_KEY, Failed!!\n"));
+            }
+            else
+            {
+                RTMPAddKey(pAdapter, pKey);
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_KEY (id=0x%x, Len=%d-byte)\n", pKey->KeyIndex, pKey->KeyLength));
+            }
+            kfree(pKey);
+            break;
+        case OID_802_11_CONFIGURATION:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_CONFIGURATION))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&Config, wrq->u.data.pointer, wrq->u.data.length);
+                pConfig = &Config;
+
+                if ((pConfig->BeaconPeriod >= 20) && (pConfig->BeaconPeriod <=400))
+                     pAdapter->CommonCfg.BeaconPeriod = (USHORT) pConfig->BeaconPeriod;
+
+                pAdapter->StaActive.AtimWin = (USHORT) pConfig->ATIMWindow;
+                MAP_KHZ_TO_CHANNEL_ID(pConfig->DSConfig, pAdapter->CommonCfg.Channel);
+                //
+				// Save the channel on MlmeAux for CntlOidRTBssidProc used.
+				//
+				pAdapter->MlmeAux.Channel = pAdapter->CommonCfg.Channel;
+
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_CONFIGURATION (BeacnPeriod=%ld,AtimW=%ld,Ch=%d)\n",
+                    pConfig->BeaconPeriod, pConfig->ATIMWindow, pAdapter->CommonCfg.Channel));
+                // Config has changed
+                pAdapter->bConfigChanged = TRUE;
+            }
+            break;
+#ifdef DOT11_N_SUPPORT
+		case RT_OID_802_11_SET_HT_PHYMODE:
+			if (wrq->u.data.length	!= sizeof(OID_SET_HT_PHYMODE))
+				Status = -EINVAL;
+			else
+			{
+			    POID_SET_HT_PHYMODE	pHTPhyMode = &HT_PhyMode;
+
+				Status = copy_from_user(&HT_PhyMode, wrq->u.data.pointer, wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::pHTPhyMode	(PhyMode = %d,TransmitNo = %d, HtMode =	%d,	ExtOffset =	%d , MCS = %d, BW =	%d,	STBC = %d, SHORTGI = %d) \n",
+				pHTPhyMode->PhyMode, pHTPhyMode->TransmitNo,pHTPhyMode->HtMode,pHTPhyMode->ExtOffset,
+				pHTPhyMode->MCS, pHTPhyMode->BW, pHTPhyMode->STBC,	pHTPhyMode->SHORTGI));
+				if (pAdapter->CommonCfg.PhyMode	>= PHY_11ABGN_MIXED)
+					RTMPSetHT(pAdapter,	pHTPhyMode);
+			}
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_HT_PHYMODE(MCS=%d,BW=%d,SGI=%d,STBC=%d)\n",
+				pAdapter->StaCfg.HTPhyMode.field.MCS, pAdapter->StaCfg.HTPhyMode.field.BW, pAdapter->StaCfg.HTPhyMode.field.ShortGI,
+				pAdapter->StaCfg.HTPhyMode.field.STBC));
+			break;
+#endif // DOT11_N_SUPPORT //
+		case RT_OID_802_11_SET_APSD_SETTING:
+			if (wrq->u.data.length != sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				ULONG apsd ;
+				Status = copy_from_user(&apsd, wrq->u.data.pointer,	wrq->u.data.length);
+
+				/*-------------------------------------------------------------------
+				|B31~B7	|	B6~B5	 |	 B4	 |	 B3	 |	B2	 |	B1	 |	   B0		|
+				---------------------------------------------------------------------
+				| Rsvd	| Max SP Len | AC_VO | AC_VI | AC_BK | AC_BE | APSD	Capable	|
+				---------------------------------------------------------------------*/
+				pAdapter->CommonCfg.bAPSDCapable = (apsd & 0x00000001) ? TRUE :	FALSE;
+				pAdapter->CommonCfg.bAPSDAC_BE = ((apsd	& 0x00000002) >> 1)	? TRUE : FALSE;
+				pAdapter->CommonCfg.bAPSDAC_BK = ((apsd	& 0x00000004) >> 2)	? TRUE : FALSE;
+				pAdapter->CommonCfg.bAPSDAC_VI = ((apsd	& 0x00000008) >> 3)	? TRUE : FALSE;
+				pAdapter->CommonCfg.bAPSDAC_VO = ((apsd	& 0x00000010) >> 4)	? TRUE : FALSE;
+				pAdapter->CommonCfg.MaxSPLength	= (UCHAR)((apsd	& 0x00000060) >> 5);
+
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_APSD_SETTING (apsd=0x%lx, APSDCap=%d, [BE,BK,VI,VO]=[%d/%d/%d/%d],	MaxSPLen=%d)\n", apsd, pAdapter->CommonCfg.bAPSDCapable,
+					pAdapter->CommonCfg.bAPSDAC_BE,	pAdapter->CommonCfg.bAPSDAC_BK,	pAdapter->CommonCfg.bAPSDAC_VI,	pAdapter->CommonCfg.bAPSDAC_VO,	pAdapter->CommonCfg.MaxSPLength));
+			}
+			break;
+
+		case RT_OID_802_11_SET_APSD_PSM:
+			if (wrq->u.data.length	!= sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				// Driver needs	to notify AP when PSM changes
+				Status = copy_from_user(&pAdapter->CommonCfg.bAPSDForcePowerSave, wrq->u.data.pointer, wrq->u.data.length);
+				if (pAdapter->CommonCfg.bAPSDForcePowerSave	!= pAdapter->StaCfg.Psm)
+				{
+					MlmeSetPsmBit(pAdapter,	pAdapter->CommonCfg.bAPSDForcePowerSave);
+					RTMPSendNullFrame(pAdapter,	pAdapter->CommonCfg.TxRate,	TRUE);
+				}
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_APSD_PSM (bAPSDForcePowerSave:%d)\n",	pAdapter->CommonCfg.bAPSDForcePowerSave));
+			}
+			break;
+#ifdef QOS_DLS_SUPPORT
+		case RT_OID_802_11_SET_DLS:
+			if (wrq->u.data.length != sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				BOOLEAN	oldvalue = pAdapter->CommonCfg.bDLSCapable;
+				Status = copy_from_user(&pAdapter->CommonCfg.bDLSCapable, wrq->u.data.pointer, wrq->u.data.length);
+				if (oldvalue &&	!pAdapter->CommonCfg.bDLSCapable)
+				{
+					int	i;
+					// tear	down local dls table entry
+					for	(i=0; i<MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+					{
+						if (pAdapter->StaCfg.DLSEntry[i].Valid && (pAdapter->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+						{
+							pAdapter->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+							pAdapter->StaCfg.DLSEntry[i].Valid	= FALSE;
+							RTMPSendDLSTearDownFrame(pAdapter, pAdapter->StaCfg.DLSEntry[i].MacAddr);
+						}
+					}
+
+					// tear	down peer dls table	entry
+					for	(i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+					{
+						if (pAdapter->StaCfg.DLSEntry[i].Valid && (pAdapter->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+						{
+							pAdapter->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+							pAdapter->StaCfg.DLSEntry[i].Valid	= FALSE;
+							RTMPSendDLSTearDownFrame(pAdapter, pAdapter->StaCfg.DLSEntry[i].MacAddr);
+						}
+					}
+				}
+
+				DBGPRINT(RT_DEBUG_TRACE,("Set::RT_OID_802_11_SET_DLS (=%d)\n", pAdapter->CommonCfg.bDLSCapable));
+			}
+			break;
+
+		case RT_OID_802_11_SET_DLS_PARAM:
+			if (wrq->u.data.length	!= sizeof(RT_802_11_DLS_UI))
+				Status = -EINVAL;
+			else
+			{
+				RT_802_11_DLS	Dls;
+
+				NdisZeroMemory(&Dls, sizeof(RT_802_11_DLS));
+				RTMPMoveMemory(&Dls, wrq->u.data.pointer, sizeof(RT_802_11_DLS_UI));
+				MlmeEnqueue(pAdapter,
+							MLME_CNTL_STATE_MACHINE,
+							RT_OID_802_11_SET_DLS_PARAM,
+							sizeof(RT_802_11_DLS),
+							&Dls);
+				DBGPRINT(RT_DEBUG_TRACE,("Set::RT_OID_802_11_SET_DLS_PARAM \n"));
+			}
+			break;
+#endif // QOS_DLS_SUPPORT //
+		case RT_OID_802_11_SET_WMM:
+			if (wrq->u.data.length	!= sizeof(BOOLEAN))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&pAdapter->CommonCfg.bWmmCapable, wrq->u.data.pointer, wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_WMM (=%d)	\n", pAdapter->CommonCfg.bWmmCapable));
+			}
+			break;
+
+		case OID_802_11_DISASSOCIATE:
+#ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+				break;
+			}
+#endif // RALINK_ATE //
+			//
+			// Set NdisRadioStateOff to	TRUE, instead of called	MlmeRadioOff.
+			// Later on, NDIS_802_11_BSSID_LIST_EX->NumberOfItems should be	0
+			// when	query OID_802_11_BSSID_LIST.
+			//
+			// TRUE:  NumberOfItems	will set to	0.
+			// FALSE: NumberOfItems	no change.
+			//
+			pAdapter->CommonCfg.NdisRadioStateOff =	TRUE;
+			// Set to immediately send the media disconnect	event
+			pAdapter->MlmeAux.CurrReqIsFromNdis	= TRUE;
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_DISASSOCIATE	\n"));
+
+			if (INFRA_ON(pAdapter))
+			{
+				if (pAdapter->Mlme.CntlMachine.CurrState !=	CNTL_IDLE)
+				{
+					RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+					DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME	busy, reset	MLME state machine !!!\n"));
+				}
+
+				MlmeEnqueue(pAdapter,
+					MLME_CNTL_STATE_MACHINE,
+					OID_802_11_DISASSOCIATE,
+					0,
+					NULL);
+
+				StateMachineTouched	= TRUE;
+			}
+			break;
+
+#ifdef DOT11_N_SUPPORT
+		case RT_OID_802_11_SET_IMME_BA_CAP:
+				if (wrq->u.data.length != sizeof(OID_BACAP_STRUC))
+					Status = -EINVAL;
+				else
+				{
+					OID_BACAP_STRUC Orde ;
+					Status = copy_from_user(&Orde, wrq->u.data.pointer, wrq->u.data.length);
+					if (Orde.Policy > BA_NOTUSE)
+					{
+						Status = NDIS_STATUS_INVALID_DATA;
+					}
+					else if (Orde.Policy == BA_NOTUSE)
+					{
+						pAdapter->CommonCfg.BACapability.field.Policy = BA_NOTUSE;
+						pAdapter->CommonCfg.BACapability.field.MpduDensity = Orde.MpduDensity;
+						pAdapter->CommonCfg.DesiredHtPhy.MpduDensity = Orde.MpduDensity;
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduEnable = Orde.AmsduEnable;
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduSize= Orde.AmsduSize;
+						pAdapter->CommonCfg.DesiredHtPhy.MimoPs= Orde.MMPSmode;
+						pAdapter->CommonCfg.BACapability.field.MMPSmode = Orde.MMPSmode;
+						// UPdata to HT IE
+						pAdapter->CommonCfg.HtCapability.HtCapInfo.MimoPs = Orde.MMPSmode;
+						pAdapter->CommonCfg.HtCapability.HtCapInfo.AMsduSize = Orde.AmsduSize;
+						pAdapter->CommonCfg.HtCapability.HtCapParm.MpduDensity = Orde.MpduDensity;
+					}
+					else
+					{
+                        pAdapter->CommonCfg.BACapability.field.AutoBA = Orde.AutoBA;
+						pAdapter->CommonCfg.BACapability.field.Policy = IMMED_BA; // we only support immediate BA.
+						pAdapter->CommonCfg.BACapability.field.MpduDensity = Orde.MpduDensity;
+						pAdapter->CommonCfg.DesiredHtPhy.MpduDensity = Orde.MpduDensity;
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduEnable = Orde.AmsduEnable;
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduSize= Orde.AmsduSize;
+						pAdapter->CommonCfg.DesiredHtPhy.MimoPs = Orde.MMPSmode;
+						pAdapter->CommonCfg.BACapability.field.MMPSmode = Orde.MMPSmode;
+
+						// UPdata to HT IE
+						pAdapter->CommonCfg.HtCapability.HtCapInfo.MimoPs = Orde.MMPSmode;
+						pAdapter->CommonCfg.HtCapability.HtCapInfo.AMsduSize = Orde.AmsduSize;
+						pAdapter->CommonCfg.HtCapability.HtCapParm.MpduDensity = Orde.MpduDensity;
+
+						if (pAdapter->CommonCfg.BACapability.field.RxBAWinLimit > MAX_RX_REORDERBUF)
+							pAdapter->CommonCfg.BACapability.field.RxBAWinLimit = MAX_RX_REORDERBUF;
+
+					}
+
+					pAdapter->CommonCfg.REGBACapability.word = pAdapter->CommonCfg.BACapability.word;
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::(Orde.AutoBA = %d) (Policy=%d)(ReBAWinLimit=%d)(TxBAWinLimit=%d)(AutoMode=%d)\n",Orde.AutoBA, pAdapter->CommonCfg.BACapability.field.Policy,
+						pAdapter->CommonCfg.BACapability.field.RxBAWinLimit,pAdapter->CommonCfg.BACapability.field.TxBAWinLimit, pAdapter->CommonCfg.BACapability.field.AutoBA));
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::(MimoPs = %d)(AmsduEnable = %d) (AmsduSize=%d)(MpduDensity=%d)\n",pAdapter->CommonCfg.DesiredHtPhy.MimoPs, pAdapter->CommonCfg.DesiredHtPhy.AmsduEnable,
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduSize, pAdapter->CommonCfg.DesiredHtPhy.MpduDensity));
+				}
+
+				break;
+		case RT_OID_802_11_ADD_IMME_BA:
+			DBGPRINT(RT_DEBUG_TRACE, (" Set :: RT_OID_802_11_ADD_IMME_BA \n"));
+			if (wrq->u.data.length != sizeof(OID_ADD_BA_ENTRY))
+					Status = -EINVAL;
+			else
+			{
+				UCHAR		        index;
+				OID_ADD_BA_ENTRY    BA;
+				MAC_TABLE_ENTRY     *pEntry;
+
+				Status = copy_from_user(&BA, wrq->u.data.pointer, wrq->u.data.length);
+				if (BA.TID > 15)
+				{
+					Status = NDIS_STATUS_INVALID_DATA;
+					break;
+				}
+				else
+				{
+					//BATableInsertEntry
+					//As ad-hoc mode, BA pair is not limited to only BSSID. so add via OID.
+					index = BA.TID;
+					// in ad hoc mode, when adding BA pair, we should insert this entry into MACEntry too
+					pEntry = MacTableLookup(pAdapter, BA.MACAddr);
+					if (!pEntry)
+					{
+						DBGPRINT(RT_DEBUG_TRACE, ("RT_OID_802_11_ADD_IMME_BA. break on no connection.----:%x:%x\n", BA.MACAddr[4], BA.MACAddr[5]));
+						break;
+					}
+					if (BA.IsRecipient == FALSE)
+					{
+					    if (pEntry->bIAmBadAtheros == TRUE)
+							pAdapter->CommonCfg.BACapability.field.RxBAWinLimit = 0x10;
+
+						BAOriSessionSetUp(pAdapter, pEntry, index, 0, 100, TRUE);
+					}
+					else
+					{
+						//BATableInsertEntry(pAdapter, pEntry->Aid, BA.MACAddr, 0, 0xffff, BA.TID, BA.nMSDU, BA.IsRecipient);
+					}
+
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_IMME_BA. Rec = %d. Mac = %x:%x:%x:%x:%x:%x . \n",
+						BA.IsRecipient, BA.MACAddr[0], BA.MACAddr[1], BA.MACAddr[2], BA.MACAddr[2]
+						, BA.MACAddr[4], BA.MACAddr[5]));
+				}
+			}
+			break;
+
+		case RT_OID_802_11_TEAR_IMME_BA:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set :: RT_OID_802_11_TEAR_IMME_BA \n"));
+			if (wrq->u.data.length != sizeof(OID_ADD_BA_ENTRY))
+					Status = -EINVAL;
+			else
+			{
+				POID_ADD_BA_ENTRY	pBA;
+				MAC_TABLE_ENTRY *pEntry;
+
+				pBA = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+
+				if (pBA == NULL)
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("Set :: RT_OID_802_11_TEAR_IMME_BA kmalloc() can't allocate enough memory\n"));
+					Status = NDIS_STATUS_FAILURE;
+				}
+				else
+				{
+					Status = copy_from_user(pBA, wrq->u.data.pointer, wrq->u.data.length);
+					DBGPRINT(RT_DEBUG_TRACE, ("Set :: RT_OID_802_11_TEAR_IMME_BA(TID=%d, bAllTid=%d)\n", pBA->TID, pBA->bAllTid));
+
+					if (!pBA->bAllTid && (pBA->TID > NUM_OF_TID))
+					{
+						Status = NDIS_STATUS_INVALID_DATA;
+						break;
+					}
+
+					if (pBA->IsRecipient == FALSE)
+					{
+						pEntry = MacTableLookup(pAdapter, pBA->MACAddr);
+						DBGPRINT(RT_DEBUG_TRACE, (" pBA->IsRecipient == FALSE\n"));
+						if (pEntry)
+						{
+							DBGPRINT(RT_DEBUG_TRACE, (" pBA->pEntry\n"));
+							BAOriSessionTearDown(pAdapter, pEntry->Aid, pBA->TID, FALSE, TRUE);
+						}
+						else
+							DBGPRINT(RT_DEBUG_TRACE, ("Set :: Not found pEntry \n"));
+					}
+					else
+					{
+						pEntry = MacTableLookup(pAdapter, pBA->MACAddr);
+						if (pEntry)
+						{
+							BARecSessionTearDown( pAdapter, (UCHAR)pEntry->Aid, pBA->TID, TRUE);
+						}
+						else
+							DBGPRINT(RT_DEBUG_TRACE, ("Set :: Not found pEntry \n"));
+					}
+					kfree(pBA);
+				}
+            }
+            break;
+#endif // DOT11_N_SUPPORT //
+
+        // For WPA_SUPPLICANT to set static wep key
+    	case OID_802_11_ADD_WEP:
+    	    pWepKey = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+
+    	    if(pWepKey == NULL)
+            {
+                Status = -ENOMEM;
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP, Failed!!\n"));
+                break;
+            }
+            Status = copy_from_user(pWepKey, wrq->u.data.pointer, wrq->u.data.length);
+            if (Status)
+            {
+                Status  = -EINVAL;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP, Failed (length mismatch)!!\n"));
+            }
+            else
+            {
+		        KeyIdx = pWepKey->KeyIndex & 0x0fffffff;
+                // KeyIdx must be 0 ~ 3
+                if (KeyIdx > 4)
+    			{
+                    Status  = -EINVAL;
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP, Failed (KeyIdx must be smaller than 4)!!\n"));
+                }
+                else
+                {
+                    UCHAR CipherAlg = 0;
+                    PUCHAR Key;
+
+                    // set key material and key length
+                    NdisZeroMemory(pAdapter->SharedKey[BSS0][KeyIdx].Key, 16);
+                    pAdapter->SharedKey[BSS0][KeyIdx].KeyLen = (UCHAR) pWepKey->KeyLength;
+                    NdisMoveMemory(pAdapter->SharedKey[BSS0][KeyIdx].Key, &pWepKey->KeyMaterial, pWepKey->KeyLength);
+
+                    switch(pWepKey->KeyLength)
+                    {
+                        case 5:
+                            CipherAlg = CIPHER_WEP64;
+                            break;
+                        case 13:
+                            CipherAlg = CIPHER_WEP128;
+                            break;
+                        default:
+                            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP, only support CIPHER_WEP64(len:5) & CIPHER_WEP128(len:13)!!\n"));
+                            Status = -EINVAL;
+                            break;
+                    }
+                    pAdapter->SharedKey[BSS0][KeyIdx].CipherAlg = CipherAlg;
+
+                    // Default key for tx (shared key)
+                    if (pWepKey->KeyIndex & 0x80000000)
+                    {
+#ifdef WPA_SUPPLICANT_SUPPORT
+                        // set key material and key length
+                        NdisZeroMemory(pAdapter->StaCfg.DesireSharedKey[KeyIdx].Key, 16);
+                        pAdapter->StaCfg.DesireSharedKey[KeyIdx].KeyLen = (UCHAR) pWepKey->KeyLength;
+                        NdisMoveMemory(pAdapter->StaCfg.DesireSharedKey[KeyIdx].Key, &pWepKey->KeyMaterial, pWepKey->KeyLength);
+                        pAdapter->StaCfg.DesireSharedKeyId = KeyIdx;
+                        pAdapter->StaCfg.DesireSharedKey[KeyIdx].CipherAlg = CipherAlg;
+#endif // WPA_SUPPLICANT_SUPPORT //
+                        pAdapter->StaCfg.DefaultKeyId = (UCHAR) KeyIdx;
+                    }
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+                    if (pAdapter->StaCfg.PortSecured == WPA_802_1X_PORT_SECURED)
+#endif // WPA_SUPPLICANT_SUPPORT
+                    {
+                        Key = pAdapter->SharedKey[BSS0][KeyIdx].Key;
+
+                        // Set key material and cipherAlg to Asic
+        				AsicAddSharedKeyEntry(pAdapter, BSS0, KeyIdx, CipherAlg, Key, NULL, NULL);
+
+                        if (pWepKey->KeyIndex & 0x80000000)
+                        {
+                            PMAC_TABLE_ENTRY pEntry = &pAdapter->MacTab.Content[BSSID_WCID];
+                            // Assign group key info
+    						RTMPAddWcidAttributeEntry(pAdapter, BSS0, KeyIdx, CipherAlg, NULL);
+    						// Assign pairwise key info
+    						RTMPAddWcidAttributeEntry(pAdapter, BSS0, KeyIdx, CipherAlg, pEntry);
+                        }
+                    }
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP (id=0x%x, Len=%d-byte), %s\n", pWepKey->KeyIndex, pWepKey->KeyLength, (pAdapter->StaCfg.PortSecured == WPA_802_1X_PORT_SECURED) ? "Port Secured":"Port NOT Secured"));
+				}
+            }
+            kfree(pWepKey);
+            break;
+#ifdef WPA_SUPPLICANT_SUPPORT
+	    case OID_SET_COUNTERMEASURES:
+            if (wrq->u.data.length != sizeof(int))
+                Status  = -EINVAL;
+            else
+            {
+                int enabled = 0;
+                Status = copy_from_user(&enabled, wrq->u.data.pointer, wrq->u.data.length);
+                if (enabled == 1)
+                    pAdapter->StaCfg.bBlockAssoc = TRUE;
+                else
+                    // WPA MIC error should block association attempt for 60 seconds
+                    pAdapter->StaCfg.bBlockAssoc = FALSE;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_SET_COUNTERMEASURES bBlockAssoc=%s\n", pAdapter->StaCfg.bBlockAssoc ? "TRUE":"FALSE"));
+            }
+	        break;
+        case RT_OID_WPA_SUPPLICANT_SUPPORT:
+			if (wrq->u.data.length != sizeof(UCHAR))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&wpa_supplicant_enable, wrq->u.data.pointer, wrq->u.data.length);
+    			pAdapter->StaCfg.WpaSupplicantUP = wpa_supplicant_enable;
+    			DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_WPA_SUPPLICANT_SUPPORT (=%d)\n", pAdapter->StaCfg.WpaSupplicantUP));
+			}
+            break;
+        case OID_802_11_DEAUTHENTICATION:
+            if (wrq->u.data.length != sizeof(MLME_DEAUTH_REQ_STRUCT))
+                Status  = -EINVAL;
+            else
+            {
+                MLME_DEAUTH_REQ_STRUCT      *pInfo;
+				MLME_QUEUE_ELEM *MsgElem = (MLME_QUEUE_ELEM *) kmalloc(sizeof(MLME_QUEUE_ELEM), MEM_ALLOC_FLAG);
+
+                pInfo = (MLME_DEAUTH_REQ_STRUCT *) MsgElem->Msg;
+                Status = copy_from_user(pInfo, wrq->u.data.pointer, wrq->u.data.length);
+                MlmeDeauthReqAction(pAdapter, MsgElem);
+				kfree(MsgElem);
+
+                if (INFRA_ON(pAdapter))
+                {
+                    LinkDown(pAdapter, FALSE);
+                    pAdapter->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+                }
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_DEAUTHENTICATION (Reason=%d)\n", pInfo->Reason));
+            }
+            break;
+        case OID_802_11_DROP_UNENCRYPTED:
+            if (wrq->u.data.length != sizeof(int))
+                Status  = -EINVAL;
+            else
+            {
+                int enabled = 0;
+                Status = copy_from_user(&enabled, wrq->u.data.pointer, wrq->u.data.length);
+                if (enabled == 1)
+                    pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+                else
+                    pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+				NdisAcquireSpinLock(&pAdapter->MacTabLock);
+				pAdapter->MacTab.Content[BSSID_WCID].PortSecured = pAdapter->StaCfg.PortSecured;
+				NdisReleaseSpinLock(&pAdapter->MacTabLock);
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_DROP_UNENCRYPTED (=%d)\n", enabled));
+            }
+            break;
+        case OID_802_11_SET_IEEE8021X:
+            if (wrq->u.data.length != sizeof(BOOLEAN))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&IEEE8021xState, wrq->u.data.pointer, wrq->u.data.length);
+		        pAdapter->StaCfg.IEEE8021X = IEEE8021xState;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_SET_IEEE8021X (=%d)\n", IEEE8021xState));
+            }
+            break;
+        case OID_802_11_SET_IEEE8021X_REQUIRE_KEY:
+			if (wrq->u.data.length != sizeof(BOOLEAN))
+				 Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&IEEE8021x_required_keys, wrq->u.data.pointer, wrq->u.data.length);
+				pAdapter->StaCfg.IEEE8021x_required_keys = IEEE8021x_required_keys;
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_SET_IEEE8021X_REQUIRE_KEY (%d)\n", IEEE8021x_required_keys));
+			}
+			break;
+        case OID_802_11_PMKID:
+	        pPmkId = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+
+	        if(pPmkId == NULL) {
+                Status = -ENOMEM;
+                break;
+            }
+            Status = copy_from_user(pPmkId, wrq->u.data.pointer, wrq->u.data.length);
+
+	        // check the PMKID information
+	        if (pPmkId->BSSIDInfoCount == 0)
+                NdisZeroMemory(pAdapter->StaCfg.SavedPMK, sizeof(BSSID_INFO)*PMKID_NO);
+	        else
+	        {
+		        PBSSID_INFO	pBssIdInfo;
+		        UINT		BssIdx;
+		        UINT		CachedIdx;
+
+		        for (BssIdx = 0; BssIdx < pPmkId->BSSIDInfoCount; BssIdx++)
+		        {
+			        // point to the indexed BSSID_INFO structure
+			        pBssIdInfo = (PBSSID_INFO) ((PUCHAR) pPmkId + 2 * sizeof(UINT) + BssIdx * sizeof(BSSID_INFO));
+			        // Find the entry in the saved data base.
+			        for (CachedIdx = 0; CachedIdx < pAdapter->StaCfg.SavedPMKNum; CachedIdx++)
+			        {
+				        // compare the BSSID
+				        if (NdisEqualMemory(pBssIdInfo->BSSID, pAdapter->StaCfg.SavedPMK[CachedIdx].BSSID, sizeof(NDIS_802_11_MAC_ADDRESS)))
+					        break;
+			        }
+
+			        // Found, replace it
+			        if (CachedIdx < PMKID_NO)
+			        {
+				        DBGPRINT(RT_DEBUG_OFF, ("Update OID_802_11_PMKID, idx = %d\n", CachedIdx));
+				        NdisMoveMemory(&pAdapter->StaCfg.SavedPMK[CachedIdx], pBssIdInfo, sizeof(BSSID_INFO));
+				        pAdapter->StaCfg.SavedPMKNum++;
+			        }
+			        // Not found, replace the last one
+			        else
+			        {
+				        // Randomly replace one
+				        CachedIdx = (pBssIdInfo->BSSID[5] % PMKID_NO);
+				        DBGPRINT(RT_DEBUG_OFF, ("Update OID_802_11_PMKID, idx = %d\n", CachedIdx));
+				        NdisMoveMemory(&pAdapter->StaCfg.SavedPMK[CachedIdx], pBssIdInfo, sizeof(BSSID_INFO));
+			        }
+		        }
+			}
+			if(pPmkId)
+				kfree(pPmkId);
+	        break;
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+
+
+#ifdef SNMP_SUPPORT
+		case OID_802_11_SHORTRETRYLIMIT:
+			if (wrq->u.data.length != sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&ShortRetryLimit, wrq->u.data.pointer, wrq->u.data.length);
+				RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+				tx_rty_cfg.field.ShortRtyLimit = ShortRetryLimit;
+				RTMP_IO_WRITE32(pAdapter, TX_RTY_CFG, tx_rty_cfg.word);
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_SHORTRETRYLIMIT (tx_rty_cfg.field.ShortRetryLimit=%d, ShortRetryLimit=%ld)\n", tx_rty_cfg.field.ShortRtyLimit, ShortRetryLimit));
+			}
+			break;
+
+		case OID_802_11_LONGRETRYLIMIT:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_LONGRETRYLIMIT \n"));
+			if (wrq->u.data.length != sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&LongRetryLimit, wrq->u.data.pointer, wrq->u.data.length);
+				RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+				tx_rty_cfg.field.LongRtyLimit = LongRetryLimit;
+				RTMP_IO_WRITE32(pAdapter, TX_RTY_CFG, tx_rty_cfg.word);
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_LONGRETRYLIMIT (tx_rty_cfg.field.LongRetryLimit= %d,LongRetryLimit=%ld)\n", tx_rty_cfg.field.LongRtyLimit, LongRetryLimit));
+			}
+			break;
+
+		case OID_802_11_WEPDEFAULTKEYVALUE:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_WEPDEFAULTKEYVALUE\n"));
+			pKey = kmalloc(wrq->u.data.length, GFP_KERNEL);
+			Status = copy_from_user(pKey, wrq->u.data.pointer, wrq->u.data.length);
+			//pKey = &WepKey;
+
+			if ( pKey->Length != wrq->u.data.length)
+			{
+				Status = -EINVAL;
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_WEPDEFAULTKEYVALUE, Failed!!\n"));
+			}
+			KeyIdx = pKey->KeyIndex & 0x0fffffff;
+			DBGPRINT(RT_DEBUG_TRACE,("pKey->KeyIndex =%d, pKey->KeyLength=%d\n", pKey->KeyIndex, pKey->KeyLength));
+
+			// it is a shared key
+			if (KeyIdx > 4)
+				Status = -EINVAL;
+			else
+			{
+				pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen = (UCHAR) pKey->KeyLength;
+				NdisMoveMemory(&pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].Key, &pKey->KeyMaterial, pKey->KeyLength);
+				if (pKey->KeyIndex & 0x80000000)
+				{
+					// Default key for tx (shared key)
+					pAdapter->StaCfg.DefaultKeyId = (UCHAR) KeyIdx;
+				}
+				//RestartAPIsRequired = TRUE;
+			}
+			break;
+
+
+		case OID_802_11_WEPDEFAULTKEYID:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_WEPDEFAULTKEYID \n"));
+
+			if (wrq->u.data.length != sizeof(UCHAR))
+				Status = -EINVAL;
+			else
+				Status = copy_from_user(&pAdapter->StaCfg.DefaultKeyId, wrq->u.data.pointer, wrq->u.data.length);
+
+			break;
+
+
+		case OID_802_11_CURRENTCHANNEL:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_CURRENTCHANNEL \n"));
+			if (wrq->u.data.length != sizeof(UCHAR))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&ctmp, wrq->u.data.pointer, wrq->u.data.length);
+				sprintf(&ctmp,"%d", ctmp);
+				Set_Channel_Proc(pAdapter, &ctmp);
+			}
+			break;
+#endif
+
+
+
+        default:
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::unknown IOCTL's subcmd = 0x%08x\n", cmd));
+            Status = -EOPNOTSUPP;
+            break;
+    }
+
+
+    return Status;
+}
+
+INT RTMPQueryInformation(
+    IN  PRTMP_ADAPTER pAdapter,
+    IN  OUT struct ifreq    *rq,
+    IN  INT                 cmd)
+{
+    struct iwreq                        *wrq = (struct iwreq *) rq;
+    NDIS_802_11_BSSID_LIST_EX           *pBssidList = NULL;
+    PNDIS_WLAN_BSSID_EX                 pBss;
+    NDIS_802_11_SSID                    Ssid;
+    NDIS_802_11_CONFIGURATION           *pConfiguration = NULL;
+    RT_802_11_LINK_STATUS               *pLinkStatus = NULL;
+    RT_802_11_STA_CONFIG                *pStaConfig = NULL;
+    NDIS_802_11_STATISTICS              *pStatistics = NULL;
+    NDIS_802_11_RTS_THRESHOLD           RtsThresh;
+    NDIS_802_11_FRAGMENTATION_THRESHOLD FragThresh;
+    NDIS_802_11_POWER_MODE              PowerMode;
+    NDIS_802_11_NETWORK_INFRASTRUCTURE  BssType;
+    RT_802_11_PREAMBLE                  PreamType;
+    NDIS_802_11_AUTHENTICATION_MODE     AuthMode;
+    NDIS_802_11_WEP_STATUS              WepStatus;
+    NDIS_MEDIA_STATE                    MediaState;
+    ULONG                               BssBufSize, ulInfo=0, NetworkTypeList[4], apsd = 0;
+    USHORT                              BssLen = 0;
+    PUCHAR                              pBuf = NULL, pPtr;
+    INT                                 Status = NDIS_STATUS_SUCCESS;
+    UINT                                we_version_compiled;
+    UCHAR                               i, Padding = 0;
+    BOOLEAN                             RadioState;
+	UCHAR	driverVersion[8];
+    OID_SET_HT_PHYMODE			        *pHTPhyMode = NULL;
+
+
+#ifdef SNMP_SUPPORT
+	//for snmp, kathy
+	DefaultKeyIdxValue			*pKeyIdxValue;
+	INT							valueLen;
+	TX_RTY_CFG_STRUC			tx_rty_cfg;
+	ULONG						ShortRetryLimit, LongRetryLimit;
+	UCHAR						tmp[64];
+#endif //SNMP
+
+    switch(cmd)
+    {
+        case RT_OID_DEVICE_NAME:
+            wrq->u.data.length = sizeof(STA_NIC_DEVICE_NAME);
+            Status = copy_to_user(wrq->u.data.pointer, STA_NIC_DEVICE_NAME, wrq->u.data.length);
+            break;
+        case RT_OID_VERSION_INFO:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_VERSION_INFO \n"));
+			wrq->u.data.length = 8*sizeof(UCHAR);
+			sprintf(&driverVersion[0], "%s", STA_DRIVER_VERSION);
+			driverVersion[7] = '\0';
+			if (copy_to_user(wrq->u.data.pointer, &driverVersion, wrq->u.data.length))
+            {
+				Status = -EFAULT;
+            }
+            break;
+#ifdef RALINK_ATE
+		case RT_QUERY_ATE_TXDONE_COUNT:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_QUERY_ATE_TXDONE_COUNT \n"));
+			wrq->u.data.length = sizeof(UINT32);
+			if (copy_to_user(wrq->u.data.pointer, &pAdapter->ate.TxDoneCount, wrq->u.data.length))
+			{
+				Status = -EFAULT;
+			}
+			break;
+#endif // RALINK_ATE //
+        case OID_802_11_BSSID_LIST:
+            if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+            {
+            	/*
+            	 * Still scanning, indicate the caller should try again.
+            	 */
+            	DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_BSSID_LIST (Still scanning)\n"));
+				return -EAGAIN;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_BSSID_LIST (%d BSS returned)\n",pAdapter->ScanTab.BssNr));
+			pAdapter->StaCfg.bScanReqIsFromWebUI = FALSE;
+            // Claculate total buffer size required
+            BssBufSize = sizeof(ULONG);
+
+            for (i = 0; i < pAdapter->ScanTab.BssNr; i++)
+            {
+                // Align pointer to 4 bytes boundary.
+                //Padding = 4 - (pAdapter->ScanTab.BssEntry[i].VarIELen & 0x0003);
+                //if (Padding == 4)
+                //    Padding = 0;
+                BssBufSize += (sizeof(NDIS_WLAN_BSSID_EX) - 1 + sizeof(NDIS_802_11_FIXED_IEs) + pAdapter->ScanTab.BssEntry[i].VarIELen + Padding);
+            }
+
+            // For safety issue, we add 256 bytes just in case
+            BssBufSize += 256;
+            // Allocate the same size as passed from higher layer
+            pBuf = kmalloc(BssBufSize, MEM_ALLOC_FLAG);
+            if(pBuf == NULL)
+            {
+                Status = -ENOMEM;
+                break;
+            }
+            // Init 802_11_BSSID_LIST_EX structure
+            NdisZeroMemory(pBuf, BssBufSize);
+            pBssidList = (PNDIS_802_11_BSSID_LIST_EX) pBuf;
+            pBssidList->NumberOfItems = pAdapter->ScanTab.BssNr;
+
+            // Calculate total buffer length
+            BssLen = 4; // Consist of NumberOfItems
+            // Point to start of NDIS_WLAN_BSSID_EX
+            // pPtr = pBuf + sizeof(ULONG);
+            pPtr = (PUCHAR) &pBssidList->Bssid[0];
+            for (i = 0; i < pAdapter->ScanTab.BssNr; i++)
+            {
+                pBss = (PNDIS_WLAN_BSSID_EX) pPtr;
+                NdisMoveMemory(&pBss->MacAddress, &pAdapter->ScanTab.BssEntry[i].Bssid, MAC_ADDR_LEN);
+                if ((pAdapter->ScanTab.BssEntry[i].Hidden == 1) && (pAdapter->StaCfg.bShowHiddenSSID == FALSE))
+                {
+                    //
+					// We must return this SSID during 4way handshaking, otherwise Aegis will failed to parse WPA infomation
+					// and then failed to send EAPOl farame.
+					//
+					if ((pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA) && (pAdapter->StaCfg.PortSecured != WPA_802_1X_PORT_SECURED))
+					{
+						pBss->Ssid.SsidLength = pAdapter->ScanTab.BssEntry[i].SsidLen;
+						NdisMoveMemory(pBss->Ssid.Ssid, pAdapter->ScanTab.BssEntry[i].Ssid, pAdapter->ScanTab.BssEntry[i].SsidLen);
+					}
+					else
+                    	pBss->Ssid.SsidLength = 0;
+                }
+                else
+                {
+                    pBss->Ssid.SsidLength = pAdapter->ScanTab.BssEntry[i].SsidLen;
+                    NdisMoveMemory(pBss->Ssid.Ssid, pAdapter->ScanTab.BssEntry[i].Ssid, pAdapter->ScanTab.BssEntry[i].SsidLen);
+                }
+                pBss->Privacy = pAdapter->ScanTab.BssEntry[i].Privacy;
+                pBss->Rssi = pAdapter->ScanTab.BssEntry[i].Rssi - pAdapter->BbpRssiToDbmDelta;
+                pBss->NetworkTypeInUse = NetworkTypeInUseSanity(&pAdapter->ScanTab.BssEntry[i]);
+                pBss->Configuration.Length = sizeof(NDIS_802_11_CONFIGURATION);
+                pBss->Configuration.BeaconPeriod = pAdapter->ScanTab.BssEntry[i].BeaconPeriod;
+                pBss->Configuration.ATIMWindow = pAdapter->ScanTab.BssEntry[i].AtimWin;
+
+                MAP_CHANNEL_ID_TO_KHZ(pAdapter->ScanTab.BssEntry[i].Channel, pBss->Configuration.DSConfig);
+
+                if (pAdapter->ScanTab.BssEntry[i].BssType == BSS_INFRA)
+                    pBss->InfrastructureMode = Ndis802_11Infrastructure;
+                else
+                    pBss->InfrastructureMode = Ndis802_11IBSS;
+
+                NdisMoveMemory(pBss->SupportedRates, pAdapter->ScanTab.BssEntry[i].SupRate, pAdapter->ScanTab.BssEntry[i].SupRateLen);
+                NdisMoveMemory(pBss->SupportedRates + pAdapter->ScanTab.BssEntry[i].SupRateLen,
+                               pAdapter->ScanTab.BssEntry[i].ExtRate,
+                               pAdapter->ScanTab.BssEntry[i].ExtRateLen);
+
+                if (pAdapter->ScanTab.BssEntry[i].VarIELen == 0)
+                {
+                    pBss->IELength = sizeof(NDIS_802_11_FIXED_IEs);
+                    NdisMoveMemory(pBss->IEs, &pAdapter->ScanTab.BssEntry[i].FixIEs, sizeof(NDIS_802_11_FIXED_IEs));
+                    pPtr = pPtr + sizeof(NDIS_WLAN_BSSID_EX) - 1 + sizeof(NDIS_802_11_FIXED_IEs);
+                }
+                else
+                {
+                    pBss->IELength = (ULONG)(sizeof(NDIS_802_11_FIXED_IEs) + pAdapter->ScanTab.BssEntry[i].VarIELen);
+                    pPtr = pPtr + sizeof(NDIS_WLAN_BSSID_EX) - 1 + sizeof(NDIS_802_11_FIXED_IEs);
+                    NdisMoveMemory(pBss->IEs, &pAdapter->ScanTab.BssEntry[i].FixIEs, sizeof(NDIS_802_11_FIXED_IEs));
+                    NdisMoveMemory(pBss->IEs + sizeof(NDIS_802_11_FIXED_IEs), pAdapter->ScanTab.BssEntry[i].VarIEs, pAdapter->ScanTab.BssEntry[i].VarIELen);
+                    pPtr += pAdapter->ScanTab.BssEntry[i].VarIELen;
+                }
+                pBss->Length = (ULONG)(sizeof(NDIS_WLAN_BSSID_EX) - 1 + sizeof(NDIS_802_11_FIXED_IEs) + pAdapter->ScanTab.BssEntry[i].VarIELen + Padding);
+
+#if WIRELESS_EXT < 17
+                if ((BssLen + pBss->Length) < wrq->u.data.length)
+                BssLen += pBss->Length;
+                else
+                {
+                    pBssidList->NumberOfItems = i;
+                    break;
+                }
+#else
+                BssLen += pBss->Length;
+#endif
+            }
+
+#if WIRELESS_EXT < 17
+            wrq->u.data.length = BssLen;
+#else
+            if (BssLen > wrq->u.data.length)
+            {
+                kfree(pBssidList);
+                return -E2BIG;
+            }
+            else
+                wrq->u.data.length = BssLen;
+#endif
+            Status = copy_to_user(wrq->u.data.pointer, pBssidList, BssLen);
+            kfree(pBssidList);
+            break;
+        case OID_802_3_CURRENT_ADDRESS:
+            wrq->u.data.length = MAC_ADDR_LEN;
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CurrentAddress, wrq->u.data.length);
+            break;
+        case OID_GEN_MEDIA_CONNECT_STATUS:
+            if (pAdapter->IndicateMediaState == NdisMediaStateConnected)
+                MediaState = NdisMediaStateConnected;
+            else
+                MediaState = NdisMediaStateDisconnected;
+
+            wrq->u.data.length = sizeof(NDIS_MEDIA_STATE);
+            Status = copy_to_user(wrq->u.data.pointer, &MediaState, wrq->u.data.length);
+            break;
+        case OID_802_11_BSSID:
+#ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+				Status = NDIS_STATUS_RESOURCES;
+				break;
+			}
+#endif // RALINK_ATE //
+            if (INFRA_ON(pAdapter) || ADHOC_ON(pAdapter))
+            {
+                Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.Bssid, sizeof(NDIS_802_11_MAC_ADDRESS));
+
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_BSSID(=EMPTY)\n"));
+                Status = -ENOTCONN;
+            }
+            break;
+        case OID_802_11_SSID:
+			NdisZeroMemory(&Ssid, sizeof(NDIS_802_11_SSID));
+			NdisZeroMemory(Ssid.Ssid, MAX_LEN_OF_SSID);
+            Ssid.SsidLength = pAdapter->CommonCfg.SsidLen;
+			memcpy(Ssid.Ssid, pAdapter->CommonCfg.Ssid,	Ssid.SsidLength);
+            wrq->u.data.length = sizeof(NDIS_802_11_SSID);
+            Status = copy_to_user(wrq->u.data.pointer, &Ssid, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_SSID (Len=%d, ssid=%s)\n", Ssid.SsidLength,Ssid.Ssid));
+            break;
+        case RT_OID_802_11_QUERY_LINK_STATUS:
+            pLinkStatus = (RT_802_11_LINK_STATUS *) kmalloc(sizeof(RT_802_11_LINK_STATUS), MEM_ALLOC_FLAG);
+            if (pLinkStatus)
+            {
+                pLinkStatus->CurrTxRate = RateIdTo500Kbps[pAdapter->CommonCfg.TxRate];   // unit : 500 kbps
+                pLinkStatus->ChannelQuality = pAdapter->Mlme.ChannelQuality;
+                pLinkStatus->RxByteCount = pAdapter->RalinkCounters.ReceivedByteCount;
+                pLinkStatus->TxByteCount = pAdapter->RalinkCounters.TransmittedByteCount;
+        		pLinkStatus->CentralChannel = pAdapter->CommonCfg.CentralChannel;
+                wrq->u.data.length = sizeof(RT_802_11_LINK_STATUS);
+                Status = copy_to_user(wrq->u.data.pointer, pLinkStatus, wrq->u.data.length);
+                kfree(pLinkStatus);
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_LINK_STATUS\n"));
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_LINK_STATUS(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case OID_802_11_CONFIGURATION:
+            pConfiguration = (NDIS_802_11_CONFIGURATION *) kmalloc(sizeof(NDIS_802_11_CONFIGURATION), MEM_ALLOC_FLAG);
+            if (pConfiguration)
+            {
+                pConfiguration->Length = sizeof(NDIS_802_11_CONFIGURATION);
+                pConfiguration->BeaconPeriod = pAdapter->CommonCfg.BeaconPeriod;
+                pConfiguration->ATIMWindow = pAdapter->StaActive.AtimWin;
+                MAP_CHANNEL_ID_TO_KHZ(pAdapter->CommonCfg.Channel, pConfiguration->DSConfig);
+                wrq->u.data.length = sizeof(NDIS_802_11_CONFIGURATION);
+                Status = copy_to_user(wrq->u.data.pointer, pConfiguration, wrq->u.data.length);
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_CONFIGURATION(BeaconPeriod=%ld,AtimW=%ld,Channel=%d) \n",
+                                        pConfiguration->BeaconPeriod, pConfiguration->ATIMWindow, pAdapter->CommonCfg.Channel));
+				kfree(pConfiguration);
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_CONFIGURATION(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+		case RT_OID_802_11_SNR_0:
+			if ((pAdapter->StaCfg.LastSNR0 > 0))
+			{
+				ulInfo = ((0xeb	- pAdapter->StaCfg.LastSNR0) * 3) /	16 ;
+				wrq->u.data.length = sizeof(ulInfo);
+				Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_SNR_0(0x=%lx)\n", ulInfo));
+			}
+            else
+			    Status = -EFAULT;
+			break;
+		case RT_OID_802_11_SNR_1:
+			if ((pAdapter->Antenna.field.RxPath	> 1) &&
+                (pAdapter->StaCfg.LastSNR1 > 0))
+			{
+				ulInfo = ((0xeb	- pAdapter->StaCfg.LastSNR1) * 3) /	16 ;
+				wrq->u.data.length = sizeof(ulInfo);
+				Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE,("Query::RT_OID_802_11_SNR_1(0x=%lx)\n",ulInfo));
+			}
+			else
+				Status = -EFAULT;
+            DBGPRINT(RT_DEBUG_TRACE,("Query::RT_OID_802_11_SNR_1(pAdapter->StaCfg.LastSNR1=%d)\n",pAdapter->StaCfg.LastSNR1));
+			break;
+        case OID_802_11_RSSI_TRIGGER:
+            ulInfo = pAdapter->StaCfg.RssiSample.LastRssi0 - pAdapter->BbpRssiToDbmDelta;
+            wrq->u.data.length = sizeof(ulInfo);
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_RSSI_TRIGGER(=%ld)\n", ulInfo));
+            break;
+		case OID_802_11_RSSI:
+        case RT_OID_802_11_RSSI:
+			ulInfo = pAdapter->StaCfg.RssiSample.LastRssi0;
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+			break;
+		case RT_OID_802_11_RSSI_1:
+            ulInfo = pAdapter->StaCfg.RssiSample.LastRssi1;
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+			break;
+        case RT_OID_802_11_RSSI_2:
+            ulInfo = pAdapter->StaCfg.RssiSample.LastRssi2;
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+			break;
+        case OID_802_11_STATISTICS:
+            pStatistics = (NDIS_802_11_STATISTICS *) kmalloc(sizeof(NDIS_802_11_STATISTICS), MEM_ALLOC_FLAG);
+            if (pStatistics)
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_STATISTICS \n"));
+                // add the most up-to-date h/w raw counters into software counters
+			    NICUpdateRawCounters(pAdapter);
+
+                // Sanity check for calculation of sucessful count
+                if (pAdapter->WlanCounters.TransmittedFragmentCount.QuadPart < pAdapter->WlanCounters.RetryCount.QuadPart)
+                    pAdapter->WlanCounters.TransmittedFragmentCount.QuadPart = pAdapter->WlanCounters.RetryCount.QuadPart;
+
+                pStatistics->TransmittedFragmentCount.QuadPart = pAdapter->WlanCounters.TransmittedFragmentCount.QuadPart;
+                pStatistics->MulticastTransmittedFrameCount.QuadPart = pAdapter->WlanCounters.MulticastTransmittedFrameCount.QuadPart;
+                pStatistics->FailedCount.QuadPart = pAdapter->WlanCounters.FailedCount.QuadPart;
+                pStatistics->RetryCount.QuadPart = pAdapter->WlanCounters.RetryCount.QuadPart;
+                pStatistics->MultipleRetryCount.QuadPart = pAdapter->WlanCounters.MultipleRetryCount.QuadPart;
+                pStatistics->RTSSuccessCount.QuadPart = pAdapter->WlanCounters.RTSSuccessCount.QuadPart;
+                pStatistics->RTSFailureCount.QuadPart = pAdapter->WlanCounters.RTSFailureCount.QuadPart;
+                pStatistics->ACKFailureCount.QuadPart = pAdapter->WlanCounters.ACKFailureCount.QuadPart;
+                pStatistics->FrameDuplicateCount.QuadPart = pAdapter->WlanCounters.FrameDuplicateCount.QuadPart;
+                pStatistics->ReceivedFragmentCount.QuadPart = pAdapter->WlanCounters.ReceivedFragmentCount.QuadPart;
+                pStatistics->MulticastReceivedFrameCount.QuadPart = pAdapter->WlanCounters.MulticastReceivedFrameCount.QuadPart;
+#ifdef DBG
+                pStatistics->FCSErrorCount = pAdapter->RalinkCounters.RealFcsErrCount;
+#else
+                pStatistics->FCSErrorCount.QuadPart = pAdapter->WlanCounters.FCSErrorCount.QuadPart;
+                pStatistics->FrameDuplicateCount.u.LowPart = pAdapter->WlanCounters.FrameDuplicateCount.u.LowPart / 100;
+#endif
+                wrq->u.data.length = sizeof(NDIS_802_11_STATISTICS);
+                Status = copy_to_user(wrq->u.data.pointer, pStatistics, wrq->u.data.length);
+                kfree(pStatistics);
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_STATISTICS(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case OID_GEN_RCV_OK:
+            ulInfo = pAdapter->Counters8023.GoodReceives;
+            wrq->u.data.length = sizeof(ulInfo);
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+            break;
+        case OID_GEN_RCV_NO_BUFFER:
+            ulInfo = pAdapter->Counters8023.RxNoBuffer;
+            wrq->u.data.length = sizeof(ulInfo);
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+            break;
+        case RT_OID_802_11_PHY_MODE:
+            ulInfo = (ULONG)pAdapter->CommonCfg.PhyMode;
+            wrq->u.data.length = sizeof(ulInfo);
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_PHY_MODE (=%ld)\n", ulInfo));
+            break;
+        case RT_OID_802_11_STA_CONFIG:
+            pStaConfig = (RT_802_11_STA_CONFIG *) kmalloc(sizeof(RT_802_11_STA_CONFIG), MEM_ALLOC_FLAG);
+            if (pStaConfig)
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_STA_CONFIG\n"));
+                pStaConfig->EnableTxBurst = pAdapter->CommonCfg.bEnableTxBurst;
+                pStaConfig->EnableTurboRate = 0;
+                pStaConfig->UseBGProtection = pAdapter->CommonCfg.UseBGProtection;
+                pStaConfig->UseShortSlotTime = pAdapter->CommonCfg.bUseShortSlotTime;
+                //pStaConfig->AdhocMode = pAdapter->StaCfg.AdhocMode;
+                pStaConfig->HwRadioStatus = (pAdapter->StaCfg.bHwRadio == TRUE) ? 1 : 0;
+                pStaConfig->Rsv1 = 0;
+                pStaConfig->SystemErrorBitmap = pAdapter->SystemErrorBitmap;
+                wrq->u.data.length = sizeof(RT_802_11_STA_CONFIG);
+                Status = copy_to_user(wrq->u.data.pointer, pStaConfig, wrq->u.data.length);
+                kfree(pStaConfig);
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_STA_CONFIG(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case OID_802_11_RTS_THRESHOLD:
+            RtsThresh = pAdapter->CommonCfg.RtsThreshold;
+            wrq->u.data.length = sizeof(RtsThresh);
+            Status = copy_to_user(wrq->u.data.pointer, &RtsThresh, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_RTS_THRESHOLD(=%ld)\n", RtsThresh));
+            break;
+        case OID_802_11_FRAGMENTATION_THRESHOLD:
+            FragThresh = pAdapter->CommonCfg.FragmentThreshold;
+            if (pAdapter->CommonCfg.bUseZeroToDisableFragment == TRUE)
+                FragThresh = 0;
+            wrq->u.data.length = sizeof(FragThresh);
+            Status = copy_to_user(wrq->u.data.pointer, &FragThresh, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_FRAGMENTATION_THRESHOLD(=%ld)\n", FragThresh));
+            break;
+        case OID_802_11_POWER_MODE:
+            PowerMode = pAdapter->StaCfg.WindowsPowerMode;
+            wrq->u.data.length = sizeof(PowerMode);
+            Status = copy_to_user(wrq->u.data.pointer, &PowerMode, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_POWER_MODE(=%d)\n", PowerMode));
+            break;
+        case RT_OID_802_11_RADIO:
+            RadioState = (BOOLEAN) pAdapter->StaCfg.bSwRadio;
+            wrq->u.data.length = sizeof(RadioState);
+            Status = copy_to_user(wrq->u.data.pointer, &RadioState, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_RADIO (=%d)\n", RadioState));
+            break;
+        case OID_802_11_INFRASTRUCTURE_MODE:
+            if (pAdapter->StaCfg.BssType == BSS_ADHOC)
+                BssType = Ndis802_11IBSS;
+            else if (pAdapter->StaCfg.BssType == BSS_INFRA)
+                BssType = Ndis802_11Infrastructure;
+            else if (pAdapter->StaCfg.BssType == BSS_MONITOR)
+                BssType = Ndis802_11Monitor;
+            else
+                BssType = Ndis802_11AutoUnknown;
+
+            wrq->u.data.length = sizeof(BssType);
+            Status = copy_to_user(wrq->u.data.pointer, &BssType, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_INFRASTRUCTURE_MODE(=%d)\n", BssType));
+            break;
+        case RT_OID_802_11_PREAMBLE:
+            PreamType = pAdapter->CommonCfg.TxPreamble;
+            wrq->u.data.length = sizeof(PreamType);
+            Status = copy_to_user(wrq->u.data.pointer, &PreamType, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_PREAMBLE(=%d)\n", PreamType));
+            break;
+        case OID_802_11_AUTHENTICATION_MODE:
+            AuthMode = pAdapter->StaCfg.AuthMode;
+            wrq->u.data.length = sizeof(AuthMode);
+            Status = copy_to_user(wrq->u.data.pointer, &AuthMode, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_AUTHENTICATION_MODE(=%d)\n", AuthMode));
+            break;
+        case OID_802_11_WEP_STATUS:
+            WepStatus = pAdapter->StaCfg.WepStatus;
+            wrq->u.data.length = sizeof(WepStatus);
+            Status = copy_to_user(wrq->u.data.pointer, &WepStatus, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_WEP_STATUS(=%d)\n", WepStatus));
+            break;
+        case OID_802_11_TX_POWER_LEVEL:
+			wrq->u.data.length = sizeof(ULONG);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.TxPower, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_TX_POWER_LEVEL %x\n",pAdapter->CommonCfg.TxPower));
+			break;
+        case RT_OID_802_11_TX_POWER_LEVEL_1:
+            wrq->u.data.length = sizeof(ULONG);
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.TxPowerPercentage, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_TX_POWER_LEVEL_1 (=%ld)\n", pAdapter->CommonCfg.TxPowerPercentage));
+			break;
+        case OID_802_11_NETWORK_TYPES_SUPPORTED:
+			if ((pAdapter->RfIcType	== RFIC_2850) || (pAdapter->RfIcType ==	RFIC_2750))
+			{
+				NetworkTypeList[0] = 3;                 // NumberOfItems = 3
+				NetworkTypeList[1] = Ndis802_11DS;      // NetworkType[1] = 11b
+				NetworkTypeList[2] = Ndis802_11OFDM24;  // NetworkType[2] = 11g
+				NetworkTypeList[3] = Ndis802_11OFDM5;   // NetworkType[3] = 11a
+                wrq->u.data.length = 16;
+				Status = copy_to_user(wrq->u.data.pointer, &NetworkTypeList[0], wrq->u.data.length);
+			}
+			else
+			{
+				NetworkTypeList[0] = 2;                 // NumberOfItems = 2
+				NetworkTypeList[1] = Ndis802_11DS;      // NetworkType[1] = 11b
+				NetworkTypeList[2] = Ndis802_11OFDM24;  // NetworkType[2] = 11g
+			    wrq->u.data.length = 12;
+				Status = copy_to_user(wrq->u.data.pointer, &NetworkTypeList[0], wrq->u.data.length);
+			}
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_NETWORK_TYPES_SUPPORTED\n"));
+				break;
+	    case OID_802_11_NETWORK_TYPE_IN_USE:
+            wrq->u.data.length = sizeof(ULONG);
+			if (pAdapter->CommonCfg.PhyMode == PHY_11A)
+				ulInfo = Ndis802_11OFDM5;
+			else if ((pAdapter->CommonCfg.PhyMode == PHY_11BG_MIXED) || (pAdapter->CommonCfg.PhyMode == PHY_11G))
+				ulInfo = Ndis802_11OFDM24;
+			else
+				ulInfo = Ndis802_11DS;
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+			break;
+        case RT_OID_802_11_QUERY_LAST_RX_RATE:
+            ulInfo = (ULONG)pAdapter->LastRxRate;
+            wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_LAST_RX_RATE (=%ld)\n", ulInfo));
+			break;
+		case RT_OID_802_11_QUERY_LAST_TX_RATE:
+			//ulInfo = (ULONG)pAdapter->LastTxRate;
+			ulInfo = (ULONG)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.word;
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_LAST_TX_RATE (=%lx)\n", ulInfo));
+			break;
+        case RT_OID_802_11_QUERY_EEPROM_VERSION:
+            wrq->u.data.length = sizeof(ULONG);
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->EepromVersion, wrq->u.data.length);
+            break;
+        case RT_OID_802_11_QUERY_FIRMWARE_VERSION:
+            wrq->u.data.length = sizeof(ULONG);
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->FirmwareVersion, wrq->u.data.length);
+			break;
+	    case RT_OID_802_11_QUERY_NOISE_LEVEL:
+			wrq->u.data.length = sizeof(UCHAR);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->BbpWriteLatch[66], wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_NOISE_LEVEL (=%d)\n", pAdapter->BbpWriteLatch[66]));
+			break;
+	    case RT_OID_802_11_EXTRA_INFO:
+			wrq->u.data.length = sizeof(ULONG);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->ExtraInfo, wrq->u.data.length);
+	        DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_EXTRA_INFO (=%ld)\n", pAdapter->ExtraInfo));
+	        break;
+	    case RT_OID_WE_VERSION_COMPILED:
+	        wrq->u.data.length = sizeof(UINT);
+	        we_version_compiled = WIRELESS_EXT;
+	        Status = copy_to_user(wrq->u.data.pointer, &we_version_compiled, wrq->u.data.length);
+	        break;
+		case RT_OID_802_11_QUERY_APSD_SETTING:
+			apsd = (pAdapter->CommonCfg.bAPSDCapable | (pAdapter->CommonCfg.bAPSDAC_BE << 1) | (pAdapter->CommonCfg.bAPSDAC_BK << 2)
+				| (pAdapter->CommonCfg.bAPSDAC_VI << 3)	| (pAdapter->CommonCfg.bAPSDAC_VO << 4)	| (pAdapter->CommonCfg.MaxSPLength << 5));
+
+			wrq->u.data.length = sizeof(ULONG);
+			Status = copy_to_user(wrq->u.data.pointer, &apsd, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_APSD_SETTING (=0x%lx,APSDCap=%d,AC_BE=%d,AC_BK=%d,AC_VI=%d,AC_VO=%d,MAXSPLen=%d)\n",
+				apsd,pAdapter->CommonCfg.bAPSDCapable,pAdapter->CommonCfg.bAPSDAC_BE,pAdapter->CommonCfg.bAPSDAC_BK,pAdapter->CommonCfg.bAPSDAC_VI,pAdapter->CommonCfg.bAPSDAC_VO,pAdapter->CommonCfg.MaxSPLength));
+			break;
+		case RT_OID_802_11_QUERY_APSD_PSM:
+			wrq->u.data.length = sizeof(ULONG);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.bAPSDForcePowerSave, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_APSD_PSM (=%d)\n", pAdapter->CommonCfg.bAPSDForcePowerSave));
+			break;
+		case RT_OID_802_11_QUERY_WMM:
+			wrq->u.data.length = sizeof(BOOLEAN);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.bWmmCapable, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_WMM (=%d)\n",	pAdapter->CommonCfg.bWmmCapable));
+			break;
+#ifdef WPA_SUPPLICANT_SUPPORT
+        case RT_OID_NEW_DRIVER:
+            {
+                UCHAR enabled = 1;
+    	        wrq->u.data.length = sizeof(UCHAR);
+    	        Status = copy_to_user(wrq->u.data.pointer, &enabled, wrq->u.data.length);
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_NEW_DRIVER (=%d)\n", enabled));
+            }
+	        break;
+        case RT_OID_WPA_SUPPLICANT_SUPPORT:
+	        wrq->u.data.length = sizeof(UCHAR);
+	        Status = copy_to_user(wrq->u.data.pointer, &pAdapter->StaCfg.WpaSupplicantUP, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_WPA_SUPPLICANT_SUPPORT (=%d)\n", pAdapter->StaCfg.WpaSupplicantUP));
+	        break;
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+        case RT_OID_DRIVER_DEVICE_NAME:
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_DRIVER_DEVICE_NAME \n"));
+			wrq->u.data.length = 16;
+			if (copy_to_user(wrq->u.data.pointer, pAdapter->StaCfg.dev_name, wrq->u.data.length))
+			{
+				Status = -EFAULT;
+			}
+            break;
+        case RT_OID_802_11_QUERY_HT_PHYMODE:
+            pHTPhyMode = (OID_SET_HT_PHYMODE *) kmalloc(sizeof(OID_SET_HT_PHYMODE), MEM_ALLOC_FLAG);
+            if (pHTPhyMode)
+            {
+                pHTPhyMode->PhyMode = pAdapter->CommonCfg.PhyMode;
+    			pHTPhyMode->HtMode = (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE;
+    			pHTPhyMode->BW = (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.BW;
+    			pHTPhyMode->MCS= (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.MCS;
+    			pHTPhyMode->SHORTGI= (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.ShortGI;
+    			pHTPhyMode->STBC= (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.STBC;
+
+    			pHTPhyMode->ExtOffset = ((pAdapter->CommonCfg.CentralChannel < pAdapter->CommonCfg.Channel) ? (EXTCHA_BELOW) : (EXTCHA_ABOVE));
+                wrq->u.data.length = sizeof(OID_SET_HT_PHYMODE);
+                if (copy_to_user(wrq->u.data.pointer, pHTPhyMode, wrq->u.data.length))
+    			{
+    				Status = -EFAULT;
+    			}
+    			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_HT_PHYMODE (PhyMode = %d, MCS =%d, BW = %d, STBC = %d, ExtOffset=%d)\n",
+    				pHTPhyMode->HtMode, pHTPhyMode->MCS, pHTPhyMode->BW, pHTPhyMode->STBC, pHTPhyMode->ExtOffset));
+    			DBGPRINT(RT_DEBUG_TRACE, (" MlmeUpdateTxRates (.word = %x )\n", pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.word));
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_STA_CONFIG(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case RT_OID_802_11_COUNTRY_REGION:
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_COUNTRY_REGION \n"));
+			wrq->u.data.length = sizeof(ulInfo);
+            ulInfo = pAdapter->CommonCfg.CountryRegionForABand;
+            ulInfo = (ulInfo << 8)|(pAdapter->CommonCfg.CountryRegion);
+			if (copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length))
+            {
+				Status = -EFAULT;
+            }
+            break;
+        case RT_OID_802_11_QUERY_DAT_HT_PHYMODE:
+            pHTPhyMode = (OID_SET_HT_PHYMODE *) kmalloc(sizeof(OID_SET_HT_PHYMODE), MEM_ALLOC_FLAG);
+            if (pHTPhyMode)
+            {
+                pHTPhyMode->PhyMode = pAdapter->CommonCfg.PhyMode;
+    			pHTPhyMode->HtMode = (UCHAR)pAdapter->CommonCfg.RegTransmitSetting.field.HTMODE;
+    			pHTPhyMode->BW = (UCHAR)pAdapter->CommonCfg.RegTransmitSetting.field.BW;
+    			pHTPhyMode->MCS= (UCHAR)pAdapter->StaCfg.DesiredTransmitSetting.field.MCS;
+    			pHTPhyMode->SHORTGI= (UCHAR)pAdapter->CommonCfg.RegTransmitSetting.field.ShortGI;
+    			pHTPhyMode->STBC= (UCHAR)pAdapter->CommonCfg.RegTransmitSetting.field.STBC;
+
+                wrq->u.data.length = sizeof(OID_SET_HT_PHYMODE);
+                if (copy_to_user(wrq->u.data.pointer, pHTPhyMode, wrq->u.data.length))
+    			{
+    				Status = -EFAULT;
+    			}
+    			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_HT_PHYMODE (PhyMode = %d, MCS =%d, BW = %d, STBC = %d, ExtOffset=%d)\n",
+    				pHTPhyMode->HtMode, pHTPhyMode->MCS, pHTPhyMode->BW, pHTPhyMode->STBC, pHTPhyMode->ExtOffset));
+    			DBGPRINT(RT_DEBUG_TRACE, (" MlmeUpdateTxRates (.word = %x )\n", pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.word));
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_STA_CONFIG(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case RT_OID_QUERY_MULTIPLE_CARD_SUPPORT:
+			wrq->u.data.length = sizeof(UCHAR);
+            i = 0;
+#ifdef MULTIPLE_CARD_SUPPORT
+            i = 1;
+#endif // MULTIPLE_CARD_SUPPORT //
+			if (copy_to_user(wrq->u.data.pointer, &i, wrq->u.data.length))
+            {
+				Status = -EFAULT;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_QUERY_MULTIPLE_CARD_SUPPORT(=%d) \n", i));
+            break;
+#ifdef SNMP_SUPPORT
+		case RT_OID_802_11_MAC_ADDRESS:
+            wrq->u.data.length = MAC_ADDR_LEN;
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CurrentAddress, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_MANUFACTUREROUI:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_MANUFACTUREROUI \n"));
+			wrq->u.data.length = ManufacturerOUI_LEN;
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CurrentAddress, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_MANUFACTURERNAME:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_MANUFACTURERNAME \n"));
+			wrq->u.data.length = strlen(ManufacturerNAME);
+			Status = copy_to_user(wrq->u.data.pointer, ManufacturerNAME, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_RESOURCETYPEIDNAME:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_RESOURCETYPEIDNAME \n"));
+			wrq->u.data.length = strlen(ResourceTypeIdName);
+			Status = copy_to_user(wrq->u.data.pointer, ResourceTypeIdName, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_PRIVACYOPTIONIMPLEMENTED:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_PRIVACYOPTIONIMPLEMENTED \n"));
+			ulInfo = 1; // 1 is support wep else 2 is not support.
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_POWERMANAGEMENTMODE:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_POWERMANAGEMENTMODE \n"));
+			if (pAdapter->StaCfg.Psm == PSMP_ACTION)
+				ulInfo = 1; // 1 is power active else 2 is power save.
+			else
+				ulInfo = 2;
+
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+			break;
+
+		case OID_802_11_WEPDEFAULTKEYVALUE:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_WEPDEFAULTKEYVALUE \n"));
+			//KeyIdxValue.KeyIdx = pAd->PortCfg.MBSSID[pAd->IoctlIF].DefaultKeyId;
+			pKeyIdxValue = wrq->u.data.pointer;
+			DBGPRINT(RT_DEBUG_TRACE,("KeyIdxValue.KeyIdx = %d, \n",pKeyIdxValue->KeyIdx));
+			valueLen = pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen;
+			NdisMoveMemory(pKeyIdxValue->Value,
+						   &pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].Key,
+						   valueLen);
+			pKeyIdxValue->Value[valueLen]='\0';
+
+			wrq->u.data.length = sizeof(DefaultKeyIdxValue);
+
+			Status = copy_to_user(wrq->u.data.pointer, pKeyIdxValue, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE,("DefaultKeyId = %d, total len = %d, str len=%d, KeyValue= %02x %02x %02x %02x \n", pAdapter->StaCfg.DefaultKeyId, wrq->u.data.length, pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen,
+			pAdapter->SharedKey[BSS0][0].Key[0],
+			pAdapter->SharedKey[BSS0][1].Key[0],
+			pAdapter->SharedKey[BSS0][2].Key[0],
+			pAdapter->SharedKey[BSS0][3].Key[0]));
+			break;
+
+		case OID_802_11_WEPDEFAULTKEYID:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_WEPDEFAULTKEYID \n"));
+			wrq->u.data.length = sizeof(UCHAR);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->StaCfg.DefaultKeyId, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("DefaultKeyId =%d \n", pAdapter->StaCfg.DefaultKeyId));
+			break;
+
+		case RT_OID_802_11_WEPKEYMAPPINGLENGTH:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_WEPKEYMAPPINGLENGTH \n"));
+			wrq->u.data.length = sizeof(UCHAR);
+			Status = copy_to_user(wrq->u.data.pointer,
+									&pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen,
+									wrq->u.data.length);
+			break;
+
+		case OID_802_11_SHORTRETRYLIMIT:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_SHORTRETRYLIMIT \n"));
+			wrq->u.data.length = sizeof(ULONG);
+			RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+			ShortRetryLimit = tx_rty_cfg.field.ShortRtyLimit;
+			DBGPRINT(RT_DEBUG_TRACE, ("ShortRetryLimit =%ld,  tx_rty_cfg.field.ShortRetryLimit=%d\n", ShortRetryLimit, tx_rty_cfg.field.ShortRtyLimit));
+			Status = copy_to_user(wrq->u.data.pointer, &ShortRetryLimit, wrq->u.data.length);
+			break;
+
+		case OID_802_11_LONGRETRYLIMIT:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_LONGRETRYLIMIT \n"));
+			wrq->u.data.length = sizeof(ULONG);
+			RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+			LongRetryLimit = tx_rty_cfg.field.LongRtyLimit;
+			DBGPRINT(RT_DEBUG_TRACE, ("LongRetryLimit =%ld,  tx_rty_cfg.field.LongRtyLimit=%d\n", LongRetryLimit, tx_rty_cfg.field.LongRtyLimit));
+			Status = copy_to_user(wrq->u.data.pointer, &LongRetryLimit, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_PRODUCTID:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_PRODUCTID \n"));
+
+#ifdef RT2870
+			sprintf(tmp, "%04x %04x\n", ((POS_COOKIE)pAdapter->OS_Cookie)->pUsb_Dev->descriptor.idVendor ,((POS_COOKIE)pAdapter->OS_Cookie)->pUsb_Dev->descriptor.idProduct);
+
+#endif // RT2870 //
+			wrq->u.data.length = strlen(tmp);
+			Status = copy_to_user(wrq->u.data.pointer, tmp, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_MANUFACTUREID:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_MANUFACTUREID \n"));
+			wrq->u.data.length = strlen(ManufacturerNAME);
+			Status = copy_to_user(wrq->u.data.pointer, ManufacturerNAME, wrq->u.data.length);
+			break;
+
+		case OID_802_11_CURRENTCHANNEL:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_CURRENTCHANNEL \n"));
+			wrq->u.data.length = sizeof(UCHAR);
+			DBGPRINT(RT_DEBUG_TRACE, ("sizeof UCHAR=%d, channel=%d \n", sizeof(UCHAR), pAdapter->CommonCfg.Channel));
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.Channel, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+			break;
+#endif //SNMP_SUPPORT
+
+		case OID_802_11_BUILD_CHANNEL_EX:
+			{
+				UCHAR value;
+				DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_BUILD_CHANNEL_EX \n"));
+				wrq->u.data.length = sizeof(UCHAR);
+#ifdef EXT_BUILD_CHANNEL_LIST
+				DBGPRINT(RT_DEBUG_TRACE, ("Support EXT_BUILD_CHANNEL_LIST.\n"));
+				value = 1;
+#else
+				DBGPRINT(RT_DEBUG_TRACE, ("Doesn't support EXT_BUILD_CHANNEL_LIST.\n"));
+				value = 0;
+#endif // EXT_BUILD_CHANNEL_LIST //
+				Status = copy_to_user(wrq->u.data.pointer, &value, 1);
+				DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+			}
+			break;
+
+		case OID_802_11_GET_CH_LIST:
+			{
+				PRT_CHANNEL_LIST_INFO pChListBuf;
+
+				DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_GET_CH_LIST \n"));
+				if (pAdapter->ChannelListNum == 0)
+				{
+					wrq->u.data.length = 0;
+					break;
+				}
+
+				pChListBuf = (RT_CHANNEL_LIST_INFO *) kmalloc(sizeof(RT_CHANNEL_LIST_INFO), MEM_ALLOC_FLAG);
+				if (pChListBuf == NULL)
+				{
+					wrq->u.data.length = 0;
+					break;
+				}
+
+				pChListBuf->ChannelListNum = pAdapter->ChannelListNum;
+				for (i = 0; i < pChListBuf->ChannelListNum; i++)
+					pChListBuf->ChannelList[i] = pAdapter->ChannelList[i].Channel;
+
+				wrq->u.data.length = sizeof(RT_CHANNEL_LIST_INFO);
+				Status = copy_to_user(wrq->u.data.pointer, pChListBuf, sizeof(RT_CHANNEL_LIST_INFO));
+				DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+
+				if (pChListBuf)
+					kfree(pChListBuf);
+			}
+			break;
+
+		case OID_802_11_GET_COUNTRY_CODE:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_GET_COUNTRY_CODE \n"));
+			wrq->u.data.length = 2;
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.CountryCode, 2);
+			DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+			break;
+
+		case OID_802_11_GET_CHANNEL_GEOGRAPHY:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_GET_CHANNEL_GEOGRAPHY \n"));
+			wrq->u.data.length = 1;
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.Geography, 1);
+			DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+			break;
+
+
+#ifdef QOS_DLS_SUPPORT
+		case RT_OID_802_11_QUERY_DLS:
+			wrq->u.data.length = sizeof(BOOLEAN);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.bDLSCapable, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_DLS(=%d)\n", pAdapter->CommonCfg.bDLSCapable));
+			break;
+
+		case RT_OID_802_11_QUERY_DLS_PARAM:
+			{
+				PRT_802_11_DLS_INFO	pDlsInfo = kmalloc(sizeof(RT_802_11_DLS_INFO), GFP_ATOMIC);
+				if (pDlsInfo == NULL)
+					break;
+
+				for (i=0; i<MAX_NUM_OF_DLS_ENTRY; i++)
+				{
+					RTMPMoveMemory(&pDlsInfo->Entry[i], &pAdapter->StaCfg.DLSEntry[i], sizeof(RT_802_11_DLS_UI));
+				}
+
+				pDlsInfo->num = MAX_NUM_OF_DLS_ENTRY;
+				wrq->u.data.length = sizeof(RT_802_11_DLS_INFO);
+				Status = copy_to_user(wrq->u.data.pointer, pDlsInfo, wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_DLS_PARAM\n"));
+
+				if (pDlsInfo)
+					kfree(pDlsInfo);
+			}
+			break;
+#endif // QOS_DLS_SUPPORT //
+        default:
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::unknown IOCTL's subcmd = 0x%08x\n", cmd));
+            Status = -EOPNOTSUPP;
+            break;
+    }
+    return Status;
+}
+
+INT rt28xx_sta_ioctl(
+	IN	struct net_device	*net_dev,
+	IN	OUT	struct ifreq	*rq,
+	IN	INT					cmd)
+{
+	POS_COOKIE			pObj;
+	VIRTUAL_ADAPTER		*pVirtualAd = NULL;
+	RTMP_ADAPTER        *pAd = NULL;
+	struct iwreq        *wrq = (struct iwreq *) rq;
+	BOOLEAN				StateMachineTouched = FALSE;
+	INT					Status = NDIS_STATUS_SUCCESS;
+	USHORT				subcmd;
+
+	if (net_dev->priv_flags == INT_MAIN)
+	{
+		pAd = net_dev->priv;
+	}
+	else
+	{
+		pVirtualAd = net_dev->priv;
+		pAd = pVirtualAd->RtmpDev->priv;
+	}
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	if (pAd == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+    //check if the interface is down
+    if(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+	    if (wrq->u.data.pointer == NULL)
+	    {
+		    return Status;
+	    }
+
+	    if (strstr(wrq->u.data.pointer, "OpMode") == NULL)
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+        {
+            DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+		    return -ENETDOWN;
+        }
+    }
+
+	{	// determine this ioctl command is comming from which interface.
+		pObj->ioctl_if_type = INT_MAIN;
+		pObj->ioctl_if = MAIN_MBSSID;
+	}
+
+	switch(cmd)
+	{
+#ifdef RALINK_ATE
+#ifdef RALINK_28xx_QA
+		case RTPRIV_IOCTL_ATE:
+			{
+				RtmpDoAte(pAd, wrq);
+			}
+			break;
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+        case SIOCGIFHWADDR:
+			DBGPRINT(RT_DEBUG_TRACE, ("IOCTL::SIOCGIFHWADDR\n"));
+			memcpy(wrq->u.name, pAd->CurrentAddress, ETH_ALEN);
+			break;
+		case SIOCGIWNAME:
+        {
+        	char *name=&wrq->u.name[0];
+        	rt_ioctl_giwname(net_dev, NULL, name, NULL);
+			break;
+		}
+		case SIOCGIWESSID:  //Get ESSID
+        {
+        	struct iw_point *essid=&wrq->u.essid;
+        	rt_ioctl_giwessid(net_dev, NULL, essid, essid->pointer);
+			break;
+		}
+		case SIOCSIWESSID:  //Set ESSID
+        {
+        	struct iw_point	*essid=&wrq->u.essid;
+        	rt_ioctl_siwessid(net_dev, NULL, essid, essid->pointer);
+			break;
+		}
+		case SIOCSIWNWID:   // set network id (the cell)
+		case SIOCGIWNWID:   // get network id
+			Status = -EOPNOTSUPP;
+			break;
+		case SIOCSIWFREQ:   //set channel/frequency (Hz)
+        {
+        	struct iw_freq *freq=&wrq->u.freq;
+        	rt_ioctl_siwfreq(net_dev, NULL, freq, NULL);
+			break;
+		}
+		case SIOCGIWFREQ:   // get channel/frequency (Hz)
+        {
+        	struct iw_freq *freq=&wrq->u.freq;
+        	rt_ioctl_giwfreq(net_dev, NULL, freq, NULL);
+			break;
+		}
+		case SIOCSIWNICKN: //set node name/nickname
+        {
+        	struct iw_point *data=&wrq->u.data;
+        	rt_ioctl_siwnickn(net_dev, NULL, data, NULL);
+			break;
+		}
+		case SIOCGIWNICKN: //get node name/nickname
+        {
+        	struct iw_point *data=&wrq->u.data;
+        	rt_ioctl_giwnickn(net_dev, NULL, data, NULL);
+			break;
+		}
+		case SIOCGIWRATE:   //get default bit rate (bps)
+		    rt_ioctl_giwrate(net_dev, NULL, &wrq->u, NULL);
+            break;
+	    case SIOCSIWRATE:  //set default bit rate (bps)
+	        rt_ioctl_siwrate(net_dev, NULL, &wrq->u, NULL);
+            break;
+        case SIOCGIWRTS:  // get RTS/CTS threshold (bytes)
+        {
+        	struct iw_param *rts=&wrq->u.rts;
+        	rt_ioctl_giwrts(net_dev, NULL, rts, NULL);
+			break;
+		}
+        case SIOCSIWRTS:  //set RTS/CTS threshold (bytes)
+        {
+        	struct iw_param *rts=&wrq->u.rts;
+        	rt_ioctl_siwrts(net_dev, NULL, rts, NULL);
+			break;
+		}
+        case SIOCGIWFRAG:  //get fragmentation thr (bytes)
+        {
+        	struct iw_param *frag=&wrq->u.frag;
+        	rt_ioctl_giwfrag(net_dev, NULL, frag, NULL);
+			break;
+		}
+        case SIOCSIWFRAG:  //set fragmentation thr (bytes)
+        {
+        	struct iw_param *frag=&wrq->u.frag;
+        	rt_ioctl_siwfrag(net_dev, NULL, frag, NULL);
+			break;
+		}
+        case SIOCGIWENCODE:  //get encoding token & mode
+        {
+        	struct iw_point *erq=&wrq->u.encoding;
+        	if(erq->pointer)
+        		rt_ioctl_giwencode(net_dev, NULL, erq, erq->pointer);
+			break;
+		}
+        case SIOCSIWENCODE:  //set encoding token & mode
+        {
+        	struct iw_point *erq=&wrq->u.encoding;
+        	if(erq->pointer)
+        		rt_ioctl_siwencode(net_dev, NULL, erq, erq->pointer);
+			break;
+		}
+		case SIOCGIWAP:     //get access point MAC addresses
+        {
+        	struct sockaddr *ap_addr=&wrq->u.ap_addr;
+        	rt_ioctl_giwap(net_dev, NULL, ap_addr, ap_addr->sa_data);
+			break;
+		}
+	    case SIOCSIWAP:  //set access point MAC addresses
+        {
+        	struct sockaddr *ap_addr=&wrq->u.ap_addr;
+        	rt_ioctl_siwap(net_dev, NULL, ap_addr, ap_addr->sa_data);
+			break;
+		}
+		case SIOCGIWMODE:   //get operation mode
+        {
+        	__u32 *mode=&wrq->u.mode;
+        	rt_ioctl_giwmode(net_dev, NULL, mode, NULL);
+			break;
+		}
+		case SIOCSIWMODE:   //set operation mode
+        {
+        	__u32 *mode=&wrq->u.mode;
+        	rt_ioctl_siwmode(net_dev, NULL, mode, NULL);
+			break;
+		}
+		case SIOCGIWSENS:   //get sensitivity (dBm)
+		case SIOCSIWSENS:	//set sensitivity (dBm)
+		case SIOCGIWPOWER:  //get Power Management settings
+		case SIOCSIWPOWER:  //set Power Management settings
+		case SIOCGIWTXPOW:  //get transmit power (dBm)
+		case SIOCSIWTXPOW:  //set transmit power (dBm)
+		case SIOCGIWRANGE:	//Get range of parameters
+		case SIOCGIWRETRY:	//get retry limits and lifetime
+		case SIOCSIWRETRY:	//set retry limits and lifetime
+			Status = -EOPNOTSUPP;
+			break;
+		case RT_PRIV_IOCTL:
+			subcmd = wrq->u.data.flags;
+			if( subcmd & OID_GET_SET_TOGGLE)
+				Status = RTMPSetInformation(pAd, rq, subcmd);
+			else
+				Status = RTMPQueryInformation(pAd, rq, subcmd);
+			break;
+		case SIOCGIWPRIV:
+			if (wrq->u.data.pointer)
+			{
+				if ( access_ok(VERIFY_WRITE, wrq->u.data.pointer, sizeof(privtab)) != TRUE)
+					break;
+				wrq->u.data.length = sizeof(privtab) / sizeof(privtab[0]);
+				if (copy_to_user(wrq->u.data.pointer, privtab, sizeof(privtab)))
+					Status = -EFAULT;
+			}
+			break;
+		case RTPRIV_IOCTL_SET:
+			if(access_ok(VERIFY_READ, wrq->u.data.pointer, wrq->u.data.length) != TRUE)
+				break;
+			rt_ioctl_setparam(net_dev, NULL, NULL, wrq->u.data.pointer);
+			break;
+		case RTPRIV_IOCTL_GSITESURVEY:
+			RTMPIoctlGetSiteSurvey(pAd, wrq);
+		    break;
+#ifdef DBG
+		case RTPRIV_IOCTL_MAC:
+			RTMPIoctlMAC(pAd, wrq);
+			break;
+		case RTPRIV_IOCTL_E2P:
+			RTMPIoctlE2PROM(pAd, wrq);
+			break;
+#endif // DBG //
+        case SIOCETHTOOL:
+                break;
+		default:
+			DBGPRINT(RT_DEBUG_ERROR, ("IOCTL::unknown IOCTL's cmd = 0x%08x\n", cmd));
+			Status = -EOPNOTSUPP;
+			break;
+	}
+
+    if(StateMachineTouched) // Upper layer sent a MLME-related operations
+    	RT28XX_MLME_HANDLER(pAd);
+
+	return Status;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set SSID
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_SSID_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    NDIS_802_11_SSID                    Ssid, *pSsid=NULL;
+    BOOLEAN                             StateMachineTouched = FALSE;
+    int                                 success = TRUE;
+
+    if( strlen(arg) <= MAX_LEN_OF_SSID)
+    {
+        NdisZeroMemory(&Ssid, sizeof(NDIS_802_11_SSID));
+        if (strlen(arg) != 0)
+        {
+            NdisMoveMemory(Ssid.Ssid, arg, strlen(arg));
+            Ssid.SsidLength = strlen(arg);
+        }
+        else   //ANY ssid
+        {
+            Ssid.SsidLength = 0;
+		    memcpy(Ssid.Ssid, "", 0);
+			pAdapter->StaCfg.BssType = BSS_INFRA;
+			pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+	        pAdapter->StaCfg.WepStatus  = Ndis802_11EncryptionDisabled;
+		}
+        pSsid = &Ssid;
+
+        if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+        {
+            RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+            DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+        }
+
+        pAdapter->MlmeAux.CurrReqIsFromNdis = TRUE;
+        pAdapter->StaCfg.bScanReqIsFromWebUI = FALSE;
+		pAdapter->bConfigChanged = TRUE;
+
+        MlmeEnqueue(pAdapter,
+                    MLME_CNTL_STATE_MACHINE,
+                    OID_802_11_SSID,
+                    sizeof(NDIS_802_11_SSID),
+                    (VOID *)pSsid);
+
+        StateMachineTouched = TRUE;
+        DBGPRINT(RT_DEBUG_TRACE, ("Set_SSID_Proc::(Len=%d,Ssid=%s)\n", Ssid.SsidLength, Ssid.Ssid));
+    }
+    else
+        success = FALSE;
+
+    if (StateMachineTouched) // Upper layer sent a MLME-related operations
+    	RT28XX_MLME_HANDLER(pAdapter);
+
+    return success;
+}
+
+#ifdef WMM_SUPPORT
+/*
+    ==========================================================================
+    Description:
+        Set WmmCapable Enable or Disable
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_WmmCapable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	BOOLEAN	bWmmCapable;
+
+	bWmmCapable = simple_strtol(arg, 0, 10);
+
+	if ((bWmmCapable == 1)
+#ifdef RT2870
+		&& (pAd->NumberOfPipes >= 5)
+#endif // RT2870 //
+		)
+		pAd->CommonCfg.bWmmCapable = TRUE;
+	else if (bWmmCapable == 0)
+		pAd->CommonCfg.bWmmCapable = FALSE;
+	else
+		return FALSE;  //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_WmmCapable_Proc::(bWmmCapable=%d)\n",
+		pAd->CommonCfg.bWmmCapable));
+
+	return TRUE;
+}
+#endif // WMM_SUPPORT //
+
+/*
+    ==========================================================================
+    Description:
+        Set Network Type(Infrastructure/Adhoc mode)
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_NetworkType_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    UINT32	Value = 0;
+
+    if (strcmp(arg, "Adhoc") == 0)
+	{
+		if (pAdapter->StaCfg.BssType != BSS_ADHOC)
+		{
+			// Config has changed
+			pAdapter->bConfigChanged = TRUE;
+            if (MONITOR_ON(pAdapter))
+            {
+                RTMP_IO_WRITE32(pAdapter, RX_FILTR_CFG, STANORMAL);
+                RTMP_IO_READ32(pAdapter, MAC_SYS_CTRL, &Value);
+				Value &= (~0x80);
+				RTMP_IO_WRITE32(pAdapter, MAC_SYS_CTRL, Value);
+                OPSTATUS_CLEAR_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED);
+                pAdapter->StaCfg.bAutoReconnect = TRUE;
+                LinkDown(pAdapter, FALSE);
+            }
+			if (INFRA_ON(pAdapter))
+			{
+				//BOOLEAN Cancelled;
+				// Set the AutoReconnectSsid to prevent it reconnect to old SSID
+				// Since calling this indicate user don't want to connect to that SSID anymore.
+				pAdapter->MlmeAux.AutoReconnectSsidLen= 32;
+				NdisZeroMemory(pAdapter->MlmeAux.AutoReconnectSsid, pAdapter->MlmeAux.AutoReconnectSsidLen);
+
+				LinkDown(pAdapter, FALSE);
+
+				DBGPRINT(RT_DEBUG_TRACE, ("NDIS_STATUS_MEDIA_DISCONNECT Event BB!\n"));
+			}
+		}
+		pAdapter->StaCfg.BssType = BSS_ADHOC;
+        pAdapter->net_dev->type = pAdapter->StaCfg.OriDevType;
+		DBGPRINT(RT_DEBUG_TRACE, ("===>Set_NetworkType_Proc::(AD-HOC)\n"));
+	}
+    else if (strcmp(arg, "Infra") == 0)
+	{
+		if (pAdapter->StaCfg.BssType != BSS_INFRA)
+		{
+			// Config has changed
+			pAdapter->bConfigChanged = TRUE;
+            if (MONITOR_ON(pAdapter))
+            {
+                RTMP_IO_WRITE32(pAdapter, RX_FILTR_CFG, STANORMAL);
+                RTMP_IO_READ32(pAdapter, MAC_SYS_CTRL, &Value);
+				Value &= (~0x80);
+				RTMP_IO_WRITE32(pAdapter, MAC_SYS_CTRL, Value);
+                OPSTATUS_CLEAR_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED);
+                pAdapter->StaCfg.bAutoReconnect = TRUE;
+                LinkDown(pAdapter, FALSE);
+            }
+			if (ADHOC_ON(pAdapter))
+			{
+				// Set the AutoReconnectSsid to prevent it reconnect to old SSID
+				// Since calling this indicate user don't want to connect to that SSID anymore.
+				pAdapter->MlmeAux.AutoReconnectSsidLen= 32;
+				NdisZeroMemory(pAdapter->MlmeAux.AutoReconnectSsid, pAdapter->MlmeAux.AutoReconnectSsidLen);
+
+				LinkDown(pAdapter, FALSE);
+			}
+		}
+		pAdapter->StaCfg.BssType = BSS_INFRA;
+        pAdapter->net_dev->type = pAdapter->StaCfg.OriDevType;
+		DBGPRINT(RT_DEBUG_TRACE, ("===>Set_NetworkType_Proc::(INFRA)\n"));
+
+        pAdapter->StaCfg.BssType = BSS_INFRA;
+	}
+    else if (strcmp(arg, "Monitor") == 0)
+    {
+		UCHAR	bbpValue = 0;
+		BCN_TIME_CFG_STRUC csr;
+		OPSTATUS_CLEAR_FLAG(pAdapter, fOP_STATUS_INFRA_ON);
+        OPSTATUS_CLEAR_FLAG(pAdapter, fOP_STATUS_ADHOC_ON);
+		OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED);
+		// disable all periodic state machine
+		pAdapter->StaCfg.bAutoReconnect = FALSE;
+		// reset all mlme state machine
+		RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+		DBGPRINT(RT_DEBUG_TRACE, ("fOP_STATUS_MEDIA_STATE_CONNECTED \n"));
+        if (pAdapter->CommonCfg.CentralChannel == 0)
+        {
+#ifdef DOT11_N_SUPPORT
+            if (pAdapter->CommonCfg.PhyMode == PHY_11AN_MIXED)
+                pAdapter->CommonCfg.CentralChannel = 36;
+            else
+#endif // DOT11_N_SUPPORT //
+                pAdapter->CommonCfg.CentralChannel = 6;
+        }
+#ifdef DOT11_N_SUPPORT
+        else
+            N_ChannelCheck(pAdapter);
+#endif // DOT11_N_SUPPORT //
+
+#ifdef DOT11_N_SUPPORT
+	if (pAdapter->CommonCfg.PhyMode >= PHY_11ABGN_MIXED &&
+            pAdapter->CommonCfg.RegTransmitSetting.field.BW == BW_40 &&
+            pAdapter->CommonCfg.RegTransmitSetting.field.EXTCHA == EXTCHA_ABOVE)
+		{
+			// 40MHz ,control channel at lower
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R4, &bbpValue);
+			bbpValue &= (~0x18);
+			bbpValue |= 0x10;
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R4, bbpValue);
+			pAdapter->CommonCfg.BBPCurrentBW = BW_40;
+			//  RX : control channel at lower
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R3, &bbpValue);
+			bbpValue &= (~0x20);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R3, bbpValue);
+
+			RTMP_IO_READ32(pAdapter, TX_BAND_CFG, &Value);
+			Value &= 0xfffffffe;
+			RTMP_IO_WRITE32(pAdapter, TX_BAND_CFG, Value);
+			pAdapter->CommonCfg.CentralChannel = pAdapter->CommonCfg.Channel + 2;
+            AsicSwitchChannel(pAdapter, pAdapter->CommonCfg.CentralChannel, FALSE);
+		    AsicLockChannel(pAdapter, pAdapter->CommonCfg.CentralChannel);
+            DBGPRINT(RT_DEBUG_TRACE, ("BW_40 ,control_channel(%d), CentralChannel(%d) \n",
+                                       pAdapter->CommonCfg.Channel,
+                                       pAdapter->CommonCfg.CentralChannel));
+		}
+		else if (pAdapter->CommonCfg.PhyMode >= PHY_11ABGN_MIXED &&
+                 pAdapter->CommonCfg.RegTransmitSetting.field.BW == BW_40 &&
+                 pAdapter->CommonCfg.RegTransmitSetting.field.EXTCHA == EXTCHA_BELOW)
+		{
+			// 40MHz ,control channel at upper
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R4, &bbpValue);
+			bbpValue &= (~0x18);
+			bbpValue |= 0x10;
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R4, bbpValue);
+			pAdapter->CommonCfg.BBPCurrentBW = BW_40;
+			RTMP_IO_READ32(pAdapter, TX_BAND_CFG, &Value);
+			Value |= 0x1;
+			RTMP_IO_WRITE32(pAdapter, TX_BAND_CFG, Value);
+
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R3, &bbpValue);
+			bbpValue |= (0x20);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R3, bbpValue);
+			pAdapter->CommonCfg.CentralChannel = pAdapter->CommonCfg.Channel - 2;
+            AsicSwitchChannel(pAdapter, pAdapter->CommonCfg.CentralChannel, FALSE);
+		    AsicLockChannel(pAdapter, pAdapter->CommonCfg.CentralChannel);
+            DBGPRINT(RT_DEBUG_TRACE, ("BW_40 ,control_channel(%d), CentralChannel(%d) \n",
+                                       pAdapter->CommonCfg.Channel,
+                                       pAdapter->CommonCfg.CentralChannel));
+		}
+		else
+#endif // DOT11_N_SUPPORT //
+		{
+			// 20MHz
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R4, &bbpValue);
+			bbpValue &= (~0x18);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R4, bbpValue);
+			pAdapter->CommonCfg.BBPCurrentBW = BW_20;
+			AsicSwitchChannel(pAdapter, pAdapter->CommonCfg.Channel, FALSE);
+			AsicLockChannel(pAdapter, pAdapter->CommonCfg.Channel);
+			DBGPRINT(RT_DEBUG_TRACE, ("BW_20, Channel(%d)\n", pAdapter->CommonCfg.Channel));
+		}
+		// Enable Rx with promiscuous reception
+		RTMP_IO_WRITE32(pAdapter, RX_FILTR_CFG, 0x3);
+		// ASIC supporsts sniffer function with replacing RSSI with timestamp.
+		//RTMP_IO_READ32(pAdapter, MAC_SYS_CTRL, &Value);
+		//Value |= (0x80);
+		//RTMP_IO_WRITE32(pAdapter, MAC_SYS_CTRL, Value);
+		// disable sync
+		RTMP_IO_READ32(pAdapter, BCN_TIME_CFG, &csr.word);
+		csr.field.bBeaconGen = 0;
+		csr.field.bTBTTEnable = 0;
+		csr.field.TsfSyncMode = 0;
+		RTMP_IO_WRITE32(pAdapter, BCN_TIME_CFG, csr.word);
+
+		pAdapter->StaCfg.BssType = BSS_MONITOR;
+        pAdapter->net_dev->type = ARPHRD_IEEE80211_PRISM; //ARPHRD_IEEE80211; // IEEE80211
+		DBGPRINT(RT_DEBUG_TRACE, ("===>Set_NetworkType_Proc::(MONITOR)\n"));
+    }
+
+    // Reset Ralink supplicant to not use, it will be set to start when UI set PMK key
+    pAdapter->StaCfg.WpaState = SS_NOTUSE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_NetworkType_Proc::(NetworkType=%d)\n", pAdapter->StaCfg.BssType));
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Authentication mode
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_AuthMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    if ((strcmp(arg, "WEPAUTO") == 0) || (strcmp(arg, "wepauto") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeAutoSwitch;
+    else if ((strcmp(arg, "OPEN") == 0) || (strcmp(arg, "open") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+    else if ((strcmp(arg, "SHARED") == 0) || (strcmp(arg, "shared") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeShared;
+    else if ((strcmp(arg, "WPAPSK") == 0) || (strcmp(arg, "wpapsk") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPAPSK;
+    else if ((strcmp(arg, "WPANONE") == 0) || (strcmp(arg, "wpanone") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPANone;
+    else if ((strcmp(arg, "WPA2PSK") == 0) || (strcmp(arg, "wpa2psk") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA2PSK;
+#ifdef WPA_SUPPLICANT_SUPPORT
+    else if ((strcmp(arg, "WPA") == 0) || (strcmp(arg, "wpa") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA;
+    else if ((strcmp(arg, "WPA2") == 0) || (strcmp(arg, "wpa2") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA2;
+#endif // WPA_SUPPLICANT_SUPPORT //
+    else
+        return FALSE;
+
+    pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_AuthMode_Proc::(AuthMode=%d)\n", pAdapter->StaCfg.AuthMode));
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Encryption Type
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_EncrypType_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    if ((strcmp(arg, "NONE") == 0) || (strcmp(arg, "none") == 0))
+    {
+        if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+            return TRUE;    // do nothing
+
+        pAdapter->StaCfg.WepStatus     = Ndis802_11WEPDisabled;
+        pAdapter->StaCfg.PairCipher    = Ndis802_11WEPDisabled;
+	    pAdapter->StaCfg.GroupCipher   = Ndis802_11WEPDisabled;
+    }
+    else if ((strcmp(arg, "WEP") == 0) || (strcmp(arg, "wep") == 0))
+    {
+        if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+            return TRUE;    // do nothing
+
+        pAdapter->StaCfg.WepStatus     = Ndis802_11WEPEnabled;
+        pAdapter->StaCfg.PairCipher    = Ndis802_11WEPEnabled;
+	    pAdapter->StaCfg.GroupCipher   = Ndis802_11WEPEnabled;
+    }
+    else if ((strcmp(arg, "TKIP") == 0) || (strcmp(arg, "tkip") == 0))
+    {
+        if (pAdapter->StaCfg.AuthMode < Ndis802_11AuthModeWPA)
+            return TRUE;    // do nothing
+
+        pAdapter->StaCfg.WepStatus     = Ndis802_11Encryption2Enabled;
+        pAdapter->StaCfg.PairCipher    = Ndis802_11Encryption2Enabled;
+	    pAdapter->StaCfg.GroupCipher   = Ndis802_11Encryption2Enabled;
+    }
+    else if ((strcmp(arg, "AES") == 0) || (strcmp(arg, "aes") == 0))
+    {
+        if (pAdapter->StaCfg.AuthMode < Ndis802_11AuthModeWPA)
+            return TRUE;    // do nothing
+
+        pAdapter->StaCfg.WepStatus     = Ndis802_11Encryption3Enabled;
+        pAdapter->StaCfg.PairCipher    = Ndis802_11Encryption3Enabled;
+	    pAdapter->StaCfg.GroupCipher   = Ndis802_11Encryption3Enabled;
+    }
+    else
+        return FALSE;
+
+    pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_EncrypType_Proc::(EncrypType=%d)\n", pAdapter->StaCfg.WepStatus));
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Default Key ID
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_DefaultKeyID_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    ULONG                               KeyIdx;
+
+    KeyIdx = simple_strtol(arg, 0, 10);
+    if((KeyIdx >= 1 ) && (KeyIdx <= 4))
+        pAdapter->StaCfg.DefaultKeyId = (UCHAR) (KeyIdx - 1 );
+    else
+        return FALSE;  //Invalid argument
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_DefaultKeyID_Proc::(DefaultKeyID=%d)\n", pAdapter->StaCfg.DefaultKeyId));
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set WEP KEY1
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Key1_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    int                                 KeyLen;
+    int                                 i;
+    UCHAR                               CipherAlg=CIPHER_WEP64;
+
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        return TRUE;    // do nothing
+
+    KeyLen = strlen(arg);
+
+    switch (KeyLen)
+    {
+        case 5: //wep 40 Ascii type
+            pAdapter->SharedKey[BSS0][0].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][0].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::(Key1=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 10: //wep 40 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][0].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][0].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::(Key1=%s and type=%s)\n", arg, "Hex"));
+            break;
+        case 13: //wep 104 Ascii type
+            pAdapter->SharedKey[BSS0][0].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][0].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::(Key1=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 26: //wep 104 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][0].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][0].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::(Key1=%s and type=%s)\n", arg, "Hex"));
+            break;
+        default: //Invalid argument
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::Invalid argument (=%s)\n", arg));
+            return FALSE;
+    }
+
+    pAdapter->SharedKey[BSS0][0].CipherAlg = CipherAlg;
+
+    // Set keys (into ASIC)
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        ;   // not support
+    else    // Old WEP stuff
+    {
+        AsicAddSharedKeyEntry(pAdapter,
+                              0,
+                              0,
+                              pAdapter->SharedKey[BSS0][0].CipherAlg,
+                              pAdapter->SharedKey[BSS0][0].Key,
+                              NULL,
+                              NULL);
+    }
+
+    return TRUE;
+}
+/*
+    ==========================================================================
+
+    Description:
+        Set WEP KEY2
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Key2_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    int                                 KeyLen;
+    int                                 i;
+    UCHAR                               CipherAlg=CIPHER_WEP64;
+
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        return TRUE;    // do nothing
+
+    KeyLen = strlen(arg);
+
+    switch (KeyLen)
+    {
+        case 5: //wep 40 Ascii type
+            pAdapter->SharedKey[BSS0][1].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][1].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::(Key2=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 10: //wep 40 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][1].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][1].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::(Key2=%s and type=%s)\n", arg, "Hex"));
+            break;
+        case 13: //wep 104 Ascii type
+            pAdapter->SharedKey[BSS0][1].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][1].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::(Key2=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 26: //wep 104 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][1].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][1].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::(Key2=%s and type=%s)\n", arg, "Hex"));
+            break;
+        default: //Invalid argument
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::Invalid argument (=%s)\n", arg));
+            return FALSE;
+    }
+    pAdapter->SharedKey[BSS0][1].CipherAlg = CipherAlg;
+
+    // Set keys (into ASIC)
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        ;   // not support
+    else    // Old WEP stuff
+    {
+        AsicAddSharedKeyEntry(pAdapter,
+                              0,
+                              1,
+                              pAdapter->SharedKey[BSS0][1].CipherAlg,
+                              pAdapter->SharedKey[BSS0][1].Key,
+                              NULL,
+                              NULL);
+    }
+
+    return TRUE;
+}
+/*
+    ==========================================================================
+    Description:
+        Set WEP KEY3
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Key3_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    int                                 KeyLen;
+    int                                 i;
+    UCHAR                               CipherAlg=CIPHER_WEP64;
+
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        return TRUE;    // do nothing
+
+    KeyLen = strlen(arg);
+
+    switch (KeyLen)
+    {
+        case 5: //wep 40 Ascii type
+            pAdapter->SharedKey[BSS0][2].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][2].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::(Key3=%s and type=Ascii)\n", arg));
+            break;
+        case 10: //wep 40 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][2].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][2].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::(Key3=%s and type=Hex)\n", arg));
+            break;
+        case 13: //wep 104 Ascii type
+            pAdapter->SharedKey[BSS0][2].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][2].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::(Key3=%s and type=Ascii)\n", arg));
+            break;
+        case 26: //wep 104 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][2].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][2].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::(Key3=%s and type=Hex)\n", arg));
+            break;
+        default: //Invalid argument
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::Invalid argument (=%s)\n", arg));
+            return FALSE;
+    }
+    pAdapter->SharedKey[BSS0][2].CipherAlg = CipherAlg;
+
+    // Set keys (into ASIC)
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        ;   // not support
+    else    // Old WEP stuff
+    {
+        AsicAddSharedKeyEntry(pAdapter,
+                              0,
+                              2,
+                              pAdapter->SharedKey[BSS0][2].CipherAlg,
+                              pAdapter->SharedKey[BSS0][2].Key,
+                              NULL,
+                              NULL);
+    }
+
+    return TRUE;
+}
+/*
+    ==========================================================================
+    Description:
+        Set WEP KEY4
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Key4_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    int                                 KeyLen;
+    int                                 i;
+    UCHAR                               CipherAlg=CIPHER_WEP64;
+
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        return TRUE;    // do nothing
+
+    KeyLen = strlen(arg);
+
+    switch (KeyLen)
+    {
+        case 5: //wep 40 Ascii type
+            pAdapter->SharedKey[BSS0][3].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][3].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::(Key4=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 10: //wep 40 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][3].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][3].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::(Key4=%s and type=%s)\n", arg, "Hex"));
+            break;
+        case 13: //wep 104 Ascii type
+            pAdapter->SharedKey[BSS0][3].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][3].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::(Key4=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 26: //wep 104 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][3].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][3].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::(Key4=%s and type=%s)\n", arg, "Hex"));
+            break;
+        default: //Invalid argument
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::Invalid argument (=%s)\n", arg));
+            return FALSE;
+    }
+    pAdapter->SharedKey[BSS0][3].CipherAlg = CipherAlg;
+
+    // Set keys (into ASIC)
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        ;   // not support
+    else    // Old WEP stuff
+    {
+        AsicAddSharedKeyEntry(pAdapter,
+                              0,
+                              3,
+                              pAdapter->SharedKey[BSS0][3].CipherAlg,
+                              pAdapter->SharedKey[BSS0][3].Key,
+                              NULL,
+                              NULL);
+    }
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set WPA PSK key
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_WPAPSK_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    UCHAR                   keyMaterial[40];
+
+    if ((pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPAPSK) &&
+        (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPA2PSK) &&
+	    (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPANone)
+		)
+        return TRUE;    // do nothing
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_WPAPSK_Proc::(WPAPSK=%s)\n", arg));
+
+    NdisZeroMemory(keyMaterial, 40);
+
+    if ((strlen(arg) < 8) || (strlen(arg) > 64))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("Set failed!!(WPAPSK=%s), WPAPSK key-string required 8 ~ 64 characters \n", arg));
+        return FALSE;
+    }
+
+    if (strlen(arg) == 64)
+    {
+        AtoH(arg, keyMaterial, 32);
+        NdisMoveMemory(pAdapter->StaCfg.PMK, keyMaterial, 32);
+
+    }
+    else
+    {
+        PasswordHash((char *)arg, pAdapter->MlmeAux.Ssid, pAdapter->MlmeAux.SsidLen, keyMaterial);
+        NdisMoveMemory(pAdapter->StaCfg.PMK, keyMaterial, 32);
+    }
+
+
+
+    if(pAdapter->StaCfg.BssType == BSS_ADHOC &&
+       pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+    {
+         pAdapter->StaCfg.WpaState = SS_NOTUSE;
+    }
+    else
+    {
+        // Start STA supplicant state machine
+        pAdapter->StaCfg.WpaState = SS_START;
+    }
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Power Saving mode
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_PSMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    if (pAdapter->StaCfg.BssType == BSS_INFRA)
+    {
+        if ((strcmp(arg, "Max_PSP") == 0) ||
+			(strcmp(arg, "max_psp") == 0) ||
+			(strcmp(arg, "MAX_PSP") == 0))
+        {
+            // do NOT turn on PSM bit here, wait until MlmeCheckForPsmChange()
+            // to exclude certain situations.
+            if (pAdapter->StaCfg.bWindowsACCAMEnable == FALSE)
+                pAdapter->StaCfg.WindowsPowerMode = Ndis802_11PowerModeMAX_PSP;
+            pAdapter->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeMAX_PSP;
+            OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_RECEIVE_DTIM);
+            pAdapter->StaCfg.DefaultListenCount = 5;
+
+        }
+        else if ((strcmp(arg, "Fast_PSP") == 0) ||
+				 (strcmp(arg, "fast_psp") == 0) ||
+                 (strcmp(arg, "FAST_PSP") == 0))
+        {
+            // do NOT turn on PSM bit here, wait until MlmeCheckForPsmChange()
+            // to exclude certain situations.
+            OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_RECEIVE_DTIM);
+            if (pAdapter->StaCfg.bWindowsACCAMEnable == FALSE)
+                pAdapter->StaCfg.WindowsPowerMode = Ndis802_11PowerModeFast_PSP;
+            pAdapter->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeFast_PSP;
+            pAdapter->StaCfg.DefaultListenCount = 3;
+        }
+        else if ((strcmp(arg, "Legacy_PSP") == 0) ||
+                 (strcmp(arg, "legacy_psp") == 0) ||
+                 (strcmp(arg, "LEGACY_PSP") == 0))
+        {
+            // do NOT turn on PSM bit here, wait until MlmeCheckForPsmChange()
+            // to exclude certain situations.
+            OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_RECEIVE_DTIM);
+            if (pAdapter->StaCfg.bWindowsACCAMEnable == FALSE)
+                pAdapter->StaCfg.WindowsPowerMode = Ndis802_11PowerModeLegacy_PSP;
+            pAdapter->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeLegacy_PSP;
+            pAdapter->StaCfg.DefaultListenCount = 3;
+        }
+        else
+        {
+            //Default Ndis802_11PowerModeCAM
+            // clear PSM bit immediately
+            MlmeSetPsmBit(pAdapter, PWR_ACTIVE);
+            OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_RECEIVE_DTIM);
+            if (pAdapter->StaCfg.bWindowsACCAMEnable == FALSE)
+                pAdapter->StaCfg.WindowsPowerMode = Ndis802_11PowerModeCAM;
+            pAdapter->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeCAM;
+        }
+
+        DBGPRINT(RT_DEBUG_TRACE, ("Set_PSMode_Proc::(PSMode=%ld)\n", pAdapter->StaCfg.WindowsPowerMode));
+    }
+    else
+        return FALSE;
+
+
+    return TRUE;
+}
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+/*
+    ==========================================================================
+    Description:
+        Set WpaSupport flag.
+    Value:
+        0: Driver ignore wpa_supplicant.
+        1: wpa_supplicant initiates scanning and AP selection.
+        2: driver takes care of scanning, AP selection, and IEEE 802.11 association parameters.
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Wpa_Support(
+    IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+
+    if ( simple_strtol(arg, 0, 10) == 0)
+        pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_DISABLE;
+    else if ( simple_strtol(arg, 0, 10) == 1)
+        pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_ENABLE;
+    else if ( simple_strtol(arg, 0, 10) == 2)
+        pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_ENABLE_WITH_WEB_UI;
+    else
+        pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_DISABLE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_Wpa_Support::(WpaSupplicantUP=%d)\n", pAd->StaCfg.WpaSupplicantUP));
+
+    return TRUE;
+}
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef DBG
+/*
+    ==========================================================================
+    Description:
+        Read / Write MAC
+    Arguments:
+        pAdapter                    Pointer to our adapter
+        wrq                         Pointer to the ioctl argument
+
+    Return Value:
+        None
+
+    Note:
+        Usage:
+               1.) iwpriv ra0 mac 0        ==> read MAC where Addr=0x0
+               2.) iwpriv ra0 mac 0=12     ==> write MAC where Addr=0x0, value=12
+    ==========================================================================
+*/
+VOID RTMPIoctlMAC(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq)
+{
+	CHAR				*this_char;
+	CHAR				*value;
+	INT					j = 0, k = 0;
+	CHAR				msg[1024];
+	CHAR				arg[255];
+	ULONG				macAddr = 0;
+	UCHAR				temp[16], temp2[16];
+	UINT32				macValue = 0;
+	INT					Status;
+
+
+	memset(msg, 0x00, 1024);
+	if (wrq->u.data.length > 1) //No parameters.
+	{
+	    Status = copy_from_user(arg, wrq->u.data.pointer, (wrq->u.data.length > 255) ? 255 : wrq->u.data.length);
+		sprintf(msg, "\n");
+
+		//Parsing Read or Write
+	    this_char = arg;
+		if (!*this_char)
+			goto next;
+
+		if ((value = rtstrchr(this_char, '=')) != NULL)
+			*value++ = 0;
+
+		if (!value || !*value)
+		{ //Read
+			// Sanity check
+			if(strlen(this_char) > 4)
+				goto next;
+
+			j = strlen(this_char);
+			while(j-- > 0)
+			{
+				if(this_char[j] > 'f' || this_char[j] < '0')
+					return;
+			}
+
+			// Mac Addr
+			k = j = strlen(this_char);
+			while(j-- > 0)
+			{
+				this_char[4-k+j] = this_char[j];
+			}
+
+			while(k < 4)
+				this_char[3-k++]='0';
+			this_char[4]='\0';
+
+			if(strlen(this_char) == 4)
+			{
+				AtoH(this_char, temp, 2);
+				macAddr = *temp*256 + temp[1];
+				if (macAddr < 0xFFFF)
+				{
+					RTMP_IO_READ32(pAdapter, macAddr, &macValue);
+					DBGPRINT(RT_DEBUG_TRACE, ("MacAddr=%lx, MacValue=%x\n", macAddr, macValue));
+					sprintf(msg+strlen(msg), "[0x%08lX]:%08X  ", macAddr , macValue);
+				}
+				else
+				{//Invalid parametes, so default printk all bbp
+					goto next;
+				}
+			}
+		}
+		else
+		{ //Write
+			memcpy(&temp2, value, strlen(value));
+			temp2[strlen(value)] = '\0';
+
+			// Sanity check
+			if((strlen(this_char) > 4) || strlen(temp2) > 8)
+				goto next;
+
+			j = strlen(this_char);
+			while(j-- > 0)
+			{
+				if(this_char[j] > 'f' || this_char[j] < '0')
+					return;
+			}
+
+			j = strlen(temp2);
+			while(j-- > 0)
+			{
+				if(temp2[j] > 'f' || temp2[j] < '0')
+					return;
+			}
+
+			//MAC Addr
+			k = j = strlen(this_char);
+			while(j-- > 0)
+			{
+				this_char[4-k+j] = this_char[j];
+			}
+
+			while(k < 4)
+				this_char[3-k++]='0';
+			this_char[4]='\0';
+
+			//MAC value
+			k = j = strlen(temp2);
+			while(j-- > 0)
+			{
+				temp2[8-k+j] = temp2[j];
+			}
+
+			while(k < 8)
+				temp2[7-k++]='0';
+			temp2[8]='\0';
+
+			{
+				AtoH(this_char, temp, 2);
+				macAddr = *temp*256 + temp[1];
+
+				AtoH(temp2, temp, 4);
+				macValue = *temp*256*256*256 + temp[1]*256*256 + temp[2]*256 + temp[3];
+
+				// debug mode
+				if (macAddr == (HW_DEBUG_SETTING_BASE + 4))
+				{
+					// 0x2bf4: byte0 non-zero: enable R17 tuning, 0: disable R17 tuning
+                    if (macValue & 0x000000ff)
+                    {
+                        pAdapter->BbpTuning.bEnable = TRUE;
+                        DBGPRINT(RT_DEBUG_TRACE,("turn on R17 tuning\n"));
+                    }
+                    else
+                    {
+                        UCHAR R66;
+                        pAdapter->BbpTuning.bEnable = FALSE;
+                        R66 = 0x26 + GET_LNA_GAIN(pAdapter);
+#ifdef RALINK_ATE
+						if (ATE_ON(pAdapter))
+						{
+							ATE_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R66, (0x26 + GET_LNA_GAIN(pAdapter)));
+						}
+						else
+#endif // RALINK_ATE //
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R66, (0x26 + GET_LNA_GAIN(pAdapter)));
+                        DBGPRINT(RT_DEBUG_TRACE,("turn off R17 tuning, restore to 0x%02x\n", R66));
+                    }
+					return;
+				}
+
+				DBGPRINT(RT_DEBUG_TRACE, ("MacAddr=%02lx, MacValue=0x%x\n", macAddr, macValue));
+
+				RTMP_IO_WRITE32(pAdapter, macAddr, macValue);
+				sprintf(msg+strlen(msg), "[0x%08lX]:%08X  ", macAddr, macValue);
+			}
+		}
+	}
+next:
+	if(strlen(msg) == 1)
+		sprintf(msg+strlen(msg), "===>Error command format!");
+
+	// Copy the information into the user buffer
+	wrq->u.data.length = strlen(msg);
+	Status = copy_to_user(wrq->u.data.pointer, msg, wrq->u.data.length);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<==RTMPIoctlMAC\n\n"));
+}
+
+/*
+    ==========================================================================
+    Description:
+        Read / Write E2PROM
+    Arguments:
+        pAdapter                    Pointer to our adapter
+        wrq                         Pointer to the ioctl argument
+
+    Return Value:
+        None
+
+    Note:
+        Usage:
+               1.) iwpriv ra0 e2p 0     	==> read E2PROM where Addr=0x0
+               2.) iwpriv ra0 e2p 0=1234    ==> write E2PROM where Addr=0x0, value=1234
+    ==========================================================================
+*/
+VOID RTMPIoctlE2PROM(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq)
+{
+	CHAR				*this_char;
+	CHAR				*value;
+	INT					j = 0, k = 0;
+	CHAR				msg[1024];
+	CHAR				arg[255];
+	USHORT				eepAddr = 0;
+	UCHAR				temp[16], temp2[16];
+	USHORT				eepValue;
+	int					Status;
+
+
+	memset(msg, 0x00, 1024);
+	if (wrq->u.data.length > 1) //No parameters.
+	{
+	    Status = copy_from_user(arg, wrq->u.data.pointer, (wrq->u.data.length > 255) ? 255 : wrq->u.data.length);
+		sprintf(msg, "\n");
+
+	    //Parsing Read or Write
+		this_char = arg;
+
+
+		if (!*this_char)
+			goto next;
+
+		if ((value = rtstrchr(this_char, '=')) != NULL)
+			*value++ = 0;
+
+		if (!value || !*value)
+		{ //Read
+
+			// Sanity check
+			if(strlen(this_char) > 4)
+				goto next;
+
+			j = strlen(this_char);
+			while(j-- > 0)
+			{
+				if(this_char[j] > 'f' || this_char[j] < '0')
+					return;
+			}
+
+			// E2PROM addr
+			k = j = strlen(this_char);
+			while(j-- > 0)
+			{
+				this_char[4-k+j] = this_char[j];
+			}
+
+			while(k < 4)
+				this_char[3-k++]='0';
+			this_char[4]='\0';
+
+			if(strlen(this_char) == 4)
+			{
+				AtoH(this_char, temp, 2);
+				eepAddr = *temp*256 + temp[1];
+				if (eepAddr < 0xFFFF)
+				{
+					RT28xx_EEPROM_READ16(pAdapter, eepAddr, eepValue);
+					sprintf(msg+strlen(msg), "[0x%04X]:0x%04X  ", eepAddr , eepValue);
+				}
+				else
+				{//Invalid parametes, so default printk all bbp
+					goto next;
+				}
+			}
+		}
+		else
+		{ //Write
+			memcpy(&temp2, value, strlen(value));
+			temp2[strlen(value)] = '\0';
+
+			// Sanity check
+			if((strlen(this_char) > 4) || strlen(temp2) > 8)
+				goto next;
+
+			j = strlen(this_char);
+			while(j-- > 0)
+			{
+				if(this_char[j] > 'f' || this_char[j] < '0')
+					return;
+			}
+			j = strlen(temp2);
+			while(j-- > 0)
+			{
+				if(temp2[j] > 'f' || temp2[j] < '0')
+					return;
+			}
+
+			//MAC Addr
+			k = j = strlen(this_char);
+			while(j-- > 0)
+			{
+				this_char[4-k+j] = this_char[j];
+			}
+
+			while(k < 4)
+				this_char[3-k++]='0';
+			this_char[4]='\0';
+
+			//MAC value
+			k = j = strlen(temp2);
+			while(j-- > 0)
+			{
+				temp2[4-k+j] = temp2[j];
+			}
+
+			while(k < 4)
+				temp2[3-k++]='0';
+			temp2[4]='\0';
+
+			AtoH(this_char, temp, 2);
+			eepAddr = *temp*256 + temp[1];
+
+			AtoH(temp2, temp, 2);
+			eepValue = *temp*256 + temp[1];
+
+			RT28xx_EEPROM_WRITE16(pAdapter, eepAddr, eepValue);
+			sprintf(msg+strlen(msg), "[0x%02X]:%02X  ", eepAddr, eepValue);
+		}
+	}
+next:
+	if(strlen(msg) == 1)
+		sprintf(msg+strlen(msg), "===>Error command format!");
+
+
+	// Copy the information into the user buffer
+	wrq->u.data.length = strlen(msg);
+	Status = copy_to_user(wrq->u.data.pointer, msg, wrq->u.data.length);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<==RTMPIoctlE2PROM\n"));
+}
+#endif // DBG //
+
+
+
+
+INT Set_TGnWifiTest_Proc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          arg)
+{
+    if (simple_strtol(arg, 0, 10) == 0)
+        pAd->StaCfg.bTGnWifiTest = FALSE;
+    else
+        pAd->StaCfg.bTGnWifiTest = TRUE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("IF Set_TGnWifiTest_Proc::(bTGnWifiTest=%d)\n", pAd->StaCfg.bTGnWifiTest));
+	return TRUE;
+}
+
+INT Set_LongRetryLimit_Proc(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PUCHAR			arg)
+{
+	TX_RTY_CFG_STRUC	tx_rty_cfg;
+	UCHAR				LongRetryLimit = (UCHAR)simple_strtol(arg, 0, 10);
+
+	RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+	tx_rty_cfg.field.LongRtyLimit = LongRetryLimit;
+	RTMP_IO_WRITE32(pAdapter, TX_RTY_CFG, tx_rty_cfg.word);
+	DBGPRINT(RT_DEBUG_TRACE, ("IF Set_LongRetryLimit_Proc::(tx_rty_cfg=0x%x)\n", tx_rty_cfg.word));
+	return TRUE;
+}
+
+INT Set_ShortRetryLimit_Proc(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PUCHAR			arg)
+{
+	TX_RTY_CFG_STRUC	tx_rty_cfg;
+	UCHAR				ShortRetryLimit = (UCHAR)simple_strtol(arg, 0, 10);
+
+	RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+	tx_rty_cfg.field.ShortRtyLimit = ShortRetryLimit;
+	RTMP_IO_WRITE32(pAdapter, TX_RTY_CFG, tx_rty_cfg.word);
+	DBGPRINT(RT_DEBUG_TRACE, ("IF Set_ShortRetryLimit_Proc::(tx_rty_cfg=0x%x)\n", tx_rty_cfg.word));
+	return TRUE;
+}
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+INT Set_Ieee80211dClientMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    if (simple_strtol(arg, 0, 10) == 0)
+        pAdapter->StaCfg.IEEE80211dClientMode = Rt802_11_D_None;
+    else if (simple_strtol(arg, 0, 10) == 1)
+        pAdapter->StaCfg.IEEE80211dClientMode = Rt802_11_D_Flexible;
+    else if (simple_strtol(arg, 0, 10) == 2)
+        pAdapter->StaCfg.IEEE80211dClientMode = Rt802_11_D_Strict;
+    else
+        return FALSE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_Ieee802dMode_Proc::(IEEEE0211dMode=%d)\n", pAdapter->StaCfg.IEEE80211dClientMode));
+    return TRUE;
+}
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+#ifdef CARRIER_DETECTION_SUPPORT
+INT Set_CarrierDetect_Proc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          arg)
+{
+    if (simple_strtol(arg, 0, 10) == 0)
+        pAd->CommonCfg.CarrierDetect.Enable = FALSE;
+    else
+        pAd->CommonCfg.CarrierDetect.Enable = TRUE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("IF Set_CarrierDetect_Proc::(CarrierDetect.Enable=%d)\n", pAd->CommonCfg.CarrierDetect.Enable));
+	return TRUE;
+}
+#endif // CARRIER_DETECTION_SUPPORT //
+
+
+INT	Show_Adhoc_MacTable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PCHAR			extra)
+{
+	INT i;
+
+	sprintf(extra, "\n");
+
+#ifdef DOT11_N_SUPPORT
+	sprintf(extra, "%sHT Operating Mode : %d\n", extra, pAd->CommonCfg.AddHTInfo.AddHtInfo2.OperaionMode);
+#endif // DOT11_N_SUPPORT //
+
+	sprintf(extra, "%s\n%-19s%-4s%-4s%-7s%-7s%-7s%-10s%-6s%-6s%-6s%-6s\n", extra,
+			"MAC", "AID", "BSS", "RSSI0", "RSSI1", "RSSI2", "PhMd", "BW", "MCS", "SGI", "STBC");
+
+	for (i=1; i<MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		PMAC_TABLE_ENTRY pEntry = &pAd->MacTab.Content[i];
+
+		if (strlen(extra) > (IW_PRIV_SIZE_MASK - 30))
+		    break;
+		if ((pEntry->ValidAsCLI || pEntry->ValidAsApCli) && (pEntry->Sst == SST_ASSOC))
+		{
+			sprintf(extra, "%s%02X:%02X:%02X:%02X:%02X:%02X  ", extra,
+				pEntry->Addr[0], pEntry->Addr[1], pEntry->Addr[2],
+				pEntry->Addr[3], pEntry->Addr[4], pEntry->Addr[5]);
+			sprintf(extra, "%s%-4d", extra, (int)pEntry->Aid);
+			sprintf(extra, "%s%-4d", extra, (int)pEntry->apidx);
+			sprintf(extra, "%s%-7d", extra, pEntry->RssiSample.AvgRssi0);
+			sprintf(extra, "%s%-7d", extra, pEntry->RssiSample.AvgRssi1);
+			sprintf(extra, "%s%-7d", extra, pEntry->RssiSample.AvgRssi2);
+			sprintf(extra, "%s%-10s", extra, GetPhyMode(pEntry->HTPhyMode.field.MODE));
+			sprintf(extra, "%s%-6s", extra, GetBW(pEntry->HTPhyMode.field.BW));
+			sprintf(extra, "%s%-6d", extra, pEntry->HTPhyMode.field.MCS);
+			sprintf(extra, "%s%-6d", extra, pEntry->HTPhyMode.field.ShortGI);
+			sprintf(extra, "%s%-6d", extra, pEntry->HTPhyMode.field.STBC);
+			sprintf(extra, "%s%-10d, %d, %d%%\n", extra, pEntry->DebugFIFOCount, pEntry->DebugTxCount,
+						(pEntry->DebugTxCount) ? ((pEntry->DebugTxCount-pEntry->DebugFIFOCount)*100/pEntry->DebugTxCount) : 0);
+			sprintf(extra, "%s\n", extra);
+		}
+	}
+
+	return TRUE;
+}
+
+
diff --git a/drivers/staging/rt2870/tmp61 b/drivers/staging/rt2870/tmp61
new file mode 100644
index 00000000000..992096a248c
--- /dev/null
+++ b/drivers/staging/rt2870/tmp61
@@ -0,0 +1,7037 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+    Module Name:
+    sta_ioctl.c
+
+    Abstract:
+    IOCTL related subroutines
+
+    Revision History:
+    Who         When          What
+    --------    ----------    ----------------------------------------------
+    Rory Chen   01-03-2003    created
+	Rory Chen   02-14-2005    modify to support RT61
+*/
+
+#include	"rt_config.h"
+
+#ifdef DBG
+extern ULONG    RTDebugLevel;
+#endif
+
+#define NR_WEP_KEYS 				4
+#define WEP_SMALL_KEY_LEN 			(40/8)
+#define WEP_LARGE_KEY_LEN 			(104/8)
+
+#define GROUP_KEY_NO                4
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
+#define IWE_STREAM_ADD_EVENT(_A, _B, _C, _D, _E)		iwe_stream_add_event(_A, _B, _C, _D, _E)
+#define IWE_STREAM_ADD_POINT(_A, _B, _C, _D, _E)		iwe_stream_add_point(_A, _B, _C, _D, _E)
+#define IWE_STREAM_ADD_VALUE(_A, _B, _C, _D, _E, _F)	iwe_stream_add_value(_A, _B, _C, _D, _E, _F)
+#else
+#define IWE_STREAM_ADD_EVENT(_A, _B, _C, _D, _E)		iwe_stream_add_event(_B, _C, _D, _E)
+#define IWE_STREAM_ADD_POINT(_A, _B, _C, _D, _E)		iwe_stream_add_point(_B, _C, _D, _E)
+#define IWE_STREAM_ADD_VALUE(_A, _B, _C, _D, _E, _F)	iwe_stream_add_value(_B, _C, _D, _E, _F)
+#endif
+
+extern UCHAR    CipherWpa2Template[];
+extern UCHAR    CipherWpaPskTkip[];
+extern UCHAR    CipherWpaPskTkipLen;
+
+typedef struct PACKED _RT_VERSION_INFO{
+    UCHAR       DriverVersionW;
+    UCHAR       DriverVersionX;
+    UCHAR       DriverVersionY;
+    UCHAR       DriverVersionZ;
+    UINT        DriverBuildYear;
+    UINT        DriverBuildMonth;
+    UINT        DriverBuildDay;
+} RT_VERSION_INFO, *PRT_VERSION_INFO;
+
+struct iw_priv_args privtab[] = {
+{ RTPRIV_IOCTL_SET,
+  IW_PRIV_TYPE_CHAR | 1024, 0,
+  "set"},
+
+{ RTPRIV_IOCTL_SHOW, 0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK,
+  ""},
+{ RTPRIV_IOCTL_SHOW, IW_PRIV_TYPE_CHAR | 1024, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK,
+  ""},
+/* --- sub-ioctls definitions --- */
+    { SHOW_CONN_STATUS,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "connStatus" },
+	{ SHOW_DRVIER_VERION,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "driverVer" },
+    { SHOW_BA_INFO,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "bainfo" },
+	{ SHOW_DESC_INFO,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "descinfo" },
+    { RAIO_OFF,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "radio_off" },
+	{ RAIO_ON,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "radio_on" },
+#ifdef QOS_DLS_SUPPORT
+	{ SHOW_DLS_ENTRY_INFO,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "dlsentryinfo" },
+#endif // QOS_DLS_SUPPORT //
+	{ SHOW_CFG_VALUE,
+	  IW_PRIV_TYPE_CHAR | 1024, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "show" },
+	{ SHOW_ADHOC_ENTRY_INFO,
+	  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, "adhocEntry" },
+
+/* --- sub-ioctls relations --- */
+
+#ifdef DBG
+{ RTPRIV_IOCTL_BBP,
+  IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK,
+  "bbp"},
+{ RTPRIV_IOCTL_MAC,
+  IW_PRIV_TYPE_CHAR | 1024, IW_PRIV_TYPE_CHAR | 1024,
+  "mac"},
+{ RTPRIV_IOCTL_E2P,
+  IW_PRIV_TYPE_CHAR | 1024, IW_PRIV_TYPE_CHAR | 1024,
+  "e2p"},
+#endif  /* DBG */
+
+{ RTPRIV_IOCTL_STATISTICS,
+  0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_MASK,
+  "stat"},
+{ RTPRIV_IOCTL_GSITESURVEY,
+  0, IW_PRIV_TYPE_CHAR | 1024,
+  "get_site_survey"},
+};
+
+INT Set_SSID_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+#ifdef WMM_SUPPORT
+INT	Set_WmmCapable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif
+
+INT Set_NetworkType_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_AuthMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_EncrypType_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_DefaultKeyID_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_Key1_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_Key2_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_Key3_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_Key4_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+INT Set_WPAPSK_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+
+INT Set_PSMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+INT Set_Wpa_Support(
+    IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg);
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef DBG
+VOID RTMPIoctlBBP(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq);
+
+VOID RTMPIoctlMAC(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq);
+
+VOID RTMPIoctlE2PROM(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  struct iwreq    *wrq);
+#endif // DBG //
+
+
+NDIS_STATUS RTMPWPANoneAddKeyProc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN	PVOID			pBuf);
+
+INT Set_FragTest_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+
+#ifdef DOT11_N_SUPPORT
+INT Set_TGnWifiTest_Proc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          arg);
+#endif // DOT11_N_SUPPORT //
+
+INT Set_LongRetryLimit_Proc(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PUCHAR			arg);
+
+INT Set_ShortRetryLimit_Proc(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PUCHAR			arg);
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+INT Set_Ieee80211dClientMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg);
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+#ifdef CARRIER_DETECTION_SUPPORT
+INT Set_CarrierDetect_Proc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          arg);
+#endif // CARRIER_DETECTION_SUPPORT //
+
+INT	Show_Adhoc_MacTable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PCHAR			extra);
+
+static struct {
+	CHAR *name;
+	INT (*set_proc)(PRTMP_ADAPTER pAdapter, PUCHAR arg);
+} *PRTMP_PRIVATE_SET_PROC, RTMP_PRIVATE_SUPPORT_PROC[] = {
+	{"DriverVersion",				Set_DriverVersion_Proc},
+	{"CountryRegion",				Set_CountryRegion_Proc},
+	{"CountryRegionABand",			Set_CountryRegionABand_Proc},
+	{"SSID",						Set_SSID_Proc},
+	{"WirelessMode",				Set_WirelessMode_Proc},
+	{"TxBurst",					Set_TxBurst_Proc},
+	{"TxPreamble",				Set_TxPreamble_Proc},
+	{"TxPower",					Set_TxPower_Proc},
+	{"Channel",					Set_Channel_Proc},
+	{"BGProtection",				Set_BGProtection_Proc},
+	{"RTSThreshold",				Set_RTSThreshold_Proc},
+	{"FragThreshold",				Set_FragThreshold_Proc},
+#ifdef DOT11_N_SUPPORT
+	{"HtBw",		                Set_HtBw_Proc},
+	{"HtMcs",		                Set_HtMcs_Proc},
+	{"HtGi",		                Set_HtGi_Proc},
+	{"HtOpMode",		            Set_HtOpMode_Proc},
+	{"HtExtcha",		            Set_HtExtcha_Proc},
+	{"HtMpduDensity",		        Set_HtMpduDensity_Proc},
+	{"HtBaWinSize",		        	Set_HtBaWinSize_Proc},
+	{"HtRdg",		        		Set_HtRdg_Proc},
+	{"HtAmsdu",		        		Set_HtAmsdu_Proc},
+	{"HtAutoBa",		        	Set_HtAutoBa_Proc},
+	{"HtBaDecline",					Set_BADecline_Proc},
+	{"HtProtect",		        	Set_HtProtect_Proc},
+	{"HtMimoPs",		        	Set_HtMimoPs_Proc},
+#endif // DOT11_N_SUPPORT //
+
+#ifdef AGGREGATION_SUPPORT
+	{"PktAggregate",				Set_PktAggregate_Proc},
+#endif
+
+#ifdef WMM_SUPPORT
+	{"WmmCapable",					Set_WmmCapable_Proc},
+#endif
+	{"IEEE80211H",					Set_IEEE80211H_Proc},
+    {"NetworkType",                 Set_NetworkType_Proc},
+	{"AuthMode",					Set_AuthMode_Proc},
+	{"EncrypType",					Set_EncrypType_Proc},
+	{"DefaultKeyID",				Set_DefaultKeyID_Proc},
+	{"Key1",						Set_Key1_Proc},
+	{"Key2",						Set_Key2_Proc},
+	{"Key3",						Set_Key3_Proc},
+	{"Key4",						Set_Key4_Proc},
+	{"WPAPSK",						Set_WPAPSK_Proc},
+	{"ResetCounter",				Set_ResetStatCounter_Proc},
+	{"PSMode",                      Set_PSMode_Proc},
+#ifdef DBG
+	{"Debug",						Set_Debug_Proc},
+#endif
+
+#ifdef RALINK_ATE
+	{"ATE",							Set_ATE_Proc},
+	{"ATEDA",						Set_ATE_DA_Proc},
+	{"ATESA",						Set_ATE_SA_Proc},
+	{"ATEBSSID",					Set_ATE_BSSID_Proc},
+	{"ATECHANNEL",					Set_ATE_CHANNEL_Proc},
+	{"ATETXPOW0",					Set_ATE_TX_POWER0_Proc},
+	{"ATETXPOW1",					Set_ATE_TX_POWER1_Proc},
+	{"ATETXANT",					Set_ATE_TX_Antenna_Proc},
+	{"ATERXANT",					Set_ATE_RX_Antenna_Proc},
+	{"ATETXFREQOFFSET",				Set_ATE_TX_FREQOFFSET_Proc},
+	{"ATETXBW",						Set_ATE_TX_BW_Proc},
+	{"ATETXLEN",					Set_ATE_TX_LENGTH_Proc},
+	{"ATETXCNT",					Set_ATE_TX_COUNT_Proc},
+	{"ATETXMCS",					Set_ATE_TX_MCS_Proc},
+	{"ATETXMODE",					Set_ATE_TX_MODE_Proc},
+	{"ATETXGI",						Set_ATE_TX_GI_Proc},
+	{"ATERXFER",					Set_ATE_RX_FER_Proc},
+	{"ATERRF",						Set_ATE_Read_RF_Proc},
+	{"ATEWRF1",						Set_ATE_Write_RF1_Proc},
+	{"ATEWRF2",						Set_ATE_Write_RF2_Proc},
+	{"ATEWRF3",						Set_ATE_Write_RF3_Proc},
+	{"ATEWRF4",						Set_ATE_Write_RF4_Proc},
+	{"ATELDE2P",				    Set_ATE_Load_E2P_Proc},
+	{"ATERE2P",						Set_ATE_Read_E2P_Proc},
+	{"ATESHOW",						Set_ATE_Show_Proc},
+	{"ATEHELP",						Set_ATE_Help_Proc},
+
+#ifdef RALINK_28xx_QA
+	{"TxStop",						Set_TxStop_Proc},
+	{"RxStop",						Set_RxStop_Proc},
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+    {"WpaSupport",                  Set_Wpa_Support},
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+
+
+	{"FixedTxMode",                 Set_FixedTxMode_Proc},
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+	{"OpMode",						Set_OpMode_Proc},
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+#ifdef DOT11_N_SUPPORT
+    {"TGnWifiTest",                 Set_TGnWifiTest_Proc},
+    {"ForceGF",		        		Set_ForceGF_Proc},
+#endif // DOT11_N_SUPPORT //
+#ifdef QOS_DLS_SUPPORT
+	{"DlsAddEntry",					Set_DlsAddEntry_Proc},
+	{"DlsTearDownEntry",			Set_DlsTearDownEntry_Proc},
+#endif // QOS_DLS_SUPPORT //
+	{"LongRetry",	        		Set_LongRetryLimit_Proc},
+	{"ShortRetry",	        		Set_ShortRetryLimit_Proc},
+#ifdef EXT_BUILD_CHANNEL_LIST
+	{"11dClientMode",				Set_Ieee80211dClientMode_Proc},
+#endif // EXT_BUILD_CHANNEL_LIST //
+#ifdef CARRIER_DETECTION_SUPPORT
+	{"CarrierDetect",				Set_CarrierDetect_Proc},
+#endif // CARRIER_DETECTION_SUPPORT //
+
+	{NULL,}
+};
+
+
+VOID RTMPAddKey(
+	IN	PRTMP_ADAPTER	    pAd,
+	IN	PNDIS_802_11_KEY    pKey)
+{
+	ULONG				KeyIdx;
+	MAC_TABLE_ENTRY  	*pEntry;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("RTMPAddKey ------>\n"));
+
+	if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+	{
+		if (pKey->KeyIndex & 0x80000000)
+		{
+		    if (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+            {
+                NdisZeroMemory(pAd->StaCfg.PMK, 32);
+                NdisMoveMemory(pAd->StaCfg.PMK, pKey->KeyMaterial, pKey->KeyLength);
+                goto end;
+            }
+		    // Update PTK
+		    NdisZeroMemory(&pAd->SharedKey[BSS0][0], sizeof(CIPHER_KEY));
+            pAd->SharedKey[BSS0][0].KeyLen = LEN_TKIP_EK;
+            NdisMoveMemory(pAd->SharedKey[BSS0][0].Key, pKey->KeyMaterial, LEN_TKIP_EK);
+#ifdef WPA_SUPPLICANT_SUPPORT
+            if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
+            {
+                NdisMoveMemory(pAd->SharedKey[BSS0][0].RxMic, pKey->KeyMaterial + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+                NdisMoveMemory(pAd->SharedKey[BSS0][0].TxMic, pKey->KeyMaterial + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+            }
+            else
+#endif // WPA_SUPPLICANT_SUPPORT //
+            {
+            	NdisMoveMemory(pAd->SharedKey[BSS0][0].TxMic, pKey->KeyMaterial + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+                NdisMoveMemory(pAd->SharedKey[BSS0][0].RxMic, pKey->KeyMaterial + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+            }
+
+            // Decide its ChiperAlg
+        	if (pAd->StaCfg.PairCipher == Ndis802_11Encryption2Enabled)
+        		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_TKIP;
+        	else if (pAd->StaCfg.PairCipher == Ndis802_11Encryption3Enabled)
+        		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_AES;
+        	else
+        		pAd->SharedKey[BSS0][0].CipherAlg = CIPHER_NONE;
+
+            // Update these related information to MAC_TABLE_ENTRY
+        	pEntry = &pAd->MacTab.Content[BSSID_WCID];
+            NdisMoveMemory(pEntry->PairwiseKey.Key, pAd->SharedKey[BSS0][0].Key, LEN_TKIP_EK);
+        	NdisMoveMemory(pEntry->PairwiseKey.RxMic, pAd->SharedKey[BSS0][0].RxMic, LEN_TKIP_RXMICK);
+        	NdisMoveMemory(pEntry->PairwiseKey.TxMic, pAd->SharedKey[BSS0][0].TxMic, LEN_TKIP_TXMICK);
+        	pEntry->PairwiseKey.CipherAlg = pAd->SharedKey[BSS0][0].CipherAlg;
+
+        	// Update pairwise key information to ASIC Shared Key Table
+        	AsicAddSharedKeyEntry(pAd,
+        						  BSS0,
+        						  0,
+        						  pAd->SharedKey[BSS0][0].CipherAlg,
+        						  pAd->SharedKey[BSS0][0].Key,
+        						  pAd->SharedKey[BSS0][0].TxMic,
+        						  pAd->SharedKey[BSS0][0].RxMic);
+
+        	// Update ASIC WCID attribute table and IVEIV table
+        	RTMPAddWcidAttributeEntry(pAd,
+        							  BSS0,
+        							  0,
+        							  pAd->SharedKey[BSS0][0].CipherAlg,
+        							  pEntry);
+
+            if (pAd->StaCfg.AuthMode >= Ndis802_11AuthModeWPA2)
+            {
+                // set 802.1x port control
+	            //pAd->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+				STA_PORT_SECURED(pAd);
+
+                // Indicate Connected for GUI
+                pAd->IndicateMediaState = NdisMediaStateConnected;
+            }
+		}
+        else
+        {
+            // Update GTK
+            pAd->StaCfg.DefaultKeyId = (pKey->KeyIndex & 0xFF);
+            NdisZeroMemory(&pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId], sizeof(CIPHER_KEY));
+            pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].KeyLen = LEN_TKIP_EK;
+            NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key, pKey->KeyMaterial, LEN_TKIP_EK);
+#ifdef WPA_SUPPLICANT_SUPPORT
+            if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption2Enabled)
+            {
+                NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic, pKey->KeyMaterial + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+                NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic, pKey->KeyMaterial + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+            }
+            else
+#endif // WPA_SUPPLICANT_SUPPORT //
+            {
+            	NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic, pKey->KeyMaterial + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+                NdisMoveMemory(pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic, pKey->KeyMaterial + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+            }
+
+            // Update Shared Key CipherAlg
+    		pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_NONE;
+    		if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption2Enabled)
+    			pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_TKIP;
+    		else if (pAd->StaCfg.GroupCipher == Ndis802_11Encryption3Enabled)
+    			pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg = CIPHER_AES;
+
+            // Update group key information to ASIC Shared Key Table
+        	AsicAddSharedKeyEntry(pAd,
+        						  BSS0,
+        						  pAd->StaCfg.DefaultKeyId,
+        						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
+        						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].Key,
+        						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].TxMic,
+        						  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].RxMic);
+
+        	// Update ASIC WCID attribute table and IVEIV table
+        	RTMPAddWcidAttributeEntry(pAd,
+        							  BSS0,
+        							  pAd->StaCfg.DefaultKeyId,
+        							  pAd->SharedKey[BSS0][pAd->StaCfg.DefaultKeyId].CipherAlg,
+        							  NULL);
+
+            // set 802.1x port control
+	        //pAd->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+			STA_PORT_SECURED(pAd);
+
+            // Indicate Connected for GUI
+            pAd->IndicateMediaState = NdisMediaStateConnected;
+        }
+	}
+	else	// dynamic WEP from wpa_supplicant
+	{
+		UCHAR	CipherAlg;
+    	PUCHAR	Key;
+
+		if(pKey->KeyLength == 32)
+			goto end;
+
+		KeyIdx = pKey->KeyIndex & 0x0fffffff;
+
+		if (KeyIdx < 4)
+		{
+			// it is a default shared key, for Pairwise key setting
+			if (pKey->KeyIndex & 0x80000000)
+			{
+				pEntry = MacTableLookup(pAd, pKey->BSSID);
+
+				if (pEntry)
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("RTMPAddKey: Set Pair-wise Key\n"));
+
+					// set key material and key length
+ 					pEntry->PairwiseKey.KeyLen = (UCHAR)pKey->KeyLength;
+					NdisMoveMemory(pEntry->PairwiseKey.Key, &pKey->KeyMaterial, pKey->KeyLength);
+
+					// set Cipher type
+					if (pKey->KeyLength == 5)
+						pEntry->PairwiseKey.CipherAlg = CIPHER_WEP64;
+					else
+						pEntry->PairwiseKey.CipherAlg = CIPHER_WEP128;
+
+					// Add Pair-wise key to Asic
+					AsicAddPairwiseKeyEntry(
+						pAd,
+						pEntry->Addr,
+						(UCHAR)pEntry->Aid,
+                		&pEntry->PairwiseKey);
+
+					// update WCID attribute table and IVEIV table for this entry
+					RTMPAddWcidAttributeEntry(
+						pAd,
+						BSS0,
+						KeyIdx, // The value may be not zero
+						pEntry->PairwiseKey.CipherAlg,
+						pEntry);
+
+				}
+			}
+			else
+            {
+				// Default key for tx (shared key)
+				pAd->StaCfg.DefaultKeyId = (UCHAR) KeyIdx;
+
+				// set key material and key length
+				pAd->SharedKey[BSS0][KeyIdx].KeyLen = (UCHAR) pKey->KeyLength;
+				NdisMoveMemory(pAd->SharedKey[BSS0][KeyIdx].Key, &pKey->KeyMaterial, pKey->KeyLength);
+
+				// Set Ciper type
+				if (pKey->KeyLength == 5)
+					pAd->SharedKey[BSS0][KeyIdx].CipherAlg = CIPHER_WEP64;
+				else
+					pAd->SharedKey[BSS0][KeyIdx].CipherAlg = CIPHER_WEP128;
+
+    			CipherAlg = pAd->SharedKey[BSS0][KeyIdx].CipherAlg;
+    			Key = pAd->SharedKey[BSS0][KeyIdx].Key;
+
+				// Set Group key material to Asic
+    			AsicAddSharedKeyEntry(pAd, BSS0, KeyIdx, CipherAlg, Key, NULL, NULL);
+
+				// Update WCID attribute table and IVEIV table for this group key table
+				RTMPAddWcidAttributeEntry(pAd, BSS0, KeyIdx, CipherAlg, NULL);
+
+			}
+		}
+	}
+end:
+	return;
+}
+
+char * rtstrchr(const char * s, int c)
+{
+    for(; *s != (char) c; ++s)
+        if (*s == '\0')
+            return NULL;
+    return (char *) s;
+}
+
+/*
+This is required for LinEX2004/kernel2.6.7 to provide iwlist scanning function
+*/
+
+int
+rt_ioctl_giwname(struct net_device *dev,
+		   struct iw_request_info *info,
+		   char *name, char *extra)
+{
+//	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+
+#ifdef RT2870
+	strncpy(name, "RT2870 Wireless", IFNAMSIZ);
+#endif // RT2870 //
+	return 0;
+}
+
+int rt_ioctl_siwfreq(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_freq *freq, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+	int 	chan = -1;
+
+    //check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        return -ENETDOWN;
+    }
+
+
+	if (freq->e > 1)
+		return -EINVAL;
+
+	if((freq->e == 0) && (freq->m <= 1000))
+		chan = freq->m;	// Setting by channel number
+	else
+		MAP_KHZ_TO_CHANNEL_ID( (freq->m /100) , chan); // Setting by frequency - search the table , like 2.412G, 2.422G,
+
+    if (ChannelSanity(pAdapter, chan) == TRUE)
+    {
+	pAdapter->CommonCfg.Channel = chan;
+	DBGPRINT(RT_DEBUG_ERROR, ("==>rt_ioctl_siwfreq::SIOCSIWFREQ[cmd=0x%x] (Channel=%d)\n", SIOCSIWFREQ, pAdapter->CommonCfg.Channel));
+    }
+    else
+        return -EINVAL;
+
+	return 0;
+}
+int rt_ioctl_giwfreq(struct net_device *dev,
+		   struct iw_request_info *info,
+		   struct iw_freq *freq, char *extra)
+{
+    VIRTUAL_ADAPTER *pVirtualAd = NULL;
+	PRTMP_ADAPTER pAdapter = NULL;
+	UCHAR ch;
+	ULONG	m;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+		ch = pAdapter->CommonCfg.Channel;
+
+	DBGPRINT(RT_DEBUG_TRACE,("==>rt_ioctl_giwfreq  %d\n", ch));
+
+    MAP_CHANNEL_ID_TO_KHZ(ch, m);
+	freq->m = m * 100;
+	freq->e = 1;
+	return 0;
+}
+
+int rt_ioctl_siwmode(struct net_device *dev,
+		   struct iw_request_info *info,
+		   __u32 *mode, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+
+	//check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+    	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+       	return -ENETDOWN;
+    }
+
+	switch (*mode)
+	{
+		case IW_MODE_ADHOC:
+			Set_NetworkType_Proc(pAdapter, "Adhoc");
+			break;
+		case IW_MODE_INFRA:
+			Set_NetworkType_Proc(pAdapter, "Infra");
+			break;
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,4,20))
+        case IW_MODE_MONITOR:
+			Set_NetworkType_Proc(pAdapter, "Monitor");
+			break;
+#endif
+		default:
+			DBGPRINT(RT_DEBUG_TRACE, ("===>rt_ioctl_siwmode::SIOCSIWMODE (unknown %d)\n", *mode));
+			return -EINVAL;
+	}
+
+	// Reset Ralink supplicant to not use, it will be set to start when UI set PMK key
+	pAdapter->StaCfg.WpaState = SS_NOTUSE;
+
+	return 0;
+}
+
+int rt_ioctl_giwmode(struct net_device *dev,
+		   struct iw_request_info *info,
+		   __u32 *mode, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	if (ADHOC_ON(pAdapter))
+		*mode = IW_MODE_ADHOC;
+    else if (INFRA_ON(pAdapter))
+		*mode = IW_MODE_INFRA;
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,4,20))
+    else if (MONITOR_ON(pAdapter))
+    {
+        *mode = IW_MODE_MONITOR;
+    }
+#endif
+    else
+        *mode = IW_MODE_AUTO;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("==>rt_ioctl_giwmode(mode=%d)\n", *mode));
+	return 0;
+}
+
+int rt_ioctl_siwsens(struct net_device *dev,
+		   struct iw_request_info *info,
+		   char *name, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+
+	//check if the interface is down
+    	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    	{
+        	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        	return -ENETDOWN;
+    	}
+
+	return 0;
+}
+
+int rt_ioctl_giwsens(struct net_device *dev,
+		   struct iw_request_info *info,
+		   char *name, char *extra)
+{
+	return 0;
+}
+
+int rt_ioctl_giwrange(struct net_device *dev,
+		   struct iw_request_info *info,
+		   struct iw_point *data, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+	struct iw_range *range = (struct iw_range *) extra;
+	u16 val;
+	int i;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE ,("===>rt_ioctl_giwrange\n"));
+	data->length = sizeof(struct iw_range);
+	memset(range, 0, sizeof(struct iw_range));
+
+	range->txpower_capa = IW_TXPOW_DBM;
+
+	if (INFRA_ON(pAdapter)||ADHOC_ON(pAdapter))
+	{
+		range->min_pmp = 1 * 1024;
+		range->max_pmp = 65535 * 1024;
+		range->min_pmt = 1 * 1024;
+		range->max_pmt = 1000 * 1024;
+		range->pmp_flags = IW_POWER_PERIOD;
+		range->pmt_flags = IW_POWER_TIMEOUT;
+		range->pm_capa = IW_POWER_PERIOD | IW_POWER_TIMEOUT |
+			IW_POWER_UNICAST_R | IW_POWER_ALL_R;
+	}
+
+	range->we_version_compiled = WIRELESS_EXT;
+	range->we_version_source = 14;
+
+	range->retry_capa = IW_RETRY_LIMIT;
+	range->retry_flags = IW_RETRY_LIMIT;
+	range->min_retry = 0;
+	range->max_retry = 255;
+
+	range->num_channels =  pAdapter->ChannelListNum;
+
+	val = 0;
+	for (i = 1; i <= range->num_channels; i++)
+	{
+		u32 m;
+		range->freq[val].i = pAdapter->ChannelList[i-1].Channel;
+		MAP_CHANNEL_ID_TO_KHZ(pAdapter->ChannelList[i-1].Channel, m);
+		range->freq[val].m = m * 100; /* HZ */
+
+		range->freq[val].e = 1;
+		val++;
+		if (val == IW_MAX_FREQUENCIES)
+			break;
+	}
+	range->num_frequency = val;
+
+	range->max_qual.qual = 100; /* what is correct max? This was not
+					* documented exactly. At least
+					* 69 has been observed. */
+	range->max_qual.level = 0; /* dB */
+	range->max_qual.noise = 0; /* dB */
+
+	/* What would be suitable values for "average/typical" qual? */
+	range->avg_qual.qual = 20;
+	range->avg_qual.level = -60;
+	range->avg_qual.noise = -95;
+	range->sensitivity = 3;
+
+	range->max_encoding_tokens = NR_WEP_KEYS;
+	range->num_encoding_sizes = 2;
+	range->encoding_size[0] = 5;
+	range->encoding_size[1] = 13;
+
+	range->min_rts = 0;
+	range->max_rts = 2347;
+	range->min_frag = 256;
+	range->max_frag = 2346;
+
+#if WIRELESS_EXT > 17
+	/* IW_ENC_CAPA_* bit field */
+	range->enc_capa = IW_ENC_CAPA_WPA | IW_ENC_CAPA_WPA2 |
+					IW_ENC_CAPA_CIPHER_TKIP | IW_ENC_CAPA_CIPHER_CCMP;
+#endif
+
+	return 0;
+}
+
+int rt_ioctl_siwap(struct net_device *dev,
+		      struct iw_request_info *info,
+		      struct sockaddr *ap_addr, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+    NDIS_802_11_MAC_ADDRESS Bssid;
+
+	//check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+       	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+       	return -ENETDOWN;
+    }
+
+	if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+    {
+        RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+        DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+    }
+
+    // tell CNTL state machine to call NdisMSetInformationComplete() after completing
+    // this request, because this request is initiated by NDIS.
+    pAdapter->MlmeAux.CurrReqIsFromNdis = FALSE;
+	// Prevent to connect AP again in STAMlmePeriodicExec
+	pAdapter->MlmeAux.AutoReconnectSsidLen= 32;
+
+    memset(Bssid, 0, MAC_ADDR_LEN);
+    memcpy(Bssid, ap_addr->sa_data, MAC_ADDR_LEN);
+    MlmeEnqueue(pAdapter,
+                MLME_CNTL_STATE_MACHINE,
+                OID_802_11_BSSID,
+                sizeof(NDIS_802_11_MAC_ADDRESS),
+                (VOID *)&Bssid);
+
+    DBGPRINT(RT_DEBUG_TRACE, ("IOCTL::SIOCSIWAP %02x:%02x:%02x:%02x:%02x:%02x\n",
+        Bssid[0], Bssid[1], Bssid[2], Bssid[3], Bssid[4], Bssid[5]));
+
+	return 0;
+}
+
+int rt_ioctl_giwap(struct net_device *dev,
+		      struct iw_request_info *info,
+		      struct sockaddr *ap_addr, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	if (INFRA_ON(pAdapter) || ADHOC_ON(pAdapter))
+	{
+		ap_addr->sa_family = ARPHRD_ETHER;
+		memcpy(ap_addr->sa_data, &pAdapter->CommonCfg.Bssid, ETH_ALEN);
+	}
+#ifdef WPA_SUPPLICANT_SUPPORT
+    // Add for RT2870
+    else if (pAdapter->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_DISABLE)
+    {
+        ap_addr->sa_family = ARPHRD_ETHER;
+        memcpy(ap_addr->sa_data, &pAdapter->MlmeAux.Bssid, ETH_ALEN);
+    }
+#endif // WPA_SUPPLICANT_SUPPORT //
+	else
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("IOCTL::SIOCGIWAP(=EMPTY)\n"));
+		return -ENOTCONN;
+	}
+
+	return 0;
+}
+
+/*
+ * Units are in db above the noise floor. That means the
+ * rssi values reported in the tx/rx descriptors in the
+ * driver are the SNR expressed in db.
+ *
+ * If you assume that the noise floor is -95, which is an
+ * excellent assumption 99.5 % of the time, then you can
+ * derive the absolute signal level (i.e. -95 + rssi).
+ * There are some other slight factors to take into account
+ * depending on whether the rssi measurement is from 11b,
+ * 11g, or 11a.   These differences are at most 2db and
+ * can be documented.
+ *
+ * NB: various calculations are based on the orinoco/wavelan
+ *     drivers for compatibility
+ */
+static void set_quality(PRTMP_ADAPTER pAdapter,
+                        struct iw_quality *iq,
+                        signed char rssi)
+{
+	__u8 ChannelQuality;
+
+	// Normalize Rssi
+	if (rssi >= -50)
+		ChannelQuality = 100;
+	else if (rssi >= -80) // between -50 ~ -80dbm
+		ChannelQuality = (__u8)(24 + ((rssi + 80) * 26)/10);
+	else if (rssi >= -90)   // between -80 ~ -90dbm
+        ChannelQuality = (__u8)((rssi + 90) * 26)/10;
+	else
+		ChannelQuality = 0;
+
+    iq->qual = (__u8)ChannelQuality;
+
+    iq->level = (__u8)(rssi);
+    iq->noise = (pAdapter->BbpWriteLatch[66] > pAdapter->BbpTuning.FalseCcaUpperThreshold) ? ((__u8)pAdapter->BbpTuning.FalseCcaUpperThreshold) : ((__u8) pAdapter->BbpWriteLatch[66]); 	// noise level (dBm)
+    iq->noise += 256 - 143;
+    iq->updated = pAdapter->iw_stats.qual.updated;
+}
+
+int rt_ioctl_iwaplist(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_point *data, char *extra)
+{
+ 	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+
+	struct sockaddr addr[IW_MAX_AP];
+	struct iw_quality qual[IW_MAX_AP];
+	int i;
+
+   	//check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+       	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+		data->length = 0;
+		return 0;
+        //return -ENETDOWN;
+	}
+
+	for (i = 0; i <IW_MAX_AP ; i++)
+	{
+		if (i >=  pAdapter->ScanTab.BssNr)
+			break;
+		addr[i].sa_family = ARPHRD_ETHER;
+			memcpy(addr[i].sa_data, &pAdapter->ScanTab.BssEntry[i].Bssid, MAC_ADDR_LEN);
+		set_quality(pAdapter, &qual[i], pAdapter->ScanTab.BssEntry[i].Rssi);
+	}
+	data->length = i;
+	memcpy(extra, &addr, i*sizeof(addr[0]));
+	data->flags = 1;		/* signal quality present (sort of) */
+	memcpy(extra + i*sizeof(addr[0]), &qual, i*sizeof(qual[i]));
+
+	return 0;
+}
+
+#ifdef SIOCGIWSCAN
+int rt_ioctl_siwscan(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_point *data, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+
+	ULONG								Now;
+	int Status = NDIS_STATUS_SUCCESS;
+
+	//check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+		return -ENETDOWN;
+	}
+
+	if (MONITOR_ON(pAdapter))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("!!! Driver is in Monitor Mode now !!!\n"));
+        return -EINVAL;
+    }
+
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+	if (pAdapter->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_ENABLE)
+	{
+		pAdapter->StaCfg.WpaSupplicantScanCount++;
+	}
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+    pAdapter->StaCfg.bScanReqIsFromWebUI = TRUE;
+	if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+		return 0;
+	do{
+		Now = jiffies;
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+		if ((pAdapter->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_ENABLE) &&
+			(pAdapter->StaCfg.WpaSupplicantScanCount > 3))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("!!! WpaSupplicantScanCount > 3\n"));
+			Status = NDIS_STATUS_SUCCESS;
+			break;
+		}
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+		if ((OPSTATUS_TEST_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED)) &&
+			((pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
+			(pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK)) &&
+			(pAdapter->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
+		{
+			DBGPRINT(RT_DEBUG_TRACE, ("!!! Link UP, Port Not Secured! ignore this set::OID_802_11_BSSID_LIST_SCAN\n"));
+			Status = NDIS_STATUS_SUCCESS;
+			break;
+		}
+
+		if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+		{
+			RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+			DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+		}
+
+		// tell CNTL state machine to call NdisMSetInformationComplete() after completing
+		// this request, because this request is initiated by NDIS.
+		pAdapter->MlmeAux.CurrReqIsFromNdis = FALSE;
+		// Reset allowed scan retries
+		pAdapter->StaCfg.ScanCnt = 0;
+		pAdapter->StaCfg.LastScanTime = Now;
+
+		MlmeEnqueue(pAdapter,
+			MLME_CNTL_STATE_MACHINE,
+			OID_802_11_BSSID_LIST_SCAN,
+			0,
+			NULL);
+
+		Status = NDIS_STATUS_SUCCESS;
+		RT28XX_MLME_HANDLER(pAdapter);
+	}while(0);
+	return 0;
+}
+
+int rt_ioctl_giwscan(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_point *data, char *extra)
+{
+
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+	int i=0;
+	char *current_ev = extra, *previous_ev = extra;
+	char *end_buf;
+	char *current_val, custom[MAX_CUSTOM_LEN] = {0};
+#ifndef IWEVGENIE
+	char idx;
+#endif // IWEVGENIE //
+	struct iw_event iwe;
+
+	if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+    {
+		/*
+		 * Still scanning, indicate the caller should try again.
+		 */
+		return -EAGAIN;
+	}
+
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+	if (pAdapter->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_ENABLE)
+	{
+		pAdapter->StaCfg.WpaSupplicantScanCount = 0;
+	}
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+	if (pAdapter->ScanTab.BssNr == 0)
+	{
+		data->length = 0;
+		return 0;
+	}
+
+#if WIRELESS_EXT >= 17
+    if (data->length > 0)
+        end_buf = extra + data->length;
+    else
+        end_buf = extra + IW_SCAN_MAX_DATA;
+#else
+    end_buf = extra + IW_SCAN_MAX_DATA;
+#endif
+
+	for (i = 0; i < pAdapter->ScanTab.BssNr; i++)
+	{
+		if (current_ev >= end_buf)
+        {
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+        }
+
+		//MAC address
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWAP;
+		iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
+				memcpy(iwe.u.ap_addr.sa_data, &pAdapter->ScanTab.BssEntry[i].Bssid, ETH_ALEN);
+
+        previous_ev = current_ev;
+		current_ev = IWE_STREAM_ADD_EVENT(info, current_ev,end_buf, &iwe, IW_EV_ADDR_LEN);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//ESSID
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWESSID;
+		iwe.u.data.length = pAdapter->ScanTab.BssEntry[i].SsidLen;
+		iwe.u.data.flags = 1;
+
+        previous_ev = current_ev;
+		current_ev = IWE_STREAM_ADD_POINT(info, current_ev,end_buf, &iwe, pAdapter->ScanTab.BssEntry[i].Ssid);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//Network Type
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWMODE;
+		if (pAdapter->ScanTab.BssEntry[i].BssType == Ndis802_11IBSS)
+		{
+			iwe.u.mode = IW_MODE_ADHOC;
+		}
+		else if (pAdapter->ScanTab.BssEntry[i].BssType == Ndis802_11Infrastructure)
+		{
+			iwe.u.mode = IW_MODE_INFRA;
+		}
+		else
+		{
+			iwe.u.mode = IW_MODE_AUTO;
+		}
+		iwe.len = IW_EV_UINT_LEN;
+
+        previous_ev = current_ev;
+		current_ev = IWE_STREAM_ADD_EVENT(info, current_ev, end_buf, &iwe,  IW_EV_UINT_LEN);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//Channel and Frequency
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWFREQ;
+		if (INFRA_ON(pAdapter) || ADHOC_ON(pAdapter))
+			iwe.u.freq.m = pAdapter->ScanTab.BssEntry[i].Channel;
+		else
+			iwe.u.freq.m = pAdapter->ScanTab.BssEntry[i].Channel;
+		iwe.u.freq.e = 0;
+		iwe.u.freq.i = 0;
+
+		previous_ev = current_ev;
+		current_ev = IWE_STREAM_ADD_EVENT(info, current_ev,end_buf, &iwe, IW_EV_FREQ_LEN);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+        //Add quality statistics
+        //================================
+        memset(&iwe, 0, sizeof(iwe));
+    	iwe.cmd = IWEVQUAL;
+    	iwe.u.qual.level = 0;
+    	iwe.u.qual.noise = 0;
+        set_quality(pAdapter, &iwe.u.qual, pAdapter->ScanTab.BssEntry[i].Rssi);
+    	current_ev = IWE_STREAM_ADD_EVENT(info, current_ev, end_buf, &iwe, IW_EV_QUAL_LEN);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//Encyption key
+		//================================
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = SIOCGIWENCODE;
+		if (CAP_IS_PRIVACY_ON (pAdapter->ScanTab.BssEntry[i].CapabilityInfo ))
+			iwe.u.data.flags =IW_ENCODE_ENABLED | IW_ENCODE_NOKEY;
+		else
+			iwe.u.data.flags = IW_ENCODE_DISABLED;
+
+        previous_ev = current_ev;
+        current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf,&iwe, (char *)pAdapter->SharedKey[BSS0][(iwe.u.data.flags & IW_ENCODE_INDEX)-1].Key);
+        if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+            return -E2BIG;
+#else
+			break;
+#endif
+
+		//Bit Rate
+		//================================
+		if (pAdapter->ScanTab.BssEntry[i].SupRateLen)
+        {
+            UCHAR tmpRate = pAdapter->ScanTab.BssEntry[i].SupRate[pAdapter->ScanTab.BssEntry[i].SupRateLen-1];
+			memset(&iwe, 0, sizeof(iwe));
+			iwe.cmd = SIOCGIWRATE;
+    		current_val = current_ev + IW_EV_LCP_LEN;
+            if (tmpRate == 0x82)
+                iwe.u.bitrate.value =  1 * 1000000;
+            else if (tmpRate == 0x84)
+                iwe.u.bitrate.value =  2 * 1000000;
+            else if (tmpRate == 0x8B)
+                iwe.u.bitrate.value =  5.5 * 1000000;
+            else if (tmpRate == 0x96)
+                iwe.u.bitrate.value =  11 * 1000000;
+            else
+    		    iwe.u.bitrate.value =  (tmpRate/2) * 1000000;
+
+			iwe.u.bitrate.disabled = 0;
+			current_val = IWE_STREAM_ADD_VALUE(info, current_ev,
+				current_val, end_buf, &iwe,
+    			IW_EV_PARAM_LEN);
+
+        	if((current_val-current_ev)>IW_EV_LCP_LEN)
+            	current_ev = current_val;
+        	else
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+        }
+
+#ifdef IWEVGENIE
+		//WPA IE
+		if (pAdapter->ScanTab.BssEntry[i].WpaIE.IELen > 0)
+		{
+			memset(&iwe, 0, sizeof(iwe));
+			memset(&custom[0], 0, MAX_CUSTOM_LEN);
+			memcpy(custom, &(pAdapter->ScanTab.BssEntry[i].WpaIE.IE[0]),
+						   pAdapter->ScanTab.BssEntry[i].WpaIE.IELen);
+			iwe.cmd = IWEVGENIE;
+			iwe.u.data.length = pAdapter->ScanTab.BssEntry[i].WpaIE.IELen;
+			current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf, &iwe, custom);
+			if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+		}
+
+		//WPA2 IE
+        if (pAdapter->ScanTab.BssEntry[i].RsnIE.IELen > 0)
+        {
+        	memset(&iwe, 0, sizeof(iwe));
+			memset(&custom[0], 0, MAX_CUSTOM_LEN);
+			memcpy(custom, &(pAdapter->ScanTab.BssEntry[i].RsnIE.IE[0]),
+						   pAdapter->ScanTab.BssEntry[i].RsnIE.IELen);
+			iwe.cmd = IWEVGENIE;
+			iwe.u.data.length = pAdapter->ScanTab.BssEntry[i].RsnIE.IELen;
+			current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf, &iwe, custom);
+			if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+        }
+#else
+        //WPA IE
+		//================================
+        if (pAdapter->ScanTab.BssEntry[i].WpaIE.IELen > 0)
+        {
+    		NdisZeroMemory(&iwe, sizeof(iwe));
+			memset(&custom[0], 0, MAX_CUSTOM_LEN);
+    		iwe.cmd = IWEVCUSTOM;
+            iwe.u.data.length = (pAdapter->ScanTab.BssEntry[i].WpaIE.IELen * 2) + 7;
+            NdisMoveMemory(custom, "wpa_ie=", 7);
+            for (idx = 0; idx < pAdapter->ScanTab.BssEntry[i].WpaIE.IELen; idx++)
+                sprintf(custom, "%s%02x", custom, pAdapter->ScanTab.BssEntry[i].WpaIE.IE[idx]);
+            previous_ev = current_ev;
+    		current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf, &iwe,  custom);
+            if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+        }
+
+        //WPA2 IE
+        if (pAdapter->ScanTab.BssEntry[i].RsnIE.IELen > 0)
+        {
+    		NdisZeroMemory(&iwe, sizeof(iwe));
+			memset(&custom[0], 0, MAX_CUSTOM_LEN);
+    		iwe.cmd = IWEVCUSTOM;
+            iwe.u.data.length = (pAdapter->ScanTab.BssEntry[i].RsnIE.IELen * 2) + 7;
+            NdisMoveMemory(custom, "rsn_ie=", 7);
+			for (idx = 0; idx < pAdapter->ScanTab.BssEntry[i].RsnIE.IELen; idx++)
+                sprintf(custom, "%s%02x", custom, pAdapter->ScanTab.BssEntry[i].RsnIE.IE[idx]);
+            previous_ev = current_ev;
+    		current_ev = IWE_STREAM_ADD_POINT(info, current_ev, end_buf, &iwe,  custom);
+            if (current_ev == previous_ev)
+#if WIRELESS_EXT >= 17
+                return -E2BIG;
+#else
+			    break;
+#endif
+        }
+#endif // IWEVGENIE //
+	}
+
+	data->length = current_ev - extra;
+    pAdapter->StaCfg.bScanReqIsFromWebUI = FALSE;
+	DBGPRINT(RT_DEBUG_ERROR ,("===>rt_ioctl_giwscan. %d(%d) BSS returned, data->length = %d\n",i , pAdapter->ScanTab.BssNr, data->length));
+	return 0;
+}
+#endif
+
+int rt_ioctl_siwessid(struct net_device *dev,
+			 struct iw_request_info *info,
+			 struct iw_point *data, char *essid)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+
+	//check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+       	DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+       	return -ENETDOWN;
+    }
+
+	if (data->flags)
+	{
+		PCHAR	pSsidString = NULL;
+
+		// Includes null character.
+		if (data->length > (IW_ESSID_MAX_SIZE + 1))
+			return -E2BIG;
+
+		pSsidString = (CHAR *) kmalloc(MAX_LEN_OF_SSID+1, MEM_ALLOC_FLAG);
+		if (pSsidString)
+		{
+			NdisZeroMemory(pSsidString, MAX_LEN_OF_SSID+1);
+			NdisMoveMemory(pSsidString, essid, data->length);
+			if (Set_SSID_Proc(pAdapter, pSsidString) == FALSE)
+				return -EINVAL;
+		}
+		else
+			return -ENOMEM;
+	}
+	else
+	{
+		// ANY ssid
+		if (Set_SSID_Proc(pAdapter, "") == FALSE)
+			return -EINVAL;
+    }
+	return 0;
+}
+
+int rt_ioctl_giwessid(struct net_device *dev,
+			 struct iw_request_info *info,
+			 struct iw_point *data, char *essid)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	data->flags = 1;
+    if (MONITOR_ON(pAdapter))
+    {
+        data->length  = 0;
+        return 0;
+    }
+
+	if (OPSTATUS_TEST_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED))
+	{
+		DBGPRINT(RT_DEBUG_TRACE ,("MediaState is connected\n"));
+		data->length = pAdapter->CommonCfg.SsidLen;
+		memcpy(essid, pAdapter->CommonCfg.Ssid, pAdapter->CommonCfg.SsidLen);
+	}
+#ifdef RT2870
+#ifdef WPA_SUPPLICANT_SUPPORT
+    // Add for RT2870
+    else if (pAdapter->StaCfg.WpaSupplicantUP != WPA_SUPPLICANT_DISABLE)
+    {
+        data->length = pAdapter->CommonCfg.SsidLen;
+		memcpy(essid, pAdapter->CommonCfg.Ssid, pAdapter->CommonCfg.SsidLen);
+	}
+#endif // WPA_SUPPLICANT_SUPPORT //
+#endif // RT2870 //
+	else
+	{//the ANY ssid was specified
+		data->length  = 0;
+		DBGPRINT(RT_DEBUG_TRACE ,("MediaState is not connected, ess\n"));
+	}
+
+	return 0;
+
+}
+
+int rt_ioctl_siwnickn(struct net_device *dev,
+			 struct iw_request_info *info,
+			 struct iw_point *data, char *nickname)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+
+    //check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+        DBGPRINT(RT_DEBUG_TRACE ,("INFO::Network is down!\n"));
+        return -ENETDOWN;
+    }
+
+	if (data->length > IW_ESSID_MAX_SIZE)
+		return -EINVAL;
+
+	memset(pAdapter->nickname, 0, IW_ESSID_MAX_SIZE + 1);
+	memcpy(pAdapter->nickname, nickname, data->length);
+
+
+	return 0;
+}
+
+int rt_ioctl_giwnickn(struct net_device *dev,
+			 struct iw_request_info *info,
+			 struct iw_point *data, char *nickname)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	if (data->length > strlen(pAdapter->nickname) + 1)
+		data->length = strlen(pAdapter->nickname) + 1;
+	if (data->length > 0) {
+		memcpy(nickname, pAdapter->nickname, data->length-1);
+		nickname[data->length-1] = '\0';
+	}
+	return 0;
+}
+
+int rt_ioctl_siwrts(struct net_device *dev,
+		       struct iw_request_info *info,
+		       struct iw_param *rts, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+	u16 val;
+
+    //check if the interface is down
+    if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        return -ENETDOWN;
+    }
+
+	if (rts->disabled)
+		val = MAX_RTS_THRESHOLD;
+	else if (rts->value < 0 || rts->value > MAX_RTS_THRESHOLD)
+		return -EINVAL;
+	else if (rts->value == 0)
+	    val = MAX_RTS_THRESHOLD;
+	else
+		val = rts->value;
+
+	if (val != pAdapter->CommonCfg.RtsThreshold)
+		pAdapter->CommonCfg.RtsThreshold = val;
+
+	return 0;
+}
+
+int rt_ioctl_giwrts(struct net_device *dev,
+		       struct iw_request_info *info,
+		       struct iw_param *rts, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	//check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+	rts->value = pAdapter->CommonCfg.RtsThreshold;
+	rts->disabled = (rts->value == MAX_RTS_THRESHOLD);
+	rts->fixed = 1;
+
+	return 0;
+}
+
+int rt_ioctl_siwfrag(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_param *frag, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+	u16 val;
+
+	//check if the interface is down
+    	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    	{
+      		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        	return -ENETDOWN;
+    	}
+
+	if (frag->disabled)
+		val = MAX_FRAG_THRESHOLD;
+	else if (frag->value >= MIN_FRAG_THRESHOLD || frag->value <= MAX_FRAG_THRESHOLD)
+        val = __cpu_to_le16(frag->value & ~0x1); /* even numbers only */
+	else if (frag->value == 0)
+	    val = MAX_FRAG_THRESHOLD;
+	else
+		return -EINVAL;
+
+	pAdapter->CommonCfg.FragmentThreshold = val;
+	return 0;
+}
+
+int rt_ioctl_giwfrag(struct net_device *dev,
+			struct iw_request_info *info,
+			struct iw_param *frag, char *extra)
+{
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	//check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+	frag->value = pAdapter->CommonCfg.FragmentThreshold;
+	frag->disabled = (frag->value == MAX_FRAG_THRESHOLD);
+	frag->fixed = 1;
+
+	return 0;
+}
+
+#define MAX_WEP_KEY_SIZE 13
+#define MIN_WEP_KEY_SIZE 5
+int rt_ioctl_siwencode(struct net_device *dev,
+			  struct iw_request_info *info,
+			  struct iw_point *erq, char *extra)
+{
+	PRTMP_ADAPTER pAdapter = (PRTMP_ADAPTER) dev->priv;
+
+	//check if the interface is down
+    	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    	{
+      		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+        	return -ENETDOWN;
+    	}
+
+	if ((erq->length == 0) &&
+        (erq->flags & IW_ENCODE_DISABLED))
+	{
+		pAdapter->StaCfg.PairCipher = Ndis802_11WEPDisabled;
+		pAdapter->StaCfg.GroupCipher = Ndis802_11WEPDisabled;
+		pAdapter->StaCfg.WepStatus = Ndis802_11WEPDisabled;
+        pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+        goto done;
+	}
+	else if ((erq->length == 0) &&
+             (erq->flags & IW_ENCODE_RESTRICTED || erq->flags & IW_ENCODE_OPEN))
+	{
+	    //pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+		STA_PORT_SECURED(pAdapter);
+		pAdapter->StaCfg.PairCipher = Ndis802_11WEPEnabled;
+		pAdapter->StaCfg.GroupCipher = Ndis802_11WEPEnabled;
+		pAdapter->StaCfg.WepStatus = Ndis802_11WEPEnabled;
+        pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+		if (erq->flags & IW_ENCODE_RESTRICTED)
+			pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeShared;
+    	else
+			pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+        goto done;
+	}
+
+    if (erq->length > 0)
+	{
+		int keyIdx = (erq->flags & IW_ENCODE_INDEX) - 1;
+		/* Check the size of the key */
+		if (erq->length > MAX_WEP_KEY_SIZE) {
+			return -EINVAL;
+		}
+		/* Check key index */
+		if ((keyIdx < 0) || (keyIdx >= NR_WEP_KEYS))
+        {
+            DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::Wrong keyIdx=%d! Using default key instead (%d)\n",
+                                        keyIdx, pAdapter->StaCfg.DefaultKeyId));
+
+            //Using default key
+			keyIdx = pAdapter->StaCfg.DefaultKeyId;
+        }
+
+        NdisZeroMemory(pAdapter->SharedKey[BSS0][keyIdx].Key,  16);
+
+		if (erq->length == MAX_WEP_KEY_SIZE)
+        {
+			pAdapter->SharedKey[BSS0][keyIdx].KeyLen = MAX_WEP_KEY_SIZE;
+            pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_WEP128;
+		}
+		else if (erq->length == MIN_WEP_KEY_SIZE)
+        {
+            pAdapter->SharedKey[BSS0][keyIdx].KeyLen = MIN_WEP_KEY_SIZE;
+            pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_WEP64;
+		}
+		else
+			/* Disable the key */
+			pAdapter->SharedKey[BSS0][keyIdx].KeyLen = 0;
+
+		/* Check if the key is not marked as invalid */
+		if(!(erq->flags & IW_ENCODE_NOKEY)) {
+			/* Copy the key in the driver */
+			NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].Key, extra, erq->length);
+        }
+	}
+    else
+			{
+		/* Do we want to just set the transmit key index ? */
+		int index = (erq->flags & IW_ENCODE_INDEX) - 1;
+		if ((index >= 0) && (index < 4))
+        {
+			pAdapter->StaCfg.DefaultKeyId = index;
+            }
+        else
+			/* Don't complain if only change the mode */
+			if(!erq->flags & IW_ENCODE_MODE) {
+				return -EINVAL;
+		}
+	}
+
+done:
+    DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::erq->flags=%x\n",erq->flags));
+	DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::AuthMode=%x\n",pAdapter->StaCfg.AuthMode));
+	DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::DefaultKeyId=%x, KeyLen = %d\n",pAdapter->StaCfg.DefaultKeyId , pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen));
+	DBGPRINT(RT_DEBUG_TRACE ,("==>rt_ioctl_siwencode::WepStatus=%x\n",pAdapter->StaCfg.WepStatus));
+	return 0;
+}
+
+int
+rt_ioctl_giwencode(struct net_device *dev,
+			  struct iw_request_info *info,
+			  struct iw_point *erq, char *key)
+{
+	int kid;
+	PRTMP_ADAPTER 	pAdapter = NULL;
+	VIRTUAL_ADAPTER *pVirtualAd = NULL;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		if (pVirtualAd && pVirtualAd->RtmpDev)
+			pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	//check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+	kid = erq->flags & IW_ENCODE_INDEX;
+	DBGPRINT(RT_DEBUG_TRACE, ("===>rt_ioctl_giwencode %d\n", erq->flags & IW_ENCODE_INDEX));
+
+	if (pAdapter->StaCfg.WepStatus == Ndis802_11WEPDisabled)
+	{
+		erq->length = 0;
+		erq->flags = IW_ENCODE_DISABLED;
+	}
+	else if ((kid > 0) && (kid <=4))
+	{
+		// copy wep key
+		erq->flags = kid ;			/* NB: base 1 */
+		if (erq->length > pAdapter->SharedKey[BSS0][kid-1].KeyLen)
+			erq->length = pAdapter->SharedKey[BSS0][kid-1].KeyLen;
+		memcpy(key, pAdapter->SharedKey[BSS0][kid-1].Key, erq->length);
+		//if ((kid == pAdapter->PortCfg.DefaultKeyId))
+		//erq->flags |= IW_ENCODE_ENABLED;	/* XXX */
+		if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeShared)
+			erq->flags |= IW_ENCODE_RESTRICTED;		/* XXX */
+		else
+			erq->flags |= IW_ENCODE_OPEN;		/* XXX */
+
+	}
+	else if (kid == 0)
+	{
+		if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeShared)
+			erq->flags |= IW_ENCODE_RESTRICTED;		/* XXX */
+		else
+			erq->flags |= IW_ENCODE_OPEN;		/* XXX */
+		erq->length = pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen;
+		memcpy(key, pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].Key, erq->length);
+		// copy default key ID
+		if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeShared)
+			erq->flags |= IW_ENCODE_RESTRICTED;		/* XXX */
+		else
+			erq->flags |= IW_ENCODE_OPEN;		/* XXX */
+		erq->flags = pAdapter->StaCfg.DefaultKeyId + 1;			/* NB: base 1 */
+		erq->flags |= IW_ENCODE_ENABLED;	/* XXX */
+	}
+
+	return 0;
+
+}
+
+static int
+rt_ioctl_setparam(struct net_device *dev, struct iw_request_info *info,
+			 void *w, char *extra)
+{
+    VIRTUAL_ADAPTER	*pVirtualAd = NULL;
+	PRTMP_ADAPTER pAdapter;
+	POS_COOKIE pObj;
+	char *this_char = extra;
+	char *value;
+	int  Status=0;
+
+	if (dev->priv_flags == INT_MAIN)
+	{
+		pAdapter = dev->priv;
+	}
+	else
+	{
+		pVirtualAd = dev->priv;
+		pAdapter = pVirtualAd->RtmpDev->priv;
+	}
+	pObj = (POS_COOKIE) pAdapter->OS_Cookie;
+
+	if (pAdapter == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+	{
+		pObj->ioctl_if_type = INT_MAIN;
+        pObj->ioctl_if = MAIN_MBSSID;
+	}
+
+	//check if the interface is down
+    	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    	{
+      		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+			return -ENETDOWN;
+    	}
+
+	if (!*this_char)
+		return -EINVAL;
+
+	if ((value = rtstrchr(this_char, '=')) != NULL)
+	    *value++ = 0;
+
+	if (!value)
+	    return -EINVAL;
+
+	// reject setting nothing besides ANY ssid(ssidLen=0)
+    if (!*value && (strcmp(this_char, "SSID") != 0))
+        return -EINVAL;
+
+	for (PRTMP_PRIVATE_SET_PROC = RTMP_PRIVATE_SUPPORT_PROC; PRTMP_PRIVATE_SET_PROC->name; PRTMP_PRIVATE_SET_PROC++)
+	{
+	    if (strcmp(this_char, PRTMP_PRIVATE_SET_PROC->name) == 0)
+	    {
+	        if(!PRTMP_PRIVATE_SET_PROC->set_proc(pAdapter, value))
+	        {	//FALSE:Set private failed then return Invalid argument
+			    Status = -EINVAL;
+	        }
+		    break;	//Exit for loop.
+	    }
+	}
+
+	if(PRTMP_PRIVATE_SET_PROC->name == NULL)
+	{  //Not found argument
+	    Status = -EINVAL;
+	    DBGPRINT(RT_DEBUG_TRACE, ("===>rt_ioctl_setparam:: (iwpriv) Not Support Set Command [%s=%s]\n", this_char, value));
+	}
+
+    return Status;
+}
+
+
+static int
+rt_private_get_statistics(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra)
+{
+	INT				Status = 0;
+    PRTMP_ADAPTER   pAd = (PRTMP_ADAPTER) dev->priv;
+
+    if (extra == NULL)
+    {
+        wrq->length = 0;
+        return -EIO;
+    }
+
+    memset(extra, 0x00, IW_PRIV_SIZE_MASK);
+    sprintf(extra, "\n\n");
+
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+	{
+	    sprintf(extra+strlen(extra), "Tx success                      = %ld\n", (ULONG)pAd->ate.TxDoneCount);
+	    //sprintf(extra+strlen(extra), "Tx success without retry        = %ld\n", (ULONG)pAd->ate.TxDoneCount);
+	}
+	else
+#endif // RALINK_ATE //
+	{
+    sprintf(extra+strlen(extra), "Tx success                      = %ld\n", (ULONG)pAd->WlanCounters.TransmittedFragmentCount.QuadPart);
+    sprintf(extra+strlen(extra), "Tx success without retry        = %ld\n", (ULONG)pAd->WlanCounters.TransmittedFragmentCount.QuadPart - (ULONG)pAd->WlanCounters.RetryCount.QuadPart);
+	}
+    sprintf(extra+strlen(extra), "Tx success after retry          = %ld\n", (ULONG)pAd->WlanCounters.RetryCount.QuadPart);
+    sprintf(extra+strlen(extra), "Tx fail to Rcv ACK after retry  = %ld\n", (ULONG)pAd->WlanCounters.FailedCount.QuadPart);
+    sprintf(extra+strlen(extra), "RTS Success Rcv CTS             = %ld\n", (ULONG)pAd->WlanCounters.RTSSuccessCount.QuadPart);
+    sprintf(extra+strlen(extra), "RTS Fail Rcv CTS                = %ld\n", (ULONG)pAd->WlanCounters.RTSFailureCount.QuadPart);
+
+    sprintf(extra+strlen(extra), "Rx success                      = %ld\n", (ULONG)pAd->WlanCounters.ReceivedFragmentCount.QuadPart);
+    sprintf(extra+strlen(extra), "Rx with CRC                     = %ld\n", (ULONG)pAd->WlanCounters.FCSErrorCount.QuadPart);
+    sprintf(extra+strlen(extra), "Rx drop due to out of resource  = %ld\n", (ULONG)pAd->Counters8023.RxNoBuffer);
+    sprintf(extra+strlen(extra), "Rx duplicate frame              = %ld\n", (ULONG)pAd->WlanCounters.FrameDuplicateCount.QuadPart);
+
+    sprintf(extra+strlen(extra), "False CCA (one second)          = %ld\n", (ULONG)pAd->RalinkCounters.OneSecFalseCCACnt);
+#ifdef RALINK_ATE
+	if (ATE_ON(pAd))
+	{
+		if (pAd->ate.RxAntennaSel == 0)
+		{
+    		sprintf(extra+strlen(extra), "RSSI-A                          = %ld\n", (LONG)(pAd->ate.LastRssi0 - pAd->BbpRssiToDbmDelta));
+			sprintf(extra+strlen(extra), "RSSI-B (if available)           = %ld\n", (LONG)(pAd->ate.LastRssi1 - pAd->BbpRssiToDbmDelta));
+			sprintf(extra+strlen(extra), "RSSI-C (if available)           = %ld\n\n", (LONG)(pAd->ate.LastRssi2 - pAd->BbpRssiToDbmDelta));
+		}
+		else
+		{
+    		sprintf(extra+strlen(extra), "RSSI                            = %ld\n", (LONG)(pAd->ate.LastRssi0 - pAd->BbpRssiToDbmDelta));
+		}
+	}
+	else
+#endif // RALINK_ATE //
+	{
+    	sprintf(extra+strlen(extra), "RSSI-A                          = %ld\n", (LONG)(pAd->StaCfg.RssiSample.LastRssi0 - pAd->BbpRssiToDbmDelta));
+        sprintf(extra+strlen(extra), "RSSI-B (if available)           = %ld\n", (LONG)(pAd->StaCfg.RssiSample.LastRssi1 - pAd->BbpRssiToDbmDelta));
+        sprintf(extra+strlen(extra), "RSSI-C (if available)           = %ld\n\n", (LONG)(pAd->StaCfg.RssiSample.LastRssi2 - pAd->BbpRssiToDbmDelta));
+	}
+#ifdef WPA_SUPPLICANT_SUPPORT
+    sprintf(extra+strlen(extra), "WpaSupplicantUP                 = %d\n\n", pAd->StaCfg.WpaSupplicantUP);
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+
+    wrq->length = strlen(extra) + 1; // 1: size of '\0'
+    DBGPRINT(RT_DEBUG_TRACE, ("<== rt_private_get_statistics, wrq->length = %d\n", wrq->length));
+
+    return Status;
+}
+
+#ifdef DOT11_N_SUPPORT
+void	getBaInfo(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			pOutBuf)
+{
+	INT i, j;
+	BA_ORI_ENTRY *pOriBAEntry;
+	BA_REC_ENTRY *pRecBAEntry;
+
+	for (i=0; i<MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		PMAC_TABLE_ENTRY pEntry = &pAd->MacTab.Content[i];
+		if (((pEntry->ValidAsCLI || pEntry->ValidAsApCli) && (pEntry->Sst == SST_ASSOC))
+			|| (pEntry->ValidAsWDS) || (pEntry->ValidAsMesh))
+		{
+			sprintf(pOutBuf, "%s\n%02X:%02X:%02X:%02X:%02X:%02X (Aid = %d) (AP) -\n",
+                pOutBuf,
+				pEntry->Addr[0], pEntry->Addr[1], pEntry->Addr[2],
+				pEntry->Addr[3], pEntry->Addr[4], pEntry->Addr[5], pEntry->Aid);
+
+			sprintf(pOutBuf, "%s[Recipient]\n", pOutBuf);
+			for (j=0; j < NUM_OF_TID; j++)
+			{
+				if (pEntry->BARecWcidArray[j] != 0)
+				{
+					pRecBAEntry =&pAd->BATable.BARecEntry[pEntry->BARecWcidArray[j]];
+					sprintf(pOutBuf, "%sTID=%d, BAWinSize=%d, LastIndSeq=%d, ReorderingPkts=%d\n", pOutBuf, j, pRecBAEntry->BAWinSize, pRecBAEntry->LastIndSeq, pRecBAEntry->list.qlen);
+				}
+			}
+			sprintf(pOutBuf, "%s\n", pOutBuf);
+
+			sprintf(pOutBuf, "%s[Originator]\n", pOutBuf);
+			for (j=0; j < NUM_OF_TID; j++)
+			{
+				if (pEntry->BAOriWcidArray[j] != 0)
+				{
+					pOriBAEntry =&pAd->BATable.BAOriEntry[pEntry->BAOriWcidArray[j]];
+					sprintf(pOutBuf, "%sTID=%d, BAWinSize=%d, StartSeq=%d, CurTxSeq=%d\n", pOutBuf, j, pOriBAEntry->BAWinSize, pOriBAEntry->Sequence, pEntry->TxSeq[j]);
+				}
+			}
+			sprintf(pOutBuf, "%s\n\n", pOutBuf);
+		}
+        if (strlen(pOutBuf) > (IW_PRIV_SIZE_MASK - 30))
+                break;
+	}
+
+	return;
+}
+#endif // DOT11_N_SUPPORT //
+
+static int
+rt_private_show(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra)
+{
+    INT				Status = 0;
+    VIRTUAL_ADAPTER	*pVirtualAd = NULL;
+    PRTMP_ADAPTER   pAd;
+	POS_COOKIE		pObj;
+    u32             subcmd = wrq->flags;
+
+	if (dev->priv_flags == INT_MAIN)
+		pAd = dev->priv;
+	else
+	{
+		pVirtualAd = dev->priv;
+		pAd = pVirtualAd->RtmpDev->priv;
+	}
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	if (pAd == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+    if (extra == NULL)
+    {
+        wrq->length = 0;
+        return -EIO;
+    }
+    memset(extra, 0x00, IW_PRIV_SIZE_MASK);
+
+	{
+		pObj->ioctl_if_type = INT_MAIN;
+        pObj->ioctl_if = MAIN_MBSSID;
+	}
+
+    switch(subcmd)
+    {
+
+        case SHOW_CONN_STATUS:
+            if (MONITOR_ON(pAd))
+            {
+#ifdef DOT11_N_SUPPORT
+                if (pAd->CommonCfg.PhyMode >= PHY_11ABGN_MIXED &&
+                    pAd->CommonCfg.RegTransmitSetting.field.BW)
+                    sprintf(extra, "Monitor Mode(CentralChannel %d)\n", pAd->CommonCfg.CentralChannel);
+                else
+#endif // DOT11_N_SUPPORT //
+                    sprintf(extra, "Monitor Mode(Channel %d)\n", pAd->CommonCfg.Channel);
+            }
+            else
+            {
+                if (pAd->IndicateMediaState == NdisMediaStateConnected)
+            	{
+            	    if (INFRA_ON(pAd))
+                    {
+                    sprintf(extra, "Connected(AP: %s[%02X:%02X:%02X:%02X:%02X:%02X])\n",
+                                    pAd->CommonCfg.Ssid,
+                                    pAd->CommonCfg.Bssid[0],
+                                    pAd->CommonCfg.Bssid[1],
+                                    pAd->CommonCfg.Bssid[2],
+                                    pAd->CommonCfg.Bssid[3],
+                                    pAd->CommonCfg.Bssid[4],
+                                    pAd->CommonCfg.Bssid[5]);
+            		DBGPRINT(RT_DEBUG_TRACE ,("Ssid=%s ,Ssidlen = %d\n",pAd->CommonCfg.Ssid, pAd->CommonCfg.SsidLen));
+            	}
+                    else if (ADHOC_ON(pAd))
+                        sprintf(extra, "Connected\n");
+            	}
+            	else
+            	{
+            	    sprintf(extra, "Disconnected\n");
+            		DBGPRINT(RT_DEBUG_TRACE ,("ConnStatus is not connected\n"));
+            	}
+            }
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+        case SHOW_DRVIER_VERION:
+            sprintf(extra, "Driver version-%s, %s %s\n", STA_DRIVER_VERSION, __DATE__, __TIME__ );
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+#ifdef DOT11_N_SUPPORT
+        case SHOW_BA_INFO:
+            getBaInfo(pAd, extra);
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+#endif // DOT11_N_SUPPORT //
+		case SHOW_DESC_INFO:
+			{
+				Show_DescInfo_Proc(pAd, NULL);
+				wrq->length = 0; // 1: size of '\0'
+			}
+			break;
+        case RAIO_OFF:
+            if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+            {
+                sprintf(extra, "Scanning\n");
+                wrq->length = strlen(extra) + 1; // 1: size of '\0'
+                break;
+            }
+            pAd->StaCfg.bSwRadio = FALSE;
+            if (pAd->StaCfg.bRadio != (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio))
+            {
+                pAd->StaCfg.bRadio = (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio);
+                if (pAd->StaCfg.bRadio == FALSE)
+                {
+                    MlmeRadioOff(pAd);
+                    // Update extra information
+					pAd->ExtraInfo = SW_RADIO_OFF;
+                }
+            }
+            sprintf(extra, "Radio Off\n");
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+        case RAIO_ON:
+            if (RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+            {
+                sprintf(extra, "Scanning\n");
+                wrq->length = strlen(extra) + 1; // 1: size of '\0'
+                break;
+            }
+            pAd->StaCfg.bSwRadio = TRUE;
+            //if (pAd->StaCfg.bRadio != (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio))
+            {
+                pAd->StaCfg.bRadio = (pAd->StaCfg.bHwRadio && pAd->StaCfg.bSwRadio);
+                if (pAd->StaCfg.bRadio == TRUE)
+                {
+                    MlmeRadioOn(pAd);
+                    // Update extra information
+					pAd->ExtraInfo = EXTRA_INFO_CLEAR;
+                }
+            }
+            sprintf(extra, "Radio On\n");
+            wrq->length = strlen(extra) + 1; // 1: size of '\0'
+            break;
+
+
+#ifdef QOS_DLS_SUPPORT
+		case SHOW_DLS_ENTRY_INFO:
+			{
+				Set_DlsEntryInfo_Display_Proc(pAd, NULL);
+				wrq->length = 0; // 1: size of '\0'
+			}
+			break;
+#endif // QOS_DLS_SUPPORT //
+
+		case SHOW_CFG_VALUE:
+			{
+				Status = RTMPShowCfgValue(pAd, wrq->pointer, extra);
+				if (Status == 0)
+					wrq->length = strlen(extra) + 1; // 1: size of '\0'
+			}
+			break;
+		case SHOW_ADHOC_ENTRY_INFO:
+			Show_Adhoc_MacTable_Proc(pAd, extra);
+			wrq->length = strlen(extra) + 1; // 1: size of '\0'
+			break;
+        default:
+            DBGPRINT(RT_DEBUG_TRACE, ("%s - unknow subcmd = %d\n", __FUNCTION__, subcmd));
+            break;
+    }
+
+    return Status;
+}
+
+#ifdef SIOCSIWMLME
+int rt_ioctl_siwmlme(struct net_device *dev,
+			   struct iw_request_info *info,
+			   union iwreq_data *wrqu,
+			   char *extra)
+{
+	PRTMP_ADAPTER   pAd = (PRTMP_ADAPTER) dev->priv;
+	struct iw_mlme *pMlme = (struct iw_mlme *)wrqu->data.pointer;
+	MLME_QUEUE_ELEM				MsgElem;
+	MLME_DISASSOC_REQ_STRUCT	DisAssocReq;
+	MLME_DEAUTH_REQ_STRUCT      DeAuthReq;
+
+	DBGPRINT(RT_DEBUG_TRACE, ("====> %s\n", __FUNCTION__));
+
+	if (pMlme == NULL)
+		return -EINVAL;
+
+	switch(pMlme->cmd)
+	{
+#ifdef IW_MLME_DEAUTH
+		case IW_MLME_DEAUTH:
+			DBGPRINT(RT_DEBUG_TRACE, ("====> %s - IW_MLME_DEAUTH\n", __FUNCTION__));
+			COPY_MAC_ADDR(DeAuthReq.Addr, pAd->CommonCfg.Bssid);
+			DeAuthReq.Reason = pMlme->reason_code;
+			MsgElem.MsgLen = sizeof(MLME_DEAUTH_REQ_STRUCT);
+			NdisMoveMemory(MsgElem.Msg, &DeAuthReq, sizeof(MLME_DEAUTH_REQ_STRUCT));
+			MlmeDeauthReqAction(pAd, &MsgElem);
+			if (INFRA_ON(pAd))
+			{
+			    LinkDown(pAd, FALSE);
+			    pAd->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+			}
+			break;
+#endif // IW_MLME_DEAUTH //
+#ifdef IW_MLME_DISASSOC
+		case IW_MLME_DISASSOC:
+			DBGPRINT(RT_DEBUG_TRACE, ("====> %s - IW_MLME_DISASSOC\n", __FUNCTION__));
+			COPY_MAC_ADDR(DisAssocReq.Addr, pAd->CommonCfg.Bssid);
+			DisAssocReq.Reason =  pMlme->reason_code;
+
+			MsgElem.Machine = ASSOC_STATE_MACHINE;
+			MsgElem.MsgType = MT2_MLME_DISASSOC_REQ;
+			MsgElem.MsgLen = sizeof(MLME_DISASSOC_REQ_STRUCT);
+			NdisMoveMemory(MsgElem.Msg, &DisAssocReq, sizeof(MLME_DISASSOC_REQ_STRUCT));
+
+			pAd->Mlme.CntlMachine.CurrState = CNTL_WAIT_OID_DISASSOC;
+			MlmeDisassocReqAction(pAd, &MsgElem);
+			break;
+#endif // IW_MLME_DISASSOC //
+		default:
+			DBGPRINT(RT_DEBUG_TRACE, ("====> %s - Unknow Command\n", __FUNCTION__));
+			break;
+	}
+
+	return 0;
+}
+#endif // SIOCSIWMLME //
+
+#if WIRELESS_EXT > 17
+int rt_ioctl_siwauth(struct net_device *dev,
+			  struct iw_request_info *info,
+			  union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER   pAdapter = (PRTMP_ADAPTER) dev->priv;
+	struct iw_param *param = &wrqu->param;
+
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+	switch (param->flags & IW_AUTH_INDEX) {
+    	case IW_AUTH_WPA_VERSION:
+            if (param->value == IW_AUTH_WPA_VERSION_WPA)
+            {
+                pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPAPSK;
+				if (pAdapter->StaCfg.BssType == BSS_ADHOC)
+					pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPANone;
+            }
+            else if (param->value == IW_AUTH_WPA_VERSION_WPA2)
+                pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA2PSK;
+
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_WPA_VERSION - param->value = %d!\n", __FUNCTION__, param->value));
+            break;
+    	case IW_AUTH_CIPHER_PAIRWISE:
+            if (param->value == IW_AUTH_CIPHER_NONE)
+            {
+                pAdapter->StaCfg.WepStatus = Ndis802_11WEPDisabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11WEPDisabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_WEP40 ||
+                     param->value == IW_AUTH_CIPHER_WEP104)
+            {
+                pAdapter->StaCfg.WepStatus = Ndis802_11WEPEnabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11WEPEnabled;
+#ifdef WPA_SUPPLICANT_SUPPORT
+                pAdapter->StaCfg.IEEE8021X = FALSE;
+#endif // WPA_SUPPLICANT_SUPPORT //
+            }
+            else if (param->value == IW_AUTH_CIPHER_TKIP)
+            {
+                pAdapter->StaCfg.WepStatus = Ndis802_11Encryption2Enabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11Encryption2Enabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_CCMP)
+            {
+                pAdapter->StaCfg.WepStatus = Ndis802_11Encryption3Enabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11Encryption3Enabled;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_CIPHER_PAIRWISE - param->value = %d!\n", __FUNCTION__, param->value));
+            break;
+    	case IW_AUTH_CIPHER_GROUP:
+            if (param->value == IW_AUTH_CIPHER_NONE)
+            {
+                pAdapter->StaCfg.GroupCipher = Ndis802_11WEPDisabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_WEP40 ||
+                     param->value == IW_AUTH_CIPHER_WEP104)
+            {
+                pAdapter->StaCfg.GroupCipher = Ndis802_11WEPEnabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_TKIP)
+            {
+                pAdapter->StaCfg.GroupCipher = Ndis802_11Encryption2Enabled;
+            }
+            else if (param->value == IW_AUTH_CIPHER_CCMP)
+            {
+                pAdapter->StaCfg.GroupCipher = Ndis802_11Encryption3Enabled;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_CIPHER_GROUP - param->value = %d!\n", __FUNCTION__, param->value));
+            break;
+    	case IW_AUTH_KEY_MGMT:
+            if (param->value == IW_AUTH_KEY_MGMT_802_1X)
+            {
+                if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK)
+                {
+                    pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA;
+#ifdef WPA_SUPPLICANT_SUPPORT
+                    pAdapter->StaCfg.IEEE8021X = FALSE;
+#endif // WPA_SUPPLICANT_SUPPORT //
+                }
+                else if (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)
+                {
+                    pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA2;
+#ifdef WPA_SUPPLICANT_SUPPORT
+                    pAdapter->StaCfg.IEEE8021X = FALSE;
+#endif // WPA_SUPPLICANT_SUPPORT //
+                }
+#ifdef WPA_SUPPLICANT_SUPPORT
+                else
+                    // WEP 1x
+                    pAdapter->StaCfg.IEEE8021X = TRUE;
+#endif // WPA_SUPPLICANT_SUPPORT //
+            }
+            else if (param->value == 0)
+            {
+                //pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+				STA_PORT_SECURED(pAdapter);
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_KEY_MGMT - param->value = %d!\n", __FUNCTION__, param->value));
+            break;
+    	case IW_AUTH_RX_UNENCRYPTED_EAPOL:
+            break;
+    	case IW_AUTH_PRIVACY_INVOKED:
+            /*if (param->value == 0)
+			{
+                pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+                pAdapter->StaCfg.WepStatus = Ndis802_11WEPDisabled;
+                pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+                pAdapter->StaCfg.PairCipher = Ndis802_11WEPDisabled;
+        	    pAdapter->StaCfg.GroupCipher = Ndis802_11WEPDisabled;
+            }*/
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_PRIVACY_INVOKED - param->value = %d!\n", __FUNCTION__, param->value));
+    		break;
+    	case IW_AUTH_DROP_UNENCRYPTED:
+            if (param->value != 0)
+                pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+			else
+			{
+                //pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+				STA_PORT_SECURED(pAdapter);
+			}
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_WPA_VERSION - param->value = %d!\n", __FUNCTION__, param->value));
+    		break;
+    	case IW_AUTH_80211_AUTH_ALG:
+			if (param->value & IW_AUTH_ALG_SHARED_KEY)
+            {
+				pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeShared;
+			}
+            else if (param->value & IW_AUTH_ALG_OPEN_SYSTEM)
+            {
+				pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+			}
+            else
+				return -EINVAL;
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_80211_AUTH_ALG - param->value = %d!\n", __FUNCTION__, param->value));
+			break;
+    	case IW_AUTH_WPA_ENABLED:
+    		DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_AUTH_WPA_ENABLED - Driver supports WPA!(param->value = %d)\n", __FUNCTION__, param->value));
+    		break;
+    	default:
+    		return -EOPNOTSUPP;
+}
+
+	return 0;
+}
+
+int rt_ioctl_giwauth(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER   pAdapter = (PRTMP_ADAPTER) dev->priv;
+	struct iw_param *param = &wrqu->param;
+
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+    }
+
+	switch (param->flags & IW_AUTH_INDEX) {
+	case IW_AUTH_DROP_UNENCRYPTED:
+        param->value = (pAdapter->StaCfg.WepStatus == Ndis802_11WEPDisabled) ? 0 : 1;
+		break;
+
+	case IW_AUTH_80211_AUTH_ALG:
+        param->value = (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeShared) ? IW_AUTH_ALG_SHARED_KEY : IW_AUTH_ALG_OPEN_SYSTEM;
+		break;
+
+	case IW_AUTH_WPA_ENABLED:
+		param->value = (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA) ? 1 : 0;
+		break;
+
+	default:
+		return -EOPNOTSUPP;
+	}
+    DBGPRINT(RT_DEBUG_TRACE, ("rt_ioctl_giwauth::param->value = %d!\n", param->value));
+	return 0;
+}
+
+void fnSetCipherKey(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  INT             keyIdx,
+    IN  UCHAR           CipherAlg,
+    IN  BOOLEAN         bGTK,
+    IN  struct iw_encode_ext *ext)
+{
+    NdisZeroMemory(&pAdapter->SharedKey[BSS0][keyIdx], sizeof(CIPHER_KEY));
+    pAdapter->SharedKey[BSS0][keyIdx].KeyLen = LEN_TKIP_EK;
+    NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].Key, ext->key, LEN_TKIP_EK);
+    NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].TxMic, ext->key + LEN_TKIP_EK, LEN_TKIP_TXMICK);
+    NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].RxMic, ext->key + LEN_TKIP_EK + LEN_TKIP_TXMICK, LEN_TKIP_RXMICK);
+    pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CipherAlg;
+
+    // Update group key information to ASIC Shared Key Table
+	AsicAddSharedKeyEntry(pAdapter,
+						  BSS0,
+						  keyIdx,
+						  pAdapter->SharedKey[BSS0][keyIdx].CipherAlg,
+						  pAdapter->SharedKey[BSS0][keyIdx].Key,
+						  pAdapter->SharedKey[BSS0][keyIdx].TxMic,
+						  pAdapter->SharedKey[BSS0][keyIdx].RxMic);
+
+    if (bGTK)
+        // Update ASIC WCID attribute table and IVEIV table
+    	RTMPAddWcidAttributeEntry(pAdapter,
+    							  BSS0,
+    							  keyIdx,
+    							  pAdapter->SharedKey[BSS0][keyIdx].CipherAlg,
+    							  NULL);
+    else
+        // Update ASIC WCID attribute table and IVEIV table
+    	RTMPAddWcidAttributeEntry(pAdapter,
+    							  BSS0,
+    							  keyIdx,
+    							  pAdapter->SharedKey[BSS0][keyIdx].CipherAlg,
+    							  &pAdapter->MacTab.Content[BSSID_WCID]);
+}
+
+int rt_ioctl_siwencodeext(struct net_device *dev,
+			   struct iw_request_info *info,
+			   union iwreq_data *wrqu,
+			   char *extra)
+			{
+    PRTMP_ADAPTER   pAdapter = (PRTMP_ADAPTER) dev->priv;
+	struct iw_point *encoding = &wrqu->encoding;
+	struct iw_encode_ext *ext = (struct iw_encode_ext *)extra;
+    int keyIdx, alg = ext->alg;
+
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+    if (encoding->flags & IW_ENCODE_DISABLED)
+	{
+        keyIdx = (encoding->flags & IW_ENCODE_INDEX) - 1;
+        // set BSSID wcid entry of the Pair-wise Key table as no-security mode
+	    AsicRemovePairwiseKeyEntry(pAdapter, BSS0, BSSID_WCID);
+        pAdapter->SharedKey[BSS0][keyIdx].KeyLen = 0;
+		pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_NONE;
+		AsicRemoveSharedKeyEntry(pAdapter, 0, (UCHAR)keyIdx);
+        NdisZeroMemory(&pAdapter->SharedKey[BSS0][keyIdx], sizeof(CIPHER_KEY));
+        DBGPRINT(RT_DEBUG_TRACE, ("%s::Remove all keys!(encoding->flags = %x)\n", __FUNCTION__, encoding->flags));
+    }
+					else
+    {
+        // Get Key Index and convet to our own defined key index
+    	keyIdx = (encoding->flags & IW_ENCODE_INDEX) - 1;
+    	if((keyIdx < 0) || (keyIdx >= NR_WEP_KEYS))
+    		return -EINVAL;
+
+        if (ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY)
+        {
+            pAdapter->StaCfg.DefaultKeyId = keyIdx;
+            DBGPRINT(RT_DEBUG_TRACE, ("%s::DefaultKeyId = %d\n", __FUNCTION__, pAdapter->StaCfg.DefaultKeyId));
+        }
+
+        switch (alg) {
+    		case IW_ENCODE_ALG_NONE:
+                DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_ENCODE_ALG_NONE\n", __FUNCTION__));
+    			break;
+    		case IW_ENCODE_ALG_WEP:
+                DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_ENCODE_ALG_WEP - ext->key_len = %d, keyIdx = %d\n", __FUNCTION__, ext->key_len, keyIdx));
+    			if (ext->key_len == MAX_WEP_KEY_SIZE)
+                {
+        			pAdapter->SharedKey[BSS0][keyIdx].KeyLen = MAX_WEP_KEY_SIZE;
+                    pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_WEP128;
+				}
+        		else if (ext->key_len == MIN_WEP_KEY_SIZE)
+                {
+                    pAdapter->SharedKey[BSS0][keyIdx].KeyLen = MIN_WEP_KEY_SIZE;
+                    pAdapter->SharedKey[BSS0][keyIdx].CipherAlg = CIPHER_WEP64;
+			}
+        		else
+                    return -EINVAL;
+
+                NdisZeroMemory(pAdapter->SharedKey[BSS0][keyIdx].Key,  16);
+			    NdisMoveMemory(pAdapter->SharedKey[BSS0][keyIdx].Key, ext->key, ext->key_len);
+    			break;
+            case IW_ENCODE_ALG_TKIP:
+                DBGPRINT(RT_DEBUG_TRACE, ("%s::IW_ENCODE_ALG_TKIP - keyIdx = %d, ext->key_len = %d\n", __FUNCTION__, keyIdx, ext->key_len));
+                if (ext->key_len == 32)
+                {
+                    if (ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY)
+                    {
+                        fnSetCipherKey(pAdapter, keyIdx, CIPHER_TKIP, FALSE, ext);
+                        if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA2)
+                        {
+                            //pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+                            STA_PORT_SECURED(pAdapter);
+                        }
+		}
+                    else if (ext->ext_flags & IW_ENCODE_EXT_GROUP_KEY)
+                    {
+                        fnSetCipherKey(pAdapter, keyIdx, CIPHER_TKIP, TRUE, ext);
+
+                        // set 802.1x port control
+            	        //pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+            	        STA_PORT_SECURED(pAdapter);
+                    }
+                }
+                else
+                    return -EINVAL;
+                break;
+            case IW_ENCODE_ALG_CCMP:
+                if (ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY)
+		{
+                    fnSetCipherKey(pAdapter, keyIdx, CIPHER_AES, FALSE, ext);
+                    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA2)
+                    	//pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+                    	STA_PORT_SECURED(pAdapter);
+                }
+                else if (ext->ext_flags & IW_ENCODE_EXT_GROUP_KEY)
+                {
+                    fnSetCipherKey(pAdapter, keyIdx, CIPHER_AES, TRUE, ext);
+
+                    // set 802.1x port control
+        	        //pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+        	        STA_PORT_SECURED(pAdapter);
+                }
+                break;
+    		default:
+    			return -EINVAL;
+		}
+    }
+
+    return 0;
+}
+
+int
+rt_ioctl_giwencodeext(struct net_device *dev,
+			  struct iw_request_info *info,
+			  union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER pAd = (PRTMP_ADAPTER) dev->priv;
+	PCHAR pKey = NULL;
+	struct iw_point *encoding = &wrqu->encoding;
+	struct iw_encode_ext *ext = (struct iw_encode_ext *)extra;
+	int idx, max_key_len;
+
+	DBGPRINT(RT_DEBUG_TRACE ,("===> rt_ioctl_giwencodeext\n"));
+
+	max_key_len = encoding->length - sizeof(*ext);
+	if (max_key_len < 0)
+		return -EINVAL;
+
+	idx = encoding->flags & IW_ENCODE_INDEX;
+	if (idx)
+	{
+		if (idx < 1 || idx > 4)
+			return -EINVAL;
+		idx--;
+
+		if ((pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled) ||
+			(pAd->StaCfg.WepStatus == Ndis802_11Encryption3Enabled))
+		{
+			if (idx != pAd->StaCfg.DefaultKeyId)
+			{
+				ext->key_len = 0;
+				return 0;
+			}
+		}
+	}
+	else
+		idx = pAd->StaCfg.DefaultKeyId;
+
+	encoding->flags = idx + 1;
+	memset(ext, 0, sizeof(*ext));
+
+	ext->key_len = 0;
+	switch(pAd->StaCfg.WepStatus) {
+		case Ndis802_11WEPDisabled:
+			ext->alg = IW_ENCODE_ALG_NONE;
+			encoding->flags |= IW_ENCODE_DISABLED;
+			break;
+		case Ndis802_11WEPEnabled:
+			ext->alg = IW_ENCODE_ALG_WEP;
+			if (pAd->SharedKey[BSS0][idx].KeyLen > max_key_len)
+				return -E2BIG;
+			else
+			{
+				ext->key_len = pAd->SharedKey[BSS0][idx].KeyLen;
+				pKey = &(pAd->SharedKey[BSS0][idx].Key[0]);
+			}
+			break;
+		case Ndis802_11Encryption2Enabled:
+		case Ndis802_11Encryption3Enabled:
+			if (pAd->StaCfg.WepStatus == Ndis802_11Encryption2Enabled)
+				ext->alg = IW_ENCODE_ALG_TKIP;
+			else
+				ext->alg = IW_ENCODE_ALG_CCMP;
+
+			if (max_key_len < 32)
+				return -E2BIG;
+			else
+			{
+				ext->key_len = 32;
+				pKey = &pAd->StaCfg.PMK[0];
+			}
+			break;
+		default:
+			return -EINVAL;
+	}
+
+	if (ext->key_len && pKey)
+	{
+		encoding->flags |= IW_ENCODE_ENABLED;
+		memcpy(ext->key, pKey, ext->key_len);
+	}
+
+	return 0;
+}
+
+#ifdef SIOCSIWGENIE
+int rt_ioctl_siwgenie(struct net_device *dev,
+			  struct iw_request_info *info,
+			  union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER   pAd = (PRTMP_ADAPTER) dev->priv;
+
+	if (wrqu->data.length > MAX_LEN_OF_RSNIE ||
+	    (wrqu->data.length && extra == NULL))
+		return -EINVAL;
+
+	if (wrqu->data.length)
+	{
+		pAd->StaCfg.RSNIE_Len = wrqu->data.length;
+		NdisMoveMemory(&pAd->StaCfg.RSN_IE[0], extra, pAd->StaCfg.RSNIE_Len);
+	}
+	else
+	{
+		pAd->StaCfg.RSNIE_Len = 0;
+		NdisZeroMemory(&pAd->StaCfg.RSN_IE[0], MAX_LEN_OF_RSNIE);
+	}
+
+	return 0;
+}
+#endif // SIOCSIWGENIE //
+
+int rt_ioctl_giwgenie(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	PRTMP_ADAPTER   pAd = (PRTMP_ADAPTER) dev->priv;
+
+	if ((pAd->StaCfg.RSNIE_Len == 0) ||
+		(pAd->StaCfg.AuthMode < Ndis802_11AuthModeWPA))
+	{
+		wrqu->data.length = 0;
+		return 0;
+	}
+
+#ifdef NATIVE_WPA_SUPPLICANT_SUPPORT
+#ifdef SIOCSIWGENIE
+	if (pAd->StaCfg.WpaSupplicantUP == WPA_SUPPLICANT_ENABLE)
+	{
+	if (wrqu->data.length < pAd->StaCfg.RSNIE_Len)
+		return -E2BIG;
+
+	wrqu->data.length = pAd->StaCfg.RSNIE_Len;
+	memcpy(extra, &pAd->StaCfg.RSN_IE[0], pAd->StaCfg.RSNIE_Len);
+	}
+	else
+#endif // SIOCSIWGENIE //
+#endif // NATIVE_WPA_SUPPLICANT_SUPPORT //
+	{
+		UCHAR RSNIe = IE_WPA;
+
+		if (wrqu->data.length < (pAd->StaCfg.RSNIE_Len + 2)) // ID, Len
+			return -E2BIG;
+		wrqu->data.length = pAd->StaCfg.RSNIE_Len + 2;
+
+		if ((pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK) ||
+            (pAd->StaCfg.AuthMode == Ndis802_11AuthModeWPA2))
+			RSNIe = IE_RSN;
+
+		extra[0] = (char)RSNIe;
+		extra[1] = pAd->StaCfg.RSNIE_Len;
+		memcpy(extra+2, &pAd->StaCfg.RSN_IE[0], pAd->StaCfg.RSNIE_Len);
+	}
+
+	return 0;
+}
+
+int rt_ioctl_siwpmksa(struct net_device *dev,
+			   struct iw_request_info *info,
+			   union iwreq_data *wrqu,
+			   char *extra)
+{
+	PRTMP_ADAPTER   pAd = (PRTMP_ADAPTER) dev->priv;
+	struct iw_pmksa *pPmksa = (struct iw_pmksa *)wrqu->data.pointer;
+	INT	CachedIdx = 0, idx = 0;
+
+	if (pPmksa == NULL)
+		return -EINVAL;
+
+	DBGPRINT(RT_DEBUG_TRACE ,("===> rt_ioctl_siwpmksa\n"));
+	switch(pPmksa->cmd)
+	{
+		case IW_PMKSA_FLUSH:
+			NdisZeroMemory(pAd->StaCfg.SavedPMK, sizeof(BSSID_INFO)*PMKID_NO);
+			DBGPRINT(RT_DEBUG_TRACE ,("rt_ioctl_siwpmksa - IW_PMKSA_FLUSH\n"));
+			break;
+		case IW_PMKSA_REMOVE:
+			for (CachedIdx = 0; CachedIdx < pAd->StaCfg.SavedPMKNum; CachedIdx++)
+			{
+		        // compare the BSSID
+		        if (NdisEqualMemory(pPmksa->bssid.sa_data, pAd->StaCfg.SavedPMK[CachedIdx].BSSID, MAC_ADDR_LEN))
+		        {
+		        	NdisZeroMemory(pAd->StaCfg.SavedPMK[CachedIdx].BSSID, MAC_ADDR_LEN);
+					NdisZeroMemory(pAd->StaCfg.SavedPMK[CachedIdx].PMKID, 16);
+					for (idx = CachedIdx; idx < (pAd->StaCfg.SavedPMKNum - 1); idx++)
+					{
+						NdisMoveMemory(&pAd->StaCfg.SavedPMK[idx].BSSID[0], &pAd->StaCfg.SavedPMK[idx+1].BSSID[0], MAC_ADDR_LEN);
+						NdisMoveMemory(&pAd->StaCfg.SavedPMK[idx].PMKID[0], &pAd->StaCfg.SavedPMK[idx+1].PMKID[0], 16);
+					}
+					pAd->StaCfg.SavedPMKNum--;
+			        break;
+		        }
+	        }
+
+			DBGPRINT(RT_DEBUG_TRACE ,("rt_ioctl_siwpmksa - IW_PMKSA_REMOVE\n"));
+			break;
+		case IW_PMKSA_ADD:
+			for (CachedIdx = 0; CachedIdx < pAd->StaCfg.SavedPMKNum; CachedIdx++)
+			{
+		        // compare the BSSID
+		        if (NdisEqualMemory(pPmksa->bssid.sa_data, pAd->StaCfg.SavedPMK[CachedIdx].BSSID, MAC_ADDR_LEN))
+			        break;
+	        }
+
+	        // Found, replace it
+	        if (CachedIdx < PMKID_NO)
+	        {
+		        DBGPRINT(RT_DEBUG_OFF, ("Update PMKID, idx = %d\n", CachedIdx));
+		        NdisMoveMemory(&pAd->StaCfg.SavedPMK[CachedIdx].BSSID[0], pPmksa->bssid.sa_data, MAC_ADDR_LEN);
+				NdisMoveMemory(&pAd->StaCfg.SavedPMK[CachedIdx].PMKID[0], pPmksa->pmkid, 16);
+		        pAd->StaCfg.SavedPMKNum++;
+	        }
+	        // Not found, replace the last one
+	        else
+	        {
+		        // Randomly replace one
+		        CachedIdx = (pPmksa->bssid.sa_data[5] % PMKID_NO);
+		        DBGPRINT(RT_DEBUG_OFF, ("Update PMKID, idx = %d\n", CachedIdx));
+		        NdisMoveMemory(&pAd->StaCfg.SavedPMK[CachedIdx].BSSID[0], pPmksa->bssid.sa_data, MAC_ADDR_LEN);
+				NdisMoveMemory(&pAd->StaCfg.SavedPMK[CachedIdx].PMKID[0], pPmksa->pmkid, 16);
+	        }
+
+			DBGPRINT(RT_DEBUG_TRACE ,("rt_ioctl_siwpmksa - IW_PMKSA_ADD\n"));
+			break;
+		default:
+			DBGPRINT(RT_DEBUG_TRACE ,("rt_ioctl_siwpmksa - Unknow Command!!\n"));
+			break;
+	}
+
+	return 0;
+}
+#endif // #if WIRELESS_EXT > 17
+
+#ifdef DBG
+static int
+rt_private_ioctl_bbp(struct net_device *dev, struct iw_request_info *info,
+		struct iw_point *wrq, char *extra)
+			{
+	CHAR				*this_char;
+	CHAR				*value = NULL;
+	UCHAR				regBBP = 0;
+//	CHAR				arg[255]={0};
+	UINT32				bbpId;
+	UINT32				bbpValue;
+	BOOLEAN				bIsPrintAllBBP = FALSE;
+	INT					Status = 0;
+    PRTMP_ADAPTER       pAdapter = (PRTMP_ADAPTER) dev->priv;
+
+
+	memset(extra, 0x00, IW_PRIV_SIZE_MASK);
+
+	if (wrq->length > 1) //No parameters.
+				{
+		sprintf(extra, "\n");
+
+		//Parsing Read or Write
+		this_char = wrq->pointer;
+		DBGPRINT(RT_DEBUG_TRACE, ("this_char=%s\n", this_char));
+		if (!*this_char)
+			goto next;
+
+		if ((value = rtstrchr(this_char, '=')) != NULL)
+			*value++ = 0;
+
+		if (!value || !*value)
+		{ //Read
+			DBGPRINT(RT_DEBUG_TRACE, ("this_char=%s, value=%s\n", this_char, value));
+			if (sscanf(this_char, "%d", &(bbpId)) == 1)
+			{
+				if (bbpId <= 136)
+				{
+#ifdef RALINK_ATE
+					if (ATE_ON(pAdapter))
+					{
+						ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+					}
+					else
+#endif // RALINK_ATE //
+					{
+					RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+					}
+					sprintf(extra+strlen(extra), "R%02d[0x%02X]:%02X\n", bbpId, bbpId*2, regBBP);
+                    wrq->length = strlen(extra) + 1; // 1: size of '\0'
+					DBGPRINT(RT_DEBUG_TRACE, ("msg=%s\n", extra));
+				}
+				else
+				{//Invalid parametes, so default printk all bbp
+					bIsPrintAllBBP = TRUE;
+					goto next;
+				}
+			}
+			else
+			{ //Invalid parametes, so default printk all bbp
+				bIsPrintAllBBP = TRUE;
+				goto next;
+			}
+		}
+		else
+		{ //Write
+			if ((sscanf(this_char, "%d", &(bbpId)) == 1) && (sscanf(value, "%x", &(bbpValue)) == 1))
+			{
+				if (bbpId <= 136)
+				{
+#ifdef RALINK_ATE
+					if (ATE_ON(pAdapter))
+					{
+						ATE_BBP_IO_WRITE8_BY_REG_ID(pAdapter, bbpId, bbpValue);
+						//Read it back for showing
+						ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+					}
+					else
+#endif // RALINK_ATE //
+					{
+					    RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, bbpId, bbpValue);
+    					//Read it back for showing
+    					RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+			}
+					sprintf(extra+strlen(extra), "R%02d[0x%02X]:%02X\n", bbpId, bbpId*2, regBBP);
+                    wrq->length = strlen(extra) + 1; // 1: size of '\0'
+					DBGPRINT(RT_DEBUG_TRACE, ("msg=%s\n", extra));
+				}
+				else
+				{//Invalid parametes, so default printk all bbp
+					bIsPrintAllBBP = TRUE;
+					goto next;
+				}
+			}
+			else
+			{ //Invalid parametes, so default printk all bbp
+				bIsPrintAllBBP = TRUE;
+				goto next;
+			}
+		}
+		}
+	else
+		bIsPrintAllBBP = TRUE;
+
+next:
+	if (bIsPrintAllBBP)
+	{
+		memset(extra, 0x00, IW_PRIV_SIZE_MASK);
+		sprintf(extra, "\n");
+		for (bbpId = 0; bbpId <= 136; bbpId++)
+		{
+		    if (strlen(extra) >= (IW_PRIV_SIZE_MASK - 10))
+                break;
+#ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				ATE_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+			}
+			else
+#endif // RALINK_ATE //
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, bbpId, &regBBP);
+			sprintf(extra+strlen(extra), "R%02d[0x%02X]:%02X    ", bbpId, bbpId*2, regBBP);
+			if (bbpId%5 == 4)
+				sprintf(extra+strlen(extra), "\n");
+		}
+
+        wrq->length = strlen(extra) + 1; // 1: size of '\0'
+        DBGPRINT(RT_DEBUG_TRACE, ("wrq->length = %d\n", wrq->length));
+	}
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<==rt_private_ioctl_bbp\n\n"));
+
+    return Status;
+}
+#endif // DBG //
+
+int rt_ioctl_siwrate(struct net_device *dev,
+			struct iw_request_info *info,
+			union iwreq_data *wrqu, char *extra)
+{
+    PRTMP_ADAPTER   pAd = (PRTMP_ADAPTER) dev->priv;
+    UINT32          rate = wrqu->bitrate.value, fixed = wrqu->bitrate.fixed;
+
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("rt_ioctl_siwrate::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+    DBGPRINT(RT_DEBUG_TRACE, ("rt_ioctl_siwrate::(rate = %d, fixed = %d)\n", rate, fixed));
+    /* rate = -1 => auto rate
+       rate = X, fixed = 1 => (fixed rate X)
+    */
+    if (rate == -1)
+    {
+		//Auto Rate
+		pAd->StaCfg.DesiredTransmitSetting.field.MCS = MCS_AUTO;
+		pAd->StaCfg.bAutoTxRateSwitch = TRUE;
+		if ((pAd->CommonCfg.PhyMode <= PHY_11G) ||
+		    (pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE <= MODE_OFDM))
+			RTMPSetDesiredRates(pAd, -1);
+
+#ifdef DOT11_N_SUPPORT
+		SetCommonHT(pAd);
+#endif // DOT11_N_SUPPORT //
+    }
+    else
+    {
+        if (fixed)
+        {
+        	pAd->StaCfg.bAutoTxRateSwitch = FALSE;
+            if ((pAd->CommonCfg.PhyMode <= PHY_11G) ||
+                (pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE <= MODE_OFDM))
+                RTMPSetDesiredRates(pAd, rate);
+            else
+            {
+                pAd->StaCfg.DesiredTransmitSetting.field.MCS = MCS_AUTO;
+#ifdef DOT11_N_SUPPORT
+                SetCommonHT(pAd);
+#endif // DOT11_N_SUPPORT //
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("rt_ioctl_siwrate::(HtMcs=%d)\n",pAd->StaCfg.DesiredTransmitSetting.field.MCS));
+        }
+        else
+        {
+            // TODO: rate = X, fixed = 0 => (rates <= X)
+            return -EOPNOTSUPP;
+        }
+    }
+
+    return 0;
+}
+
+int rt_ioctl_giwrate(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+    PRTMP_ADAPTER   pAd = (PRTMP_ADAPTER) dev->priv;
+    int rate_index = 0, rate_count = 0;
+    HTTRANSMIT_SETTING ht_setting;
+    __s32 ralinkrate[] =
+	{2,  4,   11,  22, // CCK
+	12, 18,   24,  36, 48, 72, 96, 108, // OFDM
+	13, 26,   39,  52,  78, 104, 117, 130, 26,  52,  78, 104, 156, 208, 234, 260, // 20MHz, 800ns GI, MCS: 0 ~ 15
+	39, 78,  117, 156, 234, 312, 351, 390,										  // 20MHz, 800ns GI, MCS: 16 ~ 23
+	27, 54,   81, 108, 162, 216, 243, 270, 54, 108, 162, 216, 324, 432, 486, 540, // 40MHz, 800ns GI, MCS: 0 ~ 15
+	81, 162, 243, 324, 486, 648, 729, 810,										  // 40MHz, 800ns GI, MCS: 16 ~ 23
+	14, 29,   43,  57,  87, 115, 130, 144, 29, 59,   87, 115, 173, 230, 260, 288, // 20MHz, 400ns GI, MCS: 0 ~ 15
+	43, 87,  130, 173, 260, 317, 390, 433,										  // 20MHz, 400ns GI, MCS: 16 ~ 23
+	30, 60,   90, 120, 180, 240, 270, 300, 60, 120, 180, 240, 360, 480, 540, 600, // 40MHz, 400ns GI, MCS: 0 ~ 15
+	90, 180, 270, 360, 540, 720, 810, 900};										  // 40MHz, 400ns GI, MCS: 16 ~ 23
+
+    rate_count = sizeof(ralinkrate)/sizeof(__s32);
+    //check if the interface is down
+	if(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+	{
+  		DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+    	return -ENETDOWN;
+	}
+
+    if ((pAd->StaCfg.bAutoTxRateSwitch == FALSE) &&
+        (INFRA_ON(pAd)) &&
+        ((pAd->CommonCfg.PhyMode <= PHY_11G) || (pAd->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE <= MODE_OFDM)))
+        ht_setting.word = pAd->StaCfg.HTPhyMode.word;
+    else
+        ht_setting.word = pAd->MacTab.Content[BSSID_WCID].HTPhyMode.word;
+
+#ifdef DOT11_N_SUPPORT
+    if (ht_setting.field.MODE >= MODE_HTMIX)
+    {
+//    	rate_index = 12 + ((UCHAR)ht_setting.field.BW *16) + ((UCHAR)ht_setting.field.ShortGI *32) + ((UCHAR)ht_setting.field.MCS);
+    	rate_index = 12 + ((UCHAR)ht_setting.field.BW *24) + ((UCHAR)ht_setting.field.ShortGI *48) + ((UCHAR)ht_setting.field.MCS);
+    }
+    else
+#endif // DOT11_N_SUPPORT //
+    if (ht_setting.field.MODE == MODE_OFDM)
+    	rate_index = (UCHAR)(ht_setting.field.MCS) + 4;
+    else if (ht_setting.field.MODE == MODE_CCK)
+    	rate_index = (UCHAR)(ht_setting.field.MCS);
+
+    if (rate_index < 0)
+        rate_index = 0;
+
+    if (rate_index > rate_count)
+        rate_index = rate_count;
+
+    wrqu->bitrate.value = ralinkrate[rate_index] * 500000;
+    wrqu->bitrate.disabled = 0;
+
+    return 0;
+}
+
+static const iw_handler rt_handler[] =
+{
+	(iw_handler) NULL,			            /* SIOCSIWCOMMIT */
+	(iw_handler) rt_ioctl_giwname,			/* SIOCGIWNAME   */
+	(iw_handler) NULL,			            /* SIOCSIWNWID   */
+	(iw_handler) NULL,			            /* SIOCGIWNWID   */
+	(iw_handler) rt_ioctl_siwfreq,		    /* SIOCSIWFREQ   */
+	(iw_handler) rt_ioctl_giwfreq,		    /* SIOCGIWFREQ   */
+	(iw_handler) rt_ioctl_siwmode,		    /* SIOCSIWMODE   */
+	(iw_handler) rt_ioctl_giwmode,		    /* SIOCGIWMODE   */
+	(iw_handler) NULL,		                /* SIOCSIWSENS   */
+	(iw_handler) NULL,		                /* SIOCGIWSENS   */
+	(iw_handler) NULL /* not used */,		/* SIOCSIWRANGE  */
+	(iw_handler) rt_ioctl_giwrange,		    /* SIOCGIWRANGE  */
+	(iw_handler) NULL /* not used */,		/* SIOCSIWPRIV   */
+	(iw_handler) NULL /* kernel code */,    /* SIOCGIWPRIV   */
+	(iw_handler) NULL /* not used */,		/* SIOCSIWSTATS  */
+	(iw_handler) rt28xx_get_wireless_stats /* kernel code */,    /* SIOCGIWSTATS  */
+	(iw_handler) NULL,		                /* SIOCSIWSPY    */
+	(iw_handler) NULL,		                /* SIOCGIWSPY    */
+	(iw_handler) NULL,				        /* SIOCSIWTHRSPY */
+	(iw_handler) NULL,				        /* SIOCGIWTHRSPY */
+	(iw_handler) rt_ioctl_siwap,            /* SIOCSIWAP     */
+	(iw_handler) rt_ioctl_giwap,		    /* SIOCGIWAP     */
+#ifdef SIOCSIWMLME
+	(iw_handler) rt_ioctl_siwmlme,	        /* SIOCSIWMLME   */
+#else
+	(iw_handler) NULL,				        /* SIOCSIWMLME */
+#endif // SIOCSIWMLME //
+	(iw_handler) rt_ioctl_iwaplist,		    /* SIOCGIWAPLIST */
+#ifdef SIOCGIWSCAN
+	(iw_handler) rt_ioctl_siwscan,		    /* SIOCSIWSCAN   */
+	(iw_handler) rt_ioctl_giwscan,		    /* SIOCGIWSCAN   */
+#else
+	(iw_handler) NULL,				        /* SIOCSIWSCAN   */
+	(iw_handler) NULL,				        /* SIOCGIWSCAN   */
+#endif /* SIOCGIWSCAN */
+	(iw_handler) rt_ioctl_siwessid,		    /* SIOCSIWESSID  */
+	(iw_handler) rt_ioctl_giwessid,		    /* SIOCGIWESSID  */
+	(iw_handler) rt_ioctl_siwnickn,		    /* SIOCSIWNICKN  */
+	(iw_handler) rt_ioctl_giwnickn,		    /* SIOCGIWNICKN  */
+	(iw_handler) NULL,				        /* -- hole --    */
+	(iw_handler) NULL,				        /* -- hole --    */
+	(iw_handler) rt_ioctl_siwrate,          /* SIOCSIWRATE   */
+	(iw_handler) rt_ioctl_giwrate,          /* SIOCGIWRATE   */
+	(iw_handler) rt_ioctl_siwrts,		    /* SIOCSIWRTS    */
+	(iw_handler) rt_ioctl_giwrts,		    /* SIOCGIWRTS    */
+	(iw_handler) rt_ioctl_siwfrag,		    /* SIOCSIWFRAG   */
+	(iw_handler) rt_ioctl_giwfrag,		    /* SIOCGIWFRAG   */
+	(iw_handler) NULL,		                /* SIOCSIWTXPOW  */
+	(iw_handler) NULL,		                /* SIOCGIWTXPOW  */
+	(iw_handler) NULL,		                /* SIOCSIWRETRY  */
+	(iw_handler) NULL,		                /* SIOCGIWRETRY  */
+	(iw_handler) rt_ioctl_siwencode,		/* SIOCSIWENCODE */
+	(iw_handler) rt_ioctl_giwencode,		/* SIOCGIWENCODE */
+	(iw_handler) NULL,		                /* SIOCSIWPOWER  */
+	(iw_handler) NULL,		                /* SIOCGIWPOWER  */
+	(iw_handler) NULL,						/* -- hole -- */
+	(iw_handler) NULL,						/* -- hole -- */
+#if WIRELESS_EXT > 17
+    (iw_handler) rt_ioctl_siwgenie,         /* SIOCSIWGENIE  */
+	(iw_handler) rt_ioctl_giwgenie,         /* SIOCGIWGENIE  */
+	(iw_handler) rt_ioctl_siwauth,		    /* SIOCSIWAUTH   */
+	(iw_handler) rt_ioctl_giwauth,		    /* SIOCGIWAUTH   */
+	(iw_handler) rt_ioctl_siwencodeext,	    /* SIOCSIWENCODEEXT */
+	(iw_handler) rt_ioctl_giwencodeext,		/* SIOCGIWENCODEEXT */
+	(iw_handler) rt_ioctl_siwpmksa,         /* SIOCSIWPMKSA  */
+#endif
+};
+
+static const iw_handler rt_priv_handlers[] = {
+	(iw_handler) NULL, /* + 0x00 */
+	(iw_handler) NULL, /* + 0x01 */
+#ifndef CONFIG_AP_SUPPORT
+	(iw_handler) rt_ioctl_setparam, /* + 0x02 */
+#else
+	(iw_handler) NULL, /* + 0x02 */
+#endif // CONFIG_AP_SUPPORT //
+#ifdef DBG
+	(iw_handler) rt_private_ioctl_bbp, /* + 0x03 */
+#else
+	(iw_handler) NULL, /* + 0x03 */
+#endif
+	(iw_handler) NULL, /* + 0x04 */
+	(iw_handler) NULL, /* + 0x05 */
+	(iw_handler) NULL, /* + 0x06 */
+	(iw_handler) NULL, /* + 0x07 */
+	(iw_handler) NULL, /* + 0x08 */
+	(iw_handler) rt_private_get_statistics, /* + 0x09 */
+	(iw_handler) NULL, /* + 0x0A */
+	(iw_handler) NULL, /* + 0x0B */
+	(iw_handler) NULL, /* + 0x0C */
+	(iw_handler) NULL, /* + 0x0D */
+	(iw_handler) NULL, /* + 0x0E */
+	(iw_handler) NULL, /* + 0x0F */
+	(iw_handler) NULL, /* + 0x10 */
+	(iw_handler) rt_private_show, /* + 0x11 */
+    (iw_handler) NULL, /* + 0x12 */
+	(iw_handler) NULL, /* + 0x13 */
+	(iw_handler) NULL, /* + 0x15 */
+	(iw_handler) NULL, /* + 0x17 */
+	(iw_handler) NULL, /* + 0x18 */
+};
+
+const struct iw_handler_def rt28xx_iw_handler_def =
+{
+#define	N(a)	(sizeof (a) / sizeof (a[0]))
+	.standard	= (iw_handler *) rt_handler,
+	.num_standard	= sizeof(rt_handler) / sizeof(iw_handler),
+	.private	= (iw_handler *) rt_priv_handlers,
+	.num_private		= N(rt_priv_handlers),
+	.private_args	= (struct iw_priv_args *) privtab,
+	.num_private_args	= N(privtab),
+#if IW_HANDLER_VERSION >= 7
+    .get_wireless_stats = rt28xx_get_wireless_stats,
+#endif
+};
+
+INT RTMPSetInformation(
+    IN  PRTMP_ADAPTER pAdapter,
+    IN  OUT struct ifreq    *rq,
+    IN  INT                 cmd)
+{
+    struct iwreq                        *wrq = (struct iwreq *) rq;
+    NDIS_802_11_SSID                    Ssid;
+    NDIS_802_11_MAC_ADDRESS             Bssid;
+    RT_802_11_PHY_MODE                  PhyMode;
+    RT_802_11_STA_CONFIG                StaConfig;
+    NDIS_802_11_RATES                   aryRates;
+    RT_802_11_PREAMBLE                  Preamble;
+    NDIS_802_11_WEP_STATUS              WepStatus;
+    NDIS_802_11_AUTHENTICATION_MODE     AuthMode = Ndis802_11AuthModeMax;
+    NDIS_802_11_NETWORK_INFRASTRUCTURE  BssType;
+    NDIS_802_11_RTS_THRESHOLD           RtsThresh;
+    NDIS_802_11_FRAGMENTATION_THRESHOLD FragThresh;
+    NDIS_802_11_POWER_MODE              PowerMode;
+    PNDIS_802_11_KEY                    pKey = NULL;
+    PNDIS_802_11_WEP			        pWepKey =NULL;
+    PNDIS_802_11_REMOVE_KEY             pRemoveKey = NULL;
+    NDIS_802_11_CONFIGURATION           Config, *pConfig = NULL;
+    NDIS_802_11_NETWORK_TYPE            NetType;
+    ULONG                               Now;
+    UINT                                KeyIdx = 0;
+    INT                                 Status = NDIS_STATUS_SUCCESS, MaxPhyMode = PHY_11G;
+    ULONG                               PowerTemp;
+    BOOLEAN                             RadioState;
+    BOOLEAN                             StateMachineTouched = FALSE;
+#ifdef DOT11_N_SUPPORT
+	OID_SET_HT_PHYMODE					HT_PhyMode;	//11n ,kathy
+#endif // DOT11_N_SUPPORT //
+#ifdef WPA_SUPPLICANT_SUPPORT
+    PNDIS_802_11_PMKID                  pPmkId = NULL;
+    BOOLEAN				                IEEE8021xState = FALSE;
+    BOOLEAN				                IEEE8021x_required_keys = FALSE;
+    UCHAR                               wpa_supplicant_enable = 0;
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef SNMP_SUPPORT
+	TX_RTY_CFG_STRUC			tx_rty_cfg;
+	ULONG						ShortRetryLimit, LongRetryLimit;
+	UCHAR						ctmp;
+#endif // SNMP_SUPPORT //
+
+
+
+#ifdef DOT11_N_SUPPORT
+	MaxPhyMode = PHY_11N_5G;
+#endif // DOT11_N_SUPPORT //
+
+
+	DBGPRINT(RT_DEBUG_TRACE, ("-->RTMPSetInformation(),	0x%08x\n", cmd&0x7FFF));
+    switch(cmd & 0x7FFF) {
+        case RT_OID_802_11_COUNTRY_REGION:
+            if (wrq->u.data.length < sizeof(UCHAR))
+                Status = -EINVAL;
+			// Only avaliable when EEPROM not programming
+            else if (!(pAdapter->CommonCfg.CountryRegion & 0x80) && !(pAdapter->CommonCfg.CountryRegionForABand & 0x80))
+            {
+                ULONG   Country;
+                UCHAR	TmpPhy;
+
+				Status = copy_from_user(&Country, wrq->u.data.pointer, wrq->u.data.length);
+				pAdapter->CommonCfg.CountryRegion = (UCHAR)(Country & 0x000000FF);
+				pAdapter->CommonCfg.CountryRegionForABand = (UCHAR)((Country >> 8) & 0x000000FF);
+                TmpPhy = pAdapter->CommonCfg.PhyMode;
+				pAdapter->CommonCfg.PhyMode = 0xff;
+				// Build all corresponding channel information
+				RTMPSetPhyMode(pAdapter, TmpPhy);
+#ifdef DOT11_N_SUPPORT
+				SetCommonHT(pAdapter);
+#endif // DOT11_N_SUPPORT //
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_COUNTRY_REGION (A:%d  B/G:%d)\n", pAdapter->CommonCfg.CountryRegionForABand,
+				    pAdapter->CommonCfg.CountryRegion));
+            }
+            break;
+        case OID_802_11_BSSID_LIST_SCAN:
+ #ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+				break;
+			}
+#endif // RALINK_ATE //
+            Now = jiffies;
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_BSSID_LIST_SCAN, TxCnt = %d \n", pAdapter->RalinkCounters.LastOneSecTotalTxCount));
+
+            if (MONITOR_ON(pAdapter))
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! Driver is in Monitor Mode now !!!\n"));
+                break;
+            }
+
+			//Benson add 20080527, when radio off, sta don't need to scan
+			if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_RADIO_OFF))
+				break;
+
+			if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+			{
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! Driver is scanning now !!!\n"));
+				pAdapter->StaCfg.bScanReqIsFromWebUI = TRUE;
+				Status = NDIS_STATUS_SUCCESS;
+                break;
+            }
+
+			if (pAdapter->RalinkCounters.LastOneSecTotalTxCount > 100)
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! Link UP, ignore this set::OID_802_11_BSSID_LIST_SCAN\n"));
+				Status = NDIS_STATUS_SUCCESS;
+				pAdapter->StaCfg.ScanCnt = 99;		// Prevent auto scan triggered by this OID
+				break;
+            }
+
+            if ((OPSTATUS_TEST_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED)) &&
+				((pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA) ||
+				(pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+				(pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA2) ||
+				(pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK)) &&
+                (pAdapter->StaCfg.PortSecured == WPA_802_1X_PORT_NOT_SECURED))
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! Link UP, Port Not Secured! ignore this set::OID_802_11_BSSID_LIST_SCAN\n"));
+				Status = NDIS_STATUS_SUCCESS;
+				pAdapter->StaCfg.ScanCnt = 99;		// Prevent auto scan triggered by this OID
+				break;
+            }
+
+
+            if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+            {
+                RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+                DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+            }
+
+            // tell CNTL state machine to call NdisMSetInformationComplete() after completing
+            // this request, because this request is initiated by NDIS.
+            pAdapter->MlmeAux.CurrReqIsFromNdis = FALSE;
+            // Reset allowed scan retries
+            pAdapter->StaCfg.ScanCnt = 0;
+            pAdapter->StaCfg.LastScanTime = Now;
+
+			pAdapter->StaCfg.bScanReqIsFromWebUI = TRUE;
+            RTMP_SET_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS);
+            MlmeEnqueue(pAdapter,
+                        MLME_CNTL_STATE_MACHINE,
+                        OID_802_11_BSSID_LIST_SCAN,
+                        0,
+                        NULL);
+
+            Status = NDIS_STATUS_SUCCESS;
+            StateMachineTouched = TRUE;
+            break;
+        case OID_802_11_SSID:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_SSID))
+                Status = -EINVAL;
+            else
+            {
+            	PCHAR pSsidString = NULL;
+                Status = copy_from_user(&Ssid, wrq->u.data.pointer, wrq->u.data.length);
+
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_SSID (Len=%d,Ssid=%s)\n", Ssid.SsidLength, Ssid.Ssid));
+                if (Ssid.SsidLength > MAX_LEN_OF_SSID)
+                    Status = -EINVAL;
+                else
+                {
+                	if (Ssid.SsidLength == 0)
+                	{
+                		Set_SSID_Proc(pAdapter, "");
+                	}
+					else
+                	{
+	                	pSsidString = (CHAR *) kmalloc(MAX_LEN_OF_SSID+1, MEM_ALLOC_FLAG);
+						if (pSsidString)
+						{
+							NdisZeroMemory(pSsidString, MAX_LEN_OF_SSID+1);
+							NdisMoveMemory(pSsidString, Ssid.Ssid, Ssid.SsidLength);
+	                		Set_SSID_Proc(pAdapter, pSsidString);
+							kfree(pSsidString);
+						}
+						else
+							Status = -ENOMEM;
+                	}
+                }
+            }
+            break;
+        case OID_802_11_BSSID:
+#ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+				break;
+			}
+#endif // RALINK_ATE //
+            if (wrq->u.data.length != sizeof(NDIS_802_11_MAC_ADDRESS))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&Bssid, wrq->u.data.pointer, wrq->u.data.length);
+
+                // tell CNTL state machine to call NdisMSetInformationComplete() after completing
+                // this request, because this request is initiated by NDIS.
+                pAdapter->MlmeAux.CurrReqIsFromNdis = FALSE;
+
+				// Prevent to connect AP again in STAMlmePeriodicExec
+				pAdapter->MlmeAux.AutoReconnectSsidLen= 32;
+
+                // Reset allowed scan retries
+				pAdapter->StaCfg.ScanCnt = 0;
+
+                if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+                {
+                    RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+                    DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+                }
+                MlmeEnqueue(pAdapter,
+                            MLME_CNTL_STATE_MACHINE,
+                            OID_802_11_BSSID,
+                            sizeof(NDIS_802_11_MAC_ADDRESS),
+                            (VOID *)&Bssid);
+                Status = NDIS_STATUS_SUCCESS;
+                StateMachineTouched = TRUE;
+
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_BSSID %02x:%02x:%02x:%02x:%02x:%02x\n",
+                                        Bssid[0], Bssid[1], Bssid[2], Bssid[3], Bssid[4], Bssid[5]));
+            }
+            break;
+        case RT_OID_802_11_RADIO:
+            if (wrq->u.data.length != sizeof(BOOLEAN))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&RadioState, wrq->u.data.pointer, wrq->u.data.length);
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_RADIO (=%d)\n", RadioState));
+                if (pAdapter->StaCfg.bSwRadio != RadioState)
+                {
+                    pAdapter->StaCfg.bSwRadio = RadioState;
+                    if (pAdapter->StaCfg.bRadio != (pAdapter->StaCfg.bHwRadio && pAdapter->StaCfg.bSwRadio))
+                    {
+                        pAdapter->StaCfg.bRadio = (pAdapter->StaCfg.bHwRadio && pAdapter->StaCfg.bSwRadio);
+                        if (pAdapter->StaCfg.bRadio == TRUE)
+                        {
+                            MlmeRadioOn(pAdapter);
+                            // Update extra information
+							pAdapter->ExtraInfo = EXTRA_INFO_CLEAR;
+                        }
+                        else
+                        {
+                            MlmeRadioOff(pAdapter);
+                            // Update extra information
+							pAdapter->ExtraInfo = SW_RADIO_OFF;
+                        }
+                    }
+                }
+            }
+            break;
+        case RT_OID_802_11_PHY_MODE:
+            if (wrq->u.data.length != sizeof(RT_802_11_PHY_MODE))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&PhyMode, wrq->u.data.pointer, wrq->u.data.length);
+				if (PhyMode <= MaxPhyMode)
+				{
+                	RTMPSetPhyMode(pAdapter, PhyMode);
+#ifdef DOT11_N_SUPPORT
+					SetCommonHT(pAdapter);
+#endif // DOT11_N_SUPPORT //
+				}
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_PHY_MODE (=%d)\n", PhyMode));
+            }
+            break;
+        case RT_OID_802_11_STA_CONFIG:
+            if (wrq->u.data.length != sizeof(RT_802_11_STA_CONFIG))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&StaConfig, wrq->u.data.pointer, wrq->u.data.length);
+                pAdapter->CommonCfg.bEnableTxBurst = StaConfig.EnableTxBurst;
+                pAdapter->CommonCfg.UseBGProtection = StaConfig.UseBGProtection;
+                pAdapter->CommonCfg.bUseShortSlotTime = 1; // 2003-10-30 always SHORT SLOT capable
+                if ((pAdapter->CommonCfg.PhyMode != StaConfig.AdhocMode) &&
+					(StaConfig.AdhocMode <= MaxPhyMode))
+                {
+                    // allow dynamic change of "USE OFDM rate or not" in ADHOC mode
+                    // if setting changed, need to reset current TX rate as well as BEACON frame format
+                    if (pAdapter->StaCfg.BssType == BSS_ADHOC)
+                    {
+						pAdapter->CommonCfg.PhyMode = StaConfig.AdhocMode;
+                    	RTMPSetPhyMode(pAdapter, PhyMode);
+                        MlmeUpdateTxRates(pAdapter, FALSE, 0);
+                        MakeIbssBeacon(pAdapter);           // re-build BEACON frame
+                        AsicEnableIbssSync(pAdapter);   // copy to on-chip memory
+                    }
+                }
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_STA_CONFIG (Burst=%d, Protection=%ld,ShortSlot=%d\n",
+                                        pAdapter->CommonCfg.bEnableTxBurst,
+                                        pAdapter->CommonCfg.UseBGProtection,
+                                        pAdapter->CommonCfg.bUseShortSlotTime));
+            }
+            break;
+        case OID_802_11_DESIRED_RATES:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_RATES))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&aryRates, wrq->u.data.pointer, wrq->u.data.length);
+                NdisZeroMemory(pAdapter->CommonCfg.DesireRate, MAX_LEN_OF_SUPPORTED_RATES);
+                NdisMoveMemory(pAdapter->CommonCfg.DesireRate, &aryRates, sizeof(NDIS_802_11_RATES));
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_DESIRED_RATES (%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x)\n",
+                    pAdapter->CommonCfg.DesireRate[0],pAdapter->CommonCfg.DesireRate[1],
+                    pAdapter->CommonCfg.DesireRate[2],pAdapter->CommonCfg.DesireRate[3],
+                    pAdapter->CommonCfg.DesireRate[4],pAdapter->CommonCfg.DesireRate[5],
+                    pAdapter->CommonCfg.DesireRate[6],pAdapter->CommonCfg.DesireRate[7] ));
+                // Changing DesiredRate may affect the MAX TX rate we used to TX frames out
+                MlmeUpdateTxRates(pAdapter, FALSE, 0);
+            }
+            break;
+        case RT_OID_802_11_PREAMBLE:
+            if (wrq->u.data.length != sizeof(RT_802_11_PREAMBLE))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&Preamble, wrq->u.data.pointer, wrq->u.data.length);
+                if (Preamble == Rt802_11PreambleShort)
+                {
+                    pAdapter->CommonCfg.TxPreamble = Preamble;
+                    MlmeSetTxPreamble(pAdapter, Rt802_11PreambleShort);
+                }
+                else if ((Preamble == Rt802_11PreambleLong) || (Preamble == Rt802_11PreambleAuto))
+                {
+                    // if user wants AUTO, initialize to LONG here, then change according to AP's
+                    // capability upon association.
+                    pAdapter->CommonCfg.TxPreamble = Preamble;
+                    MlmeSetTxPreamble(pAdapter, Rt802_11PreambleLong);
+                }
+                else
+                {
+                    Status = -EINVAL;
+                    break;
+                }
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_PREAMBLE (=%d)\n", Preamble));
+            }
+            break;
+        case OID_802_11_WEP_STATUS:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_WEP_STATUS))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&WepStatus, wrq->u.data.pointer, wrq->u.data.length);
+                // Since TKIP, AES, WEP are all supported. It should not have any invalid setting
+                if (WepStatus <= Ndis802_11Encryption3KeyAbsent)
+                {
+                    if (pAdapter->StaCfg.WepStatus != WepStatus)
+                    {
+                        // Config has changed
+                        pAdapter->bConfigChanged = TRUE;
+                    }
+                    pAdapter->StaCfg.WepStatus     = WepStatus;
+                    pAdapter->StaCfg.OrigWepStatus = WepStatus;
+                    pAdapter->StaCfg.PairCipher    = WepStatus;
+                	pAdapter->StaCfg.GroupCipher   = WepStatus;
+                }
+                else
+                {
+                    Status  = -EINVAL;
+                    break;
+                }
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_WEP_STATUS (=%d)\n",WepStatus));
+            }
+            break;
+        case OID_802_11_AUTHENTICATION_MODE:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_AUTHENTICATION_MODE))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&AuthMode, wrq->u.data.pointer, wrq->u.data.length);
+                if (AuthMode > Ndis802_11AuthModeMax)
+                {
+                    Status  = -EINVAL;
+                    break;
+                }
+                else
+                {
+                    if (pAdapter->StaCfg.AuthMode != AuthMode)
+                    {
+                        // Config has changed
+                        pAdapter->bConfigChanged = TRUE;
+                    }
+                    pAdapter->StaCfg.AuthMode = AuthMode;
+                }
+                pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_AUTHENTICATION_MODE (=%d) \n",pAdapter->StaCfg.AuthMode));
+            }
+            break;
+        case OID_802_11_INFRASTRUCTURE_MODE:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_NETWORK_INFRASTRUCTURE))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&BssType, wrq->u.data.pointer, wrq->u.data.length);
+
+				if (BssType == Ndis802_11IBSS)
+					Set_NetworkType_Proc(pAdapter, "Adhoc");
+				else if (BssType == Ndis802_11Infrastructure)
+					Set_NetworkType_Proc(pAdapter, "Infra");
+				else if (BssType == Ndis802_11Monitor)
+					Set_NetworkType_Proc(pAdapter, "Monitor");
+				else
+				{
+					Status  = -EINVAL;
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_INFRASTRUCTURE_MODE (unknown)\n"));
+				}
+			}
+			break;
+	 case OID_802_11_REMOVE_WEP:
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_WEP\n"));
+            if (wrq->u.data.length != sizeof(NDIS_802_11_KEY_INDEX))
+            {
+				Status = -EINVAL;
+            }
+            else
+            {
+				KeyIdx = *(NDIS_802_11_KEY_INDEX *) wrq->u.data.pointer;
+
+				if (KeyIdx & 0x80000000)
+				{
+					// Should never set default bit when remove key
+					Status = -EINVAL;
+				}
+				else
+				{
+					KeyIdx = KeyIdx & 0x0fffffff;
+					if (KeyIdx >= 4){
+						Status = -EINVAL;
+					}
+					else
+					{
+						pAdapter->SharedKey[BSS0][KeyIdx].KeyLen = 0;
+						pAdapter->SharedKey[BSS0][KeyIdx].CipherAlg = CIPHER_NONE;
+						AsicRemoveSharedKeyEntry(pAdapter, 0, (UCHAR)KeyIdx);
+					}
+				}
+            }
+            break;
+        case RT_OID_802_11_RESET_COUNTERS:
+            NdisZeroMemory(&pAdapter->WlanCounters, sizeof(COUNTER_802_11));
+            NdisZeroMemory(&pAdapter->Counters8023, sizeof(COUNTER_802_3));
+            NdisZeroMemory(&pAdapter->RalinkCounters, sizeof(COUNTER_RALINK));
+            pAdapter->Counters8023.RxNoBuffer   = 0;
+			pAdapter->Counters8023.GoodReceives = 0;
+			pAdapter->Counters8023.RxNoBuffer   = 0;
+#ifdef RT2870
+			pAdapter->BulkOutComplete	= 0;
+			pAdapter->BulkOutCompleteOther= 0;
+			pAdapter->BulkOutCompleteCancel = 0;
+			pAdapter->BulkOutReq = 0;
+			pAdapter->BulkInReq= 0;
+			pAdapter->BulkInComplete = 0;
+			pAdapter->BulkInCompleteFail = 0;
+#endif // RT2870 //
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_RESET_COUNTERS \n"));
+            break;
+        case OID_802_11_RTS_THRESHOLD:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_RTS_THRESHOLD))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&RtsThresh, wrq->u.data.pointer, wrq->u.data.length);
+                if (RtsThresh > MAX_RTS_THRESHOLD)
+                    Status  = -EINVAL;
+                else
+                    pAdapter->CommonCfg.RtsThreshold = (USHORT)RtsThresh;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_RTS_THRESHOLD (=%ld)\n",RtsThresh));
+            break;
+        case OID_802_11_FRAGMENTATION_THRESHOLD:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_FRAGMENTATION_THRESHOLD))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&FragThresh, wrq->u.data.pointer, wrq->u.data.length);
+                pAdapter->CommonCfg.bUseZeroToDisableFragment = FALSE;
+                if (FragThresh > MAX_FRAG_THRESHOLD || FragThresh < MIN_FRAG_THRESHOLD)
+                {
+                    if (FragThresh == 0)
+                    {
+                        pAdapter->CommonCfg.FragmentThreshold = MAX_FRAG_THRESHOLD;
+                        pAdapter->CommonCfg.bUseZeroToDisableFragment = TRUE;
+                    }
+                    else
+                        Status  = -EINVAL;
+                }
+                else
+                    pAdapter->CommonCfg.FragmentThreshold = (USHORT)FragThresh;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_FRAGMENTATION_THRESHOLD (=%ld) \n",FragThresh));
+            break;
+        case OID_802_11_POWER_MODE:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_POWER_MODE))
+                Status = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&PowerMode, wrq->u.data.pointer, wrq->u.data.length);
+                if (PowerMode == Ndis802_11PowerModeCAM)
+                	Set_PSMode_Proc(pAdapter, "CAM");
+                else if (PowerMode == Ndis802_11PowerModeMAX_PSP)
+                	Set_PSMode_Proc(pAdapter, "Max_PSP");
+                else if (PowerMode == Ndis802_11PowerModeFast_PSP)
+					Set_PSMode_Proc(pAdapter, "Fast_PSP");
+                else if (PowerMode == Ndis802_11PowerModeLegacy_PSP)
+					Set_PSMode_Proc(pAdapter, "Legacy_PSP");
+                else
+                    Status = -EINVAL;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_POWER_MODE (=%d)\n",PowerMode));
+            break;
+         case RT_OID_802_11_TX_POWER_LEVEL_1:
+			if (wrq->u.data.length  < sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&PowerTemp, wrq->u.data.pointer, wrq->u.data.length);
+				if (PowerTemp > 100)
+					PowerTemp = 0xffffffff;  // AUTO
+				pAdapter->CommonCfg.TxPowerDefault = PowerTemp; //keep current setting.
+					pAdapter->CommonCfg.TxPowerPercentage = pAdapter->CommonCfg.TxPowerDefault;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_TX_POWER_LEVEL_1 (=%ld)\n", pAdapter->CommonCfg.TxPowerPercentage));
+			}
+	        break;
+		case OID_802_11_NETWORK_TYPE_IN_USE:
+			if (wrq->u.data.length != sizeof(NDIS_802_11_NETWORK_TYPE))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&NetType, wrq->u.data.pointer, wrq->u.data.length);
+
+				if (NetType == Ndis802_11DS)
+					RTMPSetPhyMode(pAdapter, PHY_11B);
+				else if (NetType == Ndis802_11OFDM24)
+					RTMPSetPhyMode(pAdapter, PHY_11BG_MIXED);
+				else if (NetType == Ndis802_11OFDM5)
+					RTMPSetPhyMode(pAdapter, PHY_11A);
+				else
+					Status = -EINVAL;
+#ifdef DOT11_N_SUPPORT
+				if (Status == NDIS_STATUS_SUCCESS)
+					SetCommonHT(pAdapter);
+#endif // DOT11_N_SUPPORT //
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_NETWORK_TYPE_IN_USE (=%d)\n",NetType));
+		    }
+			break;
+        // For WPA PSK PMK key
+        case RT_OID_802_11_ADD_WPA:
+            pKey = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+            if(pKey == NULL)
+            {
+                Status = -ENOMEM;
+                break;
+            }
+
+            Status = copy_from_user(pKey, wrq->u.data.pointer, wrq->u.data.length);
+            if (pKey->Length != wrq->u.data.length)
+            {
+                Status  = -EINVAL;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_WPA, Failed!!\n"));
+            }
+            else
+            {
+                if ((pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPAPSK) &&
+				    (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPA2PSK) &&
+				    (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPANone) )
+                {
+                    Status = -EOPNOTSUPP;
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_WPA, Failed!! [AuthMode != WPAPSK/WPA2PSK/WPANONE]\n"));
+                }
+                else if ((pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPAPSK) ||
+						 (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPA2PSK) ||
+						 (pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPANone) )     // Only for WPA PSK mode
+				{
+                    NdisMoveMemory(pAdapter->StaCfg.PMK, &pKey->KeyMaterial, pKey->KeyLength);
+                    // Use RaConfig as PSK agent.
+                    // Start STA supplicant state machine
+                    if (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPANone)
+                        pAdapter->StaCfg.WpaState = SS_START;
+
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_WPA (id=0x%x, Len=%d-byte)\n", pKey->KeyIndex, pKey->KeyLength));
+                }
+                else
+                {
+                    pAdapter->StaCfg.WpaState = SS_NOTUSE;
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_WPA (id=0x%x, Len=%d-byte)\n", pKey->KeyIndex, pKey->KeyLength));
+                }
+            }
+            kfree(pKey);
+            break;
+        case OID_802_11_REMOVE_KEY:
+            pRemoveKey = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+            if(pRemoveKey == NULL)
+            {
+                Status = -ENOMEM;
+                break;
+            }
+
+            Status = copy_from_user(pRemoveKey, wrq->u.data.pointer, wrq->u.data.length);
+            if (pRemoveKey->Length != wrq->u.data.length)
+            {
+                Status  = -EINVAL;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY, Failed!!\n"));
+            }
+            else
+            {
+                if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+                {
+                    RTMPWPARemoveKeyProc(pAdapter, pRemoveKey);
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY, Remove WPA Key!!\n"));
+                }
+                else
+                {
+                    KeyIdx = pRemoveKey->KeyIndex;
+
+                    if (KeyIdx & 0x80000000)
+                    {
+                        // Should never set default bit when remove key
+                        Status  = -EINVAL;
+                        DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY, Failed!!(Should never set default bit when remove key)\n"));
+                    }
+                    else
+                    {
+                        KeyIdx = KeyIdx & 0x0fffffff;
+                        if (KeyIdx > 3)
+                        {
+                            Status  = -EINVAL;
+                            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY, Failed!!(KeyId[%d] out of range)\n", KeyIdx));
+                        }
+                        else
+                        {
+                            pAdapter->SharedKey[BSS0][KeyIdx].KeyLen = 0;
+                            pAdapter->SharedKey[BSS0][KeyIdx].CipherAlg = CIPHER_NONE;
+                            AsicRemoveSharedKeyEntry(pAdapter, 0, (UCHAR)KeyIdx);
+                            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_REMOVE_KEY (id=0x%x, Len=%d-byte)\n", pRemoveKey->KeyIndex, pRemoveKey->Length));
+                        }
+                    }
+                }
+            }
+            kfree(pRemoveKey);
+            break;
+        // New for WPA
+        case OID_802_11_ADD_KEY:
+            pKey = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+            if(pKey == NULL)
+            {
+                Status = -ENOMEM;
+                break;
+            }
+            Status = copy_from_user(pKey, wrq->u.data.pointer, wrq->u.data.length);
+            if (pKey->Length != wrq->u.data.length)
+            {
+                Status  = -EINVAL;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_KEY, Failed!!\n"));
+            }
+            else
+            {
+                RTMPAddKey(pAdapter, pKey);
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_KEY (id=0x%x, Len=%d-byte)\n", pKey->KeyIndex, pKey->KeyLength));
+            }
+            kfree(pKey);
+            break;
+        case OID_802_11_CONFIGURATION:
+            if (wrq->u.data.length != sizeof(NDIS_802_11_CONFIGURATION))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&Config, wrq->u.data.pointer, wrq->u.data.length);
+                pConfig = &Config;
+
+                if ((pConfig->BeaconPeriod >= 20) && (pConfig->BeaconPeriod <=400))
+                     pAdapter->CommonCfg.BeaconPeriod = (USHORT) pConfig->BeaconPeriod;
+
+                pAdapter->StaActive.AtimWin = (USHORT) pConfig->ATIMWindow;
+                MAP_KHZ_TO_CHANNEL_ID(pConfig->DSConfig, pAdapter->CommonCfg.Channel);
+                //
+				// Save the channel on MlmeAux for CntlOidRTBssidProc used.
+				//
+				pAdapter->MlmeAux.Channel = pAdapter->CommonCfg.Channel;
+
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_CONFIGURATION (BeacnPeriod=%ld,AtimW=%ld,Ch=%d)\n",
+                    pConfig->BeaconPeriod, pConfig->ATIMWindow, pAdapter->CommonCfg.Channel));
+                // Config has changed
+                pAdapter->bConfigChanged = TRUE;
+            }
+            break;
+#ifdef DOT11_N_SUPPORT
+		case RT_OID_802_11_SET_HT_PHYMODE:
+			if (wrq->u.data.length	!= sizeof(OID_SET_HT_PHYMODE))
+				Status = -EINVAL;
+			else
+			{
+			    POID_SET_HT_PHYMODE	pHTPhyMode = &HT_PhyMode;
+
+				Status = copy_from_user(&HT_PhyMode, wrq->u.data.pointer, wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::pHTPhyMode	(PhyMode = %d,TransmitNo = %d, HtMode =	%d,	ExtOffset =	%d , MCS = %d, BW =	%d,	STBC = %d, SHORTGI = %d) \n",
+				pHTPhyMode->PhyMode, pHTPhyMode->TransmitNo,pHTPhyMode->HtMode,pHTPhyMode->ExtOffset,
+				pHTPhyMode->MCS, pHTPhyMode->BW, pHTPhyMode->STBC,	pHTPhyMode->SHORTGI));
+				if (pAdapter->CommonCfg.PhyMode	>= PHY_11ABGN_MIXED)
+					RTMPSetHT(pAdapter,	pHTPhyMode);
+			}
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_HT_PHYMODE(MCS=%d,BW=%d,SGI=%d,STBC=%d)\n",
+				pAdapter->StaCfg.HTPhyMode.field.MCS, pAdapter->StaCfg.HTPhyMode.field.BW, pAdapter->StaCfg.HTPhyMode.field.ShortGI,
+				pAdapter->StaCfg.HTPhyMode.field.STBC));
+			break;
+#endif // DOT11_N_SUPPORT //
+		case RT_OID_802_11_SET_APSD_SETTING:
+			if (wrq->u.data.length != sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				ULONG apsd ;
+				Status = copy_from_user(&apsd, wrq->u.data.pointer,	wrq->u.data.length);
+
+				/*-------------------------------------------------------------------
+				|B31~B7	|	B6~B5	 |	 B4	 |	 B3	 |	B2	 |	B1	 |	   B0		|
+				---------------------------------------------------------------------
+				| Rsvd	| Max SP Len | AC_VO | AC_VI | AC_BK | AC_BE | APSD	Capable	|
+				---------------------------------------------------------------------*/
+				pAdapter->CommonCfg.bAPSDCapable = (apsd & 0x00000001) ? TRUE :	FALSE;
+				pAdapter->CommonCfg.bAPSDAC_BE = ((apsd	& 0x00000002) >> 1)	? TRUE : FALSE;
+				pAdapter->CommonCfg.bAPSDAC_BK = ((apsd	& 0x00000004) >> 2)	? TRUE : FALSE;
+				pAdapter->CommonCfg.bAPSDAC_VI = ((apsd	& 0x00000008) >> 3)	? TRUE : FALSE;
+				pAdapter->CommonCfg.bAPSDAC_VO = ((apsd	& 0x00000010) >> 4)	? TRUE : FALSE;
+				pAdapter->CommonCfg.MaxSPLength	= (UCHAR)((apsd	& 0x00000060) >> 5);
+
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_APSD_SETTING (apsd=0x%lx, APSDCap=%d, [BE,BK,VI,VO]=[%d/%d/%d/%d],	MaxSPLen=%d)\n", apsd, pAdapter->CommonCfg.bAPSDCapable,
+					pAdapter->CommonCfg.bAPSDAC_BE,	pAdapter->CommonCfg.bAPSDAC_BK,	pAdapter->CommonCfg.bAPSDAC_VI,	pAdapter->CommonCfg.bAPSDAC_VO,	pAdapter->CommonCfg.MaxSPLength));
+			}
+			break;
+
+		case RT_OID_802_11_SET_APSD_PSM:
+			if (wrq->u.data.length	!= sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				// Driver needs	to notify AP when PSM changes
+				Status = copy_from_user(&pAdapter->CommonCfg.bAPSDForcePowerSave, wrq->u.data.pointer, wrq->u.data.length);
+				if (pAdapter->CommonCfg.bAPSDForcePowerSave	!= pAdapter->StaCfg.Psm)
+				{
+					MlmeSetPsmBit(pAdapter,	pAdapter->CommonCfg.bAPSDForcePowerSave);
+					RTMPSendNullFrame(pAdapter,	pAdapter->CommonCfg.TxRate,	TRUE);
+				}
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_APSD_PSM (bAPSDForcePowerSave:%d)\n",	pAdapter->CommonCfg.bAPSDForcePowerSave));
+			}
+			break;
+#ifdef QOS_DLS_SUPPORT
+		case RT_OID_802_11_SET_DLS:
+			if (wrq->u.data.length != sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				BOOLEAN	oldvalue = pAdapter->CommonCfg.bDLSCapable;
+				Status = copy_from_user(&pAdapter->CommonCfg.bDLSCapable, wrq->u.data.pointer, wrq->u.data.length);
+				if (oldvalue &&	!pAdapter->CommonCfg.bDLSCapable)
+				{
+					int	i;
+					// tear	down local dls table entry
+					for	(i=0; i<MAX_NUM_OF_INIT_DLS_ENTRY; i++)
+					{
+						if (pAdapter->StaCfg.DLSEntry[i].Valid && (pAdapter->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+						{
+							pAdapter->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+							pAdapter->StaCfg.DLSEntry[i].Valid	= FALSE;
+							RTMPSendDLSTearDownFrame(pAdapter, pAdapter->StaCfg.DLSEntry[i].MacAddr);
+						}
+					}
+
+					// tear	down peer dls table	entry
+					for	(i=MAX_NUM_OF_INIT_DLS_ENTRY; i<MAX_NUM_OF_DLS_ENTRY; i++)
+					{
+						if (pAdapter->StaCfg.DLSEntry[i].Valid && (pAdapter->StaCfg.DLSEntry[i].Status == DLS_FINISH))
+						{
+							pAdapter->StaCfg.DLSEntry[i].Status	= DLS_NONE;
+							pAdapter->StaCfg.DLSEntry[i].Valid	= FALSE;
+							RTMPSendDLSTearDownFrame(pAdapter, pAdapter->StaCfg.DLSEntry[i].MacAddr);
+						}
+					}
+				}
+
+				DBGPRINT(RT_DEBUG_TRACE,("Set::RT_OID_802_11_SET_DLS (=%d)\n", pAdapter->CommonCfg.bDLSCapable));
+			}
+			break;
+
+		case RT_OID_802_11_SET_DLS_PARAM:
+			if (wrq->u.data.length	!= sizeof(RT_802_11_DLS_UI))
+				Status = -EINVAL;
+			else
+			{
+				RT_802_11_DLS	Dls;
+
+				NdisZeroMemory(&Dls, sizeof(RT_802_11_DLS));
+				RTMPMoveMemory(&Dls, wrq->u.data.pointer, sizeof(RT_802_11_DLS_UI));
+				MlmeEnqueue(pAdapter,
+							MLME_CNTL_STATE_MACHINE,
+							RT_OID_802_11_SET_DLS_PARAM,
+							sizeof(RT_802_11_DLS),
+							&Dls);
+				DBGPRINT(RT_DEBUG_TRACE,("Set::RT_OID_802_11_SET_DLS_PARAM \n"));
+			}
+			break;
+#endif // QOS_DLS_SUPPORT //
+		case RT_OID_802_11_SET_WMM:
+			if (wrq->u.data.length	!= sizeof(BOOLEAN))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&pAdapter->CommonCfg.bWmmCapable, wrq->u.data.pointer, wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_SET_WMM (=%d)	\n", pAdapter->CommonCfg.bWmmCapable));
+			}
+			break;
+
+		case OID_802_11_DISASSOCIATE:
+#ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+				break;
+			}
+#endif // RALINK_ATE //
+			//
+			// Set NdisRadioStateOff to	TRUE, instead of called	MlmeRadioOff.
+			// Later on, NDIS_802_11_BSSID_LIST_EX->NumberOfItems should be	0
+			// when	query OID_802_11_BSSID_LIST.
+			//
+			// TRUE:  NumberOfItems	will set to	0.
+			// FALSE: NumberOfItems	no change.
+			//
+			pAdapter->CommonCfg.NdisRadioStateOff =	TRUE;
+			// Set to immediately send the media disconnect	event
+			pAdapter->MlmeAux.CurrReqIsFromNdis	= TRUE;
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_DISASSOCIATE	\n"));
+
+			if (INFRA_ON(pAdapter))
+			{
+				if (pAdapter->Mlme.CntlMachine.CurrState !=	CNTL_IDLE)
+				{
+					RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+					DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME	busy, reset	MLME state machine !!!\n"));
+				}
+
+				MlmeEnqueue(pAdapter,
+					MLME_CNTL_STATE_MACHINE,
+					OID_802_11_DISASSOCIATE,
+					0,
+					NULL);
+
+				StateMachineTouched	= TRUE;
+			}
+			break;
+
+#ifdef DOT11_N_SUPPORT
+		case RT_OID_802_11_SET_IMME_BA_CAP:
+				if (wrq->u.data.length != sizeof(OID_BACAP_STRUC))
+					Status = -EINVAL;
+				else
+				{
+					OID_BACAP_STRUC Orde ;
+					Status = copy_from_user(&Orde, wrq->u.data.pointer, wrq->u.data.length);
+					if (Orde.Policy > BA_NOTUSE)
+					{
+						Status = NDIS_STATUS_INVALID_DATA;
+					}
+					else if (Orde.Policy == BA_NOTUSE)
+					{
+						pAdapter->CommonCfg.BACapability.field.Policy = BA_NOTUSE;
+						pAdapter->CommonCfg.BACapability.field.MpduDensity = Orde.MpduDensity;
+						pAdapter->CommonCfg.DesiredHtPhy.MpduDensity = Orde.MpduDensity;
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduEnable = Orde.AmsduEnable;
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduSize= Orde.AmsduSize;
+						pAdapter->CommonCfg.DesiredHtPhy.MimoPs= Orde.MMPSmode;
+						pAdapter->CommonCfg.BACapability.field.MMPSmode = Orde.MMPSmode;
+						// UPdata to HT IE
+						pAdapter->CommonCfg.HtCapability.HtCapInfo.MimoPs = Orde.MMPSmode;
+						pAdapter->CommonCfg.HtCapability.HtCapInfo.AMsduSize = Orde.AmsduSize;
+						pAdapter->CommonCfg.HtCapability.HtCapParm.MpduDensity = Orde.MpduDensity;
+					}
+					else
+					{
+                        pAdapter->CommonCfg.BACapability.field.AutoBA = Orde.AutoBA;
+						pAdapter->CommonCfg.BACapability.field.Policy = IMMED_BA; // we only support immediate BA.
+						pAdapter->CommonCfg.BACapability.field.MpduDensity = Orde.MpduDensity;
+						pAdapter->CommonCfg.DesiredHtPhy.MpduDensity = Orde.MpduDensity;
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduEnable = Orde.AmsduEnable;
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduSize= Orde.AmsduSize;
+						pAdapter->CommonCfg.DesiredHtPhy.MimoPs = Orde.MMPSmode;
+						pAdapter->CommonCfg.BACapability.field.MMPSmode = Orde.MMPSmode;
+
+						// UPdata to HT IE
+						pAdapter->CommonCfg.HtCapability.HtCapInfo.MimoPs = Orde.MMPSmode;
+						pAdapter->CommonCfg.HtCapability.HtCapInfo.AMsduSize = Orde.AmsduSize;
+						pAdapter->CommonCfg.HtCapability.HtCapParm.MpduDensity = Orde.MpduDensity;
+
+						if (pAdapter->CommonCfg.BACapability.field.RxBAWinLimit > MAX_RX_REORDERBUF)
+							pAdapter->CommonCfg.BACapability.field.RxBAWinLimit = MAX_RX_REORDERBUF;
+
+					}
+
+					pAdapter->CommonCfg.REGBACapability.word = pAdapter->CommonCfg.BACapability.word;
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::(Orde.AutoBA = %d) (Policy=%d)(ReBAWinLimit=%d)(TxBAWinLimit=%d)(AutoMode=%d)\n",Orde.AutoBA, pAdapter->CommonCfg.BACapability.field.Policy,
+						pAdapter->CommonCfg.BACapability.field.RxBAWinLimit,pAdapter->CommonCfg.BACapability.field.TxBAWinLimit, pAdapter->CommonCfg.BACapability.field.AutoBA));
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::(MimoPs = %d)(AmsduEnable = %d) (AmsduSize=%d)(MpduDensity=%d)\n",pAdapter->CommonCfg.DesiredHtPhy.MimoPs, pAdapter->CommonCfg.DesiredHtPhy.AmsduEnable,
+						pAdapter->CommonCfg.DesiredHtPhy.AmsduSize, pAdapter->CommonCfg.DesiredHtPhy.MpduDensity));
+				}
+
+				break;
+		case RT_OID_802_11_ADD_IMME_BA:
+			DBGPRINT(RT_DEBUG_TRACE, (" Set :: RT_OID_802_11_ADD_IMME_BA \n"));
+			if (wrq->u.data.length != sizeof(OID_ADD_BA_ENTRY))
+					Status = -EINVAL;
+			else
+			{
+				UCHAR		        index;
+				OID_ADD_BA_ENTRY    BA;
+				MAC_TABLE_ENTRY     *pEntry;
+
+				Status = copy_from_user(&BA, wrq->u.data.pointer, wrq->u.data.length);
+				if (BA.TID > 15)
+				{
+					Status = NDIS_STATUS_INVALID_DATA;
+					break;
+				}
+				else
+				{
+					//BATableInsertEntry
+					//As ad-hoc mode, BA pair is not limited to only BSSID. so add via OID.
+					index = BA.TID;
+					// in ad hoc mode, when adding BA pair, we should insert this entry into MACEntry too
+					pEntry = MacTableLookup(pAdapter, BA.MACAddr);
+					if (!pEntry)
+					{
+						DBGPRINT(RT_DEBUG_TRACE, ("RT_OID_802_11_ADD_IMME_BA. break on no connection.----:%x:%x\n", BA.MACAddr[4], BA.MACAddr[5]));
+						break;
+					}
+					if (BA.IsRecipient == FALSE)
+					{
+					    if (pEntry->bIAmBadAtheros == TRUE)
+							pAdapter->CommonCfg.BACapability.field.RxBAWinLimit = 0x10;
+
+						BAOriSessionSetUp(pAdapter, pEntry, index, 0, 100, TRUE);
+					}
+					else
+					{
+						//BATableInsertEntry(pAdapter, pEntry->Aid, BA.MACAddr, 0, 0xffff, BA.TID, BA.nMSDU, BA.IsRecipient);
+					}
+
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_802_11_ADD_IMME_BA. Rec = %d. Mac = %x:%x:%x:%x:%x:%x . \n",
+						BA.IsRecipient, BA.MACAddr[0], BA.MACAddr[1], BA.MACAddr[2], BA.MACAddr[2]
+						, BA.MACAddr[4], BA.MACAddr[5]));
+				}
+			}
+			break;
+
+		case RT_OID_802_11_TEAR_IMME_BA:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set :: RT_OID_802_11_TEAR_IMME_BA \n"));
+			if (wrq->u.data.length != sizeof(OID_ADD_BA_ENTRY))
+					Status = -EINVAL;
+			else
+			{
+				POID_ADD_BA_ENTRY	pBA;
+				MAC_TABLE_ENTRY *pEntry;
+
+				pBA = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+
+				if (pBA == NULL)
+				{
+					DBGPRINT(RT_DEBUG_TRACE, ("Set :: RT_OID_802_11_TEAR_IMME_BA kmalloc() can't allocate enough memory\n"));
+					Status = NDIS_STATUS_FAILURE;
+				}
+				else
+				{
+					Status = copy_from_user(pBA, wrq->u.data.pointer, wrq->u.data.length);
+					DBGPRINT(RT_DEBUG_TRACE, ("Set :: RT_OID_802_11_TEAR_IMME_BA(TID=%d, bAllTid=%d)\n", pBA->TID, pBA->bAllTid));
+
+					if (!pBA->bAllTid && (pBA->TID > NUM_OF_TID))
+					{
+						Status = NDIS_STATUS_INVALID_DATA;
+						break;
+					}
+
+					if (pBA->IsRecipient == FALSE)
+					{
+						pEntry = MacTableLookup(pAdapter, pBA->MACAddr);
+						DBGPRINT(RT_DEBUG_TRACE, (" pBA->IsRecipient == FALSE\n"));
+						if (pEntry)
+						{
+							DBGPRINT(RT_DEBUG_TRACE, (" pBA->pEntry\n"));
+							BAOriSessionTearDown(pAdapter, pEntry->Aid, pBA->TID, FALSE, TRUE);
+						}
+						else
+							DBGPRINT(RT_DEBUG_TRACE, ("Set :: Not found pEntry \n"));
+					}
+					else
+					{
+						pEntry = MacTableLookup(pAdapter, pBA->MACAddr);
+						if (pEntry)
+						{
+							BARecSessionTearDown( pAdapter, (UCHAR)pEntry->Aid, pBA->TID, TRUE);
+						}
+						else
+							DBGPRINT(RT_DEBUG_TRACE, ("Set :: Not found pEntry \n"));
+					}
+					kfree(pBA);
+				}
+            }
+            break;
+#endif // DOT11_N_SUPPORT //
+
+        // For WPA_SUPPLICANT to set static wep key
+    	case OID_802_11_ADD_WEP:
+    	    pWepKey = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+
+    	    if(pWepKey == NULL)
+            {
+                Status = -ENOMEM;
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP, Failed!!\n"));
+                break;
+            }
+            Status = copy_from_user(pWepKey, wrq->u.data.pointer, wrq->u.data.length);
+            if (Status)
+            {
+                Status  = -EINVAL;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP, Failed (length mismatch)!!\n"));
+            }
+            else
+            {
+		        KeyIdx = pWepKey->KeyIndex & 0x0fffffff;
+                // KeyIdx must be 0 ~ 3
+                if (KeyIdx > 4)
+    			{
+                    Status  = -EINVAL;
+                    DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP, Failed (KeyIdx must be smaller than 4)!!\n"));
+                }
+                else
+                {
+                    UCHAR CipherAlg = 0;
+                    PUCHAR Key;
+
+                    // set key material and key length
+                    NdisZeroMemory(pAdapter->SharedKey[BSS0][KeyIdx].Key, 16);
+                    pAdapter->SharedKey[BSS0][KeyIdx].KeyLen = (UCHAR) pWepKey->KeyLength;
+                    NdisMoveMemory(pAdapter->SharedKey[BSS0][KeyIdx].Key, &pWepKey->KeyMaterial, pWepKey->KeyLength);
+
+                    switch(pWepKey->KeyLength)
+                    {
+                        case 5:
+                            CipherAlg = CIPHER_WEP64;
+                            break;
+                        case 13:
+                            CipherAlg = CIPHER_WEP128;
+                            break;
+                        default:
+                            DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP, only support CIPHER_WEP64(len:5) & CIPHER_WEP128(len:13)!!\n"));
+                            Status = -EINVAL;
+                            break;
+                    }
+                    pAdapter->SharedKey[BSS0][KeyIdx].CipherAlg = CipherAlg;
+
+                    // Default key for tx (shared key)
+                    if (pWepKey->KeyIndex & 0x80000000)
+                    {
+#ifdef WPA_SUPPLICANT_SUPPORT
+                        // set key material and key length
+                        NdisZeroMemory(pAdapter->StaCfg.DesireSharedKey[KeyIdx].Key, 16);
+                        pAdapter->StaCfg.DesireSharedKey[KeyIdx].KeyLen = (UCHAR) pWepKey->KeyLength;
+                        NdisMoveMemory(pAdapter->StaCfg.DesireSharedKey[KeyIdx].Key, &pWepKey->KeyMaterial, pWepKey->KeyLength);
+                        pAdapter->StaCfg.DesireSharedKeyId = KeyIdx;
+                        pAdapter->StaCfg.DesireSharedKey[KeyIdx].CipherAlg = CipherAlg;
+#endif // WPA_SUPPLICANT_SUPPORT //
+                        pAdapter->StaCfg.DefaultKeyId = (UCHAR) KeyIdx;
+                    }
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+                    if (pAdapter->StaCfg.PortSecured == WPA_802_1X_PORT_SECURED)
+#endif // WPA_SUPPLICANT_SUPPORT
+                    {
+                        Key = pAdapter->SharedKey[BSS0][KeyIdx].Key;
+
+                        // Set key material and cipherAlg to Asic
+        				AsicAddSharedKeyEntry(pAdapter, BSS0, KeyIdx, CipherAlg, Key, NULL, NULL);
+
+                        if (pWepKey->KeyIndex & 0x80000000)
+                        {
+                            PMAC_TABLE_ENTRY pEntry = &pAdapter->MacTab.Content[BSSID_WCID];
+                            // Assign group key info
+    						RTMPAddWcidAttributeEntry(pAdapter, BSS0, KeyIdx, CipherAlg, NULL);
+    						// Assign pairwise key info
+    						RTMPAddWcidAttributeEntry(pAdapter, BSS0, KeyIdx, CipherAlg, pEntry);
+                        }
+                    }
+					DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_ADD_WEP (id=0x%x, Len=%d-byte), %s\n", pWepKey->KeyIndex, pWepKey->KeyLength, (pAdapter->StaCfg.PortSecured == WPA_802_1X_PORT_SECURED) ? "Port Secured":"Port NOT Secured"));
+				}
+            }
+            kfree(pWepKey);
+            break;
+#ifdef WPA_SUPPLICANT_SUPPORT
+	    case OID_SET_COUNTERMEASURES:
+            if (wrq->u.data.length != sizeof(int))
+                Status  = -EINVAL;
+            else
+            {
+                int enabled = 0;
+                Status = copy_from_user(&enabled, wrq->u.data.pointer, wrq->u.data.length);
+                if (enabled == 1)
+                    pAdapter->StaCfg.bBlockAssoc = TRUE;
+                else
+                    // WPA MIC error should block association attempt for 60 seconds
+                    pAdapter->StaCfg.bBlockAssoc = FALSE;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_SET_COUNTERMEASURES bBlockAssoc=%s\n", pAdapter->StaCfg.bBlockAssoc ? "TRUE":"FALSE"));
+            }
+	        break;
+        case RT_OID_WPA_SUPPLICANT_SUPPORT:
+			if (wrq->u.data.length != sizeof(UCHAR))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&wpa_supplicant_enable, wrq->u.data.pointer, wrq->u.data.length);
+    			pAdapter->StaCfg.WpaSupplicantUP = wpa_supplicant_enable;
+    			DBGPRINT(RT_DEBUG_TRACE, ("Set::RT_OID_WPA_SUPPLICANT_SUPPORT (=%d)\n", pAdapter->StaCfg.WpaSupplicantUP));
+			}
+            break;
+        case OID_802_11_DEAUTHENTICATION:
+            if (wrq->u.data.length != sizeof(MLME_DEAUTH_REQ_STRUCT))
+                Status  = -EINVAL;
+            else
+            {
+                MLME_DEAUTH_REQ_STRUCT      *pInfo;
+				MLME_QUEUE_ELEM *MsgElem = (MLME_QUEUE_ELEM *) kmalloc(sizeof(MLME_QUEUE_ELEM), MEM_ALLOC_FLAG);
+
+                pInfo = (MLME_DEAUTH_REQ_STRUCT *) MsgElem->Msg;
+                Status = copy_from_user(pInfo, wrq->u.data.pointer, wrq->u.data.length);
+                MlmeDeauthReqAction(pAdapter, MsgElem);
+				kfree(MsgElem);
+
+                if (INFRA_ON(pAdapter))
+                {
+                    LinkDown(pAdapter, FALSE);
+                    pAdapter->Mlme.AssocMachine.CurrState = ASSOC_IDLE;
+                }
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_DEAUTHENTICATION (Reason=%d)\n", pInfo->Reason));
+            }
+            break;
+        case OID_802_11_DROP_UNENCRYPTED:
+            if (wrq->u.data.length != sizeof(int))
+                Status  = -EINVAL;
+            else
+            {
+                int enabled = 0;
+                Status = copy_from_user(&enabled, wrq->u.data.pointer, wrq->u.data.length);
+                if (enabled == 1)
+                    pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+                else
+                    pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_SECURED;
+				NdisAcquireSpinLock(&pAdapter->MacTabLock);
+				pAdapter->MacTab.Content[BSSID_WCID].PortSecured = pAdapter->StaCfg.PortSecured;
+				NdisReleaseSpinLock(&pAdapter->MacTabLock);
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_DROP_UNENCRYPTED (=%d)\n", enabled));
+            }
+            break;
+        case OID_802_11_SET_IEEE8021X:
+            if (wrq->u.data.length != sizeof(BOOLEAN))
+                Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&IEEE8021xState, wrq->u.data.pointer, wrq->u.data.length);
+		        pAdapter->StaCfg.IEEE8021X = IEEE8021xState;
+                DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_SET_IEEE8021X (=%d)\n", IEEE8021xState));
+            }
+            break;
+        case OID_802_11_SET_IEEE8021X_REQUIRE_KEY:
+			if (wrq->u.data.length != sizeof(BOOLEAN))
+				 Status  = -EINVAL;
+            else
+            {
+                Status = copy_from_user(&IEEE8021x_required_keys, wrq->u.data.pointer, wrq->u.data.length);
+				pAdapter->StaCfg.IEEE8021x_required_keys = IEEE8021x_required_keys;
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_SET_IEEE8021X_REQUIRE_KEY (%d)\n", IEEE8021x_required_keys));
+			}
+			break;
+        case OID_802_11_PMKID:
+	        pPmkId = kmalloc(wrq->u.data.length, MEM_ALLOC_FLAG);
+
+	        if(pPmkId == NULL) {
+                Status = -ENOMEM;
+                break;
+            }
+            Status = copy_from_user(pPmkId, wrq->u.data.pointer, wrq->u.data.length);
+
+	        // check the PMKID information
+	        if (pPmkId->BSSIDInfoCount == 0)
+                NdisZeroMemory(pAdapter->StaCfg.SavedPMK, sizeof(BSSID_INFO)*PMKID_NO);
+	        else
+	        {
+		        PBSSID_INFO	pBssIdInfo;
+		        UINT		BssIdx;
+		        UINT		CachedIdx;
+
+		        for (BssIdx = 0; BssIdx < pPmkId->BSSIDInfoCount; BssIdx++)
+		        {
+			        // point to the indexed BSSID_INFO structure
+			        pBssIdInfo = (PBSSID_INFO) ((PUCHAR) pPmkId + 2 * sizeof(UINT) + BssIdx * sizeof(BSSID_INFO));
+			        // Find the entry in the saved data base.
+			        for (CachedIdx = 0; CachedIdx < pAdapter->StaCfg.SavedPMKNum; CachedIdx++)
+			        {
+				        // compare the BSSID
+				        if (NdisEqualMemory(pBssIdInfo->BSSID, pAdapter->StaCfg.SavedPMK[CachedIdx].BSSID, sizeof(NDIS_802_11_MAC_ADDRESS)))
+					        break;
+			        }
+
+			        // Found, replace it
+			        if (CachedIdx < PMKID_NO)
+			        {
+				        DBGPRINT(RT_DEBUG_OFF, ("Update OID_802_11_PMKID, idx = %d\n", CachedIdx));
+				        NdisMoveMemory(&pAdapter->StaCfg.SavedPMK[CachedIdx], pBssIdInfo, sizeof(BSSID_INFO));
+				        pAdapter->StaCfg.SavedPMKNum++;
+			        }
+			        // Not found, replace the last one
+			        else
+			        {
+				        // Randomly replace one
+				        CachedIdx = (pBssIdInfo->BSSID[5] % PMKID_NO);
+				        DBGPRINT(RT_DEBUG_OFF, ("Update OID_802_11_PMKID, idx = %d\n", CachedIdx));
+				        NdisMoveMemory(&pAdapter->StaCfg.SavedPMK[CachedIdx], pBssIdInfo, sizeof(BSSID_INFO));
+			        }
+		        }
+			}
+			if(pPmkId)
+				kfree(pPmkId);
+	        break;
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+
+
+#ifdef SNMP_SUPPORT
+		case OID_802_11_SHORTRETRYLIMIT:
+			if (wrq->u.data.length != sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&ShortRetryLimit, wrq->u.data.pointer, wrq->u.data.length);
+				RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+				tx_rty_cfg.field.ShortRtyLimit = ShortRetryLimit;
+				RTMP_IO_WRITE32(pAdapter, TX_RTY_CFG, tx_rty_cfg.word);
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_SHORTRETRYLIMIT (tx_rty_cfg.field.ShortRetryLimit=%d, ShortRetryLimit=%ld)\n", tx_rty_cfg.field.ShortRtyLimit, ShortRetryLimit));
+			}
+			break;
+
+		case OID_802_11_LONGRETRYLIMIT:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_LONGRETRYLIMIT \n"));
+			if (wrq->u.data.length != sizeof(ULONG))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&LongRetryLimit, wrq->u.data.pointer, wrq->u.data.length);
+				RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+				tx_rty_cfg.field.LongRtyLimit = LongRetryLimit;
+				RTMP_IO_WRITE32(pAdapter, TX_RTY_CFG, tx_rty_cfg.word);
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_LONGRETRYLIMIT (tx_rty_cfg.field.LongRetryLimit= %d,LongRetryLimit=%ld)\n", tx_rty_cfg.field.LongRtyLimit, LongRetryLimit));
+			}
+			break;
+
+		case OID_802_11_WEPDEFAULTKEYVALUE:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_WEPDEFAULTKEYVALUE\n"));
+			pKey = kmalloc(wrq->u.data.length, GFP_KERNEL);
+			Status = copy_from_user(pKey, wrq->u.data.pointer, wrq->u.data.length);
+			//pKey = &WepKey;
+
+			if ( pKey->Length != wrq->u.data.length)
+			{
+				Status = -EINVAL;
+				DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_WEPDEFAULTKEYVALUE, Failed!!\n"));
+			}
+			KeyIdx = pKey->KeyIndex & 0x0fffffff;
+			DBGPRINT(RT_DEBUG_TRACE,("pKey->KeyIndex =%d, pKey->KeyLength=%d\n", pKey->KeyIndex, pKey->KeyLength));
+
+			// it is a shared key
+			if (KeyIdx > 4)
+				Status = -EINVAL;
+			else
+			{
+				pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen = (UCHAR) pKey->KeyLength;
+				NdisMoveMemory(&pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].Key, &pKey->KeyMaterial, pKey->KeyLength);
+				if (pKey->KeyIndex & 0x80000000)
+				{
+					// Default key for tx (shared key)
+					pAdapter->StaCfg.DefaultKeyId = (UCHAR) KeyIdx;
+				}
+				//RestartAPIsRequired = TRUE;
+			}
+			break;
+
+
+		case OID_802_11_WEPDEFAULTKEYID:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_WEPDEFAULTKEYID \n"));
+
+			if (wrq->u.data.length != sizeof(UCHAR))
+				Status = -EINVAL;
+			else
+				Status = copy_from_user(&pAdapter->StaCfg.DefaultKeyId, wrq->u.data.pointer, wrq->u.data.length);
+
+			break;
+
+
+		case OID_802_11_CURRENTCHANNEL:
+			DBGPRINT(RT_DEBUG_TRACE, ("Set::OID_802_11_CURRENTCHANNEL \n"));
+			if (wrq->u.data.length != sizeof(UCHAR))
+				Status = -EINVAL;
+			else
+			{
+				Status = copy_from_user(&ctmp, wrq->u.data.pointer, wrq->u.data.length);
+				sprintf(&ctmp,"%d", ctmp);
+				Set_Channel_Proc(pAdapter, &ctmp);
+			}
+			break;
+#endif
+
+
+
+        default:
+            DBGPRINT(RT_DEBUG_TRACE, ("Set::unknown IOCTL's subcmd = 0x%08x\n", cmd));
+            Status = -EOPNOTSUPP;
+            break;
+    }
+
+
+    return Status;
+}
+
+INT RTMPQueryInformation(
+    IN  PRTMP_ADAPTER pAdapter,
+    IN  OUT struct ifreq    *rq,
+    IN  INT                 cmd)
+{
+    struct iwreq                        *wrq = (struct iwreq *) rq;
+    NDIS_802_11_BSSID_LIST_EX           *pBssidList = NULL;
+    PNDIS_WLAN_BSSID_EX                 pBss;
+    NDIS_802_11_SSID                    Ssid;
+    NDIS_802_11_CONFIGURATION           *pConfiguration = NULL;
+    RT_802_11_LINK_STATUS               *pLinkStatus = NULL;
+    RT_802_11_STA_CONFIG                *pStaConfig = NULL;
+    NDIS_802_11_STATISTICS              *pStatistics = NULL;
+    NDIS_802_11_RTS_THRESHOLD           RtsThresh;
+    NDIS_802_11_FRAGMENTATION_THRESHOLD FragThresh;
+    NDIS_802_11_POWER_MODE              PowerMode;
+    NDIS_802_11_NETWORK_INFRASTRUCTURE  BssType;
+    RT_802_11_PREAMBLE                  PreamType;
+    NDIS_802_11_AUTHENTICATION_MODE     AuthMode;
+    NDIS_802_11_WEP_STATUS              WepStatus;
+    NDIS_MEDIA_STATE                    MediaState;
+    ULONG                               BssBufSize, ulInfo=0, NetworkTypeList[4], apsd = 0;
+    USHORT                              BssLen = 0;
+    PUCHAR                              pBuf = NULL, pPtr;
+    INT                                 Status = NDIS_STATUS_SUCCESS;
+    UINT                                we_version_compiled;
+    UCHAR                               i, Padding = 0;
+    BOOLEAN                             RadioState;
+	UCHAR	driverVersion[8];
+    OID_SET_HT_PHYMODE			        *pHTPhyMode = NULL;
+
+
+#ifdef SNMP_SUPPORT
+	//for snmp, kathy
+	DefaultKeyIdxValue			*pKeyIdxValue;
+	INT							valueLen;
+	TX_RTY_CFG_STRUC			tx_rty_cfg;
+	ULONG						ShortRetryLimit, LongRetryLimit;
+	UCHAR						tmp[64];
+#endif //SNMP
+
+    switch(cmd)
+    {
+        case RT_OID_DEVICE_NAME:
+            wrq->u.data.length = sizeof(STA_NIC_DEVICE_NAME);
+            Status = copy_to_user(wrq->u.data.pointer, STA_NIC_DEVICE_NAME, wrq->u.data.length);
+            break;
+        case RT_OID_VERSION_INFO:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_VERSION_INFO \n"));
+			wrq->u.data.length = 8*sizeof(UCHAR);
+			sprintf(&driverVersion[0], "%s", STA_DRIVER_VERSION);
+			driverVersion[7] = '\0';
+			if (copy_to_user(wrq->u.data.pointer, &driverVersion, wrq->u.data.length))
+            {
+				Status = -EFAULT;
+            }
+            break;
+#ifdef RALINK_ATE
+		case RT_QUERY_ATE_TXDONE_COUNT:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_QUERY_ATE_TXDONE_COUNT \n"));
+			wrq->u.data.length = sizeof(UINT32);
+			if (copy_to_user(wrq->u.data.pointer, &pAdapter->ate.TxDoneCount, wrq->u.data.length))
+			{
+				Status = -EFAULT;
+			}
+			break;
+#endif // RALINK_ATE //
+        case OID_802_11_BSSID_LIST:
+            if (RTMP_TEST_FLAG(pAdapter, fRTMP_ADAPTER_BSS_SCAN_IN_PROGRESS))
+            {
+            	/*
+            	 * Still scanning, indicate the caller should try again.
+            	 */
+            	DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_BSSID_LIST (Still scanning)\n"));
+				return -EAGAIN;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_BSSID_LIST (%d BSS returned)\n",pAdapter->ScanTab.BssNr));
+			pAdapter->StaCfg.bScanReqIsFromWebUI = FALSE;
+            // Claculate total buffer size required
+            BssBufSize = sizeof(ULONG);
+
+            for (i = 0; i < pAdapter->ScanTab.BssNr; i++)
+            {
+                // Align pointer to 4 bytes boundary.
+                //Padding = 4 - (pAdapter->ScanTab.BssEntry[i].VarIELen & 0x0003);
+                //if (Padding == 4)
+                //    Padding = 0;
+                BssBufSize += (sizeof(NDIS_WLAN_BSSID_EX) - 1 + sizeof(NDIS_802_11_FIXED_IEs) + pAdapter->ScanTab.BssEntry[i].VarIELen + Padding);
+            }
+
+            // For safety issue, we add 256 bytes just in case
+            BssBufSize += 256;
+            // Allocate the same size as passed from higher layer
+            pBuf = kmalloc(BssBufSize, MEM_ALLOC_FLAG);
+            if(pBuf == NULL)
+            {
+                Status = -ENOMEM;
+                break;
+            }
+            // Init 802_11_BSSID_LIST_EX structure
+            NdisZeroMemory(pBuf, BssBufSize);
+            pBssidList = (PNDIS_802_11_BSSID_LIST_EX) pBuf;
+            pBssidList->NumberOfItems = pAdapter->ScanTab.BssNr;
+
+            // Calculate total buffer length
+            BssLen = 4; // Consist of NumberOfItems
+            // Point to start of NDIS_WLAN_BSSID_EX
+            // pPtr = pBuf + sizeof(ULONG);
+            pPtr = (PUCHAR) &pBssidList->Bssid[0];
+            for (i = 0; i < pAdapter->ScanTab.BssNr; i++)
+            {
+                pBss = (PNDIS_WLAN_BSSID_EX) pPtr;
+                NdisMoveMemory(&pBss->MacAddress, &pAdapter->ScanTab.BssEntry[i].Bssid, MAC_ADDR_LEN);
+                if ((pAdapter->ScanTab.BssEntry[i].Hidden == 1) && (pAdapter->StaCfg.bShowHiddenSSID == FALSE))
+                {
+                    //
+					// We must return this SSID during 4way handshaking, otherwise Aegis will failed to parse WPA infomation
+					// and then failed to send EAPOl farame.
+					//
+					if ((pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA) && (pAdapter->StaCfg.PortSecured != WPA_802_1X_PORT_SECURED))
+					{
+						pBss->Ssid.SsidLength = pAdapter->ScanTab.BssEntry[i].SsidLen;
+						NdisMoveMemory(pBss->Ssid.Ssid, pAdapter->ScanTab.BssEntry[i].Ssid, pAdapter->ScanTab.BssEntry[i].SsidLen);
+					}
+					else
+                    	pBss->Ssid.SsidLength = 0;
+                }
+                else
+                {
+                    pBss->Ssid.SsidLength = pAdapter->ScanTab.BssEntry[i].SsidLen;
+                    NdisMoveMemory(pBss->Ssid.Ssid, pAdapter->ScanTab.BssEntry[i].Ssid, pAdapter->ScanTab.BssEntry[i].SsidLen);
+                }
+                pBss->Privacy = pAdapter->ScanTab.BssEntry[i].Privacy;
+                pBss->Rssi = pAdapter->ScanTab.BssEntry[i].Rssi - pAdapter->BbpRssiToDbmDelta;
+                pBss->NetworkTypeInUse = NetworkTypeInUseSanity(&pAdapter->ScanTab.BssEntry[i]);
+                pBss->Configuration.Length = sizeof(NDIS_802_11_CONFIGURATION);
+                pBss->Configuration.BeaconPeriod = pAdapter->ScanTab.BssEntry[i].BeaconPeriod;
+                pBss->Configuration.ATIMWindow = pAdapter->ScanTab.BssEntry[i].AtimWin;
+
+                MAP_CHANNEL_ID_TO_KHZ(pAdapter->ScanTab.BssEntry[i].Channel, pBss->Configuration.DSConfig);
+
+                if (pAdapter->ScanTab.BssEntry[i].BssType == BSS_INFRA)
+                    pBss->InfrastructureMode = Ndis802_11Infrastructure;
+                else
+                    pBss->InfrastructureMode = Ndis802_11IBSS;
+
+                NdisMoveMemory(pBss->SupportedRates, pAdapter->ScanTab.BssEntry[i].SupRate, pAdapter->ScanTab.BssEntry[i].SupRateLen);
+                NdisMoveMemory(pBss->SupportedRates + pAdapter->ScanTab.BssEntry[i].SupRateLen,
+                               pAdapter->ScanTab.BssEntry[i].ExtRate,
+                               pAdapter->ScanTab.BssEntry[i].ExtRateLen);
+
+                if (pAdapter->ScanTab.BssEntry[i].VarIELen == 0)
+                {
+                    pBss->IELength = sizeof(NDIS_802_11_FIXED_IEs);
+                    NdisMoveMemory(pBss->IEs, &pAdapter->ScanTab.BssEntry[i].FixIEs, sizeof(NDIS_802_11_FIXED_IEs));
+                    pPtr = pPtr + sizeof(NDIS_WLAN_BSSID_EX) - 1 + sizeof(NDIS_802_11_FIXED_IEs);
+                }
+                else
+                {
+                    pBss->IELength = (ULONG)(sizeof(NDIS_802_11_FIXED_IEs) + pAdapter->ScanTab.BssEntry[i].VarIELen);
+                    pPtr = pPtr + sizeof(NDIS_WLAN_BSSID_EX) - 1 + sizeof(NDIS_802_11_FIXED_IEs);
+                    NdisMoveMemory(pBss->IEs, &pAdapter->ScanTab.BssEntry[i].FixIEs, sizeof(NDIS_802_11_FIXED_IEs));
+                    NdisMoveMemory(pBss->IEs + sizeof(NDIS_802_11_FIXED_IEs), pAdapter->ScanTab.BssEntry[i].VarIEs, pAdapter->ScanTab.BssEntry[i].VarIELen);
+                    pPtr += pAdapter->ScanTab.BssEntry[i].VarIELen;
+                }
+                pBss->Length = (ULONG)(sizeof(NDIS_WLAN_BSSID_EX) - 1 + sizeof(NDIS_802_11_FIXED_IEs) + pAdapter->ScanTab.BssEntry[i].VarIELen + Padding);
+
+#if WIRELESS_EXT < 17
+                if ((BssLen + pBss->Length) < wrq->u.data.length)
+                BssLen += pBss->Length;
+                else
+                {
+                    pBssidList->NumberOfItems = i;
+                    break;
+                }
+#else
+                BssLen += pBss->Length;
+#endif
+            }
+
+#if WIRELESS_EXT < 17
+            wrq->u.data.length = BssLen;
+#else
+            if (BssLen > wrq->u.data.length)
+            {
+                kfree(pBssidList);
+                return -E2BIG;
+            }
+            else
+                wrq->u.data.length = BssLen;
+#endif
+            Status = copy_to_user(wrq->u.data.pointer, pBssidList, BssLen);
+            kfree(pBssidList);
+            break;
+        case OID_802_3_CURRENT_ADDRESS:
+            wrq->u.data.length = MAC_ADDR_LEN;
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CurrentAddress, wrq->u.data.length);
+            break;
+        case OID_GEN_MEDIA_CONNECT_STATUS:
+            if (pAdapter->IndicateMediaState == NdisMediaStateConnected)
+                MediaState = NdisMediaStateConnected;
+            else
+                MediaState = NdisMediaStateDisconnected;
+
+            wrq->u.data.length = sizeof(NDIS_MEDIA_STATE);
+            Status = copy_to_user(wrq->u.data.pointer, &MediaState, wrq->u.data.length);
+            break;
+        case OID_802_11_BSSID:
+#ifdef RALINK_ATE
+			if (ATE_ON(pAdapter))
+			{
+				DBGPRINT(RT_DEBUG_TRACE, ("The driver is in ATE mode now\n"));
+				Status = NDIS_STATUS_RESOURCES;
+				break;
+			}
+#endif // RALINK_ATE //
+            if (INFRA_ON(pAdapter) || ADHOC_ON(pAdapter))
+            {
+                Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.Bssid, sizeof(NDIS_802_11_MAC_ADDRESS));
+
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_BSSID(=EMPTY)\n"));
+                Status = -ENOTCONN;
+            }
+            break;
+        case OID_802_11_SSID:
+			NdisZeroMemory(&Ssid, sizeof(NDIS_802_11_SSID));
+			NdisZeroMemory(Ssid.Ssid, MAX_LEN_OF_SSID);
+            Ssid.SsidLength = pAdapter->CommonCfg.SsidLen;
+			memcpy(Ssid.Ssid, pAdapter->CommonCfg.Ssid,	Ssid.SsidLength);
+            wrq->u.data.length = sizeof(NDIS_802_11_SSID);
+            Status = copy_to_user(wrq->u.data.pointer, &Ssid, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_SSID (Len=%d, ssid=%s)\n", Ssid.SsidLength,Ssid.Ssid));
+            break;
+        case RT_OID_802_11_QUERY_LINK_STATUS:
+            pLinkStatus = (RT_802_11_LINK_STATUS *) kmalloc(sizeof(RT_802_11_LINK_STATUS), MEM_ALLOC_FLAG);
+            if (pLinkStatus)
+            {
+                pLinkStatus->CurrTxRate = RateIdTo500Kbps[pAdapter->CommonCfg.TxRate];   // unit : 500 kbps
+                pLinkStatus->ChannelQuality = pAdapter->Mlme.ChannelQuality;
+                pLinkStatus->RxByteCount = pAdapter->RalinkCounters.ReceivedByteCount;
+                pLinkStatus->TxByteCount = pAdapter->RalinkCounters.TransmittedByteCount;
+        		pLinkStatus->CentralChannel = pAdapter->CommonCfg.CentralChannel;
+                wrq->u.data.length = sizeof(RT_802_11_LINK_STATUS);
+                Status = copy_to_user(wrq->u.data.pointer, pLinkStatus, wrq->u.data.length);
+                kfree(pLinkStatus);
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_LINK_STATUS\n"));
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_LINK_STATUS(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case OID_802_11_CONFIGURATION:
+            pConfiguration = (NDIS_802_11_CONFIGURATION *) kmalloc(sizeof(NDIS_802_11_CONFIGURATION), MEM_ALLOC_FLAG);
+            if (pConfiguration)
+            {
+                pConfiguration->Length = sizeof(NDIS_802_11_CONFIGURATION);
+                pConfiguration->BeaconPeriod = pAdapter->CommonCfg.BeaconPeriod;
+                pConfiguration->ATIMWindow = pAdapter->StaActive.AtimWin;
+                MAP_CHANNEL_ID_TO_KHZ(pAdapter->CommonCfg.Channel, pConfiguration->DSConfig);
+                wrq->u.data.length = sizeof(NDIS_802_11_CONFIGURATION);
+                Status = copy_to_user(wrq->u.data.pointer, pConfiguration, wrq->u.data.length);
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_CONFIGURATION(BeaconPeriod=%ld,AtimW=%ld,Channel=%d) \n",
+                                        pConfiguration->BeaconPeriod, pConfiguration->ATIMWindow, pAdapter->CommonCfg.Channel));
+				kfree(pConfiguration);
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_CONFIGURATION(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+		case RT_OID_802_11_SNR_0:
+			if ((pAdapter->StaCfg.LastSNR0 > 0))
+			{
+				ulInfo = ((0xeb	- pAdapter->StaCfg.LastSNR0) * 3) /	16 ;
+				wrq->u.data.length = sizeof(ulInfo);
+				Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_SNR_0(0x=%lx)\n", ulInfo));
+			}
+            else
+			    Status = -EFAULT;
+			break;
+		case RT_OID_802_11_SNR_1:
+			if ((pAdapter->Antenna.field.RxPath	> 1) &&
+                (pAdapter->StaCfg.LastSNR1 > 0))
+			{
+				ulInfo = ((0xeb	- pAdapter->StaCfg.LastSNR1) * 3) /	16 ;
+				wrq->u.data.length = sizeof(ulInfo);
+				Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE,("Query::RT_OID_802_11_SNR_1(0x=%lx)\n",ulInfo));
+			}
+			else
+				Status = -EFAULT;
+            DBGPRINT(RT_DEBUG_TRACE,("Query::RT_OID_802_11_SNR_1(pAdapter->StaCfg.LastSNR1=%d)\n",pAdapter->StaCfg.LastSNR1));
+			break;
+        case OID_802_11_RSSI_TRIGGER:
+            ulInfo = pAdapter->StaCfg.RssiSample.LastRssi0 - pAdapter->BbpRssiToDbmDelta;
+            wrq->u.data.length = sizeof(ulInfo);
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_RSSI_TRIGGER(=%ld)\n", ulInfo));
+            break;
+		case OID_802_11_RSSI:
+        case RT_OID_802_11_RSSI:
+			ulInfo = pAdapter->StaCfg.RssiSample.LastRssi0;
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+			break;
+		case RT_OID_802_11_RSSI_1:
+            ulInfo = pAdapter->StaCfg.RssiSample.LastRssi1;
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+			break;
+        case RT_OID_802_11_RSSI_2:
+            ulInfo = pAdapter->StaCfg.RssiSample.LastRssi2;
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+			break;
+        case OID_802_11_STATISTICS:
+            pStatistics = (NDIS_802_11_STATISTICS *) kmalloc(sizeof(NDIS_802_11_STATISTICS), MEM_ALLOC_FLAG);
+            if (pStatistics)
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_STATISTICS \n"));
+                // add the most up-to-date h/w raw counters into software counters
+			    NICUpdateRawCounters(pAdapter);
+
+                // Sanity check for calculation of sucessful count
+                if (pAdapter->WlanCounters.TransmittedFragmentCount.QuadPart < pAdapter->WlanCounters.RetryCount.QuadPart)
+                    pAdapter->WlanCounters.TransmittedFragmentCount.QuadPart = pAdapter->WlanCounters.RetryCount.QuadPart;
+
+                pStatistics->TransmittedFragmentCount.QuadPart = pAdapter->WlanCounters.TransmittedFragmentCount.QuadPart;
+                pStatistics->MulticastTransmittedFrameCount.QuadPart = pAdapter->WlanCounters.MulticastTransmittedFrameCount.QuadPart;
+                pStatistics->FailedCount.QuadPart = pAdapter->WlanCounters.FailedCount.QuadPart;
+                pStatistics->RetryCount.QuadPart = pAdapter->WlanCounters.RetryCount.QuadPart;
+                pStatistics->MultipleRetryCount.QuadPart = pAdapter->WlanCounters.MultipleRetryCount.QuadPart;
+                pStatistics->RTSSuccessCount.QuadPart = pAdapter->WlanCounters.RTSSuccessCount.QuadPart;
+                pStatistics->RTSFailureCount.QuadPart = pAdapter->WlanCounters.RTSFailureCount.QuadPart;
+                pStatistics->ACKFailureCount.QuadPart = pAdapter->WlanCounters.ACKFailureCount.QuadPart;
+                pStatistics->FrameDuplicateCount.QuadPart = pAdapter->WlanCounters.FrameDuplicateCount.QuadPart;
+                pStatistics->ReceivedFragmentCount.QuadPart = pAdapter->WlanCounters.ReceivedFragmentCount.QuadPart;
+                pStatistics->MulticastReceivedFrameCount.QuadPart = pAdapter->WlanCounters.MulticastReceivedFrameCount.QuadPart;
+#ifdef DBG
+                pStatistics->FCSErrorCount = pAdapter->RalinkCounters.RealFcsErrCount;
+#else
+                pStatistics->FCSErrorCount.QuadPart = pAdapter->WlanCounters.FCSErrorCount.QuadPart;
+                pStatistics->FrameDuplicateCount.u.LowPart = pAdapter->WlanCounters.FrameDuplicateCount.u.LowPart / 100;
+#endif
+                wrq->u.data.length = sizeof(NDIS_802_11_STATISTICS);
+                Status = copy_to_user(wrq->u.data.pointer, pStatistics, wrq->u.data.length);
+                kfree(pStatistics);
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_STATISTICS(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case OID_GEN_RCV_OK:
+            ulInfo = pAdapter->Counters8023.GoodReceives;
+            wrq->u.data.length = sizeof(ulInfo);
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+            break;
+        case OID_GEN_RCV_NO_BUFFER:
+            ulInfo = pAdapter->Counters8023.RxNoBuffer;
+            wrq->u.data.length = sizeof(ulInfo);
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+            break;
+        case RT_OID_802_11_PHY_MODE:
+            ulInfo = (ULONG)pAdapter->CommonCfg.PhyMode;
+            wrq->u.data.length = sizeof(ulInfo);
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_PHY_MODE (=%ld)\n", ulInfo));
+            break;
+        case RT_OID_802_11_STA_CONFIG:
+            pStaConfig = (RT_802_11_STA_CONFIG *) kmalloc(sizeof(RT_802_11_STA_CONFIG), MEM_ALLOC_FLAG);
+            if (pStaConfig)
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_STA_CONFIG\n"));
+                pStaConfig->EnableTxBurst = pAdapter->CommonCfg.bEnableTxBurst;
+                pStaConfig->EnableTurboRate = 0;
+                pStaConfig->UseBGProtection = pAdapter->CommonCfg.UseBGProtection;
+                pStaConfig->UseShortSlotTime = pAdapter->CommonCfg.bUseShortSlotTime;
+                //pStaConfig->AdhocMode = pAdapter->StaCfg.AdhocMode;
+                pStaConfig->HwRadioStatus = (pAdapter->StaCfg.bHwRadio == TRUE) ? 1 : 0;
+                pStaConfig->Rsv1 = 0;
+                pStaConfig->SystemErrorBitmap = pAdapter->SystemErrorBitmap;
+                wrq->u.data.length = sizeof(RT_802_11_STA_CONFIG);
+                Status = copy_to_user(wrq->u.data.pointer, pStaConfig, wrq->u.data.length);
+                kfree(pStaConfig);
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_STA_CONFIG(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case OID_802_11_RTS_THRESHOLD:
+            RtsThresh = pAdapter->CommonCfg.RtsThreshold;
+            wrq->u.data.length = sizeof(RtsThresh);
+            Status = copy_to_user(wrq->u.data.pointer, &RtsThresh, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_RTS_THRESHOLD(=%ld)\n", RtsThresh));
+            break;
+        case OID_802_11_FRAGMENTATION_THRESHOLD:
+            FragThresh = pAdapter->CommonCfg.FragmentThreshold;
+            if (pAdapter->CommonCfg.bUseZeroToDisableFragment == TRUE)
+                FragThresh = 0;
+            wrq->u.data.length = sizeof(FragThresh);
+            Status = copy_to_user(wrq->u.data.pointer, &FragThresh, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_FRAGMENTATION_THRESHOLD(=%ld)\n", FragThresh));
+            break;
+        case OID_802_11_POWER_MODE:
+            PowerMode = pAdapter->StaCfg.WindowsPowerMode;
+            wrq->u.data.length = sizeof(PowerMode);
+            Status = copy_to_user(wrq->u.data.pointer, &PowerMode, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_POWER_MODE(=%d)\n", PowerMode));
+            break;
+        case RT_OID_802_11_RADIO:
+            RadioState = (BOOLEAN) pAdapter->StaCfg.bSwRadio;
+            wrq->u.data.length = sizeof(RadioState);
+            Status = copy_to_user(wrq->u.data.pointer, &RadioState, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_RADIO (=%d)\n", RadioState));
+            break;
+        case OID_802_11_INFRASTRUCTURE_MODE:
+            if (pAdapter->StaCfg.BssType == BSS_ADHOC)
+                BssType = Ndis802_11IBSS;
+            else if (pAdapter->StaCfg.BssType == BSS_INFRA)
+                BssType = Ndis802_11Infrastructure;
+            else if (pAdapter->StaCfg.BssType == BSS_MONITOR)
+                BssType = Ndis802_11Monitor;
+            else
+                BssType = Ndis802_11AutoUnknown;
+
+            wrq->u.data.length = sizeof(BssType);
+            Status = copy_to_user(wrq->u.data.pointer, &BssType, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_INFRASTRUCTURE_MODE(=%d)\n", BssType));
+            break;
+        case RT_OID_802_11_PREAMBLE:
+            PreamType = pAdapter->CommonCfg.TxPreamble;
+            wrq->u.data.length = sizeof(PreamType);
+            Status = copy_to_user(wrq->u.data.pointer, &PreamType, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_PREAMBLE(=%d)\n", PreamType));
+            break;
+        case OID_802_11_AUTHENTICATION_MODE:
+            AuthMode = pAdapter->StaCfg.AuthMode;
+            wrq->u.data.length = sizeof(AuthMode);
+            Status = copy_to_user(wrq->u.data.pointer, &AuthMode, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_AUTHENTICATION_MODE(=%d)\n", AuthMode));
+            break;
+        case OID_802_11_WEP_STATUS:
+            WepStatus = pAdapter->StaCfg.WepStatus;
+            wrq->u.data.length = sizeof(WepStatus);
+            Status = copy_to_user(wrq->u.data.pointer, &WepStatus, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_WEP_STATUS(=%d)\n", WepStatus));
+            break;
+        case OID_802_11_TX_POWER_LEVEL:
+			wrq->u.data.length = sizeof(ULONG);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.TxPower, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_TX_POWER_LEVEL %x\n",pAdapter->CommonCfg.TxPower));
+			break;
+        case RT_OID_802_11_TX_POWER_LEVEL_1:
+            wrq->u.data.length = sizeof(ULONG);
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.TxPowerPercentage, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_TX_POWER_LEVEL_1 (=%ld)\n", pAdapter->CommonCfg.TxPowerPercentage));
+			break;
+        case OID_802_11_NETWORK_TYPES_SUPPORTED:
+			if ((pAdapter->RfIcType	== RFIC_2850) || (pAdapter->RfIcType ==	RFIC_2750))
+			{
+				NetworkTypeList[0] = 3;                 // NumberOfItems = 3
+				NetworkTypeList[1] = Ndis802_11DS;      // NetworkType[1] = 11b
+				NetworkTypeList[2] = Ndis802_11OFDM24;  // NetworkType[2] = 11g
+				NetworkTypeList[3] = Ndis802_11OFDM5;   // NetworkType[3] = 11a
+                wrq->u.data.length = 16;
+				Status = copy_to_user(wrq->u.data.pointer, &NetworkTypeList[0], wrq->u.data.length);
+			}
+			else
+			{
+				NetworkTypeList[0] = 2;                 // NumberOfItems = 2
+				NetworkTypeList[1] = Ndis802_11DS;      // NetworkType[1] = 11b
+				NetworkTypeList[2] = Ndis802_11OFDM24;  // NetworkType[2] = 11g
+			    wrq->u.data.length = 12;
+				Status = copy_to_user(wrq->u.data.pointer, &NetworkTypeList[0], wrq->u.data.length);
+			}
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_NETWORK_TYPES_SUPPORTED\n"));
+				break;
+	    case OID_802_11_NETWORK_TYPE_IN_USE:
+            wrq->u.data.length = sizeof(ULONG);
+			if (pAdapter->CommonCfg.PhyMode == PHY_11A)
+				ulInfo = Ndis802_11OFDM5;
+			else if ((pAdapter->CommonCfg.PhyMode == PHY_11BG_MIXED) || (pAdapter->CommonCfg.PhyMode == PHY_11G))
+				ulInfo = Ndis802_11OFDM24;
+			else
+				ulInfo = Ndis802_11DS;
+            Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+			break;
+        case RT_OID_802_11_QUERY_LAST_RX_RATE:
+            ulInfo = (ULONG)pAdapter->LastRxRate;
+            wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_LAST_RX_RATE (=%ld)\n", ulInfo));
+			break;
+		case RT_OID_802_11_QUERY_LAST_TX_RATE:
+			//ulInfo = (ULONG)pAdapter->LastTxRate;
+			ulInfo = (ULONG)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.word;
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo,	wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_LAST_TX_RATE (=%lx)\n", ulInfo));
+			break;
+        case RT_OID_802_11_QUERY_EEPROM_VERSION:
+            wrq->u.data.length = sizeof(ULONG);
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->EepromVersion, wrq->u.data.length);
+            break;
+        case RT_OID_802_11_QUERY_FIRMWARE_VERSION:
+            wrq->u.data.length = sizeof(ULONG);
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->FirmwareVersion, wrq->u.data.length);
+			break;
+	    case RT_OID_802_11_QUERY_NOISE_LEVEL:
+			wrq->u.data.length = sizeof(UCHAR);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->BbpWriteLatch[66], wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_NOISE_LEVEL (=%d)\n", pAdapter->BbpWriteLatch[66]));
+			break;
+	    case RT_OID_802_11_EXTRA_INFO:
+			wrq->u.data.length = sizeof(ULONG);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->ExtraInfo, wrq->u.data.length);
+	        DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_EXTRA_INFO (=%ld)\n", pAdapter->ExtraInfo));
+	        break;
+	    case RT_OID_WE_VERSION_COMPILED:
+	        wrq->u.data.length = sizeof(UINT);
+	        we_version_compiled = WIRELESS_EXT;
+	        Status = copy_to_user(wrq->u.data.pointer, &we_version_compiled, wrq->u.data.length);
+	        break;
+		case RT_OID_802_11_QUERY_APSD_SETTING:
+			apsd = (pAdapter->CommonCfg.bAPSDCapable | (pAdapter->CommonCfg.bAPSDAC_BE << 1) | (pAdapter->CommonCfg.bAPSDAC_BK << 2)
+				| (pAdapter->CommonCfg.bAPSDAC_VI << 3)	| (pAdapter->CommonCfg.bAPSDAC_VO << 4)	| (pAdapter->CommonCfg.MaxSPLength << 5));
+
+			wrq->u.data.length = sizeof(ULONG);
+			Status = copy_to_user(wrq->u.data.pointer, &apsd, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_APSD_SETTING (=0x%lx,APSDCap=%d,AC_BE=%d,AC_BK=%d,AC_VI=%d,AC_VO=%d,MAXSPLen=%d)\n",
+				apsd,pAdapter->CommonCfg.bAPSDCapable,pAdapter->CommonCfg.bAPSDAC_BE,pAdapter->CommonCfg.bAPSDAC_BK,pAdapter->CommonCfg.bAPSDAC_VI,pAdapter->CommonCfg.bAPSDAC_VO,pAdapter->CommonCfg.MaxSPLength));
+			break;
+		case RT_OID_802_11_QUERY_APSD_PSM:
+			wrq->u.data.length = sizeof(ULONG);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.bAPSDForcePowerSave, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_APSD_PSM (=%d)\n", pAdapter->CommonCfg.bAPSDForcePowerSave));
+			break;
+		case RT_OID_802_11_QUERY_WMM:
+			wrq->u.data.length = sizeof(BOOLEAN);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.bWmmCapable, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_WMM (=%d)\n",	pAdapter->CommonCfg.bWmmCapable));
+			break;
+#ifdef WPA_SUPPLICANT_SUPPORT
+        case RT_OID_NEW_DRIVER:
+            {
+                UCHAR enabled = 1;
+    	        wrq->u.data.length = sizeof(UCHAR);
+    	        Status = copy_to_user(wrq->u.data.pointer, &enabled, wrq->u.data.length);
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_NEW_DRIVER (=%d)\n", enabled));
+            }
+	        break;
+        case RT_OID_WPA_SUPPLICANT_SUPPORT:
+	        wrq->u.data.length = sizeof(UCHAR);
+	        Status = copy_to_user(wrq->u.data.pointer, &pAdapter->StaCfg.WpaSupplicantUP, wrq->u.data.length);
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_WPA_SUPPLICANT_SUPPORT (=%d)\n", pAdapter->StaCfg.WpaSupplicantUP));
+	        break;
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+        case RT_OID_DRIVER_DEVICE_NAME:
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_DRIVER_DEVICE_NAME \n"));
+			wrq->u.data.length = 16;
+			if (copy_to_user(wrq->u.data.pointer, pAdapter->StaCfg.dev_name, wrq->u.data.length))
+			{
+				Status = -EFAULT;
+			}
+            break;
+        case RT_OID_802_11_QUERY_HT_PHYMODE:
+            pHTPhyMode = (OID_SET_HT_PHYMODE *) kmalloc(sizeof(OID_SET_HT_PHYMODE), MEM_ALLOC_FLAG);
+            if (pHTPhyMode)
+            {
+                pHTPhyMode->PhyMode = pAdapter->CommonCfg.PhyMode;
+    			pHTPhyMode->HtMode = (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.MODE;
+    			pHTPhyMode->BW = (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.BW;
+    			pHTPhyMode->MCS= (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.MCS;
+    			pHTPhyMode->SHORTGI= (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.ShortGI;
+    			pHTPhyMode->STBC= (UCHAR)pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.field.STBC;
+
+    			pHTPhyMode->ExtOffset = ((pAdapter->CommonCfg.CentralChannel < pAdapter->CommonCfg.Channel) ? (EXTCHA_BELOW) : (EXTCHA_ABOVE));
+                wrq->u.data.length = sizeof(OID_SET_HT_PHYMODE);
+                if (copy_to_user(wrq->u.data.pointer, pHTPhyMode, wrq->u.data.length))
+    			{
+    				Status = -EFAULT;
+    			}
+    			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_HT_PHYMODE (PhyMode = %d, MCS =%d, BW = %d, STBC = %d, ExtOffset=%d)\n",
+    				pHTPhyMode->HtMode, pHTPhyMode->MCS, pHTPhyMode->BW, pHTPhyMode->STBC, pHTPhyMode->ExtOffset));
+    			DBGPRINT(RT_DEBUG_TRACE, (" MlmeUpdateTxRates (.word = %x )\n", pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.word));
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_STA_CONFIG(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case RT_OID_802_11_COUNTRY_REGION:
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_COUNTRY_REGION \n"));
+			wrq->u.data.length = sizeof(ulInfo);
+            ulInfo = pAdapter->CommonCfg.CountryRegionForABand;
+            ulInfo = (ulInfo << 8)|(pAdapter->CommonCfg.CountryRegion);
+			if (copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length))
+            {
+				Status = -EFAULT;
+            }
+            break;
+        case RT_OID_802_11_QUERY_DAT_HT_PHYMODE:
+            pHTPhyMode = (OID_SET_HT_PHYMODE *) kmalloc(sizeof(OID_SET_HT_PHYMODE), MEM_ALLOC_FLAG);
+            if (pHTPhyMode)
+            {
+                pHTPhyMode->PhyMode = pAdapter->CommonCfg.PhyMode;
+    			pHTPhyMode->HtMode = (UCHAR)pAdapter->CommonCfg.RegTransmitSetting.field.HTMODE;
+    			pHTPhyMode->BW = (UCHAR)pAdapter->CommonCfg.RegTransmitSetting.field.BW;
+    			pHTPhyMode->MCS= (UCHAR)pAdapter->StaCfg.DesiredTransmitSetting.field.MCS;
+    			pHTPhyMode->SHORTGI= (UCHAR)pAdapter->CommonCfg.RegTransmitSetting.field.ShortGI;
+    			pHTPhyMode->STBC= (UCHAR)pAdapter->CommonCfg.RegTransmitSetting.field.STBC;
+
+                wrq->u.data.length = sizeof(OID_SET_HT_PHYMODE);
+                if (copy_to_user(wrq->u.data.pointer, pHTPhyMode, wrq->u.data.length))
+    			{
+    				Status = -EFAULT;
+    			}
+    			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_HT_PHYMODE (PhyMode = %d, MCS =%d, BW = %d, STBC = %d, ExtOffset=%d)\n",
+    				pHTPhyMode->HtMode, pHTPhyMode->MCS, pHTPhyMode->BW, pHTPhyMode->STBC, pHTPhyMode->ExtOffset));
+    			DBGPRINT(RT_DEBUG_TRACE, (" MlmeUpdateTxRates (.word = %x )\n", pAdapter->MacTab.Content[BSSID_WCID].HTPhyMode.word));
+            }
+            else
+            {
+                DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_STA_CONFIG(kmalloc failed)\n"));
+                Status = -EFAULT;
+            }
+            break;
+        case RT_OID_QUERY_MULTIPLE_CARD_SUPPORT:
+			wrq->u.data.length = sizeof(UCHAR);
+            i = 0;
+#ifdef MULTIPLE_CARD_SUPPORT
+            i = 1;
+#endif // MULTIPLE_CARD_SUPPORT //
+			if (copy_to_user(wrq->u.data.pointer, &i, wrq->u.data.length))
+            {
+				Status = -EFAULT;
+            }
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_QUERY_MULTIPLE_CARD_SUPPORT(=%d) \n", i));
+            break;
+#ifdef SNMP_SUPPORT
+		case RT_OID_802_11_MAC_ADDRESS:
+            wrq->u.data.length = MAC_ADDR_LEN;
+            Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CurrentAddress, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_MANUFACTUREROUI:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_MANUFACTUREROUI \n"));
+			wrq->u.data.length = ManufacturerOUI_LEN;
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CurrentAddress, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_MANUFACTURERNAME:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_MANUFACTURERNAME \n"));
+			wrq->u.data.length = strlen(ManufacturerNAME);
+			Status = copy_to_user(wrq->u.data.pointer, ManufacturerNAME, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_RESOURCETYPEIDNAME:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_RESOURCETYPEIDNAME \n"));
+			wrq->u.data.length = strlen(ResourceTypeIdName);
+			Status = copy_to_user(wrq->u.data.pointer, ResourceTypeIdName, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_PRIVACYOPTIONIMPLEMENTED:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_PRIVACYOPTIONIMPLEMENTED \n"));
+			ulInfo = 1; // 1 is support wep else 2 is not support.
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_POWERMANAGEMENTMODE:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_POWERMANAGEMENTMODE \n"));
+			if (pAdapter->StaCfg.Psm == PSMP_ACTION)
+				ulInfo = 1; // 1 is power active else 2 is power save.
+			else
+				ulInfo = 2;
+
+			wrq->u.data.length = sizeof(ulInfo);
+			Status = copy_to_user(wrq->u.data.pointer, &ulInfo, wrq->u.data.length);
+			break;
+
+		case OID_802_11_WEPDEFAULTKEYVALUE:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_WEPDEFAULTKEYVALUE \n"));
+			//KeyIdxValue.KeyIdx = pAd->PortCfg.MBSSID[pAd->IoctlIF].DefaultKeyId;
+			pKeyIdxValue = wrq->u.data.pointer;
+			DBGPRINT(RT_DEBUG_TRACE,("KeyIdxValue.KeyIdx = %d, \n",pKeyIdxValue->KeyIdx));
+			valueLen = pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen;
+			NdisMoveMemory(pKeyIdxValue->Value,
+						   &pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].Key,
+						   valueLen);
+			pKeyIdxValue->Value[valueLen]='\0';
+
+			wrq->u.data.length = sizeof(DefaultKeyIdxValue);
+
+			Status = copy_to_user(wrq->u.data.pointer, pKeyIdxValue, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE,("DefaultKeyId = %d, total len = %d, str len=%d, KeyValue= %02x %02x %02x %02x \n", pAdapter->StaCfg.DefaultKeyId, wrq->u.data.length, pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen,
+			pAdapter->SharedKey[BSS0][0].Key[0],
+			pAdapter->SharedKey[BSS0][1].Key[0],
+			pAdapter->SharedKey[BSS0][2].Key[0],
+			pAdapter->SharedKey[BSS0][3].Key[0]));
+			break;
+
+		case OID_802_11_WEPDEFAULTKEYID:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_WEPDEFAULTKEYID \n"));
+			wrq->u.data.length = sizeof(UCHAR);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->StaCfg.DefaultKeyId, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("DefaultKeyId =%d \n", pAdapter->StaCfg.DefaultKeyId));
+			break;
+
+		case RT_OID_802_11_WEPKEYMAPPINGLENGTH:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_WEPKEYMAPPINGLENGTH \n"));
+			wrq->u.data.length = sizeof(UCHAR);
+			Status = copy_to_user(wrq->u.data.pointer,
+									&pAdapter->SharedKey[BSS0][pAdapter->StaCfg.DefaultKeyId].KeyLen,
+									wrq->u.data.length);
+			break;
+
+		case OID_802_11_SHORTRETRYLIMIT:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_SHORTRETRYLIMIT \n"));
+			wrq->u.data.length = sizeof(ULONG);
+			RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+			ShortRetryLimit = tx_rty_cfg.field.ShortRtyLimit;
+			DBGPRINT(RT_DEBUG_TRACE, ("ShortRetryLimit =%ld,  tx_rty_cfg.field.ShortRetryLimit=%d\n", ShortRetryLimit, tx_rty_cfg.field.ShortRtyLimit));
+			Status = copy_to_user(wrq->u.data.pointer, &ShortRetryLimit, wrq->u.data.length);
+			break;
+
+		case OID_802_11_LONGRETRYLIMIT:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_LONGRETRYLIMIT \n"));
+			wrq->u.data.length = sizeof(ULONG);
+			RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+			LongRetryLimit = tx_rty_cfg.field.LongRtyLimit;
+			DBGPRINT(RT_DEBUG_TRACE, ("LongRetryLimit =%ld,  tx_rty_cfg.field.LongRtyLimit=%d\n", LongRetryLimit, tx_rty_cfg.field.LongRtyLimit));
+			Status = copy_to_user(wrq->u.data.pointer, &LongRetryLimit, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_PRODUCTID:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_PRODUCTID \n"));
+
+#ifdef RT2870
+			sprintf(tmp, "%04x %04x\n", ((POS_COOKIE)pAdapter->OS_Cookie)->pUsb_Dev->descriptor.idVendor ,((POS_COOKIE)pAdapter->OS_Cookie)->pUsb_Dev->descriptor.idProduct);
+
+#endif // RT2870 //
+			wrq->u.data.length = strlen(tmp);
+			Status = copy_to_user(wrq->u.data.pointer, tmp, wrq->u.data.length);
+			break;
+
+		case RT_OID_802_11_MANUFACTUREID:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_MANUFACTUREID \n"));
+			wrq->u.data.length = strlen(ManufacturerNAME);
+			Status = copy_to_user(wrq->u.data.pointer, ManufacturerNAME, wrq->u.data.length);
+			break;
+
+		case OID_802_11_CURRENTCHANNEL:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_CURRENTCHANNEL \n"));
+			wrq->u.data.length = sizeof(UCHAR);
+			DBGPRINT(RT_DEBUG_TRACE, ("sizeof UCHAR=%d, channel=%d \n", sizeof(UCHAR), pAdapter->CommonCfg.Channel));
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.Channel, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+			break;
+#endif //SNMP_SUPPORT
+
+		case OID_802_11_BUILD_CHANNEL_EX:
+			{
+				UCHAR value;
+				DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_BUILD_CHANNEL_EX \n"));
+				wrq->u.data.length = sizeof(UCHAR);
+#ifdef EXT_BUILD_CHANNEL_LIST
+				DBGPRINT(RT_DEBUG_TRACE, ("Support EXT_BUILD_CHANNEL_LIST.\n"));
+				value = 1;
+#else
+				DBGPRINT(RT_DEBUG_TRACE, ("Doesn't support EXT_BUILD_CHANNEL_LIST.\n"));
+				value = 0;
+#endif // EXT_BUILD_CHANNEL_LIST //
+				Status = copy_to_user(wrq->u.data.pointer, &value, 1);
+				DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+			}
+			break;
+
+		case OID_802_11_GET_CH_LIST:
+			{
+				PRT_CHANNEL_LIST_INFO pChListBuf;
+
+				DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_GET_CH_LIST \n"));
+				if (pAdapter->ChannelListNum == 0)
+				{
+					wrq->u.data.length = 0;
+					break;
+				}
+
+				pChListBuf = (RT_CHANNEL_LIST_INFO *) kmalloc(sizeof(RT_CHANNEL_LIST_INFO), MEM_ALLOC_FLAG);
+				if (pChListBuf == NULL)
+				{
+					wrq->u.data.length = 0;
+					break;
+				}
+
+				pChListBuf->ChannelListNum = pAdapter->ChannelListNum;
+				for (i = 0; i < pChListBuf->ChannelListNum; i++)
+					pChListBuf->ChannelList[i] = pAdapter->ChannelList[i].Channel;
+
+				wrq->u.data.length = sizeof(RT_CHANNEL_LIST_INFO);
+				Status = copy_to_user(wrq->u.data.pointer, pChListBuf, sizeof(RT_CHANNEL_LIST_INFO));
+				DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+
+				if (pChListBuf)
+					kfree(pChListBuf);
+			}
+			break;
+
+		case OID_802_11_GET_COUNTRY_CODE:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_GET_COUNTRY_CODE \n"));
+			wrq->u.data.length = 2;
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.CountryCode, 2);
+			DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+			break;
+
+		case OID_802_11_GET_CHANNEL_GEOGRAPHY:
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::OID_802_11_GET_CHANNEL_GEOGRAPHY \n"));
+			wrq->u.data.length = 1;
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.Geography, 1);
+			DBGPRINT(RT_DEBUG_TRACE, ("Status=%d\n", Status));
+			break;
+
+
+#ifdef QOS_DLS_SUPPORT
+		case RT_OID_802_11_QUERY_DLS:
+			wrq->u.data.length = sizeof(BOOLEAN);
+			Status = copy_to_user(wrq->u.data.pointer, &pAdapter->CommonCfg.bDLSCapable, wrq->u.data.length);
+			DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_DLS(=%d)\n", pAdapter->CommonCfg.bDLSCapable));
+			break;
+
+		case RT_OID_802_11_QUERY_DLS_PARAM:
+			{
+				PRT_802_11_DLS_INFO	pDlsInfo = kmalloc(sizeof(RT_802_11_DLS_INFO), GFP_ATOMIC);
+				if (pDlsInfo == NULL)
+					break;
+
+				for (i=0; i<MAX_NUM_OF_DLS_ENTRY; i++)
+				{
+					RTMPMoveMemory(&pDlsInfo->Entry[i], &pAdapter->StaCfg.DLSEntry[i], sizeof(RT_802_11_DLS_UI));
+				}
+
+				pDlsInfo->num = MAX_NUM_OF_DLS_ENTRY;
+				wrq->u.data.length = sizeof(RT_802_11_DLS_INFO);
+				Status = copy_to_user(wrq->u.data.pointer, pDlsInfo, wrq->u.data.length);
+				DBGPRINT(RT_DEBUG_TRACE, ("Query::RT_OID_802_11_QUERY_DLS_PARAM\n"));
+
+				if (pDlsInfo)
+					kfree(pDlsInfo);
+			}
+			break;
+#endif // QOS_DLS_SUPPORT //
+        default:
+            DBGPRINT(RT_DEBUG_TRACE, ("Query::unknown IOCTL's subcmd = 0x%08x\n", cmd));
+            Status = -EOPNOTSUPP;
+            break;
+    }
+    return Status;
+}
+
+INT rt28xx_sta_ioctl(
+	IN	struct net_device	*net_dev,
+	IN	OUT	struct ifreq	*rq,
+	IN	INT					cmd)
+{
+	POS_COOKIE			pObj;
+	VIRTUAL_ADAPTER		*pVirtualAd = NULL;
+	RTMP_ADAPTER        *pAd = NULL;
+	struct iwreq        *wrq = (struct iwreq *) rq;
+	BOOLEAN				StateMachineTouched = FALSE;
+	INT					Status = NDIS_STATUS_SUCCESS;
+	USHORT				subcmd;
+
+	if (net_dev->priv_flags == INT_MAIN)
+	{
+		pAd = net_dev->priv;
+	}
+	else
+	{
+		pVirtualAd = net_dev->priv;
+		pAd = pVirtualAd->RtmpDev->priv;
+	}
+	pObj = (POS_COOKIE) pAd->OS_Cookie;
+
+	if (pAd == NULL)
+	{
+		/* if 1st open fail, pAd will be free;
+		   So the net_dev->priv will be NULL in 2rd open */
+		return -ENETDOWN;
+	}
+
+    //check if the interface is down
+    if(!RTMP_TEST_FLAG(pAd, fRTMP_ADAPTER_INTERRUPT_IN_USE))
+    {
+#ifdef CONFIG_APSTA_MIXED_SUPPORT
+	    if (wrq->u.data.pointer == NULL)
+	    {
+		    return Status;
+	    }
+
+	    if (strstr(wrq->u.data.pointer, "OpMode") == NULL)
+#endif // CONFIG_APSTA_MIXED_SUPPORT //
+        {
+            DBGPRINT(RT_DEBUG_TRACE, ("INFO::Network is down!\n"));
+		    return -ENETDOWN;
+        }
+    }
+
+	{	// determine this ioctl command is comming from which interface.
+		pObj->ioctl_if_type = INT_MAIN;
+		pObj->ioctl_if = MAIN_MBSSID;
+	}
+
+	switch(cmd)
+	{
+#ifdef RALINK_ATE
+#ifdef RALINK_28xx_QA
+		case RTPRIV_IOCTL_ATE:
+			{
+				RtmpDoAte(pAd, wrq);
+			}
+			break;
+#endif // RALINK_28xx_QA //
+#endif // RALINK_ATE //
+        case SIOCGIFHWADDR:
+			DBGPRINT(RT_DEBUG_TRACE, ("IOCTL::SIOCGIFHWADDR\n"));
+			memcpy(wrq->u.name, pAd->CurrentAddress, ETH_ALEN);
+			break;
+		case SIOCGIWNAME:
+        {
+        	char *name=&wrq->u.name[0];
+        	rt_ioctl_giwname(net_dev, NULL, name, NULL);
+			break;
+		}
+		case SIOCGIWESSID:  //Get ESSID
+        {
+        	struct iw_point *essid=&wrq->u.essid;
+        	rt_ioctl_giwessid(net_dev, NULL, essid, essid->pointer);
+			break;
+		}
+		case SIOCSIWESSID:  //Set ESSID
+        {
+        	struct iw_point	*essid=&wrq->u.essid;
+        	rt_ioctl_siwessid(net_dev, NULL, essid, essid->pointer);
+			break;
+		}
+		case SIOCSIWNWID:   // set network id (the cell)
+		case SIOCGIWNWID:   // get network id
+			Status = -EOPNOTSUPP;
+			break;
+		case SIOCSIWFREQ:   //set channel/frequency (Hz)
+        {
+        	struct iw_freq *freq=&wrq->u.freq;
+        	rt_ioctl_siwfreq(net_dev, NULL, freq, NULL);
+			break;
+		}
+		case SIOCGIWFREQ:   // get channel/frequency (Hz)
+        {
+        	struct iw_freq *freq=&wrq->u.freq;
+        	rt_ioctl_giwfreq(net_dev, NULL, freq, NULL);
+			break;
+		}
+		case SIOCSIWNICKN: //set node name/nickname
+        {
+        	struct iw_point *data=&wrq->u.data;
+        	rt_ioctl_siwnickn(net_dev, NULL, data, NULL);
+			break;
+		}
+		case SIOCGIWNICKN: //get node name/nickname
+        {
+        	struct iw_point *data=&wrq->u.data;
+        	rt_ioctl_giwnickn(net_dev, NULL, data, NULL);
+			break;
+		}
+		case SIOCGIWRATE:   //get default bit rate (bps)
+		    rt_ioctl_giwrate(net_dev, NULL, &wrq->u, NULL);
+            break;
+	    case SIOCSIWRATE:  //set default bit rate (bps)
+	        rt_ioctl_siwrate(net_dev, NULL, &wrq->u, NULL);
+            break;
+        case SIOCGIWRTS:  // get RTS/CTS threshold (bytes)
+        {
+        	struct iw_param *rts=&wrq->u.rts;
+        	rt_ioctl_giwrts(net_dev, NULL, rts, NULL);
+			break;
+		}
+        case SIOCSIWRTS:  //set RTS/CTS threshold (bytes)
+        {
+        	struct iw_param *rts=&wrq->u.rts;
+        	rt_ioctl_siwrts(net_dev, NULL, rts, NULL);
+			break;
+		}
+        case SIOCGIWFRAG:  //get fragmentation thr (bytes)
+        {
+        	struct iw_param *frag=&wrq->u.frag;
+        	rt_ioctl_giwfrag(net_dev, NULL, frag, NULL);
+			break;
+		}
+        case SIOCSIWFRAG:  //set fragmentation thr (bytes)
+        {
+        	struct iw_param *frag=&wrq->u.frag;
+        	rt_ioctl_siwfrag(net_dev, NULL, frag, NULL);
+			break;
+		}
+        case SIOCGIWENCODE:  //get encoding token & mode
+        {
+        	struct iw_point *erq=&wrq->u.encoding;
+        	if(erq->pointer)
+        		rt_ioctl_giwencode(net_dev, NULL, erq, erq->pointer);
+			break;
+		}
+        case SIOCSIWENCODE:  //set encoding token & mode
+        {
+        	struct iw_point *erq=&wrq->u.encoding;
+        	if(erq->pointer)
+        		rt_ioctl_siwencode(net_dev, NULL, erq, erq->pointer);
+			break;
+		}
+		case SIOCGIWAP:     //get access point MAC addresses
+        {
+        	struct sockaddr *ap_addr=&wrq->u.ap_addr;
+        	rt_ioctl_giwap(net_dev, NULL, ap_addr, ap_addr->sa_data);
+			break;
+		}
+	    case SIOCSIWAP:  //set access point MAC addresses
+        {
+        	struct sockaddr *ap_addr=&wrq->u.ap_addr;
+        	rt_ioctl_siwap(net_dev, NULL, ap_addr, ap_addr->sa_data);
+			break;
+		}
+		case SIOCGIWMODE:   //get operation mode
+        {
+        	__u32 *mode=&wrq->u.mode;
+        	rt_ioctl_giwmode(net_dev, NULL, mode, NULL);
+			break;
+		}
+		case SIOCSIWMODE:   //set operation mode
+        {
+        	__u32 *mode=&wrq->u.mode;
+        	rt_ioctl_siwmode(net_dev, NULL, mode, NULL);
+			break;
+		}
+		case SIOCGIWSENS:   //get sensitivity (dBm)
+		case SIOCSIWSENS:	//set sensitivity (dBm)
+		case SIOCGIWPOWER:  //get Power Management settings
+		case SIOCSIWPOWER:  //set Power Management settings
+		case SIOCGIWTXPOW:  //get transmit power (dBm)
+		case SIOCSIWTXPOW:  //set transmit power (dBm)
+		case SIOCGIWRANGE:	//Get range of parameters
+		case SIOCGIWRETRY:	//get retry limits and lifetime
+		case SIOCSIWRETRY:	//set retry limits and lifetime
+			Status = -EOPNOTSUPP;
+			break;
+		case RT_PRIV_IOCTL:
+			subcmd = wrq->u.data.flags;
+			if( subcmd & OID_GET_SET_TOGGLE)
+				Status = RTMPSetInformation(pAd, rq, subcmd);
+			else
+				Status = RTMPQueryInformation(pAd, rq, subcmd);
+			break;
+		case SIOCGIWPRIV:
+			if (wrq->u.data.pointer)
+			{
+				if ( access_ok(VERIFY_WRITE, wrq->u.data.pointer, sizeof(privtab)) != TRUE)
+					break;
+				wrq->u.data.length = sizeof(privtab) / sizeof(privtab[0]);
+				if (copy_to_user(wrq->u.data.pointer, privtab, sizeof(privtab)))
+					Status = -EFAULT;
+			}
+			break;
+		case RTPRIV_IOCTL_SET:
+			if(access_ok(VERIFY_READ, wrq->u.data.pointer, wrq->u.data.length) != TRUE)
+				break;
+			rt_ioctl_setparam(net_dev, NULL, NULL, wrq->u.data.pointer);
+			break;
+		case RTPRIV_IOCTL_GSITESURVEY:
+			RTMPIoctlGetSiteSurvey(pAd, wrq);
+		    break;
+#ifdef DBG
+		case RTPRIV_IOCTL_MAC:
+			RTMPIoctlMAC(pAd, wrq);
+			break;
+		case RTPRIV_IOCTL_E2P:
+			RTMPIoctlE2PROM(pAd, wrq);
+			break;
+#endif // DBG //
+        case SIOCETHTOOL:
+                break;
+		default:
+			DBGPRINT(RT_DEBUG_ERROR, ("IOCTL::unknown IOCTL's cmd = 0x%08x\n", cmd));
+			Status = -EOPNOTSUPP;
+			break;
+	}
+
+    if(StateMachineTouched) // Upper layer sent a MLME-related operations
+    	RT28XX_MLME_HANDLER(pAd);
+
+	return Status;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set SSID
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_SSID_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    NDIS_802_11_SSID                    Ssid, *pSsid=NULL;
+    BOOLEAN                             StateMachineTouched = FALSE;
+    int                                 success = TRUE;
+
+    if( strlen(arg) <= MAX_LEN_OF_SSID)
+    {
+        NdisZeroMemory(&Ssid, sizeof(NDIS_802_11_SSID));
+        if (strlen(arg) != 0)
+        {
+            NdisMoveMemory(Ssid.Ssid, arg, strlen(arg));
+            Ssid.SsidLength = strlen(arg);
+        }
+        else   //ANY ssid
+        {
+            Ssid.SsidLength = 0;
+		    memcpy(Ssid.Ssid, "", 0);
+			pAdapter->StaCfg.BssType = BSS_INFRA;
+			pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+	        pAdapter->StaCfg.WepStatus  = Ndis802_11EncryptionDisabled;
+		}
+        pSsid = &Ssid;
+
+        if (pAdapter->Mlme.CntlMachine.CurrState != CNTL_IDLE)
+        {
+            RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+            DBGPRINT(RT_DEBUG_TRACE, ("!!! MLME busy, reset MLME state machine !!!\n"));
+        }
+
+        pAdapter->MlmeAux.CurrReqIsFromNdis = TRUE;
+        pAdapter->StaCfg.bScanReqIsFromWebUI = FALSE;
+		pAdapter->bConfigChanged = TRUE;
+
+        MlmeEnqueue(pAdapter,
+                    MLME_CNTL_STATE_MACHINE,
+                    OID_802_11_SSID,
+                    sizeof(NDIS_802_11_SSID),
+                    (VOID *)pSsid);
+
+        StateMachineTouched = TRUE;
+        DBGPRINT(RT_DEBUG_TRACE, ("Set_SSID_Proc::(Len=%d,Ssid=%s)\n", Ssid.SsidLength, Ssid.Ssid));
+    }
+    else
+        success = FALSE;
+
+    if (StateMachineTouched) // Upper layer sent a MLME-related operations
+    	RT28XX_MLME_HANDLER(pAdapter);
+
+    return success;
+}
+
+#ifdef WMM_SUPPORT
+/*
+    ==========================================================================
+    Description:
+        Set WmmCapable Enable or Disable
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT	Set_WmmCapable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+	BOOLEAN	bWmmCapable;
+
+	bWmmCapable = simple_strtol(arg, 0, 10);
+
+	if ((bWmmCapable == 1)
+#ifdef RT2870
+		&& (pAd->NumberOfPipes >= 5)
+#endif // RT2870 //
+		)
+		pAd->CommonCfg.bWmmCapable = TRUE;
+	else if (bWmmCapable == 0)
+		pAd->CommonCfg.bWmmCapable = FALSE;
+	else
+		return FALSE;  //Invalid argument
+
+	DBGPRINT(RT_DEBUG_TRACE, ("Set_WmmCapable_Proc::(bWmmCapable=%d)\n",
+		pAd->CommonCfg.bWmmCapable));
+
+	return TRUE;
+}
+#endif // WMM_SUPPORT //
+
+/*
+    ==========================================================================
+    Description:
+        Set Network Type(Infrastructure/Adhoc mode)
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_NetworkType_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    UINT32	Value = 0;
+
+    if (strcmp(arg, "Adhoc") == 0)
+	{
+		if (pAdapter->StaCfg.BssType != BSS_ADHOC)
+		{
+			// Config has changed
+			pAdapter->bConfigChanged = TRUE;
+            if (MONITOR_ON(pAdapter))
+            {
+                RTMP_IO_WRITE32(pAdapter, RX_FILTR_CFG, STANORMAL);
+                RTMP_IO_READ32(pAdapter, MAC_SYS_CTRL, &Value);
+				Value &= (~0x80);
+				RTMP_IO_WRITE32(pAdapter, MAC_SYS_CTRL, Value);
+                OPSTATUS_CLEAR_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED);
+                pAdapter->StaCfg.bAutoReconnect = TRUE;
+                LinkDown(pAdapter, FALSE);
+            }
+			if (INFRA_ON(pAdapter))
+			{
+				//BOOLEAN Cancelled;
+				// Set the AutoReconnectSsid to prevent it reconnect to old SSID
+				// Since calling this indicate user don't want to connect to that SSID anymore.
+				pAdapter->MlmeAux.AutoReconnectSsidLen= 32;
+				NdisZeroMemory(pAdapter->MlmeAux.AutoReconnectSsid, pAdapter->MlmeAux.AutoReconnectSsidLen);
+
+				LinkDown(pAdapter, FALSE);
+
+				DBGPRINT(RT_DEBUG_TRACE, ("NDIS_STATUS_MEDIA_DISCONNECT Event BB!\n"));
+			}
+		}
+		pAdapter->StaCfg.BssType = BSS_ADHOC;
+        pAdapter->net_dev->type = pAdapter->StaCfg.OriDevType;
+		DBGPRINT(RT_DEBUG_TRACE, ("===>Set_NetworkType_Proc::(AD-HOC)\n"));
+	}
+    else if (strcmp(arg, "Infra") == 0)
+	{
+		if (pAdapter->StaCfg.BssType != BSS_INFRA)
+		{
+			// Config has changed
+			pAdapter->bConfigChanged = TRUE;
+            if (MONITOR_ON(pAdapter))
+            {
+                RTMP_IO_WRITE32(pAdapter, RX_FILTR_CFG, STANORMAL);
+                RTMP_IO_READ32(pAdapter, MAC_SYS_CTRL, &Value);
+				Value &= (~0x80);
+				RTMP_IO_WRITE32(pAdapter, MAC_SYS_CTRL, Value);
+                OPSTATUS_CLEAR_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED);
+                pAdapter->StaCfg.bAutoReconnect = TRUE;
+                LinkDown(pAdapter, FALSE);
+            }
+			if (ADHOC_ON(pAdapter))
+			{
+				// Set the AutoReconnectSsid to prevent it reconnect to old SSID
+				// Since calling this indicate user don't want to connect to that SSID anymore.
+				pAdapter->MlmeAux.AutoReconnectSsidLen= 32;
+				NdisZeroMemory(pAdapter->MlmeAux.AutoReconnectSsid, pAdapter->MlmeAux.AutoReconnectSsidLen);
+
+				LinkDown(pAdapter, FALSE);
+			}
+		}
+		pAdapter->StaCfg.BssType = BSS_INFRA;
+        pAdapter->net_dev->type = pAdapter->StaCfg.OriDevType;
+		DBGPRINT(RT_DEBUG_TRACE, ("===>Set_NetworkType_Proc::(INFRA)\n"));
+
+        pAdapter->StaCfg.BssType = BSS_INFRA;
+	}
+    else if (strcmp(arg, "Monitor") == 0)
+    {
+		UCHAR	bbpValue = 0;
+		BCN_TIME_CFG_STRUC csr;
+		OPSTATUS_CLEAR_FLAG(pAdapter, fOP_STATUS_INFRA_ON);
+        OPSTATUS_CLEAR_FLAG(pAdapter, fOP_STATUS_ADHOC_ON);
+		OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_MEDIA_STATE_CONNECTED);
+		// disable all periodic state machine
+		pAdapter->StaCfg.bAutoReconnect = FALSE;
+		// reset all mlme state machine
+		RT28XX_MLME_RESET_STATE_MACHINE(pAdapter);
+		DBGPRINT(RT_DEBUG_TRACE, ("fOP_STATUS_MEDIA_STATE_CONNECTED \n"));
+        if (pAdapter->CommonCfg.CentralChannel == 0)
+        {
+#ifdef DOT11_N_SUPPORT
+            if (pAdapter->CommonCfg.PhyMode == PHY_11AN_MIXED)
+                pAdapter->CommonCfg.CentralChannel = 36;
+            else
+#endif // DOT11_N_SUPPORT //
+                pAdapter->CommonCfg.CentralChannel = 6;
+        }
+#ifdef DOT11_N_SUPPORT
+        else
+            N_ChannelCheck(pAdapter);
+#endif // DOT11_N_SUPPORT //
+
+#ifdef DOT11_N_SUPPORT
+	if (pAdapter->CommonCfg.PhyMode >= PHY_11ABGN_MIXED &&
+            pAdapter->CommonCfg.RegTransmitSetting.field.BW == BW_40 &&
+            pAdapter->CommonCfg.RegTransmitSetting.field.EXTCHA == EXTCHA_ABOVE)
+		{
+			// 40MHz ,control channel at lower
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R4, &bbpValue);
+			bbpValue &= (~0x18);
+			bbpValue |= 0x10;
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R4, bbpValue);
+			pAdapter->CommonCfg.BBPCurrentBW = BW_40;
+			//  RX : control channel at lower
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R3, &bbpValue);
+			bbpValue &= (~0x20);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R3, bbpValue);
+
+			RTMP_IO_READ32(pAdapter, TX_BAND_CFG, &Value);
+			Value &= 0xfffffffe;
+			RTMP_IO_WRITE32(pAdapter, TX_BAND_CFG, Value);
+			pAdapter->CommonCfg.CentralChannel = pAdapter->CommonCfg.Channel + 2;
+            AsicSwitchChannel(pAdapter, pAdapter->CommonCfg.CentralChannel, FALSE);
+		    AsicLockChannel(pAdapter, pAdapter->CommonCfg.CentralChannel);
+            DBGPRINT(RT_DEBUG_TRACE, ("BW_40 ,control_channel(%d), CentralChannel(%d) \n",
+                                       pAdapter->CommonCfg.Channel,
+                                       pAdapter->CommonCfg.CentralChannel));
+		}
+		else if (pAdapter->CommonCfg.PhyMode >= PHY_11ABGN_MIXED &&
+                 pAdapter->CommonCfg.RegTransmitSetting.field.BW == BW_40 &&
+                 pAdapter->CommonCfg.RegTransmitSetting.field.EXTCHA == EXTCHA_BELOW)
+		{
+			// 40MHz ,control channel at upper
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R4, &bbpValue);
+			bbpValue &= (~0x18);
+			bbpValue |= 0x10;
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R4, bbpValue);
+			pAdapter->CommonCfg.BBPCurrentBW = BW_40;
+			RTMP_IO_READ32(pAdapter, TX_BAND_CFG, &Value);
+			Value |= 0x1;
+			RTMP_IO_WRITE32(pAdapter, TX_BAND_CFG, Value);
+
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R3, &bbpValue);
+			bbpValue |= (0x20);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R3, bbpValue);
+			pAdapter->CommonCfg.CentralChannel = pAdapter->CommonCfg.Channel - 2;
+            AsicSwitchChannel(pAdapter, pAdapter->CommonCfg.CentralChannel, FALSE);
+		    AsicLockChannel(pAdapter, pAdapter->CommonCfg.CentralChannel);
+            DBGPRINT(RT_DEBUG_TRACE, ("BW_40 ,control_channel(%d), CentralChannel(%d) \n",
+                                       pAdapter->CommonCfg.Channel,
+                                       pAdapter->CommonCfg.CentralChannel));
+		}
+		else
+#endif // DOT11_N_SUPPORT //
+		{
+			// 20MHz
+			RTMP_BBP_IO_READ8_BY_REG_ID(pAdapter, BBP_R4, &bbpValue);
+			bbpValue &= (~0x18);
+			RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R4, bbpValue);
+			pAdapter->CommonCfg.BBPCurrentBW = BW_20;
+			AsicSwitchChannel(pAdapter, pAdapter->CommonCfg.Channel, FALSE);
+			AsicLockChannel(pAdapter, pAdapter->CommonCfg.Channel);
+			DBGPRINT(RT_DEBUG_TRACE, ("BW_20, Channel(%d)\n", pAdapter->CommonCfg.Channel));
+		}
+		// Enable Rx with promiscuous reception
+		RTMP_IO_WRITE32(pAdapter, RX_FILTR_CFG, 0x3);
+		// ASIC supporsts sniffer function with replacing RSSI with timestamp.
+		//RTMP_IO_READ32(pAdapter, MAC_SYS_CTRL, &Value);
+		//Value |= (0x80);
+		//RTMP_IO_WRITE32(pAdapter, MAC_SYS_CTRL, Value);
+		// disable sync
+		RTMP_IO_READ32(pAdapter, BCN_TIME_CFG, &csr.word);
+		csr.field.bBeaconGen = 0;
+		csr.field.bTBTTEnable = 0;
+		csr.field.TsfSyncMode = 0;
+		RTMP_IO_WRITE32(pAdapter, BCN_TIME_CFG, csr.word);
+
+		pAdapter->StaCfg.BssType = BSS_MONITOR;
+        pAdapter->net_dev->type = ARPHRD_IEEE80211_PRISM; //ARPHRD_IEEE80211; // IEEE80211
+		DBGPRINT(RT_DEBUG_TRACE, ("===>Set_NetworkType_Proc::(MONITOR)\n"));
+    }
+
+    // Reset Ralink supplicant to not use, it will be set to start when UI set PMK key
+    pAdapter->StaCfg.WpaState = SS_NOTUSE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_NetworkType_Proc::(NetworkType=%d)\n", pAdapter->StaCfg.BssType));
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Authentication mode
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_AuthMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    if ((strcmp(arg, "WEPAUTO") == 0) || (strcmp(arg, "wepauto") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeAutoSwitch;
+    else if ((strcmp(arg, "OPEN") == 0) || (strcmp(arg, "open") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeOpen;
+    else if ((strcmp(arg, "SHARED") == 0) || (strcmp(arg, "shared") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeShared;
+    else if ((strcmp(arg, "WPAPSK") == 0) || (strcmp(arg, "wpapsk") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPAPSK;
+    else if ((strcmp(arg, "WPANONE") == 0) || (strcmp(arg, "wpanone") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPANone;
+    else if ((strcmp(arg, "WPA2PSK") == 0) || (strcmp(arg, "wpa2psk") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA2PSK;
+#ifdef WPA_SUPPLICANT_SUPPORT
+    else if ((strcmp(arg, "WPA") == 0) || (strcmp(arg, "wpa") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA;
+    else if ((strcmp(arg, "WPA2") == 0) || (strcmp(arg, "wpa2") == 0))
+        pAdapter->StaCfg.AuthMode = Ndis802_11AuthModeWPA2;
+#endif // WPA_SUPPLICANT_SUPPORT //
+    else
+        return FALSE;
+
+    pAdapter->StaCfg.PortSecured = WPA_802_1X_PORT_NOT_SECURED;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_AuthMode_Proc::(AuthMode=%d)\n", pAdapter->StaCfg.AuthMode));
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Encryption Type
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_EncrypType_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    if ((strcmp(arg, "NONE") == 0) || (strcmp(arg, "none") == 0))
+    {
+        if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+            return TRUE;    // do nothing
+
+        pAdapter->StaCfg.WepStatus     = Ndis802_11WEPDisabled;
+        pAdapter->StaCfg.PairCipher    = Ndis802_11WEPDisabled;
+	    pAdapter->StaCfg.GroupCipher   = Ndis802_11WEPDisabled;
+    }
+    else if ((strcmp(arg, "WEP") == 0) || (strcmp(arg, "wep") == 0))
+    {
+        if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+            return TRUE;    // do nothing
+
+        pAdapter->StaCfg.WepStatus     = Ndis802_11WEPEnabled;
+        pAdapter->StaCfg.PairCipher    = Ndis802_11WEPEnabled;
+	    pAdapter->StaCfg.GroupCipher   = Ndis802_11WEPEnabled;
+    }
+    else if ((strcmp(arg, "TKIP") == 0) || (strcmp(arg, "tkip") == 0))
+    {
+        if (pAdapter->StaCfg.AuthMode < Ndis802_11AuthModeWPA)
+            return TRUE;    // do nothing
+
+        pAdapter->StaCfg.WepStatus     = Ndis802_11Encryption2Enabled;
+        pAdapter->StaCfg.PairCipher    = Ndis802_11Encryption2Enabled;
+	    pAdapter->StaCfg.GroupCipher   = Ndis802_11Encryption2Enabled;
+    }
+    else if ((strcmp(arg, "AES") == 0) || (strcmp(arg, "aes") == 0))
+    {
+        if (pAdapter->StaCfg.AuthMode < Ndis802_11AuthModeWPA)
+            return TRUE;    // do nothing
+
+        pAdapter->StaCfg.WepStatus     = Ndis802_11Encryption3Enabled;
+        pAdapter->StaCfg.PairCipher    = Ndis802_11Encryption3Enabled;
+	    pAdapter->StaCfg.GroupCipher   = Ndis802_11Encryption3Enabled;
+    }
+    else
+        return FALSE;
+
+    pAdapter->StaCfg.OrigWepStatus = pAdapter->StaCfg.WepStatus;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_EncrypType_Proc::(EncrypType=%d)\n", pAdapter->StaCfg.WepStatus));
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Default Key ID
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_DefaultKeyID_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    ULONG                               KeyIdx;
+
+    KeyIdx = simple_strtol(arg, 0, 10);
+    if((KeyIdx >= 1 ) && (KeyIdx <= 4))
+        pAdapter->StaCfg.DefaultKeyId = (UCHAR) (KeyIdx - 1 );
+    else
+        return FALSE;  //Invalid argument
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_DefaultKeyID_Proc::(DefaultKeyID=%d)\n", pAdapter->StaCfg.DefaultKeyId));
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set WEP KEY1
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Key1_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    int                                 KeyLen;
+    int                                 i;
+    UCHAR                               CipherAlg=CIPHER_WEP64;
+
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        return TRUE;    // do nothing
+
+    KeyLen = strlen(arg);
+
+    switch (KeyLen)
+    {
+        case 5: //wep 40 Ascii type
+            pAdapter->SharedKey[BSS0][0].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][0].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::(Key1=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 10: //wep 40 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][0].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][0].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::(Key1=%s and type=%s)\n", arg, "Hex"));
+            break;
+        case 13: //wep 104 Ascii type
+            pAdapter->SharedKey[BSS0][0].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][0].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::(Key1=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 26: //wep 104 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][0].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][0].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::(Key1=%s and type=%s)\n", arg, "Hex"));
+            break;
+        default: //Invalid argument
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key1_Proc::Invalid argument (=%s)\n", arg));
+            return FALSE;
+    }
+
+    pAdapter->SharedKey[BSS0][0].CipherAlg = CipherAlg;
+
+    // Set keys (into ASIC)
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        ;   // not support
+    else    // Old WEP stuff
+    {
+        AsicAddSharedKeyEntry(pAdapter,
+                              0,
+                              0,
+                              pAdapter->SharedKey[BSS0][0].CipherAlg,
+                              pAdapter->SharedKey[BSS0][0].Key,
+                              NULL,
+                              NULL);
+    }
+
+    return TRUE;
+}
+/*
+    ==========================================================================
+
+    Description:
+        Set WEP KEY2
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Key2_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    int                                 KeyLen;
+    int                                 i;
+    UCHAR                               CipherAlg=CIPHER_WEP64;
+
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        return TRUE;    // do nothing
+
+    KeyLen = strlen(arg);
+
+    switch (KeyLen)
+    {
+        case 5: //wep 40 Ascii type
+            pAdapter->SharedKey[BSS0][1].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][1].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::(Key2=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 10: //wep 40 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][1].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][1].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::(Key2=%s and type=%s)\n", arg, "Hex"));
+            break;
+        case 13: //wep 104 Ascii type
+            pAdapter->SharedKey[BSS0][1].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][1].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::(Key2=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 26: //wep 104 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][1].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][1].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::(Key2=%s and type=%s)\n", arg, "Hex"));
+            break;
+        default: //Invalid argument
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key2_Proc::Invalid argument (=%s)\n", arg));
+            return FALSE;
+    }
+    pAdapter->SharedKey[BSS0][1].CipherAlg = CipherAlg;
+
+    // Set keys (into ASIC)
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        ;   // not support
+    else    // Old WEP stuff
+    {
+        AsicAddSharedKeyEntry(pAdapter,
+                              0,
+                              1,
+                              pAdapter->SharedKey[BSS0][1].CipherAlg,
+                              pAdapter->SharedKey[BSS0][1].Key,
+                              NULL,
+                              NULL);
+    }
+
+    return TRUE;
+}
+/*
+    ==========================================================================
+    Description:
+        Set WEP KEY3
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Key3_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    int                                 KeyLen;
+    int                                 i;
+    UCHAR                               CipherAlg=CIPHER_WEP64;
+
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        return TRUE;    // do nothing
+
+    KeyLen = strlen(arg);
+
+    switch (KeyLen)
+    {
+        case 5: //wep 40 Ascii type
+            pAdapter->SharedKey[BSS0][2].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][2].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::(Key3=%s and type=Ascii)\n", arg));
+            break;
+        case 10: //wep 40 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][2].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][2].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::(Key3=%s and type=Hex)\n", arg));
+            break;
+        case 13: //wep 104 Ascii type
+            pAdapter->SharedKey[BSS0][2].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][2].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::(Key3=%s and type=Ascii)\n", arg));
+            break;
+        case 26: //wep 104 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][2].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][2].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::(Key3=%s and type=Hex)\n", arg));
+            break;
+        default: //Invalid argument
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key3_Proc::Invalid argument (=%s)\n", arg));
+            return FALSE;
+    }
+    pAdapter->SharedKey[BSS0][2].CipherAlg = CipherAlg;
+
+    // Set keys (into ASIC)
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        ;   // not support
+    else    // Old WEP stuff
+    {
+        AsicAddSharedKeyEntry(pAdapter,
+                              0,
+                              2,
+                              pAdapter->SharedKey[BSS0][2].CipherAlg,
+                              pAdapter->SharedKey[BSS0][2].Key,
+                              NULL,
+                              NULL);
+    }
+
+    return TRUE;
+}
+/*
+    ==========================================================================
+    Description:
+        Set WEP KEY4
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Key4_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    int                                 KeyLen;
+    int                                 i;
+    UCHAR                               CipherAlg=CIPHER_WEP64;
+
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        return TRUE;    // do nothing
+
+    KeyLen = strlen(arg);
+
+    switch (KeyLen)
+    {
+        case 5: //wep 40 Ascii type
+            pAdapter->SharedKey[BSS0][3].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][3].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::(Key4=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 10: //wep 40 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][3].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][3].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP64;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::(Key4=%s and type=%s)\n", arg, "Hex"));
+            break;
+        case 13: //wep 104 Ascii type
+            pAdapter->SharedKey[BSS0][3].KeyLen = KeyLen;
+            memcpy(pAdapter->SharedKey[BSS0][3].Key, arg, KeyLen);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::(Key4=%s and type=%s)\n", arg, "Ascii"));
+            break;
+        case 26: //wep 104 Hex type
+            for(i=0; i < KeyLen; i++)
+            {
+                if( !isxdigit(*(arg+i)) )
+                    return FALSE;  //Not Hex value;
+            }
+            pAdapter->SharedKey[BSS0][3].KeyLen = KeyLen / 2 ;
+            AtoH(arg, pAdapter->SharedKey[BSS0][3].Key, KeyLen / 2);
+            CipherAlg = CIPHER_WEP128;
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::(Key4=%s and type=%s)\n", arg, "Hex"));
+            break;
+        default: //Invalid argument
+            DBGPRINT(RT_DEBUG_TRACE, ("Set_Key4_Proc::Invalid argument (=%s)\n", arg));
+            return FALSE;
+    }
+    pAdapter->SharedKey[BSS0][3].CipherAlg = CipherAlg;
+
+    // Set keys (into ASIC)
+    if (pAdapter->StaCfg.AuthMode >= Ndis802_11AuthModeWPA)
+        ;   // not support
+    else    // Old WEP stuff
+    {
+        AsicAddSharedKeyEntry(pAdapter,
+                              0,
+                              3,
+                              pAdapter->SharedKey[BSS0][3].CipherAlg,
+                              pAdapter->SharedKey[BSS0][3].Key,
+                              NULL,
+                              NULL);
+    }
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set WPA PSK key
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_WPAPSK_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    UCHAR                   keyMaterial[40];
+
+    if ((pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPAPSK) &&
+        (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPA2PSK) &&
+	    (pAdapter->StaCfg.AuthMode != Ndis802_11AuthModeWPANone)
+		)
+        return TRUE;    // do nothing
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_WPAPSK_Proc::(WPAPSK=%s)\n", arg));
+
+    NdisZeroMemory(keyMaterial, 40);
+
+    if ((strlen(arg) < 8) || (strlen(arg) > 64))
+    {
+        DBGPRINT(RT_DEBUG_TRACE, ("Set failed!!(WPAPSK=%s), WPAPSK key-string required 8 ~ 64 characters \n", arg));
+        return FALSE;
+    }
+
+    if (strlen(arg) == 64)
+    {
+        AtoH(arg, keyMaterial, 32);
+        NdisMoveMemory(pAdapter->StaCfg.PMK, keyMaterial, 32);
+
+    }
+    else
+    {
+        PasswordHash((char *)arg, pAdapter->MlmeAux.Ssid, pAdapter->MlmeAux.SsidLen, keyMaterial);
+        NdisMoveMemory(pAdapter->StaCfg.PMK, keyMaterial, 32);
+    }
+
+
+
+    if(pAdapter->StaCfg.BssType == BSS_ADHOC &&
+       pAdapter->StaCfg.AuthMode == Ndis802_11AuthModeWPANone)
+    {
+         pAdapter->StaCfg.WpaState = SS_NOTUSE;
+    }
+    else
+    {
+        // Start STA supplicant state machine
+        pAdapter->StaCfg.WpaState = SS_START;
+    }
+
+    return TRUE;
+}
+
+/*
+    ==========================================================================
+    Description:
+        Set Power Saving mode
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_PSMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    if (pAdapter->StaCfg.BssType == BSS_INFRA)
+    {
+        if ((strcmp(arg, "Max_PSP") == 0) ||
+			(strcmp(arg, "max_psp") == 0) ||
+			(strcmp(arg, "MAX_PSP") == 0))
+        {
+            // do NOT turn on PSM bit here, wait until MlmeCheckForPsmChange()
+            // to exclude certain situations.
+            if (pAdapter->StaCfg.bWindowsACCAMEnable == FALSE)
+                pAdapter->StaCfg.WindowsPowerMode = Ndis802_11PowerModeMAX_PSP;
+            pAdapter->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeMAX_PSP;
+            OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_RECEIVE_DTIM);
+            pAdapter->StaCfg.DefaultListenCount = 5;
+
+        }
+        else if ((strcmp(arg, "Fast_PSP") == 0) ||
+				 (strcmp(arg, "fast_psp") == 0) ||
+                 (strcmp(arg, "FAST_PSP") == 0))
+        {
+            // do NOT turn on PSM bit here, wait until MlmeCheckForPsmChange()
+            // to exclude certain situations.
+            OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_RECEIVE_DTIM);
+            if (pAdapter->StaCfg.bWindowsACCAMEnable == FALSE)
+                pAdapter->StaCfg.WindowsPowerMode = Ndis802_11PowerModeFast_PSP;
+            pAdapter->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeFast_PSP;
+            pAdapter->StaCfg.DefaultListenCount = 3;
+        }
+        else if ((strcmp(arg, "Legacy_PSP") == 0) ||
+                 (strcmp(arg, "legacy_psp") == 0) ||
+                 (strcmp(arg, "LEGACY_PSP") == 0))
+        {
+            // do NOT turn on PSM bit here, wait until MlmeCheckForPsmChange()
+            // to exclude certain situations.
+            OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_RECEIVE_DTIM);
+            if (pAdapter->StaCfg.bWindowsACCAMEnable == FALSE)
+                pAdapter->StaCfg.WindowsPowerMode = Ndis802_11PowerModeLegacy_PSP;
+            pAdapter->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeLegacy_PSP;
+            pAdapter->StaCfg.DefaultListenCount = 3;
+        }
+        else
+        {
+            //Default Ndis802_11PowerModeCAM
+            // clear PSM bit immediately
+            MlmeSetPsmBit(pAdapter, PWR_ACTIVE);
+            OPSTATUS_SET_FLAG(pAdapter, fOP_STATUS_RECEIVE_DTIM);
+            if (pAdapter->StaCfg.bWindowsACCAMEnable == FALSE)
+                pAdapter->StaCfg.WindowsPowerMode = Ndis802_11PowerModeCAM;
+            pAdapter->StaCfg.WindowsBatteryPowerMode = Ndis802_11PowerModeCAM;
+        }
+
+        DBGPRINT(RT_DEBUG_TRACE, ("Set_PSMode_Proc::(PSMode=%ld)\n", pAdapter->StaCfg.WindowsPowerMode));
+    }
+    else
+        return FALSE;
+
+
+    return TRUE;
+}
+
+#ifdef WPA_SUPPLICANT_SUPPORT
+/*
+    ==========================================================================
+    Description:
+        Set WpaSupport flag.
+    Value:
+        0: Driver ignore wpa_supplicant.
+        1: wpa_supplicant initiates scanning and AP selection.
+        2: driver takes care of scanning, AP selection, and IEEE 802.11 association parameters.
+    Return:
+        TRUE if all parameters are OK, FALSE otherwise
+    ==========================================================================
+*/
+INT Set_Wpa_Support(
+    IN	PRTMP_ADAPTER	pAd,
+	IN	PUCHAR			arg)
+{
+
+    if ( simple_strtol(arg, 0, 10) == 0)
+        pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_DISABLE;
+    else if ( simple_strtol(arg, 0, 10) == 1)
+        pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_ENABLE;
+    else if ( simple_strtol(arg, 0, 10) == 2)
+        pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_ENABLE_WITH_WEB_UI;
+    else
+        pAd->StaCfg.WpaSupplicantUP = WPA_SUPPLICANT_DISABLE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_Wpa_Support::(WpaSupplicantUP=%d)\n", pAd->StaCfg.WpaSupplicantUP));
+
+    return TRUE;
+}
+#endif // WPA_SUPPLICANT_SUPPORT //
+
+#ifdef DBG
+/*
+    ==========================================================================
+    Description:
+        Read / Write MAC
+    Arguments:
+        pAdapter                    Pointer to our adapter
+        wrq                         Pointer to the ioctl argument
+
+    Return Value:
+        None
+
+    Note:
+        Usage:
+               1.) iwpriv ra0 mac 0        ==> read MAC where Addr=0x0
+               2.) iwpriv ra0 mac 0=12     ==> write MAC where Addr=0x0, value=12
+    ==========================================================================
+*/
+VOID RTMPIoctlMAC(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq)
+{
+	CHAR				*this_char;
+	CHAR				*value;
+	INT					j = 0, k = 0;
+	CHAR				msg[1024];
+	CHAR				arg[255];
+	ULONG				macAddr = 0;
+	UCHAR				temp[16], temp2[16];
+	UINT32				macValue = 0;
+	INT					Status;
+
+
+	memset(msg, 0x00, 1024);
+	if (wrq->u.data.length > 1) //No parameters.
+	{
+	    Status = copy_from_user(arg, wrq->u.data.pointer, (wrq->u.data.length > 255) ? 255 : wrq->u.data.length);
+		sprintf(msg, "\n");
+
+		//Parsing Read or Write
+	    this_char = arg;
+		if (!*this_char)
+			goto next;
+
+		if ((value = rtstrchr(this_char, '=')) != NULL)
+			*value++ = 0;
+
+		if (!value || !*value)
+		{ //Read
+			// Sanity check
+			if(strlen(this_char) > 4)
+				goto next;
+
+			j = strlen(this_char);
+			while(j-- > 0)
+			{
+				if(this_char[j] > 'f' || this_char[j] < '0')
+					return;
+			}
+
+			// Mac Addr
+			k = j = strlen(this_char);
+			while(j-- > 0)
+			{
+				this_char[4-k+j] = this_char[j];
+			}
+
+			while(k < 4)
+				this_char[3-k++]='0';
+			this_char[4]='\0';
+
+			if(strlen(this_char) == 4)
+			{
+				AtoH(this_char, temp, 2);
+				macAddr = *temp*256 + temp[1];
+				if (macAddr < 0xFFFF)
+				{
+					RTMP_IO_READ32(pAdapter, macAddr, &macValue);
+					DBGPRINT(RT_DEBUG_TRACE, ("MacAddr=%lx, MacValue=%x\n", macAddr, macValue));
+					sprintf(msg+strlen(msg), "[0x%08lX]:%08X  ", macAddr , macValue);
+				}
+				else
+				{//Invalid parametes, so default printk all bbp
+					goto next;
+				}
+			}
+		}
+		else
+		{ //Write
+			memcpy(&temp2, value, strlen(value));
+			temp2[strlen(value)] = '\0';
+
+			// Sanity check
+			if((strlen(this_char) > 4) || strlen(temp2) > 8)
+				goto next;
+
+			j = strlen(this_char);
+			while(j-- > 0)
+			{
+				if(this_char[j] > 'f' || this_char[j] < '0')
+					return;
+			}
+
+			j = strlen(temp2);
+			while(j-- > 0)
+			{
+				if(temp2[j] > 'f' || temp2[j] < '0')
+					return;
+			}
+
+			//MAC Addr
+			k = j = strlen(this_char);
+			while(j-- > 0)
+			{
+				this_char[4-k+j] = this_char[j];
+			}
+
+			while(k < 4)
+				this_char[3-k++]='0';
+			this_char[4]='\0';
+
+			//MAC value
+			k = j = strlen(temp2);
+			while(j-- > 0)
+			{
+				temp2[8-k+j] = temp2[j];
+			}
+
+			while(k < 8)
+				temp2[7-k++]='0';
+			temp2[8]='\0';
+
+			{
+				AtoH(this_char, temp, 2);
+				macAddr = *temp*256 + temp[1];
+
+				AtoH(temp2, temp, 4);
+				macValue = *temp*256*256*256 + temp[1]*256*256 + temp[2]*256 + temp[3];
+
+				// debug mode
+				if (macAddr == (HW_DEBUG_SETTING_BASE + 4))
+				{
+					// 0x2bf4: byte0 non-zero: enable R17 tuning, 0: disable R17 tuning
+                    if (macValue & 0x000000ff)
+                    {
+                        pAdapter->BbpTuning.bEnable = TRUE;
+                        DBGPRINT(RT_DEBUG_TRACE,("turn on R17 tuning\n"));
+                    }
+                    else
+                    {
+                        UCHAR R66;
+                        pAdapter->BbpTuning.bEnable = FALSE;
+                        R66 = 0x26 + GET_LNA_GAIN(pAdapter);
+#ifdef RALINK_ATE
+						if (ATE_ON(pAdapter))
+						{
+							ATE_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R66, (0x26 + GET_LNA_GAIN(pAdapter)));
+						}
+						else
+#endif // RALINK_ATE //
+						RTMP_BBP_IO_WRITE8_BY_REG_ID(pAdapter, BBP_R66, (0x26 + GET_LNA_GAIN(pAdapter)));
+                        DBGPRINT(RT_DEBUG_TRACE,("turn off R17 tuning, restore to 0x%02x\n", R66));
+                    }
+					return;
+				}
+
+				DBGPRINT(RT_DEBUG_TRACE, ("MacAddr=%02lx, MacValue=0x%x\n", macAddr, macValue));
+
+				RTMP_IO_WRITE32(pAdapter, macAddr, macValue);
+				sprintf(msg+strlen(msg), "[0x%08lX]:%08X  ", macAddr, macValue);
+			}
+		}
+	}
+next:
+	if(strlen(msg) == 1)
+		sprintf(msg+strlen(msg), "===>Error command format!");
+
+	// Copy the information into the user buffer
+	wrq->u.data.length = strlen(msg);
+	Status = copy_to_user(wrq->u.data.pointer, msg, wrq->u.data.length);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<==RTMPIoctlMAC\n\n"));
+}
+
+/*
+    ==========================================================================
+    Description:
+        Read / Write E2PROM
+    Arguments:
+        pAdapter                    Pointer to our adapter
+        wrq                         Pointer to the ioctl argument
+
+    Return Value:
+        None
+
+    Note:
+        Usage:
+               1.) iwpriv ra0 e2p 0     	==> read E2PROM where Addr=0x0
+               2.) iwpriv ra0 e2p 0=1234    ==> write E2PROM where Addr=0x0, value=1234
+    ==========================================================================
+*/
+VOID RTMPIoctlE2PROM(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	struct iwreq	*wrq)
+{
+	CHAR				*this_char;
+	CHAR				*value;
+	INT					j = 0, k = 0;
+	CHAR				msg[1024];
+	CHAR				arg[255];
+	USHORT				eepAddr = 0;
+	UCHAR				temp[16], temp2[16];
+	USHORT				eepValue;
+	int					Status;
+
+
+	memset(msg, 0x00, 1024);
+	if (wrq->u.data.length > 1) //No parameters.
+	{
+	    Status = copy_from_user(arg, wrq->u.data.pointer, (wrq->u.data.length > 255) ? 255 : wrq->u.data.length);
+		sprintf(msg, "\n");
+
+	    //Parsing Read or Write
+		this_char = arg;
+
+
+		if (!*this_char)
+			goto next;
+
+		if ((value = rtstrchr(this_char, '=')) != NULL)
+			*value++ = 0;
+
+		if (!value || !*value)
+		{ //Read
+
+			// Sanity check
+			if(strlen(this_char) > 4)
+				goto next;
+
+			j = strlen(this_char);
+			while(j-- > 0)
+			{
+				if(this_char[j] > 'f' || this_char[j] < '0')
+					return;
+			}
+
+			// E2PROM addr
+			k = j = strlen(this_char);
+			while(j-- > 0)
+			{
+				this_char[4-k+j] = this_char[j];
+			}
+
+			while(k < 4)
+				this_char[3-k++]='0';
+			this_char[4]='\0';
+
+			if(strlen(this_char) == 4)
+			{
+				AtoH(this_char, temp, 2);
+				eepAddr = *temp*256 + temp[1];
+				if (eepAddr < 0xFFFF)
+				{
+					RT28xx_EEPROM_READ16(pAdapter, eepAddr, eepValue);
+					sprintf(msg+strlen(msg), "[0x%04X]:0x%04X  ", eepAddr , eepValue);
+				}
+				else
+				{//Invalid parametes, so default printk all bbp
+					goto next;
+				}
+			}
+		}
+		else
+		{ //Write
+			memcpy(&temp2, value, strlen(value));
+			temp2[strlen(value)] = '\0';
+
+			// Sanity check
+			if((strlen(this_char) > 4) || strlen(temp2) > 8)
+				goto next;
+
+			j = strlen(this_char);
+			while(j-- > 0)
+			{
+				if(this_char[j] > 'f' || this_char[j] < '0')
+					return;
+			}
+			j = strlen(temp2);
+			while(j-- > 0)
+			{
+				if(temp2[j] > 'f' || temp2[j] < '0')
+					return;
+			}
+
+			//MAC Addr
+			k = j = strlen(this_char);
+			while(j-- > 0)
+			{
+				this_char[4-k+j] = this_char[j];
+			}
+
+			while(k < 4)
+				this_char[3-k++]='0';
+			this_char[4]='\0';
+
+			//MAC value
+			k = j = strlen(temp2);
+			while(j-- > 0)
+			{
+				temp2[4-k+j] = temp2[j];
+			}
+
+			while(k < 4)
+				temp2[3-k++]='0';
+			temp2[4]='\0';
+
+			AtoH(this_char, temp, 2);
+			eepAddr = *temp*256 + temp[1];
+
+			AtoH(temp2, temp, 2);
+			eepValue = *temp*256 + temp[1];
+
+			RT28xx_EEPROM_WRITE16(pAdapter, eepAddr, eepValue);
+			sprintf(msg+strlen(msg), "[0x%02X]:%02X  ", eepAddr, eepValue);
+		}
+	}
+next:
+	if(strlen(msg) == 1)
+		sprintf(msg+strlen(msg), "===>Error command format!");
+
+
+	// Copy the information into the user buffer
+	wrq->u.data.length = strlen(msg);
+	Status = copy_to_user(wrq->u.data.pointer, msg, wrq->u.data.length);
+
+	DBGPRINT(RT_DEBUG_TRACE, ("<==RTMPIoctlE2PROM\n"));
+}
+#endif // DBG //
+
+
+
+
+INT Set_TGnWifiTest_Proc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          arg)
+{
+    if (simple_strtol(arg, 0, 10) == 0)
+        pAd->StaCfg.bTGnWifiTest = FALSE;
+    else
+        pAd->StaCfg.bTGnWifiTest = TRUE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("IF Set_TGnWifiTest_Proc::(bTGnWifiTest=%d)\n", pAd->StaCfg.bTGnWifiTest));
+	return TRUE;
+}
+
+INT Set_LongRetryLimit_Proc(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PUCHAR			arg)
+{
+	TX_RTY_CFG_STRUC	tx_rty_cfg;
+	UCHAR				LongRetryLimit = (UCHAR)simple_strtol(arg, 0, 10);
+
+	RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+	tx_rty_cfg.field.LongRtyLimit = LongRetryLimit;
+	RTMP_IO_WRITE32(pAdapter, TX_RTY_CFG, tx_rty_cfg.word);
+	DBGPRINT(RT_DEBUG_TRACE, ("IF Set_LongRetryLimit_Proc::(tx_rty_cfg=0x%x)\n", tx_rty_cfg.word));
+	return TRUE;
+}
+
+INT Set_ShortRetryLimit_Proc(
+	IN	PRTMP_ADAPTER	pAdapter,
+	IN	PUCHAR			arg)
+{
+	TX_RTY_CFG_STRUC	tx_rty_cfg;
+	UCHAR				ShortRetryLimit = (UCHAR)simple_strtol(arg, 0, 10);
+
+	RTMP_IO_READ32(pAdapter, TX_RTY_CFG, &tx_rty_cfg.word);
+	tx_rty_cfg.field.ShortRtyLimit = ShortRetryLimit;
+	RTMP_IO_WRITE32(pAdapter, TX_RTY_CFG, tx_rty_cfg.word);
+	DBGPRINT(RT_DEBUG_TRACE, ("IF Set_ShortRetryLimit_Proc::(tx_rty_cfg=0x%x)\n", tx_rty_cfg.word));
+	return TRUE;
+}
+
+#ifdef EXT_BUILD_CHANNEL_LIST
+INT Set_Ieee80211dClientMode_Proc(
+    IN  PRTMP_ADAPTER   pAdapter,
+    IN  PUCHAR          arg)
+{
+    if (simple_strtol(arg, 0, 10) == 0)
+        pAdapter->StaCfg.IEEE80211dClientMode = Rt802_11_D_None;
+    else if (simple_strtol(arg, 0, 10) == 1)
+        pAdapter->StaCfg.IEEE80211dClientMode = Rt802_11_D_Flexible;
+    else if (simple_strtol(arg, 0, 10) == 2)
+        pAdapter->StaCfg.IEEE80211dClientMode = Rt802_11_D_Strict;
+    else
+        return FALSE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("Set_Ieee802dMode_Proc::(IEEEE0211dMode=%d)\n", pAdapter->StaCfg.IEEE80211dClientMode));
+    return TRUE;
+}
+#endif // EXT_BUILD_CHANNEL_LIST //
+
+#ifdef CARRIER_DETECTION_SUPPORT
+INT Set_CarrierDetect_Proc(
+    IN  PRTMP_ADAPTER   pAd,
+    IN  PUCHAR          arg)
+{
+    if (simple_strtol(arg, 0, 10) == 0)
+        pAd->CommonCfg.CarrierDetect.Enable = FALSE;
+    else
+        pAd->CommonCfg.CarrierDetect.Enable = TRUE;
+
+    DBGPRINT(RT_DEBUG_TRACE, ("IF Set_CarrierDetect_Proc::(CarrierDetect.Enable=%d)\n", pAd->CommonCfg.CarrierDetect.Enable));
+	return TRUE;
+}
+#endif // CARRIER_DETECTION_SUPPORT //
+
+
+INT	Show_Adhoc_MacTable_Proc(
+	IN	PRTMP_ADAPTER	pAd,
+	IN	PCHAR			extra)
+{
+	INT i;
+
+	sprintf(extra, "\n");
+
+#ifdef DOT11_N_SUPPORT
+	sprintf(extra, "%sHT Operating Mode : %d\n", extra, pAd->CommonCfg.AddHTInfo.AddHtInfo2.OperaionMode);
+#endif // DOT11_N_SUPPORT //
+
+	sprintf(extra, "%s\n%-19s%-4s%-4s%-7s%-7s%-7s%-10s%-6s%-6s%-6s%-6s\n", extra,
+			"MAC", "AID", "BSS", "RSSI0", "RSSI1", "RSSI2", "PhMd", "BW", "MCS", "SGI", "STBC");
+
+	for (i=1; i<MAX_LEN_OF_MAC_TABLE; i++)
+	{
+		PMAC_TABLE_ENTRY pEntry = &pAd->MacTab.Content[i];
+
+		if (strlen(extra) > (IW_PRIV_SIZE_MASK - 30))
+		    break;
+		if ((pEntry->ValidAsCLI || pEntry->ValidAsApCli) && (pEntry->Sst == SST_ASSOC))
+		{
+			sprintf(extra, "%s%02X:%02X:%02X:%02X:%02X:%02X  ", extra,
+				pEntry->Addr[0], pEntry->Addr[1], pEntry->Addr[2],
+				pEntry->Addr[3], pEntry->Addr[4], pEntry->Addr[5]);
+			sprintf(extra, "%s%-4d", extra, (int)pEntry->Aid);
+			sprintf(extra, "%s%-4d", extra, (int)pEntry->apidx);
+			sprintf(extra, "%s%-7d", extra, pEntry->RssiSample.AvgRssi0);
+			sprintf(extra, "%s%-7d", extra, pEntry->RssiSample.AvgRssi1);
+			sprintf(extra, "%s%-7d", extra, pEntry->RssiSample.AvgRssi2);
+			sprintf(extra, "%s%-10s", extra, GetPhyMode(pEntry->HTPhyMode.field.MODE));
+			sprintf(extra, "%s%-6s", extra, GetBW(pEntry->HTPhyMode.field.BW));
+			sprintf(extra, "%s%-6d", extra, pEntry->HTPhyMode.field.MCS);
+			sprintf(extra, "%s%-6d", extra, pEntry->HTPhyMode.field.ShortGI);
+			sprintf(extra, "%s%-6d", extra, pEntry->HTPhyMode.field.STBC);
+			sprintf(extra, "%s%-10d, %d, %d%%\n", extra, pEntry->DebugFIFOCount, pEntry->DebugTxCount,
+						(pEntry->DebugTxCount) ? ((pEntry->DebugTxCount-pEntry->DebugFIFOCount)*100/pEntry->DebugTxCount) : 0);
+			sprintf(extra, "%s\n", extra);
+		}
+	}
+
+	return TRUE;
+}
+
+
diff --git a/drivers/staging/rt2870/wpa.h b/drivers/staging/rt2870/wpa.h
new file mode 100644
index 00000000000..0134ae6097c
--- /dev/null
+++ b/drivers/staging/rt2870/wpa.h
@@ -0,0 +1,357 @@
+/*
+ *************************************************************************
+ * Ralink Tech Inc.
+ * 5F., No.36, Taiyuan St., Jhubei City,
+ * Hsinchu County 302,
+ * Taiwan, R.O.C.
+ *
+ * (c) Copyright 2002-2007, Ralink Technology, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation; either version 2 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * This program is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ * GNU General Public License for more details.                          *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program; if not, write to the                         *
+ * Free Software Foundation, Inc.,                                       *
+ * 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *                                                                       *
+ *************************************************************************
+
+	Module Name:
+	wpa.h
+
+	Abstract:
+
+	Revision History:
+	Who			When			What
+	--------	----------		----------------------------------------------
+	Name		Date			Modification logs
+*/
+
+#ifndef	__WPA_H__
+#define	__WPA_H__
+
+// EAPOL Key descripter frame format related length
+#define LEN_KEY_DESC_NONCE			32
+#define LEN_KEY_DESC_IV				16
+#define LEN_KEY_DESC_RSC			8
+#define LEN_KEY_DESC_ID				8
+#define LEN_KEY_DESC_REPLAY			8
+#define LEN_KEY_DESC_MIC			16
+
+// The length is the EAPoL-Key frame except key data field.
+// Please refer to 802.11i-2004 ,Figure 43u in p.78
+#define LEN_EAPOL_KEY_MSG			(sizeof(KEY_DESCRIPTER) - MAX_LEN_OF_RSNIE)
+
+// EAP Code Type.
+#define EAP_CODE_REQUEST	1
+#define EAP_CODE_RESPONSE	2
+#define EAP_CODE_SUCCESS    3
+#define EAP_CODE_FAILURE    4
+
+// EAPOL frame Protocol Version
+#define	EAPOL_VER					1
+#define	EAPOL_VER2					2
+
+// EAPOL-KEY Descriptor Type
+#define	WPA1_KEY_DESC				0xfe
+#define WPA2_KEY_DESC               0x02
+
+// Key Descriptor Version of Key Information
+#define	DESC_TYPE_TKIP				1
+#define	DESC_TYPE_AES				2
+#define DESC_TYPE_MESH				3
+
+#define LEN_MSG1_2WAY               0x7f
+#define MAX_LEN_OF_EAP_HS           256
+
+#define LEN_MASTER_KEY				32
+
+// EAPOL EK, MK
+#define LEN_EAP_EK					16
+#define LEN_EAP_MICK				16
+#define LEN_EAP_KEY					((LEN_EAP_EK)+(LEN_EAP_MICK))
+// TKIP key related
+#define LEN_PMKID					16
+#define LEN_TKIP_EK					16
+#define LEN_TKIP_RXMICK				8
+#define LEN_TKIP_TXMICK				8
+#define LEN_AES_EK					16
+#define LEN_AES_KEY					LEN_AES_EK
+#define LEN_TKIP_KEY				((LEN_TKIP_EK)+(LEN_TKIP_RXMICK)+(LEN_TKIP_TXMICK))
+#define TKIP_AP_TXMICK_OFFSET		((LEN_EAP_KEY)+(LEN_TKIP_EK))
+#define TKIP_AP_RXMICK_OFFSET		(TKIP_AP_TXMICK_OFFSET+LEN_TKIP_TXMICK)
+#define TKIP_GTK_LENGTH				((LEN_TKIP_EK)+(LEN_TKIP_RXMICK)+(LEN_TKIP_TXMICK))
+#define LEN_PTK						((LEN_EAP_KEY)+(LEN_TKIP_KEY))
+#define MIN_LEN_OF_GTK				5
+
+// RSN IE Length definition
+#define MAX_LEN_OF_RSNIE         	90
+#define MIN_LEN_OF_RSNIE         	8
+
+//EAP Packet Type
+#define	EAPPacket		0
+#define	EAPOLStart		1
+#define	EAPOLLogoff		2
+#define	EAPOLKey		3
+#define	EAPOLASFAlert	4
+#define	EAPTtypeMax		5
+
+#define	EAPOL_MSG_INVALID	0
+#define	EAPOL_PAIR_MSG_1	1
+#define	EAPOL_PAIR_MSG_2	2
+#define	EAPOL_PAIR_MSG_3	3
+#define	EAPOL_PAIR_MSG_4	4
+#define	EAPOL_GROUP_MSG_1	5
+#define	EAPOL_GROUP_MSG_2	6
+
+#define PAIRWISEKEY					1
+#define GROUPKEY					0
+
+// Retry timer counter initial value
+#define PEER_MSG1_RETRY_TIMER_CTR           0
+#define PEER_MSG3_RETRY_TIMER_CTR           10
+#define GROUP_MSG1_RETRY_TIMER_CTR          20
+
+
+#define EAPOL_START_DISABLE					0
+#define EAPOL_START_PSK						1
+#define EAPOL_START_1X						2
+
+#define MIX_CIPHER_WPA_TKIP_ON(x)       (((x) & 0x08) != 0)
+#define MIX_CIPHER_WPA_AES_ON(x)        (((x) & 0x04) != 0)
+#define MIX_CIPHER_WPA2_TKIP_ON(x)      (((x) & 0x02) != 0)
+#define MIX_CIPHER_WPA2_AES_ON(x)       (((x) & 0x01) != 0)
+
+#define ROUND_UP(__x, __y) \
+	(((ULONG)((__x)+((__y)-1))) & ((ULONG)~((__y)-1)))
+
+#define	ADD_ONE_To_64BIT_VAR(_V)		\
+{										\
+	UCHAR	cnt = LEN_KEY_DESC_REPLAY;	\
+	do									\
+	{									\
+		cnt--;							\
+		_V[cnt]++;						\
+		if (cnt == 0)					\
+			break;						\
+	}while (_V[cnt] == 0);				\
+}
+
+#define IS_WPA_CAPABILITY(a)       (((a) >= Ndis802_11AuthModeWPA) && ((a) <= Ndis802_11AuthModeWPA1PSKWPA2PSK))
+
+// EAPOL Key Information definition within Key descriptor format
+typedef	struct PACKED _KEY_INFO
+{
+#ifdef RT_BIG_ENDIAN
+	UCHAR	KeyAck:1;
+    UCHAR	Install:1;
+    UCHAR	KeyIndex:2;
+    UCHAR	KeyType:1;
+    UCHAR	KeyDescVer:3;
+    UCHAR	Rsvd:3;
+    UCHAR	EKD_DL:1;		// EKD for AP; DL for STA
+    UCHAR	Request:1;
+    UCHAR	Error:1;
+    UCHAR	Secure:1;
+    UCHAR	KeyMic:1;
+#else
+	UCHAR	KeyMic:1;
+	UCHAR	Secure:1;
+	UCHAR	Error:1;
+	UCHAR	Request:1;
+	UCHAR	EKD_DL:1;       // EKD for AP; DL for STA
+	UCHAR	Rsvd:3;
+	UCHAR	KeyDescVer:3;
+	UCHAR	KeyType:1;
+	UCHAR	KeyIndex:2;
+	UCHAR	Install:1;
+	UCHAR	KeyAck:1;
+#endif
+}	KEY_INFO, *PKEY_INFO;
+
+// EAPOL Key descriptor format
+typedef	struct PACKED _KEY_DESCRIPTER
+{
+	UCHAR		Type;
+	KEY_INFO	KeyInfo;
+	UCHAR		KeyLength[2];
+	UCHAR		ReplayCounter[LEN_KEY_DESC_REPLAY];
+	UCHAR		KeyNonce[LEN_KEY_DESC_NONCE];
+	UCHAR		KeyIv[LEN_KEY_DESC_IV];
+	UCHAR		KeyRsc[LEN_KEY_DESC_RSC];
+	UCHAR		KeyId[LEN_KEY_DESC_ID];
+	UCHAR		KeyMic[LEN_KEY_DESC_MIC];
+	UCHAR		KeyDataLen[2];
+	UCHAR		KeyData[MAX_LEN_OF_RSNIE];
+}	KEY_DESCRIPTER, *PKEY_DESCRIPTER;
+
+typedef	struct PACKED _EAPOL_PACKET
+{
+	UCHAR	 			ProVer;
+	UCHAR	 			ProType;
+	UCHAR	 			Body_Len[2];
+	KEY_DESCRIPTER		KeyDesc;
+}	EAPOL_PACKET, *PEAPOL_PACKET;
+
+//802.11i D10 page 83
+typedef struct PACKED _GTK_ENCAP
+{
+#ifndef RT_BIG_ENDIAN
+    UCHAR               Kid:2;
+    UCHAR               tx:1;
+    UCHAR               rsv:5;
+    UCHAR               rsv1;
+#else
+    UCHAR               rsv:5;
+    UCHAR               tx:1;
+    UCHAR               Kid:2;
+    UCHAR               rsv1;
+#endif
+    UCHAR               GTK[TKIP_GTK_LENGTH];
+}   GTK_ENCAP, *PGTK_ENCAP;
+
+typedef struct PACKED _KDE_ENCAP
+{
+    UCHAR               Type;
+    UCHAR               Len;
+    UCHAR               OUI[3];
+    UCHAR               DataType;
+    GTK_ENCAP      GTKEncap;
+}   KDE_ENCAP, *PKDE_ENCAP;
+
+// For WPA1
+typedef struct PACKED _RSNIE {
+    UCHAR   oui[4];
+    USHORT  version;
+    UCHAR   mcast[4];
+    USHORT  ucount;
+    struct PACKED {
+        UCHAR oui[4];
+    }ucast[1];
+} RSNIE, *PRSNIE;
+
+// For WPA2
+typedef struct PACKED _RSNIE2 {
+    USHORT  version;
+    UCHAR   mcast[4];
+    USHORT  ucount;
+    struct PACKED {
+        UCHAR oui[4];
+    }ucast[1];
+} RSNIE2, *PRSNIE2;
+
+// AKM Suite
+typedef struct PACKED _RSNIE_AUTH {
+    USHORT acount;
+    struct PACKED {
+        UCHAR oui[4];
+    }auth[1];
+} RSNIE_AUTH,*PRSNIE_AUTH;
+
+typedef	union PACKED _RSN_CAPABILITIES	{
+	struct	PACKED {
+#ifdef RT_BIG_ENDIAN
+        USHORT		Rsvd:10;
+        USHORT		GTKSA_R_Counter:2;
+        USHORT		PTKSA_R_Counter:2;
+        USHORT		No_Pairwise:1;
+		USHORT		PreAuth:1;
+#else
+        USHORT		PreAuth:1;
+		USHORT		No_Pairwise:1;
+		USHORT		PTKSA_R_Counter:2;
+		USHORT		GTKSA_R_Counter:2;
+		USHORT		Rsvd:10;
+#endif
+	}	field;
+	USHORT			word;
+}	RSN_CAPABILITIES, *PRSN_CAPABILITIES;
+
+typedef struct PACKED _EAP_HDR {
+    UCHAR   ProVer;
+    UCHAR   ProType;
+    UCHAR   Body_Len[2];
+    UCHAR   code;
+    UCHAR   identifier;
+    UCHAR   length[2]; // including code and identifier, followed by length-2 octets of data
+} EAP_HDR, *PEAP_HDR;
+
+// For supplicant state machine states. 802.11i Draft 4.1, p. 97
+// We simplified it
+typedef	enum	_WpaState
+{
+	SS_NOTUSE,				// 0
+	SS_START,				// 1
+	SS_WAIT_MSG_3,			// 2
+	SS_WAIT_GROUP,			// 3
+	SS_FINISH,  			// 4
+	SS_KEYUPDATE,			// 5
+}	WPA_STATE;
+
+//
+//	The definition of the cipher combination
+//
+// 	 bit3	bit2  bit1   bit0
+//	+------------+------------+
+// 	|	  WPA	 |	   WPA2   |
+//	+------+-----+------+-----+
+//	| TKIP | AES | TKIP | AES |
+//	|	0  |  1  |   1  |  0  | -> 0x06
+//	|	0  |  1  |   1  |  1  | -> 0x07
+//	|	1  |  0  |   0  |  1  | -> 0x09
+//	|	1  |  0  |   1  |  1  | -> 0x0B
+//	|	1  |  1  |   0  |  1  | -> 0x0D
+//	|	1  |  1  |   1  |  0  | -> 0x0E
+//	|	1  |  1  |   1  |  1  |	-> 0x0F
+//	+------+-----+------+-----+
+//
+typedef	enum	_WpaMixPairCipher
+{
+	MIX_CIPHER_NOTUSE 			= 0x00,
+	WPA_NONE_WPA2_TKIPAES		= 0x03,		// WPA2-TKIPAES
+	WPA_AES_WPA2_TKIP 			= 0x06,
+	WPA_AES_WPA2_TKIPAES		= 0x07,
+	WPA_TKIP_WPA2_AES			= 0x09,
+	WPA_TKIP_WPA2_TKIPAES		= 0x0B,
+	WPA_TKIPAES_WPA2_NONE		= 0x0C,		// WPA-TKIPAES
+	WPA_TKIPAES_WPA2_AES		= 0x0D,
+	WPA_TKIPAES_WPA2_TKIP		= 0x0E,
+	WPA_TKIPAES_WPA2_TKIPAES	= 0x0F,
+}	WPA_MIX_PAIR_CIPHER;
+
+typedef struct PACKED _RSN_IE_HEADER_STRUCT	{
+	UCHAR		Eid;
+	UCHAR		Length;
+	USHORT		Version;	// Little endian format
+}	RSN_IE_HEADER_STRUCT, *PRSN_IE_HEADER_STRUCT;
+
+// Cipher suite selector types
+typedef struct PACKED _CIPHER_SUITE_STRUCT	{
+	UCHAR		Oui[3];
+	UCHAR		Type;
+}	CIPHER_SUITE_STRUCT, *PCIPHER_SUITE_STRUCT;
+
+// Authentication and Key Management suite selector
+typedef struct PACKED _AKM_SUITE_STRUCT	{
+	UCHAR		Oui[3];
+	UCHAR		Type;
+}	AKM_SUITE_STRUCT, *PAKM_SUITE_STRUCT;
+
+// RSN capability
+typedef struct	PACKED _RSN_CAPABILITY	{
+	USHORT		Rsv:10;
+	USHORT		GTKSAReplayCnt:2;
+	USHORT		PTKSAReplayCnt:2;
+	USHORT		NoPairwise:1;
+	USHORT		PreAuth:1;
+}	RSN_CAPABILITY, *PRSN_CAPABILITY;
+
+#endif
diff --git a/drivers/staging/rtl8187se/Kconfig b/drivers/staging/rtl8187se/Kconfig
new file mode 100644
index 00000000000..79c225acd1a
--- /dev/null
+++ b/drivers/staging/rtl8187se/Kconfig
@@ -0,0 +1,5 @@
+config RTL8187SE
+	tristate "RealTek RTL8187SE Wireless LAN NIC driver"
+	depends on PCI
+	default N
+	---help---
diff --git a/drivers/staging/rtl8187se/Makefile b/drivers/staging/rtl8187se/Makefile
new file mode 100644
index 00000000000..6bc7e29771c
--- /dev/null
+++ b/drivers/staging/rtl8187se/Makefile
@@ -0,0 +1,55 @@
+
+#EXTRA_CFLAGS += -DCONFIG_IEEE80211_NOWEP=y
+#EXTRA_CFLAGS += -DCONFIG_RTL8180_IOMAP
+#EXTRA_CFLAGS += -std=gnu89
+#EXTRA_CFLAGS += -O2
+#CC            = gcc
+EXTRA_CFLAGS += -DTHOMAS_TURBO
+#CFLAGS += -DCONFIG_RTL8185B
+#CFLAGS += -DCONFIG_RTL818x_S
+
+#added for EeePC testing
+EXTRA_CFLAGS += -DENABLE_IPS
+EXTRA_CFLAGS += -DSW_ANTE
+EXTRA_CFLAGS += -DTX_TRACK
+EXTRA_CFLAGS += -DHIGH_POWER
+EXTRA_CFLAGS += -DSW_DIG
+EXTRA_CFLAGS += -DRATE_ADAPT
+EXTRA_CFLAGS += -DCONFIG_RTL8180_PM
+
+#+YJ,080626
+EXTRA_CFLAGS += -DENABLE_DOT11D
+
+#enable it for legacy power save, disable it for leisure  power save
+EXTRA_CFLAGS += -DENABLE_LPS
+
+
+#EXTRA_CFLAGS += -mhard-float -DCONFIG_FORCE_HARD_FLOAT=y
+
+rtl8187se-objs :=			\
+		r8180_core.o		\
+		r8180_sa2400.o		\
+		r8180_93cx6.o		\
+		r8180_wx.o		\
+		r8180_max2820.o		\
+		r8180_gct.o		\
+		r8180_rtl8225.o		\
+		r8180_rtl8255.o		\
+		r8180_rtl8225z2.o	\
+		r8185b_init.o		\
+		r8180_dm.o		\
+		r8180_pm.o		\
+		ieee80211/dot11d.o			\
+		ieee80211/ieee80211_softmac.o		\
+		ieee80211/ieee80211_rx.o		\
+		ieee80211/ieee80211_tx.o		\
+		ieee80211/ieee80211_wx.o		\
+		ieee80211/ieee80211_module.o		\
+		ieee80211/ieee80211_softmac_wx.o	\
+		ieee80211/ieee80211_crypt.o		\
+		ieee80211/ieee80211_crypt_tkip.o	\
+		ieee80211/ieee80211_crypt_ccmp.o	\
+		ieee80211/ieee80211_crypt_wep.o
+
+obj-$(CONFIG_RTL8187SE)	+= rtl8187se.o
+
diff --git a/drivers/staging/rtl8187se/dot11d.h b/drivers/staging/rtl8187se/dot11d.h
new file mode 100644
index 00000000000..49f53fe52af
--- /dev/null
+++ b/drivers/staging/rtl8187se/dot11d.h
@@ -0,0 +1,101 @@
+#ifndef __INC_DOT11D_H

+#define __INC_DOT11D_H

+

+#include "ieee80211.h"

+

+//#define ENABLE_DOT11D

+

+//#define DOT11D_MAX_CHNL_NUM 83

+

+typedef struct _CHNL_TXPOWER_TRIPLE {

+	u8 FirstChnl;

+	u8  NumChnls;

+	u8  MaxTxPowerInDbm;

+}CHNL_TXPOWER_TRIPLE, *PCHNL_TXPOWER_TRIPLE;

+

+typedef enum _DOT11D_STATE {

+	DOT11D_STATE_NONE = 0,

+	DOT11D_STATE_LEARNED,

+	DOT11D_STATE_DONE,

+}DOT11D_STATE;

+

+typedef struct _RT_DOT11D_INFO {

+	//DECLARE_RT_OBJECT(RT_DOT11D_INFO);

+

+	bool bEnabled; // dot11MultiDomainCapabilityEnabled

+

+	u16 CountryIeLen; // > 0 if CountryIeBuf[] contains valid country information element.

+	u8  CountryIeBuf[MAX_IE_LEN];

+	u8  CountryIeSrcAddr[6]; // Source AP of the country IE.

+	u8  CountryIeWatchdog; 

+

+	u8  channel_map[MAX_CHANNEL_NUMBER+1];  //!!!Value 0: Invalid, 1: Valid (active scan), 2: Valid (passive scan)

+	//u8  ChnlListLen; // #Bytes valid in ChnlList[].

+	//u8  ChnlList[DOT11D_MAX_CHNL_NUM];

+	u8  MaxTxPwrDbmList[MAX_CHANNEL_NUMBER+1];

+

+	DOT11D_STATE State;

+}RT_DOT11D_INFO, *PRT_DOT11D_INFO;

+#define eqMacAddr(a,b)		( ((a)[0]==(b)[0] && (a)[1]==(b)[1] && (a)[2]==(b)[2] && (a)[3]==(b)[3] && (a)[4]==(b)[4] && (a)[5]==(b)[5]) ? 1:0 )

+#define cpMacAddr(des,src)	      ((des)[0]=(src)[0],(des)[1]=(src)[1],(des)[2]=(src)[2],(des)[3]=(src)[3],(des)[4]=(src)[4],(des)[5]=(src)[5])

+#define GET_DOT11D_INFO(__pIeeeDev) ((PRT_DOT11D_INFO)((__pIeeeDev)->pDot11dInfo))

+

+#define IS_DOT11D_ENABLE(__pIeeeDev) GET_DOT11D_INFO(__pIeeeDev)->bEnabled

+#define IS_COUNTRY_IE_VALID(__pIeeeDev) (GET_DOT11D_INFO(__pIeeeDev)->CountryIeLen > 0)

+

+#define IS_EQUAL_CIE_SRC(__pIeeeDev, __pTa) eqMacAddr(GET_DOT11D_INFO(__pIeeeDev)->CountryIeSrcAddr, __pTa) 

+#define UPDATE_CIE_SRC(__pIeeeDev, __pTa) cpMacAddr(GET_DOT11D_INFO(__pIeeeDev)->CountryIeSrcAddr, __pTa)

+

+#define IS_COUNTRY_IE_CHANGED(__pIeeeDev, __Ie) \

+	(((__Ie).Length == 0 || (__Ie).Length != GET_DOT11D_INFO(__pIeeeDev)->CountryIeLen) ? \

+	FALSE : \

+	(!memcmp(GET_DOT11D_INFO(__pIeeeDev)->CountryIeBuf, (__Ie).Octet, (__Ie).Length)))

+

+#define CIE_WATCHDOG_TH 1

+#define GET_CIE_WATCHDOG(__pIeeeDev) GET_DOT11D_INFO(__pIeeeDev)->CountryIeWatchdog

+#define RESET_CIE_WATCHDOG(__pIeeeDev) GET_CIE_WATCHDOG(__pIeeeDev) = 0 

+#define UPDATE_CIE_WATCHDOG(__pIeeeDev) ++GET_CIE_WATCHDOG(__pIeeeDev)

+

+#define IS_DOT11D_STATE_DONE(__pIeeeDev) (GET_DOT11D_INFO(__pIeeeDev)->State == DOT11D_STATE_DONE)

+

+

+void

+Dot11d_Init(

+	struct ieee80211_device *dev

+	);

+

+void

+Dot11d_Reset(

+	struct ieee80211_device *dev

+	);

+

+void

+Dot11d_UpdateCountryIe(

+	struct ieee80211_device *dev,

+	u8 *		pTaddr,

+	u16	CoutryIeLen,

+	u8 * pCoutryIe	 

+	);

+

+u8

+DOT11D_GetMaxTxPwrInDbm(

+	struct ieee80211_device *dev,

+	u8 Channel

+	);

+

+void

+DOT11D_ScanComplete(

+	struct ieee80211_device * dev

+	);

+

+int IsLegalChannel(

+	struct ieee80211_device * dev,

+	u8 channel

+);

+

+int ToLegalChannel(

+	struct ieee80211_device * dev,

+	u8 channel

+);

+

+#endif // #ifndef __INC_DOT11D_H

diff --git a/drivers/staging/rtl8187se/ieee80211.h b/drivers/staging/rtl8187se/ieee80211.h
new file mode 100644
index 00000000000..bf06abeeaeb
--- /dev/null
+++ b/drivers/staging/rtl8187se/ieee80211.h
@@ -0,0 +1,1755 @@
+/*
+ * Merged with mainline ieee80211.h in Aug 2004.  Original ieee802_11
+ * remains copyright by the original authors
+ *
+ * Portions of the merged code are based on Host AP (software wireless
+ * LAN access point) driver for Intersil Prism2/2.5/3.
+ *
+ * Copyright (c) 2001-2002, SSH Communications Security Corp and Jouni Malinen
+ * <jkmaline@cc.hut.fi>
+ * Copyright (c) 2002-2003, Jouni Malinen <jkmaline@cc.hut.fi>
+ *
+ * Adaption to a generic IEEE 802.11 stack by James Ketrenos
+ * <jketreno@linux.intel.com>
+ * Copyright (c) 2004, Intel Corporation
+ *
+ * Modified for Realtek's wi-fi cards by Andrea Merello
+ * <andreamrl@tiscali.it>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation. See README and COPYING for
+ * more details.
+ */
+#ifndef IEEE80211_H
+#define IEEE80211_H
+#include <linux/if_ether.h> /* ETH_ALEN */
+#include <linux/kernel.h>   /* ARRAY_SIZE */
+#include <linux/version.h>
+#include <linux/jiffies.h>
+#include <linux/timer.h>
+#include <linux/sched.h>
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,13))
+#include <linux/wireless.h>
+#endif
+
+/*
+#ifndef bool
+#define bool int
+#endif
+
+#ifndef true
+#define true   1
+#endif
+
+#ifndef false
+#define false  0
+#endif
+*/
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20))
+#ifndef bool
+typedef enum{false = 0, true} bool;
+#endif
+#endif
+//#ifdef JOHN_HWSEC
+#define KEY_TYPE_NA		0x0
+#define KEY_TYPE_WEP40 		0x1
+#define KEY_TYPE_TKIP		0x2
+#define KEY_TYPE_CCMP		0x4
+#define KEY_TYPE_WEP104		0x5
+//#endif
+
+
+#define aSifsTime					10
+
+#define MGMT_QUEUE_NUM 5
+
+
+#define IEEE_CMD_SET_WPA_PARAM			1
+#define	IEEE_CMD_SET_WPA_IE			2
+#define IEEE_CMD_SET_ENCRYPTION			3
+#define IEEE_CMD_MLME				4
+
+#define IEEE_PARAM_WPA_ENABLED			1
+#define IEEE_PARAM_TKIP_COUNTERMEASURES		2
+#define IEEE_PARAM_DROP_UNENCRYPTED		3
+#define IEEE_PARAM_PRIVACY_INVOKED		4
+#define IEEE_PARAM_AUTH_ALGS			5
+#define IEEE_PARAM_IEEE_802_1X			6
+//It should consistent with the driver_XXX.c
+//   David, 2006.9.26
+#define IEEE_PARAM_WPAX_SELECT			7
+//Added for notify the encryption type selection
+//   David, 2006.9.26
+#define IEEE_PROTO_WPA				1
+#define IEEE_PROTO_RSN				2
+//Added for notify the encryption type selection
+//   David, 2006.9.26
+#define IEEE_WPAX_USEGROUP			0
+#define IEEE_WPAX_WEP40				1
+#define IEEE_WPAX_TKIP				2
+#define IEEE_WPAX_WRAP   			3
+#define IEEE_WPAX_CCMP				4
+#define IEEE_WPAX_WEP104			5
+
+#define IEEE_KEY_MGMT_IEEE8021X			1
+#define IEEE_KEY_MGMT_PSK			2
+
+
+
+#define IEEE_MLME_STA_DEAUTH			1
+#define IEEE_MLME_STA_DISASSOC			2
+
+
+#define IEEE_CRYPT_ERR_UNKNOWN_ALG		2
+#define IEEE_CRYPT_ERR_UNKNOWN_ADDR		3
+#define IEEE_CRYPT_ERR_CRYPT_INIT_FAILED		4
+#define IEEE_CRYPT_ERR_KEY_SET_FAILED		5
+#define IEEE_CRYPT_ERR_TX_KEY_SET_FAILED		6
+#define IEEE_CRYPT_ERR_CARD_CONF_FAILED		7
+
+
+#define	IEEE_CRYPT_ALG_NAME_LEN			16
+
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,6,10))
+#define ieee80211_wx_get_scan ieee80211_wx_get_scan_rtl
+#define ieee80211_wx_set_encode ieee80211_wx_set_encode_rtl
+#define ieee80211_wx_get_encode ieee80211_wx_get_encode_rtl
+////////////////////////////////
+// added for kernel conflict under FC5
+#define ieee80211_wx_get_name   ieee80211_wx_get_name_rtl
+#define free_ieee80211          free_ieee80211_rtl
+#define alloc_ieee80211        alloc_ieee80211_rtl
+///////////////////////////////
+#endif
+//error in ubuntu2.6.22,so add these
+#define ieee80211_wake_queue ieee80211_wake_queue_rtl
+#define ieee80211_stop_queue ieee80211_stop_queue_rtl
+
+#define ieee80211_rx ieee80211_rx_rtl
+
+#define ieee80211_register_crypto_ops	ieee80211_register_crypto_ops_rtl
+#define ieee80211_unregister_crypto_ops	ieee80211_unregister_crypto_ops_rtl
+#define ieee80211_get_crypto_ops	ieee80211_get_crypto_ops_rtl
+#define ieee80211_crypt_deinit_entries	ieee80211_crypt_deinit_entries_rtl
+#define ieee80211_crypt_deinit_handler	ieee80211_crypt_deinit_handler_rtl
+#define ieee80211_crypt_delayed_deinit	ieee80211_crypt_delayed_deinit_rtl
+
+#define ieee80211_txb_free	ieee80211_txb_free_rtl
+#define ieee80211_wx_get_essid	ieee80211_wx_get_essid_rtl
+#define ieee80211_wx_set_essid	ieee80211_wx_set_essid_rtl
+#define ieee80211_wx_set_rate	ieee80211_wx_set_rate_rtl
+#define ieee80211_wx_get_rate	ieee80211_wx_get_rate_rtl
+#define ieee80211_wx_set_wap	ieee80211_wx_set_wap_rtl
+#define ieee80211_wx_get_wap	ieee80211_wx_get_wap_rtl
+#define ieee80211_wx_set_mode	ieee80211_wx_set_mode_rtl
+#define ieee80211_wx_get_mode	ieee80211_wx_get_mode_rtl
+#define ieee80211_wx_set_scan	ieee80211_wx_set_scan_rtl
+#define ieee80211_wx_get_freq	ieee80211_wx_get_freq_rtl
+#define ieee80211_wx_set_freq	ieee80211_wx_set_freq_rtl
+#define ieee80211_wx_set_rawtx	ieee80211_wx_set_rawtx_rtl
+#define ieee80211_wx_set_power	ieee80211_wx_set_power_rtl
+#define ieee80211_wx_get_power	ieee80211_wx_get_power_rtl
+#define ieee80211_wlan_frequencies	ieee80211_wlan_frequencies_rtl
+#define ieee80211_softmac_stop_protocol	ieee80211_softmac_stop_protocol_rtl
+#define ieee80211_softmac_start_protocol ieee80211_softmac_start_protocol_rtl
+#define	ieee80211_start_protocol	ieee80211_start_protocol_rtl
+#define	ieee80211_stop_protocol		ieee80211_stop_protocol_rtl
+#define	ieee80211_rx_mgt		ieee80211_rx_mgt_rtl
+
+#define ieee80211_wx_set_auth ieee80211_wx_set_auth_rtl
+//by amy for ps
+#define notify_wx_assoc_event  notify_wx_assoc_event_rtl
+#define ieee80211_stop_send_beacons ieee80211_stop_send_beacons_rtl
+#define ieee80211_disassociate ieee80211_disassociate_rtl
+#define ieee80211_start_scan ieee80211_start_scan_rtl
+//by amy for ps
+typedef struct ieee_param {
+	u32 cmd;
+	u8 sta_addr[ETH_ALEN];
+        union {
+		struct {
+			u8 name;
+			u32 value;
+		} wpa_param;
+		struct {
+			u32 len;
+			u8 reserved[32];
+			u8 data[0];
+		} wpa_ie;
+	        struct{
+			int command;
+    			int reason_code;
+		} mlme;
+		struct {
+			u8 alg[IEEE_CRYPT_ALG_NAME_LEN];
+			u8 set_tx;
+			u32 err;
+			u8 idx;
+			u8 seq[8]; /* sequence counter (set: RX, get: TX) */
+			u16 key_len;
+			u8 key[0];
+		} crypt;
+
+	} u;
+}ieee_param;
+
+
+#if WIRELESS_EXT < 17
+#define IW_QUAL_QUAL_INVALID   0x10
+#define IW_QUAL_LEVEL_INVALID  0x20
+#define IW_QUAL_NOISE_INVALID  0x40
+#define IW_QUAL_QUAL_UPDATED   0x1
+#define IW_QUAL_LEVEL_UPDATED  0x2
+#define IW_QUAL_NOISE_UPDATED  0x4
+#endif
+
+// linux under 2.6.9 release may not support it, so modify it for common use
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,9))
+#define MSECS(t)	(1000 * ((t) / HZ) + 1000 * ((t) % HZ) / HZ)
+static inline unsigned long msleep_interruptible_rtl(unsigned int msecs)
+{
+         unsigned long timeout = MSECS(msecs) + 1;
+
+         while (timeout) {
+                 set_current_state(TASK_UNINTERRUPTIBLE);
+                 timeout = schedule_timeout(timeout);
+         }
+         return timeout;
+}
+#else
+#define MSECS(t) msecs_to_jiffies(t)
+#define msleep_interruptible_rtl  msleep_interruptible
+#endif
+
+#define IEEE80211_DATA_LEN		2304
+/* Maximum size for the MA-UNITDATA primitive, 802.11 standard section
+   6.2.1.1.2.
+
+   The figure in section 7.1.2 suggests a body size of up to 2312
+   bytes is allowed, which is a bit confusing, I suspect this
+   represents the 2304 bytes of real data, plus a possible 8 bytes of
+   WEP IV and ICV. (this interpretation suggested by Ramiro Barreiro) */
+
+
+#define IEEE80211_HLEN			30
+#define IEEE80211_FRAME_LEN		(IEEE80211_DATA_LEN + IEEE80211_HLEN)
+
+/* this is stolen and modified from the madwifi driver*/
+#define IEEE80211_FC0_TYPE_MASK		0x0c
+#define IEEE80211_FC0_TYPE_DATA		0x08
+#define IEEE80211_FC0_SUBTYPE_MASK	0xB0
+#define IEEE80211_FC0_SUBTYPE_QOS	0x80
+
+#define IEEE80211_QOS_HAS_SEQ(fc) \
+	(((fc) & (IEEE80211_FC0_TYPE_MASK | IEEE80211_FC0_SUBTYPE_MASK)) == \
+	 (IEEE80211_FC0_TYPE_DATA | IEEE80211_FC0_SUBTYPE_QOS))
+
+/* this is stolen from ipw2200 driver */
+#define IEEE_IBSS_MAC_HASH_SIZE 31
+struct ieee_ibss_seq {
+	u8 mac[ETH_ALEN];
+	u16 seq_num[17];
+	u16 frag_num[17];
+	unsigned long packet_time[17];
+	struct list_head list;
+};
+
+struct ieee80211_hdr {
+	u16 frame_ctl;
+	u16 duration_id;
+	u8 addr1[ETH_ALEN];
+	u8 addr2[ETH_ALEN];
+	u8 addr3[ETH_ALEN];
+	u16 seq_ctl;
+	u8 addr4[ETH_ALEN];
+} __attribute__ ((packed));
+
+struct ieee80211_hdr_QOS {
+	u16 frame_ctl;
+	u16 duration_id;
+	u8 addr1[ETH_ALEN];
+	u8 addr2[ETH_ALEN];
+	u8 addr3[ETH_ALEN];
+	u16 seq_ctl;
+	u8 addr4[ETH_ALEN];
+	u16 QOS_ctl;
+} __attribute__ ((packed));
+
+struct ieee80211_hdr_3addr {
+	u16 frame_ctl;
+	u16 duration_id;
+	u8 addr1[ETH_ALEN];
+	u8 addr2[ETH_ALEN];
+	u8 addr3[ETH_ALEN];
+	u16 seq_ctl;
+} __attribute__ ((packed));
+
+struct ieee80211_hdr_3addr_QOS {
+	u16 frame_ctl;
+	u16 duration_id;
+	u8 addr1[ETH_ALEN];
+	u8 addr2[ETH_ALEN];
+	u8 addr3[ETH_ALEN];
+	u16 seq_ctl;
+	u16 QOS_ctl;
+} __attribute__ ((packed));
+
+enum eap_type {
+	EAP_PACKET = 0,
+	EAPOL_START,
+	EAPOL_LOGOFF,
+	EAPOL_KEY,
+	EAPOL_ENCAP_ASF_ALERT
+};
+
+static const char *eap_types[] = {
+	[EAP_PACKET]		= "EAP-Packet",
+	[EAPOL_START]		= "EAPOL-Start",
+	[EAPOL_LOGOFF]		= "EAPOL-Logoff",
+	[EAPOL_KEY]		= "EAPOL-Key",
+	[EAPOL_ENCAP_ASF_ALERT]	= "EAPOL-Encap-ASF-Alert"
+};
+
+static inline const char *eap_get_type(int type)
+{
+	return (type >= ARRAY_SIZE(eap_types)) ? "Unknown" : eap_types[type];
+}
+
+struct eapol {
+	u8 snap[6];
+	u16 ethertype;
+	u8 version;
+	u8 type;
+	u16 length;
+} __attribute__ ((packed));
+
+#define IEEE80211_3ADDR_LEN 24
+#define IEEE80211_4ADDR_LEN 30
+#define IEEE80211_FCS_LEN    4
+
+#define MIN_FRAG_THRESHOLD     256U
+#define	MAX_FRAG_THRESHOLD     2346U
+
+/* Frame control field constants */
+#define IEEE80211_FCTL_VERS		0x0002
+#define IEEE80211_FCTL_FTYPE		0x000c
+#define IEEE80211_FCTL_STYPE		0x00f0
+#define IEEE80211_FCTL_TODS		0x0100
+#define IEEE80211_FCTL_FROMDS		0x0200
+#define IEEE80211_FCTL_DSTODS		0x0300 //added by david
+#define IEEE80211_FCTL_MOREFRAGS	0x0400
+#define IEEE80211_FCTL_RETRY		0x0800
+#define IEEE80211_FCTL_PM		0x1000
+#define IEEE80211_FCTL_MOREDATA	0x2000
+#define IEEE80211_FCTL_WEP		0x4000
+#define IEEE80211_FCTL_ORDER		0x8000
+
+#define IEEE80211_FTYPE_MGMT		0x0000
+#define IEEE80211_FTYPE_CTL		0x0004
+#define IEEE80211_FTYPE_DATA		0x0008
+
+/* management */
+#define IEEE80211_STYPE_ASSOC_REQ	0x0000
+#define IEEE80211_STYPE_ASSOC_RESP 	0x0010
+#define IEEE80211_STYPE_REASSOC_REQ	0x0020
+#define IEEE80211_STYPE_REASSOC_RESP	0x0030
+#define IEEE80211_STYPE_PROBE_REQ	0x0040
+#define IEEE80211_STYPE_PROBE_RESP	0x0050
+#define IEEE80211_STYPE_BEACON		0x0080
+#define IEEE80211_STYPE_ATIM		0x0090
+#define IEEE80211_STYPE_DISASSOC	0x00A0
+#define IEEE80211_STYPE_AUTH		0x00B0
+#define IEEE80211_STYPE_DEAUTH		0x00C0
+#define IEEE80211_STYPE_MANAGE_ACT	0x00D0
+
+/* control */
+#define IEEE80211_STYPE_PSPOLL		0x00A0
+#define IEEE80211_STYPE_RTS		0x00B0
+#define IEEE80211_STYPE_CTS		0x00C0
+#define IEEE80211_STYPE_ACK		0x00D0
+#define IEEE80211_STYPE_CFEND		0x00E0
+#define IEEE80211_STYPE_CFENDACK	0x00F0
+
+/* data */
+#define IEEE80211_STYPE_DATA		0x0000
+#define IEEE80211_STYPE_DATA_CFACK	0x0010
+#define IEEE80211_STYPE_DATA_CFPOLL	0x0020
+#define IEEE80211_STYPE_DATA_CFACKPOLL	0x0030
+#define IEEE80211_STYPE_NULLFUNC	0x0040
+#define IEEE80211_STYPE_CFACK		0x0050
+#define IEEE80211_STYPE_CFPOLL		0x0060
+#define IEEE80211_STYPE_CFACKPOLL	0x0070
+#define IEEE80211_STYPE_QOS_DATA	0x0080 //added for WMM 2006/8/2
+#define IEEE80211_STYPE_QOS_NULL	0x00C0
+
+
+#define IEEE80211_SCTL_FRAG		0x000F
+#define IEEE80211_SCTL_SEQ		0xFFF0
+
+
+/* debug macros */
+
+#ifdef CONFIG_IEEE80211_DEBUG
+extern u32 ieee80211_debug_level;
+#define IEEE80211_DEBUG(level, fmt, args...) \
+do { if (ieee80211_debug_level & (level)) \
+  printk(KERN_DEBUG "ieee80211: %c %s " fmt, \
+         in_interrupt() ? 'I' : 'U', __FUNCTION__ , ## args); } while (0)
+#else
+#define IEEE80211_DEBUG(level, fmt, args...) do {} while (0)
+#endif	/* CONFIG_IEEE80211_DEBUG */
+
+/*
+ * To use the debug system;
+ *
+ * If you are defining a new debug classification, simply add it to the #define
+ * list here in the form of:
+ *
+ * #define IEEE80211_DL_xxxx VALUE
+ *
+ * shifting value to the left one bit from the previous entry.  xxxx should be
+ * the name of the classification (for example, WEP)
+ *
+ * You then need to either add a IEEE80211_xxxx_DEBUG() macro definition for your
+ * classification, or use IEEE80211_DEBUG(IEEE80211_DL_xxxx, ...) whenever you want
+ * to send output to that classification.
+ *
+ * To add your debug level to the list of levels seen when you perform
+ *
+ * % cat /proc/net/ipw/debug_level
+ *
+ * you simply need to add your entry to the ipw_debug_levels array.
+ *
+ * If you do not see debug_level in /proc/net/ipw then you do not have
+ * CONFIG_IEEE80211_DEBUG defined in your kernel configuration
+ *
+ */
+
+#define IEEE80211_DL_INFO          (1<<0)
+#define IEEE80211_DL_WX            (1<<1)
+#define IEEE80211_DL_SCAN          (1<<2)
+#define IEEE80211_DL_STATE         (1<<3)
+#define IEEE80211_DL_MGMT          (1<<4)
+#define IEEE80211_DL_FRAG          (1<<5)
+#define IEEE80211_DL_EAP           (1<<6)
+#define IEEE80211_DL_DROP          (1<<7)
+
+#define IEEE80211_DL_TX            (1<<8)
+#define IEEE80211_DL_RX            (1<<9)
+
+#define IEEE80211_ERROR(f, a...) printk(KERN_ERR "ieee80211: " f, ## a)
+#define IEEE80211_WARNING(f, a...) printk(KERN_WARNING "ieee80211: " f, ## a)
+#define IEEE80211_DEBUG_INFO(f, a...)   IEEE80211_DEBUG(IEEE80211_DL_INFO, f, ## a)
+
+#define IEEE80211_DEBUG_WX(f, a...)     IEEE80211_DEBUG(IEEE80211_DL_WX, f, ## a)
+#define IEEE80211_DEBUG_SCAN(f, a...)   IEEE80211_DEBUG(IEEE80211_DL_SCAN, f, ## a)
+//#define IEEE_DEBUG_SCAN  IEEE80211_WARNING
+#define IEEE80211_DEBUG_STATE(f, a...)  IEEE80211_DEBUG(IEEE80211_DL_STATE, f, ## a)
+#define IEEE80211_DEBUG_MGMT(f, a...)  IEEE80211_DEBUG(IEEE80211_DL_MGMT, f, ## a)
+#define IEEE80211_DEBUG_FRAG(f, a...)  IEEE80211_DEBUG(IEEE80211_DL_FRAG, f, ## a)
+#define IEEE80211_DEBUG_EAP(f, a...)  IEEE80211_DEBUG(IEEE80211_DL_EAP, f, ## a)
+#define IEEE80211_DEBUG_DROP(f, a...)  IEEE80211_DEBUG(IEEE80211_DL_DROP, f, ## a)
+#define IEEE80211_DEBUG_TX(f, a...)  IEEE80211_DEBUG(IEEE80211_DL_TX, f, ## a)
+#define IEEE80211_DEBUG_RX(f, a...)  IEEE80211_DEBUG(IEEE80211_DL_RX, f, ## a)
+#include <linux/netdevice.h>
+#include <linux/wireless.h>
+#include <linux/if_arp.h> /* ARPHRD_ETHER */
+
+#ifndef WIRELESS_SPY
+#define WIRELESS_SPY		// enable iwspy support
+#endif
+#include <net/iw_handler.h>	// new driver API
+
+#ifndef ETH_P_PAE
+#define ETH_P_PAE 0x888E /* Port Access Entity (IEEE 802.1X) */
+#endif /* ETH_P_PAE */
+
+#define ETH_P_PREAUTH 0x88C7 /* IEEE 802.11i pre-authentication */
+
+#ifndef ETH_P_80211_RAW
+#define ETH_P_80211_RAW (ETH_P_ECONET + 1)
+#endif
+
+/* IEEE 802.11 defines */
+
+#define P80211_OUI_LEN 3
+
+struct ieee80211_snap_hdr {
+
+        u8    dsap;   /* always 0xAA */
+        u8    ssap;   /* always 0xAA */
+        u8    ctrl;   /* always 0x03 */
+        u8    oui[P80211_OUI_LEN];    /* organizational universal id */
+
+} __attribute__ ((packed));
+
+#define SNAP_SIZE sizeof(struct ieee80211_snap_hdr)
+
+#define WLAN_FC_GET_TYPE(fc) ((fc) & IEEE80211_FCTL_FTYPE)
+#define WLAN_FC_GET_STYPE(fc) ((fc) & IEEE80211_FCTL_STYPE)
+
+#define WLAN_GET_SEQ_FRAG(seq) ((seq) & IEEE80211_SCTL_FRAG)
+#define WLAN_GET_SEQ_SEQ(seq)  ((seq) & IEEE80211_SCTL_SEQ)
+
+/* Authentication algorithms */
+#define WLAN_AUTH_OPEN 0
+#define WLAN_AUTH_SHARED_KEY 1
+
+#define WLAN_AUTH_CHALLENGE_LEN 128
+
+#define WLAN_CAPABILITY_BSS (1<<0)
+#define WLAN_CAPABILITY_IBSS (1<<1)
+#define WLAN_CAPABILITY_CF_POLLABLE (1<<2)
+#define WLAN_CAPABILITY_CF_POLL_REQUEST (1<<3)
+#define WLAN_CAPABILITY_PRIVACY (1<<4)
+#define WLAN_CAPABILITY_SHORT_PREAMBLE (1<<5)
+#define WLAN_CAPABILITY_PBCC (1<<6)
+#define WLAN_CAPABILITY_CHANNEL_AGILITY (1<<7)
+#define WLAN_CAPABILITY_SHORT_SLOT (1<<10)
+
+/* Status codes */
+#define WLAN_STATUS_SUCCESS 0
+#define WLAN_STATUS_UNSPECIFIED_FAILURE 1
+#define WLAN_STATUS_CAPS_UNSUPPORTED 10
+#define WLAN_STATUS_REASSOC_NO_ASSOC 11
+#define WLAN_STATUS_ASSOC_DENIED_UNSPEC 12
+#define WLAN_STATUS_NOT_SUPPORTED_AUTH_ALG 13
+#define WLAN_STATUS_UNKNOWN_AUTH_TRANSACTION 14
+#define WLAN_STATUS_CHALLENGE_FAIL 15
+#define WLAN_STATUS_AUTH_TIMEOUT 16
+#define WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA 17
+#define WLAN_STATUS_ASSOC_DENIED_RATES 18
+/* 802.11b */
+#define WLAN_STATUS_ASSOC_DENIED_NOSHORT 19
+#define WLAN_STATUS_ASSOC_DENIED_NOPBCC 20
+#define WLAN_STATUS_ASSOC_DENIED_NOAGILITY 21
+
+/* Reason codes */
+#define WLAN_REASON_UNSPECIFIED 1
+#define WLAN_REASON_PREV_AUTH_NOT_VALID 2
+#define WLAN_REASON_DEAUTH_LEAVING 3
+#define WLAN_REASON_DISASSOC_DUE_TO_INACTIVITY 4
+#define WLAN_REASON_DISASSOC_AP_BUSY 5
+#define WLAN_REASON_CLASS2_FRAME_FROM_NONAUTH_STA 6
+#define WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA 7
+#define WLAN_REASON_DISASSOC_STA_HAS_LEFT 8
+#define WLAN_REASON_STA_REQ_ASSOC_WITHOUT_AUTH 9
+
+
+/* Information Element IDs */
+#define WLAN_EID_SSID 0
+#define WLAN_EID_SUPP_RATES 1
+#define WLAN_EID_FH_PARAMS 2
+#define WLAN_EID_DS_PARAMS 3
+#define WLAN_EID_CF_PARAMS 4
+#define WLAN_EID_TIM 5
+#define WLAN_EID_IBSS_PARAMS 6
+#define WLAN_EID_CHALLENGE 16
+#define WLAN_EID_RSN 48
+#define WLAN_EID_GENERIC 221
+
+#define IEEE80211_MGMT_HDR_LEN 24
+#define IEEE80211_DATA_HDR3_LEN 24
+#define IEEE80211_DATA_HDR4_LEN 30
+
+
+#define IEEE80211_STATMASK_SIGNAL (1<<0)
+#define IEEE80211_STATMASK_RSSI (1<<1)
+#define IEEE80211_STATMASK_NOISE (1<<2)
+#define IEEE80211_STATMASK_RATE (1<<3)
+#define IEEE80211_STATMASK_WEMASK 0x7
+
+
+#define IEEE80211_CCK_MODULATION    (1<<0)
+#define IEEE80211_OFDM_MODULATION   (1<<1)
+
+#define IEEE80211_24GHZ_BAND     (1<<0)
+#define IEEE80211_52GHZ_BAND     (1<<1)
+
+#define IEEE80211_CCK_RATE_LEN  		4
+#define IEEE80211_CCK_RATE_1MB		        0x02
+#define IEEE80211_CCK_RATE_2MB		        0x04
+#define IEEE80211_CCK_RATE_5MB		        0x0B
+#define IEEE80211_CCK_RATE_11MB		        0x16
+#define IEEE80211_OFDM_RATE_LEN 		8
+#define IEEE80211_OFDM_RATE_6MB		        0x0C
+#define IEEE80211_OFDM_RATE_9MB		        0x12
+#define IEEE80211_OFDM_RATE_12MB		0x18
+#define IEEE80211_OFDM_RATE_18MB		0x24
+#define IEEE80211_OFDM_RATE_24MB		0x30
+#define IEEE80211_OFDM_RATE_36MB		0x48
+#define IEEE80211_OFDM_RATE_48MB		0x60
+#define IEEE80211_OFDM_RATE_54MB		0x6C
+#define IEEE80211_BASIC_RATE_MASK		0x80
+
+#define IEEE80211_CCK_RATE_1MB_MASK		(1<<0)
+#define IEEE80211_CCK_RATE_2MB_MASK		(1<<1)
+#define IEEE80211_CCK_RATE_5MB_MASK		(1<<2)
+#define IEEE80211_CCK_RATE_11MB_MASK		(1<<3)
+#define IEEE80211_OFDM_RATE_6MB_MASK		(1<<4)
+#define IEEE80211_OFDM_RATE_9MB_MASK		(1<<5)
+#define IEEE80211_OFDM_RATE_12MB_MASK		(1<<6)
+#define IEEE80211_OFDM_RATE_18MB_MASK		(1<<7)
+#define IEEE80211_OFDM_RATE_24MB_MASK		(1<<8)
+#define IEEE80211_OFDM_RATE_36MB_MASK		(1<<9)
+#define IEEE80211_OFDM_RATE_48MB_MASK		(1<<10)
+#define IEEE80211_OFDM_RATE_54MB_MASK		(1<<11)
+
+#define IEEE80211_CCK_RATES_MASK	        0x0000000F
+#define IEEE80211_CCK_BASIC_RATES_MASK	(IEEE80211_CCK_RATE_1MB_MASK | \
+	IEEE80211_CCK_RATE_2MB_MASK)
+#define IEEE80211_CCK_DEFAULT_RATES_MASK	(IEEE80211_CCK_BASIC_RATES_MASK | \
+        IEEE80211_CCK_RATE_5MB_MASK | \
+        IEEE80211_CCK_RATE_11MB_MASK)
+
+#define IEEE80211_OFDM_RATES_MASK		0x00000FF0
+#define IEEE80211_OFDM_BASIC_RATES_MASK	(IEEE80211_OFDM_RATE_6MB_MASK | \
+	IEEE80211_OFDM_RATE_12MB_MASK | \
+	IEEE80211_OFDM_RATE_24MB_MASK)
+#define IEEE80211_OFDM_DEFAULT_RATES_MASK	(IEEE80211_OFDM_BASIC_RATES_MASK | \
+	IEEE80211_OFDM_RATE_9MB_MASK  | \
+	IEEE80211_OFDM_RATE_18MB_MASK | \
+	IEEE80211_OFDM_RATE_36MB_MASK | \
+	IEEE80211_OFDM_RATE_48MB_MASK | \
+	IEEE80211_OFDM_RATE_54MB_MASK)
+#define IEEE80211_DEFAULT_RATES_MASK (IEEE80211_OFDM_DEFAULT_RATES_MASK | \
+                                IEEE80211_CCK_DEFAULT_RATES_MASK)
+
+#define IEEE80211_NUM_OFDM_RATES	    8
+#define IEEE80211_NUM_CCK_RATES	            4
+#define IEEE80211_OFDM_SHIFT_MASK_A         4
+
+
+
+
+/* NOTE: This data is for statistical purposes; not all hardware provides this
+ *       information for frames received.  Not setting these will not cause
+ *       any adverse affects. */
+struct ieee80211_rx_stats {
+	u32 mac_time[2];
+	u8 signalstrength;
+	s8 rssi;
+	u8 signal;
+	u8 noise;
+	u16 rate; /* in 100 kbps */
+	u8 received_channel;
+	u8 control;
+	u8 mask;
+	u8 freq;
+	u16 len;
+	u8 nic_type;
+};
+
+/* IEEE 802.11 requires that STA supports concurrent reception of at least
+ * three fragmented frames. This define can be increased to support more
+ * concurrent frames, but it should be noted that each entry can consume about
+ * 2 kB of RAM and increasing cache size will slow down frame reassembly. */
+#define IEEE80211_FRAG_CACHE_LEN 4
+
+struct ieee80211_frag_entry {
+	unsigned long first_frag_time;
+	unsigned int seq;
+	unsigned int last_frag;
+	struct sk_buff *skb;
+	u8 src_addr[ETH_ALEN];
+	u8 dst_addr[ETH_ALEN];
+};
+
+struct ieee80211_stats {
+	unsigned int tx_unicast_frames;
+	unsigned int tx_multicast_frames;
+	unsigned int tx_fragments;
+	unsigned int tx_unicast_octets;
+	unsigned int tx_multicast_octets;
+	unsigned int tx_deferred_transmissions;
+	unsigned int tx_single_retry_frames;
+	unsigned int tx_multiple_retry_frames;
+	unsigned int tx_retry_limit_exceeded;
+	unsigned int tx_discards;
+	unsigned int rx_unicast_frames;
+	unsigned int rx_multicast_frames;
+	unsigned int rx_fragments;
+	unsigned int rx_unicast_octets;
+	unsigned int rx_multicast_octets;
+	unsigned int rx_fcs_errors;
+	unsigned int rx_discards_no_buffer;
+	unsigned int tx_discards_wrong_sa;
+	unsigned int rx_discards_undecryptable;
+	unsigned int rx_message_in_msg_fragments;
+	unsigned int rx_message_in_bad_msg_fragments;
+};
+
+struct ieee80211_softmac_stats{
+	unsigned int rx_ass_ok;
+	unsigned int rx_ass_err;
+	unsigned int rx_probe_rq;
+	unsigned int tx_probe_rs;
+	unsigned int tx_beacons;
+	unsigned int rx_auth_rq;
+	unsigned int rx_auth_rs_ok;
+	unsigned int rx_auth_rs_err;
+	unsigned int tx_auth_rq;
+	unsigned int no_auth_rs;
+	unsigned int no_ass_rs;
+	unsigned int tx_ass_rq;
+	unsigned int rx_ass_rq;
+	unsigned int tx_probe_rq;
+	unsigned int reassoc;
+	unsigned int swtxstop;
+	unsigned int swtxawake;
+};
+
+struct ieee80211_device;
+
+#include "ieee80211_crypt.h"
+
+#define SEC_KEY_1         (1<<0)
+#define SEC_KEY_2         (1<<1)
+#define SEC_KEY_3         (1<<2)
+#define SEC_KEY_4         (1<<3)
+#define SEC_ACTIVE_KEY    (1<<4)
+#define SEC_AUTH_MODE     (1<<5)
+#define SEC_UNICAST_GROUP (1<<6)
+#define SEC_LEVEL         (1<<7)
+#define SEC_ENABLED       (1<<8)
+
+#define SEC_LEVEL_0      0 /* None */
+#define SEC_LEVEL_1      1 /* WEP 40 and 104 bit */
+#define SEC_LEVEL_2      2 /* Level 1 + TKIP */
+#define SEC_LEVEL_2_CKIP 3 /* Level 1 + CKIP */
+#define SEC_LEVEL_3      4 /* Level 2 + CCMP */
+
+#define WEP_KEYS 4
+#define WEP_KEY_LEN 13
+
+#define WEP_KEY_LEN_MODIF 32
+
+struct ieee80211_security {
+	u16 active_key:2,
+            enabled:1,
+	    auth_mode:2,
+            auth_algo:4,
+            unicast_uses_group:1;
+	u8 key_sizes[WEP_KEYS];
+	u8 keys[WEP_KEYS][WEP_KEY_LEN_MODIF];
+	u8 level;
+	u16 flags;
+} __attribute__ ((packed));
+
+
+/*
+
+ 802.11 data frame from AP
+
+      ,-------------------------------------------------------------------.
+Bytes |  2   |  2   |    6    |    6    |    6    |  2   | 0..2312 |   4  |
+      |------|------|---------|---------|---------|------|---------|------|
+Desc. | ctrl | dura |  DA/RA  |   TA    |    SA   | Sequ |  frame  |  fcs |
+      |      | tion | (BSSID) |         |         | ence |  data   |      |
+      `-------------------------------------------------------------------'
+
+Total: 28-2340 bytes
+
+*/
+
+struct ieee80211_header_data {
+	u16 frame_ctl;
+	u16 duration_id;
+	u8 addr1[6];
+	u8 addr2[6];
+	u8 addr3[6];
+	u16 seq_ctrl;
+};
+
+#define BEACON_PROBE_SSID_ID_POSITION 12
+
+/* Management Frame Information Element Types */
+#define MFIE_TYPE_SSID       0
+#define MFIE_TYPE_RATES      1
+#define MFIE_TYPE_FH_SET     2
+#define MFIE_TYPE_DS_SET     3
+#define MFIE_TYPE_CF_SET     4
+#define MFIE_TYPE_TIM        5
+#define MFIE_TYPE_IBSS_SET   6
+#define MFIE_TYPE_COUNTRY  7 //+YJ,080625
+#define MFIE_TYPE_CHALLENGE  16
+#define MFIE_TYPE_ERP        42
+#define MFIE_TYPE_RSN	     48
+#define MFIE_TYPE_RATES_EX   50
+#define MFIE_TYPE_GENERIC    221
+
+#ifdef ENABLE_DOT11D
+typedef enum
+{
+	COUNTRY_CODE_FCC = 0,
+	COUNTRY_CODE_IC = 1,
+	COUNTRY_CODE_ETSI = 2,
+	COUNTRY_CODE_SPAIN = 3,
+	COUNTRY_CODE_FRANCE = 4,
+	COUNTRY_CODE_MKK = 5,
+	COUNTRY_CODE_MKK1 = 6,
+	COUNTRY_CODE_ISRAEL = 7,
+	COUNTRY_CODE_TELEC = 8,
+	COUNTRY_CODE_GLOBAL_DOMAIN = 9,
+	COUNTRY_CODE_WORLD_WIDE_13_INDEX = 10
+}country_code_type_t;
+#endif
+
+struct ieee80211_info_element_hdr {
+	u8 id;
+	u8 len;
+} __attribute__ ((packed));
+
+struct ieee80211_info_element {
+	u8 id;
+	u8 len;
+	u8 data[0];
+} __attribute__ ((packed));
+
+/*
+ * These are the data types that can make up management packets
+ *
+	u16 auth_algorithm;
+	u16 auth_sequence;
+	u16 beacon_interval;
+	u16 capability;
+	u8 current_ap[ETH_ALEN];
+	u16 listen_interval;
+	struct {
+		u16 association_id:14, reserved:2;
+	} __attribute__ ((packed));
+	u32 time_stamp[2];
+	u16 reason;
+	u16 status;
+*/
+
+#define IEEE80211_DEFAULT_TX_ESSID "Penguin"
+#define IEEE80211_DEFAULT_BASIC_RATE 10
+
+struct ieee80211_authentication {
+	struct ieee80211_header_data header;
+	u16 algorithm;
+	u16 transaction;
+	u16 status;
+	//struct ieee80211_info_element_hdr info_element;
+} __attribute__ ((packed));
+
+
+struct ieee80211_probe_response {
+	struct ieee80211_header_data header;
+	u32 time_stamp[2];
+	u16 beacon_interval;
+	u16 capability;
+	struct ieee80211_info_element info_element;
+} __attribute__ ((packed));
+
+struct ieee80211_probe_request {
+	struct ieee80211_header_data header;
+	/*struct ieee80211_info_element info_element;*/
+} __attribute__ ((packed));
+
+struct ieee80211_assoc_request_frame {
+	struct ieee80211_hdr_3addr header;
+	u16 capability;
+	u16 listen_interval;
+	//u8 current_ap[ETH_ALEN];
+	struct ieee80211_info_element_hdr info_element;
+} __attribute__ ((packed));
+
+struct ieee80211_assoc_response_frame {
+	struct ieee80211_hdr_3addr header;
+	u16 capability;
+	u16 status;
+	u16 aid;
+	struct ieee80211_info_element info_element; /* supported rates */
+} __attribute__ ((packed));
+
+struct ieee80211_disassoc_frame{
+        struct ieee80211_hdr_3addr header;
+        u16    reasoncode;
+}__attribute__ ((packed));
+
+struct ieee80211_txb {
+	u8 nr_frags;
+	u8 encrypted;
+	u16 reserved;
+	u16 frag_size;
+	u16 payload_size;
+	struct sk_buff *fragments[0];
+};
+
+struct ieee80211_wmm_ac_param {
+	u8 ac_aci_acm_aifsn;
+	u8 ac_ecwmin_ecwmax;
+	u16 ac_txop_limit;
+};
+
+struct ieee80211_wmm_ts_info {
+	u8 ac_dir_tid;
+	u8 ac_up_psb;
+	u8 reserved;
+} __attribute__ ((packed));
+
+struct ieee80211_wmm_tspec_elem {
+	struct ieee80211_wmm_ts_info ts_info;
+	u16 norm_msdu_size;
+	u16 max_msdu_size;
+	u32 min_serv_inter;
+	u32 max_serv_inter;
+	u32 inact_inter;
+	u32 suspen_inter;
+	u32 serv_start_time;
+	u32 min_data_rate;
+	u32 mean_data_rate;
+	u32 peak_data_rate;
+	u32 max_burst_size;
+	u32 delay_bound;
+	u32 min_phy_rate;
+	u16 surp_band_allow;
+	u16 medium_time;
+}__attribute__((packed));
+
+enum {WMM_all_frame, WMM_two_frame, WMM_four_frame, WMM_six_frame};
+#define MAX_SP_Len  (WMM_all_frame << 4)
+#define IEEE80211_QOS_TID 0x0f
+#define QOS_CTL_NOTCONTAIN_ACK (0x01 << 5)
+
+/* SWEEP TABLE ENTRIES NUMBER*/
+#define MAX_SWEEP_TAB_ENTRIES		  42
+#define MAX_SWEEP_TAB_ENTRIES_PER_PACKET  7
+/* MAX_RATES_LENGTH needs to be 12.  The spec says 8, and many APs
+ * only use 8, and then use extended rates for the remaining supported
+ * rates.  Other APs, however, stick all of their supported rates on the
+ * main rates information element... */
+#define MAX_RATES_LENGTH                  ((u8)12)
+#define MAX_RATES_EX_LENGTH               ((u8)16)
+#define MAX_NETWORK_COUNT                  128
+//#define MAX_CHANNEL_NUMBER                 161
+#define MAX_CHANNEL_NUMBER                 165 //YJ,modified,080625
+#define MAX_IE_LEN						0xFF //+YJ,080625
+
+typedef struct _CHANNEL_LIST{
+	u8	Channel[MAX_CHANNEL_NUMBER + 1];
+	u8	Len;
+}CHANNEL_LIST, *PCHANNEL_LIST;
+
+#define IEEE80211_SOFTMAC_SCAN_TIME	  100//400
+//(HZ / 2)
+//by amy for ps
+#define IEEE80211_WATCH_DOG_TIME    2000
+//by amy for ps
+//by amy for antenna
+#define ANTENNA_DIVERSITY_TIMER_PERIOD		1000 // 1000 m
+//by amy for antenna
+#define IEEE80211_SOFTMAC_ASSOC_RETRY_TIME (HZ * 2)
+
+#define CRC_LENGTH                 4U
+
+#define MAX_WPA_IE_LEN 64
+
+#define NETWORK_EMPTY_ESSID (1<<0)
+#define NETWORK_HAS_OFDM    (1<<1)
+#define NETWORK_HAS_CCK     (1<<2)
+
+#define IEEE80211_DTIM_MBCAST 4
+#define IEEE80211_DTIM_UCAST 2
+#define IEEE80211_DTIM_VALID 1
+#define IEEE80211_DTIM_INVALID 0
+
+#define IEEE80211_PS_DISABLED 0
+#define IEEE80211_PS_UNICAST IEEE80211_DTIM_UCAST
+#define IEEE80211_PS_MBCAST IEEE80211_DTIM_MBCAST
+#define IEEE80211_PS_ENABLE   IEEE80211_DTIM_VALID
+//added by David for QoS 2006/6/30
+//#define WMM_Hang_8187
+#ifdef WMM_Hang_8187
+#undef WMM_Hang_8187
+#endif
+
+#define WME_AC_BE   0x00
+#define WME_AC_BK   0x01
+#define WME_AC_VI   0x02
+#define WME_AC_VO   0x03
+#define WME_ACI_MASK 0x03
+#define WME_AIFSN_MASK 0x03
+#define WME_AC_PRAM_LEN 16
+
+//UP Mapping to AC, using in MgntQuery_SequenceNumber() and maybe for DSCP
+//#define UP2AC(up)	((up<3) ? ((up==0)?1:0) : (up>>1))
+#define UP2AC(up) (		   \
+	((up) < 1) ? WME_AC_BE : \
+	((up) < 3) ? WME_AC_BK : \
+	((up) < 4) ? WME_AC_BE : \
+	((up) < 6) ? WME_AC_VI : \
+	WME_AC_VO)
+//AC Mapping to UP, using in Tx part for selecting the corresponding TX queue
+#define AC2UP(_ac)	(       \
+	((_ac) == WME_AC_VO) ? 6 : \
+	((_ac) == WME_AC_VI) ? 5 : \
+	((_ac) == WME_AC_BK) ? 1 : \
+	0)
+
+#define	ETHER_ADDR_LEN		6	/* length of an Ethernet address */
+struct	ether_header {
+	u8 ether_dhost[ETHER_ADDR_LEN];
+	u8 ether_shost[ETHER_ADDR_LEN];
+	u16 ether_type;
+} __attribute__((packed));
+
+#ifndef ETHERTYPE_PAE
+#define	ETHERTYPE_PAE	0x888e		/* EAPOL PAE/802.1x */
+#endif
+#ifndef ETHERTYPE_IP
+#define	ETHERTYPE_IP	0x0800		/* IP protocol */
+#endif
+
+struct ieee80211_network {
+	/* These entries are used to identify a unique network */
+	u8 bssid[ETH_ALEN];
+	u8 channel;
+	/* Ensure null-terminated for any debug msgs */
+	u8 ssid[IW_ESSID_MAX_SIZE + 1];
+	u8 ssid_len;
+
+	/* These are network statistics */
+	struct ieee80211_rx_stats stats;
+	u16 capability;
+	u8 rates[MAX_RATES_LENGTH];
+	u8 rates_len;
+	u8 rates_ex[MAX_RATES_EX_LENGTH];
+	u8 rates_ex_len;
+	unsigned long last_scanned;
+	u8 mode;
+	u8 flags;
+	u32 last_associate;
+	u32 time_stamp[2];
+	u16 beacon_interval;
+	u16 listen_interval;
+	u16 atim_window;
+	u8 wpa_ie[MAX_WPA_IE_LEN];
+	size_t wpa_ie_len;
+	u8 rsn_ie[MAX_WPA_IE_LEN];
+	size_t rsn_ie_len;
+	u8 dtim_period;
+	u8 dtim_data;
+	u32 last_dtim_sta_time[2];
+	struct list_head list;
+	//appeded for QoS
+	u8 wmm_info;
+	struct ieee80211_wmm_ac_param wmm_param[4];
+	u8 QoS_Enable;
+	u8 SignalStrength;
+//by amy 080312
+	u8 HighestOperaRate;
+//by amy 080312
+#ifdef THOMAS_TURBO
+	u8 Turbo_Enable;//enable turbo mode, added by thomas
+#endif
+#ifdef ENABLE_DOT11D
+	u16 CountryIeLen;
+	u8 CountryIeBuf[MAX_IE_LEN];
+#endif
+};
+
+enum ieee80211_state {
+
+	/* the card is not linked at all */
+	IEEE80211_NOLINK = 0,
+
+	/* IEEE80211_ASSOCIATING* are for BSS client mode
+	 * the driver shall not perform RX filtering unless
+	 * the state is LINKED.
+	 * The driver shall just check for the state LINKED and
+	 * defaults to NOLINK for ALL the other states (including
+	 * LINKED_SCANNING)
+	 */
+
+	/* the association procedure will start (wq scheduling)*/
+	IEEE80211_ASSOCIATING,
+	IEEE80211_ASSOCIATING_RETRY,
+
+	/* the association procedure is sending AUTH request*/
+	IEEE80211_ASSOCIATING_AUTHENTICATING,
+
+	/* the association procedure has successfully authentcated
+	 * and is sending association request
+	 */
+	IEEE80211_ASSOCIATING_AUTHENTICATED,
+
+	/* the link is ok. the card associated to a BSS or linked
+	 * to a ibss cell or acting as an AP and creating the bss
+	 */
+	IEEE80211_LINKED,
+
+	/* same as LINKED, but the driver shall apply RX filter
+	 * rules as we are in NO_LINK mode. As the card is still
+	 * logically linked, but it is doing a syncro site survey
+	 * then it will be back to LINKED state.
+	 */
+	IEEE80211_LINKED_SCANNING,
+
+};
+
+#define DEFAULT_MAX_SCAN_AGE (15 * HZ)
+#define DEFAULT_FTS 2346
+#define MAC_FMT "%02x:%02x:%02x:%02x:%02x:%02x"
+#define MAC_ARG(x) ((u8*)(x))[0],((u8*)(x))[1],((u8*)(x))[2],((u8*)(x))[3],((u8*)(x))[4],((u8*)(x))[5]
+
+
+#if (LINUX_VERSION_CODE <= KERNEL_VERSION(2,6,11))
+extern inline int is_multicast_ether_addr(const u8 *addr)
+{
+        return ((addr[0] != 0xff) && (0x01 & addr[0]));
+}
+#endif
+
+#if (LINUX_VERSION_CODE <= KERNEL_VERSION(2,6,13))
+extern inline int is_broadcast_ether_addr(const u8 *addr)
+{
+	return ((addr[0] == 0xff) && (addr[1] == 0xff) && (addr[2] == 0xff) &&   \
+		(addr[3] == 0xff) && (addr[4] == 0xff) && (addr[5] == 0xff));
+}
+#endif
+
+#define CFG_IEEE80211_RESERVE_FCS (1<<0)
+#define CFG_IEEE80211_COMPUTE_FCS (1<<1)
+
+typedef struct tx_pending_t{
+	int frag;
+	struct ieee80211_txb *txb;
+}tx_pending_t;
+
+
+struct ieee80211_device {
+	struct net_device *dev;
+
+	/* Bookkeeping structures */
+	struct net_device_stats stats;
+	struct ieee80211_stats ieee_stats;
+	struct ieee80211_softmac_stats softmac_stats;
+
+	/* Probe / Beacon management */
+	struct list_head network_free_list;
+	struct list_head network_list;
+	struct ieee80211_network *networks;
+	int scans;
+	int scan_age;
+
+	int iw_mode; /* operating mode (IW_MODE_*) */
+
+	spinlock_t lock;
+	spinlock_t wpax_suitlist_lock;
+
+	int tx_headroom; /* Set to size of any additional room needed at front
+			  * of allocated Tx SKBs */
+	u32 config;
+
+	/* WEP and other encryption related settings at the device level */
+	int open_wep; /* Set to 1 to allow unencrypted frames */
+
+	int reset_on_keychange; /* Set to 1 if the HW needs to be reset on
+				 * WEP key changes */
+
+	/* If the host performs {en,de}cryption, then set to 1 */
+	int host_encrypt;
+	int host_decrypt;
+	int ieee802_1x; /* is IEEE 802.1X used */
+
+	/* WPA data */
+	int wpa_enabled;
+	int drop_unencrypted;
+	int tkip_countermeasures;
+	int privacy_invoked;
+	size_t wpa_ie_len;
+	u8 *wpa_ie;
+
+	u8 ap_mac_addr[6];
+	u16 pairwise_key_type;
+	u16 broadcast_key_type;
+
+	struct list_head crypt_deinit_list;
+	struct ieee80211_crypt_data *crypt[WEP_KEYS];
+	int tx_keyidx; /* default TX key index (crypt[tx_keyidx]) */
+	struct timer_list crypt_deinit_timer;
+
+	int bcrx_sta_key; /* use individual keys to override default keys even
+			   * with RX of broad/multicast frames */
+
+	/* Fragmentation structures */
+	// each streaming contain a entry
+	struct ieee80211_frag_entry frag_cache[17][IEEE80211_FRAG_CACHE_LEN];
+	unsigned int frag_next_idx[17];
+	u16 fts; /* Fragmentation Threshold */
+
+	/* This stores infos for the current network.
+	 * Either the network we are associated in INFRASTRUCTURE
+	 * or the network that we are creating in MASTER mode.
+	 * ad-hoc is a mixture ;-).
+	 * Note that in infrastructure mode, even when not associated,
+	 * fields bssid and essid may be valid (if wpa_set and essid_set
+	 * are true) as thy carry the value set by the user via iwconfig
+	 */
+	struct ieee80211_network current_network;
+
+
+	enum ieee80211_state state;
+
+	int short_slot;
+	int mode;       /* A, B, G */
+	int modulation; /* CCK, OFDM */
+	int freq_band;  /* 2.4Ghz, 5.2Ghz, Mixed */
+	int abg_true;   /* ABG flag              */
+
+	/* used for forcing the ibss workqueue to terminate
+	 * without wait for the syncro scan to terminate
+	 */
+	short sync_scan_hurryup;
+
+#ifdef ENABLE_DOT11D
+	void * pDot11dInfo;
+	bool bGlobalDomain;
+
+	// For Liteon Ch12~13 passive scan
+	u8	MinPassiveChnlNum;
+	u8	IbssStartChnl;
+#else
+	/* map of allowed channels. 0 is dummy */
+	// FIXME: remeber to default to a basic channel plan depending of the PHY type
+	int channel_map[MAX_CHANNEL_NUMBER+1];
+#endif
+
+	int rate;       /* current rate */
+	int basic_rate;
+	//FIXME: pleace callback, see if redundant with softmac_features
+	short active_scan;
+
+	/* this contains flags for selectively enable softmac support */
+	u16 softmac_features;
+
+	/* if the sequence control field is not filled by HW */
+	u16 seq_ctrl[5];
+
+	/* association procedure transaction sequence number */
+	u16 associate_seq;
+
+	/* AID for RTXed association responses */
+	u16 assoc_id;
+
+	/* power save mode related*/
+	short ps;
+	short sta_sleep;
+	int ps_timeout;
+	struct tasklet_struct ps_task;
+	u32 ps_th;
+	u32 ps_tl;
+
+	short raw_tx;
+	/* used if IEEE_SOFTMAC_TX_QUEUE is set */
+	short queue_stop;
+	short scanning;
+	short proto_started;
+
+	struct semaphore wx_sem;
+	struct semaphore scan_sem;
+
+	spinlock_t mgmt_tx_lock;
+	spinlock_t beacon_lock;
+
+	short beacon_txing;
+
+	short wap_set;
+	short ssid_set;
+
+	u8  wpax_type_set;    //{added by David, 2006.9.28}
+	u32 wpax_type_notify; //{added by David, 2006.9.26}
+
+	/* QoS related flag */
+	char init_wmmparam_flag;
+
+	/* for discarding duplicated packets in IBSS */
+	struct list_head ibss_mac_hash[IEEE_IBSS_MAC_HASH_SIZE];
+
+	/* for discarding duplicated packets in BSS */
+	u16 last_rxseq_num[17]; /* rx seq previous per-tid */
+	u16 last_rxfrag_num[17];/* tx frag previous per-tid */
+	unsigned long last_packet_time[17];
+
+	/* for PS mode */
+	unsigned long last_rx_ps_time;
+
+	/* used if IEEE_SOFTMAC_SINGLE_QUEUE is set */
+	struct sk_buff *mgmt_queue_ring[MGMT_QUEUE_NUM];
+	int mgmt_queue_head;
+	int mgmt_queue_tail;
+
+
+	/* used if IEEE_SOFTMAC_TX_QUEUE is set */
+	struct  tx_pending_t tx_pending;
+
+	/* used if IEEE_SOFTMAC_ASSOCIATE is set */
+	struct timer_list associate_timer;
+
+	/* used if IEEE_SOFTMAC_BEACONS is set */
+	struct timer_list beacon_timer;
+
+	struct work_struct associate_complete_wq;
+//	struct work_struct associate_retry_wq;
+	struct work_struct associate_procedure_wq;
+//	struct work_struct softmac_scan_wq;
+	struct work_struct wx_sync_scan_wq;
+	struct work_struct wmm_param_update_wq;
+	struct work_struct ps_request_tx_ack_wq;//for ps
+//	struct work_struct hw_wakeup_wq;
+//	struct work_struct hw_sleep_wq;
+//	struct work_struct watch_dog_wq;
+	bool bInactivePs;
+	bool actscanning;
+	bool beinretry;
+	u16 ListenInterval;
+	unsigned long NumRxDataInPeriod; //YJ,add,080828
+	unsigned long NumRxBcnInPeriod;  //YJ,add,080828
+	unsigned long NumRxOkTotal;
+	unsigned long NumRxUnicast;//YJ,add,080828,for keep alive
+	bool bHwRadioOff;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20)
+        struct delayed_work softmac_scan_wq;
+        struct delayed_work associate_retry_wq;
+	struct delayed_work hw_wakeup_wq;
+	struct delayed_work hw_sleep_wq;//+by amy 080324
+	struct delayed_work watch_dog_wq;
+	struct delayed_work sw_antenna_wq;
+	struct delayed_work  start_ibss_wq;
+//by amy for rate adaptive 080312
+    struct delayed_work rate_adapter_wq;
+//by amy for rate adaptive
+	struct delayed_work hw_dig_wq;
+	struct delayed_work tx_pw_wq;
+
+//Added for RF power on power off by lizhaoming 080512
+	struct delayed_work GPIOChangeRFWorkItem;
+#else
+
+	struct work_struct start_ibss_wq;
+        struct work_struct softmac_scan_wq;
+        struct work_struct associate_retry_wq;
+	struct work_struct hw_wakeup_wq;
+	struct work_struct hw_sleep_wq;
+	struct work_struct watch_dog_wq;
+	struct work_struct sw_antenna_wq;
+//by amy for rate adaptive 080312
+    struct work_struct rate_adapter_wq;
+//by amy for rate adaptive
+	struct work_struct hw_dig_wq;
+	struct work_struct tx_pw_wq;
+
+//Added for RF power on power off by lizhaoming 080512
+	struct work_struct GPIOChangeRFWorkItem;
+#endif
+	struct workqueue_struct *wq;
+
+	/* Callback functions */
+	void (*set_security)(struct net_device *dev,
+			     struct ieee80211_security *sec);
+
+	/* Used to TX data frame by using txb structs.
+	 * this is not used if in the softmac_features
+	 * is set the flag IEEE_SOFTMAC_TX_QUEUE
+	 */
+	int (*hard_start_xmit)(struct ieee80211_txb *txb,
+			       struct net_device *dev);
+
+	int (*reset_port)(struct net_device *dev);
+
+	/* Softmac-generated frames (mamagement) are TXed via this
+	 * callback if the flag IEEE_SOFTMAC_SINGLE_QUEUE is
+	 * not set. As some cards may have different HW queues that
+	 * one might want to use for data and management frames
+	 * the option to have two callbacks might be useful.
+	 * This fucntion can't sleep.
+	 */
+	int (*softmac_hard_start_xmit)(struct sk_buff *skb,
+			       struct net_device *dev);
+
+	/* used instead of hard_start_xmit (not softmac_hard_start_xmit)
+	 * if the IEEE_SOFTMAC_TX_QUEUE feature is used to TX data
+	 * frames. I the option IEEE_SOFTMAC_SINGLE_QUEUE is also set
+	 * then also management frames are sent via this callback.
+	 * This function can't sleep.
+	 */
+	void (*softmac_data_hard_start_xmit)(struct sk_buff *skb,
+			       struct net_device *dev,int rate);
+
+	/* stops the HW queue for DATA frames. Useful to avoid
+	 * waste time to TX data frame when we are reassociating
+	 * This function can sleep.
+	 */
+	void (*data_hard_stop)(struct net_device *dev);
+
+	/* OK this is complementar to data_poll_hard_stop */
+	void (*data_hard_resume)(struct net_device *dev);
+
+	/* ask to the driver to retune the radio .
+	 * This function can sleep. the driver should ensure
+	 * the radio has been swithced before return.
+	 */
+	void (*set_chan)(struct net_device *dev,short ch);
+
+	/* These are not used if the ieee stack takes care of
+	 * scanning (IEEE_SOFTMAC_SCAN feature set).
+	 * In this case only the set_chan is used.
+	 *
+	 * The syncro version is similar to the start_scan but
+	 * does not return until all channels has been scanned.
+	 * this is called in user context and should sleep,
+	 * it is called in a work_queue when swithcing to ad-hoc mode
+	 * or in behalf of iwlist scan when the card is associated
+	 * and root user ask for a scan.
+	 * the fucntion stop_scan should stop both the syncro and
+	 * background scanning and can sleep.
+	 * The fucntion start_scan should initiate the background
+	 * scanning and can't sleep.
+	 */
+	void (*scan_syncro)(struct net_device *dev);
+	void (*start_scan)(struct net_device *dev);
+	void (*stop_scan)(struct net_device *dev);
+
+	/* indicate the driver that the link state is changed
+	 * for example it may indicate the card is associated now.
+	 * Driver might be interested in this to apply RX filter
+	 * rules or simply light the LINK led
+	 */
+	void (*link_change)(struct net_device *dev);
+
+	/* these two function indicates to the HW when to start
+	 * and stop to send beacons. This is used when the
+	 * IEEE_SOFTMAC_BEACONS is not set. For now the
+	 * stop_send_bacons is NOT guaranteed to be called only
+	 * after start_send_beacons.
+	 */
+	void (*start_send_beacons) (struct net_device *dev);
+	void (*stop_send_beacons) (struct net_device *dev);
+
+	/* power save mode related */
+	void (*sta_wake_up) (struct net_device *dev);
+	void (*ps_request_tx_ack) (struct net_device *dev);
+	void (*enter_sleep_state) (struct net_device *dev, u32 th, u32 tl);
+	short (*ps_is_queue_empty) (struct net_device *dev);
+
+	/* QoS related */
+	//void (*wmm_param_update) (struct net_device *dev, u8 *ac_param);
+	//void (*wmm_param_update) (struct ieee80211_device *ieee);
+
+	/* This must be the last item so that it points to the data
+	 * allocated beyond this structure by alloc_ieee80211 */
+	u8 priv[0];
+};
+
+#define IEEE_A            (1<<0)
+#define IEEE_B            (1<<1)
+#define IEEE_G            (1<<2)
+#define IEEE_MODE_MASK    (IEEE_A|IEEE_B|IEEE_G)
+
+/* Generate a 802.11 header */
+
+/* Uses the channel change callback directly
+ * instead of [start/stop] scan callbacks
+ */
+#define IEEE_SOFTMAC_SCAN (1<<2)
+
+/* Perform authentication and association handshake */
+#define IEEE_SOFTMAC_ASSOCIATE (1<<3)
+
+/* Generate probe requests */
+#define IEEE_SOFTMAC_PROBERQ (1<<4)
+
+/* Generate respones to probe requests */
+#define IEEE_SOFTMAC_PROBERS (1<<5)
+
+/* The ieee802.11 stack will manages the netif queue
+ * wake/stop for the driver, taking care of 802.11
+ * fragmentation. See softmac.c for details. */
+#define IEEE_SOFTMAC_TX_QUEUE (1<<7)
+
+/* Uses only the softmac_data_hard_start_xmit
+ * even for TX management frames.
+ */
+#define IEEE_SOFTMAC_SINGLE_QUEUE (1<<8)
+
+/* Generate beacons.  The stack will enqueue beacons
+ * to the card
+ */
+#define IEEE_SOFTMAC_BEACONS (1<<6)
+
+
+
+static inline void *ieee80211_priv(struct net_device *dev)
+{
+	return ((struct ieee80211_device *)netdev_priv(dev))->priv;
+}
+
+extern inline int ieee80211_is_empty_essid(const char *essid, int essid_len)
+{
+	/* Single white space is for Linksys APs */
+	if (essid_len == 1 && essid[0] == ' ')
+		return 1;
+
+	/* Otherwise, if the entire essid is 0, we assume it is hidden */
+	while (essid_len) {
+		essid_len--;
+		if (essid[essid_len] != '\0')
+			return 0;
+	}
+
+	return 1;
+}
+
+extern inline int ieee80211_is_valid_mode(struct ieee80211_device *ieee, int mode)
+{
+	/*
+	 * It is possible for both access points and our device to support
+	 * combinations of modes, so as long as there is one valid combination
+	 * of ap/device supported modes, then return success
+	 *
+	 */
+	if ((mode & IEEE_A) &&
+	    (ieee->modulation & IEEE80211_OFDM_MODULATION) &&
+	    (ieee->freq_band & IEEE80211_52GHZ_BAND))
+		return 1;
+
+	if ((mode & IEEE_G) &&
+	    (ieee->modulation & IEEE80211_OFDM_MODULATION) &&
+	    (ieee->freq_band & IEEE80211_24GHZ_BAND))
+		return 1;
+
+	if ((mode & IEEE_B) &&
+	    (ieee->modulation & IEEE80211_CCK_MODULATION) &&
+	    (ieee->freq_band & IEEE80211_24GHZ_BAND))
+		return 1;
+
+	return 0;
+}
+
+extern inline int ieee80211_get_hdrlen(u16 fc)
+{
+	int hdrlen = 24;
+
+	switch (WLAN_FC_GET_TYPE(fc)) {
+	case IEEE80211_FTYPE_DATA:
+		if ((fc & IEEE80211_FCTL_FROMDS) && (fc & IEEE80211_FCTL_TODS))
+			hdrlen = 30; /* Addr4 */
+		if(IEEE80211_QOS_HAS_SEQ(fc))
+			hdrlen += 2; /* QOS ctrl*/
+		break;
+	case IEEE80211_FTYPE_CTL:
+		switch (WLAN_FC_GET_STYPE(fc)) {
+		case IEEE80211_STYPE_CTS:
+		case IEEE80211_STYPE_ACK:
+			hdrlen = 10;
+			break;
+		default:
+			hdrlen = 16;
+			break;
+		}
+		break;
+	}
+
+	return hdrlen;
+}
+
+
+
+/* ieee80211.c */
+extern void free_ieee80211(struct net_device *dev);
+extern struct net_device *alloc_ieee80211(int sizeof_priv);
+
+extern int ieee80211_set_encryption(struct ieee80211_device *ieee);
+
+/* ieee80211_tx.c */
+
+extern int ieee80211_encrypt_fragment(
+	struct ieee80211_device *ieee,
+	struct sk_buff *frag,
+	int hdr_len);
+
+extern int ieee80211_xmit(struct sk_buff *skb,
+			  struct net_device *dev);
+extern void ieee80211_txb_free(struct ieee80211_txb *);
+
+
+/* ieee80211_rx.c */
+extern int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
+			struct ieee80211_rx_stats *rx_stats);
+extern void ieee80211_rx_mgt(struct ieee80211_device *ieee,
+			     struct ieee80211_hdr *header,
+			     struct ieee80211_rx_stats *stats);
+
+/* ieee80211_wx.c */
+extern int ieee80211_wx_get_scan(struct ieee80211_device *ieee,
+				 struct iw_request_info *info,
+				 union iwreq_data *wrqu, char *key);
+extern int ieee80211_wx_set_encode(struct ieee80211_device *ieee,
+				   struct iw_request_info *info,
+				   union iwreq_data *wrqu, char *key);
+extern int ieee80211_wx_get_encode(struct ieee80211_device *ieee,
+				   struct iw_request_info *info,
+				   union iwreq_data *wrqu, char *key);
+extern int ieee80211_wx_set_encode_ext(struct ieee80211_device *ieee,
+                            struct iw_request_info *info,
+                            union iwreq_data* wrqu, char *extra);
+int ieee80211_wx_set_auth(struct ieee80211_device *ieee,
+                               struct iw_request_info *info,
+                               struct iw_param *data, char *extra);
+int ieee80211_wx_set_mlme(struct ieee80211_device *ieee,
+                               struct iw_request_info *info,
+                               union iwreq_data *wrqu, char *extra);
+
+int ieee80211_wx_set_gen_ie(struct ieee80211_device *ieee, u8 *ie, size_t len);
+/* ieee80211_softmac.c */
+extern short ieee80211_is_54g(struct ieee80211_network net);
+extern short ieee80211_is_shortslot(struct ieee80211_network net);
+extern int ieee80211_rx_frame_softmac(struct ieee80211_device *ieee, struct sk_buff *skb,
+			struct ieee80211_rx_stats *rx_stats, u16 type,
+			u16 stype);
+extern void ieee80211_softmac_new_net(struct ieee80211_device *ieee, struct ieee80211_network *net);
+
+extern void ieee80211_softmac_xmit(struct ieee80211_txb *txb, struct ieee80211_device *ieee);
+extern void ieee80211_softmac_check_all_nets(struct ieee80211_device *ieee);
+extern void ieee80211_start_bss(struct ieee80211_device *ieee);
+extern void ieee80211_start_master_bss(struct ieee80211_device *ieee);
+extern void ieee80211_start_ibss(struct ieee80211_device *ieee);
+extern void ieee80211_softmac_init(struct ieee80211_device *ieee);
+extern void ieee80211_softmac_free(struct ieee80211_device *ieee);
+extern void ieee80211_associate_abort(struct ieee80211_device *ieee);
+extern void ieee80211_disassociate(struct ieee80211_device *ieee);
+extern void ieee80211_stop_scan(struct ieee80211_device *ieee);
+extern void ieee80211_start_scan_syncro(struct ieee80211_device *ieee);
+extern void ieee80211_check_all_nets(struct ieee80211_device *ieee);
+extern void ieee80211_start_protocol(struct ieee80211_device *ieee);
+extern void ieee80211_stop_protocol(struct ieee80211_device *ieee);
+extern void ieee80211_softmac_start_protocol(struct ieee80211_device *ieee);
+extern void ieee80211_softmac_stop_protocol(struct ieee80211_device *ieee);
+extern void ieee80211_reset_queue(struct ieee80211_device *ieee);
+extern void ieee80211_wake_queue(struct ieee80211_device *ieee);
+extern void ieee80211_stop_queue(struct ieee80211_device *ieee);
+extern struct sk_buff *ieee80211_get_beacon(struct ieee80211_device *ieee);
+extern void ieee80211_start_send_beacons(struct ieee80211_device *ieee);
+extern void ieee80211_stop_send_beacons(struct ieee80211_device *ieee);
+extern int ieee80211_wpa_supplicant_ioctl(struct ieee80211_device *ieee, struct iw_point *p);
+extern void notify_wx_assoc_event(struct ieee80211_device *ieee);
+extern void ieee80211_ps_tx_ack(struct ieee80211_device *ieee, short success);
+extern void SendDisassociation(struct ieee80211_device *ieee,u8* asSta,u8 asRsn);
+extern void ieee80211_start_scan(struct ieee80211_device *ieee);
+
+//Add for RF power on power off by lizhaoming 080512
+extern void SendDisassociation(struct ieee80211_device *ieee,
+       			 u8*                     asSta,
+        		 u8                      asRsn);
+
+/* ieee80211_crypt_ccmp&tkip&wep.c */
+extern void ieee80211_tkip_null(void);
+extern void ieee80211_wep_null(void);
+extern void ieee80211_ccmp_null(void);
+/* ieee80211_softmac_wx.c */
+
+extern int ieee80211_wx_get_wap(struct ieee80211_device *ieee,
+			    struct iw_request_info *info,
+			    union iwreq_data *wrqu, char *ext);
+
+extern int ieee80211_wx_set_wap(struct ieee80211_device *ieee,
+			 struct iw_request_info *info,
+			 union iwreq_data *awrq,
+			 char *extra);
+
+extern int ieee80211_wx_get_essid(struct ieee80211_device *ieee, struct iw_request_info *a,union iwreq_data *wrqu,char *b);
+
+extern int ieee80211_wx_set_rate(struct ieee80211_device *ieee,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra);
+
+extern int ieee80211_wx_get_rate(struct ieee80211_device *ieee,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra);
+
+extern int ieee80211_wx_set_mode(struct ieee80211_device *ieee, struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b);
+
+extern int ieee80211_wx_set_scan(struct ieee80211_device *ieee, struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b);
+
+extern int ieee80211_wx_set_essid(struct ieee80211_device *ieee,
+			      struct iw_request_info *a,
+			      union iwreq_data *wrqu, char *extra);
+
+extern int ieee80211_wx_get_mode(struct ieee80211_device *ieee, struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b);
+
+extern int ieee80211_wx_set_freq(struct ieee80211_device *ieee, struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b);
+
+extern int ieee80211_wx_get_freq(struct ieee80211_device *ieee, struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b);
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+extern void ieee80211_wx_sync_scan_wq(struct work_struct *work);
+#else
+ extern void ieee80211_wx_sync_scan_wq(struct ieee80211_device *ieee);
+#endif
+//extern void ieee80211_wx_sync_scan_wq(struct ieee80211_device *ieee);
+
+extern int ieee80211_wx_set_rawtx(struct ieee80211_device *ieee,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra);
+
+extern int ieee80211_wx_get_name(struct ieee80211_device *ieee,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra);
+
+extern int ieee80211_wx_set_power(struct ieee80211_device *ieee,
+				 struct iw_request_info *info,
+				 union iwreq_data *wrqu, char *extra);
+
+extern int ieee80211_wx_get_power(struct ieee80211_device *ieee,
+				 struct iw_request_info *info,
+				 union iwreq_data *wrqu, char *extra);
+
+extern void ieee80211_softmac_ips_scan_syncro(struct ieee80211_device *ieee);
+
+extern void ieee80211_sta_ps_send_null_frame(struct ieee80211_device *ieee, short pwr);
+
+extern const long ieee80211_wlan_frequencies[];
+
+extern inline void ieee80211_increment_scans(struct ieee80211_device *ieee)
+{
+	ieee->scans++;
+}
+
+extern inline int ieee80211_get_scans(struct ieee80211_device *ieee)
+{
+	return ieee->scans;
+}
+
+static inline const char *escape_essid(const char *essid, u8 essid_len) {
+	static char escaped[IW_ESSID_MAX_SIZE * 2 + 1];
+	const char *s = essid;
+	char *d = escaped;
+
+	if (ieee80211_is_empty_essid(essid, essid_len)) {
+		memcpy(escaped, "<hidden>", sizeof("<hidden>"));
+		return escaped;
+	}
+
+	essid_len = min(essid_len, (u8)IW_ESSID_MAX_SIZE);
+	while (essid_len--) {
+		if (*s == '\0') {
+			*d++ = '\\';
+			*d++ = '0';
+			s++;
+		} else {
+			*d++ = *s++;
+		}
+	}
+	*d = '\0';
+	return escaped;
+}
+#endif /* IEEE80211_H */
diff --git a/drivers/staging/rtl8187se/ieee80211/dot11d.c b/drivers/staging/rtl8187se/ieee80211/dot11d.c
new file mode 100644
index 00000000000..5d8b752fc0f
--- /dev/null
+++ b/drivers/staging/rtl8187se/ieee80211/dot11d.c
@@ -0,0 +1,246 @@
+#ifdef ENABLE_DOT11D

+//-----------------------------------------------------------------------------

+//	File:

+//		Dot11d.c

+//

+//	Description:

+//		Implement 802.11d. 

+//

+//-----------------------------------------------------------------------------

+

+#include "dot11d.h"

+

+void

+Dot11d_Init(struct ieee80211_device *ieee)

+{

+	PRT_DOT11D_INFO pDot11dInfo = GET_DOT11D_INFO(ieee);

+

+	pDot11dInfo->bEnabled = 0;

+

+	pDot11dInfo->State = DOT11D_STATE_NONE;

+	pDot11dInfo->CountryIeLen = 0;

+	memset(pDot11dInfo->channel_map, 0, MAX_CHANNEL_NUMBER+1);  

+	memset(pDot11dInfo->MaxTxPwrDbmList, 0xFF, MAX_CHANNEL_NUMBER+1);

+	RESET_CIE_WATCHDOG(ieee);

+

+	printk("Dot11d_Init()\n");

+}

+

+//

+//	Description:

+//		Reset to the state as we are just entering a regulatory domain.

+//

+void

+Dot11d_Reset(struct ieee80211_device *ieee)

+{

+	u32 i;

+	PRT_DOT11D_INFO pDot11dInfo = GET_DOT11D_INFO(ieee);

+

+	// Clear old channel map

+	memset(pDot11dInfo->channel_map, 0, MAX_CHANNEL_NUMBER+1);

+	memset(pDot11dInfo->MaxTxPwrDbmList, 0xFF, MAX_CHANNEL_NUMBER+1);

+	// Set new channel map

+	for (i=1; i<=11; i++) {

+		(pDot11dInfo->channel_map)[i] = 1;

+	}

+	for (i=12; i<=14; i++) {

+		(pDot11dInfo->channel_map)[i] = 2;

+	}

+

+	pDot11dInfo->State = DOT11D_STATE_NONE;

+	pDot11dInfo->CountryIeLen = 0;

+	RESET_CIE_WATCHDOG(ieee);

+

+	//printk("Dot11d_Reset()\n");

+}

+

+//

+//	Description:

+//		Update country IE from Beacon or Probe Resopnse 

+//		and configure PHY for operation in the regulatory domain.

+//

+//	TODO: 

+//		Configure Tx power.

+//

+//	Assumption:

+//		1. IS_DOT11D_ENABLE() is TRUE.

+//		2. Input IE is an valid one.

+//

+void

+Dot11d_UpdateCountryIe(

+	struct ieee80211_device *dev,

+	u8 *		pTaddr,

+	u16	CoutryIeLen,

+	u8 * pCoutryIe	 

+	)

+{

+	PRT_DOT11D_INFO pDot11dInfo = GET_DOT11D_INFO(dev);

+	u8 i, j, NumTriples, MaxChnlNum;

+	PCHNL_TXPOWER_TRIPLE pTriple;

+

+	if((CoutryIeLen - 3)%3 != 0)

+	{

+		printk("Dot11d_UpdateCountryIe(): Invalid country IE, skip it........1\n");

+		Dot11d_Reset(dev);

+		return;

+	}

+

+	memset(pDot11dInfo->channel_map, 0, MAX_CHANNEL_NUMBER+1);

+	memset(pDot11dInfo->MaxTxPwrDbmList, 0xFF, MAX_CHANNEL_NUMBER+1);

+	MaxChnlNum = 0;

+	NumTriples = (CoutryIeLen - 3) / 3; // skip 3-byte country string.

+	pTriple = (PCHNL_TXPOWER_TRIPLE)(pCoutryIe + 3);

+	for(i = 0; i < NumTriples; i++)

+	{

+		if(MaxChnlNum >= pTriple->FirstChnl)

+		{ // It is not in a monotonically increasing order, so stop processing.

+			printk("Dot11d_UpdateCountryIe(): Invalid country IE, skip it........1\n");

+			Dot11d_Reset(dev);

+			return; 

+		}

+		if(MAX_CHANNEL_NUMBER < (pTriple->FirstChnl + pTriple->NumChnls))

+		{ // It is not a valid set of channel id, so stop processing.

+			printk("Dot11d_UpdateCountryIe(): Invalid country IE, skip it........2\n");

+			Dot11d_Reset(dev);

+			return; 

+		}

+

+		for(j = 0 ; j < pTriple->NumChnls; j++)

+		{

+			pDot11dInfo->channel_map[pTriple->FirstChnl + j] = 1;

+			pDot11dInfo->MaxTxPwrDbmList[pTriple->FirstChnl + j] = pTriple->MaxTxPowerInDbm;

+			MaxChnlNum = pTriple->FirstChnl + j;

+		}	

+

+		pTriple = (PCHNL_TXPOWER_TRIPLE)((u8*)pTriple + 3);

+	}

+#if 1

+	//printk("Dot11d_UpdateCountryIe(): Channel List:\n");

+	printk("Channel List:");

+	for(i=1; i<= MAX_CHANNEL_NUMBER; i++)

+		if(pDot11dInfo->channel_map[i] > 0)

+			printk(" %d", i);

+	printk("\n");

+#endif

+

+	UPDATE_CIE_SRC(dev, pTaddr);

+

+	pDot11dInfo->CountryIeLen = CoutryIeLen;

+	memcpy(pDot11dInfo->CountryIeBuf, pCoutryIe,CoutryIeLen);

+	pDot11dInfo->State = DOT11D_STATE_LEARNED;

+}

+

+void dump_chnl_map(u8 * channel_map)

+{

+	int i;

+	printk("Channel List:");

+	for(i=1; i<= MAX_CHANNEL_NUMBER; i++)

+		if(channel_map[i] > 0)

+			printk(" %d(%d)", i, channel_map[i]);

+	printk("\n");

+}

+

+u8

+DOT11D_GetMaxTxPwrInDbm(

+	struct ieee80211_device *dev,

+	u8 Channel

+	)

+{

+	PRT_DOT11D_INFO pDot11dInfo = GET_DOT11D_INFO(dev);

+	u8 MaxTxPwrInDbm = 255;

+

+	if(MAX_CHANNEL_NUMBER < Channel)

+	{ 

+		printk("DOT11D_GetMaxTxPwrInDbm(): Invalid Channel\n");

+		return MaxTxPwrInDbm; 

+	}

+	if(pDot11dInfo->channel_map[Channel])

+	{

+		MaxTxPwrInDbm = pDot11dInfo->MaxTxPwrDbmList[Channel];	

+	}

+

+	return MaxTxPwrInDbm;

+}

+

+

+void

+DOT11D_ScanComplete(

+	struct ieee80211_device * dev

+	)

+{

+	PRT_DOT11D_INFO pDot11dInfo = GET_DOT11D_INFO(dev);

+

+	switch(pDot11dInfo->State)

+	{

+	case DOT11D_STATE_LEARNED:

+		pDot11dInfo->State = DOT11D_STATE_DONE;

+		break;

+

+	case DOT11D_STATE_DONE:

+		if( GET_CIE_WATCHDOG(dev) == 0 )

+		{ // Reset country IE if previous one is gone. 

+			Dot11d_Reset(dev); 

+		}

+		break;

+	case DOT11D_STATE_NONE:

+		break;

+	}

+}

+

+int IsLegalChannel(

+	struct ieee80211_device * dev,

+	u8 channel

+)

+{

+	PRT_DOT11D_INFO pDot11dInfo = GET_DOT11D_INFO(dev);

+

+	if(MAX_CHANNEL_NUMBER < channel)

+	{ 

+		printk("IsLegalChannel(): Invalid Channel\n");

+		return 0; 

+	}

+	if(pDot11dInfo->channel_map[channel] > 0)

+		return 1;

+	return 0;

+}

+

+int ToLegalChannel(

+	struct ieee80211_device * dev,

+	u8 channel

+)

+{

+	PRT_DOT11D_INFO pDot11dInfo = GET_DOT11D_INFO(dev);

+	u8 default_chn = 0;

+	u32 i = 0;

+

+	for (i=1; i<= MAX_CHANNEL_NUMBER; i++)

+	{

+		if(pDot11dInfo->channel_map[i] > 0)

+		{

+			default_chn = i;

+			break;

+		}

+	}

+

+	if(MAX_CHANNEL_NUMBER < channel)

+	{ 

+		printk("IsLegalChannel(): Invalid Channel\n");

+		return default_chn; 

+	}

+	

+	if(pDot11dInfo->channel_map[channel] > 0)

+		return channel;

+	

+	return default_chn;

+}

+

+#if 0

+EXPORT_SYMBOL(Dot11d_Init);

+EXPORT_SYMBOL(Dot11d_Reset);

+EXPORT_SYMBOL(Dot11d_UpdateCountryIe);

+EXPORT_SYMBOL(DOT11D_GetMaxTxPwrInDbm);

+EXPORT_SYMBOL(DOT11D_ScanComplete);

+EXPORT_SYMBOL(IsLegalChannel);

+EXPORT_SYMBOL(ToLegalChannel);

+#endif

+#endif

diff --git a/drivers/staging/rtl8187se/ieee80211/dot11d.h b/drivers/staging/rtl8187se/ieee80211/dot11d.h
new file mode 100644
index 00000000000..64bcf15bd21
--- /dev/null
+++ b/drivers/staging/rtl8187se/ieee80211/dot11d.h
@@ -0,0 +1,102 @@
+#ifndef __INC_DOT11D_H

+#define __INC_DOT11D_H

+

+#include "ieee80211.h"

+

+//#define ENABLE_DOT11D

+

+//#define DOT11D_MAX_CHNL_NUM 83

+

+typedef struct _CHNL_TXPOWER_TRIPLE {

+	u8 FirstChnl;

+	u8  NumChnls;

+	u8  MaxTxPowerInDbm;

+}CHNL_TXPOWER_TRIPLE, *PCHNL_TXPOWER_TRIPLE;

+

+typedef enum _DOT11D_STATE {

+	DOT11D_STATE_NONE = 0,

+	DOT11D_STATE_LEARNED,

+	DOT11D_STATE_DONE,

+}DOT11D_STATE;

+

+typedef struct _RT_DOT11D_INFO {

+	//DECLARE_RT_OBJECT(RT_DOT11D_INFO);

+

+	bool bEnabled; // dot11MultiDomainCapabilityEnabled

+

+	u16 CountryIeLen; // > 0 if CountryIeBuf[] contains valid country information element.

+	u8  CountryIeBuf[MAX_IE_LEN];

+	u8  CountryIeSrcAddr[6]; // Source AP of the country IE.

+	u8  CountryIeWatchdog; 

+

+	u8  channel_map[MAX_CHANNEL_NUMBER+1];  //!!!Value 0: Invalid, 1: Valid (active scan), 2: Valid (passive scan)

+	//u8  ChnlListLen; // #Bytes valid in ChnlList[].

+	//u8  ChnlList[DOT11D_MAX_CHNL_NUM];

+	u8  MaxTxPwrDbmList[MAX_CHANNEL_NUMBER+1];

+

+	DOT11D_STATE State;

+}RT_DOT11D_INFO, *PRT_DOT11D_INFO;

+#define eqMacAddr(a,b)		( ((a)[0]==(b)[0] && (a)[1]==(b)[1] && (a)[2]==(b)[2] && (a)[3]==(b)[3] && (a)[4]==(b)[4] && (a)[5]==(b)[5]) ? 1:0 )

+#define cpMacAddr(des,src)	      ((des)[0]=(src)[0],(des)[1]=(src)[1],(des)[2]=(src)[2],(des)[3]=(src)[3],(des)[4]=(src)[4],(des)[5]=(src)[5])

+#define GET_DOT11D_INFO(__pIeeeDev) ((PRT_DOT11D_INFO)((__pIeeeDev)->pDot11dInfo))

+

+#define IS_DOT11D_ENABLE(__pIeeeDev) GET_DOT11D_INFO(__pIeeeDev)->bEnabled

+#define IS_COUNTRY_IE_VALID(__pIeeeDev) (GET_DOT11D_INFO(__pIeeeDev)->CountryIeLen > 0)

+

+#define IS_EQUAL_CIE_SRC(__pIeeeDev, __pTa) eqMacAddr(GET_DOT11D_INFO(__pIeeeDev)->CountryIeSrcAddr, __pTa) 

+#define UPDATE_CIE_SRC(__pIeeeDev, __pTa) cpMacAddr(GET_DOT11D_INFO(__pIeeeDev)->CountryIeSrcAddr, __pTa)

+

+#define IS_COUNTRY_IE_CHANGED(__pIeeeDev, __Ie) \

+	(((__Ie).Length == 0 || (__Ie).Length != GET_DOT11D_INFO(__pIeeeDev)->CountryIeLen) ? \

+	FALSE : \

+	(!memcmp(GET_DOT11D_INFO(__pIeeeDev)->CountryIeBuf, (__Ie).Octet, (__Ie).Length)))

+

+#define CIE_WATCHDOG_TH 1

+#define GET_CIE_WATCHDOG(__pIeeeDev) GET_DOT11D_INFO(__pIeeeDev)->CountryIeWatchdog

+#define RESET_CIE_WATCHDOG(__pIeeeDev) GET_CIE_WATCHDOG(__pIeeeDev) = 0 

+#define UPDATE_CIE_WATCHDOG(__pIeeeDev) ++GET_CIE_WATCHDOG(__pIeeeDev)

+

+#define IS_DOT11D_STATE_DONE(__pIeeeDev) (GET_DOT11D_INFO(__pIeeeDev)->State == DOT11D_STATE_DONE)

+

+

+void

+Dot11d_Init(

+	struct ieee80211_device *dev

+	);

+

+void

+Dot11d_Reset(

+	struct ieee80211_device *dev

+	);

+

+void

+Dot11d_UpdateCountryIe(

+	struct ieee80211_device *dev,

+	u8 *		pTaddr,

+	u16	CoutryIeLen,

+	u8 * pCoutryIe	 

+	);

+

+u8

+DOT11D_GetMaxTxPwrInDbm(

+	struct ieee80211_device *dev,

+	u8 Channel

+	);

+

+void

+DOT11D_ScanComplete(

+	struct ieee80211_device * dev

+	);

+

+int IsLegalChannel(

+	struct ieee80211_device * dev,

+	u8 channel

+);

+

+int ToLegalChannel(

+	struct ieee80211_device * dev,

+	u8 channel

+);

+

+void dump_chnl_map(u8 * channel_map);

+#endif // #ifndef __INC_DOT11D_H

diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211.h b/drivers/staging/rtl8187se/ieee80211/ieee80211.h
new file mode 100644
index 00000000000..bf06abeeaeb
--- /dev/null
+++ b/drivers/staging/rtl8187se/ieee80211/ieee80211.h
@@ -0,0 +1,1755 @@
+/*
+ * Merged with mainline ieee80211.h in Aug 2004.  Original ieee802_11
+ * remains copyright by the original authors
+ *
+ * Portions of the merged code are based on Host AP (software wireless
+ * LAN access point) driver for Intersil Prism2/2.5/3.
+ *
+ * Copyright (c) 2001-2002, SSH Communications Security Corp and Jouni Malinen
+ * <jkmaline@cc.hut.fi>
+ * Copyright (c) 2002-2003, Jouni Malinen <jkmaline@cc.hut.fi>
+ *
+ * Adaption to a generic IEEE 802.11 stack by James Ketrenos
+ * <jketreno@linux.intel.com>
+ * Copyright (c) 2004, Intel Corporation
+ *
+ * Modified for Realtek's wi-fi cards by Andrea Merello
+ * <andreamrl@tiscali.it>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation. See README and COPYING for
+ * more details.
+ */
+#ifndef IEEE80211_H
+#define IEEE80211_H
+#include <linux/if_ether.h> /* ETH_ALEN */
+#include <linux/kernel.h>   /* ARRAY_SIZE */
+#include <linux/version.h>
+#include <linux/jiffies.h>
+#include <linux/timer.h>
+#include <linux/sched.h>
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,13))
+#include <linux/wireless.h>
+#endif
+
+/*
+#ifndef bool
+#define bool int
+#endif
+
+#ifndef true
+#define true   1
+#endif
+
+#ifndef false
+#define false  0
+#endif
+*/
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20))
+#ifndef bool
+typedef enum{false = 0, true} bool;
+#endif
+#endif
+//#ifdef JOHN_HWSEC
+#define KEY_TYPE_NA		0x0
+#define KEY_TYPE_WEP40 		0x1
+#define KEY_TYPE_TKIP		0x2
+#define KEY_TYPE_CCMP		0x4
+#define KEY_TYPE_WEP104		0x5
+//#endif
+
+
+#define aSifsTime					10
+
+#define MGMT_QUEUE_NUM 5
+
+
+#define IEEE_CMD_SET_WPA_PARAM			1
+#define	IEEE_CMD_SET_WPA_IE			2
+#define IEEE_CMD_SET_ENCRYPTION			3
+#define IEEE_CMD_MLME				4
+
+#define IEEE_PARAM_WPA_ENABLED			1
+#define IEEE_PARAM_TKIP_COUNTERMEASURES		2
+#define IEEE_PARAM_DROP_UNENCRYPTED		3
+#define IEEE_PARAM_PRIVACY_INVOKED		4
+#define IEEE_PARAM_AUTH_ALGS			5
+#define IEEE_PARAM_IEEE_802_1X			6
+//It should consistent with the driver_XXX.c
+//   David, 2006.9.26
+#define IEEE_PARAM_WPAX_SELECT			7
+//Added for notify the encryption type selection
+//   David, 2006.9.26
+#define IEEE_PROTO_WPA				1
+#define IEEE_PROTO_RSN				2
+//Added for notify the encryption type selection
+//   David, 2006.9.26
+#define IEEE_WPAX_USEGROUP			0
+#define IEEE_WPAX_WEP40				1
+#define IEEE_WPAX_TKIP				2
+#define IEEE_WPAX_WRAP   			3
+#define IEEE_WPAX_CCMP				4
+#define IEEE_WPAX_WEP104			5
+
+#define IEEE_KEY_MGMT_IEEE8021X			1
+#define IEEE_KEY_MGMT_PSK			2
+
+
+
+#define IEEE_MLME_STA_DEAUTH			1
+#define IEEE_MLME_STA_DISASSOC			2
+
+
+#define IEEE_CRYPT_ERR_UNKNOWN_ALG		2
+#define IEEE_CRYPT_ERR_UNKNOWN_ADDR		3
+#define IEEE_CRYPT_ERR_CRYPT_INIT_FAILED		4
+#define IEEE_CRYPT_ERR_KEY_SET_FAILED		5
+#define IEEE_CRYPT_ERR_TX_KEY_SET_FAILED		6
+#define IEEE_CRYPT_ERR_CARD_CONF_FAILED		7
+
+
+#define	IEEE_CRYPT_ALG_NAME_LEN			16
+
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,6,10))
+#define ieee80211_wx_get_scan ieee80211_wx_get_scan_rtl
+#define ieee80211_wx_set_encode ieee80211_wx_set_encode_rtl
+#define ieee80211_wx_get_encode ieee80211_wx_get_encode_rtl
+////////////////////////////////
+// added for kernel conflict under FC5
+#define ieee80211_wx_get_name   ieee80211_wx_get_name_rtl
+#define free_ieee80211          free_ieee80211_rtl
+#define alloc_ieee80211        alloc_ieee80211_rtl
+///////////////////////////////
+#endif
+//error in ubuntu2.6.22,so add these
+#define ieee80211_wake_queue ieee80211_wake_queue_rtl
+#define ieee80211_stop_queue ieee80211_stop_queue_rtl
+
+#define ieee80211_rx ieee80211_rx_rtl
+
+#define ieee80211_register_crypto_ops	ieee80211_register_crypto_ops_rtl
+#define ieee80211_unregister_crypto_ops	ieee80211_unregister_crypto_ops_rtl
+#define ieee80211_get_crypto_ops	ieee80211_get_crypto_ops_rtl
+#define ieee80211_crypt_deinit_entries	ieee80211_crypt_deinit_entries_rtl
+#define ieee80211_crypt_deinit_handler	ieee80211_crypt_deinit_handler_rtl
+#define ieee80211_crypt_delayed_deinit	ieee80211_crypt_delayed_deinit_rtl
+
+#define ieee80211_txb_free	ieee80211_txb_free_rtl
+#define ieee80211_wx_get_essid	ieee80211_wx_get_essid_rtl
+#define ieee80211_wx_set_essid	ieee80211_wx_set_essid_rtl
+#define ieee80211_wx_set_rate	ieee80211_wx_set_rate_rtl
+#define ieee80211_wx_get_rate	ieee80211_wx_get_rate_rtl
+#define ieee80211_wx_set_wap	ieee80211_wx_set_wap_rtl
+#define ieee80211_wx_get_wap	ieee80211_wx_get_wap_rtl
+#define ieee80211_wx_set_mode	ieee80211_wx_set_mode_rtl
+#define ieee80211_wx_get_mode	ieee80211_wx_get_mode_rtl
+#define ieee80211_wx_set_scan	ieee80211_wx_set_scan_rtl
+#define ieee80211_wx_get_freq	ieee80211_wx_get_freq_rtl
+#define ieee80211_wx_set_freq	ieee80211_wx_set_freq_rtl
+#define ieee80211_wx_set_rawtx	ieee80211_wx_set_rawtx_rtl
+#define ieee80211_wx_set_power	ieee80211_wx_set_power_rtl
+#define ieee80211_wx_get_power	ieee80211_wx_get_power_rtl
+#define ieee80211_wlan_frequencies	ieee80211_wlan_frequencies_rtl
+#define ieee80211_softmac_stop_protocol	ieee80211_softmac_stop_protocol_rtl
+#define ieee80211_softmac_start_protocol ieee80211_softmac_start_protocol_rtl
+#define	ieee80211_start_protocol	ieee80211_start_protocol_rtl
+#define	ieee80211_stop_protocol		ieee80211_stop_protocol_rtl
+#define	ieee80211_rx_mgt		ieee80211_rx_mgt_rtl
+
+#define ieee80211_wx_set_auth ieee80211_wx_set_auth_rtl
+//by amy for ps
+#define notify_wx_assoc_event  notify_wx_assoc_event_rtl
+#define ieee80211_stop_send_beacons ieee80211_stop_send_beacons_rtl
+#define ieee80211_disassociate ieee80211_disassociate_rtl
+#define ieee80211_start_scan ieee80211_start_scan_rtl
+//by amy for ps
+typedef struct ieee_param {
+	u32 cmd;
+	u8 sta_addr[ETH_ALEN];
+        union {
+		struct {
+			u8 name;
+			u32 value;
+		} wpa_param;
+		struct {
+			u32 len;
+			u8 reserved[32];
+			u8 data[0];
+		} wpa_ie;
+	        struct{
+			int command;
+    			int reason_code;
+		} mlme;
+		struct {
+			u8 alg[IEEE_CRYPT_ALG_NAME_LEN];
+			u8 set_tx;
+			u32 err;
+			u8 idx;
+			u8 seq[8]; /* sequence counter (set: RX, get: TX) */
+			u16 key_len;
+			u8 key[0];
+		} crypt;
+
+	} u;
+}ieee_param;
+
+
+#if WIRELESS_EXT < 17
+#define IW_QUAL_QUAL_INVALID   0x10
+#define IW_QUAL_LEVEL_INVALID  0x20
+#define IW_QUAL_NOISE_INVALID  0x40
+#define IW_QUAL_QUAL_UPDATED   0x1
+#define IW_QUAL_LEVEL_UPDATED  0x2
+#define IW_QUAL_NOISE_UPDATED  0x4
+#endif
+
+// linux under 2.6.9 release may not support it, so modify it for common use
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,9))
+#define MSECS(t)	(1000 * ((t) / HZ) + 1000 * ((t) % HZ) / HZ)
+static inline unsigned long msleep_interruptible_rtl(unsigned int msecs)
+{
+         unsigned long timeout = MSECS(msecs) + 1;
+
+         while (timeout) {
+                 set_current_state(TASK_UNINTERRUPTIBLE);
+                 timeout = schedule_timeout(timeout);
+         }
+         return timeout;
+}
+#else
+#define MSECS(t) msecs_to_jiffies(t)
+#define msleep_interruptible_rtl  msleep_interruptible
+#endif
+
+#define IEEE80211_DATA_LEN		2304
+/* Maximum size for the MA-UNITDATA primitive, 802.11 standard section
+   6.2.1.1.2.
+
+   The figure in section 7.1.2 suggests a body size of up to 2312
+   bytes is allowed, which is a bit confusing, I suspect this
+   represents the 2304 bytes of real data, plus a possible 8 bytes of
+   WEP IV and ICV. (this interpretation suggested by Ramiro Barreiro) */
+
+
+#define IEEE80211_HLEN			30
+#define IEEE80211_FRAME_LEN		(IEEE80211_DATA_LEN + IEEE80211_HLEN)
+
+/* this is stolen and modified from the madwifi driver*/
+#define IEEE80211_FC0_TYPE_MASK		0x0c
+#define IEEE80211_FC0_TYPE_DATA		0x08
+#define IEEE80211_FC0_SUBTYPE_MASK	0xB0
+#define IEEE80211_FC0_SUBTYPE_QOS	0x80
+
+#define IEEE80211_QOS_HAS_SEQ(fc) \
+	(((fc) & (IEEE80211_FC0_TYPE_MASK | IEEE80211_FC0_SUBTYPE_MASK)) == \
+	 (IEEE80211_FC0_TYPE_DATA | IEEE80211_FC0_SUBTYPE_QOS))
+
+/* this is stolen from ipw2200 driver */
+#define IEEE_IBSS_MAC_HASH_SIZE 31
+struct ieee_ibss_seq {
+	u8 mac[ETH_ALEN];
+	u16 seq_num[17];
+	u16 frag_num[17];
+	unsigned long packet_time[17];
+	struct list_head list;
+};
+
+struct ieee80211_hdr {
+	u16 frame_ctl;
+	u16 duration_id;
+	u8 addr1[ETH_ALEN];
+	u8 addr2[ETH_ALEN];
+	u8 addr3[ETH_ALEN];
+	u16 seq_ctl;
+	u8 addr4[ETH_ALEN];
+} __attribute__ ((packed));
+
+struct ieee80211_hdr_QOS {
+	u16 frame_ctl;
+	u16 duration_id;
+	u8 addr1[ETH_ALEN];
+	u8 addr2[ETH_ALEN];
+	u8 addr3[ETH_ALEN];
+	u16 seq_ctl;
+	u8 addr4[ETH_ALEN];
+	u16 QOS_ctl;
+} __attribute__ ((packed));
+
+struct ieee80211_hdr_3addr {
+	u16 frame_ctl;
+	u16 duration_id;
+	u8 addr1[ETH_ALEN];
+	u8 addr2[ETH_ALEN];
+	u8 addr3[ETH_ALEN];
+	u16 seq_ctl;
+} __attribute__ ((packed));
+
+struct ieee80211_hdr_3addr_QOS {
+	u16 frame_ctl;
+	u16 duration_id;
+	u8 addr1[ETH_ALEN];
+	u8 addr2[ETH_ALEN];
+	u8 addr3[ETH_ALEN];
+	u16 seq_ctl;
+	u16 QOS_ctl;
+} __attribute__ ((packed));
+
+enum eap_type {
+	EAP_PACKET = 0,
+	EAPOL_START,
+	EAPOL_LOGOFF,
+	EAPOL_KEY,
+	EAPOL_ENCAP_ASF_ALERT
+};
+
+static const char *eap_types[] = {
+	[EAP_PACKET]		= "EAP-Packet",
+	[EAPOL_START]		= "EAPOL-Start",
+	[EAPOL_LOGOFF]		= "EAPOL-Logoff",
+	[EAPOL_KEY]		= "EAPOL-Key",
+	[EAPOL_ENCAP_ASF_ALERT]	= "EAPOL-Encap-ASF-Alert"
+};
+
+static inline const char *eap_get_type(int type)
+{
+	return (type >= ARRAY_SIZE(eap_types)) ? "Unknown" : eap_types[type];
+}
+
+struct eapol {
+	u8 snap[6];
+	u16 ethertype;
+	u8 version;
+	u8 type;
+	u16 length;
+} __attribute__ ((packed));
+
+#define IEEE80211_3ADDR_LEN 24
+#define IEEE80211_4ADDR_LEN 30
+#define IEEE80211_FCS_LEN    4
+
+#define MIN_FRAG_THRESHOLD     256U
+#define	MAX_FRAG_THRESHOLD     2346U
+
+/* Frame control field constants */
+#define IEEE80211_FCTL_VERS		0x0002
+#define IEEE80211_FCTL_FTYPE		0x000c
+#define IEEE80211_FCTL_STYPE		0x00f0
+#define IEEE80211_FCTL_TODS		0x0100
+#define IEEE80211_FCTL_FROMDS		0x0200
+#define IEEE80211_FCTL_DSTODS		0x0300 //added by david
+#define IEEE80211_FCTL_MOREFRAGS	0x0400
+#define IEEE80211_FCTL_RETRY		0x0800
+#define IEEE80211_FCTL_PM		0x1000
+#define IEEE80211_FCTL_MOREDATA	0x2000
+#define IEEE80211_FCTL_WEP		0x4000
+#define IEEE80211_FCTL_ORDER		0x8000
+
+#define IEEE80211_FTYPE_MGMT		0x0000
+#define IEEE80211_FTYPE_CTL		0x0004
+#define IEEE80211_FTYPE_DATA		0x0008
+
+/* management */
+#define IEEE80211_STYPE_ASSOC_REQ	0x0000
+#define IEEE80211_STYPE_ASSOC_RESP 	0x0010
+#define IEEE80211_STYPE_REASSOC_REQ	0x0020
+#define IEEE80211_STYPE_REASSOC_RESP	0x0030
+#define IEEE80211_STYPE_PROBE_REQ	0x0040
+#define IEEE80211_STYPE_PROBE_RESP	0x0050
+#define IEEE80211_STYPE_BEACON		0x0080
+#define IEEE80211_STYPE_ATIM		0x0090
+#define IEEE80211_STYPE_DISASSOC	0x00A0
+#define IEEE80211_STYPE_AUTH		0x00B0
+#define IEEE80211_STYPE_DEAUTH		0x00C0
+#define IEEE80211_STYPE_MANAGE_ACT	0x00D0
+
+/* control */
+#define IEEE80211_STYPE_PSPOLL		0x00A0
+#define IEEE80211_STYPE_RTS		0x00B0
+#define IEEE80211_STYPE_CTS		0x00C0
+#define IEEE80211_STYPE_ACK		0x00D0
+#define IEEE80211_STYPE_CFEND		0x00E0
+#define IEEE80211_STYPE_CFENDACK	0x00F0
+
+/* data */
+#define IEEE80211_STYPE_DATA		0x0000
+#define IEEE80211_STYPE_DATA_CFACK	0x0010
+#define IEEE80211_STYPE_DATA_CFPOLL	0x0020
+#define IEEE80211_STYPE_DATA_CFACKPOLL	0x0030
+#define IEEE80211_STYPE_NULLFUNC	0x0040
+#define IEEE80211_STYPE_CFACK		0x0050
+#define IEEE80211_STYPE_CFPOLL		0x0060
+#define IEEE80211_STYPE_CFACKPOLL	0x0070
+#define IEEE80211_STYPE_QOS_DATA	0x0080 //added for WMM 2006/8/2
+#define IEEE80211_STYPE_QOS_NULL	0x00C0
+
+
+#define IEEE80211_SCTL_FRAG		0x000F
+#define IEEE80211_SCTL_SEQ		0xFFF0
+
+
+/* debug macros */
+
+#ifdef CONFIG_IEEE80211_DEBUG
+extern u32 ieee80211_debug_level;
+#define IEEE80211_DEBUG(level, fmt, args...) \
+do { if (ieee80211_debug_level & (level)) \
+  printk(KERN_DEBUG "ieee80211: %c %s " fmt, \
+         in_interrupt() ? 'I' : 'U', __FUNCTION__ , ## args); } while (0)
+#else
+#define IEEE80211_DEBUG(level, fmt, args...) do {} while (0)
+#endif	/* CONFIG_IEEE80211_DEBUG */
+
+/*
+ * To use the debug system;
+ *
+ * If you are defining a new debug classification, simply add it to the #define
+ * list here in the form of:
+ *
+ * #define IEEE80211_DL_xxxx VALUE
+ *
+ * shifting value to the left one bit from the previous entry.  xxxx should be
+ * the name of the classification (for example, WEP)
+ *
+ * You then need to either add a IEEE80211_xxxx_DEBUG() macro definition for your
+ * classification, or use IEEE80211_DEBUG(IEEE80211_DL_xxxx, ...) whenever you want
+ * to send output to that classification.
+ *
+ * To add your debug level to the list of levels seen when you perform
+ *
+ * % cat /proc/net/ipw/debug_level
+ *
+ * you simply need to add your entry to the ipw_debug_levels array.
+ *
+ * If you do not see debug_level in /proc/net/ipw then you do not have
+ * CONFIG_IEEE80211_DEBUG defined in your kernel configuration
+ *
+ */
+
+#define IEEE80211_DL_INFO          (1<<0)
+#define IEEE80211_DL_WX            (1<<1)
+#define IEEE80211_DL_SCAN          (1<<2)
+#define IEEE80211_DL_STATE         (1<<3)
+#define IEEE80211_DL_MGMT          (1<<4)
+#define IEEE80211_DL_FRAG          (1<<5)
+#define IEEE80211_DL_EAP           (1<<6)
+#define IEEE80211_DL_DROP          (1<<7)
+
+#define IEEE80211_DL_TX            (1<<8)
+#define IEEE80211_DL_RX            (1<<9)
+
+#define IEEE80211_ERROR(f, a...) printk(KERN_ERR "ieee80211: " f, ## a)
+#define IEEE80211_WARNING(f, a...) printk(KERN_WARNING "ieee80211: " f, ## a)
+#define IEEE80211_DEBUG_INFO(f, a...)   IEEE80211_DEBUG(IEEE80211_DL_INFO, f, ## a)
+
+#define IEEE80211_DEBUG_WX(f, a...)     IEEE80211_DEBUG(IEEE80211_DL_WX, f, ## a)
+#define IEEE80211_DEBUG_SCAN(f, a...)   IEEE80211_DEBUG(IEEE80211_DL_SCAN, f, ## a)
+//#define IEEE_DEBUG_SCAN  IEEE80211_WARNING
+#define IEEE80211_DEBUG_STATE(f, a...)  IEEE80211_DEBUG(IEEE80211_DL_STATE, f, ## a)
+#define IEEE80211_DEBUG_MGMT(f, a...)  IEEE80211_DEBUG(IEEE80211_DL_MGMT, f, ## a)
+#define IEEE80211_DEBUG_FRAG(f, a...)  IEEE80211_DEBUG(IEEE80211_DL_FRAG, f, ## a)
+#define IEEE80211_DEBUG_EAP(f, a...)  IEEE80211_DEBUG(IEEE80211_DL_EAP, f, ## a)
+#define IEEE80211_DEBUG_DROP(f, a...)  IEEE80211_DEBUG(IEEE80211_DL_DROP, f, ## a)
+#define IEEE80211_DEBUG_TX(f, a...)  IEEE80211_DEBUG(IEEE80211_DL_TX, f, ## a)
+#define IEEE80211_DEBUG_RX(f, a...)  IEEE80211_DEBUG(IEEE80211_DL_RX, f, ## a)
+#include <linux/netdevice.h>
+#include <linux/wireless.h>
+#include <linux/if_arp.h> /* ARPHRD_ETHER */
+
+#ifndef WIRELESS_SPY
+#define WIRELESS_SPY		// enable iwspy support
+#endif
+#include <net/iw_handler.h>	// new driver API
+
+#ifndef ETH_P_PAE
+#define ETH_P_PAE 0x888E /* Port Access Entity (IEEE 802.1X) */
+#endif /* ETH_P_PAE */
+
+#define ETH_P_PREAUTH 0x88C7 /* IEEE 802.11i pre-authentication */
+
+#ifndef ETH_P_80211_RAW
+#define ETH_P_80211_RAW (ETH_P_ECONET + 1)
+#endif
+
+/* IEEE 802.11 defines */
+
+#define P80211_OUI_LEN 3
+
+struct ieee80211_snap_hdr {
+
+        u8    dsap;   /* always 0xAA */
+        u8    ssap;   /* always 0xAA */
+        u8    ctrl;   /* always 0x03 */
+        u8    oui[P80211_OUI_LEN];    /* organizational universal id */
+
+} __attribute__ ((packed));
+
+#define SNAP_SIZE sizeof(struct ieee80211_snap_hdr)
+
+#define WLAN_FC_GET_TYPE(fc) ((fc) & IEEE80211_FCTL_FTYPE)
+#define WLAN_FC_GET_STYPE(fc) ((fc) & IEEE80211_FCTL_STYPE)
+
+#define WLAN_GET_SEQ_FRAG(seq) ((seq) & IEEE80211_SCTL_FRAG)
+#define WLAN_GET_SEQ_SEQ(seq)  ((seq) & IEEE80211_SCTL_SEQ)
+
+/* Authentication algorithms */
+#define WLAN_AUTH_OPEN 0
+#define WLAN_AUTH_SHARED_KEY 1
+
+#define WLAN_AUTH_CHALLENGE_LEN 128
+
+#define WLAN_CAPABILITY_BSS (1<<0)
+#define WLAN_CAPABILITY_IBSS (1<<1)
+#define WLAN_CAPABILITY_CF_POLLABLE (1<<2)
+#define WLAN_CAPABILITY_CF_POLL_REQUEST (1<<3)
+#define WLAN_CAPABILITY_PRIVACY (1<<4)
+#define WLAN_CAPABILITY_SHORT_PREAMBLE (1<<5)
+#define WLAN_CAPABILITY_PBCC (1<<6)
+#define WLAN_CAPABILITY_CHANNEL_AGILITY (1<<7)
+#define WLAN_CAPABILITY_SHORT_SLOT (1<<10)
+
+/* Status codes */
+#define WLAN_STATUS_SUCCESS 0
+#define WLAN_STATUS_UNSPECIFIED_FAILURE 1
+#define WLAN_STATUS_CAPS_UNSUPPORTED 10
+#define WLAN_STATUS_REASSOC_NO_ASSOC 11
+#define WLAN_STATUS_ASSOC_DENIED_UNSPEC 12
+#define WLAN_STATUS_NOT_SUPPORTED_AUTH_ALG 13
+#define WLAN_STATUS_UNKNOWN_AUTH_TRANSACTION 14
+#define WLAN_STATUS_CHALLENGE_FAIL 15
+#define WLAN_STATUS_AUTH_TIMEOUT 16
+#define WLAN_STATUS_AP_UNABLE_TO_HANDLE_NEW_STA 17
+#define WLAN_STATUS_ASSOC_DENIED_RATES 18
+/* 802.11b */
+#define WLAN_STATUS_ASSOC_DENIED_NOSHORT 19
+#define WLAN_STATUS_ASSOC_DENIED_NOPBCC 20
+#define WLAN_STATUS_ASSOC_DENIED_NOAGILITY 21
+
+/* Reason codes */
+#define WLAN_REASON_UNSPECIFIED 1
+#define WLAN_REASON_PREV_AUTH_NOT_VALID 2
+#define WLAN_REASON_DEAUTH_LEAVING 3
+#define WLAN_REASON_DISASSOC_DUE_TO_INACTIVITY 4
+#define WLAN_REASON_DISASSOC_AP_BUSY 5
+#define WLAN_REASON_CLASS2_FRAME_FROM_NONAUTH_STA 6
+#define WLAN_REASON_CLASS3_FRAME_FROM_NONASSOC_STA 7
+#define WLAN_REASON_DISASSOC_STA_HAS_LEFT 8
+#define WLAN_REASON_STA_REQ_ASSOC_WITHOUT_AUTH 9
+
+
+/* Information Element IDs */
+#define WLAN_EID_SSID 0
+#define WLAN_EID_SUPP_RATES 1
+#define WLAN_EID_FH_PARAMS 2
+#define WLAN_EID_DS_PARAMS 3
+#define WLAN_EID_CF_PARAMS 4
+#define WLAN_EID_TIM 5
+#define WLAN_EID_IBSS_PARAMS 6
+#define WLAN_EID_CHALLENGE 16
+#define WLAN_EID_RSN 48
+#define WLAN_EID_GENERIC 221
+
+#define IEEE80211_MGMT_HDR_LEN 24
+#define IEEE80211_DATA_HDR3_LEN 24
+#define IEEE80211_DATA_HDR4_LEN 30
+
+
+#define IEEE80211_STATMASK_SIGNAL (1<<0)
+#define IEEE80211_STATMASK_RSSI (1<<1)
+#define IEEE80211_STATMASK_NOISE (1<<2)
+#define IEEE80211_STATMASK_RATE (1<<3)
+#define IEEE80211_STATMASK_WEMASK 0x7
+
+
+#define IEEE80211_CCK_MODULATION    (1<<0)
+#define IEEE80211_OFDM_MODULATION   (1<<1)
+
+#define IEEE80211_24GHZ_BAND     (1<<0)
+#define IEEE80211_52GHZ_BAND     (1<<1)
+
+#define IEEE80211_CCK_RATE_LEN  		4
+#define IEEE80211_CCK_RATE_1MB		        0x02
+#define IEEE80211_CCK_RATE_2MB		        0x04
+#define IEEE80211_CCK_RATE_5MB		        0x0B
+#define IEEE80211_CCK_RATE_11MB		        0x16
+#define IEEE80211_OFDM_RATE_LEN 		8
+#define IEEE80211_OFDM_RATE_6MB		        0x0C
+#define IEEE80211_OFDM_RATE_9MB		        0x12
+#define IEEE80211_OFDM_RATE_12MB		0x18
+#define IEEE80211_OFDM_RATE_18MB		0x24
+#define IEEE80211_OFDM_RATE_24MB		0x30
+#define IEEE80211_OFDM_RATE_36MB		0x48
+#define IEEE80211_OFDM_RATE_48MB		0x60
+#define IEEE80211_OFDM_RATE_54MB		0x6C
+#define IEEE80211_BASIC_RATE_MASK		0x80
+
+#define IEEE80211_CCK_RATE_1MB_MASK		(1<<0)
+#define IEEE80211_CCK_RATE_2MB_MASK		(1<<1)
+#define IEEE80211_CCK_RATE_5MB_MASK		(1<<2)
+#define IEEE80211_CCK_RATE_11MB_MASK		(1<<3)
+#define IEEE80211_OFDM_RATE_6MB_MASK		(1<<4)
+#define IEEE80211_OFDM_RATE_9MB_MASK		(1<<5)
+#define IEEE80211_OFDM_RATE_12MB_MASK		(1<<6)
+#define IEEE80211_OFDM_RATE_18MB_MASK		(1<<7)
+#define IEEE80211_OFDM_RATE_24MB_MASK		(1<<8)
+#define IEEE80211_OFDM_RATE_36MB_MASK		(1<<9)
+#define IEEE80211_OFDM_RATE_48MB_MASK		(1<<10)
+#define IEEE80211_OFDM_RATE_54MB_MASK		(1<<11)
+
+#define IEEE80211_CCK_RATES_MASK	        0x0000000F
+#define IEEE80211_CCK_BASIC_RATES_MASK	(IEEE80211_CCK_RATE_1MB_MASK | \
+	IEEE80211_CCK_RATE_2MB_MASK)
+#define IEEE80211_CCK_DEFAULT_RATES_MASK	(IEEE80211_CCK_BASIC_RATES_MASK | \
+        IEEE80211_CCK_RATE_5MB_MASK | \
+        IEEE80211_CCK_RATE_11MB_MASK)
+
+#define IEEE80211_OFDM_RATES_MASK		0x00000FF0
+#define IEEE80211_OFDM_BASIC_RATES_MASK	(IEEE80211_OFDM_RATE_6MB_MASK | \
+	IEEE80211_OFDM_RATE_12MB_MASK | \
+	IEEE80211_OFDM_RATE_24MB_MASK)
+#define IEEE80211_OFDM_DEFAULT_RATES_MASK	(IEEE80211_OFDM_BASIC_RATES_MASK | \
+	IEEE80211_OFDM_RATE_9MB_MASK  | \
+	IEEE80211_OFDM_RATE_18MB_MASK | \
+	IEEE80211_OFDM_RATE_36MB_MASK | \
+	IEEE80211_OFDM_RATE_48MB_MASK | \
+	IEEE80211_OFDM_RATE_54MB_MASK)
+#define IEEE80211_DEFAULT_RATES_MASK (IEEE80211_OFDM_DEFAULT_RATES_MASK | \
+                                IEEE80211_CCK_DEFAULT_RATES_MASK)
+
+#define IEEE80211_NUM_OFDM_RATES	    8
+#define IEEE80211_NUM_CCK_RATES	            4
+#define IEEE80211_OFDM_SHIFT_MASK_A         4
+
+
+
+
+/* NOTE: This data is for statistical purposes; not all hardware provides this
+ *       information for frames received.  Not setting these will not cause
+ *       any adverse affects. */
+struct ieee80211_rx_stats {
+	u32 mac_time[2];
+	u8 signalstrength;
+	s8 rssi;
+	u8 signal;
+	u8 noise;
+	u16 rate; /* in 100 kbps */
+	u8 received_channel;
+	u8 control;
+	u8 mask;
+	u8 freq;
+	u16 len;
+	u8 nic_type;
+};
+
+/* IEEE 802.11 requires that STA supports concurrent reception of at least
+ * three fragmented frames. This define can be increased to support more
+ * concurrent frames, but it should be noted that each entry can consume about
+ * 2 kB of RAM and increasing cache size will slow down frame reassembly. */
+#define IEEE80211_FRAG_CACHE_LEN 4
+
+struct ieee80211_frag_entry {
+	unsigned long first_frag_time;
+	unsigned int seq;
+	unsigned int last_frag;
+	struct sk_buff *skb;
+	u8 src_addr[ETH_ALEN];
+	u8 dst_addr[ETH_ALEN];
+};
+
+struct ieee80211_stats {
+	unsigned int tx_unicast_frames;
+	unsigned int tx_multicast_frames;
+	unsigned int tx_fragments;
+	unsigned int tx_unicast_octets;
+	unsigned int tx_multicast_octets;
+	unsigned int tx_deferred_transmissions;
+	unsigned int tx_single_retry_frames;
+	unsigned int tx_multiple_retry_frames;
+	unsigned int tx_retry_limit_exceeded;
+	unsigned int tx_discards;
+	unsigned int rx_unicast_frames;
+	unsigned int rx_multicast_frames;
+	unsigned int rx_fragments;
+	unsigned int rx_unicast_octets;
+	unsigned int rx_multicast_octets;
+	unsigned int rx_fcs_errors;
+	unsigned int rx_discards_no_buffer;
+	unsigned int tx_discards_wrong_sa;
+	unsigned int rx_discards_undecryptable;
+	unsigned int rx_message_in_msg_fragments;
+	unsigned int rx_message_in_bad_msg_fragments;
+};
+
+struct ieee80211_softmac_stats{
+	unsigned int rx_ass_ok;
+	unsigned int rx_ass_err;
+	unsigned int rx_probe_rq;
+	unsigned int tx_probe_rs;
+	unsigned int tx_beacons;
+	unsigned int rx_auth_rq;
+	unsigned int rx_auth_rs_ok;
+	unsigned int rx_auth_rs_err;
+	unsigned int tx_auth_rq;
+	unsigned int no_auth_rs;
+	unsigned int no_ass_rs;
+	unsigned int tx_ass_rq;
+	unsigned int rx_ass_rq;
+	unsigned int tx_probe_rq;
+	unsigned int reassoc;
+	unsigned int swtxstop;
+	unsigned int swtxawake;
+};
+
+struct ieee80211_device;
+
+#include "ieee80211_crypt.h"
+
+#define SEC_KEY_1         (1<<0)
+#define SEC_KEY_2         (1<<1)
+#define SEC_KEY_3         (1<<2)
+#define SEC_KEY_4         (1<<3)
+#define SEC_ACTIVE_KEY    (1<<4)
+#define SEC_AUTH_MODE     (1<<5)
+#define SEC_UNICAST_GROUP (1<<6)
+#define SEC_LEVEL         (1<<7)
+#define SEC_ENABLED       (1<<8)
+
+#define SEC_LEVEL_0      0 /* None */
+#define SEC_LEVEL_1      1 /* WEP 40 and 104 bit */
+#define SEC_LEVEL_2      2 /* Level 1 + TKIP */
+#define SEC_LEVEL_2_CKIP 3 /* Level 1 + CKIP */
+#define SEC_LEVEL_3      4 /* Level 2 + CCMP */
+
+#define WEP_KEYS 4
+#define WEP_KEY_LEN 13
+
+#define WEP_KEY_LEN_MODIF 32
+
+struct ieee80211_security {
+	u16 active_key:2,
+            enabled:1,
+	    auth_mode:2,
+            auth_algo:4,
+            unicast_uses_group:1;
+	u8 key_sizes[WEP_KEYS];
+	u8 keys[WEP_KEYS][WEP_KEY_LEN_MODIF];
+	u8 level;
+	u16 flags;
+} __attribute__ ((packed));
+
+
+/*
+
+ 802.11 data frame from AP
+
+      ,-------------------------------------------------------------------.
+Bytes |  2   |  2   |    6    |    6    |    6    |  2   | 0..2312 |   4  |
+      |------|------|---------|---------|---------|------|---------|------|
+Desc. | ctrl | dura |  DA/RA  |   TA    |    SA   | Sequ |  frame  |  fcs |
+      |      | tion | (BSSID) |         |         | ence |  data   |      |
+      `-------------------------------------------------------------------'
+
+Total: 28-2340 bytes
+
+*/
+
+struct ieee80211_header_data {
+	u16 frame_ctl;
+	u16 duration_id;
+	u8 addr1[6];
+	u8 addr2[6];
+	u8 addr3[6];
+	u16 seq_ctrl;
+};
+
+#define BEACON_PROBE_SSID_ID_POSITION 12
+
+/* Management Frame Information Element Types */
+#define MFIE_TYPE_SSID       0
+#define MFIE_TYPE_RATES      1
+#define MFIE_TYPE_FH_SET     2
+#define MFIE_TYPE_DS_SET     3
+#define MFIE_TYPE_CF_SET     4
+#define MFIE_TYPE_TIM        5
+#define MFIE_TYPE_IBSS_SET   6
+#define MFIE_TYPE_COUNTRY  7 //+YJ,080625
+#define MFIE_TYPE_CHALLENGE  16
+#define MFIE_TYPE_ERP        42
+#define MFIE_TYPE_RSN	     48
+#define MFIE_TYPE_RATES_EX   50
+#define MFIE_TYPE_GENERIC    221
+
+#ifdef ENABLE_DOT11D
+typedef enum
+{
+	COUNTRY_CODE_FCC = 0,
+	COUNTRY_CODE_IC = 1,
+	COUNTRY_CODE_ETSI = 2,
+	COUNTRY_CODE_SPAIN = 3,
+	COUNTRY_CODE_FRANCE = 4,
+	COUNTRY_CODE_MKK = 5,
+	COUNTRY_CODE_MKK1 = 6,
+	COUNTRY_CODE_ISRAEL = 7,
+	COUNTRY_CODE_TELEC = 8,
+	COUNTRY_CODE_GLOBAL_DOMAIN = 9,
+	COUNTRY_CODE_WORLD_WIDE_13_INDEX = 10
+}country_code_type_t;
+#endif
+
+struct ieee80211_info_element_hdr {
+	u8 id;
+	u8 len;
+} __attribute__ ((packed));
+
+struct ieee80211_info_element {
+	u8 id;
+	u8 len;
+	u8 data[0];
+} __attribute__ ((packed));
+
+/*
+ * These are the data types that can make up management packets
+ *
+	u16 auth_algorithm;
+	u16 auth_sequence;
+	u16 beacon_interval;
+	u16 capability;
+	u8 current_ap[ETH_ALEN];
+	u16 listen_interval;
+	struct {
+		u16 association_id:14, reserved:2;
+	} __attribute__ ((packed));
+	u32 time_stamp[2];
+	u16 reason;
+	u16 status;
+*/
+
+#define IEEE80211_DEFAULT_TX_ESSID "Penguin"
+#define IEEE80211_DEFAULT_BASIC_RATE 10
+
+struct ieee80211_authentication {
+	struct ieee80211_header_data header;
+	u16 algorithm;
+	u16 transaction;
+	u16 status;
+	//struct ieee80211_info_element_hdr info_element;
+} __attribute__ ((packed));
+
+
+struct ieee80211_probe_response {
+	struct ieee80211_header_data header;
+	u32 time_stamp[2];
+	u16 beacon_interval;
+	u16 capability;
+	struct ieee80211_info_element info_element;
+} __attribute__ ((packed));
+
+struct ieee80211_probe_request {
+	struct ieee80211_header_data header;
+	/*struct ieee80211_info_element info_element;*/
+} __attribute__ ((packed));
+
+struct ieee80211_assoc_request_frame {
+	struct ieee80211_hdr_3addr header;
+	u16 capability;
+	u16 listen_interval;
+	//u8 current_ap[ETH_ALEN];
+	struct ieee80211_info_element_hdr info_element;
+} __attribute__ ((packed));
+
+struct ieee80211_assoc_response_frame {
+	struct ieee80211_hdr_3addr header;
+	u16 capability;
+	u16 status;
+	u16 aid;
+	struct ieee80211_info_element info_element; /* supported rates */
+} __attribute__ ((packed));
+
+struct ieee80211_disassoc_frame{
+        struct ieee80211_hdr_3addr header;
+        u16    reasoncode;
+}__attribute__ ((packed));
+
+struct ieee80211_txb {
+	u8 nr_frags;
+	u8 encrypted;
+	u16 reserved;
+	u16 frag_size;
+	u16 payload_size;
+	struct sk_buff *fragments[0];
+};
+
+struct ieee80211_wmm_ac_param {
+	u8 ac_aci_acm_aifsn;
+	u8 ac_ecwmin_ecwmax;
+	u16 ac_txop_limit;
+};
+
+struct ieee80211_wmm_ts_info {
+	u8 ac_dir_tid;
+	u8 ac_up_psb;
+	u8 reserved;
+} __attribute__ ((packed));
+
+struct ieee80211_wmm_tspec_elem {
+	struct ieee80211_wmm_ts_info ts_info;
+	u16 norm_msdu_size;
+	u16 max_msdu_size;
+	u32 min_serv_inter;
+	u32 max_serv_inter;
+	u32 inact_inter;
+	u32 suspen_inter;
+	u32 serv_start_time;
+	u32 min_data_rate;
+	u32 mean_data_rate;
+	u32 peak_data_rate;
+	u32 max_burst_size;
+	u32 delay_bound;
+	u32 min_phy_rate;
+	u16 surp_band_allow;
+	u16 medium_time;
+}__attribute__((packed));
+
+enum {WMM_all_frame, WMM_two_frame, WMM_four_frame, WMM_six_frame};
+#define MAX_SP_Len  (WMM_all_frame << 4)
+#define IEEE80211_QOS_TID 0x0f
+#define QOS_CTL_NOTCONTAIN_ACK (0x01 << 5)
+
+/* SWEEP TABLE ENTRIES NUMBER*/
+#define MAX_SWEEP_TAB_ENTRIES		  42
+#define MAX_SWEEP_TAB_ENTRIES_PER_PACKET  7
+/* MAX_RATES_LENGTH needs to be 12.  The spec says 8, and many APs
+ * only use 8, and then use extended rates for the remaining supported
+ * rates.  Other APs, however, stick all of their supported rates on the
+ * main rates information element... */
+#define MAX_RATES_LENGTH                  ((u8)12)
+#define MAX_RATES_EX_LENGTH               ((u8)16)
+#define MAX_NETWORK_COUNT                  128
+//#define MAX_CHANNEL_NUMBER                 161
+#define MAX_CHANNEL_NUMBER                 165 //YJ,modified,080625
+#define MAX_IE_LEN						0xFF //+YJ,080625
+
+typedef struct _CHANNEL_LIST{
+	u8	Channel[MAX_CHANNEL_NUMBER + 1];
+	u8	Len;
+}CHANNEL_LIST, *PCHANNEL_LIST;
+
+#define IEEE80211_SOFTMAC_SCAN_TIME	  100//400
+//(HZ / 2)
+//by amy for ps
+#define IEEE80211_WATCH_DOG_TIME    2000
+//by amy for ps
+//by amy for antenna
+#define ANTENNA_DIVERSITY_TIMER_PERIOD		1000 // 1000 m
+//by amy for antenna
+#define IEEE80211_SOFTMAC_ASSOC_RETRY_TIME (HZ * 2)
+
+#define CRC_LENGTH                 4U
+
+#define MAX_WPA_IE_LEN 64
+
+#define NETWORK_EMPTY_ESSID (1<<0)
+#define NETWORK_HAS_OFDM    (1<<1)
+#define NETWORK_HAS_CCK     (1<<2)
+
+#define IEEE80211_DTIM_MBCAST 4
+#define IEEE80211_DTIM_UCAST 2
+#define IEEE80211_DTIM_VALID 1
+#define IEEE80211_DTIM_INVALID 0
+
+#define IEEE80211_PS_DISABLED 0
+#define IEEE80211_PS_UNICAST IEEE80211_DTIM_UCAST
+#define IEEE80211_PS_MBCAST IEEE80211_DTIM_MBCAST
+#define IEEE80211_PS_ENABLE   IEEE80211_DTIM_VALID
+//added by David for QoS 2006/6/30
+//#define WMM_Hang_8187
+#ifdef WMM_Hang_8187
+#undef WMM_Hang_8187
+#endif
+
+#define WME_AC_BE   0x00
+#define WME_AC_BK   0x01
+#define WME_AC_VI   0x02
+#define WME_AC_VO   0x03
+#define WME_ACI_MASK 0x03
+#define WME_AIFSN_MASK 0x03
+#define WME_AC_PRAM_LEN 16
+
+//UP Mapping to AC, using in MgntQuery_SequenceNumber() and maybe for DSCP
+//#define UP2AC(up)	((up<3) ? ((up==0)?1:0) : (up>>1))
+#define UP2AC(up) (		   \
+	((up) < 1) ? WME_AC_BE : \
+	((up) < 3) ? WME_AC_BK : \
+	((up) < 4) ? WME_AC_BE : \
+	((up) < 6) ? WME_AC_VI : \
+	WME_AC_VO)
+//AC Mapping to UP, using in Tx part for selecting the corresponding TX queue
+#define AC2UP(_ac)	(       \
+	((_ac) == WME_AC_VO) ? 6 : \
+	((_ac) == WME_AC_VI) ? 5 : \
+	((_ac) == WME_AC_BK) ? 1 : \
+	0)
+
+#define	ETHER_ADDR_LEN		6	/* length of an Ethernet address */
+struct	ether_header {
+	u8 ether_dhost[ETHER_ADDR_LEN];
+	u8 ether_shost[ETHER_ADDR_LEN];
+	u16 ether_type;
+} __attribute__((packed));
+
+#ifndef ETHERTYPE_PAE
+#define	ETHERTYPE_PAE	0x888e		/* EAPOL PAE/802.1x */
+#endif
+#ifndef ETHERTYPE_IP
+#define	ETHERTYPE_IP	0x0800		/* IP protocol */
+#endif
+
+struct ieee80211_network {
+	/* These entries are used to identify a unique network */
+	u8 bssid[ETH_ALEN];
+	u8 channel;
+	/* Ensure null-terminated for any debug msgs */
+	u8 ssid[IW_ESSID_MAX_SIZE + 1];
+	u8 ssid_len;
+
+	/* These are network statistics */
+	struct ieee80211_rx_stats stats;
+	u16 capability;
+	u8 rates[MAX_RATES_LENGTH];
+	u8 rates_len;
+	u8 rates_ex[MAX_RATES_EX_LENGTH];
+	u8 rates_ex_len;
+	unsigned long last_scanned;
+	u8 mode;
+	u8 flags;
+	u32 last_associate;
+	u32 time_stamp[2];
+	u16 beacon_interval;
+	u16 listen_interval;
+	u16 atim_window;
+	u8 wpa_ie[MAX_WPA_IE_LEN];
+	size_t wpa_ie_len;
+	u8 rsn_ie[MAX_WPA_IE_LEN];
+	size_t rsn_ie_len;
+	u8 dtim_period;
+	u8 dtim_data;
+	u32 last_dtim_sta_time[2];
+	struct list_head list;
+	//appeded for QoS
+	u8 wmm_info;
+	struct ieee80211_wmm_ac_param wmm_param[4];
+	u8 QoS_Enable;
+	u8 SignalStrength;
+//by amy 080312
+	u8 HighestOperaRate;
+//by amy 080312
+#ifdef THOMAS_TURBO
+	u8 Turbo_Enable;//enable turbo mode, added by thomas
+#endif
+#ifdef ENABLE_DOT11D
+	u16 CountryIeLen;
+	u8 CountryIeBuf[MAX_IE_LEN];
+#endif
+};
+
+enum ieee80211_state {
+
+	/* the card is not linked at all */
+	IEEE80211_NOLINK = 0,
+
+	/* IEEE80211_ASSOCIATING* are for BSS client mode
+	 * the driver shall not perform RX filtering unless
+	 * the state is LINKED.
+	 * The driver shall just check for the state LINKED and
+	 * defaults to NOLINK for ALL the other states (including
+	 * LINKED_SCANNING)
+	 */
+
+	/* the association procedure will start (wq scheduling)*/
+	IEEE80211_ASSOCIATING,
+	IEEE80211_ASSOCIATING_RETRY,
+
+	/* the association procedure is sending AUTH request*/
+	IEEE80211_ASSOCIATING_AUTHENTICATING,
+
+	/* the association procedure has successfully authentcated
+	 * and is sending association request
+	 */
+	IEEE80211_ASSOCIATING_AUTHENTICATED,
+
+	/* the link is ok. the card associated to a BSS or linked
+	 * to a ibss cell or acting as an AP and creating the bss
+	 */
+	IEEE80211_LINKED,
+
+	/* same as LINKED, but the driver shall apply RX filter
+	 * rules as we are in NO_LINK mode. As the card is still
+	 * logically linked, but it is doing a syncro site survey
+	 * then it will be back to LINKED state.
+	 */
+	IEEE80211_LINKED_SCANNING,
+
+};
+
+#define DEFAULT_MAX_SCAN_AGE (15 * HZ)
+#define DEFAULT_FTS 2346
+#define MAC_FMT "%02x:%02x:%02x:%02x:%02x:%02x"
+#define MAC_ARG(x) ((u8*)(x))[0],((u8*)(x))[1],((u8*)(x))[2],((u8*)(x))[3],((u8*)(x))[4],((u8*)(x))[5]
+
+
+#if (LINUX_VERSION_CODE <= KERNEL_VERSION(2,6,11))
+extern inline int is_multicast_ether_addr(const u8 *addr)
+{
+        return ((addr[0] != 0xff) && (0x01 & addr[0]));
+}
+#endif
+
+#if (LINUX_VERSION_CODE <= KERNEL_VERSION(2,6,13))
+extern inline int is_broadcast_ether_addr(const u8 *addr)
+{
+	return ((addr[0] == 0xff) && (addr[1] == 0xff) && (addr[2] == 0xff) &&   \
+		(addr[3] == 0xff) && (addr[4] == 0xff) && (addr[5] == 0xff));
+}
+#endif
+
+#define CFG_IEEE80211_RESERVE_FCS (1<<0)
+#define CFG_IEEE80211_COMPUTE_FCS (1<<1)
+
+typedef struct tx_pending_t{
+	int frag;
+	struct ieee80211_txb *txb;
+}tx_pending_t;
+
+
+struct ieee80211_device {
+	struct net_device *dev;
+
+	/* Bookkeeping structures */
+	struct net_device_stats stats;
+	struct ieee80211_stats ieee_stats;
+	struct ieee80211_softmac_stats softmac_stats;
+
+	/* Probe / Beacon management */
+	struct list_head network_free_list;
+	struct list_head network_list;
+	struct ieee80211_network *networks;
+	int scans;
+	int scan_age;
+
+	int iw_mode; /* operating mode (IW_MODE_*) */
+
+	spinlock_t lock;
+	spinlock_t wpax_suitlist_lock;
+
+	int tx_headroom; /* Set to size of any additional room needed at front
+			  * of allocated Tx SKBs */
+	u32 config;
+
+	/* WEP and other encryption related settings at the device level */
+	int open_wep; /* Set to 1 to allow unencrypted frames */
+
+	int reset_on_keychange; /* Set to 1 if the HW needs to be reset on
+				 * WEP key changes */
+
+	/* If the host performs {en,de}cryption, then set to 1 */
+	int host_encrypt;
+	int host_decrypt;
+	int ieee802_1x; /* is IEEE 802.1X used */
+
+	/* WPA data */
+	int wpa_enabled;
+	int drop_unencrypted;
+	int tkip_countermeasures;
+	int privacy_invoked;
+	size_t wpa_ie_len;
+	u8 *wpa_ie;
+
+	u8 ap_mac_addr[6];
+	u16 pairwise_key_type;
+	u16 broadcast_key_type;
+
+	struct list_head crypt_deinit_list;
+	struct ieee80211_crypt_data *crypt[WEP_KEYS];
+	int tx_keyidx; /* default TX key index (crypt[tx_keyidx]) */
+	struct timer_list crypt_deinit_timer;
+
+	int bcrx_sta_key; /* use individual keys to override default keys even
+			   * with RX of broad/multicast frames */
+
+	/* Fragmentation structures */
+	// each streaming contain a entry
+	struct ieee80211_frag_entry frag_cache[17][IEEE80211_FRAG_CACHE_LEN];
+	unsigned int frag_next_idx[17];
+	u16 fts; /* Fragmentation Threshold */
+
+	/* This stores infos for the current network.
+	 * Either the network we are associated in INFRASTRUCTURE
+	 * or the network that we are creating in MASTER mode.
+	 * ad-hoc is a mixture ;-).
+	 * Note that in infrastructure mode, even when not associated,
+	 * fields bssid and essid may be valid (if wpa_set and essid_set
+	 * are true) as thy carry the value set by the user via iwconfig
+	 */
+	struct ieee80211_network current_network;
+
+
+	enum ieee80211_state state;
+
+	int short_slot;
+	int mode;       /* A, B, G */
+	int modulation; /* CCK, OFDM */
+	int freq_band;  /* 2.4Ghz, 5.2Ghz, Mixed */
+	int abg_true;   /* ABG flag              */
+
+	/* used for forcing the ibss workqueue to terminate
+	 * without wait for the syncro scan to terminate
+	 */
+	short sync_scan_hurryup;
+
+#ifdef ENABLE_DOT11D
+	void * pDot11dInfo;
+	bool bGlobalDomain;
+
+	// For Liteon Ch12~13 passive scan
+	u8	MinPassiveChnlNum;
+	u8	IbssStartChnl;
+#else
+	/* map of allowed channels. 0 is dummy */
+	// FIXME: remeber to default to a basic channel plan depending of the PHY type
+	int channel_map[MAX_CHANNEL_NUMBER+1];
+#endif
+
+	int rate;       /* current rate */
+	int basic_rate;
+	//FIXME: pleace callback, see if redundant with softmac_features
+	short active_scan;
+
+	/* this contains flags for selectively enable softmac support */
+	u16 softmac_features;
+
+	/* if the sequence control field is not filled by HW */
+	u16 seq_ctrl[5];
+
+	/* association procedure transaction sequence number */
+	u16 associate_seq;
+
+	/* AID for RTXed association responses */
+	u16 assoc_id;
+
+	/* power save mode related*/
+	short ps;
+	short sta_sleep;
+	int ps_timeout;
+	struct tasklet_struct ps_task;
+	u32 ps_th;
+	u32 ps_tl;
+
+	short raw_tx;
+	/* used if IEEE_SOFTMAC_TX_QUEUE is set */
+	short queue_stop;
+	short scanning;
+	short proto_started;
+
+	struct semaphore wx_sem;
+	struct semaphore scan_sem;
+
+	spinlock_t mgmt_tx_lock;
+	spinlock_t beacon_lock;
+
+	short beacon_txing;
+
+	short wap_set;
+	short ssid_set;
+
+	u8  wpax_type_set;    //{added by David, 2006.9.28}
+	u32 wpax_type_notify; //{added by David, 2006.9.26}
+
+	/* QoS related flag */
+	char init_wmmparam_flag;
+
+	/* for discarding duplicated packets in IBSS */
+	struct list_head ibss_mac_hash[IEEE_IBSS_MAC_HASH_SIZE];
+
+	/* for discarding duplicated packets in BSS */
+	u16 last_rxseq_num[17]; /* rx seq previous per-tid */
+	u16 last_rxfrag_num[17];/* tx frag previous per-tid */
+	unsigned long last_packet_time[17];
+
+	/* for PS mode */
+	unsigned long last_rx_ps_time;
+
+	/* used if IEEE_SOFTMAC_SINGLE_QUEUE is set */
+	struct sk_buff *mgmt_queue_ring[MGMT_QUEUE_NUM];
+	int mgmt_queue_head;
+	int mgmt_queue_tail;
+
+
+	/* used if IEEE_SOFTMAC_TX_QUEUE is set */
+	struct  tx_pending_t tx_pending;
+
+	/* used if IEEE_SOFTMAC_ASSOCIATE is set */
+	struct timer_list associate_timer;
+
+	/* used if IEEE_SOFTMAC_BEACONS is set */
+	struct timer_list beacon_timer;
+
+	struct work_struct associate_complete_wq;
+//	struct work_struct associate_retry_wq;
+	struct work_struct associate_procedure_wq;
+//	struct work_struct softmac_scan_wq;
+	struct work_struct wx_sync_scan_wq;
+	struct work_struct wmm_param_update_wq;
+	struct work_struct ps_request_tx_ack_wq;//for ps
+//	struct work_struct hw_wakeup_wq;
+//	struct work_struct hw_sleep_wq;
+//	struct work_struct watch_dog_wq;
+	bool bInactivePs;
+	bool actscanning;
+	bool beinretry;
+	u16 ListenInterval;
+	unsigned long NumRxDataInPeriod; //YJ,add,080828
+	unsigned long NumRxBcnInPeriod;  //YJ,add,080828
+	unsigned long NumRxOkTotal;
+	unsigned long NumRxUnicast;//YJ,add,080828,for keep alive
+	bool bHwRadioOff;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20)
+        struct delayed_work softmac_scan_wq;
+        struct delayed_work associate_retry_wq;
+	struct delayed_work hw_wakeup_wq;
+	struct delayed_work hw_sleep_wq;//+by amy 080324
+	struct delayed_work watch_dog_wq;
+	struct delayed_work sw_antenna_wq;
+	struct delayed_work  start_ibss_wq;
+//by amy for rate adaptive 080312
+    struct delayed_work rate_adapter_wq;
+//by amy for rate adaptive
+	struct delayed_work hw_dig_wq;
+	struct delayed_work tx_pw_wq;
+
+//Added for RF power on power off by lizhaoming 080512
+	struct delayed_work GPIOChangeRFWorkItem;
+#else
+
+	struct work_struct start_ibss_wq;
+        struct work_struct softmac_scan_wq;
+        struct work_struct associate_retry_wq;
+	struct work_struct hw_wakeup_wq;
+	struct work_struct hw_sleep_wq;
+	struct work_struct watch_dog_wq;
+	struct work_struct sw_antenna_wq;
+//by amy for rate adaptive 080312
+    struct work_struct rate_adapter_wq;
+//by amy for rate adaptive
+	struct work_struct hw_dig_wq;
+	struct work_struct tx_pw_wq;
+
+//Added for RF power on power off by lizhaoming 080512
+	struct work_struct GPIOChangeRFWorkItem;
+#endif
+	struct workqueue_struct *wq;
+
+	/* Callback functions */
+	void (*set_security)(struct net_device *dev,
+			     struct ieee80211_security *sec);
+
+	/* Used to TX data frame by using txb structs.
+	 * this is not used if in the softmac_features
+	 * is set the flag IEEE_SOFTMAC_TX_QUEUE
+	 */
+	int (*hard_start_xmit)(struct ieee80211_txb *txb,
+			       struct net_device *dev);
+
+	int (*reset_port)(struct net_device *dev);
+
+	/* Softmac-generated frames (mamagement) are TXed via this
+	 * callback if the flag IEEE_SOFTMAC_SINGLE_QUEUE is
+	 * not set. As some cards may have different HW queues that
+	 * one might want to use for data and management frames
+	 * the option to have two callbacks might be useful.
+	 * This fucntion can't sleep.
+	 */
+	int (*softmac_hard_start_xmit)(struct sk_buff *skb,
+			       struct net_device *dev);
+
+	/* used instead of hard_start_xmit (not softmac_hard_start_xmit)
+	 * if the IEEE_SOFTMAC_TX_QUEUE feature is used to TX data
+	 * frames. I the option IEEE_SOFTMAC_SINGLE_QUEUE is also set
+	 * then also management frames are sent via this callback.
+	 * This function can't sleep.
+	 */
+	void (*softmac_data_hard_start_xmit)(struct sk_buff *skb,
+			       struct net_device *dev,int rate);
+
+	/* stops the HW queue for DATA frames. Useful to avoid
+	 * waste time to TX data frame when we are reassociating
+	 * This function can sleep.
+	 */
+	void (*data_hard_stop)(struct net_device *dev);
+
+	/* OK this is complementar to data_poll_hard_stop */
+	void (*data_hard_resume)(struct net_device *dev);
+
+	/* ask to the driver to retune the radio .
+	 * This function can sleep. the driver should ensure
+	 * the radio has been swithced before return.
+	 */
+	void (*set_chan)(struct net_device *dev,short ch);
+
+	/* These are not used if the ieee stack takes care of
+	 * scanning (IEEE_SOFTMAC_SCAN feature set).
+	 * In this case only the set_chan is used.
+	 *
+	 * The syncro version is similar to the start_scan but
+	 * does not return until all channels has been scanned.
+	 * this is called in user context and should sleep,
+	 * it is called in a work_queue when swithcing to ad-hoc mode
+	 * or in behalf of iwlist scan when the card is associated
+	 * and root user ask for a scan.
+	 * the fucntion stop_scan should stop both the syncro and
+	 * background scanning and can sleep.
+	 * The fucntion start_scan should initiate the background
+	 * scanning and can't sleep.
+	 */
+	void (*scan_syncro)(struct net_device *dev);
+	void (*start_scan)(struct net_device *dev);
+	void (*stop_scan)(struct net_device *dev);
+
+	/* indicate the driver that the link state is changed
+	 * for example it may indicate the card is associated now.
+	 * Driver might be interested in this to apply RX filter
+	 * rules or simply light the LINK led
+	 */
+	void (*link_change)(struct net_device *dev);
+
+	/* these two function indicates to the HW when to start
+	 * and stop to send beacons. This is used when the
+	 * IEEE_SOFTMAC_BEACONS is not set. For now the
+	 * stop_send_bacons is NOT guaranteed to be called only
+	 * after start_send_beacons.
+	 */
+	void (*start_send_beacons) (struct net_device *dev);
+	void (*stop_send_beacons) (struct net_device *dev);
+
+	/* power save mode related */
+	void (*sta_wake_up) (struct net_device *dev);
+	void (*ps_request_tx_ack) (struct net_device *dev);
+	void (*enter_sleep_state) (struct net_device *dev, u32 th, u32 tl);
+	short (*ps_is_queue_empty) (struct net_device *dev);
+
+	/* QoS related */
+	//void (*wmm_param_update) (struct net_device *dev, u8 *ac_param);
+	//void (*wmm_param_update) (struct ieee80211_device *ieee);
+
+	/* This must be the last item so that it points to the data
+	 * allocated beyond this structure by alloc_ieee80211 */
+	u8 priv[0];
+};
+
+#define IEEE_A            (1<<0)
+#define IEEE_B            (1<<1)
+#define IEEE_G            (1<<2)
+#define IEEE_MODE_MASK    (IEEE_A|IEEE_B|IEEE_G)
+
+/* Generate a 802.11 header */
+
+/* Uses the channel change callback directly
+ * instead of [start/stop] scan callbacks
+ */
+#define IEEE_SOFTMAC_SCAN (1<<2)
+
+/* Perform authentication and association handshake */
+#define IEEE_SOFTMAC_ASSOCIATE (1<<3)
+
+/* Generate probe requests */
+#define IEEE_SOFTMAC_PROBERQ (1<<4)
+
+/* Generate respones to probe requests */
+#define IEEE_SOFTMAC_PROBERS (1<<5)
+
+/* The ieee802.11 stack will manages the netif queue
+ * wake/stop for the driver, taking care of 802.11
+ * fragmentation. See softmac.c for details. */
+#define IEEE_SOFTMAC_TX_QUEUE (1<<7)
+
+/* Uses only the softmac_data_hard_start_xmit
+ * even for TX management frames.
+ */
+#define IEEE_SOFTMAC_SINGLE_QUEUE (1<<8)
+
+/* Generate beacons.  The stack will enqueue beacons
+ * to the card
+ */
+#define IEEE_SOFTMAC_BEACONS (1<<6)
+
+
+
+static inline void *ieee80211_priv(struct net_device *dev)
+{
+	return ((struct ieee80211_device *)netdev_priv(dev))->priv;
+}
+
+extern inline int ieee80211_is_empty_essid(const char *essid, int essid_len)
+{
+	/* Single white space is for Linksys APs */
+	if (essid_len == 1 && essid[0] == ' ')
+		return 1;
+
+	/* Otherwise, if the entire essid is 0, we assume it is hidden */
+	while (essid_len) {
+		essid_len--;
+		if (essid[essid_len] != '\0')
+			return 0;
+	}
+
+	return 1;
+}
+
+extern inline int ieee80211_is_valid_mode(struct ieee80211_device *ieee, int mode)
+{
+	/*
+	 * It is possible for both access points and our device to support
+	 * combinations of modes, so as long as there is one valid combination
+	 * of ap/device supported modes, then return success
+	 *
+	 */
+	if ((mode & IEEE_A) &&
+	    (ieee->modulation & IEEE80211_OFDM_MODULATION) &&
+	    (ieee->freq_band & IEEE80211_52GHZ_BAND))
+		return 1;
+
+	if ((mode & IEEE_G) &&
+	    (ieee->modulation & IEEE80211_OFDM_MODULATION) &&
+	    (ieee->freq_band & IEEE80211_24GHZ_BAND))
+		return 1;
+
+	if ((mode & IEEE_B) &&
+	    (ieee->modulation & IEEE80211_CCK_MODULATION) &&
+	    (ieee->freq_band & IEEE80211_24GHZ_BAND))
+		return 1;
+
+	return 0;
+}
+
+extern inline int ieee80211_get_hdrlen(u16 fc)
+{
+	int hdrlen = 24;
+
+	switch (WLAN_FC_GET_TYPE(fc)) {
+	case IEEE80211_FTYPE_DATA:
+		if ((fc & IEEE80211_FCTL_FROMDS) && (fc & IEEE80211_FCTL_TODS))
+			hdrlen = 30; /* Addr4 */
+		if(IEEE80211_QOS_HAS_SEQ(fc))
+			hdrlen += 2; /* QOS ctrl*/
+		break;
+	case IEEE80211_FTYPE_CTL:
+		switch (WLAN_FC_GET_STYPE(fc)) {
+		case IEEE80211_STYPE_CTS:
+		case IEEE80211_STYPE_ACK:
+			hdrlen = 10;
+			break;
+		default:
+			hdrlen = 16;
+			break;
+		}
+		break;
+	}
+
+	return hdrlen;
+}
+
+
+
+/* ieee80211.c */
+extern void free_ieee80211(struct net_device *dev);
+extern struct net_device *alloc_ieee80211(int sizeof_priv);
+
+extern int ieee80211_set_encryption(struct ieee80211_device *ieee);
+
+/* ieee80211_tx.c */
+
+extern int ieee80211_encrypt_fragment(
+	struct ieee80211_device *ieee,
+	struct sk_buff *frag,
+	int hdr_len);
+
+extern int ieee80211_xmit(struct sk_buff *skb,
+			  struct net_device *dev);
+extern void ieee80211_txb_free(struct ieee80211_txb *);
+
+
+/* ieee80211_rx.c */
+extern int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
+			struct ieee80211_rx_stats *rx_stats);
+extern void ieee80211_rx_mgt(struct ieee80211_device *ieee,
+			     struct ieee80211_hdr *header,
+			     struct ieee80211_rx_stats *stats);
+
+/* ieee80211_wx.c */
+extern int ieee80211_wx_get_scan(struct ieee80211_device *ieee,
+				 struct iw_request_info *info,
+				 union iwreq_data *wrqu, char *key);
+extern int ieee80211_wx_set_encode(struct ieee80211_device *ieee,
+				   struct iw_request_info *info,
+				   union iwreq_data *wrqu, char *key);
+extern int ieee80211_wx_get_encode(struct ieee80211_device *ieee,
+				   struct iw_request_info *info,
+				   union iwreq_data *wrqu, char *key);
+extern int ieee80211_wx_set_encode_ext(struct ieee80211_device *ieee,
+                            struct iw_request_info *info,
+                            union iwreq_data* wrqu, char *extra);
+int ieee80211_wx_set_auth(struct ieee80211_device *ieee,
+                               struct iw_request_info *info,
+                               struct iw_param *data, char *extra);
+int ieee80211_wx_set_mlme(struct ieee80211_device *ieee,
+                               struct iw_request_info *info,
+                               union iwreq_data *wrqu, char *extra);
+
+int ieee80211_wx_set_gen_ie(struct ieee80211_device *ieee, u8 *ie, size_t len);
+/* ieee80211_softmac.c */
+extern short ieee80211_is_54g(struct ieee80211_network net);
+extern short ieee80211_is_shortslot(struct ieee80211_network net);
+extern int ieee80211_rx_frame_softmac(struct ieee80211_device *ieee, struct sk_buff *skb,
+			struct ieee80211_rx_stats *rx_stats, u16 type,
+			u16 stype);
+extern void ieee80211_softmac_new_net(struct ieee80211_device *ieee, struct ieee80211_network *net);
+
+extern void ieee80211_softmac_xmit(struct ieee80211_txb *txb, struct ieee80211_device *ieee);
+extern void ieee80211_softmac_check_all_nets(struct ieee80211_device *ieee);
+extern void ieee80211_start_bss(struct ieee80211_device *ieee);
+extern void ieee80211_start_master_bss(struct ieee80211_device *ieee);
+extern void ieee80211_start_ibss(struct ieee80211_device *ieee);
+extern void ieee80211_softmac_init(struct ieee80211_device *ieee);
+extern void ieee80211_softmac_free(struct ieee80211_device *ieee);
+extern void ieee80211_associate_abort(struct ieee80211_device *ieee);
+extern void ieee80211_disassociate(struct ieee80211_device *ieee);
+extern void ieee80211_stop_scan(struct ieee80211_device *ieee);
+extern void ieee80211_start_scan_syncro(struct ieee80211_device *ieee);
+extern void ieee80211_check_all_nets(struct ieee80211_device *ieee);
+extern void ieee80211_start_protocol(struct ieee80211_device *ieee);
+extern void ieee80211_stop_protocol(struct ieee80211_device *ieee);
+extern void ieee80211_softmac_start_protocol(struct ieee80211_device *ieee);
+extern void ieee80211_softmac_stop_protocol(struct ieee80211_device *ieee);
+extern void ieee80211_reset_queue(struct ieee80211_device *ieee);
+extern void ieee80211_wake_queue(struct ieee80211_device *ieee);
+extern void ieee80211_stop_queue(struct ieee80211_device *ieee);
+extern struct sk_buff *ieee80211_get_beacon(struct ieee80211_device *ieee);
+extern void ieee80211_start_send_beacons(struct ieee80211_device *ieee);
+extern void ieee80211_stop_send_beacons(struct ieee80211_device *ieee);
+extern int ieee80211_wpa_supplicant_ioctl(struct ieee80211_device *ieee, struct iw_point *p);
+extern void notify_wx_assoc_event(struct ieee80211_device *ieee);
+extern void ieee80211_ps_tx_ack(struct ieee80211_device *ieee, short success);
+extern void SendDisassociation(struct ieee80211_device *ieee,u8* asSta,u8 asRsn);
+extern void ieee80211_start_scan(struct ieee80211_device *ieee);
+
+//Add for RF power on power off by lizhaoming 080512
+extern void SendDisassociation(struct ieee80211_device *ieee,
+       			 u8*                     asSta,
+        		 u8                      asRsn);
+
+/* ieee80211_crypt_ccmp&tkip&wep.c */
+extern void ieee80211_tkip_null(void);
+extern void ieee80211_wep_null(void);
+extern void ieee80211_ccmp_null(void);
+/* ieee80211_softmac_wx.c */
+
+extern int ieee80211_wx_get_wap(struct ieee80211_device *ieee,
+			    struct iw_request_info *info,
+			    union iwreq_data *wrqu, char *ext);
+
+extern int ieee80211_wx_set_wap(struct ieee80211_device *ieee,
+			 struct iw_request_info *info,
+			 union iwreq_data *awrq,
+			 char *extra);
+
+extern int ieee80211_wx_get_essid(struct ieee80211_device *ieee, struct iw_request_info *a,union iwreq_data *wrqu,char *b);
+
+extern int ieee80211_wx_set_rate(struct ieee80211_device *ieee,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra);
+
+extern int ieee80211_wx_get_rate(struct ieee80211_device *ieee,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra);
+
+extern int ieee80211_wx_set_mode(struct ieee80211_device *ieee, struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b);
+
+extern int ieee80211_wx_set_scan(struct ieee80211_device *ieee, struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b);
+
+extern int ieee80211_wx_set_essid(struct ieee80211_device *ieee,
+			      struct iw_request_info *a,
+			      union iwreq_data *wrqu, char *extra);
+
+extern int ieee80211_wx_get_mode(struct ieee80211_device *ieee, struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b);
+
+extern int ieee80211_wx_set_freq(struct ieee80211_device *ieee, struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b);
+
+extern int ieee80211_wx_get_freq(struct ieee80211_device *ieee, struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b);
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+extern void ieee80211_wx_sync_scan_wq(struct work_struct *work);
+#else
+ extern void ieee80211_wx_sync_scan_wq(struct ieee80211_device *ieee);
+#endif
+//extern void ieee80211_wx_sync_scan_wq(struct ieee80211_device *ieee);
+
+extern int ieee80211_wx_set_rawtx(struct ieee80211_device *ieee,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra);
+
+extern int ieee80211_wx_get_name(struct ieee80211_device *ieee,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra);
+
+extern int ieee80211_wx_set_power(struct ieee80211_device *ieee,
+				 struct iw_request_info *info,
+				 union iwreq_data *wrqu, char *extra);
+
+extern int ieee80211_wx_get_power(struct ieee80211_device *ieee,
+				 struct iw_request_info *info,
+				 union iwreq_data *wrqu, char *extra);
+
+extern void ieee80211_softmac_ips_scan_syncro(struct ieee80211_device *ieee);
+
+extern void ieee80211_sta_ps_send_null_frame(struct ieee80211_device *ieee, short pwr);
+
+extern const long ieee80211_wlan_frequencies[];
+
+extern inline void ieee80211_increment_scans(struct ieee80211_device *ieee)
+{
+	ieee->scans++;
+}
+
+extern inline int ieee80211_get_scans(struct ieee80211_device *ieee)
+{
+	return ieee->scans;
+}
+
+static inline const char *escape_essid(const char *essid, u8 essid_len) {
+	static char escaped[IW_ESSID_MAX_SIZE * 2 + 1];
+	const char *s = essid;
+	char *d = escaped;
+
+	if (ieee80211_is_empty_essid(essid, essid_len)) {
+		memcpy(escaped, "<hidden>", sizeof("<hidden>"));
+		return escaped;
+	}
+
+	essid_len = min(essid_len, (u8)IW_ESSID_MAX_SIZE);
+	while (essid_len--) {
+		if (*s == '\0') {
+			*d++ = '\\';
+			*d++ = '0';
+			s++;
+		} else {
+			*d++ = *s++;
+		}
+	}
+	*d = '\0';
+	return escaped;
+}
+#endif /* IEEE80211_H */
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt.c b/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt.c
new file mode 100644
index 00000000000..af64cfbe16d
--- /dev/null
+++ b/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt.c
@@ -0,0 +1,265 @@
+/*
+ * Host AP crypto routines
+ *
+ * Copyright (c) 2002-2003, Jouni Malinen <jkmaline@cc.hut.fi>
+ * Portions Copyright (C) 2004, Intel Corporation <jketreno@linux.intel.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation. See README and COPYING for
+ * more details.
+ *
+ */
+
+//#include <linux/config.h>
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <asm/string.h>
+#include <asm/errno.h>
+
+#if (LINUX_VERSION_CODE<KERNEL_VERSION(2,6,18))
+#include<linux/config.h>
+#endif
+
+#include "ieee80211.h"
+
+MODULE_AUTHOR("Jouni Malinen");
+MODULE_DESCRIPTION("HostAP crypto");
+MODULE_LICENSE("GPL");
+
+struct ieee80211_crypto_alg {
+	struct list_head list;
+	struct ieee80211_crypto_ops *ops;
+};
+
+
+struct ieee80211_crypto {
+	struct list_head algs;
+	spinlock_t lock;
+};
+
+static struct ieee80211_crypto *hcrypt;
+
+void ieee80211_crypt_deinit_entries(struct ieee80211_device *ieee,
+					   int force)
+{
+	struct list_head *ptr, *n;
+	struct ieee80211_crypt_data *entry;
+
+	for (ptr = ieee->crypt_deinit_list.next, n = ptr->next;
+	     ptr != &ieee->crypt_deinit_list; ptr = n, n = ptr->next) {
+		entry = list_entry(ptr, struct ieee80211_crypt_data, list);
+
+		if (atomic_read(&entry->refcnt) != 0 && !force)
+			continue;
+
+		list_del(ptr);
+
+		if (entry->ops) {
+			entry->ops->deinit(entry->priv);
+			module_put(entry->ops->owner);
+		}
+		kfree(entry);
+	}
+}
+
+void ieee80211_crypt_deinit_handler(unsigned long data)
+{
+	struct ieee80211_device *ieee = (struct ieee80211_device *)data;
+	unsigned long flags;
+
+	spin_lock_irqsave(&ieee->lock, flags);
+	ieee80211_crypt_deinit_entries(ieee, 0);
+	if (!list_empty(&ieee->crypt_deinit_list)) {
+		printk(KERN_DEBUG "%s: entries remaining in delayed crypt "
+		       "deletion list\n", ieee->dev->name);
+		ieee->crypt_deinit_timer.expires = jiffies + HZ;
+		add_timer(&ieee->crypt_deinit_timer);
+	}
+	spin_unlock_irqrestore(&ieee->lock, flags);
+
+}
+
+void ieee80211_crypt_delayed_deinit(struct ieee80211_device *ieee,
+				    struct ieee80211_crypt_data **crypt)
+{
+	struct ieee80211_crypt_data *tmp;
+	unsigned long flags;
+
+	if (*crypt == NULL)
+		return;
+
+	tmp = *crypt;
+	*crypt = NULL;
+
+	/* must not run ops->deinit() while there may be pending encrypt or
+	 * decrypt operations. Use a list of delayed deinits to avoid needing
+	 * locking. */
+
+	spin_lock_irqsave(&ieee->lock, flags);
+	list_add(&tmp->list, &ieee->crypt_deinit_list);
+	if (!timer_pending(&ieee->crypt_deinit_timer)) {
+		ieee->crypt_deinit_timer.expires = jiffies + HZ;
+		add_timer(&ieee->crypt_deinit_timer);
+	}
+	spin_unlock_irqrestore(&ieee->lock, flags);
+}
+
+int ieee80211_register_crypto_ops(struct ieee80211_crypto_ops *ops)
+{
+	unsigned long flags;
+	struct ieee80211_crypto_alg *alg;
+
+	if (hcrypt == NULL)
+		return -1;
+
+	alg = kmalloc(sizeof(*alg), GFP_KERNEL);
+	if (alg == NULL)
+		return -ENOMEM;
+
+	memset(alg, 0, sizeof(*alg));
+	alg->ops = ops;
+
+	spin_lock_irqsave(&hcrypt->lock, flags);
+	list_add(&alg->list, &hcrypt->algs);
+	spin_unlock_irqrestore(&hcrypt->lock, flags);
+
+	printk(KERN_DEBUG "ieee80211_crypt: registered algorithm '%s'\n",
+	       ops->name);
+
+	return 0;
+}
+
+int ieee80211_unregister_crypto_ops(struct ieee80211_crypto_ops *ops)
+{
+	unsigned long flags;
+	struct list_head *ptr;
+	struct ieee80211_crypto_alg *del_alg = NULL;
+
+	if (hcrypt == NULL)
+		return -1;
+
+	spin_lock_irqsave(&hcrypt->lock, flags);
+	for (ptr = hcrypt->algs.next; ptr != &hcrypt->algs; ptr = ptr->next) {
+		struct ieee80211_crypto_alg *alg =
+			(struct ieee80211_crypto_alg *) ptr;
+		if (alg->ops == ops) {
+			list_del(&alg->list);
+			del_alg = alg;
+			break;
+		}
+	}
+	spin_unlock_irqrestore(&hcrypt->lock, flags);
+
+	if (del_alg) {
+		printk(KERN_DEBUG "ieee80211_crypt: unregistered algorithm "
+		       "'%s'\n", ops->name);
+		kfree(del_alg);
+	}
+
+	return del_alg ? 0 : -1;
+}
+
+
+struct ieee80211_crypto_ops * ieee80211_get_crypto_ops(const char *name)
+{
+	unsigned long flags;
+	struct list_head *ptr;
+	struct ieee80211_crypto_alg *found_alg = NULL;
+
+	if (hcrypt == NULL)
+		return NULL;
+
+	spin_lock_irqsave(&hcrypt->lock, flags);
+	for (ptr = hcrypt->algs.next; ptr != &hcrypt->algs; ptr = ptr->next) {
+		struct ieee80211_crypto_alg *alg =
+			(struct ieee80211_crypto_alg *) ptr;
+		if (strcmp(alg->ops->name, name) == 0) {
+			found_alg = alg;
+			break;
+		}
+	}
+	spin_unlock_irqrestore(&hcrypt->lock, flags);
+
+	if (found_alg)
+		return found_alg->ops;
+	else
+		return NULL;
+}
+
+
+static void * ieee80211_crypt_null_init(int keyidx) { return (void *) 1; }
+static void ieee80211_crypt_null_deinit(void *priv) {}
+
+static struct ieee80211_crypto_ops ieee80211_crypt_null = {
+	.name			= "NULL",
+	.init			= ieee80211_crypt_null_init,
+	.deinit			= ieee80211_crypt_null_deinit,
+	.encrypt_mpdu		= NULL,
+	.decrypt_mpdu		= NULL,
+	.encrypt_msdu		= NULL,
+	.decrypt_msdu		= NULL,
+	.set_key		= NULL,
+	.get_key		= NULL,
+	.extra_prefix_len	= 0,
+	.extra_postfix_len	= 0,
+	.owner			= THIS_MODULE,
+};
+
+
+int ieee80211_crypto_init(void)
+{
+	int ret = -ENOMEM;
+
+	hcrypt = kmalloc(sizeof(*hcrypt), GFP_KERNEL);
+	if (!hcrypt)
+		goto out;
+
+	memset(hcrypt, 0, sizeof(*hcrypt));
+	INIT_LIST_HEAD(&hcrypt->algs);
+	spin_lock_init(&hcrypt->lock);
+
+	ret = ieee80211_register_crypto_ops(&ieee80211_crypt_null);
+	if (ret < 0) {
+		kfree(hcrypt);
+		hcrypt = NULL;
+	}
+out:
+	return ret;
+}
+
+
+void ieee80211_crypto_deinit(void)
+{
+	struct list_head *ptr, *n;
+
+	if (hcrypt == NULL)
+		return;
+
+	for (ptr = hcrypt->algs.next, n = ptr->next; ptr != &hcrypt->algs;
+	     ptr = n, n = ptr->next) {
+		struct ieee80211_crypto_alg *alg =
+			(struct ieee80211_crypto_alg *) ptr;
+		list_del(ptr);
+		printk(KERN_DEBUG "ieee80211_crypt: unregistered algorithm "
+		       "'%s' (deinit)\n", alg->ops->name);
+		kfree(alg);
+	}
+
+	kfree(hcrypt);
+}
+
+#if 0
+EXPORT_SYMBOL(ieee80211_crypt_deinit_entries);
+EXPORT_SYMBOL(ieee80211_crypt_deinit_handler);
+EXPORT_SYMBOL(ieee80211_crypt_delayed_deinit);
+
+EXPORT_SYMBOL(ieee80211_register_crypto_ops);
+EXPORT_SYMBOL(ieee80211_unregister_crypto_ops);
+EXPORT_SYMBOL(ieee80211_get_crypto_ops);
+#endif
+
+//module_init(ieee80211_crypto_init);
+//module_exit(ieee80211_crypto_deinit);
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt.h b/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt.h
new file mode 100644
index 00000000000..b58a3bcc0dc
--- /dev/null
+++ b/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt.h
@@ -0,0 +1,86 @@
+/*
+ * Original code based on Host AP (software wireless LAN access point) driver
+ * for Intersil Prism2/2.5/3.
+ *
+ * Copyright (c) 2001-2002, SSH Communications Security Corp and Jouni Malinen
+ * <jkmaline@cc.hut.fi>
+ * Copyright (c) 2002-2003, Jouni Malinen <jkmaline@cc.hut.fi>
+ *
+ * Adaption to a generic IEEE 802.11 stack by James Ketrenos
+ * <jketreno@linux.intel.com>
+ *
+ * Copyright (c) 2004, Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation. See README and COPYING for
+ * more details.
+ */
+
+/*
+ * This file defines the interface to the ieee80211 crypto module.
+ */
+#ifndef IEEE80211_CRYPT_H
+#define IEEE80211_CRYPT_H
+
+#include <linux/skbuff.h>
+
+struct ieee80211_crypto_ops {
+	const char *name;
+
+	/* init new crypto context (e.g., allocate private data space,
+	 * select IV, etc.); returns NULL on failure or pointer to allocated
+	 * private data on success */
+	void * (*init)(int keyidx);
+
+	/* deinitialize crypto context and free allocated private data */
+	void (*deinit)(void *priv);
+
+	/* encrypt/decrypt return < 0 on error or >= 0 on success. The return
+	 * value from decrypt_mpdu is passed as the keyidx value for
+	 * decrypt_msdu. skb must have enough head and tail room for the
+	 * encryption; if not, error will be returned; these functions are
+	 * called for all MPDUs (i.e., fragments).
+	 */
+	int (*encrypt_mpdu)(struct sk_buff *skb, int hdr_len, void *priv);
+	int (*decrypt_mpdu)(struct sk_buff *skb, int hdr_len, void *priv);
+
+	/* These functions are called for full MSDUs, i.e. full frames.
+	 * These can be NULL if full MSDU operations are not needed. */
+	int (*encrypt_msdu)(struct sk_buff *skb, int hdr_len, void *priv);
+	int (*decrypt_msdu)(struct sk_buff *skb, int keyidx, int hdr_len,
+			    void *priv);
+
+	int (*set_key)(void *key, int len, u8 *seq, void *priv);
+	int (*get_key)(void *key, int len, u8 *seq, void *priv);
+
+	/* procfs handler for printing out key information and possible
+	 * statistics */
+	char * (*print_stats)(char *p, void *priv);
+
+	/* maximum number of bytes added by encryption; encrypt buf is
+	 * allocated with extra_prefix_len bytes, copy of in_buf, and
+	 * extra_postfix_len; encrypt need not use all this space, but
+	 * the result must start at the beginning of the buffer and correct
+	 * length must be returned */
+	int extra_prefix_len, extra_postfix_len;
+
+	struct module *owner;
+};
+
+struct ieee80211_crypt_data {
+	struct list_head list; /* delayed deletion list */
+	struct ieee80211_crypto_ops *ops;
+	void *priv;
+	atomic_t refcnt;
+};
+
+int ieee80211_register_crypto_ops(struct ieee80211_crypto_ops *ops);
+int ieee80211_unregister_crypto_ops(struct ieee80211_crypto_ops *ops);
+struct ieee80211_crypto_ops * ieee80211_get_crypto_ops(const char *name);
+void ieee80211_crypt_deinit_entries(struct ieee80211_device *, int);
+void ieee80211_crypt_deinit_handler(unsigned long);
+void ieee80211_crypt_delayed_deinit(struct ieee80211_device *ieee,
+				    struct ieee80211_crypt_data **crypt);
+
+#endif
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt_ccmp.c b/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt_ccmp.c
new file mode 100644
index 00000000000..7d890c328b0
--- /dev/null
+++ b/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt_ccmp.c
@@ -0,0 +1,533 @@
+/*
+ * Host AP crypt: host-based CCMP encryption implementation for Host AP driver
+ *
+ * Copyright (c) 2003-2004, Jouni Malinen <jkmaline@cc.hut.fi>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation. See README and COPYING for
+ * more details.
+ */
+
+//#include <linux/config.h>
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/random.h>
+#include <linux/skbuff.h>
+#include <linux/netdevice.h>
+#include <linux/if_ether.h>
+#include <linux/if_arp.h>
+#include <asm/string.h>
+#include <linux/wireless.h>
+
+#include "ieee80211.h"
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0))
+#include "rtl_crypto.h"
+#else
+#include <linux/crypto.h>
+#endif
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+    #include <asm/scatterlist.h>
+#else
+    #include <linux/scatterlist.h>
+#endif
+
+//#include <asm/scatterlist.h>
+
+MODULE_AUTHOR("Jouni Malinen");
+MODULE_DESCRIPTION("Host AP crypt: CCMP");
+MODULE_LICENSE("GPL");
+
+#ifdef OPENSUSE_SLED
+#ifndef IN_OPENSUSE_SLED
+#define IN_OPENSUSE_SLED 1
+#endif
+#endif
+
+#define AES_BLOCK_LEN 16
+#define CCMP_HDR_LEN 8
+#define CCMP_MIC_LEN 8
+#define CCMP_TK_LEN 16
+#define CCMP_PN_LEN 6
+
+struct ieee80211_ccmp_data {
+	u8 key[CCMP_TK_LEN];
+	int key_set;
+
+	u8 tx_pn[CCMP_PN_LEN];
+	u8 rx_pn[CCMP_PN_LEN];
+
+	u32 dot11RSNAStatsCCMPFormatErrors;
+	u32 dot11RSNAStatsCCMPReplays;
+	u32 dot11RSNAStatsCCMPDecryptErrors;
+
+	int key_idx;
+
+	struct crypto_tfm *tfm;
+
+	/* scratch buffers for virt_to_page() (crypto API) */
+	u8 tx_b0[AES_BLOCK_LEN], tx_b[AES_BLOCK_LEN],
+		tx_e[AES_BLOCK_LEN], tx_s0[AES_BLOCK_LEN];
+	u8 rx_b0[AES_BLOCK_LEN], rx_b[AES_BLOCK_LEN], rx_a[AES_BLOCK_LEN];
+};
+
+void ieee80211_ccmp_aes_encrypt(struct crypto_tfm *tfm,
+			     const u8 pt[16], u8 ct[16])
+{
+      	#if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,21))||(IN_OPENSUSE_SLED))
+	crypto_cipher_encrypt_one((void *)tfm, ct, pt);
+	#else
+	struct scatterlist src, dst;
+
+	src.page = virt_to_page(pt);
+	src.offset = offset_in_page(pt);
+	src.length = AES_BLOCK_LEN;
+
+	dst.page = virt_to_page(ct);
+	dst.offset = offset_in_page(ct);
+	dst.length = AES_BLOCK_LEN;
+
+	crypto_cipher_encrypt(tfm, &dst, &src, AES_BLOCK_LEN);
+	#endif
+}
+
+static void * ieee80211_ccmp_init(int key_idx)
+{
+	struct ieee80211_ccmp_data *priv;
+
+	priv = kmalloc(sizeof(*priv), GFP_ATOMIC);
+	if (priv == NULL)
+		goto fail;
+	memset(priv, 0, sizeof(*priv));
+	priv->key_idx = key_idx;
+
+       #if((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21)) && (!IN_OPENSUSE_SLED))
+	priv->tfm = crypto_alloc_tfm("aes", 0);
+	if (priv->tfm == NULL) {
+		printk(KERN_DEBUG "ieee80211_crypt_ccmp: could not allocate "
+		       "crypto API aes\n");
+		goto fail;
+	}
+       #else
+       priv->tfm = (void *)crypto_alloc_cipher("aes", 0, CRYPTO_ALG_ASYNC);
+	if (IS_ERR(priv->tfm)) {
+		printk(KERN_DEBUG "ieee80211_crypt_ccmp: could not allocate "
+		       "crypto API aes\n");
+		priv->tfm = NULL;
+		goto fail;
+	}
+	#endif
+	return priv;
+
+fail:
+	if (priv) {
+		if (priv->tfm)
+			//#if(LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))
+       			#if((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21)) && (!IN_OPENSUSE_SLED))
+			crypto_free_tfm(priv->tfm);
+                    #else
+			crypto_free_cipher((void *)priv->tfm);
+		      #endif
+		kfree(priv);
+	}
+
+	return NULL;
+}
+
+
+static void ieee80211_ccmp_deinit(void *priv)
+{
+	struct ieee80211_ccmp_data *_priv = priv;
+	if (_priv && _priv->tfm)
+		//#if(LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))
+       		#if((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21)) && (!IN_OPENSUSE_SLED))
+		crypto_free_tfm(_priv->tfm);
+             #else
+		crypto_free_cipher((void *)_priv->tfm);
+		#endif
+	kfree(priv);
+}
+
+
+static inline void xor_block(u8 *b, u8 *a, size_t len)
+{
+	int i;
+	for (i = 0; i < len; i++)
+		b[i] ^= a[i];
+}
+
+#ifndef JOHN_CCMP
+static void ccmp_init_blocks(struct crypto_tfm *tfm,
+			     struct ieee80211_hdr *hdr,
+			     u8 *pn, size_t dlen, u8 *b0, u8 *auth,
+			     u8 *s0)
+{
+	u8 *pos, qc = 0;
+	size_t aad_len;
+	u16 fc;
+	int a4_included, qc_included;
+	u8 aad[2 * AES_BLOCK_LEN];
+
+	fc = le16_to_cpu(hdr->frame_ctl);
+	a4_included = ((fc & (IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS)) ==
+		       (IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS));
+	/*
+	qc_included = ((WLAN_FC_GET_TYPE(fc) == IEEE80211_FTYPE_DATA) &&
+		       (WLAN_FC_GET_STYPE(fc) & 0x08));
+        */
+	// fixed by David :2006.9.6
+	qc_included = ((WLAN_FC_GET_TYPE(fc) == IEEE80211_FTYPE_DATA) &&
+		       (WLAN_FC_GET_STYPE(fc) & 0x80));
+	aad_len = 22;
+	if (a4_included)
+		aad_len += 6;
+	if (qc_included) {
+		pos = (u8 *) &hdr->addr4;
+		if (a4_included)
+			pos += 6;
+		qc = *pos & 0x0f;
+		aad_len += 2;
+	}
+	/* CCM Initial Block:
+	 * Flag (Include authentication header, M=3 (8-octet MIC),
+	 *       L=1 (2-octet Dlen))
+	 * Nonce: 0x00 | A2 | PN
+	 * Dlen */
+	b0[0] = 0x59;
+	b0[1] = qc;
+	memcpy(b0 + 2, hdr->addr2, ETH_ALEN);
+	memcpy(b0 + 8, pn, CCMP_PN_LEN);
+	b0[14] = (dlen >> 8) & 0xff;
+	b0[15] = dlen & 0xff;
+
+	/* AAD:
+	 * FC with bits 4..6 and 11..13 masked to zero; 14 is always one
+	 * A1 | A2 | A3
+	 * SC with bits 4..15 (seq#) masked to zero
+	 * A4 (if present)
+	 * QC (if present)
+	 */
+	pos = (u8 *) hdr;
+	aad[0] = 0; /* aad_len >> 8 */
+	aad[1] = aad_len & 0xff;
+	aad[2] = pos[0] & 0x8f;
+	aad[3] = pos[1] & 0xc7;
+	memcpy(aad + 4, hdr->addr1, 3 * ETH_ALEN);
+	pos = (u8 *) &hdr->seq_ctl;
+	aad[22] = pos[0] & 0x0f;
+	aad[23] = 0; /* all bits masked */
+	memset(aad + 24, 0, 8);
+	if (a4_included)
+		memcpy(aad + 24, hdr->addr4, ETH_ALEN);
+	if (qc_included) {
+		aad[a4_included ? 30 : 24] = qc;
+		/* rest of QC masked */
+	}
+
+	/* Start with the first block and AAD */
+	ieee80211_ccmp_aes_encrypt(tfm, b0, auth);
+	xor_block(auth, aad, AES_BLOCK_LEN);
+	ieee80211_ccmp_aes_encrypt(tfm, auth, auth);
+	xor_block(auth, &aad[AES_BLOCK_LEN], AES_BLOCK_LEN);
+	ieee80211_ccmp_aes_encrypt(tfm, auth, auth);
+	b0[0] &= 0x07;
+	b0[14] = b0[15] = 0;
+	ieee80211_ccmp_aes_encrypt(tfm, b0, s0);
+}
+#endif
+
+static int ieee80211_ccmp_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
+{
+	struct ieee80211_ccmp_data *key = priv;
+	int data_len, i;
+	u8 *pos;
+	struct ieee80211_hdr *hdr;
+#ifndef JOHN_CCMP
+	int blocks, last, len;
+	u8 *mic;
+	u8 *b0 = key->tx_b0;
+	u8 *b = key->tx_b;
+	u8 *e = key->tx_e;
+	u8 *s0 = key->tx_s0;
+#endif
+	if (skb_headroom(skb) < CCMP_HDR_LEN ||
+	    skb_tailroom(skb) < CCMP_MIC_LEN ||
+	    skb->len < hdr_len)
+		return -1;
+
+	data_len = skb->len - hdr_len;
+	pos = skb_push(skb, CCMP_HDR_LEN);
+	memmove(pos, pos + CCMP_HDR_LEN, hdr_len);
+	pos += hdr_len;
+//	mic = skb_put(skb, CCMP_MIC_LEN);
+
+	i = CCMP_PN_LEN - 1;
+	while (i >= 0) {
+		key->tx_pn[i]++;
+		if (key->tx_pn[i] != 0)
+			break;
+		i--;
+	}
+
+	*pos++ = key->tx_pn[5];
+	*pos++ = key->tx_pn[4];
+	*pos++ = 0;
+	*pos++ = (key->key_idx << 6) | (1 << 5) /* Ext IV included */;
+	*pos++ = key->tx_pn[3];
+	*pos++ = key->tx_pn[2];
+	*pos++ = key->tx_pn[1];
+	*pos++ = key->tx_pn[0];
+
+	hdr = (struct ieee80211_hdr *) skb->data;
+#ifndef JOHN_CCMP
+	//mic is moved to here by john
+	mic = skb_put(skb, CCMP_MIC_LEN);
+
+	ccmp_init_blocks(key->tfm, hdr, key->tx_pn, data_len, b0, b, s0);
+
+	blocks = (data_len + AES_BLOCK_LEN - 1) / AES_BLOCK_LEN;
+	last = data_len % AES_BLOCK_LEN;
+
+	for (i = 1; i <= blocks; i++) {
+		len = (i == blocks && last) ? last : AES_BLOCK_LEN;
+		/* Authentication */
+		xor_block(b, pos, len);
+		ieee80211_ccmp_aes_encrypt(key->tfm, b, b);
+		/* Encryption, with counter */
+		b0[14] = (i >> 8) & 0xff;
+		b0[15] = i & 0xff;
+		ieee80211_ccmp_aes_encrypt(key->tfm, b0, e);
+		xor_block(pos, e, len);
+		pos += len;
+	}
+
+	for (i = 0; i < CCMP_MIC_LEN; i++)
+		mic[i] = b[i] ^ s0[i];
+#endif
+	return 0;
+}
+
+
+static int ieee80211_ccmp_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
+{
+	struct ieee80211_ccmp_data *key = priv;
+	u8 keyidx, *pos;
+	struct ieee80211_hdr *hdr;
+	u8 pn[6];
+#ifndef JOHN_CCMP
+	size_t data_len = skb->len - hdr_len - CCMP_HDR_LEN - CCMP_MIC_LEN;
+	u8 *mic = skb->data + skb->len - CCMP_MIC_LEN;
+	u8 *b0 = key->rx_b0;
+	u8 *b = key->rx_b;
+	u8 *a = key->rx_a;
+	int i, blocks, last, len;
+#endif
+	if (skb->len < hdr_len + CCMP_HDR_LEN + CCMP_MIC_LEN) {
+		key->dot11RSNAStatsCCMPFormatErrors++;
+		return -1;
+	}
+
+	hdr = (struct ieee80211_hdr *) skb->data;
+	pos = skb->data + hdr_len;
+	keyidx = pos[3];
+	if (!(keyidx & (1 << 5))) {
+		if (net_ratelimit()) {
+			printk(KERN_DEBUG "CCMP: received packet without ExtIV"
+			       " flag from " MAC_FMT "\n", MAC_ARG(hdr->addr2));
+		}
+		key->dot11RSNAStatsCCMPFormatErrors++;
+		return -2;
+	}
+	keyidx >>= 6;
+	if (key->key_idx != keyidx) {
+		printk(KERN_DEBUG "CCMP: RX tkey->key_idx=%d frame "
+		       "keyidx=%d priv=%p\n", key->key_idx, keyidx, priv);
+		return -6;
+	}
+	if (!key->key_set) {
+		if (net_ratelimit()) {
+			printk(KERN_DEBUG "CCMP: received packet from " MAC_FMT
+			       " with keyid=%d that does not have a configured"
+			       " key\n", MAC_ARG(hdr->addr2), keyidx);
+		}
+		return -3;
+	}
+
+	pn[0] = pos[7];
+	pn[1] = pos[6];
+	pn[2] = pos[5];
+	pn[3] = pos[4];
+	pn[4] = pos[1];
+	pn[5] = pos[0];
+	pos += 8;
+
+	if (memcmp(pn, key->rx_pn, CCMP_PN_LEN) <= 0) {
+		if (net_ratelimit()) {
+			printk(KERN_DEBUG "CCMP: replay detected: STA=" MAC_FMT
+			       " previous PN %02x%02x%02x%02x%02x%02x "
+			       "received PN %02x%02x%02x%02x%02x%02x\n",
+			       MAC_ARG(hdr->addr2), MAC_ARG(key->rx_pn),
+			       MAC_ARG(pn));
+		}
+		key->dot11RSNAStatsCCMPReplays++;
+		return -4;
+	}
+
+#ifndef JOHN_CCMP
+	ccmp_init_blocks(key->tfm, hdr, pn, data_len, b0, a, b);
+	xor_block(mic, b, CCMP_MIC_LEN);
+
+	blocks = (data_len + AES_BLOCK_LEN - 1) / AES_BLOCK_LEN;
+	last = data_len % AES_BLOCK_LEN;
+
+	for (i = 1; i <= blocks; i++) {
+		len = (i == blocks && last) ? last : AES_BLOCK_LEN;
+		/* Decrypt, with counter */
+		b0[14] = (i >> 8) & 0xff;
+		b0[15] = i & 0xff;
+		ieee80211_ccmp_aes_encrypt(key->tfm, b0, b);
+		xor_block(pos, b, len);
+		/* Authentication */
+		xor_block(a, pos, len);
+		ieee80211_ccmp_aes_encrypt(key->tfm, a, a);
+		pos += len;
+	}
+
+	if (memcmp(mic, a, CCMP_MIC_LEN) != 0) {
+		if (net_ratelimit()) {
+			printk(KERN_DEBUG "CCMP: decrypt failed: STA="
+			       MAC_FMT "\n", MAC_ARG(hdr->addr2));
+		}
+		key->dot11RSNAStatsCCMPDecryptErrors++;
+		return -5;
+	}
+
+	memcpy(key->rx_pn, pn, CCMP_PN_LEN);
+
+#endif
+	/* Remove hdr and MIC */
+	memmove(skb->data + CCMP_HDR_LEN, skb->data, hdr_len);
+	skb_pull(skb, CCMP_HDR_LEN);
+	skb_trim(skb, skb->len - CCMP_MIC_LEN);
+
+	return keyidx;
+}
+
+
+static int ieee80211_ccmp_set_key(void *key, int len, u8 *seq, void *priv)
+{
+	struct ieee80211_ccmp_data *data = priv;
+	int keyidx;
+	struct crypto_tfm *tfm = data->tfm;
+
+	keyidx = data->key_idx;
+	memset(data, 0, sizeof(*data));
+	data->key_idx = keyidx;
+	data->tfm = tfm;
+	if (len == CCMP_TK_LEN) {
+		memcpy(data->key, key, CCMP_TK_LEN);
+		data->key_set = 1;
+		if (seq) {
+			data->rx_pn[0] = seq[5];
+			data->rx_pn[1] = seq[4];
+			data->rx_pn[2] = seq[3];
+			data->rx_pn[3] = seq[2];
+			data->rx_pn[4] = seq[1];
+			data->rx_pn[5] = seq[0];
+		}
+		crypto_cipher_setkey((void *)data->tfm, data->key, CCMP_TK_LEN);
+	} else if (len == 0)
+		data->key_set = 0;
+	else
+		return -1;
+
+	return 0;
+}
+
+
+static int ieee80211_ccmp_get_key(void *key, int len, u8 *seq, void *priv)
+{
+	struct ieee80211_ccmp_data *data = priv;
+
+	if (len < CCMP_TK_LEN)
+		return -1;
+
+	if (!data->key_set)
+		return 0;
+	memcpy(key, data->key, CCMP_TK_LEN);
+
+	if (seq) {
+		seq[0] = data->tx_pn[5];
+		seq[1] = data->tx_pn[4];
+		seq[2] = data->tx_pn[3];
+		seq[3] = data->tx_pn[2];
+		seq[4] = data->tx_pn[1];
+		seq[5] = data->tx_pn[0];
+	}
+
+	return CCMP_TK_LEN;
+}
+
+
+static char * ieee80211_ccmp_print_stats(char *p, void *priv)
+{
+	struct ieee80211_ccmp_data *ccmp = priv;
+	p += sprintf(p, "key[%d] alg=CCMP key_set=%d "
+		     "tx_pn=%02x%02x%02x%02x%02x%02x "
+		     "rx_pn=%02x%02x%02x%02x%02x%02x "
+		     "format_errors=%d replays=%d decrypt_errors=%d\n",
+		     ccmp->key_idx, ccmp->key_set,
+		     MAC_ARG(ccmp->tx_pn), MAC_ARG(ccmp->rx_pn),
+		     ccmp->dot11RSNAStatsCCMPFormatErrors,
+		     ccmp->dot11RSNAStatsCCMPReplays,
+		     ccmp->dot11RSNAStatsCCMPDecryptErrors);
+
+	return p;
+}
+
+void ieee80211_ccmp_null(void)
+{
+//    printk("============>%s()\n", __FUNCTION__);
+	return;
+}
+static struct ieee80211_crypto_ops ieee80211_crypt_ccmp = {
+	.name			= "CCMP",
+	.init			= ieee80211_ccmp_init,
+	.deinit			= ieee80211_ccmp_deinit,
+	.encrypt_mpdu		= ieee80211_ccmp_encrypt,
+	.decrypt_mpdu		= ieee80211_ccmp_decrypt,
+	.encrypt_msdu		= NULL,
+	.decrypt_msdu		= NULL,
+	.set_key		= ieee80211_ccmp_set_key,
+	.get_key		= ieee80211_ccmp_get_key,
+	.print_stats		= ieee80211_ccmp_print_stats,
+	.extra_prefix_len	= CCMP_HDR_LEN,
+	.extra_postfix_len	= CCMP_MIC_LEN,
+	.owner			= THIS_MODULE,
+};
+
+
+int ieee80211_crypto_ccmp_init(void)
+{
+	return ieee80211_register_crypto_ops(&ieee80211_crypt_ccmp);
+}
+
+
+void ieee80211_crypto_ccmp_exit(void)
+{
+	ieee80211_unregister_crypto_ops(&ieee80211_crypt_ccmp);
+}
+
+#if 0
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,5,0))
+EXPORT_SYMBOL(ieee80211_ccmp_null);
+#else
+EXPORT_SYMBOL_NOVERS(ieee80211_ccmp_null);
+#endif
+#endif
+
+//module_init(ieee80211_crypto_ccmp_init);
+//module_exit(ieee80211_crypto_ccmp_exit);
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt_tkip.c b/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt_tkip.c
new file mode 100644
index 00000000000..e560b1e1102
--- /dev/null
+++ b/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt_tkip.c
@@ -0,0 +1,1001 @@
+/*
+ * Host AP crypt: host-based TKIP encryption implementation for Host AP driver
+ *
+ * Copyright (c) 2003-2004, Jouni Malinen <jkmaline@cc.hut.fi>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation. See README and COPYING for
+ * more details.
+ */
+
+//#include <linux/config.h>
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/random.h>
+#include <linux/skbuff.h>
+#include <linux/netdevice.h>
+#include <linux/if_ether.h>
+#include <linux/if_arp.h>
+#include <asm/string.h>
+
+#include "ieee80211.h"
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0))
+#include "rtl_crypto.h"
+#else
+#include <linux/crypto.h>
+#endif
+//#include <asm/scatterlist.h>
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20))
+    #include <asm/scatterlist.h>
+#else
+    #include <linux/scatterlist.h>
+#endif
+
+#include <linux/crc32.h>
+
+MODULE_AUTHOR("Jouni Malinen");
+MODULE_DESCRIPTION("Host AP crypt: TKIP");
+MODULE_LICENSE("GPL");
+
+#ifdef OPENSUSE_SLED
+#ifndef IN_OPENSUSE_SLED
+#define IN_OPENSUSE_SLED 1
+#endif
+#endif
+
+struct ieee80211_tkip_data {
+#define TKIP_KEY_LEN 32
+	u8 key[TKIP_KEY_LEN];
+	int key_set;
+
+	u32 tx_iv32;
+	u16 tx_iv16;
+	u16 tx_ttak[5];
+	int tx_phase1_done;
+
+	u32 rx_iv32;
+	u16 rx_iv16;
+	u16 rx_ttak[5];
+	int rx_phase1_done;
+	u32 rx_iv32_new;
+	u16 rx_iv16_new;
+
+	u32 dot11RSNAStatsTKIPReplays;
+	u32 dot11RSNAStatsTKIPICVErrors;
+	u32 dot11RSNAStatsTKIPLocalMICFailures;
+
+	int key_idx;
+
+       #if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,21))||(IN_OPENSUSE_SLED))
+	   	struct crypto_blkcipher *rx_tfm_arc4;
+	       struct crypto_hash *rx_tfm_michael;
+	       struct crypto_blkcipher *tx_tfm_arc4;
+	       struct crypto_hash *tx_tfm_michael;
+       #endif
+
+	struct crypto_tfm *tfm_arc4;
+	struct crypto_tfm *tfm_michael;
+
+	/* scratch buffers for virt_to_page() (crypto API) */
+	u8 rx_hdr[16], tx_hdr[16];
+};
+
+static void * ieee80211_tkip_init(int key_idx)
+{
+	struct ieee80211_tkip_data *priv;
+
+	priv = kmalloc(sizeof(*priv), GFP_ATOMIC);
+	if (priv == NULL)
+		goto fail;
+	memset(priv, 0, sizeof(*priv));
+	priv->key_idx = key_idx;
+
+      #if((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))&&(!IN_OPENSUSE_SLED))
+	priv->tfm_arc4 = crypto_alloc_tfm("arc4", 0);
+	if (priv->tfm_arc4 == NULL) {
+		printk(KERN_DEBUG "ieee80211_crypt_tkip: could not allocate "
+		       "crypto API arc4\n");
+		goto fail;
+	}
+
+	priv->tfm_michael = crypto_alloc_tfm("michael_mic", 0);
+	if (priv->tfm_michael == NULL) {
+		printk(KERN_DEBUG "ieee80211_crypt_tkip: could not allocate "
+		       "crypto API michael_mic\n");
+		goto fail;
+	}
+
+	#else
+	priv->tx_tfm_arc4 = crypto_alloc_blkcipher("ecb(arc4)", 0,
+						CRYPTO_ALG_ASYNC);
+	if (IS_ERR(priv->tx_tfm_arc4)) {
+		printk(KERN_DEBUG "ieee80211_crypt_tkip: could not allocate "
+		       "crypto API arc4\n");
+		priv->tx_tfm_arc4 = NULL;
+		goto fail;
+	}
+
+	priv->tx_tfm_michael = crypto_alloc_hash("michael_mic", 0,
+						 CRYPTO_ALG_ASYNC);
+	if (IS_ERR(priv->tx_tfm_michael)) {
+		printk(KERN_DEBUG "ieee80211_crypt_tkip: could not allocate "
+		       "crypto API michael_mic\n");
+		priv->tx_tfm_michael = NULL;
+		goto fail;
+	}
+
+	priv->rx_tfm_arc4 = crypto_alloc_blkcipher("ecb(arc4)", 0,
+						CRYPTO_ALG_ASYNC);
+	if (IS_ERR(priv->rx_tfm_arc4)) {
+		printk(KERN_DEBUG "ieee80211_crypt_tkip: could not allocate "
+		       "crypto API arc4\n");
+		priv->rx_tfm_arc4 = NULL;
+		goto fail;
+	}
+
+	priv->rx_tfm_michael = crypto_alloc_hash("michael_mic", 0,
+						 CRYPTO_ALG_ASYNC);
+	if (IS_ERR(priv->rx_tfm_michael)) {
+		printk(KERN_DEBUG "ieee80211_crypt_tkip: could not allocate "
+		       "crypto API michael_mic\n");
+		priv->rx_tfm_michael = NULL;
+		goto fail;
+	}
+       #endif
+	return priv;
+
+fail:
+	if (priv) {
+		#if((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))&&(!IN_OPENSUSE_SLED))
+		if (priv->tfm_michael)
+			crypto_free_tfm(priv->tfm_michael);
+		if (priv->tfm_arc4)
+			crypto_free_tfm(priv->tfm_arc4);
+             #else
+		if (priv->tx_tfm_michael)
+			crypto_free_hash(priv->tx_tfm_michael);
+		if (priv->tx_tfm_arc4)
+			crypto_free_blkcipher(priv->tx_tfm_arc4);
+		if (priv->rx_tfm_michael)
+			crypto_free_hash(priv->rx_tfm_michael);
+		if (priv->rx_tfm_arc4)
+			crypto_free_blkcipher(priv->rx_tfm_arc4);
+		#endif
+		kfree(priv);
+	}
+
+	return NULL;
+}
+
+
+static void ieee80211_tkip_deinit(void *priv)
+{
+	struct ieee80211_tkip_data *_priv = priv;
+	#if((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))&&(!IN_OPENSUSE_SLED))
+	if (_priv && _priv->tfm_michael)
+		crypto_free_tfm(_priv->tfm_michael);
+	if (_priv && _priv->tfm_arc4)
+		crypto_free_tfm(_priv->tfm_arc4);
+	#else
+	if (_priv) {
+		if (_priv->tx_tfm_michael)
+			crypto_free_hash(_priv->tx_tfm_michael);
+		if (_priv->tx_tfm_arc4)
+			crypto_free_blkcipher(_priv->tx_tfm_arc4);
+		if (_priv->rx_tfm_michael)
+			crypto_free_hash(_priv->rx_tfm_michael);
+		if (_priv->rx_tfm_arc4)
+			crypto_free_blkcipher(_priv->rx_tfm_arc4);
+	}
+	#endif
+	kfree(priv);
+}
+
+
+static inline u16 RotR1(u16 val)
+{
+	return (val >> 1) | (val << 15);
+}
+
+
+static inline u8 Lo8(u16 val)
+{
+	return val & 0xff;
+}
+
+
+static inline u8 Hi8(u16 val)
+{
+	return val >> 8;
+}
+
+
+static inline u16 Lo16(u32 val)
+{
+	return val & 0xffff;
+}
+
+
+static inline u16 Hi16(u32 val)
+{
+	return val >> 16;
+}
+
+
+static inline u16 Mk16(u8 hi, u8 lo)
+{
+	return lo | (((u16) hi) << 8);
+}
+
+
+static inline u16 Mk16_le(u16 *v)
+{
+	return le16_to_cpu(*v);
+}
+
+
+static const u16 Sbox[256] =
+{
+	0xC6A5, 0xF884, 0xEE99, 0xF68D, 0xFF0D, 0xD6BD, 0xDEB1, 0x9154,
+	0x6050, 0x0203, 0xCEA9, 0x567D, 0xE719, 0xB562, 0x4DE6, 0xEC9A,
+	0x8F45, 0x1F9D, 0x8940, 0xFA87, 0xEF15, 0xB2EB, 0x8EC9, 0xFB0B,
+	0x41EC, 0xB367, 0x5FFD, 0x45EA, 0x23BF, 0x53F7, 0xE496, 0x9B5B,
+	0x75C2, 0xE11C, 0x3DAE, 0x4C6A, 0x6C5A, 0x7E41, 0xF502, 0x834F,
+	0x685C, 0x51F4, 0xD134, 0xF908, 0xE293, 0xAB73, 0x6253, 0x2A3F,
+	0x080C, 0x9552, 0x4665, 0x9D5E, 0x3028, 0x37A1, 0x0A0F, 0x2FB5,
+	0x0E09, 0x2436, 0x1B9B, 0xDF3D, 0xCD26, 0x4E69, 0x7FCD, 0xEA9F,
+	0x121B, 0x1D9E, 0x5874, 0x342E, 0x362D, 0xDCB2, 0xB4EE, 0x5BFB,
+	0xA4F6, 0x764D, 0xB761, 0x7DCE, 0x527B, 0xDD3E, 0x5E71, 0x1397,
+	0xA6F5, 0xB968, 0x0000, 0xC12C, 0x4060, 0xE31F, 0x79C8, 0xB6ED,
+	0xD4BE, 0x8D46, 0x67D9, 0x724B, 0x94DE, 0x98D4, 0xB0E8, 0x854A,
+	0xBB6B, 0xC52A, 0x4FE5, 0xED16, 0x86C5, 0x9AD7, 0x6655, 0x1194,
+	0x8ACF, 0xE910, 0x0406, 0xFE81, 0xA0F0, 0x7844, 0x25BA, 0x4BE3,
+	0xA2F3, 0x5DFE, 0x80C0, 0x058A, 0x3FAD, 0x21BC, 0x7048, 0xF104,
+	0x63DF, 0x77C1, 0xAF75, 0x4263, 0x2030, 0xE51A, 0xFD0E, 0xBF6D,
+	0x814C, 0x1814, 0x2635, 0xC32F, 0xBEE1, 0x35A2, 0x88CC, 0x2E39,
+	0x9357, 0x55F2, 0xFC82, 0x7A47, 0xC8AC, 0xBAE7, 0x322B, 0xE695,
+	0xC0A0, 0x1998, 0x9ED1, 0xA37F, 0x4466, 0x547E, 0x3BAB, 0x0B83,
+	0x8CCA, 0xC729, 0x6BD3, 0x283C, 0xA779, 0xBCE2, 0x161D, 0xAD76,
+	0xDB3B, 0x6456, 0x744E, 0x141E, 0x92DB, 0x0C0A, 0x486C, 0xB8E4,
+	0x9F5D, 0xBD6E, 0x43EF, 0xC4A6, 0x39A8, 0x31A4, 0xD337, 0xF28B,
+	0xD532, 0x8B43, 0x6E59, 0xDAB7, 0x018C, 0xB164, 0x9CD2, 0x49E0,
+	0xD8B4, 0xACFA, 0xF307, 0xCF25, 0xCAAF, 0xF48E, 0x47E9, 0x1018,
+	0x6FD5, 0xF088, 0x4A6F, 0x5C72, 0x3824, 0x57F1, 0x73C7, 0x9751,
+	0xCB23, 0xA17C, 0xE89C, 0x3E21, 0x96DD, 0x61DC, 0x0D86, 0x0F85,
+	0xE090, 0x7C42, 0x71C4, 0xCCAA, 0x90D8, 0x0605, 0xF701, 0x1C12,
+	0xC2A3, 0x6A5F, 0xAEF9, 0x69D0, 0x1791, 0x9958, 0x3A27, 0x27B9,
+	0xD938, 0xEB13, 0x2BB3, 0x2233, 0xD2BB, 0xA970, 0x0789, 0x33A7,
+	0x2DB6, 0x3C22, 0x1592, 0xC920, 0x8749, 0xAAFF, 0x5078, 0xA57A,
+	0x038F, 0x59F8, 0x0980, 0x1A17, 0x65DA, 0xD731, 0x84C6, 0xD0B8,
+	0x82C3, 0x29B0, 0x5A77, 0x1E11, 0x7BCB, 0xA8FC, 0x6DD6, 0x2C3A,
+};
+
+
+static inline u16 _S_(u16 v)
+{
+	u16 t = Sbox[Hi8(v)];
+	return Sbox[Lo8(v)] ^ ((t << 8) | (t >> 8));
+}
+
+#ifndef JOHN_TKIP
+#define PHASE1_LOOP_COUNT 8
+
+static void tkip_mixing_phase1(u16 *TTAK, const u8 *TK, const u8 *TA, u32 IV32)
+{
+	int i, j;
+
+	/* Initialize the 80-bit TTAK from TSC (IV32) and TA[0..5] */
+	TTAK[0] = Lo16(IV32);
+	TTAK[1] = Hi16(IV32);
+	TTAK[2] = Mk16(TA[1], TA[0]);
+	TTAK[3] = Mk16(TA[3], TA[2]);
+	TTAK[4] = Mk16(TA[5], TA[4]);
+
+	for (i = 0; i < PHASE1_LOOP_COUNT; i++) {
+		j = 2 * (i & 1);
+		TTAK[0] += _S_(TTAK[4] ^ Mk16(TK[1 + j], TK[0 + j]));
+		TTAK[1] += _S_(TTAK[0] ^ Mk16(TK[5 + j], TK[4 + j]));
+		TTAK[2] += _S_(TTAK[1] ^ Mk16(TK[9 + j], TK[8 + j]));
+		TTAK[3] += _S_(TTAK[2] ^ Mk16(TK[13 + j], TK[12 + j]));
+		TTAK[4] += _S_(TTAK[3] ^ Mk16(TK[1 + j], TK[0 + j])) + i;
+	}
+}
+
+
+static void tkip_mixing_phase2(u8 *WEPSeed, const u8 *TK, const u16 *TTAK,
+			       u16 IV16)
+{
+	/* Make temporary area overlap WEP seed so that the final copy can be
+	 * avoided on little endian hosts. */
+	u16 *PPK = (u16 *) &WEPSeed[4];
+
+	/* Step 1 - make copy of TTAK and bring in TSC */
+	PPK[0] = TTAK[0];
+	PPK[1] = TTAK[1];
+	PPK[2] = TTAK[2];
+	PPK[3] = TTAK[3];
+	PPK[4] = TTAK[4];
+	PPK[5] = TTAK[4] + IV16;
+
+	/* Step 2 - 96-bit bijective mixing using S-box */
+	PPK[0] += _S_(PPK[5] ^ Mk16_le((u16 *) &TK[0]));
+	PPK[1] += _S_(PPK[0] ^ Mk16_le((u16 *) &TK[2]));
+	PPK[2] += _S_(PPK[1] ^ Mk16_le((u16 *) &TK[4]));
+	PPK[3] += _S_(PPK[2] ^ Mk16_le((u16 *) &TK[6]));
+	PPK[4] += _S_(PPK[3] ^ Mk16_le((u16 *) &TK[8]));
+	PPK[5] += _S_(PPK[4] ^ Mk16_le((u16 *) &TK[10]));
+
+	PPK[0] += RotR1(PPK[5] ^ Mk16_le((u16 *) &TK[12]));
+	PPK[1] += RotR1(PPK[0] ^ Mk16_le((u16 *) &TK[14]));
+	PPK[2] += RotR1(PPK[1]);
+	PPK[3] += RotR1(PPK[2]);
+	PPK[4] += RotR1(PPK[3]);
+	PPK[5] += RotR1(PPK[4]);
+
+	/* Step 3 - bring in last of TK bits, assign 24-bit WEP IV value
+	 * WEPSeed[0..2] is transmitted as WEP IV */
+	WEPSeed[0] = Hi8(IV16);
+	WEPSeed[1] = (Hi8(IV16) | 0x20) & 0x7F;
+	WEPSeed[2] = Lo8(IV16);
+	WEPSeed[3] = Lo8((PPK[5] ^ Mk16_le((u16 *) &TK[0])) >> 1);
+
+#ifdef __BIG_ENDIAN
+	{
+		int i;
+		for (i = 0; i < 6; i++)
+			PPK[i] = (PPK[i] << 8) | (PPK[i] >> 8);
+	}
+#endif
+}
+#endif
+static int ieee80211_tkip_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
+{
+        struct ieee80211_tkip_data *tkey = priv;
+        #if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,21))||(IN_OPENSUSE_SLED))
+        struct blkcipher_desc desc = {.tfm = tkey->tx_tfm_arc4};
+        #endif
+	int len;
+	u8  *pos;
+	struct ieee80211_hdr *hdr;
+#ifndef JOHN_TKIP
+	u8 rc4key[16],*icv;
+	u32 crc;
+	struct scatterlist sg;
+#endif
+	int ret;
+
+	ret = 0;
+	if (skb_headroom(skb) < 8 || skb_tailroom(skb) < 4 ||
+	    skb->len < hdr_len)
+		return -1;
+
+	hdr = (struct ieee80211_hdr *) skb->data;
+#if 0
+printk("@@ tkey\n");
+printk("%x|", ((u32*)tkey->key)[0]);
+printk("%x|", ((u32*)tkey->key)[1]);
+printk("%x|", ((u32*)tkey->key)[2]);
+printk("%x|", ((u32*)tkey->key)[3]);
+printk("%x|", ((u32*)tkey->key)[4]);
+printk("%x|", ((u32*)tkey->key)[5]);
+printk("%x|", ((u32*)tkey->key)[6]);
+printk("%x\n", ((u32*)tkey->key)[7]);
+#endif
+
+#ifndef JOHN_TKIP
+	if (!tkey->tx_phase1_done) {
+		tkip_mixing_phase1(tkey->tx_ttak, tkey->key, hdr->addr2,
+				   tkey->tx_iv32);
+		tkey->tx_phase1_done = 1;
+	}
+	tkip_mixing_phase2(rc4key, tkey->key, tkey->tx_ttak, tkey->tx_iv16);
+
+#else
+	tkey->tx_phase1_done = 1;
+#endif  /*JOHN_TKIP*/
+
+	len = skb->len - hdr_len;
+	pos = skb_push(skb, 8);
+	memmove(pos, pos + 8, hdr_len);
+	pos += hdr_len;
+
+#ifdef JOHN_TKIP
+	*pos++ = Hi8(tkey->tx_iv16);
+	*pos++ = (Hi8(tkey->tx_iv16) | 0x20) & 0x7F;
+	*pos++ = Lo8(tkey->tx_iv16);
+#else
+	*pos++ = rc4key[0];
+	*pos++ = rc4key[1];
+	*pos++ = rc4key[2];
+#endif
+	*pos++ = (tkey->key_idx << 6) | (1 << 5) /* Ext IV included */;
+	*pos++ = tkey->tx_iv32 & 0xff;
+	*pos++ = (tkey->tx_iv32 >> 8) & 0xff;
+	*pos++ = (tkey->tx_iv32 >> 16) & 0xff;
+	*pos++ = (tkey->tx_iv32 >> 24) & 0xff;
+#ifndef JOHN_TKIP
+	icv = skb_put(skb, 4);
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,5,0))
+	crc = ~crc32_le(~0, pos, len);
+#else
+	crc = ~ether_crc_le(len, pos);
+#endif
+	icv[0] = crc;
+	icv[1] = crc >> 8;
+	icv[2] = crc >> 16;
+	icv[3] = crc >> 24;
+      #if((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))&&(!IN_OPENSUSE_SLED))
+	crypto_cipher_setkey(tkey->tfm_arc4, rc4key, 16);
+	sg.page = virt_to_page(pos);
+	sg.offset = offset_in_page(pos);
+	sg.length = len + 4;
+	crypto_cipher_encrypt(tkey->tfm_arc4, &sg, &sg, len + 4);
+      #else
+	crypto_blkcipher_setkey(tkey->tx_tfm_arc4, rc4key, 16);
+        #if(LINUX_VERSION_CODE < KERNEL_VERSION(2,6,24))
+          sg.page = virt_to_page(pos);
+          sg.offset = offset_in_page(pos);
+          sg.length = len + 4;
+        #else
+          sg_init_one(&sg, pos, len+4);
+        #endif
+	ret= crypto_blkcipher_encrypt(&desc, &sg, &sg, len + 4);
+      #endif
+#endif
+	tkey->tx_iv16++;
+	if (tkey->tx_iv16 == 0) {
+		tkey->tx_phase1_done = 0;
+		tkey->tx_iv32++;
+	}
+#ifndef JOHN_TKIP
+      #if((LINUX_VERSION_CODE <KERNEL_VERSION(2,6,21))&&(!IN_OPENSUSE_SLED))
+	   return 0;
+      #else
+	   return ret;
+      #endif
+#else
+	return 0;
+#endif
+}
+
+static int ieee80211_tkip_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
+{
+        struct ieee80211_tkip_data *tkey = priv;
+        #if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,21)) ||(IN_OPENSUSE_SLED))
+        struct blkcipher_desc desc = {.tfm = tkey->rx_tfm_arc4};
+        #endif
+	u8 keyidx, *pos;
+	u32 iv32;
+	u16 iv16;
+	struct ieee80211_hdr *hdr;
+#ifndef JOHN_TKIP
+	u8 icv[4];
+	u32 crc;
+	struct scatterlist sg;
+	u8 rc4key[16];
+	int plen;
+#endif
+	if (skb->len < hdr_len + 8 + 4)
+		return -1;
+
+	hdr = (struct ieee80211_hdr *) skb->data;
+	pos = skb->data + hdr_len;
+	keyidx = pos[3];
+	if (!(keyidx & (1 << 5))) {
+		if (net_ratelimit()) {
+			printk(KERN_DEBUG "TKIP: received packet without ExtIV"
+			       " flag from " MAC_FMT "\n", MAC_ARG(hdr->addr2));
+		}
+		return -2;
+	}
+	keyidx >>= 6;
+	if (tkey->key_idx != keyidx) {
+		printk(KERN_DEBUG "TKIP: RX tkey->key_idx=%d frame "
+		       "keyidx=%d priv=%p\n", tkey->key_idx, keyidx, priv);
+		return -6;
+	}
+	if (!tkey->key_set) {
+		if (net_ratelimit()) {
+			printk(KERN_DEBUG "TKIP: received packet from " MAC_FMT
+			       " with keyid=%d that does not have a configured"
+			       " key\n", MAC_ARG(hdr->addr2), keyidx);
+		}
+		return -3;
+	}
+	iv16 = (pos[0] << 8) | pos[2];
+	iv32 = pos[4] | (pos[5] << 8) | (pos[6] << 16) | (pos[7] << 24);
+	pos += 8;
+#ifndef JOHN_TKIP
+
+	if (iv32 < tkey->rx_iv32 ||
+	    (iv32 == tkey->rx_iv32 && iv16 <= tkey->rx_iv16)) {
+		if (net_ratelimit()) {
+			printk(KERN_DEBUG "TKIP: replay detected: STA=" MAC_FMT
+			       " previous TSC %08x%04x received TSC "
+			       "%08x%04x\n", MAC_ARG(hdr->addr2),
+			       tkey->rx_iv32, tkey->rx_iv16, iv32, iv16);
+		}
+		tkey->dot11RSNAStatsTKIPReplays++;
+		return -4;
+	}
+
+	if (iv32 != tkey->rx_iv32 || !tkey->rx_phase1_done) {
+		tkip_mixing_phase1(tkey->rx_ttak, tkey->key, hdr->addr2, iv32);
+		tkey->rx_phase1_done = 1;
+	}
+	tkip_mixing_phase2(rc4key, tkey->key, tkey->rx_ttak, iv16);
+
+	plen = skb->len - hdr_len - 12;
+       #if((LINUX_VERSION_CODE <KERNEL_VERSION(2,6,21))&&(!IN_OPENSUSE_SLED))
+	crypto_cipher_setkey(tkey->tfm_arc4, rc4key, 16);
+	sg.page = virt_to_page(pos);
+	sg.offset = offset_in_page(pos);
+	sg.length = plen + 4;
+	crypto_cipher_decrypt(tkey->tfm_arc4, &sg, &sg, plen + 4);
+	#else
+	crypto_blkcipher_setkey(tkey->rx_tfm_arc4, rc4key, 16);
+        #if(LINUX_VERSION_CODE <KERNEL_VERSION(2,6,24))
+          sg.page = virt_to_page(pos);
+          sg.offset = offset_in_page(pos);
+          sg.length = plen + 4;
+        #else
+          sg_init_one(&sg, pos, plen+4);
+        #endif
+	if (crypto_blkcipher_decrypt(&desc, &sg, &sg, plen + 4)) {
+		if (net_ratelimit()) {
+			printk(KERN_DEBUG ": TKIP: failed to decrypt "
+			       "received packet from " MAC_FMT "\n",
+			       MAC_ARG(hdr->addr2));
+		}
+		return -7;
+	}
+	#endif
+
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,5,0))
+	crc = ~crc32_le(~0, pos, plen);
+#else
+	crc = ~ether_crc_le(plen, pos);
+#endif
+	icv[0] = crc;
+	icv[1] = crc >> 8;
+	icv[2] = crc >> 16;
+	icv[3] = crc >> 24;
+	if (memcmp(icv, pos + plen, 4) != 0) {
+		if (iv32 != tkey->rx_iv32) {
+			/* Previously cached Phase1 result was already lost, so
+			 * it needs to be recalculated for the next packet. */
+			tkey->rx_phase1_done = 0;
+		}
+		if (net_ratelimit()) {
+			printk(KERN_DEBUG "TKIP: ICV error detected: STA="
+			       MAC_FMT "\n", MAC_ARG(hdr->addr2));
+		}
+		tkey->dot11RSNAStatsTKIPICVErrors++;
+		return -5;
+	}
+
+#endif 	/* JOHN_TKIP */
+
+	/* Update real counters only after Michael MIC verification has
+	 * completed */
+	tkey->rx_iv32_new = iv32;
+	tkey->rx_iv16_new = iv16;
+
+	/* Remove IV and ICV */
+	memmove(skb->data + 8, skb->data, hdr_len);
+	skb_pull(skb, 8);
+	skb_trim(skb, skb->len - 4);
+
+//john's test
+#ifdef JOHN_DUMP
+if( ((u16*)skb->data)[0] & 0x4000){
+        printk("@@ rx decrypted skb->data");
+        int i;
+        for(i=0;i<skb->len;i++){
+                if( (i%24)==0 ) printk("\n");
+                printk("%2x ", ((u8*)skb->data)[i]);
+        }
+        printk("\n");
+}
+#endif /*JOHN_DUMP*/
+	return keyidx;
+}
+
+#if((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21)) && (!IN_OPENSUSE_SLED))
+static int michael_mic(struct ieee80211_tkip_data *tkey, u8 *key, u8 *hdr,
+		       u8 *data, size_t data_len, u8 *mic)
+{
+	struct scatterlist sg[2];
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20)
+	struct hash_desc desc;
+	int ret=0;
+#endif
+	if (tkey->tfm_michael == NULL) {
+		printk(KERN_WARNING "michael_mic: tfm_michael == NULL\n");
+		return -1;
+	}
+	sg[0].page = virt_to_page(hdr);
+	sg[0].offset = offset_in_page(hdr);
+	sg[0].length = 16;
+
+	sg[1].page = virt_to_page(data);
+	sg[1].offset = offset_in_page(data);
+	sg[1].length = data_len;
+
+	//crypto_digest_init(tkey->tfm_michael);
+	//crypto_digest_setkey(tkey->tfm_michael, key, 8);
+	//crypto_digest_update(tkey->tfm_michael, sg, 2);
+	//crypto_digest_final(tkey->tfm_michael, mic);
+
+	//return 0;
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+	crypto_digest_init(tkey->tfm_michael);
+	crypto_digest_setkey(tkey->tfm_michael, key, 8);
+	crypto_digest_update(tkey->tfm_michael, sg, 2);
+	crypto_digest_final(tkey->tfm_michael, mic);
+
+	return 0;
+#else
+if (crypto_hash_setkey(tkey->tfm_michael, key, 8))
+		return -1;
+
+//	return 0;
+	desc.tfm = tkey->tfm_michael;
+	desc.flags = 0;
+	ret = crypto_hash_digest(&desc, sg, data_len + 16, mic);
+	return ret;
+#endif
+}
+#else
+static int michael_mic(struct crypto_hash *tfm_michael, u8 * key, u8 * hdr,
+                       u8 * data, size_t data_len, u8 * mic)
+{
+        struct hash_desc desc;
+        struct scatterlist sg[2];
+
+        if (tfm_michael == NULL) {
+                printk(KERN_WARNING "michael_mic: tfm_michael == NULL\n");
+                return -1;
+        }
+        #if(LINUX_VERSION_CODE < KERNEL_VERSION(2,6,24))
+          sg[0].page = virt_to_page(hdr);
+          sg[0].offset = offset_in_page(hdr);
+          sg[0].length = 16;
+          sg[1].page = virt_to_page(data);
+          sg[1].offset = offset_in_page(data);
+          sg[1].length = data_len;
+        #else
+          sg_init_table(sg, 2);
+          sg_set_buf(&sg[0], hdr, 16);
+          sg_set_buf(&sg[1], data, data_len);
+        #endif
+
+        if (crypto_hash_setkey(tfm_michael, key, 8))
+                return -1;
+
+        desc.tfm = tfm_michael;
+        desc.flags = 0;
+        return crypto_hash_digest(&desc, sg, data_len + 16, mic);
+}
+#endif
+
+
+
+static void michael_mic_hdr(struct sk_buff *skb, u8 *hdr)
+{
+	struct ieee80211_hdr *hdr11;
+
+	hdr11 = (struct ieee80211_hdr *) skb->data;
+	switch (le16_to_cpu(hdr11->frame_ctl) &
+		(IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS)) {
+	case IEEE80211_FCTL_TODS:
+		memcpy(hdr, hdr11->addr3, ETH_ALEN); /* DA */
+		memcpy(hdr + ETH_ALEN, hdr11->addr2, ETH_ALEN); /* SA */
+		break;
+	case IEEE80211_FCTL_FROMDS:
+		memcpy(hdr, hdr11->addr1, ETH_ALEN); /* DA */
+		memcpy(hdr + ETH_ALEN, hdr11->addr3, ETH_ALEN); /* SA */
+		break;
+	case IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS:
+		memcpy(hdr, hdr11->addr3, ETH_ALEN); /* DA */
+		memcpy(hdr + ETH_ALEN, hdr11->addr4, ETH_ALEN); /* SA */
+		break;
+	case 0:
+		memcpy(hdr, hdr11->addr1, ETH_ALEN); /* DA */
+		memcpy(hdr + ETH_ALEN, hdr11->addr2, ETH_ALEN); /* SA */
+		break;
+	}
+
+	hdr[12] = 0; /* priority */
+
+	hdr[13] = hdr[14] = hdr[15] = 0; /* reserved */
+}
+
+
+static int ieee80211_michael_mic_add(struct sk_buff *skb, int hdr_len, void *priv)
+{
+	struct ieee80211_tkip_data *tkey = priv;
+	u8 *pos;
+	struct ieee80211_hdr *hdr;
+
+	hdr = (struct ieee80211_hdr *) skb->data;
+
+	if (skb_tailroom(skb) < 8 || skb->len < hdr_len) {
+		printk(KERN_DEBUG "Invalid packet for Michael MIC add "
+		       "(tailroom=%d hdr_len=%d skb->len=%d)\n",
+		       skb_tailroom(skb), hdr_len, skb->len);
+		return -1;
+	}
+
+	michael_mic_hdr(skb, tkey->tx_hdr);
+
+	// { david, 2006.9.1
+	// fix the wpa process with wmm enabled.
+	if(IEEE80211_QOS_HAS_SEQ(le16_to_cpu(hdr->frame_ctl))) {
+		tkey->tx_hdr[12] = *(skb->data + hdr_len - 2) & 0x07;
+	}
+	// }
+	pos = skb_put(skb, 8);
+        #if((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))&&(!IN_OPENSUSE_SLED))
+	if (michael_mic(tkey, &tkey->key[16], tkey->tx_hdr,
+			skb->data + hdr_len, skb->len - 8 - hdr_len, pos))
+        #else
+        if (michael_mic(tkey->tx_tfm_michael, &tkey->key[16], tkey->tx_hdr,
+                        skb->data + hdr_len, skb->len - 8 - hdr_len, pos))
+        #endif
+		return -1;
+
+	return 0;
+}
+
+
+#if WIRELESS_EXT >= 18
+static void ieee80211_michael_mic_failure(struct net_device *dev,
+				       struct ieee80211_hdr *hdr,
+				       int keyidx)
+{
+	union iwreq_data wrqu;
+	struct iw_michaelmicfailure ev;
+
+	/* TODO: needed parameters: count, keyid, key type, TSC */
+	memset(&ev, 0, sizeof(ev));
+	ev.flags = keyidx & IW_MICFAILURE_KEY_ID;
+	if (hdr->addr1[0] & 0x01)
+		ev.flags |= IW_MICFAILURE_GROUP;
+	else
+		ev.flags |= IW_MICFAILURE_PAIRWISE;
+	ev.src_addr.sa_family = ARPHRD_ETHER;
+	memcpy(ev.src_addr.sa_data, hdr->addr2, ETH_ALEN);
+	memset(&wrqu, 0, sizeof(wrqu));
+	wrqu.data.length = sizeof(ev);
+	wireless_send_event(dev, IWEVMICHAELMICFAILURE, &wrqu, (char *) &ev);
+}
+#elif WIRELESS_EXT >= 15
+static void ieee80211_michael_mic_failure(struct net_device *dev,
+				       struct ieee80211_hdr *hdr,
+				       int keyidx)
+{
+	union iwreq_data wrqu;
+	char buf[128];
+
+	/* TODO: needed parameters: count, keyid, key type, TSC */
+	sprintf(buf, "MLME-MICHAELMICFAILURE.indication(keyid=%d %scast addr="
+		MAC_FMT ")", keyidx, hdr->addr1[0] & 0x01 ? "broad" : "uni",
+		MAC_ARG(hdr->addr2));
+	memset(&wrqu, 0, sizeof(wrqu));
+	wrqu.data.length = strlen(buf);
+	wireless_send_event(dev, IWEVCUSTOM, &wrqu, buf);
+}
+#else /* WIRELESS_EXT >= 15 */
+static inline void ieee80211_michael_mic_failure(struct net_device *dev,
+					      struct ieee80211_hdr *hdr,
+					      int keyidx)
+{
+}
+#endif /* WIRELESS_EXT >= 15 */
+
+
+static int ieee80211_michael_mic_verify(struct sk_buff *skb, int keyidx,
+				     int hdr_len, void *priv)
+{
+	struct ieee80211_tkip_data *tkey = priv;
+	u8 mic[8];
+	struct ieee80211_hdr *hdr;
+
+	hdr = (struct ieee80211_hdr *) skb->data;
+
+	if (!tkey->key_set)
+		return -1;
+
+	michael_mic_hdr(skb, tkey->rx_hdr);
+	// { david, 2006.9.1
+	// fix the wpa process with wmm enabled.
+	if(IEEE80211_QOS_HAS_SEQ(le16_to_cpu(hdr->frame_ctl))) {
+		tkey->rx_hdr[12] = *(skb->data + hdr_len - 2) & 0x07;
+	}
+	// }
+        #if((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))&&(!IN_OPENSUSE_SLED))
+	if (michael_mic(tkey, &tkey->key[24], tkey->rx_hdr,
+			skb->data + hdr_len, skb->len - 8 - hdr_len, mic))
+        #else
+	if (michael_mic(tkey->rx_tfm_michael, &tkey->key[24], tkey->rx_hdr,
+                        skb->data + hdr_len, skb->len - 8 - hdr_len, mic))
+        #endif
+            	return -1;
+	if (memcmp(mic, skb->data + skb->len - 8, 8) != 0) {
+		struct ieee80211_hdr *hdr;
+		hdr = (struct ieee80211_hdr *) skb->data;
+		printk(KERN_DEBUG "%s: Michael MIC verification failed for "
+		       "MSDU from " MAC_FMT " keyidx=%d\n",
+		       skb->dev ? skb->dev->name : "N/A", MAC_ARG(hdr->addr2),
+		       keyidx);
+		if (skb->dev)
+			ieee80211_michael_mic_failure(skb->dev, hdr, keyidx);
+		tkey->dot11RSNAStatsTKIPLocalMICFailures++;
+		return -1;
+	}
+
+	/* Update TSC counters for RX now that the packet verification has
+	 * completed. */
+	tkey->rx_iv32 = tkey->rx_iv32_new;
+	tkey->rx_iv16 = tkey->rx_iv16_new;
+
+	skb_trim(skb, skb->len - 8);
+
+	return 0;
+}
+
+
+static int ieee80211_tkip_set_key(void *key, int len, u8 *seq, void *priv)
+{
+	struct ieee80211_tkip_data *tkey = priv;
+	int keyidx;
+	#if ((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))&&(!IN_OPENSUSE_SLED))
+	struct crypto_tfm *tfm = tkey->tfm_michael;
+	struct crypto_tfm *tfm2 = tkey->tfm_arc4;
+	#else
+	struct crypto_hash *tfm = tkey->tx_tfm_michael;
+	struct crypto_blkcipher *tfm2 = tkey->tx_tfm_arc4;
+	struct crypto_hash *tfm3 = tkey->rx_tfm_michael;
+	struct crypto_blkcipher *tfm4 = tkey->rx_tfm_arc4;
+	#endif
+
+	keyidx = tkey->key_idx;
+	memset(tkey, 0, sizeof(*tkey));
+	tkey->key_idx = keyidx;
+
+	#if((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))&&(!IN_OPENSUSE_SLED))
+	tkey->tfm_michael = tfm;
+	tkey->tfm_arc4 = tfm2;
+       #else
+	tkey->tx_tfm_michael = tfm;
+	tkey->tx_tfm_arc4 = tfm2;
+	tkey->rx_tfm_michael = tfm3;
+	tkey->rx_tfm_arc4 = tfm4;
+	#endif
+
+	if (len == TKIP_KEY_LEN) {
+		memcpy(tkey->key, key, TKIP_KEY_LEN);
+		tkey->key_set = 1;
+		tkey->tx_iv16 = 1; /* TSC is initialized to 1 */
+		if (seq) {
+			tkey->rx_iv32 = (seq[5] << 24) | (seq[4] << 16) |
+				(seq[3] << 8) | seq[2];
+			tkey->rx_iv16 = (seq[1] << 8) | seq[0];
+		}
+	} else if (len == 0)
+		tkey->key_set = 0;
+	else
+		return -1;
+
+	return 0;
+}
+
+
+static int ieee80211_tkip_get_key(void *key, int len, u8 *seq, void *priv)
+{
+	struct ieee80211_tkip_data *tkey = priv;
+
+	if (len < TKIP_KEY_LEN)
+		return -1;
+
+	if (!tkey->key_set)
+		return 0;
+	memcpy(key, tkey->key, TKIP_KEY_LEN);
+
+	if (seq) {
+		/* Return the sequence number of the last transmitted frame. */
+		u16 iv16 = tkey->tx_iv16;
+		u32 iv32 = tkey->tx_iv32;
+		if (iv16 == 0)
+			iv32--;
+		iv16--;
+		seq[0] = tkey->tx_iv16;
+		seq[1] = tkey->tx_iv16 >> 8;
+		seq[2] = tkey->tx_iv32;
+		seq[3] = tkey->tx_iv32 >> 8;
+		seq[4] = tkey->tx_iv32 >> 16;
+		seq[5] = tkey->tx_iv32 >> 24;
+	}
+
+	return TKIP_KEY_LEN;
+}
+
+
+static char * ieee80211_tkip_print_stats(char *p, void *priv)
+{
+	struct ieee80211_tkip_data *tkip = priv;
+	p += sprintf(p, "key[%d] alg=TKIP key_set=%d "
+		     "tx_pn=%02x%02x%02x%02x%02x%02x "
+		     "rx_pn=%02x%02x%02x%02x%02x%02x "
+		     "replays=%d icv_errors=%d local_mic_failures=%d\n",
+		     tkip->key_idx, tkip->key_set,
+		     (tkip->tx_iv32 >> 24) & 0xff,
+		     (tkip->tx_iv32 >> 16) & 0xff,
+		     (tkip->tx_iv32 >> 8) & 0xff,
+		     tkip->tx_iv32 & 0xff,
+		     (tkip->tx_iv16 >> 8) & 0xff,
+		     tkip->tx_iv16 & 0xff,
+		     (tkip->rx_iv32 >> 24) & 0xff,
+		     (tkip->rx_iv32 >> 16) & 0xff,
+		     (tkip->rx_iv32 >> 8) & 0xff,
+		     tkip->rx_iv32 & 0xff,
+		     (tkip->rx_iv16 >> 8) & 0xff,
+		     tkip->rx_iv16 & 0xff,
+		     tkip->dot11RSNAStatsTKIPReplays,
+		     tkip->dot11RSNAStatsTKIPICVErrors,
+		     tkip->dot11RSNAStatsTKIPLocalMICFailures);
+	return p;
+}
+
+
+static struct ieee80211_crypto_ops ieee80211_crypt_tkip = {
+	.name			= "TKIP",
+	.init			= ieee80211_tkip_init,
+	.deinit			= ieee80211_tkip_deinit,
+	.encrypt_mpdu		= ieee80211_tkip_encrypt,
+	.decrypt_mpdu		= ieee80211_tkip_decrypt,
+	.encrypt_msdu		= ieee80211_michael_mic_add,
+	.decrypt_msdu		= ieee80211_michael_mic_verify,
+	.set_key		= ieee80211_tkip_set_key,
+	.get_key		= ieee80211_tkip_get_key,
+	.print_stats		= ieee80211_tkip_print_stats,
+	.extra_prefix_len	= 4 + 4, /* IV + ExtIV */
+	.extra_postfix_len	= 8 + 4, /* MIC + ICV */
+	.owner		        = THIS_MODULE,
+};
+
+
+int ieee80211_crypto_tkip_init(void)
+{
+	return ieee80211_register_crypto_ops(&ieee80211_crypt_tkip);
+}
+
+
+void ieee80211_crypto_tkip_exit(void)
+{
+	ieee80211_unregister_crypto_ops(&ieee80211_crypt_tkip);
+}
+
+
+void ieee80211_tkip_null(void)
+{
+//    printk("============>%s()\n", __FUNCTION__);
+        return;
+}
+
+#if 0
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,5,0))
+EXPORT_SYMBOL(ieee80211_tkip_null);
+#else
+EXPORT_SYMBOL_NOVERS(ieee80211_tkip_null);
+#endif
+#endif
+
+
+//module_init(ieee80211_crypto_tkip_init);
+//module_exit(ieee80211_crypto_tkip_exit);
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt_wep.c b/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt_wep.c
new file mode 100644
index 00000000000..f973daeeeb0
--- /dev/null
+++ b/drivers/staging/rtl8187se/ieee80211/ieee80211_crypt_wep.c
@@ -0,0 +1,394 @@
+/*
+ * Host AP crypt: host-based WEP encryption implementation for Host AP driver
+ *
+ * Copyright (c) 2002-2004, Jouni Malinen <jkmaline@cc.hut.fi>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation. See README and COPYING for
+ * more details.
+ */
+
+//#include <linux/config.h>
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/random.h>
+#include <linux/skbuff.h>
+#include <asm/string.h>
+
+#include "ieee80211.h"
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0))
+#include "rtl_crypto.h"
+#else
+#include <linux/crypto.h>
+#endif
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+    #include <asm/scatterlist.h>
+#else
+    #include <linux/scatterlist.h>
+#endif
+//#include <asm/scatterlist.h>
+#include <linux/crc32.h>
+
+MODULE_AUTHOR("Jouni Malinen");
+MODULE_DESCRIPTION("Host AP crypt: WEP");
+MODULE_LICENSE("GPL");
+
+#ifdef OPENSUSE_SLED
+#ifndef IN_OPENSUSE_SLED
+#define IN_OPENSUSE_SLED 1
+#endif
+#endif
+
+
+struct prism2_wep_data {
+	u32 iv;
+#define WEP_KEY_LEN 13
+	u8 key[WEP_KEY_LEN + 1];
+	u8 key_len;
+	u8 key_idx;
+	#if((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))&&(!IN_OPENSUSE_SLED))
+	struct crypto_tfm *tfm;
+	#else
+	struct crypto_blkcipher *tx_tfm;
+	struct crypto_blkcipher *rx_tfm;
+	#endif
+};
+
+
+static void * prism2_wep_init(int keyidx)
+{
+	struct prism2_wep_data *priv;
+
+	priv = kmalloc(sizeof(*priv), GFP_ATOMIC);
+	if (priv == NULL)
+		goto fail;
+	memset(priv, 0, sizeof(*priv));
+	priv->key_idx = keyidx;
+	#if((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))&&(!IN_OPENSUSE_SLED))
+	priv->tfm = crypto_alloc_tfm("arc4", 0);
+     	if (priv->tfm == NULL) {
+		printk(KERN_DEBUG "ieee80211_crypt_wep: could not allocate "
+		       "crypto API arc4\n");
+		goto fail;
+	}
+	#else
+	priv->tx_tfm = crypto_alloc_blkcipher("ecb(arc4)", 0, CRYPTO_ALG_ASYNC);
+	if (IS_ERR(priv->tx_tfm)) {
+		printk(KERN_DEBUG "ieee80211_crypt_wep: could not allocate "
+		       "crypto API arc4\n");
+		priv->tx_tfm = NULL;
+		goto fail;
+	}
+	priv->rx_tfm = crypto_alloc_blkcipher("ecb(arc4)", 0, CRYPTO_ALG_ASYNC);
+	if (IS_ERR(priv->rx_tfm)) {
+		printk(KERN_DEBUG "ieee80211_crypt_wep: could not allocate "
+		       "crypto API arc4\n");
+		priv->rx_tfm = NULL;
+		goto fail;
+	}
+	#endif
+
+	/* start WEP IV from a random value */
+	get_random_bytes(&priv->iv, 4);
+
+	return priv;
+
+fail:
+	//#if(LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))
+	#if((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))&&(!IN_OPENSUSE_SLED))
+	if (priv) {
+		if (priv->tfm)
+			crypto_free_tfm(priv->tfm);
+		kfree(priv);
+	}
+       #else
+	if (priv) {
+		if (priv->tx_tfm)
+			crypto_free_blkcipher(priv->tx_tfm);
+		if (priv->rx_tfm)
+			crypto_free_blkcipher(priv->rx_tfm);
+		kfree(priv);
+	}
+	#endif
+	return NULL;
+}
+
+
+static void prism2_wep_deinit(void *priv)
+{
+	struct prism2_wep_data *_priv = priv;
+	//#if(LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))
+	#if((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))&&(!IN_OPENSUSE_SLED))
+	if (_priv && _priv->tfm)
+		crypto_free_tfm(_priv->tfm);
+	#else
+	if (_priv) {
+		if (_priv->tx_tfm)
+			crypto_free_blkcipher(_priv->tx_tfm);
+		if (_priv->rx_tfm)
+			crypto_free_blkcipher(_priv->rx_tfm);
+	}
+        #endif
+	kfree(priv);
+}
+
+
+/* Perform WEP encryption on given skb that has at least 4 bytes of headroom
+ * for IV and 4 bytes of tailroom for ICV. Both IV and ICV will be transmitted,
+ * so the payload length increases with 8 bytes.
+ *
+ * WEP frame payload: IV + TX key idx, RC4(data), ICV = RC4(CRC32(data))
+ */
+static int prism2_wep_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
+{
+	struct prism2_wep_data *wep = priv;
+//#if(LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,21))
+#if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,21))||(IN_OPENSUSE_SLED))
+        struct blkcipher_desc desc = {.tfm = wep->tx_tfm};
+#endif
+	u32 klen, len;
+	u8 key[WEP_KEY_LEN + 3];
+	u8 *pos;
+#ifndef JOHN_HWSEC
+	u32 crc;
+	u8 *icv;
+	struct scatterlist sg;
+#endif
+	if (skb_headroom(skb) < 4 || skb_tailroom(skb) < 4 ||
+	    skb->len < hdr_len)
+		return -1;
+
+	len = skb->len - hdr_len;
+	pos = skb_push(skb, 4);
+	memmove(pos, pos + 4, hdr_len);
+	pos += hdr_len;
+
+	klen = 3 + wep->key_len;
+
+	wep->iv++;
+
+	/* Fluhrer, Mantin, and Shamir have reported weaknesses in the key
+	 * scheduling algorithm of RC4. At least IVs (KeyByte + 3, 0xff, N)
+	 * can be used to speedup attacks, so avoid using them. */
+	if ((wep->iv & 0xff00) == 0xff00) {
+		u8 B = (wep->iv >> 16) & 0xff;
+		if (B >= 3 && B < klen)
+			wep->iv += 0x0100;
+	}
+
+	/* Prepend 24-bit IV to RC4 key and TX frame */
+	*pos++ = key[0] = (wep->iv >> 16) & 0xff;
+	*pos++ = key[1] = (wep->iv >> 8) & 0xff;
+	*pos++ = key[2] = wep->iv & 0xff;
+	*pos++ = wep->key_idx << 6;
+
+	/* Copy rest of the WEP key (the secret part) */
+	memcpy(key + 3, wep->key, wep->key_len);
+
+#ifndef JOHN_HWSEC
+	/* Append little-endian CRC32 and encrypt it to produce ICV */
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,5,0))
+	crc = ~crc32_le(~0, pos, len);
+#else
+	crc = ~ether_crc_le(len, pos);
+#endif
+	icv = skb_put(skb, 4);
+	icv[0] = crc;
+	icv[1] = crc >> 8;
+	icv[2] = crc >> 16;
+	icv[3] = crc >> 24;
+
+        //#if(LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))
+	#if((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))&&(!IN_OPENSUSE_SLED))
+	crypto_cipher_setkey(wep->tfm, key, klen);
+	sg.page = virt_to_page(pos);
+	sg.offset = offset_in_page(pos);
+	sg.length = len + 4;
+	crypto_cipher_encrypt(wep->tfm, &sg, &sg, len + 4);
+
+	return 0;
+	#else
+	crypto_blkcipher_setkey(wep->tx_tfm, key, klen);
+        #if(LINUX_VERSION_CODE < KERNEL_VERSION(2,6,24))
+          sg.page = virt_to_page(pos);
+          sg.offset = offset_in_page(pos);
+          sg.length = len + 4;
+        #else
+          sg_init_one(&sg, pos, len+4);
+        #endif
+	return crypto_blkcipher_encrypt(&desc, &sg, &sg, len + 4);
+	#endif
+#endif /* JOHN_HWSEC */
+	return 0;
+}
+
+
+/* Perform WEP decryption on given buffer. Buffer includes whole WEP part of
+ * the frame: IV (4 bytes), encrypted payload (including SNAP header),
+ * ICV (4 bytes). len includes both IV and ICV.
+ *
+ * Returns 0 if frame was decrypted successfully and ICV was correct and -1 on
+ * failure. If frame is OK, IV and ICV will be removed.
+ */
+static int prism2_wep_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
+{
+	struct prism2_wep_data *wep = priv;
+        //#if(LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,21))
+	#if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,21))||(IN_OPENSUSE_SLED))
+        struct blkcipher_desc desc = {.tfm = wep->rx_tfm};
+        #endif
+	u32 klen, plen;
+	u8 key[WEP_KEY_LEN + 3];
+	u8 keyidx, *pos;
+#ifndef JOHN_HWSEC
+	u32 crc;
+	u8 icv[4];
+	struct scatterlist sg;
+#endif
+	if (skb->len < hdr_len + 8)
+		return -1;
+
+	pos = skb->data + hdr_len;
+	key[0] = *pos++;
+	key[1] = *pos++;
+	key[2] = *pos++;
+	keyidx = *pos++ >> 6;
+	if (keyidx != wep->key_idx)
+		return -1;
+
+	klen = 3 + wep->key_len;
+
+	/* Copy rest of the WEP key (the secret part) */
+	memcpy(key + 3, wep->key, wep->key_len);
+
+	/* Apply RC4 to data and compute CRC32 over decrypted data */
+	plen = skb->len - hdr_len - 8;
+#ifndef JOHN_HWSEC
+//#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))
+#if((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))&&(!IN_OPENSUSE_SLED))
+	crypto_cipher_setkey(wep->tfm, key, klen);
+	sg.page = virt_to_page(pos);
+	sg.offset = offset_in_page(pos);
+	sg.length = plen + 4;
+	crypto_cipher_decrypt(wep->tfm, &sg, &sg, plen + 4);
+#else
+	crypto_blkcipher_setkey(wep->rx_tfm, key, klen);
+        #if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,24))
+          sg.page = virt_to_page(pos);
+          sg.offset = offset_in_page(pos);
+          sg.length = plen + 4;
+        #else
+          sg_init_one(&sg, pos, plen+4);
+        #endif
+	if (crypto_blkcipher_decrypt(&desc, &sg, &sg, plen + 4))
+		return -7;
+#endif
+
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,5,0))
+	crc = ~crc32_le(~0, pos, plen);
+#else
+	crc = ~ether_crc_le(plen, pos);
+#endif
+	icv[0] = crc;
+	icv[1] = crc >> 8;
+	icv[2] = crc >> 16;
+	icv[3] = crc >> 24;
+
+	if (memcmp(icv, pos + plen, 4) != 0) {
+		/* ICV mismatch - drop frame */
+		return -2;
+	}
+#endif 	/* JOHN_HWSEC */
+
+	/* Remove IV and ICV */
+	memmove(skb->data + 4, skb->data, hdr_len);
+	skb_pull(skb, 4);
+	skb_trim(skb, skb->len - 4);
+        return 0;
+}
+
+
+static int prism2_wep_set_key(void *key, int len, u8 *seq, void *priv)
+{
+	struct prism2_wep_data *wep = priv;
+
+	if (len < 0 || len > WEP_KEY_LEN)
+		return -1;
+
+	memcpy(wep->key, key, len);
+	wep->key_len = len;
+
+	return 0;
+}
+
+
+static int prism2_wep_get_key(void *key, int len, u8 *seq, void *priv)
+{
+	struct prism2_wep_data *wep = priv;
+
+	if (len < wep->key_len)
+		return -1;
+
+	memcpy(key, wep->key, wep->key_len);
+
+	return wep->key_len;
+}
+
+
+static char * prism2_wep_print_stats(char *p, void *priv)
+{
+	struct prism2_wep_data *wep = priv;
+	p += sprintf(p, "key[%d] alg=WEP len=%d\n",
+		     wep->key_idx, wep->key_len);
+	return p;
+}
+
+
+static struct ieee80211_crypto_ops ieee80211_crypt_wep = {
+	.name			= "WEP",
+	.init			= prism2_wep_init,
+	.deinit			= prism2_wep_deinit,
+	.encrypt_mpdu		= prism2_wep_encrypt,
+	.decrypt_mpdu		= prism2_wep_decrypt,
+	.encrypt_msdu		= NULL,
+	.decrypt_msdu		= NULL,
+	.set_key		= prism2_wep_set_key,
+	.get_key		= prism2_wep_get_key,
+	.print_stats		= prism2_wep_print_stats,
+	.extra_prefix_len	= 4, /* IV */
+	.extra_postfix_len	= 4, /* ICV */
+	.owner			= THIS_MODULE,
+};
+
+
+int ieee80211_crypto_wep_init(void)
+{
+	return ieee80211_register_crypto_ops(&ieee80211_crypt_wep);
+}
+
+
+void ieee80211_crypto_wep_exit(void)
+{
+	ieee80211_unregister_crypto_ops(&ieee80211_crypt_wep);
+}
+
+
+void ieee80211_wep_null(void)
+{
+//	printk("============>%s()\n", __FUNCTION__);
+        return;
+}
+#if 0
+#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,5,0))
+EXPORT_SYMBOL(ieee80211_wep_null);
+#else
+EXPORT_SYMBOL_NOVERS(ieee80211_wep_null);
+#endif
+#endif
+//module_init(ieee80211_crypto_wep_init);
+//module_exit(ieee80211_crypto_wep_exit);
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211_module.c b/drivers/staging/rtl8187se/ieee80211/ieee80211_module.c
new file mode 100644
index 00000000000..0c9fef0b4e3
--- /dev/null
+++ b/drivers/staging/rtl8187se/ieee80211/ieee80211_module.c
@@ -0,0 +1,301 @@
+/*******************************************************************************
+
+  Copyright(c) 2004 Intel Corporation. All rights reserved.
+
+  Portions of this file are based on the WEP enablement code provided by the
+  Host AP project hostap-drivers v0.1.3
+  Copyright (c) 2001-2002, SSH Communications Security Corp and Jouni Malinen
+  <jkmaline@cc.hut.fi>
+  Copyright (c) 2002-2003, Jouni Malinen <jkmaline@cc.hut.fi>
+
+  This program is free software; you can redistribute it and/or modify it
+  under the terms of version 2 of the GNU General Public License as
+  published by the Free Software Foundation.
+
+  This program is distributed in the hope that it will be useful, but WITHOUT
+  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+  more details.
+
+  You should have received a copy of the GNU General Public License along with
+  this program; if not, write to the Free Software Foundation, Inc., 59
+  Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+
+  The full GNU General Public License is included in this distribution in the
+  file called LICENSE.
+
+  Contact Information:
+  James P. Ketrenos <ipw2100-admin@linux.intel.com>
+  Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+
+*******************************************************************************/
+
+#include <linux/compiler.h>
+//#include <linux/config.h>
+#include <linux/errno.h>
+#include <linux/if_arp.h>
+#include <linux/in6.h>
+#include <linux/in.h>
+#include <linux/ip.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/netdevice.h>
+#include <linux/pci.h>
+#include <linux/proc_fs.h>
+#include <linux/skbuff.h>
+#include <linux/slab.h>
+#include <linux/tcp.h>
+#include <linux/types.h>
+#include <linux/version.h>
+#include <linux/wireless.h>
+#include <linux/etherdevice.h>
+#include <asm/uaccess.h>
+#include <net/arp.h>
+#include <net/net_namespace.h>
+
+#include "ieee80211.h"
+
+MODULE_DESCRIPTION("802.11 data/management/control stack");
+MODULE_AUTHOR("Copyright (C) 2004 Intel Corporation <jketreno@linux.intel.com>");
+MODULE_LICENSE("GPL");
+
+#define DRV_NAME "ieee80211"
+
+static inline int ieee80211_networks_allocate(struct ieee80211_device *ieee)
+{
+	if (ieee->networks)
+		return 0;
+
+	ieee->networks = kmalloc(
+		MAX_NETWORK_COUNT * sizeof(struct ieee80211_network),
+		GFP_KERNEL);
+	if (!ieee->networks) {
+		printk(KERN_WARNING "%s: Out of memory allocating beacons\n",
+		       ieee->dev->name);
+		return -ENOMEM;
+	}
+
+	memset(ieee->networks, 0,
+	       MAX_NETWORK_COUNT * sizeof(struct ieee80211_network));
+
+	return 0;
+}
+
+static inline void ieee80211_networks_free(struct ieee80211_device *ieee)
+{
+	if (!ieee->networks)
+		return;
+	kfree(ieee->networks);
+	ieee->networks = NULL;
+}
+
+static inline void ieee80211_networks_initialize(struct ieee80211_device *ieee)
+{
+	int i;
+
+	INIT_LIST_HEAD(&ieee->network_free_list);
+	INIT_LIST_HEAD(&ieee->network_list);
+	for (i = 0; i < MAX_NETWORK_COUNT; i++)
+		list_add_tail(&ieee->networks[i].list, &ieee->network_free_list);
+}
+
+
+struct net_device *alloc_ieee80211(int sizeof_priv)
+{
+	struct ieee80211_device *ieee;
+	struct net_device *dev;
+	int i,err;
+
+	IEEE80211_DEBUG_INFO("Initializing...\n");
+
+	dev = alloc_etherdev(sizeof(struct ieee80211_device) + sizeof_priv);
+	if (!dev) {
+		IEEE80211_ERROR("Unable to network device.\n");
+		goto failed;
+	}
+	ieee = netdev_priv(dev);
+	dev->hard_start_xmit = ieee80211_xmit;
+
+	ieee->dev = dev;
+
+	err = ieee80211_networks_allocate(ieee);
+	if (err) {
+		IEEE80211_ERROR("Unable to allocate beacon storage: %d\n",
+				err);
+		goto failed;
+	}
+	ieee80211_networks_initialize(ieee);
+
+	/* Default fragmentation threshold is maximum payload size */
+	ieee->fts = DEFAULT_FTS;
+	ieee->scan_age = DEFAULT_MAX_SCAN_AGE;
+	ieee->open_wep = 1;
+
+	/* Default to enabling full open WEP with host based encrypt/decrypt */
+	ieee->host_encrypt = 1;
+	ieee->host_decrypt = 1;
+	ieee->ieee802_1x = 1; /* Default to supporting 802.1x */
+
+	INIT_LIST_HEAD(&ieee->crypt_deinit_list);
+	init_timer(&ieee->crypt_deinit_timer);
+	ieee->crypt_deinit_timer.data = (unsigned long)ieee;
+	ieee->crypt_deinit_timer.function = ieee80211_crypt_deinit_handler;
+
+	spin_lock_init(&ieee->lock);
+	spin_lock_init(&ieee->wpax_suitlist_lock);
+
+	ieee->wpax_type_set = 0;
+ 	ieee->wpa_enabled = 0;
+ 	ieee->tkip_countermeasures = 0;
+ 	ieee->drop_unencrypted = 0;
+ 	ieee->privacy_invoked = 0;
+ 	ieee->ieee802_1x = 1;
+	ieee->raw_tx = 0;
+
+	ieee80211_softmac_init(ieee);
+
+	for (i = 0; i < IEEE_IBSS_MAC_HASH_SIZE; i++)
+		INIT_LIST_HEAD(&ieee->ibss_mac_hash[i]);
+
+	for (i = 0; i < 17; i++) {
+	  ieee->last_rxseq_num[i] = -1;
+	  ieee->last_rxfrag_num[i] = -1;
+	  ieee->last_packet_time[i] = 0;
+	}
+//These function were added to load crypte module autoly
+	ieee80211_tkip_null();
+	ieee80211_wep_null();
+	ieee80211_ccmp_null();
+	return dev;
+
+ failed:
+	if (dev)
+		free_netdev(dev);
+	return NULL;
+}
+
+
+void free_ieee80211(struct net_device *dev)
+{
+	struct ieee80211_device *ieee = netdev_priv(dev);
+
+	int i;
+	struct list_head *p, *q;
+
+
+	ieee80211_softmac_free(ieee);
+	del_timer_sync(&ieee->crypt_deinit_timer);
+	ieee80211_crypt_deinit_entries(ieee, 1);
+
+	for (i = 0; i < WEP_KEYS; i++) {
+		struct ieee80211_crypt_data *crypt = ieee->crypt[i];
+		if (crypt) {
+			if (crypt->ops) {
+				crypt->ops->deinit(crypt->priv);
+				module_put(crypt->ops->owner);
+			}
+			kfree(crypt);
+			ieee->crypt[i] = NULL;
+		}
+	}
+
+	ieee80211_networks_free(ieee);
+
+	for (i = 0; i < IEEE_IBSS_MAC_HASH_SIZE; i++) {
+		list_for_each_safe(p, q, &ieee->ibss_mac_hash[i]) {
+			kfree(list_entry(p, struct ieee_ibss_seq, list));
+			list_del(p);
+		}
+	}
+
+
+	free_netdev(dev);
+}
+
+//#ifdef CONFIG_IEEE80211_DEBUG
+#if 0
+
+static int debug = 0;
+u32 ieee80211_debug_level = 0;
+struct proc_dir_entry *ieee80211_proc = NULL;
+
+static int show_debug_level(char *page, char **start, off_t offset,
+			    int count, int *eof, void *data)
+{
+	return snprintf(page, count, "0x%08X\n", ieee80211_debug_level);
+}
+
+static int store_debug_level(struct file *file, const char *buffer,
+			     unsigned long count, void *data)
+{
+	char buf[] = "0x00000000";
+	unsigned long len = min(sizeof(buf) - 1, (u32)count);
+	char *p = (char *)buf;
+	unsigned long val;
+
+	if (copy_from_user(buf, buffer, len))
+		return count;
+	buf[len] = 0;
+	if (p[1] == 'x' || p[1] == 'X' || p[0] == 'x' || p[0] == 'X') {
+		p++;
+		if (p[0] == 'x' || p[0] == 'X')
+			p++;
+		val = simple_strtoul(p, &p, 16);
+	} else
+		val = simple_strtoul(p, &p, 10);
+	if (p == buf)
+		printk(KERN_INFO DRV_NAME
+		       ": %s is not in hex or decimal form.\n", buf);
+	else
+		ieee80211_debug_level = val;
+
+	return strnlen(buf, count);
+}
+
+static int __init ieee80211_init(void)
+{
+	struct proc_dir_entry *e;
+
+	ieee80211_debug_level = debug;
+	ieee80211_proc = create_proc_entry(DRV_NAME, S_IFDIR, proc_net);
+	if (ieee80211_proc == NULL) {
+		IEEE80211_ERROR("Unable to create " DRV_NAME
+				" proc directory\n");
+		return -EIO;
+	}
+	e = create_proc_entry("debug_level", S_IFREG | S_IRUGO | S_IWUSR,
+			      ieee80211_proc);
+	if (!e) {
+		remove_proc_entry(DRV_NAME, proc_net);
+		ieee80211_proc = NULL;
+		return -EIO;
+	}
+	e->read_proc = show_debug_level;
+	e->write_proc = store_debug_level;
+	e->data = NULL;
+
+	return 0;
+}
+
+static void __exit ieee80211_exit(void)
+{
+	if (ieee80211_proc) {
+		remove_proc_entry("debug_level", ieee80211_proc);
+		remove_proc_entry(DRV_NAME, proc_net);
+		ieee80211_proc = NULL;
+	}
+}
+
+#include <linux/moduleparam.h>
+module_param(debug, int, 0444);
+MODULE_PARM_DESC(debug, "debug output mask");
+
+
+module_exit(ieee80211_exit);
+module_init(ieee80211_init);
+#endif
+
+#if 0
+EXPORT_SYMBOL(alloc_ieee80211);
+EXPORT_SYMBOL(free_ieee80211);
+#endif
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211_rx.c b/drivers/staging/rtl8187se/ieee80211/ieee80211_rx.c
new file mode 100644
index 00000000000..79ec64959cf
--- /dev/null
+++ b/drivers/staging/rtl8187se/ieee80211/ieee80211_rx.c
@@ -0,0 +1,1971 @@
+/*
+ * Original code based Host AP (software wireless LAN access point) driver
+ * for Intersil Prism2/2.5/3 - hostap.o module, common routines
+ *
+ * Copyright (c) 2001-2002, SSH Communications Security Corp and Jouni Malinen
+ * <jkmaline@cc.hut.fi>
+ * Copyright (c) 2002-2003, Jouni Malinen <jkmaline@cc.hut.fi>
+ * Copyright (c) 2004, Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation. See README and COPYING for
+ * more details.
+ ******************************************************************************
+
+  Few modifications for Realtek's Wi-Fi drivers by
+  Andrea Merello <andreamrl@tiscali.it>
+
+  A special thanks goes to Realtek for their support !
+
+******************************************************************************/
+
+
+#include <linux/compiler.h>
+//#include <linux/config.h>
+#include <linux/errno.h>
+#include <linux/if_arp.h>
+#include <linux/in6.h>
+#include <linux/in.h>
+#include <linux/ip.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/netdevice.h>
+#include <linux/pci.h>
+#include <linux/proc_fs.h>
+#include <linux/skbuff.h>
+#include <linux/slab.h>
+#include <linux/tcp.h>
+#include <linux/types.h>
+#include <linux/version.h>
+#include <linux/wireless.h>
+#include <linux/etherdevice.h>
+#include <asm/uaccess.h>
+#include <linux/ctype.h>
+
+#include "ieee80211.h"
+#ifdef ENABLE_DOT11D
+#include "dot11d.h"
+#endif
+static inline void ieee80211_monitor_rx(struct ieee80211_device *ieee,
+					struct sk_buff *skb,
+					struct ieee80211_rx_stats *rx_stats)
+{
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
+	u16 fc = le16_to_cpu(hdr->frame_ctl);
+
+	skb->dev = ieee->dev;
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,22)
+        skb_reset_mac_header(skb);
+#else
+        skb->mac.raw = skb->data;
+#endif
+	skb_pull(skb, ieee80211_get_hdrlen(fc));
+	skb->pkt_type = PACKET_OTHERHOST;
+	skb->protocol = __constant_htons(ETH_P_80211_RAW);
+	memset(skb->cb, 0, sizeof(skb->cb));
+	netif_rx(skb);
+}
+
+
+/* Called only as a tasklet (software IRQ) */
+static struct ieee80211_frag_entry *
+ieee80211_frag_cache_find(struct ieee80211_device *ieee, unsigned int seq,
+			  unsigned int frag, u8 tid,u8 *src, u8 *dst)
+{
+	struct ieee80211_frag_entry *entry;
+	int i;
+
+	for (i = 0; i < IEEE80211_FRAG_CACHE_LEN; i++) {
+		entry = &ieee->frag_cache[tid][i];
+		if (entry->skb != NULL &&
+		    time_after(jiffies, entry->first_frag_time + 2 * HZ)) {
+			IEEE80211_DEBUG_FRAG(
+				"expiring fragment cache entry "
+				"seq=%u last_frag=%u\n",
+				entry->seq, entry->last_frag);
+			dev_kfree_skb_any(entry->skb);
+			entry->skb = NULL;
+		}
+
+		if (entry->skb != NULL && entry->seq == seq &&
+		    (entry->last_frag + 1 == frag || frag == -1) &&
+		    memcmp(entry->src_addr, src, ETH_ALEN) == 0 &&
+		    memcmp(entry->dst_addr, dst, ETH_ALEN) == 0)
+			return entry;
+	}
+
+	return NULL;
+}
+
+/* Called only as a tasklet (software IRQ) */
+static struct sk_buff *
+ieee80211_frag_cache_get(struct ieee80211_device *ieee,
+			 struct ieee80211_hdr *hdr)
+{
+	struct sk_buff *skb = NULL;
+	u16 fc = le16_to_cpu(hdr->frame_ctl);
+	u16 sc = le16_to_cpu(hdr->seq_ctl);
+	unsigned int frag = WLAN_GET_SEQ_FRAG(sc);
+	unsigned int seq = WLAN_GET_SEQ_SEQ(sc);
+	struct ieee80211_frag_entry *entry;
+	struct ieee80211_hdr_3addr_QOS *hdr_3addr_QoS;
+	struct ieee80211_hdr_QOS *hdr_4addr_QoS;
+	u8 tid;
+
+#ifdef _RTL8187_EXT_PATCH_
+	if(ieee->iw_mode == ieee->iw_ext_mode)
+	{
+		tid = (hdr->addr2[ETH_ALEN-2] ^ hdr->addr2[ETH_ALEN-1]) & IEEE80211_QOS_TID;
+	}
+	else
+#endif
+	if (((fc & IEEE80211_FCTL_DSTODS) == IEEE80211_FCTL_DSTODS)&&IEEE80211_QOS_HAS_SEQ(fc)) {
+	  hdr_4addr_QoS = (struct ieee80211_hdr_QOS *)hdr;
+	  tid = le16_to_cpu(hdr_4addr_QoS->QOS_ctl) & IEEE80211_QOS_TID;
+	  tid = UP2AC(tid);
+	  tid ++;
+	} else if (IEEE80211_QOS_HAS_SEQ(fc)) {
+	  hdr_3addr_QoS = (struct ieee80211_hdr_3addr_QOS *)hdr;
+	  tid = le16_to_cpu(hdr_3addr_QoS->QOS_ctl) & IEEE80211_QOS_TID;
+	  tid = UP2AC(tid);
+	  tid ++;
+	} else {
+	  tid = 0;
+	}
+
+	if (frag == 0) {
+		/* Reserve enough space to fit maximum frame length */
+		skb = dev_alloc_skb(ieee->dev->mtu +
+				    sizeof(struct ieee80211_hdr) +
+				    8 /* LLC */ +
+				    2 /* alignment */ +
+				    8 /* WEP */ +
+				    ETH_ALEN /* WDS */ +
+				    (IEEE80211_QOS_HAS_SEQ(fc)?2:0) /* QOS Control */);
+		if (skb == NULL)
+			return NULL;
+
+		entry = &ieee->frag_cache[tid][ieee->frag_next_idx[tid]];
+		ieee->frag_next_idx[tid]++;
+		if (ieee->frag_next_idx[tid] >= IEEE80211_FRAG_CACHE_LEN)
+			ieee->frag_next_idx[tid] = 0;
+
+		if (entry->skb != NULL)
+			dev_kfree_skb_any(entry->skb);
+
+		entry->first_frag_time = jiffies;
+		entry->seq = seq;
+		entry->last_frag = frag;
+		entry->skb = skb;
+		memcpy(entry->src_addr, hdr->addr2, ETH_ALEN);
+		memcpy(entry->dst_addr, hdr->addr1, ETH_ALEN);
+	} else {
+		/* received a fragment of a frame for which the head fragment
+		 * should have already been received */
+		entry = ieee80211_frag_cache_find(ieee, seq, frag, tid,hdr->addr2,
+						  hdr->addr1);
+		if (entry != NULL) {
+			entry->last_frag = frag;
+			skb = entry->skb;
+		}
+	}
+
+	return skb;
+}
+
+
+/* Called only as a tasklet (software IRQ) */
+static int ieee80211_frag_cache_invalidate(struct ieee80211_device *ieee,
+					   struct ieee80211_hdr *hdr)
+{
+	u16 fc = le16_to_cpu(hdr->frame_ctl);
+	u16 sc = le16_to_cpu(hdr->seq_ctl);
+	unsigned int seq = WLAN_GET_SEQ_SEQ(sc);
+	struct ieee80211_frag_entry *entry;
+	struct ieee80211_hdr_3addr_QOS *hdr_3addr_QoS;
+	struct ieee80211_hdr_QOS *hdr_4addr_QoS;
+	u8 tid;
+
+#ifdef _RTL8187_EXT_PATCH_
+	if(ieee->iw_mode == ieee->iw_ext_mode)
+	{
+		tid = (hdr->addr2[ETH_ALEN-2] ^ hdr->addr2[ETH_ALEN-1]) & IEEE80211_QOS_TID;
+	}
+	else
+#endif
+	if(((fc & IEEE80211_FCTL_DSTODS) == IEEE80211_FCTL_DSTODS)&&IEEE80211_QOS_HAS_SEQ(fc)) {
+	  hdr_4addr_QoS = (struct ieee80211_hdr_QOS *)hdr;
+	  tid = le16_to_cpu(hdr_4addr_QoS->QOS_ctl) & IEEE80211_QOS_TID;
+	  tid = UP2AC(tid);
+	  tid ++;
+	} else if (IEEE80211_QOS_HAS_SEQ(fc)) {
+	  hdr_3addr_QoS = (struct ieee80211_hdr_3addr_QOS *)hdr;
+	  tid = le16_to_cpu(hdr_3addr_QoS->QOS_ctl) & IEEE80211_QOS_TID;
+	  tid = UP2AC(tid);
+	  tid ++;
+	} else {
+	  tid = 0;
+	}
+
+	entry = ieee80211_frag_cache_find(ieee, seq, -1, tid,hdr->addr2,
+					  hdr->addr1);
+
+	if (entry == NULL) {
+		IEEE80211_DEBUG_FRAG(
+			"could not invalidate fragment cache "
+			"entry (seq=%u)\n", seq);
+		return -1;
+	}
+
+	entry->skb = NULL;
+	return 0;
+}
+
+
+
+/* ieee80211_rx_frame_mgtmt
+ *
+ * Responsible for handling management control frames
+ *
+ * Called by ieee80211_rx */
+static inline int
+ieee80211_rx_frame_mgmt(struct ieee80211_device *ieee, struct sk_buff *skb,
+			struct ieee80211_rx_stats *rx_stats, u16 type,
+			u16 stype)
+{
+	struct ieee80211_hdr *hdr;
+
+	// cheat the the hdr type
+	hdr = (struct ieee80211_hdr *)skb->data;
+
+	/* On the struct stats definition there is written that
+	 * this is not mandatory.... but seems that the probe
+	 * response parser uses it
+	 */
+	rx_stats->len = skb->len;
+	ieee80211_rx_mgt(ieee,(struct ieee80211_hdr *)skb->data,rx_stats);
+
+	if((ieee->state == IEEE80211_LINKED)&&(memcmp(hdr->addr3,ieee->current_network.bssid,ETH_ALEN))) {
+		dev_kfree_skb_any(skb);
+		return 0;
+	}
+
+	ieee80211_rx_frame_softmac(ieee, skb, rx_stats, type, stype);
+
+	dev_kfree_skb_any(skb);
+
+	return 0;
+
+	#ifdef NOT_YET
+	if (ieee->iw_mode == IW_MODE_MASTER) {
+		printk(KERN_DEBUG "%s: Master mode not yet suppported.\n",
+		       ieee->dev->name);
+		return 0;
+/*
+  hostap_update_sta_ps(ieee, (struct hostap_ieee80211_hdr *)
+  skb->data);*/
+	}
+
+	if (ieee->hostapd && type == IEEE80211_TYPE_MGMT) {
+		if (stype == WLAN_FC_STYPE_BEACON &&
+		    ieee->iw_mode == IW_MODE_MASTER) {
+			struct sk_buff *skb2;
+			/* Process beacon frames also in kernel driver to
+			 * update STA(AP) table statistics */
+			skb2 = skb_clone(skb, GFP_ATOMIC);
+			if (skb2)
+				hostap_rx(skb2->dev, skb2, rx_stats);
+		}
+
+		/* send management frames to the user space daemon for
+		 * processing */
+		ieee->apdevstats.rx_packets++;
+		ieee->apdevstats.rx_bytes += skb->len;
+		prism2_rx_80211(ieee->apdev, skb, rx_stats, PRISM2_RX_MGMT);
+		return 0;
+	}
+
+	    if (ieee->iw_mode == IW_MODE_MASTER) {
+		if (type != WLAN_FC_TYPE_MGMT && type != WLAN_FC_TYPE_CTRL) {
+			printk(KERN_DEBUG "%s: unknown management frame "
+			       "(type=0x%02x, stype=0x%02x) dropped\n",
+			       skb->dev->name, type, stype);
+			return -1;
+		}
+
+		hostap_rx(skb->dev, skb, rx_stats);
+		return 0;
+	}
+
+	printk(KERN_DEBUG "%s: hostap_rx_frame_mgmt: management frame "
+	       "received in non-Host AP mode\n", skb->dev->name);
+	return -1;
+	#endif
+}
+
+
+
+/* See IEEE 802.1H for LLC/SNAP encapsulation/decapsulation */
+/* Ethernet-II snap header (RFC1042 for most EtherTypes) */
+static unsigned char rfc1042_header[] =
+{ 0xaa, 0xaa, 0x03, 0x00, 0x00, 0x00 };
+/* Bridge-Tunnel header (for EtherTypes ETH_P_AARP and ETH_P_IPX) */
+static unsigned char bridge_tunnel_header[] =
+{ 0xaa, 0xaa, 0x03, 0x00, 0x00, 0xf8 };
+/* No encapsulation header if EtherType < 0x600 (=length) */
+
+/* Called by ieee80211_rx_frame_decrypt */
+static int ieee80211_is_eapol_frame(struct ieee80211_device *ieee,
+				    struct sk_buff *skb, size_t hdrlen)
+{
+	struct net_device *dev = ieee->dev;
+	u16 fc, ethertype;
+	struct ieee80211_hdr *hdr;
+	u8 *pos;
+
+	if (skb->len < 24)
+		return 0;
+
+	hdr = (struct ieee80211_hdr *) skb->data;
+	fc = le16_to_cpu(hdr->frame_ctl);
+
+	/* check that the frame is unicast frame to us */
+	if ((fc & (IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS)) ==
+	    IEEE80211_FCTL_TODS &&
+	    memcmp(hdr->addr1, dev->dev_addr, ETH_ALEN) == 0 &&
+	    memcmp(hdr->addr3, dev->dev_addr, ETH_ALEN) == 0) {
+		/* ToDS frame with own addr BSSID and DA */
+	} else if ((fc & (IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS)) ==
+		   IEEE80211_FCTL_FROMDS &&
+		   memcmp(hdr->addr1, dev->dev_addr, ETH_ALEN) == 0) {
+		/* FromDS frame with own addr as DA */
+	} else
+		return 0;
+
+	if (skb->len < 24 + 8)
+		return 0;
+
+	/* check for port access entity Ethernet type */
+//	pos = skb->data + 24;
+	pos = skb->data + hdrlen;
+	ethertype = (pos[6] << 8) | pos[7];
+	if (ethertype == ETH_P_PAE)
+		return 1;
+
+	return 0;
+}
+
+/* Called only as a tasklet (software IRQ), by ieee80211_rx */
+static inline int
+ieee80211_rx_frame_decrypt(struct ieee80211_device* ieee, struct sk_buff *skb,
+			   struct ieee80211_crypt_data *crypt)
+{
+	struct ieee80211_hdr *hdr;
+	int res, hdrlen;
+
+	if (crypt == NULL || crypt->ops->decrypt_mpdu == NULL)
+		return 0;
+
+	hdr = (struct ieee80211_hdr *) skb->data;
+#ifdef _RTL8187_EXT_PATCH_
+	if((ieee->iw_mode == ieee->iw_ext_mode) && (ieee->ext_patch_ieee80211_rx_frame_get_hdrlen))
+	{
+		hdrlen = ieee->ext_patch_ieee80211_rx_frame_get_hdrlen(ieee, skb);
+	}
+	else
+#endif
+	hdrlen = ieee80211_get_hdrlen(le16_to_cpu(hdr->frame_ctl));
+
+#ifdef CONFIG_IEEE80211_CRYPT_TKIP
+	if (ieee->tkip_countermeasures &&
+	    strcmp(crypt->ops->name, "TKIP") == 0) {
+		if (net_ratelimit()) {
+			printk(KERN_DEBUG "%s: TKIP countermeasures: dropped "
+			       "received packet from " MAC_FMT "\n",
+			       ieee->dev->name, MAC_ARG(hdr->addr2));
+		}
+		return -1;
+	}
+#endif
+
+	atomic_inc(&crypt->refcnt);
+	res = crypt->ops->decrypt_mpdu(skb, hdrlen, crypt->priv);
+	atomic_dec(&crypt->refcnt);
+	if (res < 0) {
+		IEEE80211_DEBUG_DROP(
+			"decryption failed (SA=" MAC_FMT
+			") res=%d\n", MAC_ARG(hdr->addr2), res);
+		if (res == -2)
+			IEEE80211_DEBUG_DROP("Decryption failed ICV "
+					     "mismatch (key %d)\n",
+					     skb->data[hdrlen + 3] >> 6);
+		ieee->ieee_stats.rx_discards_undecryptable++;
+		return -1;
+	}
+
+	return res;
+}
+
+
+/* Called only as a tasklet (software IRQ), by ieee80211_rx */
+static inline int
+ieee80211_rx_frame_decrypt_msdu(struct ieee80211_device* ieee, struct sk_buff *skb,
+			     int keyidx, struct ieee80211_crypt_data *crypt)
+{
+	struct ieee80211_hdr *hdr;
+	int res, hdrlen;
+
+	if (crypt == NULL || crypt->ops->decrypt_msdu == NULL)
+		return 0;
+
+	hdr = (struct ieee80211_hdr *) skb->data;
+#ifdef _RTL8187_EXT_PATCH_
+	if((ieee->iw_mode == ieee->iw_ext_mode) && (ieee->ext_patch_ieee80211_rx_frame_get_hdrlen))
+	{
+		hdrlen = ieee->ext_patch_ieee80211_rx_frame_get_hdrlen(ieee, skb);
+	}
+	else
+#endif
+	hdrlen = ieee80211_get_hdrlen(le16_to_cpu(hdr->frame_ctl));
+
+	atomic_inc(&crypt->refcnt);
+	res = crypt->ops->decrypt_msdu(skb, keyidx, hdrlen, crypt->priv);
+	atomic_dec(&crypt->refcnt);
+	if (res < 0) {
+		printk(KERN_DEBUG "%s: MSDU decryption/MIC verification failed"
+		       " (SA=" MAC_FMT " keyidx=%d)\n",
+		       ieee->dev->name, MAC_ARG(hdr->addr2), keyidx);
+		return -1;
+	}
+
+	return 0;
+}
+
+
+/* this function is stolen from ipw2200 driver*/
+#define IEEE_PACKET_RETRY_TIME (5*HZ)
+static int is_duplicate_packet(struct ieee80211_device *ieee,
+				      struct ieee80211_hdr *header)
+{
+	u16 fc = le16_to_cpu(header->frame_ctl);
+	u16 sc = le16_to_cpu(header->seq_ctl);
+	u16 seq = WLAN_GET_SEQ_SEQ(sc);
+	u16 frag = WLAN_GET_SEQ_FRAG(sc);
+	u16 *last_seq, *last_frag;
+	unsigned long *last_time;
+	struct ieee80211_hdr_3addr_QOS *hdr_3addr_QoS;
+	struct ieee80211_hdr_QOS *hdr_4addr_QoS;
+	u8 tid;
+
+#ifdef _RTL8187_EXT_PATCH_
+	if(ieee->iw_mode == ieee->iw_ext_mode)
+	{
+		tid = (header->addr2[ETH_ALEN-2] ^ header->addr2[ETH_ALEN-1]) & IEEE80211_QOS_TID;
+	}
+	else
+#endif
+	//TO2DS and QoS
+	if(((fc & IEEE80211_FCTL_DSTODS) == IEEE80211_FCTL_DSTODS)&&IEEE80211_QOS_HAS_SEQ(fc)) {
+	  hdr_4addr_QoS = (struct ieee80211_hdr_QOS *)header;
+	  tid = le16_to_cpu(hdr_4addr_QoS->QOS_ctl) & IEEE80211_QOS_TID;
+	  tid = UP2AC(tid);
+	  tid ++;
+	} else if(IEEE80211_QOS_HAS_SEQ(fc)) { //QoS
+	  hdr_3addr_QoS = (struct ieee80211_hdr_3addr_QOS*)header;
+	  tid = le16_to_cpu(hdr_3addr_QoS->QOS_ctl) & IEEE80211_QOS_TID;
+	  tid = UP2AC(tid);
+	  tid ++;
+	} else { // no QoS
+	  tid = 0;
+	}
+	switch (ieee->iw_mode) {
+	case IW_MODE_ADHOC:
+	{
+		struct list_head *p;
+		struct ieee_ibss_seq *entry = NULL;
+		u8 *mac = header->addr2;
+		int index = mac[5] % IEEE_IBSS_MAC_HASH_SIZE;
+		//for (pos = (head)->next; pos != (head); pos = pos->next)
+		__list_for_each(p, &ieee->ibss_mac_hash[index]) {
+			entry = list_entry(p, struct ieee_ibss_seq, list);
+			if (!memcmp(entry->mac, mac, ETH_ALEN))
+				break;
+		}
+	//	if (memcmp(entry->mac, mac, ETH_ALEN)){
+		if (p == &ieee->ibss_mac_hash[index]) {
+			entry = kmalloc(sizeof(struct ieee_ibss_seq), GFP_ATOMIC);
+			if (!entry) {
+				printk(KERN_WARNING "Cannot malloc new mac entry\n");
+				return 0;
+			}
+			memcpy(entry->mac, mac, ETH_ALEN);
+			entry->seq_num[tid] = seq;
+			entry->frag_num[tid] = frag;
+			entry->packet_time[tid] = jiffies;
+			list_add(&entry->list, &ieee->ibss_mac_hash[index]);
+			return 0;
+		}
+		last_seq = &entry->seq_num[tid];
+		last_frag = &entry->frag_num[tid];
+		last_time = &entry->packet_time[tid];
+		break;
+	}
+
+	case IW_MODE_INFRA:
+		last_seq = &ieee->last_rxseq_num[tid];
+		last_frag = &ieee->last_rxfrag_num[tid];
+		last_time = &ieee->last_packet_time[tid];
+
+		break;
+	default:
+#ifdef _RTL8187_EXT_PATCH_
+		if(ieee->iw_mode == ieee->iw_ext_mode)
+		{
+			last_seq = &ieee->last_rxseq_num[tid];
+			last_frag = &ieee->last_rxfrag_num[tid];
+			last_time = &ieee->last_packet_time[tid];
+			break;
+		}
+		else
+#endif
+		return 0;
+	}
+
+//	if(tid != 0) {
+//		printk(KERN_WARNING ":)))))))))))%x %x %x, fc(%x)\n", tid, *last_seq, seq, header->frame_ctl);
+//	}
+	if ((*last_seq == seq) &&
+	    time_after(*last_time + IEEE_PACKET_RETRY_TIME, jiffies)) {
+		if (*last_frag == frag){
+			//printk(KERN_WARNING "[1] go drop!\n");
+			goto drop;
+
+		}
+		if (*last_frag + 1 != frag)
+			/* out-of-order fragment */
+			//printk(KERN_WARNING "[2] go drop!\n");
+			goto drop;
+	} else
+		*last_seq = seq;
+
+	*last_frag = frag;
+	*last_time = jiffies;
+	return 0;
+
+drop:
+//	BUG_ON(!(fc & IEEE80211_FCTL_RETRY));
+//	printk("DUP\n");
+
+	return 1;
+}
+
+
+/* All received frames are sent to this function. @skb contains the frame in
+ * IEEE 802.11 format, i.e., in the format it was sent over air.
+ * This function is called only as a tasklet (software IRQ). */
+int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
+		 struct ieee80211_rx_stats *rx_stats)
+{
+	struct net_device *dev = ieee->dev;
+	//struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	struct ieee80211_hdr *hdr;
+	//struct ieee80211_hdr_3addr_QOS *hdr;
+
+	size_t hdrlen;
+	u16 fc, type, stype, sc;
+	struct net_device_stats *stats;
+	unsigned int frag;
+	u8 *payload;
+	u16 ethertype;
+#ifdef NOT_YET
+	struct net_device *wds = NULL;
+	struct sk_buff *skb2 = NULL;
+	struct net_device *wds = NULL;
+	int frame_authorized = 0;
+	int from_assoc_ap = 0;
+	void *sta = NULL;
+#endif
+//	u16 QOS_ctl = 0;
+	u8 dst[ETH_ALEN];
+	u8 src[ETH_ALEN];
+	u8 bssid[ETH_ALEN];
+	struct ieee80211_crypt_data *crypt = NULL;
+	int keyidx = 0;
+
+	//Added for mesh by Lawrence.
+#ifdef _RTL8187_EXT_PATCH_
+	u8 status;
+	u32 flags;
+#endif
+	// cheat the the hdr type
+	hdr = (struct ieee80211_hdr *)skb->data;
+	stats = &ieee->stats;
+
+	if (skb->len < 10) {
+		printk(KERN_INFO "%s: SKB length < 10\n",
+		       dev->name);
+		goto rx_dropped;
+	}
+
+	fc = le16_to_cpu(hdr->frame_ctl);
+	type = WLAN_FC_GET_TYPE(fc);
+	stype = WLAN_FC_GET_STYPE(fc);
+	sc = le16_to_cpu(hdr->seq_ctl);
+
+	frag = WLAN_GET_SEQ_FRAG(sc);
+
+//YJ,add,080828,for keep alive
+	if((fc & IEEE80211_FCTL_TODS) != IEEE80211_FCTL_TODS)
+	{
+		if(!memcmp(hdr->addr1,dev->dev_addr, ETH_ALEN))
+		{
+			ieee->NumRxUnicast++;
+		}
+	}
+	else
+	{
+		if(!memcmp(hdr->addr3, dev->dev_addr, ETH_ALEN))
+		{
+			ieee->NumRxUnicast++;
+		}
+	}
+//YJ,add,080828,for keep alive,end
+
+#ifdef _RTL8187_EXT_PATCH_
+	if((ieee->iw_mode == ieee->iw_ext_mode) && (ieee->ext_patch_ieee80211_rx_frame_get_hdrlen))
+	{
+		hdrlen = ieee->ext_patch_ieee80211_rx_frame_get_hdrlen(ieee, skb);
+		if(skb->len < hdrlen)
+			goto rx_dropped;
+	}
+	else
+#endif
+	hdrlen = ieee80211_get_hdrlen(fc);
+
+#ifdef NOT_YET
+#if WIRELESS_EXT > 15
+	/* Put this code here so that we avoid duplicating it in all
+	 * Rx paths. - Jean II */
+#ifdef IW_WIRELESS_SPY		/* defined in iw_handler.h */
+	/* If spy monitoring on */
+	if (iface->spy_data.spy_number > 0) {
+		struct iw_quality wstats;
+		wstats.level = rx_stats->signal;
+		wstats.noise = rx_stats->noise;
+		wstats.updated = 6;	/* No qual value */
+		/* Update spy records */
+		wireless_spy_update(dev, hdr->addr2, &wstats);
+	}
+#endif /* IW_WIRELESS_SPY */
+#endif /* WIRELESS_EXT > 15 */
+	hostap_update_rx_stats(local->ap, hdr, rx_stats);
+#endif
+
+#if WIRELESS_EXT > 15
+	if (ieee->iw_mode == IW_MODE_MONITOR) {
+		ieee80211_monitor_rx(ieee, skb, rx_stats);
+		stats->rx_packets++;
+		stats->rx_bytes += skb->len;
+		return 1;
+	}
+#endif
+	if (ieee->host_decrypt) {
+		int idx = 0;
+		if (skb->len >= hdrlen + 3)
+			idx = skb->data[hdrlen + 3] >> 6;
+		crypt = ieee->crypt[idx];
+#ifdef NOT_YET
+		sta = NULL;
+
+		/* Use station specific key to override default keys if the
+		 * receiver address is a unicast address ("individual RA"). If
+		 * bcrx_sta_key parameter is set, station specific key is used
+		 * even with broad/multicast targets (this is against IEEE
+		 * 802.11, but makes it easier to use different keys with
+		 * stations that do not support WEP key mapping). */
+
+		if (!(hdr->addr1[0] & 0x01) || local->bcrx_sta_key)
+			(void) hostap_handle_sta_crypto(local, hdr, &crypt,
+							&sta);
+#endif
+
+		/* allow NULL decrypt to indicate an station specific override
+		 * for default encryption */
+		if (crypt && (crypt->ops == NULL ||
+			      crypt->ops->decrypt_mpdu == NULL))
+			crypt = NULL;
+
+		if (!crypt && (fc & IEEE80211_FCTL_WEP)) {
+			/* This seems to be triggered by some (multicast?)
+			 * frames from other than current BSS, so just drop the
+			 * frames silently instead of filling system log with
+			 * these reports. */
+			IEEE80211_DEBUG_DROP("Decryption failed (not set)"
+					     " (SA=" MAC_FMT ")\n",
+					     MAC_ARG(hdr->addr2));
+			ieee->ieee_stats.rx_discards_undecryptable++;
+			goto rx_dropped;
+		}
+	}
+
+	if (skb->len < IEEE80211_DATA_HDR3_LEN)
+		goto rx_dropped;
+
+#ifdef _RTL8187_EXT_PATCH_
+	if( ieee->iw_mode == ieee->iw_ext_mode && ieee->ext_patch_ieee80211_rx_mgt_update_expire )
+		ieee->ext_patch_ieee80211_rx_mgt_update_expire( ieee, skb );
+#endif
+
+	// if QoS enabled, should check the sequence for each of the AC
+	if (is_duplicate_packet(ieee, hdr))
+		goto rx_dropped;
+
+
+	if (type == IEEE80211_FTYPE_MGMT) {
+
+	#if 0
+		if ( stype == IEEE80211_STYPE_AUTH &&
+		    fc & IEEE80211_FCTL_WEP && ieee->host_decrypt &&
+		    (keyidx = hostap_rx_frame_decrypt(ieee, skb, crypt)) < 0)
+		{
+			printk(KERN_DEBUG "%s: failed to decrypt mgmt::auth "
+			       "from " MAC_FMT "\n", dev->name,
+			       MAC_ARG(hdr->addr2));
+			/* TODO: could inform hostapd about this so that it
+			 * could send auth failure report */
+			goto rx_dropped;
+		}
+	#endif
+
+
+		if (ieee80211_rx_frame_mgmt(ieee, skb, rx_stats, type, stype))
+			goto rx_dropped;
+		else
+			goto rx_exit;
+	}
+#ifdef _RTL8187_EXT_PATCH_
+	if((ieee->iw_mode == ieee->iw_ext_mode) && ieee->ext_patch_ieee80211_rx_on_rx)
+	{
+		if(ieee->ext_patch_ieee80211_rx_on_rx(ieee, skb, rx_stats, type, stype)==0)
+		{
+			goto rx_exit;
+		}
+	}
+#endif
+
+	/* Data frame - extract src/dst addresses */
+	switch (fc & (IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS)) {
+	case IEEE80211_FCTL_FROMDS:
+		memcpy(dst, hdr->addr1, ETH_ALEN);
+		memcpy(src, hdr->addr3, ETH_ALEN);
+		memcpy(bssid,hdr->addr2,ETH_ALEN);
+		break;
+	case IEEE80211_FCTL_TODS:
+		memcpy(dst, hdr->addr3, ETH_ALEN);
+		memcpy(src, hdr->addr2, ETH_ALEN);
+		memcpy(bssid,hdr->addr1,ETH_ALEN);
+		break;
+	case IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS:
+		if (skb->len < IEEE80211_DATA_HDR4_LEN)
+			goto rx_dropped;
+		memcpy(dst, hdr->addr3, ETH_ALEN);
+		memcpy(src, hdr->addr4, ETH_ALEN);
+		memcpy(bssid, ieee->current_network.bssid, ETH_ALEN);
+		break;
+	case 0:
+		memcpy(dst, hdr->addr1, ETH_ALEN);
+		memcpy(src, hdr->addr2, ETH_ALEN);
+		memcpy(bssid,hdr->addr3,ETH_ALEN);
+		break;
+	}
+
+#ifdef NOT_YET
+	if (hostap_rx_frame_wds(ieee, hdr, fc, &wds))
+		goto rx_dropped;
+	if (wds) {
+		skb->dev = dev = wds;
+		stats = hostap_get_stats(dev);
+	}
+
+	if (ieee->iw_mode == IW_MODE_MASTER && !wds &&
+	    (fc & (IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS)) == IEEE80211_FCTL_FROMDS &&
+	    ieee->stadev &&
+	    memcmp(hdr->addr2, ieee->assoc_ap_addr, ETH_ALEN) == 0) {
+		/* Frame from BSSID of the AP for which we are a client */
+		skb->dev = dev = ieee->stadev;
+		stats = hostap_get_stats(dev);
+		from_assoc_ap = 1;
+	}
+#endif
+
+	dev->last_rx = jiffies;
+
+#ifdef NOT_YET
+	if ((ieee->iw_mode == IW_MODE_MASTER ||
+	     ieee->iw_mode == IW_MODE_REPEAT) &&
+	    !from_assoc_ap) {
+		switch (hostap_handle_sta_rx(ieee, dev, skb, rx_stats,
+					     wds != NULL)) {
+		case AP_RX_CONTINUE_NOT_AUTHORIZED:
+			frame_authorized = 0;
+			break;
+		case AP_RX_CONTINUE:
+			frame_authorized = 1;
+			break;
+		case AP_RX_DROP:
+			goto rx_dropped;
+		case AP_RX_EXIT:
+			goto rx_exit;
+		}
+	}
+#endif
+
+#ifdef _RTL8187_EXT_PATCH_
+	if((ieee->iw_mode == ieee->iw_ext_mode) && ieee->ext_patch_ieee80211_rx_is_valid_framectl)
+	{
+		if(ieee->ext_patch_ieee80211_rx_is_valid_framectl(ieee, fc, type, stype)==0)
+			goto rx_dropped;
+	}
+	else
+#endif
+	/* Nullfunc frames may have PS-bit set, so they must be passed to
+	 * hostap_handle_sta_rx() before being dropped here. */
+	if (stype != IEEE80211_STYPE_DATA &&
+	    stype != IEEE80211_STYPE_DATA_CFACK &&
+	    stype != IEEE80211_STYPE_DATA_CFPOLL &&
+	    stype != IEEE80211_STYPE_DATA_CFACKPOLL&&
+	    stype != IEEE80211_STYPE_QOS_DATA//add by David,2006.8.4
+	    ) {
+		if (stype != IEEE80211_STYPE_NULLFUNC)
+			IEEE80211_DEBUG_DROP(
+				"RX: dropped data frame "
+				"with no data (type=0x%02x, "
+				"subtype=0x%02x, len=%d)\n",
+				type, stype, skb->len);
+		goto rx_dropped;
+	}
+	if(memcmp(bssid,ieee->current_network.bssid,ETH_ALEN)) {
+		goto rx_dropped;
+	}
+
+	ieee->NumRxDataInPeriod++;
+	ieee->NumRxOkTotal++;
+	/* skb: hdr + (possibly fragmented, possibly encrypted) payload */
+
+	if (ieee->host_decrypt && (fc & IEEE80211_FCTL_WEP) &&
+	    (keyidx = ieee80211_rx_frame_decrypt(ieee, skb, crypt)) < 0)
+		goto rx_dropped;
+
+	hdr = (struct ieee80211_hdr *) skb->data;
+
+	/* skb: hdr + (possibly fragmented) plaintext payload */
+	// PR: FIXME: hostap has additional conditions in the "if" below:
+	// ieee->host_decrypt && (fc & IEEE80211_FCTL_WEP) &&
+	if ((frag != 0 || (fc & IEEE80211_FCTL_MOREFRAGS))) {
+		int flen;
+		struct sk_buff *frag_skb = ieee80211_frag_cache_get(ieee, hdr);
+		IEEE80211_DEBUG_FRAG("Rx Fragment received (%u)\n", frag);
+
+		if (!frag_skb) {
+			IEEE80211_DEBUG(IEEE80211_DL_RX | IEEE80211_DL_FRAG,
+					"Rx cannot get skb from fragment "
+					"cache (morefrag=%d seq=%u frag=%u)\n",
+					(fc & IEEE80211_FCTL_MOREFRAGS) != 0,
+					WLAN_GET_SEQ_SEQ(sc), frag);
+			goto rx_dropped;
+		}
+		flen = skb->len;
+		if (frag != 0)
+			flen -= hdrlen;
+
+		if (frag_skb->tail + flen > frag_skb->end) {
+			printk(KERN_WARNING "%s: host decrypted and "
+			       "reassembled frame did not fit skb\n",
+			       dev->name);
+			ieee80211_frag_cache_invalidate(ieee, hdr);
+			goto rx_dropped;
+		}
+
+		if (frag == 0) {
+			/* copy first fragment (including full headers) into
+			 * beginning of the fragment cache skb */
+			memcpy(skb_put(frag_skb, flen), skb->data, flen);
+		} else {
+			/* append frame payload to the end of the fragment
+			 * cache skb */
+			memcpy(skb_put(frag_skb, flen), skb->data + hdrlen,
+			       flen);
+		}
+		dev_kfree_skb_any(skb);
+		skb = NULL;
+
+		if (fc & IEEE80211_FCTL_MOREFRAGS) {
+			/* more fragments expected - leave the skb in fragment
+			 * cache for now; it will be delivered to upper layers
+			 * after all fragments have been received */
+			goto rx_exit;
+		}
+
+		/* this was the last fragment and the frame will be
+		 * delivered, so remove skb from fragment cache */
+		skb = frag_skb;
+		hdr = (struct ieee80211_hdr *) skb->data;
+		ieee80211_frag_cache_invalidate(ieee, hdr);
+	}
+
+	/* skb: hdr + (possible reassembled) full MSDU payload; possibly still
+	 * encrypted/authenticated */
+	if (ieee->host_decrypt && (fc & IEEE80211_FCTL_WEP) &&
+	    ieee80211_rx_frame_decrypt_msdu(ieee, skb, keyidx, crypt))
+		goto rx_dropped;
+
+	hdr = (struct ieee80211_hdr *) skb->data;
+	if (crypt && !(fc & IEEE80211_FCTL_WEP) && !ieee->open_wep) {
+		if (/*ieee->ieee802_1x &&*/
+		    ieee80211_is_eapol_frame(ieee, skb, hdrlen)) {
+
+#ifdef CONFIG_IEEE80211_DEBUG
+			/* pass unencrypted EAPOL frames even if encryption is
+			 * configured */
+			struct eapol *eap = (struct eapol *)(skb->data +
+				24);
+			IEEE80211_DEBUG_EAP("RX: IEEE 802.1X EAPOL frame: %s\n",
+						eap_get_type(eap->type));
+#endif
+		} else {
+			IEEE80211_DEBUG_DROP(
+				"encryption configured, but RX "
+				"frame not encrypted (SA=" MAC_FMT ")\n",
+				MAC_ARG(hdr->addr2));
+			goto rx_dropped;
+		}
+	}
+
+#ifdef CONFIG_IEEE80211_DEBUG
+	if (crypt && !(fc & IEEE80211_FCTL_WEP) &&
+	    ieee80211_is_eapol_frame(ieee, skb, hdrlen)) {
+			struct eapol *eap = (struct eapol *)(skb->data +
+				24);
+			IEEE80211_DEBUG_EAP("RX: IEEE 802.1X EAPOL frame: %s\n",
+						eap_get_type(eap->type));
+	}
+#endif
+
+	if (crypt && !(fc & IEEE80211_FCTL_WEP) && !ieee->open_wep &&
+	    !ieee80211_is_eapol_frame(ieee, skb, hdrlen)) {
+		IEEE80211_DEBUG_DROP(
+			"dropped unencrypted RX data "
+			"frame from " MAC_FMT
+			" (drop_unencrypted=1)\n",
+			MAC_ARG(hdr->addr2));
+		goto rx_dropped;
+	}
+/*
+	if(ieee80211_is_eapol_frame(ieee, skb, hdrlen)) {
+		printk(KERN_WARNING "RX: IEEE802.1X EPAOL frame!\n");
+	}
+*/
+	/* skb: hdr + (possible reassembled) full plaintext payload */
+	payload = skb->data + hdrlen;
+	ethertype = (payload[6] << 8) | payload[7];
+
+#ifdef NOT_YET
+	/* If IEEE 802.1X is used, check whether the port is authorized to send
+	 * the received frame. */
+	if (ieee->ieee802_1x && ieee->iw_mode == IW_MODE_MASTER) {
+		if (ethertype == ETH_P_PAE) {
+			printk(KERN_DEBUG "%s: RX: IEEE 802.1X frame\n",
+			       dev->name);
+			if (ieee->hostapd && ieee->apdev) {
+				/* Send IEEE 802.1X frames to the user
+				 * space daemon for processing */
+				prism2_rx_80211(ieee->apdev, skb, rx_stats,
+						PRISM2_RX_MGMT);
+				ieee->apdevstats.rx_packets++;
+				ieee->apdevstats.rx_bytes += skb->len;
+				goto rx_exit;
+			}
+		} else if (!frame_authorized) {
+			printk(KERN_DEBUG "%s: dropped frame from "
+			       "unauthorized port (IEEE 802.1X): "
+			       "ethertype=0x%04x\n",
+			       dev->name, ethertype);
+			goto rx_dropped;
+		}
+	}
+#endif
+
+#ifdef _RTL8187_EXT_PATCH_
+	if((ieee->iw_mode == ieee->iw_ext_mode) && ieee->ext_patch_ieee80211_rx_process_dataframe)
+	{
+	//Added for mesh rx interrupt.
+		//spin_lock_irqsave(&ieee->lock,flags);
+		status = ieee->ext_patch_ieee80211_rx_process_dataframe(ieee, skb, rx_stats);
+		//spin_unlock_irqrestore(&ieee->lock,flags);
+
+		if(status)
+//	if(ieee->ext_patch_ieee80211_rx_process_dataframe(ieee, skb, rx_stats))
+			goto rx_exit;
+		else
+			goto rx_dropped;
+	}
+#endif
+
+	/* convert hdr + possible LLC headers into Ethernet header */
+	if (skb->len - hdrlen >= 8 &&
+	    ((memcmp(payload, rfc1042_header, SNAP_SIZE) == 0 &&
+	      ethertype != ETH_P_AARP && ethertype != ETH_P_IPX) ||
+	     memcmp(payload, bridge_tunnel_header, SNAP_SIZE) == 0)) {
+		/* remove RFC1042 or Bridge-Tunnel encapsulation and
+		 * replace EtherType */
+		skb_pull(skb, hdrlen + SNAP_SIZE);
+		memcpy(skb_push(skb, ETH_ALEN), src, ETH_ALEN);
+		memcpy(skb_push(skb, ETH_ALEN), dst, ETH_ALEN);
+	} else {
+		u16 len;
+		/* Leave Ethernet header part of hdr and full payload */
+		skb_pull(skb, hdrlen);
+		len = htons(skb->len);
+		memcpy(skb_push(skb, 2), &len, 2);
+		memcpy(skb_push(skb, ETH_ALEN), src, ETH_ALEN);
+		memcpy(skb_push(skb, ETH_ALEN), dst, ETH_ALEN);
+	}
+
+#ifdef NOT_YET
+	if (wds && ((fc & (IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS)) ==
+		    IEEE80211_FCTL_TODS) &&
+	    skb->len >= ETH_HLEN + ETH_ALEN) {
+		/* Non-standard frame: get addr4 from its bogus location after
+		 * the payload */
+		memcpy(skb->data + ETH_ALEN,
+		       skb->data + skb->len - ETH_ALEN, ETH_ALEN);
+		skb_trim(skb, skb->len - ETH_ALEN);
+	}
+#endif
+
+	stats->rx_packets++;
+	stats->rx_bytes += skb->len;
+
+#ifdef NOT_YET
+	if (ieee->iw_mode == IW_MODE_MASTER && !wds &&
+	    ieee->ap->bridge_packets) {
+		if (dst[0] & 0x01) {
+			/* copy multicast frame both to the higher layers and
+			 * to the wireless media */
+			ieee->ap->bridged_multicast++;
+			skb2 = skb_clone(skb, GFP_ATOMIC);
+			if (skb2 == NULL)
+				printk(KERN_DEBUG "%s: skb_clone failed for "
+				       "multicast frame\n", dev->name);
+		} else if (hostap_is_sta_assoc(ieee->ap, dst)) {
+			/* send frame directly to the associated STA using
+			 * wireless media and not passing to higher layers */
+			ieee->ap->bridged_unicast++;
+			skb2 = skb;
+			skb = NULL;
+		}
+	}
+
+	if (skb2 != NULL) {
+		/* send to wireless media */
+		skb2->protocol = __constant_htons(ETH_P_802_3);
+		skb2->mac.raw = skb2->nh.raw = skb2->data;
+		/* skb2->nh.raw = skb2->data + ETH_HLEN; */
+		skb2->dev = dev;
+		dev_queue_xmit(skb2);
+	}
+
+#endif
+	if (skb) {
+		skb->protocol = eth_type_trans(skb, dev);
+		memset(skb->cb, 0, sizeof(skb->cb));
+		skb->dev = dev;
+		skb->ip_summed = CHECKSUM_NONE; /* 802.11 crc not sufficient */
+		ieee->last_rx_ps_time = jiffies;
+		netif_rx(skb);
+	}
+
+ rx_exit:
+#ifdef NOT_YET
+	if (sta)
+		hostap_handle_sta_release(sta);
+#endif
+	return 1;
+
+ rx_dropped:
+	stats->rx_dropped++;
+
+	/* Returning 0 indicates to caller that we have not handled the SKB--
+	 * so it is still allocated and can be used again by underlying
+	 * hardware as a DMA target */
+	return 0;
+}
+
+#ifdef _RTL8187_EXT_PATCH_
+int ieee_ext_skb_p80211_to_ether(struct sk_buff *skb, int hdrlen, u8 *dst, u8 *src)
+{
+	u8 *payload;
+	u16 ethertype;
+
+	/* skb: hdr + (possible reassembled) full plaintext payload */
+	payload = skb->data + hdrlen;
+	ethertype = (payload[6] << 8) | payload[7];
+
+	/* convert hdr + possible LLC headers into Ethernet header */
+	if (skb->len - hdrlen >= 8 &&
+	    ((memcmp(payload, rfc1042_header, SNAP_SIZE) == 0 &&
+	      ethertype != ETH_P_AARP && ethertype != ETH_P_IPX) ||
+	     memcmp(payload, bridge_tunnel_header, SNAP_SIZE) == 0)) {
+		/* remove RFC1042 or Bridge-Tunnel encapsulation and
+		 * replace EtherType */
+		skb_pull(skb, hdrlen + SNAP_SIZE);
+		memcpy(skb_push(skb, ETH_ALEN), src, ETH_ALEN);
+		memcpy(skb_push(skb, ETH_ALEN), dst, ETH_ALEN);
+	} else {
+		u16 len;
+		/* Leave Ethernet header part of hdr and full payload */
+		skb_pull(skb, hdrlen);
+		len = htons(skb->len);
+		memcpy(skb_push(skb, 2), &len, 2);
+		memcpy(skb_push(skb, ETH_ALEN), src, ETH_ALEN);
+		memcpy(skb_push(skb, ETH_ALEN), dst, ETH_ALEN);
+	}
+
+	return 1;
+}
+#endif // _RTL8187_EXT_PATCH_
+
+
+#define MGMT_FRAME_FIXED_PART_LENGTH		0x24
+
+static inline int ieee80211_is_ofdm_rate(u8 rate)
+{
+	switch (rate & ~IEEE80211_BASIC_RATE_MASK) {
+	case IEEE80211_OFDM_RATE_6MB:
+	case IEEE80211_OFDM_RATE_9MB:
+	case IEEE80211_OFDM_RATE_12MB:
+	case IEEE80211_OFDM_RATE_18MB:
+	case IEEE80211_OFDM_RATE_24MB:
+	case IEEE80211_OFDM_RATE_36MB:
+	case IEEE80211_OFDM_RATE_48MB:
+	case IEEE80211_OFDM_RATE_54MB:
+		return 1;
+	}
+        return 0;
+}
+
+static inline int ieee80211_SignalStrengthTranslate(
+	int  CurrSS
+	)
+{
+	int RetSS;
+
+	// Step 1. Scale mapping.
+	if(CurrSS >= 71 && CurrSS <= 100)
+	{
+		RetSS = 90 + ((CurrSS - 70) / 3);
+	}
+	else if(CurrSS >= 41 && CurrSS <= 70)
+	{
+		RetSS = 78 + ((CurrSS - 40) / 3);
+	}
+	else if(CurrSS >= 31 && CurrSS <= 40)
+	{
+		RetSS = 66 + (CurrSS - 30);
+	}
+	else if(CurrSS >= 21 && CurrSS <= 30)
+	{
+		RetSS = 54 + (CurrSS - 20);
+	}
+	else if(CurrSS >= 5 && CurrSS <= 20)
+	{
+		RetSS = 42 + (((CurrSS - 5) * 2) / 3);
+	}
+	else if(CurrSS == 4)
+	{
+		RetSS = 36;
+	}
+	else if(CurrSS == 3)
+	{
+		RetSS = 27;
+	}
+	else if(CurrSS == 2)
+	{
+		RetSS = 18;
+	}
+	else if(CurrSS == 1)
+	{
+		RetSS = 9;
+	}
+	else
+	{
+		RetSS = CurrSS;
+	}
+	//RT_TRACE(COMP_DBG, DBG_LOUD, ("##### After Mapping:  LastSS: %d, CurrSS: %d, RetSS: %d\n", LastSS, CurrSS, RetSS));
+
+	// Step 2. Smoothing.
+
+	//RT_TRACE(COMP_DBG, DBG_LOUD, ("$$$$$ After Smoothing:  LastSS: %d, CurrSS: %d, RetSS: %d\n", LastSS, CurrSS, RetSS));
+
+	return RetSS;
+}
+
+#ifdef ENABLE_DOT11D
+static inline void ieee80211_extract_country_ie(
+	struct ieee80211_device *ieee,
+	struct ieee80211_info_element *info_element,
+	struct ieee80211_network *network,
+	u8 * addr2
+)
+{
+#if 0
+	u32 i = 0;
+	u8 * p = (u8*)info_element->data;
+	printk("-----------------------\n");
+	printk("%s Country IE:", network->ssid);
+	for(i=0; i<info_element->len; i++)
+		printk("\t%2.2x", *(p+i));
+	printk("\n-----------------------\n");
+#endif
+	if(IS_DOT11D_ENABLE(ieee))
+	{
+		if(info_element->len!= 0)
+		{
+			memcpy(network->CountryIeBuf, info_element->data, info_element->len);
+			network->CountryIeLen = info_element->len;
+
+			if(!IS_COUNTRY_IE_VALID(ieee))
+			{
+				Dot11d_UpdateCountryIe(ieee, addr2, info_element->len, info_element->data);
+			}
+		}
+
+		//
+		// 070305, rcnjko: I update country IE watch dog here because
+		// some AP (e.g. Cisco 1242) don't include country IE in their
+		// probe response frame.
+		//
+		if(IS_EQUAL_CIE_SRC(ieee, addr2) )
+		{
+			UPDATE_CIE_WATCHDOG(ieee);
+		}
+	}
+
+}
+#endif
+
+int
+ieee80211_TranslateToDbm(
+	unsigned char SignalStrengthIndex	// 0-100 index.
+	)
+{
+	unsigned char SignalPower; // in dBm.
+
+	// Translate to dBm (x=0.5y-95).
+	SignalPower = (int)SignalStrengthIndex * 7 / 10;
+	SignalPower -= 95;
+
+	return SignalPower;
+}
+inline int ieee80211_network_init(
+	struct ieee80211_device *ieee,
+	struct ieee80211_probe_response *beacon,
+	struct ieee80211_network *network,
+	struct ieee80211_rx_stats *stats)
+{
+#ifdef CONFIG_IEEE80211_DEBUG
+	char rates_str[64];
+	char *p;
+#endif
+	struct ieee80211_info_element *info_element;
+ 	u16 left;
+	u8 i;
+	short offset;
+	u8 curRate = 0,hOpRate = 0,curRate_ex = 0;
+
+	/* Pull out fixed field data */
+	memcpy(network->bssid, beacon->header.addr3, ETH_ALEN);
+	network->capability = beacon->capability;
+	network->last_scanned = jiffies;
+	network->time_stamp[0] = beacon->time_stamp[0];
+	network->time_stamp[1] = beacon->time_stamp[1];
+	network->beacon_interval = beacon->beacon_interval;
+	/* Where to pull this? beacon->listen_interval;*/
+	network->listen_interval = 0x0A;
+	network->rates_len = network->rates_ex_len = 0;
+	network->last_associate = 0;
+	network->ssid_len = 0;
+	network->flags = 0;
+	network->atim_window = 0;
+	network->QoS_Enable = 0;
+//by amy 080312
+	network->HighestOperaRate = 0;
+//by amy 080312
+#ifdef THOMAS_TURBO
+	network->Turbo_Enable = 0;
+#endif
+#ifdef ENABLE_DOT11D
+	network->CountryIeLen = 0;
+	memset(network->CountryIeBuf, 0, MAX_IE_LEN);
+#endif
+
+	if (stats->freq == IEEE80211_52GHZ_BAND) {
+		/* for A band (No DS info) */
+		network->channel = stats->received_channel;
+	} else
+		network->flags |= NETWORK_HAS_CCK;
+
+ 	network->wpa_ie_len = 0;
+ 	network->rsn_ie_len = 0;
+
+ 	info_element = &beacon->info_element;
+	left = stats->len - ((void *)info_element - (void *)beacon);
+	while (left >= sizeof(struct ieee80211_info_element_hdr)) {
+		if (sizeof(struct ieee80211_info_element_hdr) + info_element->len > left) {
+			IEEE80211_DEBUG_SCAN("SCAN: parse failed: info_element->len + 2 > left : info_element->len+2=%d left=%d.\n",
+					     info_element->len + sizeof(struct ieee80211_info_element),
+					     left);
+			return 1;
+               	}
+
+		switch (info_element->id) {
+		case MFIE_TYPE_SSID:
+			if (ieee80211_is_empty_essid(info_element->data,
+						     info_element->len)) {
+				network->flags |= NETWORK_EMPTY_ESSID;
+				break;
+			}
+
+			network->ssid_len = min(info_element->len,
+						(u8)IW_ESSID_MAX_SIZE);
+			memcpy(network->ssid, info_element->data, network->ssid_len);
+        		if (network->ssid_len < IW_ESSID_MAX_SIZE)
+                		memset(network->ssid + network->ssid_len, 0,
+				       IW_ESSID_MAX_SIZE - network->ssid_len);
+
+			IEEE80211_DEBUG_SCAN("MFIE_TYPE_SSID: '%s' len=%d.\n",
+					     network->ssid, network->ssid_len);
+			break;
+
+		case MFIE_TYPE_RATES:
+#ifdef CONFIG_IEEE80211_DEBUG
+			p = rates_str;
+#endif
+			network->rates_len = min(info_element->len, MAX_RATES_LENGTH);
+			for (i = 0; i < network->rates_len; i++) {
+				network->rates[i] = info_element->data[i];
+				curRate = network->rates[i] & 0x7f;
+				if( hOpRate < curRate )
+					hOpRate = curRate;
+#ifdef CONFIG_IEEE80211_DEBUG
+				p += snprintf(p, sizeof(rates_str) - (p - rates_str), "%02X ", network->rates[i]);
+#endif
+				if (ieee80211_is_ofdm_rate(info_element->data[i])) {
+					network->flags |= NETWORK_HAS_OFDM;
+					if (info_element->data[i] &
+					    IEEE80211_BASIC_RATE_MASK)
+						network->flags &=
+							~NETWORK_HAS_CCK;
+				}
+			}
+
+			IEEE80211_DEBUG_SCAN("MFIE_TYPE_RATES: '%s' (%d)\n",
+					     rates_str, network->rates_len);
+			break;
+
+		case MFIE_TYPE_RATES_EX:
+#ifdef CONFIG_IEEE80211_DEBUG
+			p = rates_str;
+#endif
+			network->rates_ex_len = min(info_element->len, MAX_RATES_EX_LENGTH);
+			for (i = 0; i < network->rates_ex_len; i++) {
+				network->rates_ex[i] = info_element->data[i];
+				curRate_ex = network->rates_ex[i] & 0x7f;
+				if( hOpRate < curRate_ex )
+					hOpRate = curRate_ex;
+#ifdef CONFIG_IEEE80211_DEBUG
+				p += snprintf(p, sizeof(rates_str) - (p - rates_str), "%02X ", network->rates[i]);
+#endif
+				if (ieee80211_is_ofdm_rate(info_element->data[i])) {
+					network->flags |= NETWORK_HAS_OFDM;
+					if (info_element->data[i] &
+					    IEEE80211_BASIC_RATE_MASK)
+						network->flags &=
+							~NETWORK_HAS_CCK;
+				}
+			}
+
+			IEEE80211_DEBUG_SCAN("MFIE_TYPE_RATES_EX: '%s' (%d)\n",
+					     rates_str, network->rates_ex_len);
+			break;
+
+		case MFIE_TYPE_DS_SET:
+  			IEEE80211_DEBUG_SCAN("MFIE_TYPE_DS_SET: %d\n",
+					     info_element->data[0]);
+			if (stats->freq == IEEE80211_24GHZ_BAND)
+				network->channel = info_element->data[0];
+			break;
+
+	 	case MFIE_TYPE_FH_SET:
+  			IEEE80211_DEBUG_SCAN("MFIE_TYPE_FH_SET: ignored\n");
+			break;
+
+		case MFIE_TYPE_CF_SET:
+			IEEE80211_DEBUG_SCAN("MFIE_TYPE_CF_SET: ignored\n");
+			break;
+
+		case MFIE_TYPE_TIM:
+
+			if(info_element->len < 4)
+				break;
+
+			network->dtim_period = info_element->data[1];
+
+			if(ieee->state != IEEE80211_LINKED)
+				break;
+#if 0
+			network->last_dtim_sta_time[0] = stats->mac_time[0];
+#else
+			network->last_dtim_sta_time[0] = jiffies;
+#endif
+			network->last_dtim_sta_time[1] = stats->mac_time[1];
+
+			network->dtim_data = IEEE80211_DTIM_VALID;
+
+			if(info_element->data[0] != 0)
+				break;
+
+			if(info_element->data[2] & 1)
+				network->dtim_data |= IEEE80211_DTIM_MBCAST;
+
+			offset = (info_element->data[2] >> 1)*2;
+
+			//printk("offset1:%x aid:%x\n",offset, ieee->assoc_id);
+
+			/* add and modified for ps 2008.1.22 */
+			if(ieee->assoc_id < 8*offset ||
+				ieee->assoc_id > 8*(offset + info_element->len -3)) {
+				break;
+			}
+
+			offset = (ieee->assoc_id/8) - offset;// + ((aid % 8)? 0 : 1) ;
+
+		//	printk("offset:%x data:%x, ucast:%d\n", offset,
+			//	info_element->data[3+offset] ,
+			//	info_element->data[3+offset] & (1<<(ieee->assoc_id%8)));
+
+			if(info_element->data[3+offset] & (1<<(ieee->assoc_id%8))) {
+				network->dtim_data |= IEEE80211_DTIM_UCAST;
+			}
+			break;
+
+		case MFIE_TYPE_IBSS_SET:
+			IEEE80211_DEBUG_SCAN("MFIE_TYPE_IBSS_SET: ignored\n");
+			break;
+
+		case MFIE_TYPE_CHALLENGE:
+			IEEE80211_DEBUG_SCAN("MFIE_TYPE_CHALLENGE: ignored\n");
+			break;
+
+		case MFIE_TYPE_GENERIC:
+			//nic is 87B
+			IEEE80211_DEBUG_SCAN("MFIE_TYPE_GENERIC: %d bytes\n",
+					     info_element->len);
+			if (info_element->len >= 4  &&
+			    info_element->data[0] == 0x00 &&
+			    info_element->data[1] == 0x50 &&
+			    info_element->data[2] == 0xf2 &&
+			    info_element->data[3] == 0x01) {
+				network->wpa_ie_len = min(info_element->len + 2,
+							 MAX_WPA_IE_LEN);
+				memcpy(network->wpa_ie, info_element,
+				       network->wpa_ie_len);
+			}
+
+#ifdef THOMAS_TURBO
+			if (info_element->len == 7 &&
+			    info_element->data[0] == 0x00 &&
+			    info_element->data[1] == 0xe0 &&
+			    info_element->data[2] == 0x4c &&
+			    info_element->data[3] == 0x01 &&
+			    info_element->data[4] == 0x02) {
+				network->Turbo_Enable = 1;
+			}
+#endif
+			if (1 == stats->nic_type) {//nic 87
+				break;
+			}
+
+			if (info_element->len >= 5  &&
+			    info_element->data[0] == 0x00 &&
+			    info_element->data[1] == 0x50 &&
+			    info_element->data[2] == 0xf2 &&
+			    info_element->data[3] == 0x02 &&
+			    info_element->data[4] == 0x00) {
+				//printk(KERN_WARNING "wmm info updated: %x\n", info_element->data[6]);
+				//WMM Information Element
+				network->wmm_info = info_element->data[6];
+				network->QoS_Enable = 1;
+			}
+
+			if (info_element->len >= 8  &&
+			    info_element->data[0] == 0x00 &&
+			    info_element->data[1] == 0x50 &&
+			    info_element->data[2] == 0xf2 &&
+			    info_element->data[3] == 0x02 &&
+			    info_element->data[4] == 0x01) {
+				// Not care about version at present.
+				//WMM Information Element
+				//printk(KERN_WARNING "wmm info&param updated: %x\n", info_element->data[6]);
+				network->wmm_info = info_element->data[6];
+				//WMM Parameter Element
+				memcpy(network->wmm_param, (u8 *)(info_element->data + 8),(info_element->len - 8));
+				network->QoS_Enable = 1;
+			}
+			break;
+
+		case MFIE_TYPE_RSN:
+			IEEE80211_DEBUG_SCAN("MFIE_TYPE_RSN: %d bytes\n",
+					     info_element->len);
+			network->rsn_ie_len = min(info_element->len + 2,
+						 MAX_WPA_IE_LEN);
+			memcpy(network->rsn_ie, info_element,
+			       network->rsn_ie_len);
+			break;
+#ifdef ENABLE_DOT11D
+		case MFIE_TYPE_COUNTRY:
+			IEEE80211_DEBUG_SCAN("MFIE_TYPE_COUNTRY: %d bytes\n",
+					     info_element->len);
+//			printk("=====>Receive <%s> Country IE\n",network->ssid);
+			ieee80211_extract_country_ie(ieee, info_element, network, beacon->header.addr2);
+			break;
+#endif
+		default:
+			IEEE80211_DEBUG_SCAN("unsupported IE %d\n",
+					     info_element->id);
+                        break;
+  		}
+
+		left -= sizeof(struct ieee80211_info_element_hdr) +
+			info_element->len;
+		info_element = (struct ieee80211_info_element *)
+                	&info_element->data[info_element->len];
+  	}
+//by amy 080312
+	network->HighestOperaRate = hOpRate;
+//by amy 080312
+	network->mode = 0;
+	if (stats->freq == IEEE80211_52GHZ_BAND)
+		network->mode = IEEE_A;
+	else {
+		if (network->flags & NETWORK_HAS_OFDM)
+			network->mode |= IEEE_G;
+		if (network->flags & NETWORK_HAS_CCK)
+			network->mode |= IEEE_B;
+	}
+
+	if (network->mode == 0) {
+		IEEE80211_DEBUG_SCAN("Filtered out '%s (" MAC_FMT ")' "
+				     "network.\n",
+				     escape_essid(network->ssid,
+						  network->ssid_len),
+				     MAC_ARG(network->bssid));
+		return 1;
+	}
+
+	if (ieee80211_is_empty_essid(network->ssid, network->ssid_len))
+		network->flags |= NETWORK_EMPTY_ESSID;
+#if 0
+	stats->signal = ieee80211_SignalStrengthTranslate(stats->signal);
+#endif
+	stats->signal = ieee80211_TranslateToDbm(stats->signalstrength);
+	//stats->noise = stats->signal - stats->noise;
+	stats->noise = ieee80211_TranslateToDbm(100 - stats->signalstrength) - 25;
+	memcpy(&network->stats, stats, sizeof(network->stats));
+
+	return 0;
+}
+
+static inline int is_same_network(struct ieee80211_network *src,
+				  struct ieee80211_network *dst,
+				  struct ieee80211_device * ieee)
+{
+	/* A network is only a duplicate if the channel, BSSID, ESSID
+	 * and the capability field (in particular IBSS and BSS) all match.
+	 * We treat all <hidden> with the same BSSID and channel
+	 * as one network */
+	return (((src->ssid_len == dst->ssid_len) || (ieee->iw_mode == IW_MODE_INFRA)) &&  //YJ,mod,080819,for hidden ap
+		//((src->ssid_len == dst->ssid_len) &&
+		(src->channel == dst->channel) &&
+		!memcmp(src->bssid, dst->bssid, ETH_ALEN) &&
+		(!memcmp(src->ssid, dst->ssid, src->ssid_len) || (ieee->iw_mode == IW_MODE_INFRA)) && //YJ,mod,080819,for hidden ap
+		//!memcmp(src->ssid, dst->ssid, src->ssid_len) &&
+		((src->capability & WLAN_CAPABILITY_IBSS) ==
+		(dst->capability & WLAN_CAPABILITY_IBSS)) &&
+		((src->capability & WLAN_CAPABILITY_BSS) ==
+		(dst->capability & WLAN_CAPABILITY_BSS)));
+}
+
+inline void update_network(struct ieee80211_network *dst,
+				  struct ieee80211_network *src)
+{
+	unsigned char quality = src->stats.signalstrength;
+	unsigned char signal = 0;
+	unsigned char noise = 0;
+        if(dst->stats.signalstrength > 0) {
+                quality = (dst->stats.signalstrength * 5 + src->stats.signalstrength + 5)/6;
+        }
+	signal = ieee80211_TranslateToDbm(quality);
+	//noise = signal - src->stats.noise;
+	if(dst->stats.noise > 0)
+		noise = (dst->stats.noise * 5 + src->stats.noise)/6;
+        //if(strcmp(dst->ssid, "linksys_lzm000") == 0)
+//	printk("ssid:%s, quality:%d, signal:%d\n", dst->ssid, quality, signal);
+	memcpy(&dst->stats, &src->stats, sizeof(struct ieee80211_rx_stats));
+	dst->stats.signalstrength = quality;
+	dst->stats.signal = signal;
+//	printk("==================>stats.signal is %d\n",dst->stats.signal);
+	dst->stats.noise = noise;
+
+
+	dst->capability = src->capability;
+	memcpy(dst->rates, src->rates, src->rates_len);
+	dst->rates_len = src->rates_len;
+	memcpy(dst->rates_ex, src->rates_ex, src->rates_ex_len);
+	dst->rates_ex_len = src->rates_ex_len;
+	dst->HighestOperaRate= src->HighestOperaRate;
+	//printk("==========>in %s: src->ssid is %s,chan is %d\n",__FUNCTION__,src->ssid,src->channel);
+
+	//YJ,add,080819,for hidden ap
+	if(src->ssid_len > 0)
+	{
+		//if(src->ssid_len == 13)
+		//	printk("=====================>>>>>>>> Dst ssid: %s Src ssid: %s\n", dst->ssid, src->ssid);
+		memset(dst->ssid, 0, dst->ssid_len);
+		dst->ssid_len = src->ssid_len;
+		memcpy(dst->ssid, src->ssid, src->ssid_len);
+	}
+	//YJ,add,080819,for hidden ap,end
+
+	dst->channel = src->channel;
+	dst->mode = src->mode;
+	dst->flags = src->flags;
+	dst->time_stamp[0] = src->time_stamp[0];
+	dst->time_stamp[1] = src->time_stamp[1];
+
+	dst->beacon_interval = src->beacon_interval;
+	dst->listen_interval = src->listen_interval;
+	dst->atim_window = src->atim_window;
+	dst->dtim_period = src->dtim_period;
+	dst->dtim_data = src->dtim_data;
+	dst->last_dtim_sta_time[0] = src->last_dtim_sta_time[0];
+	dst->last_dtim_sta_time[1] = src->last_dtim_sta_time[1];
+//	printk("update:%s, dtim_period:%x, dtim_data:%x\n", src->ssid, src->dtim_period, src->dtim_data);
+	memcpy(dst->wpa_ie, src->wpa_ie, src->wpa_ie_len);
+	dst->wpa_ie_len = src->wpa_ie_len;
+	memcpy(dst->rsn_ie, src->rsn_ie, src->rsn_ie_len);
+	dst->rsn_ie_len = src->rsn_ie_len;
+
+	dst->last_scanned = jiffies;
+	/* dst->last_associate is not overwritten */
+// disable QoS process now, added by David 2006/7/25
+#if 1
+	dst->wmm_info = src->wmm_info; //sure to exist in beacon or probe response frame.
+/*
+	if((dst->wmm_info^src->wmm_info)&0x0f) {//Param Set Count change, update Parameter
+	  memcpy(dst->wmm_param, src->wmm_param, IEEE80211_AC_PRAM_LEN);
+	}
+*/
+	if(src->wmm_param[0].ac_aci_acm_aifsn|| \
+	   src->wmm_param[1].ac_aci_acm_aifsn|| \
+	   src->wmm_param[2].ac_aci_acm_aifsn|| \
+	   src->wmm_param[1].ac_aci_acm_aifsn) {
+	  memcpy(dst->wmm_param, src->wmm_param, WME_AC_PRAM_LEN);
+	}
+	dst->QoS_Enable = src->QoS_Enable;
+#else
+	dst->QoS_Enable = 1;//for Rtl8187 simulation
+#endif
+	dst->SignalStrength = src->SignalStrength;
+#ifdef THOMAS_TURBO
+	dst->Turbo_Enable = src->Turbo_Enable;
+#endif
+#ifdef ENABLE_DOT11D
+	dst->CountryIeLen = src->CountryIeLen;
+	memcpy(dst->CountryIeBuf, src->CountryIeBuf, src->CountryIeLen);
+#endif
+}
+
+
+inline void ieee80211_process_probe_response(
+	struct ieee80211_device *ieee,
+	struct ieee80211_probe_response *beacon,
+	struct ieee80211_rx_stats *stats)
+{
+	struct ieee80211_network network;
+	struct ieee80211_network *target;
+	struct ieee80211_network *oldest = NULL;
+#ifdef CONFIG_IEEE80211_DEBUG
+	struct ieee80211_info_element *info_element = &beacon->info_element;
+#endif
+	unsigned long flags;
+	short renew;
+	u8 wmm_info;
+	u8 is_beacon = (WLAN_FC_GET_STYPE(beacon->header.frame_ctl) == IEEE80211_STYPE_BEACON)? 1:0;  //YJ,add,080819,for hidden ap
+
+	memset(&network, 0, sizeof(struct ieee80211_network));
+//rz
+#ifdef _RTL8187_EXT_PATCH_
+	if((ieee->iw_mode == ieee->iw_ext_mode) && ieee->ext_patch_ieee80211_process_probe_response_1) {
+		ieee->ext_patch_ieee80211_process_probe_response_1(ieee, beacon, stats);
+		return;
+	}
+#endif
+
+	IEEE80211_DEBUG_SCAN(
+		"'%s' (" MAC_FMT "): %c%c%c%c %c%c%c%c-%c%c%c%c %c%c%c%c\n",
+		escape_essid(info_element->data, info_element->len),
+		MAC_ARG(beacon->header.addr3),
+		(beacon->capability & (1<<0xf)) ? '1' : '0',
+		(beacon->capability & (1<<0xe)) ? '1' : '0',
+		(beacon->capability & (1<<0xd)) ? '1' : '0',
+		(beacon->capability & (1<<0xc)) ? '1' : '0',
+		(beacon->capability & (1<<0xb)) ? '1' : '0',
+		(beacon->capability & (1<<0xa)) ? '1' : '0',
+		(beacon->capability & (1<<0x9)) ? '1' : '0',
+		(beacon->capability & (1<<0x8)) ? '1' : '0',
+		(beacon->capability & (1<<0x7)) ? '1' : '0',
+		(beacon->capability & (1<<0x6)) ? '1' : '0',
+		(beacon->capability & (1<<0x5)) ? '1' : '0',
+		(beacon->capability & (1<<0x4)) ? '1' : '0',
+		(beacon->capability & (1<<0x3)) ? '1' : '0',
+		(beacon->capability & (1<<0x2)) ? '1' : '0',
+		(beacon->capability & (1<<0x1)) ? '1' : '0',
+		(beacon->capability & (1<<0x0)) ? '1' : '0');
+#if 0
+	if(strcmp(escape_essid(beacon->info_element.data, beacon->info_element.len), "rtl_softap") == 0)
+	{
+		if(WLAN_FC_GET_STYPE(beacon->header.frame_ctl) == IEEE80211_STYPE_BEACON)
+		{
+			u32 i = 0, len = stats->len;
+			u8 * p = (u8*)beacon;
+			printk("-----------------------\n");
+			printk("rtl_softap Beacon:");
+			for(i=0; i<len; i++)
+				printk("\t%2.2x", *(p+i));
+			printk("\n-----------------------\n");
+		}
+	}
+#endif
+	if (ieee80211_network_init(ieee, beacon, &network, stats)) {
+		IEEE80211_DEBUG_SCAN("Dropped '%s' (" MAC_FMT ") via %s.\n",
+				     escape_essid(info_element->data,
+						  info_element->len),
+				     MAC_ARG(beacon->header.addr3),
+				     WLAN_FC_GET_STYPE(beacon->header.frame_ctl) ==
+				     IEEE80211_STYPE_PROBE_RESP ?
+				     "PROBE RESPONSE" : "BEACON");
+		return;
+	}
+
+#ifdef ENABLE_DOT11D
+	// For Asus EeePc request,
+	// (1) if wireless adapter receive get any 802.11d country code in AP beacon,
+	//	   wireless adapter should follow the country code.
+	// (2)  If there is no any country code in beacon,
+	//       then wireless adapter should do active scan from ch1~11 and
+	//       passive scan from ch12~14
+	if(ieee->bGlobalDomain)
+	{
+		if (WLAN_FC_GET_STYPE(beacon->header.frame_ctl) == IEEE80211_STYPE_PROBE_RESP)
+		{
+			// Case 1: Country code
+			if(IS_COUNTRY_IE_VALID(ieee) )
+			{
+				if( !IsLegalChannel(ieee, network.channel) )
+				{
+					printk("GetScanInfo(): For Country code, filter probe response at channel(%d).\n", network.channel);
+					return;
+				}
+			}
+			// Case 2: No any country code.
+			else
+			{
+				// Filter over channel ch12~14
+				if(network.channel > 11)
+				{
+					printk("GetScanInfo(): For Global Domain, filter probe response at channel(%d).\n", network.channel);
+					return;
+				}
+			}
+		}
+		else
+		{
+			// Case 1: Country code
+			if(IS_COUNTRY_IE_VALID(ieee) )
+			{
+				if( !IsLegalChannel(ieee, network.channel) )
+				{
+					printk("GetScanInfo(): For Country code, filter beacon at channel(%d).\n",network.channel);
+					return;
+				}
+			}
+			// Case 2: No any country code.
+			else
+			{
+				// Filter over channel ch12~14
+				if(network.channel > 14)
+				{
+					printk("GetScanInfo(): For Global Domain, filter beacon at channel(%d).\n",network.channel);
+					return;
+				}
+			}
+		}
+	}
+#endif
+	/* The network parsed correctly -- so now we scan our known networks
+	 * to see if we can find it in our list.
+	 *
+	 * NOTE:  This search is definitely not optimized.  Once its doing
+	 *        the "right thing" we'll optimize it for efficiency if
+	 *        necessary */
+
+	/* Search for this entry in the list and update it if it is
+	 * already there. */
+
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	if(is_same_network(&ieee->current_network, &network, ieee)) {
+		wmm_info = ieee->current_network.wmm_info;
+		//YJ,add,080819,for hidden ap
+		if(is_beacon == 0)
+			network.flags = (~NETWORK_EMPTY_ESSID & network.flags)|(NETWORK_EMPTY_ESSID & ieee->current_network.flags);
+		else if(ieee->state == IEEE80211_LINKED)
+			ieee->NumRxBcnInPeriod++;
+		//YJ,add,080819,for hidden ap,end
+		//printk("====>network.ssid=%s cur_ssid=%s\n", network.ssid, ieee->current_network.ssid);
+		update_network(&ieee->current_network, &network);
+	}
+
+	list_for_each_entry(target, &ieee->network_list, list) {
+		if (is_same_network(target, &network, ieee))
+			break;
+		if ((oldest == NULL) ||
+		    (target->last_scanned < oldest->last_scanned))
+			oldest = target;
+	}
+
+	/* If we didn't find a match, then get a new network slot to initialize
+	 * with this beacon's information */
+	if (&target->list == &ieee->network_list) {
+		if (list_empty(&ieee->network_free_list)) {
+			/* If there are no more slots, expire the oldest */
+			list_del(&oldest->list);
+			target = oldest;
+			IEEE80211_DEBUG_SCAN("Expired '%s' (" MAC_FMT ") from "
+					     "network list.\n",
+					     escape_essid(target->ssid,
+							  target->ssid_len),
+					     MAC_ARG(target->bssid));
+		} else {
+			/* Otherwise just pull from the free list */
+			target = list_entry(ieee->network_free_list.next,
+					    struct ieee80211_network, list);
+			list_del(ieee->network_free_list.next);
+		}
+
+
+#ifdef CONFIG_IEEE80211_DEBUG
+		IEEE80211_DEBUG_SCAN("Adding '%s' (" MAC_FMT ") via %s.\n",
+				     escape_essid(network.ssid,
+						  network.ssid_len),
+				     MAC_ARG(network.bssid),
+				     WLAN_FC_GET_STYPE(beacon->header.frame_ctl) ==
+				     IEEE80211_STYPE_PROBE_RESP ?
+				     "PROBE RESPONSE" : "BEACON");
+#endif
+
+#ifdef _RTL8187_EXT_PATCH_
+	network.ext_entry = target->ext_entry;
+#endif
+		memcpy(target, &network, sizeof(*target));
+		list_add_tail(&target->list, &ieee->network_list);
+		if(ieee->softmac_features & IEEE_SOFTMAC_ASSOCIATE)
+			ieee80211_softmac_new_net(ieee,&network);
+	} else {
+		IEEE80211_DEBUG_SCAN("Updating '%s' (" MAC_FMT ") via %s.\n",
+				     escape_essid(target->ssid,
+						  target->ssid_len),
+				     MAC_ARG(target->bssid),
+				     WLAN_FC_GET_STYPE(beacon->header.frame_ctl) ==
+				     IEEE80211_STYPE_PROBE_RESP ?
+				     "PROBE RESPONSE" : "BEACON");
+
+		/* we have an entry and we are going to update it. But this entry may
+		 * be already expired. In this case we do the same as we found a new
+		 * net and call the new_net handler
+		 */
+		renew = !time_after(target->last_scanned + ieee->scan_age, jiffies);
+		//YJ,add,080819,for hidden ap
+		if(is_beacon == 0)
+			network.flags = (~NETWORK_EMPTY_ESSID & network.flags)|(NETWORK_EMPTY_ESSID & target->flags);
+		//if(strncmp(network.ssid, "linksys-c",9) == 0)
+		//	printk("====>2 network.ssid=%s FLAG=%d target.ssid=%s FLAG=%d\n", network.ssid, network.flags, target->ssid, target->flags);
+		if(((network.flags & NETWORK_EMPTY_ESSID) == NETWORK_EMPTY_ESSID) \
+		    && (((network.ssid_len > 0) && (strncmp(target->ssid, network.ssid, network.ssid_len)))\
+		    ||((ieee->current_network.ssid_len == network.ssid_len)&&(strncmp(ieee->current_network.ssid, network.ssid, network.ssid_len) == 0)&&(ieee->state == IEEE80211_NOLINK))))
+			renew = 1;
+		//YJ,add,080819,for hidden ap,end
+		update_network(target, &network);
+		if(renew && (ieee->softmac_features & IEEE_SOFTMAC_ASSOCIATE))
+			ieee80211_softmac_new_net(ieee,&network);
+	}
+
+	spin_unlock_irqrestore(&ieee->lock, flags);
+}
+
+void ieee80211_rx_mgt(struct ieee80211_device *ieee,
+		      struct ieee80211_hdr *header,
+		      struct ieee80211_rx_stats *stats)
+{
+	switch (WLAN_FC_GET_STYPE(header->frame_ctl)) {
+
+	case IEEE80211_STYPE_BEACON:
+		IEEE80211_DEBUG_MGMT("received BEACON (%d)\n",
+				     WLAN_FC_GET_STYPE(header->frame_ctl));
+		IEEE80211_DEBUG_SCAN("Beacon\n");
+		ieee80211_process_probe_response(
+			ieee, (struct ieee80211_probe_response *)header, stats);
+		break;
+
+	case IEEE80211_STYPE_PROBE_RESP:
+		IEEE80211_DEBUG_MGMT("received PROBE RESPONSE (%d)\n",
+				     WLAN_FC_GET_STYPE(header->frame_ctl));
+		IEEE80211_DEBUG_SCAN("Probe response\n");
+		ieee80211_process_probe_response(
+			ieee, (struct ieee80211_probe_response *)header, stats);
+		break;
+//rz
+#ifdef _RTL8187_EXT_PATCH_
+	case IEEE80211_STYPE_PROBE_REQ:
+		IEEE80211_DEBUG_MGMT("received PROBE REQUEST (%d)\n",
+				     WLAN_FC_GET_STYPE(header->frame_ctl));
+		IEEE80211_DEBUG_SCAN("Probe request\n");
+		///
+		if( ieee->iw_mode == ieee->iw_ext_mode && ieee->ext_patch_ieee80211_rx_mgt_on_probe_req )
+			ieee->ext_patch_ieee80211_rx_mgt_on_probe_req( ieee, (struct ieee80211_probe_request *)header, stats);
+		break;
+#endif // _RTL8187_EXT_PATCH_
+
+	}
+}
+
+#if 0
+EXPORT_SYMBOL(ieee80211_rx_mgt);
+EXPORT_SYMBOL(ieee80211_rx);
+EXPORT_SYMBOL(ieee80211_network_init);
+#ifdef _RTL8187_EXT_PATCH_
+EXPORT_SYMBOL(ieee_ext_skb_p80211_to_ether);
+#endif
+#endif
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211_softmac.c b/drivers/staging/rtl8187se/ieee80211/ieee80211_softmac.c
new file mode 100644
index 00000000000..fcffee516d5
--- /dev/null
+++ b/drivers/staging/rtl8187se/ieee80211/ieee80211_softmac.c
@@ -0,0 +1,4029 @@
+/* IEEE 802.11 SoftMAC layer
+ * Copyright (c) 2005 Andrea Merello <andreamrl@tiscali.it>
+ *
+ * Mostly extracted from the rtl8180-sa2400 driver for the
+ * in-kernel generic ieee802.11 stack.
+ *
+ * Few lines might be stolen from other part of the ieee80211
+ * stack. Copyright who own it's copyright
+ *
+ * WPA code stolen from the ipw2200 driver.
+ * Copyright who own it's copyright.
+ *
+ * released under the GPL
+ */
+
+
+#include "ieee80211.h"
+
+#include <linux/random.h>
+#include <linux/delay.h>
+#include <linux/version.h>
+#include <asm/uaccess.h>
+
+#ifdef ENABLE_DOT11D
+#include "dot11d.h"
+#endif
+u8 rsn_authen_cipher_suite[16][4] = {
+	{0x00,0x0F,0xAC,0x00}, //Use group key, //Reserved
+	{0x00,0x0F,0xAC,0x01}, //WEP-40         //RSNA default
+	{0x00,0x0F,0xAC,0x02}, //TKIP           //NONE		//{used just as default}
+	{0x00,0x0F,0xAC,0x03}, //WRAP-historical
+	{0x00,0x0F,0xAC,0x04}, //CCMP
+	{0x00,0x0F,0xAC,0x05}, //WEP-104
+};
+
+short ieee80211_is_54g(struct ieee80211_network net)
+{
+	return ((net.rates_ex_len > 0) || (net.rates_len > 4));
+}
+
+short ieee80211_is_shortslot(struct ieee80211_network net)
+{
+	return (net.capability & WLAN_CAPABILITY_SHORT_SLOT);
+}
+
+/* returns the total length needed for pleacing the RATE MFIE
+ * tag and the EXTENDED RATE MFIE tag if needed.
+ * It encludes two bytes per tag for the tag itself and its len
+ */
+unsigned int ieee80211_MFIE_rate_len(struct ieee80211_device *ieee)
+{
+	unsigned int rate_len = 0;
+
+	if (ieee->modulation & IEEE80211_CCK_MODULATION)
+		rate_len = IEEE80211_CCK_RATE_LEN + 2;
+
+	if (ieee->modulation & IEEE80211_OFDM_MODULATION)
+
+		rate_len += IEEE80211_OFDM_RATE_LEN + 2;
+
+	return rate_len;
+}
+
+/* pleace the MFIE rate, tag to the memory (double) poined.
+ * Then it updates the pointer so that
+ * it points after the new MFIE tag added.
+ */
+void ieee80211_MFIE_Brate(struct ieee80211_device *ieee, u8 **tag_p)
+{
+	u8 *tag = *tag_p;
+
+	if (ieee->modulation & IEEE80211_CCK_MODULATION){
+		*tag++ = MFIE_TYPE_RATES;
+		*tag++ = 4;
+		*tag++ = IEEE80211_BASIC_RATE_MASK | IEEE80211_CCK_RATE_1MB;
+		*tag++ = IEEE80211_BASIC_RATE_MASK | IEEE80211_CCK_RATE_2MB;
+		*tag++ = IEEE80211_BASIC_RATE_MASK | IEEE80211_CCK_RATE_5MB;
+		*tag++ = IEEE80211_BASIC_RATE_MASK | IEEE80211_CCK_RATE_11MB;
+	}
+
+	/* We may add an option for custom rates that specific HW might support */
+	*tag_p = tag;
+}
+
+void ieee80211_MFIE_Grate(struct ieee80211_device *ieee, u8 **tag_p)
+{
+	u8 *tag = *tag_p;
+
+		if (ieee->modulation & IEEE80211_OFDM_MODULATION){
+
+		*tag++ = MFIE_TYPE_RATES_EX;
+		*tag++ = 8;
+		*tag++ = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_6MB;
+		*tag++ = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_9MB;
+		*tag++ = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_12MB;
+		*tag++ = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_18MB;
+		*tag++ = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_24MB;
+		*tag++ = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_36MB;
+		*tag++ = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_48MB;
+		*tag++ = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_54MB;
+
+	}
+
+	/* We may add an option for custom rates that specific HW might support */
+	*tag_p = tag;
+}
+
+
+void ieee80211_WMM_Info(struct ieee80211_device *ieee, u8 **tag_p) {
+	u8 *tag = *tag_p;
+
+	*tag++ = MFIE_TYPE_GENERIC; //0
+	*tag++ = 7;
+	*tag++ = 0x00;
+	*tag++ = 0x50;
+	*tag++ = 0xf2;
+	*tag++ = 0x02;//5
+	*tag++ = 0x00;
+	*tag++ = 0x01;
+#ifdef SUPPORT_USPD
+	if(ieee->current_network.wmm_info & 0x80) {
+		*tag++ = 0x0f|MAX_SP_Len;
+	} else {
+		*tag++ = MAX_SP_Len;
+	}
+#else
+	*tag++ = MAX_SP_Len;
+#endif
+	*tag_p = tag;
+}
+
+#ifdef THOMAS_TURBO
+void ieee80211_TURBO_Info(struct ieee80211_device *ieee, u8 **tag_p) {
+	u8 *tag = *tag_p;
+
+        *tag++ = MFIE_TYPE_GENERIC; //0
+        *tag++ = 7;
+        *tag++ = 0x00;
+        *tag++ = 0xe0;
+        *tag++ = 0x4c;
+        *tag++ = 0x01;//5
+        *tag++ = 0x02;
+        *tag++ = 0x11;
+	*tag++ = 0x00;
+
+	*tag_p = tag;
+	printk(KERN_ALERT "This is enable turbo mode IE process\n");
+}
+#endif
+
+void enqueue_mgmt(struct ieee80211_device *ieee, struct sk_buff *skb)
+{
+	int nh;
+	nh = (ieee->mgmt_queue_head +1) % MGMT_QUEUE_NUM;
+
+/*
+ * if the queue is full but we have newer frames then
+ * just overwrites the oldest.
+ *
+ * if (nh == ieee->mgmt_queue_tail)
+ *		return -1;
+ */
+	ieee->mgmt_queue_head = nh;
+	ieee->mgmt_queue_ring[nh] = skb;
+
+	//return 0;
+}
+
+struct sk_buff *dequeue_mgmt(struct ieee80211_device *ieee)
+{
+	struct sk_buff *ret;
+
+	if(ieee->mgmt_queue_tail == ieee->mgmt_queue_head)
+		return NULL;
+
+	ret = ieee->mgmt_queue_ring[ieee->mgmt_queue_tail];
+
+	ieee->mgmt_queue_tail =
+		(ieee->mgmt_queue_tail+1) % MGMT_QUEUE_NUM;
+
+	return ret;
+}
+
+void init_mgmt_queue(struct ieee80211_device *ieee)
+{
+	ieee->mgmt_queue_tail = ieee->mgmt_queue_head = 0;
+}
+
+
+void ieee80211_sta_wakeup(struct ieee80211_device *ieee, short nl);
+
+inline void softmac_mgmt_xmit(struct sk_buff *skb, struct ieee80211_device *ieee)
+{
+	unsigned long flags;
+	short single = ieee->softmac_features & IEEE_SOFTMAC_SINGLE_QUEUE;
+	struct ieee80211_hdr_3addr  *header=
+		(struct ieee80211_hdr_3addr  *) skb->data;
+
+
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	/* called with 2nd param 0, no mgmt lock required */
+	ieee80211_sta_wakeup(ieee,0);
+
+	if(single){
+		if(ieee->queue_stop){
+
+			enqueue_mgmt(ieee,skb);
+		}else{
+			header->seq_ctl = cpu_to_le16(ieee->seq_ctrl[0]<<4);
+
+			if (ieee->seq_ctrl[0] == 0xFFF)
+				ieee->seq_ctrl[0] = 0;
+			else
+				ieee->seq_ctrl[0]++;
+
+			/* avoid watchdog triggers */
+			ieee->dev->trans_start = jiffies;
+			ieee->softmac_data_hard_start_xmit(skb,ieee->dev,ieee->basic_rate);
+		}
+
+		spin_unlock_irqrestore(&ieee->lock, flags);
+	}else{
+		spin_unlock_irqrestore(&ieee->lock, flags);
+		spin_lock_irqsave(&ieee->mgmt_tx_lock, flags);
+
+		header->seq_ctl = cpu_to_le16(ieee->seq_ctrl[0] << 4);
+
+		if (ieee->seq_ctrl[0] == 0xFFF)
+			ieee->seq_ctrl[0] = 0;
+		else
+			ieee->seq_ctrl[0]++;
+
+		/* avoid watchdog triggers */
+		ieee->dev->trans_start = jiffies;
+		ieee->softmac_hard_start_xmit(skb,ieee->dev);
+
+		spin_unlock_irqrestore(&ieee->mgmt_tx_lock, flags);
+	}
+}
+
+
+inline void softmac_ps_mgmt_xmit(struct sk_buff *skb, struct ieee80211_device *ieee)
+{
+
+	short single = ieee->softmac_features & IEEE_SOFTMAC_SINGLE_QUEUE;
+	struct ieee80211_hdr_3addr  *header =
+		(struct ieee80211_hdr_3addr  *) skb->data;
+
+
+	if(single){
+
+		header->seq_ctl = cpu_to_le16(ieee->seq_ctrl[0] << 4);
+
+		if (ieee->seq_ctrl[0] == 0xFFF)
+			ieee->seq_ctrl[0] = 0;
+		else
+			ieee->seq_ctrl[0]++;
+
+		/* avoid watchdog triggers */
+		ieee->dev->trans_start = jiffies;
+		ieee->softmac_data_hard_start_xmit(skb,ieee->dev,ieee->basic_rate);
+
+	}else{
+
+		header->seq_ctl = cpu_to_le16(ieee->seq_ctrl[0] << 4);
+
+		if (ieee->seq_ctrl[0] == 0xFFF)
+			ieee->seq_ctrl[0] = 0;
+		else
+			ieee->seq_ctrl[0]++;
+
+		/* avoid watchdog triggers */
+		ieee->dev->trans_start = jiffies;
+		ieee->softmac_hard_start_xmit(skb,ieee->dev);
+
+	}
+//	dev_kfree_skb_any(skb);//edit by thomas
+}
+//by amy for power save
+inline struct sk_buff *ieee80211_disassociate_skb(
+							struct ieee80211_network *beacon,
+							struct ieee80211_device *ieee,
+							u8	asRsn)
+{
+	struct sk_buff *skb;
+	struct ieee80211_disassoc_frame *disass;
+
+	skb = dev_alloc_skb(sizeof(struct ieee80211_disassoc_frame));
+	if (!skb)
+		return NULL;
+
+	disass = (struct ieee80211_disassoc_frame *) skb_put(skb,sizeof(struct ieee80211_disassoc_frame));
+	disass->header.frame_ctl = cpu_to_le16(IEEE80211_STYPE_DISASSOC);
+	disass->header.duration_id = 0;
+
+	memcpy(disass->header.addr1, beacon->bssid, ETH_ALEN);
+	memcpy(disass->header.addr2, ieee->dev->dev_addr, ETH_ALEN);
+	memcpy(disass->header.addr3, beacon->bssid, ETH_ALEN);
+
+	disass->reasoncode = asRsn;
+	return skb;
+}
+void
+SendDisassociation(
+        struct ieee80211_device *ieee,
+        u8*                     asSta,
+        u8                      asRsn
+)
+{
+        struct ieee80211_network *beacon = &ieee->current_network;
+        struct sk_buff *skb;
+        skb = ieee80211_disassociate_skb(beacon,ieee,asRsn);
+        if (skb){
+                softmac_mgmt_xmit(skb, ieee);
+                //dev_kfree_skb_any(skb);//edit by thomas
+        }
+}
+
+//by amy for power save
+inline struct sk_buff *ieee80211_probe_req(struct ieee80211_device *ieee)
+{
+	unsigned int len,rate_len;
+	u8 *tag;
+	struct sk_buff *skb;
+	struct ieee80211_probe_request *req;
+
+#ifdef _RTL8187_EXT_PATCH_
+	short extMore = 0;
+	if(ieee->ext_patch_ieee80211_probe_req_1)
+		extMore = ieee->ext_patch_ieee80211_probe_req_1(ieee);
+#endif
+
+	len = ieee->current_network.ssid_len;
+
+	rate_len = ieee80211_MFIE_rate_len(ieee);
+
+#ifdef _RTL8187_EXT_PATCH_
+	if(!extMore)
+#endif
+	skb = dev_alloc_skb(sizeof(struct ieee80211_probe_request) +
+			    2 + len + rate_len);
+#ifdef _RTL8187_EXT_PATCH_
+	else
+		skb = dev_alloc_skb(sizeof(struct ieee80211_probe_request) +
+			    2 + len + rate_len+128); // MESHID + CAP
+#endif
+
+	if (!skb)
+		return NULL;
+
+	req = (struct ieee80211_probe_request *) skb_put(skb,sizeof(struct ieee80211_probe_request));
+	req->header.frame_ctl = cpu_to_le16(IEEE80211_STYPE_PROBE_REQ);
+	req->header.duration_id = 0; //FIXME: is this OK ?
+
+	memset(req->header.addr1, 0xff, ETH_ALEN);
+	memcpy(req->header.addr2, ieee->dev->dev_addr, ETH_ALEN);
+	memset(req->header.addr3, 0xff, ETH_ALEN);
+
+	tag = (u8 *) skb_put(skb,len+2+rate_len);
+
+	*tag++ = MFIE_TYPE_SSID;
+	*tag++ = len;
+	memcpy(tag, ieee->current_network.ssid, len);
+	tag += len;
+	ieee80211_MFIE_Brate(ieee,&tag);
+	ieee80211_MFIE_Grate(ieee,&tag);
+
+#ifdef _RTL8187_EXT_PATCH_
+	if(extMore)
+		ieee->ext_patch_ieee80211_probe_req_2(ieee, skb, tag);
+#endif
+	return skb;
+}
+
+struct sk_buff *ieee80211_get_beacon_(struct ieee80211_device *ieee);
+
+//#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20)
+//void ext_ieee80211_send_beacon_wq(struct work_struct *work)
+//{
+//	struct ieee80211_device *ieee = container_of(work, struct ieee80211_device, ext_send_beacon_wq);
+//#else
+void ext_ieee80211_send_beacon_wq(struct ieee80211_device *ieee)
+{
+//#endif
+
+	struct sk_buff *skb;
+
+	//unsigned long flags;
+
+	skb = ieee80211_get_beacon_(ieee);
+
+	if (skb){
+		softmac_mgmt_xmit(skb, ieee);
+		ieee->softmac_stats.tx_beacons++;
+		dev_kfree_skb_any(skb);//edit by thomas
+	}
+
+
+	//printk(KERN_WARNING "[1] beacon sending!\n");
+	ieee->beacon_timer.expires = jiffies +
+		(MSECS( ieee->current_network.beacon_interval -5));
+
+	//spin_lock_irqsave(&ieee->beacon_lock,flags);
+	if(ieee->beacon_txing)
+		add_timer(&ieee->beacon_timer);
+	//spin_unlock_irqrestore(&ieee->beacon_lock,flags);
+}
+
+void ieee80211_send_beacon(struct ieee80211_device *ieee)
+{
+	struct sk_buff *skb;
+
+	//unsigned long flags;
+
+	skb = ieee80211_get_beacon_(ieee);
+
+	if (skb){
+		softmac_mgmt_xmit(skb, ieee);
+		ieee->softmac_stats.tx_beacons++;
+		dev_kfree_skb_any(skb);//edit by thomas
+	}
+
+	//printk(KERN_WARNING "[1] beacon sending!\n");
+	ieee->beacon_timer.expires = jiffies +
+		(MSECS( ieee->current_network.beacon_interval -5));
+
+	//spin_lock_irqsave(&ieee->beacon_lock,flags);
+	if(ieee->beacon_txing)
+		add_timer(&ieee->beacon_timer);
+	//spin_unlock_irqrestore(&ieee->beacon_lock,flags);
+}
+
+
+void ieee80211_send_beacon_cb(unsigned long _ieee)
+{
+	struct ieee80211_device *ieee =
+		(struct ieee80211_device *) _ieee;
+	unsigned long flags;
+
+	spin_lock_irqsave(&ieee->beacon_lock, flags);
+	ieee80211_send_beacon(ieee);
+	spin_unlock_irqrestore(&ieee->beacon_lock, flags);
+}
+
+#ifdef _RTL8187_EXT_PATCH_
+
+inline struct sk_buff *ieee80211_probe_req_with_SSID(struct ieee80211_device *ieee, char *ssid, int len_ssid)
+{
+	unsigned int len,rate_len;
+	u8 *tag;
+	struct sk_buff *skb;
+	struct ieee80211_probe_request *req;
+
+#ifdef _RTL8187_EXT_PATCH_
+	short extMore = 0;
+	if(ieee->ext_patch_ieee80211_probe_req_1)
+		extMore = ieee->ext_patch_ieee80211_probe_req_1(ieee);
+#endif
+
+	len = len_ssid;
+
+	rate_len = ieee80211_MFIE_rate_len(ieee);
+
+#ifdef _RTL8187_EXT_PATCH_
+	if(!extMore)
+#endif
+	skb = dev_alloc_skb(sizeof(struct ieee80211_probe_request) +
+			    2 + len + rate_len);
+#ifdef _RTL8187_EXT_PATCH_
+	else
+		skb = dev_alloc_skb(sizeof(struct ieee80211_probe_request) +
+			    2 + len + rate_len+128); // MESHID + CAP
+#endif
+
+	if (!skb)
+		return NULL;
+
+	req = (struct ieee80211_probe_request *) skb_put(skb,sizeof(struct ieee80211_probe_request));
+	req->header.frame_ctl = cpu_to_le16(IEEE80211_STYPE_PROBE_REQ);
+	req->header.duration_id = 0; //FIXME: is this OK ?
+
+	memset(req->header.addr1, 0xff, ETH_ALEN);
+	memcpy(req->header.addr2, ieee->dev->dev_addr, ETH_ALEN);
+	memset(req->header.addr3, 0xff, ETH_ALEN);
+
+	tag = (u8 *) skb_put(skb,len+2+rate_len);
+
+	*tag++ = MFIE_TYPE_SSID;
+	*tag++ = len;
+	if(len)
+	{
+		memcpy(tag, ssid, len);
+		tag += len;
+	}
+
+	ieee80211_MFIE_Brate(ieee,&tag);
+	ieee80211_MFIE_Grate(ieee,&tag);
+
+#ifdef _RTL8187_EXT_PATCH_
+	if(extMore)
+		ieee->ext_patch_ieee80211_probe_req_2(ieee, skb, tag);
+#endif
+	return skb;
+}
+
+#endif // _RTL8187_EXT_PATCH_
+
+
+void ieee80211_send_probe(struct ieee80211_device *ieee)
+{
+	struct sk_buff *skb;
+
+#ifdef _RTL8187_EXT_PATCH_
+	if(ieee->iw_mode == ieee->iw_ext_mode)
+		skb = ieee80211_probe_req_with_SSID(ieee, NULL, 0);
+	else
+#endif
+	skb = ieee80211_probe_req(ieee);
+	if (skb){
+		softmac_mgmt_xmit(skb, ieee);
+		ieee->softmac_stats.tx_probe_rq++;
+		//dev_kfree_skb_any(skb);//edit by thomas
+	}
+}
+
+void ieee80211_send_probe_requests(struct ieee80211_device *ieee)
+{
+	if (ieee->active_scan && (ieee->softmac_features & IEEE_SOFTMAC_PROBERQ)){
+		ieee80211_send_probe(ieee);
+		ieee80211_send_probe(ieee);
+	}
+}
+
+/* this performs syncro scan blocking the caller until all channels
+ * in the allowed channel map has been checked.
+ */
+void ieee80211_softmac_scan_syncro(struct ieee80211_device *ieee)
+{
+	short ch = 0;
+#ifdef ENABLE_DOT11D
+	u8 channel_map[MAX_CHANNEL_NUMBER+1];
+	memcpy(channel_map, GET_DOT11D_INFO(ieee)->channel_map, MAX_CHANNEL_NUMBER+1);
+#endif
+	down(&ieee->scan_sem);
+//	printk("==================> Sync scan\n");
+//	dump_chnl_map(channel_map);
+
+	while(1)
+	{
+
+		do{
+			ch++;
+			if (ch > MAX_CHANNEL_NUMBER)
+				goto out; /* scan completed */
+
+#ifdef ENABLE_DOT11D
+		}while(!channel_map[ch]);
+#else
+		}while(!ieee->channel_map[ch]);
+#endif
+		/* this fuction can be called in two situations
+		 * 1- We have switched to ad-hoc mode and we are
+		 *    performing a complete syncro scan before conclude
+		 *    there are no interesting cell and to create a
+		 *    new one. In this case the link state is
+		 *    IEEE80211_NOLINK until we found an interesting cell.
+		 *    If so the ieee8021_new_net, called by the RX path
+		 *    will set the state to IEEE80211_LINKED, so we stop
+		 *    scanning
+		 * 2- We are linked and the root uses run iwlist scan.
+		 *    So we switch to IEEE80211_LINKED_SCANNING to remember
+		 *    that we are still logically linked (not interested in
+		 *    new network events, despite for updating the net list,
+		 *    but we are temporarly 'unlinked' as the driver shall
+		 *    not filter RX frames and the channel is changing.
+		 * So the only situation in witch are interested is to check
+		 * if the state become LINKED because of the #1 situation
+		 */
+
+		if (ieee->state == IEEE80211_LINKED)
+			goto out;
+
+		ieee->set_chan(ieee->dev, ch);
+//		printk("=====>channel=%d   ",ch);
+#ifdef ENABLE_DOT11D
+		if(channel_map[ch] == 1)
+#endif
+		{
+//			printk("====send probe request\n");
+			ieee80211_send_probe_requests(ieee);
+		}
+		/* this prevent excessive time wait when we
+		 * need to wait for a syncro scan to end..
+		 */
+		if (ieee->sync_scan_hurryup)
+			goto out;
+
+
+		msleep_interruptible_rtl(IEEE80211_SOFTMAC_SCAN_TIME);
+
+	}
+out:
+	ieee->sync_scan_hurryup = 0;
+	up(&ieee->scan_sem);
+#ifdef ENABLE_DOT11D
+	if(IS_DOT11D_ENABLE(ieee))
+		DOT11D_ScanComplete(ieee);
+#endif
+}
+
+void ieee80211_softmac_ips_scan_syncro(struct ieee80211_device *ieee)
+{
+	int ch;
+        unsigned int watch_dog = 0;
+#ifdef ENABLE_DOT11D
+	u8 channel_map[MAX_CHANNEL_NUMBER+1];
+	memcpy(channel_map, GET_DOT11D_INFO(ieee)->channel_map, MAX_CHANNEL_NUMBER+1);
+#endif
+        down(&ieee->scan_sem);
+	ch = ieee->current_network.channel;
+//      	if(ieee->sync_scan_hurryup)
+//	{
+
+//		printk("stop scan sync\n");
+//   		goto out;
+//  	}
+//	printk("=======hh===============>ips scan\n");
+     	while(1)
+        {
+                /* this fuction can be called in two situations
+                 * 1- We have switched to ad-hoc mode and we are
+                 *    performing a complete syncro scan before conclude
+                 *    there are no interesting cell and to create a
+                 *    new one. In this case the link state is
+                 *    IEEE80211_NOLINK until we found an interesting cell.
+                 *    If so the ieee8021_new_net, called by the RX path
+                 *    will set the state to IEEE80211_LINKED, so we stop
+                 *    scanning
+                 * 2- We are linked and the root uses run iwlist scan.
+                 *    So we switch to IEEE80211_LINKED_SCANNING to remember
+                 *    that we are still logically linked (not interested in
+                 *    new network events, despite for updating the net list,
+                 *    but we are temporarly 'unlinked' as the driver shall
+                 *    not filter RX frames and the channel is changing.
+                 * So the only situation in witch are interested is to check
+                 * if the state become LINKED because of the #1 situation
+                 */
+		if (ieee->state == IEEE80211_LINKED)
+		{
+			goto out;
+		}
+#ifdef ENABLE_DOT11D
+		if(channel_map[ieee->current_network.channel] > 0)
+#endif
+		{
+			ieee->set_chan(ieee->dev, ieee->current_network.channel);
+//			printk("======>channel=%d  ",ieee->current_network.channel);
+		}
+#ifdef ENABLE_DOT11D
+		if(channel_map[ieee->current_network.channel] == 1)
+#endif
+		{
+//			printk("====send probe request\n");
+			ieee80211_send_probe_requests(ieee);
+                }
+		/* this prevent excessive time wait when we
+                 * need to wait for a syncro scan to end..
+                 */
+//                if (ieee->sync_scan_hurryup)
+//                        goto out;
+
+		msleep_interruptible_rtl(IEEE80211_SOFTMAC_SCAN_TIME);
+
+		do{
+			if (watch_dog++ >= MAX_CHANNEL_NUMBER)
+		//	if (++watch_dog >= 15);//MAX_CHANNEL_NUMBER)  //YJ,modified,080630
+				goto out; /* scan completed */
+
+			ieee->current_network.channel = (ieee->current_network.channel + 1)%MAX_CHANNEL_NUMBER;
+#ifdef ENABLE_DOT11D
+		}while(!channel_map[ieee->current_network.channel]);
+#else
+		}while(!ieee->channel_map[ieee->current_network.channel]);
+#endif
+        }
+out:
+	//ieee->sync_scan_hurryup = 0;
+   	//ieee->set_chan(ieee->dev, ch);
+   	//ieee->current_network.channel = ch;
+	ieee->actscanning = false;
+	up(&ieee->scan_sem);
+#ifdef ENABLE_DOT11D
+	if(IS_DOT11D_ENABLE(ieee))
+		DOT11D_ScanComplete(ieee);
+#endif
+}
+
+
+#if 0
+/* called both by wq with ieee->lock held */
+void ieee80211_softmac_scan(struct ieee80211_device *ieee)
+{
+	short watchdog = 0;
+
+	do{
+		ieee->current_network.channel =
+			(ieee->current_network.channel + 1) % MAX_CHANNEL_NUMBER;
+		if (watchdog++ > MAX_CHANNEL_NUMBER)
+				return; /* no good chans */
+
+	}while(!ieee->channel_map[ieee->current_network.channel]);
+
+
+	schedule_work(&ieee->softmac_scan_wq);
+}
+#endif
+#ifdef ENABLE_IPS
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+void ieee80211_softmac_scan_wq(struct work_struct *work)
+{
+	struct delayed_work *dwork = container_of(work, struct delayed_work, work);
+	struct ieee80211_device *ieee = container_of(dwork, struct ieee80211_device, softmac_scan_wq);
+#else
+void ieee80211_softmac_scan_wq(struct ieee80211_device *ieee)
+{
+#endif
+	static short watchdog = 0;
+#ifdef ENABLE_DOT11D
+	u8 channel_map[MAX_CHANNEL_NUMBER+1];
+	memcpy(channel_map, GET_DOT11D_INFO(ieee)->channel_map, MAX_CHANNEL_NUMBER+1);
+#endif
+//	printk("ieee80211_softmac_scan_wq ENABLE_IPS\n");
+//	printk("in %s\n",__FUNCTION__);
+	down(&ieee->scan_sem);
+
+	do{
+		ieee->current_network.channel =
+			(ieee->current_network.channel + 1) % MAX_CHANNEL_NUMBER;
+		if (watchdog++ > MAX_CHANNEL_NUMBER)
+				goto out; /* no good chans */
+
+#ifdef ENABLE_DOT11D
+ 	}while(!channel_map[ieee->current_network.channel]);
+#else
+ 	}while(!ieee->channel_map[ieee->current_network.channel]);
+#endif
+
+	//printk("current_network.channel:%d\n", ieee->current_network.channel);
+	if (ieee->scanning == 0 )
+	{
+		printk("error out, scanning = 0\n");
+		goto out;
+	}
+	ieee->set_chan(ieee->dev, ieee->current_network.channel);
+#ifdef ENABLE_DOT11D
+	if(channel_map[ieee->current_network.channel] == 1)
+#endif
+		ieee80211_send_probe_requests(ieee);
+
+	queue_delayed_work(ieee->wq, &ieee->softmac_scan_wq, IEEE80211_SOFTMAC_SCAN_TIME);
+	up(&ieee->scan_sem);
+	return;
+out:
+	ieee->actscanning = false;
+	watchdog = 0;
+	ieee->scanning = 0;
+	up(&ieee->scan_sem);
+
+#ifdef ENABLE_DOT11D
+	if(IS_DOT11D_ENABLE(ieee))
+		DOT11D_ScanComplete(ieee);
+#endif
+	return;
+}
+#else
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+void ieee80211_softmac_scan_wq(struct work_struct *work)
+{
+        struct delayed_work *dwork = container_of(work, struct delayed_work, work);
+        struct ieee80211_device *ieee = container_of(work, struct ieee80211_device, softmac_scan_wq);
+#else
+void ieee80211_softmac_scan_wq(struct ieee80211_device *ieee)
+{
+#endif
+
+        short watchdog = 0;
+#ifdef ENABLE_DOT11D
+	u8 channel_map[MAX_CHANNEL_NUMBER+1];
+	memcpy(channel_map, GET_DOT11D_INFO(ieee)->channel_map, MAX_CHANNEL_NUMBER+1);
+#endif
+//      printk("enter scan wq,watchdog is %d\n",watchdog);
+        down(&ieee->scan_sem);
+
+        do{
+                ieee->current_network.channel =
+                        (ieee->current_network.channel + 1) % MAX_CHANNEL_NUMBER;
+                if (watchdog++ > MAX_CHANNEL_NUMBER)
+                                goto out; /* no good chans */
+
+#ifdef ENABLE_DOT11D
+        }while(!channel_map[ieee->current_network.channel]);
+#else
+        }while(!ieee->channel_map[ieee->current_network.channel]);
+#endif
+
+//      printk("current_network.channel:%d\n", ieee->current_network.channel);
+        if (ieee->scanning == 0 )
+        {
+                printk("error out, scanning = 0\n");
+                goto out;
+        }
+        ieee->set_chan(ieee->dev, ieee->current_network.channel);
+#ifdef ENABLE_DOT11D
+	if(channel_map[ieee->current_network.channel] == 1)
+#endif
+		ieee80211_send_probe_requests(ieee);
+
+	queue_delayed_work(ieee->wq, &ieee->softmac_scan_wq, IEEE80211_SOFTMAC_SCAN_TIME);
+out:
+	up(&ieee->scan_sem);
+#ifdef ENABLE_DOT11D
+	if(IS_DOT11D_ENABLE(ieee))
+		DOT11D_ScanComplete(ieee);
+#endif
+}
+
+#endif
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)
+void ieee80211_softmac_scan_cb(unsigned long _dev)
+{
+	unsigned long flags;
+	struct ieee80211_device *ieee = (struct ieee80211_device *)_dev;
+
+	spin_lock_irqsave(&ieee->lock, flags);
+	ieee80211_softmac_scan(ieee);
+	spin_unlock_irqrestore(&ieee->lock, flags);
+}
+#endif
+
+
+void ieee80211_beacons_start(struct ieee80211_device *ieee)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&ieee->beacon_lock,flags);
+
+	ieee->beacon_txing = 1;
+	ieee80211_send_beacon(ieee);
+
+	spin_unlock_irqrestore(&ieee->beacon_lock,flags);
+}
+
+void ieee80211_beacons_stop(struct ieee80211_device *ieee)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&ieee->beacon_lock,flags);
+
+	ieee->beacon_txing = 0;
+ 	del_timer_sync(&ieee->beacon_timer);
+
+	spin_unlock_irqrestore(&ieee->beacon_lock,flags);
+
+}
+
+
+void ieee80211_stop_send_beacons(struct ieee80211_device *ieee)
+{
+	if(ieee->stop_send_beacons)
+		ieee->stop_send_beacons(ieee->dev);
+	if (ieee->softmac_features & IEEE_SOFTMAC_BEACONS)
+		ieee80211_beacons_stop(ieee);
+}
+
+
+void ieee80211_start_send_beacons(struct ieee80211_device *ieee)
+{
+	if(ieee->start_send_beacons)
+		ieee->start_send_beacons(ieee->dev);
+	if(ieee->softmac_features & IEEE_SOFTMAC_BEACONS)
+		ieee80211_beacons_start(ieee);
+}
+
+
+void ieee80211_softmac_stop_scan(struct ieee80211_device *ieee)
+{
+//	unsigned long flags;
+
+	//ieee->sync_scan_hurryup = 1;
+
+	down(&ieee->scan_sem);
+//	spin_lock_irqsave(&ieee->lock, flags);
+
+	if (ieee->scanning == 1){
+		ieee->scanning = 0;
+		//del_timer_sync(&ieee->scan_timer);
+		cancel_delayed_work(&ieee->softmac_scan_wq);
+	}
+
+//	spin_unlock_irqrestore(&ieee->lock, flags);
+	up(&ieee->scan_sem);
+}
+
+void ieee80211_stop_scan(struct ieee80211_device *ieee)
+{
+	if (ieee->softmac_features & IEEE_SOFTMAC_SCAN)
+		ieee80211_softmac_stop_scan(ieee);
+	else
+		ieee->stop_scan(ieee->dev);
+}
+
+/* called with ieee->lock held */
+void ieee80211_start_scan(struct ieee80211_device *ieee)
+{
+#ifdef ENABLE_DOT11D
+	if(IS_DOT11D_ENABLE(ieee) )
+	{
+		if(IS_COUNTRY_IE_VALID(ieee))
+		{
+			RESET_CIE_WATCHDOG(ieee);
+		}
+	}
+#endif
+	if (ieee->softmac_features & IEEE_SOFTMAC_SCAN){
+		if (ieee->scanning == 0)
+		{
+			ieee->scanning = 1;
+			//ieee80211_softmac_scan(ieee);
+		//	queue_work(ieee->wq, &ieee->softmac_scan_wq);
+		//care this,1203,2007,by lawrence
+#if 1
+			queue_delayed_work(ieee->wq, &ieee->softmac_scan_wq,0);
+#endif
+		}
+	}else
+		ieee->start_scan(ieee->dev);
+
+}
+
+/* called with wx_sem held */
+void ieee80211_start_scan_syncro(struct ieee80211_device *ieee)
+{
+#ifdef ENABLE_DOT11D
+	if(IS_DOT11D_ENABLE(ieee) )
+	{
+		if(IS_COUNTRY_IE_VALID(ieee))
+		{
+			RESET_CIE_WATCHDOG(ieee);
+		}
+	}
+#endif
+	ieee->sync_scan_hurryup = 0;
+
+	if (ieee->softmac_features & IEEE_SOFTMAC_SCAN)
+		ieee80211_softmac_scan_syncro(ieee);
+	else
+		ieee->scan_syncro(ieee->dev);
+
+}
+
+inline struct sk_buff *ieee80211_authentication_req(struct ieee80211_network *beacon,
+	struct ieee80211_device *ieee, int challengelen)
+{
+	struct sk_buff *skb;
+	struct ieee80211_authentication *auth;
+
+	skb = dev_alloc_skb(sizeof(struct ieee80211_authentication) + challengelen);
+
+	if (!skb) return NULL;
+
+	auth = (struct ieee80211_authentication *)
+		skb_put(skb, sizeof(struct ieee80211_authentication));
+
+	auth->header.frame_ctl = IEEE80211_STYPE_AUTH;
+	if (challengelen) auth->header.frame_ctl |= IEEE80211_FCTL_WEP;
+
+	auth->header.duration_id = 0x013a; //FIXME
+
+	memcpy(auth->header.addr1, beacon->bssid, ETH_ALEN);
+	memcpy(auth->header.addr2, ieee->dev->dev_addr, ETH_ALEN);
+	memcpy(auth->header.addr3, beacon->bssid, ETH_ALEN);
+
+	auth->algorithm = ieee->open_wep ? WLAN_AUTH_OPEN : WLAN_AUTH_SHARED_KEY;
+
+	auth->transaction = cpu_to_le16(ieee->associate_seq);
+	ieee->associate_seq++;
+
+	auth->status = cpu_to_le16(WLAN_STATUS_SUCCESS);
+
+	return skb;
+
+}
+
+static struct sk_buff* ieee80211_probe_resp(struct ieee80211_device *ieee, u8 *dest)
+{
+	u8 *tag;
+	int beacon_size;
+	struct ieee80211_probe_response *beacon_buf;
+	struct sk_buff *skb;
+	int encrypt;
+	int atim_len,erp_len;
+	struct ieee80211_crypt_data* crypt;
+
+	char *ssid = ieee->current_network.ssid;
+	int ssid_len = ieee->current_network.ssid_len;
+	int rate_len = ieee->current_network.rates_len+2;
+	int rate_ex_len = ieee->current_network.rates_ex_len;
+	int wpa_ie_len = ieee->wpa_ie_len;
+	if(rate_ex_len > 0) rate_ex_len+=2;
+
+	if(ieee->current_network.capability & WLAN_CAPABILITY_IBSS)
+		atim_len = 4;
+	else
+		atim_len = 0;
+
+	if(ieee80211_is_54g(ieee->current_network))
+		erp_len = 3;
+	else
+		erp_len = 0;
+
+	beacon_size = sizeof(struct ieee80211_probe_response)+
+		ssid_len
+		+3 //channel
+		+rate_len
+		+rate_ex_len
+		+atim_len
+		+wpa_ie_len
+		+erp_len;
+
+	skb = dev_alloc_skb(beacon_size);
+
+	if (!skb)
+		return NULL;
+
+	beacon_buf = (struct ieee80211_probe_response*) skb_put(skb, beacon_size);
+
+	memcpy (beacon_buf->header.addr1, dest,ETH_ALEN);
+	memcpy (beacon_buf->header.addr2, ieee->dev->dev_addr, ETH_ALEN);
+	memcpy (beacon_buf->header.addr3, ieee->current_network.bssid, ETH_ALEN);
+
+	beacon_buf->header.duration_id = 0; //FIXME
+	beacon_buf->beacon_interval =
+		cpu_to_le16(ieee->current_network.beacon_interval);
+	beacon_buf->capability =
+		cpu_to_le16(ieee->current_network.capability & WLAN_CAPABILITY_IBSS);
+
+	if(ieee->short_slot && (ieee->current_network.capability & WLAN_CAPABILITY_SHORT_SLOT))
+		cpu_to_le16((beacon_buf->capability |= WLAN_CAPABILITY_SHORT_SLOT));
+
+	crypt = ieee->crypt[ieee->tx_keyidx];
+
+	encrypt = ieee->host_encrypt && crypt && crypt->ops &&
+		((0 == strcmp(crypt->ops->name, "WEP")) || wpa_ie_len);
+
+	if (encrypt)
+		beacon_buf->capability |= cpu_to_le16(WLAN_CAPABILITY_PRIVACY);
+
+
+	beacon_buf->header.frame_ctl = cpu_to_le16(IEEE80211_STYPE_PROBE_RESP);
+
+	beacon_buf->info_element.id = MFIE_TYPE_SSID;
+	beacon_buf->info_element.len = ssid_len;
+
+	tag = (u8*) beacon_buf->info_element.data;
+
+	memcpy(tag, ssid, ssid_len);
+
+	tag += ssid_len;
+
+	*(tag++) = MFIE_TYPE_RATES;
+	*(tag++) = rate_len-2;
+	memcpy(tag,ieee->current_network.rates,rate_len-2);
+	tag+=rate_len-2;
+
+	*(tag++) = MFIE_TYPE_DS_SET;
+	*(tag++) = 1;
+	*(tag++) = ieee->current_network.channel;
+
+	if(atim_len){
+		*(tag++) = MFIE_TYPE_IBSS_SET;
+		*(tag++) = 2;
+		*((u16*)(tag)) = cpu_to_le16(ieee->current_network.atim_window);
+		tag+=2;
+	}
+
+	if(erp_len){
+		*(tag++) = MFIE_TYPE_ERP;
+		*(tag++) = 1;
+		*(tag++) = 0;
+	}
+
+	if(rate_ex_len){
+		*(tag++) = MFIE_TYPE_RATES_EX;
+		*(tag++) = rate_ex_len-2;
+		memcpy(tag,ieee->current_network.rates_ex,rate_ex_len-2);
+		tag+=rate_ex_len-2;
+	}
+
+	if (wpa_ie_len)
+	{
+		if (ieee->iw_mode == IW_MODE_ADHOC)
+		{//as Windows will set pairwise key same as the group key which is not allowed in Linux, so set this for IOT issue. WB 2008.07.07
+			memcpy(&ieee->wpa_ie[14], &ieee->wpa_ie[8], 4);
+		}
+
+		memcpy(tag, ieee->wpa_ie, ieee->wpa_ie_len);
+	}
+
+	skb->dev = ieee->dev;
+	return skb;
+}
+#ifdef _RTL8187_EXT_PATCH_
+struct sk_buff* ieee80211_ext_probe_resp_by_net(struct ieee80211_device *ieee, u8 *dest, struct ieee80211_network *net)
+{
+	u8 *tag;
+	int beacon_size;
+	struct ieee80211_probe_response *beacon_buf;
+	struct sk_buff *skb;
+	int encrypt;
+	int atim_len,erp_len;
+	struct ieee80211_crypt_data* crypt;
+	u8 broadcast_addr[] = {0xff,0xff,0xff,0xff,0xff,0xff};
+	int wpa_ie_len = ieee->wpa_ie_len;
+	char *ssid = net->ssid;
+	int ssid_len = net->ssid_len;
+
+	int rate_len = ieee->current_network.rates_len+2;
+	int rate_ex_len = ieee->current_network.rates_ex_len;
+	if(rate_ex_len > 0) rate_ex_len+=2;
+
+	if( ieee->meshScanMode&4)
+		ieee->current_network.channel = ieee->ext_patch_ieee80211_ext_stop_scan_wq_set_channel(ieee);
+	if( ieee->meshScanMode&6)
+	{
+
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,5,0)
+		queue_work(ieee->wq, &ieee->ext_stop_scan_wq);
+#else
+		schedule_task(&ieee->ext_stop_scan_wq);
+#endif
+	}
+	if(ieee->current_network.capability & WLAN_CAPABILITY_IBSS) // use current_network here
+		atim_len = 4;
+	else
+		atim_len = 0;
+
+	if(ieee80211_is_54g(*net))
+		erp_len = 3;
+	else
+		erp_len = 0;
+
+	beacon_size = sizeof(struct ieee80211_probe_response)+
+		ssid_len
+		+3 //channel
+		+rate_len
+		+rate_ex_len
+		+atim_len
+		+erp_len;
+//b
+	skb = dev_alloc_skb(beacon_size+196);
+
+	if (!skb)
+ 		return NULL;
+
+	beacon_buf = (struct ieee80211_probe_response*) skb_put(skb, beacon_size);
+
+	memcpy (beacon_buf->header.addr1, dest,ETH_ALEN);
+	memcpy (beacon_buf->header.addr2, ieee->dev->dev_addr, ETH_ALEN);
+	memcpy (beacon_buf->header.addr3, ieee->current_network.bssid, ETH_ALEN);
+
+	beacon_buf->header.duration_id = 0; //FIXME
+
+	beacon_buf->beacon_interval =
+		cpu_to_le16(ieee->current_network.beacon_interval);  // use current_network here
+	beacon_buf->capability =
+		cpu_to_le16(ieee->current_network.capability & WLAN_CAPABILITY_IBSS);
+
+	if(ieee->short_slot && (ieee->current_network.capability & WLAN_CAPABILITY_SHORT_SLOT))
+		cpu_to_le16((beacon_buf->capability |= WLAN_CAPABILITY_SHORT_SLOT));
+
+	crypt = ieee->crypt[ieee->tx_keyidx];
+
+	encrypt = ieee->host_encrypt && crypt && crypt->ops &&
+		((0 == strcmp(crypt->ops->name, "WEP"))||wpa_ie_len);
+
+	if (encrypt)
+		beacon_buf->capability |= cpu_to_le16(WLAN_CAPABILITY_PRIVACY);
+
+
+	beacon_buf->header.frame_ctl = cpu_to_le16(IEEE80211_STYPE_PROBE_RESP);
+
+	beacon_buf->info_element.id = MFIE_TYPE_SSID;
+	beacon_buf->info_element.len = ssid_len;
+
+	tag = (u8*) beacon_buf->info_element.data;
+
+	// brocad cast / probe rsp
+	if(memcmp(dest, broadcast_addr, ETH_ALEN ))
+		memcpy(tag, ssid, ssid_len);
+	else
+		ssid_len=0;
+
+	tag += ssid_len;
+
+//get_bssrate_set(priv, _SUPPORTEDRATES_IE_, &pbssrate, &bssrate_len);
+//pbuf = set_ie(pbuf, _SUPPORTEDRATES_IE_, bssrate_len, pbssrate, &frlen);
+
+	*(tag++) = MFIE_TYPE_RATES;
+	*(tag++) = rate_len-2;
+	memcpy(tag,ieee->current_network.rates,rate_len-2);
+	tag+=rate_len-2;
+
+	*(tag++) = MFIE_TYPE_DS_SET;
+	*(tag++) = 1;
+	*(tag++) = ieee->current_network.channel;  // use current_network here
+
+
+	if(atim_len){
+		*(tag++) = MFIE_TYPE_IBSS_SET;
+		*(tag++) = 2;
+		*((u16*)(tag)) = cpu_to_le16(ieee->current_network.atim_window); // use current_network here
+		tag+=2;
+	}
+
+	if(erp_len){
+		*(tag++) = MFIE_TYPE_ERP;
+		*(tag++) = 1;
+		*(tag++) = 0;
+	}
+
+	if(rate_ex_len){
+		*(tag++) = MFIE_TYPE_RATES_EX;
+		*(tag++) = rate_ex_len-2;
+		memcpy(tag,ieee->current_network.rates_ex,rate_ex_len-2);
+		tag+=rate_ex_len-2;
+	}
+	if (wpa_ie_len)
+		memcpy(tag, ieee->wpa_ie, ieee->wpa_ie_len);
+
+	skb->dev = ieee->dev;
+	return skb;
+}
+#endif // _RTL8187_EXT_PATCH_
+
+struct sk_buff* ieee80211_assoc_resp(struct ieee80211_device *ieee, u8 *dest)
+{
+	struct sk_buff *skb;
+	u8* tag;
+
+	struct ieee80211_crypt_data* crypt;
+	struct ieee80211_assoc_response_frame *assoc;
+	short encrypt;
+
+	unsigned int rate_len = ieee80211_MFIE_rate_len(ieee);
+	int len = sizeof(struct ieee80211_assoc_response_frame) + rate_len;
+
+	skb = dev_alloc_skb(len);
+
+	if (!skb)
+		return NULL;
+
+	assoc = (struct ieee80211_assoc_response_frame *)
+		skb_put(skb,sizeof(struct ieee80211_assoc_response_frame));
+
+	assoc->header.frame_ctl = cpu_to_le16(IEEE80211_STYPE_ASSOC_RESP);
+	memcpy(assoc->header.addr1, dest,ETH_ALEN);
+	memcpy(assoc->header.addr3, ieee->dev->dev_addr, ETH_ALEN);
+	memcpy(assoc->header.addr2, ieee->dev->dev_addr, ETH_ALEN);
+	assoc->capability = cpu_to_le16(ieee->iw_mode == IW_MODE_MASTER ?
+		WLAN_CAPABILITY_BSS : WLAN_CAPABILITY_IBSS);
+
+
+	if(ieee->short_slot)
+		assoc->capability |= cpu_to_le16(WLAN_CAPABILITY_SHORT_SLOT);
+
+	if (ieee->host_encrypt)
+		crypt = ieee->crypt[ieee->tx_keyidx];
+	else crypt = NULL;
+
+	encrypt = ( crypt && crypt->ops);
+
+	if (encrypt)
+		assoc->capability |= cpu_to_le16(WLAN_CAPABILITY_PRIVACY);
+
+	assoc->status = 0;
+	assoc->aid = cpu_to_le16(ieee->assoc_id);
+	if (ieee->assoc_id == 0x2007) ieee->assoc_id=0;
+	else ieee->assoc_id++;
+
+	tag = (u8*) skb_put(skb, rate_len);
+
+	ieee80211_MFIE_Brate(ieee, &tag);
+	ieee80211_MFIE_Grate(ieee, &tag);
+
+	return skb;
+}
+
+struct sk_buff* ieee80211_auth_resp(struct ieee80211_device *ieee,int status, u8 *dest)
+{
+	struct sk_buff *skb;
+	struct ieee80211_authentication *auth;
+
+	skb = dev_alloc_skb(sizeof(struct ieee80211_authentication)+1);
+
+	if (!skb)
+		return NULL;
+
+	skb->len = sizeof(struct ieee80211_authentication);
+
+	auth = (struct ieee80211_authentication *)skb->data;
+
+	auth->status = cpu_to_le16(status);
+	auth->transaction = cpu_to_le16(2);
+	auth->algorithm = cpu_to_le16(WLAN_AUTH_OPEN);
+
+#ifdef _RTL8187_EXT_PATCH_
+	if(ieee->iw_mode == ieee->iw_ext_mode)
+		memcpy(auth->header.addr3, dest, ETH_ALEN);
+#else
+	memcpy(auth->header.addr3, ieee->dev->dev_addr, ETH_ALEN);
+#endif
+	memcpy(auth->header.addr2, ieee->dev->dev_addr, ETH_ALEN);
+	memcpy(auth->header.addr1, dest, ETH_ALEN);
+	auth->header.frame_ctl = cpu_to_le16(IEEE80211_STYPE_AUTH);
+	return skb;
+
+
+}
+
+struct sk_buff* ieee80211_null_func(struct ieee80211_device *ieee,short pwr)
+{
+	struct sk_buff *skb;
+	struct ieee80211_hdr_3addr* hdr;
+
+	skb = dev_alloc_skb(sizeof(struct ieee80211_hdr_3addr));
+
+	if (!skb)
+		return NULL;
+
+	hdr = (struct ieee80211_hdr_3addr*)skb_put(skb,sizeof(struct ieee80211_hdr_3addr));
+
+	memcpy(hdr->addr1, ieee->current_network.bssid, ETH_ALEN);
+	memcpy(hdr->addr2, ieee->dev->dev_addr, ETH_ALEN);
+	memcpy(hdr->addr3, ieee->current_network.bssid, ETH_ALEN);
+
+	hdr->frame_ctl = cpu_to_le16(IEEE80211_FTYPE_DATA |
+		IEEE80211_STYPE_NULLFUNC | IEEE80211_FCTL_TODS |
+		(pwr ? IEEE80211_FCTL_PM:0));
+
+	return skb;
+
+
+}
+
+
+void ieee80211_resp_to_assoc_rq(struct ieee80211_device *ieee, u8* dest)
+{
+	struct sk_buff *buf = ieee80211_assoc_resp(ieee, dest);
+
+	if (buf){
+		softmac_mgmt_xmit(buf, ieee);
+		dev_kfree_skb_any(buf);//edit by thomas
+	}
+}
+
+
+void ieee80211_resp_to_auth(struct ieee80211_device *ieee, int s, u8* dest)
+{
+	struct sk_buff *buf = ieee80211_auth_resp(ieee, s, dest);
+
+	if (buf){
+		softmac_mgmt_xmit(buf, ieee);
+		dev_kfree_skb_any(buf);//edit by thomas
+	}
+}
+
+
+void ieee80211_resp_to_probe(struct ieee80211_device *ieee, u8 *dest)
+{
+
+	struct sk_buff *buf = ieee80211_probe_resp(ieee, dest);
+
+	if (buf) {
+		softmac_mgmt_xmit(buf, ieee);
+		dev_kfree_skb_any(buf);//edit by thomas
+	}
+}
+
+
+inline struct sk_buff *ieee80211_association_req(struct ieee80211_network *beacon,struct ieee80211_device *ieee)
+{
+	struct sk_buff *skb;
+	//unsigned long flags;
+
+	struct ieee80211_assoc_request_frame *hdr;
+	u8 *tag;
+	//short info_addr = 0;
+	//int i;
+	//u16 suite_count = 0;
+	//u8 suit_select = 0;
+	unsigned int wpa_len = beacon->wpa_ie_len;
+	//struct net_device *dev = ieee->dev;
+	//union iwreq_data wrqu;
+	//u8 *buff;
+	//u8 *p;
+#if 1
+	// for testing purpose
+	unsigned int rsn_len = beacon->rsn_ie_len;
+#else
+	unsigned int rsn_len = beacon->rsn_ie_len - 4;
+#endif
+	unsigned int rate_len = ieee80211_MFIE_rate_len(ieee);
+	unsigned int wmm_info_len = beacon->QoS_Enable?9:0;
+#ifdef THOMAS_TURBO
+	unsigned int turbo_info_len = beacon->Turbo_Enable?9:0;
+#endif
+
+	u8  encry_proto = ieee->wpax_type_notify & 0xff;
+	//u8  pairwise_type = (ieee->wpax_type_notify >> 8) & 0xff;
+	//u8  authen_type = (ieee->wpax_type_notify >> 16) & 0xff;
+
+	int len = 0;
+
+	//[0] Notify type of encryption: WPA/WPA2
+	//[1] pair wise type
+	//[2] authen type
+	if(ieee->wpax_type_set) {
+		if (IEEE_PROTO_WPA == encry_proto) {
+			rsn_len = 0;
+		} else if (IEEE_PROTO_RSN == encry_proto) {
+			wpa_len = 0;
+		}
+	}
+#ifdef THOMAS_TURBO
+	len = sizeof(struct ieee80211_assoc_request_frame)+
+		+ beacon->ssid_len//essid tagged val
+		+ rate_len//rates tagged val
+		+ wpa_len
+		+ rsn_len
+		+ wmm_info_len
+		+ turbo_info_len;
+#else
+	len = sizeof(struct ieee80211_assoc_request_frame)+
+		+ beacon->ssid_len//essid tagged val
+		+ rate_len//rates tagged val
+		+ wpa_len
+		+ rsn_len
+		+ wmm_info_len;
+#endif
+
+#ifdef _RTL8187_EXT_PATCH_
+	if(ieee->iw_mode == ieee->iw_ext_mode)
+		skb = dev_alloc_skb(len+256); // stanley
+	else
+#endif
+	skb = dev_alloc_skb(len);
+
+	if (!skb)
+		return NULL;
+
+	hdr = (struct ieee80211_assoc_request_frame *)
+		skb_put(skb, sizeof(struct ieee80211_assoc_request_frame));
+
+
+	hdr->header.frame_ctl = IEEE80211_STYPE_ASSOC_REQ;
+	hdr->header.duration_id= 37; //FIXME
+	memcpy(hdr->header.addr1, beacon->bssid, ETH_ALEN);
+	memcpy(hdr->header.addr2, ieee->dev->dev_addr, ETH_ALEN);
+	memcpy(hdr->header.addr3, beacon->bssid, ETH_ALEN);
+	memcpy(ieee->ap_mac_addr, beacon->bssid, ETH_ALEN);//for HW security, John
+
+	hdr->capability = cpu_to_le16(WLAN_CAPABILITY_BSS);
+	if (beacon->capability & WLAN_CAPABILITY_PRIVACY )
+		hdr->capability |= cpu_to_le16(WLAN_CAPABILITY_PRIVACY);
+	if (beacon->capability & WLAN_CAPABILITY_SHORT_PREAMBLE)
+		hdr->capability |= cpu_to_le16(WLAN_CAPABILITY_SHORT_PREAMBLE);
+
+	if(ieee->short_slot)
+		hdr->capability |= cpu_to_le16(WLAN_CAPABILITY_SHORT_SLOT);
+
+#ifdef _RTL8187_EXT_PATCH_
+	if((ieee->iw_mode == ieee->iw_ext_mode) && ieee->ext_patch_ieee80211_association_req_1)
+		ieee->ext_patch_ieee80211_association_req_1(hdr);
+#endif
+
+	hdr->listen_interval = 0xa; //FIXME
+
+	hdr->info_element.id = MFIE_TYPE_SSID;
+
+	hdr->info_element.len = beacon->ssid_len;
+	tag = skb_put(skb, beacon->ssid_len);
+	memcpy(tag, beacon->ssid, beacon->ssid_len);
+
+	tag = skb_put(skb, rate_len);
+
+	ieee80211_MFIE_Brate(ieee, &tag);
+	ieee80211_MFIE_Grate(ieee, &tag);
+
+	//add rsn==0 condition for ap's mix security mode(wpa+wpa2), john2007.8.9
+	//choose AES encryption as default algorithm while using mixed mode
+#if 0
+	if(rsn_len == 0){
+
+		tag = skb_put(skb,wpa_len);
+
+		if(wpa_len) {
+
+
+		  //{add by david. 2006.8.31
+		  //fix linksys compatibility bug
+		  //}
+		  if(wpa_len > 24) {//22+2, mean include the capability
+			beacon->wpa_ie[wpa_len - 2] = 0;
+		  }
+		//multicast cipher OUI
+                  if(  beacon->wpa_ie[11]==0x2      ){ //0x0050f202 is the oui of tkip
+                  ieee->broadcast_key_type = KEY_TYPE_TKIP;
+                }
+                  else if(  beacon->wpa_ie[11]==0x4      ){//0x0050f204 is the oui of ccmp
+                  ieee->broadcast_key_type = KEY_TYPE_CCMP;
+                }
+ 		//unicast cipher OUI
+		  if(	beacon->wpa_ie[14]==0
+			&& beacon->wpa_ie[15]==0x50
+                        && beacon->wpa_ie[16]==0xf2
+                        && beacon->wpa_ie[17]==0x2  	){ //0x0050f202 is the oui of tkip
+                  ieee->pairwise_key_type = KEY_TYPE_TKIP;
+		}
+
+                  else if(   beacon->wpa_ie[14]==0
+                        && beacon->wpa_ie[15]==0x50
+                        && beacon->wpa_ie[16]==0xf2
+                        && beacon->wpa_ie[17]==0x4      ){//0x0050f204 is the oui of ccmp
+                  ieee->pairwise_key_type = KEY_TYPE_CCMP;
+		}
+		//indicate the wpa_ie content to WPA_SUPPLICANT
+		buff = kmalloc(IW_CUSTOM_MAX, GFP_ATOMIC);
+		memset(buff, 0, IW_CUSTOM_MAX);
+		p=buff;
+		p += sprintf(p, "ASSOCINFO(ReqIEs=");
+		for(i=0;i<wpa_len;i++){
+			p += sprintf(p, "%02x", beacon->wpa_ie[i]);
+		}
+		p += sprintf(p, ")");
+		memset(&wrqu, 0, sizeof(wrqu) );
+		wrqu.data.length = p - buff;
+
+		wireless_send_event(dev, IWEVCUSTOM, &wrqu, buff);
+		  memcpy(tag,beacon->wpa_ie,wpa_len);
+		}
+
+	}
+
+	if(rsn_len > 22) {
+
+	  					if(     beacon->rsn_ie[4]==0x0 &&
+                                beacon->rsn_ie[5]==0xf &&
+                                beacon->rsn_ie[6]==0xac){
+
+                                switch(beacon->rsn_ie[7]){
+                                        case 0x1:
+                                                ieee->broadcast_key_type = KEY_TYPE_WEP40;
+                                                break;
+                                        case 0x2:
+                                                ieee->broadcast_key_type = KEY_TYPE_TKIP;
+                                                break;
+                                        case 0x4:
+                                                ieee->broadcast_key_type = KEY_TYPE_CCMP;
+                                                break;
+                                        case 0x5:
+                                                ieee->broadcast_key_type = KEY_TYPE_WEP104;
+                                                break;
+                                        default:
+                                                printk("fault suite type in RSN broadcast key\n");
+                                                break;
+                                }
+                        }
+
+                        if(     beacon->rsn_ie[10]==0x0 &&
+                                beacon->rsn_ie[11]==0xf &&
+                                beacon->rsn_ie[12]==0xac){
+				if(beacon->rsn_ie[8]==1){//not mixed mode
+	                                switch(beacon->rsn_ie[13]){
+        	                                case 0x2:
+                	                                ieee->pairwise_key_type = KEY_TYPE_TKIP;
+                        	                        break;
+                                	        case 0x4:
+                                        	        ieee->pairwise_key_type = KEY_TYPE_CCMP;
+                                                	break;
+        	                                default:
+	                                                printk("fault suite type in RSN pairwise key\n");
+                	                                break;
+                                	}
+				}
+				else if(beacon->rsn_ie[8]==2){//mixed mode
+					ieee->pairwise_key_type = KEY_TYPE_CCMP;
+				}
+                        }
+
+
+
+		tag = skb_put(skb,22);
+		memcpy(tag,(beacon->rsn_ie + info_addr),8);
+		tag[1] =  20;
+		tag += 8;
+		info_addr += 8;
+
+		spin_lock_irqsave(&ieee->wpax_suitlist_lock,flags);
+		for (i = 0; i < 2; i++) {
+			tag[0] = 1;
+			tag[1] = 0;
+			tag += 2;
+			suite_count = beacon->rsn_ie[info_addr] + \
+				      (beacon->rsn_ie[info_addr + 1] << 8);
+			info_addr += 2;
+			if(1 == suite_count) {
+				memcpy(tag,(beacon->rsn_ie + info_addr),4);
+				info_addr += 4;
+			} else {
+				// if the wpax_type_notify has been set by the application,
+				// just use it, otherwise just use the default one.
+				if(ieee->wpax_type_set) {
+					suit_select = ((0 == i) ? pairwise_type:authen_type)&0x0f ;
+					memcpy(tag,rsn_authen_cipher_suite[suit_select],4);
+				} else {
+					//default set as ccmp, or none authentication
+					if(i == 0) {
+						memcpy(tag,rsn_authen_cipher_suite[4],4);
+					} else {
+						memcpy(tag,rsn_authen_cipher_suite[2],4);
+					}
+
+				}
+
+				info_addr += (suite_count * 4);
+			}
+			tag += 4;
+		}
+		spin_unlock_irqrestore(&ieee->wpax_suitlist_lock,flags);
+
+		tag[0] = 0;
+		tag[1] = beacon->rsn_ie[info_addr+1];
+
+	} else {
+		tag = skb_put(skb,rsn_len);
+		if(rsn_len) {
+
+
+			if( 	beacon->rsn_ie[4]==0x0 &&
+				beacon->rsn_ie[5]==0xf &&
+				beacon->rsn_ie[6]==0xac){
+				switch(beacon->rsn_ie[7]){
+					case 0x1:
+						ieee->broadcast_key_type = KEY_TYPE_WEP40;
+                                                break;
+					case 0x2:
+						ieee->broadcast_key_type = KEY_TYPE_TKIP;
+						break;
+					case 0x4:
+   	                                        ieee->broadcast_key_type = KEY_TYPE_CCMP;
+                                                break;
+                                        case 0x5:
+                                                ieee->broadcast_key_type = KEY_TYPE_WEP104;
+                                                break;
+					default:
+						printk("fault suite type in RSN broadcast key\n");
+						break;
+				}
+			}
+                        if(     beacon->rsn_ie[10]==0x0 &&
+                                beacon->rsn_ie[11]==0xf &&
+                                beacon->rsn_ie[12]==0xac){
+                                if(beacon->rsn_ie[8]==1){//not mixed mode
+                                        switch(beacon->rsn_ie[13]){
+                                                case 0x2:
+                                                        ieee->pairwise_key_type = KEY_TYPE_TKIP;
+                                                        break;
+                                                case 0x4:
+                                                        ieee->pairwise_key_type = KEY_TYPE_CCMP;
+                                                        break;
+                                                default:
+                                                        printk("fault suite type in RSN pairwise key\n");
+                                                        break;
+                                	}
+
+				}
+                                else if(beacon->rsn_ie[8]==2){//mixed mode
+                                        ieee->pairwise_key_type = KEY_TYPE_CCMP;
+                                }
+                        }
+
+
+			beacon->rsn_ie[rsn_len - 2] = 0;
+			memcpy(tag,beacon->rsn_ie,rsn_len);
+		}
+	}
+#else
+	tag = skb_put(skb,ieee->wpa_ie_len);
+	memcpy(tag,ieee->wpa_ie,ieee->wpa_ie_len);
+#endif
+	tag = skb_put(skb,wmm_info_len);
+	if(wmm_info_len) {
+	  ieee80211_WMM_Info(ieee, &tag);
+	}
+#ifdef THOMAS_TURBO
+	tag = skb_put(skb,turbo_info_len);
+        if(turbo_info_len) {
+                ieee80211_TURBO_Info(ieee, &tag);
+        }
+#endif
+
+#ifdef _RTL8187_EXT_PATCH_
+	if((ieee->iw_mode == ieee->iw_ext_mode) && ieee->ext_patch_ieee80211_association_req_2)
+		ieee->ext_patch_ieee80211_association_req_2(ieee, beacon, skb);
+#endif
+
+	return skb;
+}
+
+void ieee80211_associate_abort(struct ieee80211_device *ieee)
+{
+
+	unsigned long flags;
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	ieee->associate_seq++;
+
+	/* don't scan, and avoid to have the RX path possibily
+	 * try again to associate. Even do not react to AUTH or
+	 * ASSOC response. Just wait for the retry wq to be scheduled.
+	 * Here we will check if there are good nets to associate
+	 * with, so we retry or just get back to NO_LINK and scanning
+	 */
+	if (ieee->state == IEEE80211_ASSOCIATING_AUTHENTICATING){
+		IEEE80211_DEBUG_MGMT("Authentication failed\n");
+		ieee->softmac_stats.no_auth_rs++;
+	}else{
+		IEEE80211_DEBUG_MGMT("Association failed\n");
+		ieee->softmac_stats.no_ass_rs++;
+	}
+
+	ieee->state = IEEE80211_ASSOCIATING_RETRY;
+
+	queue_delayed_work(ieee->wq, &ieee->associate_retry_wq,IEEE80211_SOFTMAC_ASSOC_RETRY_TIME);
+
+	spin_unlock_irqrestore(&ieee->lock, flags);
+}
+
+void ieee80211_associate_abort_cb(unsigned long dev)
+{
+	ieee80211_associate_abort((struct ieee80211_device *) dev);
+}
+
+
+void ieee80211_associate_step1(struct ieee80211_device *ieee)
+{
+	struct ieee80211_network *beacon = &ieee->current_network;
+	struct sk_buff *skb;
+
+	IEEE80211_DEBUG_MGMT("Stopping scan\n");
+	ieee->softmac_stats.tx_auth_rq++;
+	skb=ieee80211_authentication_req(beacon, ieee, 0);
+#ifdef _RTL8187_EXT_PATCH_
+	if(ieee->iw_mode == ieee->iw_ext_mode )	{
+		if(skb)
+			softmac_mgmt_xmit(skb, ieee);
+		return;
+	}else
+#endif
+	if (!skb){
+
+		ieee80211_associate_abort(ieee);
+	}
+	else{
+		ieee->state = IEEE80211_ASSOCIATING_AUTHENTICATING ;
+		IEEE80211_DEBUG_MGMT("Sending authentication request\n");
+		//printk("---Sending authentication request\n");
+		softmac_mgmt_xmit(skb, ieee);
+		//BUGON when you try to add_timer twice, using mod_timer may be better, john0709
+		if(!timer_pending(&ieee->associate_timer)){
+			ieee->associate_timer.expires = jiffies + (HZ / 2);
+			add_timer(&ieee->associate_timer);
+		}
+		//If call dev_kfree_skb_any,a warning will ocur....
+		//KERNEL: assertion (!atomic_read(&skb->users)) failed at net/core/dev.c (1708)
+		//So ... 1204 by lawrence.
+		//printk("\nIn %s,line %d call kfree skb.",__FUNCTION__,__LINE__);
+		//dev_kfree_skb_any(skb);//edit by thomas
+	}
+}
+
+void ieee80211_auth_challenge(struct ieee80211_device *ieee, u8 *challenge, int chlen)
+{
+	u8 *c;
+	struct sk_buff *skb;
+	struct ieee80211_network *beacon = &ieee->current_network;
+//	int hlen = sizeof(struct ieee80211_authentication);
+	del_timer_sync(&ieee->associate_timer);
+	ieee->associate_seq++;
+	ieee->softmac_stats.tx_auth_rq++;
+
+	skb = ieee80211_authentication_req(beacon, ieee, chlen+2);
+	if (!skb)
+		ieee80211_associate_abort(ieee);
+	else{
+		c = skb_put(skb, chlen+2);
+		*(c++) = MFIE_TYPE_CHALLENGE;
+		*(c++) = chlen;
+		memcpy(c, challenge, chlen);
+
+		IEEE80211_DEBUG_MGMT("Sending authentication challenge response\n");
+
+		ieee80211_encrypt_fragment(ieee, skb, sizeof(struct ieee80211_hdr_3addr  ));
+
+		softmac_mgmt_xmit(skb, ieee);
+		if (!timer_pending(&ieee->associate_timer)){
+		//printk("=========>add timer again, to crash\n");
+		ieee->associate_timer.expires = jiffies + (HZ / 2);
+		add_timer(&ieee->associate_timer);
+		}
+		dev_kfree_skb_any(skb);//edit by thomas
+	}
+	kfree(challenge);
+}
+
+#ifdef _RTL8187_EXT_PATCH_
+
+// based on ieee80211_assoc_resp
+struct sk_buff* ieee80211_assoc_resp_by_net(struct ieee80211_device *ieee, u8 *dest, unsigned short status, struct ieee80211_network *pstat, int pkt_type)
+{
+	struct sk_buff *skb;
+	u8* tag;
+
+	struct ieee80211_crypt_data* crypt;
+	struct ieee80211_assoc_response_frame *assoc;
+	short encrypt;
+
+	unsigned int rate_len = ieee80211_MFIE_rate_len(ieee);
+	int len = sizeof(struct ieee80211_assoc_response_frame) + rate_len;
+
+	if(ieee->iw_mode == ieee->iw_ext_mode)
+		skb = dev_alloc_skb(len+256); // stanley
+	else
+		skb = dev_alloc_skb(len);
+
+	if (!skb)
+		return NULL;
+
+	assoc = (struct ieee80211_assoc_response_frame *)
+		skb_put(skb,sizeof(struct ieee80211_assoc_response_frame));
+
+	assoc->header.frame_ctl = cpu_to_le16(pkt_type);
+
+	memcpy(assoc->header.addr1, dest,ETH_ALEN);
+	memcpy(assoc->header.addr3, ieee->dev->dev_addr, ETH_ALEN);
+	memcpy(assoc->header.addr2, ieee->dev->dev_addr, ETH_ALEN);
+	assoc->capability = cpu_to_le16(ieee->iw_mode == IW_MODE_MASTER ?
+		WLAN_CAPABILITY_BSS : WLAN_CAPABILITY_IBSS);
+
+	if((ieee->iw_mode == ieee->iw_ext_mode) && ieee->ext_patch_ieee80211_assoc_resp_by_net_1)
+		ieee->ext_patch_ieee80211_assoc_resp_by_net_1(assoc);
+
+	if(ieee->short_slot)
+		assoc->capability |= cpu_to_le16(WLAN_CAPABILITY_SHORT_SLOT);
+
+	if (ieee->host_encrypt)
+		crypt = ieee->crypt[ieee->tx_keyidx];
+	else crypt = NULL;
+
+	encrypt = ( crypt && crypt->ops);
+
+	if (encrypt)
+		assoc->capability |= cpu_to_le16(WLAN_CAPABILITY_PRIVACY);
+
+	assoc->status = 0;
+	assoc->aid = cpu_to_le16(ieee->assoc_id);
+	if (ieee->assoc_id == 0x2007) ieee->assoc_id=0;
+	else ieee->assoc_id++;
+
+	assoc->info_element.id = 230; // Stanley, an unused id (just a hot fix)
+	assoc->info_element.len = 0;
+
+	tag = (u8*) skb_put(skb, rate_len);
+
+	ieee80211_MFIE_Brate(ieee, &tag);
+	ieee80211_MFIE_Grate(ieee, &tag);
+
+	if((ieee->iw_mode == ieee->iw_ext_mode) && ieee->ext_patch_ieee80211_assoc_resp_by_net_2)
+		ieee->ext_patch_ieee80211_assoc_resp_by_net_2(ieee, pstat, pkt_type, skb);
+
+	return skb;
+}
+
+// based on ieee80211_resp_to_assoc_rq
+void ieee80211_ext_issue_assoc_rsp(struct ieee80211_device *ieee, u8 *dest, unsigned short status, struct ieee80211_network *pstat, int pkt_type)
+{
+	struct sk_buff *buf = ieee80211_assoc_resp_by_net(ieee, dest, status, pstat, pkt_type);
+
+	if (buf)
+		softmac_mgmt_xmit(buf, ieee);
+}
+
+// based on ieee80211_associate_step2
+void ieee80211_ext_issue_assoc_req(struct ieee80211_device *ieee, struct ieee80211_network *pstat)
+{
+
+	struct sk_buff* skb;
+
+	// printk("@@@@@ ieee80211_ext_issue_assoc_req on channel: %d\n", ieee->current_network.channel);
+
+	ieee->softmac_stats.tx_ass_rq++;
+	skb=ieee80211_association_req(pstat, ieee);
+	if (skb)
+		softmac_mgmt_xmit(skb, ieee);
+}
+
+void ieee80211_ext_issue_disassoc(struct ieee80211_device *ieee, struct ieee80211_network *pstat, int reason, unsigned char extReason)
+{
+	// do nothing
+	// printk("@@@@@ ieee80211_ext_issue_disassoc\n");
+	return;
+}
+#endif // _RTL8187_EXT_PATCH_
+
+void ieee80211_associate_step2(struct ieee80211_device *ieee)
+{
+	struct sk_buff* skb;
+	struct ieee80211_network *beacon = &ieee->current_network;
+
+	del_timer_sync(&ieee->associate_timer);
+
+	IEEE80211_DEBUG_MGMT("Sending association request\n");
+	ieee->softmac_stats.tx_ass_rq++;
+	skb=ieee80211_association_req(beacon, ieee);
+	if (!skb)
+		ieee80211_associate_abort(ieee);
+	else{
+		softmac_mgmt_xmit(skb, ieee);
+		if (!timer_pending(&ieee->associate_timer)){
+		ieee->associate_timer.expires = jiffies + (HZ / 2);
+		add_timer(&ieee->associate_timer);
+		}
+		//dev_kfree_skb_any(skb);//edit by thomas
+	}
+}
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+void ieee80211_associate_complete_wq(struct work_struct *work)
+{
+	struct ieee80211_device *ieee = container_of(work, struct ieee80211_device, associate_complete_wq);
+#else
+void ieee80211_associate_complete_wq(struct ieee80211_device *ieee)
+{
+#endif
+	printk(KERN_INFO "Associated successfully\n");
+	if(ieee80211_is_54g(ieee->current_network) &&
+		(ieee->modulation & IEEE80211_OFDM_MODULATION)){
+
+		ieee->rate = 540;
+		printk(KERN_INFO"Using G rates\n");
+	}else{
+		ieee->rate = 110;
+		printk(KERN_INFO"Using B rates\n");
+	}
+	ieee->link_change(ieee->dev);
+	notify_wx_assoc_event(ieee);
+	if (ieee->data_hard_resume)
+		ieee->data_hard_resume(ieee->dev);
+	netif_carrier_on(ieee->dev);
+}
+
+void ieee80211_associate_complete(struct ieee80211_device *ieee)
+{
+	int i;
+	del_timer_sync(&ieee->associate_timer);
+
+	for(i = 0; i < 6; i++) {
+	  //ieee->seq_ctrl[i] = 0;
+	}
+	ieee->state = IEEE80211_LINKED;
+	IEEE80211_DEBUG_MGMT("Successfully associated\n");
+
+	queue_work(ieee->wq, &ieee->associate_complete_wq);
+}
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+void ieee80211_associate_procedure_wq(struct work_struct *work)
+{
+	struct ieee80211_device *ieee = container_of(work, struct ieee80211_device, associate_procedure_wq);
+#else
+void ieee80211_associate_procedure_wq(struct ieee80211_device *ieee)
+{
+#endif
+	ieee->sync_scan_hurryup = 1;
+	down(&ieee->wx_sem);
+
+	if (ieee->data_hard_stop)
+		ieee->data_hard_stop(ieee->dev);
+
+	ieee80211_stop_scan(ieee);
+	ieee->set_chan(ieee->dev, ieee->current_network.channel);
+
+	ieee->associate_seq = 1;
+	ieee80211_associate_step1(ieee);
+
+	up(&ieee->wx_sem);
+}
+#ifdef _RTL8187_EXT_PATCH_
+// based on ieee80211_associate_procedure_wq
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20)
+void ieee80211_ext_stop_scan_wq(struct work_struct *work)
+{
+	struct ieee80211_device *ieee = container_of(work, struct ieee80211_device, ext_stop_scan_wq);
+#else
+void ieee80211_ext_stop_scan_wq(struct ieee80211_device *ieee)
+{
+#endif
+	  if (ieee->scanning == 0)
+	{
+		if((ieee->iw_mode == ieee->iw_ext_mode) && ieee->ext_patch_ieee80211_ext_stop_scan_wq_set_channel
+				&& ( ieee->current_network.channel == ieee->ext_patch_ieee80211_ext_stop_scan_wq_set_channel(ieee) ) )
+		return;
+	}
+
+	ieee->sync_scan_hurryup = 1;
+
+	down(&ieee->wx_sem);
+
+	// printk("@@@@@@@@@@ ieee80211_ext_stop_scan_wq\n");
+	if (ieee->data_hard_stop)
+		ieee->data_hard_stop(ieee->dev);
+
+	ieee80211_stop_scan(ieee);
+
+	// set channel
+	if((ieee->iw_mode == ieee->iw_ext_mode) && ieee->ext_patch_ieee80211_ext_stop_scan_wq_set_channel)
+		ieee->set_chan(ieee->dev, ieee->ext_patch_ieee80211_ext_stop_scan_wq_set_channel(ieee));
+	else
+		ieee->set_chan(ieee->dev, ieee->current_network.channel);
+	//
+	up(&ieee->wx_sem);
+}
+
+
+void ieee80211_ext_send_11s_beacon(struct ieee80211_device *ieee)
+{
+	#if LINUX_VERSION_CODE > KERNEL_VERSION(2,5,0)
+		queue_work(ieee->wq, &ieee->ext_send_beacon_wq);
+	#else
+		schedule_task(&ieee->ext_send_beacon_wq);
+	#endif
+
+}
+
+#endif // _RTL8187_EXT_PATCH_
+
+inline void ieee80211_softmac_new_net(struct ieee80211_device *ieee, struct ieee80211_network *net)
+{
+	u8 tmp_ssid[IW_ESSID_MAX_SIZE+1];
+	int tmp_ssid_len = 0;
+
+	short apset,ssidset,ssidbroad,apmatch,ssidmatch;
+
+	/* we are interested in new new only if we are not associated
+	 * and we are not associating / authenticating
+	 */
+	if (ieee->state != IEEE80211_NOLINK)
+		return;
+
+	if ((ieee->iw_mode == IW_MODE_INFRA) && !(net->capability & WLAN_CAPABILITY_BSS))
+		return;
+
+	if ((ieee->iw_mode == IW_MODE_ADHOC) && !(net->capability & WLAN_CAPABILITY_IBSS))
+		return;
+
+
+	if (ieee->iw_mode == IW_MODE_INFRA || ieee->iw_mode == IW_MODE_ADHOC){
+		/* if the user specified the AP MAC, we need also the essid
+		 * This could be obtained by beacons or, if the network does not
+		 * broadcast it, it can be put manually.
+		 */
+		apset = ieee->wap_set;//(memcmp(ieee->current_network.bssid, zero,ETH_ALEN)!=0 );
+		ssidset = ieee->ssid_set;//ieee->current_network.ssid[0] != '\0';
+		ssidbroad =  !(net->ssid_len == 0 || net->ssid[0]== '\0');
+		apmatch = (memcmp(ieee->current_network.bssid, net->bssid, ETH_ALEN)==0);
+
+		if(ieee->current_network.ssid_len != net->ssid_len)
+			ssidmatch = 0;
+		else
+			ssidmatch = (0==strncmp(ieee->current_network.ssid, net->ssid, net->ssid_len));
+
+		//printk("cur: %s, %d, net:%s, %d\n", ieee->current_network.ssid, ieee->current_network.ssid_len, net->ssid, net->ssid_len);
+		//printk("apset=%d apmatch=%d ssidset=%d ssidbroad=%d ssidmatch=%d\n",apset,apmatch,ssidset,ssidbroad,ssidmatch);
+
+		if (	/* if the user set the AP check if match.
+			 * if the network does not broadcast essid we check the user supplyed ANY essid
+			 * if the network does broadcast and the user does not set essid it is OK
+			 * if the network does broadcast and the user did set essid chech if essid match
+			 */
+				( apset && apmatch &&
+				  ((ssidset && ssidbroad && ssidmatch) || (ssidbroad && !ssidset) || (!ssidbroad && ssidset)) ) ||
+				/* if the ap is not set, check that the user set the bssid
+				 * and the network does bradcast and that those two bssid matches
+				 */
+				(!apset && ssidset && ssidbroad && ssidmatch)
+		   ){
+
+
+			/* if the essid is hidden replace it with the
+			 * essid provided by the user.
+			 */
+			if (!ssidbroad){
+				strncpy(tmp_ssid, ieee->current_network.ssid, IW_ESSID_MAX_SIZE);
+				tmp_ssid_len = ieee->current_network.ssid_len;
+			}
+			memcpy(&ieee->current_network, net, sizeof(struct ieee80211_network));
+
+			if (!ssidbroad){
+				strncpy(ieee->current_network.ssid, tmp_ssid, IW_ESSID_MAX_SIZE);
+				ieee->current_network.ssid_len = tmp_ssid_len;
+			}
+			printk(KERN_INFO"Linking with %s: channel is %d\n",ieee->current_network.ssid,ieee->current_network.channel);
+
+			if (ieee->iw_mode == IW_MODE_INFRA){
+				ieee->state = IEEE80211_ASSOCIATING;
+				ieee->beinretry = false;
+				queue_work(ieee->wq, &ieee->associate_procedure_wq);
+			}else{
+				if(ieee80211_is_54g(ieee->current_network) &&
+						(ieee->modulation & IEEE80211_OFDM_MODULATION)){
+					ieee->rate = 540;
+					printk(KERN_INFO"Using G rates\n");
+				}else{
+					ieee->rate = 110;
+					printk(KERN_INFO"Using B rates\n");
+				}
+				ieee->state = IEEE80211_LINKED;
+				ieee->beinretry = false;
+			}
+
+		}
+	}
+
+}
+
+void ieee80211_softmac_check_all_nets(struct ieee80211_device *ieee)
+{
+	unsigned long flags;
+	struct ieee80211_network *target;
+
+	spin_lock_irqsave(&ieee->lock, flags);
+	list_for_each_entry(target, &ieee->network_list, list) {
+
+		/* if the state become different that NOLINK means
+		 * we had found what we are searching for
+		 */
+
+		if (ieee->state != IEEE80211_NOLINK)
+			break;
+
+		if (ieee->scan_age == 0 || time_after(target->last_scanned + ieee->scan_age, jiffies))
+			ieee80211_softmac_new_net(ieee, target);
+	}
+
+	spin_unlock_irqrestore(&ieee->lock, flags);
+
+}
+
+
+static inline u16 auth_parse(struct sk_buff *skb, u8** challenge, int *chlen)
+{
+	struct ieee80211_authentication *a;
+	u8 *t;
+	if (skb->len <  (sizeof(struct ieee80211_authentication)-sizeof(struct ieee80211_info_element))){
+		IEEE80211_DEBUG_MGMT("invalid len in auth resp: %d\n",skb->len);
+		return 0xcafe;
+	}
+	*challenge = NULL;
+	a = (struct ieee80211_authentication*) skb->data;
+	if(skb->len > (sizeof(struct ieee80211_authentication) +3)){
+		t = skb->data + sizeof(struct ieee80211_authentication);
+
+		if(*(t++) == MFIE_TYPE_CHALLENGE){
+			*chlen = *(t++);
+			*challenge = (u8*)kmalloc(*chlen, GFP_ATOMIC);
+			memcpy(*challenge, t, *chlen);
+		}
+	}
+
+	return cpu_to_le16(a->status);
+
+}
+
+
+int auth_rq_parse(struct sk_buff *skb,u8* dest)
+{
+	struct ieee80211_authentication *a;
+
+	if (skb->len <  (sizeof(struct ieee80211_authentication)-sizeof(struct ieee80211_info_element))){
+		IEEE80211_DEBUG_MGMT("invalid len in auth request: %d\n",skb->len);
+		return -1;
+	}
+	a = (struct ieee80211_authentication*) skb->data;
+
+	memcpy(dest,a->header.addr2, ETH_ALEN);
+
+	if (le16_to_cpu(a->algorithm) != WLAN_AUTH_OPEN)
+		return  WLAN_STATUS_NOT_SUPPORTED_AUTH_ALG;
+
+	return WLAN_STATUS_SUCCESS;
+}
+
+static short probe_rq_parse(struct ieee80211_device *ieee, struct sk_buff *skb, u8 *src)
+{
+	u8 *tag;
+	u8 *skbend;
+	u8 *ssid=NULL;
+	u8 ssidlen = 0;
+
+	struct ieee80211_hdr_3addr   *header =
+		(struct ieee80211_hdr_3addr   *) skb->data;
+
+	if (skb->len < sizeof (struct ieee80211_hdr_3addr  ))
+		return -1; /* corrupted */
+
+	memcpy(src,header->addr2, ETH_ALEN);
+
+	skbend = (u8*)skb->data + skb->len;
+
+	tag = skb->data + sizeof (struct ieee80211_hdr_3addr  );
+
+	while (tag+1 < skbend){
+		if (*tag == 0){
+			ssid = tag+2;
+			ssidlen = *(tag+1);
+			break;
+		}
+		tag++; /* point to the len field */
+		tag = tag + *(tag); /* point to the last data byte of the tag */
+		tag++; /* point to the next tag */
+	}
+
+	//IEEE80211DMESG("Card MAC address is "MACSTR, MAC2STR(src));
+	if (ssidlen == 0) return 1;
+
+	if (!ssid) return 1; /* ssid not found in tagged param */
+	return (!strncmp(ssid, ieee->current_network.ssid, ssidlen));
+
+}
+
+int assoc_rq_parse(struct sk_buff *skb,u8* dest)
+{
+	struct ieee80211_assoc_request_frame *a;
+
+	if (skb->len < (sizeof(struct ieee80211_assoc_request_frame) -
+		sizeof(struct ieee80211_info_element))) {
+
+		IEEE80211_DEBUG_MGMT("invalid len in auth request:%d \n", skb->len);
+		return -1;
+	}
+
+	a = (struct ieee80211_assoc_request_frame*) skb->data;
+
+	memcpy(dest,a->header.addr2,ETH_ALEN);
+
+	return 0;
+}
+
+static inline u16 assoc_parse(struct sk_buff *skb, int *aid)
+{
+	struct ieee80211_assoc_response_frame *a;
+	if (skb->len <  sizeof(struct ieee80211_assoc_response_frame)){
+		IEEE80211_DEBUG_MGMT("invalid len in auth resp: %d\n", skb->len);
+		return 0xcafe;
+	}
+
+	a = (struct ieee80211_assoc_response_frame*) skb->data;
+	*aid = le16_to_cpu(a->aid) & 0x3fff;
+	return le16_to_cpu(a->status);
+}
+
+static inline void
+ieee80211_rx_probe_rq(struct ieee80211_device *ieee, struct sk_buff *skb)
+{
+	u8 dest[ETH_ALEN];
+
+	//IEEE80211DMESG("Rx probe");
+	ieee->softmac_stats.rx_probe_rq++;
+	//DMESG("Dest is "MACSTR, MAC2STR(dest));
+	if (probe_rq_parse(ieee, skb, dest)){
+		//IEEE80211DMESG("Was for me!");
+		ieee->softmac_stats.tx_probe_rs++;
+		ieee80211_resp_to_probe(ieee, dest);
+	}
+}
+
+inline void
+ieee80211_rx_auth_rq(struct ieee80211_device *ieee, struct sk_buff *skb)
+{
+	u8 dest[ETH_ALEN];
+	int status;
+	//IEEE80211DMESG("Rx probe");
+	ieee->softmac_stats.rx_auth_rq++;
+
+	if ((status = auth_rq_parse(skb, dest))!= -1){
+		ieee80211_resp_to_auth(ieee, status, dest);
+	}
+	//DMESG("Dest is "MACSTR, MAC2STR(dest));
+
+}
+
+ inline void
+ieee80211_rx_assoc_rq(struct ieee80211_device *ieee, struct sk_buff *skb)
+{
+
+	u8 dest[ETH_ALEN];
+	//unsigned long flags;
+
+	ieee->softmac_stats.rx_ass_rq++;
+	if (assoc_rq_parse(skb,dest) != -1){
+		ieee80211_resp_to_assoc_rq(ieee, dest);
+	}
+
+	printk(KERN_INFO"New client associated: "MAC_FMT"\n", MAC_ARG(dest));
+	//FIXME
+	#if 0
+	spin_lock_irqsave(&ieee->lock,flags);
+	add_associate(ieee,dest);
+	spin_unlock_irqrestore(&ieee->lock,flags);
+	#endif
+}
+
+
+
+void ieee80211_sta_ps_send_null_frame(struct ieee80211_device *ieee, short pwr)
+{
+
+	struct sk_buff *buf = ieee80211_null_func(ieee, pwr);
+
+	if (buf)
+		softmac_ps_mgmt_xmit(buf, ieee);
+
+}
+
+
+short ieee80211_sta_ps_sleep(struct ieee80211_device *ieee, u32 *time_h, u32 *time_l)
+{
+#if 0
+        int timeout = ieee->ps_timeout;
+#else
+        int timeout = 0;
+#endif
+	u8 dtim;
+	/*if(ieee->ps == IEEE80211_PS_DISABLED ||
+		ieee->iw_mode != IW_MODE_INFRA ||
+		ieee->state != IEEE80211_LINKED)
+
+		return 0;
+	*/
+	dtim = ieee->current_network.dtim_data;
+	//printk("DTIM\n");
+
+	if(!(dtim & IEEE80211_DTIM_VALID))
+		return 0;
+        else
+                timeout = ieee->current_network.beacon_interval;
+
+	//printk("VALID\n");
+	ieee->current_network.dtim_data = IEEE80211_DTIM_INVALID;
+
+	if(dtim & ((IEEE80211_DTIM_UCAST | IEEE80211_DTIM_MBCAST)& ieee->ps))
+		return 2;
+
+	if(!time_after(jiffies, ieee->dev->trans_start + MSECS(timeout)))
+		return 0;
+
+	if(!time_after(jiffies, ieee->last_rx_ps_time + MSECS(timeout)))
+		return 0;
+
+	if((ieee->softmac_features & IEEE_SOFTMAC_SINGLE_QUEUE ) &&
+		(ieee->mgmt_queue_tail != ieee->mgmt_queue_head))
+		return 0;
+#if 0
+	if(time_l){
+		*time_l = ieee->current_network.last_dtim_sta_time[0]
+			+ (ieee->current_network.beacon_interval
+			* ieee->current_network.dtim_period) * 1000;
+	}
+#else
+	if(time_l){
+		*time_l = ieee->current_network.last_dtim_sta_time[0]
+			+ MSECS((ieee->current_network.beacon_interval));
+			//* ieee->current_network.dtim_period));
+			//printk("beacon_interval:%x, dtim_period:%x, totol to Msecs:%x, HZ:%x\n", ieee->current_network.beacon_interval, ieee->current_network.dtim_period, MSECS(((ieee->current_network.beacon_interval * ieee->current_network.dtim_period))), HZ);
+	}
+
+#endif
+	if(time_h){
+		*time_h = ieee->current_network.last_dtim_sta_time[1];
+		if(time_l && *time_l < ieee->current_network.last_dtim_sta_time[0])
+			*time_h += 1;
+	}
+
+	return 1;
+
+
+}
+
+inline void ieee80211_sta_ps(struct ieee80211_device *ieee)
+{
+
+	u32 th,tl;
+	short sleep;
+
+	unsigned long flags,flags2;
+
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	if((ieee->ps == IEEE80211_PS_DISABLED ||
+
+		ieee->iw_mode != IW_MODE_INFRA ||
+		ieee->state != IEEE80211_LINKED)){
+
+		//#warning CHECK_LOCK_HERE
+		spin_lock_irqsave(&ieee->mgmt_tx_lock, flags2);
+
+		ieee80211_sta_wakeup(ieee, 1);
+
+		spin_unlock_irqrestore(&ieee->mgmt_tx_lock, flags2);
+	}
+
+	sleep = ieee80211_sta_ps_sleep(ieee,&th, &tl);
+//	printk("===>%s,%d[2 wake, 1 sleep, 0 do nothing], ieee->sta_sleep = %d\n",__FUNCTION__, sleep,ieee->sta_sleep);
+	/* 2 wake, 1 sleep, 0 do nothing */
+	if(sleep == 0)
+		goto out;
+
+	if(sleep == 1){
+
+		if(ieee->sta_sleep == 1)
+			ieee->enter_sleep_state(ieee->dev,th,tl);
+
+		else if(ieee->sta_sleep == 0){
+	//		printk("send null 1\n");
+			spin_lock_irqsave(&ieee->mgmt_tx_lock, flags2);
+
+			if(ieee->ps_is_queue_empty(ieee->dev)){
+
+
+				ieee->sta_sleep = 2;
+
+				ieee->ps_request_tx_ack(ieee->dev);
+
+				ieee80211_sta_ps_send_null_frame(ieee,1);
+
+				ieee->ps_th = th;
+				ieee->ps_tl = tl;
+			}
+			spin_unlock_irqrestore(&ieee->mgmt_tx_lock, flags2);
+
+		}
+
+
+	}else if(sleep == 2){
+//#warning CHECK_LOCK_HERE
+		spin_lock_irqsave(&ieee->mgmt_tx_lock, flags2);
+
+	//	printk("send wakeup packet\n");
+		ieee80211_sta_wakeup(ieee,1);
+
+		spin_unlock_irqrestore(&ieee->mgmt_tx_lock, flags2);
+	}
+
+out:
+	spin_unlock_irqrestore(&ieee->lock, flags);
+
+}
+
+void ieee80211_sta_wakeup(struct ieee80211_device *ieee, short nl)
+{
+	if(ieee->sta_sleep == 0){
+		if(nl){
+		//	printk("Warning: driver is probably failing to report TX ps error\n");
+			ieee->ps_request_tx_ack(ieee->dev);
+			ieee80211_sta_ps_send_null_frame(ieee, 0);
+		}
+		return;
+
+	}
+
+	if(ieee->sta_sleep == 1)
+		ieee->sta_wake_up(ieee->dev);
+
+	ieee->sta_sleep = 0;
+
+	if(nl){
+		ieee->ps_request_tx_ack(ieee->dev);
+		ieee80211_sta_ps_send_null_frame(ieee, 0);
+	}
+}
+
+void ieee80211_ps_tx_ack(struct ieee80211_device *ieee, short success)
+{
+	unsigned long flags,flags2;
+
+	spin_lock_irqsave(&ieee->lock, flags);
+	if(ieee->sta_sleep == 2){
+		/* Null frame with PS bit set */
+		if(success){
+
+		//	printk("==================> %s::enter sleep state\n",__FUNCTION__);
+			ieee->sta_sleep = 1;
+			ieee->enter_sleep_state(ieee->dev,ieee->ps_th,ieee->ps_tl);
+		}
+		/* if the card report not success we can't be sure the AP
+		 * has not RXed so we can't assume the AP believe us awake
+		 */
+	}
+	/* 21112005 - tx again null without PS bit if lost */
+	else {
+
+		if((ieee->sta_sleep == 0) && !success){
+			spin_lock_irqsave(&ieee->mgmt_tx_lock, flags2);
+			ieee80211_sta_ps_send_null_frame(ieee, 0);
+			spin_unlock_irqrestore(&ieee->mgmt_tx_lock, flags2);
+		}
+	}
+	spin_unlock_irqrestore(&ieee->lock, flags);
+}
+
+inline int
+ieee80211_rx_frame_softmac(struct ieee80211_device *ieee, struct sk_buff *skb,
+			struct ieee80211_rx_stats *rx_stats, u16 type,
+			u16 stype)
+{
+	struct ieee80211_hdr_3addr *header = (struct ieee80211_hdr_3addr *) skb->data;
+	u16 errcode;
+	u8* challenge=NULL;
+	int chlen=0;
+	int aid=0;
+	struct ieee80211_assoc_response_frame *assoc_resp;
+	struct ieee80211_info_element *info_element;
+
+	if(!ieee->proto_started)
+		return 0;
+
+	if(ieee->sta_sleep || (ieee->ps != IEEE80211_PS_DISABLED &&
+		ieee->iw_mode == IW_MODE_INFRA &&
+		ieee->state == IEEE80211_LINKED))
+
+		tasklet_schedule(&ieee->ps_task);
+
+	if(WLAN_FC_GET_STYPE(header->frame_ctl) != IEEE80211_STYPE_PROBE_RESP &&
+		WLAN_FC_GET_STYPE(header->frame_ctl) != IEEE80211_STYPE_BEACON)
+		ieee->last_rx_ps_time = jiffies;
+
+	switch (WLAN_FC_GET_STYPE(header->frame_ctl)) {
+
+		case IEEE80211_STYPE_ASSOC_RESP:
+		case IEEE80211_STYPE_REASSOC_RESP:
+
+			IEEE80211_DEBUG_MGMT("received [RE]ASSOCIATION RESPONSE (%d)\n",
+					WLAN_FC_GET_STYPE(header->frame_ctl));
+			if ((ieee->softmac_features & IEEE_SOFTMAC_ASSOCIATE) &&
+				ieee->state == IEEE80211_ASSOCIATING_AUTHENTICATED &&
+				ieee->iw_mode == IW_MODE_INFRA){
+				if (0 == (errcode=assoc_parse(skb, &aid))){
+					u16 left;
+
+					ieee->state=IEEE80211_LINKED;
+					ieee->assoc_id = aid;
+					ieee->softmac_stats.rx_ass_ok++;
+
+					//printk(KERN_WARNING "nic_type = %s", (rx_stats->nic_type == 1)?"rtl8187":"rtl8187B");
+					if(1 == rx_stats->nic_type) //card type is 8187
+					{
+						goto associate_complete;
+					}
+					assoc_resp = (struct ieee80211_assoc_response_frame*)skb->data;
+					info_element = 	&assoc_resp->info_element;
+					left = skb->len - ((void*)info_element - (void*)assoc_resp);
+
+					while (left >= sizeof(struct ieee80211_info_element_hdr)) {
+						if (sizeof(struct ieee80211_info_element_hdr) + info_element->len > left) {
+							printk(KERN_WARNING "[re]associate reeponse error!");
+							return 1;
+						}
+						switch (info_element->id) {
+						  case MFIE_TYPE_GENERIC:
+						         IEEE80211_DEBUG_SCAN("MFIE_TYPE_GENERIC: %d bytes\n", info_element->len);
+							if (info_element->len >= 8  &&
+							    info_element->data[0] == 0x00 &&
+							    info_element->data[1] == 0x50 &&
+							    info_element->data[2] == 0xf2 &&
+							    info_element->data[3] == 0x02 &&
+							    info_element->data[4] == 0x01) {
+							    // Not care about version at present.
+							    //WMM Parameter Element
+							    memcpy(ieee->current_network.wmm_param,(u8*)(info_element->data\
+										    + 8),(info_element->len - 8));
+
+					 	            if (((ieee->current_network.wmm_info^info_element->data[6])& \
+										    0x0f)||(!ieee->init_wmmparam_flag)) {
+						   	      //refresh paramete element for current network
+							      // update the register parameter for hardware
+							      ieee->init_wmmparam_flag = 1;
+							      queue_work(ieee->wq, &ieee->wmm_param_update_wq);
+
+						            }
+						            //update info_element for current network
+						            ieee->current_network.wmm_info  = info_element->data[6];
+							}
+							break;
+						  default:
+							//nothing to do at present!!!
+							break;
+						}
+
+						left -= sizeof(struct ieee80211_info_element_hdr) +
+							info_element->len;
+						info_element = (struct ieee80211_info_element *)
+							&info_element->data[info_element->len];
+					}
+					if(!ieee->init_wmmparam_flag) //legacy AP, reset the AC_xx_param register
+					{
+						queue_work(ieee->wq,&ieee->wmm_param_update_wq);
+						ieee->init_wmmparam_flag = 1;//indicate AC_xx_param upated since last associate
+					}
+associate_complete:
+					ieee80211_associate_complete(ieee);
+				}else{
+					ieee->softmac_stats.rx_ass_err++;
+					IEEE80211_DEBUG_MGMT(
+						"Association response status code 0x%x\n",
+						errcode);
+					ieee80211_associate_abort(ieee);
+				}
+			}
+#ifdef _RTL8187_EXT_PATCH_
+			else if ((ieee->iw_mode == ieee->iw_ext_mode) && ieee->ext_patch_ieee80211_rx_frame_softmac_on_assoc_rsp)
+			{
+					ieee->ext_patch_ieee80211_rx_frame_softmac_on_assoc_rsp(ieee, skb);
+			}
+#endif
+			break;
+
+		case IEEE80211_STYPE_ASSOC_REQ:
+		case IEEE80211_STYPE_REASSOC_REQ:
+
+			if ((ieee->softmac_features & IEEE_SOFTMAC_ASSOCIATE) &&
+				ieee->iw_mode == IW_MODE_MASTER)
+
+				ieee80211_rx_assoc_rq(ieee, skb);
+#ifdef _RTL8187_EXT_PATCH_
+			else if ((ieee->iw_mode == ieee->iw_ext_mode) && ieee->ext_patch_ieee80211_rx_frame_softmac_on_assoc_req)
+			{
+					ieee->ext_patch_ieee80211_rx_frame_softmac_on_assoc_req(ieee, skb);
+			}
+#endif
+			break;
+
+		case IEEE80211_STYPE_AUTH:
+
+#ifdef _RTL8187_EXT_PATCH_
+printk("IEEE80211_STYPE_AUTH\n");
+			if((ieee->iw_mode == ieee->iw_ext_mode) && ieee->ext_patch_ieee80211_rx_frame_softmac_on_auth)
+			if( ieee->ext_patch_ieee80211_rx_frame_softmac_on_auth(ieee, skb, rx_stats) );
+#endif
+			if (ieee->softmac_features & IEEE_SOFTMAC_ASSOCIATE){
+				if (ieee->state == IEEE80211_ASSOCIATING_AUTHENTICATING &&
+				ieee->iw_mode == IW_MODE_INFRA){
+
+						IEEE80211_DEBUG_MGMT("Received authentication response");
+
+						if (0 == (errcode=auth_parse(skb, &challenge, &chlen))){
+							if(ieee->open_wep || !challenge){
+								ieee->state = IEEE80211_ASSOCIATING_AUTHENTICATED;
+								ieee->softmac_stats.rx_auth_rs_ok++;
+
+								ieee80211_associate_step2(ieee);
+							}else{
+								ieee80211_auth_challenge(ieee, challenge, chlen);
+							}
+						}else{
+							ieee->softmac_stats.rx_auth_rs_err++;
+							IEEE80211_DEBUG_MGMT("Authentication respose status code 0x%x",errcode);
+							ieee80211_associate_abort(ieee);
+						}
+
+					}else if (ieee->iw_mode == IW_MODE_MASTER){
+						ieee80211_rx_auth_rq(ieee, skb);
+					}
+				}
+			break;
+
+		case IEEE80211_STYPE_PROBE_REQ:
+
+			if ((ieee->softmac_features & IEEE_SOFTMAC_PROBERS) &&
+				((ieee->iw_mode == IW_MODE_ADHOC ||
+				ieee->iw_mode == IW_MODE_MASTER) &&
+				ieee->state == IEEE80211_LINKED))
+
+				ieee80211_rx_probe_rq(ieee, skb);
+			break;
+
+		case IEEE80211_STYPE_DISASSOC:
+		case IEEE80211_STYPE_DEAUTH:
+#ifdef _RTL8187_EXT_PATCH_
+printk("IEEE80211_STYPE_DEAUTH\n");
+		if((ieee->iw_mode == ieee->iw_ext_mode) && ieee->ext_patch_ieee80211_rx_frame_softmac_on_deauth)
+		if( ieee->ext_patch_ieee80211_rx_frame_softmac_on_deauth(ieee, skb, rx_stats) )	;
+#endif
+			/* FIXME for now repeat all the association procedure
+			* both for disassociation and deauthentication
+			*/
+			if ((ieee->softmac_features & IEEE_SOFTMAC_ASSOCIATE) &&
+				(ieee->state == IEEE80211_LINKED) &&
+				(ieee->iw_mode == IW_MODE_INFRA) &&
+				(!memcmp(header->addr2,ieee->current_network.bssid,ETH_ALEN))){
+				ieee->state = IEEE80211_ASSOCIATING;
+				ieee->softmac_stats.reassoc++;
+
+				//notify_wx_assoc_event(ieee);  //YJ,del,080828, do not notify os here
+				queue_work(ieee->wq, &ieee->associate_procedure_wq);
+			}
+
+			break;
+
+		default:
+			return -1;
+			break;
+	}
+
+	//dev_kfree_skb_any(skb);
+	return 0;
+}
+
+
+
+/* following are for a simplier TX queue management.
+ * Instead of using netif_[stop/wake]_queue the driver
+ * will uses these two function (plus a reset one), that
+ * will internally uses the kernel netif_* and takes
+ * care of the ieee802.11 fragmentation.
+ * So the driver receives a fragment per time and might
+ * call the stop function when it want without take care
+ * to have enought room to TX an entire packet.
+ * This might be useful if each fragment need it's own
+ * descriptor, thus just keep a total free memory > than
+ * the max fragmentation treshold is not enought.. If the
+ * ieee802.11 stack passed a TXB struct then you needed
+ * to keep N free descriptors where
+ * N = MAX_PACKET_SIZE / MIN_FRAG_TRESHOLD
+ * In this way you need just one and the 802.11 stack
+ * will take care of buffering fragments and pass them to
+ * to the driver later, when it wakes the queue.
+ */
+
+void ieee80211_softmac_xmit(struct ieee80211_txb *txb, struct ieee80211_device *ieee)
+{
+
+
+	unsigned long flags;
+	int  i;
+#ifdef _RTL8187_EXT_PATCH_
+	int rate = ieee->rate;
+#endif
+
+	spin_lock_irqsave(&ieee->lock,flags);
+	#if 0
+	if(ieee->queue_stop){
+		IEEE80211DMESG("EE: IEEE hard_start_xmit invoked when kernel queue should be stopped");
+		netif_stop_queue(ieee->dev);
+		ieee->ieee_stats.swtxstop++;
+		//dev_kfree_skb_any(skb);
+		err = 1;
+		goto exit;
+	}
+
+	ieee->stats.tx_bytes+=skb->len;
+
+
+	txb=ieee80211_skb_to_txb(ieee,skb);
+
+
+	if(txb==NULL){
+		IEEE80211DMESG("WW: IEEE stack failed to provide txb");
+		//dev_kfree_skb_any(skb);
+		err = 1;
+		goto exit;
+	}
+	#endif
+
+#ifdef _RTL8187_EXT_PATCH_
+	if((ieee->iw_mode == ieee->iw_ext_mode) && ieee->ext_patch_ieee80211_softmac_xmit_get_rate && txb->nr_frags)
+	{
+		rate = ieee->ext_patch_ieee80211_softmac_xmit_get_rate(ieee, txb->fragments[0]);
+	}
+#endif
+	/* called with 2nd parm 0, no tx mgmt lock required */
+	ieee80211_sta_wakeup(ieee,0);
+
+	for(i = 0; i < txb->nr_frags; i++) {
+
+		if (ieee->queue_stop){
+			ieee->tx_pending.txb = txb;
+			ieee->tx_pending.frag = i;
+			goto exit;
+		}else{
+			ieee->softmac_data_hard_start_xmit(
+				txb->fragments[i],
+#ifdef _RTL8187_EXT_PATCH_
+				ieee->dev, rate);
+#else
+				ieee->dev,ieee->rate);
+#endif
+				//(i+1)<txb->nr_frags);
+			ieee->stats.tx_packets++;
+			ieee->stats.tx_bytes += txb->fragments[i]->len;
+			ieee->dev->trans_start = jiffies;
+		}
+	}
+
+	ieee80211_txb_free(txb);
+
+	exit:
+	spin_unlock_irqrestore(&ieee->lock,flags);
+
+}
+
+/* called with ieee->lock acquired */
+void ieee80211_resume_tx(struct ieee80211_device *ieee)
+{
+	int i;
+	for(i = ieee->tx_pending.frag; i < ieee->tx_pending.txb->nr_frags; i++) {
+
+		if (ieee->queue_stop){
+			ieee->tx_pending.frag = i;
+			return;
+		}else{
+
+			ieee->softmac_data_hard_start_xmit(
+				ieee->tx_pending.txb->fragments[i],
+				ieee->dev,ieee->rate);
+				//(i+1)<ieee->tx_pending.txb->nr_frags);
+			ieee->stats.tx_packets++;
+			ieee->dev->trans_start = jiffies;
+		}
+	}
+
+
+	ieee80211_txb_free(ieee->tx_pending.txb);
+	ieee->tx_pending.txb = NULL;
+}
+
+
+void ieee80211_reset_queue(struct ieee80211_device *ieee)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&ieee->lock,flags);
+	init_mgmt_queue(ieee);
+	if (ieee->tx_pending.txb){
+		ieee80211_txb_free(ieee->tx_pending.txb);
+		ieee->tx_pending.txb = NULL;
+	}
+	ieee->queue_stop = 0;
+	spin_unlock_irqrestore(&ieee->lock,flags);
+
+}
+
+void ieee80211_wake_queue(struct ieee80211_device *ieee)
+{
+
+	unsigned long flags;
+	struct sk_buff *skb;
+	struct ieee80211_hdr_3addr  *header;
+
+	spin_lock_irqsave(&ieee->lock,flags);
+	if (! ieee->queue_stop) goto exit;
+
+	ieee->queue_stop = 0;
+
+	if(ieee->softmac_features & IEEE_SOFTMAC_SINGLE_QUEUE){
+		while (!ieee->queue_stop && (skb = dequeue_mgmt(ieee))){
+
+			header = (struct ieee80211_hdr_3addr  *) skb->data;
+
+			header->seq_ctl = cpu_to_le16(ieee->seq_ctrl[0] << 4);
+
+			if (ieee->seq_ctrl[0] == 0xFFF)
+				ieee->seq_ctrl[0] = 0;
+			else
+				ieee->seq_ctrl[0]++;
+
+			//printk(KERN_ALERT "ieee80211_wake_queue \n");
+			ieee->softmac_data_hard_start_xmit(skb,ieee->dev,ieee->basic_rate);
+			dev_kfree_skb_any(skb);//edit by thomas
+		}
+	}
+	if (!ieee->queue_stop && ieee->tx_pending.txb)
+		ieee80211_resume_tx(ieee);
+
+	if (!ieee->queue_stop && netif_queue_stopped(ieee->dev)){
+		ieee->softmac_stats.swtxawake++;
+		netif_wake_queue(ieee->dev);
+	}
+
+exit :
+	spin_unlock_irqrestore(&ieee->lock,flags);
+}
+
+
+void ieee80211_stop_queue(struct ieee80211_device *ieee)
+{
+	//unsigned long flags;
+	//spin_lock_irqsave(&ieee->lock,flags);
+
+	if (! netif_queue_stopped(ieee->dev)){
+		netif_stop_queue(ieee->dev);
+		ieee->softmac_stats.swtxstop++;
+	}
+	ieee->queue_stop = 1;
+	//spin_unlock_irqrestore(&ieee->lock,flags);
+
+}
+
+
+inline void ieee80211_randomize_cell(struct ieee80211_device *ieee)
+{
+
+	get_random_bytes(ieee->current_network.bssid, ETH_ALEN);
+
+	/* an IBSS cell address must have the two less significant
+	 * bits of the first byte = 2
+	 */
+	ieee->current_network.bssid[0] &= ~0x01;
+	ieee->current_network.bssid[0] |= 0x02;
+}
+
+/* called in user context only */
+void ieee80211_start_master_bss(struct ieee80211_device *ieee)
+{
+	ieee->assoc_id = 1;
+
+	if (ieee->current_network.ssid_len == 0){
+		strncpy(ieee->current_network.ssid,
+			IEEE80211_DEFAULT_TX_ESSID,
+			IW_ESSID_MAX_SIZE);
+
+		ieee->current_network.ssid_len = strlen(IEEE80211_DEFAULT_TX_ESSID);
+		ieee->ssid_set = 1;
+	}
+
+	memcpy(ieee->current_network.bssid, ieee->dev->dev_addr, ETH_ALEN);
+
+	ieee->set_chan(ieee->dev, ieee->current_network.channel);
+	ieee->state = IEEE80211_LINKED;
+	ieee->link_change(ieee->dev);
+	notify_wx_assoc_event(ieee);
+
+	if (ieee->data_hard_resume)
+		ieee->data_hard_resume(ieee->dev);
+
+	netif_carrier_on(ieee->dev);
+}
+
+void ieee80211_start_monitor_mode(struct ieee80211_device *ieee)
+{
+	if(ieee->raw_tx){
+
+		if (ieee->data_hard_resume)
+			ieee->data_hard_resume(ieee->dev);
+
+		netif_carrier_on(ieee->dev);
+	}
+}
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+void ieee80211_start_ibss_wq(struct work_struct *work)
+{
+	struct delayed_work *dwork = container_of(work, struct delayed_work, work);
+	struct ieee80211_device *ieee = container_of(dwork, struct ieee80211_device, start_ibss_wq);
+#else
+void ieee80211_start_ibss_wq(struct ieee80211_device *ieee)
+{
+#endif
+
+	/* iwconfig mode ad-hoc will schedule this and return
+	 * on the other hand this will block further iwconfig SET
+	 * operations because of the wx_sem hold.
+	 * Anyway some most set operations set a flag to speed-up
+	 * (abort) this wq (when syncro scanning) before sleeping
+	 * on the semaphore
+	 */
+
+	down(&ieee->wx_sem);
+
+
+	if (ieee->current_network.ssid_len == 0){
+		strcpy(ieee->current_network.ssid,IEEE80211_DEFAULT_TX_ESSID);
+		ieee->current_network.ssid_len = strlen(IEEE80211_DEFAULT_TX_ESSID);
+		ieee->ssid_set = 1;
+	}
+
+	/* check if we have this cell in our network list */
+	ieee80211_softmac_check_all_nets(ieee);
+
+#ifdef ENABLE_DOT11D
+	if(ieee->state == IEEE80211_NOLINK)
+		ieee->current_network.channel = 10;
+#endif
+	/* if not then the state is not linked. Maybe the user swithced to
+	 * ad-hoc mode just after being in monitor mode, or just after
+	 * being very few time in managed mode (so the card have had no
+	 * time to scan all the chans..) or we have just run up the iface
+	 * after setting ad-hoc mode. So we have to give another try..
+	 * Here, in ibss mode, should be safe to do this without extra care
+	 * (in bss mode we had to make sure no-one tryed to associate when
+	 * we had just checked the ieee->state and we was going to start the
+	 * scan) beacause in ibss mode the ieee80211_new_net function, when
+	 * finds a good net, just set the ieee->state to IEEE80211_LINKED,
+	 * so, at worst, we waste a bit of time to initiate an unneeded syncro
+	 * scan, that will stop at the first round because it sees the state
+	 * associated.
+	 */
+	if (ieee->state == IEEE80211_NOLINK)
+		ieee80211_start_scan_syncro(ieee);
+
+	/* the network definitively is not here.. create a new cell */
+	if (ieee->state == IEEE80211_NOLINK){
+		printk("creating new IBSS cell\n");
+		if(!ieee->wap_set)
+			ieee80211_randomize_cell(ieee);
+
+		if(ieee->modulation & IEEE80211_CCK_MODULATION){
+
+			ieee->current_network.rates_len = 4;
+
+			ieee->current_network.rates[0] = IEEE80211_BASIC_RATE_MASK | IEEE80211_CCK_RATE_1MB;
+			ieee->current_network.rates[1] = IEEE80211_BASIC_RATE_MASK | IEEE80211_CCK_RATE_2MB;
+			ieee->current_network.rates[2] = IEEE80211_BASIC_RATE_MASK | IEEE80211_CCK_RATE_5MB;
+			ieee->current_network.rates[3] = IEEE80211_BASIC_RATE_MASK | IEEE80211_CCK_RATE_11MB;
+
+		}else
+			ieee->current_network.rates_len = 0;
+
+		if(ieee->modulation & IEEE80211_OFDM_MODULATION){
+			ieee->current_network.rates_ex_len = 8;
+
+			ieee->current_network.rates_ex[0] = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_6MB;
+			ieee->current_network.rates_ex[1] = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_9MB;
+			ieee->current_network.rates_ex[2] = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_12MB;
+			ieee->current_network.rates_ex[3] = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_18MB;
+			ieee->current_network.rates_ex[4] = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_24MB;
+			ieee->current_network.rates_ex[5] = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_36MB;
+			ieee->current_network.rates_ex[6] = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_48MB;
+			ieee->current_network.rates_ex[7] = IEEE80211_BASIC_RATE_MASK | IEEE80211_OFDM_RATE_54MB;
+
+			ieee->rate = 540;
+		}else{
+			ieee->current_network.rates_ex_len = 0;
+			ieee->rate = 110;
+		}
+
+		// By default, WMM function will be disabled in IBSS mode
+		ieee->current_network.QoS_Enable = 0;
+
+		ieee->current_network.atim_window = 0;
+		ieee->current_network.capability = WLAN_CAPABILITY_IBSS;
+		if(ieee->short_slot)
+			ieee->current_network.capability |= WLAN_CAPABILITY_SHORT_SLOT;
+
+	}
+
+	ieee->state = IEEE80211_LINKED;
+	ieee->set_chan(ieee->dev, ieee->current_network.channel);
+	ieee->link_change(ieee->dev);
+
+	notify_wx_assoc_event(ieee);
+
+	ieee80211_start_send_beacons(ieee);
+	printk(KERN_WARNING "after sending beacon packet!\n");
+
+	if (ieee->data_hard_resume)
+		ieee->data_hard_resume(ieee->dev);
+
+	netif_carrier_on(ieee->dev);
+
+	up(&ieee->wx_sem);
+}
+inline void ieee80211_start_ibss(struct ieee80211_device *ieee)
+{
+	queue_delayed_work(ieee->wq, &ieee->start_ibss_wq, 100);
+}
+
+/* this is called only in user context, with wx_sem held */
+void ieee80211_start_bss(struct ieee80211_device *ieee)
+{
+	unsigned long flags;
+#ifdef ENABLE_DOT11D
+	//
+	// Ref: 802.11d 11.1.3.3
+	// STA shall not start a BSS unless properly formed Beacon frame including a Country IE.
+	//
+	if(IS_DOT11D_ENABLE(ieee) && !IS_COUNTRY_IE_VALID(ieee))
+	{
+		if(! ieee->bGlobalDomain)
+		{
+			return;
+		}
+	}
+#endif
+	/* check if we have already found the net we
+	 * are interested in (if any).
+	 * if not (we are disassociated and we are not
+	 * in associating / authenticating phase) start the background scanning.
+	 */
+	ieee80211_softmac_check_all_nets(ieee);
+
+	/* ensure no-one start an associating process (thus setting
+	 * the ieee->state to ieee80211_ASSOCIATING) while we
+	 * have just cheked it and we are going to enable scan.
+	 * The ieee80211_new_net function is always called with
+	 * lock held (from both ieee80211_softmac_check_all_nets and
+	 * the rx path), so we cannot be in the middle of such function
+	 */
+	spin_lock_irqsave(&ieee->lock, flags);
+
+//#ifdef ENABLE_IPS
+//	printk("start bss ENABLE_IPS\n");
+//#else
+	if (ieee->state == IEEE80211_NOLINK){
+		ieee->actscanning = true;
+		ieee80211_start_scan(ieee);
+	}
+//#endif
+	spin_unlock_irqrestore(&ieee->lock, flags);
+}
+
+/* called only in userspace context */
+void ieee80211_disassociate(struct ieee80211_device *ieee)
+{
+	netif_carrier_off(ieee->dev);
+
+	if (ieee->softmac_features & IEEE_SOFTMAC_TX_QUEUE)
+			ieee80211_reset_queue(ieee);
+
+	if (ieee->data_hard_stop)
+			ieee->data_hard_stop(ieee->dev);
+
+#ifdef ENABLE_DOT11D
+	if(IS_DOT11D_ENABLE(ieee))
+		Dot11d_Reset(ieee);
+#endif
+	ieee->state = IEEE80211_NOLINK;
+	ieee->link_change(ieee->dev);
+	notify_wx_assoc_event(ieee);
+
+}
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+void ieee80211_associate_retry_wq(struct work_struct *work)
+{
+	struct delayed_work *dwork = container_of(work, struct delayed_work, work);
+	struct ieee80211_device *ieee = container_of(dwork, struct ieee80211_device, associate_retry_wq);
+#else
+void ieee80211_associate_retry_wq(struct ieee80211_device *ieee)
+{
+#endif
+	unsigned long flags;
+	down(&ieee->wx_sem);
+	if(!ieee->proto_started)
+		goto exit;
+	if(ieee->state != IEEE80211_ASSOCIATING_RETRY)
+		goto exit;
+	/* until we do not set the state to IEEE80211_NOLINK
+	* there are no possibility to have someone else trying
+	* to start an association procdure (we get here with
+	* ieee->state = IEEE80211_ASSOCIATING).
+	* When we set the state to IEEE80211_NOLINK it is possible
+	* that the RX path run an attempt to associate, but
+	* both ieee80211_softmac_check_all_nets and the
+	* RX path works with ieee->lock held so there are no
+	* problems. If we are still disassociated then start a scan.
+	* the lock here is necessary to ensure no one try to start
+	* an association procedure when we have just checked the
+	* state and we are going to start the scan.
+	*/
+	ieee->state = IEEE80211_NOLINK;
+	ieee->beinretry = true;
+	ieee80211_softmac_check_all_nets(ieee);
+
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	if(ieee->state == IEEE80211_NOLINK){
+		ieee->beinretry = false;
+		ieee->actscanning = true;
+		ieee80211_start_scan(ieee);
+	}
+	//YJ,add,080828, notify os here
+	if(ieee->state == IEEE80211_NOLINK)
+	{
+		notify_wx_assoc_event(ieee);
+	}
+	//YJ,add,080828,end
+	spin_unlock_irqrestore(&ieee->lock, flags);
+
+exit:
+	up(&ieee->wx_sem);
+}
+
+struct sk_buff *ieee80211_get_beacon_(struct ieee80211_device *ieee)
+{
+	u8 broadcast_addr[] = {0xff,0xff,0xff,0xff,0xff,0xff};
+
+	struct sk_buff *skb = NULL;
+	struct ieee80211_probe_response *b;
+
+//rz
+#ifdef _RTL8187_EXT_PATCH_
+	if((ieee->iw_mode == ieee->iw_ext_mode) && ieee->ext_patch_get_beacon_get_probersp )
+		skb = ieee->ext_patch_get_beacon_get_probersp(ieee, broadcast_addr, &(ieee->current_network));
+	else
+		skb = ieee80211_probe_resp(ieee, broadcast_addr);
+#else
+	skb = ieee80211_probe_resp(ieee, broadcast_addr);
+#endif
+//
+	if (!skb)
+		return NULL;
+
+	b = (struct ieee80211_probe_response *) skb->data;
+	b->header.frame_ctl = cpu_to_le16(IEEE80211_STYPE_BEACON);
+
+	return skb;
+
+}
+
+struct sk_buff *ieee80211_get_beacon(struct ieee80211_device *ieee)
+{
+	struct sk_buff *skb;
+	struct ieee80211_probe_response *b;
+
+	skb = ieee80211_get_beacon_(ieee);
+	if(!skb)
+		return NULL;
+
+	b = (struct ieee80211_probe_response *) skb->data;
+	b->header.seq_ctrl = cpu_to_le16(ieee->seq_ctrl[0] << 4);
+
+	if (ieee->seq_ctrl[0] == 0xFFF)
+		ieee->seq_ctrl[0] = 0;
+	else
+		ieee->seq_ctrl[0]++;
+
+	return skb;
+}
+
+void ieee80211_softmac_stop_protocol(struct ieee80211_device *ieee)
+{
+	ieee->sync_scan_hurryup = 1;
+	down(&ieee->wx_sem);
+	ieee80211_stop_protocol(ieee);
+	up(&ieee->wx_sem);
+}
+
+
+void ieee80211_stop_protocol(struct ieee80211_device *ieee)
+{
+	if (!ieee->proto_started)
+		return;
+
+	ieee->proto_started = 0;
+
+#ifdef _RTL8187_EXT_PATCH_
+	if(ieee->ext_patch_ieee80211_stop_protocol)
+		ieee->ext_patch_ieee80211_stop_protocol(ieee);
+//if call queue_delayed_work,can call this,or do nothing..
+//edit by lawrence,20071118
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,5,0)
+//	cancel_delayed_work(&ieee->ext_stop_scan_wq);
+//	cancel_delayed_work(&ieee->ext_send_beacon_wq);
+#endif
+#endif // _RTL8187_EXT_PATCH_
+
+	ieee80211_stop_send_beacons(ieee);
+	if((ieee->iw_mode == IW_MODE_INFRA)&&(ieee->state == IEEE80211_LINKED)) {
+		SendDisassociation(ieee,NULL,WLAN_REASON_DISASSOC_STA_HAS_LEFT);
+	}
+	del_timer_sync(&ieee->associate_timer);
+	cancel_delayed_work(&ieee->associate_retry_wq);
+	cancel_delayed_work(&ieee->start_ibss_wq);
+	ieee80211_stop_scan(ieee);
+
+	ieee80211_disassociate(ieee);
+}
+
+void ieee80211_softmac_start_protocol(struct ieee80211_device *ieee)
+{
+	ieee->sync_scan_hurryup = 0;
+	down(&ieee->wx_sem);
+	ieee80211_start_protocol(ieee);
+	up(&ieee->wx_sem);
+}
+
+void ieee80211_start_protocol(struct ieee80211_device *ieee)
+{
+	short ch = 0;
+ 	int i = 0;
+
+	if (ieee->proto_started)
+		return;
+
+	ieee->proto_started = 1;
+
+	if (ieee->current_network.channel == 0){
+		do{
+			ch++;
+			if (ch > MAX_CHANNEL_NUMBER)
+				return; /* no channel found */
+
+#ifdef ENABLE_DOT11D
+		}while(!GET_DOT11D_INFO(ieee)->channel_map[ch]);
+#else
+		}while(!ieee->channel_map[ch]);
+#endif
+
+		ieee->current_network.channel = ch;
+	}
+
+	if (ieee->current_network.beacon_interval == 0)
+		ieee->current_network.beacon_interval = 100;
+	ieee->set_chan(ieee->dev,ieee->current_network.channel);
+
+       	for(i = 0; i < 17; i++) {
+	  ieee->last_rxseq_num[i] = -1;
+	  ieee->last_rxfrag_num[i] = -1;
+	  ieee->last_packet_time[i] = 0;
+	}
+
+	ieee->init_wmmparam_flag = 0;//reinitialize AC_xx_PARAM registers.
+
+
+	/* if the user set the MAC of the ad-hoc cell and then
+	 * switch to managed mode, shall we  make sure that association
+	 * attempts does not fail just because the user provide the essid
+	 * and the nic is still checking for the AP MAC ??
+	 */
+	switch (ieee->iw_mode) {
+		case IW_MODE_AUTO:
+			ieee->iw_mode = IW_MODE_INFRA;
+			//not set break here intentionly
+		case IW_MODE_INFRA:
+			ieee80211_start_bss(ieee);
+			break;
+
+		case IW_MODE_ADHOC:
+			ieee80211_start_ibss(ieee);
+			break;
+
+		case IW_MODE_MASTER:
+			ieee80211_start_master_bss(ieee);
+		break;
+
+		case IW_MODE_MONITOR:
+			ieee80211_start_monitor_mode(ieee);
+			break;
+
+		default:
+#ifdef _RTL8187_EXT_PATCH_
+			if((ieee->iw_mode == ieee->iw_ext_mode) &&\
+			    ieee->ext_patch_ieee80211_start_protocol &&\
+                            ieee->ext_patch_ieee80211_start_protocol(ieee)) {
+#if LINUX_VERSION_CODE > KERNEL_VERSION(2,5,0)
+				queue_work(ieee->wq, &ieee->ext_stop_scan_wq);
+#endif
+				// By default, WMM function will be disabled in
+				// EXTENSION mode
+				ieee->current_network.QoS_Enable = 0;
+
+				if(ieee->modulation & IEEE80211_CCK_MODULATION){
+					ieee->current_network.rates_len = 4;
+					ieee->current_network.rates[0] = \
+                                              IEEE80211_BASIC_RATE_MASK | \
+					      IEEE80211_CCK_RATE_1MB;
+					ieee->current_network.rates[1] = \
+                                              IEEE80211_BASIC_RATE_MASK |\
+                                              IEEE80211_CCK_RATE_2MB;
+					ieee->current_network.rates[2] = \
+                                              IEEE80211_BASIC_RATE_MASK |\
+                                              IEEE80211_CCK_RATE_5MB;
+					ieee->current_network.rates[3] = \
+                                              IEEE80211_BASIC_RATE_MASK |\
+                                              IEEE80211_CCK_RATE_11MB;
+				}else
+					ieee->current_network.rates_len = 0;
+
+				if(ieee->modulation & IEEE80211_OFDM_MODULATION){
+					ieee->current_network.rates_ex_len = 8;
+					ieee->current_network.rates_ex[0] = \
+                                              IEEE80211_BASIC_RATE_MASK |\
+                                              IEEE80211_OFDM_RATE_6MB;
+					ieee->current_network.rates_ex[1] = \
+                                              IEEE80211_BASIC_RATE_MASK |\
+                                              IEEE80211_OFDM_RATE_9MB;
+					ieee->current_network.rates_ex[2] = \
+                                              IEEE80211_BASIC_RATE_MASK |\
+                                              IEEE80211_OFDM_RATE_12MB;
+					ieee->current_network.rates_ex[3] = \
+                                              IEEE80211_BASIC_RATE_MASK | \
+                                              IEEE80211_OFDM_RATE_18MB;
+					ieee->current_network.rates_ex[4] =\
+                                              IEEE80211_BASIC_RATE_MASK |\
+                                              IEEE80211_OFDM_RATE_24MB;
+					ieee->current_network.rates_ex[5] =\
+                                              IEEE80211_BASIC_RATE_MASK |\
+                                              IEEE80211_OFDM_RATE_36MB;
+					ieee->current_network.rates_ex[6] = \
+                                              IEEE80211_BASIC_RATE_MASK |\
+                                              IEEE80211_OFDM_RATE_48MB;
+					ieee->current_network.rates_ex[7] =\
+                                              IEEE80211_BASIC_RATE_MASK |\
+                                              IEEE80211_OFDM_RATE_54MB;
+					ieee->rate = 540;
+				}else{
+					ieee->current_network.rates_ex_len = 0;
+					ieee->rate = 110;
+				}
+
+				/*
+				   spin_lock_irqsave(&ieee->lock, flags);
+				   if (ieee->state == IEEE80211_NOLINK)
+				   ieee80211_start_scan(ieee);
+				// ieee->set_chan(ieee->dev, 8);
+
+				spin_unlock_irqrestore(&ieee->lock, flags);
+				*/
+				memcpy(ieee->current_network.bssid, ieee->dev->dev_addr,\
+				      	 ETH_ALEN);
+				ieee->link_change(ieee->dev);
+				notify_wx_assoc_event(ieee);
+
+				if (ieee->data_hard_resume)
+					ieee->data_hard_resume(ieee->dev);
+
+				netif_carrier_on(ieee->dev);
+			} else {
+				ieee->iw_mode = IW_MODE_INFRA;
+				ieee80211_start_bss(ieee);
+			}
+#else
+			ieee->iw_mode = IW_MODE_INFRA;
+			ieee80211_start_bss(ieee);
+
+#endif
+			break;
+	}
+}
+
+
+#define DRV_NAME  "Ieee80211"
+void ieee80211_softmac_init(struct ieee80211_device *ieee)
+{
+	int i;
+	memset(&ieee->current_network, 0, sizeof(struct ieee80211_network));
+
+	ieee->state = IEEE80211_NOLINK;
+	ieee->sync_scan_hurryup = 0;
+	for(i = 0; i < 5; i++) {
+	  ieee->seq_ctrl[i] = 0;
+	}
+
+	ieee->assoc_id = 0;
+	ieee->queue_stop = 0;
+	ieee->scanning = 0;
+	ieee->softmac_features = 0; //so IEEE2100-like driver are happy
+	ieee->wap_set = 0;
+	ieee->ssid_set = 0;
+	ieee->proto_started = 0;
+	ieee->basic_rate = IEEE80211_DEFAULT_BASIC_RATE;
+	ieee->rate = 3;
+//#ifdef ENABLE_LPS
+	ieee->ps = IEEE80211_PS_MBCAST|IEEE80211_PS_UNICAST;
+//#else
+//	ieee->ps = IEEE80211_PS_DISABLED;
+//#endif
+	ieee->sta_sleep = 0;
+//by amy
+	ieee->bInactivePs = false;
+	ieee->actscanning = false;
+	ieee->ListenInterval = 2;
+	ieee->NumRxDataInPeriod = 0; //YJ,add,080828
+	ieee->NumRxBcnInPeriod = 0; //YJ,add,080828
+	ieee->NumRxOkTotal = 0;//+by amy 080312
+	ieee->NumRxUnicast = 0;//YJ,add,080828,for keep alive
+	ieee->beinretry = false;
+	ieee->bHwRadioOff = false;
+//by amy
+#ifdef _RTL8187_EXT_PATCH_
+	ieee->iw_ext_mode = 999;
+#endif
+
+	init_mgmt_queue(ieee);
+#if 0
+	init_timer(&ieee->scan_timer);
+	ieee->scan_timer.data = (unsigned long)ieee;
+	ieee->scan_timer.function = ieee80211_softmac_scan_cb;
+#endif
+	ieee->tx_pending.txb = NULL;
+
+	init_timer(&ieee->associate_timer);
+	ieee->associate_timer.data = (unsigned long)ieee;
+	ieee->associate_timer.function = ieee80211_associate_abort_cb;
+
+	init_timer(&ieee->beacon_timer);
+	ieee->beacon_timer.data = (unsigned long) ieee;
+	ieee->beacon_timer.function = ieee80211_send_beacon_cb;
+
+#ifdef PF_SYNCTHREAD
+	ieee->wq = create_workqueue(DRV_NAME,0);
+#else
+	ieee->wq = create_workqueue(DRV_NAME);
+#endif
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20)//added by lawrence,070702
+	INIT_DELAYED_WORK(&ieee->start_ibss_wq,(void*) ieee80211_start_ibss_wq);
+	INIT_WORK(&ieee->associate_complete_wq,(void*) ieee80211_associate_complete_wq);
+	INIT_WORK(&ieee->associate_procedure_wq,(void*) ieee80211_associate_procedure_wq);
+	INIT_DELAYED_WORK(&ieee->softmac_scan_wq,(void*) ieee80211_softmac_scan_wq);
+	INIT_DELAYED_WORK(&ieee->associate_retry_wq,(void*) ieee80211_associate_retry_wq);
+	INIT_WORK(&ieee->wx_sync_scan_wq,(void*) ieee80211_wx_sync_scan_wq);
+//	INIT_WORK(&ieee->watch_dog_wq,(void*) ieee80211_watch_dog_wq);
+//added by lawrence,20071118
+#ifdef _RTL8187_EXT_PATCH_
+	INIT_WORK(&ieee->ext_stop_scan_wq,(void*) ieee80211_ext_stop_scan_wq);
+	//INIT_WORK(&ieee->ext_send_beacon_wq,(void*) ieee80211_beacons_start,ieee);
+	INIT_WORK(&ieee->ext_send_beacon_wq,(void*) ext_ieee80211_send_beacon_wq);
+#endif //_RTL8187_EXT_PATCH_
+#else
+	INIT_WORK(&ieee->start_ibss_wq,(void*) ieee80211_start_ibss_wq,ieee);
+	INIT_WORK(&ieee->associate_retry_wq,(void*) ieee80211_associate_retry_wq,ieee);
+	INIT_WORK(&ieee->associate_complete_wq,(void*) ieee80211_associate_complete_wq,ieee);
+	INIT_WORK(&ieee->associate_procedure_wq,(void*) ieee80211_associate_procedure_wq,ieee);
+	INIT_WORK(&ieee->softmac_scan_wq,(void*) ieee80211_softmac_scan_wq,ieee);
+	INIT_WORK(&ieee->wx_sync_scan_wq,(void*) ieee80211_wx_sync_scan_wq,ieee);
+//	INIT_WORK(&ieee->watch_dog_wq,(void*) ieee80211_watch_dog_wq,ieee);
+#ifdef _RTL8187_EXT_PATCH_
+	INIT_WORK(&ieee->ext_stop_scan_wq,(void*) ieee80211_ext_stop_scan_wq,ieee);
+	//INIT_WORK(&ieee->ext_send_beacon_wq,(void*) ieee80211_beacons_start,ieee);
+	INIT_WORK(&ieee->ext_send_beacon_wq,(void*) ext_ieee80211_send_beacon_wq,ieee);
+#endif
+#endif
+	sema_init(&ieee->wx_sem, 1);
+	sema_init(&ieee->scan_sem, 1);
+
+	spin_lock_init(&ieee->mgmt_tx_lock);
+	spin_lock_init(&ieee->beacon_lock);
+
+	tasklet_init(&ieee->ps_task,
+	     (void(*)(unsigned long)) ieee80211_sta_ps,
+	     (unsigned long)ieee);
+#ifdef ENABLE_DOT11D
+	ieee->pDot11dInfo = kmalloc(sizeof(RT_DOT11D_INFO), GFP_ATOMIC);
+#endif
+}
+
+void ieee80211_softmac_free(struct ieee80211_device *ieee)
+{
+	down(&ieee->wx_sem);
+
+	del_timer_sync(&ieee->associate_timer);
+	cancel_delayed_work(&ieee->associate_retry_wq);
+
+
+	//add for RF power on power of by lizhaoming 080512
+	cancel_delayed_work(&ieee->GPIOChangeRFWorkItem);
+
+#ifdef _RTL8187_EXT_PATCH_
+	cancel_delayed_work(&ieee->ext_stop_scan_wq);
+	cancel_delayed_work(&ieee->ext_send_beacon_wq);
+#endif
+	destroy_workqueue(ieee->wq);
+#ifdef ENABLE_DOT11D
+	if(NULL != ieee->pDot11dInfo)
+		kfree(ieee->pDot11dInfo);
+#endif
+	up(&ieee->wx_sem);
+}
+
+/********************************************************
+ * Start of WPA code.                                   *
+ * this is stolen from the ipw2200 driver               *
+ ********************************************************/
+
+
+static int ieee80211_wpa_enable(struct ieee80211_device *ieee, int value)
+{
+	/* This is called when wpa_supplicant loads and closes the driver
+	 * interface. */
+	printk("%s WPA\n",value ? "enabling" : "disabling");
+	ieee->wpa_enabled = value;
+	return 0;
+}
+
+
+void ieee80211_wpa_assoc_frame(struct ieee80211_device *ieee, char *wpa_ie, int wpa_ie_len)
+{
+	/* make sure WPA is enabled */
+	ieee80211_wpa_enable(ieee, 1);
+
+	ieee80211_disassociate(ieee);
+}
+
+
+static int ieee80211_wpa_mlme(struct ieee80211_device *ieee, int command, int reason)
+{
+
+	int ret = 0;
+
+	switch (command) {
+	case IEEE_MLME_STA_DEAUTH:
+		// silently ignore
+		break;
+
+	case IEEE_MLME_STA_DISASSOC:
+		ieee80211_disassociate(ieee);
+		break;
+
+	default:
+		printk("Unknown MLME request: %d\n", command);
+		ret = -EOPNOTSUPP;
+	}
+
+	return ret;
+}
+
+
+static int ieee80211_wpa_set_wpa_ie(struct ieee80211_device *ieee,
+			      struct ieee_param *param, int plen)
+{
+	u8 *buf;
+
+	if (param->u.wpa_ie.len > MAX_WPA_IE_LEN ||
+	    (param->u.wpa_ie.len && param->u.wpa_ie.data == NULL))
+		return -EINVAL;
+
+	if (param->u.wpa_ie.len) {
+		buf = kmalloc(param->u.wpa_ie.len, GFP_KERNEL);
+		if (buf == NULL)
+			return -ENOMEM;
+
+		memcpy(buf, param->u.wpa_ie.data, param->u.wpa_ie.len);
+		kfree(ieee->wpa_ie);
+		ieee->wpa_ie = buf;
+		ieee->wpa_ie_len = param->u.wpa_ie.len;
+	} else {
+		kfree(ieee->wpa_ie);
+		ieee->wpa_ie = NULL;
+		ieee->wpa_ie_len = 0;
+	}
+
+	ieee80211_wpa_assoc_frame(ieee, ieee->wpa_ie, ieee->wpa_ie_len);
+	return 0;
+}
+
+#define AUTH_ALG_OPEN_SYSTEM			0x1
+#define AUTH_ALG_SHARED_KEY			0x2
+
+static int ieee80211_wpa_set_auth_algs(struct ieee80211_device *ieee, int value)
+{
+
+	struct ieee80211_security sec = {
+		.flags = SEC_AUTH_MODE,
+	};
+	int ret = 0;
+
+	if (value & AUTH_ALG_SHARED_KEY) {
+		sec.auth_mode = WLAN_AUTH_SHARED_KEY;
+		ieee->open_wep = 0;
+	} else {
+		sec.auth_mode = WLAN_AUTH_OPEN;
+		ieee->open_wep = 1;
+	}
+
+	if (ieee->set_security)
+		ieee->set_security(ieee->dev, &sec);
+	else
+		ret = -EOPNOTSUPP;
+
+	return ret;
+}
+
+static int ieee80211_wpa_set_param(struct ieee80211_device *ieee, u8 name, u32 value)
+{
+	int ret=0;
+	unsigned long flags;
+
+	switch (name) {
+	case IEEE_PARAM_WPA_ENABLED:
+		ret = ieee80211_wpa_enable(ieee, value);
+		break;
+
+	case IEEE_PARAM_TKIP_COUNTERMEASURES:
+		ieee->tkip_countermeasures=value;
+		break;
+
+	case IEEE_PARAM_DROP_UNENCRYPTED: {
+		/* HACK:
+		 *
+		 * wpa_supplicant calls set_wpa_enabled when the driver
+		 * is loaded and unloaded, regardless of if WPA is being
+		 * used.  No other calls are made which can be used to
+		 * determine if encryption will be used or not prior to
+		 * association being expected.  If encryption is not being
+		 * used, drop_unencrypted is set to false, else true -- we
+		 * can use this to determine if the CAP_PRIVACY_ON bit should
+		 * be set.
+		 */
+		struct ieee80211_security sec = {
+			.flags = SEC_ENABLED,
+			.enabled = value,
+		};
+ 		ieee->drop_unencrypted = value;
+		/* We only change SEC_LEVEL for open mode. Others
+		 * are set by ipw_wpa_set_encryption.
+		 */
+		if (!value) {
+			sec.flags |= SEC_LEVEL;
+			sec.level = SEC_LEVEL_0;
+		}
+		else {
+			sec.flags |= SEC_LEVEL;
+			sec.level = SEC_LEVEL_1;
+		}
+		if (ieee->set_security)
+			ieee->set_security(ieee->dev, &sec);
+		break;
+	}
+
+	case IEEE_PARAM_PRIVACY_INVOKED:
+		ieee->privacy_invoked=value;
+		break;
+
+	case IEEE_PARAM_AUTH_ALGS:
+		ret = ieee80211_wpa_set_auth_algs(ieee, value);
+		break;
+
+	case IEEE_PARAM_IEEE_802_1X:
+		ieee->ieee802_1x=value;
+		break;
+	case IEEE_PARAM_WPAX_SELECT:
+		// added for WPA2 mixed mode
+		//printk(KERN_WARNING "------------------------>wpax value = %x\n", value);
+		spin_lock_irqsave(&ieee->wpax_suitlist_lock,flags);
+		ieee->wpax_type_set = 1;
+		ieee->wpax_type_notify = value;
+		spin_unlock_irqrestore(&ieee->wpax_suitlist_lock,flags);
+		break;
+
+	default:
+		printk("Unknown WPA param: %d\n",name);
+		ret = -EOPNOTSUPP;
+	}
+
+	return ret;
+}
+
+/* implementation borrowed from hostap driver */
+
+static int ieee80211_wpa_set_encryption(struct ieee80211_device *ieee,
+				  struct ieee_param *param, int param_len)
+{
+	int ret = 0;
+
+	struct ieee80211_crypto_ops *ops;
+	struct ieee80211_crypt_data **crypt;
+
+	struct ieee80211_security sec = {
+		.flags = 0,
+	};
+
+	param->u.crypt.err = 0;
+	param->u.crypt.alg[IEEE_CRYPT_ALG_NAME_LEN - 1] = '\0';
+
+	if (param_len !=
+	    (int) ((char *) param->u.crypt.key - (char *) param) +
+	    param->u.crypt.key_len) {
+		printk("Len mismatch %d, %d\n", param_len,
+			       param->u.crypt.key_len);
+		return -EINVAL;
+	}
+	if (param->sta_addr[0] == 0xff && param->sta_addr[1] == 0xff &&
+	    param->sta_addr[2] == 0xff && param->sta_addr[3] == 0xff &&
+	    param->sta_addr[4] == 0xff && param->sta_addr[5] == 0xff) {
+		if (param->u.crypt.idx >= WEP_KEYS)
+			return -EINVAL;
+		crypt = &ieee->crypt[param->u.crypt.idx];
+	} else {
+		return -EINVAL;
+	}
+
+	if (strcmp(param->u.crypt.alg, "none") == 0) {
+		if (crypt) {
+			sec.enabled = 0;
+			// FIXME FIXME
+			//sec.encrypt = 0;
+			sec.level = SEC_LEVEL_0;
+			sec.flags |= SEC_ENABLED | SEC_LEVEL;
+			ieee80211_crypt_delayed_deinit(ieee, crypt);
+		}
+		goto done;
+	}
+	sec.enabled = 1;
+// FIXME FIXME
+//	sec.encrypt = 1;
+	sec.flags |= SEC_ENABLED;
+
+	/* IPW HW cannot build TKIP MIC, host decryption still needed. */
+	if (!(ieee->host_encrypt || ieee->host_decrypt) &&
+	    strcmp(param->u.crypt.alg, "TKIP"))
+		goto skip_host_crypt;
+
+	ops = ieee80211_get_crypto_ops(param->u.crypt.alg);
+	if (ops == NULL && strcmp(param->u.crypt.alg, "WEP") == 0) {
+		request_module("ieee80211_crypt_wep");
+		ops = ieee80211_get_crypto_ops(param->u.crypt.alg);
+	} else if (ops == NULL && strcmp(param->u.crypt.alg, "TKIP") == 0) {
+		request_module("ieee80211_crypt_tkip");
+		ops = ieee80211_get_crypto_ops(param->u.crypt.alg);
+	} else if (ops == NULL && strcmp(param->u.crypt.alg, "CCMP") == 0) {
+		request_module("ieee80211_crypt_ccmp");
+		ops = ieee80211_get_crypto_ops(param->u.crypt.alg);
+	}
+	if (ops == NULL) {
+		printk("unknown crypto alg '%s'\n", param->u.crypt.alg);
+		param->u.crypt.err = IEEE_CRYPT_ERR_UNKNOWN_ALG;
+		ret = -EINVAL;
+		goto done;
+	}
+
+	if (*crypt == NULL || (*crypt)->ops != ops) {
+		struct ieee80211_crypt_data *new_crypt;
+
+		ieee80211_crypt_delayed_deinit(ieee, crypt);
+
+		new_crypt = (struct ieee80211_crypt_data *)
+			kmalloc(sizeof(*new_crypt), GFP_KERNEL);
+		if (new_crypt == NULL) {
+			ret = -ENOMEM;
+			goto done;
+		}
+		memset(new_crypt, 0, sizeof(struct ieee80211_crypt_data));
+		new_crypt->ops = ops;
+		if (new_crypt->ops && try_module_get(new_crypt->ops->owner))
+			new_crypt->priv =
+				new_crypt->ops->init(param->u.crypt.idx);
+
+		if (new_crypt->priv == NULL) {
+			kfree(new_crypt);
+			param->u.crypt.err = IEEE_CRYPT_ERR_CRYPT_INIT_FAILED;
+			ret = -EINVAL;
+			goto done;
+		}
+
+		*crypt = new_crypt;
+	}
+
+	if (param->u.crypt.key_len > 0 && (*crypt)->ops->set_key &&
+	    (*crypt)->ops->set_key(param->u.crypt.key,
+				   param->u.crypt.key_len, param->u.crypt.seq,
+				   (*crypt)->priv) < 0) {
+		printk("key setting failed\n");
+		param->u.crypt.err = IEEE_CRYPT_ERR_KEY_SET_FAILED;
+		ret = -EINVAL;
+		goto done;
+	}
+
+ skip_host_crypt:
+	if (param->u.crypt.set_tx) {
+		ieee->tx_keyidx = param->u.crypt.idx;
+		sec.active_key = param->u.crypt.idx;
+		sec.flags |= SEC_ACTIVE_KEY;
+	} else
+		sec.flags &= ~SEC_ACTIVE_KEY;
+
+	if (param->u.crypt.alg != NULL) {
+		memcpy(sec.keys[param->u.crypt.idx],
+		       param->u.crypt.key,
+		       param->u.crypt.key_len);
+		sec.key_sizes[param->u.crypt.idx] = param->u.crypt.key_len;
+		sec.flags |= (1 << param->u.crypt.idx);
+
+		if (strcmp(param->u.crypt.alg, "WEP") == 0) {
+			sec.flags |= SEC_LEVEL;
+			sec.level = SEC_LEVEL_1;
+		} else if (strcmp(param->u.crypt.alg, "TKIP") == 0) {
+			sec.flags |= SEC_LEVEL;
+			sec.level = SEC_LEVEL_2;
+		} else if (strcmp(param->u.crypt.alg, "CCMP") == 0) {
+			sec.flags |= SEC_LEVEL;
+			sec.level = SEC_LEVEL_3;
+		}
+	}
+ done:
+	if (ieee->set_security)
+		ieee->set_security(ieee->dev, &sec);
+
+	/* Do not reset port if card is in Managed mode since resetting will
+	 * generate new IEEE 802.11 authentication which may end up in looping
+	 * with IEEE 802.1X.  If your hardware requires a reset after WEP
+	 * configuration (for example... Prism2), implement the reset_port in
+	 * the callbacks structures used to initialize the 802.11 stack. */
+	if (ieee->reset_on_keychange &&
+	    ieee->iw_mode != IW_MODE_INFRA &&
+	    ieee->reset_port &&
+	    ieee->reset_port(ieee->dev)) {
+		printk("reset_port failed\n");
+		param->u.crypt.err = IEEE_CRYPT_ERR_CARD_CONF_FAILED;
+		return -EINVAL;
+	}
+
+	return ret;
+}
+
+int ieee80211_wpa_supplicant_ioctl(struct ieee80211_device *ieee, struct iw_point *p)
+{
+	struct ieee_param *param;
+	int ret=0;
+
+	down(&ieee->wx_sem);
+	//IEEE_DEBUG_INFO("wpa_supplicant: len=%d\n", p->length);
+
+	if (p->length < sizeof(struct ieee_param) || !p->pointer){
+		ret = -EINVAL;
+		goto out;
+	}
+
+	param = (struct ieee_param *)kmalloc(p->length, GFP_KERNEL);
+	if (param == NULL){
+		ret = -ENOMEM;
+		goto out;
+	}
+	if (copy_from_user(param, p->pointer, p->length)) {
+		kfree(param);
+		ret = -EFAULT;
+		goto out;
+	}
+
+	switch (param->cmd) {
+
+	case IEEE_CMD_SET_WPA_PARAM:
+		ret = ieee80211_wpa_set_param(ieee, param->u.wpa_param.name,
+					param->u.wpa_param.value);
+		break;
+
+	case IEEE_CMD_SET_WPA_IE:
+		ret = ieee80211_wpa_set_wpa_ie(ieee, param, p->length);
+		break;
+
+	case IEEE_CMD_SET_ENCRYPTION:
+		ret = ieee80211_wpa_set_encryption(ieee, param, p->length);
+		break;
+
+	case IEEE_CMD_MLME:
+		ret = ieee80211_wpa_mlme(ieee, param->u.mlme.command,
+				   param->u.mlme.reason_code);
+		break;
+
+	default:
+		printk("Unknown WPA supplicant request: %d\n",param->cmd);
+		ret = -EOPNOTSUPP;
+		break;
+	}
+
+	if (ret == 0 && copy_to_user(p->pointer, param, p->length))
+		ret = -EFAULT;
+
+	kfree(param);
+out:
+	up(&ieee->wx_sem);
+
+	return ret;
+}
+
+void notify_wx_assoc_event(struct ieee80211_device *ieee)
+{
+	union iwreq_data wrqu;
+	wrqu.ap_addr.sa_family = ARPHRD_ETHER;
+	if (ieee->state == IEEE80211_LINKED)
+		memcpy(wrqu.ap_addr.sa_data, ieee->current_network.bssid, ETH_ALEN);
+	else
+		memset(wrqu.ap_addr.sa_data, 0, ETH_ALEN);
+	wireless_send_event(ieee->dev, SIOCGIWAP, &wrqu, NULL);
+}
+
+
+#if 0
+EXPORT_SYMBOL(ieee80211_get_beacon);
+EXPORT_SYMBOL(ieee80211_wake_queue);
+EXPORT_SYMBOL(ieee80211_stop_queue);
+EXPORT_SYMBOL(ieee80211_reset_queue);
+EXPORT_SYMBOL(ieee80211_softmac_stop_protocol);
+EXPORT_SYMBOL(ieee80211_softmac_start_protocol);
+EXPORT_SYMBOL(ieee80211_is_shortslot);
+EXPORT_SYMBOL(ieee80211_is_54g);
+EXPORT_SYMBOL(ieee80211_wpa_supplicant_ioctl);
+EXPORT_SYMBOL(ieee80211_ps_tx_ack);
+EXPORT_SYMBOL(ieee80211_start_protocol);
+EXPORT_SYMBOL(ieee80211_stop_protocol);
+EXPORT_SYMBOL(notify_wx_assoc_event);
+EXPORT_SYMBOL(ieee80211_stop_send_beacons);
+EXPORT_SYMBOL(SendDisassociation);
+EXPORT_SYMBOL(ieee80211_disassociate);
+EXPORT_SYMBOL(ieee80211_start_scan);
+EXPORT_SYMBOL(ieee80211_softmac_ips_scan_syncro);
+#ifdef _RTL8187_EXT_PATCH_
+EXPORT_SYMBOL(ieee80211_ext_issue_assoc_req);
+EXPORT_SYMBOL(ieee80211_ext_issue_disassoc);
+EXPORT_SYMBOL(ieee80211_ext_issue_assoc_rsp);
+EXPORT_SYMBOL(softmac_mgmt_xmit);
+EXPORT_SYMBOL(ieee80211_ext_probe_resp_by_net);
+EXPORT_SYMBOL(ieee80211_start_scan);
+EXPORT_SYMBOL(ieee80211_stop_scan);
+EXPORT_SYMBOL(ieee80211_ext_send_11s_beacon);
+EXPORT_SYMBOL(ieee80211_rx_auth_rq);
+EXPORT_SYMBOL(ieee80211_associate_step1);
+#endif // _RTL8187_EXT_PATCH_
+EXPORT_SYMBOL(ieee80211_sta_ps_send_null_frame);
+#endif
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211_softmac_wx.c b/drivers/staging/rtl8187se/ieee80211/ieee80211_softmac_wx.c
new file mode 100644
index 00000000000..43b8aecee9d
--- /dev/null
+++ b/drivers/staging/rtl8187se/ieee80211/ieee80211_softmac_wx.c
@@ -0,0 +1,602 @@
+/* IEEE 802.11 SoftMAC layer
+ * Copyright (c) 2005 Andrea Merello <andreamrl@tiscali.it>
+ *
+ * Mostly extracted from the rtl8180-sa2400 driver for the
+ * in-kernel generic ieee802.11 stack.
+ *
+ * Some pieces of code might be stolen from ipw2100 driver
+ * copyright of who own it's copyright ;-)
+ *
+ * PS wx handler mostly stolen from hostap, copyright who
+ * own it's copyright ;-)
+ *
+ * released under the GPL
+ */
+
+
+#include "ieee80211.h"
+
+/* FIXME: add A freqs */
+
+const long ieee80211_wlan_frequencies[] = {
+	2412, 2417, 2422, 2427,
+	2432, 2437, 2442, 2447,
+	2452, 2457, 2462, 2467,
+	2472, 2484
+};
+
+
+int ieee80211_wx_set_freq(struct ieee80211_device *ieee, struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b)
+{
+	int ret;
+	struct iw_freq *fwrq = & wrqu->freq;
+//	printk("in %s\n",__FUNCTION__);
+	down(&ieee->wx_sem);
+
+	if(ieee->iw_mode == IW_MODE_INFRA){
+		ret = -EOPNOTSUPP;
+		goto out;
+	}
+
+	/* if setting by freq convert to channel */
+	if (fwrq->e == 1) {
+		if ((fwrq->m >= (int) 2.412e8 &&
+		     fwrq->m <= (int) 2.487e8)) {
+			int f = fwrq->m / 100000;
+			int c = 0;
+
+			while ((c < 14) && (f != ieee80211_wlan_frequencies[c]))
+				c++;
+
+			/* hack to fall through */
+			fwrq->e = 0;
+			fwrq->m = c + 1;
+		}
+	}
+
+	if (fwrq->e > 0 || fwrq->m > 14 || fwrq->m < 1 ){
+		ret = -EOPNOTSUPP;
+		goto out;
+
+	}else { /* Set the channel */
+
+
+		ieee->current_network.channel = fwrq->m;
+		ieee->set_chan(ieee->dev, ieee->current_network.channel);
+
+		if(ieee->iw_mode == IW_MODE_ADHOC || ieee->iw_mode == IW_MODE_MASTER)
+			if(ieee->state == IEEE80211_LINKED){
+
+			ieee80211_stop_send_beacons(ieee);
+			ieee80211_start_send_beacons(ieee);
+			}
+	}
+
+	ret = 0;
+out:
+	up(&ieee->wx_sem);
+	return ret;
+}
+
+
+int ieee80211_wx_get_freq(struct ieee80211_device *ieee,
+			     struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b)
+{
+	struct iw_freq *fwrq = & wrqu->freq;
+
+	if (ieee->current_network.channel == 0)
+		return -1;
+
+	fwrq->m = ieee->current_network.channel;
+	fwrq->e = 0;
+
+	return 0;
+}
+
+int ieee80211_wx_get_wap(struct ieee80211_device *ieee,
+			    struct iw_request_info *info,
+			    union iwreq_data *wrqu, char *extra)
+{
+	unsigned long flags;
+
+	wrqu->ap_addr.sa_family = ARPHRD_ETHER;
+
+	if (ieee->iw_mode == IW_MODE_MONITOR)
+		return -1;
+
+	/* We want avoid to give to the user inconsistent infos*/
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	if (ieee->state != IEEE80211_LINKED &&
+		ieee->state != IEEE80211_LINKED_SCANNING &&
+		ieee->wap_set == 0)
+
+		memset(wrqu->ap_addr.sa_data, 0, ETH_ALEN);
+	else
+		memcpy(wrqu->ap_addr.sa_data,
+		       ieee->current_network.bssid, ETH_ALEN);
+
+	spin_unlock_irqrestore(&ieee->lock, flags);
+
+	return 0;
+}
+
+
+int ieee80211_wx_set_wap(struct ieee80211_device *ieee,
+			 struct iw_request_info *info,
+			 union iwreq_data *awrq,
+			 char *extra)
+{
+
+	int ret = 0;
+	u8 zero[] = {0,0,0,0,0,0};
+	unsigned long flags;
+
+	short ifup = ieee->proto_started;//dev->flags & IFF_UP;
+	struct sockaddr *temp = (struct sockaddr *)awrq;
+
+	//printk("=======Set WAP:");
+	ieee->sync_scan_hurryup = 1;
+
+	down(&ieee->wx_sem);
+	/* use ifconfig hw ether */
+	if (ieee->iw_mode == IW_MODE_MASTER){
+		ret = -1;
+		goto out;
+	}
+
+	if (temp->sa_family != ARPHRD_ETHER){
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (ifup)
+		ieee80211_stop_protocol(ieee);
+
+	/* just to avoid to give inconsistent infos in the
+	 * get wx method. not really needed otherwise
+	 */
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	memcpy(ieee->current_network.bssid, temp->sa_data, ETH_ALEN);
+	ieee->wap_set = memcmp(temp->sa_data, zero,ETH_ALEN)!=0;
+	//printk(" %x:%x:%x:%x:%x:%x\n", ieee->current_network.bssid[0],ieee->current_network.bssid[1],ieee->current_network.bssid[2],ieee->current_network.bssid[3],ieee->current_network.bssid[4],ieee->current_network.bssid[5]);
+
+	spin_unlock_irqrestore(&ieee->lock, flags);
+
+	if (ifup)
+		ieee80211_start_protocol(ieee);
+
+out:
+	up(&ieee->wx_sem);
+	return ret;
+}
+
+ int ieee80211_wx_get_essid(struct ieee80211_device *ieee, struct iw_request_info *a,union iwreq_data *wrqu,char *b)
+{
+	int len,ret = 0;
+	unsigned long flags;
+
+	if (ieee->iw_mode == IW_MODE_MONITOR)
+		return -1;
+
+	/* We want avoid to give to the user inconsistent infos*/
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	if (ieee->current_network.ssid[0] == '\0' ||
+		ieee->current_network.ssid_len == 0){
+		ret = -1;
+		goto out;
+	}
+
+	if (ieee->state != IEEE80211_LINKED &&
+		ieee->state != IEEE80211_LINKED_SCANNING &&
+		ieee->ssid_set == 0){
+		ret = -1;
+		goto out;
+	}
+	len = ieee->current_network.ssid_len;
+	wrqu->essid.length = len;
+	strncpy(b,ieee->current_network.ssid,len);
+	wrqu->essid.flags = 1;
+
+out:
+	spin_unlock_irqrestore(&ieee->lock, flags);
+
+	return ret;
+
+}
+
+int ieee80211_wx_set_rate(struct ieee80211_device *ieee,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+
+	u32 target_rate = wrqu->bitrate.value;
+
+	//added by lizhaoming for auto mode
+	if(target_rate == -1){
+	ieee->rate = 110;
+	} else {
+	ieee->rate = target_rate/100000;
+	}
+	//FIXME: we might want to limit rate also in management protocols.
+	return 0;
+}
+
+
+
+int ieee80211_wx_get_rate(struct ieee80211_device *ieee,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+
+	wrqu->bitrate.value = ieee->rate * 100000;
+
+	return 0;
+}
+
+int ieee80211_wx_set_mode(struct ieee80211_device *ieee, struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b)
+{
+
+	ieee->sync_scan_hurryup = 1;
+
+	down(&ieee->wx_sem);
+
+	if (wrqu->mode == ieee->iw_mode)
+		goto out;
+
+	if (wrqu->mode == IW_MODE_MONITOR){
+
+		ieee->dev->type = ARPHRD_IEEE80211;
+	}else{
+		ieee->dev->type = ARPHRD_ETHER;
+	}
+
+	if (!ieee->proto_started){
+		ieee->iw_mode = wrqu->mode;
+	}else{
+		ieee80211_stop_protocol(ieee);
+		ieee->iw_mode = wrqu->mode;
+		ieee80211_start_protocol(ieee);
+	}
+
+out:
+	up(&ieee->wx_sem);
+	return 0;
+}
+
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20)
+void ieee80211_wx_sync_scan_wq(struct work_struct *work)
+{
+	struct ieee80211_device *ieee = container_of(work, struct ieee80211_device, wx_sync_scan_wq);
+#else
+void ieee80211_wx_sync_scan_wq(struct ieee80211_device *ieee)
+{
+#endif
+//void ieee80211_wx_sync_scan_wq(struct ieee80211_device *ieee)
+//{
+	short chan;
+
+	chan = ieee->current_network.channel;
+
+	netif_carrier_off(ieee->dev);
+
+	if (ieee->data_hard_stop)
+		ieee->data_hard_stop(ieee->dev);
+
+	ieee80211_stop_send_beacons(ieee);
+
+	ieee->state = IEEE80211_LINKED_SCANNING;
+	ieee->link_change(ieee->dev);
+
+	ieee80211_start_scan_syncro(ieee);
+
+	ieee->set_chan(ieee->dev, chan);
+
+	ieee->state = IEEE80211_LINKED;
+	ieee->link_change(ieee->dev);
+
+	if (ieee->data_hard_resume)
+		ieee->data_hard_resume(ieee->dev);
+
+	if(ieee->iw_mode == IW_MODE_ADHOC || ieee->iw_mode == IW_MODE_MASTER)
+		ieee80211_start_send_beacons(ieee);
+
+	netif_carrier_on(ieee->dev);
+
+	//YJ,add,080828, In prevent of lossing ping packet during scanning
+	//ieee80211_sta_ps_send_null_frame(ieee, false);
+	//YJ,add,080828,end
+
+	up(&ieee->wx_sem);
+
+}
+
+int ieee80211_wx_set_scan(struct ieee80211_device *ieee, struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b)
+{
+	int ret = 0;
+
+	down(&ieee->wx_sem);
+
+	if (ieee->iw_mode == IW_MODE_MONITOR || !(ieee->proto_started)){
+		ret = -1;
+		goto out;
+	}
+	//YJ,add,080828
+	//In prevent of lossing ping packet during scanning
+	//ieee80211_sta_ps_send_null_frame(ieee, true);
+	//YJ,add,080828,end
+
+	if ( ieee->state == IEEE80211_LINKED){
+		queue_work(ieee->wq, &ieee->wx_sync_scan_wq);
+		/* intentionally forget to up sem */
+		return 0;
+	}
+
+out:
+	up(&ieee->wx_sem);
+	return ret;
+}
+
+int ieee80211_wx_set_essid(struct ieee80211_device *ieee,
+			      struct iw_request_info *a,
+			      union iwreq_data *wrqu, char *extra)
+{
+
+	int ret=0,len;
+	short proto_started;
+	unsigned long flags;
+
+	ieee->sync_scan_hurryup = 1;
+
+	down(&ieee->wx_sem);
+
+	proto_started = ieee->proto_started;
+
+	if (wrqu->essid.length > IW_ESSID_MAX_SIZE){
+		ret= -E2BIG;
+		goto out;
+	}
+
+	if (ieee->iw_mode == IW_MODE_MONITOR){
+		ret= -1;
+		goto out;
+	}
+
+	if(proto_started)
+		ieee80211_stop_protocol(ieee);
+
+	/* this is just to be sure that the GET wx callback
+	 * has consisten infos. not needed otherwise
+	 */
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	if (wrqu->essid.flags && wrqu->essid.length) {
+//YJ,modified,080819
+#if LINUX_VERSION_CODE <  KERNEL_VERSION(2,6,20)
+		len = ((wrqu->essid.length-1) < IW_ESSID_MAX_SIZE) ? (wrqu->essid.length-1) : IW_ESSID_MAX_SIZE;
+#else
+		len = (wrqu->essid.length < IW_ESSID_MAX_SIZE) ? (wrqu->essid.length) : IW_ESSID_MAX_SIZE;
+#endif
+		memset(ieee->current_network.ssid, 0, ieee->current_network.ssid_len); //YJ,add,080819
+		strncpy(ieee->current_network.ssid, extra, len);
+		ieee->current_network.ssid_len = len;
+		ieee->ssid_set = 1;
+//YJ,modified,080819,end
+
+		//YJ,add,080819,for hidden ap
+		if(len == 0){
+			memset(ieee->current_network.bssid, 0, ETH_ALEN);
+			ieee->current_network.capability = 0;
+		}
+		//YJ,add,080819,for hidden ap,end
+	}
+	else{
+		ieee->ssid_set = 0;
+		ieee->current_network.ssid[0] = '\0';
+		ieee->current_network.ssid_len = 0;
+	}
+	//printk("==========set essid %s!\n",ieee->current_network.ssid);
+	spin_unlock_irqrestore(&ieee->lock, flags);
+
+	if (proto_started)
+		ieee80211_start_protocol(ieee);
+out:
+	up(&ieee->wx_sem);
+	return ret;
+}
+
+ int ieee80211_wx_get_mode(struct ieee80211_device *ieee, struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b)
+{
+
+	wrqu->mode = ieee->iw_mode;
+	return 0;
+}
+
+ int ieee80211_wx_set_rawtx(struct ieee80211_device *ieee,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+
+	int *parms = (int *)extra;
+	int enable = (parms[0] > 0);
+	short prev = ieee->raw_tx;
+
+	down(&ieee->wx_sem);
+
+	if(enable)
+		ieee->raw_tx = 1;
+	else
+		ieee->raw_tx = 0;
+
+	printk(KERN_INFO"raw TX is %s\n",
+	      ieee->raw_tx ? "enabled" : "disabled");
+
+	if(ieee->iw_mode == IW_MODE_MONITOR)
+	{
+		if(prev == 0 && ieee->raw_tx){
+			if (ieee->data_hard_resume)
+				ieee->data_hard_resume(ieee->dev);
+
+			netif_carrier_on(ieee->dev);
+		}
+
+		if(prev && ieee->raw_tx == 1)
+			netif_carrier_off(ieee->dev);
+	}
+
+	up(&ieee->wx_sem);
+
+	return 0;
+}
+
+int ieee80211_wx_get_name(struct ieee80211_device *ieee,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	strcpy(wrqu->name, "802.11");
+	if(ieee->modulation & IEEE80211_CCK_MODULATION){
+		strcat(wrqu->name, "b");
+		if(ieee->modulation & IEEE80211_OFDM_MODULATION)
+			strcat(wrqu->name, "/g");
+	}else if(ieee->modulation & IEEE80211_OFDM_MODULATION)
+		strcat(wrqu->name, "g");
+
+	if((ieee->state == IEEE80211_LINKED) ||
+		(ieee->state == IEEE80211_LINKED_SCANNING))
+		strcat(wrqu->name," linked");
+	else if(ieee->state != IEEE80211_NOLINK)
+		strcat(wrqu->name," link..");
+
+
+	return 0;
+}
+
+
+/* this is mostly stolen from hostap */
+int ieee80211_wx_set_power(struct ieee80211_device *ieee,
+				 struct iw_request_info *info,
+				 union iwreq_data *wrqu, char *extra)
+{
+	int ret = 0;
+
+	if(
+		(!ieee->sta_wake_up) ||
+		(!ieee->ps_request_tx_ack) ||
+		(!ieee->enter_sleep_state) ||
+		(!ieee->ps_is_queue_empty)){
+
+		printk("ERROR. PS mode is tryied to be use but\
+driver missed a callback\n\n");
+
+		return -1;
+	}
+
+	down(&ieee->wx_sem);
+
+	if (wrqu->power.disabled){
+		ieee->ps = IEEE80211_PS_DISABLED;
+
+		goto exit;
+	}
+	switch (wrqu->power.flags & IW_POWER_MODE) {
+	case IW_POWER_UNICAST_R:
+		ieee->ps = IEEE80211_PS_UNICAST;
+
+		break;
+	case IW_POWER_ALL_R:
+		ieee->ps = IEEE80211_PS_UNICAST | IEEE80211_PS_MBCAST;
+		break;
+
+	case IW_POWER_ON:
+		ieee->ps = IEEE80211_PS_DISABLED;
+		break;
+
+	default:
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	if (wrqu->power.flags & IW_POWER_TIMEOUT) {
+
+		ieee->ps_timeout = wrqu->power.value / 1000;
+		printk("Timeout %d\n",ieee->ps_timeout);
+	}
+
+	if (wrqu->power.flags & IW_POWER_PERIOD) {
+
+		ret = -EOPNOTSUPP;
+		goto exit;
+		//wrq->value / 1024;
+
+	}
+exit:
+	up(&ieee->wx_sem);
+	return ret;
+
+}
+
+/* this is stolen from hostap */
+int ieee80211_wx_get_power(struct ieee80211_device *ieee,
+				 struct iw_request_info *info,
+				 union iwreq_data *wrqu, char *extra)
+{
+	int ret =0;
+
+	down(&ieee->wx_sem);
+
+	if(ieee->ps == IEEE80211_PS_DISABLED){
+		wrqu->power.disabled = 1;
+		goto exit;
+	}
+
+	wrqu->power.disabled = 0;
+
+//	if ((wrqu->power.flags & IW_POWER_TYPE) == IW_POWER_TIMEOUT) {
+		wrqu->power.flags = IW_POWER_TIMEOUT;
+		wrqu->power.value = ieee->ps_timeout * 1000;
+//	} else {
+//		ret = -EOPNOTSUPP;
+//		goto exit;
+		//wrqu->power.flags = IW_POWER_PERIOD;
+		//wrqu->power.value = ieee->current_network.dtim_period *
+		//	ieee->current_network.beacon_interval * 1024;
+//	}
+
+
+	if (ieee->ps & IEEE80211_PS_MBCAST)
+		wrqu->power.flags |= IW_POWER_ALL_R;
+	else
+		wrqu->power.flags |= IW_POWER_UNICAST_R;
+
+exit:
+	up(&ieee->wx_sem);
+	return ret;
+
+}
+
+#if 0
+EXPORT_SYMBOL(ieee80211_wx_get_essid);
+EXPORT_SYMBOL(ieee80211_wx_set_essid);
+EXPORT_SYMBOL(ieee80211_wx_set_rate);
+EXPORT_SYMBOL(ieee80211_wx_get_rate);
+EXPORT_SYMBOL(ieee80211_wx_set_wap);
+EXPORT_SYMBOL(ieee80211_wx_get_wap);
+EXPORT_SYMBOL(ieee80211_wx_set_mode);
+EXPORT_SYMBOL(ieee80211_wx_get_mode);
+EXPORT_SYMBOL(ieee80211_wx_set_scan);
+EXPORT_SYMBOL(ieee80211_wx_get_freq);
+EXPORT_SYMBOL(ieee80211_wx_set_freq);
+EXPORT_SYMBOL(ieee80211_wx_set_rawtx);
+EXPORT_SYMBOL(ieee80211_wx_get_name);
+EXPORT_SYMBOL(ieee80211_wx_set_power);
+EXPORT_SYMBOL(ieee80211_wx_get_power);
+EXPORT_SYMBOL(ieee80211_wlan_frequencies);
+#endif
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211_tx.c b/drivers/staging/rtl8187se/ieee80211/ieee80211_tx.c
new file mode 100644
index 00000000000..33a0687252a
--- /dev/null
+++ b/drivers/staging/rtl8187se/ieee80211/ieee80211_tx.c
@@ -0,0 +1,828 @@
+/******************************************************************************
+
+  Copyright(c) 2003 - 2004 Intel Corporation. All rights reserved.
+
+  This program is free software; you can redistribute it and/or modify it
+  under the terms of version 2 of the GNU General Public License as
+  published by the Free Software Foundation.
+
+  This program is distributed in the hope that it will be useful, but WITHOUT
+  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+  more details.
+
+  You should have received a copy of the GNU General Public License along with
+  this program; if not, write to the Free Software Foundation, Inc., 59
+  Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+
+  The full GNU General Public License is included in this distribution in the
+  file called LICENSE.
+
+  Contact Information:
+  James P. Ketrenos <ipw2100-admin@linux.intel.com>
+  Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+
+******************************************************************************
+
+  Few modifications for Realtek's Wi-Fi drivers by
+  Andrea Merello <andreamrl@tiscali.it>
+
+  A special thanks goes to Realtek for their support !
+
+******************************************************************************/
+
+#include <linux/compiler.h>
+//#include <linux/config.h>
+#include <linux/errno.h>
+#include <linux/if_arp.h>
+#include <linux/in6.h>
+#include <linux/in.h>
+#include <linux/ip.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/netdevice.h>
+#include <linux/pci.h>
+#include <linux/proc_fs.h>
+#include <linux/skbuff.h>
+#include <linux/slab.h>
+#include <linux/tcp.h>
+#include <linux/types.h>
+#include <linux/version.h>
+#include <linux/wireless.h>
+#include <linux/etherdevice.h>
+#include <asm/uaccess.h>
+#include <linux/if_vlan.h>
+
+#include "ieee80211.h"
+
+
+/*
+
+
+802.11 Data Frame
+
+
+802.11 frame_contorl for data frames - 2 bytes
+     ,-----------------------------------------------------------------------------------------.
+bits | 0  |  1  |  2  |  3  |  4  |  5  |  6  |  7  |  8  |  9  |  a  |  b  |  c  |  d  |  e   |
+     |----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|------|
+val  | 0  |  0  |  0  |  1  |  x  |  0  |  0  |  0  |  1  |  0  |  x  |  x  |  x  |  x  |  x   |
+     |----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|-----|------|
+desc | ^-ver-^  |  ^type-^  |  ^-----subtype-----^  | to  |from |more |retry| pwr |more |wep   |
+     |          |           | x=0 data,x=1 data+ack | DS  | DS  |frag |     | mgm |data |      |
+     '-----------------------------------------------------------------------------------------'
+		                                    /\
+                                                    |
+802.11 Data Frame                                   |
+           ,--------- 'ctrl' expands to >-----------'
+          |
+      ,--'---,-------------------------------------------------------------.
+Bytes |  2   |  2   |    6    |    6    |    6    |  2   | 0..2312 |   4  |
+      |------|------|---------|---------|---------|------|---------|------|
+Desc. | ctrl | dura |  DA/RA  |   TA    |    SA   | Sequ |  Frame  |  fcs |
+      |      | tion | (BSSID) |         |         | ence |  data   |      |
+      `--------------------------------------------------|         |------'
+Total: 28 non-data bytes                                 `----.----'
+                                                              |
+       .- 'Frame data' expands to <---------------------------'
+       |
+       V
+      ,---------------------------------------------------.
+Bytes |  1   |  1   |    1    |    3     |  2   |  0-2304 |
+      |------|------|---------|----------|------|---------|
+Desc. | SNAP | SNAP | Control |Eth Tunnel| Type | IP      |
+      | DSAP | SSAP |         |          |      | Packet  |
+      | 0xAA | 0xAA |0x03 (UI)|0x00-00-F8|      |         |
+      `-----------------------------------------|         |
+Total: 8 non-data bytes                         `----.----'
+                                                     |
+       .- 'IP Packet' expands, if WEP enabled, to <--'
+       |
+       V
+      ,-----------------------.
+Bytes |  4  |   0-2296  |  4  |
+      |-----|-----------|-----|
+Desc. | IV  | Encrypted | ICV |
+      |     | IP Packet |     |
+      `-----------------------'
+Total: 8 non-data bytes
+
+
+802.3 Ethernet Data Frame
+
+      ,-----------------------------------------.
+Bytes |   6   |   6   |  2   |  Variable |   4  |
+      |-------|-------|------|-----------|------|
+Desc. | Dest. | Source| Type | IP Packet |  fcs |
+      |  MAC  |  MAC  |      |           |      |
+      `-----------------------------------------'
+Total: 18 non-data bytes
+
+In the event that fragmentation is required, the incoming payload is split into
+N parts of size ieee->fts.  The first fragment contains the SNAP header and the
+remaining packets are just data.
+
+If encryption is enabled, each fragment payload size is reduced by enough space
+to add the prefix and postfix (IV and ICV totalling 8 bytes in the case of WEP)
+So if you have 1500 bytes of payload with ieee->fts set to 500 without
+encryption it will take 3 frames.  With WEP it will take 4 frames as the
+payload of each frame is reduced to 492 bytes.
+
+* SKB visualization
+*
+*  ,- skb->data
+* |
+* |    ETHERNET HEADER        ,-<-- PAYLOAD
+* |                           |     14 bytes from skb->data
+* |  2 bytes for Type --> ,T. |     (sizeof ethhdr)
+* |                       | | |
+* |,-Dest.--. ,--Src.---. | | |
+* |  6 bytes| | 6 bytes | | | |
+* v         | |         | | | |
+* 0         | v       1 | v | v           2
+* 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5
+*     ^     | ^         | ^ |
+*     |     | |         | | |
+*     |     | |         | `T' <---- 2 bytes for Type
+*     |     | |         |
+*     |     | '---SNAP--' <-------- 6 bytes for SNAP
+*     |     |
+*     `-IV--' <-------------------- 4 bytes for IV (WEP)
+*
+*      SNAP HEADER
+*
+*/
+
+static u8 P802_1H_OUI[P80211_OUI_LEN] = { 0x00, 0x00, 0xf8 };
+static u8 RFC1042_OUI[P80211_OUI_LEN] = { 0x00, 0x00, 0x00 };
+
+static inline int ieee80211_put_snap(u8 *data, u16 h_proto)
+{
+	struct ieee80211_snap_hdr *snap;
+	u8 *oui;
+
+	snap = (struct ieee80211_snap_hdr *)data;
+	snap->dsap = 0xaa;
+	snap->ssap = 0xaa;
+	snap->ctrl = 0x03;
+
+	if (h_proto == 0x8137 || h_proto == 0x80f3)
+		oui = P802_1H_OUI;
+	else
+		oui = RFC1042_OUI;
+	snap->oui[0] = oui[0];
+	snap->oui[1] = oui[1];
+	snap->oui[2] = oui[2];
+
+	*(u16 *)(data + SNAP_SIZE) = htons(h_proto);
+
+	return SNAP_SIZE + sizeof(u16);
+}
+
+int ieee80211_encrypt_fragment(
+	struct ieee80211_device *ieee,
+	struct sk_buff *frag,
+	int hdr_len)
+{
+	struct ieee80211_crypt_data* crypt = ieee->crypt[ieee->tx_keyidx];
+	int res;
+
+ /*added to care about null crypt condition, to solve that system hangs when shared keys error*/
+        if (!crypt || !crypt->ops)
+        return -1;
+
+#ifdef CONFIG_IEEE80211_CRYPT_TKIP
+	struct ieee80211_hdr *header;
+
+	if (ieee->tkip_countermeasures &&
+	    crypt && crypt->ops && strcmp(crypt->ops->name, "TKIP") == 0) {
+		header = (struct ieee80211_hdr *) frag->data;
+		if (net_ratelimit()) {
+			printk(KERN_DEBUG "%s: TKIP countermeasures: dropped "
+			       "TX packet to " MAC_FMT "\n",
+			       ieee->dev->name, MAC_ARG(header->addr1));
+		}
+		return -1;
+	}
+#endif
+	/* To encrypt, frame format is:
+	 * IV (4 bytes), clear payload (including SNAP), ICV (4 bytes) */
+
+	// PR: FIXME: Copied from hostap. Check fragmentation/MSDU/MPDU encryption.
+	/* Host-based IEEE 802.11 fragmentation for TX is not yet supported, so
+	 * call both MSDU and MPDU encryption functions from here. */
+	atomic_inc(&crypt->refcnt);
+	res = 0;
+	if (crypt->ops->encrypt_msdu)
+		res = crypt->ops->encrypt_msdu(frag, hdr_len, crypt->priv);
+	if (res == 0 && crypt->ops->encrypt_mpdu)
+		res = crypt->ops->encrypt_mpdu(frag, hdr_len, crypt->priv);
+
+	atomic_dec(&crypt->refcnt);
+	if (res < 0) {
+		printk(KERN_INFO "%s: Encryption failed: len=%d.\n",
+		       ieee->dev->name, frag->len);
+		ieee->ieee_stats.tx_discards++;
+		return -1;
+	}
+
+	return 0;
+}
+
+
+void ieee80211_txb_free(struct ieee80211_txb *txb) {
+	int i;
+	if (unlikely(!txb))
+		return;
+	for (i = 0; i < txb->nr_frags; i++)
+		if (txb->fragments[i])
+			dev_kfree_skb_any(txb->fragments[i]);
+	kfree(txb);
+}
+
+struct ieee80211_txb *ieee80211_alloc_txb(int nr_frags, int txb_size,
+					  int gfp_mask)
+{
+	struct ieee80211_txb *txb;
+	int i;
+	txb = kmalloc(
+		sizeof(struct ieee80211_txb) + (sizeof(u8*) * nr_frags),
+		gfp_mask);
+	if (!txb)
+		return NULL;
+
+	memset(txb, 0, sizeof(struct ieee80211_txb));
+	txb->nr_frags = nr_frags;
+	txb->frag_size = txb_size;
+
+	for (i = 0; i < nr_frags; i++) {
+		txb->fragments[i] = dev_alloc_skb(txb_size);
+		if (unlikely(!txb->fragments[i])) {
+			i--;
+			break;
+		}
+	}
+	if (unlikely(i != nr_frags)) {
+		while (i >= 0)
+			dev_kfree_skb_any(txb->fragments[i--]);
+		kfree(txb);
+		return NULL;
+	}
+	return txb;
+}
+
+// Classify the to-be send data packet
+// Need to acquire the sent queue index.
+static int
+ieee80211_classify(struct sk_buff *skb, struct ieee80211_network *network)
+{
+  struct ether_header *eh = (struct ether_header*)skb->data;
+  unsigned int wme_UP = 0;
+
+  if(!network->QoS_Enable) {
+     skb->priority = 0;
+     return(wme_UP);
+  }
+
+  if(eh->ether_type == __constant_htons(ETHERTYPE_IP)) {
+    const struct iphdr *ih = (struct iphdr*)(skb->data + \
+		    sizeof(struct ether_header));
+    wme_UP = (ih->tos >> 5)&0x07;
+  } else if (vlan_tx_tag_present(skb)) {//vtag packet
+#ifndef VLAN_PRI_SHIFT
+#define VLAN_PRI_SHIFT  13              /* Shift to find VLAN user priority */
+#define VLAN_PRI_MASK   7               /* Mask for user priority bits in VLAN */
+#endif
+	u32 tag = vlan_tx_tag_get(skb);
+	wme_UP = (tag >> VLAN_PRI_SHIFT) & VLAN_PRI_MASK;
+  } else if(ETH_P_PAE ==  ntohs(((struct ethhdr *)skb->data)->h_proto)) {
+    //printk(KERN_WARNING "type = normal packet\n");
+    wme_UP = 7;
+  }
+
+  skb->priority = wme_UP;
+  return(wme_UP);
+}
+
+#ifdef _RTL8187_EXT_PATCH_
+// based on part of ieee80211_xmit. Mainly allocate txb. ieee->lock is held
+struct ieee80211_txb *ieee80211_ext_alloc_txb(struct sk_buff *skb, struct net_device *dev, struct ieee80211_hdr_3addr *header, int hdr_len, u8 isQoS, u16 *pQOS_ctl, int isEncrypt, struct ieee80211_crypt_data* crypt)
+{
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0))
+	struct ieee80211_device *ieee = netdev_priv(dev);
+#else
+	struct ieee80211_device *ieee = (struct ieee80211_device *)dev->priv;
+#endif
+	struct ieee80211_txb *txb = NULL;
+	struct ieee80211_hdr_3addr *frag_hdr;
+	int i, bytes_per_frag, nr_frags, bytes_last_frag, frag_size;
+	int ether_type;
+	int bytes, QOS_ctl;
+	struct sk_buff *skb_frag;
+
+	ether_type = ntohs(((struct ethhdr *)skb->data)->h_proto);
+
+	/* Advance the SKB to the start of the payload */
+	skb_pull(skb, sizeof(struct ethhdr));
+
+	/* Determine total amount of storage required for TXB packets */
+	bytes = skb->len + SNAP_SIZE + sizeof(u16);
+
+	/* Determine fragmentation size based on destination (multicast
+	 * and broadcast are not fragmented) */
+	// if (is_multicast_ether_addr(dest) ||
+	// is_broadcast_ether_addr(dest)) {
+	if (is_multicast_ether_addr(header->addr1) ||
+			is_broadcast_ether_addr(header->addr1)) {
+		frag_size = MAX_FRAG_THRESHOLD;
+		QOS_ctl = QOS_CTL_NOTCONTAIN_ACK;
+	}
+	else {
+		//printk(KERN_WARNING "&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&frag_size = %d\n", frag_size);
+		frag_size = ieee->fts;//default:392
+		QOS_ctl = 0;
+	}
+
+	if(isQoS) {
+		QOS_ctl |= skb->priority; //set in the ieee80211_classify
+		*pQOS_ctl = cpu_to_le16(QOS_ctl);
+	}
+	//printk(KERN_WARNING "header size = %d, QOS_ctl = %x\n", hdr_len,QOS_ctl);
+	/* Determine amount of payload per fragment.  Regardless of if
+	 * this stack is providing the full 802.11 header, one will
+	 * eventually be affixed to this fragment -- so we must account for
+	 * it when determining the amount of payload space. */
+	//bytes_per_frag = frag_size - (IEEE80211_3ADDR_LEN + (ieee->current_network->QoS_Enable ? 2:0));
+	bytes_per_frag = frag_size - hdr_len;
+	if (ieee->config &
+			(CFG_IEEE80211_COMPUTE_FCS | CFG_IEEE80211_RESERVE_FCS))
+		bytes_per_frag -= IEEE80211_FCS_LEN;
+
+	/* Each fragment may need to have room for encryptiong pre/postfix */
+	if (isEncrypt)
+		bytes_per_frag -= crypt->ops->extra_prefix_len +
+			crypt->ops->extra_postfix_len;
+
+	/* Number of fragments is the total bytes_per_frag /
+	 * payload_per_fragment */
+	nr_frags = bytes / bytes_per_frag;
+	bytes_last_frag = bytes % bytes_per_frag;
+	if (bytes_last_frag)
+		nr_frags++;
+	else
+		bytes_last_frag = bytes_per_frag;
+
+	/* When we allocate the TXB we allocate enough space for the reserve
+	 * and full fragment bytes (bytes_per_frag doesn't include prefix,
+	 * postfix, header, FCS, etc.) */
+	txb = ieee80211_alloc_txb(nr_frags, frag_size, GFP_ATOMIC);
+	if (unlikely(!txb)) {
+		printk(KERN_WARNING "%s: Could not allocate TXB\n",
+			ieee->dev->name);
+		return NULL;
+	}
+	txb->encrypted = isEncrypt;
+	txb->payload_size = bytes;
+
+	for (i = 0; i < nr_frags; i++) {
+		skb_frag = txb->fragments[i];
+		skb_frag->priority = UP2AC(skb->priority);
+		if (isEncrypt)
+			skb_reserve(skb_frag, crypt->ops->extra_prefix_len);
+
+		frag_hdr = (struct ieee80211_hdr_3addr *)skb_put(skb_frag, hdr_len);
+		memcpy(frag_hdr, (void *)header, hdr_len);
+
+		/* If this is not the last fragment, then add the MOREFRAGS
+		 * bit to the frame control */
+		if (i != nr_frags - 1) {
+			frag_hdr->frame_ctl = cpu_to_le16(
+					header->frame_ctl | IEEE80211_FCTL_MOREFRAGS);
+			bytes = bytes_per_frag;
+
+		} else {
+			/* The last fragment takes the remaining length */
+			bytes = bytes_last_frag;
+		}
+
+		frag_hdr->seq_ctl = cpu_to_le16(ieee->seq_ctrl[0]<<4 | i);
+		//frag_hdr->seq_ctl = cpu_to_le16(ieee->seq_ctrl<<4 | i);
+		//
+
+		/* Put a SNAP header on the first fragment */
+		if (i == 0) {
+			ieee80211_put_snap(
+				skb_put(skb_frag, SNAP_SIZE + sizeof(u16)), ether_type);
+			bytes -= SNAP_SIZE + sizeof(u16);
+		}
+
+		memcpy(skb_put(skb_frag, bytes), skb->data, bytes);
+
+		/* Advance the SKB... */
+		skb_pull(skb, bytes);
+
+		/* Encryption routine will move the header forward in order
+		 * to insert the IV between the header and the payload */
+		if (isEncrypt)
+			ieee80211_encrypt_fragment(ieee, skb_frag, hdr_len);
+		if (ieee->config &
+				(CFG_IEEE80211_COMPUTE_FCS | CFG_IEEE80211_RESERVE_FCS))
+			skb_put(skb_frag, 4);
+	}
+	// Advance sequence number in data frame.
+	//printk(KERN_WARNING "QoS Enalbed? %s\n", ieee->current_network.QoS_Enable?"Y":"N");
+	if (ieee->seq_ctrl[0] == 0xFFF)
+		ieee->seq_ctrl[0] = 0;
+	else
+		ieee->seq_ctrl[0]++;
+	// stanley, just for debug
+/*
+{
+	int j=0;
+	for(j=0;j<nr_frags;j++)
+	{
+			int i;
+		struct sk_buff *skb = txb->fragments[j];
+			printk("send(%d): ", j);
+			for (i=0;i<skb->len;i++)
+				printk("%02X ", skb->data[i]&0xff);
+			printk("\n");
+	}
+}
+*/
+
+	return txb;
+}
+
+
+// based on part of ieee80211_xmit. Mainly allocate txb. ieee->lock is held
+// Assume no encryption, no FCS computing
+struct ieee80211_txb *ieee80211_ext_reuse_txb(struct sk_buff *skb, struct net_device *dev, struct ieee80211_hdr_3addr *header, int hdr_len, u8 isQoS, u16 *pQOS_ctl, int isEncrypt, struct ieee80211_crypt_data* crypt)
+{
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0))
+	struct ieee80211_device *ieee = netdev_priv(dev);
+#else
+	struct ieee80211_device *ieee = (struct ieee80211_device *)dev->priv;
+#endif
+	struct ieee80211_txb *txb = NULL;
+	struct ieee80211_hdr_3addr *frag_hdr;
+	int ether_type;
+	int bytes, QOS_ctl;
+
+	ether_type = ntohs(((struct ethhdr *)skb->data)->h_proto);
+
+	/* Advance the SKB to the start of the payload */
+	skb_pull(skb, sizeof(struct ethhdr));
+
+	/* Determine total amount of storage required for TXB packets */
+	bytes = skb->len + SNAP_SIZE + sizeof(u16);
+
+	if (is_multicast_ether_addr(header->addr1) ||
+			is_broadcast_ether_addr(header->addr1)) {
+		QOS_ctl = QOS_CTL_NOTCONTAIN_ACK;
+	}
+	else {
+		QOS_ctl = 0;
+	}
+
+	if(isQoS) {
+		QOS_ctl |= skb->priority; //set in the ieee80211_classify
+		*pQOS_ctl = cpu_to_le16(QOS_ctl);
+	}
+
+	txb = kmalloc( sizeof(struct ieee80211_txb) + sizeof(u8*), GFP_ATOMIC );
+	if (unlikely(!txb)) {
+		printk(KERN_WARNING "%s: Could not allocate TXB\n",
+			ieee->dev->name);
+		return NULL;
+	}
+
+	txb->nr_frags = 1;
+	txb->frag_size = bytes;
+	txb->encrypted = isEncrypt;
+	txb->payload_size = bytes;
+
+	txb->fragments[0] = skb;
+	ieee80211_put_snap(
+			skb_push(skb, SNAP_SIZE + sizeof(u16)), ether_type);
+	frag_hdr = (struct ieee80211_hdr_3addr *)skb_push(skb, hdr_len);
+	memcpy(frag_hdr, (void *)header, hdr_len);
+	frag_hdr->seq_ctl = cpu_to_le16(ieee->seq_ctrl[0]<<4 | 0);
+	skb->priority = UP2AC(skb->priority);
+
+	// Advance sequence number in data frame.
+	//printk(KERN_WARNING "QoS Enalbed? %s\n", ieee->current_network.QoS_Enable?"Y":"N");
+	if (ieee->seq_ctrl[0] == 0xFFF)
+		ieee->seq_ctrl[0] = 0;
+	else
+		ieee->seq_ctrl[0]++;
+
+	return txb;
+}
+
+#endif // _RTL8187_EXT_PATCH_
+
+/* SKBs are added to the ieee->tx_queue. */
+int ieee80211_xmit(struct sk_buff *skb,
+		   struct net_device *dev)
+{
+	struct ieee80211_device *ieee = netdev_priv(dev);
+	struct ieee80211_txb *txb = NULL;
+	struct ieee80211_hdr_3addr_QOS *frag_hdr;
+	int i, bytes_per_frag, nr_frags, bytes_last_frag, frag_size;
+	unsigned long flags;
+	struct net_device_stats *stats = &ieee->stats;
+	int ether_type, encrypt;
+	int bytes, fc, QOS_ctl, hdr_len;
+	struct sk_buff *skb_frag;
+	//struct ieee80211_hdr header = { /* Ensure zero initialized */
+	//	.duration_id = 0,
+	//	.seq_ctl = 0
+	//};
+	struct ieee80211_hdr_3addr_QOS header = { /* Ensure zero initialized */
+		.duration_id = 0,
+		.seq_ctl = 0,
+		.QOS_ctl = 0
+	};
+	u8 dest[ETH_ALEN], src[ETH_ALEN];
+
+	struct ieee80211_crypt_data* crypt;
+
+	//printk(KERN_WARNING "upper layer packet!\n");
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	/* If there is no driver handler to take the TXB, dont' bother
+	 * creating it... */
+	if ((!ieee->hard_start_xmit && !(ieee->softmac_features & IEEE_SOFTMAC_TX_QUEUE))||
+	   ((!ieee->softmac_data_hard_start_xmit && (ieee->softmac_features & IEEE_SOFTMAC_TX_QUEUE)))) {
+		printk(KERN_WARNING "%s: No xmit handler.\n",
+		       ieee->dev->name);
+		goto success;
+	}
+
+	ieee80211_classify(skb,&ieee->current_network);
+	if(likely(ieee->raw_tx == 0)){
+
+		if (unlikely(skb->len < SNAP_SIZE + sizeof(u16))) {
+			printk(KERN_WARNING "%s: skb too small (%d).\n",
+			ieee->dev->name, skb->len);
+			goto success;
+		}
+
+
+#ifdef _RTL8187_EXT_PATCH_
+		// note, skb->priority which was set by ieee80211_classify, and used by physical tx
+		if((ieee->iw_mode == ieee->iw_ext_mode) && (ieee->ext_patch_ieee80211_xmit))
+		{
+			txb = ieee->ext_patch_ieee80211_xmit(skb, dev);
+			goto success;
+		}
+#endif
+
+		ether_type = ntohs(((struct ethhdr *)skb->data)->h_proto);
+
+		crypt = ieee->crypt[ieee->tx_keyidx];
+
+		encrypt = !(ether_type == ETH_P_PAE && ieee->ieee802_1x) &&
+			ieee->host_encrypt && crypt && crypt->ops;
+
+		if (!encrypt && ieee->ieee802_1x &&
+		ieee->drop_unencrypted && ether_type != ETH_P_PAE) {
+			stats->tx_dropped++;
+			goto success;
+		}
+
+	#ifdef CONFIG_IEEE80211_DEBUG
+		if (crypt && !encrypt && ether_type == ETH_P_PAE) {
+			struct eapol *eap = (struct eapol *)(skb->data +
+				sizeof(struct ethhdr) - SNAP_SIZE - sizeof(u16));
+			IEEE80211_DEBUG_EAP("TX: IEEE 802.11 EAPOL frame: %s\n",
+				eap_get_type(eap->type));
+		}
+	#endif
+
+		/* Save source and destination addresses */
+		memcpy(&dest, skb->data, ETH_ALEN);
+		memcpy(&src, skb->data+ETH_ALEN, ETH_ALEN);
+
+		/* Advance the SKB to the start of the payload */
+		skb_pull(skb, sizeof(struct ethhdr));
+
+		/* Determine total amount of storage required for TXB packets */
+		bytes = skb->len + SNAP_SIZE + sizeof(u16);
+
+		if(ieee->current_network.QoS_Enable) {
+			if (encrypt)
+				fc = IEEE80211_FTYPE_DATA | IEEE80211_STYPE_QOS_DATA |
+					IEEE80211_FCTL_WEP;
+			else
+				fc = IEEE80211_FTYPE_DATA | IEEE80211_STYPE_QOS_DATA;
+
+		} else {
+			if (encrypt)
+				fc = IEEE80211_FTYPE_DATA | IEEE80211_STYPE_DATA |
+					IEEE80211_FCTL_WEP;
+			else
+				fc = IEEE80211_FTYPE_DATA | IEEE80211_STYPE_DATA;
+		}
+
+		if (ieee->iw_mode == IW_MODE_INFRA) {
+			fc |= IEEE80211_FCTL_TODS;
+			/* To DS: Addr1 = BSSID, Addr2 = SA,
+			Addr3 = DA */
+			memcpy(&header.addr1, ieee->current_network.bssid, ETH_ALEN);
+			memcpy(&header.addr2, &src, ETH_ALEN);
+			memcpy(&header.addr3, &dest, ETH_ALEN);
+		} else if (ieee->iw_mode == IW_MODE_ADHOC) {
+			/* not From/To DS: Addr1 = DA, Addr2 = SA,
+			Addr3 = BSSID */
+			memcpy(&header.addr1, dest, ETH_ALEN);
+			memcpy(&header.addr2, src, ETH_ALEN);
+			memcpy(&header.addr3, ieee->current_network.bssid, ETH_ALEN);
+		}
+	//	printk(KERN_WARNING "essid MAC address is "MAC_FMT, MAC_ARG(&header.addr1));
+		header.frame_ctl = cpu_to_le16(fc);
+		//hdr_len = IEEE80211_3ADDR_LEN;
+
+		/* Determine fragmentation size based on destination (multicast
+		* and broadcast are not fragmented) */
+//		if (is_multicast_ether_addr(dest) ||
+//		is_broadcast_ether_addr(dest)) {
+		if (is_multicast_ether_addr(header.addr1) ||
+		is_broadcast_ether_addr(header.addr1)) {
+			frag_size = MAX_FRAG_THRESHOLD;
+			QOS_ctl = QOS_CTL_NOTCONTAIN_ACK;
+		}
+		else {
+			//printk(KERN_WARNING "&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&frag_size = %d\n", frag_size);
+			frag_size = ieee->fts;//default:392
+			QOS_ctl = 0;
+		}
+
+		if (ieee->current_network.QoS_Enable)	{
+			hdr_len = IEEE80211_3ADDR_LEN + 2;
+			QOS_ctl |= skb->priority; //set in the ieee80211_classify
+			header.QOS_ctl = cpu_to_le16(QOS_ctl);
+		} else {
+			hdr_len = IEEE80211_3ADDR_LEN;
+		}
+		//printk(KERN_WARNING "header size = %d, QOS_ctl = %x\n", hdr_len,QOS_ctl);
+		/* Determine amount of payload per fragment.  Regardless of if
+		* this stack is providing the full 802.11 header, one will
+		* eventually be affixed to this fragment -- so we must account for
+		* it when determining the amount of payload space. */
+		//bytes_per_frag = frag_size - (IEEE80211_3ADDR_LEN + (ieee->current_network->QoS_Enable ? 2:0));
+		bytes_per_frag = frag_size - hdr_len;
+		if (ieee->config &
+		(CFG_IEEE80211_COMPUTE_FCS | CFG_IEEE80211_RESERVE_FCS))
+			bytes_per_frag -= IEEE80211_FCS_LEN;
+
+		/* Each fragment may need to have room for encryptiong pre/postfix */
+		if (encrypt)
+			bytes_per_frag -= crypt->ops->extra_prefix_len +
+				crypt->ops->extra_postfix_len;
+
+		/* Number of fragments is the total bytes_per_frag /
+		* payload_per_fragment */
+		nr_frags = bytes / bytes_per_frag;
+		bytes_last_frag = bytes % bytes_per_frag;
+		if (bytes_last_frag)
+			nr_frags++;
+		else
+			bytes_last_frag = bytes_per_frag;
+
+		/* When we allocate the TXB we allocate enough space for the reserve
+		* and full fragment bytes (bytes_per_frag doesn't include prefix,
+		* postfix, header, FCS, etc.) */
+		txb = ieee80211_alloc_txb(nr_frags, frag_size, GFP_ATOMIC);
+		if (unlikely(!txb)) {
+			printk(KERN_WARNING "%s: Could not allocate TXB\n",
+			ieee->dev->name);
+			goto failed;
+		}
+		txb->encrypted = encrypt;
+		txb->payload_size = bytes;
+
+		for (i = 0; i < nr_frags; i++) {
+			skb_frag = txb->fragments[i];
+			skb_frag->priority = UP2AC(skb->priority);
+			if (encrypt)
+				skb_reserve(skb_frag, crypt->ops->extra_prefix_len);
+
+			frag_hdr = (struct ieee80211_hdr_3addr_QOS *)skb_put(skb_frag, hdr_len);
+			memcpy(frag_hdr, &header, hdr_len);
+
+			/* If this is not the last fragment, then add the MOREFRAGS
+			* bit to the frame control */
+			if (i != nr_frags - 1) {
+				frag_hdr->frame_ctl = cpu_to_le16(
+					fc | IEEE80211_FCTL_MOREFRAGS);
+				bytes = bytes_per_frag;
+
+			} else {
+				/* The last fragment takes the remaining length */
+				bytes = bytes_last_frag;
+			}
+			if(ieee->current_network.QoS_Enable) {
+			  // add 1 only indicate to corresponding seq number control 2006/7/12
+			  frag_hdr->seq_ctl = cpu_to_le16(ieee->seq_ctrl[UP2AC(skb->priority)+1]<<4 | i);
+			  //printk(KERN_WARNING "skb->priority = %d,", skb->priority);
+			  //printk(KERN_WARNING "type:%d: seq = %d\n",UP2AC(skb->priority),ieee->seq_ctrl[UP2AC(skb->priority)+1]);
+			} else {
+			  frag_hdr->seq_ctl = cpu_to_le16(ieee->seq_ctrl[0]<<4 | i);
+			}
+			//frag_hdr->seq_ctl = cpu_to_le16(ieee->seq_ctrl<<4 | i);
+			//
+
+			/* Put a SNAP header on the first fragment */
+			if (i == 0) {
+				ieee80211_put_snap(
+					skb_put(skb_frag, SNAP_SIZE + sizeof(u16)),
+					ether_type);
+				bytes -= SNAP_SIZE + sizeof(u16);
+			}
+
+			memcpy(skb_put(skb_frag, bytes), skb->data, bytes);
+
+			/* Advance the SKB... */
+			skb_pull(skb, bytes);
+
+			/* Encryption routine will move the header forward in order
+			* to insert the IV between the header and the payload */
+			if (encrypt)
+				ieee80211_encrypt_fragment(ieee, skb_frag, hdr_len);
+			if (ieee->config &
+			(CFG_IEEE80211_COMPUTE_FCS | CFG_IEEE80211_RESERVE_FCS))
+				skb_put(skb_frag, 4);
+		}
+		// Advance sequence number in data frame.
+		//printk(KERN_WARNING "QoS Enalbed? %s\n", ieee->current_network.QoS_Enable?"Y":"N");
+		if (ieee->current_network.QoS_Enable) {
+		  if (ieee->seq_ctrl[UP2AC(skb->priority) + 1] == 0xFFF)
+			ieee->seq_ctrl[UP2AC(skb->priority) + 1] = 0;
+		  else
+			ieee->seq_ctrl[UP2AC(skb->priority) + 1]++;
+		} else {
+  		  if (ieee->seq_ctrl[0] == 0xFFF)
+			ieee->seq_ctrl[0] = 0;
+		  else
+			ieee->seq_ctrl[0]++;
+		}
+		//---
+	}else{
+		if (unlikely(skb->len < sizeof(struct ieee80211_hdr_3addr))) {
+			printk(KERN_WARNING "%s: skb too small (%d).\n",
+			ieee->dev->name, skb->len);
+			goto success;
+		}
+
+		txb = ieee80211_alloc_txb(1, skb->len, GFP_ATOMIC);
+		if(!txb){
+			printk(KERN_WARNING "%s: Could not allocate TXB\n",
+			ieee->dev->name);
+			goto failed;
+		}
+
+		txb->encrypted = 0;
+		txb->payload_size = skb->len;
+		memcpy(skb_put(txb->fragments[0],skb->len), skb->data, skb->len);
+	}
+
+ success:
+	spin_unlock_irqrestore(&ieee->lock, flags);
+#ifdef _RTL8187_EXT_PATCH_
+	// Sometimes, extension mode can reuse skb (by txb->fragments[0])
+	if( ! ((ieee->iw_mode == ieee->iw_ext_mode) && txb && (txb->fragments[0] == skb)) )
+#endif
+		dev_kfree_skb_any(skb);
+	if (txb) {
+		if (ieee->softmac_features & IEEE_SOFTMAC_TX_QUEUE){
+			ieee80211_softmac_xmit(txb, ieee);
+		}else{
+			if ((*ieee->hard_start_xmit)(txb, dev) == 0) {
+				stats->tx_packets++;
+				stats->tx_bytes += txb->payload_size;
+				return 0;
+			}
+			ieee80211_txb_free(txb);
+		}
+	}
+
+	return 0;
+
+ failed:
+	spin_unlock_irqrestore(&ieee->lock, flags);
+	netif_stop_queue(dev);
+	stats->tx_errors++;
+	return 1;
+
+}
+
+#if 0
+EXPORT_SYMBOL(ieee80211_txb_free);
+#ifdef _RTL8187_EXT_PATCH_
+EXPORT_SYMBOL(ieee80211_alloc_txb);
+EXPORT_SYMBOL(ieee80211_ext_alloc_txb);
+EXPORT_SYMBOL(ieee80211_ext_reuse_txb);
+#endif // _RTL8187_EXT_PATCH_
+#endif
diff --git a/drivers/staging/rtl8187se/ieee80211/ieee80211_wx.c b/drivers/staging/rtl8187se/ieee80211/ieee80211_wx.c
new file mode 100644
index 00000000000..c7d9f4fda41
--- /dev/null
+++ b/drivers/staging/rtl8187se/ieee80211/ieee80211_wx.c
@@ -0,0 +1,884 @@
+/******************************************************************************
+
+  Copyright(c) 2004 Intel Corporation. All rights reserved.
+
+  Portions of this file are based on the WEP enablement code provided by the
+  Host AP project hostap-drivers v0.1.3
+  Copyright (c) 2001-2002, SSH Communications Security Corp and Jouni Malinen
+  <jkmaline@cc.hut.fi>
+  Copyright (c) 2002-2003, Jouni Malinen <jkmaline@cc.hut.fi>
+
+  This program is free software; you can redistribute it and/or modify it
+  under the terms of version 2 of the GNU General Public License as
+  published by the Free Software Foundation.
+
+  This program is distributed in the hope that it will be useful, but WITHOUT
+  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+  more details.
+
+  You should have received a copy of the GNU General Public License along with
+  this program; if not, write to the Free Software Foundation, Inc., 59
+  Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+
+  The full GNU General Public License is included in this distribution in the
+  file called LICENSE.
+
+  Contact Information:
+  James P. Ketrenos <ipw2100-admin@linux.intel.com>
+  Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+
+******************************************************************************/
+#include <linux/wireless.h>
+#include <linux/version.h>
+#include <linux/kmod.h>
+#include <linux/module.h>
+
+#include "ieee80211.h"
+static const char *ieee80211_modes[] = {
+	"?", "a", "b", "ab", "g", "ag", "bg", "abg"
+};
+
+#ifdef FEDORACORE_9
+#define IN_FEDORACORE_9 1
+#else
+#define IN_FEDORACORE_9 0
+#endif
+
+#define MAX_CUSTOM_LEN 64
+static inline char *rtl818x_translate_scan(struct ieee80211_device *ieee,
+		                           char *start, char *stop,
+                                           struct ieee80211_network *network,
+                                           struct iw_request_info *info)
+{
+	char custom[MAX_CUSTOM_LEN];
+	char *p;
+	struct iw_event iwe;
+	int i, j;
+	u8 max_rate, rate;
+
+	/* First entry *MUST* be the AP MAC address */
+	iwe.cmd = SIOCGIWAP;
+	iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
+	memcpy(iwe.u.ap_addr.sa_data, network->bssid, ETH_ALEN);
+#if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27))||IN_FEDORACORE_9)
+	start = iwe_stream_add_event(info, start, stop, &iwe, IW_EV_ADDR_LEN);
+#else
+	start = iwe_stream_add_event(start, stop, &iwe, IW_EV_ADDR_LEN);
+#endif
+
+	/* Remaining entries will be displayed in the order we provide them */
+
+	/* Add the ESSID */
+	iwe.cmd = SIOCGIWESSID;
+	iwe.u.data.flags = 1;
+	//YJ,modified,080903,for hidden ap
+	//if (network->flags & NETWORK_EMPTY_ESSID) {
+	if (network->ssid_len == 0) {
+	//YJ,modified,080903,end
+		iwe.u.data.length = sizeof("<hidden>");
+#if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27))||IN_FEDORACORE_9)
+		start = iwe_stream_add_point(info, start, stop, &iwe, "<hidden>");
+#else
+		start = iwe_stream_add_point(start, stop, &iwe, "<hidden>");
+#endif
+	} else {
+		iwe.u.data.length = min(network->ssid_len, (u8)32);
+#if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27))||IN_FEDORACORE_9)
+		start = iwe_stream_add_point(info, start, stop, &iwe, network->ssid);
+#else
+		start = iwe_stream_add_point(start, stop, &iwe, network->ssid);
+#endif
+	}
+	//printk("ESSID: %s\n",network->ssid);
+	/* Add the protocol name */
+	iwe.cmd = SIOCGIWNAME;
+	snprintf(iwe.u.name, IFNAMSIZ, "IEEE 802.11%s", ieee80211_modes[network->mode]);
+#if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27))||IN_FEDORACORE_9)
+	start = iwe_stream_add_event(info, start, stop, &iwe, IW_EV_CHAR_LEN);
+#else
+	start = iwe_stream_add_event(start, stop, &iwe, IW_EV_CHAR_LEN);
+#endif
+
+        /* Add mode */
+        iwe.cmd = SIOCGIWMODE;
+        if (network->capability &
+	    (WLAN_CAPABILITY_BSS | WLAN_CAPABILITY_IBSS)) {
+		if (network->capability & WLAN_CAPABILITY_BSS)
+			iwe.u.mode = IW_MODE_MASTER;
+		else
+			iwe.u.mode = IW_MODE_ADHOC;
+
+#if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27))||IN_FEDORACORE_9)
+		start = iwe_stream_add_event(info, start, stop, &iwe, IW_EV_UINT_LEN);
+#else
+		start = iwe_stream_add_event(start, stop, &iwe, IW_EV_UINT_LEN);
+#endif
+	}
+
+        /* Add frequency/channel */
+	iwe.cmd = SIOCGIWFREQ;
+/*	iwe.u.freq.m = ieee80211_frequency(network->channel, network->mode);
+	iwe.u.freq.e = 3; */
+	iwe.u.freq.m = network->channel;
+	iwe.u.freq.e = 0;
+	iwe.u.freq.i = 0;
+#if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27))||IN_FEDORACORE_9)
+	start = iwe_stream_add_event(info, start, stop, &iwe, IW_EV_FREQ_LEN);
+#else
+	start = iwe_stream_add_event(start, stop, &iwe, IW_EV_FREQ_LEN);
+#endif
+
+	/* Add encryption capability */
+	iwe.cmd = SIOCGIWENCODE;
+	if (network->capability & WLAN_CAPABILITY_PRIVACY)
+		iwe.u.data.flags = IW_ENCODE_ENABLED | IW_ENCODE_NOKEY;
+	else
+		iwe.u.data.flags = IW_ENCODE_DISABLED;
+	iwe.u.data.length = 0;
+#if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27))||IN_FEDORACORE_9)
+	start = iwe_stream_add_point(info, start, stop, &iwe, network->ssid);
+#else
+	start = iwe_stream_add_point(start, stop, &iwe, network->ssid);
+#endif
+
+	/* Add basic and extended rates */
+	max_rate = 0;
+	p = custom;
+	p += snprintf(p, MAX_CUSTOM_LEN - (p - custom), " Rates (Mb/s): ");
+	for (i = 0, j = 0; i < network->rates_len; ) {
+		if (j < network->rates_ex_len &&
+		    ((network->rates_ex[j] & 0x7F) <
+		     (network->rates[i] & 0x7F)))
+			rate = network->rates_ex[j++] & 0x7F;
+		else
+			rate = network->rates[i++] & 0x7F;
+		if (rate > max_rate)
+			max_rate = rate;
+		p += snprintf(p, MAX_CUSTOM_LEN - (p - custom),
+			      "%d%s ", rate >> 1, (rate & 1) ? ".5" : "");
+	}
+	for (; j < network->rates_ex_len; j++) {
+		rate = network->rates_ex[j] & 0x7F;
+		p += snprintf(p, MAX_CUSTOM_LEN - (p - custom),
+			      "%d%s ", rate >> 1, (rate & 1) ? ".5" : "");
+		if (rate > max_rate)
+			max_rate = rate;
+	}
+
+	iwe.cmd = SIOCGIWRATE;
+	iwe.u.bitrate.fixed = iwe.u.bitrate.disabled = 0;
+	iwe.u.bitrate.value = max_rate * 500000;
+#if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27))||IN_FEDORACORE_9)
+	start = iwe_stream_add_event(info, start, stop, &iwe, IW_EV_PARAM_LEN);
+#else
+	start = iwe_stream_add_event(start, stop, &iwe, IW_EV_PARAM_LEN);
+#endif
+
+	iwe.cmd = IWEVCUSTOM;
+	iwe.u.data.length = p - custom;
+	if (iwe.u.data.length)
+#if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27))||IN_FEDORACORE_9)
+		start = iwe_stream_add_point(info, start, stop, &iwe, custom);
+#else
+		start = iwe_stream_add_point(start, stop, &iwe, custom);
+#endif
+
+	/* Add quality statistics */
+	/* TODO: Fix these values... */
+	if (network->stats.signal == 0 || network->stats.rssi == 0)
+	printk("========>signal:%d, rssi:%d\n", network->stats.signal, network->stats.rssi);
+	iwe.cmd = IWEVQUAL;
+//	printk("SIGNAL: %d,RSSI: %d,NOISE: %d\n",network->stats.signal,network->stats.rssi,network->stats.noise);
+	iwe.u.qual.qual = network->stats.signalstrength;
+	iwe.u.qual.level = network->stats.signal;
+	iwe.u.qual.noise = network->stats.noise;
+	iwe.u.qual.updated = network->stats.mask & IEEE80211_STATMASK_WEMASK;
+	if (!(network->stats.mask & IEEE80211_STATMASK_RSSI))
+		iwe.u.qual.updated |= IW_QUAL_LEVEL_INVALID;
+	if (!(network->stats.mask & IEEE80211_STATMASK_NOISE))
+		iwe.u.qual.updated |= IW_QUAL_NOISE_INVALID;
+	if (!(network->stats.mask & IEEE80211_STATMASK_SIGNAL))
+		iwe.u.qual.updated |= IW_QUAL_QUAL_INVALID;
+	iwe.u.qual.updated = 7;
+#if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27))||IN_FEDORACORE_9)
+	start = iwe_stream_add_event(info, start, stop, &iwe, IW_EV_QUAL_LEN);
+#else
+	start = iwe_stream_add_event(start, stop, &iwe, IW_EV_QUAL_LEN);
+#endif
+
+	iwe.cmd = IWEVCUSTOM;
+	p = custom;
+
+	iwe.u.data.length = p - custom;
+	if (iwe.u.data.length)
+#if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27))||IN_FEDORACORE_9)
+		start = iwe_stream_add_point(info, start, stop, &iwe, custom);
+#else
+		start = iwe_stream_add_point(start, stop, &iwe, custom);
+#endif
+
+#if 0
+	if (ieee->wpa_enabled && network->wpa_ie_len){
+		char buf[MAX_WPA_IE_LEN * 2 + 30];
+	//	printk("WPA IE\n");
+		u8 *p = buf;
+		p += sprintf(p, "wpa_ie=");
+		for (i = 0; i < network->wpa_ie_len; i++) {
+			p += sprintf(p, "%02x", network->wpa_ie[i]);
+		}
+
+		memset(&iwe, 0, sizeof(iwe));
+		iwe.cmd = IWEVCUSTOM;
+		iwe.u.data.length = strlen(buf);
+#if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27))||IN_FEDORACORE_9)
+		start = iwe_stream_add_point(info, start, stop, &iwe, buf);
+#else
+		start = iwe_stream_add_point(start, stop, &iwe, buf);
+#endif
+	}
+
+	if (ieee->wpa_enabled && network->rsn_ie_len){
+		char buf[MAX_WPA_IE_LEN * 2 + 30];
+
+		u8 *p = buf;
+		p += sprintf(p, "rsn_ie=");
+		for (i = 0; i < network->rsn_ie_len; i++) {
+			p += sprintf(p, "%02x", network->rsn_ie[i]);
+		}
+
+
+#else
+		memset(&iwe, 0, sizeof(iwe));
+        if (network->wpa_ie_len) {
+	//	printk("wpa_ie_len:%d\n", network->wpa_ie_len);
+                char buf[MAX_WPA_IE_LEN];
+                memcpy(buf, network->wpa_ie, network->wpa_ie_len);
+                iwe.cmd = IWEVGENIE;
+                iwe.u.data.length = network->wpa_ie_len;
+#if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27))||IN_FEDORACORE_9)
+                start = iwe_stream_add_point(info, start, stop, &iwe, buf);
+#else
+                start = iwe_stream_add_point(start, stop, &iwe, buf);
+#endif
+        }
+
+        memset(&iwe, 0, sizeof(iwe));
+        if (network->rsn_ie_len) {
+	//	printk("=====>rsn_ie_len:\n", network->rsn_ie_len);
+		#if 0
+		{
+			int i;
+			for (i=0; i<network->rsn_ie_len; i++);
+			printk("%2x ", network->rsn_ie[i]);
+			printk("\n");
+		}
+		#endif
+                char buf[MAX_WPA_IE_LEN];
+                memcpy(buf, network->rsn_ie, network->rsn_ie_len);
+                iwe.cmd = IWEVGENIE;
+                iwe.u.data.length = network->rsn_ie_len;
+#if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27))||IN_FEDORACORE_9)
+		start = iwe_stream_add_point(info, start, stop, &iwe, buf);
+#else
+		start = iwe_stream_add_point(start, stop, &iwe, buf);
+#endif
+	}
+
+#endif
+
+	/* Add EXTRA: Age to display seconds since last beacon/probe response
+	 * for given network. */
+	iwe.cmd = IWEVCUSTOM;
+	p = custom;
+	p += snprintf(p, MAX_CUSTOM_LEN - (p - custom),
+		      " Last beacon: %lums ago", (jiffies - network->last_scanned) / (HZ / 100));
+	iwe.u.data.length = p - custom;
+	if (iwe.u.data.length)
+#if((LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27))||IN_FEDORACORE_9)
+		start = iwe_stream_add_point(info, start, stop, &iwe, custom);
+#else
+		start = iwe_stream_add_point(start, stop, &iwe, custom);
+#endif
+
+	return start;
+}
+
+int ieee80211_wx_get_scan(struct ieee80211_device *ieee,
+			  struct iw_request_info *info,
+			  union iwreq_data *wrqu, char *extra)
+{
+	struct ieee80211_network *network;
+	unsigned long flags;
+	int err = 0;
+	char *ev = extra;
+	char *stop = ev + wrqu->data.length;//IW_SCAN_MAX_DATA;
+	//char *stop = ev + IW_SCAN_MAX_DATA;
+	int i = 0;
+
+	IEEE80211_DEBUG_WX("Getting scan\n");
+	down(&ieee->wx_sem);
+	spin_lock_irqsave(&ieee->lock, flags);
+
+	if(!ieee->bHwRadioOff)
+	{
+		list_for_each_entry(network, &ieee->network_list, list) {
+			i++;
+
+			if((stop-ev)<200)
+			{
+				err = -E2BIG;
+				break;
+			}
+			if (ieee->scan_age == 0 ||
+			    time_after(network->last_scanned + ieee->scan_age, jiffies))
+			{
+				ev = rtl818x_translate_scan(ieee, ev, stop, network, info);
+			}
+			else
+				IEEE80211_DEBUG_SCAN(
+					"Not showing network '%s ("
+					MAC_FMT ")' due to age (%lums).\n",
+					escape_essid(network->ssid,
+						     network->ssid_len),
+					MAC_ARG(network->bssid),
+					(jiffies - network->last_scanned) / (HZ / 100));
+		}
+	}
+	spin_unlock_irqrestore(&ieee->lock, flags);
+	up(&ieee->wx_sem);
+	wrqu->data.length = ev -  extra;
+	wrqu->data.flags = 0;
+	IEEE80211_DEBUG_WX("exit: %d networks returned.\n", i);
+
+	return err;
+}
+
+int ieee80211_wx_set_encode(struct ieee80211_device *ieee,
+			    struct iw_request_info *info,
+			    union iwreq_data *wrqu, char *keybuf)
+{
+	struct iw_point *erq = &(wrqu->encoding);
+	struct net_device *dev = ieee->dev;
+	struct ieee80211_security sec = {
+		.flags = 0
+	};
+	int i, key, key_provided, len;
+	struct ieee80211_crypt_data **crypt;
+
+	IEEE80211_DEBUG_WX("SET_ENCODE\n");
+
+	key = erq->flags & IW_ENCODE_INDEX;
+	if (key) {
+		if (key > WEP_KEYS)
+			return -EINVAL;
+		key--;
+		key_provided = 1;
+	} else {
+		key_provided = 0;
+		key = ieee->tx_keyidx;
+	}
+
+	IEEE80211_DEBUG_WX("Key: %d [%s]\n", key, key_provided ?
+			   "provided" : "default");
+
+	crypt = &ieee->crypt[key];
+
+	if (erq->flags & IW_ENCODE_DISABLED) {
+		if (key_provided && *crypt) {
+			IEEE80211_DEBUG_WX("Disabling encryption on key %d.\n",
+					   key);
+			ieee80211_crypt_delayed_deinit(ieee, crypt);
+		} else
+			IEEE80211_DEBUG_WX("Disabling encryption.\n");
+
+		/* Check all the keys to see if any are still configured,
+		 * and if no key index was provided, de-init them all */
+		for (i = 0; i < WEP_KEYS; i++) {
+			if (ieee->crypt[i] != NULL) {
+				if (key_provided)
+					break;
+				ieee80211_crypt_delayed_deinit(
+					ieee, &ieee->crypt[i]);
+			}
+		}
+
+		if (i == WEP_KEYS) {
+			sec.enabled = 0;
+			sec.level = SEC_LEVEL_0;
+			sec.flags |= SEC_ENABLED | SEC_LEVEL;
+		}
+
+		goto done;
+	}
+
+
+
+	sec.enabled = 1;
+	sec.flags |= SEC_ENABLED;
+
+	if (*crypt != NULL && (*crypt)->ops != NULL &&
+	    strcmp((*crypt)->ops->name, "WEP") != 0) {
+		/* changing to use WEP; deinit previously used algorithm
+		 * on this key */
+		ieee80211_crypt_delayed_deinit(ieee, crypt);
+	}
+
+	if (*crypt == NULL) {
+		struct ieee80211_crypt_data *new_crypt;
+
+		/* take WEP into use */
+		new_crypt = kmalloc(sizeof(struct ieee80211_crypt_data),
+				    GFP_KERNEL);
+		if (new_crypt == NULL)
+			return -ENOMEM;
+		memset(new_crypt, 0, sizeof(struct ieee80211_crypt_data));
+		new_crypt->ops = ieee80211_get_crypto_ops("WEP");
+		if (!new_crypt->ops) {
+			request_module("ieee80211_crypt_wep");
+			new_crypt->ops = ieee80211_get_crypto_ops("WEP");
+		}
+
+		if (new_crypt->ops && try_module_get(new_crypt->ops->owner))
+			new_crypt->priv = new_crypt->ops->init(key);
+
+		if (!new_crypt->ops || !new_crypt->priv) {
+			kfree(new_crypt);
+			new_crypt = NULL;
+
+			printk(KERN_WARNING "%s: could not initialize WEP: "
+			       "load module ieee80211_crypt_wep\n",
+			       dev->name);
+			return -EOPNOTSUPP;
+		}
+		*crypt = new_crypt;
+	}
+
+	/* If a new key was provided, set it up */
+	if (erq->length > 0) {
+		len = erq->length <= 5 ? 5 : 13;
+		memcpy(sec.keys[key], keybuf, erq->length);
+		if (len > erq->length)
+			memset(sec.keys[key] + erq->length, 0,
+			       len - erq->length);
+		IEEE80211_DEBUG_WX("Setting key %d to '%s' (%d:%d bytes)\n",
+				   key, escape_essid(sec.keys[key], len),
+				   erq->length, len);
+		sec.key_sizes[key] = len;
+ 		(*crypt)->ops->set_key(sec.keys[key], len, NULL,
+				       (*crypt)->priv);
+		sec.flags |= (1 << key);
+		/* This ensures a key will be activated if no key is
+		 * explicitely set */
+		if (key == sec.active_key)
+			sec.flags |= SEC_ACTIVE_KEY;
+		ieee->tx_keyidx = key;//by wb 080312
+	} else {
+		len = (*crypt)->ops->get_key(sec.keys[key], WEP_KEY_LEN,
+					     NULL, (*crypt)->priv);
+		if (len == 0) {
+			/* Set a default key of all 0 */
+			IEEE80211_DEBUG_WX("Setting key %d to all zero.\n",
+					   key);
+			memset(sec.keys[key], 0, 13);
+			(*crypt)->ops->set_key(sec.keys[key], 13, NULL,
+					       (*crypt)->priv);
+			sec.key_sizes[key] = 13;
+			sec.flags |= (1 << key);
+		}
+
+		/* No key data - just set the default TX key index */
+		if (key_provided) {
+			IEEE80211_DEBUG_WX(
+				"Setting key %d to default Tx key.\n", key);
+			ieee->tx_keyidx = key;
+			sec.active_key = key;
+			sec.flags |= SEC_ACTIVE_KEY;
+		}
+	}
+
+ done:
+	ieee->open_wep = !(erq->flags & IW_ENCODE_RESTRICTED);
+	sec.auth_mode = ieee->open_wep ? WLAN_AUTH_OPEN : WLAN_AUTH_SHARED_KEY;
+	sec.flags |= SEC_AUTH_MODE;
+	IEEE80211_DEBUG_WX("Auth: %s\n", sec.auth_mode == WLAN_AUTH_OPEN ?
+			   "OPEN" : "SHARED KEY");
+
+	/* For now we just support WEP, so only set that security level...
+	 * TODO: When WPA is added this is one place that needs to change */
+	sec.flags |= SEC_LEVEL;
+	sec.level = SEC_LEVEL_1; /* 40 and 104 bit WEP */
+
+	if (ieee->set_security)
+		ieee->set_security(dev, &sec);
+
+	/* Do not reset port if card is in Managed mode since resetting will
+	 * generate new IEEE 802.11 authentication which may end up in looping
+	 * with IEEE 802.1X.  If your hardware requires a reset after WEP
+	 * configuration (for example... Prism2), implement the reset_port in
+	 * the callbacks structures used to initialize the 802.11 stack. */
+	if (ieee->reset_on_keychange &&
+	    ieee->iw_mode != IW_MODE_INFRA &&
+	    ieee->reset_port && ieee->reset_port(dev)) {
+		printk(KERN_DEBUG "%s: reset_port failed\n", dev->name);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+int ieee80211_wx_get_encode(struct ieee80211_device *ieee,
+			    struct iw_request_info *info,
+			    union iwreq_data *wrqu, char *keybuf)
+{
+	struct iw_point *erq = &(wrqu->encoding);
+	int len, key;
+	struct ieee80211_crypt_data *crypt;
+
+	IEEE80211_DEBUG_WX("GET_ENCODE\n");
+
+	if(ieee->iw_mode == IW_MODE_MONITOR)
+		return -1;
+
+	key = erq->flags & IW_ENCODE_INDEX;
+	if (key) {
+		if (key > WEP_KEYS)
+			return -EINVAL;
+		key--;
+	} else
+		key = ieee->tx_keyidx;
+
+	crypt = ieee->crypt[key];
+	erq->flags = key + 1;
+
+	if (crypt == NULL || crypt->ops == NULL) {
+		erq->length = 0;
+		erq->flags |= IW_ENCODE_DISABLED;
+		return 0;
+	}
+
+	if (strcmp(crypt->ops->name, "WEP") != 0) {
+		/* only WEP is supported with wireless extensions, so just
+		 * report that encryption is used */
+		erq->length = 0;
+		erq->flags |= IW_ENCODE_ENABLED;
+		return 0;
+	}
+
+	len = crypt->ops->get_key(keybuf, WEP_KEY_LEN, NULL, crypt->priv);
+	erq->length = (len >= 0 ? len : 0);
+
+	erq->flags |= IW_ENCODE_ENABLED;
+
+	if (ieee->open_wep)
+		erq->flags |= IW_ENCODE_OPEN;
+	else
+		erq->flags |= IW_ENCODE_RESTRICTED;
+
+	return 0;
+}
+
+int ieee80211_wx_set_encode_ext(struct ieee80211_device *ieee,
+                               struct iw_request_info *info,
+                               union iwreq_data *wrqu, char *extra)
+{
+	struct net_device *dev = ieee->dev;
+        struct iw_point *encoding = &wrqu->encoding;
+        struct iw_encode_ext *ext = (struct iw_encode_ext *)extra;
+        int i, idx, ret = 0;
+        int group_key = 0;
+        const char *alg, *module;
+        struct ieee80211_crypto_ops *ops;
+        struct ieee80211_crypt_data **crypt;
+
+        struct ieee80211_security sec = {
+                .flags = 0,
+        };
+	//printk("======>encoding flag:%x,ext flag:%x, ext alg:%d\n", encoding->flags,ext->ext_flags, ext->alg);
+        idx = encoding->flags & IW_ENCODE_INDEX;
+        if (idx) {
+                if (idx < 1 || idx > WEP_KEYS)
+                        return -EINVAL;
+                idx--;
+        } else
+                idx = ieee->tx_keyidx;
+
+        if (ext->ext_flags & IW_ENCODE_EXT_GROUP_KEY) {
+                crypt = &ieee->crypt[idx];
+                group_key = 1;
+        } else {
+                /* some Cisco APs use idx>0 for unicast in dynamic WEP */
+		//printk("not group key, flags:%x, ext->alg:%d\n", ext->ext_flags, ext->alg);
+                if (idx != 0 && ext->alg != IW_ENCODE_ALG_WEP)
+                        return -EINVAL;
+                if (ieee->iw_mode == IW_MODE_INFRA)
+                        crypt = &ieee->crypt[idx];
+                else
+                        return -EINVAL;
+        }
+
+        sec.flags |= SEC_ENABLED;// | SEC_ENCRYPT;
+        if ((encoding->flags & IW_ENCODE_DISABLED) ||
+            ext->alg == IW_ENCODE_ALG_NONE) {
+                if (*crypt)
+                        ieee80211_crypt_delayed_deinit(ieee, crypt);
+
+                for (i = 0; i < WEP_KEYS; i++)
+                        if (ieee->crypt[i] != NULL)
+                                break;
+
+                if (i == WEP_KEYS) {
+                        sec.enabled = 0;
+                      //  sec.encrypt = 0;
+                        sec.level = SEC_LEVEL_0;
+                        sec.flags |= SEC_LEVEL;
+                }
+		//printk("disabled: flag:%x\n", encoding->flags);
+                goto done;
+        }
+
+	sec.enabled = 1;
+    //    sec.encrypt = 1;
+#if 0
+        if (group_key ? !ieee->host_mc_decrypt :
+            !(ieee->host_encrypt || ieee->host_decrypt ||
+              ieee->host_encrypt_msdu))
+                goto skip_host_crypt;
+#endif
+        switch (ext->alg) {
+        case IW_ENCODE_ALG_WEP:
+                alg = "WEP";
+                module = "ieee80211_crypt_wep";
+                break;
+        case IW_ENCODE_ALG_TKIP:
+                alg = "TKIP";
+                module = "ieee80211_crypt_tkip";
+                break;
+        case IW_ENCODE_ALG_CCMP:
+                alg = "CCMP";
+                module = "ieee80211_crypt_ccmp";
+                break;
+        default:
+                IEEE80211_DEBUG_WX("%s: unknown crypto alg %d\n",
+                                   dev->name, ext->alg);
+                ret = -EINVAL;
+                goto done;
+        }
+//	printk("8-09-08-9=====>%s, alg name:%s\n",__FUNCTION__, alg);
+
+	 ops = ieee80211_get_crypto_ops(alg);
+        if (ops == NULL) {
+                request_module(module);
+                ops = ieee80211_get_crypto_ops(alg);
+        }
+        if (ops == NULL) {
+                IEEE80211_DEBUG_WX("%s: unknown crypto alg %d\n",
+                                   dev->name, ext->alg);
+		printk("========>unknown crypto alg %d\n", ext->alg);
+                ret = -EINVAL;
+                goto done;
+        }
+
+        if (*crypt == NULL || (*crypt)->ops != ops) {
+                struct ieee80211_crypt_data *new_crypt;
+
+                ieee80211_crypt_delayed_deinit(ieee, crypt);
+
+                new_crypt = kzalloc(sizeof(*new_crypt), GFP_KERNEL);
+                if (new_crypt == NULL) {
+                        ret = -ENOMEM;
+                        goto done;
+                }
+                new_crypt->ops = ops;
+                if (new_crypt->ops && try_module_get(new_crypt->ops->owner))
+                        new_crypt->priv = new_crypt->ops->init(idx);
+                if (new_crypt->priv == NULL) {
+                        kfree(new_crypt);
+                        ret = -EINVAL;
+                        goto done;
+                }
+                *crypt = new_crypt;
+
+ 	}
+
+        if (ext->key_len > 0 && (*crypt)->ops->set_key &&
+            (*crypt)->ops->set_key(ext->key, ext->key_len, ext->rx_seq,
+                                   (*crypt)->priv) < 0) {
+                IEEE80211_DEBUG_WX("%s: key setting failed\n", dev->name);
+		printk("key setting failed\n");
+                ret = -EINVAL;
+                goto done;
+        }
+#if 1
+ //skip_host_crypt:
+	//printk("skip_host_crypt:ext_flags:%x\n", ext->ext_flags);
+        if (ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY) {
+                ieee->tx_keyidx = idx;
+                sec.active_key = idx;
+                sec.flags |= SEC_ACTIVE_KEY;
+        }
+
+        if (ext->alg != IW_ENCODE_ALG_NONE) {
+                memcpy(sec.keys[idx], ext->key, ext->key_len);
+                sec.key_sizes[idx] = ext->key_len;
+                sec.flags |= (1 << idx);
+                if (ext->alg == IW_ENCODE_ALG_WEP) {
+                      //  sec.encode_alg[idx] = SEC_ALG_WEP;
+                        sec.flags |= SEC_LEVEL;
+                        sec.level = SEC_LEVEL_1;
+                } else if (ext->alg == IW_ENCODE_ALG_TKIP) {
+                      //  sec.encode_alg[idx] = SEC_ALG_TKIP;
+                        sec.flags |= SEC_LEVEL;
+                        sec.level = SEC_LEVEL_2;
+                } else if (ext->alg == IW_ENCODE_ALG_CCMP) {
+                       // sec.encode_alg[idx] = SEC_ALG_CCMP;
+                        sec.flags |= SEC_LEVEL;
+                        sec.level = SEC_LEVEL_3;
+                }
+                /* Don't set sec level for group keys. */
+                if (group_key)
+                        sec.flags &= ~SEC_LEVEL;
+        }
+#endif
+done:
+        if (ieee->set_security)
+                ieee->set_security(ieee->dev, &sec);
+
+	 if (ieee->reset_on_keychange &&
+            ieee->iw_mode != IW_MODE_INFRA &&
+            ieee->reset_port && ieee->reset_port(dev)) {
+                IEEE80211_DEBUG_WX("%s: reset_port failed\n", dev->name);
+                return -EINVAL;
+        }
+
+        return ret;
+}
+int ieee80211_wx_set_mlme(struct ieee80211_device *ieee,
+                               struct iw_request_info *info,
+                               union iwreq_data *wrqu, char *extra)
+{
+	struct iw_mlme *mlme = (struct iw_mlme *) extra;
+//	printk("\ndkgadfslkdjgalskdf===============>%s(), cmd:%x\n", __FUNCTION__, mlme->cmd);
+#if 1
+	switch (mlme->cmd) {
+        case IW_MLME_DEAUTH:
+	case IW_MLME_DISASSOC:
+	//	printk("disassoc now\n");
+		ieee80211_disassociate(ieee);
+		break;
+	 default:
+                return -EOPNOTSUPP;
+        }
+#endif
+	return 0;
+}
+
+int ieee80211_wx_set_auth(struct ieee80211_device *ieee,
+                               struct iw_request_info *info,
+                               struct iw_param *data, char *extra)
+{
+/*
+	 struct ieee80211_security sec = {
+                .flags = SEC_AUTH_MODE,
+	}
+*/
+	//printk("set auth:flag:%x, data value:%x\n", data->flags, data->value);
+	switch (data->flags & IW_AUTH_INDEX) {
+        case IW_AUTH_WPA_VERSION:
+	     /*need to support wpa2 here*/
+		//printk("wpa version:%x\n", data->value);
+		break;
+        case IW_AUTH_CIPHER_PAIRWISE:
+        case IW_AUTH_CIPHER_GROUP:
+        case IW_AUTH_KEY_MGMT:
+                /*
+ *                  * Host AP driver does not use these parameters and allows
+ *                                   * wpa_supplicant to control them internally.
+ *                                                    */
+                break;
+        case IW_AUTH_TKIP_COUNTERMEASURES:
+                ieee->tkip_countermeasures = data->value;
+                break;
+        case IW_AUTH_DROP_UNENCRYPTED:
+                ieee->drop_unencrypted = data->value;
+		break;
+
+	case IW_AUTH_80211_AUTH_ALG:
+		ieee->open_wep = (data->value&IW_AUTH_ALG_OPEN_SYSTEM)?1:0;
+		//printk("open_wep:%d\n", ieee->open_wep);
+		break;
+
+#if 1
+	case IW_AUTH_WPA_ENABLED:
+		ieee->wpa_enabled = (data->value)?1:0;
+		//printk("enalbe wpa:%d\n", ieee->wpa_enabled);
+		break;
+
+#endif
+	case IW_AUTH_RX_UNENCRYPTED_EAPOL:
+                ieee->ieee802_1x = data->value;
+		break;
+	case IW_AUTH_PRIVACY_INVOKED:
+		ieee->privacy_invoked = data->value;
+		break;
+	default:
+                return -EOPNOTSUPP;
+	}
+	return 0;
+}
+
+#if 1
+int ieee80211_wx_set_gen_ie(struct ieee80211_device *ieee, u8 *ie, size_t len)
+{
+#if 0
+	printk("====>%s()\n", __FUNCTION__);
+	{
+		int i;
+		for (i=0; i<len; i++)
+		printk("%2x ", ie[i]&0xff);
+		printk("\n");
+	}
+#endif
+	u8 *buf = NULL;
+
+	if (len>MAX_WPA_IE_LEN || (len && ie == NULL))
+	{
+		printk("return error out, len:%d\n", len);
+	return -EINVAL;
+	}
+
+	if (len)
+	{
+		if (len != ie[1]+2){
+			printk("len:%d, ie:%d\n", len, ie[1]);
+			return -EINVAL;
+		}
+		buf = kmalloc(len, GFP_KERNEL);
+		if (buf == NULL)
+			return -ENOMEM;
+		memcpy(buf, ie, len);
+		kfree(ieee->wpa_ie);
+		ieee->wpa_ie = buf;
+		ieee->wpa_ie_len = len;
+	}
+	else{
+		if (ieee->wpa_ie)
+		kfree(ieee->wpa_ie);
+		ieee->wpa_ie = NULL;
+		ieee->wpa_ie_len = 0;
+	}
+//	printk("<=====out %s()\n", __FUNCTION__);
+
+	return 0;
+
+}
+#endif
+
+#if 0
+EXPORT_SYMBOL(ieee80211_wx_set_gen_ie);
+EXPORT_SYMBOL(ieee80211_wx_set_mlme);
+EXPORT_SYMBOL(ieee80211_wx_set_auth);
+EXPORT_SYMBOL(ieee80211_wx_set_encode_ext);
+EXPORT_SYMBOL(ieee80211_wx_get_scan);
+EXPORT_SYMBOL(ieee80211_wx_set_encode);
+EXPORT_SYMBOL(ieee80211_wx_get_encode);
+#endif
diff --git a/drivers/staging/rtl8187se/ieee80211/internal.h b/drivers/staging/rtl8187se/ieee80211/internal.h
new file mode 100644
index 00000000000..ddc22350d00
--- /dev/null
+++ b/drivers/staging/rtl8187se/ieee80211/internal.h
@@ -0,0 +1,115 @@
+/*
+ * Cryptographic API.
+ *
+ * Copyright (c) 2002 James Morris <jmorris@intercode.com.au>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the Free
+ * Software Foundation; either version 2 of the License, or (at your option)
+ * any later version.
+ *
+ */
+#ifndef _CRYPTO_INTERNAL_H
+#define _CRYPTO_INTERNAL_H
+
+
+//#include <linux/crypto.h>
+#include "rtl_crypto.h"
+#include <linux/mm.h>
+#include <linux/highmem.h>
+#include <linux/init.h>
+#include <asm/hardirq.h>
+#include <asm/softirq.h>
+#include <asm/kmap_types.h>
+
+#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,4,20))
+#define list_for_each_entry(pos, head, member)				\
+	for (pos = list_entry((head)->next, typeof(*pos), member),	\
+		     prefetch(pos->member.next);			\
+	     &pos->member != (head); 					\
+	     pos = list_entry(pos->member.next, typeof(*pos), member),	\
+		     prefetch(pos->member.next))
+
+static inline void cond_resched(void)
+{
+	if (need_resched()) {
+		set_current_state(TASK_RUNNING);
+		schedule();
+	}
+}
+#endif
+
+extern enum km_type crypto_km_types[];
+
+static inline enum km_type crypto_kmap_type(int out)
+{
+	return crypto_km_types[(in_softirq() ? 2 : 0) + out];
+}
+
+static inline void *crypto_kmap(struct page *page, int out)
+{
+	return kmap_atomic(page, crypto_kmap_type(out));
+}
+
+static inline void crypto_kunmap(void *vaddr, int out)
+{
+	kunmap_atomic(vaddr, crypto_kmap_type(out));
+}
+
+static inline void crypto_yield(struct crypto_tfm *tfm)
+{
+	if (!in_softirq())
+		cond_resched();
+}
+
+static inline void *crypto_tfm_ctx(struct crypto_tfm *tfm)
+{
+	return (void *)&tfm[1];
+}
+
+struct crypto_alg *crypto_alg_lookup(const char *name);
+
+#ifdef CONFIG_KMOD
+void crypto_alg_autoload(const char *name);
+struct crypto_alg *crypto_alg_mod_lookup(const char *name);
+#else
+static inline struct crypto_alg *crypto_alg_mod_lookup(const char *name)
+{
+	return crypto_alg_lookup(name);
+}
+#endif
+
+#ifdef CONFIG_CRYPTO_HMAC
+int crypto_alloc_hmac_block(struct crypto_tfm *tfm);
+void crypto_free_hmac_block(struct crypto_tfm *tfm);
+#else
+static inline int crypto_alloc_hmac_block(struct crypto_tfm *tfm)
+{
+	return 0;
+}
+
+static inline void crypto_free_hmac_block(struct crypto_tfm *tfm)
+{ }
+#endif
+
+#ifdef CONFIG_PROC_FS
+void __init crypto_init_proc(void);
+#else
+static inline void crypto_init_proc(void)
+{ }
+#endif
+
+int crypto_init_digest_flags(struct crypto_tfm *tfm, u32 flags);
+int crypto_init_cipher_flags(struct crypto_tfm *tfm, u32 flags);
+int crypto_init_compress_flags(struct crypto_tfm *tfm, u32 flags);
+
+int crypto_init_digest_ops(struct crypto_tfm *tfm);
+int crypto_init_cipher_ops(struct crypto_tfm *tfm);
+int crypto_init_compress_ops(struct crypto_tfm *tfm);
+
+void crypto_exit_digest_ops(struct crypto_tfm *tfm);
+void crypto_exit_cipher_ops(struct crypto_tfm *tfm);
+void crypto_exit_compress_ops(struct crypto_tfm *tfm);
+
+#endif	/* _CRYPTO_INTERNAL_H */
+
diff --git a/drivers/staging/rtl8187se/ieee80211/rtl_crypto.h b/drivers/staging/rtl8187se/ieee80211/rtl_crypto.h
new file mode 100644
index 00000000000..9ed0ca42085
--- /dev/null
+++ b/drivers/staging/rtl8187se/ieee80211/rtl_crypto.h
@@ -0,0 +1,399 @@
+/*
+ * Scatterlist Cryptographic API.
+ *
+ * Copyright (c) 2002 James Morris <jmorris@intercode.com.au>
+ * Copyright (c) 2002 David S. Miller (davem@redhat.com)
+ *
+ * Portions derived from Cryptoapi, by Alexander Kjeldaas <astor@fast.no>
+ * and Nettle, by Niels Mé°ˆler.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the Free
+ * Software Foundation; either version 2 of the License, or (at your option)
+ * any later version.
+ *
+ */
+#ifndef _LINUX_CRYPTO_H
+#define _LINUX_CRYPTO_H
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/list.h>
+#include <linux/string.h>
+#include <asm/page.h>
+#include <asm/errno.h>
+
+#define crypto_register_alg crypto_register_alg_rtl
+#define crypto_unregister_alg crypto_unregister_alg_rtl
+#define crypto_alloc_tfm crypto_alloc_tfm_rtl
+#define crypto_free_tfm crypto_free_tfm_rtl
+#define crypto_alg_available crypto_alg_available_rtl
+
+/*
+ * Algorithm masks and types.
+ */
+#define CRYPTO_ALG_TYPE_MASK		0x000000ff
+#define CRYPTO_ALG_TYPE_CIPHER		0x00000001
+#define CRYPTO_ALG_TYPE_DIGEST		0x00000002
+#define CRYPTO_ALG_TYPE_COMPRESS	0x00000004
+
+/*
+ * Transform masks and values (for crt_flags).
+ */
+#define CRYPTO_TFM_MODE_MASK		0x000000ff
+#define CRYPTO_TFM_REQ_MASK		0x000fff00
+#define CRYPTO_TFM_RES_MASK		0xfff00000
+
+#define CRYPTO_TFM_MODE_ECB		0x00000001
+#define CRYPTO_TFM_MODE_CBC		0x00000002
+#define CRYPTO_TFM_MODE_CFB		0x00000004
+#define CRYPTO_TFM_MODE_CTR		0x00000008
+
+#define CRYPTO_TFM_REQ_WEAK_KEY		0x00000100
+#define CRYPTO_TFM_RES_WEAK_KEY		0x00100000
+#define CRYPTO_TFM_RES_BAD_KEY_LEN   	0x00200000
+#define CRYPTO_TFM_RES_BAD_KEY_SCHED 	0x00400000
+#define CRYPTO_TFM_RES_BAD_BLOCK_LEN 	0x00800000
+#define CRYPTO_TFM_RES_BAD_FLAGS 	0x01000000
+
+/*
+ * Miscellaneous stuff.
+ */
+#define CRYPTO_UNSPEC			0
+#define CRYPTO_MAX_ALG_NAME		64
+
+struct scatterlist;
+
+/*
+ * Algorithms: modular crypto algorithm implementations, managed
+ * via crypto_register_alg() and crypto_unregister_alg().
+ */
+struct cipher_alg {
+	unsigned int cia_min_keysize;
+	unsigned int cia_max_keysize;
+	int (*cia_setkey)(void *ctx, const u8 *key,
+	                  unsigned int keylen, u32 *flags);
+	void (*cia_encrypt)(void *ctx, u8 *dst, const u8 *src);
+	void (*cia_decrypt)(void *ctx, u8 *dst, const u8 *src);
+};
+
+struct digest_alg {
+	unsigned int dia_digestsize;
+	void (*dia_init)(void *ctx);
+	void (*dia_update)(void *ctx, const u8 *data, unsigned int len);
+	void (*dia_final)(void *ctx, u8 *out);
+	int (*dia_setkey)(void *ctx, const u8 *key,
+	                  unsigned int keylen, u32 *flags);
+};
+
+struct compress_alg {
+	int (*coa_init)(void *ctx);
+	void (*coa_exit)(void *ctx);
+	int (*coa_compress)(void *ctx, const u8 *src, unsigned int slen,
+	                    u8 *dst, unsigned int *dlen);
+	int (*coa_decompress)(void *ctx, const u8 *src, unsigned int slen,
+	                      u8 *dst, unsigned int *dlen);
+};
+
+#define cra_cipher	cra_u.cipher
+#define cra_digest	cra_u.digest
+#define cra_compress	cra_u.compress
+
+struct crypto_alg {
+	struct list_head cra_list;
+	u32 cra_flags;
+	unsigned int cra_blocksize;
+	unsigned int cra_ctxsize;
+	const char cra_name[CRYPTO_MAX_ALG_NAME];
+
+	union {
+		struct cipher_alg cipher;
+		struct digest_alg digest;
+		struct compress_alg compress;
+	} cra_u;
+
+	struct module *cra_module;
+};
+
+/*
+ * Algorithm registration interface.
+ */
+int crypto_register_alg(struct crypto_alg *alg);
+int crypto_unregister_alg(struct crypto_alg *alg);
+
+/*
+ * Algorithm query interface.
+ */
+int crypto_alg_available(const char *name, u32 flags);
+
+/*
+ * Transforms: user-instantiated objects which encapsulate algorithms
+ * and core processing logic.  Managed via crypto_alloc_tfm() and
+ * crypto_free_tfm(), as well as the various helpers below.
+ */
+struct crypto_tfm;
+
+struct cipher_tfm {
+	void *cit_iv;
+	unsigned int cit_ivsize;
+	u32 cit_mode;
+	int (*cit_setkey)(struct crypto_tfm *tfm,
+	                  const u8 *key, unsigned int keylen);
+	int (*cit_encrypt)(struct crypto_tfm *tfm,
+			   struct scatterlist *dst,
+			   struct scatterlist *src,
+			   unsigned int nbytes);
+	int (*cit_encrypt_iv)(struct crypto_tfm *tfm,
+	                      struct scatterlist *dst,
+	                      struct scatterlist *src,
+	                      unsigned int nbytes, u8 *iv);
+	int (*cit_decrypt)(struct crypto_tfm *tfm,
+			   struct scatterlist *dst,
+			   struct scatterlist *src,
+			   unsigned int nbytes);
+	int (*cit_decrypt_iv)(struct crypto_tfm *tfm,
+			   struct scatterlist *dst,
+			   struct scatterlist *src,
+			   unsigned int nbytes, u8 *iv);
+	void (*cit_xor_block)(u8 *dst, const u8 *src);
+};
+
+struct digest_tfm {
+	void (*dit_init)(struct crypto_tfm *tfm);
+	void (*dit_update)(struct crypto_tfm *tfm,
+	                   struct scatterlist *sg, unsigned int nsg);
+	void (*dit_final)(struct crypto_tfm *tfm, u8 *out);
+	void (*dit_digest)(struct crypto_tfm *tfm, struct scatterlist *sg,
+	                   unsigned int nsg, u8 *out);
+	int (*dit_setkey)(struct crypto_tfm *tfm,
+	                  const u8 *key, unsigned int keylen);
+#ifdef CONFIG_CRYPTO_HMAC
+	void *dit_hmac_block;
+#endif
+};
+
+struct compress_tfm {
+	int (*cot_compress)(struct crypto_tfm *tfm,
+	                    const u8 *src, unsigned int slen,
+	                    u8 *dst, unsigned int *dlen);
+	int (*cot_decompress)(struct crypto_tfm *tfm,
+	                      const u8 *src, unsigned int slen,
+	                      u8 *dst, unsigned int *dlen);
+};
+
+#define crt_cipher	crt_u.cipher
+#define crt_digest	crt_u.digest
+#define crt_compress	crt_u.compress
+
+struct crypto_tfm {
+
+	u32 crt_flags;
+
+	union {
+		struct cipher_tfm cipher;
+		struct digest_tfm digest;
+		struct compress_tfm compress;
+	} crt_u;
+
+	struct crypto_alg *__crt_alg;
+};
+
+/*
+ * Transform user interface.
+ */
+
+/*
+ * crypto_alloc_tfm() will first attempt to locate an already loaded algorithm.
+ * If that fails and the kernel supports dynamically loadable modules, it
+ * will then attempt to load a module of the same name or alias.  A refcount
+ * is grabbed on the algorithm which is then associated with the new transform.
+ *
+ * crypto_free_tfm() frees up the transform and any associated resources,
+ * then drops the refcount on the associated algorithm.
+ */
+struct crypto_tfm *crypto_alloc_tfm(const char *alg_name, u32 tfm_flags);
+void crypto_free_tfm(struct crypto_tfm *tfm);
+
+/*
+ * Transform helpers which query the underlying algorithm.
+ */
+static inline const char *crypto_tfm_alg_name(struct crypto_tfm *tfm)
+{
+	return tfm->__crt_alg->cra_name;
+}
+
+static inline const char *crypto_tfm_alg_modname(struct crypto_tfm *tfm)
+{
+	struct crypto_alg *alg = tfm->__crt_alg;
+
+	if (alg->cra_module)
+		return alg->cra_module->name;
+	else
+		return NULL;
+}
+
+static inline u32 crypto_tfm_alg_type(struct crypto_tfm *tfm)
+{
+	return tfm->__crt_alg->cra_flags & CRYPTO_ALG_TYPE_MASK;
+}
+
+static inline unsigned int crypto_tfm_alg_min_keysize(struct crypto_tfm *tfm)
+{
+	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_CIPHER);
+	return tfm->__crt_alg->cra_cipher.cia_min_keysize;
+}
+
+static inline unsigned int crypto_tfm_alg_max_keysize(struct crypto_tfm *tfm)
+{
+	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_CIPHER);
+	return tfm->__crt_alg->cra_cipher.cia_max_keysize;
+}
+
+static inline unsigned int crypto_tfm_alg_ivsize(struct crypto_tfm *tfm)
+{
+	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_CIPHER);
+	return tfm->crt_cipher.cit_ivsize;
+}
+
+static inline unsigned int crypto_tfm_alg_blocksize(struct crypto_tfm *tfm)
+{
+	return tfm->__crt_alg->cra_blocksize;
+}
+
+static inline unsigned int crypto_tfm_alg_digestsize(struct crypto_tfm *tfm)
+{
+	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_DIGEST);
+	return tfm->__crt_alg->cra_digest.dia_digestsize;
+}
+
+/*
+ * API wrappers.
+ */
+static inline void crypto_digest_init(struct crypto_tfm *tfm)
+{
+	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_DIGEST);
+	tfm->crt_digest.dit_init(tfm);
+}
+
+static inline void crypto_digest_update(struct crypto_tfm *tfm,
+                                        struct scatterlist *sg,
+                                        unsigned int nsg)
+{
+	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_DIGEST);
+	tfm->crt_digest.dit_update(tfm, sg, nsg);
+}
+
+static inline void crypto_digest_final(struct crypto_tfm *tfm, u8 *out)
+{
+	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_DIGEST);
+	tfm->crt_digest.dit_final(tfm, out);
+}
+
+static inline void crypto_digest_digest(struct crypto_tfm *tfm,
+                                        struct scatterlist *sg,
+                                        unsigned int nsg, u8 *out)
+{
+	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_DIGEST);
+	tfm->crt_digest.dit_digest(tfm, sg, nsg, out);
+}
+
+static inline int crypto_digest_setkey(struct crypto_tfm *tfm,
+                                       const u8 *key, unsigned int keylen)
+{
+	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_DIGEST);
+	if (tfm->crt_digest.dit_setkey == NULL)
+		return -ENOSYS;
+	return tfm->crt_digest.dit_setkey(tfm, key, keylen);
+}
+
+static inline int crypto_cipher_setkey(struct crypto_tfm *tfm,
+                                       const u8 *key, unsigned int keylen)
+{
+	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_CIPHER);
+	return tfm->crt_cipher.cit_setkey(tfm, key, keylen);
+}
+
+static inline int crypto_cipher_encrypt(struct crypto_tfm *tfm,
+                                        struct scatterlist *dst,
+                                        struct scatterlist *src,
+                                        unsigned int nbytes)
+{
+	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_CIPHER);
+	return tfm->crt_cipher.cit_encrypt(tfm, dst, src, nbytes);
+}
+
+static inline int crypto_cipher_encrypt_iv(struct crypto_tfm *tfm,
+                                           struct scatterlist *dst,
+                                           struct scatterlist *src,
+                                           unsigned int nbytes, u8 *iv)
+{
+	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_CIPHER);
+	BUG_ON(tfm->crt_cipher.cit_mode == CRYPTO_TFM_MODE_ECB);
+	return tfm->crt_cipher.cit_encrypt_iv(tfm, dst, src, nbytes, iv);
+}
+
+static inline int crypto_cipher_decrypt(struct crypto_tfm *tfm,
+                                        struct scatterlist *dst,
+                                        struct scatterlist *src,
+                                        unsigned int nbytes)
+{
+	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_CIPHER);
+	return tfm->crt_cipher.cit_decrypt(tfm, dst, src, nbytes);
+}
+
+static inline int crypto_cipher_decrypt_iv(struct crypto_tfm *tfm,
+                                           struct scatterlist *dst,
+                                           struct scatterlist *src,
+                                           unsigned int nbytes, u8 *iv)
+{
+	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_CIPHER);
+	BUG_ON(tfm->crt_cipher.cit_mode == CRYPTO_TFM_MODE_ECB);
+	return tfm->crt_cipher.cit_decrypt_iv(tfm, dst, src, nbytes, iv);
+}
+
+static inline void crypto_cipher_set_iv(struct crypto_tfm *tfm,
+                                        const u8 *src, unsigned int len)
+{
+	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_CIPHER);
+	memcpy(tfm->crt_cipher.cit_iv, src, len);
+}
+
+static inline void crypto_cipher_get_iv(struct crypto_tfm *tfm,
+                                        u8 *dst, unsigned int len)
+{
+	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_CIPHER);
+	memcpy(dst, tfm->crt_cipher.cit_iv, len);
+}
+
+static inline int crypto_comp_compress(struct crypto_tfm *tfm,
+                                       const u8 *src, unsigned int slen,
+                                       u8 *dst, unsigned int *dlen)
+{
+	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_COMPRESS);
+	return tfm->crt_compress.cot_compress(tfm, src, slen, dst, dlen);
+}
+
+static inline int crypto_comp_decompress(struct crypto_tfm *tfm,
+                                         const u8 *src, unsigned int slen,
+                                         u8 *dst, unsigned int *dlen)
+{
+	BUG_ON(crypto_tfm_alg_type(tfm) != CRYPTO_ALG_TYPE_COMPRESS);
+	return tfm->crt_compress.cot_decompress(tfm, src, slen, dst, dlen);
+}
+
+/*
+ * HMAC support.
+ */
+#ifdef CONFIG_CRYPTO_HMAC
+void crypto_hmac_init(struct crypto_tfm *tfm, u8 *key, unsigned int *keylen);
+void crypto_hmac_update(struct crypto_tfm *tfm,
+                        struct scatterlist *sg, unsigned int nsg);
+void crypto_hmac_final(struct crypto_tfm *tfm, u8 *key,
+                       unsigned int *keylen, u8 *out);
+void crypto_hmac(struct crypto_tfm *tfm, u8 *key, unsigned int *keylen,
+                 struct scatterlist *sg, unsigned int nsg, u8 *out);
+#endif	/* CONFIG_CRYPTO_HMAC */
+
+#endif	/* _LINUX_CRYPTO_H */
+
diff --git a/drivers/staging/rtl8187se/ieee80211_crypt.h b/drivers/staging/rtl8187se/ieee80211_crypt.h
new file mode 100644
index 00000000000..b58a3bcc0dc
--- /dev/null
+++ b/drivers/staging/rtl8187se/ieee80211_crypt.h
@@ -0,0 +1,86 @@
+/*
+ * Original code based on Host AP (software wireless LAN access point) driver
+ * for Intersil Prism2/2.5/3.
+ *
+ * Copyright (c) 2001-2002, SSH Communications Security Corp and Jouni Malinen
+ * <jkmaline@cc.hut.fi>
+ * Copyright (c) 2002-2003, Jouni Malinen <jkmaline@cc.hut.fi>
+ *
+ * Adaption to a generic IEEE 802.11 stack by James Ketrenos
+ * <jketreno@linux.intel.com>
+ *
+ * Copyright (c) 2004, Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation. See README and COPYING for
+ * more details.
+ */
+
+/*
+ * This file defines the interface to the ieee80211 crypto module.
+ */
+#ifndef IEEE80211_CRYPT_H
+#define IEEE80211_CRYPT_H
+
+#include <linux/skbuff.h>
+
+struct ieee80211_crypto_ops {
+	const char *name;
+
+	/* init new crypto context (e.g., allocate private data space,
+	 * select IV, etc.); returns NULL on failure or pointer to allocated
+	 * private data on success */
+	void * (*init)(int keyidx);
+
+	/* deinitialize crypto context and free allocated private data */
+	void (*deinit)(void *priv);
+
+	/* encrypt/decrypt return < 0 on error or >= 0 on success. The return
+	 * value from decrypt_mpdu is passed as the keyidx value for
+	 * decrypt_msdu. skb must have enough head and tail room for the
+	 * encryption; if not, error will be returned; these functions are
+	 * called for all MPDUs (i.e., fragments).
+	 */
+	int (*encrypt_mpdu)(struct sk_buff *skb, int hdr_len, void *priv);
+	int (*decrypt_mpdu)(struct sk_buff *skb, int hdr_len, void *priv);
+
+	/* These functions are called for full MSDUs, i.e. full frames.
+	 * These can be NULL if full MSDU operations are not needed. */
+	int (*encrypt_msdu)(struct sk_buff *skb, int hdr_len, void *priv);
+	int (*decrypt_msdu)(struct sk_buff *skb, int keyidx, int hdr_len,
+			    void *priv);
+
+	int (*set_key)(void *key, int len, u8 *seq, void *priv);
+	int (*get_key)(void *key, int len, u8 *seq, void *priv);
+
+	/* procfs handler for printing out key information and possible
+	 * statistics */
+	char * (*print_stats)(char *p, void *priv);
+
+	/* maximum number of bytes added by encryption; encrypt buf is
+	 * allocated with extra_prefix_len bytes, copy of in_buf, and
+	 * extra_postfix_len; encrypt need not use all this space, but
+	 * the result must start at the beginning of the buffer and correct
+	 * length must be returned */
+	int extra_prefix_len, extra_postfix_len;
+
+	struct module *owner;
+};
+
+struct ieee80211_crypt_data {
+	struct list_head list; /* delayed deletion list */
+	struct ieee80211_crypto_ops *ops;
+	void *priv;
+	atomic_t refcnt;
+};
+
+int ieee80211_register_crypto_ops(struct ieee80211_crypto_ops *ops);
+int ieee80211_unregister_crypto_ops(struct ieee80211_crypto_ops *ops);
+struct ieee80211_crypto_ops * ieee80211_get_crypto_ops(const char *name);
+void ieee80211_crypt_deinit_entries(struct ieee80211_device *, int);
+void ieee80211_crypt_deinit_handler(unsigned long);
+void ieee80211_crypt_delayed_deinit(struct ieee80211_device *ieee,
+				    struct ieee80211_crypt_data **crypt);
+
+#endif
diff --git a/drivers/staging/rtl8187se/r8180.h b/drivers/staging/rtl8187se/r8180.h
new file mode 100644
index 00000000000..12215fc61dd
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180.h
@@ -0,0 +1,761 @@
+/*
+   This is part of rtl8180 OpenSource driver.
+   Copyright (C) Andrea Merello 2004-2005  <andreamrl@tiscali.it>
+   Released under the terms of GPL (General Public Licence)
+
+   Parts of this driver are based on the GPL part of the
+   official realtek driver
+
+   Parts of this driver are based on the rtl8180 driver skeleton
+   from Patric Schenke & Andres Salomon
+
+   Parts of this driver are based on the Intel Pro Wireless 2100 GPL driver
+
+   We want to tanks the Authors of those projects and the Ndiswrapper
+   project Authors.
+*/
+
+#ifndef R8180H
+#define R8180H
+
+
+#define RTL8180_MODULE_NAME "rtl8180"
+#define DMESG(x,a...) printk(KERN_INFO RTL8180_MODULE_NAME ": " x "\n", ## a)
+#define DMESGW(x,a...) printk(KERN_WARNING RTL8180_MODULE_NAME ": WW:" x "\n", ## a)
+#define DMESGE(x,a...) printk(KERN_WARNING RTL8180_MODULE_NAME ": EE:" x "\n", ## a)
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+//#include <linux/config.h>
+#include <linux/init.h>
+#include <linux/ioport.h>
+#include <linux/sched.h>
+#include <linux/types.h>
+#include <linux/slab.h>
+#include <linux/netdevice.h>
+#include <linux/pci.h>
+#include <linux/etherdevice.h>
+#include <linux/delay.h>
+#include <linux/rtnetlink.h>	//for rtnl_lock()
+#include <linux/wireless.h>
+#include <linux/timer.h>
+#include <linux/proc_fs.h>	// Necessary because we use the proc fs
+#include <linux/if_arp.h>
+#include "ieee80211.h"
+#include <asm/io.h>
+//#include <asm/semaphore.h>
+
+#define EPROM_93c46 0
+#define EPROM_93c56 1
+
+#define RTL_IOCTL_WPA_SUPPLICANT		SIOCIWFIRSTPRIV+30
+
+#define DEFAULT_FRAG_THRESHOLD 2342U
+#define MIN_FRAG_THRESHOLD     256U
+//#define	MAX_FRAG_THRESHOLD     2342U
+#define DEFAULT_RTS_THRESHOLD 2342U
+#define MIN_RTS_THRESHOLD 0U
+#define MAX_RTS_THRESHOLD 2342U
+#define DEFAULT_BEACONINTERVAL 0x64U
+#define DEFAULT_BEACON_ESSID "Rtl8180"
+
+#define DEFAULT_SSID ""
+#define DEFAULT_RETRY_RTS 7
+#define DEFAULT_RETRY_DATA 7
+#define PRISM_HDR_SIZE 64
+
+#ifdef CONFIG_RTL8185B
+
+#define MGNT_QUEUE						0
+#define BK_QUEUE						1
+#define BE_QUEUE						2
+#define VI_QUEUE						3
+#define VO_QUEUE						4
+#define HIGH_QUEUE						5
+#define BEACON_QUEUE					6
+
+#define LOW_QUEUE						BE_QUEUE
+#define NORMAL_QUEUE					MGNT_QUEUE
+
+#define aSifsTime 	10
+
+#define sCrcLng         4
+#define sAckCtsLng	112		// bits in ACK and CTS frames
+//+by amy 080312
+#define RATE_ADAPTIVE_TIMER_PERIOD	300
+
+typedef enum _WIRELESS_MODE {
+	WIRELESS_MODE_UNKNOWN = 0x00,
+	WIRELESS_MODE_A = 0x01,
+	WIRELESS_MODE_B = 0x02,
+	WIRELESS_MODE_G = 0x04,
+	WIRELESS_MODE_AUTO = 0x08,
+} WIRELESS_MODE;
+
+typedef enum _VERSION_8185{
+	// RTL8185
+	VERSION_8185_UNKNOWN,
+	VERSION_8185_C, // C-cut
+	VERSION_8185_D, // D-cut
+	// RTL8185B
+	VERSION_8185B_B, // B-cut
+	VERSION_8185B_D, // D-cut
+	VERSION_8185B_E, // E-cut
+	//RTL8187S-PCIE
+	VERSION_8187S_B, // B-cut
+	VERSION_8187S_C, // C-cut
+	VERSION_8187S_D, // D-cut
+
+}VERSION_8185,*PVERSION_8185;
+typedef struct 	ChnlAccessSetting {
+	u16 SIFS_Timer;
+	u16 DIFS_Timer;
+	u16 SlotTimeTimer;
+	u16 EIFS_Timer;
+	u16 CWminIndex;
+	u16 CWmaxIndex;
+}*PCHANNEL_ACCESS_SETTING,CHANNEL_ACCESS_SETTING;
+
+typedef enum{
+        NIC_8185 = 1,
+        NIC_8185B
+        } nic_t;
+
+typedef u32 AC_CODING;
+#define AC0_BE	0		// ACI: 0x00	// Best Effort
+#define AC1_BK	1		// ACI: 0x01	// Background
+#define AC2_VI	2		// ACI: 0x10	// Video
+#define AC3_VO	3		// ACI: 0x11	// Voice
+#define AC_MAX	4		// Max: define total number; Should not to be used as a real enum.
+
+//
+// ECWmin/ECWmax field.
+// Ref: WMM spec 2.2.2: WME Parameter Element, p.13.
+//
+typedef	union _ECW{
+	u8	charData;
+	struct
+	{
+		u8	ECWmin:4;
+		u8	ECWmax:4;
+	}f;	// Field
+}ECW, *PECW;
+
+//
+// ACI/AIFSN Field.
+// Ref: WMM spec 2.2.2: WME Parameter Element, p.12.
+//
+typedef	union _ACI_AIFSN{
+	u8	charData;
+
+	struct
+	{
+		u8	AIFSN:4;
+		u8	ACM:1;
+		u8	ACI:2;
+		u8	Reserved:1;
+	}f;	// Field
+}ACI_AIFSN, *PACI_AIFSN;
+
+//
+// AC Parameters Record Format.
+// Ref: WMM spec 2.2.2: WME Parameter Element, p.12.
+//
+typedef	union _AC_PARAM{
+	u32	longData;
+	u8	charData[4];
+
+	struct
+	{
+		ACI_AIFSN	AciAifsn;
+		ECW		Ecw;
+		u16		TXOPLimit;
+	}f;	// Field
+}AC_PARAM, *PAC_PARAM;
+
+/* it is a wrong definition. -xiong-2006-11-17
+typedef struct ThreeWireReg {
+	u16	longData;
+	struct {
+		u8	enableB;
+		u8	data;
+		u8	clk;
+		u8	read_write;
+	} struc;
+} ThreeWireReg;
+*/
+
+typedef	union _ThreeWire{
+	struct _ThreeWireStruc{
+		u16		data:1;
+		u16		clk:1;
+		u16		enableB:1;
+		u16		read_write:1;
+		u16		resv1:12;
+//		u2Byte	resv2:14;
+//		u2Byte	ThreeWireEnable:1;
+//		u2Byte	resv3:1;
+	}struc;
+	u16			longData;
+}ThreeWireReg;
+
+#endif
+
+typedef struct buffer
+{
+	struct buffer *next;
+	u32 *buf;
+	dma_addr_t dma;
+} buffer;
+
+//YJ,modified,080828
+typedef struct Stats
+{
+	unsigned long txrdu;
+	unsigned long rxrdu;
+	unsigned long rxnolast;
+	unsigned long rxnodata;
+//	unsigned long rxreset;
+//	unsigned long rxwrkaround;
+	unsigned long rxnopointer;
+	unsigned long txnperr;
+	unsigned long txresumed;
+	unsigned long rxerr;
+	unsigned long rxoverflow;
+	unsigned long rxint;
+	unsigned long txbkpokint;
+	unsigned long txbepoking;
+	unsigned long txbkperr;
+	unsigned long txbeperr;
+	unsigned long txnpokint;
+	unsigned long txhpokint;
+	unsigned long txhperr;
+	unsigned long ints;
+	unsigned long shints;
+	unsigned long txoverflow;
+	unsigned long rxdmafail;
+	unsigned long txbeacon;
+	unsigned long txbeaconerr;
+	unsigned long txlpokint;
+	unsigned long txlperr;
+	unsigned long txretry;//retry number  tony 20060601
+	unsigned long rxcrcerrmin;//crc error (0-500)
+	unsigned long rxcrcerrmid;//crc error (500-1000)
+	unsigned long rxcrcerrmax;//crc error (>1000)
+	unsigned long rxicverr;//ICV error
+} Stats;
+
+#define MAX_LD_SLOT_NUM 10
+#define KEEP_ALIVE_INTERVAL 				20 // in seconds.
+#define CHECK_FOR_HANG_PERIOD			2 //be equal to watchdog check time
+#define DEFAULT_KEEP_ALIVE_LEVEL			1
+#define DEFAULT_SLOT_NUM					2
+#define POWER_PROFILE_AC					0
+#define POWER_PROFILE_BATTERY			1
+
+typedef struct _link_detect_t
+{
+	u32				RxFrameNum[MAX_LD_SLOT_NUM];	// number of Rx Frame / CheckForHang_period  to determine link status
+	u16				SlotNum;	// number of CheckForHang period to determine link status, default is 2
+	u16				SlotIndex;
+
+	u32				NumTxOkInPeriod;  //number of packet transmitted during CheckForHang
+	u32				NumRxOkInPeriod;  //number of packet received during CheckForHang
+
+	u8				IdleCount;     // (KEEP_ALIVE_INTERVAL / CHECK_FOR_HANG_PERIOD)
+	u32				LastNumTxUnicast;
+	u32				LastNumRxUnicast;
+
+	bool				bBusyTraffic;    //when it is set to 1, UI cann't scan at will.
+}link_detect_t, *plink_detect_t;
+
+//YJ,modified,080828,end
+
+//by amy for led
+//================================================================================
+// LED customization.
+//================================================================================
+
+typedef	enum _LED_STRATEGY_8185{
+	SW_LED_MODE0, //
+	SW_LED_MODE1, //
+	HW_LED, // HW control 2 LEDs, LED0 and LED1 (there are 4 different control modes)
+}LED_STRATEGY_8185, *PLED_STRATEGY_8185;
+//by amy for led
+//by amy for power save
+typedef	enum _LED_CTL_MODE{
+	LED_CTL_POWER_ON = 1,
+	LED_CTL_LINK = 2,
+	LED_CTL_NO_LINK = 3,
+	LED_CTL_TX = 4,
+	LED_CTL_RX = 5,
+	LED_CTL_SITE_SURVEY = 6,
+	LED_CTL_POWER_OFF = 7
+}LED_CTL_MODE;
+
+typedef	enum _RT_RF_POWER_STATE
+{
+	eRfOn,
+	eRfSleep,
+	eRfOff
+}RT_RF_POWER_STATE;
+
+enum	_ReasonCode{
+	unspec_reason	= 0x1,
+	auth_not_valid	= 0x2,
+	deauth_lv_ss	= 0x3,
+	inactivity		= 0x4,
+	ap_overload		= 0x5,
+	class2_err		= 0x6,
+	class3_err		= 0x7,
+	disas_lv_ss		= 0x8,
+	asoc_not_auth	= 0x9,
+
+	//----MIC_CHECK
+	mic_failure		= 0xe,
+	//----END MIC_CHECK
+
+	// Reason code defined in 802.11i D10.0 p.28.
+	invalid_IE		= 0x0d,
+	four_way_tmout	= 0x0f,
+	two_way_tmout	= 0x10,
+	IE_dismatch		= 0x11,
+	invalid_Gcipher	= 0x12,
+	invalid_Pcipher	= 0x13,
+	invalid_AKMP	= 0x14,
+	unsup_RSNIEver = 0x15,
+	invalid_RSNIE	= 0x16,
+	auth_802_1x_fail= 0x17,
+	ciper_reject		= 0x18,
+
+	// Reason code defined in 7.3.1.7, 802.1e D13.0, p.42. Added by Annie, 2005-11-15.
+	QoS_unspec		= 0x20,	// 32
+	QAP_bandwidth	= 0x21,	// 33
+	poor_condition	= 0x22,	// 34
+	no_facility		= 0x23,	// 35
+							// Where is 36???
+	req_declined	= 0x25,	// 37
+	invalid_param	= 0x26,	// 38
+	req_not_honored= 0x27,	// 39
+	TS_not_created	= 0x2F,	// 47
+	DL_not_allowed	= 0x30,	// 48
+	dest_not_exist	= 0x31,	// 49
+	dest_not_QSTA	= 0x32,	// 50
+};
+typedef	enum _RT_PS_MODE
+{
+	eActive,	// Active/Continuous access.
+	eMaxPs,		// Max power save mode.
+	eFastPs		// Fast power save mode.
+}RT_PS_MODE;
+//by amy for power save
+typedef struct r8180_priv
+{
+	struct pci_dev *pdev;
+
+	short epromtype;
+	int irq;
+	struct ieee80211_device *ieee80211;
+
+        short card_8185; /* O: rtl8180, 1:rtl8185 V B/C, 2:rtl8185 V D, 3:rtl8185B */
+	short card_8185_Bversion; /* if TCR reports card V B/C this discriminates */
+	short phy_ver; /* meaningful for rtl8225 1:A 2:B 3:C */
+	short enable_gpio0;
+	enum card_type {PCI,MINIPCI,CARDBUS,USB/*rtl8187*/}card_type;
+	short hw_plcp_len;
+	short plcp_preamble_mode; // 0:auto 1:short 2:long
+
+	spinlock_t irq_lock;
+	spinlock_t irq_th_lock;
+	spinlock_t tx_lock;
+	spinlock_t ps_lock;
+	spinlock_t rf_ps_lock;
+
+	u16 irq_mask;
+	short irq_enabled;
+	struct net_device *dev;
+	short chan;
+	short sens;
+	short max_sens;
+	u8 chtxpwr[15]; //channels from 1 to 14, 0 not used
+	u8 chtxpwr_ofdm[15]; //channels from 1 to 14, 0 not used
+	//u8 challow[15]; //channels from 1 to 14, 0 not used
+	u8 channel_plan;  // it's the channel plan index
+	short up;
+	short crcmon; //if 1 allow bad crc frame reception in monitor mode
+	short prism_hdr;
+
+	struct timer_list scan_timer;
+	/*short scanpending;
+	short stopscan;*/
+	spinlock_t scan_lock;
+	u8 active_probe;
+	//u8 active_scan_num;
+	struct semaphore wx_sem;
+	struct semaphore rf_state;
+	short hw_wep;
+
+	short digphy;
+	short antb;
+	short diversity;
+	u8 cs_treshold;
+	short rcr_csense;
+	short rf_chip;
+	u32 key0[4];
+	short (*rf_set_sens)(struct net_device *dev,short sens);
+	void (*rf_set_chan)(struct net_device *dev,short ch);
+	void (*rf_close)(struct net_device *dev);
+	void (*rf_init)(struct net_device *dev);
+	void (*rf_sleep)(struct net_device *dev);
+	void (*rf_wakeup)(struct net_device *dev);
+	//short rate;
+	short promisc;
+	/*stats*/
+	struct Stats stats;
+	struct _link_detect_t link_detect;  //YJ,add,080828
+	struct iw_statistics wstats;
+	struct proc_dir_entry *dir_dev;
+
+	/*RX stuff*/
+	u32 *rxring;
+	u32 *rxringtail;
+	dma_addr_t rxringdma;
+	struct buffer *rxbuffer;
+	struct buffer *rxbufferhead;
+	int rxringcount;
+	u16 rxbuffersize;
+
+	struct sk_buff *rx_skb;
+
+	short rx_skb_complete;
+
+	u32 rx_prevlen;
+
+	/*TX stuff*/
+/*
+	u32 *txlpring;
+	u32 *txhpring;
+	u32 *txnpring;
+	dma_addr_t txlpringdma;
+	dma_addr_t txhpringdma;
+	dma_addr_t txnpringdma;
+	u32 *txlpringtail;
+	u32 *txhpringtail;
+	u32 *txnpringtail;
+	u32 *txlpringhead;
+	u32 *txhpringhead;
+	u32 *txnpringhead;
+	struct buffer *txlpbufs;
+	struct buffer *txhpbufs;
+	struct buffer *txnpbufs;
+	struct buffer *txlpbufstail;
+	struct buffer *txhpbufstail;
+	struct buffer *txnpbufstail;
+*/
+	u32 *txmapring;
+	u32 *txbkpring;
+	u32 *txbepring;
+	u32 *txvipring;
+	u32 *txvopring;
+	u32 *txhpring;
+	dma_addr_t txmapringdma;
+	dma_addr_t txbkpringdma;
+	dma_addr_t txbepringdma;
+	dma_addr_t txvipringdma;
+	dma_addr_t txvopringdma;
+	dma_addr_t txhpringdma;
+	u32 *txmapringtail;
+	u32 *txbkpringtail;
+	u32 *txbepringtail;
+	u32 *txvipringtail;
+	u32 *txvopringtail;
+	u32 *txhpringtail;
+	u32 *txmapringhead;
+	u32 *txbkpringhead;
+	u32 *txbepringhead;
+	u32 *txvipringhead;
+	u32 *txvopringhead;
+	u32 *txhpringhead;
+	struct buffer *txmapbufs;
+	struct buffer *txbkpbufs;
+	struct buffer *txbepbufs;
+	struct buffer *txvipbufs;
+	struct buffer *txvopbufs;
+	struct buffer *txhpbufs;
+	struct buffer *txmapbufstail;
+	struct buffer *txbkpbufstail;
+	struct buffer *txbepbufstail;
+	struct buffer *txvipbufstail;
+	struct buffer *txvopbufstail;
+	struct buffer *txhpbufstail;
+
+	int txringcount;
+	int txbuffsize;
+	//struct tx_pendingbuf txnp_pending;
+	//struct tasklet_struct irq_tx_tasklet;
+	struct tasklet_struct irq_rx_tasklet;
+	u8 dma_poll_mask;
+	//short tx_suspend;
+
+	/* adhoc/master mode stuff */
+	u32 *txbeaconringtail;
+	dma_addr_t txbeaconringdma;
+	u32 *txbeaconring;
+	int txbeaconcount;
+	struct buffer *txbeaconbufs;
+	struct buffer *txbeaconbufstail;
+	//char *master_essid;
+	//u16 master_beaconinterval;
+	//u32 master_beaconsize;
+	//u16 beacon_interval;
+
+	u8 retry_data;
+	u8 retry_rts;
+	u16 rts;
+
+//add for RF power on power off by lizhaoming 080512
+	u8	 RegThreeWireMode; // See "Three wire mode" defined above, 2006.05.31, by rcnjko.
+
+//by amy for led
+	LED_STRATEGY_8185 LedStrategy;
+//by amy for led
+
+//by amy for power save
+	struct timer_list watch_dog_timer;
+	bool bInactivePs;
+	bool bSwRfProcessing;
+	RT_RF_POWER_STATE	eInactivePowerState;
+	RT_RF_POWER_STATE eRFPowerState;
+	u32 RfOffReason;
+	bool RFChangeInProgress;
+	bool bInHctTest;
+	bool SetRFPowerStateInProgress;
+	u8   RFProgType;
+	bool bLeisurePs;
+	RT_PS_MODE dot11PowerSaveMode;
+	//u32 NumRxOkInPeriod;   //YJ,del,080828
+	//u32 NumTxOkInPeriod;   //YJ,del,080828
+	u8   TxPollingTimes;
+
+	bool	bApBufOurFrame;// TRUE if AP buffer our unicast data , we will keep eAwake untill receive data or timeout.
+	u8	WaitBufDataBcnCount;
+	u8	WaitBufDataTimeOut;
+
+//by amy for power save
+//by amy for antenna
+	u8 EEPROMSwAntennaDiversity;
+	bool EEPROMDefaultAntenna1;
+	u8 RegSwAntennaDiversityMechanism;
+	bool bSwAntennaDiverity;
+	u8 RegDefaultAntenna;
+	bool bDefaultAntenna1;
+	u8 SignalStrength;
+	long Stats_SignalStrength;
+	long LastSignalStrengthInPercent; // In percentange, used for smoothing, e.g. Moving Average.
+	u8	 SignalQuality; // in 0-100 index.
+	long Stats_SignalQuality;
+	long RecvSignalPower; // in dBm.
+	long Stats_RecvSignalPower;
+	u8	 LastRxPktAntenna;	// +by amy 080312 Antenn which received the lasted packet. 0: Aux, 1:Main. Added by Roger, 2008.01.25.
+	u32 AdRxOkCnt;
+	long AdRxSignalStrength;
+	u8 CurrAntennaIndex;			// Index to current Antenna (both Tx and Rx).
+	u8 AdTickCount;				// Times of SwAntennaDiversityTimer happened.
+	u8 AdCheckPeriod;				// # of period SwAntennaDiversityTimer to check Rx signal strength for SW Antenna Diversity.
+	u8 AdMinCheckPeriod;			// Min value of AdCheckPeriod.
+	u8 AdMaxCheckPeriod;			// Max value of AdCheckPeriod.
+	long AdRxSsThreshold;			// Signal strength threshold to switch antenna.
+	long AdMaxRxSsThreshold;			// Max value of AdRxSsThreshold.
+	bool bAdSwitchedChecking;		// TRUE if we shall shall check Rx signal strength for last time switching antenna.
+	long AdRxSsBeforeSwitched;		// Rx signal strength before we swithed antenna.
+	struct timer_list SwAntennaDiversityTimer;
+//by amy for antenna
+//{by amy 080312
+//
+	// Crystal calibration.
+	// Added by Roger, 2007.12.11.
+	//
+	bool		bXtalCalibration; // Crystal calibration.
+	u8			XtalCal_Xin; // Crystal calibration for Xin. 0~7.5pF
+	u8			XtalCal_Xout; // Crystal calibration for Xout. 0~7.5pF
+	//
+	// Tx power tracking with thermal meter indication.
+	// Added by Roger, 2007.12.11.
+	//
+	bool		bTxPowerTrack; // Tx Power tracking.
+	u8			ThermalMeter; // Thermal meter reference indication.
+	//
+	// Dynamic Initial Gain Adjustment Mechanism. Added by Bruce, 2007-02-14.
+	//
+	bool				bDigMechanism; // TRUE if DIG is enabled, FALSE ow.
+	bool				bRegHighPowerMechanism; // For High Power Mechanism. 061010, by rcnjko.
+	u32					FalseAlarmRegValue;
+	u8					RegDigOfdmFaUpTh; // Upper threhold of OFDM false alarm, which is used in DIG.
+	u8					DIG_NumberFallbackVote;
+	u8					DIG_NumberUpgradeVote;
+	// For HW antenna diversity, added by Roger, 2008.01.30.
+	u32			AdMainAntennaRxOkCnt;		// Main antenna Rx OK count.
+	u32			AdAuxAntennaRxOkCnt;		// Aux antenna Rx OK count.
+	bool		bHWAdSwitched;				// TRUE if we has switched default antenna by HW evaluation.
+	// RF High Power upper/lower threshold.
+	u8					RegHiPwrUpperTh;
+	u8					RegHiPwrLowerTh;
+	// RF RSSI High Power upper/lower Threshold.
+	u8					RegRSSIHiPwrUpperTh;
+	u8					RegRSSIHiPwrLowerTh;
+	// Current CCK RSSI value to determine CCK high power, asked by SD3 DZ, by Bruce, 2007-04-12.
+	u8			CurCCKRSSI;
+	bool        bCurCCKPkt;
+	//
+	// High Power Mechanism. Added by amy, 080312.
+	//
+	bool					bToUpdateTxPwr;
+	long					UndecoratedSmoothedSS;
+	long					UndercorateSmoothedRxPower;
+	u8						RSSI;
+	char					RxPower;
+	 u8 InitialGain;
+	 //For adjust Dig Threshhold during Legacy/Leisure Power Save Mode
+	u32				DozePeriodInPast2Sec;
+	 // Don't access BB/RF under disable PLL situation.
+	u8					InitialGainBackUp;
+	 u8 RegBModeGainStage;
+//by amy for rate adaptive
+    struct timer_list rateadapter_timer;
+	u32    RateAdaptivePeriod;
+	bool   bEnhanceTxPwr;
+	bool   bUpdateARFR;
+	int	   ForcedDataRate; // Force Data Rate. 0: Auto, 0x02: 1M ~ 0x6C: 54M.)
+	u32     NumTxUnicast; //YJ,add,080828,for keep alive
+	u8      keepAliveLevel; //YJ,add,080828,for KeepAlive
+	unsigned long 	NumTxOkTotal;
+	u16                                 LastRetryCnt;
+        u16                                     LastRetryRate;
+        unsigned long       LastTxokCnt;
+        unsigned long           LastRxokCnt;
+        u16                                     CurrRetryCnt;
+        unsigned long           LastTxOKBytes;
+	unsigned long 		    NumTxOkBytesTotal;
+        u8                          LastFailTxRate;
+        long                        LastFailTxRateSS;
+        u8                          FailTxRateCount;
+        u32                         LastTxThroughput;
+        //for up rate
+        unsigned short          bTryuping;
+        u8                                      CurrTxRate;     //the rate before up
+        u16                                     CurrRetryRate;
+        u16                                     TryupingCount;
+        u8                                      TryDownCountLowData;
+        u8                                      TryupingCountNoData;
+
+        u8                  CurrentOperaRate;
+//by amy for rate adaptive
+//by amy 080312}
+//	short wq_hurryup;
+//	struct workqueue_struct *workqueue;
+	struct work_struct reset_wq;
+	struct work_struct watch_dog_wq;
+	struct work_struct tx_irq_wq;
+	short ack_tx_to_ieee;
+
+	u8 PowerProfile;
+#ifdef CONFIG_RTL8185B
+	u32 CSMethod;
+	u8 cck_txpwr_base;
+	u8 ofdm_txpwr_base;
+	u8 dma_poll_stop_mask;
+
+	//u8 RegThreeWireMode;
+	u8 MWIEnable;
+	u16 ShortRetryLimit;
+	u16 LongRetryLimit;
+	u16 EarlyRxThreshold;
+	u32 TransmitConfig;
+	u32 ReceiveConfig;
+	u32 IntrMask;
+
+	struct 	ChnlAccessSetting  ChannelAccessSetting;
+#endif
+}r8180_priv;
+
+#define MANAGE_PRIORITY 0
+#define BK_PRIORITY 1
+#define BE_PRIORITY 2
+#define VI_PRIORITY 3
+#define VO_PRIORITY 4
+#define HI_PRIORITY 5
+#define BEACON_PRIORITY 6
+
+#define LOW_PRIORITY VI_PRIORITY
+#define NORM_PRIORITY VO_PRIORITY
+//AC2Queue mapping
+#define AC2Q(_ac) (((_ac) == WME_AC_VO) ? VO_PRIORITY : \
+		((_ac) == WME_AC_VI) ? VI_PRIORITY : \
+		((_ac) == WME_AC_BK) ? BK_PRIORITY : \
+		BE_PRIORITY)
+
+short rtl8180_tx(struct net_device *dev,u8* skbuf, int len,int priority,
+	short morefrag,short fragdesc,int rate);
+
+u8 read_nic_byte(struct net_device *dev, int x);
+u32 read_nic_dword(struct net_device *dev, int x);
+u16 read_nic_word(struct net_device *dev, int x) ;
+void write_nic_byte(struct net_device *dev, int x,u8 y);
+void write_nic_word(struct net_device *dev, int x,u16 y);
+void write_nic_dword(struct net_device *dev, int x,u32 y);
+void force_pci_posting(struct net_device *dev);
+
+void rtl8180_rtx_disable(struct net_device *);
+void rtl8180_rx_enable(struct net_device *);
+void rtl8180_tx_enable(struct net_device *);
+void rtl8180_start_scanning(struct net_device *dev);
+void rtl8180_start_scanning_s(struct net_device *dev);
+void rtl8180_stop_scanning(struct net_device *dev);
+void rtl8180_disassociate(struct net_device *dev);
+//void fix_rx_fifo(struct net_device *dev);
+void rtl8180_set_anaparam(struct net_device *dev,u32 a);
+void rtl8185_set_anaparam2(struct net_device *dev,u32 a);
+void rtl8180_set_hw_wep(struct net_device *dev);
+void rtl8180_no_hw_wep(struct net_device *dev);
+void rtl8180_update_msr(struct net_device *dev);
+//void rtl8180_BSS_create(struct net_device *dev);
+void rtl8180_beacon_tx_disable(struct net_device *dev);
+void rtl8180_beacon_rx_disable(struct net_device *dev);
+void rtl8180_conttx_enable(struct net_device *dev);
+void rtl8180_conttx_disable(struct net_device *dev);
+int rtl8180_down(struct net_device *dev);
+int rtl8180_up(struct net_device *dev);
+void rtl8180_commit(struct net_device *dev);
+void rtl8180_set_chan(struct net_device *dev,short ch);
+void rtl8180_set_master_essid(struct net_device *dev,char *essid);
+void rtl8180_update_beacon_security(struct net_device *dev);
+void write_phy(struct net_device *dev, u8 adr, u8 data);
+void write_phy_cck(struct net_device *dev, u8 adr, u32 data);
+void write_phy_ofdm(struct net_device *dev, u8 adr, u32 data);
+void rtl8185_tx_antenna(struct net_device *dev, u8 ant);
+void rtl8185_rf_pins_enable(struct net_device *dev);
+void IBSS_randomize_cell(struct net_device *dev);
+void IPSEnter(struct net_device *dev);
+void IPSLeave(struct net_device *dev);
+int get_curr_tx_free_desc(struct net_device *dev, int priority);
+void UpdateInitialGain(struct net_device *dev);
+bool SetAntennaConfig87SE(struct net_device *dev, u8  DefaultAnt, bool bAntDiversity);
+
+//#ifdef CONFIG_RTL8185B
+void rtl8185b_adapter_start(struct net_device *dev);
+void rtl8185b_rx_enable(struct net_device *dev);
+void rtl8185b_tx_enable(struct net_device *dev);
+void rtl8180_reset(struct net_device *dev);
+void rtl8185b_irq_enable(struct net_device *dev);
+void fix_rx_fifo(struct net_device *dev);
+void fix_tx_fifo(struct net_device *dev);
+void rtl8225z2_SetTXPowerLevel(struct net_device *dev, short ch);
+#if LINUX_VERSION_CODE >=KERNEL_VERSION(2,6,20)
+void rtl8180_rate_adapter(struct work_struct * work);
+#else
+void rtl8180_rate_adapter(struct net_device *dev);
+#endif
+//#endif
+bool MgntActSet_RF_State(struct net_device *dev, RT_RF_POWER_STATE StateToSet, u32 ChangeSource);
+
+#endif
diff --git a/drivers/staging/rtl8187se/r8180_93cx6.c b/drivers/staging/rtl8187se/r8180_93cx6.c
new file mode 100644
index 00000000000..7e4711fb930
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_93cx6.c
@@ -0,0 +1,146 @@
+/*
+   This files contains card eeprom (93c46 or 93c56) programming routines,
+   memory is addressed by 16 bits words.
+
+   This is part of rtl8180 OpenSource driver.
+   Copyright (C) Andrea Merello 2004  <andreamrl@tiscali.it>
+   Released under the terms of GPL (General Public Licence)
+
+   Parts of this driver are based on the GPL part of the
+   official realtek driver.
+
+   Parts of this driver are based on the rtl8180 driver skeleton
+   from Patric Schenke & Andres Salomon.
+
+   Parts of this driver are based on the Intel Pro Wireless 2100 GPL driver.
+
+   We want to tanks the Authors of those projects and the Ndiswrapper
+   project Authors.
+*/
+
+#include "r8180_93cx6.h"
+
+void eprom_cs(struct net_device *dev, short bit)
+{
+	if(bit)
+		write_nic_byte(dev, EPROM_CMD,
+			       (1<<EPROM_CS_SHIFT) | \
+			       read_nic_byte(dev, EPROM_CMD)); //enable EPROM
+	else
+		write_nic_byte(dev, EPROM_CMD, read_nic_byte(dev, EPROM_CMD)\
+			       &~(1<<EPROM_CS_SHIFT)); //disable EPROM
+
+	force_pci_posting(dev);
+	udelay(EPROM_DELAY);
+}
+
+
+void eprom_ck_cycle(struct net_device *dev)
+{
+	write_nic_byte(dev, EPROM_CMD,
+		       (1<<EPROM_CK_SHIFT) | read_nic_byte(dev,EPROM_CMD));
+	force_pci_posting(dev);
+	udelay(EPROM_DELAY);
+	write_nic_byte(dev, EPROM_CMD,
+		       read_nic_byte(dev, EPROM_CMD) &~ (1<<EPROM_CK_SHIFT));
+	force_pci_posting(dev);
+	udelay(EPROM_DELAY);
+}
+
+
+void eprom_w(struct net_device *dev,short bit)
+{
+	if(bit)
+		write_nic_byte(dev, EPROM_CMD, (1<<EPROM_W_SHIFT) | \
+			       read_nic_byte(dev,EPROM_CMD));
+	else
+		write_nic_byte(dev, EPROM_CMD, read_nic_byte(dev,EPROM_CMD)\
+			       &~(1<<EPROM_W_SHIFT));
+
+	force_pci_posting(dev);
+	udelay(EPROM_DELAY);
+}
+
+
+short eprom_r(struct net_device *dev)
+{
+	short bit;
+
+	bit=(read_nic_byte(dev, EPROM_CMD) & (1<<EPROM_R_SHIFT) );
+	udelay(EPROM_DELAY);
+
+	if(bit) return 1;
+	return 0;
+}
+
+
+void eprom_send_bits_string(struct net_device *dev, short b[], int len)
+{
+	int i;
+
+	for(i=0; i<len; i++){
+		eprom_w(dev, b[i]);
+		eprom_ck_cycle(dev);
+	}
+}
+
+
+u32 eprom_read(struct net_device *dev, u32 addr)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	short read_cmd[]={1,1,0};
+	short addr_str[8];
+	int i;
+	int addr_len;
+	u32 ret;
+
+	ret=0;
+        //enable EPROM programming
+	write_nic_byte(dev, EPROM_CMD,
+		       (EPROM_CMD_PROGRAM<<EPROM_CMD_OPERATING_MODE_SHIFT));
+	force_pci_posting(dev);
+	udelay(EPROM_DELAY);
+
+	if (priv->epromtype==EPROM_93c56){
+		addr_str[7]=addr & 1;
+		addr_str[6]=addr & (1<<1);
+		addr_str[5]=addr & (1<<2);
+		addr_str[4]=addr & (1<<3);
+		addr_str[3]=addr & (1<<4);
+		addr_str[2]=addr & (1<<5);
+		addr_str[1]=addr & (1<<6);
+		addr_str[0]=addr & (1<<7);
+		addr_len=8;
+	}else{
+		addr_str[5]=addr & 1;
+		addr_str[4]=addr & (1<<1);
+		addr_str[3]=addr & (1<<2);
+		addr_str[2]=addr & (1<<3);
+		addr_str[1]=addr & (1<<4);
+		addr_str[0]=addr & (1<<5);
+		addr_len=6;
+	}
+	eprom_cs(dev, 1);
+	eprom_ck_cycle(dev);
+	eprom_send_bits_string(dev, read_cmd, 3);
+	eprom_send_bits_string(dev, addr_str, addr_len);
+
+	//keep chip pin D to low state while reading.
+	//I'm unsure if it is necessary, but anyway shouldn't hurt
+	eprom_w(dev, 0);
+
+	for(i=0;i<16;i++){
+		//eeprom needs a clk cycle between writing opcode&adr
+		//and reading data. (eeprom outs a dummy 0)
+		eprom_ck_cycle(dev);
+		ret |= (eprom_r(dev)<<(15-i));
+	}
+
+	eprom_cs(dev, 0);
+	eprom_ck_cycle(dev);
+
+	//disable EPROM programming
+	write_nic_byte(dev, EPROM_CMD,
+		       (EPROM_CMD_NORMAL<<EPROM_CMD_OPERATING_MODE_SHIFT));
+	return ret;
+}
diff --git a/drivers/staging/rtl8187se/r8180_93cx6.h b/drivers/staging/rtl8187se/r8180_93cx6.h
new file mode 100644
index 00000000000..a028a51b23f
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_93cx6.h
@@ -0,0 +1,59 @@
+/*
+	This is part of rtl8180 OpenSource driver
+	Copyright (C) Andrea Merello 2004-2005  <andreamrl@tiscali.it>
+	Released under the terms of GPL (General Public Licence)
+
+	Parts of this driver are based on the GPL part of the official realtek driver
+	Parts of this driver are based on the rtl8180 driver skeleton from Patric Schenke & Andres Salomon
+	Parts of this driver are based on the Intel Pro Wireless 2100 GPL driver
+
+	We want to tanks the Authors of such projects and the Ndiswrapper project Authors.
+*/
+
+/*This files contains card eeprom (93c46 or 93c56) programming routines*/
+/*memory is addressed by WORDS*/
+
+#include "r8180.h"
+#include "r8180_hw.h"
+
+#define EPROM_DELAY 10
+
+#define EPROM_ANAPARAM_ADDRLWORD 0xd
+#define EPROM_ANAPARAM_ADDRHWORD 0xe
+
+#define RFCHIPID 0x6
+#define	RFCHIPID_INTERSIL 1
+#define	RFCHIPID_RFMD 2
+#define	RFCHIPID_PHILIPS 3
+#define	RFCHIPID_MAXIM 4
+#define	RFCHIPID_GCT 5
+#define RFCHIPID_RTL8225 9
+#ifdef CONFIG_RTL8185B
+#define RF_ZEBRA2 11
+#define EPROM_TXPW_BASE 0x05
+#define RF_ZEBRA4 12
+#endif
+#define RFCHIPID_RTL8255 0xa
+#define RF_PARAM 0x19
+#define RF_PARAM_DIGPHY_SHIFT 0
+#define RF_PARAM_ANTBDEFAULT_SHIFT 1
+#define RF_PARAM_CARRIERSENSE_SHIFT 2
+#define RF_PARAM_CARRIERSENSE_MASK (3<<2)
+#define ENERGY_TRESHOLD 0x17
+#define EPROM_VERSION 0x1E
+#define MAC_ADR 0x7
+
+#define CIS 0x18
+
+#define	EPROM_TXPW_OFDM_CH1_2 0x20
+
+//#define	EPROM_TXPW_CH1_2 0x10
+#define  EPROM_TXPW_CH1_2 0x30
+#define	EPROM_TXPW_CH3_4 0x11
+#define	EPROM_TXPW_CH5_6 0x12
+#define	EPROM_TXPW_CH7_8 0x13
+#define	EPROM_TXPW_CH9_10 0x14
+#define	EPROM_TXPW_CH11_12 0x15
+#define	EPROM_TXPW_CH13_14 0x16
+
+u32 eprom_read(struct net_device *dev,u32 addr); //reads a 16 bits word
diff --git a/drivers/staging/rtl8187se/r8180_core.c b/drivers/staging/rtl8187se/r8180_core.c
new file mode 100644
index 00000000000..00f4df49bc0
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_core.c
@@ -0,0 +1,6828 @@
+/*
+   This is part of rtl818x pci OpenSource driver - v 0.1
+   Copyright (C) Andrea Merello 2004-2005  <andreamrl@tiscali.it>
+   Released under the terms of GPL (General Public License)
+
+   Parts of this driver are based on the GPL part of the official
+   Realtek driver.
+
+   Parts of this driver are based on the rtl8180 driver skeleton
+   from Patric Schenke & Andres Salomon.
+
+   Parts of this driver are based on the Intel Pro Wireless 2100 GPL driver.
+
+   Parts of BB/RF code are derived from David Young rtl8180 netbsd driver.
+
+   RSSI calc function from 'The Deuce'
+
+   Some ideas borrowed from the 8139too.c driver included in linux kernel.
+
+   We (I?) want to thanks the Authors of those projecs and also the
+   Ndiswrapper's project Authors.
+
+   A big big thanks goes also to Realtek corp. for their help in my attempt to
+   add RTL8185 and RTL8225 support, and to David Young also.
+*/
+
+#if 0
+double __floatsidf (int i) { return i; }
+unsigned int __fixunsdfsi (double d) { return d; }
+double __adddf3(double a, double b) { return a+b; }
+double __addsf3(float a, float b) { return a+b; }
+double __subdf3(double a, double b) { return a-b; }
+double __extendsfdf2(float a) {return a;}
+#endif
+
+
+#undef DEBUG_TX_DESC2
+#undef RX_DONT_PASS_UL
+#undef DEBUG_EPROM
+#undef DEBUG_RX_VERBOSE
+#undef DUMMY_RX
+#undef DEBUG_ZERO_RX
+#undef DEBUG_RX_SKB
+#undef DEBUG_TX_FRAG
+#undef DEBUG_RX_FRAG
+#undef DEBUG_TX_FILLDESC
+#undef DEBUG_TX
+#undef DEBUG_IRQ
+#undef DEBUG_RX
+#undef DEBUG_RXALLOC
+#undef DEBUG_REGISTERS
+#undef DEBUG_RING
+#undef DEBUG_IRQ_TASKLET
+#undef DEBUG_TX_ALLOC
+#undef DEBUG_TX_DESC
+
+//#define DEBUG_TX
+//#define DEBUG_TX_DESC2
+//#define DEBUG_RX
+//#define DEBUG_RX_SKB
+
+//#define CONFIG_RTL8180_IO_MAP
+#include <linux/syscalls.h>
+//#include <linux/fcntl.h>
+//#include <asm/uaccess.h>
+#include "r8180_hw.h"
+#include "r8180.h"
+#include "r8180_sa2400.h"  /* PHILIPS Radio frontend */
+#include "r8180_max2820.h" /* MAXIM Radio frontend */
+#include "r8180_gct.h"     /* GCT Radio frontend */
+#include "r8180_rtl8225.h" /* RTL8225 Radio frontend */
+#include "r8180_rtl8255.h" /* RTL8255 Radio frontend */
+#include "r8180_93cx6.h"   /* Card EEPROM */
+#include "r8180_wx.h"
+#include "r8180_dm.h"
+
+#ifdef CONFIG_RTL8180_PM
+#include "r8180_pm.h"
+#endif
+
+#ifdef ENABLE_DOT11D
+#include "dot11d.h"
+#endif
+
+#ifdef CONFIG_RTL8185B
+//#define CONFIG_RTL8180_IO_MAP
+#endif
+
+#ifndef PCI_VENDOR_ID_BELKIN
+	#define PCI_VENDOR_ID_BELKIN 0x1799
+#endif
+#ifndef PCI_VENDOR_ID_DLINK
+	#define PCI_VENDOR_ID_DLINK 0x1186
+#endif
+
+static struct pci_device_id rtl8180_pci_id_tbl[] __devinitdata = {
+        {
+                .vendor = PCI_VENDOR_ID_REALTEK,
+//                .device = 0x8180,
+                .device = 0x8199,
+                .subvendor = PCI_ANY_ID,
+                .subdevice = PCI_ANY_ID,
+                .driver_data = 0,
+        },
+#if 0
+        {
+                .vendor = PCI_VENDOR_ID_BELKIN,
+                .device = 0x6001,
+                .subvendor = PCI_ANY_ID,
+                .subdevice = PCI_ANY_ID,
+                .driver_data = 1,
+        },
+        {       /* Belkin F5D6020 v3 */
+	        .vendor = PCI_VENDOR_ID_BELKIN,
+                .device = 0x6020,
+                .subvendor = PCI_ANY_ID,
+                .subdevice = PCI_ANY_ID,
+                .driver_data = 2,
+        },
+        {       /* D-Link DWL-610 */
+                .vendor = PCI_VENDOR_ID_DLINK,
+                .device = 0x3300,
+                .subvendor = PCI_ANY_ID,
+                .subdevice = PCI_ANY_ID,
+                .driver_data = 3,
+        },
+	{
+		.vendor = PCI_VENDOR_ID_REALTEK,
+		.device = 0x8185,
+		.subvendor = PCI_ANY_ID,
+		.subdevice = PCI_ANY_ID,
+		.driver_data = 4,
+	},
+#endif
+        {
+                .vendor = 0,
+                .device = 0,
+                .subvendor = 0,
+                .subdevice = 0,
+                .driver_data = 0,
+        }
+};
+
+
+static char* ifname = "wlan%d";
+static int hwseqnum = 0;
+//static char* ifname = "ath%d";
+static int hwwep = 0;
+static int channels = 0x3fff;
+
+#define eqMacAddr(a,b)		( ((a)[0]==(b)[0] && (a)[1]==(b)[1] && (a)[2]==(b)[2] && (a)[3]==(b)[3] && (a)[4]==(b)[4] && (a)[5]==(b)[5]) ? 1:0 )
+#define cpMacAddr(des,src)	      ((des)[0]=(src)[0],(des)[1]=(src)[1],(des)[2]=(src)[2],(des)[3]=(src)[3],(des)[4]=(src)[4],(des)[5]=(src)[5])
+MODULE_LICENSE("GPL");
+MODULE_DEVICE_TABLE(pci, rtl8180_pci_id_tbl);
+MODULE_AUTHOR("Andrea Merello <andreamrl@tiscali.it>");
+MODULE_DESCRIPTION("Linux driver for Realtek RTL8180 / RTL8185 WiFi cards");
+
+
+
+/*
+MODULE_PARM(ifname, "s");
+MODULE_PARM_DESC(devname," Net interface name, wlan%d=default");
+
+MODULE_PARM(hwseqnum,"i");
+MODULE_PARM_DESC(hwseqnum," Try to use hardware 802.11 header sequence numbers. Zero=default");
+
+MODULE_PARM(hwwep,"i");
+MODULE_PARM_DESC(hwwep," Try to use hardware WEP support. Still broken and not available on all cards");
+
+MODULE_PARM(channels,"i");
+MODULE_PARM_DESC(channels," Channel bitmask for specific locales. NYI");
+*/
+
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(2, 6, 9)
+module_param(ifname, charp, S_IRUGO|S_IWUSR );
+module_param(hwseqnum,int, S_IRUGO|S_IWUSR);
+module_param(hwwep,int, S_IRUGO|S_IWUSR);
+module_param(channels,int, S_IRUGO|S_IWUSR);
+#else
+MODULE_PARM(ifname, "s");
+MODULE_PARM(hwseqnum,"i");
+MODULE_PARM(hwwep,"i");
+MODULE_PARM(channels,"i");
+#endif
+
+MODULE_PARM_DESC(devname," Net interface name, wlan%d=default");
+//MODULE_PARM_DESC(devname," Net interface name, ath%d=default");
+MODULE_PARM_DESC(hwseqnum," Try to use hardware 802.11 header sequence numbers. Zero=default");
+MODULE_PARM_DESC(hwwep," Try to use hardware WEP support. Still broken and not available on all cards");
+MODULE_PARM_DESC(channels," Channel bitmask for specific locales. NYI");
+
+
+static int __devinit rtl8180_pci_probe(struct pci_dev *pdev,
+				       const struct pci_device_id *id);
+
+static void __devexit rtl8180_pci_remove(struct pci_dev *pdev);
+
+static void rtl8180_shutdown (struct pci_dev *pdev)
+{
+	struct net_device *dev = pci_get_drvdata(pdev);
+	dev->stop(dev);
+	pci_disable_device(pdev);
+}
+
+static struct pci_driver rtl8180_pci_driver = {
+	.name		= RTL8180_MODULE_NAME,	          /* Driver name   */
+	.id_table	= rtl8180_pci_id_tbl,	          /* PCI_ID table  */
+	.probe		= rtl8180_pci_probe,	          /* probe fn      */
+	.remove		= __devexit_p(rtl8180_pci_remove),/* remove fn     */
+#ifdef CONFIG_RTL8180_PM
+	.suspend	= rtl8180_suspend,	          /* PM suspend fn */
+	.resume		= rtl8180_resume,                 /* PM resume fn  */
+#else
+	.suspend	= NULL,			          /* PM suspend fn */
+	.resume      	= NULL,			          /* PM resume fn  */
+#endif
+	.shutdown	= rtl8180_shutdown,
+};
+
+
+
+#ifdef CONFIG_RTL8180_IO_MAP
+
+u8 read_nic_byte(struct net_device *dev, int x)
+{
+        return 0xff&inb(dev->base_addr +x);
+}
+
+u32 read_nic_dword(struct net_device *dev, int x)
+{
+        return inl(dev->base_addr +x);
+}
+
+u16 read_nic_word(struct net_device *dev, int x)
+{
+        return inw(dev->base_addr +x);
+}
+
+void write_nic_byte(struct net_device *dev, int x,u8 y)
+{
+        outb(y&0xff,dev->base_addr +x);
+}
+
+void write_nic_word(struct net_device *dev, int x,u16 y)
+{
+        outw(y,dev->base_addr +x);
+}
+
+void write_nic_dword(struct net_device *dev, int x,u32 y)
+{
+        outl(y,dev->base_addr +x);
+}
+
+#else /* RTL_IO_MAP */
+
+u8 read_nic_byte(struct net_device *dev, int x)
+{
+        return 0xff&readb((u8*)dev->mem_start +x);
+}
+
+u32 read_nic_dword(struct net_device *dev, int x)
+{
+        return readl((u8*)dev->mem_start +x);
+}
+
+u16 read_nic_word(struct net_device *dev, int x)
+{
+        return readw((u8*)dev->mem_start +x);
+}
+
+void write_nic_byte(struct net_device *dev, int x,u8 y)
+{
+        writeb(y,(u8*)dev->mem_start +x);
+	udelay(20);
+}
+
+void write_nic_dword(struct net_device *dev, int x,u32 y)
+{
+        writel(y,(u8*)dev->mem_start +x);
+	udelay(20);
+}
+
+void write_nic_word(struct net_device *dev, int x,u16 y)
+{
+        writew(y,(u8*)dev->mem_start +x);
+	udelay(20);
+}
+
+#endif /* RTL_IO_MAP */
+
+
+
+
+
+inline void force_pci_posting(struct net_device *dev)
+{
+	read_nic_byte(dev,EPROM_CMD);
+#ifndef CONFIG_RTL8180_IO_MAP
+	mb();
+#endif
+}
+
+
+irqreturn_t rtl8180_interrupt(int irq, void *netdev, struct pt_regs *regs);
+void set_nic_rxring(struct net_device *dev);
+void set_nic_txring(struct net_device *dev);
+static struct net_device_stats *rtl8180_stats(struct net_device *dev);
+void rtl8180_commit(struct net_device *dev);
+void rtl8180_start_tx_beacon(struct net_device *dev);
+
+/****************************************************************************
+   -----------------------------PROCFS STUFF-------------------------
+*****************************************************************************/
+
+static struct proc_dir_entry *rtl8180_proc = NULL;
+
+static int proc_get_registers(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	struct net_device *dev = data;
+//	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+	int len = 0;
+	int i,n;
+
+	int max=0xff;
+
+	/* This dump the current register page */
+	for(n=0;n<=max;)
+	{
+		//printk( "\nD: %2x> ", n);
+		len += snprintf(page + len, count - len,
+			"\nD:  %2x > ",n);
+
+		for(i=0;i<16 && n<=max;i++,n++)
+		len += snprintf(page + len, count - len,
+			"%2x ",read_nic_byte(dev,n));
+
+		//	printk("%2x ",read_nic_byte(dev,n));
+	}
+	len += snprintf(page + len, count - len,"\n");
+
+
+
+	*eof = 1;
+	return len;
+
+}
+
+int get_curr_tx_free_desc(struct net_device *dev, int priority);
+
+static int proc_get_stats_hw(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	//struct net_device *dev = data;
+	//struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+	int len = 0;
+#ifdef 	CONFIG_RTL8185B
+
+#else
+	len += snprintf(page + len, count - len,
+		"NIC int: %lu\n"
+		"Total int: %lu\n"
+		"--------------------\n"
+		"LP avail desc %d\n"
+		"NP avail desc %d\n"
+		"--------------------\n"
+		"LP phys dma addr %x\n"
+		"LP NIC ptr %x\n"
+		"LP virt 32base %x\n"
+		"LP virt 32tail %x\n"
+		"--------------------\n"
+		"NP phys dma addr %x\n"
+		"NP NIC ptr %x\n"
+		"NP virt 32base %x\n"
+		"NP virt 32tail %x\n"
+		"--------------------\n"
+		"BP phys dma addr %x\n"
+		"BP NIC ptr %x\n"
+		"BP virt 32base %x\n"
+		"BP virt 32tail %x\n",
+		priv->stats.ints,
+		priv->stats.shints,
+		get_curr_tx_free_desc(dev,LOW_PRIORITY),
+		get_curr_tx_free_desc(dev,NORM_PRIORITY),
+		(u32)priv->txvipringdma,
+		read_nic_dword(dev,TLPDA),
+		(u32)priv->txvipring,
+		(u32)priv->txvipringtail,
+		(u32)priv->txvopringdma,
+		read_nic_dword(dev,TNPDA),
+		(u32)priv->txvopring,
+		(u32)priv->txvopringtail,
+		(u32)priv->txbeaconringdma,
+		read_nic_dword(dev,TBDA),
+		(u32)priv->txbeaconring,
+		(u32)priv->txbeaconringtail);
+#endif
+	*eof = 1;
+	return len;
+}
+
+
+static int proc_get_stats_rx(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	struct net_device *dev = data;
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+	int len = 0;
+
+	len += snprintf(page + len, count - len,
+	/*	"RX descriptor not available: %lu\n"
+		"RX incomplete (missing last descriptor): %lu\n"
+		"RX not data: %lu\n"
+		//"RX descriptor pointer reset: %lu\n"
+		"RX descriptor pointer lost: %lu\n"
+		//"RX pointer workaround: %lu\n"
+		"RX error int: %lu\n"
+		"RX fifo overflow: %lu\n"
+		"RX int: %lu\n"
+		"RX packet: %lu\n"
+		"RX bytes: %lu\n"
+		"RX DMA fail: %lu\n",
+		priv->stats.rxrdu,
+		priv->stats.rxnolast,
+		priv->stats.rxnodata,
+		//priv->stats.rxreset,
+		priv->stats.rxnopointer,
+		//priv->stats.rxwrkaround,
+		priv->stats.rxerr,
+		priv->stats.rxoverflow,
+		priv->stats.rxint,
+		priv->ieee80211->stats.rx_packets,
+		priv->ieee80211->stats.rx_bytes,
+		priv->stats.rxdmafail  */
+		"RX OK: %lu\n"
+		"RX Retry: %lu\n"
+		"RX CRC Error(0-500): %lu\n"
+		"RX CRC Error(500-1000): %lu\n"
+		"RX CRC Error(>1000): %lu\n"
+		"RX ICV Error: %lu\n",
+		priv->stats.rxint,
+		priv->stats.rxerr,
+		priv->stats.rxcrcerrmin,
+		priv->stats.rxcrcerrmid,
+		priv->stats.rxcrcerrmax,
+		priv->stats.rxicverr
+		);
+
+	*eof = 1;
+	return len;
+}
+
+#if 0
+static int proc_get_stats_ieee(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	struct net_device *dev = data;
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+	int len = 0;
+
+	len += snprintf(page + len, count - len,
+		"TXed association requests: %u\n"
+		"TXed authentication requests: %u\n"
+		"RXed successful association response: %u\n"
+		"RXed failed association response: %u\n"
+		"RXed successful authentication response: %u\n"
+		"RXed failed authentication response: %u\n"
+		"Association requests without response: %u\n"
+		"Authentication requests without response: %u\n"
+		"TX probe response: %u\n"
+		"RX probe request: %u\n"
+		"TX probe request: %lu\n"
+		"RX authentication requests: %lu\n"
+		"RX association requests: %lu\n"
+		"Reassociations: %lu\n",
+		priv->ieee80211->ieee_stats.tx_ass,
+		priv->ieee80211->ieee_stats.tx_aut,
+		priv->ieee80211->ieee_stats.rx_ass_ok,
+		priv->ieee80211->ieee_stats.rx_ass_err,
+		priv->ieee80211->ieee_stats.rx_aut_ok,
+		priv->ieee80211->ieee_stats.rx_aut_err,
+		priv->ieee80211->ieee_stats.ass_noresp,
+		priv->ieee80211->ieee_stats.aut_noresp,
+		priv->ieee80211->ieee_stats.tx_probe,
+		priv->ieee80211->ieee_stats.rx_probe,
+		priv->ieee80211->ieee_stats.tx_probe_rq,
+		priv->ieee80211->ieee_stats.rx_auth_rq,
+		priv->ieee80211->ieee_stats.rx_assoc_rq,
+		priv->ieee80211->ieee_stats.reassoc);
+
+	*eof = 1;
+	return len;
+}
+#endif
+#if 0
+static int proc_get_stats_ap(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	struct net_device *dev = data;
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	struct mac_htable_t *list;
+	int i;
+	int len = 0;
+
+	if(priv->ieee80211->iw_mode != IW_MODE_MASTER){
+		len += snprintf(page + len, count - len,
+		"Card is not acting as AP...\n"
+		);
+	}else{
+		len += snprintf(page + len, count - len,
+		"List of associated STA:\n"
+		);
+
+		for(i=0;i<MAC_HTABLE_ENTRY;i++)
+			for (list = priv->ieee80211->assoc_htable[i]; list!=NULL; list = list->next){
+				len += snprintf(page + len, count - len,
+					MACSTR"\n",MAC2STR(list->adr));
+			}
+
+	}
+	*eof = 1;
+	return len;
+}
+#endif
+
+static int proc_get_stats_tx(char *page, char **start,
+			  off_t offset, int count,
+			  int *eof, void *data)
+{
+	struct net_device *dev = data;
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+	int len = 0;
+	unsigned long totalOK;
+
+	totalOK=priv->stats.txnpokint+priv->stats.txhpokint+priv->stats.txlpokint;
+	len += snprintf(page + len, count - len,
+	/*	"TX normal priority ok int: %lu\n"
+		"TX normal priority error int: %lu\n"
+		"TX high priority ok int: %lu\n"
+		"TX high priority failed error int: %lu\n"
+		"TX low priority ok int: %lu\n"
+		"TX low priority failed error int: %lu\n"
+		"TX bytes: %lu\n"
+		"TX packets: %lu\n"
+		"TX queue resume: %lu\n"
+		"TX queue stopped?: %d\n"
+		"TX fifo overflow: %lu\n"
+		//"SW TX stop: %lu\n"
+		//"SW TX wake: %lu\n"
+		"TX beacon: %lu\n"
+		"TX beacon aborted: %lu\n",
+		priv->stats.txnpokint,
+		priv->stats.txnperr,
+		priv->stats.txhpokint,
+		priv->stats.txhperr,
+		priv->stats.txlpokint,
+		priv->stats.txlperr,
+		priv->ieee80211->stats.tx_bytes,
+		priv->ieee80211->stats.tx_packets,
+		priv->stats.txresumed,
+		netif_queue_stopped(dev),
+		priv->stats.txoverflow,
+		//priv->ieee80211->ieee_stats.swtxstop,
+		//priv->ieee80211->ieee_stats.swtxawake,
+		priv->stats.txbeacon,
+		priv->stats.txbeaconerr  */
+		"TX OK: %lu\n"
+		"TX Error: %lu\n"
+		"TX Retry: %lu\n"
+		"TX beacon OK: %lu\n"
+		"TX beacon error: %lu\n",
+		totalOK,
+		priv->stats.txnperr+priv->stats.txhperr+priv->stats.txlperr,
+		priv->stats.txretry,
+		priv->stats.txbeacon,
+		priv->stats.txbeaconerr
+	);
+
+	*eof = 1;
+	return len;
+}
+
+
+#if WIRELESS_EXT < 17
+static struct iw_statistics *r8180_get_wireless_stats(struct net_device *dev)
+{
+       struct r8180_priv *priv = ieee80211_priv(dev);
+
+       return &priv->wstats;
+}
+#endif
+void rtl8180_proc_module_init(void)
+{
+	DMESG("Initializing proc filesystem");
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,24)
+        rtl8180_proc=create_proc_entry(RTL8180_MODULE_NAME, S_IFDIR, proc_net);
+#else
+        rtl8180_proc=create_proc_entry(RTL8180_MODULE_NAME, S_IFDIR, init_net.proc_net);
+#endif
+}
+
+
+void rtl8180_proc_module_remove(void)
+{
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,24)
+        remove_proc_entry(RTL8180_MODULE_NAME, proc_net);
+#else
+        remove_proc_entry(RTL8180_MODULE_NAME, init_net.proc_net);
+#endif
+}
+
+
+void rtl8180_proc_remove_one(struct net_device *dev)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	if (priv->dir_dev) {
+		remove_proc_entry("stats-hw", priv->dir_dev);
+		remove_proc_entry("stats-tx", priv->dir_dev);
+		remove_proc_entry("stats-rx", priv->dir_dev);
+//		remove_proc_entry("stats-ieee", priv->dir_dev);
+//		remove_proc_entry("stats-ap", priv->dir_dev);
+		remove_proc_entry("registers", priv->dir_dev);
+		remove_proc_entry(dev->name, rtl8180_proc);
+		priv->dir_dev = NULL;
+	}
+}
+
+
+void rtl8180_proc_init_one(struct net_device *dev)
+{
+	struct proc_dir_entry *e;
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	priv->dir_dev = create_proc_entry(dev->name,
+					  S_IFDIR | S_IRUGO | S_IXUGO,
+					  rtl8180_proc);
+	if (!priv->dir_dev) {
+		DMESGE("Unable to initialize /proc/net/rtl8180/%s\n",
+		      dev->name);
+		return;
+	}
+
+	e = create_proc_read_entry("stats-hw", S_IFREG | S_IRUGO,
+				   priv->dir_dev, proc_get_stats_hw, dev);
+
+	if (!e) {
+		DMESGE("Unable to initialize "
+		      "/proc/net/rtl8180/%s/stats-hw\n",
+		      dev->name);
+	}
+
+	e = create_proc_read_entry("stats-rx", S_IFREG | S_IRUGO,
+				   priv->dir_dev, proc_get_stats_rx, dev);
+
+	if (!e) {
+		DMESGE("Unable to initialize "
+		      "/proc/net/rtl8180/%s/stats-rx\n",
+		      dev->name);
+	}
+
+
+	e = create_proc_read_entry("stats-tx", S_IFREG | S_IRUGO,
+				   priv->dir_dev, proc_get_stats_tx, dev);
+
+	if (!e) {
+		DMESGE("Unable to initialize "
+		      "/proc/net/rtl8180/%s/stats-tx\n",
+		      dev->name);
+	}
+	#if 0
+	e = create_proc_read_entry("stats-ieee", S_IFREG | S_IRUGO,
+				   priv->dir_dev, proc_get_stats_ieee, dev);
+
+	if (!e) {
+		DMESGE("Unable to initialize "
+		      "/proc/net/rtl8180/%s/stats-ieee\n",
+		      dev->name);
+	}
+	#endif
+	#if 0
+	e = create_proc_read_entry("stats-ap", S_IFREG | S_IRUGO,
+				   priv->dir_dev, proc_get_stats_ap, dev);
+
+	if (!e) {
+		DMESGE("Unable to initialize "
+		      "/proc/net/rtl8180/%s/stats-ap\n",
+		      dev->name);
+	}
+	#endif
+
+	e = create_proc_read_entry("registers", S_IFREG | S_IRUGO,
+				   priv->dir_dev, proc_get_registers, dev);
+
+	if (!e) {
+		DMESGE("Unable to initialize "
+		      "/proc/net/rtl8180/%s/registers\n",
+		      dev->name);
+	}
+}
+/****************************************************************************
+   -----------------------------MISC STUFF-------------------------
+*****************************************************************************/
+/*
+  FIXME: check if we can use some standard already-existent
+  data type+functions in kernel
+*/
+
+short buffer_add(struct buffer **buffer, u32 *buf, dma_addr_t dma,
+		struct buffer **bufferhead)
+{
+#ifdef DEBUG_RING
+	DMESG("adding buffer to TX/RX struct");
+#endif
+
+        struct buffer *tmp;
+
+	if(! *buffer){
+
+		*buffer = kmalloc(sizeof(struct buffer),GFP_KERNEL);
+
+		if (*buffer == NULL) {
+			DMESGE("Failed to kmalloc head of TX/RX struct");
+			return -1;
+		}
+		(*buffer)->next=*buffer;
+		(*buffer)->buf=buf;
+		(*buffer)->dma=dma;
+		if(bufferhead !=NULL)
+			(*bufferhead) = (*buffer);
+		return 0;
+	}
+	tmp=*buffer;
+
+	while(tmp->next!=(*buffer)) tmp=tmp->next;
+	if ((tmp->next= kmalloc(sizeof(struct buffer),GFP_KERNEL)) == NULL){
+		DMESGE("Failed to kmalloc TX/RX struct");
+		return -1;
+	}
+	tmp->next->buf=buf;
+	tmp->next->dma=dma;
+	tmp->next->next=*buffer;
+
+	return 0;
+}
+
+
+void buffer_free(struct net_device *dev,struct buffer **buffer,int len,short
+consistent)
+{
+
+	struct buffer *tmp,*next;
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	struct pci_dev *pdev=priv->pdev;
+	//int i;
+
+	if(! *buffer) return;
+
+	/*for(tmp=*buffer; tmp->next != *buffer; tmp=tmp->next)
+
+	*/
+	tmp=*buffer;
+	do{
+		next=tmp->next;
+		if(consistent){
+			pci_free_consistent(pdev,len,
+				    tmp->buf,tmp->dma);
+		}else{
+			pci_unmap_single(pdev, tmp->dma,
+			len,PCI_DMA_FROMDEVICE);
+			kfree(tmp->buf);
+		}
+		kfree(tmp);
+		tmp = next;
+	}
+	while(next != *buffer);
+
+	*buffer=NULL;
+}
+
+
+void print_buffer(u32 *buffer, int len)
+{
+	int i;
+	u8 *buf =(u8*)buffer;
+
+	printk("ASCII BUFFER DUMP (len: %x):\n",len);
+
+	for(i=0;i<len;i++)
+		printk("%c",buf[i]);
+
+	printk("\nBINARY BUFFER DUMP (len: %x):\n",len);
+
+	for(i=0;i<len;i++)
+		printk("%02x",buf[i]);
+
+	printk("\n");
+}
+
+
+int get_curr_tx_free_desc(struct net_device *dev, int priority)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	u32* tail;
+	u32* head;
+	int ret;
+
+	switch (priority){
+		case MANAGE_PRIORITY:
+			head = priv->txmapringhead;
+			tail = priv->txmapringtail;
+			break;
+		case BK_PRIORITY:
+			head = priv->txbkpringhead;
+			tail = priv->txbkpringtail;
+			break;
+		case BE_PRIORITY:
+			head = priv->txbepringhead;
+			tail = priv->txbepringtail;
+			break;
+		case VI_PRIORITY:
+			head = priv->txvipringhead;
+			tail = priv->txvipringtail;
+			break;
+		case VO_PRIORITY:
+			head = priv->txvopringhead;
+			tail = priv->txvopringtail;
+			break;
+		case HI_PRIORITY:
+			head = priv->txhpringhead;
+			tail = priv->txhpringtail;
+			break;
+		default:
+			return -1;
+	}
+
+	//DMESG("%x %x", head, tail);
+
+	/* FIXME FIXME FIXME FIXME */
+
+#if 0
+	if( head <= tail ) return priv->txringcount-1 - (tail - head)/8;
+	return (head - tail)/8/4;
+#else
+	if( head <= tail )
+		ret = priv->txringcount - (tail - head)/8;
+	else
+		ret = (head - tail)/8;
+
+	if(ret > priv->txringcount ) DMESG("BUG");
+	return ret;
+#endif
+}
+
+
+short check_nic_enought_desc(struct net_device *dev, int priority)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	struct ieee80211_device *ieee = netdev_priv(dev);
+
+	int requiredbyte, required;
+	requiredbyte = priv->ieee80211->fts + sizeof(struct ieee80211_header_data);
+
+	if(ieee->current_network.QoS_Enable) {
+		requiredbyte += 2;
+	};
+
+	required = requiredbyte / (priv->txbuffsize-4);
+	if (requiredbyte % priv->txbuffsize) required++;
+	/* for now we keep two free descriptor as a safety boundary
+	 * between the tail and the head
+	 */
+
+	return (required+2 < get_curr_tx_free_desc(dev,priority));
+}
+
+
+/* This function is only for debuging purpose */
+void check_tx_ring(struct net_device *dev, int pri)
+{
+	static int maxlog =3;
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	u32* tmp;
+	struct buffer *buf;
+	int i;
+	int nic;
+	u32* tail;
+	u32* head;
+	u32* begin;
+	u32 nicbegin;
+	struct buffer* buffer;
+
+	maxlog --;
+	if (maxlog <0 ) return;
+
+	switch(pri) {
+	case MANAGE_PRIORITY:
+		tail = priv->txmapringtail;
+		begin = priv->txmapring;
+		head = priv->txmapringhead;
+		nic = read_nic_dword(dev,TX_MANAGEPRIORITY_RING_ADDR);
+		buffer = priv->txmapbufs;
+		nicbegin = priv->txmapringdma;
+		break;
+
+
+	case BK_PRIORITY:
+		tail = priv->txbkpringtail;
+		begin = priv->txbkpring;
+		head = priv->txbkpringhead;
+		nic = read_nic_dword(dev,TX_BKPRIORITY_RING_ADDR);
+		buffer = priv->txbkpbufs;
+		nicbegin = priv->txbkpringdma;
+		break;
+
+	case BE_PRIORITY:
+		tail = priv->txbepringtail;
+		begin = priv->txbepring;
+		head = priv->txbepringhead;
+		nic = read_nic_dword(dev,TX_BEPRIORITY_RING_ADDR);
+		buffer = priv->txbepbufs;
+		nicbegin = priv->txbepringdma;
+		break;
+
+	case VI_PRIORITY:
+		tail = priv->txvipringtail;
+		begin = priv->txvipring;
+		head = priv->txvipringhead;
+		nic = read_nic_dword(dev,TX_VIPRIORITY_RING_ADDR);
+		buffer = priv->txvipbufs;
+		nicbegin = priv->txvipringdma;
+		break;
+
+
+	case VO_PRIORITY:
+		tail = priv->txvopringtail;
+		begin = priv->txvopring;
+		head = priv->txvopringhead;
+		nic = read_nic_dword(dev,TX_VOPRIORITY_RING_ADDR);
+		buffer = priv->txvopbufs;
+		nicbegin = priv->txvopringdma;
+		break;
+
+	case HI_PRIORITY:
+		tail = priv->txhpringtail;
+		begin = priv->txhpring;
+		head = priv->txhpringhead;
+		nic = read_nic_dword(dev,TX_HIGHPRIORITY_RING_ADDR);
+		buffer = priv->txhpbufs;
+		nicbegin = priv->txhpringdma;
+		break;
+
+	default:
+		return ;
+		break;
+	}
+
+	if(!priv->txvopbufs)
+		DMESGE ("NIC TX ack, but TX queue corrupted!");
+	else{
+
+		for(i=0,buf=buffer, tmp=begin;
+			tmp<begin+(priv->txringcount)*8;
+			tmp+=8,buf=buf->next,i++)
+
+			DMESG("BUF%d %s %x %s. Next : %x",i,
+			      *tmp & (1<<31) ? "filled" : "empty",
+			      *(buf->buf),
+			      *tmp & (1<<15)? "ok": "err", *(tmp+4));
+	}
+
+	DMESG("nic at %d",
+		(nic-nicbegin) / 8 /4);
+	DMESG("tail at %d", ((int)tail - (int)begin) /8 /4);
+	DMESG("head at %d", ((int)head - (int)begin) /8 /4);
+	DMESG("check free desc returns %d", check_nic_enought_desc(dev,pri));
+	DMESG("free desc is %d\n", get_curr_tx_free_desc(dev,pri));
+	//rtl8180_reset(dev);
+	return;
+}
+
+
+
+/* this function is only for debugging purpose */
+void check_rxbuf(struct net_device *dev)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	u32* tmp;
+	struct buffer *buf;
+	u8 rx_desc_size;
+
+#ifdef CONFIG_RTL8185B
+	rx_desc_size = 8;
+#else
+	rx_desc_size = 4;
+#endif
+
+	if(!priv->rxbuffer)
+		DMESGE ("NIC RX ack, but RX queue corrupted!");
+
+	else{
+
+		for(buf=priv->rxbuffer, tmp=priv->rxring;
+		    tmp < priv->rxring+(priv->rxringcount)*rx_desc_size;
+		    tmp+=rx_desc_size, buf=buf->next)
+
+			DMESG("BUF %s %x",
+			      *tmp & (1<<31) ? "empty" : "filled",
+			      *(buf->buf));
+	}
+
+	return;
+}
+
+
+void dump_eprom(struct net_device *dev)
+{
+	int i;
+	for(i=0; i<63; i++)
+		DMESG("EEPROM addr %x : %x", i, eprom_read(dev,i));
+}
+
+
+void rtl8180_dump_reg(struct net_device *dev)
+{
+	int i;
+	int n;
+	int max=0xff;
+
+	DMESG("Dumping NIC register map");
+
+	for(n=0;n<=max;)
+	{
+		printk( "\nD: %2x> ", n);
+		for(i=0;i<16 && n<=max;i++,n++)
+			printk("%2x ",read_nic_byte(dev,n));
+	}
+	printk("\n");
+}
+
+
+void fix_tx_fifo(struct net_device *dev)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	u32 *tmp;
+	int i;
+#ifdef DEBUG_TX_ALLOC
+	DMESG("FIXING TX FIFOs");
+#endif
+	for (tmp=priv->txmapring, i=0;
+	     i < priv->txringcount;
+	     tmp+=8, i++){
+		*tmp = *tmp &~ (1<<31);
+	}
+
+	for (tmp=priv->txbkpring, i=0;
+	     i < priv->txringcount;
+	     tmp+=8, i++) {
+		*tmp = *tmp &~ (1<<31);
+	}
+
+	for (tmp=priv->txbepring, i=0;
+	     i < priv->txringcount;
+	     tmp+=8, i++){
+		*tmp = *tmp &~ (1<<31);
+	}
+	for (tmp=priv->txvipring, i=0;
+	     i < priv->txringcount;
+	     tmp+=8, i++) {
+		*tmp = *tmp &~ (1<<31);
+	}
+
+	for (tmp=priv->txvopring, i=0;
+	     i < priv->txringcount;
+	     tmp+=8, i++){
+		*tmp = *tmp &~ (1<<31);
+	}
+
+	for (tmp=priv->txhpring, i=0;
+	     i < priv->txringcount;
+	     tmp+=8,i++){
+		*tmp = *tmp &~ (1<<31);
+	}
+
+	for (tmp=priv->txbeaconring, i=0;
+	     i < priv->txbeaconcount;
+	     tmp+=8, i++){
+		*tmp = *tmp &~ (1<<31);
+	}
+#ifdef DEBUG_TX_ALLOC
+	DMESG("TX FIFOs FIXED");
+#endif
+	priv->txmapringtail = priv->txmapring;
+	priv->txmapringhead = priv->txmapring;
+	priv->txmapbufstail = priv->txmapbufs;
+
+	priv->txbkpringtail = priv->txbkpring;
+	priv->txbkpringhead = priv->txbkpring;
+	priv->txbkpbufstail = priv->txbkpbufs;
+
+	priv->txbepringtail = priv->txbepring;
+	priv->txbepringhead = priv->txbepring;
+	priv->txbepbufstail = priv->txbepbufs;
+
+	priv->txvipringtail = priv->txvipring;
+	priv->txvipringhead = priv->txvipring;
+	priv->txvipbufstail = priv->txvipbufs;
+
+	priv->txvopringtail = priv->txvopring;
+	priv->txvopringhead = priv->txvopring;
+	priv->txvopbufstail = priv->txvopbufs;
+
+	priv->txhpringtail = priv->txhpring;
+	priv->txhpringhead = priv->txhpring;
+	priv->txhpbufstail = priv->txhpbufs;
+
+	priv->txbeaconringtail = priv->txbeaconring;
+	priv->txbeaconbufstail = priv->txbeaconbufs;
+	set_nic_txring(dev);
+
+	ieee80211_reset_queue(priv->ieee80211);
+	priv->ack_tx_to_ieee = 0;
+}
+
+
+void fix_rx_fifo(struct net_device *dev)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	u32 *tmp;
+	struct buffer *rxbuf;
+	u8 rx_desc_size;
+
+#ifdef CONFIG_RTL8185B
+	rx_desc_size = 8; // 4*8 = 32 bytes
+#else
+	rx_desc_size = 4;
+#endif
+
+#ifdef DEBUG_RXALLOC
+	DMESG("FIXING RX FIFO");
+	check_rxbuf(dev);
+#endif
+
+	for (tmp=priv->rxring, rxbuf=priv->rxbufferhead;
+	     (tmp < (priv->rxring)+(priv->rxringcount)*rx_desc_size);
+	     tmp+=rx_desc_size,rxbuf=rxbuf->next){
+		*(tmp+2) = rxbuf->dma;
+		*tmp=*tmp &~ 0xfff;
+		*tmp=*tmp | priv->rxbuffersize;
+		*tmp |= (1<<31);
+	}
+
+#ifdef DEBUG_RXALLOC
+	DMESG("RX FIFO FIXED");
+	check_rxbuf(dev);
+#endif
+
+	priv->rxringtail=priv->rxring;
+	priv->rxbuffer=priv->rxbufferhead;
+	priv->rx_skb_complete=1;
+	set_nic_rxring(dev);
+}
+
+
+/****************************************************************************
+      ------------------------------HW STUFF---------------------------
+*****************************************************************************/
+
+unsigned char QUALITY_MAP[] = {
+  0x64, 0x64, 0x64, 0x63, 0x63, 0x62, 0x62, 0x61,
+  0x61, 0x60, 0x60, 0x5f, 0x5f, 0x5e, 0x5d, 0x5c,
+  0x5b, 0x5a, 0x59, 0x57, 0x56, 0x54, 0x52, 0x4f,
+  0x4c, 0x49, 0x45, 0x41, 0x3c, 0x37, 0x31, 0x29,
+  0x24, 0x22, 0x22, 0x22, 0x22, 0x22, 0x22, 0x22,
+  0x22, 0x22, 0x21, 0x21, 0x21, 0x21, 0x21, 0x20,
+  0x20, 0x20, 0x20, 0x1f, 0x1f, 0x1e, 0x1e, 0x1e,
+  0x1d, 0x1d, 0x1c, 0x1c, 0x1b, 0x1a, 0x19, 0x19,
+  0x18, 0x17, 0x16, 0x15, 0x14, 0x12, 0x11, 0x0f,
+  0x0e, 0x0c, 0x0a, 0x08, 0x06, 0x04, 0x01, 0x00
+};
+
+unsigned char STRENGTH_MAP[] = {
+  0x64, 0x64, 0x63, 0x62, 0x61, 0x60, 0x5f, 0x5e,
+  0x5d, 0x5c, 0x5b, 0x5a, 0x57, 0x54, 0x52, 0x50,
+  0x4e, 0x4c, 0x4a, 0x48, 0x46, 0x44, 0x41, 0x3f,
+  0x3c, 0x3a, 0x37, 0x36, 0x36, 0x1c, 0x1c, 0x1b,
+  0x1b, 0x1a, 0x1a, 0x19, 0x19, 0x18, 0x18, 0x17,
+  0x17, 0x16, 0x16, 0x15, 0x15, 0x14, 0x14, 0x13,
+  0x13, 0x12, 0x12, 0x11, 0x11, 0x10, 0x10, 0x0f,
+  0x0f, 0x0e, 0x0e, 0x0d, 0x0d, 0x0c, 0x0c, 0x0b,
+  0x0b, 0x0a, 0x0a, 0x09, 0x09, 0x08, 0x08, 0x07,
+  0x07, 0x06, 0x06, 0x05, 0x04, 0x03, 0x02, 0x00
+};
+
+void rtl8180_RSSI_calc(struct net_device *dev, u8 *rssi, u8 *qual){
+	//void Mlme_UpdateRssiSQ(struct net_device *dev, u8 *rssi, u8 *qual){
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	u32 temp;
+	u32 temp2;
+	u32 temp3;
+	u32 lsb;
+	u32 q;
+	u32 orig_qual;
+	u8  _rssi;
+
+	q = *qual;
+	orig_qual = *qual;
+	_rssi = 0; // avoid gcc complains..
+
+	if (q <= 0x4e) {
+		temp = QUALITY_MAP[q];
+	} else {
+		if( q & 0x80 ) {
+			temp = 0x32;
+		} else {
+			temp = 1;
+		}
+	}
+
+	*qual = temp;
+	temp2 = *rssi;
+
+	switch(priv->rf_chip){
+	case RFCHIPID_RFMD:
+		lsb = temp2 & 1;
+		temp2 &= 0x7e;
+		if ( !lsb || !(temp2 <= 0x3c) ) {
+			temp2 = 0x64;
+		} else {
+			temp2 = 100 * temp2 / 0x3c;
+		}
+		*rssi = temp2 & 0xff;
+		_rssi = temp2 & 0xff;
+		break;
+	case RFCHIPID_INTERSIL:
+		lsb = temp2;
+		temp2 &= 0xfffffffe;
+		temp2 *= 251;
+		temp3 = temp2;
+		temp2 <<= 6;
+		temp3 += temp2;
+		temp3 <<= 1;
+		temp2 = 0x4950df;
+		temp2 -= temp3;
+		lsb &= 1;
+		if ( temp2 <= 0x3e0000 ) {
+			if ( temp2 < 0xffef0000 )
+				temp2 = 0xffef0000;
+		} else {
+			temp2 = 0x3e0000;
+		}
+		if ( !lsb ) {
+			temp2 -= 0xf0000;
+		} else {
+			temp2 += 0xf0000;
+		}
+
+		temp3 = 0x4d0000;
+		temp3 -= temp2;
+		temp3 *= 100;
+		temp3 = temp3 / 0x6d;
+		temp3 >>= 0x10;
+		_rssi = temp3 & 0xff;
+		*rssi = temp3 & 0xff;
+		break;
+	case RFCHIPID_GCT:
+	        lsb = temp2 & 1;
+		temp2 &= 0x7e;
+		if ( ! lsb || !(temp2 <= 0x3c) ){
+			temp2 = 0x64;
+		} else {
+			temp2 = (100 * temp2) / 0x3c;
+		}
+		*rssi = temp2 & 0xff;
+		_rssi = temp2 & 0xff;
+		break;
+	case RFCHIPID_PHILIPS:
+		if( orig_qual <= 0x4e ){
+			_rssi = STRENGTH_MAP[orig_qual];
+			*rssi = _rssi;
+		} else {
+			orig_qual -= 0x80;
+			if ( !orig_qual ){
+				_rssi = 1;
+				*rssi = 1;
+			} else {
+				_rssi = 0x32;
+				*rssi = 0x32;
+			}
+		}
+		break;
+
+	/* case 4 */
+	case RFCHIPID_MAXIM:
+		lsb = temp2 & 1;
+		temp2 &= 0x7e;
+		temp2 >>= 1;
+		temp2 += 0x42;
+		if( lsb != 0 ){
+			temp2 += 0xa;
+		}
+		*rssi = temp2 & 0xff;
+		_rssi = temp2 & 0xff;
+		break;
+	}
+
+	if ( _rssi < 0x64 ){
+		if ( _rssi == 0 ) {
+			*rssi = 1;
+		}
+	} else {
+		*rssi = 0x64;
+	}
+
+	return;
+}
+
+
+void rtl8180_irq_enable(struct net_device *dev)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	priv->irq_enabled = 1;
+/*
+	write_nic_word(dev,INTA_MASK,INTA_RXOK | INTA_RXDESCERR | INTA_RXOVERFLOW |\
+	INTA_TXOVERFLOW | INTA_HIPRIORITYDESCERR | INTA_HIPRIORITYDESCOK |\
+	INTA_NORMPRIORITYDESCERR | INTA_NORMPRIORITYDESCOK |\
+	INTA_LOWPRIORITYDESCERR | INTA_LOWPRIORITYDESCOK | INTA_TIMEOUT);
+*/
+	write_nic_word(dev,INTA_MASK, priv->irq_mask);
+}
+
+
+void rtl8180_irq_disable(struct net_device *dev)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+#ifdef CONFIG_RTL8185B
+	write_nic_dword(dev,IMR,0);
+#else
+	write_nic_word(dev,INTA_MASK,0);
+#endif
+	force_pci_posting(dev);
+	priv->irq_enabled = 0;
+}
+
+
+void rtl8180_set_mode(struct net_device *dev,int mode)
+{
+	u8 ecmd;
+	ecmd=read_nic_byte(dev, EPROM_CMD);
+	ecmd=ecmd &~ EPROM_CMD_OPERATING_MODE_MASK;
+	ecmd=ecmd | (mode<<EPROM_CMD_OPERATING_MODE_SHIFT);
+	ecmd=ecmd &~ (1<<EPROM_CS_SHIFT);
+	ecmd=ecmd &~ (1<<EPROM_CK_SHIFT);
+	write_nic_byte(dev, EPROM_CMD, ecmd);
+}
+
+void rtl8180_adapter_start(struct net_device *dev);
+void rtl8180_beacon_tx_enable(struct net_device *dev);
+
+void rtl8180_update_msr(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	u8 msr;
+	u32 rxconf;
+
+	msr  = read_nic_byte(dev, MSR);
+	msr &= ~ MSR_LINK_MASK;
+
+	rxconf=read_nic_dword(dev,RX_CONF);
+
+	if(priv->ieee80211->state == IEEE80211_LINKED)
+	{
+		if(priv->ieee80211->iw_mode == IW_MODE_ADHOC)
+			msr |= (MSR_LINK_ADHOC<<MSR_LINK_SHIFT);
+		else if (priv->ieee80211->iw_mode == IW_MODE_MASTER)
+			msr |= (MSR_LINK_MASTER<<MSR_LINK_SHIFT);
+		else if (priv->ieee80211->iw_mode == IW_MODE_INFRA)
+			msr |= (MSR_LINK_MANAGED<<MSR_LINK_SHIFT);
+		else
+			msr |= (MSR_LINK_NONE<<MSR_LINK_SHIFT);
+		rxconf |= (1<<RX_CHECK_BSSID_SHIFT);
+
+	}else {
+		msr |= (MSR_LINK_NONE<<MSR_LINK_SHIFT);
+		rxconf &= ~(1<<RX_CHECK_BSSID_SHIFT);
+	}
+
+	write_nic_byte(dev, MSR, msr);
+	write_nic_dword(dev, RX_CONF, rxconf);
+
+}
+
+
+
+void rtl8180_set_chan(struct net_device *dev,short ch)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+	if((ch > 14) || (ch < 1))
+	{
+		printk("In %s: Invalid chnanel %d\n", __FUNCTION__, ch);
+		return;
+	}
+
+	priv->chan=ch;
+	//printk("in %s:channel is %d\n",__FUNCTION__,ch);
+	priv->rf_set_chan(dev,priv->chan);
+
+}
+
+
+void rtl8180_rx_enable(struct net_device *dev)
+{
+	u8 cmd;
+	u32 rxconf;
+	/* for now we accept data, management & ctl frame*/
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+	rxconf=read_nic_dword(dev,RX_CONF);
+	rxconf = rxconf &~ MAC_FILTER_MASK;
+	rxconf = rxconf | (1<<ACCEPT_MNG_FRAME_SHIFT);
+	rxconf = rxconf | (1<<ACCEPT_DATA_FRAME_SHIFT);
+	rxconf = rxconf | (1<<ACCEPT_BCAST_FRAME_SHIFT);
+	rxconf = rxconf | (1<<ACCEPT_MCAST_FRAME_SHIFT);
+//	rxconf = rxconf | (1<<ACCEPT_CRCERR_FRAME_SHIFT);
+	if (dev->flags & IFF_PROMISC) DMESG ("NIC in promisc mode");
+
+	if(priv->ieee80211->iw_mode == IW_MODE_MONITOR || \
+	   dev->flags & IFF_PROMISC){
+		rxconf = rxconf | (1<<ACCEPT_ALLMAC_FRAME_SHIFT);
+	}else{
+		rxconf = rxconf | (1<<ACCEPT_NICMAC_FRAME_SHIFT);
+		if(priv->card_8185 == 0)
+			rxconf = rxconf | (1<<RX_CHECK_BSSID_SHIFT);
+	}
+
+	/*if(priv->ieee80211->iw_mode == IW_MODE_MASTER){
+		rxconf = rxconf | (1<<ACCEPT_ALLMAC_FRAME_SHIFT);
+		rxconf = rxconf | (1<<RX_CHECK_BSSID_SHIFT);
+	}*/
+
+	if(priv->ieee80211->iw_mode == IW_MODE_MONITOR){
+		rxconf = rxconf | (1<<ACCEPT_CTL_FRAME_SHIFT);
+		rxconf = rxconf | (1<<ACCEPT_ICVERR_FRAME_SHIFT);
+		rxconf = rxconf | (1<<ACCEPT_PWR_FRAME_SHIFT);
+	}
+
+	if( priv->crcmon == 1 && priv->ieee80211->iw_mode == IW_MODE_MONITOR)
+		rxconf = rxconf | (1<<ACCEPT_CRCERR_FRAME_SHIFT);
+
+	//if(!priv->card_8185){
+		rxconf = rxconf &~ RX_FIFO_THRESHOLD_MASK;
+		rxconf = rxconf | (RX_FIFO_THRESHOLD_NONE<<RX_FIFO_THRESHOLD_SHIFT);
+	//}
+
+	rxconf = rxconf | (1<<RX_AUTORESETPHY_SHIFT);
+	rxconf = rxconf &~ MAX_RX_DMA_MASK;
+	rxconf = rxconf | (MAX_RX_DMA_2048<<MAX_RX_DMA_SHIFT);
+
+	//if(!priv->card_8185)
+		rxconf = rxconf | RCR_ONLYERLPKT;
+
+	rxconf = rxconf &~ RCR_CS_MASK;
+	if(!priv->card_8185)
+		rxconf |= (priv->rcr_csense<<RCR_CS_SHIFT);
+//	rxconf &=~ 0xfff00000;
+//	rxconf |= 0x90100000;//9014f76f;
+	write_nic_dword(dev, RX_CONF, rxconf);
+
+	fix_rx_fifo(dev);
+
+#ifdef DEBUG_RX
+	DMESG("rxconf: %x %x",rxconf ,read_nic_dword(dev,RX_CONF));
+#endif
+	cmd=read_nic_byte(dev,CMD);
+	write_nic_byte(dev,CMD,cmd | (1<<CMD_RX_ENABLE_SHIFT));
+
+	/* In rtl8139 driver seems that DMA threshold has to be written
+	 *  after enabling RX, so we rewrite RX_CONFIG register
+	 */
+	//mdelay(100);
+//	write_nic_dword(dev, RX_CONF, rxconf);
+
+}
+
+
+void set_nic_txring(struct net_device *dev)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+//		DMESG("ring %x %x", priv->txlpringdma,read_nic_dword(dev,TLPDA));
+
+	write_nic_dword(dev, TX_MANAGEPRIORITY_RING_ADDR, priv->txmapringdma);
+//		DMESG("ring %x %x", priv->txlpringdma,read_nic_dword(dev,TLPDA));
+	write_nic_dword(dev, TX_BKPRIORITY_RING_ADDR, priv->txbkpringdma);
+//		DMESG("ring %x %x", priv->txlpringdma,read_nic_dword(dev,TLPDA));
+	write_nic_dword(dev, TX_BEPRIORITY_RING_ADDR, priv->txbepringdma);
+//		DMESG("ring %x %x", priv->txlpringdma,read_nic_dword(dev,TLPDA));
+	write_nic_dword(dev, TX_VIPRIORITY_RING_ADDR, priv->txvipringdma);
+//		DMESG("ring %x %x", priv->txlpringdma,read_nic_dword(dev,TLPDA));
+	write_nic_dword(dev, TX_VOPRIORITY_RING_ADDR, priv->txvopringdma);
+//		DMESG("ring %x %x", priv->txlpringdma,read_nic_dword(dev,TLPDA));
+	write_nic_dword(dev, TX_HIGHPRIORITY_RING_ADDR, priv->txhpringdma);
+//		DMESG("ring %x %x", priv->txlpringdma,read_nic_dword(dev,TLPDA));
+
+	write_nic_dword(dev, TX_BEACON_RING_ADDR, priv->txbeaconringdma);
+}
+
+
+void rtl8180_conttx_enable(struct net_device *dev)
+{
+	u32 txconf;
+	txconf = read_nic_dword(dev,TX_CONF);
+	txconf = txconf &~ TX_LOOPBACK_MASK;
+	txconf = txconf | (TX_LOOPBACK_CONTINUE <<TX_LOOPBACK_SHIFT);
+	write_nic_dword(dev,TX_CONF,txconf);
+}
+
+
+void rtl8180_conttx_disable(struct net_device *dev)
+{
+	u32 txconf;
+	txconf = read_nic_dword(dev,TX_CONF);
+	txconf = txconf &~ TX_LOOPBACK_MASK;
+	txconf = txconf | (TX_LOOPBACK_NONE <<TX_LOOPBACK_SHIFT);
+	write_nic_dword(dev,TX_CONF,txconf);
+}
+
+
+void rtl8180_tx_enable(struct net_device *dev)
+{
+	u8 cmd;
+	u8 tx_agc_ctl;
+	u8 byte;
+	u32 txconf;
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	txconf= read_nic_dword(dev,TX_CONF);
+
+
+	if(priv->card_8185){
+
+
+		byte = read_nic_byte(dev,CW_CONF);
+		byte &= ~(1<<CW_CONF_PERPACKET_CW_SHIFT);
+		byte &= ~(1<<CW_CONF_PERPACKET_RETRY_SHIFT);
+		write_nic_byte(dev, CW_CONF, byte);
+
+		tx_agc_ctl = read_nic_byte(dev, TX_AGC_CTL);
+		tx_agc_ctl &= ~(1<<TX_AGC_CTL_PERPACKET_GAIN_SHIFT);
+		tx_agc_ctl &= ~(1<<TX_AGC_CTL_PERPACKET_ANTSEL_SHIFT);
+		tx_agc_ctl |=(1<<TX_AGC_CTL_FEEDBACK_ANT);
+		write_nic_byte(dev, TX_AGC_CTL, tx_agc_ctl);
+		/*
+		write_nic_word(dev, 0x5e, 0x01);
+		force_pci_posting(dev);
+		mdelay(1);
+		write_nic_word(dev, 0xfe, 0x10);
+		force_pci_posting(dev);
+		mdelay(1);
+		write_nic_word(dev, 0x5e, 0x00);
+		force_pci_posting(dev);
+		mdelay(1);
+		*/
+		write_nic_byte(dev, 0xec, 0x3f); /* Disable early TX */
+	}
+
+	if(priv->card_8185){
+
+		txconf = txconf &~ (1<<TCR_PROBE_NOTIMESTAMP_SHIFT);
+
+	}else{
+
+		if(hwseqnum)
+			txconf= txconf &~ (1<<TX_CONF_HEADER_AUTOICREMENT_SHIFT);
+		else
+			txconf= txconf | (1<<TX_CONF_HEADER_AUTOICREMENT_SHIFT);
+	}
+
+	txconf = txconf &~ TX_LOOPBACK_MASK;
+	txconf = txconf | (TX_LOOPBACK_NONE <<TX_LOOPBACK_SHIFT);
+	txconf = txconf &~ TCR_DPRETRY_MASK;
+	txconf = txconf &~ TCR_RTSRETRY_MASK;
+	txconf = txconf | (priv->retry_data<<TX_DPRETRY_SHIFT);
+	txconf = txconf | (priv->retry_rts<<TX_RTSRETRY_SHIFT);
+	txconf = txconf &~ (1<<TX_NOCRC_SHIFT);
+
+	if(priv->card_8185){
+		if(priv->hw_plcp_len)
+			txconf = txconf &~ TCR_PLCP_LEN;
+		else
+			txconf = txconf | TCR_PLCP_LEN;
+	}else{
+		txconf = txconf &~ TCR_SAT;
+	}
+	txconf = txconf &~ TCR_MXDMA_MASK;
+	txconf = txconf | (TCR_MXDMA_2048<<TCR_MXDMA_SHIFT);
+	txconf = txconf | TCR_CWMIN;
+	txconf = txconf | TCR_DISCW;
+
+//	if(priv->ieee80211->hw_wep)
+//		txconf=txconf &~ (1<<TX_NOICV_SHIFT);
+//	else
+		txconf=txconf | (1<<TX_NOICV_SHIFT);
+
+	write_nic_dword(dev,TX_CONF,txconf);
+
+
+	fix_tx_fifo(dev);
+
+#ifdef DEBUG_TX
+	DMESG("txconf: %x %x",txconf,read_nic_dword(dev,TX_CONF));
+#endif
+
+	cmd=read_nic_byte(dev,CMD);
+	write_nic_byte(dev,CMD,cmd | (1<<CMD_TX_ENABLE_SHIFT));
+
+//	mdelay(100);
+	write_nic_dword(dev,TX_CONF,txconf);
+//	#endif
+/*
+	rtl8180_set_mode(dev,EPROM_CMD_CONFIG);
+	write_nic_byte(dev, TX_DMA_POLLING, priv->dma_poll_mask);
+	rtl8180_set_mode(dev,EPROM_CMD_NORMAL);
+	*/
+}
+
+
+void rtl8180_beacon_tx_enable(struct net_device *dev)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+	rtl8180_set_mode(dev,EPROM_CMD_CONFIG);
+#ifdef CONFIG_RTL8185B
+	priv->dma_poll_stop_mask &= ~(TPPOLLSTOP_BQ);
+	write_nic_byte(dev,TPPollStop, priv->dma_poll_mask);
+#else
+	priv->dma_poll_mask &=~(1<<TX_DMA_STOP_BEACON_SHIFT);
+	write_nic_byte(dev,TX_DMA_POLLING,priv->dma_poll_mask);
+#endif
+	rtl8180_set_mode(dev,EPROM_CMD_NORMAL);
+}
+
+
+void rtl8180_beacon_tx_disable(struct net_device *dev)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+	rtl8180_set_mode(dev,EPROM_CMD_CONFIG);
+#ifdef CONFIG_RTL8185B
+	priv->dma_poll_stop_mask |= TPPOLLSTOP_BQ;
+	write_nic_byte(dev,TPPollStop, priv->dma_poll_stop_mask);
+#else
+	priv->dma_poll_mask |= (1<<TX_DMA_STOP_BEACON_SHIFT);
+	write_nic_byte(dev,TX_DMA_POLLING,priv->dma_poll_mask);
+#endif
+	rtl8180_set_mode(dev,EPROM_CMD_NORMAL);
+
+}
+
+
+void rtl8180_rtx_disable(struct net_device *dev)
+{
+	u8 cmd;
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	cmd=read_nic_byte(dev,CMD);
+	write_nic_byte(dev, CMD, cmd &~ \
+		       ((1<<CMD_RX_ENABLE_SHIFT)|(1<<CMD_TX_ENABLE_SHIFT)));
+	force_pci_posting(dev);
+	mdelay(10);
+	/*while (read_nic_byte(dev,CMD) & (1<<CMD_RX_ENABLE_SHIFT))
+	  udelay(10);
+	*/
+
+	if(!priv->rx_skb_complete)
+		dev_kfree_skb_any(priv->rx_skb);
+}
+
+#if 0
+int alloc_tx_beacon_desc_ring(struct net_device *dev, int count)
+{
+	int i;
+	u32 *tmp;
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+	priv->txbeaconring = (u32*)pci_alloc_consistent(priv->pdev,
+					  sizeof(u32)*8*count,
+					  &priv->txbeaconringdma);
+	if (!priv->txbeaconring) return -1;
+	for (tmp=priv->txbeaconring,i=0;i<count;i++){
+		*tmp = *tmp &~ (1<<31); // descriptor empty, owned by the drv
+		/*
+		*(tmp+2) = (u32)dma_tmp;
+		*(tmp+3) = bufsize;
+		*/
+		if(i+1<count)
+			*(tmp+4) = (u32)priv->txbeaconringdma+((i+1)*8*4);
+		else
+			*(tmp+4) = (u32)priv->txbeaconringdma;
+
+		tmp=tmp+8;
+	}
+	return 0;
+}
+#endif
+
+short alloc_tx_desc_ring(struct net_device *dev, int bufsize, int count,
+			 int addr)
+{
+	int i;
+	u32 *desc;
+	u32 *tmp;
+	dma_addr_t dma_desc, dma_tmp;
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	struct pci_dev *pdev = priv->pdev;
+	void *buf;
+
+	if((bufsize & 0xfff) != bufsize) {
+		DMESGE ("TX buffer allocation too large");
+		return 0;
+	}
+	desc = (u32*)pci_alloc_consistent(pdev,
+					  sizeof(u32)*8*count+256, &dma_desc);
+	if(desc==NULL) return -1;
+	if(dma_desc & 0xff){
+
+		/*
+		 * descriptor's buffer must be 256 byte aligned
+		 * we shouldn't be here, since we set DMA mask !
+		 */
+		DMESGW("Fixing TX alignment");
+		desc = (u32*)((u8*)desc + 256);
+#if (defined(CONFIG_HIGHMEM64G) || defined(CONFIG_64BIT_PHYS_ADDR))
+		desc = (u32*)((u64)desc &~ 0xff);
+		dma_desc = (dma_addr_t)((u8*)dma_desc + 256);
+		dma_desc = (dma_addr_t)((u64)dma_desc &~ 0xff);
+#else
+		desc = (u32*)((u32)desc &~ 0xff);
+		dma_desc = (dma_addr_t)((u8*)dma_desc + 256);
+		dma_desc = (dma_addr_t)((u32)dma_desc &~ 0xff);
+#endif
+	}
+	tmp=desc;
+	for (i=0;i<count;i++)
+	{
+		buf = (void*)pci_alloc_consistent(pdev,bufsize,&dma_tmp);
+		if (buf == NULL) return -ENOMEM;
+
+		switch(addr) {
+#if 0
+		case TX_NORMPRIORITY_RING_ADDR:
+			if(-1 == buffer_add(&(priv->txnpbufs),buf,dma_tmp,NULL)){
+				DMESGE("Unable to allocate mem for buffer NP");
+				return -ENOMEM;
+			}
+			break;
+
+		case TX_LOWPRIORITY_RING_ADDR:
+			if(-1 == buffer_add(&(priv->txlpbufs),buf,dma_tmp,NULL)){
+				DMESGE("Unable to allocate mem for buffer LP");
+				return -ENOMEM;
+			}
+			break;
+
+		case TX_HIGHPRIORITY_RING_ADDR:
+			if(-1 == buffer_add(&(priv->txhpbufs),buf,dma_tmp,NULL)){
+				DMESGE("Unable to allocate mem for buffer HP");
+				return -ENOMEM;
+			}
+			break;
+#else
+		case TX_MANAGEPRIORITY_RING_ADDR:
+			if(-1 == buffer_add(&(priv->txmapbufs),buf,dma_tmp,NULL)){
+				DMESGE("Unable to allocate mem for buffer NP");
+				return -ENOMEM;
+			}
+			break;
+
+		case TX_BKPRIORITY_RING_ADDR:
+			if(-1 == buffer_add(&(priv->txbkpbufs),buf,dma_tmp,NULL)){
+				DMESGE("Unable to allocate mem for buffer LP");
+				return -ENOMEM;
+			}
+			break;
+		case TX_BEPRIORITY_RING_ADDR:
+			if(-1 == buffer_add(&(priv->txbepbufs),buf,dma_tmp,NULL)){
+				DMESGE("Unable to allocate mem for buffer NP");
+				return -ENOMEM;
+			}
+			break;
+
+		case TX_VIPRIORITY_RING_ADDR:
+			if(-1 == buffer_add(&(priv->txvipbufs),buf,dma_tmp,NULL)){
+				DMESGE("Unable to allocate mem for buffer LP");
+				return -ENOMEM;
+			}
+			break;
+		case TX_VOPRIORITY_RING_ADDR:
+			if(-1 == buffer_add(&(priv->txvopbufs),buf,dma_tmp,NULL)){
+				DMESGE("Unable to allocate mem for buffer NP");
+				return -ENOMEM;
+			}
+			break;
+#endif
+		case TX_HIGHPRIORITY_RING_ADDR:
+			if(-1 == buffer_add(&(priv->txhpbufs),buf,dma_tmp,NULL)){
+				DMESGE("Unable to allocate mem for buffer HP");
+				return -ENOMEM;
+			}
+			break;
+		case TX_BEACON_RING_ADDR:
+		        if(-1 == buffer_add(&(priv->txbeaconbufs),buf,dma_tmp,NULL)){
+			DMESGE("Unable to allocate mem for buffer BP");
+				return -ENOMEM;
+			}
+			break;
+		}
+		*tmp = *tmp &~ (1<<31); // descriptor empty, owned by the drv
+		*(tmp+2) = (u32)dma_tmp;
+		*(tmp+3) = bufsize;
+
+		if(i+1<count)
+			*(tmp+4) = (u32)dma_desc+((i+1)*8*4);
+		else
+			*(tmp+4) = (u32)dma_desc;
+
+		tmp=tmp+8;
+	}
+
+	switch(addr) {
+	case TX_MANAGEPRIORITY_RING_ADDR:
+		priv->txmapringdma=dma_desc;
+		priv->txmapring=desc;
+		break;
+
+	case TX_BKPRIORITY_RING_ADDR:
+		priv->txbkpringdma=dma_desc;
+		priv->txbkpring=desc;
+		break;
+
+	case TX_BEPRIORITY_RING_ADDR:
+		priv->txbepringdma=dma_desc;
+		priv->txbepring=desc;
+		break;
+
+	case TX_VIPRIORITY_RING_ADDR:
+		priv->txvipringdma=dma_desc;
+		priv->txvipring=desc;
+		break;
+
+	case TX_VOPRIORITY_RING_ADDR:
+		priv->txvopringdma=dma_desc;
+		priv->txvopring=desc;
+		break;
+
+	case TX_HIGHPRIORITY_RING_ADDR:
+		priv->txhpringdma=dma_desc;
+		priv->txhpring=desc;
+		break;
+
+	case TX_BEACON_RING_ADDR:
+		priv->txbeaconringdma=dma_desc;
+		priv->txbeaconring=desc;
+		break;
+
+	}
+
+#ifdef DEBUG_TX
+	DMESG("Tx dma physical address: %x",dma_desc);
+#endif
+
+	return 0;
+}
+
+
+void free_tx_desc_rings(struct net_device *dev)
+{
+
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	struct pci_dev *pdev=priv->pdev;
+	int count = priv->txringcount;
+
+	pci_free_consistent(pdev, sizeof(u32)*8*count+256,
+			    priv->txmapring, priv->txmapringdma);
+	buffer_free(dev,&(priv->txmapbufs),priv->txbuffsize,1);
+
+	pci_free_consistent(pdev, sizeof(u32)*8*count+256,
+			    priv->txbkpring, priv->txbkpringdma);
+	buffer_free(dev,&(priv->txbkpbufs),priv->txbuffsize,1);
+
+	pci_free_consistent(pdev, sizeof(u32)*8*count+256,
+			    priv->txbepring, priv->txbepringdma);
+	buffer_free(dev,&(priv->txbepbufs),priv->txbuffsize,1);
+
+	pci_free_consistent(pdev, sizeof(u32)*8*count+256,
+			    priv->txvipring, priv->txvipringdma);
+	buffer_free(dev,&(priv->txvipbufs),priv->txbuffsize,1);
+
+	pci_free_consistent(pdev, sizeof(u32)*8*count+256,
+			    priv->txvopring, priv->txvopringdma);
+	buffer_free(dev,&(priv->txvopbufs),priv->txbuffsize,1);
+
+	pci_free_consistent(pdev, sizeof(u32)*8*count+256,
+			    priv->txhpring, priv->txhpringdma);
+	buffer_free(dev,&(priv->txhpbufs),priv->txbuffsize,1);
+
+	count = priv->txbeaconcount;
+	pci_free_consistent(pdev, sizeof(u32)*8*count+256,
+			    priv->txbeaconring, priv->txbeaconringdma);
+	buffer_free(dev,&(priv->txbeaconbufs),priv->txbuffsize,1);
+}
+
+#if 0
+void free_beacon_desc_ring(struct net_device *dev,int count)
+{
+
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	struct pci_dev *pdev=priv->pdev;
+
+	pci_free_consistent(pdev, sizeof(u32)*8*count+256,
+			    priv->txbeaconring, priv->txbeaconringdma);
+
+	if (priv->beacon_buf)
+		pci_free_consistent(priv->pdev,
+			priv->master_beaconsize,priv->beacon_buf,priv->beacondmabuf);
+
+}
+#endif
+void free_rx_desc_ring(struct net_device *dev)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	struct pci_dev *pdev = priv->pdev;
+
+	int count = priv->rxringcount;
+
+#ifdef CONFIG_RTL8185B
+	pci_free_consistent(pdev, sizeof(u32)*8*count+256,
+			    priv->rxring, priv->rxringdma);
+#else
+	pci_free_consistent(pdev, sizeof(u32)*4*count+256,
+			    priv->rxring, priv->rxringdma);
+#endif
+
+	buffer_free(dev,&(priv->rxbuffer),priv->rxbuffersize,0);
+}
+
+
+short alloc_rx_desc_ring(struct net_device *dev, u16 bufsize, int count)
+{
+	int i;
+	u32 *desc;
+	u32 *tmp;
+	dma_addr_t dma_desc,dma_tmp;
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	struct pci_dev *pdev=priv->pdev;
+	void *buf;
+	u8 rx_desc_size;
+
+#ifdef CONFIG_RTL8185B
+	rx_desc_size = 8; // 4*8 = 32 bytes
+#else
+	rx_desc_size = 4;
+#endif
+
+	if((bufsize & 0xfff) != bufsize){
+		DMESGE ("RX buffer allocation too large");
+		return -1;
+	}
+
+	desc = (u32*)pci_alloc_consistent(pdev,sizeof(u32)*rx_desc_size*count+256,
+					  &dma_desc);
+
+	if(dma_desc & 0xff){
+
+		/*
+		 * descriptor's buffer must be 256 byte aligned
+		 * should never happen since we specify the DMA mask
+		 */
+
+		DMESGW("Fixing RX alignment");
+		desc = (u32*)((u8*)desc + 256);
+#if (defined(CONFIG_HIGHMEM64G) || defined(CONFIG_64BIT_PHYS_ADDR))
+		desc = (u32*)((u64)desc &~ 0xff);
+		dma_desc = (dma_addr_t)((u8*)dma_desc + 256);
+		dma_desc = (dma_addr_t)((u64)dma_desc &~ 0xff);
+#else
+		desc = (u32*)((u32)desc &~ 0xff);
+		dma_desc = (dma_addr_t)((u8*)dma_desc + 256);
+		dma_desc = (dma_addr_t)((u32)dma_desc &~ 0xff);
+#endif
+	}
+
+	priv->rxring=desc;
+	priv->rxringdma=dma_desc;
+	tmp=desc;
+
+	for (i=0;i<count;i++){
+
+		if ((buf= kmalloc(bufsize * sizeof(u8),GFP_ATOMIC)) == NULL){
+			DMESGE("Failed to kmalloc RX buffer");
+			return -1;
+		}
+
+		dma_tmp = pci_map_single(pdev,buf,bufsize * sizeof(u8),
+					 PCI_DMA_FROMDEVICE);
+
+#ifdef DEBUG_ZERO_RX
+		int j;
+		for(j=0;j<bufsize;j++) ((u8*)buf)[i] = 0;
+#endif
+
+		//buf = (void*)pci_alloc_consistent(pdev,bufsize,&dma_tmp);
+		if(-1 == buffer_add(&(priv->rxbuffer), buf,dma_tmp,
+			   &(priv->rxbufferhead))){
+			   DMESGE("Unable to allocate mem RX buf");
+			   return -1;
+		}
+		*tmp = 0; //zero pads the header of the descriptor
+		*tmp = *tmp |( bufsize&0xfff);
+		*(tmp+2) = (u32)dma_tmp;
+		*tmp = *tmp |(1<<31); // descriptor void, owned by the NIC
+
+#ifdef DEBUG_RXALLOC
+		DMESG("Alloc %x size buffer, DMA mem @ %x, virtual mem @ %x",
+		      (u32)(bufsize&0xfff), (u32)dma_tmp, (u32)buf);
+#endif
+
+		tmp=tmp+rx_desc_size;
+	}
+
+	*(tmp-rx_desc_size) = *(tmp-rx_desc_size) | (1<<30); // this is the last descriptor
+
+
+#ifdef DEBUG_RXALLOC
+	DMESG("RX DMA physical address: %x",dma_desc);
+#endif
+
+	return 0;
+}
+
+
+void set_nic_rxring(struct net_device *dev)
+{
+	u8 pgreg;
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+	//rtl8180_set_mode(dev, EPROM_CMD_CONFIG);
+
+	pgreg=read_nic_byte(dev, PGSELECT);
+	write_nic_byte(dev, PGSELECT, pgreg &~ (1<<PGSELECT_PG_SHIFT));
+
+	//rtl8180_set_mode(dev, EPROM_CMD_NORMAL);
+
+	write_nic_dword(dev, RXRING_ADDR,priv->rxringdma);
+}
+
+
+void rtl8180_reset(struct net_device *dev)
+{
+	//u32 txconf = 0x80e00707; //FIXME: Make me understandable
+	u8 cr;
+
+	//write_nic_dword(dev,TX_CONF,txconf);
+
+	rtl8180_irq_disable(dev);
+
+	cr=read_nic_byte(dev,CMD);
+	cr = cr & 2;
+	cr = cr | (1<<CMD_RST_SHIFT);
+	write_nic_byte(dev,CMD,cr);
+
+	force_pci_posting(dev);
+
+	mdelay(200);
+
+	if(read_nic_byte(dev,CMD) & (1<<CMD_RST_SHIFT))
+		DMESGW("Card reset timeout!");
+	else
+		DMESG("Card successfully reset");
+
+//#ifndef CONFIG_RTL8185B
+	rtl8180_set_mode(dev,EPROM_CMD_LOAD);
+	force_pci_posting(dev);
+	mdelay(200);
+//#endif
+}
+
+inline u16 ieeerate2rtlrate(int rate)
+{
+	switch(rate){
+	case 10:
+	return 0;
+	case 20:
+	return 1;
+	case 55:
+	return 2;
+	case 110:
+	return 3;
+	case 60:
+	return 4;
+	case 90:
+	return 5;
+	case 120:
+	return 6;
+	case 180:
+	return 7;
+	case 240:
+	return 8;
+	case 360:
+	return 9;
+	case 480:
+	return 10;
+	case 540:
+	return 11;
+	default:
+	return 3;
+
+	}
+}
+
+static u16 rtl_rate[] = {10,20,55,110,60,90,120,180,240,360,480,540,720};
+inline u16 rtl8180_rate2rate(short rate)
+{
+	if (rate >12) return 10;
+	return rtl_rate[rate];
+}
+inline u8 rtl8180_IsWirelessBMode(u16 rate)
+{
+	if( ((rate <= 110) && (rate != 60) && (rate != 90)) || (rate == 220) )
+		return 1;
+	else return 0;
+}
+u16 N_DBPSOfRate(u16 DataRate);
+u16 ComputeTxTime(
+	u16		FrameLength,
+	u16		DataRate,
+	u8		bManagementFrame,
+	u8		bShortPreamble
+)
+{
+	u16	FrameTime;
+	u16	N_DBPS;
+	u16	Ceiling;
+
+	if( rtl8180_IsWirelessBMode(DataRate) )
+	{
+		if( bManagementFrame || !bShortPreamble || DataRate == 10 )
+		{	// long preamble
+			FrameTime = (u16)(144+48+(FrameLength*8/(DataRate/10)));
+		}
+		else
+		{	// Short preamble
+			FrameTime = (u16)(72+24+(FrameLength*8/(DataRate/10)));
+		}
+		if( ( FrameLength*8 % (DataRate/10) ) != 0 ) //Get the Ceilling
+				FrameTime ++;
+	} else {	//802.11g DSSS-OFDM PLCP length field calculation.
+		N_DBPS = N_DBPSOfRate(DataRate);
+		Ceiling = (16 + 8*FrameLength + 6) / N_DBPS
+				+ (((16 + 8*FrameLength + 6) % N_DBPS) ? 1 : 0);
+		FrameTime = (u16)(16 + 4 + 4*Ceiling + 6);
+	}
+	return FrameTime;
+}
+u16 N_DBPSOfRate(u16 DataRate)
+{
+	 u16 N_DBPS = 24;
+
+	 switch(DataRate)
+	 {
+	 case 60:
+	  N_DBPS = 24;
+	  break;
+
+	 case 90:
+	  N_DBPS = 36;
+	  break;
+
+	 case 120:
+	  N_DBPS = 48;
+	  break;
+
+	 case 180:
+	  N_DBPS = 72;
+	  break;
+
+	 case 240:
+	  N_DBPS = 96;
+	  break;
+
+	 case 360:
+	  N_DBPS = 144;
+	  break;
+
+	 case 480:
+	  N_DBPS = 192;
+	  break;
+
+	 case 540:
+	  N_DBPS = 216;
+	  break;
+
+	 default:
+	  break;
+	 }
+
+	 return N_DBPS;
+}
+
+//{by amy 080312
+//
+//	Description:
+// 	For Netgear case, they want good-looking singal strength.
+//		2004.12.05, by rcnjko.
+//
+long
+NetgearSignalStrengthTranslate(
+	long LastSS,
+	long CurrSS
+	)
+{
+	long RetSS;
+
+	// Step 1. Scale mapping.
+	if(CurrSS >= 71 && CurrSS <= 100)
+	{
+		RetSS = 90 + ((CurrSS - 70) / 3);
+	}
+	else if(CurrSS >= 41 && CurrSS <= 70)
+	{
+		RetSS = 78 + ((CurrSS - 40) / 3);
+	}
+	else if(CurrSS >= 31 && CurrSS <= 40)
+	{
+		RetSS = 66 + (CurrSS - 30);
+	}
+	else if(CurrSS >= 21 && CurrSS <= 30)
+	{
+		RetSS = 54 + (CurrSS - 20);
+	}
+	else if(CurrSS >= 5 && CurrSS <= 20)
+	{
+		RetSS = 42 + (((CurrSS - 5) * 2) / 3);
+	}
+	else if(CurrSS == 4)
+	{
+		RetSS = 36;
+	}
+	else if(CurrSS == 3)
+	{
+		RetSS = 27;
+	}
+	else if(CurrSS == 2)
+	{
+		RetSS = 18;
+	}
+	else if(CurrSS == 1)
+	{
+		RetSS = 9;
+	}
+	else
+	{
+		RetSS = CurrSS;
+	}
+	//RT_TRACE(COMP_DBG, DBG_LOUD, ("##### After Mapping:  LastSS: %d, CurrSS: %d, RetSS: %d\n", LastSS, CurrSS, RetSS));
+
+	// Step 2. Smoothing.
+	if(LastSS > 0)
+	{
+		RetSS = ((LastSS * 5) + (RetSS)+ 5) / 6;
+	}
+	//RT_TRACE(COMP_DBG, DBG_LOUD, ("$$$$$ After Smoothing:  LastSS: %d, CurrSS: %d, RetSS: %d\n", LastSS, CurrSS, RetSS));
+
+	return RetSS;
+}
+//
+//	Description:
+//		Translate 0-100 signal strength index into dBm.
+//
+long
+TranslateToDbm8185(
+	u8 SignalStrengthIndex	// 0-100 index.
+	)
+{
+	long	SignalPower; // in dBm.
+
+	// Translate to dBm (x=0.5y-95).
+	SignalPower = (long)((SignalStrengthIndex + 1) >> 1);
+	SignalPower -= 95;
+
+	return SignalPower;
+}
+//
+//	Description:
+//		Perform signal smoothing for dynamic mechanism.
+//		This is different with PerformSignalSmoothing8185 in smoothing fomula.
+//		No dramatic adjustion is apply because dynamic mechanism need some degree
+//		of correctness. Ported from 8187B.
+//	2007-02-26, by Bruce.
+//
+void
+PerformUndecoratedSignalSmoothing8185(
+	struct r8180_priv *priv,
+	bool    bCckRate
+	)
+{
+
+
+	// Determin the current packet is CCK rate.
+	priv->bCurCCKPkt = bCckRate;
+
+	if(priv->UndecoratedSmoothedSS >= 0)
+	{
+		priv->UndecoratedSmoothedSS = ( (priv->UndecoratedSmoothedSS * 5) + (priv->SignalStrength * 10) ) / 6;
+	}
+	else
+	{
+		priv->UndecoratedSmoothedSS = priv->SignalStrength * 10;
+	}
+
+	priv->UndercorateSmoothedRxPower = ( (priv->UndercorateSmoothedRxPower * 50) + (priv->RxPower* 11)) / 60;
+
+//	printk("Sommthing SignalSterngth (%d) => UndecoratedSmoothedSS (%d)\n", priv->SignalStrength, priv->UndecoratedSmoothedSS);
+//	printk("Sommthing RxPower (%d) => UndecoratedRxPower (%d)\n", priv->RxPower, priv->UndercorateSmoothedRxPower);
+
+	//if(priv->CurCCKRSSI >= 0 && bCckRate)
+	if(bCckRate)
+	{
+		priv->CurCCKRSSI = priv->RSSI;
+	}
+	else
+	{
+		priv->CurCCKRSSI = 0;
+	}
+
+	// Boundary checking.
+	// TODO: The overflow condition does happen, if we want to fix,
+	// we shall recalculate thresholds first.
+	if(priv->UndecoratedSmoothedSS > 100)
+	{
+//		printk("UndecoratedSmoothedSS(%d) overflow, SignalStrength(%d)\n", priv->UndecoratedSmoothedSS, priv->SignalStrength);
+	}
+	if(priv->UndecoratedSmoothedSS < 0)
+	{
+//		printk("UndecoratedSmoothedSS(%d) underflow, SignalStrength(%d)\n", priv->UndecoratedSmoothedSS, priv->SignalStrength);
+	}
+
+}
+
+//by amy 080312}
+
+/* This is rough RX isr handling routine*/
+void rtl8180_rx(struct net_device *dev)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	struct sk_buff *tmp_skb;
+
+	//struct sk_buff *skb;
+	short first,last;
+	u32 len;
+	int lastlen;
+	unsigned char quality, signal;
+	u8 rate;
+	//u32 *prism_hdr;
+	u32 *tmp,*tmp2;
+	u8 rx_desc_size;
+	u8 padding;
+	//u32 count=0;
+	char rxpower = 0;
+	u32 RXAGC = 0;
+	long RxAGC_dBm = 0;
+	u8	LNA=0, BB=0;
+	u8 	LNA_gain[4]={02, 17, 29, 39};
+	u8  Antenna = 0;
+	struct ieee80211_hdr *hdr;//by amy
+	u16 fc,type;
+	u8 bHwError = 0,bCRC = 0,bICV = 0;
+	//bHwError = 0;
+	//bCRC = 0;
+	//bICV = 0;
+	bool	bCckRate = false;
+	u8     RSSI = 0;
+	long	SignalStrengthIndex = 0;//+by amy 080312
+//	u8 SignalStrength = 0;
+	struct ieee80211_rx_stats stats = {
+		.signal = 0,
+		.noise = -98,
+		.rate = 0,
+	//	.mac_time = jiffies,
+		.freq = IEEE80211_24GHZ_BAND,
+	};
+
+#ifdef CONFIG_RTL8185B
+	stats.nic_type = NIC_8185B;
+	rx_desc_size = 8;
+
+#else
+	stats.nic_type = NIC_8185;
+	rx_desc_size = 4;
+#endif
+	//printk("receive frame!%d\n",count++);
+	//if (!priv->rxbuffer) DMESG ("EE: NIC RX ack, but RX queue corrupted!");
+	//else {
+
+	if ((*(priv->rxringtail)) & (1<<31)) {
+
+		/* we have got an RX int, but the descriptor
+		 * we are pointing is empty*/
+
+		priv->stats.rxnodata++;
+		priv->ieee80211->stats.rx_errors++;
+
+	/*	if (! *(priv->rxring) & (1<<31)) {
+
+			priv->stats.rxreset++;
+			priv->rxringtail=priv->rxring;
+			priv->rxbuffer=priv->rxbufferhead;
+
+		}else{*/
+
+		#if 0
+		/* Maybe it is possible that the NIC has skipped some descriptors or
+		 * it has reset its internal pointer to the beginning of the ring
+		 * we search for the first filled descriptor in the ring, or we break
+		 * putting again the pointer in the old location if we do not found any.
+		 * This is quite dangerous, what does happen if the nic writes
+		 * two descriptor (say A and B) when we have just checked the descriptor
+		 * A and we are going to check the descriptor B..This might happen if the
+		 * interrupt was dummy, there was not really filled descriptors and
+		 * the NIC didn't lose pointer
+		 */
+
+		//priv->stats.rxwrkaround++;
+
+		tmp = priv->rxringtail;
+		while (*(priv->rxringtail) & (1<<31)){
+
+			priv->rxringtail+=4;
+
+			if(priv->rxringtail >=
+				(priv->rxring)+(priv->rxringcount )*4)
+				priv->rxringtail=priv->rxring;
+
+			priv->rxbuffer=(priv->rxbuffer->next);
+
+			if(priv->rxringtail == tmp ){
+				//DMESG("EE: Could not find RX pointer");
+				priv->stats.rxnopointer++;
+				break;
+			}
+		}
+		#else
+
+		tmp2 = NULL;
+		tmp = priv->rxringtail;
+		do{
+			if(tmp == priv->rxring)
+				//tmp  = priv->rxring + (priv->rxringcount )*rx_desc_size; xiong-2006-11-15
+				tmp  = priv->rxring + (priv->rxringcount - 1)*rx_desc_size;
+			else
+				tmp -= rx_desc_size;
+
+			if(! (*tmp & (1<<31)))
+				tmp2 = tmp;
+		}while(tmp != priv->rxring);
+
+		if(tmp2) priv->rxringtail = tmp2;
+		#endif
+		//}
+	}
+
+	/* while there are filled descriptors */
+	while(!(*(priv->rxringtail) & (1<<31))){
+		if(*(priv->rxringtail) & (1<<26))
+			DMESGW("RX buffer overflow");
+		if(*(priv->rxringtail) & (1<<12))
+			priv->stats.rxicverr++;
+
+		if(*(priv->rxringtail) & (1<<27)){
+			priv->stats.rxdmafail++;
+			//DMESG("EE: RX DMA FAILED at buffer pointed by descriptor %x",(u32)priv->rxringtail);
+			goto drop;
+		}
+
+		pci_dma_sync_single_for_cpu(priv->pdev,
+				    priv->rxbuffer->dma,
+				    priv->rxbuffersize * \
+				    sizeof(u8),
+				    PCI_DMA_FROMDEVICE);
+
+		first = *(priv->rxringtail) & (1<<29) ? 1:0;
+		if(first) priv->rx_prevlen=0;
+
+		last = *(priv->rxringtail) & (1<<28) ? 1:0;
+		if(last){
+			lastlen=((*priv->rxringtail) &0xfff);
+
+			/* if the last descriptor (that should
+			 * tell us the total packet len) tell
+			 * us something less than the descriptors
+			 * len we had until now, then there is some
+			 * problem..
+			 * workaround to prevent kernel panic
+			 */
+			if(lastlen < priv->rx_prevlen)
+				len=0;
+			else
+				len=lastlen-priv->rx_prevlen;
+
+			if(*(priv->rxringtail) & (1<<13)) {
+//lastlen=((*priv->rxringtail) &0xfff);
+				if ((*(priv->rxringtail) & 0xfff) <500)
+					priv->stats.rxcrcerrmin++;
+				else if ((*(priv->rxringtail) & 0x0fff) >1000)
+					priv->stats.rxcrcerrmax++;
+				else
+					priv->stats.rxcrcerrmid++;
+
+			}
+
+		}else{
+			len = priv->rxbuffersize;
+		}
+
+#ifdef CONFIG_RTL8185B
+		if(first && last) {
+			padding = ((*(priv->rxringtail+3))&(0x04000000))>>26;
+		}else if(first) {
+			padding = ((*(priv->rxringtail+3))&(0x04000000))>>26;
+			if(padding) {
+				len -= 2;
+			}
+		}else {
+			padding = 0;
+		}
+#ifdef CONFIG_RTL818X_S
+               padding = 0;
+#endif
+#endif
+		priv->rx_prevlen+=len;
+
+		if(priv->rx_prevlen > MAX_FRAG_THRESHOLD + 100){
+			/* HW is probably passing several buggy frames
+			* without FD or LD flag set.
+			* Throw this garbage away to prevent skb
+			* memory exausting
+			*/
+			if(!priv->rx_skb_complete)
+				dev_kfree_skb_any(priv->rx_skb);
+			priv->rx_skb_complete = 1;
+		}
+
+#ifdef DEBUG_RX_FRAG
+		DMESG("Iteration.. len %x",len);
+		if(first) DMESG ("First descriptor");
+		if(last) DMESG("Last descriptor");
+
+#endif
+#ifdef DEBUG_RX_VERBOSE
+		print_buffer( priv->rxbuffer->buf, len);
+#endif
+
+#ifdef CONFIG_RTL8185B
+		signal=(unsigned char)(((*(priv->rxringtail+3))& (0x00ff0000))>>16);
+		signal=(signal&0xfe)>>1;	// Modify by hikaru 6.6
+
+		quality=(unsigned char)((*(priv->rxringtail+3)) & (0xff));
+
+		stats.mac_time[0] = *(priv->rxringtail+1);
+		stats.mac_time[1] = *(priv->rxringtail+2);
+		rxpower =((char)(((*(priv->rxringtail+4))& (0x00ff0000))>>16))/2 - 42;
+		RSSI = ((u8)(((*(priv->rxringtail+3)) & (0x0000ff00))>> 8)) & (0x7f);
+
+#else
+		signal=((*(priv->rxringtail+1))& (0xff0000))>>16;
+		signal=(signal&0xfe)>>1;	// Modify by hikaru 6.6
+
+		quality=((*(priv->rxringtail+1)) & (0xff));
+
+		stats.mac_time[0] = *(priv->rxringtail+2);
+		stats.mac_time[1] = *(priv->rxringtail+3);
+#endif
+		rate=((*(priv->rxringtail)) &
+			((1<<23)|(1<<22)|(1<<21)|(1<<20)))>>20;
+
+		stats.rate = rtl8180_rate2rate(rate);
+		//DMESG("%d",rate);
+		Antenna = (((*(priv->rxringtail +3))& (0x00008000)) == 0 )? 0:1 ;
+//		printk("in rtl8180_rx():Antenna is %d\n",Antenna);
+//by amy for antenna
+		if(!rtl8180_IsWirelessBMode(stats.rate))
+		{ // OFDM rate.
+
+			RxAGC_dBm = rxpower+1;	//bias
+		}
+		else
+		{ // CCK rate.
+			RxAGC_dBm = signal;//bit 0 discard
+
+			LNA = (u8) (RxAGC_dBm & 0x60 ) >> 5 ; //bit 6~ bit 5
+			BB  = (u8) (RxAGC_dBm & 0x1F);  // bit 4 ~ bit 0
+
+   			RxAGC_dBm = -( LNA_gain[LNA] + (BB *2) ); //Pin_11b=-(LNA_gain+BB_gain) (dBm)
+
+			RxAGC_dBm +=4; //bias
+		}
+
+		if(RxAGC_dBm & 0x80) //absolute value
+   			RXAGC= ~(RxAGC_dBm)+1;
+		bCckRate = rtl8180_IsWirelessBMode(stats.rate);
+		// Translate RXAGC into 1-100.
+		if(!rtl8180_IsWirelessBMode(stats.rate))
+		{ // OFDM rate.
+			if(RXAGC>90)
+				RXAGC=90;
+			else if(RXAGC<25)
+				RXAGC=25;
+			RXAGC=(90-RXAGC)*100/65;
+		}
+		else
+		{ // CCK rate.
+			if(RXAGC>95)
+				RXAGC=95;
+			else if(RXAGC<30)
+				RXAGC=30;
+			RXAGC=(95-RXAGC)*100/65;
+		}
+		priv->SignalStrength = (u8)RXAGC;
+		priv->RecvSignalPower = RxAGC_dBm ;  // It can use directly by SD3 CMLin
+		priv->RxPower = rxpower;
+		priv->RSSI = RSSI;
+//{by amy 080312
+		// SQ translation formular is provided by SD3 DZ. 2006.06.27, by rcnjko.
+		if(quality >= 127)
+			quality = 1;//0; //0 will cause epc to show signal zero , walk aroud now;
+		else if(quality < 27)
+			quality = 100;
+		else
+			quality = 127 - quality;
+		priv->SignalQuality = quality;
+		if(!priv->card_8185)
+			printk("check your card type\n");
+
+		stats.signal = (u8)quality;//priv->wstats.qual.level = priv->SignalStrength;
+		stats.signalstrength = RXAGC;
+		if(stats.signalstrength > 100)
+			stats.signalstrength = 100;
+		stats.signalstrength = (stats.signalstrength * 70)/100 + 30;
+	//	printk("==========================>rx : RXAGC is %d,signalstrength is %d\n",RXAGC,stats.signalstrength);
+		stats.rssi = priv->wstats.qual.qual = priv->SignalQuality;
+		stats.noise = priv->wstats.qual.noise = 100 - priv ->wstats.qual.qual;
+//by amy 080312}
+		bHwError = (((*(priv->rxringtail))& (0x00000fff)) == 4080)| (((*(priv->rxringtail))& (0x04000000)) != 0 )
+			| (((*(priv->rxringtail))& (0x08000000)) != 0 )| (((~(*(priv->rxringtail)))& (0x10000000)) != 0 )| (((~(*(priv->rxringtail)))& (0x20000000)) != 0 );
+		bCRC = ((*(priv->rxringtail)) & (0x00002000)) >> 13;
+		bICV = ((*(priv->rxringtail)) & (0x00001000)) >> 12;
+            hdr = (struct ieee80211_hdr *)priv->rxbuffer->buf;
+		    fc = le16_to_cpu(hdr->frame_ctl);
+	        type = WLAN_FC_GET_TYPE(fc);
+
+			if((IEEE80211_FTYPE_CTL != type) &&
+				(eqMacAddr(priv->ieee80211->current_network.bssid, (fc & IEEE80211_FCTL_TODS)? hdr->addr1 : (fc & IEEE80211_FCTL_FROMDS )? hdr->addr2 : hdr->addr3))
+				 && (!bHwError) && (!bCRC)&& (!bICV))
+			{
+//by amy 080312
+				// Perform signal smoothing for dynamic mechanism on demand.
+				// This is different with PerformSignalSmoothing8185 in smoothing fomula.
+				// No dramatic adjustion is apply because dynamic mechanism need some degree
+				// of correctness. 2007.01.23, by shien chang.
+				PerformUndecoratedSignalSmoothing8185(priv,bCckRate);
+				//
+				// For good-looking singal strength.
+				//
+				SignalStrengthIndex = NetgearSignalStrengthTranslate(
+								priv->LastSignalStrengthInPercent,
+								priv->SignalStrength);
+
+				priv->LastSignalStrengthInPercent = SignalStrengthIndex;
+				priv->Stats_SignalStrength = TranslateToDbm8185((u8)SignalStrengthIndex);
+		//
+		// We need more correct power of received packets and the  "SignalStrength" of RxStats is beautified,
+		// so we record the correct power here.
+		//
+				priv->Stats_SignalQuality =(long) (priv->Stats_SignalQuality * 5 + (long)priv->SignalQuality + 5) / 6;
+				priv->Stats_RecvSignalPower = (long)(priv->Stats_RecvSignalPower * 5 + priv->RecvSignalPower -1) / 6;
+
+		// Figure out which antenna that received the lasted packet.
+				priv->LastRxPktAntenna = Antenna ? 1 : 0; // 0: aux, 1: main.
+//by amy 080312
+			    SwAntennaDiversityRxOk8185(dev, priv->SignalStrength);
+			}
+
+//by amy for antenna
+
+
+
+
+
+
+#ifndef DUMMY_RX
+		if(first){
+			if(!priv->rx_skb_complete){
+				/* seems that HW sometimes fails to reiceve and
+				   doesn't provide the last descriptor */
+#ifdef DEBUG_RX_SKB
+				DMESG("going to free incomplete skb");
+#endif
+				dev_kfree_skb_any(priv->rx_skb);
+				priv->stats.rxnolast++;
+#ifdef DEBUG_RX_SKB
+				DMESG("free incomplete skb OK");
+#endif
+			}
+			/* support for prism header has been originally added by Christian */
+			if(priv->prism_hdr && priv->ieee80211->iw_mode == IW_MODE_MONITOR){
+
+#if 0
+				priv->rx_skb = dev_alloc_skb(len+2+PRISM_HDR_SIZE);
+				if(! priv->rx_skb) goto drop;
+
+				prism_hdr = (u32*) skb_put(priv->rx_skb,PRISM_HDR_SIZE);
+				prism_hdr[0]=htonl(0x80211001);        //version
+				prism_hdr[1]=htonl(0x40);              //length
+				prism_hdr[2]=htonl(stats.mac_time[1]);    //mactime (HIGH)
+				prism_hdr[3]=htonl(stats.mac_time[0]);    //mactime (LOW)
+				rdtsc(prism_hdr[5], prism_hdr[4]);         //hostime (LOW+HIGH)
+				prism_hdr[4]=htonl(prism_hdr[4]);          //Byte-Order aendern
+				prism_hdr[5]=htonl(prism_hdr[5]);          //Byte-Order aendern
+				prism_hdr[6]=0x00;                     //phytype
+				prism_hdr[7]=htonl(priv->chan);        //channel
+				prism_hdr[8]=htonl(stats.rate);        //datarate
+				prism_hdr[9]=0x00;                     //antenna
+				prism_hdr[10]=0x00;                    //priority
+				prism_hdr[11]=0x00;                    //ssi_type
+				prism_hdr[12]=htonl(stats.signal);     //ssi_signal
+				prism_hdr[13]=htonl(stats.noise);      //ssi_noise
+				prism_hdr[14]=0x00;                    //preamble
+				prism_hdr[15]=0x00;                    //encoding
+
+#endif
+			}else{
+				priv->rx_skb = dev_alloc_skb(len+2);
+				if( !priv->rx_skb) goto drop;
+#ifdef DEBUG_RX_SKB
+				DMESG("Alloc initial skb %x",len+2);
+#endif
+			}
+
+			priv->rx_skb_complete=0;
+			priv->rx_skb->dev=dev;
+		}else{
+			/* if we are here we should  have already RXed
+			* the first frame.
+			* If we get here and the skb is not allocated then
+			* we have just throw out garbage (skb not allocated)
+			* and we are still rxing garbage....
+			*/
+			if(!priv->rx_skb_complete){
+
+				tmp_skb= dev_alloc_skb(priv->rx_skb->len +len+2);
+
+				if(!tmp_skb) goto drop;
+
+				tmp_skb->dev=dev;
+#ifdef DEBUG_RX_SKB
+				DMESG("Realloc skb %x",len+2);
+#endif
+
+#ifdef DEBUG_RX_SKB
+				DMESG("going copy prev frag %x",priv->rx_skb->len);
+#endif
+				memcpy(skb_put(tmp_skb,priv->rx_skb->len),
+					priv->rx_skb->data,
+					priv->rx_skb->len);
+#ifdef DEBUG_RX_SKB
+				DMESG("skb copy prev frag complete");
+#endif
+
+				dev_kfree_skb_any(priv->rx_skb);
+#ifdef DEBUG_RX_SKB
+				DMESG("prev skb free ok");
+#endif
+
+				priv->rx_skb=tmp_skb;
+			}
+		}
+#ifdef DEBUG_RX_SKB
+		DMESG("going to copy current payload %x",len);
+#endif
+		if(!priv->rx_skb_complete) {
+#ifdef CONFIG_RTL8185B
+			if(padding) {
+				memcpy(skb_put(priv->rx_skb,len),
+					(((unsigned char *)priv->rxbuffer->buf) + 2),len);
+			} else {
+#endif
+				memcpy(skb_put(priv->rx_skb,len),
+					priv->rxbuffer->buf,len);
+#ifdef CONFIG_RTL8185B
+			}
+#endif
+		}
+#ifdef DEBUG_RX_SKB
+		DMESG("current fragment skb copy complete");
+#endif
+
+		if(last && !priv->rx_skb_complete){
+
+#ifdef DEBUG_RX_SKB
+			DMESG("Got last fragment");
+#endif
+
+			if(priv->rx_skb->len > 4)
+				skb_trim(priv->rx_skb,priv->rx_skb->len-4);
+#ifdef DEBUG_RX_SKB
+			DMESG("yanked out crc, passing to the upper layer");
+#endif
+
+#ifndef RX_DONT_PASS_UL
+			if(!ieee80211_rx(priv->ieee80211,
+					 priv->rx_skb, &stats)){
+#ifdef DEBUG_RX
+				DMESGW("Packet not consumed");
+#endif
+#endif // RX_DONT_PASS_UL
+
+				dev_kfree_skb_any(priv->rx_skb);
+#ifndef RX_DONT_PASS_UL
+			}
+#endif
+#ifdef DEBUG_RX
+			else{
+					DMESG("Rcv frag");
+			}
+#endif
+			priv->rx_skb_complete=1;
+		}
+
+#endif //DUMMY_RX
+
+		pci_dma_sync_single_for_device(priv->pdev,
+				    priv->rxbuffer->dma,
+				    priv->rxbuffersize * \
+				    sizeof(u8),
+				    PCI_DMA_FROMDEVICE);
+
+
+drop: // this is used when we have not enought mem
+
+		/* restore the descriptor */
+		*(priv->rxringtail+2)=priv->rxbuffer->dma;
+		*(priv->rxringtail)=*(priv->rxringtail) &~ 0xfff;
+		*(priv->rxringtail)=
+			*(priv->rxringtail) | priv->rxbuffersize;
+
+		*(priv->rxringtail)=
+			*(priv->rxringtail) | (1<<31);
+			//^empty descriptor
+
+			//wmb();
+
+#ifdef DEBUG_RX
+		DMESG("Current descriptor: %x",(u32)priv->rxringtail);
+#endif
+		//unsigned long flags;
+		//spin_lock_irqsave(&priv->irq_lock,flags);
+
+		priv->rxringtail+=rx_desc_size;
+		if(priv->rxringtail >=
+		   (priv->rxring)+(priv->rxringcount )*rx_desc_size)
+			priv->rxringtail=priv->rxring;
+
+		//spin_unlock_irqrestore(&priv->irq_lock,flags);
+
+
+		priv->rxbuffer=(priv->rxbuffer->next);
+
+	}
+
+
+
+//	if(get_curr_tx_free_desc(dev,priority))
+//	ieee80211_sta_ps_sleep(priv->ieee80211, &tmp, &tmp2);
+
+
+
+}
+
+
+void rtl8180_dma_kick(struct net_device *dev, int priority)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+	rtl8180_set_mode(dev,EPROM_CMD_CONFIG);
+/*
+
+	switch(priority){
+
+		case LOW_PRIORITY:
+
+		write_nic_byte(dev,TX_DMA_POLLING,
+		       (1<< TX_DMA_POLLING_LOWPRIORITY_SHIFT) |
+			        priv->dma_poll_mask);
+		break;
+
+		case NORM_PRIORITY:
+
+		write_nic_byte(dev,TX_DMA_POLLING,
+		       (1<< TX_DMA_POLLING_NORMPRIORITY_SHIFT) |
+			        priv->dma_poll_mask);
+		break;
+
+		case HI_PRIORITY:
+
+		write_nic_byte(dev,TX_DMA_POLLING,
+		       (1<< TX_DMA_POLLING_HIPRIORITY_SHIFT) |
+			        priv->dma_poll_mask);
+		break;
+
+	}
+*/
+	write_nic_byte(dev, TX_DMA_POLLING,
+			(1 << (priority + 1)) | priv->dma_poll_mask);
+	rtl8180_set_mode(dev,EPROM_CMD_NORMAL);
+
+	force_pci_posting(dev);
+}
+
+#if 0
+void rtl8180_tx_queues_stop(struct net_device *dev)
+{
+	//struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	u8 dma_poll_mask = (1<<TX_DMA_STOP_LOWPRIORITY_SHIFT);
+	dma_poll_mask |= (1<<TX_DMA_STOP_HIPRIORITY_SHIFT);
+	dma_poll_mask |= (1<<TX_DMA_STOP_NORMPRIORITY_SHIFT);
+	dma_poll_mask |= (1<<TX_DMA_STOP_BEACON_SHIFT);
+
+	rtl8180_set_mode(dev,EPROM_CMD_CONFIG);
+	write_nic_byte(dev,TX_DMA_POLLING,dma_poll_mask);
+	rtl8180_set_mode(dev,EPROM_CMD_NORMAL);
+}
+#endif
+
+void rtl8180_data_hard_stop(struct net_device *dev)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+	rtl8180_set_mode(dev,EPROM_CMD_CONFIG);
+#ifdef CONFIG_RTL8185B
+	priv->dma_poll_stop_mask |= TPPOLLSTOP_AC_VIQ;
+	write_nic_byte(dev,TPPollStop, priv->dma_poll_stop_mask);
+#else
+	priv->dma_poll_mask |= (1<<TX_DMA_STOP_LOWPRIORITY_SHIFT);
+	write_nic_byte(dev,TX_DMA_POLLING,priv->dma_poll_mask);
+#endif
+	rtl8180_set_mode(dev,EPROM_CMD_NORMAL);
+}
+
+
+void rtl8180_data_hard_resume(struct net_device *dev)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+	rtl8180_set_mode(dev,EPROM_CMD_CONFIG);
+#ifdef  CONFIG_RTL8185B
+	priv->dma_poll_stop_mask &= ~(TPPOLLSTOP_AC_VIQ);
+	write_nic_byte(dev,TPPollStop, priv->dma_poll_stop_mask);
+#else
+	priv->dma_poll_mask &= ~(1<<TX_DMA_STOP_LOWPRIORITY_SHIFT);
+	write_nic_byte(dev,TX_DMA_POLLING,priv->dma_poll_mask);
+#endif
+	rtl8180_set_mode(dev,EPROM_CMD_NORMAL);
+}
+
+
+/* this function TX data frames when the ieee80211 stack requires this.
+ * It checks also if we need to stop the ieee tx queue, eventually do it
+ */
+void rtl8180_hard_data_xmit(struct sk_buff *skb,struct net_device *dev, int
+rate)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	int mode;
+	struct ieee80211_hdr_3addr  *h = (struct ieee80211_hdr_3addr  *) skb->data;
+	short morefrag = (h->frame_ctl) & IEEE80211_FCTL_MOREFRAGS;
+	unsigned long flags;
+	int priority;
+	//static int count = 0;
+
+	mode = priv->ieee80211->iw_mode;
+
+	rate = ieeerate2rtlrate(rate);
+	/*
+	* This function doesn't require lock because we make
+	* sure it's called with the tx_lock already acquired.
+	* this come from the kernel's hard_xmit callback (trought
+	* the ieee stack, or from the try_wake_queue (again trought
+	* the ieee stack.
+	*/
+#ifdef CONFIG_RTL8185B
+	priority = AC2Q(skb->priority);
+#else
+	priority = LOW_PRIORITY;
+#endif
+	spin_lock_irqsave(&priv->tx_lock,flags);
+
+	if(priv->ieee80211->bHwRadioOff)
+	{
+		spin_unlock_irqrestore(&priv->tx_lock,flags);
+
+		return;
+	}
+
+	//printk(KERN_WARNING "priority = %d@%d\n", priority, count++);
+	if (!check_nic_enought_desc(dev, priority)){
+		//DMESG("Error: no descriptor left by previous TX (avail %d) ",
+		//	get_curr_tx_free_desc(dev, priority));
+		DMESGW("Error: no descriptor left by previous TX (avail %d) ",
+			get_curr_tx_free_desc(dev, priority));
+	//printk(KERN_WARNING "==============================================================> \n");
+		ieee80211_stop_queue(priv->ieee80211);
+	}
+	rtl8180_tx(dev, skb->data, skb->len, priority, morefrag,0,rate);
+	if (!check_nic_enought_desc(dev, priority))
+		ieee80211_stop_queue(priv->ieee80211);
+
+	//dev_kfree_skb_any(skb);
+	spin_unlock_irqrestore(&priv->tx_lock,flags);
+
+}
+
+/* This is a rough attempt to TX a frame
+ * This is called by the ieee 80211 stack to TX management frames.
+ * If the ring is full packet are dropped (for data frame the queue
+ * is stopped before this can happen). For this reason it is better
+ * if the descriptors are larger than the largest management frame
+ * we intend to TX: i'm unsure what the HW does if it will not found
+ * the last fragment of a frame because it has been dropped...
+ * Since queues for Management and Data frames are different we
+ * might use a different lock than tx_lock (for example mgmt_tx_lock)
+ */
+/* these function may loops if invoked with 0 descriptors or 0 len buffer*/
+int rtl8180_hard_start_xmit(struct sk_buff *skb,struct net_device *dev)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+	unsigned long flags;
+
+	int priority;
+
+#ifdef CONFIG_RTL8185B
+	priority = MANAGE_PRIORITY;
+#else
+	priority = NORM_PRIORITY;
+#endif
+
+	spin_lock_irqsave(&priv->tx_lock,flags);
+
+	if(priv->ieee80211->bHwRadioOff)
+	{
+		spin_unlock_irqrestore(&priv->tx_lock,flags);
+
+		dev_kfree_skb_any(skb);
+		return 0;
+	}
+
+	rtl8180_tx(dev, skb->data, skb->len, priority,
+		0, 0,ieeerate2rtlrate(priv->ieee80211->basic_rate));
+
+	priv->ieee80211->stats.tx_bytes+=skb->len;
+	priv->ieee80211->stats.tx_packets++;
+	spin_unlock_irqrestore(&priv->tx_lock,flags);
+
+	dev_kfree_skb_any(skb);
+	return 0;
+}
+
+// longpre 144+48 shortpre 72+24
+u16 rtl8180_len2duration(u32 len, short rate,short* ext)
+{
+	u16 duration;
+	u16 drift;
+	*ext=0;
+
+	switch(rate){
+	case 0://1mbps
+		*ext=0;
+		duration = ((len+4)<<4) /0x2;
+		drift = ((len+4)<<4) % 0x2;
+		if(drift ==0 ) break;
+		duration++;
+		break;
+
+	case 1://2mbps
+		*ext=0;
+		duration = ((len+4)<<4) /0x4;
+		drift = ((len+4)<<4) % 0x4;
+		if(drift ==0 ) break;
+		duration++;
+		break;
+
+	case 2: //5.5mbps
+		*ext=0;
+		duration = ((len+4)<<4) /0xb;
+		drift = ((len+4)<<4) % 0xb;
+		if(drift ==0 )
+			break;
+		duration++;
+		break;
+
+	default:
+	case 3://11mbps
+		*ext=0;
+		duration = ((len+4)<<4) /0x16;
+		drift = ((len+4)<<4) % 0x16;
+		if(drift ==0 )
+			break;
+		duration++;
+		if(drift > 6)
+			break;
+		*ext=1;
+		break;
+	}
+
+	return duration;
+}
+
+
+void rtl8180_prepare_beacon(struct net_device *dev)
+{
+
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+	struct sk_buff *skb;
+
+	u16 word  = read_nic_word(dev, BcnItv);
+	word &= ~BcnItv_BcnItv; // clear Bcn_Itv
+	word |= cpu_to_le16(priv->ieee80211->current_network.beacon_interval);//0x64;
+	write_nic_word(dev, BcnItv, word);
+
+
+	skb = ieee80211_get_beacon(priv->ieee80211);
+	if(skb){
+		rtl8180_tx(dev,skb->data,skb->len,BEACON_PRIORITY,
+			0,0,ieeerate2rtlrate(priv->ieee80211->basic_rate));
+		dev_kfree_skb_any(skb);
+	}
+	#if 0
+	//DMESG("size %x",len);
+	if(*tail & (1<<31)){
+
+		//DMESG("No more beacon TX desc");
+		return ;
+
+	}
+	//while(! *tail & (1<<31)){
+		*tail= 0; // zeroes header
+
+		*tail = *tail| (1<<29) ; //fist segment of the packet
+		*tail = (*tail) | (1<<28); // last segment
+	//	*tail = *tail | (1<<18); // this is  a beacon frame
+		*(tail+3)=*(tail+3) &~ 0xfff;
+		*(tail+3)=*(tail+3) | len; // buffer lenght
+		*tail = *tail |len;
+		// zeroes the second 32-bits dword of the descriptor
+		*(tail+1)= 0;
+		*tail = *tail | (rate << 24);
+
+			duration = rtl8180_len2duration(len,rate,&ext);
+
+		*(tail+1) = *(tail+1) | ((duration & 0x7fff)<<16);
+
+		*tail = *tail | (1<<31);
+		//^ descriptor ready to be txed
+		if((tail - begin)/8 == priv->txbeaconcount-1)
+			tail=begin;
+		else
+			tail=tail+8;
+	//}
+#endif
+}
+
+/* This function do the real dirty work: it enqueues a TX command
+ * descriptor in the ring buffer, copyes the frame in a TX buffer
+ * and kicks the NIC to ensure it does the DMA transfer.
+ */
+short rtl8180_tx(struct net_device *dev, u8* txbuf, int len, int priority,
+		 short morefrag, short descfrag, int rate)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	u32 *tail,*temp_tail;
+	u32 *begin;
+	u32 *buf;
+	int i;
+	int remain;
+	int buflen;
+	int count;
+	//u16	AckCtsTime;
+	//u16	FrameTime;
+	u16 duration;
+	short ext;
+	struct buffer* buflist;
+	//unsigned long flags;
+#ifdef CONFIG_RTL8185B
+	struct ieee80211_hdr_3addr *frag_hdr = (struct ieee80211_hdr_3addr *)txbuf;
+	u8 dest[ETH_ALEN];
+	u8			bUseShortPreamble = 0;
+	u8			bCTSEnable = 0;
+	u8			bRTSEnable = 0;
+	//u16			RTSRate = 22;
+	//u8			RetryLimit = 0;
+	u16 			Duration = 0;
+	u16			RtsDur = 0;
+	u16			ThisFrameTime = 0;
+	u16			TxDescDuration = 0;
+	u8 			ownbit_flag = false; //added by david woo for sync Tx, 2007.12.14
+#endif
+
+	switch(priority) {
+	case MANAGE_PRIORITY:
+		tail=priv->txmapringtail;
+		begin=priv->txmapring;
+		buflist = priv->txmapbufstail;
+		count = priv->txringcount;
+		break;
+
+	case BK_PRIORITY:
+		tail=priv->txbkpringtail;
+		begin=priv->txbkpring;
+		buflist = priv->txbkpbufstail;
+		count = priv->txringcount;
+		break;
+
+	case BE_PRIORITY:
+		tail=priv->txbepringtail;
+		begin=priv->txbepring;
+		buflist = priv->txbepbufstail;
+		count = priv->txringcount;
+		break;
+
+	case VI_PRIORITY:
+		tail=priv->txvipringtail;
+		begin=priv->txvipring;
+		buflist = priv->txvipbufstail;
+		count = priv->txringcount;
+		break;
+
+	case VO_PRIORITY:
+		tail=priv->txvopringtail;
+		begin=priv->txvopring;
+		buflist = priv->txvopbufstail;
+		count = priv->txringcount;
+		break;
+
+	case HI_PRIORITY:
+		tail=priv->txhpringtail;
+		begin=priv->txhpring;
+		buflist = priv->txhpbufstail;
+		count = priv->txringcount;
+		break;
+
+	case BEACON_PRIORITY:
+		tail=priv->txbeaconringtail;
+		begin=priv->txbeaconring;
+		buflist = priv->txbeaconbufstail;
+		count = priv->txbeaconcount;
+		break;
+
+	default:
+		return -1;
+		break;
+ 	}
+
+	//printk("in rtl8180_tx(): rate is %d\n",priv->ieee80211->rate);
+#if 1
+		memcpy(&dest, frag_hdr->addr1, ETH_ALEN);
+		if (is_multicast_ether_addr(dest) ||
+				is_broadcast_ether_addr(dest))
+		{
+			Duration = 0;
+			RtsDur = 0;
+			bRTSEnable = 0;
+			bCTSEnable = 0;
+
+			ThisFrameTime = ComputeTxTime(len + sCrcLng, rtl8180_rate2rate(rate), 0, bUseShortPreamble);
+			TxDescDuration = ThisFrameTime;
+		} else {// Unicast packet
+			//u8 AckRate;
+			u16 AckTime;
+
+			//YJ,add,080828,for Keep alive
+			priv->NumTxUnicast++;
+
+			// Figure out ACK rate according to BSS basic rate and Tx rate, 2006.03.08 by rcnjko.
+			//AckRate = ComputeAckRate( pMgntInfo->mBrates, (u1Byte)(pTcb->DataRate) );
+			// Figure out ACK time according to the AckRate and assume long preamble is used on receiver, 2006.03.08, by rcnjko.
+			//AckTime = ComputeTxTime( sAckCtsLng/8, AckRate, FALSE, FALSE);
+			//For simplicity, just use the 1M basic rate
+			//AckTime = ComputeTxTime(14, 540,0, 0);	// AckCTSLng = 14 use 1M bps send
+			AckTime = ComputeTxTime(14, 10,0, 0);	// AckCTSLng = 14 use 1M bps send
+			//AckTime = ComputeTxTime(14, 2,false, false);	// AckCTSLng = 14 use 1M bps send
+
+			if ( ((len + sCrcLng) > priv->rts) && priv->rts )
+			{ // RTS/CTS.
+				u16 RtsTime, CtsTime;
+				//u16 CtsRate;
+				bRTSEnable = 1;
+				bCTSEnable = 0;
+
+				// Rate and time required for RTS.
+				RtsTime = ComputeTxTime( sAckCtsLng/8,priv->ieee80211->basic_rate, 0, 0);
+				// Rate and time required for CTS.
+				CtsTime = ComputeTxTime(14, 10,0, 0);	// AckCTSLng = 14 use 1M bps send
+
+				// Figure out time required to transmit this frame.
+				ThisFrameTime = ComputeTxTime(len + sCrcLng,
+						rtl8180_rate2rate(rate),
+						0,
+						bUseShortPreamble);
+
+				// RTS-CTS-ThisFrame-ACK.
+				RtsDur = CtsTime + ThisFrameTime + AckTime + 3*aSifsTime;
+
+				TxDescDuration = RtsTime + RtsDur;
+			}
+			else {// Normal case.
+				bCTSEnable = 0;
+				bRTSEnable = 0;
+				RtsDur = 0;
+
+				ThisFrameTime = ComputeTxTime(len + sCrcLng, rtl8180_rate2rate(rate), 0, bUseShortPreamble);
+				TxDescDuration = ThisFrameTime + aSifsTime + AckTime;
+			}
+
+			if(!(frag_hdr->frame_ctl & IEEE80211_FCTL_MOREFRAGS)) { //no more fragment
+				// ThisFrame-ACK.
+				Duration = aSifsTime + AckTime;
+			} else { // One or more fragments remained.
+				u16 NextFragTime;
+				NextFragTime = ComputeTxTime( len + sCrcLng, //pretend following packet length equal current packet
+						rtl8180_rate2rate(rate),
+						0,
+						bUseShortPreamble );
+
+				//ThisFrag-ACk-NextFrag-ACK.
+				Duration = NextFragTime + 3*aSifsTime + 2*AckTime;
+			}
+
+		} // End of Unicast packet
+
+		frag_hdr->duration_id = Duration;
+#endif
+
+	buflen=priv->txbuffsize;
+	remain=len;
+	temp_tail = tail;
+//printk("================================>buflen = %d, remain = %d!\n", buflen,remain);
+	while(remain!=0){
+#ifdef DEBUG_TX_FRAG
+		DMESG("TX iteration");
+#endif
+#ifdef DEBUG_TX
+		DMESG("TX: filling descriptor %x",(u32)tail);
+#endif
+		mb();
+		if(!buflist){
+			DMESGE("TX buffer error, cannot TX frames. pri %d.", priority);
+			//spin_unlock_irqrestore(&priv->tx_lock,flags);
+			return -1;
+		}
+		buf=buflist->buf;
+
+		if( (*tail & (1<<31)) && (priority != BEACON_PRIORITY)){
+
+				DMESGW("No more TX desc, returning %x of %x",
+				remain,len);
+				priv->stats.txrdu++;
+#ifdef DEBUG_TX_DESC
+				check_tx_ring(dev,priority);
+			//	netif_stop_queue(dev);
+			//	netif_carrier_off(dev);
+#endif
+			//	spin_unlock_irqrestore(&priv->tx_lock,flags);
+
+			return remain;
+
+		}
+
+		*tail= 0; // zeroes header
+		*(tail+1) = 0;
+		*(tail+3) = 0;
+		*(tail+5) = 0;
+		*(tail+6) = 0;
+		*(tail+7) = 0;
+
+		if(priv->card_8185){
+			//FIXME: this should be triggered by HW encryption parameters.
+			*tail |= (1<<15); //no encrypt
+//			*tail |= (1<<30); //raise int when completed
+		}
+	//	*tail = *tail | (1<<16);
+		if(remain==len && !descfrag) {
+			ownbit_flag = false;	//added by david woo,2007.12.14
+#ifdef DEBUG_TX_FRAG
+			DMESG("First descriptor");
+#endif
+			*tail = *tail| (1<<29) ; //fist segment of the packet
+			*tail = *tail |(len);
+		} else {
+			ownbit_flag = true;
+		}
+
+		for(i=0;i<buflen&& remain >0;i++,remain--){
+			((u8*)buf)[i]=txbuf[i]; //copy data into descriptor pointed DMAble buffer
+			if(remain == 4 && i+4 >= buflen) break;
+			/* ensure the last desc has at least 4 bytes payload */
+
+		}
+		txbuf = txbuf + i;
+		*(tail+3)=*(tail+3) &~ 0xfff;
+		*(tail+3)=*(tail+3) | i; // buffer lenght
+		// Use short preamble or not
+		if (priv->ieee80211->current_network.capability&WLAN_CAPABILITY_SHORT_PREAMBLE)
+			if (priv->plcp_preamble_mode==1 && rate!=0)	//  short mode now, not long!
+			//	*tail |= (1<<16);				// enable short preamble mode.
+
+#ifdef CONFIG_RTL8185B
+		if(bCTSEnable) {
+			*tail |= (1<<18);
+		}
+
+		if(bRTSEnable) //rts enable
+		{
+			*tail |= ((ieeerate2rtlrate(priv->ieee80211->basic_rate))<<19);//RTS RATE
+			*tail |= (1<<23);//rts enable
+			*(tail+1) |=(RtsDur&0xffff);//RTS Duration
+		}
+		*(tail+3) |= ((TxDescDuration&0xffff)<<16); //DURATION
+//	        *(tail+3) |= (0xe6<<16);
+        	*(tail+5) |= (11<<8);//(priv->retry_data<<8); //retry lim ;
+#else
+		//Use RTS or not
+#ifdef CONFIG_RTL8187B
+		if ( (len>priv->rts) && priv->rts && priority!=MANAGE_PRIORITY){
+#else
+		if ( (len>priv->rts) && priv->rts && priority==LOW_PRIORITY){
+#endif
+			*tail |= (1<<23);	//enalbe RTS function
+			*tail |= (0<<19);	//use 1M bps send RTS packet
+			AckCtsTime = ComputeTxTime(14, 10,0, 0);	// AckCTSLng = 14 use 1M bps send
+			FrameTime = ComputeTxTime(len + 4, rtl8180_rate2rate(rate), 0, *tail&(1<<16));
+			// RTS/CTS time is calculate as follow
+			duration = FrameTime + 3*10 + 2*AckCtsTime;	//10us is the SifsTime;
+			*(tail+1) |= duration; 	//Need to edit here!  ----hikaru
+		}else{
+			*(tail+1)= 0; // zeroes the second 32-bits dword of the descriptor
+		}
+#endif
+
+		*tail = *tail | ((rate&0xf) << 24);
+		//DMESG("rate %d",rate);
+
+		if(priv->card_8185){
+
+			#if 0
+			*(tail+5)&= ~(1<<24); /* tx ant 0 */
+
+			*(tail+5) &= ~(1<<23); /* random tx agc 23-16 */
+			*(tail+5) |= (1<<22)|(1<<21)|(1<<20)|(1<<19)|(1<<18)|(1<<17)|(1<<16);
+
+			*(tail+5) &=
+~((1<<15)|(1<<14)|(1<<13)|(1<<12)|(1<<11)|(1<<10)|(1<<9)|(1<<8));
+			*(tail+5) |= (7<<8); // Max retry limit
+
+			*(tail+5) &= ~((1<<7)|(1<<6)|(1<<5)|(1<<4)|(1<<3)|(1<<2)|(1<<1)|(1<<0));
+			*(tail+5) |= (8<<4); // Max contention window
+			*(tail+6) |= 4; // Min contention window
+			#endif
+           //    	*(tail+5) = 0;
+		}
+
+		/* hw_plcp_len is not used for rtl8180 chip */
+		/* FIXME */
+		if(priv->card_8185 == 0 || !priv->hw_plcp_len){
+
+			duration = rtl8180_len2duration(len,
+				rate,&ext);
+
+
+#ifdef DEBUG_TX
+			DMESG("PLCP duration %d",duration );
+			//DMESG("drift %d",drift);
+			DMESG("extension %s", (ext==1) ? "on":"off");
+#endif
+			*(tail+1) = *(tail+1) | ((duration & 0x7fff)<<16);
+			if(ext) *(tail+1) = *(tail+1) |(1<<31); //plcp length extension
+		}
+
+		if(morefrag) *tail = (*tail) | (1<<17); // more fragment
+		if(!remain) *tail = (*tail) | (1<<28); // last segment of frame
+
+#ifdef DEBUG_TX_FRAG
+		if(!remain)DMESG("Last descriptor");
+		if(morefrag)DMESG("More frag");
+#endif
+               *(tail+5) = *(tail+5)|(2<<27);
+                *(tail+7) = *(tail+7)|(1<<4);
+
+		wmb();
+		if(ownbit_flag)
+		{
+			*tail = *tail | (1<<31); // descriptor ready to be txed
+		}
+
+#ifdef DEBUG_TX_DESC2
+                printk("tx desc is:\n");
+		DMESG("%8x %8x %8x %8x %8x %8x %8x %8x", tail[0], tail[1], tail[2], tail[3],
+			tail[4], tail[5], tail[6], tail[7]);
+#endif
+
+		if((tail - begin)/8 == count-1)
+			tail=begin;
+
+		else
+			tail=tail+8;
+
+		buflist=buflist->next;
+
+		mb();
+
+		switch(priority) {
+			case MANAGE_PRIORITY:
+				priv->txmapringtail=tail;
+				priv->txmapbufstail=buflist;
+				break;
+
+			case BK_PRIORITY:
+				priv->txbkpringtail=tail;
+				priv->txbkpbufstail=buflist;
+				break;
+
+			case BE_PRIORITY:
+				priv->txbepringtail=tail;
+				priv->txbepbufstail=buflist;
+				break;
+
+			case VI_PRIORITY:
+				priv->txvipringtail=tail;
+				priv->txvipbufstail=buflist;
+				break;
+
+			case VO_PRIORITY:
+				priv->txvopringtail=tail;
+				priv->txvopbufstail=buflist;
+				break;
+
+			case HI_PRIORITY:
+				priv->txhpringtail=tail;
+				priv->txhpbufstail = buflist;
+				break;
+
+			case BEACON_PRIORITY:
+				/* the HW seems to be happy with the 1st
+				 * descriptor filled and the 2nd empty...
+				 * So always update descriptor 1 and never
+				 * touch 2nd
+				 */
+			//	priv->txbeaconringtail=tail;
+			//	priv->txbeaconbufstail=buflist;
+
+				break;
+
+		}
+
+		//rtl8180_dma_kick(dev,priority);
+	}
+	*temp_tail = *temp_tail | (1<<31); // descriptor ready to be txed
+	rtl8180_dma_kick(dev,priority);
+	//spin_unlock_irqrestore(&priv->tx_lock,flags);
+
+	return 0;
+
+}
+
+
+void rtl8180_irq_rx_tasklet(struct r8180_priv * priv);
+
+
+void rtl8180_link_change(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	u16 beacon_interval;
+
+	struct ieee80211_network *net = &priv->ieee80211->current_network;
+//	rtl8180_adapter_start(dev);
+	rtl8180_update_msr(dev);
+
+
+	rtl8180_set_mode(dev,EPROM_CMD_CONFIG);
+
+	write_nic_dword(dev,BSSID,((u32*)net->bssid)[0]);
+	write_nic_word(dev,BSSID+4,((u16*)net->bssid)[2]);
+
+
+	beacon_interval  = read_nic_dword(dev,BEACON_INTERVAL);
+	beacon_interval &= ~ BEACON_INTERVAL_MASK;
+	beacon_interval |= net->beacon_interval;
+	write_nic_dword(dev, BEACON_INTERVAL, beacon_interval);
+
+	rtl8180_set_mode(dev, EPROM_CMD_NORMAL);
+
+
+	/*
+	u16 atim = read_nic_dword(dev,ATIM);
+	u16 = u16 &~ ATIM_MASK;
+	u16 = u16 | beacon->atim;
+	*/
+#if 0
+	if (net->capability & WLAN_CAPABILITY_PRIVACY) {
+		if (priv->hw_wep) {
+			DMESG("Enabling hardware WEP support");
+			rtl8180_set_hw_wep(dev);
+			priv->ieee80211->host_encrypt=0;
+			priv->ieee80211->host_decrypt=0;
+		}
+#ifndef CONFIG_IEEE80211_NOWEP
+		else {
+			priv->ieee80211->host_encrypt=1;
+			priv->ieee80211->host_decrypt=1;
+		}
+#endif
+	}
+#ifndef CONFIG_IEEE80211_NOWEP
+	else{
+		priv->ieee80211->host_encrypt=0;
+		priv->ieee80211->host_decrypt=0;
+	}
+#endif
+#endif
+
+
+	if(priv->card_8185)
+		rtl8180_set_chan(dev, priv->chan);
+
+
+}
+
+void rtl8180_rq_tx_ack(struct net_device *dev){
+
+	struct r8180_priv *priv = ieee80211_priv(dev);
+//	printk("====================>%s\n",__FUNCTION__);
+	write_nic_byte(dev,CONFIG4,read_nic_byte(dev,CONFIG4)|CONFIG4_PWRMGT);
+	priv->ack_tx_to_ieee = 1;
+}
+
+short rtl8180_is_tx_queue_empty(struct net_device *dev){
+
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	u32* d;
+
+	for (d = priv->txmapring;
+		d < priv->txmapring + priv->txringcount;d+=8)
+			if(*d & (1<<31)) return 0;
+
+	for (d = priv->txbkpring;
+		d < priv->txbkpring + priv->txringcount;d+=8)
+			if(*d & (1<<31)) return 0;
+
+	for (d = priv->txbepring;
+		d < priv->txbepring + priv->txringcount;d+=8)
+			if(*d & (1<<31)) return 0;
+
+	for (d = priv->txvipring;
+		d < priv->txvipring + priv->txringcount;d+=8)
+			if(*d & (1<<31)) return 0;
+
+	for (d = priv->txvopring;
+		d < priv->txvopring + priv->txringcount;d+=8)
+			if(*d & (1<<31)) return 0;
+
+	for (d = priv->txhpring;
+		d < priv->txhpring + priv->txringcount;d+=8)
+			if(*d & (1<<31)) return 0;
+	return 1;
+}
+/* FIXME FIXME 5msecs is random */
+#define HW_WAKE_DELAY 5
+
+void rtl8180_hw_wakeup(struct net_device *dev)
+{
+	unsigned long flags;
+
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	spin_lock_irqsave(&priv->ps_lock,flags);
+	//DMESG("Waken up!");
+	write_nic_byte(dev,CONFIG4,read_nic_byte(dev,CONFIG4)&~CONFIG4_PWRMGT);
+
+	if(priv->rf_wakeup)
+		priv->rf_wakeup(dev);
+//	mdelay(HW_WAKE_DELAY);
+	spin_unlock_irqrestore(&priv->ps_lock,flags);
+}
+
+void rtl8180_hw_sleep_down(struct net_device *dev)
+{
+        unsigned long flags;
+
+        struct r8180_priv *priv = ieee80211_priv(dev);
+
+        spin_lock_irqsave(&priv->ps_lock,flags);
+       //DMESG("Sleep!");
+
+        if(priv->rf_sleep)
+                priv->rf_sleep(dev);
+        spin_unlock_irqrestore(&priv->ps_lock,flags);
+}
+
+
+void rtl8180_hw_sleep(struct net_device *dev, u32 th, u32 tl)
+{
+
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	u32 rb = jiffies;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->ps_lock,flags);
+
+	/* Writing HW register with 0 equals to disable
+	 * the timer, that is not really what we want
+	 */
+	tl -= MSECS(4+16+7);
+
+	//if(tl == 0) tl = 1;
+
+	/* FIXME HACK FIXME HACK */
+//	force_pci_posting(dev);
+	//mdelay(1);
+
+//	rb = read_nic_dword(dev, TSFTR);
+
+	/* If the interval in witch we are requested to sleep is too
+	 * short then give up and remain awake
+	 */
+	if(((tl>=rb)&& (tl-rb) <= MSECS(MIN_SLEEP_TIME))
+		||((rb>tl)&& (rb-tl) < MSECS(MIN_SLEEP_TIME))) {
+		spin_unlock_irqrestore(&priv->ps_lock,flags);
+		printk("too short to sleep\n");
+		return;
+	}
+
+//	write_nic_dword(dev, TimerInt, tl);
+//	rb = read_nic_dword(dev, TSFTR);
+	{
+		u32 tmp = (tl>rb)?(tl-rb):(rb-tl);
+	//	if (tl<rb)
+
+		//lzm,add,080828
+		priv->DozePeriodInPast2Sec += jiffies_to_msecs(tmp);
+
+		queue_delayed_work(priv->ieee80211->wq, &priv->ieee80211->hw_wakeup_wq, tmp); //as tl may be less than rb
+	}
+	/* if we suspect the TimerInt is gone beyond tl
+	 * while setting it, then give up
+	 */
+#if 1
+	if(((tl > rb) && ((tl-rb) > MSECS(MAX_SLEEP_TIME)))||
+		((tl < rb) && ((rb-tl) > MSECS(MAX_SLEEP_TIME)))) {
+		spin_unlock_irqrestore(&priv->ps_lock,flags);
+		return;
+	}
+#endif
+//	if(priv->rf_sleep)
+//		priv->rf_sleep(dev);
+
+	queue_work(priv->ieee80211->wq, (void *)&priv->ieee80211->hw_sleep_wq);
+	spin_unlock_irqrestore(&priv->ps_lock,flags);
+}
+
+
+//void rtl8180_wmm_param_update(struct net_device *dev,u8 *ac_param)
+#if LINUX_VERSION_CODE >=KERNEL_VERSION(2,6,20)
+void rtl8180_wmm_param_update(struct work_struct * work)
+{
+	struct ieee80211_device * ieee = container_of(work, struct ieee80211_device,wmm_param_update_wq);
+	//struct r8180_priv *priv = (struct r8180_priv*)(ieee->priv);
+	struct net_device *dev = ieee->dev;
+#else
+void rtl8180_wmm_param_update(struct ieee80211_device *ieee)
+{
+	struct net_device *dev = ieee->dev;
+	struct r8180_priv *priv = ieee80211_priv(dev);
+#endif
+	u8 *ac_param = (u8 *)(ieee->current_network.wmm_param);
+	u8 mode = ieee->current_network.mode;
+	AC_CODING	eACI;
+	AC_PARAM	AcParam;
+	PAC_PARAM	pAcParam;
+	u8 i;
+
+#ifndef CONFIG_RTL8185B
+        //for legacy 8185 keep the PARAM unchange.
+	return;
+#else
+	if(!ieee->current_network.QoS_Enable){
+		//legacy ac_xx_param update
+		AcParam.longData = 0;
+		AcParam.f.AciAifsn.f.AIFSN = 2; // Follow 802.11 DIFS.
+		AcParam.f.AciAifsn.f.ACM = 0;
+		AcParam.f.Ecw.f.ECWmin = 3; // Follow 802.11 CWmin.
+		AcParam.f.Ecw.f.ECWmax = 7; // Follow 802.11 CWmax.
+		AcParam.f.TXOPLimit = 0;
+		for(eACI = 0; eACI < AC_MAX; eACI++){
+			AcParam.f.AciAifsn.f.ACI = (u8)eACI;
+			{
+				u8		u1bAIFS;
+				u32		u4bAcParam;
+				pAcParam = (PAC_PARAM)(&AcParam);
+				// Retrive paramters to udpate.
+				u1bAIFS = pAcParam->f.AciAifsn.f.AIFSN *(((mode&IEEE_G) == IEEE_G)?9:20) + aSifsTime;
+				u4bAcParam = ((((u32)(pAcParam->f.TXOPLimit))<<AC_PARAM_TXOP_LIMIT_OFFSET)|
+					      (((u32)(pAcParam->f.Ecw.f.ECWmax))<<AC_PARAM_ECW_MAX_OFFSET)|
+					      (((u32)(pAcParam->f.Ecw.f.ECWmin))<<AC_PARAM_ECW_MIN_OFFSET)|
+					       (((u32)u1bAIFS) << AC_PARAM_AIFS_OFFSET));
+				switch(eACI){
+					case AC1_BK:
+						write_nic_dword(dev, AC_BK_PARAM, u4bAcParam);
+						break;
+
+					case AC0_BE:
+						write_nic_dword(dev, AC_BE_PARAM, u4bAcParam);
+						break;
+
+					case AC2_VI:
+						write_nic_dword(dev, AC_VI_PARAM, u4bAcParam);
+						break;
+
+					case AC3_VO:
+						write_nic_dword(dev, AC_VO_PARAM, u4bAcParam);
+						break;
+
+					default:
+						printk(KERN_WARNING "SetHwReg8185():invalid ACI: %d!\n", eACI);
+						break;
+				}
+			}
+		}
+		return;
+	}
+
+	for(i = 0; i < AC_MAX; i++){
+		//AcParam.longData = 0;
+		pAcParam = (AC_PARAM * )ac_param;
+		{
+			AC_CODING	eACI;
+			u8		u1bAIFS;
+			u32		u4bAcParam;
+
+			// Retrive paramters to udpate.
+			eACI = pAcParam->f.AciAifsn.f.ACI;
+			//Mode G/A: slotTimeTimer = 9; Mode B: 20
+			u1bAIFS = pAcParam->f.AciAifsn.f.AIFSN * (((mode&IEEE_G) == IEEE_G)?9:20) + aSifsTime;
+			u4bAcParam = (	(((u32)(pAcParam->f.TXOPLimit)) << AC_PARAM_TXOP_LIMIT_OFFSET)	|
+					(((u32)(pAcParam->f.Ecw.f.ECWmax)) << AC_PARAM_ECW_MAX_OFFSET)	|
+					(((u32)(pAcParam->f.Ecw.f.ECWmin)) << AC_PARAM_ECW_MIN_OFFSET)	|
+					(((u32)u1bAIFS) << AC_PARAM_AIFS_OFFSET));
+
+			switch(eACI){
+				case AC1_BK:
+					write_nic_dword(dev, AC_BK_PARAM, u4bAcParam);
+					break;
+
+				case AC0_BE:
+					write_nic_dword(dev, AC_BE_PARAM, u4bAcParam);
+					break;
+
+				case AC2_VI:
+					write_nic_dword(dev, AC_VI_PARAM, u4bAcParam);
+					break;
+
+				case AC3_VO:
+					write_nic_dword(dev, AC_VO_PARAM, u4bAcParam);
+					break;
+
+				default:
+					printk(KERN_WARNING "SetHwReg8185(): invalid ACI: %d !\n", eACI);
+					break;
+			}
+		}
+		ac_param += (sizeof(AC_PARAM));
+	}
+#endif
+}
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+void rtl8180_tx_irq_wq(struct work_struct *work);
+#else
+void rtl8180_tx_irq_wq(struct net_device *dev);
+#endif
+
+
+
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+void rtl8180_restart_wq(struct work_struct *work);
+//void rtl8180_rq_tx_ack(struct work_struct *work);
+#else
+ void rtl8180_restart_wq(struct net_device *dev);
+//void rtl8180_rq_tx_ack(struct net_device *dev);
+#endif
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+void rtl8180_watch_dog_wq(struct work_struct *work);
+#else
+void rtl8180_watch_dog_wq(struct net_device *dev);
+#endif
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+void rtl8180_hw_wakeup_wq(struct work_struct *work);
+#else
+void rtl8180_hw_wakeup_wq(struct net_device *dev);
+#endif
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+void rtl8180_hw_sleep_wq(struct work_struct *work);
+#else
+void rtl8180_hw_sleep_wq(struct net_device *dev);
+#endif
+
+
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+void rtl8180_sw_antenna_wq(struct work_struct *work);
+#else
+void rtl8180_sw_antenna_wq(struct net_device *dev);
+#endif
+ void rtl8180_watch_dog(struct net_device *dev);
+void watch_dog_adaptive(unsigned long data)
+{
+    struct r8180_priv* priv = ieee80211_priv((struct net_device *)data);
+//	DMESG("---->watch_dog_adaptive()\n");
+	if(!priv->up)
+	{
+		DMESG("<----watch_dog_adaptive():driver is not up!\n");
+		return;
+	}
+
+  //      queue_work(priv->ieee80211->wq,&priv->ieee80211->watch_dog_wq);
+//{by amy 080312
+#if 1
+	// Tx High Power Mechanism.
+#ifdef HIGH_POWER
+	if(CheckHighPower((struct net_device *)data))
+	{
+		queue_work(priv->ieee80211->wq, (void *)&priv->ieee80211->tx_pw_wq);
+	}
+#endif
+
+#ifdef CONFIG_RTL818X_S
+	// Tx Power Tracking on 87SE.
+#ifdef TX_TRACK
+	//if( priv->bTxPowerTrack )	//lzm mod 080826
+	if( CheckTxPwrTracking((struct net_device *)data));
+		TxPwrTracking87SE((struct net_device *)data);
+#endif
+#endif
+
+	// Perform DIG immediately.
+#ifdef SW_DIG
+	if(CheckDig((struct net_device *)data) == true)
+	{
+		queue_work(priv->ieee80211->wq, (void *)&priv->ieee80211->hw_dig_wq);
+	}
+#endif
+#endif
+//by amy 080312}
+   	rtl8180_watch_dog((struct net_device *)data);
+
+
+	queue_work(priv->ieee80211->wq, (void *)&priv->ieee80211->GPIOChangeRFWorkItem);
+
+   	priv->watch_dog_timer.expires = jiffies + MSECS(IEEE80211_WATCH_DOG_TIME);
+	add_timer(&priv->watch_dog_timer);
+//        DMESG("<----watch_dog_adaptive()\n");
+}
+
+#ifdef ENABLE_DOT11D
+
+static CHANNEL_LIST ChannelPlan[] = {
+	{{1,2,3,4,5,6,7,8,9,10,11,36,40,44,48,52,56,60,64},19},  		//FCC
+	{{1,2,3,4,5,6,7,8,9,10,11},11},                    				//IC
+	{{1,2,3,4,5,6,7,8,9,10,11,12,13,36,40,44,48,52,56,60,64},21},  	//ETSI
+	{{1,2,3,4,5,6,7,8,9,10,11,12,13,36,40,44,48,52,56,60,64},21},    //Spain. Change to ETSI.
+	{{1,2,3,4,5,6,7,8,9,10,11,12,13,36,40,44,48,52,56,60,64},21},  	//France. Change to ETSI.
+	{{14,36,40,44,48,52,56,60,64},9},						//MKK
+	{{1,2,3,4,5,6,7,8,9,10,11,12,13,14, 36,40,44,48,52,56,60,64},22},//MKK1
+	{{1,2,3,4,5,6,7,8,9,10,11,12,13,36,40,44,48,52,56,60,64},21},	//Israel.
+	{{1,2,3,4,5,6,7,8,9,10,11,12,13,34,38,42,46},17},			// For 11a , TELEC
+	{{1,2,3,4,5,6,7,8,9,10,11,12,13,14},14},  //For Global Domain. 1-11:active scan, 12-14 passive scan. //+YJ, 080626
+	{{1,2,3,4,5,6,7,8,9,10,11,12,13},13} //world wide 13: ch1~ch11 active scan, ch12~13 passive //lzm add 080826
+};
+
+static void rtl8180_set_channel_map(u8 channel_plan, struct ieee80211_device *ieee)
+{
+	int i;
+
+	//lzm add 080826
+	ieee->MinPassiveChnlNum=MAX_CHANNEL_NUMBER+1;
+	ieee->IbssStartChnl=0;
+
+	switch (channel_plan)
+	{
+		case COUNTRY_CODE_FCC:
+		case COUNTRY_CODE_IC:
+		case COUNTRY_CODE_ETSI:
+		case COUNTRY_CODE_SPAIN:
+		case COUNTRY_CODE_FRANCE:
+		case COUNTRY_CODE_MKK:
+		case COUNTRY_CODE_MKK1:
+		case COUNTRY_CODE_ISRAEL:
+		case COUNTRY_CODE_TELEC:
+		{
+			Dot11d_Init(ieee);
+			ieee->bGlobalDomain = false;
+			if (ChannelPlan[channel_plan].Len != 0){
+				// Clear old channel map
+				memset(GET_DOT11D_INFO(ieee)->channel_map, 0, sizeof(GET_DOT11D_INFO(ieee)->channel_map));
+				// Set new channel map
+				for (i=0;i<ChannelPlan[channel_plan].Len;i++)
+				{
+					if(ChannelPlan[channel_plan].Channel[i] <= 14)
+						GET_DOT11D_INFO(ieee)->channel_map[ChannelPlan[channel_plan].Channel[i]] = 1;
+				}
+			}
+			break;
+		}
+		case COUNTRY_CODE_GLOBAL_DOMAIN:
+		{
+			GET_DOT11D_INFO(ieee)->bEnabled = 0;
+			Dot11d_Reset(ieee);
+			ieee->bGlobalDomain = true;
+			break;
+		}
+		case COUNTRY_CODE_WORLD_WIDE_13_INDEX://lzm add 080826
+		{
+		ieee->MinPassiveChnlNum=12;
+		ieee->IbssStartChnl= 10;
+		break;
+		}
+		default:
+		{
+			Dot11d_Init(ieee);
+			ieee->bGlobalDomain = false;
+			memset(GET_DOT11D_INFO(ieee)->channel_map, 0, sizeof(GET_DOT11D_INFO(ieee)->channel_map));
+			for (i=1;i<=14;i++)
+			{
+				GET_DOT11D_INFO(ieee)->channel_map[i] = 1;
+			}
+			break;
+		}
+	}
+}
+#endif
+
+//Add for RF power on power off by lizhaoming 080512
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+void GPIOChangeRFWorkItemCallBack(struct work_struct *work);
+#else
+void GPIOChangeRFWorkItemCallBack(struct ieee80211_device *ieee);
+#endif
+
+//YJ,add,080828
+static void rtl8180_statistics_init(struct Stats *pstats)
+{
+	memset(pstats, 0, sizeof(struct Stats));
+}
+static void rtl8180_link_detect_init(plink_detect_t plink_detect)
+{
+	memset(plink_detect, 0, sizeof(link_detect_t));
+	plink_detect->SlotNum = DEFAULT_SLOT_NUM;
+}
+//YJ,add,080828,end
+
+short rtl8180_init(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	u16 word;
+	u16 version;
+	u8 hw_version;
+	//u8 config3;
+	u32 usValue;
+	u16 tmpu16;
+	int i, j;
+
+#ifdef ENABLE_DOT11D
+#if 0
+	for(i=0;i<0xFF;i++) {
+		if(i%16 == 0)
+			printk("\n[%x]: ", i/16);
+		printk("\t%4.4x", eprom_read(dev,i));
+	}
+#endif
+	priv->channel_plan = eprom_read(dev, EEPROM_COUNTRY_CODE>>1) & 0xFF;
+	if(priv->channel_plan > COUNTRY_CODE_GLOBAL_DOMAIN){
+		printk("rtl8180_init:Error channel plan! Set to default.\n");
+		priv->channel_plan = 0;
+	}
+	//priv->channel_plan = 9;  //Global Domain
+
+	DMESG("Channel plan is %d\n",priv->channel_plan);
+	rtl8180_set_channel_map(priv->channel_plan, priv->ieee80211);
+#else
+	int ch;
+	//Set Default Channel Plan
+	if(!channels){
+		DMESG("No channels, aborting");
+		return -1;
+	}
+	ch=channels;
+	priv->channel_plan = 0;//hikaru
+	 // set channels 1..14 allowed in given locale
+	for (i=1; i<=14; i++) {
+		(priv->ieee80211->channel_map)[i] = (u8)(ch & 0x01);
+		ch >>= 1;
+	}
+#endif
+
+	//memcpy(priv->stats,0,sizeof(struct Stats));
+
+	//FIXME: these constants are placed in a bad pleace.
+	priv->txbuffsize = 2048;//1024;
+	priv->txringcount = 32;//32;
+	priv->rxbuffersize = 2048;//1024;
+	priv->rxringcount = 64;//32;
+	priv->txbeaconcount = 2;
+	priv->rx_skb_complete = 1;
+	//priv->txnp_pending.ispending=0;
+	/* ^^ the SKB does not containt a partial RXed
+	 * packet (is empty)
+	 */
+
+#ifdef CONFIG_RTL8185B
+#ifdef CONFIG_RTL818X_S
+	priv->RegThreeWireMode = HW_THREE_WIRE_SI;
+#else
+        priv->RegThreeWireMode = SW_THREE_WIRE;
+#endif
+#endif
+
+//Add for RF power on power off by lizhaoming 080512
+	priv->RFChangeInProgress = false;
+	priv->SetRFPowerStateInProgress = false;
+	priv->RFProgType = 0;
+	priv->bInHctTest = false;
+
+	priv->irq_enabled=0;
+
+//YJ,modified,080828
+#if 0
+	priv->stats.rxdmafail=0;
+	priv->stats.txrdu=0;
+	priv->stats.rxrdu=0;
+	priv->stats.rxnolast=0;
+	priv->stats.rxnodata=0;
+	//priv->stats.rxreset=0;
+	//priv->stats.rxwrkaround=0;
+	priv->stats.rxnopointer=0;
+	priv->stats.txnperr=0;
+	priv->stats.txresumed=0;
+	priv->stats.rxerr=0;
+	priv->stats.rxoverflow=0;
+	priv->stats.rxint=0;
+	priv->stats.txnpokint=0;
+	priv->stats.txhpokint=0;
+	priv->stats.txhperr=0;
+	priv->stats.ints=0;
+	priv->stats.shints=0;
+	priv->stats.txoverflow=0;
+	priv->stats.txbeacon=0;
+	priv->stats.txbeaconerr=0;
+	priv->stats.txlperr=0;
+	priv->stats.txlpokint=0;
+	priv->stats.txretry=0;//tony 20060601
+	priv->stats.rxcrcerrmin=0;
+	priv->stats.rxcrcerrmid=0;
+	priv->stats.rxcrcerrmax=0;
+	priv->stats.rxicverr=0;
+#else
+	rtl8180_statistics_init(&priv->stats);
+	rtl8180_link_detect_init(&priv->link_detect);
+#endif
+//YJ,modified,080828,end
+
+
+	priv->ack_tx_to_ieee = 0;
+	priv->ieee80211->current_network.beacon_interval = DEFAULT_BEACONINTERVAL;
+	priv->ieee80211->iw_mode = IW_MODE_INFRA;
+	priv->ieee80211->softmac_features  = IEEE_SOFTMAC_SCAN |
+		IEEE_SOFTMAC_ASSOCIATE | IEEE_SOFTMAC_PROBERQ |
+		IEEE_SOFTMAC_PROBERS | IEEE_SOFTMAC_TX_QUEUE;
+	priv->ieee80211->active_scan = 1;
+	priv->ieee80211->rate = 110; //11 mbps
+	priv->ieee80211->modulation = IEEE80211_CCK_MODULATION;
+	priv->ieee80211->host_encrypt = 1;
+	priv->ieee80211->host_decrypt = 1;
+	priv->ieee80211->sta_wake_up = rtl8180_hw_wakeup;
+	priv->ieee80211->ps_request_tx_ack = rtl8180_rq_tx_ack;
+	priv->ieee80211->enter_sleep_state = rtl8180_hw_sleep;
+	priv->ieee80211->ps_is_queue_empty = rtl8180_is_tx_queue_empty;
+
+	priv->hw_wep = hwwep;
+	priv->prism_hdr=0;
+	priv->dev=dev;
+	priv->retry_rts = DEFAULT_RETRY_RTS;
+	priv->retry_data = DEFAULT_RETRY_DATA;
+	priv->RFChangeInProgress = false;
+	priv->SetRFPowerStateInProgress = false;
+	priv->RFProgType = 0;
+	priv->bInHctTest = false;
+	priv->bInactivePs = true;//false;
+	priv->ieee80211->bInactivePs = priv->bInactivePs;
+	priv->bSwRfProcessing = false;
+	priv->eRFPowerState = eRfOff;
+	priv->RfOffReason = 0;
+	priv->LedStrategy = SW_LED_MODE0;
+	//priv->NumRxOkInPeriod = 0;  //YJ,del,080828
+	//priv->NumTxOkInPeriod = 0;  //YJ,del,080828
+	priv->TxPollingTimes = 0;//lzm add 080826
+	priv->bLeisurePs = true;
+	priv->dot11PowerSaveMode = eActive;
+//by amy for antenna
+	priv->AdMinCheckPeriod = 5;
+	priv->AdMaxCheckPeriod = 10;
+// Lower signal strength threshold to fit the HW participation in antenna diversity. +by amy 080312
+	priv->AdMaxRxSsThreshold = 30;//60->30
+	priv->AdRxSsThreshold = 20;//50->20
+	priv->AdCheckPeriod = priv->AdMinCheckPeriod;
+	priv->AdTickCount = 0;
+	priv->AdRxSignalStrength = -1;
+	priv->RegSwAntennaDiversityMechanism = 0;
+	priv->RegDefaultAntenna = 0;
+	priv->SignalStrength = 0;
+	priv->AdRxOkCnt = 0;
+	priv->CurrAntennaIndex = 0;
+	priv->AdRxSsBeforeSwitched = 0;
+	init_timer(&priv->SwAntennaDiversityTimer);
+	priv->SwAntennaDiversityTimer.data = (unsigned long)dev;
+	priv->SwAntennaDiversityTimer.function = (void *)SwAntennaDiversityTimerCallback;
+//by amy for antenna
+//{by amy 080312
+	priv->bDigMechanism = 1;
+	priv->InitialGain = 6;
+	priv->bXtalCalibration = false;
+	priv->XtalCal_Xin = 0;
+	priv->XtalCal_Xout = 0;
+	priv->bTxPowerTrack = false;
+	priv->ThermalMeter = 0;
+	priv->FalseAlarmRegValue = 0;
+	priv->RegDigOfdmFaUpTh = 0xc; // Upper threhold of OFDM false alarm, which is used in DIG.
+	priv->DIG_NumberFallbackVote = 0;
+	priv->DIG_NumberUpgradeVote = 0;
+	priv->LastSignalStrengthInPercent = 0;
+	priv->Stats_SignalStrength = 0;
+	priv->LastRxPktAntenna = 0;
+	priv->SignalQuality = 0; // in 0-100 index.
+	priv->Stats_SignalQuality = 0;
+	priv->RecvSignalPower = 0; // in dBm.
+	priv->Stats_RecvSignalPower = 0;
+	priv->AdMainAntennaRxOkCnt = 0;
+	priv->AdAuxAntennaRxOkCnt = 0;
+	priv->bHWAdSwitched = false;
+	priv->bRegHighPowerMechanism = true;
+	priv->RegHiPwrUpperTh = 77;
+	priv->RegHiPwrLowerTh = 75;
+	priv->RegRSSIHiPwrUpperTh = 70;
+	priv->RegRSSIHiPwrLowerTh = 20;
+	priv->bCurCCKPkt = false;
+	priv->UndecoratedSmoothedSS = -1;
+	priv->bToUpdateTxPwr = false;
+	priv->CurCCKRSSI = 0;
+	priv->RxPower = 0;
+	priv->RSSI = 0;
+	//YJ,add,080828
+	priv->NumTxOkTotal = 0;
+	priv->NumTxUnicast = 0;
+	priv->keepAliveLevel = DEFAULT_KEEP_ALIVE_LEVEL;
+	priv->PowerProfile = POWER_PROFILE_AC;
+	//YJ,add,080828,end
+//by amy for rate adaptive
+    priv->CurrRetryCnt=0;
+    priv->LastRetryCnt=0;
+    priv->LastTxokCnt=0;
+    priv->LastRxokCnt=0;
+    priv->LastRetryRate=0;
+    priv->bTryuping=0;
+    priv->CurrTxRate=0;
+    priv->CurrRetryRate=0;
+    priv->TryupingCount=0;
+    priv->TryupingCountNoData=0;
+    priv->TryDownCountLowData=0;
+    priv->LastTxOKBytes=0;
+    priv->LastFailTxRate=0;
+    priv->LastFailTxRateSS=0;
+    priv->FailTxRateCount=0;
+    priv->LastTxThroughput=0;
+    priv->NumTxOkBytesTotal=0;
+	priv->ForcedDataRate = 0;
+	priv->RegBModeGainStage = 1;
+
+//by amy for rate adaptive
+//by amy 080312}
+	priv->promisc = (dev->flags & IFF_PROMISC) ? 1:0;
+	spin_lock_init(&priv->irq_lock);
+	spin_lock_init(&priv->irq_th_lock);
+	spin_lock_init(&priv->tx_lock);
+	spin_lock_init(&priv->ps_lock);
+	spin_lock_init(&priv->rf_ps_lock);
+	sema_init(&priv->wx_sem,1);
+	sema_init(&priv->rf_state,1);
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+	INIT_WORK(&priv->reset_wq,(void*) rtl8180_restart_wq);
+	INIT_WORK(&priv->tx_irq_wq,(void*) rtl8180_tx_irq_wq);
+	INIT_DELAYED_WORK(&priv->ieee80211->hw_wakeup_wq,(void*) rtl8180_hw_wakeup_wq);
+	INIT_DELAYED_WORK(&priv->ieee80211->hw_sleep_wq,(void*) rtl8180_hw_sleep_wq);
+	//INIT_DELAYED_WORK(&priv->ieee80211->watch_dog_wq,(void*) rtl8180_watch_dog_wq);
+	//INIT_DELAYED_WORK(&priv->ieee80211->sw_antenna_wq,(void*) rtl8180_sw_antenna_wq);
+	INIT_WORK(&priv->ieee80211->wmm_param_update_wq,(void*) rtl8180_wmm_param_update);
+	INIT_DELAYED_WORK(&priv->ieee80211->rate_adapter_wq,(void*)rtl8180_rate_adapter);//+by amy 080312
+	INIT_DELAYED_WORK(&priv->ieee80211->hw_dig_wq,(void*)rtl8180_hw_dig_wq);//+by amy 080312
+	INIT_DELAYED_WORK(&priv->ieee80211->tx_pw_wq,(void*)rtl8180_tx_pw_wq);//+by amy 080312
+
+	//add for RF power on power off by lizhaoming 080512
+	INIT_DELAYED_WORK(&priv->ieee80211->GPIOChangeRFWorkItem,(void*) GPIOChangeRFWorkItemCallBack);
+#else
+	INIT_WORK(&priv->reset_wq,(void*) rtl8180_restart_wq,dev);
+	INIT_WORK(&priv->tx_irq_wq,(void*) rtl8180_tx_irq_wq,dev);
+	//INIT_WORK(&priv->ieee80211->watch_dog_wq,(void*) rtl8180_watch_dog_wq,dev);
+	INIT_WORK(&priv->ieee80211->hw_wakeup_wq,(void*) rtl8180_hw_wakeup_wq,dev);
+	INIT_WORK(&priv->ieee80211->hw_sleep_wq,(void*) rtl8180_hw_sleep_wq,dev);
+	//INIT_WORK(&priv->ieee80211->sw_antenna_wq,(void*) rtl8180_sw_antenna_wq,dev);
+	INIT_WORK(&priv->ieee80211->wmm_param_update_wq,(void*) rtl8180_wmm_param_update,priv->ieee80211);
+    INIT_WORK(&priv->ieee80211->rate_adapter_wq,(void*)rtl8180_rate_adapter,dev);//+by amy 080312
+	INIT_WORK(&priv->ieee80211->hw_dig_wq,(void*)rtl8180_hw_dig_wq,dev);//+by amy 080312
+	INIT_WORK(&priv->ieee80211->tx_pw_wq,(void*)rtl8180_tx_pw_wq,dev);//+by amy 080312
+
+	//add for RF power on power off by lizhaoming 080512
+	INIT_WORK(&priv->ieee80211->GPIOChangeRFWorkItem,(void*) GPIOChangeRFWorkItemCallBack, priv->ieee80211);
+#endif
+	//INIT_WORK(&priv->reset_wq,(void*) rtl8180_restart_wq,dev);
+
+	tasklet_init(&priv->irq_rx_tasklet,
+		     (void(*)(unsigned long)) rtl8180_irq_rx_tasklet,
+		     (unsigned long)priv);
+//by amy
+    init_timer(&priv->watch_dog_timer);
+	priv->watch_dog_timer.data = (unsigned long)dev;
+	priv->watch_dog_timer.function = watch_dog_adaptive;
+//by amy
+
+//{by amy 080312
+//by amy for rate adaptive
+    init_timer(&priv->rateadapter_timer);
+        priv->rateadapter_timer.data = (unsigned long)dev;
+        priv->rateadapter_timer.function = timer_rate_adaptive;
+		priv->RateAdaptivePeriod= RATE_ADAPTIVE_TIMER_PERIOD;
+		priv->bEnhanceTxPwr=false;
+//by amy for rate adaptive
+//by amy 080312}
+	//priv->ieee80211->func =
+	//	kmalloc(sizeof(struct ieee80211_helper_functions),GFP_KERNEL);
+	//memset(priv->ieee80211->func, 0,
+	  //     sizeof(struct ieee80211_helper_functions));
+
+	priv->ieee80211->softmac_hard_start_xmit = rtl8180_hard_start_xmit;
+	priv->ieee80211->set_chan = rtl8180_set_chan;
+	priv->ieee80211->link_change = rtl8180_link_change;
+	priv->ieee80211->softmac_data_hard_start_xmit = rtl8180_hard_data_xmit;
+	priv->ieee80211->data_hard_stop = rtl8180_data_hard_stop;
+	priv->ieee80211->data_hard_resume = rtl8180_data_hard_resume;
+
+        priv->ieee80211->init_wmmparam_flag = 0;
+
+	priv->ieee80211->start_send_beacons = rtl8180_start_tx_beacon;
+	priv->ieee80211->stop_send_beacons = rtl8180_beacon_tx_disable;
+	priv->ieee80211->fts = DEFAULT_FRAG_THRESHOLD;
+
+#ifdef CONFIG_RTL8185B
+	priv->MWIEnable = 0;
+
+	priv->ShortRetryLimit = 7;
+	priv->LongRetryLimit = 7;
+	priv->EarlyRxThreshold = 7;
+
+	priv->CSMethod = (0x01 << 29);
+
+	priv->TransmitConfig	=
+									1<<TCR_DurProcMode_OFFSET |		//for RTL8185B, duration setting by HW
+									(7<<TCR_MXDMA_OFFSET) |	// Max DMA Burst Size per Tx DMA Burst, 7: reservied.
+									(priv->ShortRetryLimit<<TCR_SRL_OFFSET) |	// Short retry limit
+									(priv->LongRetryLimit<<TCR_LRL_OFFSET) |	// Long retry limit
+									(0 ? TCR_SAT : 0);	// FALSE: HW provies PLCP length and LENGEXT, TURE: SW proiveds them
+
+	priv->ReceiveConfig	=
+#ifdef CONFIG_RTL818X_S
+#else
+                                                        priv->CSMethod |
+#endif
+//								RCR_ENMARP |
+								RCR_AMF | RCR_ADF |				//accept management/data
+								RCR_ACF |						//accept control frame for SW AP needs PS-poll, 2005.07.07, by rcnjko.
+								RCR_AB | RCR_AM | RCR_APM |		//accept BC/MC/UC
+								//RCR_AICV | RCR_ACRC32 | 		//accept ICV/CRC error packet
+								(7<<RCR_MXDMA_OFFSET) | // Max DMA Burst Size per Rx DMA Burst, 7: unlimited.
+								(priv->EarlyRxThreshold<<RCR_FIFO_OFFSET) | // Rx FIFO Threshold, 7: No Rx threshold.
+								(priv->EarlyRxThreshold == 7 ? RCR_ONLYERLPKT:0);
+
+	priv->IntrMask		= IMR_TMGDOK | IMR_TBDER | IMR_THPDER |
+								IMR_THPDER | IMR_THPDOK |
+								IMR_TVODER | IMR_TVODOK |
+								IMR_TVIDER | IMR_TVIDOK |
+								IMR_TBEDER | IMR_TBEDOK |
+								IMR_TBKDER | IMR_TBKDOK |
+								IMR_RDU |						// To handle the defragmentation not enough Rx descriptors case. Annie, 2006-03-27.
+								IMR_RER | IMR_ROK |
+								IMR_RQoSOK; // <NOTE> ROK and RQoSOK are mutually exclusive, so, we must handle RQoSOK interrupt to receive QoS frames, 2005.12.09, by rcnjko.
+
+	priv->InitialGain = 6;
+#endif
+
+	hw_version =( read_nic_dword(dev, TCR) & TCR_HWVERID_MASK)>>TCR_HWVERID_SHIFT;
+
+	switch (hw_version){
+#ifdef CONFIG_RTL8185B
+		case HW_VERID_R8185B_B:
+#ifdef CONFIG_RTL818X_S
+                        priv->card_8185 = VERSION_8187S_C;
+		        DMESG("MAC controller is a RTL8187SE b/g");
+			priv->phy_ver = 2;
+			break;
+#else
+			DMESG("MAC controller is a RTL8185B b/g");
+			priv->card_8185 = 3;
+			priv->phy_ver = 2;
+			break;
+#endif
+#endif
+		case HW_VERID_R8185_ABC:
+			DMESG("MAC controller is a RTL8185 b/g");
+			priv->card_8185 = 1;
+			/* you should not find a card with 8225 PHY ver < C*/
+			priv->phy_ver = 2;
+			break;
+
+		case HW_VERID_R8185_D:
+			DMESG("MAC controller is a RTL8185 b/g (V. D)");
+			priv->card_8185 = 2;
+			/* you should not find a card with 8225 PHY ver < C*/
+			priv->phy_ver = 2;
+			break;
+
+		case HW_VERID_R8180_ABCD:
+			DMESG("MAC controller is a RTL8180");
+			priv->card_8185 = 0;
+			break;
+
+		case HW_VERID_R8180_F:
+			DMESG("MAC controller is a RTL8180 (v. F)");
+			priv->card_8185 = 0;
+			break;
+
+		default:
+			DMESGW("MAC chip not recognized: version %x. Assuming RTL8180",hw_version);
+			priv->card_8185 = 0;
+			break;
+	}
+
+	if(priv->card_8185){
+		priv->ieee80211->modulation |= IEEE80211_OFDM_MODULATION;
+		priv->ieee80211->short_slot = 1;
+	}
+	/* you should not found any 8185 Ver B Card */
+	priv->card_8185_Bversion = 0;
+
+#ifdef CONFIG_RTL8185B
+#ifdef CONFIG_RTL818X_S
+	// just for sync 85
+	priv->card_type = PCI;
+        DMESG("This is a PCI NIC");
+#else
+	config3 = read_nic_byte(dev, CONFIG3);
+	if(config3 & 0x8){
+		priv->card_type = CARDBUS;
+		DMESG("This is a CARDBUS NIC");
+	}
+	else if( config3 & 0x4){
+		priv->card_type = MINIPCI;
+		DMESG("This is a MINI-PCI NIC");
+	}else{
+		priv->card_type = PCI;
+		DMESG("This is a PCI NIC");
+	}
+#endif
+#endif
+	priv->enable_gpio0 = 0;
+
+//by amy for antenna
+#ifdef CONFIG_RTL8185B
+	usValue = eprom_read(dev, EEPROM_SW_REVD_OFFSET);
+	DMESG("usValue is 0x%x\n",usValue);
+#ifdef CONFIG_RTL818X_S
+	//3Read AntennaDiversity
+	// SW Antenna Diversity.
+	if(	(usValue & EEPROM_SW_AD_MASK) != EEPROM_SW_AD_ENABLE )
+	{
+		priv->EEPROMSwAntennaDiversity = false;
+		//printk("EEPROM Disable SW Antenna Diversity\n");
+	}
+	else
+	{
+		priv->EEPROMSwAntennaDiversity = true;
+		//printk("EEPROM Enable SW Antenna Diversity\n");
+	}
+	// Default Antenna to use.
+	if( (usValue & EEPROM_DEF_ANT_MASK) != EEPROM_DEF_ANT_1 )
+	{
+		priv->EEPROMDefaultAntenna1 = false;
+		//printk("EEPROM Default Antenna 0\n");
+	}
+	else
+	{
+		priv->EEPROMDefaultAntenna1 = true;
+		//printk("EEPROM Default Antenna 1\n");
+	}
+
+	//
+	// Antenna diversity mechanism. Added by Roger, 2007.11.05.
+	//
+	if( priv->RegSwAntennaDiversityMechanism == 0 ) // Auto
+	{// 0: default from EEPROM.
+		priv->bSwAntennaDiverity = priv->EEPROMSwAntennaDiversity;
+	}
+	else
+	{// 1:disable antenna diversity, 2: enable antenna diversity.
+		priv->bSwAntennaDiverity = ((priv->RegSwAntennaDiversityMechanism == 1)? false : true);
+	}
+	//printk("bSwAntennaDiverity = %d\n", priv->bSwAntennaDiverity);
+
+
+	//
+	// Default antenna settings. Added by Roger, 2007.11.05.
+	//
+	if( priv->RegDefaultAntenna == 0)
+	{// 0: default from EEPROM.
+		priv->bDefaultAntenna1 = priv->EEPROMDefaultAntenna1;
+	}
+	else
+	{// 1: main, 2: aux.
+		priv->bDefaultAntenna1 = ((priv->RegDefaultAntenna== 2) ? true : false);
+	}
+	//printk("bDefaultAntenna1 = %d\n", priv->bDefaultAntenna1);
+#endif
+#endif
+//by amy for antenna
+	/* rtl8185 can calc plcp len in HW.*/
+	priv->hw_plcp_len = 1;
+
+	priv->plcp_preamble_mode = 2;
+	/*the eeprom type is stored in RCR register bit #6 */
+	if (RCR_9356SEL & read_nic_dword(dev, RCR)){
+		priv->epromtype=EPROM_93c56;
+		//DMESG("Reported EEPROM chip is a 93c56 (2Kbit)");
+	}else{
+		priv->epromtype=EPROM_93c46;
+		//DMESG("Reported EEPROM chip is a 93c46 (1Kbit)");
+	}
+
+	dev->get_stats = rtl8180_stats;
+
+	dev->dev_addr[0]=eprom_read(dev,MAC_ADR) & 0xff;
+	dev->dev_addr[1]=(eprom_read(dev,MAC_ADR) & 0xff00)>>8;
+	dev->dev_addr[2]=eprom_read(dev,MAC_ADR+1) & 0xff;
+	dev->dev_addr[3]=(eprom_read(dev,MAC_ADR+1) & 0xff00)>>8;
+	dev->dev_addr[4]=eprom_read(dev,MAC_ADR+2) & 0xff;
+	dev->dev_addr[5]=(eprom_read(dev,MAC_ADR+2) & 0xff00)>>8;
+	//DMESG("Card MAC address is "MAC_FMT, MAC_ARG(dev->dev_addr));
+
+
+	for(i=1,j=0; i<14; i+=2,j++){
+
+		word = eprom_read(dev,EPROM_TXPW_CH1_2 + j);
+		priv->chtxpwr[i]=word & 0xff;
+		priv->chtxpwr[i+1]=(word & 0xff00)>>8;
+#ifdef DEBUG_EPROM
+		DMESG("tx word %x:%x",j,word);
+		DMESG("ch %d pwr %x",i,priv->chtxpwr[i]);
+		DMESG("ch %d pwr %x",i+1,priv->chtxpwr[i+1]);
+#endif
+	}
+	if(priv->card_8185){
+		for(i=1,j=0; i<14; i+=2,j++){
+
+			word = eprom_read(dev,EPROM_TXPW_OFDM_CH1_2 + j);
+			priv->chtxpwr_ofdm[i]=word & 0xff;
+			priv->chtxpwr_ofdm[i+1]=(word & 0xff00)>>8;
+#ifdef DEBUG_EPROM
+			DMESG("ofdm tx word %x:%x",j,word);
+			DMESG("ofdm ch %d pwr %x",i,priv->chtxpwr_ofdm[i]);
+			DMESG("ofdm ch %d pwr %x",i+1,priv->chtxpwr_ofdm[i+1]);
+#endif
+		}
+	}
+//{by amy 080312
+	//3Read crystal calibtration and thermal meter indication on 87SE.
+
+	// By SD3 SY's request. Added by Roger, 2007.12.11.
+
+	tmpu16 = eprom_read(dev, EEPROM_RSV>>1);
+
+	//printk("ReadAdapterInfo8185(): EEPROM_RSV(%04x)\n", tmpu16);
+
+		// Crystal calibration for Xin and Xout resp.
+		priv->XtalCal_Xout = tmpu16 & EEPROM_XTAL_CAL_XOUT_MASK; // 0~7.5pF
+		priv->XtalCal_Xin = (tmpu16 & EEPROM_XTAL_CAL_XIN_MASK)>>4; // 0~7.5pF
+		if((tmpu16 & EEPROM_XTAL_CAL_ENABLE)>>12)
+			priv->bXtalCalibration = true;
+
+		// Thermal meter reference indication.
+		priv->ThermalMeter =  (u8)((tmpu16 & EEPROM_THERMAL_METER_MASK)>>8);
+		if((tmpu16 & EEPROM_THERMAL_METER_ENABLE)>>13)
+			priv->bTxPowerTrack = true;
+
+//by amy 080312}
+#ifdef CONFIG_RTL8185B
+	word = eprom_read(dev,EPROM_TXPW_BASE);
+	priv->cck_txpwr_base = word & 0xf;
+	priv->ofdm_txpwr_base = (word>>4) & 0xf;
+#endif
+
+	version = eprom_read(dev,EPROM_VERSION);
+	DMESG("EEPROM version %x",version);
+	if( (!priv->card_8185) && version < 0x0101){
+		DMESG ("EEPROM version too old, assuming defaults");
+		DMESG ("If you see this message *plase* send your \
+DMESG output to andreamrl@tiscali.it THANKS");
+		priv->digphy=1;
+		priv->antb=0;
+		priv->diversity=1;
+		priv->cs_treshold=0xc;
+		priv->rcr_csense=1;
+		priv->rf_chip=RFCHIPID_PHILIPS;
+	}else{
+		if(!priv->card_8185){
+			u8 rfparam = eprom_read(dev,RF_PARAM);
+			DMESG("RfParam: %x",rfparam);
+
+			priv->digphy = rfparam & (1<<RF_PARAM_DIGPHY_SHIFT) ? 0:1;
+			priv->antb =  rfparam & (1<<RF_PARAM_ANTBDEFAULT_SHIFT) ? 1:0;
+
+			priv->rcr_csense = (rfparam & RF_PARAM_CARRIERSENSE_MASK) >>
+					RF_PARAM_CARRIERSENSE_SHIFT;
+
+			priv->diversity =
+				(read_nic_byte(dev,CONFIG2)&(1<<CONFIG2_ANTENNA_SHIFT)) ? 1:0;
+		}else{
+			priv->rcr_csense = 3;
+		}
+
+		priv->cs_treshold = (eprom_read(dev,ENERGY_TRESHOLD)&0xff00) >>8;
+
+		priv->rf_chip = 0xff & eprom_read(dev,RFCHIPID);
+	}
+
+#ifdef CONFIG_RTL8185B
+#ifdef CONFIG_RTL818X_S
+	priv->rf_chip = RF_ZEBRA4;
+	priv->rf_sleep = rtl8225z4_rf_sleep;
+	priv->rf_wakeup = rtl8225z4_rf_wakeup;
+#else
+        priv->rf_chip = RF_ZEBRA2;
+#endif
+	//DMESG("Card reports RF frontend Realtek 8225z2");
+	//DMESGW("This driver has EXPERIMENTAL support for this chipset.");
+	//DMESGW("use it with care and at your own risk and");
+	DMESGW("**PLEASE** REPORT SUCCESSFUL/UNSUCCESSFUL TO Realtek!");
+
+	priv->rf_close = rtl8225z2_rf_close;
+	priv->rf_init = rtl8225z2_rf_init;
+	priv->rf_set_chan = rtl8225z2_rf_set_chan;
+	priv->rf_set_sens = NULL;
+	//priv->rf_sleep = rtl8225_rf_sleep;
+	//priv->rf_wakeup = rtl8225_rf_wakeup;
+
+#else
+	/* check RF frontend chipset */
+	switch (priv->rf_chip) {
+
+		case RFCHIPID_RTL8225:
+
+		if(priv->card_8185){
+			DMESG("Card reports RF frontend Realtek 8225");
+			DMESGW("This driver has EXPERIMENTAL support for this chipset.");
+			DMESGW("use it with care and at your own risk and");
+			DMESGW("**PLEASE** REPORT SUCCESS/INSUCCESS TO andreamrl@tiscali.it");
+
+			priv->rf_close = rtl8225_rf_close;
+			priv->rf_init = rtl8225_rf_init;
+			priv->rf_set_chan = rtl8225_rf_set_chan;
+			priv->rf_set_sens = NULL;
+			priv->rf_sleep = rtl8225_rf_sleep;
+			priv->rf_wakeup = rtl8225_rf_wakeup;
+
+		}else{
+			DMESGW("Detected RTL8225 radio on a card recognized as RTL8180");
+			DMESGW("This could not be... something went wrong....");
+			return -ENODEV;
+		}
+			break;
+
+		case RFCHIPID_RTL8255:
+		if(priv->card_8185){
+			DMESG("Card reports RF frontend Realtek 8255");
+			DMESGW("This driver has EXPERIMENTAL support for this chipset.");
+			DMESGW("use it with care and at your own risk and");
+			DMESGW("**PLEASE** REPORT SUCCESS/INSUCCESS TO andreamrl@tiscali.it");
+
+			priv->rf_close = rtl8255_rf_close;
+			priv->rf_init = rtl8255_rf_init;
+			priv->rf_set_chan = rtl8255_rf_set_chan;
+			priv->rf_set_sens = NULL;
+			priv->rf_sleep = NULL;
+			priv->rf_wakeup = NULL;
+
+		}else{
+			DMESGW("Detected RTL8255 radio on a card recognized as RTL8180");
+			DMESGW("This could not be... something went wrong....");
+			return -ENODEV;
+		}
+			break;
+
+
+		case RFCHIPID_INTERSIL:
+			DMESGW("Card reports RF frontend by Intersil.");
+			DMESGW("This driver has NO support for this chipset.");
+			return -ENODEV;
+			break;
+
+		case RFCHIPID_RFMD:
+			DMESGW("Card reports RF frontend by RFMD.");
+			DMESGW("This driver has NO support for this chipset.");
+			return -ENODEV;
+			break;
+
+		case RFCHIPID_GCT:
+			DMESGW("Card reports RF frontend by GCT.");
+			DMESGW("This driver has EXPERIMENTAL support for this chipset.");
+			DMESGW("use it with care and at your own risk and");
+			DMESGW("**PLEASE** REPORT SUCCESS/INSUCCESS TO andreamrl@tiscali.it");
+			priv->rf_close = gct_rf_close;
+			priv->rf_init = gct_rf_init;
+			priv->rf_set_chan = gct_rf_set_chan;
+			priv->rf_set_sens = NULL;
+			priv->rf_sleep = NULL;
+			priv->rf_wakeup = NULL;
+			break;
+
+		case RFCHIPID_MAXIM:
+			DMESGW("Card reports RF frontend by MAXIM.");
+			DMESGW("This driver has EXPERIMENTAL support for this chipset.");
+			DMESGW("use it with care and at your own risk and");
+			DMESGW("**PLEASE** REPORT SUCCESS/INSUCCESS TO andreamrl@tiscali.it");
+			priv->rf_close = maxim_rf_close;
+			priv->rf_init = maxim_rf_init;
+			priv->rf_set_chan = maxim_rf_set_chan;
+			priv->rf_set_sens = NULL;
+			priv->rf_sleep = NULL;
+			priv->rf_wakeup = NULL;
+			break;
+
+		case RFCHIPID_PHILIPS:
+			DMESG("Card reports RF frontend by Philips.");
+			DMESG("OK! Philips SA2400 radio chipset is supported.");
+			priv->rf_close = sa2400_rf_close;
+			priv->rf_init = sa2400_rf_init;
+			priv->rf_set_chan = sa2400_rf_set_chan;
+			priv->rf_set_sens = sa2400_rf_set_sens;
+			priv->sens = SA2400_RF_DEF_SENS; /* default sensitivity */
+			priv->max_sens = SA2400_RF_MAX_SENS; /* maximum sensitivity */
+			priv->rf_sleep = NULL;
+			priv->rf_wakeup = NULL;
+
+			if(priv->digphy){
+				DMESGW("Digital PHY found");
+				DMESGW("Philips DIGITAL PHY is untested! *Please*\
+	report success/failure to <andreamrl@tiscali.it>");
+			}else{
+				DMESG ("Analog PHY found");
+			}
+
+			break;
+
+		default:
+			DMESGW("Unknown RF module %x",priv->rf_chip);
+			DMESGW("Exiting...");
+			return -1;
+
+	}
+#endif
+
+
+	if(!priv->card_8185){
+		if(priv->antb)
+			DMESG ("Antenna B is default antenna");
+		else
+			DMESG ("Antenna A is default antenna");
+
+		if(priv->diversity)
+			DMESG ("Antenna diversity is enabled");
+		else
+			DMESG("Antenna diversity is disabled");
+
+		DMESG("Carrier sense %d",priv->rcr_csense);
+	}
+
+	if (0!=alloc_rx_desc_ring(dev, priv->rxbuffersize, priv->rxringcount))
+		return -ENOMEM;
+
+	if (0!=alloc_tx_desc_ring(dev, priv->txbuffsize, priv->txringcount,
+				  TX_MANAGEPRIORITY_RING_ADDR))
+		return -ENOMEM;
+
+	if (0!=alloc_tx_desc_ring(dev, priv->txbuffsize, priv->txringcount,
+				 TX_BKPRIORITY_RING_ADDR))
+		return -ENOMEM;
+
+	if (0!=alloc_tx_desc_ring(dev, priv->txbuffsize, priv->txringcount,
+				 TX_BEPRIORITY_RING_ADDR))
+		return -ENOMEM;
+
+	if (0!=alloc_tx_desc_ring(dev, priv->txbuffsize, priv->txringcount,
+				  TX_VIPRIORITY_RING_ADDR))
+		return -ENOMEM;
+
+	if (0!=alloc_tx_desc_ring(dev, priv->txbuffsize, priv->txringcount,
+				  TX_VOPRIORITY_RING_ADDR))
+		return -ENOMEM;
+
+	if (0!=alloc_tx_desc_ring(dev, priv->txbuffsize, priv->txringcount,
+				  TX_HIGHPRIORITY_RING_ADDR))
+		return -ENOMEM;
+
+	if (0!=alloc_tx_desc_ring(dev, priv->txbuffsize, priv->txbeaconcount,
+				  TX_BEACON_RING_ADDR))
+		return -ENOMEM;
+
+
+	//priv->beacon_buf=NULL;
+
+	if(!priv->card_8185){
+
+		if(read_nic_byte(dev, CONFIG0) & (1<<CONFIG0_WEP40_SHIFT))
+			DMESG ("40-bit WEP is supported in hardware");
+		else
+			DMESG ("40-bit WEP is NOT supported in hardware");
+
+		if(read_nic_byte(dev,CONFIG0) & (1<<CONFIG0_WEP104_SHIFT))
+			DMESG ("104-bit WEP is supported in hardware");
+		else
+			DMESG ("104-bit WEP is NOT supported in hardware");
+	}
+#if !defined(SA_SHIRQ)
+        if(request_irq(dev->irq, (void *)rtl8180_interrupt, IRQF_SHARED, dev->name, dev)){
+#else
+        if(request_irq(dev->irq, (void *)rtl8180_interrupt, SA_SHIRQ, dev->name, dev)){
+#endif
+                DMESGE("Error allocating IRQ %d",dev->irq);
+                return -1;
+	}else{
+		priv->irq=dev->irq;
+		DMESG("IRQ %d",dev->irq);
+	}
+
+#ifdef DEBUG_EPROM
+	dump_eprom(dev);
+#endif
+
+	return 0;
+
+}
+
+
+void rtl8180_no_hw_wep(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	if(!priv->card_8185)
+	{
+		u8 security;
+
+		security  = read_nic_byte(dev, SECURITY);
+		security &=~(1<<SECURITY_WEP_TX_ENABLE_SHIFT);
+		security &=~(1<<SECURITY_WEP_RX_ENABLE_SHIFT);
+
+		write_nic_byte(dev, SECURITY, security);
+
+	}else{
+
+		//FIXME!!!
+	}
+	/*
+	  write_nic_dword(dev,TX_CONF,read_nic_dword(dev,TX_CONF) |
+	  (1<<TX_NOICV_SHIFT) );
+	*/
+//	priv->ieee80211->hw_wep=0;
+}
+
+
+void rtl8180_set_hw_wep(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	u8 pgreg;
+	u8 security;
+	u32 key0_word4;
+
+	pgreg=read_nic_byte(dev, PGSELECT);
+	write_nic_byte(dev, PGSELECT, pgreg &~ (1<<PGSELECT_PG_SHIFT));
+
+	key0_word4 = read_nic_dword(dev, KEY0+4+4+4);
+	key0_word4 &= ~ 0xff;
+	key0_word4 |= priv->key0[3]& 0xff;
+	write_nic_dword(dev,KEY0,(priv->key0[0]));
+	write_nic_dword(dev,KEY0+4,(priv->key0[1]));
+	write_nic_dword(dev,KEY0+4+4,(priv->key0[2]));
+	write_nic_dword(dev,KEY0+4+4+4,(key0_word4));
+
+	/*
+	  TX_CONF,read_nic_dword(dev,TX_CONF) &~(1<<TX_NOICV_SHIFT));
+	*/
+
+	security  = read_nic_byte(dev,SECURITY);
+	security |= (1<<SECURITY_WEP_TX_ENABLE_SHIFT);
+	security |= (1<<SECURITY_WEP_RX_ENABLE_SHIFT);
+	security &= ~ SECURITY_ENCRYP_MASK;
+	security |= (SECURITY_ENCRYP_104<<SECURITY_ENCRYP_SHIFT);
+
+	write_nic_byte(dev, SECURITY, security);
+
+	DMESG("key %x %x %x %x",read_nic_dword(dev,KEY0+4+4+4),
+	      read_nic_dword(dev,KEY0+4+4),read_nic_dword(dev,KEY0+4),
+	      read_nic_dword(dev,KEY0));
+
+	//priv->ieee80211->hw_wep=1;
+}
+
+
+void rtl8185_rf_pins_enable(struct net_device *dev)
+{
+//	u16 tmp;
+//	tmp = read_nic_word(dev, RFPinsEnable);
+	write_nic_word(dev, RFPinsEnable, 0x1fff);// | tmp);
+//	write_nic_word(dev, RFPinsEnable,7 | tmp);
+}
+
+
+void rtl8185_set_anaparam2(struct net_device *dev, u32 a)
+{
+	u8 conf3;
+
+	rtl8180_set_mode(dev, EPROM_CMD_CONFIG);
+
+	conf3 = read_nic_byte(dev, CONFIG3);
+	write_nic_byte(dev, CONFIG3, conf3 | (1<<CONFIG3_ANAPARAM_W_SHIFT));
+	write_nic_dword(dev, ANAPARAM2, a);
+
+	conf3 = read_nic_byte(dev, CONFIG3);
+	write_nic_byte(dev, CONFIG3, conf3 &~(1<<CONFIG3_ANAPARAM_W_SHIFT));
+	rtl8180_set_mode(dev, EPROM_CMD_NORMAL);
+
+}
+
+
+void rtl8180_set_anaparam(struct net_device *dev, u32 a)
+{
+	u8 conf3;
+
+	rtl8180_set_mode(dev, EPROM_CMD_CONFIG);
+
+	conf3 = read_nic_byte(dev, CONFIG3);
+	write_nic_byte(dev, CONFIG3, conf3 | (1<<CONFIG3_ANAPARAM_W_SHIFT));
+	write_nic_dword(dev, ANAPARAM, a);
+
+	conf3 = read_nic_byte(dev, CONFIG3);
+	write_nic_byte(dev, CONFIG3, conf3 &~(1<<CONFIG3_ANAPARAM_W_SHIFT));
+	rtl8180_set_mode(dev, EPROM_CMD_NORMAL);
+}
+
+
+void rtl8185_tx_antenna(struct net_device *dev, u8 ant)
+{
+	write_nic_byte(dev, TX_ANTENNA, ant);
+	force_pci_posting(dev);
+	mdelay(1);
+}
+
+
+void rtl8185_write_phy(struct net_device *dev, u8 adr, u32 data)
+{
+	//u8 phyr;
+	u32 phyw;
+	//int i;
+
+	adr |= 0x80;
+
+	phyw= ((data<<8) | adr);
+#if 0
+
+	write_nic_dword(dev, PHY_ADR, phyw);
+
+	//read_nic_dword(dev, PHY_ADR);
+	for(i=0;i<10;i++){
+		write_nic_dword(dev, PHY_ADR, 0xffffff7f & phyw);
+		phyr = read_nic_byte(dev, PHY_READ);
+		if(phyr == (data&0xff)) break;
+
+	}
+#else
+	// Note that, we must write 0xff7c after 0x7d-0x7f to write BB register.
+	write_nic_byte(dev, 0x7f, ((phyw & 0xff000000) >> 24));
+	write_nic_byte(dev, 0x7e, ((phyw & 0x00ff0000) >> 16));
+	write_nic_byte(dev, 0x7d, ((phyw & 0x0000ff00) >> 8));
+	write_nic_byte(dev, 0x7c, ((phyw & 0x000000ff) ));
+#endif
+	/* this is ok to fail when we write AGC table. check for AGC table might be
+	 * done by masking with 0x7f instead of 0xff
+	 */
+	//if(phyr != (data&0xff)) DMESGW("Phy write timeout %x %x %x", phyr, data,adr);
+}
+
+
+inline void write_phy_ofdm (struct net_device *dev, u8 adr, u32 data)
+{
+	data = data & 0xff;
+	rtl8185_write_phy(dev, adr, data);
+}
+
+
+void write_phy_cck (struct net_device *dev, u8 adr, u32 data)
+{
+	data = data & 0xff;
+	rtl8185_write_phy(dev, adr, data | 0x10000);
+}
+
+
+/* 70*3 = 210 ms
+ * I hope this is enougth
+ */
+#define MAX_PHY 70
+void write_phy(struct net_device *dev, u8 adr, u8 data)
+{
+	u32 phy;
+	int i;
+
+	phy = 0xff0000;
+	phy |= adr;
+	phy |= 0x80; /* this should enable writing */
+	phy |= (data<<8);
+
+	//PHY_ADR, PHY_R and PHY_W  are contig and treated as one dword
+	write_nic_dword(dev,PHY_ADR, phy);
+
+	phy= 0xffff00;
+	phy |= adr;
+
+	write_nic_dword(dev,PHY_ADR, phy);
+	for(i=0;i<MAX_PHY;i++){
+		phy=read_nic_dword(dev,PHY_ADR);
+		phy= phy & 0xff0000;
+		phy= phy >> 16;
+		if(phy == data){ //SUCCESS!
+			force_pci_posting(dev);
+			mdelay(3); //random value
+#ifdef DEBUG_BB
+			DMESG("Phy wr %x,%x",adr,data);
+#endif
+			return;
+		}else{
+			force_pci_posting(dev);
+			mdelay(3); //random value
+		}
+	}
+	DMESGW ("Phy writing %x %x failed!", adr,data);
+}
+
+void rtl8185_set_rate(struct net_device *dev)
+{
+	int i;
+	u16 word;
+	int basic_rate,min_rr_rate,max_rr_rate;
+
+//	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	//if (ieee80211_is_54g(priv->ieee80211->current_network) &&
+//		priv->ieee80211->state == IEEE80211_LINKED){
+	basic_rate = ieeerate2rtlrate(240);
+	min_rr_rate = ieeerate2rtlrate(60);
+	max_rr_rate = ieeerate2rtlrate(240);
+
+//
+//	}else{
+//		basic_rate = ieeerate2rtlrate(20);
+//		min_rr_rate = ieeerate2rtlrate(10);
+//		max_rr_rate = ieeerate2rtlrate(110);
+//	}
+
+	write_nic_byte(dev, RESP_RATE,
+			max_rr_rate<<MAX_RESP_RATE_SHIFT| min_rr_rate<<MIN_RESP_RATE_SHIFT);
+
+	word  = read_nic_word(dev, BRSR);
+	word &= ~BRSR_MBR_8185;
+
+
+	for(i=0;i<=basic_rate;i++)
+		word |= (1<<i);
+
+	write_nic_word(dev, BRSR, word);
+	//DMESG("RR:%x BRSR: %x", read_nic_byte(dev,RESP_RATE),read_nic_word(dev,BRSR));
+}
+
+
+
+void rtl8180_adapter_start(struct net_device *dev)
+{
+        struct r8180_priv *priv = ieee80211_priv(dev);
+	u32 anaparam;
+	u16 word;
+	u8 config3;
+//	int i;
+
+	rtl8180_rtx_disable(dev);
+	rtl8180_reset(dev);
+
+	/* seems that 0xffff or 0xafff will cause
+	 * HW interrupt line crash
+	 */
+
+	//priv->irq_mask = 0xafff;
+//	priv->irq_mask = 0x4fcf;
+
+	/* enable beacon timeout, beacon TX ok and err
+	 * LP tx ok and err, HP TX ok and err, NP TX ok and err,
+	 * RX ok and ERR, and GP timer */
+	priv->irq_mask = 0x6fcf;
+
+	priv->dma_poll_mask = 0;
+
+	rtl8180_beacon_tx_disable(dev);
+
+	if(priv->card_type == CARDBUS ){
+		config3=read_nic_byte(dev, CONFIG3);
+		write_nic_byte(dev,CONFIG3,config3 | CONFIG3_FuncRegEn);
+		write_nic_word(dev,FEMR, FEMR_INTR | FEMR_WKUP | FEMR_GWAKE |
+			read_nic_word(dev, FEMR));
+	}
+	rtl8180_set_mode(dev, EPROM_CMD_CONFIG);
+	write_nic_dword(dev, MAC0, ((u32*)dev->dev_addr)[0]);
+	write_nic_word(dev, MAC4, ((u32*)dev->dev_addr)[1] & 0xffff );
+	rtl8180_set_mode(dev, EPROM_CMD_NORMAL);
+
+	rtl8180_update_msr(dev);
+
+	if(!priv->card_8185){
+		anaparam  = eprom_read(dev,EPROM_ANAPARAM_ADDRLWORD);
+		anaparam |= eprom_read(dev,EPROM_ANAPARAM_ADDRHWORD)<<16;
+
+		rtl8180_set_anaparam(dev,anaparam);
+	}
+	/* These might be unnecessary since we do in rx_enable / tx_enable */
+	fix_rx_fifo(dev);
+	fix_tx_fifo(dev);
+	/*set_nic_rxring(dev);
+	  set_nic_txring(dev);*/
+
+	rtl8180_set_mode(dev, EPROM_CMD_CONFIG);
+
+	/*
+	   The following is very strange. seems to be that 1 means test mode,
+	   but we need to acknolwledges the nic when a packet is ready
+	   altought we set it to 0
+	*/
+
+	write_nic_byte(dev,
+		       CONFIG2, read_nic_byte(dev,CONFIG2) &~\
+		       (1<<CONFIG2_DMA_POLLING_MODE_SHIFT));
+	//^the nic isn't in test mode
+	if(priv->card_8185)
+			write_nic_byte(dev,
+		       CONFIG2, read_nic_byte(dev,CONFIG2)|(1<<4));
+
+	rtl8180_set_mode(dev,EPROM_CMD_NORMAL);
+
+	write_nic_dword(dev,INT_TIMEOUT,0);
+#ifdef DEBUG_REGISTERS
+	rtl8180_dump_reg(dev);
+#endif
+
+	if(!priv->card_8185)
+	{
+		/*
+		experimental - this might be needed to calibrate AGC,
+		anyway it shouldn't hurt
+		*/
+		write_nic_byte(dev, CONFIG5,
+			read_nic_byte(dev, CONFIG5) | (1<<AGCRESET_SHIFT));
+		read_nic_byte(dev, CONFIG5);
+		udelay(15);
+		write_nic_byte(dev, CONFIG5,
+			read_nic_byte(dev, CONFIG5) &~ (1<<AGCRESET_SHIFT));
+	}else{
+
+		write_nic_byte(dev, WPA_CONFIG, 0);
+		//write_nic_byte(dev, TESTR, 0xd);
+	}
+
+	rtl8180_no_hw_wep(dev);
+
+	if(priv->card_8185){
+		rtl8185_set_rate(dev);
+		write_nic_byte(dev, RATE_FALLBACK, 0x81);
+	//	write_nic_byte(dev, 0xdf, 0x15);
+	}else{
+		word  = read_nic_word(dev, BRSR);
+		word &= ~BRSR_MBR;
+		word &= ~BRSR_BPLCP;
+		word |= ieeerate2rtlrate(priv->ieee80211->basic_rate);
+//by amy
+              word |= 0x0f;
+//by amy
+		write_nic_word(dev, BRSR, word);
+	}
+
+
+	if(priv->card_8185){
+		write_nic_byte(dev, GP_ENABLE,read_nic_byte(dev, GP_ENABLE) & ~(1<<6));
+
+		//FIXME cfg 3 ClkRun enable - isn't it ReadOnly ?
+		rtl8180_set_mode(dev, EPROM_CMD_CONFIG);
+		write_nic_byte(dev,CONFIG3, read_nic_byte(dev, CONFIG3)
+|(1<<CONFIG3_CLKRUN_SHIFT));
+		rtl8180_set_mode(dev, EPROM_CMD_NORMAL);
+
+	}
+
+	priv->rf_init(dev);
+
+	if(priv->rf_set_sens != NULL)
+		priv->rf_set_sens(dev,priv->sens);
+	rtl8180_irq_enable(dev);
+
+	netif_start_queue(dev);
+	/*DMESG ("lfree %d",get_curr_tx_free_desc(dev,LOW_PRIORITY));
+
+	DMESG ("nfree %d",get_curr_tx_free_desc(dev,NORM_PRIORITY));
+
+	DMESG ("hfree %d",get_curr_tx_free_desc(dev,HI_PRIORITY));
+	if(check_nic_enought_desc(dev,NORM_PRIORITY)) DMESG("NORM OK");
+	if(check_nic_enought_desc(dev,HI_PRIORITY)) DMESG("HI OK");
+	if(check_nic_enought_desc(dev,LOW_PRIORITY)) DMESG("LOW OK");*/
+}
+
+
+
+/* this configures registers for beacon tx and enables it via
+ * rtl8180_beacon_tx_enable(). rtl8180_beacon_tx_disable() might
+ * be used to stop beacon transmission
+ */
+void rtl8180_start_tx_beacon(struct net_device *dev)
+{
+//	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	u16 word;
+//	DMESG("ring %x %x", priv->txlpringdma,read_nic_dword(dev,TLPDA));
+
+	DMESG("Enabling beacon TX");
+	//write_nic_byte(dev, 0x42,0xe6);// TCR
+//	set_nic_txring(dev);
+//	fix_tx_fifo(dev);
+	rtl8180_prepare_beacon(dev);
+	rtl8180_irq_disable(dev);
+	rtl8180_beacon_tx_enable(dev);
+#if 0
+	rtl8180_set_mode(dev,EPROM_CMD_CONFIG);
+	//write_nic_byte(dev,0x9d,0x20); //DMA Poll
+	//write_nic_word(dev,0x7a,0);
+	//write_nic_word(dev,0x7a,0x8000);
+
+#if 0
+	word  = read_nic_word(dev, BcnItv);
+	word &= ~BcnItv_BcnItv; // clear Bcn_Itv
+	word |= priv->ieee80211->current_network.beacon_interval;//0x64;
+	write_nic_word(dev, BcnItv, word);
+#endif
+#endif
+	word = read_nic_word(dev, AtimWnd) &~ AtimWnd_AtimWnd;
+	write_nic_word(dev, AtimWnd,word);// word |=
+//priv->ieee80211->current_network.atim_window);
+
+	word  = read_nic_word(dev, BintrItv);
+	word &= ~BintrItv_BintrItv;
+	word |= 1000;/*priv->ieee80211->current_network.beacon_interval *
+		((priv->txbeaconcount > 1)?(priv->txbeaconcount-1):1);
+	// FIXME: check if correct ^^ worked with 0x3e8;
+	*/
+	write_nic_word(dev, BintrItv, word);
+
+
+	rtl8180_set_mode(dev, EPROM_CMD_NORMAL);
+
+//	rtl8180_beacon_tx_enable(dev);
+#ifdef CONFIG_RTL8185B
+        rtl8185b_irq_enable(dev);
+#else
+	rtl8180_irq_enable(dev);
+#endif
+	/* VV !!!!!!!!!! VV*/
+	/*
+	rtl8180_set_mode(dev,EPROM_CMD_CONFIG);
+	write_nic_byte(dev,0x9d,0x00);
+	rtl8180_set_mode(dev,EPROM_CMD_NORMAL);
+*/
+//	DMESG("ring %x %x", priv->txlpringdma,read_nic_dword(dev,TLPDA));
+
+}
+
+
+
+/***************************************************************************
+    -------------------------------NET STUFF---------------------------
+***************************************************************************/
+static struct net_device_stats *rtl8180_stats(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	return &priv->ieee80211->stats;
+}
+//
+// Change current and default preamble mode.
+// 2005.01.06, by rcnjko.
+//
+bool
+MgntActSet_802_11_PowerSaveMode(
+	struct r8180_priv *priv,
+	RT_PS_MODE		rtPsMode
+)
+{
+
+	// Currently, we do not change power save mode on IBSS mode.
+	if(priv->ieee80211->iw_mode == IW_MODE_ADHOC)
+	{
+		return false;
+	}
+
+	//
+	// <RJ_NOTE> If we make HW to fill up the PwrMgt bit for us,
+	// some AP will not response to our mgnt frames with PwrMgt bit set,
+	// e.g. cannot associate the AP.
+	// So I commented out it. 2005.02.16, by rcnjko.
+	//
+//	// Change device's power save mode.
+//	Adapter->HalFunc.SetPSModeHandler( Adapter, rtPsMode );
+
+	// Update power save mode configured.
+//	priv->dot11PowerSaveMode = rtPsMode;
+	priv->ieee80211->ps = rtPsMode;
+	// Determine ListenInterval.
+#if 0
+	if(priv->dot11PowerSaveMode == eMaxPs)
+	{
+		priv->ieee80211->ListenInterval = 10;
+	}
+	else
+	{
+		priv->ieee80211->ListenInterval = 2;
+	}
+#endif
+	return true;
+}
+
+//================================================================================
+// Leisure Power Save in linked state.
+//================================================================================
+
+//
+//	Description:
+//		Enter the leisure power save mode.
+//
+void
+LeisurePSEnter(
+	struct r8180_priv *priv
+	)
+{
+	if (priv->bLeisurePs)
+	{
+		if (priv->ieee80211->ps == IEEE80211_PS_DISABLED)
+		{
+			//printk("----Enter PS\n");
+			MgntActSet_802_11_PowerSaveMode(priv, IEEE80211_PS_MBCAST|IEEE80211_PS_UNICAST);//IEEE80211_PS_ENABLE
+		}
+	}
+}
+
+
+//
+//	Description:
+//		Leave the leisure power save mode.
+//
+void
+LeisurePSLeave(
+	struct r8180_priv *priv
+	)
+{
+	if (priv->bLeisurePs)
+	{
+		if (priv->ieee80211->ps != IEEE80211_PS_DISABLED)
+		{
+			//printk("----Leave PS\n");
+			MgntActSet_802_11_PowerSaveMode(priv, IEEE80211_PS_DISABLED);
+		}
+	}
+}
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+void rtl8180_hw_wakeup_wq (struct work_struct *work)
+{
+//	struct r8180_priv *priv = container_of(work, struct r8180_priv, watch_dog_wq);
+//	struct ieee80211_device * ieee = (struct ieee80211_device*)
+//	                                       container_of(work, struct ieee80211_device, watch_dog_wq);
+	struct delayed_work *dwork = container_of(work,struct delayed_work,work);
+	struct ieee80211_device *ieee = container_of(dwork,struct ieee80211_device,hw_wakeup_wq);
+	struct net_device *dev = ieee->dev;
+#else
+void rtl8180_hw_wakeup_wq(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+#endif
+
+//	printk("dev is %d\n",dev);
+//	printk("&*&(^*(&(&=========>%s()\n", __FUNCTION__);
+	rtl8180_hw_wakeup(dev);
+
+}
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+void rtl8180_hw_sleep_wq (struct work_struct *work)
+{
+//      struct r8180_priv *priv = container_of(work, struct r8180_priv, watch_dog_wq);
+//      struct ieee80211_device * ieee = (struct ieee80211_device*)
+//                                             container_of(work, struct ieee80211_device, watch_dog_wq);
+        struct delayed_work *dwork = container_of(work,struct delayed_work,work);
+        struct ieee80211_device *ieee = container_of(dwork,struct ieee80211_device,hw_sleep_wq);
+        struct net_device *dev = ieee->dev;
+#else
+void rtl8180_hw_sleep_wq(struct net_device *dev)
+{
+        struct r8180_priv *priv = ieee80211_priv(dev);
+#endif
+
+        rtl8180_hw_sleep_down(dev);
+}
+
+//YJ,add,080828,for KeepAlive
+static void MgntLinkKeepAlive(struct r8180_priv *priv )
+{
+	if (priv->keepAliveLevel == 0)
+		return;
+
+	if(priv->ieee80211->state == IEEE80211_LINKED)
+	{
+		//
+		// Keep-Alive.
+		//
+		//printk("LastTx:%d Tx:%d LastRx:%d Rx:%ld Idle:%d\n",priv->link_detect.LastNumTxUnicast,priv->NumTxUnicast, priv->link_detect.LastNumRxUnicast, priv->ieee80211->NumRxUnicast, priv->link_detect.IdleCount);
+
+		if ( (priv->keepAliveLevel== 2) ||
+			(priv->link_detect.LastNumTxUnicast == priv->NumTxUnicast &&
+			priv->link_detect.LastNumRxUnicast == priv->ieee80211->NumRxUnicast )
+			)
+		{
+			priv->link_detect.IdleCount++;
+
+			//
+			// Send a Keep-Alive packet packet to AP if we had been idle for a while.
+			//
+			if(priv->link_detect.IdleCount >= ((KEEP_ALIVE_INTERVAL / CHECK_FOR_HANG_PERIOD)-1) )
+			{
+				priv->link_detect.IdleCount = 0;
+				ieee80211_sta_ps_send_null_frame(priv->ieee80211, false);
+			}
+		}
+		else
+		{
+			priv->link_detect.IdleCount = 0;
+		}
+		priv->link_detect.LastNumTxUnicast = priv->NumTxUnicast;
+		priv->link_detect.LastNumRxUnicast = priv->ieee80211->NumRxUnicast;
+	}
+}
+//YJ,add,080828,for KeepAlive,end
+
+static u8 read_acadapter_file(char *filename);
+void rtl8180_watch_dog(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	bool bEnterPS = false;
+	bool bBusyTraffic = false;
+	u32 TotalRxNum = 0;
+	u16 SlotIndex = 0;
+	u16 i = 0;
+#ifdef ENABLE_IPS
+	if(priv->ieee80211->actscanning == false){
+		if((priv->ieee80211->iw_mode != IW_MODE_ADHOC) && (priv->ieee80211->state == IEEE80211_NOLINK) && (priv->ieee80211->beinretry == false) && (priv->eRFPowerState == eRfOn)){
+			IPSEnter(dev);
+		}
+	}
+#endif
+	//YJ,add,080828,for link state check
+	if((priv->ieee80211->state == IEEE80211_LINKED) && (priv->ieee80211->iw_mode == IW_MODE_INFRA)){
+		SlotIndex = (priv->link_detect.SlotIndex++) % priv->link_detect.SlotNum;
+		priv->link_detect.RxFrameNum[SlotIndex] = priv->ieee80211->NumRxDataInPeriod + priv->ieee80211->NumRxBcnInPeriod;
+		for( i=0; i<priv->link_detect.SlotNum; i++ )
+			TotalRxNum+= priv->link_detect.RxFrameNum[i];
+		//printk("&&&&&=== TotalRxNum = %d\n", TotalRxNum);
+		if(TotalRxNum == 0){
+			priv->ieee80211->state = IEEE80211_ASSOCIATING;
+			queue_work(priv->ieee80211->wq, &priv->ieee80211->associate_procedure_wq);
+		}
+	}
+
+	//YJ,add,080828,for KeepAlive
+	MgntLinkKeepAlive(priv);
+
+	//YJ,add,080828,for LPS
+#ifdef ENABLE_LPS
+	if(priv->PowerProfile == POWER_PROFILE_BATTERY )
+	{
+		//Turn on LeisurePS on battery power
+		//printk("!!!!!On battery power\n");
+		priv->bLeisurePs = true;
+	}
+	else if(priv->PowerProfile == POWER_PROFILE_AC )
+	{
+		// Turn off LeisurePS on AC power
+		//printk("----On AC power\n");
+		LeisurePSLeave(priv);
+		priv->bLeisurePs= false;
+	}
+#endif
+
+#if 0
+#ifndef ENABLE_LPS
+	if(priv->ieee80211->state == IEEE80211_LINKED){
+		if(	priv->NumRxOkInPeriod> 666 ||
+			priv->NumTxOkInPeriod > 666 ) {
+			bBusyTraffic = true;
+		}
+		if((priv->ieee80211->NumRxData + priv->NumTxOkInPeriod)<8) {
+			bEnterPS= true;
+		}
+		if(bEnterPS) {
+			LeisurePSEnter(priv);
+		}
+		else {
+			LeisurePSLeave(priv);
+		}
+	}
+	else	{
+		LeisurePSLeave(priv);
+	}
+#endif
+	priv->NumRxOkInPeriod = 0;
+	priv->NumTxOkInPeriod = 0;
+	priv->ieee80211->NumRxData = 0;
+#else
+#ifdef ENABLE_LPS
+	if(priv->ieee80211->state == IEEE80211_LINKED){
+		priv->link_detect.NumRxOkInPeriod = priv->ieee80211->NumRxDataInPeriod;
+		//printk("TxOk=%d RxOk=%d\n", priv->link_detect.NumTxOkInPeriod, priv->link_detect.NumRxOkInPeriod);
+		if(	priv->link_detect.NumRxOkInPeriod> 666 ||
+			priv->link_detect.NumTxOkInPeriod> 666 ) {
+			bBusyTraffic = true;
+		}
+		if(((priv->link_detect.NumRxOkInPeriod + priv->link_detect.NumTxOkInPeriod) > 8)
+			|| (priv->link_detect.NumRxOkInPeriod > 2)) {
+			bEnterPS= false;
+		}
+		else {
+			bEnterPS= true;
+		}
+
+		if(bEnterPS) {
+			LeisurePSEnter(priv);
+		}
+		else {
+			LeisurePSLeave(priv);
+		}
+	}
+	else{
+		LeisurePSLeave(priv);
+	}
+#endif
+	priv->link_detect.bBusyTraffic = bBusyTraffic;
+	priv->link_detect.NumRxOkInPeriod = 0;
+	priv->link_detect.NumTxOkInPeriod = 0;
+	priv->ieee80211->NumRxDataInPeriod = 0;
+	priv->ieee80211->NumRxBcnInPeriod = 0;
+#endif
+}
+int _rtl8180_up(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	//int i;
+
+	priv->up=1;
+
+	DMESG("Bringing up iface");
+#ifdef CONFIG_RTL8185B
+	rtl8185b_adapter_start(dev);
+	rtl8185b_rx_enable(dev);
+	rtl8185b_tx_enable(dev);
+#else
+	rtl8180_adapter_start(dev);
+	rtl8180_rx_enable(dev);
+	rtl8180_tx_enable(dev);
+#endif
+#ifdef ENABLE_IPS
+	if(priv->bInactivePs){
+		if(priv->ieee80211->iw_mode == IW_MODE_ADHOC)
+			IPSLeave(dev);
+	}
+#endif
+//by amy 080312
+#ifdef RATE_ADAPT
+       timer_rate_adaptive((unsigned long)dev);
+#endif
+//by amy 080312
+	watch_dog_adaptive((unsigned long)dev);
+#ifdef SW_ANTE
+        if(priv->bSwAntennaDiverity)
+			SwAntennaDiversityTimerCallback(dev);
+#endif
+//	IPSEnter(dev);
+	ieee80211_softmac_start_protocol(priv->ieee80211);
+
+//Add for RF power on power off by lizhaoming 080512
+//	priv->eRFPowerState = eRfOn;
+//	printk("\n--------Start queue_work:GPIOChangeRFWorkItem");
+//	queue_delayed_work(priv->ieee80211->wq,&priv->ieee80211->GPIOChangeRFWorkItem,1000);
+
+	return 0;
+}
+
+
+int rtl8180_open(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	int ret;
+
+	down(&priv->wx_sem);
+	ret = rtl8180_up(dev);
+	up(&priv->wx_sem);
+	return ret;
+
+}
+
+
+int rtl8180_up(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	if (priv->up == 1) return -1;
+
+	return _rtl8180_up(dev);
+}
+
+
+int rtl8180_close(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	int ret;
+
+	down(&priv->wx_sem);
+	ret = rtl8180_down(dev);
+	up(&priv->wx_sem);
+
+	return ret;
+
+}
+
+int rtl8180_down(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	if (priv->up == 0) return -1;
+
+	priv->up=0;
+
+	ieee80211_softmac_stop_protocol(priv->ieee80211);
+	/* FIXME */
+	if (!netif_queue_stopped(dev))
+		netif_stop_queue(dev);
+	rtl8180_rtx_disable(dev);
+	rtl8180_irq_disable(dev);
+	del_timer_sync(&priv->watch_dog_timer);
+	//cancel_delayed_work(&priv->ieee80211->watch_dog_wq);
+//{by amy 080312
+    del_timer_sync(&priv->rateadapter_timer);
+    cancel_delayed_work(&priv->ieee80211->rate_adapter_wq);
+//by amy 080312}
+	cancel_delayed_work(&priv->ieee80211->hw_wakeup_wq);
+	cancel_delayed_work(&priv->ieee80211->hw_sleep_wq);
+	cancel_delayed_work(&priv->ieee80211->hw_dig_wq);
+	cancel_delayed_work(&priv->ieee80211->tx_pw_wq);
+	del_timer_sync(&priv->SwAntennaDiversityTimer);
+	SetZebraRFPowerState8185(dev,eRfOff);
+	//ieee80211_softmac_stop_protocol(priv->ieee80211);
+	memset(&(priv->ieee80211->current_network),0,sizeof(struct ieee80211_network));
+	priv->ieee80211->state = IEEE80211_NOLINK;
+	return 0;
+}
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+void rtl8180_restart_wq(struct work_struct *work)
+{
+	struct r8180_priv *priv = container_of(work, struct r8180_priv, reset_wq);
+	struct net_device *dev = priv->dev;
+#else
+void rtl8180_restart_wq(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+#endif
+	down(&priv->wx_sem);
+
+	rtl8180_commit(dev);
+
+	up(&priv->wx_sem);
+}
+
+void rtl8180_restart(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	//rtl8180_commit(dev);
+	schedule_work(&priv->reset_wq);
+	//DMESG("TXTIMEOUT");
+}
+
+
+void rtl8180_commit(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	if (priv->up == 0) return ;
+//+by amy 080312
+	del_timer_sync(&priv->watch_dog_timer);
+	//cancel_delayed_work(&priv->ieee80211->watch_dog_wq);
+//{by amy 080312
+//by amy for rate adaptive
+    del_timer_sync(&priv->rateadapter_timer);
+    cancel_delayed_work(&priv->ieee80211->rate_adapter_wq);
+//by amy for rate adaptive
+//by amy 080312}
+	cancel_delayed_work(&priv->ieee80211->hw_wakeup_wq);
+	cancel_delayed_work(&priv->ieee80211->hw_sleep_wq);
+	cancel_delayed_work(&priv->ieee80211->hw_dig_wq);
+	cancel_delayed_work(&priv->ieee80211->tx_pw_wq);
+	del_timer_sync(&priv->SwAntennaDiversityTimer);
+	ieee80211_softmac_stop_protocol(priv->ieee80211);
+	rtl8180_irq_disable(dev);
+	rtl8180_rtx_disable(dev);
+	_rtl8180_up(dev);
+}
+
+
+static void r8180_set_multicast(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	short promisc;
+
+	//down(&priv->wx_sem);
+
+	promisc = (dev->flags & IFF_PROMISC) ? 1:0;
+
+	if (promisc != priv->promisc)
+		rtl8180_restart(dev);
+
+	priv->promisc = promisc;
+
+	//up(&priv->wx_sem);
+}
+
+#if 0
+/* this is called by the kernel when it needs to TX a 802.3 encapsulated frame*/
+int rtl8180_8023_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	int ret;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->tx_lock,flags);
+	ret = ieee80211_r8180_8023_hardstartxmit(skb,priv->ieee80211);
+	spin_unlock_irqrestore(&priv->tx_lock,flags);
+	return ret;
+}
+#endif
+
+int r8180_set_mac_adr(struct net_device *dev, void *mac)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	struct sockaddr *addr = mac;
+
+	down(&priv->wx_sem);
+
+	memcpy(dev->dev_addr, addr->sa_data, ETH_ALEN);
+
+	if(priv->ieee80211->iw_mode == IW_MODE_MASTER)
+		memcpy(priv->ieee80211->current_network.bssid, dev->dev_addr, ETH_ALEN);
+
+	if (priv->up) {
+		rtl8180_down(dev);
+		rtl8180_up(dev);
+	}
+
+	up(&priv->wx_sem);
+
+	return 0;
+}
+
+/* based on ipw2200 driver */
+int rtl8180_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+	struct iwreq *wrq = (struct iwreq *) rq;
+	int ret=-1;
+	switch (cmd) {
+	    case RTL_IOCTL_WPA_SUPPLICANT:
+		ret = ieee80211_wpa_supplicant_ioctl(priv->ieee80211, &wrq->u.data);
+		return ret;
+
+	    default:
+		return -EOPNOTSUPP;
+	}
+
+	return -EOPNOTSUPP;
+}
+
+
+
+/****************************************************************************
+     -----------------------------PCI STUFF---------------------------
+*****************************************************************************/
+
+
+static int __devinit rtl8180_pci_probe(struct pci_dev *pdev,
+				       const struct pci_device_id *id)
+{
+	unsigned long ioaddr = 0;
+	struct net_device *dev = NULL;
+	struct r8180_priv *priv= NULL;
+	//u8 *ptr;
+	u8 unit = 0;
+
+#ifdef CONFIG_RTL8180_IO_MAP
+	unsigned long pio_start, pio_len, pio_flags;
+#else
+	unsigned long pmem_start, pmem_len, pmem_flags;
+#endif //end #ifdef RTL_IO_MAP
+
+	DMESG("Configuring chip resources");
+
+	if( pci_enable_device (pdev) ){
+		DMESG("Failed to enable PCI device");
+		return -EIO;
+	}
+
+	pci_set_master(pdev);
+	//pci_set_wmi(pdev);
+	pci_set_dma_mask(pdev, 0xffffff00ULL);
+	pci_set_consistent_dma_mask(pdev,0xffffff00ULL);
+	dev = alloc_ieee80211(sizeof(struct r8180_priv));
+	if (!dev)
+		return -ENOMEM;
+	priv = ieee80211_priv(dev);
+	priv->ieee80211 = netdev_priv(dev);
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,24)
+        SET_MODULE_OWNER(dev);
+#endif
+	pci_set_drvdata(pdev, dev);
+	SET_NETDEV_DEV(dev, &pdev->dev);
+
+	priv = ieee80211_priv(dev);
+//	memset(priv,0,sizeof(struct r8180_priv));
+	priv->pdev=pdev;
+
+
+#ifdef CONFIG_RTL8180_IO_MAP
+
+	pio_start = (unsigned long)pci_resource_start (pdev, 0);
+	pio_len = (unsigned long)pci_resource_len (pdev, 0);
+	pio_flags = (unsigned long)pci_resource_flags (pdev, 0);
+
+      	if (!(pio_flags & IORESOURCE_IO)) {
+		DMESG("region #0 not a PIO resource, aborting");
+		goto fail;
+	}
+
+	//DMESG("IO space @ 0x%08lx", pio_start );
+	if( ! request_region( pio_start, pio_len, RTL8180_MODULE_NAME ) ){
+		DMESG("request_region failed!");
+		goto fail;
+	}
+
+	ioaddr = pio_start;
+	dev->base_addr = ioaddr; // device I/O address
+
+#else
+
+	pmem_start = pci_resource_start(pdev, 1);
+	pmem_len = pci_resource_len(pdev, 1);
+	pmem_flags = pci_resource_flags (pdev, 1);
+
+	if (!(pmem_flags & IORESOURCE_MEM)) {
+		DMESG("region #1 not a MMIO resource, aborting");
+		goto fail;
+	}
+
+	//DMESG("Memory mapped space @ 0x%08lx ", pmem_start);
+	if( ! request_mem_region(pmem_start, pmem_len, RTL8180_MODULE_NAME)) {
+		DMESG("request_mem_region failed!");
+		goto fail;
+	}
+
+
+	ioaddr = (unsigned long)ioremap_nocache( pmem_start, pmem_len);
+	if( ioaddr == (unsigned long)NULL ){
+		DMESG("ioremap failed!");
+	       // release_mem_region( pmem_start, pmem_len );
+		goto fail1;
+	}
+
+	dev->mem_start = ioaddr; // shared mem start
+	dev->mem_end = ioaddr + pci_resource_len(pdev, 0); // shared mem end
+
+#endif //end #ifdef RTL_IO_MAP
+
+#ifdef CONFIG_RTL8185B
+	//pci_read_config_byte(pdev, 0x05, ptr);
+	//pci_write_config_byte(pdev, 0x05, (*ptr) & (~0x04));
+	pci_read_config_byte(pdev, 0x05, &unit);
+	pci_write_config_byte(pdev, 0x05, unit & (~0x04));
+#endif
+
+	dev->irq = pdev->irq;
+	priv->irq = 0;
+
+	dev->open = rtl8180_open;
+	dev->stop = rtl8180_close;
+	//dev->hard_start_xmit = ieee80211_xmit;
+	dev->tx_timeout = rtl8180_restart;
+	dev->wireless_handlers = &r8180_wx_handlers_def;
+	dev->do_ioctl = rtl8180_ioctl;
+	dev->set_multicast_list = r8180_set_multicast;
+	dev->set_mac_address = r8180_set_mac_adr;
+
+#if WIRELESS_EXT >= 12
+#if WIRELESS_EXT < 17
+	dev->get_wireless_stats = r8180_get_wireless_stats;
+#endif
+	dev->wireless_handlers = (struct iw_handler_def *) &r8180_wx_handlers_def;
+#endif
+
+	dev->type=ARPHRD_ETHER;
+	dev->watchdog_timeo = HZ*3; //added by david woo, 2007.12.13
+
+	if (dev_alloc_name(dev, ifname) < 0){
+                DMESG("Oops: devname already taken! Trying wlan%%d...\n");
+		ifname = "wlan%d";
+	//	ifname = "ath%d";
+		dev_alloc_name(dev, ifname);
+        }
+
+
+	if(rtl8180_init(dev)!=0){
+		DMESG("Initialization failed");
+		goto fail1;
+	}
+
+	netif_carrier_off(dev);
+
+	register_netdev(dev);
+
+	rtl8180_proc_init_one(dev);
+
+	DMESG("Driver probe completed\n");
+	return 0;
+
+fail1:
+
+#ifdef CONFIG_RTL8180_IO_MAP
+
+	if( dev->base_addr != 0 ){
+
+		release_region(dev->base_addr,
+	       pci_resource_len(pdev, 0) );
+	}
+#else
+	if( dev->mem_start != (unsigned long)NULL ){
+		iounmap( (void *)dev->mem_start );
+		release_mem_region( pci_resource_start(pdev, 1),
+				    pci_resource_len(pdev, 1) );
+	}
+#endif //end #ifdef RTL_IO_MAP
+
+
+fail:
+	if(dev){
+
+		if (priv->irq) {
+			free_irq(dev->irq, dev);
+			dev->irq=0;
+		}
+		free_ieee80211(dev);
+	}
+
+	pci_disable_device(pdev);
+
+	DMESG("wlan driver load failed\n");
+	pci_set_drvdata(pdev, NULL);
+	return -ENODEV;
+
+}
+
+
+static void __devexit rtl8180_pci_remove(struct pci_dev *pdev)
+{
+	struct r8180_priv *priv;
+	struct net_device *dev = pci_get_drvdata(pdev);
+ 	if(dev){
+
+		unregister_netdev(dev);
+
+		priv=ieee80211_priv(dev);
+
+		rtl8180_proc_remove_one(dev);
+		rtl8180_down(dev);
+		priv->rf_close(dev);
+		rtl8180_reset(dev);
+		//rtl8180_rtx_disable(dev);
+		//rtl8180_irq_disable(dev);
+		mdelay(10);
+		//write_nic_word(dev,INTA,read_nic_word(dev,INTA));
+		//force_pci_posting(dev);
+		//mdelay(10);
+
+		if(priv->irq){
+
+			DMESG("Freeing irq %d",dev->irq);
+			free_irq(dev->irq, dev);
+			priv->irq=0;
+
+		}
+
+		free_rx_desc_ring(dev);
+		free_tx_desc_rings(dev);
+	//	free_beacon_desc_ring(dev,priv->txbeaconcount);
+
+#ifdef CONFIG_RTL8180_IO_MAP
+
+		if( dev->base_addr != 0 ){
+
+			release_region(dev->base_addr,
+				       pci_resource_len(pdev, 0) );
+		}
+#else
+		if( dev->mem_start != (unsigned long)NULL ){
+			iounmap( (void *)dev->mem_start );
+			release_mem_region( pci_resource_start(pdev, 1),
+					    pci_resource_len(pdev, 1) );
+		}
+#endif /*end #ifdef RTL_IO_MAP*/
+
+		free_ieee80211(dev);
+	}
+	pci_disable_device(pdev);
+
+	DMESG("wlan driver removed\n");
+}
+
+
+/* fun with the built-in ieee80211 stack... */
+extern int ieee80211_crypto_init(void);
+extern void ieee80211_crypto_deinit(void);
+extern int ieee80211_crypto_tkip_init(void);
+extern void ieee80211_crypto_tkip_exit(void);
+extern int ieee80211_crypto_ccmp_init(void);
+extern void ieee80211_crypto_ccmp_exit(void);
+extern int ieee80211_crypto_wep_init(void);
+extern void ieee80211_crypto_wep_exit(void);
+
+static int __init rtl8180_pci_module_init(void)
+{
+	int ret;
+
+	ret = ieee80211_crypto_init();
+	if (ret) {
+		printk(KERN_ERR "ieee80211_crypto_init() failed %d\n", ret);
+		return ret;
+	}
+	ret = ieee80211_crypto_tkip_init();
+	if (ret) {
+		printk(KERN_ERR "ieee80211_crypto_tkip_init() failed %d\n", ret);
+		return ret;
+	}
+	ret = ieee80211_crypto_ccmp_init();
+	if (ret) {
+		printk(KERN_ERR "ieee80211_crypto_ccmp_init() failed %d\n", ret);
+		return ret;
+	}
+	ret = ieee80211_crypto_wep_init();
+	if (ret) {
+		printk(KERN_ERR "ieee80211_crypto_wep_init() failed %d\n", ret);
+		return ret;
+	}
+
+	printk(KERN_INFO "\nLinux kernel driver for RTL8180 \
+/ RTL8185 based WLAN cards\n");
+	printk(KERN_INFO "Copyright (c) 2004-2005, Andrea Merello\n");
+	DMESG("Initializing module");
+	DMESG("Wireless extensions version %d", WIRELESS_EXT);
+	rtl8180_proc_module_init();
+
+#if(LINUX_VERSION_CODE < KERNEL_VERSION(2,6,22))
+      if(0!=pci_module_init(&rtl8180_pci_driver))
+#else
+      if(0!=pci_register_driver(&rtl8180_pci_driver))
+#endif
+	//if(0!=pci_module_init(&rtl8180_pci_driver))
+	{
+		DMESG("No device found");
+		/*pci_unregister_driver (&rtl8180_pci_driver);*/
+		return -ENODEV;
+	}
+	return 0;
+}
+
+
+static void __exit rtl8180_pci_module_exit(void)
+{
+	pci_unregister_driver (&rtl8180_pci_driver);
+	rtl8180_proc_module_remove();
+	ieee80211_crypto_deinit();
+	ieee80211_crypto_tkip_exit();
+	ieee80211_crypto_ccmp_exit();
+	ieee80211_crypto_wep_exit();
+	DMESG("Exiting");
+}
+
+
+void rtl8180_try_wake_queue(struct net_device *dev, int pri)
+{
+	unsigned long flags;
+	short enough_desc;
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+	spin_lock_irqsave(&priv->tx_lock,flags);
+	enough_desc = check_nic_enought_desc(dev,pri);
+	spin_unlock_irqrestore(&priv->tx_lock,flags);
+
+	if(enough_desc)
+		ieee80211_wake_queue(priv->ieee80211);
+}
+
+/*****************************************************************************
+      -----------------------------IRQ STUFF---------------------------
+******************************************************************************/
+
+void rtl8180_tx_isr(struct net_device *dev, int pri,short error)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+	u32 *tail; //tail virtual addr
+	u32 *head; //head virtual addr
+	u32 *begin;//start of ring virtual addr
+	u32 *nicv; //nic pointer virtual addr
+//	u32 *txdv; //packet just TXed
+	u32 nic; //nic pointer physical addr
+	u32 nicbegin;// start of ring physical addr
+//	short txed;
+	unsigned long flag;
+	/* physical addr are ok on 32 bits since we set DMA mask*/
+
+	int offs;
+	int j,i;
+	int hd;
+	if (error) priv->stats.txretry++; //tony 20060601
+	spin_lock_irqsave(&priv->tx_lock,flag);
+	switch(pri) {
+	case MANAGE_PRIORITY:
+		tail = priv->txmapringtail;
+		begin = priv->txmapring;
+		head = priv->txmapringhead;
+		nic = read_nic_dword(dev,TX_MANAGEPRIORITY_RING_ADDR);
+		nicbegin = priv->txmapringdma;
+		break;
+
+	case BK_PRIORITY:
+		tail = priv->txbkpringtail;
+		begin = priv->txbkpring;
+		head = priv->txbkpringhead;
+		nic = read_nic_dword(dev,TX_BKPRIORITY_RING_ADDR);
+		nicbegin = priv->txbkpringdma;
+		break;
+
+	case BE_PRIORITY:
+		tail = priv->txbepringtail;
+		begin = priv->txbepring;
+		head = priv->txbepringhead;
+		nic = read_nic_dword(dev,TX_BEPRIORITY_RING_ADDR);
+		nicbegin = priv->txbepringdma;
+		break;
+
+	case VI_PRIORITY:
+		tail = priv->txvipringtail;
+		begin = priv->txvipring;
+		head = priv->txvipringhead;
+		nic = read_nic_dword(dev,TX_VIPRIORITY_RING_ADDR);
+		nicbegin = priv->txvipringdma;
+		break;
+
+	case VO_PRIORITY:
+		tail = priv->txvopringtail;
+		begin = priv->txvopring;
+		head = priv->txvopringhead;
+		nic = read_nic_dword(dev,TX_VOPRIORITY_RING_ADDR);
+		nicbegin = priv->txvopringdma;
+		break;
+
+	case HI_PRIORITY:
+		tail = priv->txhpringtail;
+		begin = priv->txhpring;
+		head = priv->txhpringhead;
+		nic = read_nic_dword(dev,TX_HIGHPRIORITY_RING_ADDR);
+		nicbegin = priv->txhpringdma;
+		break;
+
+	default:
+		spin_unlock_irqrestore(&priv->tx_lock,flag);
+		return ;
+	}
+/*	DMESG("%x %s %x %x",((int)nic & 0xfff)/8/4,
+	*(priv->txnpring + ((int)nic&0xfff)/4/8) & (1<<31) ? "filled" : "empty",
+	(priv->txnpringtail - priv->txnpring)/8,(priv->txnpringhead -
+priv->txnpring)/8);
+*/
+	//nicv = (u32*) ((nic - nicbegin) + (int)begin);
+	nicv = (u32*) ((nic - nicbegin) + (u8*)begin);
+	if((head <= tail && (nicv > tail || nicv < head)) ||
+		(head > tail && (nicv > tail && nicv < head))){
+
+			DMESGW("nic has lost pointer");
+#ifdef DEBUG_TX_DESC
+			//check_tx_ring(dev,NORM_PRIORITY);
+			check_tx_ring(dev,pri);
+#endif
+			spin_unlock_irqrestore(&priv->tx_lock,flag);
+			rtl8180_restart(dev);
+			return;
+		}
+
+	/* we check all the descriptors between the head and the nic,
+	 * but not the currenly pointed by the nic (the next to be txed)
+	 * and the previous of the pointed (might be in process ??)
+	*/
+	//if (head == nic) return;
+	//DMESG("%x %x",head,nic);
+	offs = (nic - nicbegin);
+	//DMESG("%x %x %x",nic ,(u32)nicbegin, (int)nic -nicbegin);
+
+	offs = offs / 8 /4;
+
+	hd = (head - begin) /8;
+
+	if(offs >= hd)
+		j = offs - hd;
+	else
+		j = offs + (priv->txringcount -1 -hd);
+	//	j= priv->txringcount -1- (hd - offs);
+
+	j-=2;
+	if(j<0) j=0;
+
+
+	for(i=0;i<j;i++)
+	{
+//		printk("+++++++++++++check status desc\n");
+		if((*head) & (1<<31))
+			break;
+		if(((*head)&(0x10000000)) != 0){
+//			printk("++++++++++++++last desc,retry count is %d\n",((*head) & (0x000000ff)));
+			priv->CurrRetryCnt += (u16)((*head) & (0x000000ff));
+#if 1
+			if(!error)
+			{
+				priv->NumTxOkTotal++;
+//				printk("NumTxOkTotal is %d\n",priv->NumTxOkTotal++);
+			}
+#endif
+			//	printk("in function %s:curr_retry_count is %d\n",__FUNCTION__,((*head) & (0x000000ff)));
+		}
+		if(!error){
+			priv->NumTxOkBytesTotal += (*(head+3)) & (0x00000fff);
+		}
+//		printk("in function %s:curr_txokbyte_count is %d\n",__FUNCTION__,(*(head+3)) & (0x00000fff));
+		*head = *head &~ (1<<31);
+
+		if((head - begin)/8 == priv->txringcount-1)
+			head=begin;
+
+		else
+			head+=8;
+	}
+#if 0
+	if(nicv == begin)
+		txdv = begin + (priv->txringcount -1)*8;
+	else
+		txdv = nicv - 8;
+
+	txed = !(txdv[0] &(1<<31));
+
+	if(txed){
+		if(!(txdv[0] & (1<<15))) error = 1;
+		//if(!(txdv[0] & (1<<30))) error = 1;
+		if(error)DMESG("%x",txdv[0]);
+ 	}
+#endif
+	//DMESG("%x",txdv[0]);
+	/* the head has been moved to the last certainly TXed
+	 * (or at least processed by the nic) packet.
+	 * The driver take forcefully owning of all these packets
+	 * If the packet previous of the nic pointer has been
+	 * processed this doesn't matter: it will be checked
+	 * here at the next round. Anyway if no more packet are
+	 * TXed no memory leak occour at all.
+	 */
+
+	switch(pri) {
+	case MANAGE_PRIORITY:
+		priv->txmapringhead = head;
+			//printk("1==========================================> priority check!\n");
+		if(priv->ack_tx_to_ieee){
+				// try to implement power-save mode 2008.1.22
+		//	printk("2==========================================> priority check!\n");
+#if 1
+			if(rtl8180_is_tx_queue_empty(dev)){
+			//	printk("tx queue empty, after send null sleep packet, try to sleep !\n");
+				priv->ack_tx_to_ieee = 0;
+				ieee80211_ps_tx_ack(priv->ieee80211,!error);
+			}
+#endif
+		}
+		break;
+
+	case BK_PRIORITY:
+		priv->txbkpringhead = head;
+		break;
+
+	case BE_PRIORITY:
+		priv->txbepringhead = head;
+		break;
+
+	case VI_PRIORITY:
+		priv->txvipringhead = head;
+		break;
+
+	case VO_PRIORITY:
+		priv->txvopringhead = head;
+		break;
+
+	case HI_PRIORITY:
+		priv->txhpringhead = head;
+		break;
+	}
+
+	/*DMESG("%x %x %x", (priv->txnpringhead - priv->txnpring) /8 ,
+		(priv->txnpringtail - priv->txnpring) /8,
+		offs );
+	*/
+
+	spin_unlock_irqrestore(&priv->tx_lock,flag);
+
+}
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+void rtl8180_tx_irq_wq(struct work_struct *work)
+{
+	//struct r8180_priv *priv = container_of(work, struct r8180_priv, reset_wq);
+        struct delayed_work *dwork = container_of(work,struct delayed_work,work);
+	struct ieee80211_device * ieee = (struct ieee80211_device*)
+	                                       container_of(dwork, struct ieee80211_device, watch_dog_wq);
+	struct net_device *dev = ieee->dev;
+#else
+void rtl8180_tx_irq_wq(struct net_device *dev)
+{
+	//struct r8180_priv *priv = ieee80211_priv(dev);
+#endif
+	rtl8180_tx_isr(dev,MANAGE_PRIORITY,0);
+}
+irqreturn_t rtl8180_interrupt(int irq, void *netdev, struct pt_regs *regs)
+{
+	struct net_device *dev = (struct net_device *) netdev;
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	unsigned long flags;
+	u32 inta;
+
+	/* We should return IRQ_NONE, but for now let me keep this */
+	if(priv->irq_enabled == 0) return IRQ_HANDLED;
+
+	spin_lock_irqsave(&priv->irq_th_lock,flags);
+
+#ifdef CONFIG_RTL8185B
+	//ISR: 4bytes
+	inta = read_nic_dword(dev, ISR);// & priv->IntrMask;
+	write_nic_dword(dev,ISR,inta); // reset int situation
+#else
+	inta = read_nic_word(dev,INTA) & priv->irq_mask;
+	write_nic_word(dev,INTA,inta); // reset int situation
+#endif
+
+	priv->stats.shints++;
+
+	//DMESG("Enter interrupt, ISR value = 0x%08x", inta);
+
+	if(!inta){
+		spin_unlock_irqrestore(&priv->irq_th_lock,flags);
+		return IRQ_HANDLED;
+	/*
+	   most probably we can safely return IRQ_NONE,
+	   but for now is better to avoid problems
+	*/
+	}
+
+	if(inta == 0xffff){
+			/* HW disappared */
+			spin_unlock_irqrestore(&priv->irq_th_lock,flags);
+			return IRQ_HANDLED;
+	}
+
+	priv->stats.ints++;
+#ifdef DEBUG_IRQ
+	DMESG("NIC irq %x",inta);
+#endif
+	//priv->irqpending = inta;
+
+
+	if(!netif_running(dev)) {
+		spin_unlock_irqrestore(&priv->irq_th_lock,flags);
+		return IRQ_HANDLED;
+	}
+
+	if(inta & ISR_TimeOut){
+		write_nic_dword(dev, TimerInt, 0);
+		//DMESG("=================>waking up");
+//		rtl8180_hw_wakeup(dev);
+	}
+
+	if(inta & ISR_TBDOK){
+		priv->stats.txbeacon++;
+	}
+
+	if(inta & ISR_TBDER){
+		priv->stats.txbeaconerr++;
+	}
+
+	if(inta  & IMR_TMGDOK ) {
+//		priv->NumTxOkTotal++;
+		rtl8180_tx_isr(dev,MANAGE_PRIORITY,0);
+//			schedule_work(&priv->tx_irq_wq);
+
+	}
+
+	if(inta & ISR_THPDER){
+#ifdef DEBUG_TX
+		DMESG ("TX high priority ERR");
+#endif
+		priv->stats.txhperr++;
+		rtl8180_tx_isr(dev,HI_PRIORITY,1);
+		priv->ieee80211->stats.tx_errors++;
+	}
+
+	if(inta & ISR_THPDOK){ //High priority tx ok
+#ifdef DEBUG_TX
+		DMESG ("TX high priority OK");
+#endif
+//		priv->NumTxOkTotal++;
+		//priv->NumTxOkInPeriod++;  //YJ,del,080828
+		priv->link_detect.NumTxOkInPeriod++; //YJ,add,080828
+		priv->stats.txhpokint++;
+		rtl8180_tx_isr(dev,HI_PRIORITY,0);
+	}
+
+	if(inta & ISR_RER) {
+		priv->stats.rxerr++;
+#ifdef DEBUG_RX
+		DMESGW("RX error int");
+#endif
+	}
+#ifdef CONFIG_RTL8185B
+	if(inta & ISR_TBKDER){ //corresponding to BK_PRIORITY
+		priv->stats.txbkperr++;
+		priv->ieee80211->stats.tx_errors++;
+#ifdef DEBUG_TX
+		DMESGW("TX bkp error int");
+#endif
+		//tasklet_schedule(&priv->irq_tx_tasklet);
+		rtl8180_tx_isr(dev,BK_PRIORITY,1);
+		rtl8180_try_wake_queue(dev, BE_PRIORITY);
+	}
+
+	if(inta & ISR_TBEDER){ //corresponding to BE_PRIORITY
+		priv->stats.txbeperr++;
+		priv->ieee80211->stats.tx_errors++;
+#ifdef DEBUG_TX
+		DMESGW("TX bep error int");
+#endif
+		rtl8180_tx_isr(dev,BE_PRIORITY,1);
+		//tasklet_schedule(&priv->irq_tx_tasklet);
+		rtl8180_try_wake_queue(dev, BE_PRIORITY);
+	}
+#endif
+	if(inta & ISR_TNPDER){ //corresponding to VO_PRIORITY
+		priv->stats.txnperr++;
+		priv->ieee80211->stats.tx_errors++;
+#ifdef DEBUG_TX
+		DMESGW("TX np error int");
+#endif
+		//tasklet_schedule(&priv->irq_tx_tasklet);
+		rtl8180_tx_isr(dev,NORM_PRIORITY,1);
+#ifdef CONFIG_RTL8185B
+		rtl8180_try_wake_queue(dev, NORM_PRIORITY);
+#endif
+	}
+
+	if(inta & ISR_TLPDER){ //corresponding to VI_PRIORITY
+		priv->stats.txlperr++;
+		priv->ieee80211->stats.tx_errors++;
+#ifdef DEBUG_TX
+		DMESGW("TX lp error int");
+#endif
+		rtl8180_tx_isr(dev,LOW_PRIORITY,1);
+		//tasklet_schedule(&priv->irq_tx_tasklet);
+		rtl8180_try_wake_queue(dev, LOW_PRIORITY);
+	}
+
+	if(inta & ISR_ROK){
+#ifdef DEBUG_RX
+		DMESG("Frame arrived !");
+#endif
+		//priv->NumRxOkInPeriod++;  //YJ,del,080828
+		priv->stats.rxint++;
+		tasklet_schedule(&priv->irq_rx_tasklet);
+	}
+
+	if(inta & ISR_RQoSOK ){
+#ifdef DEBUG_RX
+		DMESG("QoS Frame arrived !");
+#endif
+		//priv->NumRxOkInPeriod++;  //YJ,del,080828
+		priv->stats.rxint++;
+		tasklet_schedule(&priv->irq_rx_tasklet);
+	}
+	if(inta & ISR_BcnInt) {
+		//DMESG("Preparing Beacons");
+		rtl8180_prepare_beacon(dev);
+	}
+
+	if(inta & ISR_RDU){
+//#ifdef DEBUG_RX
+		DMESGW("No RX descriptor available");
+		priv->stats.rxrdu++;
+//#endif
+		tasklet_schedule(&priv->irq_rx_tasklet);
+		/*queue_work(priv->workqueue ,&priv->restart_work);*/
+
+	}
+	if(inta & ISR_RXFOVW){
+#ifdef DEBUG_RX
+		DMESGW("RX fifo overflow");
+#endif
+		priv->stats.rxoverflow++;
+		tasklet_schedule(&priv->irq_rx_tasklet);
+		//queue_work(priv->workqueue ,&priv->restart_work);
+	}
+
+	if(inta & ISR_TXFOVW) priv->stats.txoverflow++;
+
+	if(inta & ISR_TNPDOK){ //Normal priority tx ok
+#ifdef DEBUG_TX
+		DMESG ("TX normal priority OK");
+#endif
+//		priv->NumTxOkTotal++;
+		//priv->NumTxOkInPeriod++;  //YJ,del,080828
+		priv->link_detect.NumTxOkInPeriod++; //YJ,add,080828
+		//	priv->ieee80211->stats.tx_packets++;
+		priv->stats.txnpokint++;
+		rtl8180_tx_isr(dev,NORM_PRIORITY,0);
+	}
+
+	if(inta & ISR_TLPDOK){ //Low priority tx ok
+#ifdef DEBUG_TX
+		DMESG ("TX low priority OK");
+#endif
+//		priv->NumTxOkTotal++;
+		//priv->NumTxOkInPeriod++;  //YJ,del,080828
+		priv->link_detect.NumTxOkInPeriod++; //YJ,add,080828
+		//	priv->ieee80211->stats.tx_packets++;
+		priv->stats.txlpokint++;
+		rtl8180_tx_isr(dev,LOW_PRIORITY,0);
+		rtl8180_try_wake_queue(dev, LOW_PRIORITY);
+	}
+
+#ifdef CONFIG_RTL8185B
+	if(inta & ISR_TBKDOK){ //corresponding to BK_PRIORITY
+		priv->stats.txbkpokint++;
+#ifdef DEBUG_TX
+		DMESGW("TX bk priority ok");
+#endif
+//		priv->NumTxOkTotal++;
+		//priv->NumTxOkInPeriod++;  //YJ,del,080828
+		priv->link_detect.NumTxOkInPeriod++; //YJ,add,080828
+		rtl8180_tx_isr(dev,BK_PRIORITY,0);
+		rtl8180_try_wake_queue(dev, BE_PRIORITY);
+	}
+
+	if(inta & ISR_TBEDOK){ //corresponding to BE_PRIORITY
+		priv->stats.txbeperr++;
+#ifdef DEBUG_TX
+		DMESGW("TX be priority ok");
+#endif
+//		priv->NumTxOkTotal++;
+		//priv->NumTxOkInPeriod++;  //YJ,del,080828
+		priv->link_detect.NumTxOkInPeriod++; //YJ,add,080828
+		rtl8180_tx_isr(dev,BE_PRIORITY,0);
+		rtl8180_try_wake_queue(dev, BE_PRIORITY);
+	}
+#endif
+	force_pci_posting(dev);
+	spin_unlock_irqrestore(&priv->irq_th_lock,flags);
+
+	return IRQ_HANDLED;
+}
+
+
+void rtl8180_irq_rx_tasklet(struct r8180_priv* priv)
+{
+//	unsigned long flags;
+
+/*	spin_lock_irqsave(&priv->irq_lock, flags);
+	priv->irq_mask &=~IMR_ROK;
+	priv->irq_mask &=~IMR_RDU;
+
+	rtl8180_irq_enable(priv->dev);
+	spin_unlock_irqrestore(&priv->irq_lock, flags);
+*/
+	rtl8180_rx(priv->dev);
+
+/*	spin_lock_irqsave(&priv->irq_lock, flags);
+	priv->irq_mask |= IMR_ROK;
+	priv->irq_mask |= IMR_RDU;
+	rtl8180_irq_enable(priv->dev);
+	spin_unlock_irqrestore(&priv->irq_lock, flags);
+*/
+}
+
+/****************************************************************************
+lizhaoming--------------------------- RF power on/power off -----------------
+*****************************************************************************/
+
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))
+void GPIOChangeRFWorkItemCallBack(struct work_struct *work)
+{
+	//struct delayed_work *dwork = container_of(work, struct delayed_work, work);
+	struct ieee80211_device *ieee = container_of(work, struct ieee80211_device, GPIOChangeRFWorkItem.work);
+	struct net_device *dev = ieee->dev;
+	struct r8180_priv *priv = ieee80211_priv(dev);
+#else
+void GPIOChangeRFWorkItemCallBack(struct ieee80211_device *ieee)
+{
+	struct net_device *dev = ieee->dev;
+	struct r8180_priv *priv = ieee80211_priv(dev);
+#endif
+
+	//u16 tmp2byte;
+	u8 btPSR;
+	u8 btConfig0;
+	RT_RF_POWER_STATE	eRfPowerStateToSet;
+	bool 	bActuallySet=false;
+
+	char *argv[3];
+        static char *RadioPowerPath = "/etc/acpi/events/RadioPower.sh";
+        static char *envp[] = {"HOME=/", "TERM=linux", "PATH=/usr/bin:/bin", NULL};
+	static int readf_count = 0;
+	//printk("============>%s in \n", __func__);
+
+#ifdef ENABLE_LPS
+	if(readf_count % 10 == 0)
+		priv->PowerProfile = read_acadapter_file("/proc/acpi/ac_adapter/AC0/state");
+
+	readf_count = (readf_count+1)%0xffff;
+#endif
+#if 0
+	if(priv->up == 0)//driver stopped
+		{
+			printk("\nDo nothing...");
+			goto out;
+		}
+	else
+#endif
+		{
+			// We should turn off LED before polling FF51[4].
+
+			//Turn off LED.
+			btPSR = read_nic_byte(dev, PSR);
+			write_nic_byte(dev, PSR, (btPSR & ~BIT3));
+
+			//It need to delay 4us suggested by Jong, 2008-01-16
+			udelay(4);
+
+			//HW radio On/Off according to the value of FF51[4](config0)
+			btConfig0 = btPSR = read_nic_byte(dev, CONFIG0);
+
+			//Turn on LED.
+			write_nic_byte(dev, PSR, btPSR| BIT3);
+
+			eRfPowerStateToSet = (btConfig0 & BIT4) ?  eRfOn : eRfOff;
+
+			if((priv->ieee80211->bHwRadioOff == true) && (eRfPowerStateToSet == eRfOn))
+			{
+				priv->ieee80211->bHwRadioOff = false;
+				bActuallySet = true;
+			}
+			else if((priv->ieee80211->bHwRadioOff == false) && (eRfPowerStateToSet == eRfOff))
+			{
+				priv->ieee80211->bHwRadioOff = true;
+				bActuallySet = true;
+			}
+
+			if(bActuallySet)
+			{
+				MgntActSet_RF_State(dev, eRfPowerStateToSet, RF_CHANGE_BY_HW);
+
+				/* To update the UI status for Power status changed */
+                                if(priv->ieee80211->bHwRadioOff == true)
+                                        argv[1] = "RFOFF";
+                                else{
+                                        //if(!priv->RfOffReason)
+                                                argv[1] = "RFON";
+                                        //else
+                                        //      argv[1] = "RFOFF";
+                                }
+                                argv[0] = RadioPowerPath;
+                                argv[2] = NULL;
+
+                                call_usermodehelper(RadioPowerPath,argv,envp,1);
+			}
+
+		}
+
+}
+
+static u8 read_acadapter_file(char *filename)
+{
+//#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,21))
+#if 0
+	int fd;
+	char buf[1];
+	char ret[50];
+	int i = 0;
+	int n = 0;
+	mm_segment_t old_fs = get_fs();
+	set_fs(KERNEL_DS);
+
+	fd = sys_open(filename, O_RDONLY, 0);
+	if (fd >= 0) {
+		while (sys_read(fd, buf, 1) == 1)
+		{
+			i++;
+			if(i>10)
+			{
+				if(buf[0]!=' ')
+				{
+					ret[n]=buf[0];
+					n++;
+				}
+			}
+		}
+		sys_close(fd);
+	}
+	ret[n]='\0';
+//	printk("%s \n", ret);
+	set_fs(old_fs);
+
+	if(strncmp(ret, "off-line",8) == 0)
+	{
+		return 1;
+	}
+#endif
+	return 0;
+}
+
+/***************************************************************************
+     ------------------- module init / exit stubs ----------------
+****************************************************************************/
+module_init(rtl8180_pci_module_init);
+module_exit(rtl8180_pci_module_exit);
+
diff --git a/drivers/staging/rtl8187se/r8180_dm.c b/drivers/staging/rtl8187se/r8180_dm.c
new file mode 100644
index 00000000000..742dc11fcac
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_dm.c
@@ -0,0 +1,1725 @@
+//#include "r8180.h"

+#include "r8180_dm.h"

+#include "r8180_hw.h"

+#include "r8180_93cx6.h"

+//{by amy 080312

+

+//

+//	Description:

+//		Return TRUE if we shall perform High Power Mecahnism, FALSE otherwise.	

+//

+//+by amy 080312

+#define RATE_ADAPTIVE_TIMER_PERIOD      300

+

+bool CheckHighPower(struct net_device *dev)

+{

+	struct r8180_priv *priv = ieee80211_priv(dev);

+	struct ieee80211_device *ieee = priv->ieee80211;

+

+	if(!priv->bRegHighPowerMechanism)

+	{

+		return false;

+	}

+		

+	if(ieee->state == IEEE80211_LINKED_SCANNING)

+	{

+		return false;

+	}

+

+	return true;

+}

+

+//

+//	Description:

+//		Update Tx power level if necessary.

+//		See also DoRxHighPower() and SetTxPowerLevel8185() for reference.

+//

+//	Note:

+//		The reason why we udpate Tx power level here instead of DoRxHighPower() 

+//		is the number of IO to change Tx power is much more than chane TR switch 

+//		and they are related to OFDM and MAC registers. 

+//		So, we don't want to update it so frequently in per-Rx packet base. 

+//

+void

+DoTxHighPower(

+	struct net_device *dev

+	)

+{

+	struct r8180_priv *priv = ieee80211_priv(dev);

+	u16			HiPwrUpperTh = 0;

+	u16			HiPwrLowerTh = 0;

+	u8			RSSIHiPwrUpperTh;

+	u8			RSSIHiPwrLowerTh;

+	u8			u1bTmp;

+	char			OfdmTxPwrIdx, CckTxPwrIdx;

+

+	//printk("----> DoTxHighPower()\n");

+

+	HiPwrUpperTh = priv->RegHiPwrUpperTh;

+	HiPwrLowerTh = priv->RegHiPwrLowerTh;						

+	

+	HiPwrUpperTh = HiPwrUpperTh * 10;

+	HiPwrLowerTh = HiPwrLowerTh * 10;

+	RSSIHiPwrUpperTh = priv->RegRSSIHiPwrUpperTh;

+	RSSIHiPwrLowerTh = priv->RegRSSIHiPwrLowerTh;

+

+	//lzm add 080826

+	OfdmTxPwrIdx  = priv->chtxpwr_ofdm[priv->ieee80211->current_network.channel]; 

+	CckTxPwrIdx  = priv->chtxpwr[priv->ieee80211->current_network.channel]; 

+

+	//	printk("DoTxHighPower() - UndecoratedSmoothedSS:%d, CurCCKRSSI = %d , bCurCCKPkt= %d \n", priv->UndecoratedSmoothedSS, priv->CurCCKRSSI, priv->bCurCCKPkt );

+	

+	if((priv->UndecoratedSmoothedSS > HiPwrUpperTh) ||

+		(priv->bCurCCKPkt && (priv->CurCCKRSSI > RSSIHiPwrUpperTh)))

+	{

+		// Stevenl suggested that degrade 8dbm in high power sate. 2007-12-04 Isaiah 

+		

+	//	printk("=====>DoTxHighPower() - High Power - UndecoratedSmoothedSS:%d,  HiPwrUpperTh = %d \n", priv->UndecoratedSmoothedSS, HiPwrUpperTh );

+		priv->bToUpdateTxPwr = true;

+		u1bTmp= read_nic_byte(dev, CCK_TXAGC);

+

+		// If it never enter High Power.

+		if( CckTxPwrIdx == u1bTmp)

+		{

+		u1bTmp = (u1bTmp > 16) ? (u1bTmp -16): 0;  // 8dbm

+		write_nic_byte(dev, CCK_TXAGC, u1bTmp);

+

+		u1bTmp= read_nic_byte(dev, OFDM_TXAGC);

+		u1bTmp = (u1bTmp > 16) ? (u1bTmp -16): 0;  // 8dbm

+		write_nic_byte(dev, OFDM_TXAGC, u1bTmp);

+		}

+		

+	}

+	else if((priv->UndecoratedSmoothedSS < HiPwrLowerTh) &&

+		(!priv->bCurCCKPkt || priv->CurCCKRSSI < RSSIHiPwrLowerTh))

+	{

+	//	 printk("DoTxHighPower() - lower Power - UndecoratedSmoothedSS:%d,  HiPwrUpperTh = %d \n", priv->UndecoratedSmoothedSS, HiPwrLowerTh );

+		if(priv->bToUpdateTxPwr)

+		{

+			priv->bToUpdateTxPwr = false;

+			//SD3 required.

+			u1bTmp= read_nic_byte(dev, CCK_TXAGC);			

+			if(u1bTmp < CckTxPwrIdx)

+			{

+			//u1bTmp = ((u1bTmp+16) > 35) ? 35: (u1bTmp+16);  // 8dbm

+			//write_nic_byte(dev, CCK_TXAGC, u1bTmp);

+			write_nic_byte(dev, CCK_TXAGC, CckTxPwrIdx);

+			}

+

+			u1bTmp= read_nic_byte(dev, OFDM_TXAGC);

+			if(u1bTmp < OfdmTxPwrIdx)

+			{

+			//u1bTmp = ((u1bTmp+16) > 35) ? 35: (u1bTmp+16);  // 8dbm

+			//write_nic_byte(dev, OFDM_TXAGC, u1bTmp);

+			write_nic_byte(dev, OFDM_TXAGC, OfdmTxPwrIdx);

+			}

+		}

+	}

+

+	//printk("<---- DoTxHighPower()\n");

+}

+

+

+//

+//	Description:

+//		Callback function of UpdateTxPowerWorkItem.

+//		Because of some event happend, e.g. CCX TPC, High Power Mechanism, 

+//		We update Tx power of current channel again. 

+//

+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))

+void rtl8180_tx_pw_wq (struct work_struct *work)

+{

+//      struct r8180_priv *priv = container_of(work, struct r8180_priv, watch_dog_wq);

+//      struct ieee80211_device * ieee = (struct ieee80211_device*)

+//                                             container_of(work, struct ieee80211_device, watch_dog_wq);

+        struct delayed_work *dwork = container_of(work,struct delayed_work,work);

+        struct ieee80211_device *ieee = container_of(dwork,struct ieee80211_device,tx_pw_wq);

+        struct net_device *dev = ieee->dev;

+#else

+void rtl8180_tx_pw_wq(struct net_device *dev)

+{

+	// struct r8180_priv *priv = ieee80211_priv(dev);

+#endif

+

+//	printk("----> UpdateTxPowerWorkItemCallback()\n");

+	

+	DoTxHighPower(dev);	

+	

+//	printk("<---- UpdateTxPowerWorkItemCallback()\n");

+}

+

+

+//

+//	Description:

+//		Return TRUE if we shall perform DIG Mecahnism, FALSE otherwise.	

+//

+bool

+CheckDig(

+	struct net_device *dev

+	)

+{

+	struct r8180_priv *priv = ieee80211_priv(dev);

+	struct ieee80211_device *ieee = priv->ieee80211;

+

+	if(!priv->bDigMechanism)

+		return false;

+

+	if(ieee->state != IEEE80211_LINKED)

+		return false;

+

+	//if(priv->CurrentOperaRate < 36) // Schedule Dig under all OFDM rates. By Bruce, 2007-06-01.

+	if((priv->ieee80211->rate/5) < 36) // Schedule Dig under all OFDM rates. By Bruce, 2007-06-01.

+		return false;

+	return true;

+}

+//

+//	Description:

+//		Implementation of DIG for Zebra and Zebra2.	

+//

+void

+DIG_Zebra(

+	struct net_device *dev

+	)

+{

+	struct r8180_priv *priv = ieee80211_priv(dev);

+	u16			CCKFalseAlarm, OFDMFalseAlarm;

+	u16			OfdmFA1, OfdmFA2;

+	int			InitialGainStep = 7; // The number of initial gain stages.

+	int			LowestGainStage = 4; // The capable lowest stage of performing dig workitem.

+	u32 			AwakePeriodIn2Sec=0;

+

+	//printk("---------> DIG_Zebra()\n");

+

+	CCKFalseAlarm = (u16)(priv->FalseAlarmRegValue & 0x0000ffff);

+	OFDMFalseAlarm = (u16)((priv->FalseAlarmRegValue >> 16) & 0x0000ffff);

+	OfdmFA1 =  0x15;

+	OfdmFA2 = ((u16)(priv->RegDigOfdmFaUpTh)) << 8;

+

+//	printk("DIG**********CCK False Alarm: %#X \n",CCKFalseAlarm);

+//	printk("DIG**********OFDM False Alarm: %#X \n",OFDMFalseAlarm);

+

+        // The number of initial gain steps is different, by Bruce, 2007-04-13.

+	if (priv->InitialGain == 0 ) //autoDIG

+	{ // Advised from SD3 DZ

+		priv->InitialGain = 4; // In 87B, m74dBm means State 4 (m82dBm)

+	}

+	//if(pHalData->VersionID != VERSION_8187B_B)

+	{ // Advised from SD3 DZ

+		OfdmFA1 =  0x20;

+	}

+	

+#if 1 //lzm reserved 080826

+	AwakePeriodIn2Sec = (2000-priv ->DozePeriodInPast2Sec);

+	//printk("&&& DozePeriod=%d AwakePeriod=%d\n", priv->DozePeriodInPast2Sec, AwakePeriodIn2Sec);

+	priv ->DozePeriodInPast2Sec=0;

+	

+	if(AwakePeriodIn2Sec)

+	{	

+		//RT_TRACE(COMP_DIG, DBG_TRACE, ("DIG: AwakePeriodIn2Sec(%d) - FATh(0x%X , 0x%X) ->",AwakePeriodIn2Sec, OfdmFA1, OfdmFA2));

+		// adjuest DIG threshold.

+		OfdmFA1 =  (u16)((OfdmFA1*AwakePeriodIn2Sec)  / 2000) ;  	

+		OfdmFA2 =  (u16)((OfdmFA2*AwakePeriodIn2Sec)  / 2000) ;

+		//RT_TRACE(COMP_DIG, DBG_TRACE, ("( 0x%X , 0x%X)\n", OfdmFA1, OfdmFA2));

+	}

+	else

+	{

+		;//RT_TRACE(COMP_DIG, DBG_WARNING, ("ERROR!!  AwakePeriodIn2Sec should not be ZERO!!\n"));

+	}

+#endif

+

+	InitialGainStep = 8;

+	LowestGainStage = priv->RegBModeGainStage; // Lowest gain stage.

+

+	if (OFDMFalseAlarm > OfdmFA1)

+	{

+		if (OFDMFalseAlarm > OfdmFA2)

+		{

+			priv->DIG_NumberFallbackVote++;

+			if (priv->DIG_NumberFallbackVote >1)

+			{

+				//serious OFDM  False Alarm, need fallback

+				if (priv->InitialGain < InitialGainStep)

+				{

+					priv->InitialGainBackUp= priv->InitialGain;

+

+					priv->InitialGain = (priv->InitialGain + 1);

+//					printk("DIG**********OFDM False Alarm: %#X,  OfdmFA1: %#X, OfdmFA2: %#X\n", OFDMFalseAlarm, OfdmFA1, OfdmFA2);

+//					printk("DIG+++++++ fallback OFDM:%d \n", priv->InitialGain);

+					UpdateInitialGain(dev); 

+				}

+				priv->DIG_NumberFallbackVote = 0;

+				priv->DIG_NumberUpgradeVote=0;

+			}

+		}

+		else

+		{

+			if (priv->DIG_NumberFallbackVote)

+				priv->DIG_NumberFallbackVote--;

+		}

+		priv->DIG_NumberUpgradeVote=0;		

+	}

+	else	

+	{

+		if (priv->DIG_NumberFallbackVote)

+			priv->DIG_NumberFallbackVote--;

+		priv->DIG_NumberUpgradeVote++;

+

+		if (priv->DIG_NumberUpgradeVote>9)

+		{

+			if (priv->InitialGain > LowestGainStage) // In 87B, m78dBm means State 4 (m864dBm)

+			{

+				priv->InitialGainBackUp= priv->InitialGain;

+

+				priv->InitialGain = (priv->InitialGain - 1);

+//				printk("DIG**********OFDM False Alarm: %#X,  OfdmFA1: %#X, OfdmFA2: %#X\n", OFDMFalseAlarm, OfdmFA1, OfdmFA2);

+//				printk("DIG--------- Upgrade OFDM:%d \n", priv->InitialGain);

+				UpdateInitialGain(dev); 

+			}

+			priv->DIG_NumberFallbackVote = 0;

+			priv->DIG_NumberUpgradeVote=0;

+		}

+	}

+

+//	printk("DIG+++++++ OFDM:%d\n", priv->InitialGain);	

+	//printk("<--------- DIG_Zebra()\n");

+}

+

+//

+//	Description:

+//		Dispatch DIG implementation according to RF. 	

+//

+void

+DynamicInitGain(

+	struct net_device *dev

+	)

+{

+	struct r8180_priv *priv = ieee80211_priv(dev);

+

+	switch(priv->rf_chip)

+	{

+		case RF_ZEBRA2:  // [AnnieWorkaround] For Zebra2, 2005-08-01.

+		case RF_ZEBRA4:

+			DIG_Zebra( dev );

+			break;

+		

+		default:

+			printk("DynamicInitGain(): unknown RFChipID(%d) !!!\n", priv->rf_chip);

+			break;

+	}

+}

+

+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))

+void rtl8180_hw_dig_wq (struct work_struct *work)

+{

+//      struct r8180_priv *priv = container_of(work, struct r8180_priv, watch_dog_wq);

+//      struct ieee80211_device * ieee = (struct ieee80211_device*)

+//                                             container_of(work, struct ieee80211_device, watch_dog_wq);

+        struct delayed_work *dwork = container_of(work,struct delayed_work,work);

+        struct ieee80211_device *ieee = container_of(dwork,struct ieee80211_device,hw_dig_wq);

+        struct net_device *dev = ieee->dev;

+#else

+void rtl8180_hw_dig_wq(struct net_device *dev)

+{

+	

+#endif

+	struct r8180_priv *priv = ieee80211_priv(dev);

+

+	// Read CCK and OFDM False Alarm.

+	priv->FalseAlarmRegValue = read_nic_dword(dev, CCK_FALSE_ALARM);

+	

+

+	// Adjust Initial Gain dynamically.

+	DynamicInitGain(dev);

+	

+}

+

+int

+IncludedInSupportedRates(

+        struct r8180_priv       *priv,

+        u8              TxRate  )

+{

+    u8 rate_len;

+        u8 rate_ex_len;

+        u8                      RateMask = 0x7F;

+        u8                      idx;

+        unsigned short          Found = 0;

+        u8                      NaiveTxRate = TxRate&RateMask;

+

+    rate_len = priv->ieee80211->current_network.rates_len;

+        rate_ex_len = priv->ieee80211->current_network.rates_ex_len;

+        for( idx=0; idx< rate_len; idx++ )

+        {

+                if( (priv->ieee80211->current_network.rates[idx] & RateMask) == NaiveTxRate )

+                {

+                        Found = 1;

+                        goto found_rate;

+                }

+        }

+    for( idx=0; idx< rate_ex_len; idx++ )

+        {

+                if( (priv->ieee80211->current_network.rates_ex[idx] & RateMask) == NaiveTxRate )

+                {

+                        Found = 1;

+                        goto found_rate;

+                }

+        }

+        return Found;

+        found_rate:

+        return Found;

+}

+

+//

+//      Description:

+//              Get the Tx rate one degree up form the input rate in the supported rates.

+//              Return the upgrade rate if it is successed, otherwise return the input rate.

+//      By Bruce, 2007-06-05.

+// 

+u8

+GetUpgradeTxRate(

+        struct net_device *dev,

+        u8                              rate

+        )

+{

+        struct r8180_priv *priv = ieee80211_priv(dev);

+        u8                      UpRate;

+

+        // Upgrade 1 degree.

+        switch(rate)

+        {

+        case 108: // Up to 54Mbps.

+                UpRate = 108;

+                break;

+

+        case 96: // Up to 54Mbps.

+                UpRate = 108;

+                break;

+

+        case 72: // Up to 48Mbps.

+                UpRate = 96;

+                break;

+

+        case 48: // Up to 36Mbps.

+                UpRate = 72;

+                break;

+

+        case 36: // Up to 24Mbps.

+                UpRate = 48;

+                break;

+

+        case 22: // Up to 18Mbps.

+                UpRate = 36;

+                break;

+

+        case 11: // Up to 11Mbps.

+                UpRate = 22;

+                break;

+

+        case 4: // Up to 5.5Mbps.

+                UpRate = 11;

+                break;

+

+        case 2: // Up to 2Mbps.

+                UpRate = 4;

+                break;

+

+        default:

+                printk("GetUpgradeTxRate(): Input Tx Rate(%d) is undefined!\n", rate);

+                return rate;

+        }

+        // Check if the rate is valid.

+        if(IncludedInSupportedRates(priv, UpRate))

+        {

+//              printk("GetUpgradeTxRate(): GetUpgrade Tx rate(%d) from %d !\n", UpRate, priv->CurrentOperaRate);

+                return UpRate;

+        }

+        else

+        {

+                //printk("GetUpgradeTxRate(): Tx rate (%d) is not in supported rates\n", UpRate);

+                return rate;

+        }

+        return rate;

+}

+//

+//      Description:

+//              Get the Tx rate one degree down form the input rate in the supported rates.

+//              Return the degrade rate if it is successed, otherwise return the input rate.

+//      By Bruce, 2007-06-05.

+// 

+u8

+GetDegradeTxRate(

+        struct net_device *dev,

+        u8         rate

+        )

+{

+        struct r8180_priv *priv = ieee80211_priv(dev);

+        u8                      DownRate;

+

+        // Upgrade 1 degree.

+        switch(rate)

+        {

+        case 108: // Down to 48Mbps.

+                DownRate = 96;

+                break;

+

+        case 96: // Down to 36Mbps.

+                DownRate = 72;

+                break;

+

+        case 72: // Down to 24Mbps.

+                DownRate = 48;

+                break;

+

+        case 48: // Down to 18Mbps.

+                DownRate = 36;

+                break;

+

+        case 36: // Down to 11Mbps.

+                DownRate = 22;

+                break;

+

+        case 22: // Down to 5.5Mbps.

+                DownRate = 11;

+                break;

+

+        case 11: // Down to 2Mbps.

+                DownRate = 4;

+                break;

+

+        case 4: // Down to 1Mbps.

+                DownRate = 2;

+                break;

+

+        case 2: // Down to 1Mbps.

+                DownRate = 2;

+                break;

+

+        default:

+                printk("GetDegradeTxRate(): Input Tx Rate(%d) is undefined!\n", rate);

+                return rate;

+        }

+        // Check if the rate is valid.

+        if(IncludedInSupportedRates(priv, DownRate))

+        {

+//              printk("GetDegradeTxRate(): GetDegrade Tx rate(%d) from %d!\n", DownRate, priv->CurrentOperaRate);

+                return DownRate;

+        }

+        else

+        {

+                //printk("GetDegradeTxRate(): Tx rate (%d) is not in supported rates\n", DownRate);

+                return rate;

+        }

+        return rate;

+}

+//

+//      Helper function to determine if specified data rate is 

+//      CCK rate.

+//      2005.01.25, by rcnjko.

+//

+bool

+MgntIsCckRate(

+        u16     rate

+        )

+{

+        bool bReturn = false;

+

+        if((rate <= 22) && (rate != 12) && (rate != 18))

+        {

+                bReturn = true;

+        }

+

+        return bReturn;

+}

+#ifdef CONFIG_RTL818X_S

+//

+//	Description:

+//		Tx Power tracking mechanism routine on 87SE.

+// 	Created by Roger, 2007.12.11.

+//

+void

+TxPwrTracking87SE(

+	struct net_device *dev

+)

+{		

+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);

+	u8	tmpu1Byte, CurrentThermal, Idx;	

+	char	CckTxPwrIdx, OfdmTxPwrIdx;	

+	//u32	u4bRfReg;

+	

+	tmpu1Byte = read_nic_byte(dev, EN_LPF_CAL);

+	CurrentThermal = (tmpu1Byte & 0xf0)>>4; //[ 7:4]: thermal meter indication.

+	CurrentThermal = (CurrentThermal>0x0c)? 0x0c:CurrentThermal;//lzm add 080826

+

+	//printk("TxPwrTracking87SE(): CurrentThermal(%d)\n", CurrentThermal);

+	

+	if( CurrentThermal != priv->ThermalMeter)

+	{	

+//		printk("TxPwrTracking87SE(): Thermal meter changed!!!\n");

+

+		// Update Tx Power level on each channel.

+		for(Idx = 1; Idx<15; Idx++)

+		{			

+			CckTxPwrIdx = priv->chtxpwr[Idx];

+			OfdmTxPwrIdx = priv->chtxpwr_ofdm[Idx];			

+			

+			if( CurrentThermal > priv->ThermalMeter )

+			{ // higher thermal meter.		

+				CckTxPwrIdx += (CurrentThermal - priv->ThermalMeter)*2;

+				OfdmTxPwrIdx += (CurrentThermal - priv->ThermalMeter)*2;

+			

+				if(CckTxPwrIdx >35)

+					CckTxPwrIdx = 35; // Force TxPower to maximal index.

+				if(OfdmTxPwrIdx >35)

+					OfdmTxPwrIdx = 35;				

+			}

+			else

+			{ // lower thermal meter.				

+				CckTxPwrIdx -= (priv->ThermalMeter - CurrentThermal)*2;

+				OfdmTxPwrIdx -= (priv->ThermalMeter - CurrentThermal)*2;

+

+				if(CckTxPwrIdx <0)

+					CckTxPwrIdx = 0;		

+				if(OfdmTxPwrIdx <0)

+					OfdmTxPwrIdx = 0;				

+			}					

+			

+			// Update TxPower level on CCK and OFDM resp.

+			priv->chtxpwr[Idx] = CckTxPwrIdx;

+			priv->chtxpwr_ofdm[Idx] = OfdmTxPwrIdx;				

+		}	

+

+		// Update TxPower level immediately.

+		rtl8225z2_SetTXPowerLevel(dev, priv->ieee80211->current_network.channel);

+	}	

+	priv->ThermalMeter = CurrentThermal;					

+}

+void

+StaRateAdaptive87SE(

+	struct net_device *dev

+	)

+{

+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);

+	unsigned long 			CurrTxokCnt;

+	u16			CurrRetryCnt;

+	u16			CurrRetryRate;

+	//u16			i,idx;

+	unsigned long       	CurrRxokCnt;

+	bool			bTryUp = false;

+	bool			bTryDown = false;

+	u8			TryUpTh = 1;

+	u8			TryDownTh = 2;

+	u32			TxThroughput;

+	long		CurrSignalStrength;

+	bool		bUpdateInitialGain = false;

+    	u8			u1bOfdm=0, u1bCck = 0;

+	char		OfdmTxPwrIdx, CckTxPwrIdx;	

+

+	priv->RateAdaptivePeriod= RATE_ADAPTIVE_TIMER_PERIOD;

+

+

+	CurrRetryCnt	= priv->CurrRetryCnt;

+	CurrTxokCnt	= priv->NumTxOkTotal - priv->LastTxokCnt;

+	CurrRxokCnt	= priv->ieee80211->NumRxOkTotal - priv->LastRxokCnt;

+	CurrSignalStrength = priv->Stats_RecvSignalPower;

+	TxThroughput = (u32)(priv->NumTxOkBytesTotal - priv->LastTxOKBytes);

+	priv->LastTxOKBytes = priv->NumTxOkBytesTotal;

+	priv->CurrentOperaRate = priv->ieee80211->rate/5;

+	//printk("priv->CurrentOperaRate is %d\n",priv->CurrentOperaRate);

+	//2 Compute retry ratio.

+	if (CurrTxokCnt>0)

+	{

+		CurrRetryRate = (u16)(CurrRetryCnt*100/CurrTxokCnt);

+	}

+	else

+	{ // It may be serious retry. To distinguish serious retry or no packets modified by Bruce

+		CurrRetryRate = (u16)(CurrRetryCnt*100/1);

+	}

+

+

+	//

+	// Added by Roger, 2007.01.02.

+	// For debug information.

+	//

+	//printk("\n(1) pHalData->LastRetryRate: %d \n",priv->LastRetryRate);

+	//printk("(2) RetryCnt = %d  \n", CurrRetryCnt);	

+	//printk("(3) TxokCnt = %d \n", CurrTxokCnt);

+	//printk("(4) CurrRetryRate = %d \n", CurrRetryRate);	

+	//printk("(5) CurrSignalStrength = %d \n",CurrSignalStrength);

+	//printk("(6) TxThroughput is %d\n",TxThroughput);

+	//printk("priv->NumTxOkBytesTotal is %d\n",priv->NumTxOkBytesTotal);		

+

+	priv->LastRetryCnt = priv->CurrRetryCnt;

+	priv->LastTxokCnt = priv->NumTxOkTotal;

+	priv->LastRxokCnt = priv->ieee80211->NumRxOkTotal;

+	priv->CurrRetryCnt = 0;

+

+	//2No Tx packets, return to init_rate or not?

+	if (CurrRetryRate==0 && CurrTxokCnt == 0)

+	{	

+		//

+		//After 9 (30*300ms) seconds in this condition, we try to raise rate.

+		//

+		priv->TryupingCountNoData++;

+		

+//		printk("No Tx packets, TryupingCountNoData(%d)\n", priv->TryupingCountNoData);

+		//[TRC Dell Lab] Extend raised period from 4.5sec to 9sec, Isaiah 2008-02-15 18:00 

+		if (priv->TryupingCountNoData>30)

+		{

+			priv->TryupingCountNoData = 0;

+		 	priv->CurrentOperaRate = GetUpgradeTxRate(dev, priv->CurrentOperaRate);		

+			// Reset Fail Record

+			priv->LastFailTxRate = 0;

+			priv->LastFailTxRateSS = -200;

+			priv->FailTxRateCount = 0;

+		}

+		goto SetInitialGain;

+	}

+        else

+	{

+		priv->TryupingCountNoData=0; //Reset trying up times.

+	}

+

+

+	//

+	// For Netgear case, I comment out the following signal strength estimation,

+	// which can results in lower rate to transmit when sample is NOT enough (e.g. PING request).  

+	// 2007.04.09, by Roger.	

+	//	

+

+	//

+	// Restructure rate adaptive as the following main stages:

+	// (1) Add retry threshold in 54M upgrading condition with signal strength.

+	// (2) Add the mechanism to degrade to CCK rate according to signal strength 

+	//		and retry rate.

+	// (3) Remove all Initial Gain Updates over OFDM rate. To avoid the complicated 

+	//		situation, Initial Gain Update is upon on DIG mechanism except CCK rate.

+	// (4) Add the mehanism of trying to upgrade tx rate.

+	// (5) Record the information of upping tx rate to avoid trying upping tx rate constantly.

+	// By Bruce, 2007-06-05.

+	//	

+	//

+

+	// 11Mbps or 36Mbps

+	// Check more times in these rate(key rates).

+	//

+	if(priv->CurrentOperaRate == 22 || priv->CurrentOperaRate == 72)

+	{ 

+		TryUpTh += 9;

+	}

+	//

+	// Let these rates down more difficult.

+	//

+	if(MgntIsCckRate(priv->CurrentOperaRate) || priv->CurrentOperaRate == 36)

+	{

+			TryDownTh += 1;

+	}

+

+	//1 Adjust Rate.

+	if (priv->bTryuping == true)

+	{	

+		//2 For Test Upgrading mechanism

+		// Note:

+		// 	Sometimes the throughput is upon on the capability bwtween the AP and NIC,

+		// 	thus the low data rate does not improve the performance.

+		// 	We randomly upgrade the data rate and check if the retry rate is improved.

+		

+		// Upgrading rate did not improve the retry rate, fallback to the original rate.

+		if ( (CurrRetryRate > 25) && TxThroughput < priv->LastTxThroughput)

+		{

+			//Not necessary raising rate, fall back rate.

+			bTryDown = true;

+			//printk("case1-1: Not necessary raising rate, fall back rate....\n");

+			//printk("case1-1: pMgntInfo->CurrentOperaRate =%d, TxThroughput = %d, LastThroughput = %d\n",

+			//		priv->CurrentOperaRate, TxThroughput, priv->LastTxThroughput);			

+		}

+		else

+		{

+			priv->bTryuping = false;

+		}

+	}	

+	else if (CurrSignalStrength > -47 && (CurrRetryRate < 50))

+	{

+		//2For High Power

+		//

+		// Added by Roger, 2007.04.09.

+		// Return to highest data rate, if signal strength is good enough.

+		// SignalStrength threshold(-50dbm) is for RTL8186.

+		// Revise SignalStrength threshold to -51dbm.	

+		//

+		// Also need to check retry rate for safety, by Bruce, 2007-06-05.

+		if(priv->CurrentOperaRate != priv->ieee80211->current_network.HighestOperaRate )

+		{

+			bTryUp = true;

+			// Upgrade Tx Rate directly.

+			priv->TryupingCount += TryUpTh;

+		}

+//		printk("case2: StaRateAdaptive87SE: Power(%d) is high enough!!. \n", CurrSignalStrength);			

+					

+	}

+	else if(CurrTxokCnt > 9 && CurrTxokCnt< 100 && CurrRetryRate >= 600) 

+	{

+		//2 For Serious Retry

+		//

+		// Traffic is not busy but our Tx retry is serious. 

+		//

+		bTryDown = true;

+		// Let Rate Mechanism to degrade tx rate directly.

+		priv->TryDownCountLowData += TryDownTh;

+//		printk("case3: RA: Tx Retry is serious. Degrade Tx Rate to %d directly...\n", priv->CurrentOperaRate);	

+	}

+	else if ( priv->CurrentOperaRate == 108 )

+	{

+		//2For 54Mbps

+		// Air Link

+		if ( (CurrRetryRate>26)&&(priv->LastRetryRate>25))

+//		if ( (CurrRetryRate>40)&&(priv->LastRetryRate>39))

+		{

+			//Down to rate 48Mbps.

+			bTryDown = true;

+		}

+		// Cable Link

+		else if ( (CurrRetryRate>17)&&(priv->LastRetryRate>16) && (CurrSignalStrength > -72)) 

+//		else if ( (CurrRetryRate>17)&&(priv->LastRetryRate>16) && (CurrSignalStrength > -72))

+		{

+			//Down to rate 48Mbps.

+			bTryDown = true;

+		}

+

+		if(bTryDown && (CurrSignalStrength < -75)) //cable link

+		{

+			priv->TryDownCountLowData += TryDownTh;

+		}

+		//printk("case4---54M \n");	

+

+	}

+	else if ( priv->CurrentOperaRate == 96 )

+	{

+		//2For 48Mbps

+		//Air Link

+		if ( ((CurrRetryRate>48) && (priv->LastRetryRate>47)))

+//		if ( ((CurrRetryRate>65) && (priv->LastRetryRate>64)))

+

+		{	

+			//Down to rate 36Mbps.

+			bTryDown = true;

+		}

+		//Cable Link

+		else if ( ((CurrRetryRate>21) && (priv->LastRetryRate>20)) && (CurrSignalStrength > -74))

+		{

+			//Down to rate 36Mbps.

+			bTryDown = true;

+		}

+		else if((CurrRetryRate>  (priv->LastRetryRate + 50 )) && (priv->FailTxRateCount >2 ))

+//		else if((CurrRetryRate>  (priv->LastRetryRate + 70 )) && (priv->FailTxRateCount >2 ))

+		{

+			bTryDown = true;

+			priv->TryDownCountLowData += TryDownTh;

+		}

+		else if ( (CurrRetryRate<8) && (priv->LastRetryRate<8) ) //TO DO: need to consider (RSSI)

+//		else if ( (CurrRetryRate<28) && (priv->LastRetryRate<8) ) 

+		{

+			bTryUp = true;

+		}

+

+		if(bTryDown && (CurrSignalStrength < -75))

+		{

+			priv->TryDownCountLowData += TryDownTh;

+		}

+		//printk("case5---48M \n");	

+	}

+	else if ( priv->CurrentOperaRate == 72 )

+	{

+		//2For 36Mbps

+		if ( (CurrRetryRate>43) && (priv->LastRetryRate>41)) 

+//		if ( (CurrRetryRate>60) && (priv->LastRetryRate>59))

+		{	

+			//Down to rate 24Mbps.

+			bTryDown = true;

+		}

+		else if((CurrRetryRate>  (priv->LastRetryRate + 50 )) && (priv->FailTxRateCount >2 ))

+//		else if((CurrRetryRate>  (priv->LastRetryRate + 70 )) && (priv->FailTxRateCount >2 ))

+		{

+			bTryDown = true;

+			priv->TryDownCountLowData += TryDownTh;

+		}

+		else if ( (CurrRetryRate<15) &&  (priv->LastRetryRate<16)) //TO DO: need to consider (RSSI)

+//		else if ( (CurrRetryRate<35) &&  (priv->LastRetryRate<36))

+		{

+			bTryUp = true;

+		}

+

+		if(bTryDown && (CurrSignalStrength < -80))

+		{

+			priv->TryDownCountLowData += TryDownTh;

+		}

+		//printk("case6---36M \n");	

+	}

+	else if ( priv->CurrentOperaRate == 48 )

+	{

+		//2For 24Mbps

+		// Air Link

+		if ( ((CurrRetryRate>63) && (priv->LastRetryRate>62)))

+//		if ( ((CurrRetryRate>83) && (priv->LastRetryRate>82)))

+		{	

+			//Down to rate 18Mbps.

+			bTryDown = true;

+		}

+		//Cable Link

+		else if ( ((CurrRetryRate>33) && (priv->LastRetryRate>32)) && (CurrSignalStrength > -82) )

+//		 else if ( ((CurrRetryRate>50) && (priv->LastRetryRate>49)) && (CurrSignalStrength > -82) )

+		{

+			//Down to rate 18Mbps.

+			bTryDown = true;

+		}

+		else if((CurrRetryRate>  (priv->LastRetryRate + 50 )) && (priv->FailTxRateCount >2 ))

+//		else if((CurrRetryRate>  (priv->LastRetryRate + 70 )) && (priv->FailTxRateCount >2 ))

+

+		{

+			bTryDown = true;

+			priv->TryDownCountLowData += TryDownTh;

+		}

+  		else if ( (CurrRetryRate<20) && (priv->LastRetryRate<21)) //TO DO: need to consider (RSSI)

+//		else if ( (CurrRetryRate<40) && (priv->LastRetryRate<41))

+		{	

+			bTryUp = true;	

+		}

+

+		if(bTryDown && (CurrSignalStrength < -82))

+		{

+			priv->TryDownCountLowData += TryDownTh;

+		}

+		//printk("case7---24M \n");	

+	}

+	else if ( priv->CurrentOperaRate == 36 )

+	{

+		//2For 18Mbps

+		// original (109, 109) 

+		//[TRC Dell Lab] (90, 91), Isaiah 2008-02-18 23:24

+		//			     (85, 86), Isaiah 2008-02-18 24:00

+		if ( ((CurrRetryRate>85) && (priv->LastRetryRate>86)))

+//		if ( ((CurrRetryRate>115) && (priv->LastRetryRate>116)))

+		{

+			//Down to rate 11Mbps.

+			bTryDown = true;

+		}

+		//[TRC Dell Lab]  Isaiah 2008-02-18 23:24

+		else if((CurrRetryRate>  (priv->LastRetryRate + 50 )) && (priv->FailTxRateCount >2 ))

+//		else if((CurrRetryRate>  (priv->LastRetryRate + 70 )) && (priv->FailTxRateCount >2 ))

+		{

+			bTryDown = true;

+			priv->TryDownCountLowData += TryDownTh;

+		}

+		else if ( (CurrRetryRate<22) && (priv->LastRetryRate<23)) //TO DO: need to consider (RSSI)

+//		else if ( (CurrRetryRate<42) && (priv->LastRetryRate<43))

+		{	

+			bTryUp = true;	

+		}

+		//printk("case8---18M \n");	

+	}

+	else if ( priv->CurrentOperaRate == 22 )

+	{

+		//2For 11Mbps

+		if (CurrRetryRate>95)

+//		if (CurrRetryRate>155)

+		{

+			bTryDown = true;

+		}

+		else if ( (CurrRetryRate<29) && (priv->LastRetryRate <30) )//TO DO: need to consider (RSSI)

+//		else if ( (CurrRetryRate<49) && (priv->LastRetryRate <50) )

+			{

+			bTryUp = true;

+			}

+		//printk("case9---11M \n");	

+		}	

+	else if ( priv->CurrentOperaRate == 11 )

+	{

+		//2For 5.5Mbps

+		if (CurrRetryRate>149) 

+//		if (CurrRetryRate>189)

+		{	

+			bTryDown = true;			

+		}

+		else if ( (CurrRetryRate<60) && (priv->LastRetryRate < 65))

+//		else if ( (CurrRetryRate<80) && (priv->LastRetryRate < 85))

+

+			{

+			bTryUp = true;

+			}		

+		//printk("case10---5.5M \n");	

+		}

+	else if ( priv->CurrentOperaRate == 4 )

+	{

+		//2For 2 Mbps

+		if((CurrRetryRate>99) && (priv->LastRetryRate>99))

+//		if((CurrRetryRate>199) && (priv->LastRetryRate>199))

+		{

+			bTryDown = true;			

+		}

+		else if ( (CurrRetryRate < 65) && (priv->LastRetryRate < 70))

+//		else if ( (CurrRetryRate < 85) && (priv->LastRetryRate < 90))

+		{

+			bTryUp = true;

+		}

+		//printk("case11---2M \n");	

+	}

+	else if ( priv->CurrentOperaRate == 2 )

+	{

+		//2For 1 Mbps

+		if( (CurrRetryRate<70) && (priv->LastRetryRate<75))

+//		if( (CurrRetryRate<90) && (priv->LastRetryRate<95))

+		{

+			bTryUp = true;

+		}

+		//printk("case12---1M \n");	

+	}

+

+	if(bTryUp && bTryDown)

+    	printk("StaRateAdaptive87B(): Tx Rate tried upping and downing simultaneously!\n");

+				

+	//1 Test Upgrading Tx Rate

+	// Sometimes the cause of the low throughput (high retry rate) is the compatibility between the AP and NIC.

+	// To test if the upper rate may cause lower retry rate, this mechanism randomly occurs to test upgrading tx rate.

+	if(!bTryUp && !bTryDown && (priv->TryupingCount == 0) && (priv->TryDownCountLowData == 0)

+		&& priv->CurrentOperaRate != priv->ieee80211->current_network.HighestOperaRate && priv->FailTxRateCount < 2)

+	{

+		if(jiffies% (CurrRetryRate + 101) == 0)

+		{

+			bTryUp = true;	

+			priv->bTryuping = true;

+			//printk("StaRateAdaptive87SE(): Randomly try upgrading...\n");

+		}

+	}

+

+	//1 Rate Mechanism

+	if(bTryUp)

+	{

+		priv->TryupingCount++;

+		priv->TryDownCountLowData = 0;

+			

+		{

+//			printk("UP: pHalData->TryupingCount = %d\n", priv->TryupingCount);

+//			printk("UP: TryUpTh(%d)+ (FailTxRateCount(%d))^2 =%d\n", 

+//				TryUpTh, priv->FailTxRateCount, (TryUpTh + priv->FailTxRateCount * priv->FailTxRateCount) );

+//			printk("UP: pHalData->bTryuping=%d\n",  priv->bTryuping);

+

+		}

+		

+		//

+		// Check more times if we need to upgrade indeed.

+		// Because the largest value of pHalData->TryupingCount is 0xFFFF and 

+		// the largest value of pHalData->FailTxRateCount is 0x14,

+		// this condition will be satisfied at most every 2 min.

+		//

+

+		if((priv->TryupingCount > (TryUpTh + priv->FailTxRateCount * priv->FailTxRateCount)) ||

+			(CurrSignalStrength > priv->LastFailTxRateSS) || priv->bTryuping)

+		{

+			priv->TryupingCount = 0;

+			// 

+			// When transfering from CCK to OFDM, DIG is an important issue.

+			//

+			if(priv->CurrentOperaRate == 22)

+				bUpdateInitialGain = true;

+

+			// The difference in throughput between 48Mbps and 36Mbps is 8M.

+			// So, we must be carefully in this rate scale. Isaiah 2008-02-15.

+			//

+			if(  ((priv->CurrentOperaRate == 72) || (priv->CurrentOperaRate == 48) || (priv->CurrentOperaRate == 36)) &&

+				(priv->FailTxRateCount > 2) )

+				priv->RateAdaptivePeriod= (RATE_ADAPTIVE_TIMER_PERIOD/2);

+			

+			// (1)To avoid upgrade frequently to the fail tx rate, add the FailTxRateCount into the threshold.

+			// (2)If the signal strength is increased, it may be able to upgrade.

+

+			priv->CurrentOperaRate = GetUpgradeTxRate(dev, priv->CurrentOperaRate);

+//			printk("StaRateAdaptive87SE(): Upgrade Tx Rate to %d\n", priv->CurrentOperaRate);

+

+			//[TRC Dell Lab] Bypass 12/9/6, Isaiah 2008-02-18 20:00 

+			if(priv->CurrentOperaRate ==36)

+			{

+				priv->bUpdateARFR=true;

+				write_nic_word(dev, ARFR, 0x0F8F); //bypass 12/9/6

+//				printk("UP: ARFR=0xF8F\n");

+			}

+			else if(priv->bUpdateARFR)

+			{

+				priv->bUpdateARFR=false;

+				write_nic_word(dev, ARFR, 0x0FFF); //set 1M ~ 54Mbps.

+//				printk("UP: ARFR=0xFFF\n");

+			}

+				

+			// Update Fail Tx rate and count.

+			if(priv->LastFailTxRate != priv->CurrentOperaRate)

+			{

+				priv->LastFailTxRate = priv->CurrentOperaRate;

+				priv->FailTxRateCount = 0;

+				priv->LastFailTxRateSS = -200; // Set lowest power.

+			}

+		}

+	}

+	else

+	{	

+		if(priv->TryupingCount > 0)

+			priv->TryupingCount --;

+	}

+	

+	if(bTryDown)

+	{

+		priv->TryDownCountLowData++;

+		priv->TryupingCount = 0;

+		{

+//			printk("DN: pHalData->TryDownCountLowData = %d\n",priv->TryDownCountLowData);

+//			printk("DN: TryDownTh =%d\n", TryDownTh);

+//			printk("DN: pHalData->bTryuping=%d\n",  priv->bTryuping);

+		}

+						

+		//Check if Tx rate can be degraded or Test trying upgrading should fallback.

+		if(priv->TryDownCountLowData > TryDownTh || priv->bTryuping)

+		{

+			priv->TryDownCountLowData = 0;

+			priv->bTryuping = false;

+			// Update fail information.

+			if(priv->LastFailTxRate == priv->CurrentOperaRate)

+			{

+				priv->FailTxRateCount ++;

+				// Record the Tx fail rate signal strength.

+				if(CurrSignalStrength > priv->LastFailTxRateSS)

+				{

+					priv->LastFailTxRateSS = CurrSignalStrength;

+				}

+			}

+			else

+			{

+				priv->LastFailTxRate = priv->CurrentOperaRate;

+				priv->FailTxRateCount = 1;

+				priv->LastFailTxRateSS = CurrSignalStrength;

+			}

+			priv->CurrentOperaRate = GetDegradeTxRate(dev, priv->CurrentOperaRate);

+

+			// Reduce chariot training time at weak signal strength situation. SD3 ED demand. 

+			//[TRC Dell Lab] Revise Signal Threshold from -75 to -80 , Isaiah 2008-02-18 20:00 

+			if( (CurrSignalStrength < -80) && (priv->CurrentOperaRate > 72 ))

+			{

+				priv->CurrentOperaRate = 72;

+//				printk("DN: weak signal strength (%d), degrade to 36Mbps\n", CurrSignalStrength);

+			}

+

+			//[TRC Dell Lab] Bypass 12/9/6, Isaiah 2008-02-18 20:00 

+			if(priv->CurrentOperaRate ==36)

+			{

+				priv->bUpdateARFR=true;

+				write_nic_word(dev, ARFR, 0x0F8F); //bypass 12/9/6

+//				printk("DN: ARFR=0xF8F\n");

+			}

+			else if(priv->bUpdateARFR)

+			{

+				priv->bUpdateARFR=false;

+				write_nic_word(dev, ARFR, 0x0FFF); //set 1M ~ 54Mbps.

+//				printk("DN: ARFR=0xFFF\n");

+			}

+				

+			//

+			// When it is CCK rate, it may need to update initial gain to receive lower power packets.

+			//

+			if(MgntIsCckRate(priv->CurrentOperaRate))

+			{

+				bUpdateInitialGain = true;

+			}

+//			printk("StaRateAdaptive87SE(): Degrade Tx Rate to %d\n", priv->CurrentOperaRate);

+		}

+	}

+	else

+	{

+		if(priv->TryDownCountLowData > 0)

+			priv->TryDownCountLowData --;

+	}

+			

+	// Keep the Tx fail rate count to equal to 0x15 at most.

+	// Reduce the fail count at least to 10 sec if tx rate is tending stable.

+	if(priv->FailTxRateCount >= 0x15 || 

+		(!bTryUp && !bTryDown && priv->TryDownCountLowData == 0 && priv->TryupingCount && priv->FailTxRateCount > 0x6))

+	{

+		priv->FailTxRateCount --;

+	}		

+

+

+	OfdmTxPwrIdx  = priv->chtxpwr_ofdm[priv->ieee80211->current_network.channel]; 

+	CckTxPwrIdx  = priv->chtxpwr[priv->ieee80211->current_network.channel]; 

+	

+	//[TRC Dell Lab] Mac0x9e increase 2 level in 36M~18M situation, Isaiah 2008-02-18 24:00 

+	if((priv->CurrentOperaRate < 96) &&(priv->CurrentOperaRate > 22))

+	{

+		u1bCck = read_nic_byte(dev, CCK_TXAGC);

+		u1bOfdm = read_nic_byte(dev, OFDM_TXAGC);

+

+		// case 1: Never enter High power

+		if(u1bCck == CckTxPwrIdx )

+		{

+			if(u1bOfdm != (OfdmTxPwrIdx+2) )

+			{

+			priv->bEnhanceTxPwr= true;

+			u1bOfdm = ((u1bOfdm+2) > 35) ? 35: (u1bOfdm+2);

+			write_nic_byte(dev, OFDM_TXAGC, u1bOfdm);

+//			printk("Enhance OFDM_TXAGC : +++++ u1bOfdm= 0x%x\n", u1bOfdm);

+			}

+		}

+		// case 2: enter high power

+		else if(u1bCck < CckTxPwrIdx)

+		{

+			if(!priv->bEnhanceTxPwr)

+			{

+				priv->bEnhanceTxPwr= true;

+				u1bOfdm = ((u1bOfdm+2) > 35) ? 35: (u1bOfdm+2);

+				write_nic_byte(dev, OFDM_TXAGC, u1bOfdm);

+				//RT_TRACE(COMP_RATE, DBG_TRACE, ("Enhance OFDM_TXAGC(2) : +++++ u1bOfdm= 0x%x\n", u1bOfdm));

+			}

+		}

+	}

+	else if(priv->bEnhanceTxPwr)  //54/48/11/5.5/2/1

+	{

+		u1bCck = read_nic_byte(dev, CCK_TXAGC);

+		u1bOfdm = read_nic_byte(dev, OFDM_TXAGC);

+

+		// case 1: Never enter High power

+		if(u1bCck == CckTxPwrIdx )

+		{

+		priv->bEnhanceTxPwr= false;

+		write_nic_byte(dev, OFDM_TXAGC, OfdmTxPwrIdx);

+		//printk("Recover OFDM_TXAGC : ===== u1bOfdm= 0x%x\n", OfdmTxPwrIdx);

+		}

+		// case 2: enter high power

+		else if(u1bCck < CckTxPwrIdx)

+		{

+			priv->bEnhanceTxPwr= false;

+			u1bOfdm = ((u1bOfdm-2) > 0) ? (u1bOfdm-2): 0;

+			write_nic_byte(dev, OFDM_TXAGC, u1bOfdm);

+			//RT_TRACE(COMP_RATE, DBG_TRACE, ("Recover OFDM_TXAGC(2): ===== u1bOfdm= 0x%x\n", u1bOfdm));

+

+		}

+	}

+

+	//

+	// We need update initial gain when we set tx rate "from OFDM to CCK" or

+	// "from CCK to OFDM". 

+	//

+SetInitialGain:

+	if(bUpdateInitialGain)

+	{

+		if(MgntIsCckRate(priv->CurrentOperaRate)) // CCK

+		{

+			if(priv->InitialGain > priv->RegBModeGainStage)

+			{

+				priv->InitialGainBackUp= priv->InitialGain;

+

+				if(CurrSignalStrength < -85) // Low power, OFDM [0x17] = 26.

+				{

+					//SD3 SYs suggest that CurrSignalStrength < -65, ofdm 0x17=26.

+					priv->InitialGain = priv->RegBModeGainStage;

+				}

+				else if(priv->InitialGain > priv->RegBModeGainStage + 1)

+				{

+					priv->InitialGain -= 2;

+				}

+				else

+				{

+					priv->InitialGain --;

+				}

+				printk("StaRateAdaptive87SE(): update init_gain to index %d for date rate %d\n",priv->InitialGain, priv->CurrentOperaRate);			

+				UpdateInitialGain(dev);

+			}

+		}

+		else // OFDM

+		{			

+			if(priv->InitialGain < 4)

+			{

+				priv->InitialGainBackUp= priv->InitialGain;

+

+				priv->InitialGain ++;

+				printk("StaRateAdaptive87SE(): update init_gain to index %d for date rate %d\n",priv->InitialGain, priv->CurrentOperaRate);			

+				UpdateInitialGain(dev);

+			}					

+		}

+	}

+

+	//Record the related info

+	priv->LastRetryRate = CurrRetryRate;

+	priv->LastTxThroughput = TxThroughput;

+	priv->ieee80211->rate = priv->CurrentOperaRate * 5;

+}

+

+#endif

+#if LINUX_VERSION_CODE >=KERNEL_VERSION(2,6,20)

+void rtl8180_rate_adapter(struct work_struct * work)

+{

+	struct delayed_work *dwork = container_of(work,struct delayed_work,work);

+        struct ieee80211_device *ieee = container_of(dwork,struct ieee80211_device,rate_adapter_wq);

+        struct net_device *dev = ieee->dev;

+#else

+void rtl8180_rate_adapter(struct net_device *dev)

+{

+

+#endif

+        //struct r8180_priv *priv = ieee80211_priv(dev);

+//    DMESG("---->rtl8180_rate_adapter");

+        StaRateAdaptive87SE(dev);

+//   DMESG("<----rtl8180_rate_adapter");

+}

+void timer_rate_adaptive(unsigned long data)

+{

+	struct r8180_priv* priv = ieee80211_priv((struct net_device *)data);

+	//DMESG("---->timer_rate_adaptive()\n");

+	if(!priv->up)

+	{

+//		DMESG("<----timer_rate_adaptive():driver is not up!\n");

+		return;

+	}

+	if((priv->ieee80211->iw_mode != IW_MODE_MASTER)

+			&& (priv->ieee80211->state == IEEE80211_LINKED) &&

+			(priv->ForcedDataRate == 0) )

+	{

+//	DMESG("timer_rate_adaptive():schedule rate_adapter_wq\n");

+#ifdef CONFIG_RTL818X_S

+		queue_work(priv->ieee80211->wq, (void *)&priv->ieee80211->rate_adapter_wq);

+//		StaRateAdaptive87SE((struct net_device *)data);

+#endif

+	}

+	priv->rateadapter_timer.expires = jiffies + MSECS(priv->RateAdaptivePeriod);

+	add_timer(&priv->rateadapter_timer);

+	//DMESG("<----timer_rate_adaptive()\n");

+}

+//by amy 080312}

+void

+SwAntennaDiversityRxOk8185(

+	struct net_device *dev, 

+	u8 SignalStrength

+	)

+{

+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);

+

+//	printk("+SwAntennaDiversityRxOk8185: RxSs: %d\n", SignalStrength);

+

+	priv->AdRxOkCnt++;

+

+	if( priv->AdRxSignalStrength != -1)

+	{

+		priv->AdRxSignalStrength = ((priv->AdRxSignalStrength*7) + (SignalStrength*3)) / 10;

+	}

+	else

+	{ // Initialization case.

+		priv->AdRxSignalStrength = SignalStrength;

+	}

+//{+by amy 080312

+	if( priv->LastRxPktAntenna ) //Main antenna.	

+		priv->AdMainAntennaRxOkCnt++;	

+	else	 // Aux antenna.

+		priv->AdAuxAntennaRxOkCnt++;

+//+by amy 080312

+//	printk("-SwAntennaDiversityRxOk8185: AdRxOkCnt: %d AdRxSignalStrength: %d\n", priv->AdRxOkCnt, priv->AdRxSignalStrength);

+}

+//

+//	Description:

+//		Change Antenna Switch.

+//

+bool

+SetAntenna8185(

+	struct net_device *dev,

+	u8		u1bAntennaIndex

+	)

+{

+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);

+	bool bAntennaSwitched = false;

+

+//	printk("+SetAntenna8185(): Antenna is switching to: %d \n", u1bAntennaIndex);

+

+	switch(u1bAntennaIndex)

+	{

+	case 0:

+		switch(priv->rf_chip)

+		{

+		case RF_ZEBRA2:

+		case RF_ZEBRA4:

+#ifdef CONFIG_RTL8185B

+#ifdef CONFIG_RTL818X_S

+			// Mac register, main antenna

+			write_nic_byte(dev, ANTSEL, 0x03); 

+			//base band

+			write_phy_cck(dev,0x11, 0x9b); // Config CCK RX antenna.

+			write_phy_ofdm(dev, 0x0d, 0x5c); // Config OFDM RX antenna.

+

+#else

+			// Mac register, main antenna

+			write_nic_byte(dev, ANTSEL, 0x03); 

+			//base band

+			write_phy_cck(dev, 0x10, 0x9b); // Config CCK RX antenna.

+			write_phy_ofdm(dev, 0x0d, 0x5c); // Config OFDM RX antenna.

+#endif

+#endif

+

+			bAntennaSwitched = true;

+			break;

+

+		default:

+			printk("SetAntenna8185: unkown RFChipID(%d)\n", priv->rf_chip);

+			break;

+		}

+		break;

+

+	case 1:

+		switch(priv->rf_chip)

+		{

+		case RF_ZEBRA2:

+		case RF_ZEBRA4:

+#ifdef CONFIG_RTL8185B

+#ifdef CONFIG_RTL818X_S

+			// Mac register, aux antenna

+			write_nic_byte(dev, ANTSEL, 0x00); 

+			//base band

+			write_phy_cck(dev, 0x11, 0xbb); // Config CCK RX antenna.

+			write_phy_ofdm(dev, 0x0d, 0x54); // Config OFDM RX antenna.

+#else

+			// Mac register, aux antenna

+			write_nic_byte(dev, ANTSEL, 0x00); 

+			//base band

+			write_phy_cck(dev, 0x10, 0xbb); // Config CCK RX antenna.

+			write_phy_ofdm(dev, 0x0d, 0x54); // Config OFDM RX antenna.

+#endif

+#endif

+

+			bAntennaSwitched = true;

+			break;

+

+		default:

+			printk("SetAntenna8185: unkown RFChipID(%d)\n", priv->rf_chip);

+			break;

+		}

+		break;

+

+	default:

+		printk("SetAntenna8185: unkown u1bAntennaIndex(%d)\n", u1bAntennaIndex);

+		break;

+	}

+

+	if(bAntennaSwitched)

+	{

+		priv->CurrAntennaIndex = u1bAntennaIndex;

+	}

+

+//	printk("-SetAntenna8185(): return (%#X)\n", bAntennaSwitched);

+

+	return bAntennaSwitched;

+}

+//

+//	Description:

+//		Toggle Antenna switch.

+//

+bool

+SwitchAntenna(

+	struct net_device *dev

+	)

+{

+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);

+

+	bool		bResult;

+

+	if(priv->CurrAntennaIndex == 0)

+	{

+#if 0//lzm del 080826

+//by amy 080312

+#ifdef CONFIG_RTL818X_S

+		if(priv->bSwAntennaDiverity)

+			bResult = SetAntennaConfig87SE(dev, 1, true);

+		else			

+#endif

+#endif

+			bResult = SetAntenna8185(dev, 1);

+//by amy 080312

+//		printk("SwitchAntenna(): switching to antenna 1 ......\n");

+//		bResult = SetAntenna8185(dev, 1);//-by amy 080312

+	}

+	else

+	{

+#if 0//lzm del 080826

+//by amy 080312

+#ifdef CONFIG_RTL818X_S

+		if(priv->bSwAntennaDiverity)

+			bResult = SetAntennaConfig87SE(dev, 0, true);

+		else	

+#endif

+#endif

+			bResult = SetAntenna8185(dev, 0);

+//by amy 080312

+//		printk("SwitchAntenna(): switching to antenna 0 ......\n");

+//		bResult = SetAntenna8185(dev, 0);//-by amy 080312

+	}

+

+	return bResult;

+}

+//

+//	Description:

+//		Engine of SW Antenna Diversity mechanism.

+//		Since 8187 has no Tx part information, 

+//		this implementation is only dependend on Rx part information. 

+//

+//	2006.04.17, by rcnjko.

+//

+void

+SwAntennaDiversity(

+	struct net_device *dev

+	)

+{

+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);

+	bool   bSwCheckSS=false;

+//	printk("+SwAntennaDiversity(): CurrAntennaIndex: %d\n", priv->CurrAntennaIndex);

+//	printk("AdTickCount is %d\n",priv->AdTickCount);

+//by amy 080312

+	if(bSwCheckSS)

+	{

+		priv->AdTickCount++;

+	

+		printk("(1) AdTickCount: %d, AdCheckPeriod: %d\n", 

+			priv->AdTickCount, priv->AdCheckPeriod);

+		printk("(2) AdRxSignalStrength: %ld, AdRxSsThreshold: %ld\n", 

+			priv->AdRxSignalStrength, priv->AdRxSsThreshold);

+	}

+//	priv->AdTickCount++;//-by amy 080312

+	

+	// Case 1. No Link.

+	if(priv->ieee80211->state != IEEE80211_LINKED)

+	{

+	//	printk("SwAntennaDiversity(): Case 1. No Link.\n");

+

+		priv->bAdSwitchedChecking = false;

+		// I switch antenna here to prevent any one of antenna is broken before link established, 2006.04.18, by rcnjko..

+		SwitchAntenna(dev);

+	}

+	// Case 2. Linked but no packet received.

+	else if(priv->AdRxOkCnt == 0)

+	{

+	//	printk("SwAntennaDiversity(): Case 2. Linked but no packet received.\n");

+

+		priv->bAdSwitchedChecking = false;

+		SwitchAntenna(dev);

+	}

+	// Case 3. Evaluate last antenna switch action and undo it if necessary.

+	else if(priv->bAdSwitchedChecking == true)

+	{

+	//	printk("SwAntennaDiversity(): Case 3. Evaluate last antenna switch action.\n");

+

+		priv->bAdSwitchedChecking = false;

+

+		// Adjust Rx signal strength threashold.

+		priv->AdRxSsThreshold = (priv->AdRxSignalStrength + priv->AdRxSsBeforeSwitched) / 2;

+

+		priv->AdRxSsThreshold = (priv->AdRxSsThreshold > priv->AdMaxRxSsThreshold) ?	

+					priv->AdMaxRxSsThreshold: priv->AdRxSsThreshold;

+		if(priv->AdRxSignalStrength < priv->AdRxSsBeforeSwitched)

+		{ // Rx signal strength is not improved after we swtiched antenna. => Swich back.

+//			printk("SwAntennaDiversity(): Rx Signal Strength is not improved, CurrRxSs: %d, LastRxSs: %d\n", 

+//				priv->AdRxSignalStrength, priv->AdRxSsBeforeSwitched);

+//by amy 080312

+			// Increase Antenna Diversity checking period due to bad decision.

+			priv->AdCheckPeriod *= 2;

+//by amy 080312

+			// Increase Antenna Diversity checking period.

+			if(priv->AdCheckPeriod > priv->AdMaxCheckPeriod)

+				priv->AdCheckPeriod = priv->AdMaxCheckPeriod;

+	

+			// Wrong deceision => switch back.

+			SwitchAntenna(dev);

+		}

+		else

+		{ // Rx Signal Strength is improved. 

+//			printk("SwAntennaDiversity(): Rx Signal Strength is improved, CurrRxSs: %d, LastRxSs: %d\n", 

+//				priv->AdRxSignalStrength, priv->AdRxSsBeforeSwitched);

+

+			// Reset Antenna Diversity checking period to its min value.

+			priv->AdCheckPeriod = priv->AdMinCheckPeriod;

+		}

+

+//		printk("SwAntennaDiversity(): AdRxSsThreshold: %d, AdCheckPeriod: %d\n",

+//			priv->AdRxSsThreshold, priv->AdCheckPeriod);

+	}

+	// Case 4. Evaluate if we shall switch antenna now.

+	// Cause Table Speed is very fast in TRC Dell Lab, we check it every time. 

+	else// if(priv->AdTickCount >= priv->AdCheckPeriod)//-by amy 080312

+	{

+//		printk("SwAntennaDiversity(): Case 4. Evaluate if we shall switch antenna now.\n");

+

+		priv->AdTickCount = 0;

+

+		//

+		// <Roger_Notes> We evaluate RxOk counts for each antenna first and than 

+		// evaluate signal strength. 

+		// The following operation can overcome the disability of CCA on both two antennas

+		// When signal strength was extremely low or high.

+		// 2008.01.30.

+		// 

+		

+		//

+		// Evaluate RxOk count from each antenna if we shall switch default antenna now.

+		// Added by Roger, 2008.02.21.

+//{by amy 080312

+		if((priv->AdMainAntennaRxOkCnt < priv->AdAuxAntennaRxOkCnt) 

+			&& (priv->CurrAntennaIndex == 0))

+		{ // We set Main antenna as default but RxOk count was less than Aux ones.

+

+	//		printk("SwAntennaDiversity(): Main antenna RxOK is poor, AdMainAntennaRxOkCnt: %d, AdAuxAntennaRxOkCnt: %d\n", 

+	//			priv->AdMainAntennaRxOkCnt, priv->AdAuxAntennaRxOkCnt);

+			

+			// Switch to Aux antenna.

+			SwitchAntenna(dev);	

+			priv->bHWAdSwitched = true;

+		}

+		else if((priv->AdAuxAntennaRxOkCnt < priv->AdMainAntennaRxOkCnt) 

+			&& (priv->CurrAntennaIndex == 1))

+		{ // We set Aux antenna as default but RxOk count was less than Main ones.

+

+	//		printk("SwAntennaDiversity(): Aux antenna RxOK is poor, AdMainAntennaRxOkCnt: %d, AdAuxAntennaRxOkCnt: %d\n", 

+	//			priv->AdMainAntennaRxOkCnt, priv->AdAuxAntennaRxOkCnt);

+			

+			// Switch to Main antenna.

+			SwitchAntenna(dev);

+			priv->bHWAdSwitched = true;

+		}

+		else

+		{// Default antenna is better.

+

+	//		printk("SwAntennaDiversity(): Default antenna is better., AdMainAntennaRxOkCnt: %d, AdAuxAntennaRxOkCnt: %d\n", 

+	//			priv->AdMainAntennaRxOkCnt, priv->AdAuxAntennaRxOkCnt);

+

+			// Still need to check current signal strength.

+			priv->bHWAdSwitched = false;	

+		}

+		//

+		// <Roger_Notes> We evaluate Rx signal strength ONLY when default antenna 

+		// didn't changed by HW evaluation. 

+		// 2008.02.27.

+		//

+		// [TRC Dell Lab] SignalStrength is inaccuracy. Isaiah 2008-03-05 

+		// For example, Throughput of aux is better than main antenna(about 10M v.s 2M), 

+		// but AdRxSignalStrength is less than main. 

+		// Our guess is that main antenna have lower throughput and get many change 

+		// to receive more CCK packets(ex.Beacon) which have stronger SignalStrength.

+		//

+		if( (!priv->bHWAdSwitched) && (bSwCheckSS))

+		{

+//by amy 080312}

+		// Evaluate Rx signal strength if we shall switch antenna now.

+		if(priv->AdRxSignalStrength < priv->AdRxSsThreshold)

+		{ // Rx signal strength is weak => Switch Antenna.

+//			printk("SwAntennaDiversity(): Rx Signal Strength is weak, CurrRxSs: %d, RxSsThreshold: %d\n", 

+//				priv->AdRxSignalStrength, priv->AdRxSsThreshold);	

+

+			priv->AdRxSsBeforeSwitched = priv->AdRxSignalStrength; 

+			priv->bAdSwitchedChecking = true;

+

+			SwitchAntenna(dev);

+		}

+		else

+		{ // Rx signal strength is OK. 

+//			printk("SwAntennaDiversity(): Rx Signal Strength is OK, CurrRxSs: %d, RxSsThreshold: %d\n", 

+//				priv->AdRxSignalStrength, priv->AdRxSsThreshold);

+

+			priv->bAdSwitchedChecking = false;

+			// Increase Rx signal strength threashold if necessary.

+			if(	(priv->AdRxSignalStrength > (priv->AdRxSsThreshold + 10)) && // Signal is much stronger than current threshold

+				priv->AdRxSsThreshold <= priv->AdMaxRxSsThreshold) // Current threhold is not yet reach upper limit.

+			{

+				priv->AdRxSsThreshold = (priv->AdRxSsThreshold + priv->AdRxSignalStrength) / 2;

+				priv->AdRxSsThreshold = (priv->AdRxSsThreshold > priv->AdMaxRxSsThreshold) ?

+												priv->AdMaxRxSsThreshold: priv->AdRxSsThreshold;//+by amy 080312

+			}

+

+			// Reduce Antenna Diversity checking period if possible. 

+			if( priv->AdCheckPeriod > priv->AdMinCheckPeriod )

+			{

+				priv->AdCheckPeriod /= 2; 

+			}

+		}

+		}

+	}

+//by amy 080312

+	// Reset antenna diversity Rx related statistics.

+	priv->AdRxOkCnt = 0;

+	priv->AdMainAntennaRxOkCnt = 0;

+	priv->AdAuxAntennaRxOkCnt = 0;

+//by amy 080312

+

+//	priv->AdRxOkCnt = 0;//-by amy 080312

+

+//	printk("-SwAntennaDiversity()\n");

+}

+

+//

+//	Description:

+//		Return TRUE if we shall perform Tx Power Tracking Mecahnism, FALSE otherwise.	

+//

+bool

+CheckTxPwrTracking(	struct net_device *dev)

+{

+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);

+

+	if(!priv->bTxPowerTrack)

+	{

+		return false;

+	}

+

+//lzm reserved 080826

+	//if(priv->bScanInProgress)

+	//{

+	//	return false;

+	//}

+

+	//if 87SE is in High Power , don't do Tx Power Tracking. asked by SD3 ED. 2008-08-08 Isaiah 

+	if(priv->bToUpdateTxPwr)

+	{

+		return false;

+	}

+		

+	return true;

+}

+

+

+//

+//	Description:

+//		Timer callback function of SW Antenna Diversity.

+//

+void

+SwAntennaDiversityTimerCallback(

+	struct net_device *dev

+	)

+{

+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);

+	RT_RF_POWER_STATE rtState;

+	

+	//printk("+SwAntennaDiversityTimerCallback()\n");

+

+	//

+	// We do NOT need to switch antenna while RF is off.

+	// 2007.05.09, added by Roger.

+	//

+	rtState = priv->eRFPowerState;

+	do{

+		if (rtState == eRfOff)

+		{	

+//			printk("SwAntennaDiversityTimer - RF is OFF.\n");

+			break;

+		}  	

+		else if (rtState == eRfSleep)

+		{	

+			// Don't access BB/RF under Disable PLL situation.

+			//RT_TRACE((COMP_RF|COMP_ANTENNA), DBG_LOUD, ("SwAntennaDiversityTimerCallback(): RF is Sleep => skip it\n"));

+			break;

+		}  

+		SwAntennaDiversity(dev);

+

+	}while(false);

+

+	if(priv->up)

+	{

+		priv->SwAntennaDiversityTimer.expires = jiffies + MSECS(ANTENNA_DIVERSITY_TIMER_PERIOD);

+		add_timer(&priv->SwAntennaDiversityTimer);

+	}

+

+	//printk("-SwAntennaDiversityTimerCallback()\n");

+}

+

diff --git a/drivers/staging/rtl8187se/r8180_dm.h b/drivers/staging/rtl8187se/r8180_dm.h
new file mode 100644
index 00000000000..3de92f040f9
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_dm.h
@@ -0,0 +1,41 @@
+#ifndef R8180_DM_H 

+#define R8180_DM_H

+

+#include "r8180.h"

+//#include "r8180_hw.h"

+//#include "r8180_93cx6.h"

+void SwAntennaDiversityRxOk8185(struct net_device *dev, u8 SignalStrength);

+bool SetAntenna8185(struct net_device *dev,	u8 u1bAntennaIndex);

+bool SwitchAntenna(	struct net_device *dev);

+void SwAntennaDiversity(struct net_device *dev	);

+void SwAntennaDiversityTimerCallback(struct net_device *dev);

+bool CheckDig(struct net_device *dev);

+bool CheckHighPower(struct net_device *dev);

+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))

+void rtl8180_hw_dig_wq (struct work_struct *work);

+#else

+void rtl8180_hw_dig_wq(struct net_device *dev);

+#endif

+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,20))

+void rtl8180_tx_pw_wq (struct work_struct *work);

+#else

+void rtl8180_tx_pw_wq(struct net_device *dev);

+#endif

+#if LINUX_VERSION_CODE >=KERNEL_VERSION(2,6,20)

+void rtl8180_rate_adapter(struct work_struct * work);

+

+#else

+void rtl8180_rate_adapter(struct net_device *dev);

+

+#endif

+void TxPwrTracking87SE(struct net_device *dev);

+bool CheckTxPwrTracking(struct net_device *dev);

+#if LINUX_VERSION_CODE >=KERNEL_VERSION(2,6,20)

+void rtl8180_rate_adapter(struct work_struct * work);

+#else

+void rtl8180_rate_adapter(struct net_device *dev);

+#endif

+void timer_rate_adaptive(unsigned long data);

+

+

+#endif

diff --git a/drivers/staging/rtl8187se/r8180_gct.c b/drivers/staging/rtl8187se/r8180_gct.c
new file mode 100644
index 00000000000..86cb427a7a4
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_gct.c
@@ -0,0 +1,296 @@
+/*
+   This files contains GCT radio frontend programming routines.
+
+   This is part of rtl8180 OpenSource driver
+   Copyright (C) Andrea Merello 2004-2005  <andreamrl@tiscali.it>
+   Released under the terms of GPL (General Public Licence)
+
+   Parts of this driver are based on the GPL part of the
+   official realtek driver
+
+   Parts of this driver are based on the rtl8180 driver skeleton
+   from Patric Schenke & Andres Salomon
+
+   Parts of this driver are based on the Intel Pro Wireless 2100 GPL driver.
+
+   Code from Rtw8180 NetBSD driver by David Young has been really useful to
+   understand some things and gets some ideas
+
+   Code from rtl8181 project has been useful to me to understand some things.
+
+   Some code from 'Deuce' work
+
+   We want to tanks the Authors of such projects and the Ndiswrapper
+   project Authors.
+*/
+
+
+#include "r8180.h"
+#include "r8180_hw.h"
+#include "r8180_gct.h"
+
+
+//#define DEBUG_GCT
+
+/* the following experiment are just experiments.
+ * this means if you enable them you can have every kind
+ * of result, included damage the RF chip, so don't
+ * touch them if you don't know what you are doing.
+ * In any case, if you do it, do at your own risk
+ */
+
+//#define GCT_EXPERIMENT1  //improve RX sensivity
+
+//#define GCT_EXPERIMENT2
+
+//#define GCT_EXPERIMENT3  //iprove a bit RX signal quality ?
+
+//#define GCT_EXPERIMENT4 //maybe solve some brokeness with experiment1 ?
+
+//#define GCT_EXPERIMENT5
+
+//#define GCT_EXPERIMENT6  //not good
+
+
+u32 gct_chan[] = {
+	0x0,	//dummy channel 0
+	0x0, //1
+	0x1, //2
+	0x2, //3
+	0x3, //4
+	0x4, //5
+	0x5, //6
+	0x6, //7
+	0x7, //8
+	0x8, //9
+	0x9, //10
+	0xa, //11
+	0xb, //12
+	0xc, //13
+	0xd, //14
+};
+
+int gct_encode[16] = {
+	0, 8, 4, 0xC,
+	2, 0xA, 6, 0xE,
+	1, 9, 5, 0xD,
+	3, 0xB, 7, 0xF
+};
+
+void gct_rf_stabilize(struct net_device *dev)
+{
+	force_pci_posting(dev);
+	mdelay(3); //for now use a great value.. we may optimize in future
+}
+
+
+void write_gct(struct net_device *dev, u8 adr, u32 data)
+{
+//	struct r8180_priv *priv = ieee80211_priv(dev);
+	u32 phy_config;
+
+	phy_config =  gct_encode[(data & 0xf00) >> 8];
+	phy_config |= gct_encode[(data & 0xf0) >> 4 ] << 4;
+	phy_config |= gct_encode[(data & 0xf)       ] << 8;
+	phy_config |= gct_encode[(adr >> 1) & 0xf   ] << 12;
+	phy_config |=            (adr & 1 )           << 16;
+	phy_config |= gct_encode[(data & 0xf000)>>12] << 24;
+
+	phy_config |= 0x90000000; // MAC will bang bits to the chip
+
+
+	write_nic_dword(dev,PHY_CONFIG,phy_config);
+#ifdef DEBUG_GCT
+	DMESG("Writing GCT: %x (adr %x)",phy_config,adr);
+#endif
+	gct_rf_stabilize(dev);
+}
+
+
+
+void gct_write_phy_antenna(struct net_device *dev,short ch)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	u8 ant;
+
+	ant = GCT_ANTENNA;
+	if(priv->antb) /*default antenna is antenna B */
+		ant |= BB_ANTENNA_B;
+	if(ch == 14)
+		ant |= BB_ANTATTEN_CHAN14;
+	write_phy(dev,0x10,ant);
+	//DMESG("BB antenna %x ",ant);
+}
+
+
+void gct_rf_set_chan(struct net_device *dev, short ch)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	u32 txpw = 0xff & priv->chtxpwr[ch];
+	u32 chan = gct_chan[ch];
+
+	//write_phy(dev,3,txpw);
+#ifdef DEBUG_GCT
+	DMESG("Gct set channel");
+#endif
+	/* set TX power */
+	write_gct(dev,0x15,0);
+ 	write_gct(dev,6, txpw);
+	write_gct(dev,0x15, 0x10);
+	write_gct(dev,0x15,0);
+
+	/*set frequency*/
+	write_gct(dev,7, 0);
+      	write_gct(dev,0xB, chan);
+      	write_gct(dev,7, 0x1000);
+
+#ifdef DEBUG_GCT
+	DMESG("Gct set channel > write phy antenna");
+#endif
+
+
+	gct_write_phy_antenna(dev,ch);
+
+}
+
+
+void gct_rf_close(struct net_device *dev)
+{
+	u32 anaparam;
+
+	anaparam = read_nic_dword(dev,ANAPARAM);
+	anaparam &= 0x000fffff;
+	anaparam |= 0x3f900000;
+	rtl8180_set_anaparam(dev, anaparam);
+
+	write_gct(dev, 0x7, 0);
+	write_gct(dev, 0x1f, 0x45);
+	write_gct(dev, 0x1f, 0x5);
+	write_gct(dev, 0x0, 0x8e4);
+}
+
+
+void gct_rf_init(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	//u32 anaparam;
+
+
+	write_nic_byte(dev,PHY_DELAY,0x6);	//this is general
+	write_nic_byte(dev,CARRIER_SENSE_COUNTER,0x4c); //this is general
+
+	//DMESG("%x", read_nic_dword(dev,ANAPARAM));
+	/* we should set anaparm here*/
+	//rtl8180_set_anaparam(dev,anaparam);
+
+	write_gct(dev,0x1f,0);
+	write_gct(dev,0x1f,0);
+	write_gct(dev,0x1f,0x40);
+	write_gct(dev,0x1f,0x60);
+	write_gct(dev,0x1f,0x61);
+	write_gct(dev,0x1f,0x61);
+	write_gct(dev,0x0,0xae4);
+	write_gct(dev,0x1f,0x1);
+	write_gct(dev,0x1f,0x41);
+	write_gct(dev,0x1f,0x61);
+	write_gct(dev,0x1,0x1a23);
+	write_gct(dev,0x2,0x4971);
+	write_gct(dev,0x3,0x41de);
+	write_gct(dev,0x4,0x2d80);
+#ifdef GCT_EXPERIMENT1
+	//write_gct(dev,0x5,0x6810);  // from zydas driver. sens+ but quite slow
+	//write_gct(dev,0x5,0x681f);  //good+ (somewhat stable, better sens, performance decent)
+	write_gct(dev,0x5,0x685f);  //good performances, not sure sens is really so beeter
+	//write_gct(dev,0x5,0x687f);  //good performances, maybe sens is not improved
+	//write_gct(dev,0x5,0x689f);  //like above
+	//write_gct(dev,0x5,0x685e);  //bad
+	//write_gct(dev,0x5,0x68ff);  //good+ (somewhat stable, better sens(?), performance decent)
+	//write_gct(dev,0x5,0x68f0);  //bad
+	//write_gct(dev,0x5,0x6cff);  //sens+ but not so good
+	//write_gct(dev,0x5,0x6dff);  //sens+,apparentely very good but broken
+	//write_gct(dev,0x5,0x65ff);  //sens+,good
+	//write_gct(dev,0x5,0x78ff);  //sens + but almost broken
+	//write_gct(dev,0x5,0x7810);  //- //snes + but broken
+	//write_gct(dev,0x5,0x781f);  //-- //sens +
+	//write_gct(dev,0x5,0x78f0);  //low sens
+#else
+	write_gct(dev,0x5,0x61ff);   //best performance but weak sensitivity
+#endif
+#ifdef GCT_EXPERIMENT2
+	write_gct(dev,0x6,0xe);
+#else
+	write_gct(dev,0x6,0x0);
+#endif
+	write_gct(dev,0x7,0x0);
+	write_gct(dev,0x8,0x7533);
+	write_gct(dev,0x9,0xc401);
+	write_gct(dev,0xa,0x0);
+	write_gct(dev,0xc,0x1c7);
+	write_gct(dev,0xd,0x29d3);
+	write_gct(dev,0xe,0x2e8);
+	write_gct(dev,0x10,0x192);
+#ifdef GCT_EXPERIMENT3
+	write_gct(dev,0x11,0x246);
+#else
+	write_gct(dev,0x11,0x248);
+#endif
+	write_gct(dev,0x12,0x0);
+	write_gct(dev,0x13,0x20c4);
+#ifdef GCT_EXPERIMENT4
+	write_gct(dev,0x14,0xf488);
+#else
+	write_gct(dev,0x14,0xf4fc);
+#endif
+#ifdef GCT_EXPERIMENT5
+	write_gct(dev,0x15,0xb152);
+#else
+	write_gct(dev,0x15,0x0);
+#endif
+#ifdef GCT_EXPERIMENT6
+	write_gct(dev,0x1e,0x1);
+#endif
+	write_gct(dev,0x16,0x1500);
+
+	write_gct(dev,0x7,0x1000);
+	/*write_gct(dev,0x15,0x0);
+	write_gct(dev,0x6,0x15);
+	write_gct(dev,0x15,0x8);
+	write_gct(dev,0x15,0x0);
+*/
+	write_phy(dev,0,0xa8);
+
+/*	write_gct(dev,0x15,0x0);
+	write_gct(dev,0x6,0x12);
+	write_gct(dev,0x15,0x8);
+	write_gct(dev,0x15,0x0);
+*/
+	write_phy(dev,3,0x0);
+	write_phy(dev,4,0xc0); /* lna det*/
+	write_phy(dev,5,0x90);
+	write_phy(dev,6,0x1e);
+	write_phy(dev,7,0x64);
+
+#ifdef DEBUG_GCT
+	DMESG("Gct init> write phy antenna");
+#endif
+
+	gct_write_phy_antenna(dev,priv->chan);
+
+	write_phy(dev,0x11,0x88);
+	if(!priv->diversity)
+		write_phy(dev,0x12,0xc0);
+	else
+		write_phy(dev,0x12,0x40);
+
+	write_phy(dev,0x13,0x90 | priv->cs_treshold );
+
+	write_phy(dev,0x19,0x0);
+	write_phy(dev,0x1a,0xa0);
+	write_phy(dev,0x1b,0x44);
+
+#ifdef DEBUG_GCT
+	DMESG("Gct init > set channel2");
+#endif
+
+	gct_rf_set_chan(dev,priv->chan);
+}
diff --git a/drivers/staging/rtl8187se/r8180_gct.h b/drivers/staging/rtl8187se/r8180_gct.h
new file mode 100644
index 00000000000..fe965ca6430
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_gct.h
@@ -0,0 +1,25 @@
+/*
+	This is part of rtl8180 OpenSource driver - v 0.20
+	Copyright (C) Andrea Merello 2004  <andreamrl@tiscali.it>
+	Released under the terms of GPL (General Public Licence)
+
+	Parts of this driver are based on the GPL part of the official realtek driver
+	Parts of this driver are based on the rtl8180 driver skeleton from Patric Schenke & Andres Salomon
+	Parts of this driver are based on the Intel Pro Wireless 2100 GPL driver
+
+	We want to tanks the Authors of such projects and the Ndiswrapper project Authors.
+*/
+
+#define GCT_ANTENNA 0xA3
+
+
+// we use the untouched eeprom value- cross your finger ;-)
+#define GCT_ANAPARAM_PWR1_ON ??
+#define GCT_ANAPARAM_PWR0_ON ??
+
+
+
+void gct_rf_init(struct net_device *dev);
+void gct_rf_set_chan(struct net_device *dev,short ch);
+
+void gct_rf_close(struct net_device *dev);
diff --git a/drivers/staging/rtl8187se/r8180_hw.h b/drivers/staging/rtl8187se/r8180_hw.h
new file mode 100644
index 00000000000..bf38934bc09
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_hw.h
@@ -0,0 +1,956 @@
+/*
+	This is part of rtl8180 OpenSource driver.
+	Copyright (C) Andrea Merello 2004-2005  <andreamrl@tiscali.it>
+	Released under the terms of GPL (General Public Licence)
+
+	Parts of this driver are based on the GPL part of the
+	official Realtek driver.
+	Parts of this driver are based on the rtl8180 driver skeleton
+	from Patric Schenke & Andres Salomon.
+	Parts of this driver are based on the Intel Pro Wireless
+	2100 GPL driver.
+
+	We want to tanks the Authors of those projects
+	and the Ndiswrapper project Authors.
+*/
+
+/* Mariusz Matuszek added full registers definition with Realtek's name */
+
+/* this file contains register definitions for the rtl8180 MAC controller */
+#ifndef R8180_HW
+#define R8180_HW
+
+#define CONFIG_RTL8185B  //support for rtl8185B, xiong-2006-11-15
+#define CONFIG_RTL818X_S
+
+#define BIT0	0x00000001
+#define BIT1	0x00000002
+#define BIT2	0x00000004
+#define BIT3	0x00000008
+#define BIT4	0x00000010
+#define BIT5	0x00000020
+#define BIT6	0x00000040
+#define BIT7	0x00000080
+#define BIT8	0x00000100
+#define BIT9	0x00000200
+#define BIT10	0x00000400
+#define BIT11	0x00000800
+#define BIT12	0x00001000
+#define BIT13	0x00002000
+#define BIT14	0x00004000
+#define BIT15	0x00008000
+#define BIT16	0x00010000
+#define BIT17	0x00020000
+#define BIT18	0x00040000
+#define BIT19	0x00080000
+#define BIT20	0x00100000
+#define BIT21	0x00200000
+#define BIT22	0x00400000
+#define BIT23	0x00800000
+#define BIT24	0x01000000
+#define BIT25	0x02000000
+#define BIT26	0x04000000
+#define BIT27	0x08000000
+#define BIT28	0x10000000
+#define BIT29	0x20000000
+#define BIT30	0x40000000
+#define BIT31	0x80000000
+
+#define MAX_SLEEP_TIME (10000)
+#define MIN_SLEEP_TIME (50)
+
+#define BB_ANTATTEN_CHAN14	0x0c
+#define BB_ANTENNA_B 0x40
+
+#define BB_HOST_BANG (1<<30)
+#define BB_HOST_BANG_EN (1<<2)
+#define BB_HOST_BANG_CLK (1<<1)
+#define BB_HOST_BANG_DATA	 1
+
+#define ANAPARAM_TXDACOFF_SHIFT 27
+#define ANAPARAM_PWR0_MASK ((1<<30)|(1<<29)|(1<<28))
+#define ANAPARAM_PWR0_SHIFT 28
+#define ANAPARAM_PWR1_MASK ((1<<26)|(1<<25)|(1<<24)|(1<<23)|(1<<22)|(1<<21)|(1<<20))
+#define ANAPARAM_PWR1_SHIFT 20
+
+#define MAC0 0
+#define MAC1 1
+#define MAC2 2
+#define MAC3 3
+#define MAC4 4
+#define MAC5 5
+#define CMD 0x37
+#define CMD_RST_SHIFT 4
+#define CMD_RESERVED_MASK ((1<<1) | (1<<5) | (1<<6) | (1<<7))
+#define CMD_RX_ENABLE_SHIFT 3
+#define CMD_TX_ENABLE_SHIFT 2
+
+#define EPROM_CMD 0x50
+#define EPROM_CMD_RESERVED_MASK ((1<<5)|(1<<4))
+#define EPROM_CMD_OPERATING_MODE_SHIFT 6
+#define EPROM_CMD_OPERATING_MODE_MASK ((1<<7)|(1<<6))
+#define EPROM_CMD_CONFIG 0x3
+#define EPROM_CMD_NORMAL 0
+#define EPROM_CMD_LOAD 1
+#define EPROM_CMD_PROGRAM 2
+#define EPROM_CS_SHIFT 3
+#define EPROM_CK_SHIFT 2
+#define EPROM_W_SHIFT 1
+#define EPROM_R_SHIFT 0
+#define CONFIG2_DMA_POLLING_MODE_SHIFT 3
+#define INTA 0x3e
+#define INTA_TXOVERFLOW (1<<15)
+#define INTA_TIMEOUT (1<<14)
+#define INTA_BEACONTIMEOUT (1<<13)
+#define INTA_ATIM (1<<12)
+#define INTA_BEACONDESCERR (1<<11)
+#define INTA_BEACONDESCOK (1<<10)
+#define INTA_HIPRIORITYDESCERR (1<<9)
+#define INTA_HIPRIORITYDESCOK (1<<8)
+#define INTA_NORMPRIORITYDESCERR (1<<7)
+#define INTA_NORMPRIORITYDESCOK (1<<6)
+#define INTA_RXOVERFLOW (1<<5)
+#define INTA_RXDESCERR (1<<4)
+#define INTA_LOWPRIORITYDESCERR (1<<3)
+#define INTA_LOWPRIORITYDESCOK (1<<2)
+#define INTA_RXCRCERR (1<<1)
+#define INTA_RXOK (1)
+#define INTA_MASK 0x3c
+#define RXRING_ADDR 0xe4 // page 0
+#define PGSELECT 0x5e
+#define PGSELECT_PG_SHIFT 0
+#define RX_CONF 0x44
+#define MAC_FILTER_MASK ((1<<0) | (1<<1) | (1<<2) | (1<<3) | (1<<5) | \
+(1<<12) | (1<<18) | (1<<19) | (1<<20) | (1<<21) | (1<<22) | (1<<23))
+#define RX_CHECK_BSSID_SHIFT 23
+#define ACCEPT_PWR_FRAME_SHIFT 22
+#define ACCEPT_MNG_FRAME_SHIFT 20
+#define ACCEPT_CTL_FRAME_SHIFT 19
+#define ACCEPT_DATA_FRAME_SHIFT 18
+#define ACCEPT_ICVERR_FRAME_SHIFT 12
+#define ACCEPT_CRCERR_FRAME_SHIFT 5
+#define ACCEPT_BCAST_FRAME_SHIFT 3
+#define ACCEPT_MCAST_FRAME_SHIFT 2
+#define ACCEPT_ALLMAC_FRAME_SHIFT 0
+#define ACCEPT_NICMAC_FRAME_SHIFT 1
+#define RX_FIFO_THRESHOLD_MASK ((1<<13) | (1<<14) | (1<<15))
+#define RX_FIFO_THRESHOLD_SHIFT 13
+#define RX_FIFO_THRESHOLD_128 3
+#define RX_FIFO_THRESHOLD_256 4
+#define RX_FIFO_THRESHOLD_512 5
+#define RX_FIFO_THRESHOLD_1024 6
+#define RX_FIFO_THRESHOLD_NONE 7
+#define RX_AUTORESETPHY_SHIFT 28
+#define EPROM_TYPE_SHIFT 6
+#define TX_CONF 0x40
+#define TX_CONF_HEADER_AUTOICREMENT_SHIFT 30
+#define TX_LOOPBACK_SHIFT 17
+#define TX_LOOPBACK_MAC 1
+#define TX_LOOPBACK_BASEBAND 2
+#define TX_LOOPBACK_NONE 0
+#define TX_LOOPBACK_CONTINUE 3
+#define TX_LOOPBACK_MASK ((1<<17)|(1<<18))
+#define TX_DPRETRY_SHIFT 0
+#define R8180_MAX_RETRY 255
+#define TX_RTSRETRY_SHIFT 8
+#define TX_NOICV_SHIFT 19
+#define TX_NOCRC_SHIFT 16
+#define TX_DMA_POLLING 0xd9
+#define TX_DMA_POLLING_BEACON_SHIFT 7
+#define TX_DMA_POLLING_HIPRIORITY_SHIFT 6
+#define TX_DMA_POLLING_NORMPRIORITY_SHIFT 5
+#define TX_DMA_POLLING_LOWPRIORITY_SHIFT 4
+#define TX_DMA_STOP_BEACON_SHIFT 3
+#define TX_DMA_STOP_HIPRIORITY_SHIFT 2
+#define TX_DMA_STOP_NORMPRIORITY_SHIFT 1
+#define TX_DMA_STOP_LOWPRIORITY_SHIFT 0
+#define TX_MANAGEPRIORITY_RING_ADDR 0x0C
+#define TX_BKPRIORITY_RING_ADDR 0x10
+#define TX_BEPRIORITY_RING_ADDR 0x14
+#define TX_VIPRIORITY_RING_ADDR 0x20
+#define TX_VOPRIORITY_RING_ADDR 0x24
+#define TX_HIGHPRIORITY_RING_ADDR 0x28
+//AC_VI and Low priority share the sane queue
+#define TX_LOWPRIORITY_RING_ADDR TX_VIPRIORITY_RING_ADDR
+//AC_VO and Norm priority share the same queue
+#define TX_NORMPRIORITY_RING_ADDR TX_VOPRIORITY_RING_ADDR
+
+#define MAX_RX_DMA_MASK ((1<<8) | (1<<9) | (1<<10))
+#define MAX_RX_DMA_2048 7
+#define MAX_RX_DMA_1024	6
+#define MAX_RX_DMA_SHIFT 10
+#define INT_TIMEOUT 0x48
+#define CONFIG3_CLKRUN_SHIFT 2
+#define CONFIG3_ANAPARAM_W_SHIFT 6
+#define ANAPARAM 0x54
+#define BEACON_INTERVAL 0x70
+#define BEACON_INTERVAL_MASK ((1<<0)|(1<<1)|(1<<2)|(1<<3)|(1<<4)|(1<<5)| \
+(1<<6)|(1<<7)|(1<<8)|(1<<9))
+#define ATIM_MASK ((1<<0)|(1<<1)|(1<<2)|(1<<3)|(1<<4)|(1<<5)|(1<<6)|(1<<7)| \
+(1<<8)|(1<<9))
+#define ATIM 0x72
+#define EPROM_CS_SHIFT 3
+#define EPROM_CK_SHIFT 2
+#define PHY_DELAY 0x78
+#define PHY_CONFIG 0x80
+#define PHY_ADR 0x7c
+#define PHY_READ 0x7e
+#define CARRIER_SENSE_COUNTER 0x79 //byte
+#define SECURITY 0x5f //1209 this is sth wrong
+#define SECURITY_WEP_TX_ENABLE_SHIFT 1
+#define SECURITY_WEP_RX_ENABLE_SHIFT 0
+#define SECURITY_ENCRYP_104 1
+#define SECURITY_ENCRYP_SHIFT 4
+#define SECURITY_ENCRYP_MASK ((1<<4)|(1<<5))
+#define KEY0 0x90  //1209 this is sth wrong
+#define CONFIG2_ANTENNA_SHIFT 6
+#define TX_BEACON_RING_ADDR 0x4c
+#define CONFIG0_WEP40_SHIFT 7
+#define CONFIG0_WEP104_SHIFT 6
+#define AGCRESET_SHIFT 5
+
+
+
+/*
+ * Operational registers offsets in PCI (I/O) space.
+ * RealTek names are used.
+ */
+
+#define IDR0 0x0000
+#define IDR1 0x0001
+#define IDR2 0x0002
+#define IDR3 0x0003
+#define IDR4 0x0004
+#define IDR5 0x0005
+
+/* 0x0006 - 0x0007 - reserved */
+
+#define MAR0 0x0008
+#define MAR1 0x0009
+#define MAR2 0x000A
+#define MAR3 0x000B
+#define MAR4 0x000C
+#define MAR5 0x000D
+#define MAR6 0x000E
+#define MAR7 0x000F
+
+/* 0x0010 - 0x0017 - reserved */
+
+#define TSFTR 0x0018
+#define TSFTR_END 0x001F
+
+#define TLPDA 0x0020
+#define TLPDA_END 0x0023
+#define TNPDA 0x0024
+#define TNPDA_END 0x0027
+#define THPDA 0x0028
+#define THPDA_END 0x002B
+
+#define BSSID 0x002E
+#define BSSID_END 0x0033
+
+#define CR 0x0037
+
+#ifdef CONFIG_RTL8185B
+#define RF_SW_CONFIG	        0x8			// store data which is transmitted to RF for driver
+#define RF_SW_CFG_SI		BIT1
+#define PIFS			0x2C			// PCF InterFrame Spacing Timer Setting.
+#define EIFS			0x2D			// Extended InterFrame Space Timer, in unit of 4 us.
+
+#define BRSR			0x34			// Basic rate set
+
+#define IMR 0x006C
+#define ISR 0x003C
+#else
+#define BRSR 0x002C
+#define BRSR_END 0x002D
+
+/* 0x0034 - 0x0034 - reserved */
+#define EIFS 0x0035
+
+#define IMR 0x003C
+#define IMR_END 0x003D
+#define ISR 0x003E
+#define ISR_END 0x003F
+#endif
+
+#define TCR 0x0040
+#define TCR_END 0x0043
+
+#define RCR 0x0044
+#define RCR_END 0x0047
+
+#define TimerInt 0x0048
+#define TimerInt_END 0x004B
+
+#define TBDA 0x004C
+#define TBDA_END 0x004F
+
+#define CR9346 0x0050
+
+#define CONFIG0 0x0051
+#define CONFIG1 0x0052
+#define CONFIG2 0x0053
+
+#define ANA_PARM 0x0054
+#define ANA_PARM_END 0x0x0057
+
+#define MSR 0x0058
+
+#define CONFIG3 0x0059
+#define CONFIG4 0x005A
+#ifdef CONFIG_RTL8185B
+#ifdef CONFIG_RTL818X_S
+	// SD3 szuyitasi: Mac0x57= CC -> B0 Mac0x60= D1 -> C6
+	// Mac0x60 = 0x000004C6 power save parameters
+	#define ANAPARM_ASIC_ON    0xB0054D00
+	#define ANAPARM2_ASIC_ON  0x000004C6
+
+	#define ANAPARM_ON ANAPARM_ASIC_ON
+	#define ANAPARM2_ON ANAPARM2_ASIC_ON
+#else
+	// SD3 CMLin:
+	#define ANAPARM_ASIC_ON    0x45090658
+	#define ANAPARM2_ASIC_ON  0x727f3f52
+
+	#define ANAPARM_ON ANAPARM_ASIC_ON
+	#define ANAPARM2_ON ANAPARM2_ASIC_ON
+#endif
+#endif
+
+#define TESTR 0x005B
+
+/* 0x005C - 0x005D - reserved */
+
+#define PSR 0x005E
+
+/* 0x0060 - 0x006F - reserved */
+
+#define BcnItv 0x0070
+#define BcnItv_END 0x0071
+
+#define AtimWnd 0x0072
+#define AtimWnd_END 0x0073
+
+#define BintrItv 0x0074
+#define BintrItv_END 0x0075
+
+#define AtimtrItv 0x0076
+#define AtimtrItv_END 0x0077
+
+#define PhyDelay 0x0078
+
+#define CRCount 0x0079
+
+/* 0x007A - 0x007B - reserved */
+
+#define PhyAddr 0x007C
+#define PhyDataW 0x007D
+#define PhyDataR 0x007E
+
+#define PhyCFG 0x0080
+#define PhyCFG_END 0x0083
+
+/* following are for rtl8185 */
+#define RFPinsOutput 0x80
+#define RFPinsEnable 0x82
+#define RF_TIMING 0x8c
+#define RFPinsSelect 0x84
+#define ANAPARAM2 0x60
+#define RF_PARA 0x88
+#define RFPinsInput 0x86
+#define GP_ENABLE 0x90
+#define GPIO 0x91
+#define SW_CONTROL_GPIO 0x400
+#define TX_ANTENNA 0x9f
+#define TX_GAIN_OFDM 0x9e
+#define TX_GAIN_CCK 0x9d
+#define WPA_CONFIG 0xb0
+#define TX_AGC_CTL 0x9c
+#define TX_AGC_CTL_PERPACKET_GAIN_SHIFT 0
+#define TX_AGC_CTL_PERPACKET_ANTSEL_SHIFT 1
+#define TX_AGC_CTL_FEEDBACK_ANT 2
+#define RESP_RATE 0x34
+#define SIFS 0xb4
+#define DIFS 0xb5
+
+#define SLOT 0xb6
+#define CW_CONF 0xbc
+#define CW_CONF_PERPACKET_RETRY_SHIFT 1
+#define CW_CONF_PERPACKET_CW_SHIFT 0
+#define CW_VAL 0xbd
+#define MAX_RESP_RATE_SHIFT 4
+#define MIN_RESP_RATE_SHIFT 0
+#define RATE_FALLBACK 0xbe
+/*
+ *  0x0084 - 0x00D3 is selected to page 1 when PSEn bit (bit0, PSR)
+ *  is set to 1
+ */
+
+#define Wakeup0 0x0084
+#define Wakeup0_END 0x008B
+
+#define Wakeup1 0x008C
+#define Wakeup1_END 0x0093
+
+#define Wakeup2LD 0x0094
+#define Wakeup2LD_END 0x009B
+#define Wakeup2HD 0x009C
+#define Wakeup2HD_END 0x00A3
+
+#define Wakeup3LD 0x00A4
+#define Wakeup3LD_END 0x00AB
+#define Wakeup3HD 0x00AC
+#define Wakeup3HD_END 0x00B3
+
+#define Wakeup4LD 0x00B4
+#define Wakeup4LD_END 0x00BB
+#define Wakeup4HD 0x00BC
+#define Wakeup4HD_END 0x00C3
+
+#define CRC0 0x00C4
+#define CRC0_END 0x00C5
+#define CRC1 0x00C6
+#define CRC1_END 0x00C7
+#define CRC2 0x00C8
+#define CRC2_END 0x00C9
+#define CRC3 0x00CA
+#define CRC3_END 0x00CB
+#define CRC4 0x00CC
+#define CRC4_END 0x00CD
+
+/* 0x00CE - 0x00D3 - reserved */
+
+
+
+/*
+ *  0x0084 - 0x00D3 is selected to page 0 when PSEn bit (bit0, PSR)
+ *  is set to 0
+ */
+
+/* 0x0084 - 0x008F - reserved */
+
+#define DK0 0x0090
+#define DK0_END 0x009F
+#define DK1 0x00A0
+#define DK1_END 0x00AF
+#define DK2 0x00B0
+#define DK2_END 0x00BF
+#define DK3 0x00C0
+#define DK3_END 0x00CF
+
+/* 0x00D0 - 0x00D3 - reserved */
+
+
+
+
+
+/* 0x00D4 - 0x00D7 - reserved */
+
+#define CONFIG5 0x00D8
+
+#define TPPoll 0x00D9
+
+/* 0x00DA - 0x00DB - reserved */
+
+#ifdef CONFIG_RTL818X_S
+#define PHYPR			0xDA			//0xDA - 0x0B PHY Parameter Register.
+#endif
+
+#define CWR 0x00DC
+#define CWR_END 0x00DD
+
+#define RetryCTR 0x00DE
+
+/* 0x00DF - 0x00E3 - reserved */
+
+#define RDSAR 0x00E4
+#define RDSAR_END 0x00E7
+
+/* 0x00E8 - 0x00EF - reserved */
+#ifdef CONFIG_RTL818X_S
+#define LED_CONTROL		0xED
+#endif
+
+#define FER 0x00F0
+#define FER_END 0x00F3
+
+#ifdef CONFIG_RTL8185B
+#define FEMR			0x1D4	// Function Event Mask register
+#else
+#define FEMR 0x00F4
+#define FEMR_END 0x00F7
+#endif
+
+#define FPSR 0x00F8
+#define FPSR_END 0x00FB
+
+#define FFER 0x00FC
+#define FFER_END 0x00FF
+
+
+
+/*
+ * Bitmasks for specific register functions.
+ * Names are derived from the register name and function name.
+ *
+ * <REGISTER>_<FUNCTION>[<bit>]
+ *
+ * this leads to some awkward names...
+ */
+
+#define BRSR_BPLCP  ((1<< 8))
+#define BRSR_MBR    ((1<< 1)|(1<< 0))
+#define BRSR_MBR_8185 ((1<< 11)|(1<< 10)|(1<< 9)|(1<< 8)|(1<< 7)|(1<< 6)|(1<< 5)|(1<< 4)|(1<< 3)|(1<< 2)|(1<< 1)|(1<< 0))
+#define BRSR_MBR0   ((1<< 0))
+#define BRSR_MBR1   ((1<< 1))
+
+#define CR_RST      ((1<< 4))
+#define CR_RE       ((1<< 3))
+#define CR_TE       ((1<< 2))
+#define CR_MulRW    ((1<< 0))
+
+#ifdef CONFIG_RTL8185B
+#define IMR_Dot11hInt	((1<< 25))			// 802.11h Measurement Interrupt
+#define IMR_BcnDmaInt	((1<< 24))			// Beacon DMA Interrupt // What differenct between BcnDmaInt and BcnInt???
+#define IMR_WakeInt		((1<< 23))			// Wake Up Interrupt
+#define IMR_TXFOVW		((1<< 22))			// Tx FIFO Overflow Interrupt
+#define IMR_TimeOut1	((1<< 21))			// Time Out Interrupt 1
+#define IMR_BcnInt		((1<< 20))			// Beacon Time out Interrupt
+#define IMR_ATIMInt		((1<< 19))			// ATIM Time Out Interrupt
+#define IMR_TBDER		((1<< 18))			// Tx Beacon Descriptor Error Interrupt
+#define IMR_TBDOK		((1<< 17))			// Tx Beacon Descriptor OK Interrupt
+#define IMR_THPDER		((1<< 16))			// Tx High Priority Descriptor Error Interrupt
+#define IMR_THPDOK		((1<< 15))			// Tx High Priority Descriptor OK Interrupt
+#define IMR_TVODER		((1<< 14))			// Tx AC_VO Descriptor Error Interrupt
+#define IMR_TVODOK		((1<< 13))			// Tx AC_VO Descriptor OK Interrupt
+#define IMR_FOVW		((1<< 12))			// Rx FIFO Overflow Interrupt
+#define IMR_RDU			((1<< 11))			// Rx Descriptor Unavailable Interrupt
+#define IMR_TVIDER		((1<< 10))			// Tx AC_VI Descriptor Error Interrupt
+#define IMR_TVIDOK		((1<< 9))		// Tx AC_VI Descriptor OK Interrupt
+#define IMR_RER			((1<< 8))		// Rx Error Interrupt
+#define IMR_ROK			((1<< 7))		// Receive OK Interrupt
+#define IMR_TBEDER		((1<< 6))			// Tx AC_BE Descriptor Error Interrupt
+#define IMR_TBEDOK		((1<< 5))			// Tx AC_BE Descriptor OK Interrupt
+#define IMR_TBKDER		((1<< 4))		// Tx AC_BK Descriptor Error Interrupt
+#define IMR_TBKDOK		((1<< 3))			// Tx AC_BK Descriptor OK Interrupt
+#define IMR_RQoSOK		((1<< 2))		// Rx QoS OK Interrupt
+#define IMR_TimeOut2	((1<< 1))		// Time Out Interrupt 2
+#define IMR_TimeOut3	((1<< 0))			// Time Out Interrupt 3
+#define IMR_TMGDOK      ((1<<30))
+#define ISR_Dot11hInt	((1<< 25))			// 802.11h Measurement Interrupt
+#define ISR_BcnDmaInt	((1<< 24))			// Beacon DMA Interrupt // What differenct between BcnDmaInt and BcnInt???
+#define ISR_WakeInt		((1<< 23))			// Wake Up Interrupt
+#define ISR_TXFOVW		((1<< 22))			// Tx FIFO Overflow Interrupt
+#define ISR_TimeOut1	((1<< 21))			// Time Out Interrupt 1
+#define ISR_BcnInt		((1<< 20))			// Beacon Time out Interrupt
+#define ISR_ATIMInt		((1<< 19))			// ATIM Time Out Interrupt
+#define ISR_TBDER		((1<< 18))			// Tx Beacon Descriptor Error Interrupt
+#define ISR_TBDOK		((1<< 17))			// Tx Beacon Descriptor OK Interrupt
+#define ISR_THPDER		((1<< 16))			// Tx High Priority Descriptor Error Interrupt
+#define ISR_THPDOK		((1<< 15))			// Tx High Priority Descriptor OK Interrupt
+#define ISR_TVODER		((1<< 14))			// Tx AC_VO Descriptor Error Interrupt
+#define ISR_TVODOK		((1<< 13))			// Tx AC_VO Descriptor OK Interrupt
+#define ISR_FOVW		((1<< 12))			// Rx FIFO Overflow Interrupt
+#define ISR_RDU			((1<< 11))			// Rx Descriptor Unavailable Interrupt
+#define ISR_TVIDER		((1<< 10))			// Tx AC_VI Descriptor Error Interrupt
+#define ISR_TVIDOK		((1<< 9))		// Tx AC_VI Descriptor OK Interrupt
+#define ISR_RER			((1<< 8))		// Rx Error Interrupt
+#define ISR_ROK			((1<< 7))		// Receive OK Interrupt
+#define ISR_TBEDER		((1<< 6))			// Tx AC_BE Descriptor Error Interrupt
+#define ISR_TBEDOK		((1<< 5))			// Tx AC_BE Descriptor OK Interrupt
+#define ISR_TBKDER		((1<< 4))		// Tx AC_BK Descriptor Error Interrupt
+#define ISR_TBKDOK		((1<< 3))			// Tx AC_BK Descriptor OK Interrupt
+#define ISR_RQoSOK		((1<< 2))		// Rx QoS OK Interrupt
+#define ISR_TimeOut2	((1<< 1))		// Time Out Interrupt 2
+#define ISR_TimeOut3	((1<< 0))			// Time Out Interrupt 3
+
+//these definition is used for Tx/Rx test temporarily
+#define ISR_TLPDER  ISR_TVIDER
+#define ISR_TLPDOK  ISR_TVIDOK
+#define ISR_TNPDER  ISR_TVODER
+#define ISR_TNPDOK  ISR_TVODOK
+#define ISR_TimeOut ISR_TimeOut1
+#define ISR_RXFOVW ISR_FOVW
+
+#else
+#define IMR_TXFOVW  ((1<<15))
+#define IMR_TimeOut ((1<<14))
+#define IMR_BcnInt  ((1<<13))
+#define IMR_ATIMInt ((1<<12))
+#define IMR_TBDER   ((1<<11))
+#define IMR_TBDOK   ((1<<10))
+#define IMR_THPDER  ((1<< 9))
+#define IMR_THPDOK  ((1<< 8))
+#define IMR_TNPDER  ((1<< 7))
+#define IMR_TNPDOK  ((1<< 6))
+#define IMR_RXFOVW  ((1<< 5))
+#define IMR_RDU     ((1<< 4))
+#define IMR_TLPDER  ((1<< 3))
+#define IMR_TLPDOK  ((1<< 2))
+#define IMR_RER     ((1<< 1))
+#define IMR_ROK     ((1<< 0))
+
+#define ISR_TXFOVW  ((1<<15))
+#define ISR_TimeOut ((1<<14))
+#define ISR_BcnInt  ((1<<13))
+#define ISR_ATIMInt ((1<<12))
+#define ISR_TBDER   ((1<<11))
+#define ISR_TBDOK   ((1<<10))
+#define ISR_THPDER  ((1<< 9))
+#define ISR_THPDOK  ((1<< 8))
+#define ISR_TNPDER  ((1<< 7))
+#define ISR_TNPDOK  ((1<< 6))
+#define ISR_RXFOVW  ((1<< 5))
+#define ISR_RDU     ((1<< 4))
+#define ISR_TLPDER  ((1<< 3))
+#define ISR_TLPDOK  ((1<< 2))
+#define ISR_RER     ((1<< 1))
+#define ISR_ROK     ((1<< 0))
+#endif
+
+#define HW_VERID_R8180_F 3
+#define HW_VERID_R8180_ABCD 2
+#define HW_VERID_R8185_ABC 4
+#define HW_VERID_R8185_D 5
+#ifdef CONFIG_RTL8185B
+#define HW_VERID_R8185B_B 6
+#endif
+
+#define TCR_CWMIN   ((1<<31))
+#define TCR_SWSEQ   ((1<<30))
+#define TCR_HWVERID_MASK ((1<<27)|(1<<26)|(1<<25))
+#define TCR_HWVERID_SHIFT 25
+#define TCR_SAT     ((1<<24))
+#define TCR_PLCP_LEN TCR_SAT // rtl8180
+#define TCR_MXDMA_MASK   ((1<<23)|(1<<22)|(1<<21))
+#define TCR_MXDMA_1024 6
+#define TCR_MXDMA_2048 7
+#define TCR_MXDMA_SHIFT  21
+#define TCR_DISCW   ((1<<20))
+#define TCR_ICV     ((1<<19))
+#define TCR_LBK     ((1<<18)|(1<<17))
+#define TCR_LBK1    ((1<<18))
+#define TCR_LBK0    ((1<<17))
+#define TCR_CRC     ((1<<16))
+#define TCR_DPRETRY_MASK   ((1<<15)|(1<<14)|(1<<13)|(1<<12)|(1<<11)|(1<<10)|(1<<9)|(1<<8))
+#define TCR_RTSRETRY_MASK   ((1<<0)|(1<<1)|(1<<2)|(1<<3)|(1<<4)|(1<<5)|(1<<6)|(1<<7))
+#define TCR_PROBE_NOTIMESTAMP_SHIFT 29 //rtl8185
+
+#define RCR_ONLYERLPKT ((1<<31))
+#define RCR_CS_SHIFT   29
+#define RCR_CS_MASK    ((1<<30) | (1<<29))
+#define RCR_ENMARP     ((1<<28))
+#define RCR_CBSSID     ((1<<23))
+#define RCR_APWRMGT    ((1<<22))
+#define RCR_ADD3       ((1<<21))
+#define RCR_AMF        ((1<<20))
+#define RCR_ACF        ((1<<19))
+#define RCR_ADF        ((1<<18))
+#define RCR_RXFTH      ((1<<15)|(1<<14)|(1<<13))
+#define RCR_RXFTH2     ((1<<15))
+#define RCR_RXFTH1     ((1<<14))
+#define RCR_RXFTH0     ((1<<13))
+#define RCR_AICV       ((1<<12))
+#define RCR_MXDMA      ((1<<10)|(1<< 9)|(1<< 8))
+#define RCR_MXDMA2     ((1<<10))
+#define RCR_MXDMA1     ((1<< 9))
+#define RCR_MXDMA0     ((1<< 8))
+#define RCR_9356SEL    ((1<< 6))
+#define RCR_ACRC32     ((1<< 5))
+#define RCR_AB         ((1<< 3))
+#define RCR_AM         ((1<< 2))
+#define RCR_APM        ((1<< 1))
+#define RCR_AAP        ((1<< 0))
+
+#define CR9346_EEM     ((1<<7)|(1<<6))
+#define CR9346_EEM1    ((1<<7))
+#define CR9346_EEM0    ((1<<6))
+#define CR9346_EECS    ((1<<3))
+#define CR9346_EESK    ((1<<2))
+#define CR9346_EED1    ((1<<1))
+#define CR9346_EED0    ((1<<0))
+
+#define CONFIG0_WEP104     ((1<<6))
+#define CONFIG0_LEDGPO_En  ((1<<4))
+#define CONFIG0_Aux_Status ((1<<3))
+#define CONFIG0_GL         ((1<<1)|(1<<0))
+#define CONFIG0_GL1        ((1<<1))
+#define CONFIG0_GL0        ((1<<0))
+
+#define CONFIG1_LEDS       ((1<<7)|(1<<6))
+#define CONFIG1_LEDS1      ((1<<7))
+#define CONFIG1_LEDS0      ((1<<6))
+#define CONFIG1_LWACT      ((1<<4))
+#define CONFIG1_MEMMAP     ((1<<3))
+#define CONFIG1_IOMAP      ((1<<2))
+#define CONFIG1_VPD        ((1<<1))
+#define CONFIG1_PMEn       ((1<<0))
+
+#define CONFIG2_LCK        ((1<<7))
+#define CONFIG2_ANT        ((1<<6))
+#define CONFIG2_DPS        ((1<<3))
+#define CONFIG2_PAPE_sign  ((1<<2))
+#define CONFIG2_PAPE_time  ((1<<1)|(1<<0))
+#define CONFIG2_PAPE_time1 ((1<<1))
+#define CONFIG2_PAPE_time0 ((1<<0))
+
+#define CONFIG3_GNTSel     ((1<<7))
+#define CONFIG3_PARM_En    ((1<<6))
+#define CONFIG3_Magic      ((1<<5))
+#define CONFIG3_CardB_En   ((1<<3))
+#define CONFIG3_CLKRUN_En  ((1<<2))
+#define CONFIG3_FuncRegEn  ((1<<1))
+#define CONFIG3_FBtbEn     ((1<<0))
+
+#define CONFIG4_VCOPDN     ((1<<7))
+#define CONFIG4_PWROFF     ((1<<6))
+#define CONFIG4_PWRMGT     ((1<<5))
+#define CONFIG4_LWPME      ((1<<4))
+#define CONFIG4_LWPTN      ((1<<2))
+#define CONFIG4_RFTYPE     ((1<<1)|(1<<0))
+#define CONFIG4_RFTYPE1    ((1<<1))
+#define CONFIG4_RFTYPE0    ((1<<0))
+
+#define CONFIG5_TX_FIFO_OK ((1<<7))
+#define CONFIG5_RX_FIFO_OK ((1<<6))
+#define CONFIG5_CALON      ((1<<5))
+#define CONFIG5_EACPI      ((1<<2))
+#define CONFIG5_LANWake    ((1<<1))
+#define CONFIG5_PME_STS    ((1<<0))
+
+#define MSR_LINK_MASK      ((1<<2)|(1<<3))
+#define MSR_LINK_MANAGED   2
+#define MSR_LINK_NONE      0
+#define MSR_LINK_SHIFT     2
+#define MSR_LINK_ADHOC     1
+#define MSR_LINK_MASTER    3
+
+#define PSR_GPO            ((1<<7))
+#define PSR_GPI            ((1<<6))
+#define PSR_LEDGPO1        ((1<<5))
+#define PSR_LEDGPO0        ((1<<4))
+#define PSR_UWF            ((1<<1))
+#define PSR_PSEn           ((1<<0))
+
+#define SCR_KM             ((1<<5)|(1<<4))
+#define SCR_KM1            ((1<<5))
+#define SCR_KM0            ((1<<4))
+#define SCR_TXSECON        ((1<<1))
+#define SCR_RXSECON        ((1<<0))
+
+#define BcnItv_BcnItv      (0x01FF)
+
+#define AtimWnd_AtimWnd    (0x01FF)
+
+#define BintrItv_BintrItv  (0x01FF)
+
+#define AtimtrItv_AtimtrItv (0x01FF)
+
+#define PhyDelay_PhyDelay  ((1<<2)|(1<<1)|(1<<0))
+
+#define TPPoll_BQ    ((1<<7))
+#define TPPoll_HPQ   ((1<<6))
+#define TPPoll_NPQ   ((1<<5))
+#define TPPoll_LPQ   ((1<<4))
+#define TPPoll_SBQ   ((1<<3))
+#define TPPoll_SHPQ  ((1<<2))
+#define TPPoll_SNPQ  ((1<<1))
+#define TPPoll_SLPQ  ((1<<0))
+
+#define CWR_CW       (0x01FF)
+
+#define FER_INTR     ((1<<15))
+#define FER_GWAKE    ((1<< 4))
+
+#define FEMR_INTR    ((1<<15))
+#define FEMR_WKUP    ((1<<14))
+#define FEMR_GWAKE   ((1<< 4))
+
+#define FPSR_INTR    ((1<<15))
+#define FPSR_GWAKE   ((1<< 4))
+
+#define FFER_INTR    ((1<<15))
+#define FFER_GWAKE   ((1<< 4))
+
+#ifdef CONFIG_RTL8185B
+// Three wire mode.
+#define SW_THREE_WIRE			0
+#define HW_THREE_WIRE			2
+//RTL8187S by amy
+#define HW_THREE_WIRE_PI		5
+#define HW_THREE_WIRE_SI		6
+//by amy
+#define TCR_LRL_OFFSET		0
+#define TCR_SRL_OFFSET		8
+#define TCR_MXDMA_OFFSET	21
+#define TCR_DISReqQsize_OFFSET		28
+#define TCR_DurProcMode_OFFSET		30
+
+#define RCR_MXDMA_OFFSET				8
+#define RCR_FIFO_OFFSET					13
+
+#define TMGDS			0x0C			// Tx Management Descriptor Address
+#define TBKDS			0x10			// Tx AC_BK Descriptor Address
+#define TBEDS			0x14			// Tx AC_BE Descriptor Address
+#define TLPDS			0x20			// Tx AC_VI Descriptor Address
+#define TNPDS			0x24			// Tx AC_VO Descriptor Address
+#define THPDS			0x28			// Tx Hign Priority Descriptor Address
+
+#define TBDS			0x4c			// Beacon descriptor queue start address
+
+#define RDSA			0xE4			// Receive descriptor queue start address
+
+#define AckTimeOutReg	0x79		// ACK timeout register, in unit of 4 us.
+
+#define RFTiming			0x8C
+
+#define TPPollStop 		0x93
+
+#define TXAGC_CTL		0x9C			// <RJ_TODO_8185B> TX_AGC_CONTROL (0x9C seems be removed at 8185B, see p37).
+#define CCK_TXAGC		0x9D
+#define OFDM_TXAGC		0x9E
+#define ANTSEL			0x9F
+
+#define ACM_CONTROL             0x00BF      // ACM Control Registe
+
+#define RTL8185B_VER_REG    0xE1
+
+#define	IntMig			0xE2			// Interrupt Migration (0xE2 ~ 0xE3)
+
+#define TID_AC_MAP		0xE8			// TID to AC Mapping Register
+
+#define ANAPARAM3		0xEE			// <RJ_TODO_8185B> How to use it?
+
+#define AC_VO_PARAM		0xF0			// AC_VO Parameters Record
+#define AC_VI_PARAM		0xF4			// AC_VI Parameters Record
+#define AC_BE_PARAM		0xF8			// AC_BE Parameters Record
+#define AC_BK_PARAM		0xFC			// AC_BK Parameters Record
+
+#ifdef CONFIG_RTL818X_S
+#define BcnTimingAdjust	0x16A			// Beacon Timing Adjust Register.
+#define GPIOCtrl			0x16B			// GPIO Control Register.
+#define PSByGC			0x180			// 0x180 - 0x183 Power Saving by Gated Clock.
+#endif
+#define ARFR			0x1E0	// Auto Rate Fallback Register (0x1e0 ~ 0x1e2)
+
+#define RFSW_CTRL			0x272	// 0x272-0x273.
+#define SW_3W_DB0			0x274	// Software 3-wire data buffer bit 31~0.
+#define SW_3W_DB1			0x278	// Software 3-wire data buffer bit 63~32.
+#define SW_3W_CMD0			0x27C	// Software 3-wire Control/Status Register.
+#define SW_3W_CMD1			0x27D	// Software 3-wire Control/Status Register.
+
+#ifdef CONFIG_RTL818X_S
+#define PI_DATA_READ		0X360	// 0x360 - 0x361  Parallel Interface Data Register.
+#define SI_DATA_READ		0x362	// 0x362 - 0x363  Serial Interface Data Register.
+#endif
+
+//----------------------------------------------------------------------------
+//       8185B TPPoll bits 				(offset 0xd9, 1 byte)
+//----------------------------------------------------------------------------
+#define TPPOLL_BQ			(0x01 << 7)
+#define TPPOLL_HPQ			(0x01 << 6)
+#define TPPOLL_AC_VOQ		(0x01 << 5)
+#define TPPOLL_AC_VIQ		(0x01 << 4)
+#define TPPOLL_AC_BEQ		(0x01 << 3)
+#define TPPOLL_AC_BKQ		(0x01 << 2)
+#define TPPOLL_AC_MGQ		(0x01 << 1)
+
+//----------------------------------------------------------------------------
+//       8185B TPPollStop bits 				(offset 0x93, 1 byte)
+//----------------------------------------------------------------------------
+#define TPPOLLSTOP_BQ			(0x01 << 7)
+#define TPPOLLSTOP_HPQ			(0x01 << 6)
+#define TPPOLLSTOP_AC_VOQ		(0x01 << 5)
+#define TPPOLLSTOP_AC_VIQ		(0x01 << 4)
+#define TPPOLLSTOP_AC_BEQ		(0x01 << 3)
+#define TPPOLLSTOP_AC_BKQ		(0x01 << 2)
+#define TPPOLLSTOP_AC_MGQ		(0x01 << 1)
+
+
+#define MSR_LINK_ENEDCA	   (1<<4)
+
+//----------------------------------------------------------------------------
+//       8187B AC_XX_PARAM bits
+//----------------------------------------------------------------------------
+#define AC_PARAM_TXOP_LIMIT_OFFSET		16
+#define AC_PARAM_ECW_MAX_OFFSET			12
+#define AC_PARAM_ECW_MIN_OFFSET			8
+#define AC_PARAM_AIFS_OFFSET			0
+
+//----------------------------------------------------------------------------
+//       8187B ACM_CONTROL bits						(Offset 0xBF, 1 Byte)
+//----------------------------------------------------------------------------
+#define VOQ_ACM_EN				(0x01 << 7) //BIT7
+#define VIQ_ACM_EN				(0x01 << 6) //BIT6
+#define BEQ_ACM_EN				(0x01 << 5) //BIT5
+#define ACM_HW_EN				(0x01 << 4) //BIT4
+#define TXOPSEL					(0x01 << 3) //BIT3
+#define VOQ_ACM_CTL				(0x01 << 2) //BIT2 // Set to 1 when AC_VO used time reaches or exceeds the admitted time
+#define VIQ_ACM_CTL				(0x01 << 1) //BIT1 // Set to 1 when AC_VI used time reaches or exceeds the admitted time
+#define BEQ_ACM_CTL				(0x01 << 0) //BIT0 // Set to 1 when AC_BE used time reaches or exceeds the admitted time
+
+
+//----------------------------------------------------------------------------
+//       8185B SW_3W_CMD bits					(Offset 0x27C-0x27D, 16bit)
+//----------------------------------------------------------------------------
+#define SW_3W_CMD0_HOLD		((1<< 7))
+#define SW_3W_CMD1_RE		 	((1<< 0)) // BIT8
+#define SW_3W_CMD1_WE		((1<< 1)) // BIT9
+#define SW_3W_CMD1_DONE		((1<< 2)) // BIT10
+
+#define BB_HOST_BANG_RW 	(1<<3)
+
+//----------------------------------------------------------------------------
+//       8185B RATE_FALLBACK_CTL bits				(Offset 0xBE, 8bit)
+//----------------------------------------------------------------------------
+#define RATE_FALLBACK_CTL_ENABLE				((1<< 7))
+#define RATE_FALLBACK_CTL_ENABLE_RTSCTS		((1<< 6))
+// Auto rate fallback per 2^n retry.
+#define RATE_FALLBACK_CTL_AUTO_STEP0	0x00
+#define RATE_FALLBACK_CTL_AUTO_STEP1	0x01
+#define RATE_FALLBACK_CTL_AUTO_STEP2	0x02
+#define RATE_FALLBACK_CTL_AUTO_STEP3	0x03
+
+
+#define RTL8225z2_ANAPARAM_OFF	0x55480658
+#define RTL8225z2_ANAPARAM2_OFF	0x72003f70
+//by amy for power save
+#define RF_CHANGE_BY_SW BIT31
+#define RF_CHANGE_BY_HW BIT30
+#define RF_CHANGE_BY_PS BIT29
+#define RF_CHANGE_BY_IPS BIT28
+//by amy for power save
+//by amy for antenna
+#define EEPROM_SW_REVD_OFFSET 0x3f
+// BIT[8-9] is for SW Antenna Diversity. Only the value EEPROM_SW_AD_ENABLE means enable, other values are diable.
+#define EEPROM_SW_AD_MASK			0x0300
+#define EEPROM_SW_AD_ENABLE			0x0100
+
+// BIT[10-11] determine if Antenna 1 is the Default Antenna. Only the value EEPROM_DEF_ANT_1 means TRUE, other values are FALSE.
+#define EEPROM_DEF_ANT_MASK			0x0C00
+#define EEPROM_DEF_ANT_1			0x0400
+//by amy for antenna
+//{by amy 080312
+//0x7C, 0x7D Crystal calibration and Tx Power tracking mechanism. Added by Roger. 2007.12.10.
+#define EEPROM_RSV						0x7C
+#define EEPROM_XTAL_CAL_MASK			0x00FF	// 0x7C[7:0], Crystal calibration mask.
+#define EEPROM_XTAL_CAL_XOUT_MASK	0x0F	// 0x7C[3:0], Crystal calibration for Xout.
+#define EEPROM_XTAL_CAL_XIN_MASK		0xF0	// 0x7C[7:4], Crystal calibration for Xin.
+#define EEPROM_THERMAL_METER_MASK	0x0F00	// 0x7D[3:0], Thermal meter reference level.
+#define EEPROM_XTAL_CAL_ENABLE		0x1000	// 0x7D[4], Crystal calibration enabled/disabled BIT.
+#define EEPROM_THERMAL_METER_ENABLE	0x2000	// 0x7D[5], Thermal meter enabled/disabled BIT.
+#define EEPROM_CID_RSVD1				0x3F
+#define EN_LPF_CAL			0x238	// Enable LPF Calibration.
+#define PWR_METER_EN		BIT1
+// <RJ_TODO_8185B> where are false alarm counters in 8185B?
+#define CCK_FALSE_ALARM		0xD0
+#define OFDM_FALSE_ALARM	0xD2
+//by amy 080312}
+
+//YJ,add for Country IE, 080630
+#define EEPROM_COUNTRY_CODE  0x2E
+//YJ,add,080630,end
+#endif
+
+#endif
diff --git a/drivers/staging/rtl8187se/r8180_max2820.c b/drivers/staging/rtl8187se/r8180_max2820.c
new file mode 100644
index 00000000000..cea08463d5e
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_max2820.c
@@ -0,0 +1,240 @@
+/*
+   This files contains MAXIM MAX2820 radio frontend programming routines.
+
+   This is part of rtl8180 OpenSource driver
+   Copyright (C) Andrea Merello 2004-2005  <andreamrl@tiscali.it>
+   Released under the terms of GPL (General Public Licence)
+
+   Parts of this driver are based on the GPL part of the
+   official realtek driver
+
+   Parts of this driver are based on the rtl8180 driver skeleton
+   from Patric Schenke & Andres Salomon
+
+   Parts of this driver are based on the Intel Pro Wireless 2100 GPL driver.
+
+   NetBSD rtl8180 driver from Dave Young has been really useful to
+   understand how to program the MAXIM radio. Thanks a lot!!!
+
+   'The Deuce' tested this and fixed some bugs.
+
+   Code from rtl8181 project has been useful to me to understand some things.
+
+   We want to tanks the Authors of such projects and the Ndiswrapper
+   project Authors.
+*/
+
+
+#include "r8180.h"
+#include "r8180_hw.h"
+#include "r8180_max2820.h"
+
+
+//#define DEBUG_MAXIM
+
+u32 maxim_chan[] = {
+	0,	//dummy channel 0
+	12, //1
+	17, //2
+	22, //3
+	27, //4
+	32, //5
+	37, //6
+	42, //7
+	47, //8
+	52, //9
+	57, //10
+	62, //11
+	67, //12
+	72, //13
+	84, //14
+};
+
+#if 0
+/* maxim expects 4 bit address MSF, then 12 bit data MSF*/
+void write_maxim(struct net_device *dev,u8 adr, u32 data)
+{
+
+	int shift;
+	short bit;
+	u16 word;
+
+	adr = adr &0xf;
+	word = (u16)data & 0xfff;
+	word |= (adr<<12);
+	/*write_nic_dword(dev,PHY_CONFIG,BB_HOST_BANG | BB_HOST_BANG_EN);
+	read_nic_dword(dev,PHY_CONFIG);
+	mdelay(1);
+
+	write_nic_dword(dev,PHY_CONFIG,BB_HOST_BANG | BB_HOST_BANG_EN | BB_HOST_BANG_CLK);
+	read_nic_dword(dev,PHY_CONFIG);
+	mdelay(1);
+	*/
+
+	/* MAX2820 will sample data on rising edge of clock */
+	for(shift = 15;shift >=0; shift--){
+		bit = word>>shift & 1;
+
+		write_nic_dword(dev,PHY_CONFIG,BB_HOST_BANG | (bit<<BB_HOST_BANG_DATA));
+
+		read_nic_dword(dev,PHY_CONFIG);
+		mdelay(2);
+
+		write_nic_dword(dev,PHY_CONFIG,BB_HOST_BANG |
+		(bit<<BB_HOST_BANG_DATA) | BB_HOST_BANG_CLK); /* sample data */
+
+		read_nic_dword(dev,PHY_CONFIG);
+		mdelay(1);
+
+		write_nic_dword(dev,PHY_CONFIG,BB_HOST_BANG |
+		(bit<<BB_HOST_BANG_DATA));
+
+		read_nic_dword(dev,PHY_CONFIG);
+		mdelay(2);
+
+	}
+	write_nic_dword(dev,PHY_CONFIG,BB_HOST_BANG | (bit<<BB_HOST_BANG_DATA)|
+					BB_HOST_BANG_EN);
+	read_nic_dword(dev,PHY_CONFIG);
+	mdelay(2);
+
+	/* The shift register fill flush to the requested register the
+	 * last 12 bits data shifted in
+	 */
+	write_nic_dword(dev,PHY_CONFIG,BB_HOST_BANG | (bit<<BB_HOST_BANG_DATA)|
+					BB_HOST_BANG_EN | BB_HOST_BANG_CLK);
+	read_nic_dword(dev,PHY_CONFIG);
+	mdelay(2);
+
+	write_nic_dword(dev,PHY_CONFIG,BB_HOST_BANG | (bit<<BB_HOST_BANG_DATA)|
+					BB_HOST_BANG_EN);
+	read_nic_dword(dev,PHY_CONFIG);
+	mdelay(2);
+
+
+#ifdef DEBUG_MAXIM
+	DMESG("Writing maxim: %x (adr %x)",phy_config,adr);
+#endif
+
+}
+#endif
+
+void write_maxim(struct net_device *dev,u8 adr, u32 data) {
+	u32 temp;
+	temp =  0x90 + (data & 0xf);
+	temp <<= 16;
+	temp += adr;
+	temp <<= 8;
+	temp += (data >> 4) & 0xff;
+#ifdef DEBUG_MAXIM
+	DMESG("write_maxim: %08x", temp);
+#endif
+	write_nic_dword(dev, PHY_CONFIG, temp);
+	force_pci_posting(dev);
+	mdelay(1);
+}
+
+
+void maxim_write_phy_antenna(struct net_device *dev,short ch)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	u8 ant;
+
+	ant = MAXIM_ANTENNA;
+	if(priv->antb) /*default antenna is antenna B */
+		ant |= BB_ANTENNA_B;
+	if(ch == 14)
+		ant |= BB_ANTATTEN_CHAN14;
+	write_phy(dev,0x10,ant);
+	//DMESG("BB antenna %x ",ant);
+}
+
+
+void maxim_rf_set_chan(struct net_device *dev, short ch)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	u32 txpw = 0xff & priv->chtxpwr[ch];
+	u32 chan = maxim_chan[ch];
+
+	/*While philips SA2400 drive the PA bias
+	 *seems that for MAXIM we delegate this
+	 *to the BB
+	 */
+
+	//write_maxim(dev,5,txpw);
+	write_phy(dev,3,txpw);
+
+	maxim_write_phy_antenna(dev,ch);
+	write_maxim(dev,3,chan);
+}
+
+
+void maxim_rf_close(struct net_device *dev)
+{
+	write_phy(dev, 3, 0x8);
+	write_maxim(dev, 1, 0);
+}
+
+
+void maxim_rf_init(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	u32 anaparam;
+
+	write_nic_byte(dev,PHY_DELAY,0x6);	//this is general
+	write_nic_byte(dev,CARRIER_SENSE_COUNTER,0x4c); //this is general
+
+	/*these are maxim specific*/
+	anaparam = read_nic_dword(dev,ANAPARAM);
+	anaparam = anaparam &~ (ANAPARAM_TXDACOFF_SHIFT);
+	anaparam = anaparam &~ANAPARAM_PWR1_MASK;
+	anaparam = anaparam &~ANAPARAM_PWR0_MASK;
+	anaparam |= (MAXIM_ANAPARAM_PWR1_ON<<ANAPARAM_PWR1_SHIFT);
+	anaparam |= (MAXIM_ANAPARAM_PWR0_ON<<ANAPARAM_PWR0_SHIFT);
+
+	//rtl8180_set_anaparam(dev,anaparam);
+
+	/* MAXIM from netbsd driver */
+
+	write_maxim(dev,0, 7); /* test mode as indicated in datasheet*/
+	write_maxim(dev,1, 0x1e); /* enable register*/
+	write_maxim(dev,2, 1); /* synt register */
+
+
+	maxim_rf_set_chan(dev,priv->chan);
+
+	write_maxim(dev,4, 0x313); /* rx register*/
+
+	/* PA is driven directly by the BB, we keep the MAXIM bias
+	 * at the highest value in the boubt tha pleacing it to lower
+	 * values may introduce some further attenuation somewhere..
+	 */
+
+	write_maxim(dev,5, 0xf);
+
+
+	/*baseband configuration*/
+	write_phy(dev,0,0x88); //sys1
+	write_phy(dev,3,0x8); //txagc
+	write_phy(dev,4,0xf8); // lnadet
+	write_phy(dev,5,0x90); // ifagcinit
+	write_phy(dev,6,0x1a); // ifagclimit
+	write_phy(dev,7,0x64); // ifagcdet
+
+	/*Should be done something more here??*/
+
+	maxim_write_phy_antenna(dev,priv->chan);
+
+	write_phy(dev,0x11,0x88); //trl
+	if(priv->diversity)
+		write_phy(dev,0x12,0xc7);
+	else
+		write_phy(dev,0x12,0x47);
+
+	write_phy(dev,0x13,0x9b);
+
+	write_phy(dev,0x19,0x0); //CHESTLIM
+	write_phy(dev,0x1a,0x9f); //CHSQLIM
+
+	maxim_rf_set_chan(dev,priv->chan);
+}
diff --git a/drivers/staging/rtl8187se/r8180_max2820.h b/drivers/staging/rtl8187se/r8180_max2820.h
new file mode 100644
index 00000000000..5d4fb550484
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_max2820.h
@@ -0,0 +1,21 @@
+/*
+	This is part of rtl8180 OpenSource driver
+	Copyright (C) Andrea Merello 2004-2005  <andreamrl@tiscali.it>
+	Released under the terms of GPL (General Public Licence)
+
+	Parts of this driver are based on the GPL part of the official realtek driver
+	Parts of this driver are based on the rtl8180 driver skeleton from Patric Schenke & Andres Salomon
+	Parts of this driver are based on the Intel Pro Wireless 2100 GPL driver
+
+	We want to tanks the Authors of such projects and the Ndiswrapper project Authors.
+*/
+
+#define MAXIM_ANTENNA 0xb3
+#define MAXIM_ANAPARAM_PWR1_ON 0x8
+#define MAXIM_ANAPARAM_PWR0_ON 0x0
+
+
+void maxim_rf_init(struct net_device *dev);
+void maxim_rf_set_chan(struct net_device *dev,short ch);
+
+void maxim_rf_close(struct net_device *dev);
diff --git a/drivers/staging/rtl8187se/r8180_pm.c b/drivers/staging/rtl8187se/r8180_pm.c
new file mode 100644
index 00000000000..3851b936835
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_pm.c
@@ -0,0 +1,90 @@
+/*
+   Power management interface routines.
+   Written by Mariusz Matuszek.
+   This code is currently just a placeholder for later work and
+   does not do anything useful.
+
+   This is part of rtl8180 OpenSource driver.
+   Copyright (C) Andrea Merello 2004  <andreamrl@tiscali.it>
+   Released under the terms of GPL (General Public Licence)
+*/
+
+#ifdef CONFIG_RTL8180_PM
+
+
+#include "r8180_hw.h"
+#include "r8180_pm.h"
+#include "r8180.h"
+
+int rtl8180_save_state (struct pci_dev *dev, u32 state)
+{
+        printk(KERN_NOTICE "r8180 save state call (state %u).\n", state);
+	return(-EAGAIN);
+}
+
+int rtl8180_suspend (struct pci_dev *pdev, pm_message_t state)
+{
+	struct net_device *dev = pci_get_drvdata(pdev);
+//	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	if (!netif_running(dev))
+		goto out_pci_suspend;
+
+	dev->stop(dev);
+
+	netif_device_detach(dev);
+
+out_pci_suspend:
+	pci_save_state(pdev);
+	pci_disable_device(pdev);
+	pci_set_power_state(pdev,pci_choose_state(pdev,state));
+	return 0;
+}
+
+int rtl8180_resume (struct pci_dev *pdev)
+{
+	struct net_device *dev = pci_get_drvdata(pdev);
+//	struct r8180_priv *priv = ieee80211_priv(dev);
+	int err;
+	u32 val;
+
+	pci_set_power_state(pdev, PCI_D0);
+
+	err = pci_enable_device(pdev);
+	if(err) {
+		printk(KERN_ERR "%s: pci_enable_device failed on resume\n",
+				dev->name);
+
+		return err;
+	}
+	pci_restore_state(pdev);
+	/*
+	 * Suspend/Resume resets the PCI configuration space, so we have to
+	 * re-disable the RETRY_TIMEOUT register (0x41) to keep PCI Tx retries
+	 * from interfering with C3 CPU state. pci_restore_state won't help
+	 * here since it only restores the first 64 bytes pci config header.
+	 */
+	pci_read_config_dword(pdev, 0x40, &val);
+	if ((val & 0x0000ff00) != 0)
+		pci_write_config_dword(pdev, 0x40, val & 0xffff00ff);
+
+	if(!netif_running(dev))
+		goto out;
+
+	dev->open(dev);
+	netif_device_attach(dev);
+out:
+	return 0;
+}
+
+
+int rtl8180_enable_wake (struct pci_dev *dev, u32 state, int enable)
+{
+        printk(KERN_NOTICE "r8180 enable wake call (state %u, enable %d).\n",
+	       state, enable);
+	return(-EAGAIN);
+}
+
+
+
+#endif //CONFIG_RTL8180_PM
diff --git a/drivers/staging/rtl8187se/r8180_pm.h b/drivers/staging/rtl8187se/r8180_pm.h
new file mode 100644
index 00000000000..7958b3a734d
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_pm.h
@@ -0,0 +1,28 @@
+/*
+        Power management interface routines.
+	Written by Mariusz Matuszek.
+	This code is currently just a placeholder for later work and
+	does not do anything useful.
+
+	This is part of rtl8180 OpenSource driver.
+	Copyright (C) Andrea Merello 2004  <andreamrl@tiscali.it>
+	Released under the terms of GPL (General Public Licence)
+
+*/
+
+#ifdef CONFIG_RTL8180_PM
+
+#ifndef R8180_PM_H
+#define R8180_PM_H
+
+#include <linux/types.h>
+#include <linux/pci.h>
+
+int rtl8180_save_state (struct pci_dev *dev, u32 state);
+int rtl8180_suspend (struct pci_dev *pdev, pm_message_t state);
+int rtl8180_resume (struct pci_dev *pdev);
+int rtl8180_enable_wake (struct pci_dev *dev, u32 state, int enable);
+
+#endif //R8180_PM_H
+
+#endif // CONFIG_RTL8180_PM
diff --git a/drivers/staging/rtl8187se/r8180_rtl8225.c b/drivers/staging/rtl8187se/r8180_rtl8225.c
new file mode 100644
index 00000000000..96ed029ed64
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_rtl8225.c
@@ -0,0 +1,933 @@
+/*
+  This is part of the rtl8180-sa2400 driver
+  released under the GPL (See file COPYING for details).
+  Copyright (c) 2005 Andrea Merello <andreamrl@tiscali.it>
+
+  This files contains programming code for the rtl8225
+  radio frontend.
+
+  *Many* thanks to Realtek Corp. for their great support!
+
+*/
+
+
+
+#include "r8180_hw.h"
+#include "r8180_rtl8225.h"
+
+
+u8 rtl8225_gain[]={
+	0x23,0x88,0x7c,0xa5,// -82dbm
+	0x23,0x88,0x7c,0xb5,// -82dbm
+	0x23,0x88,0x7c,0xc5,// -82dbm
+	0x33,0x80,0x79,0xc5,// -78dbm
+	0x43,0x78,0x76,0xc5,// -74dbm
+	0x53,0x60,0x73,0xc5,// -70dbm
+	0x63,0x58,0x70,0xc5,// -66dbm
+};
+
+#if 0
+u8 rtl8225_init_gain[]={
+	//0x00,0x00,0x00,0x00,//0x00,0x00,0x00,0x00,
+	0x33,0x80,0x6c,0xc5,//0x00,0x49,0x06,0xb5,//Gain = 0 ~ -78dbm
+	0x43,0x78,0x69,0xc5,//0x00,0x45,0x06,0xb1,//Gain = 1 ~ -74dbm
+	0x53,0x60,0x66,0xc5,//0x00,0x41,0x06,0xab,//Gain = 2 ~ -70dbm
+	0x63,0x58,0x63,0xc5,//0x00,0x3d,0x06,0xa5,//Gain = 3 ~ -66dbm
+	0x73,0x50,0x62,0xc5,//0x00,0x39,0x06,0xa1,//Gain = 4 ~ -62dbm
+	0x83,0x43,0x61,0xc5,//0x00,0x35,0x06,0x9b,//Gain = 5 ~ -58dbm
+	0x93,0x38,0x5a,0xc5,//0x00,0x31,0x06,0x99,//Gain = 6 ~ -54dbm
+};
+#endif
+#ifdef CONFIG_RTL818X_S
+u32 rtl8225_chan[] ={
+              0,
+		0x0080, //ch1
+		0x0100, //ch2
+		0x0180, //ch3
+		0x0200, //ch4
+		0x0280,
+		0x0300,
+		0x0380,
+		0x0400,
+		0x0480,
+		0x0500,
+		0x0580,
+		0x0600,
+		0x0680,
+		0x074A, //ch14
+};
+#else
+u32 rtl8225_chan[] = {
+	0,	//dummy channel 0
+	0x085c, //1
+	0x08dc, //2
+	0x095c, //3
+	0x09dc, //4
+	0x0a5c, //5
+	0x0adc, //6
+	0x0b5c, //7
+	0x0bdc, //8
+	0x0c5c, //9
+	0x0cdc, //10
+	0x0d5c, //11
+	0x0ddc, //12
+	0x0e5c, //13
+	//0x0f5c, //14
+	0x0f72, // 14
+};
+#endif
+
+u16 rtl8225bcd_rxgain[]={
+	0x0400, 0x0401, 0x0402, 0x0403, 0x0404, 0x0405, 0x0408, 0x0409,
+	0x040a, 0x040b, 0x0502, 0x0503, 0x0504, 0x0505, 0x0540, 0x0541,
+	0x0542, 0x0543, 0x0544, 0x0545, 0x0580, 0x0581, 0x0582, 0x0583,
+	0x0584, 0x0585, 0x0588, 0x0589, 0x058a, 0x058b, 0x0643, 0x0644,
+	0x0645, 0x0680, 0x0681, 0x0682, 0x0683, 0x0684, 0x0685, 0x0688,
+	0x0689, 0x068a, 0x068b, 0x068c, 0x0742, 0x0743, 0x0744, 0x0745,
+	0x0780, 0x0781, 0x0782, 0x0783, 0x0784, 0x0785, 0x0788, 0x0789,
+	0x078a, 0x078b, 0x078c, 0x078d, 0x0790, 0x0791, 0x0792, 0x0793,
+	0x0794, 0x0795, 0x0798, 0x0799, 0x079a, 0x079b, 0x079c, 0x079d,
+	0x07a0, 0x07a1, 0x07a2, 0x07a3, 0x07a4, 0x07a5, 0x07a8, 0x07a9,
+	0x07aa, 0x07ab, 0x07ac, 0x07ad, 0x07b0, 0x07b1, 0x07b2, 0x07b3,
+	0x07b4, 0x07b5, 0x07b8, 0x07b9, 0x07ba, 0x07bb, 0x07bb
+
+};
+
+
+#if 0
+u16 rtl8225bc_rxgain[]={
+	0x0400, 0x0401, 0x0402, 0x0403, 0x0404, 0x0405, 0x0408, 0x0409,
+	0x040a, 0x040b, 0x0502, 0x0503, 0x0504, 0x0505, 0x0540, 0x0541,
+	0x0542, 0x0543, 0x0544, 0x0545, 0x0580, 0x0581, 0x0582, 0x0583,
+	0x0584, 0x0585, 0x0588, 0x0589, 0x058a, 0x058b, 0x0643, 0x0644,
+	0x0645, 0x0680, 0x0681, 0x0682, 0x0683, 0x0684, 0x0685, 0x0688,
+	0x0689, 0x068a, 0x068b, 0x068c, 0x0742, 0x0743, 0x0744, 0x0745,
+	0x0780, 0x0781, 0x0782, 0x0783, 0x0784, 0x0785, 0x0788, 0x0789,
+	0x078a, 0x078b, 0x078c, 0x078d, 0x0790, 0x0791, 0x0792, 0x0793,
+	0x0794, 0x0795, 0x0798, 0x0799, 0x039a, 0x039b, 0x039c, 0x039d,
+	0x03a0, 0x03a1, 0x03a2, 0x03a3, 0x03a4, 0x03a5, 0x03a8, 0x03a9,
+	0x03aa, 0x03ab, 0x03ac, 0x03ad, 0x03b0, 0x03b1, 0x03b2, 0x03b3,
+	0x03b4, 0x03b5, 0x03b8, 0x03b9, 0x03ba, 0x03bb, 0x03bb
+
+};
+
+
+u16 rtl8225a_rxgain[]={
+	0x0400, 0x0401, 0x0402, 0x0403, 0x0404, 0x0405, 0x0408, 0x0409,
+	0x040a, 0x040b, 0x0502, 0x0503, 0x0504, 0x0505, 0x0540, 0x0541,
+	0x0542, 0x0543, 0x0544, 0x0545, 0x0580, 0x0581, 0x0582, 0x0583,
+	0x0584, 0x0585, 0x0588, 0x0589, 0x058a, 0x058b, 0x0643, 0x0644,
+	0x0645, 0x0680, 0x0681, 0x0682, 0x0683, 0x0684, 0x0685, 0x0688,
+	0x0689, 0x068a, 0x068b, 0x068c, 0x0742, 0x0743, 0x0744, 0x0745,
+	0x0780, 0x0781, 0x0782, 0x0783, 0x0784, 0x0785, 0x0788, 0x0789,
+	0x078a, 0x078b, 0x078c, 0x078d, 0x0790, 0x0791, 0x0792, 0x0793,
+	0x0794, 0x0795, 0x0798, 0x0799, 0x079a, 0x079b, 0x079c, 0x079d,
+	0x07a0, 0x07a1, 0x07a2, 0x07a3, 0x07a4, 0x07a5, 0x07a8, 0x07a9,
+	0x07aa, 0x07ab, 0x07ac, 0x07ad, 0x07ad, 0x07ad, 0x07ad, 0x07ad,
+	0x07ad, 0x07ad, 0x07ad, 0x07ad, 0x07ad, 0x07ad, 0x07ad
+};
+#endif
+
+u8 rtl8225_agc[]={
+	0x9e,0x9e,0x9e,0x9e,0x9e,0x9e,0x9e,0x9e,0x9d,0x9c,0x9b,0x9a,0x99,0x98,0x97,0x96,
+	0x95,0x94,0x93,0x92,0x91,0x90,0x8f,0x8e,0x8d,0x8c,0x8b,0x8a,0x89,0x88,0x87,0x86,
+	0x85,0x84,0x83,0x82,0x81,0x80,0x3f,0x3e,0x3d,0x3c,0x3b,0x3a,0x39,0x38,0x37,0x36,
+	0x35,0x34,0x33,0x32,0x31,0x30,0x2f,0x2e,0x2d,0x2c,0x2b,0x2a,0x29,0x28,0x27,0x26,
+	0x25,0x24,0x23,0x22,0x21,0x20,0x1f,0x1e,0x1d,0x1c,0x1b,0x1a,0x19,0x18,0x17,0x16,
+	0x15,0x14,0x13,0x12,0x11,0x10,0x0f,0x0e,0x0d,0x0c,0x0b,0x0a,0x09,0x08,0x07,0x06,
+	0x05,0x04,0x03,0x02,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,
+	0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,
+};
+
+
+u8 rtl8225_tx_gain_cck_ofdm[]={
+	0x02,0x06,0x0e,0x1e,0x3e,0x7e
+};
+
+
+u8 rtl8225_tx_power_ofdm[]={
+	0x80,0x90,0xa2,0xb5,0xcb,0xe4
+};
+
+
+u8 rtl8225_tx_power_cck_ch14[]={
+	0x18,0x17,0x15,0x0c,0x00,0x00,0x00,0x00,
+	0x1b,0x1a,0x17,0x0e,0x00,0x00,0x00,0x00,
+	0x1f,0x1e,0x1a,0x0f,0x00,0x00,0x00,0x00,
+	0x22,0x21,0x1d,0x11,0x00,0x00,0x00,0x00,
+	0x26,0x25,0x21,0x13,0x00,0x00,0x00,0x00,
+	0x2b,0x2a,0x25,0x15,0x00,0x00,0x00,0x00
+};
+
+
+u8 rtl8225_tx_power_cck[]={
+	0x18,0x17,0x15,0x11,0x0c,0x08,0x04,0x02,
+	0x1b,0x1a,0x17,0x13,0x0e,0x09,0x04,0x02,
+	0x1f,0x1e,0x1a,0x15,0x10,0x0a,0x05,0x02,
+	0x22,0x21,0x1d,0x18,0x11,0x0b,0x06,0x02,
+	0x26,0x25,0x21,0x1b,0x14,0x0d,0x06,0x03,
+	0x2b,0x2a,0x25,0x1e,0x16,0x0e,0x07,0x03
+};
+
+
+void rtl8225_set_gain(struct net_device *dev, short gain)
+{
+	write_phy_ofdm(dev, 0x0d, rtl8225_gain[gain * 4]);
+	write_phy_ofdm(dev, 0x23, rtl8225_gain[gain * 4 + 1]);
+	write_phy_ofdm(dev, 0x1b, rtl8225_gain[gain * 4 + 2]);
+	write_phy_ofdm(dev, 0x1d, rtl8225_gain[gain * 4 + 3]);
+}
+#if 0
+
+void rtl8225_set_gain(struct net_device *dev, short gain)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	rtl8180_set_anaparam(dev, RTL8225_ANAPARAM_ON);
+
+	if(priv->card_8185 == 2)
+		write_phy_ofdm(dev, 0x21, 0x27);
+	else
+		write_phy_ofdm(dev, 0x21, 0x37);
+
+	write_phy_ofdm(dev, 0x25, 0x20);
+	write_phy_ofdm(dev, 0x11, 0x6);
+
+	if(priv->card_8185 == 1 && priv->card_8185_Bversion)
+		write_phy_ofdm(dev, 0x27, 0x8);
+	else
+		write_phy_ofdm(dev, 0x27, 0x88);
+
+	write_phy_ofdm(dev, 0x14, 0);
+	write_phy_ofdm(dev, 0x16, 0);
+	write_phy_ofdm(dev, 0x15, 0x40);
+	write_phy_ofdm(dev, 0x17, 0x40);
+
+	write_phy_ofdm(dev, 0x0d, rtl8225_gain[gain * 4]);
+	write_phy_ofdm(dev, 0x23, rtl8225_gain[gain * 4 + 1]);
+	write_phy_ofdm(dev, 0x1b, rtl8225_gain[gain * 4 + 2]);
+	write_phy_ofdm(dev, 0x1d, rtl8225_gain[gain * 4 + 3]);
+	//rtl8225_set_gain_usb(dev, gain);
+}
+#endif
+
+
+void write_rtl8225(struct net_device *dev, u8 adr, u16 data)
+{
+	int i;
+	u16 out,select;
+	u8 bit;
+	u32 bangdata = (data << 4) | (adr & 0xf);
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	out = read_nic_word(dev, RFPinsOutput) & 0xfff3;
+
+	write_nic_word(dev,RFPinsEnable,
+		(read_nic_word(dev,RFPinsEnable) | 0x7));
+
+	select = read_nic_word(dev, RFPinsSelect);
+
+	write_nic_word(dev, RFPinsSelect, select | 0x7 |
+		((priv->card_type == USB) ? 0 : SW_CONTROL_GPIO));
+
+	force_pci_posting(dev);
+	udelay(10);
+
+	write_nic_word(dev, RFPinsOutput, out | BB_HOST_BANG_EN );//| 0x1fff);
+
+	force_pci_posting(dev);
+	udelay(2);
+
+	write_nic_word(dev, RFPinsOutput, out);
+
+	force_pci_posting(dev);
+	udelay(10);
+
+
+	for(i=15; i>=0;i--){
+
+		bit = (bangdata & (1<<i)) >> i;
+
+		write_nic_word(dev, RFPinsOutput, bit | out);
+
+		write_nic_word(dev, RFPinsOutput, bit | out | BB_HOST_BANG_CLK);
+		write_nic_word(dev, RFPinsOutput, bit | out | BB_HOST_BANG_CLK);
+
+		i--;
+		bit = (bangdata & (1<<i)) >> i;
+
+		write_nic_word(dev, RFPinsOutput, bit | out | BB_HOST_BANG_CLK);
+		write_nic_word(dev, RFPinsOutput, bit | out | BB_HOST_BANG_CLK);
+
+		write_nic_word(dev, RFPinsOutput, bit | out);
+
+	}
+
+	write_nic_word(dev, RFPinsOutput, out | BB_HOST_BANG_EN);
+
+	force_pci_posting(dev);
+	udelay(10);
+
+	write_nic_word(dev, RFPinsOutput, out |
+		((priv->card_type == USB) ? 4 : BB_HOST_BANG_EN));
+
+	write_nic_word(dev, RFPinsSelect, select |
+		((priv->card_type == USB) ? 0 : SW_CONTROL_GPIO));
+
+	if(priv->card_type == USB)
+		mdelay(2);
+	else
+		rtl8185_rf_pins_enable(dev);
+}
+
+void rtl8225_rf_close(struct net_device *dev)
+{
+	write_rtl8225(dev, 0x4, 0x1f);
+
+	force_pci_posting(dev);
+	mdelay(1);
+
+	rtl8180_set_anaparam(dev, RTL8225_ANAPARAM_OFF);
+	rtl8185_set_anaparam2(dev, RTL8225_ANAPARAM2_OFF);
+}
+
+void rtl8225_SetTXPowerLevel(struct net_device *dev, short ch)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	int GainIdx;
+	int GainSetting;
+	int i;
+	u8 power;
+	u8 *cck_power_table;
+	u8 max_cck_power_level;
+	u8 max_ofdm_power_level;
+	u8 min_ofdm_power_level;
+	u8 cck_power_level = 0xff & priv->chtxpwr[ch];
+	u8 ofdm_power_level = 0xff & priv->chtxpwr_ofdm[ch];
+
+	if(priv->card_type == USB){
+		max_cck_power_level = 11;
+		max_ofdm_power_level = 25; //  12 -> 25
+		min_ofdm_power_level = 10;
+	}else{
+		max_cck_power_level = 35;
+		max_ofdm_power_level = 35;
+		min_ofdm_power_level = 0;
+	}
+	/* CCK power setting */
+	if(cck_power_level > max_cck_power_level)
+		cck_power_level = max_cck_power_level;
+	GainIdx=cck_power_level % 6;
+	GainSetting=cck_power_level / 6;
+
+	if(ch == 14)
+		cck_power_table = rtl8225_tx_power_cck_ch14;
+	else
+		cck_power_table = rtl8225_tx_power_cck;
+
+//	if(priv->card_8185 == 1 && priv->card_8185_Bversion ){
+		/*Ver B*/
+//		write_nic_byte(dev, TX_GAIN_CCK, rtl8225_tx_gain_cck_ofdm[GainSetting]);
+//	}else{
+		/*Ver C - D */
+	write_nic_byte(dev, TX_GAIN_CCK, rtl8225_tx_gain_cck_ofdm[GainSetting]>>1);
+//	}
+
+	for(i=0;i<8;i++){
+
+		power = cck_power_table[GainIdx * 8 + i];
+		write_phy_cck(dev, 0x44 + i, power);
+	}
+
+	/* FIXME Is this delay really needeed ? */
+	force_pci_posting(dev);
+	mdelay(1);
+
+	/* OFDM power setting */
+//  Old:
+//	if(ofdm_power_level > max_ofdm_power_level)
+//		ofdm_power_level = 35;
+//	ofdm_power_level += min_ofdm_power_level;
+//  Latest:
+	if(ofdm_power_level > (max_ofdm_power_level - min_ofdm_power_level))
+		ofdm_power_level = max_ofdm_power_level;
+	else
+		ofdm_power_level += min_ofdm_power_level;
+	if(ofdm_power_level > 35)
+		ofdm_power_level = 35;
+//
+
+	GainIdx=ofdm_power_level % 6;
+	GainSetting=ofdm_power_level / 6;
+#if 1
+//	if(priv->card_type == USB){
+		rtl8185_set_anaparam2(dev,RTL8225_ANAPARAM2_ON);
+
+		write_phy_ofdm(dev,2,0x42);
+		write_phy_ofdm(dev,6,0);
+		write_phy_ofdm(dev,8,0);
+//	}
+#endif
+//	if(priv->card_8185 == 1 && priv->card_8185_Bversion){
+//		/*Ver B*/
+//		write_nic_byte(dev, TX_GAIN_OFDM, rtl8225_tx_gain_cck_ofdm[GainSetting]);
+//	}else{
+		/*Ver C - D */
+	write_nic_byte(dev, TX_GAIN_OFDM, rtl8225_tx_gain_cck_ofdm[GainSetting]>>1);
+//	}
+
+
+	power = rtl8225_tx_power_ofdm[GainIdx];
+
+	write_phy_ofdm(dev, 0x5, power);
+	write_phy_ofdm(dev, 0x7, power);
+
+	force_pci_posting(dev);
+	mdelay(1);
+	//write_nic_byte(dev, TX_AGC_CONTROL,4);
+}
+#if 0
+/* switch between mode B and G */
+void rtl8225_set_mode(struct net_device *dev, short modeb)
+{
+	write_phy_ofdm(dev, 0x15, (modeb ? 0x0 : 0x40));
+	write_phy_ofdm(dev, 0x17, (modeb ? 0x0 : 0x40));
+}
+#endif
+void rtl8225_rf_set_chan(struct net_device *dev, short ch)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	short gset = (priv->ieee80211->state == IEEE80211_LINKED &&
+		ieee80211_is_54g(priv->ieee80211->current_network)) ||
+		priv->ieee80211->iw_mode == IW_MODE_MONITOR;
+
+	rtl8225_SetTXPowerLevel(dev, ch);
+
+	write_rtl8225(dev, 0x7, rtl8225_chan[ch]);
+
+	force_pci_posting(dev);
+	mdelay(10);
+
+	// A mode sifs 0x44, difs 34-14, slot 9, eifs 23, cwm 3, cwM 7, ctstoself 0x10
+	if(gset){
+		write_nic_byte(dev,SIFS,0x22);// SIFS: 0x22
+		write_nic_byte(dev,DIFS,0x14); //DIFS: 20
+		//write_nic_byte(dev,DIFS,20); //DIFS: 20
+	}else{
+		write_nic_byte(dev,SIFS,0x44);// SIFS: 0x22
+		write_nic_byte(dev,DIFS,50 - 14); //DIFS: 36
+	}
+	if(priv->ieee80211->state == IEEE80211_LINKED &&
+		ieee80211_is_shortslot(priv->ieee80211->current_network))
+		write_nic_byte(dev,SLOT,0x9); //SLOT: 9
+
+	else
+		write_nic_byte(dev,SLOT,0x14); //SLOT: 20 (0x14)
+
+
+	if(gset){
+		write_nic_byte(dev,EIFS,81);//91 - 20); // EIFS: 91 (0x5B)
+		write_nic_byte(dev,CW_VAL,0x73); //CW VALUE: 0x37
+		//DMESG("using G net params");
+	}else{
+		write_nic_byte(dev,EIFS,81); // EIFS: 91 (0x5B)
+		write_nic_byte(dev,CW_VAL,0xa5); //CW VALUE: 0x37
+		//DMESG("using B net params");
+	}
+
+
+}
+
+void rtl8225_host_pci_init(struct net_device *dev)
+{
+	write_nic_word(dev, RFPinsOutput, 0x480);
+
+	rtl8185_rf_pins_enable(dev);
+
+	//if(priv->card_8185 == 2 && priv->enable_gpio0 ) /* version D */
+	//write_nic_word(dev, RFPinsSelect, 0x88);
+	//else
+	write_nic_word(dev, RFPinsSelect, 0x88 | SW_CONTROL_GPIO); /* 0x488 | SW_CONTROL_GPIO */
+
+	write_nic_byte(dev, GP_ENABLE, 0);
+
+	force_pci_posting(dev);
+	mdelay(200);
+
+	write_nic_word(dev, GP_ENABLE, 0xff & (~(1<<6))); /* bit 6 is for RF on/off detection */
+
+
+}
+
+void rtl8225_host_usb_init(struct net_device *dev)
+{
+	#if 0
+	write_nic_byte(dev,RFPinsSelect+1,0);
+
+	write_nic_byte(dev,GPIO,0);
+
+	write_nic_byte_E(dev,0x53,read_nic_byte_E(dev,0x53) | (1<<7));
+
+	write_nic_byte(dev,RFPinsSelect+1,4);
+
+	write_nic_byte(dev,GPIO,0x20);
+
+	write_nic_byte(dev,GP_ENABLE,0);
+
+
+	/* Config BB & RF */
+	write_nic_word(dev, RFPinsOutput, 0x80);
+
+	write_nic_word(dev, RFPinsSelect, 0x80);
+
+	write_nic_word(dev, RFPinsEnable, 0x80);
+
+
+	mdelay(100);
+
+	mdelay(1000);
+#endif
+
+}
+
+void rtl8225_rf_sleep(struct net_device *dev)
+{
+	write_rtl8225(dev,0x4,0xdff);
+	force_pci_posting(dev);
+	mdelay(1);
+	rtl8180_set_anaparam(dev, RTL8225_ANAPARAM_SLEEP);
+	rtl8185_set_anaparam2(dev, RTL8225_ANAPARAM2_SLEEP);
+	force_pci_posting(dev);
+}
+
+void rtl8225_rf_wakeup(struct net_device *dev)
+{
+	write_rtl8225(dev,0x4,0x9ff);
+	rtl8180_set_anaparam(dev, RTL8225_ANAPARAM_ON);
+	rtl8185_set_anaparam2(dev, RTL8225_ANAPARAM2_ON);
+	force_pci_posting(dev);
+}
+
+void rtl8225_rf_init(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	int i;
+	short channel = 1;
+	u16 brsr;
+
+	priv->chan = channel;
+
+	rtl8180_set_anaparam(dev, RTL8225_ANAPARAM_ON);
+
+
+	if(priv->card_type == USB)
+		rtl8225_host_usb_init(dev);
+	else
+		rtl8225_host_pci_init(dev);
+
+	write_nic_dword(dev, RF_TIMING, 0x000a8008);
+
+	brsr = read_nic_word(dev, BRSR);
+
+	write_nic_word(dev, BRSR, 0xffff);
+
+	#if 0
+	if(priv->card_8185 == 1){/* version C or B */
+		if(priv->card_8185_Bversion)  /* version B*/
+			write_nic_dword(dev, RF_PARA, 0x44);
+		else    /* version C */
+			write_nic_dword(dev, RF_PARA, 0x100044);
+	}else{ /* version D */
+		if(priv->enable_gpio0)
+			write_nic_dword(dev, RF_PARA, 0x20100044);
+		else /* also USB */
+			write_nic_dword(dev, RF_PARA, 0x100044);
+	}
+	#endif
+
+	write_nic_dword(dev, RF_PARA, 0x100044);
+
+	#if 1  //0->1
+	rtl8180_set_mode(dev, EPROM_CMD_CONFIG);
+	write_nic_byte(dev, CONFIG3, 0x44);
+	rtl8180_set_mode(dev, EPROM_CMD_NORMAL);
+	#endif
+
+	if(priv->card_type == USB){
+		rtl8185_rf_pins_enable(dev);
+
+		mdelay(1000);
+	}
+
+	write_rtl8225(dev, 0x0, 0x67); mdelay(1);
+
+
+	write_rtl8225(dev, 0x1, 0xfe0); mdelay(1);
+
+	write_rtl8225(dev, 0x2, 0x44d); mdelay(1);
+
+	write_rtl8225(dev, 0x3, 0x441); mdelay(1);
+
+	if(priv->card_type == USB)
+		write_rtl8225(dev, 0x4, 0x486);
+	else
+		write_rtl8225(dev, 0x4, 0x8be);
+
+	mdelay(1);
+
+
+	#if 0
+	}else if(priv->phy_ver == 1){
+		/* version A */
+		write_rtl8225(dev, 0x5, 0xbc0 + 2);
+	}else{
+	#endif
+	/* version B & C */
+
+	if(priv->card_type == USB)
+		write_rtl8225(dev, 0x5, 0xbc0);
+	else if(priv->card_type == MINIPCI)
+		write_rtl8225(dev, 0x5, 0xbc0 + 3 +(6<<3));
+	else
+		write_rtl8225(dev, 0x5, 0xbc0 + (6<<3));
+
+	 mdelay(1);
+//	}
+
+	write_rtl8225(dev, 0x6, 0xae6);  mdelay(1);
+
+	write_rtl8225(dev, 0x7, ((priv->card_type == USB)? 0x82a : rtl8225_chan[channel]));  mdelay(1);
+
+	write_rtl8225(dev, 0x8, 0x1f);  mdelay(1);
+
+	write_rtl8225(dev, 0x9, 0x334);  mdelay(1);
+
+	write_rtl8225(dev, 0xa, 0xfd4);  mdelay(1);
+
+	write_rtl8225(dev, 0xb, 0x391);  mdelay(1);
+
+	write_rtl8225(dev, 0xc, 0x50);  mdelay(1);
+
+
+	write_rtl8225(dev, 0xd, 0x6db);   mdelay(1);
+
+	write_rtl8225(dev, 0xe, 0x29);  mdelay(1);
+
+	write_rtl8225(dev, 0xf, 0x914);
+
+	if(priv->card_type == USB){
+		//force_pci_posting(dev);
+		mdelay(100);
+	}
+
+	write_rtl8225(dev, 0x2, 0xc4d);
+
+	if(priv->card_type == USB){
+	//	force_pci_posting(dev);
+		mdelay(200);
+
+		write_rtl8225(dev, 0x2, 0x44d);
+
+	//	force_pci_posting(dev);
+		mdelay(100);
+
+	}//End of if(priv->card_type == USB)
+	/* FIXME!! rtl8187 we have to check if calibrarion
+	 * is successful and eventually cal. again (repeat
+	 * the two write on reg 2)
+	*/
+	force_pci_posting(dev);
+
+	mdelay(100); //200 for 8187
+
+	//if(priv->card_type != USB) /* maybe not needed even for 8185 */
+//	write_rtl8225(dev, 0x7, rtl8225_chan[channel]);
+
+	write_rtl8225(dev, 0x0, 0x127);
+
+	for(i=0;i<95;i++){
+		write_rtl8225(dev, 0x1, (u8)(i+1));
+
+		#if 0
+		if(priv->phy_ver == 1)
+			/* version A */
+			write_rtl8225(dev, 0x2, rtl8225a_rxgain[i]);
+		else
+		#endif
+		/* version B & C & D*/
+
+		write_rtl8225(dev, 0x2, rtl8225bcd_rxgain[i]);
+	}
+
+	write_rtl8225(dev, 0x0, 0x27);
+
+
+//	//if(priv->card_type != USB){
+//		write_rtl8225(dev, 0x2, 0x44d);
+//		write_rtl8225(dev, 0x7, rtl8225_chan[channel]);
+//		write_rtl8225(dev, 0x2, 0x47d);
+//
+//		force_pci_posting(dev);
+//		mdelay(100);
+//
+//		write_rtl8225(dev, 0x2, 0x44d);
+//	//}
+
+	write_rtl8225(dev, 0x0, 0x22f);
+
+	if(priv->card_type != USB)
+		rtl8185_rf_pins_enable(dev);
+
+	for(i=0;i<128;i++){
+		write_phy_ofdm(dev, 0xb, rtl8225_agc[i]);
+
+		mdelay(1);
+		write_phy_ofdm(dev, 0xa, (u8)i+ 0x80);
+
+		mdelay(1);
+	}
+
+	force_pci_posting(dev);
+	mdelay(1);
+
+	write_phy_ofdm(dev, 0x0, 0x1); mdelay(1);
+	write_phy_ofdm(dev, 0x1, 0x2); mdelay(1);
+	write_phy_ofdm(dev, 0x2, ((priv->card_type == USB)? 0x42 : 0x62)); mdelay(1);
+	write_phy_ofdm(dev, 0x3, 0x0); mdelay(1);
+	write_phy_ofdm(dev, 0x4, 0x0); mdelay(1);
+	write_phy_ofdm(dev, 0x5, 0x0); mdelay(1);
+	write_phy_ofdm(dev, 0x6, 0x40); mdelay(1);
+	write_phy_ofdm(dev, 0x7, 0x0); mdelay(1);
+	write_phy_ofdm(dev, 0x8, 0x40); mdelay(1);
+	write_phy_ofdm(dev, 0x9, 0xfe); mdelay(1);
+
+	#if 0
+	if(priv->card_type == USB){
+		write_phy_ofdm(dev, 0xa, 0x9);
+	}else{
+		if(priv->card_8185 == 1 && priv->card_8185_Bversion){
+			/* Ver B
+			* maybe later version can accept this also?
+			*/
+			write_phy_ofdm(dev, 0xa, 0x6);
+			write_phy_ofdm(dev, 0x18, 0x6f);
+		}else{
+	#endif
+			/* ver C & D */
+	write_phy_ofdm(dev, 0xa, 0x9); mdelay(1);
+
+	//write_phy_ofdm(dev, 0x18, 0xef);
+	//	}
+	//}
+	write_phy_ofdm(dev, 0xb, 0x80); mdelay(1);
+
+	write_phy_ofdm(dev, 0xc, 0x1);mdelay(1);
+
+
+	//if(priv->card_type != USB)
+	//write_phy_ofdm(dev, 0xd, 0x33); // <>
+
+	write_phy_ofdm(dev, 0xe, 0xd3);mdelay(1);
+
+
+	#if 0
+	if(priv->card_8185 == 1){
+		if(priv->card_8185_Bversion)
+			write_phy_ofdm(dev, 0xf, 0x20);/*ver B*/
+		else
+			write_phy_ofdm(dev, 0xf, 0x28);/*ver C*/
+	}else{
+	#endif
+	write_phy_ofdm(dev, 0xf, 0x38);mdelay(1);
+/*ver D & 8187*/
+//	}
+
+//	if(priv->card_8185 == 1 && priv->card_8185_Bversion)
+//		write_phy_ofdm(dev, 0x10, 0x04);/*ver B*/
+//	else
+	write_phy_ofdm(dev, 0x10, 0x84);mdelay(1);
+/*ver C & D & 8187*/
+
+	write_phy_ofdm(dev, 0x11, 0x06);mdelay(1);
+/*agc resp time 700*/
+
+
+//	if(priv->card_8185 == 2){
+	/* Ver D & 8187*/
+	write_phy_ofdm(dev, 0x12, 0x20);mdelay(1);
+
+	write_phy_ofdm(dev, 0x13, 0x20);mdelay(1);
+
+#if 0
+	}else{
+		/* Ver B & C*/
+		write_phy_ofdm(dev, 0x12, 0x0);
+		write_phy_ofdm(dev, 0x13, 0x0);
+	}
+#endif
+	write_phy_ofdm(dev, 0x14, 0x0); mdelay(1);
+	write_phy_ofdm(dev, 0x15, 0x40); mdelay(1);
+	write_phy_ofdm(dev, 0x16, 0x0); mdelay(1);
+	write_phy_ofdm(dev, 0x17, 0x40); mdelay(1);
+
+//	if (priv->card_type == USB)
+//		write_phy_ofdm(dev, 0x18, 0xef);
+
+	write_phy_ofdm(dev, 0x18, 0xef);mdelay(1);
+
+
+	write_phy_ofdm(dev, 0x19, 0x19); mdelay(1);
+	write_phy_ofdm(dev, 0x1a, 0x20); mdelay(1);
+
+//	if (priv->card_type != USB){
+//		if(priv->card_8185 == 1 && priv->card_8185_Bversion)
+//			write_phy_ofdm(dev, 0x1b, 0x66); /* Ver B */
+//		else
+	write_phy_ofdm(dev, 0x1b, 0x76);mdelay(1);
+ /* Ver C & D */ //FIXME:MAYBE not needed
+//	}
+
+	write_phy_ofdm(dev, 0x1c, 0x4);mdelay(1);
+
+#if 0
+	if(priv->card_8185 == 1){
+		if(priv->card_8185_Bversion){
+			/*ver B*/
+			write_phy_ofdm(dev, 0x1e, 0x95);
+			write_phy_ofdm(dev, 0x1f, 0x55);
+		}else{
+			/*ver C*/
+			write_phy_ofdm(dev, 0x1e, 0x90);
+			write_phy_ofdm(dev, 0x1f, 0x34);
+
+		}
+	}else{
+#endif
+		/*ver D & 8187*/
+	write_phy_ofdm(dev, 0x1e, 0x95);mdelay(1);
+
+	write_phy_ofdm(dev, 0x1f, 0x75);	mdelay(1);
+
+//	}
+
+	write_phy_ofdm(dev, 0x20, 0x1f);mdelay(1);
+
+	write_phy_ofdm(dev, 0x21, 0x27);mdelay(1);
+
+	write_phy_ofdm(dev, 0x22, 0x16);mdelay(1);
+
+//	if(priv->card_type != USB)
+	//write_phy_ofdm(dev, 0x23, 0x43); //FIXME maybe not needed // <>
+
+	write_phy_ofdm(dev, 0x24, 0x46); mdelay(1);
+	write_phy_ofdm(dev, 0x25, 0x20); mdelay(1);
+	write_phy_ofdm(dev, 0x26, 0x90); mdelay(1);
+#if 0
+	if(priv->card_8185 == 1 && priv->card_8185_Bversion)
+		write_phy_ofdm(dev, 0x27, 0x08); /* Ver B. might work also fo ver C&D ?*/
+	else
+#endif
+	write_phy_ofdm(dev, 0x27, 0x88); mdelay(1);
+/* Ver C & D & 8187*/
+
+	// <> Set init. gain to m74dBm.
+	write_phy_ofdm(dev, 0x0d, 0x43);	 mdelay(1);
+	write_phy_ofdm(dev, 0x1b, 0x76);	 mdelay(1);
+	write_phy_ofdm(dev, 0x1d, 0xc5);	 mdelay(1);
+	write_phy_ofdm(dev, 0x23, 0x78);	 mdelay(1);
+
+	//if(priv->card_type == USB);
+	//	rtl8225_set_gain_usb(dev, 1); /* FIXME this '2' is random */
+
+	write_phy_cck(dev, 0x0, 0x98); mdelay(1);
+	write_phy_cck(dev, 0x3, 0x20); mdelay(1);
+	write_phy_cck(dev, 0x4, 0x7e); mdelay(1);
+	write_phy_cck(dev, 0x5, 0x12); mdelay(1);
+	write_phy_cck(dev, 0x6, 0xfc); mdelay(1);
+#if 0
+	if(priv->card_8185 == 1 && priv->card_8185_Bversion)
+		write_phy_cck(dev, 0x7, 0xd8); /* Ver B */
+	else
+#endif
+	write_phy_cck(dev, 0x7, 0x78);mdelay(1);
+ /* Ver C & D & 8187*/
+
+	write_phy_cck(dev, 0x8, 0x2e);mdelay(1);
+
+	write_phy_cck(dev, 0x10, ((priv->card_type == USB) ? 0x9b: 0x93)); mdelay(1);
+	write_phy_cck(dev, 0x11, 0x88); mdelay(1);
+	write_phy_cck(dev, 0x12, 0x47); mdelay(1);
+#if 0
+	if(priv->card_8185 == 1 && priv->card_8185_Bversion)
+		write_phy_cck(dev, 0x13, 0x98); /* Ver B */
+	else
+#endif
+	write_phy_cck(dev, 0x13, 0xd0); /* Ver C & D & 8187*/
+
+	write_phy_cck(dev, 0x19, 0x0);
+	write_phy_cck(dev, 0x1a, 0xa0);
+	write_phy_cck(dev, 0x1b, 0x8);
+	write_phy_cck(dev, 0x40, 0x86); /* CCK Carrier Sense Threshold */
+
+	write_phy_cck(dev, 0x41, 0x8d);mdelay(1);
+
+
+	write_phy_cck(dev, 0x42, 0x15); mdelay(1);
+	write_phy_cck(dev, 0x43, 0x18); mdelay(1);
+	write_phy_cck(dev, 0x44, 0x1f); mdelay(1);
+	write_phy_cck(dev, 0x45, 0x1e); mdelay(1);
+	write_phy_cck(dev, 0x46, 0x1a); mdelay(1);
+	write_phy_cck(dev, 0x47, 0x15); mdelay(1);
+	write_phy_cck(dev, 0x48, 0x10); mdelay(1);
+	write_phy_cck(dev, 0x49, 0xa); mdelay(1);
+	write_phy_cck(dev, 0x4a, 0x5); mdelay(1);
+	write_phy_cck(dev, 0x4b, 0x2); mdelay(1);
+	write_phy_cck(dev, 0x4c, 0x5);mdelay(1);
+
+
+	write_nic_byte(dev, 0x5b, 0x0d); mdelay(1);
+
+
+
+// <>
+//	// TESTR 0xb 8187
+//	write_phy_cck(dev, 0x10, 0x93);// & 0xfb);
+//
+//	//if(priv->card_type != USB){
+//		write_phy_ofdm(dev, 0x2, 0x62);
+//		write_phy_ofdm(dev, 0x6, 0x0);
+//		write_phy_ofdm(dev, 0x8, 0x0);
+//	//}
+
+	rtl8225_SetTXPowerLevel(dev, channel);
+
+	write_phy_cck(dev, 0x10, 0x9b); mdelay(1); /* Rx ant A, 0xdb for B */
+	write_phy_ofdm(dev, 0x26, 0x90); mdelay(1); /* Rx ant A, 0x10 for B */
+
+	rtl8185_tx_antenna(dev, 0x3); /* TX ant A, 0x0 for B */
+
+	/* switch to high-speed 3-wire
+	 * last digit. 2 for both cck and ofdm
+	 */
+	if(priv->card_type == USB)
+		write_nic_dword(dev, 0x94, 0x3dc00002);
+	else{
+		write_nic_dword(dev, 0x94, 0x15c00002);
+		rtl8185_rf_pins_enable(dev);
+	}
+
+//	if(priv->card_type != USB)
+//	rtl8225_set_gain(dev, 4); /* FIXME this '1' is random */ // <>
+//	 rtl8225_set_mode(dev, 1); /* FIXME start in B mode */ // <>
+//
+//	/* make sure is waken up! */
+//	write_rtl8225(dev,0x4, 0x9ff);
+//	rtl8180_set_anaparam(dev, RTL8225_ANAPARAM_ON);
+//	rtl8185_set_anaparam2(dev, RTL8225_ANAPARAM2_ON);
+
+	rtl8225_rf_set_chan(dev, priv->chan);
+
+	write_nic_word(dev,BRSR,brsr);
+
+}
diff --git a/drivers/staging/rtl8187se/r8180_rtl8225.h b/drivers/staging/rtl8187se/r8180_rtl8225.h
new file mode 100644
index 00000000000..458de6629ad
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_rtl8225.h
@@ -0,0 +1,44 @@
+/*
+  This is part of the rtl8180-sa2400 driver
+  released under the GPL (See file COPYING for details).
+  Copyright (c) 2005 Andrea Merello <andreamrl@tiscali.it>
+
+  This files contains programming code for the rtl8225
+  radio frontend.
+
+  *Many* thanks to Realtek Corp. for their great support!
+
+*/
+
+#include "r8180.h"
+
+#define RTL8225_ANAPARAM_ON  0xa0000b59
+#define RTL8225_ANAPARAM_OFF 0xa00beb59
+#define RTL8225_ANAPARAM2_OFF 0x840dec11
+#define RTL8225_ANAPARAM2_ON  0x860dec11
+#define RTL8225_ANAPARAM_SLEEP 0xa00bab59
+#define RTL8225_ANAPARAM2_SLEEP 0x840dec11
+
+#ifdef CONFIG_RTL8185B
+void rtl8225z2_rf_init(struct net_device *dev);
+void rtl8225z2_rf_set_chan(struct net_device *dev,short ch);
+void rtl8225z2_rf_close(struct net_device *dev);
+
+void rtl8225_host_pci_init(struct net_device *dev);
+void rtl8225_host_usb_init(struct net_device *dev);
+
+void write_rtl8225(struct net_device *dev, u8 adr, u16 data);
+void RF_WriteReg(struct net_device *dev, u8 offset, u32	data);
+u32 RF_ReadReg(struct net_device *dev, u8 offset);
+#endif
+void rtl8225_rf_init(struct net_device *dev);
+void rtl8225_rf_set_chan(struct net_device *dev,short ch);
+void rtl8225_rf_close(struct net_device *dev);
+void rtl8225_rf_sleep(struct net_device *dev);
+void rtl8225_rf_wakeup(struct net_device *dev);
+void rtl8180_set_mode(struct net_device *dev,int mode);
+void rtl8180_set_mode(struct net_device *dev,int mode);
+bool SetZebraRFPowerState8185(struct net_device *dev,RT_RF_POWER_STATE  eRFPowerState);
+void rtl8225z4_rf_sleep(struct net_device *dev);
+void rtl8225z4_rf_wakeup(struct net_device *dev);
+
diff --git a/drivers/staging/rtl8187se/r8180_rtl8225z2.c b/drivers/staging/rtl8187se/r8180_rtl8225z2.c
new file mode 100644
index 00000000000..90a574d46da
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_rtl8225z2.c
@@ -0,0 +1,1587 @@
+/*
+  This is part of the rtl8180-sa2400 driver
+  released under the GPL (See file COPYING for details).
+  Copyright (c) 2005 Andrea Merello <andreamrl@tiscali.it>
+
+  This files contains programming code for the rtl8225
+  radio frontend.
+
+  *Many* thanks to Realtek Corp. for their great support!
+
+*/
+
+#include "r8180_hw.h"
+#include "r8180_rtl8225.h"
+#include "r8180_93cx6.h"
+
+#ifdef ENABLE_DOT11D
+#include "dot11d.h"
+#endif
+
+#ifdef CONFIG_RTL8185B
+
+extern u8 rtl8225_agc[];
+
+extern u32 rtl8225_chan[];
+
+//2005.11.16
+u8 rtl8225z2_threshold[]={
+        0x8d, 0x8d, 0x8d, 0x8d, 0x9d, 0xad, 0xbd,
+};
+
+//      0xd 0x19 0x1b 0x21
+u8 rtl8225z2_gain_bg[]={
+	0x23, 0x15, 0xa5, // -82-1dbm
+        0x23, 0x15, 0xb5, // -82-2dbm
+        0x23, 0x15, 0xc5, // -82-3dbm
+        0x33, 0x15, 0xc5, // -78dbm
+        0x43, 0x15, 0xc5, // -74dbm
+        0x53, 0x15, 0xc5, // -70dbm
+        0x63, 0x15, 0xc5, // -66dbm
+};
+
+u8 rtl8225z2_gain_a[]={
+	0x13,0x27,0x5a,//,0x37,// -82dbm
+	0x23,0x23,0x58,//,0x37,// -82dbm
+	0x33,0x1f,0x56,//,0x37,// -82dbm
+	0x43,0x1b,0x54,//,0x37,// -78dbm
+	0x53,0x17,0x51,//,0x37,// -74dbm
+	0x63,0x24,0x4f,//,0x37,// -70dbm
+	0x73,0x0f,0x4c,//,0x37,// -66dbm
+};
+#if 0
+u32 rtl8225_chan[] = {
+	0,	//dummy channel 0
+	0x085c, //1
+	0x08dc, //2
+	0x095c, //3
+	0x09dc, //4
+	0x0a5c, //5
+	0x0adc, //6
+	0x0b5c, //7
+	0x0bdc, //8
+	0x0c5c, //9
+	0x0cdc, //10
+	0x0d5c, //11
+	0x0ddc, //12
+	0x0e5c, //13
+	//0x0f5c, //14
+	0x0f72, // 14
+};
+#endif
+
+//-
+u16 rtl8225z2_rxgain[]={
+	0x0400, 0x0401, 0x0402, 0x0403, 0x0404, 0x0405, 0x0408, 0x0409,
+	0x040a, 0x040b, 0x0502, 0x0503, 0x0504, 0x0505, 0x0540, 0x0541,
+	0x0542, 0x0543, 0x0544, 0x0545, 0x0580, 0x0581, 0x0582, 0x0583,
+	0x0584, 0x0585, 0x0588, 0x0589, 0x058a, 0x058b, 0x0643, 0x0644,
+	0x0645, 0x0680, 0x0681, 0x0682, 0x0683, 0x0684, 0x0685, 0x0688,
+	0x0689, 0x068a, 0x068b, 0x068c, 0x0742, 0x0743, 0x0744, 0x0745,
+	0x0780, 0x0781, 0x0782, 0x0783, 0x0784, 0x0785, 0x0788, 0x0789,
+	0x078a, 0x078b, 0x078c, 0x078d, 0x0790, 0x0791, 0x0792, 0x0793,
+	0x0794, 0x0795, 0x0798, 0x0799, 0x079a, 0x079b, 0x079c, 0x079d,
+	0x07a0, 0x07a1, 0x07a2, 0x07a3, 0x07a4, 0x07a5, 0x07a8, 0x07a9,
+	0x03aa, 0x03ab, 0x03ac, 0x03ad, 0x03b0, 0x03b1, 0x03b2, 0x03b3,
+	0x03b4, 0x03b5, 0x03b8, 0x03b9, 0x03ba, 0x03bb, 0x03bb
+
+};
+
+//2005.11.16,
+u8 ZEBRA2_CCK_OFDM_GAIN_SETTING[]={
+        0x00,0x01,0x02,0x03,0x04,0x05,
+        0x06,0x07,0x08,0x09,0x0a,0x0b,
+        0x0c,0x0d,0x0e,0x0f,0x10,0x11,
+        0x12,0x13,0x14,0x15,0x16,0x17,
+        0x18,0x19,0x1a,0x1b,0x1c,0x1d,
+        0x1e,0x1f,0x20,0x21,0x22,0x23,
+};
+
+#if 0
+//-
+u8 rtl8225_agc[]={
+	0x9e,0x9e,0x9e,0x9e,0x9e,0x9e,0x9e,0x9e,0x9d,0x9c,0x9b,0x9a,0x99,0x98,0x97,0x96,
+	0x95,0x94,0x93,0x92,0x91,0x90,0x8f,0x8e,0x8d,0x8c,0x8b,0x8a,0x89,0x88,0x87,0x86,
+	0x85,0x84,0x83,0x82,0x81,0x80,0x3f,0x3e,0x3d,0x3c,0x3b,0x3a,0x39,0x38,0x37,0x36,
+	0x35,0x34,0x33,0x32,0x31,0x30,0x2f,0x2e,0x2d,0x2c,0x2b,0x2a,0x29,0x28,0x27,0x26,
+	0x25,0x24,0x23,0x22,0x21,0x20,0x1f,0x1e,0x1d,0x1c,0x1b,0x1a,0x19,0x18,0x17,0x16,
+	0x15,0x14,0x13,0x12,0x11,0x10,0x0f,0x0e,0x0d,0x0c,0x0b,0x0a,0x09,0x08,0x07,0x06,
+	0x05,0x04,0x03,0x02,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,
+	0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,
+};
+#endif
+/*
+ from 0 to 0x23
+u8 rtl8225_tx_gain_cck_ofdm[]={
+	0x02,0x06,0x0e,0x1e,0x3e,0x7e
+};
+*/
+
+//-
+u8 rtl8225z2_tx_power_ofdm[]={
+	0x42,0x00,0x40,0x00,0x40
+};
+
+
+//-
+u8 rtl8225z2_tx_power_cck_ch14[]={
+	0x36,0x35,0x2e,0x1b,0x00,0x00,0x00,0x00
+};
+
+
+//-
+u8 rtl8225z2_tx_power_cck[]={
+	0x36,0x35,0x2e,0x25,0x1c,0x12,0x09,0x04
+};
+
+
+void rtl8225z2_set_gain(struct net_device *dev, short gain)
+{
+	u8* rtl8225_gain;
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	u8 mode = priv->ieee80211->mode;
+
+	if(mode == IEEE_B || mode == IEEE_G)
+		rtl8225_gain = rtl8225z2_gain_bg;
+	else
+		rtl8225_gain = rtl8225z2_gain_a;
+
+	//write_phy_ofdm(dev, 0x0d, rtl8225_gain[gain * 3]);
+	//write_phy_ofdm(dev, 0x19, rtl8225_gain[gain * 3 + 1]);
+	//write_phy_ofdm(dev, 0x1b, rtl8225_gain[gain * 3 + 2]);
+        //2005.11.17, by ch-hsu
+        write_phy_ofdm(dev, 0x0b, rtl8225_gain[gain * 3]);
+        write_phy_ofdm(dev, 0x1b, rtl8225_gain[gain * 3 + 1]);
+        write_phy_ofdm(dev, 0x1d, rtl8225_gain[gain * 3 + 2]);
+	write_phy_ofdm(dev, 0x21, 0x37);
+
+}
+
+#if 0
+
+void rtl8225_set_gain(struct net_device *dev, short gain)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	rtl8180_set_anaparam(dev, RTL8225_ANAPARAM_ON);
+
+	if(priv->card_8185 == 2)
+		write_phy_ofdm(dev, 0x21, 0x27);
+	else
+		write_phy_ofdm(dev, 0x21, 0x37);
+
+	write_phy_ofdm(dev, 0x25, 0x20);
+	write_phy_ofdm(dev, 0x11, 0x6);
+
+	if(priv->card_8185 == 1 && priv->card_8185_Bversion)
+		write_phy_ofdm(dev, 0x27, 0x8);
+	else
+		write_phy_ofdm(dev, 0x27, 0x88);
+
+	write_phy_ofdm(dev, 0x14, 0);
+	write_phy_ofdm(dev, 0x16, 0);
+	write_phy_ofdm(dev, 0x15, 0x40);
+	write_phy_ofdm(dev, 0x17, 0x40);
+
+	write_phy_ofdm(dev, 0x0d, rtl8225_gain[gain * 4]);
+	write_phy_ofdm(dev, 0x23, rtl8225_gain[gain * 4 + 1]);
+	write_phy_ofdm(dev, 0x1b, rtl8225_gain[gain * 4 + 2]);
+	write_phy_ofdm(dev, 0x1d, rtl8225_gain[gain * 4 + 3]);
+	//rtl8225_set_gain_usb(dev, gain);
+}
+#endif
+
+u32 read_rtl8225(struct net_device *dev, u8 adr)
+{
+	u32 data2Write = ((u32)(adr & 0x1f)) << 27;
+	u32 dataRead;
+	u32 mask;
+	u16 oval,oval2,oval3,tmp;
+//	ThreeWireReg twreg;
+//	ThreeWireReg tdata;
+	int i;
+	short bit, rw;
+
+	u8 wLength = 6;
+	u8 rLength = 12;
+	u8 low2high = 0;
+
+	oval = read_nic_word(dev, RFPinsOutput);
+	oval2 = read_nic_word(dev, RFPinsEnable);
+	oval3 = read_nic_word(dev, RFPinsSelect);
+
+	write_nic_word(dev, RFPinsEnable, (oval2|0xf));
+	write_nic_word(dev, RFPinsSelect, (oval3|0xf));
+
+	dataRead = 0;
+
+	oval &= ~0xf;
+
+	write_nic_word(dev, RFPinsOutput, oval | BB_HOST_BANG_EN ); udelay(4);
+
+	write_nic_word(dev, RFPinsOutput, oval ); udelay(5);
+
+	rw = 0;
+
+	mask = (low2high) ? 0x01 : (((u32)0x01)<<(32-1));
+	for(i = 0; i < wLength/2; i++)
+	{
+		bit = ((data2Write&mask) != 0) ? 1 : 0;
+		write_nic_word(dev, RFPinsOutput, bit|oval | rw); udelay(1);
+
+		write_nic_word(dev, RFPinsOutput, bit|oval | BB_HOST_BANG_CLK | rw); udelay(2);
+		write_nic_word(dev, RFPinsOutput, bit|oval | BB_HOST_BANG_CLK | rw); udelay(2);
+
+		mask = (low2high) ? (mask<<1): (mask>>1);
+
+		if(i == 2)
+		{
+			rw = BB_HOST_BANG_RW;
+			write_nic_word(dev, RFPinsOutput, bit|oval | BB_HOST_BANG_CLK | rw); udelay(2);
+			write_nic_word(dev, RFPinsOutput, bit|oval | rw); udelay(2);
+			break;
+		}
+
+		bit = ((data2Write&mask) != 0) ? 1: 0;
+
+		write_nic_word(dev, RFPinsOutput, oval|bit|rw| BB_HOST_BANG_CLK); udelay(2);
+		write_nic_word(dev, RFPinsOutput, oval|bit|rw| BB_HOST_BANG_CLK); udelay(2);
+
+		write_nic_word(dev, RFPinsOutput, oval| bit |rw); udelay(1);
+
+		mask = (low2high) ? (mask<<1) : (mask>>1);
+	}
+
+	//twreg.struc.clk = 0;
+	//twreg.struc.data = 0;
+	write_nic_word(dev, RFPinsOutput, rw|oval); udelay(2);
+	mask = (low2high) ? 0x01 : (((u32)0x01) << (12-1));
+
+	// We must set data pin to HW controled, otherwise RF can't driver it and
+	// value RF register won't be able to read back properly. 2006.06.13, by rcnjko.
+	write_nic_word(dev, RFPinsEnable, (oval2 & (~0x01)));
+
+	for(i = 0; i < rLength; i++)
+	{
+		write_nic_word(dev, RFPinsOutput, rw|oval); udelay(1);
+
+		write_nic_word(dev, RFPinsOutput, rw|oval|BB_HOST_BANG_CLK); udelay(2);
+		write_nic_word(dev, RFPinsOutput, rw|oval|BB_HOST_BANG_CLK); udelay(2);
+		write_nic_word(dev, RFPinsOutput, rw|oval|BB_HOST_BANG_CLK); udelay(2);
+		tmp = read_nic_word(dev, RFPinsInput);
+
+		dataRead |= (tmp & BB_HOST_BANG_CLK ? mask : 0);
+
+		write_nic_word(dev, RFPinsOutput, (rw|oval)); udelay(2);
+
+		mask = (low2high) ? (mask<<1) : (mask>>1);
+	}
+
+	write_nic_word(dev, RFPinsOutput, BB_HOST_BANG_EN|BB_HOST_BANG_RW|oval); udelay(2);
+
+	write_nic_word(dev, RFPinsEnable, oval2);
+	write_nic_word(dev, RFPinsSelect, oval3);   // Set To SW Switch
+	write_nic_word(dev, RFPinsOutput, 0x3a0);
+
+	return dataRead;
+
+}
+#if 0
+void write_rtl8225(struct net_device *dev, u8 adr, u16 data)
+{
+	int i;
+	u16 out,select;
+	u8 bit;
+	u32 bangdata = (data << 4) | (adr & 0xf);
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	out = read_nic_word(dev, RFPinsOutput) & 0xfff3;
+
+	write_nic_word(dev,RFPinsEnable,
+		(read_nic_word(dev,RFPinsEnable) | 0x7));
+
+	select = read_nic_word(dev, RFPinsSelect);
+
+	write_nic_word(dev, RFPinsSelect, select | 0x7 |
+		((priv->card_type == USB) ? 0 : SW_CONTROL_GPIO));
+
+	force_pci_posting(dev);
+	udelay(10);
+
+	write_nic_word(dev, RFPinsOutput, out | BB_HOST_BANG_EN );//| 0x1fff);
+
+	force_pci_posting(dev);
+	udelay(2);
+
+	write_nic_word(dev, RFPinsOutput, out);
+
+	force_pci_posting(dev);
+	udelay(10);
+
+
+	for(i=15; i>=0;i--){
+
+		bit = (bangdata & (1<<i)) >> i;
+
+		write_nic_word(dev, RFPinsOutput, bit | out);
+
+		write_nic_word(dev, RFPinsOutput, bit | out | BB_HOST_BANG_CLK);
+		write_nic_word(dev, RFPinsOutput, bit | out | BB_HOST_BANG_CLK);
+
+		i--;
+		bit = (bangdata & (1<<i)) >> i;
+
+		write_nic_word(dev, RFPinsOutput, bit | out | BB_HOST_BANG_CLK);
+		write_nic_word(dev, RFPinsOutput, bit | out | BB_HOST_BANG_CLK);
+
+		write_nic_word(dev, RFPinsOutput, bit | out);
+
+	}
+
+	write_nic_word(dev, RFPinsOutput, out | BB_HOST_BANG_EN);
+
+	force_pci_posting(dev);
+	udelay(10);
+
+	write_nic_word(dev, RFPinsOutput, out |
+		((priv->card_type == USB) ? 4 : BB_HOST_BANG_EN));
+
+	write_nic_word(dev, RFPinsSelect, select |
+		((priv->card_type == USB) ? 0 : SW_CONTROL_GPIO));
+
+	if(priv->card_type == USB)
+		mdelay(2);
+	else
+		rtl8185_rf_pins_enable(dev);
+}
+
+#endif
+short rtl8225_is_V_z2(struct net_device *dev)
+{
+	short vz2 = 1;
+	//int i;
+	/* sw to reg pg 1 */
+	//write_rtl8225(dev, 0, 0x1b7);
+	//write_rtl8225(dev, 0, 0x0b7);
+
+	/* reg 8 pg 1 = 23*/
+	//printk(KERN_WARNING "RF Rigisters:\n");
+#if 0
+	for(i = 0; i <= 0xf; i++)
+		printk(KERN_WARNING "%08x,", read_rtl8225(dev, i));
+	//printk(KERN_WARNING "reg[9]@pg1 = 0x%x\n", read_rtl8225(dev, 0x0F));
+
+//	printk(KERN_WARNING "RF:\n");
+#endif
+	if( read_rtl8225(dev, 8) != 0x588)
+		vz2 = 0;
+
+	else	/* reg 9 pg 1 = 24 */
+		if( read_rtl8225(dev, 9) != 0x700)
+			vz2 = 0;
+
+	/* sw back to pg 0 */
+	write_rtl8225(dev, 0, 0xb7);
+
+	return vz2;
+
+}
+
+#if 0
+void rtl8225_rf_close(struct net_device *dev)
+{
+	write_rtl8225(dev, 0x4, 0x1f);
+
+	force_pci_posting(dev);
+	mdelay(1);
+
+	rtl8180_set_anaparam(dev, RTL8225_ANAPARAM_OFF);
+	rtl8185_set_anaparam2(dev, RTL8225_ANAPARAM2_OFF);
+}
+#endif
+#if 0
+short rtl8225_rf_set_sens(struct net_device *dev, short sens)
+{
+	if (sens <0 || sens > 6) return -1;
+
+	if(sens > 4)
+		write_rtl8225(dev, 0x0c, 0x850);
+	else
+		write_rtl8225(dev, 0x0c, 0x50);
+
+	sens= 6-sens;
+	rtl8225_set_gain(dev, sens);
+
+	write_phy_cck(dev, 0x41, rtl8225_threshold[sens]);
+	return 0;
+
+}
+#endif
+
+
+void rtl8225z2_rf_close(struct net_device *dev)
+{
+	RF_WriteReg(dev, 0x4, 0x1f);
+
+	force_pci_posting(dev);
+	mdelay(1);
+
+	rtl8180_set_anaparam(dev, RTL8225z2_ANAPARAM_OFF);
+	rtl8185_set_anaparam2(dev, RTL8225z2_ANAPARAM2_OFF);
+}
+
+#ifdef ENABLE_DOT11D
+//
+//	Description:
+//		Map dBm into Tx power index according to
+//		current HW model, for example, RF and PA, and
+//		current wireless mode.
+//
+s8
+DbmToTxPwrIdx(
+	struct r8180_priv *priv,
+	WIRELESS_MODE	WirelessMode,
+	s32			PowerInDbm
+	)
+{
+ 	bool bUseDefault = true;
+	s8 TxPwrIdx = 0;
+
+#ifdef CONFIG_RTL818X_S
+	//
+	// 071011, SD3 SY:
+	// OFDM Power in dBm = Index * 0.5 + 0
+	// CCK Power in dBm = Index * 0.25 + 13
+	//
+	if(priv->card_8185 >= VERSION_8187S_B)
+	{
+		s32 tmp = 0;
+
+		if(WirelessMode == WIRELESS_MODE_G)
+		{
+			bUseDefault = false;
+			tmp = (2 * PowerInDbm);
+
+			if(tmp < 0)
+				TxPwrIdx = 0;
+			else if(tmp > 40) // 40 means 20 dBm.
+				TxPwrIdx = 40;
+			else
+				TxPwrIdx = (s8)tmp;
+		}
+		else if(WirelessMode == WIRELESS_MODE_B)
+		{
+			bUseDefault = false;
+			tmp = (4 * PowerInDbm) - 52;
+
+			if(tmp < 0)
+				TxPwrIdx = 0;
+			else if(tmp > 28) // 28 means 20 dBm.
+				TxPwrIdx = 28;
+			else
+				TxPwrIdx = (s8)tmp;
+		}
+	}
+#endif
+
+	//
+	// TRUE if we want to use a default implementation.
+	// We shall set it to FALSE when we have exact translation formular
+	// for target IC. 070622, by rcnjko.
+	//
+	if(bUseDefault)
+	{
+		if(PowerInDbm < 0)
+			TxPwrIdx = 0;
+		else if(PowerInDbm > 35)
+			TxPwrIdx = 35;
+		else
+			TxPwrIdx = (u8)PowerInDbm;
+	}
+
+	return TxPwrIdx;
+}
+#endif
+
+void rtl8225z2_SetTXPowerLevel(struct net_device *dev, short ch)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+//	int GainIdx;
+//	int GainSetting;
+	//int i;
+	//u8 power;
+	//u8 *cck_power_table;
+	u8 max_cck_power_level;
+	//u8 min_cck_power_level;
+	u8 max_ofdm_power_level;
+	u8 min_ofdm_power_level;
+//	u8 cck_power_level = 0xff & priv->chtxpwr[ch];//-by amy 080312
+//	u8 ofdm_power_level = 0xff & priv->chtxpwr_ofdm[ch];//-by amy 080312
+	char cck_power_level = (char)(0xff & priv->chtxpwr[ch]);//+by amy 080312
+	char ofdm_power_level = (char)(0xff & priv->chtxpwr_ofdm[ch]);//+by amy 080312
+#if 0
+	//
+	// CCX 2 S31, AP control of client transmit power:
+	// 1. We shall not exceed Cell Power Limit as possible as we can.
+	// 2. Tolerance is +/- 5dB.
+	// 3. 802.11h Power Contraint takes higher precedence over CCX Cell Power Limit.
+	//
+	// TODO:
+	// 1. 802.11h power contraint
+	//
+	// 071011, by rcnjko.
+	//
+	if(	priv->OpMode == RT_OP_MODE_INFRASTRUCTURE &&
+		priv->bWithCcxCellPwr &&
+		ch == priv->dot11CurrentChannelNumber)
+	{
+		u8 CckCellPwrIdx = DbmToTxPwrIdx(dev, WIRELESS_MODE_B, pMgntInfo->CcxCellPwr);
+		u8 OfdmCellPwrIdx = DbmToTxPwrIdx(dev, WIRELESS_MODE_G, pMgntInfo->CcxCellPwr);
+
+		printk("CCX Cell Limit: %d dBm => CCK Tx power index : %d, OFDM Tx power index: %d\n",
+			priv->CcxCellPwr, CckCellPwrIdx, OfdmCellPwrIdx);
+		printk("EEPROM channel(%d) => CCK Tx power index: %d, OFDM Tx power index: %d\n",
+			channel, CckTxPwrIdx, OfdmTxPwrIdx);
+
+		if(cck_power_level > CckCellPwrIdx)
+			cck_power_level = CckCellPwrIdx;
+		if(ofdm_power_level > OfdmCellPwrIdx)
+			ofdm_power_level = OfdmCellPwrIdx;
+
+		printk("Altered CCK Tx power index : %d, OFDM Tx power index: %d\n",
+			CckTxPwrIdx, OfdmTxPwrIdx);
+	}
+#endif
+#ifdef ENABLE_DOT11D
+	if(IS_DOT11D_ENABLE(priv->ieee80211) &&
+		IS_DOT11D_STATE_DONE(priv->ieee80211) )
+	{
+		//PRT_DOT11D_INFO pDot11dInfo = GET_DOT11D_INFO(priv->ieee80211);
+		u8 MaxTxPwrInDbm = DOT11D_GetMaxTxPwrInDbm(priv->ieee80211, ch);
+		u8 CckMaxPwrIdx = DbmToTxPwrIdx(priv, WIRELESS_MODE_B, MaxTxPwrInDbm);
+		u8 OfdmMaxPwrIdx = DbmToTxPwrIdx(priv, WIRELESS_MODE_G, MaxTxPwrInDbm);
+
+		//printk("Max Tx Power dBm (%d) => CCK Tx power index : %d, OFDM Tx power index: %d\n", MaxTxPwrInDbm, CckMaxPwrIdx, OfdmMaxPwrIdx);
+
+		//printk("EEPROM channel(%d) => CCK Tx power index: %d, OFDM Tx power index: %d\n",
+		//	ch, cck_power_level, ofdm_power_level);
+
+		if(cck_power_level > CckMaxPwrIdx)
+			cck_power_level = CckMaxPwrIdx;
+		if(ofdm_power_level > OfdmMaxPwrIdx)
+			ofdm_power_level = OfdmMaxPwrIdx;
+	}
+
+	//priv->CurrentCckTxPwrIdx = cck_power_level;
+	//priv->CurrentOfdmTxPwrIdx = ofdm_power_level;
+#endif
+
+	max_cck_power_level = 15;
+	max_ofdm_power_level = 25; //  12 -> 25
+	min_ofdm_power_level = 10;
+
+#ifdef CONFIG_RTL8185B
+#ifdef CONFIG_RTL818X_S
+
+	if(cck_power_level > 35)
+	{
+		cck_power_level = 35;
+	}
+	//
+	// Set up CCK TXAGC. suggested by SD3 SY.
+	//
+       write_nic_byte(dev, CCK_TXAGC, (ZEBRA2_CCK_OFDM_GAIN_SETTING[(u8)cck_power_level]) );
+       //printk("CCK TX power is %x\n", (ZEBRA2_CCK_OFDM_GAIN_SETTING[cck_power_level]));
+       force_pci_posting(dev);
+	mdelay(1);
+#else
+
+	/* CCK power setting */
+	if(cck_power_level > max_cck_power_level)
+		cck_power_level = max_cck_power_level;
+
+	cck_power_level += priv->cck_txpwr_base;
+
+	if(cck_power_level > 35)
+		cck_power_level = 35;
+
+	if(ch == 14)
+		cck_power_table = rtl8225z2_tx_power_cck_ch14;
+	else
+		cck_power_table = rtl8225z2_tx_power_cck;
+
+
+	for(i=0;i<8;i++){
+
+		power = cck_power_table[i];
+		write_phy_cck(dev, 0x44 + i, power);
+	}
+
+	//write_nic_byte(dev, TX_GAIN_CCK, power);
+	//2005.11.17,
+	write_nic_byte(dev, CCK_TXAGC, ZEBRA2_CCK_OFDM_GAIN_SETTING[(u8)cck_power_level]);
+
+	force_pci_posting(dev);
+	mdelay(1);
+#endif
+#endif
+	/* OFDM power setting */
+//  Old:
+//	if(ofdm_power_level > max_ofdm_power_level)
+//		ofdm_power_level = 35;
+//	ofdm_power_level += min_ofdm_power_level;
+//  Latest:
+/*	if(ofdm_power_level > (max_ofdm_power_level - min_ofdm_power_level))
+		ofdm_power_level = max_ofdm_power_level;
+	else
+		ofdm_power_level += min_ofdm_power_level;
+
+	ofdm_power_level += priv->ofdm_txpwr_base;
+*/
+	if(ofdm_power_level > 35)
+		ofdm_power_level = 35;
+
+//	rtl8185_set_anaparam2(dev,RTL8225_ANAPARAM2_ON);
+
+	//rtl8185_set_anaparam2(dev, ANAPARM2_ASIC_ON);
+
+	if (priv->up == 0) {
+		//must add these for rtl8185B down, xiong-2006-11-21
+		write_phy_ofdm(dev,2,0x42);
+		write_phy_ofdm(dev,5,0);
+		write_phy_ofdm(dev,6,0x40);
+		write_phy_ofdm(dev,7,0);
+		write_phy_ofdm(dev,8,0x40);
+	}
+
+	//write_nic_byte(dev, TX_GAIN_OFDM, ofdm_power_level);
+	//2005.11.17,
+#ifdef CONFIG_RTL818X_S
+        write_nic_byte(dev, OFDM_TXAGC, ZEBRA2_CCK_OFDM_GAIN_SETTING[(u8)ofdm_power_level]);
+#else
+        write_nic_byte(dev, OFDM_TXAGC, ZEBRA2_CCK_OFDM_GAIN_SETTING[(u8)ofdm_power_level]*2);
+#endif
+        if(ofdm_power_level<=11)
+        {
+//            write_nic_dword(dev,PHY_ADR,0x00005c87);
+//            write_nic_dword(dev,PHY_ADR,0x00005c89);
+		write_phy_ofdm(dev,0x07,0x5c);
+		write_phy_ofdm(dev,0x09,0x5c);
+        }
+	if(ofdm_power_level<=17)
+        {
+//             write_nic_dword(dev,PHY_ADR,0x00005487);
+//             write_nic_dword(dev,PHY_ADR,0x00005489);
+		write_phy_ofdm(dev,0x07,0x54);
+		write_phy_ofdm(dev,0x09,0x54);
+        }
+        else
+        {
+//             write_nic_dword(dev,PHY_ADR,0x00005087);
+//             write_nic_dword(dev,PHY_ADR,0x00005089);
+		write_phy_ofdm(dev,0x07,0x50);
+		write_phy_ofdm(dev,0x09,0x50);
+        }
+	force_pci_posting(dev);
+	mdelay(1);
+
+}
+#if 0
+/* switch between mode B and G */
+void rtl8225_set_mode(struct net_device *dev, short modeb)
+{
+	write_phy_ofdm(dev, 0x15, (modeb ? 0x0 : 0x40));
+	write_phy_ofdm(dev, 0x17, (modeb ? 0x0 : 0x40));
+}
+#endif
+
+void rtl8225z2_rf_set_chan(struct net_device *dev, short ch)
+{
+/*
+	short gset = (priv->ieee80211->state == IEEE80211_LINKED &&
+		ieee80211_is_54g(priv->ieee80211->current_network)) ||
+		priv->ieee80211->iw_mode == IW_MODE_MONITOR;
+*/
+	rtl8225z2_SetTXPowerLevel(dev, ch);
+
+	RF_WriteReg(dev, 0x7, rtl8225_chan[ch]);
+
+	//YJ,add,080828, if set channel failed, write again
+	if((RF_ReadReg(dev, 0x7) & 0x0F80) != rtl8225_chan[ch])
+	{
+		RF_WriteReg(dev, 0x7, rtl8225_chan[ch]);
+	}
+
+	mdelay(1);
+
+	force_pci_posting(dev);
+	mdelay(10);
+//deleted by David : 2006/8/9
+#if 0
+	write_nic_byte(dev,SIFS,0x22);// SIFS: 0x22
+
+	if(gset)
+		write_nic_byte(dev,DIFS,20); //DIFS: 20
+	else
+		write_nic_byte(dev,DIFS,0x24); //DIFS: 36
+
+	if(priv->ieee80211->state == IEEE80211_LINKED &&
+		ieee80211_is_shortslot(priv->ieee80211->current_network))
+		write_nic_byte(dev,SLOT,0x9); //SLOT: 9
+
+	else
+		write_nic_byte(dev,SLOT,0x14); //SLOT: 20 (0x14)
+
+
+	if(gset){
+		write_nic_byte(dev,EIFS,91 - 20); // EIFS: 91 (0x5B)
+		write_nic_byte(dev,CW_VAL,0x73); //CW VALUE: 0x37
+		//DMESG("using G net params");
+	}else{
+		write_nic_byte(dev,EIFS,91 - 0x24); // EIFS: 91 (0x5B)
+		write_nic_byte(dev,CW_VAL,0xa5); //CW VALUE: 0x37
+		//DMESG("using B net params");
+	}
+#endif
+
+}
+#if 0
+void rtl8225_host_pci_init(struct net_device *dev)
+{
+	write_nic_word(dev, RFPinsOutput, 0x480);
+
+	rtl8185_rf_pins_enable(dev);
+
+	//if(priv->card_8185 == 2 && priv->enable_gpio0 ) /* version D */
+	//write_nic_word(dev, RFPinsSelect, 0x88);
+	//else
+	write_nic_word(dev, RFPinsSelect, 0x88 | SW_CONTROL_GPIO); /* 0x488 | SW_CONTROL_GPIO */
+
+	write_nic_byte(dev, GP_ENABLE, 0);
+
+	force_pci_posting(dev);
+	mdelay(200);
+
+	write_nic_word(dev, GP_ENABLE, 0xff & (~(1<<6))); /* bit 6 is for RF on/off detection */
+
+
+}
+
+void rtl8225_host_usb_init(struct net_device *dev)
+{
+	write_nic_byte(dev,RFPinsSelect+1,0);
+
+	write_nic_byte(dev,GPIO,0);
+
+	write_nic_byte_E(dev,0x53,read_nic_byte_E(dev,0x53) | (1<<7));
+
+	write_nic_byte(dev,RFPinsSelect+1,4);
+
+	write_nic_byte(dev,GPIO,0x20);
+
+	write_nic_byte(dev,GP_ENABLE,0);
+
+
+	/* Config BB & RF */
+	write_nic_word(dev, RFPinsOutput, 0x80);
+
+	write_nic_word(dev, RFPinsSelect, 0x80);
+
+	write_nic_word(dev, RFPinsEnable, 0x80);
+
+
+	mdelay(100);
+
+	mdelay(1000);
+
+}
+#endif
+void rtl8225z2_rf_init(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	int i;
+	short channel = 1;
+	u16	brsr;
+	u32	data,addr;
+
+	priv->chan = channel;
+
+//	rtl8180_set_anaparam(dev, RTL8225_ANAPARAM_ON);
+
+
+	if(priv->card_type == USB)
+		rtl8225_host_usb_init(dev);
+	else
+		rtl8225_host_pci_init(dev);
+
+	write_nic_dword(dev, RF_TIMING, 0x000a8008);
+
+	brsr = read_nic_word(dev, BRSR);
+
+	write_nic_word(dev, BRSR, 0xffff);
+
+
+	write_nic_dword(dev, RF_PARA, 0x100044);
+
+	#if 1  //0->1
+	rtl8180_set_mode(dev, EPROM_CMD_CONFIG);
+	write_nic_byte(dev, CONFIG3, 0x44);
+	rtl8180_set_mode(dev, EPROM_CMD_NORMAL);
+	#endif
+
+
+	rtl8185_rf_pins_enable(dev);
+
+//		mdelay(1000);
+
+	write_rtl8225(dev, 0x0, 0x2bf); mdelay(1);
+
+
+	write_rtl8225(dev, 0x1, 0xee0); mdelay(1);
+
+	write_rtl8225(dev, 0x2, 0x44d); mdelay(1);
+
+	write_rtl8225(dev, 0x3, 0x441); mdelay(1);
+
+
+	write_rtl8225(dev, 0x4, 0x8c3);mdelay(1);
+
+
+
+	write_rtl8225(dev, 0x5, 0xc72);mdelay(1);
+//	}
+
+	write_rtl8225(dev, 0x6, 0xe6);  mdelay(1);
+
+	write_rtl8225(dev, 0x7, ((priv->card_type == USB)? 0x82a : rtl8225_chan[channel]));  mdelay(1);
+
+	write_rtl8225(dev, 0x8, 0x3f);  mdelay(1);
+
+	write_rtl8225(dev, 0x9, 0x335);  mdelay(1);
+
+	write_rtl8225(dev, 0xa, 0x9d4);  mdelay(1);
+
+	write_rtl8225(dev, 0xb, 0x7bb);  mdelay(1);
+
+	write_rtl8225(dev, 0xc, 0x850);  mdelay(1);
+
+
+	write_rtl8225(dev, 0xd, 0xcdf);   mdelay(1);
+
+	write_rtl8225(dev, 0xe, 0x2b);  mdelay(1);
+
+	write_rtl8225(dev, 0xf, 0x114);
+
+
+	mdelay(100);
+
+
+	//if(priv->card_type != USB) /* maybe not needed even for 8185 */
+//	write_rtl8225(dev, 0x7, rtl8225_chan[channel]);
+
+	write_rtl8225(dev, 0x0, 0x1b7);
+
+	for(i=0;i<95;i++){
+		write_rtl8225(dev, 0x1, (u8)(i+1));
+
+		#if 0
+		if(priv->phy_ver == 1)
+			/* version A */
+			write_rtl8225(dev, 0x2, rtl8225a_rxgain[i]);
+		else
+		#endif
+		/* version B & C & D*/
+
+		write_rtl8225(dev, 0x2, rtl8225z2_rxgain[i]);
+	}
+	write_rtl8225(dev, 0x3, 0x80);
+	write_rtl8225(dev, 0x5, 0x4);
+
+	write_rtl8225(dev, 0x0, 0xb7);
+
+	write_rtl8225(dev, 0x2, 0xc4d);
+
+	if(priv->card_type == USB){
+	//	force_pci_posting(dev);
+		mdelay(200);
+
+		write_rtl8225(dev, 0x2, 0x44d);
+
+	//	force_pci_posting(dev);
+		mdelay(100);
+
+	}//End of if(priv->card_type == USB)
+	/* FIXME!! rtl8187 we have to check if calibrarion
+	 * is successful and eventually cal. again (repeat
+	 * the two write on reg 2)
+	*/
+	// Check for calibration status, 2005.11.17,
+        data = read_rtl8225(dev, 6);
+        if (!(data&0x00000080))
+        {
+                write_rtl8225(dev, 0x02, 0x0c4d);
+                force_pci_posting(dev); mdelay(200);
+                write_rtl8225(dev, 0x02, 0x044d);
+                force_pci_posting(dev); mdelay(100);
+                data = read_rtl8225(dev, 6);
+                if (!(data&0x00000080))
+                        {
+                                DMESGW("RF Calibration Failed!!!!\n");
+                        }
+        }
+	//force_pci_posting(dev);
+
+	mdelay(200); //200 for 8187
+
+
+//	//if(priv->card_type != USB){
+//		write_rtl8225(dev, 0x2, 0x44d);
+//		write_rtl8225(dev, 0x7, rtl8225_chan[channel]);
+//		write_rtl8225(dev, 0x2, 0x47d);
+//
+//		force_pci_posting(dev);
+//		mdelay(100);
+//
+//		write_rtl8225(dev, 0x2, 0x44d);
+//	//}
+
+	write_rtl8225(dev, 0x0, 0x2bf);
+
+	if(priv->card_type != USB)
+		rtl8185_rf_pins_enable(dev);
+	//set up ZEBRA AGC table, 2005.11.17,
+        for(i=0;i<128;i++){
+                data = rtl8225_agc[i];
+
+                addr = i + 0x80; //enable writing AGC table
+                write_phy_ofdm(dev, 0xb, data);
+
+                mdelay(1);
+                write_phy_ofdm(dev, 0xa, addr);
+
+                mdelay(1);
+        }
+#if 0
+	for(i=0;i<128;i++){
+		write_phy_ofdm(dev, 0xb, rtl8225_agc[i]);
+
+		mdelay(1);
+		write_phy_ofdm(dev, 0xa, (u8)i+ 0x80);
+
+		mdelay(1);
+	}
+#endif
+
+	force_pci_posting(dev);
+	mdelay(1);
+
+	write_phy_ofdm(dev, 0x0, 0x1); mdelay(1);
+	write_phy_ofdm(dev, 0x1, 0x2); mdelay(1);
+	write_phy_ofdm(dev, 0x2, ((priv->card_type == USB)? 0x42 : 0x62)); mdelay(1);
+	write_phy_ofdm(dev, 0x3, 0x0); mdelay(1);
+	write_phy_ofdm(dev, 0x4, 0x0); mdelay(1);
+	write_phy_ofdm(dev, 0x5, 0x0); mdelay(1);
+	write_phy_ofdm(dev, 0x6, 0x40); mdelay(1);
+	write_phy_ofdm(dev, 0x7, 0x0); mdelay(1);
+	write_phy_ofdm(dev, 0x8, 0x40); mdelay(1);
+	write_phy_ofdm(dev, 0x9, 0xfe); mdelay(1);
+
+	write_phy_ofdm(dev, 0xa, 0x8); mdelay(1);
+
+	//write_phy_ofdm(dev, 0x18, 0xef);
+	//	}
+	//}
+	write_phy_ofdm(dev, 0xb, 0x80); mdelay(1);
+
+	write_phy_ofdm(dev, 0xc, 0x1);mdelay(1);
+
+
+	//if(priv->card_type != USB)
+	write_phy_ofdm(dev, 0xd, 0x43);
+
+	write_phy_ofdm(dev, 0xe, 0xd3);mdelay(1);
+
+
+	#if 0
+	if(priv->card_8185 == 1){
+		if(priv->card_8185_Bversion)
+			write_phy_ofdm(dev, 0xf, 0x20);/*ver B*/
+		else
+			write_phy_ofdm(dev, 0xf, 0x28);/*ver C*/
+	}else{
+	#endif
+	write_phy_ofdm(dev, 0xf, 0x38);mdelay(1);
+/*ver D & 8187*/
+//	}
+
+//	if(priv->card_8185 == 1 && priv->card_8185_Bversion)
+//		write_phy_ofdm(dev, 0x10, 0x04);/*ver B*/
+//	else
+	write_phy_ofdm(dev, 0x10, 0x84);mdelay(1);
+/*ver C & D & 8187*/
+
+	write_phy_ofdm(dev, 0x11, 0x07);mdelay(1);
+/*agc resp time 700*/
+
+
+//	if(priv->card_8185 == 2){
+	/* Ver D & 8187*/
+	write_phy_ofdm(dev, 0x12, 0x20);mdelay(1);
+
+	write_phy_ofdm(dev, 0x13, 0x20);mdelay(1);
+
+#if 0
+	}else{
+		/* Ver B & C*/
+		write_phy_ofdm(dev, 0x12, 0x0);
+		write_phy_ofdm(dev, 0x13, 0x0);
+	}
+#endif
+	write_phy_ofdm(dev, 0x14, 0x0); mdelay(1);
+	write_phy_ofdm(dev, 0x15, 0x40); mdelay(1);
+	write_phy_ofdm(dev, 0x16, 0x0); mdelay(1);
+	write_phy_ofdm(dev, 0x17, 0x40); mdelay(1);
+
+//	if (priv->card_type == USB)
+//		write_phy_ofdm(dev, 0x18, 0xef);
+
+	write_phy_ofdm(dev, 0x18, 0xef);mdelay(1);
+
+
+	write_phy_ofdm(dev, 0x19, 0x19); mdelay(1);
+	write_phy_ofdm(dev, 0x1a, 0x20); mdelay(1);
+	write_phy_ofdm(dev, 0x1b, 0x15);mdelay(1);
+
+	write_phy_ofdm(dev, 0x1c, 0x4);mdelay(1);
+
+	write_phy_ofdm(dev, 0x1d, 0xc5);mdelay(1); //2005.11.17,
+
+	write_phy_ofdm(dev, 0x1e, 0x95);mdelay(1);
+
+	write_phy_ofdm(dev, 0x1f, 0x75);	mdelay(1);
+
+//	}
+
+	write_phy_ofdm(dev, 0x20, 0x1f);mdelay(1);
+
+	write_phy_ofdm(dev, 0x21, 0x17);mdelay(1);
+
+	write_phy_ofdm(dev, 0x22, 0x16);mdelay(1);
+
+//	if(priv->card_type != USB)
+	write_phy_ofdm(dev, 0x23, 0x80);mdelay(1); //FIXME maybe not needed // <>
+
+	write_phy_ofdm(dev, 0x24, 0x46); mdelay(1);
+	write_phy_ofdm(dev, 0x25, 0x00); mdelay(1);
+	write_phy_ofdm(dev, 0x26, 0x90); mdelay(1);
+
+	write_phy_ofdm(dev, 0x27, 0x88); mdelay(1);
+
+
+	// <> Set init. gain to m74dBm.
+
+	rtl8225z2_set_gain(dev,4);
+
+	write_phy_cck(dev, 0x0, 0x98); mdelay(1);
+	write_phy_cck(dev, 0x3, 0x20); mdelay(1);
+	write_phy_cck(dev, 0x4, 0x7e); mdelay(1);
+	write_phy_cck(dev, 0x5, 0x12); mdelay(1);
+	write_phy_cck(dev, 0x6, 0xfc); mdelay(1);
+
+	write_phy_cck(dev, 0x7, 0x78);mdelay(1);
+ /* Ver C & D & 8187*/
+
+	write_phy_cck(dev, 0x8, 0x2e);mdelay(1);
+
+	write_phy_cck(dev, 0x10, ((priv->card_type == USB) ? 0x9b: 0x93)); mdelay(1);
+	write_phy_cck(dev, 0x11, 0x88); mdelay(1);
+	write_phy_cck(dev, 0x12, 0x47); mdelay(1);
+#if 0
+	if(priv->card_8185 == 1 && priv->card_8185_Bversion)
+		write_phy_cck(dev, 0x13, 0x98); /* Ver B */
+	else
+#endif
+	write_phy_cck(dev, 0x13, 0xd0); /* Ver C & D & 8187*/
+
+	write_phy_cck(dev, 0x19, 0x0);
+	write_phy_cck(dev, 0x1a, 0xa0);
+	write_phy_cck(dev, 0x1b, 0x8);
+	write_phy_cck(dev, 0x40, 0x86); /* CCK Carrier Sense Threshold */
+
+	write_phy_cck(dev, 0x41, 0x8d);mdelay(1);
+
+
+	write_phy_cck(dev, 0x42, 0x15); mdelay(1);
+	write_phy_cck(dev, 0x43, 0x18); mdelay(1);
+
+
+	write_phy_cck(dev, 0x44, 0x36); mdelay(1);
+	write_phy_cck(dev, 0x45, 0x35); mdelay(1);
+	write_phy_cck(dev, 0x46, 0x2e); mdelay(1);
+	write_phy_cck(dev, 0x47, 0x25); mdelay(1);
+	write_phy_cck(dev, 0x48, 0x1c); mdelay(1);
+	write_phy_cck(dev, 0x49, 0x12); mdelay(1);
+	write_phy_cck(dev, 0x4a, 0x9); mdelay(1);
+	write_phy_cck(dev, 0x4b, 0x4); mdelay(1);
+	write_phy_cck(dev, 0x4c, 0x5);mdelay(1);
+
+
+	write_nic_byte(dev, 0x5b, 0x0d); mdelay(1);
+
+
+
+// <>
+//	// TESTR 0xb 8187
+//	write_phy_cck(dev, 0x10, 0x93);// & 0xfb);
+//
+//	//if(priv->card_type != USB){
+//		write_phy_ofdm(dev, 0x2, 0x62);
+//		write_phy_ofdm(dev, 0x6, 0x0);
+//		write_phy_ofdm(dev, 0x8, 0x0);
+//	//}
+
+	rtl8225z2_SetTXPowerLevel(dev, channel);
+#ifdef CONFIG_RTL818X_S
+        write_phy_cck(dev, 0x11, 0x9b); mdelay(1); /* Rx ant A, 0xdb for B */
+#else
+	write_phy_cck(dev, 0x10, 0x9b); mdelay(1); /* Rx ant A, 0xdb for B */
+#endif
+	write_phy_ofdm(dev, 0x26, 0x90); mdelay(1); /* Rx ant A, 0x10 for B */
+
+	rtl8185_tx_antenna(dev, 0x3); /* TX ant A, 0x0 for B */
+
+	/* switch to high-speed 3-wire
+	 * last digit. 2 for both cck and ofdm
+	 */
+	if(priv->card_type == USB)
+		write_nic_dword(dev, 0x94, 0x3dc00002);
+	else{
+		write_nic_dword(dev, 0x94, 0x15c00002);
+		rtl8185_rf_pins_enable(dev);
+	}
+
+//	if(priv->card_type != USB)
+//	rtl8225_set_gain(dev, 4); /* FIXME this '1' is random */ // <>
+//	 rtl8225_set_mode(dev, 1); /* FIXME start in B mode */ // <>
+//
+//	/* make sure is waken up! */
+//	write_rtl8225(dev,0x4, 0x9ff);
+//	rtl8180_set_anaparam(dev, RTL8225_ANAPARAM_ON);
+//	rtl8185_set_anaparam2(dev, RTL8225_ANAPARAM2_ON);
+
+	rtl8225_rf_set_chan(dev, priv->chan);
+
+	//write_nic_word(dev,BRSR,brsr);
+
+	//rtl8225z2_rf_set_mode(dev);
+}
+
+void rtl8225z2_rf_set_mode(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	if(priv->ieee80211->mode == IEEE_A)
+	{
+		write_rtl8225(dev, 0x5, 0x1865);
+		write_nic_dword(dev, RF_PARA, 0x10084);
+		write_nic_dword(dev, RF_TIMING, 0xa8008);
+		write_phy_ofdm(dev, 0x0, 0x0);
+		write_phy_ofdm(dev, 0xa, 0x6);
+		write_phy_ofdm(dev, 0xb, 0x99);
+		write_phy_ofdm(dev, 0xf, 0x20);
+		write_phy_ofdm(dev, 0x11, 0x7);
+
+		rtl8225z2_set_gain(dev,4);
+
+		write_phy_ofdm(dev,0x15, 0x40);
+		write_phy_ofdm(dev,0x17, 0x40);
+
+		write_nic_dword(dev, 0x94,0x10000000);
+	}else{
+
+		write_rtl8225(dev, 0x5, 0x1864);
+		write_nic_dword(dev, RF_PARA, 0x10044);
+		write_nic_dword(dev, RF_TIMING, 0xa8008);
+		write_phy_ofdm(dev, 0x0, 0x1);
+		write_phy_ofdm(dev, 0xa, 0x6);
+		write_phy_ofdm(dev, 0xb, 0x99);
+		write_phy_ofdm(dev, 0xf, 0x20);
+		write_phy_ofdm(dev, 0x11, 0x7);
+
+		rtl8225z2_set_gain(dev,4);
+
+		write_phy_ofdm(dev,0x15, 0x40);
+		write_phy_ofdm(dev,0x17, 0x40);
+
+		write_nic_dword(dev, 0x94,0x04000002);
+	}
+}
+
+//lzm mod 080826
+//#define MAX_DOZE_WAITING_TIMES_85B 64
+//#define MAX_POLLING_24F_TIMES_87SE 	5
+#define MAX_DOZE_WAITING_TIMES_85B 		20
+#define MAX_POLLING_24F_TIMES_87SE 			10
+#define LPS_MAX_SLEEP_WAITING_TIMES_87SE 	5
+
+bool
+SetZebraRFPowerState8185(
+	struct net_device *dev,
+	RT_RF_POWER_STATE	eRFPowerState
+	)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	u8			btCR9346, btConfig3;
+	bool bActionAllowed= true, bTurnOffBB = true;//lzm mod 080826
+	//u32			DWordContent;
+	u8			u1bTmp;
+	int			i;
+	//u16			u2bTFPC = 0;
+	bool		bResult = true;
+	u8			QueueID;
+
+	if(priv->SetRFPowerStateInProgress == true)
+		return false;
+
+	priv->SetRFPowerStateInProgress = true;
+
+	// enable EEM0 and EEM1 in 9346CR
+	btCR9346 = read_nic_byte(dev, CR9346);
+	write_nic_byte(dev, CR9346, (btCR9346|0xC0) );
+	// enable PARM_En in Config3
+	btConfig3 = read_nic_byte(dev, CONFIG3);
+	write_nic_byte(dev, CONFIG3, (btConfig3|CONFIG3_PARM_En) );
+
+	switch( priv->rf_chip )
+	{
+	case RF_ZEBRA2:
+		switch( eRFPowerState )
+		{
+		case eRfOn:
+			RF_WriteReg(dev,0x4,0x9FF);
+
+			write_nic_dword(dev, ANAPARAM, ANAPARM_ON);
+			write_nic_dword(dev, ANAPARAM2, ANAPARM2_ON);
+
+			write_nic_byte(dev, CONFIG4, priv->RFProgType);
+
+			//Follow 87B, Isaiah 2007-04-27
+			u1bTmp = read_nic_byte(dev, 0x24E);
+			write_nic_byte(dev, 0x24E, (u1bTmp & (~(BIT5|BIT6))) );// 070124 SD1 Alex: turn on CCK and OFDM.
+			break;
+
+		case eRfSleep:
+			break;
+
+		case eRfOff:
+			break;
+
+		default:
+			bResult = false;
+			break;
+		}
+		break;
+
+	case RF_ZEBRA4:
+		switch( eRFPowerState )
+		{
+		case eRfOn:
+			//printk("===================================power on@jiffies:%d\n",jiffies);
+			write_nic_word(dev, 0x37C, 0x00EC);
+
+			//turn on AFE
+			write_nic_byte(dev, 0x54, 0x00);
+			write_nic_byte(dev, 0x62, 0x00);
+
+			//lzm mod 080826
+			//turn on RF
+			//RF_WriteReg(dev, 0x0, 0x009f); //mdelay(1);
+			//RF_WriteReg(dev, 0x4, 0x0972); //mdelay(1);
+			RF_WriteReg(dev, 0x0, 0x009f); udelay(500);
+			RF_WriteReg(dev, 0x4, 0x0972); udelay(500);
+			//turn on RF again, suggested by SD3 stevenl.
+			RF_WriteReg(dev, 0x0, 0x009f); udelay(500);
+			RF_WriteReg(dev, 0x4, 0x0972); udelay(500);
+
+			//turn on BB
+//			write_nic_dword(dev, PhyAddr, 0x4090); //ofdm 10=00
+//			write_nic_dword(dev, PhyAddr, 0x4092); //ofdm 12=00
+			write_phy_ofdm(dev,0x10,0x40);
+			write_phy_ofdm(dev,0x12,0x40);
+			//Avoid power down at init time.
+			write_nic_byte(dev, CONFIG4, priv->RFProgType);
+
+			u1bTmp = read_nic_byte(dev, 0x24E);
+			write_nic_byte(dev, 0x24E, (u1bTmp & (~(BIT5|BIT6))) );
+
+			break;
+
+		case eRfSleep:
+			// Make sure BusyQueue is empty befor turn off RFE pwoer.
+			//printk("===================================power sleep@jiffies:%d\n",jiffies);
+
+			for(QueueID = 0, i = 0; QueueID < 6; )
+			{
+				if(get_curr_tx_free_desc(dev,QueueID) == priv->txringcount)
+				{
+					QueueID++;
+					continue;
+				}
+#if 0		//reserved amy
+				else if(priv->NdisAdapter.CurrentPowerState != NdisDeviceStateD0)
+				{
+					RT_TRACE(COMP_POWER, DBG_LOUD, ("eRfSleep: %d times TcbBusyQueue[%d] !=0 but lower power state!\n", (pMgntInfo->TxPollingTimes+1), QueueID));
+					break;
+				}
+#endif
+				else//lzm mod 080826
+				{
+					priv->TxPollingTimes ++;
+					if(priv->TxPollingTimes >= LPS_MAX_SLEEP_WAITING_TIMES_87SE)
+						{
+							//RT_TRACE(COMP_POWER, DBG_WARNING, ("\n\n\n SetZebraRFPowerState8185B():eRfSleep:  %d times TcbBusyQueue[%d] != 0 !!!\n\n\n", LPS_MAX_SLEEP_WAITING_TIMES_87SE, QueueID));
+							bActionAllowed=false;
+							break;
+						}
+						else
+						{
+							udelay(10);  // Windows may delay 3~16ms actually.
+							//RT_TRACE(COMP_POWER, DBG_LOUD, ("eRfSleep: %d times TcbBusyQueue[%d] !=0 before doze!\n", (pMgntInfo->TxPollingTimes), QueueID));
+						}
+				}
+
+				//lzm del 080826
+				//if(i >= MAX_DOZE_WAITING_TIMES_85B)
+				//{
+					//printk("\n\n\n SetZebraRFPowerState8185B(): %d times BusyQueue[%d] != 0 !!!\n\n\n", MAX_DOZE_WAITING_TIMES_85B, QueueID);
+					//break;
+				//}
+			}
+
+			if(bActionAllowed)//lzm add 080826
+			{
+				//turn off BB RXIQ matrix to cut off rx signal
+//				write_nic_dword(dev, PhyAddr, 0x0090); //ofdm 10=00
+//				write_nic_dword(dev, PhyAddr, 0x0092); //ofdm 12=00
+				write_phy_ofdm(dev,0x10,0x00);
+				write_phy_ofdm(dev,0x12,0x00);
+				//turn off RF
+				RF_WriteReg(dev, 0x4, 0x0000); //mdelay(1);
+				RF_WriteReg(dev, 0x0, 0x0000); //mdelay(1);
+				//turn off AFE except PLL
+				write_nic_byte(dev, 0x62, 0xff);
+				write_nic_byte(dev, 0x54, 0xec);
+//				mdelay(10);
+
+#if 1
+				mdelay(1);
+				{
+					int i = 0;
+					while (true)
+					{
+						u8 tmp24F = read_nic_byte(dev, 0x24f);
+						if ((tmp24F == 0x01) || (tmp24F == 0x09))
+						{
+							bTurnOffBB = true;
+							break;
+						}
+						else//lzm mod 080826
+						{
+							udelay(10);
+							i++;
+							priv->TxPollingTimes++;
+
+							if(priv->TxPollingTimes >= LPS_MAX_SLEEP_WAITING_TIMES_87SE)
+							{
+								//RT_TRACE(COMP_POWER, DBG_WARNING, ("\n\n\n SetZebraRFPowerState8185B(): eRfOff: %d times Rx Mac0x24F=0x%x !!!\n\n\n", i, u1bTmp24F));
+								bTurnOffBB=false;
+								break;
+							}
+							else
+							{
+								udelay(10);// Windows may delay 3~16ms actually.
+								//RT_TRACE(COMP_POWER, DBG_LOUD,("(%d)eRfSleep- u1bTmp24F= 0x%X\n", i, u1bTmp24F));
+
+							}
+						}
+
+						//lzm del 080826
+						//if (i > MAX_POLLING_24F_TIMES_87SE)
+						//	break;
+					}
+				}
+#endif
+				if (bTurnOffBB)//lzm mod 080826
+				{
+				//turn off BB
+				u1bTmp = read_nic_byte(dev, 0x24E);
+				write_nic_byte(dev, 0x24E, (u1bTmp|BIT5|BIT6));
+
+				//turn off AFE PLL
+				//write_nic_byte(dev, 0x54, 0xec);
+				//write_nic_word(dev, 0x37C, 0x00ec);
+				write_nic_byte(dev, 0x54, 0xFC);  //[ECS] FC-> EC->FC, asked by SD3 Stevenl
+				write_nic_word(dev, 0x37C, 0x00FC);//[ECS] FC-> EC->FC, asked by SD3 Stevenl
+				}
+			}
+			break;
+
+		case eRfOff:
+			// Make sure BusyQueue is empty befor turn off RFE pwoer.
+			//printk("===================================power off@jiffies:%d\n",jiffies);
+			for(QueueID = 0, i = 0; QueueID < 6; )
+			{
+				if(get_curr_tx_free_desc(dev,QueueID) == priv->txringcount)
+				{
+					QueueID++;
+					continue;
+				}
+#if 0
+				else if(Adapter->NdisAdapter.CurrentPowerState != NdisDeviceStateD0)
+				{
+					RT_TRACE(COMP_POWER, DBG_LOUD, ("%d times TcbBusyQueue[%d] !=0 but lower power state!\n", (i+1), QueueID));
+					break;
+				}
+#endif
+				else
+				{
+					udelay(10);
+					i++;
+				}
+
+				if(i >= MAX_DOZE_WAITING_TIMES_85B)
+				{
+					//printk("\n\n\n SetZebraRFPowerState8185B(): %d times BusyQueue[%d] != 0 !!!\n\n\n", MAX_DOZE_WAITING_TIMES_85B, QueueID);
+					break;
+				}
+			}
+
+			//turn off BB RXIQ matrix to cut off rx signal
+//			write_nic_dword(dev, PhyAddr, 0x0090); //ofdm 10=00
+//			write_nic_dword(dev, PhyAddr, 0x0092); //ofdm 12=00
+			write_phy_ofdm(dev,0x10,0x00);
+			write_phy_ofdm(dev,0x12,0x00);
+			//turn off RF
+			RF_WriteReg(dev, 0x4, 0x0000); //mdelay(1);
+			RF_WriteReg(dev, 0x0, 0x0000); //mdelay(1);
+			//turn off AFE except PLL
+			write_nic_byte(dev, 0x62, 0xff);
+			write_nic_byte(dev, 0x54, 0xec);
+//			mdelay(10);
+#if 1
+			mdelay(1);
+			{
+				int i = 0;
+				while (true)
+				{
+					u8 tmp24F = read_nic_byte(dev, 0x24f);
+					if ((tmp24F == 0x01) || (tmp24F == 0x09))
+					{
+						bTurnOffBB = true;
+						break;
+					}
+					else
+					{
+						bTurnOffBB = false;
+						udelay(10);
+						i++;
+					}
+					if (i > MAX_POLLING_24F_TIMES_87SE)
+						break;
+				}
+			}
+#endif
+			if (bTurnOffBB)//lzm mod 080826
+			{
+
+			//turn off BB
+			u1bTmp = read_nic_byte(dev, 0x24E);
+			write_nic_byte(dev, 0x24E, (u1bTmp|BIT5|BIT6));
+			//turn off AFE PLL (80M)
+			//write_nic_byte(dev, 0x54, 0xec);
+			//write_nic_word(dev, 0x37C, 0x00ec);
+			write_nic_byte(dev, 0x54, 0xFC); //[ECS] FC-> EC->FC, asked by SD3 Stevenl
+			write_nic_word(dev, 0x37C, 0x00FC); //[ECS] FC-> EC->FC, asked by SD3 Stevenl
+			}
+
+			break;
+
+		default:
+			bResult = false;
+			printk("SetZebraRFPowerState8185(): unknow state to set: 0x%X!!!\n", eRFPowerState);
+			break;
+		}
+		break;
+	}
+
+	// disable PARM_En in Config3
+	btConfig3 &= ~(CONFIG3_PARM_En);
+	write_nic_byte(dev, CONFIG3, btConfig3);
+	// disable EEM0 and EEM1 in 9346CR
+	btCR9346 &= ~(0xC0);
+	write_nic_byte(dev, CR9346, btCR9346);
+
+	if(bResult && bActionAllowed)//lzm mod 080826
+	{
+		// Update current RF state variable.
+		priv->eRFPowerState = eRFPowerState;
+#if 0
+		switch(priv->eRFPowerState)
+		{
+		case eRfOff:
+			//
+			//If Rf off reason is from IPS, Led should blink with no link, by Maddest 071015
+			//
+			if(priv->RfOffReason==RF_CHANGE_BY_IPS )
+			{
+				Adapter->HalFunc.LedControlHandler(Adapter,LED_CTL_NO_LINK);
+			}
+			else
+			{
+				// Turn off LED if RF is not ON.
+				Adapter->HalFunc.LedControlHandler(Adapter, LED_CTL_POWER_OFF);
+			}
+			break;
+
+		case eRfOn:
+			// Turn on RF we are still linked, which might happen when
+			// we quickly turn off and on HW RF. 2006.05.12, by rcnjko.
+			if( pMgntInfo->bMediaConnect == TRUE )
+			{
+				Adapter->HalFunc.LedControlHandler(Adapter, LED_CTL_LINK);
+			}
+			break;
+
+		default:
+			// do nothing.
+			break;
+		}
+#endif
+
+	}
+
+	priv->SetRFPowerStateInProgress = false;
+
+	return (bResult && bActionAllowed) ;
+}
+void rtl8225z4_rf_sleep(struct net_device *dev)
+{
+	//
+	// Turn off RF power.
+	//
+	//printk("=========>%s()\n", __FUNCTION__);
+	MgntActSet_RF_State(dev, eRfSleep, RF_CHANGE_BY_PS);
+	//mdelay(2);	//FIXME
+}
+void rtl8225z4_rf_wakeup(struct net_device *dev)
+{
+	//
+	// Turn on RF power.
+	//
+	//printk("=========>%s()\n", __FUNCTION__);
+	MgntActSet_RF_State(dev, eRfOn, RF_CHANGE_BY_PS);
+}
+#endif
+
diff --git a/drivers/staging/rtl8187se/r8180_rtl8255.c b/drivers/staging/rtl8187se/r8180_rtl8255.c
new file mode 100644
index 00000000000..1a62444dcc5
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_rtl8255.c
@@ -0,0 +1,1838 @@
+/*
+  This is part of the rtl8180-sa2400 driver
+  released under the GPL (See file COPYING for details).
+  Copyright (c) 2005 Andrea Merello <andreamrl@tiscali.it>
+
+  This files contains programming code for the rtl8255
+  radio frontend.
+
+  *Many* thanks to Realtek Corp. for their great support!
+
+*/
+
+#define BAND_A 1
+#define BAND_BG 2
+
+#include "r8180.h"
+#include "r8180_hw.h"
+#include "r8180_rtl8255.h"
+
+u32 rtl8255_chan[] = {
+	0,	//dummy channel 0
+	0x13, //1
+	0x115, //2
+	0x217, //3
+	0x219, //4
+	0x31b, //5
+	0x41d, //6
+	0x41f, //7
+	0x621, //8
+	0x623, //9
+	0x625, //10
+	0x627, //11
+	0x829, //12
+	0x82b, //13
+	0x92f, // 14
+};
+
+static short rtl8255_gain_2G[]={
+	0x33, 0x17, 0x7c, 0xc5,//-78
+	0x43, 0x17, 0x7a, 0xc5,//-74
+	0x53, 0x17, 0x78, 0xc5,//-70
+	0x63, 0x17, 0x76, 0xc5,//-66
+};
+
+
+static short rtl8255_agc[]={
+	0x1, 0x1, 0x1, 0x1, 0x1,         0x1, 0x1, 0x1, 0x1, 0x1,
+
+	0x1, 0x1, 0x2, 0x2, 0x3,         0x3, 0x4, 0x4, 0x5, 0x5,
+	0x6, 0x6, 0x7, 0x7, 0x8,         0x8, 0x9, 0x9, 0xa, 0xa,
+	0xb, 0xb, 0xc, 0xc, 0xd,         0xd, 0xe, 0xe, 0xf, 0xf,
+
+	0x10, 0x10, 0x11, 0x11, 0x12,    0x12, 0x13, 0x13, 0x14, 0x14,
+	0x15, 0x15, 0x16, 0x16, 0x17,    0x17, 0x18, 0x18, 0x19, 0x19,
+	0x1a, 0x1a, 0x1b, 0x1b, 0x1c,    0x1c, 0x1d, 0x1d, 0x1e, 0x1e,
+	0x1f, 0x1f,
+
+	0x20, 0x20, 0x21, 0x21, 0x22,    0x22, 0x23, 0x23, 0x24, 0x24,
+	0x25, 0x25, 0x26, 0x26, 0x27,    0x27, 0x28, 0x28, 0x29, 0x29,
+	0x2a, 0x2a,
+
+	0x2a, 0x2a, 0x2a, 0x2a, 0x2a,    0x2a, 0x2a, 0x2a, 0x2a, 0x2a,
+	0x2a, 0x2a, 0x2a, 0x2a, 0x2a,    0x2a, 0x2a, 0x2a, 0x2a, 0x2a,
+	0x2a, 0x2a, 0x2a, 0x2a, 0x2a,    0x2a, 0x2a, 0x2a, 0x2a, 0x2a,
+	0x2a, 0x2a, 0x2a, 0x2a
+
+};
+
+void rtl8255_set_gain(struct net_device *dev, short gain)
+{
+
+//	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	write_phy_ofdm(dev, 0x0d, rtl8255_gain_2G[gain * 4]);
+	write_phy_ofdm(dev, 0x23, rtl8255_gain_2G[gain * 4 + 1]);
+	write_phy_ofdm(dev, 0x1b, rtl8255_gain_2G[gain * 4 + 2]);
+	write_phy_ofdm(dev, 0x1d, rtl8255_gain_2G[gain * 4 + 3]);
+	//rtl8225_set_gain_usb(dev, gain);
+}
+
+void write_rtl8255_reg0c(struct net_device *dev, u32 d1, u32 d2, u32 d3, u32 d4,
+u32 d5, u32 d6, u32 d7, u32 d8, u32 d9, u32 d10)
+{
+	int i,j;
+	u16 out,select;
+	u8 bit;
+	u32 bangdata;
+//	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	write_nic_word(dev,RFPinsEnable,
+		(read_nic_word(dev,RFPinsEnable) | 0x7));
+
+	select = read_nic_word(dev, RFPinsSelect);
+
+	write_nic_word(dev, RFPinsSelect, select | 0x7 | SW_CONTROL_GPIO);
+
+	out = read_nic_word(dev, RFPinsOutput) & 0xfff3;
+
+	write_nic_word(dev, RFPinsOutput, out | BB_HOST_BANG_EN );//| 0x1fff);
+
+	force_pci_posting(dev);
+	udelay(2);
+
+	write_nic_word(dev, RFPinsOutput, out);
+
+	force_pci_posting(dev);
+	udelay(2);
+
+	for(j=0;j<10;j++)
+	{
+		switch(j)
+		{
+			case 9:
+			bangdata = d10 | 0x0c;
+			break;
+			case 8:
+			bangdata = d9;
+			break;
+			case 7:
+			bangdata = d8;
+			break;
+			case 6:
+			bangdata = d7;
+			break;
+			case 5:
+			bangdata = d6;
+			break;
+			case 4:
+			bangdata = d5;
+			break;
+			case 3:
+			bangdata = d4;
+			break;
+			case 2:
+			bangdata = d3;
+			break;
+			case 1:
+			bangdata = d2;
+			break;
+			case 0:
+			bangdata = d1;
+			break;
+			default:
+			bangdata=0xbadc0de; /* avoid gcc complaints */
+			break;
+		}
+
+		for(i=31; i>=0;i--){
+
+			bit = (bangdata & (1<<i)) >> i;
+
+			write_nic_word(dev, RFPinsOutput, bit | out);
+			force_pci_posting(dev);
+			udelay(1);
+			write_nic_word(dev, RFPinsOutput, bit | out | BB_HOST_BANG_CLK);
+			force_pci_posting(dev);
+			udelay(1);
+		//	write_nic_word(dev, RFPinsOutput, bit | out | BB_HOST_BANG_CLK);
+			i--;
+			bit = (bangdata & (1<<i)) >> i;
+
+			write_nic_word(dev, RFPinsOutput, bit | out | BB_HOST_BANG_CLK);
+			force_pci_posting(dev);
+			udelay(1);
+		//	write_nic_word(dev, RFPinsOutput, bit | out | BB_HOST_BANG_CLK);
+			write_nic_word(dev, RFPinsOutput, bit | out);
+			force_pci_posting(dev);
+			udelay(1);
+		}
+	}
+
+	write_nic_word(dev, RFPinsOutput, out | BB_HOST_BANG_EN);
+	force_pci_posting(dev);
+	udelay(10);
+
+//	write_nic_word(dev, RFPinsOutput, out | BB_HOST_BANG_EN);
+	write_nic_word(dev, RFPinsSelect, select | SW_CONTROL_GPIO);
+//	rtl8185_rf_pins_enable(dev);
+
+}
+
+void write_rtl8255(struct net_device *dev, u8 adr, u16 data)
+{
+	int i;
+	u16 out,select;
+	u8 bit;
+	u32 bangdata = (data << 4) | (adr & 0xf);
+//	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	out = read_nic_word(dev, RFPinsOutput) & 0xfff3;
+
+	write_nic_word(dev,RFPinsEnable,
+		(read_nic_word(dev,RFPinsEnable) | 0x7));
+
+	select = read_nic_word(dev, RFPinsSelect);
+
+	write_nic_word(dev, RFPinsSelect, select | 0x7 | SW_CONTROL_GPIO);
+
+	force_pci_posting(dev);
+	udelay(10);
+
+	write_nic_word(dev, RFPinsOutput, out | BB_HOST_BANG_EN );//| 0x1fff);
+
+	force_pci_posting(dev);
+	udelay(2);
+
+	write_nic_word(dev, RFPinsOutput, out);
+
+	force_pci_posting(dev);
+	udelay(10);
+
+
+	for(i=15; i>=0;i--){
+
+		bit = (bangdata & (1<<i)) >> i;
+
+		write_nic_word(dev, RFPinsOutput, bit | out);
+		write_nic_word(dev, RFPinsOutput, bit | out | BB_HOST_BANG_CLK);
+		write_nic_word(dev, RFPinsOutput, bit | out | BB_HOST_BANG_CLK);
+		i--;
+		bit = (bangdata & (1<<i)) >> i;
+
+		write_nic_word(dev, RFPinsOutput, bit | out | BB_HOST_BANG_CLK);
+		write_nic_word(dev, RFPinsOutput, bit | out | BB_HOST_BANG_CLK);
+		write_nic_word(dev, RFPinsOutput, bit | out);
+	}
+
+
+	write_nic_word(dev, RFPinsOutput, out | BB_HOST_BANG_EN);
+
+	force_pci_posting(dev);
+	udelay(10);
+
+	write_nic_word(dev, RFPinsOutput, out | BB_HOST_BANG_EN);
+	write_nic_word(dev, RFPinsSelect, select | SW_CONTROL_GPIO);
+
+	rtl8185_rf_pins_enable(dev);
+}
+
+void rtl8255_rf_close(struct net_device *dev)
+{
+
+//	rtl8180_set_anaparam(dev, RTL8225_ANAPARAM_OFF);
+//	rtl8185_set_anaparam2(dev, RTL8225_ANAPARAM2_OFF);
+}
+
+void rtl8255_SetTXPowerLevel(struct net_device *dev, short ch)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	u8 cck_power_level = 0xff & priv->chtxpwr[ch];
+	u8 ofdm_power_level = 0xff & priv->chtxpwr_ofdm[ch];
+	write_nic_byte(dev, TX_GAIN_OFDM, ofdm_power_level);
+	write_nic_byte(dev, TX_GAIN_CCK, cck_power_level);
+	force_pci_posting(dev);
+	mdelay(1);
+	//write_nic_byte(dev, TX_AGC_CONTROL,4);
+}
+#if 0
+/* switch between mode B and G */
+void rtl8255_set_mode(struct net_device *dev, short modeb)
+{
+	write_phy_ofdm(dev, 0x15, (modeb ? 0x0 : 0x40));
+	write_phy_ofdm(dev, 0x17, (modeb ? 0x0 : 0x40));
+}
+#endif
+
+void rtl8255_rf_set_chan(struct net_device *dev, short ch)
+{
+	//write_rtl8225(dev, 0x7, rtl8225_chan[1]);
+	write_rtl8255(dev, 0x5, 0x65);
+	write_rtl8255(dev, 0x6, rtl8255_chan[ch]);
+	write_rtl8255(dev, 0x7, 0x7c);
+	write_rtl8255(dev, 0x8, 0x6);
+
+
+	force_pci_posting(dev);
+	set_current_state(TASK_INTERRUPTIBLE);
+	schedule_timeout(HZ);
+//	rtl8225_set_mode_B(dev);
+
+	rtl8255_SetTXPowerLevel(dev, ch);
+	/* FIXME FIXME FIXME */
+
+	#if 0
+	write_nic_byte(dev,DIFS,0xe); //DIFS
+	write_nic_byte(dev,SLOT,0x14); //SLOT
+	write_nic_byte(dev,EIFS,0x5b); // EIFS
+	//write_nic_byte(dev,0xbc,0); //CW CONFIG
+	write_nic_byte(dev,0xbd,0xa4); //CW VALUE
+	//write_nic_byte(dev,TX_AGC_CONTROL,4);
+	//write_nic_byte(dev, 0x9d,7);
+//Apr 20 13:25:03 localhost kernel: w8. 409d<-7  // CCK AGC
+	/*write_nic_word(dev,0x84,0x488);
+	write_nic_byte(dev,0x91,0x3e);
+	write_nic_byte(dev,0x90,0x30);
+	write_nic_word(dev,0x84,0x488);
+	write_nic_byte(dev,0x91,0x3e);
+	write_nic_byte(dev,0x90,0x20);
+	*/
+	//mdelay(100);
+	#endif
+}
+
+void rtl8255_init_BGband(struct net_device *dev)
+{
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x804187cf, 0x40000027,
+		0x92402ac0, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc00);
+	write_rtl8255(dev, 0x4, 0xe00);
+	write_rtl8255(dev, 0x4, 0xc00);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x800);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa00);
+	write_rtl8255(dev, 0x4, 0x800);
+	write_rtl8255(dev, 0x4, 0x400);
+	write_rtl8255(dev, 0x3, 0x26);
+	write_rtl8255(dev, 0x2, 0x27);
+	write_rtl8255(dev, 0x4, 0x600);
+	write_rtl8255(dev, 0x4, 0x400);
+	write_rtl8255(dev, 0x4, 0x400);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x600);
+	write_rtl8255(dev, 0x4, 0x400);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x804187ce, 0x80000027,
+		0x92402ac0, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc01);
+	write_rtl8255(dev, 0x4, 0xe01);
+	write_rtl8255(dev, 0x4, 0xc01);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x801);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa01);
+	write_rtl8255(dev, 0x4, 0x801);
+	write_rtl8255(dev, 0x4, 0x401);
+	write_rtl8255(dev, 0x3, 0x26);
+	write_rtl8255(dev, 0x2, 0x27);
+	write_rtl8255(dev, 0x4, 0x601);
+	write_rtl8255(dev, 0x4, 0x401);
+	write_rtl8255(dev, 0x4, 0x401);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x601);
+	write_rtl8255(dev, 0x4, 0x401);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80418bdf, 0x40000027,
+		0x92402ac4, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc02);
+	write_rtl8255(dev, 0x4, 0xe02);
+	write_rtl8255(dev, 0x4, 0xc02);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x802);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa02);
+	write_rtl8255(dev, 0x4, 0x802);
+	write_rtl8255(dev, 0x4, 0x402);
+	write_rtl8255(dev, 0x3, 0x26);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x602);
+	write_rtl8255(dev, 0x4, 0x402);
+	write_rtl8255(dev, 0x4, 0x402);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x602);
+	write_rtl8255(dev, 0x4, 0x402);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80418bbf, 0x40000027,
+		0x92402ac4, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc03);
+	write_rtl8255(dev, 0x4, 0xe03);
+	write_rtl8255(dev, 0x4, 0xc03);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x803);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa03);
+	write_rtl8255(dev, 0x4, 0x803);
+	write_rtl8255(dev, 0x4, 0x403);
+	write_rtl8255(dev, 0x3, 0x26);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x603);
+	write_rtl8255(dev, 0x4, 0x403);
+	write_rtl8255(dev, 0x4, 0x403);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x603);
+	write_rtl8255(dev, 0x4, 0x403);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80418b9f, 0x40000027,
+		0x92402ac8, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc04);
+	write_rtl8255(dev, 0x4, 0xe04);
+	write_rtl8255(dev, 0x4, 0xc04);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x804);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa04);
+	write_rtl8255(dev, 0x4, 0x804);
+	write_rtl8255(dev, 0x4, 0x404);
+	write_rtl8255(dev, 0x3, 0x26);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x604);
+	write_rtl8255(dev, 0x4, 0x404);
+	write_rtl8255(dev, 0x4, 0x404);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x604);
+	write_rtl8255(dev, 0x4, 0x404);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x804183df, 0x40000027,
+		0x92402ac8, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc05);
+	write_rtl8255(dev, 0x4, 0xe05);
+	write_rtl8255(dev, 0x4, 0xc05);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x805);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa05);
+	write_rtl8255(dev, 0x4, 0x805);
+	write_rtl8255(dev, 0x4, 0x405);
+	write_rtl8255(dev, 0x3, 0x26);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x605);
+	write_rtl8255(dev, 0x4, 0x405);
+	write_rtl8255(dev, 0x4, 0x405);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x605);
+	write_rtl8255(dev, 0x4, 0x405);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x804183cf, 0x27,
+		0x92402acc, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc06);
+	write_rtl8255(dev, 0x4, 0xe06);
+	write_rtl8255(dev, 0x4, 0xc06);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x806);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa06);
+	write_rtl8255(dev, 0x4, 0x806);
+	write_rtl8255(dev, 0x4, 0x406);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x606);
+	write_rtl8255(dev, 0x4, 0x406);
+	write_rtl8255(dev, 0x4, 0x406);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x606);
+	write_rtl8255(dev, 0x4, 0x406);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x804183af, 0x27,
+		0x92402acc, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc07);
+	write_rtl8255(dev, 0x4, 0xe07);
+	write_rtl8255(dev, 0x4, 0xc07);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x807);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa07);
+	write_rtl8255(dev, 0x4, 0x807);
+	write_rtl8255(dev, 0x4, 0x407);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x607);
+	write_rtl8255(dev, 0x4, 0x407);
+	write_rtl8255(dev, 0x4, 0x407);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x607);
+	write_rtl8255(dev, 0x4, 0x407);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x804083d7, 0x40000027,
+		0x92402ad0, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc08);
+	write_rtl8255(dev, 0x4, 0xe08);
+	write_rtl8255(dev, 0x4, 0xc08);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x808);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa08);
+	write_rtl8255(dev, 0x4, 0x808);
+	write_rtl8255(dev, 0x4, 0x408);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x608);
+	write_rtl8255(dev, 0x4, 0x408);
+	write_rtl8255(dev, 0x4, 0x408);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x608);
+	write_rtl8255(dev, 0x4, 0x408);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x804083c7, 0x27,
+		0x92402ad0, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc09);
+	write_rtl8255(dev, 0x4, 0xe09);
+	write_rtl8255(dev, 0x4, 0xc09);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x809);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa09);
+	write_rtl8255(dev, 0x4, 0x809);
+	write_rtl8255(dev, 0x4, 0x409);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x609);
+	write_rtl8255(dev, 0x4, 0x409);
+	write_rtl8255(dev, 0x4, 0x409);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x609);
+	write_rtl8255(dev, 0x4, 0x409);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x804043d7, 0x40000027,
+		0x92402ad4, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc0a);
+	write_rtl8255(dev, 0x4, 0xe0a);
+	write_rtl8255(dev, 0x4, 0xc0a);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x80a);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa0a);
+	write_rtl8255(dev, 0x4, 0x80a);
+	write_rtl8255(dev, 0x4, 0x40a);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x60a);
+	write_rtl8255(dev, 0x4, 0x40a);
+	write_rtl8255(dev, 0x4, 0x40a);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x60a);
+	write_rtl8255(dev, 0x4, 0x40a);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x804043d7, 0x40000027,
+		0x92402ad4, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc0b);
+	write_rtl8255(dev, 0x4, 0xe0b);
+	write_rtl8255(dev, 0x4, 0xc0b);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x80b);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa0b);
+	write_rtl8255(dev, 0x4, 0x80b);
+	write_rtl8255(dev, 0x4, 0x40b);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x60b);
+	write_rtl8255(dev, 0x4, 0x40b);
+	write_rtl8255(dev, 0x4, 0x40b);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x60b);
+	write_rtl8255(dev, 0x4, 0x40b);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x804043c7, 0x27,
+		0x92402ad8, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc0c);
+	write_rtl8255(dev, 0x4, 0xe0c);
+	write_rtl8255(dev, 0x4, 0xc0c);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x80c);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa0c);
+	write_rtl8255(dev, 0x4, 0x80c);
+	write_rtl8255(dev, 0x4, 0x40c);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x60c);
+	write_rtl8255(dev, 0x4, 0x40c);
+	write_rtl8255(dev, 0x4, 0x40c);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x60c);
+	write_rtl8255(dev, 0x4, 0x40c);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x804043a7, 0x27,
+		0x92402ad8, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc0d);
+	write_rtl8255(dev, 0x4, 0xe0d);
+	write_rtl8255(dev, 0x4, 0xc0d);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x80d);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa0d);
+	write_rtl8255(dev, 0x4, 0x80d);
+	write_rtl8255(dev, 0x4, 0x40d);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x60d);
+	write_rtl8255(dev, 0x4, 0x40d);
+	write_rtl8255(dev, 0x4, 0x40d);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x60d);
+	write_rtl8255(dev, 0x4, 0x40d);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404387, 0x27,
+		0x92402aa8, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc0e);
+	write_rtl8255(dev, 0x4, 0xe0e);
+	write_rtl8255(dev, 0x4, 0xc0e);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x80e);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa0e);
+	write_rtl8255(dev, 0x4, 0x80e);
+	write_rtl8255(dev, 0x4, 0x40e);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x60e);
+	write_rtl8255(dev, 0x4, 0x40e);
+	write_rtl8255(dev, 0x4, 0x40e);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x60e);
+	write_rtl8255(dev, 0x4, 0x40e);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x804041c7, 0x27,
+		0x92402aa8, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc0f);
+	write_rtl8255(dev, 0x4, 0xe0f);
+	write_rtl8255(dev, 0x4, 0xc0f);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x80f);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa0f);
+	write_rtl8255(dev, 0x4, 0x80f);
+	write_rtl8255(dev, 0x4, 0x40f);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x60f);
+	write_rtl8255(dev, 0x4, 0x40f);
+	write_rtl8255(dev, 0x4, 0x40f);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x60f);
+	write_rtl8255(dev, 0x4, 0x40f);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x804041a7, 0x27,
+		0x92402aac, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc10);
+	write_rtl8255(dev, 0x4, 0xe10);
+	write_rtl8255(dev, 0x4, 0xc10);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x810);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa10);
+	write_rtl8255(dev, 0x4, 0x810);
+	write_rtl8255(dev, 0x4, 0x410);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x610);
+	write_rtl8255(dev, 0x4, 0x410);
+	write_rtl8255(dev, 0x4, 0x410);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x610);
+	write_rtl8255(dev, 0x4, 0x410);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404187, 0x27,
+		0x92402aac, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc11);
+	write_rtl8255(dev, 0x4, 0xe11);
+	write_rtl8255(dev, 0x4, 0xc11);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x811);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa11);
+	write_rtl8255(dev, 0x4, 0x811);
+	write_rtl8255(dev, 0x4, 0x411);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x611);
+	write_rtl8255(dev, 0x4, 0x411);
+	write_rtl8255(dev, 0x4, 0x411);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x611);
+	write_rtl8255(dev, 0x4, 0x411);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404186, 0x80000027,
+		0x92402ab0, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc12);
+	write_rtl8255(dev, 0x4, 0xe12);
+	write_rtl8255(dev, 0x4, 0xc12);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x812);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa12);
+	write_rtl8255(dev, 0x4, 0x812);
+	write_rtl8255(dev, 0x4, 0x412);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x612);
+	write_rtl8255(dev, 0x4, 0x412);
+	write_rtl8255(dev, 0x4, 0x412);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x612);
+	write_rtl8255(dev, 0x4, 0x412);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404186, 0x27,
+		0x92402ab0, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc13);
+	write_rtl8255(dev, 0x4, 0xe13);
+	write_rtl8255(dev, 0x4, 0xc13);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x813);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa13);
+	write_rtl8255(dev, 0x4, 0x813);
+	write_rtl8255(dev, 0x4, 0x413);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x613);
+	write_rtl8255(dev, 0x4, 0x413);
+	write_rtl8255(dev, 0x4, 0x413);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x613);
+	write_rtl8255(dev, 0x4, 0x413);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404146, 0x27,
+		0x92402ab4, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc14);
+	write_rtl8255(dev, 0x4, 0xe14);
+	write_rtl8255(dev, 0x4, 0xc14);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x814);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa14);
+	write_rtl8255(dev, 0x4, 0x814);
+	write_rtl8255(dev, 0x4, 0x414);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x614);
+	write_rtl8255(dev, 0x4, 0x414);
+	write_rtl8255(dev, 0x4, 0x414);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x614);
+	write_rtl8255(dev, 0x4, 0x414);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404126, 0x27,
+		0x92402ab4, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc15);
+	write_rtl8255(dev, 0x4, 0xe15);
+	write_rtl8255(dev, 0x4, 0xc15);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x815);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa15);
+	write_rtl8255(dev, 0x4, 0x815);
+	write_rtl8255(dev, 0x4, 0x415);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x615);
+	write_rtl8255(dev, 0x4, 0x415);
+	write_rtl8255(dev, 0x4, 0x415);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x615);
+	write_rtl8255(dev, 0x4, 0x415);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404106, 0x27,
+		0x92402ab8, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc16);
+	write_rtl8255(dev, 0x4, 0xe16);
+	write_rtl8255(dev, 0x4, 0xc16);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x816);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa16);
+	write_rtl8255(dev, 0x4, 0x816);
+	write_rtl8255(dev, 0x4, 0x416);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x616);
+	write_rtl8255(dev, 0x4, 0x416);
+	write_rtl8255(dev, 0x4, 0x416);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x616);
+	write_rtl8255(dev, 0x4, 0x416);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404105, 0x27,
+		0x92402ab8, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc17);
+	write_rtl8255(dev, 0x4, 0xe17);
+	write_rtl8255(dev, 0x4, 0xc17);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x817);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa17);
+	write_rtl8255(dev, 0x4, 0x817);
+	write_rtl8255(dev, 0x4, 0x417);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x617);
+	write_rtl8255(dev, 0x4, 0x417);
+	write_rtl8255(dev, 0x4, 0x417);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x617);
+	write_rtl8255(dev, 0x4, 0x417);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404104, 0x80000027,
+		0x92402a88, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc18);
+	write_rtl8255(dev, 0x4, 0xe18);
+	write_rtl8255(dev, 0x4, 0xc18);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x818);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa18);
+	write_rtl8255(dev, 0x4, 0x818);
+	write_rtl8255(dev, 0x4, 0x418);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x618);
+	write_rtl8255(dev, 0x4, 0x418);
+	write_rtl8255(dev, 0x4, 0x418);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x618);
+	write_rtl8255(dev, 0x4, 0x418);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404104, 0x27,
+		0x92402a88, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc19);
+	write_rtl8255(dev, 0x4, 0xe19);
+	write_rtl8255(dev, 0x4, 0xc19);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x819);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa19);
+	write_rtl8255(dev, 0x4, 0x819);
+	write_rtl8255(dev, 0x4, 0x419);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x619);
+	write_rtl8255(dev, 0x4, 0x419);
+	write_rtl8255(dev, 0x4, 0x419);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x619);
+	write_rtl8255(dev, 0x4, 0x419);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404044, 0x27,
+		0x92402a8c, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc1a);
+	write_rtl8255(dev, 0x4, 0xe1a);
+	write_rtl8255(dev, 0x4, 0xc1a);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x81a);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa1a);
+	write_rtl8255(dev, 0x4, 0x81a);
+	write_rtl8255(dev, 0x4, 0x41a);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x61a);
+	write_rtl8255(dev, 0x4, 0x41a);
+	write_rtl8255(dev, 0x4, 0x41a);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x61a);
+	write_rtl8255(dev, 0x4, 0x41a);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404024, 0x27,
+		0x92402a8c, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc1b);
+	write_rtl8255(dev, 0x4, 0xe1b);
+	write_rtl8255(dev, 0x4, 0xc1b);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x81b);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa1b);
+	write_rtl8255(dev, 0x4, 0x81b);
+	write_rtl8255(dev, 0x4, 0x41b);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x61b);
+	write_rtl8255(dev, 0x4, 0x41b);
+	write_rtl8255(dev, 0x4, 0x41b);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x61b);
+	write_rtl8255(dev, 0x4, 0x41b);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404004, 0x27,
+		0x92402a90, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc1c);
+	write_rtl8255(dev, 0x4, 0xe1c);
+	write_rtl8255(dev, 0x4, 0xc1c);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x81c);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa1c);
+	write_rtl8255(dev, 0x4, 0x81c);
+	write_rtl8255(dev, 0x4, 0x41c);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x61c);
+	write_rtl8255(dev, 0x4, 0x41c);
+	write_rtl8255(dev, 0x4, 0x41c);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x61c);
+	write_rtl8255(dev, 0x4, 0x41c);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404001, 0x27,
+		0x92402a90, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc1d);
+	write_rtl8255(dev, 0x4, 0xe1d);
+	write_rtl8255(dev, 0x4, 0xc1d);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x81d);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa1d);
+	write_rtl8255(dev, 0x4, 0x81d);
+	write_rtl8255(dev, 0x4, 0x41d);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x61d);
+	write_rtl8255(dev, 0x4, 0x41d);
+	write_rtl8255(dev, 0x4, 0x41d);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x61d);
+	write_rtl8255(dev, 0x4, 0x41d);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a94, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc1e);
+	write_rtl8255(dev, 0x4, 0xe1e);
+	write_rtl8255(dev, 0x4, 0xc1e);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x81e);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa1e);
+	write_rtl8255(dev, 0x4, 0x81e);
+	write_rtl8255(dev, 0x4, 0x41e);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x61e);
+	write_rtl8255(dev, 0x4, 0x41e);
+	write_rtl8255(dev, 0x4, 0x41e);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x61e);
+	write_rtl8255(dev, 0x4, 0x41e);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x27,
+		0x92402a94, 0xf0009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc1f);
+	write_rtl8255(dev, 0x4, 0xe1f);
+	write_rtl8255(dev, 0x4, 0xc1f);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x81f);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa1f);
+	write_rtl8255(dev, 0x4, 0x81f);
+	write_rtl8255(dev, 0x4, 0x41f);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x61f);
+	write_rtl8255(dev, 0x4, 0x41f);
+	write_rtl8255(dev, 0x4, 0x41f);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x61f);
+	write_rtl8255(dev, 0x4, 0x41f);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404020, 0x80000027,
+		0x92402a98, 0xf8009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc20);
+	write_rtl8255(dev, 0x4, 0xe20);
+	write_rtl8255(dev, 0x4, 0xc20);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x820);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa20);
+	write_rtl8255(dev, 0x4, 0x820);
+	write_rtl8255(dev, 0x4, 0x420);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x620);
+	write_rtl8255(dev, 0x4, 0x420);
+	write_rtl8255(dev, 0x4, 0x420);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x620);
+	write_rtl8255(dev, 0x4, 0x420);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404020, 0x27,
+		0x92402a98, 0xf8009, 0x28000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc21);
+	write_rtl8255(dev, 0x4, 0xe21);
+	write_rtl8255(dev, 0x4, 0xc21);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x821);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa21);
+	write_rtl8255(dev, 0x4, 0x821);
+	write_rtl8255(dev, 0x4, 0x421);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x621);
+	write_rtl8255(dev, 0x4, 0x421);
+	write_rtl8255(dev, 0x4, 0x421);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x621);
+	write_rtl8255(dev, 0x4, 0x421);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a68, 0xf0009, 0x10028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc22);
+	write_rtl8255(dev, 0x4, 0xe22);
+	write_rtl8255(dev, 0x4, 0xc22);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x822);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa22);
+	write_rtl8255(dev, 0x4, 0x822);
+	write_rtl8255(dev, 0x4, 0x422);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x622);
+	write_rtl8255(dev, 0x4, 0x422);
+	write_rtl8255(dev, 0x4, 0x422);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x622);
+	write_rtl8255(dev, 0x4, 0x422);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404010, 0x80000027,
+		0x92402a68, 0xf0009, 0x20028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc23);
+	write_rtl8255(dev, 0x4, 0xe23);
+	write_rtl8255(dev, 0x4, 0xc23);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x823);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa23);
+	write_rtl8255(dev, 0x4, 0x823);
+	write_rtl8255(dev, 0x4, 0x423);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x623);
+	write_rtl8255(dev, 0x4, 0x423);
+	write_rtl8255(dev, 0x4, 0x423);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x623);
+	write_rtl8255(dev, 0x4, 0x423);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404010, 0x80000027,
+		0x92402a6c, 0xf0009, 0x30028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc24);
+	write_rtl8255(dev, 0x4, 0xe24);
+	write_rtl8255(dev, 0x4, 0xc24);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x824);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa24);
+	write_rtl8255(dev, 0x4, 0x824);
+	write_rtl8255(dev, 0x4, 0x424);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x624);
+	write_rtl8255(dev, 0x4, 0x424);
+	write_rtl8255(dev, 0x4, 0x424);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x624);
+	write_rtl8255(dev, 0x4, 0x424);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404010, 0x80000027,
+		0x92402a6c, 0xf0009, 0x40028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc25);
+	write_rtl8255(dev, 0x4, 0xe25);
+	write_rtl8255(dev, 0x4, 0xc25);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x825);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa25);
+	write_rtl8255(dev, 0x4, 0x825);
+	write_rtl8255(dev, 0x4, 0x425);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x625);
+	write_rtl8255(dev, 0x4, 0x425);
+	write_rtl8255(dev, 0x4, 0x425);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x625);
+	write_rtl8255(dev, 0x4, 0x425);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a70, 0xf0009, 0x60028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc26);
+	write_rtl8255(dev, 0x4, 0xe26);
+	write_rtl8255(dev, 0x4, 0xc26);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x826);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa26);
+	write_rtl8255(dev, 0x4, 0x826);
+	write_rtl8255(dev, 0x4, 0x426);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x626);
+	write_rtl8255(dev, 0x4, 0x426);
+	write_rtl8255(dev, 0x4, 0x426);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x626);
+	write_rtl8255(dev, 0x4, 0x426);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404031, 0x40000027,
+		0x92402a70, 0xf0011, 0x60028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc27);
+	write_rtl8255(dev, 0x4, 0xe27);
+	write_rtl8255(dev, 0x4, 0xc27);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x827);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa27);
+	write_rtl8255(dev, 0x4, 0x827);
+	write_rtl8255(dev, 0x4, 0x427);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x627);
+	write_rtl8255(dev, 0x4, 0x427);
+	write_rtl8255(dev, 0x4, 0x427);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x627);
+	write_rtl8255(dev, 0x4, 0x427);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404011, 0x40000027,
+		0x92402a74, 0xf0011, 0x60028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc28);
+	write_rtl8255(dev, 0x4, 0xe28);
+	write_rtl8255(dev, 0x4, 0xc28);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x828);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa28);
+	write_rtl8255(dev, 0x4, 0x828);
+	write_rtl8255(dev, 0x4, 0x428);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x628);
+	write_rtl8255(dev, 0x4, 0x428);
+	write_rtl8255(dev, 0x4, 0x428);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x628);
+	write_rtl8255(dev, 0x4, 0x428);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404010, 0xc0000027,
+		0x92402a74, 0xf0011, 0x60028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc29);
+	write_rtl8255(dev, 0x4, 0xe29);
+	write_rtl8255(dev, 0x4, 0xc29);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x829);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa29);
+	write_rtl8255(dev, 0x4, 0x829);
+	write_rtl8255(dev, 0x4, 0x429);
+	write_rtl8255(dev, 0x3, 0x25);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x629);
+	write_rtl8255(dev, 0x4, 0x429);
+	write_rtl8255(dev, 0x4, 0x429);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x629);
+	write_rtl8255(dev, 0x4, 0x429);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a78, 0xf0011, 0x60028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc2a);
+	write_rtl8255(dev, 0x4, 0xe2a);
+	write_rtl8255(dev, 0x4, 0xc2a);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x82a);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa2a);
+	write_rtl8255(dev, 0x4, 0x82a);
+	write_rtl8255(dev, 0x4, 0x42a);
+	write_rtl8255(dev, 0x3, 0x24);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x62a);
+	write_rtl8255(dev, 0x4, 0x42a);
+	write_rtl8255(dev, 0x4, 0x42a);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x62a);
+	write_rtl8255(dev, 0x4, 0x42a);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a78, 0xf0011, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc2b);
+	write_rtl8255(dev, 0x4, 0xe2b);
+	write_rtl8255(dev, 0x4, 0xc2b);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x82b);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa2b);
+	write_rtl8255(dev, 0x4, 0x82b);
+	write_rtl8255(dev, 0x4, 0x42b);
+	write_rtl8255(dev, 0x3, 0x24);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x62b);
+	write_rtl8255(dev, 0x4, 0x42b);
+	write_rtl8255(dev, 0x4, 0x42b);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x62b);
+	write_rtl8255(dev, 0x4, 0x42b);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a48, 0xf0019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc2c);
+	write_rtl8255(dev, 0x4, 0xe2c);
+	write_rtl8255(dev, 0x4, 0xc2c);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x82c);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa2c);
+	write_rtl8255(dev, 0x4, 0x82c);
+	write_rtl8255(dev, 0x4, 0x42c);
+	write_rtl8255(dev, 0x3, 0x24);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x62c);
+	write_rtl8255(dev, 0x4, 0x42c);
+	write_rtl8255(dev, 0x4, 0x42c);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x62c);
+	write_rtl8255(dev, 0x4, 0x42c);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a48, 0xf8019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc2d);
+	write_rtl8255(dev, 0x4, 0xe2d);
+	write_rtl8255(dev, 0x4, 0xc2d);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x82d);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa2d);
+	write_rtl8255(dev, 0x4, 0x82d);
+	write_rtl8255(dev, 0x4, 0x42d);
+	write_rtl8255(dev, 0x3, 0x24);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x62d);
+	write_rtl8255(dev, 0x4, 0x42d);
+	write_rtl8255(dev, 0x4, 0x42d);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x62d);
+	write_rtl8255(dev, 0x4, 0x42d);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a4c, 0xf8019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc2e);
+	write_rtl8255(dev, 0x4, 0xe2e);
+	write_rtl8255(dev, 0x4, 0xc2e);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x82e);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa2e);
+	write_rtl8255(dev, 0x4, 0x82e);
+	write_rtl8255(dev, 0x4, 0x42e);
+	write_rtl8255(dev, 0x3, 0x24);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x62e);
+	write_rtl8255(dev, 0x4, 0x42e);
+	write_rtl8255(dev, 0x4, 0x42e);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x62e);
+	write_rtl8255(dev, 0x4, 0x42e);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a4c, 0xf8019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc2f);
+	write_rtl8255(dev, 0x4, 0xe2f);
+	write_rtl8255(dev, 0x4, 0xc2f);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x82f);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa2f);
+	write_rtl8255(dev, 0x4, 0x82f);
+	write_rtl8255(dev, 0x4, 0x42f);
+	write_rtl8255(dev, 0x3, 0x24);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x62f);
+	write_rtl8255(dev, 0x4, 0x42f);
+	write_rtl8255(dev, 0x4, 0x42f);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x62f);
+	write_rtl8255(dev, 0x4, 0x42f);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a50, 0xf8019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc30);
+	write_rtl8255(dev, 0x4, 0xe30);
+	write_rtl8255(dev, 0x4, 0xc30);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x830);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa30);
+	write_rtl8255(dev, 0x4, 0x830);
+	write_rtl8255(dev, 0x4, 0x430);
+	write_rtl8255(dev, 0x3, 0x24);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x630);
+	write_rtl8255(dev, 0x4, 0x430);
+	write_rtl8255(dev, 0x4, 0x430);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x630);
+	write_rtl8255(dev, 0x4, 0x430);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a50, 0xf8019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc31);
+	write_rtl8255(dev, 0x4, 0xe31);
+	write_rtl8255(dev, 0x4, 0xc31);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x831);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa31);
+	write_rtl8255(dev, 0x4, 0x831);
+	write_rtl8255(dev, 0x4, 0x431);
+	write_rtl8255(dev, 0x3, 0x24);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x631);
+	write_rtl8255(dev, 0x4, 0x431);
+	write_rtl8255(dev, 0x4, 0x431);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x631);
+	write_rtl8255(dev, 0x4, 0x431);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a54, 0xf8019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc32);
+	write_rtl8255(dev, 0x4, 0xe32);
+	write_rtl8255(dev, 0x4, 0xc32);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x832);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa32);
+	write_rtl8255(dev, 0x4, 0x832);
+	write_rtl8255(dev, 0x4, 0x432);
+	write_rtl8255(dev, 0x3, 0x24);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x632);
+	write_rtl8255(dev, 0x4, 0x432);
+	write_rtl8255(dev, 0x4, 0x432);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x632);
+	write_rtl8255(dev, 0x4, 0x432);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a54, 0xf8019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc33);
+	write_rtl8255(dev, 0x4, 0xe33);
+	write_rtl8255(dev, 0x4, 0xc33);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x833);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa33);
+	write_rtl8255(dev, 0x4, 0x833);
+	write_rtl8255(dev, 0x4, 0x433);
+	write_rtl8255(dev, 0x3, 0x24);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x633);
+	write_rtl8255(dev, 0x4, 0x433);
+	write_rtl8255(dev, 0x4, 0x433);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x633);
+	write_rtl8255(dev, 0x4, 0x433);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a58, 0xf8019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc34);
+	write_rtl8255(dev, 0x4, 0xe34);
+	write_rtl8255(dev, 0x4, 0xc34);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x834);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa34);
+	write_rtl8255(dev, 0x4, 0x834);
+	write_rtl8255(dev, 0x4, 0x434);
+	write_rtl8255(dev, 0x3, 0x24);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x634);
+	write_rtl8255(dev, 0x4, 0x434);
+	write_rtl8255(dev, 0x4, 0x434);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x634);
+	write_rtl8255(dev, 0x4, 0x434);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a58, 0xf8019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc35);
+	write_rtl8255(dev, 0x4, 0xe35);
+	write_rtl8255(dev, 0x4, 0xc35);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x835);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa35);
+	write_rtl8255(dev, 0x4, 0x835);
+	write_rtl8255(dev, 0x4, 0x435);
+	write_rtl8255(dev, 0x3, 0x24);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x4, 0x635);
+	write_rtl8255(dev, 0x4, 0x435);
+	write_rtl8255(dev, 0x4, 0x435);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x635);
+	write_rtl8255(dev, 0x4, 0x435);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a24, 0xf8019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc36);
+	write_rtl8255(dev, 0x4, 0xe36);
+	write_rtl8255(dev, 0x4, 0xc36);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x836);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa36);
+	write_rtl8255(dev, 0x4, 0x836);
+	write_rtl8255(dev, 0x4, 0x436);
+	write_rtl8255(dev, 0x3, 0x24);
+	write_rtl8255(dev, 0x2, 0x25);
+	write_rtl8255(dev, 0x4, 0x636);
+	write_rtl8255(dev, 0x4, 0x436);
+	write_rtl8255(dev, 0x4, 0x436);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x636);
+	write_rtl8255(dev, 0x4, 0x436);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a24, 0xf8019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc37);
+	write_rtl8255(dev, 0x4, 0xe37);
+	write_rtl8255(dev, 0x4, 0xc37);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x837);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa37);
+	write_rtl8255(dev, 0x4, 0x837);
+	write_rtl8255(dev, 0x4, 0x437);
+	write_rtl8255(dev, 0x3, 0x24);
+	write_rtl8255(dev, 0x2, 0x25);
+	write_rtl8255(dev, 0x4, 0x637);
+	write_rtl8255(dev, 0x4, 0x437);
+	write_rtl8255(dev, 0x4, 0x437);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x637);
+	write_rtl8255(dev, 0x4, 0x437);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a28, 0xf8019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc38);
+	write_rtl8255(dev, 0x4, 0xe38);
+	write_rtl8255(dev, 0x4, 0xc38);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x838);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa38);
+	write_rtl8255(dev, 0x4, 0x838);
+	write_rtl8255(dev, 0x4, 0x438);
+	write_rtl8255(dev, 0x3, 0x24);
+	write_rtl8255(dev, 0x2, 0x25);
+	write_rtl8255(dev, 0x4, 0x638);
+	write_rtl8255(dev, 0x4, 0x438);
+	write_rtl8255(dev, 0x4, 0x438);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x638);
+	write_rtl8255(dev, 0x4, 0x438);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a28, 0xf8019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc39);
+	write_rtl8255(dev, 0x4, 0xe39);
+	write_rtl8255(dev, 0x4, 0xc39);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x839);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa39);
+	write_rtl8255(dev, 0x4, 0x839);
+	write_rtl8255(dev, 0x4, 0x439);
+	write_rtl8255(dev, 0x3, 0x24);
+	write_rtl8255(dev, 0x2, 0x25);
+	write_rtl8255(dev, 0x4, 0x639);
+	write_rtl8255(dev, 0x4, 0x439);
+	write_rtl8255(dev, 0x4, 0x439);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x639);
+	write_rtl8255(dev, 0x4, 0x439);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a00, 0xf8019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc3a);
+	write_rtl8255(dev, 0x4, 0xe3a);
+	write_rtl8255(dev, 0x4, 0xc3a);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x83a);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa3a);
+	write_rtl8255(dev, 0x4, 0x83a);
+	write_rtl8255(dev, 0x4, 0x43a);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0x63a);
+	write_rtl8255(dev, 0x4, 0x43a);
+	write_rtl8255(dev, 0x4, 0x43a);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x63a);
+	write_rtl8255(dev, 0x4, 0x43a);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a00, 0xf8019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc3b);
+	write_rtl8255(dev, 0x4, 0xe3b);
+	write_rtl8255(dev, 0x4, 0xc3b);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x83b);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa3b);
+	write_rtl8255(dev, 0x4, 0x83b);
+	write_rtl8255(dev, 0x4, 0x43b);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0x63b);
+	write_rtl8255(dev, 0x4, 0x43b);
+	write_rtl8255(dev, 0x4, 0x43b);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x63b);
+	write_rtl8255(dev, 0x4, 0x43b);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a00, 0xf8019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc3c);
+	write_rtl8255(dev, 0x4, 0xe3c);
+	write_rtl8255(dev, 0x4, 0xc3c);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x83c);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa3c);
+	write_rtl8255(dev, 0x4, 0x83c);
+	write_rtl8255(dev, 0x4, 0x43c);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0x63c);
+	write_rtl8255(dev, 0x4, 0x43c);
+	write_rtl8255(dev, 0x4, 0x43c);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x63c);
+	write_rtl8255(dev, 0x4, 0x43c);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a00, 0xf8019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc3d);
+	write_rtl8255(dev, 0x4, 0xe3d);
+	write_rtl8255(dev, 0x4, 0xc3d);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x83d);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa3d);
+	write_rtl8255(dev, 0x4, 0x83d);
+	write_rtl8255(dev, 0x4, 0x43d);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0x63d);
+	write_rtl8255(dev, 0x4, 0x43d);
+	write_rtl8255(dev, 0x4, 0x43d);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x63d);
+	write_rtl8255(dev, 0x4, 0x43d);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a00, 0xf8019, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc3e);
+	write_rtl8255(dev, 0x4, 0xe3e);
+	write_rtl8255(dev, 0x4, 0xc3e);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x83e);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa3e);
+	write_rtl8255(dev, 0x4, 0x83e);
+	write_rtl8255(dev, 0x4, 0x43e);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0x63e);
+	write_rtl8255(dev, 0x4, 0x43e);
+	write_rtl8255(dev, 0x4, 0x43e);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x63e);
+	write_rtl8255(dev, 0x4, 0x43e);
+	write_rtl8255_reg0c(dev, 0x1554, 0xa800403b, 0xf6d44278, 0x80404000, 0x80000027,
+		0x92402a00, 0xf8011, 0x70028000, 0xc00, 0x0);
+	write_rtl8255(dev, 0x1, 0x807);
+	write_rtl8255(dev, 0x4, 0xc3f);
+	write_rtl8255(dev, 0x4, 0xe3f);
+	write_rtl8255(dev, 0x4, 0xc3f);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255(dev, 0x4, 0x83f);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0xa3f);
+	write_rtl8255(dev, 0x4, 0x83f);
+	write_rtl8255(dev, 0x4, 0x43f);
+	write_rtl8255(dev, 0x3, 0x0);
+	write_rtl8255(dev, 0x2, 0x0);
+	write_rtl8255(dev, 0x4, 0x63f);
+	write_rtl8255(dev, 0x4, 0x43f);
+	write_rtl8255(dev, 0x4, 0x43f);
+	write_rtl8255(dev, 0x3, 0x100);
+	write_rtl8255(dev, 0x4, 0x63f);
+	write_rtl8255(dev, 0x4, 0x43f);
+	write_rtl8255(dev, 0x4, 0x0);
+	write_rtl8255(dev, 0x1, 0x0);
+	write_rtl8255_reg0c(dev, 0x3539, 0x70000c03, 0xfef46178, 0x408000, 0x403307,
+		0x924f80c0, 0xf955c, 0x8400, 0x429200, 0x1ce20);
+	write_rtl8255(dev, 0x1, 0x1c7);
+	write_rtl8255(dev, 0x2, 0x26);
+	write_rtl8255(dev, 0x3, 0x27);
+	write_rtl8255(dev, 0x1, 0x47);
+	write_rtl8255(dev, 0x4, 0x98c);
+	write_rtl8255(dev, 0x5, 0x65);
+	write_rtl8255(dev, 0x6, 0x13);
+	write_rtl8255(dev, 0x7, 0x7c);
+	write_rtl8255(dev, 0x8, 0x6);
+	write_rtl8255(dev, 0x8, 0x7);
+	write_rtl8255(dev, 0x8, 0x6);
+	write_rtl8255(dev, 0x9, 0xce2);
+	write_rtl8255(dev, 0xb, 0x1c5);
+	write_rtl8255(dev, 0xd, 0xd7f);
+	write_rtl8255(dev, 0xe, 0x369);
+	write_rtl8255(dev, 0xa, 0xd56);
+	write_rtl8255(dev, 0xa, 0xd57);
+	mdelay(20);
+	write_rtl8255(dev, 0xd, 0xd7e);
+
+}
+
+
+void rtl8255_set_band_param(struct net_device *dev, short band)
+{
+	if(band != BAND_A){
+		write_nic_dword(dev, 0x94, 0x3dc00002);
+		write_nic_dword(dev, 0x88, 0x00100040);
+
+		write_phy_cck(dev, 0x13, 0xd0);
+
+		write_phy_cck(dev, 0x41, 0x9d);
+		write_nic_dword(dev, 0x8c, 0x00082205);
+		write_nic_byte(dev, 0xb4, 0x66);
+	}
+}
+
+void rtl8255_rf_init(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	int i;
+	u16 brsr;
+//	short channel /*= priv->chan*/ = 1;
+	priv->chan = 1;
+
+	write_nic_word(dev, RFPinsOutput, 0x80);
+	write_nic_word(dev, RFPinsSelect, 0x80 | SW_CONTROL_GPIO);
+	write_nic_word(dev, RFPinsEnable, 0x80);
+	write_nic_word(dev, RFPinsSelect, SW_CONTROL_GPIO);
+
+	write_nic_dword(dev, RF_TIMING, 0x000f800f);
+
+	brsr = read_nic_word(dev, BRSR);
+
+	write_nic_word(dev, 0x2c, 0xffff);
+
+
+	rtl8180_set_anaparam(dev, RTL8255_ANAPARAM_ON);
+	rtl8185_set_anaparam2(dev, RTL8255_ANAPARAM2_ON);
+
+	write_nic_dword(dev, 0x94, 0x11c00002);
+
+	write_nic_dword(dev, RF_PARA, 0x100040);
+
+	rtl8185_rf_pins_enable(dev);
+
+	rtl8255_init_BGband(dev);
+	rtl8255_set_band_param(dev,BAND_BG);
+
+	write_phy_cck(dev, 0x0, 0x98);
+	write_phy_cck(dev, 0x3, 0x20);
+	write_phy_cck(dev, 0x4, 0x2e);
+	write_phy_cck(dev, 0x5, 0x12);
+	write_phy_cck(dev, 0x6, 0xfc);
+	write_phy_cck(dev, 0x7, 0xd8);
+	write_phy_cck(dev, 0x8, 0x2e);
+	write_phy_cck(dev, 0x10, 0xd3);
+	write_phy_cck(dev, 0x11, 0x88);
+	write_phy_cck(dev, 0x12, 0x47);
+	write_phy_cck(dev, 0x13, 0xd0); /* Ver C & D & 8187*/
+
+	write_phy_cck(dev, 0x19, 0x0);
+	write_phy_cck(dev, 0x1a, 0xa0);
+	write_phy_cck(dev, 0x1b, 0x8);
+	write_phy_cck(dev, 0x40, 0x86); /* CCK Carrier Sense Threshold */
+	write_phy_cck(dev, 0x41, 0x9d); /* Energy Threshold */
+	//write_phy_cck(dev, 0x42, 0x0);
+	write_phy_cck(dev, 0x43, 0x8);
+
+	write_nic_byte(dev, TESTR,0x8);
+
+	for(i=0;i<128;i++){
+		write_phy_ofdm(dev, 0x4b, rtl8255_agc[i]);
+		write_phy_ofdm(dev, 0x4a, (u8)i+ 0x80);
+	}
+
+
+	write_phy_ofdm(dev, 0x0, 0x1);
+	write_phy_ofdm(dev, 0x1, 0x2);
+	write_phy_ofdm(dev, 0x2, 0x43);
+	write_phy_ofdm(dev, 0x3, 0x0);
+	write_phy_ofdm(dev, 0x4, 0x0);
+	write_phy_ofdm(dev, 0x5, 0x0);
+	write_phy_ofdm(dev, 0x6, 0x40);
+	write_phy_ofdm(dev, 0x7, 0x0);
+	write_phy_ofdm(dev, 0x8, 0x40);
+	write_phy_ofdm(dev, 0x9, 0xfe);
+	write_phy_ofdm(dev, 0xa, 0x9);
+	write_phy_ofdm(dev, 0xb, 0x80);
+	write_phy_ofdm(dev, 0xc, 0x1);
+	write_phy_ofdm(dev, 0xd, 0x43);
+	write_phy_ofdm(dev, 0xe, 0xd3);
+	write_phy_ofdm(dev, 0xf, 0x38);
+	write_phy_ofdm(dev, 0x10, 0x4);
+	write_phy_ofdm(dev, 0x11, 0x06);/*agc resp time 700*/
+	write_phy_ofdm(dev, 0x12, 0x20);
+	write_phy_ofdm(dev, 0x13, 0x20);
+	write_phy_ofdm(dev, 0x14, 0x0);
+	write_phy_ofdm(dev, 0x15, 0x40);
+	write_phy_ofdm(dev, 0x16, 0x0);
+	write_phy_ofdm(dev, 0x17, 0x40);
+	write_phy_ofdm(dev, 0x18, 0xef);
+	write_phy_ofdm(dev, 0x19, 0x25);
+	write_phy_ofdm(dev, 0x1a, 0x20);
+	write_phy_ofdm(dev, 0x1b, 0x7a);
+	write_phy_ofdm(dev, 0x1c, 0x84);
+	write_phy_ofdm(dev, 0x1e, 0x95);
+	write_phy_ofdm(dev, 0x1f, 0x75);
+	write_phy_ofdm(dev, 0x20, 0x1f);
+	write_phy_ofdm(dev, 0x21, 0x17);
+	write_phy_ofdm(dev, 0x22, 0x16);
+	write_phy_ofdm(dev, 0x23, 0x70); //FIXME maybe not needed
+	write_phy_ofdm(dev, 0x24, 0x70);
+	write_phy_ofdm(dev, 0x25, 0x0);
+	write_phy_ofdm(dev, 0x26, 0x10);
+	write_phy_ofdm(dev, 0x27, 0x88);
+
+
+	write_nic_dword(dev, 0x94, 0x3dc00002); //BAND DEPEND.
+//	write_nic_dword(dev, 0x94, 0x15c00002); //BAND DEPEND.
+
+	write_phy_cck(dev, 0x4, 0x18);
+	write_phy_cck(dev, 0x43, 0x18);
+	write_phy_cck(dev, 0x6, 0xdc);
+	write_phy_cck(dev, 0x44, 0x2b);
+	write_phy_cck(dev, 0x45, 0x2b);
+	write_phy_cck(dev, 0x46, 0x25);
+	write_phy_cck(dev, 0x47, 0x15);
+	write_phy_cck(dev, 0x48, 0x0);
+	write_phy_cck(dev, 0x49, 0x0);
+	write_phy_cck(dev, 0x4a, 0x0);
+	write_phy_cck(dev, 0x4b, 0x0);
+//	write_phy_cck(dev, 0x4c, 0x5);
+#if 0
+	write_phy_cck(dev, 0x41, 0x9d); /* Energy Threshold */
+	// TESTR 0xb 8187
+	write_phy_cck(dev, 0x10, 0x93);// & 0xfb);
+#endif
+	//rtl8255_set_gain(dev, 1); /* FIXME this '1' is random */
+
+	rtl8255_SetTXPowerLevel(dev, priv->chan);
+
+	write_phy_cck(dev, 0x10, 0x93 |0x4); /* Rx ant B, 0xd3 for A */
+	write_phy_ofdm(dev, 0x26, 0x90); /* Rx ant B, 0x10 for A */
+
+	rtl8185_tx_antenna(dev, 0x3); /* TX ant B, 0x0 for A*/
+	/* make sure is waken up! */
+	rtl8180_set_anaparam(dev, RTL8255_ANAPARAM_ON);
+	rtl8185_set_anaparam2(dev, RTL8255_ANAPARAM2_ON);
+
+	rtl8255_set_band_param(dev,BAND_BG);
+
+	write_phy_cck(dev, 0x41, 0x9d);
+
+	rtl8255_set_gain(dev, 4);
+	//rtl8255_set_energy_threshold(dev);
+	write_phy_cck(dev, 0x41, 0x9d);
+	rtl8255_rf_set_chan(dev, priv->chan);
+
+	write_nic_word(dev, BRSR, brsr);
+}
+
diff --git a/drivers/staging/rtl8187se/r8180_rtl8255.h b/drivers/staging/rtl8187se/r8180_rtl8255.h
new file mode 100644
index 00000000000..be44ca6eb1d
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_rtl8255.h
@@ -0,0 +1,19 @@
+/*
+  This is part of the rtl8180-sa2400 driver
+  released under the GPL (See file COPYING for details).
+  Copyright (c) 2005 Andrea Merello <andreamrl@tiscali.it>
+
+  This files contains programming code for the rtl8255
+  radio frontend.
+
+  *Many* thanks to Realtek Corp. for their great support!
+
+*/
+
+#define RTL8255_ANAPARAM_ON 0xa0000b59
+#define RTL8255_ANAPARAM2_ON 0x840cf311
+
+
+void rtl8255_rf_init(struct net_device *dev);
+void rtl8255_rf_set_chan(struct net_device *dev,short ch);
+void rtl8255_rf_close(struct net_device *dev);
diff --git a/drivers/staging/rtl8187se/r8180_sa2400.c b/drivers/staging/rtl8187se/r8180_sa2400.c
new file mode 100644
index 00000000000..d6495601715
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_sa2400.c
@@ -0,0 +1,233 @@
+/*
+   This files contains PHILIPS SA2400 radio frontend programming routines.
+
+   This is part of rtl8180 OpenSource driver
+   Copyright (C) Andrea Merello 2004-2005  <andreamrl@tiscali.it>
+   Released under the terms of GPL (General Public Licence)
+
+   Parts of this driver are based on the GPL part of the
+   official realtek driver
+
+   Parts of this driver are based on the rtl8180 driver skeleton
+   from Patric Schenke & Andres Salomon
+
+   Parts of this driver are based on the Intel Pro Wireless 2100 GPL driver.
+
+   Code at http://che.ojctech.com/~dyoung/rtw/ has been useful to me to
+   understand some things.
+
+   Code from rtl8181 project has been useful to me to understand some things.
+
+   We want to tanks the Authors of such projects and the Ndiswrapper
+   project Authors.
+*/
+
+
+#include "r8180.h"
+#include "r8180_hw.h"
+#include "r8180_sa2400.h"
+
+
+//#define DEBUG_SA2400
+
+u32 sa2400_chan[] = {
+	0x0,	//dummy channel 0
+	0x00096c, //1
+	0x080970, //2
+	0x100974, //3
+	0x180978, //4
+	0x000980, //5
+	0x080984, //6
+	0x100988, //7
+	0x18098c, //8
+	0x000994, //9
+	0x080998, //10
+	0x10099c, //11
+	0x1809a0, //12
+	0x0009a8, //13
+	0x0009b4, //14
+};
+
+
+void rf_stabilize(struct net_device *dev)
+{
+	force_pci_posting(dev);
+	mdelay(3); //for now use a great value.. we may optimize in future
+}
+
+
+void write_sa2400(struct net_device *dev,u8 adr, u32 data)
+{
+//	struct r8180_priv *priv = ieee80211_priv(dev);
+	u32 phy_config;
+
+        // philips sa2400 expects 24 bits data
+
+	/*if(adr == 4 && priv->digphy){
+		phy_config=0x60000000;
+	}else{
+		phy_config=0xb0000000;
+	}*/
+
+	phy_config = 0xb0000000; // MAC will bang bits to the sa2400
+
+	phy_config |= (((u32)(adr&0xf))<< 24);
+	phy_config |= (data & 0xffffff);
+	write_nic_dword(dev,PHY_CONFIG,phy_config);
+#ifdef DEBUG_SA2400
+	DMESG("Writing sa2400: %x (adr %x)",phy_config,adr);
+#endif
+	rf_stabilize(dev);
+}
+
+
+
+void sa2400_write_phy_antenna(struct net_device *dev,short ch)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	u8 ant;
+
+	ant = SA2400_ANTENNA;
+	if(priv->antb) /*default antenna is antenna B */
+		ant |= BB_ANTENNA_B;
+	if(ch == 14)
+		ant |= BB_ANTATTEN_CHAN14;
+	write_phy(dev,0x10,ant);
+	//DMESG("BB antenna %x ",ant);
+}
+
+
+/* from the rtl8181 embedded driver */
+short sa2400_rf_set_sens(struct net_device *dev, short sens)
+{
+	u8 finetune = 0;
+	if ((sens > 85) || (sens < 54)) return -1;
+
+	write_sa2400(dev,5,0x1dfb | (sens-54) << 15 |(finetune<<20));  // AGC	0xc9dfb
+
+	return 0;
+}
+
+
+void sa2400_rf_set_chan(struct net_device *dev, short ch)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	u32 txpw = 0xff & priv->chtxpwr[ch];
+	u32 chan = sa2400_chan[ch];
+
+	write_sa2400(dev,7,txpw);
+	//write_phy(dev,0x10,0xd1);
+	sa2400_write_phy_antenna(dev,ch);
+	write_sa2400(dev,0,chan);
+	write_sa2400(dev,1,0xbb50);
+	write_sa2400(dev,2,0x80);
+	write_sa2400(dev,3,0);
+}
+
+
+void sa2400_rf_close(struct net_device *dev)
+{
+	write_sa2400(dev, 4, 0);
+}
+
+
+void sa2400_rf_init(struct net_device *dev)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	u32 anaparam;
+	u8 firdac;
+
+	write_nic_byte(dev,PHY_DELAY,0x6);	//this is general
+	write_nic_byte(dev,CARRIER_SENSE_COUNTER,0x4c); //this is general
+
+	/*these are philips sa2400 specific*/
+	anaparam = read_nic_dword(dev,ANAPARAM);
+	anaparam = anaparam &~ (1<<ANAPARAM_TXDACOFF_SHIFT);
+
+	anaparam = anaparam &~ANAPARAM_PWR1_MASK;
+	anaparam = anaparam &~ANAPARAM_PWR0_MASK;
+	if(priv->digphy){
+		anaparam |= (SA2400_DIG_ANAPARAM_PWR1_ON<<ANAPARAM_PWR1_SHIFT);
+		anaparam |= (SA2400_ANAPARAM_PWR0_ON<<ANAPARAM_PWR0_SHIFT);
+	}else{
+		anaparam |= (SA2400_ANA_ANAPARAM_PWR1_ON<<ANAPARAM_PWR1_SHIFT);
+	}
+
+	rtl8180_set_anaparam(dev,anaparam);
+
+	firdac = (priv->digphy) ? (1<<SA2400_REG4_FIRDAC_SHIFT) : 0;
+	write_sa2400(dev,0,sa2400_chan[priv->chan]);
+	write_sa2400(dev,1,0xbb50);
+	write_sa2400(dev,2,0x80);
+	write_sa2400(dev,3,0);
+	write_sa2400(dev,4,0x19340 | firdac);
+	write_sa2400(dev,5,0xc9dfb);  // AGC
+	write_sa2400(dev,4,0x19348 | firdac);  //calibrates VCO
+
+	if(priv->digphy)
+		write_sa2400(dev,4,0x1938c); /*???*/
+
+	write_sa2400(dev,4,0x19340 | firdac);
+
+	write_sa2400(dev,0,sa2400_chan[priv->chan]);
+	write_sa2400(dev,1,0xbb50);
+	write_sa2400(dev,2,0x80);
+	write_sa2400(dev,3,0);
+	write_sa2400(dev,4,0x19344 | firdac); //calibrates filter
+
+	/* new from rtl8180 embedded driver (rtl8181 project) */
+	write_sa2400(dev,6,0x13ff | (1<<23)); // MANRX
+	write_sa2400(dev,8,0); //VCO
+
+	if(!priv->digphy)
+	{
+		rtl8180_set_anaparam(dev, anaparam | \
+				     (1<<ANAPARAM_TXDACOFF_SHIFT));
+
+		rtl8180_conttx_enable(dev);
+
+		write_sa2400(dev, 4, 0x19341); // calibrates DC
+
+		/* a 5us sleep is required here,
+		   we rely on the 3ms delay introduced in write_sa2400
+		*/
+		write_sa2400(dev, 4, 0x19345);
+		/* a 20us sleep is required here,
+		   we rely on the 3ms delay introduced in write_sa2400
+		*/
+		rtl8180_conttx_disable(dev);
+
+		rtl8180_set_anaparam(dev, anaparam);
+	}
+	/* end new */
+
+	write_sa2400(dev,4,0x19341 | firdac ); //RTX MODE
+
+	// Set tx power level !?
+
+
+	/*baseband configuration*/
+	write_phy(dev,0,0x98);
+	write_phy(dev,3,0x38);
+	write_phy(dev,4,0xe0);
+	write_phy(dev,5,0x90);
+	write_phy(dev,6,0x1a);
+	write_phy(dev,7,0x64);
+
+	/*Should be done something more here??*/
+
+	sa2400_write_phy_antenna(dev,priv->chan);
+
+	write_phy(dev,0x11,0x80);
+	if(priv->diversity)
+		write_phy(dev,0x12,0xc7);
+	else
+		write_phy(dev,0x12,0x47);
+
+	write_phy(dev,0x13,0x90 | priv->cs_treshold );
+
+	write_phy(dev,0x19,0x0);
+	write_phy(dev,0x1a,0xa0);
+
+	sa2400_rf_set_chan(dev,priv->chan);
+}
diff --git a/drivers/staging/rtl8187se/r8180_sa2400.h b/drivers/staging/rtl8187se/r8180_sa2400.h
new file mode 100644
index 00000000000..683a69b96af
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_sa2400.h
@@ -0,0 +1,26 @@
+/*
+	This is part of rtl8180 OpenSource driver - v 0.7
+	Copyright (C) Andrea Merello 2004  <andreamrl@tiscali.it>
+	Released under the terms of GPL (General Public Licence)
+
+	Parts of this driver are based on the GPL part of the official realtek driver
+	Parts of this driver are based on the rtl8180 driver skeleton from Patric Schenke & Andres Salomon
+	Parts of this driver are based on the Intel Pro Wireless 2100 GPL driver
+
+	We want to tanks the Authors of such projects and the Ndiswrapper project Authors.
+*/
+
+#define SA2400_ANTENNA 0x91
+#define SA2400_DIG_ANAPARAM_PWR1_ON 0x8
+#define SA2400_ANA_ANAPARAM_PWR1_ON 0x28
+#define SA2400_ANAPARAM_PWR0_ON 0x3
+
+#define SA2400_RF_MAX_SENS 85
+#define SA2400_RF_DEF_SENS 80
+
+#define SA2400_REG4_FIRDAC_SHIFT 7
+
+void sa2400_rf_init(struct net_device *dev);
+void sa2400_rf_set_chan(struct net_device *dev,short ch);
+short sa2400_rf_set_sens(struct net_device *dev,short sens);
+void sa2400_rf_close(struct net_device *dev);
diff --git a/drivers/staging/rtl8187se/r8180_wx.c b/drivers/staging/rtl8187se/r8180_wx.c
new file mode 100644
index 00000000000..c77abe502b7
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_wx.c
@@ -0,0 +1,1644 @@
+/*
+   This file contains wireless extension handlers.
+
+   This is part of rtl8180 OpenSource driver.
+   Copyright (C) Andrea Merello 2004-2005  <andreamrl@tiscali.it>
+   Released under the terms of GPL (General Public Licence)
+
+   Parts of this driver are based on the GPL part
+   of the official realtek driver.
+
+   Parts of this driver are based on the rtl8180 driver skeleton
+   from Patric Schenke & Andres Salomon.
+
+   Parts of this driver are based on the Intel Pro Wireless 2100 GPL driver.
+
+   We want to tanks the Authors of those projects and the Ndiswrapper
+   project Authors.
+*/
+
+
+#include "r8180.h"
+#include "r8180_hw.h"
+#include "r8180_sa2400.h"
+
+#ifdef ENABLE_DOT11D
+#include "dot11d.h"
+#endif
+
+//#define RATE_COUNT 4
+u32 rtl8180_rates[] = {1000000,2000000,5500000,11000000,
+	6000000,9000000,12000000,18000000,24000000,36000000,48000000,54000000};
+
+#define RATE_COUNT (sizeof(rtl8180_rates)/sizeof(rtl8180_rates[0]))
+
+static CHANNEL_LIST DefaultChannelPlan[] = {
+//	{{1,2,3,4,5,6,7,8,9,10,11,12,13,14},14},			//Default channel plan
+	{{1,2,3,4,5,6,7,8,9,10,11,36,40,44,48,52,56,60,64},19},  		//FCC
+	{{1,2,3,4,5,6,7,8,9,10,11},11},                    				//IC
+	{{1,2,3,4,5,6,7,8,9,10,11,12,13,36,40,44,48,52,56,60,64},21},  	//ETSI
+	{{1,2,3,4,5,6,7,8,9,10,11,12,13,36,40,44,48,52,56,60,64},21},    //Spain. Change to ETSI.
+	{{1,2,3,4,5,6,7,8,9,10,11,12,13,36,40,44,48,52,56,60,64},21},  	//France. Change to ETSI.
+	{{14,36,40,44,48,52,56,60,64},9},						//MKK
+	{{1,2,3,4,5,6,7,8,9,10,11,12,13,14, 36,40,44,48,52,56,60,64},22},//MKK1
+	{{1,2,3,4,5,6,7,8,9,10,11,12,13,36,40,44,48,52,56,60,64},21},	//Israel.
+	{{1,2,3,4,5,6,7,8,9,10,11,12,13,34,38,42,46},17},			// For 11a , TELEC
+	{{1,2,3,4,5,6,7,8,9,10,11,12,13,14},14}					//For Global Domain. 1-11:active scan, 12-14 passive scan. //+YJ, 080626
+};
+static int r8180_wx_get_freq(struct net_device *dev,
+			     struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	return ieee80211_wx_get_freq(priv->ieee80211, a, wrqu, b);
+}
+
+
+int r8180_wx_set_key(struct net_device *dev, struct iw_request_info *info,
+		     union iwreq_data *wrqu, char *key)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	struct iw_point *erq = &(wrqu->encoding);
+
+	if(priv->ieee80211->bHwRadioOff)
+		return 0;
+
+	if (erq->flags & IW_ENCODE_DISABLED) {
+	}
+
+
+/*	i = erq->flags & IW_ENCODE_INDEX;
+	if (i < 1 || i > 4)
+*/
+
+	if (erq->length > 0) {
+
+		//int len = erq->length <= 5 ? 5 : 13;
+
+		u32* tkey= (u32*) key;
+		priv->key0[0] = tkey[0];
+		priv->key0[1] = tkey[1];
+		priv->key0[2] = tkey[2];
+		priv->key0[3] = tkey[3] &0xff;
+		DMESG("Setting wep key to %x %x %x %x",
+		      tkey[0],tkey[1],tkey[2],tkey[3]);
+		rtl8180_set_hw_wep(dev);
+	}
+	return 0;
+}
+
+
+static int r8180_wx_set_beaconinterval(struct net_device *dev, struct iw_request_info *aa,
+			  union iwreq_data *wrqu, char *b)
+{
+	int *parms = (int *)b;
+	int bi = parms[0];
+
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	if(priv->ieee80211->bHwRadioOff)
+		return 0;
+
+	down(&priv->wx_sem);
+	DMESG("setting beacon interval to %x",bi);
+
+	priv->ieee80211->current_network.beacon_interval=bi;
+	rtl8180_commit(dev);
+	up(&priv->wx_sem);
+
+	return 0;
+}
+
+
+
+static int r8180_wx_get_mode(struct net_device *dev, struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	return ieee80211_wx_get_mode(priv->ieee80211,a,wrqu,b);
+}
+
+
+
+static int r8180_wx_get_rate(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	return ieee80211_wx_get_rate(priv->ieee80211,info,wrqu,extra);
+}
+
+
+
+static int r8180_wx_set_rate(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	int ret;
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+
+	if(priv->ieee80211->bHwRadioOff)
+		return 0;
+
+	down(&priv->wx_sem);
+
+	ret = ieee80211_wx_set_rate(priv->ieee80211,info,wrqu,extra);
+
+	up(&priv->wx_sem);
+
+	return ret;
+}
+
+
+static int r8180_wx_set_crcmon(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	int *parms = (int *)extra;
+	int enable = (parms[0] > 0);
+	short prev = priv->crcmon;
+
+
+	if(priv->ieee80211->bHwRadioOff)
+		return 0;
+
+	down(&priv->wx_sem);
+
+	if(enable)
+		priv->crcmon=1;
+	else
+		priv->crcmon=0;
+
+	DMESG("bad CRC in monitor mode are %s",
+	      priv->crcmon ? "accepted" : "rejected");
+
+	if(prev != priv->crcmon && priv->up){
+		rtl8180_down(dev);
+		rtl8180_up(dev);
+	}
+
+	up(&priv->wx_sem);
+
+	return 0;
+}
+
+
+static int r8180_wx_set_mode(struct net_device *dev, struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	int ret;
+
+
+	if(priv->ieee80211->bHwRadioOff)
+		return 0;
+
+	down(&priv->wx_sem);
+#ifdef ENABLE_IPS
+//	printk("set mode ENABLE_IPS\n");
+	if(priv->bInactivePs){
+		if(wrqu->mode == IW_MODE_ADHOC)
+			IPSLeave(dev);
+	}
+#endif
+	ret = ieee80211_wx_set_mode(priv->ieee80211,a,wrqu,b);
+
+	//rtl8180_commit(dev);
+
+	up(&priv->wx_sem);
+	return ret;
+}
+
+//YJ,add,080819,for hidden ap
+struct  iw_range_with_scan_capa
+{
+        /* Informative stuff (to choose between different interface) */
+        __u32           throughput;     /* To give an idea... */
+        /* In theory this value should be the maximum benchmarked
+         * TCP/IP throughput, because with most of these devices the
+         * bit rate is meaningless (overhead an co) to estimate how
+         * fast the connection will go and pick the fastest one.
+         * I suggest people to play with Netperf or any benchmark...
+         */
+
+        /* NWID (or domain id) */
+        __u32           min_nwid;       /* Minimal NWID we are able to set */
+        __u32           max_nwid;       /* Maximal NWID we are able to set */
+
+        /* Old Frequency (backward compat - moved lower ) */
+        __u16           old_num_channels;
+        __u8            old_num_frequency;
+
+        /* Scan capabilities */
+        __u8            scan_capa;
+};
+//YJ,add,080819,for hidden ap
+
+
+static int rtl8180_wx_get_range(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra)
+{
+	struct iw_range *range = (struct iw_range *)extra;
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	u16 val;
+	int i;
+	//struct iw_range_with_scan_capa* tmp = (struct iw_range_with_scan_capa*)range; //YJ,add,080819,for hidden ap
+
+	wrqu->data.length = sizeof(*range);
+	memset(range, 0, sizeof(*range));
+
+	/* Let's try to keep this struct in the same order as in
+	 * linux/include/wireless.h
+	 */
+
+	/* TODO: See what values we can set, and remove the ones we can't
+	 * set, or fill them with some default data.
+	 */
+
+	/* ~5 Mb/s real (802.11b) */
+	range->throughput = 5 * 1000 * 1000;
+
+	// TODO: Not used in 802.11b?
+//	range->min_nwid;	/* Minimal NWID we are able to set */
+	// TODO: Not used in 802.11b?
+//	range->max_nwid;	/* Maximal NWID we are able to set */
+
+        /* Old Frequency (backward compat - moved lower ) */
+//	range->old_num_channels;
+//	range->old_num_frequency;
+//	range->old_freq[6]; /* Filler to keep "version" at the same offset */
+	if(priv->rf_set_sens != NULL)
+		range->sensitivity = priv->max_sens;	/* signal level threshold range */
+
+	range->max_qual.qual = 100;
+	/* TODO: Find real max RSSI and stick here */
+	range->max_qual.level = 0;
+	range->max_qual.noise = -98;
+	range->max_qual.updated = 7; /* Updated all three */
+
+	range->avg_qual.qual = 92; /* > 8% missed beacons is 'bad' */
+	/* TODO: Find real 'good' to 'bad' threshol value for RSSI */
+	range->avg_qual.level = 20 + -98;
+	range->avg_qual.noise = 0;
+	range->avg_qual.updated = 7; /* Updated all three */
+
+	range->num_bitrates = RATE_COUNT;
+
+	for (i = 0; i < RATE_COUNT && i < IW_MAX_BITRATES; i++) {
+		range->bitrate[i] = rtl8180_rates[i];
+	}
+
+	range->min_frag = MIN_FRAG_THRESHOLD;
+	range->max_frag = MAX_FRAG_THRESHOLD;
+
+	range->pm_capa = 0;
+
+	range->we_version_compiled = WIRELESS_EXT;
+	range->we_version_source = 16;
+
+//	range->retry_capa;	/* What retry options are supported */
+//	range->retry_flags;	/* How to decode max/min retry limit */
+//	range->r_time_flags;	/* How to decode max/min retry life */
+//	range->min_retry;	/* Minimal number of retries */
+//	range->max_retry;	/* Maximal number of retries */
+//	range->min_r_time;	/* Minimal retry lifetime */
+//	range->max_r_time;	/* Maximal retry lifetime */
+
+        range->num_channels = 14;
+
+	for (i = 0, val = 0; i < 14; i++) {
+
+		// Include only legal frequencies for some countries
+#ifdef ENABLE_DOT11D
+		if ((GET_DOT11D_INFO(priv->ieee80211)->channel_map)[i+1]) {
+#else
+		if ((priv->ieee80211->channel_map)[i+1]) {
+#endif
+		        range->freq[val].i = i + 1;
+			range->freq[val].m = ieee80211_wlan_frequencies[i] * 100000;
+			range->freq[val].e = 1;
+			val++;
+		} else {
+			// FIXME: do we need to set anything for channels
+			// we don't use ?
+		}
+
+		if (val == IW_MAX_FREQUENCIES)
+		break;
+	}
+
+	range->num_frequency = val;
+	range->enc_capa = IW_ENC_CAPA_WPA | IW_ENC_CAPA_WPA2 |
+                          IW_ENC_CAPA_CIPHER_TKIP | IW_ENC_CAPA_CIPHER_CCMP;
+
+	//tmp->scan_capa = 0x01; //YJ,add,080819,for hidden ap
+
+	return 0;
+}
+
+
+static int r8180_wx_set_scan(struct net_device *dev, struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	int ret;
+	struct ieee80211_device* ieee = priv->ieee80211;
+
+
+	if(priv->ieee80211->bHwRadioOff)
+		return 0;
+
+//YJ,add,080819, for hidden ap
+	//printk("==*&*&*&==>%s in\n", __func__);
+	//printk("=*&*&*&*===>flag:%x, %x\n", wrqu->data.flags, IW_SCAN_THIS_ESSID);
+	if (wrqu->data.flags & IW_SCAN_THIS_ESSID)
+	{
+		struct iw_scan_req* req = (struct iw_scan_req*)b;
+		if (req->essid_len)
+		{
+			//printk("==**&*&*&**===>scan set ssid:%s\n", req->essid);
+			ieee->current_network.ssid_len = req->essid_len;
+			memcpy(ieee->current_network.ssid, req->essid, req->essid_len);
+			//printk("=====>network ssid:%s\n", ieee->current_network.ssid);
+		}
+	}
+//YJ,add,080819, for hidden ap, end
+
+	down(&priv->wx_sem);
+	if(priv->up){
+#ifdef ENABLE_IPS
+//		printk("set scan ENABLE_IPS\n");
+		priv->ieee80211->actscanning = true;
+		if(priv->bInactivePs && (priv->ieee80211->state != IEEE80211_LINKED)){
+			IPSLeave(dev);
+//			down(&priv->ieee80211->wx_sem);
+
+//			if (priv->ieee80211->iw_mode == IW_MODE_MONITOR || !(priv->ieee80211->proto_started)){
+//				ret = -1;
+//				up(&priv->ieee80211->wx_sem);
+//				up(&priv->wx_sem);
+//				return ret;
+//			}
+
+	//	queue_work(priv->ieee80211->wq, &priv->ieee80211->wx_sync_scan_wq);
+		//printk("start scan============================>\n");
+		ieee80211_softmac_ips_scan_syncro(priv->ieee80211);
+//ieee80211_start_scan(priv->ieee80211);
+		/* intentionally forget to up sem */
+//			up(&priv->ieee80211->wx_sem);
+			ret = 0;
+		}
+		else
+#endif
+		{
+			//YJ,add,080828, prevent scan in BusyTraffic
+			//FIXME: Need to consider last scan time
+			if ((priv->link_detect.bBusyTraffic) && (true))
+			{
+				ret = 0;
+				printk("Now traffic is busy, please try later!\n");
+			}
+			else
+			//YJ,add,080828, prevent scan in BusyTraffic,end
+				ret = ieee80211_wx_set_scan(priv->ieee80211,a,wrqu,b);
+		}
+	}
+	else
+		ret = -1;
+
+	up(&priv->wx_sem);
+
+	return ret;
+}
+
+
+static int r8180_wx_get_scan(struct net_device *dev, struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b)
+{
+
+	int ret;
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	down(&priv->wx_sem);
+	if(priv->up)
+		ret = ieee80211_wx_get_scan(priv->ieee80211,a,wrqu,b);
+	else
+		ret = -1;
+
+	up(&priv->wx_sem);
+	return ret;
+}
+
+
+static int r8180_wx_set_essid(struct net_device *dev,
+			      struct iw_request_info *a,
+			      union iwreq_data *wrqu, char *b)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	int ret;
+
+	if(priv->ieee80211->bHwRadioOff)
+		return 0;
+
+	down(&priv->wx_sem);
+#ifdef ENABLE_IPS
+	//printk("set essid ENABLE_IPS\n");
+	if(priv->bInactivePs)
+		IPSLeave(dev);
+#endif
+//	printk("haha:set essid %s essid_len = %d essid_flgs = %d\n",b,  wrqu->essid.length, wrqu->essid.flags);
+
+	ret = ieee80211_wx_set_essid(priv->ieee80211,a,wrqu,b);
+
+	up(&priv->wx_sem);
+	return ret;
+}
+
+
+static int r8180_wx_get_essid(struct net_device *dev,
+			      struct iw_request_info *a,
+			      union iwreq_data *wrqu, char *b)
+{
+	int ret;
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	down(&priv->wx_sem);
+
+	ret = ieee80211_wx_get_essid(priv->ieee80211, a, wrqu, b);
+
+	up(&priv->wx_sem);
+
+	return ret;
+}
+
+
+static int r8180_wx_set_freq(struct net_device *dev, struct iw_request_info *a,
+			     union iwreq_data *wrqu, char *b)
+{
+	int ret;
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+
+	if(priv->ieee80211->bHwRadioOff)
+		return 0;
+
+	down(&priv->wx_sem);
+
+	ret = ieee80211_wx_set_freq(priv->ieee80211, a, wrqu, b);
+
+	up(&priv->wx_sem);
+	return ret;
+}
+
+
+static int r8180_wx_get_name(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	return ieee80211_wx_get_name(priv->ieee80211, info, wrqu, extra);
+}
+
+static int r8180_wx_set_frag(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	if(priv->ieee80211->bHwRadioOff)
+		return 0;
+
+	if (wrqu->frag.disabled)
+		priv->ieee80211->fts = DEFAULT_FRAG_THRESHOLD;
+	else {
+		if (wrqu->frag.value < MIN_FRAG_THRESHOLD ||
+		    wrqu->frag.value > MAX_FRAG_THRESHOLD)
+			return -EINVAL;
+
+		priv->ieee80211->fts = wrqu->frag.value & ~0x1;
+	}
+
+	return 0;
+}
+
+
+static int r8180_wx_get_frag(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	wrqu->frag.value = priv->ieee80211->fts;
+	wrqu->frag.fixed = 0;	/* no auto select */
+	wrqu->frag.disabled = (wrqu->frag.value == DEFAULT_FRAG_THRESHOLD);
+
+	return 0;
+}
+
+
+static int r8180_wx_set_wap(struct net_device *dev,
+			 struct iw_request_info *info,
+			 union iwreq_data *awrq,
+			 char *extra)
+{
+	int ret;
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	if(priv->ieee80211->bHwRadioOff)
+		return 0;
+
+	down(&priv->wx_sem);
+
+	ret = ieee80211_wx_set_wap(priv->ieee80211,info,awrq,extra);
+
+	up(&priv->wx_sem);
+	return ret;
+
+}
+
+
+static int r8180_wx_get_wap(struct net_device *dev,
+			    struct iw_request_info *info,
+			    union iwreq_data *wrqu, char *extra)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	return ieee80211_wx_get_wap(priv->ieee80211,info,wrqu,extra);
+}
+
+
+static int r8180_wx_set_enc(struct net_device *dev,
+			    struct iw_request_info *info,
+			    union iwreq_data *wrqu, char *key)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	int ret;
+
+	if(priv->ieee80211->bHwRadioOff)
+		return 0;
+
+
+	down(&priv->wx_sem);
+
+	if(priv->hw_wep) ret = r8180_wx_set_key(dev,info,wrqu,key);
+	else{
+		DMESG("Setting SW wep key");
+		ret = ieee80211_wx_set_encode(priv->ieee80211,info,wrqu,key);
+	}
+
+	up(&priv->wx_sem);
+	return ret;
+}
+
+
+static int r8180_wx_get_enc(struct net_device *dev,
+			    struct iw_request_info *info,
+			    union iwreq_data *wrqu, char *key)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	return ieee80211_wx_get_encode(priv->ieee80211, info, wrqu, key);
+}
+
+
+static int r8180_wx_set_scan_type(struct net_device *dev, struct iw_request_info *aa, union
+ iwreq_data *wrqu, char *p){
+
+ 	struct r8180_priv *priv = ieee80211_priv(dev);
+	int *parms=(int*)p;
+	int mode=parms[0];
+
+	if(priv->ieee80211->bHwRadioOff)
+		return 0;
+
+	priv->ieee80211->active_scan = mode;
+
+	return 1;
+}
+
+
+/* added by christian */
+/*
+static int r8180_wx_set_monitor_type(struct net_device *dev, struct iw_request_info *aa, union
+ iwreq_data *wrqu, char *p){
+
+ 	struct r8180_priv *priv = ieee80211_priv(dev);
+	int *parms=(int*)p;
+	int mode=parms[0];
+
+	if(priv->ieee80211->iw_mode != IW_MODE_MONITOR) return -1;
+  	priv->prism_hdr = mode;
+	if(!mode)dev->type=ARPHRD_IEEE80211;
+	else dev->type=ARPHRD_IEEE80211_PRISM;
+	DMESG("using %s RX encap", mode ? "AVS":"80211");
+	return 0;
+
+}
+*/
+//of         r8180_wx_set_monitor_type
+/* end added christian */
+
+static int r8180_wx_set_retry(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	int err = 0;
+
+	if(priv->ieee80211->bHwRadioOff)
+		return 0;
+
+	down(&priv->wx_sem);
+
+	if (wrqu->retry.flags & IW_RETRY_LIFETIME ||
+	    wrqu->retry.disabled){
+		err = -EINVAL;
+		goto exit;
+	}
+	if (!(wrqu->retry.flags & IW_RETRY_LIMIT)){
+		err = -EINVAL;
+		goto exit;
+	}
+
+	if(wrqu->retry.value > R8180_MAX_RETRY){
+		err= -EINVAL;
+		goto exit;
+	}
+	if (wrqu->retry.flags & IW_RETRY_MAX) {
+		priv->retry_rts = wrqu->retry.value;
+		DMESG("Setting retry for RTS/CTS data to %d", wrqu->retry.value);
+
+	}else {
+		priv->retry_data = wrqu->retry.value;
+		DMESG("Setting retry for non RTS/CTS data to %d", wrqu->retry.value);
+	}
+
+	/* FIXME !
+	 * We might try to write directly the TX config register
+	 * or to restart just the (R)TX process.
+	 * I'm unsure if whole reset is really needed
+	 */
+
+ 	rtl8180_commit(dev);
+	/*
+	if(priv->up){
+		rtl8180_rtx_disable(dev);
+		rtl8180_rx_enable(dev);
+		rtl8180_tx_enable(dev);
+
+	}
+	*/
+exit:
+	up(&priv->wx_sem);
+
+	return err;
+}
+
+static int r8180_wx_get_retry(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+
+	wrqu->retry.disabled = 0; /* can't be disabled */
+
+	if ((wrqu->retry.flags & IW_RETRY_TYPE) ==
+	    IW_RETRY_LIFETIME)
+		return -EINVAL;
+
+	if (wrqu->retry.flags & IW_RETRY_MAX) {
+		wrqu->retry.flags = IW_RETRY_LIMIT & IW_RETRY_MAX;
+		wrqu->retry.value = priv->retry_rts;
+	} else {
+		wrqu->retry.flags = IW_RETRY_LIMIT & IW_RETRY_MIN;
+		wrqu->retry.value = priv->retry_data;
+	}
+	//DMESG("returning %d",wrqu->retry.value);
+
+
+	return 0;
+}
+
+static int r8180_wx_get_sens(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	if(priv->rf_set_sens == NULL)
+		return -1; /* we have not this support for this radio */
+	wrqu->sens.value = priv->sens;
+	return 0;
+}
+
+
+static int r8180_wx_set_sens(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra)
+{
+
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	short err = 0;
+
+	if(priv->ieee80211->bHwRadioOff)
+		return 0;
+
+	down(&priv->wx_sem);
+	//DMESG("attempt to set sensivity to %ddb",wrqu->sens.value);
+	if(priv->rf_set_sens == NULL) {
+		err= -1; /* we have not this support for this radio */
+		goto exit;
+	}
+	if(priv->rf_set_sens(dev, wrqu->sens.value) == 0)
+		priv->sens = wrqu->sens.value;
+	else
+		err= -EINVAL;
+
+exit:
+	up(&priv->wx_sem);
+
+	return err;
+}
+
+
+static int r8180_wx_set_rawtx(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	int ret;
+
+	if(priv->ieee80211->bHwRadioOff)
+		return 0;
+
+	down(&priv->wx_sem);
+
+	ret = ieee80211_wx_set_rawtx(priv->ieee80211, info, wrqu, extra);
+
+	up(&priv->wx_sem);
+
+	return ret;
+
+}
+
+static int r8180_wx_get_power(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	int ret;
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+	down(&priv->wx_sem);
+
+	ret = ieee80211_wx_get_power(priv->ieee80211, info, wrqu, extra);
+
+	up(&priv->wx_sem);
+
+	return ret;
+}
+
+static int r8180_wx_set_power(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	int ret;
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+
+	if(priv->ieee80211->bHwRadioOff)
+		return 0;
+
+	down(&priv->wx_sem);
+	printk("=>>>>>>>>>>=============================>set power:%d,%d!\n",wrqu->power.disabled, wrqu->power.flags);
+	if (wrqu->power.disabled==0) {
+		wrqu->power.flags|=IW_POWER_ALL_R;
+		wrqu->power.flags|=IW_POWER_TIMEOUT;
+		wrqu->power.value =1000;
+	}
+
+	ret = ieee80211_wx_set_power(priv->ieee80211, info, wrqu, extra);
+
+	up(&priv->wx_sem);
+
+	return ret;
+}
+
+static int r8180_wx_set_rts(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+
+	if(priv->ieee80211->bHwRadioOff)
+		return 0;
+
+	if (wrqu->rts.disabled)
+		priv->rts = DEFAULT_RTS_THRESHOLD;
+	else {
+		if (wrqu->rts.value < MIN_RTS_THRESHOLD ||
+		    wrqu->rts.value > MAX_RTS_THRESHOLD)
+			return -EINVAL;
+
+		priv->rts = wrqu->rts.value;
+	}
+
+	return 0;
+}
+static int r8180_wx_get_rts(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+
+
+	wrqu->rts.value = priv->rts;
+	wrqu->rts.fixed = 0;	/* no auto select */
+	wrqu->rts.disabled = (wrqu->rts.value == 0);
+
+	return 0;
+}
+static int dummy(struct net_device *dev, struct iw_request_info *a,
+		 union iwreq_data *wrqu,char *b)
+{
+	return -1;
+}
+
+/*
+static int r8180_wx_get_psmode(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	struct ieee80211_device *ieee;
+	int ret = 0;
+
+
+
+	down(&priv->wx_sem);
+
+	if(priv) {
+		ieee = priv->ieee80211;
+		if(ieee->ps == IEEE80211_PS_DISABLED) {
+			*((unsigned int *)extra) = IEEE80211_PS_DISABLED;
+			goto exit;
+		}
+		*((unsigned int *)extra) = IW_POWER_TIMEOUT;
+ 	if (ieee->ps & IEEE80211_PS_MBCAST)
+			*((unsigned int *)extra) |= IW_POWER_ALL_R;
+		else
+			*((unsigned int *)extra) |= IW_POWER_UNICAST_R;
+	} else
+		ret = -1;
+exit:
+	up(&priv->wx_sem);
+
+	return ret;
+}
+static int r8180_wx_set_psmode(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	//struct ieee80211_device *ieee;
+	int ret = 0;
+
+
+
+	down(&priv->wx_sem);
+
+	ret = ieee80211_wx_set_power(priv->ieee80211, info, wrqu, extra);
+
+	up(&priv->wx_sem);
+
+	return ret;
+
+}
+*/
+
+static int r8180_wx_get_iwmode(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	struct ieee80211_device *ieee;
+	int ret = 0;
+
+
+
+	down(&priv->wx_sem);
+
+	ieee = priv->ieee80211;
+
+	strcpy(extra, "802.11");
+	if(ieee->modulation & IEEE80211_CCK_MODULATION) {
+		strcat(extra, "b");
+		if(ieee->modulation & IEEE80211_OFDM_MODULATION)
+			strcat(extra, "/g");
+	} else if(ieee->modulation & IEEE80211_OFDM_MODULATION)
+		strcat(extra, "g");
+
+	up(&priv->wx_sem);
+
+	return ret;
+}
+static int r8180_wx_set_iwmode(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	struct ieee80211_device *ieee = priv->ieee80211;
+	int *param = (int *)extra;
+	int ret = 0;
+	int modulation = 0, mode = 0;
+
+
+	if(priv->ieee80211->bHwRadioOff)
+		return 0;
+
+	down(&priv->wx_sem);
+
+	if (*param == 1) {
+		modulation |= IEEE80211_CCK_MODULATION;
+		mode = IEEE_B;
+	printk(KERN_INFO "B mode!\n");
+	} else if (*param == 2) {
+		modulation |= IEEE80211_OFDM_MODULATION;
+		mode = IEEE_G;
+	printk(KERN_INFO "G mode!\n");
+	} else if (*param == 3) {
+		modulation |= IEEE80211_CCK_MODULATION;
+		modulation |= IEEE80211_OFDM_MODULATION;
+		mode = IEEE_B|IEEE_G;
+	printk(KERN_INFO "B/G mode!\n");
+	}
+
+	if(ieee->proto_started) {
+		ieee80211_stop_protocol(ieee);
+		ieee->mode = mode;
+		ieee->modulation = modulation;
+		ieee80211_start_protocol(ieee);
+	} else {
+		ieee->mode = mode;
+		ieee->modulation = modulation;
+//		ieee80211_start_protocol(ieee);
+	}
+
+	up(&priv->wx_sem);
+
+	return ret;
+}
+static int r8180_wx_get_preamble(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+
+
+	down(&priv->wx_sem);
+
+
+
+	*extra = (char) priv->plcp_preamble_mode; 	// 0:auto 1:short 2:long
+	up(&priv->wx_sem);
+
+	return 0;
+}
+static int r8180_wx_set_preamble(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	int ret = 0;
+
+
+	if(priv->ieee80211->bHwRadioOff)
+		return 0;
+
+	down(&priv->wx_sem);
+	if (*extra<0||*extra>2)
+		ret = -1;
+	else
+		priv->plcp_preamble_mode = *((short *)extra) ;
+
+
+
+	up(&priv->wx_sem);
+
+	return ret;
+}
+static int r8180_wx_get_siglevel(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	//struct ieee80211_network *network = &(priv->ieee80211->current_network);
+	int ret = 0;
+
+
+
+	down(&priv->wx_sem);
+	// Modify by hikaru 6.5
+	*((int *)extra) = priv->wstats.qual.level;//for interface test ,it should be the priv->wstats.qual.level;
+
+
+
+	up(&priv->wx_sem);
+
+	return ret;
+}
+static int r8180_wx_get_sigqual(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	//struct ieee80211_network *network = &(priv->ieee80211->current_network);
+	int ret = 0;
+
+
+
+	down(&priv->wx_sem);
+	// Modify by hikaru 6.5
+	*((int *)extra) = priv->wstats.qual.qual;//for interface test ,it should be the priv->wstats.qual.qual;
+
+
+
+	up(&priv->wx_sem);
+
+	return ret;
+}
+static int r8180_wx_reset_stats(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra)
+{
+	struct r8180_priv *priv =ieee80211_priv(dev);
+	down(&priv->wx_sem);
+
+	priv->stats.txrdu = 0;
+	priv->stats.rxrdu = 0;
+	priv->stats.rxnolast = 0;
+	priv->stats.rxnodata = 0;
+	priv->stats.rxnopointer = 0;
+	priv->stats.txnperr = 0;
+	priv->stats.txresumed = 0;
+	priv->stats.rxerr = 0;
+	priv->stats.rxoverflow = 0;
+	priv->stats.rxint = 0;
+
+	priv->stats.txnpokint = 0;
+	priv->stats.txhpokint = 0;
+	priv->stats.txhperr = 0;
+	priv->stats.ints = 0;
+	priv->stats.shints = 0;
+	priv->stats.txoverflow = 0;
+	priv->stats.rxdmafail = 0;
+	priv->stats.txbeacon = 0;
+	priv->stats.txbeaconerr = 0;
+	priv->stats.txlpokint = 0;
+	priv->stats.txlperr = 0;
+	priv->stats.txretry =0;//20060601
+	priv->stats.rxcrcerrmin=0;
+	priv->stats.rxcrcerrmid=0;
+	priv->stats.rxcrcerrmax=0;
+	priv->stats.rxicverr=0;
+
+	up(&priv->wx_sem);
+
+	return 0;
+
+}
+static int r8180_wx_radio_on(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra)
+{
+	struct r8180_priv *priv =ieee80211_priv(dev);
+
+	if(priv->ieee80211->bHwRadioOff)
+		return 0;
+
+
+	down(&priv->wx_sem);
+	priv->rf_wakeup(dev);
+
+	up(&priv->wx_sem);
+
+	return 0;
+
+}
+
+static int r8180_wx_radio_off(struct net_device *dev,
+				struct iw_request_info *info,
+				union iwreq_data *wrqu, char *extra)
+{
+	struct r8180_priv *priv =ieee80211_priv(dev);
+
+	if(priv->ieee80211->bHwRadioOff)
+		return 0;
+
+
+	down(&priv->wx_sem);
+	priv->rf_sleep(dev);
+
+	up(&priv->wx_sem);
+
+	return 0;
+
+}
+static int r8180_wx_get_channelplan(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+
+
+	down(&priv->wx_sem);
+	*extra = priv->channel_plan;
+
+
+
+	up(&priv->wx_sem);
+
+	return 0;
+}
+static int r8180_wx_set_channelplan(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	//struct ieee80211_device *ieee = netdev_priv(dev);
+	int *val = (int *)extra;
+	int i;
+	printk("-----in fun %s\n", __FUNCTION__);
+
+	if(priv->ieee80211->bHwRadioOff)
+		return 0;
+
+	//unsigned long flags;
+	down(&priv->wx_sem);
+	if (DefaultChannelPlan[*val].Len != 0){
+		priv ->channel_plan = *val;
+		// Clear old channel map
+		for (i=1;i<=MAX_CHANNEL_NUMBER;i++)
+		{
+#ifdef ENABLE_DOT11D
+			GET_DOT11D_INFO(priv->ieee80211)->channel_map[i] = 0;
+#else
+			priv->ieee80211->channel_map[i] = 0;
+#endif
+		}
+		// Set new channel map
+		for (i=1;i<=DefaultChannelPlan[*val].Len;i++)
+		{
+#ifdef ENABLE_DOT11D
+			GET_DOT11D_INFO(priv->ieee80211)->channel_map[DefaultChannelPlan[*val].Channel[i-1]] = 1;
+#else
+			priv->ieee80211->channel_map[DefaultChannelPlan[*val].Channel[i-1]] = 1;
+#endif
+		}
+	}
+	up(&priv->wx_sem);
+
+	return 0;
+}
+
+static int r8180_wx_get_version(struct net_device *dev,
+			       struct iw_request_info *info,
+			       union iwreq_data *wrqu, char *extra)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	//struct ieee80211_device *ieee;
+
+	down(&priv->wx_sem);
+	strcpy(extra, "1020.0808");
+	up(&priv->wx_sem);
+
+	return 0;
+}
+
+//added by amy 080818
+//receive datarate from user typing valid rate is from 2 to 108 (1 - 54M), if input 0, return to normal rate adaptive.
+static int r8180_wx_set_forcerate(struct net_device *dev,
+			     struct iw_request_info *info,
+			     union iwreq_data *wrqu, char *extra)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	u8 forcerate = *extra;
+
+	down(&priv->wx_sem);
+
+	printk("==============>%s(): forcerate is %d\n",__FUNCTION__,forcerate);
+	if((forcerate == 2) || (forcerate == 4) || (forcerate == 11) || (forcerate == 22) || (forcerate == 12) ||
+		(forcerate == 18) || (forcerate == 24) || (forcerate == 36) || (forcerate == 48) || (forcerate == 72) ||
+		(forcerate == 96) || (forcerate == 108))
+	{
+		priv->ForcedDataRate = 1;
+		priv->ieee80211->rate = forcerate * 5;
+	}
+	else if(forcerate == 0)
+	{
+		priv->ForcedDataRate = 0;
+		printk("OK! return rate adaptive\n");
+	}
+	else
+		printk("ERR: wrong rate\n");
+	up(&priv->wx_sem);
+	return 0;
+}
+
+static int r8180_wx_set_enc_ext(struct net_device *dev,
+                                        struct iw_request_info *info,
+                                        union iwreq_data *wrqu, char *extra)
+{
+
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	//printk("===>%s()\n", __FUNCTION__);
+
+	int ret=0;
+
+	if(priv->ieee80211->bHwRadioOff)
+		return 0;
+
+	down(&priv->wx_sem);
+	ret = ieee80211_wx_set_encode_ext(priv->ieee80211, info, wrqu, extra);
+	up(&priv->wx_sem);
+	return ret;
+
+}
+static int r8180_wx_set_auth(struct net_device *dev,
+                                        struct iw_request_info *info,
+                                        struct iw_param *data, char *extra)
+{
+	//printk("====>%s()\n", __FUNCTION__);
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	int ret=0;
+
+	if(priv->ieee80211->bHwRadioOff)
+		return 0;
+
+	down(&priv->wx_sem);
+	ret = ieee80211_wx_set_auth(priv->ieee80211, info, data, extra);
+	up(&priv->wx_sem);
+	return ret;
+}
+
+static int r8180_wx_set_mlme(struct net_device *dev,
+                                        struct iw_request_info *info,
+                                        union iwreq_data *wrqu, char *extra)
+{
+	//printk("====>%s()\n", __FUNCTION__);
+
+	int ret=0;
+	struct r8180_priv *priv = ieee80211_priv(dev);
+
+
+	if(priv->ieee80211->bHwRadioOff)
+		return 0;
+
+
+	down(&priv->wx_sem);
+#if 1
+	ret = ieee80211_wx_set_mlme(priv->ieee80211, info, wrqu, extra);
+#endif
+	up(&priv->wx_sem);
+	return ret;
+}
+static int r8180_wx_set_gen_ie(struct net_device *dev,
+                                        struct iw_request_info *info,
+                                        struct iw_point *data, char *extra)
+{
+//	printk("====>%s(), len:%d\n", __FUNCTION__, data->length);
+	int ret=0;
+        struct r8180_priv *priv = ieee80211_priv(dev);
+
+
+	if(priv->ieee80211->bHwRadioOff)
+		return 0;
+
+        down(&priv->wx_sem);
+#if 1
+        ret = ieee80211_wx_set_gen_ie(priv->ieee80211, extra, data->length);
+#endif
+        up(&priv->wx_sem);
+	//printk("<======%s(), ret:%d\n", __FUNCTION__, ret);
+        return ret;
+
+
+}
+static iw_handler r8180_wx_handlers[] =
+{
+        NULL,                     /* SIOCSIWCOMMIT */
+        r8180_wx_get_name,   	  /* SIOCGIWNAME */
+        dummy,                    /* SIOCSIWNWID */
+        dummy,                    /* SIOCGIWNWID */
+        r8180_wx_set_freq,        /* SIOCSIWFREQ */
+        r8180_wx_get_freq,        /* SIOCGIWFREQ */
+        r8180_wx_set_mode,        /* SIOCSIWMODE */
+        r8180_wx_get_mode,        /* SIOCGIWMODE */
+        r8180_wx_set_sens,        /* SIOCSIWSENS */
+        r8180_wx_get_sens,        /* SIOCGIWSENS */
+        NULL,                     /* SIOCSIWRANGE */
+        rtl8180_wx_get_range,	  /* SIOCGIWRANGE */
+        NULL,                     /* SIOCSIWPRIV */
+        NULL,                     /* SIOCGIWPRIV */
+        NULL,                     /* SIOCSIWSTATS */
+        NULL,                     /* SIOCGIWSTATS */
+        dummy,                    /* SIOCSIWSPY */
+        dummy,                    /* SIOCGIWSPY */
+        NULL,                     /* SIOCGIWTHRSPY */
+        NULL,                     /* SIOCWIWTHRSPY */
+        r8180_wx_set_wap,      	  /* SIOCSIWAP */
+        r8180_wx_get_wap,         /* SIOCGIWAP */
+	r8180_wx_set_mlme,        /* SIOCSIWMLME*/
+        dummy,                    /* SIOCGIWAPLIST -- depricated */
+        r8180_wx_set_scan,        /* SIOCSIWSCAN */
+        r8180_wx_get_scan,        /* SIOCGIWSCAN */
+        r8180_wx_set_essid,       /* SIOCSIWESSID */
+        r8180_wx_get_essid,       /* SIOCGIWESSID */
+        dummy,                    /* SIOCSIWNICKN */
+        dummy,                    /* SIOCGIWNICKN */
+        NULL,                     /* -- hole -- */
+        NULL,                     /* -- hole -- */
+        r8180_wx_set_rate,        /* SIOCSIWRATE */
+        r8180_wx_get_rate,        /* SIOCGIWRATE */
+        r8180_wx_set_rts,         /* SIOCSIWRTS */
+        r8180_wx_get_rts,         /* SIOCGIWRTS */
+        r8180_wx_set_frag,        /* SIOCSIWFRAG */
+        r8180_wx_get_frag,        /* SIOCGIWFRAG */
+        dummy,                    /* SIOCSIWTXPOW */
+        dummy,                    /* SIOCGIWTXPOW */
+        r8180_wx_set_retry,       /* SIOCSIWRETRY */
+        r8180_wx_get_retry,       /* SIOCGIWRETRY */
+        r8180_wx_set_enc,         /* SIOCSIWENCODE */
+        r8180_wx_get_enc,         /* SIOCGIWENCODE */
+        r8180_wx_set_power,       /* SIOCSIWPOWER */
+        r8180_wx_get_power,       /* SIOCGIWPOWER */
+        NULL,			  /*---hole---*/
+	NULL, 			  /*---hole---*/
+	r8180_wx_set_gen_ie,      /* SIOCSIWGENIE */
+	NULL, 			  /* SIOCSIWGENIE */
+	r8180_wx_set_auth,	  /* SIOCSIWAUTH */
+	NULL,                     /* SIOCSIWAUTH */
+	r8180_wx_set_enc_ext, 	  /* SIOCSIWENCODEEXT */
+	NULL,                  	  /* SIOCSIWENCODEEXT */
+	NULL, 			 /* SIOCSIWPMKSA */
+	NULL, 			  /*---hole---*/
+};
+
+
+static const struct iw_priv_args r8180_private_args[] = {
+	{
+		SIOCIWFIRSTPRIV + 0x0,
+		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "badcrc"
+	},
+	{	SIOCIWFIRSTPRIV + 0x1,
+		0, 0, "dummy"
+
+	},
+	{
+		SIOCIWFIRSTPRIV + 0x2,
+		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "beaconint"
+	},
+	{	SIOCIWFIRSTPRIV + 0x3,
+		0, 0, "dummy"
+
+	},
+	/* added by christian */
+	//{
+	//	SIOCIWFIRSTPRIV + 0x2,
+	//	IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "prismhdr"
+	//},
+	/* end added by christian */
+	{
+		SIOCIWFIRSTPRIV + 0x4,
+		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "activescan"
+
+	},
+	{	SIOCIWFIRSTPRIV + 0x5,
+		0, 0, "dummy"
+
+	},
+	{
+		SIOCIWFIRSTPRIV + 0x6,
+		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "rawtx"
+
+	},
+	{	SIOCIWFIRSTPRIV + 0x7,
+		0, 0, "dummy"
+
+	},
+//	{
+//		SIOCIWFIRSTPRIV + 0x5,
+//		0, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, "getpsmode"
+//	},
+//	{
+//		SIOCIWFIRSTPRIV + 0x6,
+//		IW_PRIV_SIZE_FIXED, 0, "setpsmode"
+//	},
+//set/get mode have been realized in public handlers
+
+	{
+		SIOCIWFIRSTPRIV + 0x8,
+		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "setiwmode"
+	},
+	{
+		SIOCIWFIRSTPRIV + 0x9,
+		0, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | 32, "getiwmode"
+	},
+	{
+		SIOCIWFIRSTPRIV + 0xA,
+		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "setpreamble"
+	},
+	{
+		SIOCIWFIRSTPRIV + 0xB,
+		0, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, "getpreamble"
+	},
+	{	SIOCIWFIRSTPRIV + 0xC,
+		0, 0, "dummy"
+	},
+	{
+		SIOCIWFIRSTPRIV + 0xD,
+		0, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, "getrssi"
+	},
+	{	SIOCIWFIRSTPRIV + 0xE,
+		0, 0, "dummy"
+	},
+	{
+		SIOCIWFIRSTPRIV + 0xF,
+		0, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, "getlinkqual"
+ 	},
+	{
+		SIOCIWFIRSTPRIV + 0x10,
+		0, 0, "resetstats"
+ 	},
+	{
+		SIOCIWFIRSTPRIV + 0x11,
+		0,0, "dummy"
+ 	},
+	{
+		SIOCIWFIRSTPRIV + 0x12,
+		0, 0, "radioon"
+ 	},
+	{
+		SIOCIWFIRSTPRIV + 0x13,
+		0, 0, "radiooff"
+ 	},
+ 	{
+		SIOCIWFIRSTPRIV + 0x14,
+		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "setchannel"
+ 	},
+	{
+		SIOCIWFIRSTPRIV + 0x15,
+		0, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, "getchannel"
+ 	},
+	{
+		SIOCIWFIRSTPRIV + 0x16,
+		0,0, "dummy"
+ 	},
+	{
+		SIOCIWFIRSTPRIV + 0x17,
+		0,IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | 32, "getversion"
+ 	},
+	{
+		SIOCIWFIRSTPRIV + 0x18,
+		IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "setrate"
+	},
+};
+
+
+static iw_handler r8180_private_handler[] = {
+	r8180_wx_set_crcmon,   /*SIOCIWSECONDPRIV*/
+	dummy,
+	r8180_wx_set_beaconinterval,
+	dummy,
+	//r8180_wx_set_monitor_type,
+	r8180_wx_set_scan_type,
+	dummy,
+	r8180_wx_set_rawtx,
+	dummy,
+	r8180_wx_set_iwmode,
+	r8180_wx_get_iwmode,
+	r8180_wx_set_preamble,
+	r8180_wx_get_preamble,
+	dummy,
+	r8180_wx_get_siglevel,
+	dummy,
+	r8180_wx_get_sigqual,
+	r8180_wx_reset_stats,
+	dummy,//r8180_wx_get_stats
+	r8180_wx_radio_on,
+	r8180_wx_radio_off,
+	r8180_wx_set_channelplan,
+	r8180_wx_get_channelplan,
+	dummy,
+	r8180_wx_get_version,
+	r8180_wx_set_forcerate,
+};
+
+#if WIRELESS_EXT >= 17
+static inline int is_same_network(struct ieee80211_network *src,
+                                  struct ieee80211_network *dst,
+				  struct ieee80211_device *ieee)
+{
+        /* A network is only a duplicate if the channel, BSSID, ESSID
+         * and the capability field (in particular IBSS and BSS) all match.
+         * We treat all <hidden> with the same BSSID and channel
+         * as one network */
+        return (((src->ssid_len == dst->ssid_len)||(ieee->iw_mode == IW_MODE_INFRA)) &&  //YJ,mod, 080819,for hidden ap
+			//((src->ssid_len == dst->ssid_len) &&
+			(src->channel == dst->channel) &&
+			!memcmp(src->bssid, dst->bssid, ETH_ALEN) &&
+			(!memcmp(src->ssid, dst->ssid, src->ssid_len)||(ieee->iw_mode == IW_MODE_INFRA)) &&  //YJ,mod, 080819,for hidden ap
+			//!memcmp(src->ssid, dst->ssid, src->ssid_len) &&
+			((src->capability & WLAN_CAPABILITY_IBSS) ==
+			(dst->capability & WLAN_CAPABILITY_IBSS)) &&
+			((src->capability & WLAN_CAPABILITY_BSS) ==
+			(dst->capability & WLAN_CAPABILITY_BSS)));
+}
+
+//WB modefied to show signal to GUI on 18-01-2008
+static struct iw_statistics *r8180_get_wireless_stats(struct net_device *dev)
+{
+       struct r8180_priv *priv = ieee80211_priv(dev);
+	struct ieee80211_device* ieee = priv->ieee80211;
+	struct iw_statistics* wstats = &priv->wstats;
+	//struct ieee80211_network* target = NULL;
+	int tmp_level = 0;
+	int tmp_qual = 0;
+	int tmp_noise = 0;
+	//unsigned long flag;
+
+	if (ieee->state < IEEE80211_LINKED)
+	{
+		wstats->qual.qual = 0;
+		wstats->qual.level = 0;
+		wstats->qual.noise = 0;
+		wstats->qual.updated = IW_QUAL_ALL_UPDATED | IW_QUAL_DBM;
+		return wstats;
+	}
+#if 0
+	spin_lock_irqsave(&ieee->lock, flag);
+	list_for_each_entry(target, &ieee->network_list, list)
+	{
+		if (is_same_network(target, &ieee->current_network, ieee))
+		{
+			printk("it's same network:%s\n", target->ssid);
+#if 0
+			if (!tmp_level)
+			{
+				tmp_level = target->stats.signalstrength;
+				tmp_qual = target->stats.signal;
+			}
+			else
+			{
+
+				tmp_level = (15*tmp_level + target->stats.signalstrength)/16;
+				tmp_qual = (15*tmp_qual + target->stats.signal)/16;
+			}
+#else
+			tmp_level = target->stats.signal;
+			tmp_qual = target->stats.signalstrength;
+			tmp_noise = target->stats.noise;
+			printk("level:%d, qual:%d, noise:%d\n", tmp_level, tmp_qual, tmp_noise);
+#endif
+			break;
+		}
+	}
+	spin_unlock_irqrestore(&ieee->lock, flag);
+#endif
+	tmp_level = (&ieee->current_network)->stats.signal;
+	tmp_qual = (&ieee->current_network)->stats.signalstrength;
+	tmp_noise = (&ieee->current_network)->stats.noise;
+	//printk("level:%d, qual:%d, noise:%d\n", tmp_level, tmp_qual, tmp_noise);
+
+//	printk("level:%d\n", tmp_level);
+	wstats->qual.level = tmp_level;
+	wstats->qual.qual = tmp_qual;
+	wstats->qual.noise = tmp_noise;
+	wstats->qual.updated = IW_QUAL_ALL_UPDATED| IW_QUAL_DBM;
+	return wstats;
+}
+#endif
+
+
+struct iw_handler_def  r8180_wx_handlers_def={
+	.standard = r8180_wx_handlers,
+	.num_standard = sizeof(r8180_wx_handlers) / sizeof(iw_handler),
+	.private = r8180_private_handler,
+	.num_private = sizeof(r8180_private_handler) / sizeof(iw_handler),
+ 	.num_private_args = sizeof(r8180_private_args) / sizeof(struct iw_priv_args),
+#if WIRELESS_EXT >= 17
+	.get_wireless_stats = r8180_get_wireless_stats,
+#endif
+	.private_args = (struct iw_priv_args *)r8180_private_args,
+};
+
+
diff --git a/drivers/staging/rtl8187se/r8180_wx.h b/drivers/staging/rtl8187se/r8180_wx.h
new file mode 100644
index 00000000000..b20a67d3341
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8180_wx.h
@@ -0,0 +1,21 @@
+/*
+	This is part of rtl8180 OpenSource driver - v 0.3
+	Copyright (C) Andrea Merello 2004  <andreamrl@tiscali.it>
+	Released under the terms of GPL (General Public Licence)
+
+	Parts of this driver are based on the GPL part of the official realtek driver
+	Parts of this driver are based on the rtl8180 driver skeleton from Patric Schenke & Andres Salomon
+	Parts of this driver are based on the Intel Pro Wireless 2100 GPL driver
+
+	We want to tanks the Authors of such projects and the Ndiswrapper project Authors.
+*/
+
+/* this file (will) contains wireless extension handlers*/
+
+#ifndef R8180_WX_H
+#define R8180_WX_H
+#include <linux/wireless.h>
+#include "ieee80211.h"
+extern struct iw_handler_def r8180_wx_handlers_def;
+
+#endif
diff --git a/drivers/staging/rtl8187se/r8185b_init.c b/drivers/staging/rtl8187se/r8185b_init.c
new file mode 100644
index 00000000000..4b885a2319e
--- /dev/null
+++ b/drivers/staging/rtl8187se/r8185b_init.c
@@ -0,0 +1,3342 @@
+/*++
+Copyright (c) Realtek Semiconductor Corp. All rights reserved.
+
+Module Name:
+ 	r8185b_init.c
+
+Abstract:
+ 	Hardware Initialization and Hardware IO for RTL8185B
+
+Major Change History:
+	When        Who      What
+	----------    ---------------   -------------------------------
+	2006-11-15    Xiong		Created
+
+Notes:
+	This file is ported from RTL8185B Windows driver.
+
+
+--*/
+
+/*--------------------------Include File------------------------------------*/
+#include <linux/spinlock.h>
+#include "r8180_hw.h"
+#include "r8180.h"
+#include "r8180_sa2400.h"  /* PHILIPS Radio frontend */
+#include "r8180_max2820.h" /* MAXIM Radio frontend */
+#include "r8180_gct.h"     /* GCT Radio frontend */
+#include "r8180_rtl8225.h" /* RTL8225 Radio frontend */
+#include "r8180_rtl8255.h" /* RTL8255 Radio frontend */
+#include "r8180_93cx6.h"   /* Card EEPROM */
+#include "r8180_wx.h"
+
+#ifdef CONFIG_RTL8180_PM
+#include "r8180_pm.h"
+#endif
+
+#ifdef ENABLE_DOT11D
+#include "dot11d.h"
+#endif
+
+#ifdef CONFIG_RTL8185B
+
+//#define CONFIG_RTL8180_IO_MAP
+
+#define TC_3W_POLL_MAX_TRY_CNT 5
+#ifdef CONFIG_RTL818X_S
+static u8 MAC_REG_TABLE[][2]={
+                        //PAGA 0:
+                        // 0x34(BRSR), 0xBE(RATE_FALLBACK_CTL), 0x1E0(ARFR) would set in HwConfigureRTL8185()
+                        // 0x272(RFSW_CTRL), 0x1CE(AESMSK_QC) set in InitializeAdapter8185().
+                        // 0x1F0~0x1F8  set in MacConfig_85BASIC()
+                        {0x08, 0xae}, {0x0a, 0x72}, {0x5b, 0x42},
+                        {0x84, 0x88}, {0x85, 0x24}, {0x88, 0x54}, {0x8b, 0xb8}, {0x8c, 0x03},
+                        {0x8d, 0x40}, {0x8e, 0x00}, {0x8f, 0x00}, {0x5b, 0x18}, {0x91, 0x03},
+                        {0x94, 0x0F}, {0x95, 0x32},
+                        {0x96, 0x00}, {0x97, 0x07}, {0xb4, 0x22}, {0xdb, 0x00},
+                        {0xf0, 0x32}, {0xf1, 0x32}, {0xf2, 0x00}, {0xf3, 0x00}, {0xf4, 0x32},
+                        {0xf5, 0x43}, {0xf6, 0x00}, {0xf7, 0x00}, {0xf8, 0x46}, {0xf9, 0xa4},
+                        {0xfa, 0x00}, {0xfb, 0x00}, {0xfc, 0x96}, {0xfd, 0xa4}, {0xfe, 0x00},
+                        {0xff, 0x00},
+
+                        //PAGE 1:
+                        // For Flextronics system Logo PCIHCT failure:
+			// 0x1C4~0x1CD set no-zero value to avoid PCI configuration space 0x45[7]=1
+                        {0x5e, 0x01},
+                        {0x58, 0x00}, {0x59, 0x00}, {0x5a, 0x04}, {0x5b, 0x00}, {0x60, 0x24},
+                        {0x61, 0x97}, {0x62, 0xF0}, {0x63, 0x09}, {0x80, 0x0F}, {0x81, 0xFF},
+                        {0x82, 0xFF}, {0x83, 0x03},
+                        {0xC4, 0x22}, {0xC5, 0x22}, {0xC6, 0x22}, {0xC7, 0x22}, {0xC8, 0x22}, //lzm add 080826
+			{0xC9, 0x22}, {0xCA, 0x22}, {0xCB, 0x22}, {0xCC, 0x22}, {0xCD, 0x22},//lzm add 080826
+                        {0xe2, 0x00},
+
+
+                        //PAGE 2:
+                        {0x5e, 0x02},
+                        {0x0c, 0x04}, {0x4c, 0x30}, {0x4d, 0x08}, {0x50, 0x05}, {0x51, 0xf5},
+                        {0x52, 0x04}, {0x53, 0xa0}, {0x54, 0xff}, {0x55, 0xff}, {0x56, 0xff},
+                        {0x57, 0xff}, {0x58, 0x08}, {0x59, 0x08}, {0x5a, 0x08}, {0x5b, 0x08},
+                        {0x60, 0x08}, {0x61, 0x08}, {0x62, 0x08}, {0x63, 0x08}, {0x64, 0x2f},
+                        {0x8c, 0x3f}, {0x8d, 0x3f}, {0x8e, 0x3f},
+                        {0x8f, 0x3f}, {0xc4, 0xff}, {0xc5, 0xff}, {0xc6, 0xff}, {0xc7, 0xff},
+                        {0xc8, 0x00}, {0xc9, 0x00}, {0xca, 0x80}, {0xcb, 0x00},
+
+                        //PAGA 0:
+                        {0x5e, 0x00},{0x9f, 0x03}
+                };
+
+
+static u8  ZEBRA_AGC[]={
+			0,
+			0x7E,0x7E,0x7E,0x7E,0x7D,0x7C,0x7B,0x7A,0x79,0x78,0x77,0x76,0x75,0x74,0x73,0x72,
+			0x71,0x70,0x6F,0x6E,0x6D,0x6C,0x6B,0x6A,0x69,0x68,0x67,0x66,0x65,0x64,0x63,0x62,
+			0x48,0x47,0x46,0x45,0x44,0x29,0x28,0x27,0x26,0x25,0x24,0x23,0x22,0x21,0x08,0x07,
+			0x06,0x05,0x04,0x03,0x02,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+			0x0f,0x0f,0x0f,0x0f,0x0f,0x0f,0x0f,0x0f,0x0f,0x0f,0x10,0x11,0x12,0x13,0x15,0x16,
+			0x17,0x17,0x18,0x18,0x19,0x1a,0x1a,0x1b,0x1b,0x1c,0x1c,0x1d,0x1d,0x1d,0x1e,0x1e,
+			0x1f,0x1f,0x1f,0x20,0x20,0x20,0x20,0x21,0x21,0x21,0x22,0x22,0x22,0x23,0x23,0x24,
+			0x24,0x25,0x25,0x25,0x26,0x26,0x27,0x27,0x2F,0x2F,0x2F,0x2F,0x2F,0x2F,0x2F,0x2F
+			};
+
+static u32 ZEBRA_RF_RX_GAIN_TABLE[]={
+			0x0096,0x0076,0x0056,0x0036,0x0016,0x01f6,0x01d6,0x01b6,
+			0x0196,0x0176,0x00F7,0x00D7,0x00B7,0x0097,0x0077,0x0057,
+			0x0037,0x00FB,0x00DB,0x00BB,0x00FF,0x00E3,0x00C3,0x00A3,
+			0x0083,0x0063,0x0043,0x0023,0x0003,0x01E3,0x01C3,0x01A3,
+			0x0183,0x0163,0x0143,0x0123,0x0103
+	};
+
+static u8 OFDM_CONFIG[]={
+			// OFDM reg0x06[7:0]=0xFF: Enable power saving mode in RX
+			// OFDM reg0x3C[4]=1'b1: Enable RX power saving mode
+			// ofdm 0x3a = 0x7b ,(original : 0xfb) For ECS shielding room TP test
+
+			// 0x00
+			0x10, 0x0F, 0x0A, 0x0C, 0x14, 0xFA, 0xFF, 0x50,
+			0x00, 0x50, 0x00, 0x00, 0x00, 0x5C, 0x00, 0x00,
+			// 0x10
+			0x40, 0x00, 0x40, 0x00, 0x00, 0x00, 0xA8, 0x26,
+			0x32, 0x33, 0x06, 0xA5, 0x6F, 0x55, 0xC8, 0xBB,
+			// 0x20
+			0x0A, 0xE1, 0x2C, 0x4A, 0x86, 0x83, 0x34, 0x00,
+			0x4F, 0x24, 0x6F, 0xC2, 0x03, 0x40, 0x80, 0x00,
+			// 0x30
+			0xC0, 0xC1, 0x58, 0xF1, 0x00, 0xC4, 0x90, 0x3e,
+			0xD8, 0x3C, 0x7B, 0x10, 0x10
+		};
+#else
+ static u8 MAC_REG_TABLE[][2]={
+			//PAGA 0:
+			{0xf0, 0x32}, {0xf1, 0x32}, {0xf2, 0x00}, {0xf3, 0x00}, {0xf4, 0x32},
+			{0xf5, 0x43}, {0xf6, 0x00}, {0xf7, 0x00}, {0xf8, 0x46}, {0xf9, 0xa4},
+			{0xfa, 0x00}, {0xfb, 0x00}, {0xfc, 0x96}, {0xfd, 0xa4}, {0xfe, 0x00},
+			{0xff, 0x00},
+
+			//PAGE 1:
+			{0x5e, 0x01},
+			{0x58, 0x4b}, {0x59, 0x00}, {0x5a, 0x4b}, {0x5b, 0x00}, {0x60, 0x4b},
+			{0x61, 0x09}, {0x62, 0x4b}, {0x63, 0x09}, {0xce, 0x0f}, {0xcf, 0x00},
+			{0xe0, 0xff}, {0xe1, 0x0f}, {0xe2, 0x00}, {0xf0, 0x4e}, {0xf1, 0x01},
+			{0xf2, 0x02}, {0xf3, 0x03}, {0xf4, 0x04}, {0xf5, 0x05}, {0xf6, 0x06},
+			{0xf7, 0x07}, {0xf8, 0x08},
+
+
+			//PAGE 2:
+			{0x5e, 0x02},
+			{0x0c, 0x04}, {0x21, 0x61}, {0x22, 0x68}, {0x23, 0x6f}, {0x24, 0x76},
+			{0x25, 0x7d}, {0x26, 0x84}, {0x27, 0x8d}, {0x4d, 0x08}, {0x4e, 0x00},
+			{0x50, 0x05}, {0x51, 0xf5}, {0x52, 0x04}, {0x53, 0xa0}, {0x54, 0x1f},
+			{0x55, 0x23}, {0x56, 0x45}, {0x57, 0x67}, {0x58, 0x08}, {0x59, 0x08},
+			{0x5a, 0x08}, {0x5b, 0x08}, {0x60, 0x08}, {0x61, 0x08}, {0x62, 0x08},
+			{0x63, 0x08}, {0x64, 0xcf}, {0x72, 0x56}, {0x73, 0x9a},
+
+			//PAGA 0:
+			{0x5e, 0x00},
+			{0x34, 0xff}, {0x35, 0x0f}, {0x5b, 0x40}, {0x84, 0x88}, {0x85, 0x24},
+			{0x88, 0x54}, {0x8b, 0xb8}, {0x8c, 0x07}, {0x8d, 0x00}, {0x94, 0x1b},
+			{0x95, 0x12}, {0x96, 0x00}, {0x97, 0x06}, {0x9d, 0x1a}, {0x9f, 0x10},
+			{0xb4, 0x22}, {0xbe, 0x80}, {0xdb, 0x00}, {0xee, 0x00}, {0x5b, 0x42},
+			{0x91, 0x03},
+
+			//PAGE 2:
+			{0x5e, 0x02},
+			{0x4c, 0x03},
+
+			//PAGE 0:
+			{0x5e, 0x00},
+
+			//PAGE 3:
+			{0x5e, 0x03},
+			{0x9f, 0x00},
+
+			//PAGE 0:
+			{0x5e, 0x00},
+			{0x8c, 0x01}, {0x8d, 0x10},{0x8e, 0x08}, {0x8f, 0x00}
+		};
+
+
+static u8  ZEBRA_AGC[]={
+	0,
+	0x5e,0x5e,0x5e,0x5e,0x5d,0x5b,0x59,0x57,0x55,0x53,0x51,0x4f,0x4d,0x4b,0x49,0x47,
+	0x45,0x43,0x41,0x3f,0x3d,0x3b,0x39,0x37,0x35,0x33,0x31,0x2f,0x2d,0x2b,0x29,0x27,
+	0x25,0x23,0x21,0x1f,0x1d,0x1b,0x19,0x17,0x15,0x13,0x11,0x0f,0x0d,0x0b,0x09,0x07,
+	0x05,0x03,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,
+	0x19,0x19,0x19,0x019,0x19,0x19,0x19,0x19,0x19,0x19,0x1e,0x1f,0x20,0x21,0x21,0x22,
+	0x23,0x24,0x24,0x25,0x25,0x26,0x26,0x27,0x27,0x28,0x28,0x28,0x29,0x2a,0x2a,0x2b,
+	0x2b,0x2b,0x2c,0x2c,0x2c,0x2d,0x2d,0x2d,0x2e,0x2e,0x2f,0x30,0x31,0x31,0x31,0x31,
+	0x31,0x31,0x31,0x31,0x31,0x31,0x31,0x31,0x31,0x31,0x31,0x31,0x31,0x31,0x31,0x31
+	};
+
+static u32 ZEBRA_RF_RX_GAIN_TABLE[]={
+	0,
+	0x0400,0x0401,0x0402,0x0403,0x0404,0x0405,0x0408,0x0409,
+	0x040a,0x040b,0x0502,0x0503,0x0504,0x0505,0x0540,0x0541,
+	0x0542,0x0543,0x0544,0x0545,0x0580,0x0581,0x0582,0x0583,
+	0x0584,0x0585,0x0588,0x0589,0x058a,0x058b,0x0643,0x0644,
+	0x0645,0x0680,0x0681,0x0682,0x0683,0x0684,0x0685,0x0688,
+	0x0689,0x068a,0x068b,0x068c,0x0742,0x0743,0x0744,0x0745,
+	0x0780,0x0781,0x0782,0x0783,0x0784,0x0785,0x0788,0x0789,
+	0x078a,0x078b,0x078c,0x078d,0x0790,0x0791,0x0792,0x0793,
+	0x0794,0x0795,0x0798,0x0799,0x079a,0x079b,0x079c,0x079d,
+	0x07a0,0x07a1,0x07a2,0x07a3,0x07a4,0x07a5,0x07a8,0x07a9,
+	0x03aa,0x03ab,0x03ac,0x03ad,0x03b0,0x03b1,0x03b2,0x03b3,
+	0x03b4,0x03b5,0x03b8,0x03b9,0x03ba,0x03bb,0x03bb
+};
+
+// 2006.07.13, SD3 szuyitasi:
+//	OFDM.0x03=0x0C (original is 0x0F)
+// Use the new SD3 given param, by shien chang, 2006.07.14
+static u8 OFDM_CONFIG[]={
+	0x10, 0x0d, 0x01, 0x0C, 0x14, 0xfb, 0x0f, 0x60, 0x00, 0x60,
+	0x00, 0x00, 0x00, 0x5c, 0x00, 0x00, 0x40, 0x00, 0x40, 0x00,
+	0x00, 0x00, 0xa8, 0x46, 0xb2, 0x33, 0x07, 0xa5, 0x6f, 0x55,
+	0xc8, 0xb3, 0x0a, 0xe1, 0x1c, 0x8a, 0xb6, 0x83, 0x34, 0x0f,
+	0x4f, 0x23, 0x6f, 0xc2, 0x6b, 0x40, 0x80, 0x00, 0xc0, 0xc1,
+	0x58, 0xf1, 0x00, 0xe4, 0x90, 0x3e, 0x6d, 0x3c, 0xff, 0x07
+};
+#endif
+
+/*---------------------------------------------------------------
+  * Hardware IO
+  * the code is ported from Windows source code
+  ----------------------------------------------------------------*/
+
+void
+PlatformIOWrite1Byte(
+	struct net_device *dev,
+	u32		offset,
+	u8		data
+	)
+{
+#ifndef CONFIG_RTL8180_IO_MAP
+	write_nic_byte(dev, offset, data);
+	read_nic_byte(dev, offset); // To make sure write operation is completed, 2005.11.09, by rcnjko.
+
+#else // Port IO
+	u32 Page = (offset >> 8);
+
+	switch(Page)
+	{
+	case 0: // Page 0
+		write_nic_byte(dev, offset, data);
+		break;
+
+	case 1: // Page 1
+	case 2: // Page 2
+	case 3: // Page 3
+		{
+			u8 psr = read_nic_byte(dev, PSR);
+
+			write_nic_byte(dev, PSR, ((psr & 0xfc) | (u8)Page)); // Switch to page N.
+			write_nic_byte(dev, (offset & 0xff), data);
+			write_nic_byte(dev, PSR, (psr & 0xfc)); // Switch to page 0.
+		}
+		break;
+
+	default:
+		// Illegal page number.
+		DMESGE("PlatformIOWrite1Byte(): illegal page number: %d, offset: %#X", Page, offset);
+		break;
+	}
+#endif
+}
+
+void
+PlatformIOWrite2Byte(
+	struct net_device *dev,
+	u32		offset,
+	u16		data
+	)
+{
+#ifndef CONFIG_RTL8180_IO_MAP
+	write_nic_word(dev, offset, data);
+	read_nic_word(dev, offset); // To make sure write operation is completed, 2005.11.09, by rcnjko.
+
+
+#else // Port IO
+	u32 Page = (offset >> 8);
+
+	switch(Page)
+	{
+	case 0: // Page 0
+		write_nic_word(dev, offset, data);
+		break;
+
+	case 1: // Page 1
+	case 2: // Page 2
+	case 3: // Page 3
+		{
+			u8 psr = read_nic_byte(dev, PSR);
+
+			write_nic_byte(dev, PSR, ((psr & 0xfc) | (u8)Page)); // Switch to page N.
+			write_nic_word(dev, (offset & 0xff), data);
+			write_nic_byte(dev, PSR, (psr & 0xfc)); // Switch to page 0.
+		}
+		break;
+
+	default:
+		// Illegal page number.
+		DMESGE("PlatformIOWrite2Byte(): illegal page number: %d, offset: %#X", Page, offset);
+		break;
+	}
+#endif
+}
+u8 PlatformIORead1Byte(struct net_device *dev, u32 offset);
+
+void
+PlatformIOWrite4Byte(
+	struct net_device *dev,
+	u32		offset,
+	u32		data
+	)
+{
+#ifndef CONFIG_RTL8180_IO_MAP
+//{by amy 080312
+if (offset == PhyAddr)
+	{//For Base Band configuration.
+		unsigned char	cmdByte;
+		unsigned long	dataBytes;
+		unsigned char	idx;
+		u8	u1bTmp;
+
+		cmdByte = (u8)(data & 0x000000ff);
+		dataBytes = data>>8;
+
+		//
+		// 071010, rcnjko:
+		// The critical section is only BB read/write race condition.
+		// Assumption:
+		// 1. We assume NO one will access BB at DIRQL, otherwise, system will crash for
+		// acquiring the spinlock in such context.
+		// 2. PlatformIOWrite4Byte() MUST NOT be recursive.
+		//
+//		NdisAcquireSpinLock( &(pDevice->IoSpinLock) );
+
+		for(idx = 0; idx < 30; idx++)
+		{ // Make sure command bit is clear before access it.
+			u1bTmp = PlatformIORead1Byte(dev, PhyAddr);
+			if((u1bTmp & BIT7) == 0)
+				break;
+			else
+				mdelay(10);
+		}
+
+		for(idx=0; idx < 3; idx++)
+		{
+			PlatformIOWrite1Byte(dev,offset+1+idx,((u8*)&dataBytes)[idx] );
+		}
+		write_nic_byte(dev, offset, cmdByte);
+
+//		NdisReleaseSpinLock( &(pDevice->IoSpinLock) );
+	}
+//by amy 080312}
+	else{
+		write_nic_dword(dev, offset, data);
+		read_nic_dword(dev, offset); // To make sure write operation is completed, 2005.11.09, by rcnjko.
+	}
+#else // Port IO
+	u32 Page = (offset >> 8);
+
+	switch(Page)
+	{
+	case 0: // Page 0
+		write_nic_word(dev, offset, data);
+		break;
+
+	case 1: // Page 1
+	case 2: // Page 2
+	case 3: // Page 3
+		{
+			u8 psr = read_nic_byte(dev, PSR);
+
+			write_nic_byte(dev, PSR, ((psr & 0xfc) | (u8)Page)); // Switch to page N.
+			write_nic_dword(dev, (offset & 0xff), data);
+			write_nic_byte(dev, PSR, (psr & 0xfc)); // Switch to page 0.
+		}
+		break;
+
+	default:
+		// Illegal page number.
+		DMESGE("PlatformIOWrite4Byte(): illegal page number: %d, offset: %#X", Page, offset);
+		break;
+	}
+#endif
+}
+
+u8
+PlatformIORead1Byte(
+	struct net_device *dev,
+	u32		offset
+	)
+{
+	u8	data = 0;
+
+#ifndef CONFIG_RTL8180_IO_MAP
+	data = read_nic_byte(dev, offset);
+
+#else // Port IO
+	u32 Page = (offset >> 8);
+
+	switch(Page)
+	{
+	case 0: // Page 0
+		data = read_nic_byte(dev, offset);
+		break;
+
+	case 1: // Page 1
+	case 2: // Page 2
+	case 3: // Page 3
+		{
+			u8 psr = read_nic_byte(dev, PSR);
+
+			write_nic_byte(dev, PSR, ((psr & 0xfc) | (u8)Page)); // Switch to page N.
+			data = read_nic_byte(dev, (offset & 0xff));
+			write_nic_byte(dev, PSR, (psr & 0xfc)); // Switch to page 0.
+		}
+		break;
+
+	default:
+		// Illegal page number.
+		DMESGE("PlatformIORead1Byte(): illegal page number: %d, offset: %#X", Page, offset);
+		break;
+	}
+#endif
+
+	return data;
+}
+
+u16
+PlatformIORead2Byte(
+	struct net_device *dev,
+	u32		offset
+	)
+{
+	u16	data = 0;
+
+#ifndef CONFIG_RTL8180_IO_MAP
+	data = read_nic_word(dev, offset);
+
+#else // Port IO
+	u32 Page = (offset >> 8);
+
+	switch(Page)
+	{
+	case 0: // Page 0
+		data = read_nic_word(dev, offset);
+		break;
+
+	case 1: // Page 1
+	case 2: // Page 2
+	case 3: // Page 3
+		{
+			u8 psr = read_nic_byte(dev, PSR);
+
+			write_nic_byte(dev, PSR, ((psr & 0xfc) | (u8)Page)); // Switch to page N.
+			data = read_nic_word(dev, (offset & 0xff));
+			write_nic_byte(dev, PSR, (psr & 0xfc)); // Switch to page 0.
+		}
+		break;
+
+	default:
+		// Illegal page number.
+		DMESGE("PlatformIORead2Byte(): illegal page number: %d, offset: %#X", Page, offset);
+		break;
+	}
+#endif
+
+	return data;
+}
+
+u32
+PlatformIORead4Byte(
+	struct net_device *dev,
+	u32		offset
+	)
+{
+	u32	data = 0;
+
+#ifndef CONFIG_RTL8180_IO_MAP
+	data = read_nic_dword(dev, offset);
+
+#else // Port IO
+	u32 Page = (offset >> 8);
+
+	switch(Page)
+	{
+	case 0: // Page 0
+		data = read_nic_dword(dev, offset);
+		break;
+
+	case 1: // Page 1
+	case 2: // Page 2
+	case 3: // Page 3
+		{
+			u8 psr = read_nic_byte(dev, PSR);
+
+			write_nic_byte(dev, PSR, ((psr & 0xfc) | (u8)Page)); // Switch to page N.
+			data = read_nic_dword(dev, (offset & 0xff));
+			write_nic_byte(dev, PSR, (psr & 0xfc)); // Switch to page 0.
+		}
+		break;
+
+	default:
+		// Illegal page number.
+		DMESGE("PlatformIORead4Byte(): illegal page number: %d, offset: %#X\n", Page, offset);
+		break;
+	}
+#endif
+
+	return data;
+}
+
+void
+SetOutputEnableOfRfPins(
+	struct net_device *dev
+	)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+	switch(priv->rf_chip)
+	{
+	case RFCHIPID_RTL8225:
+	case RF_ZEBRA2:
+	case RF_ZEBRA4:
+		write_nic_word(dev, RFPinsEnable, 0x1bff);
+		//write_nic_word(dev, RFPinsEnable, 0x1fff);
+		break;
+	}
+}
+
+void
+ZEBRA_RFSerialWrite(
+	struct net_device *dev,
+	u32			data2Write,
+	u8			totalLength,
+	u8			low2high
+	)
+{
+	ThreeWireReg		twreg;
+	int 				i;
+	u16				oval,oval2,oval3;
+	u32				mask;
+	u16				UshortBuffer;
+
+	u8			u1bTmp;
+#ifdef CONFIG_RTL818X_S
+	// RTL8187S HSSI Read/Write Function
+	u1bTmp = read_nic_byte(dev, RF_SW_CONFIG);
+	u1bTmp |=   RF_SW_CFG_SI;   //reg08[1]=1 Serial Interface(SI)
+	write_nic_byte(dev, RF_SW_CONFIG, u1bTmp);
+#endif
+	UshortBuffer = read_nic_word(dev, RFPinsOutput);
+	oval = UshortBuffer & 0xfff8; // We shall clear bit0, 1, 2 first, 2005.10.28, by rcnjko.
+
+	oval2 = read_nic_word(dev, RFPinsEnable);
+	oval3 = read_nic_word(dev, RFPinsSelect);
+
+	// <RJ_NOTE> 3-wire should be controled by HW when we finish SW 3-wire programming. 2005.08.10, by rcnjko.
+	oval3 &= 0xfff8;
+
+	write_nic_word(dev, RFPinsEnable, (oval2|0x0007)); // Set To Output Enable
+	write_nic_word(dev, RFPinsSelect, (oval3|0x0007)); // Set To SW Switch
+	udelay(10);
+
+	// Add this to avoid hardware and software 3-wire conflict.
+	// 2005.03.01, by rcnjko.
+	twreg.longData = 0;
+	twreg.struc.enableB = 1;
+	write_nic_word(dev, RFPinsOutput, (twreg.longData|oval)); // Set SI_EN (RFLE)
+	udelay(2);
+	twreg.struc.enableB = 0;
+	write_nic_word(dev, RFPinsOutput, (twreg.longData|oval)); // Clear SI_EN (RFLE)
+	udelay(10);
+
+	mask = (low2high)?0x01:((u32)0x01<<(totalLength-1));
+
+	for(i=0; i<totalLength/2; i++)
+	{
+		twreg.struc.data = ((data2Write&mask)!=0) ? 1 : 0;
+		write_nic_word(dev, RFPinsOutput, (twreg.longData|oval));
+		twreg.struc.clk = 1;
+		write_nic_word(dev, RFPinsOutput, (twreg.longData|oval));
+		write_nic_word(dev, RFPinsOutput, (twreg.longData|oval));
+
+		mask = (low2high)?(mask<<1):(mask>>1);
+		twreg.struc.data = ((data2Write&mask)!=0) ? 1 : 0;
+		write_nic_word(dev, RFPinsOutput, (twreg.longData|oval));
+		write_nic_word(dev, RFPinsOutput, (twreg.longData|oval));
+		twreg.struc.clk = 0;
+		write_nic_word(dev, RFPinsOutput, (twreg.longData|oval));
+		mask = (low2high)?(mask<<1):(mask>>1);
+	}
+
+	twreg.struc.enableB = 1;
+	twreg.struc.clk = 0;
+	twreg.struc.data = 0;
+	write_nic_word(dev, RFPinsOutput, twreg.longData|oval);
+	udelay(10);
+
+	write_nic_word(dev, RFPinsOutput, oval|0x0004);
+	write_nic_word(dev, RFPinsSelect, oval3|0x0000);
+
+	SetOutputEnableOfRfPins(dev);
+}
+//by amy
+
+
+int
+HwHSSIThreeWire(
+	struct net_device *dev,
+	u8			*pDataBuf,
+	u8			nDataBufBitCnt,
+	int			bSI,
+	int			bWrite
+	)
+{
+	int	bResult = 1;
+	u8	TryCnt;
+	u8	u1bTmp;
+
+	do
+	{
+		// Check if WE and RE are cleared.
+		for(TryCnt = 0; TryCnt < TC_3W_POLL_MAX_TRY_CNT; TryCnt++)
+		{
+			u1bTmp = read_nic_byte(dev, SW_3W_CMD1);
+			if( (u1bTmp & (SW_3W_CMD1_RE|SW_3W_CMD1_WE)) == 0 )
+			{
+				break;
+			}
+			udelay(10);
+		}
+		if (TryCnt == TC_3W_POLL_MAX_TRY_CNT)
+			panic("HwThreeWire(): CmdReg: %#X RE|WE bits are not clear!!\n", u1bTmp);
+
+		// RTL8187S HSSI Read/Write Function
+		u1bTmp = read_nic_byte(dev, RF_SW_CONFIG);
+
+		if(bSI)
+		{
+			u1bTmp |=   RF_SW_CFG_SI;   //reg08[1]=1 Serial Interface(SI)
+		}else
+		{
+			u1bTmp &= ~RF_SW_CFG_SI;  //reg08[1]=0 Parallel Interface(PI)
+		}
+
+		write_nic_byte(dev, RF_SW_CONFIG, u1bTmp);
+
+		if(bSI)
+		{
+			// jong: HW SI read must set reg84[3]=0.
+			u1bTmp = read_nic_byte(dev, RFPinsSelect);
+			u1bTmp &= ~BIT3;
+			write_nic_byte(dev, RFPinsSelect, u1bTmp );
+		}
+	 	// Fill up data buffer for write operation.
+
+		if(bWrite)
+		{
+			if(nDataBufBitCnt == 16)
+			{
+				write_nic_word(dev, SW_3W_DB0, *((u16*)pDataBuf));
+			}
+			else if(nDataBufBitCnt == 64)  // RTL8187S shouldn't enter this case
+			{
+				write_nic_dword(dev, SW_3W_DB0, *((u32*)pDataBuf));
+				write_nic_dword(dev, SW_3W_DB1, *((u32*)(pDataBuf + 4)));
+			}
+			else
+			{
+				int idx;
+				int ByteCnt = nDataBufBitCnt / 8;
+                                //printk("%d\n",nDataBufBitCnt);
+				if ((nDataBufBitCnt % 8) != 0)
+				panic("HwThreeWire(): nDataBufBitCnt(%d) should be multiple of 8!!!\n",
+				nDataBufBitCnt);
+
+			       if (nDataBufBitCnt > 64)
+				panic("HwThreeWire(): nDataBufBitCnt(%d) should <= 64!!!\n",
+				nDataBufBitCnt);
+
+				for(idx = 0; idx < ByteCnt; idx++)
+				{
+					write_nic_byte(dev, (SW_3W_DB0+idx), *(pDataBuf+idx));
+				}
+			}
+		}
+		else		//read
+		{
+			if(bSI)
+			{
+				// SI - reg274[3:0] : RF register's Address
+				write_nic_word(dev, SW_3W_DB0, *((u16*)pDataBuf) );
+			}
+			else
+			{
+				// PI - reg274[15:12] : RF register's Address
+				write_nic_word(dev, SW_3W_DB0, (*((u16*)pDataBuf)) << 12);
+			}
+		}
+
+		// Set up command: WE or RE.
+		if(bWrite)
+		{
+			write_nic_byte(dev, SW_3W_CMD1, SW_3W_CMD1_WE);
+		}
+		else
+		{
+			write_nic_byte(dev, SW_3W_CMD1, SW_3W_CMD1_RE);
+		}
+
+		// Check if DONE is set.
+		for(TryCnt = 0; TryCnt < TC_3W_POLL_MAX_TRY_CNT; TryCnt++)
+		{
+			u1bTmp = read_nic_byte(dev, SW_3W_CMD1);
+			if(  (u1bTmp & SW_3W_CMD1_DONE) != 0 )
+			{
+				break;
+			}
+			udelay(10);
+		}
+
+		write_nic_byte(dev, SW_3W_CMD1, 0);
+
+		// Read back data for read operation.
+		if(bWrite == 0)
+		{
+			if(bSI)
+			{
+				//Serial Interface : reg363_362[11:0]
+				*((u16*)pDataBuf) = read_nic_word(dev, SI_DATA_READ) ;
+			}
+			else
+			{
+				//Parallel Interface : reg361_360[11:0]
+				*((u16*)pDataBuf) = read_nic_word(dev, PI_DATA_READ);
+			}
+
+			*((u16*)pDataBuf) &= 0x0FFF;
+		}
+
+	}while(0);
+
+	return bResult;
+}
+//by amy
+
+int
+HwThreeWire(
+	struct net_device *dev,
+	u8			*pDataBuf,
+	u8			nDataBufBitCnt,
+	int			bHold,
+	int			bWrite
+	)
+{
+	int	bResult = 1;
+	u8	TryCnt;
+	u8	u1bTmp;
+
+	do
+	{
+		// Check if WE and RE are cleared.
+		for(TryCnt = 0; TryCnt < TC_3W_POLL_MAX_TRY_CNT; TryCnt++)
+		{
+			u1bTmp = read_nic_byte(dev, SW_3W_CMD1);
+			if( (u1bTmp & (SW_3W_CMD1_RE|SW_3W_CMD1_WE)) == 0 )
+			{
+				break;
+			}
+			udelay(10);
+		}
+		if (TryCnt == TC_3W_POLL_MAX_TRY_CNT)
+			panic("HwThreeWire(): CmdReg: %#X RE|WE bits are not clear!!\n", u1bTmp);
+
+		// Fill up data buffer for write operation.
+		if(nDataBufBitCnt == 16)
+		{
+			write_nic_word(dev, SW_3W_DB0, *((u16 *)pDataBuf));
+		}
+		else if(nDataBufBitCnt == 64)
+		{
+			write_nic_dword(dev, SW_3W_DB0, *((u32 *)pDataBuf));
+			write_nic_dword(dev, SW_3W_DB1, *((u32 *)(pDataBuf + 4)));
+		}
+		else
+		{
+			int idx;
+			int ByteCnt = nDataBufBitCnt / 8;
+
+			if ((nDataBufBitCnt % 8) != 0)
+				panic("HwThreeWire(): nDataBufBitCnt(%d) should be multiple of 8!!!\n",
+				nDataBufBitCnt);
+
+			if (nDataBufBitCnt > 64)
+				panic("HwThreeWire(): nDataBufBitCnt(%d) should <= 64!!!\n",
+				nDataBufBitCnt);
+
+			for(idx = 0; idx < ByteCnt; idx++)
+			{
+				write_nic_byte(dev, (SW_3W_DB0+idx), *(pDataBuf+idx));
+			}
+		}
+
+		// Fill up length field.
+		u1bTmp = (u8)(nDataBufBitCnt - 1); // Number of bits - 1.
+		if(bHold)
+			u1bTmp |= SW_3W_CMD0_HOLD;
+		write_nic_byte(dev, SW_3W_CMD0, u1bTmp);
+
+		// Set up command: WE or RE.
+		if(bWrite)
+		{
+			write_nic_byte(dev, SW_3W_CMD1, SW_3W_CMD1_WE);
+		}
+		else
+		{
+			write_nic_byte(dev, SW_3W_CMD1, SW_3W_CMD1_RE);
+		}
+
+		// Check if WE and RE are cleared and DONE is set.
+		for(TryCnt = 0; TryCnt < TC_3W_POLL_MAX_TRY_CNT; TryCnt++)
+		{
+			u1bTmp = read_nic_byte(dev, SW_3W_CMD1);
+			if( (u1bTmp & (SW_3W_CMD1_RE|SW_3W_CMD1_WE)) == 0 &&
+				(u1bTmp & SW_3W_CMD1_DONE) != 0 )
+			{
+				break;
+			}
+			udelay(10);
+		}
+		if(TryCnt == TC_3W_POLL_MAX_TRY_CNT)
+		{
+			//RT_ASSERT(TryCnt != TC_3W_POLL_MAX_TRY_CNT,
+			//	("HwThreeWire(): CmdReg: %#X RE|WE bits are not clear or DONE is not set!!\n", u1bTmp));
+			// Workaround suggested by wcchu: clear WE here. 2006.07.07, by rcnjko.
+			write_nic_byte(dev, SW_3W_CMD1, 0);
+		}
+
+		// Read back data for read operation.
+		// <RJ_TODO> I am not sure if this is correct output format of a read operation.
+		if(bWrite == 0)
+		{
+			if(nDataBufBitCnt == 16)
+			{
+				*((u16 *)pDataBuf) = read_nic_word(dev, SW_3W_DB0);
+			}
+			else if(nDataBufBitCnt == 64)
+			{
+				*((u32 *)pDataBuf) = read_nic_dword(dev, SW_3W_DB0);
+				*((u32 *)(pDataBuf + 4)) = read_nic_dword(dev, SW_3W_DB1);
+			}
+			else
+			{
+				int idx;
+				int ByteCnt = nDataBufBitCnt / 8;
+
+				if ((nDataBufBitCnt % 8) != 0)
+					panic("HwThreeWire(): nDataBufBitCnt(%d) should be multiple of 8!!!\n",
+					nDataBufBitCnt);
+
+				if (nDataBufBitCnt > 64)
+					panic("HwThreeWire(): nDataBufBitCnt(%d) should <= 64!!!\n",
+					nDataBufBitCnt);
+
+				for(idx = 0; idx < ByteCnt; idx++)
+				{
+					*(pDataBuf+idx) = read_nic_byte(dev, (SW_3W_DB0+idx));
+				}
+			}
+		}
+
+	}while(0);
+
+	return bResult;
+}
+
+
+void
+RF_WriteReg(
+	struct net_device *dev,
+	u8		offset,
+	u32		data
+	)
+{
+	//RFReg			reg;
+	u32			data2Write;
+	u8			len;
+	u8			low2high;
+	//u32			RF_Read = 0;
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+
+	switch(priv->rf_chip)
+	{
+	case RFCHIPID_RTL8225:
+	case RF_ZEBRA2:		// Annie 2006-05-12.
+	case RF_ZEBRA4:        //by amy
+		switch(priv->RegThreeWireMode)
+		{
+		case SW_THREE_WIRE:
+			{ // Perform SW 3-wire programming by driver.
+				data2Write = (data << 4) | (u32)(offset & 0x0f);
+				len = 16;
+				low2high = 0;
+				ZEBRA_RFSerialWrite(dev, data2Write, len, low2high);
+       			}
+			break;
+
+ 		case HW_THREE_WIRE:
+			{ // Pure HW 3-wire.
+				data2Write = (data << 4) | (u32)(offset & 0x0f);
+				len = 16;
+				HwThreeWire(
+					dev,
+					(u8 *)(&data2Write),	// pDataBuf,
+					len,				// nDataBufBitCnt,
+					0,					// bHold,
+					1);					// bWrite
+         		}
+			break;
+  #ifdef CONFIG_RTL818X_S
+			case HW_THREE_WIRE_PI: //Parallel Interface
+			{ // Pure HW 3-wire.
+				data2Write = (data << 4) | (u32)(offset & 0x0f);
+				len = 16;
+					HwHSSIThreeWire(
+						dev,
+						(u8*)(&data2Write),	// pDataBuf,
+						len,						// nDataBufBitCnt,
+						0, 					// bSI
+						1); 					// bWrite
+
+                                //printk("33333\n");
+			}
+			break;
+
+			case HW_THREE_WIRE_SI: //Serial Interface
+			{ // Pure HW 3-wire.
+				data2Write = (data << 4) | (u32)(offset & 0x0f);
+				len = 16;
+//                                printk(" enter  ZEBRA_RFSerialWrite\n ");
+//                                low2high = 0;
+//                                ZEBRA_RFSerialWrite(dev, data2Write, len, low2high);
+
+				HwHSSIThreeWire(
+					dev,
+					(u8*)(&data2Write),	// pDataBuf,
+					len,						// nDataBufBitCnt,
+					1, 					// bSI
+					1); 					// bWrite
+
+//                                 printk(" exit ZEBRA_RFSerialWrite\n ");
+			}
+			break;
+  #endif
+
+
+		default:
+			DMESGE("RF_WriteReg(): invalid RegThreeWireMode(%d) !!!", priv->RegThreeWireMode);
+			break;
+		}
+		break;
+
+	default:
+		DMESGE("RF_WriteReg(): unknown RFChipID: %#X", priv->rf_chip);
+		break;
+	}
+}
+
+
+void
+ZEBRA_RFSerialRead(
+	struct net_device *dev,
+	u32		data2Write,
+	u8		wLength,
+	u32		*data2Read,
+	u8		rLength,
+	u8		low2high
+	)
+{
+	ThreeWireReg	twreg;
+	int				i;
+	u16			oval,oval2,oval3,tmp, wReg80;
+	u32			mask;
+	u8			u1bTmp;
+	ThreeWireReg	tdata;
+	//PHAL_DATA_8187	pHalData = GetHalData8187(pAdapter);
+#ifdef CONFIG_RTL818X_S
+	{ // RTL8187S HSSI Read/Write Function
+		u1bTmp = read_nic_byte(dev, RF_SW_CONFIG);
+		u1bTmp |=   RF_SW_CFG_SI;   //reg08[1]=1 Serial Interface(SI)
+		write_nic_byte(dev, RF_SW_CONFIG, u1bTmp);
+	}
+#endif
+
+	wReg80 = oval = read_nic_word(dev, RFPinsOutput);
+	oval2 = read_nic_word(dev, RFPinsEnable);
+	oval3 = read_nic_word(dev, RFPinsSelect);
+
+	write_nic_word(dev, RFPinsEnable, oval2|0xf);
+	write_nic_word(dev, RFPinsSelect, oval3|0xf);
+
+	*data2Read = 0;
+
+	// We must clear BIT0-3 here, otherwise,
+	// SW_Enalbe will be true when we first call ZEBRA_RFSerialRead() after 8187MPVC open,
+	// which will cause the value read become 0. 2005.04.11, by rcnjko.
+	oval &= ~0xf;
+
+	// Avoid collision with hardware three-wire.
+	twreg.longData = 0;
+	twreg.struc.enableB = 1;
+	write_nic_word(dev, RFPinsOutput, twreg.longData|oval); udelay(4);
+
+	twreg.longData = 0;
+	twreg.struc.enableB = 0;
+	twreg.struc.clk = 0;
+	twreg.struc.read_write = 0;
+	write_nic_word(dev, RFPinsOutput, twreg.longData|oval); udelay(5);
+
+	mask = (low2high) ? 0x01 : ((u32)0x01<<(32-1));
+	for(i = 0; i < wLength/2; i++)
+	{
+		twreg.struc.data = ((data2Write&mask) != 0) ? 1 : 0;
+		write_nic_word(dev, RFPinsOutput, twreg.longData|oval); udelay(1);
+		twreg.struc.clk = 1;
+		write_nic_word(dev, RFPinsOutput, twreg.longData|oval); udelay(2);
+		write_nic_word(dev, RFPinsOutput, twreg.longData|oval); udelay(2);
+
+		mask = (low2high) ? (mask<<1): (mask>>1);
+
+		if(i == 2)
+		{
+			// Commented out by Jackie, 2004.08.26. <RJ_NOTE> We must comment out the following two lines for we cannot pull down VCOPDN during RF Serail Read.
+			//PlatformEFIOWrite2Byte(pAdapter, RFPinsEnable, 0xe);     // turn off data enable
+			//PlatformEFIOWrite2Byte(pAdapter, RFPinsSelect, 0xe);
+
+			twreg.struc.read_write=1;
+			write_nic_word(dev, RFPinsOutput, twreg.longData|oval); udelay(2);
+			twreg.struc.clk = 0;
+			write_nic_word(dev, RFPinsOutput, twreg.longData|oval); udelay(2);
+			break;
+		}
+		twreg.struc.data = ((data2Write&mask) != 0) ? 1: 0;
+		write_nic_word(dev, RFPinsOutput, twreg.longData|oval); udelay(2);
+		write_nic_word(dev, RFPinsOutput, twreg.longData|oval); udelay(2);
+
+		twreg.struc.clk = 0;
+		write_nic_word(dev, RFPinsOutput, twreg.longData|oval); udelay(1);
+
+		mask = (low2high) ? (mask<<1) : (mask>>1);
+	}
+
+	twreg.struc.clk = 0;
+	twreg.struc.data = 0;
+	write_nic_word(dev, RFPinsOutput, twreg.longData|oval); udelay(2);
+	mask = (low2high) ? 0x01 : ((u32)0x01 << (12-1));
+
+	//
+	// 061016, by rcnjko:
+	// We must set data pin to HW controled, otherwise RF can't driver it and
+	// value RF register won't be able to read back properly.
+	//
+	write_nic_word(dev, RFPinsEnable, ( ((oval2|0x0E) & (~0x01))) );
+
+	for(i = 0; i < rLength; i++)
+	{
+		write_nic_word(dev, RFPinsOutput, twreg.longData|oval); udelay(1);
+		twreg.struc.clk = 1;
+		write_nic_word(dev, RFPinsOutput, twreg.longData|oval); udelay(2);
+		write_nic_word(dev, RFPinsOutput, twreg.longData|oval); udelay(2);
+		write_nic_word(dev, RFPinsOutput, twreg.longData|oval); udelay(2);
+		tmp = read_nic_word(dev, RFPinsInput);
+		tdata.longData = tmp;
+		*data2Read |= tdata.struc.clk ? mask : 0;
+
+		twreg.struc.clk = 0;
+		write_nic_word(dev, RFPinsOutput, twreg.longData|oval); udelay(2);
+
+		mask = (low2high) ? (mask<<1) : (mask>>1);
+	}
+	twreg.struc.enableB = 1;
+	twreg.struc.clk = 0;
+	twreg.struc.data = 0;
+	twreg.struc.read_write = 1;
+	write_nic_word(dev, RFPinsOutput, twreg.longData|oval); udelay(2);
+
+	//PlatformEFIOWrite2Byte(pAdapter, RFPinsEnable, oval2|0x8);   // Set To Output Enable
+	write_nic_word(dev, RFPinsEnable, oval2);   // Set To Output Enable, <RJ_NOTE> We cannot enable BIT3 here, otherwise, we will failed to switch channel. 2005.04.12.
+	//PlatformEFIOWrite2Byte(pAdapter, RFPinsEnable, 0x1bff);
+	write_nic_word(dev, RFPinsSelect, oval3);   // Set To SW Switch
+	//PlatformEFIOWrite2Byte(pAdapter, RFPinsSelect, 0x0488);
+	write_nic_word(dev, RFPinsOutput, 0x3a0);
+	//PlatformEFIOWrite2Byte(pAdapter, RFPinsOutput, 0x0480);
+}
+
+
+u32
+RF_ReadReg(
+	struct net_device *dev,
+	u8		offset
+	)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	u32			data2Write;
+	u8			wlen;
+	u8			rlen;
+	u8			low2high;
+	u32			dataRead;
+
+	switch(priv->rf_chip)
+	{
+	case RFCHIPID_RTL8225:
+	case RF_ZEBRA2:
+	case RF_ZEBRA4:
+		switch(priv->RegThreeWireMode)
+		{
+#ifdef CONFIG_RTL818X_S
+			case HW_THREE_WIRE_PI: // For 87S  Parallel Interface.
+			{
+				data2Write = ((u32)(offset&0x0f));
+				wlen=16;
+				HwHSSIThreeWire(
+					dev,
+					(u8*)(&data2Write),	// pDataBuf,
+					wlen,					// nDataBufBitCnt,
+					0, 					// bSI
+					0); 					// bWrite
+				dataRead= data2Write;
+			}
+			break;
+
+			case HW_THREE_WIRE_SI: // For 87S Serial Interface.
+			{
+				data2Write = ((u32)(offset&0x0f)) ;
+				wlen=16;
+				HwHSSIThreeWire(
+					dev,
+					(u8*)(&data2Write),	// pDataBuf,
+					wlen,					// nDataBufBitCnt,
+					1, 					// bSI
+					0					// bWrite
+					);
+				dataRead= data2Write;
+			}
+			break;
+
+#endif
+			// Perform SW 3-wire programming by driver.
+			default:
+			{
+				data2Write = ((u32)(offset&0x1f)) << 27; // For Zebra E-cut. 2005.04.11, by rcnjko.
+				wlen = 6;
+				rlen = 12;
+				low2high = 0;
+				ZEBRA_RFSerialRead(dev, data2Write, wlen,&dataRead,rlen, low2high);
+			}
+			break;
+		}
+		break;
+	default:
+		dataRead = 0;
+		break;
+	}
+
+	return dataRead;
+}
+
+
+// by Owen on 04/07/14 for writing BB register successfully
+void
+WriteBBPortUchar(
+	struct net_device *dev,
+	u32		Data
+	)
+{
+	//u8	TimeoutCounter;
+	u8	RegisterContent;
+	u8	UCharData;
+
+	UCharData = (u8)((Data & 0x0000ff00) >> 8);
+	PlatformIOWrite4Byte(dev, PhyAddr, Data);
+	//for(TimeoutCounter = 10; TimeoutCounter > 0; TimeoutCounter--)
+	{
+		PlatformIOWrite4Byte(dev, PhyAddr, Data & 0xffffff7f);
+		RegisterContent = PlatformIORead1Byte(dev, PhyDataR);
+		//if(UCharData == RegisterContent)
+		//	break;
+	}
+}
+
+u8
+ReadBBPortUchar(
+	struct net_device *dev,
+	u32		addr
+	)
+{
+	//u8	TimeoutCounter;
+	u8	RegisterContent;
+
+	PlatformIOWrite4Byte(dev, PhyAddr, addr & 0xffffff7f);
+	RegisterContent = PlatformIORead1Byte(dev, PhyDataR);
+
+	return RegisterContent;
+}
+//{by amy 080312
+#ifdef CONFIG_RTL818X_S
+//
+//	Description:
+//		Perform Antenna settings with antenna diversity on 87SE.
+//    Created by Roger, 2008.01.25.
+//
+bool
+SetAntennaConfig87SE(
+	struct net_device *dev,
+	u8			DefaultAnt,		// 0: Main, 1: Aux.
+	bool		bAntDiversity 	// 1:Enable, 0: Disable.
+)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	bool   bAntennaSwitched = true;
+
+	//printk("SetAntennaConfig87SE(): DefaultAnt(%d), bAntDiversity(%d)\n", DefaultAnt, bAntDiversity);
+
+	// Threshold for antenna diversity.
+	write_phy_cck(dev, 0x0c, 0x09); // Reg0c : 09
+
+	if( bAntDiversity )  //  Enable Antenna Diversity.
+	{
+		if( DefaultAnt == 1 )  // aux antenna
+		{
+			// Mac register, aux antenna
+			write_nic_byte(dev, ANTSEL, 0x00);
+
+			// Config CCK RX antenna.
+			write_phy_cck(dev, 0x11, 0xbb); // Reg11 : bb
+			write_phy_cck(dev, 0x01, 0xc7); // Reg01 : c7
+
+			// Config OFDM RX antenna.
+			write_phy_ofdm(dev, 0x0D, 0x54);   // Reg0d : 54
+			write_phy_ofdm(dev, 0x18, 0xb2);  // Reg18 : b2
+		}
+		else //  use main antenna
+		{
+			// Mac register, main antenna
+			write_nic_byte(dev, ANTSEL, 0x03);
+			//base band
+			// Config CCK RX antenna.
+			write_phy_cck(dev, 0x11, 0x9b); // Reg11 : 9b
+			write_phy_cck(dev, 0x01, 0xc7); // Reg01 : c7
+
+			// Config OFDM RX antenna.
+			write_phy_ofdm(dev, 0x0d, 0x5c);   // Reg0d : 5c
+			write_phy_ofdm(dev, 0x18, 0xb2);  // Reg18 : b2
+		}
+	}
+	else   // Disable Antenna Diversity.
+	{
+		if( DefaultAnt == 1 ) // aux Antenna
+		{
+			// Mac register, aux antenna
+			write_nic_byte(dev, ANTSEL, 0x00);
+
+			// Config CCK RX antenna.
+			write_phy_cck(dev, 0x11, 0xbb); // Reg11 : bb
+			write_phy_cck(dev, 0x01, 0x47); // Reg01 : 47
+
+			// Config OFDM RX antenna.
+			write_phy_ofdm(dev, 0x0D, 0x54);   // Reg0d : 54
+			write_phy_ofdm(dev, 0x18, 0x32);  // Reg18 : 32
+		}
+		else // main Antenna
+		{
+			// Mac register, main antenna
+			write_nic_byte(dev, ANTSEL, 0x03);
+
+			// Config CCK RX antenna.
+			write_phy_cck(dev, 0x11, 0x9b); // Reg11 : 9b
+			write_phy_cck(dev, 0x01, 0x47); // Reg01 : 47
+
+			// Config OFDM RX antenna.
+			write_phy_ofdm(dev, 0x0D, 0x5c);   // Reg0d : 5c
+			write_phy_ofdm(dev, 0x18, 0x32);  // Reg18 : 32
+		}
+	}
+	priv->CurrAntennaIndex = DefaultAnt; // Update default settings.
+	return	bAntennaSwitched;
+}
+#endif
+//by amy 080312
+/*---------------------------------------------------------------
+  * Hardware Initialization.
+  * the code is ported from Windows source code
+  ----------------------------------------------------------------*/
+
+void
+ZEBRA_Config_85BASIC_HardCode(
+	struct net_device *dev
+	)
+{
+
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	u32			i;
+	u32	addr,data;
+	u32	u4bRegOffset, u4bRegValue, u4bRF23, u4bRF24;
+       u8			u1b24E;
+
+#ifdef CONFIG_RTL818X_S
+
+	//=============================================================================
+	// 87S_PCIE :: RADIOCFG.TXT
+	//=============================================================================
+
+
+	// Page1 : reg16-reg30
+	RF_WriteReg(dev, 0x00, 0x013f);			mdelay(1); // switch to page1
+	u4bRF23= RF_ReadReg(dev, 0x08);			mdelay(1);
+	u4bRF24= RF_ReadReg(dev, 0x09);			mdelay(1);
+
+	if (u4bRF23==0x818 && u4bRF24==0x70C && priv->card_8185 == VERSION_8187S_C)
+		priv->card_8185 = VERSION_8187S_D;
+
+	// Page0 : reg0-reg15
+
+//	RF_WriteReg(dev, 0x00, 0x003f);			mdelay(1);//1
+	RF_WriteReg(dev, 0x00, 0x009f);      	mdelay(1);// 1
+
+	RF_WriteReg(dev, 0x01, 0x06e0);			mdelay(1);
+
+//	RF_WriteReg(dev, 0x02, 0x004c);			mdelay(1);//2
+	RF_WriteReg(dev, 0x02, 0x004d);			mdelay(1);// 2
+
+//	RF_WriteReg(dev, 0x03, 0x0000);			mdelay(1);//3
+	RF_WriteReg(dev, 0x03, 0x07f1);			mdelay(1);// 3
+
+	RF_WriteReg(dev, 0x04, 0x0975);			mdelay(1);
+	RF_WriteReg(dev, 0x05, 0x0c72);			mdelay(1);
+	RF_WriteReg(dev, 0x06, 0x0ae6);			mdelay(1);
+	RF_WriteReg(dev, 0x07, 0x00ca);			mdelay(1);
+	RF_WriteReg(dev, 0x08, 0x0e1c);			mdelay(1);
+	RF_WriteReg(dev, 0x09, 0x02f0);			mdelay(1);
+	RF_WriteReg(dev, 0x0a, 0x09d0);			mdelay(1);
+	RF_WriteReg(dev, 0x0b, 0x01ba);			mdelay(1);
+	RF_WriteReg(dev, 0x0c, 0x0640);			mdelay(1);
+	RF_WriteReg(dev, 0x0d, 0x08df);			mdelay(1);
+	RF_WriteReg(dev, 0x0e, 0x0020);			mdelay(1);
+	RF_WriteReg(dev, 0x0f, 0x0990);			mdelay(1);
+
+
+	// Page1 : reg16-reg30
+	RF_WriteReg(dev, 0x00, 0x013f);			mdelay(1);
+
+	RF_WriteReg(dev, 0x03, 0x0806);			mdelay(1);
+
+	if(priv->card_8185 < VERSION_8187S_C)
+	{
+		RF_WriteReg(dev, 0x04, 0x03f7);			mdelay(1);
+		RF_WriteReg(dev, 0x05, 0x05ab);			mdelay(1);
+		RF_WriteReg(dev, 0x06, 0x00c1);			mdelay(1);
+	}
+	else
+	{
+		RF_WriteReg(dev, 0x04, 0x03a7);			mdelay(1);
+		RF_WriteReg(dev, 0x05, 0x059b);			mdelay(1);
+		RF_WriteReg(dev, 0x06, 0x0081);			mdelay(1);
+	}
+
+
+	RF_WriteReg(dev, 0x07, 0x01A0);			mdelay(1);
+// Don't write RF23/RF24 to make a difference between 87S C cut and D cut. asked by SD3 stevenl.
+//	RF_WriteReg(dev, 0x08, 0x0597);			mdelay(1);
+//	RF_WriteReg(dev, 0x09, 0x050a);			mdelay(1);
+	RF_WriteReg(dev, 0x0a, 0x0001);			mdelay(1);
+	RF_WriteReg(dev, 0x0b, 0x0418);			mdelay(1);
+
+	if(priv->card_8185 == VERSION_8187S_D)
+	{
+		RF_WriteReg(dev, 0x0c, 0x0fbe);			mdelay(1);
+		RF_WriteReg(dev, 0x0d, 0x0008);			mdelay(1);
+		RF_WriteReg(dev, 0x0e, 0x0807);			mdelay(1); // RX LO buffer
+	}
+	else
+	{
+		RF_WriteReg(dev, 0x0c, 0x0fbe);			mdelay(1);
+		RF_WriteReg(dev, 0x0d, 0x0008);			mdelay(1);
+		RF_WriteReg(dev, 0x0e, 0x0806);			mdelay(1); // RX LO buffer
+	}
+
+	RF_WriteReg(dev, 0x0f, 0x0acc);			mdelay(1);
+
+//	RF_WriteReg(dev, 0x00, 0x017f);			mdelay(1);//6
+	RF_WriteReg(dev, 0x00, 0x01d7);			mdelay(1);// 6
+
+	RF_WriteReg(dev, 0x03, 0x0e00);			mdelay(1);
+	RF_WriteReg(dev, 0x04, 0x0e50);			mdelay(1);
+	for(i=0;i<=36;i++)
+	{
+		RF_WriteReg(dev, 0x01, i);                     mdelay(1);
+		RF_WriteReg(dev, 0x02, ZEBRA_RF_RX_GAIN_TABLE[i]); mdelay(1);
+		//DbgPrint("RF - 0x%x = 0x%x", i, ZEBRA_RF_RX_GAIN_TABLE[i]);
+	}
+
+	RF_WriteReg(dev, 0x05, 0x0203);			mdelay(1); 	/// 203, 343
+	//RF_WriteReg(dev, 0x06, 0x0300);			mdelay(1);	// 400
+	RF_WriteReg(dev, 0x06, 0x0200);			mdelay(1);	// 400
+
+	RF_WriteReg(dev, 0x00, 0x0137);			mdelay(1);	// switch to reg16-reg30, and HSSI disable 137
+	mdelay(10); 	// Deay 10 ms. //0xfd
+
+//	RF_WriteReg(dev, 0x0c, 0x09be);			mdelay(1);	// 7
+	//RF_WriteReg(dev, 0x0c, 0x07be);			mdelay(1);
+	//mdelay(10); 	// Deay 10 ms. //0xfd
+
+	RF_WriteReg(dev, 0x0d, 0x0008);			mdelay(1);	// Z4 synthesizer loop filter setting, 392
+	mdelay(10); 	// Deay 10 ms. //0xfd
+
+	RF_WriteReg(dev, 0x00, 0x0037);			mdelay(1);	// switch to reg0-reg15, and HSSI disable
+	mdelay(10); 	// Deay 10 ms. //0xfd
+
+	RF_WriteReg(dev, 0x04, 0x0160);			mdelay(1); 	// CBC on, Tx Rx disable, High gain
+	mdelay(10); 	// Deay 10 ms. //0xfd
+
+	RF_WriteReg(dev, 0x07, 0x0080);			mdelay(1);	// Z4 setted channel 1
+	mdelay(10); 	// Deay 10 ms. //0xfd
+
+	RF_WriteReg(dev, 0x02, 0x088D);			mdelay(1);	// LC calibration
+	mdelay(200); 	// Deay 200 ms. //0xfd
+	mdelay(10); 	// Deay 10 ms. //0xfd
+	mdelay(10); 	// Deay 10 ms. //0xfd
+
+	RF_WriteReg(dev, 0x00, 0x0137);			mdelay(1);	// switch to reg16-reg30 137, and HSSI disable 137
+	mdelay(10); 	// Deay 10 ms. //0xfd
+
+	RF_WriteReg(dev, 0x07, 0x0000);			mdelay(1);
+	RF_WriteReg(dev, 0x07, 0x0180);			mdelay(1);
+	RF_WriteReg(dev, 0x07, 0x0220);			mdelay(1);
+	RF_WriteReg(dev, 0x07, 0x03E0);			mdelay(1);
+
+	// DAC calibration off 20070702
+	RF_WriteReg(dev, 0x06, 0x00c1);			mdelay(1);
+	RF_WriteReg(dev, 0x0a, 0x0001);			mdelay(1);
+//{by amy 080312
+	// For crystal calibration, added by Roger, 2007.12.11.
+	if( priv->bXtalCalibration ) // reg 30.
+	{ // enable crystal calibration.
+		// RF Reg[30], (1)Xin:[12:9], Xout:[8:5],  addr[4:0].
+		// (2)PA Pwr delay timer[15:14], default: 2.4us, set BIT15=0
+		// (3)RF signal on/off when calibration[13], default: on, set BIT13=0.
+		// So we should minus 4 BITs offset.
+		RF_WriteReg(dev, 0x0f, (priv->XtalCal_Xin<<5)|(priv->XtalCal_Xout<<1)|BIT11|BIT9);			mdelay(1);
+		printk("ZEBRA_Config_85BASIC_HardCode(): (%02x)\n",
+				(priv->XtalCal_Xin<<5) | (priv->XtalCal_Xout<<1) | BIT11| BIT9);
+	}
+	else
+	{ // using default value. Xin=6, Xout=6.
+		RF_WriteReg(dev, 0x0f, 0x0acc);			mdelay(1);
+	}
+//by amy 080312
+//	RF_WriteReg(dev, 0x0f, 0x0acc);			mdelay(1);  //-by amy 080312
+
+	RF_WriteReg(dev, 0x00, 0x00bf);			mdelay(1); // switch to reg0-reg15, and HSSI enable
+//	RF_WriteReg(dev, 0x0d, 0x009f);			mdelay(1); // Rx BB start calibration, 00c//-edward
+	RF_WriteReg(dev, 0x0d, 0x08df);			mdelay(1); // Rx BB start calibration, 00c//+edward
+	RF_WriteReg(dev, 0x02, 0x004d);			mdelay(1); // temperature meter off
+	RF_WriteReg(dev, 0x04, 0x0975);			mdelay(1); // Rx mode
+	mdelay(10);	// Deay 10 ms. //0xfe
+	mdelay(10);	// Deay 10 ms. //0xfe
+	mdelay(10);	// Deay 10 ms. //0xfe
+	RF_WriteReg(dev, 0x00, 0x0197);			mdelay(1); // Rx mode//+edward
+	RF_WriteReg(dev, 0x05, 0x05ab);			mdelay(1); // Rx mode//+edward
+	RF_WriteReg(dev, 0x00, 0x009f);			mdelay(1); // Rx mode//+edward
+
+#if 0//-edward
+	RF_WriteReg(dev, 0x00, 0x0197);			mdelay(1);
+	RF_WriteReg(dev, 0x05, 0x05ab);			mdelay(1);
+	RF_WriteReg(dev, 0x00, 0x009F);			mdelay(1);
+#endif
+	RF_WriteReg(dev, 0x01, 0x0000);			mdelay(1); // Rx mode//+edward
+	RF_WriteReg(dev, 0x02, 0x0000);			mdelay(1); // Rx mode//+edward
+	//power save parameters.
+	u1b24E = read_nic_byte(dev, 0x24E);
+	write_nic_byte(dev, 0x24E, (u1b24E & (~(BIT5|BIT6))));
+
+	//=============================================================================
+
+	//=============================================================================
+	// CCKCONF.TXT
+	//=============================================================================
+
+	/*	[POWER SAVE] Power Saving Parameters by jong. 2007-11-27
+	   	CCK reg0x00[7]=1'b1 :power saving for TX (default)
+		CCK reg0x00[6]=1'b1: power saving for RX (default)
+		CCK reg0x06[4]=1'b1: turn off channel estimation related circuits if not doing channel estimation.
+		CCK reg0x06[3]=1'b1: turn off unused circuits before cca = 1
+		CCK reg0x06[2]=1'b1: turn off cck's circuit if macrst =0
+	*/
+#if 0
+	write_nic_dword(dev, PHY_ADR, 0x0100c880);
+	write_nic_dword(dev, PHY_ADR, 0x01001c86);
+	write_nic_dword(dev, PHY_ADR, 0x01007890);
+	write_nic_dword(dev, PHY_ADR, 0x0100d0ae);
+	write_nic_dword(dev, PHY_ADR, 0x010006af);
+	write_nic_dword(dev, PHY_ADR, 0x01004681);
+#endif
+	write_phy_cck(dev,0x00,0xc8);
+	write_phy_cck(dev,0x06,0x1c);
+	write_phy_cck(dev,0x10,0x78);
+	write_phy_cck(dev,0x2e,0xd0);
+	write_phy_cck(dev,0x2f,0x06);
+	write_phy_cck(dev,0x01,0x46);
+
+	// power control
+	write_nic_byte(dev, CCK_TXAGC, 0x10);
+	write_nic_byte(dev, OFDM_TXAGC, 0x1B);
+	write_nic_byte(dev, ANTSEL, 0x03);
+#else
+	//=============================================================================
+	// RADIOCFG.TXT
+	//=============================================================================
+
+	RF_WriteReg(dev, 0x00, 0x00b7);			mdelay(1);
+	RF_WriteReg(dev, 0x01, 0x0ee0);			mdelay(1);
+	RF_WriteReg(dev, 0x02, 0x044d);			mdelay(1);
+	RF_WriteReg(dev, 0x03, 0x0441);			mdelay(1);
+	RF_WriteReg(dev, 0x04, 0x08c3);			mdelay(1);
+	RF_WriteReg(dev, 0x05, 0x0c72);			mdelay(1);
+	RF_WriteReg(dev, 0x06, 0x00e6);			mdelay(1);
+	RF_WriteReg(dev, 0x07, 0x082a);			mdelay(1);
+	RF_WriteReg(dev, 0x08, 0x003f);			mdelay(1);
+	RF_WriteReg(dev, 0x09, 0x0335);			mdelay(1);
+	RF_WriteReg(dev, 0x0a, 0x09d4);			mdelay(1);
+	RF_WriteReg(dev, 0x0b, 0x07bb);			mdelay(1);
+	RF_WriteReg(dev, 0x0c, 0x0850);			mdelay(1);
+	RF_WriteReg(dev, 0x0d, 0x0cdf);			mdelay(1);
+	RF_WriteReg(dev, 0x0e, 0x002b);			mdelay(1);
+	RF_WriteReg(dev, 0x0f, 0x0114);			mdelay(1);
+
+	RF_WriteReg(dev, 0x00, 0x01b7);			mdelay(1);
+
+
+	for(i=1;i<=95;i++)
+	{
+		RF_WriteReg(dev, 0x01, i);	mdelay(1);
+		RF_WriteReg(dev, 0x02, ZEBRA_RF_RX_GAIN_TABLE[i]); mdelay(1);
+		//DbgPrint("RF - 0x%x = 0x%x", i, ZEBRA_RF_RX_GAIN_TABLE[i]);
+	}
+
+	RF_WriteReg(dev, 0x03, 0x0080);			mdelay(1); 	// write reg 18
+	RF_WriteReg(dev, 0x05, 0x0004);			mdelay(1);	// write reg 20
+	RF_WriteReg(dev, 0x00, 0x00b7);			mdelay(1);	// switch to reg0-reg15
+	//0xfd
+	//0xfd
+	//0xfd
+	RF_WriteReg(dev, 0x02, 0x0c4d);			mdelay(1);
+	mdelay(100);	// Deay 100 ms. //0xfe
+	mdelay(100);	// Deay 100 ms. //0xfe
+	RF_WriteReg(dev, 0x02, 0x044d);			mdelay(1);
+	RF_WriteReg(dev, 0x00, 0x02bf);			mdelay(1);	//0x002f disable 6us corner change,  06f--> enable
+
+	//=============================================================================
+
+	//=============================================================================
+	// CCKCONF.TXT
+	//=============================================================================
+
+	//=============================================================================
+
+	//=============================================================================
+	// Follow WMAC RTL8225_Config()
+	//=============================================================================
+
+	// power control
+	write_nic_byte(dev, CCK_TXAGC, 0x03);
+	write_nic_byte(dev, OFDM_TXAGC, 0x07);
+	write_nic_byte(dev, ANTSEL, 0x03);
+
+	//=============================================================================
+
+	// OFDM BBP setup
+//	SetOutputEnableOfRfPins(dev);//by amy
+#endif
+
+
+
+	//=============================================================================
+	// AGC.txt
+	//=============================================================================
+
+//	PlatformIOWrite4Byte( dev, PhyAddr, 0x00001280);	// Annie, 2006-05-05
+	write_phy_ofdm(dev, 0x00, 0x12);
+	//WriteBBPortUchar(dev, 0x00001280);
+
+	for (i=0; i<128; i++)
+	{
+		//DbgPrint("AGC - [%x+1] = 0x%x\n", i, ZEBRA_AGC[i+1]);
+
+		data = ZEBRA_AGC[i+1];
+		data = data << 8;
+		data = data | 0x0000008F;
+
+		addr = i + 0x80; //enable writing AGC table
+		addr = addr << 8;
+		addr = addr | 0x0000008E;
+
+		WriteBBPortUchar(dev, data);
+		WriteBBPortUchar(dev, addr);
+		WriteBBPortUchar(dev, 0x0000008E);
+	}
+
+	PlatformIOWrite4Byte( dev, PhyAddr, 0x00001080);	// Annie, 2006-05-05
+	//WriteBBPortUchar(dev, 0x00001080);
+
+	//=============================================================================
+
+	//=============================================================================
+	// OFDMCONF.TXT
+	//=============================================================================
+
+	for(i=0; i<60; i++)
+	{
+		u4bRegOffset=i;
+		u4bRegValue=OFDM_CONFIG[i];
+
+		//DbgPrint("OFDM - 0x%x = 0x%x\n", u4bRegOffset, u4bRegValue);
+
+		WriteBBPortUchar(dev,
+						(0x00000080 |
+						(u4bRegOffset & 0x7f) |
+						((u4bRegValue & 0xff) << 8)));
+	}
+
+	//=============================================================================
+//by amy for antenna
+	//=============================================================================
+//{by amy 080312
+#ifdef CONFIG_RTL818X_S
+	// Config Sw/Hw  Combinational Antenna Diversity. Added by Roger, 2008.02.26.
+	SetAntennaConfig87SE(dev, priv->bDefaultAntenna1, priv->bSwAntennaDiverity);
+#endif
+//by amy 080312}
+#if 0
+	// Config Sw/Hw  Antenna Diversity
+	if( priv->bSwAntennaDiverity )  //  Use SW+Hw Antenna Diversity
+	{
+		if( priv->bDefaultAntenna1 == true )  // aux antenna
+		{
+			// Mac register, aux antenna
+			write_nic_byte(dev, ANTSEL, 0x00);
+			// Config CCK RX antenna.
+			write_phy_cck(dev, 0x11, 0xbb); // Reg11 : bb
+			write_phy_cck(dev, 0x0c, 0x09); // Reg0c : 09
+			write_phy_cck(dev, 0x01, 0xc7); // Reg01 : c7
+			// Config OFDM RX antenna.
+			write_phy_ofdm(dev, 0x0d, 0x54);   // Reg0d : 54
+			write_phy_ofdm(dev, 0x18, 0xb2);  // Reg18 : b2
+		}
+		else //  main antenna
+		{
+			// Mac register, main antenna
+			write_nic_byte(dev, ANTSEL, 0x03);
+			//base band
+			// Config CCK RX antenna.
+			write_phy_cck(dev, 0x11, 0x9b); // Reg11 : 9b
+			write_phy_cck(dev, 0x0c, 0x09); // Reg0c : 09
+			write_phy_cck(dev, 0x01, 0xc7); // Reg01 : c7
+			// Config OFDM RX antenna.
+			write_phy_ofdm(dev, 0x0d, 0x5c);   // Reg0d : 5c
+			write_phy_ofdm(dev, 0x18, 0xb2);  // Reg18 : b2
+		}
+	}
+	else   // Disable Antenna Diversity
+	{
+		if( priv->bDefaultAntenna1 == true ) // aux Antenna
+		{
+			// Mac register, aux antenna
+			write_nic_byte(dev, ANTSEL, 0x00);
+			// Config CCK RX antenna.
+			write_phy_cck(dev, 0x11, 0xbb); // Reg11 : bb
+			write_phy_cck(dev, 0x0c, 0x09); // Reg0c : 09
+			write_phy_cck(dev, 0x01, 0x47); // Reg01 : 47
+			// Config OFDM RX antenna.
+			write_phy_ofdm(dev, 0x0d, 0x54);   // Reg0d : 54
+			write_phy_ofdm(dev, 0x18, 0x32);  // Reg18 : 32
+		}
+		else // main Antenna
+		{
+			// Mac register, main antenna
+			write_nic_byte(dev, ANTSEL, 0x03);
+			// Config CCK RX antenna.
+			write_phy_cck(dev, 0x11, 0x9b); // Reg11 : 9b
+			write_phy_cck(dev, 0x0c, 0x09); // Reg0c : 09
+			write_phy_cck(dev, 0x01, 0x47); // Reg01 : 47
+			// Config OFDM RX antenna.
+			write_phy_ofdm(dev, 0x0d, 0x5c);   // Reg0d : 5c
+			write_phy_ofdm(dev, 0x18, 0x32);  // Reg18 : 32
+		}
+	}
+#endif
+//by amy for antenna
+}
+
+
+void
+UpdateInitialGain(
+	struct net_device *dev
+	)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	//unsigned char* IGTable;
+	//u8			DIG_CurrentInitialGain = 4;
+	//unsigned char u1Tmp;
+
+	//lzm add 080826
+	if(priv->eRFPowerState != eRfOn)
+	{
+		//Don't access BB/RF under disable PLL situation.
+		//RT_TRACE(COMP_DIG, DBG_LOUD, ("UpdateInitialGain - pHalData->eRFPowerState!=eRfOn\n"));
+		// Back to the original state
+		priv->InitialGain= priv->InitialGainBackUp;
+		return;
+	}
+
+	switch(priv->rf_chip)
+	{
+#if 0
+	case RF_ZEBRA2:
+		// Dynamic set initial gain, by shien chang, 2006.07.14
+		switch(priv->InitialGain)
+		{
+			case 1: //m861dBm
+				DMESG("RTL8185B + 8225 Initial Gain State 1: -82 dBm \n");
+				write_nic_dword(dev, PhyAddr, 0x2697);	mdelay(1);
+				write_nic_dword(dev, PhyAddr, 0x86a4);	mdelay(1);
+				write_nic_dword(dev, PhyAddr, 0xfa85);	mdelay(1);
+				break;
+
+			case 2: //m862dBm
+				DMESG("RTL8185B + 8225 Initial Gain State 2: -82 dBm \n");
+				write_nic_dword(dev, PhyAddr, 0x2697);	mdelay(1);
+				write_nic_dword(dev, PhyAddr, 0x86a4);	mdelay(1);
+				write_nic_dword(dev, PhyAddr, 0xfb85);	mdelay(1);
+				break;
+
+			case 3: //m863dBm
+				DMESG("RTL8185B + 8225 Initial Gain State 3: -82 dBm \n");
+				write_nic_dword(dev, PhyAddr, 0x2697);	mdelay(1);
+				write_nic_dword(dev, PhyAddr, 0x96a4);	mdelay(1);
+				write_nic_dword(dev, PhyAddr, 0xfb85);	mdelay(1);
+				break;
+
+			case 4: //m864dBm
+				DMESG("RTL8185B + 8225 Initial Gain State 4: -78 dBm \n");
+				write_nic_dword(dev, PhyAddr, 0x2697);	mdelay(1);
+				write_nic_dword(dev, PhyAddr, 0xa6a4);	mdelay(1);
+				write_nic_dword(dev, PhyAddr, 0xfb85);	mdelay(1);
+				break;
+
+			case 5: //m82dBm
+				DMESG("RTL8185B + 8225 Initial Gain State 5: -74 dBm \n");
+				write_nic_dword(dev, PhyAddr, 0x3697);	mdelay(1);
+				write_nic_dword(dev, PhyAddr, 0xa6a4);	mdelay(1);
+				write_nic_dword(dev, PhyAddr, 0xfb85);	mdelay(1);
+				break;
+
+			case 6: //m78dBm
+				DMESG("RTL8185B + 8225 Initial Gain State 6: -70 dBm \n");
+				write_nic_dword(dev, PhyAddr, 0x4697);	mdelay(1);
+				write_nic_dword(dev, PhyAddr, 0xa6a4);	mdelay(1);
+				write_nic_dword(dev, PhyAddr, 0xfb85);	mdelay(1);
+				break;
+
+			case 7: //m74dBm
+				DMESG("RTL8185B + 8225 Initial Gain State 7: -66 dBm \n");
+				write_nic_dword(dev, PhyAddr, 0x5697);	mdelay(1);
+				write_nic_dword(dev, PhyAddr, 0xa6a4);	mdelay(1);
+				write_nic_dword(dev, PhyAddr, 0xfb85);	mdelay(1);
+				break;
+
+			default:	//MP
+				DMESG("RTL8185B + 8225 Initial Gain State 1: -82 dBm (default)\n");
+				write_nic_dword(dev, PhyAddr, 0x2697);	mdelay(1);
+				write_nic_dword(dev, PhyAddr, 0x86a4);	mdelay(1);
+				write_nic_dword(dev, PhyAddr, 0xfa85);	mdelay(1);
+				break;
+		}
+		break;
+#endif
+	case RF_ZEBRA4:
+		// Dynamic set initial gain, follow 87B
+		switch(priv->InitialGain)
+		{
+			case 1: //m861dBm
+				//DMESG("RTL8187 + 8225 Initial Gain State 1: -82 dBm \n");
+				write_phy_ofdm(dev, 0x17, 0x26);	mdelay(1);
+				write_phy_ofdm(dev, 0x24, 0x86);	mdelay(1);
+				write_phy_ofdm(dev, 0x05, 0xfa);	mdelay(1);
+				break;
+
+			case 2: //m862dBm
+				//DMESG("RTL8187 + 8225 Initial Gain State 2: -82 dBm \n");
+				write_phy_ofdm(dev, 0x17, 0x36);	mdelay(1);
+				write_phy_ofdm(dev, 0x24, 0x86);	mdelay(1);
+				write_phy_ofdm(dev, 0x05, 0xfa);	mdelay(1);
+				break;
+
+			case 3: //m863dBm
+				//DMESG("RTL8187 + 8225 Initial Gain State 3: -82 dBm \n");
+				write_phy_ofdm(dev, 0x17, 0x36);	mdelay(1);
+				write_phy_ofdm(dev, 0x24, 0x86);	mdelay(1);
+				write_phy_ofdm(dev, 0x05, 0xfb);	mdelay(1);
+				break;
+
+			case 4: //m864dBm
+				//DMESG("RTL8187 + 8225 Initial Gain State 4: -78 dBm \n");
+				write_phy_ofdm(dev, 0x17, 0x46);	mdelay(1);
+				write_phy_ofdm(dev, 0x24, 0x86);	mdelay(1);
+				write_phy_ofdm(dev, 0x05, 0xfb);	mdelay(1);
+				break;
+
+			case 5: //m82dBm
+				//DMESG("RTL8187 + 8225 Initial Gain State 5: -74 dBm \n");
+				write_phy_ofdm(dev, 0x17, 0x46);	mdelay(1);
+				write_phy_ofdm(dev, 0x24, 0x96);	mdelay(1);
+				write_phy_ofdm(dev, 0x05, 0xfb);	mdelay(1);
+				break;
+
+			case 6: //m78dBm
+				//DMESG ("RTL8187 + 8225 Initial Gain State 6: -70 dBm \n");
+				write_phy_ofdm(dev, 0x17, 0x56);	mdelay(1);
+				write_phy_ofdm(dev, 0x24, 0x96);	mdelay(1);
+				write_phy_ofdm(dev, 0x05, 0xfc);	mdelay(1);
+				break;
+
+			case 7: //m74dBm
+				//DMESG("RTL8187 + 8225 Initial Gain State 7: -66 dBm \n");
+				write_phy_ofdm(dev, 0x17, 0x56);	mdelay(1);
+				write_phy_ofdm(dev, 0x24, 0xa6);	mdelay(1);
+				write_phy_ofdm(dev, 0x05, 0xfc);	mdelay(1);
+				break;
+
+			case 8:
+				//DMESG("RTL8187 + 8225 Initial Gain State 8:\n");
+				write_phy_ofdm(dev, 0x17, 0x66);	mdelay(1);
+				write_phy_ofdm(dev, 0x24, 0xb6);	mdelay(1);
+				write_phy_ofdm(dev, 0x05, 0xfc);	mdelay(1);
+				break;
+
+
+			default:	//MP
+				//DMESG("RTL8187 + 8225 Initial Gain State 1: -82 dBm (default)\n");
+				write_phy_ofdm(dev, 0x17, 0x26);	mdelay(1);
+				write_phy_ofdm(dev, 0x24, 0x86);	mdelay(1);
+				write_phy_ofdm(dev, 0x05, 0xfa);	mdelay(1);
+				break;
+		}
+		break;
+
+
+	default:
+		DMESG("UpdateInitialGain(): unknown RFChipID: %#X\n", priv->rf_chip);
+		break;
+	}
+}
+#ifdef CONFIG_RTL818X_S
+//
+//	Description:
+//		Tx Power tracking mechanism routine on 87SE.
+// 	Created by Roger, 2007.12.11.
+//
+void
+InitTxPwrTracking87SE(
+	struct net_device *dev
+)
+{
+	//struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	u32	u4bRfReg;
+
+	u4bRfReg = RF_ReadReg(dev, 0x02);
+
+	// Enable Thermal meter indication.
+	//printk("InitTxPwrTracking87SE(): Enable thermal meter indication, Write RF[0x02] = %#x", u4bRfReg|PWR_METER_EN);
+	RF_WriteReg(dev, 0x02, u4bRfReg|PWR_METER_EN);			mdelay(1);
+}
+
+#endif
+void
+PhyConfig8185(
+	struct net_device *dev
+	)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+       write_nic_dword(dev, RCR, priv->ReceiveConfig);
+	   priv->RFProgType = read_nic_byte(dev, CONFIG4) & 0x03;
+     	// RF config
+	switch(priv->rf_chip)
+	{
+	case RF_ZEBRA2:
+	case RF_ZEBRA4:
+		ZEBRA_Config_85BASIC_HardCode( dev);
+		break;
+	}
+//{by amy 080312
+#ifdef CONFIG_RTL818X_S
+	// Set default initial gain state to 4, approved by SD3 DZ, by Bruce, 2007-06-06.
+	if(priv->bDigMechanism)
+	{
+		if(priv->InitialGain == 0)
+			priv->InitialGain = 4;
+		//printk("PhyConfig8185(): DIG is enabled, set default initial gain index to %d\n", priv->InitialGain);
+	}
+
+	//
+	// Enable thermal meter indication to implement TxPower tracking on 87SE.
+	// We initialize thermal meter here to avoid unsuccessful configuration.
+	// Added by Roger, 2007.12.11.
+	//
+	if(priv->bTxPowerTrack)
+		InitTxPwrTracking87SE(dev);
+
+#endif
+//by amy 080312}
+	priv->InitialGainBackUp= priv->InitialGain;
+	UpdateInitialGain(dev);
+
+	return;
+}
+
+
+
+
+void
+HwConfigureRTL8185(
+		struct net_device *dev
+		)
+{
+	//RTL8185_TODO: Determine Retrylimit, TxAGC, AutoRateFallback control.
+//	u8		bUNIVERSAL_CONTROL_RL = 1;
+        u8              bUNIVERSAL_CONTROL_RL = 0;
+
+	u8		bUNIVERSAL_CONTROL_AGC = 1;
+	u8		bUNIVERSAL_CONTROL_ANT = 1;
+	u8		bAUTO_RATE_FALLBACK_CTL = 1;
+	u8		val8;
+	//struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	//struct ieee80211_device *ieee = priv->ieee80211;
+      	//if(IS_WIRELESS_MODE_A(dev) || IS_WIRELESS_MODE_G(dev))
+//{by amy 080312	if((ieee->mode == IEEE_G)||(ieee->mode == IEEE_A))
+//	{
+//		write_nic_word(dev, BRSR, 0xffff);
+//	}
+//	else
+//	{
+//		write_nic_word(dev, BRSR, 0x000f);
+//	}
+//by amy 080312}
+        write_nic_word(dev, BRSR, 0x0fff);
+	// Retry limit
+	val8 = read_nic_byte(dev, CW_CONF);
+
+	if(bUNIVERSAL_CONTROL_RL)
+		val8 = val8 & 0xfd;
+	else
+		val8 = val8 | 0x02;
+
+	write_nic_byte(dev, CW_CONF, val8);
+
+	// Tx AGC
+	val8 = read_nic_byte(dev, TXAGC_CTL);
+	if(bUNIVERSAL_CONTROL_AGC)
+	{
+		write_nic_byte(dev, CCK_TXAGC, 128);
+		write_nic_byte(dev, OFDM_TXAGC, 128);
+		val8 = val8 & 0xfe;
+	}
+	else
+	{
+		val8 = val8 | 0x01 ;
+	}
+
+
+	write_nic_byte(dev, TXAGC_CTL, val8);
+
+	// Tx Antenna including Feedback control
+	val8 = read_nic_byte(dev, TXAGC_CTL );
+
+	if(bUNIVERSAL_CONTROL_ANT)
+	{
+		write_nic_byte(dev, ANTSEL, 0x00);
+		val8 = val8 & 0xfd;
+	}
+	else
+	{
+		val8 = val8 & (val8|0x02); //xiong-2006-11-15
+	}
+
+	write_nic_byte(dev, TXAGC_CTL, val8);
+
+	// Auto Rate fallback control
+	val8 = read_nic_byte(dev, RATE_FALLBACK);
+	val8 &= 0x7c;
+	if( bAUTO_RATE_FALLBACK_CTL )
+	{
+		val8 |= RATE_FALLBACK_CTL_ENABLE | RATE_FALLBACK_CTL_AUTO_STEP1;
+
+		// <RJ_TODO_8185B> We shall set up the ARFR according to user's setting.
+		//write_nic_word(dev, ARFR, 0x0fff); // set 1M ~ 54M
+//by amy
+#if 0
+		PlatformIOWrite2Byte(dev, ARFR, 0x0fff); 	// set 1M ~ 54M
+#endif
+#ifdef CONFIG_RTL818X_S
+	        // Aadded by Roger, 2007.11.15.
+	        PlatformIOWrite2Byte(dev, ARFR, 0x0fff); //set 1M ~ 54Mbps.
+#else
+		PlatformIOWrite2Byte(dev, ARFR, 0x0c00); //set 48Mbps, 54Mbps.
+                // By SD3 szuyi's request. by Roger, 2007.03.26.
+#endif
+//by amy
+	}
+	else
+	{
+	}
+	write_nic_byte(dev, RATE_FALLBACK, val8);
+}
+
+
+
+static void
+MacConfig_85BASIC_HardCode(
+	struct net_device *dev)
+{
+	//============================================================================
+	// MACREG.TXT
+	//============================================================================
+	int			nLinesRead = 0;
+
+	u32	u4bRegOffset, u4bRegValue,u4bPageIndex = 0;
+	int	i;
+
+	nLinesRead=sizeof(MAC_REG_TABLE)/2;
+
+	for(i = 0; i < nLinesRead; i++)  //nLinesRead=101
+	{
+		u4bRegOffset=MAC_REG_TABLE[i][0];
+		u4bRegValue=MAC_REG_TABLE[i][1];
+
+                if(u4bRegOffset == 0x5e)
+                {
+                    u4bPageIndex = u4bRegValue;
+                }
+                else
+                {
+                    u4bRegOffset |= (u4bPageIndex << 8);
+                }
+                //DbgPrint("MAC - 0x%x = 0x%x\n", u4bRegOffset, u4bRegValue);
+		write_nic_byte(dev, u4bRegOffset, (u8)u4bRegValue);
+	}
+	//============================================================================
+}
+
+
+
+static void
+MacConfig_85BASIC(
+	struct net_device *dev)
+{
+
+       u8			u1DA;
+	MacConfig_85BASIC_HardCode(dev);
+
+	//============================================================================
+
+	// Follow TID_AC_MAP of WMac.
+	write_nic_word(dev, TID_AC_MAP, 0xfa50);
+
+	// Interrupt Migration, Jong suggested we use set 0x0000 first, 2005.12.14, by rcnjko.
+	write_nic_word(dev, IntMig, 0x0000);
+
+	// Prevent TPC to cause CRC error. Added by Annie, 2006-06-10.
+	PlatformIOWrite4Byte(dev, 0x1F0, 0x00000000);
+	PlatformIOWrite4Byte(dev, 0x1F4, 0x00000000);
+	PlatformIOWrite1Byte(dev, 0x1F8, 0x00);
+
+	// Asked for by SD3 CM Lin, 2006.06.27, by rcnjko.
+	//PlatformIOWrite4Byte(dev, RFTiming, 0x00004001);
+//by amy
+#if 0
+	write_nic_dword(dev, RFTiming, 0x00004001);
+#endif
+#ifdef CONFIG_RTL818X_S
+	// power save parameter based on "87SE power save parameters 20071127.doc", as follow.
+
+	//Enable DA10 TX power saving
+	u1DA = read_nic_byte(dev, PHYPR);
+	write_nic_byte(dev, PHYPR, (u1DA | BIT2) );
+
+	//POWER:
+	write_nic_word(dev, 0x360, 0x1000);
+	write_nic_word(dev, 0x362, 0x1000);
+
+	// AFE.
+	write_nic_word(dev, 0x370, 0x0560);
+	write_nic_word(dev, 0x372, 0x0560);
+	write_nic_word(dev, 0x374, 0x0DA4);
+	write_nic_word(dev, 0x376, 0x0DA4);
+	write_nic_word(dev, 0x378, 0x0560);
+	write_nic_word(dev, 0x37A, 0x0560);
+	write_nic_word(dev, 0x37C, 0x00EC);
+//	write_nic_word(dev, 0x37E, 0x00FE);//-edward
+	write_nic_word(dev, 0x37E, 0x00EC);//+edward
+#else
+       write_nic_dword(dev, RFTiming, 0x00004003);
+#endif
+       write_nic_byte(dev, 0x24E,0x01);
+//by amy
+
+}
+
+
+
+
+u8
+GetSupportedWirelessMode8185(
+	struct net_device *dev
+)
+{
+	u8			btSupportedWirelessMode = 0;
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+	switch(priv->rf_chip)
+	{
+	case RF_ZEBRA2:
+	case RF_ZEBRA4:
+		btSupportedWirelessMode = (WIRELESS_MODE_B | WIRELESS_MODE_G);
+		break;
+	default:
+		btSupportedWirelessMode = WIRELESS_MODE_B;
+		break;
+	}
+
+	return btSupportedWirelessMode;
+}
+
+void
+ActUpdateChannelAccessSetting(
+	struct net_device *dev,
+	WIRELESS_MODE			WirelessMode,
+	PCHANNEL_ACCESS_SETTING	ChnlAccessSetting
+	)
+{
+	struct r8180_priv *priv = ieee80211_priv(dev);
+	struct ieee80211_device *ieee = priv->ieee80211;
+	AC_CODING	eACI;
+	AC_PARAM	AcParam;
+	//PSTA_QOS	pStaQos = Adapter->MgntInfo.pStaQos;
+	u8	bFollowLegacySetting = 0;
+	u8   u1bAIFS;
+
+	//
+	// <RJ_TODO_8185B>
+	// TODO: We still don't know how to set up these registers, just follow WMAC to
+	// verify 8185B FPAG.
+	//
+	// <RJ_TODO_8185B>
+	// Jong said CWmin/CWmax register are not functional in 8185B,
+	// so we shall fill channel access realted register into AC parameter registers,
+	// even in nQBss.
+	//
+	ChnlAccessSetting->SIFS_Timer = 0x22; // Suggested by Jong, 2005.12.08.
+	ChnlAccessSetting->DIFS_Timer = 0x1C; // 2006.06.02, by rcnjko.
+	ChnlAccessSetting->SlotTimeTimer = 9; // 2006.06.02, by rcnjko.
+	ChnlAccessSetting->EIFS_Timer = 0x5B; // Suggested by wcchu, it is the default value of EIFS register, 2005.12.08.
+	ChnlAccessSetting->CWminIndex = 3; // 2006.06.02, by rcnjko.
+	ChnlAccessSetting->CWmaxIndex = 7; // 2006.06.02, by rcnjko.
+
+	write_nic_byte(dev, SIFS, ChnlAccessSetting->SIFS_Timer);
+	//Adapter->HalFunc.SetHwRegHandler( Adapter, HW_VAR_SLOT_TIME, &ChnlAccessSetting->SlotTimeTimer );	// Rewrited from directly use PlatformEFIOWrite1Byte(), by Annie, 2006-03-29.
+	write_nic_byte(dev, SLOT, ChnlAccessSetting->SlotTimeTimer);	// Rewrited from directly use PlatformEFIOWrite1Byte(), by Annie, 2006-03-29.
+
+	u1bAIFS = aSifsTime + (2 * ChnlAccessSetting->SlotTimeTimer );
+
+	//write_nic_byte(dev, AC_VO_PARAM, u1bAIFS);
+	//write_nic_byte(dev, AC_VI_PARAM, u1bAIFS);
+	//write_nic_byte(dev, AC_BE_PARAM, u1bAIFS);
+	//write_nic_byte(dev, AC_BK_PARAM, u1bAIFS);
+
+	write_nic_byte(dev, EIFS, ChnlAccessSetting->EIFS_Timer);
+
+	write_nic_byte(dev, AckTimeOutReg, 0x5B); // <RJ_EXPR_QOS> Suggested by wcchu, it is the default value of EIFS register, 2005.12.08.
+
+#ifdef TODO
+	// <RJ_TODO_NOW_8185B> Update ECWmin/ECWmax, AIFS, TXOP Limit of each AC to the value defined by SPEC.
+	if( pStaQos->CurrentQosMode > QOS_DISABLE )
+	{ // QoS mode.
+		if(pStaQos->QBssWirelessMode == WirelessMode)
+		{
+			// Follow AC Parameters of the QBSS.
+			for(eACI = 0; eACI < AC_MAX; eACI++)
+			{
+				Adapter->HalFunc.SetHwRegHandler(Adapter, HW_VAR_AC_PARAM, (pu1Byte)(&(pStaQos->WMMParamEle.AcParam[eACI])) );
+			}
+		}
+		else
+		{
+			// Follow Default WMM AC Parameters.
+			bFollowLegacySetting = 1;
+		}
+	}
+	else
+#endif
+	{ // Legacy 802.11.
+		bFollowLegacySetting = 1;
+
+	}
+
+	// this setting is copied from rtl8187B.  xiong-2006-11-13
+	if(bFollowLegacySetting)
+	{
+
+
+		//
+		// Follow 802.11 seeting to AC parameter, all AC shall use the same parameter.
+		// 2005.12.01, by rcnjko.
+		//
+		AcParam.longData = 0;
+		AcParam.f.AciAifsn.f.AIFSN = 2; // Follow 802.11 DIFS.
+		AcParam.f.AciAifsn.f.ACM = 0;
+		AcParam.f.Ecw.f.ECWmin = ChnlAccessSetting->CWminIndex; // Follow 802.11 CWmin.
+		AcParam.f.Ecw.f.ECWmax = ChnlAccessSetting->CWmaxIndex; // Follow 802.11 CWmax.
+		AcParam.f.TXOPLimit = 0;
+
+		//lzm reserved 080826
+#if 1
+#ifdef THOMAS_TURBO
+		// For turbo mode setting. port from 87B by Isaiah 2008-08-01
+		if( ieee->current_network.Turbo_Enable == 1 )
+			AcParam.f.TXOPLimit = 0x01FF;
+#endif
+		// For 87SE with Intel 4965  Ad-Hoc mode have poor throughput (19MB)
+		if (ieee->iw_mode == IW_MODE_ADHOC)
+			AcParam.f.TXOPLimit = 0x0020;
+#endif
+
+		for(eACI = 0; eACI < AC_MAX; eACI++)
+		{
+			AcParam.f.AciAifsn.f.ACI = (u8)eACI;
+			{
+				PAC_PARAM	pAcParam = (PAC_PARAM)(&AcParam);
+				AC_CODING	eACI;
+				u8		u1bAIFS;
+				u32		u4bAcParam;
+
+				// Retrive paramters to udpate.
+				eACI = pAcParam->f.AciAifsn.f.ACI;
+				u1bAIFS = pAcParam->f.AciAifsn.f.AIFSN * ChnlAccessSetting->SlotTimeTimer + aSifsTime;
+				u4bAcParam = (	(((u32)(pAcParam->f.TXOPLimit)) << AC_PARAM_TXOP_LIMIT_OFFSET)	|
+						(((u32)(pAcParam->f.Ecw.f.ECWmax)) << AC_PARAM_ECW_MAX_OFFSET)	|
+						(((u32)(pAcParam->f.Ecw.f.ECWmin)) << AC_PARAM_ECW_MIN_OFFSET)	|
+						(((u32)u1bAIFS) << AC_PARAM_AIFS_OFFSET));
+
+				switch(eACI)
+				{
+					case AC1_BK:
+						//write_nic_dword(dev, AC_BK_PARAM, u4bAcParam);
+						break;
+
+					case AC0_BE:
+						//write_nic_dword(dev, AC_BE_PARAM, u4bAcParam);
+						break;
+
+					case AC2_VI:
+						//write_nic_dword(dev, AC_VI_PARAM, u4bAcParam);
+						break;
+
+					case AC3_VO:
+						//write_nic_dword(dev, AC_VO_PARAM, u4bAcParam);
+						break;
+
+					default:
+						DMESGW( "SetHwReg8185(): invalid ACI: %d !\n", eACI);
+						break;
+				}
+
+				// Cehck ACM bit.
+				// If it is set, immediately set ACM control bit to downgrading AC for passing WMM testplan. Annie, 2005-12-13.
+				//write_nic_byte(dev, ACM_CONTROL, pAcParam->f.AciAifsn);
+				{
+					PACI_AIFSN	pAciAifsn = (PACI_AIFSN)(&pAcParam->f.AciAifsn);
+					AC_CODING	eACI = pAciAifsn->f.ACI;
+
+					//modified Joseph
+					//for 8187B AsynIORead issue
+#ifdef TODO
+					u8	AcmCtrl = pHalData->AcmControl;
+#else
+					u8	AcmCtrl = 0;
+#endif
+					if( pAciAifsn->f.ACM )
+					{ // ACM bit is 1.
+						switch(eACI)
+						{
+							case AC0_BE:
+								AcmCtrl |= (BEQ_ACM_EN|BEQ_ACM_CTL|ACM_HW_EN);	// or 0x21
+								break;
+
+							case AC2_VI:
+								AcmCtrl |= (VIQ_ACM_EN|VIQ_ACM_CTL|ACM_HW_EN);	// or 0x42
+								break;
+
+							case AC3_VO:
+								AcmCtrl |= (VOQ_ACM_EN|VOQ_ACM_CTL|ACM_HW_EN);	// or 0x84
+								break;
+
+							default:
+								DMESGW("SetHwReg8185(): [HW_VAR_ACM_CTRL] ACM set failed: eACI is %d\n", eACI );
+								break;
+						}
+					}
+					else
+					{ // ACM bit is 0.
+						switch(eACI)
+						{
+							case AC0_BE:
+								AcmCtrl &= ( (~BEQ_ACM_EN) & (~BEQ_ACM_CTL) & (~ACM_HW_EN) );	// and 0xDE
+								break;
+
+							case AC2_VI:
+								AcmCtrl &= ( (~VIQ_ACM_EN) & (~VIQ_ACM_CTL) & (~ACM_HW_EN) );	// and 0xBD
+								break;
+
+							case AC3_VO:
+								AcmCtrl &= ( (~VOQ_ACM_EN) & (~VOQ_ACM_CTL) & (~ACM_HW_EN) );	// and 0x7B
+								break;
+
+							default:
+								break;
+						}
+					}
+
+					//printk(KERN_WARNING "SetHwReg8185(): [HW_VAR_ACM_CTRL] Write 0x%X\n", AcmCtrl);
+
+#ifdef TO_DO
+					pHalData->AcmControl = AcmCtrl;
+#endif
+					//write_nic_byte(dev, ACM_CONTROL, AcmCtrl);
+					write_nic_byte(dev, ACM_CONTROL, 0);
+				}
+			}
+		}
+
+
+	}
+}
+
+void
+ActSetWirelessMode8185(
+	struct net_device *dev,
+	u8				btWirelessMode
+	)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	struct ieee80211_device *ieee = priv->ieee80211;
+	//PMGNT_INFO		pMgntInfo = &(Adapter->MgntInfo);
+	u8	btSupportedWirelessMode = GetSupportedWirelessMode8185(dev);
+
+	if( (btWirelessMode & btSupportedWirelessMode) == 0 )
+	{ // Don't switch to unsupported wireless mode, 2006.02.15, by rcnjko.
+		DMESGW("ActSetWirelessMode8185(): WirelessMode(%d) is not supported (%d)!\n",
+			btWirelessMode, btSupportedWirelessMode);
+		return;
+	}
+
+	// 1. Assign wireless mode to swtich if necessary.
+	if (btWirelessMode == WIRELESS_MODE_AUTO)
+	{
+		if((btSupportedWirelessMode & WIRELESS_MODE_A))
+		{
+			btWirelessMode = WIRELESS_MODE_A;
+		}
+		else if((btSupportedWirelessMode & WIRELESS_MODE_G))
+		{
+			btWirelessMode = WIRELESS_MODE_G;
+		}
+		else if((btSupportedWirelessMode & WIRELESS_MODE_B))
+		{
+			btWirelessMode = WIRELESS_MODE_B;
+		}
+		else
+		{
+			DMESGW("ActSetWirelessMode8185(): No valid wireless mode supported, btSupportedWirelessMode(%x)!!!\n",
+					 btSupportedWirelessMode);
+			btWirelessMode = WIRELESS_MODE_B;
+		}
+	}
+
+
+	// 2. Swtich band: RF or BB specific actions,
+	// for example, refresh tables in omc8255, or change initial gain if necessary.
+	switch(priv->rf_chip)
+	{
+	case RF_ZEBRA2:
+	case RF_ZEBRA4:
+		{
+			// Nothing to do for Zebra to switch band.
+			// Update current wireless mode if we swtich to specified band successfully.
+			ieee->mode = (WIRELESS_MODE)btWirelessMode;
+		}
+		break;
+
+	default:
+		DMESGW("ActSetWirelessMode8185(): unsupported RF: 0x%X !!!\n", priv->rf_chip);
+		break;
+	}
+
+	// 3. Change related setting.
+	if( ieee->mode == WIRELESS_MODE_A ){
+		DMESG("WIRELESS_MODE_A\n");
+	}
+	else if( ieee->mode == WIRELESS_MODE_B ){
+		DMESG("WIRELESS_MODE_B\n");
+	}
+	else if( ieee->mode == WIRELESS_MODE_G ){
+		DMESG("WIRELESS_MODE_G\n");
+	}
+
+	ActUpdateChannelAccessSetting( dev, ieee->mode, &priv->ChannelAccessSetting);
+}
+
+void rtl8185b_irq_enable(struct net_device *dev)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+	priv->irq_enabled = 1;
+	write_nic_dword(dev, IMR, priv->IntrMask);
+}
+//by amy for power save
+void
+DrvIFIndicateDisassociation(
+	struct net_device *dev,
+	u16			reason
+	)
+{
+	//printk("==> DrvIFIndicateDisassociation()\n");
+
+	// nothing is needed after disassociation request.
+
+	//printk("<== DrvIFIndicateDisassociation()\n");
+}
+void
+MgntDisconnectIBSS(
+	struct net_device *dev
+)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	u8			i;
+
+	//printk("XXXXXXXXXX MgntDisconnect IBSS\n");
+
+	DrvIFIndicateDisassociation(dev, unspec_reason);
+
+//	PlatformZeroMemory( pMgntInfo->Bssid, 6 );
+	for(i=0;i<6;i++)  priv->ieee80211->current_network.bssid[i] = 0x55;
+
+	priv->ieee80211->state = IEEE80211_NOLINK;
+
+	//Stop Beacon.
+
+	// Vista add a Adhoc profile, HW radio off untill OID_DOT11_RESET_REQUEST
+	// Driver would set MSR=NO_LINK, then HW Radio ON, MgntQueue Stuck.
+	// Because Bcn DMA isn't complete, mgnt queue would stuck until Bcn packet send.
+
+	// Disable Beacon Queue Own bit, suggested by jong
+//	Adapter->HalFunc.SetTxDescOWNHandler(Adapter, BEACON_QUEUE, 0, 0);
+	ieee80211_stop_send_beacons(priv->ieee80211);
+
+	priv->ieee80211->link_change(dev);
+	notify_wx_assoc_event(priv->ieee80211);
+
+	// Stop SW Beacon.Use hw beacon so do not need to do so.by amy
+#if 0
+	if(pMgntInfo->bEnableSwBeaconTimer)
+	{
+		// SwBeaconTimer will stop if pMgntInfo->mIbss==FALSE, see SwBeaconCallback() for details.
+// comment out by haich, 2007.10.01
+//#if DEV_BUS_TYPE==USB_INTERFACE
+		PlatformCancelTimer( Adapter, &pMgntInfo->SwBeaconTimer);
+//#endif
+	}
+#endif
+
+//		MgntIndicateMediaStatus( Adapter, RT_MEDIA_DISCONNECT, GENERAL_INDICATE );
+
+}
+void
+MlmeDisassociateRequest(
+	struct net_device *dev,
+	u8*			asSta,
+	u8			asRsn
+	)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	u8 i;
+
+	SendDisassociation(priv->ieee80211, asSta, asRsn );
+
+	if( memcmp(priv->ieee80211->current_network.bssid, asSta, 6 ) == 0 ){
+		//ShuChen TODO: change media status.
+		//ShuChen TODO: What to do when disassociate.
+		DrvIFIndicateDisassociation(dev, unspec_reason);
+
+
+	//	pMgntInfo->AsocTimestamp = 0;
+		for(i=0;i<6;i++)  priv->ieee80211->current_network.bssid[i] = 0x22;
+//		pMgntInfo->mBrates.Length = 0;
+//		Adapter->HalFunc.SetHwRegHandler( Adapter, HW_VAR_BASIC_RATE, (pu1Byte)(&pMgntInfo->mBrates) );
+
+		ieee80211_disassociate(priv->ieee80211);
+
+
+	}
+
+}
+
+void
+MgntDisconnectAP(
+	struct net_device *dev,
+	u8			asRsn
+)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+//
+// Commented out by rcnjko, 2005.01.27:
+// I move SecClearAllKeys() to MgntActSet_802_11_DISASSOCIATE().
+//
+//	//2004/09/15, kcwu, the key should be cleared, or the new handshaking will not success
+//	SecClearAllKeys(Adapter);
+
+	// In WPA WPA2 need to Clear all key ... because new key will set after new handshaking.
+#ifdef TODO
+	if(   pMgntInfo->SecurityInfo.AuthMode > RT_802_11AuthModeAutoSwitch ||
+		(pMgntInfo->bAPSuportCCKM && pMgntInfo->bCCX8021xenable) )  // In CCKM mode will Clear key
+	{
+		SecClearAllKeys(Adapter);
+		RT_TRACE(COMP_SEC, DBG_LOUD,("======>CCKM clear key..."))
+	}
+#endif
+	// 2004.10.11, by rcnjko.
+	//MlmeDisassociateRequest( Adapter, pMgntInfo->Bssid, disas_lv_ss );
+	MlmeDisassociateRequest( dev, priv->ieee80211->current_network.bssid, asRsn );
+
+	priv->ieee80211->state = IEEE80211_NOLINK;
+//	pMgntInfo->AsocTimestamp = 0;
+}
+bool
+MgntDisconnect(
+	struct net_device *dev,
+	u8			asRsn
+)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	//
+	// Schedule an workitem to wake up for ps mode, 070109, by rcnjko.
+	//
+#ifdef TODO
+	if(pMgntInfo->mPss != eAwake)
+	{
+		//
+		// Using AwkaeTimer to prevent mismatch ps state.
+		// In the timer the state will be changed according to the RF is being awoke or not. By Bruce, 2007-10-31.
+		//
+		// PlatformScheduleWorkItem( &(pMgntInfo->AwakeWorkItem) );
+		PlatformSetTimer( Adapter, &(pMgntInfo->AwakeTimer), 0 );
+	}
+#endif
+
+	// Indication of disassociation event.
+	//DrvIFIndicateDisassociation(Adapter, asRsn);
+#ifdef ENABLE_DOT11D
+	if(IS_DOT11D_ENABLE(priv->ieee80211))
+		Dot11d_Reset(priv->ieee80211);
+#endif
+	// In adhoc mode, update beacon frame.
+	if( priv->ieee80211->state == IEEE80211_LINKED )
+	{
+		if( priv->ieee80211->iw_mode == IW_MODE_ADHOC )
+		{
+//			RT_TRACE(COMP_MLME, DBG_LOUD, ("MgntDisconnect() ===> MgntDisconnectIBSS\n"));
+			//printk("MgntDisconnect() ===> MgntDisconnectIBSS\n");
+			MgntDisconnectIBSS(dev);
+		}
+		if( priv->ieee80211->iw_mode == IW_MODE_INFRA )
+		{
+			// We clear key here instead of MgntDisconnectAP() because that
+			// MgntActSet_802_11_DISASSOCIATE() is an interface called by OS,
+			// e.g. OID_802_11_DISASSOCIATE in Windows while as MgntDisconnectAP() is
+			// used to handle disassociation related things to AP, e.g. send Disassoc
+			// frame to AP.  2005.01.27, by rcnjko.
+//			SecClearAllKeys(Adapter);
+
+//			RT_TRACE(COMP_MLME, DBG_LOUD, ("MgntDisconnect() ===> MgntDisconnectAP\n"));
+			//printk("MgntDisconnect() ===> MgntDisconnectAP\n");
+			MgntDisconnectAP(dev, asRsn);
+		}
+
+		// Inidicate Disconnect, 2005.02.23, by rcnjko.
+//		MgntIndicateMediaStatus( Adapter, RT_MEDIA_DISCONNECT, GENERAL_INDICATE);
+	}
+
+	return true;
+}
+//
+//	Description:
+//		Chang RF Power State.
+//		Note that, only MgntActSet_RF_State() is allowed to set HW_VAR_RF_STATE.
+//
+//	Assumption:
+//		PASSIVE LEVEL.
+//
+bool
+SetRFPowerState(
+	struct net_device *dev,
+	RT_RF_POWER_STATE	eRFPowerState
+	)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	bool			bResult = false;
+
+//	printk("---------> SetRFPowerState(): eRFPowerState(%d)\n", eRFPowerState);
+	if(eRFPowerState == priv->eRFPowerState)
+	{
+//		printk("<--------- SetRFPowerState(): discard the request for eRFPowerState(%d) is the same.\n", eRFPowerState);
+		return bResult;
+	}
+
+	switch(priv->rf_chip)
+	{
+		case RF_ZEBRA2:
+		case RF_ZEBRA4:
+			 bResult = SetZebraRFPowerState8185(dev, eRFPowerState);
+			break;
+
+		default:
+			printk("SetRFPowerState8185(): unknown RFChipID: 0x%X!!!\n", priv->rf_chip);
+			break;;
+}
+//	printk("<--------- SetRFPowerState(): bResult(%d)\n", bResult);
+
+	return bResult;
+}
+void
+HalEnableRx8185Dummy(
+	struct net_device *dev
+	)
+{
+}
+void
+HalDisableRx8185Dummy(
+	struct net_device *dev
+	)
+{
+}
+
+bool
+MgntActSet_RF_State(
+	struct net_device *dev,
+	RT_RF_POWER_STATE	StateToSet,
+	u32	ChangeSource
+	)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	bool				bActionAllowed = false;
+	bool				bConnectBySSID = false;
+	RT_RF_POWER_STATE 	rtState;
+	u16				RFWaitCounter = 0;
+	unsigned long flag;
+//	 printk("===>MgntActSet_RF_State(): StateToSet(%d), ChangeSource(0x%x)\n",StateToSet, ChangeSource);
+	//
+	// Prevent the race condition of RF state change. By Bruce, 2007-11-28.
+	// Only one thread can change the RF state at one time, and others should wait to be executed.
+	//
+#if 1
+	while(true)
+	{
+//		down(&priv->rf_state);
+		spin_lock_irqsave(&priv->rf_ps_lock,flag);
+		if(priv->RFChangeInProgress)
+		{
+//			printk("====================>haha111111111\n");
+//			up(&priv->rf_state);
+//			RT_TRACE(COMP_RF, DBG_LOUD, ("MgntActSet_RF_State(): RF Change in progress! Wait to set..StateToSet(%d).\n", StateToSet));
+			spin_unlock_irqrestore(&priv->rf_ps_lock,flag);
+			// Set RF after the previous action is done.
+			while(priv->RFChangeInProgress)
+			{
+				RFWaitCounter ++;
+//				RT_TRACE(COMP_RF, DBG_LOUD, ("MgntActSet_RF_State(): Wait 1 ms (%d times)...\n", RFWaitCounter));
+				udelay(1000); // 1 ms
+
+				// Wait too long, return FALSE to avoid to be stuck here.
+				if(RFWaitCounter > 1000) // 1sec
+				{
+//					RT_ASSERT(FALSE, ("MgntActSet_RF_State(): Wait too logn to set RF\n"));
+					printk("MgntActSet_RF_State(): Wait too long to set RF\n");
+					// TODO: Reset RF state?
+					return false;
+				}
+			}
+		}
+		else
+		{
+//			printk("========================>haha2\n");
+			priv->RFChangeInProgress = true;
+//			up(&priv->rf_state);
+			spin_unlock_irqrestore(&priv->rf_ps_lock,flag);
+			break;
+		}
+	}
+#endif
+	rtState = priv->eRFPowerState;
+
+
+	switch(StateToSet)
+	{
+	case eRfOn:
+		//
+		// Turn On RF no matter the IPS setting because we need to update the RF state to Ndis under Vista, or
+		// the Windows does not allow the driver to perform site survey any more. By Bruce, 2007-10-02.
+		//
+		priv->RfOffReason &= (~ChangeSource);
+
+		if(! priv->RfOffReason)
+		{
+			priv->RfOffReason = 0;
+			bActionAllowed = true;
+
+			if(rtState == eRfOff && ChangeSource >=RF_CHANGE_BY_HW && !priv->bInHctTest)
+			{
+				bConnectBySSID = true;
+			}
+		}
+		else
+//			RT_TRACE(COMP_RF, DBG_LOUD, ("MgntActSet_RF_State - eRfon reject pMgntInfo->RfOffReason= 0x%x, ChangeSource=0x%X\n", pMgntInfo->RfOffReason, ChangeSource));
+			;
+		break;
+
+	case eRfOff:
+		 // 070125, rcnjko: we always keep connected in AP mode.
+
+			if (priv->RfOffReason > RF_CHANGE_BY_IPS)
+			{
+				//
+				// 060808, Annie:
+				// Disconnect to current BSS when radio off. Asked by QuanTa.
+				//
+
+				//
+				// Calling MgntDisconnect() instead of MgntActSet_802_11_DISASSOCIATE(),
+				// because we do NOT need to set ssid to dummy ones.
+				// Revised by Roger, 2007.12.04.
+				//
+				MgntDisconnect( dev, disas_lv_ss );
+
+				// Clear content of bssDesc[] and bssDesc4Query[] to avoid reporting old bss to UI.
+				// 2007.05.28, by shien chang.
+//				PlatformZeroMemory( pMgntInfo->bssDesc, sizeof(RT_WLAN_BSS)*MAX_BSS_DESC );
+//				pMgntInfo->NumBssDesc = 0;
+//				PlatformZeroMemory( pMgntInfo->bssDesc4Query, sizeof(RT_WLAN_BSS)*MAX_BSS_DESC );
+//				pMgntInfo->NumBssDesc4Query = 0;
+			}
+
+
+
+		priv->RfOffReason |= ChangeSource;
+		bActionAllowed = true;
+		break;
+
+	case eRfSleep:
+		priv->RfOffReason |= ChangeSource;
+		bActionAllowed = true;
+		break;
+
+	default:
+		break;
+	}
+
+	if(bActionAllowed)
+	{
+//		RT_TRACE(COMP_RF, DBG_LOUD, ("MgntActSet_RF_State(): Action is allowed.... StateToSet(%d), RfOffReason(%#X)\n", StateToSet, pMgntInfo->RfOffReason));
+                // Config HW to the specified mode.
+//		printk("MgntActSet_RF_State(): Action is allowed.... StateToSet(%d), RfOffReason(%#X)\n", StateToSet, priv->RfOffReason);
+		SetRFPowerState(dev, StateToSet);
+
+		// Turn on RF.
+		if(StateToSet == eRfOn)
+		{
+			HalEnableRx8185Dummy(dev);
+			if(bConnectBySSID)
+			{
+			// by amy not supported
+//				MgntActSet_802_11_SSID(Adapter, Adapter->MgntInfo.Ssid.Octet, Adapter->MgntInfo.Ssid.Length, TRUE );
+			}
+		}
+		// Turn off RF.
+		else if(StateToSet == eRfOff)
+		{
+			HalDisableRx8185Dummy(dev);
+		}
+	}
+	else
+	{
+	//	printk("MgntActSet_RF_State(): Action is rejected.... StateToSet(%d), ChangeSource(%#X), RfOffReason(%#X)\n", StateToSet, ChangeSource, priv->RfOffReason);
+	}
+
+	// Release RF spinlock
+//	down(&priv->rf_state);
+	spin_lock_irqsave(&priv->rf_ps_lock,flag);
+	priv->RFChangeInProgress = false;
+//	up(&priv->rf_state);
+	spin_unlock_irqrestore(&priv->rf_ps_lock,flag);
+//	printk("<===MgntActSet_RF_State()\n");
+	return bActionAllowed;
+}
+void
+InactivePowerSave(
+	struct net_device *dev
+	)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	//u8 index = 0;
+
+	//
+	// This flag "bSwRfProcessing", indicates the status of IPS procedure, should be set if the IPS workitem
+	// is really scheduled.
+	// The old code, sets this flag before scheduling the IPS workitem and however, at the same time the
+	// previous IPS workitem did not end yet, fails to schedule the current workitem. Thus, bSwRfProcessing
+	// blocks the IPS procedure of switching RF.
+	// By Bruce, 2007-12-25.
+	//
+	priv->bSwRfProcessing = true;
+
+	MgntActSet_RF_State(dev, priv->eInactivePowerState, RF_CHANGE_BY_IPS);
+
+	//
+	// To solve CAM values miss in RF OFF, rewrite CAM values after RF ON. By Bruce, 2007-09-20.
+	//
+#if 0
+	while( index < 4 )
+	{
+		if( ( pMgntInfo->SecurityInfo.PairwiseEncAlgorithm == WEP104_Encryption ) ||
+			(pMgntInfo->SecurityInfo.PairwiseEncAlgorithm == WEP40_Encryption) )
+		{
+			if( pMgntInfo->SecurityInfo.KeyLen[index] != 0)
+			pAdapter->HalFunc.SetKeyHandler(pAdapter, index, 0, FALSE, pMgntInfo->SecurityInfo.PairwiseEncAlgorithm, TRUE, FALSE);
+
+		}
+		index++;
+	}
+#endif
+	priv->bSwRfProcessing = false;
+}
+
+//
+//	Description:
+//		Enter the inactive power save mode. RF will be off
+//	2007.08.17, by shien chang.
+//
+void
+IPSEnter(
+	struct net_device *dev
+	)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	RT_RF_POWER_STATE rtState;
+	//printk("==============================>enter IPS\n");
+	if (priv->bInactivePs)
+	{
+		rtState = priv->eRFPowerState;
+
+		//
+		// Added by Bruce, 2007-12-25.
+		// Do not enter IPS in the following conditions:
+		// (1) RF is already OFF or Sleep
+		// (2) bSwRfProcessing (indicates the IPS is still under going)
+		// (3) Connectted (only disconnected can trigger IPS)
+		// (4) IBSS (send Beacon)
+		// (5) AP mode (send Beacon)
+		//
+		if (rtState == eRfOn && !priv->bSwRfProcessing
+			&& (priv->ieee80211->state != IEEE80211_LINKED ))
+		{
+	//		printk("IPSEnter(): Turn off RF.\n");
+			priv->eInactivePowerState = eRfOff;
+			InactivePowerSave(dev);
+		}
+	}
+//	printk("priv->eRFPowerState is %d\n",priv->eRFPowerState);
+}
+void
+IPSLeave(
+	struct net_device *dev
+	)
+{
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+	RT_RF_POWER_STATE rtState;
+	//printk("===================================>leave IPS\n");
+	if (priv->bInactivePs)
+	{
+		rtState = priv->eRFPowerState;
+		if ((rtState == eRfOff || rtState == eRfSleep) && (!priv->bSwRfProcessing) && priv->RfOffReason <= RF_CHANGE_BY_IPS)
+		{
+//			printk("IPSLeave(): Turn on RF.\n");
+			priv->eInactivePowerState = eRfOn;
+			InactivePowerSave(dev);
+		}
+	}
+//	printk("priv->eRFPowerState is %d\n",priv->eRFPowerState);
+}
+//by amy for power save
+void rtl8185b_adapter_start(struct net_device *dev)
+{
+      struct r8180_priv *priv = ieee80211_priv(dev);
+	struct ieee80211_device *ieee = priv->ieee80211;
+
+	u8 SupportedWirelessMode;
+	u8			InitWirelessMode;
+	u8			bInvalidWirelessMode = 0;
+	//int i;
+	u8 tmpu8;
+    	//u8 u1tmp,u2tmp;
+	u8 btCR9346;
+	u8 TmpU1b;
+	u8 btPSR;
+
+	//rtl8180_rtx_disable(dev);
+//{by amy 080312
+	write_nic_byte(dev,0x24e, (BIT5|BIT6|BIT0));
+//by amy 080312}
+	rtl8180_reset(dev);
+
+	priv->dma_poll_mask = 0;
+	priv->dma_poll_stop_mask = 0;
+
+	//rtl8180_beacon_tx_disable(dev);
+
+	HwConfigureRTL8185(dev);
+
+	write_nic_dword(dev, MAC0, ((u32*)dev->dev_addr)[0]);
+	write_nic_word(dev, MAC4, ((u32*)dev->dev_addr)[1] & 0xffff );
+
+	write_nic_byte(dev, MSR, read_nic_byte(dev, MSR) & 0xf3);	// default network type to 'No	Link'
+
+	//write_nic_byte(dev, BRSR, 0x0);		// Set BRSR= 1M
+
+	write_nic_word(dev, BcnItv, 100);
+	write_nic_word(dev, AtimWnd, 2);
+
+	//PlatformEFIOWrite2Byte(dev, FEMR, 0xFFFF);
+	PlatformIOWrite2Byte(dev, FEMR, 0xFFFF);
+
+	write_nic_byte(dev, WPA_CONFIG, 0);
+
+	MacConfig_85BASIC(dev);
+
+	// Override the RFSW_CTRL (MAC offset 0x272-0x273), 2006.06.07, by rcnjko.
+	// BT_DEMO_BOARD type
+	PlatformIOWrite2Byte(dev, RFSW_CTRL, 0x569a);
+//by amy
+//#ifdef CONFIG_RTL818X_S
+		// for jong required
+//	PlatformIOWrite2Byte(dev, RFSW_CTRL, 0x9a56);
+//#endif
+//by amy
+	//BT_QA_BOARD
+	//PlatformIOWrite2Byte(dev, RFSW_CTRL, 0x9a56);
+
+	//-----------------------------------------------------------------------------
+	// Set up PHY related.
+	//-----------------------------------------------------------------------------
+	// Enable Config3.PARAM_En to revise AnaaParm.
+	write_nic_byte(dev, CR9346, 0xc0);	// enable config register write
+//by amy
+	tmpu8 = read_nic_byte(dev, CONFIG3);
+#ifdef CONFIG_RTL818X_S
+	write_nic_byte(dev, CONFIG3, (tmpu8 |CONFIG3_PARM_En) );
+#else
+	write_nic_byte(dev, CONFIG3, (tmpu8 |CONFIG3_PARM_En | CONFIG3_CLKRUN_En) );
+#endif
+//by amy
+	// Turn on Analog power.
+	// Asked for by William, otherwise, MAC 3-wire can't work, 2006.06.27, by rcnjko.
+	write_nic_dword(dev, ANAPARAM2, ANAPARM2_ASIC_ON);
+	write_nic_dword(dev, ANAPARAM, ANAPARM_ASIC_ON);
+//by amy
+#ifdef CONFIG_RTL818X_S
+	write_nic_word(dev, ANAPARAM3, 0x0010);
+#else
+      write_nic_byte(dev, ANAPARAM3, 0x00);
+#endif
+//by amy
+
+	write_nic_byte(dev, CONFIG3, tmpu8);
+	write_nic_byte(dev, CR9346, 0x00);
+//{by amy 080312 for led
+	// enable EEM0 and EEM1 in 9346CR
+	btCR9346 = read_nic_byte(dev, CR9346);
+	write_nic_byte(dev, CR9346, (btCR9346|0xC0) );
+
+	// B cut use LED1 to control HW RF on/off
+	TmpU1b = read_nic_byte(dev, CONFIG5);
+	TmpU1b = TmpU1b & ~BIT3;
+	write_nic_byte(dev,CONFIG5, TmpU1b);
+
+	// disable EEM0 and EEM1 in 9346CR
+	btCR9346 &= ~(0xC0);
+	write_nic_byte(dev, CR9346, btCR9346);
+
+	//Enable Led (suggested by Jong)
+	// B-cut RF Radio on/off  5e[3]=0
+	btPSR = read_nic_byte(dev, PSR);
+	write_nic_byte(dev, PSR, (btPSR | BIT3));
+//by amy 080312 for led}
+	// setup initial timing for RFE.
+	write_nic_word(dev, RFPinsOutput, 0x0480);
+	SetOutputEnableOfRfPins(dev);
+	write_nic_word(dev, RFPinsSelect, 0x2488);
+
+	// PHY config.
+	PhyConfig8185(dev);
+
+	// We assume RegWirelessMode has already been initialized before,
+	// however, we has to validate the wireless mode here and provide a reasonble
+	// initialized value if necessary. 2005.01.13, by rcnjko.
+	SupportedWirelessMode = GetSupportedWirelessMode8185(dev);
+	if(	(ieee->mode != WIRELESS_MODE_B) &&
+		(ieee->mode != WIRELESS_MODE_G) &&
+		(ieee->mode != WIRELESS_MODE_A) &&
+		(ieee->mode != WIRELESS_MODE_AUTO))
+	{ // It should be one of B, G, A, or AUTO.
+		bInvalidWirelessMode = 1;
+	}
+	else
+	{ // One of B, G, A, or AUTO.
+		// Check if the wireless mode is supported by RF.
+		if( (ieee->mode != WIRELESS_MODE_AUTO) &&
+			(ieee->mode & SupportedWirelessMode) == 0 )
+		{
+			bInvalidWirelessMode = 1;
+		}
+	}
+
+	if(bInvalidWirelessMode || ieee->mode==WIRELESS_MODE_AUTO)
+	{ // Auto or other invalid value.
+		// Assigne a wireless mode to initialize.
+		if((SupportedWirelessMode & WIRELESS_MODE_A))
+		{
+			InitWirelessMode = WIRELESS_MODE_A;
+		}
+		else if((SupportedWirelessMode & WIRELESS_MODE_G))
+		{
+			InitWirelessMode = WIRELESS_MODE_G;
+		}
+		else if((SupportedWirelessMode & WIRELESS_MODE_B))
+		{
+			InitWirelessMode = WIRELESS_MODE_B;
+		}
+		else
+		{
+			DMESGW("InitializeAdapter8185(): No valid wireless mode supported, SupportedWirelessMode(%x)!!!\n",
+				 SupportedWirelessMode);
+			InitWirelessMode = WIRELESS_MODE_B;
+		}
+
+		// Initialize RegWirelessMode if it is not a valid one.
+		if(bInvalidWirelessMode)
+		{
+			ieee->mode = (WIRELESS_MODE)InitWirelessMode;
+		}
+	}
+	else
+	{ // One of B, G, A.
+		InitWirelessMode = ieee->mode;
+	}
+//by amy for power save
+#ifdef ENABLE_IPS
+//	printk("initialize ENABLE_IPS\n");
+	priv->eRFPowerState = eRfOff;
+	priv->RfOffReason = 0;
+	{
+	//	u32 tmp2;
+	//	u32 tmp = jiffies;
+		MgntActSet_RF_State(dev, eRfOn, 0);
+	//	tmp2 = jiffies;
+	//	printk("rf on cost jiffies:%lx\n", (tmp2-tmp)*1000/HZ);
+	}
+//	DrvIFIndicateCurrentPhyStatus(priv);
+		//
+		// If inactive power mode is enabled, disable rf while in disconnected state.
+		// 2007.07.16, by shien chang.
+		//
+	if (priv->bInactivePs)
+	{
+	//	u32 tmp2;
+	//	u32 tmp = jiffies;
+		MgntActSet_RF_State(dev,eRfOff, RF_CHANGE_BY_IPS);
+	//	tmp2 = jiffies;
+	//	printk("rf off cost jiffies:%lx\n", (tmp2-tmp)*1000/HZ);
+
+	}
+#endif
+//	IPSEnter(dev);
+//by amy for power save
+#ifdef TODO
+	// Turn off RF if necessary. 2005.08.23, by rcnjko.
+	// We shall turn off RF after setting CMDR, otherwise,
+	// RF will be turnned on after we enable MAC Tx/Rx.
+	if(Adapter->MgntInfo.RegRfOff == TRUE)
+	{
+		SetRFPowerState8185(Adapter, RF_OFF);
+	}
+	else
+	{
+		SetRFPowerState8185(Adapter, RF_ON);
+	}
+#endif
+
+/*   //these is equal with above TODO.
+	write_nic_byte(dev, CR9346, 0xc0);	// enable config register write
+	write_nic_byte(dev, CONFIG3, read_nic_byte(dev, CONFIG3) | CONFIG3_PARM_En);
+	RF_WriteReg(dev, 0x4, 0x9FF);
+	write_nic_dword(dev, ANAPARAM2, ANAPARM2_ASIC_ON);
+	write_nic_dword(dev, ANAPARAM, ANAPARM_ASIC_ON);
+	write_nic_byte(dev, CONFIG3, (read_nic_byte(dev, CONFIG3)&(~CONFIG3_PARM_En)));
+	write_nic_byte(dev, CR9346, 0x00);
+*/
+
+	ActSetWirelessMode8185(dev, (u8)(InitWirelessMode));
+
+	//-----------------------------------------------------------------------------
+
+	rtl8185b_irq_enable(dev);
+
+	netif_start_queue(dev);
+
+ }
+
+
+void rtl8185b_rx_enable(struct net_device *dev)
+{
+	u8 cmd;
+	//u32 rxconf;
+	/* for now we accept data, management & ctl frame*/
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+#if 0
+	rxconf=read_nic_dword(dev,RX_CONF);
+	rxconf = rxconf &~ MAC_FILTER_MASK;
+	rxconf = rxconf | (1<<ACCEPT_MNG_FRAME_SHIFT);
+	rxconf = rxconf | (1<<ACCEPT_DATA_FRAME_SHIFT);
+	rxconf = rxconf | (1<<ACCEPT_BCAST_FRAME_SHIFT);
+	rxconf = rxconf | (1<<ACCEPT_MCAST_FRAME_SHIFT);
+//	rxconf = rxconf | (1<<ACCEPT_CRCERR_FRAME_SHIFT);
+	if (dev->flags & IFF_PROMISC) DMESG ("NIC in promisc mode");
+
+	if(priv->ieee80211->iw_mode == IW_MODE_MONITOR || \
+	   dev->flags & IFF_PROMISC){
+		rxconf = rxconf | (1<<ACCEPT_ALLMAC_FRAME_SHIFT);
+	}else{
+		rxconf = rxconf | (1<<ACCEPT_NICMAC_FRAME_SHIFT);
+		if(priv->card_8185 == 0)
+			rxconf = rxconf | (1<<RX_CHECK_BSSID_SHIFT);
+	}
+
+	/*if(priv->ieee80211->iw_mode == IW_MODE_MASTER){
+		rxconf = rxconf | (1<<ACCEPT_ALLMAC_FRAME_SHIFT);
+		rxconf = rxconf | (1<<RX_CHECK_BSSID_SHIFT);
+	}*/
+
+	if(priv->ieee80211->iw_mode == IW_MODE_MONITOR){
+		rxconf = rxconf | (1<<ACCEPT_CTL_FRAME_SHIFT);
+		rxconf = rxconf | (1<<ACCEPT_ICVERR_FRAME_SHIFT);
+		rxconf = rxconf | (1<<ACCEPT_PWR_FRAME_SHIFT);
+	}
+
+	if( priv->crcmon == 1 && priv->ieee80211->iw_mode == IW_MODE_MONITOR)
+		rxconf = rxconf | (1<<ACCEPT_CRCERR_FRAME_SHIFT);
+
+	//if(!priv->card_8185){
+		rxconf = rxconf &~ RX_FIFO_THRESHOLD_MASK;
+		rxconf = rxconf | (RX_FIFO_THRESHOLD_NONE<<RX_FIFO_THRESHOLD_SHIFT);
+	//}
+
+	rxconf = rxconf | (1<<RX_AUTORESETPHY_SHIFT);
+	rxconf = rxconf &~ MAX_RX_DMA_MASK;
+	rxconf = rxconf | (MAX_RX_DMA_2048<<MAX_RX_DMA_SHIFT);
+
+	//if(!priv->card_8185)
+		rxconf = rxconf | RCR_ONLYERLPKT;
+
+	rxconf = rxconf &~ RCR_CS_MASK;
+	if(!priv->card_8185)
+		rxconf |= (priv->rcr_csense<<RCR_CS_SHIFT);
+//	rxconf &=~ 0xfff00000;
+//	rxconf |= 0x90100000;//9014f76f;
+	write_nic_dword(dev, RX_CONF, rxconf);
+#endif
+
+	if (dev->flags & IFF_PROMISC) DMESG ("NIC in promisc mode");
+
+	if(priv->ieee80211->iw_mode == IW_MODE_MONITOR || \
+	   dev->flags & IFF_PROMISC){
+	   	priv->ReceiveConfig = priv->ReceiveConfig & (~RCR_APM);
+		priv->ReceiveConfig = priv->ReceiveConfig | RCR_AAP;
+	}
+
+	/*if(priv->ieee80211->iw_mode == IW_MODE_MASTER){
+		rxconf = rxconf | (1<<ACCEPT_ALLMAC_FRAME_SHIFT);
+		rxconf = rxconf | (1<<RX_CHECK_BSSID_SHIFT);
+	}*/
+
+	if(priv->ieee80211->iw_mode == IW_MODE_MONITOR){
+		priv->ReceiveConfig = priv->ReceiveConfig | RCR_ACF | RCR_APWRMGT | RCR_AICV;
+	}
+
+	if( priv->crcmon == 1 && priv->ieee80211->iw_mode == IW_MODE_MONITOR)
+		priv->ReceiveConfig = priv->ReceiveConfig | RCR_ACRC32;
+
+	write_nic_dword(dev, RCR, priv->ReceiveConfig);
+
+	fix_rx_fifo(dev);
+
+#ifdef DEBUG_RX
+	DMESG("rxconf: %x %x",priv->ReceiveConfig ,read_nic_dword(dev,RCR));
+#endif
+	cmd=read_nic_byte(dev,CMD);
+	write_nic_byte(dev,CMD,cmd | (1<<CMD_RX_ENABLE_SHIFT));
+
+}
+
+void rtl8185b_tx_enable(struct net_device *dev)
+{
+	u8 cmd;
+	//u8 tx_agc_ctl;
+	u8 byte;
+	//u32 txconf;
+	struct r8180_priv *priv = (struct r8180_priv *)ieee80211_priv(dev);
+
+#if 0
+	txconf= read_nic_dword(dev,TX_CONF);
+	if(priv->card_8185){
+
+
+		byte = read_nic_byte(dev,CW_CONF);
+		byte &= ~(1<<CW_CONF_PERPACKET_CW_SHIFT);
+		byte &= ~(1<<CW_CONF_PERPACKET_RETRY_SHIFT);
+		write_nic_byte(dev, CW_CONF, byte);
+
+		tx_agc_ctl = read_nic_byte(dev, TX_AGC_CTL);
+		tx_agc_ctl &= ~(1<<TX_AGC_CTL_PERPACKET_GAIN_SHIFT);
+		tx_agc_ctl &= ~(1<<TX_AGC_CTL_PERPACKET_ANTSEL_SHIFT);
+		tx_agc_ctl |=(1<<TX_AGC_CTL_FEEDBACK_ANT);
+		write_nic_byte(dev, TX_AGC_CTL, tx_agc_ctl);
+		/*
+		write_nic_word(dev, 0x5e, 0x01);
+		force_pci_posting(dev);
+		mdelay(1);
+		write_nic_word(dev, 0xfe, 0x10);
+		force_pci_posting(dev);
+		mdelay(1);
+		write_nic_word(dev, 0x5e, 0x00);
+		force_pci_posting(dev);
+		mdelay(1);
+		*/
+		write_nic_byte(dev, 0xec, 0x3f); /* Disable early TX */
+	}
+
+	if(priv->card_8185){
+
+		txconf = txconf &~ (1<<TCR_PROBE_NOTIMESTAMP_SHIFT);
+
+	}else{
+
+		if(hwseqnum)
+			txconf= txconf &~ (1<<TX_CONF_HEADER_AUTOICREMENT_SHIFT);
+		else
+			txconf= txconf | (1<<TX_CONF_HEADER_AUTOICREMENT_SHIFT);
+	}
+
+	txconf = txconf &~ TX_LOOPBACK_MASK;
+	txconf = txconf | (TX_LOOPBACK_NONE <<TX_LOOPBACK_SHIFT);
+	txconf = txconf &~ TCR_DPRETRY_MASK;
+	txconf = txconf &~ TCR_RTSRETRY_MASK;
+	txconf = txconf | (priv->retry_data<<TX_DPRETRY_SHIFT);
+	txconf = txconf | (priv->retry_rts<<TX_RTSRETRY_SHIFT);
+	txconf = txconf &~ (1<<TX_NOCRC_SHIFT);
+
+	if(priv->card_8185){
+		if(priv->hw_plcp_len)
+			txconf = txconf &~ TCR_PLCP_LEN;
+		else
+			txconf = txconf | TCR_PLCP_LEN;
+	}else{
+		txconf = txconf &~ TCR_SAT;
+	}
+	txconf = txconf &~ TCR_MXDMA_MASK;
+	txconf = txconf | (TCR_MXDMA_2048<<TCR_MXDMA_SHIFT);
+	txconf = txconf | TCR_CWMIN;
+	txconf = txconf | TCR_DISCW;
+
+//	if(priv->ieee80211->hw_wep)
+//		txconf=txconf &~ (1<<TX_NOICV_SHIFT);
+//	else
+		txconf=txconf | (1<<TX_NOICV_SHIFT);
+
+	write_nic_dword(dev,TX_CONF,txconf);
+#endif
+
+	write_nic_dword(dev, TCR, priv->TransmitConfig);
+	byte = read_nic_byte(dev, MSR);
+	byte |= MSR_LINK_ENEDCA;
+	write_nic_byte(dev, MSR, byte);
+
+	fix_tx_fifo(dev);
+
+#ifdef DEBUG_TX
+	DMESG("txconf: %x %x",priv->TransmitConfig,read_nic_dword(dev,TCR));
+#endif
+
+	cmd=read_nic_byte(dev,CMD);
+	write_nic_byte(dev,CMD,cmd | (1<<CMD_TX_ENABLE_SHIFT));
+
+	//write_nic_dword(dev,TX_CONF,txconf);
+
+
+/*
+	rtl8180_set_mode(dev,EPROM_CMD_CONFIG);
+	write_nic_byte(dev, TX_DMA_POLLING, priv->dma_poll_mask);
+	rtl8180_set_mode(dev,EPROM_CMD_NORMAL);
+	*/
+}
+
+
+#endif
diff --git a/drivers/staging/slicoss/slic.h b/drivers/staging/slicoss/slic.h
index 0d5dc24c0b7..a8ea59d7ea1 100644
--- a/drivers/staging/slicoss/slic.h
+++ b/drivers/staging/slicoss/slic.h
@@ -41,6 +41,40 @@
 #ifndef __SLIC_DRIVER_H__
 #define __SLIC_DRIVER_H__
 
+/* firmware stuff */
+#define OASIS_UCODE_VERS_STRING	"1.2"
+#define OASIS_UCODE_VERS_DATE  	"2006/03/27 15:10:37"
+#define OASIS_UCODE_HOSTIF_ID  	3
+
+static s32 ONumSections = 0x2;
+static u32 OSectionSize[] = {
+	0x00004000, 0x00010000,
+};
+
+static u32 OSectionStart[] = {
+	0x00000000, 0x00008000,
+};
+
+#define MOJAVE_UCODE_VERS_STRING	"1.2"
+#define MOJAVE_UCODE_VERS_DATE  	"2006/03/27 15:12:22"
+#define MOJAVE_UCODE_HOSTIF_ID  	3
+
+static s32 MNumSections = 0x2;
+static u32 MSectionSize[] =
+{
+	0x00008000, 0x00010000,
+};
+
+static u32 MSectionStart[] =
+{
+	0x00000000, 0x00008000,
+};
+
+#define GB_RCVUCODE_VERS_STRING	"1.2"
+#define GB_RCVUCODE_VERS_DATE  	"2006/03/27 15:12:15"
+static u32 OasisRcvUCodeLen = 512;
+static u32 GBRcvUCodeLen = 512;
+#define SECTION_SIZE 65536
 
 struct slic_spinlock {
 	spinlock_t	lock;
diff --git a/drivers/staging/slicoss/slicoss.c b/drivers/staging/slicoss/slicoss.c
index 00390362f10..bf7da8f898a 100644
--- a/drivers/staging/slicoss/slicoss.c
+++ b/drivers/staging/slicoss/slicoss.c
@@ -94,6 +94,7 @@
 #include <linux/module.h>
 #include <linux/moduleparam.h>
 
+#include <linux/firmware.h>
 #include <linux/types.h>
 #include <linux/dma-mapping.h>
 #include <linux/mii.h>
@@ -105,15 +106,6 @@
 
 #include <linux/uaccess.h>
 #include "slicinc.h"
-#include "gbdownload.h"
-#include "gbrcvucode.h"
-#include "oasisrcvucode.h"
-
-#ifdef DEBUG_MICROCODE
-#include "oasisdbgdownload.h"
-#else
-#include "oasisdownload.h"
-#endif
 
 #if SLIC_DUMP_ENABLED
 #include "slicdump.h"
@@ -323,7 +315,7 @@ static void slic_init_adapter(struct net_device *netdev,
 	index, pslic_handle, adapter->pfree_slic_handles, pslic_handle->next);*/
 	adapter->pshmem = (struct slic_shmem *)
 					pci_alloc_consistent(adapter->pcidev,
-					sizeof(struct slic_shmem *),
+					sizeof(struct slic_shmem),
 					&adapter->
 					phys_shmem);
 /*
@@ -1431,7 +1423,7 @@ static void slic_init_cleanup(struct adapter *adapter)
 		DBG_MSG("adapter[%p] port %d pshmem[%p] FreeShmem ",
 			adapter, adapter->port, (void *) adapter->pshmem);
 		pci_free_consistent(adapter->pcidev,
-				    sizeof(struct slic_shmem *),
+				    sizeof(struct slic_shmem),
 				    adapter->pshmem, adapter->phys_shmem);
 		adapter->pshmem = NULL;
 		adapter->phys_shmem = (dma_addr_t) NULL;
@@ -2186,6 +2178,9 @@ static void slic_card_cleanup(struct sliccard *card)
 
 static int slic_card_download_gbrcv(struct adapter *adapter)
 {
+	const struct firmware *fw;
+	const char *file = "";
+	int ret;
 	__iomem struct slic_regs *slic_regs = adapter->slic_regs;
 	u32 codeaddr;
 	unsigned char *instruction = NULL;
@@ -2193,12 +2188,32 @@ static int slic_card_download_gbrcv(struct adapter *adapter)
 
 	switch (adapter->devid) {
 	case SLIC_2GB_DEVICE_ID:
-		instruction = (unsigned char *)&OasisRcvUCode[0];
-		rcvucodelen = OasisRcvUCodeLen;
+		file = "oasis_rcv.bin";
 		break;
 	case SLIC_1GB_DEVICE_ID:
-		instruction = (unsigned char *)&GBRcvUCode[0];
-		rcvucodelen = GBRcvUCodeLen;
+		file = "gb_rcv.bin";
+		break;
+	default:
+		ASSERT(0);
+		break;
+	}
+
+	ret = request_firmware(&fw, file, &adapter->pcidev->dev);
+	if (ret) {
+		printk(KERN_ERR "SLICOSS: Failed to load firmware %s\n", file);
+		return ret;
+	}
+
+	instruction = (unsigned char *)fw->data;
+	rcvucodelen = fw->size;
+	switch (adapter->devid) {
+	case SLIC_2GB_DEVICE_ID:
+		if (rcvucodelen != OasisRcvUCodeLen)
+			return -EINVAL;
+		break;
+	case SLIC_1GB_DEVICE_ID:
+		if (rcvucodelen != GBRcvUCodeLen)
+			return -EINVAL;
 		break;
 	default:
 		ASSERT(0);
@@ -2225,13 +2240,16 @@ static int slic_card_download_gbrcv(struct adapter *adapter)
 	}
 
 	/* download finished */
+	release_firmware(fw);
 	WRITE_REG(slic_regs->slic_rcv_wcs, SLIC_RCVWCS_FINISH, FLUSH);
-
 	return 0;
 }
 
 static int slic_card_download(struct adapter *adapter)
 {
+	const struct firmware *fw;
+	const char *file = "";
+	int ret;
 	u32 section;
 	int thissectionsize;
 	int codeaddr;
@@ -2255,6 +2273,7 @@ static int slic_card_download(struct adapter *adapter)
 	case SLIC_2GB_DEVICE_ID:
 /*      DBG_MSG ("slicoss: %s devid==SLIC_2GB_DEVICE_ID sections[%x]\n",
 	__func__, (uint) ONumSections); */
+		file = "slic_oasis.bin";
 		numsects = ONumSections;
 		for (i = 0; i < numsects; i++) {
 			sectsize[i] = OSectionSize[i];
@@ -2264,6 +2283,7 @@ static int slic_card_download(struct adapter *adapter)
 	case SLIC_1GB_DEVICE_ID:
 /*              DBG_MSG ("slicoss: %s devid==SLIC_1GB_DEVICE_ID sections[%x]\n",
 		__func__, (uint) MNumSections); */
+		file = "slic_mojave.bin";
 		numsects = MNumSections;
 		for (i = 0; i < numsects; i++) {
 			sectsize[i] = MSectionSize[i];
@@ -2274,26 +2294,33 @@ static int slic_card_download(struct adapter *adapter)
 		ASSERT(0);
 		break;
 	}
+	ret = request_firmware(&fw, file, &adapter->pcidev->dev);
+	if (ret) {
+		printk(KERN_ERR "SLICOSS: Failed to load firmware %s\n", file);
+		return ret;
+	}
 
 	ASSERT(numsects <= 3);
 
 	for (section = 0; section < numsects; section++) {
 		switch (adapter->devid) {
 		case SLIC_2GB_DEVICE_ID:
-			instruction = (u32 *) &OasisUCode[section][0];
+			instruction = (u32 *)(fw->data + (SECTION_SIZE *
+								section));
 			baseaddress = sectstart[section];
 			thissectionsize = sectsize[section] >> 3;
 			lastinstruct =
-			    (u32 *) &OasisUCode[section][sectsize[section] -
-							     8];
+			    (u32 *)(fw->data + (SECTION_SIZE * section) +
+					sectsize[section] - 8);
 			break;
 		case SLIC_1GB_DEVICE_ID:
-			instruction = (u32 *) &MojaveUCode[section][0];
+			instruction = (u32 *)(fw->data + (SECTION_SIZE *
+								section));
 			baseaddress = sectstart[section];
 			thissectionsize = sectsize[section] >> 3;
 			lastinstruct =
-			    (u32 *) &MojaveUCode[section][sectsize[section]
-							      - 8];
+			    (u32 *)(fw->data + (SECTION_SIZE * section) +
+					sectsize[section] - 8);
 			break;
 		default:
 			ASSERT(0);
@@ -2329,10 +2356,12 @@ static int slic_card_download(struct adapter *adapter)
 	for (section = 0; section < numsects; section++) {
 		switch (adapter->devid) {
 		case SLIC_2GB_DEVICE_ID:
-			instruction = (u32 *)&OasisUCode[section][0];
+			instruction = (u32 *)fw->data + (SECTION_SIZE *
+								section);
 			break;
 		case SLIC_1GB_DEVICE_ID:
-			instruction = (u32 *)&MojaveUCode[section][0];
+			instruction = (u32 *)fw->data + (SECTION_SIZE *
+								section);
 			break;
 		default:
 			ASSERT(0);
@@ -2374,13 +2403,13 @@ static int slic_card_download(struct adapter *adapter)
 				    thissectionsize[%x] failure[%x]\n",
 				     __func__, codeaddr, thissectionsize,
 				     failure);
-
+				release_firmware(fw);
 				return -EIO;
 			}
 		}
 	}
 /*    DBG_MSG ("slicoss: Compare done\n");*/
-
+	release_firmware(fw);
 	/* Everything OK, kick off the card */
 	mdelay(10);
 	WRITE_REG(slic_regs->slic_wcs, SLIC_WCS_START, FLUSH);
@@ -2832,9 +2861,8 @@ static u32 slic_card_locate(struct adapter *adapter)
 	}
 	if (!physcard) {
 		/* no structure allocated for this physical card yet */
-		physcard = kmalloc(sizeof(struct physcard *), GFP_ATOMIC);
+		physcard = kzalloc(sizeof(struct physcard), GFP_ATOMIC);
 		ASSERT(physcard);
-		memset(physcard, 0, sizeof(struct physcard *));
 
 		DBG_MSG
 		    ("\n%s Allocate a PHYSICALcard:\n    PHYSICAL_Card[%p]\n\
diff --git a/drivers/staging/sxg/README b/drivers/staging/sxg/README
index d514d184880..e42f344ea5f 100644
--- a/drivers/staging/sxg/README
+++ b/drivers/staging/sxg/README
@@ -2,8 +2,6 @@ This is the rough cut at a driver for the Alacritech SLIC Technology
 Non-Accelerated 10Gbe network driver.
 
 TODO:
-	- lindent the code
-	- remove typedefs
 	- remove wrappers
 	- checkpatch.pl cleanups
 	- new functionality that the card needs
diff --git a/drivers/staging/sxg/sxg.c b/drivers/staging/sxg/sxg.c
index 5272a18e204..1e0cfcd7f0f 100644
--- a/drivers/staging/sxg/sxg.c
+++ b/drivers/staging/sxg/sxg.c
@@ -80,13 +80,13 @@
 #include "sxgphycode.h"
 #include "saharadbgdownload.h"
 
-static int sxg_allocate_buffer_memory(p_adapter_t adapter, u32 Size,
-				      SXG_BUFFER_TYPE BufferType);
-static void sxg_allocate_rcvblock_complete(p_adapter_t adapter, void *RcvBlock,
+static int sxg_allocate_buffer_memory(struct adapter_t *adapter, u32 Size,
+				      enum SXG_BUFFER_TYPE BufferType);
+static void sxg_allocate_rcvblock_complete(struct adapter_t *adapter, void *RcvBlock,
 					   dma_addr_t PhysicalAddress,
 					   u32 Length);
-static void sxg_allocate_sgl_buffer_complete(p_adapter_t adapter,
-					     PSXG_SCATTER_GATHER SxgSgl,
+static void sxg_allocate_sgl_buffer_complete(struct adapter_t *adapter,
+					     struct SXG_SCATTER_GATHER *SxgSgl,
 					     dma_addr_t PhysicalAddress,
 					     u32 Length);
 
@@ -96,17 +96,17 @@ static int sxg_entry_open(p_net_device dev);
 static int sxg_entry_halt(p_net_device dev);
 static int sxg_ioctl(p_net_device dev, struct ifreq *rq, int cmd);
 static int sxg_send_packets(struct sk_buff *skb, p_net_device dev);
-static int sxg_transmit_packet(p_adapter_t adapter, struct sk_buff *skb);
-static void sxg_dumb_sgl(PSCATTER_GATHER_LIST pSgl, PSXG_SCATTER_GATHER SxgSgl);
-
-static void sxg_handle_interrupt(p_adapter_t adapter);
-static int sxg_process_isr(p_adapter_t adapter, u32 MessageId);
-static u32 sxg_process_event_queue(p_adapter_t adapter, u32 RssId);
-static void sxg_complete_slow_send(p_adapter_t adapter);
-static struct sk_buff *sxg_slow_receive(p_adapter_t adapter, PSXG_EVENT Event);
-static void sxg_process_rcv_error(p_adapter_t adapter, u32 ErrorStatus);
-static bool sxg_mac_filter(p_adapter_t adapter,
-			   p_ether_header EtherHdr, ushort length);
+static int sxg_transmit_packet(struct adapter_t *adapter, struct sk_buff *skb);
+static void sxg_dumb_sgl(struct SCATTER_GATHER_LIST *pSgl, struct SXG_SCATTER_GATHER *SxgSgl);
+
+static void sxg_handle_interrupt(struct adapter_t *adapter);
+static int sxg_process_isr(struct adapter_t *adapter, u32 MessageId);
+static u32 sxg_process_event_queue(struct adapter_t *adapter, u32 RssId);
+static void sxg_complete_slow_send(struct adapter_t *adapter);
+static struct sk_buff *sxg_slow_receive(struct adapter_t *adapter, struct SXG_EVENT *Event);
+static void sxg_process_rcv_error(struct adapter_t *adapter, u32 ErrorStatus);
+static bool sxg_mac_filter(struct adapter_t *adapter,
+			   struct ether_header *EtherHdr, ushort length);
 
 #if SLIC_GET_STATS_ENABLED
 static struct net_device_stats *sxg_get_stats(p_net_device dev);
@@ -119,22 +119,22 @@ static int sxg_mac_set_address(p_net_device dev, void *ptr);
 static void sxg_mcast_set_list(p_net_device dev);
 #endif
 
-static void sxg_adapter_set_hwaddr(p_adapter_t adapter);
+static void sxg_adapter_set_hwaddr(struct adapter_t *adapter);
 
-static void sxg_unmap_mmio_space(p_adapter_t adapter);
+static void sxg_unmap_mmio_space(struct adapter_t *adapter);
 
-static int sxg_initialize_adapter(p_adapter_t adapter);
-static void sxg_stock_rcv_buffers(p_adapter_t adapter);
-static void sxg_complete_descriptor_blocks(p_adapter_t adapter,
+static int sxg_initialize_adapter(struct adapter_t *adapter);
+static void sxg_stock_rcv_buffers(struct adapter_t *adapter);
+static void sxg_complete_descriptor_blocks(struct adapter_t *adapter,
 					   unsigned char Index);
-static int sxg_initialize_link(p_adapter_t adapter);
-static int sxg_phy_init(p_adapter_t adapter);
-static void sxg_link_event(p_adapter_t adapter);
-static SXG_LINK_STATE sxg_get_link_state(p_adapter_t adapter);
-static void sxg_link_state(p_adapter_t adapter, SXG_LINK_STATE LinkState);
-static int sxg_write_mdio_reg(p_adapter_t adapter,
+static int sxg_initialize_link(struct adapter_t *adapter);
+static int sxg_phy_init(struct adapter_t *adapter);
+static void sxg_link_event(struct adapter_t *adapter);
+static enum SXG_LINK_STATE sxg_get_link_state(struct adapter_t *adapter);
+static void sxg_link_state(struct adapter_t *adapter, enum SXG_LINK_STATE LinkState);
+static int sxg_write_mdio_reg(struct adapter_t *adapter,
 			      u32 DevAddr, u32 RegAddr, u32 Value);
-static int sxg_read_mdio_reg(p_adapter_t adapter,
+static int sxg_read_mdio_reg(struct adapter_t *adapter,
 			     u32 DevAddr, u32 RegAddr, u32 *pValue);
 
 static unsigned int sxg_first_init = 1;
@@ -145,7 +145,7 @@ static int sxg_debug = 1;
 static int debug = -1;
 static p_net_device head_netdevice = NULL;
 
-static sxgbase_driver_t sxg_global = {
+static struct sxgbase_driver_t sxg_global = {
 	.dynamic_intagg = 1,
 };
 static int intagg_delay = 100;
@@ -186,7 +186,7 @@ static inline void sxg_reg32_write(void __iomem *reg, u32 value, bool flush)
 		mb();
 }
 
-static inline void sxg_reg64_write(p_adapter_t adapter, void __iomem *reg,
+static inline void sxg_reg64_write(struct adapter_t *adapter, void __iomem *reg,
 				   u64 value, u32 cpu)
 {
 	u32 value_high = (u32) (value >> 32);
@@ -209,7 +209,7 @@ static void sxg_init_driver(void)
 	}
 }
 
-static void sxg_dbg_macaddrs(p_adapter_t adapter)
+static void sxg_dbg_macaddrs(struct adapter_t *adapter)
 {
 	DBG_ERROR("  (%s) curr %2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X\n",
 		  adapter->netdev->name, adapter->currmacaddr[0],
@@ -225,12 +225,12 @@ static void sxg_dbg_macaddrs(p_adapter_t adapter)
 }
 
 /* SXG Globals */
-static SXG_DRIVER SxgDriver;
+static struct SXG_DRIVER SxgDriver;
 
 #ifdef  ATKDBG
-static sxg_trace_buffer_t LSxgTraceBuffer;
+static struct sxg_trace_buffer_t LSxgTraceBuffer;
 #endif /* ATKDBG */
-static sxg_trace_buffer_t *SxgTraceBuffer = NULL;
+static struct sxg_trace_buffer_t *SxgTraceBuffer = NULL;
 
 /*
  * sxg_download_microcode
@@ -244,9 +244,9 @@ static sxg_trace_buffer_t *SxgTraceBuffer = NULL;
  * Return
  *	int
  */
-static bool sxg_download_microcode(p_adapter_t adapter, SXG_UCODE_SEL UcodeSel)
+static bool sxg_download_microcode(struct adapter_t *adapter, enum SXG_UCODE_SEL UcodeSel)
 {
-	PSXG_HW_REGS HwRegs = adapter->HwRegs;
+	struct SXG_HW_REGS *HwRegs = adapter->HwRegs;
 	u32 Section;
 	u32 ThisSectionSize;
 	u32 *Instruction = NULL;
@@ -416,13 +416,13 @@ static bool sxg_download_microcode(p_adapter_t adapter, SXG_UCODE_SEL UcodeSel)
  * Return
  *	int
  */
-static int sxg_allocate_resources(p_adapter_t adapter)
+static int sxg_allocate_resources(struct adapter_t *adapter)
 {
 	int status;
 	u32 i;
 	u32 RssIds, IsrCount;
-/*      PSXG_XMT_RING                                   XmtRing; */
-/*      PSXG_RCV_RING                                   RcvRing; */
+/*      struct SXG_XMT_RING                                   *XmtRing; */
+/*      struct SXG_RCV_RING                                   *RcvRing; */
 
 	DBG_ERROR("%s ENTER\n", __func__);
 
@@ -461,13 +461,13 @@ static int sxg_allocate_resources(p_adapter_t adapter)
 
 	for (;;) {
 		DBG_ERROR("%s Allocate XmtRings size[%x]\n", __func__,
-			  (unsigned int)(sizeof(SXG_XMT_RING) * 1));
+			  (unsigned int)(sizeof(struct SXG_XMT_RING) * 1));
 
 		/* Start with big items first - receive and transmit rings.  At the moment */
 		/* I'm going to keep the ring size fixed and adjust the number of */
 		/* TCBs if we fail.  Later we might consider reducing the ring size as well.. */
 		adapter->XmtRings = pci_alloc_consistent(adapter->pcidev,
-							 sizeof(SXG_XMT_RING) *
+							 sizeof(struct SXG_XMT_RING) *
 							 1,
 							 &adapter->PXmtRings);
 		DBG_ERROR("%s XmtRings[%p]\n", __func__, adapter->XmtRings);
@@ -475,33 +475,33 @@ static int sxg_allocate_resources(p_adapter_t adapter)
 		if (!adapter->XmtRings) {
 			goto per_tcb_allocation_failed;
 		}
-		memset(adapter->XmtRings, 0, sizeof(SXG_XMT_RING) * 1);
+		memset(adapter->XmtRings, 0, sizeof(struct SXG_XMT_RING) * 1);
 
 		DBG_ERROR("%s Allocate RcvRings size[%x]\n", __func__,
-			  (unsigned int)(sizeof(SXG_RCV_RING) * 1));
+			  (unsigned int)(sizeof(struct SXG_RCV_RING) * 1));
 		adapter->RcvRings =
 		    pci_alloc_consistent(adapter->pcidev,
-					 sizeof(SXG_RCV_RING) * 1,
+					 sizeof(struct SXG_RCV_RING) * 1,
 					 &adapter->PRcvRings);
 		DBG_ERROR("%s RcvRings[%p]\n", __func__, adapter->RcvRings);
 		if (!adapter->RcvRings) {
 			goto per_tcb_allocation_failed;
 		}
-		memset(adapter->RcvRings, 0, sizeof(SXG_RCV_RING) * 1);
+		memset(adapter->RcvRings, 0, sizeof(struct SXG_RCV_RING) * 1);
 		break;
 
 	      per_tcb_allocation_failed:
 		/* an allocation failed.  Free any successful allocations. */
 		if (adapter->XmtRings) {
 			pci_free_consistent(adapter->pcidev,
-					    sizeof(SXG_XMT_RING) * 4096,
+					    sizeof(struct SXG_XMT_RING) * 4096,
 					    adapter->XmtRings,
 					    adapter->PXmtRings);
 			adapter->XmtRings = NULL;
 		}
 		if (adapter->RcvRings) {
 			pci_free_consistent(adapter->pcidev,
-					    sizeof(SXG_RCV_RING) * 4096,
+					    sizeof(struct SXG_RCV_RING) * 4096,
 					    adapter->RcvRings,
 					    adapter->PRcvRings);
 			adapter->RcvRings = NULL;
@@ -517,7 +517,7 @@ static int sxg_allocate_resources(p_adapter_t adapter)
 	/* Sanity check receive data structure format */
 	ASSERT((adapter->ReceiveBufferSize == SXG_RCV_DATA_BUFFER_SIZE) ||
 	       (adapter->ReceiveBufferSize == SXG_RCV_JUMBO_BUFFER_SIZE));
-	ASSERT(sizeof(SXG_RCV_DESCRIPTOR_BLOCK) ==
+	ASSERT(sizeof(struct SXG_RCV_DESCRIPTOR_BLOCK) ==
 	       SXG_RCV_DESCRIPTOR_BLOCK_SIZE);
 
 	/* Allocate receive data buffers.  We allocate a block of buffers and */
@@ -539,11 +539,11 @@ static int sxg_allocate_resources(p_adapter_t adapter)
 	}
 
 	DBG_ERROR("%s Allocate EventRings size[%x]\n", __func__,
-		  (unsigned int)(sizeof(SXG_EVENT_RING) * RssIds));
+		  (unsigned int)(sizeof(struct SXG_EVENT_RING) * RssIds));
 
 	/* Allocate event queues. */
 	adapter->EventRings = pci_alloc_consistent(adapter->pcidev,
-						   sizeof(SXG_EVENT_RING) *
+						   sizeof(struct SXG_EVENT_RING) *
 						   RssIds,
 						   &adapter->PEventRings);
 
@@ -554,7 +554,7 @@ static int sxg_allocate_resources(p_adapter_t adapter)
 		status = STATUS_RESOURCES;
 		goto per_tcb_allocation_failed;
 	}
-	memset(adapter->EventRings, 0, sizeof(SXG_EVENT_RING) * RssIds);
+	memset(adapter->EventRings, 0, sizeof(struct SXG_EVENT_RING) * RssIds);
 
 	DBG_ERROR("%s Allocate ISR size[%x]\n", __func__, IsrCount);
 	/* Allocate ISR */
@@ -628,7 +628,7 @@ static int sxg_entry_probe(struct pci_dev *pcidev,
 	static int did_version = 0;
 	int err;
 	struct net_device *netdev;
-	p_adapter_t adapter;
+	struct adapter_t *adapter;
 	void __iomem *memmapped_ioaddr;
 	u32 status = 0;
 	ulong mmio_start = 0;
@@ -681,7 +681,7 @@ static int sxg_entry_probe(struct pci_dev *pcidev,
 	pci_set_master(pcidev);
 
 	DBG_ERROR("call alloc_etherdev\n");
-	netdev = alloc_etherdev(sizeof(adapter_t));
+	netdev = alloc_etherdev(sizeof(struct adapter_t));
 	if (!netdev) {
 		err = -ENOMEM;
 		goto err_out_exit_sxg_probe;
@@ -871,7 +871,7 @@ static int sxg_entry_probe(struct pci_dev *pcidev,
  * Return Value:
  * 	None.
  */
-static void sxg_disable_interrupt(p_adapter_t adapter)
+static void sxg_disable_interrupt(struct adapter_t *adapter)
 {
 	SXG_TRACE(TRACE_SXG, SxgTraceBuffer, TRACE_NOISY, "DisIntr",
 		  adapter, adapter->InterruptsEnabled, 0, 0);
@@ -902,7 +902,7 @@ static void sxg_disable_interrupt(p_adapter_t adapter)
  * Return Value:
  * 	None.
  */
-static void sxg_enable_interrupt(p_adapter_t adapter)
+static void sxg_enable_interrupt(struct adapter_t *adapter)
 {
 	SXG_TRACE(TRACE_SXG, SxgTraceBuffer, TRACE_NOISY, "EnIntr",
 		  adapter, adapter->InterruptsEnabled, 0, 0);
@@ -935,7 +935,7 @@ static void sxg_enable_interrupt(p_adapter_t adapter)
 static irqreturn_t sxg_isr(int irq, void *dev_id)
 {
 	p_net_device dev = (p_net_device) dev_id;
-	p_adapter_t adapter = (p_adapter_t) netdev_priv(dev);
+	struct adapter_t *adapter = (struct adapter_t *) netdev_priv(dev);
 /*      u32                 CpuMask = 0, i; */
 
 	adapter->Stats.NumInts++;
@@ -963,8 +963,8 @@ static irqreturn_t sxg_isr(int irq, void *dev_id)
 		for (i = 0;
 		     i < adapter->RssSystemInfo->ProcessorInfo.RssCpuCount;
 		     i++) {
-			PSXG_EVENT_RING EventRing = &adapter->EventRings[i];
-			PSXG_EVENT Event =
+			struct XG_EVENT_RING *EventRing = &adapter->EventRings[i];
+			struct SXG_EVENT *Event =
 			    &EventRing->Ring[adapter->NextEvent[i]];
 			unsigned char Cpu =
 			    adapter->RssSystemInfo->RssIdToCpu[i];
@@ -992,7 +992,7 @@ static irqreturn_t sxg_isr(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-static void sxg_handle_interrupt(p_adapter_t adapter)
+static void sxg_handle_interrupt(struct adapter_t *adapter)
 {
 /*    unsigned char           RssId   = 0; */
 	u32 NewIsr;
@@ -1056,7 +1056,7 @@ static void sxg_handle_interrupt(p_adapter_t adapter)
  * Return Value:
  * 	None
  */
-static int sxg_process_isr(p_adapter_t adapter, u32 MessageId)
+static int sxg_process_isr(struct adapter_t *adapter, u32 MessageId)
 {
 	u32 Isr = adapter->IsrCopy[MessageId];
 	u32 NewIsr = 0;
@@ -1153,10 +1153,10 @@ static int sxg_process_isr(p_adapter_t adapter, u32 MessageId)
  * Return Value:
  * 	None.
  */
-static u32 sxg_process_event_queue(p_adapter_t adapter, u32 RssId)
+static u32 sxg_process_event_queue(struct adapter_t *adapter, u32 RssId)
 {
-	PSXG_EVENT_RING EventRing = &adapter->EventRings[RssId];
-	PSXG_EVENT Event = &EventRing->Ring[adapter->NextEvent[RssId]];
+	struct SXG_EVENT_RING *EventRing = &adapter->EventRings[RssId];
+	struct SXG_EVENT *Event = &EventRing->Ring[adapter->NextEvent[RssId]];
 	u32 EventsProcessed = 0, Batches = 0;
 	u32 num_skbs = 0;
 	struct sk_buff *skb;
@@ -1164,7 +1164,7 @@ static u32 sxg_process_event_queue(p_adapter_t adapter, u32 RssId)
 	struct sk_buff *prev_skb = NULL;
 	struct sk_buff *IndicationList[SXG_RCV_ARRAYSIZE];
 	u32 Index;
-	PSXG_RCV_DATA_BUFFER_HDR RcvDataBufferHdr;
+	struct SXG_RCV_DATA_BUFFER_HDR *RcvDataBufferHdr;
 #endif
 	u32 ReturnStatus = 0;
 
@@ -1293,12 +1293,12 @@ static u32 sxg_process_event_queue(p_adapter_t adapter, u32 RssId)
  * Return
  *	None
  */
-static void sxg_complete_slow_send(p_adapter_t adapter)
+static void sxg_complete_slow_send(struct adapter_t *adapter)
 {
-	PSXG_XMT_RING XmtRing = &adapter->XmtRings[0];
-	PSXG_RING_INFO XmtRingInfo = &adapter->XmtRingZeroInfo;
+	struct SXG_XMT_RING *XmtRing = &adapter->XmtRings[0];
+	struct SXG_RING_INFO *XmtRingInfo = &adapter->XmtRingZeroInfo;
 	u32 *ContextType;
-	PSXG_CMD XmtCmd;
+	struct SXG_CMD *XmtCmd;
 
 	/* NOTE - This lock is dropped and regrabbed in this loop. */
 	/* This means two different processors can both be running */
@@ -1359,12 +1359,12 @@ static void sxg_complete_slow_send(p_adapter_t adapter)
  * Return
  *	 skb
  */
-static struct sk_buff *sxg_slow_receive(p_adapter_t adapter, PSXG_EVENT Event)
+static struct sk_buff *sxg_slow_receive(struct adapter_t *adapter, struct SXG_EVENT *Event)
 {
-	PSXG_RCV_DATA_BUFFER_HDR RcvDataBufferHdr;
+	struct SXG_RCV_DATA_BUFFER_HDR *RcvDataBufferHdr;
 	struct sk_buff *Packet;
 
-	RcvDataBufferHdr = (PSXG_RCV_DATA_BUFFER_HDR) Event->HostHandle;
+	RcvDataBufferHdr = (struct SXG_RCV_DATA_BUFFER_HDR*) Event->HostHandle;
 	ASSERT(RcvDataBufferHdr);
 	ASSERT(RcvDataBufferHdr->State == SXG_BUFFER_ONCARD);
 	ASSERT(SXG_RECEIVE_DATA_LOCATION(RcvDataBufferHdr) ==
@@ -1400,7 +1400,7 @@ static struct sk_buff *sxg_slow_receive(p_adapter_t adapter, PSXG_EVENT Event)
 	}
 #if XXXTODO			/* VLAN stuff */
 	/* If there's a VLAN tag, extract it and validate it */
-	if (((p_ether_header) (SXG_RECEIVE_DATA_LOCATION(RcvDataBufferHdr)))->
+	if (((struct ether_header*) (SXG_RECEIVE_DATA_LOCATION(RcvDataBufferHdr)))->
 	    EtherType == ETHERTYPE_VLAN) {
 		if (SxgExtractVlanHeader(adapter, RcvDataBufferHdr, Event) !=
 		    STATUS_SUCCESS) {
@@ -1415,7 +1415,7 @@ static struct sk_buff *sxg_slow_receive(p_adapter_t adapter, PSXG_EVENT Event)
 	/* */
 	/* Dumb-nic frame.  See if it passes our mac filter and update stats */
 	/* */
-	if (!sxg_mac_filter(adapter, (p_ether_header)
+	if (!sxg_mac_filter(adapter, (struct ether_header*)
 			    SXG_RECEIVE_DATA_LOCATION(RcvDataBufferHdr),
 			    Event->Length)) {
 		SXG_TRACE(TRACE_SXG, SxgTraceBuffer, TRACE_NOISY, "RcvFiltr",
@@ -1456,7 +1456,7 @@ static struct sk_buff *sxg_slow_receive(p_adapter_t adapter, PSXG_EVENT Event)
  * Return Value:
  * 	None
  */
-static void sxg_process_rcv_error(p_adapter_t adapter, u32 ErrorStatus)
+static void sxg_process_rcv_error(struct adapter_t *adapter, u32 ErrorStatus)
 {
 	u32 Error;
 
@@ -1535,7 +1535,7 @@ static void sxg_process_rcv_error(p_adapter_t adapter, u32 ErrorStatus)
  * Return Value:
  * 	TRUE if the frame is to be allowed
  */
-static bool sxg_mac_filter(p_adapter_t adapter, p_ether_header EtherHdr,
+static bool sxg_mac_filter(struct adapter_t *adapter, struct ether_header *EtherHdr,
 			   ushort length)
 {
 	bool EqualAddr;
@@ -1560,7 +1560,7 @@ static bool sxg_mac_filter(p_adapter_t adapter, p_ether_header EtherHdr,
 				return (TRUE);
 			}
 			if (adapter->MacFilter & MAC_MCAST) {
-				PSXG_MULTICAST_ADDRESS MulticastAddrs =
+				struct SXG_MULTICAST_ADDRESS *MulticastAddrs =
 				    adapter->MulticastAddrs;
 				while (MulticastAddrs) {
 					ETHER_EQ_ADDR(MulticastAddrs->Address,
@@ -1600,7 +1600,7 @@ static bool sxg_mac_filter(p_adapter_t adapter, p_ether_header EtherHdr,
 	return (FALSE);
 }
 
-static int sxg_register_interrupt(p_adapter_t adapter)
+static int sxg_register_interrupt(struct adapter_t *adapter)
 {
 	if (!adapter->intrregistered) {
 		int retval;
@@ -1635,7 +1635,7 @@ static int sxg_register_interrupt(p_adapter_t adapter)
 	return (STATUS_SUCCESS);
 }
 
-static void sxg_deregister_interrupt(p_adapter_t adapter)
+static void sxg_deregister_interrupt(struct adapter_t *adapter)
 {
 	DBG_ERROR("sxg: %s ENTER adapter[%p]\n", __func__, adapter);
 #if XXXTODO
@@ -1661,7 +1661,7 @@ static void sxg_deregister_interrupt(p_adapter_t adapter)
  *  Perform initialization of our slic interface.
  *
  */
-static int sxg_if_init(p_adapter_t adapter)
+static int sxg_if_init(struct adapter_t *adapter)
 {
 	p_net_device dev = adapter->netdev;
 	int status = 0;
@@ -1721,7 +1721,7 @@ static int sxg_if_init(p_adapter_t adapter)
 
 static int sxg_entry_open(p_net_device dev)
 {
-	p_adapter_t adapter = (p_adapter_t) netdev_priv(dev);
+	struct adapter_t *adapter = (struct adapter_t *) netdev_priv(dev);
 	int status;
 
 	ASSERT(adapter);
@@ -1777,7 +1777,7 @@ static void __devexit sxg_entry_remove(struct pci_dev *pcidev)
 	p_net_device dev = pci_get_drvdata(pcidev);
 	u32 mmio_start = 0;
 	unsigned int mmio_len = 0;
-	p_adapter_t adapter = (p_adapter_t) netdev_priv(dev);
+	struct adapter_t *adapter = (struct adapter_t *) netdev_priv(dev);
 
 	ASSERT(adapter);
 	DBG_ERROR("sxg: %s ENTER dev[%p] adapter[%p]\n", __func__, dev,
@@ -1805,7 +1805,7 @@ static void __devexit sxg_entry_remove(struct pci_dev *pcidev)
 
 static int sxg_entry_halt(p_net_device dev)
 {
-	p_adapter_t adapter = (p_adapter_t) netdev_priv(dev);
+	struct adapter_t *adapter = (struct adapter_t *) netdev_priv(dev);
 
 	spin_lock_irqsave(&sxg_global.driver_lock, sxg_global.flags);
 	DBG_ERROR("sxg: %s (%s) ENTER\n", __func__, dev->name);
@@ -1830,7 +1830,7 @@ static int sxg_ioctl(p_net_device dev, struct ifreq *rq, int cmd)
 	switch (cmd) {
 	case SIOCSLICSETINTAGG:
 		{
-/*                      p_adapter_t adapter = (p_adapter_t) netdev_priv(dev); */
+/*                      struct adapter_t *adapter = (struct adapter_t *) netdev_priv(dev); */
 			u32 data[7];
 			u32 intagg;
 
@@ -1868,7 +1868,7 @@ static int sxg_ioctl(p_net_device dev, struct ifreq *rq, int cmd)
  */
 static int sxg_send_packets(struct sk_buff *skb, p_net_device dev)
 {
-	p_adapter_t adapter = (p_adapter_t) netdev_priv(dev);
+	struct adapter_t *adapter = (struct adapter_t *) netdev_priv(dev);
 	u32 status = STATUS_SUCCESS;
 
 	DBG_ERROR("sxg: %s ENTER sxg_send_packets skb[%p]\n", __func__,
@@ -1934,10 +1934,10 @@ static int sxg_send_packets(struct sk_buff *skb, p_net_device dev)
  * Return -
  * 		STATUS of send
  */
-static int sxg_transmit_packet(p_adapter_t adapter, struct sk_buff *skb)
+static int sxg_transmit_packet(struct adapter_t *adapter, struct sk_buff *skb)
 {
-	PSCATTER_GATHER_LIST pSgl;
-	PSXG_SCATTER_GATHER SxgSgl;
+	struct SCATTER_GATHER_LIST *pSgl;
+	struct SXG_SCATTER_GATHER *SxgSgl;
 	void *SglBuffer;
 	u32 SglBufferLength;
 
@@ -1980,14 +1980,14 @@ static int sxg_transmit_packet(p_adapter_t adapter, struct sk_buff *skb)
  * Return Value:
  * 	None.
  */
-static void sxg_dumb_sgl(PSCATTER_GATHER_LIST pSgl, PSXG_SCATTER_GATHER SxgSgl)
+static void sxg_dumb_sgl(struct SCATTER_GATHER_LIST *pSgl, struct SXG_SCATTER_GATHER *SxgSgl)
 {
-	p_adapter_t adapter = SxgSgl->adapter;
+	struct adapter_t *adapter = SxgSgl->adapter;
 	struct sk_buff *skb = SxgSgl->DumbPacket;
 	/* For now, all dumb-nic sends go on RSS queue zero */
-	PSXG_XMT_RING XmtRing = &adapter->XmtRings[0];
-	PSXG_RING_INFO XmtRingInfo = &adapter->XmtRingZeroInfo;
-	PSXG_CMD XmtCmd = NULL;
+	struct SXG_XMT_RING *XmtRing = &adapter->XmtRings[0];
+	struct SXG_RING_INFO *XmtRingInfo = &adapter->XmtRingZeroInfo;
+	struct SXG_CMD *XmtCmd = NULL;
 /*      u32                         Index = 0; */
 	u32 DataLength = skb->len;
 /*  unsigned int                                BufLen; */
@@ -2117,9 +2117,9 @@ static void sxg_dumb_sgl(PSCATTER_GATHER_LIST pSgl, PSXG_SCATTER_GATHER SxgSgl)
  * Return
  *	status
  */
-static int sxg_initialize_link(p_adapter_t adapter)
+static int sxg_initialize_link(struct adapter_t *adapter)
 {
-	PSXG_HW_REGS HwRegs = adapter->HwRegs;
+	struct SXG_HW_REGS *HwRegs = adapter->HwRegs;
 	u32 Value;
 	u32 ConfigData;
 	u32 MaxFrame;
@@ -2274,10 +2274,10 @@ static int sxg_initialize_link(p_adapter_t adapter)
  * Return
  *	status
  */
-static int sxg_phy_init(p_adapter_t adapter)
+static int sxg_phy_init(struct adapter_t *adapter)
 {
 	u32 Value;
-	PPHY_UCODE p;
+	struct PHY_UCODE *p;
 	int status;
 
 	DBG_ERROR("ENTER %s\n", __func__);
@@ -2322,10 +2322,10 @@ static int sxg_phy_init(p_adapter_t adapter)
  * Return
  *	None
  */
-static void sxg_link_event(p_adapter_t adapter)
+static void sxg_link_event(struct adapter_t *adapter)
 {
-	PSXG_HW_REGS HwRegs = adapter->HwRegs;
-	SXG_LINK_STATE LinkState;
+	struct SXG_HW_REGS *HwRegs = adapter->HwRegs;
+	enum SXG_LINK_STATE LinkState;
 	int status;
 	u32 Value;
 
@@ -2379,7 +2379,7 @@ static void sxg_link_event(p_adapter_t adapter)
  * Return
  *	Link State
  */
-static SXG_LINK_STATE sxg_get_link_state(p_adapter_t adapter)
+static enum SXG_LINK_STATE sxg_get_link_state(struct adapter_t *adapter)
 {
 	int status;
 	u32 Value;
@@ -2433,8 +2433,8 @@ static SXG_LINK_STATE sxg_get_link_state(p_adapter_t adapter)
 	return (SXG_LINK_DOWN);
 }
 
-static void sxg_indicate_link_state(p_adapter_t adapter,
-				    SXG_LINK_STATE LinkState)
+static void sxg_indicate_link_state(struct adapter_t *adapter,
+				    enum SXG_LINK_STATE LinkState)
 {
 	if (adapter->LinkState == SXG_LINK_UP) {
 		DBG_ERROR("%s: LINK now UP, call netif_start_queue\n",
@@ -2460,7 +2460,7 @@ static void sxg_indicate_link_state(p_adapter_t adapter,
  * Return
  *	None
  */
-static void sxg_link_state(p_adapter_t adapter, SXG_LINK_STATE LinkState)
+static void sxg_link_state(struct adapter_t *adapter, enum SXG_LINK_STATE LinkState)
 {
 	SXG_TRACE(TRACE_SXG, SxgTraceBuffer, TRACE_IMPORTANT, "LnkINDCT",
 		  adapter, LinkState, adapter->LinkState, adapter->State);
@@ -2498,10 +2498,10 @@ static void sxg_link_state(p_adapter_t adapter, SXG_LINK_STATE LinkState)
  * Return
  *	status
  */
-static int sxg_write_mdio_reg(p_adapter_t adapter,
+static int sxg_write_mdio_reg(struct adapter_t *adapter,
 			      u32 DevAddr, u32 RegAddr, u32 Value)
 {
-	PSXG_HW_REGS HwRegs = adapter->HwRegs;
+	struct SXG_HW_REGS *HwRegs = adapter->HwRegs;
 	u32 AddrOp;		/* Address operation (written to MIIM field reg) */
 	u32 WriteOp;		/* Write operation (written to MIIM field reg) */
 	u32 Cmd;		/* Command (written to MIIM command reg) */
@@ -2588,10 +2588,10 @@ static int sxg_write_mdio_reg(p_adapter_t adapter,
  * Return
  *	status
  */
-static int sxg_read_mdio_reg(p_adapter_t adapter,
+static int sxg_read_mdio_reg(struct adapter_t *adapter,
 			     u32 DevAddr, u32 RegAddr, u32 *pValue)
 {
-	PSXG_HW_REGS HwRegs = adapter->HwRegs;
+	struct SXG_HW_REGS *HwRegs = adapter->HwRegs;
 	u32 AddrOp;		/* Address operation (written to MIIM field reg) */
 	u32 ReadOp;		/* Read operation (written to MIIM field reg) */
 	u32 Cmd;		/* Command (written to MIIM command reg) */
@@ -2735,9 +2735,9 @@ static unsigned char sxg_mcast_get_mac_hash(char *macaddr)
 	return (machash);
 }
 
-static void sxg_mcast_set_mask(p_adapter_t adapter)
+static void sxg_mcast_set_mask(struct adapter_t *adapter)
 {
-	PSXG_UCODE_REGS sxg_regs = adapter->UcodeRegs;
+	struct SXG_UCODE_REGS *sxg_regs = adapter->UcodeRegs;
 
 	DBG_ERROR("%s ENTER (%s) macopts[%x] mask[%llx]\n", __func__,
 		  adapter->netdev->name, (unsigned int)adapter->MacFilter,
@@ -2775,7 +2775,7 @@ static void sxg_mcast_set_mask(p_adapter_t adapter)
  *  Allocate a mcast_address structure to hold the multicast address.
  *  Link it in.
  */
-static int sxg_mcast_add_list(p_adapter_t adapter, char *address)
+static int sxg_mcast_add_list(struct adapter_t *adapter, char *address)
 {
 	p_mcast_address_t mcaddr, mlist;
 	bool equaladdr;
@@ -2803,7 +2803,7 @@ static int sxg_mcast_add_list(p_adapter_t adapter, char *address)
 	return (STATUS_SUCCESS);
 }
 
-static void sxg_mcast_set_bit(p_adapter_t adapter, char *address)
+static void sxg_mcast_set_bit(struct adapter_t *adapter, char *address)
 {
 	unsigned char crcpoly;
 
@@ -2821,7 +2821,7 @@ static void sxg_mcast_set_bit(p_adapter_t adapter, char *address)
 
 static void sxg_mcast_set_list(p_net_device dev)
 {
-	p_adapter_t adapter = (p_adapter_t) netdev_priv(dev);
+	struct adapter_t *adapter = (struct adapter_t *) netdev_priv(dev);
 	int status = STATUS_SUCCESS;
 	int i;
 	char *addresses;
@@ -2876,7 +2876,7 @@ static void sxg_mcast_set_list(p_net_device dev)
 }
 #endif
 
-static void sxg_unmap_mmio_space(p_adapter_t adapter)
+static void sxg_unmap_mmio_space(struct adapter_t *adapter)
 {
 #if LINUX_FREES_ADAPTER_RESOURCES
 /*      if (adapter->Regs) { */
@@ -2896,7 +2896,7 @@ static void sxg_unmap_mmio_space(p_adapter_t adapter)
  * Return
  *	none
  */
-void SxgFreeResources(p_adapter_t adapter)
+void SxgFreeResources(struct adapter_t *adapter)
 {
 	u32 RssIds, IsrCount;
 	PTCP_OBJECT TcpObject;
@@ -2924,7 +2924,7 @@ void SxgFreeResources(p_adapter_t adapter)
 	/* Free event queues. */
 	if (adapter->EventRings) {
 		pci_free_consistent(adapter->pcidev,
-				    sizeof(SXG_EVENT_RING) * RssIds,
+				    sizeof(struct SXG_EVENT_RING) * RssIds,
 				    adapter->EventRings, adapter->PEventRings);
 	}
 	if (adapter->Isr) {
@@ -2991,7 +2991,7 @@ void SxgFreeResources(p_adapter_t adapter)
  * This routine is called when a memory allocation has completed.
  *
  * Arguments -
- *	p_adapter_t    	- Our adapter structure
+ *	struct adapter_t *   	- Our adapter structure
  *	VirtualAddress	- Memory virtual address
  *	PhysicalAddress	- Memory physical address
  *	Length		- Length of memory allocated (or 0)
@@ -3000,10 +3000,10 @@ void SxgFreeResources(p_adapter_t adapter)
  * Return
  *	None.
  */
-static void sxg_allocate_complete(p_adapter_t adapter,
+static void sxg_allocate_complete(struct adapter_t *adapter,
 				  void *VirtualAddress,
 				  dma_addr_t PhysicalAddress,
-				  u32 Length, SXG_BUFFER_TYPE Context)
+				  u32 Length, enum SXG_BUFFER_TYPE Context)
 {
 	SXG_TRACE(TRACE_SXG, SxgTraceBuffer, TRACE_NOISY, "AllocCmp",
 		  adapter, VirtualAddress, Length, Context);
@@ -3018,7 +3018,7 @@ static void sxg_allocate_complete(p_adapter_t adapter,
 					       PhysicalAddress, Length);
 		break;
 	case SXG_BUFFER_TYPE_SGL:
-		sxg_allocate_sgl_buffer_complete(adapter, (PSXG_SCATTER_GATHER)
+		sxg_allocate_sgl_buffer_complete(adapter, (struct SXG_SCATTER_GATHER*)
 						 VirtualAddress,
 						 PhysicalAddress, Length);
 		break;
@@ -3039,8 +3039,8 @@ static void sxg_allocate_complete(p_adapter_t adapter,
  * Return
  *	int
  */
-static int sxg_allocate_buffer_memory(p_adapter_t adapter,
-				      u32 Size, SXG_BUFFER_TYPE BufferType)
+static int sxg_allocate_buffer_memory(struct adapter_t *adapter,
+				      u32 Size, enum SXG_BUFFER_TYPE BufferType)
 {
 	int status;
 	void *Buffer;
@@ -3091,7 +3091,7 @@ static int sxg_allocate_buffer_memory(p_adapter_t adapter,
  * Return
  *
  */
-static void sxg_allocate_rcvblock_complete(p_adapter_t adapter,
+static void sxg_allocate_rcvblock_complete(struct adapter_t *adapter,
 					   void *RcvBlock,
 					   dma_addr_t PhysicalAddress,
 					   u32 Length)
@@ -3099,11 +3099,11 @@ static void sxg_allocate_rcvblock_complete(p_adapter_t adapter,
 	u32 i;
 	u32 BufferSize = adapter->ReceiveBufferSize;
 	u64 Paddr;
-	PSXG_RCV_BLOCK_HDR RcvBlockHdr;
+	struct SXG_RCV_BLOCK_HDR *RcvBlockHdr;
 	unsigned char *RcvDataBuffer;
-	PSXG_RCV_DATA_BUFFER_HDR RcvDataBufferHdr;
-	PSXG_RCV_DESCRIPTOR_BLOCK RcvDescriptorBlock;
-	PSXG_RCV_DESCRIPTOR_BLOCK_HDR RcvDescriptorBlockHdr;
+	struct SXG_RCV_DATA_BUFFER_HDR *RcvDataBufferHdr;
+	struct SXG_RCV_DESCRIPTOR_BLOCK *RcvDescriptorBlock;
+	struct SXG_RCV_DESCRIPTOR_BLOCK_HDR *RcvDescriptorBlockHdr;
 
 	SXG_TRACE(TRACE_SXG, SxgTraceBuffer, TRACE_NOISY, "AlRcvBlk",
 		  adapter, RcvBlock, Length, 0);
@@ -3129,7 +3129,7 @@ static void sxg_allocate_rcvblock_complete(p_adapter_t adapter,
 		     i++, Paddr += BufferSize, RcvDataBuffer += BufferSize) {
 			/* */
 			RcvDataBufferHdr =
-			    (PSXG_RCV_DATA_BUFFER_HDR) (RcvDataBuffer +
+			    (struct SXG_RCV_DATA_BUFFER_HDR*) (RcvDataBuffer +
 							SXG_RCV_DATA_BUFFER_HDR_OFFSET
 							(BufferSize));
 			RcvDataBufferHdr->VirtualAddress = RcvDataBuffer;
@@ -3147,7 +3147,7 @@ static void sxg_allocate_rcvblock_complete(p_adapter_t adapter,
 	/* Place this entire block of memory on the AllRcvBlocks queue so it can be */
 	/* free later */
 	RcvBlockHdr =
-	    (PSXG_RCV_BLOCK_HDR) ((unsigned char *)RcvBlock +
+	    (struct SXG_RCV_BLOCK_HDR*) ((unsigned char *)RcvBlock +
 				  SXG_RCV_BLOCK_HDR_OFFSET(BufferSize));
 	RcvBlockHdr->VirtualAddress = RcvBlock;
 	RcvBlockHdr->PhysicalAddress = PhysicalAddress;
@@ -3161,7 +3161,7 @@ static void sxg_allocate_rcvblock_complete(p_adapter_t adapter,
 	for (i = 0, Paddr = PhysicalAddress;
 	     i < SXG_RCV_DESCRIPTORS_PER_BLOCK;
 	     i++, Paddr += BufferSize, RcvDataBuffer += BufferSize) {
-		RcvDataBufferHdr = (PSXG_RCV_DATA_BUFFER_HDR) (RcvDataBuffer +
+		RcvDataBufferHdr = (struct SXG_RCV_DATA_BUFFER_HDR*) (RcvDataBuffer +
 							       SXG_RCV_DATA_BUFFER_HDR_OFFSET
 							       (BufferSize));
 		spin_lock(&adapter->RcvQLock);
@@ -3171,11 +3171,11 @@ static void sxg_allocate_rcvblock_complete(p_adapter_t adapter,
 
 	/* Locate the descriptor block and put it on a separate free queue */
 	RcvDescriptorBlock =
-	    (PSXG_RCV_DESCRIPTOR_BLOCK) ((unsigned char *)RcvBlock +
+	    (struct SXG_RCV_DESCRIPTOR_BLOCK*) ((unsigned char *)RcvBlock +
 					 SXG_RCV_DESCRIPTOR_BLOCK_OFFSET
 					 (BufferSize));
 	RcvDescriptorBlockHdr =
-	    (PSXG_RCV_DESCRIPTOR_BLOCK_HDR) ((unsigned char *)RcvBlock +
+	    (struct SXG_RCV_DESCRIPTOR_BLOCK_HDR*) ((unsigned char *)RcvBlock +
 					     SXG_RCV_DESCRIPTOR_BLOCK_HDR_OFFSET
 					     (BufferSize));
 	RcvDescriptorBlockHdr->VirtualAddress = RcvDescriptorBlock;
@@ -3193,7 +3193,7 @@ static void sxg_allocate_rcvblock_complete(p_adapter_t adapter,
 		for (i = 0; i < SXG_RCV_DESCRIPTORS_PER_BLOCK;
 		     i++, RcvDataBuffer += BufferSize) {
 			RcvDataBufferHdr =
-			    (PSXG_RCV_DATA_BUFFER_HDR) (RcvDataBuffer +
+			    (struct SXG_RCV_DATA_BUFFER_HDR*) (RcvDataBuffer +
 							SXG_RCV_DATA_BUFFER_HDR_OFFSET
 							(BufferSize));
 			SXG_FREE_RCV_PACKET(RcvDataBufferHdr);
@@ -3220,8 +3220,8 @@ static void sxg_allocate_rcvblock_complete(p_adapter_t adapter,
  * Return
  *
  */
-static void sxg_allocate_sgl_buffer_complete(p_adapter_t adapter,
-					     PSXG_SCATTER_GATHER SxgSgl,
+static void sxg_allocate_sgl_buffer_complete(struct adapter_t *adapter,
+					     struct SXG_SCATTER_GATHER *SxgSgl,
 					     dma_addr_t PhysicalAddress,
 					     u32 Length)
 {
@@ -3229,7 +3229,7 @@ static void sxg_allocate_sgl_buffer_complete(p_adapter_t adapter,
 		  adapter, SxgSgl, Length, 0);
 	spin_lock(&adapter->SglQLock);
 	adapter->AllSglBufferCount++;
-	memset(SxgSgl, 0, sizeof(SXG_SCATTER_GATHER));
+	memset(SxgSgl, 0, sizeof(struct SXG_SCATTER_GATHER*));
 	SxgSgl->PhysicalAddress = PhysicalAddress;	/* *PhysicalAddress; */
 	SxgSgl->adapter = adapter;	/* Initialize backpointer once */
 	InsertTailList(&adapter->AllSglBuffers, &SxgSgl->AllList);
@@ -3243,14 +3243,14 @@ static void sxg_allocate_sgl_buffer_complete(p_adapter_t adapter,
 static unsigned char temp_mac_address[6] =
     { 0x00, 0xab, 0xcd, 0xef, 0x12, 0x69 };
 
-static void sxg_adapter_set_hwaddr(p_adapter_t adapter)
+static void sxg_adapter_set_hwaddr(struct adapter_t *adapter)
 {
 /*  DBG_ERROR ("%s ENTER card->config_set[%x] port[%d] physport[%d] funct#[%d]\n", __func__, */
 /*             card->config_set, adapter->port, adapter->physport, adapter->functionnumber); */
 /* */
 /*  sxg_dbg_macaddrs(adapter); */
 
-	memcpy(adapter->macaddr, temp_mac_address, sizeof(SXG_CONFIG_MAC));
+	memcpy(adapter->macaddr, temp_mac_address, sizeof(struct SXG_CONFIG_MAC));
 /*      DBG_ERROR ("%s AFTER copying from config.macinfo into currmacaddr\n", __func__); */
 /*      sxg_dbg_macaddrs(adapter); */
 	if (!(adapter->currmacaddr[0] ||
@@ -3271,7 +3271,7 @@ static void sxg_adapter_set_hwaddr(p_adapter_t adapter)
 #if XXXTODO
 static int sxg_mac_set_address(p_net_device dev, void *ptr)
 {
-	p_adapter_t adapter = (p_adapter_t) netdev_priv(dev);
+	struct adapter_t *adapter = (struct adapter_t *) netdev_priv(dev);
 	struct sockaddr *addr = ptr;
 
 	DBG_ERROR("%s ENTER (%s)\n", __func__, adapter->netdev->name);
@@ -3313,7 +3313,7 @@ static int sxg_mac_set_address(p_net_device dev, void *ptr)
  * Return
  *	int
  */
-static int sxg_initialize_adapter(p_adapter_t adapter)
+static int sxg_initialize_adapter(struct adapter_t *adapter)
 {
 	u32 RssIds, IsrCount;
 	u32 i;
@@ -3327,7 +3327,7 @@ static int sxg_initialize_adapter(p_adapter_t adapter)
 
 	/* Sanity check SXG_UCODE_REGS structure definition to */
 	/* make sure the length is correct */
-	ASSERT(sizeof(SXG_UCODE_REGS) == SXG_REGISTER_SIZE_PER_CPU);
+	ASSERT(sizeof(struct SXG_UCODE_REGS) == SXG_REGISTER_SIZE_PER_CPU);
 
 	/* Disable interrupts */
 	SXG_DISABLE_ALL_INTERRUPTS(adapter);
@@ -3412,16 +3412,16 @@ static int sxg_initialize_adapter(p_adapter_t adapter)
  * Return
  *	status
  */
-static int sxg_fill_descriptor_block(p_adapter_t adapter,
-				     PSXG_RCV_DESCRIPTOR_BLOCK_HDR
-				     RcvDescriptorBlockHdr)
+static int sxg_fill_descriptor_block(struct adapter_t *adapter,
+				     struct SXG_RCV_DESCRIPTOR_BLOCK_HDR
+				     *RcvDescriptorBlockHdr)
 {
 	u32 i;
-	PSXG_RING_INFO RcvRingInfo = &adapter->RcvRingZeroInfo;
-	PSXG_RCV_DATA_BUFFER_HDR RcvDataBufferHdr;
-	PSXG_RCV_DESCRIPTOR_BLOCK RcvDescriptorBlock;
-	PSXG_CMD RingDescriptorCmd;
-	PSXG_RCV_RING RingZero = &adapter->RcvRings[0];
+	struct SXG_RING_INFO *RcvRingInfo = &adapter->RcvRingZeroInfo;
+	struct SXG_RCV_DATA_BUFFER_HDR *RcvDataBufferHdr;
+	struct SXG_RCV_DESCRIPTOR_BLOCK *RcvDescriptorBlock;
+	struct SXG_CMD *RingDescriptorCmd;
+	struct SXG_RCV_RING *RingZero = &adapter->RcvRings[0];
 
 	SXG_TRACE(TRACE_SXG, SxgTraceBuffer, TRACE_NOISY, "FilBlk",
 		  adapter, adapter->RcvBuffersOnCard,
@@ -3442,7 +3442,7 @@ static int sxg_fill_descriptor_block(p_adapter_t adapter,
 	ASSERT(RingDescriptorCmd);
 	RcvDescriptorBlockHdr->State = SXG_BUFFER_ONCARD;
 	RcvDescriptorBlock =
-	    (PSXG_RCV_DESCRIPTOR_BLOCK) RcvDescriptorBlockHdr->VirtualAddress;
+	    (struct SXG_RCV_DESCRIPTOR_BLOCK*) RcvDescriptorBlockHdr->VirtualAddress;
 
 	/* Fill in the descriptor block */
 	for (i = 0; i < SXG_RCV_DESCRIPTORS_PER_BLOCK; i++) {
@@ -3484,9 +3484,9 @@ static int sxg_fill_descriptor_block(p_adapter_t adapter,
  * Return
  *	None
  */
-static void sxg_stock_rcv_buffers(p_adapter_t adapter)
+static void sxg_stock_rcv_buffers(struct adapter_t *adapter)
 {
-	PSXG_RCV_DESCRIPTOR_BLOCK_HDR RcvDescriptorBlockHdr;
+	struct SXG_RCV_DESCRIPTOR_BLOCK_HDR *RcvDescriptorBlockHdr;
 
 	SXG_TRACE(TRACE_SXG, SxgTraceBuffer, TRACE_NOISY, "StockBuf",
 		  adapter, adapter->RcvBuffersOnCard,
@@ -3506,14 +3506,14 @@ static void sxg_stock_rcv_buffers(p_adapter_t adapter)
 	/* Now grab the RcvQLock lock and proceed */
 	spin_lock(&adapter->RcvQLock);
 	while (adapter->RcvBuffersOnCard < SXG_RCV_DATA_BUFFERS) {
-		PLIST_ENTRY _ple;
+		struct LIST_ENTRY *_ple;
 
 		/* Get a descriptor block */
 		RcvDescriptorBlockHdr = NULL;
 		if (adapter->FreeRcvBlockCount) {
 			_ple = RemoveHeadList(&adapter->FreeRcvBlocks);
 			RcvDescriptorBlockHdr =
-			    container_of(_ple, SXG_RCV_DESCRIPTOR_BLOCK_HDR,
+			    container_of(_ple, struct SXG_RCV_DESCRIPTOR_BLOCK_HDR,
 					 FreeList);
 			adapter->FreeRcvBlockCount--;
 			RcvDescriptorBlockHdr->State = SXG_BUFFER_BUSY;
@@ -3550,13 +3550,13 @@ static void sxg_stock_rcv_buffers(p_adapter_t adapter)
  * Return
  *	None
  */
-static void sxg_complete_descriptor_blocks(p_adapter_t adapter,
+static void sxg_complete_descriptor_blocks(struct adapter_t *adapter,
 					   unsigned char Index)
 {
-	PSXG_RCV_RING RingZero = &adapter->RcvRings[0];
-	PSXG_RING_INFO RcvRingInfo = &adapter->RcvRingZeroInfo;
-	PSXG_RCV_DESCRIPTOR_BLOCK_HDR RcvDescriptorBlockHdr;
-	PSXG_CMD RingDescriptorCmd;
+	struct SXG_RCV_RING *RingZero = &adapter->RcvRings[0];
+	struct SXG_RING_INFO *RcvRingInfo = &adapter->RcvRingZeroInfo;
+	struct SXG_RCV_DESCRIPTOR_BLOCK_HDR *RcvDescriptorBlockHdr;
+	struct SXG_CMD *RingDescriptorCmd;
 
 	SXG_TRACE(TRACE_SXG, SxgTraceBuffer, TRACE_NOISY, "CmpRBlks",
 		  adapter, Index, RcvRingInfo->Head, RcvRingInfo->Tail);
diff --git a/drivers/staging/sxg/sxg.h b/drivers/staging/sxg/sxg.h
index 844ca56f280..653cf3ba0c4 100644
--- a/drivers/staging/sxg/sxg.h
+++ b/drivers/staging/sxg/sxg.h
@@ -45,7 +45,7 @@
 #define p_net_device struct net_device *
 // SXG_STATS - Probably move these to someplace where
 // the slicstat (sxgstat?) program can get them.
-typedef struct _SXG_STATS {
+struct SXG_STATS {
 	// Xmt
 	u32				XmtNBL;				// Offload send NBL count
 	u64				DumbXmtBytes;		// Dumbnic send bytes
@@ -109,7 +109,7 @@ typedef struct _SXG_STATS {
 	u64				LinkCrc;			// SXG_RCV_STATUS_LINK_CRC:
 	u64				LinkOflow;			// SXG_RCV_STATUS_LINK_OFLOW:
 	u64				LinkUflow;			// SXG_RCV_STATUS_LINK_UFLOW:
-} SXG_STATS, *PSXG_STATS;
+};
 
 
 /****************************************************************************
@@ -215,12 +215,12 @@ typedef struct _SXG_STATS {
 ///////////////////////////////////////////////////////////////////////////////
 // NOTE - Lock must be held with RCV macros
 #define SXG_GET_RCV_DATA_BUFFER(_pAdapt, _Hdr) {								\
-	PLIST_ENTRY     				_ple;										\
+	struct LIST_ENTRY     				*_ple;										\
 	_Hdr = NULL;																\
 	if((_pAdapt)->FreeRcvBufferCount) {											\
 		ASSERT(!(IsListEmpty(&(_pAdapt)->FreeRcvBuffers)));						\
 		_ple = RemoveHeadList(&(_pAdapt)->FreeRcvBuffers);	    				\
-		(_Hdr) = container_of(_ple, SXG_RCV_DATA_BUFFER_HDR, FreeList);	        \
+		(_Hdr) = container_of(_ple, struct SXG_RCV_DATA_BUFFER_HDR, FreeList);	        \
 		(_pAdapt)->FreeRcvBufferCount--;										\
 		ASSERT((_Hdr)->State == SXG_BUFFER_FREE);								\
 	}																			\
@@ -263,12 +263,12 @@ typedef struct _SXG_STATS {
 // until after that.  We're dealing with round numbers here, so we don't need to,
 // and not grabbing it avoids a possible double-trip.
 #define SXG_GET_SGL_BUFFER(_pAdapt, _Sgl) {				\
-	PLIST_ENTRY _ple;						\
+	struct LIST_ENTRY *_ple;						\
 	if ((_pAdapt->FreeSglBufferCount < SXG_MIN_SGL_BUFFERS) &&	\
 	   (_pAdapt->AllSglBufferCount < SXG_MAX_SGL_BUFFERS) &&	\
 	   (_pAdapt->AllocationsPending == 0)) {			\
 		sxg_allocate_buffer_memory(_pAdapt,			\
-			(sizeof(SXG_SCATTER_GATHER) + SXG_SGL_BUF_SIZE),\
+			(sizeof(struct SXG_SCATTER_GATHER) + SXG_SGL_BUF_SIZE),\
 			SXG_BUFFER_TYPE_SGL);				\
 	}								\
 	_Sgl = NULL;							\
@@ -276,7 +276,7 @@ typedef struct _SXG_STATS {
 	if((_pAdapt)->FreeSglBufferCount) {				\
 		ASSERT(!(IsListEmpty(&(_pAdapt)->FreeSglBuffers)));	\
 		_ple = RemoveHeadList(&(_pAdapt)->FreeSglBuffers);	\
-		(_Sgl) = container_of(_ple, SXG_SCATTER_GATHER, FreeList); \
+		(_Sgl) = container_of(_ple, struct SXG_SCATTER_GATHER, FreeList); \
             (_pAdapt)->FreeSglBufferCount--;				\
 		ASSERT((_Sgl)->State == SXG_BUFFER_FREE);		\
 		(_Sgl)->State = SXG_BUFFER_BUSY;			\
@@ -289,17 +289,17 @@ typedef struct _SXG_STATS {
 // SXG_MULTICAST_ADDRESS
 //
 // Linked list of multicast addresses.
-typedef struct _SXG_MULTICAST_ADDRESS {
+struct SXG_MULTICAST_ADDRESS {
 	unsigned char							Address[6];
-	struct _SXG_MULTICAST_ADDRESS	*Next;
-} SXG_MULTICAST_ADDRESS, *PSXG_MULTICAST_ADDRESS;
+	struct SXG_MULTICAST_ADDRESS	*Next;
+};
 
 // Structure to maintain chimney send and receive buffer queues.
 // This structure maintains NET_BUFFER_LIST queues that are
 // given to us via the Chimney MiniportTcpOffloadSend and
 // MiniportTcpOffloadReceive routines.  This structure DOES NOT
 // manage our data buffer queue
-typedef struct _SXG_BUFFER_QUEUE {
+struct SXG_BUFFER_QUEUE {
 	u32						Type;			// Slow or fast - See below
 	u32						Direction;		// Xmt or Rcv
 	u32						Bytes;			// Byte count
@@ -307,7 +307,7 @@ typedef struct _SXG_BUFFER_QUEUE {
 	u32 *        			Tail;			// Send queue tail
 //	PNET_BUFFER_LIST			NextNBL;		// Short cut - next NBL
 //	PNET_BUFFER					NextNB;			// Short cut - next NB
-} SXG_BUFFER_QUEUE, *PSXG_BUFFER_QUEUE;
+};
 
 #define		SXG_SLOW_SEND_BUFFER	0
 #define		SXG_FAST_SEND_BUFFER	1
@@ -335,7 +335,7 @@ typedef struct _SXG_BUFFER_QUEUE {
 
 // Adapter states - These states closely match the adapter states
 // documented in the DDK (with a few exceptions).
-typedef enum _SXG_STATE {
+enum SXG_STATE {
 	SXG_STATE_INITIALIZING,			// Initializing
 	SXG_STATE_BOOTDIAG,				// Boot-Diagnostic mode
 	SXG_STATE_PAUSING,				// Pausing
@@ -347,24 +347,24 @@ typedef enum _SXG_STATE {
 	SXG_STATE_HALTING,				// Halting
 	SXG_STATE_HALTED,				// Down or not-initialized
 	SXG_STATE_SHUTDOWN				// shutdown
-} SXG_STATE, *PSXG_STATE;
+};
 
 // Link state
-typedef enum _SXG_LINK_STATE {
+enum SXG_LINK_STATE {
 	SXG_LINK_DOWN,
 	SXG_LINK_UP
-} SXG_LINK_STATE, *PSXG_LINK_STATE;
+};
 
 // Link initialization timeout in 100us units
 #define SXG_LINK_TIMEOUT	100000		// 10 Seconds - REDUCE!
 
 
 // Microcode file selection codes
-typedef enum _SXG_UCODE_SEL {
+enum SXG_UCODE_SEL {
 	SXG_UCODE_SAHARA,				// Sahara ucode
 	SXG_UCODE_SDIAGCPU,				// Sahara CPU diagnostic ucode
 	SXG_UCODE_SDIAGSYS				// Sahara system diagnostic ucode
-} SXG_UCODE_SEL;
+};
 
 
 #define SXG_DISABLE_ALL_INTERRUPTS(_padapt) sxg_disable_interrupt(_padapt)
@@ -384,10 +384,10 @@ typedef enum _SXG_UCODE_SEL {
 //
 // contains information about the sxg driver.  There is only
 // one of these, and it is defined as a global.
-typedef struct _SXG_DRIVER {
-	struct _adapter_t	*Adapters;		// Linked list of adapters
+struct SXG_DRIVER {
+	struct adapter_t	*Adapters;		// Linked list of adapters
 	ushort				AdapterID;		// Maintain unique adapter ID
-} SXG_DRIVER, *PSXG_DRIVER;
+};
 
 #ifdef STATUS_SUCCESS
 #undef STATUS_SUCCESS
@@ -416,11 +416,10 @@ typedef struct _SXG_DRIVER {
 #define MIN(a, b) ((u32)(a) < (u32)(b) ? (a) : (b))
 #define MAX(a, b) ((u32)(a) > (u32)(b) ? (a) : (b))
 
-typedef struct _mcast_address_t
-{
+struct mcast_address_t {
     unsigned char                     address[6];
-    struct _mcast_address_t   *next;
-}  mcast_address_t, *p_mcast_address_t;
+    struct mcast_address_t   *next;
+};
 
 #define CARD_DOWN        0x00000000
 #define CARD_UP          0x00000001
@@ -472,41 +471,37 @@ typedef struct _mcast_address_t
 #define SLIC_CARD_STATE(x)    ((x==CARD_UP) ? "UP" : "Down")
 
 
-typedef struct _ether_header
-{
+struct ether_header {
     unsigned char    ether_dhost[6];
     unsigned char    ether_shost[6];
     ushort   ether_type;
-} ether_header, *p_ether_header;
+};
 
 
 #define NUM_CFG_SPACES      2
 #define NUM_CFG_REGS        64
 
-typedef struct _physcard_t
-{
-    struct _adapter_t  *adapter[SLIC_MAX_PORTS];
-    struct _physcard_t *next;
+struct physcard_t {
+    struct adapter_t  *adapter[SLIC_MAX_PORTS];
+    struct physcard_t *next;
     unsigned int                adapters_allocd;
-} physcard_t, *p_physcard_t;
+};
 
-typedef struct _sxgbase_driver
-{
+struct sxgbase_driver_t {
 	spinlock_t	driver_lock;
 	unsigned long	flags;	/* irqsave for spinlock */
 	u32		num_sxg_cards;
 	u32		num_sxg_ports;
 	u32		num_sxg_ports_active;
 	u32		dynamic_intagg;
-	p_physcard_t	phys_card;
-} sxgbase_driver_t;
+	struct physcard_t	*phys_card;
+};
 
 
-typedef struct _adapter_t
-{
+struct adapter_t {
 	void *               ifp;
 	unsigned int                port;
-	p_physcard_t        physcard;
+	struct physcard_t        *physcard;
 	unsigned int                physport;
 	unsigned int                cardindex;
 	unsigned int                card_size;
@@ -544,7 +539,7 @@ typedef struct _adapter_t
 	u32             macopts;
 	ushort              devflags_prev;
 	u64             mcastmask;
-	p_mcast_address_t   mcastaddrs;
+	struct mcast_address_t   *mcastaddrs;
 	struct timer_list   pingtimer;
 	u32             pingtimerset;
 	struct timer_list   statstimer;
@@ -580,11 +575,11 @@ typedef struct _adapter_t
 	u32             intagg_period;
 	struct net_device_stats stats;
 	u32 *					MiniportHandle;		// Our miniport handle
-	SXG_STATE					State;				// Adapter state
-	SXG_LINK_STATE				LinkState;			// Link state
+	enum SXG_STATE					State;				// Adapter state
+	enum SXG_LINK_STATE				LinkState;			// Link state
 	u64						LinkSpeed;			// Link Speed
 	u32						PowerState;			// NDIS power state
-	struct _adapter_t   		*Next;				// Linked list
+	struct adapter_t   		*Next;				// Linked list
 	ushort						AdapterID;			// 1..n
 	unsigned char						MacAddr[6];			// Our permanent HW mac address
 	unsigned char						CurrMacAddr[6];		// Our Current mac address
@@ -592,16 +587,16 @@ typedef struct _adapter_t
 	p_net_device                next_netdevice;
 	struct pci_dev            * pcidev;
 
-	PSXG_MULTICAST_ADDRESS		MulticastAddrs;		// Multicast list
+	struct SXG_MULTICAST_ADDRESS		*MulticastAddrs;		// Multicast list
 	u64     				MulticastMask;		// Multicast mask
 	u32 *					InterruptHandle;	// Register Interrupt handle
 	u32						InterruptLevel;		// From Resource list
 	u32						InterruptVector;	// From Resource list
 	spinlock_t	AdapterLock;	/* Serialize access adapter routines */
 	spinlock_t	Bit64RegLock;	/* For writing 64-bit addresses */
-	PSXG_HW_REGS				HwRegs;				// Sahara HW Register Memory (BAR0/1)
-	PSXG_UCODE_REGS				UcodeRegs;			// Microcode Register Memory (BAR2/3)
-	PSXG_TCB_REGS				TcbRegs;			// Same as Ucode regs - See sxghw.h
+	struct SXG_HW_REGS			*HwRegs;				// Sahara HW Register Memory (BAR0/1)
+	struct SXG_UCODE_REGS			*UcodeRegs;			// Microcode Register Memory (BAR2/3)
+	struct SXG_TCB_REGS			*TcbRegs;			// Same as Ucode regs - See sxghw.h
 	ushort						ResetDpcCount;		// For timeout
 	ushort						RssDpcCount;		// For timeout
 	ushort						VendorID;			// Vendor ID
@@ -613,25 +608,25 @@ typedef struct _adapter_t
 	u32 *					BufferPoolHandle;	// Used with NDIS 5.2 only.  Don't ifdef out
 	u32						MacFilter;			// NDIS MAC Filter
 	ushort						IpId;				// For slowpath
-	PSXG_EVENT_RING				EventRings;			// Host event rings.  1/CPU to 16 max
+	struct SXG_EVENT_RING			*EventRings;			// Host event rings.  1/CPU to 16 max
 	dma_addr_t              	PEventRings;		// Physical address
 	u32						NextEvent[SXG_MAX_RSS];	// Current location in ring
 	dma_addr_t          		PTcbBuffers;		// TCB Buffers - physical address
 	dma_addr_t	            	PTcbCompBuffers;	// TCB Composite Buffers - phys addr
-	PSXG_XMT_RING				XmtRings;			// Transmit rings
+	struct SXG_XMT_RING				*XmtRings;			// Transmit rings
 	dma_addr_t		            PXmtRings;			// Transmit rings - physical address
-	SXG_RING_INFO				XmtRingZeroInfo;	// Transmit ring 0 info
+	struct SXG_RING_INFO				XmtRingZeroInfo;	// Transmit ring 0 info
 	spinlock_t	XmtZeroLock;	/* Transmit ring 0 lock */
 	u32 *					XmtRingZeroIndex;	// Shared XMT ring 0 index
 	dma_addr_t          		PXmtRingZeroIndex;	// Shared XMT ring 0 index - physical
-	LIST_ENTRY					FreeProtocolHeaders;// Free protocol headers
+	struct LIST_ENTRY					FreeProtocolHeaders;// Free protocol headers
 	u32						FreeProtoHdrCount;	// Count
 	void *						ProtocolHeaders;	// Block of protocol header
 	dma_addr_t	            	PProtocolHeaders;	// Block of protocol headers - phys
 
-	PSXG_RCV_RING				RcvRings;			// Receive rings
+	struct SXG_RCV_RING		*RcvRings;			// Receive rings
 	dma_addr_t	            	PRcvRings;			// Receive rings - physical address
-	SXG_RING_INFO				RcvRingZeroInfo;	// Receive ring 0 info
+	struct SXG_RING_INFO				RcvRingZeroInfo;	// Receive ring 0 info
 
 	u32 *					Isr;				// Interrupt status register
 	dma_addr_t	            	PIsr;				// ISR - physical address
@@ -645,9 +640,9 @@ typedef struct _adapter_t
 	u32						HashInformation;
 	// Receive buffer queues
 	spinlock_t	RcvQLock;	/* Receive Queue Lock */
-	LIST_ENTRY					FreeRcvBuffers;		// Free SXG_DATA_BUFFER queue
-	LIST_ENTRY					FreeRcvBlocks;		// Free SXG_RCV_DESCRIPTOR_BLOCK Q
-	LIST_ENTRY					AllRcvBlocks;		// All SXG_RCV_BLOCKs
+	struct LIST_ENTRY					FreeRcvBuffers;		// Free SXG_DATA_BUFFER queue
+	struct LIST_ENTRY					FreeRcvBlocks;		// Free SXG_RCV_DESCRIPTOR_BLOCK Q
+	struct LIST_ENTRY					AllRcvBlocks;		// All SXG_RCV_BLOCKs
 	ushort						FreeRcvBufferCount;	// Number of free rcv data buffers
 	ushort						FreeRcvBlockCount;	// # of free rcv descriptor blocks
 	ushort						AllRcvBlockCount;	// Number of total receive blocks
@@ -656,8 +651,8 @@ typedef struct _adapter_t
 	u32						RcvBuffersOnCard;	// SXG_DATA_BUFFERS owned by card
 	// SGL buffers
 	spinlock_t	SglQLock;	/* SGL Queue Lock */
-	LIST_ENTRY					FreeSglBuffers;		// Free SXG_SCATTER_GATHER
-	LIST_ENTRY					AllSglBuffers;		// All SXG_SCATTER_GATHER
+	struct LIST_ENTRY					FreeSglBuffers;		// Free SXG_SCATTER_GATHER
+	struct LIST_ENTRY					AllSglBuffers;		// All SXG_SCATTER_GATHER
 	ushort						FreeSglBufferCount;	// Number of free SGL buffers
 	ushort						AllSglBufferCount;	// Number of total SGL buffers
 	u32						CurrentTime;		// Tick count
@@ -679,7 +674,7 @@ typedef struct _adapter_t
 	// Stats
 	u32						PendingRcvCount;	// Outstanding rcv indications
 	u32						PendingXmtCount;	// Outstanding send requests
-	SXG_STATS					Stats;				// Statistics
+	struct SXG_STATS				Stats;				// Statistics
 	u32						ReassBufs;			// Number of reassembly buffers
 	// Card Crash Info
 	ushort						CrashLocation;		// Microcode crash location
@@ -708,7 +703,7 @@ typedef struct _adapter_t
 	//	dma_addr_t		PDumpBuffer;		// Physical address
 	//#endif // SXG_FAILURE_DUMP
 
-} adapter_t, *p_adapter_t;
+};
 
 #if SLIC_DUMP_ENABLED
 #define SLIC_DUMP_REQUESTED      1
@@ -721,10 +716,10 @@ typedef struct _adapter_t
  * structure is written out to the card's SRAM when the microcode panic's.
  *
  ****************************************************************************/
-typedef struct _slic_crash_info {
+struct slic_crash_info {
     ushort  cpu_id;
     ushort  crash_pc;
-} slic_crash_info, *p_slic_crash_info;
+};
 
 #define CRASH_INFO_OFFSET   0x155C
 
diff --git a/drivers/staging/sxg/sxg_os.h b/drivers/staging/sxg/sxg_os.h
index 01182689aab..6d3f23fb5e1 100644
--- a/drivers/staging/sxg/sxg_os.h
+++ b/drivers/staging/sxg/sxg_os.h
@@ -44,10 +44,10 @@
 #define FALSE	(0)
 #define TRUE	(1)
 
-typedef struct _LIST_ENTRY {
-	struct _LIST_ENTRY *nle_flink;
-	struct _LIST_ENTRY *nle_blink;
-} list_entry, LIST_ENTRY, *PLIST_ENTRY;
+struct LIST_ENTRY {
+	struct LIST_ENTRY *nle_flink;
+	struct LIST_ENTRY *nle_blink;
+};
 
 #define InitializeListHead(l)                   \
         (l)->nle_flink = (l)->nle_blink = (l)
@@ -68,10 +68,10 @@ typedef struct _LIST_ENTRY {
 
 /* These two have to be inlined since they return things. */
 
-static __inline PLIST_ENTRY RemoveHeadList(list_entry * l)
+static __inline struct LIST_ENTRY *RemoveHeadList(struct LIST_ENTRY *l)
 {
-	list_entry *f;
-	list_entry *e;
+	struct LIST_ENTRY *f;
+	struct LIST_ENTRY *e;
 
 	e = l->nle_flink;
 	f = e->nle_flink;
@@ -81,10 +81,10 @@ static __inline PLIST_ENTRY RemoveHeadList(list_entry * l)
 	return (e);
 }
 
-static __inline PLIST_ENTRY RemoveTailList(list_entry * l)
+static __inline struct LIST_ENTRY *RemoveTailList(struct LIST_ENTRY *l)
 {
-	list_entry *b;
-	list_entry *e;
+	struct LIST_ENTRY *b;
+	struct LIST_ENTRY *e;
 
 	e = l->nle_blink;
 	b = e->nle_blink;
@@ -96,7 +96,7 @@ static __inline PLIST_ENTRY RemoveTailList(list_entry * l)
 
 #define InsertTailList(l, e)                    \
         do {                                    \
-                list_entry              *b;     \
+                struct LIST_ENTRY       *b;     \
                                                 \
                 b = (l)->nle_blink;             \
                 (e)->nle_flink = (l);           \
@@ -107,7 +107,7 @@ static __inline PLIST_ENTRY RemoveTailList(list_entry * l)
 
 #define InsertHeadList(l, e)                    \
         do {                                    \
-                list_entry              *f;     \
+                struct LIST_ENTRY       *f;     \
                                                 \
                 f = (l)->nle_flink;             \
                 (e)->nle_flink = f;             \
diff --git a/drivers/staging/sxg/sxgdbg.h b/drivers/staging/sxg/sxgdbg.h
index 4522b8d7149..bb58ddf39f3 100644
--- a/drivers/staging/sxg/sxgdbg.h
+++ b/drivers/staging/sxg/sxgdbg.h
@@ -86,7 +86,7 @@ extern ulong ATKTimerDiv;
  * needs of the trace entry.  Typically they are function call
  * parameters.
  */
-typedef struct _trace_entry_s {
+struct trace_entry_t {
         char      name[8];        /* 8 character name - like 's'i'm'b'a'r'c'v' */
         u32   time;           /* Current clock tic */
         unsigned char     cpu;            /* Current CPU */
@@ -97,7 +97,7 @@ typedef struct _trace_entry_s {
         u32   arg2;           /* Caller arg2 */
         u32   arg3;           /* Caller arg3 */
         u32   arg4;           /* Caller arg4 */
-} trace_entry_t, *ptrace_entry_t;
+};
 
 /*
  * Driver types for driver field in trace_entry_t
@@ -108,14 +108,13 @@ typedef struct _trace_entry_s {
 
 #define TRACE_ENTRIES   1024
 
-typedef struct _sxg_trace_buffer_t
-{
+struct sxg_trace_buffer_t {
         unsigned int                    size;                  /* aid for windbg extension */
         unsigned int                    in;                    /* Where to add */
         unsigned int                    level;                 /* Current Trace level */
 	spinlock_t	lock;                  /* For MP tracing */
-        trace_entry_t           entries[TRACE_ENTRIES];/* The circular buffer */
-} sxg_trace_buffer_t;
+        struct trace_entry_t           entries[TRACE_ENTRIES];/* The circular buffer */
+};
 
 /*
  * The trace levels
@@ -137,7 +136,7 @@ typedef struct _sxg_trace_buffer_t
 #if ATK_TRACE_ENABLED
 #define SXG_TRACE_INIT(buffer, tlevel)				\
 {								\
-	memset((buffer), 0, sizeof(sxg_trace_buffer_t));	\
+	memset((buffer), 0, sizeof(struct sxg_trace_buffer_t));	\
 	(buffer)->level = (tlevel);				\
 	(buffer)->size = TRACE_ENTRIES;				\
 	spin_lock_init(&(buffer)->lock);			\
@@ -154,7 +153,7 @@ typedef struct _sxg_trace_buffer_t
         if ((buffer) && ((buffer)->level >= (tlevel))) {                      \
                 unsigned int            trace_irql = 0;    /* ?????? FIX THIS  */    \
                 unsigned int            trace_len;                                   \
-                ptrace_entry_t  trace_entry;                                 \
+                struct trace_entry_t	*trace_entry;				\
                 struct timeval  timev;                                       \
                                                                              \
                 spin_lock(&(buffer)->lock);                       \
diff --git a/drivers/staging/sxg/sxghif.h b/drivers/staging/sxg/sxghif.h
index 88bffbaa3be..a4e94685c54 100644
--- a/drivers/staging/sxg/sxghif.h
+++ b/drivers/staging/sxg/sxghif.h
@@ -12,7 +12,7 @@
 /*******************************************************************************
  * UCODE Registers
  *******************************************************************************/
-typedef struct _SXG_UCODE_REGS {
+struct SXG_UCODE_REGS {
 	// Address 0 - 0x3F = Command codes 0-15 for TCB 0.  Excode 0
 	u32 Icr;		// Code = 0 (extended), ExCode = 0 - Int control
 	u32 RsvdReg1;		// Code = 1 - TOE -NA
@@ -127,7 +127,7 @@ typedef struct _SXG_UCODE_REGS {
 	// base.  As extended codes are added, reduce the first array value in
 	// the following field
 	u32 PadToNextCpu[94][16];	// 94 = 128 - 34 (34 = Excodes 0 - 33)
-} SXG_UCODE_REGS, *PSXG_UCODE_REGS;
+};
 
 // Interrupt control register (0) values
 #define SXG_ICR_DISABLE					0x00000000
@@ -169,7 +169,7 @@ typedef struct _SXG_UCODE_REGS {
  * is happening is that these registers occupy the "PadEx[15]" areas in the
  * SXG_UCODE_REGS definition above
  */
-typedef struct _SXG_TCB_REGS {
+struct SXG_TCB_REGS {
 	u32 ExCode;		/* Extended codes - see SXG_UCODE_REGS */
 	u32 Xmt;		/* Code = 1 - # of Xmt descriptors added to ring */
 	u32 Rcv;		/* Code = 2 - # of Rcv descriptors added to ring */
@@ -180,7 +180,7 @@ typedef struct _SXG_TCB_REGS {
 	u32 Rsvd4;		/* Code = 7 - TOE NA */
 	u32 Rsvd5;		/* Code = 8 - TOE NA */
 	u32 Pad[7];		/* Codes 8-15 - Not used. */
-} SXG_TCB_REGS, *PSXG_TCB_REGS;
+};
 
 /***************************************************************************
  * ISR Format
@@ -272,7 +272,7 @@ typedef struct _SXG_TCB_REGS {
  *
  */
 #pragma pack(push, 1)
-typedef struct _SXG_EVENT {
+struct SXG_EVENT {
 	u32 Pad[1];		// not used
 	u32 SndUna;		// SndUna value
 	u32 Resid;		// receive MDL resid
@@ -294,7 +294,7 @@ typedef struct _SXG_EVENT {
 	unsigned char Code;	// Event code
 	unsigned char CommandIndex;	// New ring index
 	unsigned char Status;	// Event status
-} SXG_EVENT, *PSXG_EVENT;
+};
 #pragma pack(pop)
 
 // Event code definitions
@@ -321,9 +321,9 @@ typedef struct _SXG_EVENT {
 #define EVENT_RING_BATCH	16	// Hand entries back 16 at a time.
 #define EVENT_BATCH_LIMIT	256	// Stop processing events after 256 (16 * 16)
 
-typedef struct _SXG_EVENT_RING {
-	SXG_EVENT Ring[EVENT_RING_SIZE];
-} SXG_EVENT_RING, *PSXG_EVENT_RING;
+struct SXG_EVENT_RING {
+	struct SXG_EVENT Ring[EVENT_RING_SIZE];
+};
 
 /***************************************************************************
  *
@@ -400,12 +400,12 @@ typedef struct _SXG_EVENT_RING {
 #define SXG_MAX_ENTRIES     4096
 
 // Structure and macros to manage a ring
-typedef struct _SXG_RING_INFO {
+struct SXG_RING_INFO {
 	unsigned char Head;	// Where we add entries - Note unsigned char:RING_SIZE
 	unsigned char Tail;	// Where we pull off completed entries
 	ushort Size;		// Ring size - Must be multiple of 2
 	void *Context[SXG_MAX_RING_SIZE];	// Shadow ring
-} SXG_RING_INFO, *PSXG_RING_INFO;
+};
 
 #define SXG_INITIALIZE_RING(_ring, _size) {							\
 	(_ring).Head = 0;												\
@@ -481,7 +481,7 @@ typedef struct _SXG_RING_INFO {
  *  |_________|_________|_________|_________|28		0x1c
  */
 #pragma pack(push, 1)
-typedef struct _SXG_CMD {
+struct SXG_CMD {
 	dma_addr_t Sgl;		// Physical address of SGL
 	union {
 		struct {
@@ -518,14 +518,14 @@ typedef struct _SXG_CMD {
 			unsigned char NotUsed;
 		} Status;
 	};
-} SXG_CMD, *PSXG_CMD;
+};
 #pragma pack(pop)
 
 #pragma pack(push, 1)
-typedef struct _VLAN_HDR {
+struct VLAN_HDR {
 	ushort VlanTci;
 	ushort VlanTpid;
-} VLAN_HDR, *PVLAN_HDR;
+};
 #pragma pack(pop)
 
 /*
@@ -564,22 +564,22 @@ typedef struct _VLAN_HDR {
 #define SXG_SLOWCMD_CSUM_TCP		0x02	// Checksum TCP
 #define SXG_SLOWCMD_LSO				0x04	// Large segment send
 
-typedef struct _SXG_XMT_RING {
-	SXG_CMD Descriptors[SXG_XMT_RING_SIZE];
-} SXG_XMT_RING, *PSXG_XMT_RING;
+struct SXG_XMT_RING {
+	struct SXG_CMD Descriptors[SXG_XMT_RING_SIZE];
+};
 
-typedef struct _SXG_RCV_RING {
-	SXG_CMD Descriptors[SXG_RCV_RING_SIZE];
-} SXG_RCV_RING, *PSXG_RCV_RING;
+struct SXG_RCV_RING {
+	struct SXG_CMD Descriptors[SXG_RCV_RING_SIZE];
+};
 
 /***************************************************************************
  * Share memory buffer types - Used to identify asynchronous
  * shared memory allocation
  ***************************************************************************/
-typedef enum {
+enum SXG_BUFFER_TYPE {
 	SXG_BUFFER_TYPE_RCV,	// Receive buffer
 	SXG_BUFFER_TYPE_SGL	// SGL buffer
-} SXG_BUFFER_TYPE;
+};
 
 // State for SXG buffers
 #define SXG_BUFFER_FREE		0x01
@@ -670,19 +670,19 @@ typedef enum {
 #define SXG_MAX_RCV_BLOCKS				128	// = 16384 receive buffers
 
 // Receive buffer header
-typedef struct _SXG_RCV_DATA_BUFFER_HDR {
+struct SXG_RCV_DATA_BUFFER_HDR {
 	dma_addr_t PhysicalAddress;	// Buffer physical address
 	// Note - DO NOT USE the VirtualAddress field to locate data.
 	// Use the sxg.h:SXG_RECEIVE_DATA_LOCATION macro instead.
 	void *VirtualAddress;	// Start of buffer
-	LIST_ENTRY FreeList;	// Free queue of buffers
-	struct _SXG_RCV_DATA_BUFFER_HDR *Next;	// Fastpath data buffer queue
+	struct LIST_ENTRY FreeList;	// Free queue of buffers
+	struct SXG_RCV_DATA_BUFFER_HDR *Next;	// Fastpath data buffer queue
 	u32 Size;		// Buffer size
 	u32 ByteOffset;		// See SXG_RESTORE_MDL_OFFSET
 	unsigned char State;	// See SXG_BUFFER state above
 	unsigned char Status;	// Event status (to log PUSH)
 	struct sk_buff *skb;	// Double mapped (nbl and pkt)
-} SXG_RCV_DATA_BUFFER_HDR, *PSXG_RCV_DATA_BUFFER_HDR;
+};
 
 // SxgSlowReceive uses the PACKET (skb) contained
 // in the SXG_RCV_DATA_BUFFER_HDR when indicating dumb-nic data
@@ -693,42 +693,43 @@ typedef struct _SXG_RCV_DATA_BUFFER_HDR {
 #define SXG_RCV_JUMBO_BUFFER_SIZE		10240	// jumbo = 10k including HDR
 
 // Receive data descriptor
-typedef struct _SXG_RCV_DATA_DESCRIPTOR {
+struct SXG_RCV_DATA_DESCRIPTOR {
 	union {
 		struct sk_buff *VirtualAddress;	// Host handle
 		u64 ForceTo8Bytes;	// Force x86 to 8-byte boundary
 	};
 	dma_addr_t PhysicalAddress;
-} SXG_RCV_DATA_DESCRIPTOR, *PSXG_RCV_DATA_DESCRIPTOR;
+};
 
 // Receive descriptor block
 #define SXG_RCV_DESCRIPTORS_PER_BLOCK		128
 #define SXG_RCV_DESCRIPTOR_BLOCK_SIZE		2048	// For sanity check
-typedef struct _SXG_RCV_DESCRIPTOR_BLOCK {
-	SXG_RCV_DATA_DESCRIPTOR Descriptors[SXG_RCV_DESCRIPTORS_PER_BLOCK];
-} SXG_RCV_DESCRIPTOR_BLOCK, *PSXG_RCV_DESCRIPTOR_BLOCK;
+
+struct SXG_RCV_DESCRIPTOR_BLOCK {
+	struct SXG_RCV_DATA_DESCRIPTOR Descriptors[SXG_RCV_DESCRIPTORS_PER_BLOCK];
+};
 
 // Receive descriptor block header
-typedef struct _SXG_RCV_DESCRIPTOR_BLOCK_HDR {
+struct SXG_RCV_DESCRIPTOR_BLOCK_HDR {
 	void *VirtualAddress;	// Start of 2k buffer
 	dma_addr_t PhysicalAddress;	// ..and it's physical address
-	LIST_ENTRY FreeList;	// Free queue of descriptor blocks
+	struct LIST_ENTRY FreeList;	// Free queue of descriptor blocks
 	unsigned char State;	// See SXG_BUFFER state above
-} SXG_RCV_DESCRIPTOR_BLOCK_HDR, *PSXG_RCV_DESCRIPTOR_BLOCK_HDR;
+};
 
 // Receive block header
-typedef struct _SXG_RCV_BLOCK_HDR {
+struct SXG_RCV_BLOCK_HDR {
 	void *VirtualAddress;	// Start of virtual memory
 	dma_addr_t PhysicalAddress;	// ..and it's physical address
-	LIST_ENTRY AllList;	// Queue of all SXG_RCV_BLOCKS
-} SXG_RCV_BLOCK_HDR, *PSXG_RCV_BLOCK_HDR;
+	struct LIST_ENTRY AllList;	// Queue of all SXG_RCV_BLOCKS
+};
 
 // Macros to determine data structure offsets into receive block
 #define SXG_RCV_BLOCK_SIZE(_Buffersize) 					\
 	(((_Buffersize) * SXG_RCV_DESCRIPTORS_PER_BLOCK) +		\
-	 (sizeof(SXG_RCV_DESCRIPTOR_BLOCK))              +		\
-	 (sizeof(SXG_RCV_DESCRIPTOR_BLOCK_HDR))          +		\
-	 (sizeof(SXG_RCV_BLOCK_HDR)))
+	 (sizeof(struct SXG_RCV_DESCRIPTOR_BLOCK))              +		\
+	 (sizeof(struct SXG_RCV_DESCRIPTOR_BLOCK_HDR))          +		\
+	 (sizeof(struct SXG_RCV_BLOCK_HDR)))
 #define SXG_RCV_BUFFER_DATA_SIZE(_Buffersize)				\
 	((_Buffersize) - SXG_RCV_DATA_HDR_SIZE)
 #define SXG_RCV_DATA_BUFFER_HDR_OFFSET(_Buffersize)			\
@@ -737,18 +738,18 @@ typedef struct _SXG_RCV_BLOCK_HDR {
 	((_Buffersize) * SXG_RCV_DESCRIPTORS_PER_BLOCK)
 #define SXG_RCV_DESCRIPTOR_BLOCK_HDR_OFFSET(_Buffersize)	\
 	(((_Buffersize) * SXG_RCV_DESCRIPTORS_PER_BLOCK) +		\
-	 (sizeof(SXG_RCV_DESCRIPTOR_BLOCK)))
+	 (sizeof(struct SXG_RCV_DESCRIPTOR_BLOCK)))
 #define SXG_RCV_BLOCK_HDR_OFFSET(_Buffersize)				\
 	(((_Buffersize) * SXG_RCV_DESCRIPTORS_PER_BLOCK) +		\
-	 (sizeof(SXG_RCV_DESCRIPTOR_BLOCK))              +		\
-	 (sizeof(SXG_RCV_DESCRIPTOR_BLOCK_HDR)))
+	 (sizeof(struct SXG_RCV_DESCRIPTOR_BLOCK))              +		\
+	 (sizeof(struct SXG_RCV_DESCRIPTOR_BLOCK_HDR)))
 
 // Use the miniport reserved portion of the NBL to locate
 // our SXG_RCV_DATA_BUFFER_HDR structure.
-typedef struct _SXG_RCV_NBL_RESERVED {
-	PSXG_RCV_DATA_BUFFER_HDR RcvDataBufferHdr;
+struct SXG_RCV_NBL_RESERVED {
+	struct SXG_RCV_DATA_BUFFER_HDR *RcvDataBufferHdr;
 	void *Available;
-} SXG_RCV_NBL_RESERVED, *PSXG_RCV_NBL_RESERVED;
+};
 
 #define SXG_RCV_NBL_BUFFER_HDR(_NBL) (((PSXG_RCV_NBL_RESERVED)NET_BUFFER_LIST_MINIPORT_RESERVED(_NBL))->RcvDataBufferHdr)
 
@@ -760,11 +761,11 @@ typedef struct _SXG_RCV_NBL_RESERVED {
 #define SXG_MAX_SGL_BUFFERS			16384	// Maximum to allocate (note ADAPT:ushort)
 
 // Self identifying structure type
-typedef enum _SXG_SGL_TYPE {
+enum SXG_SGL_TYPE {
 	SXG_SGL_DUMB,		// Dumb NIC SGL
 	SXG_SGL_SLOW,		// Slowpath protocol header - see below
 	SXG_SGL_CHIMNEY		// Chimney offload SGL
-} SXG_SGL_TYPE, PSXG_SGL_TYPE;
+};
 
 // Note - the description below is Microsoft specific
 //
@@ -798,41 +799,41 @@ typedef enum _SXG_SGL_TYPE {
 // to the card directly.  For x86 systems we must reconstruct
 // the SGL.  The following structure defines an x64
 // formatted SGL entry
-typedef struct _SXG_X64_SGE {
+struct SXG_X64_SGE {
 	dma64_addr_t Address;	// same as wdm.h
 	u32 Length;		// same as wdm.h
 	u32 CompilerPad;	// The compiler pads to 8-bytes
 	u64 Reserved;		// u32 * in wdm.h.  Force to 8 bytes
-} SXG_X64_SGE, *PSXG_X64_SGE;
+};
 
-typedef struct _SCATTER_GATHER_ELEMENT {
+struct SCATTER_GATHER_ELEMENT {
 	dma64_addr_t Address;	// same as wdm.h
 	u32 Length;		// same as wdm.h
 	u32 CompilerPad;	// The compiler pads to 8-bytes
 	u64 Reserved;		// u32 * in wdm.h.  Force to 8 bytes
-} SCATTER_GATHER_ELEMENT, *PSCATTER_GATHER_ELEMENT;
+};
 
-typedef struct _SCATTER_GATHER_LIST {
+struct SCATTER_GATHER_LIST {
 	u32 NumberOfElements;
 	u32 *Reserved;
-	SCATTER_GATHER_ELEMENT Elements[];
-} SCATTER_GATHER_LIST, *PSCATTER_GATHER_LIST;
+	struct SCATTER_GATHER_ELEMENT Elements[];
+};
 
 // The card doesn't care about anything except elements, so
 // we can leave the u32 * reserved field alone in the following
 // SGL structure.  But redefine from wdm.h:SCATTER_GATHER_LIST so
 // we can specify SXG_X64_SGE and define a fixed number of elements
-typedef struct _SXG_X64_SGL {
+struct SXG_X64_SGL {
 	u32 NumberOfElements;
 	u32 *Reserved;
-	SXG_X64_SGE Elements[SXG_SGL_ENTRIES];
-} SXG_X64_SGL, *PSXG_X64_SGL;
+	struct SXG_X64_SGE Elements[SXG_SGL_ENTRIES];
+};
 
-typedef struct _SXG_SCATTER_GATHER {
-	SXG_SGL_TYPE Type;	// FIRST! Dumb-nic or offload
+struct SXG_SCATTER_GATHER {
+	enum SXG_SGL_TYPE Type;	// FIRST! Dumb-nic or offload
 	void *adapter;		// Back pointer to adapter
-	LIST_ENTRY FreeList;	// Free SXG_SCATTER_GATHER blocks
-	LIST_ENTRY AllList;	// All SXG_SCATTER_GATHER blocks
+	struct LIST_ENTRY FreeList;	// Free SXG_SCATTER_GATHER blocks
+	struct LIST_ENTRY AllList;	// All SXG_SCATTER_GATHER blocks
 	dma_addr_t PhysicalAddress;	// physical address
 	unsigned char State;	// See SXG_BUFFER state above
 	unsigned char CmdIndex;	// Command ring index
@@ -840,18 +841,18 @@ typedef struct _SXG_SCATTER_GATHER {
 	u32 Direction;		// For asynchronous completions
 	u32 CurOffset;		// Current SGL offset
 	u32 SglRef;		// SGL reference count
-	VLAN_HDR VlanTag;	// VLAN tag to be inserted into SGL
-	PSCATTER_GATHER_LIST pSgl;	// SGL Addr. Possibly &Sgl
-	SXG_X64_SGL Sgl;	// SGL handed to card
-} SXG_SCATTER_GATHER, *PSXG_SCATTER_GATHER;
+	struct VLAN_HDR VlanTag;	// VLAN tag to be inserted into SGL
+	struct SCATTER_GATHER_LIST *pSgl;	// SGL Addr. Possibly &Sgl
+	struct SXG_X64_SGL Sgl;	// SGL handed to card
+};
 
 #if defined(CONFIG_X86_64)
 #define SXG_SGL_BUFFER(_SxgSgl)		(&_SxgSgl->Sgl)
-#define SXG_SGL_BUF_SIZE			sizeof(SXG_X64_SGL)
+#define SXG_SGL_BUF_SIZE			sizeof(struct SXG_X64_SGL)
 #elif defined(CONFIG_X86)
 // Force NDIS to give us it's own buffer so we can reformat to our own
 #define SXG_SGL_BUFFER(_SxgSgl)		NULL
 #define SXG_SGL_BUF_SIZE			0
 #else
-Stop Compilation;
+#error staging: sxg: driver is for X86 only!
 #endif
diff --git a/drivers/staging/sxg/sxghw.h b/drivers/staging/sxg/sxghw.h
index 2222ae91fd9..b0efff9ff11 100644
--- a/drivers/staging/sxg/sxghw.h
+++ b/drivers/staging/sxg/sxghw.h
@@ -48,7 +48,7 @@
 #define SXG_HWREG_MEMSIZE	0x4000		// 16k
 
 #pragma pack(push, 1)
-typedef struct _SXG_HW_REGS {
+struct SXG_HW_REGS {
 	u32		Reset;				// Write 0xdead to invoke soft reset
 	u32		Pad1;				// No register defined at offset 4
 	u32		InterruptMask0;		// Deassert legacy interrupt on function 0
@@ -113,7 +113,7 @@ typedef struct _SXG_HW_REGS {
 	u32		Software[1920];		// 0x200 - 0x2000 - Software defined (not used)
 	u32		MsixTable[1024];	// 0x2000 - 0x3000 - MSIX Table
 	u32		MsixBitArray[1024];	// 0x3000 - 0x4000 - MSIX Pending Bit Array
-} SXG_HW_REGS, *PSXG_HW_REGS;
+};
 #pragma pack(pop)
 
 // Microcode Address Flags
@@ -519,10 +519,10 @@ typedef struct _SXG_HW_REGS {
 #define	XS_LANE_ALIGN			0x1000			// XS transmit lanes aligned
 
 // PHY Microcode download data structure
-typedef struct _PHY_UCODE {
+struct PHY_UCODE {
 	ushort	Addr;
 	ushort	Data;
-} PHY_UCODE, *PPHY_UCODE;
+};
 
 
 /*****************************************************************************
@@ -537,7 +537,7 @@ typedef struct _PHY_UCODE {
 // all commands - see the Sahara spec for details.  Note that this structure is
 // only valid when compiled on a little endian machine.
 #pragma pack(push, 1)
-typedef struct _XMT_DESC {
+struct XMT_DESC {
 	ushort	XmtLen;			// word 0, bits [15:0] -  transmit length
 	unsigned char	XmtCtl;			// word 0, bits [23:16] - transmit control byte
 	unsigned char	Cmd;			// word 0, bits [31:24] - transmit command plus misc.
@@ -551,7 +551,7 @@ typedef struct _XMT_DESC {
 	u32	Rsvd3;			// word 5, bits [31:0] -  PAD
 	u32	Rsvd4;		    // word 6, bits [31:0] -  PAD
 	u32	Rsvd5;		    // word 7, bits [31:0] -  PAD
-} XMT_DESC, *PXMT_DESC;
+};
 #pragma pack(pop)
 
 // XMT_DESC Cmd byte definitions
@@ -600,7 +600,7 @@ typedef struct _XMT_DESC {
 // Format of the 18 byte Receive Buffer returned by the
 // Receive Sequencer for received packets
 #pragma pack(push, 1)
-typedef struct _RCV_BUF_HDR {
+struct RCV_BUF_HDR {
 	u32	Status;				// Status word from Rcv Seq Parser
 	ushort	Length;				// Rcv packet byte count
 	union {
@@ -615,7 +615,7 @@ typedef struct _RCV_BUF_HDR {
 	unsigned char	IpHdrOffset;		// IP header offset into packet
 	u32	TpzHash;			// Toeplitz hash
 	ushort	Reserved;			// Reserved
-} RCV_BUF_HDR, *PRCV_BUF_HDR;
+};
 #pragma pack(pop)
 
 
@@ -665,28 +665,28 @@ typedef struct _RCV_BUF_HDR {
 #pragma pack(push, 1)
 
 /* */
-typedef struct _HW_CFG_DATA {
+struct HW_CFG_DATA {
 	ushort		Addr;
 	union {
 		ushort	Data;
 		ushort	Checksum;
 	};
-} HW_CFG_DATA, *PHW_CFG_DATA;
+};
 
 /* */
-#define	NUM_HW_CFG_ENTRIES	((128/sizeof(HW_CFG_DATA)) - 4)
+#define	NUM_HW_CFG_ENTRIES	((128/sizeof(struct HW_CFG_DATA)) - 4)
 
 /* MAC address */
-typedef struct _SXG_CONFIG_MAC {
+struct SXG_CONFIG_MAC {
 	unsigned char		MacAddr[6];			/* MAC Address */
-} SXG_CONFIG_MAC, *PSXG_CONFIG_MAC;
+};
 
 /* */
-typedef struct _ATK_FRU {
+struct ATK_FRU {
 	unsigned char		PartNum[6];
 	unsigned char		Revision[2];
 	unsigned char		Serial[14];
-} ATK_FRU, *PATK_FRU;
+};
 
 /* OEM FRU Format types */
 #define	ATK_FRU_FORMAT		0x0000
@@ -698,24 +698,24 @@ typedef struct _ATK_FRU {
 #define NO_FRU_FORMAT		0xFFFF
 
 /* EEPROM/Flash Format */
-typedef struct _SXG_CONFIG {
+struct SXG_CONFIG {
 	/* */
 	/* Section 1 (128 bytes) */
 	/* */
 	ushort			MagicWord;			/* EEPROM/FLASH Magic code 'A5A5' */
 	ushort			SpiClks;			/* SPI bus clock dividers */
-	HW_CFG_DATA		HwCfg[NUM_HW_CFG_ENTRIES];
+	struct HW_CFG_DATA		HwCfg[NUM_HW_CFG_ENTRIES];
 	/* */
 	/* */
 	/* */
 	ushort			Version;			/* EEPROM format version */
-	SXG_CONFIG_MAC	MacAddr[4];			/* space for 4 MAC addresses */
-	ATK_FRU			AtkFru;				/* FRU information */
+	struct SXG_CONFIG_MAC	MacAddr[4];			/* space for 4 MAC addresses */
+	struct ATK_FRU			AtkFru;				/* FRU information */
 	ushort			OemFruFormat;		/* OEM FRU format type */
 	unsigned char			OemFru[76];			/* OEM FRU information (optional) */
 	ushort			Checksum;			/* Checksum of section 2 */
 	/* CS info XXXTODO */
-} SXG_CONFIG, *PSXG_CONFIG;
+};
 #pragma pack(pop)
 
 /*****************************************************************************
diff --git a/drivers/staging/sxg/sxgphycode.h b/drivers/staging/sxg/sxgphycode.h
index 8dbaeda7eca..167f356ef86 100644
--- a/drivers/staging/sxg/sxgphycode.h
+++ b/drivers/staging/sxg/sxgphycode.h
@@ -18,7 +18,7 @@
 /*
  * Download for AEL2005C PHY with SR/LR transceiver (10GBASE-SR or 10GBASE-LR)
  */
-static PHY_UCODE PhyUcode[] = {
+static struct PHY_UCODE PhyUcode[] = {
 	/*
 	 * NOTE:  An address of 0 is a special case.  When the download routine
 	 * sees an address of 0, it does not write to the PHY.  Instead, it
diff --git a/drivers/staging/usbip/stub.h b/drivers/staging/usbip/stub.h
index f541a3a83bd..022d0649ac5 100644
--- a/drivers/staging/usbip/stub.h
+++ b/drivers/staging/usbip/stub.h
@@ -91,5 +91,5 @@ void stub_rx_loop(struct usbip_task *);
 void stub_enqueue_ret_unlink(struct stub_device *, __u32, __u32);
 
 /* stub_main.c */
-int match_busid(char *busid);
+int match_busid(const char *busid);
 void stub_device_cleanup_urbs(struct stub_device *sdev);
diff --git a/drivers/staging/usbip/stub_dev.c b/drivers/staging/usbip/stub_dev.c
index ee455a087ea..1e320caf3d1 100644
--- a/drivers/staging/usbip/stub_dev.c
+++ b/drivers/staging/usbip/stub_dev.c
@@ -389,7 +389,7 @@ static int stub_probe(struct usb_interface *interface,
 {
 	struct usb_device *udev = interface_to_usbdev(interface);
 	struct stub_device *sdev = NULL;
-	char *udev_busid = interface->dev.parent->bus_id;
+	const char *udev_busid = dev_name(interface->dev.parent);
 	int err = 0;
 
 	dev_dbg(&interface->dev, "Enter\n");
diff --git a/drivers/staging/usbip/stub_main.c b/drivers/staging/usbip/stub_main.c
index c665d7f1ca9..05e4c606439 100644
--- a/drivers/staging/usbip/stub_main.c
+++ b/drivers/staging/usbip/stub_main.c
@@ -40,11 +40,12 @@ struct kmem_cache *stub_priv_cache;
  * remote host.
  */
 #define MAX_BUSID 16
-static char busid_table[MAX_BUSID][BUS_ID_SIZE];
+#define BUSID_SIZE 20
+static char busid_table[MAX_BUSID][BUSID_SIZE];
 static spinlock_t busid_table_lock;
 
 
-int match_busid(char *busid)
+int match_busid(const char *busid)
 {
 	int i;
 
@@ -52,7 +53,7 @@ int match_busid(char *busid)
 
 	for (i = 0; i < MAX_BUSID; i++)
 		if (busid_table[i][0])
-			if (!strncmp(busid_table[i], busid, BUS_ID_SIZE)) {
+			if (!strncmp(busid_table[i], busid, BUSID_SIZE)) {
 				/* already registerd */
 				spin_unlock(&busid_table_lock);
 				return 0;
@@ -92,7 +93,7 @@ static int add_match_busid(char *busid)
 
 	for (i = 0; i < MAX_BUSID; i++)
 		if (!busid_table[i][0]) {
-			strncpy(busid_table[i], busid, BUS_ID_SIZE);
+			strncpy(busid_table[i], busid, BUSID_SIZE);
 			spin_unlock(&busid_table_lock);
 			return 0;
 		}
@@ -109,9 +110,9 @@ static int del_match_busid(char *busid)
 	spin_lock(&busid_table_lock);
 
 	for (i = 0; i < MAX_BUSID; i++)
-		if (!strncmp(busid_table[i], busid, BUS_ID_SIZE)) {
+		if (!strncmp(busid_table[i], busid, BUSID_SIZE)) {
 			/* found */
-			memset(busid_table[i], 0, BUS_ID_SIZE);
+			memset(busid_table[i], 0, BUSID_SIZE);
 			spin_unlock(&busid_table_lock);
 			return 0;
 		}
@@ -125,19 +126,19 @@ static ssize_t store_match_busid(struct device_driver *dev, const char *buf,
 		size_t count)
 {
 	int len;
-	char busid[BUS_ID_SIZE];
+	char busid[BUSID_SIZE];
 
 	if (count < 5)
 		return -EINVAL;
 
 	/* strnlen() does not include \0 */
-	len = strnlen(buf + 4, BUS_ID_SIZE);
+	len = strnlen(buf + 4, BUSID_SIZE);
 
 	/* busid needs to include \0 termination */
-	if (!(len < BUS_ID_SIZE))
+	if (!(len < BUSID_SIZE))
 		return -EINVAL;
 
-	strncpy(busid, buf + 4, BUS_ID_SIZE);
+	strncpy(busid, buf + 4, BUSID_SIZE);
 
 
 	if (!strncmp(buf, "add ", 4)) {
diff --git a/drivers/staging/usbip/stub_rx.c b/drivers/staging/usbip/stub_rx.c
index 36ce898fced..2eb61372fe0 100644
--- a/drivers/staging/usbip/stub_rx.c
+++ b/drivers/staging/usbip/stub_rx.c
@@ -157,7 +157,7 @@ static int tweak_set_configuration_cmd(struct urb *urb)
 	 * A user may need to set a special configuration value before
 	 * exporting the device.
 	 */
-	uinfo("set_configuration (%d) to %s\n", config, urb->dev->dev.bus_id);
+	uinfo("set_configuration (%d) to %s\n", config, dev_name(&urb->dev->dev));
 	uinfo("but, skip!\n");
 
 	return 0;
@@ -175,7 +175,7 @@ static int tweak_reset_device_cmd(struct urb *urb)
 	value = le16_to_cpu(req->wValue);
 	index = le16_to_cpu(req->wIndex);
 
-	uinfo("reset_device (port %d) to %s\n", index, urb->dev->dev.bus_id);
+	uinfo("reset_device (port %d) to %s\n", index, dev_name(&urb->dev->dev));
 
 	/* all interfaces should be owned by usbip driver, so just reset it.  */
 	ret = usb_lock_device_for_reset(urb->dev, NULL);
@@ -234,8 +234,6 @@ static void tweak_special_requests(struct urb *urb)
 static int stub_recv_cmd_unlink(struct stub_device *sdev,
 						struct usbip_header *pdu)
 {
-	struct list_head *listhead = &sdev->priv_init;
-	struct list_head *ptr;
 	unsigned long flags;
 
 	struct stub_priv *priv;
@@ -243,8 +241,7 @@ static int stub_recv_cmd_unlink(struct stub_device *sdev,
 
 	spin_lock_irqsave(&sdev->priv_lock, flags);
 
-	for (ptr = listhead->next; ptr != listhead; ptr = ptr->next) {
-		priv = list_entry(ptr, struct stub_priv, list);
+	list_for_each_entry(priv, &sdev->priv_init, list) {
 		if (priv->seqnum == pdu->u.cmd_unlink.seqnum) {
 			int ret;
 
diff --git a/drivers/staging/usbip/stub_tx.c b/drivers/staging/usbip/stub_tx.c
index d5563cd980b..78058f5362b 100644
--- a/drivers/staging/usbip/stub_tx.c
+++ b/drivers/staging/usbip/stub_tx.c
@@ -54,7 +54,6 @@ void stub_enqueue_ret_unlink(struct stub_device *sdev, __u32 seqnum,
 /**
  * stub_complete - completion handler of a usbip urb
  * @urb: pointer to the urb completed
- * @regs:
  *
  * When a urb has completed, the USB core driver calls this function mostly in
  * the interrupt context. To return the result of a urb, the completed urb is
diff --git a/drivers/staging/usbip/vhci_hcd.c b/drivers/staging/usbip/vhci_hcd.c
index 5b5a2e348ec..f69ca346aa2 100644
--- a/drivers/staging/usbip/vhci_hcd.c
+++ b/drivers/staging/usbip/vhci_hcd.c
@@ -1091,7 +1091,7 @@ static int vhci_hcd_probe(struct platform_device *pdev)
 	 * Allocate and initialize hcd.
 	 * Our private data is also allocated automatically.
 	 */
-	hcd = usb_create_hcd(&vhci_hc_driver, &pdev->dev, pdev->dev.bus_id);
+	hcd = usb_create_hcd(&vhci_hc_driver, &pdev->dev, dev_name(&pdev->dev));
 	if (!hcd) {
 		uerr("create hcd failed\n");
 		return -ENOMEM;
diff --git a/drivers/staging/usbip/vhci_sysfs.c b/drivers/staging/usbip/vhci_sysfs.c
index 24c2851a8f8..0fd33a62d93 100644
--- a/drivers/staging/usbip/vhci_sysfs.c
+++ b/drivers/staging/usbip/vhci_sysfs.c
@@ -60,7 +60,7 @@ static ssize_t show_status(struct device *dev, struct device_attribute *attr,
 			out += sprintf(out, "%03u %08x ",
 					vdev->speed, vdev->devid);
 			out += sprintf(out, "%16p ", vdev->ud.tcp_socket);
-			out += sprintf(out, "%s", vdev->udev->dev.bus_id);
+			out += sprintf(out, "%s", dev_name(&vdev->udev->dev));
 
 		} else
 			out += sprintf(out, "000 000 000 0000000000000000 0-0");
diff --git a/drivers/staging/winbond/Kconfig b/drivers/staging/winbond/Kconfig
index 425219ed7ab..940460c39f3 100644
--- a/drivers/staging/winbond/Kconfig
+++ b/drivers/staging/winbond/Kconfig
@@ -1,7 +1,11 @@
 config W35UND
-	tristate "Winbond driver"
-	depends on MAC80211 && WLAN_80211 && USB && EXPERIMENTAL && !4KSTACKS
+	tristate "IS89C35 WLAN USB driver"
+	depends on MAC80211 && WLAN_80211 && USB && EXPERIMENTAL
 	default n
 	---help---
-	  This is highly experimental driver for winbond wifi card on some Kohjinsha notebooks
-	  Check http://code.google.com/p/winbondport/ for new version
+	  This is highly experimental driver for Winbond WIFI card.
+
+	  Hardware is present in some Kohjinsha subnotebooks, and in some
+	  stand-alone USB modules. Chipset name seems to be w89c35d.
+
+	  Check http://code.google.com/p/winbondport/ for new version.
diff --git a/drivers/staging/winbond/Makefile b/drivers/staging/winbond/Makefile
index 29c98bf1bc9..b49c9730edd 100644
--- a/drivers/staging/winbond/Makefile
+++ b/drivers/staging/winbond/Makefile
@@ -1,15 +1,14 @@
-	DRIVER_DIR=./linux
-
-w35und-objs := $(DRIVER_DIR)/wbusb.o $(DRIVER_DIR)/wb35reg.o $(DRIVER_DIR)/wb35rx.o $(DRIVER_DIR)/wb35tx.o \
-	mds.o \
-	mlmetxrx.o \
-	mto.o	\
+w35und-objs :=			\
+	mds.o			\
+	mlmetxrx.o		\
+	mto.o			\
 	phy_calibration.o	\
 	reg.o			\
-	rxisr.o			\
-	sme_api.o		\
+	wb35reg.o		\
+	wb35rx.o		\
+	wb35tx.o		\
 	wbhal.o			\
-	wblinux.o		\
+	wbusb.o			\
 
 
 obj-$(CONFIG_W35UND) += w35und.o
diff --git a/drivers/staging/winbond/adapter.h b/drivers/staging/winbond/adapter.h
deleted file mode 100644
index 609701d21cf..00000000000
--- a/drivers/staging/winbond/adapter.h
+++ /dev/null
@@ -1,23 +0,0 @@
-//
-// ADAPTER.H -
-// Windows NDIS global variable 'Adapter' typedef
-//
-#define MAX_ANSI_STRING		40
-typedef struct WB32_ADAPTER
-{
-	u32 AdapterIndex; // 20060703.4 Add for using pAdapterContext global Adapter point
-
-	WB_LOCALDESCRIPT	sLocalPara;		// Myself connected parameters
-	PWB_BSSDESCRIPTION	asBSSDescriptElement;
-
-	MLME_FRAME		sMlmeFrame;		// connect to peerSTA parameters
-
-	MTO_PARAMETERS		sMtoPara; // MTO_struct ...
-	hw_data_t			sHwData; //For HAL
-	MDS					Mds;
-
-	WBLINUX		WbLinux;
-        struct iw_statistics iw_stats;
-
-	u8	LinkName[MAX_ANSI_STRING];
-} WB32_ADAPTER, ADAPTER, *PWB32_ADAPTER, *PADAPTER;
diff --git a/drivers/staging/winbond/bss_f.h b/drivers/staging/winbond/bss_f.h
index 01318315399..a433b5a8592 100644
--- a/drivers/staging/winbond/bss_f.h
+++ b/drivers/staging/winbond/bss_f.h
@@ -1,59 +1,63 @@
+#ifndef __WINBOND_BSS_F_H
+#define __WINBOND_BSS_F_H
+
+#include "core.h"
+
+struct PMKID_Information_Element;
+
 //
 // BSS descriptor DataBase management global function
 //
 
-void vBSSdescriptionInit(PWB32_ADAPTER Adapter);
-void vBSSfoundList(PWB32_ADAPTER Adapter);
-u8 boChanFilter(PWB32_ADAPTER Adapter, u8 ChanNo);
-u16 wBSSallocateEntry(PWB32_ADAPTER Adapter);
-u16 wBSSGetEntry(PWB32_ADAPTER Adapter);
-void vSimpleHouseKeeping(PWB32_ADAPTER Adapter);
-u16 wBSShouseKeeping(PWB32_ADAPTER Adapter);
-void ClearBSSdescpt(PWB32_ADAPTER Adapter, u16 i);
-u16 wBSSfindBssID(PWB32_ADAPTER Adapter, u8 *pbBssid);
-u16 wBSSfindDedicateCandidate(PWB32_ADAPTER Adapter, struct SSID_Element *psSsid, u8 *pbBssid);
-u16 wBSSfindMACaddr(PWB32_ADAPTER Adapter, u8 *pbMacAddr);
-u16 wBSSsearchMACaddr(PWB32_ADAPTER Adapter, u8 *pbMacAddr, u8 band);
-u16 wBSSaddScanData(PWB32_ADAPTER, u16, psRXDATA);
-u16 wBSSUpdateScanData(PWB32_ADAPTER Adapter, u16 wBssIdx, psRXDATA psRcvData);
-u16 wBSScreateIBSSdata(PWB32_ADAPTER Adapter, PWB_BSSDESCRIPTION psDesData);
-void DesiredRate2BSSdescriptor(PWB32_ADAPTER Adapter, PWB_BSSDESCRIPTION psDesData,
+void vBSSdescriptionInit(struct wbsoft_priv * adapter);
+void vBSSfoundList(struct wbsoft_priv * adapter);
+u8 boChanFilter(struct wbsoft_priv * adapter, u8 ChanNo);
+u16 wBSSallocateEntry(struct wbsoft_priv * adapter);
+u16 wBSSGetEntry(struct wbsoft_priv * adapter);
+void vSimpleHouseKeeping(struct wbsoft_priv * adapter);
+u16 wBSShouseKeeping(struct wbsoft_priv * adapter);
+void ClearBSSdescpt(struct wbsoft_priv * adapter, u16 i);
+u16 wBSSfindBssID(struct wbsoft_priv * adapter, u8 *pbBssid);
+u16 wBSSfindDedicateCandidate(struct wbsoft_priv * adapter, struct SSID_Element *psSsid, u8 *pbBssid);
+u16 wBSSfindMACaddr(struct wbsoft_priv * adapter, u8 *pbMacAddr);
+u16 wBSSsearchMACaddr(struct wbsoft_priv * adapter, u8 *pbMacAddr, u8 band);
+u16 wBSSaddScanData(struct wbsoft_priv *, u16, psRXDATA);
+u16 wBSSUpdateScanData(struct wbsoft_priv * adapter, u16 wBssIdx, psRXDATA psRcvData);
+u16 wBSScreateIBSSdata(struct wbsoft_priv * adapter, PWB_BSSDESCRIPTION psDesData);
+void DesiredRate2BSSdescriptor(struct wbsoft_priv * adapter, PWB_BSSDESCRIPTION psDesData,
 							 u8 *pBasicRateSet, u8 BasicRateCount,
 							 u8 *pOperationRateSet, u8 OperationRateCount);
-void DesiredRate2InfoElement(PWB32_ADAPTER Adapter, u8	*addr, u16 *iFildOffset,
+void DesiredRate2InfoElement(struct wbsoft_priv * adapter, u8	*addr, u16 *iFildOffset,
 							 u8 *pBasicRateSet, u8 BasicRateCount,
 							 u8 *pOperationRateSet, u8 OperationRateCount);
-void BSSAddIBSSdata(PWB32_ADAPTER Adapter, PWB_BSSDESCRIPTION psDesData);
+void BSSAddIBSSdata(struct wbsoft_priv * adapter, PWB_BSSDESCRIPTION psDesData);
 unsigned char boCmpMacAddr( u8 *, u8 *);
 unsigned char boCmpSSID(struct SSID_Element *psSSID1, struct SSID_Element *psSSID2);
-u16 wBSSfindSSID(PWB32_ADAPTER Adapter, struct SSID_Element *psSsid);
-u16 wRoamingQuery(PWB32_ADAPTER Adapter);
-void vRateToBitmap(PWB32_ADAPTER Adapter, u16 index);
-u8 bRateToBitmapIndex(PWB32_ADAPTER Adapter, u8 bRate);
+u16 wBSSfindSSID(struct wbsoft_priv * adapter, struct SSID_Element *psSsid);
+u16 wRoamingQuery(struct wbsoft_priv * adapter);
+void vRateToBitmap(struct wbsoft_priv * adapter, u16 index);
+u8 bRateToBitmapIndex(struct wbsoft_priv * adapter, u8 bRate);
 u8 bBitmapToRate(u8 i);
-unsigned char boIsERPsta(PWB32_ADAPTER Adapter, u16 i);
-unsigned char boCheckConnect(PWB32_ADAPTER Adapter);
-unsigned char boCheckSignal(PWB32_ADAPTER Adapter);
-void AddIBSSIe(PWB32_ADAPTER Adapter,PWB_BSSDESCRIPTION psDesData );//added by ws for WPA_None06/01/04
-void BssScanUpToDate(PWB32_ADAPTER Adapter);
-void BssUpToDate(PWB32_ADAPTER Adapter);
+unsigned char boIsERPsta(struct wbsoft_priv * adapter, u16 i);
+unsigned char boCheckConnect(struct wbsoft_priv * adapter);
+unsigned char boCheckSignal(struct wbsoft_priv * adapter);
+void AddIBSSIe(struct wbsoft_priv * adapter,PWB_BSSDESCRIPTION psDesData );//added by ws for WPA_None06/01/04
+void BssScanUpToDate(struct wbsoft_priv * adapter);
+void BssUpToDate(struct wbsoft_priv * adapter);
 void RateSort(u8 *RateArray, u8 num, u8 mode);
-void RateReSortForSRate(PWB32_ADAPTER Adapter, u8 *RateArray, u8 num);
-void Assemble_IE(PWB32_ADAPTER Adapter, u16 wBssIdx);
-void SetMaxTxRate(PWB32_ADAPTER Adapter);
+void RateReSortForSRate(struct wbsoft_priv * adapter, u8 *RateArray, u8 num);
+void Assemble_IE(struct wbsoft_priv * adapter, u16 wBssIdx);
+void SetMaxTxRate(struct wbsoft_priv * adapter);
 
-void CreateWpaIE(PWB32_ADAPTER Adapter, u16* iFildOffset, u8 *msg, struct  Management_Frame* msgHeader,
+void CreateWpaIE(struct wbsoft_priv * adapter, u16* iFildOffset, u8 *msg, struct  Management_Frame* msgHeader,
 				 struct Association_Request_Frame_Body* msgBody, u16 iMSindex); //added by WS 05/14/05
 
 #ifdef _WPA2_
-void CreateRsnIE(PWB32_ADAPTER Adapter, u16* iFildOffset, u8 *msg, struct  Management_Frame* msgHeader,
+void CreateRsnIE(struct wbsoft_priv * adapter, u16* iFildOffset, u8 *msg, struct  Management_Frame* msgHeader,
 				 struct Association_Request_Frame_Body* msgBody, u16 iMSindex);//added by WS 05/14/05
 
-u16 SearchPmkid(PWB32_ADAPTER Adapter, struct  Management_Frame* msgHeader,
+u16 SearchPmkid(struct wbsoft_priv * adapter, struct  Management_Frame* msgHeader,
 				   struct PMKID_Information_Element * AssoReq_PMKID );
 #endif
 
-
-
-
-
+#endif
diff --git a/drivers/staging/winbond/bssdscpt.h b/drivers/staging/winbond/bssdscpt.h
index 97150a2655f..3a71d4efb89 100644
--- a/drivers/staging/winbond/bssdscpt.h
+++ b/drivers/staging/winbond/bssdscpt.h
@@ -1,3 +1,11 @@
+#ifndef __WINBOND_BSSDSCPT_H
+#define __WINBOND_BSSDSCPT_H
+
+#include <linux/types.h>
+
+#include "mds_s.h"
+#include "mlme_s.h"
+
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //	bssdscpt.c
 //		BSS descriptor data base
@@ -78,8 +86,8 @@ typedef struct BSSDescriptionElement
     u16		wState;			// the current state of the system
 	u16		wIndex;			// THIS BSS element entry index
 
-	void*	psAdapter;		// pointer to THIS Adapter
-	OS_TIMER	nTimer;  // MLME timer
+	void*	psadapter;		// pointer to THIS adapter
+	struct timer_list timer;  // MLME timer
 
     // Authentication
     u16		wAuthAlgo;      // peer MAC MLME use Auth algorithm, default OPEN_AUTH
@@ -148,9 +156,9 @@ typedef struct BSSDescriptionElement
 
 } WB_BSSDESCRIPTION, *PWB_BSSDESCRIPTION;
 
-#define wBSSConnectedSTA(Adapter)             \
-    ((u16)(Adapter)->sLocalPara.wConnectedSTAindex)
-
-#define psBSS(i)			(&(Adapter->asBSSDescriptElement[(i)]))
+#define wBSSConnectedSTA(adapter)             \
+    ((u16)(adapter)->sLocalPara.wConnectedSTAindex)
 
+#define psBSS(i)			(&(adapter->asBSSDescriptElement[(i)]))
 
+#endif
diff --git a/drivers/staging/winbond/common.h b/drivers/staging/winbond/common.h
new file mode 100644
index 00000000000..c4d96041e9a
--- /dev/null
+++ b/drivers/staging/winbond/common.h
@@ -0,0 +1,27 @@
+//
+// common.h
+//
+// This file contains the OS dependant definition and function.
+// Every OS has this file individual.
+//
+
+#define DebugUsbdStatusInformation( _A )
+
+#ifndef COMMON_DEF
+#define COMMON_DEF
+
+//#define DEBUG_ENABLED  1
+
+//==================================================================================================
+// Common function definition
+//==================================================================================================
+#define DEBUG_ENABLED
+#define ETH_LENGTH_OF_ADDRESS	6
+#ifdef DEBUG_ENABLED
+#define WBDEBUG( _M )	printk _M
+#else
+#define WBDEBUG( _M )	0
+#endif
+
+#endif // COMMON_DEF
+
diff --git a/drivers/staging/winbond/core.h b/drivers/staging/winbond/core.h
new file mode 100644
index 00000000000..fe142a10ea8
--- /dev/null
+++ b/drivers/staging/winbond/core.h
@@ -0,0 +1,42 @@
+#ifndef __WINBOND_CORE_H
+#define __WINBOND_CORE_H
+
+#include <linux/wireless.h>
+
+#include "bssdscpt.h"
+#include "mto.h"
+#include "wbhal_s.h"
+
+#define WBLINUX_PACKET_ARRAY_SIZE (ETHERNET_TX_DESCRIPTORS*4)
+
+#define WB_MAX_LINK_NAME_LEN 40
+
+struct wbsoft_priv {
+	u32 adapterIndex;	// 20060703.4 Add for using padapterContext global adapter point
+
+	WB_LOCALDESCRIPT sLocalPara;	// Myself connected parameters
+	PWB_BSSDESCRIPTION asBSSDescriptElement;
+
+	MLME_FRAME sMlmeFrame;	// connect to peerSTA parameters
+
+	MTO_PARAMETERS sMtoPara;	// MTO_struct ...
+	hw_data_t sHwData;	//For HAL
+	MDS Mds;
+
+	spinlock_t SpinLock;
+
+	atomic_t ThreadCount;
+
+	u32 RxByteCount;
+	u32 TxByteCount;
+
+	struct sk_buff *packet_return;
+	s32 netif_state_stop;	// 1: stop  0: normal
+	struct iw_statistics iw_stats;
+
+	u8 LinkName[WB_MAX_LINK_NAME_LEN];
+
+	bool enabled;
+};
+
+#endif /* __WINBOND_CORE_H */
diff --git a/drivers/staging/winbond/ds_tkip.h b/drivers/staging/winbond/ds_tkip.h
index 6841d66e7e8..9c5c4e73f2c 100644
--- a/drivers/staging/winbond/ds_tkip.h
+++ b/drivers/staging/winbond/ds_tkip.h
@@ -1,3 +1,8 @@
+#ifndef __WINBOND_DS_TKIP_H
+#define __WINBOND_DS_TKIP_H
+
+#include <linux/types.h>
+
 // Rotation functions on 32 bit values
 #define ROL32( A, n ) \
     ( ((A) << (n)) | ( ((A)>>(32-(n)))  & ( (1UL << (n)) - 1 ) ) )
@@ -26,8 +31,7 @@ typedef struct tkip
 } tkip_t;
 
 //void _append_data( u8 *pData, u16 size, tkip_t *p );
-void Mds_MicGet(  void* Adapter,  void* pRxLayer1,  u8 *pKey,  u8 *pMic );
-void Mds_MicFill(  void* Adapter,  void* pDes,  u8 *XmitBufAddress );
-
-
+void Mds_MicGet(  void* adapter,  void* pRxLayer1,  u8 *pKey,  u8 *pMic );
+void Mds_MicFill(  void* adapter,  void* pDes,  u8 *XmitBufAddress );
 
+#endif
diff --git a/drivers/staging/winbond/gl_80211.h b/drivers/staging/winbond/gl_80211.h
index 1806d817496..5a244c44a61 100644
--- a/drivers/staging/winbond/gl_80211.h
+++ b/drivers/staging/winbond/gl_80211.h
@@ -1,7 +1,8 @@
-
 #ifndef __GL_80211_H__
 #define __GL_80211_H__
 
+#include <linux/types.h>
+
 /****************** CONSTANT AND MACRO SECTION ******************************/
 
 /* BSS Type */
diff --git a/drivers/staging/winbond/linux/common.h b/drivers/staging/winbond/linux/common.h
deleted file mode 100644
index 712a86cfa68..00000000000
--- a/drivers/staging/winbond/linux/common.h
+++ /dev/null
@@ -1,128 +0,0 @@
-//
-// common.h
-//
-// This file contains the OS dependant definition and function.
-// Every OS has this file individual.
-//
-
-#define DebugUsbdStatusInformation( _A )
-
-#ifndef COMMON_DEF
-#define COMMON_DEF
-
-#include <linux/version.h>
-#include <linux/usb.h>
-#include <linux/kernel.h> //need for kernel alert
-#include <linux/autoconf.h>
-#include <linux/sched.h>
-#include <linux/signal.h>
-#include <linux/slab.h> //memory allocate
-#include <linux/module.h>
-#include <linux/netdevice.h>
-#include <linux/etherdevice.h>
-#include <linux/init.h>//need for init and exit modules marco
-#include <linux/ctype.h>
-#include <linux/wait.h>
-#include <linux/list.h>
-#include <linux/wireless.h>
-#include <linux/if_arp.h>
-#include <asm/uaccess.h>
-#include <net/iw_handler.h>
-#include <linux/skbuff.h>
-
-
-//#define DEBUG_ENABLED  1
-
-
-
-//===============================================================
-// Common type definition
-//===============================================================
-
-//===========================================
-#define IGNORE      2
-#define	SUCCESS     1
-#define	FAILURE     0
-
-
-#ifndef true
-#define true        1
-#endif
-
-#ifndef false
-#define false       0
-#endif
-
-// PD43 20021108
-#ifndef TRUE
-#define TRUE        1
-#endif
-
-#ifndef FALSE
-#define FALSE       0
-#endif
-
-#define STATUS_MEDIA_CONNECT 1
-#define STATUS_MEDIA_DISCONNECT 0
-
-#ifndef BIT
-#define BIT(x)                  (1 << (x))
-#endif
-
-typedef struct urb * PURB;
-
-
-
-//==================================================================================================
-// Common function definition
-//==================================================================================================
-#ifndef abs
-#define abs(_T)							((_T) < 0 ? -_T : _T)
-#endif
-#define DEBUG_ENABLED
-#define ETH_LENGTH_OF_ADDRESS	6
-#ifdef DEBUG_ENABLED
-#define WBDEBUG( _M )	printk _M
-#else
-#define WBDEBUG( _M )	0
-#endif
-
-#define OS_DISCONNECTED	0
-#define OS_CONNECTED	1
-
-
-#define OS_EVENT_INDICATE( _A, _B, _F )
-#define OS_PMKID_STATUS_EVENT( _A )
-
-
-/* Uff, no, longs are not atomic on all architectures Linux
- * supports. This should really use atomic_t */
-
-#define OS_ATOMIC			u32
-#define OS_ATOMIC_READ( _A, _V )	_V
-#define OS_ATOMIC_INC( _A, _V )		EncapAtomicInc( _A, (void*)_V )
-#define OS_ATOMIC_DEC( _A, _V )		EncapAtomicDec( _A, (void*)_V )
-#define OS_MEMORY_CLEAR( _A, _S )	memset( (u8 *)_A,0,_S)
-#define OS_MEMORY_COMPARE( _A, _B, _S )	(memcmp(_A,_B,_S)? 0 : 1) // Definition is reverse with Ndis 1: the same 0: different
-
-#define OS_TIMER	struct timer_list
-#define OS_TIMER_INITIAL( _T, _F, _P )			\
-{							\
-	init_timer( _T );				\
-	(_T)->function = (void *)_F##_1a;		\
-	(_T)->data = (unsigned long)_P;			\
-}
-
-// _S : Millisecond
-// 20060420 At least 1 large than jiffies
-#define OS_TIMER_SET( _T, _S )					\
-{								\
-	(_T)->expires = jiffies + ((_S*HZ+999)/1000);\
-	add_timer( _T );					\
-}
-#define OS_TIMER_CANCEL( _T, _B )		del_timer_sync( _T )
-#define OS_TIMER_GET_SYS_TIME( _T )		(*_T=jiffies)
-
-
-#endif // COMMON_DEF
-
diff --git a/drivers/staging/winbond/linux/wb35rx_f.h b/drivers/staging/winbond/linux/wb35rx_f.h
deleted file mode 100644
index daa3e73042b..00000000000
--- a/drivers/staging/winbond/linux/wb35rx_f.h
+++ /dev/null
@@ -1,17 +0,0 @@
-//====================================
-// Interface function declare
-//====================================
-void		Wb35Rx_reset_descriptor(  phw_data_t pHwData );
-unsigned char		Wb35Rx_initial(  phw_data_t pHwData );
-void		Wb35Rx_destroy(  phw_data_t pHwData );
-void		Wb35Rx_stop(  phw_data_t pHwData );
-u16		Wb35Rx_indicate(  phw_data_t pHwData );
-void		Wb35Rx_adjust(  PDESCRIPTOR pRxDes );
-void		Wb35Rx_start(  phw_data_t pHwData );
-
-void		Wb35Rx(  phw_data_t pHwData );
-void		Wb35Rx_Complete(  PURB pUrb );
-
-
-
-
diff --git a/drivers/staging/winbond/linux/wb35tx_f.h b/drivers/staging/winbond/linux/wb35tx_f.h
deleted file mode 100644
index 107b1291813..00000000000
--- a/drivers/staging/winbond/linux/wb35tx_f.h
+++ /dev/null
@@ -1,20 +0,0 @@
-//====================================
-// Interface function declare
-//====================================
-unsigned char Wb35Tx_initial(	 phw_data_t pHwData );
-void Wb35Tx_destroy(  phw_data_t pHwData );
-unsigned char Wb35Tx_get_tx_buffer(  phw_data_t pHwData,  u8 **pBuffer );
-
-void Wb35Tx_EP2VM(  phw_data_t pHwData );
-void Wb35Tx_EP2VM_start(  phw_data_t pHwData );
-void Wb35Tx_EP2VM_complete(  PURB purb );
-
-void Wb35Tx_start(  phw_data_t pHwData );
-void Wb35Tx_stop(  phw_data_t pHwData );
-void Wb35Tx(  phw_data_t pHwData );
-void Wb35Tx_complete(  PURB purb );
-void Wb35Tx_reset_descriptor(  phw_data_t pHwData );
-
-void Wb35Tx_CurrentTime(  phw_data_t pHwData,  u32 TimeCount );
-
-
diff --git a/drivers/staging/winbond/linux/wbusb.c b/drivers/staging/winbond/linux/wbusb.c
deleted file mode 100644
index 39ca9b9878f..00000000000
--- a/drivers/staging/winbond/linux/wbusb.c
+++ /dev/null
@@ -1,391 +0,0 @@
-/*
- * Copyright 2008 Pavel Machek <pavel@suse.cz>
- *
- * Distribute under GPLv2.
- */
-#include "sysdef.h"
-#include <net/mac80211.h>
-
-MODULE_AUTHOR(DRIVER_AUTHOR);
-MODULE_DESCRIPTION(DRIVER_DESC);
-MODULE_LICENSE("GPL");
-MODULE_VERSION("0.1");
-
-static struct usb_device_id wb35_table[] __devinitdata = {
-	{USB_DEVICE(0x0416, 0x0035)},
-	{USB_DEVICE(0x18E8, 0x6201)},
-	{USB_DEVICE(0x18E8, 0x6206)},
-	{USB_DEVICE(0x18E8, 0x6217)},
-	{USB_DEVICE(0x18E8, 0x6230)},
-	{USB_DEVICE(0x18E8, 0x6233)},
-	{USB_DEVICE(0x1131, 0x2035)},
-	{ 0, }
-};
-
-MODULE_DEVICE_TABLE(usb, wb35_table);
-
-static struct ieee80211_rate wbsoft_rates[] = {
-	{ .bitrate = 10, .flags = IEEE80211_RATE_SHORT_PREAMBLE },
-};
-
-static struct ieee80211_channel wbsoft_channels[] = {
-	{ .center_freq = 2412},
-};
-
-int wbsoft_enabled;
-struct ieee80211_hw *my_dev;
-PADAPTER my_adapter;
-
-static int wbsoft_add_interface(struct ieee80211_hw *dev,
-				 struct ieee80211_if_init_conf *conf)
-{
-	printk("wbsoft_add interface called\n");
-	return 0;
-}
-
-static void wbsoft_remove_interface(struct ieee80211_hw *dev,
-				     struct ieee80211_if_init_conf *conf)
-{
-	printk("wbsoft_remove interface called\n");
-}
-
-static void wbsoft_stop(struct ieee80211_hw *hw)
-{
-	printk(KERN_INFO "%s called\n", __func__);
-}
-
-static int wbsoft_get_stats(struct ieee80211_hw *hw,
-			    struct ieee80211_low_level_stats *stats)
-{
-	printk(KERN_INFO "%s called\n", __func__);
-	return 0;
-}
-
-static int wbsoft_get_tx_stats(struct ieee80211_hw *hw,
-			       struct ieee80211_tx_queue_stats *stats)
-{
-	printk(KERN_INFO "%s called\n", __func__);
-	return 0;
-}
-
-static void wbsoft_configure_filter(struct ieee80211_hw *dev,
-				     unsigned int changed_flags,
-				     unsigned int *total_flags,
-				     int mc_count, struct dev_mc_list *mclist)
-{
-	unsigned int bit_nr, new_flags;
-	u32 mc_filter[2];
-	int i;
-
-	new_flags = 0;
-
-	if (*total_flags & FIF_PROMISC_IN_BSS) {
-		new_flags |= FIF_PROMISC_IN_BSS;
-		mc_filter[1] = mc_filter[0] = ~0;
-	} else if ((*total_flags & FIF_ALLMULTI) || (mc_count > 32)) {
-		new_flags |= FIF_ALLMULTI;
-		mc_filter[1] = mc_filter[0] = ~0;
-	} else {
-		mc_filter[1] = mc_filter[0] = 0;
-		for (i = 0; i < mc_count; i++) {
-			if (!mclist)
-				break;
-			printk("Should call ether_crc here\n");
-			//bit_nr = ether_crc(ETH_ALEN, mclist->dmi_addr) >> 26;
-			bit_nr = 0;
-
-			bit_nr &= 0x3F;
-			mc_filter[bit_nr >> 5] |= 1 << (bit_nr & 31);
-			mclist = mclist->next;
-		}
-	}
-
-	dev->flags &= ~IEEE80211_HW_RX_INCLUDES_FCS;
-
-	*total_flags = new_flags;
-}
-
-static int wbsoft_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
-{
-	char *buffer = kmalloc(skb->len, GFP_ATOMIC);
-	printk("Sending frame %d bytes\n", skb->len);
-	memcpy(buffer, skb->data, skb->len);
-	if (1 == MLMESendFrame(my_adapter, buffer, skb->len, FRAME_TYPE_802_11_MANAGEMENT))
-		printk("frame sent ok (%d bytes)?\n", skb->len);
-	return NETDEV_TX_OK;
-}
-
-
-static int wbsoft_start(struct ieee80211_hw *dev)
-{
-	wbsoft_enabled = 1;
-	printk("wbsoft_start called\n");
-	return 0;
-}
-
-static int wbsoft_config(struct ieee80211_hw *dev, struct ieee80211_conf *conf)
-{
-	ChanInfo ch;
-	printk("wbsoft_config called\n");
-
-	ch.band = 1;
-	ch.ChanNo = 1;	/* Should use channel_num, or something, as that is already pre-translated */
-
-
-	hal_set_current_channel(&my_adapter->sHwData, ch);
-	hal_set_beacon_period(&my_adapter->sHwData, conf->beacon_int);
-//	hal_set_cap_info(&my_adapter->sHwData, ?? );
-// hal_set_ssid(phw_data_t pHwData,  u8 * pssid,  u8 ssid_len); ??
-	hal_set_accept_broadcast(&my_adapter->sHwData, 1);
-	hal_set_accept_promiscuous(&my_adapter->sHwData,  1);
-	hal_set_accept_multicast(&my_adapter->sHwData,  1);
-	hal_set_accept_beacon(&my_adapter->sHwData,  1);
-	hal_set_radio_mode(&my_adapter->sHwData,  0);
-	//hal_set_antenna_number(  phw_data_t pHwData, u8 number )
-	//hal_set_rf_power(phw_data_t pHwData, u8 PowerIndex)
-
-
-//	hal_start_bss(&my_adapter->sHwData, WLAN_BSSTYPE_INFRASTRUCTURE);	??
-
-//void hal_set_rates(phw_data_t pHwData, u8 * pbss_rates,
-//		   u8 length, unsigned char basic_rate_set)
-
-	return 0;
-}
-
-static int wbsoft_config_interface(struct ieee80211_hw *dev,
-				    struct ieee80211_vif *vif,
-				    struct ieee80211_if_conf *conf)
-{
-	printk("wbsoft_config_interface called\n");
-	return 0;
-}
-
-static u64 wbsoft_get_tsf(struct ieee80211_hw *dev)
-{
-	printk("wbsoft_get_tsf called\n");
-	return 0;
-}
-
-static const struct ieee80211_ops wbsoft_ops = {
-	.tx			= wbsoft_tx,
-	.start			= wbsoft_start,		/* Start can be pretty much empty as we do WbWLanInitialize() during probe? */
-	.stop			= wbsoft_stop,
-	.add_interface		= wbsoft_add_interface,
-	.remove_interface	= wbsoft_remove_interface,
-	.config			= wbsoft_config,
-	.config_interface	= wbsoft_config_interface,
-	.configure_filter	= wbsoft_configure_filter,
-	.get_stats		= wbsoft_get_stats,
-	.get_tx_stats		= wbsoft_get_tx_stats,
-	.get_tsf		= wbsoft_get_tsf,
-// conf_tx: hal_set_cwmin()/hal_set_cwmax;
-};
-
-struct wbsoft_priv {
-};
-
-
-// Usb kernel subsystem will call this function when a new device is plugged into.
-int wb35_probe(struct usb_interface *intf, const struct usb_device_id *id_table)
-{
-	PADAPTER	Adapter;
-	PWBLINUX	pWbLinux;
-	PWBUSB		pWbUsb;
-        struct usb_host_interface *interface;
-	struct usb_endpoint_descriptor *endpoint;
-	int	ret = -1;
-	u32	ltmp;
-	struct usb_device *udev = interface_to_usbdev(intf);
-
-	usb_get_dev(udev);
-
-	printk("[w35und]wb35_probe ->\n");
-
-	// 20060630.2 Check the device if it already be opened
-	ret = usb_control_msg(udev, usb_rcvctrlpipe( udev, 0 ),
-			      0x01, USB_TYPE_VENDOR|USB_RECIP_DEVICE|USB_DIR_IN,
-			      0x0, 0x400, &ltmp, 4, HZ*100 );
-	if (ret < 0)
-		goto error;
-
-	ltmp = cpu_to_le32(ltmp);
-	if (ltmp)  // Is already initialized?
-		goto error;
-
-	Adapter = kzalloc(sizeof(ADAPTER), GFP_KERNEL);
-
-	my_adapter = Adapter;
-	pWbLinux = &Adapter->WbLinux;
-	pWbUsb = &Adapter->sHwData.WbUsb;
-	pWbUsb->udev = udev;
-
-        interface = intf->cur_altsetting;
-        endpoint = &interface->endpoint[0].desc;
-
-	if (endpoint[2].wMaxPacketSize == 512) {
-		printk("[w35und] Working on USB 2.0\n");
-		pWbUsb->IsUsb20 = 1;
-	}
-
-	if (!WbWLanInitialize(Adapter)) {
-		printk("[w35und]WbWLanInitialize fail\n");
-		goto error;
-	}
-
-	{
-		struct wbsoft_priv *priv;
-		struct ieee80211_hw *dev;
-		static struct ieee80211_supported_band band;
-		int res;
-
-		dev = ieee80211_alloc_hw(sizeof(*priv), &wbsoft_ops);
-
-		if (!dev) {
-			printk("w35und: ieee80211 alloc failed\n" );
-			BUG();
-		}
-
-		my_dev = dev;
-
-		SET_IEEE80211_DEV(dev, &udev->dev);
-		{
-			phw_data_t pHwData = &Adapter->sHwData;
-			unsigned char		dev_addr[MAX_ADDR_LEN];
-			hal_get_permanent_address(pHwData, dev_addr);
-			SET_IEEE80211_PERM_ADDR(dev, dev_addr);
-		}
-
-
-		dev->extra_tx_headroom = 12;	/* FIXME */
-		dev->flags = 0;
-
-		dev->channel_change_time = 1000;
-//		dev->max_rssi = 100;
-
-		dev->queues = 1;
-
-		band.channels = wbsoft_channels;
-		band.n_channels = ARRAY_SIZE(wbsoft_channels);
-		band.bitrates = wbsoft_rates;
-		band.n_bitrates = ARRAY_SIZE(wbsoft_rates);
-
-		dev->wiphy->bands[IEEE80211_BAND_2GHZ] = &band;
-#if 0
-		wbsoft_modes[0].num_channels = 1;
-		wbsoft_modes[0].channels = wbsoft_channels;
-		wbsoft_modes[0].mode = MODE_IEEE80211B;
-		wbsoft_modes[0].num_rates = ARRAY_SIZE(wbsoft_rates);
-		wbsoft_modes[0].rates = wbsoft_rates;
-
-		res = ieee80211_register_hwmode(dev, &wbsoft_modes[0]);
-		BUG_ON(res);
-#endif
-
-		res = ieee80211_register_hw(dev);
-		BUG_ON(res);
-	}
-
-	usb_set_intfdata( intf, Adapter );
-
-	printk("[w35und] _probe OK\n");
-	return 0;
-error:
-	return -ENOMEM;
-}
-
-void packet_came(char *pRxBufferAddress, int PacketSize)
-{
-	struct sk_buff *skb;
-	struct ieee80211_rx_status rx_status = {0};
-
-	if (!wbsoft_enabled)
-		return;
-
-	skb = dev_alloc_skb(PacketSize);
-	if (!skb) {
-		printk("Not enough memory for packet, FIXME\n");
-		return;
-	}
-
-	memcpy(skb_put(skb, PacketSize),
-	       pRxBufferAddress,
-	       PacketSize);
-
-/*
-	rx_status.rate = 10;
-	rx_status.channel = 1;
-	rx_status.freq = 12345;
-	rx_status.phymode = MODE_IEEE80211B;
-*/
-
-	ieee80211_rx_irqsafe(my_dev, skb, &rx_status);
-}
-
-unsigned char
-WbUsb_initial(phw_data_t pHwData)
-{
-	return 1;
-}
-
-
-void
-WbUsb_destroy(phw_data_t pHwData)
-{
-}
-
-int wb35_open(struct net_device *netdev)
-{
-	/* netdev_priv() or netdev->ml_priv should reference to the address of
-	 * private data(PADAPTER). It depends on whether private data memory is
-	 * allocated when alloc_netdev().
-	 */
-	PADAPTER Adapter = (PADAPTER)netdev_priv(netdev);
-	phw_data_t pHwData = &Adapter->sHwData;
-
-        netif_start_queue(netdev);
-
-	//TODO : put here temporarily
-	hal_set_accept_broadcast(pHwData, 1); // open accept broadcast
-
-	return 0;
-}
-
-int wb35_close(struct net_device *netdev)
-{
-	netif_stop_queue(netdev);
-	return 0;
-}
-
-void wb35_disconnect(struct usb_interface *intf)
-{
-	PWBLINUX pWbLinux;
-	PADAPTER Adapter = usb_get_intfdata(intf);
-	usb_set_intfdata(intf, NULL);
-
-        pWbLinux = &Adapter->WbLinux;
-
-	// Card remove
-	WbWlanHalt(Adapter);
-
-}
-
-static struct usb_driver wb35_driver = {
-	.name		= "w35und",
-	.id_table	= wb35_table,
-	.probe		= wb35_probe,
-	.disconnect	= wb35_disconnect,
-};
-
-static int __init wb35_init(void)
-{
-	return usb_register(&wb35_driver);
-}
-
-static void __exit wb35_exit(void)
-{
-	usb_deregister(&wb35_driver);
-}
-
-module_init(wb35_init);
-module_exit(wb35_exit);
diff --git a/drivers/staging/winbond/linux/wbusb_f.h b/drivers/staging/winbond/linux/wbusb_f.h
deleted file mode 100644
index cae29e107e1..00000000000
--- a/drivers/staging/winbond/linux/wbusb_f.h
+++ /dev/null
@@ -1,34 +0,0 @@
-//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// Copyright (c) 1996-2004 Winbond Electronic Corporation
-//
-//  Module Name:
-//    wbusb_f.h
-//
-//  Abstract:
-//    Linux driver.
-//
-//  Author:
-//
-//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
-int wb35_open(struct net_device *netdev);
-int wb35_close(struct net_device *netdev);
-unsigned char WbUsb_initial(phw_data_t pHwData);
-void WbUsb_destroy(phw_data_t pHwData);
-unsigned char WbWLanInitialize(PADAPTER Adapter);
-#define	WbUsb_Stop( _A )
-
-int wb35_probe(struct usb_interface *intf,const struct usb_device_id *id_table);
-void wb35_disconnect(struct usb_interface *intf);
-
-
-#define wb_usb_submit_urb(_A) usb_submit_urb(_A, GFP_ATOMIC)
-#define wb_usb_alloc_urb(_A) usb_alloc_urb(_A, GFP_ATOMIC)
-
-#define WbUsb_CheckForHang( _P )
-#define WbUsb_DetectStart( _P, _I )
-
-
-
-
-
diff --git a/drivers/staging/winbond/localpara.h b/drivers/staging/winbond/localpara.h
index 268cf916ab4..607bb0526cf 100644
--- a/drivers/staging/winbond/localpara.h
+++ b/drivers/staging/winbond/localpara.h
@@ -1,6 +1,12 @@
+#ifndef __WINBOND_LOCALPARA_H
+#define __WINBOND_LOCALPARA_H
+
 //=============================================================
 // LocalPara.h -
 //=============================================================
+
+#include "mac_structures.h"
+
 //Define the local ability
 
 #define LOCAL_DEFAULT_BEACON_PERIOD			100		//ms
@@ -25,7 +31,7 @@
 #define LOCAL_UNKNOWN_5_CHANNEL_NUM			34	//not include 165
 
 
-#define psLOCAL			(&(Adapter->sLocalPara))
+#define psLOCAL			(&(adapter->sLocalPara))
 
 #define MODE_802_11_BG			0
 #define MODE_802_11_A			1
@@ -143,7 +149,6 @@ typedef struct LOCAL_PARA
 
     //// power-save variables
     u8  		  	iPowerSaveMode;     // 0 indicates it is on, 1 indicates it is off
-	u8			ShutDowned;
 	u8			ATIMmode;
 	u8			ExcludeUnencrypted;
 
@@ -272,4 +277,4 @@ typedef struct LOCAL_PARA
 
 } WB_LOCALDESCRIPT, *PWB_LOCALDESCRIPT;
 
-
+#endif
diff --git a/drivers/staging/winbond/mac_structures.h b/drivers/staging/winbond/mac_structures.h
index 031d2cb6cd6..0d1619601c0 100644
--- a/drivers/staging/winbond/mac_structures.h
+++ b/drivers/staging/winbond/mac_structures.h
@@ -21,6 +21,7 @@
 #ifndef _MAC_Structures_H_
 #define _MAC_Structures_H_
 
+#include <linux/skbuff.h>
 
 //=========================================================
 // Some miscellaneous definitions
@@ -115,10 +116,6 @@
 #define WLAN_MAX_PAIRWISE_CIPHER_SUITE_COUNT    ((u16) 6)
 #define WLAN_MAX_AUTH_KEY_MGT_SUITE_LIST_COUNT  ((u16) 2)
 
-#ifdef WB_LINUX
-#define UNALIGNED
-#endif
-
 //========================================================
 typedef enum enum_PowerManagementMode
 {
@@ -464,7 +461,7 @@ struct	RSN_Information_Element
 {
 	u8					Element_ID;
 	u8					Length;
-	UNALIGNED SUITE_SELECTOR	OuiWPAAdditional;//WPA version 2.0 additional field, and should be 00:50:F2:01
+	SUITE_SELECTOR	OuiWPAAdditional;//WPA version 2.0 additional field, and should be 00:50:F2:01
 	u16					Version;
 	SUITE_SELECTOR		GroupKeySuite;
 	u16					PairwiseKeySuiteCount;
diff --git a/drivers/staging/winbond/mds.c b/drivers/staging/winbond/mds.c
index f1de813f9c7..e431406e25a 100644
--- a/drivers/staging/winbond/mds.c
+++ b/drivers/staging/winbond/mds.c
@@ -1,445 +1,39 @@
+#include "ds_tkip.h"
+#include "gl_80211.h"
+#include "mds_f.h"
+#include "mlmetxrx_f.h"
+#include "mto_f.h"
 #include "os_common.h"
-
-void
-Mds_reset_descriptor(PADAPTER Adapter)
-{
-	PMDS pMds = &Adapter->Mds;
-
-	pMds->TxPause = 0;
-	pMds->TxThreadCount = 0;
-	pMds->TxFillIndex = 0;
-	pMds->TxDesIndex = 0;
-	pMds->ScanTxPause = 0;
-	memset(pMds->TxOwner, 0, ((MAX_USB_TX_BUFFER_NUMBER + 3) & ~0x03));
-}
+#include "wbhal_f.h"
+#include "wblinux_f.h"
 
 unsigned char
-Mds_initial(PADAPTER Adapter)
+Mds_initial(struct wbsoft_priv * adapter)
 {
-	PMDS pMds = &Adapter->Mds;
+	PMDS pMds = &adapter->Mds;
 
-	pMds->TxPause = FALSE;
+	pMds->TxPause = false;
 	pMds->TxRTSThreshold = DEFAULT_RTSThreshold;
 	pMds->TxFragmentThreshold = DEFAULT_FRAGMENT_THRESHOLD;
 
-	vRxTimerInit(Adapter);//for WPA countermeasure
-
-	return hal_get_tx_buffer( &Adapter->sHwData, &pMds->pTxBuffer );
-}
-
-void
-Mds_Destroy(PADAPTER Adapter)
-{
-	vRxTimerStop(Adapter);
-}
-
-void
-Mds_Tx(PADAPTER Adapter)
-{
-	phw_data_t	pHwData = &Adapter->sHwData;
-	PMDS		pMds = &Adapter->Mds;
-	DESCRIPTOR	TxDes;
-	PDESCRIPTOR	pTxDes = &TxDes;
-	u8		*XmitBufAddress;
-	u16		XmitBufSize, PacketSize, stmp, CurrentSize, FragmentThreshold;
-	u8		FillIndex, TxDesIndex, FragmentCount, FillCount;
-	unsigned char	BufferFilled = FALSE, MICAdd = 0;
-
-
-	if (pMds->TxPause)
-		return;
-	if (!hal_driver_init_OK(pHwData))
-		return;
-
-	//Only one thread can be run here
-	if (!OS_ATOMIC_INC( Adapter, &pMds->TxThreadCount) == 1)
-		goto cleanup;
-
-	// Start to fill the data
-	do {
-		FillIndex = pMds->TxFillIndex;
-		if (pMds->TxOwner[FillIndex]) { // Is owned by software 0:Yes 1:No
-#ifdef _PE_TX_DUMP_
-			WBDEBUG(("[Mds_Tx] Tx Owner is H/W.\n"));
-#endif
-			break;
-		}
-
-		XmitBufAddress = pMds->pTxBuffer + (MAX_USB_TX_BUFFER * FillIndex); //Get buffer
-		XmitBufSize = 0;
-		FillCount = 0;
-		do {
-			PacketSize = Adapter->sMlmeFrame.len;
-			if (!PacketSize)
-				break;
-
-			//For Check the buffer resource
-			FragmentThreshold = CURRENT_FRAGMENT_THRESHOLD;
-			//931130.5.b
-			FragmentCount = PacketSize/FragmentThreshold + 1;
-			stmp = PacketSize + FragmentCount*32 + 8;//931130.5.c 8:MIC
-			if ((XmitBufSize + stmp) >= MAX_USB_TX_BUFFER) {
-				printk("[Mds_Tx] Excess max tx buffer.\n");
-				break; // buffer is not enough
-			}
-
-
-			//
-			// Start transmitting
-			//
-			BufferFilled = TRUE;
-
-			/* Leaves first u8 intact */
-			memset((u8 *)pTxDes + 1, 0, sizeof(DESCRIPTOR) - 1);
-
-			TxDesIndex = pMds->TxDesIndex;//Get the current ID
-			pTxDes->Descriptor_ID = TxDesIndex;
-			pMds->TxDesFrom[ TxDesIndex ] = 2;//Storing the information of source comming from
-			pMds->TxDesIndex++;
-			pMds->TxDesIndex %= MAX_USB_TX_DESCRIPTOR;
-
-			MLME_GetNextPacket( Adapter, pTxDes );
-
-			// Copy header. 8byte USB + 24byte 802.11Hdr. Set TxRate, Preamble type
-			Mds_HeaderCopy( Adapter, pTxDes, XmitBufAddress );
-
-			// For speed up Key setting
-			if (pTxDes->EapFix) {
-#ifdef _PE_TX_DUMP_
-				WBDEBUG(("35: EPA 4th frame detected. Size = %d\n", PacketSize));
-#endif
-				pHwData->IsKeyPreSet = 1;
-			}
-
-			// Copy (fragment) frame body, and set USB, 802.11 hdr flag
-			CurrentSize = Mds_BodyCopy(Adapter, pTxDes, XmitBufAddress);
-
-			// Set RTS/CTS and Normal duration field into buffer
-			Mds_DurationSet(Adapter, pTxDes, XmitBufAddress);
-
-			//
-			// Calculation MIC from buffer which maybe fragment, then fill into temporary address 8 byte
-			// 931130.5.e
-			if (MICAdd)
-				Mds_MicFill( Adapter, pTxDes, XmitBufAddress );
-
-			//Shift to the next address
-			XmitBufSize += CurrentSize;
-			XmitBufAddress += CurrentSize;
-
-#ifdef _IBSS_BEACON_SEQ_STICK_
-			if ((XmitBufAddress[ DOT_11_DA_OFFSET+8 ] & 0xfc) != MAC_SUBTYPE_MNGMNT_PROBE_REQUEST) // +8 for USB hdr
-#endif
-				pMds->TxToggle = TRUE;
-
-			// Get packet to transmit completed, 1:TESTSTA 2:MLME 3: Ndis data
-			MLME_SendComplete(Adapter, 0, TRUE);
-
-			// Software TSC count 20060214
-			pMds->TxTsc++;
-			if (pMds->TxTsc == 0)
-				pMds->TxTsc_2++;
-
-			FillCount++; // 20060928
-		} while (HAL_USB_MODE_BURST(pHwData)); // End of multiple MSDU copy loop. FALSE = single TRUE = multiple sending
-
-		// Move to the next one, if necessary
-		if (BufferFilled) {
-			// size setting
-			pMds->TxBufferSize[ FillIndex ] = XmitBufSize;
-
-			// 20060928 set Tx count
-			pMds->TxCountInBuffer[FillIndex] = FillCount;
-
-			// Set owner flag
-			pMds->TxOwner[FillIndex] = 1;
-
-			pMds->TxFillIndex++;
-			pMds->TxFillIndex %= MAX_USB_TX_BUFFER_NUMBER;
-			BufferFilled = FALSE;
-		} else
-			break;
-
-		if (!PacketSize) // No more pk for transmitting
-			break;
-
-	} while(TRUE);
-
-	//
-	// Start to send by lower module
-	//
-	if (!pHwData->IsKeyPreSet)
-		Wb35Tx_start(pHwData);
-
- cleanup:
-	OS_ATOMIC_DEC( Adapter, &pMds->TxThreadCount );
+	return hal_get_tx_buffer( &adapter->sHwData, &pMds->pTxBuffer );
 }
 
 void
-Mds_SendComplete(PADAPTER Adapter, PT02_DESCRIPTOR pT02)
-{
-	PMDS	pMds = &Adapter->Mds;
-	phw_data_t	pHwData = &Adapter->sHwData;
-	u8	PacketId = (u8)pT02->T02_Tx_PktID;
-	unsigned char	SendOK = TRUE;
-	u8	RetryCount, TxRate;
-
-	if (pT02->T02_IgnoreResult) // Don't care the result
-		return;
-	if (pT02->T02_IsLastMpdu) {
-		//TODO: DTO -- get the retry count and fragment count
-		// Tx rate
-		TxRate = pMds->TxRate[ PacketId ][ 0 ];
-		RetryCount = (u8)pT02->T02_MPDU_Cnt;
-		if (pT02->value & FLAG_ERROR_TX_MASK) {
-			SendOK = FALSE;
-
-			if (pT02->T02_transmit_abort || pT02->T02_out_of_MaxTxMSDULiftTime) {
-				//retry error
-				pHwData->dto_tx_retry_count += (RetryCount+1);
-				//[for tx debug]
-				if (RetryCount<7)
-					pHwData->tx_retry_count[RetryCount] += RetryCount;
-				else
-					pHwData->tx_retry_count[7] += RetryCount;
-				#ifdef _PE_STATE_DUMP_
-				WBDEBUG(("dto_tx_retry_count =%d\n", pHwData->dto_tx_retry_count));
-				#endif
-				MTO_SetTxCount(Adapter, TxRate, RetryCount);
-			}
-			pHwData->dto_tx_frag_count += (RetryCount+1);
-
-			//[for tx debug]
-			if (pT02->T02_transmit_abort_due_to_TBTT)
-				pHwData->tx_TBTT_start_count++;
-			if (pT02->T02_transmit_without_encryption_due_to_wep_on_false)
-				pHwData->tx_WepOn_false_count++;
-			if (pT02->T02_discard_due_to_null_wep_key)
-				pHwData->tx_Null_key_count++;
-		} else {
-			if (pT02->T02_effective_transmission_rate)
-				pHwData->tx_ETR_count++;
-			MTO_SetTxCount(Adapter, TxRate, RetryCount);
-		}
-
-		// Clear send result buffer
-		pMds->TxResult[ PacketId ] = 0;
-	} else
-		pMds->TxResult[ PacketId ] |= ((u16)(pT02->value & 0x0ffff));
-}
-
-void
-Mds_HeaderCopy(PADAPTER Adapter, PDESCRIPTOR pDes, u8 *TargetBuffer)
-{
-	PMDS	pMds = &Adapter->Mds;
-	u8	*src_buffer = pDes->buffer_address[0];//931130.5.g
-	PT00_DESCRIPTOR	pT00;
-	PT01_DESCRIPTOR	pT01;
-	u16	stmp;
-	u8	i, ctmp1, ctmp2, ctmpf;
-	u16	FragmentThreshold = CURRENT_FRAGMENT_THRESHOLD;
-
-
-	stmp = pDes->buffer_total_size;
-	//
-	// Set USB header 8 byte
-	//
-	pT00 = (PT00_DESCRIPTOR)TargetBuffer;
-	TargetBuffer += 4;
-	pT01 = (PT01_DESCRIPTOR)TargetBuffer;
-	TargetBuffer += 4;
-
-	pT00->value = 0;// Clear
-	pT01->value = 0;// Clear
-
-	pT00->T00_tx_packet_id = pDes->Descriptor_ID;// Set packet ID
-	pT00->T00_header_length = 24;// Set header length
-	pT01->T01_retry_abort_ebable = 1;//921013 931130.5.h
-
-	// Key ID setup
-	pT01->T01_wep_id = 0;
-
-	FragmentThreshold = DEFAULT_FRAGMENT_THRESHOLD;	//Do not fragment
-	// Copy full data, the 1'st buffer contain all the data 931130.5.j
-	memcpy( TargetBuffer, src_buffer, DOT_11_MAC_HEADER_SIZE );// Copy header
-	pDes->buffer_address[0] = src_buffer + DOT_11_MAC_HEADER_SIZE;
-	pDes->buffer_total_size -= DOT_11_MAC_HEADER_SIZE;
-	pDes->buffer_size[0] = pDes->buffer_total_size;
-
-	// Set fragment threshold
-	FragmentThreshold -= (DOT_11_MAC_HEADER_SIZE + 4);
-	pDes->FragmentThreshold = FragmentThreshold;
-
-	// Set more frag bit
-	TargetBuffer[1] |= 0x04;// Set more frag bit
-
-	//
-	// Set tx rate
-	//
-	stmp = *(u16 *)(TargetBuffer+30); // 2n alignment address
-
-	//Use basic rate
-	ctmp1 = ctmpf = CURRENT_TX_RATE_FOR_MNG;
-
-	pDes->TxRate = ctmp1;
-	#ifdef _PE_TX_DUMP_
-	WBDEBUG(("Tx rate =%x\n", ctmp1));
-	#endif
-
-	pT01->T01_modulation_type = (ctmp1%3) ? 0 : 1;
-
-	for( i=0; i<2; i++ ) {
-		if( i == 1 )
-			ctmp1 = ctmpf;
-
-		pMds->TxRate[pDes->Descriptor_ID][i] = ctmp1; // backup the ta rate and fall back rate
-
-		if( ctmp1 == 108) ctmp2 = 7;
-		else if( ctmp1 == 96 ) ctmp2 = 6; // Rate convert for USB
-		else if( ctmp1 == 72 ) ctmp2 = 5;
-		else if( ctmp1 == 48 ) ctmp2 = 4;
-		else if( ctmp1 == 36 ) ctmp2 = 3;
-		else if( ctmp1 == 24 ) ctmp2 = 2;
-		else if( ctmp1 == 18 ) ctmp2 = 1;
-		else if( ctmp1 == 12 ) ctmp2 = 0;
-		else if( ctmp1 == 22 ) ctmp2 = 3;
-		else if( ctmp1 == 11 ) ctmp2 = 2;
-		else if( ctmp1 == 4  ) ctmp2 = 1;
-		else ctmp2 = 0; // if( ctmp1 == 2  ) or default
-
-		if( i == 0 )
-			pT01->T01_transmit_rate = ctmp2;
-		else
-			pT01->T01_fall_back_rate = ctmp2;
-	}
-
-	//
-	// Set preamble type
-	//
-	if ((pT01->T01_modulation_type == 0) && (pT01->T01_transmit_rate == 0))	// RATE_1M
-		pDes->PreambleMode =  WLAN_PREAMBLE_TYPE_LONG;
-	else
-		pDes->PreambleMode =  CURRENT_PREAMBLE_MODE;
-	pT01->T01_plcp_header_length = pDes->PreambleMode;	// Set preamble
-
-}
-
-// The function return the 4n size of usb pk
-u16
-Mds_BodyCopy(PADAPTER Adapter, PDESCRIPTOR pDes, u8 *TargetBuffer)
+Mds_Destroy(struct wbsoft_priv * adapter)
 {
-	PT00_DESCRIPTOR	pT00;
-	PMDS	pMds = &Adapter->Mds;
-	u8	*buffer;
-	u8	*src_buffer;
-	u8	*pctmp;
-	u16	Size = 0;
-	u16	SizeLeft, CopySize, CopyLeft, stmp;
-	u8	buf_index, FragmentCount = 0;
-
-
-	// Copy fragment body
-	buffer = TargetBuffer; // shift 8B usb + 24B 802.11
-	SizeLeft = pDes->buffer_total_size;
-	buf_index = pDes->buffer_start_index;
-
-	pT00 = (PT00_DESCRIPTOR)buffer;
-	while (SizeLeft) {
-		pT00 = (PT00_DESCRIPTOR)buffer;
-		CopySize = SizeLeft;
-		if (SizeLeft > pDes->FragmentThreshold) {
-			CopySize = pDes->FragmentThreshold;
-			pT00->T00_frame_length = 24 + CopySize;//Set USB length
-		} else
-			pT00->T00_frame_length = 24 + SizeLeft;//Set USB length
-
-		SizeLeft -= CopySize;
-
-		// 1 Byte operation
-		pctmp = (u8 *)( buffer + 8 + DOT_11_SEQUENCE_OFFSET );
-		*pctmp &= 0xf0;
-		*pctmp |= FragmentCount;//931130.5.m
-		if( !FragmentCount )
-			pT00->T00_first_mpdu = 1;
-
-		buffer += 32; // 8B usb + 24B 802.11 header
-		Size += 32;
-
-		// Copy into buffer
-		stmp = CopySize + 3;
-		stmp &= ~0x03;//4n Alignment
-		Size += stmp;// Current 4n offset of mpdu
-
-		while (CopySize) {
-			// Copy body
-			src_buffer = pDes->buffer_address[buf_index];
-			CopyLeft = CopySize;
-			if (CopySize >= pDes->buffer_size[buf_index]) {
-				CopyLeft = pDes->buffer_size[buf_index];
-
-				// Get the next buffer of descriptor
-				buf_index++;
-				buf_index %= MAX_DESCRIPTOR_BUFFER_INDEX;
-			} else {
-				u8	*pctmp = pDes->buffer_address[buf_index];
-				pctmp += CopySize;
-				pDes->buffer_address[buf_index] = pctmp;
-				pDes->buffer_size[buf_index] -= CopySize;
-			}
-
-			memcpy(buffer, src_buffer, CopyLeft);
-			buffer += CopyLeft;
-			CopySize -= CopyLeft;
-		}
-
-		// 931130.5.n
-		if (pMds->MicAdd) {
-			if (!SizeLeft) {
-				pMds->MicWriteAddress[ pMds->MicWriteIndex ] = buffer - pMds->MicAdd;
-				pMds->MicWriteSize[ pMds->MicWriteIndex ] = pMds->MicAdd;
-				pMds->MicAdd = 0;
-			}
-			else if( SizeLeft < 8 ) //931130.5.p
-			{
-				pMds->MicAdd = SizeLeft;
-				pMds->MicWriteAddress[ pMds->MicWriteIndex ] = buffer - ( 8 - SizeLeft );
-				pMds->MicWriteSize[ pMds->MicWriteIndex ] = 8 - SizeLeft;
-				pMds->MicWriteIndex++;
-			}
-		}
-
-		// Does it need to generate the new header for next mpdu?
-		if (SizeLeft) {
-			buffer = TargetBuffer + Size; // Get the next 4n start address
-			memcpy( buffer, TargetBuffer, 32 );//Copy 8B USB +24B 802.11
-			pT00 = (PT00_DESCRIPTOR)buffer;
-			pT00->T00_first_mpdu = 0;
-		}
-
-		FragmentCount++;
-	}
-
-	pT00->T00_last_mpdu = 1;
-	pT00->T00_IsLastMpdu = 1;
-	buffer = (u8 *)pT00 + 8; // +8 for USB hdr
-	buffer[1] &= ~0x04; // Clear more frag bit of 802.11 frame control
-	pDes->FragmentCount = FragmentCount; // Update the correct fragment number
-	return Size;
 }
 
-
-void
-Mds_DurationSet(  PADAPTER Adapter,  PDESCRIPTOR pDes,  u8 *buffer )
+static void Mds_DurationSet(struct wbsoft_priv *adapter,  PDESCRIPTOR pDes,  u8 *buffer)
 {
 	PT00_DESCRIPTOR	pT00;
 	PT01_DESCRIPTOR	pT01;
 	u16	Duration, NextBodyLen, OffsetSize;
 	u8	Rate, i;
-	unsigned char	CTS_on = FALSE, RTS_on = FALSE;
+	unsigned char	CTS_on = false, RTS_on = false;
 	PT00_DESCRIPTOR pNextT00;
 	u16 BodyLen = 0;
-	unsigned char boGroupAddr = FALSE;
-
+	unsigned char boGroupAddr = false;
 
 	OffsetSize = pDes->FragmentThreshold + 32 + 3;
 	OffsetSize &= ~0x03;
@@ -452,7 +46,7 @@ Mds_DurationSet(  PADAPTER Adapter,  PDESCRIPTOR pDes,  u8 *buffer )
 	pNextT00 = (PT00_DESCRIPTOR)(buffer+OffsetSize);
 
 	if( buffer[ DOT_11_DA_OFFSET+8 ] & 0x1 ) // +8 for USB hdr
-		boGroupAddr = TRUE;
+		boGroupAddr = true;
 
 	//========================================
 	// Set RTS/CTS mechanism
@@ -467,13 +61,13 @@ Mds_DurationSet(  PADAPTER Adapter,  PDESCRIPTOR pDes,  u8 *buffer )
 		BodyLen += 4;	//CRC
 
 		if( BodyLen >= CURRENT_RTS_THRESHOLD )
-			RTS_on = TRUE; // Using RTS
+			RTS_on = true; // Using RTS
 		else
 		{
 			if( pT01->T01_modulation_type ) // Is using OFDM
 			{
 				if( CURRENT_PROTECT_MECHANISM ) // Is using protect
-					CTS_on = TRUE; // Using CTS
+					CTS_on = true; // Using CTS
 			}
 		}
 	}
@@ -624,9 +218,394 @@ Mds_DurationSet(  PADAPTER Adapter,  PDESCRIPTOR pDes,  u8 *buffer )
 
 }
 
-void MDS_EthernetPacketReceive(  PADAPTER Adapter,  PRXLAYER1 pRxLayer1 )
+// The function return the 4n size of usb pk
+static u16 Mds_BodyCopy(struct wbsoft_priv *adapter, PDESCRIPTOR pDes, u8 *TargetBuffer)
+{
+	PT00_DESCRIPTOR	pT00;
+	PMDS	pMds = &adapter->Mds;
+	u8	*buffer;
+	u8	*src_buffer;
+	u8	*pctmp;
+	u16	Size = 0;
+	u16	SizeLeft, CopySize, CopyLeft, stmp;
+	u8	buf_index, FragmentCount = 0;
+
+
+	// Copy fragment body
+	buffer = TargetBuffer; // shift 8B usb + 24B 802.11
+	SizeLeft = pDes->buffer_total_size;
+	buf_index = pDes->buffer_start_index;
+
+	pT00 = (PT00_DESCRIPTOR)buffer;
+	while (SizeLeft) {
+		pT00 = (PT00_DESCRIPTOR)buffer;
+		CopySize = SizeLeft;
+		if (SizeLeft > pDes->FragmentThreshold) {
+			CopySize = pDes->FragmentThreshold;
+			pT00->T00_frame_length = 24 + CopySize;//Set USB length
+		} else
+			pT00->T00_frame_length = 24 + SizeLeft;//Set USB length
+
+		SizeLeft -= CopySize;
+
+		// 1 Byte operation
+		pctmp = (u8 *)( buffer + 8 + DOT_11_SEQUENCE_OFFSET );
+		*pctmp &= 0xf0;
+		*pctmp |= FragmentCount;//931130.5.m
+		if( !FragmentCount )
+			pT00->T00_first_mpdu = 1;
+
+		buffer += 32; // 8B usb + 24B 802.11 header
+		Size += 32;
+
+		// Copy into buffer
+		stmp = CopySize + 3;
+		stmp &= ~0x03;//4n Alignment
+		Size += stmp;// Current 4n offset of mpdu
+
+		while (CopySize) {
+			// Copy body
+			src_buffer = pDes->buffer_address[buf_index];
+			CopyLeft = CopySize;
+			if (CopySize >= pDes->buffer_size[buf_index]) {
+				CopyLeft = pDes->buffer_size[buf_index];
+
+				// Get the next buffer of descriptor
+				buf_index++;
+				buf_index %= MAX_DESCRIPTOR_BUFFER_INDEX;
+			} else {
+				u8	*pctmp = pDes->buffer_address[buf_index];
+				pctmp += CopySize;
+				pDes->buffer_address[buf_index] = pctmp;
+				pDes->buffer_size[buf_index] -= CopySize;
+			}
+
+			memcpy(buffer, src_buffer, CopyLeft);
+			buffer += CopyLeft;
+			CopySize -= CopyLeft;
+		}
+
+		// 931130.5.n
+		if (pMds->MicAdd) {
+			if (!SizeLeft) {
+				pMds->MicWriteAddress[ pMds->MicWriteIndex ] = buffer - pMds->MicAdd;
+				pMds->MicWriteSize[ pMds->MicWriteIndex ] = pMds->MicAdd;
+				pMds->MicAdd = 0;
+			}
+			else if( SizeLeft < 8 ) //931130.5.p
+			{
+				pMds->MicAdd = SizeLeft;
+				pMds->MicWriteAddress[ pMds->MicWriteIndex ] = buffer - ( 8 - SizeLeft );
+				pMds->MicWriteSize[ pMds->MicWriteIndex ] = 8 - SizeLeft;
+				pMds->MicWriteIndex++;
+			}
+		}
+
+		// Does it need to generate the new header for next mpdu?
+		if (SizeLeft) {
+			buffer = TargetBuffer + Size; // Get the next 4n start address
+			memcpy( buffer, TargetBuffer, 32 );//Copy 8B USB +24B 802.11
+			pT00 = (PT00_DESCRIPTOR)buffer;
+			pT00->T00_first_mpdu = 0;
+		}
+
+		FragmentCount++;
+	}
+
+	pT00->T00_last_mpdu = 1;
+	pT00->T00_IsLastMpdu = 1;
+	buffer = (u8 *)pT00 + 8; // +8 for USB hdr
+	buffer[1] &= ~0x04; // Clear more frag bit of 802.11 frame control
+	pDes->FragmentCount = FragmentCount; // Update the correct fragment number
+	return Size;
+}
+
+static void Mds_HeaderCopy(struct wbsoft_priv * adapter, PDESCRIPTOR pDes, u8 *TargetBuffer)
+{
+	PMDS	pMds = &adapter->Mds;
+	u8	*src_buffer = pDes->buffer_address[0];//931130.5.g
+	PT00_DESCRIPTOR	pT00;
+	PT01_DESCRIPTOR	pT01;
+	u16	stmp;
+	u8	i, ctmp1, ctmp2, ctmpf;
+	u16	FragmentThreshold = CURRENT_FRAGMENT_THRESHOLD;
+
+
+	stmp = pDes->buffer_total_size;
+	//
+	// Set USB header 8 byte
+	//
+	pT00 = (PT00_DESCRIPTOR)TargetBuffer;
+	TargetBuffer += 4;
+	pT01 = (PT01_DESCRIPTOR)TargetBuffer;
+	TargetBuffer += 4;
+
+	pT00->value = 0;// Clear
+	pT01->value = 0;// Clear
+
+	pT00->T00_tx_packet_id = pDes->Descriptor_ID;// Set packet ID
+	pT00->T00_header_length = 24;// Set header length
+	pT01->T01_retry_abort_ebable = 1;//921013 931130.5.h
+
+	// Key ID setup
+	pT01->T01_wep_id = 0;
+
+	FragmentThreshold = DEFAULT_FRAGMENT_THRESHOLD;	//Do not fragment
+	// Copy full data, the 1'st buffer contain all the data 931130.5.j
+	memcpy( TargetBuffer, src_buffer, DOT_11_MAC_HEADER_SIZE );// Copy header
+	pDes->buffer_address[0] = src_buffer + DOT_11_MAC_HEADER_SIZE;
+	pDes->buffer_total_size -= DOT_11_MAC_HEADER_SIZE;
+	pDes->buffer_size[0] = pDes->buffer_total_size;
+
+	// Set fragment threshold
+	FragmentThreshold -= (DOT_11_MAC_HEADER_SIZE + 4);
+	pDes->FragmentThreshold = FragmentThreshold;
+
+	// Set more frag bit
+	TargetBuffer[1] |= 0x04;// Set more frag bit
+
+	//
+	// Set tx rate
+	//
+	stmp = *(u16 *)(TargetBuffer+30); // 2n alignment address
+
+	//Use basic rate
+	ctmp1 = ctmpf = CURRENT_TX_RATE_FOR_MNG;
+
+	pDes->TxRate = ctmp1;
+	#ifdef _PE_TX_DUMP_
+	WBDEBUG(("Tx rate =%x\n", ctmp1));
+	#endif
+
+	pT01->T01_modulation_type = (ctmp1%3) ? 0 : 1;
+
+	for( i=0; i<2; i++ ) {
+		if( i == 1 )
+			ctmp1 = ctmpf;
+
+		pMds->TxRate[pDes->Descriptor_ID][i] = ctmp1; // backup the ta rate and fall back rate
+
+		if( ctmp1 == 108) ctmp2 = 7;
+		else if( ctmp1 == 96 ) ctmp2 = 6; // Rate convert for USB
+		else if( ctmp1 == 72 ) ctmp2 = 5;
+		else if( ctmp1 == 48 ) ctmp2 = 4;
+		else if( ctmp1 == 36 ) ctmp2 = 3;
+		else if( ctmp1 == 24 ) ctmp2 = 2;
+		else if( ctmp1 == 18 ) ctmp2 = 1;
+		else if( ctmp1 == 12 ) ctmp2 = 0;
+		else if( ctmp1 == 22 ) ctmp2 = 3;
+		else if( ctmp1 == 11 ) ctmp2 = 2;
+		else if( ctmp1 == 4  ) ctmp2 = 1;
+		else ctmp2 = 0; // if( ctmp1 == 2  ) or default
+
+		if( i == 0 )
+			pT01->T01_transmit_rate = ctmp2;
+		else
+			pT01->T01_fall_back_rate = ctmp2;
+	}
+
+	//
+	// Set preamble type
+	//
+	if ((pT01->T01_modulation_type == 0) && (pT01->T01_transmit_rate == 0))	// RATE_1M
+		pDes->PreambleMode =  WLAN_PREAMBLE_TYPE_LONG;
+	else
+		pDes->PreambleMode =  CURRENT_PREAMBLE_MODE;
+	pT01->T01_plcp_header_length = pDes->PreambleMode;	// Set preamble
+
+}
+
+void
+Mds_Tx(struct wbsoft_priv * adapter)
 {
-		OS_RECEIVE_PACKET_INDICATE( Adapter, pRxLayer1 );
+	phw_data_t	pHwData = &adapter->sHwData;
+	PMDS		pMds = &adapter->Mds;
+	DESCRIPTOR	TxDes;
+	PDESCRIPTOR	pTxDes = &TxDes;
+	u8		*XmitBufAddress;
+	u16		XmitBufSize, PacketSize, stmp, CurrentSize, FragmentThreshold;
+	u8		FillIndex, TxDesIndex, FragmentCount, FillCount;
+	unsigned char	BufferFilled = false, MICAdd = 0;
+
+
+	if (pMds->TxPause)
+		return;
+	if (!hal_driver_init_OK(pHwData))
+		return;
+
+	//Only one thread can be run here
+	if (!atomic_inc_return(&pMds->TxThreadCount) == 1)
+		goto cleanup;
+
+	// Start to fill the data
+	do {
+		FillIndex = pMds->TxFillIndex;
+		if (pMds->TxOwner[FillIndex]) { // Is owned by software 0:Yes 1:No
+#ifdef _PE_TX_DUMP_
+			WBDEBUG(("[Mds_Tx] Tx Owner is H/W.\n"));
+#endif
+			break;
+		}
+
+		XmitBufAddress = pMds->pTxBuffer + (MAX_USB_TX_BUFFER * FillIndex); //Get buffer
+		XmitBufSize = 0;
+		FillCount = 0;
+		do {
+			PacketSize = adapter->sMlmeFrame.len;
+			if (!PacketSize)
+				break;
+
+			//For Check the buffer resource
+			FragmentThreshold = CURRENT_FRAGMENT_THRESHOLD;
+			//931130.5.b
+			FragmentCount = PacketSize/FragmentThreshold + 1;
+			stmp = PacketSize + FragmentCount*32 + 8;//931130.5.c 8:MIC
+			if ((XmitBufSize + stmp) >= MAX_USB_TX_BUFFER) {
+				printk("[Mds_Tx] Excess max tx buffer.\n");
+				break; // buffer is not enough
+			}
+
+
+			//
+			// Start transmitting
+			//
+			BufferFilled = true;
+
+			/* Leaves first u8 intact */
+			memset((u8 *)pTxDes + 1, 0, sizeof(DESCRIPTOR) - 1);
+
+			TxDesIndex = pMds->TxDesIndex;//Get the current ID
+			pTxDes->Descriptor_ID = TxDesIndex;
+			pMds->TxDesFrom[ TxDesIndex ] = 2;//Storing the information of source comming from
+			pMds->TxDesIndex++;
+			pMds->TxDesIndex %= MAX_USB_TX_DESCRIPTOR;
+
+			MLME_GetNextPacket( adapter, pTxDes );
+
+			// Copy header. 8byte USB + 24byte 802.11Hdr. Set TxRate, Preamble type
+			Mds_HeaderCopy( adapter, pTxDes, XmitBufAddress );
+
+			// For speed up Key setting
+			if (pTxDes->EapFix) {
+#ifdef _PE_TX_DUMP_
+				WBDEBUG(("35: EPA 4th frame detected. Size = %d\n", PacketSize));
+#endif
+				pHwData->IsKeyPreSet = 1;
+			}
+
+			// Copy (fragment) frame body, and set USB, 802.11 hdr flag
+			CurrentSize = Mds_BodyCopy(adapter, pTxDes, XmitBufAddress);
+
+			// Set RTS/CTS and Normal duration field into buffer
+			Mds_DurationSet(adapter, pTxDes, XmitBufAddress);
+
+			//
+			// Calculation MIC from buffer which maybe fragment, then fill into temporary address 8 byte
+			// 931130.5.e
+			if (MICAdd)
+				Mds_MicFill( adapter, pTxDes, XmitBufAddress );
+
+			//Shift to the next address
+			XmitBufSize += CurrentSize;
+			XmitBufAddress += CurrentSize;
+
+#ifdef _IBSS_BEACON_SEQ_STICK_
+			if ((XmitBufAddress[ DOT_11_DA_OFFSET+8 ] & 0xfc) != MAC_SUBTYPE_MNGMNT_PROBE_REQUEST) // +8 for USB hdr
+#endif
+				pMds->TxToggle = true;
+
+			// Get packet to transmit completed, 1:TESTSTA 2:MLME 3: Ndis data
+			MLME_SendComplete(adapter, 0, true);
+
+			// Software TSC count 20060214
+			pMds->TxTsc++;
+			if (pMds->TxTsc == 0)
+				pMds->TxTsc_2++;
+
+			FillCount++; // 20060928
+		} while (HAL_USB_MODE_BURST(pHwData)); // End of multiple MSDU copy loop. false = single true = multiple sending
+
+		// Move to the next one, if necessary
+		if (BufferFilled) {
+			// size setting
+			pMds->TxBufferSize[ FillIndex ] = XmitBufSize;
+
+			// 20060928 set Tx count
+			pMds->TxCountInBuffer[FillIndex] = FillCount;
+
+			// Set owner flag
+			pMds->TxOwner[FillIndex] = 1;
+
+			pMds->TxFillIndex++;
+			pMds->TxFillIndex %= MAX_USB_TX_BUFFER_NUMBER;
+			BufferFilled = false;
+		} else
+			break;
+
+		if (!PacketSize) // No more pk for transmitting
+			break;
+
+	} while(true);
+
+	//
+	// Start to send by lower module
+	//
+	if (!pHwData->IsKeyPreSet)
+		Wb35Tx_start(adapter);
+
+ cleanup:
+	atomic_dec(&pMds->TxThreadCount);
 }
 
+void
+Mds_SendComplete(struct wbsoft_priv * adapter, PT02_DESCRIPTOR pT02)
+{
+	PMDS	pMds = &adapter->Mds;
+	phw_data_t	pHwData = &adapter->sHwData;
+	u8	PacketId = (u8)pT02->T02_Tx_PktID;
+	unsigned char	SendOK = true;
+	u8	RetryCount, TxRate;
 
+	if (pT02->T02_IgnoreResult) // Don't care the result
+		return;
+	if (pT02->T02_IsLastMpdu) {
+		//TODO: DTO -- get the retry count and fragment count
+		// Tx rate
+		TxRate = pMds->TxRate[ PacketId ][ 0 ];
+		RetryCount = (u8)pT02->T02_MPDU_Cnt;
+		if (pT02->value & FLAG_ERROR_TX_MASK) {
+			SendOK = false;
+
+			if (pT02->T02_transmit_abort || pT02->T02_out_of_MaxTxMSDULiftTime) {
+				//retry error
+				pHwData->dto_tx_retry_count += (RetryCount+1);
+				//[for tx debug]
+				if (RetryCount<7)
+					pHwData->tx_retry_count[RetryCount] += RetryCount;
+				else
+					pHwData->tx_retry_count[7] += RetryCount;
+				#ifdef _PE_STATE_DUMP_
+				WBDEBUG(("dto_tx_retry_count =%d\n", pHwData->dto_tx_retry_count));
+				#endif
+				MTO_SetTxCount(adapter, TxRate, RetryCount);
+			}
+			pHwData->dto_tx_frag_count += (RetryCount+1);
+
+			//[for tx debug]
+			if (pT02->T02_transmit_abort_due_to_TBTT)
+				pHwData->tx_TBTT_start_count++;
+			if (pT02->T02_transmit_without_encryption_due_to_wep_on_false)
+				pHwData->tx_WepOn_false_count++;
+			if (pT02->T02_discard_due_to_null_wep_key)
+				pHwData->tx_Null_key_count++;
+		} else {
+			if (pT02->T02_effective_transmission_rate)
+				pHwData->tx_ETR_count++;
+			MTO_SetTxCount(adapter, TxRate, RetryCount);
+		}
+
+		// Clear send result buffer
+		pMds->TxResult[ PacketId ] = 0;
+	} else
+		pMds->TxResult[ PacketId ] |= ((u16)(pT02->value & 0x0ffff));
+}
diff --git a/drivers/staging/winbond/mds_f.h b/drivers/staging/winbond/mds_f.h
index 7a682d4cfbd..ee0f12093db 100644
--- a/drivers/staging/winbond/mds_f.h
+++ b/drivers/staging/winbond/mds_f.h
@@ -1,33 +1,23 @@
-unsigned char Mds_initial(  PADAPTER Adapter );
-void Mds_Destroy(  PADAPTER Adapter );
-void Mds_Tx(  PADAPTER Adapter );
-void Mds_HeaderCopy(  PADAPTER Adapter,  PDESCRIPTOR pDes,  u8 *TargetBuffer );
-u16 Mds_BodyCopy(  PADAPTER Adapter,  PDESCRIPTOR pDes,  u8 *TargetBuffer );
-void Mds_DurationSet(  PADAPTER Adapter,  PDESCRIPTOR pDes,  u8 *TargetBuffer );
-void Mds_SendComplete(  PADAPTER Adapter,  PT02_DESCRIPTOR pT02 );
-void Mds_MpduProcess(  PADAPTER Adapter,  PDESCRIPTOR pRxDes );
-void Mds_reset_descriptor(  PADAPTER Adapter );
-extern void DataDmp(u8 *pdata, u32 len, u32 offset);
-
+#ifndef __WINBOND_MDS_F_H
+#define __WINBOND_MDS_F_H
 
-void vRxTimerInit(PWB32_ADAPTER Adapter);
-void vRxTimerStart(PWB32_ADAPTER Adapter, int timeout_value);
-void RxTimerHandler_1a( PADAPTER Adapter);
-void vRxTimerStop(PWB32_ADAPTER Adapter);
-void RxTimerHandler( void*			SystemSpecific1,
-					   PWB32_ADAPTER 	Adapter,
-					   void*			SystemSpecific2,
-					   void*			SystemSpecific3);
+#include "wbhal_s.h"
+#include "core.h"
 
+unsigned char Mds_initial(  struct wbsoft_priv *adapter );
+void Mds_Destroy(  struct wbsoft_priv *adapter );
+void Mds_Tx(  struct wbsoft_priv *adapter );
+void Mds_SendComplete(  struct wbsoft_priv *adapter,  PT02_DESCRIPTOR pT02 );
+void Mds_MpduProcess(  struct wbsoft_priv *adapter,  PDESCRIPTOR pRxDes );
+extern void DataDmp(u8 *pdata, u32 len, u32 offset);
 
 // For Asynchronous indicating. The routine collocates with USB.
-void Mds_MsduProcess(  PWB32_ADAPTER Adapter,  PRXLAYER1 pRxLayer1,  u8 SlotIndex);
+void Mds_MsduProcess(  struct wbsoft_priv *adapter,  PRXLAYER1 pRxLayer1,  u8 SlotIndex);
 
 // For data frame sending 20060802
-u16 MDS_GetPacketSize(  PADAPTER Adapter );
-void MDS_GetNextPacket(  PADAPTER Adapter,  PDESCRIPTOR pDes );
-void MDS_GetNextPacketComplete(  PADAPTER Adapter,  PDESCRIPTOR pDes );
-void MDS_SendResult(  PADAPTER Adapter,  u8 PacketId,  unsigned char SendOK );
-void MDS_EthernetPacketReceive(  PADAPTER Adapter,  PRXLAYER1 pRxLayer1 );
-
+u16 MDS_GetPacketSize(  struct wbsoft_priv *adapter );
+void MDS_GetNextPacket(  struct wbsoft_priv *adapter,  PDESCRIPTOR pDes );
+void MDS_GetNextPacketComplete(  struct wbsoft_priv *adapter,  PDESCRIPTOR pDes );
+void MDS_SendResult(  struct wbsoft_priv *adapter,  u8 PacketId,  unsigned char SendOK );
 
+#endif
diff --git a/drivers/staging/winbond/mds_s.h b/drivers/staging/winbond/mds_s.h
index 9df2e0936bf..ebf61e3ce1d 100644
--- a/drivers/staging/winbond/mds_s.h
+++ b/drivers/staging/winbond/mds_s.h
@@ -1,9 +1,19 @@
+#ifndef __WINBOND_MDS_H
+#define __WINBOND_MDS_H
+
+#include <linux/timer.h>
+#include <linux/types.h>
+#include <asm/atomic.h>
+
+#include "localpara.h"
+#include "mac_structures.h"
+#include "scan_s.h"
+
 ////////////////////////////////////////////////////////////////////////////////////////////////////////
 #define MAX_USB_TX_DESCRIPTOR		15		// IS89C35 ability
 #define MAX_USB_TX_BUFFER_NUMBER	4		// Virtual pre-buffer number of MAX_USB_TX_BUFFER
 #define MAX_USB_TX_BUFFER			4096	// IS89C35 ability 4n alignment is required for hardware
 
-#define MDS_EVENT_INDICATE( _A, _B, _F )	OS_EVENT_INDICATE( _A, _B, _F )
 #define AUTH_REQUEST_PAIRWISE_ERROR			0		// _F flag setting
 #define AUTH_REQUEST_GROUP_ERROR			1		// _F flag setting
 
@@ -21,20 +31,19 @@
 #define CURRENT_PAIRWISE_KEY			psSME->tx_mic_key
 #define CURRENT_GROUP_KEY				psSME->group_tx_mic_key
 #define CURRENT_ENCRYPT_STATUS			psSME->encrypt_status
-#define CURRENT_WEP_ID					Adapter->sSmePara._dot11WEPDefaultKeyID
-#define CURRENT_CONTROL_PORT_BLOCK		( psSME->wpa_ok!=1 || (Adapter->Mds.boCounterMeasureBlock==1 && (CURRENT_ENCRYPT_STATUS==ENCRYPT_TKIP)) )
-#define CURRENT_FRAGMENT_THRESHOLD		(Adapter->Mds.TxFragmentThreshold & ~0x1)
+#define CURRENT_WEP_ID					adapter->sSmePara._dot11WEPDefaultKeyID
+#define CURRENT_CONTROL_PORT_BLOCK		( psSME->wpa_ok!=1 || (adapter->Mds.boCounterMeasureBlock==1 && (CURRENT_ENCRYPT_STATUS==ENCRYPT_TKIP)) )
+#define CURRENT_FRAGMENT_THRESHOLD		(adapter->Mds.TxFragmentThreshold & ~0x1)
 #define CURRENT_PREAMBLE_MODE			psLOCAL->boShortPreamble?WLAN_PREAMBLE_TYPE_SHORT:WLAN_PREAMBLE_TYPE_LONG
-#define CURRENT_LINK_ON					OS_LINK_STATUS
-#define CURRENT_TX_RATE					Adapter->sLocalPara.CurrentTxRate
-#define CURRENT_FALL_BACK_TX_RATE		Adapter->sLocalPara.CurrentTxFallbackRate
-#define CURRENT_TX_RATE_FOR_MNG			Adapter->sLocalPara.CurrentTxRateForMng
+#define CURRENT_TX_RATE					adapter->sLocalPara.CurrentTxRate
+#define CURRENT_FALL_BACK_TX_RATE		adapter->sLocalPara.CurrentTxFallbackRate
+#define CURRENT_TX_RATE_FOR_MNG			adapter->sLocalPara.CurrentTxRateForMng
 #define CURRENT_PROTECT_MECHANISM		psLOCAL->boProtectMechanism
-#define CURRENT_RTS_THRESHOLD			Adapter->Mds.TxRTSThreshold
+#define CURRENT_RTS_THRESHOLD			adapter->Mds.TxRTSThreshold
 
-#define MIB_GS_XMIT_OK_INC				Adapter->sLocalPara.GS_XMIT_OK++
-#define MIB_GS_RCV_OK_INC				Adapter->sLocalPara.GS_RCV_OK++
-#define MIB_GS_XMIT_ERROR_INC			Adapter->sLocalPara.GS_XMIT_ERROR
+#define MIB_GS_XMIT_OK_INC				adapter->sLocalPara.GS_XMIT_OK++
+#define MIB_GS_RCV_OK_INC				adapter->sLocalPara.GS_RCV_OK++
+#define MIB_GS_XMIT_ERROR_INC			adapter->sLocalPara.GS_XMIT_ERROR
 
 //---------- TX -----------------------------------
 #define ETHERNET_TX_DESCRIPTORS         MAX_USB_TX_BUFFER_NUMBER
@@ -96,9 +105,9 @@ typedef struct _MDS
 	u8	ScanTxPause;	//data Tx pause because the scanning is progressing, but probe request Tx won't.
 	u8	TxPause;//For pause the Mds_Tx modult
 
-	OS_ATOMIC	TxThreadCount;//For thread counting 931130.4.v
+	atomic_t	TxThreadCount;//For thread counting 931130.4.v
 //950301 delete due to HW
-//	OS_ATOMIC	TxConcurrentCount;//931130.4.w
+//	atomic_t	TxConcurrentCount;//931130.4.w
 
 	u16	TxResult[ ((MAX_USB_TX_DESCRIPTOR + 1) & ~0x01) ];//Collect the sending result of Mpdu
 
@@ -133,9 +142,6 @@ typedef struct _MDS
 	u8		boCounterMeasureBlock;
 	u8		reserved_4[2];
 
-	//NDIS_MINIPORT_TIMER	nTimer;
-	OS_TIMER	nTimer;
-
 	u32	TxTsc; // 20060214
 	u32	TxTsc_2; // 20060214
 
@@ -180,4 +186,4 @@ typedef struct _RXLAYER1
 
 }RXLAYER1, * PRXLAYER1;
 
-
+#endif
diff --git a/drivers/staging/winbond/mlme_mib.h b/drivers/staging/winbond/mlme_mib.h
index 89759739cba..ca8922ec633 100644
--- a/drivers/staging/winbond/mlme_mib.h
+++ b/drivers/staging/winbond/mlme_mib.h
@@ -22,15 +22,15 @@
 //   Set the dot11ExcludeUnencrypted value.
 //
 // Arguments:
-//   Adapter        - The pointer to the miniport adapter context.
+//   adapter        - The pointer to the miniport adapter context.
 //   ExUnencrypted  - unsigned char type. The value to be set.
 //
 // Return values:
 //   None.
 //============================================================================
-#define MLMESetExcludeUnencrypted(Adapter, ExUnencrypted)     \
+#define MLMESetExcludeUnencrypted(adapter, ExUnencrypted)     \
 {                                                              \
-    (Adapter)->sLocalPara.ExcludeUnencrypted = ExUnencrypted;             \
+    (adapter)->sLocalPara.ExcludeUnencrypted = ExUnencrypted;             \
 }
 
 //============================================================================
@@ -40,12 +40,12 @@
 //   Get the dot11ExcludeUnencrypted value.
 //
 // Arguments:
-//   Adapter        - The pointer to the miniport adapter context.
+//   adapter        - The pointer to the miniport adapter context.
 //
 // Return values:
 //   unsigned char type. The current dot11ExcludeUnencrypted value.
 //============================================================================
-#define MLMEGetExcludeUnencrypted(Adapter) ((unsigned char) (Adapter)->sLocalPara.ExcludeUnencrypted)
+#define MLMEGetExcludeUnencrypted(adapter) ((unsigned char) (adapter)->sLocalPara.ExcludeUnencrypted)
 
 //============================================================================
 // MLMESetMaxReceiveLifeTime --
@@ -54,15 +54,15 @@
 //   Set the dot11MaxReceiveLifeTime value.
 //
 // Arguments:
-//   Adapter        - The pointer to the miniport adapter context.
+//   adapter        - The pointer to the miniport adapter context.
 //   ReceiveLifeTime- u32 type. The value to be set.
 //
 // Return values:
 //   None.
 //============================================================================
-#define MLMESetMaxReceiveLifeTime(Adapter, ReceiveLifeTime)    \
+#define MLMESetMaxReceiveLifeTime(adapter, ReceiveLifeTime)    \
 {                                                               \
-    (Adapter)->Mds.MaxReceiveTime = ReceiveLifeTime;                \
+    (adapter)->Mds.MaxReceiveTime = ReceiveLifeTime;                \
 }
 
 //============================================================================
@@ -72,12 +72,12 @@
 //   Get the dot11MaxReceiveLifeTime value.
 //
 // Arguments:
-//   Adapter        - The pointer to the miniport adapter context.
+//   adapter        - The pointer to the miniport adapter context.
 //
 // Return values:
 //   u32 type. The current dot11MaxReceiveLifeTime value.
 //============================================================================
-#define MLMEGetMaxReceiveLifeTime(Adapter) ((u32) (Adapter)->Mds.MaxReceiveTime)
+#define MLMEGetMaxReceiveLifeTime(adapter) ((u32) (adapter)->Mds.MaxReceiveTime)
 
 #endif
 
diff --git a/drivers/staging/winbond/mlme_s.h b/drivers/staging/winbond/mlme_s.h
index 039fd408ba6..ea12684a2b1 100644
--- a/drivers/staging/winbond/mlme_s.h
+++ b/drivers/staging/winbond/mlme_s.h
@@ -1,3 +1,12 @@
+#ifndef __WINBOND_MLME_H
+#define __WINBOND_MLME_H
+
+#include <linux/types.h>
+#include <linux/spinlock.h>
+
+#include "mac_structures.h"
+#include "mds_s.h"
+
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //	Mlme.h
 //		Define the related definitions of MLME module
@@ -192,4 +201,4 @@ typedef struct _RXDATA
 
 }__attribute__ ((packed)) RXDATA, *psRXDATA;
 
-
+#endif
diff --git a/drivers/staging/winbond/mlmetxrx.c b/drivers/staging/winbond/mlmetxrx.c
index e8533b8d197..07802afd2b8 100644
--- a/drivers/staging/winbond/mlmetxrx.c
+++ b/drivers/staging/winbond/mlmetxrx.c
@@ -17,113 +17,56 @@
 //============================================================================
 #include "os_common.h"
 
-void MLMEResetTxRx(PWB32_ADAPTER Adapter)
-{
-	s32     i;
-
-	// Reset the interface between MDS and MLME
-	for (i = 0; i < MAX_NUM_TX_MMPDU; i++)
-		Adapter->sMlmeFrame.TxMMPDUInUse[i] = FALSE;
-	for (i = 0; i < MAX_NUM_RX_MMPDU; i++)
-		Adapter->sMlmeFrame.SaveRxBufSlotInUse[i] = FALSE;
-
-	Adapter->sMlmeFrame.wNumRxMMPDUInMLME   = 0;
-	Adapter->sMlmeFrame.wNumRxMMPDUDiscarded = 0;
-	Adapter->sMlmeFrame.wNumRxMMPDU          = 0;
-	Adapter->sMlmeFrame.wNumTxMMPDUDiscarded = 0;
-	Adapter->sMlmeFrame.wNumTxMMPDU          = 0;
-	Adapter->sLocalPara.boCCAbusy    = FALSE;
-	Adapter->sLocalPara.iPowerSaveMode     = PWR_ACTIVE;     // Power active
-}
+#include "mds_f.h"
 
 //=============================================================================
-//	Function:
-//    MLMEGetMMPDUBuffer()
-//
-//	Description:
-//    Return the pointer to an available data buffer with
-//    the size MAX_MMPDU_SIZE for a MMPDU.
-//
-//  Arguments:
-//    Adapter   - pointer to the miniport adapter context.
-//
-//	Return value:
-//    NULL     : No available data buffer available
-//    Otherwise: Pointer to the data buffer
-//=============================================================================
-
-/* FIXME: Should this just be replaced with kmalloc() and kfree()? */
-u8 *MLMEGetMMPDUBuffer(PWB32_ADAPTER Adapter)
-{
-	s32 i;
-	u8 *returnVal;
-
-	for (i = 0; i< MAX_NUM_TX_MMPDU; i++) {
-		if (Adapter->sMlmeFrame.TxMMPDUInUse[i] == FALSE)
-			break;
-	}
-	if (i >= MAX_NUM_TX_MMPDU) return NULL;
-
-	returnVal = (u8 *)&(Adapter->sMlmeFrame.TxMMPDU[i]);
-	Adapter->sMlmeFrame.TxMMPDUInUse[i] = TRUE;
-
-	return returnVal;
-}
-
-//=============================================================================
-u8 MLMESendFrame(PWB32_ADAPTER Adapter, u8 *pMMPDU, u16 len, u8 DataType)
+u8 MLMESendFrame(struct wbsoft_priv * adapter, u8 *pMMPDU, u16 len, u8 DataType)
 /*	DataType : FRAME_TYPE_802_11_MANAGEMENT, FRAME_TYPE_802_11_MANAGEMENT_CHALLENGE,
 				FRAME_TYPE_802_11_DATA */
 {
-	if (Adapter->sMlmeFrame.IsInUsed != PACKET_FREE_TO_USE) {
-		Adapter->sMlmeFrame.wNumTxMMPDUDiscarded++;
-		return FALSE;
+	if (adapter->sMlmeFrame.IsInUsed != PACKET_FREE_TO_USE) {
+		adapter->sMlmeFrame.wNumTxMMPDUDiscarded++;
+		return false;
 	}
-	Adapter->sMlmeFrame.IsInUsed = PACKET_COME_FROM_MLME;
+	adapter->sMlmeFrame.IsInUsed = PACKET_COME_FROM_MLME;
 
 	// Keep information for sending
-	Adapter->sMlmeFrame.pMMPDU = pMMPDU;
-	Adapter->sMlmeFrame.DataType = DataType;
+	adapter->sMlmeFrame.pMMPDU = pMMPDU;
+	adapter->sMlmeFrame.DataType = DataType;
 	// len must be the last setting due to QUERY_SIZE_SECOND of Mds
-	Adapter->sMlmeFrame.len = len;
-	Adapter->sMlmeFrame.wNumTxMMPDU++;
+	adapter->sMlmeFrame.len = len;
+	adapter->sMlmeFrame.wNumTxMMPDU++;
 
 	// H/W will enter power save by set the register. S/W don't send null frame
 	//with PWRMgt bit enbled to enter power save now.
 
 	// Transmit NDIS packet
-	Mds_Tx(Adapter);
-	return TRUE;
+	Mds_Tx(adapter);
+	return true;
 }
 
-void
-MLME_GetNextPacket(PADAPTER Adapter, PDESCRIPTOR pDes)
+void MLME_GetNextPacket(struct wbsoft_priv *adapter, PDESCRIPTOR desc)
 {
-#define DESCRIPTOR_ADD_BUFFER( _D, _A, _S ) \
-{\
-	_D->InternalUsed = _D->buffer_start_index + _D->buffer_number; \
-	_D->InternalUsed %= MAX_DESCRIPTOR_BUFFER_INDEX; \
-	_D->buffer_address[ _D->InternalUsed ] = _A; \
-	_D->buffer_size[ _D->InternalUsed ] = _S; \
-	_D->buffer_total_size += _S; \
-	_D->buffer_number++;\
-}
-
-	DESCRIPTOR_ADD_BUFFER( pDes, Adapter->sMlmeFrame.pMMPDU, Adapter->sMlmeFrame.len );
-	pDes->Type = Adapter->sMlmeFrame.DataType;
+	desc->InternalUsed = desc->buffer_start_index + desc->buffer_number;
+	desc->InternalUsed %= MAX_DESCRIPTOR_BUFFER_INDEX;
+	desc->buffer_address[desc->InternalUsed] = adapter->sMlmeFrame.pMMPDU;
+	desc->buffer_size[desc->InternalUsed] = adapter->sMlmeFrame.len;
+	desc->buffer_total_size += adapter->sMlmeFrame.len;
+	desc->buffer_number++;
+	desc->Type = adapter->sMlmeFrame.DataType;
 }
 
-void MLMEfreeMMPDUBuffer(PWB32_ADAPTER Adapter, s8 *pData)
+static void MLMEfreeMMPDUBuffer(struct wbsoft_priv *adapter, s8 *pData)
 {
 	int i;
 
 	// Reclaim the data buffer
 	for (i = 0; i < MAX_NUM_TX_MMPDU; i++) {
-		if (pData == (s8 *)&(Adapter->sMlmeFrame.TxMMPDU[i]))
+		if (pData == (s8 *)&(adapter->sMlmeFrame.TxMMPDU[i]))
 			break;
 	}
-	if (Adapter->sMlmeFrame.TxMMPDUInUse[i])
-		Adapter->sMlmeFrame.TxMMPDUInUse[i] = FALSE;
+	if (adapter->sMlmeFrame.TxMMPDUInUse[i])
+		adapter->sMlmeFrame.TxMMPDUInUse[i] = false;
 	else  {
 		// Something wrong
 		// PD43 Add debug code here???
@@ -131,19 +74,19 @@ void MLMEfreeMMPDUBuffer(PWB32_ADAPTER Adapter, s8 *pData)
 }
 
 void
-MLME_SendComplete(PADAPTER Adapter, u8 PacketID, unsigned char SendOK)
+MLME_SendComplete(struct wbsoft_priv * adapter, u8 PacketID, unsigned char SendOK)
 {
 	MLME_TXCALLBACK	TxCallback;
 
     // Reclaim the data buffer
-	Adapter->sMlmeFrame.len = 0;
-	MLMEfreeMMPDUBuffer( Adapter, Adapter->sMlmeFrame.pMMPDU );
+	adapter->sMlmeFrame.len = 0;
+	MLMEfreeMMPDUBuffer( adapter, adapter->sMlmeFrame.pMMPDU );
 
 
 	TxCallback.bResult = MLME_SUCCESS;
 
 	// Return resource
-	Adapter->sMlmeFrame.IsInUsed = PACKET_FREE_TO_USE;
+	adapter->sMlmeFrame.IsInUsed = PACKET_FREE_TO_USE;
 }
 
 
diff --git a/drivers/staging/winbond/mlmetxrx_f.h b/drivers/staging/winbond/mlmetxrx_f.h
index 24cd5f308d9..5f05a6e1cda 100644
--- a/drivers/staging/winbond/mlmetxrx_f.h
+++ b/drivers/staging/winbond/mlmetxrx_f.h
@@ -8,32 +8,25 @@
 #ifndef _MLMETXRX_H
 #define _MLMETXRX_H
 
+#include "core.h"
+
 void
 MLMEProcThread(
-     PWB32_ADAPTER    Adapter
+     struct wbsoft_priv *    adapter
 	);
 
-void MLMEResetTxRx( PWB32_ADAPTER Adapter);
-
-u8 *
-MLMEGetMMPDUBuffer(
-     PWB32_ADAPTER    Adapter
-   );
-
-void MLMEfreeMMPDUBuffer( PWB32_ADAPTER Adapter,  s8 * pData);
-
-void MLME_GetNextPacket(  PADAPTER Adapter,  PDESCRIPTOR pDes );
-u8 MLMESendFrame( PWB32_ADAPTER Adapter,
+void MLME_GetNextPacket(  struct wbsoft_priv * adapter,  PDESCRIPTOR pDes );
+u8 MLMESendFrame( struct wbsoft_priv * adapter,
 					u8	*pMMPDU,
 					u16	len,
 					 u8	DataType);
 
 void
-MLME_SendComplete(  PWB32_ADAPTER Adapter,  u8 PacketID,  unsigned char SendOK );
+MLME_SendComplete(  struct wbsoft_priv * adapter,  u8 PacketID,  unsigned char SendOK );
 
 void
 MLMERcvFrame(
-     PWB32_ADAPTER    Adapter,
+     struct wbsoft_priv *    adapter,
      PRXBUFFER        pRxBufferArray,
      u8            NumOfBuffer,
      u8            ReturnSlotIndex
@@ -41,11 +34,11 @@ MLMERcvFrame(
 
 void
 MLMEReturnPacket(
-     PWB32_ADAPTER    Adapter,
+     struct wbsoft_priv *    adapter,
      u8 *          pRxBufer
    );
 #ifdef _IBSS_BEACON_SEQ_STICK_
-s8 SendBCNullData(PWB32_ADAPTER Adapter, u16 wIdx);
+s8 SendBCNullData(struct wbsoft_priv * adapter, u16 wIdx);
 #endif
 
 #endif
diff --git a/drivers/staging/winbond/mto.c b/drivers/staging/winbond/mto.c
index 2ef60e5120c..de11a05efae 100644
--- a/drivers/staging/winbond/mto.c
+++ b/drivers/staging/winbond/mto.c
@@ -23,10 +23,12 @@
 
 // LA20040210_DTO kevin
 #include "os_common.h"
+#include "sme_api.h"
+#include "gl_80211.h"
+#include "wbhal_f.h"
 
 // Declare SQ3 to rate and fragmentation threshold table
 // Declare fragmentation thresholds table
-#define MTO_MAX_SQ3_LEVELS                      14
 #define MTO_MAX_FRAG_TH_LEVELS                  5
 #define MTO_MAX_DATA_RATE_LEVELS                12
 
@@ -35,181 +37,15 @@ u16 MTO_Frag_Th_Tbl[MTO_MAX_FRAG_TH_LEVELS] =
     256, 384, 512, 768, 1536
 };
 
-u8  MTO_SQ3_Level[MTO_MAX_SQ3_LEVELS] =
-{
-    0, 26, 30, 32, 34, 35, 37, 42, 44, 46, 54, 62, 78, 81
-};
-u8  MTO_SQ3toRate[MTO_MAX_SQ3_LEVELS] =
-{
-    0, 1, 1, 2, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11
-};
-u8  MTO_SQ3toFrag[MTO_MAX_SQ3_LEVELS] =
-{
-    0, 2, 2, 3, 3, 4, 4, 4, 4, 4, 4, 4, 4, 4
-};
-
-// One Exchange Time table
-//
-u16 MTO_One_Exchange_Time_Tbl_l[MTO_MAX_FRAG_TH_LEVELS][MTO_MAX_DATA_RATE_LEVELS] =
-{
-    { 2554, 1474,  822,    0,    0,  636,    0,    0,    0,    0,    0,    0},
-    { 3578, 1986, 1009,    0,    0,  729,    0,    0,    0,    0,    0,    0},
-    { 4602, 2498, 1195,    0,    0,  822,    0,    0,    0,    0,    0,    0},
-    { 6650, 3522, 1567,    0,    0, 1009,    0,    0,    0,    0,    0,    0},
-    {12794, 6594, 2684,    0,    0, 1567,    0,    0,    0,    0,    0,    0}
-};
-
-u16 MTO_One_Exchange_Time_Tbl_s[MTO_MAX_FRAG_TH_LEVELS][MTO_MAX_DATA_RATE_LEVELS] =
-{
-    {    0, 1282,  630,  404,  288,  444,  232,  172,  144,  116,  100,   96},
-    {    0, 1794,  817,  572,  400,  537,  316,  228,  188,  144,  124,  116},
-    {    0, 2306, 1003,  744,  516,  630,  400,  288,  228,  172,  144,  136},
-    {    0, 3330, 1375, 1084,  744,  817,  572,  400,  316,  228,  188,  172},
-    {    0, 6402, 2492, 2108, 1424, 1375, 1084,  740,  572,  400,  316,  284}
-};
-
-#define MTO_ONE_EXCHANGE_TIME(preamble_type, frag_th_lvl, data_rate_lvl) \
-            (preamble_type) ?   MTO_One_Exchange_Time_Tbl_s[frag_th_lvl][data_rate_lvl] : \
-                                MTO_One_Exchange_Time_Tbl_l[frag_th_lvl][data_rate_lvl]
-
 // Declare data rate table
 //The following table will be changed at anytime if the opration rate supported by AP don't
 //match the table
-u8  MTO_Data_Rate_Tbl[MTO_MAX_DATA_RATE_LEVELS] =
-{
+static u8 MTO_Data_Rate_Tbl[MTO_MAX_DATA_RATE_LEVELS] = {
     2, 4, 11, 22, 12, 18, 24, 36, 48, 72, 96, 108
 };
 
-//The Stardard_Data_Rate_Tbl and Level2PerTbl table is used to indirectly retreive PER
-//information from Rate_PER_TBL
-//The default settings is AP can support full rate set.
-static u8  Stardard_Data_Rate_Tbl[MTO_MAX_DATA_RATE_LEVELS] =
-{
-	2, 4, 11, 22, 12, 18, 24, 36, 48, 72, 96, 108
-};
-static u8  Level2PerTbl[MTO_MAX_DATA_RATE_LEVELS] =
-{
-	0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11
-};
-//How many kind of tx rate can be supported by AP
-//DTO will change Rate between MTO_Data_Rate_Tbl[0] and MTO_Data_Rate_Tbl[MTO_DataRateAvailableLevel-1]
-static u8  MTO_DataRateAvailableLevel = MTO_MAX_DATA_RATE_LEVELS;
-//Smoothed PER table for each different RATE based on packet length of 1514
-static int Rate_PER_TBL[91][MTO_MAX_DATA_RATE_LEVELS] = {
-//        1M    2M    5.5M  11M   6M    9M    12M     18M    24M    36M    48M   54M
-/* 0%  */{ 93,   177,  420,  538,  690,  774,  1001,  1401,  1768,  2358,  2838,  3039},
-/* 1%  */{ 92,   176,  416,  533,  683,  767,  992,   1389,  1752,  2336,  2811,  3010},
-/* 2%  */{ 91,   174,  412,  528,  675,  760,  983,   1376,  1735,  2313,  2783,  2979},
-/* 3%  */{ 90,   172,  407,  523,  667,  753,  973,   1363,  1719,  2290,  2755,  2948},
-/* 4%  */{ 90,   170,  403,  518,  659,  746,  964,   1350,  1701,  2266,  2726,  2916},
-/* 5%  */{ 89,   169,  398,  512,  651,  738,  954,   1336,  1684,  2242,  2696,  2884},
-/* 6%  */{ 88,   167,  394,  507,  643,  731,  944,   1322,  1666,  2217,  2665,  2851},
-/* 7%  */{ 87,   165,  389,  502,  635,  723,  935,   1308,  1648,  2192,  2634,  2817},
-/* 8%  */{ 86,   163,  384,  497,  626,  716,  924,   1294,  1629,  2166,  2602,  2782},
-/* 9%  */{ 85,   161,  380,  491,  618,  708,  914,   1279,  1611,  2140,  2570,  2747},
-/* 10% */{ 84,   160,  375,  486,  609,  700,  904,   1265,  1591,  2113,  2537,  2711},
-/* 11% */{ 83,   158,  370,  480,  600,  692,  894,   1250,  1572,  2086,  2503,  2675},
-/* 12% */{ 82,   156,  365,  475,  592,  684,  883,   1234,  1552,  2059,  2469,  2638},
-/* 13% */{ 81,   154,  360,  469,  583,  676,  872,   1219,  1532,  2031,  2435,  2600},
-/* 14% */{ 80,   152,  355,  464,  574,  668,  862,   1204,  1512,  2003,  2400,  2562},
-/* 15% */{ 79,   150,  350,  458,  565,  660,  851,   1188,  1492,  1974,  2365,  2524},
-/* 16% */{ 78,   148,  345,  453,  556,  652,  840,   1172,  1471,  1945,  2329,  2485},
-/* 17% */{ 77,   146,  340,  447,  547,  643,  829,   1156,  1450,  1916,  2293,  2446},
-/* 18% */{ 76,   144,  335,  441,  538,  635,  818,   1140,  1429,  1887,  2256,  2406},
-/* 19% */{ 75,   143,  330,  436,  529,  627,  807,   1124,  1408,  1857,  2219,  2366},
-/* 20% */{ 74,   141,  325,  430,  520,  618,  795,   1107,  1386,  1827,  2182,  2326},
-/* 21% */{ 73,   139,  320,  424,  510,  610,  784,   1091,  1365,  1797,  2145,  2285},
-/* 22% */{ 72,   137,  314,  418,  501,  601,  772,   1074,  1343,  1766,  2107,  2244},
-/* 23% */{ 71,   135,  309,  412,  492,  592,  761,   1057,  1321,  1736,  2069,  2203},
-/* 24% */{ 70,   133,  304,  407,  482,  584,  749,   1040,  1299,  1705,  2031,  2161},
-/* 25% */{ 69,   131,  299,  401,  473,  575,  738,   1023,  1277,  1674,  1992,  2120},
-/* 26% */{ 68,   129,  293,  395,  464,  566,  726,   1006,  1254,  1642,  1953,  2078},
-/* 27% */{ 67,   127,  288,  389,  454,  557,  714,   989,   1232,  1611,  1915,  2035},
-/* 28% */{ 66,   125,  283,  383,  445,  549,  703,   972,   1209,  1579,  1876,  1993},
-/* 29% */{ 65,   123,  278,  377,  436,  540,  691,   955,   1187,  1548,  1836,  1951},
-/* 30% */{ 64,   121,  272,  371,  426,  531,  679,   937,   1164,  1516,  1797,  1908},
-/* 31% */{ 63,   119,  267,  365,  417,  522,  667,   920,   1141,  1484,  1758,  1866},
-/* 32% */{ 62,   117,  262,  359,  407,  513,  655,   902,   1118,  1453,  1719,  1823},
-/* 33% */{ 61,   115,  256,  353,  398,  504,  643,   885,   1095,  1421,  1679,  1781},
-/* 34% */{ 60,   113,  251,  347,  389,  495,  631,   867,   1072,  1389,  1640,  1738},
-/* 35% */{ 59,   111,  246,  341,  379,  486,  619,   850,   1049,  1357,  1600,  1695},
-/* 36% */{ 58,   108,  240,  335,  370,  477,  607,   832,   1027,  1325,  1561,  1653},
-/* 37% */{ 57,   106,  235,  329,  361,  468,  595,   815,   1004,  1293,  1522,  1610},
-/* 38% */{ 56,   104,  230,  323,  351,  459,  584,   797,   981,   1261,  1483,  1568},
-/* 39% */{ 55,   102,  224,  317,  342,  450,  572,   780,   958,   1230,  1443,  1526},
-/* 40% */{ 54,   100,  219,  311,  333,  441,  560,   762,   935,   1198,  1404,  1484},
-/* 41% */{ 53,   98,   214,  305,  324,  432,  548,   744,   912,   1166,  1366,  1442},
-/* 42% */{ 52,   96,   209,  299,  315,  423,  536,   727,   889,   1135,  1327,  1400},
-/* 43% */{ 51,   94,   203,  293,  306,  414,  524,   709,   866,   1104,  1289,  1358},
-/* 44% */{ 50,   92,   198,  287,  297,  405,  512,   692,   844,   1072,  1250,  1317},
-/* 45% */{ 49,   90,   193,  281,  288,  396,  500,   675,   821,   1041,  1212,  1276},
-/* 46% */{ 48,   88,   188,  275,  279,  387,  488,   657,   799,   1011,  1174,  1236},
-/* 47% */{ 47,   86,   183,  269,  271,  378,  476,   640,   777,   980,   1137,  1195},
-/* 48% */{ 46,   84,   178,  262,  262,  369,  464,   623,   754,   949,   1100,  1155},
-/* 49% */{ 45,   82,   173,  256,  254,  360,  452,   606,   732,   919,   1063,  1116},
-/* 50% */{ 44,   80,   168,  251,  245,  351,  441,   589,   710,   889,   1026,  1076},
-/* 51% */{ 43,   78,   163,  245,  237,  342,  429,   572,   689,   860,   990,   1038},
-/* 52% */{ 42,   76,   158,  239,  228,  333,  417,   555,   667,   830,   955,   999},
-/* 53% */{ 41,   74,   153,  233,  220,  324,  406,   538,   645,   801,   919,   961},
-/* 54% */{ 40,   72,   148,  227,  212,  315,  394,   522,   624,   773,   884,   924},
-/* 55% */{ 39,   70,   143,  221,  204,  307,  383,   505,   603,   744,   850,   887},
-/* 56% */{ 38,   68,   138,  215,  196,  298,  371,   489,   582,   716,   816,   851},
-/* 57% */{ 37,   67,   134,  209,  189,  289,  360,   473,   562,   688,   783,   815},
-/* 58% */{ 36,   65,   129,  203,  181,  281,  349,   457,   541,   661,   750,   780},
-/* 59% */{ 35,   63,   124,  197,  174,  272,  338,   441,   521,   634,   717,   745},
-/* 60% */{ 34,   61,   120,  192,  166,  264,  327,   425,   501,   608,   686,   712},
-/* 61% */{ 33,   59,   115,  186,  159,  255,  316,   409,   482,   582,   655,   678},
-/* 62% */{ 32,   57,   111,  180,  152,  247,  305,   394,   462,   556,   624,   646},
-/* 63% */{ 31,   55,   107,  174,  145,  238,  294,   379,   443,   531,   594,   614},
-/* 64% */{ 30,   53,   102,  169,  138,  230,  283,   364,   425,   506,   565,   583},
-/* 65% */{ 29,   52,   98,   163,  132,  222,  273,   349,   406,   482,   536,   553},
-/* 66% */{ 28,   50,   94,   158,  125,  214,  262,   334,   388,   459,   508,   523},
-/* 67% */{ 27,   48,   90,   152,  119,  206,  252,   320,   370,   436,   481,   495},
-/* 68% */{ 26,   46,   86,   147,  113,  198,  242,   306,   353,   413,   455,   467},
-/* 69% */{ 26,   44,   82,   141,  107,  190,  231,   292,   336,   391,   429,   440},
-/* 70% */{ 25,   43,   78,   136,  101,  182,  221,   278,   319,   370,   405,   414},
-/* 71% */{ 24,   41,   74,   130,  95,   174,  212,   265,   303,   350,   381,   389},
-/* 72% */{ 23,   39,   71,   125,  90,   167,  202,   252,   287,   329,   358,   365},
-/* 73% */{ 22,   37,   67,   119,  85,   159,  192,   239,   271,   310,   335,   342},
-/* 74% */{ 21,   36,   63,   114,  80,   151,  183,   226,   256,   291,   314,   320},
-/* 75% */{ 20,   34,   60,   109,  75,   144,  174,   214,   241,   273,   294,   298},
-/* 76% */{ 19,   32,   57,   104,  70,   137,  164,   202,   227,   256,   274,   278},
-/* 77% */{ 18,   31,   53,   99,   66,   130,  155,   190,   213,   239,   256,   259},
-/* 78% */{ 17,   29,   50,   94,   62,   122,  146,   178,   200,   223,   238,   241},
-/* 79% */{ 16,   28,   47,   89,   58,   115,  138,   167,   187,   208,   222,   225},
-/* 80% */{ 16,   26,   44,   84,   54,   109,  129,   156,   175,   194,   206,   209},
-/* 81% */{ 15,   24,   41,   79,   50,   102,  121,   146,   163,   180,   192,   194},
-/* 82% */{ 14,   23,   39,   74,   47,   95,   113,   136,   151,   167,   178,   181},
-/* 83% */{ 13,   21,   36,   69,   44,   89,   105,   126,   140,   155,   166,   169},
-/* 84% */{ 12,   20,   33,   64,   41,   82,   97,    116,   130,   144,   155,   158},
-/* 85% */{ 11,   19,   31,   60,   39,   76,   89,    107,   120,   134,   145,   149},
-/* 86% */{ 11,   17,   29,   55,   36,   70,   82,    98,    110,   125,   136,   140},
-/* 87% */{ 10,   16,   26,   51,   34,   64,   75,    90,    102,   116,   128,   133},
-/* 88% */{ 9,    14,   24,   46,   32,   58,   68,    81,    93,    108,   121,   128},
-/* 89% */{ 8,    13,   22,   42,   31,   52,   61,    74,    86,    102,   116,   124},
-/* 90% */{ 7,    12,   21,   37,   29,   46,   54,    66,    79,    96,    112,   121}
-};
-
-#define RSSIBUF_NUM 10
-#define RSSI2RATE_SIZE 9
-
-static TXRETRY_REC TxRateRec={MTO_MAX_DATA_RATE_LEVELS - 1, 0};   //new record=>TxRateRec
-static int TxRetryRate;
-//static int SQ3, BSS_PK_CNT, NIDLESLOT, SLOT_CNT, INTERF_CNT, GAP_CNT, DS_EVM;
-static s32 RSSIBuf[RSSIBUF_NUM]={-70, -70, -70, -70, -70, -70, -70, -70, -70, -70};
-static s32 RSSISmoothed=-700;
-static int RSSIBufIndex=0;
-static u8 max_rssi_rate;
-static int rate_tbl[13] = {0,1,2,5,11,6,9,12,18,24,36,48,54};
-//[WKCHEN]static core_data_t *pMTOcore_data=NULL;
-
 static int TotalTxPkt = 0;
 static int TotalTxPktRetry = 0;
-static int TxPktPerAnt[3] = {0,0,0};
-static int RXRSSIANT[3] ={-70,-70,-70};
-static int TxPktRetryPerAnt[3] = {0,0,0};
-//static int TxDominateFlag=FALSE;
-static u8 old_antenna[4]={1 ,0 ,1 ,0};
 static int retryrate_rec[MTO_MAX_DATA_RATE_LEVELS];//this record the retry rate at different data rate
 
 static int PeriodTotalTxPkt = 0;
@@ -221,128 +57,14 @@ typedef struct
 	u8  TxRate;
 }RSSI2RATE;
 
-static RSSI2RATE RSSI2RateTbl[RSSI2RATE_SIZE] =
-{
-	{-740, 108},  // 54M
-	{-760, 96},  // 48M
-	{-820, 72},  // 36M
-	{-850, 48},  // 24M
-	{-870, 36},  // 18M
-	{-890, 24},  // 12M
-	{-900, 12},  // 6M
-	{-920, 11}, // 5.5M
-	{-950, 4}, // 2M
-};
-static u8 untogglecount;
-static u8 last_rate_ant; //this is used for antenna backoff-hh
-
-u8	boSparseTxTraffic = FALSE;
+static u8 boSparseTxTraffic = false;
 
 void MTO_Init(MTO_FUNC_INPUT);
-void AntennaToggleInitiator(MTO_FUNC_INPUT);
-void AntennaToggleState(MTO_FUNC_INPUT);
-void TxPwrControl(MTO_FUNC_INPUT);
-void GetFreshAntennaData(MTO_FUNC_INPUT);
 void TxRateReductionCtrl(MTO_FUNC_INPUT);
 /** 1.1.31.1000 Turbo modify */
-//void MTO_SetDTORateRange(int type);
-void MTO_SetDTORateRange(MTO_FUNC_INPUT, u8 *pRateArray, u8 ArraySize);
 void MTO_SetTxCount(MTO_FUNC_INPUT, u8 t0, u8 index);
 void MTO_TxFailed(MTO_FUNC_INPUT);
-void SmoothRSSI(s32 new_rssi);
 void hal_get_dto_para(MTO_FUNC_INPUT, char *buffer);
-u8 CalcNewRate(MTO_FUNC_INPUT, u8 old_rate, u32 retry_cnt, u32 tx_frag_cnt);
-u8 GetMaxRateLevelFromRSSI(void);
-u8 MTO_GetTxFallbackRate(MTO_FUNC_INPUT);
-int Divide(int a, int b);
-void multiagc(MTO_FUNC_INPUT, u8 high_gain_mode);
-
-//===========================================================================
-//  MTO_Init --
-//
-//  Description:
-//    Set DTO Tx Rate Scope because different AP could have different Rate set.
-//    After our staion join with AP, LM core will call this function to initialize
-//    Tx Rate table.
-//
-//  Arguments:
-//    pRateArray      - The pointer to the Tx Rate Array by the following order
-//                    - 2, 4, 11, 22, 12, 18, 24, 36, 48, 72, 96, 108
-//                    - DTO won't check whether rate order is invalid or not
-//    ArraySize       - The array size to indicate how many tx rate we can choose
-//
-//  sample code:
-//	{
-//		u8 RateArray[4] = {2, 4, 11, 22};
-//		MTO_SetDTORateRange(RateArray, 4);
-//	}
-//
-//  Return Value:
-//    None
-//============================================================================
-void MTO_SetDTORateRange(MTO_FUNC_INPUT,u8 *pRateArray, u8 ArraySize)
-{
-	u8	i, j=0;
-
-	for(i=0;i<ArraySize;i++)
-	{
-		if(pRateArray[i] == 22)
-			break;
-	}
-	if(i < ArraySize) //we need adjust the order of rate list because 11Mbps rate exists
-	{
-		for(;i>0;i--)
-		{
-			if(pRateArray[i-1] <= 11)
-				break;
-			pRateArray[i] = pRateArray[i-1];
-		}
-		pRateArray[i] = 22;
-		MTO_OFDM_RATE_LEVEL() = i;
-	}
-	else
-	{
-		for(i=0; i<ArraySize; i++)
-		{
-			if (pRateArray[i] >= 12)
-				break;
-		}
-		MTO_OFDM_RATE_LEVEL() = i;
-	}
-
-	for(i=0;i<ArraySize;i++)
-	{
-		MTO_Data_Rate_Tbl[i] = pRateArray[i];
-		for(;j<MTO_MAX_DATA_RATE_LEVELS;j++)
-		{
-			if(Stardard_Data_Rate_Tbl[j] == pRateArray[i])
-				break;
-		}
-		Level2PerTbl[i] = j;
-		#ifdef _PE_DTO_DUMP_
-		WBDEBUG(("[MTO]:Op Rate[%d]: %d\n",i, MTO_Data_Rate_Tbl[i]));
-		#endif
-	}
-	MTO_DataRateAvailableLevel = ArraySize;
-	if( MTO_DATA().RatePolicy ) // 0 means that no registry setting
-	{
-		if( MTO_DATA().RatePolicy == 1 )
-			TxRateRec.tx_rate = 0;	//ascent
-		else
-			TxRateRec.tx_rate = MTO_DataRateAvailableLevel -1 ;	//descent
-	}
-	else
-	{
-		if( MTO_INITTXRATE_MODE )
-			TxRateRec.tx_rate = 0;	//ascent
-		else
-			TxRateRec.tx_rate = MTO_DataRateAvailableLevel -1 ;	//descent
-	}
-	TxRateRec.tx_retry_rate = 0;
-	//set default rate for initial use
-	MTO_RATE_LEVEL() = TxRateRec.tx_rate;
-	MTO_FALLBACK_RATE_LEVEL() = MTO_RATE_LEVEL();
-}
 
 //===========================================================================
 //  MTO_Init --
@@ -353,7 +75,7 @@ void MTO_SetDTORateRange(MTO_FUNC_INPUT,u8 *pRateArray, u8 ArraySize)
 //    This function should be invoked during system initialization.
 //
 //  Arguments:
-//    Adapter      - The pointer to the Miniport Adapter Context
+//    adapter      - The pointer to the Miniport adapter Context
 //
 //  Return Value:
 //    None
@@ -393,8 +115,8 @@ void MTO_Init(MTO_FUNC_INPUT)
     MTO_SQ_ANT(0)       = 0;
     MTO_SQ_ANT(1)       = 0;
     MTO_ANT_DIVERSITY() = MTO_ANTENNA_DIVERSITY_ON;
-    //CardSet_AntennaDiversity(Adapter, MTO_ANT_DIVERSITY());
-    //PLMESetAntennaDiversity( Adapter, MTO_ANT_DIVERSITY());
+    //CardSet_AntennaDiversity(adapter, MTO_ANT_DIVERSITY());
+    //PLMESetAntennaDiversity( adapter, MTO_ANT_DIVERSITY());
 
     MTO_AGING_TIMEOUT() = 0;//MTO_TMR_AGING() / MTO_TMR_PERIODIC();
 
@@ -402,7 +124,6 @@ void MTO_Init(MTO_FUNC_INPUT)
     //
     //MTO_RATE_LEVEL()            = 10;
     MTO_RATE_LEVEL()            = 0;
-	MTO_FALLBACK_RATE_LEVEL()	= MTO_RATE_LEVEL();
     MTO_FRAG_TH_LEVEL()         = 4;
     /** 1.1.23.1000 Turbo modify from -1 to +1
 	MTO_RTS_THRESHOLD()         = MTO_FRAG_TH() - 1;
@@ -470,669 +191,6 @@ void MTO_Init(MTO_FUNC_INPUT)
 	MTO_DATA().RSSI_low = -60;
 }
 
-//---------------------------------------------------------------------------//
-static u32 DTO_Rx_Info[13][3];
-static u32 DTO_RxCRCFail_Info[13][3];
-static u32 AntennaToggleBkoffTimer=5;
-typedef struct{
-	int RxRate;
-	int RxRatePkts;
-	int index;
-}RXRATE_ANT;
-RXRATE_ANT RxRatePeakAnt[3];
-
-#define ANT0    0
-#define ANT1    1
-#define OLD_ANT 2
-
-void SearchPeakRxRate(int index)
-{
-	int i;
-	RxRatePeakAnt[index].RxRatePkts=0;
-	//Find out the best rx rate which is used on different antenna
-	for(i=1;i<13;i++)
-	{
-		if(DTO_Rx_Info[i][index] > (u32) RxRatePeakAnt[index].RxRatePkts)
-		{
-			RxRatePeakAnt[index].RxRatePkts = DTO_Rx_Info[i][index];
-			RxRatePeakAnt[index].RxRate = rate_tbl[i];
-			RxRatePeakAnt[index].index = i;
-		}
-	}
-}
-
-void ResetDTO_RxInfo(int index, MTO_FUNC_INPUT)
-{
-	int i;
-
-	#ifdef _PE_DTO_DUMP_
-	WBDEBUG(("ResetDTOrx\n"));
-	#endif
-
-	for(i=0;i<13;i++)
-		DTO_Rx_Info[i][index] = MTO_HAL()->rx_ok_count[i];
-
-	for(i=0;i<13;i++)
-		DTO_RxCRCFail_Info[i][index] = MTO_HAL()->rx_err_count[i];
-
-	TotalTxPkt = 0;
-	TotalTxPktRetry = 0;
-}
-
-void GetDTO_RxInfo(int index, MTO_FUNC_INPUT)
-{
-	int i;
-
-	#ifdef _PE_DTO_DUMP_
-	WBDEBUG(("GetDTOrx\n"));
-	#endif
-
-	//PDEBUG(("[MTO]:DTO_Rx_Info[%d]=%d, rx_ok_count=%d\n", index, DTO_Rx_Info[0][index], phw_data->rx_ok_count[0]));
-	for(i=0;i<13;i++)
-		DTO_Rx_Info[i][index] = abs(MTO_HAL()->rx_ok_count[i] - DTO_Rx_Info[i][index]);
-	if(DTO_Rx_Info[0][index]==0) DTO_Rx_Info[0][index] = 1;
-
-	for(i=0;i<13;i++)
-		DTO_RxCRCFail_Info[i][index] = MTO_HAL()->rx_err_count[i] - DTO_RxCRCFail_Info[i][index];
-
-	TxPktPerAnt[index] = TotalTxPkt;
-	TxPktRetryPerAnt[index] = TotalTxPktRetry;
-	TotalTxPkt = 0;
-	TotalTxPktRetry = 0;
-}
-
-void OutputDebugInfo(int index1, int index2)
-{
-	#ifdef _PE_DTO_DUMP_
-	WBDEBUG(("[HHDTO]:Total Rx (%d)\t\t(%d) \n ", DTO_Rx_Info[0][index1], DTO_Rx_Info[0][index2]));
-    WBDEBUG(("[HHDTO]:RECEIVE RSSI: (%d)\t\t(%d) \n ", RXRSSIANT[index1], RXRSSIANT[index2]));
-	WBDEBUG(("[HHDTO]:TX packet correct rate: (%d)%%\t\t(%d)%% \n ",Divide(TxPktPerAnt[index1]*100,TxPktRetryPerAnt[index1]), Divide(TxPktPerAnt[index2]*100,TxPktRetryPerAnt[index2])));
-	#endif
-	{
-		int tmp1, tmp2;
-		#ifdef _PE_DTO_DUMP_
-		WBDEBUG(("[HHDTO]:Total Tx (%d)\t\t(%d) \n ", TxPktPerAnt[index1], TxPktPerAnt[index2]));
-		WBDEBUG(("[HHDTO]:Total Tx retry (%d)\t\t(%d) \n ", TxPktRetryPerAnt[index1], TxPktRetryPerAnt[index2]));
-		#endif
-		tmp1 = TxPktPerAnt[index1] + DTO_Rx_Info[0][index1];
-		tmp2 = TxPktPerAnt[index2] + DTO_Rx_Info[0][index2];
-		#ifdef _PE_DTO_DUMP_
-		WBDEBUG(("[HHDTO]:Total Tx+RX (%d)\t\t(%d) \n ", tmp1, tmp2));
-		#endif
-	}
-}
-
-unsigned char TxDominate(int index)
-{
-	int tmp;
-
-	tmp = TxPktPerAnt[index] + DTO_Rx_Info[0][index];
-
-	if(Divide(TxPktPerAnt[index]*100, tmp) > 40)
-		return TRUE;
-	else
-		return FALSE;
-}
-
-unsigned char CmpTxRetryRate(int index1, int index2)
-{
-	int tx_retry_rate1, tx_retry_rate2;
-	tx_retry_rate1 = Divide((TxPktRetryPerAnt[index1] - TxPktPerAnt[index1])*100, TxPktRetryPerAnt[index1]);
-	tx_retry_rate2 = Divide((TxPktRetryPerAnt[index2] - TxPktPerAnt[index2])*100, TxPktRetryPerAnt[index2]);
-	#ifdef _PE_DTO_DUMP_
-	WBDEBUG(("[MTO]:TxRetry Ant0: (%d%%)  Ant1: (%d%%) \n ", tx_retry_rate1, tx_retry_rate2));
-	#endif
-
-	if(tx_retry_rate1 > tx_retry_rate2)
-		return TRUE;
-	else
-		return FALSE;
-}
-
-void GetFreshAntennaData(MTO_FUNC_INPUT)
-{
-    u8      x;
-
-	x = hal_get_antenna_number(MTO_HAL());
-	//hal_get_bss_pk_cnt(MTO_HAL());
-	//hal_get_est_sq3(MTO_HAL(), 1);
-	old_antenna[0] = x;
-	//if this is the function for timer
-	ResetDTO_RxInfo(x, MTO_FUNC_INPUT_DATA);
-	if(AntennaToggleBkoffTimer)
-			AntennaToggleBkoffTimer--;
-	if (abs(last_rate_ant-MTO_RATE_LEVEL())>1)  //backoff timer reset
-		AntennaToggleBkoffTimer=0;
-
-	if (MTO_ANT_DIVERSITY() != MTO_ANTENNA_DIVERSITY_ON ||
-		MTO_ANT_DIVERSITY_ENABLE() != 1)
-	AntennaToggleBkoffTimer=1;
-	#ifdef _PE_DTO_DUMP_
-	WBDEBUG(("[HHDTO]:**last data rate=%d,now data rate=%d**antenna toggle timer=%d",last_rate_ant,MTO_RATE_LEVEL(),AntennaToggleBkoffTimer));
-	#endif
-	last_rate_ant=MTO_RATE_LEVEL();
-	if(AntennaToggleBkoffTimer==0)
-	{
-		MTO_TOGGLE_STATE() = TOGGLE_STATE_WAIT0;
-		#ifdef _PE_DTO_DUMP_
-		WBDEBUG(("[HHDTO]:===state is starting==for antenna toggle==="));
-		#endif
-	}
-	else
-		MTO_TOGGLE_STATE() = TOGGLE_STATE_IDLE;
-
-	if ((MTO_BACKOFF_TMR()!=0)&&(MTO_RATE_LEVEL()>MTO_DataRateAvailableLevel - 3))
-	{
-		MTO_TOGGLE_STATE() = TOGGLE_STATE_IDLE;
-		#ifdef _PE_DTO_DUMP_
-		WBDEBUG(("[HHDTO]:===the data rate is %d (good)and will not toogle  ===",MTO_DATA_RATE()>>1));
-		#endif
-	}
-
-
-}
-
-int WB_PCR[2]; //packet correct rate
-
-void AntennaToggleState(MTO_FUNC_INPUT)
-{
-    int decideantflag = 0;
-	u8      x;
-	s32     rssi;
-
-	if(MTO_ANT_DIVERSITY_ENABLE() != 1)
-		return;
-	x = hal_get_antenna_number(MTO_HAL());
-	switch(MTO_TOGGLE_STATE())
-	{
-
-	   //Missing.....
-	   case TOGGLE_STATE_IDLE:
-	 case TOGGLE_STATE_BKOFF:
-	             break;;
-
-		case TOGGLE_STATE_WAIT0://========
-	               GetDTO_RxInfo(x, MTO_FUNC_INPUT_DATA);
-			sme_get_rssi(MTO_FUNC_INPUT_DATA, &rssi);
-			RXRSSIANT[x] = rssi;
-			#ifdef _PE_DTO_DUMP_
-			WBDEBUG(("[HHDTO] **wait0==== Collecting Ant%d--rssi=%d\n", x,RXRSSIANT[x]));
-			#endif
-
-			//change antenna and reset the data at changed antenna
-			x = (~x) & 0x01;
-			MTO_ANT_SEL() = x;
-			hal_set_antenna_number(MTO_HAL(), MTO_ANT_SEL());
-			LOCAL_ANTENNA_NO() = x;
-
-			MTO_TOGGLE_STATE() = TOGGLE_STATE_WAIT1;//go to wait1
-			ResetDTO_RxInfo(x, MTO_FUNC_INPUT_DATA);
-			break;
-		case TOGGLE_STATE_WAIT1://=====wait1
-			//MTO_CNT_ANT(x) = hal_get_bss_pk_cnt(MTO_HAL());
-			//RXRSSIANT[x] = hal_get_rssi(MTO_HAL());
-			sme_get_rssi(MTO_FUNC_INPUT_DATA, &rssi);
-			RXRSSIANT[x] = rssi;
-			GetDTO_RxInfo(x, MTO_FUNC_INPUT_DATA);
-			#ifdef _PE_DTO_DUMP_
-			WBDEBUG(("[HHDTO] **wait1==== Collecting Ant%d--rssi=%d\n", x,RXRSSIANT[x]));
-			#endif
-			MTO_TOGGLE_STATE() = TOGGLE_STATE_MAKEDESISION;
-			break;
-		case TOGGLE_STATE_MAKEDESISION:
-			#ifdef _PE_DTO_DUMP_
-			WBDEBUG(("[HHDTO]:Ant--0-----------------1---\n"));
-			OutputDebugInfo(ANT0,ANT1);
-			#endif
-			//PDEBUG(("[HHDTO] **decision====\n "));
-
-			//=====following is the decision produrce
-			//
-			//    first: compare the rssi if difference >10
-			//           select the larger one
-			//           ,others go to second
-			//    second: comapre the tx+rx packet count if difference >100
-			//            use larger total packets antenna
-			//    third::compare the tx PER if packets>20
-			//                           if difference >5% using the bigger one
-			//
-			//    fourth:compare the RX PER if packets>20
-			//                    if PER difference <5%
-			//                   using old antenna
-			//
-			//
-			if (abs(RXRSSIANT[ANT0]-RXRSSIANT[ANT1]) > MTOPARA_RSSI_TH_FOR_ANTDIV())//====rssi_th
-			{
-				if (RXRSSIANT[ANT0]>RXRSSIANT[ANT1])
-				{
-					decideantflag=1;
-					MTO_ANT_MAC() = ANT0;
-				}
-				else
-				{
-					decideantflag=1;
-					MTO_ANT_MAC() = ANT1;
-				}
-				#ifdef _PE_DTO_DUMP_
-				WBDEBUG(("Select antenna by RSSI\n"));
-				#endif
-			}
-			else if  (abs(TxPktPerAnt[ANT0] + DTO_Rx_Info[0][ANT0]-TxPktPerAnt[ANT1]-DTO_Rx_Info[0][ANT1])<50)//=====total packet_th
-			{
-				#ifdef _PE_DTO_DUMP_
-				WBDEBUG(("Total tx/rx is close\n"));
-				#endif
-				if (TxDominate(ANT0) && TxDominate(ANT1))
-				{
-					if ((TxPktPerAnt[ANT0]>10) && (TxPktPerAnt[ANT1]>10))//====tx packet_th
-					{
-						WB_PCR[ANT0]=Divide(TxPktPerAnt[ANT0]*100,TxPktRetryPerAnt[ANT0]);
-						WB_PCR[ANT1]=Divide(TxPktPerAnt[ANT1]*100,TxPktRetryPerAnt[ANT1]);
-						if (abs(WB_PCR[ANT0]-WB_PCR[ANT1])>5)// tx PER_th
-						{
-							#ifdef _PE_DTO_DUMP_
-							WBDEBUG(("Decide by Tx correct rate\n"));
-							#endif
-							if (WB_PCR[ANT0]>WB_PCR[ANT1])
-							{
-								decideantflag=1;
-								MTO_ANT_MAC() = ANT0;
-							}
-							else
-							{
-								decideantflag=1;
-								MTO_ANT_MAC() = ANT1;
-							}
-						}
-						else
-						{
-							decideantflag=0;
-							untogglecount++;
-							MTO_ANT_MAC() = old_antenna[0];
-						}
-					}
-					else
-					{
-						decideantflag=0;
-						MTO_ANT_MAC() = old_antenna[0];
-					}
-				}
-				else if ((DTO_Rx_Info[0][ANT0]>10)&&(DTO_Rx_Info[0][ANT1]>10))//rx packet th
-				{
-					#ifdef _PE_DTO_DUMP_
-					WBDEBUG(("Decide by Rx\n"));
-					#endif
-					if (abs(DTO_Rx_Info[0][ANT0] - DTO_Rx_Info[0][ANT1])>50)
-					{
-						if (DTO_Rx_Info[0][ANT0] > DTO_Rx_Info[0][ANT1])
-						{
-							decideantflag=1;
-							MTO_ANT_MAC() = ANT0;
-						}
-						else
-						{
-							decideantflag=1;
-							MTO_ANT_MAC() = ANT1;
-						}
-					}
-					else
-					{
-						decideantflag=0;
-						untogglecount++;
-						MTO_ANT_MAC() = old_antenna[0];
-					}
-				}
-				else
-				{
-					decideantflag=0;
-					MTO_ANT_MAC() = old_antenna[0];
-				}
-			}
-			else if ((TxPktPerAnt[ANT0]+DTO_Rx_Info[0][ANT0])>(TxPktPerAnt[ANT1]+DTO_Rx_Info[0][ANT1]))//use more packekts
-			{
-				#ifdef _PE_DTO_DUMP_
-				WBDEBUG(("decide by total tx/rx : ANT 0\n"));
-				#endif
-
-				decideantflag=1;
-				MTO_ANT_MAC() = ANT0;
-			}
-			else
-			{
-				#ifdef _PE_DTO_DUMP_
-				WBDEBUG(("decide by total tx/rx : ANT 1\n"));
-				#endif
-				decideantflag=1;
-				MTO_ANT_MAC() = ANT1;
-
-			}
-			//this is force ant toggle
-			if (decideantflag==1)
-				untogglecount=0;
-
-			untogglecount=untogglecount%4;
-			if (untogglecount==3) //change antenna
-				MTO_ANT_MAC() = ((~old_antenna[0]) & 0x1);
-			#ifdef _PE_DTO_DUMP_
-			WBDEBUG(("[HHDTO]:==================untoggle-count=%d",untogglecount));
-			#endif
-
-
-
-
-			//PDEBUG(("[HHDTO] **********************************DTO ENABLE=%d",MTO_ANT_DIVERSITY_ENABLE()));
-			if(MTO_ANT_DIVERSITY_ENABLE() == 1)
-			{
-					MTO_ANT_SEL() = MTO_ANT_MAC();
-					hal_set_antenna_number(MTO_HAL(), MTO_ANT_SEL());
-					LOCAL_ANTENNA_NO() = MTO_ANT_SEL();
-					#ifdef _PE_DTO_DUMP_
-					WBDEBUG(("[HHDTO] ==decision==*******antflag=%d******************selected antenna=%d\n",decideantflag,MTO_ANT_SEL()));
-					#endif
-			}
-			if (decideantflag)
-			{
-				old_antenna[3]=old_antenna[2];//store antenna info
-				old_antenna[2]=old_antenna[1];
-				old_antenna[1]=old_antenna[0];
-				old_antenna[0]= MTO_ANT_MAC();
-			}
-			#ifdef _PE_DTO_DUMP_
-			WBDEBUG(("[HHDTO]:**old antenna=[%d][%d][%d][%d]\n",old_antenna[0],old_antenna[1],old_antenna[2],old_antenna[3]));
-			#endif
-			if (old_antenna[0]!=old_antenna[1])
-				AntennaToggleBkoffTimer=0;
-			else if (old_antenna[1]!=old_antenna[2])
-				AntennaToggleBkoffTimer=1;
-			else if (old_antenna[2]!=old_antenna[3])
-				AntennaToggleBkoffTimer=2;
-			else
-				AntennaToggleBkoffTimer=4;
-
-			#ifdef _PE_DTO_DUMP_
-			WBDEBUG(("[HHDTO]:**back off timer=%d",AntennaToggleBkoffTimer));
-			#endif
-
-			ResetDTO_RxInfo(MTO_ANT_MAC(), MTO_FUNC_INPUT_DATA);
-			if (AntennaToggleBkoffTimer==0 && decideantflag)
-				MTO_TOGGLE_STATE() = TOGGLE_STATE_WAIT0;
-			else
-				MTO_TOGGLE_STATE() = TOGGLE_STATE_IDLE;
-			break;
-	}
-
-}
-
-void multiagc(MTO_FUNC_INPUT, u8 high_gain_mode )
-{
-	s32		rssi;
-	hw_data_t	*pHwData = MTO_HAL();
-
-	sme_get_rssi(MTO_FUNC_INPUT_DATA, &rssi);
-
-	if( (RF_WB_242 == pHwData->phy_type) ||
-		(RF_WB_242_1 == pHwData->phy_type) ) // 20060619.5 Add
-	{
-		if (high_gain_mode==1)
-		{
-			//hw_set_dxx_reg(phw_data, 0x0C, 0xf8f52230);
-			//hw_set_dxx_reg(phw_data, 0x20, 0x06C43440);
-			Wb35Reg_Write( pHwData, 0x100C, 0xF2F32232 ); // 940916 0xf8f52230 );
-			Wb35Reg_Write( pHwData, 0x1020, 0x04cb3440 ); // 940915 0x06C43440
-		}
-		else if (high_gain_mode==0)
-		{
-			//hw_set_dxx_reg(phw_data, 0x0C, 0xEEEE000D);
-			//hw_set_dxx_reg(phw_data, 0x20, 0x06c41440);
-			Wb35Reg_Write( pHwData, 0x100C, 0xEEEE000D );
-			Wb35Reg_Write( pHwData, 0x1020, 0x04cb1440 ); // 940915 0x06c41440
-		}
-		#ifdef _PE_DTO_DUMP_
-		WBDEBUG(("[HHDTOAGC] **rssi=%d, high gain mode=%d", rssi, high_gain_mode));
-		#endif
-	}
-}
-
-void TxPwrControl(MTO_FUNC_INPUT)
-{
-    s32     rssi;
-	hw_data_t	*pHwData = MTO_HAL();
-
-	sme_get_rssi(MTO_FUNC_INPUT_DATA, &rssi);
-	if( (RF_WB_242 == pHwData->phy_type) ||
-		(RF_WB_242_1 == pHwData->phy_type) ) // 20060619.5 Add
-	{
-		static u8 high_gain_mode; //this is for winbond RF switch LNA
-		                          //using different register setting
-
-		if (high_gain_mode==1)
-		{
-			if( rssi > MTO_DATA().RSSI_high )
-			{
-				//hw_set_dxx_reg(phw_data, 0x0C, 0xf8f52230);
-				//hw_set_dxx_reg(phw_data, 0x20, 0x05541640);
-				high_gain_mode=0;
-			}
-			else
-			{
-				//hw_set_dxx_reg(phw_data, 0x0C, 0xf8f51830);
-				//hw_set_dxx_reg(phw_data, 0x20, 0x05543E40);
-				high_gain_mode=1;
-			}
-		}
-		else //if (high_gain_mode==0)
-		{
-			if( rssi < MTO_DATA().RSSI_low )
-			{
-				//hw_set_dxx_reg(phw_data, 0x0C, 0xf8f51830);
-				//hw_set_dxx_reg(phw_data, 0x20, 0x05543E40);
-				high_gain_mode=1;
-			}
-			else
-			{
-				//hw_set_dxx_reg(phw_data, 0x0C, 0xf8f52230);
-				//hw_set_dxx_reg(phw_data, 0x20, 0x05541640);
-				high_gain_mode=0;
-			}
-		}
-
-		// Always high gain 20051014. Using the initial value only.
-		multiagc(MTO_FUNC_INPUT_DATA, high_gain_mode);
-	}
-}
-
-
-u8 CalcNewRate(MTO_FUNC_INPUT, u8 old_rate, u32 retry_cnt, u32 tx_frag_cnt)
-{
-	int i;
-	u8 new_rate;
-    u32 retry_rate;
-	int TxThrouput1, TxThrouput2, TxThrouput3, BestThroupht;
-
-	if(tx_frag_cnt < MTOPARA_TXCOUNT_TH_FOR_CALC_RATE()) //too few packets transmit
-	{
-		return 0xff;
-	}
-	retry_rate = Divide(retry_cnt * 100, tx_frag_cnt);
-
-	if(retry_rate > 90) retry_rate = 90; //always truncate to 90% due to lookup table size
-	#ifdef _PE_DTO_DUMP_
-	WBDEBUG(("##### Current level =%d, Retry count =%d, Frag count =%d\n",
-						old_rate, retry_cnt, tx_frag_cnt));
-	WBDEBUG(("*##* Retry rate =%d, throughput =%d\n",
-						retry_rate, Rate_PER_TBL[retry_rate][old_rate]));
-	WBDEBUG(("TxRateRec.tx_rate =%d, Retry rate = %d, throughput = %d\n",
-						TxRateRec.tx_rate, TxRateRec.tx_retry_rate,
-						Rate_PER_TBL[TxRateRec.tx_retry_rate][Level2PerTbl[TxRateRec.tx_rate]]));
-	WBDEBUG(("old_rate-1 =%d, Retry rate = %d, throughput = %d\n",
-						old_rate-1, retryrate_rec[old_rate-1],
-						Rate_PER_TBL[retryrate_rec[old_rate-1]][old_rate-1]));
-	WBDEBUG(("old_rate+1 =%d, Retry rate = %d, throughput = %d\n",
-						old_rate+1, retryrate_rec[old_rate+1],
-						Rate_PER_TBL[retryrate_rec[old_rate+1]][old_rate+1]));
-	#endif
-
-	//following is for record the retry rate at the different data rate
-	if (abs(retry_rate-retryrate_rec[old_rate])<50)//---the per TH
-		retryrate_rec[old_rate] = retry_rate; //update retry rate
-	else
-	{
-		for (i=0;i<MTO_DataRateAvailableLevel;i++) //reset all retry rate
-			retryrate_rec[i]=0;
-		retryrate_rec[old_rate] = retry_rate;
-		#ifdef _PE_DTO_DUMP_
-		WBDEBUG(("Reset retry rate table\n"));
-		#endif
-	}
-
-	if(TxRateRec.tx_rate > old_rate)   //Decrease Tx Rate
-	{
-		TxThrouput1 = Rate_PER_TBL[TxRateRec.tx_retry_rate][Level2PerTbl[TxRateRec.tx_rate]];
-		TxThrouput2 = Rate_PER_TBL[retry_rate][Level2PerTbl[old_rate]];
-		if(TxThrouput1 > TxThrouput2)
-		{
-			new_rate = TxRateRec.tx_rate;
-			BestThroupht = TxThrouput1;
-		}
-		else
-		{
-			new_rate = old_rate;
-			BestThroupht = TxThrouput2;
-		}
-		if((old_rate > 0) &&(retry_rate>MTOPARA_TXRATE_DEC_TH()))   //Min Rate
-		{
-			TxThrouput3 = Rate_PER_TBL[retryrate_rec[old_rate-1]][Level2PerTbl[old_rate-1]];
-			if(BestThroupht < TxThrouput3)
-			{
-				new_rate = old_rate - 1;
-				#ifdef _PE_DTO_DUMP_
-				WBDEBUG(("--------\n"));
-				#endif
-				BestThroupht = TxThrouput3;
-			}
-		}
-	}
-	else if(TxRateRec.tx_rate < old_rate)  //Increase Tx Rate
-	{
-		TxThrouput1 = Rate_PER_TBL[TxRateRec.tx_retry_rate][Level2PerTbl[TxRateRec.tx_rate]];
-		TxThrouput2 = Rate_PER_TBL[retry_rate][Level2PerTbl[old_rate]];
-		if(TxThrouput1 > TxThrouput2)
-		{
-			new_rate = TxRateRec.tx_rate;
-			BestThroupht = TxThrouput1;
-		}
-		else
-		{
-			new_rate = old_rate;
-			BestThroupht = TxThrouput2;
-		}
-		if ((old_rate < MTO_DataRateAvailableLevel - 1)&&(retry_rate<MTOPARA_TXRATE_INC_TH()))
-		{
-			//TxThrouput3 = Rate_PER_TBL[retryrate_rec[old_rate+1]][Level2PerTbl[old_rate+1]];
-			if (retryrate_rec[old_rate+1] > MTOPARA_TXRETRYRATE_REDUCE())
-				TxThrouput3 = Rate_PER_TBL[retryrate_rec[old_rate+1]-MTOPARA_TXRETRYRATE_REDUCE()][Level2PerTbl[old_rate+1]];
-			else
-				TxThrouput3 = Rate_PER_TBL[retryrate_rec[old_rate+1]][Level2PerTbl[old_rate+1]];
-			if(BestThroupht < TxThrouput3)
-			{
-				new_rate = old_rate + 1;
-				#ifdef _PE_DTO_DUMP_
-				WBDEBUG(("++++++++++\n"));
-				#endif
-				BestThroupht = TxThrouput3;
-			}
-		}
-	}
-	else  //Tx Rate no change
-	{
-		TxThrouput2 = Rate_PER_TBL[retry_rate][Level2PerTbl[old_rate]];
-		new_rate = old_rate;
-		BestThroupht = TxThrouput2;
-
-		if (retry_rate <MTOPARA_TXRATE_EQ_TH())    //th for change higher rate
-		{
-			if(old_rate < MTO_DataRateAvailableLevel - 1)
-			{
-				//TxThrouput3 = Rate_PER_TBL[retryrate_rec[old_rate+1]][Level2PerTbl[old_rate+1]];
-				if (retryrate_rec[old_rate+1] > MTOPARA_TXRETRYRATE_REDUCE())
-					TxThrouput3 = Rate_PER_TBL[retryrate_rec[old_rate+1]-MTOPARA_TXRETRYRATE_REDUCE()][Level2PerTbl[old_rate+1]];
-				else
-					TxThrouput3 = Rate_PER_TBL[retryrate_rec[old_rate+1]][Level2PerTbl[old_rate+1]];
-				if(BestThroupht < TxThrouput3)
-				{
-					new_rate = old_rate + 1;
-					BestThroupht = TxThrouput3;
-					#ifdef _PE_DTO_DUMP_
-					WBDEBUG(("=++++++++++\n"));
-					#endif
-				}
-			}
-		}
-		else
-		if(old_rate > 0)   //Min Rate
-		{
-			TxThrouput3 = Rate_PER_TBL[retryrate_rec[old_rate-1]][Level2PerTbl[old_rate-1]];
-			if(BestThroupht < TxThrouput3)
-			{
-				new_rate = old_rate - 1;
-				#ifdef _PE_DTO_DUMP_
-				WBDEBUG(("=--------\n"));
-				#endif
-				BestThroupht = TxThrouput3;
-			}
-		}
-	}
-
-	if (!LOCAL_IS_IBSS_MODE())
-	{
-	max_rssi_rate = GetMaxRateLevelFromRSSI();
-	#ifdef _PE_DTO_DUMP_
-	WBDEBUG(("[MTO]:RSSI2Rate=%d\n", MTO_Data_Rate_Tbl[max_rssi_rate]));
-	#endif
-	if(new_rate > max_rssi_rate)
-		new_rate = max_rssi_rate;
-	}
-
-	//save new rate;
-	TxRateRec.tx_rate = old_rate;
-	TxRateRec.tx_retry_rate = (u8) retry_rate;
-	TxRetryRate = retry_rate;
-	return new_rate;
-}
-
-void SmoothRSSI(s32 new_rssi)
-{
-	RSSISmoothed = RSSISmoothed + new_rssi - RSSIBuf[RSSIBufIndex];
-	RSSIBuf[RSSIBufIndex] = new_rssi;
-	RSSIBufIndex = (RSSIBufIndex + 1) % 10;
-}
-
-u8 GetMaxRateLevelFromRSSI(void)
-{
-	u8 i;
-	u8 TxRate;
-
-	for(i=0;i<RSSI2RATE_SIZE;i++)
-	{
-		if(RSSISmoothed > RSSI2RateTbl[i].RSSI)
-			break;
-	}
-	#ifdef _PE_DTO_DUMP_
-	WBDEBUG(("[MTO]:RSSI=%d\n", Divide(RSSISmoothed, 10)));
-	#endif
-	if(i < RSSI2RATE_SIZE)
-		TxRate = RSSI2RateTbl[i].TxRate;
-	else
-		TxRate = 2;  //divided by 2 = 1Mbps
-
-	for(i=MTO_DataRateAvailableLevel-1;i>0;i--)
-	{
-		if(TxRate >=MTO_Data_Rate_Tbl[i])
-			break;
-	}
-	return i;
-}
-
 //===========================================================================
 //  Description:
 //      If we enable DTO, we will ignore the tx count with different tx rate from
@@ -1194,36 +252,3 @@ void MTO_SetTxCount(MTO_FUNC_INPUT, u8 tx_rate, u8 index)
 	PeriodTotalTxPkt ++;
 	PeriodTotalTxPktRetry += (index+1);
 }
-
-u8 MTO_GetTxFallbackRate(MTO_FUNC_INPUT)
-{
-	return MTO_DATA_FALLBACK_RATE();
-}
-
-
-//===========================================================================
-//  MTO_TxFailed --
-//
-//  Description:
-//    Failure of transmitting a packet indicates that certain MTO parmeters
-//    may need to be adjusted. This function is called when NIC just failed
-//    to transmit a packet or when MSDULifeTime expired.
-//
-//  Arguments:
-//    Adapter      - The pointer to the Miniport Adapter Context
-//
-//  Return Value:
-//    None
-//============================================================================
-void MTO_TxFailed(MTO_FUNC_INPUT)
-{
-	return;
-}
-
-int Divide(int a, int b)
-{
-	if (b==0) b=1;
-	return a/b;
-}
-
-
diff --git a/drivers/staging/winbond/mto.h b/drivers/staging/winbond/mto.h
index f47936f46d1..0d3775eb257 100644
--- a/drivers/staging/winbond/mto.h
+++ b/drivers/staging/winbond/mto.h
@@ -11,6 +11,8 @@
 #ifndef __MTO_H__
 #define __MTO_H__
 
+#include <linux/types.h>
+
 #define MTO_DEFAULT_TH_CNT              5
 #define MTO_DEFAULT_TH_SQ3              112  //OLD IS 13 reference JohnXu
 #define MTO_DEFAULT_TH_IDLE_SLOT        15
@@ -129,17 +131,17 @@ typedef struct _MTO_PARAMETERS
 } MTO_PARAMETERS, *PMTO_PARAMETERS;
 
 
-#define MTO_FUNC_INPUT              PWB32_ADAPTER	Adapter
-#define MTO_FUNC_INPUT_DATA         Adapter
-#define MTO_DATA()                  (Adapter->sMtoPara)
-#define MTO_HAL()                   (&Adapter->sHwData)
+#define MTO_FUNC_INPUT              struct wbsoft_priv *	adapter
+#define MTO_FUNC_INPUT_DATA         adapter
+#define MTO_DATA()                  (adapter->sMtoPara)
+#define MTO_HAL()                   (&adapter->sHwData)
 #define MTO_SET_PREAMBLE_TYPE(x)    // 20040511 Turbo mark LM_PREAMBLE_TYPE(&pcore_data->lm_data) = (x)
-#define MTO_ENABLE					(Adapter->sLocalPara.TxRateMode == RATE_AUTO)
-#define MTO_TXPOWER_FROM_EEPROM		(Adapter->sHwData.PowerIndexFromEEPROM)
-#define LOCAL_ANTENNA_NO()			(Adapter->sLocalPara.bAntennaNo)
-#define LOCAL_IS_CONNECTED()		(Adapter->sLocalPara.wConnectedSTAindex != 0)
-#define LOCAL_IS_IBSS_MODE()		(Adapter->asBSSDescriptElement[Adapter->sLocalPara.wConnectedSTAindex].bBssType == IBSS_NET)
-#define MTO_INITTXRATE_MODE			(Adapter->sHwData.SoftwareSet&0x2)	//bit 1
+#define MTO_ENABLE					(adapter->sLocalPara.TxRateMode == RATE_AUTO)
+#define MTO_TXPOWER_FROM_EEPROM		(adapter->sHwData.PowerIndexFromEEPROM)
+#define LOCAL_ANTENNA_NO()			(adapter->sLocalPara.bAntennaNo)
+#define LOCAL_IS_CONNECTED()		(adapter->sLocalPara.wConnectedSTAindex != 0)
+#define LOCAL_IS_IBSS_MODE()		(adapter->asBSSDescriptElement[adapter->sLocalPara.wConnectedSTAindex].bBssType == IBSS_NET)
+#define MTO_INITTXRATE_MODE			(adapter->sHwData.SoftwareSet&0x2)	//bit 1
 // 20040510 Turbo add
 #define MTO_TMR_CNT()               MTO_DATA().TmrCnt
 #define MTO_TOGGLE_STATE()          MTO_DATA().ToggleState
@@ -157,7 +159,7 @@ typedef struct _MTO_PARAMETERS
 #define MTO_TMR_PERIODIC()          MTO_DATA().Tmr_Periodic
 
 #define MTO_POWER_CHANGE_ENABLE()   MTO_DATA().PowerChangeEnable
-#define MTO_ANT_DIVERSITY_ENABLE()  Adapter->sLocalPara.boAntennaDiversity
+#define MTO_ANT_DIVERSITY_ENABLE()  adapter->sLocalPara.boAntennaDiversity
 #define MTO_ANT_MAC()               MTO_DATA().Ant_mac
 #define MTO_ANT_DIVERSITY()         MTO_DATA().Ant_div
 #define MTO_CCA_MODE()              MTO_DATA().CCA_Mode
@@ -166,7 +168,6 @@ typedef struct _MTO_PARAMETERS
 #define MTO_PREAMBLE_CHANGE_ENABLE()         MTO_DATA().PreambleChangeEnable
 
 #define MTO_RATE_LEVEL()            MTO_DATA().DataRateLevel
-#define MTO_FALLBACK_RATE_LEVEL()	MTO_DATA().FallbackRateLevel
 #define MTO_OFDM_RATE_LEVEL()		MTO_DATA().OfdmRateLevel
 #define MTO_RATE_CHANGE_ENABLE()    MTO_DATA().DataRateChangeEnable
 #define MTO_FRAG_TH_LEVEL()         MTO_DATA().FragThresholdLevel
@@ -199,11 +200,9 @@ typedef struct _MTO_PARAMETERS
 //------------------------------------------------
 
 
-extern u8   MTO_Data_Rate_Tbl[];
 extern u16  MTO_Frag_Th_Tbl[];
 
 #define MTO_DATA_RATE()          MTO_Data_Rate_Tbl[MTO_RATE_LEVEL()]
-#define MTO_DATA_FALLBACK_RATE() MTO_Data_Rate_Tbl[MTO_FALLBACK_RATE_LEVEL()]	//next level
 #define MTO_FRAG_TH()            MTO_Frag_Th_Tbl[MTO_FRAG_TH_LEVEL()]
 
 typedef struct {
diff --git a/drivers/staging/winbond/mto_f.h b/drivers/staging/winbond/mto_f.h
index 30b3df2ccb3..81f59137c6d 100644
--- a/drivers/staging/winbond/mto_f.h
+++ b/drivers/staging/winbond/mto_f.h
@@ -1,7 +1,13 @@
-extern void MTO_Init(PWB32_ADAPTER);
-extern void MTO_PeriodicTimerExpired(PWB32_ADAPTER);
-extern void MTO_SetDTORateRange(PWB32_ADAPTER, u8 *, u8);
+#ifndef __WINBOND_MTO_F_H
+#define __WINBOND_MTO_F_H
+
+#include "core.h"
+
+extern void MTO_Init(struct wbsoft_priv *);
+extern void MTO_PeriodicTimerExpired(struct wbsoft_priv *);
+extern void MTO_SetDTORateRange(struct wbsoft_priv *, u8 *, u8);
 extern u8 MTO_GetTxRate(MTO_FUNC_INPUT, u32 fpdu_len);
 extern u8 MTO_GetTxFallbackRate(MTO_FUNC_INPUT);
 extern void MTO_SetTxCount(MTO_FUNC_INPUT, u8 t0, u8 index);
 
+#endif
diff --git a/drivers/staging/winbond/os_common.h b/drivers/staging/winbond/os_common.h
index e24ff41e871..2c276e3ab51 100644
--- a/drivers/staging/winbond/os_common.h
+++ b/drivers/staging/winbond/os_common.h
@@ -1,2 +1,2 @@
-#include "linux/sysdef.h"
+#include "sysdef.h"
 
diff --git a/drivers/staging/winbond/phy_calibration.c b/drivers/staging/winbond/phy_calibration.c
index 272a65066ab..6782552d366 100644
--- a/drivers/staging/winbond/phy_calibration.c
+++ b/drivers/staging/winbond/phy_calibration.c
@@ -12,6 +12,7 @@
 /****************** INCLUDE FILES SECTION ***********************************/
 #include "os_common.h"
 #include "phy_calibration.h"
+#include "wbhal_f.h"
 
 
 /****************** DEBUG CONSTANT AND MACRO SECTION ************************/
@@ -431,7 +432,6 @@ void _rxadc_dc_offset_cancellation_winbond(hw_data_t *phw_data, u32 frequency)
 
 	val |= MASK_ADC_DC_CAL_STR;
 	hw_set_dxx_reg(phw_data, REG_MODE_CTRL, val);
-	pa_stall_execution(US); // *MUST* wait for a while
 
 	// e. The result are shown in "adc_dc_cal_i[8:0] and adc_dc_cal_q[8:0]"
 #ifdef _DEBUG
@@ -522,7 +522,6 @@ void _txidac_dc_offset_cancellation_winbond(hw_data_t *phw_data)
 	reg_mode_ctrl |= (MASK_CALIB_START|2|(2<<2));
 	hw_set_dxx_reg(phw_data, REG_MODE_CTRL, reg_mode_ctrl);
 	PHY_DEBUG(("[CAL]    MODE_CTRL (write) = 0x%08X\n", reg_mode_ctrl));
-	pa_stall_execution(US);
 
 	hw_get_dxx_reg(phw_data, 0x5C, &reg_dc_cancel);
 	PHY_DEBUG(("[CAL]    DC_CANCEL (read) = 0x%08X\n", reg_dc_cancel));
@@ -536,7 +535,6 @@ void _txidac_dc_offset_cancellation_winbond(hw_data_t *phw_data)
 		reg_dc_cancel &= ~(0x03FF);
 		PHY_DEBUG(("[CAL]    DC_CANCEL (write) = 0x%08X\n", reg_dc_cancel));
 		hw_set_dxx_reg(phw_data, 0x5C, reg_dc_cancel);
-		pa_stall_execution(US);
 
 		hw_get_dxx_reg(phw_data, REG_CALIB_READ2, &val);
 		PHY_DEBUG(("[CAL]    CALIB_READ2 = 0x%08X\n", val));
@@ -552,7 +550,6 @@ void _txidac_dc_offset_cancellation_winbond(hw_data_t *phw_data)
 		reg_dc_cancel |= (1 << CANCEL_DC_I_SHIFT);
 		PHY_DEBUG(("[CAL]    DC_CANCEL (write) = 0x%08X\n", reg_dc_cancel));
 		hw_set_dxx_reg(phw_data, 0x5C, reg_dc_cancel);
-		pa_stall_execution(US);
 
 		hw_get_dxx_reg(phw_data, REG_CALIB_READ2, &val);
 		PHY_DEBUG(("[CAL]    CALIB_READ2 = 0x%08X\n", val));
@@ -600,7 +597,6 @@ void _txidac_dc_offset_cancellation_winbond(hw_data_t *phw_data)
 	reg_mode_ctrl &= ~MASK_CALIB_START;
 	hw_set_dxx_reg(phw_data, REG_MODE_CTRL, reg_mode_ctrl);
 	PHY_DEBUG(("[CAL]    MODE_CTRL (write) = 0x%08X\n", reg_mode_ctrl));
-	pa_stall_execution(US);
 }
 
 ///////////////////////////////////////////////////////
@@ -651,7 +647,6 @@ void _txqdac_dc_offset_cacellation_winbond(hw_data_t *phw_data)
 	reg_mode_ctrl |= (MASK_CALIB_START|3);
 	hw_set_dxx_reg(phw_data, REG_MODE_CTRL, reg_mode_ctrl);
 	PHY_DEBUG(("[CAL]    MODE_CTRL (write) = 0x%08X\n", reg_mode_ctrl));
-	pa_stall_execution(US);
 
 	hw_get_dxx_reg(phw_data, 0x5C, &reg_dc_cancel);
 	PHY_DEBUG(("[CAL]    DC_CANCEL (read) = 0x%08X\n", reg_dc_cancel));
@@ -665,11 +660,9 @@ void _txqdac_dc_offset_cacellation_winbond(hw_data_t *phw_data)
 		reg_dc_cancel &= ~(0x001F);
 		PHY_DEBUG(("[CAL]    DC_CANCEL (write) = 0x%08X\n", reg_dc_cancel));
 		hw_set_dxx_reg(phw_data, 0x5C, reg_dc_cancel);
-		pa_stall_execution(US);
 
 		hw_get_dxx_reg(phw_data, REG_CALIB_READ2, &val);
 		PHY_DEBUG(("[CAL]    CALIB_READ2 = 0x%08X\n", val));
-		pa_stall_execution(US);
 
 		iqcal_image_i = _s13_to_s32(val & 0x00001FFF);
 		iqcal_image_q = _s13_to_s32((val & 0x03FFE000) >> 13);
@@ -682,11 +675,9 @@ void _txqdac_dc_offset_cacellation_winbond(hw_data_t *phw_data)
 		reg_dc_cancel |= (1 << CANCEL_DC_Q_SHIFT);
 		PHY_DEBUG(("[CAL]    DC_CANCEL (write) = 0x%08X\n", reg_dc_cancel));
 		hw_set_dxx_reg(phw_data, 0x5C, reg_dc_cancel);
-		pa_stall_execution(US);
 
 		hw_get_dxx_reg(phw_data, REG_CALIB_READ2, &val);
 		PHY_DEBUG(("[CAL]    CALIB_READ2 = 0x%08X\n", val));
-		pa_stall_execution(US);
 
 		iqcal_image_i = _s13_to_s32(val & 0x00001FFF);
 		iqcal_image_q = _s13_to_s32((val & 0x03FFE000) >> 13);
@@ -732,7 +723,6 @@ void _txqdac_dc_offset_cacellation_winbond(hw_data_t *phw_data)
 	reg_mode_ctrl &= ~MASK_CALIB_START;
 	hw_set_dxx_reg(phw_data, REG_MODE_CTRL, reg_mode_ctrl);
 	PHY_DEBUG(("[CAL]    MODE_CTRL (write) = 0x%08X\n", reg_mode_ctrl));
-	pa_stall_execution(US);
 }
 
 //20060612.1.a 20060718.1 Modify
@@ -792,12 +782,10 @@ u8 _tx_iq_calibration_loop_winbond(hw_data_t *phw_data,
 			reg_mode_ctrl |= (MASK_CALIB_START|0x02|2<<2);
 			hw_set_dxx_reg(phw_data, REG_MODE_CTRL, reg_mode_ctrl);
 			PHY_DEBUG(("[CAL]    MODE_CTRL (write) = 0x%08X\n", reg_mode_ctrl));
-			pa_stall_execution(US);
 
 			// b.
 			hw_get_dxx_reg(phw_data, REG_CALIB_READ1, &val);
 			PHY_DEBUG(("[CAL]    CALIB_READ1 = 0x%08X\n", val));
-			pa_stall_execution(US);
 
 			iqcal_tone_i0 = _s13_to_s32(val & 0x00001FFF);
 			iqcal_tone_q0 = _s13_to_s32((val & 0x03FFE000) >> 13);
@@ -813,7 +801,6 @@ u8 _tx_iq_calibration_loop_winbond(hw_data_t *phw_data,
 			reg_mode_ctrl &= ~MASK_CALIB_START;
 			hw_set_dxx_reg(phw_data, REG_MODE_CTRL, reg_mode_ctrl);
 			PHY_DEBUG(("[CAL]    MODE_CTRL (write) = 0x%08X\n", reg_mode_ctrl));
-			pa_stall_execution(US);
 
 			// d. Set iqcal_mode[1:0] to 0x3 and set "calib_start" to 0x1 to
 			//    enable "IQ alibration Mode II"
@@ -823,12 +810,10 @@ u8 _tx_iq_calibration_loop_winbond(hw_data_t *phw_data,
 			reg_mode_ctrl |= (MASK_CALIB_START|0x03);
 			hw_set_dxx_reg(phw_data, REG_MODE_CTRL, reg_mode_ctrl);
 			PHY_DEBUG(("[CAL]    MODE_CTRL (write) = 0x%08X\n", reg_mode_ctrl));
-			pa_stall_execution(US);
 
 			// e.
 			hw_get_dxx_reg(phw_data, REG_CALIB_READ1, &val);
 			PHY_DEBUG(("[CAL]    CALIB_READ1 = 0x%08X\n", val));
-			pa_stall_execution(US);
 
 			iqcal_tone_i = _s13_to_s32(val & 0x00001FFF);
 			iqcal_tone_q = _s13_to_s32((val & 0x03FFE000) >> 13);
@@ -1075,7 +1060,7 @@ void _tx_iq_calibration_winbond(hw_data_t *phw_data)
 	//; [BB-chip]: Calibration (6h). Caculate TX-path IQ imbalance and setting TX path IQ compensation table
 	//phy_set_rf_data(phw_data, 3, (3<<24)|0x025586);
 
-	OS_SLEEP(30000); // 20060612.1.a 30ms delay. Add the follow 2 lines
+	msleep(30); // 20060612.1.a 30ms delay. Add the follow 2 lines
 	//To adjust TXVGA to fit iq_mag_0 range from 1250 ~ 1750
 	adjust_TXVGA_for_iq_mag( phw_data );
 
@@ -1282,13 +1267,11 @@ u8 _rx_iq_calibration_loop_winbond(hw_data_t *phw_data, u16 factor, u32 frequenc
 		if( !hw_set_dxx_reg(phw_data, REG_MODE_CTRL, reg_mode_ctrl) )//20060718.1 modify
 			return 0;
 		PHY_DEBUG(("[CAL]    MODE_CTRL (write) = 0x%08X\n", reg_mode_ctrl));
-		pa_stall_execution(US);
 
 		reg_mode_ctrl &= ~MASK_IQCAL_MODE;
 		reg_mode_ctrl |= (MASK_CALIB_START|0x1);
 		hw_set_dxx_reg(phw_data, REG_MODE_CTRL, reg_mode_ctrl);
 		PHY_DEBUG(("[CAL]    MODE_CTRL (write) = 0x%08X\n", reg_mode_ctrl));
-		pa_stall_execution(US);  //Should be read out after 450us
 
 		// c.
 		hw_get_dxx_reg(phw_data, REG_CALIB_READ1, &val);
@@ -1697,11 +1680,10 @@ unsigned char adjust_TXVGA_for_iq_mag(hw_data_t *phw_data)
 		phy_set_rf_data(phw_data, 5, ((5<<24)|current_txvga) );
 		phw_data->txvga_setting_for_cal = current_txvga;
 
-		//pa_stall_execution(30000);//Sleep(30);
-		OS_SLEEP(30000); // 20060612.1.a
+		msleep(30); // 20060612.1.a
 
 		if( !hw_get_dxx_reg(phw_data, REG_MODE_CTRL, &reg_mode_ctrl) ) // 20060718.1 modify
-			return FALSE;
+			return false;
 
 		PHY_DEBUG(("[CAL]    MODE_CTRL (read) = 0x%08X\n", reg_mode_ctrl));
 
@@ -1714,19 +1696,15 @@ unsigned char adjust_TXVGA_for_iq_mag(hw_data_t *phw_data)
 		hw_set_dxx_reg(phw_data, REG_MODE_CTRL, reg_mode_ctrl);
 		PHY_DEBUG(("[CAL]    MODE_CTRL (write) = 0x%08X\n", reg_mode_ctrl));
 
-		//pa_stall_execution(US);
-		OS_SLEEP(1); // 20060612.1.a
+		udelay(1); // 20060612.1.a
 
-		//pa_stall_execution(300);//Sleep(30);
-		OS_SLEEP(300); // 20060612.1.a
+		udelay(300); // 20060612.1.a
 
 		// b.
 		hw_get_dxx_reg(phw_data, REG_CALIB_READ1, &val);
 
 		PHY_DEBUG(("[CAL]    CALIB_READ1 = 0x%08X\n", val));
-		//pa_stall_execution(US);
-		//pa_stall_execution(300);//Sleep(30);
-		OS_SLEEP(300); // 20060612.1.a
+		udelay(300); // 20060612.1.a
 
 		iqcal_tone_i0 = _s13_to_s32(val & 0x00001FFF);
 		iqcal_tone_q0 = _s13_to_s32((val & 0x03FFE000) >> 13);
@@ -1750,9 +1728,9 @@ unsigned char adjust_TXVGA_for_iq_mag(hw_data_t *phw_data)
 	}
 
 	if( iq_mag_0_tx>=700 && iq_mag_0_tx<=1750 )
-		return TRUE;
+		return true;
 	else
-		return FALSE;
+		return false;
 }
 
 
diff --git a/drivers/staging/winbond/phy_calibration.h b/drivers/staging/winbond/phy_calibration.h
index b6a65d31301..03b820c6181 100644
--- a/drivers/staging/winbond/phy_calibration.h
+++ b/drivers/staging/winbond/phy_calibration.h
@@ -1,3 +1,8 @@
+#ifndef __WINBOND_PHY_CALIBRATION_H
+#define __WINBOND_PHY_CALIBRATION_H
+
+#include "wbhal_f.h"
+
 // 20031229 Turbo add
 #define REG_AGC_CTRL1               0x1000
 #define REG_AGC_CTRL2               0x1004
@@ -99,3 +104,4 @@
 void phy_set_rf_data(  phw_data_t pHwData,  u32 index,  u32 value );
 #define phy_init_rf( _A )	//RFSynthesizer_initial( _A )
 
+#endif
diff --git a/drivers/staging/winbond/reg.c b/drivers/staging/winbond/reg.c
index 57af5b83150..cd21272d7a9 100644
--- a/drivers/staging/winbond/reg.c
+++ b/drivers/staging/winbond/reg.c
@@ -1,4 +1,5 @@
 #include "os_common.h"
+#include "wbhal_f.h"
 
 ///////////////////////////////////////////////////////////////////////////////////////////////////
 // Original Phy.h
@@ -976,9 +977,9 @@ void Uxx_power_on_procedure(  phw_data_t pHwData )
 
 		// 20060511.1 Fix the following 4 steps for Rx of RF 2230 initial fail
 		Wb35Reg_WriteSync( pHwData, 0x03d4, 0x80 );// regulator on only
-		OS_SLEEP(10000); // Modify 20051221.1.b
+		msleep(10); // Modify 20051221.1.b
 		Wb35Reg_WriteSync( pHwData, 0x03d4, 0xb8 );// REG_ON RF_RSTN on, and
-		OS_SLEEP(10000); // Modify 20051221.1.b
+		msleep(10); // Modify 20051221.1.b
 
 		ltmp = 0x4968;
 		if( (pHwData->phy_type == RF_WB_242) ||
@@ -988,12 +989,12 @@ void Uxx_power_on_procedure(  phw_data_t pHwData )
 
 		Wb35Reg_WriteSync( pHwData, 0x03d4, 0xa0 );// PLL_PD REF_PD set to 0
 
-		OS_SLEEP(20000); // Modify 20051221.1.b
+		msleep(20); // Modify 20051221.1.b
 		Wb35Reg_ReadSync( pHwData, 0x03d0, &ltmp );
 		loop = 500; // Wait for 5 second 20061101
 		while( !(ltmp & 0x20) && loop-- )
 		{
-			OS_SLEEP(10000); // Modify 20051221.1.b
+			msleep(10); // Modify 20051221.1.b
 			if( !Wb35Reg_ReadSync( pHwData, 0x03d0, &ltmp ) )
 				break;
 		}
@@ -1002,7 +1003,7 @@ void Uxx_power_on_procedure(  phw_data_t pHwData )
 	}
 
 	Wb35Reg_WriteSync( pHwData, 0x03b0, 1 );// Reset hardware first
-	OS_SLEEP(10000); // Add this 20051221.1.b
+	msleep(10); // Add this 20051221.1.b
 
 	// Set burst write delay
 	Wb35Reg_WriteSync( pHwData, 0x03f8, 0x7ff );
@@ -1167,23 +1168,23 @@ RFSynthesizer_initial(phw_data_t pHwData)
 			// 20060511.1 --- Modifying the follow step for Rx issue-----------------
 			ltmp = (1 << 31) | (0 << 30) | (20 << 24) | BitReverse( (0x07<<20)|0xE168E, 20);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP(10000);
+			msleep(10);
 			ltmp = (1 << 31) | (0 << 30) | (20 << 24) | BitReverse( al2230_rf_data[7], 20);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP(10000);
+			msleep(10);
 
 		case RF_AIROHA_2230S: // 20060420 Add this
 
 			// 20060511.1 --- Modifying the follow step for Rx issue-----------------
 			Wb35Reg_WriteSync( pHwData, 0x03d4, 0x80 );// regulator on only
-			OS_SLEEP(10000); // Modify 20051221.1.b
+			msleep(10); // Modify 20051221.1.b
 
 			Wb35Reg_WriteSync( pHwData, 0x03d4, 0xa0 );// PLL_PD REF_PD set to 0
-			OS_SLEEP(10000); // Modify 20051221.1.b
+			msleep(10); // Modify 20051221.1.b
 
 			Wb35Reg_WriteSync( pHwData, 0x03d4, 0xe0 );// MLK_EN
 			Wb35Reg_WriteSync( pHwData, 0x03b0, 1 );// Reset hardware first
-			OS_SLEEP(10000); // Add this 20051221.1.b
+			msleep(10); // Add this 20051221.1.b
 			//------------------------------------------------------------------------
 
 			// The follow code doesn't use the burst-write mode
@@ -1191,30 +1192,30 @@ RFSynthesizer_initial(phw_data_t pHwData)
 			ltmp = (1 << 31) | (0 << 30) | (20 << 24) | BitReverse( (0x0F<<20) | 0xF01A0, 20);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
 
-			ltmp = pHwData->Wb35Reg.BB5C & 0xfffff000;
+			ltmp = pHwData->reg.BB5C & 0xfffff000;
 			Wb35Reg_WriteSync( pHwData, 0x105c, ltmp );
-			pHwData->Wb35Reg.BB50 |= 0x13;//(MASK_IQCAL_MODE|MASK_CALIB_START);//20060315.1 modify
-        	Wb35Reg_WriteSync( pHwData, 0x1050, pHwData->Wb35Reg.BB50);
-			OS_SLEEP(5000);
+			pHwData->reg.BB50 |= 0x13;//(MASK_IQCAL_MODE|MASK_CALIB_START);//20060315.1 modify
+	        	Wb35Reg_WriteSync(pHwData, 0x1050, pHwData->reg.BB50);
+			msleep(5);
 
 			//phy_set_rf_data(phw_data, 0x0F, (0x0F<<20) | 0xF01B0); //Activate Filter Cal.
 			ltmp = (1 << 31) | (0 << 30) | (20 << 24) | BitReverse( (0x0F<<20) | 0xF01B0, 20);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP(5000);
+			msleep(5);
 
 			//phy_set_rf_data(phw_data, 0x0F, (0x0F<<20) | 0xF01e0); //Activate TX DCC
 			ltmp = (1 << 31) | (0 << 30) | (20 << 24) | BitReverse( (0x0F<<20) | 0xF01E0, 20);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP(5000);
+			msleep(5);
 
 			//phy_set_rf_data(phw_data, 0x0F, (0x0F<<20) | 0xF01A0); //Resotre Initial Setting
 			ltmp = (1 << 31) | (0 << 30) | (20 << 24) | BitReverse( (0x0F<<20) | 0xF01A0, 20);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
 
 //			//Force TXI(Q)P(N) to normal control
-			Wb35Reg_WriteSync( pHwData, 0x105c, pHwData->Wb35Reg.BB5C );
-			pHwData->Wb35Reg.BB50 &= ~0x13;//(MASK_IQCAL_MODE|MASK_CALIB_START);
-        	Wb35Reg_WriteSync( pHwData, 0x1050, pHwData->Wb35Reg.BB50);
+			Wb35Reg_WriteSync( pHwData, 0x105c, pHwData->reg.BB5C );
+			pHwData->reg.BB50 &= ~0x13;//(MASK_IQCAL_MODE|MASK_CALIB_START);
+        	Wb35Reg_WriteSync( pHwData, 0x1050, pHwData->reg.BB50);
 			break;
 
 		case RF_AIROHA_7230:
@@ -1229,16 +1230,16 @@ RFSynthesizer_initial(phw_data_t pHwData)
 			//2.4GHz
 			//ltmp = (1 << 31) | (0 << 30) | (24 << 24) | 0x1ABA8F;
 			//Wb35Reg_WriteSync pHwData, 0x0864, ltmp );
-			//OS_SLEEP(1000); // Sleep 1 ms
+			//msleep(1); // Sleep 1 ms
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | 0x9ABA8F;
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP(5000);
+			msleep(5);
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | 0x3ABA8F;
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP(5000);
+			msleep(5);
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | 0x1ABA8F;
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP(5000);
+			msleep(5);
 
 			//5GHz
 			Wb35Reg_WriteSync( pHwData, 0x03dc, 0x00000000 );
@@ -1251,7 +1252,7 @@ RFSynthesizer_initial(phw_data_t pHwData)
 			// Write to register. number must less and equal than 16
 			for( i=0; i<number; i++ )
 				Wb35Reg_WriteSync( pHwData, 0x0864, pltmp[i] );
-			OS_SLEEP(5000);
+			msleep(5);
 
 			Wb35Reg_WriteSync( pHwData, 0x03dc, 0x00000080 );
 			#ifdef _PE_STATE_DUMP_
@@ -1262,13 +1263,13 @@ RFSynthesizer_initial(phw_data_t pHwData)
 			//Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | 0x9ABA8F;
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP(5000);
+			msleep(5);
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | 0x3ABA8F;
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP(5000);
+			msleep(5);
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | 0x12BACF;
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP(5000);
+			msleep(5);
 
 			//Wb35Reg_WriteSync( pHwData, 0x03dc, 0x00000080 );
 			//WBDEBUG(("* PLL_ON    high\n"));
@@ -1280,21 +1281,21 @@ RFSynthesizer_initial(phw_data_t pHwData)
 			//
 			// ; Version 1.3B revision items: for FA5976A , October 3, 2005 by HTHo
 			//
-			ltmp = pHwData->Wb35Reg.BB5C & 0xfffff000;
+			ltmp = pHwData->reg.BB5C & 0xfffff000;
 			Wb35Reg_WriteSync( pHwData, 0x105c, ltmp );
 			Wb35Reg_WriteSync( pHwData, 0x1058, 0 );
-			pHwData->Wb35Reg.BB50 |= 0x3;//(MASK_IQCAL_MODE|MASK_CALIB_START);//20060630
-	      	Wb35Reg_WriteSync( pHwData, 0x1050, pHwData->Wb35Reg.BB50);
+			pHwData->reg.BB50 |= 0x3;//(MASK_IQCAL_MODE|MASK_CALIB_START);//20060630
+		      	Wb35Reg_WriteSync(pHwData, 0x1050, pHwData->reg.BB50);
 
 			//----- Calibration (1). VCO frequency calibration
 			//Calibration (1a.0). Synthesizer reset (HTHo corrected 2005/05/10)
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | BitReverse( (0x0F<<24) | 0x00101E, 24);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP( 5000 ); // Sleep 5ms
+			msleep(5); // Sleep 5ms
 			//Calibration (1a). VCO frequency calibration mode ; waiting 2msec VCO calibration time
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | BitReverse( (0x00<<24) | 0xFE69c0, 24);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP( 2000 ); // Sleep 2ms
+			msleep(2); // Sleep 2ms
 
 			//----- Calibration (2). TX baseband Gm-C filter auto-tuning
 			//Calibration (2a). turn off ENCAL signal
@@ -1309,7 +1310,7 @@ RFSynthesizer_initial(phw_data_t pHwData)
 			//Calibration (2c). turn-on TX Gm-C filter auto-tuning
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | BitReverse( (0x00<<24) | 0xFCEBC0, 24);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP( 150 ); // Sleep 150 us
+			udelay(150); // Sleep 150 us
 			//turn off ENCAL signal
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | BitReverse( (0x00<<24) | 0xF8EBC0, 24);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
@@ -1327,7 +1328,7 @@ RFSynthesizer_initial(phw_data_t pHwData)
 			//Calibration (3c). turn-on RX Gm-C filter auto-tuning
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | BitReverse( (0x00<<24) | 0xFEEDC0, 24);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP( 150 ); // Sleep 150 us
+			udelay(150); // Sleep 150 us
 			//Calibration (3e). turn off ENCAL signal
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | BitReverse( (0x00<<24) | 0xFAEDC0, 24);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
@@ -1336,7 +1337,7 @@ RFSynthesizer_initial(phw_data_t pHwData)
 			//Calibration (4a). TX LO leakage calibration
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | BitReverse( (0x00<<24) | 0xFD6BC0, 24);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP( 150 ); // Sleep 150 us
+			udelay(150); // Sleep 150 us
 
 			//----- Calibration (5). RX DC offset calibration
 			//Calibration (5a). turn off ENCAL signal and set to RX SW DC caliration mode
@@ -1353,7 +1354,7 @@ RFSynthesizer_initial(phw_data_t pHwData)
 			//Calibration (5d). turn on RX DC offset cal function; and waiting 2 msec cal time
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | BitReverse( (0x00<<24) | 0xFF6DC0, 24);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP(2000); // Sleep 2ms
+			msleep(2); // Sleep 2ms
 			//Calibration (5f). turn off ENCAL signal
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | BitReverse( (0x00<<24) | 0xFAEDC0, 24);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
@@ -1365,7 +1366,7 @@ RFSynthesizer_initial(phw_data_t pHwData)
 			//Calibration (5d). turn on RX DC offset cal function; and waiting 2 msec cal time
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | BitReverse( (0x00<<24) | 0xFF6DC0, 24);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP(2000); // Sleep 2ms
+			msleep(2); // Sleep 2ms
 			//Calibration (5f). turn off ENCAL signal
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | BitReverse( (0x00<<24) | 0xFAEDC0, 24);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
@@ -1377,7 +1378,7 @@ RFSynthesizer_initial(phw_data_t pHwData)
 			//Calibration (5d). turn on RX DC offset cal function; and waiting 2 msec cal time
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | BitReverse( (0x00<<24) | 0xFF6DC0, 24);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP(2000); // Sleep 2ms
+			msleep(2); // Sleep 2ms
 			//Calibration (5f). turn off ENCAL signal
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | BitReverse( (0x00<<24) | 0xFAEDC0, 24);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
@@ -1389,7 +1390,7 @@ RFSynthesizer_initial(phw_data_t pHwData)
 			//Calibration (5d). turn on RX DC offset cal function; and waiting 2 msec cal time
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | BitReverse( (0x00<<24) | 0xFF6DC0, 24);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP(2000); // Sleep 2ms
+			msleep(2); // Sleep 2ms
 			//Calibration (5f). turn off ENCAL signal
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | BitReverse( (0x00<<24) | 0xFAEDC0, 24);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
@@ -1399,30 +1400,30 @@ RFSynthesizer_initial(phw_data_t pHwData)
 
 			//; ----- Calibration (7). Switch RF chip to normal mode
 			//0x00 0xF86100 ; 3E184   ; Switch RF chip to normal mode
-//			OS_SLEEP(10000); // @@ 20060721
+//			msleep(10); // @@ 20060721
 			ltmp = (1 << 31) | (0 << 30) | (24 << 24) | BitReverse( (0x00<<24) | 0xF86100, 24);
 			Wb35Reg_WriteSync( pHwData, 0x0864, ltmp );
-			OS_SLEEP(5000); // Sleep 5 ms
+			msleep(5); // Sleep 5 ms
 
 //			//write back
-//			Wb35Reg_WriteSync( pHwData, 0x105c, pHwData->Wb35Reg.BB5C );
-//			pHwData->Wb35Reg.BB50 &= ~0x13;//(MASK_IQCAL_MODE|MASK_CALIB_START); // 20060315.1 fix
-//      	Wb35Reg_WriteSync( pHwData, 0x1050, pHwData->Wb35Reg.BB50);
-//			OS_SLEEP(1000); // Sleep 1 ms
+//			Wb35Reg_WriteSync(pHwData, 0x105c, pHwData->reg.BB5C);
+//			pHwData->reg.BB50 &= ~0x13;//(MASK_IQCAL_MODE|MASK_CALIB_START); // 20060315.1 fix
+//		      	Wb35Reg_WriteSync(pHwData, 0x1050, pHwData->reg.BB50);
+//			msleep(1); // Sleep 1 ms
 			break;
 	}
 }
 
 void BBProcessor_AL7230_2400(  phw_data_t pHwData)
 {
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
+	struct wb35_reg *reg = &pHwData->reg;
 	u32	pltmp[12];
 
 	pltmp[0] = 0x16A8337A; // 0x16a5215f; // 0x1000 AGC_Ctrl1
 	pltmp[1] = 0x9AFF9AA6; // 0x9aff9ca6; // 0x1004 AGC_Ctrl2
 	pltmp[2] = 0x55D00A04; // 0x55d00a04; // 0x1008 AGC_Ctrl3
 	pltmp[3] = 0xFFF72031; // 0xFfFf2138; // 0x100c AGC_Ctrl4
-	pWb35Reg->BB0C = 0xFFF72031;
+	reg->BB0C = 0xFFF72031;
 	pltmp[4] = 0x0FacDCC5; // 0x1010 AGC_Ctrl5 // 20050927 0x0FacDCB7
 	pltmp[5] = 0x00CAA333; // 0x00eaa333; // 0x1014 AGC_Ctrl6
 	pltmp[6] = 0xF2211111; // 0x11111111; // 0x1018 AGC_Ctrl7
@@ -1431,25 +1432,25 @@ void BBProcessor_AL7230_2400(  phw_data_t pHwData)
 	pltmp[9] = 0xA8002A79; // 0xa9002A79; // 0x1024 AGC_Ctrl10
 	pltmp[10] = 0x40000528; // 20050927 0x40000228
 	pltmp[11] = 0x232D7F30; // 0x23457f30;// 0x102c A_ACQ_Ctrl
-	pWb35Reg->BB2C = 0x232D7F30;
+	reg->BB2C = 0x232D7F30;
 	Wb35Reg_BurstWrite( pHwData, 0x1000, pltmp, 12, AUTO_INCREMENT );
 
 	pltmp[0] = 0x00002c54; // 0x1030 B_ACQ_Ctrl
-	pWb35Reg->BB30 = 0x00002c54;
+	reg->BB30 = 0x00002c54;
 	pltmp[1] = 0x00C0D6C5; // 0x1034 A_TXRX_Ctrl
 	pltmp[2] = 0x5B2C8769; // 0x1038 B_TXRX_Ctrl
 	pltmp[3] = 0x00000000; // 0x103c 11a TX LS filter
-	pWb35Reg->BB3C = 0x00000000;
+	reg->BB3C = 0x00000000;
 	pltmp[4] = 0x00003F29; // 0x1040 11a TX LS filter
 	pltmp[5] = 0x0EFEFBFE; // 0x1044 11a TX LS filter
 	pltmp[6] = 0x00332C1B; // 0x00453B24; // 0x1048 11b TX RC filter
 	pltmp[7] = 0x0A00FEFF; // 0x0E00FEFF; // 0x104c 11b TX RC filter
 	pltmp[8] = 0x2B106208; // 0x1050 MODE_Ctrl
-	pWb35Reg->BB50 = 0x2B106208;
+	reg->BB50 = 0x2B106208;
 	pltmp[9] = 0; // 0x1054
-	pWb35Reg->BB54 = 0x00000000;
+	reg->BB54 = 0x00000000;
 	pltmp[10] = 0x52524242; // 0x64645252; // 0x1058 IQ_Alpha
-	pWb35Reg->BB58 = 0x52524242;
+	reg->BB58 = 0x52524242;
 	pltmp[11] = 0xAA0AC000; // 0x105c DC_Cancel
 	Wb35Reg_BurstWrite( pHwData, 0x1030, pltmp, 12, AUTO_INCREMENT );
 
@@ -1457,14 +1458,14 @@ void BBProcessor_AL7230_2400(  phw_data_t pHwData)
 
 void BBProcessor_AL7230_5000(  phw_data_t pHwData)
 {
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
+	struct wb35_reg *reg = &pHwData->reg;
 	u32	pltmp[12];
 
 	pltmp[0] = 0x16AA6678; // 0x1000 AGC_Ctrl1
 	pltmp[1] = 0x9AFFA0B2; // 0x1004 AGC_Ctrl2
 	pltmp[2] = 0x55D00A04; // 0x1008 AGC_Ctrl3
 	pltmp[3] = 0xEFFF233E; // 0x100c AGC_Ctrl4
-	pWb35Reg->BB0C = 0xEFFF233E;
+	reg->BB0C = 0xEFFF233E;
 	pltmp[4] = 0x0FacDCC5; // 0x1010 AGC_Ctrl5 // 20050927 0x0FacDCB7
 	pltmp[5] = 0x00CAA333; // 0x1014 AGC_Ctrl6
 	pltmp[6] = 0xF2432111; // 0x1018 AGC_Ctrl7
@@ -1473,24 +1474,24 @@ void BBProcessor_AL7230_5000(  phw_data_t pHwData)
 	pltmp[9] = 0x00002A79; // 0x1024 AGC_Ctrl10
 	pltmp[10] = 0x40000528; // 20050927 0x40000228
 	pltmp[11] = 0x232FDF30;// 0x102c A_ACQ_Ctrl
-	pWb35Reg->BB2C = 0x232FDF30;
+	reg->BB2C = 0x232FDF30;
 	Wb35Reg_BurstWrite( pHwData, 0x1000, pltmp, 12, AUTO_INCREMENT );
 
 	pltmp[0] = 0x80002C7C; // 0x1030 B_ACQ_Ctrl
 	pltmp[1] = 0x00C0D6C5; // 0x1034 A_TXRX_Ctrl
 	pltmp[2] = 0x5B2C8769; // 0x1038 B_TXRX_Ctrl
 	pltmp[3] = 0x00000000; // 0x103c 11a TX LS filter
-	pWb35Reg->BB3C = 0x00000000;
+	reg->BB3C = 0x00000000;
 	pltmp[4] = 0x00003F29; // 0x1040 11a TX LS filter
 	pltmp[5] = 0x0EFEFBFE; // 0x1044 11a TX LS filter
 	pltmp[6] = 0x00332C1B; // 0x1048 11b TX RC filter
 	pltmp[7] = 0x0A00FEFF; // 0x104c 11b TX RC filter
 	pltmp[8] = 0x2B107208; // 0x1050 MODE_Ctrl
-	pWb35Reg->BB50 = 0x2B107208;
+	reg->BB50 = 0x2B107208;
 	pltmp[9] = 0; // 0x1054
-	pWb35Reg->BB54 = 0x00000000;
+	reg->BB54 = 0x00000000;
 	pltmp[10] = 0x52524242; // 0x1058 IQ_Alpha
-	pWb35Reg->BB58 = 0x52524242;
+	reg->BB58 = 0x52524242;
 	pltmp[11] = 0xAA0AC000; // 0x105c DC_Cancel
 	Wb35Reg_BurstWrite( pHwData, 0x1030, pltmp, 12, AUTO_INCREMENT );
 
@@ -1511,7 +1512,7 @@ void BBProcessor_AL7230_5000(  phw_data_t pHwData)
 void
 BBProcessor_initial(  phw_data_t pHwData )
 {
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
+	struct wb35_reg *reg = &pHwData->reg;
 	u32	i, pltmp[12];
 
     switch( pHwData->phy_type )
@@ -1522,7 +1523,7 @@ BBProcessor_initial(  phw_data_t pHwData )
 			pltmp[1] = 0x9AFFAEA4; // 0x1004 AGC_Ctrl2
 			pltmp[2] = 0x55D00A04; // 0x1008 AGC_Ctrl3
 			pltmp[3] = 0xEFFF1A34; // 0x100c AGC_Ctrl4
-			pWb35Reg->BB0C = 0xEFFF1A34;
+			reg->BB0C = 0xEFFF1A34;
 			pltmp[4] = 0x0FABE0B7; // 0x1010 AGC_Ctrl5
 			pltmp[5] = 0x00CAA332; // 0x1014 AGC_Ctrl6
 			pltmp[6] = 0xF6632111; // 0x1018 AGC_Ctrl7
@@ -1531,25 +1532,25 @@ BBProcessor_initial(  phw_data_t pHwData )
 			pltmp[9] = 0x00002A79; // 0x1024 AGC_Ctrl10
 			pltmp[10] = (pHwData->phy_type==3) ? 0x40000a28 : 0x40000228; // 0x1028 MAXIM_331(b31=0) + WBRF_V1(b11=1) : MAXIM_331(b31=0) + WBRF_V2(b11=0)
 			pltmp[11] = 0x232FDF30; // 0x102c A_ACQ_Ctrl
-			pWb35Reg->BB2C = 0x232FDF30; //Modify for 33's 1.0.95.xxx version, antenna 1
+			reg->BB2C = 0x232FDF30; //Modify for 33's 1.0.95.xxx version, antenna 1
 			Wb35Reg_BurstWrite( pHwData, 0x1000, pltmp, 12, AUTO_INCREMENT );
 
 			pltmp[0] = 0x00002C54; // 0x1030 B_ACQ_Ctrl
-			pWb35Reg->BB30 = 0x00002C54;
+			reg->BB30 = 0x00002C54;
 			pltmp[1] = 0x00C0D6C5; // 0x1034 A_TXRX_Ctrl
 			pltmp[2] = 0x5B6C8769; // 0x1038 B_TXRX_Ctrl
 			pltmp[3] = 0x00000000; // 0x103c 11a TX LS filter
-			pWb35Reg->BB3C = 0x00000000;
+			reg->BB3C = 0x00000000;
 			pltmp[4] = 0x00003F29; // 0x1040 11a TX LS filter
 			pltmp[5] = 0x0EFEFBFE; // 0x1044 11a TX LS filter
 			pltmp[6] = 0x00453B24; // 0x1048 11b TX RC filter
 			pltmp[7] = 0x0E00FEFF; // 0x104c 11b TX RC filter
 			pltmp[8] = 0x27106208; // 0x1050 MODE_Ctrl
-			pWb35Reg->BB50 = 0x27106208;
+			reg->BB50 = 0x27106208;
 			pltmp[9] = 0; // 0x1054
-			pWb35Reg->BB54 = 0x00000000;
+			reg->BB54 = 0x00000000;
 			pltmp[10] = 0x64646464; // 0x1058 IQ_Alpha
-			pWb35Reg->BB58 = 0x64646464;
+			reg->BB58 = 0x64646464;
 			pltmp[11] = 0xAA0AC000; // 0x105c DC_Cancel
 			Wb35Reg_BurstWrite( pHwData, 0x1030, pltmp, 12, AUTO_INCREMENT );
 
@@ -1568,7 +1569,7 @@ BBProcessor_initial(  phw_data_t pHwData )
 			pltmp[1] = 0x9affaea4; // 0x1004 AGC_Ctrl2
 			pltmp[2] = 0x55d00a04; // 0x1008 AGC_Ctrl3
 			pltmp[3] = 0xefff1a34; // 0x100c AGC_Ctrl4
-			pWb35Reg->BB0C = 0xefff1a34;
+			reg->BB0C = 0xefff1a34;
 			pltmp[4] = 0x0fabe0b7; // 0x1010 AGC_Ctrl5
 			pltmp[5] = 0x00caa332; // 0x1014 AGC_Ctrl6
 			pltmp[6] = 0xf6632111; // 0x1018 AGC_Ctrl7
@@ -1577,25 +1578,25 @@ BBProcessor_initial(  phw_data_t pHwData )
 			pltmp[9] = 0x00002A79; // 0x1024 AGC_Ctrl10
 			pltmp[10] = 0x40000528; // 0x40000128; Modify for 33's 1.0.95
 			pltmp[11] = 0x232fdf30; // 0x102c A_ACQ_Ctrl
-			pWb35Reg->BB2C = 0x232fdf30; //Modify for 33's 1.0.95.xxx version, antenna 1
+			reg->BB2C = 0x232fdf30; //Modify for 33's 1.0.95.xxx version, antenna 1
 			Wb35Reg_BurstWrite( pHwData, 0x1000, pltmp, 12, AUTO_INCREMENT );
 
 			pltmp[0] = 0x00002C54; // 0x1030 B_ACQ_Ctrl
-			pWb35Reg->BB30 = 0x00002C54;
+			reg->BB30 = 0x00002C54;
 			pltmp[1] = 0x00C0D6C5; // 0x1034 A_TXRX_Ctrl
 			pltmp[2] = 0x5B6C8769; // 0x1038 B_TXRX_Ctrl
 			pltmp[3] = 0x00000000; // 0x103c 11a TX LS filter
-			pWb35Reg->BB3C = 0x00000000;
+			reg->BB3C = 0x00000000;
 			pltmp[4] = 0x00003F29; // 0x1040 11a TX LS filter
 			pltmp[5] = 0x0EFEFBFE; // 0x1044 11a TX LS filter
 			pltmp[6] = 0x00453B24; // 0x1048 11b TX RC filter
 			pltmp[7] = 0x0D00FDFF; // 0x104c 11b TX RC filter
 			pltmp[8] = 0x27106208; // 0x1050 MODE_Ctrl
-			pWb35Reg->BB50 = 0x27106208;
+			reg->BB50 = 0x27106208;
 			pltmp[9] = 0; // 0x1054
-			pWb35Reg->BB54 = 0x00000000;
+			reg->BB54 = 0x00000000;
 			pltmp[10] = 0x64646464; // 0x1058 IQ_Alpha
-			pWb35Reg->BB58 = 0x64646464;
+			reg->BB58 = 0x64646464;
 			pltmp[11] = 0xAA28C000; // 0x105c DC_Cancel
 			Wb35Reg_BurstWrite( pHwData, 0x1030, pltmp, 12, AUTO_INCREMENT );
 
@@ -1608,7 +1609,7 @@ BBProcessor_initial(  phw_data_t pHwData )
 			pltmp[1] = 0x9affaea4; // 0x1004 AGC_Ctrl2
 			pltmp[2] = 0x55d00a04; // 0x1008 AGC_Ctrl3
 			pltmp[3] = 0xf4ff1632; // 0xefff1a34; // 0x100c AGC_Ctrl4 Modify for 33's 1.0.95
-			pWb35Reg->BB0C = 0xf4ff1632; // 0xefff1a34; Modify for 33's 1.0.95
+			reg->BB0C = 0xf4ff1632; // 0xefff1a34; Modify for 33's 1.0.95
 			pltmp[4] = 0x0fabe0b7; // 0x1010 AGC_Ctrl5
 			pltmp[5] = 0x00caa332; // 0x1014 AGC_Ctrl6
 			pltmp[6] = 0xf8632112; // 0xf6632111; // 0x1018 AGC_Ctrl7 Modify for 33's 1.0.95
@@ -1617,25 +1618,25 @@ BBProcessor_initial(  phw_data_t pHwData )
 			pltmp[9] = 0x00002A79; // 0x1024 AGC_Ctrl10
 			pltmp[10] = 0x40000528; // 0x40000128; modify for 33's 1.0.95
 			pltmp[11] = 0x232fdf30; // 0x102c A_ACQ_Ctrl
-			pWb35Reg->BB2C = 0x232fdf30; //Modify for 33's 1.0.95.xxx version, antenna 1
+			reg->BB2C = 0x232fdf30; //Modify for 33's 1.0.95.xxx version, antenna 1
 			Wb35Reg_BurstWrite( pHwData, 0x1000, pltmp, 12, AUTO_INCREMENT );
 
 			pltmp[0] = 0x00002C54; // 0x1030 B_ACQ_Ctrl
-			pWb35Reg->BB30 = 0x00002C54;
+			reg->BB30 = 0x00002C54;
 			pltmp[1] = 0x00C0D6C5; // 0x1034 A_TXRX_Ctrl
 			pltmp[2] = 0x5b2c8769; // 0x5B6C8769; // 0x1038 B_TXRX_Ctrl Modify for 33's 1.0.95
 			pltmp[3] = 0x00000000; // 0x103c 11a TX LS filter
-			pWb35Reg->BB3C = 0x00000000;
+			reg->BB3C = 0x00000000;
 			pltmp[4] = 0x00003F29; // 0x1040 11a TX LS filter
 			pltmp[5] = 0x0EFEFBFE; // 0x1044 11a TX LS filter
 			pltmp[6] = 0x002c2617; // 0x00453B24; // 0x1048 11b TX RC filter Modify for 33's 1.0.95
 			pltmp[7] = 0x0800feff; // 0x0D00FDFF; // 0x104c 11b TX RC filter Modify for 33's 1.0.95
 			pltmp[8] = 0x27106208; // 0x1050 MODE_Ctrl
-			pWb35Reg->BB50 = 0x27106208;
+			reg->BB50 = 0x27106208;
 			pltmp[9] = 0; // 0x1054
-			pWb35Reg->BB54 = 0x00000000;
+			reg->BB54 = 0x00000000;
 			pltmp[10] = 0x64644a4a; // 0x64646464; // 0x1058 IQ_Alpha Modify for 33's 1.0.95
-			pWb35Reg->BB58 = 0x64646464;
+			reg->BB58 = 0x64646464;
 			pltmp[11] = 0xAA28C000; // 0x105c DC_Cancel
 			Wb35Reg_BurstWrite( pHwData, 0x1030, pltmp, 12, AUTO_INCREMENT );
 
@@ -1648,7 +1649,7 @@ BBProcessor_initial(  phw_data_t pHwData )
 			pltmp[1] = 0x9affafb2; // 0x1004 AGC_Ctrl2
 			pltmp[2] = 0x55d00a04; // 0x1008 AGC_Ctrl3
 			pltmp[3] = 0xFFFd203c; // 0xFFFb203a; // 0x100c AGC_Ctrl4 Modify for 33's 1.0.95.xxx version
-			pWb35Reg->BB0C = 0xFFFd203c;
+			reg->BB0C = 0xFFFd203c;
 			pltmp[4] = 0X0FBFDCc5; // 0X0FBFDCA0; // 0x1010 AGC_Ctrl5 //0x0FB2E0B7 Modify for 33's 1.0.95.xxx version
 			pltmp[5] = 0x00caa332; // 0x00caa333; // 0x1014 AGC_Ctrl6 Modify for 33's 1.0.95.xxx version
 			pltmp[6] = 0XF6632111; // 0XF1632112; // 0x1018 AGC_Ctrl7		//0xf6632112 Modify for 33's 1.0.95.xxx version
@@ -1657,27 +1658,27 @@ BBProcessor_initial(  phw_data_t pHwData )
 			pltmp[9] = 0x00002A79; // 0x1024 AGC_Ctrl10
 			pltmp[10] = 0X40000528;							//0x40000228
 			pltmp[11] = 0x232dfF30; // 0x232A9F30; // 0x102c A_ACQ_Ctrl	//0x232a9730
-			pWb35Reg->BB2C = 0x232dfF30; //Modify for 33's 1.0.95.xxx version, antenna 1
+			reg->BB2C = 0x232dfF30; //Modify for 33's 1.0.95.xxx version, antenna 1
 			Wb35Reg_BurstWrite( pHwData, 0x1000, pltmp, 12, AUTO_INCREMENT );
 
 			pltmp[0] = 0x00002C54; // 0x1030 B_ACQ_Ctrl
-			pWb35Reg->BB30 = 0x00002C54;
+			reg->BB30 = 0x00002C54;
 			pltmp[1] = 0x00C0D6C5; // 0x1034 A_TXRX_Ctrl
 			pltmp[2] = 0x5B2C8769; // 0x1038 B_TXRX_Ctrl	//0x5B6C8769
 			pltmp[3] = 0x00000000; // 0x103c 11a TX LS filter
-			pWb35Reg->BB3C = 0x00000000;
+			reg->BB3C = 0x00000000;
 			pltmp[4] = 0x00003F29; // 0x1040 11a TX LS filter
 			pltmp[5] = 0x0EFEFBFE; // 0x1044 11a TX LS filter
 			pltmp[6] = BB48_DEFAULT_AL2230_11G; // 0x1048 11b TX RC filter 20060613.2
-			pWb35Reg->BB48 = BB48_DEFAULT_AL2230_11G; // 20051221 ch14 20060613.2
+			reg->BB48 = BB48_DEFAULT_AL2230_11G; // 20051221 ch14 20060613.2
 			pltmp[7] = BB4C_DEFAULT_AL2230_11G; // 0x104c 11b TX RC filter 20060613.2
-			pWb35Reg->BB4C = BB4C_DEFAULT_AL2230_11G; // 20060613.1 20060613.2
+			reg->BB4C = BB4C_DEFAULT_AL2230_11G; // 20060613.1 20060613.2
 			pltmp[8] = 0x27106200; // 0x1050 MODE_Ctrl
-			pWb35Reg->BB50 = 0x27106200;
+			reg->BB50 = 0x27106200;
 			pltmp[9] = 0; // 0x1054
-			pWb35Reg->BB54 = 0x00000000;
+			reg->BB54 = 0x00000000;
 			pltmp[10] = 0x52524242; // 0x1058 IQ_Alpha
-			pWb35Reg->BB58 = 0x52524242;
+			reg->BB58 = 0x52524242;
 			pltmp[11] = 0xAA0AC000; // 0x105c DC_Cancel
 			Wb35Reg_BurstWrite( pHwData, 0x1030, pltmp, 12, AUTO_INCREMENT );
 
@@ -1690,7 +1691,7 @@ BBProcessor_initial(  phw_data_t pHwData )
 			pltmp[1] = 0x9affafb2; // 0x1004 AGC_Ctrl2
 			pltmp[2] = 0x55d00a04; // 0x1008 AGC_Ctrl3
 			pltmp[3] = 0xFFFd203c; // 0xFFFb203a; // 0x100c AGC_Ctrl4 Modify for 33's 1.0.95.xxx version
-			pWb35Reg->BB0C = 0xFFFd203c;
+			reg->BB0C = 0xFFFd203c;
 			pltmp[4] = 0X0FBFDCc5; // 0X0FBFDCA0; // 0x1010 AGC_Ctrl5 //0x0FB2E0B7 Modify for 33's 1.0.95.xxx version
 			pltmp[5] = 0x00caa332; // 0x00caa333; // 0x1014 AGC_Ctrl6 Modify for 33's 1.0.95.xxx version
 			pltmp[6] = 0XF6632111; // 0XF1632112; // 0x1018 AGC_Ctrl7		//0xf6632112 Modify for 33's 1.0.95.xxx version
@@ -1699,27 +1700,27 @@ BBProcessor_initial(  phw_data_t pHwData )
 			pltmp[9] = 0x00002A79; // 0x1024 AGC_Ctrl10
 			pltmp[10] = 0X40000528;							//0x40000228
 			pltmp[11] = 0x232dfF30; // 0x232A9F30; // 0x102c A_ACQ_Ctrl	//0x232a9730
-			pWb35Reg->BB2C = 0x232dfF30; //Modify for 33's 1.0.95.xxx version, antenna 1
+			reg->BB2C = 0x232dfF30; //Modify for 33's 1.0.95.xxx version, antenna 1
 			Wb35Reg_BurstWrite( pHwData, 0x1000, pltmp, 12, AUTO_INCREMENT );
 
 			pltmp[0] = 0x00002C54; // 0x1030 B_ACQ_Ctrl
-			pWb35Reg->BB30 = 0x00002C54;
+			reg->BB30 = 0x00002C54;
 			pltmp[1] = 0x00C0D6C5; // 0x1034 A_TXRX_Ctrl
 			pltmp[2] = 0x5B2C8769; // 0x1038 B_TXRX_Ctrl	//0x5B6C8769
 			pltmp[3] = 0x00000000; // 0x103c 11a TX LS filter
-			pWb35Reg->BB3C = 0x00000000;
+			reg->BB3C = 0x00000000;
 			pltmp[4] = 0x00003F29; // 0x1040 11a TX LS filter
 			pltmp[5] = 0x0EFEFBFE; // 0x1044 11a TX LS filter
 			pltmp[6] = BB48_DEFAULT_AL2230_11G; // 0x1048 11b TX RC filter 20060613.2
-			pWb35Reg->BB48 = BB48_DEFAULT_AL2230_11G; // 20051221 ch14 20060613.2
+			reg->BB48 = BB48_DEFAULT_AL2230_11G; // 20051221 ch14 20060613.2
 			pltmp[7] = BB4C_DEFAULT_AL2230_11G; // 0x104c 11b TX RC filter 20060613.2
-			pWb35Reg->BB4C = BB4C_DEFAULT_AL2230_11G; // 20060613.1
+			reg->BB4C = BB4C_DEFAULT_AL2230_11G; // 20060613.1
 			pltmp[8] = 0x27106200; // 0x1050 MODE_Ctrl
-			pWb35Reg->BB50 = 0x27106200;
+			reg->BB50 = 0x27106200;
 			pltmp[9] = 0; // 0x1054
-			pWb35Reg->BB54 = 0x00000000;
+			reg->BB54 = 0x00000000;
 			pltmp[10] = 0x52523232; // 20060419 0x52524242; // 0x1058 IQ_Alpha
-			pWb35Reg->BB58 = 0x52523232; // 20060419 0x52524242;
+			reg->BB58 = 0x52523232; // 20060419 0x52524242;
 			pltmp[11] = 0xAA0AC000; // 0x105c DC_Cancel
 			Wb35Reg_BurstWrite( pHwData, 0x1030, pltmp, 12, AUTO_INCREMENT );
 
@@ -1732,7 +1733,7 @@ BBProcessor_initial(  phw_data_t pHwData )
 			pltmp[1] = 0x9affafb2; // 0x1004 AGC_Ctrl2
 			pltmp[2] = 0x55d00a04; // 0x1008 AGC_Ctrl3
 			pltmp[3] = 0xFFFb203a; // 0x100c AGC_Ctrl4
-			pWb35Reg->BB0c = 0xFFFb203a;
+			reg->BB0c = 0xFFFb203a;
 			pltmp[4] = 0x0FBFDCB7; // 0x1010 AGC_Ctrl5
 			pltmp[5] = 0x00caa333; // 0x1014 AGC_Ctrl6
 			pltmp[6] = 0xf6632112; // 0x1018 AGC_Ctrl7
@@ -1741,25 +1742,25 @@ BBProcessor_initial(  phw_data_t pHwData )
 			pltmp[9] = 0x00002A79; // 0x1024 AGC_Ctrl10
 			pltmp[10] = 0x40000228;
 			pltmp[11] = 0x232A9F30;// 0x102c A_ACQ_Ctrl
-			pWb35Reg->BB2c = 0x232A9F30;
+			reg->BB2c = 0x232A9F30;
 			Wb35Reg_BurstWrite( pHwData, 0x1000, pltmp, 12, AUTO_INCREMENT );
 
 			pltmp[0] = 0x00002C54; // 0x1030 B_ACQ_Ctrl
-			pWb35Reg->BB30 = 0x00002C54;
+			reg->BB30 = 0x00002C54;
 			pltmp[1] = 0x00C0D6C5; // 0x1034 A_TXRX_Ctrl
 			pltmp[2] = 0x5B2C8769; // 0x1038 B_TXRX_Ctrl
 			pltmp[3] = 0x00000000; // 0x103c 11a TX LS filter
-			pWb35Reg->BB3c = 0x00000000;
+			reg->BB3c = 0x00000000;
 			pltmp[4] = 0x00003F29; // 0x1040 11a TX LS filter
 			pltmp[5] = 0x0EFEFBFE; // 0x1044 11a TX LS filter
 			pltmp[6] = 0x00453B24; // 0x1048 11b TX RC filter
 			pltmp[7] = 0x0E00FEFF; // 0x104c 11b TX RC filter
 			pltmp[8] = 0x27106200; // 0x1050 MODE_Ctrl
-			pWb35Reg->BB50 = 0x27106200;
+			reg->BB50 = 0x27106200;
 			pltmp[9] = 0; // 0x1054
-			pWb35Reg->BB54 = 0x00000000;
+			reg->BB54 = 0x00000000;
 			pltmp[10] = 0x64645252; // 0x1058 IQ_Alpha
-			pWb35Reg->BB58 = 0x64645252;
+			reg->BB58 = 0x64645252;
 			pltmp[11] = 0xAA0AC000; // 0x105c DC_Cancel
 			Wb35Reg_BurstWrite( pHwData, 0x1030, pltmp, 12, AUTO_INCREMENT );
 */
@@ -1775,7 +1776,7 @@ BBProcessor_initial(  phw_data_t pHwData )
 			pltmp[1] = 0x9AFF9ABA; // 0x1004 AGC_Ctrl2
 			pltmp[2] = 0x55D00A04; // 0x1008 AGC_Ctrl3
 			pltmp[3] = 0xEEE91C32; // 0x100c AGC_Ctrl4
-			pWb35Reg->BB0C = 0xEEE91C32;
+			reg->BB0C = 0xEEE91C32;
 			pltmp[4] = 0x0FACDCC5; // 0x1010 AGC_Ctrl5
 			pltmp[5] = 0x000AA344; // 0x1014 AGC_Ctrl6
 			pltmp[6] = 0x22222221; // 0x1018 AGC_Ctrl7
@@ -1784,27 +1785,27 @@ BBProcessor_initial(  phw_data_t pHwData )
 			pltmp[9] = 0xA9002A79; // 0x1024 AGC_Ctrl10
 			pltmp[10] = 0x40000528; // 0x1028
 			pltmp[11] = 0x23457F30; // 0x102c A_ACQ_Ctrl
-			pWb35Reg->BB2C = 0x23457F30;
+			reg->BB2C = 0x23457F30;
 			Wb35Reg_BurstWrite( pHwData, 0x1000, pltmp, 12, AUTO_INCREMENT );
 
 			pltmp[0] = 0x00002C54; // 0x1030 B_ACQ_Ctrl
-			pWb35Reg->BB30 = 0x00002C54;
+			reg->BB30 = 0x00002C54;
 			pltmp[1] = 0x00C0D6C5; // 0x1034 A_TXRX_Ctrl
 			pltmp[2] = 0x5B2C8769; // 0x1038 B_TXRX_Ctrl
 			pltmp[3] = pHwData->BB3c_cal; // 0x103c 11a TX LS filter
-			pWb35Reg->BB3C = pHwData->BB3c_cal;
+			reg->BB3C = pHwData->BB3c_cal;
 			pltmp[4] = 0x00003F29; // 0x1040 11a TX LS filter
 			pltmp[5] = 0x0EFEFBFE; // 0x1044 11a TX LS filter
 			pltmp[6] = BB48_DEFAULT_WB242_11G; // 0x1048 11b TX RC filter 20060613.2
-			pWb35Reg->BB48 = BB48_DEFAULT_WB242_11G; // 20060613.1 20060613.2
+			reg->BB48 = BB48_DEFAULT_WB242_11G; // 20060613.1 20060613.2
 			pltmp[7] = BB4C_DEFAULT_WB242_11G; // 0x104c 11b TX RC filter 20060613.2
-			pWb35Reg->BB4C = BB4C_DEFAULT_WB242_11G; // 20060613.1 20060613.2
+			reg->BB4C = BB4C_DEFAULT_WB242_11G; // 20060613.1 20060613.2
 			pltmp[8] = 0x27106208; // 0x1050 MODE_Ctrl
-			pWb35Reg->BB50 = 0x27106208;
+			reg->BB50 = 0x27106208;
 			pltmp[9] = pHwData->BB54_cal; // 0x1054
-			pWb35Reg->BB54 = pHwData->BB54_cal;
+			reg->BB54 = pHwData->BB54_cal;
 			pltmp[10] = 0x52523131; // 0x1058 IQ_Alpha
-			pWb35Reg->BB58 = 0x52523131;
+			reg->BB58 = 0x52523131;
 			pltmp[11] = 0xAA0AC000; // 20060825 0xAA2AC000; // 0x105c DC_Cancel
 			Wb35Reg_BurstWrite( pHwData, 0x1030, pltmp, 12, AUTO_INCREMENT );
 
@@ -1813,14 +1814,14 @@ BBProcessor_initial(  phw_data_t pHwData )
     }
 
 	// Fill the LNA table
-	pWb35Reg->LNAValue[0] = (u8)(pWb35Reg->BB0C & 0xff);
-	pWb35Reg->LNAValue[1] = 0;
-	pWb35Reg->LNAValue[2] = (u8)((pWb35Reg->BB0C & 0xff00)>>8);
-	pWb35Reg->LNAValue[3] = 0;
+	reg->LNAValue[0] = (u8)(reg->BB0C & 0xff);
+	reg->LNAValue[1] = 0;
+	reg->LNAValue[2] = (u8)((reg->BB0C & 0xff00)>>8);
+	reg->LNAValue[3] = 0;
 
 	// Fill SQ3 table
 	for( i=0; i<MAX_SQ3_FILTER_SIZE; i++ )
-		pWb35Reg->SQ3_filter[i] = 0x2f; // half of Bit 0 ~ 6
+		reg->SQ3_filter[i] = 0x2f; // half of Bit 0 ~ 6
 }
 
 void set_tx_power_per_channel_max2829(  phw_data_t pHwData,  ChanInfo Channel)
@@ -1903,7 +1904,7 @@ void set_tx_power_per_channel_wb242(  phw_data_t pHwData,  ChanInfo Channel)
 void
 RFSynthesizer_SwitchingChannel(  phw_data_t pHwData,  ChanInfo Channel )
 {
-	PWB35REG pWb35Reg = &pHwData->Wb35Reg;
+	struct wb35_reg *reg = &pHwData->reg;
 	u32	pltmp[16]; // The 16 is the maximum capability of hardware
 	u32	count, ltmp;
 	u8	i, j, number;
@@ -2090,40 +2091,40 @@ RFSynthesizer_SwitchingChannel(  phw_data_t pHwData,  ChanInfo Channel )
 	if( Channel.band <= BAND_TYPE_OFDM_24 )
 	{
         // BB: select 2.4 GHz, bit[12-11]=00
-		pWb35Reg->BB50 &= ~(BIT(11)|BIT(12));
-		Wb35Reg_Write( pHwData, 0x1050, pWb35Reg->BB50 ); // MODE_Ctrl
+		reg->BB50 &= ~(BIT(11)|BIT(12));
+		Wb35Reg_Write( pHwData, 0x1050, reg->BB50 ); // MODE_Ctrl
         // MAC: select 2.4 GHz, bit[5]=0
-		pWb35Reg->M78_ERPInformation &= ~BIT(5);
-		Wb35Reg_Write( pHwData, 0x0878, pWb35Reg->M78_ERPInformation );
+		reg->M78_ERPInformation &= ~BIT(5);
+		Wb35Reg_Write( pHwData, 0x0878, reg->M78_ERPInformation );
         // enable 11b Baseband
-		pWb35Reg->BB30 &= ~BIT(31);
-		Wb35Reg_Write( pHwData, 0x1030, pWb35Reg->BB30 );
+		reg->BB30 &= ~BIT(31);
+		Wb35Reg_Write( pHwData, 0x1030, reg->BB30 );
 	}
 	else if( (Channel.band == BAND_TYPE_OFDM_5) )
 	{
         // BB: select 5 GHz
-		pWb35Reg->BB50 &= ~(BIT(11)|BIT(12));
+		reg->BB50 &= ~(BIT(11)|BIT(12));
 		if (Channel.ChanNo <=64 )
-			pWb35Reg->BB50 |= BIT(12);				// 10-5.25GHz
+			reg->BB50 |= BIT(12);				// 10-5.25GHz
 		else if ((Channel.ChanNo >= 100) && (Channel.ChanNo <= 124))
-			pWb35Reg->BB50 |= BIT(11);				// 01-5.48GHz
+			reg->BB50 |= BIT(11);				// 01-5.48GHz
 		else if ((Channel.ChanNo >=128) && (Channel.ChanNo <= 161))
-			pWb35Reg->BB50 |= (BIT(12)|BIT(11));	// 11-5.775GHz
+			reg->BB50 |= (BIT(12)|BIT(11));	// 11-5.775GHz
 		else	//Chan 184 ~ 196 will use bit[12-11] = 10 in version sh-src-1.2.25
-			pWb35Reg->BB50 |= BIT(12);
-		Wb35Reg_Write( pHwData, 0x1050, pWb35Reg->BB50 ); // MODE_Ctrl
+			reg->BB50 |= BIT(12);
+		Wb35Reg_Write( pHwData, 0x1050, reg->BB50 ); // MODE_Ctrl
 
 		//(1) M78 should alway use 2.4G setting when using RF_AIROHA_7230
 		//(2) BB30 has been updated previously.
 		if (pHwData->phy_type != RF_AIROHA_7230)
 		{
     	    // MAC: select 5 GHz, bit[5]=1
-			pWb35Reg->M78_ERPInformation |= BIT(5);
-			Wb35Reg_Write( pHwData, 0x0878, pWb35Reg->M78_ERPInformation );
+			reg->M78_ERPInformation |= BIT(5);
+			Wb35Reg_Write( pHwData, 0x0878, reg->M78_ERPInformation );
 
     	    // disable 11b Baseband
-			pWb35Reg->BB30 |= BIT(31);
-			Wb35Reg_Write( pHwData, 0x1030, pWb35Reg->BB30 );
+			reg->BB30 |= BIT(31);
+			Wb35Reg_Write( pHwData, 0x1030, reg->BB30 );
 		}
 	}
 }
@@ -2313,21 +2314,21 @@ u8 RFSynthesizer_SetWinbond242Power(  phw_data_t pHwData, u8 index )
 //===========================================================================================================
 void Dxx_initial(  phw_data_t pHwData )
 {
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
+	struct wb35_reg *reg = &pHwData->reg;
 
 	// Old IC:Single mode only.
 	// New IC: operation decide by Software set bit[4]. 1:multiple 0: single
-	pWb35Reg->D00_DmaControl = 0xc0000004;	//Txon, Rxon, multiple Rx for new 4k DMA
+	reg->D00_DmaControl = 0xc0000004;	//Txon, Rxon, multiple Rx for new 4k DMA
 											//Txon, Rxon, single Rx for old 8k ASIC
 	if( !HAL_USB_MODE_BURST( pHwData ) )
-		pWb35Reg->D00_DmaControl = 0xc0000000;//Txon, Rxon, single Rx for new 4k DMA
+		reg->D00_DmaControl = 0xc0000000;//Txon, Rxon, single Rx for new 4k DMA
 
-	Wb35Reg_WriteSync( pHwData, 0x0400, pWb35Reg->D00_DmaControl );
+	Wb35Reg_WriteSync( pHwData, 0x0400, reg->D00_DmaControl );
 }
 
 void Mxx_initial(  phw_data_t pHwData )
 {
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
+	struct wb35_reg *reg = &pHwData->reg;
 	u32		tmp;
 	u32		pltmp[11];
 	u16	i;
@@ -2339,23 +2340,23 @@ void Mxx_initial(  phw_data_t pHwData )
 
 	// M00 bit set
 #ifdef _IBSS_BEACON_SEQ_STICK_
-	pWb35Reg->M00_MacControl = 0; // Solve beacon sequence number stop by software
+	reg->M00_MacControl = 0; // Solve beacon sequence number stop by software
 #else
-	pWb35Reg->M00_MacControl = 0x80000000; // Solve beacon sequence number stop by hardware
+	reg->M00_MacControl = 0x80000000; // Solve beacon sequence number stop by hardware
 #endif
 
 	// M24 disable enter power save, BB RxOn and enable NAV attack
-	pWb35Reg->M24_MacControl = 0x08040042;
-	pltmp[0] = pWb35Reg->M24_MacControl;
+	reg->M24_MacControl = 0x08040042;
+	pltmp[0] = reg->M24_MacControl;
 
 	pltmp[1] = 0; // Skip M28, because no initialize value is required.
 
 	// M2C CWmin and CWmax setting
 	pHwData->cwmin = DEFAULT_CWMIN;
 	pHwData->cwmax = DEFAULT_CWMAX;
-	pWb35Reg->M2C_MacControl = DEFAULT_CWMIN << 10;
-	pWb35Reg->M2C_MacControl |= DEFAULT_CWMAX;
-	pltmp[2] = pWb35Reg->M2C_MacControl;
+	reg->M2C_MacControl = DEFAULT_CWMIN << 10;
+	reg->M2C_MacControl |= DEFAULT_CWMAX;
+	pltmp[2] = reg->M2C_MacControl;
 
 	// M30 BSSID
 	pltmp[3] = *(u32 *)pHwData->bssid;
@@ -2367,35 +2368,35 @@ void Mxx_initial(  phw_data_t pHwData )
 	pltmp[4] = tmp;
 
 	// M38
-	pWb35Reg->M38_MacControl = (DEFAULT_RATE_RETRY_LIMIT<<8) | (DEFAULT_LONG_RETRY_LIMIT << 4) | DEFAULT_SHORT_RETRY_LIMIT;
-	pltmp[5] = pWb35Reg->M38_MacControl;
+	reg->M38_MacControl = (DEFAULT_RATE_RETRY_LIMIT<<8) | (DEFAULT_LONG_RETRY_LIMIT << 4) | DEFAULT_SHORT_RETRY_LIMIT;
+	pltmp[5] = reg->M38_MacControl;
 
 	// M3C
 	tmp = (DEFAULT_PIFST << 26) | (DEFAULT_EIFST << 16) | (DEFAULT_DIFST << 8) | (DEFAULT_SIFST << 4) | DEFAULT_OSIFST ;
-	pWb35Reg->M3C_MacControl = tmp;
+	reg->M3C_MacControl = tmp;
 	pltmp[6] = tmp;
 
 	// M40
 	pHwData->slot_time_select = DEFAULT_SLOT_TIME;
 	tmp = (DEFAULT_ATIMWD << 16) | DEFAULT_SLOT_TIME;
-	pWb35Reg->M40_MacControl = tmp;
+	reg->M40_MacControl = tmp;
 	pltmp[7] = tmp;
 
 	// M44
 	tmp = DEFAULT_MAX_TX_MSDU_LIFE_TIME << 10; // *1024
-	pWb35Reg->M44_MacControl = tmp;
+	reg->M44_MacControl = tmp;
 	pltmp[8] = tmp;
 
 	// M48
 	pHwData->BeaconPeriod = DEFAULT_BEACON_INTERVAL;
 	pHwData->ProbeDelay = DEFAULT_PROBE_DELAY_TIME;
 	tmp = (DEFAULT_BEACON_INTERVAL << 16) | DEFAULT_PROBE_DELAY_TIME;
-	pWb35Reg->M48_MacControl = tmp;
+	reg->M48_MacControl = tmp;
 	pltmp[9] = tmp;
 
 	//M4C
-	pWb35Reg->M4C_MacStatus = (DEFAULT_PROTOCOL_VERSION << 30) | (DEFAULT_MAC_POWER_STATE << 28) | (DEFAULT_DTIM_ALERT_TIME << 24);
-	pltmp[10] = pWb35Reg->M4C_MacStatus;
+	reg->M4C_MacStatus = (DEFAULT_PROTOCOL_VERSION << 30) | (DEFAULT_MAC_POWER_STATE << 28) | (DEFAULT_DTIM_ALERT_TIME << 24);
+	pltmp[10] = reg->M4C_MacStatus;
 
 	// Burst write
 	//Wb35Reg_BurstWrite( pHwData, 0x0824, pltmp, 11, AUTO_INCREMENT );
@@ -2404,15 +2405,15 @@ void Mxx_initial(  phw_data_t pHwData )
 
 	// M60
 	Wb35Reg_WriteSync( pHwData, 0x0860, 0x12481248 );
-	pWb35Reg->M60_MacControl = 0x12481248;
+	reg->M60_MacControl = 0x12481248;
 
 	// M68
 	Wb35Reg_WriteSync( pHwData, 0x0868, 0x00050900 ); // 20051018 0x000F0F00 ); // 940930 0x00131300
-	pWb35Reg->M68_MacControl = 0x00050900;
+	reg->M68_MacControl = 0x00050900;
 
 	// M98
 	Wb35Reg_WriteSync( pHwData, 0x0898, 0xffff8888 );
-	pWb35Reg->M98_MacControl = 0xffff8888;
+	reg->M98_MacControl = 0xffff8888;
 }
 
 
@@ -2620,7 +2621,7 @@ void EEPROMTxVgaAdjust(  phw_data_t pHwData ) // 20060619.5 Add
 
 void BBProcessor_RateChanging(  phw_data_t pHwData,  u8 rate ) // 20060613.1
 {
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
+	struct wb35_reg *reg = &pHwData->reg;
 	unsigned char		Is11bRate;
 
 	Is11bRate = (rate % 6) ? 1 : 0;
@@ -2630,8 +2631,8 @@ void BBProcessor_RateChanging(  phw_data_t pHwData,  u8 rate ) // 20060613.1
 		case RF_AIROHA_2230S: // 20060420 Add this
 			if( Is11bRate )
 			{
-				if( (pWb35Reg->BB48 != BB48_DEFAULT_AL2230_11B) &&
-					(pWb35Reg->BB4C != BB4C_DEFAULT_AL2230_11B) )
+				if( (reg->BB48 != BB48_DEFAULT_AL2230_11B) &&
+					(reg->BB4C != BB4C_DEFAULT_AL2230_11B) )
 				{
 					Wb35Reg_Write( pHwData, 0x1048, BB48_DEFAULT_AL2230_11B );
 					Wb35Reg_Write( pHwData, 0x104c, BB4C_DEFAULT_AL2230_11B );
@@ -2639,8 +2640,8 @@ void BBProcessor_RateChanging(  phw_data_t pHwData,  u8 rate ) // 20060613.1
 			}
 			else
 			{
-				if( (pWb35Reg->BB48 != BB48_DEFAULT_AL2230_11G) &&
-					(pWb35Reg->BB4C != BB4C_DEFAULT_AL2230_11G) )
+				if( (reg->BB48 != BB48_DEFAULT_AL2230_11G) &&
+					(reg->BB4C != BB4C_DEFAULT_AL2230_11G) )
 				{
 					Wb35Reg_Write( pHwData, 0x1048, BB48_DEFAULT_AL2230_11G );
 					Wb35Reg_Write( pHwData, 0x104c, BB4C_DEFAULT_AL2230_11G );
@@ -2651,22 +2652,22 @@ void BBProcessor_RateChanging(  phw_data_t pHwData,  u8 rate ) // 20060613.1
 		case RF_WB_242: // 20060623 The fix only for old TxVGA setting
 			if( Is11bRate )
 			{
-				if( (pWb35Reg->BB48 != BB48_DEFAULT_WB242_11B) &&
-					(pWb35Reg->BB4C != BB4C_DEFAULT_WB242_11B) )
+				if( (reg->BB48 != BB48_DEFAULT_WB242_11B) &&
+					(reg->BB4C != BB4C_DEFAULT_WB242_11B) )
 				{
-					pWb35Reg->BB48 = BB48_DEFAULT_WB242_11B;
-					pWb35Reg->BB4C = BB4C_DEFAULT_WB242_11B;
+					reg->BB48 = BB48_DEFAULT_WB242_11B;
+					reg->BB4C = BB4C_DEFAULT_WB242_11B;
 					Wb35Reg_Write( pHwData, 0x1048, BB48_DEFAULT_WB242_11B );
 					Wb35Reg_Write( pHwData, 0x104c, BB4C_DEFAULT_WB242_11B );
 				}
 			}
 			else
 			{
-				if( (pWb35Reg->BB48 != BB48_DEFAULT_WB242_11G) &&
-					(pWb35Reg->BB4C != BB4C_DEFAULT_WB242_11G) )
+				if( (reg->BB48 != BB48_DEFAULT_WB242_11G) &&
+					(reg->BB4C != BB4C_DEFAULT_WB242_11G) )
 				{
-					pWb35Reg->BB48 = BB48_DEFAULT_WB242_11G;
-					pWb35Reg->BB4C = BB4C_DEFAULT_WB242_11G;
+					reg->BB48 = BB48_DEFAULT_WB242_11G;
+					reg->BB4C = BB4C_DEFAULT_WB242_11G;
 					Wb35Reg_Write( pHwData, 0x1048, BB48_DEFAULT_WB242_11G );
 					Wb35Reg_Write( pHwData, 0x104c, BB4C_DEFAULT_WB242_11G );
 				}
diff --git a/drivers/staging/winbond/rxisr.c b/drivers/staging/winbond/rxisr.c
deleted file mode 100644
index 18e942c9b82..00000000000
--- a/drivers/staging/winbond/rxisr.c
+++ /dev/null
@@ -1,30 +0,0 @@
-#include "os_common.h"
-
-void vRxTimerInit(PWB32_ADAPTER Adapter)
-{
-	OS_TIMER_INITIAL(&(Adapter->Mds.nTimer), (void*) RxTimerHandler, (void*) Adapter);
-}
-
-void vRxTimerStart(PWB32_ADAPTER Adapter, int timeout_value)
-{
-	if (timeout_value<MIN_TIMEOUT_VAL)
-		timeout_value=MIN_TIMEOUT_VAL;
-
-	OS_TIMER_SET( &(Adapter->Mds.nTimer), timeout_value );
-}
-
-void vRxTimerStop(PWB32_ADAPTER Adapter)
-{
-	OS_TIMER_CANCEL( &(Adapter->Mds.nTimer), 0 );
-}
-
-void RxTimerHandler_1a( PADAPTER Adapter)
-{
-	RxTimerHandler(NULL, Adapter, NULL, NULL);
-}
-
-void RxTimerHandler(void* SystemSpecific1, PWB32_ADAPTER Adapter,
-		    void* SystemSpecific2, void* SystemSpecific3)
-{
-	WARN_ON(1);
-}
diff --git a/drivers/staging/winbond/scan_s.h b/drivers/staging/winbond/scan_s.h
index 1d1b0c4fec1..775bb81f23c 100644
--- a/drivers/staging/winbond/scan_s.h
+++ b/drivers/staging/winbond/scan_s.h
@@ -1,3 +1,9 @@
+#ifndef __WINBOND_SCAN_S_H
+#define __WINBOND_SCAN_S_H
+
+#include <linux/types.h>
+#include "localpara.h"
+
 //
 // SCAN task global CONSTANTS, STRUCTURES, variables
 //
@@ -62,8 +68,7 @@ typedef struct _SCAN_PARAMETERS
 	u8				boCCAbusy;					// Wb: HWMAC CCA busy status
 	u8				reserved_2;
 
-	//NDIS_MINIPORT_TIMER	nTimer;
-	OS_TIMER			nTimer;
+	struct timer_list timer;
 
 	u32				ScanTimeStamp;			//Increase 1 per background scan(1 minute)
 	u32				BssTimeStamp;			//Increase 1 per connect status check
@@ -78,9 +83,9 @@ typedef struct _SCAN_PARAMETERS
 
 } SCAN_PARAMETERS, *psSCAN_PARAMETERS;
 
-// Encapsulate 'Adapter' data structure
-#define psSCAN			(&(Adapter->sScanPara))
-#define psSCANREQ			(&(Adapter->sScanPara.sScanReq))
+// Encapsulate 'adapter' data structure
+#define psSCAN			(&(adapter->sScanPara))
+#define psSCANREQ			(&(adapter->sScanPara.sScanReq))
 
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 //	scan.h
@@ -109,7 +114,8 @@ typedef struct _SCAN_PARAMETERS
 
 // static functions
 
-//static void ScanTimerHandler(PWB32_ADAPTER Adapter);
-//static void vScanTimerStart(PWB32_ADAPTER	Adapter, int timeout_value);
-//static void vScanTimerStop(PWB32_ADAPTER Adapter);
+//static void ScanTimerHandler(struct wbsoft_priv * adapter);
+//static void vScanTimerStart(struct wbsoft_priv *	adapter, int timeout_value);
+//static void vScanTimerStop(struct wbsoft_priv * adapter);
 
+#endif
diff --git a/drivers/staging/winbond/sme_api.c b/drivers/staging/winbond/sme_api.c
deleted file mode 100644
index 31c9673ea86..00000000000
--- a/drivers/staging/winbond/sme_api.c
+++ /dev/null
@@ -1,14 +0,0 @@
-//------------------------------------------------------------------------------------
-// sme_api.c
-//
-// Copyright(C) 2002 Winbond Electronics Corp.
-//
-//
-//------------------------------------------------------------------------------------
-#include "os_common.h"
-
-s8 sme_get_rssi(void *pcore_data, s32 *prssi)
-{
-       BUG();
-       return 0;
-}
diff --git a/drivers/staging/winbond/sme_api.h b/drivers/staging/winbond/sme_api.h
index 745eb376bc7..188a2532bbf 100644
--- a/drivers/staging/winbond/sme_api.h
+++ b/drivers/staging/winbond/sme_api.h
@@ -13,6 +13,10 @@
 #ifndef __SME_API_H__
 #define __SME_API_H__
 
+#include <linux/types.h>
+
+#include "localpara.h"
+
 /****************** INCLUDE FILES SECTION ***********************************/
 //#include "GL\gl_core.h"
 
@@ -52,9 +56,6 @@ s8 sme_set_fragment_threshold(void *pcore_data, u32 threshold);
 s8 sme_get_rts_threshold(void *pcore_data, u32 *pthreshold);
 s8 sme_set_rts_threshold(void *pcore_data, u32 threshold);
 
-// OID_802_11_RSSI
-s8 sme_get_rssi(void *pcore_data, s32 *prssi);
-
 // OID_802_11_CONFIGURATION
 s8 sme_get_beacon_period(void *pcore_data, u16 *pbeacon_period);
 s8 sme_set_beacon_period(void *pcore_data, u16 beacon_period);
diff --git a/drivers/staging/winbond/sme_s.h b/drivers/staging/winbond/sme_s.h
index dfd2fbc4ede..1bd118f83d2 100644
--- a/drivers/staging/winbond/sme_s.h
+++ b/drivers/staging/winbond/sme_s.h
@@ -1,3 +1,11 @@
+#ifndef __WINBOND_SME_S_H
+#define __WINBOND_SME_S_H
+
+#include <linux/types.h>
+
+#include "mac_structures.h"
+#include "localpara.h"
+
 //
 // SME_S.H -
 // SME task global CONSTANTS, STRUCTURES, variables
@@ -106,8 +114,7 @@ typedef struct _SME_PARAMETERS
 	u8				bDesiredPowerSave;
 
 	// SME timer and timeout value
-	//NDIS_MINIPORT_TIMER	nTimer;
-	OS_TIMER			nTimer;
+	struct timer_list timer;
 
 	u8				boInTimerHandler;
 	u8 				boAuthRetryActive;
@@ -196,9 +203,9 @@ typedef struct _SME_PARAMETERS
 
 } SME_PARAMETERS, *PSME_PARAMETERS;
 
-#define psSME			(&(Adapter->sSmePara))
+#define psSME			(&(adapter->sSmePara))
 
-#define wSMEGetCurrentSTAState(Adapter)		((u16)(Adapter)->sSmePara.wState)
+#define wSMEGetCurrentSTAState(adapter)		((u16)(adapter)->sSmePara.wState)
 
 
 
@@ -226,3 +233,4 @@ typedef struct _SME_PARAMETERS
 
 // Static function
 
+#endif
diff --git a/drivers/staging/winbond/linux/sysdef.h b/drivers/staging/winbond/sysdef.h
index d46d63e5c67..251b9c553b6 100644
--- a/drivers/staging/winbond/linux/sysdef.h
+++ b/drivers/staging/winbond/sysdef.h
@@ -37,37 +37,4 @@
 #define _PE_USB_INI_DUMP_
 #endif
 
-
-
-#include "common.h"	// Individual file depends on OS
-
-#include "../wb35_ver.h"
-#include "../mac_structures.h"
-#include "../ds_tkip.h"
-#include "../localpara.h"
-#include "../sme_s.h"
-#include "../scan_s.h"
-#include "../mds_s.h"
-#include "../mlme_s.h"
-#include "../bssdscpt.h"
-#include "../sme_api.h"
-#include "../gl_80211.h"
-#include "../mto.h"
-#include "../wblinux_s.h"
-#include "../wbhal_s.h"
-
-
-#include "../adapter.h"
-
-#include "../mlme_mib.h"
-#include "../mds_f.h"
-#include "../bss_f.h"
-#include "../mlmetxrx_f.h"
-#include "../mto_f.h"
-#include "../wbhal_f.h"
-#include "../wblinux_f.h"
-// Kernel Timer resolution, NDIS is 10ms, 10000us
-#define MIN_TIMEOUT_VAL	(10) //ms
-
-
 #endif
diff --git a/drivers/staging/winbond/linux/wb35reg.c b/drivers/staging/winbond/wb35reg.c
index ebb6db5438a..c74b3fdcbda 100644
--- a/drivers/staging/winbond/linux/wb35reg.c
+++ b/drivers/staging/winbond/wb35reg.c
@@ -1,9 +1,12 @@
 #include "sysdef.h"
+#include "wb35reg_f.h"
+
+#include <linux/usb.h>
 
 extern void phy_calibration_winbond(hw_data_t *phw_data, u32 frequency);
 
-// TRUE  : read command process successfully
-// FALSE : register not support
+// true  : read command process successfully
+// false : register not support
 // RegisterNo : start base
 // pRegisterData : data point
 // NumberOfData : number of register data
@@ -12,135 +15,135 @@ extern void phy_calibration_winbond(hw_data_t *phw_data, u32 frequency);
 unsigned char
 Wb35Reg_BurstWrite(phw_data_t pHwData, u16 RegisterNo, u32 * pRegisterData, u8 NumberOfData, u8 Flag)
 {
-	PWB35REG pWb35Reg = &pHwData->Wb35Reg;
-	PURB		pUrb = NULL;
-	PREG_QUEUE	pRegQueue = NULL;
+	struct wb35_reg *reg = &pHwData->reg;
+	struct urb	*urb = NULL;
+	struct wb35_reg_queue *reg_queue = NULL;
 	u16		UrbSize;
 	struct      usb_ctrlrequest *dr;
 	u16		i, DataSize = NumberOfData*4;
 
 	// Module shutdown
 	if (pHwData->SurpriseRemove)
-		return FALSE;
+		return false;
 
 	// Trying to use burst write function if use new hardware
-	UrbSize = sizeof(REG_QUEUE) + DataSize + sizeof(struct usb_ctrlrequest);
-	OS_MEMORY_ALLOC( (void* *)&pRegQueue, UrbSize );
-	pUrb = wb_usb_alloc_urb(0);
-	if( pUrb && pRegQueue ) {
-		pRegQueue->DIRECT = 2;// burst write register
-		pRegQueue->INDEX = RegisterNo;
-		pRegQueue->pBuffer = (u32 *)((u8 *)pRegQueue + sizeof(REG_QUEUE));
-		memcpy( pRegQueue->pBuffer, pRegisterData, DataSize );
+	UrbSize = sizeof(struct wb35_reg_queue) + DataSize + sizeof(struct usb_ctrlrequest);
+	reg_queue = kzalloc(UrbSize, GFP_ATOMIC);
+	urb = usb_alloc_urb(0, GFP_ATOMIC);
+	if( urb && reg_queue ) {
+		reg_queue->DIRECT = 2;// burst write register
+		reg_queue->INDEX = RegisterNo;
+		reg_queue->pBuffer = (u32 *)((u8 *)reg_queue + sizeof(struct wb35_reg_queue));
+		memcpy( reg_queue->pBuffer, pRegisterData, DataSize );
 		//the function for reversing register data from little endian to big endian
 		for( i=0; i<NumberOfData ; i++ )
-			pRegQueue->pBuffer[i] = cpu_to_le32( pRegQueue->pBuffer[i] );
+			reg_queue->pBuffer[i] = cpu_to_le32( reg_queue->pBuffer[i] );
 
-		dr = (struct usb_ctrlrequest *)((u8 *)pRegQueue + sizeof(REG_QUEUE) + DataSize);
+		dr = (struct usb_ctrlrequest *)((u8 *)reg_queue + sizeof(struct wb35_reg_queue) + DataSize);
 		dr->bRequestType = USB_TYPE_VENDOR | USB_DIR_OUT | USB_RECIP_DEVICE;
 		dr->bRequest = 0x04; // USB or vendor-defined request code, burst mode
 		dr->wValue = cpu_to_le16( Flag ); // 0: Register number auto-increment, 1: No auto increment
 		dr->wIndex = cpu_to_le16( RegisterNo );
 		dr->wLength = cpu_to_le16( DataSize );
-		pRegQueue->Next = NULL;
-		pRegQueue->pUsbReq = dr;
-		pRegQueue->pUrb = pUrb;
+		reg_queue->Next = NULL;
+		reg_queue->pUsbReq = dr;
+		reg_queue->urb = urb;
 
-		spin_lock_irq( &pWb35Reg->EP0VM_spin_lock );
-		if (pWb35Reg->pRegFirst == NULL)
-			pWb35Reg->pRegFirst = pRegQueue;
+		spin_lock_irq( &reg->EP0VM_spin_lock );
+		if (reg->reg_first == NULL)
+			reg->reg_first = reg_queue;
 		else
-			pWb35Reg->pRegLast->Next = pRegQueue;
-		pWb35Reg->pRegLast = pRegQueue;
+			reg->reg_last->Next = reg_queue;
+		reg->reg_last = reg_queue;
 
-		spin_unlock_irq( &pWb35Reg->EP0VM_spin_lock );
+		spin_unlock_irq( &reg->EP0VM_spin_lock );
 
 		// Start EP0VM
 		Wb35Reg_EP0VM_start(pHwData);
 
-		return TRUE;
+		return true;
 	} else {
-		if (pUrb)
-			usb_free_urb(pUrb);
-		if (pRegQueue)
-			kfree(pRegQueue);
-		return FALSE;
+		if (urb)
+			usb_free_urb(urb);
+		if (reg_queue)
+			kfree(reg_queue);
+		return false;
 	}
-   return FALSE;
+   return false;
 }
 
 void
 Wb35Reg_Update(phw_data_t pHwData,  u16 RegisterNo,  u32 RegisterValue)
 {
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
+	struct wb35_reg *reg = &pHwData->reg;
 	switch (RegisterNo) {
-	case 0x3b0: pWb35Reg->U1B0 = RegisterValue; break;
-	case 0x3bc: pWb35Reg->U1BC_LEDConfigure = RegisterValue; break;
-	case 0x400: pWb35Reg->D00_DmaControl = RegisterValue; break;
-	case 0x800: pWb35Reg->M00_MacControl = RegisterValue; break;
-	case 0x804: pWb35Reg->M04_MulticastAddress1 = RegisterValue; break;
-	case 0x808: pWb35Reg->M08_MulticastAddress2 = RegisterValue; break;
-	case 0x824: pWb35Reg->M24_MacControl = RegisterValue; break;
-	case 0x828: pWb35Reg->M28_MacControl = RegisterValue; break;
-	case 0x82c: pWb35Reg->M2C_MacControl = RegisterValue; break;
-	case 0x838: pWb35Reg->M38_MacControl = RegisterValue; break;
-	case 0x840: pWb35Reg->M40_MacControl = RegisterValue; break;
-	case 0x844: pWb35Reg->M44_MacControl = RegisterValue; break;
-	case 0x848: pWb35Reg->M48_MacControl = RegisterValue; break;
-	case 0x84c: pWb35Reg->M4C_MacStatus = RegisterValue; break;
-	case 0x860: pWb35Reg->M60_MacControl = RegisterValue; break;
-	case 0x868: pWb35Reg->M68_MacControl = RegisterValue; break;
-	case 0x870: pWb35Reg->M70_MacControl = RegisterValue; break;
-	case 0x874: pWb35Reg->M74_MacControl = RegisterValue; break;
-	case 0x878: pWb35Reg->M78_ERPInformation = RegisterValue; break;
-	case 0x87C: pWb35Reg->M7C_MacControl = RegisterValue; break;
-	case 0x880: pWb35Reg->M80_MacControl = RegisterValue; break;
-	case 0x884: pWb35Reg->M84_MacControl = RegisterValue; break;
-	case 0x888: pWb35Reg->M88_MacControl = RegisterValue; break;
-	case 0x898: pWb35Reg->M98_MacControl = RegisterValue; break;
-	case 0x100c: pWb35Reg->BB0C = RegisterValue; break;
-	case 0x102c: pWb35Reg->BB2C = RegisterValue; break;
-	case 0x1030: pWb35Reg->BB30 = RegisterValue; break;
-	case 0x103c: pWb35Reg->BB3C = RegisterValue; break;
-	case 0x1048: pWb35Reg->BB48 = RegisterValue; break;
-	case 0x104c: pWb35Reg->BB4C = RegisterValue; break;
-	case 0x1050: pWb35Reg->BB50 = RegisterValue; break;
-	case 0x1054: pWb35Reg->BB54 = RegisterValue; break;
-	case 0x1058: pWb35Reg->BB58 = RegisterValue; break;
-	case 0x105c: pWb35Reg->BB5C = RegisterValue; break;
-	case 0x1060: pWb35Reg->BB60 = RegisterValue; break;
+	case 0x3b0: reg->U1B0 = RegisterValue; break;
+	case 0x3bc: reg->U1BC_LEDConfigure = RegisterValue; break;
+	case 0x400: reg->D00_DmaControl = RegisterValue; break;
+	case 0x800: reg->M00_MacControl = RegisterValue; break;
+	case 0x804: reg->M04_MulticastAddress1 = RegisterValue; break;
+	case 0x808: reg->M08_MulticastAddress2 = RegisterValue; break;
+	case 0x824: reg->M24_MacControl = RegisterValue; break;
+	case 0x828: reg->M28_MacControl = RegisterValue; break;
+	case 0x82c: reg->M2C_MacControl = RegisterValue; break;
+	case 0x838: reg->M38_MacControl = RegisterValue; break;
+	case 0x840: reg->M40_MacControl = RegisterValue; break;
+	case 0x844: reg->M44_MacControl = RegisterValue; break;
+	case 0x848: reg->M48_MacControl = RegisterValue; break;
+	case 0x84c: reg->M4C_MacStatus = RegisterValue; break;
+	case 0x860: reg->M60_MacControl = RegisterValue; break;
+	case 0x868: reg->M68_MacControl = RegisterValue; break;
+	case 0x870: reg->M70_MacControl = RegisterValue; break;
+	case 0x874: reg->M74_MacControl = RegisterValue; break;
+	case 0x878: reg->M78_ERPInformation = RegisterValue; break;
+	case 0x87C: reg->M7C_MacControl = RegisterValue; break;
+	case 0x880: reg->M80_MacControl = RegisterValue; break;
+	case 0x884: reg->M84_MacControl = RegisterValue; break;
+	case 0x888: reg->M88_MacControl = RegisterValue; break;
+	case 0x898: reg->M98_MacControl = RegisterValue; break;
+	case 0x100c: reg->BB0C = RegisterValue; break;
+	case 0x102c: reg->BB2C = RegisterValue; break;
+	case 0x1030: reg->BB30 = RegisterValue; break;
+	case 0x103c: reg->BB3C = RegisterValue; break;
+	case 0x1048: reg->BB48 = RegisterValue; break;
+	case 0x104c: reg->BB4C = RegisterValue; break;
+	case 0x1050: reg->BB50 = RegisterValue; break;
+	case 0x1054: reg->BB54 = RegisterValue; break;
+	case 0x1058: reg->BB58 = RegisterValue; break;
+	case 0x105c: reg->BB5C = RegisterValue; break;
+	case 0x1060: reg->BB60 = RegisterValue; break;
 	}
 }
 
-// TRUE  : read command process successfully
-// FALSE : register not support
+// true  : read command process successfully
+// false : register not support
 unsigned char
 Wb35Reg_WriteSync(  phw_data_t pHwData,  u16 RegisterNo,  u32 RegisterValue )
 {
-	PWB35REG pWb35Reg = &pHwData->Wb35Reg;
+	struct wb35_reg *reg = &pHwData->reg;
 	int ret = -1;
 
 	// Module shutdown
 	if (pHwData->SurpriseRemove)
-		return FALSE;
+		return false;
 
 	RegisterValue = cpu_to_le32(RegisterValue);
 
 	// update the register by send usb message------------------------------------
-	pWb35Reg->SyncIoPause = 1;
+	reg->SyncIoPause = 1;
 
 	// 20060717.5 Wait until EP0VM stop
-	while (pWb35Reg->EP0vm_state != VM_STOP)
-		OS_SLEEP(10000);
+	while (reg->EP0vm_state != VM_STOP)
+		msleep(10);
 
 	// Sync IoCallDriver
-	pWb35Reg->EP0vm_state = VM_RUNNING;
+	reg->EP0vm_state = VM_RUNNING;
 	ret = usb_control_msg( pHwData->WbUsb.udev,
 			       usb_sndctrlpipe( pHwData->WbUsb.udev, 0 ),
 			       0x03, USB_TYPE_VENDOR | USB_RECIP_DEVICE | USB_DIR_OUT,
 			       0x0,RegisterNo, &RegisterValue, 4, HZ*100 );
-	pWb35Reg->EP0vm_state = VM_STOP;
-	pWb35Reg->SyncIoPause = 0;
+	reg->EP0vm_state = VM_STOP;
+	reg->SyncIoPause = 0;
 
 	Wb35Reg_EP0VM_start(pHwData);
 
@@ -150,38 +153,38 @@ Wb35Reg_WriteSync(  phw_data_t pHwData,  u16 RegisterNo,  u32 RegisterValue )
 		#endif
 
 		pHwData->SurpriseRemove = 1; // 20060704.2
-		return FALSE;
+		return false;
 	}
 
-	return TRUE;
+	return true;
 }
 
-// TRUE  : read command process successfully
-// FALSE : register not support
+// true  : read command process successfully
+// false : register not support
 unsigned char
 Wb35Reg_Write(  phw_data_t pHwData,  u16 RegisterNo,  u32 RegisterValue )
 {
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
+	struct wb35_reg *reg = &pHwData->reg;
 	struct usb_ctrlrequest *dr;
-	PURB		pUrb = NULL;
-	PREG_QUEUE	pRegQueue = NULL;
+	struct urb	*urb = NULL;
+	struct wb35_reg_queue *reg_queue = NULL;
 	u16		UrbSize;
 
 
 	// Module shutdown
 	if (pHwData->SurpriseRemove)
-		return FALSE;
+		return false;
 
 	// update the register by send urb request------------------------------------
-	UrbSize = sizeof(REG_QUEUE) + sizeof(struct usb_ctrlrequest);
-	OS_MEMORY_ALLOC( (void* *)&pRegQueue, UrbSize );
-	pUrb = wb_usb_alloc_urb(0);
-	if (pUrb && pRegQueue) {
-		pRegQueue->DIRECT = 1;// burst write register
-		pRegQueue->INDEX = RegisterNo;
-		pRegQueue->VALUE = cpu_to_le32(RegisterValue);
-		pRegQueue->RESERVED_VALID = FALSE;
-		dr = (struct usb_ctrlrequest *)((u8 *)pRegQueue + sizeof(REG_QUEUE));
+	UrbSize = sizeof(struct wb35_reg_queue) + sizeof(struct usb_ctrlrequest);
+	reg_queue = kzalloc(UrbSize, GFP_ATOMIC);
+	urb = usb_alloc_urb(0, GFP_ATOMIC);
+	if (urb && reg_queue) {
+		reg_queue->DIRECT = 1;// burst write register
+		reg_queue->INDEX = RegisterNo;
+		reg_queue->VALUE = cpu_to_le32(RegisterValue);
+		reg_queue->RESERVED_VALID = false;
+		dr = (struct usb_ctrlrequest *)((u8 *)reg_queue + sizeof(struct wb35_reg_queue));
 		dr->bRequestType = USB_TYPE_VENDOR|USB_DIR_OUT |USB_RECIP_DEVICE;
 		dr->bRequest = 0x03; // USB or vendor-defined request code, burst mode
 		dr->wValue = cpu_to_le16(0x0);
@@ -189,61 +192,61 @@ Wb35Reg_Write(  phw_data_t pHwData,  u16 RegisterNo,  u32 RegisterValue )
 		dr->wLength = cpu_to_le16(4);
 
 		// Enter the sending queue
-		pRegQueue->Next = NULL;
-		pRegQueue->pUsbReq = dr;
-		pRegQueue->pUrb = pUrb;
+		reg_queue->Next = NULL;
+		reg_queue->pUsbReq = dr;
+		reg_queue->urb = urb;
 
-		spin_lock_irq(&pWb35Reg->EP0VM_spin_lock );
-		if (pWb35Reg->pRegFirst == NULL)
-			pWb35Reg->pRegFirst = pRegQueue;
+		spin_lock_irq(&reg->EP0VM_spin_lock );
+		if (reg->reg_first == NULL)
+			reg->reg_first = reg_queue;
 		else
-			pWb35Reg->pRegLast->Next = pRegQueue;
-		pWb35Reg->pRegLast = pRegQueue;
+			reg->reg_last->Next = reg_queue;
+		reg->reg_last = reg_queue;
 
-		spin_unlock_irq( &pWb35Reg->EP0VM_spin_lock );
+		spin_unlock_irq( &reg->EP0VM_spin_lock );
 
 		// Start EP0VM
 		Wb35Reg_EP0VM_start(pHwData);
 
-		return TRUE;
+		return true;
 	} else {
-		if (pUrb)
-			usb_free_urb(pUrb);
-		kfree(pRegQueue);
-		return FALSE;
+		if (urb)
+			usb_free_urb(urb);
+		kfree(reg_queue);
+		return false;
 	}
 }
 
 //This command will be executed with a user defined value. When it completes,
 //this value is useful. For example, hal_set_current_channel will use it.
-// TRUE  : read command process successfully
-// FALSE : register not support
+// true  : read command process successfully
+// false : register not support
 unsigned char
 Wb35Reg_WriteWithCallbackValue( phw_data_t pHwData, u16 RegisterNo, u32 RegisterValue,
 				s8 *pValue, s8 Len)
 {
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
+	struct wb35_reg *reg = &pHwData->reg;
 	struct usb_ctrlrequest *dr;
-	PURB		pUrb = NULL;
-	PREG_QUEUE	pRegQueue = NULL;
+	struct urb	*urb = NULL;
+	struct wb35_reg_queue *reg_queue = NULL;
 	u16		UrbSize;
 
 	// Module shutdown
 	if (pHwData->SurpriseRemove)
-		return FALSE;
+		return false;
 
 	// update the register by send urb request------------------------------------
-	UrbSize = sizeof(REG_QUEUE) + sizeof(struct usb_ctrlrequest);
-	OS_MEMORY_ALLOC((void* *) &pRegQueue, UrbSize );
-	pUrb = wb_usb_alloc_urb(0);
-	if (pUrb && pRegQueue) {
-		pRegQueue->DIRECT = 1;// burst write register
-		pRegQueue->INDEX = RegisterNo;
-		pRegQueue->VALUE = cpu_to_le32(RegisterValue);
+	UrbSize = sizeof(struct wb35_reg_queue) + sizeof(struct usb_ctrlrequest);
+	reg_queue = kzalloc(UrbSize, GFP_ATOMIC);
+	urb = usb_alloc_urb(0, GFP_ATOMIC);
+	if (urb && reg_queue) {
+		reg_queue->DIRECT = 1;// burst write register
+		reg_queue->INDEX = RegisterNo;
+		reg_queue->VALUE = cpu_to_le32(RegisterValue);
 		//NOTE : Users must guarantee the size of value will not exceed the buffer size.
-		memcpy(pRegQueue->RESERVED, pValue, Len);
-		pRegQueue->RESERVED_VALID = TRUE;
-		dr = (struct usb_ctrlrequest *)((u8 *)pRegQueue + sizeof(REG_QUEUE));
+		memcpy(reg_queue->RESERVED, pValue, Len);
+		reg_queue->RESERVED_VALID = true;
+		dr = (struct usb_ctrlrequest *)((u8 *)reg_queue + sizeof(struct wb35_reg_queue));
 		dr->bRequestType = USB_TYPE_VENDOR|USB_DIR_OUT |USB_RECIP_DEVICE;
 		dr->bRequest = 0x03; // USB or vendor-defined request code, burst mode
 		dr->wValue = cpu_to_le16(0x0);
@@ -251,52 +254,52 @@ Wb35Reg_WriteWithCallbackValue( phw_data_t pHwData, u16 RegisterNo, u32 Register
 		dr->wLength = cpu_to_le16(4);
 
 		// Enter the sending queue
-		pRegQueue->Next = NULL;
-		pRegQueue->pUsbReq = dr;
-		pRegQueue->pUrb = pUrb;
-		spin_lock_irq (&pWb35Reg->EP0VM_spin_lock );
-		if( pWb35Reg->pRegFirst == NULL )
-			pWb35Reg->pRegFirst = pRegQueue;
+		reg_queue->Next = NULL;
+		reg_queue->pUsbReq = dr;
+		reg_queue->urb = urb;
+		spin_lock_irq (&reg->EP0VM_spin_lock );
+		if( reg->reg_first == NULL )
+			reg->reg_first = reg_queue;
 		else
-			pWb35Reg->pRegLast->Next = pRegQueue;
-		pWb35Reg->pRegLast = pRegQueue;
+			reg->reg_last->Next = reg_queue;
+		reg->reg_last = reg_queue;
 
-		spin_unlock_irq ( &pWb35Reg->EP0VM_spin_lock );
+		spin_unlock_irq ( &reg->EP0VM_spin_lock );
 
 		// Start EP0VM
 		Wb35Reg_EP0VM_start(pHwData);
-		return TRUE;
+		return true;
 	} else {
-		if (pUrb)
-			usb_free_urb(pUrb);
-		kfree(pRegQueue);
-		return FALSE;
+		if (urb)
+			usb_free_urb(urb);
+		kfree(reg_queue);
+		return false;
 	}
 }
 
-// TRUE  : read command process successfully
-// FALSE : register not support
+// true  : read command process successfully
+// false : register not support
 // pRegisterValue : It must be a resident buffer due to asynchronous read register.
 unsigned char
 Wb35Reg_ReadSync(  phw_data_t pHwData,  u16 RegisterNo,   u32 * pRegisterValue )
 {
-	PWB35REG pWb35Reg = &pHwData->Wb35Reg;
+	struct wb35_reg *reg = &pHwData->reg;
 	u32 *	pltmp = pRegisterValue;
 	int ret = -1;
 
 	// Module shutdown
 	if (pHwData->SurpriseRemove)
-		return FALSE;
+		return false;
 
 	// Read the register by send usb message------------------------------------
 
-	pWb35Reg->SyncIoPause = 1;
+	reg->SyncIoPause = 1;
 
 	// 20060717.5 Wait until EP0VM stop
-	while (pWb35Reg->EP0vm_state != VM_STOP)
-		OS_SLEEP(10000);
+	while (reg->EP0vm_state != VM_STOP)
+		msleep(10);
 
-	pWb35Reg->EP0vm_state = VM_RUNNING;
+	reg->EP0vm_state = VM_RUNNING;
 	ret = usb_control_msg( pHwData->WbUsb.udev,
 			       usb_rcvctrlpipe(pHwData->WbUsb.udev, 0),
 			       0x01, USB_TYPE_VENDOR|USB_RECIP_DEVICE|USB_DIR_IN,
@@ -304,10 +307,10 @@ Wb35Reg_ReadSync(  phw_data_t pHwData,  u16 RegisterNo,   u32 * pRegisterValue )
 
 	*pRegisterValue = cpu_to_le32(*pltmp);
 
-	pWb35Reg->EP0vm_state = VM_STOP;
+	reg->EP0vm_state = VM_STOP;
 
 	Wb35Reg_Update( pHwData, RegisterNo, *pRegisterValue );
-	pWb35Reg->SyncIoPause = 0;
+	reg->SyncIoPause = 0;
 
 	Wb35Reg_EP0VM_start( pHwData );
 
@@ -317,38 +320,38 @@ Wb35Reg_ReadSync(  phw_data_t pHwData,  u16 RegisterNo,   u32 * pRegisterValue )
 		#endif
 
 		pHwData->SurpriseRemove = 1; // 20060704.2
-		return FALSE;
+		return false;
 	}
 
-	return TRUE;
+	return true;
 }
 
-// TRUE  : read command process successfully
-// FALSE : register not support
+// true  : read command process successfully
+// false : register not support
 // pRegisterValue : It must be a resident buffer due to asynchronous read register.
 unsigned char
 Wb35Reg_Read(phw_data_t pHwData, u16 RegisterNo,  u32 * pRegisterValue )
 {
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
+	struct wb35_reg *reg = &pHwData->reg;
 	struct usb_ctrlrequest * dr;
-	PURB		pUrb;
-	PREG_QUEUE	pRegQueue;
+	struct urb	*urb;
+	struct wb35_reg_queue *reg_queue;
 	u16		UrbSize;
 
 	// Module shutdown
 	if (pHwData->SurpriseRemove)
-		return FALSE;
+		return false;
 
 	// update the variable by send Urb to read register ------------------------------------
-	UrbSize = sizeof(REG_QUEUE) + sizeof(struct usb_ctrlrequest);
-	OS_MEMORY_ALLOC( (void* *)&pRegQueue, UrbSize );
-	pUrb = wb_usb_alloc_urb(0);
-	if( pUrb && pRegQueue )
+	UrbSize = sizeof(struct wb35_reg_queue) + sizeof(struct usb_ctrlrequest);
+	reg_queue = kzalloc(UrbSize, GFP_ATOMIC);
+	urb = usb_alloc_urb(0, GFP_ATOMIC);
+	if( urb && reg_queue )
 	{
-		pRegQueue->DIRECT = 0;// read register
-		pRegQueue->INDEX = RegisterNo;
-		pRegQueue->pBuffer = pRegisterValue;
-		dr = (struct usb_ctrlrequest *)((u8 *)pRegQueue + sizeof(REG_QUEUE));
+		reg_queue->DIRECT = 0;// read register
+		reg_queue->INDEX = RegisterNo;
+		reg_queue->pBuffer = pRegisterValue;
+		dr = (struct usb_ctrlrequest *)((u8 *)reg_queue + sizeof(struct wb35_reg_queue));
 		dr->bRequestType = USB_TYPE_VENDOR|USB_RECIP_DEVICE|USB_DIR_IN;
 		dr->bRequest = 0x01; // USB or vendor-defined request code, burst mode
 		dr->wValue = cpu_to_le16(0x0);
@@ -356,27 +359,27 @@ Wb35Reg_Read(phw_data_t pHwData, u16 RegisterNo,  u32 * pRegisterValue )
 		dr->wLength = cpu_to_le16 (4);
 
 		// Enter the sending queue
-		pRegQueue->Next = NULL;
-		pRegQueue->pUsbReq = dr;
-		pRegQueue->pUrb = pUrb;
-		spin_lock_irq ( &pWb35Reg->EP0VM_spin_lock );
-		if( pWb35Reg->pRegFirst == NULL )
-			pWb35Reg->pRegFirst = pRegQueue;
+		reg_queue->Next = NULL;
+		reg_queue->pUsbReq = dr;
+		reg_queue->urb = urb;
+		spin_lock_irq ( &reg->EP0VM_spin_lock );
+		if( reg->reg_first == NULL )
+			reg->reg_first = reg_queue;
 		else
-			pWb35Reg->pRegLast->Next = pRegQueue;
-		pWb35Reg->pRegLast = pRegQueue;
+			reg->reg_last->Next = reg_queue;
+		reg->reg_last = reg_queue;
 
-		spin_unlock_irq( &pWb35Reg->EP0VM_spin_lock );
+		spin_unlock_irq( &reg->EP0VM_spin_lock );
 
 		// Start EP0VM
 		Wb35Reg_EP0VM_start( pHwData );
 
-		return TRUE;
+		return true;
 	} else {
-		if (pUrb)
-			usb_free_urb( pUrb );
-		kfree(pRegQueue);
-		return FALSE;
+		if (urb)
+			usb_free_urb( urb );
+		kfree(reg_queue);
+		return false;
 	}
 }
 
@@ -384,57 +387,57 @@ Wb35Reg_Read(phw_data_t pHwData, u16 RegisterNo,  u32 * pRegisterValue )
 void
 Wb35Reg_EP0VM_start(  phw_data_t pHwData )
 {
-	PWB35REG pWb35Reg = &pHwData->Wb35Reg;
+	struct wb35_reg *reg = &pHwData->reg;
 
-	if (OS_ATOMIC_INC( pHwData->Adapter, &pWb35Reg->RegFireCount) == 1) {
-		pWb35Reg->EP0vm_state = VM_RUNNING;
+	if (atomic_inc_return(&reg->RegFireCount) == 1) {
+		reg->EP0vm_state = VM_RUNNING;
 		Wb35Reg_EP0VM(pHwData);
 	} else
-		OS_ATOMIC_DEC( pHwData->Adapter, &pWb35Reg->RegFireCount );
+		atomic_dec(&reg->RegFireCount);
 }
 
 void
 Wb35Reg_EP0VM(phw_data_t pHwData )
 {
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
-	PURB		pUrb;
+	struct wb35_reg *reg = &pHwData->reg;
+	struct urb	*urb;
 	struct usb_ctrlrequest *dr;
 	u32 *		pBuffer;
 	int			ret = -1;
-	PREG_QUEUE	pRegQueue;
+	struct wb35_reg_queue *reg_queue;
 
 
-	if (pWb35Reg->SyncIoPause)
+	if (reg->SyncIoPause)
 		goto cleanup;
 
 	if (pHwData->SurpriseRemove)
 		goto cleanup;
 
 	// Get the register data and send to USB through Irp
-	spin_lock_irq( &pWb35Reg->EP0VM_spin_lock );
-	pRegQueue = pWb35Reg->pRegFirst;
-	spin_unlock_irq( &pWb35Reg->EP0VM_spin_lock );
+	spin_lock_irq( &reg->EP0VM_spin_lock );
+	reg_queue = reg->reg_first;
+	spin_unlock_irq( &reg->EP0VM_spin_lock );
 
-	if (!pRegQueue)
+	if (!reg_queue)
 		goto cleanup;
 
 	// Get an Urb, send it
-	pUrb = (PURB)pRegQueue->pUrb;
+	urb = (struct urb *)reg_queue->urb;
 
-	dr = pRegQueue->pUsbReq;
-	pUrb = pRegQueue->pUrb;
-	pBuffer = pRegQueue->pBuffer;
-	if (pRegQueue->DIRECT == 1) // output
-		pBuffer = &pRegQueue->VALUE;
+	dr = reg_queue->pUsbReq;
+	urb = reg_queue->urb;
+	pBuffer = reg_queue->pBuffer;
+	if (reg_queue->DIRECT == 1) // output
+		pBuffer = &reg_queue->VALUE;
 
-	usb_fill_control_urb( pUrb, pHwData->WbUsb.udev,
-			      REG_DIRECTION(pHwData->WbUsb.udev,pRegQueue),
+	usb_fill_control_urb( urb, pHwData->WbUsb.udev,
+			      REG_DIRECTION(pHwData->WbUsb.udev,reg_queue),
 			      (u8 *)dr,pBuffer,cpu_to_le16(dr->wLength),
 			      Wb35Reg_EP0VM_complete, (void*)pHwData);
 
-	pWb35Reg->EP0vm_state = VM_RUNNING;
+	reg->EP0vm_state = VM_RUNNING;
 
-	ret = wb_usb_submit_urb( pUrb );
+	ret = usb_submit_urb(urb, GFP_ATOMIC);
 
 	if (ret < 0) {
 #ifdef _PE_REG_DUMP_
@@ -446,41 +449,41 @@ Wb35Reg_EP0VM(phw_data_t pHwData )
 	return;
 
  cleanup:
-	pWb35Reg->EP0vm_state = VM_STOP;
-	OS_ATOMIC_DEC( pHwData->Adapter, &pWb35Reg->RegFireCount );
+	reg->EP0vm_state = VM_STOP;
+	atomic_dec(&reg->RegFireCount);
 }
 
 
 void
-Wb35Reg_EP0VM_complete(PURB pUrb)
+Wb35Reg_EP0VM_complete(struct urb *urb)
 {
-	phw_data_t  pHwData = (phw_data_t)pUrb->context;
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
-	PREG_QUEUE	pRegQueue;
+	phw_data_t  pHwData = (phw_data_t)urb->context;
+	struct wb35_reg *reg = &pHwData->reg;
+	struct wb35_reg_queue *reg_queue;
 
 
 	// Variable setting
-	pWb35Reg->EP0vm_state = VM_COMPLETED;
-	pWb35Reg->EP0VM_status = pUrb->status;
+	reg->EP0vm_state = VM_COMPLETED;
+	reg->EP0VM_status = urb->status;
 
 	if (pHwData->SurpriseRemove) { // Let WbWlanHalt to handle surprise remove
-		pWb35Reg->EP0vm_state = VM_STOP;
-		OS_ATOMIC_DEC( pHwData->Adapter, &pWb35Reg->RegFireCount );
+		reg->EP0vm_state = VM_STOP;
+		atomic_dec(&reg->RegFireCount);
 	} else {
 		// Complete to send, remove the URB from the first
-		spin_lock_irq( &pWb35Reg->EP0VM_spin_lock );
-		pRegQueue = pWb35Reg->pRegFirst;
-		if (pRegQueue == pWb35Reg->pRegLast)
-			pWb35Reg->pRegLast = NULL;
-		pWb35Reg->pRegFirst = pWb35Reg->pRegFirst->Next;
-		spin_unlock_irq( &pWb35Reg->EP0VM_spin_lock );
-
-		if (pWb35Reg->EP0VM_status) {
+		spin_lock_irq( &reg->EP0VM_spin_lock );
+		reg_queue = reg->reg_first;
+		if (reg_queue == reg->reg_last)
+			reg->reg_last = NULL;
+		reg->reg_first = reg->reg_first->Next;
+		spin_unlock_irq( &reg->EP0VM_spin_lock );
+
+		if (reg->EP0VM_status) {
 #ifdef _PE_REG_DUMP_
 			WBDEBUG(("EP0 IoCompleteRoutine return error\n"));
-			DebugUsbdStatusInformation( pWb35Reg->EP0VM_status );
+			DebugUsbdStatusInformation( reg->EP0VM_status );
 #endif
-			pWb35Reg->EP0vm_state = VM_STOP;
+			reg->EP0vm_state = VM_STOP;
 			pHwData->SurpriseRemove = 1;
 		} else {
 			// Success. Update the result
@@ -489,52 +492,52 @@ Wb35Reg_EP0VM_complete(PURB pUrb)
 			Wb35Reg_EP0VM(pHwData);
 		}
 
-   		kfree(pRegQueue);
+   		kfree(reg_queue);
 	}
 
-	usb_free_urb(pUrb);
+	usb_free_urb(urb);
 }
 
 
 void
 Wb35Reg_destroy(phw_data_t pHwData)
 {
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
-	PURB		pUrb;
-	PREG_QUEUE	pRegQueue;
+	struct wb35_reg *reg = &pHwData->reg;
+	struct urb	*urb;
+	struct wb35_reg_queue *reg_queue;
 
 
 	Uxx_power_off_procedure(pHwData);
 
 	// Wait for Reg operation completed
 	do {
-		OS_SLEEP(10000); // Delay for waiting function enter 940623.1.a
-	} while (pWb35Reg->EP0vm_state != VM_STOP);
-	OS_SLEEP(10000);  // Delay for waiting function enter 940623.1.b
+		msleep(10); // Delay for waiting function enter 940623.1.a
+	} while (reg->EP0vm_state != VM_STOP);
+	msleep(10);  // Delay for waiting function enter 940623.1.b
 
 	// Release all the data in RegQueue
-	spin_lock_irq( &pWb35Reg->EP0VM_spin_lock );
-	pRegQueue = pWb35Reg->pRegFirst;
-	while (pRegQueue) {
-		if (pRegQueue == pWb35Reg->pRegLast)
-			pWb35Reg->pRegLast = NULL;
-		pWb35Reg->pRegFirst = pWb35Reg->pRegFirst->Next;
-
-		pUrb = pRegQueue->pUrb;
-		spin_unlock_irq( &pWb35Reg->EP0VM_spin_lock );
-		if (pUrb) {
-			usb_free_urb(pUrb);
-			kfree(pRegQueue);
+	spin_lock_irq( &reg->EP0VM_spin_lock );
+	reg_queue = reg->reg_first;
+	while (reg_queue) {
+		if (reg_queue == reg->reg_last)
+			reg->reg_last = NULL;
+		reg->reg_first = reg->reg_first->Next;
+
+		urb = reg_queue->urb;
+		spin_unlock_irq( &reg->EP0VM_spin_lock );
+		if (urb) {
+			usb_free_urb(urb);
+			kfree(reg_queue);
 		} else {
 			#ifdef _PE_REG_DUMP_
 			WBDEBUG(("EP0 queue release error\n"));
 			#endif
 		}
-		spin_lock_irq( &pWb35Reg->EP0VM_spin_lock );
+		spin_lock_irq( &reg->EP0VM_spin_lock );
 
-		pRegQueue = pWb35Reg->pRegFirst;
+		reg_queue = reg->reg_first;
 	}
-	spin_unlock_irq( &pWb35Reg->EP0VM_spin_lock );
+	spin_unlock_irq( &reg->EP0VM_spin_lock );
 }
 
 //====================================================================================
@@ -542,35 +545,35 @@ Wb35Reg_destroy(phw_data_t pHwData)
 //====================================================================================
 unsigned char Wb35Reg_initial(phw_data_t pHwData)
 {
-	PWB35REG pWb35Reg=&pHwData->Wb35Reg;
+	struct wb35_reg *reg=&pHwData->reg;
 	u32 ltmp;
 	u32 SoftwareSet, VCO_trim, TxVga, Region_ScanInterval;
 
 	// Spin lock is acquired for read and write IRP command
-	spin_lock_init( &pWb35Reg->EP0VM_spin_lock );
+	spin_lock_init( &reg->EP0VM_spin_lock );
 
 	// Getting RF module type from EEPROM ------------------------------------
 	Wb35Reg_WriteSync( pHwData, 0x03b4, 0x080d0000 ); // Start EEPROM access + Read + address(0x0d)
 	Wb35Reg_ReadSync( pHwData, 0x03b4, &ltmp );
 
 	//Update RF module type and determine the PHY type by inf or EEPROM
-	pWb35Reg->EEPROMPhyType = (u8)( ltmp & 0xff );
+	reg->EEPROMPhyType = (u8)( ltmp & 0xff );
 	// 0 V MAX2825, 1 V MAX2827, 2 V MAX2828, 3 V MAX2829
 	// 16V AL2230, 17 - AL7230, 18 - AL2230S
 	// 32 Reserved
 	// 33 - W89RF242(TxVGA 0~19), 34 - W89RF242(TxVGA 0~34)
-	if (pWb35Reg->EEPROMPhyType != RF_DECIDE_BY_INF) {
-		if( (pWb35Reg->EEPROMPhyType == RF_MAXIM_2825)	||
-			(pWb35Reg->EEPROMPhyType == RF_MAXIM_2827)	||
-			(pWb35Reg->EEPROMPhyType == RF_MAXIM_2828)	||
-			(pWb35Reg->EEPROMPhyType == RF_MAXIM_2829)	||
-			(pWb35Reg->EEPROMPhyType == RF_MAXIM_V1)	||
-			(pWb35Reg->EEPROMPhyType == RF_AIROHA_2230)	||
-			(pWb35Reg->EEPROMPhyType == RF_AIROHA_2230S)    ||
-			(pWb35Reg->EEPROMPhyType == RF_AIROHA_7230)	||
-			(pWb35Reg->EEPROMPhyType == RF_WB_242)		||
-			(pWb35Reg->EEPROMPhyType == RF_WB_242_1))
-			pHwData->phy_type = pWb35Reg->EEPROMPhyType;
+	if (reg->EEPROMPhyType != RF_DECIDE_BY_INF) {
+		if( (reg->EEPROMPhyType == RF_MAXIM_2825)	||
+			(reg->EEPROMPhyType == RF_MAXIM_2827)	||
+			(reg->EEPROMPhyType == RF_MAXIM_2828)	||
+			(reg->EEPROMPhyType == RF_MAXIM_2829)	||
+			(reg->EEPROMPhyType == RF_MAXIM_V1)	||
+			(reg->EEPROMPhyType == RF_AIROHA_2230)	||
+			(reg->EEPROMPhyType == RF_AIROHA_2230S)    ||
+			(reg->EEPROMPhyType == RF_AIROHA_7230)	||
+			(reg->EEPROMPhyType == RF_WB_242)		||
+			(reg->EEPROMPhyType == RF_WB_242_1))
+			pHwData->phy_type = reg->EEPROMPhyType;
 	}
 
 	// Power On procedure running. The relative parameter will be set according to phy_type
@@ -606,9 +609,9 @@ unsigned char Wb35Reg_initial(phw_data_t pHwData)
 	if (pHwData->VCO_trim == 0xff)
 		pHwData->VCO_trim = 0x28;
 
-	pWb35Reg->EEPROMRegion = (u8)(Region_ScanInterval>>8); // 20060720
-	if( pWb35Reg->EEPROMRegion<1 || pWb35Reg->EEPROMRegion>6 )
-		pWb35Reg->EEPROMRegion = REGION_AUTO;
+	reg->EEPROMRegion = (u8)(Region_ScanInterval>>8); // 20060720
+	if( reg->EEPROMRegion<1 || reg->EEPROMRegion>6 )
+		reg->EEPROMRegion = REGION_AUTO;
 
 	//For Get Tx VGA from EEPROM 20060315.5 move here
 	GetTxVgaFromEEPROM( pHwData );
@@ -629,9 +632,9 @@ unsigned char Wb35Reg_initial(phw_data_t pHwData)
 	Dxx_initial(pHwData);
 
 	if (pHwData->SurpriseRemove)
-		return FALSE;
+		return false;
 	else
-		return TRUE; // Initial fail
+		return true; // Initial fail
 }
 
 //===================================================================================
diff --git a/drivers/staging/winbond/linux/wb35reg_f.h b/drivers/staging/winbond/wb35reg_f.h
index 3006cfe99cc..d97215a1eba 100644
--- a/drivers/staging/winbond/linux/wb35reg_f.h
+++ b/drivers/staging/winbond/wb35reg_f.h
@@ -1,3 +1,8 @@
+#ifndef __WINBOND_WB35REG_F_H
+#define __WINBOND_WB35REG_F_H
+
+#include "wbhal_s.h"
+
 //====================================
 // Interface function declare
 //====================================
@@ -42,7 +47,7 @@ unsigned char Wb35Reg_BurstWrite(  phw_data_t pHwData,  u16 RegisterNo,  u32 * p
 
 void Wb35Reg_EP0VM(  phw_data_t pHwData );
 void Wb35Reg_EP0VM_start(  phw_data_t pHwData );
-void Wb35Reg_EP0VM_complete(  PURB pUrb );
+void Wb35Reg_EP0VM_complete(struct urb *urb);
 
 u32 BitReverse( u32 dwData, u32 DataLength);
 
@@ -53,4 +58,4 @@ void Wb35Reg_phy_calibration(  phw_data_t pHwData );
 void Wb35Reg_Update(  phw_data_t pHwData,  u16 RegisterNo,  u32 RegisterValue );
 unsigned char adjust_TXVGA_for_iq_mag(  phw_data_t pHwData );
 
-
+#endif
diff --git a/drivers/staging/winbond/linux/wb35reg_s.h b/drivers/staging/winbond/wb35reg_s.h
index 8b35b93f7f0..32ef4b8a2d2 100644
--- a/drivers/staging/winbond/linux/wb35reg_s.h
+++ b/drivers/staging/winbond/wb35reg_s.h
@@ -1,3 +1,10 @@
+#ifndef __WINBOND_WB35REG_S_H
+#define __WINBOND_WB35REG_S_H
+
+#include <linux/spinlock.h>
+#include <linux/types.h>
+#include <asm/atomic.h>
+
 //=======================================================================================
 /*
 				HAL setting function
@@ -67,30 +74,25 @@
 #define DEFAULT_DTIM_ALERT_TIME			0
 
 
-typedef struct _REG_QUEUE
-{
-    struct  urb *pUrb;
-	void*	pUsbReq;
-	void*	Next;
-	union
-	{
+struct wb35_reg_queue {
+	struct urb 	*urb;
+	void		*pUsbReq;
+	void		*Next;
+	union {
 		u32	VALUE;
-		u32 *	pBuffer;
+		u32	*pBuffer;
 	};
-	u8	RESERVED[4];// space reserved for communication
-
-    u16	INDEX; // For storing the register index
-    u8	RESERVED_VALID;	//Indicate whether the RESERVED space is valid at this command.
-	u8	DIRECT; // 0:In   1:Out
-
-} REG_QUEUE, *PREG_QUEUE;
+	u8		RESERVED[4]; // space reserved for communication
+	u16		INDEX; // For storing the register index
+	u8		RESERVED_VALID;	// Indicate whether the RESERVED space is valid at this command.
+	u8		DIRECT; // 0:In   1:Out
+};
 
 //====================================
 // Internal variable for module
 //====================================
 #define MAX_SQ3_FILTER_SIZE		5
-typedef struct _WB35REG
-{
+struct wb35_reg {
 	//============================
 	// Register Bank backup
 	//============================
@@ -145,9 +147,9 @@ typedef struct _WB35REG
 	//-------------------
 	spinlock_t	EP0VM_spin_lock; // 4B
 	u32	        EP0VM_status;//$$
-	PREG_QUEUE	    pRegFirst;
-	PREG_QUEUE	    pRegLast;
-	OS_ATOMIC       RegFireCount;
+	struct wb35_reg_queue *reg_first;
+	struct wb35_reg_queue *reg_last;
+	atomic_t       RegFireCount;
 
 	// Hardware status
 	u8	EP0vm_state;
@@ -165,6 +167,6 @@ typedef struct _WB35REG
 	u32	SQ3_filter[MAX_SQ3_FILTER_SIZE];
 	u32	SQ3_index;
 
-} WB35REG, *PWB35REG;
-
+};
 
+#endif
diff --git a/drivers/staging/winbond/linux/wb35rx.c b/drivers/staging/winbond/wb35rx.c
index b4b9f5f371d..7f8b6d749a4 100644
--- a/drivers/staging/winbond/linux/wb35rx.c
+++ b/drivers/staging/winbond/wb35rx.c
@@ -8,85 +8,160 @@
 //    Processing the Rx message from down layer
 //
 //============================================================================
+#include <linux/usb.h>
+
+#include "core.h"
 #include "sysdef.h"
+#include "wb35rx_f.h"
+
+static void packet_came(struct ieee80211_hw *hw, char *pRxBufferAddress, int PacketSize)
+{
+	struct wbsoft_priv *priv = hw->priv;
+	struct sk_buff *skb;
+	struct ieee80211_rx_status rx_status = {0};
+
+	if (!priv->enabled)
+		return;
+
+	skb = dev_alloc_skb(PacketSize);
+	if (!skb) {
+		printk("Not enough memory for packet, FIXME\n");
+		return;
+	}
 
+	memcpy(skb_put(skb, PacketSize),
+	       pRxBufferAddress,
+	       PacketSize);
 
-void Wb35Rx_start(phw_data_t pHwData)
+/*
+	rx_status.rate = 10;
+	rx_status.channel = 1;
+	rx_status.freq = 12345;
+	rx_status.phymode = MODE_IEEE80211B;
+*/
+
+	ieee80211_rx_irqsafe(hw, skb, &rx_status);
+}
+
+static void Wb35Rx_adjust(PDESCRIPTOR pRxDes)
 {
-	PWB35RX pWb35Rx = &pHwData->Wb35Rx;
+	u32 *	pRxBufferAddress;
+	u32	DecryptionMethod;
+	u32	i;
+	u16	BufferSize;
 
-	// Allow only one thread to run into the Wb35Rx() function
-	if (OS_ATOMIC_INC(pHwData->Adapter, &pWb35Rx->RxFireCounter) == 1) {
-		pWb35Rx->EP3vm_state = VM_RUNNING;
-		Wb35Rx(pHwData);
-	} else
-		OS_ATOMIC_DEC(pHwData->Adapter, &pWb35Rx->RxFireCounter);
+	DecryptionMethod = pRxDes->R01.R01_decryption_method;
+	pRxBufferAddress = pRxDes->buffer_address[0];
+	BufferSize = pRxDes->buffer_size[0];
+
+	// Adjust the last part of data. Only data left
+	BufferSize -= 4; // For CRC-32
+	if (DecryptionMethod)
+		BufferSize -= 4;
+	if (DecryptionMethod == 3) // For CCMP
+		BufferSize -= 4;
+
+	// Adjust the IV field which after 802.11 header and ICV field.
+	if (DecryptionMethod == 1) // For WEP
+	{
+		for( i=6; i>0; i-- )
+			pRxBufferAddress[i] = pRxBufferAddress[i-1];
+		pRxDes->buffer_address[0] = pRxBufferAddress + 1;
+		BufferSize -= 4; // 4 byte for IV
+	}
+	else if( DecryptionMethod ) // For TKIP and CCMP
+	{
+		for (i=7; i>1; i--)
+			pRxBufferAddress[i] = pRxBufferAddress[i-2];
+		pRxDes->buffer_address[0] = pRxBufferAddress + 2;//Update the descriptor, shift 8 byte
+		BufferSize -= 8; // 8 byte for IV + ICV
+	}
+	pRxDes->buffer_size[0] = BufferSize;
 }
 
-// This function cannot reentrain
-void Wb35Rx(  phw_data_t pHwData )
+static u16 Wb35Rx_indicate(struct ieee80211_hw *hw)
 {
+	struct wbsoft_priv *priv = hw->priv;
+	phw_data_t pHwData = &priv->sHwData;
+	DESCRIPTOR	RxDes;
 	PWB35RX	pWb35Rx = &pHwData->Wb35Rx;
-	u8 *	pRxBufferAddress;
-	PURB	pUrb = (PURB)pWb35Rx->RxUrb;
-	int	retv;
-	u32	RxBufferId;
+	u8 *		pRxBufferAddress;
+	u16		PacketSize;
+	u16		stmp, BufferSize, stmp2 = 0;
+	u32		RxBufferId;
 
-	//
-	// Issuing URB
-	//
-	if (pHwData->SurpriseRemove || pHwData->HwStop)
-		goto error;
+	// Only one thread be allowed to run into the following
+	do {
+		RxBufferId = pWb35Rx->RxProcessIndex;
+		if (pWb35Rx->RxOwner[ RxBufferId ]) //Owner by VM
+			break;
 
-	if (pWb35Rx->rx_halt)
-		goto error;
+		pWb35Rx->RxProcessIndex++;
+		pWb35Rx->RxProcessIndex %= MAX_USB_RX_BUFFER_NUMBER;
 
-	// Get RxBuffer's ID
-	RxBufferId = pWb35Rx->RxBufferId;
-	if (!pWb35Rx->RxOwner[RxBufferId]) {
-		// It's impossible to run here.
-		#ifdef _PE_RX_DUMP_
-		WBDEBUG(("Rx driver fifo unavailable\n"));
-		#endif
-		goto error;
-	}
+		pRxBufferAddress = pWb35Rx->pDRx;
+		BufferSize = pWb35Rx->RxBufferSize[ RxBufferId ];
 
-	// Update buffer point, then start to bulkin the data from USB
-	pWb35Rx->RxBufferId++;
-	pWb35Rx->RxBufferId %= MAX_USB_RX_BUFFER_NUMBER;
+		// Parse the bulkin buffer
+		while (BufferSize >= 4) {
+			if ((cpu_to_le32(*(u32 *)pRxBufferAddress) & 0x0fffffff) == RX_END_TAG) //Is ending? 921002.9.a
+				break;
 
-	pWb35Rx->CurrentRxBufferId = RxBufferId;
+			// Get the R00 R01 first
+			RxDes.R00.value = le32_to_cpu(*(u32 *)pRxBufferAddress);
+			PacketSize = (u16)RxDes.R00.R00_receive_byte_count;
+			RxDes.R01.value = le32_to_cpu(*((u32 *)(pRxBufferAddress+4)));
+			// For new DMA 4k
+			if ((PacketSize & 0x03) > 0)
+				PacketSize -= 4;
 
-	if (1 != OS_MEMORY_ALLOC((void* *)&pWb35Rx->pDRx, MAX_USB_RX_BUFFER)) {
-		printk("w35und: Rx memory alloc failed\n");
-		goto error;
-	}
-	pRxBufferAddress = pWb35Rx->pDRx;
+			// Basic check for Rx length. Is length valid?
+			if (PacketSize > MAX_PACKET_SIZE) {
+				#ifdef _PE_RX_DUMP_
+				WBDEBUG(("Serious ERROR : Rx data size too long, size =%d\n", PacketSize));
+				#endif
 
-	usb_fill_bulk_urb(pUrb, pHwData->WbUsb.udev,
-			  usb_rcvbulkpipe(pHwData->WbUsb.udev, 3),
-			  pRxBufferAddress, MAX_USB_RX_BUFFER,
-			  Wb35Rx_Complete, pHwData);
+				pWb35Rx->EP3vm_state = VM_STOP;
+				pWb35Rx->Ep3ErrorCount2++;
+				break;
+			}
 
-	pWb35Rx->EP3vm_state = VM_RUNNING;
+			// Start to process Rx buffer
+//			RxDes.Descriptor_ID = RxBufferId; // Due to synchronous indicate, the field doesn't necessary to use.
+			BufferSize -= 8; //subtract 8 byte for 35's USB header length
+			pRxBufferAddress += 8;
 
-	retv = wb_usb_submit_urb(pUrb);
+			RxDes.buffer_address[0] = pRxBufferAddress;
+			RxDes.buffer_size[0] = PacketSize;
+			RxDes.buffer_number = 1;
+			RxDes.buffer_start_index = 0;
+			RxDes.buffer_total_size = RxDes.buffer_size[0];
+			Wb35Rx_adjust(&RxDes);
 
-	if (retv != 0) {
-		printk("Rx URB sending error\n");
-		goto error;
-	}
-	return;
+			packet_came(hw, pRxBufferAddress, PacketSize);
 
-error:
-	// VM stop
-	pWb35Rx->EP3vm_state = VM_STOP;
-	OS_ATOMIC_DEC( pHwData->Adapter, &pWb35Rx->RxFireCounter );
+			// Move RxBuffer point to the next
+			stmp = PacketSize + 3;
+			stmp &= ~0x03; // 4n alignment
+			pRxBufferAddress += stmp;
+			BufferSize -= stmp;
+			stmp2 += stmp;
+		}
+
+		// Reclaim resource
+		pWb35Rx->RxOwner[ RxBufferId ] = 1;
+	} while (true);
+
+	return stmp2;
 }
 
-void Wb35Rx_Complete(PURB pUrb)
+static void Wb35Rx(struct ieee80211_hw *hw);
+
+static void Wb35Rx_Complete(struct urb *urb)
 {
-	phw_data_t	pHwData = pUrb->context;
+	struct ieee80211_hw *hw = urb->context;
+	struct wbsoft_priv *priv = hw->priv;
+	phw_data_t pHwData = &priv->sHwData;
 	PWB35RX		pWb35Rx = &pHwData->Wb35Rx;
 	u8 *		pRxBufferAddress;
 	u32		SizeCheck;
@@ -96,12 +171,12 @@ void Wb35Rx_Complete(PURB pUrb)
 
 	// Variable setting
 	pWb35Rx->EP3vm_state = VM_COMPLETED;
-	pWb35Rx->EP3VM_status = pUrb->status;//Store the last result of Irp
+	pWb35Rx->EP3VM_status = urb->status;//Store the last result of Irp
 
 	RxBufferId = pWb35Rx->CurrentRxBufferId;
 
 	pRxBufferAddress = pWb35Rx->pDRx;
-	BulkLength = (u16)pUrb->actual_length;
+	BulkLength = (u16)urb->actual_length;
 
 	// The IRP is completed
 	pWb35Rx->EP3vm_state = VM_COMPLETED;
@@ -147,62 +222,99 @@ void Wb35Rx_Complete(PURB pUrb)
 	pWb35Rx->RxBufferSize[ RxBufferId ] = BulkLength;
 
 	if (!pWb35Rx->RxOwner[ RxBufferId ])
-		Wb35Rx_indicate(pHwData);
+		Wb35Rx_indicate(hw);
 
 	kfree(pWb35Rx->pDRx);
 	// Do the next receive
-	Wb35Rx(pHwData);
+	Wb35Rx(hw);
 	return;
 
 error:
 	pWb35Rx->RxOwner[ RxBufferId ] = 1; // Set the owner to hardware
-	OS_ATOMIC_DEC( pHwData->Adapter, &pWb35Rx->RxFireCounter );
+	atomic_dec(&pWb35Rx->RxFireCounter);
 	pWb35Rx->EP3vm_state = VM_STOP;
 }
 
-//=====================================================================================
-unsigned char Wb35Rx_initial(phw_data_t pHwData)
+// This function cannot reentrain
+static void Wb35Rx(struct ieee80211_hw *hw)
 {
-	PWB35RX pWb35Rx = &pHwData->Wb35Rx;
-
-	// Initial the Buffer Queue
-	Wb35Rx_reset_descriptor( pHwData );
+	struct wbsoft_priv *priv = hw->priv;
+	phw_data_t pHwData = &priv->sHwData;
+	PWB35RX	pWb35Rx = &pHwData->Wb35Rx;
+	u8 *	pRxBufferAddress;
+	struct urb *urb = pWb35Rx->RxUrb;
+	int	retv;
+	u32	RxBufferId;
 
-	pWb35Rx->RxUrb = wb_usb_alloc_urb(0);
-	return (!!pWb35Rx->RxUrb);
-}
+	//
+	// Issuing URB
+	//
+	if (pHwData->SurpriseRemove || pHwData->HwStop)
+		goto error;
 
-void Wb35Rx_stop(phw_data_t pHwData)
-{
-	PWB35RX pWb35Rx = &pHwData->Wb35Rx;
+	if (pWb35Rx->rx_halt)
+		goto error;
 
-	// Canceling the Irp if already sends it out.
-	if (pWb35Rx->EP3vm_state == VM_RUNNING) {
-		usb_unlink_urb( pWb35Rx->RxUrb ); // Only use unlink, let Wb35Rx_destroy to free them
+	// Get RxBuffer's ID
+	RxBufferId = pWb35Rx->RxBufferId;
+	if (!pWb35Rx->RxOwner[RxBufferId]) {
+		// It's impossible to run here.
 		#ifdef _PE_RX_DUMP_
-		WBDEBUG(("EP3 Rx stop\n"));
+		WBDEBUG(("Rx driver fifo unavailable\n"));
 		#endif
+		goto error;
+	}
+
+	// Update buffer point, then start to bulkin the data from USB
+	pWb35Rx->RxBufferId++;
+	pWb35Rx->RxBufferId %= MAX_USB_RX_BUFFER_NUMBER;
+
+	pWb35Rx->CurrentRxBufferId = RxBufferId;
+
+	pWb35Rx->pDRx = kzalloc(MAX_USB_RX_BUFFER, GFP_ATOMIC);
+	if (!pWb35Rx->pDRx) {
+		printk("w35und: Rx memory alloc failed\n");
+		goto error;
+	}
+	pRxBufferAddress = pWb35Rx->pDRx;
+
+	usb_fill_bulk_urb(urb, pHwData->WbUsb.udev,
+			  usb_rcvbulkpipe(pHwData->WbUsb.udev, 3),
+			  pRxBufferAddress, MAX_USB_RX_BUFFER,
+			  Wb35Rx_Complete, hw);
+
+	pWb35Rx->EP3vm_state = VM_RUNNING;
+
+	retv = usb_submit_urb(urb, GFP_ATOMIC);
+
+	if (retv != 0) {
+		printk("Rx URB sending error\n");
+		goto error;
 	}
+	return;
+
+error:
+	// VM stop
+	pWb35Rx->EP3vm_state = VM_STOP;
+	atomic_dec(&pWb35Rx->RxFireCounter);
 }
 
-// Needs process context
-void Wb35Rx_destroy(phw_data_t pHwData)
+void Wb35Rx_start(struct ieee80211_hw *hw)
 {
+	struct wbsoft_priv *priv = hw->priv;
+	phw_data_t pHwData = &priv->sHwData;
 	PWB35RX pWb35Rx = &pHwData->Wb35Rx;
 
-	do {
-		OS_SLEEP(10000); // Delay for waiting function enter 940623.1.a
-	} while (pWb35Rx->EP3vm_state != VM_STOP);
-	OS_SLEEP(10000); // Delay for waiting function exit 940623.1.b
-
-	if (pWb35Rx->RxUrb)
-		usb_free_urb( pWb35Rx->RxUrb );
-	#ifdef _PE_RX_DUMP_
-	WBDEBUG(("Wb35Rx_destroy OK\n"));
-	#endif
+	// Allow only one thread to run into the Wb35Rx() function
+	if (atomic_inc_return(&pWb35Rx->RxFireCounter) == 1) {
+		pWb35Rx->EP3vm_state = VM_RUNNING;
+		Wb35Rx(hw);
+	} else
+		atomic_dec(&pWb35Rx->RxFireCounter);
 }
 
-void Wb35Rx_reset_descriptor(  phw_data_t pHwData )
+//=====================================================================================
+static void Wb35Rx_reset_descriptor(  phw_data_t pHwData )
 {
 	PWB35RX pWb35Rx = &pHwData->Wb35Rx;
 	u32	i;
@@ -218,117 +330,44 @@ void Wb35Rx_reset_descriptor(  phw_data_t pHwData )
 		pWb35Rx->RxOwner[i] = 1;
 }
 
-void Wb35Rx_adjust(PDESCRIPTOR pRxDes)
+unsigned char Wb35Rx_initial(phw_data_t pHwData)
 {
-	u32 *	pRxBufferAddress;
-	u32	DecryptionMethod;
-	u32	i;
-	u16	BufferSize;
-
-	DecryptionMethod = pRxDes->R01.R01_decryption_method;
-	pRxBufferAddress = pRxDes->buffer_address[0];
-	BufferSize = pRxDes->buffer_size[0];
+	PWB35RX pWb35Rx = &pHwData->Wb35Rx;
 
-	// Adjust the last part of data. Only data left
-	BufferSize -= 4; // For CRC-32
-	if (DecryptionMethod)
-		BufferSize -= 4;
-	if (DecryptionMethod == 3) // For CCMP
-		BufferSize -= 4;
+	// Initial the Buffer Queue
+	Wb35Rx_reset_descriptor( pHwData );
 
-	// Adjust the IV field which after 802.11 header and ICV field.
-	if (DecryptionMethod == 1) // For WEP
-	{
-		for( i=6; i>0; i-- )
-			pRxBufferAddress[i] = pRxBufferAddress[i-1];
-		pRxDes->buffer_address[0] = pRxBufferAddress + 1;
-		BufferSize -= 4; // 4 byte for IV
-	}
-	else if( DecryptionMethod ) // For TKIP and CCMP
-	{
-		for (i=7; i>1; i--)
-			pRxBufferAddress[i] = pRxBufferAddress[i-2];
-		pRxDes->buffer_address[0] = pRxBufferAddress + 2;//Update the descriptor, shift 8 byte
-		BufferSize -= 8; // 8 byte for IV + ICV
-	}
-	pRxDes->buffer_size[0] = BufferSize;
+	pWb35Rx->RxUrb = usb_alloc_urb(0, GFP_ATOMIC);
+	return (!!pWb35Rx->RxUrb);
 }
 
-extern void packet_came(char *pRxBufferAddress, int PacketSize);
+void Wb35Rx_stop(phw_data_t pHwData)
+{
+	PWB35RX pWb35Rx = &pHwData->Wb35Rx;
 
+	// Canceling the Irp if already sends it out.
+	if (pWb35Rx->EP3vm_state == VM_RUNNING) {
+		usb_unlink_urb( pWb35Rx->RxUrb ); // Only use unlink, let Wb35Rx_destroy to free them
+		#ifdef _PE_RX_DUMP_
+		WBDEBUG(("EP3 Rx stop\n"));
+		#endif
+	}
+}
 
-u16 Wb35Rx_indicate(phw_data_t pHwData)
+// Needs process context
+void Wb35Rx_destroy(phw_data_t pHwData)
 {
-	DESCRIPTOR	RxDes;
-	PWB35RX	pWb35Rx = &pHwData->Wb35Rx;
-	u8 *		pRxBufferAddress;
-	u16		PacketSize;
-	u16		stmp, BufferSize, stmp2 = 0;
-	u32		RxBufferId;
+	PWB35RX pWb35Rx = &pHwData->Wb35Rx;
 
-	// Only one thread be allowed to run into the following
 	do {
-		RxBufferId = pWb35Rx->RxProcessIndex;
-		if (pWb35Rx->RxOwner[ RxBufferId ]) //Owner by VM
-			break;
-
-		pWb35Rx->RxProcessIndex++;
-		pWb35Rx->RxProcessIndex %= MAX_USB_RX_BUFFER_NUMBER;
-
-		pRxBufferAddress = pWb35Rx->pDRx;
-		BufferSize = pWb35Rx->RxBufferSize[ RxBufferId ];
-
-		// Parse the bulkin buffer
-		while (BufferSize >= 4) {
-			if ((cpu_to_le32(*(u32 *)pRxBufferAddress) & 0x0fffffff) == RX_END_TAG) //Is ending? 921002.9.a
-				break;
-
-			// Get the R00 R01 first
-			RxDes.R00.value = le32_to_cpu(*(u32 *)pRxBufferAddress);
-			PacketSize = (u16)RxDes.R00.R00_receive_byte_count;
-			RxDes.R01.value = le32_to_cpu(*((u32 *)(pRxBufferAddress+4)));
-			// For new DMA 4k
-			if ((PacketSize & 0x03) > 0)
-				PacketSize -= 4;
-
-			// Basic check for Rx length. Is length valid?
-			if (PacketSize > MAX_PACKET_SIZE) {
-				#ifdef _PE_RX_DUMP_
-				WBDEBUG(("Serious ERROR : Rx data size too long, size =%d\n", PacketSize));
-				#endif
-
-				pWb35Rx->EP3vm_state = VM_STOP;
-				pWb35Rx->Ep3ErrorCount2++;
-				break;
-			}
-
-			// Start to process Rx buffer
-//			RxDes.Descriptor_ID = RxBufferId; // Due to synchronous indicate, the field doesn't necessary to use.
-			BufferSize -= 8; //subtract 8 byte for 35's USB header length
-			pRxBufferAddress += 8;
-
-			RxDes.buffer_address[0] = pRxBufferAddress;
-			RxDes.buffer_size[0] = PacketSize;
-			RxDes.buffer_number = 1;
-			RxDes.buffer_start_index = 0;
-			RxDes.buffer_total_size = RxDes.buffer_size[0];
-			Wb35Rx_adjust(&RxDes);
-
-			packet_came(pRxBufferAddress, PacketSize);
-
-			// Move RxBuffer point to the next
-			stmp = PacketSize + 3;
-			stmp &= ~0x03; // 4n alignment
-			pRxBufferAddress += stmp;
-			BufferSize -= stmp;
-			stmp2 += stmp;
-		}
-
-		// Reclaim resource
-		pWb35Rx->RxOwner[ RxBufferId ] = 1;
-	} while(TRUE);
+		msleep(10); // Delay for waiting function enter 940623.1.a
+	} while (pWb35Rx->EP3vm_state != VM_STOP);
+	msleep(10); // Delay for waiting function exit 940623.1.b
 
-	return stmp2;
+	if (pWb35Rx->RxUrb)
+		usb_free_urb( pWb35Rx->RxUrb );
+	#ifdef _PE_RX_DUMP_
+	WBDEBUG(("Wb35Rx_destroy OK\n"));
+	#endif
 }
 
-
diff --git a/drivers/staging/winbond/wb35rx_f.h b/drivers/staging/winbond/wb35rx_f.h
new file mode 100644
index 00000000000..d993041e0cd
--- /dev/null
+++ b/drivers/staging/winbond/wb35rx_f.h
@@ -0,0 +1,15 @@
+#ifndef __WINBOND_WB35RX_F_H
+#define __WINBOND_WB35RX_F_H
+
+#include <net/mac80211.h>
+#include "wbhal_s.h"
+
+//====================================
+// Interface function declare
+//====================================
+unsigned char		Wb35Rx_initial(  phw_data_t pHwData );
+void		Wb35Rx_destroy(  phw_data_t pHwData );
+void		Wb35Rx_stop(  phw_data_t pHwData );
+void		Wb35Rx_start(struct ieee80211_hw *hw);
+
+#endif
diff --git a/drivers/staging/winbond/linux/wb35rx_s.h b/drivers/staging/winbond/wb35rx_s.h
index b90c269e6ad..f18350b41c4 100644
--- a/drivers/staging/winbond/linux/wb35rx_s.h
+++ b/drivers/staging/winbond/wb35rx_s.h
@@ -18,7 +18,7 @@
 typedef struct _WB35RX
 {
 	u32			ByteReceived;// For calculating throughput of BulkIn
-	OS_ATOMIC		RxFireCounter;// Does Wb35Rx module fire?
+	atomic_t		RxFireCounter;// Does Wb35Rx module fire?
 
 	u8	RxBuffer[ MAX_USB_RX_BUFFER_NUMBER ][ ((MAX_USB_RX_BUFFER+3) & ~0x03 ) ];
 	u16	RxBufferSize[ ((MAX_USB_RX_BUFFER_NUMBER+1) & ~0x01) ];
diff --git a/drivers/staging/winbond/linux/wb35tx.c b/drivers/staging/winbond/wb35tx.c
index ba9d51244e2..b9b4456c808 100644
--- a/drivers/staging/winbond/linux/wb35tx.c
+++ b/drivers/staging/winbond/wb35tx.c
@@ -8,8 +8,11 @@
 //    Processing the Tx message and put into down layer
 //
 //============================================================================
-#include "sysdef.h"
+#include <linux/usb.h>
 
+#include "wb35tx_f.h"
+#include "mds_f.h"
+#include "sysdef.h"
 
 unsigned char
 Wb35Tx_get_tx_buffer(phw_data_t pHwData, u8 **pBuffer)
@@ -17,28 +20,54 @@ Wb35Tx_get_tx_buffer(phw_data_t pHwData, u8 **pBuffer)
 	PWB35TX pWb35Tx = &pHwData->Wb35Tx;
 
 	*pBuffer = pWb35Tx->TxBuffer[0];
-	return TRUE;
+	return true;
 }
 
-void Wb35Tx_start(phw_data_t pHwData)
+static void Wb35Tx(struct wbsoft_priv *adapter);
+
+static void Wb35Tx_complete(struct urb * pUrb)
 {
-	PWB35TX pWb35Tx = &pHwData->Wb35Tx;
+	struct wbsoft_priv *adapter = pUrb->context;
+	phw_data_t	pHwData = &adapter->sHwData;
+	PWB35TX		pWb35Tx = &pHwData->Wb35Tx;
+	PMDS		pMds = &adapter->Mds;
 
-	// Allow only one thread to run into function
-	if (OS_ATOMIC_INC(pHwData->Adapter, &pWb35Tx->TxFireCounter) == 1) {
-		pWb35Tx->EP4vm_state = VM_RUNNING;
-		Wb35Tx(pHwData);
-	} else
-		OS_ATOMIC_DEC( pHwData->Adapter, &pWb35Tx->TxFireCounter );
-}
+	printk("wb35: tx complete\n");
+	// Variable setting
+	pWb35Tx->EP4vm_state = VM_COMPLETED;
+	pWb35Tx->EP4VM_status = pUrb->status; //Store the last result of Irp
+	pMds->TxOwner[ pWb35Tx->TxSendIndex ] = 0;// Set the owner. Free the owner bit always.
+	pWb35Tx->TxSendIndex++;
+	pWb35Tx->TxSendIndex %= MAX_USB_TX_BUFFER_NUMBER;
 
+	if (pHwData->SurpriseRemove || pHwData->HwStop) // Let WbWlanHalt to handle surprise remove
+		goto error;
 
-void Wb35Tx(phw_data_t pHwData)
+	if (pWb35Tx->tx_halt)
+		goto error;
+
+	// The URB is completed, check the result
+	if (pWb35Tx->EP4VM_status != 0) {
+		printk("URB submission failed\n");
+		pWb35Tx->EP4vm_state = VM_STOP;
+		goto error;
+	}
+
+	Mds_Tx(adapter);
+	Wb35Tx(adapter);
+	return;
+
+error:
+	atomic_dec(&pWb35Tx->TxFireCounter);
+	pWb35Tx->EP4vm_state = VM_STOP;
+}
+
+static void Wb35Tx(struct wbsoft_priv *adapter)
 {
+	phw_data_t	pHwData = &adapter->sHwData;
 	PWB35TX		pWb35Tx = &pHwData->Wb35Tx;
-	PADAPTER	Adapter = pHwData->Adapter;
 	u8		*pTxBufferAddress;
-	PMDS		pMds = &Adapter->Mds;
+	PMDS		pMds = &adapter->Mds;
 	struct urb *	pUrb = (struct urb *)pWb35Tx->Tx4Urb;
 	int         	retv;
 	u32		SendIndex;
@@ -62,10 +91,10 @@ void Wb35Tx(phw_data_t pHwData)
 	usb_fill_bulk_urb(pUrb, pHwData->WbUsb.udev,
 			  usb_sndbulkpipe(pHwData->WbUsb.udev, 4),
 			  pTxBufferAddress, pMds->TxBufferSize[ SendIndex ],
-			  Wb35Tx_complete, pHwData);
+			  Wb35Tx_complete, adapter);
 
 	pWb35Tx->EP4vm_state = VM_RUNNING;
-	retv = wb_usb_submit_urb( pUrb );
+	retv = usb_submit_urb(pUrb, GFP_ATOMIC);
 	if (retv<0) {
 		printk("EP4 Tx Irp sending error\n");
 		goto cleanup;
@@ -74,78 +103,45 @@ void Wb35Tx(phw_data_t pHwData)
 	// Check if driver needs issue Irp for EP2
 	pWb35Tx->TxFillCount += pMds->TxCountInBuffer[SendIndex];
 	if (pWb35Tx->TxFillCount > 12)
-		Wb35Tx_EP2VM_start( pHwData );
+		Wb35Tx_EP2VM_start(adapter);
 
 	pWb35Tx->ByteTransfer += pMds->TxBufferSize[SendIndex];
 	return;
 
  cleanup:
 	pWb35Tx->EP4vm_state = VM_STOP;
-	OS_ATOMIC_DEC( pHwData->Adapter, &pWb35Tx->TxFireCounter );
-}
-
-
-void Wb35Tx_complete(struct urb * pUrb)
-{
-	phw_data_t	pHwData = pUrb->context;
-	PADAPTER	Adapter = (PADAPTER)pHwData->Adapter;
-	PWB35TX		pWb35Tx = &pHwData->Wb35Tx;
-	PMDS		pMds = &Adapter->Mds;
-
-	printk("wb35: tx complete\n");
-	// Variable setting
-	pWb35Tx->EP4vm_state = VM_COMPLETED;
-	pWb35Tx->EP4VM_status = pUrb->status; //Store the last result of Irp
-	pMds->TxOwner[ pWb35Tx->TxSendIndex ] = 0;// Set the owner. Free the owner bit always.
-	pWb35Tx->TxSendIndex++;
-	pWb35Tx->TxSendIndex %= MAX_USB_TX_BUFFER_NUMBER;
-
-	if (pHwData->SurpriseRemove || pHwData->HwStop) // Let WbWlanHalt to handle surprise remove
-		goto error;
-
-	if (pWb35Tx->tx_halt)
-		goto error;
-
-	// The URB is completed, check the result
-	if (pWb35Tx->EP4VM_status != 0) {
-		printk("URB submission failed\n");
-		pWb35Tx->EP4vm_state = VM_STOP;
-		goto error;
-	}
-
-	Mds_Tx(Adapter);
-	Wb35Tx(pHwData);
-	return;
-
-error:
-	OS_ATOMIC_DEC( pHwData->Adapter, &pWb35Tx->TxFireCounter );
-	pWb35Tx->EP4vm_state = VM_STOP;
+	atomic_dec(&pWb35Tx->TxFireCounter);
 }
 
-void Wb35Tx_reset_descriptor(  phw_data_t pHwData )
+void Wb35Tx_start(struct wbsoft_priv *adapter)
 {
+	phw_data_t pHwData = &adapter->sHwData;
 	PWB35TX pWb35Tx = &pHwData->Wb35Tx;
 
-	pWb35Tx->TxSendIndex = 0;
-	pWb35Tx->tx_halt = 0;
+	// Allow only one thread to run into function
+	if (atomic_inc_return(&pWb35Tx->TxFireCounter) == 1) {
+		pWb35Tx->EP4vm_state = VM_RUNNING;
+		Wb35Tx(adapter);
+	} else
+		atomic_dec(&pWb35Tx->TxFireCounter);
 }
 
 unsigned char Wb35Tx_initial(phw_data_t pHwData)
 {
 	PWB35TX pWb35Tx = &pHwData->Wb35Tx;
 
-	pWb35Tx->Tx4Urb = wb_usb_alloc_urb(0);
+	pWb35Tx->Tx4Urb = usb_alloc_urb(0, GFP_ATOMIC);
 	if (!pWb35Tx->Tx4Urb)
-		return FALSE;
+		return false;
 
-	pWb35Tx->Tx2Urb = wb_usb_alloc_urb(0);
+	pWb35Tx->Tx2Urb = usb_alloc_urb(0, GFP_ATOMIC);
 	if (!pWb35Tx->Tx2Urb)
 	{
 		usb_free_urb( pWb35Tx->Tx4Urb );
-		return FALSE;
+		return false;
 	}
 
-	return TRUE;
+	return true;
 }
 
 //======================================================
@@ -175,9 +171,9 @@ void Wb35Tx_destroy(phw_data_t pHwData)
 
 	// Wait for VM stop
 	do {
-		OS_SLEEP(10000);  // Delay for waiting function enter 940623.1.a
+		msleep(10);  // Delay for waiting function enter 940623.1.a
 	} while( (pWb35Tx->EP2vm_state != VM_STOP) && (pWb35Tx->EP4vm_state != VM_STOP) );
-	OS_SLEEP(10000);  // Delay for waiting function enter 940623.1.b
+	msleep(10);  // Delay for waiting function enter 940623.1.b
 
 	if (pWb35Tx->Tx4Urb)
 		usb_free_urb( pWb35Tx->Tx4Urb );
@@ -190,77 +186,30 @@ void Wb35Tx_destroy(phw_data_t pHwData)
 	#endif
 }
 
-void Wb35Tx_CurrentTime(phw_data_t pHwData, u32 TimeCount)
+void Wb35Tx_CurrentTime(struct wbsoft_priv *adapter, u32 TimeCount)
 {
+	phw_data_t pHwData = &adapter->sHwData;
 	PWB35TX pWb35Tx = &pHwData->Wb35Tx;
-	unsigned char Trigger = FALSE;
+	unsigned char Trigger = false;
 
 	if (pWb35Tx->TxTimer > TimeCount)
-		Trigger = TRUE;
+		Trigger = true;
 	else if (TimeCount > (pWb35Tx->TxTimer+500))
-		Trigger = TRUE;
+		Trigger = true;
 
 	if (Trigger) {
 		pWb35Tx->TxTimer = TimeCount;
-		Wb35Tx_EP2VM_start( pHwData );
-	}
-}
-
-void Wb35Tx_EP2VM_start(phw_data_t pHwData)
-{
-	PWB35TX pWb35Tx = &pHwData->Wb35Tx;
-
-	// Allow only one thread to run into function
-	if (OS_ATOMIC_INC( pHwData->Adapter, &pWb35Tx->TxResultCount ) == 1) {
-		pWb35Tx->EP2vm_state = VM_RUNNING;
-		Wb35Tx_EP2VM( pHwData );
+		Wb35Tx_EP2VM_start(adapter);
 	}
-	else
-		OS_ATOMIC_DEC( pHwData->Adapter, &pWb35Tx->TxResultCount );
 }
 
+static void Wb35Tx_EP2VM(struct wbsoft_priv *adapter);
 
-void Wb35Tx_EP2VM(phw_data_t pHwData)
+static void Wb35Tx_EP2VM_complete(struct urb * pUrb)
 {
-	PWB35TX pWb35Tx = &pHwData->Wb35Tx;
-	struct urb *	pUrb = (struct urb *)pWb35Tx->Tx2Urb;
-	u32 *	pltmp = (u32 *)pWb35Tx->EP2_buf;
-	int		retv;
-
-	if (pHwData->SurpriseRemove || pHwData->HwStop)
-		goto error;
-
-	if (pWb35Tx->tx_halt)
-		goto error;
-
-	//
-	// Issuing URB
-	//
-	usb_fill_int_urb( pUrb, pHwData->WbUsb.udev, usb_rcvintpipe(pHwData->WbUsb.udev,2),
-			  pltmp, MAX_INTERRUPT_LENGTH, Wb35Tx_EP2VM_complete, pHwData, 32);
-
-	pWb35Tx->EP2vm_state = VM_RUNNING;
-	retv = wb_usb_submit_urb( pUrb );
-
-	if (retv < 0) {
-		#ifdef _PE_TX_DUMP_
-		WBDEBUG(("EP2 Tx Irp sending error\n"));
-		#endif
-		goto error;
-	}
-
-	return;
-error:
-	pWb35Tx->EP2vm_state = VM_STOP;
-	OS_ATOMIC_DEC( pHwData->Adapter, &pWb35Tx->TxResultCount );
-}
-
-
-void Wb35Tx_EP2VM_complete(struct urb * pUrb)
-{
-	phw_data_t	pHwData = pUrb->context;
+	struct wbsoft_priv *adapter = pUrb->context;
+	phw_data_t	pHwData = &adapter->sHwData;
 	T02_DESCRIPTOR	T02, TSTATUS;
-	PADAPTER	Adapter = (PADAPTER)pHwData->Adapter;
 	PWB35TX		pWb35Tx = &pHwData->Wb35Tx;
 	u32 *		pltmp = (u32 *)pWb35Tx->EP2_buf;
 	u32		i;
@@ -295,13 +244,62 @@ void Wb35Tx_EP2VM_complete(struct urb * pUrb)
 		T02.value |= ((cpu_to_le32(pltmp[i]) & 0xff) << 24);
 
 		TSTATUS.value = T02.value;  //20061009 anson's endian
-		Mds_SendComplete( Adapter, &TSTATUS );
+		Mds_SendComplete( adapter, &TSTATUS );
 		T02.value = cpu_to_le32(pltmp[i]) >> 8;
 	}
 
 	return;
 error:
-	OS_ATOMIC_DEC( pHwData->Adapter, &pWb35Tx->TxResultCount );
+	atomic_dec(&pWb35Tx->TxResultCount);
+	pWb35Tx->EP2vm_state = VM_STOP;
+}
+
+static void Wb35Tx_EP2VM(struct wbsoft_priv *adapter)
+{
+	phw_data_t	pHwData = &adapter->sHwData;
+	PWB35TX pWb35Tx = &pHwData->Wb35Tx;
+	struct urb *	pUrb = (struct urb *)pWb35Tx->Tx2Urb;
+	u32 *	pltmp = (u32 *)pWb35Tx->EP2_buf;
+	int		retv;
+
+	if (pHwData->SurpriseRemove || pHwData->HwStop)
+		goto error;
+
+	if (pWb35Tx->tx_halt)
+		goto error;
+
+	//
+	// Issuing URB
+	//
+	usb_fill_int_urb( pUrb, pHwData->WbUsb.udev, usb_rcvintpipe(pHwData->WbUsb.udev,2),
+			  pltmp, MAX_INTERRUPT_LENGTH, Wb35Tx_EP2VM_complete, adapter, 32);
+
+	pWb35Tx->EP2vm_state = VM_RUNNING;
+	retv = usb_submit_urb(pUrb, GFP_ATOMIC);
+
+	if (retv < 0) {
+		#ifdef _PE_TX_DUMP_
+		WBDEBUG(("EP2 Tx Irp sending error\n"));
+		#endif
+		goto error;
+	}
+
+	return;
+error:
 	pWb35Tx->EP2vm_state = VM_STOP;
+	atomic_dec(&pWb35Tx->TxResultCount);
 }
 
+void Wb35Tx_EP2VM_start(struct wbsoft_priv *adapter)
+{
+	phw_data_t pHwData = &adapter->sHwData;
+	PWB35TX pWb35Tx = &pHwData->Wb35Tx;
+
+	// Allow only one thread to run into function
+	if (atomic_inc_return(&pWb35Tx->TxResultCount) == 1) {
+		pWb35Tx->EP2vm_state = VM_RUNNING;
+		Wb35Tx_EP2VM(adapter);
+	}
+	else
+		atomic_dec(&pWb35Tx->TxResultCount);
+}
diff --git a/drivers/staging/winbond/wb35tx_f.h b/drivers/staging/winbond/wb35tx_f.h
new file mode 100644
index 00000000000..4222fa80c7b
--- /dev/null
+++ b/drivers/staging/winbond/wb35tx_f.h
@@ -0,0 +1,21 @@
+#ifndef __WINBOND_WB35TX_F_H
+#define __WINBOND_WB35TX_F_H
+
+#include "core.h"
+#include "wbhal_f.h"
+
+//====================================
+// Interface function declare
+//====================================
+unsigned char Wb35Tx_initial(	 phw_data_t pHwData );
+void Wb35Tx_destroy(  phw_data_t pHwData );
+unsigned char Wb35Tx_get_tx_buffer(  phw_data_t pHwData,  u8 **pBuffer );
+
+void Wb35Tx_EP2VM_start(struct wbsoft_priv *adapter);
+
+void Wb35Tx_start(struct wbsoft_priv *adapter);
+void Wb35Tx_stop(  phw_data_t pHwData );
+
+void Wb35Tx_CurrentTime(struct wbsoft_priv *adapter,  u32 TimeCount);
+
+#endif
diff --git a/drivers/staging/winbond/linux/wb35tx_s.h b/drivers/staging/winbond/wb35tx_s.h
index ac432573676..3960276cae6 100644
--- a/drivers/staging/winbond/linux/wb35tx_s.h
+++ b/drivers/staging/winbond/wb35tx_s.h
@@ -1,3 +1,8 @@
+#ifndef __WINBOND_WB35_TX_S_H
+#define __WINBOND_WB35_TX_S_H
+
+#include "mds_s.h"
+
 //====================================
 // IS89C35 Tx related definition
 //====================================
@@ -21,8 +26,8 @@ typedef struct _WB35TX
 	// For Interrupt pipe
 	u8	EP2_buf[MAX_INTERRUPT_LENGTH];
 
-	OS_ATOMIC	TxResultCount;// For thread control of EP2 931130.4.m
-	OS_ATOMIC	TxFireCounter;// For thread control of EP4 931130.4.n
+	atomic_t	TxResultCount;// For thread control of EP2 931130.4.m
+	atomic_t	TxFireCounter;// For thread control of EP4 931130.4.n
 	u32			ByteTransfer;
 
 	u32	    TxSendIndex;// The next index of Mds array to be sent
@@ -41,7 +46,4 @@ typedef struct _WB35TX
 
 } WB35TX, *PWB35TX;
 
-
-
-
-
+#endif
diff --git a/drivers/staging/winbond/wbhal.c b/drivers/staging/winbond/wbhal.c
index 5d68ecec34c..8a9d21cbb0c 100644
--- a/drivers/staging/winbond/wbhal.c
+++ b/drivers/staging/winbond/wbhal.c
@@ -1,11 +1,6 @@
 #include "os_common.h"
-
-void hal_get_ethernet_address( phw_data_t pHwData, u8 *current_address )
-{
-	if( pHwData->SurpriseRemove ) return;
-
-	memcpy( current_address, pHwData->CurrentMacAddress, ETH_LENGTH_OF_ADDRESS );
-}
+#include "wbhal_f.h"
+#include "wblinux_f.h"
 
 void hal_set_ethernet_address( phw_data_t pHwData, u8 *current_address )
 {
@@ -28,423 +23,11 @@ void hal_get_permanent_address( phw_data_t pHwData, u8 *pethernet_address )
 	memcpy( pethernet_address, pHwData->PermanentMacAddress, 6 );
 }
 
-u8 hal_init_hardware(phw_data_t pHwData, PWB32_ADAPTER Adapter)
-{
-	u16 SoftwareSet;
-	pHwData->Adapter = Adapter;
-
-	// Initial the variable
-	pHwData->MaxReceiveLifeTime = DEFAULT_MSDU_LIFE_TIME; // Setting Rx maximum MSDU life time
-	pHwData->FragmentThreshold = DEFAULT_FRAGMENT_THRESHOLD; // Setting default fragment threshold
-
-	if (WbUsb_initial(pHwData)) {
-		pHwData->InitialResource = 1;
-		if( Wb35Reg_initial(pHwData)) {
-			pHwData->InitialResource = 2;
-			if (Wb35Tx_initial(pHwData)) {
-				pHwData->InitialResource = 3;
-				if (Wb35Rx_initial(pHwData)) {
-					pHwData->InitialResource = 4;
-					OS_TIMER_INITIAL( &pHwData->LEDTimer, hal_led_control, pHwData );
-					OS_TIMER_SET( &pHwData->LEDTimer, 1000 ); // 20060623
-
-					//
-					// For restrict to vendor's hardware
-					//
-					SoftwareSet = hal_software_set( pHwData );
-
-					#ifdef Vendor2
-					// Try to make sure the EEPROM contain
-					SoftwareSet >>= 8;
-					if( SoftwareSet != 0x82 )
-						return FALSE;
-					#endif
-
-					Wb35Rx_start( pHwData );
-					Wb35Tx_EP2VM_start( pHwData );
-
-					return TRUE;
-				}
-			}
-		}
-	}
-
-	pHwData->SurpriseRemove = 1;
-	return FALSE;
-}
-
-
-void hal_halt(phw_data_t pHwData, void *ppa_data)
-{
-	switch( pHwData->InitialResource )
-	{
-		case 4:
-		case 3: OS_TIMER_CANCEL( &pHwData->LEDTimer, &cancel );
-			OS_SLEEP(100000); // Wait for Timer DPC exit 940623.2
-			Wb35Rx_destroy( pHwData ); // Release the Rx
-		case 2: Wb35Tx_destroy( pHwData ); // Release the Tx
-		case 1: Wb35Reg_destroy( pHwData ); // Release the Wb35 Regisster resources
-				WbUsb_destroy( pHwData );// Release the WbUsb
-	}
-}
-
-//---------------------------------------------------------------------------------------------------
-void hal_set_rates(phw_data_t pHwData, u8 *pbss_rates,
-		   u8 length, unsigned char basic_rate_set)
-{
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
-	u32		tmp, tmp1;
-	u8		Rate[12]={ 2, 4, 11, 22, 12, 18, 24, 36, 48, 72, 96, 108 };
-	u8		SupportedRate[16];
-	u8		i, j, k, Count1, Count2, Byte;
-
-	if( pHwData->SurpriseRemove ) return;
-
-	if (basic_rate_set) {
-		pWb35Reg->M28_MacControl &= ~0x000fff00;
-		tmp1 = 0x00000100;
-	} else {
-		pWb35Reg->M28_MacControl &= ~0xfff00000;
-		tmp1 = 0x00100000;
-	}
-
-	tmp = 0;
-	for (i=0; i<length; i++) {
-		Byte = pbss_rates[i] & 0x7f;
-		for (j=0; j<12; j++) {
-			if( Byte == Rate[j] )
-				break;
-		}
-
-		if (j < 12)
-			tmp |= (tmp1<<j);
-	}
-
-	pWb35Reg->M28_MacControl |= tmp;
-	Wb35Reg_Write( pHwData, 0x0828, pWb35Reg->M28_MacControl );
-
-	// 930206.2.c M78 setting
-	j = k = Count1 = Count2 = 0;
-	memset( SupportedRate, 0, 16 );
-	tmp = 0x00100000;
-	tmp1 = 0x00000100;
-	for (i=0; i<12; i++) { // Get the supported rate
-		if (tmp & pWb35Reg->M28_MacControl) {
-			SupportedRate[j] = Rate[i];
-
-			if (tmp1 & pWb35Reg->M28_MacControl)
-				SupportedRate[j] |= 0x80;
-
-			if (k)
-				Count2++;
-			else
-				Count1++;
-
-			j++;
-		}
-
-		if (i==4 && k==0) {
-			if( !(pWb35Reg->M28_MacControl & 0x000ff000) ) // if basic rate in 11g domain)
-			{
-				k = 1;
-				j = 8;
-			}
-		}
-
-		tmp <<= 1;
-		tmp1 <<= 1;
-	}
-
-	// Fill data into support rate until buffer full
-	//---20060926 add by anson's endian
-	for (i=0; i<4; i++)
-		*(u32 *)(SupportedRate+(i<<2)) = cpu_to_le32( *(u32 *)(SupportedRate+(i<<2)) );
-	//--- end 20060926 add by anson's endian
-	Wb35Reg_BurstWrite( pHwData,0x087c, (u32 *)SupportedRate, 4, AUTO_INCREMENT );
-	pWb35Reg->M7C_MacControl = ((u32 *)SupportedRate)[0];
-	pWb35Reg->M80_MacControl = ((u32 *)SupportedRate)[1];
-	pWb35Reg->M84_MacControl = ((u32 *)SupportedRate)[2];
-	pWb35Reg->M88_MacControl = ((u32 *)SupportedRate)[3];
-
-	// Fill length
-	tmp = Count1<<28 | Count2<<24;
-	pWb35Reg->M78_ERPInformation &= ~0xff000000;
-	pWb35Reg->M78_ERPInformation |= tmp;
-	Wb35Reg_Write( pHwData, 0x0878, pWb35Reg->M78_ERPInformation );
-}
-
-
-//---------------------------------------------------------------------------------------------------
-void hal_set_beacon_period(  phw_data_t pHwData,  u16 beacon_period )
-{
-	u32	tmp;
-
-	if( pHwData->SurpriseRemove ) return;
-
-	pHwData->BeaconPeriod = beacon_period;
-	tmp = pHwData->BeaconPeriod << 16;
-	tmp |= pHwData->ProbeDelay;
-	Wb35Reg_Write( pHwData, 0x0848, tmp );
-}
-
-
-void hal_set_current_channel_ex(  phw_data_t pHwData,  ChanInfo channel )
-{
-	PWB35REG pWb35Reg = &pHwData->Wb35Reg;
-
-	if( pHwData->SurpriseRemove )
-		return;
-
-	printk("Going to channel: %d/%d\n", channel.band, channel.ChanNo);
-
-	RFSynthesizer_SwitchingChannel( pHwData, channel );// Switch channel
-	pHwData->Channel = channel.ChanNo;
-	pHwData->band = channel.band;
-	#ifdef _PE_STATE_DUMP_
-	WBDEBUG(("Set channel is %d, band =%d\n", pHwData->Channel, pHwData->band));
-	#endif
-	pWb35Reg->M28_MacControl &= ~0xff; // Clean channel information field
-	pWb35Reg->M28_MacControl |= channel.ChanNo;
-	Wb35Reg_WriteWithCallbackValue( pHwData, 0x0828, pWb35Reg->M28_MacControl,
-					(s8 *)&channel, sizeof(ChanInfo));
-}
-//---------------------------------------------------------------------------------------------------
-void hal_set_current_channel(  phw_data_t pHwData,  ChanInfo channel )
-{
-	hal_set_current_channel_ex( pHwData, channel );
-}
-//---------------------------------------------------------------------------------------------------
-void hal_get_current_channel(  phw_data_t pHwData,  ChanInfo *channel )
-{
-	channel->ChanNo = pHwData->Channel;
-	channel->band = pHwData->band;
-}
-//---------------------------------------------------------------------------------------------------
-void hal_set_accept_broadcast(  phw_data_t pHwData,  u8 enable )
-{
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
-
-	if( pHwData->SurpriseRemove ) return;
-
-	pWb35Reg->M00_MacControl &= ~0x02000000;//The HW value
-
-	if (enable)
-		pWb35Reg->M00_MacControl |= 0x02000000;//The HW value
-
-	Wb35Reg_Write( pHwData, 0x0800, pWb35Reg->M00_MacControl );
-}
-
-//for wep key error detection, we need to accept broadcast packets to be received temporary.
-void hal_set_accept_promiscuous( phw_data_t pHwData,  u8 enable)
-{
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
-
-	if (pHwData->SurpriseRemove) return;
-	if (enable) {
-		pWb35Reg->M00_MacControl |= 0x00400000;
-		Wb35Reg_Write( pHwData, 0x0800, pWb35Reg->M00_MacControl );
-	} else {
-		pWb35Reg->M00_MacControl&=~0x00400000;
-		Wb35Reg_Write( pHwData, 0x0800, pWb35Reg->M00_MacControl );
-	}
-}
-
-void hal_set_accept_multicast(  phw_data_t pHwData,  u8 enable )
-{
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
-
-	if( pHwData->SurpriseRemove ) return;
-
-	pWb35Reg->M00_MacControl &= ~0x01000000;//The HW value
-	if (enable)  pWb35Reg->M00_MacControl |= 0x01000000;//The HW value
-	Wb35Reg_Write( pHwData, 0x0800, pWb35Reg->M00_MacControl );
-}
-
-void hal_set_accept_beacon(  phw_data_t pHwData,  u8 enable )
-{
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
-
-	if( pHwData->SurpriseRemove ) return;
-
-	// 20040108 debug
-	if( !enable )//Due to SME and MLME are not suitable for 35
-		return;
-
-	pWb35Reg->M00_MacControl &= ~0x04000000;//The HW value
-	if( enable )
-		pWb35Reg->M00_MacControl |= 0x04000000;//The HW value
-
-	Wb35Reg_Write( pHwData, 0x0800, pWb35Reg->M00_MacControl );
-}
-//---------------------------------------------------------------------------------------------------
-void hal_set_multicast_address( phw_data_t pHwData, u8 *address, u8 number )
-{
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
-	u8		Byte, Bit;
-
-	if( pHwData->SurpriseRemove ) return;
-
-	//Erases and refills the card multicast registers. Used when an address
-	//    has been deleted and all bits must be recomputed.
-	pWb35Reg->M04_MulticastAddress1 = 0;
-	pWb35Reg->M08_MulticastAddress2 = 0;
-
-	while( number )
-	{
-		number--;
-		CardGetMulticastBit( (address+(number*ETH_LENGTH_OF_ADDRESS)), &Byte, &Bit);
-		pWb35Reg->Multicast[Byte] |= Bit;
-	}
-
-	// Updating register
-	Wb35Reg_BurstWrite( pHwData, 0x0804, (u32 *)pWb35Reg->Multicast, 2, AUTO_INCREMENT );
-}
-//---------------------------------------------------------------------------------------------------
-u8 hal_get_accept_beacon(  phw_data_t pHwData )
-{
-	PWB35REG pWb35Reg = &pHwData->Wb35Reg;
-
-	if( pHwData->SurpriseRemove ) return 0;
-
-	if( pWb35Reg->M00_MacControl & 0x04000000 )
-		return 1;
-	else
-		return 0;
-}
-
-unsigned char hal_reset_hardware( phw_data_t pHwData, void* ppa )
-{
-	// Not implement yet
-	return TRUE;
-}
-
-void hal_stop(  phw_data_t pHwData )
-{
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
-
-	pHwData->Wb35Rx.rx_halt = 1;
-	Wb35Rx_stop( pHwData );
-
-	pHwData->Wb35Tx.tx_halt = 1;
-	Wb35Tx_stop( pHwData );
-
-	pWb35Reg->D00_DmaControl &= ~0xc0000000;//Tx Off, Rx Off
-	Wb35Reg_Write( pHwData, 0x0400, pWb35Reg->D00_DmaControl );
-
-	WbUsb_Stop( pHwData ); // 20051230 Add.4
-}
-
-unsigned char hal_idle(phw_data_t pHwData)
-{
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
-	PWBUSB	pWbUsb = &pHwData->WbUsb;
-
-	if( !pHwData->SurpriseRemove && ( pWbUsb->DetectCount || pWb35Reg->EP0vm_state!=VM_STOP ) )
-		return FALSE;
-
-	return TRUE;
-}
-//---------------------------------------------------------------------------------------------------
-void hal_set_cwmin(  phw_data_t pHwData,  u8	cwin_min )
-{
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
-
-	if( pHwData->SurpriseRemove ) return;
-
-	pHwData->cwmin = cwin_min;
-	pWb35Reg->M2C_MacControl &= ~0x7c00;	//bit 10 ~ 14
-	pWb35Reg->M2C_MacControl |= (pHwData->cwmin<<10);
-	Wb35Reg_Write( pHwData, 0x082c, pWb35Reg->M2C_MacControl );
-}
-
-s32 hal_get_rssi(  phw_data_t pHwData,  u32 *HalRssiArry,  u8 Count )
+static void hal_led_control(unsigned long data)
 {
-	PWB35REG pWb35Reg = &pHwData->Wb35Reg;
-	R01_DESCRIPTOR	r01;
-	s32 ltmp = 0, tmp;
-	u8	i;
-
-	if( pHwData->SurpriseRemove ) return -200;
-	if( Count > MAX_ACC_RSSI_COUNT ) // Because the TS may use this funtion
-		Count = MAX_ACC_RSSI_COUNT;
-
-	// RSSI = C1 + C2 * (agc_state[7:0] + offset_map(lna_state[1:0]))
-	// C1 = -195, C2 = 0.66 = 85/128
-	for (i=0; i<Count; i++)
-	{
-		r01.value = HalRssiArry[i];
-		tmp = ((( r01.R01_AGC_state + pWb35Reg->LNAValue[r01.R01_LNA_state]) * 85 ) >>7 ) - 195;
-		ltmp += tmp;
-	}
-	ltmp /= Count;
-	if( pHwData->phy_type == RF_AIROHA_2230 ) ltmp -= 5; // 10;
-	if( pHwData->phy_type == RF_AIROHA_2230S ) ltmp -= 5; // 10; 20060420 Add this
-
-	//if( ltmp < -200 ) ltmp = -200;
-	if( ltmp < -110 ) ltmp = -110;// 1.0.24.0 For NJRC
-
-	return ltmp;
-}
-//----------------------------------------------------------------------------------------------------
-s32 hal_get_rssi_bss(  phw_data_t pHwData,  u16 idx,  u8 Count )
-{
-	PWB35REG pWb35Reg = &pHwData->Wb35Reg;
-	R01_DESCRIPTOR	r01;
-	s32 ltmp = 0, tmp;
-	u8	i, j;
-	PADAPTER	Adapter = pHwData->Adapter;
-//	u32 *HalRssiArry = psBSS(idx)->HalRssi;
-
-	if( pHwData->SurpriseRemove ) return -200;
-	if( Count > MAX_ACC_RSSI_COUNT ) // Because the TS may use this funtion
-		Count = MAX_ACC_RSSI_COUNT;
-
-	// RSSI = C1 + C2 * (agc_state[7:0] + offset_map(lna_state[1:0]))
-	// C1 = -195, C2 = 0.66 = 85/128
-#if 0
-	for (i=0; i<Count; i++)
-	{
-		r01.value = HalRssiArry[i];
-		tmp = ((( r01.R01_AGC_state + pWb35Reg->LNAValue[r01.R01_LNA_state]) * 85 ) >>7 ) - 195;
-		ltmp += tmp;
-	}
-#else
-	if (psBSS(idx)->HalRssiIndex == 0)
-		psBSS(idx)->HalRssiIndex = MAX_ACC_RSSI_COUNT;
-	j = (u8)psBSS(idx)->HalRssiIndex-1;
-
-	for (i=0; i<Count; i++)
-	{
-		r01.value = psBSS(idx)->HalRssi[j];
-		tmp = ((( r01.R01_AGC_state + pWb35Reg->LNAValue[r01.R01_LNA_state]) * 85 ) >>7 ) - 195;
-		ltmp += tmp;
-		if (j == 0)
-		{
-			j = MAX_ACC_RSSI_COUNT;
-		}
-		j--;
-	}
-#endif
-	ltmp /= Count;
-	if( pHwData->phy_type == RF_AIROHA_2230 ) ltmp -= 5; // 10;
-	if( pHwData->phy_type == RF_AIROHA_2230S ) ltmp -= 5; // 10; 20060420 Add this
-
-	//if( ltmp < -200 ) ltmp = -200;
-	if( ltmp < -110 ) ltmp = -110;// 1.0.24.0 For NJRC
-
-	return ltmp;
-}
-
-//---------------------------------------------------------------------------
-void hal_led_control_1a(  phw_data_t pHwData )
-{
-	hal_led_control( NULL, pHwData, NULL, NULL );
-}
-
-void hal_led_control(  void* S1,  phw_data_t pHwData,  void* S3,  void* S4 )
-{
-	PADAPTER	Adapter = pHwData->Adapter;
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
+	struct wbsoft_priv *adapter = (struct wbsoft_priv *) data;
+	phw_data_t pHwData = &adapter->sHwData;
+	struct wb35_reg *reg = &pHwData->reg;
 	u32	LEDSet = (pHwData->SoftwareSet & HAL_LED_SET_MASK) >> HAL_LED_SET_SHIFT;
 	u8	LEDgray[20] = { 0,3,4,6,8,10,11,12,13,14,15,14,13,12,11,10,8,6,4,2 };
 	u8	LEDgray2[30] = { 7,8,9,10,11,12,13,14,15,0,0,0,0,0,0,0,0,0,0,0,0,0,15,14,13,12,11,10,9,8 };
@@ -487,21 +70,21 @@ void hal_led_control(  void* S1,  phw_data_t pHwData,  void* S3,  void* S4 )
 			}
 			pHwData->LED_Blinking++;
 
-			pWb35Reg->U1BC_LEDConfigure = ltmp;
+			reg->U1BC_LEDConfigure = ltmp;
 			if( LEDSet != 7 ) // Only 111 mode has 2 LEDs on PCB.
 			{
-				pWb35Reg->U1BC_LEDConfigure |= (ltmp &0xff)<<8; // Copy LED result to each LED control register
-				pWb35Reg->U1BC_LEDConfigure |= (ltmp &0xff00)>>8;
+				reg->U1BC_LEDConfigure |= (ltmp &0xff)<<8; // Copy LED result to each LED control register
+				reg->U1BC_LEDConfigure |= (ltmp &0xff00)>>8;
 			}
-			Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure );
+			Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure );
 		}
 	}
 	else if( pHwData->CurrentRadioSw || pHwData->CurrentRadioHw ) // If radio off
 	{
-		if( pWb35Reg->U1BC_LEDConfigure & 0x1010 )
+		if( reg->U1BC_LEDConfigure & 0x1010 )
 		{
-			pWb35Reg->U1BC_LEDConfigure &= ~0x1010;
-			Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure );
+			reg->U1BC_LEDConfigure &= ~0x1010;
+			Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure );
 		}
 	}
 	else
@@ -516,15 +99,15 @@ void hal_led_control(  void* S1,  phw_data_t pHwData,  void* S3,  void* S4 )
 					{
 						if( pHwData->LED_Blinking == 0 )
 						{
-							pWb35Reg->U1BC_LEDConfigure |= 0x10;
-							Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure ); // LED_0 On
+							reg->U1BC_LEDConfigure |= 0x10;
+							Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure ); // LED_0 On
 							pHwData->LED_Blinking = 1;
 							TimeInterval = 300;
 						}
 						else
 						{
-							pWb35Reg->U1BC_LEDConfigure &= ~0x10;
-							Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure ); // LED_0 Off
+							reg->U1BC_LEDConfigure &= ~0x10;
+							Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure ); // LED_0 Off
 							pHwData->LED_Blinking = 0;
 							TimeInterval = 300;
 						}
@@ -532,20 +115,20 @@ void hal_led_control(  void* S1,  phw_data_t pHwData,  void* S3,  void* S4 )
 					else
 					{
 						//Turn Off LED_0
-						if( pWb35Reg->U1BC_LEDConfigure & 0x10 )
+						if( reg->U1BC_LEDConfigure & 0x10 )
 						{
-							pWb35Reg->U1BC_LEDConfigure &= ~0x10;
-							Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure ); // LED_0 Off
+							reg->U1BC_LEDConfigure &= ~0x10;
+							Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure ); // LED_0 Off
 						}
 					}
 				}
 				else
 				{
 					// Turn On LED_0
-					if( (pWb35Reg->U1BC_LEDConfigure & 0x10) == 0 )
+					if( (reg->U1BC_LEDConfigure & 0x10) == 0 )
 					{
-						pWb35Reg->U1BC_LEDConfigure |= 0x10;
-						Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure ); // LED_0 Off
+						reg->U1BC_LEDConfigure |= 0x10;
+						Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure ); // LED_0 Off
 					}
 				}
 				break;
@@ -558,16 +141,16 @@ void hal_led_control(  void* S1,  phw_data_t pHwData,  void* S3,  void* S4 )
 					{
 						if( pHwData->LED_Blinking == 0 )
 						{
-							pWb35Reg->U1BC_LEDConfigure &= ~0xf;
-							pWb35Reg->U1BC_LEDConfigure |= 0x10;
-							Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure ); // LED_0 On
+							reg->U1BC_LEDConfigure &= ~0xf;
+							reg->U1BC_LEDConfigure |= 0x10;
+							Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure ); // LED_0 On
 							pHwData->LED_Blinking = 1;
 							TimeInterval = 300;
 						}
 						else
 						{
-							pWb35Reg->U1BC_LEDConfigure &= ~0x1f;
-							Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure ); // LED_0 Off
+							reg->U1BC_LEDConfigure &= ~0x1f;
+							Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure ); // LED_0 Off
 							pHwData->LED_Blinking = 0;
 							TimeInterval = 300;
 						}
@@ -575,26 +158,26 @@ void hal_led_control(  void* S1,  phw_data_t pHwData,  void* S3,  void* S4 )
 					else
 					{
 						// 20060901 Gray blinking if in disconnect state and not scanning
-						ltmp = pWb35Reg->U1BC_LEDConfigure;
-						pWb35Reg->U1BC_LEDConfigure &= ~0x1f;
+						ltmp = reg->U1BC_LEDConfigure;
+						reg->U1BC_LEDConfigure &= ~0x1f;
 						if( LEDgray2[(pHwData->LED_Blinking%30)] )
 						{
-							pWb35Reg->U1BC_LEDConfigure |= 0x10;
-							pWb35Reg->U1BC_LEDConfigure |= LEDgray2[ (pHwData->LED_Blinking%30) ];
+							reg->U1BC_LEDConfigure |= 0x10;
+							reg->U1BC_LEDConfigure |= LEDgray2[ (pHwData->LED_Blinking%30) ];
 						}
 						pHwData->LED_Blinking++;
-						if( pWb35Reg->U1BC_LEDConfigure != ltmp )
-							Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure ); // LED_0 Off
+						if( reg->U1BC_LEDConfigure != ltmp )
+							Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure ); // LED_0 Off
 						TimeInterval = 100;
 					}
 				}
 				else
 				{
 					// Turn On LED_0
-					if( (pWb35Reg->U1BC_LEDConfigure & 0x10) == 0 )
+					if( (reg->U1BC_LEDConfigure & 0x10) == 0 )
 					{
-						pWb35Reg->U1BC_LEDConfigure |= 0x10;
-						Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure ); // LED_0 Off
+						reg->U1BC_LEDConfigure |= 0x10;
+						Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure ); // LED_0 Off
 					}
 				}
 				break;
@@ -607,15 +190,15 @@ void hal_led_control(  void* S1,  phw_data_t pHwData,  void* S3,  void* S4 )
 					{
 						if( pHwData->LED_Blinking == 0 )
 						{
-							pWb35Reg->U1BC_LEDConfigure |= 0x1000;
-							Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure ); // LED_1 On
+							reg->U1BC_LEDConfigure |= 0x1000;
+							Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure ); // LED_1 On
 							pHwData->LED_Blinking = 1;
 							TimeInterval = 300;
 						}
 						else
 						{
-							pWb35Reg->U1BC_LEDConfigure &= ~0x1000;
-							Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure ); // LED_1 Off
+							reg->U1BC_LEDConfigure &= ~0x1000;
+							Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure ); // LED_1 Off
 							pHwData->LED_Blinking = 0;
 							TimeInterval = 300;
 						}
@@ -623,57 +206,57 @@ void hal_led_control(  void* S1,  phw_data_t pHwData,  void* S3,  void* S4 )
 					else
 					{
 						//Turn Off LED_1
-						if( pWb35Reg->U1BC_LEDConfigure & 0x1000 )
+						if( reg->U1BC_LEDConfigure & 0x1000 )
 						{
-							pWb35Reg->U1BC_LEDConfigure &= ~0x1000;
-							Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure ); // LED_1 Off
+							reg->U1BC_LEDConfigure &= ~0x1000;
+							Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure ); // LED_1 Off
 						}
 					}
 				}
 				else
 				{
 					// Is transmitting/receiving ??
-					if( (OS_CURRENT_RX_BYTE( Adapter ) != pHwData->RxByteCountLast ) ||
-						(OS_CURRENT_TX_BYTE( Adapter ) != pHwData->TxByteCountLast ) )
+					if( (adapter->RxByteCount != pHwData->RxByteCountLast ) ||
+						(adapter->TxByteCount != pHwData->TxByteCountLast ) )
 					{
-						if( (pWb35Reg->U1BC_LEDConfigure & 0x3000) != 0x3000 )
+						if( (reg->U1BC_LEDConfigure & 0x3000) != 0x3000 )
 						{
-							pWb35Reg->U1BC_LEDConfigure |= 0x3000;
-							Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure ); // LED_1 On
+							reg->U1BC_LEDConfigure |= 0x3000;
+							Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure ); // LED_1 On
 						}
 
 						// Update variable
-						pHwData->RxByteCountLast = OS_CURRENT_RX_BYTE( Adapter );
-						pHwData->TxByteCountLast = OS_CURRENT_TX_BYTE( Adapter );
+						pHwData->RxByteCountLast = adapter->RxByteCount;
+						pHwData->TxByteCountLast = adapter->TxByteCount;
 						TimeInterval = 200;
 					}
 					else
 					{
 						// Turn On LED_1 and blinking if transmitting/receiving
-						 if( (pWb35Reg->U1BC_LEDConfigure & 0x3000) != 0x1000 )
+						 if( (reg->U1BC_LEDConfigure & 0x3000) != 0x1000 )
 						 {
-							 pWb35Reg->U1BC_LEDConfigure &= ~0x3000;
-							 pWb35Reg->U1BC_LEDConfigure |= 0x1000;
-							 Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure ); // LED_1 On
+							 reg->U1BC_LEDConfigure &= ~0x3000;
+							 reg->U1BC_LEDConfigure |= 0x1000;
+							 Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure ); // LED_1 On
 						 }
 					}
 				}
 				break;
 
 			default: // Default setting. 2 LED be placed on PCB. LED_0: Link On LED_1 Active
-				if( (pWb35Reg->U1BC_LEDConfigure & 0x3000) != 0x3000 )
+				if( (reg->U1BC_LEDConfigure & 0x3000) != 0x3000 )
 				{
-					pWb35Reg->U1BC_LEDConfigure |= 0x3000;// LED_1 is always on and event enable
-					Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure );
+					reg->U1BC_LEDConfigure |= 0x3000;// LED_1 is always on and event enable
+					Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure );
 				}
 
 				if( pHwData->LED_Blinking )
 				{
 					// Gray blinking
-					pWb35Reg->U1BC_LEDConfigure &= ~0x0f;
-					pWb35Reg->U1BC_LEDConfigure |= 0x10;
-					pWb35Reg->U1BC_LEDConfigure |= LEDgray[ (pHwData->LED_Blinking-1)%20 ];
-					Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure );
+					reg->U1BC_LEDConfigure &= ~0x0f;
+					reg->U1BC_LEDConfigure |= 0x10;
+					reg->U1BC_LEDConfigure |= LEDgray[ (pHwData->LED_Blinking-1)%20 ];
+					Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure );
 
 					pHwData->LED_Blinking += 2;
 					if( pHwData->LED_Blinking < 40 )
@@ -681,28 +264,28 @@ void hal_led_control(  void* S1,  phw_data_t pHwData,  void* S3,  void* S4 )
 					else
 					{
 						pHwData->LED_Blinking = 0; // Stop blinking
-						pWb35Reg->U1BC_LEDConfigure &= ~0x0f;
-						Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure );
+						reg->U1BC_LEDConfigure &= ~0x0f;
+						Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure );
 					}
 					break;
 				}
 
 				if( pHwData->LED_LinkOn )
 				{
-					if( !(pWb35Reg->U1BC_LEDConfigure & 0x10) ) // Check the LED_0
+					if( !(reg->U1BC_LEDConfigure & 0x10) ) // Check the LED_0
 					{
 						//Try to turn ON LED_0 after gray blinking
-						pWb35Reg->U1BC_LEDConfigure |= 0x10;
+						reg->U1BC_LEDConfigure |= 0x10;
 						pHwData->LED_Blinking = 1; //Start blinking
 						TimeInterval = 50;
 					}
 				}
 				else
 				{
-					if( pWb35Reg->U1BC_LEDConfigure & 0x10 ) // Check the LED_0
+					if( reg->U1BC_LEDConfigure & 0x10 ) // Check the LED_0
 					{
-						pWb35Reg->U1BC_LEDConfigure &= ~0x10;
-						Wb35Reg_Write( pHwData, 0x03bc, pWb35Reg->U1BC_LEDConfigure );
+						reg->U1BC_LEDConfigure &= ~0x10;
+						Wb35Reg_Write( pHwData, 0x03bc, reg->U1BC_LEDConfigure );
 					}
 				}
 				break;
@@ -720,84 +303,240 @@ void hal_led_control(  void* S1,  phw_data_t pHwData,  void* S3,  void* S4 )
 	}
 
 	pHwData->time_count += TimeInterval;
-	Wb35Tx_CurrentTime( pHwData, pHwData->time_count ); // 20060928 add
-	OS_TIMER_SET( &pHwData->LEDTimer, TimeInterval ); // 20060623.1
+	Wb35Tx_CurrentTime(adapter, pHwData->time_count); // 20060928 add
+	pHwData->LEDTimer.expires = jiffies + msecs_to_jiffies(TimeInterval);
+	add_timer(&pHwData->LEDTimer);
 }
 
+u8 hal_init_hardware(struct ieee80211_hw *hw)
+{
+	struct wbsoft_priv *priv = hw->priv;
+	phw_data_t pHwData = &priv->sHwData;
+	u16 SoftwareSet;
 
-void hal_set_phy_type(  phw_data_t pHwData,  u8 PhyType )
+	// Initial the variable
+	pHwData->MaxReceiveLifeTime = DEFAULT_MSDU_LIFE_TIME; // Setting Rx maximum MSDU life time
+	pHwData->FragmentThreshold = DEFAULT_FRAGMENT_THRESHOLD; // Setting default fragment threshold
+
+	pHwData->InitialResource = 1;
+	if( Wb35Reg_initial(pHwData)) {
+		pHwData->InitialResource = 2;
+		if (Wb35Tx_initial(pHwData)) {
+			pHwData->InitialResource = 3;
+			if (Wb35Rx_initial(pHwData)) {
+				pHwData->InitialResource = 4;
+				init_timer(&pHwData->LEDTimer);
+				pHwData->LEDTimer.function = hal_led_control;
+				pHwData->LEDTimer.data = (unsigned long) priv;
+				pHwData->LEDTimer.expires = jiffies + msecs_to_jiffies(1000);
+				add_timer(&pHwData->LEDTimer);
+
+				//
+				// For restrict to vendor's hardware
+				//
+				SoftwareSet = hal_software_set( pHwData );
+
+				#ifdef Vendor2
+				// Try to make sure the EEPROM contain
+				SoftwareSet >>= 8;
+				if( SoftwareSet != 0x82 )
+					return false;
+				#endif
+
+				Wb35Rx_start(hw);
+				Wb35Tx_EP2VM_start(priv);
+
+				return true;
+			}
+		}
+	}
+
+	pHwData->SurpriseRemove = 1;
+	return false;
+}
+
+
+void hal_halt(phw_data_t pHwData, void *ppa_data)
 {
-	pHwData->phy_type = PhyType;
+	switch( pHwData->InitialResource )
+	{
+		case 4:
+		case 3: del_timer_sync(&pHwData->LEDTimer);
+			msleep(100); // Wait for Timer DPC exit 940623.2
+			Wb35Rx_destroy( pHwData ); // Release the Rx
+		case 2: Wb35Tx_destroy( pHwData ); // Release the Tx
+		case 1: Wb35Reg_destroy( pHwData ); // Release the Wb35 Regisster resources
+	}
 }
 
-void hal_get_phy_type(  phw_data_t pHwData,  u8 *PhyType )
+//---------------------------------------------------------------------------------------------------
+void hal_set_beacon_period(  phw_data_t pHwData,  u16 beacon_period )
 {
-	*PhyType = pHwData->phy_type;
+	u32	tmp;
+
+	if( pHwData->SurpriseRemove ) return;
+
+	pHwData->BeaconPeriod = beacon_period;
+	tmp = pHwData->BeaconPeriod << 16;
+	tmp |= pHwData->ProbeDelay;
+	Wb35Reg_Write( pHwData, 0x0848, tmp );
 }
 
-void hal_reset_counter(  phw_data_t pHwData )
+
+static void hal_set_current_channel_ex(  phw_data_t pHwData,  ChanInfo channel )
+{
+	struct wb35_reg *reg = &pHwData->reg;
+
+	if( pHwData->SurpriseRemove )
+		return;
+
+	printk("Going to channel: %d/%d\n", channel.band, channel.ChanNo);
+
+	RFSynthesizer_SwitchingChannel( pHwData, channel );// Switch channel
+	pHwData->Channel = channel.ChanNo;
+	pHwData->band = channel.band;
+	#ifdef _PE_STATE_DUMP_
+	WBDEBUG(("Set channel is %d, band =%d\n", pHwData->Channel, pHwData->band));
+	#endif
+	reg->M28_MacControl &= ~0xff; // Clean channel information field
+	reg->M28_MacControl |= channel.ChanNo;
+	Wb35Reg_WriteWithCallbackValue( pHwData, 0x0828, reg->M28_MacControl,
+					(s8 *)&channel, sizeof(ChanInfo));
+}
+//---------------------------------------------------------------------------------------------------
+void hal_set_current_channel(  phw_data_t pHwData,  ChanInfo channel )
 {
-	pHwData->dto_tx_retry_count = 0;
-	pHwData->dto_tx_frag_count = 0;
-	memset( pHwData->tx_retry_count, 0, 8);
+	hal_set_current_channel_ex( pHwData, channel );
+}
+//---------------------------------------------------------------------------------------------------
+void hal_set_accept_broadcast(  phw_data_t pHwData,  u8 enable )
+{
+	struct wb35_reg *reg = &pHwData->reg;
+
+	if( pHwData->SurpriseRemove ) return;
+
+	reg->M00_MacControl &= ~0x02000000;//The HW value
+
+	if (enable)
+		reg->M00_MacControl |= 0x02000000;//The HW value
+
+	Wb35Reg_Write( pHwData, 0x0800, reg->M00_MacControl );
+}
+
+//for wep key error detection, we need to accept broadcast packets to be received temporary.
+void hal_set_accept_promiscuous( phw_data_t pHwData,  u8 enable)
+{
+	struct wb35_reg *reg = &pHwData->reg;
+
+	if (pHwData->SurpriseRemove) return;
+	if (enable) {
+		reg->M00_MacControl |= 0x00400000;
+		Wb35Reg_Write( pHwData, 0x0800, reg->M00_MacControl );
+	} else {
+		reg->M00_MacControl&=~0x00400000;
+		Wb35Reg_Write( pHwData, 0x0800, reg->M00_MacControl );
+	}
+}
+
+void hal_set_accept_multicast(  phw_data_t pHwData,  u8 enable )
+{
+	struct wb35_reg *reg = &pHwData->reg;
+
+	if( pHwData->SurpriseRemove ) return;
+
+	reg->M00_MacControl &= ~0x01000000;//The HW value
+	if (enable)  reg->M00_MacControl |= 0x01000000;//The HW value
+	Wb35Reg_Write( pHwData, 0x0800, reg->M00_MacControl );
+}
+
+void hal_set_accept_beacon(  phw_data_t pHwData,  u8 enable )
+{
+	struct wb35_reg *reg = &pHwData->reg;
+
+	if( pHwData->SurpriseRemove ) return;
+
+	// 20040108 debug
+	if( !enable )//Due to SME and MLME are not suitable for 35
+		return;
+
+	reg->M00_MacControl &= ~0x04000000;//The HW value
+	if( enable )
+		reg->M00_MacControl |= 0x04000000;//The HW value
+
+	Wb35Reg_Write( pHwData, 0x0800, reg->M00_MacControl );
+}
+//---------------------------------------------------------------------------------------------------
+
+void hal_stop(  phw_data_t pHwData )
+{
+	struct wb35_reg *reg = &pHwData->reg;
+
+	pHwData->Wb35Rx.rx_halt = 1;
+	Wb35Rx_stop( pHwData );
+
+	pHwData->Wb35Tx.tx_halt = 1;
+	Wb35Tx_stop( pHwData );
+
+	reg->D00_DmaControl &= ~0xc0000000;//Tx Off, Rx Off
+	Wb35Reg_Write( pHwData, 0x0400, reg->D00_DmaControl );
+}
+
+unsigned char hal_idle(phw_data_t pHwData)
+{
+	struct wb35_reg *reg = &pHwData->reg;
+	PWBUSB	pWbUsb = &pHwData->WbUsb;
+
+	if( !pHwData->SurpriseRemove && ( pWbUsb->DetectCount || reg->EP0vm_state!=VM_STOP ) )
+		return false;
+
+	return true;
+}
+//---------------------------------------------------------------------------------------------------
+void hal_set_phy_type(  phw_data_t pHwData,  u8 PhyType )
+{
+	pHwData->phy_type = PhyType;
 }
 
 void hal_set_radio_mode( phw_data_t pHwData,  unsigned char radio_off)
 {
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
+	struct wb35_reg *reg = &pHwData->reg;
 
 	if( pHwData->SurpriseRemove ) return;
 
 	if (radio_off)	//disable Baseband receive off
 	{
 		pHwData->CurrentRadioSw = 1; // off
-		pWb35Reg->M24_MacControl &= 0xffffffbf;
+		reg->M24_MacControl &= 0xffffffbf;
 	}
 	else
 	{
 		pHwData->CurrentRadioSw = 0; // on
-		pWb35Reg->M24_MacControl |= 0x00000040;
+		reg->M24_MacControl |= 0x00000040;
 	}
-	Wb35Reg_Write( pHwData, 0x0824, pWb35Reg->M24_MacControl );
+	Wb35Reg_Write( pHwData, 0x0824, reg->M24_MacControl );
 }
 
 u8 hal_get_antenna_number(  phw_data_t pHwData )
 {
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
+	struct wb35_reg *reg = &pHwData->reg;
 
-	if ((pWb35Reg->BB2C & BIT(11)) == 0)
+	if ((reg->BB2C & BIT(11)) == 0)
 		return 0;
 	else
 		return 1;
 }
 
-void hal_set_antenna_number(  phw_data_t pHwData, u8 number )
-{
-
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
-
-	if (number == 1) {
-		pWb35Reg->BB2C |= BIT(11);
-	} else {
-		pWb35Reg->BB2C &= ~BIT(11);
-	}
-	Wb35Reg_Write( pHwData, 0x102c, pWb35Reg->BB2C );
-#ifdef _PE_STATE_DUMP_
-	WBDEBUG(("Current antenna number : %d\n", number));
-#endif
-}
-
 //----------------------------------------------------------------------------------------------------
 //0 : radio on; 1: radio off
 u8 hal_get_hw_radio_off(  phw_data_t pHwData )
 {
-	PWB35REG	pWb35Reg = &pHwData->Wb35Reg;
+	struct wb35_reg *reg = &pHwData->reg;
 
 	if( pHwData->SurpriseRemove ) return 1;
 
 	//read the bit16 of register U1B0
-	Wb35Reg_Read( pHwData, 0x3b0, &pWb35Reg->U1B0 );
-	if ((pWb35Reg->U1B0 & 0x00010000)) {
+	Wb35Reg_Read( pHwData, 0x3b0, &reg->U1B0 );
+	if ((reg->U1B0 & 0x00010000)) {
 		pHwData->CurrentRadioHw = 1;
 		return 1;
 	} else {
@@ -823,56 +562,7 @@ unsigned char hal_set_dxx_reg(  phw_data_t pHwData,  u16 number,  u32 value )
 	return ret;
 }
 
-void hal_scan_status_indicate(phw_data_t pHwData, unsigned char IsOnProgress)
-{
-	if( pHwData->SurpriseRemove ) return;
-	pHwData->LED_Scanning = IsOnProgress ? 1 : 0;
-}
-
-void hal_system_power_change(phw_data_t pHwData, u32 PowerState)
-{
-	if( PowerState != 0 )
-	{
-		pHwData->SurpriseRemove = 1;
-		if( pHwData->WbUsb.IsUsb20 )
-			hal_stop( pHwData );
-	}
-	else
-	{
-		if( !pHwData->WbUsb.IsUsb20 )
-			hal_stop( pHwData );
-	}
-}
-
-void hal_surprise_remove(  phw_data_t pHwData )
-{
-	PADAPTER Adapter = pHwData->Adapter;
-	if (OS_ATOMIC_INC( Adapter, &pHwData->SurpriseRemoveCount ) == 1) {
-		#ifdef _PE_STATE_DUMP_
-		WBDEBUG(("Calling hal_surprise_remove\n"));
-		#endif
-		OS_STOP( Adapter );
-	}
-}
-
-void hal_rate_change(  phw_data_t pHwData ) // Notify the HAL rate is changing 20060613.1
-{
-	PADAPTER	Adapter = pHwData->Adapter;
-	u8		rate = CURRENT_TX_RATE;
-
-	BBProcessor_RateChanging( pHwData, rate );
-}
-
 void hal_set_rf_power(phw_data_t pHwData, u8 PowerIndex)
 {
 	RFSynthesizer_SetPowerIndex( pHwData, PowerIndex );
 }
-
-unsigned char hal_set_LED(phw_data_t pHwData, u32 Mode) // 20061108 for WPS led control
-{
-	pHwData->LED_Blinking = 0;
-	pHwData->LED_control = Mode;
-	OS_TIMER_SET( &pHwData->LEDTimer, 10 ); // 20060623
-	return TRUE;
-}
-
diff --git a/drivers/staging/winbond/wbhal_f.h b/drivers/staging/winbond/wbhal_f.h
index ea9531ac847..e805f40f635 100644
--- a/drivers/staging/winbond/wbhal_f.h
+++ b/drivers/staging/winbond/wbhal_f.h
@@ -1,25 +1,19 @@
 //=====================================================================
 // Device related include
 //=====================================================================
-#ifdef WB_LINUX
-	#include "linux/wbusb_f.h"
-	#include "linux/wb35reg_f.h"
-	#include "linux/wb35tx_f.h"
-	#include "linux/wb35rx_f.h"
-#else
-	#include "wbusb_f.h"
-	#include "wb35reg_f.h"
-	#include "wb35tx_f.h"
-	#include "wb35rx_f.h"
-#endif
+#include "wb35reg_f.h"
+#include "wb35tx_f.h"
+#include "wb35rx_f.h"
+
+#include "core.h"
 
 //====================================================================================
 // Function declaration
 //====================================================================================
 void hal_remove_mapping_key(  phw_data_t pHwData,  u8 *pmac_addr );
 void hal_remove_default_key(  phw_data_t pHwData,  u32 index );
-unsigned char hal_set_mapping_key(  phw_data_t Adapter,  u8 *pmac_addr,  u8 null_key,  u8 wep_on,  u8 *ptx_tsc,  u8 *prx_tsc,  u8 key_type,  u8 key_len,  u8 *pkey_data );
-unsigned char hal_set_default_key(  phw_data_t Adapter,  u8 index,  u8 null_key,  u8 wep_on,  u8 *ptx_tsc,  u8 *prx_tsc,  u8 key_type,  u8 key_len,  u8 *pkey_data );
+unsigned char hal_set_mapping_key(  phw_data_t adapter,  u8 *pmac_addr,  u8 null_key,  u8 wep_on,  u8 *ptx_tsc,  u8 *prx_tsc,  u8 key_type,  u8 key_len,  u8 *pkey_data );
+unsigned char hal_set_default_key(  phw_data_t adapter,  u8 index,  u8 null_key,  u8 wep_on,  u8 *ptx_tsc,  u8 *prx_tsc,  u8 key_type,  u8 key_len,  u8 *pkey_data );
 void hal_clear_all_default_key(  phw_data_t pHwData );
 void hal_clear_all_group_key(  phw_data_t pHwData );
 void hal_clear_all_mapping_key(  phw_data_t pHwData );
@@ -27,14 +21,11 @@ void hal_clear_all_key(  phw_data_t pHwData );
 void hal_get_ethernet_address(  phw_data_t pHwData,  u8 *current_address );
 void hal_set_ethernet_address(  phw_data_t pHwData,  u8 *current_address );
 void hal_get_permanent_address(  phw_data_t pHwData,  u8 *pethernet_address );
-unsigned char hal_init_hardware(  phw_data_t pHwData,  PADAPTER Adapter );
+u8 hal_init_hardware(struct ieee80211_hw *hw);
 void hal_set_power_save_mode(  phw_data_t pHwData,  unsigned char power_save,  unsigned char wakeup,  unsigned char dtim );
 void hal_get_power_save_mode(  phw_data_t pHwData,   u8 *pin_pwr_save );
 void hal_set_slot_time(  phw_data_t pHwData,  u8 type );
 #define hal_set_atim_window( _A, _ATM )
-void hal_set_rates(  phw_data_t pHwData,  u8 *pbss_rates,  u8 length,  unsigned char basic_rate_set );
-#define hal_set_basic_rates( _A, _R, _L ) hal_set_rates( _A, _R, _L, TRUE )
-#define hal_set_op_rates( _A, _R, _L ) hal_set_rates( _A, _R, _L, FALSE )
 void hal_start_bss(  phw_data_t pHwData,  u8 mac_op_mode );
 void hal_join_request(  phw_data_t pHwData,  u8 bss_type ); // 0:BSS STA 1:IBSS STA//
 void hal_stop_sync_bss(  phw_data_t pHwData );
@@ -47,39 +38,25 @@ void hal_set_listen_interval(  phw_data_t pHwData,  u16 listen_interval );
 void hal_set_cap_info(  phw_data_t pHwData,  u16 capability_info );
 void hal_set_ssid(  phw_data_t pHwData,  u8 *pssid,  u8 ssid_len );
 void hal_set_current_channel(  phw_data_t pHwData,  ChanInfo channel );
-void hal_set_current_channel_ex(  phw_data_t pHwData,  ChanInfo channel );
-void hal_get_current_channel(  phw_data_t pHwData,  ChanInfo *channel );
 void hal_set_accept_broadcast(  phw_data_t pHwData,  u8 enable );
 void hal_set_accept_multicast(  phw_data_t pHwData,  u8 enable );
 void hal_set_accept_beacon(  phw_data_t pHwData,  u8 enable );
-void hal_set_multicast_address(  phw_data_t pHwData,  u8 *address,  u8 number );
-u8 hal_get_accept_beacon(  phw_data_t pHwData );
 void hal_stop(  phw_data_t pHwData );
 void hal_halt(  phw_data_t pHwData, void *ppa_data );
 void hal_start_tx0(  phw_data_t pHwData );
 void hal_set_phy_type(  phw_data_t pHwData,  u8 PhyType );
-void hal_get_phy_type(  phw_data_t pHwData,  u8 *PhyType );
-unsigned char hal_reset_hardware(  phw_data_t pHwData,  void* ppa );
-void hal_set_cwmin(  phw_data_t pHwData,  u8	cwin_min );
 #define hal_get_cwmin( _A ) ( (_A)->cwmin )
 void hal_set_cwmax(  phw_data_t pHwData,  u16 cwin_max );
 #define hal_get_cwmax( _A ) ( (_A)->cwmax )
 void hal_set_rsn_wpa(  phw_data_t pHwData,  u32 * RSN_IE_Bitmap , u32 * RSN_OUI_type , unsigned char bDesiredAuthMode);
-//s32 hal_get_rssi(  phw_data_t pHwData,  u32 HalRssi );
-s32 hal_get_rssi(  phw_data_t pHwData,  u32 *HalRssiArry,  u8 Count );
-s32 hal_get_rssi_bss(  phw_data_t pHwData,  u16 idx,  u8 Count );
 void hal_set_connect_info(  phw_data_t pHwData,  unsigned char boConnect );
 u8 hal_get_est_sq3(  phw_data_t pHwData,  u8 Count );
-void hal_led_control_1a(  phw_data_t pHwData );
-void hal_led_control(  void* S1,  phw_data_t pHwData,  void* S3,  void* S4 );
 void hal_set_rf_power(  phw_data_t pHwData,  u8 PowerIndex ); // 20060621 Modify
-void hal_reset_counter(  phw_data_t pHwData );
 void hal_set_radio_mode(  phw_data_t pHwData,  unsigned char boValue);
 void hal_descriptor_indicate(  phw_data_t pHwData,  PDESCRIPTOR pDes );
 u8 hal_get_antenna_number(  phw_data_t pHwData );
-void hal_set_antenna_number(  phw_data_t pHwData, u8 number );
 u32 hal_get_bss_pk_cnt(  phw_data_t pHwData );
-#define hal_get_region_from_EEPROM( _A ) ( (_A)->Wb35Reg.EEPROMRegion )
+#define hal_get_region_from_EEPROM( _A ) ( (_A)->reg.EEPROMRegion )
 void hal_set_accept_promiscuous		(  phw_data_t pHwData,  u8 enable);
 #define hal_get_tx_buffer( _A, _B ) Wb35Tx_get_tx_buffer( _A, _B )
 u8 hal_get_hw_radio_off			(  phw_data_t pHwData );
@@ -88,20 +65,13 @@ u8 hal_get_hw_radio_off			(  phw_data_t pHwData );
 #define hal_rssi_boundary_high( _A ) (_A->RSSI_high)
 #define hal_rssi_boundary_low( _A ) (_A->RSSI_low)
 #define hal_scan_interval( _A )		(_A->Scan_Interval)
-void hal_scan_status_indicate(  phw_data_t pHwData, u8 status);	// 0: complete, 1: in progress
-void hal_system_power_change(  phw_data_t pHwData, u32 PowerState ); // 20051230 -=D0 1=D1 ..
-void hal_surprise_remove(  phw_data_t pHwData );
 
 #define PHY_DEBUG( msg, args... )
 
-
-
-void hal_rate_change(  phw_data_t pHwData ); // Notify the HAL rate is changing 20060613.1
 unsigned char hal_get_dxx_reg(  phw_data_t pHwData,  u16 number,  u32 * pValue );
 unsigned char hal_set_dxx_reg(  phw_data_t pHwData,  u16 number,  u32 value );
 #define hal_get_time_count( _P )	(_P->time_count/10)	// return 100ms count
 #define hal_detect_error( _P )		(_P->WbUsb.DetectCount)
-unsigned char hal_set_LED(  phw_data_t pHwData,  u32 Mode ); // 20061108 for WPS led control
 
 //-------------------------------------------------------------------------
 // The follow function is unused for IS89C35
@@ -113,7 +83,6 @@ unsigned char hal_set_LED(  phw_data_t pHwData,  u32 Mode ); // 20061108 for WPS
 #define hal_ibss_disconnect(_A) hal_stop_sync_bss(_A)
 #define hal_join_request_stop(_A)
 unsigned char	hal_idle(  phw_data_t pHwData );
-#define pa_stall_execution( _A )	//OS_SLEEP( 1 )
 #define hw_get_cxx_reg( _A, _B, _C )
 #define hw_set_cxx_reg( _A, _B, _C )
 #define hw_get_dxx_reg( _A, _B, _C )	hal_get_dxx_reg( _A, _B, (u32 *)_C )
diff --git a/drivers/staging/winbond/wbhal_s.h b/drivers/staging/winbond/wbhal_s.h
index 2ee3f0fc1ad..276d2b12632 100644
--- a/drivers/staging/winbond/wbhal_s.h
+++ b/drivers/staging/winbond/wbhal_s.h
@@ -1,3 +1,10 @@
+#ifndef __WINBOND_WBHAL_S_H
+#define __WINBOND_WBHAL_S_H
+
+#include <linux/types.h>
+
+#include "common.h"
+
 //[20040722 WK]
 #define HAL_LED_SET_MASK		0x001c	//20060901 Extend
 #define HAL_LED_SET_SHIFT		2
@@ -415,10 +422,10 @@ typedef struct _TXVGA_FOR_50 {
 // Device related include
 //=====================================================================
 
-#include "linux/wbusb_s.h"
-#include "linux/wb35reg_s.h"
-#include "linux/wb35tx_s.h"
-#include "linux/wb35rx_s.h"
+#include "wbusb_s.h"
+#include "wb35reg_s.h"
+#include "wb35tx_s.h"
+#include "wb35rx_s.h"
 
 
 // For Hal using ==================================================================
@@ -442,16 +449,6 @@ typedef struct _HW_DATA_T
 	u32	FragCount;
 	u32	DMAFix; //V1_DMA_FIX The variable can be removed if driver want to save mem space for V2.
 
-	//=======================================================================================
-	// For USB driver, hal need more variables. Due to
-	//	1. NDIS-WDM operation
-	//	2. The SME, MLME and OLD MDS need Adapter structure, but the driver under HAL doesn't
-	//		have that parameter when receiving and indicating packet.
-	//		The MDS must input the Adapter pointer as the second parameter of hal_init_hardware.
-	//		The function usage is different than PCI driver.
-	//=======================================================================================
-	void* Adapter;
-
 	//===============================================
 	// Definition for MAC address
 	//===============================================
@@ -506,11 +503,11 @@ typedef struct _HW_DATA_T
 	// Variable for each module
 	//========================================================================
 	WBUSB		WbUsb; // Need WbUsb.h
-	WB35REG		Wb35Reg; // Need Wb35Reg.h
+	struct wb35_reg	reg; // Need Wb35Reg.h
 	WB35TX		Wb35Tx; // Need Wb35Tx.h
 	WB35RX		Wb35Rx; // Need Wb35Rx.h
 
-	OS_TIMER	LEDTimer;// For LED
+	struct timer_list	LEDTimer;// For LED
 
 	u32		LEDpoint;// For LED
 
@@ -570,7 +567,7 @@ typedef struct _HW_DATA_T
 	u32		RxByteCountLast;
 	u32		TxByteCountLast;
 
-	s32		SurpriseRemoveCount;
+	atomic_t	SurpriseRemoveCount;
 
 	// For global timer
 	u32		time_count;//TICK_TIME_100ms 1 = 100ms
@@ -612,4 +609,4 @@ typedef struct _HAL_RATE
 	u32   NumRate54M;
 } HAL_RATE, *PHAL_RATE;
 
-
+#endif
diff --git a/drivers/staging/winbond/wblinux.c b/drivers/staging/winbond/wblinux.c
deleted file mode 100644
index 4ed45e48831..00000000000
--- a/drivers/staging/winbond/wblinux.c
+++ /dev/null
@@ -1,275 +0,0 @@
-//============================================================================
-//  Copyright (c) 1996-2005 Winbond Electronic Corporation
-//
-//  Module Name:
-//    wblinux.c
-//
-//  Abstract:
-//    Linux releated routines
-//
-//============================================================================
-#include "os_common.h"
-
-u32
-WBLINUX_MemoryAlloc(void* *VirtualAddress, u32 Length)
-{
-	*VirtualAddress = kzalloc( Length, GFP_ATOMIC ); //GFP_KERNEL is not suitable
-
-	if (*VirtualAddress == NULL)
-		return 0;
-	return 1;
-}
-
-s32
-EncapAtomicInc(PADAPTER Adapter, void* pAtomic)
-{
-	PWBLINUX pWbLinux = &Adapter->WbLinux;
-	u32	ltmp;
-	u32 *	pltmp = (u32 *)pAtomic;
-	spin_lock_irq( &pWbLinux->AtomicSpinLock );
-	(*pltmp)++;
-	ltmp = (*pltmp);
-	spin_unlock_irq( &pWbLinux->AtomicSpinLock );
-	return ltmp;
-}
-
-s32
-EncapAtomicDec(PADAPTER Adapter, void* pAtomic)
-{
-	PWBLINUX pWbLinux = &Adapter->WbLinux;
-	u32	ltmp;
-	u32 *	pltmp = (u32 *)pAtomic;
-	spin_lock_irq( &pWbLinux->AtomicSpinLock );
-	(*pltmp)--;
-	ltmp = (*pltmp);
-	spin_unlock_irq( &pWbLinux->AtomicSpinLock );
-	return ltmp;
-}
-
-unsigned char
-WBLINUX_Initial(PADAPTER Adapter)
-{
-	PWBLINUX pWbLinux = &Adapter->WbLinux;
-
-	spin_lock_init( &pWbLinux->SpinLock );
-	spin_lock_init( &pWbLinux->AtomicSpinLock );
-	return TRUE;
-}
-
-void
-WBLinux_ReceivePacket(PADAPTER Adapter, PRXLAYER1 pRxLayer1)
-{
-	BUG();
-}
-
-
-void
-WBLINUX_GetNextPacket(PADAPTER Adapter,  PDESCRIPTOR pDes)
-{
-	BUG();
-}
-
-void
-WBLINUX_GetNextPacketCompleted(PADAPTER Adapter, PDESCRIPTOR pDes)
-{
-	BUG();
-}
-
-void
-WBLINUX_Destroy(PADAPTER Adapter)
-{
-	WBLINUX_stop( Adapter );
-#ifdef _PE_USB_INI_DUMP_
-	WBDEBUG(("[w35und] unregister_netdev!\n"));
-#endif
-}
-
-void
-WBLINUX_stop(  PADAPTER Adapter )
-{
-	PWBLINUX	pWbLinux = &Adapter->WbLinux;
-	struct sk_buff *pSkb;
-
-	if (OS_ATOMIC_INC( Adapter, &pWbLinux->ThreadCount ) == 1) {
-		// Shutdown module immediately
-		pWbLinux->shutdown = 1;
-
-		while (pWbLinux->skb_array[ pWbLinux->skb_GetIndex ]) {
-			// Trying to free the un-sending packet
-			pSkb = pWbLinux->skb_array[ pWbLinux->skb_GetIndex ];
-			pWbLinux->skb_array[ pWbLinux->skb_GetIndex ] = NULL;
-			if( in_irq() )
-				dev_kfree_skb_irq( pSkb );
-			else
-				dev_kfree_skb( pSkb );
-
-			pWbLinux->skb_GetIndex++;
-			pWbLinux->skb_GetIndex %= WBLINUX_PACKET_ARRAY_SIZE;
-		}
-
-#ifdef _PE_STATE_DUMP_
-		WBDEBUG(( "[w35und] SKB_RELEASE OK\n" ));
-#endif
-	}
-
-	OS_ATOMIC_DEC( Adapter, &pWbLinux->ThreadCount );
-}
-
-void
-WbWlanHalt(  PADAPTER Adapter )
-{
-	//---------------------
-	Adapter->sLocalPara.ShutDowned = TRUE;
-
-	Mds_Destroy( Adapter );
-
-	// Turn off Rx and Tx hardware ability
-	hal_stop( &Adapter->sHwData );
-#ifdef _PE_USB_INI_DUMP_
-	WBDEBUG(("[w35und] Hal_stop O.K.\n"));
-#endif
-	OS_SLEEP(100000);// Waiting Irp completed
-
-	// Destroy the NDIS module
-	WBLINUX_Destroy( Adapter );
-
-	// Halt the HAL
-	hal_halt(&Adapter->sHwData, NULL);
-}
-
-unsigned char
-WbWLanInitialize(PADAPTER Adapter)
-{
-	phw_data_t	pHwData;
-	u8		*pMacAddr;
-	u8		*pMacAddr2;
-	u32		InitStep = 0;
-	u8		EEPROM_region;
-	u8		HwRadioOff;
-
-	//
-	// Setting default value for Linux
-	//
-	Adapter->sLocalPara.region_INF = REGION_AUTO;
-	Adapter->sLocalPara.TxRateMode = RATE_AUTO;
-	psLOCAL->bMacOperationMode = MODE_802_11_BG;	// B/G mode
-	Adapter->Mds.TxRTSThreshold = DEFAULT_RTSThreshold;
-	Adapter->Mds.TxFragmentThreshold = DEFAULT_FRAGMENT_THRESHOLD;
-	hal_set_phy_type( &Adapter->sHwData, RF_WB_242_1 );
-	Adapter->sLocalPara.MTUsize = MAX_ETHERNET_PACKET_SIZE;
-	psLOCAL->bPreambleMode = AUTO_MODE;
-	Adapter->sLocalPara.RadioOffStatus.boSwRadioOff = FALSE;
-	pHwData = &Adapter->sHwData;
-	hal_set_phy_type( pHwData, RF_DECIDE_BY_INF );
-
-	//
-	// Initial each module and variable
-	//
-	if (!WBLINUX_Initial(Adapter)) {
-#ifdef _PE_USB_INI_DUMP_
-		WBDEBUG(("[w35und]WBNDIS initialization failed\n"));
-#endif
-		goto error;
-	}
-
-	// Initial Software variable
-	Adapter->sLocalPara.ShutDowned = FALSE;
-
-	//added by ws for wep key error detection
-	Adapter->sLocalPara.bWepKeyError= FALSE;
-	Adapter->sLocalPara.bToSelfPacketReceived = FALSE;
-	Adapter->sLocalPara.WepKeyDetectTimerCount= 2 * 100; /// 2 seconds
-
-	// Initial USB hal
-	InitStep = 1;
-	pHwData = &Adapter->sHwData;
-	if (!hal_init_hardware(pHwData, Adapter))
-		goto error;
-
-	EEPROM_region = hal_get_region_from_EEPROM( pHwData );
-	if (EEPROM_region != REGION_AUTO)
-		psLOCAL->region = EEPROM_region;
-	else {
-		if (psLOCAL->region_INF != REGION_AUTO)
-			psLOCAL->region = psLOCAL->region_INF;
-		else
-			psLOCAL->region = REGION_USA;	//default setting
-	}
-
-	// Get Software setting flag from hal
-	Adapter->sLocalPara.boAntennaDiversity = FALSE;
-	if (hal_software_set(pHwData) & 0x00000001)
-		Adapter->sLocalPara.boAntennaDiversity = TRUE;
-
-	//
-	// For TS module
-	//
-	InitStep = 2;
-
-	// For MDS module
-	InitStep = 3;
-	Mds_initial(Adapter);
-
-	//=======================================
-	// Initialize the SME, SCAN, MLME, ROAM
-	//=======================================
-	InitStep = 4;
-	InitStep = 5;
-	InitStep = 6;
-
-	// If no user-defined address in the registry, use the addresss "burned" on the NIC instead.
-	pMacAddr = Adapter->sLocalPara.ThisMacAddress;
-	pMacAddr2 = Adapter->sLocalPara.PermanentAddress;
-	hal_get_permanent_address( pHwData, Adapter->sLocalPara.PermanentAddress );// Reading ethernet address from EEPROM
-	if (OS_MEMORY_COMPARE(pMacAddr, "\x00\x00\x00\x00\x00\x00", MAC_ADDR_LENGTH )) // Is equal
-	{
-		memcpy( pMacAddr, pMacAddr2, MAC_ADDR_LENGTH );
-	} else {
-		// Set the user define MAC address
-		hal_set_ethernet_address( pHwData, Adapter->sLocalPara.ThisMacAddress );
-	}
-
-	//get current antenna
-	psLOCAL->bAntennaNo = hal_get_antenna_number(pHwData);
-#ifdef _PE_STATE_DUMP_
-	WBDEBUG(("Driver init, antenna no = %d\n", psLOCAL->bAntennaNo));
-#endif
-	hal_get_hw_radio_off( pHwData );
-
-	// Waiting for HAL setting OK
-	while (!hal_idle(pHwData))
-		OS_SLEEP(10000);
-
-	MTO_Init(Adapter);
-
-	HwRadioOff = hal_get_hw_radio_off( pHwData );
-	psLOCAL->RadioOffStatus.boHwRadioOff = !!HwRadioOff;
-
-	hal_set_radio_mode( pHwData, (unsigned char)(psLOCAL->RadioOffStatus.boSwRadioOff || psLOCAL->RadioOffStatus.boHwRadioOff) );
-
-	hal_driver_init_OK(pHwData) = 1; // Notify hal that the driver is ready now.
-	//set a tx power for reference.....
-//	sme_set_tx_power_level(Adapter, 12);	FIXME?
-	return TRUE;
-
-error:
-	switch (InitStep) {
-	case 5:
-	case 4:
-	case 3: Mds_Destroy( Adapter );
-	case 2:
-	case 1: WBLINUX_Destroy( Adapter );
-		hal_halt( pHwData, NULL );
-	case 0: break;
-	}
-
-	return FALSE;
-}
-
-void WBLINUX_ConnectStatus(PADAPTER Adapter, u32 flag)
-{
-	PWBLINUX	pWbLinux = &Adapter->WbLinux;
-
-	pWbLinux->LinkStatus = flag; // OS_DISCONNECTED	or  OS_CONNECTED
-}
-
diff --git a/drivers/staging/winbond/wblinux_f.h b/drivers/staging/winbond/wblinux_f.h
index 68240c5fc80..868e8772724 100644
--- a/drivers/staging/winbond/wblinux_f.h
+++ b/drivers/staging/winbond/wblinux_f.h
@@ -1,23 +1,16 @@
+#ifndef __WBLINUX_F_H
+#define __WBLINUX_F_H
+
+#include "core.h"
+#include "mds_s.h"
+
 //=========================================================================
 // Copyright (c) 1996-2004 Winbond Electronic Corporation
 //
 // wblinux_f.h
 //
-u32 WBLINUX_MemoryAlloc(  void* *VirtualAddress,  u32 Length );
-s32 EncapAtomicInc(  PADAPTER Adapter,  void* pAtomic );
-s32 EncapAtomicDec(  PADAPTER Adapter,  void* pAtomic );
-void WBLinux_ReceivePacket(  PADAPTER Adapter,  PRXLAYER1 pRxLayer1 );
-unsigned char WBLINUX_Initial(  PADAPTER Adapter );
 int wb35_start_xmit(struct sk_buff *skb, struct net_device *netdev );
-void WBLINUX_GetNextPacket(  PADAPTER Adapter,  PDESCRIPTOR pDes );
-void WBLINUX_GetNextPacketCompleted(  PADAPTER Adapter,  PDESCRIPTOR pDes );
-void WBLINUX_stop(  PADAPTER Adapter );
-void WBLINUX_Destroy(  PADAPTER Adapter );
 void wb35_set_multicast( struct net_device *netdev );
 struct net_device_stats * wb35_netdev_stats( struct net_device *netdev );
-void WBLINUX_stop(  PADAPTER Adapter );
-void WbWlanHalt(  PADAPTER Adapter );
-void WBLINUX_ConnectStatus(  PADAPTER Adapter,  u32 flag );
-
-
 
+#endif
diff --git a/drivers/staging/winbond/wblinux_s.h b/drivers/staging/winbond/wblinux_s.h
deleted file mode 100644
index fd2bb43bf3c..00000000000
--- a/drivers/staging/winbond/wblinux_s.h
+++ /dev/null
@@ -1,45 +0,0 @@
-//============================================================
-// wblinux_s.h
-//
-#define OS_MEMORY_ALLOC( _V, _S )	WBLINUX_MemoryAlloc( _V, _S )
-#define OS_LINK_STATUS			(Adapter->WbLinux.LinkStatus == OS_CONNECTED)
-#define OS_SET_SHUTDOWN( _A )		_A->WbLinux.shutdown=1
-#define OS_SET_RESUME( _A )		_A->WbLinux.shutdown=0
-#define OS_CONNECT_STATUS_INDICATE( _A, _F )		WBLINUX_ConnectStatus( _A, _F )
-#define OS_DISCONNECTED	0
-#define OS_CONNECTED	1
-#define OS_STOP( _A )	WBLINUX_stop( _A )
-
-#define OS_CURRENT_RX_BYTE( _A )		_A->WbLinux.RxByteCount
-#define OS_CURRENT_TX_BYTE( _A )		_A->WbLinux.TxByteCount
-#define OS_EVENT_INDICATE( _A, _B, _F )
-#define OS_PMKID_STATUS_EVENT( _A )
-#define OS_RECEIVE_PACKET_INDICATE( _A, _D )		WBLinux_ReceivePacket( _A, _D )
-#define OS_RECEIVE_802_1X_PACKET_INDICATE( _A, _D )	EAP_ReceivePacket( _A, _D )
-#define OS_GET_PACKET( _A, _D )				WBLINUX_GetNextPacket( _A, _D )
-#define OS_GET_PACKET_COMPLETE( _A, _D )	WBLINUX_GetNextPacketCompleted( _A, _D )
-#define OS_SEND_RESULT( _A, _ID, _R )
-
-#define WBLINUX_PACKET_ARRAY_SIZE	(ETHERNET_TX_DESCRIPTORS*4)
-
-typedef struct _WBLINUX
-{
-	spinlock_t	AtomicSpinLock;
-	spinlock_t	SpinLock;
-	u32	shutdown;
-
-	OS_ATOMIC	ThreadCount;
-
-	u32	LinkStatus;		// OS_DISCONNECTED or OS_CONNECTED
-
-	u32	RxByteCount;
-	u32	TxByteCount;
-
-	struct sk_buff *skb_array[ WBLINUX_PACKET_ARRAY_SIZE ];
-	struct sk_buff *packet_return;
-	s32	skb_SetIndex;
-	s32	skb_GetIndex;
-	s32	netif_state_stop; // 1: stop  0: normal
-} WBLINUX, *PWBLINUX;
-
-
diff --git a/drivers/staging/winbond/wbusb.c b/drivers/staging/winbond/wbusb.c
new file mode 100644
index 00000000000..b003f9a7e15
--- /dev/null
+++ b/drivers/staging/winbond/wbusb.c
@@ -0,0 +1,438 @@
+/*
+ * Copyright 2008 Pavel Machek <pavel@suse.cz>
+ *
+ * Distribute under GPLv2.
+ */
+#include <net/mac80211.h>
+#include <linux/usb.h>
+
+#include "core.h"
+#include "mds_f.h"
+#include "mlmetxrx_f.h"
+#include "mto_f.h"
+#include "wbhal_f.h"
+#include "wblinux_f.h"
+
+MODULE_AUTHOR("Original by: Jeff Lee<YY_Lee@issc.com.tw> Adapted to 2.6.x by Costantino Leandro (Rxart Desktop) <le_costantino@pixartargentina.com.ar>");
+MODULE_DESCRIPTION("IS89C35 802.11bg WLAN USB Driver");
+MODULE_LICENSE("GPL");
+MODULE_VERSION("0.1");
+
+static struct usb_device_id wb35_table[] __devinitdata = {
+	{USB_DEVICE(0x0416, 0x0035)},
+	{USB_DEVICE(0x18E8, 0x6201)},
+	{USB_DEVICE(0x18E8, 0x6206)},
+	{USB_DEVICE(0x18E8, 0x6217)},
+	{USB_DEVICE(0x18E8, 0x6230)},
+	{USB_DEVICE(0x18E8, 0x6233)},
+	{USB_DEVICE(0x1131, 0x2035)},
+	{ 0, }
+};
+
+MODULE_DEVICE_TABLE(usb, wb35_table);
+
+static struct ieee80211_rate wbsoft_rates[] = {
+	{ .bitrate = 10, .flags = IEEE80211_RATE_SHORT_PREAMBLE },
+};
+
+static struct ieee80211_channel wbsoft_channels[] = {
+	{ .center_freq = 2412},
+};
+
+static struct ieee80211_supported_band wbsoft_band_2GHz = {
+	.channels	= wbsoft_channels,
+	.n_channels	= ARRAY_SIZE(wbsoft_channels),
+	.bitrates	= wbsoft_rates,
+	.n_bitrates	= ARRAY_SIZE(wbsoft_rates),
+};
+
+static int wbsoft_add_interface(struct ieee80211_hw *dev,
+				 struct ieee80211_if_init_conf *conf)
+{
+	printk("wbsoft_add interface called\n");
+	return 0;
+}
+
+static void wbsoft_remove_interface(struct ieee80211_hw *dev,
+				     struct ieee80211_if_init_conf *conf)
+{
+	printk("wbsoft_remove interface called\n");
+}
+
+static void wbsoft_stop(struct ieee80211_hw *hw)
+{
+	printk(KERN_INFO "%s called\n", __func__);
+}
+
+static int wbsoft_get_stats(struct ieee80211_hw *hw,
+			    struct ieee80211_low_level_stats *stats)
+{
+	printk(KERN_INFO "%s called\n", __func__);
+	return 0;
+}
+
+static int wbsoft_get_tx_stats(struct ieee80211_hw *hw,
+			       struct ieee80211_tx_queue_stats *stats)
+{
+	printk(KERN_INFO "%s called\n", __func__);
+	return 0;
+}
+
+static void wbsoft_configure_filter(struct ieee80211_hw *dev,
+				     unsigned int changed_flags,
+				     unsigned int *total_flags,
+				     int mc_count, struct dev_mc_list *mclist)
+{
+	unsigned int bit_nr, new_flags;
+	u32 mc_filter[2];
+	int i;
+
+	new_flags = 0;
+
+	if (*total_flags & FIF_PROMISC_IN_BSS) {
+		new_flags |= FIF_PROMISC_IN_BSS;
+		mc_filter[1] = mc_filter[0] = ~0;
+	} else if ((*total_flags & FIF_ALLMULTI) || (mc_count > 32)) {
+		new_flags |= FIF_ALLMULTI;
+		mc_filter[1] = mc_filter[0] = ~0;
+	} else {
+		mc_filter[1] = mc_filter[0] = 0;
+		for (i = 0; i < mc_count; i++) {
+			if (!mclist)
+				break;
+			printk("Should call ether_crc here\n");
+			//bit_nr = ether_crc(ETH_ALEN, mclist->dmi_addr) >> 26;
+			bit_nr = 0;
+
+			bit_nr &= 0x3F;
+			mc_filter[bit_nr >> 5] |= 1 << (bit_nr & 31);
+			mclist = mclist->next;
+		}
+	}
+
+	dev->flags &= ~IEEE80211_HW_RX_INCLUDES_FCS;
+
+	*total_flags = new_flags;
+}
+
+static int wbsoft_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
+{
+	struct wbsoft_priv *priv = dev->priv;
+
+	MLMESendFrame(priv, skb->data, skb->len, FRAME_TYPE_802_11_MANAGEMENT);
+
+	return NETDEV_TX_OK;
+}
+
+
+static int wbsoft_start(struct ieee80211_hw *dev)
+{
+	struct wbsoft_priv *priv = dev->priv;
+
+	priv->enabled = true;
+
+	return 0;
+}
+
+static int wbsoft_config(struct ieee80211_hw *dev, u32 changed)
+{
+	struct wbsoft_priv *priv = dev->priv;
+	struct ieee80211_conf *conf = &dev->conf;
+	ChanInfo ch;
+
+	printk("wbsoft_config called\n");
+
+	ch.band = 1;
+	ch.ChanNo = 1;	/* Should use channel_num, or something, as that is already pre-translated */
+
+
+	hal_set_current_channel(&priv->sHwData, ch);
+	hal_set_beacon_period(&priv->sHwData, conf->beacon_int);
+//	hal_set_cap_info(&priv->sHwData, ?? );
+// hal_set_ssid(phw_data_t pHwData,  u8 * pssid,  u8 ssid_len); ??
+	hal_set_accept_broadcast(&priv->sHwData, 1);
+	hal_set_accept_promiscuous(&priv->sHwData,  1);
+	hal_set_accept_multicast(&priv->sHwData,  1);
+	hal_set_accept_beacon(&priv->sHwData,  1);
+	hal_set_radio_mode(&priv->sHwData,  0);
+	//hal_set_antenna_number(  phw_data_t pHwData, u8 number )
+	//hal_set_rf_power(phw_data_t pHwData, u8 PowerIndex)
+
+
+//	hal_start_bss(&priv->sHwData, WLAN_BSSTYPE_INFRASTRUCTURE);	??
+
+//void hal_set_rates(phw_data_t pHwData, u8 * pbss_rates,
+//		   u8 length, unsigned char basic_rate_set)
+
+	return 0;
+}
+
+static int wbsoft_config_interface(struct ieee80211_hw *dev,
+				    struct ieee80211_vif *vif,
+				    struct ieee80211_if_conf *conf)
+{
+	printk("wbsoft_config_interface called\n");
+	return 0;
+}
+
+static u64 wbsoft_get_tsf(struct ieee80211_hw *dev)
+{
+	printk("wbsoft_get_tsf called\n");
+	return 0;
+}
+
+static const struct ieee80211_ops wbsoft_ops = {
+	.tx			= wbsoft_tx,
+	.start			= wbsoft_start,		/* Start can be pretty much empty as we do wb35_hw_init() during probe? */
+	.stop			= wbsoft_stop,
+	.add_interface		= wbsoft_add_interface,
+	.remove_interface	= wbsoft_remove_interface,
+	.config			= wbsoft_config,
+	.config_interface	= wbsoft_config_interface,
+	.configure_filter	= wbsoft_configure_filter,
+	.get_stats		= wbsoft_get_stats,
+	.get_tx_stats		= wbsoft_get_tx_stats,
+	.get_tsf		= wbsoft_get_tsf,
+// conf_tx: hal_set_cwmin()/hal_set_cwmax;
+};
+
+static unsigned char wb35_hw_init(struct ieee80211_hw *hw)
+{
+	struct wbsoft_priv *priv = hw->priv;
+	phw_data_t	pHwData;
+	u8		*pMacAddr;
+	u8		*pMacAddr2;
+	u32		InitStep = 0;
+	u8		EEPROM_region;
+	u8		HwRadioOff;
+
+	//
+	// Setting default value for Linux
+	//
+	priv->sLocalPara.region_INF = REGION_AUTO;
+	priv->sLocalPara.TxRateMode = RATE_AUTO;
+	priv->sLocalPara.bMacOperationMode = MODE_802_11_BG;	// B/G mode
+	priv->Mds.TxRTSThreshold = DEFAULT_RTSThreshold;
+	priv->Mds.TxFragmentThreshold = DEFAULT_FRAGMENT_THRESHOLD;
+	hal_set_phy_type( &priv->sHwData, RF_WB_242_1 );
+	priv->sLocalPara.MTUsize = MAX_ETHERNET_PACKET_SIZE;
+	priv->sLocalPara.bPreambleMode = AUTO_MODE;
+	priv->sLocalPara.RadioOffStatus.boSwRadioOff = false;
+	pHwData = &priv->sHwData;
+	hal_set_phy_type( pHwData, RF_DECIDE_BY_INF );
+
+	//added by ws for wep key error detection
+	priv->sLocalPara.bWepKeyError= false;
+	priv->sLocalPara.bToSelfPacketReceived = false;
+	priv->sLocalPara.WepKeyDetectTimerCount= 2 * 100; /// 2 seconds
+
+	// Initial USB hal
+	InitStep = 1;
+	pHwData = &priv->sHwData;
+	if (!hal_init_hardware(hw))
+		goto error;
+
+	EEPROM_region = hal_get_region_from_EEPROM( pHwData );
+	if (EEPROM_region != REGION_AUTO)
+		priv->sLocalPara.region = EEPROM_region;
+	else {
+		if (priv->sLocalPara.region_INF != REGION_AUTO)
+			priv->sLocalPara.region = priv->sLocalPara.region_INF;
+		else
+			priv->sLocalPara.region = REGION_USA;	//default setting
+	}
+
+	// Get Software setting flag from hal
+	priv->sLocalPara.boAntennaDiversity = false;
+	if (hal_software_set(pHwData) & 0x00000001)
+		priv->sLocalPara.boAntennaDiversity = true;
+
+	//
+	// For TS module
+	//
+	InitStep = 2;
+
+	// For MDS module
+	InitStep = 3;
+	Mds_initial(priv);
+
+	//=======================================
+	// Initialize the SME, SCAN, MLME, ROAM
+	//=======================================
+	InitStep = 4;
+	InitStep = 5;
+	InitStep = 6;
+
+	// If no user-defined address in the registry, use the addresss "burned" on the NIC instead.
+	pMacAddr = priv->sLocalPara.ThisMacAddress;
+	pMacAddr2 = priv->sLocalPara.PermanentAddress;
+	hal_get_permanent_address( pHwData, priv->sLocalPara.PermanentAddress );// Reading ethernet address from EEPROM
+	if (memcmp(pMacAddr, "\x00\x00\x00\x00\x00\x00", MAC_ADDR_LENGTH) == 0)
+		memcpy(pMacAddr, pMacAddr2, MAC_ADDR_LENGTH);
+	else {
+		// Set the user define MAC address
+		hal_set_ethernet_address(pHwData, priv->sLocalPara.ThisMacAddress);
+	}
+
+	//get current antenna
+	priv->sLocalPara.bAntennaNo = hal_get_antenna_number(pHwData);
+#ifdef _PE_STATE_DUMP_
+	WBDEBUG(("Driver init, antenna no = %d\n", psLOCAL->bAntennaNo));
+#endif
+	hal_get_hw_radio_off( pHwData );
+
+	// Waiting for HAL setting OK
+	while (!hal_idle(pHwData))
+		msleep(10);
+
+	MTO_Init(priv);
+
+	HwRadioOff = hal_get_hw_radio_off( pHwData );
+	priv->sLocalPara.RadioOffStatus.boHwRadioOff = !!HwRadioOff;
+
+	hal_set_radio_mode( pHwData, (unsigned char)(priv->sLocalPara.RadioOffStatus.boSwRadioOff || priv->sLocalPara.RadioOffStatus.boHwRadioOff) );
+
+	hal_driver_init_OK(pHwData) = 1; // Notify hal that the driver is ready now.
+	//set a tx power for reference.....
+//	sme_set_tx_power_level(priv, 12);	FIXME?
+	return true;
+
+error:
+	switch (InitStep) {
+	case 5:
+	case 4:
+	case 3: Mds_Destroy( priv );
+	case 2:
+	case 1: hal_halt( pHwData, NULL );
+	case 0: break;
+	}
+
+	return false;
+}
+
+static int wb35_probe(struct usb_interface *intf, const struct usb_device_id *id_table)
+{
+	PWBUSB		pWbUsb;
+        struct usb_host_interface *interface;
+	struct usb_endpoint_descriptor *endpoint;
+	u32	ltmp;
+	struct usb_device *udev = interface_to_usbdev(intf);
+	struct wbsoft_priv *priv;
+	struct ieee80211_hw *dev;
+	int err;
+
+	usb_get_dev(udev);
+
+	// 20060630.2 Check the device if it already be opened
+	err = usb_control_msg(udev, usb_rcvctrlpipe( udev, 0 ),
+			      0x01, USB_TYPE_VENDOR|USB_RECIP_DEVICE|USB_DIR_IN,
+			      0x0, 0x400, &ltmp, 4, HZ*100 );
+	if (err)
+		goto error;
+
+	ltmp = cpu_to_le32(ltmp);
+	if (ltmp) {  // Is already initialized?
+		err = -EBUSY;
+		goto error;
+	}
+
+	dev = ieee80211_alloc_hw(sizeof(*priv), &wbsoft_ops);
+	if (!dev)
+		goto error;
+
+	priv = dev->priv;
+
+	spin_lock_init(&priv->SpinLock);
+
+	pWbUsb = &priv->sHwData.WbUsb;
+	pWbUsb->udev = udev;
+
+        interface = intf->cur_altsetting;
+        endpoint = &interface->endpoint[0].desc;
+
+	if (endpoint[2].wMaxPacketSize == 512) {
+		printk("[w35und] Working on USB 2.0\n");
+		pWbUsb->IsUsb20 = 1;
+	}
+
+	if (!wb35_hw_init(dev)) {
+		err = -EINVAL;
+		goto error_free_hw;
+	}
+
+	SET_IEEE80211_DEV(dev, &udev->dev);
+	{
+		phw_data_t pHwData = &priv->sHwData;
+		unsigned char		dev_addr[MAX_ADDR_LEN];
+		hal_get_permanent_address(pHwData, dev_addr);
+		SET_IEEE80211_PERM_ADDR(dev, dev_addr);
+	}
+
+	dev->extra_tx_headroom = 12;	/* FIXME */
+	dev->flags = 0;
+
+	dev->channel_change_time = 1000;
+	dev->queues = 1;
+
+	dev->wiphy->bands[IEEE80211_BAND_2GHZ] = &wbsoft_band_2GHz;
+
+	err = ieee80211_register_hw(dev);
+	if (err)
+		goto error_free_hw;
+
+	usb_set_intfdata(intf, priv);
+
+	return 0;
+
+error_free_hw:
+	ieee80211_free_hw(dev);
+error:
+	usb_put_dev(udev);
+	return err;
+}
+
+static void wb35_hw_halt(struct wbsoft_priv *adapter)
+{
+	Mds_Destroy( adapter );
+
+	// Turn off Rx and Tx hardware ability
+	hal_stop( &adapter->sHwData );
+#ifdef _PE_USB_INI_DUMP_
+	WBDEBUG(("[w35und] Hal_stop O.K.\n"));
+#endif
+	msleep(100);// Waiting Irp completed
+
+	// Halt the HAL
+	hal_halt(&adapter->sHwData, NULL);
+}
+
+
+static void wb35_disconnect(struct usb_interface *intf)
+{
+	struct wbsoft_priv *priv = usb_get_intfdata(intf);
+
+	wb35_hw_halt(priv);
+
+	usb_set_intfdata(intf, NULL);
+	usb_put_dev(interface_to_usbdev(intf));
+}
+
+static struct usb_driver wb35_driver = {
+	.name		= "w35und",
+	.id_table	= wb35_table,
+	.probe		= wb35_probe,
+	.disconnect	= wb35_disconnect,
+};
+
+static int __init wb35_init(void)
+{
+	return usb_register(&wb35_driver);
+}
+
+static void __exit wb35_exit(void)
+{
+	usb_deregister(&wb35_driver);
+}
+
+module_init(wb35_init);
+module_exit(wb35_exit);
diff --git a/drivers/staging/winbond/linux/wbusb_s.h b/drivers/staging/winbond/wbusb_s.h
index d5c1d53de70..1de93600d84 100644
--- a/drivers/staging/winbond/linux/wbusb_s.h
+++ b/drivers/staging/winbond/wbusb_s.h
@@ -11,9 +11,10 @@
 //
 //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
-#define OS_SLEEP( _MT )	{ set_current_state(TASK_INTERRUPTIBLE); \
-			  schedule_timeout( _MT*HZ/1000000 ); }
+#ifndef __WINBOND_WBUSB_S_H
+#define __WINBOND_WBUSB_S_H
 
+#include <linux/types.h>
 
 //---------------------------------------------------------------------------
 //  RW_CONTEXT --
@@ -23,20 +24,14 @@
 typedef struct _RW_CONTEXT
 {
 	void*			pHwData;
-	PURB			pUrb;
+	struct urb		*urb;
 	void*			pCallBackFunctionParameter;
 } RW_CONTEXT, *PRW_CONTEXT;
 
-
-
-
-#define DRIVER_AUTHOR "Original by: Jeff Lee<YY_Lee@issc.com.tw> Adapted to 2.6.x by Costantino Leandro (Rxart Desktop) <le_costantino@pixartargentina.com.ar>"
-#define DRIVER_DESC   "IS89C35 802.11bg WLAN USB Driver"
-
-
-
 typedef struct _WBUSB {
 	u32	IsUsb20;
 	struct usb_device *udev;
 	u32	DetectCount;
 } WBUSB, *PWBUSB;
+
+#endif
diff --git a/drivers/staging/wlan-ng/Kconfig b/drivers/staging/wlan-ng/Kconfig
index 2425d860dca..9959b658c8c 100644
--- a/drivers/staging/wlan-ng/Kconfig
+++ b/drivers/staging/wlan-ng/Kconfig
@@ -1,9 +1,9 @@
 config PRISM2_USB
-	tristate "Prism2.5 USB driver"
-	depends on WLAN_80211 && USB
+	tristate "Prism2.5/3 USB driver"
+	depends on WLAN_80211 && USB && WIRELESS_EXT
 	default n
 	---help---
-	  This is the wlan-ng prism 2.5 USB driver for a wide range of
+	  This is the wlan-ng prism 2.5/3 USB driver for a wide range of
 	  old USB wireless devices.
 
 	  To compile this driver as a module, choose M here: the module
diff --git a/drivers/staging/wlan-ng/Makefile b/drivers/staging/wlan-ng/Makefile
index 777b5111b3d..5edac5c8d4e 100644
--- a/drivers/staging/wlan-ng/Makefile
+++ b/drivers/staging/wlan-ng/Makefile
@@ -1,7 +1,6 @@
 obj-$(CONFIG_PRISM2_USB) += prism2_usb.o
-obj-$(CONFIG_PRISM2_USB) += p80211.o
 
-p80211-objs := p80211mod.o \
+prism2_usb-objs := prism2usb.o \
 		p80211conv.o \
 		p80211req.o \
 		p80211wep.o \
diff --git a/drivers/staging/wlan-ng/README b/drivers/staging/wlan-ng/README
index f50e4eb6c27..9c10dbb000d 100644
--- a/drivers/staging/wlan-ng/README
+++ b/drivers/staging/wlan-ng/README
@@ -3,6 +3,5 @@ TODO:
 	- sparse warnings
 	- Lindent cleanups
 	- move to use the in-kernel wireless stack
-	- possible enable the pcmcia and pci portions of the driver
 
 Please send all patches to Greg Kroah-Hartman <greg@kroah.com>
diff --git a/drivers/staging/wlan-ng/hfa384x.c b/drivers/staging/wlan-ng/hfa384x.c
deleted file mode 100644
index 04df3fd9c52..00000000000
--- a/drivers/staging/wlan-ng/hfa384x.c
+++ /dev/null
@@ -1,4018 +0,0 @@
-/* src/prism2/driver/hfa384x.c
-*
-* Implements the functions of the Intersil hfa384x MAC
-*
-* Copyright (C) 1999 AbsoluteValue Systems, Inc.  All Rights Reserved.
-* --------------------------------------------------------------------
-*
-* linux-wlan
-*
-*   The contents of this file are subject to the Mozilla Public
-*   License Version 1.1 (the "License"); you may not use this file
-*   except in compliance with the License. You may obtain a copy of
-*   the License at http://www.mozilla.org/MPL/
-*
-*   Software distributed under the License is distributed on an "AS
-*   IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or
-*   implied. See the License for the specific language governing
-*   rights and limitations under the License.
-*
-*   Alternatively, the contents of this file may be used under the
-*   terms of the GNU Public License version 2 (the "GPL"), in which
-*   case the provisions of the GPL are applicable instead of the
-*   above.  If you wish to allow the use of your version of this file
-*   only under the terms of the GPL and not to allow others to use
-*   your version of this file under the MPL, indicate your decision
-*   by deleting the provisions above and replace them with the notice
-*   and other provisions required by the GPL.  If you do not delete
-*   the provisions above, a recipient may use your version of this
-*   file under either the MPL or the GPL.
-*
-* --------------------------------------------------------------------
-*
-* Inquiries regarding the linux-wlan Open Source project can be
-* made directly to:
-*
-* AbsoluteValue Systems Inc.
-* info@linux-wlan.com
-* http://www.linux-wlan.com
-*
-* --------------------------------------------------------------------
-*
-* Portions of the development of this software were funded by
-* Intersil Corporation as part of PRISM(R) chipset product development.
-*
-* --------------------------------------------------------------------
-*
-* This file implements functions that correspond to the prism2/hfa384x
-* 802.11 MAC hardware and firmware host interface.
-*
-* The functions can be considered to represent several levels of
-* abstraction.  The lowest level functions are simply C-callable wrappers
-* around the register accesses.  The next higher level represents C-callable
-* prism2 API functions that match the Intersil documentation as closely
-* as is reasonable.  The next higher layer implements common sequences
-* of invokations of the API layer (e.g. write to bap, followed by cmd).
-*
-* Common sequences:
-* hfa384x_drvr_xxx	Highest level abstractions provided by the
-*			hfa384x code.  They are driver defined wrappers
-*			for common sequences.  These functions generally
-*			use the services of the lower levels.
-*
-* hfa384x_drvr_xxxconfig  An example of the drvr level abstraction. These
-*			functions are wrappers for the RID get/set
-*			sequence. They 	call copy_[to|from]_bap() and
-*			cmd_access().	These functions operate on the
-*			RIDs and buffers without validation.  The caller
-*			is responsible for that.
-*
-* API wrapper functions:
-* hfa384x_cmd_xxx	functions that provide access to the f/w commands.
-*			The function arguments correspond to each command
-*			argument, even command arguments that get packed
-*			into single registers.  These functions _just_
-*			issue the command by setting the cmd/parm regs
-*			& reading the status/resp regs.  Additional
-*			activities required to fully use a command
-*			(read/write from/to bap, get/set int status etc.)
-*			are implemented separately.  Think of these as
-*			C-callable prism2 commands.
-*
-* Lowest Layer Functions:
-* hfa384x_docmd_xxx	These functions implement the sequence required
-*			to issue any prism2 command.  Primarily used by the
-*			hfa384x_cmd_xxx functions.
-*
-* hfa384x_bap_xxx	BAP read/write access functions.
-*			Note: we usually use BAP0 for non-interrupt context
-*			 and BAP1 for interrupt context.
-*
-* hfa384x_dl_xxx	download related functions.
-*
-* Driver State Issues:
-* Note that there are two pairs of functions that manage the
-* 'initialized' and 'running' states of the hw/MAC combo.  The four
-* functions are create(), destroy(), start(), and stop().  create()
-* sets up the data structures required to support the hfa384x_*
-* functions and destroy() cleans them up.  The start() function gets
-* the actual hardware running and enables the interrupts.  The stop()
-* function shuts the hardware down.  The sequence should be:
-* create()
-*  .
-*  .  Self contained test routines can run here, particularly
-*  .  corereset() and test_hostif().
-*  .
-* start()
-*  .
-*  .  Do interesting things w/ the hardware
-*  .
-* stop()
-* destroy()
-*
-* Note that destroy() can be called without calling stop() first.
-* --------------------------------------------------------------------
-*/
-
-/*================================================================*/
-
-/* System Includes */
-#define WLAN_DBVAR	prism2_debug
-#include "version.h"
-
-
-#include <linux/version.h>
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/sched.h>
-#include <linux/types.h>
-#include <linux/slab.h>
-#include <linux/wireless.h>
-#include <linux/netdevice.h>
-#include <linux/timer.h>
-#include <asm/semaphore.h>
-#include <asm/io.h>
-#include <linux/delay.h>
-#include <asm/byteorder.h>
-#include <linux/list.h>
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0))
-#include <linux/tqueue.h>
-#else
-#include <linux/workqueue.h>
-#endif
-
-#if (WLAN_HOSTIF == WLAN_PCMCIA)
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,13) )
-#include <pcmcia/version.h>
-#endif
-#include <pcmcia/cs_types.h>
-#include <pcmcia/cs.h>
-#include <pcmcia/cistpl.h>
-#include <pcmcia/ds.h>
-#include <pcmcia/cisreg.h>
-#endif
-
-#if ((WLAN_HOSTIF == WLAN_PLX) || (WLAN_HOSTIF == WLAN_PCI))
-#include <linux/ioport.h>
-#include <linux/pci.h>
-#endif
-
-#include "wlan_compat.h"
-
-// XXXX #define CMD_IRQ
-
-/*================================================================*/
-/* Project Includes */
-
-#include "p80211types.h"
-#include "p80211hdr.h"
-#include "p80211mgmt.h"
-#include "p80211conv.h"
-#include "p80211msg.h"
-#include "p80211netdev.h"
-#include "p80211req.h"
-#include "p80211metadef.h"
-#include "p80211metastruct.h"
-#include "hfa384x.h"
-#include "prism2mgmt.h"
-
-/*================================================================*/
-/* Local Constants */
-
-static const UINT16 crc16tab[256] =
-{
-	0x0000, 0xc0c1, 0xc181, 0x0140, 0xc301, 0x03c0, 0x0280, 0xc241,
-	0xc601, 0x06c0, 0x0780, 0xc741, 0x0500, 0xc5c1, 0xc481, 0x0440,
-	0xcc01, 0x0cc0, 0x0d80, 0xcd41, 0x0f00, 0xcfc1, 0xce81, 0x0e40,
-	0x0a00, 0xcac1, 0xcb81, 0x0b40, 0xc901, 0x09c0, 0x0880, 0xc841,
-	0xd801, 0x18c0, 0x1980, 0xd941, 0x1b00, 0xdbc1, 0xda81, 0x1a40,
-	0x1e00, 0xdec1, 0xdf81, 0x1f40, 0xdd01, 0x1dc0, 0x1c80, 0xdc41,
-	0x1400, 0xd4c1, 0xd581, 0x1540, 0xd701, 0x17c0, 0x1680, 0xd641,
-	0xd201, 0x12c0, 0x1380, 0xd341, 0x1100, 0xd1c1, 0xd081, 0x1040,
-	0xf001, 0x30c0, 0x3180, 0xf141, 0x3300, 0xf3c1, 0xf281, 0x3240,
-	0x3600, 0xf6c1, 0xf781, 0x3740, 0xf501, 0x35c0, 0x3480, 0xf441,
-	0x3c00, 0xfcc1, 0xfd81, 0x3d40, 0xff01, 0x3fc0, 0x3e80, 0xfe41,
-	0xfa01, 0x3ac0, 0x3b80, 0xfb41, 0x3900, 0xf9c1, 0xf881, 0x3840,
-	0x2800, 0xe8c1, 0xe981, 0x2940, 0xeb01, 0x2bc0, 0x2a80, 0xea41,
-	0xee01, 0x2ec0, 0x2f80, 0xef41, 0x2d00, 0xedc1, 0xec81, 0x2c40,
-	0xe401, 0x24c0, 0x2580, 0xe541, 0x2700, 0xe7c1, 0xe681, 0x2640,
-	0x2200, 0xe2c1, 0xe381, 0x2340, 0xe101, 0x21c0, 0x2080, 0xe041,
-	0xa001, 0x60c0, 0x6180, 0xa141, 0x6300, 0xa3c1, 0xa281, 0x6240,
-	0x6600, 0xa6c1, 0xa781, 0x6740, 0xa501, 0x65c0, 0x6480, 0xa441,
-	0x6c00, 0xacc1, 0xad81, 0x6d40, 0xaf01, 0x6fc0, 0x6e80, 0xae41,
-	0xaa01, 0x6ac0, 0x6b80, 0xab41, 0x6900, 0xa9c1, 0xa881, 0x6840,
-	0x7800, 0xb8c1, 0xb981, 0x7940, 0xbb01, 0x7bc0, 0x7a80, 0xba41,
-	0xbe01, 0x7ec0, 0x7f80, 0xbf41, 0x7d00, 0xbdc1, 0xbc81, 0x7c40,
-	0xb401, 0x74c0, 0x7580, 0xb541, 0x7700, 0xb7c1, 0xb681, 0x7640,
-	0x7200, 0xb2c1, 0xb381, 0x7340, 0xb101, 0x71c0, 0x7080, 0xb041,
-	0x5000, 0x90c1, 0x9181, 0x5140, 0x9301, 0x53c0, 0x5280, 0x9241,
-	0x9601, 0x56c0, 0x5780, 0x9741, 0x5500, 0x95c1, 0x9481, 0x5440,
-	0x9c01, 0x5cc0, 0x5d80, 0x9d41, 0x5f00, 0x9fc1, 0x9e81, 0x5e40,
-	0x5a00, 0x9ac1, 0x9b81, 0x5b40, 0x9901, 0x59c0, 0x5880, 0x9841,
-	0x8801, 0x48c0, 0x4980, 0x8941, 0x4b00, 0x8bc1, 0x8a81, 0x4a40,
-	0x4e00, 0x8ec1, 0x8f81, 0x4f40, 0x8d01, 0x4dc0, 0x4c80, 0x8c41,
-	0x4400, 0x84c1, 0x8581, 0x4540, 0x8701, 0x47c0, 0x4680, 0x8641,
-	0x8201, 0x42c0, 0x4380, 0x8341, 0x4100, 0x81c1, 0x8081, 0x4040
-};
-
-/*================================================================*/
-/* Local Macros */
-
-/*================================================================*/
-/* Local Types */
-
-/*================================================================*/
-/* Local Static Definitions */
-extern int prism2_debug;
-
-/*================================================================*/
-/* Local Function Declarations */
-
-static void	hfa384x_int_dtim(wlandevice_t *wlandev);
-static void	hfa384x_int_infdrop(wlandevice_t *wlandev);
-
-static void     hfa384x_bap_tasklet(unsigned long data);
-
-static void	hfa384x_int_info(wlandevice_t *wlandev);
-static void	hfa384x_int_txexc(wlandevice_t *wlandev);
-static void	hfa384x_int_tx(wlandevice_t *wlandev);
-static void	hfa384x_int_rx(wlandevice_t *wlandev);
-
-#ifdef CMD_IRQ
-static void	hfa384x_int_cmd(wlandevice_t *wlandev);
-#endif
-static void	hfa384x_int_rxmonitor( wlandevice_t *wlandev,
-			UINT16 rxfid, hfa384x_rx_frame_t *rxdesc);
-static void	hfa384x_int_alloc(wlandevice_t *wlandev);
-
-static int hfa384x_docmd_wait( hfa384x_t *hw, hfa384x_metacmd_t *cmd);
-
-static int hfa384x_dl_docmd_wait( hfa384x_t *hw, hfa384x_metacmd_t *cmd);
-
-static UINT16
-hfa384x_mkcrc16(UINT8 *p, int len);
-
-int hfa384x_copy_to_bap4(hfa384x_t *hw, UINT16 bap, UINT16 id, UINT16 offset,
-			 void *buf, UINT len, void* buf2, UINT len2,
-			 void *buf3, UINT len3, void* buf4, UINT len4);
-
-/*================================================================*/
-/* Function Definitions */
-
-static UINT16
-txfid_queue_empty(hfa384x_t *hw)
-{
-	return (hw->txfid_head == hw->txfid_tail) ? 1 : 0;
-}
-
-static UINT16
-txfid_queue_remove(hfa384x_t *hw)
-{
-	UINT16 result= 0;
-
-	if (txfid_queue_empty(hw)) {
-		WLAN_LOG_DEBUG(3,"queue empty.\n");
-	} else {
-		result = hw->txfid_queue[hw->txfid_head];
-		hw->txfid_head = (hw->txfid_head + 1) % hw->txfid_N;
-	}
-
-	return (UINT16)result;
-}
-
-static INT16
-txfid_queue_add(hfa384x_t *hw, UINT16 val)
-{
-	INT16 result = 0;
-
-	if (hw->txfid_head == ((hw->txfid_tail + 1) % hw->txfid_N)) {
-		result = -1;
-		WLAN_LOG_DEBUG(3,"queue full.\n");
-	} else {
-		hw->txfid_queue[hw->txfid_tail] = val;
-		result = hw->txfid_tail;
-		hw->txfid_tail = (hw->txfid_tail + 1) % hw->txfid_N;
-	}
-
-	return result;
-}
-
-/*----------------------------------------------------------------
-* hfa384x_create
-*
-* Initializes the hfa384x_t data structure for use.  Note this
-* does _not_ intialize the actual hardware, just the data structures
-* we use to keep track of its state.
-*
-* Arguments:
-*	hw		device structure
-*	irq		device irq number
-*	iobase		[pcmcia] i/o base address for register access
-*			[pci] zero
-*			[plx] i/o base address for register access
-*	membase		[pcmcia] pcmcia_cs "link" pointer
-*			[pci] memory base address for register access
-*			[plx] memory base address for card attribute memory
-*
-* Returns:
-*	nothing
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-void hfa384x_create(hfa384x_t *hw, UINT irq, UINT32 iobase,
-		    UINT8 __iomem *membase)
-{
-	DBFENTER;
-	memset(hw, 0, sizeof(hfa384x_t));
-	hw->irq = irq;
-	hw->iobase = iobase;
-	hw->membase = membase;
-	spin_lock_init(&(hw->cmdlock));
-
-	/* BAP setup */
- 	spin_lock_init(&(hw->baplock));
-	tasklet_init(&hw->bap_tasklet,
-		     hfa384x_bap_tasklet,
-		     (unsigned long) hw);
-
-	init_waitqueue_head(&hw->cmdq);
-	sema_init(&hw->infofid_sem, 1);
-
-        hw->txfid_head = 0;
-        hw->txfid_tail = 0;
-        hw->txfid_N = HFA384x_DRVR_FIDSTACKLEN_MAX;
-        memset(hw->txfid_queue, 0, sizeof(hw->txfid_queue));
-
-	hw->isram16 = 1;
-
-	/* Init the auth queue head */
-	skb_queue_head_init(&hw->authq);
-
-	INIT_WORK2(&hw->link_bh, prism2sta_processing_defer);
-
-        INIT_WORK2(&hw->commsqual_bh, prism2sta_commsqual_defer);
-
-	init_timer(&hw->commsqual_timer);
-	hw->commsqual_timer.data = (unsigned long) hw;
-	hw->commsqual_timer.function = prism2sta_commsqual_timer;
-
-	hw->link_status = HFA384x_LINK_NOTCONNECTED;
-	hw->state = HFA384x_STATE_INIT;
-
-	DBFEXIT;
-}
-
-/*----------------------------------------------------------------
-* hfa384x_destroy
-*
-* Partner to hfa384x_create().  This function cleans up the hw
-* structure so that it can be freed by the caller using a simple
-* kfree.  Currently, this function is just a placeholder.  If, at some
-* point in the future, an hw in the 'shutdown' state requires a 'deep'
-* kfree, this is where it should be done.  Note that if this function
-* is called on a _running_ hw structure, the drvr_stop() function is
-* called.
-*
-* Arguments:
-*	hw		device structure
-*
-* Returns:
-*	nothing, this function is not allowed to fail.
-*
-* Side effects:
-*
-* Call context:
-*	process
-----------------------------------------------------------------*/
-void
-hfa384x_destroy( hfa384x_t *hw)
-{
-	struct sk_buff *skb;
-
-	DBFENTER;
-
-	if ( hw->state == HFA384x_STATE_RUNNING ) {
-		hfa384x_drvr_stop(hw);
-	}
-	hw->state = HFA384x_STATE_PREINIT;
-
-	if (hw->scanresults) {
-		kfree(hw->scanresults);
-		hw->scanresults = NULL;
-	}
-
-        /* Now to clean out the auth queue */
-        while ( (skb = skb_dequeue(&hw->authq)) ) {
-                dev_kfree_skb(skb);
-        }
-
-	DBFEXIT;
-	return;
-}
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_getconfig
-*
-* Performs the sequence necessary to read a config/info item.
-*
-* Arguments:
-*	hw		device structure
-*	rid		config/info record id (host order)
-*	buf		host side record buffer.  Upon return it will
-*			contain the body portion of the record (minus the
-*			RID and len).
-*	len		buffer length (in bytes, should match record length)
-*
-* Returns:
-*	0		success
-*	>0		f/w reported error - f/w status code
-*	<0		driver reported error
-*	-ENODATA 	length mismatch between argument and retrieved
-*			record.
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_drvr_getconfig(hfa384x_t *hw, UINT16 rid, void *buf, UINT16 len)
-{
-	int 		result = 0;
-	DBFENTER;
-
-	result = hfa384x_cmd_access( hw, 0, rid, buf, len);
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_setconfig
-*
-* Performs the sequence necessary to write a config/info item.
-*
-* Arguments:
-*	hw		device structure
-*	rid		config/info record id (in host order)
-*	buf		host side record buffer
-*	len		buffer length (in bytes)
-*
-* Returns:
-*	0		success
-*	>0		f/w reported error - f/w status code
-*	<0		driver reported error
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_drvr_setconfig(hfa384x_t *hw, UINT16 rid, void *buf, UINT16 len)
-{
-	int		result = 0;
-	DBFENTER;
-
-	result = hfa384x_cmd_access( hw, 1, rid, buf, len);
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_readpda
-*
-* Performs the sequence to read the PDA space.  Note there is no
-* drvr_writepda() function.  Writing a PDA is
-* generally implemented by a calling component via calls to
-* cmd_download and writing to the flash download buffer via the
-* aux regs.
-*
-* Arguments:
-*	hw		device structure
-*	buf		buffer to store PDA in
-*	len		buffer length
-*
-* Returns:
-*	0		success
-*	>0		f/w reported error - f/w status code
-*	<0		driver reported error
-*	-ETIMEOUT	timout waiting for the cmd regs to become
-*			available, or waiting for the control reg
-*			to indicate the Aux port is enabled.
-*	-ENODATA	the buffer does NOT contain a valid PDA.
-*			Either the card PDA is bad, or the auxdata
-*			reads are giving us garbage.
-
-*
-* Side effects:
-*
-* Call context:
-*	process thread or non-card interrupt.
-----------------------------------------------------------------*/
-int hfa384x_drvr_readpda(hfa384x_t *hw, void *buf, UINT len)
-{
-	int		result = 0;
-	UINT16		*pda = buf;
-	int		pdaok = 0;
-	int		morepdrs = 1;
-	int		currpdr = 0;	/* word offset of the current pdr */
-	int		i;
-	UINT16		pdrlen;		/* pdr length in bytes, host order */
-	UINT16		pdrcode;	/* pdr code, host order */
-	UINT16		crc;
-	UINT16		pdacrc;
-	struct pdaloc {
-		UINT32	cardaddr;
-		UINT16	auxctl;
-	} pdaloc[] =
-	{
-		{ HFA3842_PDA_BASE,		HFA384x_AUX_CTL_NV},
-		{ HFA3842_PDA_BASE,		HFA384x_AUX_CTL_EXTDS},
-		{ HFA3841_PDA_BASE,		HFA384x_AUX_CTL_NV},
-		{ HFA3841_PDA_BASE,		HFA384x_AUX_CTL_EXTDS},
-		{ HFA3841_PDA_BOGUS_BASE,	HFA384x_AUX_CTL_NV}
-	};
-
-	DBFENTER;
-	/* Check for aux available */
-	result = hfa384x_cmd_aux_enable(hw, 0);
-	if ( result ) {
-		WLAN_LOG_DEBUG(1,"aux_enable() failed. result=%d\n", result);
-		goto failed;
-	}
-
-	/* Read the pda from each known address.  */
-	for ( i = 0; i < (sizeof(pdaloc)/sizeof(pdaloc[0])); i++) {
-		WLAN_LOG_DEBUG( 3, "Checking PDA@(0x%08x,%s)\n",
-			pdaloc[i].cardaddr,
-			pdaloc[i].auxctl == HFA384x_AUX_CTL_NV ?
-			"CTL_NV" : "CTL_EXTDS");
-
-		/* Copy bufsize bytes from our current pdaloc */
-		hfa384x_copy_from_aux(hw,
-			pdaloc[i].cardaddr,
-			pdaloc[i].auxctl,
-			buf,
-			len);
-
-		/* Test for garbage */
-		/* Traverse the PDR list Looking for PDA-END */
-		pdaok = 1;	/* intially assume good */
-		morepdrs = 1;
-		currpdr = 0;
-		while ( pdaok && morepdrs ) {
-			pdrlen = hfa384x2host_16(pda[currpdr]) * 2;
-			pdrcode = hfa384x2host_16(pda[currpdr+1]);
-
-			/* Test for completion at END record */
-			if ( pdrcode == HFA384x_PDR_END_OF_PDA ) {
-				if ( pdrlen == 4 ) {
-					morepdrs = 0;
-					/* Calculate CRC-16 and compare to PDA
-					 * value.  Note the addition of 2 words
-					 * for ENDREC.len and ENDREC.code
-					 * fields.
-					 */
-					crc = hfa384x_mkcrc16( (UINT8*)pda,
-						(currpdr + 2) * sizeof(UINT16));
-					pdacrc =hfa384x2host_16(pda[currpdr+2]);
-					if ( crc != pdacrc ) {
-						WLAN_LOG_DEBUG(3,
-						"PDA crc failed:"
-						"calc_crc=0x%04x,"
-						"pdr_crc=0x%04x.\n",
-						crc, pdacrc);
-						pdaok = 0;
-					}
-				} else {
-					WLAN_LOG_DEBUG(3,
-					"END record detected w/ "
-					"len(%d) != 2, assuming bad PDA\n",
-					pdrlen);
-					pdaok = 0;
-
-				}
-				break;
-			}
-
-			/* Test the record length */
-			if ( pdrlen > HFA384x_PDR_LEN_MAX || pdrlen == 0) {
-				WLAN_LOG_DEBUG(3,
-					"pdrlen for address #%d "
-					"at %#x:%#x:%d\n",
-					i, pdaloc[i].cardaddr,
-					pdaloc[i].auxctl, pdrlen);
-				WLAN_LOG_DEBUG(3,"pdrlen invalid=%d\n",
-					pdrlen);
-				pdaok = 0;
-				break;
-			}
-
-			/* Move to the next pdr */
-			if ( morepdrs ) {
-				/* note the access to pda[], we need words */
-				currpdr += hfa384x2host_16(pda[currpdr]) + 1;
-				if (currpdr*sizeof(UINT16) > len) {
-					WLAN_LOG_DEBUG(3,
-					"Didn't find PDA_END in buffer, "
-					"trying next location.\n");
-					pdaok = 0;
-					break;
-				}
-			}
-		}
-		if ( pdaok ) {
-			WLAN_LOG_INFO(
-				"PDA Read from 0x%08x in %s space.\n",
-				pdaloc[i].cardaddr,
-				pdaloc[i].auxctl == 0 ? "EXTDS" :
-				pdaloc[i].auxctl == 1 ? "NV" :
-				pdaloc[i].auxctl == 2 ? "PHY" :
-				pdaloc[i].auxctl == 3 ? "ICSRAM" :
-				"<bogus auxctl>");
-			break;
-		}
-	}
-	result = pdaok ? 0 : -ENODATA;
-
-	if ( result ) {
-		WLAN_LOG_DEBUG(3,"Failure: pda is not okay\n");
-	}
-
-	hfa384x_cmd_aux_disable(hw);
-failed:
-	DBFEXIT;
-	return result;
-}
-
-
-
-/*----------------------------------------------------------------
-* mkpda_crc
-*
-* Calculates the CRC16 for the given PDA and inserts the value
-* into the end record.
-*
-* Arguments:
-*	pda	ptr to the PDA data structure.
-*
-* Returns:
-*	0	- success
-*	~0	- failure (probably an errno)
-----------------------------------------------------------------*/
-static UINT16
-hfa384x_mkcrc16(UINT8 *p, int len)
-{
-	UINT16	crc = 0;
-	UINT8	*lim = p + len;
-
-	while (p < lim) {
-		crc = (crc >> 8 ) ^ crc16tab[(crc & 0xff) ^ *p++];
-	}
-
-	return crc;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_ramdl_enable
-*
-* Begins the ram download state.  Checks to see that we're not
-* already in a download state and that a port isn't enabled.
-* Sets the download state and calls cmd_download with the
-* ENABLE_VOLATILE subcommand and the exeaddr argument.
-*
-* Arguments:
-*	hw		device structure
-*	exeaddr		the card execution address that will be
-*                       jumped to when ramdl_disable() is called
-*			(host order).
-*
-* Returns:
-*	0		success
-*	>0		f/w reported error - f/w status code
-*	<0		driver reported error
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_drvr_ramdl_enable(hfa384x_t *hw, UINT32 exeaddr)
-{
-	int		result = 0;
-	UINT16		lowaddr;
-	UINT16		hiaddr;
-	int		i;
-	DBFENTER;
-	/* Check that a port isn't active */
-	for ( i = 0; i < HFA384x_PORTID_MAX; i++) {
-		if ( hw->port_enabled[i] ) {
-			WLAN_LOG_DEBUG(1,"Can't download with a port enabled.\n");
-			result = -EINVAL;
-			goto done;
-		}
-	}
-
-	/* Check that we're not already in a download state */
-	if ( hw->dlstate != HFA384x_DLSTATE_DISABLED ) {
-		WLAN_LOG_DEBUG(1,"Download state not disabled.\n");
-		result = -EINVAL;
-		goto done;
-	}
-
-	/* Are we supposed to go into genesis mode? */
-	if (exeaddr == 0x3f0000) {
-		UINT16 initseq[2] = { 0xe100, 0xffa1 };
-		UINT16 readbuf[2];
-		UINT8 hcr = 0x0f; /* Default to x16 SRAM */
-		hw->isram16 = 1;
-
-		WLAN_LOG_DEBUG(1, "Dropping into Genesis mode\n");
-
-		/* Issue card reset and enable aux port */
-		hfa384x_corereset(hw, prism2_reset_holdtime,
-				  prism2_reset_settletime, 0);
-		hfa384x_cmd_aux_enable(hw, 1);
-
-		/* Genesis set */
-		hfa384x_copy_to_aux(hw, 0x7E0038, HFA384x_AUX_CTL_EXTDS,
-				    initseq, sizeof(initseq));
-
-		hfa384x_corereset(hw, prism2_reset_holdtime,
-				  prism2_reset_settletime, hcr);
-
-		/* Validate memory config */
-		hfa384x_copy_to_aux(hw, 0x7E0038, HFA384x_AUX_CTL_EXTDS,
-				    initseq, sizeof(initseq));
-		hfa384x_copy_from_aux(hw, 0x7E0038, HFA384x_AUX_CTL_EXTDS,
-				    readbuf, sizeof(initseq));
-		WLAN_HEX_DUMP(3, "readback", readbuf, sizeof(readbuf));
-
-		if (memcmp(initseq, readbuf, sizeof(readbuf))) {
-			hcr = 0x1f; /* x8 SRAM */
-			hw->isram16 = 0;
-
-			hfa384x_copy_to_aux(hw, 0x7E0038, HFA384x_AUX_CTL_EXTDS,
-					    initseq, sizeof(initseq));
-			hfa384x_corereset(hw, prism2_reset_holdtime,
-					  prism2_reset_settletime, hcr);
-
-			hfa384x_copy_to_aux(hw, 0x7E0038, HFA384x_AUX_CTL_EXTDS,
-					    initseq, sizeof(initseq));
-			hfa384x_copy_from_aux(hw, 0x7E0038, HFA384x_AUX_CTL_EXTDS,
-					    readbuf, sizeof(initseq));
-			WLAN_HEX_DUMP(2, "readback", readbuf, sizeof(readbuf));
-
-			if (memcmp(initseq, readbuf, sizeof(readbuf))) {
-				WLAN_LOG_ERROR("Genesis mode failed\n");
-				result = -1;
-				goto done;
-			}
-		}
-
-		/* Now we're in genesis mode */
-		hw->dlstate = HFA384x_DLSTATE_GENESIS;
-		goto done;
-	}
-
-	/* Retrieve the buffer loc&size and timeout */
-	if ( (result = hfa384x_drvr_getconfig(hw, HFA384x_RID_DOWNLOADBUFFER,
-				&(hw->bufinfo), sizeof(hw->bufinfo))) ) {
-		goto done;
-	}
-	hw->bufinfo.page = hfa384x2host_16(hw->bufinfo.page);
-	hw->bufinfo.offset = hfa384x2host_16(hw->bufinfo.offset);
-	hw->bufinfo.len = hfa384x2host_16(hw->bufinfo.len);
-	if ( (result = hfa384x_drvr_getconfig16(hw, HFA384x_RID_MAXLOADTIME,
-				&(hw->dltimeout))) ) {
-		goto done;
-	}
-	hw->dltimeout = hfa384x2host_16(hw->dltimeout);
-
-	/* Enable the aux port */
-	if ( (result = hfa384x_cmd_aux_enable(hw, 0)) ) {
-		WLAN_LOG_DEBUG(1,"Aux enable failed, result=%d.\n", result);
-		goto done;
-	}
-
-	/* Call the download(1,addr) function */
-	lowaddr = HFA384x_ADDR_CMD_MKOFF(exeaddr);
-	hiaddr =  HFA384x_ADDR_CMD_MKPAGE(exeaddr);
-
-	result = hfa384x_cmd_download(hw, HFA384x_PROGMODE_RAM,
-			lowaddr, hiaddr, 0);
-	if ( result == 0) {
-		/* Set the download state */
-		hw->dlstate = HFA384x_DLSTATE_RAMENABLED;
-	} else {
-		WLAN_LOG_DEBUG(1,"cmd_download(0x%04x, 0x%04x) failed, result=%d.\n",
-				lowaddr,hiaddr, result);
-		/* Disable  the aux port */
-		hfa384x_cmd_aux_disable(hw);
-	}
-
- done:
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_ramdl_disable
-*
-* Ends the ram download state.
-*
-* Arguments:
-*	hw		device structure
-*
-* Returns:
-*	0		success
-*	>0		f/w reported error - f/w status code
-*	<0		driver reported error
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_drvr_ramdl_disable(hfa384x_t *hw)
-{
-	DBFENTER;
-	/* Check that we're already in the download state */
-	if ( ( hw->dlstate != HFA384x_DLSTATE_RAMENABLED ) &&
-	     ( hw->dlstate != HFA384x_DLSTATE_GENESIS ) ) {
-		return -EINVAL;
-	}
-
-	if (hw->dlstate == HFA384x_DLSTATE_GENESIS) {
-		hfa384x_corereset(hw, prism2_reset_holdtime,
-				  prism2_reset_settletime,
-				  hw->isram16 ? 0x07: 0x17);
-		goto done;
-	}
-
-	/* Disable the aux port */
-	hfa384x_cmd_download(hw, HFA384x_PROGMODE_DISABLE, 0, 0 , 0);
-
- done:
-	hw->dlstate = HFA384x_DLSTATE_DISABLED;
-	hfa384x_cmd_aux_disable(hw);
-
-	DBFEXIT;
-	return 0;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_ramdl_write
-*
-* Performs a RAM download of a chunk of data. First checks to see
-* that we're in the RAM download state, then uses the aux functions
-* to 1) copy the data, 2) readback and compare.  The download
-* state is unaffected.  When all data has been written using
-* this function, call drvr_ramdl_disable() to end the download state
-* and restart the MAC.
-*
-* Arguments:
-*	hw		device structure
-*	daddr		Card address to write to. (host order)
-*	buf		Ptr to data to write.
-*	len		Length of data (host order).
-*
-* Returns:
-*	0		success
-*	>0		f/w reported error - f/w status code
-*	<0		driver reported error
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_drvr_ramdl_write(hfa384x_t *hw, UINT32 daddr, void* buf, UINT32 len)
-{
-	int		result = 0;
-	UINT8		*verbuf;
-	DBFENTER;
-	/* Check that we're in the ram download state */
-	if ( ( hw->dlstate != HFA384x_DLSTATE_RAMENABLED ) &&
-	     ( hw->dlstate != HFA384x_DLSTATE_GENESIS ) ) {
-		return -EINVAL;
-	}
-
-	WLAN_LOG_INFO("Writing %d bytes to ram @0x%06x\n", len, daddr);
-#if 0
-WLAN_HEX_DUMP(1, "dldata", buf, len);
-#endif
-	/* Copy the data via the aux port */
-	hfa384x_copy_to_aux(hw, daddr, HFA384x_AUX_CTL_EXTDS, buf, len);
-
-	/* Create a buffer for the verify */
-	verbuf = kmalloc(len, GFP_KERNEL);
-	if (verbuf == NULL ) return 1;
-
-	/* Read back and compare */
-	hfa384x_copy_from_aux(hw, daddr, HFA384x_AUX_CTL_EXTDS, verbuf, len);
-
-	if ( memcmp(buf, verbuf, len) ) {
-		WLAN_LOG_DEBUG(1,"ramdl verify failed!\n");
-		result = -EINVAL;
-	}
-
-	kfree_s(verbuf, len);
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_flashdl_enable
-*
-* Begins the flash download state.  Checks to see that we're not
-* already in a download state and that a port isn't enabled.
-* Sets the download state and retrieves the flash download
-* buffer location, buffer size, and timeout length.
-*
-* Arguments:
-*	hw		device structure
-*
-* Returns:
-*	0		success
-*	>0		f/w reported error - f/w status code
-*	<0		driver reported error
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_drvr_flashdl_enable(hfa384x_t *hw)
-{
-	int		result = 0;
-	int		i;
-
-	DBFENTER;
-	/* Check that a port isn't active */
-	for ( i = 0; i < HFA384x_PORTID_MAX; i++) {
-		if ( hw->port_enabled[i] ) {
-			WLAN_LOG_DEBUG(1,"called when port enabled.\n");
-			return -EINVAL;
-		}
-	}
-
-	/* Check that we're not already in a download state */
-	if ( hw->dlstate != HFA384x_DLSTATE_DISABLED ) {
-		return -EINVAL;
-	}
-
-	/* Retrieve the buffer loc&size and timeout */
-	if ( (result = hfa384x_drvr_getconfig(hw, HFA384x_RID_DOWNLOADBUFFER,
-				&(hw->bufinfo), sizeof(hw->bufinfo))) ) {
-		return result;
-	}
-	hw->bufinfo.page = hfa384x2host_16(hw->bufinfo.page);
-	hw->bufinfo.offset = hfa384x2host_16(hw->bufinfo.offset);
-	hw->bufinfo.len = hfa384x2host_16(hw->bufinfo.len);
-	if ( (result = hfa384x_drvr_getconfig16(hw, HFA384x_RID_MAXLOADTIME,
-				&(hw->dltimeout))) ) {
-		return result;
-	}
-	hw->dltimeout = hfa384x2host_16(hw->dltimeout);
-
-	/* Enable the aux port */
-	if ( (result = hfa384x_cmd_aux_enable(hw, 0)) ) {
-		return result;
-	}
-
-	hw->dlstate = HFA384x_DLSTATE_FLASHENABLED;
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_flashdl_disable
-*
-* Ends the flash download state.  Note that this will cause the MAC
-* firmware to restart.
-*
-* Arguments:
-*	hw		device structure
-*
-* Returns:
-*	0		success
-*	>0		f/w reported error - f/w status code
-*	<0		driver reported error
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_drvr_flashdl_disable(hfa384x_t *hw)
-{
-	DBFENTER;
-	/* Check that we're already in the download state */
-	if ( hw->dlstate != HFA384x_DLSTATE_FLASHENABLED ) {
-		return -EINVAL;
-	}
-
-	/* There isn't much we can do at this point, so I don't */
-	/*  bother  w/ the return value */
-	hfa384x_cmd_download(hw, HFA384x_PROGMODE_DISABLE, 0, 0 , 0);
-	hw->dlstate = HFA384x_DLSTATE_DISABLED;
-
-	/* Disable the aux port */
-	hfa384x_cmd_aux_disable(hw);
-
-	DBFEXIT;
-	return 0;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_flashdl_write
-*
-* Performs a FLASH download of a chunk of data. First checks to see
-* that we're in the FLASH download state, then sets the download
-* mode, uses the aux functions to 1) copy the data to the flash
-* buffer, 2) sets the download 'write flash' mode, 3) readback and
-* compare.  Lather rinse, repeat as many times an necessary to get
-* all the given data into flash.
-* When all data has been written using this function (possibly
-* repeatedly), call drvr_flashdl_disable() to end the download state
-* and restart the MAC.
-*
-* Arguments:
-*	hw		device structure
-*	daddr		Card address to write to. (host order)
-*	buf		Ptr to data to write.
-*	len		Length of data (host order).
-*
-* Returns:
-*	0		success
-*	>0		f/w reported error - f/w status code
-*	<0		driver reported error
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_drvr_flashdl_write(hfa384x_t *hw, UINT32 daddr, void* buf, UINT32 len)
-{
-	int		result = 0;
-	UINT8		*verbuf;
-	UINT32		dlbufaddr;
-	UINT32		currlen;
-	UINT32		currdaddr;
-	UINT16		destlo;
-	UINT16		desthi;
-	int		nwrites;
-	int		i;
-
-	DBFENTER;
-	/* Check that we're in the flash download state */
-	if ( hw->dlstate != HFA384x_DLSTATE_FLASHENABLED ) {
-		return -EINVAL;
-	}
-
-	WLAN_LOG_INFO("Download %d bytes to flash @0x%06x\n", len, daddr);
-
-	/* Need a flat address for arithmetic */
-	dlbufaddr = HFA384x_ADDR_AUX_MKFLAT(
-			hw->bufinfo.page,
-			hw->bufinfo.offset);
-	verbuf = kmalloc(hw->bufinfo.len, GFP_KERNEL);
-
-#if 0
-WLAN_LOG_WARNING("dlbuf@0x%06lx len=%d to=%d\n", dlbufaddr, hw->bufinfo.len, hw->dltimeout);
-#endif
-	/* Figure out how many times to to the flash prog */
-	nwrites = len / hw->bufinfo.len;
-	nwrites += (len % hw->bufinfo.len) ? 1 : 0;
-
-	if ( verbuf == NULL ) {
-		WLAN_LOG_ERROR("Failed to allocate flash verify buffer\n");
-		return 1;
-	}
-	/* For each */
-	for ( i = 0; i < nwrites; i++) {
-		/* Get the dest address and len */
-		currlen = (len - (hw->bufinfo.len * i)) > hw->bufinfo.len ?
-				hw->bufinfo.len :
-				(len - (hw->bufinfo.len * i));
-		currdaddr = daddr + (hw->bufinfo.len * i);
-		destlo = HFA384x_ADDR_CMD_MKOFF(currdaddr);
-		desthi = HFA384x_ADDR_CMD_MKPAGE(currdaddr);
-		WLAN_LOG_INFO("Writing %d bytes to flash @0x%06x\n", currlen, currdaddr);
-#if 0
-WLAN_HEX_DUMP(1, "dldata", buf+(hw->bufinfo.len*i), currlen);
-#endif
-		/* Set the download mode */
-		result = hfa384x_cmd_download(hw, HFA384x_PROGMODE_NV,
-				destlo, desthi, currlen);
-		if ( result ) {
-			WLAN_LOG_ERROR("download(NV,lo=%x,hi=%x,len=%x) "
-				"cmd failed, result=%d. Aborting d/l\n",
-				destlo, desthi, currlen, result);
-			goto exit_proc;
-		}
-		/* copy the data to the flash buffer */
-		hfa384x_copy_to_aux(hw, dlbufaddr, HFA384x_AUX_CTL_EXTDS,
-					buf+(hw->bufinfo.len*i), currlen);
-		/* set the download 'write flash' mode */
-		result = hfa384x_cmd_download(hw, HFA384x_PROGMODE_NVWRITE, 0,0,0);
-		if ( result ) {
-			WLAN_LOG_ERROR(
-				"download(NVWRITE,lo=%x,hi=%x,len=%x) "
-				"cmd failed, result=%d. Aborting d/l\n",
-				destlo, desthi, currlen, result);
-			goto exit_proc;
-		}
-		/* readback and compare, if fail...bail */
-		hfa384x_copy_from_aux(hw,
-				currdaddr, HFA384x_AUX_CTL_NV,
-				verbuf, currlen);
-
-		if ( memcmp(buf+(hw->bufinfo.len*i), verbuf, currlen) ) {
-			return -EINVAL;
-		}
-	}
-
-exit_proc:
-         /* DOH! This kfree's for you Mark :-) My forehead hurts... */
-         kfree(verbuf);
-
-	/* Leave the firmware in the 'post-prog' mode.  flashdl_disable will */
-	/*  actually disable programming mode.  Remember, that will cause the */
-	/*  the firmware to effectively reset itself. */
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_cmd_initialize
-*
-* Issues the initialize command and sets the hw->state based
-* on the result.
-*
-* Arguments:
-*	hw		device structure
-*
-* Returns:
-*	0		success
-*	>0		f/w reported error - f/w status code
-*	<0		driver reported error
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_cmd_initialize(hfa384x_t *hw)
-{
-	int result = 0;
-	int i;
-	hfa384x_metacmd_t cmd;
-
-	DBFENTER;
-
-	/* we don't want to be interrupted during the reset */
-	hfa384x_setreg(hw, 0, HFA384x_INTEN);
-	hfa384x_setreg(hw, 0xffff, HFA384x_EVACK);
-
-	cmd.cmd = HFA384x_CMDCODE_INIT;
-	cmd.parm0 = 0;
-	cmd.parm1 = 0;
-	cmd.parm2 = 0;
-
-	spin_lock_bh(&hw->cmdlock);
-	result = hfa384x_docmd_wait(hw, &cmd);
-	spin_unlock_bh(&hw->cmdlock);
-
-	if ( result == 0 ) {
-		for ( i = 0; i < HFA384x_NUMPORTS_MAX; i++) {
-			hw->port_enabled[i] = 0;
-		}
-	}
-
-        hw->link_status = HFA384x_LINK_NOTCONNECTED;
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_commtallies
-*
-* Send a commtallies inquiry to the MAC.  Note that this is an async
-* call that will result in an info frame arriving sometime later.
-*
-* Arguments:
-*	hw		device structure
-*
-* Returns:
-*	zero		success.
-*
-* Side effects:
-*
-* Call context:
-*	process
-----------------------------------------------------------------*/
-int hfa384x_drvr_commtallies( hfa384x_t *hw )
-{
-	hfa384x_metacmd_t cmd;
-	int result;
-
-	DBFENTER;
-
-	cmd.cmd = HFA384x_CMDCODE_INQ;
-	cmd.parm0 = HFA384x_IT_COMMTALLIES;
-	cmd.parm1 = 0;
-	cmd.parm2 = 0;
-
-	spin_lock_bh(&hw->cmdlock);
-	result = hfa384x_docmd_wait(hw, &cmd);
-	spin_unlock_bh(&hw->cmdlock);
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_enable
-*
-* Issues the enable command to enable communications on one of
-* the MACs 'ports'.  Only macport 0 is valid  for stations.
-* APs may also enable macports 1-6.  Only ports that are currently
-* disabled may be enabled.
-*
-* Arguments:
-*	hw		device structure
-*	macport		MAC port number
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - f/w status code
-*	<0		driver reported error (timeout|bad arg)
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_drvr_enable(hfa384x_t *hw, UINT16 macport)
-{
-	int	result = 0;
-
-	DBFENTER;
-	if ((!hw->isap && macport != 0) ||
-	    (hw->isap && !(macport <= HFA384x_PORTID_MAX)) ||
-	    (hw->port_enabled[macport]) ){
-		result = -EINVAL;
-	} else {
-		result = hfa384x_cmd_enable(hw, macport);
-		if ( result == 0 ) {
-			hw->port_enabled[macport] = 1;
-		}
-	}
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_cmd_enable
-*
-* Issues the the enable command to enable communications on one of the
-* MACs 'ports'.
-*
-* Arguments:
-*	hw		device structure
-*	macport		MAC port number
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - f/w status code
-*	<0		driver reported error (timeout|bad arg)
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_cmd_enable(hfa384x_t *hw, UINT16 macport)
-{
-	int	result = 0;
-	hfa384x_metacmd_t cmd;
-
-	DBFENTER;
-
-	cmd.cmd = HFA384x_CMD_CMDCODE_SET(HFA384x_CMDCODE_ENABLE) |
-		  HFA384x_CMD_MACPORT_SET(macport);
-	cmd.parm0 = 0;
-	cmd.parm1 = 0;
-	cmd.parm2 = 0;
-
-	spin_lock_bh(&hw->cmdlock);
-	result = hfa384x_docmd_wait(hw, &cmd);
-	spin_unlock_bh(&hw->cmdlock);
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_disable
-*
-* Issues the disable command to stop communications on one of
-* the MACs 'ports'.  Only macport 0 is valid  for stations.
-* APs may also disable macports 1-6.  Only ports that have been
-* previously enabled may be disabled.
-*
-* Arguments:
-*	hw		device structure
-*	macport		MAC port number (host order)
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - f/w status code
-*	<0		driver reported error (timeout|bad arg)
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_drvr_disable(hfa384x_t *hw, UINT16 macport)
-{
-	int	result = 0;
-
-	DBFENTER;
-	if ((!hw->isap && macport != 0) ||
-	    (hw->isap && !(macport <= HFA384x_PORTID_MAX)) ||
-	    !(hw->port_enabled[macport]) ){
-		result = -EINVAL;
-	} else {
-		result = hfa384x_cmd_disable(hw, macport);
-		if ( result == 0 ) {
-			hw->port_enabled[macport] = 0;
-		}
-	}
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_cmd_disable
-*
-* Issues the command to disable a port.
-*
-* Arguments:
-*	hw		device structure
-*	macport		MAC port number (host order)
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - f/w status code
-*	<0		driver reported error (timeout|bad arg)
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_cmd_disable(hfa384x_t *hw, UINT16 macport)
-{
-	int	result = 0;
-	hfa384x_metacmd_t cmd;
-
-	DBFENTER;
-
-	cmd.cmd = HFA384x_CMD_CMDCODE_SET(HFA384x_CMDCODE_DISABLE) |
-		  HFA384x_CMD_MACPORT_SET(macport);
-	cmd.parm0 = 0;
-	cmd.parm1 = 0;
-	cmd.parm2 = 0;
-
-	spin_lock_bh(&hw->cmdlock);
-	result = hfa384x_docmd_wait(hw, &cmd);
-	spin_unlock_bh(&hw->cmdlock);
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_cmd_diagnose
-*
-* Issues the diagnose command to test the: register interface,
-* MAC controller (including loopback), External RAM, Non-volatile
-* memory integrity, and synthesizers.  Following execution of this
-* command, MAC/firmware are in the 'initial state'.  Therefore,
-* the Initialize command should be issued after successful
-* completion of this command.  This function may only be called
-* when the MAC is in the 'communication disabled' state.
-*
-* Arguments:
-*	hw		device structure
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - f/w status code
-*	<0		driver reported error (timeout|bad arg)
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-#define DIAG_PATTERNA ((UINT16)0xaaaa)
-#define DIAG_PATTERNB ((UINT16)0x5555)
-
-int hfa384x_cmd_diagnose(hfa384x_t *hw)
-{
-	int	result = 0;
-	hfa384x_metacmd_t cmd;
-
-	DBFENTER;
-
-	cmd.cmd = HFA384x_CMD_CMDCODE_SET(HFA384x_CMDCODE_DIAG);
-	cmd.parm0 = DIAG_PATTERNA;
-	cmd.parm1 = DIAG_PATTERNB;
-	cmd.parm2 = 0;
-
-	spin_lock_bh(&hw->cmdlock);
-	result = hfa384x_docmd_wait(hw, &cmd);
-	spin_unlock_bh(&hw->cmdlock);
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_cmd_allocate
-*
-* Issues the allocate command instructing the firmware to allocate
-* a 'frame structure buffer' in MAC controller RAM.  This command
-* does not provide the result, it only initiates one of the f/w's
-* asynchronous processes to construct the buffer.  When the
-* allocation is complete, it will be indicated via the Alloc
-* bit in the EvStat register and the FID identifying the allocated
-* space will be available from the AllocFID register.  Some care
-* should be taken when waiting for the Alloc event.  If a Tx or
-* Notify command w/ Reclaim has been previously executed, it's
-* possible the first Alloc event after execution of this command
-* will be for the reclaimed buffer and not the one you asked for.
-* This case must be handled in the Alloc event handler.
-*
-* Arguments:
-*	hw		device structure
-*	len		allocation length, must be an even value
-*			in the range [4-2400]. (host order)
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - f/w status code
-*	<0		driver reported error (timeout|bad arg)
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_cmd_allocate(hfa384x_t *hw, UINT16 len)
-{
-	int	result = 0;
-	hfa384x_metacmd_t cmd;
-
-	DBFENTER;
-
-	if ( (len % 2) ||
-	     len < HFA384x_CMD_ALLOC_LEN_MIN ||
-	     len > HFA384x_CMD_ALLOC_LEN_MAX ) {
-		result = -EINVAL;
-	} else {
-		cmd.cmd = HFA384x_CMD_CMDCODE_SET(HFA384x_CMDCODE_ALLOC);
-		cmd.parm0 = len;
-		cmd.parm1 = 0;
-		cmd.parm2 = 0;
-
-		spin_lock_bh(&hw->cmdlock);
-		result = hfa384x_docmd_wait(hw, &cmd);
-		spin_unlock_bh(&hw->cmdlock);
-	}
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_cmd_transmit
-*
-* Instructs the firmware to transmit a frame previously copied
-* to a given buffer.  This function returns immediately, the Tx
-* results are available via the Tx or TxExc events (if the frame
-* control bits are set).  The reclaim argument specifies if the
-* FID passed will be used by the f/w tx process or returned for
-* use w/ another transmit command.  If reclaim is set, expect an
-* Alloc event signalling the availibility of the FID for reuse.
-*
-* NOTE: hw->cmdlock MUST BE HELD before calling this function!
-*
-* Arguments:
-*	hw		device structure
-*	reclaim		[0|1] indicates whether the given FID will
-*			be handed back (via Alloc event) for reuse.
-*			(host order)
-*	qos		[0-3] Value to put in the QoS field of the
-*			tx command, identifies a queue to place the
-*			outgoing frame in.
-*			(host order)
-*	fid		FID of buffer containing the frame that was
-*			previously copied to MAC memory via the bap.
-*			(host order)
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - f/w status code
-*	<0		driver reported error (timeout|bad arg)
-*
-* Side effects:
-*	hw->resp0 will contain the FID being used by async tx
-*	process.  If reclaim==0, resp0 will be the same as the fid
-*	argument.  If reclaim==1, resp0 will be the different and
-*	is the value to watch for in the Tx|TxExc to indicate completion
-*	of the frame passed in fid.
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_cmd_transmit(hfa384x_t *hw, UINT16 reclaim, UINT16 qos, UINT16 fid)
-{
-	int	result = 0;
-	hfa384x_metacmd_t cmd;
-
-	DBFENTER;
-	cmd.cmd = HFA384x_CMD_CMDCODE_SET(HFA384x_CMDCODE_TX) |
-		HFA384x_CMD_RECL_SET(reclaim) |
-		HFA384x_CMD_QOS_SET(qos);
-	cmd.parm0 = fid;
-	cmd.parm1 = 0;
-	cmd.parm2 = 0;
-
-	result = hfa384x_docmd_wait(hw, &cmd);
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_cmd_clearpersist
-*
-* Instructs the firmware to clear the persistence bit in a given
-* FID.  This has the effect of telling the firmware to drop the
-* persistent frame.  The FID must be one that was previously used
-* to transmit a PRST frame.
-*
-* Arguments:
-*	hw		device structure
-*	fid		FID of the persistent frame (host order)
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - f/w status code
-*	<0		driver reported error (timeout|bad arg)
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_cmd_clearpersist(hfa384x_t *hw, UINT16 fid)
-{
-	int	result = 0;
-	hfa384x_metacmd_t cmd;
-
-	DBFENTER;
-
-	cmd.cmd = HFA384x_CMD_CMDCODE_SET(HFA384x_CMDCODE_CLRPRST);
-	cmd.parm0 = fid;
-	cmd.parm1 = 0;
-	cmd.parm2 = 0;
-
-	spin_lock_bh(&hw->cmdlock);
-	result = hfa384x_docmd_wait(hw, &cmd);
-	spin_unlock_bh(&hw->cmdlock);
-
-	DBFEXIT;
-	return result;
-}
-
-/*----------------------------------------------------------------
-* hfa384x_cmd_notify
-*
-* Sends an info frame to the firmware to alter the behavior
-* of the f/w asynch processes.  Can only be called when the MAC
-* is in the enabled state.
-*
-* Arguments:
-*	hw		device structure
-*	reclaim		[0|1] indicates whether the given FID will
-*			be handed back (via Alloc event) for reuse.
-*			(host order)
-*	fid		FID of buffer containing the frame that was
-*			previously copied to MAC memory via the bap.
-*			(host order)
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - f/w status code
-*	<0		driver reported error (timeout|bad arg)
-*
-* Side effects:
-*	hw->resp0 will contain the FID being used by async notify
-*	process.  If reclaim==0, resp0 will be the same as the fid
-*	argument.  If reclaim==1, resp0 will be the different.
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_cmd_notify(hfa384x_t *hw, UINT16 reclaim, UINT16 fid,
-		       void *buf, UINT16 len)
-{
-	int	result = 0;
-	hfa384x_metacmd_t cmd;
-
-	DBFENTER;
-	cmd.cmd = HFA384x_CMD_CMDCODE_SET(HFA384x_CMDCODE_NOTIFY) |
-		HFA384x_CMD_RECL_SET(reclaim);
-	cmd.parm0 = fid;
-	cmd.parm1 = 0;
-	cmd.parm2 = 0;
-
-	spin_lock_bh(&hw->cmdlock);
-
-        /* Copy the record to FID */
-        result = hfa384x_copy_to_bap(hw, HFA384x_BAP_PROC, hw->infofid, 0, buf, len);
-        if ( result ) {
-                WLAN_LOG_DEBUG(1,
-			"copy_to_bap(%04x, 0, %d) failed, result=0x%x\n",
-                        hw->infofid, len, result);
-		result = -EIO;
-                goto failed;
-        }
-
-	result = hfa384x_docmd_wait(hw, &cmd);
-
- failed:
-	spin_unlock_bh(&hw->cmdlock);
-
-	DBFEXIT;
-	return result;
-}
-
-
-#if 0
-/*----------------------------------------------------------------
-* hfa384x_cmd_inquiry
-*
-* Requests an info frame from the firmware.  The info frame will
-* be delivered asynchronously via the Info event.
-*
-* Arguments:
-*	hw		device structure
-*	fid		FID of the info frame requested. (host order)
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - f/w status code
-*	<0		driver reported error (timeout|bad arg)
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-static int hfa384x_cmd_inquiry(hfa384x_t *hw, UINT16 fid)
-{
-	int	result = 0;
-	hfa384x_metacmd_t cmd;
-
-	DBFENTER;
-
-	cmd.cmd = HFA384x_CMD_CMDCODE_SET(HFA384x_CMDCODE_INQ);
-	cmd.parm0 = fid;
-	cmd.parm1 = 0;
-	cmd.parm2 = 0;
-
-	spin_lock_bh(&hw->cmdlock);
-	result = hfa384x_docmd_wait(hw, &cmd);
-	spin_unlock_bh(&hw->cmdlock);
-
-	DBFEXIT;
-	return result;
-}
-#endif
-
-
-/*----------------------------------------------------------------
-* hfa384x_cmd_access
-*
-* Requests that a given record be copied to/from the record
-* buffer.  If we're writing from the record buffer, the contents
-* must previously have been written to the record buffer via the
-* bap.  If we're reading into the record buffer, the record can
-* be read out of the record buffer after this call.
-*
-* Arguments:
-*	hw		device structure
-*	write		[0|1] copy the record buffer to the given
-*			configuration record. (host order)
-*	rid		RID of the record to read/write. (host order)
-*	buf		host side record buffer.  Upon return it will
-*			contain the body portion of the record (minus the
-*			RID and len).
-*	len		buffer length (in bytes, should match record length)
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - f/w status code
-*	<0		driver reported error (timeout|bad arg)
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_cmd_access(hfa384x_t *hw, UINT16 write, UINT16 rid,
-		       void* buf, UINT16 len)
-{
-	int	result = 0;
-	hfa384x_metacmd_t cmd;
-	hfa384x_rec_t	rec;
-
-	DBFENTER;
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,4,0))
-	/* This should NOT be called in interrupt context! */
-	if (in_irq()) {
-		WLAN_LOG_ERROR("Krap, in Interrupt context!");
-#ifdef WLAN_INCLUDE_DEBUG
-		BUG();
-#endif
-	}
-#endif
-	spin_lock_bh(&hw->cmdlock);
-
-	if (write) {
-		rec.rid = host2hfa384x_16(rid);
-		rec.reclen = host2hfa384x_16((len/2) + 1); /* note conversion to words, +1 for rid field */
-		/* write the record */
-		result = hfa384x_copy_to_bap4( hw, HFA384x_BAP_PROC, rid, 0,
-					       &rec, sizeof(rec),
-					       buf, len,
-					       NULL, 0, NULL, 0);
-		if ( result ) {
-			WLAN_LOG_DEBUG(3,"Failure writing record header+data\n");
-			goto fail;
-		}
-
-	}
-
-	cmd.cmd = HFA384x_CMD_CMDCODE_SET(HFA384x_CMDCODE_ACCESS) |
-		HFA384x_CMD_WRITE_SET(write);
-	cmd.parm0 = rid;
-	cmd.parm1 = 0;
-	cmd.parm2 = 0;
-
-	result = hfa384x_docmd_wait(hw, &cmd);
-	if ( result ) {
-		WLAN_LOG_ERROR("Call to hfa384x_docmd_wait failed (%d %d)\n",
-				result, cmd.result.resp0);
-		goto fail;
-	}
-
-	if (!write) {
-		result = hfa384x_copy_from_bap( hw, HFA384x_BAP_PROC, rid, 0, &rec, sizeof(rec));
-		if ( result ) {
-			WLAN_LOG_DEBUG(3,"Call to hfa384x_copy_from_bap failed\n");
-			goto fail;
-		}
-
-		/* Validate the record length */
-		if ( ((hfa384x2host_16(rec.reclen)-1)*2) != len ) {  /* note body len calculation in bytes */
-			WLAN_LOG_DEBUG(1, "RID len mismatch, rid=0x%04x hlen=%d fwlen=%d\n",
-					rid, len, (hfa384x2host_16(rec.reclen)-1)*2);
-			result = -ENODATA;
-			goto fail;
-		}
-
-		result = hfa384x_copy_from_bap( hw, HFA384x_BAP_PROC, rid, sizeof(rec), buf, len);
-
-	}
-
- fail:
-	spin_unlock_bh(&hw->cmdlock);
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_cmd_monitor
-*
-* Enables the 'monitor mode' of the MAC.  Here's the description of
-* monitor mode that I've received thus far:
-*
-*  "The "monitor mode" of operation is that the MAC passes all
-*  frames for which the PLCP checks are correct. All received
-*  MPDUs are passed to the host with MAC Port = 7, with a
-*  receive status of good, FCS error, or undecryptable. Passing
-*  certain MPDUs is a violation of the 802.11 standard, but useful
-*  for a debugging tool."  Normal communication is not possible
-*  while monitor mode is enabled.
-*
-* Arguments:
-*	hw		device structure
-*	enable		a code (0x0b|0x0f) that enables/disables
-*			monitor mode. (host order)
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - f/w status code
-*	<0		driver reported error (timeout|bad arg)
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_cmd_monitor(hfa384x_t *hw, UINT16 enable)
-{
-	int	result = 0;
-	hfa384x_metacmd_t cmd;
-
-	DBFENTER;
-
-	cmd.cmd = HFA384x_CMD_CMDCODE_SET(HFA384x_CMDCODE_MONITOR) |
-		HFA384x_CMD_AINFO_SET(enable);
-	cmd.parm0 = 0;
-	cmd.parm1 = 0;
-	cmd.parm2 = 0;
-
-	spin_lock_bh(&hw->cmdlock);
-	result = hfa384x_docmd_wait(hw, &cmd);
-	spin_unlock_bh(&hw->cmdlock);
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_cmd_download
-*
-* Sets the controls for the MAC controller code/data download
-* process.  The arguments set the mode and address associated
-* with a download.  Note that the aux registers should be enabled
-* prior to setting one of the download enable modes.
-*
-* Arguments:
-*	hw		device structure
-*	mode		0 - Disable programming and begin code exec
-*			1 - Enable volatile mem programming
-*			2 - Enable non-volatile mem programming
-*			3 - Program non-volatile section from NV download
-*			    buffer.
-*			(host order)
-*	lowaddr
-*	highaddr	For mode 1, sets the high & low order bits of
-*			the "destination address".  This address will be
-*			the execution start address when download is
-*			subsequently disabled.
-*			For mode 2, sets the high & low order bits of
-*			the destination in NV ram.
-*			For modes 0 & 3, should be zero. (host order)
-*			NOTE: these address args are in CMD format
-*	codelen		Length of the data to write in mode 2,
-*			zero otherwise. (host order)
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - f/w status code
-*	<0		driver reported error (timeout|bad arg)
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_cmd_download(hfa384x_t *hw, UINT16 mode, UINT16 lowaddr,
-				UINT16 highaddr, UINT16 codelen)
-{
-	int	result = 0;
-	hfa384x_metacmd_t cmd;
-
-	DBFENTER;
-
-	cmd.cmd = HFA384x_CMD_CMDCODE_SET(HFA384x_CMDCODE_DOWNLD) |
-		HFA384x_CMD_PROGMODE_SET(mode);
-	cmd.parm0 = lowaddr;
-	cmd.parm1 = highaddr;
-	cmd.parm2 = codelen;
-
-	spin_lock_bh(&hw->cmdlock);
-	result = hfa384x_dl_docmd_wait(hw, &cmd);
-	spin_unlock_bh(&hw->cmdlock);
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_cmd_aux_enable
-*
-* Goes through the process of enabling the auxilary port.  This
-* is necessary prior to raw reads/writes to card data space.
-* Direct access to the card data space is only used for downloading
-* code and debugging.
-* Note that a call to this function is required before attempting
-* a download.
-*
-* Arguments:
-*	hw		device structure
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - f/w status code
-*	<0		driver reported error (timeout)
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_cmd_aux_enable(hfa384x_t *hw, int force)
-{
-	int		result = -ETIMEDOUT;
-	unsigned long	flags;
-	UINT32		retries_remaining;
-	UINT16		reg;
-	UINT		auxen_mirror = hw->auxen;
-
-	DBFENTER;
-
-	/* Check for existing enable */
-	if ( hw->auxen ) {
-		hw->auxen++;
-		return 0;
-	}
-
-	/* acquire the lock */
-	spin_lock_irqsave( &(hw->cmdlock), flags);
-	/* wait for cmd register busy bit to clear */
-	retries_remaining = 100000;
-	do {
-		reg = hfa384x_getreg(hw, HFA384x_CMD);
-		udelay(10);
-	}
-	while (HFA384x_CMD_ISBUSY(reg) && --retries_remaining);
-	if (retries_remaining != 0) {
-		/* busy bit clear, it's OK to write to ParamX regs */
-		hfa384x_setreg(hw, HFA384x_AUXPW0,
-			HFA384x_PARAM0);
-		hfa384x_setreg(hw, HFA384x_AUXPW1,
-			HFA384x_PARAM1);
-		hfa384x_setreg(hw, HFA384x_AUXPW2,
-			HFA384x_PARAM2);
-
-		/* Set the aux enable in the Control register */
-		hfa384x_setreg(hw, HFA384x_CONTROL_AUX_DOENABLE,
-			HFA384x_CONTROL);
-
-		/* Now wait for completion */
-		retries_remaining = 100000;
-		do {
-			reg = hfa384x_getreg(hw, HFA384x_CONTROL);
-			udelay(10);
-		}
-		while ( ((reg & (BIT14|BIT15)) != HFA384x_CONTROL_AUX_ISENABLED) &&
-			--retries_remaining );
-		if (retries_remaining != 0) {
-			result = 0;
-			hw->auxen++;
-		}
-	}
-
-	/* Force it enabled even if the command failed, if told.. */
-	if ((hw->auxen == auxen_mirror) && force)
-		hw->auxen++;
-
-	spin_unlock_irqrestore( &(hw->cmdlock), flags);
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_cmd_aux_disable
-*
-* Goes through the process of disabling the auxilary port
-* enabled with aux_enable().
-*
-* Arguments:
-*	hw		device structure
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - f/w status code
-*	<0		driver reported error (timeout)
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_cmd_aux_disable(hfa384x_t *hw)
-{
-	int		result = -ETIMEDOUT;
-	unsigned long   timeout;
-	UINT16		reg = 0;
-
-	DBFENTER;
-
-	/* See if there's more than one enable */
-	if (hw->auxen) hw->auxen--;
-	if (hw->auxen) return 0;
-
-	/* Clear the aux enable in the Control register */
-	hfa384x_setreg(hw, 0, HFA384x_PARAM0);
-	hfa384x_setreg(hw, 0, HFA384x_PARAM1);
-	hfa384x_setreg(hw, 0, HFA384x_PARAM2);
-	hfa384x_setreg(hw, HFA384x_CONTROL_AUX_DODISABLE,
-		HFA384x_CONTROL);
-
-	/* Now wait for completion */
-	timeout = jiffies + 1*HZ;
-	reg = hfa384x_getreg(hw, HFA384x_CONTROL);
-	while ( ((reg & (BIT14|BIT15)) != HFA384x_CONTROL_AUX_ISDISABLED) &&
-		time_before(jiffies,timeout) ){
-		udelay(10);
-		reg = hfa384x_getreg(hw, HFA384x_CONTROL);
-	}
-	if ((reg & (BIT14|BIT15)) == HFA384x_CONTROL_AUX_ISDISABLED ) {
-		result = 0;
-	}
-	DBFEXIT;
-	return result;
-}
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_low_level
-*
-* Write test commands to the card.  Some test commands don't make
-* sense without prior set-up.  For example, continous TX isn't very
-* useful until you set the channel.  That functionality should be
-*
-* Side effects:
-*
-* Call context:
-*      process thread
-* -----------------------------------------------------------------*/
-int hfa384x_drvr_low_level(hfa384x_t *hw, hfa384x_metacmd_t *cmd)
-{
-	int             result = 0;
-	DBFENTER;
-
-	/* Do i need a host2hfa... conversion ? */
-#if 0
-	printk(KERN_INFO "%#x %#x %#x %#x\n", cmd->cmd, cmd->parm0, cmd->parm1, cmd->parm2);
-#endif
-	spin_lock_bh(&hw->cmdlock);
-	result = hfa384x_docmd_wait(hw, cmd);
-	spin_unlock_bh(&hw->cmdlock);
-
-	DBFEXIT;
-	return result;
-}
-
-
-/* TODO: determine if these will ever be needed */
-#if 0
-int hfa384x_cmd_readmif(hfa384x_t *hw)
-{
-	DBFENTER;
-	DBFEXIT;
-	return 0;
-}
-
-
-int hfa384x_cmd_writemif(hfa384x_t *hw)
-{
-	DBFENTER;
-	DBFEXIT;
-	return 0;
-}
-#endif
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_mmi_read
-*
-* Read mmi registers.  mmi is intersil-speak for the baseband
-* processor registers.
-*
-* Arguments:
-*       hw              device structure
-*       register        The test register to be accessed (must be even #).
-*
-* Returns:
-*       0               success
-*       >0              f/w reported error - f/w status code
-*       <0              driver reported error
-*
-* Side effects:
-*
-* Call context:
-*       process thread
-----------------------------------------------------------------*/
-int hfa384x_drvr_mmi_read(hfa384x_t *hw, UINT32 addr, UINT32 *resp)
-{
-        int             result = 0;
-	hfa384x_metacmd_t cmd;
-
-        DBFENTER;
-	cmd.cmd = (UINT16) 0x30;
-	cmd.parm0 = (UINT16) addr;
-	cmd.parm1 = 0;
-	cmd.parm2 = 0;
-
-        /* Do i need a host2hfa... conversion ? */
-	spin_lock_bh(&hw->cmdlock);
-	result = hfa384x_docmd_wait(hw, &cmd);
-	spin_unlock_bh(&hw->cmdlock);
-
-	*resp = (UINT32) cmd.result.resp0;
-
-        DBFEXIT;
-        return result;
-}
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_mmi_write
-*
-* Read mmi registers.  mmi is intersil-speak for the baseband
-* processor registers.
-*
-* Arguments:
-*       hw              device structure
-*       addr            The test register to be accessed (must be even #).
-*       data            The data value to write to the register.
-*
-* Returns:
-*       0               success
-*       >0              f/w reported error - f/w status code
-*       <0              driver reported error
-*
-* Side effects:
-*
-* Call context:
-*       process thread
-----------------------------------------------------------------*/
-
-int
-hfa384x_drvr_mmi_write(hfa384x_t *hw, UINT32 addr, UINT32 data)
-{
-        int             result = 0;
-	hfa384x_metacmd_t cmd;
-
-        DBFENTER;
-	cmd.cmd = (UINT16) 0x31;
-	cmd.parm0 = (UINT16) addr;
-	cmd.parm1 = (UINT16) data;
-	cmd.parm2 = 0;
-
-        WLAN_LOG_DEBUG(1,"mmi write : addr = 0x%08x\n", addr);
-        WLAN_LOG_DEBUG(1,"mmi write : data = 0x%08x\n", data);
-
-        /* Do i need a host2hfa... conversion ? */
-	spin_lock_bh(&hw->cmdlock);
-	result = hfa384x_docmd_wait(hw, &cmd);
-	spin_unlock_bh(&hw->cmdlock);
-
-        DBFEXIT;
-        return result;
-}
-
-
-/* TODO: determine if these will ever be needed */
-#if 0
-int hfa384x_cmd_readmif(hfa384x_t *hw)
-{
-        DBFENTER;
-        DBFEXIT;
-        return 0;
-}
-
-
-int hfa384x_cmd_writemif(hfa384x_t *hw)
-{
-        DBFENTER;
-        DBFEXIT;
-        return 0;
-}
-#endif
-
-
-
-/*----------------------------------------------------------------
-* hfa384x_copy_from_bap
-*
-* Copies a collection of bytes from the MAC controller memory via
-* one set of BAP registers.
-*
-* Arguments:
-*	hw		device structure
-*	bap		[0|1] which BAP to use
-*	id		FID or RID, destined for the select register (host order)
-*	offset		An _even_ offset into the buffer for the given
-*			FID/RID.  We haven't the means to validate this,
-*			so be careful. (host order)
-*	buf		ptr to array of bytes
-*	len		length of data to transfer in bytes
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - value of offset reg.
-*	<0		driver reported error (timeout|bad arg)
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-*	interrupt
-----------------------------------------------------------------*/
-int hfa384x_copy_from_bap(hfa384x_t *hw, UINT16 bap, UINT16 id, UINT16 offset,
-				void *buf, UINT len)
-{
-	int		result = 0;
-	unsigned long	flags = 0;
-	UINT8		*d = (UINT8*)buf;
-	UINT		selectreg;
-	UINT		offsetreg;
-	UINT		datareg;
-	UINT		i;
-	UINT16		reg = 0;
-
-	DBFENTER;
-
-	/* Validate bap, offset, buf, and len */
-	if ( (bap > 1) ||
-	     (offset > HFA384x_BAP_OFFSET_MAX) ||
-	     (offset % 2) ||
-	     (buf == NULL) ||
-	     (len > HFA384x_BAP_DATALEN_MAX) ){
-	     	result = -EINVAL;
-	} else {
-		selectreg = (bap == 1) ?  HFA384x_SELECT1 : HFA384x_SELECT0 ;
-		offsetreg = (bap == 1) ?  HFA384x_OFFSET1 : HFA384x_OFFSET0 ;
-		datareg =   (bap == 1) ?  HFA384x_DATA1 : HFA384x_DATA0 ;
-
-		/* Obtain lock */
-		spin_lock_irqsave( &(hw->baplock), flags);
-
-		/* Write id to select reg */
-		hfa384x_setreg(hw, id, selectreg);
-		/* Write offset to offset reg */
-		hfa384x_setreg(hw, offset, offsetreg);
-		/* Wait for offset[busy] to clear (see BAP_TIMEOUT) */
-		i = 0;
-		do {
-			reg = hfa384x_getreg(hw, offsetreg);
-			if ( i > 0 ) udelay(10);
-			i++;
-		} while ( i < prism2_bap_timeout && HFA384x_OFFSET_ISBUSY(reg));
-#if (WLAN_HOSTIF != WLAN_PCI)
-		/* Release lock */
-		spin_unlock_irqrestore( &(hw->baplock), flags);
-#endif
-
-		if ( HFA384x_OFFSET_ISBUSY(reg) ){
-			/* If timeout, return -ETIMEDOUT */
-			result = reg;
-		} else if ( HFA384x_OFFSET_ISERR(reg) ){
-			/* If offset[err] == 1, return -EINVAL */
-			result = reg;
-		} else {
-			/* Read even(len) buf contents from data reg */
-			for ( i = 0; i < (len & 0xfffe); i+=2 ) {
-				*(UINT16*)(&(d[i])) =
-					hfa384x_getreg_noswap(hw, datareg);
-			}
-			/* If len odd, handle last byte */
-			if ( len % 2 ){
-				reg = hfa384x_getreg_noswap(hw, datareg);
-				d[len-1] = ((UINT8*)(&reg))[0];
-			}
-		}
-
-		/* According to Intersil errata dated 9/16/02:
-
-		"In PRISM PCI MAC host interface, if both BAPs are concurrently
-		 requesing memory access, both will accept the Ack.   There is no
-		 firmware workaround possible.  To prevent BAP access failures or
-		 hang conditions the host MUST NOT access both BAPs in sucession
-		 unless at least 5us elapses between accesses.  The safest choice
-		 is to USE ONLY ONE BAP for all data movement operations."
-
-		 What this means:
-
-		 We have to serialize ALL BAP accesses, and furthermore, add a 5us
-		 delay after access if we're using a PCI platform.
-
-		 Unfortunately, this means we have to lock out interrupts througout
-		 the entire BAP copy.
-
-		 It remains to be seen if "BAP access" means "BAP setup" or the more
-		 literal definition of "copying data back and forth"  I'm erring for
-		 the latter, safer definition.  -- SLP.
-
-		*/
-
-#if (WLAN_HOSTIF == WLAN_PCI)
-		udelay(5);
-		/* Release lock */
-		spin_unlock_irqrestore( &(hw->baplock), flags);
-#endif
-
-	}
-
-	if (result) {
-	  WLAN_LOG_DEBUG(1,
-			  "copy_from_bap(0x%04x, 0, %d) failed, result=0x%x\n",
-			  reg, len, result);
-	}
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_copy_to_bap
-*
-* Copies a collection of bytes to the MAC controller memory via
-* one set of BAP registers.
-*
-* Arguments:
-*	hw		device structure
-*	bap		[0|1] which BAP to use
-*	id		FID or RID, destined for the select register (host order)
-*	offset		An _even_ offset into the buffer for the given
-*			FID/RID.  We haven't the means to validate this,
-*			so be careful. (host order)
-*	buf		ptr to array of bytes
-*	len		length of data to transfer (in bytes)
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - value of offset reg.
-*	<0		driver reported error (timeout|bad arg)
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-*	interrupt
-----------------------------------------------------------------*/
-int hfa384x_copy_to_bap(hfa384x_t *hw, UINT16 bap, UINT16 id, UINT16 offset,
-				void *buf, UINT len)
-{
-	return hfa384x_copy_to_bap4(hw, bap, id, offset, buf, len, NULL, 0, NULL, 0, NULL, 0);
-}
-
-int hfa384x_copy_to_bap4(hfa384x_t *hw, UINT16 bap, UINT16 id, UINT16 offset,
-			 void *buf, UINT len1, void* buf2, UINT len2,
-			 void *buf3, UINT len3, void *buf4, UINT len4)
-{
-	int		result = 0;
-	unsigned long	flags = 0;
-	UINT8		*d;
-	UINT		selectreg;
-	UINT		offsetreg;
-	UINT		datareg;
-	UINT		i;
-	UINT16		reg;
-
-	DBFENTER;
-
-//	printk(KERN_DEBUG "ctb1 %d id %04x o %d %d %d %d %d\n", bap, id, offset, len1, len2, len3, len4);
-
-	/* Validate bap, offset, buf, and len */
-	if ( (bap > 1) ||
-	     (offset > HFA384x_BAP_OFFSET_MAX) ||
-	     (offset % 2) ||
-	     (buf == NULL) ||
-	     (len1+len2+len3+len4 > HFA384x_BAP_DATALEN_MAX) ){
-	     	result = -EINVAL;
-	} else {
-		selectreg = (bap == 1) ? HFA384x_SELECT1 : HFA384x_SELECT0;
-		offsetreg = (bap == 1) ? HFA384x_OFFSET1 : HFA384x_OFFSET0;
-		datareg =   (bap == 1) ? HFA384x_DATA1   : HFA384x_DATA0;
-		/* Obtain lock */
-		spin_lock_irqsave( &(hw->baplock), flags);
-
-		/* Write id to select reg */
-		hfa384x_setreg(hw, id, selectreg);
-		udelay(10);
-		/* Write offset to offset reg */
-		hfa384x_setreg(hw, offset, offsetreg);
-		/* Wait for offset[busy] to clear (see BAP_TIMEOUT) */
-		i = 0;
-		do {
-			reg = hfa384x_getreg(hw, offsetreg);
-			if ( i > 0 ) udelay(10);
-			i++;
-		} while ( i < prism2_bap_timeout && HFA384x_OFFSET_ISBUSY(reg));
-
-#if (WLAN_HOSTIF != WLAN_PCI)
-		/* Release lock */
-		spin_unlock_irqrestore( &(hw->baplock), flags);
-#endif
-
-		if ( HFA384x_OFFSET_ISBUSY(reg) ){
-			/* If timeout, return reg */
-			result = reg;
-		} else if ( HFA384x_OFFSET_ISERR(reg) ){
-			/* If offset[err] == 1, return reg */
-			result = reg;
-		} else {
-			d = (UINT8*)buf;
-			/* Write even(len1) buf contents to data reg */
-			for ( i = 0; i < (len1 & 0xfffe); i+=2 ) {
-				hfa384x_setreg_noswap(hw,
-					*(UINT16*)(&(d[i])), datareg);
-			}
-			if (len1 & 1) {
-				UINT16 data;
-				UINT8 *b = (UINT8 *) &data;
-				b[0] = d[len1-1];
-				if (buf2 != NULL) {
-					d = (UINT8*)buf2;
-					b[1] = d[0];
-					len2--;
-					buf2++;
-				}
-				hfa384x_setreg_noswap(hw, data, datareg);
-			}
-			if ((buf2 != NULL) && (len2 > 0)) {
-				/* Write even(len2) buf contents to data reg */
-				d = (UINT8*)buf2;
-				for ( i = 0; i < (len2 & 0xfffe); i+=2 ) {
-					hfa384x_setreg_noswap(hw, *(UINT16*)(&(d[i])), datareg);
-				}
-				if (len2 & 1) {
-					UINT16 data;
-					UINT8 *b = (UINT8 *) &data;
-					b[0] = d[len2-1];
-					if (buf3 != NULL) {
-						d = (UINT8*)buf3;
-						b[1] = d[0];
-						len3--;
-						buf3++;
-					}
-					hfa384x_setreg_noswap(hw, data, datareg);
-				}
-			}
-
-			if ((buf3 != NULL) && (len3 > 0)) {
-				/* Write even(len3) buf contents to data reg */
-				d = (UINT8*)buf3;
-				for ( i = 0; i < (len3 & 0xfffe); i+=2 ) {
-					hfa384x_setreg_noswap(hw, *(UINT16*)(&(d[i])), datareg);
-				}
-				if (len3 & 1) {
-					UINT16 data;
-					UINT8 *b = (UINT8 *) &data;
-					b[0] = d[len3-1];
-					if (buf4 != NULL) {
-						d = (UINT8*)buf4;
-						b[1] = d[0];
-						len4--;
-						buf4++;
-					}
-					hfa384x_setreg_noswap(hw, data, datareg);
-				}
-			}
-			if ((buf4 != NULL) && (len4 > 0)) {
-				/* Write even(len4) buf contents to data reg */
-				d = (UINT8*)buf4;
-				for ( i = 0; i < (len4 & 0xfffe); i+=2 ) {
-					hfa384x_setreg_noswap(hw, *(UINT16*)(&(d[i])), datareg);
-				}
-				if (len4 & 1) {
-					UINT16 data;
-					UINT8 *b = (UINT8 *) &data;
-					b[0] = d[len4-1];
-					b[1] = 0;
-
-					hfa384x_setreg_noswap(hw, data, datareg);
-				}
-			}
-//			printk(KERN_DEBUG "ctb2 %d id %04x o %d %d %d %d %d\n", bap, id, offset, len1, len2, len3, len4);
-
-		}
-
-#if (WLAN_HOSTIF == WLAN_PCI)
-		udelay(5);
-		/* Release lock */
-		spin_unlock_irqrestore( &(hw->baplock), flags);
-#endif
-
-	}
-
-	if (result)
-		WLAN_LOG_ERROR("copy_to_bap() failed.\n");
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_copy_from_aux
-*
-* Copies a collection of bytes from the controller memory.  The
-* Auxiliary port MUST be enabled prior to calling this function.
-* We _might_ be in a download state.
-*
-* Arguments:
-*	hw		device structure
-*	cardaddr	address in hfa384x data space to read
-*	auxctl		address space select
-*	buf		ptr to destination host buffer
-*	len		length of data to transfer (in bytes)
-*
-* Returns:
-*	nothing
-*
-* Side effects:
-*	buf contains the data copied
-*
-* Call context:
-*	process thread
-*	interrupt
-----------------------------------------------------------------*/
-void
-hfa384x_copy_from_aux(
-	hfa384x_t *hw, UINT32 cardaddr, UINT32 auxctl, void *buf, UINT len)
-{
-	UINT16		currpage;
-	UINT16		curroffset;
-	UINT		i = 0;
-
-	DBFENTER;
-
-	if ( !(hw->auxen) ) {
-		WLAN_LOG_DEBUG(1,
-			"Attempt to read 0x%04x when aux not enabled\n",
-			cardaddr);
-		return;
-
-	}
-	/* Build appropriate aux page and offset */
-	currpage = HFA384x_AUX_MKPAGE(cardaddr);
-	curroffset = HFA384x_AUX_MKOFF(cardaddr, auxctl);
-	hfa384x_setreg(hw, currpage, HFA384x_AUXPAGE);
-	hfa384x_setreg(hw, curroffset, HFA384x_AUXOFFSET);
-	udelay(5);	/* beat */
-
-	/* read the data */
-	while ( i < len) {
-		*((UINT16*)(buf+i)) = hfa384x_getreg_noswap(hw, HFA384x_AUXDATA);
-		i+=2;
-		curroffset+=2;
-		if ( (curroffset&HFA384x_ADDR_AUX_OFF_MASK) >
-			HFA384x_ADDR_AUX_OFF_MAX ) {
-			currpage++;
-			curroffset = 0;
-			curroffset = HFA384x_AUX_MKOFF(curroffset, auxctl);
-			hfa384x_setreg(hw, currpage, HFA384x_AUXPAGE);
-			hfa384x_setreg(hw, curroffset, HFA384x_AUXOFFSET);
-			udelay(5);	/* beat */
-		}
-	}
-	/* Make sure the auxctl bits are clear */
-	hfa384x_setreg(hw, 0, HFA384x_AUXOFFSET);
-	DBFEXIT;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_copy_to_aux
-*
-* Copies a collection of bytes to the controller memory.  The
-* Auxiliary port MUST be enabled prior to calling this function.
-* We _might_ be in a download state.
-*
-* Arguments:
-*	hw		device structure
-*	cardaddr	address in hfa384x data space to read
-*	auxctl		address space select
-*	buf		ptr to destination host buffer
-*	len		length of data to transfer (in bytes)
-*
-* Returns:
-*	nothing
-*
-* Side effects:
-*	Controller memory now contains a copy of buf
-*
-* Call context:
-*	process thread
-*	interrupt
-----------------------------------------------------------------*/
-void
-hfa384x_copy_to_aux(
-	hfa384x_t *hw, UINT32 cardaddr, UINT32 auxctl, void *buf, UINT len)
-{
-	UINT16		currpage;
-	UINT16		curroffset;
-	UINT		i = 0;
-
-	DBFENTER;
-
-	if ( !(hw->auxen) ) {
-		WLAN_LOG_DEBUG(1,
-			"Attempt to read 0x%04x when aux not enabled\n",
-			cardaddr);
-		return;
-
-	}
-	/* Build appropriate aux page and offset */
-	currpage = HFA384x_AUX_MKPAGE(cardaddr);
-	curroffset = HFA384x_AUX_MKOFF(cardaddr, auxctl);
-	hfa384x_setreg(hw, currpage, HFA384x_AUXPAGE);
-	hfa384x_setreg(hw, curroffset, HFA384x_AUXOFFSET);
-	udelay(5);	/* beat */
-
-	/* write the data */
-	while ( i < len) {
-		hfa384x_setreg_noswap(hw,
-			*((UINT16*)(buf+i)), HFA384x_AUXDATA);
-		i+=2;
-		curroffset+=2;
-		if ( curroffset > HFA384x_ADDR_AUX_OFF_MAX ) {
-			currpage++;
-			curroffset = 0;
-			hfa384x_setreg(hw, currpage, HFA384x_AUXPAGE);
-			hfa384x_setreg(hw, curroffset, HFA384x_AUXOFFSET);
-			udelay(5);	/* beat */
-		}
-	}
-	DBFEXIT;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_cmd_wait
-*
-* Waits for availability of the Command register, then
-* issues the given command.  Then polls the Evstat register
-* waiting for command completion.  Timeouts shouldn't be
-* possible since we're preventing overlapping commands and all
-* commands should be cleared and acknowledged.
-*
-* Arguments:
-*	wlandev		device structure
-*       cmd             cmd structure.  Includes all arguments and result
-*                       data points.  All in host order.
-*
-* Returns:
-*	0		success
-*	-ETIMEDOUT	timed out waiting for register ready or
-*			command completion
-*	>0		command indicated error, Status and Resp0-2 are
-*			in hw structure.
-*
-* Side effects:
-*
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-static int hfa384x_docmd_wait( hfa384x_t *hw, hfa384x_metacmd_t *cmd)
-{
-	int		result = -ETIMEDOUT;
-	UINT16		reg = 0;
-	UINT16          counter;
-
-	DBFENTER;
-
-	hw->cmdflag = 0;
-	hw->cmddata = cmd;
-
-	/* wait for the busy bit to clear */
-	counter = 0;
-	reg = hfa384x_getreg(hw, HFA384x_CMD);
-	while ( HFA384x_CMD_ISBUSY(reg) &&
-		(counter < 10)) {
-		reg = hfa384x_getreg(hw, HFA384x_CMD);
-		counter++;
-		udelay(10);
-	}
-
-	if (HFA384x_CMD_ISBUSY(reg)) {
-		WLAN_LOG_ERROR("hfa384x_cmd timeout(1), reg=0x%0hx.\n", reg);
-		goto failed;
-	}
-	if (!HFA384x_CMD_ISBUSY(reg)) {
-		/* busy bit clear, write command */
-		hfa384x_setreg(hw, cmd->parm0, HFA384x_PARAM0);
-		hfa384x_setreg(hw, cmd->parm1, HFA384x_PARAM1);
-		hfa384x_setreg(hw, cmd->parm2, HFA384x_PARAM2);
-		hfa384x_setreg(hw, cmd->cmd, HFA384x_CMD);
-
-#ifdef CMD_IRQ
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,2,0))
-		while (! hw->cmdflag)
-			interruptible_sleep_on(&hw->cmdq);
-#else
-		wait_event_interruptible(hw->cmdq, hw->cmdflag);
-#endif
-		result = HFA384x_STATUS_RESULT_GET(cmd->status);
-#else // CMD_IRQ
-		/* Now wait for completion */
-		counter = 0;
-		reg = hfa384x_getreg(hw, HFA384x_EVSTAT);
-		/* Initialization is the problem.  It takes about
-                   100ms. "normal" commands are typically is about
-                   200-400 us (I've never seen less than 200).  Longer
-                   is better so that we're not hammering the bus. */
-		while ( !HFA384x_EVSTAT_ISCMD(reg) &&
-			(counter < 5000)) {
-			reg = hfa384x_getreg(hw, HFA384x_EVSTAT);
-			counter++;
-			udelay(200);
-		}
-
-		if ( HFA384x_EVSTAT_ISCMD(reg) ) {
-			result = 0;
-			cmd->result.status = hfa384x_getreg(hw, HFA384x_STATUS);
-			cmd->result.resp0 = hfa384x_getreg(hw, HFA384x_RESP0);
-			cmd->result.resp1 = hfa384x_getreg(hw, HFA384x_RESP1);
-			cmd->result.resp2 = hfa384x_getreg(hw, HFA384x_RESP2);
-			hfa384x_setreg(hw, HFA384x_EVACK_CMD,
-				HFA384x_EVACK);
-			result = HFA384x_STATUS_RESULT_GET(cmd->result.status);
-		} else {
-			WLAN_LOG_ERROR("hfa384x_cmd timeout(2), reg=0x%0hx.\n", reg);
-		}
-#endif  /* CMD_IRQ */
-	}
-
- failed:
-	hw->cmdflag = 0;
-	hw->cmddata = NULL;
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_dl_docmd_wait
-*
-* Waits for availability of the Command register, then
-* issues the given command.  Then polls the Evstat register
-* waiting for command completion.  Timeouts shouldn't be
-* possible since we're preventing overlapping commands and all
-* commands should be cleared and acknowledged.
-*
-* This routine is only used for downloads.  Since it doesn't lock out
-* interrupts the system response is much better.
-*
-* Arguments:
-*	wlandev		device structure
-*       cmd             cmd structure.  Includes all arguments and result
-*                       data points.  All in host order.
-*
-* Returns:
-*	0		success
-*	-ETIMEDOUT	timed out waiting for register ready or
-*			command completion
-*	>0		command indicated error, Status and Resp0-2 are
-*			in hw structure.
-*
-* Side effects:
-*
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-static int hfa384x_dl_docmd_wait( hfa384x_t *hw, hfa384x_metacmd_t *cmd)
-{
-	int		result = -ETIMEDOUT;
-	unsigned long	timeout;
-	UINT16		reg = 0;
-
-	DBFENTER;
-	/* wait for the busy bit to clear */
-	timeout = jiffies + 1*HZ;
-	reg = hfa384x_getreg(hw, HFA384x_CMD);
-	while ( HFA384x_CMD_ISBUSY(reg) && time_before( jiffies, timeout) ) {
-		reg = hfa384x_getreg(hw, HFA384x_CMD);
-		udelay(10);
-	}
-	if (HFA384x_CMD_ISBUSY(reg)) {
-		WLAN_LOG_WARNING("Timed out waiting for cmd register.\n");
-		goto failed;
-	}
-
-	if (!HFA384x_CMD_ISBUSY(reg)) {
-		/* busy bit clear, write command */
-		hfa384x_setreg(hw, cmd->parm0, HFA384x_PARAM0);
-		hfa384x_setreg(hw, cmd->parm1, HFA384x_PARAM1);
-		hfa384x_setreg(hw, cmd->parm2, HFA384x_PARAM2);
-		hfa384x_setreg(hw, cmd->cmd, HFA384x_CMD);
-
-		/* Now wait for completion */
-		if ( (HFA384x_CMD_CMDCODE_GET(cmd->cmd) == HFA384x_CMDCODE_DOWNLD) ) {
-			/* dltimeout is in ms */
-			timeout = (((UINT32)hw->dltimeout) / 1000UL) * HZ;
-			if ( timeout > 0 ) {
-				timeout += jiffies;
-			} else {
-				timeout = jiffies + 1*HZ;
-			}
-		} else {
-			timeout = jiffies + 1*HZ;
-		}
-		reg = hfa384x_getreg(hw, HFA384x_EVSTAT);
-		while ( !HFA384x_EVSTAT_ISCMD(reg) && time_before(jiffies,timeout) ) {
-			udelay(100);
-			reg = hfa384x_getreg(hw, HFA384x_EVSTAT);
-		}
-		if ( HFA384x_EVSTAT_ISCMD(reg) ) {
-			result = 0;
-			cmd->result.status = hfa384x_getreg(hw, HFA384x_STATUS);
-			cmd->result.resp0 = hfa384x_getreg(hw, HFA384x_RESP0);
-			cmd->result.resp1 = hfa384x_getreg(hw, HFA384x_RESP1);
-			cmd->result.resp2 = hfa384x_getreg(hw, HFA384x_RESP2);
-			hfa384x_setreg(hw, HFA384x_EVACK_CMD, HFA384x_EVACK);
-			result = HFA384x_STATUS_RESULT_GET(cmd->result.status);
-		}
-	}
-
-failed:
-	DBFEXIT;
-	return result;
-}
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_start
-*
-* Issues the MAC initialize command, sets up some data structures,
-* and enables the interrupts.  After this function completes, the
-* low-level stuff should be ready for any/all commands.
-*
-* Arguments:
-*	hw		device structure
-* Returns:
-*	0		success
-*	>0		f/w reported error - f/w status code
-*	<0		driver reported error
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_drvr_start(hfa384x_t *hw)
-{
-	int	result = 0;
-	UINT16			reg;
-	int			i;
-	int			j;
-	DBFENTER;
-
-	/* call initialize */
-	result = hfa384x_cmd_initialize(hw);
-	if (result != 0) {
-		WLAN_LOG_ERROR("Initialize command failed.\n");
-		goto failed;
-	}
-
-	/* make sure interrupts are disabled and any layabout events cleared */
-	hfa384x_setreg(hw, 0, HFA384x_INTEN);
-	hfa384x_setreg(hw, 0xffff, HFA384x_EVACK);
-
-        hw->txfid_head = 0;
-        hw->txfid_tail = 0;
-        hw->txfid_N = HFA384x_DRVR_FIDSTACKLEN_MAX;
-        memset(hw->txfid_queue, 0, sizeof(hw->txfid_queue));
-
-	/* Allocate tx and notify FIDs */
-	/* First, tx */
-	for ( i = 0; i < HFA384x_DRVR_FIDSTACKLEN_MAX-1; i++) {
-		result = hfa384x_cmd_allocate(hw, HFA384x_DRVR_TXBUF_MAX);
-		if (result != 0) {
-			WLAN_LOG_ERROR("Allocate(tx) command failed.\n");
-			goto failed;
-		}
-		j = 0;
-		do {
-			reg = hfa384x_getreg(hw, HFA384x_EVSTAT);
-			udelay(10);
-			j++;
-		} while ( !HFA384x_EVSTAT_ISALLOC(reg) && j < 50); /* 50 is timeout */
-		if ( j >= 50 ) {
-			WLAN_LOG_ERROR("Timed out waiting for evalloc(tx).\n");
-			result = -ETIMEDOUT;
-			goto failed;
-		}
-		reg = hfa384x_getreg(hw, HFA384x_ALLOCFID);
-
-		txfid_queue_add(hw, reg);
-
-		WLAN_LOG_DEBUG(4,"hw->txfid_queue[%d]=0x%04x\n",i,reg);
-
-		reg = HFA384x_EVACK_ALLOC_SET(1);
-		hfa384x_setreg(hw, reg, HFA384x_EVACK);
-
-	}
-
-	/* Now, the info frame fid */
-	result = hfa384x_cmd_allocate(hw, HFA384x_INFOFRM_MAXLEN);
-	if (result != 0) {
-		WLAN_LOG_ERROR("Allocate(tx) command failed.\n");
-		goto failed;
-	}
-	i = 0;
-	do {
-		reg = hfa384x_getreg(hw, HFA384x_EVSTAT);
-		udelay(10);
-		i++;
-	} while ( !HFA384x_EVSTAT_ISALLOC(reg) && i < 50); /* 50 is timeout */
-	if ( i >= 50 ) {
-		WLAN_LOG_ERROR("Timed out waiting for evalloc(info).\n");
-		result = -ETIMEDOUT;
-		goto failed;
-	}
-	hw->infofid = hfa384x_getreg(hw, HFA384x_ALLOCFID);
-	reg = HFA384x_EVACK_ALLOC_SET(1);
-	hfa384x_setreg(hw, reg, HFA384x_EVACK);
-	WLAN_LOG_DEBUG(4,"hw->infofid=0x%04x\n", hw->infofid);
-
-	/* Set swsupport regs to magic # for card presence detection */
-	hfa384x_setreg(hw, HFA384x_DRVR_MAGIC, HFA384x_SWSUPPORT0);
-
-	/* Now enable the interrupts and set the running state */
-	hfa384x_setreg(hw, 0xffff, HFA384x_EVSTAT);
-	hfa384x_events_all(hw);
-
-	hw->state = HFA384x_STATE_RUNNING;
-
-	goto done;
-failed:
-	WLAN_LOG_ERROR("Failed, result=%d\n", result);
-done:
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_stop
-*
-* Issues the initialize command to leave us in the 'reset' state.
-*
-* Arguments:
-*	hw		device structure
-* Returns:
-*	0		success
-*	>0		f/w reported error - f/w status code
-*	<0		driver reported error
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_drvr_stop(hfa384x_t *hw)
-{
-	int	result = 0;
-	int i;
-	DBFENTER;
-
-	del_timer_sync(&hw->commsqual_timer);
-
-	if ( hw->wlandev->hwremoved ) {
-		/* only flush when we're shutting down for good */
-		flush_scheduled_work();
-	}
-
-	if (hw->state == HFA384x_STATE_RUNNING) {
-		/*
-		 * Send the MAC initialize cmd.
-		 */
-		hfa384x_cmd_initialize(hw);
-
-		/*
-		 * Make absolutely sure interrupts are disabled and any
-		 * layabout events cleared
-		 */
-		hfa384x_setreg(hw, 0, HFA384x_INTEN);
-		hfa384x_setreg(hw, 0xffff, HFA384x_EVACK);
-	}
-
-	tasklet_kill(&hw->bap_tasklet);
-
-	hw->link_status = HFA384x_LINK_NOTCONNECTED;
-	hw->state = HFA384x_STATE_INIT;
-
-	/* Clear all the port status */
-	for ( i = 0; i < HFA384x_NUMPORTS_MAX; i++) {
-		hw->port_enabled[i] = 0;
-	}
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_txframe
-*
-* Takes a frame from prism2sta and queues it for transmission.
-*
-* Arguments:
-*	hw		device structure
-*	skb		packet buffer struct.  Contains an 802.11
-*			data frame.
-*       p80211_hdr      points to the 802.11 header for the packet.
-* Returns:
-*	0		Success and more buffs available
-*	1		Success but no more buffs
-*	2		Allocation failure
-*	3		MAC Tx command failed
-*	4		Buffer full or queue busy
-*
-* Side effects:
-*
-* Call context:
-*	process thread
-----------------------------------------------------------------*/
-int hfa384x_drvr_txframe(hfa384x_t *hw, struct sk_buff *skb, p80211_hdr_t *p80211_hdr, p80211_metawep_t *p80211_wep)
-{
-	hfa384x_tx_frame_t	txdesc;
-	UINT16			macq = 0;
-	UINT16			fid;
-	int			result;
-
-	DBFENTER;
-
-	/* Build Tx frame structure */
-	/* Set up the control field */
-	memset(&txdesc, 0, sizeof(txdesc));
-
-/* Tx complete and Tx exception disable per dleach.  Might be causing
- * buf depletion
- */
-#define DOBOTH 1
-#if DOBOTH
-	txdesc.tx_control =
-		HFA384x_TX_MACPORT_SET(0) | HFA384x_TX_STRUCTYPE_SET(1) |
-		HFA384x_TX_TXEX_SET(1) | HFA384x_TX_TXOK_SET(1);
-#elif DOEXC
-	txdesc.tx_control =
-		HFA384x_TX_MACPORT_SET(0) | HFA384x_TX_STRUCTYPE_SET(1) |
-		HFA384x_TX_TXEX_SET(1) | HFA384x_TX_TXOK_SET(0);
-#else
-	txdesc.tx_control =
-		HFA384x_TX_MACPORT_SET(0) | HFA384x_TX_STRUCTYPE_SET(1) |
-		HFA384x_TX_TXEX_SET(0) | HFA384x_TX_TXOK_SET(0);
-#endif
-
-	/* if we're using host WEP, increase size by IV+ICV */
-	if (p80211_wep->data) {
-		txdesc.data_len = host2hfa384x_16(skb->len+8);
-		//		txdesc.tx_control |= HFA384x_TX_NOENCRYPT_SET(1);
-	} else {
-		txdesc.data_len =  host2hfa384x_16(skb->len);
-	}
-
-	txdesc.tx_control = host2hfa384x_16(txdesc.tx_control);
-	/* copy the header over to the txdesc */
-	memcpy(&(txdesc.frame_control), p80211_hdr, sizeof(p80211_hdr_t));
-
-	/* Since tbusy is set whenever the stack is empty, there should
-	 * always be something on the stack if we get to this point.
-	 * [MSM]: NOT TRUE!!!!! so I added the test of fid below.
-	 */
-
-	/* Allocate FID */
-
-	fid = txfid_queue_remove(hw);
-
-	if ( fid == 0 ) { /* stack or queue was empty */
-		return 4;
-	}
-
-	/* now let's get the cmdlock */
-	spin_lock(&hw->cmdlock);
-
-	/* Copy descriptor+payload to FID */
-        if (p80211_wep->data) {
-		result = hfa384x_copy_to_bap4(hw, HFA384x_BAP_PROC, fid, 0,
-					      &txdesc, sizeof(txdesc),
-					      p80211_wep->iv, sizeof(p80211_wep->iv),
-					      p80211_wep->data, skb->len,
-					      p80211_wep->icv, sizeof(p80211_wep->icv));
-	} else {
-		result = hfa384x_copy_to_bap4(hw, HFA384x_BAP_PROC, fid, 0,
-					      &txdesc, sizeof(txdesc),
-					      skb->data, skb->len,
-					      NULL, 0, NULL, 0);
-	}
-
-	if ( result ) {
-		WLAN_LOG_DEBUG(1,
-			"copy_to_bap(%04x, %d, %d) failed, result=0x%x\n",
-			fid,
-		 	sizeof(txdesc),
-	 		skb->len,
-			result);
-
-		/* put the fid back in the queue */
-		txfid_queue_add(hw, fid);
-
-		result = 3;
-		goto failed;
-	}
-
-	/* Issue Tx command */
-	result = hfa384x_cmd_transmit(hw, HFA384x_TXCMD_RECL, macq, fid);
-
-	if ( result != 0 ) {
-		txfid_queue_add(hw, fid);
-
-		WLAN_LOG_DEBUG(1,"cmd_tx(%04x) failed, result=%d\n",
-			fid, result);
-		result = 3;
-		goto failed;
-	}
-
-	/* indicate we haven't any buffers, int_alloc will clear */
-	result = txfid_queue_empty(hw);
-failed:
-
-	spin_unlock(&hw->cmdlock);
-
-	DBFEXIT;
-	return result;
-}
-
-/*----------------------------------------------------------------
-* hfa384x_interrupt
-*
-* Driver interrupt handler.
-*
-* Arguments:
-*	irq		irq number
-*	dev_id		pointer to the device
-*	regs		registers
-*
-* Returns:
-*	nothing
-*
-* Side effects:
-*	May result in a frame being passed up the stack or an info
-*	frame being handled.
-*
-* Call context:
-*	Ummm, could it be interrupt?
-----------------------------------------------------------------*/
-irqreturn_t hfa384x_interrupt(int irq, void *dev_id PT_REGS)
-{
-	int			reg;
-	wlandevice_t		*wlandev = (wlandevice_t*)dev_id;
-	hfa384x_t		*hw = wlandev->priv;
-	int			ev_read = 0;
-	DBFENTER;
-
-	if (!wlandev || wlandev->hwremoved)
-		return IRQ_NONE;  /* Not much we can do w/o hardware */
-#if (WLAN_HOSTIF == WLAN_PCMCIA)
-	if (hw->iobase == 0)  /* XXX FIXME Properly */
-		return IRQ_NONE;
-#endif
-
-	for (;;ev_read++) {
-		if (ev_read >= prism2_irq_evread_max)
-			break;
-
-		/* Check swsupport reg magic # for card presence */
-		reg = hfa384x_getreg(hw, HFA384x_SWSUPPORT0);
-		if ( reg != HFA384x_DRVR_MAGIC) {
-			WLAN_LOG_DEBUG(2, "irq=%d, no magic.  Card removed?.\n", irq);
-			break;
-		}
-
-		/* read the EvStat register for interrupt enabled events */
-		reg = hfa384x_getreg(hw, HFA384x_EVSTAT);
-
-		/* AND with the enabled interrupts */
-		reg &= hfa384x_getreg(hw, HFA384x_INTEN);
-
-		/* Handle the events */
-		if ( HFA384x_EVSTAT_ISWTERR(reg) ){
-			WLAN_LOG_ERROR(
-			"Error: WTERR interrupt received (unhandled).\n");
-			hfa384x_setreg(hw, HFA384x_EVACK_WTERR_SET(1),
-				HFA384x_EVACK);
-		}
-
-		if ( HFA384x_EVSTAT_ISINFDROP(reg) ){
-			hfa384x_int_infdrop(wlandev);
-			hfa384x_setreg(hw, HFA384x_EVACK_INFDROP_SET(1),
-				HFA384x_EVACK);
-		}
-
-		if (HFA384x_EVSTAT_ISBAP_OP(reg)) {
-			/* Disable the BAP interrupts */
-			hfa384x_events_nobap(hw);
-			tasklet_schedule(&hw->bap_tasklet);
-		}
-
-		if ( HFA384x_EVSTAT_ISALLOC(reg) ){
-			hfa384x_int_alloc(wlandev);
-			hfa384x_setreg(hw, HFA384x_EVACK_ALLOC_SET(1),
-				HFA384x_EVACK);
-		}
-
-		if ( HFA384x_EVSTAT_ISDTIM(reg) ){
-			hfa384x_int_dtim(wlandev);
-			hfa384x_setreg(hw, HFA384x_EVACK_DTIM_SET(1),
-				HFA384x_EVACK);
-		}
-#ifdef CMD_IRQ
-		if ( HFA384x_EVSTAT_ISCMD(reg) ){
-			hfa384x_int_cmd(wlandev);
-			hfa384x_setreg(hw, HFA384x_EVACK_CMD_SET(1),
-				       HFA384x_EVACK);
-		}
-#endif
-
-		/* allow the evstat to be updated after the evack */
-		udelay(20);
-	}
-
-	DBFEXIT;
-	return IRQ_HANDLED;
-}
-
-#ifdef CMD_IRQ
-/*----------------------------------------------------------------
-* hfa384x_int_cmd
-*
-* Handles command completion event.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*
-* Returns:
-*	nothing
-*
-* Side effects:
-*
-* Call context:
-*	interrupt
-----------------------------------------------------------------*/
-void hfa384x_int_cmd(wlandevice_t *wlandev)
-{
-	hfa384x_t		*hw = wlandev->priv;
-	DBFENTER;
-
-	// check to make sure it's the right command?
-	if (hw->cmddata) {
-		hw->cmddata->status = hfa384x_getreg(hw, HFA384x_STATUS);
-		hw->cmddata->resp0 = hfa384x_getreg(hw, HFA384x_RESP0);
-		hw->cmddata->resp1 = hfa384x_getreg(hw, HFA384x_RESP1);
-		hw->cmddata->resp2 = hfa384x_getreg(hw, HFA384x_RESP2);
-	}
-	hw->cmdflag = 1;
-
-	printk(KERN_INFO "um. int_cmd\n");
-
-	wake_up_interruptible(&hw->cmdq);
-
-	// XXXX perform a bap copy too?
-
-	DBFEXIT;
-	return;
-}
-#endif
-
-/*----------------------------------------------------------------
-* hfa384x_int_dtim
-*
-* Handles the DTIM early warning event.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*
-* Returns:
-*	nothing
-*
-* Side effects:
-*
-* Call context:
-*	interrupt
-----------------------------------------------------------------*/
-static void hfa384x_int_dtim(wlandevice_t *wlandev)
-{
-#if 0
-	hfa384x_t		*hw = wlandev->priv;
-#endif
-	DBFENTER;
-	prism2sta_ev_dtim(wlandev);
-	DBFEXIT;
-	return;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_int_infdrop
-*
-* Handles the InfDrop event.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*
-* Returns:
-*	nothing
-*
-* Side effects:
-*
-* Call context:
-*	interrupt
-----------------------------------------------------------------*/
-static void hfa384x_int_infdrop(wlandevice_t *wlandev)
-{
-#if 0
-	hfa384x_t		*hw = wlandev->priv;
-#endif
-	DBFENTER;
-	prism2sta_ev_infdrop(wlandev);
-	DBFEXIT;
-	return;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_int_info
-*
-* Handles the Info event.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*
-* Returns:
-*	nothing
-*
-* Side effects:
-*
-* Call context:
-*	tasklet
-----------------------------------------------------------------*/
-static void hfa384x_int_info(wlandevice_t *wlandev)
-{
-	hfa384x_t		*hw = wlandev->priv;
-	UINT16			reg;
-	hfa384x_InfFrame_t	inf;
-	int			result;
-	DBFENTER;
-	/* Retrieve the FID */
-	reg = hfa384x_getreg(hw, HFA384x_INFOFID);
-
-	/* Retrieve the length */
-	result = hfa384x_copy_from_bap( hw,
-		HFA384x_BAP_INT, reg, 0, &inf.framelen, sizeof(UINT16));
-	if ( result ) {
-		WLAN_LOG_DEBUG(1,
-			"copy_from_bap(0x%04x, 0, %d) failed, result=0x%x\n",
-			reg, sizeof(inf), result);
-		goto failed;
-	}
-	inf.framelen = hfa384x2host_16(inf.framelen);
-
-	/* Retrieve the rest */
-	result = hfa384x_copy_from_bap( hw,
-		HFA384x_BAP_INT, reg, sizeof(UINT16),
-		&(inf.infotype), inf.framelen * sizeof(UINT16));
-	if ( result ) {
-		WLAN_LOG_DEBUG(1,
-			"copy_from_bap(0x%04x, 0, %d) failed, result=0x%x\n",
-			reg, sizeof(inf), result);
-		goto failed;
-	}
-
-	prism2sta_ev_info(wlandev, &inf);
-failed:
-	DBFEXIT;
-	return;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_int_txexc
-*
-* Handles the TxExc event.  A Transmit Exception event indicates
-* that the MAC's TX process was unsuccessful - so the packet did
-* not get transmitted.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*
-* Returns:
-*	nothing
-*
-* Side effects:
-*
-* Call context:
-*	tasklet
-----------------------------------------------------------------*/
-static void hfa384x_int_txexc(wlandevice_t *wlandev)
-{
-	hfa384x_t		*hw = wlandev->priv;
-	UINT16			status;
-	UINT16			fid;
-	int			result = 0;
-	DBFENTER;
-	/* Collect the status and display */
-	fid = hfa384x_getreg(hw, HFA384x_TXCOMPLFID);
-	result = hfa384x_copy_from_bap(hw, HFA384x_BAP_INT, fid, 0, &status, sizeof(status));
-	if ( result ) {
-		WLAN_LOG_DEBUG(1,
-			"copy_from_bap(0x%04x, 0, %d) failed, result=0x%x\n",
-			fid, sizeof(status), result);
-		goto failed;
-	}
-	status = hfa384x2host_16(status);
-	prism2sta_ev_txexc(wlandev, status);
-failed:
-	DBFEXIT;
-	return;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_int_tx
-*
-* Handles the Tx event.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*
-* Returns:
-*	nothing
-*
-* Side effects:
-*
-* Call context:
-*	tasklet
-----------------------------------------------------------------*/
-static void hfa384x_int_tx(wlandevice_t *wlandev)
-{
-	hfa384x_t		*hw = wlandev->priv;
-	UINT16			fid;
-	UINT16			status;
-	int			result = 0;
-	DBFENTER;
-	fid = hfa384x_getreg(hw, HFA384x_TXCOMPLFID);
-	result = hfa384x_copy_from_bap(hw, HFA384x_BAP_INT, fid, 0, &status, sizeof(status));
-	if ( result ) {
-		WLAN_LOG_DEBUG(1,
-			"copy_from_bap(0x%04x, 0, %d) failed, result=0x%x\n",
-			fid, sizeof(status), result);
-		goto failed;
-	}
-	status = hfa384x2host_16(status);
-	prism2sta_ev_tx(wlandev, status);
-failed:
-	DBFEXIT;
-	return;
-}
-
-/*----------------------------------------------------------------
-* hfa384x_int_rx
-*
-* Handles the Rx event.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*
-* Returns:
-*	nothing
-*
-* Side effects:
-*
-* Call context:
-*	tasklet
-----------------------------------------------------------------*/
-static void hfa384x_int_rx(wlandevice_t *wlandev)
-{
-	hfa384x_t		*hw = wlandev->priv;
-	UINT16			rxfid;
-	hfa384x_rx_frame_t	rxdesc;
-	int			result;
-	int                     hdrlen;
-	UINT16                  fc;
-	p80211_rxmeta_t	*rxmeta;
-	struct sk_buff          *skb = NULL;
-	UINT8 *datap;
-
-	DBFENTER;
-
-	/* Get the FID */
-	rxfid = hfa384x_getreg(hw, HFA384x_RXFID);
-	/* Get the descriptor (including headers) */
-	result = hfa384x_copy_from_bap(hw,
-			HFA384x_BAP_INT,
-			rxfid,
-			0,
-			&rxdesc,
-			sizeof(rxdesc));
-	if ( result ) {
-		WLAN_LOG_DEBUG(1,
-			"copy_from_bap(0x%04x, %d, %d) failed, result=0x%x\n",
-			rxfid,
-			0,
-			sizeof(rxdesc),
-			result);
-		goto done;
-	}
-
-	/* Byte order convert once up front. */
-	rxdesc.status =	hfa384x2host_16(rxdesc.status);
-	rxdesc.time =	hfa384x2host_32(rxdesc.time);
-
-	/* drop errors and whatnot in promisc mode */
-	if (( wlandev->netdev->flags & IFF_PROMISC ) &&
-	    (HFA384x_RXSTATUS_ISFCSERR(rxdesc.status) ||
-	     HFA384x_RXSTATUS_ISUNDECR(rxdesc.status)))
-	  goto done;
-
-	/* Now handle frame based on port# */
-	switch( HFA384x_RXSTATUS_MACPORT_GET(rxdesc.status) )
-	{
-	case 0:
-
-		fc = ieee2host16(rxdesc.frame_control);
-
-		/* If exclude and we receive an unencrypted, drop it */
-		if ( (wlandev->hostwep & HOSTWEP_EXCLUDEUNENCRYPTED) &&
-		     !WLAN_GET_FC_ISWEP(fc)) {
-			goto done;
-		}
-
-		hdrlen = p80211_headerlen(fc);
-
-		/* Allocate the buffer, note CRC (aka FCS). pballoc */
-		/* assumes there needs to be space for one */
-		skb = dev_alloc_skb(hfa384x2host_16(rxdesc.data_len) + hdrlen + WLAN_CRC_LEN + 2); /* a little extra */
-
-		if ( ! skb ) {
-			WLAN_LOG_ERROR("alloc_skb failed.\n");
-			goto done;
-                }
-
-		skb->dev = wlandev->netdev;
-
-		/* theoretically align the IP header on a 32-bit word. */
-		if ( hdrlen == WLAN_HDR_A4_LEN )
-			skb_reserve(skb, 2);
-
-		/* Copy the 802.11 hdr to the buffer */
-		datap = skb_put(skb, WLAN_HDR_A3_LEN);
-		memcpy(datap, &rxdesc.frame_control, WLAN_HDR_A3_LEN);
-
-		/* Snag the A4 address if present */
-		if (hdrlen == WLAN_HDR_A4_LEN) {
-			datap = skb_put(skb, WLAN_ADDR_LEN);
-			memcpy(datap, &rxdesc.address4, WLAN_HDR_A3_LEN);
-		}
-
-		/* we can convert the data_len as we passed the original on */
-		rxdesc.data_len = hfa384x2host_16(rxdesc.data_len);
-
-		/* Copy the payload data to the buffer */
-		if ( rxdesc.data_len > 0 ) {
-			datap = skb_put(skb, rxdesc.data_len);
-			result = hfa384x_copy_from_bap(hw,
-				HFA384x_BAP_INT, rxfid, HFA384x_RX_DATA_OFF,
-				datap, rxdesc.data_len);
-			if ( result ) {
-				WLAN_LOG_DEBUG(1,
-					"copy_from_bap(0x%04x, %d, %d) failed, result=0x%x\n",
-					rxfid,
-					HFA384x_RX_DATA_OFF,
-					rxdesc.data_len,
-					result);
-				goto failed;
-			}
-		}
-		/* the prism2 cards don't return the FCS */
-		datap = skb_put(skb, WLAN_CRC_LEN);
-		memset (datap, 0xff, WLAN_CRC_LEN);
-		skb_reset_mac_header(skb);
-
-		/* Attach the rxmeta, set some stuff */
-		p80211skb_rxmeta_attach(wlandev, skb);
-		rxmeta = P80211SKB_RXMETA(skb);
-		rxmeta->mactime = rxdesc.time;
-		rxmeta->rxrate = rxdesc.rate;
-		rxmeta->signal = rxdesc.signal - hw->dbmadjust;
-		rxmeta->noise = rxdesc.silence - hw->dbmadjust;
-
-		prism2sta_ev_rx(wlandev, skb);
-		goto done;
-	case 7:
-
-        	if ( ! HFA384x_RXSTATUS_ISFCSERR(rxdesc.status) ) {
-                        hfa384x_int_rxmonitor( wlandev, rxfid, &rxdesc);
-                } else {
-                        WLAN_LOG_DEBUG(3,"Received monitor frame: FCSerr set\n");
-                }
-		goto done;
-
-	default:
-
-		WLAN_LOG_WARNING("Received frame on unsupported port=%d\n",
-			HFA384x_RXSTATUS_MACPORT_GET(rxdesc.status) );
-		goto done;
-	}
-
- failed:
-	dev_kfree_skb(skb);
-
- done:
-	DBFEXIT;
-	return;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_int_rxmonitor
-*
-* Helper function for int_rx.  Handles monitor frames.
-* Note that this function allocates space for the FCS and sets it
-* to 0xffffffff.  The hfa384x doesn't give us the FCS value but the
-* higher layers expect it.  0xffffffff is used as a flag to indicate
-* the FCS is bogus.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	rxfid		received FID
-*	rxdesc		rx descriptor read from card in int_rx
-*
-* Returns:
-*	nothing
-*
-* Side effects:
-*	Allocates an skb and passes it up via the PF_PACKET interface.
-* Call context:
-*	interrupt
-----------------------------------------------------------------*/
-static void hfa384x_int_rxmonitor( wlandevice_t *wlandev, UINT16 rxfid,
-				   hfa384x_rx_frame_t *rxdesc)
-{
-	hfa384x_t			*hw = wlandev->priv;
-	UINT				hdrlen = 0;
-	UINT				datalen = 0;
-	UINT				skblen = 0;
-	UINT				truncated = 0;
-	UINT8				*datap;
-	UINT16				fc;
-	struct sk_buff			*skb;
-
-	DBFENTER;
-	/* Don't forget the status, time, and data_len fields are in host order */
-	/* Figure out how big the frame is */
-	fc = ieee2host16(rxdesc->frame_control);
-	hdrlen = p80211_headerlen(fc);
-	datalen = hfa384x2host_16(rxdesc->data_len);
-
-	/* Allocate an ind message+framesize skb */
-	skblen = sizeof(p80211msg_lnxind_wlansniffrm_t) +
-		hdrlen + datalen + WLAN_CRC_LEN;
-
-	/* sanity check the length */
-	if ( skblen >
-		(sizeof(p80211msg_lnxind_wlansniffrm_t) +
-		WLAN_HDR_A4_LEN + WLAN_DATA_MAXLEN + WLAN_CRC_LEN) ) {
-		WLAN_LOG_DEBUG(1, "overlen frm: len=%d\n",
-			skblen - sizeof(p80211msg_lnxind_wlansniffrm_t));
-	}
-
-	if ( (skb = dev_alloc_skb(skblen)) == NULL ) {
-		WLAN_LOG_ERROR("alloc_skb failed trying to allocate %d bytes\n", skblen);
-		return;
-	}
-
-	/* only prepend the prism header if in the right mode */
-	if ((wlandev->netdev->type == ARPHRD_IEEE80211_PRISM) &&
-	    (hw->sniffhdr == 0)) {
-		p80211msg_lnxind_wlansniffrm_t	*msg;
-		datap = skb_put(skb, sizeof(p80211msg_lnxind_wlansniffrm_t));
-		msg = (p80211msg_lnxind_wlansniffrm_t*) datap;
-
-		/* Initialize the message members */
-		msg->msgcode = DIDmsg_lnxind_wlansniffrm;
-		msg->msglen = sizeof(p80211msg_lnxind_wlansniffrm_t);
-		strcpy(msg->devname, wlandev->name);
-
-		msg->hosttime.did = DIDmsg_lnxind_wlansniffrm_hosttime;
-		msg->hosttime.status = 0;
-		msg->hosttime.len = 4;
-		msg->hosttime.data = jiffies;
-
-		msg->mactime.did = DIDmsg_lnxind_wlansniffrm_mactime;
-		msg->mactime.status = 0;
-		msg->mactime.len = 4;
-		msg->mactime.data = rxdesc->time * 1000;
-
-		msg->channel.did = DIDmsg_lnxind_wlansniffrm_channel;
-		msg->channel.status = 0;
-		msg->channel.len = 4;
-		msg->channel.data = hw->sniff_channel;
-
-		msg->rssi.did = DIDmsg_lnxind_wlansniffrm_rssi;
-		msg->rssi.status = P80211ENUM_msgitem_status_no_value;
-		msg->rssi.len = 4;
-		msg->rssi.data = 0;
-
-		msg->sq.did = DIDmsg_lnxind_wlansniffrm_sq;
-		msg->sq.status = P80211ENUM_msgitem_status_no_value;
-		msg->sq.len = 4;
-		msg->sq.data = 0;
-
-		msg->signal.did = DIDmsg_lnxind_wlansniffrm_signal;
-		msg->signal.status = 0;
-		msg->signal.len = 4;
-		msg->signal.data = rxdesc->signal;
-
-		msg->noise.did = DIDmsg_lnxind_wlansniffrm_noise;
-		msg->noise.status = 0;
-		msg->noise.len = 4;
-		msg->noise.data = rxdesc->silence;
-
-		msg->rate.did = DIDmsg_lnxind_wlansniffrm_rate;
-		msg->rate.status = 0;
-		msg->rate.len = 4;
-		msg->rate.data = rxdesc->rate / 5; /* set to 802.11 units */
-
-		msg->istx.did = DIDmsg_lnxind_wlansniffrm_istx;
-		msg->istx.status = 0;
-		msg->istx.len = 4;
-		msg->istx.data = P80211ENUM_truth_false;
-
-		msg->frmlen.did = DIDmsg_lnxind_wlansniffrm_frmlen;
-		msg->frmlen.status = 0;
-		msg->frmlen.len = 4;
-		msg->frmlen.data = hdrlen + datalen + WLAN_CRC_LEN;
-	} else if ((wlandev->netdev->type == ARPHRD_IEEE80211_PRISM) &&
-		   (hw->sniffhdr != 0)) {
-		p80211_caphdr_t		*caphdr;
-		/* The NEW header format! */
-		datap = skb_put(skb, sizeof(p80211_caphdr_t));
-		caphdr = (p80211_caphdr_t*) datap;
-
-		caphdr->version =	htonl(P80211CAPTURE_VERSION);
-		caphdr->length =	htonl(sizeof(p80211_caphdr_t));
-		caphdr->mactime =	__cpu_to_be64(rxdesc->time);
-		caphdr->hosttime =	__cpu_to_be64(jiffies);
-		caphdr->phytype =	htonl(4); /* dss_dot11_b */
-		caphdr->channel =	htonl(hw->sniff_channel);
-		caphdr->datarate =	htonl(rxdesc->rate);
-		caphdr->antenna =	htonl(0); /* unknown */
-		caphdr->priority =	htonl(0); /* unknown */
-		caphdr->ssi_type =	htonl(3); /* rssi_raw */
-		caphdr->ssi_signal =	htonl(rxdesc->signal);
-		caphdr->ssi_noise =	htonl(rxdesc->silence);
-		caphdr->preamble =	htonl(0); /* unknown */
-		caphdr->encoding =	htonl(1); /* cck */
-	}
-	/* Copy the 802.11 header to the skb (ctl frames may be less than a full header) */
-	datap = skb_put(skb, hdrlen);
-	memcpy( datap, &(rxdesc->frame_control), hdrlen);
-
-	/* If any, copy the data from the card to the skb */
-	if ( datalen > 0 )
-	{
-		/* Truncate the packet if the user wants us to */
-		UINT	dataread = datalen;
-		if(hw->sniff_truncate > 0 && dataread > hw->sniff_truncate) {
-			dataread = hw->sniff_truncate;
-			truncated = 1;
-		}
-
-		datap = skb_put(skb, dataread);
-		hfa384x_copy_from_bap(hw,
-			HFA384x_BAP_INT, rxfid, HFA384x_RX_DATA_OFF,
-			datap, dataread);
-
-		/* check for unencrypted stuff if WEP bit set. */
-		if (*(datap - hdrlen + 1) & 0x40) // wep set
-		  if ((*(datap) == 0xaa) && (*(datap+1) == 0xaa))
-		    *(datap - hdrlen + 1) &= 0xbf; // clear wep; it's the 802.2 header!
-	}
-
-	if (!truncated && hw->sniff_fcs) {
-		/* Set the FCS */
-		datap = skb_put(skb, WLAN_CRC_LEN);
-		memset( datap, 0xff, WLAN_CRC_LEN);
-	}
-
-	/* pass it back up */
-	prism2sta_ev_rx(wlandev, skb);
-
-	DBFEXIT;
-	return;
-}
-
-/*----------------------------------------------------------------
-* hfa384x_int_alloc
-*
-* Handles the Alloc event.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*
-* Returns:
-*	nothing
-*
-* Side effects:
-*
-* Call context:
-*	interrupt
-----------------------------------------------------------------*/
-static void hfa384x_int_alloc(wlandevice_t *wlandev)
-{
-	hfa384x_t		*hw = wlandev->priv;
-	UINT16			fid;
-	INT16			result;
-
-	DBFENTER;
-
-	/* Handle the reclaimed FID */
-	/*   collect the FID and push it onto the stack */
-	fid = hfa384x_getreg(hw, HFA384x_ALLOCFID);
-
-	if ( fid != hw->infofid ) { /* It's a transmit fid */
-		WLAN_LOG_DEBUG(5, "int_alloc(%#x)\n", fid);
-		result = txfid_queue_add(hw, fid);
-		if (result != -1) {
-			prism2sta_ev_alloc(wlandev);
-			WLAN_LOG_DEBUG(5, "q_add.\n");
-		} else {
-			WLAN_LOG_DEBUG(5, "q_full.\n");
-		}
-	} else {
-		/* unlock the info fid */
-		up(&hw->infofid_sem);
-	}
-
-	DBFEXIT;
-	return;
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_handover
-*
-* Sends a handover notification to the MAC.
-*
-* Arguments:
-*	hw		device structure
-*	addr		address of station that's left
-*
-* Returns:
-*	zero		success.
-*	-ERESTARTSYS	received signal while waiting for semaphore.
-*	-EIO		failed to write to bap, or failed in cmd.
-*
-* Side effects:
-*
-* Call context:
-*	process thread, NOTE: this call may block on a semaphore!
-----------------------------------------------------------------*/
-int hfa384x_drvr_handover( hfa384x_t *hw, UINT8 *addr)
-{
-	int			result = 0;
-        hfa384x_HandoverAddr_t  rec;
-        UINT                    len;
-        DBFENTER;
-
-	/* Acquire the infofid */
-	if ( down_interruptible(&hw->infofid_sem) ) {
-		result = -ERESTARTSYS;
-		goto failed;
-	}
-
-        /* Set up the record */
-        len = sizeof(hfa384x_HandoverAddr_t);
-        rec.framelen = host2hfa384x_16(len/2 - 1);
-        rec.infotype = host2hfa384x_16(HFA384x_IT_HANDOVERADDR);
-        memcpy(rec.handover_addr, addr, sizeof(rec.handover_addr));
-
-        /* Issue the command */
-        result = hfa384x_cmd_notify(hw, 1, hw->infofid, &rec, len);
-
-        if ( result != 0 ) {
-                WLAN_LOG_DEBUG(1,"cmd_notify(%04x) failed, result=%d",
-                        hw->infofid, result);
-		result = -EIO;
-                goto failed;
-        }
-
-failed:
-	DBFEXIT;
-	return result;
-}
-
-void hfa384x_tx_timeout(wlandevice_t *wlandev)
-{
-	DBFENTER;
-
-	WLAN_LOG_WARNING("Implement me.\n");
-
-	DBFEXIT;
-}
-
-/* Handles all "rx" BAP operations */
-static void     hfa384x_bap_tasklet(unsigned long data)
-{
-	hfa384x_t *hw = (hfa384x_t *) data;
-	wlandevice_t *wlandev = hw->wlandev;
-	int counter = prism2_irq_evread_max;
-	int			reg;
-
-	DBFENTER;
-
-	while (counter-- > 0) {
-		/* Get interrupt register */
-		reg = hfa384x_getreg(hw, HFA384x_EVSTAT);
-
-		if ((reg == 0xffff) ||
-		    !(reg & HFA384x_INT_BAP_OP)) {
-			break;
-		}
-
-		if ( HFA384x_EVSTAT_ISINFO(reg) ){
-			hfa384x_int_info(wlandev);
-			hfa384x_setreg(hw, HFA384x_EVACK_INFO_SET(1),
-				HFA384x_EVACK);
-		}
-		if ( HFA384x_EVSTAT_ISTXEXC(reg) ){
-			hfa384x_int_txexc(wlandev);
-			hfa384x_setreg(hw, HFA384x_EVACK_TXEXC_SET(1),
-				HFA384x_EVACK);
-		}
-		if ( HFA384x_EVSTAT_ISTX(reg) ){
-			hfa384x_int_tx(wlandev);
-			hfa384x_setreg(hw, HFA384x_EVACK_TX_SET(1),
-				HFA384x_EVACK);
-		}
-		if ( HFA384x_EVSTAT_ISRX(reg) ){
-			hfa384x_int_rx(wlandev);
-			hfa384x_setreg(hw, HFA384x_EVACK_RX_SET(1),
-				HFA384x_EVACK);
-		}
-	}
-
-	/* re-enable interrupts */
-	hfa384x_events_all(hw);
-
-	DBFEXIT;
-}
diff --git a/drivers/staging/wlan-ng/hfa384x.h b/drivers/staging/wlan-ng/hfa384x.h
index 0dfb8ce9aae..9b746654a39 100644
--- a/drivers/staging/wlan-ng/hfa384x.h
+++ b/drivers/staging/wlan-ng/hfa384x.h
@@ -63,18 +63,18 @@
 
 /*------ Constants --------------------------------------------*/
 /*--- Mins & Maxs -----------------------------------*/
-#define		HFA384x_CMD_ALLOC_LEN_MIN	((UINT16)4)
-#define		HFA384x_CMD_ALLOC_LEN_MAX	((UINT16)2400)
-#define		HFA384x_BAP_DATALEN_MAX		((UINT16)4096)
-#define		HFA384x_BAP_OFFSET_MAX		((UINT16)4096)
-#define		HFA384x_PORTID_MAX		((UINT16)7)
-#define		HFA384x_NUMPORTS_MAX		((UINT16)(HFA384x_PORTID_MAX+1))
-#define		HFA384x_PDR_LEN_MAX		((UINT16)512)	/* in bytes, from EK */
-#define		HFA384x_PDA_RECS_MAX		((UINT16)200)	/* a guess */
-#define		HFA384x_PDA_LEN_MAX		((UINT16)1024)	/* in bytes, from EK */
-#define		HFA384x_SCANRESULT_MAX		((UINT16)31)
-#define		HFA384x_HSCANRESULT_MAX		((UINT16)31)
-#define		HFA384x_CHINFORESULT_MAX	((UINT16)16)
+#define		HFA384x_CMD_ALLOC_LEN_MIN	((u16)4)
+#define		HFA384x_CMD_ALLOC_LEN_MAX	((u16)2400)
+#define		HFA384x_BAP_DATALEN_MAX		((u16)4096)
+#define		HFA384x_BAP_OFFSET_MAX		((u16)4096)
+#define		HFA384x_PORTID_MAX		((u16)7)
+#define		HFA384x_NUMPORTS_MAX		((u16)(HFA384x_PORTID_MAX+1))
+#define		HFA384x_PDR_LEN_MAX		((u16)512)	/* in bytes, from EK */
+#define		HFA384x_PDA_RECS_MAX		((u16)200)	/* a guess */
+#define		HFA384x_PDA_LEN_MAX		((u16)1024)	/* in bytes, from EK */
+#define		HFA384x_SCANRESULT_MAX		((u16)31)
+#define		HFA384x_HSCANRESULT_MAX		((u16)31)
+#define		HFA384x_CHINFORESULT_MAX	((u16)16)
 #define		HFA384x_DRVR_FIDSTACKLEN_MAX	(10)
 #define		HFA384x_DRVR_TXBUF_MAX		(sizeof(hfa384x_tx_frame_t) + \
 						WLAN_DATA_MAXLEN - \
@@ -88,42 +88,42 @@
 #define		HFA384x_USB_RWMEM_MAXLEN	2048
 
 /*--- Support Constants -----------------------------*/
-#define		HFA384x_BAP_PROC			((UINT16)0)
-#define		HFA384x_BAP_INT				((UINT16)1)
-#define		HFA384x_PORTTYPE_IBSS			((UINT16)0)
-#define		HFA384x_PORTTYPE_BSS			((UINT16)1)
-#define		HFA384x_PORTTYPE_WDS			((UINT16)2)
-#define		HFA384x_PORTTYPE_PSUEDOIBSS		((UINT16)3)
-#define		HFA384x_PORTTYPE_HOSTAP    		((UINT16)6)
-#define		HFA384x_WEPFLAGS_PRIVINVOKED		((UINT16)BIT0)
-#define		HFA384x_WEPFLAGS_EXCLUDE		((UINT16)BIT1)
-#define		HFA384x_WEPFLAGS_DISABLE_TXCRYPT	((UINT16)BIT4)
-#define		HFA384x_WEPFLAGS_DISABLE_RXCRYPT	((UINT16)BIT7)
-#define		HFA384x_WEPFLAGS_DISALLOW_MIXED 	((UINT16)BIT11)
-#define		HFA384x_WEPFLAGS_IV_INTERVAL1		((UINT16)0)
-#define		HFA384x_WEPFLAGS_IV_INTERVAL10		((UINT16)BIT5)
-#define		HFA384x_WEPFLAGS_IV_INTERVAL50		((UINT16)BIT6)
-#define		HFA384x_WEPFLAGS_IV_INTERVAL100		((UINT16)(BIT5 | BIT6))
-#define		HFA384x_WEPFLAGS_FIRMWARE_WPA  		((UINT16)BIT8)
-#define		HFA384x_WEPFLAGS_HOST_MIC      		((UINT16)BIT9)
-#define 	HFA384x_ROAMMODE_FWSCAN_FWROAM		((UINT16)1)
-#define 	HFA384x_ROAMMODE_FWSCAN_HOSTROAM	((UINT16)2)
-#define 	HFA384x_ROAMMODE_HOSTSCAN_HOSTROAM	((UINT16)3)
-#define 	HFA384x_PORTSTATUS_DISABLED		((UINT16)1)
-#define 	HFA384x_PORTSTATUS_INITSRCH		((UINT16)2)
-#define 	HFA384x_PORTSTATUS_CONN_IBSS		((UINT16)3)
-#define 	HFA384x_PORTSTATUS_CONN_ESS		((UINT16)4)
-#define 	HFA384x_PORTSTATUS_OOR_ESS		((UINT16)5)
-#define 	HFA384x_PORTSTATUS_CONN_WDS		((UINT16)6)
-#define 	HFA384x_PORTSTATUS_HOSTAP		((UINT16)8)
-#define		HFA384x_RATEBIT_1			((UINT16)1)
-#define		HFA384x_RATEBIT_2			((UINT16)2)
-#define		HFA384x_RATEBIT_5dot5			((UINT16)4)
-#define		HFA384x_RATEBIT_11			((UINT16)8)
+#define		HFA384x_BAP_PROC			((u16)0)
+#define		HFA384x_BAP_int				((u16)1)
+#define		HFA384x_PORTTYPE_IBSS			((u16)0)
+#define		HFA384x_PORTTYPE_BSS			((u16)1)
+#define		HFA384x_PORTTYPE_WDS			((u16)2)
+#define		HFA384x_PORTTYPE_PSUEDOIBSS		((u16)3)
+#define		HFA384x_PORTTYPE_HOSTAP    		((u16)6)
+#define		HFA384x_WEPFLAGS_PRIVINVOKED		((u16)BIT0)
+#define		HFA384x_WEPFLAGS_EXCLUDE		((u16)BIT1)
+#define		HFA384x_WEPFLAGS_DISABLE_TXCRYPT	((u16)BIT4)
+#define		HFA384x_WEPFLAGS_DISABLE_RXCRYPT	((u16)BIT7)
+#define		HFA384x_WEPFLAGS_DISALLOW_MIXED 	((u16)BIT11)
+#define		HFA384x_WEPFLAGS_IV_intERVAL1		((u16)0)
+#define		HFA384x_WEPFLAGS_IV_intERVAL10		((u16)BIT5)
+#define		HFA384x_WEPFLAGS_IV_intERVAL50		((u16)BIT6)
+#define		HFA384x_WEPFLAGS_IV_intERVAL100		((u16)(BIT5 | BIT6))
+#define		HFA384x_WEPFLAGS_FIRMWARE_WPA  		((u16)BIT8)
+#define		HFA384x_WEPFLAGS_HOST_MIC      		((u16)BIT9)
+#define 	HFA384x_ROAMMODE_FWSCAN_FWROAM		((u16)1)
+#define 	HFA384x_ROAMMODE_FWSCAN_HOSTROAM	((u16)2)
+#define 	HFA384x_ROAMMODE_HOSTSCAN_HOSTROAM	((u16)3)
+#define 	HFA384x_PORTSTATUS_DISABLED		((u16)1)
+#define 	HFA384x_PORTSTATUS_INITSRCH		((u16)2)
+#define 	HFA384x_PORTSTATUS_CONN_IBSS		((u16)3)
+#define 	HFA384x_PORTSTATUS_CONN_ESS		((u16)4)
+#define 	HFA384x_PORTSTATUS_OOR_ESS		((u16)5)
+#define 	HFA384x_PORTSTATUS_CONN_WDS		((u16)6)
+#define 	HFA384x_PORTSTATUS_HOSTAP		((u16)8)
+#define		HFA384x_RATEBIT_1			((u16)1)
+#define		HFA384x_RATEBIT_2			((u16)2)
+#define		HFA384x_RATEBIT_5dot5			((u16)4)
+#define		HFA384x_RATEBIT_11			((u16)8)
 
 /*--- Just some symbolic names for legibility -------*/
-#define		HFA384x_TXCMD_NORECL		((UINT16)0)
-#define		HFA384x_TXCMD_RECL		((UINT16)1)
+#define		HFA384x_TXCMD_NORECL		((u16)0)
+#define		HFA384x_TXCMD_RECL		((u16)1)
 
 /*--- MAC Internal memory constants and macros ------*/
 /* masks and macros used to manipulate MAC internal memory addresses. */
@@ -140,7 +140,7 @@
  */
 
 /* Handy constant */
-#define		HFA384x_ADDR_AUX_OFF_MAX	((UINT16)0x007f)
+#define		HFA384x_ADDR_AUX_OFF_MAX	((u16)0x007f)
 
 /* Mask bits for discarding unwanted pieces in a flat address */
 #define		HFA384x_ADDR_FLAT_AUX_PAGE_MASK	(0x007fff80)
@@ -158,25 +158,25 @@
 
 /* Make a 32-bit flat address from AUX format 16-bit page and offset */
 #define		HFA384x_ADDR_AUX_MKFLAT(p,o)	\
-		(((UINT32)(((UINT16)(p))&HFA384x_ADDR_AUX_PAGE_MASK)) <<7) | \
-		((UINT32)(((UINT16)(o))&HFA384x_ADDR_AUX_OFF_MASK))
+		(((u32)(((u16)(p))&HFA384x_ADDR_AUX_PAGE_MASK)) <<7) | \
+		((u32)(((u16)(o))&HFA384x_ADDR_AUX_OFF_MASK))
 
 /* Make a 32-bit flat address from CMD format 16-bit page and offset */
 #define		HFA384x_ADDR_CMD_MKFLAT(p,o)	\
-		(((UINT32)(((UINT16)(p))&HFA384x_ADDR_CMD_PAGE_MASK)) <<16) | \
-		((UINT32)(((UINT16)(o))&HFA384x_ADDR_CMD_OFF_MASK))
+		(((u32)(((u16)(p))&HFA384x_ADDR_CMD_PAGE_MASK)) <<16) | \
+		((u32)(((u16)(o))&HFA384x_ADDR_CMD_OFF_MASK))
 
 /* Make AUX format offset and page from a 32-bit flat address */
 #define		HFA384x_ADDR_AUX_MKPAGE(f) \
-		((UINT16)((((UINT32)(f))&HFA384x_ADDR_FLAT_AUX_PAGE_MASK)>>7))
+		((u16)((((u32)(f))&HFA384x_ADDR_FLAT_AUX_PAGE_MASK)>>7))
 #define		HFA384x_ADDR_AUX_MKOFF(f) \
-		((UINT16)(((UINT32)(f))&HFA384x_ADDR_FLAT_AUX_OFF_MASK))
+		((u16)(((u32)(f))&HFA384x_ADDR_FLAT_AUX_OFF_MASK))
 
 /* Make CMD format offset and page from a 32-bit flat address */
 #define		HFA384x_ADDR_CMD_MKPAGE(f) \
-		((UINT16)((((UINT32)(f))&HFA384x_ADDR_FLAT_CMD_PAGE_MASK)>>16))
+		((u16)((((u32)(f))&HFA384x_ADDR_FLAT_CMD_PAGE_MASK)>>16))
 #define		HFA384x_ADDR_CMD_MKOFF(f) \
-		((UINT16)(((UINT32)(f))&HFA384x_ADDR_FLAT_CMD_OFF_MASK))
+		((u16)(((u32)(f))&HFA384x_ADDR_FLAT_CMD_OFF_MASK))
 
 /*--- Aux register masks/tests ----------------------*/
 /* Some of the upper bits of the AUX offset register are used to */
@@ -188,7 +188,7 @@
 
 /* Make AUX register offset and page values from a flat address */
 #define		HFA384x_AUX_MKOFF(f, c) \
-	(HFA384x_ADDR_AUX_MKOFF(f) | (((UINT16)(c))<<12))
+	(HFA384x_ADDR_AUX_MKOFF(f) | (((u16)(c))<<12))
 #define		HFA384x_AUX_MKPAGE(f)	HFA384x_ADDR_AUX_MKPAGE(f)
 
 
@@ -205,40 +205,6 @@
 #define		HFA384x_DLSTATE_FLASHWRITEPENDING	4
 #define		HFA384x_DLSTATE_GENESIS 		5
 
-/*--- Register I/O offsets --------------------------*/
-#if ((WLAN_HOSTIF == WLAN_PCMCIA) || (WLAN_HOSTIF == WLAN_PLX))
-
-#define		HFA384x_CMD_OFF			(0x00)
-#define		HFA384x_PARAM0_OFF		(0x02)
-#define		HFA384x_PARAM1_OFF		(0x04)
-#define		HFA384x_PARAM2_OFF		(0x06)
-#define		HFA384x_STATUS_OFF		(0x08)
-#define		HFA384x_RESP0_OFF		(0x0A)
-#define		HFA384x_RESP1_OFF		(0x0C)
-#define		HFA384x_RESP2_OFF		(0x0E)
-#define		HFA384x_INFOFID_OFF		(0x10)
-#define		HFA384x_RXFID_OFF		(0x20)
-#define		HFA384x_ALLOCFID_OFF		(0x22)
-#define		HFA384x_TXCOMPLFID_OFF		(0x24)
-#define		HFA384x_SELECT0_OFF		(0x18)
-#define		HFA384x_OFFSET0_OFF		(0x1C)
-#define		HFA384x_DATA0_OFF		(0x36)
-#define		HFA384x_SELECT1_OFF		(0x1A)
-#define		HFA384x_OFFSET1_OFF		(0x1E)
-#define		HFA384x_DATA1_OFF		(0x38)
-#define		HFA384x_EVSTAT_OFF		(0x30)
-#define		HFA384x_INTEN_OFF		(0x32)
-#define		HFA384x_EVACK_OFF		(0x34)
-#define		HFA384x_CONTROL_OFF		(0x14)
-#define		HFA384x_SWSUPPORT0_OFF		(0x28)
-#define		HFA384x_SWSUPPORT1_OFF		(0x2A)
-#define		HFA384x_SWSUPPORT2_OFF		(0x2C)
-#define		HFA384x_AUXPAGE_OFF		(0x3A)
-#define		HFA384x_AUXOFFSET_OFF		(0x3C)
-#define		HFA384x_AUXDATA_OFF		(0x3E)
-
-#elif (WLAN_HOSTIF == WLAN_PCI || WLAN_HOSTIF == WLAN_USB)
-
 #define		HFA384x_CMD_OFF			(0x00)
 #define		HFA384x_PARAM0_OFF		(0x04)
 #define		HFA384x_PARAM1_OFF		(0x08)
@@ -258,7 +224,7 @@
 #define		HFA384x_OFFSET1_OFF		(0x3c)
 #define		HFA384x_DATA1_OFF		(0x70)
 #define		HFA384x_EVSTAT_OFF		(0x60)
-#define		HFA384x_INTEN_OFF		(0x64)
+#define		HFA384x_intEN_OFF		(0x64)
 #define		HFA384x_EVACK_OFF		(0x68)
 #define		HFA384x_CONTROL_OFF		(0x28)
 #define		HFA384x_SWSUPPORT0_OFF		(0x50)
@@ -279,94 +245,92 @@
 #define		HFA384x_PCI_M1_LEN_OFF		(0xa8)
 #define		HFA384x_PCI_M1_CTL_OFF		(0xac)
 
-#endif
-
 /*--- Register Field Masks --------------------------*/
-#define		HFA384x_CMD_BUSY		((UINT16)BIT15)
-#define		HFA384x_CMD_AINFO		((UINT16)(BIT14 | BIT13 | BIT12 | BIT11 | BIT10 | BIT9 | BIT8))
-#define		HFA384x_CMD_MACPORT		((UINT16)(BIT10 | BIT9 | BIT8))
-#define		HFA384x_CMD_RECL		((UINT16)BIT8)
-#define		HFA384x_CMD_WRITE		((UINT16)BIT8)
-#define		HFA384x_CMD_PROGMODE		((UINT16)(BIT9 | BIT8))
-#define		HFA384x_CMD_CMDCODE		((UINT16)(BIT5 | BIT4 | BIT3 | BIT2 | BIT1 | BIT0))
-
-#define		HFA384x_STATUS_RESULT		((UINT16)(BIT14 | BIT13 | BIT12 | BIT11 | BIT10 | BIT9 | BIT8))
-#define		HFA384x_STATUS_CMDCODE		((UINT16)(BIT5 | BIT4 | BIT3 | BIT2 | BIT1 | BIT0))
-
-#define		HFA384x_OFFSET_BUSY		((UINT16)BIT15)
-#define		HFA384x_OFFSET_ERR		((UINT16)BIT14)
-#define		HFA384x_OFFSET_DATAOFF		((UINT16)(BIT11 | BIT10 | BIT9 | BIT8 | BIT7 | BIT6 | BIT5 | BIT4 | BIT3 | BIT2 | BIT1))
-
-#define		HFA384x_EVSTAT_TICK		((UINT16)BIT15)
-#define		HFA384x_EVSTAT_WTERR		((UINT16)BIT14)
-#define		HFA384x_EVSTAT_INFDROP		((UINT16)BIT13)
-#define		HFA384x_EVSTAT_INFO		((UINT16)BIT7)
-#define		HFA384x_EVSTAT_DTIM		((UINT16)BIT5)
-#define		HFA384x_EVSTAT_CMD		((UINT16)BIT4)
-#define		HFA384x_EVSTAT_ALLOC		((UINT16)BIT3)
-#define		HFA384x_EVSTAT_TXEXC		((UINT16)BIT2)
-#define		HFA384x_EVSTAT_TX		((UINT16)BIT1)
-#define		HFA384x_EVSTAT_RX		((UINT16)BIT0)
-
-#define         HFA384x_INT_BAP_OP           (HFA384x_EVSTAT_INFO|HFA384x_EVSTAT_RX|HFA384x_EVSTAT_TX|HFA384x_EVSTAT_TXEXC)
-
-#define         HFA384x_INT_NORMAL           (HFA384x_EVSTAT_INFO|HFA384x_EVSTAT_RX|HFA384x_EVSTAT_TX|HFA384x_EVSTAT_TXEXC|HFA384x_EVSTAT_INFDROP|HFA384x_EVSTAT_ALLOC|HFA384x_EVSTAT_DTIM)
-
-#define		HFA384x_INTEN_TICK		((UINT16)BIT15)
-#define		HFA384x_INTEN_WTERR		((UINT16)BIT14)
-#define		HFA384x_INTEN_INFDROP		((UINT16)BIT13)
-#define		HFA384x_INTEN_INFO		((UINT16)BIT7)
-#define		HFA384x_INTEN_DTIM		((UINT16)BIT5)
-#define		HFA384x_INTEN_CMD		((UINT16)BIT4)
-#define		HFA384x_INTEN_ALLOC		((UINT16)BIT3)
-#define		HFA384x_INTEN_TXEXC		((UINT16)BIT2)
-#define		HFA384x_INTEN_TX		((UINT16)BIT1)
-#define		HFA384x_INTEN_RX		((UINT16)BIT0)
-
-#define		HFA384x_EVACK_TICK		((UINT16)BIT15)
-#define		HFA384x_EVACK_WTERR		((UINT16)BIT14)
-#define		HFA384x_EVACK_INFDROP		((UINT16)BIT13)
-#define		HFA384x_EVACK_INFO		((UINT16)BIT7)
-#define		HFA384x_EVACK_DTIM		((UINT16)BIT5)
-#define		HFA384x_EVACK_CMD		((UINT16)BIT4)
-#define		HFA384x_EVACK_ALLOC		((UINT16)BIT3)
-#define		HFA384x_EVACK_TXEXC		((UINT16)BIT2)
-#define		HFA384x_EVACK_TX		((UINT16)BIT1)
-#define		HFA384x_EVACK_RX		((UINT16)BIT0)
-
-#define		HFA384x_CONTROL_AUXEN		((UINT16)(BIT15 | BIT14))
+#define		HFA384x_CMD_BUSY		((u16)BIT15)
+#define		HFA384x_CMD_AINFO		((u16)(BIT14 | BIT13 | BIT12 | BIT11 | BIT10 | BIT9 | BIT8))
+#define		HFA384x_CMD_MACPORT		((u16)(BIT10 | BIT9 | BIT8))
+#define		HFA384x_CMD_RECL		((u16)BIT8)
+#define		HFA384x_CMD_WRITE		((u16)BIT8)
+#define		HFA384x_CMD_PROGMODE		((u16)(BIT9 | BIT8))
+#define		HFA384x_CMD_CMDCODE		((u16)(BIT5 | BIT4 | BIT3 | BIT2 | BIT1 | BIT0))
+
+#define		HFA384x_STATUS_RESULT		((u16)(BIT14 | BIT13 | BIT12 | BIT11 | BIT10 | BIT9 | BIT8))
+#define		HFA384x_STATUS_CMDCODE		((u16)(BIT5 | BIT4 | BIT3 | BIT2 | BIT1 | BIT0))
+
+#define		HFA384x_OFFSET_BUSY		((u16)BIT15)
+#define		HFA384x_OFFSET_ERR		((u16)BIT14)
+#define		HFA384x_OFFSET_DATAOFF		((u16)(BIT11 | BIT10 | BIT9 | BIT8 | BIT7 | BIT6 | BIT5 | BIT4 | BIT3 | BIT2 | BIT1))
+
+#define		HFA384x_EVSTAT_TICK		((u16)BIT15)
+#define		HFA384x_EVSTAT_WTERR		((u16)BIT14)
+#define		HFA384x_EVSTAT_INFDROP		((u16)BIT13)
+#define		HFA384x_EVSTAT_INFO		((u16)BIT7)
+#define		HFA384x_EVSTAT_DTIM		((u16)BIT5)
+#define		HFA384x_EVSTAT_CMD		((u16)BIT4)
+#define		HFA384x_EVSTAT_ALLOC		((u16)BIT3)
+#define		HFA384x_EVSTAT_TXEXC		((u16)BIT2)
+#define		HFA384x_EVSTAT_TX		((u16)BIT1)
+#define		HFA384x_EVSTAT_RX		((u16)BIT0)
+
+#define         HFA384x_int_BAP_OP           (HFA384x_EVSTAT_INFO|HFA384x_EVSTAT_RX|HFA384x_EVSTAT_TX|HFA384x_EVSTAT_TXEXC)
+
+#define         HFA384x_int_NORMAL           (HFA384x_EVSTAT_INFO|HFA384x_EVSTAT_RX|HFA384x_EVSTAT_TX|HFA384x_EVSTAT_TXEXC|HFA384x_EVSTAT_INFDROP|HFA384x_EVSTAT_ALLOC|HFA384x_EVSTAT_DTIM)
+
+#define		HFA384x_intEN_TICK		((u16)BIT15)
+#define		HFA384x_intEN_WTERR		((u16)BIT14)
+#define		HFA384x_intEN_INFDROP		((u16)BIT13)
+#define		HFA384x_intEN_INFO		((u16)BIT7)
+#define		HFA384x_intEN_DTIM		((u16)BIT5)
+#define		HFA384x_intEN_CMD		((u16)BIT4)
+#define		HFA384x_intEN_ALLOC		((u16)BIT3)
+#define		HFA384x_intEN_TXEXC		((u16)BIT2)
+#define		HFA384x_intEN_TX		((u16)BIT1)
+#define		HFA384x_intEN_RX		((u16)BIT0)
+
+#define		HFA384x_EVACK_TICK		((u16)BIT15)
+#define		HFA384x_EVACK_WTERR		((u16)BIT14)
+#define		HFA384x_EVACK_INFDROP		((u16)BIT13)
+#define		HFA384x_EVACK_INFO		((u16)BIT7)
+#define		HFA384x_EVACK_DTIM		((u16)BIT5)
+#define		HFA384x_EVACK_CMD		((u16)BIT4)
+#define		HFA384x_EVACK_ALLOC		((u16)BIT3)
+#define		HFA384x_EVACK_TXEXC		((u16)BIT2)
+#define		HFA384x_EVACK_TX		((u16)BIT1)
+#define		HFA384x_EVACK_RX		((u16)BIT0)
+
+#define		HFA384x_CONTROL_AUXEN		((u16)(BIT15 | BIT14))
 
 
 /*--- Command Code Constants --------------------------*/
 /*--- Controller Commands --------------------------*/
-#define		HFA384x_CMDCODE_INIT		((UINT16)0x00)
-#define		HFA384x_CMDCODE_ENABLE		((UINT16)0x01)
-#define		HFA384x_CMDCODE_DISABLE		((UINT16)0x02)
-#define		HFA384x_CMDCODE_DIAG		((UINT16)0x03)
+#define		HFA384x_CMDCODE_INIT		((u16)0x00)
+#define		HFA384x_CMDCODE_ENABLE		((u16)0x01)
+#define		HFA384x_CMDCODE_DISABLE		((u16)0x02)
+#define		HFA384x_CMDCODE_DIAG		((u16)0x03)
 
 /*--- Buffer Mgmt Commands --------------------------*/
-#define		HFA384x_CMDCODE_ALLOC		((UINT16)0x0A)
-#define		HFA384x_CMDCODE_TX		((UINT16)0x0B)
-#define		HFA384x_CMDCODE_CLRPRST		((UINT16)0x12)
+#define		HFA384x_CMDCODE_ALLOC		((u16)0x0A)
+#define		HFA384x_CMDCODE_TX		((u16)0x0B)
+#define		HFA384x_CMDCODE_CLRPRST		((u16)0x12)
 
 /*--- Regulate Commands --------------------------*/
-#define		HFA384x_CMDCODE_NOTIFY		((UINT16)0x10)
-#define		HFA384x_CMDCODE_INQ		((UINT16)0x11)
+#define		HFA384x_CMDCODE_NOTIFY		((u16)0x10)
+#define		HFA384x_CMDCODE_INQ		((u16)0x11)
 
 /*--- Configure Commands --------------------------*/
-#define		HFA384x_CMDCODE_ACCESS		((UINT16)0x21)
-#define		HFA384x_CMDCODE_DOWNLD		((UINT16)0x22)
+#define		HFA384x_CMDCODE_ACCESS		((u16)0x21)
+#define		HFA384x_CMDCODE_DOWNLD		((u16)0x22)
 
 /*--- Debugging Commands -----------------------------*/
-#define 	HFA384x_CMDCODE_MONITOR		((UINT16)(0x38))
-#define		HFA384x_MONITOR_ENABLE		((UINT16)(0x0b))
-#define		HFA384x_MONITOR_DISABLE		((UINT16)(0x0f))
+#define 	HFA384x_CMDCODE_MONITOR		((u16)(0x38))
+#define		HFA384x_MONITOR_ENABLE		((u16)(0x0b))
+#define		HFA384x_MONITOR_DISABLE		((u16)(0x0f))
 
 /*--- Result Codes --------------------------*/
-#define		HFA384x_SUCCESS			((UINT16)(0x00))
-#define		HFA384x_CARD_FAIL		((UINT16)(0x01))
-#define		HFA384x_NO_BUFF			((UINT16)(0x05))
-#define		HFA384x_CMD_ERR			((UINT16)(0x7F))
+#define		HFA384x_SUCCESS			((u16)(0x00))
+#define		HFA384x_CARD_FAIL		((u16)(0x01))
+#define		HFA384x_NO_BUFF			((u16)(0x05))
+#define		HFA384x_CMD_ERR			((u16)(0x7F))
 
 /*--- Programming Modes --------------------------
 	MODE 0: Disable programming
@@ -374,48 +338,48 @@
 	MODE 2: Enable non-volatile memory programming
 	MODE 3: Program non-volatile memory section
 --------------------------------------------------*/
-#define		HFA384x_PROGMODE_DISABLE	((UINT16)0x00)
-#define		HFA384x_PROGMODE_RAM		((UINT16)0x01)
-#define		HFA384x_PROGMODE_NV		((UINT16)0x02)
-#define		HFA384x_PROGMODE_NVWRITE	((UINT16)0x03)
+#define		HFA384x_PROGMODE_DISABLE	((u16)0x00)
+#define		HFA384x_PROGMODE_RAM		((u16)0x01)
+#define		HFA384x_PROGMODE_NV		((u16)0x02)
+#define		HFA384x_PROGMODE_NVWRITE	((u16)0x03)
 
 /*--- AUX register enable --------------------------*/
-#define		HFA384x_AUXPW0			((UINT16)0xfe01)
-#define		HFA384x_AUXPW1			((UINT16)0xdc23)
-#define		HFA384x_AUXPW2			((UINT16)0xba45)
+#define		HFA384x_AUXPW0			((u16)0xfe01)
+#define		HFA384x_AUXPW1			((u16)0xdc23)
+#define		HFA384x_AUXPW2			((u16)0xba45)
 
-#define		HFA384x_CONTROL_AUX_ISDISABLED	((UINT16)0x0000)
-#define		HFA384x_CONTROL_AUX_ISENABLED	((UINT16)0xc000)
-#define		HFA384x_CONTROL_AUX_DOENABLE	((UINT16)0x8000)
-#define		HFA384x_CONTROL_AUX_DODISABLE	((UINT16)0x4000)
+#define		HFA384x_CONTROL_AUX_ISDISABLED	((u16)0x0000)
+#define		HFA384x_CONTROL_AUX_ISENABLED	((u16)0xc000)
+#define		HFA384x_CONTROL_AUX_DOENABLE	((u16)0x8000)
+#define		HFA384x_CONTROL_AUX_DODISABLE	((u16)0x4000)
 
 /*--- Record ID Constants --------------------------*/
 /*--------------------------------------------------------------------
 Configuration RIDs: Network Parameters, Static Configuration Entities
 --------------------------------------------------------------------*/
-#define		HFA384x_RID_CNFPORTTYPE		((UINT16)0xFC00)
-#define		HFA384x_RID_CNFOWNMACADDR	((UINT16)0xFC01)
-#define		HFA384x_RID_CNFDESIREDSSID	((UINT16)0xFC02)
-#define		HFA384x_RID_CNFOWNCHANNEL	((UINT16)0xFC03)
-#define		HFA384x_RID_CNFOWNSSID		((UINT16)0xFC04)
-#define		HFA384x_RID_CNFOWNATIMWIN	((UINT16)0xFC05)
-#define		HFA384x_RID_CNFSYSSCALE		((UINT16)0xFC06)
-#define		HFA384x_RID_CNFMAXDATALEN	((UINT16)0xFC07)
-#define		HFA384x_RID_CNFWDSADDR		((UINT16)0xFC08)
-#define		HFA384x_RID_CNFPMENABLED	((UINT16)0xFC09)
-#define		HFA384x_RID_CNFPMEPS		((UINT16)0xFC0A)
-#define		HFA384x_RID_CNFMULTICASTRX	((UINT16)0xFC0B)
-#define		HFA384x_RID_CNFMAXSLEEPDUR	((UINT16)0xFC0C)
-#define		HFA384x_RID_CNFPMHOLDDUR	((UINT16)0xFC0D)
-#define		HFA384x_RID_CNFOWNNAME		((UINT16)0xFC0E)
-#define		HFA384x_RID_CNFOWNDTIMPER	((UINT16)0xFC10)
-#define		HFA384x_RID_CNFWDSADDR1		((UINT16)0xFC11)
-#define		HFA384x_RID_CNFWDSADDR2		((UINT16)0xFC12)
-#define		HFA384x_RID_CNFWDSADDR3		((UINT16)0xFC13)
-#define		HFA384x_RID_CNFWDSADDR4		((UINT16)0xFC14)
-#define		HFA384x_RID_CNFWDSADDR5		((UINT16)0xFC15)
-#define		HFA384x_RID_CNFWDSADDR6		((UINT16)0xFC16)
-#define		HFA384x_RID_CNFMCASTPMBUFF	((UINT16)0xFC17)
+#define		HFA384x_RID_CNFPORTTYPE		((u16)0xFC00)
+#define		HFA384x_RID_CNFOWNMACADDR	((u16)0xFC01)
+#define		HFA384x_RID_CNFDESIREDSSID	((u16)0xFC02)
+#define		HFA384x_RID_CNFOWNCHANNEL	((u16)0xFC03)
+#define		HFA384x_RID_CNFOWNSSID		((u16)0xFC04)
+#define		HFA384x_RID_CNFOWNATIMWIN	((u16)0xFC05)
+#define		HFA384x_RID_CNFSYSSCALE		((u16)0xFC06)
+#define		HFA384x_RID_CNFMAXDATALEN	((u16)0xFC07)
+#define		HFA384x_RID_CNFWDSADDR		((u16)0xFC08)
+#define		HFA384x_RID_CNFPMENABLED	((u16)0xFC09)
+#define		HFA384x_RID_CNFPMEPS		((u16)0xFC0A)
+#define		HFA384x_RID_CNFMULTICASTRX	((u16)0xFC0B)
+#define		HFA384x_RID_CNFMAXSLEEPDUR	((u16)0xFC0C)
+#define		HFA384x_RID_CNFPMHOLDDUR	((u16)0xFC0D)
+#define		HFA384x_RID_CNFOWNNAME		((u16)0xFC0E)
+#define		HFA384x_RID_CNFOWNDTIMPER	((u16)0xFC10)
+#define		HFA384x_RID_CNFWDSADDR1		((u16)0xFC11)
+#define		HFA384x_RID_CNFWDSADDR2		((u16)0xFC12)
+#define		HFA384x_RID_CNFWDSADDR3		((u16)0xFC13)
+#define		HFA384x_RID_CNFWDSADDR4		((u16)0xFC14)
+#define		HFA384x_RID_CNFWDSADDR5		((u16)0xFC15)
+#define		HFA384x_RID_CNFWDSADDR6		((u16)0xFC16)
+#define		HFA384x_RID_CNFMCASTPMBUFF	((u16)0xFC17)
 
 /*--------------------------------------------------------------------
 Configuration RID lengths: Network Params, Static Config Entities
@@ -423,62 +387,62 @@ Configuration RID lengths: Network Params, Static Config Entities
   include the len or code fields)
 --------------------------------------------------------------------*/
 /* TODO: fill in the rest of these */
-#define		HFA384x_RID_CNFPORTTYPE_LEN	((UINT16)2)
-#define		HFA384x_RID_CNFOWNMACADDR_LEN	((UINT16)6)
-#define		HFA384x_RID_CNFDESIREDSSID_LEN	((UINT16)34)
-#define		HFA384x_RID_CNFOWNCHANNEL_LEN	((UINT16)2)
-#define		HFA384x_RID_CNFOWNSSID_LEN	((UINT16)34)
-#define		HFA384x_RID_CNFOWNATIMWIN_LEN	((UINT16)2)
-#define		HFA384x_RID_CNFSYSSCALE_LEN	((UINT16)0)
-#define		HFA384x_RID_CNFMAXDATALEN_LEN	((UINT16)0)
-#define		HFA384x_RID_CNFWDSADDR_LEN	((UINT16)6)
-#define		HFA384x_RID_CNFPMENABLED_LEN	((UINT16)0)
-#define		HFA384x_RID_CNFPMEPS_LEN	((UINT16)0)
-#define		HFA384x_RID_CNFMULTICASTRX_LEN	((UINT16)0)
-#define		HFA384x_RID_CNFMAXSLEEPDUR_LEN	((UINT16)0)
-#define		HFA384x_RID_CNFPMHOLDDUR_LEN	((UINT16)0)
-#define		HFA384x_RID_CNFOWNNAME_LEN	((UINT16)34)
-#define		HFA384x_RID_CNFOWNDTIMPER_LEN	((UINT16)0)
-#define		HFA384x_RID_CNFWDSADDR1_LEN	((UINT16)6)
-#define		HFA384x_RID_CNFWDSADDR2_LEN	((UINT16)6)
-#define		HFA384x_RID_CNFWDSADDR3_LEN	((UINT16)6)
-#define		HFA384x_RID_CNFWDSADDR4_LEN	((UINT16)6)
-#define		HFA384x_RID_CNFWDSADDR5_LEN	((UINT16)6)
-#define		HFA384x_RID_CNFWDSADDR6_LEN	((UINT16)6)
-#define		HFA384x_RID_CNFMCASTPMBUFF_LEN	((UINT16)0)
-#define		HFA384x_RID_CNFAUTHENTICATION_LEN ((UINT16)sizeof(UINT16))
-#define		HFA384x_RID_CNFMAXSLEEPDUR_LEN	((UINT16)0)
+#define		HFA384x_RID_CNFPORTTYPE_LEN	((u16)2)
+#define		HFA384x_RID_CNFOWNMACADDR_LEN	((u16)6)
+#define		HFA384x_RID_CNFDESIREDSSID_LEN	((u16)34)
+#define		HFA384x_RID_CNFOWNCHANNEL_LEN	((u16)2)
+#define		HFA384x_RID_CNFOWNSSID_LEN	((u16)34)
+#define		HFA384x_RID_CNFOWNATIMWIN_LEN	((u16)2)
+#define		HFA384x_RID_CNFSYSSCALE_LEN	((u16)0)
+#define		HFA384x_RID_CNFMAXDATALEN_LEN	((u16)0)
+#define		HFA384x_RID_CNFWDSADDR_LEN	((u16)6)
+#define		HFA384x_RID_CNFPMENABLED_LEN	((u16)0)
+#define		HFA384x_RID_CNFPMEPS_LEN	((u16)0)
+#define		HFA384x_RID_CNFMULTICASTRX_LEN	((u16)0)
+#define		HFA384x_RID_CNFMAXSLEEPDUR_LEN	((u16)0)
+#define		HFA384x_RID_CNFPMHOLDDUR_LEN	((u16)0)
+#define		HFA384x_RID_CNFOWNNAME_LEN	((u16)34)
+#define		HFA384x_RID_CNFOWNDTIMPER_LEN	((u16)0)
+#define		HFA384x_RID_CNFWDSADDR1_LEN	((u16)6)
+#define		HFA384x_RID_CNFWDSADDR2_LEN	((u16)6)
+#define		HFA384x_RID_CNFWDSADDR3_LEN	((u16)6)
+#define		HFA384x_RID_CNFWDSADDR4_LEN	((u16)6)
+#define		HFA384x_RID_CNFWDSADDR5_LEN	((u16)6)
+#define		HFA384x_RID_CNFWDSADDR6_LEN	((u16)6)
+#define		HFA384x_RID_CNFMCASTPMBUFF_LEN	((u16)0)
+#define		HFA384x_RID_CNFAUTHENTICATION_LEN ((u16)sizeof(u16))
+#define		HFA384x_RID_CNFMAXSLEEPDUR_LEN	((u16)0)
 
 /*--------------------------------------------------------------------
 Configuration RIDs: Network Parameters, Dynamic Configuration Entities
 --------------------------------------------------------------------*/
-#define		HFA384x_RID_GROUPADDR		((UINT16)0xFC80)
-#define		HFA384x_RID_CREATEIBSS		((UINT16)0xFC81)
-#define		HFA384x_RID_FRAGTHRESH		((UINT16)0xFC82)
-#define		HFA384x_RID_RTSTHRESH		((UINT16)0xFC83)
-#define		HFA384x_RID_TXRATECNTL		((UINT16)0xFC84)
-#define		HFA384x_RID_PROMISCMODE		((UINT16)0xFC85)
-#define		HFA384x_RID_FRAGTHRESH0		((UINT16)0xFC90)
-#define		HFA384x_RID_FRAGTHRESH1		((UINT16)0xFC91)
-#define		HFA384x_RID_FRAGTHRESH2		((UINT16)0xFC92)
-#define		HFA384x_RID_FRAGTHRESH3		((UINT16)0xFC93)
-#define		HFA384x_RID_FRAGTHRESH4		((UINT16)0xFC94)
-#define		HFA384x_RID_FRAGTHRESH5		((UINT16)0xFC95)
-#define		HFA384x_RID_FRAGTHRESH6		((UINT16)0xFC96)
-#define		HFA384x_RID_RTSTHRESH0		((UINT16)0xFC97)
-#define		HFA384x_RID_RTSTHRESH1		((UINT16)0xFC98)
-#define		HFA384x_RID_RTSTHRESH2		((UINT16)0xFC99)
-#define		HFA384x_RID_RTSTHRESH3		((UINT16)0xFC9A)
-#define		HFA384x_RID_RTSTHRESH4		((UINT16)0xFC9B)
-#define		HFA384x_RID_RTSTHRESH5		((UINT16)0xFC9C)
-#define		HFA384x_RID_RTSTHRESH6		((UINT16)0xFC9D)
-#define		HFA384x_RID_TXRATECNTL0		((UINT16)0xFC9E)
-#define		HFA384x_RID_TXRATECNTL1		((UINT16)0xFC9F)
-#define		HFA384x_RID_TXRATECNTL2		((UINT16)0xFCA0)
-#define		HFA384x_RID_TXRATECNTL3		((UINT16)0xFCA1)
-#define		HFA384x_RID_TXRATECNTL4		((UINT16)0xFCA2)
-#define		HFA384x_RID_TXRATECNTL5		((UINT16)0xFCA3)
-#define		HFA384x_RID_TXRATECNTL6		((UINT16)0xFCA4)
+#define		HFA384x_RID_GROUPADDR		((u16)0xFC80)
+#define		HFA384x_RID_CREATEIBSS		((u16)0xFC81)
+#define		HFA384x_RID_FRAGTHRESH		((u16)0xFC82)
+#define		HFA384x_RID_RTSTHRESH		((u16)0xFC83)
+#define		HFA384x_RID_TXRATECNTL		((u16)0xFC84)
+#define		HFA384x_RID_PROMISCMODE		((u16)0xFC85)
+#define		HFA384x_RID_FRAGTHRESH0		((u16)0xFC90)
+#define		HFA384x_RID_FRAGTHRESH1		((u16)0xFC91)
+#define		HFA384x_RID_FRAGTHRESH2		((u16)0xFC92)
+#define		HFA384x_RID_FRAGTHRESH3		((u16)0xFC93)
+#define		HFA384x_RID_FRAGTHRESH4		((u16)0xFC94)
+#define		HFA384x_RID_FRAGTHRESH5		((u16)0xFC95)
+#define		HFA384x_RID_FRAGTHRESH6		((u16)0xFC96)
+#define		HFA384x_RID_RTSTHRESH0		((u16)0xFC97)
+#define		HFA384x_RID_RTSTHRESH1		((u16)0xFC98)
+#define		HFA384x_RID_RTSTHRESH2		((u16)0xFC99)
+#define		HFA384x_RID_RTSTHRESH3		((u16)0xFC9A)
+#define		HFA384x_RID_RTSTHRESH4		((u16)0xFC9B)
+#define		HFA384x_RID_RTSTHRESH5		((u16)0xFC9C)
+#define		HFA384x_RID_RTSTHRESH6		((u16)0xFC9D)
+#define		HFA384x_RID_TXRATECNTL0		((u16)0xFC9E)
+#define		HFA384x_RID_TXRATECNTL1		((u16)0xFC9F)
+#define		HFA384x_RID_TXRATECNTL2		((u16)0xFCA0)
+#define		HFA384x_RID_TXRATECNTL3		((u16)0xFCA1)
+#define		HFA384x_RID_TXRATECNTL4		((u16)0xFCA2)
+#define		HFA384x_RID_TXRATECNTL5		((u16)0xFCA3)
+#define		HFA384x_RID_TXRATECNTL6		((u16)0xFCA4)
 
 /*--------------------------------------------------------------------
 Configuration RID Lengths: Network Param, Dynamic Config Entities
@@ -486,296 +450,296 @@ Configuration RID Lengths: Network Param, Dynamic Config Entities
   include the len or code fields)
 --------------------------------------------------------------------*/
 /* TODO: fill in the rest of these */
-#define		HFA384x_RID_GROUPADDR_LEN	((UINT16)16 * WLAN_ADDR_LEN)
-#define		HFA384x_RID_CREATEIBSS_LEN	((UINT16)0)
-#define		HFA384x_RID_FRAGTHRESH_LEN	((UINT16)0)
-#define		HFA384x_RID_RTSTHRESH_LEN	((UINT16)0)
-#define		HFA384x_RID_TXRATECNTL_LEN	((UINT16)4)
-#define		HFA384x_RID_PROMISCMODE_LEN	((UINT16)2)
-#define		HFA384x_RID_FRAGTHRESH0_LEN	((UINT16)0)
-#define		HFA384x_RID_FRAGTHRESH1_LEN	((UINT16)0)
-#define		HFA384x_RID_FRAGTHRESH2_LEN	((UINT16)0)
-#define		HFA384x_RID_FRAGTHRESH3_LEN	((UINT16)0)
-#define		HFA384x_RID_FRAGTHRESH4_LEN	((UINT16)0)
-#define		HFA384x_RID_FRAGTHRESH5_LEN	((UINT16)0)
-#define		HFA384x_RID_FRAGTHRESH6_LEN	((UINT16)0)
-#define		HFA384x_RID_RTSTHRESH0_LEN	((UINT16)0)
-#define		HFA384x_RID_RTSTHRESH1_LEN	((UINT16)0)
-#define		HFA384x_RID_RTSTHRESH2_LEN	((UINT16)0)
-#define		HFA384x_RID_RTSTHRESH3_LEN	((UINT16)0)
-#define		HFA384x_RID_RTSTHRESH4_LEN	((UINT16)0)
-#define		HFA384x_RID_RTSTHRESH5_LEN	((UINT16)0)
-#define		HFA384x_RID_RTSTHRESH6_LEN	((UINT16)0)
-#define		HFA384x_RID_TXRATECNTL0_LEN	((UINT16)0)
-#define		HFA384x_RID_TXRATECNTL1_LEN	((UINT16)0)
-#define		HFA384x_RID_TXRATECNTL2_LEN	((UINT16)0)
-#define		HFA384x_RID_TXRATECNTL3_LEN	((UINT16)0)
-#define		HFA384x_RID_TXRATECNTL4_LEN	((UINT16)0)
-#define		HFA384x_RID_TXRATECNTL5_LEN	((UINT16)0)
-#define		HFA384x_RID_TXRATECNTL6_LEN	((UINT16)0)
+#define		HFA384x_RID_GROUPADDR_LEN	((u16)16 * WLAN_ADDR_LEN)
+#define		HFA384x_RID_CREATEIBSS_LEN	((u16)0)
+#define		HFA384x_RID_FRAGTHRESH_LEN	((u16)0)
+#define		HFA384x_RID_RTSTHRESH_LEN	((u16)0)
+#define		HFA384x_RID_TXRATECNTL_LEN	((u16)4)
+#define		HFA384x_RID_PROMISCMODE_LEN	((u16)2)
+#define		HFA384x_RID_FRAGTHRESH0_LEN	((u16)0)
+#define		HFA384x_RID_FRAGTHRESH1_LEN	((u16)0)
+#define		HFA384x_RID_FRAGTHRESH2_LEN	((u16)0)
+#define		HFA384x_RID_FRAGTHRESH3_LEN	((u16)0)
+#define		HFA384x_RID_FRAGTHRESH4_LEN	((u16)0)
+#define		HFA384x_RID_FRAGTHRESH5_LEN	((u16)0)
+#define		HFA384x_RID_FRAGTHRESH6_LEN	((u16)0)
+#define		HFA384x_RID_RTSTHRESH0_LEN	((u16)0)
+#define		HFA384x_RID_RTSTHRESH1_LEN	((u16)0)
+#define		HFA384x_RID_RTSTHRESH2_LEN	((u16)0)
+#define		HFA384x_RID_RTSTHRESH3_LEN	((u16)0)
+#define		HFA384x_RID_RTSTHRESH4_LEN	((u16)0)
+#define		HFA384x_RID_RTSTHRESH5_LEN	((u16)0)
+#define		HFA384x_RID_RTSTHRESH6_LEN	((u16)0)
+#define		HFA384x_RID_TXRATECNTL0_LEN	((u16)0)
+#define		HFA384x_RID_TXRATECNTL1_LEN	((u16)0)
+#define		HFA384x_RID_TXRATECNTL2_LEN	((u16)0)
+#define		HFA384x_RID_TXRATECNTL3_LEN	((u16)0)
+#define		HFA384x_RID_TXRATECNTL4_LEN	((u16)0)
+#define		HFA384x_RID_TXRATECNTL5_LEN	((u16)0)
+#define		HFA384x_RID_TXRATECNTL6_LEN	((u16)0)
 
 /*--------------------------------------------------------------------
 Configuration RIDs: Behavior Parameters
 --------------------------------------------------------------------*/
-#define		HFA384x_RID_ITICKTIME		((UINT16)0xFCE0)
+#define		HFA384x_RID_ITICKTIME		((u16)0xFCE0)
 
 /*--------------------------------------------------------------------
 Configuration RID Lengths: Behavior Parameters
   This is the length of JUST the DATA part of the RID (does not
   include the len or code fields)
 --------------------------------------------------------------------*/
-#define		HFA384x_RID_ITICKTIME_LEN	((UINT16)2)
+#define		HFA384x_RID_ITICKTIME_LEN	((u16)2)
 
 /*----------------------------------------------------------------------
 Information RIDs: NIC Information
 --------------------------------------------------------------------*/
-#define		HFA384x_RID_MAXLOADTIME		((UINT16)0xFD00)
-#define		HFA384x_RID_DOWNLOADBUFFER	((UINT16)0xFD01)
-#define		HFA384x_RID_PRIIDENTITY		((UINT16)0xFD02)
-#define		HFA384x_RID_PRISUPRANGE		((UINT16)0xFD03)
-#define		HFA384x_RID_PRI_CFIACTRANGES	((UINT16)0xFD04)
-#define		HFA384x_RID_NICSERIALNUMBER	((UINT16)0xFD0A)
-#define		HFA384x_RID_NICIDENTITY		((UINT16)0xFD0B)
-#define		HFA384x_RID_MFISUPRANGE		((UINT16)0xFD0C)
-#define		HFA384x_RID_CFISUPRANGE		((UINT16)0xFD0D)
-#define		HFA384x_RID_CHANNELLIST		((UINT16)0xFD10)
-#define		HFA384x_RID_REGULATORYDOMAINS	((UINT16)0xFD11)
-#define		HFA384x_RID_TEMPTYPE		((UINT16)0xFD12)
-#define		HFA384x_RID_CIS			((UINT16)0xFD13)
-#define		HFA384x_RID_STAIDENTITY		((UINT16)0xFD20)
-#define		HFA384x_RID_STASUPRANGE		((UINT16)0xFD21)
-#define		HFA384x_RID_STA_MFIACTRANGES	((UINT16)0xFD22)
-#define		HFA384x_RID_STA_CFIACTRANGES	((UINT16)0xFD23)
-#define		HFA384x_RID_BUILDSEQ		((UINT16)0xFFFE)
-#define		HFA384x_RID_FWID		((UINT16)0xFFFF)
+#define		HFA384x_RID_MAXLOADTIME		((u16)0xFD00)
+#define		HFA384x_RID_DOWNLOADBUFFER	((u16)0xFD01)
+#define		HFA384x_RID_PRIIDENTITY		((u16)0xFD02)
+#define		HFA384x_RID_PRISUPRANGE		((u16)0xFD03)
+#define		HFA384x_RID_PRI_CFIACTRANGES	((u16)0xFD04)
+#define		HFA384x_RID_NICSERIALNUMBER	((u16)0xFD0A)
+#define		HFA384x_RID_NICIDENTITY		((u16)0xFD0B)
+#define		HFA384x_RID_MFISUPRANGE		((u16)0xFD0C)
+#define		HFA384x_RID_CFISUPRANGE		((u16)0xFD0D)
+#define		HFA384x_RID_CHANNELLIST		((u16)0xFD10)
+#define		HFA384x_RID_REGULATORYDOMAINS	((u16)0xFD11)
+#define		HFA384x_RID_TEMPTYPE		((u16)0xFD12)
+#define		HFA384x_RID_CIS			((u16)0xFD13)
+#define		HFA384x_RID_STAIDENTITY		((u16)0xFD20)
+#define		HFA384x_RID_STASUPRANGE		((u16)0xFD21)
+#define		HFA384x_RID_STA_MFIACTRANGES	((u16)0xFD22)
+#define		HFA384x_RID_STA_CFIACTRANGES	((u16)0xFD23)
+#define		HFA384x_RID_BUILDSEQ		((u16)0xFFFE)
+#define		HFA384x_RID_FWID		((u16)0xFFFF)
 
 /*----------------------------------------------------------------------
 Information RID Lengths: NIC Information
   This is the length of JUST the DATA part of the RID (does not
   include the len or code fields)
 --------------------------------------------------------------------*/
-#define		HFA384x_RID_MAXLOADTIME_LEN		((UINT16)0)
-#define		HFA384x_RID_DOWNLOADBUFFER_LEN		((UINT16)sizeof(hfa384x_downloadbuffer_t))
-#define		HFA384x_RID_PRIIDENTITY_LEN		((UINT16)8)
-#define		HFA384x_RID_PRISUPRANGE_LEN		((UINT16)10)
-#define		HFA384x_RID_CFIACTRANGES_LEN		((UINT16)10)
-#define		HFA384x_RID_NICSERIALNUMBER_LEN		((UINT16)12)
-#define		HFA384x_RID_NICIDENTITY_LEN		((UINT16)8)
-#define		HFA384x_RID_MFISUPRANGE_LEN		((UINT16)10)
-#define		HFA384x_RID_CFISUPRANGE_LEN		((UINT16)10)
-#define		HFA384x_RID_CHANNELLIST_LEN		((UINT16)0)
-#define		HFA384x_RID_REGULATORYDOMAINS_LEN	((UINT16)12)
-#define		HFA384x_RID_TEMPTYPE_LEN		((UINT16)0)
-#define		HFA384x_RID_CIS_LEN			((UINT16)480)
-#define		HFA384x_RID_STAIDENTITY_LEN		((UINT16)8)
-#define		HFA384x_RID_STASUPRANGE_LEN		((UINT16)10)
-#define		HFA384x_RID_MFIACTRANGES_LEN		((UINT16)10)
-#define		HFA384x_RID_CFIACTRANGES2_LEN		((UINT16)10)
-#define		HFA384x_RID_BUILDSEQ_LEN		((UINT16)sizeof(hfa384x_BuildSeq_t))
-#define		HFA384x_RID_FWID_LEN			((UINT16)sizeof(hfa384x_FWID_t))
+#define		HFA384x_RID_MAXLOADTIME_LEN		((u16)0)
+#define		HFA384x_RID_DOWNLOADBUFFER_LEN		((u16)sizeof(hfa384x_downloadbuffer_t))
+#define		HFA384x_RID_PRIIDENTITY_LEN		((u16)8)
+#define		HFA384x_RID_PRISUPRANGE_LEN		((u16)10)
+#define		HFA384x_RID_CFIACTRANGES_LEN		((u16)10)
+#define		HFA384x_RID_NICSERIALNUMBER_LEN		((u16)12)
+#define		HFA384x_RID_NICIDENTITY_LEN		((u16)8)
+#define		HFA384x_RID_MFISUPRANGE_LEN		((u16)10)
+#define		HFA384x_RID_CFISUPRANGE_LEN		((u16)10)
+#define		HFA384x_RID_CHANNELLIST_LEN		((u16)0)
+#define		HFA384x_RID_REGULATORYDOMAINS_LEN	((u16)12)
+#define		HFA384x_RID_TEMPTYPE_LEN		((u16)0)
+#define		HFA384x_RID_CIS_LEN			((u16)480)
+#define		HFA384x_RID_STAIDENTITY_LEN		((u16)8)
+#define		HFA384x_RID_STASUPRANGE_LEN		((u16)10)
+#define		HFA384x_RID_MFIACTRANGES_LEN		((u16)10)
+#define		HFA384x_RID_CFIACTRANGES2_LEN		((u16)10)
+#define		HFA384x_RID_BUILDSEQ_LEN		((u16)sizeof(hfa384x_BuildSeq_t))
+#define		HFA384x_RID_FWID_LEN			((u16)sizeof(hfa384x_FWID_t))
 
 /*--------------------------------------------------------------------
 Information RIDs:  MAC Information
 --------------------------------------------------------------------*/
-#define		HFA384x_RID_PORTSTATUS		((UINT16)0xFD40)
-#define		HFA384x_RID_CURRENTSSID		((UINT16)0xFD41)
-#define		HFA384x_RID_CURRENTBSSID	((UINT16)0xFD42)
-#define		HFA384x_RID_COMMSQUALITY	((UINT16)0xFD43)
-#define		HFA384x_RID_CURRENTTXRATE	((UINT16)0xFD44)
-#define		HFA384x_RID_CURRENTBCNINT	((UINT16)0xFD45)
-#define		HFA384x_RID_CURRENTSCALETHRESH	((UINT16)0xFD46)
-#define		HFA384x_RID_PROTOCOLRSPTIME	((UINT16)0xFD47)
-#define		HFA384x_RID_SHORTRETRYLIMIT	((UINT16)0xFD48)
-#define		HFA384x_RID_LONGRETRYLIMIT	((UINT16)0xFD49)
-#define		HFA384x_RID_MAXTXLIFETIME	((UINT16)0xFD4A)
-#define		HFA384x_RID_MAXRXLIFETIME	((UINT16)0xFD4B)
-#define		HFA384x_RID_CFPOLLABLE		((UINT16)0xFD4C)
-#define		HFA384x_RID_AUTHALGORITHMS	((UINT16)0xFD4D)
-#define		HFA384x_RID_PRIVACYOPTIMP	((UINT16)0xFD4F)
-#define		HFA384x_RID_DBMCOMMSQUALITY	((UINT16)0xFD51)
-#define		HFA384x_RID_CURRENTTXRATE1	((UINT16)0xFD80)
-#define		HFA384x_RID_CURRENTTXRATE2	((UINT16)0xFD81)
-#define		HFA384x_RID_CURRENTTXRATE3	((UINT16)0xFD82)
-#define		HFA384x_RID_CURRENTTXRATE4	((UINT16)0xFD83)
-#define		HFA384x_RID_CURRENTTXRATE5	((UINT16)0xFD84)
-#define		HFA384x_RID_CURRENTTXRATE6	((UINT16)0xFD85)
-#define		HFA384x_RID_OWNMACADDRESS	((UINT16)0xFD86)
-// #define	HFA384x_RID_PCFINFO		((UINT16)0xFD87)
-#define		HFA384x_RID_SCANRESULTS       	((UINT16)0xFD88) // NEW
-#define		HFA384x_RID_HOSTSCANRESULTS   	((UINT16)0xFD89) // NEW
-#define		HFA384x_RID_AUTHENTICATIONUSED	((UINT16)0xFD8A) // NEW
-#define		HFA384x_RID_ASSOCIATEFAILURE  	((UINT16)0xFD8D) // 1.8.0
+#define		HFA384x_RID_PORTSTATUS		((u16)0xFD40)
+#define		HFA384x_RID_CURRENTSSID		((u16)0xFD41)
+#define		HFA384x_RID_CURRENTBSSID	((u16)0xFD42)
+#define		HFA384x_RID_COMMSQUALITY	((u16)0xFD43)
+#define		HFA384x_RID_CURRENTTXRATE	((u16)0xFD44)
+#define		HFA384x_RID_CURRENTBCNint	((u16)0xFD45)
+#define		HFA384x_RID_CURRENTSCALETHRESH	((u16)0xFD46)
+#define		HFA384x_RID_PROTOCOLRSPTIME	((u16)0xFD47)
+#define		HFA384x_RID_SHORTRETRYLIMIT	((u16)0xFD48)
+#define		HFA384x_RID_LONGRETRYLIMIT	((u16)0xFD49)
+#define		HFA384x_RID_MAXTXLIFETIME	((u16)0xFD4A)
+#define		HFA384x_RID_MAXRXLIFETIME	((u16)0xFD4B)
+#define		HFA384x_RID_CFPOLLABLE		((u16)0xFD4C)
+#define		HFA384x_RID_AUTHALGORITHMS	((u16)0xFD4D)
+#define		HFA384x_RID_PRIVACYOPTIMP	((u16)0xFD4F)
+#define		HFA384x_RID_DBMCOMMSQUALITY	((u16)0xFD51)
+#define		HFA384x_RID_CURRENTTXRATE1	((u16)0xFD80)
+#define		HFA384x_RID_CURRENTTXRATE2	((u16)0xFD81)
+#define		HFA384x_RID_CURRENTTXRATE3	((u16)0xFD82)
+#define		HFA384x_RID_CURRENTTXRATE4	((u16)0xFD83)
+#define		HFA384x_RID_CURRENTTXRATE5	((u16)0xFD84)
+#define		HFA384x_RID_CURRENTTXRATE6	((u16)0xFD85)
+#define		HFA384x_RID_OWNMACADDRESS	((u16)0xFD86)
+// #define	HFA384x_RID_PCFINFO		((u16)0xFD87)
+#define		HFA384x_RID_SCANRESULTS       	((u16)0xFD88) // NEW
+#define		HFA384x_RID_HOSTSCANRESULTS   	((u16)0xFD89) // NEW
+#define		HFA384x_RID_AUTHENTICATIONUSED	((u16)0xFD8A) // NEW
+#define		HFA384x_RID_ASSOCIATEFAILURE  	((u16)0xFD8D) // 1.8.0
 
 /*--------------------------------------------------------------------
 Information RID Lengths:  MAC Information
   This is the length of JUST the DATA part of the RID (does not
   include the len or code fields)
 --------------------------------------------------------------------*/
-#define		HFA384x_RID_PORTSTATUS_LEN		((UINT16)0)
-#define		HFA384x_RID_CURRENTSSID_LEN		((UINT16)34)
-#define		HFA384x_RID_CURRENTBSSID_LEN		((UINT16)WLAN_BSSID_LEN)
-#define		HFA384x_RID_COMMSQUALITY_LEN		((UINT16)sizeof(hfa384x_commsquality_t))
-#define		HFA384x_RID_DBMCOMMSQUALITY_LEN		((UINT16)sizeof(hfa384x_dbmcommsquality_t))
-#define		HFA384x_RID_CURRENTTXRATE_LEN		((UINT16)0)
-#define		HFA384x_RID_CURRENTBCNINT_LEN		((UINT16)0)
-#define		HFA384x_RID_STACURSCALETHRESH_LEN	((UINT16)12)
-#define		HFA384x_RID_APCURSCALETHRESH_LEN	((UINT16)6)
-#define		HFA384x_RID_PROTOCOLRSPTIME_LEN		((UINT16)0)
-#define		HFA384x_RID_SHORTRETRYLIMIT_LEN		((UINT16)0)
-#define		HFA384x_RID_LONGRETRYLIMIT_LEN		((UINT16)0)
-#define		HFA384x_RID_MAXTXLIFETIME_LEN		((UINT16)0)
-#define		HFA384x_RID_MAXRXLIFETIME_LEN		((UINT16)0)
-#define		HFA384x_RID_CFPOLLABLE_LEN		((UINT16)0)
-#define		HFA384x_RID_AUTHALGORITHMS_LEN		((UINT16)4)
-#define		HFA384x_RID_PRIVACYOPTIMP_LEN		((UINT16)0)
-#define		HFA384x_RID_CURRENTTXRATE1_LEN		((UINT16)0)
-#define		HFA384x_RID_CURRENTTXRATE2_LEN		((UINT16)0)
-#define		HFA384x_RID_CURRENTTXRATE3_LEN		((UINT16)0)
-#define		HFA384x_RID_CURRENTTXRATE4_LEN		((UINT16)0)
-#define		HFA384x_RID_CURRENTTXRATE5_LEN		((UINT16)0)
-#define		HFA384x_RID_CURRENTTXRATE6_LEN		((UINT16)0)
-#define		HFA384x_RID_OWNMACADDRESS_LEN		((UINT16)6)
-#define		HFA384x_RID_PCFINFO_LEN			((UINT16)6)
-#define		HFA384x_RID_CNFAPPCFINFO_LEN		((UINT16)sizeof(hfa384x_PCFInfo_data_t))
-#define		HFA384x_RID_SCANREQUEST_LEN		((UINT16)sizeof(hfa384x_ScanRequest_data_t))
-#define		HFA384x_RID_JOINREQUEST_LEN		((UINT16)sizeof(hfa384x_JoinRequest_data_t))
-#define		HFA384x_RID_AUTHENTICATESTA_LEN		((UINT16)sizeof(hfa384x_authenticateStation_data_t))
-#define		HFA384x_RID_CHANNELINFOREQUEST_LEN	((UINT16)sizeof(hfa384x_ChannelInfoRequest_data_t))
+#define		HFA384x_RID_PORTSTATUS_LEN		((u16)0)
+#define		HFA384x_RID_CURRENTSSID_LEN		((u16)34)
+#define		HFA384x_RID_CURRENTBSSID_LEN		((u16)WLAN_BSSID_LEN)
+#define		HFA384x_RID_COMMSQUALITY_LEN		((u16)sizeof(hfa384x_commsquality_t))
+#define		HFA384x_RID_DBMCOMMSQUALITY_LEN		((u16)sizeof(hfa384x_dbmcommsquality_t))
+#define		HFA384x_RID_CURRENTTXRATE_LEN		((u16)0)
+#define		HFA384x_RID_CURRENTBCNint_LEN		((u16)0)
+#define		HFA384x_RID_STACURSCALETHRESH_LEN	((u16)12)
+#define		HFA384x_RID_APCURSCALETHRESH_LEN	((u16)6)
+#define		HFA384x_RID_PROTOCOLRSPTIME_LEN		((u16)0)
+#define		HFA384x_RID_SHORTRETRYLIMIT_LEN		((u16)0)
+#define		HFA384x_RID_LONGRETRYLIMIT_LEN		((u16)0)
+#define		HFA384x_RID_MAXTXLIFETIME_LEN		((u16)0)
+#define		HFA384x_RID_MAXRXLIFETIME_LEN		((u16)0)
+#define		HFA384x_RID_CFPOLLABLE_LEN		((u16)0)
+#define		HFA384x_RID_AUTHALGORITHMS_LEN		((u16)4)
+#define		HFA384x_RID_PRIVACYOPTIMP_LEN		((u16)0)
+#define		HFA384x_RID_CURRENTTXRATE1_LEN		((u16)0)
+#define		HFA384x_RID_CURRENTTXRATE2_LEN		((u16)0)
+#define		HFA384x_RID_CURRENTTXRATE3_LEN		((u16)0)
+#define		HFA384x_RID_CURRENTTXRATE4_LEN		((u16)0)
+#define		HFA384x_RID_CURRENTTXRATE5_LEN		((u16)0)
+#define		HFA384x_RID_CURRENTTXRATE6_LEN		((u16)0)
+#define		HFA384x_RID_OWNMACADDRESS_LEN		((u16)6)
+#define		HFA384x_RID_PCFINFO_LEN			((u16)6)
+#define		HFA384x_RID_CNFAPPCFINFO_LEN		((u16)sizeof(hfa384x_PCFInfo_data_t))
+#define		HFA384x_RID_SCANREQUEST_LEN		((u16)sizeof(hfa384x_ScanRequest_data_t))
+#define		HFA384x_RID_JOINREQUEST_LEN		((u16)sizeof(hfa384x_JoinRequest_data_t))
+#define		HFA384x_RID_AUTHENTICATESTA_LEN		((u16)sizeof(hfa384x_authenticateStation_data_t))
+#define		HFA384x_RID_CHANNELINFOREQUEST_LEN	((u16)sizeof(hfa384x_ChannelInfoRequest_data_t))
 /*--------------------------------------------------------------------
 Information RIDs:  Modem Information
 --------------------------------------------------------------------*/
-#define		HFA384x_RID_PHYTYPE		((UINT16)0xFDC0)
-#define		HFA384x_RID_CURRENTCHANNEL	((UINT16)0xFDC1)
-#define		HFA384x_RID_CURRENTPOWERSTATE	((UINT16)0xFDC2)
-#define		HFA384x_RID_CCAMODE		((UINT16)0xFDC3)
-#define		HFA384x_RID_SUPPORTEDDATARATES	((UINT16)0xFDC6)
-#define		HFA384x_RID_LFOSTATUS           ((UINT16)0xFDC7) // 1.7.1
+#define		HFA384x_RID_PHYTYPE		((u16)0xFDC0)
+#define		HFA384x_RID_CURRENTCHANNEL	((u16)0xFDC1)
+#define		HFA384x_RID_CURRENTPOWERSTATE	((u16)0xFDC2)
+#define		HFA384x_RID_CCAMODE		((u16)0xFDC3)
+#define		HFA384x_RID_SUPPORTEDDATARATES	((u16)0xFDC6)
+#define		HFA384x_RID_LFOSTATUS           ((u16)0xFDC7) // 1.7.1
 
 /*--------------------------------------------------------------------
 Information RID Lengths:  Modem Information
   This is the length of JUST the DATA part of the RID (does not
   include the len or code fields)
 --------------------------------------------------------------------*/
-#define		HFA384x_RID_PHYTYPE_LEN			((UINT16)0)
-#define		HFA384x_RID_CURRENTCHANNEL_LEN		((UINT16)0)
-#define		HFA384x_RID_CURRENTPOWERSTATE_LEN	((UINT16)0)
-#define		HFA384x_RID_CCAMODE_LEN			((UINT16)0)
-#define		HFA384x_RID_SUPPORTEDDATARATES_LEN	((UINT16)10)
+#define		HFA384x_RID_PHYTYPE_LEN			((u16)0)
+#define		HFA384x_RID_CURRENTCHANNEL_LEN		((u16)0)
+#define		HFA384x_RID_CURRENTPOWERSTATE_LEN	((u16)0)
+#define		HFA384x_RID_CCAMODE_LEN			((u16)0)
+#define		HFA384x_RID_SUPPORTEDDATARATES_LEN	((u16)10)
 
 /*--------------------------------------------------------------------
 API ENHANCEMENTS (NOT ALREADY IMPLEMENTED)
 --------------------------------------------------------------------*/
-#define		HFA384x_RID_CNFWEPDEFAULTKEYID	((UINT16)0xFC23)
-#define		HFA384x_RID_CNFWEPDEFAULTKEY0	((UINT16)0xFC24)
-#define		HFA384x_RID_CNFWEPDEFAULTKEY1	((UINT16)0xFC25)
-#define		HFA384x_RID_CNFWEPDEFAULTKEY2	((UINT16)0xFC26)
-#define		HFA384x_RID_CNFWEPDEFAULTKEY3	((UINT16)0xFC27)
-#define		HFA384x_RID_CNFWEPFLAGS		((UINT16)0xFC28)
-#define		HFA384x_RID_CNFWEPKEYMAPTABLE	((UINT16)0xFC29)
-#define		HFA384x_RID_CNFAUTHENTICATION	((UINT16)0xFC2A)
-#define		HFA384x_RID_CNFMAXASSOCSTATIONS	((UINT16)0xFC2B)
-#define		HFA384x_RID_CNFTXCONTROL	((UINT16)0xFC2C)
-#define		HFA384x_RID_CNFROAMINGMODE	((UINT16)0xFC2D)
-#define		HFA384x_RID_CNFHOSTAUTHASSOC	((UINT16)0xFC2E)
-#define		HFA384x_RID_CNFRCVCRCERROR	((UINT16)0xFC30)
-// #define		HFA384x_RID_CNFMMLIFE		((UINT16)0xFC31)
-#define		HFA384x_RID_CNFALTRETRYCNT	((UINT16)0xFC32)
-#define		HFA384x_RID_CNFAPBCNINT		((UINT16)0xFC33)
-#define		HFA384x_RID_CNFAPPCFINFO	((UINT16)0xFC34)
-#define		HFA384x_RID_CNFSTAPCFINFO	((UINT16)0xFC35)
-#define		HFA384x_RID_CNFPRIORITYQUSAGE	((UINT16)0xFC37)
-#define		HFA384x_RID_CNFTIMCTRL		((UINT16)0xFC40)
-#define		HFA384x_RID_CNFTHIRTY2TALLY	((UINT16)0xFC42)
-#define		HFA384x_RID_CNFENHSECURITY	((UINT16)0xFC43)
-#define		HFA384x_RID_CNFDBMADJUST  	((UINT16)0xFC46) // NEW
-#define		HFA384x_RID_CNFWPADATA       	((UINT16)0xFC48) // 1.7.0
-#define		HFA384x_RID_CNFPROPOGATIONDELAY	((UINT16)0xFC49) // 1.7.6
-#define		HFA384x_RID_CNFSHORTPREAMBLE	((UINT16)0xFCB0)
-#define		HFA384x_RID_CNFEXCLONGPREAMBLE	((UINT16)0xFCB1)
-#define		HFA384x_RID_CNFAUTHRSPTIMEOUT	((UINT16)0xFCB2)
-#define		HFA384x_RID_CNFBASICRATES	((UINT16)0xFCB3)
-#define		HFA384x_RID_CNFSUPPRATES	((UINT16)0xFCB4)
-#define		HFA384x_RID_CNFFALLBACKCTRL	((UINT16)0xFCB5) // NEW
-#define		HFA384x_RID_WEPKEYSTATUS   	((UINT16)0xFCB6) // NEW
-#define		HFA384x_RID_WEPKEYMAPINDEX 	((UINT16)0xFCB7) // NEW
-#define		HFA384x_RID_BROADCASTKEYID 	((UINT16)0xFCB8) // NEW
-#define		HFA384x_RID_ENTSECFLAGEYID 	((UINT16)0xFCB9) // NEW
-#define		HFA384x_RID_CNFPASSIVESCANCTRL	((UINT16)0xFCBA) // NEW STA
-#define		HFA384x_RID_CNFWPAHANDLING	((UINT16)0xFCBB) // 1.7.0
-#define		HFA384x_RID_MDCCONTROL        	((UINT16)0xFCBC) // 1.7.0/1.4.0
-#define		HFA384x_RID_MDCCOUNTRY        	((UINT16)0xFCBD) // 1.7.0/1.4.0
-#define		HFA384x_RID_TXPOWERMAX        	((UINT16)0xFCBE) // 1.7.0/1.4.0
-#define		HFA384x_RID_CNFLFOENBLED      	((UINT16)0xFCBF) // 1.6.3
-#define         HFA384x_RID_CAPINFO             ((UINT16)0xFCC0) // 1.7.0/1.3.7
-#define         HFA384x_RID_LISTENINTERVAL      ((UINT16)0xFCC1) // 1.7.0/1.3.7
-#define         HFA384x_RID_DIVERSITYENABLED    ((UINT16)0xFCC2) // 1.7.0/1.3.7
-#define         HFA384x_RID_LED_CONTROL         ((UINT16)0xFCC4) // 1.7.6
-#define         HFA384x_RID_HFO_DELAY           ((UINT16)0xFCC5) // 1.7.6
-#define         HFA384x_RID_DISSALOWEDBSSID     ((UINT16)0xFCC6) // 1.8.0
-#define		HFA384x_RID_SCANREQUEST		((UINT16)0xFCE1)
-#define		HFA384x_RID_JOINREQUEST		((UINT16)0xFCE2)
-#define		HFA384x_RID_AUTHENTICATESTA	((UINT16)0xFCE3)
-#define		HFA384x_RID_CHANNELINFOREQUEST	((UINT16)0xFCE4)
-#define		HFA384x_RID_HOSTSCAN          	((UINT16)0xFCE5) // NEW STA
-#define		HFA384x_RID_ASSOCIATESTA	((UINT16)0xFCE6)
-
-#define		HFA384x_RID_CNFWEPDEFAULTKEY_LEN	((UINT16)6)
-#define		HFA384x_RID_CNFWEP128DEFAULTKEY_LEN	((UINT16)14)
-#define		HFA384x_RID_CNFPRIOQUSAGE_LEN		((UINT16)4)
+#define		HFA384x_RID_CNFWEPDEFAULTKEYID	((u16)0xFC23)
+#define		HFA384x_RID_CNFWEPDEFAULTKEY0	((u16)0xFC24)
+#define		HFA384x_RID_CNFWEPDEFAULTKEY1	((u16)0xFC25)
+#define		HFA384x_RID_CNFWEPDEFAULTKEY2	((u16)0xFC26)
+#define		HFA384x_RID_CNFWEPDEFAULTKEY3	((u16)0xFC27)
+#define		HFA384x_RID_CNFWEPFLAGS		((u16)0xFC28)
+#define		HFA384x_RID_CNFWEPKEYMAPTABLE	((u16)0xFC29)
+#define		HFA384x_RID_CNFAUTHENTICATION	((u16)0xFC2A)
+#define		HFA384x_RID_CNFMAXASSOCSTATIONS	((u16)0xFC2B)
+#define		HFA384x_RID_CNFTXCONTROL	((u16)0xFC2C)
+#define		HFA384x_RID_CNFROAMINGMODE	((u16)0xFC2D)
+#define		HFA384x_RID_CNFHOSTAUTHASSOC	((u16)0xFC2E)
+#define		HFA384x_RID_CNFRCVCRCERROR	((u16)0xFC30)
+// #define		HFA384x_RID_CNFMMLIFE		((u16)0xFC31)
+#define		HFA384x_RID_CNFALTRETRYCNT	((u16)0xFC32)
+#define		HFA384x_RID_CNFAPBCNint		((u16)0xFC33)
+#define		HFA384x_RID_CNFAPPCFINFO	((u16)0xFC34)
+#define		HFA384x_RID_CNFSTAPCFINFO	((u16)0xFC35)
+#define		HFA384x_RID_CNFPRIORITYQUSAGE	((u16)0xFC37)
+#define		HFA384x_RID_CNFTIMCTRL		((u16)0xFC40)
+#define		HFA384x_RID_CNFTHIRTY2TALLY	((u16)0xFC42)
+#define		HFA384x_RID_CNFENHSECURITY	((u16)0xFC43)
+#define		HFA384x_RID_CNFDBMADJUST  	((u16)0xFC46) // NEW
+#define		HFA384x_RID_CNFWPADATA       	((u16)0xFC48) // 1.7.0
+#define		HFA384x_RID_CNFPROPOGATIONDELAY	((u16)0xFC49) // 1.7.6
+#define		HFA384x_RID_CNFSHORTPREAMBLE	((u16)0xFCB0)
+#define		HFA384x_RID_CNFEXCLONGPREAMBLE	((u16)0xFCB1)
+#define		HFA384x_RID_CNFAUTHRSPTIMEOUT	((u16)0xFCB2)
+#define		HFA384x_RID_CNFBASICRATES	((u16)0xFCB3)
+#define		HFA384x_RID_CNFSUPPRATES	((u16)0xFCB4)
+#define		HFA384x_RID_CNFFALLBACKCTRL	((u16)0xFCB5) // NEW
+#define		HFA384x_RID_WEPKEYSTATUS   	((u16)0xFCB6) // NEW
+#define		HFA384x_RID_WEPKEYMAPINDEX 	((u16)0xFCB7) // NEW
+#define		HFA384x_RID_BROADCASTKEYID 	((u16)0xFCB8) // NEW
+#define		HFA384x_RID_ENTSECFLAGEYID 	((u16)0xFCB9) // NEW
+#define		HFA384x_RID_CNFPASSIVESCANCTRL	((u16)0xFCBA) // NEW STA
+#define		HFA384x_RID_CNFWPAHANDLING	((u16)0xFCBB) // 1.7.0
+#define		HFA384x_RID_MDCCONTROL        	((u16)0xFCBC) // 1.7.0/1.4.0
+#define		HFA384x_RID_MDCCOUNTRY        	((u16)0xFCBD) // 1.7.0/1.4.0
+#define		HFA384x_RID_TXPOWERMAX        	((u16)0xFCBE) // 1.7.0/1.4.0
+#define		HFA384x_RID_CNFLFOENBLED      	((u16)0xFCBF) // 1.6.3
+#define         HFA384x_RID_CAPINFO             ((u16)0xFCC0) // 1.7.0/1.3.7
+#define         HFA384x_RID_LISTENintERVAL      ((u16)0xFCC1) // 1.7.0/1.3.7
+#define         HFA384x_RID_DIVERSITYENABLED    ((u16)0xFCC2) // 1.7.0/1.3.7
+#define         HFA384x_RID_LED_CONTROL         ((u16)0xFCC4) // 1.7.6
+#define         HFA384x_RID_HFO_DELAY           ((u16)0xFCC5) // 1.7.6
+#define         HFA384x_RID_DISSALOWEDBSSID     ((u16)0xFCC6) // 1.8.0
+#define		HFA384x_RID_SCANREQUEST		((u16)0xFCE1)
+#define		HFA384x_RID_JOINREQUEST		((u16)0xFCE2)
+#define		HFA384x_RID_AUTHENTICATESTA	((u16)0xFCE3)
+#define		HFA384x_RID_CHANNELINFOREQUEST	((u16)0xFCE4)
+#define		HFA384x_RID_HOSTSCAN          	((u16)0xFCE5) // NEW STA
+#define		HFA384x_RID_ASSOCIATESTA	((u16)0xFCE6)
+
+#define		HFA384x_RID_CNFWEPDEFAULTKEY_LEN	((u16)6)
+#define		HFA384x_RID_CNFWEP128DEFAULTKEY_LEN	((u16)14)
+#define		HFA384x_RID_CNFPRIOQUSAGE_LEN		((u16)4)
 /*--------------------------------------------------------------------
 PD Record codes
 --------------------------------------------------------------------*/
-#define HFA384x_PDR_PCB_PARTNUM		((UINT16)0x0001)
-#define HFA384x_PDR_PDAVER		((UINT16)0x0002)
-#define HFA384x_PDR_NIC_SERIAL		((UINT16)0x0003)
-#define HFA384x_PDR_MKK_MEASUREMENTS	((UINT16)0x0004)
-#define HFA384x_PDR_NIC_RAMSIZE		((UINT16)0x0005)
-#define HFA384x_PDR_MFISUPRANGE		((UINT16)0x0006)
-#define HFA384x_PDR_CFISUPRANGE		((UINT16)0x0007)
-#define HFA384x_PDR_NICID		((UINT16)0x0008)
-//#define HFA384x_PDR_REFDAC_MEASUREMENTS	((UINT16)0x0010)
-//#define HFA384x_PDR_VGDAC_MEASUREMENTS	((UINT16)0x0020)
-//#define HFA384x_PDR_LEVEL_COMP_MEASUREMENTS	((UINT16)0x0030)
-//#define HFA384x_PDR_MODEM_TRIMDAC_MEASUREMENTS	((UINT16)0x0040)
-//#define HFA384x_PDR_COREGA_HACK		((UINT16)0x00ff)
-#define HFA384x_PDR_MAC_ADDRESS		((UINT16)0x0101)
-//#define HFA384x_PDR_MKK_CALLNAME	((UINT16)0x0102)
-#define HFA384x_PDR_REGDOMAIN		((UINT16)0x0103)
-#define HFA384x_PDR_ALLOWED_CHANNEL	((UINT16)0x0104)
-#define HFA384x_PDR_DEFAULT_CHANNEL	((UINT16)0x0105)
-//#define HFA384x_PDR_PRIVACY_OPTION	((UINT16)0x0106)
-#define HFA384x_PDR_TEMPTYPE		((UINT16)0x0107)
-//#define HFA384x_PDR_REFDAC_SETUP	((UINT16)0x0110)
-//#define HFA384x_PDR_VGDAC_SETUP		((UINT16)0x0120)
-//#define HFA384x_PDR_LEVEL_COMP_SETUP	((UINT16)0x0130)
-//#define HFA384x_PDR_TRIMDAC_SETUP	((UINT16)0x0140)
-#define HFA384x_PDR_IFR_SETTING		((UINT16)0x0200)
-#define HFA384x_PDR_RFR_SETTING		((UINT16)0x0201)
-#define HFA384x_PDR_HFA3861_BASELINE	((UINT16)0x0202)
-#define HFA384x_PDR_HFA3861_SHADOW	((UINT16)0x0203)
-#define HFA384x_PDR_HFA3861_IFRF	((UINT16)0x0204)
-#define HFA384x_PDR_HFA3861_CHCALSP	((UINT16)0x0300)
-#define HFA384x_PDR_HFA3861_CHCALI	((UINT16)0x0301)
-#define HFA384x_PDR_MAX_TX_POWER  	((UINT16)0x0302)
-#define HFA384x_PDR_MASTER_CHAN_LIST	((UINT16)0x0303)
-#define HFA384x_PDR_3842_NIC_CONFIG	((UINT16)0x0400)
-#define HFA384x_PDR_USB_ID		((UINT16)0x0401)
-#define HFA384x_PDR_PCI_ID		((UINT16)0x0402)
-#define HFA384x_PDR_PCI_IFCONF		((UINT16)0x0403)
-#define HFA384x_PDR_PCI_PMCONF		((UINT16)0x0404)
-#define HFA384x_PDR_RFENRGY		((UINT16)0x0406)
-#define HFA384x_PDR_USB_POWER_TYPE      ((UINT16)0x0407)
-//#define HFA384x_PDR_UNKNOWN408		((UINT16)0x0408)
-#define HFA384x_PDR_USB_MAX_POWER	((UINT16)0x0409)
-#define HFA384x_PDR_USB_MANUFACTURER	((UINT16)0x0410)
-#define HFA384x_PDR_USB_PRODUCT  	((UINT16)0x0411)
-#define HFA384x_PDR_ANT_DIVERSITY   	((UINT16)0x0412)
-#define HFA384x_PDR_HFO_DELAY       	((UINT16)0x0413)
-#define HFA384x_PDR_SCALE_THRESH 	((UINT16)0x0414)
-
-#define HFA384x_PDR_HFA3861_MANF_TESTSP	((UINT16)0x0900)
-#define HFA384x_PDR_HFA3861_MANF_TESTI	((UINT16)0x0901)
-#define HFA384x_PDR_END_OF_PDA		((UINT16)0x0000)
+#define HFA384x_PDR_PCB_PARTNUM		((u16)0x0001)
+#define HFA384x_PDR_PDAVER		((u16)0x0002)
+#define HFA384x_PDR_NIC_SERIAL		((u16)0x0003)
+#define HFA384x_PDR_MKK_MEASUREMENTS	((u16)0x0004)
+#define HFA384x_PDR_NIC_RAMSIZE		((u16)0x0005)
+#define HFA384x_PDR_MFISUPRANGE		((u16)0x0006)
+#define HFA384x_PDR_CFISUPRANGE		((u16)0x0007)
+#define HFA384x_PDR_NICID		((u16)0x0008)
+//#define HFA384x_PDR_REFDAC_MEASUREMENTS	((u16)0x0010)
+//#define HFA384x_PDR_VGDAC_MEASUREMENTS	((u16)0x0020)
+//#define HFA384x_PDR_LEVEL_COMP_MEASUREMENTS	((u16)0x0030)
+//#define HFA384x_PDR_MODEM_TRIMDAC_MEASUREMENTS	((u16)0x0040)
+//#define HFA384x_PDR_COREGA_HACK		((u16)0x00ff)
+#define HFA384x_PDR_MAC_ADDRESS		((u16)0x0101)
+//#define HFA384x_PDR_MKK_CALLNAME	((u16)0x0102)
+#define HFA384x_PDR_REGDOMAIN		((u16)0x0103)
+#define HFA384x_PDR_ALLOWED_CHANNEL	((u16)0x0104)
+#define HFA384x_PDR_DEFAULT_CHANNEL	((u16)0x0105)
+//#define HFA384x_PDR_PRIVACY_OPTION	((u16)0x0106)
+#define HFA384x_PDR_TEMPTYPE		((u16)0x0107)
+//#define HFA384x_PDR_REFDAC_SETUP	((u16)0x0110)
+//#define HFA384x_PDR_VGDAC_SETUP		((u16)0x0120)
+//#define HFA384x_PDR_LEVEL_COMP_SETUP	((u16)0x0130)
+//#define HFA384x_PDR_TRIMDAC_SETUP	((u16)0x0140)
+#define HFA384x_PDR_IFR_SETTING		((u16)0x0200)
+#define HFA384x_PDR_RFR_SETTING		((u16)0x0201)
+#define HFA384x_PDR_HFA3861_BASELINE	((u16)0x0202)
+#define HFA384x_PDR_HFA3861_SHADOW	((u16)0x0203)
+#define HFA384x_PDR_HFA3861_IFRF	((u16)0x0204)
+#define HFA384x_PDR_HFA3861_CHCALSP	((u16)0x0300)
+#define HFA384x_PDR_HFA3861_CHCALI	((u16)0x0301)
+#define HFA384x_PDR_MAX_TX_POWER  	((u16)0x0302)
+#define HFA384x_PDR_MASTER_CHAN_LIST	((u16)0x0303)
+#define HFA384x_PDR_3842_NIC_CONFIG	((u16)0x0400)
+#define HFA384x_PDR_USB_ID		((u16)0x0401)
+#define HFA384x_PDR_PCI_ID		((u16)0x0402)
+#define HFA384x_PDR_PCI_IFCONF		((u16)0x0403)
+#define HFA384x_PDR_PCI_PMCONF		((u16)0x0404)
+#define HFA384x_PDR_RFENRGY		((u16)0x0406)
+#define HFA384x_PDR_USB_POWER_TYPE      ((u16)0x0407)
+//#define HFA384x_PDR_UNKNOWN408		((u16)0x0408)
+#define HFA384x_PDR_USB_MAX_POWER	((u16)0x0409)
+#define HFA384x_PDR_USB_MANUFACTURER	((u16)0x0410)
+#define HFA384x_PDR_USB_PRODUCT  	((u16)0x0411)
+#define HFA384x_PDR_ANT_DIVERSITY   	((u16)0x0412)
+#define HFA384x_PDR_HFO_DELAY       	((u16)0x0413)
+#define HFA384x_PDR_SCALE_THRESH 	((u16)0x0414)
+
+#define HFA384x_PDR_HFA3861_MANF_TESTSP	((u16)0x0900)
+#define HFA384x_PDR_HFA3861_MANF_TESTI	((u16)0x0901)
+#define HFA384x_PDR_END_OF_PDA		((u16)0x0000)
 
 
 /*=============================================================*/
@@ -802,7 +766,7 @@ PD Record codes
 #define		HFA384x_OFFSET1		HFA384x_OFFSET1_OFF
 #define		HFA384x_DATA1		HFA384x_DATA1_OFF
 #define		HFA384x_EVSTAT		HFA384x_EVSTAT_OFF
-#define		HFA384x_INTEN		HFA384x_INTEN_OFF
+#define		HFA384x_intEN		HFA384x_INTEN_OFF
 #define		HFA384x_EVACK		HFA384x_EVACK_OFF
 #define		HFA384x_CONTROL		HFA384x_CONTROL_OFF
 #define		HFA384x_SWSUPPORT0	HFA384x_SWSUPPORT0_OFF
@@ -817,96 +781,96 @@ PD Record codes
 
 /*--- Register Test/Get/Set Field macros ------------------------*/
 
-#define		HFA384x_CMD_ISBUSY(value)		((UINT16)(((UINT16)value) & HFA384x_CMD_BUSY))
-#define		HFA384x_CMD_AINFO_GET(value)		((UINT16)(((UINT16)(value) & HFA384x_CMD_AINFO) >> 8))
-#define		HFA384x_CMD_AINFO_SET(value)		((UINT16)((UINT16)(value) << 8))
-#define		HFA384x_CMD_MACPORT_GET(value)		((UINT16)(HFA384x_CMD_AINFO_GET((UINT16)(value) & HFA384x_CMD_MACPORT)))
-#define		HFA384x_CMD_MACPORT_SET(value)		((UINT16)HFA384x_CMD_AINFO_SET(value))
-#define		HFA384x_CMD_ISRECL(value)		((UINT16)(HFA384x_CMD_AINFO_GET((UINT16)(value) & HFA384x_CMD_RECL)))
-#define		HFA384x_CMD_RECL_SET(value)		((UINT16)HFA384x_CMD_AINFO_SET(value))
-#define		HFA384x_CMD_QOS_GET(value)		((UINT16)((((UINT16)(value))&((UINT16)0x3000)) >> 12))
-#define		HFA384x_CMD_QOS_SET(value)		((UINT16)((((UINT16)(value)) << 12) & 0x3000))
-#define		HFA384x_CMD_ISWRITE(value)		((UINT16)(HFA384x_CMD_AINFO_GET((UINT16)(value) & HFA384x_CMD_WRITE)))
-#define		HFA384x_CMD_WRITE_SET(value)		((UINT16)HFA384x_CMD_AINFO_SET((UINT16)value))
-#define		HFA384x_CMD_PROGMODE_GET(value)		((UINT16)(HFA384x_CMD_AINFO_GET((UINT16)(value) & HFA384x_CMD_PROGMODE)))
-#define		HFA384x_CMD_PROGMODE_SET(value)		((UINT16)HFA384x_CMD_AINFO_SET((UINT16)value))
-#define		HFA384x_CMD_CMDCODE_GET(value)		((UINT16)(((UINT16)(value)) & HFA384x_CMD_CMDCODE))
-#define		HFA384x_CMD_CMDCODE_SET(value)		((UINT16)(value))
-
-#define		HFA384x_STATUS_RESULT_GET(value)	((UINT16)((((UINT16)(value)) & HFA384x_STATUS_RESULT) >> 8))
-#define		HFA384x_STATUS_RESULT_SET(value)	(((UINT16)(value)) << 8)
-#define		HFA384x_STATUS_CMDCODE_GET(value)	(((UINT16)(value)) & HFA384x_STATUS_CMDCODE)
-#define		HFA384x_STATUS_CMDCODE_SET(value)	((UINT16)(value))
-
-#define		HFA384x_OFFSET_ISBUSY(value)		((UINT16)(((UINT16)(value)) & HFA384x_OFFSET_BUSY))
-#define		HFA384x_OFFSET_ISERR(value)		((UINT16)(((UINT16)(value)) & HFA384x_OFFSET_ERR))
-#define		HFA384x_OFFSET_DATAOFF_GET(value)	((UINT16)(((UINT16)(value)) & HFA384x_OFFSET_DATAOFF))
-#define		HFA384x_OFFSET_DATAOFF_SET(value)	((UINT16)(value))
-
-#define		HFA384x_EVSTAT_ISTICK(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVSTAT_TICK))
-#define		HFA384x_EVSTAT_ISWTERR(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVSTAT_WTERR))
-#define		HFA384x_EVSTAT_ISINFDROP(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVSTAT_INFDROP))
-#define		HFA384x_EVSTAT_ISINFO(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVSTAT_INFO))
-#define		HFA384x_EVSTAT_ISDTIM(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVSTAT_DTIM))
-#define		HFA384x_EVSTAT_ISCMD(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVSTAT_CMD))
-#define		HFA384x_EVSTAT_ISALLOC(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVSTAT_ALLOC))
-#define		HFA384x_EVSTAT_ISTXEXC(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVSTAT_TXEXC))
-#define		HFA384x_EVSTAT_ISTX(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVSTAT_TX))
-#define		HFA384x_EVSTAT_ISRX(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVSTAT_RX))
-
-#define		HFA384x_EVSTAT_ISBAP_OP(value)		((UINT16)(((UINT16)(value)) & HFA384x_INT_BAP_OP))
-
-#define		HFA384x_INTEN_ISTICK(value)		((UINT16)(((UINT16)(value)) & HFA384x_INTEN_TICK))
-#define		HFA384x_INTEN_TICK_SET(value)		((UINT16)(((UINT16)(value)) << 15))
-#define		HFA384x_INTEN_ISWTERR(value)		((UINT16)(((UINT16)(value)) & HFA384x_INTEN_WTERR))
-#define		HFA384x_INTEN_WTERR_SET(value)		((UINT16)(((UINT16)(value)) << 14))
-#define		HFA384x_INTEN_ISINFDROP(value)		((UINT16)(((UINT16)(value)) & HFA384x_INTEN_INFDROP))
-#define		HFA384x_INTEN_INFDROP_SET(value)	((UINT16)(((UINT16)(value)) << 13))
-#define		HFA384x_INTEN_ISINFO(value)		((UINT16)(((UINT16)(value)) & HFA384x_INTEN_INFO))
-#define		HFA384x_INTEN_INFO_SET(value)		((UINT16)(((UINT16)(value)) << 7))
-#define		HFA384x_INTEN_ISDTIM(value)		((UINT16)(((UINT16)(value)) & HFA384x_INTEN_DTIM))
-#define		HFA384x_INTEN_DTIM_SET(value)		((UINT16)(((UINT16)(value)) << 5))
-#define		HFA384x_INTEN_ISCMD(value)		((UINT16)(((UINT16)(value)) & HFA384x_INTEN_CMD))
-#define		HFA384x_INTEN_CMD_SET(value)		((UINT16)(((UINT16)(value)) << 4))
-#define		HFA384x_INTEN_ISALLOC(value)		((UINT16)(((UINT16)(value)) & HFA384x_INTEN_ALLOC))
-#define		HFA384x_INTEN_ALLOC_SET(value)		((UINT16)(((UINT16)(value)) << 3))
-#define		HFA384x_INTEN_ISTXEXC(value)		((UINT16)(((UINT16)(value)) & HFA384x_INTEN_TXEXC))
-#define		HFA384x_INTEN_TXEXC_SET(value)		((UINT16)(((UINT16)(value)) << 2))
-#define		HFA384x_INTEN_ISTX(value)		((UINT16)(((UINT16)(value)) & HFA384x_INTEN_TX))
-#define		HFA384x_INTEN_TX_SET(value)		((UINT16)(((UINT16)(value)) << 1))
-#define		HFA384x_INTEN_ISRX(value)		((UINT16)(((UINT16)(value)) & HFA384x_INTEN_RX))
-#define		HFA384x_INTEN_RX_SET(value)		((UINT16)(((UINT16)(value)) << 0))
-
-#define		HFA384x_EVACK_ISTICK(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVACK_TICK))
-#define		HFA384x_EVACK_TICK_SET(value)		((UINT16)(((UINT16)(value)) << 15))
-#define		HFA384x_EVACK_ISWTERR(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVACK_WTERR))
-#define		HFA384x_EVACK_WTERR_SET(value)		((UINT16)(((UINT16)(value)) << 14))
-#define		HFA384x_EVACK_ISINFDROP(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVACK_INFDROP))
-#define		HFA384x_EVACK_INFDROP_SET(value)	((UINT16)(((UINT16)(value)) << 13))
-#define		HFA384x_EVACK_ISINFO(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVACK_INFO))
-#define		HFA384x_EVACK_INFO_SET(value)		((UINT16)(((UINT16)(value)) << 7))
-#define		HFA384x_EVACK_ISDTIM(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVACK_DTIM))
-#define		HFA384x_EVACK_DTIM_SET(value)		((UINT16)(((UINT16)(value)) << 5))
-#define		HFA384x_EVACK_ISCMD(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVACK_CMD))
-#define		HFA384x_EVACK_CMD_SET(value)		((UINT16)(((UINT16)(value)) << 4))
-#define		HFA384x_EVACK_ISALLOC(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVACK_ALLOC))
-#define		HFA384x_EVACK_ALLOC_SET(value)		((UINT16)(((UINT16)(value)) << 3))
-#define		HFA384x_EVACK_ISTXEXC(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVACK_TXEXC))
-#define		HFA384x_EVACK_TXEXC_SET(value)		((UINT16)(((UINT16)(value)) << 2))
-#define		HFA384x_EVACK_ISTX(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVACK_TX))
-#define		HFA384x_EVACK_TX_SET(value)		((UINT16)(((UINT16)(value)) << 1))
-#define		HFA384x_EVACK_ISRX(value)		((UINT16)(((UINT16)(value)) & HFA384x_EVACK_RX))
-#define		HFA384x_EVACK_RX_SET(value)		((UINT16)(((UINT16)(value)) << 0))
-
-#define		HFA384x_CONTROL_AUXEN_SET(value)	((UINT16)(((UINT16)(value)) << 14))
-#define		HFA384x_CONTROL_AUXEN_GET(value)	((UINT16)(((UINT16)(value)) >> 14))
+#define		HFA384x_CMD_ISBUSY(value)		((u16)(((u16)value) & HFA384x_CMD_BUSY))
+#define		HFA384x_CMD_AINFO_GET(value)		((u16)(((u16)(value) & HFA384x_CMD_AINFO) >> 8))
+#define		HFA384x_CMD_AINFO_SET(value)		((u16)((u16)(value) << 8))
+#define		HFA384x_CMD_MACPORT_GET(value)		((u16)(HFA384x_CMD_AINFO_GET((u16)(value) & HFA384x_CMD_MACPORT)))
+#define		HFA384x_CMD_MACPORT_SET(value)		((u16)HFA384x_CMD_AINFO_SET(value))
+#define		HFA384x_CMD_ISRECL(value)		((u16)(HFA384x_CMD_AINFO_GET((u16)(value) & HFA384x_CMD_RECL)))
+#define		HFA384x_CMD_RECL_SET(value)		((u16)HFA384x_CMD_AINFO_SET(value))
+#define		HFA384x_CMD_QOS_GET(value)		((u16)((((u16)(value))&((u16)0x3000)) >> 12))
+#define		HFA384x_CMD_QOS_SET(value)		((u16)((((u16)(value)) << 12) & 0x3000))
+#define		HFA384x_CMD_ISWRITE(value)		((u16)(HFA384x_CMD_AINFO_GET((u16)(value) & HFA384x_CMD_WRITE)))
+#define		HFA384x_CMD_WRITE_SET(value)		((u16)HFA384x_CMD_AINFO_SET((u16)value))
+#define		HFA384x_CMD_PROGMODE_GET(value)		((u16)(HFA384x_CMD_AINFO_GET((u16)(value) & HFA384x_CMD_PROGMODE)))
+#define		HFA384x_CMD_PROGMODE_SET(value)		((u16)HFA384x_CMD_AINFO_SET((u16)value))
+#define		HFA384x_CMD_CMDCODE_GET(value)		((u16)(((u16)(value)) & HFA384x_CMD_CMDCODE))
+#define		HFA384x_CMD_CMDCODE_SET(value)		((u16)(value))
+
+#define		HFA384x_STATUS_RESULT_GET(value)	((u16)((((u16)(value)) & HFA384x_STATUS_RESULT) >> 8))
+#define		HFA384x_STATUS_RESULT_SET(value)	(((u16)(value)) << 8)
+#define		HFA384x_STATUS_CMDCODE_GET(value)	(((u16)(value)) & HFA384x_STATUS_CMDCODE)
+#define		HFA384x_STATUS_CMDCODE_SET(value)	((u16)(value))
+
+#define		HFA384x_OFFSET_ISBUSY(value)		((u16)(((u16)(value)) & HFA384x_OFFSET_BUSY))
+#define		HFA384x_OFFSET_ISERR(value)		((u16)(((u16)(value)) & HFA384x_OFFSET_ERR))
+#define		HFA384x_OFFSET_DATAOFF_GET(value)	((u16)(((u16)(value)) & HFA384x_OFFSET_DATAOFF))
+#define		HFA384x_OFFSET_DATAOFF_SET(value)	((u16)(value))
+
+#define		HFA384x_EVSTAT_ISTICK(value)		((u16)(((u16)(value)) & HFA384x_EVSTAT_TICK))
+#define		HFA384x_EVSTAT_ISWTERR(value)		((u16)(((u16)(value)) & HFA384x_EVSTAT_WTERR))
+#define		HFA384x_EVSTAT_ISINFDROP(value)		((u16)(((u16)(value)) & HFA384x_EVSTAT_INFDROP))
+#define		HFA384x_EVSTAT_ISINFO(value)		((u16)(((u16)(value)) & HFA384x_EVSTAT_INFO))
+#define		HFA384x_EVSTAT_ISDTIM(value)		((u16)(((u16)(value)) & HFA384x_EVSTAT_DTIM))
+#define		HFA384x_EVSTAT_ISCMD(value)		((u16)(((u16)(value)) & HFA384x_EVSTAT_CMD))
+#define		HFA384x_EVSTAT_ISALLOC(value)		((u16)(((u16)(value)) & HFA384x_EVSTAT_ALLOC))
+#define		HFA384x_EVSTAT_ISTXEXC(value)		((u16)(((u16)(value)) & HFA384x_EVSTAT_TXEXC))
+#define		HFA384x_EVSTAT_ISTX(value)		((u16)(((u16)(value)) & HFA384x_EVSTAT_TX))
+#define		HFA384x_EVSTAT_ISRX(value)		((u16)(((u16)(value)) & HFA384x_EVSTAT_RX))
+
+#define		HFA384x_EVSTAT_ISBAP_OP(value)		((u16)(((u16)(value)) & HFA384x_int_BAP_OP))
+
+#define		HFA384x_intEN_ISTICK(value)		((u16)(((u16)(value)) & HFA384x_INTEN_TICK))
+#define		HFA384x_intEN_TICK_SET(value)		((u16)(((u16)(value)) << 15))
+#define		HFA384x_intEN_ISWTERR(value)		((u16)(((u16)(value)) & HFA384x_INTEN_WTERR))
+#define		HFA384x_intEN_WTERR_SET(value)		((u16)(((u16)(value)) << 14))
+#define		HFA384x_intEN_ISINFDROP(value)		((u16)(((u16)(value)) & HFA384x_INTEN_INFDROP))
+#define		HFA384x_intEN_INFDROP_SET(value)	((u16)(((u16)(value)) << 13))
+#define		HFA384x_intEN_ISINFO(value)		((u16)(((u16)(value)) & HFA384x_INTEN_INFO))
+#define		HFA384x_intEN_INFO_SET(value)		((u16)(((u16)(value)) << 7))
+#define		HFA384x_intEN_ISDTIM(value)		((u16)(((u16)(value)) & HFA384x_INTEN_DTIM))
+#define		HFA384x_intEN_DTIM_SET(value)		((u16)(((u16)(value)) << 5))
+#define		HFA384x_intEN_ISCMD(value)		((u16)(((u16)(value)) & HFA384x_INTEN_CMD))
+#define		HFA384x_intEN_CMD_SET(value)		((u16)(((u16)(value)) << 4))
+#define		HFA384x_intEN_ISALLOC(value)		((u16)(((u16)(value)) & HFA384x_INTEN_ALLOC))
+#define		HFA384x_intEN_ALLOC_SET(value)		((u16)(((u16)(value)) << 3))
+#define		HFA384x_intEN_ISTXEXC(value)		((u16)(((u16)(value)) & HFA384x_INTEN_TXEXC))
+#define		HFA384x_intEN_TXEXC_SET(value)		((u16)(((u16)(value)) << 2))
+#define		HFA384x_intEN_ISTX(value)		((u16)(((u16)(value)) & HFA384x_INTEN_TX))
+#define		HFA384x_intEN_TX_SET(value)		((u16)(((u16)(value)) << 1))
+#define		HFA384x_intEN_ISRX(value)		((u16)(((u16)(value)) & HFA384x_INTEN_RX))
+#define		HFA384x_intEN_RX_SET(value)		((u16)(((u16)(value)) << 0))
+
+#define		HFA384x_EVACK_ISTICK(value)		((u16)(((u16)(value)) & HFA384x_EVACK_TICK))
+#define		HFA384x_EVACK_TICK_SET(value)		((u16)(((u16)(value)) << 15))
+#define		HFA384x_EVACK_ISWTERR(value)		((u16)(((u16)(value)) & HFA384x_EVACK_WTERR))
+#define		HFA384x_EVACK_WTERR_SET(value)		((u16)(((u16)(value)) << 14))
+#define		HFA384x_EVACK_ISINFDROP(value)		((u16)(((u16)(value)) & HFA384x_EVACK_INFDROP))
+#define		HFA384x_EVACK_INFDROP_SET(value)	((u16)(((u16)(value)) << 13))
+#define		HFA384x_EVACK_ISINFO(value)		((u16)(((u16)(value)) & HFA384x_EVACK_INFO))
+#define		HFA384x_EVACK_INFO_SET(value)		((u16)(((u16)(value)) << 7))
+#define		HFA384x_EVACK_ISDTIM(value)		((u16)(((u16)(value)) & HFA384x_EVACK_DTIM))
+#define		HFA384x_EVACK_DTIM_SET(value)		((u16)(((u16)(value)) << 5))
+#define		HFA384x_EVACK_ISCMD(value)		((u16)(((u16)(value)) & HFA384x_EVACK_CMD))
+#define		HFA384x_EVACK_CMD_SET(value)		((u16)(((u16)(value)) << 4))
+#define		HFA384x_EVACK_ISALLOC(value)		((u16)(((u16)(value)) & HFA384x_EVACK_ALLOC))
+#define		HFA384x_EVACK_ALLOC_SET(value)		((u16)(((u16)(value)) << 3))
+#define		HFA384x_EVACK_ISTXEXC(value)		((u16)(((u16)(value)) & HFA384x_EVACK_TXEXC))
+#define		HFA384x_EVACK_TXEXC_SET(value)		((u16)(((u16)(value)) << 2))
+#define		HFA384x_EVACK_ISTX(value)		((u16)(((u16)(value)) & HFA384x_EVACK_TX))
+#define		HFA384x_EVACK_TX_SET(value)		((u16)(((u16)(value)) << 1))
+#define		HFA384x_EVACK_ISRX(value)		((u16)(((u16)(value)) & HFA384x_EVACK_RX))
+#define		HFA384x_EVACK_RX_SET(value)		((u16)(((u16)(value)) << 0))
+
+#define		HFA384x_CONTROL_AUXEN_SET(value)	((u16)(((u16)(value)) << 14))
+#define		HFA384x_CONTROL_AUXEN_GET(value)	((u16)(((u16)(value)) >> 14))
 
 /* Byte Order */
 #ifdef __KERNEL__
-#define hfa384x2host_16(n)	(__le16_to_cpu((UINT16)(n)))
-#define hfa384x2host_32(n)	(__le32_to_cpu((UINT32)(n)))
-#define host2hfa384x_16(n)	(__cpu_to_le16((UINT16)(n)))
-#define host2hfa384x_32(n)	(__cpu_to_le32((UINT32)(n)))
+#define hfa384x2host_16(n)	(__le16_to_cpu((u16)(n)))
+#define hfa384x2host_32(n)	(__le32_to_cpu((u32)(n)))
+#define host2hfa384x_16(n)	(__cpu_to_le16((u16)(n)))
+#define host2hfa384x_32(n)	(__cpu_to_le32((u32)(n)))
 #endif
 
 /* Host Maintained State Info */
@@ -927,14 +891,14 @@ PD Record codes
 /* Commonly used basic types */
 typedef struct hfa384x_bytestr
 {
-	UINT16	len;
-	UINT8	data[0];
+	u16	len;
+	u8	data[0];
 } __WLAN_ATTRIB_PACK__ hfa384x_bytestr_t;
 
 typedef struct hfa384x_bytestr32
 {
-	UINT16	len;
-	UINT8	data[32];
+	u16	len;
+	u8	data[32];
 } __WLAN_ATTRIB_PACK__ hfa384x_bytestr32_t;
 
 /*--------------------------------------------------------------------
@@ -946,112 +910,112 @@ these members */
 
 typedef struct hfa384x_record
 {
-	UINT16	reclen;
-	UINT16	rid;
+	u16	reclen;
+	u16	rid;
 } __WLAN_ATTRIB_PACK__ hfa384x_rec_t;
 
 typedef struct hfa384x_record16
 {
-	UINT16	reclen;
-	UINT16	rid;
-	UINT16	val;
+	u16	reclen;
+	u16	rid;
+	u16	val;
 } __WLAN_ATTRIB_PACK__ hfa384x_rec16_t;
 
 typedef struct hfa384x_record32
 {
-	UINT16	reclen;
-	UINT16	rid;
-	UINT32	val;
+	u16	reclen;
+	u16	rid;
+	u32	val;
 } __WLAN_ATTRIB_PACK__ hfa384x_rec32;
 
 /*-- Hardware/Firmware Component Information ----------*/
 typedef struct hfa384x_compident
 {
-	UINT16	id;
-	UINT16	variant;
-	UINT16	major;
-	UINT16	minor;
+	u16	id;
+	u16	variant;
+	u16	major;
+	u16	minor;
 } __WLAN_ATTRIB_PACK__ hfa384x_compident_t;
 
 typedef struct hfa384x_caplevel
 {
-	UINT16	role;
-	UINT16	id;
-	UINT16	variant;
-	UINT16	bottom;
-	UINT16	top;
+	u16	role;
+	u16	id;
+	u16	variant;
+	u16	bottom;
+	u16	top;
 } __WLAN_ATTRIB_PACK__ hfa384x_caplevel_t;
 
 /*-- Configuration Record: cnfPortType --*/
 typedef struct hfa384x_cnfPortType
 {
-	UINT16	cnfPortType;
+	u16	cnfPortType;
 } __WLAN_ATTRIB_PACK__ hfa384x_cnfPortType_t;
 
 /*-- Configuration Record: cnfOwnMACAddress --*/
 typedef struct hfa384x_cnfOwnMACAddress
 {
-	UINT8	cnfOwnMACAddress[6];
+	u8	cnfOwnMACAddress[6];
 } __WLAN_ATTRIB_PACK__ hfa384x_cnfOwnMACAddress_t;
 
 /*-- Configuration Record: cnfDesiredSSID --*/
 typedef struct hfa384x_cnfDesiredSSID
 {
-	UINT8	cnfDesiredSSID[34];
+	u8	cnfDesiredSSID[34];
 } __WLAN_ATTRIB_PACK__ hfa384x_cnfDesiredSSID_t;
 
 /*-- Configuration Record: cnfOwnChannel --*/
 typedef struct hfa384x_cnfOwnChannel
 {
-	UINT16	cnfOwnChannel;
+	u16	cnfOwnChannel;
 } __WLAN_ATTRIB_PACK__ hfa384x_cnfOwnChannel_t;
 
 /*-- Configuration Record: cnfOwnSSID --*/
 typedef struct hfa384x_cnfOwnSSID
 {
-	UINT8	cnfOwnSSID[34];
+	u8	cnfOwnSSID[34];
 } __WLAN_ATTRIB_PACK__ hfa384x_cnfOwnSSID_t;
 
 /*-- Configuration Record: cnfOwnATIMWindow --*/
 typedef struct hfa384x_cnfOwnATIMWindow
 {
-	UINT16	cnfOwnATIMWindow;
+	u16	cnfOwnATIMWindow;
 } __WLAN_ATTRIB_PACK__ hfa384x_cnfOwnATIMWindow_t;
 
 /*-- Configuration Record: cnfSystemScale --*/
 typedef struct hfa384x_cnfSystemScale
 {
-	UINT16	cnfSystemScale;
+	u16	cnfSystemScale;
 } __WLAN_ATTRIB_PACK__ hfa384x_cnfSystemScale_t;
 
 /*-- Configuration Record: cnfMaxDataLength --*/
 typedef struct hfa384x_cnfMaxDataLength
 {
-	UINT16	cnfMaxDataLength;
+	u16	cnfMaxDataLength;
 } __WLAN_ATTRIB_PACK__ hfa384x_cnfMaxDataLength_t;
 
 /*-- Configuration Record: cnfWDSAddress --*/
 typedef struct hfa384x_cnfWDSAddress
 {
-	UINT8	cnfWDSAddress[6];
+	u8	cnfWDSAddress[6];
 } __WLAN_ATTRIB_PACK__ hfa384x_cnfWDSAddress_t;
 
 /*-- Configuration Record: cnfPMEnabled --*/
 typedef struct hfa384x_cnfPMEnabled
 {
-	UINT16	cnfPMEnabled;
+	u16	cnfPMEnabled;
 } __WLAN_ATTRIB_PACK__ hfa384x_cnfPMEnabled_t;
 
 /*-- Configuration Record: cnfPMEPS --*/
 typedef struct hfa384x_cnfPMEPS
 {
-	UINT16	cnfPMEPS;
+	u16	cnfPMEPS;
 } __WLAN_ATTRIB_PACK__ hfa384x_cnfPMEPS_t;
 
 /*-- Configuration Record: cnfMulticastReceive --*/
 typedef struct hfa384x_cnfMulticastReceive
 {
-	UINT16	cnfMulticastReceive;
+	u16	cnfMulticastReceive;
 } __WLAN_ATTRIB_PACK__ hfa384x_cnfMulticastReceive_t;
 
 /*-- Configuration Record: cnfAuthentication --*/
@@ -1062,37 +1026,37 @@ typedef struct hfa384x_cnfMulticastReceive
 /*-- Configuration Record: cnfMaxSleepDuration --*/
 typedef struct hfa384x_cnfMaxSleepDuration
 {
-	UINT16	cnfMaxSleepDuration;
+	u16	cnfMaxSleepDuration;
 } __WLAN_ATTRIB_PACK__ hfa384x_cnfMaxSleepDuration_t;
 
 /*-- Configuration Record: cnfPMHoldoverDuration --*/
 typedef struct hfa384x_cnfPMHoldoverDuration
 {
-	UINT16	cnfPMHoldoverDuration;
+	u16	cnfPMHoldoverDuration;
 } __WLAN_ATTRIB_PACK__ hfa384x_cnfPMHoldoverDuration_t;
 
 /*-- Configuration Record: cnfOwnName --*/
 typedef struct hfa384x_cnfOwnName
 {
-	UINT8	cnfOwnName[34];
+	u8	cnfOwnName[34];
 } __WLAN_ATTRIB_PACK__ hfa384x_cnfOwnName_t;
 
 /*-- Configuration Record: cnfOwnDTIMPeriod --*/
 typedef struct hfa384x_cnfOwnDTIMPeriod
 {
-	UINT16	cnfOwnDTIMPeriod;
+	u16	cnfOwnDTIMPeriod;
 } __WLAN_ATTRIB_PACK__ hfa384x_cnfOwnDTIMPeriod_t;
 
 /*-- Configuration Record: cnfWDSAddress --*/
 typedef struct hfa384x_cnfWDSAddressN
 {
-	UINT8	cnfWDSAddress[6];
+	u8	cnfWDSAddress[6];
 } __WLAN_ATTRIB_PACK__ hfa384x_cnfWDSAddressN_t;
 
 /*-- Configuration Record: cnfMulticastPMBuffering --*/
 typedef struct hfa384x_cnfMulticastPMBuffering
 {
-	UINT16	cnfMulticastPMBuffering;
+	u16	cnfMulticastPMBuffering;
 } __WLAN_ATTRIB_PACK__ hfa384x_cnfMulticastPMBuffering_t;
 
 /*--------------------------------------------------------------------
@@ -1103,13 +1067,13 @@ Configuration Record Structures:
 /*-- Configuration Record: GroupAddresses --*/
 typedef struct hfa384x_GroupAddresses
 {
-	UINT8	MACAddress[16][6];
+	u8	MACAddress[16][6];
 } __WLAN_ATTRIB_PACK__ hfa384x_GroupAddresses_t;
 
 /*-- Configuration Record: CreateIBSS --*/
 typedef struct hfa384x_CreateIBSS
 {
-	UINT16	CreateIBSS;
+	u16	CreateIBSS;
 } __WLAN_ATTRIB_PACK__ hfa384x_CreateIBSS_t;
 
 #define HFA384x_CREATEIBSS_JOINCREATEIBSS          0
@@ -1120,87 +1084,87 @@ typedef struct hfa384x_CreateIBSS
 /*-- Configuration Record: FragmentationThreshold --*/
 typedef struct hfa384x_FragmentationThreshold
 {
-	UINT16	FragmentationThreshold;
+	u16	FragmentationThreshold;
 } __WLAN_ATTRIB_PACK__ hfa384x_FragmentationThreshold_t;
 
 /*-- Configuration Record: RTSThreshold --*/
 typedef struct hfa384x_RTSThreshold
 {
-	UINT16	RTSThreshold;
+	u16	RTSThreshold;
 } __WLAN_ATTRIB_PACK__ hfa384x_RTSThreshold_t;
 
 /*-- Configuration Record: TxRateControl --*/
 typedef struct hfa384x_TxRateControl
 {
-	UINT16	TxRateControl;
+	u16	TxRateControl;
 } __WLAN_ATTRIB_PACK__ hfa384x_TxRateControl_t;
 
 /*-- Configuration Record: PromiscuousMode --*/
 typedef struct hfa384x_PromiscuousMode
 {
-	UINT16	PromiscuousMode;
+	u16	PromiscuousMode;
 } __WLAN_ATTRIB_PACK__ hfa384x_PromiscuousMode_t;
 
 /*-- Configuration Record: ScanRequest (data portion only) --*/
 typedef struct hfa384x_ScanRequest_data
 {
-	UINT16	channelList;
-	UINT16	txRate;
+	u16	channelList;
+	u16	txRate;
 } __WLAN_ATTRIB_PACK__ hfa384x_ScanRequest_data_t;
 
 /*-- Configuration Record: HostScanRequest (data portion only) --*/
 typedef struct hfa384x_HostScanRequest_data
 {
-	UINT16	channelList;
-	UINT16	txRate;
+	u16	channelList;
+	u16	txRate;
 	hfa384x_bytestr32_t ssid;
 } __WLAN_ATTRIB_PACK__ hfa384x_HostScanRequest_data_t;
 
 /*-- Configuration Record: JoinRequest (data portion only) --*/
 typedef struct hfa384x_JoinRequest_data
 {
-	UINT8	bssid[WLAN_BSSID_LEN];
-	UINT16	channel;
+	u8	bssid[WLAN_BSSID_LEN];
+	u16	channel;
 } __WLAN_ATTRIB_PACK__ hfa384x_JoinRequest_data_t;
 
 /*-- Configuration Record: authenticateStation (data portion only) --*/
 typedef struct hfa384x_authenticateStation_data
 {
-	UINT8	address[WLAN_ADDR_LEN];
-	UINT16	status;
-	UINT16	algorithm;
+	u8	address[WLAN_ADDR_LEN];
+	u16	status;
+	u16	algorithm;
 } __WLAN_ATTRIB_PACK__ hfa384x_authenticateStation_data_t;
 
 /*-- Configuration Record: associateStation (data portion only) --*/
 typedef struct hfa384x_associateStation_data
 {
-	UINT8	address[WLAN_ADDR_LEN];
-	UINT16	status;
-	UINT16	type;
+	u8	address[WLAN_ADDR_LEN];
+	u16	status;
+	u16	type;
 } __WLAN_ATTRIB_PACK__ hfa384x_associateStation_data_t;
 
 /*-- Configuration Record: ChannelInfoRequest (data portion only) --*/
 typedef struct hfa384x_ChannelInfoRequest_data
 {
-	UINT16	channelList;
-	UINT16	channelDwellTime;
+	u16	channelList;
+	u16	channelDwellTime;
 } __WLAN_ATTRIB_PACK__ hfa384x_ChannelInfoRequest_data_t;
 
 /*-- Configuration Record: WEPKeyMapping (data portion only) --*/
 typedef struct hfa384x_WEPKeyMapping
 {
-	UINT8	address[WLAN_ADDR_LEN];
-	UINT16	key_index;
-	UINT8 	key[16];
-	UINT8 	mic_transmit_key[4];
-	UINT8 	mic_receive_key[4];
+	u8	address[WLAN_ADDR_LEN];
+	u16	key_index;
+	u8 	key[16];
+	u8 	mic_transmit_key[4];
+	u8 	mic_receive_key[4];
 } __WLAN_ATTRIB_PACK__ hfa384x_WEPKeyMapping_t;
 
 /*-- Configuration Record: WPAData       (data portion only) --*/
 typedef struct hfa384x_WPAData
 {
-	UINT16	datalen;
-        UINT8 	data[0]; // max 80
+	u16	datalen;
+        u8 	data[0]; // max 80
 } __WLAN_ATTRIB_PACK__ hfa384x_WPAData_t;
 
 /*--------------------------------------------------------------------
@@ -1210,7 +1174,7 @@ Configuration Record Structures: Behavior Parameters
 /*-- Configuration Record: TickTime --*/
 typedef struct hfa384x_TickTime
 {
-	UINT16	TickTime;
+	u16	TickTime;
 } __WLAN_ATTRIB_PACK__ hfa384x_TickTime_t;
 
 /*--------------------------------------------------------------------
@@ -1220,146 +1184,146 @@ Information Record Structures: NIC Information
 /*-- Information Record: MaxLoadTime --*/
 typedef struct hfa384x_MaxLoadTime
 {
-	UINT16	MaxLoadTime;
+	u16	MaxLoadTime;
 } __WLAN_ATTRIB_PACK__ hfa384x_MaxLoadTime_t;
 
 /*-- Information Record: DownLoadBuffer --*/
 /* NOTE: The page and offset are in AUX format */
 typedef struct hfa384x_downloadbuffer
 {
-	UINT16	page;
-	UINT16	offset;
-	UINT16	len;
+	u16	page;
+	u16	offset;
+	u16	len;
 } __WLAN_ATTRIB_PACK__ hfa384x_downloadbuffer_t;
 
 /*-- Information Record: PRIIdentity --*/
 typedef struct hfa384x_PRIIdentity
 {
-	UINT16	PRICompID;
-	UINT16	PRIVariant;
-	UINT16	PRIMajorVersion;
-	UINT16	PRIMinorVersion;
+	u16	PRICompID;
+	u16	PRIVariant;
+	u16	PRIMajorVersion;
+	u16	PRIMinorVersion;
 } __WLAN_ATTRIB_PACK__ hfa384x_PRIIdentity_t;
 
 /*-- Information Record: PRISupRange --*/
 typedef struct hfa384x_PRISupRange
 {
-	UINT16	PRIRole;
-	UINT16	PRIID;
-	UINT16	PRIVariant;
-	UINT16	PRIBottom;
-	UINT16	PRITop;
+	u16	PRIRole;
+	u16	PRIID;
+	u16	PRIVariant;
+	u16	PRIBottom;
+	u16	PRITop;
 } __WLAN_ATTRIB_PACK__ hfa384x_PRISupRange_t;
 
 /*-- Information Record: CFIActRanges --*/
 typedef struct hfa384x_CFIActRanges
 {
-	UINT16	CFIRole;
-	UINT16	CFIID;
-	UINT16	CFIVariant;
-	UINT16	CFIBottom;
-	UINT16	CFITop;
+	u16	CFIRole;
+	u16	CFIID;
+	u16	CFIVariant;
+	u16	CFIBottom;
+	u16	CFITop;
 } __WLAN_ATTRIB_PACK__ hfa384x_CFIActRanges_t;
 
 /*-- Information Record: NICSerialNumber --*/
 typedef struct hfa384x_NICSerialNumber
 {
-	UINT8	NICSerialNumber[12];
+	u8	NICSerialNumber[12];
 } __WLAN_ATTRIB_PACK__ hfa384x_NICSerialNumber_t;
 
 /*-- Information Record: NICIdentity --*/
 typedef struct hfa384x_NICIdentity
 {
-	UINT16	NICCompID;
-	UINT16	NICVariant;
-	UINT16	NICMajorVersion;
-	UINT16	NICMinorVersion;
+	u16	NICCompID;
+	u16	NICVariant;
+	u16	NICMajorVersion;
+	u16	NICMinorVersion;
 } __WLAN_ATTRIB_PACK__ hfa384x_NICIdentity_t;
 
 /*-- Information Record: MFISupRange --*/
 typedef struct hfa384x_MFISupRange
 {
-	UINT16	MFIRole;
-	UINT16	MFIID;
-	UINT16	MFIVariant;
-	UINT16	MFIBottom;
-	UINT16	MFITop;
+	u16	MFIRole;
+	u16	MFIID;
+	u16	MFIVariant;
+	u16	MFIBottom;
+	u16	MFITop;
 } __WLAN_ATTRIB_PACK__ hfa384x_MFISupRange_t;
 
 /*-- Information Record: CFISupRange --*/
 typedef struct hfa384x_CFISupRange
 {
-	UINT16	CFIRole;
-	UINT16	CFIID;
-	UINT16	CFIVariant;
-	UINT16	CFIBottom;
-	UINT16	CFITop;
+	u16	CFIRole;
+	u16	CFIID;
+	u16	CFIVariant;
+	u16	CFIBottom;
+	u16	CFITop;
 } __WLAN_ATTRIB_PACK__ hfa384x_CFISupRange_t;
 
 /*-- Information Record: BUILDSEQ:BuildSeq --*/
 typedef struct hfa384x_BuildSeq {
-	UINT16	primary;
-	UINT16	secondary;
+	u16	primary;
+	u16	secondary;
 } __WLAN_ATTRIB_PACK__ hfa384x_BuildSeq_t;
 
 /*-- Information Record: FWID --*/
 #define HFA384x_FWID_LEN	14
 typedef struct hfa384x_FWID {
-	UINT8	primary[HFA384x_FWID_LEN];
-	UINT8	secondary[HFA384x_FWID_LEN];
+	u8	primary[HFA384x_FWID_LEN];
+	u8	secondary[HFA384x_FWID_LEN];
 } __WLAN_ATTRIB_PACK__ hfa384x_FWID_t;
 
 /*-- Information Record: ChannelList --*/
 typedef struct hfa384x_ChannelList
 {
-	UINT16	ChannelList;
+	u16	ChannelList;
 } __WLAN_ATTRIB_PACK__ hfa384x_ChannelList_t;
 
 /*-- Information Record: RegulatoryDomains --*/
 typedef struct hfa384x_RegulatoryDomains
 {
-	UINT8	RegulatoryDomains[12];
+	u8	RegulatoryDomains[12];
 } __WLAN_ATTRIB_PACK__ hfa384x_RegulatoryDomains_t;
 
 /*-- Information Record: TempType --*/
 typedef struct hfa384x_TempType
 {
-	UINT16	TempType;
+	u16	TempType;
 } __WLAN_ATTRIB_PACK__ hfa384x_TempType_t;
 
 /*-- Information Record: CIS --*/
 typedef struct hfa384x_CIS
 {
-	UINT8	CIS[480];
+	u8	CIS[480];
 } __WLAN_ATTRIB_PACK__ hfa384x_CIS_t;
 
 /*-- Information Record: STAIdentity --*/
 typedef struct hfa384x_STAIdentity
 {
-	UINT16	STACompID;
-	UINT16	STAVariant;
-	UINT16	STAMajorVersion;
-	UINT16	STAMinorVersion;
+	u16	STACompID;
+	u16	STAVariant;
+	u16	STAMajorVersion;
+	u16	STAMinorVersion;
 } __WLAN_ATTRIB_PACK__ hfa384x_STAIdentity_t;
 
 /*-- Information Record: STASupRange --*/
 typedef struct hfa384x_STASupRange
 {
-	UINT16	STARole;
-	UINT16	STAID;
-	UINT16	STAVariant;
-	UINT16	STABottom;
-	UINT16	STATop;
+	u16	STARole;
+	u16	STAID;
+	u16	STAVariant;
+	u16	STABottom;
+	u16	STATop;
 } __WLAN_ATTRIB_PACK__ hfa384x_STASupRange_t;
 
 /*-- Information Record: MFIActRanges --*/
 typedef struct hfa384x_MFIActRanges
 {
-	UINT16	MFIRole;
-	UINT16	MFIID;
-	UINT16	MFIVariant;
-	UINT16	MFIBottom;
-	UINT16	MFITop;
+	u16	MFIRole;
+	u16	MFIID;
+	u16	MFIVariant;
+	u16	MFIBottom;
+	u16	MFITop;
 } __WLAN_ATTRIB_PACK__ hfa384x_MFIActRanges_t;
 
 /*--------------------------------------------------------------------
@@ -1369,145 +1333,145 @@ Information Record Structures: NIC Information
 /*-- Information Record: PortStatus --*/
 typedef struct hfa384x_PortStatus
 {
-	UINT16	PortStatus;
+	u16	PortStatus;
 } __WLAN_ATTRIB_PACK__ hfa384x_PortStatus_t;
 
-#define HFA384x_PSTATUS_DISABLED	((UINT16)1)
-#define HFA384x_PSTATUS_SEARCHING	((UINT16)2)
-#define HFA384x_PSTATUS_CONN_IBSS	((UINT16)3)
-#define HFA384x_PSTATUS_CONN_ESS	((UINT16)4)
-#define HFA384x_PSTATUS_OUTOFRANGE	((UINT16)5)
-#define HFA384x_PSTATUS_CONN_WDS	((UINT16)6)
+#define HFA384x_PSTATUS_DISABLED	((u16)1)
+#define HFA384x_PSTATUS_SEARCHING	((u16)2)
+#define HFA384x_PSTATUS_CONN_IBSS	((u16)3)
+#define HFA384x_PSTATUS_CONN_ESS	((u16)4)
+#define HFA384x_PSTATUS_OUTOFRANGE	((u16)5)
+#define HFA384x_PSTATUS_CONN_WDS	((u16)6)
 
 /*-- Information Record: CurrentSSID --*/
 typedef struct hfa384x_CurrentSSID
 {
-	UINT8	CurrentSSID[34];
+	u8	CurrentSSID[34];
 } __WLAN_ATTRIB_PACK__ hfa384x_CurrentSSID_t;
 
 /*-- Information Record: CurrentBSSID --*/
 typedef struct hfa384x_CurrentBSSID
 {
-	UINT8	CurrentBSSID[6];
+	u8	CurrentBSSID[6];
 } __WLAN_ATTRIB_PACK__ hfa384x_CurrentBSSID_t;
 
 /*-- Information Record: commsquality --*/
 typedef struct hfa384x_commsquality
 {
-	UINT16	CQ_currBSS;
-	UINT16	ASL_currBSS;
-	UINT16	ANL_currFC;
+	u16	CQ_currBSS;
+	u16	ASL_currBSS;
+	u16	ANL_currFC;
 } __WLAN_ATTRIB_PACK__ hfa384x_commsquality_t;
 
 /*-- Information Record: dmbcommsquality --*/
 typedef struct hfa384x_dbmcommsquality
 {
-	UINT16	CQdbm_currBSS;
-	UINT16	ASLdbm_currBSS;
-	UINT16	ANLdbm_currFC;
+	u16	CQdbm_currBSS;
+	u16	ASLdbm_currBSS;
+	u16	ANLdbm_currFC;
 } __WLAN_ATTRIB_PACK__ hfa384x_dbmcommsquality_t;
 
 /*-- Information Record: CurrentTxRate --*/
 typedef struct hfa384x_CurrentTxRate
 {
-	UINT16	CurrentTxRate;
+	u16	CurrentTxRate;
 } __WLAN_ATTRIB_PACK__ hfa384x_CurrentTxRate_t;
 
 /*-- Information Record: CurrentBeaconInterval --*/
 typedef struct hfa384x_CurrentBeaconInterval
 {
-	UINT16	CurrentBeaconInterval;
+	u16	CurrentBeaconInterval;
 } __WLAN_ATTRIB_PACK__ hfa384x_CurrentBeaconInterval_t;
 
 /*-- Information Record: CurrentScaleThresholds --*/
 typedef struct hfa384x_CurrentScaleThresholds
 {
-	UINT16	EnergyDetectThreshold;
-	UINT16	CarrierDetectThreshold;
-	UINT16	DeferDetectThreshold;
-	UINT16	CellSearchThreshold; /* Stations only */
-	UINT16	DeadSpotThreshold; /* Stations only */
+	u16	EnergyDetectThreshold;
+	u16	CarrierDetectThreshold;
+	u16	DeferDetectThreshold;
+	u16	CellSearchThreshold; /* Stations only */
+	u16	DeadSpotThreshold; /* Stations only */
 } __WLAN_ATTRIB_PACK__ hfa384x_CurrentScaleThresholds_t;
 
 /*-- Information Record: ProtocolRspTime --*/
 typedef struct hfa384x_ProtocolRspTime
 {
-	UINT16	ProtocolRspTime;
+	u16	ProtocolRspTime;
 } __WLAN_ATTRIB_PACK__ hfa384x_ProtocolRspTime_t;
 
 /*-- Information Record: ShortRetryLimit --*/
 typedef struct hfa384x_ShortRetryLimit
 {
-	UINT16	ShortRetryLimit;
+	u16	ShortRetryLimit;
 } __WLAN_ATTRIB_PACK__ hfa384x_ShortRetryLimit_t;
 
 /*-- Information Record: LongRetryLimit --*/
 typedef struct hfa384x_LongRetryLimit
 {
-	UINT16	LongRetryLimit;
+	u16	LongRetryLimit;
 } __WLAN_ATTRIB_PACK__ hfa384x_LongRetryLimit_t;
 
 /*-- Information Record: MaxTransmitLifetime --*/
 typedef struct hfa384x_MaxTransmitLifetime
 {
-	UINT16	MaxTransmitLifetime;
+	u16	MaxTransmitLifetime;
 } __WLAN_ATTRIB_PACK__ hfa384x_MaxTransmitLifetime_t;
 
 /*-- Information Record: MaxReceiveLifetime --*/
 typedef struct hfa384x_MaxReceiveLifetime
 {
-	UINT16	MaxReceiveLifetime;
+	u16	MaxReceiveLifetime;
 } __WLAN_ATTRIB_PACK__ hfa384x_MaxReceiveLifetime_t;
 
 /*-- Information Record: CFPollable --*/
 typedef struct hfa384x_CFPollable
 {
-	UINT16	CFPollable;
+	u16	CFPollable;
 } __WLAN_ATTRIB_PACK__ hfa384x_CFPollable_t;
 
 /*-- Information Record: AuthenticationAlgorithms --*/
 typedef struct hfa384x_AuthenticationAlgorithms
 {
-	UINT16	AuthenticationType;
-	UINT16	TypeEnabled;
+	u16	AuthenticationType;
+	u16	TypeEnabled;
 } __WLAN_ATTRIB_PACK__ hfa384x_AuthenticationAlgorithms_t;
 
 /*-- Information Record: AuthenticationAlgorithms
 (data only --*/
 typedef struct hfa384x_AuthenticationAlgorithms_data
 {
-	UINT16	AuthenticationType;
-	UINT16	TypeEnabled;
+	u16	AuthenticationType;
+	u16	TypeEnabled;
 } __WLAN_ATTRIB_PACK__ hfa384x_AuthenticationAlgorithms_data_t;
 
 /*-- Information Record: PrivacyOptionImplemented --*/
 typedef struct hfa384x_PrivacyOptionImplemented
 {
-	UINT16	PrivacyOptionImplemented;
+	u16	PrivacyOptionImplemented;
 } __WLAN_ATTRIB_PACK__ hfa384x_PrivacyOptionImplemented_t;
 
 /*-- Information Record: OwnMACAddress --*/
 typedef struct hfa384x_OwnMACAddress
 {
-	UINT8	OwnMACAddress[6];
+	u8	OwnMACAddress[6];
 } __WLAN_ATTRIB_PACK__ hfa384x_OwnMACAddress_t;
 
 /*-- Information Record: PCFInfo --*/
 typedef struct hfa384x_PCFInfo
 {
-	UINT16	MediumOccupancyLimit;
-	UINT16	CFPPeriod;
-	UINT16	CFPMaxDuration;
-	UINT16	CFPFlags;
+	u16	MediumOccupancyLimit;
+	u16	CFPPeriod;
+	u16	CFPMaxDuration;
+	u16	CFPFlags;
 } __WLAN_ATTRIB_PACK__ hfa384x_PCFInfo_t;
 
 /*-- Information Record: PCFInfo (data portion only) --*/
 typedef struct hfa384x_PCFInfo_data
 {
-	UINT16	MediumOccupancyLimit;
-	UINT16	CFPPeriod;
-	UINT16	CFPMaxDuration;
-	UINT16	CFPFlags;
+	u16	MediumOccupancyLimit;
+	u16	CFPPeriod;
+	u16	CFPMaxDuration;
+	u16	CFPFlags;
 } __WLAN_ATTRIB_PACK__ hfa384x_PCFInfo_data_t;
 
 /*--------------------------------------------------------------------
@@ -1517,39 +1481,39 @@ Information Record Structures: Modem Information Records
 /*-- Information Record: PHYType --*/
 typedef struct hfa384x_PHYType
 {
-	UINT16	PHYType;
+	u16	PHYType;
 } __WLAN_ATTRIB_PACK__ hfa384x_PHYType_t;
 
 /*-- Information Record: CurrentChannel --*/
 typedef struct hfa384x_CurrentChannel
 {
-	UINT16	CurrentChannel;
+	u16	CurrentChannel;
 } __WLAN_ATTRIB_PACK__ hfa384x_CurrentChannel_t;
 
 /*-- Information Record: CurrentPowerState --*/
 typedef struct hfa384x_CurrentPowerState
 {
-	UINT16	CurrentPowerState;
+	u16	CurrentPowerState;
 } __WLAN_ATTRIB_PACK__ hfa384x_CurrentPowerState_t;
 
 /*-- Information Record: CCAMode --*/
 typedef struct hfa384x_CCAMode
 {
-	UINT16	CCAMode;
+	u16	CCAMode;
 } __WLAN_ATTRIB_PACK__ hfa384x_CCAMode_t;
 
 /*-- Information Record: SupportedDataRates --*/
 typedef struct hfa384x_SupportedDataRates
 {
-	UINT8	SupportedDataRates[10];
+	u8	SupportedDataRates[10];
 } __WLAN_ATTRIB_PACK__ hfa384x_SupportedDataRates_t;
 
 /*-- Information Record: LFOStatus --*/
 typedef struct hfa384x_LFOStatus
 {
-	UINT16  TestResults;
-	UINT16  LFOResult;
-	UINT16  VRHFOResult;
+	u16  TestResults;
+	u16  LFOResult;
+	u16  VRHFOResult;
 } __WLAN_ATTRIB_PACK__ hfa384x_LFOStatus_t;
 
 #define HFA384x_TESTRESULT_ALLPASSED    BIT0
@@ -1561,11 +1525,11 @@ typedef struct hfa384x_LFOStatus
 /*-- Information Record: LEDControl --*/
 typedef struct hfa384x_LEDControl
 {
-	UINT16  searching_on;
-	UINT16  searching_off;
-	UINT16  assoc_on;
-	UINT16  assoc_off;
-	UINT16  activity;
+	u16  searching_on;
+	u16  searching_off;
+	u16  assoc_on;
+	u16  assoc_off;
+	u16  activity;
 } __WLAN_ATTRIB_PACK__ hfa384x_LEDControl_t;
 
 /*--------------------------------------------------------------------
@@ -1576,32 +1540,32 @@ FRAME DESCRIPTORS: Offsets
 ----------------------------------------------------------------------
 Control Info (offset 44-51)
 --------------------------------------------------------------------*/
-#define		HFA384x_FD_STATUS_OFF			((UINT16)0x44)
-#define		HFA384x_FD_TIME_OFF			((UINT16)0x46)
-#define		HFA384x_FD_SWSUPPORT_OFF		((UINT16)0x4A)
-#define		HFA384x_FD_SILENCE_OFF			((UINT16)0x4A)
-#define		HFA384x_FD_SIGNAL_OFF			((UINT16)0x4B)
-#define		HFA384x_FD_RATE_OFF			((UINT16)0x4C)
-#define		HFA384x_FD_RXFLOW_OFF			((UINT16)0x4D)
-#define		HFA384x_FD_RESERVED_OFF			((UINT16)0x4E)
-#define		HFA384x_FD_TXCONTROL_OFF		((UINT16)0x50)
+#define		HFA384x_FD_STATUS_OFF			((u16)0x44)
+#define		HFA384x_FD_TIME_OFF			((u16)0x46)
+#define		HFA384x_FD_SWSUPPORT_OFF		((u16)0x4A)
+#define		HFA384x_FD_SILENCE_OFF			((u16)0x4A)
+#define		HFA384x_FD_SIGNAL_OFF			((u16)0x4B)
+#define		HFA384x_FD_RATE_OFF			((u16)0x4C)
+#define		HFA384x_FD_RXFLOW_OFF			((u16)0x4D)
+#define		HFA384x_FD_RESERVED_OFF			((u16)0x4E)
+#define		HFA384x_FD_TXCONTROL_OFF		((u16)0x50)
 /*--------------------------------------------------------------------
 802.11 Header (offset 52-6B)
 --------------------------------------------------------------------*/
-#define		HFA384x_FD_FRAMECONTROL_OFF		((UINT16)0x52)
-#define		HFA384x_FD_DURATIONID_OFF		((UINT16)0x54)
-#define		HFA384x_FD_ADDRESS1_OFF			((UINT16)0x56)
-#define		HFA384x_FD_ADDRESS2_OFF			((UINT16)0x5C)
-#define		HFA384x_FD_ADDRESS3_OFF			((UINT16)0x62)
-#define		HFA384x_FD_SEQCONTROL_OFF		((UINT16)0x68)
-#define		HFA384x_FD_ADDRESS4_OFF			((UINT16)0x6A)
-#define		HFA384x_FD_DATALEN_OFF			((UINT16)0x70)
+#define		HFA384x_FD_FRAMECONTROL_OFF		((u16)0x52)
+#define		HFA384x_FD_DURATIONID_OFF		((u16)0x54)
+#define		HFA384x_FD_ADDRESS1_OFF			((u16)0x56)
+#define		HFA384x_FD_ADDRESS2_OFF			((u16)0x5C)
+#define		HFA384x_FD_ADDRESS3_OFF			((u16)0x62)
+#define		HFA384x_FD_SEQCONTROL_OFF		((u16)0x68)
+#define		HFA384x_FD_ADDRESS4_OFF			((u16)0x6A)
+#define		HFA384x_FD_DATALEN_OFF			((u16)0x70)
 /*--------------------------------------------------------------------
 802.3 Header (offset 72-7F)
 --------------------------------------------------------------------*/
-#define		HFA384x_FD_DESTADDRESS_OFF		((UINT16)0x72)
-#define		HFA384x_FD_SRCADDRESS_OFF		((UINT16)0x78)
-#define		HFA384x_FD_DATALENGTH_OFF		((UINT16)0x7E)
+#define		HFA384x_FD_DESTADDRESS_OFF		((u16)0x72)
+#define		HFA384x_FD_SRCADDRESS_OFF		((u16)0x78)
+#define		HFA384x_FD_DATALENGTH_OFF		((u16)0x7E)
 
 /*--------------------------------------------------------------------
 FRAME STRUCTURES: Communication Frames
@@ -1611,67 +1575,67 @@ Communication Frames: Transmit Frames
 /*-- Communication Frame: Transmit Frame Structure --*/
 typedef struct hfa384x_tx_frame
 {
-	UINT16	status;
-	UINT16	reserved1;
-	UINT16	reserved2;
-	UINT32	sw_support;
-	UINT8	tx_retrycount;
-	UINT8   tx_rate;
-	UINT16	tx_control;
+	u16	status;
+	u16	reserved1;
+	u16	reserved2;
+	u32	sw_support;
+	u8	tx_retrycount;
+	u8   tx_rate;
+	u16	tx_control;
 
 	/*-- 802.11 Header Information --*/
 
-	UINT16	frame_control;
-	UINT16	duration_id;
-	UINT8	address1[6];
-	UINT8	address2[6];
-	UINT8	address3[6];
-	UINT16	sequence_control;
-	UINT8	address4[6];
-	UINT16	data_len; /* little endian format */
+	u16	frame_control;
+	u16	duration_id;
+	u8	address1[6];
+	u8	address2[6];
+	u8	address3[6];
+	u16	sequence_control;
+	u8	address4[6];
+	u16	data_len; /* little endian format */
 
 	/*-- 802.3 Header Information --*/
 
-	UINT8	dest_addr[6];
-	UINT8	src_addr[6];
-	UINT16	data_length; /* big endian format */
+	u8	dest_addr[6];
+	u8	src_addr[6];
+	u16	data_length; /* big endian format */
 } __WLAN_ATTRIB_PACK__ hfa384x_tx_frame_t;
 /*--------------------------------------------------------------------
 Communication Frames: Field Masks for Transmit Frames
 --------------------------------------------------------------------*/
 /*-- Status Field --*/
-#define		HFA384x_TXSTATUS_ACKERR			((UINT16)BIT5)
-#define		HFA384x_TXSTATUS_FORMERR		((UINT16)BIT3)
-#define		HFA384x_TXSTATUS_DISCON			((UINT16)BIT2)
-#define		HFA384x_TXSTATUS_AGEDERR		((UINT16)BIT1)
-#define		HFA384x_TXSTATUS_RETRYERR		((UINT16)BIT0)
+#define		HFA384x_TXSTATUS_ACKERR			((u16)BIT5)
+#define		HFA384x_TXSTATUS_FORMERR		((u16)BIT3)
+#define		HFA384x_TXSTATUS_DISCON			((u16)BIT2)
+#define		HFA384x_TXSTATUS_AGEDERR		((u16)BIT1)
+#define		HFA384x_TXSTATUS_RETRYERR		((u16)BIT0)
 /*-- Transmit Control Field --*/
-#define		HFA384x_TX_CFPOLL			((UINT16)BIT12)
-#define		HFA384x_TX_PRST				((UINT16)BIT11)
-#define		HFA384x_TX_MACPORT			((UINT16)(BIT10 | BIT9 | BIT8))
-#define		HFA384x_TX_NOENCRYPT			((UINT16)BIT7)
-#define		HFA384x_TX_RETRYSTRAT			((UINT16)(BIT6 | BIT5))
-#define		HFA384x_TX_STRUCTYPE			((UINT16)(BIT4 | BIT3))
-#define		HFA384x_TX_TXEX				((UINT16)BIT2)
-#define		HFA384x_TX_TXOK				((UINT16)BIT1)
+#define		HFA384x_TX_CFPOLL			((u16)BIT12)
+#define		HFA384x_TX_PRST				((u16)BIT11)
+#define		HFA384x_TX_MACPORT			((u16)(BIT10 | BIT9 | BIT8))
+#define		HFA384x_TX_NOENCRYPT			((u16)BIT7)
+#define		HFA384x_TX_RETRYSTRAT			((u16)(BIT6 | BIT5))
+#define		HFA384x_TX_STRUCTYPE			((u16)(BIT4 | BIT3))
+#define		HFA384x_TX_TXEX				((u16)BIT2)
+#define		HFA384x_TX_TXOK				((u16)BIT1)
 /*--------------------------------------------------------------------
 Communication Frames: Test/Get/Set Field Values for Transmit Frames
 --------------------------------------------------------------------*/
 /*-- Status Field --*/
 #define HFA384x_TXSTATUS_ISERROR(v)	\
-	(((UINT16)(v))&\
+	(((u16)(v))&\
 	(HFA384x_TXSTATUS_ACKERR|HFA384x_TXSTATUS_FORMERR|\
 	HFA384x_TXSTATUS_DISCON|HFA384x_TXSTATUS_AGEDERR|\
 	HFA384x_TXSTATUS_RETRYERR))
 
-#define	HFA384x_TXSTATUS_ISACKERR(v)	((UINT16)(((UINT16)(v)) & HFA384x_TXSTATUS_ACKERR))
-#define	HFA384x_TXSTATUS_ISFORMERR(v)	((UINT16)(((UINT16)(v)) & HFA384x_TXSTATUS_FORMERR))
-#define	HFA384x_TXSTATUS_ISDISCON(v)	((UINT16)(((UINT16)(v)) & HFA384x_TXSTATUS_DISCON))
-#define	HFA384x_TXSTATUS_ISAGEDERR(v)	((UINT16)(((UINT16)(v)) & HFA384x_TXSTATUS_AGEDERR))
-#define	HFA384x_TXSTATUS_ISRETRYERR(v)	((UINT16)(((UINT16)(v)) & HFA384x_TXSTATUS_RETRYERR))
+#define	HFA384x_TXSTATUS_ISACKERR(v)	((u16)(((u16)(v)) & HFA384x_TXSTATUS_ACKERR))
+#define	HFA384x_TXSTATUS_ISFORMERR(v)	((u16)(((u16)(v)) & HFA384x_TXSTATUS_FORMERR))
+#define	HFA384x_TXSTATUS_ISDISCON(v)	((u16)(((u16)(v)) & HFA384x_TXSTATUS_DISCON))
+#define	HFA384x_TXSTATUS_ISAGEDERR(v)	((u16)(((u16)(v)) & HFA384x_TXSTATUS_AGEDERR))
+#define	HFA384x_TXSTATUS_ISRETRYERR(v)	((u16)(((u16)(v)) & HFA384x_TXSTATUS_RETRYERR))
 
-#define	HFA384x_TX_GET(v,m,s)		((((UINT16)(v))&((UINT16)(m)))>>((UINT16)(s)))
-#define	HFA384x_TX_SET(v,m,s)		((((UINT16)(v))<<((UINT16)(s)))&((UINT16)(m)))
+#define	HFA384x_TX_GET(v,m,s)		((((u16)(v))&((u16)(m)))>>((u16)(s)))
+#define	HFA384x_TX_SET(v,m,s)		((((u16)(v))<<((u16)(s)))&((u16)(m)))
 
 #define	HFA384x_TX_CFPOLL_GET(v)	HFA384x_TX_GET(v, HFA384x_TX_CFPOLL,12)
 #define	HFA384x_TX_CFPOLL_SET(v)	HFA384x_TX_SET(v, HFA384x_TX_CFPOLL,12)
@@ -1696,70 +1660,70 @@ Communication Frames: Receive Frames
 typedef struct hfa384x_rx_frame
 {
 	/*-- MAC rx descriptor (hfa384x byte order) --*/
-	UINT16	status;
-	UINT32	time;
-	UINT8	silence;
-	UINT8	signal;
-	UINT8	rate;
-	UINT8	rx_flow;
-	UINT16	reserved1;
-	UINT16	reserved2;
+	u16	status;
+	u32	time;
+	u8	silence;
+	u8	signal;
+	u8	rate;
+	u8	rx_flow;
+	u16	reserved1;
+	u16	reserved2;
 
 	/*-- 802.11 Header Information (802.11 byte order) --*/
-	UINT16	frame_control;
-	UINT16	duration_id;
-	UINT8	address1[6];
-	UINT8	address2[6];
-	UINT8	address3[6];
-	UINT16	sequence_control;
-	UINT8	address4[6];
-	UINT16	data_len; /* hfa384x (little endian) format */
+	u16	frame_control;
+	u16	duration_id;
+	u8	address1[6];
+	u8	address2[6];
+	u8	address3[6];
+	u16	sequence_control;
+	u8	address4[6];
+	u16	data_len; /* hfa384x (little endian) format */
 
 	/*-- 802.3 Header Information --*/
-	UINT8	dest_addr[6];
-	UINT8	src_addr[6];
-	UINT16	data_length; /* IEEE? (big endian) format */
+	u8	dest_addr[6];
+	u8	src_addr[6];
+	u16	data_length; /* IEEE? (big endian) format */
 } __WLAN_ATTRIB_PACK__ hfa384x_rx_frame_t;
 /*--------------------------------------------------------------------
 Communication Frames: Field Masks for Receive Frames
 --------------------------------------------------------------------*/
 /*-- Offsets --------*/
-#define		HFA384x_RX_DATA_LEN_OFF			((UINT16)44)
-#define		HFA384x_RX_80211HDR_OFF			((UINT16)14)
-#define		HFA384x_RX_DATA_OFF			((UINT16)60)
+#define		HFA384x_RX_DATA_LEN_OFF			((u16)44)
+#define		HFA384x_RX_80211HDR_OFF			((u16)14)
+#define		HFA384x_RX_DATA_OFF			((u16)60)
 
 /*-- Status Fields --*/
-#define		HFA384x_RXSTATUS_MSGTYPE		((UINT16)(BIT15 | BIT14 | BIT13))
-#define		HFA384x_RXSTATUS_MACPORT		((UINT16)(BIT10 | BIT9 | BIT8))
-#define		HFA384x_RXSTATUS_UNDECR			((UINT16)BIT1)
-#define		HFA384x_RXSTATUS_FCSERR			((UINT16)BIT0)
+#define		HFA384x_RXSTATUS_MSGTYPE		((u16)(BIT15 | BIT14 | BIT13))
+#define		HFA384x_RXSTATUS_MACPORT		((u16)(BIT10 | BIT9 | BIT8))
+#define		HFA384x_RXSTATUS_UNDECR			((u16)BIT1)
+#define		HFA384x_RXSTATUS_FCSERR			((u16)BIT0)
 /*--------------------------------------------------------------------
 Communication Frames: Test/Get/Set Field Values for Receive Frames
 --------------------------------------------------------------------*/
-#define		HFA384x_RXSTATUS_MSGTYPE_GET(value)	((UINT16)((((UINT16)(value)) & HFA384x_RXSTATUS_MSGTYPE) >> 13))
-#define		HFA384x_RXSTATUS_MSGTYPE_SET(value)	((UINT16)(((UINT16)(value)) << 13))
-#define		HFA384x_RXSTATUS_MACPORT_GET(value)	((UINT16)((((UINT16)(value)) & HFA384x_RXSTATUS_MACPORT) >> 8))
-#define		HFA384x_RXSTATUS_MACPORT_SET(value)	((UINT16)(((UINT16)(value)) << 8))
-#define		HFA384x_RXSTATUS_ISUNDECR(value)	((UINT16)(((UINT16)(value)) & HFA384x_RXSTATUS_UNDECR))
-#define		HFA384x_RXSTATUS_ISFCSERR(value)	((UINT16)(((UINT16)(value)) & HFA384x_RXSTATUS_FCSERR))
+#define		HFA384x_RXSTATUS_MSGTYPE_GET(value)	((u16)((((u16)(value)) & HFA384x_RXSTATUS_MSGTYPE) >> 13))
+#define		HFA384x_RXSTATUS_MSGTYPE_SET(value)	((u16)(((u16)(value)) << 13))
+#define		HFA384x_RXSTATUS_MACPORT_GET(value)	((u16)((((u16)(value)) & HFA384x_RXSTATUS_MACPORT) >> 8))
+#define		HFA384x_RXSTATUS_MACPORT_SET(value)	((u16)(((u16)(value)) << 8))
+#define		HFA384x_RXSTATUS_ISUNDECR(value)	((u16)(((u16)(value)) & HFA384x_RXSTATUS_UNDECR))
+#define		HFA384x_RXSTATUS_ISFCSERR(value)	((u16)(((u16)(value)) & HFA384x_RXSTATUS_FCSERR))
 /*--------------------------------------------------------------------
  FRAME STRUCTURES: Information Types and Information Frame Structures
 ----------------------------------------------------------------------
 Information Types
 --------------------------------------------------------------------*/
-#define		HFA384x_IT_HANDOVERADDR			((UINT16)0xF000UL)
-#define		HFA384x_IT_HANDOVERDEAUTHADDRESS	((UINT16)0xF001UL)//AP 1.3.7
-#define		HFA384x_IT_COMMTALLIES			((UINT16)0xF100UL)
-#define		HFA384x_IT_SCANRESULTS			((UINT16)0xF101UL)
-#define		HFA384x_IT_CHINFORESULTS		((UINT16)0xF102UL)
-#define		HFA384x_IT_HOSTSCANRESULTS		((UINT16)0xF103UL)
-#define		HFA384x_IT_LINKSTATUS			((UINT16)0xF200UL)
-#define		HFA384x_IT_ASSOCSTATUS			((UINT16)0xF201UL)
-#define		HFA384x_IT_AUTHREQ			((UINT16)0xF202UL)
-#define		HFA384x_IT_PSUSERCNT			((UINT16)0xF203UL)
-#define		HFA384x_IT_KEYIDCHANGED			((UINT16)0xF204UL)
-#define		HFA384x_IT_ASSOCREQ    			((UINT16)0xF205UL)
-#define		HFA384x_IT_MICFAILURE  			((UINT16)0xF206UL)
+#define		HFA384x_IT_HANDOVERADDR			((u16)0xF000UL)
+#define		HFA384x_IT_HANDOVERDEAUTHADDRESS	((u16)0xF001UL)//AP 1.3.7
+#define		HFA384x_IT_COMMTALLIES			((u16)0xF100UL)
+#define		HFA384x_IT_SCANRESULTS			((u16)0xF101UL)
+#define		HFA384x_IT_CHINFORESULTS		((u16)0xF102UL)
+#define		HFA384x_IT_HOSTSCANRESULTS		((u16)0xF103UL)
+#define		HFA384x_IT_LINKSTATUS			((u16)0xF200UL)
+#define		HFA384x_IT_ASSOCSTATUS			((u16)0xF201UL)
+#define		HFA384x_IT_AUTHREQ			((u16)0xF202UL)
+#define		HFA384x_IT_PSUSERCNT			((u16)0xF203UL)
+#define		HFA384x_IT_KEYIDCHANGED			((u16)0xF204UL)
+#define		HFA384x_IT_ASSOCREQ    			((u16)0xF205UL)
+#define		HFA384x_IT_MICFAILURE  			((u16)0xF206UL)
 
 /*--------------------------------------------------------------------
 Information Frames Structures
@@ -1769,80 +1733,80 @@ Information Frames: Notification Frame Structures
 /*--  Notification Frame,MAC Mgmt: Handover Address --*/
 typedef struct hfa384x_HandoverAddr
 {
-	UINT16	framelen;
-	UINT16	infotype;
-	UINT8	handover_addr[WLAN_BSSID_LEN];
+	u16	framelen;
+	u16	infotype;
+	u8	handover_addr[WLAN_BSSID_LEN];
 } __WLAN_ATTRIB_PACK__ hfa384x_HandoverAddr_t;
 
 /*--  Inquiry Frame, Diagnose: Communication Tallies --*/
 typedef struct hfa384x_CommTallies16
 {
-	UINT16	txunicastframes;
-	UINT16	txmulticastframes;
-	UINT16	txfragments;
-	UINT16	txunicastoctets;
-	UINT16	txmulticastoctets;
-	UINT16	txdeferredtrans;
-	UINT16	txsingleretryframes;
-	UINT16	txmultipleretryframes;
-	UINT16	txretrylimitexceeded;
-	UINT16	txdiscards;
-	UINT16	rxunicastframes;
-	UINT16	rxmulticastframes;
-	UINT16	rxfragments;
-	UINT16	rxunicastoctets;
-	UINT16	rxmulticastoctets;
-	UINT16	rxfcserrors;
-	UINT16	rxdiscardsnobuffer;
-	UINT16	txdiscardswrongsa;
-	UINT16	rxdiscardswepundecr;
-	UINT16	rxmsginmsgfrag;
-	UINT16	rxmsginbadmsgfrag;
+	u16	txunicastframes;
+	u16	txmulticastframes;
+	u16	txfragments;
+	u16	txunicastoctets;
+	u16	txmulticastoctets;
+	u16	txdeferredtrans;
+	u16	txsingleretryframes;
+	u16	txmultipleretryframes;
+	u16	txretrylimitexceeded;
+	u16	txdiscards;
+	u16	rxunicastframes;
+	u16	rxmulticastframes;
+	u16	rxfragments;
+	u16	rxunicastoctets;
+	u16	rxmulticastoctets;
+	u16	rxfcserrors;
+	u16	rxdiscardsnobuffer;
+	u16	txdiscardswrongsa;
+	u16	rxdiscardswepundecr;
+	u16	rxmsginmsgfrag;
+	u16	rxmsginbadmsgfrag;
 } __WLAN_ATTRIB_PACK__ hfa384x_CommTallies16_t;
 
 typedef struct hfa384x_CommTallies32
 {
-	UINT32	txunicastframes;
-	UINT32	txmulticastframes;
-	UINT32	txfragments;
-	UINT32	txunicastoctets;
-	UINT32	txmulticastoctets;
-	UINT32	txdeferredtrans;
-	UINT32	txsingleretryframes;
-	UINT32	txmultipleretryframes;
-	UINT32	txretrylimitexceeded;
-	UINT32	txdiscards;
-	UINT32	rxunicastframes;
-	UINT32	rxmulticastframes;
-	UINT32	rxfragments;
-	UINT32	rxunicastoctets;
-	UINT32	rxmulticastoctets;
-	UINT32	rxfcserrors;
-	UINT32	rxdiscardsnobuffer;
-	UINT32	txdiscardswrongsa;
-	UINT32	rxdiscardswepundecr;
-	UINT32	rxmsginmsgfrag;
-	UINT32	rxmsginbadmsgfrag;
+	u32	txunicastframes;
+	u32	txmulticastframes;
+	u32	txfragments;
+	u32	txunicastoctets;
+	u32	txmulticastoctets;
+	u32	txdeferredtrans;
+	u32	txsingleretryframes;
+	u32	txmultipleretryframes;
+	u32	txretrylimitexceeded;
+	u32	txdiscards;
+	u32	rxunicastframes;
+	u32	rxmulticastframes;
+	u32	rxfragments;
+	u32	rxunicastoctets;
+	u32	rxmulticastoctets;
+	u32	rxfcserrors;
+	u32	rxdiscardsnobuffer;
+	u32	txdiscardswrongsa;
+	u32	rxdiscardswepundecr;
+	u32	rxmsginmsgfrag;
+	u32	rxmsginbadmsgfrag;
 } __WLAN_ATTRIB_PACK__ hfa384x_CommTallies32_t;
 
 /*--  Inquiry Frame, Diagnose: Scan Results & Subfields--*/
 typedef struct hfa384x_ScanResultSub
 {
-	UINT16	chid;
-	UINT16	anl;
-	UINT16	sl;
-	UINT8	bssid[WLAN_BSSID_LEN];
-	UINT16	bcnint;
-	UINT16	capinfo;
+	u16	chid;
+	u16	anl;
+	u16	sl;
+	u8	bssid[WLAN_BSSID_LEN];
+	u16	bcnint;
+	u16	capinfo;
 	hfa384x_bytestr32_t	ssid;
-	UINT8	supprates[10]; /* 802.11 info element */
-	UINT16	proberesp_rate;
+	u8	supprates[10]; /* 802.11 info element */
+	u16	proberesp_rate;
 } __WLAN_ATTRIB_PACK__ hfa384x_ScanResultSub_t;
 
 typedef struct hfa384x_ScanResult
 {
-	UINT16	rsvd;
-	UINT16	scanreason;
+	u16	rsvd;
+	u16	scanreason;
 	hfa384x_ScanResultSub_t
 		result[HFA384x_SCANRESULT_MAX];
 } __WLAN_ATTRIB_PACK__ hfa384x_ScanResult_t;
@@ -1850,10 +1814,10 @@ typedef struct hfa384x_ScanResult
 /*--  Inquiry Frame, Diagnose: ChInfo Results & Subfields--*/
 typedef struct hfa384x_ChInfoResultSub
 {
-	UINT16	chid;
-	UINT16	anl;
-	UINT16	pnl;
-	UINT16	active;
+	u16	chid;
+	u16	anl;
+	u16	pnl;
+	u16	active;
 } __WLAN_ATTRIB_PACK__ hfa384x_ChInfoResultSub_t;
 
 #define HFA384x_CHINFORESULT_BSSACTIVE	BIT0
@@ -1861,7 +1825,7 @@ typedef struct hfa384x_ChInfoResultSub
 
 typedef struct hfa384x_ChInfoResult
 {
-	UINT16	scanchannels;
+	u16	scanchannels;
 	hfa384x_ChInfoResultSub_t
 		result[HFA384x_CHINFORESULT_MAX];
 } __WLAN_ATTRIB_PACK__ hfa384x_ChInfoResult_t;
@@ -1869,75 +1833,75 @@ typedef struct hfa384x_ChInfoResult
 /*--  Inquiry Frame, Diagnose: Host Scan Results & Subfields--*/
 typedef struct hfa384x_HScanResultSub
 {
-	UINT16	chid;
-	UINT16	anl;
-	UINT16	sl;
-	UINT8	bssid[WLAN_BSSID_LEN];
-	UINT16	bcnint;
-	UINT16	capinfo;
+	u16	chid;
+	u16	anl;
+	u16	sl;
+	u8	bssid[WLAN_BSSID_LEN];
+	u16	bcnint;
+	u16	capinfo;
 	hfa384x_bytestr32_t	ssid;
-	UINT8	supprates[10]; /* 802.11 info element */
-	UINT16	proberesp_rate;
-	UINT16	atim;
+	u8	supprates[10]; /* 802.11 info element */
+	u16	proberesp_rate;
+	u16	atim;
 } __WLAN_ATTRIB_PACK__ hfa384x_HScanResultSub_t;
 
 typedef struct hfa384x_HScanResult
 {
-	UINT16	nresult;
-	UINT16	rsvd;
+	u16	nresult;
+	u16	rsvd;
 	hfa384x_HScanResultSub_t
 		result[HFA384x_HSCANRESULT_MAX];
 } __WLAN_ATTRIB_PACK__ hfa384x_HScanResult_t;
 
 /*--  Unsolicited Frame, MAC Mgmt: LinkStatus --*/
 
-#define HFA384x_LINK_NOTCONNECTED	((UINT16)0)
-#define HFA384x_LINK_CONNECTED		((UINT16)1)
-#define HFA384x_LINK_DISCONNECTED	((UINT16)2)
-#define HFA384x_LINK_AP_CHANGE		((UINT16)3)
-#define HFA384x_LINK_AP_OUTOFRANGE	((UINT16)4)
-#define HFA384x_LINK_AP_INRANGE		((UINT16)5)
-#define HFA384x_LINK_ASSOCFAIL		((UINT16)6)
+#define HFA384x_LINK_NOTCONNECTED	((u16)0)
+#define HFA384x_LINK_CONNECTED		((u16)1)
+#define HFA384x_LINK_DISCONNECTED	((u16)2)
+#define HFA384x_LINK_AP_CHANGE		((u16)3)
+#define HFA384x_LINK_AP_OUTOFRANGE	((u16)4)
+#define HFA384x_LINK_AP_INRANGE		((u16)5)
+#define HFA384x_LINK_ASSOCFAIL		((u16)6)
 
 typedef struct hfa384x_LinkStatus
 {
-	UINT16	linkstatus;
+	u16	linkstatus;
 } __WLAN_ATTRIB_PACK__ hfa384x_LinkStatus_t;
 
 
 /*--  Unsolicited Frame, MAC Mgmt: AssociationStatus (--*/
 
-#define HFA384x_ASSOCSTATUS_STAASSOC	((UINT16)1)
-#define HFA384x_ASSOCSTATUS_REASSOC	((UINT16)2)
-#define HFA384x_ASSOCSTATUS_DISASSOC	((UINT16)3)
-#define HFA384x_ASSOCSTATUS_ASSOCFAIL	((UINT16)4)
-#define HFA384x_ASSOCSTATUS_AUTHFAIL	((UINT16)5)
+#define HFA384x_ASSOCSTATUS_STAASSOC	((u16)1)
+#define HFA384x_ASSOCSTATUS_REASSOC	((u16)2)
+#define HFA384x_ASSOCSTATUS_DISASSOC	((u16)3)
+#define HFA384x_ASSOCSTATUS_ASSOCFAIL	((u16)4)
+#define HFA384x_ASSOCSTATUS_AUTHFAIL	((u16)5)
 
 typedef struct hfa384x_AssocStatus
 {
-	UINT16	assocstatus;
-	UINT8	sta_addr[WLAN_ADDR_LEN];
+	u16	assocstatus;
+	u8	sta_addr[WLAN_ADDR_LEN];
 	/* old_ap_addr is only valid if assocstatus == 2 */
-	UINT8	old_ap_addr[WLAN_ADDR_LEN];
-	UINT16	reason;
-	UINT16	reserved;
+	u8	old_ap_addr[WLAN_ADDR_LEN];
+	u16	reason;
+	u16	reserved;
 } __WLAN_ATTRIB_PACK__ hfa384x_AssocStatus_t;
 
 /*--  Unsolicited Frame, MAC Mgmt: AuthRequest (AP Only) --*/
 
 typedef struct hfa384x_AuthRequest
 {
-	UINT8	sta_addr[WLAN_ADDR_LEN];
-	UINT16	algorithm;
+	u8	sta_addr[WLAN_ADDR_LEN];
+	u16	algorithm;
 } __WLAN_ATTRIB_PACK__ hfa384x_AuthReq_t;
 
 /*--  Unsolicited Frame, MAC Mgmt: AssocRequest (AP Only) --*/
 
 typedef struct hfa384x_AssocRequest
 {
-	UINT8	sta_addr[WLAN_ADDR_LEN];
-	UINT16	type;
-	UINT8   wpa_data[80];
+	u8	sta_addr[WLAN_ADDR_LEN];
+	u16	type;
+	u8   wpa_data[80];
 } __WLAN_ATTRIB_PACK__ hfa384x_AssocReq_t;
 
 
@@ -1948,21 +1912,21 @@ typedef struct hfa384x_AssocRequest
 
 typedef struct hfa384x_MicFailure
 {
-	UINT8	sender[WLAN_ADDR_LEN];
-	UINT8	dest[WLAN_ADDR_LEN];
+	u8	sender[WLAN_ADDR_LEN];
+	u8	dest[WLAN_ADDR_LEN];
 } __WLAN_ATTRIB_PACK__ hfa384x_MicFailure_t;
 
 /*--  Unsolicited Frame, MAC Mgmt: PSUserCount (AP Only) --*/
 
 typedef struct hfa384x_PSUserCount
 {
-	UINT16	usercnt;
+	u16	usercnt;
 } __WLAN_ATTRIB_PACK__ hfa384x_PSUserCount_t;
 
 typedef struct hfa384x_KeyIDChanged
 {
-	UINT8	sta_addr[WLAN_ADDR_LEN];
-	UINT16	keyid;
+	u8	sta_addr[WLAN_ADDR_LEN];
+	u16	keyid;
 } __WLAN_ATTRIB_PACK__ hfa384x_KeyIDChanged_t;
 
 /*--  Collection of all Inf frames ---------------*/
@@ -1981,12 +1945,11 @@ typedef union hfa384x_infodata {
 
 typedef struct hfa384x_InfFrame
 {
-	UINT16			framelen;
-	UINT16			infotype;
+	u16			framelen;
+	u16			infotype;
 	hfa384x_infodata_t	info;
 } __WLAN_ATTRIB_PACK__ hfa384x_InfFrame_t;
 
-#if (WLAN_HOSTIF == WLAN_USB)
 /*--------------------------------------------------------------------
 USB Packet structures and constants.
 --------------------------------------------------------------------*/
@@ -2020,46 +1983,46 @@ USB Packet structures and constants.
 
 typedef struct hfa384x_usb_txfrm {
 	hfa384x_tx_frame_t	desc;
-	UINT8			data[WLAN_DATA_MAXLEN];
+	u8			data[WLAN_DATA_MAXLEN];
 } __WLAN_ATTRIB_PACK__ hfa384x_usb_txfrm_t;
 
 typedef struct hfa384x_usb_cmdreq {
-	UINT16		type;
-	UINT16		cmd;
-	UINT16		parm0;
-	UINT16		parm1;
-	UINT16		parm2;
-	UINT8		pad[54];
+	u16		type;
+	u16		cmd;
+	u16		parm0;
+	u16		parm1;
+	u16		parm2;
+	u8		pad[54];
 } __WLAN_ATTRIB_PACK__ hfa384x_usb_cmdreq_t;
 
 typedef struct hfa384x_usb_wridreq {
-	UINT16		type;
-	UINT16		frmlen;
-	UINT16		rid;
-	UINT8		data[HFA384x_RIDDATA_MAXLEN];
+	u16		type;
+	u16		frmlen;
+	u16		rid;
+	u8		data[HFA384x_RIDDATA_MAXLEN];
 } __WLAN_ATTRIB_PACK__ hfa384x_usb_wridreq_t;
 
 typedef struct hfa384x_usb_rridreq {
-	UINT16		type;
-	UINT16		frmlen;
-	UINT16		rid;
-	UINT8		pad[58];
+	u16		type;
+	u16		frmlen;
+	u16		rid;
+	u8		pad[58];
 } __WLAN_ATTRIB_PACK__ hfa384x_usb_rridreq_t;
 
 typedef struct hfa384x_usb_wmemreq {
-	UINT16		type;
-	UINT16		frmlen;
-	UINT16		offset;
-	UINT16		page;
-	UINT8		data[HFA384x_USB_RWMEM_MAXLEN];
+	u16		type;
+	u16		frmlen;
+	u16		offset;
+	u16		page;
+	u8		data[HFA384x_USB_RWMEM_MAXLEN];
 } __WLAN_ATTRIB_PACK__ hfa384x_usb_wmemreq_t;
 
 typedef struct hfa384x_usb_rmemreq {
-	UINT16		type;
-	UINT16		frmlen;
-	UINT16		offset;
-	UINT16		page;
-	UINT8		pad[56];
+	u16		type;
+	u16		frmlen;
+	u16		offset;
+	u16		page;
+	u8		pad[56];
 } __WLAN_ATTRIB_PACK__ hfa384x_usb_rmemreq_t;
 
 /*------------------------------------*/
@@ -2067,54 +2030,54 @@ typedef struct hfa384x_usb_rmemreq {
 
 typedef struct hfa384x_usb_rxfrm {
 	hfa384x_rx_frame_t	desc;
-	UINT8			data[WLAN_DATA_MAXLEN];
+	u8			data[WLAN_DATA_MAXLEN];
 } __WLAN_ATTRIB_PACK__ hfa384x_usb_rxfrm_t;
 
 typedef struct hfa384x_usb_infofrm {
-	UINT16			type;
+	u16			type;
 	hfa384x_InfFrame_t	info;
 } __WLAN_ATTRIB_PACK__ hfa384x_usb_infofrm_t;
 
 typedef struct hfa384x_usb_statusresp {
-	UINT16		type;
-	UINT16		status;
-	UINT16		resp0;
-	UINT16		resp1;
-	UINT16		resp2;
+	u16		type;
+	u16		status;
+	u16		resp0;
+	u16		resp1;
+	u16		resp2;
 } __WLAN_ATTRIB_PACK__ hfa384x_usb_cmdresp_t;
 
 typedef hfa384x_usb_cmdresp_t hfa384x_usb_wridresp_t;
 
 typedef struct hfa384x_usb_rridresp {
-	UINT16		type;
-	UINT16		frmlen;
-	UINT16		rid;
-	UINT8		data[HFA384x_RIDDATA_MAXLEN];
+	u16		type;
+	u16		frmlen;
+	u16		rid;
+	u8		data[HFA384x_RIDDATA_MAXLEN];
 } __WLAN_ATTRIB_PACK__ hfa384x_usb_rridresp_t;
 
 typedef hfa384x_usb_cmdresp_t hfa384x_usb_wmemresp_t;
 
 typedef struct hfa384x_usb_rmemresp {
-	UINT16		type;
-	UINT16		frmlen;
-	UINT8		data[HFA384x_USB_RWMEM_MAXLEN];
+	u16		type;
+	u16		frmlen;
+	u8		data[HFA384x_USB_RWMEM_MAXLEN];
 } __WLAN_ATTRIB_PACK__ hfa384x_usb_rmemresp_t;
 
 typedef struct hfa384x_usb_bufavail {
-	UINT16		type;
-	UINT16		frmlen;
+	u16		type;
+	u16		frmlen;
 } __WLAN_ATTRIB_PACK__ hfa384x_usb_bufavail_t;
 
 typedef struct hfa384x_usb_error {
-	UINT16		type;
-	UINT16		errortype;
+	u16		type;
+	u16		errortype;
 } __WLAN_ATTRIB_PACK__ hfa384x_usb_error_t;
 
 /*----------------------------------------------------------*/
 /* Unions for packaging all the known packet types together */
 
 typedef union hfa384x_usbout {
-	UINT16			type;
+	u16			type;
 	hfa384x_usb_txfrm_t	txfrm;
 	hfa384x_usb_cmdreq_t	cmdreq;
 	hfa384x_usb_wridreq_t	wridreq;
@@ -2124,7 +2087,7 @@ typedef union hfa384x_usbout {
 } __WLAN_ATTRIB_PACK__ hfa384x_usbout_t;
 
 typedef union hfa384x_usbin {
-	UINT16			type;
+	u16			type;
 	hfa384x_usb_rxfrm_t	rxfrm;
 	hfa384x_usb_txfrm_t	txfrm;
 	hfa384x_usb_infofrm_t	infofrm;
@@ -2135,28 +2098,26 @@ typedef union hfa384x_usbin {
 	hfa384x_usb_rmemresp_t	rmemresp;
 	hfa384x_usb_bufavail_t	bufavail;
 	hfa384x_usb_error_t	usberror;
-	UINT8			boguspad[3000];
+	u8			boguspad[3000];
 } __WLAN_ATTRIB_PACK__ hfa384x_usbin_t;
 
-#endif /* WLAN_USB */
-
 /*--------------------------------------------------------------------
 PD record structures.
 --------------------------------------------------------------------*/
 
 typedef struct hfa384x_pdr_pcb_partnum
 {
-	UINT8	num[8];
+	u8	num[8];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_pcb_partnum_t;
 
 typedef struct hfa384x_pdr_pcb_tracenum
 {
-	UINT8	num[8];
+	u8	num[8];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_pcb_tracenum_t;
 
 typedef struct hfa384x_pdr_nic_serial
 {
-	UINT8	num[12];
+	u8	num[12];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_nic_serial_t;
 
 typedef struct hfa384x_pdr_mkk_measurements
@@ -2180,170 +2141,170 @@ typedef struct hfa384x_pdr_mkk_measurements
 
 typedef struct hfa384x_pdr_nic_ramsize
 {
-	UINT8	size[12]; /* units of KB */
+	u8	size[12]; /* units of KB */
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_nic_ramsize_t;
 
 typedef struct hfa384x_pdr_mfisuprange
 {
-	UINT16	id;
-	UINT16	variant;
-	UINT16	bottom;
-	UINT16	top;
+	u16	id;
+	u16	variant;
+	u16	bottom;
+	u16	top;
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_mfisuprange_t;
 
 typedef struct hfa384x_pdr_cfisuprange
 {
-	UINT16	id;
-	UINT16	variant;
-	UINT16	bottom;
-	UINT16	top;
+	u16	id;
+	u16	variant;
+	u16	bottom;
+	u16	top;
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_cfisuprange_t;
 
 typedef struct hfa384x_pdr_nicid
 {
-	UINT16	id;
-	UINT16	variant;
-	UINT16	major;
-	UINT16	minor;
+	u16	id;
+	u16	variant;
+	u16	major;
+	u16	minor;
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_nicid_t;
 
 
 typedef struct hfa384x_pdr_refdac_measurements
 {
-	UINT16	value[0];
+	u16	value[0];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_refdac_measurements_t;
 
 typedef struct hfa384x_pdr_vgdac_measurements
 {
-	UINT16	value[0];
+	u16	value[0];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_vgdac_measurements_t;
 
 typedef struct hfa384x_pdr_level_comp_measurements
 {
-	UINT16	value[0];
+	u16	value[0];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_level_compc_measurements_t;
 
 typedef struct hfa384x_pdr_mac_address
 {
-	UINT8	addr[6];
+	u8	addr[6];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_mac_address_t;
 
 typedef struct hfa384x_pdr_mkk_callname
 {
-	UINT8	callname[8];
+	u8	callname[8];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_mkk_callname_t;
 
 typedef struct hfa384x_pdr_regdomain
 {
-	UINT16	numdomains;
-	UINT16	domain[5];
+	u16	numdomains;
+	u16	domain[5];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_regdomain_t;
 
 typedef struct hfa384x_pdr_allowed_channel
 {
-	UINT16	ch_bitmap;
+	u16	ch_bitmap;
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_allowed_channel_t;
 
 typedef struct hfa384x_pdr_default_channel
 {
-	UINT16	channel;
+	u16	channel;
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_default_channel_t;
 
 typedef struct hfa384x_pdr_privacy_option
 {
-	UINT16	available;
+	u16	available;
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_privacy_option_t;
 
 typedef struct hfa384x_pdr_temptype
 {
-	UINT16	type;
+	u16	type;
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_temptype_t;
 
 typedef struct hfa384x_pdr_refdac_setup
 {
-	UINT16	ch_value[14];
+	u16	ch_value[14];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_refdac_setup_t;
 
 typedef struct hfa384x_pdr_vgdac_setup
 {
-	UINT16	ch_value[14];
+	u16	ch_value[14];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_vgdac_setup_t;
 
 typedef struct hfa384x_pdr_level_comp_setup
 {
-	UINT16	ch_value[14];
+	u16	ch_value[14];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_level_comp_setup_t;
 
 typedef struct hfa384x_pdr_trimdac_setup
 {
-	UINT16	trimidac;
-	UINT16	trimqdac;
+	u16	trimidac;
+	u16	trimqdac;
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_trimdac_setup_t;
 
 typedef struct hfa384x_pdr_ifr_setting
 {
-	UINT16	value[3];
+	u16	value[3];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_ifr_setting_t;
 
 typedef struct hfa384x_pdr_rfr_setting
 {
-	UINT16	value[3];
+	u16	value[3];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_rfr_setting_t;
 
 typedef struct hfa384x_pdr_hfa3861_baseline
 {
-	UINT16	value[50];
+	u16	value[50];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_hfa3861_baseline_t;
 
 typedef struct hfa384x_pdr_hfa3861_shadow
 {
-	UINT32	value[32];
+	u32	value[32];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_hfa3861_shadow_t;
 
 typedef struct hfa384x_pdr_hfa3861_ifrf
 {
-	UINT32	value[20];
+	u32	value[20];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_hfa3861_ifrf_t;
 
 typedef struct hfa384x_pdr_hfa3861_chcalsp
 {
-	UINT16	value[14];
+	u16	value[14];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_hfa3861_chcalsp_t;
 
 typedef struct hfa384x_pdr_hfa3861_chcali
 {
-	UINT16	value[17];
+	u16	value[17];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_hfa3861_chcali_t;
 
 typedef struct hfa384x_pdr_hfa3861_nic_config
 {
-	UINT16	config_bitmap;
+	u16	config_bitmap;
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_nic_config_t;
 
 typedef struct hfa384x_pdr_hfo_delay
 {
-	UINT8   hfo_delay;
+	u8   hfo_delay;
 } __WLAN_ATTRIB_PACK__ hfa384x_hfo_delay_t;
 
 typedef struct hfa384x_pdr_hfa3861_manf_testsp
 {
-	UINT16	value[30];
+	u16	value[30];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_hfa3861_manf_testsp_t;
 
 typedef struct hfa384x_pdr_hfa3861_manf_testi
 {
-	UINT16	value[30];
+	u16	value[30];
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_hfa3861_manf_testi_t;
 
 typedef struct hfa384x_end_of_pda
 {
-	UINT16	crc;
+	u16	crc;
 } __WLAN_ATTRIB_PACK__ hfa384x_pdr_end_of_pda_t;
 
 typedef struct hfa384x_pdrec
 {
-	UINT16	len; /* in words */
-	UINT16	code;
+	u16	len; /* in words */
+	u16	code;
 	union pdr {
 	hfa384x_pdr_pcb_partnum_t	pcb_partnum;
 	hfa384x_pdr_pcb_tracenum_t	pcb_tracenum;
@@ -2391,14 +2352,12 @@ typedef struct hfa384x_pdrec
 --------------------------------------------------------------------*/
 typedef struct hfa384x_statusresult
 {
-	UINT16	status;
-	UINT16	resp0;
-	UINT16	resp1;
-	UINT16	resp2;
+	u16	status;
+	u16	resp0;
+	u16	resp1;
+	u16	resp2;
 } hfa384x_cmdresult_t;
 
-#if (WLAN_HOSTIF == WLAN_USB)
-
 /* USB Control Exchange (CTLX):
  *  A queue of the structure below is maintained for all of the
  *  Request/Response type USB packets supported by Prism2.
@@ -2411,9 +2370,9 @@ typedef hfa384x_cmdresult_t hfa384x_wmemresult_t;
 
 typedef struct hfa384x_rridresult
 {
-	UINT16		rid;
+	u16		rid;
 	const void	*riddata;
-	UINT		riddata_len;
+	unsigned int		riddata_len;
 } hfa384x_rridresult_t;
 
 enum ctlx_state {
@@ -2467,21 +2426,14 @@ typedef struct hfa384x_usbctlxq
 	struct list_head	completing;
 	struct list_head	reapable;
 } hfa384x_usbctlxq_t;
-#endif
 
 typedef struct hfa484x_metacmd
 {
-	UINT16		cmd;
-
-	UINT16          parm0;
-	UINT16          parm1;
-	UINT16          parm2;
+	u16		cmd;
 
-#if 0 //XXX cmd irq stuff
-	UINT16          bulkid;         /* what RID/FID to copy down. */
-	int             bulklen;        /* how much to copy from BAP */
-        char            *bulkdata;      /* And to where? */
-#endif
+	u16          parm0;
+	u16          parm1;
+	u16          parm2;
 
 	hfa384x_cmdresult_t result;
 } hfa384x_metacmd_t;
@@ -2507,28 +2459,22 @@ typedef struct hfa484x_metacmd
 /* XXX These are going away ASAP */
 typedef struct prism2sta_authlist
 {
-	UINT	cnt;
-	UINT8	addr[WLAN_AUTH_MAX][WLAN_ADDR_LEN];
-	UINT8	assoc[WLAN_AUTH_MAX];
+	unsigned int	cnt;
+	u8	addr[WLAN_AUTH_MAX][WLAN_ADDR_LEN];
+	u8	assoc[WLAN_AUTH_MAX];
 } prism2sta_authlist_t;
 
 typedef struct prism2sta_accesslist
 {
-	UINT	modify;
-	UINT	cnt;
-	UINT8	addr[WLAN_ACCESS_MAX][WLAN_ADDR_LEN];
-	UINT	cnt1;
-	UINT8	addr1[WLAN_ACCESS_MAX][WLAN_ADDR_LEN];
+	unsigned int	modify;
+	unsigned int	cnt;
+	u8	addr[WLAN_ACCESS_MAX][WLAN_ADDR_LEN];
+	unsigned int	cnt1;
+	u8	addr1[WLAN_ACCESS_MAX][WLAN_ADDR_LEN];
 } prism2sta_accesslist_t;
 
 typedef struct hfa384x
 {
-#if (WLAN_HOSTIF != WLAN_USB)
-	/* Resource config */
-	UINT32			iobase;
-	char			__iomem *membase;
-	UINT32			irq;
-#else
 	/* USB support data */
 	struct usb_device	*usb;
 	struct urb		rx_urb;
@@ -2560,16 +2506,6 @@ typedef struct hfa384x
 
 	int                     endp_in;
 	int                     endp_out;
-#endif /* !USB */
-
-#if (WLAN_HOSTIF == WLAN_PCMCIA)
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-	struct pcmcia_device *pdev;
-#else
-	dev_link_t	*link;
-#endif
-	dev_node_t	node;
-#endif
 
 	int                     sniff_fcs;
 	int                     sniff_channel;
@@ -2579,36 +2515,14 @@ typedef struct hfa384x
 	wait_queue_head_t cmdq;	        /* wait queue itself */
 
 	/* Controller state */
-	UINT32		state;
-	UINT32		isap;
-	UINT8		port_enabled[HFA384x_NUMPORTS_MAX];
-#if (WLAN_HOSTIF != WLAN_USB)
-	UINT		auxen;
-	UINT            isram16;
-#endif /* !USB */
+	u32		state;
+	u32		isap;
+	u8		port_enabled[HFA384x_NUMPORTS_MAX];
 
 	/* Download support */
-	UINT				dlstate;
+	unsigned int				dlstate;
 	hfa384x_downloadbuffer_t	bufinfo;
-	UINT16				dltimeout;
-
-#if (WLAN_HOSTIF != WLAN_USB)
-	spinlock_t	cmdlock;
-	volatile int    cmdflag;        /* wait queue flag */
-	hfa384x_metacmd_t *cmddata;      /* for our async callback */
-
-	/* BAP support */
-	spinlock_t	baplock;
-	struct tasklet_struct   bap_tasklet;
-
-	/* MAC buffer ids */
-        UINT16          txfid_head;
-        UINT16          txfid_tail;
-        UINT            txfid_N;
-        UINT16          txfid_queue[HFA384x_DRVR_FIDSTACKLEN_MAX];
-	UINT16			infofid;
-	struct semaphore	infofid_sem;
-#endif /* !USB */
+	u16				dltimeout;
 
 	int                          scanflag;    /* to signal scan comlete */
 	int                          join_ap;        /* are we joined to a specific ap */
@@ -2623,31 +2537,30 @@ typedef struct hfa384x
 	hfa384x_commsquality_t  qual;
 	struct timer_list	commsqual_timer;
 
-	UINT16 link_status;
-	UINT16 link_status_new;
+	u16 link_status;
+	u16 link_status_new;
 	struct sk_buff_head        authq;
 
 	/* And here we have stuff that used to be in priv */
 
 	/* State variables */
-	UINT		presniff_port_type;
-	UINT16		presniff_wepflags;
-	UINT32		dot11_desired_bss_type;
-	int		ap;	/* AP flag: 0 - Station, 1 - Access Point. */
+	unsigned int		presniff_port_type;
+	u16		presniff_wepflags;
+	u32		dot11_desired_bss_type;
 
 	int             dbmadjust;
 
 	/* Group Addresses - right now, there are up to a total
 	of MAX_GRP_ADDR group addresses */
-	UINT8		dot11_grp_addr[MAX_GRP_ADDR][WLAN_ADDR_LEN];
-	UINT		dot11_grpcnt;
+	u8		dot11_grp_addr[MAX_GRP_ADDR][WLAN_ADDR_LEN];
+	unsigned int		dot11_grpcnt;
 
 	/* Component Identities */
 	hfa384x_compident_t	ident_nic;
 	hfa384x_compident_t	ident_pri_fw;
 	hfa384x_compident_t	ident_sta_fw;
 	hfa384x_compident_t	ident_ap_fw;
-	UINT16			mm_mods;
+	u16			mm_mods;
 
 	/* Supplier compatibility ranges */
 	hfa384x_caplevel_t	cap_sup_mfi;
@@ -2663,14 +2576,14 @@ typedef struct hfa384x
 	hfa384x_caplevel_t	cap_act_ap_cfi;  /* ap f/w to controller interface */
 	hfa384x_caplevel_t	cap_act_ap_mfi;  /* ap f/w to modem interface */
 
-	UINT32			psusercount;  /* Power save user count. */
+	u32			psusercount;  /* Power save user count. */
 	hfa384x_CommTallies32_t	tallies;      /* Communication tallies. */
-	UINT8			comment[WLAN_COMMENT_MAX+1]; /* User comment */
+	u8			comment[WLAN_COMMENT_MAX+1]; /* User comment */
 
 	/* Channel Info request results (AP only) */
 	struct {
 		atomic_t		done;
-		UINT8			count;
+		u8			count;
 		hfa384x_ChInfoResult_t	results;
 	} channel_info;
 
@@ -2678,7 +2591,7 @@ typedef struct hfa384x
 
 
         prism2sta_authlist_t	authlist;     /* Authenticated station list. */
-	UINT			accessmode;   /* Access mode. */
+	unsigned int			accessmode;   /* Access mode. */
         prism2sta_accesslist_t	allow;        /* Allowed station list. */
         prism2sta_accesslist_t	deny;         /* Denied station list. */
 
@@ -2687,24 +2600,13 @@ typedef struct hfa384x
 /*=============================================================*/
 /*--- Function Declarations -----------------------------------*/
 /*=============================================================*/
-#if (WLAN_HOSTIF == WLAN_USB)
 void
 hfa384x_create(
 	hfa384x_t *hw,
 	struct usb_device *usb);
-#else
-void
-hfa384x_create(
-	hfa384x_t *hw,
-	UINT irq,
-	UINT32 iobase,
-	UINT8 __iomem *membase);
-#endif
 
 void hfa384x_destroy(hfa384x_t *hw);
 
-irqreturn_t
-hfa384x_interrupt(int irq, void *dev_id PT_REGS);
 int
 hfa384x_corereset( hfa384x_t *hw, int holdtime, int settletime, int genesis);
 int
@@ -2712,116 +2614,105 @@ hfa384x_drvr_chinforesults( hfa384x_t *hw);
 int
 hfa384x_drvr_commtallies( hfa384x_t *hw);
 int
-hfa384x_drvr_disable(hfa384x_t *hw, UINT16 macport);
+hfa384x_drvr_disable(hfa384x_t *hw, u16 macport);
 int
-hfa384x_drvr_enable(hfa384x_t *hw, UINT16 macport);
+hfa384x_drvr_enable(hfa384x_t *hw, u16 macport);
 int
 hfa384x_drvr_flashdl_enable(hfa384x_t *hw);
 int
 hfa384x_drvr_flashdl_disable(hfa384x_t *hw);
 int
-hfa384x_drvr_flashdl_write(hfa384x_t *hw, UINT32 daddr, void* buf, UINT32 len);
+hfa384x_drvr_flashdl_write(hfa384x_t *hw, u32 daddr, void* buf, u32 len);
 int
-hfa384x_drvr_getconfig(hfa384x_t *hw, UINT16 rid, void *buf, UINT16 len);
+hfa384x_drvr_getconfig(hfa384x_t *hw, u16 rid, void *buf, u16 len);
 int
-hfa384x_drvr_handover( hfa384x_t *hw, UINT8 *addr);
+hfa384x_drvr_handover( hfa384x_t *hw, u8 *addr);
 int
 hfa384x_drvr_hostscanresults( hfa384x_t *hw);
 int
 hfa384x_drvr_low_level(hfa384x_t *hw, hfa384x_metacmd_t *cmd);
 int
-hfa384x_drvr_mmi_read(hfa384x_t *hw, UINT32 address, UINT32 *result);
+hfa384x_drvr_mmi_read(hfa384x_t *hw, u32 address, u32 *result);
 int
-hfa384x_drvr_mmi_write(hfa384x_t *hw, UINT32 address, UINT32 data);
+hfa384x_drvr_mmi_write(hfa384x_t *hw, u32 address, u32 data);
 int
-hfa384x_drvr_ramdl_enable(hfa384x_t *hw, UINT32 exeaddr);
+hfa384x_drvr_ramdl_enable(hfa384x_t *hw, u32 exeaddr);
 int
 hfa384x_drvr_ramdl_disable(hfa384x_t *hw);
 int
-hfa384x_drvr_ramdl_write(hfa384x_t *hw, UINT32 daddr, void* buf, UINT32 len);
+hfa384x_drvr_ramdl_write(hfa384x_t *hw, u32 daddr, void* buf, u32 len);
 int
-hfa384x_drvr_readpda(hfa384x_t *hw, void *buf, UINT len);
+hfa384x_drvr_readpda(hfa384x_t *hw, void *buf, unsigned int len);
 int
 hfa384x_drvr_scanresults( hfa384x_t *hw);
 
 int
-hfa384x_drvr_setconfig(hfa384x_t *hw, UINT16 rid, void *buf, UINT16 len);
+hfa384x_drvr_setconfig(hfa384x_t *hw, u16 rid, void *buf, u16 len);
 
 static inline int
-hfa384x_drvr_getconfig16(hfa384x_t *hw, UINT16 rid, void *val)
+hfa384x_drvr_getconfig16(hfa384x_t *hw, u16 rid, void *val)
 {
 	int		result = 0;
-	result = hfa384x_drvr_getconfig(hw, rid, val, sizeof(UINT16));
+	result = hfa384x_drvr_getconfig(hw, rid, val, sizeof(u16));
 	if ( result == 0 ) {
-		*((UINT16*)val) = hfa384x2host_16(*((UINT16*)val));
+		*((u16*)val) = hfa384x2host_16(*((u16*)val));
 	}
 	return result;
 }
 
 static inline int
-hfa384x_drvr_getconfig32(hfa384x_t *hw, UINT16 rid, void *val)
+hfa384x_drvr_getconfig32(hfa384x_t *hw, u16 rid, void *val)
 {
 	int		result = 0;
 
-	result = hfa384x_drvr_getconfig(hw, rid, val, sizeof(UINT32));
+	result = hfa384x_drvr_getconfig(hw, rid, val, sizeof(u32));
 	if ( result == 0 ) {
-		*((UINT32*)val) = hfa384x2host_32(*((UINT32*)val));
+		*((u32*)val) = hfa384x2host_32(*((u32*)val));
 	}
 
 	return result;
 }
 
 static inline int
-hfa384x_drvr_setconfig16(hfa384x_t *hw, UINT16 rid, UINT16 val)
+hfa384x_drvr_setconfig16(hfa384x_t *hw, u16 rid, u16 val)
 {
-	UINT16 value = host2hfa384x_16(val);
+	u16 value = host2hfa384x_16(val);
 	return hfa384x_drvr_setconfig(hw, rid, &value, sizeof(value));
 }
 
 static inline int
-hfa384x_drvr_setconfig32(hfa384x_t *hw, UINT16 rid, UINT32 val)
+hfa384x_drvr_setconfig32(hfa384x_t *hw, u16 rid, u32 val)
 {
-	UINT32 value = host2hfa384x_32(val);
+	u32 value = host2hfa384x_32(val);
 	return hfa384x_drvr_setconfig(hw, rid, &value, sizeof(value));
 }
 
-#if (WLAN_HOSTIF == WLAN_USB)
 int
 hfa384x_drvr_getconfig_async(hfa384x_t     *hw,
-                              UINT16        rid,
+                              u16        rid,
                               ctlx_usercb_t usercb,
                               void          *usercb_data);
 
 int
 hfa384x_drvr_setconfig_async(hfa384x_t *hw,
-                              UINT16 rid,
+                              u16 rid,
                               void *buf,
-                              UINT16 len,
+                              u16 len,
                               ctlx_usercb_t usercb,
                               void *usercb_data);
-#else
-static inline int
-hfa384x_drvr_setconfig_async(hfa384x_t *hw, UINT16 rid, void *buf, UINT16 len,
-			     void *ptr1, void *ptr2)
-{
-         (void)ptr1;
-         (void)ptr2;
-         return hfa384x_drvr_setconfig(hw, rid, buf, len);
-}
-#endif
 
 static inline int
-hfa384x_drvr_setconfig16_async(hfa384x_t *hw, UINT16 rid, UINT16 val)
+hfa384x_drvr_setconfig16_async(hfa384x_t *hw, u16 rid, u16 val)
 {
-	UINT16 value = host2hfa384x_16(val);
+	u16 value = host2hfa384x_16(val);
 	return hfa384x_drvr_setconfig_async(hw, rid, &value, sizeof(value),
 					    NULL , NULL);
 }
 
 static inline int
-hfa384x_drvr_setconfig32_async(hfa384x_t *hw, UINT16 rid, UINT32 val)
+hfa384x_drvr_setconfig32_async(hfa384x_t *hw, u16 rid, u32 val)
 {
-	UINT32 value = host2hfa384x_32(val);
+	u32 value = host2hfa384x_32(val);
 	return hfa384x_drvr_setconfig_async(hw, rid, &value, sizeof(value),
 					    NULL , NULL);
 }
@@ -2839,32 +2730,28 @@ hfa384x_tx_timeout(wlandevice_t *wlandev);
 int
 hfa384x_cmd_initialize(hfa384x_t *hw);
 int
-hfa384x_cmd_enable(hfa384x_t *hw, UINT16 macport);
+hfa384x_cmd_enable(hfa384x_t *hw, u16 macport);
 int
-hfa384x_cmd_disable(hfa384x_t *hw, UINT16 macport);
+hfa384x_cmd_disable(hfa384x_t *hw, u16 macport);
 int
 hfa384x_cmd_diagnose(hfa384x_t *hw);
 int
-hfa384x_cmd_allocate(hfa384x_t *hw, UINT16 len);
-int
-hfa384x_cmd_transmit(hfa384x_t *hw, UINT16 reclaim, UINT16 qos, UINT16 fid);
-int
-hfa384x_cmd_clearpersist(hfa384x_t *hw, UINT16 fid);
+hfa384x_cmd_allocate(hfa384x_t *hw, u16 len);
 int
-hfa384x_cmd_notify(hfa384x_t *hw, UINT16 reclaim, UINT16 fid, void *buf, UINT16 len);
+hfa384x_cmd_transmit(hfa384x_t *hw, u16 reclaim, u16 qos, u16 fid);
 int
-hfa384x_cmd_inquire(hfa384x_t *hw, UINT16 fid);
+hfa384x_cmd_clearpersist(hfa384x_t *hw, u16 fid);
 int
-hfa384x_cmd_access(hfa384x_t *hw, UINT16 write, UINT16 rid, void *buf, UINT16 len);
+hfa384x_cmd_access(hfa384x_t *hw, u16 write, u16 rid, void *buf, u16 len);
 int
-hfa384x_cmd_monitor(hfa384x_t *hw, UINT16 enable);
+hfa384x_cmd_monitor(hfa384x_t *hw, u16 enable);
 int
 hfa384x_cmd_download(
 	hfa384x_t *hw,
-	UINT16 mode,
-	UINT16 lowaddr,
-	UINT16 highaddr,
-	UINT16 codelen);
+	u16 mode,
+	u16 lowaddr,
+	u16 highaddr,
+	u16 codelen);
 int
 hfa384x_cmd_aux_enable(hfa384x_t *hw, int force);
 int
@@ -2872,196 +2759,34 @@ hfa384x_cmd_aux_disable(hfa384x_t *hw);
 int
 hfa384x_copy_from_bap(
 	hfa384x_t *hw,
-	UINT16	bap,
-	UINT16	id,
-	UINT16	offset,
+	u16	bap,
+	u16	id,
+	u16	offset,
 	void	*buf,
-	UINT	len);
+	unsigned int	len);
 int
 hfa384x_copy_to_bap(
 	hfa384x_t *hw,
-	UINT16	bap,
-	UINT16	id,
-	UINT16	offset,
+	u16	bap,
+	u16	id,
+	u16	offset,
 	void	*buf,
-	UINT	len);
+	unsigned int	len);
 void
 hfa384x_copy_from_aux(
 	hfa384x_t *hw,
-	UINT32	cardaddr,
-	UINT32	auxctl,
+	u32	cardaddr,
+	u32	auxctl,
 	void	*buf,
-	UINT	len);
+	unsigned int	len);
 void
 hfa384x_copy_to_aux(
 	hfa384x_t *hw,
-	UINT32	cardaddr,
-	UINT32	auxctl,
+	u32	cardaddr,
+	u32	auxctl,
 	void	*buf,
-	UINT	len);
-
-#if (WLAN_HOSTIF != WLAN_USB)
-
-/*
-   HFA384x is a LITTLE ENDIAN part.
-
-   the get/setreg functions implicitly byte-swap the data to LE.
-   the _noswap variants do not perform a byte-swap on the data.
-*/
-
-static inline UINT16
-__hfa384x_getreg(hfa384x_t *hw, UINT reg);
-
-static inline void
-__hfa384x_setreg(hfa384x_t *hw, UINT16 val, UINT reg);
-
-static inline UINT16
-__hfa384x_getreg_noswap(hfa384x_t *hw, UINT reg);
-
-static inline void
-__hfa384x_setreg_noswap(hfa384x_t *hw, UINT16 val, UINT reg);
-
-#ifdef REVERSE_ENDIAN
-#define hfa384x_getreg __hfa384x_getreg_noswap
-#define hfa384x_setreg __hfa384x_setreg_noswap
-#define hfa384x_getreg_noswap __hfa384x_getreg
-#define hfa384x_setreg_noswap __hfa384x_setreg
-#else
-#define hfa384x_getreg __hfa384x_getreg
-#define hfa384x_setreg __hfa384x_setreg
-#define hfa384x_getreg_noswap __hfa384x_getreg_noswap
-#define hfa384x_setreg_noswap __hfa384x_setreg_noswap
-#endif
-
-/*----------------------------------------------------------------
-* hfa384x_getreg
-*
-* Retrieve the value of one of the MAC registers.  Done here
-* because different PRISM2 MAC parts use different buses and such.
-* NOTE: This function returns the value in HOST ORDER!!!!!!
-*
-* Arguments:
-*       hw         MAC part structure
-*       reg        Register identifier (offset for I/O based i/f)
-*
-* Returns:
-*       Value from the register in HOST ORDER!!!!
-----------------------------------------------------------------*/
-static inline UINT16
-__hfa384x_getreg(hfa384x_t *hw, UINT reg)
-{
-/*	printk(KERN_DEBUG "Reading from 0x%0x\n", hw->membase + reg); */
-#if ((WLAN_HOSTIF == WLAN_PCMCIA) || (WLAN_HOSTIF == WLAN_PLX))
-	return wlan_inw_le16_to_cpu(hw->iobase+reg);
-#elif (WLAN_HOSTIF == WLAN_PCI)
-	return __le16_to_cpu(readw(hw->membase + reg));
-#endif
-}
-
-/*----------------------------------------------------------------
-* hfa384x_setreg
-*
-* Set the value of one of the MAC registers.  Done here
-* because different PRISM2 MAC parts use different buses and such.
-* NOTE: This function assumes the value is in HOST ORDER!!!!!!
-*
-* Arguments:
-*       hw	MAC part structure
-*	val	Value, in HOST ORDER!!, to put in the register
-*       reg	Register identifier (offset for I/O based i/f)
-*
-* Returns:
-*       Nothing
-----------------------------------------------------------------*/
-static inline void
-__hfa384x_setreg(hfa384x_t *hw, UINT16 val, UINT reg)
-{
-#if ((WLAN_HOSTIF == WLAN_PCMCIA) || (WLAN_HOSTIF == WLAN_PLX))
-	wlan_outw_cpu_to_le16( val, hw->iobase + reg);
-	return;
-#elif (WLAN_HOSTIF == WLAN_PCI)
-	writew(__cpu_to_le16(val), hw->membase + reg);
-	return;
-#endif
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_getreg_noswap
-*
-* Retrieve the value of one of the MAC registers.  Done here
-* because different PRISM2 MAC parts use different buses and such.
-*
-* Arguments:
-*       hw         MAC part structure
-*       reg        Register identifier (offset for I/O based i/f)
-*
-* Returns:
-*       Value from the register.
-----------------------------------------------------------------*/
-static inline UINT16
-__hfa384x_getreg_noswap(hfa384x_t *hw, UINT reg)
-{
-#if ((WLAN_HOSTIF == WLAN_PCMCIA) || (WLAN_HOSTIF == WLAN_PLX))
-	return wlan_inw(hw->iobase+reg);
-#elif (WLAN_HOSTIF == WLAN_PCI)
-	return readw(hw->membase + reg);
-#endif
-}
-
-
-/*----------------------------------------------------------------
-* hfa384x_setreg_noswap
-*
-* Set the value of one of the MAC registers.  Done here
-* because different PRISM2 MAC parts use different buses and such.
-*
-* Arguments:
-*       hw	MAC part structure
-*	val	Value to put in the register
-*       reg	Register identifier (offset for I/O based i/f)
-*
-* Returns:
-*       Nothing
-----------------------------------------------------------------*/
-static inline void
-__hfa384x_setreg_noswap(hfa384x_t *hw, UINT16 val, UINT reg)
-{
-#if ((WLAN_HOSTIF == WLAN_PCMCIA) || (WLAN_HOSTIF == WLAN_PLX))
-	wlan_outw( val, hw->iobase + reg);
-	return;
-#elif (WLAN_HOSTIF == WLAN_PCI)
-	writew(val, hw->membase + reg);
-	return;
-#endif
-}
-
-
-static inline void hfa384x_events_all(hfa384x_t *hw)
-{
-	hfa384x_setreg(hw,
-		       HFA384x_INT_NORMAL
-#ifdef CMD_IRQ
-		       | HFA384x_INTEN_CMD_SET(1)
-#endif
-		       ,
-		       HFA384x_INTEN);
-
-}
-
-static inline void hfa384x_events_nobap(hfa384x_t *hw)
-{
-	hfa384x_setreg(hw,
-		        (HFA384x_INT_NORMAL & ~HFA384x_INT_BAP_OP)
-#ifdef CMD_IRQ
-		       | HFA384x_INTEN_CMD_SET(1)
-#endif
-		       ,
-		       HFA384x_INTEN);
-
-}
+	unsigned int	len);
 
-#endif /* WLAN_HOSTIF != WLAN_USB */
 #endif /* __KERNEL__ */
 
 #endif  /* _HFA384x_H */
diff --git a/drivers/staging/wlan-ng/hfa384x_usb.c b/drivers/staging/wlan-ng/hfa384x_usb.c
index db0c502f5d9..8a75b50f863 100644
--- a/drivers/staging/wlan-ng/hfa384x_usb.c
+++ b/drivers/staging/wlan-ng/hfa384x_usb.c
@@ -114,9 +114,6 @@
 /* System Includes */
 #define WLAN_DBVAR	prism2_debug
 
-#include "version.h"
-
-
 #include <linux/version.h>
 
 #include <linux/module.h>
@@ -136,63 +133,7 @@
 
 #include "wlan_compat.h"
 
-#if (WLAN_HOSTIF != WLAN_USB)
-#error "This file is specific to USB"
-#endif
-
-
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,10)
-static int
-wait_for_completion_interruptible(struct completion *x)
-{
-  int ret = 0;
-
-  might_sleep();
-
-  spin_lock_irq(&x->wait.lock);
-  if (!x->done) {
-    DECLARE_WAITQUEUE(wait, current);
-
-    wait.flags |= WQ_FLAG_EXCLUSIVE;
-    __add_wait_queue_tail(&x->wait, &wait);
-    do {
-      if (signal_pending(current)) {
-        ret = -ERESTARTSYS;
-        __remove_wait_queue(&x->wait, &wait);
-        goto out;
-      }
-      __set_current_state(TASK_INTERRUPTIBLE);
-      spin_unlock_irq(&x->wait.lock);
-      schedule();
-      spin_lock_irq(&x->wait.lock);
-    } while (!x->done);
-    __remove_wait_queue(&x->wait, &wait);
-  }
-  x->done--;
-out:
-  spin_unlock_irq(&x->wait.lock);
-
-  return ret;
-}
-#endif
-
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,69)
-static void
-usb_init_urb(struct urb *urb)
-{
-	memset(urb, 0, sizeof(*urb));
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0) /* tune me! */
-	urb->count = (atomic_t)ATOMIC_INIT(1);
-#endif
-	spin_lock_init(&urb->lock);
-}
-#endif
-
-#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0) /* tune me! */
-#  define SUBMIT_URB(u,f)  usb_submit_urb(u,f)
-#else
-#  define SUBMIT_URB(u,f)  usb_submit_urb(u)
-#endif
+#define SUBMIT_URB(u,f)  usb_submit_urb(u,f)
 
 /*================================================================*/
 /* Project Includes */
@@ -257,21 +198,12 @@ submit_tx_urb(hfa384x_t *hw, struct urb *tx_urb, gfp_t flags);
 
 /*---------------------------------------------------*/
 /* Callbacks */
-#ifdef URB_ONLY_CALLBACK
 static void
 hfa384x_usbout_callback(struct urb *urb);
 static void
 hfa384x_ctlxout_callback(struct urb *urb);
 static void
 hfa384x_usbin_callback(struct urb *urb);
-#else
-static void
-hfa384x_usbout_callback(struct urb *urb, struct pt_regs *regs);
-static void
-hfa384x_ctlxout_callback(struct urb *urb, struct pt_regs *regs);
-static void
-hfa384x_usbin_callback(struct urb *urb, struct pt_regs *regs);
-#endif
 
 static void
 hfa384x_usbin_txcompl(wlandevice_t *wlandev, hfa384x_usbin_t *usbin);
@@ -358,9 +290,9 @@ static int
 hfa384x_dorrid(
 	hfa384x_t *hw,
 	CMD_MODE mode,
-	UINT16	rid,
+	u16	rid,
 	void	*riddata,
-	UINT	riddatalen,
+	unsigned int	riddatalen,
 	ctlx_cmdcb_t cmdcb,
 	ctlx_usercb_t usercb,
 	void	*usercb_data);
@@ -369,9 +301,9 @@ static int
 hfa384x_dowrid(
 	hfa384x_t *hw,
 	CMD_MODE mode,
-	UINT16	rid,
+	u16	rid,
 	void	*riddata,
-	UINT	riddatalen,
+	unsigned int	riddatalen,
 	ctlx_cmdcb_t cmdcb,
 	ctlx_usercb_t usercb,
 	void	*usercb_data);
@@ -380,10 +312,10 @@ static int
 hfa384x_dormem(
 	hfa384x_t *hw,
 	CMD_MODE mode,
-	UINT16	page,
-	UINT16	offset,
+	u16	page,
+	u16	offset,
 	void	*data,
-	UINT	len,
+	unsigned int	len,
 	ctlx_cmdcb_t cmdcb,
 	ctlx_usercb_t usercb,
 	void	*usercb_data);
@@ -392,16 +324,16 @@ static int
 hfa384x_dowmem(
 	hfa384x_t *hw,
 	CMD_MODE mode,
-	UINT16	page,
-	UINT16	offset,
+	u16	page,
+	u16	offset,
 	void	*data,
-	UINT	len,
+	unsigned int	len,
 	ctlx_cmdcb_t cmdcb,
 	ctlx_usercb_t usercb,
 	void	*usercb_data);
 
 static int
-hfa384x_isgood_pdrcode(UINT16 pdrcode);
+hfa384x_isgood_pdrcode(u16 pdrcode);
 
 /*================================================================*/
 /* Function Definitions */
@@ -435,17 +367,17 @@ dbprint_urb(struct urb* urb)
 	WLAN_LOG_DEBUG(3,"urb->pipe=0x%08x\n", urb->pipe);
 	WLAN_LOG_DEBUG(3,"urb->status=0x%08x\n", urb->status);
 	WLAN_LOG_DEBUG(3,"urb->transfer_flags=0x%08x\n", urb->transfer_flags);
-	WLAN_LOG_DEBUG(3,"urb->transfer_buffer=0x%08x\n", (UINT)urb->transfer_buffer);
+	WLAN_LOG_DEBUG(3,"urb->transfer_buffer=0x%08x\n", (unsigned int)urb->transfer_buffer);
 	WLAN_LOG_DEBUG(3,"urb->transfer_buffer_length=0x%08x\n", urb->transfer_buffer_length);
 	WLAN_LOG_DEBUG(3,"urb->actual_length=0x%08x\n", urb->actual_length);
 	WLAN_LOG_DEBUG(3,"urb->bandwidth=0x%08x\n", urb->bandwidth);
-	WLAN_LOG_DEBUG(3,"urb->setup_packet(ctl)=0x%08x\n", (UINT)urb->setup_packet);
+	WLAN_LOG_DEBUG(3,"urb->setup_packet(ctl)=0x%08x\n", (unsigned int)urb->setup_packet);
 	WLAN_LOG_DEBUG(3,"urb->start_frame(iso/irq)=0x%08x\n", urb->start_frame);
 	WLAN_LOG_DEBUG(3,"urb->interval(irq)=0x%08x\n", urb->interval);
 	WLAN_LOG_DEBUG(3,"urb->error_count(iso)=0x%08x\n", urb->error_count);
 	WLAN_LOG_DEBUG(3,"urb->timeout=0x%08x\n", urb->timeout);
-	WLAN_LOG_DEBUG(3,"urb->context=0x%08x\n", (UINT)urb->context);
-	WLAN_LOG_DEBUG(3,"urb->complete=0x%08x\n", (UINT)urb->complete);
+	WLAN_LOG_DEBUG(3,"urb->context=0x%08x\n", (unsigned int)urb->context);
+	WLAN_LOG_DEBUG(3,"urb->complete=0x%08x\n", (unsigned int)urb->complete);
 }
 #endif
 
@@ -652,7 +584,7 @@ hfa384x_usb_defer(struct work_struct *data)
 
 	/* Resume transmitting. */
 	if ( test_and_clear_bit(WORK_TX_RESUME, &hw->usb_flags) ) {
-		p80211netdev_wake_queue(hw->wlandev);
+		netif_wake_queue(hw->wlandev->netdev);
 	}
 
 	DBFEXIT;
@@ -711,8 +643,8 @@ hfa384x_create( hfa384x_t *hw, struct usb_device *usb)
 	tasklet_init(&hw->completion_bh,
 	             hfa384x_usbctlx_completion_task,
 	             (unsigned long)hw);
-	INIT_WORK2(&hw->link_bh, prism2sta_processing_defer);
-	INIT_WORK2(&hw->usb_work, hfa384x_usb_defer);
+	INIT_WORK(&hw->link_bh, prism2sta_processing_defer);
+	INIT_WORK(&hw->usb_work, hfa384x_usb_defer);
 
 	init_timer(&hw->throttle);
 	hw->throttle.function = hfa384x_usb_throttlefn;
@@ -733,7 +665,7 @@ hfa384x_create( hfa384x_t *hw, struct usb_device *usb)
 	hw->link_status = HFA384x_LINK_NOTCONNECTED;
 	hw->state = HFA384x_STATE_INIT;
 
-        INIT_WORK2(&hw->commsqual_bh, prism2sta_commsqual_defer);
+        INIT_WORK(&hw->commsqual_bh, prism2sta_commsqual_defer);
 	init_timer(&hw->commsqual_timer);
 	hw->commsqual_timer.data = (unsigned long) hw;
 	hw->commsqual_timer.function = prism2sta_commsqual_timer;
@@ -888,7 +820,7 @@ struct usbctlx_rrid_completor
 
 	const hfa384x_usb_rridresp_t	*rridresp;
 	void			*riddata;
-	UINT			riddatalen;
+	unsigned int			riddatalen;
 };
 typedef struct usbctlx_rrid_completor usbctlx_rrid_completor_t;
 
@@ -919,7 +851,7 @@ static inline usbctlx_completor_t*
 init_rrid_completor(usbctlx_rrid_completor_t *completor,
                     const hfa384x_usb_rridresp_t *rridresp,
                     void *riddata,
-                    UINT riddatalen)
+                    unsigned int riddatalen)
 {
 	completor->head.complete = usbctlx_rrid_completor_fn;
 	completor->rridresp = rridresp;
@@ -952,7 +884,7 @@ struct usbctlx_rmem_completor
 
         const hfa384x_usb_rmemresp_t  *rmemresp;
         void                          *data;
-        UINT                          len;
+        unsigned int                          len;
 };
 typedef struct usbctlx_rmem_completor usbctlx_rmem_completor_t;
 
@@ -969,7 +901,7 @@ static inline usbctlx_completor_t*
 init_rmem_completor(usbctlx_rmem_completor_t *completor,
                     hfa384x_usb_rmemresp_t *rmemresp,
                     void *data,
-                    UINT len)
+                    unsigned int len)
 {
 	completor->head.complete = usbctlx_rmem_completor_fn;
 	completor->rmemresp = rmemresp;
@@ -1080,7 +1012,7 @@ hfa384x_docmd_async(hfa384x_t *hw,
 }
 
 static inline int
-hfa384x_dorrid_wait(hfa384x_t *hw, UINT16 rid, void *riddata, UINT riddatalen)
+hfa384x_dorrid_wait(hfa384x_t *hw, u16 rid, void *riddata, unsigned int riddatalen)
 {
 	return hfa384x_dorrid(hw, DOWAIT,
 	                      rid, riddata, riddatalen,
@@ -1089,7 +1021,7 @@ hfa384x_dorrid_wait(hfa384x_t *hw, UINT16 rid, void *riddata, UINT riddatalen)
 
 static inline int
 hfa384x_dorrid_async(hfa384x_t *hw,
-                     UINT16 rid, void *riddata, UINT riddatalen,
+                     u16 rid, void *riddata, unsigned int riddatalen,
                      ctlx_cmdcb_t cmdcb,
                      ctlx_usercb_t usercb,
                      void *usercb_data)
@@ -1100,7 +1032,7 @@ hfa384x_dorrid_async(hfa384x_t *hw,
 }
 
 static inline int
-hfa384x_dowrid_wait(hfa384x_t *hw, UINT16 rid, void *riddata, UINT riddatalen)
+hfa384x_dowrid_wait(hfa384x_t *hw, u16 rid, void *riddata, unsigned int riddatalen)
 {
 	return hfa384x_dowrid(hw, DOWAIT,
 	                      rid, riddata, riddatalen,
@@ -1109,7 +1041,7 @@ hfa384x_dowrid_wait(hfa384x_t *hw, UINT16 rid, void *riddata, UINT riddatalen)
 
 static inline int
 hfa384x_dowrid_async(hfa384x_t *hw,
-                     UINT16 rid, void *riddata, UINT riddatalen,
+                     u16 rid, void *riddata, unsigned int riddatalen,
                      ctlx_cmdcb_t cmdcb,
                      ctlx_usercb_t usercb,
                      void *usercb_data)
@@ -1121,7 +1053,7 @@ hfa384x_dowrid_async(hfa384x_t *hw,
 
 static inline int
 hfa384x_dormem_wait(hfa384x_t *hw,
-                    UINT16 page, UINT16 offset, void *data, UINT len)
+                    u16 page, u16 offset, void *data, unsigned int len)
 {
 	return hfa384x_dormem(hw, DOWAIT,
 	                      page, offset, data, len,
@@ -1130,7 +1062,7 @@ hfa384x_dormem_wait(hfa384x_t *hw,
 
 static inline int
 hfa384x_dormem_async(hfa384x_t *hw,
-                     UINT16 page, UINT16 offset, void *data, UINT len,
+                     u16 page, u16 offset, void *data, unsigned int len,
                      ctlx_cmdcb_t cmdcb,
                      ctlx_usercb_t usercb,
                      void *usercb_data)
@@ -1143,10 +1075,10 @@ hfa384x_dormem_async(hfa384x_t *hw,
 static inline int
 hfa384x_dowmem_wait(
         hfa384x_t *hw,
-        UINT16  page,
-        UINT16  offset,
+        u16  page,
+        u16  offset,
         void    *data,
-        UINT    len)
+        unsigned int    len)
 {
 	return hfa384x_dowmem(hw, DOWAIT,
                                   page, offset, data, len,
@@ -1156,10 +1088,10 @@ hfa384x_dowmem_wait(
 static inline int
 hfa384x_dowmem_async(
         hfa384x_t *hw,
-        UINT16  page,
-        UINT16  offset,
+        u16  page,
+        u16  offset,
         void    *data,
-        UINT    len,
+        unsigned int    len,
         ctlx_cmdcb_t cmdcb,
         ctlx_usercb_t usercb,
         void    *usercb_data)
@@ -1246,7 +1178,7 @@ hfa384x_cmd_initialize(hfa384x_t *hw)
 * Call context:
 *	process
 ----------------------------------------------------------------*/
-int hfa384x_cmd_disable(hfa384x_t *hw, UINT16 macport)
+int hfa384x_cmd_disable(hfa384x_t *hw, u16 macport)
 {
 	int	result = 0;
 	hfa384x_metacmd_t cmd;
@@ -1286,7 +1218,7 @@ int hfa384x_cmd_disable(hfa384x_t *hw, UINT16 macport)
 * Call context:
 *	process
 ----------------------------------------------------------------*/
-int hfa384x_cmd_enable(hfa384x_t *hw, UINT16 macport)
+int hfa384x_cmd_enable(hfa384x_t *hw, u16 macport)
 {
 	int	result = 0;
 	hfa384x_metacmd_t cmd;
@@ -1305,95 +1237,6 @@ int hfa384x_cmd_enable(hfa384x_t *hw, UINT16 macport)
 	return result;
 }
 
-
-/*----------------------------------------------------------------
-* hfa384x_cmd_notify
-*
-* Sends an info frame to the firmware to alter the behavior
-* of the f/w asynch processes.  Can only be called when the MAC
-* is in the enabled state.
-*
-* Arguments:
-*	hw		device structure
-*	reclaim		[0|1] indicates whether the given FID will
-*			be handed back (via Alloc event) for reuse.
-*			(host order)
-*	fid		FID of buffer containing the frame that was
-*			previously copied to MAC memory via the bap.
-*			(host order)
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - f/w status code
-*	<0		driver reported error (timeout|bad arg)
-*
-* Side effects:
-*	hw->resp0 will contain the FID being used by async notify
-*	process.  If reclaim==0, resp0 will be the same as the fid
-*	argument.  If reclaim==1, resp0 will be the different.
-*
-* Call context:
-*	process
-----------------------------------------------------------------*/
-int hfa384x_cmd_notify(hfa384x_t *hw, UINT16 reclaim, UINT16 fid,
-		       void *buf, UINT16 len)
-{
-#if 0
-	int	result = 0;
-	UINT16	cmd;
-	DBFENTER;
-	cmd =	HFA384x_CMD_CMDCODE_SET(HFA384x_CMDCODE_NOTIFY) |
-		HFA384x_CMD_RECL_SET(reclaim);
-	result = hfa384x_docmd_wait(hw, cmd);
-
-	DBFEXIT;
-	return result;
-#endif
-return 0;
-}
-
-
-#if 0
-/*----------------------------------------------------------------
-* hfa384x_cmd_inquiry
-*
-* Requests an info frame from the firmware.  The info frame will
-* be delivered asynchronously via the Info event.
-*
-* Arguments:
-*	hw		device structure
-*	fid		FID of the info frame requested. (host order)
-*
-* Returns:
-*	0		success
-*	>0		f/w reported failure - f/w status code
-*	<0		driver reported error (timeout|bad arg)
-*
-* Side effects:
-*
-* Call context:
-*	process
-----------------------------------------------------------------*/
-int hfa384x_cmd_inquiry(hfa384x_t *hw, UINT16 fid)
-{
-	int	result = 0;
-	hfa384x_metacmd_t cmd;
-
-	DBFENTER;
-
-	cmd.cmd = HFA384x_CMD_CMDCODE_SET(HFA384x_CMDCODE_INQ);
-	cmd.parm0 = 0;
-	cmd.parm1 = 0;
-	cmd.parm2 = 0;
-
-	result = hfa384x_docmd_wait(hw, &cmd);
-
-	DBFEXIT;
-	return result;
-}
-#endif
-
-
 /*----------------------------------------------------------------
 * hfa384x_cmd_monitor
 *
@@ -1423,7 +1266,7 @@ int hfa384x_cmd_inquiry(hfa384x_t *hw, UINT16 fid)
 * Call context:
 *	process
 ----------------------------------------------------------------*/
-int hfa384x_cmd_monitor(hfa384x_t *hw, UINT16 enable)
+int hfa384x_cmd_monitor(hfa384x_t *hw, u16 enable)
 {
 	int	result = 0;
 	hfa384x_metacmd_t cmd;
@@ -1481,8 +1324,8 @@ int hfa384x_cmd_monitor(hfa384x_t *hw, UINT16 enable)
 * Call context:
 *	process
 ----------------------------------------------------------------*/
-int hfa384x_cmd_download(hfa384x_t *hw, UINT16 mode, UINT16 lowaddr,
-				UINT16 highaddr, UINT16 codelen)
+int hfa384x_cmd_download(hfa384x_t *hw, u16 mode, u16 lowaddr,
+				u16 highaddr, u16 codelen)
 {
 	int	result = 0;
 	hfa384x_metacmd_t cmd;
@@ -1532,7 +1375,7 @@ int hfa384x_cmd_download(hfa384x_t *hw, UINT16 mode, UINT16 lowaddr,
 ----------------------------------------------------------------*/
 void
 hfa384x_copy_from_aux(
-	hfa384x_t *hw, UINT32 cardaddr, UINT32 auxctl, void *buf, UINT len)
+	hfa384x_t *hw, u32 cardaddr, u32 auxctl, void *buf, unsigned int len)
 {
 	DBFENTER;
 	WLAN_LOG_ERROR("not used in USB.\n");
@@ -1566,7 +1409,7 @@ hfa384x_copy_from_aux(
 ----------------------------------------------------------------*/
 void
 hfa384x_copy_to_aux(
-	hfa384x_t *hw, UINT32 cardaddr, UINT32 auxctl, void *buf, UINT len)
+	hfa384x_t *hw, u32 cardaddr, u32 auxctl, void *buf, unsigned int len)
 {
 	DBFENTER;
 	WLAN_LOG_ERROR("not used in USB.\n");
@@ -1599,78 +1442,10 @@ hfa384x_copy_to_aux(
 ----------------------------------------------------------------*/
 int hfa384x_corereset(hfa384x_t *hw, int holdtime, int settletime, int genesis)
 {
-#if 0
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0))
-	struct usb_device	*parent = hw->usb->parent;
-	int			i;
-	int			port = -1;
-#endif
-#endif
 	int 			result = 0;
 
-
-#define P2_USB_RT_PORT		(USB_TYPE_CLASS | USB_RECIP_OTHER)
-#define P2_USB_FEAT_RESET	4
-#define P2_USB_FEAT_C_RESET	20
-
 	DBFENTER;
 
-#if 0
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0))
-	/* Find the hub port */
-	for ( i = 0; i < parent->maxchild; i++) {
-		if (parent->children[i] == hw->usb) {
-			port = i;
-			break;
-		}
-	}
-	if (port < 0) return -ENOENT;
-
-	/* Set and clear the reset */
-	usb_control_msg(parent, usb_sndctrlpipe(parent, 0),
-		USB_REQ_SET_FEATURE, P2_USB_RT_PORT, P2_USB_FEAT_RESET,
-		port+1, NULL, 0, 1*HZ);
-	wait_ms(holdtime);
-	usb_control_msg(parent, usb_sndctrlpipe(parent, 0),
-		USB_REQ_CLEAR_FEATURE, P2_USB_RT_PORT, P2_USB_FEAT_C_RESET,
-		port+1, NULL, 0, 1*HZ);
-	wait_ms(settletime);
-
-	/* Set the device address */
-	result=usb_set_address(hw->usb);
-	if (result < 0) {
-		WLAN_LOG_ERROR("reset_usbdev: Dev not accepting address, "
-			"result=%d\n", result);
-		clear_bit(hw->usb->devnum, &hw->usb->bus->devmap.devicemap);
-		hw->usb->devnum = -1;
-		goto done;
-	}
-	/* Let the address settle */
-	wait_ms(20);
-
-	/* Assume we're reusing the original descriptor data */
-
-	/* Set the configuration. */
-	WLAN_LOG_DEBUG(3, "Setting Configuration %d\n",
-		hw->usb->config[0].bConfigurationValue);
-	result=usb_set_configuration(hw->usb, hw->usb->config[0].bConfigurationValue);
-	if ( result ) {
-		WLAN_LOG_ERROR("usb_set_configuration() failed, result=%d.\n",
-				result);
-		goto done;
-	}
-	/* Let the configuration settle */
-	wait_ms(20);
-
- done:
-#else
-	result=usb_reset_device(hw->usb);
-	if(result<0) {
-		WLAN_LOG_ERROR("usb_reset_device() failed, result=%d.\n",result);
-	}
-#endif
-#endif
-
 	result=usb_reset_device(hw->usb);
 	if(result<0) {
 		WLAN_LOG_ERROR("usb_reset_device() failed, result=%d.\n",result);
@@ -1925,9 +1700,9 @@ static int
 hfa384x_dorrid(
 	hfa384x_t *hw,
 	CMD_MODE mode,
-	UINT16	rid,
+	u16	rid,
 	void	*riddata,
-	UINT	riddatalen,
+	unsigned int	riddatalen,
         ctlx_cmdcb_t cmdcb,
 	ctlx_usercb_t usercb,
 	void	*usercb_data)
@@ -2011,9 +1786,9 @@ static int
 hfa384x_dowrid(
 	hfa384x_t *hw,
 	CMD_MODE mode,
-	UINT16	rid,
+	u16	rid,
 	void	*riddata,
-	UINT	riddatalen,
+	unsigned int	riddatalen,
 	ctlx_cmdcb_t cmdcb,
 	ctlx_usercb_t usercb,
 	void	*usercb_data)
@@ -2104,10 +1879,10 @@ static int
 hfa384x_dormem(
 	hfa384x_t *hw,
 	CMD_MODE mode,
-	UINT16	page,
-	UINT16	offset,
+	u16	page,
+	u16	offset,
 	void	*data,
-	UINT	len,
+	unsigned int	len,
 	ctlx_cmdcb_t cmdcb,
 	ctlx_usercb_t usercb,
 	void	*usercb_data)
@@ -2205,10 +1980,10 @@ static int
 hfa384x_dowmem(
 	hfa384x_t *hw,
 	CMD_MODE mode,
-	UINT16	page,
-	UINT16	offset,
+	u16	page,
+	u16	offset,
 	void	*data,
-	UINT	len,
+	unsigned int	len,
 	ctlx_cmdcb_t cmdcb,
 	ctlx_usercb_t usercb,
 	void	*usercb_data)
@@ -2325,7 +2100,7 @@ int hfa384x_drvr_commtallies( hfa384x_t *hw )
 * Call context:
 *	process
 ----------------------------------------------------------------*/
-int hfa384x_drvr_disable(hfa384x_t *hw, UINT16 macport)
+int hfa384x_drvr_disable(hfa384x_t *hw, u16 macport)
 {
 	int	result = 0;
 
@@ -2367,7 +2142,7 @@ int hfa384x_drvr_disable(hfa384x_t *hw, UINT16 macport)
 * Call context:
 *	process
 ----------------------------------------------------------------*/
-int hfa384x_drvr_enable(hfa384x_t *hw, UINT16 macport)
+int hfa384x_drvr_enable(hfa384x_t *hw, u16 macport)
 {
 	int	result = 0;
 
@@ -2520,22 +2295,22 @@ int hfa384x_drvr_flashdl_disable(hfa384x_t *hw)
 int
 hfa384x_drvr_flashdl_write(
 	hfa384x_t	*hw,
-	UINT32		daddr,
+	u32		daddr,
 	void		*buf,
-	UINT32		len)
+	u32		len)
 {
 	int		result = 0;
-	UINT32		dlbufaddr;
+	u32		dlbufaddr;
 	int		nburns;
-	UINT32		burnlen;
-	UINT32		burndaddr;
-	UINT16		burnlo;
-	UINT16		burnhi;
+	u32		burnlen;
+	u32		burndaddr;
+	u16		burnlo;
+	u16		burnhi;
 	int		nwrites;
-	UINT8		*writebuf;
-	UINT16		writepage;
-	UINT16		writeoffset;
-	UINT32		writelen;
+	u8		*writebuf;
+	u16		writepage;
+	u16		writeoffset;
+	u32		writelen;
 	int		i;
 	int		j;
 
@@ -2686,7 +2461,7 @@ exit_proc:
 * Call context:
 *	process
 ----------------------------------------------------------------*/
-int hfa384x_drvr_getconfig(hfa384x_t *hw, UINT16 rid, void *buf, UINT16 len)
+int hfa384x_drvr_getconfig(hfa384x_t *hw, u16 rid, void *buf, u16 len)
 {
 	int 			result;
 	DBFENTER;
@@ -2727,7 +2502,7 @@ int hfa384x_drvr_getconfig(hfa384x_t *hw, UINT16 rid, void *buf, UINT16 len)
 int
 hfa384x_drvr_getconfig_async(
          hfa384x_t               *hw,
-         UINT16                  rid,
+         u16                  rid,
          ctlx_usercb_t           usercb,
          void                    *usercb_data)
 {
@@ -2761,9 +2536,9 @@ hfa384x_drvr_getconfig_async(
 int
 hfa384x_drvr_setconfig_async(
          hfa384x_t       *hw,
-         UINT16          rid,
+         u16          rid,
          void            *buf,
-         UINT16          len,
+         u16          len,
          ctlx_usercb_t   usercb,
          void            *usercb_data)
 {
@@ -2790,7 +2565,7 @@ hfa384x_drvr_setconfig_async(
 * Call context:
 *	process
 ----------------------------------------------------------------*/
-int hfa384x_drvr_handover( hfa384x_t *hw, UINT8 *addr)
+int hfa384x_drvr_handover( hfa384x_t *hw, u8 *addr)
 {
         DBFENTER;
 	WLAN_LOG_ERROR("Not currently supported in USB!\n");
@@ -2824,88 +2599,6 @@ int hfa384x_drvr_low_level(hfa384x_t *hw, hfa384x_metacmd_t *cmd)
 }
 
 /*----------------------------------------------------------------
-* hfa384x_drvr_mmi_read
-*
-* Read mmi registers.  mmi is intersil-speak for the baseband
-* processor registers.
-*
-* Arguments:
-*       hw              device structure
-*       register        The test register to be accessed (must be even #).
-*
-* Returns:
-*       0               success
-*       >0              f/w reported error - f/w status code
-*       <0              driver reported error
-*
-* Side effects:
-*
-* Call context:
-*       process
-----------------------------------------------------------------*/
-int hfa384x_drvr_mmi_read(hfa384x_t *hw, UINT32 addr, UINT32 *resp)
-{
-#if 0
-        int             result = 0;
-        UINT16  cmd_code = (UINT16) 0x30;
-        UINT16 param = (UINT16) addr;
-        DBFENTER;
-
-        /* Do i need a host2hfa... conversion ? */
-        result = hfa384x_docmd_wait(hw, cmd_code);
-
-        DBFEXIT;
-        return result;
-#endif
-return 0;
-}
-
-/*----------------------------------------------------------------
-* hfa384x_drvr_mmi_write
-*
-* Read mmi registers.  mmi is intersil-speak for the baseband
-* processor registers.
-*
-* Arguments:
-*       hw              device structure
-*       addr            The test register to be accessed (must be even #).
-*       data            The data value to write to the register.
-*
-* Returns:
-*       0               success
-*       >0              f/w reported error - f/w status code
-*       <0              driver reported error
-*
-* Side effects:
-*
-* Call context:
-*       process
-----------------------------------------------------------------*/
-
-int
-hfa384x_drvr_mmi_write(hfa384x_t *hw, UINT32 addr, UINT32 data)
-{
-#if 0
-        int             result = 0;
-        UINT16  cmd_code = (UINT16) 0x31;
-        UINT16 param0 = (UINT16) addr;
-        UINT16 param1 = (UINT16) data;
-        DBFENTER;
-
-        WLAN_LOG_DEBUG(1,"mmi write : addr = 0x%08lx\n", addr);
-        WLAN_LOG_DEBUG(1,"mmi write : data = 0x%08lx\n", data);
-
-        /* Do i need a host2hfa... conversion ? */
-        result = hfa384x_docmd_wait(hw, cmd_code);
-
-        DBFEXIT;
-        return result;
-#endif
-return 0;
-}
-
-
-/*----------------------------------------------------------------
 * hfa384x_drvr_ramdl_disable
 *
 * Ends the ram download state.
@@ -2969,11 +2662,11 @@ hfa384x_drvr_ramdl_disable(hfa384x_t *hw)
 *	process
 ----------------------------------------------------------------*/
 int
-hfa384x_drvr_ramdl_enable(hfa384x_t *hw, UINT32 exeaddr)
+hfa384x_drvr_ramdl_enable(hfa384x_t *hw, u32 exeaddr)
 {
 	int		result = 0;
-	UINT16		lowaddr;
-	UINT16		hiaddr;
+	u16		lowaddr;
+	u16		hiaddr;
 	int		i;
 	DBFENTER;
 	/* Check that a port isn't active */
@@ -3044,16 +2737,16 @@ hfa384x_drvr_ramdl_enable(hfa384x_t *hw, UINT32 exeaddr)
 *	process
 ----------------------------------------------------------------*/
 int
-hfa384x_drvr_ramdl_write(hfa384x_t *hw, UINT32 daddr, void* buf, UINT32 len)
+hfa384x_drvr_ramdl_write(hfa384x_t *hw, u32 daddr, void* buf, u32 len)
 {
 	int		result = 0;
 	int		nwrites;
-	UINT8		*data = buf;
+	u8		*data = buf;
 	int		i;
-	UINT32		curraddr;
-	UINT16		currpage;
-	UINT16		curroffset;
-	UINT16		currlen;
+	u32		curraddr;
+	u16		currpage;
+	u16		curroffset;
+	u16		currlen;
 	DBFENTER;
 	/* Check that we're in the ram download state */
 	if ( hw->dlstate != HFA384x_DLSTATE_RAMENABLED ) {
@@ -3125,21 +2818,21 @@ hfa384x_drvr_ramdl_write(hfa384x_t *hw, UINT32 daddr, void* buf, UINT32 len)
 * Call context:
 *	process or non-card interrupt.
 ----------------------------------------------------------------*/
-int hfa384x_drvr_readpda(hfa384x_t *hw, void *buf, UINT len)
+int hfa384x_drvr_readpda(hfa384x_t *hw, void *buf, unsigned int len)
 {
 	int		result = 0;
-	UINT16		*pda = buf;
+	u16		*pda = buf;
 	int		pdaok = 0;
 	int		morepdrs = 1;
 	int		currpdr = 0;	/* word offset of the current pdr */
 	size_t		i;
-	UINT16		pdrlen;		/* pdr length in bytes, host order */
-	UINT16		pdrcode;	/* pdr code, host order */
-	UINT16		currpage;
-	UINT16		curroffset;
+	u16		pdrlen;		/* pdr length in bytes, host order */
+	u16		pdrcode;	/* pdr code, host order */
+	u16		currpage;
+	u16		curroffset;
 	struct pdaloc {
-		UINT32	cardaddr;
-		UINT16	auxctl;
+		u32	cardaddr;
+		u16	auxctl;
 	} pdaloc[] =
 	{
 		{ HFA3842_PDA_BASE,		0},
@@ -3243,7 +2936,7 @@ int hfa384x_drvr_readpda(hfa384x_t *hw, void *buf, UINT len)
 * Call context:
 *	process
 ----------------------------------------------------------------*/
-int hfa384x_drvr_setconfig(hfa384x_t *hw, UINT16 rid, void *buf, UINT16 len)
+int hfa384x_drvr_setconfig(hfa384x_t *hw, u16 rid, void *buf, u16 len)
 {
 	return hfa384x_dowrid_wait(hw, rid, buf, len);
 }
@@ -3267,19 +2960,38 @@ int hfa384x_drvr_setconfig(hfa384x_t *hw, UINT16 rid, void *buf, UINT16 len)
 * Call context:
 *	process
 ----------------------------------------------------------------*/
+
 int hfa384x_drvr_start(hfa384x_t *hw)
 {
-	int		result;
+	int		result, result1, result2;
+	u16		status;
 	DBFENTER;
 
 	might_sleep();
 
-	if (usb_clear_halt(hw->usb, hw->endp_in)) {
+	/* Clear endpoint stalls - but only do this if the endpoint
+	 * is showing a stall status. Some prism2 cards seem to behave
+	 * badly if a clear_halt is called when the endpoint is already
+	 * ok
+	 */
+	result = usb_get_status(hw->usb, USB_RECIP_ENDPOINT, hw->endp_in, &status);
+	if (result < 0) {
+		WLAN_LOG_ERROR(
+			"Cannot get bulk in endpoint status.\n");
+		goto done;
+	}
+	if ((status == 1) && usb_clear_halt(hw->usb, hw->endp_in)) {
 		WLAN_LOG_ERROR(
 			"Failed to reset bulk in endpoint.\n");
 	}
 
-	if (usb_clear_halt(hw->usb, hw->endp_out)) {
+	result = usb_get_status(hw->usb, USB_RECIP_ENDPOINT, hw->endp_out, &status);
+	if (result < 0) {
+		WLAN_LOG_ERROR(
+			"Cannot get bulk out endpoint status.\n");
+		goto done;
+	}
+	if ((status == 1) && usb_clear_halt(hw->usb, hw->endp_out)) {
 		WLAN_LOG_ERROR(
 			"Failed to reset bulk out endpoint.\n");
 	}
@@ -3296,14 +3008,37 @@ int hfa384x_drvr_start(hfa384x_t *hw)
 		goto done;
 	}
 
-	/* call initialize */
-	result = hfa384x_cmd_initialize(hw);
-	if (result != 0) {
-		usb_kill_urb(&hw->rx_urb);
-		WLAN_LOG_ERROR(
-			"cmd_initialize() failed, result=%d\n",
-			result);
-		goto done;
+	/* Call initialize twice, with a 1 second sleep in between.
+	 * This is a nasty work-around since many prism2 cards seem to
+	 * need time to settle after an init from cold. The second
+	 * call to initialize in theory is not necessary - but we call
+	 * it anyway as a double insurance policy:
+	 * 1) If the first init should fail, the second may well succeed
+	 *    and the card can still be used
+	 * 2) It helps ensures all is well with the card after the first
+	 *    init and settle time.
+	 */
+	result1 = hfa384x_cmd_initialize(hw);
+	msleep(1000);
+	result = result2 = hfa384x_cmd_initialize(hw);
+	if (result1 != 0) {
+		if (result2 != 0) {
+			WLAN_LOG_ERROR(
+				"cmd_initialize() failed on two attempts, results %d and %d\n",
+				result1, result2);
+			usb_kill_urb(&hw->rx_urb);
+			goto done;
+		} else {
+			WLAN_LOG_DEBUG(0, "First cmd_initialize() failed (result %d),\n",
+				result1);
+			WLAN_LOG_DEBUG(0, "but second attempt succeeded. All should be ok\n");
+		}
+	} else if (result2 != 0) {
+		WLAN_LOG_WARNING(
+			"First cmd_initialize() succeeded, but second attempt failed (result=%d)\n",
+			result2);
+		WLAN_LOG_WARNING("Most likely the card will be functional\n");
+			goto done;
 	}
 
 	hw->state = HFA384x_STATE_RUNNING;
@@ -3849,11 +3584,7 @@ hfa384x_usbctlxq_run(hfa384x_t	*hw)
 * Call context:
 *	interrupt
 ----------------------------------------------------------------*/
-#ifdef URB_ONLY_CALLBACK
 static void hfa384x_usbin_callback(struct urb *urb)
-#else
-static void hfa384x_usbin_callback(struct urb *urb, struct pt_regs *regs)
-#endif
 {
 	wlandevice_t		*wlandev = urb->context;
 	hfa384x_t		*hw;
@@ -3861,7 +3592,7 @@ static void hfa384x_usbin_callback(struct urb *urb, struct pt_regs *regs)
 	struct sk_buff          *skb = NULL;
 	int			result;
 	int                     urb_status;
-	UINT16			type;
+	u16			type;
 
 	enum USBIN_ACTION {
 		HANDLE,
@@ -3873,7 +3604,7 @@ static void hfa384x_usbin_callback(struct urb *urb, struct pt_regs *regs)
 
 	if ( !wlandev ||
 	     !wlandev->netdev ||
-	     !netif_device_present(wlandev->netdev) )
+	     wlandev->hwremoved )
 		goto exit;
 
 	hw = wlandev->priv;
@@ -4088,7 +3819,7 @@ retry:
 		if (unlocked_usbctlx_cancel_async(hw, ctlx) == 0)
 			run_queue = 1;
 	} else {
-		const UINT16 intype = (usbin->type&~host2hfa384x_16(0x8000));
+		const u16 intype = (usbin->type&~host2hfa384x_16(0x8000));
 
 		/*
 		 * Check that our message is what we're expecting ...
@@ -4168,7 +3899,7 @@ unlock:
 ----------------------------------------------------------------*/
 static void hfa384x_usbin_txcompl(wlandevice_t *wlandev, hfa384x_usbin_t *usbin)
 {
-	UINT16			status;
+	u16			status;
 	DBFENTER;
 
 	status = hfa384x2host_16(usbin->type); /* yeah I know it says type...*/
@@ -4208,8 +3939,8 @@ static void hfa384x_usbin_rx(wlandevice_t *wlandev, struct sk_buff *skb)
 	hfa384x_t               *hw = wlandev->priv;
 	int                     hdrlen;
 	p80211_rxmeta_t         *rxmeta;
-	UINT16                  data_len;
-	UINT16                  fc;
+	u16                  data_len;
+	u16                  fc;
 
 	DBFENTER;
 
@@ -4315,12 +4046,11 @@ done:
 static void hfa384x_int_rxmonitor( wlandevice_t *wlandev, hfa384x_usb_rxfrm_t *rxfrm)
 {
 	hfa384x_rx_frame_t              *rxdesc = &(rxfrm->desc);
-	UINT				hdrlen = 0;
-	UINT				datalen = 0;
-	UINT				skblen = 0;
-	p80211msg_lnxind_wlansniffrm_t	*msg;
-	UINT8				*datap;
-	UINT16				fc;
+	unsigned int				hdrlen = 0;
+	unsigned int				datalen = 0;
+	unsigned int				skblen = 0;
+	u8				*datap;
+	u16				fc;
 	struct sk_buff			*skb;
 	hfa384x_t		        *hw = wlandev->priv;
 
@@ -4333,15 +4063,15 @@ static void hfa384x_int_rxmonitor( wlandevice_t *wlandev, hfa384x_usb_rxfrm_t *r
 	datalen = hfa384x2host_16(rxdesc->data_len);
 
 	/* Allocate an ind message+framesize skb */
-	skblen = sizeof(p80211msg_lnxind_wlansniffrm_t) +
+	skblen = sizeof(p80211_caphdr_t) +
 		hdrlen + datalen + WLAN_CRC_LEN;
 
 	/* sanity check the length */
 	if ( skblen >
-		(sizeof(p80211msg_lnxind_wlansniffrm_t) +
-		WLAN_HDR_A4_LEN + WLAN_DATA_MAXLEN + WLAN_CRC_LEN) ) {
+	     (sizeof(p80211_caphdr_t) +
+	      WLAN_HDR_A4_LEN + WLAN_DATA_MAXLEN + WLAN_CRC_LEN) ) {
 		WLAN_LOG_DEBUG(1, "overlen frm: len=%zd\n",
-			skblen - sizeof(p80211msg_lnxind_wlansniffrm_t));
+			       skblen - sizeof(p80211_caphdr_t));
 	}
 
 	if ( (skb = dev_alloc_skb(skblen)) == NULL ) {
@@ -4351,66 +4081,7 @@ static void hfa384x_int_rxmonitor( wlandevice_t *wlandev, hfa384x_usb_rxfrm_t *r
 
 	/* only prepend the prism header if in the right mode */
 	if ((wlandev->netdev->type == ARPHRD_IEEE80211_PRISM) &&
-	    (hw->sniffhdr == 0)) {
-		datap = skb_put(skb, sizeof(p80211msg_lnxind_wlansniffrm_t));
-		msg = (p80211msg_lnxind_wlansniffrm_t*) datap;
-
-		/* Initialize the message members */
-		msg->msgcode = DIDmsg_lnxind_wlansniffrm;
-		msg->msglen = sizeof(p80211msg_lnxind_wlansniffrm_t);
-		strcpy(msg->devname, wlandev->name);
-
-		msg->hosttime.did = DIDmsg_lnxind_wlansniffrm_hosttime;
-		msg->hosttime.status = 0;
-		msg->hosttime.len = 4;
-		msg->hosttime.data = jiffies;
-
-		msg->mactime.did = DIDmsg_lnxind_wlansniffrm_mactime;
-		msg->mactime.status = 0;
-		msg->mactime.len = 4;
-		msg->mactime.data = rxdesc->time;
-
-		msg->channel.did = DIDmsg_lnxind_wlansniffrm_channel;
-		msg->channel.status = 0;
-		msg->channel.len = 4;
-		msg->channel.data = hw->sniff_channel;
-
-		msg->rssi.did = DIDmsg_lnxind_wlansniffrm_rssi;
-		msg->rssi.status = P80211ENUM_msgitem_status_no_value;
-		msg->rssi.len = 4;
-		msg->rssi.data = 0;
-
-		msg->sq.did = DIDmsg_lnxind_wlansniffrm_sq;
-		msg->sq.status = P80211ENUM_msgitem_status_no_value;
-		msg->sq.len = 4;
-		msg->sq.data = 0;
-
-		msg->signal.did = DIDmsg_lnxind_wlansniffrm_signal;
-		msg->signal.status = 0;
-		msg->signal.len = 4;
-		msg->signal.data = rxdesc->signal;
-
-		msg->noise.did = DIDmsg_lnxind_wlansniffrm_noise;
-		msg->noise.status = 0;
-		msg->noise.len = 4;
-		msg->noise.data = rxdesc->silence;
-
-		msg->rate.did = DIDmsg_lnxind_wlansniffrm_rate;
-		msg->rate.status = 0;
-		msg->rate.len = 4;
-		msg->rate.data = rxdesc->rate / 5; /* set to 802.11 units */
-
-		msg->istx.did = DIDmsg_lnxind_wlansniffrm_istx;
-		msg->istx.status = 0;
-		msg->istx.len = 4;
-		msg->istx.data = P80211ENUM_truth_false;
-
-		msg->frmlen.did = DIDmsg_lnxind_wlansniffrm_frmlen;
-		msg->frmlen.status = 0;
-		msg->frmlen.len = 4;
-		msg->frmlen.data = hdrlen + datalen + WLAN_CRC_LEN;
-	} else if ((wlandev->netdev->type == ARPHRD_IEEE80211_PRISM) &&
-		   (hw->sniffhdr != 0)) {
+	    (hw->sniffhdr != 0)) {
 		p80211_caphdr_t		*caphdr;
 		/* The NEW header format! */
 		datap = skb_put(skb, sizeof(p80211_caphdr_t));
@@ -4508,11 +4179,7 @@ static void hfa384x_usbin_info(wlandevice_t *wlandev, hfa384x_usbin_t *usbin)
 * Call context:
 *	interrupt
 ----------------------------------------------------------------*/
-#ifdef URB_ONLY_CALLBACK
 static void hfa384x_usbout_callback(struct urb *urb)
-#else
-static void hfa384x_usbout_callback(struct urb *urb, struct pt_regs *regs)
-#endif
 {
 	wlandevice_t		*wlandev = urb->context;
 	hfa384x_usbout_t	*usbout = urb->transfer_buffer;
@@ -4589,11 +4256,7 @@ static void hfa384x_usbout_callback(struct urb *urb, struct pt_regs *regs)
 * Call context:
 * interrupt
 ----------------------------------------------------------------*/
-#ifdef URB_ONLY_CALLBACK
 static void hfa384x_ctlxout_callback(struct urb *urb)
-#else
-static void hfa384x_ctlxout_callback(struct urb *urb, struct pt_regs *regs)
-#endif
 {
 	hfa384x_t	*hw = urb->context;
 	int             delete_resptimer = 0;
@@ -4969,7 +4632,7 @@ static void hfa384x_usbout_tx(wlandevice_t *wlandev, hfa384x_usbout_t *usbout)
 * Call context:
 ----------------------------------------------------------------*/
 static int
-hfa384x_isgood_pdrcode(UINT16 pdrcode)
+hfa384x_isgood_pdrcode(u16 pdrcode)
 {
 	switch(pdrcode) {
 	case HFA384x_PDR_END_OF_PDA:
diff --git a/drivers/staging/wlan-ng/p80211conv.c b/drivers/staging/wlan-ng/p80211conv.c
index 68121b9b34f..dfc7b3a1e9c 100644
--- a/drivers/staging/wlan-ng/p80211conv.c
+++ b/drivers/staging/wlan-ng/p80211conv.c
@@ -52,10 +52,6 @@
 /*================================================================*/
 /* System Includes */
 
-#define __NO_VERSION__		/* prevent the static definition */
-
-
-#include <linux/version.h>
 
 #include <linux/module.h>
 #include <linux/kernel.h>
@@ -70,7 +66,6 @@
 
 #include <asm/byteorder.h>
 
-#include "version.h"
 #include "wlan_compat.h"
 
 /*================================================================*/
@@ -100,8 +95,8 @@
 /*================================================================*/
 /* Local Static Definitions */
 
-static UINT8	oui_rfc1042[] = {0x00, 0x00, 0x00};
-static UINT8	oui_8021h[] = {0x00, 0x00, 0xf8};
+static u8	oui_rfc1042[] = {0x00, 0x00, 0x00};
+static u8	oui_8021h[] = {0x00, 0x00, 0xf8};
 
 /*================================================================*/
 /* Local Function Declarations */
@@ -135,11 +130,11 @@ static UINT8	oui_8021h[] = {0x00, 0x00, 0xf8};
 * Call context:
 *	May be called in interrupt or non-interrupt context
 ----------------------------------------------------------------*/
-int skb_ether_to_p80211( wlandevice_t *wlandev, UINT32 ethconv, struct sk_buff *skb, p80211_hdr_t *p80211_hdr, p80211_metawep_t *p80211_wep)
+int skb_ether_to_p80211( wlandevice_t *wlandev, u32 ethconv, struct sk_buff *skb, p80211_hdr_t *p80211_hdr, p80211_metawep_t *p80211_wep)
 {
 
-	UINT16          fc;
-	UINT16          proto;
+	u16          fc;
+	u16          proto;
 	wlan_ethhdr_t   e_hdr;
 	wlan_llc_t      *e_llc;
 	wlan_snap_t     *e_snap;
@@ -298,14 +293,14 @@ static void orinoco_spy_gather(wlandevice_t *wlandev, char *mac,
 * Call context:
 *	May be called in interrupt or non-interrupt context
 ----------------------------------------------------------------*/
-int skb_p80211_to_ether( wlandevice_t *wlandev, UINT32 ethconv, struct sk_buff *skb)
+int skb_p80211_to_ether( wlandevice_t *wlandev, u32 ethconv, struct sk_buff *skb)
 {
 	netdevice_t     *netdev = wlandev->netdev;
-	UINT16          fc;
-	UINT            payload_length;
-	UINT            payload_offset;
-	UINT8		daddr[WLAN_ETHADDR_LEN];
-	UINT8		saddr[WLAN_ETHADDR_LEN];
+	u16          fc;
+	unsigned int            payload_length;
+	unsigned int            payload_offset;
+	u8		daddr[WLAN_ETHADDR_LEN];
+	u8		saddr[WLAN_ETHADDR_LEN];
 	p80211_hdr_t    *w_hdr;
 	wlan_ethhdr_t   *e_hdr;
 	wlan_llc_t      *e_llc;
@@ -333,11 +328,11 @@ int skb_p80211_to_ether( wlandevice_t *wlandev, UINT32 ethconv, struct sk_buff *
 		memcpy(saddr, w_hdr->a3.a2, WLAN_ETHADDR_LEN);
 	} else {
 		payload_offset = WLAN_HDR_A4_LEN;
-		payload_length -= ( WLAN_HDR_A4_LEN - WLAN_HDR_A3_LEN );
-		if (payload_length < 0 ) {
+		if (payload_length < WLAN_HDR_A4_LEN - WLAN_HDR_A3_LEN) {
 			WLAN_LOG_ERROR("A4 frame too short!\n");
 			return 1;
 		}
+		payload_length -= (WLAN_HDR_A4_LEN - WLAN_HDR_A3_LEN);
 		memcpy(daddr, w_hdr->a4.a3, WLAN_ETHADDR_LEN);
 		memcpy(saddr, w_hdr->a4.a4, WLAN_ETHADDR_LEN);
 	}
@@ -497,8 +492,16 @@ int skb_p80211_to_ether( wlandevice_t *wlandev, UINT32 ethconv, struct sk_buff *
 
 	}
 
+        /*
+         * Note that eth_type_trans() expects an skb w/ skb->data pointing
+         * at the MAC header, it then sets the following skb members:
+         * skb->mac_header,
+         * skb->data, and
+         * skb->pkt_type.
+         * It then _returns_ the value that _we're_ supposed to stuff in
+         * skb->protocol.  This is nuts.
+         */
 	skb->protocol = eth_type_trans(skb, netdev);
-	skb_reset_mac_header(skb);
 
         /* jkriegl: process signal and noise as set in hfa384x_int_rx() */
 	/* jkriegl: only process signal/noise if requested by iwspy */
@@ -528,7 +531,7 @@ int skb_p80211_to_ether( wlandevice_t *wlandev, UINT32 ethconv, struct sk_buff *
 * Call context:
 *	May be called in interrupt or non-interrupt context
 ----------------------------------------------------------------*/
-int p80211_stt_findproto(UINT16 proto)
+int p80211_stt_findproto(u16 proto)
 {
 	/* Always return found for now.  This is the behavior used by the */
 	/*  Zoom Win95 driver when 802.1h mode is selected */
diff --git a/drivers/staging/wlan-ng/p80211conv.h b/drivers/staging/wlan-ng/p80211conv.h
index 3f5ab57cd9a..538e9bd1490 100644
--- a/drivers/staging/wlan-ng/p80211conv.h
+++ b/drivers/staging/wlan-ng/p80211conv.h
@@ -86,22 +86,22 @@ typedef struct p80211_rxmeta
 {
 	struct wlandevice	*wlandev;
 
-	UINT64	mactime;	/* Hi-rez MAC-supplied time value */
-	UINT64	hosttime;	/* Best-rez host supplied time value */
+	u64	mactime;	/* Hi-rez MAC-supplied time value */
+	u64	hosttime;	/* Best-rez host supplied time value */
 
-	UINT	rxrate;		/* Receive data rate in 100kbps */
-	UINT	priority;	/* 0-15, 0=contention, 6=CF */
-	INT	signal;		/* An SSI, see p80211netdev.h */
-	INT	noise;		/* An SSI, see p80211netdev.h */
-	UINT	channel;	/* Receive channel (mostly for snifs) */
-	UINT	preamble;	/* P80211ENUM_preambletype_* */
-	UINT	encoding;	/* P80211ENUM_encoding_* */
+	unsigned int	rxrate;		/* Receive data rate in 100kbps */
+	unsigned int	priority;	/* 0-15, 0=contention, 6=CF */
+	int	signal;		/* An SSI, see p80211netdev.h */
+	int	noise;		/* An SSI, see p80211netdev.h */
+	unsigned int	channel;	/* Receive channel (mostly for snifs) */
+	unsigned int	preamble;	/* P80211ENUM_preambletype_* */
+	unsigned int	encoding;	/* P80211ENUM_encoding_* */
 
 } p80211_rxmeta_t;
 
 typedef struct p80211_frmmeta
 {
-	UINT			magic;
+	unsigned int			magic;
 	p80211_rxmeta_t		*rx;
 } p80211_frmmeta_t;
 
@@ -117,20 +117,20 @@ void p80211skb_rxmeta_detach(struct sk_buff *skb);
  */
 typedef struct p80211_caphdr
 {
-	UINT32		version;
-	UINT32		length;
-	UINT64		mactime;
-	UINT64		hosttime;
-	UINT32		phytype;
-	UINT32		channel;
-	UINT32		datarate;
-	UINT32		antenna;
-	UINT32		priority;
-	UINT32		ssi_type;
-	INT32		ssi_signal;
-	INT32		ssi_noise;
-	UINT32		preamble;
-	UINT32		encoding;
+	u32		version;
+	u32		length;
+	u64		mactime;
+	u64		hosttime;
+	u32		phytype;
+	u32		channel;
+	u32		datarate;
+	u32		antenna;
+	u32		priority;
+	u32		ssi_type;
+	s32		ssi_signal;
+	s32		ssi_noise;
+	u32		preamble;
+	u32		encoding;
 } p80211_caphdr_t;
 
 /* buffer free method pointer type */
@@ -138,31 +138,31 @@ typedef void (* freebuf_method_t)(void *buf, int size);
 
 typedef struct p80211_metawep {
 	void  *data;
-	UINT8 iv[4];
-	UINT8 icv[4];
+	u8 iv[4];
+	u8 icv[4];
 } p80211_metawep_t;
 
 /* local ether header type */
 typedef struct wlan_ethhdr
 {
-	UINT8	daddr[WLAN_ETHADDR_LEN];
-	UINT8	saddr[WLAN_ETHADDR_LEN];
-	UINT16	type;
+	u8	daddr[WLAN_ETHADDR_LEN];
+	u8	saddr[WLAN_ETHADDR_LEN];
+	u16	type;
 } __WLAN_ATTRIB_PACK__ wlan_ethhdr_t;
 
 /* local llc header type */
 typedef struct wlan_llc
 {
-	UINT8	dsap;
-	UINT8	ssap;
-	UINT8	ctl;
+	u8	dsap;
+	u8	ssap;
+	u8	ctl;
 } __WLAN_ATTRIB_PACK__ wlan_llc_t;
 
 /* local snap header type */
 typedef struct wlan_snap
 {
-	UINT8	oui[WLAN_IEEE_OUI_LEN];
-	UINT16	type;
+	u8	oui[WLAN_IEEE_OUI_LEN];
+	u16	type;
 } __WLAN_ATTRIB_PACK__ wlan_snap_t;
 
 /* Circular include trick */
@@ -174,13 +174,13 @@ struct wlandevice;
 /*================================================================*/
 /*Function Declarations */
 
-int skb_p80211_to_ether( struct wlandevice *wlandev, UINT32 ethconv,
+int skb_p80211_to_ether( struct wlandevice *wlandev, u32 ethconv,
 			 struct sk_buff *skb);
-int skb_ether_to_p80211( struct wlandevice *wlandev, UINT32 ethconv,
+int skb_ether_to_p80211( struct wlandevice *wlandev, u32 ethconv,
 			 struct sk_buff *skb, p80211_hdr_t *p80211_hdr,
 			 p80211_metawep_t *p80211_wep );
 
-int p80211_stt_findproto(UINT16 proto);
-int p80211_stt_addproto(UINT16 proto);
+int p80211_stt_findproto(u16 proto);
+int p80211_stt_addproto(u16 proto);
 
 #endif
diff --git a/drivers/staging/wlan-ng/p80211hdr.h b/drivers/staging/wlan-ng/p80211hdr.h
index b7b0872fd86..72f12aff390 100644
--- a/drivers/staging/wlan-ng/p80211hdr.h
+++ b/drivers/staging/wlan-ng/p80211hdr.h
@@ -166,29 +166,29 @@
 /*                        SET_FC_FSTYPE(WLAN_FSTYPE_RTS) );   */
 /*------------------------------------------------------------*/
 
-#define WLAN_GET_FC_PVER(n)	 (((UINT16)(n)) & (BIT0 | BIT1))
-#define WLAN_GET_FC_FTYPE(n)	((((UINT16)(n)) & (BIT2 | BIT3)) >> 2)
-#define WLAN_GET_FC_FSTYPE(n)	((((UINT16)(n)) & (BIT4|BIT5|BIT6|BIT7)) >> 4)
-#define WLAN_GET_FC_TODS(n) 	((((UINT16)(n)) & (BIT8)) >> 8)
-#define WLAN_GET_FC_FROMDS(n)	((((UINT16)(n)) & (BIT9)) >> 9)
-#define WLAN_GET_FC_MOREFRAG(n) ((((UINT16)(n)) & (BIT10)) >> 10)
-#define WLAN_GET_FC_RETRY(n)	((((UINT16)(n)) & (BIT11)) >> 11)
-#define WLAN_GET_FC_PWRMGT(n)	((((UINT16)(n)) & (BIT12)) >> 12)
-#define WLAN_GET_FC_MOREDATA(n) ((((UINT16)(n)) & (BIT13)) >> 13)
-#define WLAN_GET_FC_ISWEP(n)	((((UINT16)(n)) & (BIT14)) >> 14)
-#define WLAN_GET_FC_ORDER(n)	((((UINT16)(n)) & (BIT15)) >> 15)
-
-#define WLAN_SET_FC_PVER(n)	((UINT16)(n))
-#define WLAN_SET_FC_FTYPE(n)	(((UINT16)(n)) << 2)
-#define WLAN_SET_FC_FSTYPE(n)	(((UINT16)(n)) << 4)
-#define WLAN_SET_FC_TODS(n) 	(((UINT16)(n)) << 8)
-#define WLAN_SET_FC_FROMDS(n)	(((UINT16)(n)) << 9)
-#define WLAN_SET_FC_MOREFRAG(n) (((UINT16)(n)) << 10)
-#define WLAN_SET_FC_RETRY(n)	(((UINT16)(n)) << 11)
-#define WLAN_SET_FC_PWRMGT(n)	(((UINT16)(n)) << 12)
-#define WLAN_SET_FC_MOREDATA(n) (((UINT16)(n)) << 13)
-#define WLAN_SET_FC_ISWEP(n)	(((UINT16)(n)) << 14)
-#define WLAN_SET_FC_ORDER(n)	(((UINT16)(n)) << 15)
+#define WLAN_GET_FC_PVER(n)	 (((u16)(n)) & (BIT0 | BIT1))
+#define WLAN_GET_FC_FTYPE(n)	((((u16)(n)) & (BIT2 | BIT3)) >> 2)
+#define WLAN_GET_FC_FSTYPE(n)	((((u16)(n)) & (BIT4|BIT5|BIT6|BIT7)) >> 4)
+#define WLAN_GET_FC_TODS(n) 	((((u16)(n)) & (BIT8)) >> 8)
+#define WLAN_GET_FC_FROMDS(n)	((((u16)(n)) & (BIT9)) >> 9)
+#define WLAN_GET_FC_MOREFRAG(n) ((((u16)(n)) & (BIT10)) >> 10)
+#define WLAN_GET_FC_RETRY(n)	((((u16)(n)) & (BIT11)) >> 11)
+#define WLAN_GET_FC_PWRMGT(n)	((((u16)(n)) & (BIT12)) >> 12)
+#define WLAN_GET_FC_MOREDATA(n) ((((u16)(n)) & (BIT13)) >> 13)
+#define WLAN_GET_FC_ISWEP(n)	((((u16)(n)) & (BIT14)) >> 14)
+#define WLAN_GET_FC_ORDER(n)	((((u16)(n)) & (BIT15)) >> 15)
+
+#define WLAN_SET_FC_PVER(n)	((u16)(n))
+#define WLAN_SET_FC_FTYPE(n)	(((u16)(n)) << 2)
+#define WLAN_SET_FC_FSTYPE(n)	(((u16)(n)) << 4)
+#define WLAN_SET_FC_TODS(n) 	(((u16)(n)) << 8)
+#define WLAN_SET_FC_FROMDS(n)	(((u16)(n)) << 9)
+#define WLAN_SET_FC_MOREFRAG(n) (((u16)(n)) << 10)
+#define WLAN_SET_FC_RETRY(n)	(((u16)(n)) << 11)
+#define WLAN_SET_FC_PWRMGT(n)	(((u16)(n)) << 12)
+#define WLAN_SET_FC_MOREDATA(n) (((u16)(n)) << 13)
+#define WLAN_SET_FC_ISWEP(n)	(((u16)(n)) << 14)
+#define WLAN_SET_FC_ORDER(n)	(((u16)(n)) << 15)
 
 /*--- Duration Macros ----------------------------------------*/
 /* Macros to get/set the bitfields of the Duration Field      */
@@ -201,45 +201,45 @@
 /* Macros to get/set the bitfields of the Sequence Control    */
 /* Field.                                                     */
 /*------------------------------------------------------------*/
-#define WLAN_GET_SEQ_FRGNUM(n) (((UINT16)(n)) & (BIT0|BIT1|BIT2|BIT3))
-#define WLAN_GET_SEQ_SEQNUM(n) ((((UINT16)(n)) & (~(BIT0|BIT1|BIT2|BIT3))) >> 4)
+#define WLAN_GET_SEQ_FRGNUM(n) (((u16)(n)) & (BIT0|BIT1|BIT2|BIT3))
+#define WLAN_GET_SEQ_SEQNUM(n) ((((u16)(n)) & (~(BIT0|BIT1|BIT2|BIT3))) >> 4)
 
 /*--- Data ptr macro -----------------------------------------*/
-/* Creates a UINT8* to the data portion of a frame            */
+/* Creates a u8* to the data portion of a frame            */
 /* Assumes you're passing in a ptr to the beginning of the hdr*/
 /*------------------------------------------------------------*/
-#define WLAN_HDR_A3_DATAP(p) (((UINT8*)(p)) + WLAN_HDR_A3_LEN)
-#define WLAN_HDR_A4_DATAP(p) (((UINT8*)(p)) + WLAN_HDR_A4_LEN)
+#define WLAN_HDR_A3_DATAP(p) (((u8*)(p)) + WLAN_HDR_A3_LEN)
+#define WLAN_HDR_A4_DATAP(p) (((u8*)(p)) + WLAN_HDR_A4_LEN)
 
-#define DOT11_RATE5_ISBASIC_GET(r)     (((UINT8)(r)) & BIT7)
+#define DOT11_RATE5_ISBASIC_GET(r)     (((u8)(r)) & BIT7)
 
 /*================================================================*/
 /* Types */
 
 /* BSS Timestamp */
-typedef UINT8 wlan_bss_ts_t[WLAN_BSS_TS_LEN];
+typedef u8 wlan_bss_ts_t[WLAN_BSS_TS_LEN];
 
 /* Generic 802.11 Header types */
 
 typedef struct p80211_hdr_a3
 {
-	UINT16	fc;
-	UINT16	dur;
-	UINT8	a1[WLAN_ADDR_LEN];
-	UINT8	a2[WLAN_ADDR_LEN];
-	UINT8	a3[WLAN_ADDR_LEN];
-	UINT16	seq;
+	u16	fc;
+	u16	dur;
+	u8	a1[WLAN_ADDR_LEN];
+	u8	a2[WLAN_ADDR_LEN];
+	u8	a3[WLAN_ADDR_LEN];
+	u16	seq;
 } __WLAN_ATTRIB_PACK__ p80211_hdr_a3_t;
 
 typedef struct p80211_hdr_a4
 {
-	UINT16	fc;
-	UINT16	dur;
-	UINT8	a1[WLAN_ADDR_LEN];
-	UINT8	a2[WLAN_ADDR_LEN];
-	UINT8	a3[WLAN_ADDR_LEN];
-	UINT16	seq;
-	UINT8	a4[WLAN_ADDR_LEN];
+	u16	fc;
+	u16	dur;
+	u8	a1[WLAN_ADDR_LEN];
+	u8	a2[WLAN_ADDR_LEN];
+	u8	a3[WLAN_ADDR_LEN];
+	u16	seq;
+	u8	a4[WLAN_ADDR_LEN];
 } __WLAN_ATTRIB_PACK__ p80211_hdr_a4_t;
 
 typedef union p80211_hdr
@@ -271,9 +271,9 @@ typedef union p80211_hdr
 #define WLAN_FCS_LEN			4
 
 /* ftcl in HOST order */
-inline static UINT16 p80211_headerlen(UINT16 fctl)
+inline static u16 p80211_headerlen(u16 fctl)
 {
-	UINT16 hdrlen = 0;
+	u16 hdrlen = 0;
 
 	switch ( WLAN_GET_FC_FTYPE(fctl) ) {
 	case WLAN_FTYPE_MGMT:
diff --git a/drivers/staging/wlan-ng/p80211ioctl.h b/drivers/staging/wlan-ng/p80211ioctl.h
index 25b2ea83622..ad67b698fa4 100644
--- a/drivers/staging/wlan-ng/p80211ioctl.h
+++ b/drivers/staging/wlan-ng/p80211ioctl.h
@@ -106,9 +106,9 @@ typedef struct p80211ioctl_req
 {
 	char 	name[WLAN_DEVNAMELEN_MAX];
 	caddr_t data;
-	UINT32	magic;
-	UINT16	len;
-	UINT32	result;
+	u32	magic;
+	u16	len;
+	u32	result;
 } __WLAN_ATTRIB_PACK__ p80211ioctl_req_t;
 
 
diff --git a/drivers/staging/wlan-ng/p80211meta.h b/drivers/staging/wlan-ng/p80211meta.h
index 5cb3f5ada4f..8b61e5fc292 100644
--- a/drivers/staging/wlan-ng/p80211meta.h
+++ b/drivers/staging/wlan-ng/p80211meta.h
@@ -90,7 +90,7 @@
 #define MKMIBMETASIZE(name)		p80211meta_ ## mib ## _ ## name ## _ ## size
 #define MKGRPMETASIZE(name)		p80211meta_ ## grp ## _ ## name ## _ ## size
 
-#define GETMETASIZE(aptr)		(**((UINT32**)(aptr)))
+#define GETMETASIZE(aptr)		(**((u32**)(aptr)))
 
 /*----------------------------------------------------------------*/
 /* The following ifdef depends on the following defines: */
@@ -114,14 +114,14 @@
 typedef struct p80211meta
 {
 	char			*name;		/* data item name */
-	UINT32			did;		/* partial did */
-	UINT32			flags;		/* set of various flag bits */
-	UINT32			min;		/* min value of a BOUNDEDINT */
-	UINT32			max;		/* max value of a BOUNDEDINT */
-
-	UINT32			maxlen;		/* maxlen of a OCTETSTR or DISPLAYSTR */
-	UINT32			minlen;		/* minlen of a OCTETSTR or DISPLAYSTR */
-	p80211enum_t		*enumptr;	/* ptr to the enum type for ENUMINT */
+	u32			did;		/* partial did */
+	u32			flags;		/* set of various flag bits */
+	u32			min;		/* min value of a BOUNDEDint */
+	u32			max;		/* max value of a BOUNDEDint */
+
+	u32			maxlen;		/* maxlen of a OCTETSTR or DISPLAYSTR */
+	u32			minlen;		/* minlen of a OCTETSTR or DISPLAYSTR */
+	p80211enum_t		*enumptr;	/* ptr to the enum type for ENUMint */
 	p80211_totext_t		totextptr;	/* ptr to totext conversion function */
 	p80211_fromtext_t	fromtextptr;	/* ptr to totext conversion function */
 	p80211_valid_t		validfunptr;	/* ptr to totext conversion function */
@@ -150,20 +150,20 @@ typedef struct catlistitem
 
 /*----------------------------------------------------------------*/
 /* */
-UINT32 p80211_text2did(catlistitem_t *catlist, char *catname, char *grpname, char *itemname);
-UINT32 p80211_text2catdid(catlistitem_t *list, char *name );
-UINT32 p80211_text2grpdid(grplistitem_t *list, char *name );
-UINT32 p80211_text2itemdid(p80211meta_t *list, char *name );
-UINT32 p80211_isvalid_did( catlistitem_t *catlist, UINT32 did );
-UINT32 p80211_isvalid_catdid( catlistitem_t *catlist, UINT32 did );
-UINT32 p80211_isvalid_grpdid( catlistitem_t *catlist, UINT32 did );
-UINT32 p80211_isvalid_itemdid( catlistitem_t *catlist, UINT32 did );
-catlistitem_t *p80211_did2cat( catlistitem_t *catlist, UINT32 did );
-grplistitem_t *p80211_did2grp( catlistitem_t *catlist, UINT32 did );
-p80211meta_t *p80211_did2item( catlistitem_t *catlist, UINT32 did );
-UINT32 p80211item_maxdatalen( struct catlistitem *metalist, UINT32 did );
-UINT32 p80211_metaname2did(struct catlistitem *metalist, char *itemname);
-UINT32 p80211item_getoffset( struct catlistitem *metalist, UINT32 did );
+u32 p80211_text2did(catlistitem_t *catlist, char *catname, char *grpname, char *itemname);
+u32 p80211_text2catdid(catlistitem_t *list, char *name );
+u32 p80211_text2grpdid(grplistitem_t *list, char *name );
+u32 p80211_text2itemdid(p80211meta_t *list, char *name );
+u32 p80211_isvalid_did( catlistitem_t *catlist, u32 did );
+u32 p80211_isvalid_catdid( catlistitem_t *catlist, u32 did );
+u32 p80211_isvalid_grpdid( catlistitem_t *catlist, u32 did );
+u32 p80211_isvalid_itemdid( catlistitem_t *catlist, u32 did );
+catlistitem_t *p80211_did2cat( catlistitem_t *catlist, u32 did );
+grplistitem_t *p80211_did2grp( catlistitem_t *catlist, u32 did );
+p80211meta_t *p80211_did2item( catlistitem_t *catlist, u32 did );
+u32 p80211item_maxdatalen( struct catlistitem *metalist, u32 did );
+u32 p80211_metaname2did(struct catlistitem *metalist, char *itemname);
+u32 p80211item_getoffset( struct catlistitem *metalist, u32 did );
 int p80211item_gettype(p80211meta_t *meta);
 
 #endif /* _P80211META_H */
diff --git a/drivers/staging/wlan-ng/p80211metadef.h b/drivers/staging/wlan-ng/p80211metadef.h
index 2c7f435a97e..ce4fdd547fc 100644
--- a/drivers/staging/wlan-ng/p80211metadef.h
+++ b/drivers/staging/wlan-ng/p80211metadef.h
@@ -72,25 +72,6 @@
 			(P80211DID_MKSECTION(1) | \
 			P80211DID_MKGROUP(2) | \
 			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_dot11req_powermgmt \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(3))
-#define DIDmsg_dot11req_powermgmt_powermgmtmode \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_dot11req_powermgmt_wakeup \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_dot11req_powermgmt_receivedtims \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(3) | 0x00000000)
-#define DIDmsg_dot11req_powermgmt_resultcode \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(4) | 0x00000000)
 #define DIDmsg_dot11req_scan \
 			(P80211DID_MKSECTION(1) | \
 			P80211DID_MKGROUP(4))
@@ -301,211 +282,6 @@
 			(P80211DID_MKSECTION(1) | \
 			P80211DID_MKGROUP(5) | \
 			P80211DID_MKITEM(40) | 0x00000000)
-#define DIDmsg_dot11req_join \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6))
-#define DIDmsg_dot11req_join_bssid \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_dot11req_join_joinfailuretimeout \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_dot11req_join_basicrate1 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(3) | 0x00000000)
-#define DIDmsg_dot11req_join_basicrate2 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(4) | 0x00000000)
-#define DIDmsg_dot11req_join_basicrate3 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(5) | 0x00000000)
-#define DIDmsg_dot11req_join_basicrate4 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(6) | 0x00000000)
-#define DIDmsg_dot11req_join_basicrate5 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(7) | 0x00000000)
-#define DIDmsg_dot11req_join_basicrate6 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(8) | 0x00000000)
-#define DIDmsg_dot11req_join_basicrate7 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(9) | 0x00000000)
-#define DIDmsg_dot11req_join_basicrate8 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(10) | 0x00000000)
-#define DIDmsg_dot11req_join_operationalrate1 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(11) | 0x00000000)
-#define DIDmsg_dot11req_join_operationalrate2 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(12) | 0x00000000)
-#define DIDmsg_dot11req_join_operationalrate3 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(13) | 0x00000000)
-#define DIDmsg_dot11req_join_operationalrate4 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(14) | 0x00000000)
-#define DIDmsg_dot11req_join_operationalrate5 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(15) | 0x00000000)
-#define DIDmsg_dot11req_join_operationalrate6 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(16) | 0x00000000)
-#define DIDmsg_dot11req_join_operationalrate7 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(17) | 0x00000000)
-#define DIDmsg_dot11req_join_operationalrate8 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(18) | 0x00000000)
-#define DIDmsg_dot11req_join_resultcode \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(19) | 0x00000000)
-#define DIDmsg_dot11req_authenticate \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(7))
-#define DIDmsg_dot11req_authenticate_peerstaaddress \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_dot11req_authenticate_authenticationtype \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_dot11req_authenticate_authenticationfailuretimeout \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(3) | 0x00000000)
-#define DIDmsg_dot11req_authenticate_resultcode \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(4) | 0x00000000)
-#define DIDmsg_dot11req_deauthenticate \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(8))
-#define DIDmsg_dot11req_deauthenticate_peerstaaddress \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(8) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_dot11req_deauthenticate_reasoncode \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(8) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_dot11req_deauthenticate_resultcode \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(8) | \
-			P80211DID_MKITEM(3) | 0x00000000)
-#define DIDmsg_dot11req_associate \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(9))
-#define DIDmsg_dot11req_associate_peerstaaddress \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(9) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_dot11req_associate_associatefailuretimeout \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(9) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_dot11req_associate_cfpollable \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(9) | \
-			P80211DID_MKITEM(3) | 0x00000000)
-#define DIDmsg_dot11req_associate_cfpollreq \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(9) | \
-			P80211DID_MKITEM(4) | 0x00000000)
-#define DIDmsg_dot11req_associate_privacy \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(9) | \
-			P80211DID_MKITEM(5) | 0x00000000)
-#define DIDmsg_dot11req_associate_listeninterval \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(9) | \
-			P80211DID_MKITEM(6) | 0x00000000)
-#define DIDmsg_dot11req_associate_resultcode \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(9) | \
-			P80211DID_MKITEM(7) | 0x00000000)
-#define DIDmsg_dot11req_reassociate \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(10))
-#define DIDmsg_dot11req_reassociate_newapaddress \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(10) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_dot11req_reassociate_reassociatefailuretimeout \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(10) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_dot11req_reassociate_cfpollable \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(10) | \
-			P80211DID_MKITEM(3) | 0x00000000)
-#define DIDmsg_dot11req_reassociate_cfpollreq \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(10) | \
-			P80211DID_MKITEM(4) | 0x00000000)
-#define DIDmsg_dot11req_reassociate_privacy \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(10) | \
-			P80211DID_MKITEM(5) | 0x00000000)
-#define DIDmsg_dot11req_reassociate_listeninterval \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(10) | \
-			P80211DID_MKITEM(6) | 0x00000000)
-#define DIDmsg_dot11req_reassociate_resultcode \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(10) | \
-			P80211DID_MKITEM(7) | 0x00000000)
-#define DIDmsg_dot11req_disassociate \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(11))
-#define DIDmsg_dot11req_disassociate_peerstaaddress \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(11) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_dot11req_disassociate_reasoncode \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(11) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_dot11req_disassociate_resultcode \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(11) | \
-			P80211DID_MKITEM(3) | 0x00000000)
-#define DIDmsg_dot11req_reset \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(12))
-#define DIDmsg_dot11req_reset_setdefaultmib \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(12) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_dot11req_reset_macaddress \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(12) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_dot11req_reset_resultcode \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(12) | \
-			P80211DID_MKITEM(3) | 0x00000000)
 #define DIDmsg_dot11req_start \
 			(P80211DID_MKSECTION(1) | \
 			P80211DID_MKGROUP(13))
@@ -795,147 +571,8 @@
 			(P80211DID_MKSECTION(3) | \
 			P80211DID_MKGROUP(5) | \
 			P80211DID_MKITEM(3) | 0x00000000)
-#define DIDmsg_cat_lnxind \
-			P80211DID_MKSECTION(4)
-#define DIDmsg_lnxind_wlansniffrm \
-			(P80211DID_MKSECTION(4) | \
-			P80211DID_MKGROUP(1))
-#define DIDmsg_lnxind_wlansniffrm_hosttime \
-			(P80211DID_MKSECTION(4) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_lnxind_wlansniffrm_mactime \
-			(P80211DID_MKSECTION(4) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_lnxind_wlansniffrm_channel \
-			(P80211DID_MKSECTION(4) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(3) | 0x00000000)
-#define DIDmsg_lnxind_wlansniffrm_rssi \
-			(P80211DID_MKSECTION(4) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(4) | 0x00000000)
-#define DIDmsg_lnxind_wlansniffrm_sq \
-			(P80211DID_MKSECTION(4) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(5) | 0x00000000)
-#define DIDmsg_lnxind_wlansniffrm_signal \
-			(P80211DID_MKSECTION(4) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(6) | 0x00000000)
-#define DIDmsg_lnxind_wlansniffrm_noise \
-			(P80211DID_MKSECTION(4) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(7) | 0x00000000)
-#define DIDmsg_lnxind_wlansniffrm_rate \
-			(P80211DID_MKSECTION(4) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(8) | 0x00000000)
-#define DIDmsg_lnxind_wlansniffrm_istx \
-			(P80211DID_MKSECTION(4) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(9) | 0x00000000)
-#define DIDmsg_lnxind_wlansniffrm_frmlen \
-			(P80211DID_MKSECTION(4) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(10) | 0x00000000)
-#define DIDmsg_lnxind_roam \
-			(P80211DID_MKSECTION(4) | \
-			P80211DID_MKGROUP(2))
-#define DIDmsg_lnxind_roam_reason \
-			(P80211DID_MKSECTION(4) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(1) | 0x00000000)
 #define DIDmsg_cat_p2req \
 			P80211DID_MKSECTION(5)
-#define DIDmsg_p2req_join \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1))
-#define DIDmsg_p2req_join_bssid \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_p2req_join_basicrate1 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_p2req_join_basicrate2 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(3) | 0x00000000)
-#define DIDmsg_p2req_join_basicrate3 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(4) | 0x00000000)
-#define DIDmsg_p2req_join_basicrate4 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(5) | 0x00000000)
-#define DIDmsg_p2req_join_basicrate5 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(6) | 0x00000000)
-#define DIDmsg_p2req_join_basicrate6 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(7) | 0x00000000)
-#define DIDmsg_p2req_join_basicrate7 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(8) | 0x00000000)
-#define DIDmsg_p2req_join_basicrate8 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(9) | 0x00000000)
-#define DIDmsg_p2req_join_operationalrate1 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(10) | 0x00000000)
-#define DIDmsg_p2req_join_operationalrate2 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(11) | 0x00000000)
-#define DIDmsg_p2req_join_operationalrate3 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(12) | 0x00000000)
-#define DIDmsg_p2req_join_operationalrate4 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(13) | 0x00000000)
-#define DIDmsg_p2req_join_operationalrate5 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(14) | 0x00000000)
-#define DIDmsg_p2req_join_operationalrate6 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(15) | 0x00000000)
-#define DIDmsg_p2req_join_operationalrate7 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(16) | 0x00000000)
-#define DIDmsg_p2req_join_operationalrate8 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(17) | 0x00000000)
-#define DIDmsg_p2req_join_ssid \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(18) | 0x00000000)
-#define DIDmsg_p2req_join_channel \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(19) | 0x00000000)
-#define DIDmsg_p2req_join_authtype \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(20) | 0x00000000)
-#define DIDmsg_p2req_join_resultcode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(21) | 0x00000000)
 #define DIDmsg_p2req_readpda \
 			(P80211DID_MKSECTION(5) | \
 			P80211DID_MKGROUP(2))
@@ -947,162 +584,6 @@
 			(P80211DID_MKSECTION(5) | \
 			P80211DID_MKGROUP(2) | \
 			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_p2req_readcis \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3))
-#define DIDmsg_p2req_readcis_cis \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_p2req_readcis_resultcode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_p2req_auxport_state \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(4))
-#define DIDmsg_p2req_auxport_state_enable \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(4) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_p2req_auxport_state_resultcode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(4) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_p2req_auxport_read \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5))
-#define DIDmsg_p2req_auxport_read_addr \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_p2req_auxport_read_len \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_p2req_auxport_read_data \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(3) | 0x00000000)
-#define DIDmsg_p2req_auxport_read_resultcode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(4) | 0x00000000)
-#define DIDmsg_p2req_auxport_write \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6))
-#define DIDmsg_p2req_auxport_write_addr \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_p2req_auxport_write_len \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_p2req_auxport_write_data \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(3) | 0x00000000)
-#define DIDmsg_p2req_auxport_write_resultcode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(4) | 0x00000000)
-#define DIDmsg_p2req_low_level \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(7))
-#define DIDmsg_p2req_low_level_command \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_p2req_low_level_param0 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_p2req_low_level_param1 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(3) | 0x00000000)
-#define DIDmsg_p2req_low_level_param2 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(4) | 0x00000000)
-#define DIDmsg_p2req_low_level_resp0 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(5) | 0x00000000)
-#define DIDmsg_p2req_low_level_resp1 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(6) | 0x00000000)
-#define DIDmsg_p2req_low_level_resp2 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(7) | 0x00000000)
-#define DIDmsg_p2req_low_level_resultcode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(8) | 0x00000000)
-#define DIDmsg_p2req_test_command \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(8))
-#define DIDmsg_p2req_test_command_testcode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(8) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_p2req_test_command_testparam \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(8) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_p2req_test_command_resultcode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(8) | \
-			P80211DID_MKITEM(3) | 0x00000000)
-#define DIDmsg_p2req_test_command_status \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(8) | \
-			P80211DID_MKITEM(4) | 0x00000000)
-#define DIDmsg_p2req_test_command_resp0 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(8) | \
-			P80211DID_MKITEM(5) | 0x00000000)
-#define DIDmsg_p2req_test_command_resp1 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(8) | \
-			P80211DID_MKITEM(6) | 0x00000000)
-#define DIDmsg_p2req_test_command_resp2 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(8) | \
-			P80211DID_MKITEM(7) | 0x00000000)
-#define DIDmsg_p2req_mmi_read \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(9))
-#define DIDmsg_p2req_mmi_read_addr \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(9) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_p2req_mmi_read_value \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(9) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_p2req_mmi_read_resultcode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(9) | \
-			P80211DID_MKITEM(3) | 0x00000000)
-#define DIDmsg_p2req_mmi_write \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(10))
-#define DIDmsg_p2req_mmi_write_addr \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(10) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_p2req_mmi_write_data \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(10) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_p2req_mmi_write_resultcode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(10) | \
-			P80211DID_MKITEM(3) | 0x00000000)
 #define DIDmsg_p2req_ramdl_state \
 			(P80211DID_MKSECTION(5) | \
 			P80211DID_MKGROUP(11))
@@ -1167,224 +648,8 @@
 			(P80211DID_MKSECTION(5) | \
 			P80211DID_MKGROUP(14) | \
 			P80211DID_MKITEM(4) | 0x00000000)
-#define DIDmsg_p2req_mm_state \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(15))
-#define DIDmsg_p2req_mm_state_enable \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(15) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_p2req_mm_state_resultcode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(15) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_p2req_dump_state \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(16))
-#define DIDmsg_p2req_dump_state_level \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(16) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_p2req_dump_state_resultcode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(16) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_p2req_channel_info \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(17))
-#define DIDmsg_p2req_channel_info_channellist \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(17) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_p2req_channel_info_channeldwelltime \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(17) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_p2req_channel_info_resultcode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(17) | \
-			P80211DID_MKITEM(3) | 0x00000000)
-#define DIDmsg_p2req_channel_info_numchinfo \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(17) | \
-			P80211DID_MKITEM(4) | 0x00000000)
-#define DIDmsg_p2req_channel_info_results \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(18))
-#define DIDmsg_p2req_channel_info_results_channel \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(18) | \
-			P80211DID_MKITEM(1) | 0x00000000)
-#define DIDmsg_p2req_channel_info_results_resultcode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(18) | \
-			P80211DID_MKITEM(2) | 0x00000000)
-#define DIDmsg_p2req_channel_info_results_avgnoiselevel \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(18) | \
-			P80211DID_MKITEM(3) | 0x00000000)
-#define DIDmsg_p2req_channel_info_results_peaknoiselevel \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(18) | \
-			P80211DID_MKITEM(4) | 0x00000000)
-#define DIDmsg_p2req_channel_info_results_bssactive \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(18) | \
-			P80211DID_MKITEM(5) | 0x00000000)
-#define DIDmsg_p2req_channel_info_results_pcfactive \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(18) | \
-			P80211DID_MKITEM(6) | 0x00000000)
-#define DIDmsg_p2req_enable \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(19))
-#define DIDmsg_p2req_enable_resultcode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(19) | \
-			P80211DID_MKITEM(1) | 0x00000000)
 #define DIDmib_cat_dot11smt \
 			P80211DID_MKSECTION(1)
-#define DIDmib_dot11smt_p80211Table \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(1))
-#define DIDmib_dot11smt_p80211Table_p80211_ifstate \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(1) | 0x10000000)
-#define DIDmib_dot11smt_dot11StationConfigTable \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2))
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11StationID \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(1) | 0x18000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11MediumOccupancyLimit \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(2) | 0x18000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11CFPollable \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(3) | 0x10000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11CFPPeriod \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(4) | 0x18000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11CFPMaxDuration \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(5) | 0x18000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11AuthenticationResponseTimeOut \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(6) | 0x18000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11PrivacyOptionImplemented \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(7) | 0x10000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11PowerManagementMode \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(8) | 0x18000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11DesiredSSID \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(9) | 0x18000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11DesiredBSSType \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(10) | 0x18000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11OperationalRateSet \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(11) | 0x18000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11BeaconPeriod \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(12) | 0x18000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11DTIMPeriod \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(13) | 0x18000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11AssociationResponseTimeOut \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(14) | 0x10000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11DisassociateReason \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(15) | 0x10000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11DisassociateStation \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(16) | 0x10000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11DeauthenticateReason \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(17) | 0x10000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11DeauthenticateStation \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(18) | 0x10000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11AuthenticateFailStatus \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(19) | 0x10000000)
-#define DIDmib_dot11smt_dot11StationConfigTable_dot11AuthenticateFailStation \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(20) | 0x10000000)
-#define DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(3))
-#define DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithm1 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(1) | 0x14000000)
-#define DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithmsEnable1 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(2) | 0x1c000000)
-#define DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithm2 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(3) | 0x14000000)
-#define DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithmsEnable2 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(4) | 0x1c000000)
-#define DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithm3 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(5) | 0x14000000)
-#define DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithmsEnable3 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(6) | 0x1c000000)
-#define DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithm4 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(7) | 0x14000000)
-#define DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithmsEnable4 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(8) | 0x1c000000)
-#define DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithm5 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(9) | 0x14000000)
-#define DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithmsEnable5 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(10) | 0x1c000000)
-#define DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithm6 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(11) | 0x14000000)
-#define DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithmsEnable6 \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(12) | 0x1c000000)
 #define DIDmib_dot11smt_dot11WEPDefaultKeysTable \
 			(P80211DID_MKSECTION(1) | \
 			P80211DID_MKGROUP(4))
@@ -1404,25 +669,6 @@
 			(P80211DID_MKSECTION(1) | \
 			P80211DID_MKGROUP(4) | \
 			P80211DID_MKITEM(4) | 0x0c000000)
-#define DIDmib_dot11smt_dot11WEPKeyMappingsTable \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(5))
-#define DIDmib_dot11smt_dot11WEPKeyMappingsTable_dot11WEPKeyMappingIndex \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(1) | 0x1c000000)
-#define DIDmib_dot11smt_dot11WEPKeyMappingsTable_dot11WEPKeyMappingAddress \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(2) | 0x1c000000)
-#define DIDmib_dot11smt_dot11WEPKeyMappingsTable_dot11WEPKeyMappingWEPOn \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(3) | 0x1c000000)
-#define DIDmib_dot11smt_dot11WEPKeyMappingsTable_dot11WEPKeyMappingValue \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(4) | 0x1c000000)
 #define DIDmib_dot11smt_dot11PrivacyTable \
 			(P80211DID_MKSECTION(1) | \
 			P80211DID_MKGROUP(6))
@@ -1434,31 +680,19 @@
 			(P80211DID_MKSECTION(1) | \
 			P80211DID_MKGROUP(6) | \
 			P80211DID_MKITEM(2) | 0x18000000)
-#define DIDmib_dot11smt_dot11PrivacyTable_dot11WEPKeyMappingLength \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(3) | 0x18000000)
 #define DIDmib_dot11smt_dot11PrivacyTable_dot11ExcludeUnencrypted \
 			(P80211DID_MKSECTION(1) | \
 			P80211DID_MKGROUP(6) | \
 			P80211DID_MKITEM(4) | 0x18000000)
-#define DIDmib_dot11smt_dot11PrivacyTable_dot11WEPICVErrorCount \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(5) | 0x10000000)
-#define DIDmib_dot11smt_dot11PrivacyTable_dot11WEPExcludedCount \
-			(P80211DID_MKSECTION(1) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(6) | 0x10000000)
 #define DIDmib_cat_dot11mac \
 			P80211DID_MKSECTION(2)
 #define DIDmib_dot11mac_dot11OperationTable \
 			(P80211DID_MKSECTION(2) | \
 			P80211DID_MKGROUP(1))
 #define DIDmib_dot11mac_dot11OperationTable_dot11MACAddress \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(1) | 0x18000000)
+                        (P80211DID_MKSECTION(2) | \
+                        P80211DID_MKGROUP(1) | \
+                        P80211DID_MKITEM(1) | 0x18000000)
 #define DIDmib_dot11mac_dot11OperationTable_dot11RTSThreshold \
 			(P80211DID_MKSECTION(2) | \
 			P80211DID_MKGROUP(1) | \
@@ -1476,329 +710,18 @@
 			P80211DID_MKGROUP(1) | \
 			P80211DID_MKITEM(5) | 0x18000000)
 #define DIDmib_dot11mac_dot11OperationTable_dot11MaxTransmitMSDULifetime \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(6) | 0x10000000)
-#define DIDmib_dot11mac_dot11OperationTable_dot11MaxReceiveLifetime \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(7) | 0x10000000)
-#define DIDmib_dot11mac_dot11OperationTable_dot11ManufacturerID \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(8) | 0x10000000)
-#define DIDmib_dot11mac_dot11OperationTable_dot11ProductID \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(9) | 0x10000000)
-#define DIDmib_dot11mac_dot11CountersTable \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(2))
-#define DIDmib_dot11mac_dot11CountersTable_dot11TransmittedFragmentCount \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(1) | 0x10000000)
-#define DIDmib_dot11mac_dot11CountersTable_dot11MulticastTransmittedFrameCount \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(2) | 0x10000000)
-#define DIDmib_dot11mac_dot11CountersTable_dot11FailedCount \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(3) | 0x10000000)
-#define DIDmib_dot11mac_dot11CountersTable_dot11RetryCount \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(4) | 0x10000000)
-#define DIDmib_dot11mac_dot11CountersTable_dot11MultipleRetryCount \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(5) | 0x10000000)
-#define DIDmib_dot11mac_dot11CountersTable_dot11FrameDuplicateCount \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(6) | 0x10000000)
-#define DIDmib_dot11mac_dot11CountersTable_dot11RTSSuccessCount \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(7) | 0x10000000)
-#define DIDmib_dot11mac_dot11CountersTable_dot11RTSFailureCount \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(8) | 0x10000000)
-#define DIDmib_dot11mac_dot11CountersTable_dot11ACKFailureCount \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(9) | 0x10000000)
-#define DIDmib_dot11mac_dot11CountersTable_dot11ReceivedFragmentCount \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(10) | 0x10000000)
-#define DIDmib_dot11mac_dot11CountersTable_dot11MulticastReceivedFrameCount \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(11) | 0x10000000)
-#define DIDmib_dot11mac_dot11CountersTable_dot11FCSErrorCount \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(12) | 0x10000000)
-#define DIDmib_dot11mac_dot11CountersTable_dot11TransmittedFrameCount \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(13) | 0x10000000)
-#define DIDmib_dot11mac_dot11CountersTable_dot11WEPUndecryptableCount \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(14) | 0x10000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3))
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address1 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(1) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address2 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(2) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address3 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(3) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address4 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(4) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address5 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(5) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address6 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(6) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address7 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(7) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address8 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(8) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address9 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(9) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address10 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(10) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address11 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(11) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address12 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(12) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address13 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(13) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address14 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(14) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address15 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(15) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address16 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(16) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address17 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(17) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address18 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(18) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address19 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(19) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address20 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(20) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address21 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(21) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address22 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(22) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address23 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(23) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address24 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(24) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address25 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(25) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address26 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(26) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address27 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(27) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address28 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(28) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address29 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(29) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address30 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(30) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address31 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(31) | 0x1c000000)
-#define DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address32 \
-			(P80211DID_MKSECTION(2) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(32) | 0x1c000000)
+                       (P80211DID_MKSECTION(2) | \
+                       P80211DID_MKGROUP(1) | \
+                       P80211DID_MKITEM(6) | 0x10000000)
 #define DIDmib_cat_dot11phy \
 			P80211DID_MKSECTION(3)
 #define DIDmib_dot11phy_dot11PhyOperationTable \
 			(P80211DID_MKSECTION(3) | \
 			P80211DID_MKGROUP(1))
-#define DIDmib_dot11phy_dot11PhyOperationTable_dot11PHYType \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(1) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyOperationTable_dot11CurrentRegDomain \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(2) | 0x18000000)
-#define DIDmib_dot11phy_dot11PhyOperationTable_dot11TempType \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(3) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyOperationTable_dot11ChannelAgilityPresent \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(4) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyOperationTable_dot11ChannelAgilityEnabled \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(5) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyOperationTable_dot11ShortPreambleEnabled \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(6) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyAntennaTable \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(2))
-#define DIDmib_dot11phy_dot11PhyAntennaTable_dot11CurrentTxAntenna \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(1) | 0x18000000)
-#define DIDmib_dot11phy_dot11PhyAntennaTable_dot11DiversitySupport \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(2) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyAntennaTable_dot11CurrentRxAntenna \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(3) | 0x18000000)
-#define DIDmib_dot11phy_dot11PhyTxPowerTable \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(3))
-#define DIDmib_dot11phy_dot11PhyTxPowerTable_dot11NumberSupportedPowerLevels \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(1) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel1 \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(2) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel2 \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(3) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel3 \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(4) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel4 \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(5) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel5 \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(6) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel6 \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(7) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel7 \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(8) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel8 \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(9) | 0x10000000)
 #define DIDmib_dot11phy_dot11PhyTxPowerTable_dot11CurrentTxPowerLevel \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(10) | 0x18000000)
-#define DIDmib_dot11phy_dot11PhyFHSSTable \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(4))
-#define DIDmib_dot11phy_dot11PhyFHSSTable_dot11HopTime \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(4) | \
-			P80211DID_MKITEM(1) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyFHSSTable_dot11CurrentChannelNumber \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(4) | \
-			P80211DID_MKITEM(2) | 0x18000000)
-#define DIDmib_dot11phy_dot11PhyFHSSTable_dot11MaxDwellTime \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(4) | \
-			P80211DID_MKITEM(3) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyFHSSTable_dot11CurrentDwellTime \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(4) | \
-			P80211DID_MKITEM(4) | 0x18000000)
-#define DIDmib_dot11phy_dot11PhyFHSSTable_dot11CurrentSet \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(4) | \
-			P80211DID_MKITEM(5) | 0x18000000)
-#define DIDmib_dot11phy_dot11PhyFHSSTable_dot11CurrentPattern \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(4) | \
-			P80211DID_MKITEM(6) | 0x18000000)
-#define DIDmib_dot11phy_dot11PhyFHSSTable_dot11CurrentIndex \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(4) | \
-			P80211DID_MKITEM(7) | 0x18000000)
+                       (P80211DID_MKSECTION(3) | \
+                       P80211DID_MKGROUP(3) | \
+                       P80211DID_MKITEM(10) | 0x18000000)
 #define DIDmib_dot11phy_dot11PhyDSSSTable \
 			(P80211DID_MKSECTION(3) | \
 			P80211DID_MKGROUP(5))
@@ -1806,97 +729,6 @@
 			(P80211DID_MKSECTION(3) | \
 			P80211DID_MKGROUP(5) | \
 			P80211DID_MKITEM(1) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyDSSSTable_dot11CCAModeSupported \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(2) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyDSSSTable_dot11CurrentCCAMode \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(3) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyDSSSTable_dot11EDThreshold \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(4) | 0x18000000)
-#define DIDmib_dot11phy_dot11PhyDSSSTable_dot11ShortPreambleOptionImplemented \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(5) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyDSSSTable_dot11PBCCOptionImplemented \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(6) | 0x10000000)
-#define DIDmib_dot11phy_dot11PhyIRTable \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(6))
-#define DIDmib_dot11phy_dot11PhyIRTable_dot11CCAWatchdogTimerMax \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(1) | 0x18000000)
-#define DIDmib_dot11phy_dot11PhyIRTable_dot11CCAWatchdogCountMax \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(2) | 0x18000000)
-#define DIDmib_dot11phy_dot11PhyIRTable_dot11CCAWatchdogTimerMin \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(3) | 0x18000000)
-#define DIDmib_dot11phy_dot11PhyIRTable_dot11CCAWatchdogCountMin \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(4) | 0x18000000)
-#define DIDmib_dot11phy_dot11RegDomainsSupportedTable \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(7))
-#define DIDmib_dot11phy_dot11RegDomainsSupportedTable_dot11RegDomainsSupportIndex \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(1) | 0x1c000000)
-#define DIDmib_dot11phy_dot11RegDomainsSupportedTable_dot11RegDomainsSupportValue \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(2) | 0x14000000)
-#define DIDmib_dot11phy_dot11AntennasListTable \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(8))
-#define DIDmib_dot11phy_dot11AntennasListTable_dot11AntennaListIndex \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(8) | \
-			P80211DID_MKITEM(1) | 0x1c000000)
-#define DIDmib_dot11phy_dot11AntennasListTable_dot11SupportedTxAntenna \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(8) | \
-			P80211DID_MKITEM(2) | 0x1c000000)
-#define DIDmib_dot11phy_dot11AntennasListTable_dot11SupportedRxAntenna \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(8) | \
-			P80211DID_MKITEM(3) | 0x1c000000)
-#define DIDmib_dot11phy_dot11AntennasListTable_dot11DiversitySelectionRx \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(8) | \
-			P80211DID_MKITEM(4) | 0x1c000000)
-#define DIDmib_dot11phy_dot11SupportedDataRatesTxTable \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(9))
-#define DIDmib_dot11phy_dot11SupportedDataRatesTxTable_dot11SupportedDataRatesTxIndex \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(9) | \
-			P80211DID_MKITEM(1) | 0x1c000000)
-#define DIDmib_dot11phy_dot11SupportedDataRatesTxTable_dot11SupportedDataRatesTxValue \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(9) | \
-			P80211DID_MKITEM(2) | 0x14000000)
-#define DIDmib_dot11phy_dot11SupportedDataRatesRxTable \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(10))
-#define DIDmib_dot11phy_dot11SupportedDataRatesRxTable_dot11SupportedDataRatesRxIndex \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(10) | \
-			P80211DID_MKITEM(1) | 0x1c000000)
-#define DIDmib_dot11phy_dot11SupportedDataRatesRxTable_dot11SupportedDataRatesRxValue \
-			(P80211DID_MKSECTION(3) | \
-			P80211DID_MKGROUP(10) | \
-			P80211DID_MKITEM(2) | 0x14000000)
 #define DIDmib_cat_lnx \
 			P80211DID_MKSECTION(4)
 #define DIDmib_lnx_lnxConfigTable \
@@ -1908,57 +740,6 @@
 			P80211DID_MKITEM(1) | 0x18000000)
 #define DIDmib_cat_p2 \
 			P80211DID_MKSECTION(5)
-#define DIDmib_p2_p2Table \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1))
-#define DIDmib_p2_p2Table_p2MMTx \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(1) | 0x18000000)
-#define DIDmib_p2_p2Table_p2EarlyBeacon \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(2) | 0x18000000)
-#define DIDmib_p2_p2Table_p2ReceivedFrameStatistics \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(3) | 0x10000000)
-#define DIDmib_p2_p2Table_p2CommunicationTallies \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(4) | 0x10000000)
-#define DIDmib_p2_p2Table_p2Authenticated \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(5) | 0x10000000)
-#define DIDmib_p2_p2Table_p2Associated \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(6) | 0x10000000)
-#define DIDmib_p2_p2Table_p2PowerSaveUserCount \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(7) | 0x10000000)
-#define DIDmib_p2_p2Table_p2Comment \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(8) | 0x18000000)
-#define DIDmib_p2_p2Table_p2AccessMode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(9) | 0x18000000)
-#define DIDmib_p2_p2Table_p2AccessAllow \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(10) | 0x18000000)
-#define DIDmib_p2_p2Table_p2AccessDeny \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(11) | 0x18000000)
-#define DIDmib_p2_p2Table_p2ChannelInfoResults \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(1) | \
-			P80211DID_MKITEM(12) | 0x10000000)
 #define DIDmib_p2_p2Static \
 			(P80211DID_MKSECTION(5) | \
 			P80211DID_MKGROUP(2))
@@ -1966,559 +747,11 @@
 			(P80211DID_MKSECTION(5) | \
 			P80211DID_MKGROUP(2) | \
 			P80211DID_MKITEM(1) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfOwnMACAddress \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(2) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfDesiredSSID \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(3) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfOwnChannel \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(4) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfOwnSSID \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(5) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfOwnATIMWindow \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(6) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfSystemScale \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(7) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfMaxDataLength \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(8) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfWDSAddress \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(9) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfPMEnabled \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(10) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfPMEPS \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(11) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfMulticastReceive \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(12) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfMaxSleepDuration \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(13) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfPMHoldoverDuration \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(14) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfOwnName \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(15) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfOwnDTIMPeriod \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(16) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfWDSAddress1 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(17) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfWDSAddress2 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(18) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfWDSAddress3 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(19) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfWDSAddress4 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(20) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfWDSAddress5 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(21) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfWDSAddress6 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(22) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfMulticastPMBuffering \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(23) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfWEPDefaultKeyID \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(24) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfWEPDefaultKey0 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(25) | 0x08000000)
-#define DIDmib_p2_p2Static_p2CnfWEPDefaultKey1 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(26) | 0x08000000)
-#define DIDmib_p2_p2Static_p2CnfWEPDefaultKey2 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(27) | 0x08000000)
-#define DIDmib_p2_p2Static_p2CnfWEPDefaultKey3 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(28) | 0x08000000)
-#define DIDmib_p2_p2Static_p2CnfWEPFlags \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(29) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfAuthentication \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(30) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfMaxAssociatedStations \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(31) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfTxControl \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(32) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfRoamingMode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(33) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfHostAuthentication \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(34) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfRcvCrcError \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(35) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfAltRetryCount \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(36) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfBeaconInterval \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(37) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfMediumOccupancyLimit \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(38) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfCFPPeriod \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(39) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfCFPMaxDuration \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(40) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfCFPFlags \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(41) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfSTAPCFInfo \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(42) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfPriorityQUsage \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(43) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfTIMCtrl \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(44) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfThirty2Tally \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(45) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfEnhSecurity \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(46) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfShortPreamble \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(47) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfExcludeLongPreamble \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(48) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfAuthenticationRspTO \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(49) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfBasicRates \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(50) | 0x18000000)
-#define DIDmib_p2_p2Static_p2CnfSupportedRates \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(2) | \
-			P80211DID_MKITEM(51) | 0x18000000)
-#define DIDmib_p2_p2Dynamic \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3))
-#define DIDmib_p2_p2Dynamic_p2CreateIBSS \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(1) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2FragmentationThreshold \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(2) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2RTSThreshold \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(3) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2TxRateControl \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(4) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2PromiscuousMode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(5) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2FragmentationThreshold0 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(6) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2FragmentationThreshold1 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(7) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2FragmentationThreshold2 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(8) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2FragmentationThreshold3 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(9) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2FragmentationThreshold4 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(10) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2FragmentationThreshold5 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(11) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2FragmentationThreshold6 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(12) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2RTSThreshold0 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(13) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2RTSThreshold1 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(14) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2RTSThreshold2 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(15) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2RTSThreshold3 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(16) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2RTSThreshold4 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(17) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2RTSThreshold5 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(18) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2RTSThreshold6 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(19) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2TxRateControl0 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(20) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2TxRateControl1 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(21) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2TxRateControl2 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(22) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2TxRateControl3 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(23) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2TxRateControl4 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(24) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2TxRateControl5 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(25) | 0x18000000)
-#define DIDmib_p2_p2Dynamic_p2TxRateControl6 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(3) | \
-			P80211DID_MKITEM(26) | 0x18000000)
-#define DIDmib_p2_p2Behavior \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(4))
-#define DIDmib_p2_p2Behavior_p2TickTime \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(4) | \
-			P80211DID_MKITEM(1) | 0x18000000)
-#define DIDmib_p2_p2NIC \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5))
-#define DIDmib_p2_p2NIC_p2MaxLoadTime \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(1) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2DLBufferPage \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(2) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2DLBufferOffset \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(3) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2DLBufferLength \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(4) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2PRIIdentity \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(5) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2PRISupRange \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(6) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2CFIActRanges \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(7) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2NICSerialNumber \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(8) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2NICIdentity \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(9) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2MFISupRange \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(10) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2CFISupRange \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(11) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2ChannelList \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(12) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2RegulatoryDomains \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(13) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2TempType \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(14) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2STAIdentity \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(15) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2STASupRange \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(16) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2MFIActRanges \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(17) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2STACFIActRanges \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(18) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2BuildSequence \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(19) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2PrimaryFWID \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(20) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2SecondaryFWID \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(21) | 0x10000000)
-#define DIDmib_p2_p2NIC_p2TertiaryFWID \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(5) | \
-			P80211DID_MKITEM(22) | 0x10000000)
 #define DIDmib_p2_p2MAC \
 			(P80211DID_MKSECTION(5) | \
 			P80211DID_MKGROUP(6))
-#define DIDmib_p2_p2MAC_p2PortStatus \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(1) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2CurrentSSID \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(2) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2CurrentBSSID \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(3) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2CommsQuality \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(4) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2CommsQualityCQ \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(5) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2CommsQualityASL \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(6) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2CommsQualityANL \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(7) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2dbmCommsQuality \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(8) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2dbmCommsQualityCQ \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(9) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2dbmCommsQualityASL \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(10) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2dbmCommsQualityANL \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(11) | 0x10000000)
 #define DIDmib_p2_p2MAC_p2CurrentTxRate \
 			(P80211DID_MKSECTION(5) | \
 			P80211DID_MKGROUP(6) | \
 			P80211DID_MKITEM(12) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2CurrentBeaconInterval \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(13) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2StaCurrentScaleThresholds \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(14) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2APCurrentScaleThresholds \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(15) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2ProtocolRspTime \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(16) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2ShortRetryLimit \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(17) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2LongRetryLimit \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(18) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2MaxTransmitLifetime \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(19) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2MaxReceiveLifetime \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(20) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2CFPollable \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(21) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2AuthenticationAlgorithms \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(22) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2PrivacyOptionImplemented \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(23) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2CurrentTxRate1 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(24) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2CurrentTxRate2 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(25) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2CurrentTxRate3 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(26) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2CurrentTxRate4 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(27) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2CurrentTxRate5 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(28) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2CurrentTxRate6 \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(29) | 0x10000000)
-#define DIDmib_p2_p2MAC_p2OwnMACAddress \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(6) | \
-			P80211DID_MKITEM(30) | 0x10000000)
-#define DIDmib_p2_p2Modem \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(7))
-#define DIDmib_p2_p2Modem_p2PHYType \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(1) | 0x10000000)
-#define DIDmib_p2_p2Modem_p2CurrentChannel \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(2) | 0x10000000)
-#define DIDmib_p2_p2Modem_p2CurrentPowerState \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(3) | 0x10000000)
-#define DIDmib_p2_p2Modem_p2CCAMode \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(4) | 0x10000000)
-#define DIDmib_p2_p2Modem_p2SupportedDataRates \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(5) | 0x10000000)
-#define DIDmib_p2_p2Modem_p2TxPowerMax \
-			(P80211DID_MKSECTION(5) | \
-			P80211DID_MKGROUP(7) | \
-			P80211DID_MKITEM(6) | 0x18000000)
 #endif
diff --git a/drivers/staging/wlan-ng/p80211metamib.h b/drivers/staging/wlan-ng/p80211metamib.h
index 19867fd314e..9cd72ede441 100644
--- a/drivers/staging/wlan-ng/p80211metamib.h
+++ b/drivers/staging/wlan-ng/p80211metamib.h
@@ -93,7 +93,7 @@
 /* category metadata list */
 
 extern catlistitem_t mib_catlist[];
-extern UINT32 mib_catlist_size;
+extern u32 mib_catlist_size;
 
 
 /*================================================================*/
diff --git a/drivers/staging/wlan-ng/p80211metamsg.h b/drivers/staging/wlan-ng/p80211metamsg.h
index 4d6dfcc79b8..6e659eae8af 100644
--- a/drivers/staging/wlan-ng/p80211metamsg.h
+++ b/drivers/staging/wlan-ng/p80211metamsg.h
@@ -93,7 +93,7 @@
 /* category metadata list */
 
 extern catlistitem_t msg_catlist[];
-extern UINT32 msg_catlist_size;
+extern u32 msg_catlist_size;
 
 
 /*================================================================*/
diff --git a/drivers/staging/wlan-ng/p80211metastruct.h b/drivers/staging/wlan-ng/p80211metastruct.h
index 715f4b2adc6..d2258b0e89c 100644
--- a/drivers/staging/wlan-ng/p80211metastruct.h
+++ b/drivers/staging/wlan-ng/p80211metastruct.h
@@ -50,47 +50,36 @@
 
 typedef struct p80211msg_dot11req_mibget
 {
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
+	u32		msgcode	;
+	u32		msglen	;
+	u8		devname[WLAN_DEVNAMELEN_MAX]	;
 	p80211item_unk392_t	mibattribute	;
 	p80211item_uint32_t	resultcode	;
 } __WLAN_ATTRIB_PACK__ p80211msg_dot11req_mibget_t;
 
 typedef struct p80211msg_dot11req_mibset
 {
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
+	u32		msgcode	;
+	u32		msglen	;
+	u8		devname[WLAN_DEVNAMELEN_MAX]	;
 	p80211item_unk392_t	mibattribute	;
 	p80211item_uint32_t	resultcode	;
 } __WLAN_ATTRIB_PACK__ p80211msg_dot11req_mibset_t;
 
-typedef struct p80211msg_dot11req_powermgmt
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_uint32_t	powermgmtmode	;
-	p80211item_uint32_t	wakeup	;
-	p80211item_uint32_t	receivedtims	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_dot11req_powermgmt_t;
-
 typedef struct p80211msg_dot11req_scan
 {
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
+	u32		msgcode	;
+	u32		msglen	;
+	u8		devname[WLAN_DEVNAMELEN_MAX]	;
 	p80211item_uint32_t	bsstype	;
 	p80211item_pstr6_t	bssid	;
-	UINT8	pad_0C[1]	;
+	u8	pad_0C[1]	;
 	p80211item_pstr32_t	ssid	;
-	UINT8	pad_1D[3]	;
+	u8	pad_1D[3]	;
 	p80211item_uint32_t	scantype	;
 	p80211item_uint32_t	probedelay	;
 	p80211item_pstr14_t	channellist	;
-	UINT8	pad_2C[1]	;
+	u8	pad_2C[1]	;
 	p80211item_uint32_t	minchanneltime	;
 	p80211item_uint32_t	maxchanneltime	;
 	p80211item_uint32_t	resultcode	;
@@ -100,17 +89,17 @@ typedef struct p80211msg_dot11req_scan
 
 typedef struct p80211msg_dot11req_scan_results
 {
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
+	u32		msgcode	;
+	u32		msglen	;
+	u8		devname[WLAN_DEVNAMELEN_MAX]	;
 	p80211item_uint32_t	bssindex	;
 	p80211item_uint32_t	resultcode	;
 	p80211item_uint32_t	signal	;
 	p80211item_uint32_t	noise	;
 	p80211item_pstr6_t	bssid	;
-	UINT8	pad_3C[1]	;
+	u8	pad_3C[1]	;
 	p80211item_pstr32_t	ssid	;
-	UINT8	pad_4D[3]	;
+	u8	pad_4D[3]	;
 	p80211item_uint32_t	bsstype	;
 	p80211item_uint32_t	beaconperiod	;
 	p80211item_uint32_t	dtimperiod	;
@@ -147,115 +136,13 @@ typedef struct p80211msg_dot11req_scan_results
 	p80211item_uint32_t	supprate8	;
 } __WLAN_ATTRIB_PACK__ p80211msg_dot11req_scan_results_t;
 
-typedef struct p80211msg_dot11req_join
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_pstr6_t	bssid	;
-	UINT8	pad_5C[1]	;
-	p80211item_uint32_t	joinfailuretimeout	;
-	p80211item_uint32_t	basicrate1	;
-	p80211item_uint32_t	basicrate2	;
-	p80211item_uint32_t	basicrate3	;
-	p80211item_uint32_t	basicrate4	;
-	p80211item_uint32_t	basicrate5	;
-	p80211item_uint32_t	basicrate6	;
-	p80211item_uint32_t	basicrate7	;
-	p80211item_uint32_t	basicrate8	;
-	p80211item_uint32_t	operationalrate1	;
-	p80211item_uint32_t	operationalrate2	;
-	p80211item_uint32_t	operationalrate3	;
-	p80211item_uint32_t	operationalrate4	;
-	p80211item_uint32_t	operationalrate5	;
-	p80211item_uint32_t	operationalrate6	;
-	p80211item_uint32_t	operationalrate7	;
-	p80211item_uint32_t	operationalrate8	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_dot11req_join_t;
-
-typedef struct p80211msg_dot11req_authenticate
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_pstr6_t	peerstaaddress	;
-	UINT8	pad_6C[1]	;
-	p80211item_uint32_t	authenticationtype	;
-	p80211item_uint32_t	authenticationfailuretimeout	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_dot11req_authenticate_t;
-
-typedef struct p80211msg_dot11req_deauthenticate
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_pstr6_t	peerstaaddress	;
-	UINT8	pad_7C[1]	;
-	p80211item_uint32_t	reasoncode	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_dot11req_deauthenticate_t;
-
-typedef struct p80211msg_dot11req_associate
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_pstr6_t	peerstaaddress	;
-	UINT8	pad_8C[1]	;
-	p80211item_uint32_t	associatefailuretimeout	;
-	p80211item_uint32_t	cfpollable	;
-	p80211item_uint32_t	cfpollreq	;
-	p80211item_uint32_t	privacy	;
-	p80211item_uint32_t	listeninterval	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_dot11req_associate_t;
-
-typedef struct p80211msg_dot11req_reassociate
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_pstr6_t	newapaddress	;
-	UINT8	pad_9C[1]	;
-	p80211item_uint32_t	reassociatefailuretimeout	;
-	p80211item_uint32_t	cfpollable	;
-	p80211item_uint32_t	cfpollreq	;
-	p80211item_uint32_t	privacy	;
-	p80211item_uint32_t	listeninterval	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_dot11req_reassociate_t;
-
-typedef struct p80211msg_dot11req_disassociate
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_pstr6_t	peerstaaddress	;
-	UINT8	pad_10C[1]	;
-	p80211item_uint32_t	reasoncode	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_dot11req_disassociate_t;
-
-typedef struct p80211msg_dot11req_reset
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_uint32_t	setdefaultmib	;
-	p80211item_pstr6_t	macaddress	;
-	UINT8	pad_11C[1]	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_dot11req_reset_t;
-
 typedef struct p80211msg_dot11req_start
 {
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
+	u32		msgcode	;
+	u32		msglen	;
+	u8		devname[WLAN_DEVNAMELEN_MAX]	;
 	p80211item_pstr32_t	ssid	;
-	UINT8	pad_12D[3]	;
+	u8	pad_12D[3]	;
 	p80211item_uint32_t	bsstype	;
 	p80211item_uint32_t	beaconperiod	;
 	p80211item_uint32_t	dtimperiod	;
@@ -288,72 +175,20 @@ typedef struct p80211msg_dot11req_start
 	p80211item_uint32_t	resultcode	;
 } __WLAN_ATTRIB_PACK__ p80211msg_dot11req_start_t;
 
-typedef struct p80211msg_dot11ind_authenticate
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_pstr6_t	peerstaaddress	;
-	UINT8	pad_13C[1]	;
-	p80211item_uint32_t	authenticationtype	;
-} __WLAN_ATTRIB_PACK__ p80211msg_dot11ind_authenticate_t;
-
-typedef struct p80211msg_dot11ind_deauthenticate
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_pstr6_t	peerstaaddress	;
-	UINT8	pad_14C[1]	;
-	p80211item_uint32_t	reasoncode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_dot11ind_deauthenticate_t;
-
-typedef struct p80211msg_dot11ind_associate
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_pstr6_t	peerstaaddress	;
-	UINT8	pad_15C[1]	;
-	p80211item_uint32_t	aid	;
-} __WLAN_ATTRIB_PACK__ p80211msg_dot11ind_associate_t;
-
-typedef struct p80211msg_dot11ind_reassociate
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_pstr6_t	peerstaaddress	;
-	UINT8	pad_16C[1]	;
-	p80211item_uint32_t	aid	;
-	p80211item_pstr6_t	oldapaddress	;
-	UINT8	pad_17C[1]	;
-} __WLAN_ATTRIB_PACK__ p80211msg_dot11ind_reassociate_t;
-
-typedef struct p80211msg_dot11ind_disassociate
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_pstr6_t	peerstaaddress	;
-	UINT8	pad_18C[1]	;
-	p80211item_uint32_t	reasoncode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_dot11ind_disassociate_t;
-
 typedef struct p80211msg_lnxreq_ifstate
 {
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
+	u32		msgcode	;
+	u32		msglen	;
+	u8		devname[WLAN_DEVNAMELEN_MAX]	;
 	p80211item_uint32_t	ifstate	;
 	p80211item_uint32_t	resultcode	;
 } __WLAN_ATTRIB_PACK__ p80211msg_lnxreq_ifstate_t;
 
 typedef struct p80211msg_lnxreq_wlansniff
 {
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
+	u32		msgcode	;
+	u32		msglen	;
+	u8		devname[WLAN_DEVNAMELEN_MAX]	;
 	p80211item_uint32_t	enable	;
 	p80211item_uint32_t	channel	;
 	p80211item_uint32_t	prismheader	;
@@ -366,9 +201,9 @@ typedef struct p80211msg_lnxreq_wlansniff
 
 typedef struct p80211msg_lnxreq_hostwep
 {
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
+	u32		msgcode	;
+	u32		msglen	;
+	u8		devname[WLAN_DEVNAMELEN_MAX]	;
 	p80211item_uint32_t	resultcode	;
 	p80211item_uint32_t	decrypt	;
 	p80211item_uint32_t	encrypt	;
@@ -376,9 +211,9 @@ typedef struct p80211msg_lnxreq_hostwep
 
 typedef struct p80211msg_lnxreq_commsquality
 {
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
+	u32		msgcode	;
+	u32		msglen	;
+	u8		devname[WLAN_DEVNAMELEN_MAX]	;
 	p80211item_uint32_t	resultcode	;
 	p80211item_uint32_t	dbm	;
 	p80211item_uint32_t	link	;
@@ -388,173 +223,29 @@ typedef struct p80211msg_lnxreq_commsquality
 
 typedef struct p80211msg_lnxreq_autojoin
 {
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
+	u32		msgcode	;
+	u32		msglen	;
+	u8		devname[WLAN_DEVNAMELEN_MAX]	;
 	p80211item_pstr32_t	ssid	;
-	UINT8	pad_19D[3]	;
+	u8	pad_19D[3]	;
 	p80211item_uint32_t	authtype	;
 	p80211item_uint32_t	resultcode	;
 } __WLAN_ATTRIB_PACK__ p80211msg_lnxreq_autojoin_t;
 
-typedef struct p80211msg_lnxind_wlansniffrm
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_uint32_t	hosttime	;
-	p80211item_uint32_t	mactime	;
-	p80211item_uint32_t	channel	;
-	p80211item_uint32_t	rssi	;
-	p80211item_uint32_t	sq	;
-	p80211item_uint32_t	signal	;
-	p80211item_uint32_t	noise	;
-	p80211item_uint32_t	rate	;
-	p80211item_uint32_t	istx	;
-	p80211item_uint32_t	frmlen	;
-} __WLAN_ATTRIB_PACK__ p80211msg_lnxind_wlansniffrm_t;
-
-typedef struct p80211msg_lnxind_roam
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_uint32_t	reason	;
-} __WLAN_ATTRIB_PACK__ p80211msg_lnxind_roam_t;
-
-typedef struct p80211msg_p2req_join
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_pstr6_t	bssid	;
-	UINT8	pad_20C[1]	;
-	p80211item_uint32_t	basicrate1	;
-	p80211item_uint32_t	basicrate2	;
-	p80211item_uint32_t	basicrate3	;
-	p80211item_uint32_t	basicrate4	;
-	p80211item_uint32_t	basicrate5	;
-	p80211item_uint32_t	basicrate6	;
-	p80211item_uint32_t	basicrate7	;
-	p80211item_uint32_t	basicrate8	;
-	p80211item_uint32_t	operationalrate1	;
-	p80211item_uint32_t	operationalrate2	;
-	p80211item_uint32_t	operationalrate3	;
-	p80211item_uint32_t	operationalrate4	;
-	p80211item_uint32_t	operationalrate5	;
-	p80211item_uint32_t	operationalrate6	;
-	p80211item_uint32_t	operationalrate7	;
-	p80211item_uint32_t	operationalrate8	;
-	p80211item_pstr32_t	ssid	;
-	UINT8	pad_21D[3]	;
-	p80211item_uint32_t	channel	;
-	p80211item_uint32_t	authtype	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_p2req_join_t;
-
 typedef struct p80211msg_p2req_readpda
 {
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
+	u32		msgcode	;
+	u32		msglen	;
+	u8		devname[WLAN_DEVNAMELEN_MAX]	;
 	p80211item_unk1024_t	pda	;
 	p80211item_uint32_t	resultcode	;
 } __WLAN_ATTRIB_PACK__ p80211msg_p2req_readpda_t;
 
-typedef struct p80211msg_p2req_readcis
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_unk1024_t	cis	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_p2req_readcis_t;
-
-typedef struct p80211msg_p2req_auxport_state
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_uint32_t	enable	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_p2req_auxport_state_t;
-
-typedef struct p80211msg_p2req_auxport_read
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_uint32_t	addr	;
-	p80211item_uint32_t	len	;
-	p80211item_unk1024_t	data	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_p2req_auxport_read_t;
-
-typedef struct p80211msg_p2req_auxport_write
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_uint32_t	addr	;
-	p80211item_uint32_t	len	;
-	p80211item_unk1024_t	data	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_p2req_auxport_write_t;
-
-typedef struct p80211msg_p2req_low_level
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_uint32_t	command	;
-	p80211item_uint32_t	param0	;
-	p80211item_uint32_t	param1	;
-	p80211item_uint32_t	param2	;
-	p80211item_uint32_t	resp0	;
-	p80211item_uint32_t	resp1	;
-	p80211item_uint32_t	resp2	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_p2req_low_level_t;
-
-typedef struct p80211msg_p2req_test_command
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_uint32_t	testcode	;
-	p80211item_uint32_t	testparam	;
-	p80211item_uint32_t	resultcode	;
-	p80211item_uint32_t	status	;
-	p80211item_uint32_t	resp0	;
-	p80211item_uint32_t	resp1	;
-	p80211item_uint32_t	resp2	;
-} __WLAN_ATTRIB_PACK__ p80211msg_p2req_test_command_t;
-
-typedef struct p80211msg_p2req_mmi_read
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_uint32_t	addr	;
-	p80211item_uint32_t	value	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_p2req_mmi_read_t;
-
-typedef struct p80211msg_p2req_mmi_write
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_uint32_t	addr	;
-	p80211item_uint32_t	data	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_p2req_mmi_write_t;
-
 typedef struct p80211msg_p2req_ramdl_state
 {
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
+	u32		msgcode	;
+	u32		msglen	;
+	u8		devname[WLAN_DEVNAMELEN_MAX]	;
 	p80211item_uint32_t	enable	;
 	p80211item_uint32_t	exeaddr	;
 	p80211item_uint32_t	resultcode	;
@@ -562,9 +253,9 @@ typedef struct p80211msg_p2req_ramdl_state
 
 typedef struct p80211msg_p2req_ramdl_write
 {
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
+	u32		msgcode	;
+	u32		msglen	;
+	u8		devname[WLAN_DEVNAMELEN_MAX]	;
 	p80211item_uint32_t	addr	;
 	p80211item_uint32_t	len	;
 	p80211item_unk4096_t	data	;
@@ -573,72 +264,22 @@ typedef struct p80211msg_p2req_ramdl_write
 
 typedef struct p80211msg_p2req_flashdl_state
 {
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
+	u32		msgcode	;
+	u32		msglen	;
+	u8		devname[WLAN_DEVNAMELEN_MAX]	;
 	p80211item_uint32_t	enable	;
 	p80211item_uint32_t	resultcode	;
 } __WLAN_ATTRIB_PACK__ p80211msg_p2req_flashdl_state_t;
 
 typedef struct p80211msg_p2req_flashdl_write
 {
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
+	u32		msgcode	;
+	u32		msglen	;
+	u8		devname[WLAN_DEVNAMELEN_MAX]	;
 	p80211item_uint32_t	addr	;
 	p80211item_uint32_t	len	;
 	p80211item_unk4096_t	data	;
 	p80211item_uint32_t	resultcode	;
 } __WLAN_ATTRIB_PACK__ p80211msg_p2req_flashdl_write_t;
 
-typedef struct p80211msg_p2req_mm_state
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_uint32_t	enable	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_p2req_mm_state_t;
-
-typedef struct p80211msg_p2req_dump_state
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_uint32_t	level	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_p2req_dump_state_t;
-
-typedef struct p80211msg_p2req_channel_info
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_uint32_t	channellist	;
-	p80211item_uint32_t	channeldwelltime	;
-	p80211item_uint32_t	resultcode	;
-	p80211item_uint32_t	numchinfo	;
-} __WLAN_ATTRIB_PACK__ p80211msg_p2req_channel_info_t;
-
-typedef struct p80211msg_p2req_channel_info_results
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_uint32_t	channel	;
-	p80211item_uint32_t	resultcode	;
-	p80211item_uint32_t	avgnoiselevel	;
-	p80211item_uint32_t	peaknoiselevel	;
-	p80211item_uint32_t	bssactive	;
-	p80211item_uint32_t	pcfactive	;
-} __WLAN_ATTRIB_PACK__ p80211msg_p2req_channel_info_results_t;
-
-typedef struct p80211msg_p2req_enable
-{
-	UINT32		msgcode	;
-	UINT32		msglen	;
-	UINT8		devname[WLAN_DEVNAMELEN_MAX]	;
-	p80211item_uint32_t	resultcode	;
-} __WLAN_ATTRIB_PACK__ p80211msg_p2req_enable_t;
-
 #endif
diff --git a/drivers/staging/wlan-ng/p80211mgmt.h b/drivers/staging/wlan-ng/p80211mgmt.h
index bd4c1629eab..6cc16c3f8b1 100644
--- a/drivers/staging/wlan-ng/p80211mgmt.h
+++ b/drivers/staging/wlan-ng/p80211mgmt.h
@@ -172,14 +172,14 @@
 /* Note: These offsets are from the start of the frame data       */
 
 #define WLAN_BEACON_OFF_TS			0
-#define WLAN_BEACON_OFF_BCN_INT			8
+#define WLAN_BEACON_OFF_BCN_int			8
 #define WLAN_BEACON_OFF_CAPINFO			10
 #define WLAN_BEACON_OFF_SSID			12
 
 #define WLAN_DISASSOC_OFF_REASON		0
 
 #define WLAN_ASSOCREQ_OFF_CAP_INFO		0
-#define WLAN_ASSOCREQ_OFF_LISTEN_INT		2
+#define WLAN_ASSOCREQ_OFF_LISTEN_int		2
 #define WLAN_ASSOCREQ_OFF_SSID			4
 
 #define WLAN_ASSOCRESP_OFF_CAP_INFO		0
@@ -188,7 +188,7 @@
 #define WLAN_ASSOCRESP_OFF_SUPP_RATES		6
 
 #define WLAN_REASSOCREQ_OFF_CAP_INFO		0
-#define WLAN_REASSOCREQ_OFF_LISTEN_INT		2
+#define WLAN_REASSOCREQ_OFF_LISTEN_int		2
 #define WLAN_REASSOCREQ_OFF_CURR_AP		4
 #define WLAN_REASSOCREQ_OFF_SSID		10
 
@@ -200,7 +200,7 @@
 #define WLAN_PROBEREQ_OFF_SSID			0
 
 #define WLAN_PROBERESP_OFF_TS			0
-#define WLAN_PROBERESP_OFF_BCN_INT		8
+#define WLAN_PROBERESP_OFF_BCN_int		8
 #define WLAN_PROBERESP_OFF_CAP_INFO		10
 #define WLAN_PROBERESP_OFF_SSID			12
 
@@ -245,82 +245,82 @@
 
 typedef struct wlan_ie
 {
-	UINT8	eid;
-	UINT8	len;
+	u8	eid;
+	u8	len;
 } __WLAN_ATTRIB_PACK__ wlan_ie_t;
 
 /*-- Service Set Identity (SSID)  -----------------*/
 typedef struct wlan_ie_ssid
 {
-	UINT8	eid;
-	UINT8	len;
-	UINT8	ssid[1];  /* may be zero, ptrs may overlap */
+	u8	eid;
+	u8	len;
+	u8	ssid[1];  /* may be zero, ptrs may overlap */
 } __WLAN_ATTRIB_PACK__ wlan_ie_ssid_t;
 
 /*-- Supported Rates  -----------------------------*/
 typedef struct wlan_ie_supp_rates
 {
-	UINT8	eid;
-	UINT8	len;
-	UINT8	rates[1];  /* had better be at LEAST one! */
+	u8	eid;
+	u8	len;
+	u8	rates[1];  /* had better be at LEAST one! */
 } __WLAN_ATTRIB_PACK__ wlan_ie_supp_rates_t;
 
 /*-- FH Parameter Set  ----------------------------*/
 typedef struct wlan_ie_fh_parms
 {
-	UINT8	eid;
-	UINT8	len;
-	UINT16	dwell;
-	UINT8	hopset;
-	UINT8	hoppattern;
-	UINT8	hopindex;
+	u8	eid;
+	u8	len;
+	u16	dwell;
+	u8	hopset;
+	u8	hoppattern;
+	u8	hopindex;
 } __WLAN_ATTRIB_PACK__ wlan_ie_fh_parms_t;
 
 /*-- DS Parameter Set  ----------------------------*/
 typedef struct wlan_ie_ds_parms
 {
-	UINT8	eid;
-	UINT8	len;
-	UINT8	curr_ch;
+	u8	eid;
+	u8	len;
+	u8	curr_ch;
 } __WLAN_ATTRIB_PACK__ wlan_ie_ds_parms_t;
 
 /*-- CF Parameter Set  ----------------------------*/
 
 typedef struct wlan_ie_cf_parms
 {
-	UINT8	eid;
-	UINT8	len;
-	UINT8	cfp_cnt;
-	UINT8	cfp_period;
-	UINT16	cfp_maxdur;
-	UINT16	cfp_durremaining;
+	u8	eid;
+	u8	len;
+	u8	cfp_cnt;
+	u8	cfp_period;
+	u16	cfp_maxdur;
+	u16	cfp_durremaining;
 } __WLAN_ATTRIB_PACK__ wlan_ie_cf_parms_t;
 
 /*-- TIM ------------------------------------------*/
 typedef struct wlan_ie_tim
 {
-	UINT8	eid;
-	UINT8	len;
-	UINT8	dtim_cnt;
-	UINT8	dtim_period;
-	UINT8	bitmap_ctl;
-	UINT8	virt_bm[1];
+	u8	eid;
+	u8	len;
+	u8	dtim_cnt;
+	u8	dtim_period;
+	u8	bitmap_ctl;
+	u8	virt_bm[1];
 } __WLAN_ATTRIB_PACK__ wlan_ie_tim_t;
 
 /*-- IBSS Parameter Set ---------------------------*/
 typedef struct wlan_ie_ibss_parms
 {
-	UINT8	eid;
-	UINT8	len;
-	UINT16	atim_win;
+	u8	eid;
+	u8	len;
+	u16	atim_win;
 } __WLAN_ATTRIB_PACK__ wlan_ie_ibss_parms_t;
 
 /*-- Challenge Text  ------------------------------*/
 typedef struct wlan_ie_challenge
 {
-	UINT8	eid;
-	UINT8	len;
-	UINT8	challenge[1];
+	u8	eid;
+	u8	len;
+	u8	challenge[1];
 } __WLAN_ATTRIB_PACK__ wlan_ie_challenge_t;
 
 /*-------------------------------------------------*/
@@ -329,9 +329,9 @@ typedef struct wlan_ie_challenge
 /* prototype structure, all mgmt frame types will start with these members */
 typedef struct wlan_fr_mgmt
 {
-	UINT16			type;
-	UINT16			len;	/* DOES NOT include CRC !!!!*/
-	UINT8			*buf;
+	u16			type;
+	u16			len;	/* DOES NOT include CRC !!!!*/
+	u8			*buf;
 	p80211_hdr_t		*hdr;
 	/* used for target specific data, skb in Linux */
 	void			*priv;
@@ -342,16 +342,16 @@ typedef struct wlan_fr_mgmt
 /*-- Beacon ---------------------------------------*/
 typedef struct wlan_fr_beacon
 {
-	UINT16			type;
-	UINT16			len;
-	UINT8			*buf;
+	u16			type;
+	u16			len;
+	u8			*buf;
 	p80211_hdr_t		*hdr;
 	/* used for target specific data, skb in Linux */
 	void			*priv;
 	/*-- fixed fields -----------*/
-	UINT64			*ts;
-	UINT16			*bcn_int;
-	UINT16			*cap_info;
+	u64			*ts;
+	u16			*bcn_int;
+	u16			*cap_info;
 	/*-- info elements ----------*/
 	wlan_ie_ssid_t		*ssid;
 	wlan_ie_supp_rates_t	*supp_rates;
@@ -367,9 +367,9 @@ typedef struct wlan_fr_beacon
 /*-- IBSS ATIM ------------------------------------*/
 typedef struct wlan_fr_ibssatim
 {
-	UINT16			type;
-	UINT16			len;
-	UINT8*			buf;
+	u16			type;
+	u16			len;
+	u8*			buf;
 	p80211_hdr_t		*hdr;
 	/* used for target specific data, skb in Linux */
 	void			*priv;
@@ -384,14 +384,14 @@ typedef struct wlan_fr_ibssatim
 /*-- Disassociation -------------------------------*/
 typedef struct wlan_fr_disassoc
 {
-	UINT16			type;
-	UINT16			len;
-	UINT8			*buf;
+	u16			type;
+	u16			len;
+	u8			*buf;
 	p80211_hdr_t		*hdr;
 	/* used for target specific data, skb in Linux */
 	void			*priv;
 	/*-- fixed fields -----------*/
-	UINT16			*reason;
+	u16			*reason;
 
 	/*-- info elements ----------*/
 
@@ -400,15 +400,15 @@ typedef struct wlan_fr_disassoc
 /*-- Association Request --------------------------*/
 typedef struct wlan_fr_assocreq
 {
-	UINT16			type;
-	UINT16			len;
-	UINT8*			buf;
+	u16			type;
+	u16			len;
+	u8*			buf;
 	p80211_hdr_t		*hdr;
 	/* used for target specific data, skb in Linux */
 	void			*priv;
 	/*-- fixed fields -----------*/
-	UINT16			*cap_info;
-	UINT16			*listen_int;
+	u16			*cap_info;
+	u16			*listen_int;
 	/*-- info elements ----------*/
 	wlan_ie_ssid_t		*ssid;
 	wlan_ie_supp_rates_t	*supp_rates;
@@ -418,16 +418,16 @@ typedef struct wlan_fr_assocreq
 /*-- Association Response -------------------------*/
 typedef struct wlan_fr_assocresp
 {
-	UINT16			type;
-	UINT16			len;
-	UINT8			*buf;
+	u16			type;
+	u16			len;
+	u8			*buf;
 	p80211_hdr_t		*hdr;
 	/* used for target specific data, skb in Linux */
 	void			*priv;
 	/*-- fixed fields -----------*/
-	UINT16			*cap_info;
-	UINT16			*status;
-	UINT16			*aid;
+	u16			*cap_info;
+	u16			*status;
+	u16			*aid;
 	/*-- info elements ----------*/
 	wlan_ie_supp_rates_t	*supp_rates;
 
@@ -436,16 +436,16 @@ typedef struct wlan_fr_assocresp
 /*-- Reassociation Request ------------------------*/
 typedef struct wlan_fr_reassocreq
 {
-	UINT16			type;
-	UINT16			len;
-	UINT8			*buf;
+	u16			type;
+	u16			len;
+	u8			*buf;
 	p80211_hdr_t		*hdr;
 	/* used for target specific data, skb in Linux */
 	void			*priv;
 	/*-- fixed fields -----------*/
-	UINT16			*cap_info;
-	UINT16			*listen_int;
-	UINT8			*curr_ap;
+	u16			*cap_info;
+	u16			*listen_int;
+	u8			*curr_ap;
 	/*-- info elements ----------*/
 	wlan_ie_ssid_t		*ssid;
 	wlan_ie_supp_rates_t	*supp_rates;
@@ -455,16 +455,16 @@ typedef struct wlan_fr_reassocreq
 /*-- Reassociation Response -----------------------*/
 typedef struct wlan_fr_reassocresp
 {
-	UINT16			type;
-	UINT16			len;
-	UINT8			*buf;
+	u16			type;
+	u16			len;
+	u8			*buf;
 	p80211_hdr_t		*hdr;
 	/* used for target specific data, skb in Linux */
 	void			*priv;
 	/*-- fixed fields -----------*/
-	UINT16			*cap_info;
-	UINT16			*status;
-	UINT16			*aid;
+	u16			*cap_info;
+	u16			*status;
+	u16			*aid;
 	/*-- info elements ----------*/
 	wlan_ie_supp_rates_t	*supp_rates;
 
@@ -473,9 +473,9 @@ typedef struct wlan_fr_reassocresp
 /*-- Probe Request --------------------------------*/
 typedef struct wlan_fr_probereq
 {
-	UINT16			type;
-	UINT16			len;
-	UINT8			*buf;
+	u16			type;
+	u16			len;
+	u8			*buf;
 	p80211_hdr_t		*hdr;
 	/* used for target specific data, skb in Linux */
 	void			*priv;
@@ -489,16 +489,16 @@ typedef struct wlan_fr_probereq
 /*-- Probe Response -------------------------------*/
 typedef struct wlan_fr_proberesp
 {
-	UINT16			type;
-	UINT16			len;
-	UINT8			*buf;
+	u16			type;
+	u16			len;
+	u8			*buf;
 	p80211_hdr_t		*hdr;
 	/* used for target specific data, skb in Linux */
 	void			*priv;
 	/*-- fixed fields -----------*/
-	UINT64			*ts;
-	UINT16			*bcn_int;
-	UINT16			*cap_info;
+	u64			*ts;
+	u16			*bcn_int;
+	u16			*cap_info;
 	/*-- info elements ----------*/
 	wlan_ie_ssid_t		*ssid;
 	wlan_ie_supp_rates_t	*supp_rates;
@@ -511,16 +511,16 @@ typedef struct wlan_fr_proberesp
 /*-- Authentication -------------------------------*/
 typedef struct wlan_fr_authen
 {
-	UINT16			type;
-	UINT16			len;
-	UINT8			*buf;
+	u16			type;
+	u16			len;
+	u8			*buf;
 	p80211_hdr_t		*hdr;
 	/* used for target specific data, skb in Linux */
 	void			*priv;
 	/*-- fixed fields -----------*/
-	UINT16			*auth_alg;
-	UINT16			*auth_seq;
-	UINT16			*status;
+	u16			*auth_alg;
+	u16			*auth_seq;
+	u16			*status;
 	/*-- info elements ----------*/
 	wlan_ie_challenge_t	*challenge;
 
@@ -529,14 +529,14 @@ typedef struct wlan_fr_authen
 /*-- Deauthenication -----------------------------*/
 typedef struct wlan_fr_deauthen
 {
-	UINT16			type;
-	UINT16			len;
-	UINT8			*buf;
+	u16			type;
+	u16			len;
+	u8			*buf;
 	p80211_hdr_t		*hdr;
 	/* used for target specific data, skb in Linux */
 	void			*priv;
 	/*-- fixed fields -----------*/
-	UINT16			*reason;
+	u16			*reason;
 
 	/*-- info elements ----------*/
 
diff --git a/drivers/staging/wlan-ng/p80211mod.c b/drivers/staging/wlan-ng/p80211mod.c
deleted file mode 100644
index e2c3f63be8b..00000000000
--- a/drivers/staging/wlan-ng/p80211mod.c
+++ /dev/null
@@ -1,216 +0,0 @@
-/* src/p80211/p80211mod.c
-*
-* Module entry and exit for p80211
-*
-* Copyright (C) 1999 AbsoluteValue Systems, Inc.  All Rights Reserved.
-* --------------------------------------------------------------------
-*
-* linux-wlan
-*
-*   The contents of this file are subject to the Mozilla Public
-*   License Version 1.1 (the "License"); you may not use this file
-*   except in compliance with the License. You may obtain a copy of
-*   the License at http://www.mozilla.org/MPL/
-*
-*   Software distributed under the License is distributed on an "AS
-*   IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or
-*   implied. See the License for the specific language governing
-*   rights and limitations under the License.
-*
-*   Alternatively, the contents of this file may be used under the
-*   terms of the GNU Public License version 2 (the "GPL"), in which
-*   case the provisions of the GPL are applicable instead of the
-*   above.  If you wish to allow the use of your version of this file
-*   only under the terms of the GPL and not to allow others to use
-*   your version of this file under the MPL, indicate your decision
-*   by deleting the provisions above and replace them with the notice
-*   and other provisions required by the GPL.  If you do not delete
-*   the provisions above, a recipient may use your version of this
-*   file under either the MPL or the GPL.
-*
-* --------------------------------------------------------------------
-*
-* Inquiries regarding the linux-wlan Open Source project can be
-* made directly to:
-*
-* AbsoluteValue Systems Inc.
-* info@linux-wlan.com
-* http://www.linux-wlan.com
-*
-* --------------------------------------------------------------------
-*
-* Portions of the development of this software were funded by
-* Intersil Corporation as part of PRISM(R) chipset product development.
-*
-* --------------------------------------------------------------------
-*
-* This file contains the p80211.o entry and exit points defined for linux
-* kernel modules.
-*
-* Notes:
-* - all module parameters for  p80211.o should be defined here.
-*
-* --------------------------------------------------------------------
-*/
-
-/*================================================================*/
-/* System Includes */
-
-
-#include <linux/version.h>
-
-#include <linux/module.h>
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,4,25))
-#include <linux/moduleparam.h>
-#endif
-
-#include <linux/kernel.h>
-#include <linux/sched.h>
-#include <linux/types.h>
-#include <linux/wireless.h>
-#include <linux/netdevice.h>
-
-#include "version.h"
-#include "wlan_compat.h"
-
-/*================================================================*/
-/* Project Includes */
-
-#include "p80211types.h"
-#include "p80211hdr.h"
-#include "p80211mgmt.h"
-#include "p80211conv.h"
-#include "p80211msg.h"
-#include "p80211netdev.h"
-#include "p80211req.h"
-
-/*================================================================*/
-/* Local Constants */
-
-
-/*================================================================*/
-/* Local Macros */
-
-
-/*================================================================*/
-/* Local Types */
-
-
-/*================================================================*/
-/* Local Static Definitions */
-
-static char *version = "p80211.o: " WLAN_RELEASE;
-
-
-/*----------------------------------------------------------------*/
-/* --Module Parameters */
-
-int wlan_watchdog = 5000;
-module_param(wlan_watchdog, int, 0644);
-MODULE_PARM_DESC(wlan_watchdog, "transmit timeout in milliseconds");
-
-int wlan_wext_write = 0;
-#if WIRELESS_EXT > 12
-module_param(wlan_wext_write, int, 0644);
-MODULE_PARM_DESC(wlan_wext_write, "enable write wireless extensions");
-#endif
-
-#ifdef WLAN_INCLUDE_DEBUG
-int wlan_debug=0;
-module_param(wlan_debug, int, 0644);
-MODULE_PARM_DESC(wlan_debug, "p80211 debug level");
-#endif
-
-MODULE_LICENSE("Dual MPL/GPL");
-
-/*================================================================*/
-/* Local Function Declarations */
-
-int	init_module(void);
-void	cleanup_module(void);
-
-/*================================================================*/
-/* Function Definitions */
-
-/*----------------------------------------------------------------
-* init_module
-*
-* Module initialization routine, called once at module load time.
-*
-* Arguments:
-*	none
-*
-* Returns:
-*	0	- success
-*	~0	- failure, module is unloaded.
-*
-* Side effects:
-*	TODO: define
-*
-* Call context:
-*	process thread (insmod or modprobe)
-----------------------------------------------------------------*/
-int init_module(void)
-{
-        DBFENTER;
-
-#if 0
-        printk(KERN_NOTICE "%s (%s) Loaded\n", version, WLAN_BUILD_DATE);
-#endif
-
-	p80211netdev_startup();
-#ifdef CONFIG_HOTPLUG
-	p80211_run_sbin_hotplug(NULL, WLAN_HOTPLUG_STARTUP);
-#endif
-
-        DBFEXIT;
-        return 0;
-}
-
-
-/*----------------------------------------------------------------
-* cleanup_module
-*
-* Called at module unload time.  This is our last chance to
-* clean up after ourselves.
-*
-* Arguments:
-*	none
-*
-* Returns:
-*	nothing
-*
-* Side effects:
-*	TODO: define
-*
-* Call context:
-*	process thread
-*
-----------------------------------------------------------------*/
-void cleanup_module(void)
-{
-        DBFENTER;
-
-#ifdef CONFIG_HOTPLUG
-	p80211_run_sbin_hotplug(NULL, WLAN_HOTPLUG_SHUTDOWN);
-#endif
-	p80211netdev_shutdown();
-        printk(KERN_NOTICE "%s Unloaded\n", version);
-
-        DBFEXIT;
-        return;
-}
-
-EXPORT_SYMBOL(p80211netdev_hwremoved);
-EXPORT_SYMBOL(register_wlandev);
-EXPORT_SYMBOL(p80211netdev_rx);
-EXPORT_SYMBOL(unregister_wlandev);
-EXPORT_SYMBOL(wlan_setup);
-EXPORT_SYMBOL(wlan_unsetup);
-EXPORT_SYMBOL(p80211_suspend);
-EXPORT_SYMBOL(p80211_resume);
-
-EXPORT_SYMBOL(p80211skb_free);
-EXPORT_SYMBOL(p80211skb_rxmeta_attach);
-
-EXPORT_SYMBOL(p80211wext_event_associated);
diff --git a/drivers/staging/wlan-ng/p80211msg.h b/drivers/staging/wlan-ng/p80211msg.h
index c14e9fbbd68..3a575d8cc99 100644
--- a/drivers/staging/wlan-ng/p80211msg.h
+++ b/drivers/staging/wlan-ng/p80211msg.h
@@ -78,17 +78,17 @@
 
 typedef struct p80211msg
 {
-	UINT32	msgcode;
-	UINT32	msglen;
-	UINT8	devname[WLAN_DEVNAMELEN_MAX];
+	u32	msgcode;
+	u32	msglen;
+	u8	devname[WLAN_DEVNAMELEN_MAX];
 } __WLAN_ATTRIB_PACK__ p80211msg_t;
 
 typedef struct p80211msgd
 {
-	UINT32	msgcode;
-	UINT32	msglen;
-	UINT8	devname[WLAN_DEVNAMELEN_MAX];
-	UINT8	args[0];
+	u32	msgcode;
+	u32	msglen;
+	u8	devname[WLAN_DEVNAMELEN_MAX];
+	u8	args[0];
 } __WLAN_ATTRIB_PACK__ p80211msgd_t;
 
 /*================================================================*/
diff --git a/drivers/staging/wlan-ng/p80211netdev.c b/drivers/staging/wlan-ng/p80211netdev.c
index 2b705eaa50e..59e5ad10dbd 100644
--- a/drivers/staging/wlan-ng/p80211netdev.c
+++ b/drivers/staging/wlan-ng/p80211netdev.c
@@ -79,15 +79,12 @@
 #include <linux/ethtool.h>
 #endif
 
-#if WIRELESS_EXT > 12
 #include <net/iw_handler.h>
-#endif
 #include <net/net_namespace.h>
 
 /*================================================================*/
 /* Project Includes */
 
-#include "version.h"
 #include "wlan_compat.h"
 #include "p80211types.h"
 #include "p80211hdr.h"
@@ -111,15 +108,6 @@
 /* Local Types */
 
 /*================================================================*/
-/* Local Static Definitions */
-
-#define __NO_VERSION__		/* prevent the static definition */
-
-#ifdef CONFIG_PROC_FS
-static struct proc_dir_entry	*proc_p80211;
-#endif
-
-/*================================================================*/
 /* Local Function Declarations */
 
 /* Support functions */
@@ -135,73 +123,24 @@ static void p80211knetdev_set_multicast_list(netdevice_t *dev);
 static int p80211knetdev_do_ioctl(netdevice_t *dev, struct ifreq *ifr, int cmd);
 static int p80211knetdev_set_mac_address(netdevice_t *dev, void *addr);
 static void p80211knetdev_tx_timeout(netdevice_t *netdev);
-static int p80211_rx_typedrop( wlandevice_t *wlandev, UINT16 fc);
-
-#ifdef CONFIG_PROC_FS
-static int
-p80211netdev_proc_read(
-	char	*page,
-	char	**start,
-	off_t	offset,
-	int	count,
-	int	*eof,
-	void	*data);
-#endif
+static int p80211_rx_typedrop( wlandevice_t *wlandev, u16 fc);
 
-/*================================================================*/
-/* Function Definitions */
+int wlan_watchdog = 5000;
+module_param(wlan_watchdog, int, 0644);
+MODULE_PARM_DESC(wlan_watchdog, "transmit timeout in milliseconds");
 
-/*----------------------------------------------------------------
-* p80211knetdev_startup
-*
-* Initialize the wlandevice/netdevice part of 802.11 services at
-* load time.
-*
-* Arguments:
-*	none
-*
-* Returns:
-*	nothing
-----------------------------------------------------------------*/
-void p80211netdev_startup(void)
-{
-	DBFENTER;
+int wlan_wext_write = 1;
+module_param(wlan_wext_write, int, 0644);
+MODULE_PARM_DESC(wlan_wext_write, "enable write wireless extensions");
 
-#ifdef CONFIG_PROC_FS
-	if (init_net.proc_net != NULL) {
-		proc_p80211 = create_proc_entry(
-				"p80211",
-				(S_IFDIR|S_IRUGO|S_IXUGO),
-				init_net.proc_net);
-	}
+#ifdef WLAN_INCLUDE_DEBUG
+int wlan_debug=0;
+module_param(wlan_debug, int, 0644);
+MODULE_PARM_DESC(wlan_debug, "p80211 debug level");
 #endif
-	DBFEXIT;
-	return;
-}
 
-/*----------------------------------------------------------------
-* p80211knetdev_shutdown
-*
-* Shutdown the wlandevice/netdevice part of 802.11 services at
-* unload time.
-*
-* Arguments:
-*	none
-*
-* Returns:
-*	nothing
-----------------------------------------------------------------*/
-void
-p80211netdev_shutdown(void)
-{
-	DBFENTER;
-#ifdef CONFIG_PROC_FS
-	if (proc_p80211 != NULL) {
-		remove_proc_entry("p80211", init_net.proc_net);
-	}
-#endif
-	DBFEXIT;
-}
+/*================================================================*/
+/* Function Definitions */
 
 /*----------------------------------------------------------------
 * p80211knetdev_init
@@ -285,10 +224,7 @@ static int p80211knetdev_open( netdevice_t *netdev )
 	if ( wlandev->open != NULL) {
 		result = wlandev->open(wlandev);
 		if ( result == 0 ) {
-#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,3,43) )
-			netdev->interrupt = 0;
-#endif
-			p80211netdev_start_queue(wlandev);
+			netif_start_queue(wlandev->netdev);
 			wlandev->state = WLAN_DEVICE_OPEN;
 		}
 	} else {
@@ -323,7 +259,7 @@ static int p80211knetdev_stop( netdevice_t *netdev )
 		result = wlandev->close(wlandev);
 	}
 
-	p80211netdev_stop_queue(wlandev);
+	netif_stop_queue(wlandev->netdev);
 	wlandev->state = WLAN_DEVICE_CLOSED;
 
 	DBFEXIT;
@@ -376,7 +312,7 @@ static void p80211netdev_rx_bh(unsigned long arg)
 	struct sk_buff *skb = NULL;
 	netdevice_t     *dev = wlandev->netdev;
 	p80211_hdr_a3_t *hdr;
-	UINT16 fc;
+	u16 fc;
 
         DBFENTER;
 
@@ -478,15 +414,6 @@ static int p80211knetdev_hard_start_xmit( struct sk_buff *skb, netdevice_t *netd
 	memset(&p80211_hdr, 0, sizeof(p80211_hdr_t));
 	memset(&p80211_wep, 0, sizeof(p80211_metawep_t));
 
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,3,38) )
-	if ( test_and_set_bit(0, (void*)&(netdev->tbusy)) != 0 ) {
-		/* We've been called w/ tbusy set, has the tx */
-		/* path stalled?   */
-		WLAN_LOG_DEBUG(1, "called when tbusy set\n");
-		result = 1;
-		goto failed;
-	}
-#else
 	if ( netif_queue_stopped(netdev) ) {
 		WLAN_LOG_DEBUG(1, "called when queue stopped.\n");
 		result = 1;
@@ -495,12 +422,6 @@ static int p80211knetdev_hard_start_xmit( struct sk_buff *skb, netdevice_t *netd
 
 	netif_stop_queue(netdev);
 
-	/* No timeout handling here, 2.3.38+ kernels call the
-	 * timeout function directly.
-	 * TODO: Add timeout handling.
-	*/
-#endif
-
 	/* Check to see that a valid mode is set */
 	switch( wlandev->macmode ) {
 	case WLAN_MACMODE_IBSS_STA:
@@ -513,7 +434,7 @@ static int p80211knetdev_hard_start_xmit( struct sk_buff *skb, netdevice_t *netd
 		 * TODO: we need a saner way to handle this
 		 */
 		if(skb->protocol != ETH_P_80211_RAW) {
-			p80211netdev_start_queue(wlandev);
+			netif_start_queue(wlandev->netdev);
 			WLAN_LOG_NOTICE(
 				"Tx attempt prior to association, frame dropped.\n");
 			wlandev->linux_stats.tx_dropped++;
@@ -557,7 +478,7 @@ static int p80211knetdev_hard_start_xmit( struct sk_buff *skb, netdevice_t *netd
 	if ( txresult == 0) {
 		/* success and more buf */
 		/* avail, re: hw_txdata */
-		p80211netdev_wake_queue(wlandev);
+		netif_wake_queue(wlandev->netdev);
 		result = 0;
 	} else if ( txresult == 1 ) {
 		/* success, no more avail */
@@ -619,7 +540,7 @@ static void p80211knetdev_set_multicast_list(netdevice_t *dev)
 
 static int p80211netdev_ethtool(wlandevice_t *wlandev, void __user *useraddr)
 {
-	UINT32 ethcmd;
+	u32 ethcmd;
 	struct ethtool_drvinfo info;
 	struct ethtool_value edata;
 
@@ -686,7 +607,7 @@ static int p80211netdev_ethtool(wlandevice_t *wlandev, void __user *useraddr)
 *		-EFAULT memory fault copying msg from user buffer
 *		-ENOMEM unable to allocate kernel msg buffer
 *		-ENOSYS	bad magic, it the cmd really for us?
-*		-EINTR	sleeping on cmd, awakened by signal, cmd cancelled.
+*		-EintR	sleeping on cmd, awakened by signal, cmd cancelled.
 *
 * Call Context:
 *	Process thread (ioctl caller).  TODO: SMP support may require
@@ -697,21 +618,11 @@ static int p80211knetdev_do_ioctl(netdevice_t *dev, struct ifreq *ifr, int cmd)
 	int			result = 0;
 	p80211ioctl_req_t	*req = (p80211ioctl_req_t*)ifr;
 	wlandevice_t		*wlandev = dev->ml_priv;
-	UINT8			*msgbuf;
+	u8			*msgbuf;
 	DBFENTER;
 
 	WLAN_LOG_DEBUG(2, "rx'd ioctl, cmd=%d, len=%d\n", cmd, req->len);
 
-#if WIRELESS_EXT < 13
-	/* Is this a wireless extensions ioctl? */
-	if ((cmd >= SIOCIWFIRST) && (cmd <= SIOCIWLAST)) {
-		if ((result = p80211wext_support_ioctl(dev, ifr, cmd))
-		    != (-EOPNOTSUPP)) {
-			goto bail;
-		}
-	}
-#endif
-
 #ifdef SIOCETHTOOL
 	if (cmd == SIOCETHTOOL) {
 		result = p80211netdev_ethtool(wlandev, (void __user *) ifr->ifr_data);
@@ -792,15 +703,9 @@ static int p80211knetdev_set_mac_address(netdevice_t *dev, void *addr)
 
 	DBFENTER;
 	/* If we're running, we don't allow MAC address changes */
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,3,38) )
-	if ( dev->start) {
-		return -EBUSY;
-	}
-#else
 	if (netif_running(dev)) {
 		return -EBUSY;
 	}
-#endif
 
 	/* Set up some convenience pointers. */
 	mibattr = &dot11req.mibattribute;
@@ -833,7 +738,7 @@ static int p80211knetdev_set_mac_address(netdevice_t *dev, void *addr)
 	resultcode->data = 0;
 
 	/* now fire the request */
-	result = p80211req_dorequest(dev->ml_priv, (UINT8 *)&dot11req);
+	result = p80211req_dorequest(dev->ml_priv, (u8 *)&dot11req);
 
 	/* If the request wasn't successful, report an error and don't
 	 * change the netdev address
@@ -909,13 +814,11 @@ int wlan_setup(wlandevice_t *wlandev)
 		     (unsigned long)wlandev);
 
 	/* Allocate and initialize the struct device */
-	dev = kmalloc(sizeof(netdevice_t), GFP_ATOMIC);
+	dev = alloc_netdev(0,"wlan%d",ether_setup);
 	if ( dev == NULL ) {
 		WLAN_LOG_ERROR("Failed to alloc netdev.\n");
 		result = 1;
 	} else {
-		memset( dev, 0, sizeof(netdevice_t));
-		ether_setup(dev);
 		wlandev->netdev = dev;
 		dev->ml_priv = wlandev;
 		dev->hard_start_xmit =	p80211knetdev_hard_start_xmit;
@@ -930,21 +833,12 @@ int wlan_setup(wlandevice_t *wlandev)
 		dev->open =		p80211knetdev_open;
 		dev->stop =		p80211knetdev_stop;
 
-#ifdef CONFIG_NET_WIRELESS
-#if ((WIRELESS_EXT < 17) && (WIRELESS_EXT < 21))
+#if (WIRELESS_EXT < 21)
 		dev->get_wireless_stats = p80211wext_get_wireless_stats;
 #endif
-#if WIRELESS_EXT > 12
 		dev->wireless_handlers = &p80211wext_handler_def;
-#endif
-#endif
 
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,3,38) )
-		dev->tbusy = 1;
-		dev->start = 0;
-#else
 		netif_stop_queue(dev);
-#endif
 #ifdef HAVE_CHANGE_MTU
 		dev->change_mtu = wlan_change_mtu;
 #endif
@@ -1027,44 +921,12 @@ int wlan_unsetup(wlandevice_t *wlandev)
 int register_wlandev(wlandevice_t *wlandev)
 {
 	int		i = 0;
-	netdevice_t	*dev = wlandev->netdev;
 
 	DBFENTER;
 
-	i = dev_alloc_name(wlandev->netdev, "wlan%d");
-	if (i >= 0) {
-		i = register_netdev(wlandev->netdev);
-	}
-	if (i != 0) {
-		return -EIO;
-	}
-
-#if ( LINUX_VERSION_CODE < KERNEL_VERSION(2,4,0) )
-	dev->name = wlandev->name;
-#else
-	strcpy(wlandev->name, dev->name);
-#endif
-
-#ifdef CONFIG_PROC_FS
-	if (proc_p80211) {
-		wlandev->procdir = proc_mkdir(wlandev->name, proc_p80211);
-		if ( wlandev->procdir )
-			wlandev->procwlandev =
-				create_proc_read_entry("wlandev", 0,
-						       wlandev->procdir,
-						       p80211netdev_proc_read,
-						       wlandev);
-		if (wlandev->nsd_proc_read)
-			create_proc_read_entry("nsd", 0,
-					       wlandev->procdir,
-					       wlandev->nsd_proc_read,
-					       wlandev);
-	}
-#endif
-
-#ifdef CONFIG_HOTPLUG
-	p80211_run_sbin_hotplug(wlandev, WLAN_HOTPLUG_REGISTER);
-#endif
+	i = register_netdev(wlandev->netdev);
+	if (i)
+		return i;
 
 	DBFEXIT;
 	return 0;
@@ -1094,22 +956,6 @@ int unregister_wlandev(wlandevice_t *wlandev)
 
 	DBFENTER;
 
-#ifdef CONFIG_HOTPLUG
-	p80211_run_sbin_hotplug(wlandev, WLAN_HOTPLUG_REMOVE);
-#endif
-
-#ifdef CONFIG_PROC_FS
-	if ( wlandev->procwlandev ) {
-		remove_proc_entry("wlandev", wlandev->procdir);
-	}
-	if ( wlandev->nsd_proc_read ) {
-		remove_proc_entry("nsd", wlandev->procdir);
-	}
-	if (wlandev->procdir) {
-		remove_proc_entry(wlandev->name, proc_p80211);
-	}
-#endif
-
 	unregister_netdev(wlandev->netdev);
 
 	/* Now to clean out the rx queue */
@@ -1121,76 +967,6 @@ int unregister_wlandev(wlandevice_t *wlandev)
 	return 0;
 }
 
-#ifdef CONFIG_PROC_FS
-/*----------------------------------------------------------------
-* proc_read
-*
-* Read function for /proc/net/p80211/<device>/wlandev
-*
-* Arguments:
-*	buf
-*	start
-*	offset
-*	count
-*	eof
-*	data
-* Returns:
-*	zero on success, non-zero otherwise.
-* Call Context:
-*	Can be either interrupt or not.
-----------------------------------------------------------------*/
-static int
-p80211netdev_proc_read(
-	char	*page,
-	char	**start,
-	off_t	offset,
-	int	count,
-	int	*eof,
-	void	*data)
-{
-	char	 *p = page;
-	wlandevice_t *wlandev = (wlandevice_t *) data;
-
-	DBFENTER;
-	if (offset != 0) {
-		*eof = 1;
-		goto exit;
-	}
-
-	p += sprintf(p, "p80211 version: %s (%s)\n\n",
-		     WLAN_RELEASE, WLAN_BUILD_DATE);
-	p += sprintf(p, "name       : %s\n", wlandev->name);
-	p += sprintf(p, "nsd name   : %s\n", wlandev->nsdname);
-	p += sprintf(p, "address    : %02x:%02x:%02x:%02x:%02x:%02x\n",
-		     wlandev->netdev->dev_addr[0], wlandev->netdev->dev_addr[1], wlandev->netdev->dev_addr[2],
-		     wlandev->netdev->dev_addr[3], wlandev->netdev->dev_addr[4], wlandev->netdev->dev_addr[5]);
-	p += sprintf(p, "nsd caps   : %s%s%s%s%s%s%s%s%s%s\n",
-		     (wlandev->nsdcaps & P80211_NSDCAP_HARDWAREWEP) ? "wep_hw " : "",
-		     (wlandev->nsdcaps & P80211_NSDCAP_TIEDWEP) ? "wep_tied " : "",
-		     (wlandev->nsdcaps & P80211_NSDCAP_NOHOSTWEP) ? "wep_hw_only " : "",
-		     (wlandev->nsdcaps & P80211_NSDCAP_PBCC) ? "pbcc " : "",
-		     (wlandev->nsdcaps & P80211_NSDCAP_SHORT_PREAMBLE) ? "short_preamble " : "",
-		     (wlandev->nsdcaps & P80211_NSDCAP_AGILITY) ? "agility " : "",
-		     (wlandev->nsdcaps & P80211_NSDCAP_AP_RETRANSMIT) ? "ap_retransmit " : "",
-		     (wlandev->nsdcaps & P80211_NSDCAP_HWFRAGMENT) ? "hw_frag " : "",
-		     (wlandev->nsdcaps & P80211_NSDCAP_AUTOJOIN) ? "autojoin " : "",
-		     (wlandev->nsdcaps & P80211_NSDCAP_NOSCAN) ? "" : "scan ");
-
-
-	p += sprintf(p, "bssid      : %02x:%02x:%02x:%02x:%02x:%02x\n",
-		     wlandev->bssid[0], wlandev->bssid[1], wlandev->bssid[2],
-		     wlandev->bssid[3], wlandev->bssid[4], wlandev->bssid[5]);
-
-	p += sprintf(p, "Enabled    : %s%s\n",
-		     (wlandev->shortpreamble) ? "short_preamble " : "",
-		     (wlandev->hostwep & HOSTWEP_PRIVACYINVOKED) ? "privacy" : "");
-
-
- exit:
-	DBFEXIT;
-	return (p - page);
-}
-#endif
 
 /*----------------------------------------------------------------
 * p80211netdev_hwremoved
@@ -1227,7 +1003,7 @@ void p80211netdev_hwremoved(wlandevice_t *wlandev)
 	DBFENTER;
 	wlandev->hwremoved = 1;
 	if ( wlandev->state == WLAN_DEVICE_OPEN) {
-		p80211netdev_stop_queue(wlandev);
+		netif_stop_queue(wlandev->netdev);
 	}
 
 	netif_device_detach(wlandev->netdev);
@@ -1257,10 +1033,10 @@ void p80211netdev_hwremoved(wlandevice_t *wlandev)
 * Call context:
 *	interrupt
 ----------------------------------------------------------------*/
-static int p80211_rx_typedrop( wlandevice_t *wlandev, UINT16 fc)
+static int p80211_rx_typedrop( wlandevice_t *wlandev, u16 fc)
 {
-	UINT16	ftype;
-	UINT16	fstype;
+	u16	ftype;
+	u16	fstype;
 	int	drop = 0;
 	/* Classify frame, increment counter */
 	ftype = WLAN_GET_FC_FTYPE(fc);
@@ -1416,75 +1192,6 @@ static int p80211_rx_typedrop( wlandevice_t *wlandev, UINT16 fc)
 	return drop;
 }
 
-#ifdef CONFIG_HOTPLUG
-/* Notify userspace when a netdevice event occurs,
- * by running '/sbin/hotplug net' with certain
- * environment variables set.
- */
-int p80211_run_sbin_hotplug(wlandevice_t *wlandev, char *action)
-{
-        char *argv[3], *envp[7], ifname[12 + IFNAMSIZ], action_str[32];
-	char nsdname[32], wlan_wext[32];
-        int i;
-
-	if (wlandev) {
-		sprintf(ifname, "INTERFACE=%s", wlandev->name);
-		sprintf(nsdname, "NSDNAME=%s", wlandev->nsdname);
-	} else {
-		sprintf(ifname, "INTERFACE=null");
-		sprintf(nsdname, "NSDNAME=null");
-	}
-
-	sprintf(wlan_wext, "WLAN_WEXT=%s", wlan_wext_write ? "y" : "");
-        sprintf(action_str, "ACTION=%s", action);
-
-        i = 0;
-        argv[i++] = hotplug_path;
-        argv[i++] = "wlan";
-        argv[i] = NULL;
-
-        i = 0;
-        /* minimal command environment */
-        envp [i++] = "HOME=/";
-        envp [i++] = "PATH=/sbin:/bin:/usr/sbin:/usr/bin";
-        envp [i++] = ifname;
-        envp [i++] = action_str;
-        envp [i++] = nsdname;
-        envp [i++] = wlan_wext;
-        envp [i] = NULL;
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,62))
-        return call_usermodehelper(argv [0], argv, envp);
-#else
-        return call_usermodehelper(argv [0], argv, envp, 0);
-#endif
-}
-
-#endif
-
-
-void    p80211_suspend(wlandevice_t *wlandev)
-{
-	DBFENTER;
-
-#ifdef CONFIG_HOTPLUG
-	p80211_run_sbin_hotplug(wlandev, WLAN_HOTPLUG_SUSPEND);
-#endif
-
-	DBFEXIT;
-}
-
-void    p80211_resume(wlandevice_t *wlandev)
-{
-	DBFENTER;
-
-#ifdef CONFIG_HOTPLUG
-	p80211_run_sbin_hotplug(wlandev, WLAN_HOTPLUG_RESUME);
-#endif
-
-	DBFEXIT;
-}
-
 static void p80211knetdev_tx_timeout( netdevice_t *netdev)
 {
 	wlandevice_t	*wlandev = netdev->ml_priv;
@@ -1495,7 +1202,7 @@ static void p80211knetdev_tx_timeout( netdevice_t *netdev)
 	} else {
 		WLAN_LOG_WARNING("Implement tx_timeout for %s\n",
 				 wlandev->nsdname);
-		p80211netdev_wake_queue(wlandev);
+		netif_wake_queue(wlandev->netdev);
 	}
 
 	DBFEXIT;
diff --git a/drivers/staging/wlan-ng/p80211netdev.h b/drivers/staging/wlan-ng/p80211netdev.h
index 9b2e0cdcb44..940146fba9c 100644
--- a/drivers/staging/wlan-ng/p80211netdev.h
+++ b/drivers/staging/wlan-ng/p80211netdev.h
@@ -113,54 +113,48 @@
 /* Received frame statistics */
 typedef struct p80211_frmrx_t
 {
-	UINT32	mgmt;
-	UINT32	assocreq;
-	UINT32	assocresp;
-	UINT32	reassocreq;
-	UINT32	reassocresp;
-	UINT32	probereq;
-	UINT32	proberesp;
-	UINT32	beacon;
-	UINT32	atim;
-	UINT32	disassoc;
-	UINT32	authen;
-	UINT32	deauthen;
-	UINT32	mgmt_unknown;
-	UINT32	ctl;
-	UINT32	pspoll;
-	UINT32	rts;
-	UINT32	cts;
-	UINT32	ack;
-	UINT32	cfend;
-	UINT32	cfendcfack;
-	UINT32	ctl_unknown;
-	UINT32	data;
-	UINT32	dataonly;
-	UINT32	data_cfack;
-	UINT32	data_cfpoll;
-	UINT32	data__cfack_cfpoll;
-	UINT32	null;
-	UINT32	cfack;
-	UINT32	cfpoll;
-	UINT32	cfack_cfpoll;
-	UINT32	data_unknown;
-	UINT32  decrypt;
-	UINT32  decrypt_err;
+	u32	mgmt;
+	u32	assocreq;
+	u32	assocresp;
+	u32	reassocreq;
+	u32	reassocresp;
+	u32	probereq;
+	u32	proberesp;
+	u32	beacon;
+	u32	atim;
+	u32	disassoc;
+	u32	authen;
+	u32	deauthen;
+	u32	mgmt_unknown;
+	u32	ctl;
+	u32	pspoll;
+	u32	rts;
+	u32	cts;
+	u32	ack;
+	u32	cfend;
+	u32	cfendcfack;
+	u32	ctl_unknown;
+	u32	data;
+	u32	dataonly;
+	u32	data_cfack;
+	u32	data_cfpoll;
+	u32	data__cfack_cfpoll;
+	u32	null;
+	u32	cfack;
+	u32	cfpoll;
+	u32	cfack_cfpoll;
+	u32	data_unknown;
+	u32  decrypt;
+	u32  decrypt_err;
 } p80211_frmrx_t;
 
-#ifdef WIRELESS_EXT
 /* called by /proc/net/wireless */
 struct iw_statistics* p80211wext_get_wireless_stats(netdevice_t *dev);
 /* wireless extensions' ioctls */
 int p80211wext_support_ioctl(netdevice_t *dev, struct ifreq *ifr, int cmd);
-#if WIRELESS_EXT > 12
 extern struct iw_handler_def p80211wext_handler_def;
-#endif
-
 int p80211wext_event_associated(struct wlandevice *wlandev, int assoc);
 
-#endif /* wireless extensions */
-
 /* WEP stuff */
 #define NUM_WEPKEYS 4
 #define MAX_KEYLEN 32
@@ -184,18 +178,18 @@ typedef struct wlandevice
 	char		name[WLAN_DEVNAMELEN_MAX]; /* Dev name, from register_wlandev()*/
 	char		*nsdname;
 
-	UINT32          state;          /* Device I/F state (open/closed) */
-	UINT32		msdstate;	/* state of underlying driver */
-	UINT32		hwremoved;	/* Has the hw been yanked out? */
+	u32          state;          /* Device I/F state (open/closed) */
+	u32		msdstate;	/* state of underlying driver */
+	u32		hwremoved;	/* Has the hw been yanked out? */
 
 	/* Hardware config */
-	UINT		irq;
-	UINT		iobase;
-	UINT		membase;
-	UINT32          nsdcaps;  /* NSD Capabilities flags */
+	unsigned int		irq;
+	unsigned int		iobase;
+	unsigned int		membase;
+	u32          nsdcaps;  /* NSD Capabilities flags */
 
 	/* Config vars */
-	UINT		ethconv;
+	unsigned int		ethconv;
 
 	/* device methods (init by MSD, used by p80211 */
 	int		(*open)(struct wlandevice *wlandev);
@@ -207,20 +201,15 @@ typedef struct wlandevice
 					      netdevice_t *dev);
 	void		(*tx_timeout)(struct wlandevice *wlandev);
 
-#ifdef CONFIG_PROC_FS
-	int             (*nsd_proc_read)(char *page, char **start, off_t offset, int count, int	*eof, void *data);
-#endif
-
 	/* 802.11 State */
-	UINT8		bssid[WLAN_BSSID_LEN];
+	u8		bssid[WLAN_BSSID_LEN];
 	p80211pstr32_t	ssid;
-	UINT32		macmode;
+	u32		macmode;
 	int             linkstatus;
-	int             shortpreamble;  /* C bool */
 
 	/* WEP State */
-	UINT8 wep_keys[NUM_WEPKEYS][MAX_KEYLEN];
-	UINT8 wep_keylens[NUM_WEPKEYS];
+	u8 wep_keys[NUM_WEPKEYS][MAX_KEYLEN];
+	u8 wep_keylens[NUM_WEPKEYS];
 	int   hostwep;
 
 	/* Request/Confirm i/f state (used by p80211) */
@@ -232,12 +221,6 @@ typedef struct wlandevice
 	netdevice_t		*netdev;	/* ptr to linux netdevice */
 	struct net_device_stats linux_stats;
 
-#ifdef CONFIG_PROC_FS
-	/* Procfs support */
-	struct proc_dir_entry	*procdir;
-	struct proc_dir_entry	*procwlandev;
-#endif
-
 	/* Rx bottom half */
 	struct tasklet_struct	rx_bh;
 
@@ -246,29 +229,18 @@ typedef struct wlandevice
 	/* 802.11 device statistics */
 	struct p80211_frmrx_t	rx;
 
-/* compatibility to wireless extensions */
-#ifdef WIRELESS_EXT
 	struct iw_statistics	wstats;
 
 	/* jkriegl: iwspy fields */
-        UINT8			spy_number;
+        u8			spy_number;
         char			spy_address[IW_MAX_SPY][ETH_ALEN];
         struct iw_quality       spy_stat[IW_MAX_SPY];
-
-#endif
-
 } wlandevice_t;
 
 /* WEP stuff */
-int wep_change_key(wlandevice_t *wlandev, int keynum, UINT8* key, int keylen);
-int wep_decrypt(wlandevice_t *wlandev, UINT8 *buf, UINT32 len, int key_override, UINT8 *iv, UINT8 *icv);
-int wep_encrypt(wlandevice_t *wlandev, UINT8 *buf, UINT8 *dst, UINT32 len, int keynum, UINT8 *iv, UINT8 *icv);
-
-/*================================================================*/
-/* Externs */
-
-/*================================================================*/
-/* Function Declarations */
+int wep_change_key(wlandevice_t *wlandev, int keynum, u8* key, int keylen);
+int wep_decrypt(wlandevice_t *wlandev, u8 *buf, u32 len, int key_override, u8 *iv, u8 *icv);
+int wep_encrypt(wlandevice_t *wlandev, u8 *buf, u8 *dst, u32 len, int keynum, u8 *iv, u8 *icv);
 
 void	p80211netdev_startup(void);
 void	p80211netdev_shutdown(void);
@@ -278,59 +250,5 @@ int	register_wlandev(wlandevice_t *wlandev);
 int	unregister_wlandev(wlandevice_t *wlandev);
 void	p80211netdev_rx(wlandevice_t *wlandev, struct sk_buff *skb);
 void	p80211netdev_hwremoved(wlandevice_t *wlandev);
-void    p80211_suspend(wlandevice_t *wlandev);
-void    p80211_resume(wlandevice_t *wlandev);
-
-/*================================================================*/
-/* Function Definitions */
-
-static inline void
-p80211netdev_stop_queue(wlandevice_t *wlandev)
-{
-	if ( !wlandev ) return;
-	if ( !wlandev->netdev ) return;
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,3,38) )
-	wlandev->netdev->tbusy = 1;
-	wlandev->netdev->start = 0;
-#else
-	netif_stop_queue(wlandev->netdev);
-#endif
-}
-
-static inline void
-p80211netdev_start_queue(wlandevice_t *wlandev)
-{
-	if ( !wlandev ) return;
-	if ( !wlandev->netdev ) return;
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,3,38) )
-	wlandev->netdev->tbusy = 0;
-	wlandev->netdev->start = 1;
-#else
-	netif_start_queue(wlandev->netdev);
-#endif
-}
-
-static inline void
-p80211netdev_wake_queue(wlandevice_t *wlandev)
-{
-	if ( !wlandev ) return;
-	if ( !wlandev->netdev ) return;
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,3,38) )
-	wlandev->netdev->tbusy = 0;
-	mark_bh(NET_BH);
-#else
-	netif_wake_queue(wlandev->netdev);
-#endif
-}
-
-#ifdef CONFIG_HOTPLUG
-#define WLAN_HOTPLUG_REGISTER "register"
-#define WLAN_HOTPLUG_REMOVE   "remove"
-#define WLAN_HOTPLUG_STARTUP  "startup"
-#define WLAN_HOTPLUG_SHUTDOWN "shutdown"
-#define WLAN_HOTPLUG_SUSPEND "suspend"
-#define WLAN_HOTPLUG_RESUME "resume"
-int p80211_run_sbin_hotplug(wlandevice_t *wlandev, char *action);
-#endif
 
 #endif
diff --git a/drivers/staging/wlan-ng/p80211req.c b/drivers/staging/wlan-ng/p80211req.c
index 0233abeccc4..6e20bff0e67 100644
--- a/drivers/staging/wlan-ng/p80211req.c
+++ b/drivers/staging/wlan-ng/p80211req.c
@@ -54,7 +54,6 @@
 /* System Includes */
 
 
-#include <linux/version.h>
 
 #include <linux/module.h>
 #include <linux/kernel.h>
@@ -68,7 +67,6 @@
 #include <net/sock.h>
 #include <linux/netlink.h>
 
-#include "version.h"
 #include "wlan_compat.h"
 
 /*================================================================*/
@@ -126,7 +124,7 @@ static int p80211req_mibset_mibget(wlandevice_t *wlandev, p80211msg_dot11req_mib
 *	Potentially blocks the caller, so it's a good idea to
 *	not call this function from an interrupt context.
 ----------------------------------------------------------------*/
-int p80211req_dorequest( wlandevice_t *wlandev, UINT8 *msgbuf)
+int p80211req_dorequest( wlandevice_t *wlandev, u8 *msgbuf)
 {
 	int		result = 0;
 	p80211msg_t	*msg = (p80211msg_t*)msgbuf;
@@ -224,38 +222,11 @@ static int p80211req_mibset_mibget(wlandevice_t *wlandev,
 {
 	p80211itemd_t   *mibitem = (p80211itemd_t *) mib_msg->mibattribute.data;
 	p80211pstrd_t  *pstr = (p80211pstrd_t*) mibitem->data;
-	UINT8 *key = mibitem->data + sizeof(p80211pstrd_t);
+	u8 *key = mibitem->data + sizeof(p80211pstrd_t);
 
 	DBFENTER;
 
 	switch (mibitem->did) {
-	case DIDmib_dot11smt_p80211Table_p80211_ifstate: {
-		UINT32 *data = (UINT32 *) mibitem->data;
-		if (isget)
-			switch (wlandev->msdstate) {
-			case WLAN_MSD_HWPRESENT:
-				*data = P80211ENUM_ifstate_disable;
-				break;
-			case WLAN_MSD_FWLOAD:
-				*data = P80211ENUM_ifstate_fwload;
-				break;
-			case WLAN_MSD_RUNNING:
-				*data = P80211ENUM_ifstate_enable;
-				break;
-			default:
-				*data = P80211ENUM_ifstate_enable;
-			}
-		break;
-	}
-	case DIDmib_dot11phy_dot11PhyOperationTable_dot11ShortPreambleEnabled: {
-		UINT32 *data = (UINT32 *) mibitem->data;
-
-		if (isget)
-			*data = wlandev->shortpreamble;
-		else
-			wlandev->shortpreamble = *data;
-		break;
-	}
 	case DIDmib_dot11smt_dot11WEPDefaultKeysTable_dot11WEPDefaultKey0: {
 		if (!isget)
 			wep_change_key(wlandev, 0, key, pstr->len);
@@ -277,7 +248,7 @@ static int p80211req_mibset_mibget(wlandevice_t *wlandev,
 		break;
 	}
 	case DIDmib_dot11smt_dot11PrivacyTable_dot11WEPDefaultKeyID: {
-		UINT32 *data = (UINT32 *) mibitem->data;
+		u32 *data = (u32 *) mibitem->data;
 
 		if (isget) {
 			*data = wlandev->hostwep & HOSTWEP_DEFAULTKEY_MASK;
@@ -289,7 +260,7 @@ static int p80211req_mibset_mibget(wlandevice_t *wlandev,
 		break;
 	}
 	case DIDmib_dot11smt_dot11PrivacyTable_dot11PrivacyInvoked: {
-		UINT32 *data = (UINT32 *) mibitem->data;
+		u32 *data = (u32 *) mibitem->data;
 
 		if (isget) {
 			if (wlandev->hostwep & HOSTWEP_PRIVACYINVOKED)
@@ -304,7 +275,7 @@ static int p80211req_mibset_mibget(wlandevice_t *wlandev,
 		break;
 	}
 	case DIDmib_dot11smt_dot11PrivacyTable_dot11ExcludeUnencrypted: {
-		UINT32 *data = (UINT32 *) mibitem->data;
+		u32 *data = (u32 *) mibitem->data;
 
 		if (isget) {
 			if (wlandev->hostwep & HOSTWEP_EXCLUDEUNENCRYPTED)
diff --git a/drivers/staging/wlan-ng/p80211req.h b/drivers/staging/wlan-ng/p80211req.h
index 54abdceedc5..497a4d6eb59 100644
--- a/drivers/staging/wlan-ng/p80211req.h
+++ b/drivers/staging/wlan-ng/p80211req.h
@@ -63,6 +63,6 @@
 /*================================================================*/
 /* Function Declarations */
 
-int	p80211req_dorequest(wlandevice_t *wlandev, UINT8 *msgbuf);
+int	p80211req_dorequest(wlandevice_t *wlandev, u8 *msgbuf);
 
 #endif
diff --git a/drivers/staging/wlan-ng/p80211types.h b/drivers/staging/wlan-ng/p80211types.h
index 811b0ce39bf..5be6737e3e5 100644
--- a/drivers/staging/wlan-ng/p80211types.h
+++ b/drivers/staging/wlan-ng/p80211types.h
@@ -80,13 +80,13 @@
 
 #define P80211_TYPE_OCTETSTR		1	/* pascal array of bytes */
 #define P80211_TYPE_DISPLAYSTR		2	/* pascal array of bytes containing ascii */
-#define P80211_TYPE_INT			4	/* UINT32 min and max limited by 32 bits */
-#define P80211_TYPE_ENUMINT		5	/* UINT32 holding a numeric
+#define P80211_TYPE_int			4	/* u32 min and max limited by 32 bits */
+#define P80211_TYPE_ENUMint		5	/* u32 holding a numeric
 						   code that can be mapped
 						   to a textual name */
 #define P80211_TYPE_UNKDATA		6	/* Data item containing an
 						   unknown data type */
-#define P80211_TYPE_INTARRAY		7	/* Array of 32-bit integers. */
+#define P80211_TYPE_intARRAY		7	/* Array of 32-bit integers. */
 #define P80211_TYPE_BITARRAY		8	/* Array of bits. */
 #define P80211_TYPE_MACARRAY		9	/* Array of MAC addresses. */
 
@@ -243,7 +243,7 @@
 					/* is a DID-LEN-DATA triple */
 					/* with a max size of 4+4+384 */
 
-#define P80211_SET_INT(item, value) do { \
+#define P80211_SET_int(item, value) do { \
 	(item).data   = (value); \
 	(item).status = P80211ENUM_msgitem_status_data_ok; \
 	} while(0)
@@ -279,9 +279,9 @@
 
 #define P80211ITEM_SETFLAGS(q, r, c)	( q | r | c )
 
-#define P80211ITEM_ISREQUIRED(flags)	(((UINT32)(flags & ISREQUIRED)) >> 31 )
-#define P80211ITEM_ISREQUEST(flags)	(((UINT32)(flags & ISREQUEST)) >> 30 )
-#define P80211ITEM_ISCONFIRM(flags)	(((UINT32)(flags & ISCONFIRM)) >> 29 )
+#define P80211ITEM_ISREQUIRED(flags)	(((u32)(flags & ISREQUIRED)) >> 31 )
+#define P80211ITEM_ISREQUEST(flags)	(((u32)(flags & ISREQUEST)) >> 30 )
+#define P80211ITEM_ISCONFIRM(flags)	(((u32)(flags & ISCONFIRM)) >> 29 )
 
 /*----------------------------------------------------------------*/
 /* The following macro creates a name for an enum */
@@ -320,7 +320,7 @@
 #define P80211DID_MASK_ACCESS 		(0x00000003UL)
 
 
-#define P80211DID_MK(a,m,l)	((((UINT32)(a)) & (m)) << (l))
+#define P80211DID_MK(a,m,l)	((((u32)(a)) & (m)) << (l))
 
 #define P80211DID_MKSECTION(a)	P80211DID_MK(a, \
 					P80211DID_MASK_SECTION, \
@@ -347,7 +347,7 @@
 						(a) )
 
 
-#define P80211DID_GET(a,m,l)	((((UINT32)(a)) >> (l)) & (m))
+#define P80211DID_GET(a,m,l)	((((u32)(a)) >> (l)) & (m))
 
 #define P80211DID_SECTION(a)	P80211DID_GET(a, \
 					P80211DID_MASK_SECTION, \
@@ -373,17 +373,17 @@
 
 /*----------------------------------------------------------------*/
 /* The following structure types are used for the represenation */
-/*  of ENUMINT type metadata. */
+/*  of ENUMint type metadata. */
 
 typedef struct p80211enumpair
 {
-	UINT32			val;
+	u32			val;
 	char			*name;
 } p80211enumpair_t;
 
 typedef struct p80211enum
 {
-	INT			nitems;
+	int			nitems;
 	p80211enumpair_t	*list;
 } p80211enum_t;
 
@@ -394,137 +394,137 @@ typedef struct p80211enum
 /* Template pascal string */
 typedef struct p80211pstr
 {
-	UINT8		len;
+	u8		len;
 } __WLAN_ATTRIB_PACK__ p80211pstr_t;
 
 typedef struct p80211pstrd
 {
-	UINT8		len;
-	UINT8		data[0];
+	u8		len;
+	u8		data[0];
 } __WLAN_ATTRIB_PACK__ p80211pstrd_t;
 
 /* Maximum pascal string */
 typedef struct p80211pstr255
 {
-	UINT8		len;
-	UINT8		data[MAXLEN_PSTR255];
+	u8		len;
+	u8		data[MAXLEN_PSTR255];
 } __WLAN_ATTRIB_PACK__ p80211pstr255_t;
 
 /* pascal string for macaddress and bssid */
 typedef struct p80211pstr6
 {
-	UINT8		len;
-	UINT8		data[MAXLEN_PSTR6];
+	u8		len;
+	u8		data[MAXLEN_PSTR6];
 } __WLAN_ATTRIB_PACK__ p80211pstr6_t;
 
 /* pascal string for channel list */
 typedef struct p80211pstr14
 {
-	UINT8		len;
-	UINT8		data[MAXLEN_PSTR14];
+	u8		len;
+	u8		data[MAXLEN_PSTR14];
 } __WLAN_ATTRIB_PACK__ p80211pstr14_t;
 
 /* pascal string for ssid */
 typedef struct p80211pstr32
 {
-	UINT8		len;
-	UINT8		data[MAXLEN_PSTR32];
+	u8		len;
+	u8		data[MAXLEN_PSTR32];
 } __WLAN_ATTRIB_PACK__ p80211pstr32_t;
 
 /* MAC address array */
 typedef struct p80211macarray
 {
-	UINT32		cnt;
-	UINT8		data[1][MAXLEN_PSTR6];
+	u32		cnt;
+	u8		data[1][MAXLEN_PSTR6];
 } __WLAN_ATTRIB_PACK__ p80211macarray_t;
 
 /* prototype template */
 typedef struct p80211item
 {
-	UINT32		did;
-	UINT16		status;
-	UINT16		len;
+	u32		did;
+	u16		status;
+	u16		len;
 } __WLAN_ATTRIB_PACK__ p80211item_t;
 
 /* prototype template w/ data item */
 typedef struct p80211itemd
 {
-	UINT32		did;
-	UINT16		status;
-	UINT16		len;
-	UINT8		data[0];
+	u32		did;
+	u16		status;
+	u16		len;
+	u8		data[0];
 } __WLAN_ATTRIB_PACK__ p80211itemd_t;
 
-/* message data item for INT, BOUNDEDINT, ENUMINT */
+/* message data item for int, BOUNDEDINT, ENUMINT */
 typedef struct p80211item_uint32
 {
-	UINT32		did;
-	UINT16		status;
-	UINT16		len;
-	UINT32		data;
+	u32		did;
+	u16		status;
+	u16		len;
+	u32		data;
 } __WLAN_ATTRIB_PACK__ p80211item_uint32_t;
 
 /* message data item for OCTETSTR, DISPLAYSTR */
 typedef struct p80211item_pstr6
 {
-	UINT32		did;
-	UINT16		status;
-	UINT16		len;
+	u32		did;
+	u16		status;
+	u16		len;
 	p80211pstr6_t	data;
 } __WLAN_ATTRIB_PACK__ p80211item_pstr6_t;
 
 /* message data item for OCTETSTR, DISPLAYSTR */
 typedef struct p80211item_pstr14
 {
-	UINT32			did;
-	UINT16			status;
-	UINT16			len;
+	u32			did;
+	u16			status;
+	u16			len;
 	p80211pstr14_t		data;
 } __WLAN_ATTRIB_PACK__ p80211item_pstr14_t;
 
 /* message data item for OCTETSTR, DISPLAYSTR */
 typedef struct p80211item_pstr32
 {
-	UINT32			did;
-	UINT16			status;
-	UINT16			len;
+	u32			did;
+	u16			status;
+	u16			len;
 	p80211pstr32_t		data;
 } __WLAN_ATTRIB_PACK__ p80211item_pstr32_t;
 
 /* message data item for OCTETSTR, DISPLAYSTR */
 typedef struct p80211item_pstr255
 {
-	UINT32			did;
-	UINT16			status;
-	UINT16			len;
+	u32			did;
+	u16			status;
+	u16			len;
 	p80211pstr255_t		data;
 } __WLAN_ATTRIB_PACK__ p80211item_pstr255_t;
 
 /* message data item for UNK 392, namely mib items */
 typedef struct  p80211item_unk392
 {
-	UINT32		did;
-	UINT16		status;
-	UINT16		len;
-	UINT8		data[MAXLEN_MIBATTRIBUTE];
+	u32		did;
+	u16		status;
+	u16		len;
+	u8		data[MAXLEN_MIBATTRIBUTE];
 } __WLAN_ATTRIB_PACK__ p80211item_unk392_t;
 
 /* message data item for UNK 1025, namely p2 pdas */
 typedef struct  p80211item_unk1024
 {
-	UINT32		did;
-	UINT16		status;
-	UINT16		len;
-	UINT8		data[1024];
+	u32		did;
+	u16		status;
+	u16		len;
+	u8		data[1024];
 }  __WLAN_ATTRIB_PACK__ p80211item_unk1024_t;
 
 /* message data item for UNK 4096, namely p2 download chunks */
 typedef struct  p80211item_unk4096
 {
-	UINT32		did;
-	UINT16		status;
-	UINT16		len;
-	UINT8		data[4096];
+	u32		did;
+	u16		status;
+	u16		len;
+	u8		data[4096];
 }  __WLAN_ATTRIB_PACK__ p80211item_unk4096_t;
 
 struct catlistitem;
@@ -534,9 +534,9 @@ struct catlistitem;
 /*  metadata items.  Some components may choose to use more, */
 /*  less or different metadata items. */
 
-typedef void (*p80211_totext_t)( struct catlistitem *, UINT32 did, UINT8* itembuf, char *textbuf);
-typedef void (*p80211_fromtext_t)( struct catlistitem *, UINT32 did, UINT8* itembuf, char *textbuf);
-typedef UINT32 (*p80211_valid_t)( struct catlistitem *, UINT32 did, UINT8* itembuf);
+typedef void (*p80211_totext_t)( struct catlistitem *, u32 did, u8* itembuf, char *textbuf);
+typedef void (*p80211_fromtext_t)( struct catlistitem *, u32 did, u8* itembuf, char *textbuf);
+typedef u32 (*p80211_valid_t)( struct catlistitem *, u32 did, u8* itembuf);
 
 
 /*================================================================*/
@@ -575,8 +575,8 @@ extern p80211enum_t MKENUMNAME(p2preamble);
 /* The following declare some utility functions for use with the */
 /*  p80211enum_t type. */
 
-UINT32 p80211enum_text2int(p80211enum_t *ep, char *text);
-UINT32 p80211enum_int2text(p80211enum_t *ep, UINT32 val, char *text);
+u32 p80211enum_text2int(p80211enum_t *ep, char *text);
+u32 p80211enum_int2text(p80211enum_t *ep, u32 val, char *text);
 void p80211_error2text(int err_code, char *err_str);
 
 /*----------------------------------------------------------------*/
@@ -591,85 +591,85 @@ void p80211_error2text(int err_code, char *err_str);
 
 /*-- DISPLAYSTR ------------------------------------------------------*/
 /* pstr ==> cstr */
-void p80211_totext_displaystr( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf, char *textbuf );
+void p80211_totext_displaystr( struct catlistitem *metalist, u32 did, u8 *itembuf, char *textbuf );
 
 /* cstr ==> pstr */
-void p80211_fromtext_displaystr( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf, char *textbuf );
+void p80211_fromtext_displaystr( struct catlistitem *metalist, u32 did, u8 *itembuf, char *textbuf );
 
 /* function that checks validity of a displaystr binary value */
-UINT32 p80211_isvalid_displaystr( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf );
+u32 p80211_isvalid_displaystr( struct catlistitem *metalist, u32 did, u8 *itembuf );
 
 /*-- OCTETSTR --------------------------------------------------------*/
 /* pstr ==> "xx:xx:...." */
-void p80211_totext_octetstr( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf, char *textbuf );
+void p80211_totext_octetstr( struct catlistitem *metalist, u32 did, u8 *itembuf, char *textbuf );
 
 /* "xx:xx:...." ==> pstr */
-void p80211_fromtext_octetstr( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf, char *textbuf );
+void p80211_fromtext_octetstr( struct catlistitem *metalist, u32 did, u8 *itembuf, char *textbuf );
 
 /* function that checks validity of an octetstr binary value */
-UINT32 p80211_isvalid_octetstr( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf );
+u32 p80211_isvalid_octetstr( struct catlistitem *metalist, u32 did, u8 *itembuf );
 
-/*-- INT -------------------------------------------------------------*/
-/* UINT32 ==> %d */
-void p80211_totext_int( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf, char *textbuf );
+/*-- int -------------------------------------------------------------*/
+/* u32 ==> %d */
+void p80211_totext_int( struct catlistitem *metalist, u32 did, u8 *itembuf, char *textbuf );
 
-/* %d ==> UINT32 */
-void p80211_fromtext_int( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf, char *textbuf );
+/* %d ==> u32 */
+void p80211_fromtext_int( struct catlistitem *metalist, u32 did, u8 *itembuf, char *textbuf );
 
 /* function that checks validity of an int's binary value (always successful) */
-UINT32 p80211_isvalid_int( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf );
+u32 p80211_isvalid_int( struct catlistitem *metalist, u32 did, u8 *itembuf );
 
-/*-- ENUMINT ---------------------------------------------------------*/
-/* UINT32 ==> <valuename> */
-void p80211_totext_enumint( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf, char *textbuf );
+/*-- ENUMint ---------------------------------------------------------*/
+/* u32 ==> <valuename> */
+void p80211_totext_enumint( struct catlistitem *metalist, u32 did, u8 *itembuf, char *textbuf );
 
-/* <valuename> ==> UINT32 */
-void p80211_fromtext_enumint( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf, char *textbuf );
+/* <valuename> ==> u32 */
+void p80211_fromtext_enumint( struct catlistitem *metalist, u32 did, u8 *itembuf, char *textbuf );
 
 /* function that checks validity of an enum's binary value */
-UINT32 p80211_isvalid_enumint( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf );
+u32 p80211_isvalid_enumint( struct catlistitem *metalist, u32 did, u8 *itembuf );
 
-/*-- INTARRAY --------------------------------------------------------*/
-/* UINT32[] => %d,%d,%d,... */
-void p80211_totext_intarray( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf, char *textbuf );
+/*-- intARRAY --------------------------------------------------------*/
+/* u32[] => %d,%d,%d,... */
+void p80211_totext_intarray( struct catlistitem *metalist, u32 did, u8 *itembuf, char *textbuf );
 
-/* %d,%d,%d,... ==> UINT32[] */
-void p80211_fromtext_intarray( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf, char *textbuf );
+/* %d,%d,%d,... ==> u32[] */
+void p80211_fromtext_intarray( struct catlistitem *metalist, u32 did, u8 *itembuf, char *textbuf );
 
 /* function that checks validity of an integer array's value */
-UINT32 p80211_isvalid_intarray( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf );
+u32 p80211_isvalid_intarray( struct catlistitem *metalist, u32 did, u8 *itembuf );
 
 /*-- BITARRAY --------------------------------------------------------*/
-/* UINT32 ==> %d,%d,%d,... */
-void p80211_totext_bitarray( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf, char *textbuf );
+/* u32 ==> %d,%d,%d,... */
+void p80211_totext_bitarray( struct catlistitem *metalist, u32 did, u8 *itembuf, char *textbuf );
 
-/* %d,%d,%d,... ==> UINT32 */
-void p80211_fromtext_bitarray( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf, char *textbuf );
+/* %d,%d,%d,... ==> u32 */
+void p80211_fromtext_bitarray( struct catlistitem *metalist, u32 did, u8 *itembuf, char *textbuf );
 
 /* function that checks validity of a bit array's value */
-UINT32 p80211_isvalid_bitarray( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf );
+u32 p80211_isvalid_bitarray( struct catlistitem *metalist, u32 did, u8 *itembuf );
 
 /*-- MACARRAY --------------------------------------------------------*/
-void p80211_totext_macarray( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf, char *textbuf );
+void p80211_totext_macarray( struct catlistitem *metalist, u32 did, u8 *itembuf, char *textbuf );
 
-void p80211_fromtext_macarray( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf, char *textbuf );
+void p80211_fromtext_macarray( struct catlistitem *metalist, u32 did, u8 *itembuf, char *textbuf );
 
 /* function that checks validity of a MAC address array's value */
-UINT32 p80211_isvalid_macarray( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf );
+u32 p80211_isvalid_macarray( struct catlistitem *metalist, u32 did, u8 *itembuf );
 
 /*-- MIBATTRIUBTE ------------------------------------------------------*/
 /* <mibvalue> ==> <textual representation identified in MIB metadata> */
-void p80211_totext_getmibattribute( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf, char *textbuf );
-void p80211_totext_setmibattribute( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf, char *textbuf );
+void p80211_totext_getmibattribute( struct catlistitem *metalist, u32 did, u8 *itembuf, char *textbuf );
+void p80211_totext_setmibattribute( struct catlistitem *metalist, u32 did, u8 *itembuf, char *textbuf );
 
 
 /* <textual representation identified in MIB metadata> ==> <mibvalue> */
-void p80211_fromtext_getmibattribute( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf, char *textbuf );
-void p80211_fromtext_setmibattribute( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf, char *textbuf );
+void p80211_fromtext_getmibattribute( struct catlistitem *metalist, u32 did, u8 *itembuf, char *textbuf );
+void p80211_fromtext_setmibattribute( struct catlistitem *metalist, u32 did, u8 *itembuf, char *textbuf );
 
 /* function that checks validity of a mibitem's binary value */
-UINT32 p80211_isvalid_getmibattribute( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf );
-UINT32 p80211_isvalid_setmibattribute( struct catlistitem *metalist, UINT32 did, UINT8 *itembuf );
+u32 p80211_isvalid_getmibattribute( struct catlistitem *metalist, u32 did, u8 *itembuf );
+u32 p80211_isvalid_setmibattribute( struct catlistitem *metalist, u32 did, u8 *itembuf );
 
 #endif /* _P80211TYPES_H */
 
diff --git a/drivers/staging/wlan-ng/p80211wep.c b/drivers/staging/wlan-ng/p80211wep.c
index 11a50c7fbfc..46a2a6b3bdc 100644
--- a/drivers/staging/wlan-ng/p80211wep.c
+++ b/drivers/staging/wlan-ng/p80211wep.c
@@ -56,7 +56,6 @@
 #include <linux/slab.h>
 #include <linux/random.h>
 
-#include "version.h"
 #include "wlan_compat.h"
 
 // #define WEP_DEBUG
@@ -73,7 +72,7 @@
 /*================================================================*/
 /* Local Constants */
 
-#define SSWAP(a,b) {UINT8 tmp = s[a]; s[a] = s[b]; s[b] = tmp;}
+#define SSWAP(a,b) {u8 tmp = s[a]; s[a] = s[b]; s[b] = tmp;}
 #define WEP_KEY(x)       (((x) & 0xC0) >> 6)
 
 /*================================================================*/
@@ -87,7 +86,7 @@
 /*================================================================*/
 /* Local Static Definitions */
 
-static const UINT32 wep_crc32_table[256] = {
+static const u32 wep_crc32_table[256] = {
         0x00000000L, 0x77073096L, 0xee0e612cL, 0x990951baL, 0x076dc419L,
         0x706af48fL, 0xe963a535L, 0x9e6495a3L, 0x0edb8832L, 0x79dcb8a4L,
         0xe0d5e91eL, 0x97d2d988L, 0x09b64c2bL, 0x7eb17cbdL, 0xe7b82d07L,
@@ -150,7 +149,7 @@ static const UINT32 wep_crc32_table[256] = {
 
 /* keylen in bytes! */
 
-int wep_change_key(wlandevice_t *wlandev, int keynum, UINT8* key, int keylen)
+int wep_change_key(wlandevice_t *wlandev, int keynum, u8* key, int keylen)
 {
 	if (keylen < 0)  return -1;
 	if (keylen >= MAX_KEYLEN) return -1;
@@ -173,11 +172,11 @@ int wep_change_key(wlandevice_t *wlandev, int keynum, UINT8* key, int keylen)
   4-byte IV at start of buffer, 4-byte ICV at end of buffer.
   if successful, buf start is payload begin, length -= 8;
  */
-int wep_decrypt(wlandevice_t *wlandev, UINT8 *buf, UINT32 len, int key_override, UINT8 *iv, UINT8 *icv)
+int wep_decrypt(wlandevice_t *wlandev, u8 *buf, u32 len, int key_override, u8 *iv, u8 *icv)
 {
-	UINT32 i, j, k, crc, keylen;
-	UINT8 s[256], key[64], c_crc[4];
-	UINT8 keyidx;
+	u32 i, j, k, crc, keylen;
+	u8 s[256], key[64], c_crc[4];
+	u8 keyidx;
 
 	/* Needs to be at least 8 bytes of payload */
 	if (len <= 0) return -1;
@@ -245,10 +244,10 @@ int wep_decrypt(wlandevice_t *wlandev, UINT8 *buf, UINT32 len, int key_override,
 }
 
 /* encrypts in-place. */
-int wep_encrypt(wlandevice_t *wlandev, UINT8 *buf, UINT8 *dst, UINT32 len, int keynum, UINT8 *iv, UINT8 *icv)
+int wep_encrypt(wlandevice_t *wlandev, u8 *buf, u8 *dst, u32 len, int keynum, u8 *iv, u8 *icv)
 {
-	UINT32 i, j, k, crc, keylen;
-	UINT8 s[256], key[64];
+	u32 i, j, k, crc, keylen;
+	u8 s[256], key[64];
 
 	/* no point in WEPping an empty frame */
 	if (len <= 0) return -1;
diff --git a/drivers/staging/wlan-ng/p80211wext.c b/drivers/staging/wlan-ng/p80211wext.c
index b2c9ea25fa4..0d570f1f378 100644
--- a/drivers/staging/wlan-ng/p80211wext.c
+++ b/drivers/staging/wlan-ng/p80211wext.c
@@ -38,7 +38,6 @@
 /* System Includes */
 
 
-#include <linux/version.h>
 
 #include <linux/kernel.h>
 #include <linux/sched.h>
@@ -47,9 +46,7 @@
 #include <linux/netdevice.h>
 #include <linux/etherdevice.h>
 #include <linux/wireless.h>
-#if WIRELESS_EXT > 12
 #include <net/iw_handler.h>
-#endif
 #include <linux/if_arp.h>
 #include <asm/bitops.h>
 #include <asm/uaccess.h>
@@ -58,7 +55,6 @@
 /*================================================================*/
 /* Project Includes */
 
-#include "version.h"
 #include "wlan_compat.h"
 
 #include "p80211types.h"
@@ -78,10 +74,8 @@ static int p80211wext_giwrate(netdevice_t *dev,
 static int p80211wext_giwessid(netdevice_t *dev,
 			       struct iw_request_info *info,
 			       struct iw_point *data, char *essid);
-/* compatibility to wireless extensions */
-#ifdef WIRELESS_EXT
 
-static UINT8 p80211_mhz_to_channel(UINT16 mhz)
+static u8 p80211_mhz_to_channel(u16 mhz)
 {
 	if (mhz >= 5000) {
 		return ((mhz - 5000) / 5);
@@ -97,7 +91,7 @@ static UINT8 p80211_mhz_to_channel(UINT16 mhz)
 	return 0;
 }
 
-static UINT16 p80211_channel_to_mhz(UINT8 ch, int dot11a)
+static u16 p80211_channel_to_mhz(u8 ch, int dot11a)
 {
 
 	if (ch == 0)
@@ -128,7 +122,7 @@ static const long p80211wext_channel_freq[] = {
 	2412, 2417, 2422, 2427, 2432, 2437, 2442,
 	2447, 2452, 2457, 2462, 2467, 2472, 2484
 };
-#define NUM_CHANNELS (sizeof(p80211wext_channel_freq) / sizeof(p80211wext_channel_freq[0]))
+#define NUM_CHANNELS ARRAY_SIZE(p80211wext_channel_freq)
 
 /* steal a spare bit to store the shared/opensystems state. should default to open if not set */
 #define HOSTWEP_SHAREDKEY BIT3
@@ -147,7 +141,7 @@ static int qual_as_percent(int snr ) {
 
 
 
-static int p80211wext_dorequest(wlandevice_t *wlandev, UINT32 did, UINT32 data)
+static int p80211wext_dorequest(wlandevice_t *wlandev, u32 did, u32 data)
 {
 	p80211msg_dot11req_mibset_t	msg;
 	p80211item_uint32_t		mibitem;
@@ -159,7 +153,7 @@ static int p80211wext_dorequest(wlandevice_t *wlandev, UINT32 did, UINT32 data)
 	mibitem.did = did;
 	mibitem.data = data;
 	memcpy(&msg.mibattribute.data, &mibitem, sizeof(mibitem));
-	result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+	result = p80211req_dorequest(wlandev, (u8*)&msg);
 
 	DBFEXIT;
 	return result;
@@ -200,7 +194,7 @@ static int p80211wext_autojoin(wlandevice_t *wlandev)
 	memcpy(msg.ssid.data.data, ssid, data.length);
 	msg.ssid.data.len = data.length;
 
-	result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+	result = p80211req_dorequest(wlandev, (u8*)&msg);
 
 	if (result) {
 		err = -EFAULT;
@@ -245,20 +239,14 @@ struct iw_statistics* p80211wext_get_wireless_stats (netdevice_t *dev)
 	wstats->qual.level = quality.level.data;  /* instant signal level */
 	wstats->qual.noise = quality.noise.data;  /* instant noise level */
 
-#if WIRELESS_EXT > 18
 	wstats->qual.updated = IW_QUAL_ALL_UPDATED | IW_QUAL_DBM;
-#else
-	wstats->qual.updated = 7;
-#endif
 	wstats->discard.code = wlandev->rx.decrypt_err;
 	wstats->discard.nwid = 0;
 	wstats->discard.misc = 0;
 
-#if WIRELESS_EXT > 11
 	wstats->discard.fragment = 0;  // incomplete fragments
 	wstats->discard.retries = 0;   // tx retries.
 	wstats->miss.beacon = 0;
-#endif
 
 	DBFEXIT;
 
@@ -312,7 +300,7 @@ static int p80211wext_giwfreq(netdevice_t *dev,
 	msg.msgcode = DIDmsg_dot11req_mibget;
 	mibitem.did = DIDmib_dot11phy_dot11PhyDSSSTable_dot11CurrentChannel;
 	memcpy(&msg.mibattribute.data, &mibitem, sizeof(mibitem));
-	result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+	result = p80211req_dorequest(wlandev, (u8*)&msg);
 
 	if (result) {
 		err = -EFAULT;
@@ -362,7 +350,7 @@ static int p80211wext_siwfreq(netdevice_t *dev,
 		mibitem.data = p80211_mhz_to_channel(freq->m);
 
 	memcpy(&msg.mibattribute.data, &mibitem, sizeof(mibitem));
-	result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+	result = p80211req_dorequest(wlandev, (u8*)&msg);
 
 	if (result) {
 		err = -EFAULT;
@@ -374,8 +362,6 @@ static int p80211wext_siwfreq(netdevice_t *dev,
 	return err;
 }
 
-#if WIRELESS_EXT > 8
-
 static int p80211wext_giwmode(netdevice_t *dev,
 			      struct iw_request_info *info,
 			      __u32 *mode, char *extra)
@@ -447,14 +433,11 @@ static int p80211wext_siwmode(netdevice_t *dev,
 	}
 
 	/* Set Operation mode to the PORT TYPE RID */
-
-#warning "get rid of p2mib here"
-
 	msg.msgcode = DIDmsg_dot11req_mibset;
 	mibitem.did = DIDmib_p2_p2Static_p2CnfPortType;
 	mibitem.data = (*mode == IW_MODE_ADHOC) ? 0 : 1;
 	memcpy(&msg.mibattribute.data, &mibitem, sizeof(mibitem));
-	result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+	result = p80211req_dorequest(wlandev, (u8*)&msg);
 
 	if (result)
 		err = -EFAULT;
@@ -479,12 +462,9 @@ static int p80211wext_giwrange(netdevice_t *dev,
 	data->length = sizeof(*range);
 	memset(range,0,sizeof(*range));
 
-#if WIRELESS_EXT > 9
 	range->txpower_capa = IW_TXPOW_DBM;
 	// XXX what about min/max_pmp, min/max_pmt, etc.
-#endif
 
-#if WIRELESS_EXT > 10
 	range->we_version_compiled = WIRELESS_EXT;
 	range->we_version_source = 13;
 
@@ -492,16 +472,13 @@ static int p80211wext_giwrange(netdevice_t *dev,
 	range->retry_flags = IW_RETRY_LIMIT;
 	range->min_retry = 0;
 	range->max_retry = 255;
-#endif /* WIRELESS_EXT > 10 */
 
-#if WIRELESS_EXT > 16
         range->event_capa[0] = (IW_EVENT_CAPA_K_0 |  //mode/freq/ssid
                                 IW_EVENT_CAPA_MASK(SIOCGIWAP) |
                                 IW_EVENT_CAPA_MASK(SIOCGIWSCAN));
         range->event_capa[1] = IW_EVENT_CAPA_K_1;  //encode
         range->event_capa[4] = (IW_EVENT_CAPA_MASK(IWEVQUAL) |
                                 IW_EVENT_CAPA_MASK(IWEVCUSTOM) );
-#endif
 
 	range->num_channels = NUM_CHANNELS;
 
@@ -543,7 +520,6 @@ static int p80211wext_giwrange(netdevice_t *dev,
 	DBFEXIT;
 	return 0;
 }
-#endif
 
 static int p80211wext_giwap(netdevice_t *dev,
 			    struct iw_request_info *info,
@@ -561,7 +537,6 @@ static int p80211wext_giwap(netdevice_t *dev,
 	return 0;
 }
 
-#if WIRELESS_EXT > 8
 static int p80211wext_giwencode(netdevice_t *dev,
 				struct iw_request_info *info,
 				struct iw_point *erq, char *key)
@@ -572,10 +547,13 @@ static int p80211wext_giwencode(netdevice_t *dev,
 
 	DBFENTER;
 
+	i = (erq->flags & IW_ENCODE_INDEX) - 1;
+	erq->flags = 0;
+
 	if (wlandev->hostwep & HOSTWEP_PRIVACYINVOKED)
-		erq->flags = IW_ENCODE_ENABLED;
+		erq->flags |= IW_ENCODE_ENABLED;
 	else
-		erq->flags = IW_ENCODE_DISABLED;
+		erq->flags |= IW_ENCODE_DISABLED;
 
 	if (wlandev->hostwep & HOSTWEP_EXCLUDEUNENCRYPTED)
 		erq->flags |= IW_ENCODE_RESTRICTED;
@@ -613,7 +591,6 @@ static int p80211wext_siwencode(netdevice_t *dev,
 
 	int err = 0;
 	int result = 0;
-	int enable = 0;
 	int i;
 
 	DBFENTER;
@@ -632,23 +609,23 @@ static int p80211wext_siwencode(netdevice_t *dev,
 		else
 			i--;
 
-		result = p80211wext_dorequest(wlandev, DIDmib_dot11smt_dot11PrivacyTable_dot11WEPDefaultKeyID, i);
+		/* Set current key number only if no keys are given */
+		if (erq->flags & IW_ENCODE_NOKEY) {
+			result = p80211wext_dorequest(wlandev, DIDmib_dot11smt_dot11PrivacyTable_dot11WEPDefaultKeyID, i);
 
-		if (result) {
-			err = -EFAULT;
-			goto exit;
-		}
-		else {
-			enable = 1;
+			if (result) {
+				err = -EFAULT;
+				goto exit;
+			}
 		}
 
-	}
-	else {
-		// Do not thing when no Key Index
+	} else {
+		// Use defaultkey if no Key Index
+		i = wlandev->hostwep & HOSTWEP_DEFAULTKEY_MASK;
 	}
 
 	/* Check if there is no key information in the iwconfig request */
-	if((erq->flags & IW_ENCODE_NOKEY) == 0 && enable == 1) {
+	if((erq->flags & IW_ENCODE_NOKEY) == 0 ) {
 
 		/*------------------------------------------------------------
 		 * If there is WEP Key for setting, check the Key Information
@@ -690,7 +667,7 @@ static int p80211wext_siwencode(netdevice_t *dev,
 
 			msg.msgcode = DIDmsg_dot11req_mibset;
 			memcpy(&msg.mibattribute.data, &pstr, sizeof(pstr));
-			result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+			result = p80211req_dorequest(wlandev, (u8*)&msg);
 
 			if (result) {
 				err = -EFAULT;
@@ -703,8 +680,7 @@ static int p80211wext_siwencode(netdevice_t *dev,
 	/* Check the PrivacyInvoked flag */
 	if (erq->flags & IW_ENCODE_DISABLED) {
 		result = p80211wext_dorequest(wlandev, DIDmib_dot11smt_dot11PrivacyTable_dot11PrivacyInvoked, P80211ENUM_truth_false);
-	}
-	else if((erq->flags & IW_ENCODE_ENABLED) || enable == 1) {
+	} else {
 		result = p80211wext_dorequest(wlandev, DIDmib_dot11smt_dot11PrivacyTable_dot11PrivacyInvoked, P80211ENUM_truth_true);
 	}
 
@@ -713,7 +689,13 @@ static int p80211wext_siwencode(netdevice_t *dev,
 		goto exit;
 	}
 
-	/* Check the ExcludeUnencrypted flag */
+	/*  The  security  mode  may  be open or restricted, and its meaning
+	    depends on the card used. With  most  cards,  in  open  mode  no
+	    authentication  is  used  and  the  card  may  also  accept non-
+	    encrypted sessions, whereas in restricted  mode  only  encrypted
+	    sessions  are  accepted  and the card will use authentication if
+	    available.
+	*/
 	if (erq->flags & IW_ENCODE_RESTRICTED) {
 		result = p80211wext_dorequest(wlandev, DIDmib_dot11smt_dot11PrivacyTable_dot11ExcludeUnencrypted, P80211ENUM_truth_true);
 	}
@@ -798,7 +780,7 @@ static int p80211wext_siwessid(netdevice_t *dev,
 	msg.ssid.data.len = length;
 
 	WLAN_LOG_DEBUG(1,"autojoin_ssid for %s \n",essid);
-	result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+	result = p80211req_dorequest(wlandev, (u8*)&msg);
         WLAN_LOG_DEBUG(1,"autojoin_ssid %d\n",result);
 
 	if (result) {
@@ -850,7 +832,7 @@ static int p80211wext_giwrate(netdevice_t *dev,
 	msg.msgcode = DIDmsg_dot11req_mibget;
 	mibitem.did = DIDmib_p2_p2MAC_p2CurrentTxRate;
 	memcpy(&msg.mibattribute.data, &mibitem, sizeof(mibitem));
-	result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+	result = p80211req_dorequest(wlandev, (u8*)&msg);
 
 	if (result) {
 		err = -EFAULT;
@@ -863,10 +845,10 @@ static int p80211wext_giwrate(netdevice_t *dev,
 	rrq->disabled = 0;
 	rrq->value = 0;
 
-#define		HFA384x_RATEBIT_1			((UINT16)1)
-#define		HFA384x_RATEBIT_2			((UINT16)2)
-#define		HFA384x_RATEBIT_5dot5			((UINT16)4)
-#define		HFA384x_RATEBIT_11			((UINT16)8)
+#define		HFA384x_RATEBIT_1			((u16)1)
+#define		HFA384x_RATEBIT_2			((u16)2)
+#define		HFA384x_RATEBIT_5dot5			((u16)4)
+#define		HFA384x_RATEBIT_11			((u16)8)
 
 	switch (mibitem.data) {
 	case HFA384x_RATEBIT_1:
@@ -904,7 +886,7 @@ static int p80211wext_giwrts(netdevice_t *dev,
 	msg.msgcode = DIDmsg_dot11req_mibget;
 	mibitem.did = DIDmib_dot11mac_dot11OperationTable_dot11RTSThreshold;
 	memcpy(&msg.mibattribute.data, &mibitem, sizeof(mibitem));
-	result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+	result = p80211req_dorequest(wlandev, (u8*)&msg);
 
 	if (result) {
 		err = -EFAULT;
@@ -948,7 +930,7 @@ static int p80211wext_siwrts(netdevice_t *dev,
 		mibitem.data = rts->value;
 
 	memcpy(&msg.mibattribute.data, &mibitem, sizeof(mibitem));
-	result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+	result = p80211req_dorequest(wlandev, (u8*)&msg);
 
 	if (result) {
 		err = -EFAULT;
@@ -975,7 +957,7 @@ static int p80211wext_giwfrag(netdevice_t *dev,
 	msg.msgcode = DIDmsg_dot11req_mibget;
 	mibitem.did = DIDmib_dot11mac_dot11OperationTable_dot11FragmentationThreshold;
 	memcpy(&msg.mibattribute.data, &mibitem, sizeof(mibitem));
-	result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+	result = p80211req_dorequest(wlandev, (u8*)&msg);
 
 	if (result) {
 		err = -EFAULT;
@@ -1019,7 +1001,7 @@ static int p80211wext_siwfrag(netdevice_t *dev,
 		mibitem.data = frag->value;
 
 	memcpy(&msg.mibattribute.data, &mibitem, sizeof(mibitem));
-	result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+	result = p80211req_dorequest(wlandev, (u8*)&msg);
 
 	if (result) {
 		err = -EFAULT;
@@ -1031,10 +1013,6 @@ static int p80211wext_siwfrag(netdevice_t *dev,
 	return err;
 }
 
-#endif  /* WIRELESS_EXT > 8 */
-
-#if WIRELESS_EXT > 10
-
 #ifndef IW_RETRY_LONG
 #define IW_RETRY_LONG IW_RETRY_MAX
 #endif
@@ -1052,7 +1030,7 @@ static int p80211wext_giwretry(netdevice_t *dev,
 	p80211msg_dot11req_mibset_t     msg;
 	int result;
 	int err = 0;
-	UINT16 shortretry, longretry, lifetime;
+	u16 shortretry, longretry, lifetime;
 
 	DBFENTER;
 
@@ -1060,7 +1038,7 @@ static int p80211wext_giwretry(netdevice_t *dev,
 	mibitem.did = DIDmib_dot11mac_dot11OperationTable_dot11ShortRetryLimit;
 
 	memcpy(&msg.mibattribute.data, &mibitem, sizeof(mibitem));
-	result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+	result = p80211req_dorequest(wlandev, (u8*)&msg);
 
 	if (result) {
 		err = -EFAULT;
@@ -1074,7 +1052,7 @@ static int p80211wext_giwretry(netdevice_t *dev,
 	mibitem.did = DIDmib_dot11mac_dot11OperationTable_dot11LongRetryLimit;
 
 	memcpy(&msg.mibattribute.data, &mibitem, sizeof(mibitem));
-	result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+	result = p80211req_dorequest(wlandev, (u8*)&msg);
 
 	if (result) {
 		err = -EFAULT;
@@ -1088,7 +1066,7 @@ static int p80211wext_giwretry(netdevice_t *dev,
 	mibitem.did = DIDmib_dot11mac_dot11OperationTable_dot11MaxTransmitMSDULifetime;
 
 	memcpy(&msg.mibattribute.data, &mibitem, sizeof(mibitem));
-	result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+	result = p80211req_dorequest(wlandev, (u8*)&msg);
 
 	if (result) {
 		err = -EFAULT;
@@ -1151,7 +1129,7 @@ static int p80211wext_siwretry(netdevice_t *dev,
 		mibitem.data =  rrq->value /= 1024;
 
 		memcpy(&msg.mibattribute.data, &mibitem, sizeof(mibitem));
-		result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+		result = p80211req_dorequest(wlandev, (u8*)&msg);
 
 		if (result) {
 			err = -EFAULT;
@@ -1163,7 +1141,7 @@ static int p80211wext_siwretry(netdevice_t *dev,
 			mibitem.data = rrq->value;
 
 			memcpy(&msg.mibattribute.data, &mibitem, sizeof(mibitem));
-			result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+			result = p80211req_dorequest(wlandev, (u8*)&msg);
 
 			if (result) {
 				err = -EFAULT;
@@ -1176,7 +1154,7 @@ static int p80211wext_siwretry(netdevice_t *dev,
 			mibitem.data = rrq->value;
 
 			memcpy(&msg.mibattribute.data, &mibitem, sizeof(mibitem));
-			result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+			result = p80211req_dorequest(wlandev, (u8*)&msg);
 
 			if (result) {
 				err = -EFAULT;
@@ -1191,9 +1169,6 @@ static int p80211wext_siwretry(netdevice_t *dev,
 
 }
 
-#endif /* WIRELESS_EXT > 10 */
-
-#if WIRELESS_EXT > 9
 static int p80211wext_siwtxpow(netdevice_t *dev,
                                struct iw_request_info *info,
                                struct iw_param *rrq, char *extra)
@@ -1212,22 +1187,13 @@ static int p80211wext_siwtxpow(netdevice_t *dev,
         }
 
         msg.msgcode = DIDmsg_dot11req_mibset;
-
-        switch (rrq->value) {
-
-          case 1 : mibitem.did = DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel1; break;
-          case 2 : mibitem.did = DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel2; break;
-          case 3 : mibitem.did = DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel3; break;
-          case 4 : mibitem.did = DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel4; break;
-          case 5 : mibitem.did = DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel5; break;
-          case 6 : mibitem.did = DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel6; break;
-          case 7 : mibitem.did = DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel7; break;
-          case 8 : mibitem.did = DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel8; break;
-          default: mibitem.did = DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel8; break;
-	}
-
+	mibitem.did = DIDmib_dot11phy_dot11PhyTxPowerTable_dot11CurrentTxPowerLevel;
+	if (rrq->fixed == 0)
+	  mibitem.data = 30;
+	else
+	  mibitem.data = rrq->value;
         memcpy(&msg.mibattribute.data, &mibitem, sizeof(mibitem));
-        result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+        result = p80211req_dorequest(wlandev, (u8*)&msg);
 
         if (result) {
                 err = -EFAULT;
@@ -1255,7 +1221,7 @@ static int p80211wext_giwtxpow(netdevice_t *dev,
 	mibitem.did = DIDmib_dot11phy_dot11PhyTxPowerTable_dot11CurrentTxPowerLevel;
 
 	memcpy(&msg.mibattribute.data, &mibitem, sizeof(mibitem));
-	result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+	result = p80211req_dorequest(wlandev, (u8*)&msg);
 
 	if (result) {
 		err = -EFAULT;
@@ -1275,7 +1241,6 @@ static int p80211wext_giwtxpow(netdevice_t *dev,
 	DBFEXIT;
 	return err;
 }
-#endif /* WIRELESS_EXT > 9 */
 
 static int p80211wext_siwspy(netdevice_t *dev,
 			     struct iw_request_info *info,
@@ -1373,7 +1338,6 @@ static int prism2_result2err (int prism2_result)
 	return err;
 }
 
-#if WIRELESS_EXT > 13
 static int p80211wext_siwscan(netdevice_t *dev,
 			     struct iw_request_info *info,
 			     struct iw_point *srq, char *extra)
@@ -1409,7 +1373,7 @@ static int p80211wext_siwscan(netdevice_t *dev,
 	msg.maxchanneltime.data = 250;
 	msg.minchanneltime.data = 200;
 
-	result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+	result = p80211req_dorequest(wlandev, (u8*)&msg);
 	if (result)
 		err = prism2_result2err (msg.resultcode.data);
 
@@ -1520,7 +1484,7 @@ static int p80211wext_giwscan(netdevice_t *dev,
 		msg.msgcode = DIDmsg_dot11req_scan_results;
 		msg.bssindex.data = i;
 
-		result = p80211req_dorequest(wlandev, (UINT8*)&msg);
+		result = p80211req_dorequest(wlandev, (u8*)&msg);
 		if ((result != 0) ||
 		    (msg.resultcode.data != P80211ENUM_resultcode_success)) {
 			break;
@@ -1540,12 +1504,10 @@ static int p80211wext_giwscan(netdevice_t *dev,
 	DBFEXIT;
 	return err;
 }
-#endif
 
 /*****************************************************/
 //extra wireless extensions stuff to support NetworkManager (I hope)
 
-#if WIRELESS_EXT > 17
 /* SIOCSIWENCODEEXT */
 static int p80211wext_set_encodeext(struct net_device *dev,
 				struct iw_request_info *info,
@@ -1580,7 +1542,7 @@ static int p80211wext_set_encodeext(struct net_device *dev,
 
 
   if ( ext->ext_flags & IW_ENCODE_EXT_SET_TX_KEY ) {
-    if ( ! ext->alg & IW_ENCODE_ALG_WEP) {
+    if (!(ext->alg & IW_ENCODE_ALG_WEP)) {
       WLAN_LOG_DEBUG(1,"asked to set a non wep key :(");
       return -EINVAL;
     }
@@ -1615,7 +1577,7 @@ static int p80211wext_set_encodeext(struct net_device *dev,
       break;
     }
     msg.msgcode = DIDmsg_dot11req_mibset;
-    result = p80211req_dorequest(wlandev,(UINT8*)&msg);
+    result = p80211req_dorequest(wlandev,(u8*)&msg);
     WLAN_LOG_DEBUG(1,"result (%d)\n",result);
   }
   return result;
@@ -1763,26 +1725,6 @@ static int p80211_wext_get_iwauth (struct net_device *dev,
   return result;
 }
 
-
-#endif
-
-
-
-
-
-
-/*****************************************************/
-
-
-
-
-
-/*
-typedef int (*iw_handler)(netdevice_t *dev, struct iw_request_info *info,
-                          union iwreq_data *wrqu, char *extra);
-*/
-
-#if WIRELESS_EXT > 12
 static iw_handler p80211wext_handlers[] =  {
 	(iw_handler) p80211wext_siwcommit,		/* SIOCSIWCOMMIT */
 	(iw_handler) p80211wext_giwname,		/* SIOCGIWNAME */
@@ -1808,13 +1750,8 @@ static iw_handler p80211wext_handlers[] =  {
 	(iw_handler) p80211wext_giwap,         		/* SIOCGIWAP */
 	(iw_handler) NULL,				/* -- hole -- */
 	(iw_handler) NULL,                  		/* SIOCGIWAPLIST */
-#if WIRELESS_EXT > 13
-	(iw_handler) p80211wext_siwscan,			/* SIOCSIWSCAN */
-	(iw_handler) p80211wext_giwscan,			/* SIOCGIWSCAN */
-#else /* WIRELESS_EXT > 13 */
-	(iw_handler) NULL,	/* null */		/* SIOCSIWSCAN */
-	(iw_handler) NULL,	/* null */		/* SIOCGIWSCAN */
-#endif /* WIRELESS_EXT > 13 */
+	(iw_handler) p80211wext_siwscan,		/* SIOCSIWSCAN */
+	(iw_handler) p80211wext_giwscan,		/* SIOCGIWSCAN */
 	(iw_handler) p80211wext_siwessid,  		/* SIOCSIWESSID */
 	(iw_handler) p80211wext_giwessid,      		/* SIOCGIWESSID */
 	(iw_handler) NULL,                 		/* SIOCSIWNICKN */
@@ -1835,9 +1772,7 @@ static iw_handler p80211wext_handlers[] =  {
 	(iw_handler) p80211wext_giwencode,  		/* SIOCGIWENCODE */
 	(iw_handler) NULL,                 		/* SIOCSIWPOWER */
 	(iw_handler) NULL,                  		/* SIOCGIWPOWER */
-#if WIRELESS_EXT > 17
 /* WPA operations */
-
 	(iw_handler) NULL,				/* -- hole -- */
 	(iw_handler) NULL,				/* -- hole -- */
 	(iw_handler) NULL, /* SIOCSIWGENIE	set generic IE */
@@ -1848,170 +1783,18 @@ static iw_handler p80211wext_handlers[] =  {
 	(iw_handler) p80211wext_set_encodeext, /* SIOCSIWENCODEEXT  set encoding token & mode */
 	(iw_handler) p80211wext_get_encodeext, /* SIOCGIWENCODEEXT  get encoding token & mode */
 	(iw_handler) NULL, /* SIOCSIWPMKSA	PMKSA cache operation */
-#endif
 };
 
 struct iw_handler_def p80211wext_handler_def = {
-	.num_standard = sizeof(p80211wext_handlers) / sizeof(iw_handler),
+	.num_standard = ARRAY_SIZE(p80211wext_handlers),
 	.num_private = 0,
 	.num_private_args = 0,
         .standard = p80211wext_handlers,
 	.private = NULL,
 	.private_args = NULL,
-#if WIRELESS_EXT > 16
 	.get_wireless_stats = p80211wext_get_wireless_stats
-#endif
 };
 
-#endif
-
-/* wireless extensions' ioctls */
-int p80211wext_support_ioctl(netdevice_t *dev, struct ifreq *ifr, int cmd)
-{
-	wlandevice_t *wlandev = dev->ml_priv;
-
-#if WIRELESS_EXT < 13
-	struct iwreq *iwr = (struct iwreq*)ifr;
-#endif
-
-	p80211item_uint32_t             mibitem;
-	int err = 0;
-
-	DBFENTER;
-
-	mibitem.status = P80211ENUM_msgitem_status_data_ok;
-
-	if ( wlandev->msdstate != WLAN_MSD_RUNNING ) {
-		err = -ENODEV;
-		goto exit;
-	}
-
-	WLAN_LOG_DEBUG(1, "Received wireless extension ioctl #%d.\n", cmd);
-
-	switch (cmd) {
-#if WIRELESS_EXT < 13
-	case SIOCSIWNAME:  /* unused  */
-		err = (-EOPNOTSUPP);
-		break;
-	case SIOCGIWNAME: /* get name == wireless protocol */
-                err = p80211wext_giwname(dev, NULL, (char *) &iwr->u, NULL);
-		break;
-	case SIOCSIWNWID:
-	case SIOCGIWNWID:
-		err = (-EOPNOTSUPP);
-		break;
-	case SIOCSIWFREQ: /* set channel */
-                err = p80211wext_siwfreq(dev, NULL, &(iwr->u.freq), NULL);
-		break;
-	case SIOCGIWFREQ: /* get channel */
-                err = p80211wext_giwfreq(dev, NULL, &(iwr->u.freq), NULL);
-		break;
-	case SIOCSIWRANGE:
-	case SIOCSIWPRIV:
-	case SIOCSIWAP: /* set access point MAC addresses (BSSID) */
-		err = (-EOPNOTSUPP);
-		break;
-
-	case SIOCGIWAP:	/* get access point MAC addresses (BSSID) */
-                err = p80211wext_giwap(dev, NULL, &(iwr->u.ap_addr), NULL);
-		break;
-
-#if WIRELESS_EXT > 8
-	case SIOCSIWMODE: /* set operation mode */
-	case SIOCSIWESSID: /* set SSID (network name) */
-	case SIOCSIWRATE: /* set default bit rate (bps) */
-		err = (-EOPNOTSUPP);
-		break;
-
-	case SIOCGIWMODE: /* get operation mode */
-		err = p80211wext_giwmode(dev, NULL, &iwr->u.mode, NULL);
-
-		break;
-	case SIOCGIWNICKN: /* get node name/nickname */
-	case SIOCGIWESSID: /* get SSID */
-		if(iwr->u.essid.pointer) {
-                        char ssid[IW_ESSID_MAX_SIZE+1];
-			memset(ssid, 0, sizeof(ssid));
-
-			err = p80211wext_giwessid(dev, NULL, &iwr->u.essid, ssid);
-			if(copy_to_user(iwr->u.essid.pointer, ssid, sizeof(ssid)))
-				err = (-EFAULT);
-		}
-		break;
-	case SIOCGIWRATE:
-                err = p80211wext_giwrate(dev, NULL, &iwr->u.bitrate, NULL);
-		break;
-	case SIOCGIWRTS:
-		err = p80211wext_giwrts(dev, NULL, &iwr->u.rts, NULL);
-		break;
-	case SIOCGIWFRAG:
-		err = p80211wext_giwfrag(dev, NULL, &iwr->u.rts, NULL);
-		break;
-	case SIOCGIWENCODE:
-		if (!capable(CAP_NET_ADMIN))
-			err = -EPERM;
-		else if (iwr->u.encoding.pointer) {
-			char keybuf[MAX_KEYLEN];
-			err = p80211wext_giwencode(dev, NULL,
-						     &iwr->u.encoding, keybuf);
-			if (copy_to_user(iwr->u.encoding.pointer, keybuf,
-					 iwr->u.encoding.length))
-				err = -EFAULT;
-		}
-		break;
-	case SIOCGIWAPLIST:
-	case SIOCSIWRTS:
-	case SIOCSIWFRAG:
-	case SIOCSIWSENS:
-	case SIOCGIWSENS:
-	case SIOCSIWNICKN: /* set node name/nickname */
-	case SIOCSIWENCODE: /* set encoding token & mode */
-	case SIOCSIWSPY:
-	case SIOCGIWSPY:
-	case SIOCSIWPOWER:
-	case SIOCGIWPOWER:
-	case SIOCGIWPRIV:
-		err = (-EOPNOTSUPP);
-		break;
-	case SIOCGIWRANGE:
-		if(iwr->u.data.pointer != NULL) {
-                        struct iw_range range;
-                        err = p80211wext_giwrange(dev, NULL, &iwr->u.data,
-						  (char *) &range);
-			/* Push that up to the caller */
-			if (copy_to_user(iwr->u.data.pointer, &range, sizeof(range)))
-				err = -EFAULT;
-		}
-		break;
-#endif /* WIRELESS_EXT > 8 */
-#if WIRELESS_EXT > 9
-	case SIOCSIWTXPOW:
-		err = (-EOPNOTSUPP);
-		break;
-	case SIOCGIWTXPOW:
-		err = p80211wext_giwtxpow(dev, NULL, &iwr->u.txpower, NULL);
-		break;
-#endif /* WIRELESS_EXT > 9 */
-#if WIRELESS_EXT > 10
-	case SIOCSIWRETRY:
-		err = (-EOPNOTSUPP);
-		break;
-	case SIOCGIWRETRY:
-		err = p80211wext_giwretry(dev, NULL, &iwr->u.retry, NULL);
-		break;
-#endif /* WIRELESS_EXT > 10 */
-
-#endif /* WIRELESS_EXT <= 12 */
-
-	default:
-		err = (-EOPNOTSUPP);
-		break;
-	}
-
- exit:
-	DBFEXIT;
-	return (err);
-}
 
 int p80211wext_event_associated(wlandevice_t *wlandev, int assoc)
 {
@@ -2019,7 +1802,6 @@ int p80211wext_event_associated(wlandevice_t *wlandev, int assoc)
 
         DBFENTER;
 
-#if WIRELESS_EXT > 13
         /* Send the association state first */
         data.ap_addr.sa_family = ARPHRD_ETHER;
         if (assoc) {
@@ -2034,15 +1816,12 @@ int p80211wext_event_associated(wlandevice_t *wlandev, int assoc)
         if (!assoc) goto done;
 
         // XXX send association data, like IEs, etc etc.
-#endif
+
  done:
         DBFEXIT;
         return 0;
 }
 
 
-#endif /* compatibility to wireless extensions */
-
-
 
 
diff --git a/drivers/staging/wlan-ng/prism2_cs.c b/drivers/staging/wlan-ng/prism2_cs.c
deleted file mode 100644
index 63ce5659f21..00000000000
--- a/drivers/staging/wlan-ng/prism2_cs.c
+++ /dev/null
@@ -1,1487 +0,0 @@
-#define WLAN_HOSTIF WLAN_PCMCIA
-#include "hfa384x.c"
-#include "prism2mgmt.c"
-#include "prism2mib.c"
-#include "prism2sta.c"
-
-#if (LINUX_VERSION_CODE <= KERNEL_VERSION(2,4,21) )
-#if (WLAN_CPU_FAMILY == WLAN_Ix86)
-#ifndef CONFIG_ISA
-#warning "You may need to enable ISA support in your kernel."
-#endif
-#endif
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,11) )
-static u_int	irq_mask = 0xdeb8;		/* Interrupt mask */
-static int	irq_list[4] = { -1 };		/* Interrupt list */
-#endif
-static u_int	prism2_ignorevcc=1;		/* Boolean, if set, we
-						 * ignore what the Vcc
-						 * is set to and what the CIS
-						 * says.
-						 */
-module_param( prism2_ignorevcc, int, 0644);
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,11) )
-#if (LINUX_VERSION_CODE <= KERNEL_VERSION(2,6,9))
-static int numlist = 4;
-module_param_array(irq_list, int, numlist, 0444);
-#else
-module_param_array(irq_list, int, NULL, 0444);
-#endif
-module_param( irq_mask, int, 0644);
-#endif
-
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,15)
-static int prism2_cs_suspend(struct pcmcia_device *pdev);
-static int prism2_cs_resume(struct pcmcia_device *pdev);
-static void prism2_cs_remove(struct pcmcia_device *pdev);
-static int prism2_cs_probe(struct pcmcia_device *pdev);
-#else
-dev_link_t	*prism2sta_attach(void);
-static void	prism2sta_detach(dev_link_t *link);
-static void	prism2sta_config(dev_link_t *link);
-static void	prism2sta_release(u_long arg);
-static int 	prism2sta_event (event_t event, int priority, event_callback_args_t *args);
-
-static dev_link_t	*dev_list = NULL;	/* head of instance list */
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,68))
-/*----------------------------------------------------------------
-* cs_error
-*
-* Utility function to print card services error messages.
-*
-* Arguments:
-*	handle	client handle identifying this CS client
-*	func	CS function number that generated the error
-*	ret	CS function return code
-*
-* Returns:
-*	nothing
-* Side effects:
-*
-* Call context:
-*	process thread
-*	interrupt
-----------------------------------------------------------------*/
-static void cs_error(client_handle_t handle, int func, int ret)
-{
-#if (defined(CS_RELEASE_CODE) && (CS_RELEASE_CODE < 0x2911))
-	CardServices(ReportError, dev_info, (void *)func, (void *)ret);
-#else
-	error_info_t err = { func, ret };
-	pcmcia_report_error(handle, &err);
-#endif
-}
-#else // kernel_version
-
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,12)
-static struct pcmcia_device_id prism2_cs_ids[] = {
-	PCMCIA_DEVICE_PROD_ID12("INTERSIL",  "HFA384x/IEEE", 0x74c5e40d, 0xdb472a18), // Intersil PRISM2 Reference Design 11Mb/s 802.11b WLAN Card
-	PCMCIA_DEVICE_MANF_CARD(0x0138, 0x0002), // Compaq WL100/200 11Mb/s 802.11b WLAN Card
-	PCMCIA_DEVICE_MANF_CARD(0x028a, 0x0002), // Compaq iPaq HNW-100 11Mb/s 802.11b WLAN Card
-	PCMCIA_DEVICE_MANF_CARD(0x0250, 0x0002), // Samsung SWL2000-N 11Mb/s 802.11b WLAN Card
-	PCMCIA_DEVICE_MANF_CARD(0xd601, 0x0002), // Z-Com XI300 11Mb/s 802.11b WLAN Card
-	PCMCIA_DEVICE_PROD_ID12("ZoomAir 11Mbps High",  "Rate wireless Networking", 0x273fe3db, 0x32a1eaee), // ZoomAir 4100 11Mb/s 802.11b WLAN Card
-	PCMCIA_DEVICE_PROD_ID123("Instant Wireless ",  " Network PC CARD",  "Version 01.02", 0x11d901af, 0x6e9bd926, 0x4b74baa0), // Linksys WPC11 11Mbps 802.11b WLAN Card
-	PCMCIA_DEVICE_PROD_ID123("Addtron",  "AWP-100 Wireless PCMCIA",  "Version 01.02", 0xe6ec52ce, 0x8649af2, 0x4b74baa0), // Addtron AWP-100 11Mbps 802.11b WLAN Card
-	PCMCIA_DEVICE_PROD_ID123("D",  "Link DWL-650 11Mbps WLAN Card",  "Version 01.02", 0x71b18589, 0xb6f1b0ab, 0x4b74baa0), // D-Link DWL-650 11Mbps 802.11b WLAN Card
-	PCMCIA_DEVICE_PROD_ID123("SMC",  "SMC2632W",  "Version 01.02", 0xc4f8b18b, 0x474a1f2a, 0x4b74baa0), // SMC 2632W 11Mbps 802.11b WLAN Card
-	PCMCIA_DEVICE_PROD_ID1234("Intersil",  "PRISM 2_5 PCMCIA ADAPTER",  "ISL37300P",  "Eval-RevA", 0x4b801a17, 0x6345a0bf, 0xc9049a39, 0xc23adc0e), // BroMax Freeport 11Mbps 802.11b WLAN Card (Prism 2.5)
-	PCMCIA_DEVICE_PROD_ID123("U.S. Robotics",  "IEEE 802.11b PC-CARD",  "Version 01.02", 0xc7b8df9d, 0x1700d087, 0x4b74baa0), // U.S. Robotics IEEE 802.11b PC-CARD
-	PCMCIA_DEVICE_PROD_ID12("Digital Data Communications",  "WPC-0100", 0xfdd73470, 0xe0b6f146), // Level-One WPC-0100
-	PCMCIA_DEVICE_MANF_CARD(0x0274, 0x1612), // Bromax OEM 11Mbps 802.11b WLAN Card (Prism 2.5)
-	PCMCIA_DEVICE_MANF_CARD(0x0274, 0x1613), // Bromax OEM 11Mbps 802.11b WLAN Card (Prism 3)
-	PCMCIA_DEVICE_PROD_ID12("corega K.K.",  "Wireless LAN PCC-11", 0x5261440f, 0xa6405584), // corega K.K. Wireless LAN PCC-11
-	PCMCIA_DEVICE_PROD_ID12("corega K.K.",  "Wireless LAN PCCA-11", 0x5261440f, 0xdf6115f9), // corega K.K. Wireless LAN PCCA-11
-	PCMCIA_DEVICE_MANF_CARD(0xc001, 0x0008), // CONTEC FLEXSCAN/FX-DDS110-PCC
-	PCMCIA_DEVICE_PROD_ID12("PLANEX",  "GeoWave/GW-NS110", 0x209f40ab, 0x46263178), // PLANEX GeoWave/GW-NS110
-	PCMCIA_DEVICE_PROD_ID123("OEM",  "PRISM2 IEEE 802.11 PC-Card",  "Version 01.02", 0xfea54c90, 0x48f2bdd6, 0x4b74baa0), // Ambicom WL1100 11Mbps 802.11b WLAN Card
-	PCMCIA_DEVICE_PROD_ID123("LeArtery",  "SYNCBYAIR 11Mbps Wireless LAN PC Card",  "Version 01.02", 0x7e3b326a, 0x49893e92, 0x4b74baa0), // LeArtery SYNCBYAIR 11Mbps 802.11b WLAN Card
-PCMCIA_DEVICE_MANF_CARD(0x01ff, 0x0008), // Intermec MobileLAN 11Mbps 802.11b WLAN Card
-	PCMCIA_DEVICE_PROD_ID123("NETGEAR MA401 Wireless PC",  "Card",  "Version 01.00", 0xa37434e9, 0x9762e8f1, 0xa57adb8c), // NETGEAR MA401 11Mbps 802.11 WLAN Card
-	PCMCIA_DEVICE_PROD_ID1234("Intersil",  "PRISM Freedom PCMCIA Adapter",  "ISL37100P",  "Eval-RevA", 0x4b801a17, 0xf222ec2d, 0x630d52b2, 0xc23adc0e), // Intersil PRISM Freedom 11mbps 802.11 WLAN Card
-	PCMCIA_DEVICE_PROD_ID123("OTC",  "Wireless AirEZY 2411-PCC WLAN Card",  "Version 01.02", 0x4ac44287, 0x235a6bed, 0x4b74baa0), // OTC Wireless AirEZY 2411-PCC 11Mbps 802.11 WLAN Card
-	PCMCIA_DEVICE_PROD_ID1234("802.11",  "11Mbps Wireless LAN Card",  "v08C1",  ""   , 0xb67a610e, 0x655aa7b7, 0x264b451a, 0x0), // Dynalink L11HDT 11Mbps 802.11 WLAN Card
-	PCMCIA_DEVICE_MANF_CARD(0xc250, 0x0002), // Dynalink L11HDT 11Mbps 802.11 WLAN Card
-	PCMCIA_DEVICE_PROD_ID12("PROXIM",  "RangeLAN-DS/LAN PC CARD", 0xc6536a5e, 0x3f35797d), // PROXIM RangeLAN-DS/LAN PC CARD
-	PCMCIA_DEVICE_PROD_ID1234("ACTIONTEC",  "PRISM Wireless LAN PC Card",  "0381",  "RevA", 0x393089da, 0xa71e69d5, 0x90471fa9, 0x57a66194), // ACTIONTEC PRISM Wireless LAN PC Card
-	PCMCIA_DEVICE_MANF_CARD(0x1668, 0x0101), // ACTIONTEC PRISM Wireless LAN PC Card
-	PCMCIA_DEVICE_PROD_ID12("3Com",  "3CRWE737A AirConnect Wireless LAN PC Card", 0x41240e5b, 0x56010af3), // 3Com AirConnect 3CRWE737A
-	PCMCIA_DEVICE_PROD_ID12("3Com",  "3CRWE777A AirConnect Wireless LAN PCI Card"  , 0x41240e5b, 0xafc7c33e), // 3Com AirConnect 3CRWE777A
-	PCMCIA_DEVICE_PROD_ID12("ASUS",  "802_11b_PC_CARD_25", 0x78fc06ee, 0xdb9aa842), // ASUS WL-100 802.11b WLAN  PC Card
-	PCMCIA_DEVICE_PROD_ID12("ASUS",  "802_11B_CF_CARD_25", 0x78fc06ee, 0x45a50c1e), // ASUS WL-110 802.11b WLAN CF Card
-	PCMCIA_DEVICE_PROD_ID12("BUFFALO",  "WLI-CF-S11G", 0x2decece3, 0x82067c18), // BUFFALO WLI-CF-S11G 802.11b WLAN Card
-	PCMCIA_DEVICE_PROD_ID1234("The Linksys Group, Inc.", "Wireless Network CF Card", "ISL37300P", "RevA", 0xa5f472c2, 0x9c05598d, 0xc9049a39, 0x57a66194), // Linksys WCF11 11Mbps 802.11b WLAN Card (Prism 2.5)
-	PCMCIA_DEVICE_PROD_ID1234("Linksys",  "Wireless CompactFlash Card",  "",  "", 0x733cc81, 0xc52f395, 0x0, 0x0), // Linksys WCF12 11Mbps 802.11b WLAN Card (Prism 3)
-	PCMCIA_DEVICE_MANF_CARD(0x028a, 0x0673), // Linksys WCF12 11Mbps 802.11b WLAN Card (Prism 3)
-	PCMCIA_DEVICE_PROD_ID1234("NETGEAR MA401RA Wireless PC",  "Card",  "ISL37300P",  "Eval-RevA", 0x306467f, 0x9762e8f1, 0xc9049a39, 0xc23adc0e), // NETGEAR MA401RA 11Mbps 802.11 WLAN Card
-	PCMCIA_DEVICE_MANF_CARD(0xd601, 0x0005), // D-Link DCF-660W  11Mbps 802.11b WLAN Card
-	PCMCIA_DEVICE_MANF_CARD(0x02d2, 0x0001), // Microsoft Wireless Notebook Adapter MN-520
-	PCMCIA_DEVICE_MANF_CARD(0x0089, 0x0002), // AnyPoint(TM) Wireless II PC Card
-	PCMCIA_DEVICE_PROD_ID1234("D",  "Link DRC-650 11Mbps WLAN Card",  "Version 01.02",  "" , 0x71b18589, 0xf144e3ac, 0x4b74baa0, 0x0), // D-Link DRC-650 802.11b WLAN Card
-	PCMCIA_DEVICE_MANF_CARD(0x9005, 0x0021), // Adaptec AWN-8030
-	PCMCIA_DEVICE_MANF_CARD(0x000b, 0x7110), // D-Link DWL-650 rev P 802.11b WLAN card
-	// PCMCIA_DEVICE_PROD_ID1234("D-Link",  "DWL-650 Wireless PC Card RevP",  "ISL37101P-10",  "A3", 0x1a424a1c, 0x6ea57632, 0xdd97a26b, 0x56b21f52), // D-Link DWL-650 rev P 802.11b WLAN card
-	PCMCIA_DEVICE_PROD_ID123("INTERSIL",   "I-GATE 11M PC Card / PC Card plus",  "Version 01.02", 0x74c5e40d, 0x8304ff77, 0x4b74baa0), // I-Gate 11M PC Card
-	PCMCIA_DEVICE_PROD_ID1234("BENQ",  "AWL100 PCMCIA ADAPTER",  "ISL37300P",  "Eval-RevA", 0x35dadc74, 0x1f7fedb, 0xc9049a39, 0xc23adc0e), // benQ AWL100 802.11b WLAN Card
-	PCMCIA_DEVICE_MANF_CARD(0x000b, 0x7300), // benQ AWL100 802.11b WLAN Card
-	// PCMCIA_DEVICE_PROD_ID1("INTERSIL", 0x74c5e40d), // Intersil Prism 2 card
-	// PCMCIA_DEVICE_MANF_CARD(0x0156, 0x0002), // Intersil Prism 2 card
-
-	PCMCIA_DEVICE_NULL
-};
-
-MODULE_DEVICE_TABLE(pcmcia, prism2_cs_ids);
-#endif
-
-static struct pcmcia_driver prism2_cs_driver = {
-	.drv = {
-		.name = "prism2_cs",
-	},
-	.owner = THIS_MODULE,
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,15)
-	.suspend = prism2_cs_suspend,
-	.resume = prism2_cs_resume,
-	.remove = prism2_cs_remove,
-	.probe = prism2_cs_probe,
-	.id_table = prism2_cs_ids,
-#else
-	.attach = prism2sta_attach,
-	.detach = prism2sta_detach,
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,12)
-	.id_table = prism2_cs_ids,
-	.event =  prism2sta_event,
-#endif // > 2.6.12
-#endif // <= 2.6.15
-};
-#endif /* kernel_version */
-
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,15)
-#define CS_CHECK(fn, ret) \
-do { last_fn = (fn); if ((last_ret = (ret)) != 0) goto cs_failed; } while (0)
-
-#define CFG_CHECK(fn, retf) \
-do { int ret = (retf); \
-if (ret != 0) { \
-        WLAN_LOG_DEBUG(1, "CardServices(" #fn ") returned %d\n", ret); \
-        cs_error(pdev, fn, ret); \
-        goto next_entry; \
-} \
-} while (0)
-
-static void prism2_cs_remove(struct pcmcia_device *pdev)
-{
-	struct wlandevice  *wlandev;
-
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,17)
-        dev_link_t *link = dev_to_instance(pdev);
-#endif
-
-	DBFENTER;
-
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-	wlandev = pdev->priv;
-#else
-	wlandev = link->priv;
-#endif
-
-	if (wlandev) {
-		p80211netdev_hwremoved(wlandev);
-		unregister_wlandev(wlandev);
-		wlan_unsetup(wlandev);
-		if (wlandev->priv) {
-			hfa384x_t *hw = wlandev->priv;
-			wlandev->priv = NULL;
-			if (hw) {
-				hfa384x_destroy(hw);
-				kfree(hw);
-			}
-		}
-		kfree(wlandev);
-	}
-
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-	pdev->priv = NULL;
-        pcmcia_disable_device(pdev);
-#else
-        if (link->state & DEV_CONFIG) {
-	        if (link->win)
-			pcmcia_release_window(link->win);
-		pcmcia_release_configuration(link->handle);
-		if (link->io.NumPorts1)
-			pcmcia_release_io(link->handle, &link->io);
-		if (link->irq.AssignedIRQ)
-			pcmcia_release_irq(link->handle, &link->irq);
-
-		link->state &= ~DEV_CONFIG;
-	}
-
-	link->priv = NULL;
-	kfree(link);
-#endif
-
-	DBFEXIT;
-	return;
-}
-
-static int prism2_cs_suspend(struct pcmcia_device *pdev)
-{
-	struct wlandevice  *wlandev;
-
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,17)
-        dev_link_t *link = dev_to_instance(pdev);
-#endif
-
-	DBFENTER;
-
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-	wlandev = pdev->priv;
-	prism2sta_ifstate(wlandev, P80211ENUM_ifstate_disable);
-#else
-	wlandev = link->priv;
-
-        link->state |= DEV_SUSPEND;
-        if (link->state & DEV_CONFIG) {
-		prism2sta_ifstate(wlandev, P80211ENUM_ifstate_disable);
-		pcmcia_release_configuration(link->handle);
-	}
-#endif
-
-	DBFEXIT;
-
-	return 0;
-}
-
-static int prism2_cs_resume(struct pcmcia_device *pdev)
-{
-	struct wlandevice  *wlandev;
-
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,17)
-        dev_link_t *link = dev_to_instance(pdev);
-#endif
-
-	DBFENTER;
-
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-	wlandev = pdev->priv;
-	// XXX do something here?
-#else
-	wlandev = link->priv;
-        link->state &= ~DEV_SUSPEND;
-        if (link->state & DEV_CONFIG) {
-                pcmcia_request_configuration(link->handle, &link->conf);
-		// XXX do something here?
-	}
-#endif
-
-
-	DBFEXIT;
-
-	return 0;
-}
-
-static int prism2_cs_probe(struct pcmcia_device *pdev)
-{
-	int rval = 0;
-	struct wlandevice *wlandev = NULL;
-	hfa384x_t *hw = NULL;
-
-        config_info_t socketconf;
-        cisparse_t *parse = NULL;
-	tuple_t tuple;
-	uint8_t	buf[64];
-        int last_fn, last_ret;
-        cistpl_cftable_entry_t dflt = { 0 };
-
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,17)
-	dev_link_t *link;
-#endif
-
-	DBFENTER;
-
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-	/* Set up interrupt type */
-        pdev->conf.IntType = INT_MEMORY_AND_IO;
-#else
-        link = kmalloc(sizeof(dev_link_t), GFP_KERNEL);
-        if (link == NULL)
-                return -ENOMEM;
-        memset(link, 0, sizeof(dev_link_t));
-
-        link->conf.Vcc = 33;
-        link->conf.IntType = INT_MEMORY_AND_IO;
-
-        link->handle = pdev;
-        pdev->instance = link;
-        link->state |= DEV_PRESENT | DEV_CONFIG_PENDING;
-
-#endif
-
-	// VCC crap?
-        parse = kmalloc(sizeof(cisparse_t), GFP_KERNEL);
-
-	wlandev = create_wlan();
-	if (!wlandev || !parse) {
-		WLAN_LOG_ERROR("%s: Memory allocation failure.\n", dev_info);
-		rval = -EIO;
-		goto failed;
-	}
-	hw = wlandev->priv;
-
-	if ( wlan_setup(wlandev) != 0 ) {
-		WLAN_LOG_ERROR("%s: wlan_setup() failed.\n", dev_info);
-		rval = -EIO;
-		goto failed;
-	}
-
-	/* Initialize the hw struct for now */
-	hfa384x_create(hw, 0, 0, NULL);
-	hw->wlandev = wlandev;
-
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-	hw->pdev = pdev;
-	pdev->priv = wlandev;
-#else
-	hw->link = link;
-	link->priv = wlandev;
-#endif
-
-        tuple.DesiredTuple = CISTPL_CONFIG;
-        tuple.Attributes = 0;
-        tuple.TupleData = buf;
-        tuple.TupleDataMax = sizeof(buf);
-        tuple.TupleOffset = 0;
-        CS_CHECK(GetFirstTuple, pcmcia_get_first_tuple(pdev, &tuple));
-        CS_CHECK(GetTupleData, pcmcia_get_tuple_data(pdev, &tuple));
-        CS_CHECK(ParseTuple, pcmcia_parse_tuple(pdev, &tuple, parse));
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-        pdev->conf.ConfigBase = parse->config.base;
-        pdev->conf.Present = parse->config.rmask[0];
-#else
-        link->conf.ConfigBase = parse->config.base;
-        link->conf.Present = parse->config.rmask[0];
-
-	link->conf.Vcc = socketconf.Vcc;
-#endif
-        CS_CHECK(GetConfigurationInfo,
-                 pcmcia_get_configuration_info(pdev, &socketconf));
-
-	tuple.DesiredTuple = CISTPL_CFTABLE_ENTRY;
-	CS_CHECK(GetFirstTuple, pcmcia_get_first_tuple(pdev, &tuple));
-        for (;;) {
-		cistpl_cftable_entry_t *cfg = &(parse->cftable_entry);
-                CFG_CHECK(GetTupleData,
-                           pcmcia_get_tuple_data(pdev, &tuple));
-                CFG_CHECK(ParseTuple,
-                           pcmcia_parse_tuple(pdev, &tuple, parse));
-
-                if (cfg->flags & CISTPL_CFTABLE_DEFAULT)
-                        dflt = *cfg;
-                if (cfg->index == 0)
-                        goto next_entry;
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-                pdev->conf.ConfigIndex = cfg->index;
-#else
-                link->conf.ConfigIndex = cfg->index;
-#endif
-
-                /* Does this card need audio output? */
-                if (cfg->flags & CISTPL_CFTABLE_AUDIO) {
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-                        pdev->conf.Attributes |= CONF_ENABLE_SPKR;
-                        pdev->conf.Status = CCSR_AUDIO_ENA;
-#else
-                        link->conf.Attributes |= CONF_ENABLE_SPKR;
-                        link->conf.Status = CCSR_AUDIO_ENA;
-#endif
-                }
-
-                /* Use power settings for Vcc and Vpp if present */
-                /*  Note that the CIS values need to be rescaled */
-                if (cfg->vcc.present & (1 << CISTPL_POWER_VNOM)) {
-                        if (socketconf.Vcc != cfg->vcc.param[CISTPL_POWER_VNOM] /
-                            10000 && !prism2_ignorevcc) {
-                                WLAN_LOG_DEBUG(1, "  Vcc mismatch - skipping"
-                                       " this entry\n");
-                                goto next_entry;
-                        }
-                } else if (dflt.vcc.present & (1 << CISTPL_POWER_VNOM)) {
-                        if (socketconf.Vcc != dflt.vcc.param[CISTPL_POWER_VNOM] /
-                            10000 && !prism2_ignorevcc) {
-                                WLAN_LOG_DEBUG(1, "  Vcc (default) mismatch "
-                                       "- skipping this entry\n");
-                                goto next_entry;
-                        }
-                }
-
-                if (cfg->vpp1.present & (1 << CISTPL_POWER_VNOM)) {
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-                        pdev->conf.Vpp =
-                                cfg->vpp1.param[CISTPL_POWER_VNOM] / 10000;
-#else
-                        link->conf.Vpp1 = link->conf.Vpp2 =
-                                cfg->vpp1.param[CISTPL_POWER_VNOM] / 10000;
-#endif
-                } else if (dflt.vpp1.present & (1 << CISTPL_POWER_VNOM)) {
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-                        pdev->conf.Vpp =
-                                dflt.vpp1.param[CISTPL_POWER_VNOM] / 10000;
-#else
-                        link->conf.Vpp1 = link->conf.Vpp2 =
-                                dflt.vpp1.param[CISTPL_POWER_VNOM] / 10000;
-#endif
-		}
-
-		/* Do we need to allocate an interrupt? */
-		/* HACK: due to a bad CIS....we ALWAYS need an interrupt */
-		/* if (cfg->irq.IRQInfo1 || dflt.irq.IRQInfo1) */
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-		pdev->conf.Attributes |= CONF_ENABLE_IRQ;
-#else
-		link->conf.Attributes |= CONF_ENABLE_IRQ;
-#endif
-
-		/* IO window settings */
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-		pdev->io.NumPorts1 = pdev->io.NumPorts2 = 0;
-		if ((cfg->io.nwin > 0) || (dflt.io.nwin > 0)) {
-			cistpl_io_t *io = (cfg->io.nwin) ? &cfg->io : &dflt.io;
-			pdev->io.Attributes1 = IO_DATA_PATH_WIDTH_AUTO;
-			if (!(io->flags & CISTPL_IO_8BIT))
-				pdev->io.Attributes1 = IO_DATA_PATH_WIDTH_16;
-			if (!(io->flags & CISTPL_IO_16BIT))
-				pdev->io.Attributes1 = IO_DATA_PATH_WIDTH_8;
-			pdev->io.BasePort1 = io->win[0].base;
-			if  ( pdev->io.BasePort1 != 0 ) {
-				WLAN_LOG_WARNING(
-				"Brain damaged CIS: hard coded iobase="
-				"0x%x, try letting pcmcia_cs decide...\n",
-				pdev->io.BasePort1 );
-				pdev->io.BasePort1 = 0;
-			}
-			pdev->io.NumPorts1 = io->win[0].len;
-			if (io->nwin > 1) {
-				pdev->io.Attributes2 = pdev->io.Attributes1;
-				pdev->io.BasePort2 = io->win[1].base;
-				pdev->io.NumPorts2 = io->win[1].len;
-			}
-		}
-		/* This reserves IO space but doesn't actually enable it */
-		CFG_CHECK(RequestIO, pcmcia_request_io(pdev, &pdev->io));
-#else
-		link->io.NumPorts1 = link->io.NumPorts2 = 0;
-		if ((cfg->io.nwin > 0) || (dflt.io.nwin > 0)) {
-			cistpl_io_t *io = (cfg->io.nwin) ? &cfg->io : &dflt.io;
-			link->io.Attributes1 = IO_DATA_PATH_WIDTH_AUTO;
-			if (!(io->flags & CISTPL_IO_8BIT))
-				link->io.Attributes1 = IO_DATA_PATH_WIDTH_16;
-			if (!(io->flags & CISTPL_IO_16BIT))
-				link->io.Attributes1 = IO_DATA_PATH_WIDTH_8;
-			link->io.BasePort1 = io->win[0].base;
-			if  ( link->io.BasePort1 != 0 ) {
-				WLAN_LOG_WARNING(
-				"Brain damaged CIS: hard coded iobase="
-				"0x%x, try letting pcmcia_cs decide...\n",
-				link->io.BasePort1 );
-				link->io.BasePort1 = 0;
-			}
-			link->io.NumPorts1 = io->win[0].len;
-			if (io->nwin > 1) {
-				link->io.Attributes2 = link->io.Attributes1;
-				link->io.BasePort2 = io->win[1].base;
-				link->io.NumPorts2 = io->win[1].len;
-			}
-		}
-		/* This reserves IO space but doesn't actually enable it */
-		CFG_CHECK(RequestIO, pcmcia_request_io(pdev, &link->io));
-#endif
-		/* If we got this far, we're cool! */
-		break;
-
-	next_entry:
-		if (cfg->flags & CISTPL_CFTABLE_DEFAULT)
-			dflt = *cfg;
-		CS_CHECK(GetNextTuple, pcmcia_get_next_tuple(pdev, &tuple));
-
-	}
-
-	/* Let pcmcia know the device name */
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-	pdev->dev_node = &hw->node;
-#else
-	link->dev = &hw->node;
-#endif
-
-	/* Register the network device and get assigned a name */
-	SET_MODULE_OWNER(wlandev->netdev);
-	SET_NETDEV_DEV(wlandev->netdev,  &handle_to_dev(pdev));
-	if (register_wlandev(wlandev) != 0) {
-		WLAN_LOG_NOTICE("prism2sta_cs: register_wlandev() failed.\n");
-		goto failed;
-	}
-
-	strcpy(hw->node.dev_name, wlandev->name);
-
-	/* Allocate an interrupt line.  Note that this does not assign a */
-	/* handler to the interrupt, unless the 'Handler' member of the */
-	/* irq structure is initialized. */
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-	if (pdev->conf.Attributes & CONF_ENABLE_IRQ) {
-		pdev->irq.IRQInfo1 = IRQ_LEVEL_ID;
-		pdev->irq.Attributes = IRQ_TYPE_EXCLUSIVE | IRQ_HANDLE_PRESENT;
-		pdev->irq.Handler = hfa384x_interrupt;
-		pdev->irq.Instance = wlandev;
-		CS_CHECK(RequestIRQ, pcmcia_request_irq(pdev, &pdev->irq));
-	}
-#else
-	if (link->conf.Attributes & CONF_ENABLE_IRQ) {
-		link->irq.IRQInfo1 = IRQ_LEVEL_ID;
-		link->irq.Attributes = IRQ_TYPE_EXCLUSIVE | IRQ_HANDLE_PRESENT;
-		link->irq.Handler = hfa384x_interrupt;
-		link->irq.Instance = wlandev;
-		CS_CHECK(RequestIRQ, pcmcia_request_irq(pdev, &link->irq));
-	}
-#endif
-
-	/* This actually configures the PCMCIA socket -- setting up */
-	/* the I/O windows and the interrupt mapping, and putting the */
-	/* card and host interface into "Memory and IO" mode. */
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-	CS_CHECK(RequestConfiguration, pcmcia_request_configuration(pdev, &pdev->conf));
-#else
-	CS_CHECK(RequestConfiguration, pcmcia_request_configuration(pdev, &link->conf));
-#endif
-
-	/* Fill the netdevice with this info */
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-	wlandev->netdev->irq = pdev->irq.AssignedIRQ;
-	wlandev->netdev->base_addr = pdev->io.BasePort1;
-#else
-	wlandev->netdev->irq = link->irq.AssignedIRQ;
-	wlandev->netdev->base_addr = link->io.BasePort1;
-#endif
-
-	/* And the rest of the hw structure */
-	hw->irq = wlandev->netdev->irq;
-	hw->iobase = wlandev->netdev->base_addr;
-
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,17)
-	link->state |= DEV_CONFIG;
-	link->state &= ~DEV_CONFIG_PENDING;
-#endif
-
-	/* And now we're done! */
-	wlandev->msdstate = WLAN_MSD_HWPRESENT;
-
-	goto done;
-
- cs_failed:
-        cs_error(pdev, last_fn, last_ret);
-
-failed:
-	// wlandev, hw, etc etc..
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-	pdev->priv = NULL;
-#else
-	pdev->instance = NULL;
-	if (link) {
-		link->priv = NULL;
-		kfree(link);
-	}
-#endif
-	if (wlandev) {
-		wlan_unsetup(wlandev);
-		if (wlandev->priv) {
-			hw = wlandev->priv;
-			wlandev->priv = NULL;
-			if (hw) {
-				hfa384x_destroy(hw);
-				kfree(hw);
-			}
-		}
-		kfree(wlandev);
-	}
-
-done:
-	if (parse) kfree(parse);
-
-	DBFEXIT;
-	return rval;
-}
-#else  // <= 2.6.15
-#define CS_CHECK(fn, ret) \
-do { last_fn = (fn); if ((last_ret = (ret)) != 0) goto cs_failed; } while (0)
-
-#define CFG_CHECK(fn, retf) \
-do { int ret = (retf); \
-if (ret != 0) { \
-        WLAN_LOG_DEBUG(1, "CardServices(" #fn ") returned %d\n", ret); \
-        cs_error(link->handle, fn, ret); \
-        goto next_entry; \
-} \
-} while (0)
-
-/*----------------------------------------------------------------
-* prism2sta_attach
-*
-* Half of the attach/detach pair.  Creates and registers a device
-* instance with Card Services.  In this case, it also creates the
-* wlandev structure and device private structure.  These are
-* linked to the device instance via its priv member.
-*
-* Arguments:
-*	none
-*
-* Returns:
-*	A valid ptr to dev_link_t on success, NULL otherwise
-*
-* Side effects:
-*
-*
-* Call context:
-*	process thread (insmod/init_module/register_pccard_driver)
-----------------------------------------------------------------*/
-dev_link_t *prism2sta_attach(void)
-{
-	client_reg_t		client_reg;
-	int			result;
-	dev_link_t		*link = NULL;
-	wlandevice_t		*wlandev = NULL;
-	hfa384x_t		*hw = NULL;
-
-	DBFENTER;
-
-	/* Alloc our structures */
-	link =		kmalloc(sizeof(struct dev_link_t), GFP_KERNEL);
-
-	if (!link || ((wlandev = create_wlan()) == NULL)) {
-		WLAN_LOG_ERROR("%s: Memory allocation failure.\n", dev_info);
-		result = -EIO;
-		goto failed;
-	}
-	hw = wlandev->priv;
-
-	/* Clear all the structs */
-	memset(link, 0, sizeof(struct dev_link_t));
-
-	if ( wlan_setup(wlandev) != 0 ) {
-		WLAN_LOG_ERROR("%s: wlan_setup() failed.\n", dev_info);
-		result = -EIO;
-		goto failed;
-	}
-
-	/* Initialize the hw struct for now */
-	hfa384x_create(hw, 0, 0, NULL);
-	hw->wlandev = wlandev;
-
-	/* Initialize the PC card device object. */
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0))
-	init_timer(&link->release);
-	link->release.function = &prism2sta_release;
-	link->release.data = (u_long)link;
-#endif
-	link->conf.IntType = INT_MEMORY_AND_IO;
-	link->priv = wlandev;
-#if (defined(CS_RELEASE_CODE) && (CS_RELEASE_CODE < 0x2911))
-	link->irq.Instance = wlandev;
-#endif
-
-	/* Link in to the list of devices managed by this driver */
-	link->next = dev_list;
-	dev_list = link;
-
-	/* Register with Card Services */
-	client_reg.dev_info = &dev_info;
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,11) )
-	client_reg.Attributes = INFO_IO_CLIENT | INFO_CARD_SHARE;
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,13) )
-	client_reg.EventMask =
-		CS_EVENT_CARD_INSERTION | CS_EVENT_CARD_REMOVAL |
-		CS_EVENT_RESET_REQUEST |
-		CS_EVENT_RESET_PHYSICAL | CS_EVENT_CARD_RESET |
-		CS_EVENT_PM_SUSPEND | CS_EVENT_PM_RESUME;
-	client_reg.event_handler = &prism2sta_event;
-#endif
-
-	client_reg.Version = 0x0210;
-	client_reg.event_callback_args.client_data = link;
-
-	result = pcmcia_register_client(&link->handle, &client_reg);
-	if (result != 0) {
-		cs_error(link->handle, RegisterClient, result);
-		prism2sta_detach(link);
-		return NULL;
-	}
-
-	goto done;
-
- failed:
-	if (link)	kfree(link);
-	if (wlandev)	kfree(wlandev);
-	if (hw)		kfree(hw);
-	link = NULL;
-
- done:
-	DBFEXIT;
-	return link;
-}
-
-
-/*----------------------------------------------------------------
-* prism2sta_detach
-*
-* Remove one of the device instances managed by this driver.
-*   Search the list for the given instance,
-*   check our flags for a waiting timer'd release call
-*   call release
-*   Deregister the instance with Card Services
-*   (netdevice) unregister the network device.
-*   unlink the instance from the list
-*   free the link, priv, and priv->priv memory
-* Note: the dev_list variable is a driver scoped static used to
-*	maintain a list of device instances managed by this
-*	driver.
-*
-* Arguments:
-*	link	ptr to the instance to detach
-*
-* Returns:
-*	nothing
-*
-* Side effects:
-*	the link structure is gone, the netdevice is gone
-*
-* Call context:
-*	Might be interrupt, don't block.
-----------------------------------------------------------------*/
-void prism2sta_detach(dev_link_t *link)
-{
-	dev_link_t		**linkp;
-	wlandevice_t		*wlandev;
-	hfa384x_t		*hw;
-
-	DBFENTER;
-
-	/* Locate prev device structure */
-	for (linkp = &dev_list; *linkp; linkp = &(*linkp)->next) {
-		if (*linkp == link) break;
-	}
-
-	if (*linkp != NULL) {
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,4,0))
-		unsigned long	flags;
-		/* Get rid of any timer'd release call */
-		save_flags(flags);
-		cli();
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0))
-		if (link->state & DEV_RELEASE_PENDING) {
-			del_timer_sync(&link->release);
-			link->state &= ~DEV_RELEASE_PENDING;
-		}
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,4,0))
-		restore_flags(flags);
-#endif
-
-		/* If link says we're still config'd, call release */
-		if (link->state & DEV_CONFIG) {
-			prism2sta_release((u_long)link);
-			if (link->state & DEV_STALE_CONFIG) {
-				link->state |= DEV_STALE_LINK;
-				return;
-			}
-		}
-
-		/* Tell Card Services we're not around any more */
-		if (link->handle) {
-			pcmcia_deregister_client(link->handle);
-		}
-
-		/* Unlink device structure, free bits */
-		*linkp = link->next;
-		if ( link->priv != NULL ) {
-			wlandev = (wlandevice_t*)link->priv;
-			p80211netdev_hwremoved(wlandev);
-			if (link->dev != NULL) {
-				unregister_wlandev(wlandev);
-			}
-			wlan_unsetup(wlandev);
-			if (wlandev->priv) {
-				hw = wlandev->priv;
-				wlandev->priv = NULL;
-				if (hw) {
-					hfa384x_destroy(hw);
-					kfree(hw);
-				}
-			}
-			link->priv = NULL;
-			kfree(wlandev);
-		}
-		kfree(link);
-	}
-
-	DBFEXIT;
-	return;
-}
-
-/*----------------------------------------------------------------
-* prism2sta_config
-*
-* Half of the config/release pair.  Usually called in response to
-* a card insertion event.  At this point, we _know_ there's some
-* physical device present.  That means we can start poking around
-* at the CIS and at any device specific config data we want.
-*
-* Note the gotos and the macros.  I recoded this once without
-* them, and it got incredibly ugly.  It's actually simpler with
-* them.
-*
-* Arguments:
-*	link	the dev_link_t structure created in attach that
-*		represents this device instance.
-*
-* Returns:
-*	nothing
-*
-* Side effects:
-*	Resources (irq, io, mem) are allocated
-*	The pcmcia dev_link->node->name is set
-*	(For netcards) The device structure is finished and,
-*	  most importantly, registered.  This means that there
-*	  is now a _named_ device that can be configured from
-*	  userland.
-*
-* Call context:
-*	May be called from a timer.  Don't block!
-----------------------------------------------------------------*/
-#define CS_CHECK(fn, ret) \
-do { last_fn = (fn); if ((last_ret = (ret)) != 0) goto cs_failed; } while (0)
-
-#define CFG_CHECK(fn, retf) \
-do { int ret = (retf); \
-if (ret != 0) { \
-        WLAN_LOG_DEBUG(1, "CardServices(" #fn ") returned %d\n", ret); \
-        cs_error(link->handle, fn, ret); \
-        goto next_entry; \
-} \
-} while (0)
-
-void prism2sta_config(dev_link_t *link)
-{
-	client_handle_t		handle;
-	wlandevice_t		*wlandev;
-	hfa384x_t               *hw;
-	int			last_fn;
-	int			last_ret;
-	tuple_t			tuple;
-	cisparse_t		parse;
-	config_info_t		socketconf;
-	UINT8			buf[64];
-	int			minVcc = 0;
-	int			maxVcc = 0;
-	cistpl_cftable_entry_t	dflt = { 0 };
-
-	DBFENTER;
-
-	handle = link->handle;
-	wlandev = (wlandevice_t*)link->priv;
-	hw = wlandev->priv;
-
-	/* Collect the config register info */
-	tuple.DesiredTuple = CISTPL_CONFIG;
-	tuple.Attributes = 0;
-	tuple.TupleData = buf;
-	tuple.TupleDataMax = sizeof(buf);
-	tuple.TupleOffset = 0;
-	CS_CHECK(GetFirstTuple, pcmcia_get_first_tuple(handle, &tuple));
-	CS_CHECK(GetTupleData, pcmcia_get_tuple_data(handle, &tuple));
-	CS_CHECK(ParseTuple, pcmcia_parse_tuple(handle, &tuple, &parse));
-
-	link->conf.ConfigBase = parse.config.base;
-	link->conf.Present = parse.config.rmask[0];
-
-	/* Configure card */
-	link->state |= DEV_CONFIG;
-
-	/* Acquire the current socket config (need Vcc setting) */
-	CS_CHECK(GetConfigurationInfo, pcmcia_get_configuration_info(handle, &socketconf));
-
-	/* Loop through the config table entries until we find one that works */
-	/* Assumes a complete and valid CIS */
-	tuple.DesiredTuple = CISTPL_CFTABLE_ENTRY;
-	CS_CHECK(GetFirstTuple, pcmcia_get_first_tuple(handle, &tuple));
-	while (1) {
-		cistpl_cftable_entry_t *cfg = &(parse.cftable_entry);
-		CFG_CHECK(GetTupleData, pcmcia_get_tuple_data(handle, &tuple));
-		CFG_CHECK(ParseTuple, pcmcia_parse_tuple(handle, &tuple, &parse));
-
-		if (cfg->index == 0) goto next_entry;
-		link->conf.ConfigIndex = cfg->index;
-
-		/* Lets print out the Vcc that the controller+pcmcia-cs set
-		 * for us, cause that's what we're going to use.
-		 */
-		WLAN_LOG_DEBUG(1,"Initial Vcc=%d/10v\n", socketconf.Vcc);
-		if (prism2_ignorevcc) {
-			link->conf.Vcc = socketconf.Vcc;
-			goto skipvcc;
-		}
-
-		/* Use power settings for Vcc and Vpp if present */
-		/* Note that the CIS values need to be rescaled */
-		if (cfg->vcc.present & (1<<CISTPL_POWER_VNOM)) {
-			WLAN_LOG_DEBUG(1, "Vcc obtained from curtupl.VNOM\n");
-			minVcc = maxVcc =
-				cfg->vcc.param[CISTPL_POWER_VNOM]/10000;
-		} else if (dflt.vcc.present & (1<<CISTPL_POWER_VNOM)) {
-			WLAN_LOG_DEBUG(1, "Vcc set from dflt.VNOM\n");
-			minVcc = maxVcc =
-				dflt.vcc.param[CISTPL_POWER_VNOM]/10000;
-		} else if ((cfg->vcc.present & (1<<CISTPL_POWER_VMAX)) &&
-			   (cfg->vcc.present & (1<<CISTPL_POWER_VMIN)) ) {
-			WLAN_LOG_DEBUG(1, "Vcc set from curtupl(VMIN,VMAX)\n");			minVcc = cfg->vcc.param[CISTPL_POWER_VMIN]/10000;
-			maxVcc = cfg->vcc.param[CISTPL_POWER_VMAX]/10000;
-		} else if ((dflt.vcc.present & (1<<CISTPL_POWER_VMAX)) &&
-			   (dflt.vcc.present & (1<<CISTPL_POWER_VMIN)) ) {
-			WLAN_LOG_DEBUG(1, "Vcc set from dflt(VMIN,VMAX)\n");
-			minVcc = dflt.vcc.param[CISTPL_POWER_VMIN]/10000;
-			maxVcc = dflt.vcc.param[CISTPL_POWER_VMAX]/10000;
-		}
-
-		if ( socketconf.Vcc >= minVcc && socketconf.Vcc <= maxVcc) {
-			link->conf.Vcc = socketconf.Vcc;
-		} else {
-			/* [MSM]: Note that I've given up trying to change
-			 * the Vcc if a change is indicated.  It seems the
-			 * system&socketcontroller&card vendors can't seem
-			 * to get it right, so I'm tired of trying to hack
-			 * my way around it.  pcmcia-cs does its best using
-			 * the voltage sense pins but sometimes the controller
-			 * lies.  Then, even if we have a good read on the VS
-			 * pins, some system designs will silently ignore our
-			 * requests to set the voltage.  Additionally, some
-			 * vendors have 3.3v indicated on their sense pins,
-			 * but 5v specified in the CIS or vice-versa.  I've
-			 * had it.  My only recommendation is "let the buyer
-			 * beware".  Your system might supply 5v to a 3v card
-			 * (possibly causing damage) or a 3v capable system
-			 * might supply 5v to a 3v capable card (wasting
-			 * precious battery life).
-			 * My only recommendation (if you care) is to get
-			 * yourself an extender card (I don't know where, I
-			 * have only one myself) and a meter and test it for
-			 * yourself.
-			 */
-			goto next_entry;
-		}
-skipvcc:
-		WLAN_LOG_DEBUG(1, "link->conf.Vcc=%d\n", link->conf.Vcc);
-
-		/* Do we need to allocate an interrupt? */
-		/* HACK: due to a bad CIS....we ALWAYS need an interrupt */
-		/* if (cfg->irq.IRQInfo1 || dflt.irq.IRQInfo1) */
-			link->conf.Attributes |= CONF_ENABLE_IRQ;
-
-		/* IO window settings */
-		link->io.NumPorts1 = link->io.NumPorts2 = 0;
-		if ((cfg->io.nwin > 0) || (dflt.io.nwin > 0)) {
-			cistpl_io_t *io = (cfg->io.nwin) ? &cfg->io : &dflt.io;
-			link->io.Attributes1 = IO_DATA_PATH_WIDTH_AUTO;
-			if (!(io->flags & CISTPL_IO_8BIT))
-				link->io.Attributes1 = IO_DATA_PATH_WIDTH_16;
-			if (!(io->flags & CISTPL_IO_16BIT))
-				link->io.Attributes1 = IO_DATA_PATH_WIDTH_8;
-			link->io.BasePort1 = io->win[0].base;
-			if  ( link->io.BasePort1 != 0 ) {
-				WLAN_LOG_WARNING(
-				"Brain damaged CIS: hard coded iobase="
-				"0x%x, try letting pcmcia_cs decide...\n",
-				link->io.BasePort1 );
-				link->io.BasePort1 = 0;
-			}
-			link->io.NumPorts1 = io->win[0].len;
-			if (io->nwin > 1) {
-				link->io.Attributes2 = link->io.Attributes1;
-				link->io.BasePort2 = io->win[1].base;
-				link->io.NumPorts2 = io->win[1].len;
-			}
-		}
-
-		/* This reserves IO space but doesn't actually enable it */
-		CFG_CHECK(RequestIO, pcmcia_request_io(link->handle, &link->io));
-
-		/* If we got this far, we're cool! */
-		break;
-
-next_entry:
-		if (cfg->flags & CISTPL_CFTABLE_DEFAULT)
-			dflt = *cfg;
-		CS_CHECK(GetNextTuple,
-                         pcmcia_get_next_tuple(handle, &tuple));
-	}
-
-	/* Allocate an interrupt line.  Note that this does not assign a */
-	/* handler to the interrupt, unless the 'Handler' member of the */
-	/* irq structure is initialized. */
-	if (link->conf.Attributes & CONF_ENABLE_IRQ)
-	{
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,11) )
-		int			i;
-		link->irq.IRQInfo1 = IRQ_INFO2_VALID | IRQ_LEVEL_ID;
-		if (irq_list[0] == -1)
-			link->irq.IRQInfo2 = irq_mask;
-		else
-			for (i=0; i<4; i++)
-				link->irq.IRQInfo2 |= 1 << irq_list[i];
-#else
-		link->irq.IRQInfo1 = IRQ_LEVEL_ID;
-#endif
-		link->irq.Attributes = IRQ_TYPE_EXCLUSIVE | IRQ_HANDLE_PRESENT;
-		link->irq.Handler = hfa384x_interrupt;
-		link->irq.Instance = wlandev;
-		CS_CHECK(RequestIRQ, pcmcia_request_irq(link->handle, &link->irq));
-	}
-
-	/* This actually configures the PCMCIA socket -- setting up */
-	/* the I/O windows and the interrupt mapping, and putting the */
-	/* card and host interface into "Memory and IO" mode. */
-	CS_CHECK(RequestConfiguration, pcmcia_request_configuration(link->handle, &link->conf));
-
-	/* Fill the netdevice with this info */
-	wlandev->netdev->irq = link->irq.AssignedIRQ;
-	wlandev->netdev->base_addr = link->io.BasePort1;
-
-	/* Report what we've done */
-	WLAN_LOG_INFO("%s: index 0x%02x: Vcc %d.%d",
-		dev_info, link->conf.ConfigIndex,
-		link->conf.Vcc/10, link->conf.Vcc%10);
-	if (link->conf.Vpp1)
-		printk(", Vpp %d.%d", link->conf.Vpp1/10, link->conf.Vpp1%10);
-	if (link->conf.Attributes & CONF_ENABLE_IRQ)
-		printk(", irq %d", link->irq.AssignedIRQ);
-	if (link->io.NumPorts1)
-		printk(", io 0x%04x-0x%04x", link->io.BasePort1, link->io.BasePort1+link->io.NumPorts1-1);
-	if (link->io.NumPorts2)
-		printk(" & 0x%04x-0x%04x", link->io.BasePort2, link->io.BasePort2+link->io.NumPorts2-1);
-	printk("\n");
-
-	link->state &= ~DEV_CONFIG_PENDING;
-
-	/* Let pcmcia know the device name */
-	link->dev = &hw->node;
-
-	/* Register the network device and get assigned a name */
-	SET_MODULE_OWNER(wlandev->netdev);
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,11) )
-	SET_NETDEV_DEV(wlandev->netdev,  &handle_to_dev(link->handle));
-#endif
-	if (register_wlandev(wlandev) != 0) {
-		WLAN_LOG_NOTICE("prism2sta_cs: register_wlandev() failed.\n");
-		goto failed;
-	}
-
-	strcpy(hw->node.dev_name, wlandev->name);
-
-	/* Any device custom config/query stuff should be done here */
-	/* For a netdevice, we should at least grab the mac address */
-
-	return;
-cs_failed:
-	cs_error(link->handle, last_fn, last_ret);
-	WLAN_LOG_ERROR("NextTuple failure? It's probably a Vcc mismatch.\n");
-
-failed:
-	prism2sta_release((u_long)link);
-	return;
-}
-
-/*----------------------------------------------------------------
-* prism2sta_release
-*
-* Half of the config/release pair.  Usually called in response to
-* a card ejection event.  Checks to make sure no higher layers
-* are still (or think they are) using the card via the link->open
-* field.
-*
-* NOTE: Don't forget to increment the link->open variable in the
-*  device_open method, and decrement it in the device_close
-*  method.
-*
-* Arguments:
-*	arg	a generic 32 bit variable.  It's the value that
-*		we assigned to link->release.data in sta_attach().
-*
-* Returns:
-*	nothing
-*
-* Side effects:
-*	All resources should be released after this function
-*	executes and finds the device !open.
-*
-* Call context:
-*	Possibly in a timer context.  Don't do anything that'll
-*	block.
-----------------------------------------------------------------*/
-void prism2sta_release(u_long arg)
-{
-        dev_link_t	*link = (dev_link_t *)arg;
-
-	DBFENTER;
-
-	/* First thing we should do is get the MSD back to the
-	 * HWPRESENT state.  I.e. everything quiescent.
-	 */
-	prism2sta_ifstate(link->priv, P80211ENUM_ifstate_disable);
-
-        if (link->open) {
-		/* TODO: I don't think we're even using this bit of code
-		 * and I don't think it's hurting us at the moment.
-		 */
-                WLAN_LOG_DEBUG(1,
-			"prism2sta_cs: release postponed, '%s' still open\n",
-			link->dev->dev_name);
-                link->state |= DEV_STALE_CONFIG;
-                return;
-        }
-
-        pcmcia_release_configuration(link->handle);
-        pcmcia_release_io(link->handle, &link->io);
-        pcmcia_release_irq(link->handle, &link->irq);
-
-        link->state &= ~(DEV_CONFIG | DEV_RELEASE_PENDING);
-
-	DBFEXIT;
-}
-
-/*----------------------------------------------------------------
-* prism2sta_event
-*
-* Handler for card services events.
-*
-* Arguments:
-*	event		The event code
-*	priority	hi/low - REMOVAL is the only hi
-*	args		ptr to card services struct containing info about
-*			pcmcia status
-*
-* Returns:
-*	Zero on success, non-zero otherwise
-*
-* Side effects:
-*
-*
-* Call context:
-*	Both interrupt and process thread, depends on the event.
-----------------------------------------------------------------*/
-static int
-prism2sta_event (
-	event_t event,
-	int priority,
-	event_callback_args_t *args)
-{
-	int			result = 0;
-	dev_link_t		*link = (dev_link_t *) args->client_data;
-	wlandevice_t		*wlandev = (wlandevice_t*)link->priv;
-	hfa384x_t		*hw = NULL;
-
-	DBFENTER;
-
-	if (wlandev) hw = wlandev->priv;
-
-	switch (event)
-	{
-	case CS_EVENT_CARD_INSERTION:
-		WLAN_LOG_DEBUG(5,"event is INSERTION\n");
-		link->state |= DEV_PRESENT | DEV_CONFIG_PENDING;
-		prism2sta_config(link);
-		if (!(link->state & DEV_CONFIG)) {
-			wlandev->netdev->irq = 0;
-			WLAN_LOG_ERROR(
-				"%s: Initialization failed!\n", dev_info);
-			wlandev->msdstate = WLAN_MSD_HWFAIL;
-			break;
-		}
-
-		/* Fill in the rest of the hw struct */
-		hw->irq = wlandev->netdev->irq;
-		hw->iobase = wlandev->netdev->base_addr;
-		hw->link = link;
-
-		if (prism2_doreset) {
-			result = hfa384x_corereset(hw,
-					prism2_reset_holdtime,
-					prism2_reset_settletime, 0);
-			if ( result ) {
-				WLAN_LOG_ERROR(
-					"corereset() failed, result=%d.\n",
-					result);
-				wlandev->msdstate = WLAN_MSD_HWFAIL;
-				break;
-			}
-		}
-
-#if 0
-		/*
-		 * TODO: test_hostif() not implemented yet.
-		 */
-		result = hfa384x_test_hostif(hw);
-		if (result) {
-			WLAN_LOG_ERROR(
-			"test_hostif() failed, result=%d.\n", result);
-			wlandev->msdstate = WLAN_MSD_HWFAIL;
-			break;
-		}
-#endif
-		wlandev->msdstate = WLAN_MSD_HWPRESENT;
-		break;
-
-	case CS_EVENT_CARD_REMOVAL:
-		WLAN_LOG_DEBUG(5,"event is REMOVAL\n");
-		link->state &= ~DEV_PRESENT;
-
-		if (wlandev) {
-			p80211netdev_hwremoved(wlandev);
-		}
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0))
-		if (link->state & DEV_CONFIG)
-		{
-			link->release.expires = jiffies + (HZ/20);
-			add_timer(&link->release);
-		}
-#endif
-		break;
-	case CS_EVENT_RESET_REQUEST:
-		WLAN_LOG_DEBUG(5,"event is RESET_REQUEST\n");
-		WLAN_LOG_NOTICE(
-			"prism2 card reset not supported "
-			"due to post-reset user mode configuration "
-			"requirements.\n");
-		WLAN_LOG_NOTICE(
-			"  From user mode, use "
-			"'cardctl suspend;cardctl resume' "
-			"instead.\n");
-		break;
-	case CS_EVENT_RESET_PHYSICAL:
-	case CS_EVENT_CARD_RESET:
-		WLAN_LOG_WARNING("Rx'd CS_EVENT_RESET_xxx, should not "
-			"be possible since RESET_REQUEST was denied.\n");
-		break;
-
-	case CS_EVENT_PM_SUSPEND:
-		WLAN_LOG_DEBUG(5,"event is SUSPEND\n");
-		link->state |= DEV_SUSPEND;
-		if (link->state & DEV_CONFIG)
-		{
-			prism2sta_ifstate(wlandev, P80211ENUM_ifstate_disable);
-			pcmcia_release_configuration(link->handle);
-		}
-		break;
-
-	case CS_EVENT_PM_RESUME:
-		WLAN_LOG_DEBUG(5,"event is RESUME\n");
-		link->state &= ~DEV_SUSPEND;
-		if (link->state & DEV_CONFIG) {
-			pcmcia_request_configuration(link->handle, &link->conf);
-		}
-		break;
-	}
-
-	DBFEXIT;
-	return 0;  /* noone else does anthing with the return value */
-}
-#endif // <= 2.6.15
-
-
-
-int hfa384x_corereset(hfa384x_t *hw, int holdtime, int settletime, int genesis)
-{
-	int		result = 0;
-	conf_reg_t	reg;
-	UINT8		corsave;
-	DBFENTER;
-
-	WLAN_LOG_DEBUG(3, "Doing reset via CardServices().\n");
-
-	/* Collect COR */
-	reg.Function = 0;
-	reg.Action = CS_READ;
-	reg.Offset = CISREG_COR;
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-	result = pcmcia_access_configuration_register(hw->pdev, &reg);
-#else
-	result = pcmcia_access_configuration_register(
-			hw->link->handle,
-			&reg);
-#endif
-	if (result != CS_SUCCESS ) {
-		WLAN_LOG_ERROR(
-			":0: AccessConfigurationRegister(CS_READ) failed,"
-			"result=%d.\n", result);
-		result = -EIO;
-	}
-	corsave = reg.Value;
-
-	/* Write reset bit (BIT7) */
-	reg.Value |= BIT7;
-	reg.Action = CS_WRITE;
-	reg.Offset = CISREG_COR;
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-	result = pcmcia_access_configuration_register(hw->pdev, &reg);
-#else
-	result = pcmcia_access_configuration_register(
-			hw->link->handle,
-			&reg);
-#endif
-	if (result != CS_SUCCESS ) {
-		WLAN_LOG_ERROR(
-			":1: AccessConfigurationRegister(CS_WRITE) failed,"
-			"result=%d.\n", result);
-		result = -EIO;
-	}
-
-	/* Hold for holdtime */
-	mdelay(holdtime);
-
-	if (genesis) {
-		reg.Value = genesis;
-		reg.Action = CS_WRITE;
-		reg.Offset = CISREG_CCSR;
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-		result = pcmcia_access_configuration_register(hw->pdev, &reg);
-#else
-		result = pcmcia_access_configuration_register(
-				      hw->link->handle,
-				      &reg);
-#endif
-		if (result != CS_SUCCESS ) {
-			WLAN_LOG_ERROR(
-				":1: AccessConfigurationRegister(CS_WRITE) failed,"
-				"result=%d.\n", result);
-			result = -EIO;
-		}
-	}
-
-	/* Hold for holdtime */
-	mdelay(holdtime);
-
-	/* Clear reset bit */
-	reg.Value &= ~BIT7;
-	reg.Action = CS_WRITE;
-	reg.Offset = CISREG_COR;
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-	result = pcmcia_access_configuration_register(hw->pdev, &reg);
-#else
-	result = pcmcia_access_configuration_register(
-				      hw->link->handle,
-				      &reg);
-#endif
-	if (result != CS_SUCCESS ) {
-		WLAN_LOG_ERROR(
-			":2: AccessConfigurationRegister(CS_WRITE) failed,"
-			"result=%d.\n", result);
-		result = -EIO;
-		goto done;
-	}
-
-	/* Wait for settletime */
-	mdelay(settletime);
-
-	/* Set non-reset bits back what they were */
-	reg.Value = corsave;
-	reg.Action = CS_WRITE;
-	reg.Offset = CISREG_COR;
-#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,16)
-	result = pcmcia_access_configuration_register(hw->pdev, &reg);
-#else
-	result = pcmcia_access_configuration_register(
-				      hw->link->handle,
-				      &reg);
-#endif
-	if (result != CS_SUCCESS ) {
-		WLAN_LOG_ERROR(
-			":2: AccessConfigurationRegister(CS_WRITE) failed,"
-			"result=%d.\n", result);
-		result = -EIO;
-		goto done;
-	}
-
-done:
-	DBFEXIT;
-	return result;
-}
-
-#ifdef MODULE
-
-static int __init prism2cs_init(void)
-{
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,68))
-	servinfo_t	serv;
-#endif
-
-	DBFENTER;
-
-        WLAN_LOG_NOTICE("%s Loaded\n", version);
-        WLAN_LOG_NOTICE("dev_info is: %s\n", dev_info);
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,68))
-	pcmcia_get_card_services_info(&serv);
-	if ( serv.Revision != CS_RELEASE_CODE )
-	{
-		printk(KERN_NOTICE"%s: CardServices release does not match!\n", dev_info);
-		return -1;
-	}
-
-	/* This call will result in a call to prism2sta_attach */
-	/*   and eventually prism2sta_detach */
-	register_pccard_driver( &dev_info, &prism2sta_attach, &prism2sta_detach);
-#else
-	pcmcia_register_driver(&prism2_cs_driver);
-#endif
-
-	DBFEXIT;
-	return 0;
-}
-
-static void __exit prism2cs_cleanup(void)
-{
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,68))
-        dev_link_t *link = dev_list;
-        dev_link_t *nlink;
-        DBFENTER;
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,10) )
-	for (link=dev_list; link != NULL; link = nlink) {
-		nlink = link->next;
-		if ( link->state & DEV_CONFIG ) {
-			prism2sta_release((u_long)link);
-		}
-		prism2sta_detach(link); /* remember detach() frees link */
-	}
-#endif
-
-	unregister_pccard_driver( &dev_info);
-#else
-	pcmcia_unregister_driver(&prism2_cs_driver);
-#endif
-
-        printk(KERN_NOTICE "%s Unloaded\n", version);
-
-        DBFEXIT;
-        return;
-}
-
-module_init(prism2cs_init);
-module_exit(prism2cs_cleanup);
-
-#endif // MODULE
-
diff --git a/drivers/staging/wlan-ng/prism2_pci.c b/drivers/staging/wlan-ng/prism2_pci.c
deleted file mode 100644
index afe32dfbf6b..00000000000
--- a/drivers/staging/wlan-ng/prism2_pci.c
+++ /dev/null
@@ -1,332 +0,0 @@
-#define WLAN_HOSTIF WLAN_PCI
-#include "hfa384x.c"
-#include "prism2mgmt.c"
-#include "prism2mib.c"
-#include "prism2sta.c"
-
-#define PCI_SIZE		0x1000		/* Memory size - 4K bytes */
-
-/* ISL3874A 11Mb/s WLAN controller */
-#define PCIVENDOR_INTERSIL	0x1260UL
-#define PCIDEVICE_ISL3874	0x3873UL /* [MSM] yeah I know...the ID says
-					    3873. Trust me, it's a 3874. */
-
-/* Samsung SWL-2210P 11Mb/s WLAN controller (uses ISL3874A) */
-#define PCIVENDOR_SAMSUNG      0x167dUL
-#define PCIDEVICE_SWL_2210P    0xa000UL
-
-#define PCIVENDOR_NETGEAR      0x1385UL /* for MA311 */
-
-/* PCI Class & Sub-Class code, Network-'Other controller' */
-#define PCI_CLASS_NETWORK_OTHERS 0x280
-
-
-/*----------------------------------------------------------------
-* prism2sta_probe_pci
-*
-* Probe routine called when a PCI device w/ matching ID is found.
-* The ISL3874 implementation uses the following map:
-*   BAR0: Prism2.x registers memory mapped, size=4k
-* Here's the sequence:
-*   - Allocate the PCI resources.
-*   - Read the PCMCIA attribute memory to make sure we have a WLAN card
-*   - Reset the MAC
-*   - Initialize the netdev and wlan data
-*   - Initialize the MAC
-*
-* Arguments:
-*	pdev		ptr to pci device structure containing info about
-*			pci configuration.
-*	id		ptr to the device id entry that matched this device.
-*
-* Returns:
-*	zero		- success
-*	negative	- failed
-*
-* Side effects:
-*
-*
-* Call context:
-*	process thread
-*
-----------------------------------------------------------------*/
-static int __devinit
-prism2sta_probe_pci(
-	struct pci_dev *pdev,
-	const struct pci_device_id *id)
-{
-	int		result;
-	phys_t		phymem = 0;
-	void		__iomem *mem = NULL;
-        wlandevice_t    *wlandev = NULL;
-	hfa384x_t	*hw = NULL;
-
-	DBFENTER;
-
-	/* Enable the pci device */
-	if (pci_enable_device(pdev)) {
-		WLAN_LOG_ERROR("%s: pci_enable_device() failed.\n", dev_info);
-		result = -EIO;
-		goto fail;
-	}
-
-	/* Figure out our resources */
-	phymem = pci_resource_start(pdev, 0);
-
-        if (!request_mem_region(phymem, pci_resource_len(pdev, 0), "Prism2")) {
-		printk(KERN_ERR "prism2: Cannot reserve PCI memory region\n");
-		result = -EIO;
-		goto fail;
-        }
-
-	mem = ioremap(phymem, PCI_SIZE);
-	if ( mem == 0 ) {
-		WLAN_LOG_ERROR("%s: ioremap() failed.\n", dev_info);
-		result = -EIO;
-		goto fail;
-	}
-
-	/* Log the device */
-        WLAN_LOG_INFO("A Prism2.5 PCI device found, "
-		"phymem:0x%llx, irq:%d, mem:0x%p\n",
-		(unsigned long long)phymem, pdev->irq, mem);
-
-	if ((wlandev = create_wlan()) == NULL) {
-		WLAN_LOG_ERROR("%s: Memory allocation failure.\n", dev_info);
-		result = -EIO;
-		goto fail;
-	}
-	hw = wlandev->priv;
-
-	if ( wlan_setup(wlandev) != 0 ) {
-		WLAN_LOG_ERROR("%s: wlan_setup() failed.\n", dev_info);
-		result = -EIO;
-		goto fail;
-	}
-
-	/* Setup netdevice's ability to report resources
-	 * Note: the netdevice was allocated by wlan_setup()
-	 */
-        wlandev->netdev->irq = pdev->irq;
-        wlandev->netdev->mem_start = (unsigned long) mem;
-        wlandev->netdev->mem_end = wlandev->netdev->mem_start +
-		pci_resource_len(pdev, 0);
-
-	/* Initialize the hw data */
-        hfa384x_create(hw, wlandev->netdev->irq, 0, mem);
-	hw->wlandev = wlandev;
-
-	/* Register the wlandev, this gets us a name and registers the
-	 * linux netdevice.
-	 */
-	SET_MODULE_OWNER(wlandev->netdev);
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0))
-       SET_NETDEV_DEV(wlandev->netdev, &(pdev->dev));
-#endif
-        if ( register_wlandev(wlandev) != 0 ) {
-		WLAN_LOG_ERROR("%s: register_wlandev() failed.\n", dev_info);
-		result = -EIO;
-		goto fail;
-        }
-
-#if 0
-	/* TODO: Move this and an irq test into an hfa384x_testif() routine.
-	 */
-	outw(PRISM2STA_MAGIC, HFA384x_SWSUPPORT(wlandev->netdev->base_addr));
-	reg=inw( HFA384x_SWSUPPORT(wlandev->netdev->base_addr));
-	if ( reg != PRISM2STA_MAGIC ) {
-		WLAN_LOG_ERROR("MAC register access test failed!\n");
-		result = -EIO;
-		goto fail;
-	}
-#endif
-
-	/* Do a chip-level reset on the MAC */
-	if (prism2_doreset) {
-		result = hfa384x_corereset(hw,
-				prism2_reset_holdtime,
-				prism2_reset_settletime, 0);
-		if (result != 0) {
-			WLAN_LOG_ERROR(
-				"%s: hfa384x_corereset() failed.\n",
-				dev_info);
-			unregister_wlandev(wlandev);
-			hfa384x_destroy(hw);
-			result = -EIO;
-			goto fail;
-		}
-	}
-
-        pci_set_drvdata(pdev, wlandev);
-
-	/* Shouldn't actually hook up the IRQ until we
-	 * _know_ things are alright.  A test routine would help.
-	 */
-       	request_irq(wlandev->netdev->irq, hfa384x_interrupt,
-		SA_SHIRQ, wlandev->name, wlandev);
-
-	wlandev->msdstate = WLAN_MSD_HWPRESENT;
-
-	result = 0;
-	goto done;
-
- fail:
-	pci_set_drvdata(pdev, NULL);
-	if (wlandev)	kfree(wlandev);
-	if (hw)		kfree(hw);
-        if (mem)        iounmap(mem);
-	pci_release_regions(pdev);
-        pci_disable_device(pdev);
-
- done:
-	DBFEXIT;
-	return result;
-}
-
-static void __devexit prism2sta_remove_pci(struct pci_dev *pdev)
-{
-       	wlandevice_t		*wlandev;
-	hfa384x_t	*hw;
-
-	wlandev = (wlandevice_t *) pci_get_drvdata(pdev);
-	hw = wlandev->priv;
-
-	p80211netdev_hwremoved(wlandev);
-
-	/* reset hardware */
-	prism2sta_ifstate(wlandev, P80211ENUM_ifstate_disable);
-
-        if (pdev->irq)
-		free_irq(pdev->irq, wlandev);
-
-	unregister_wlandev(wlandev);
-
-	/* free local stuff */
-	if (hw) {
-		hfa384x_destroy(hw);
-		kfree(hw);
-	}
-
-	iounmap((void __iomem *)wlandev->netdev->mem_start);
-	wlan_unsetup(wlandev);
-
-	pci_release_regions(pdev);
-        pci_disable_device(pdev);
-	pci_set_drvdata(pdev, NULL);
-
-	kfree(wlandev);
-}
-
-
-static struct pci_device_id pci_id_tbl[] = {
-	{
-		PCIVENDOR_INTERSIL, PCIDEVICE_ISL3874,
-		PCI_ANY_ID, PCI_ANY_ID,
-		0, 0,
-		/* Driver data, we just put the name here */
-		(unsigned long)"Intersil Prism2.5 ISL3874 11Mb/s WLAN Controller"
-	},
-	{
-		PCIVENDOR_INTERSIL, 0x3872,
-		PCI_ANY_ID, PCI_ANY_ID,
-		0, 0,
-		/* Driver data, we just put the name here */
-		(unsigned long)"Intersil Prism2.5 ISL3872 11Mb/s WLAN Controller"
-	},
-        {
-               PCIVENDOR_SAMSUNG, PCIDEVICE_SWL_2210P,
-               PCI_ANY_ID, PCI_ANY_ID,
-               0, 0,
-               /* Driver data, we just put the name here */
-               (unsigned long)"Samsung MagicLAN SWL-2210P 11Mb/s WLAN Controller"
-	},
-	{ /* for NetGear MA311 */
-		PCIVENDOR_NETGEAR, 0x3872,
-		PCI_ANY_ID, PCI_ANY_ID,
-		0, 0,
-		/* Driver data, we just put the name here */
-		(unsigned long)"Netgear MA311 WLAN Controller"
-	},
-	{
-		0, 0, 0, 0, 0, 0, 0
-	}
-};
-
-MODULE_DEVICE_TABLE(pci, pci_id_tbl);
-
-/* Function declared here because of ptr reference below */
-static int  __devinit prism2sta_probe_pci(struct pci_dev *pdev,
-				const struct pci_device_id *id);
-static void  __devexit prism2sta_remove_pci(struct pci_dev *pdev);
-
-static struct pci_driver prism2_pci_drv_id = {
-        .name = "prism2_pci",
-        .id_table = pci_id_tbl,
-        .probe = prism2sta_probe_pci,
-        .remove = prism2sta_remove_pci,
-#ifdef CONFIG_PM
-        .suspend = prism2sta_suspend_pci,
-        .resume = prism2sta_resume_pci,
-#endif
-};
-
-#ifdef MODULE
-
-static int __init prism2pci_init(void)
-{
-        WLAN_LOG_NOTICE("%s Loaded\n", version);
-	return pci_module_init(&prism2_pci_drv_id);
-};
-
-static void __exit prism2pci_cleanup(void)
-{
-	pci_unregister_driver(&prism2_pci_drv_id);
-};
-
-module_init(prism2pci_init);
-module_exit(prism2pci_cleanup);
-
-#endif
-
-int hfa384x_corereset(hfa384x_t *hw, int holdtime, int settletime, int genesis)
-{
-	int		result = 0;
-	unsigned long	timeout;
-	UINT16	reg;
-	DBFENTER;
-
-	/* Assert reset and wait awhile
-	 * (note: these delays are _really_ long, but they appear to be
-	 *        necessary.)
-	 */
-	hfa384x_setreg(hw, 0xc5, HFA384x_PCICOR);
-	timeout = jiffies + HZ/4;
-	while(time_before(jiffies, timeout)) udelay(5);
-
-	if (genesis) {
-		hfa384x_setreg(hw, genesis, HFA384x_PCIHCR);
-		timeout = jiffies + HZ/4;
-		while(time_before(jiffies, timeout)) udelay(5);
-	}
-
-	/* Clear the reset and wait some more
-	 */
-	hfa384x_setreg(hw, 0x45, HFA384x_PCICOR);
-	timeout = jiffies + HZ/2;
-	while(time_before(jiffies, timeout)) udelay(5);
-
-	/* Wait for f/w to complete initialization (CMD:BUSY == 0)
-	 */
-	timeout = jiffies + 2*HZ;
-	reg = hfa384x_getreg(hw, HFA384x_CMD);
-	while ( HFA384x_CMD_ISBUSY(reg) && time_before( jiffies, timeout) ) {
-		reg = hfa384x_getreg(hw, HFA384x_CMD);
-		udelay(10);
-	}
-	if (HFA384x_CMD_ISBUSY(reg)) {
-		WLAN_LOG_WARNING("corereset: Timed out waiting for cmd register.\n");
-		result=1;
-	}
-	DBFEXIT;
-	return result;
-}
diff --git a/drivers/staging/wlan-ng/prism2_plx.c b/drivers/staging/wlan-ng/prism2_plx.c
deleted file mode 100644
index 320443f37a8..00000000000
--- a/drivers/staging/wlan-ng/prism2_plx.c
+++ /dev/null
@@ -1,472 +0,0 @@
-#define WLAN_HOSTIF WLAN_PLX
-#include "hfa384x.c"
-#include "prism2mgmt.c"
-#include "prism2mib.c"
-#include "prism2sta.c"
-
-#define PLX_ATTR_SIZE	0x1000	/* Attribute memory size - 4K bytes */
-#define COR_OFFSET	0x3e0	/* COR attribute offset of Prism2 PC card */
-#define COR_VALUE	0x41	/* Enable PC card with irq in level trigger */
-#define PLX_INTCSR	0x4c	/* Interrupt Control and Status Register */
-#define PLX_INTCSR_INTEN (1<<6) /* Interrupt Enable bit */
-#define PLX_MIN_ATTR_LEN 512    /* at least 2 x 256 is needed for CIS */
-
-/* 3Com 3CRW777A (PLX) board ID */
-#define PCIVENDOR_3COM       0x10B7
-#define PCIDEVICE_AIRCONNECT 0x7770
-
-/* Eumitcom PCI WL11000 PCI Adapter (PLX) board device+vendor ID */
-#define PCIVENDOR_EUMITCOM	0x1638UL
-#define PCIDEVICE_WL11000	0x1100UL
-
-/* Global Sun Tech GL24110P PCI Adapter (PLX) board device+vendor ID */
-#define PCIVENDOR_GLOBALSUN	0x16abUL
-#define PCIDEVICE_GL24110P	0x1101UL
-#define PCIDEVICE_GL24110P_ALT	0x1102UL
-
-/* Netgear MA301 PCI Adapter (PLX) board device+vendor ID */
-#define PCIVENDOR_NETGEAR	0x1385UL
-#define PCIDEVICE_MA301		0x4100UL
-
-/* US Robotics USR2410 PCI Adapter (PLX) board device+vendor ID */
-#define	PCIVENDOR_USROBOTICS    0x16ecUL
-#define PCIDEVICE_USR2410       0x3685UL
-
-/* Linksys WPC11 card with the WDT11 adapter (PLX) board device+vendor ID */
-#define	PCIVENDOR_Linksys       0x16abUL
-#define PCIDEVICE_Wpc11Wdt11    0x1102UL
-
-/* National Datacomm Corp SOHOware Netblaster II PCI */
-#define PCIVENDOR_NDC	        0x15e8UL
-#define PCIDEVICE_NCP130_PLX 	0x0130UL
-#define PCIDEVICE_NCP130_ASIC	0x0131UL
-
-/* NDC NCP130_PLX is also sold by Corega. Their name is CGWLPCIA11 */
-#define PCIVENDOR_COREGA       PCIVENDOR_NDC
-#define PCIDEVICE_CGWLPCIA11   PCIDEVICE_NCP130_PLX
-
-/* PCI Class & Sub-Class code, Network-'Other controller' */
-#define PCI_CLASS_NETWORK_OTHERS 0x280
-
-/*----------------------------------------------------------------
-* prism2sta_probe_plx
-*
-* Probe routine called when a PCI device w/ matching ID is found.
-* This PLX implementation uses the following map:
-*   BAR0: Unused
-*   BAR1: ????
-*   BAR2: PCMCIA attribute memory
-*   BAR3: PCMCIA i/o space
-* Here's the sequence:
-*   - Allocate the PCI resources.
-*   - Read the PCMCIA attribute memory to make sure we have a WLAN card
-*   - Reset the MAC using the PCMCIA COR
-*   - Initialize the netdev and wlan data
-*   - Initialize the MAC
-*
-* Arguments:
-*	pdev		ptr to pci device structure containing info about
-*			pci configuration.
-*	id		ptr to the device id entry that matched this device.
-*
-* Returns:
-*	zero		- success
-*	negative	- failed
-*
-* Side effects:
-*
-*
-* Call context:
-*	process thread
-*
-----------------------------------------------------------------*/
-static int __devinit
-prism2sta_probe_plx(
-	struct pci_dev			*pdev,
-	const struct pci_device_id	*id)
-{
-	int		result;
-        phys_t	pccard_ioaddr;
-	phys_t  pccard_attr_mem;
-        unsigned int    pccard_attr_len;
-	void __iomem *attr_mem = NULL;
-	UINT32		plx_addr;
-        wlandevice_t    *wlandev = NULL;
-	hfa384x_t	*hw = NULL;
-	int		reg;
-        u32		regic;
-
-	if (pci_enable_device(pdev))
-		return -EIO;
-
-	/* TMC7160 boards are special */
-	if ((pdev->vendor == PCIVENDOR_NDC) &&
-	    (pdev->device == PCIDEVICE_NCP130_ASIC)) {
-		unsigned long delay;
-
-		pccard_attr_mem = 0;
-		pccard_ioaddr = pci_resource_start(pdev, 1);
-
-		outb(0x45, pccard_ioaddr);
-		delay = jiffies + 1*HZ;
-		while (time_before(jiffies, delay));
-
-		if (inb(pccard_ioaddr) != 0x45) {
-			WLAN_LOG_ERROR("Initialize the TMC7160 failed. (0x%x)\n", inb(pccard_ioaddr));
-			return -EIO;
-		}
-
-		pccard_ioaddr = pci_resource_start(pdev, 2);
-		prism2_doreset = 0;
-
-		WLAN_LOG_INFO("NDC NCP130 with TMC716(ASIC) PCI interface device found at io:0x%x, irq:%d\n", pccard_ioaddr, pdev->irq);
-		goto init;
-	}
-
-	/* Collect the resource requirements */
-	pccard_attr_mem = pci_resource_start(pdev, 2);
-	pccard_attr_len = pci_resource_len(pdev, 2);
-        if (pccard_attr_len < PLX_MIN_ATTR_LEN)
-		return -EIO;
-
-	pccard_ioaddr = pci_resource_start(pdev, 3);
-
-	/* bjoern: We need to tell the card to enable interrupts, in
-	 * case the serial eprom didn't do this already. See the
-	 * PLX9052 data book, p8-1 and 8-24 for reference.
-	 * [MSM]: This bit of code came from the orinoco_cs driver.
-	 */
-	plx_addr = pci_resource_start(pdev, 1);
-
-	regic = 0;
-	regic = inl(plx_addr+PLX_INTCSR);
-	if(regic & PLX_INTCSR_INTEN) {
-		WLAN_LOG_DEBUG(1,
-			"%s: Local Interrupt already enabled\n", dev_info);
-	} else {
-		regic |= PLX_INTCSR_INTEN;
-		outl(regic, plx_addr+PLX_INTCSR);
-		regic = inl(plx_addr+PLX_INTCSR);
-		if(!(regic & PLX_INTCSR_INTEN)) {
-			WLAN_LOG_ERROR(
-				"%s: Couldn't enable Local Interrupts\n",
-				dev_info);
-			return -EIO;
-		}
-	}
-
-	/* These assignments are here in case of future mappings for
-	 * io space and irq that might be similar to ioremap
-	 */
-        if (!request_mem_region(pccard_attr_mem, pci_resource_len(pdev, 2), "Prism2")) {
-		WLAN_LOG_ERROR("%s: Couldn't reserve PCI memory region\n", dev_info);
-		return -EIO;
-        }
-
-	attr_mem = ioremap(pccard_attr_mem, pccard_attr_len);
-
-	WLAN_LOG_INFO("A PLX PCI/PCMCIA interface device found, "
-		"phymem:0x%llx, phyio=0x%x, irq:%d, "
-		"mem: 0x%lx\n",
-		(unsigned long long)pccard_attr_mem, pccard_ioaddr, pdev->irq,
-		(unsigned long)attr_mem);
-
-	/* Verify whether PC card is present.
-	 * [MSM] This needs improvement, the right thing to do is
-	 * probably to walk the CIS looking for the vendor and product
-	 * IDs.  It would be nice if this could be tied in with the
-	 * etc/pcmcia/wlan-ng.conf file.  Any volunteers?  ;-)
-	 */
-	if (
-	readb(attr_mem + 0) != 0x01 || readb(attr_mem + 2) != 0x03 ||
-	readb(attr_mem + 4) != 0x00 || readb(attr_mem + 6) != 0x00 ||
-	readb(attr_mem + 8) != 0xFF || readb(attr_mem + 10) != 0x17 ||
-	readb(attr_mem + 12) != 0x04 || readb(attr_mem + 14) != 0x67) {
-		WLAN_LOG_ERROR("Prism2 PC card CIS is invalid.\n");
-		return -EIO;
-        }
-        WLAN_LOG_INFO("A PCMCIA WLAN adapter was found.\n");
-
-        /* Write COR to enable PC card */
-	writeb(COR_VALUE, attr_mem + COR_OFFSET);
-	reg = readb(attr_mem + COR_OFFSET);
-
- init:
-
-	/*
-	 * Now do everything the same as a PCI device
-	 * [MSM] TODO: We could probably factor this out of pcmcia/pci/plx
-	 * and perhaps usb.  Perhaps a task for another day.......
-	 */
-
-	if ((wlandev = create_wlan()) == NULL) {
-		WLAN_LOG_ERROR("%s: Memory allocation failure.\n", dev_info);
-		result = -EIO;
-		goto failed;
-	}
-
-	hw = wlandev->priv;
-
-	if ( wlan_setup(wlandev) != 0 ) {
-		WLAN_LOG_ERROR("%s: wlan_setup() failed.\n", dev_info);
-		result = -EIO;
-		goto failed;
-	}
-
-	/* Setup netdevice's ability to report resources
-	 * Note: the netdevice was allocated by wlan_setup()
-	 */
-        wlandev->netdev->irq = pdev->irq;
-        wlandev->netdev->base_addr = pccard_ioaddr;
-        wlandev->netdev->mem_start = (unsigned long)attr_mem;
-        wlandev->netdev->mem_end = (unsigned long)attr_mem + pci_resource_len(pdev, 0);
-
-	/* Initialize the hw data */
-        hfa384x_create(hw, wlandev->netdev->irq, pccard_ioaddr, attr_mem);
-	hw->wlandev = wlandev;
-
-	/* Register the wlandev, this gets us a name and registers the
-	 * linux netdevice.
-	 */
-	SET_MODULE_OWNER(wlandev->netdev);
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0))
-       SET_NETDEV_DEV(wlandev->netdev, &(pdev->dev));
-#endif
-        if ( register_wlandev(wlandev) != 0 ) {
-		WLAN_LOG_ERROR("%s: register_wlandev() failed.\n", dev_info);
-		result = -EIO;
-		goto failed;
-        }
-
-#if 0
-	/* TODO: Move this and an irq test into an hfa384x_testif() routine.
-	 */
-	outw(PRISM2STA_MAGIC, HFA384x_SWSUPPORT(wlandev->netdev->base_addr));
-	reg=inw( HFA384x_SWSUPPORT(wlandev->netdev->base_addr));
-	if ( reg != PRISM2STA_MAGIC ) {
-		WLAN_LOG_ERROR("MAC register access test failed!\n");
- 		result = -EIO;
-		goto failed;
-	}
-#endif
-
-	/* Do a chip-level reset on the MAC */
-	if (prism2_doreset) {
-		result = hfa384x_corereset(hw,
-				prism2_reset_holdtime,
-				prism2_reset_settletime, 0);
-		if (result != 0) {
-			unregister_wlandev(wlandev);
-			hfa384x_destroy(hw);
-			WLAN_LOG_ERROR(
-				"%s: hfa384x_corereset() failed.\n",
-				dev_info);
-			result = -EIO;
-			goto failed;
-		}
-	}
-
-	pci_set_drvdata(pdev, wlandev);
-
-	/* Shouldn't actually hook up the IRQ until we
-	 * _know_ things are alright.  A test routine would help.
-	 */
-       	request_irq(wlandev->netdev->irq, hfa384x_interrupt,
-		SA_SHIRQ, wlandev->name, wlandev);
-
-	wlandev->msdstate = WLAN_MSD_HWPRESENT;
-
-	result = 0;
-
-	goto done;
-
- failed:
-
-	pci_set_drvdata(pdev, NULL);
-	if (wlandev)	kfree(wlandev);
-	if (hw)		kfree(hw);
-        if (attr_mem)        iounmap(attr_mem);
-	pci_release_regions(pdev);
-        pci_disable_device(pdev);
-
- done:
-	DBFEXIT;
-        return result;
-}
-
-static void __devexit prism2sta_remove_plx(struct pci_dev *pdev)
-{
-       	wlandevice_t		*wlandev;
-	hfa384x_t               *hw;
-
-	wlandev = (wlandevice_t *) pci_get_drvdata(pdev);
-	hw = wlandev->priv;
-
-	p80211netdev_hwremoved(wlandev);
-
-	/* reset hardware */
-	prism2sta_ifstate(wlandev, P80211ENUM_ifstate_disable);
-
-        if (pdev->irq)
-		free_irq(pdev->irq, wlandev);
-
-	unregister_wlandev(wlandev);
-
-	/* free local stuff */
-	if (hw) {
-		hfa384x_destroy(hw);
-		kfree(hw);
-	}
-
-	iounmap((void __iomem *)wlandev->netdev->mem_start);
-	wlan_unsetup(wlandev);
-
-	pci_release_regions(pdev);
-        pci_disable_device(pdev);
-	pci_set_drvdata(pdev, NULL);
-
-	kfree(wlandev);
-}
-
-static struct pci_device_id plx_id_tbl[] = {
-	{
-		PCIVENDOR_EUMITCOM, PCIDEVICE_WL11000,
-		PCI_ANY_ID, PCI_ANY_ID,
-		0, 0,
-		/* Driver data, we just put the name here */
-		(unsigned long)"Eumitcom WL11000 PCI(PLX) card"
-	},
-	{
-		PCIVENDOR_GLOBALSUN, PCIDEVICE_GL24110P,
-		PCI_ANY_ID, PCI_ANY_ID,
-		0, 0,
-		/* Driver data, we just put the name here */
-		(unsigned long)"Global Sun Tech GL24110P PCI(PLX) card"
-	},
-	{
-		PCIVENDOR_GLOBALSUN, PCIDEVICE_GL24110P_ALT,
-		PCI_ANY_ID, PCI_ANY_ID,
-		0, 0,
-		/* Driver data, we just put the name here */
-		(unsigned long)"Global Sun Tech GL24110P PCI(PLX) card"
-	},
-	{
-		PCIVENDOR_NETGEAR, PCIDEVICE_MA301,
-		PCI_ANY_ID, PCI_ANY_ID,
-		0, 0,
-		/* Driver data, we just put the name here */
-		(unsigned long)"Global Sun Tech GL24110P PCI(PLX) card"
-	},
-	{
-		PCIVENDOR_USROBOTICS, PCIDEVICE_USR2410,
-		PCI_ANY_ID, PCI_ANY_ID,
-		0, 0,
-		/* Driver data, we just put the name here */
-		(unsigned long)"US Robotics USR2410 PCI(PLX) card"
-	},
-	{
-		PCIVENDOR_Linksys, PCIDEVICE_Wpc11Wdt11,
-		PCI_ANY_ID, PCI_ANY_ID,
-		0, 0,
-		/* Driver data, we just put the name here */
-		(unsigned long)"Linksys WPC11 with WDT11 PCI(PLX) adapter"
-	},
-	{
-	        PCIVENDOR_NDC, PCIDEVICE_NCP130_PLX,
-		PCI_ANY_ID, PCI_ANY_ID,
-		0, 0,
-		/* Driver data, we just put the name here */
-		(unsigned long)"NDC Netblaster II PCI(PLX)"
-	},
-	{
-	        PCIVENDOR_NDC, PCIDEVICE_NCP130_ASIC,
-		PCI_ANY_ID, PCI_ANY_ID,
-		0, 0,
-		/* Driver data, we just put the name here */
-		(unsigned long)"NDC Netblaster II PCI(TMC7160)"
-	},
-	{
-		PCIVENDOR_3COM, PCIDEVICE_AIRCONNECT,
-		PCI_ANY_ID, PCI_ANY_ID,
-		0, 0,
-		/* Driver data, we just put the name here */
-		(unsigned long)"3Com AirConnect PCI 802.11b 11Mb/s WLAN Controller"
-	},
-	{
-		0, 0, 0, 0, 0, 0, 0
-	}
-};
-
-MODULE_DEVICE_TABLE(pci, plx_id_tbl);
-
-/* Function declared here because of ptr reference below */
-static int __devinit prism2sta_probe_plx(struct pci_dev *pdev,
-					 const struct pci_device_id *);
-static void __devexit prism2sta_remove_plx(struct pci_dev *pdev);
-
-static struct pci_driver prism2_plx_drv_id = {
-        .name = "prism2_plx",
-        .id_table = plx_id_tbl,
-        .probe = prism2sta_probe_plx,
-        .remove = prism2sta_remove_plx,
-#ifdef CONFIG_PM
-        .suspend = prism2sta_suspend_pci,
-        .resume = prism2sta_resume_pci,
-#endif
-};
-
-#ifdef MODULE
-
-static int __init prism2plx_init(void)
-{
-        WLAN_LOG_NOTICE("%s Loaded\n", version);
-	return pci_module_init(&prism2_plx_drv_id);
-};
-
-static void __exit prism2plx_cleanup(void)
-{
-	pci_unregister_driver(&prism2_plx_drv_id);
-};
-
-module_init(prism2plx_init);
-module_exit(prism2plx_cleanup);
-
-#endif // MODULE
-
-
-int hfa384x_corereset(hfa384x_t *hw, int holdtime, int settletime, int genesis)
-{
-	int		result = 0;
-
-#define COR_OFFSET	0x3e0	/* COR attribute offset of Prism2 PC card */
-#define COR_VALUE	0x41	/* Enable PC card with irq in level trigger */
-
-#define HCR_OFFSET	0x3e2	/* HCR attribute offset of Prism2 PC card */
-
-	UINT8		corsave;
-	DBFENTER;
-
-	WLAN_LOG_DEBUG(3, "Doing reset via direct COR access.\n");
-
-	/* Collect COR */
-	corsave = readb(hw->membase + COR_OFFSET);
-	/* Write reset bit (BIT7) */
-	writeb(corsave | BIT7, hw->membase + COR_OFFSET);
-	/* Hold for holdtime */
-	mdelay(holdtime);
-
-	if (genesis) {
-		writeb(genesis, hw->membase + HCR_OFFSET);
-		/* Hold for holdtime */
-		mdelay(holdtime);
-	}
-
-	/* Clear reset bit */
-	writeb(corsave & ~BIT7, hw->membase + COR_OFFSET);
-	/* Wait for settletime */
-	mdelay(settletime);
-	/* Set non-reset bits back what they were */
-	writeb(corsave, hw->membase + COR_OFFSET);
-	DBFEXIT;
-	return result;
-}
diff --git a/drivers/staging/wlan-ng/prism2mgmt.c b/drivers/staging/wlan-ng/prism2mgmt.c
index c975025b6ae..f1727ba6ec6 100644
--- a/drivers/staging/wlan-ng/prism2mgmt.c
+++ b/drivers/staging/wlan-ng/prism2mgmt.c
@@ -61,10 +61,6 @@
 /* System Includes */
 #define WLAN_DBVAR	prism2_debug
 
-#include "version.h"
-
-
-#include <linux/version.h>
 
 #include <linux/if_arp.h>
 #include <linux/module.h>
@@ -79,19 +75,7 @@
 #include <asm/io.h>
 #include <asm/byteorder.h>
 #include <linux/random.h>
-
-#if (WLAN_HOSTIF == WLAN_USB)
 #include <linux/usb.h>
-#endif
-
-#if (WLAN_HOSTIF == WLAN_PCMCIA)
-#include <pcmcia/version.h>
-#include <pcmcia/cs_types.h>
-#include <pcmcia/cs.h>
-#include <pcmcia/cistpl.h>
-#include <pcmcia/ds.h>
-#include <pcmcia/cisreg.h>
-#endif
 
 #include "wlan_compat.h"
 
@@ -109,89 +93,12 @@
 #include "hfa384x.h"
 #include "prism2mgmt.h"
 
-/*================================================================*/
-/* Local Constants */
-
-
-/*================================================================*/
-/* Local Macros */
-
 /* Converts 802.11 format rate specifications to prism2 */
 #define p80211rate_to_p2bit(n)	((((n)&~BIT7) == 2) ? BIT0 : \
 				 (((n)&~BIT7) == 4) ? BIT1 : \
 				 (((n)&~BIT7) == 11) ? BIT2 : \
 				 (((n)&~BIT7) == 22) ? BIT3 : 0)
 
-/*================================================================*/
-/* Local Types */
-
-
-/*================================================================*/
-/* Local Static Definitions */
-
-
-/*================================================================*/
-/* Local Function Declarations */
-
-
-/*================================================================*/
-/* Function Definitions */
-
-
-/*----------------------------------------------------------------
-* prism2mgmt_powermgmt
-*
-* Set the power management state of this station's MAC.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-*	interrupt
-----------------------------------------------------------------*/
-int prism2mgmt_powermgmt(wlandevice_t *wlandev, void *msgp)
-{
-	int 			result = 0;
-	hfa384x_t		*hw = wlandev->priv;
-	p80211msg_dot11req_powermgmt_t	*msg = msgp;
-
-	DBFENTER;
-
-	if (!hw->ap) {
-
-		/*** STATION ***/
-
-		/*
-		 * Set CNFPMENABLED (on or off)
-		 * Set CNFMULTICASTRX (if PM on, otherwise clear)
-		 * Spout a notice stating that SleepDuration and
-		 * HoldoverDuration and PMEPS also have an impact.
-		 */
-		/* Powermgmt is currently unsupported for STA */
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-	} else {
-
-		/*** ACCESS POINT ***/
-
-		/* Powermgmt is never supported for AP */
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-	}
-
-	DBFEXIT;
-	return result;
-}
-
-
 /*----------------------------------------------------------------
 * prism2mgmt_scan
 *
@@ -221,7 +128,7 @@ int prism2mgmt_scan(wlandevice_t *wlandev, void *msgp)
 	int 			result = 0;
 	hfa384x_t		*hw = wlandev->priv;
 	p80211msg_dot11req_scan_t	*msg = msgp;
-        UINT16                  roamingmode, word;
+        u16                  roamingmode, word;
 	int                     i, timeout;
 	int                     istmpenable = 0;
 
@@ -229,13 +136,6 @@ int prism2mgmt_scan(wlandevice_t *wlandev, void *msgp)
 
 	DBFENTER;
 
-        if (hw->ap) {
-                WLAN_LOG_ERROR("Prism2 in AP mode cannot perform scans.\n");
-                result = 1;
-                msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-                goto exit;
-        }
-
         /* gatekeeper check */
         if (HFA384x_FIRMWARE_VERSION(hw->ident_sta_fw.major,
                                      hw->ident_sta_fw.minor,
@@ -296,7 +196,7 @@ int prism2mgmt_scan(wlandevice_t *wlandev, void *msgp)
         /* set up the channel list */
         word = 0;
         for (i = 0; i < msg->channellist.data.len; i++) {
-                UINT8 channel = msg->channellist.data.data[i];
+                u8 channel = msg->channellist.data.data[i];
                 if (channel > 14) continue;
                 /* channel 1 is BIT0 ... channel 14 is BIT13 */
                 word |= (1 << (channel-1));
@@ -317,7 +217,7 @@ int prism2mgmt_scan(wlandevice_t *wlandev, void *msgp)
 		goto exit;
 	}
 	if (word == HFA384x_PORTSTATUS_DISABLED) {
-		UINT16 wordbuf[17];
+		u16 wordbuf[17];
 
 		result = hfa384x_drvr_setconfig16(hw,
 			HFA384x_RID_CNFROAMINGMODE,
@@ -480,12 +380,6 @@ int prism2mgmt_scan_results(wlandevice_t *wlandev, void *msgp)
 
 	req->resultcode.status = P80211ENUM_msgitem_status_data_ok;
 
-	if (hw->ap) {
-		result = 1;
-		req->resultcode.data = P80211ENUM_resultcode_not_supported;
-		goto exit;
-	}
-
 	if (! hw->scanresults) {
 		WLAN_LOG_ERROR("dot11req_scan_results can only be used after a successful dot11req_scan.\n");
 		result = 2;
@@ -612,567 +506,6 @@ int prism2mgmt_scan_results(wlandevice_t *wlandev, void *msgp)
 	return result;
 }
 
-
-/*----------------------------------------------------------------
-* prism2mgmt_join
-*
-* Join a BSS whose BSS description was previously obtained with
-* a scan.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-*	interrupt
-----------------------------------------------------------------*/
-int prism2mgmt_join(wlandevice_t *wlandev, void *msgp)
-{
-	int 			result = 0;
-	hfa384x_t		*hw = wlandev->priv;
-	p80211msg_dot11req_join_t	*msg = msgp;
-	DBFENTER;
-
-	if (!hw->ap) {
-
-		/*** STATION ***/
-
-		/* TODO: Implement after scan */
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-	} else {
-
-		/*** ACCESS POINT ***/
-
-		/* Never supported by APs */
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-	}
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* prism2mgmt_p2_join
-*
-* Join a specific BSS
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-*	interrupt
-----------------------------------------------------------------*/
-int prism2mgmt_p2_join(wlandevice_t *wlandev, void *msgp)
-{
-	int 			result = 0;
-	hfa384x_t		*hw = wlandev->priv;
-	p80211msg_p2req_join_t	*msg = msgp;
-	UINT16			reg;
-	p80211pstrd_t		*pstr;
-	UINT8			bytebuf[256];
-	hfa384x_bytestr_t	*p2bytestr = (hfa384x_bytestr_t*)bytebuf;
-        hfa384x_JoinRequest_data_t	joinreq;
-	DBFENTER;
-
-	if (!hw->ap) {
-
-		wlandev->macmode = WLAN_MACMODE_NONE;
-
-		/*** STATION ***/
-		/* Set the PortType */
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_success;
-
-		/* ess port */
-		result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFPORTTYPE, 1);
-		if ( result ) {
-			WLAN_LOG_ERROR("Failed to set Port Type\n");
-			goto failed;
-		}
-
-		/* Set the auth type */
-		if ( msg->authtype.data == P80211ENUM_authalg_sharedkey ) {
-			reg = HFA384x_CNFAUTHENTICATION_SHAREDKEY;
-		} else {
-			reg = HFA384x_CNFAUTHENTICATION_OPENSYSTEM;
-		}
-		result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFAUTHENTICATION, reg);
-		if ( result ) {
-			WLAN_LOG_ERROR("Failed to set Authentication\n");
-			goto failed;
-		}
-
-		/* Turn off all roaming */
-		hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFROAMINGMODE, 3);
-		if ( result ) {
-			WLAN_LOG_ERROR("Failed to Turn off Roaming\n");
-			goto failed;
-		}
-
-		/* Basic rates */
-                reg = 0;
-		if ( msg->basicrate1.status == P80211ENUM_msgitem_status_data_ok ) {
-			reg = p80211rate_to_p2bit(msg->basicrate1.data);
-		}
-		if ( msg->basicrate2.status == P80211ENUM_msgitem_status_data_ok ) {
-			reg |= p80211rate_to_p2bit(msg->basicrate2.data);
-		}
-		if ( msg->basicrate3.status == P80211ENUM_msgitem_status_data_ok ) {
-			reg |= p80211rate_to_p2bit(msg->basicrate3.data);
-		}
-		if ( msg->basicrate4.status == P80211ENUM_msgitem_status_data_ok ) {
-			reg |= p80211rate_to_p2bit(msg->basicrate4.data);
-		}
-		if ( msg->basicrate5.status == P80211ENUM_msgitem_status_data_ok ) {
-			reg |= p80211rate_to_p2bit(msg->basicrate5.data);
-		}
-		if ( msg->basicrate6.status == P80211ENUM_msgitem_status_data_ok ) {
-			reg |= p80211rate_to_p2bit(msg->basicrate6.data);
-		}
-		if ( msg->basicrate7.status == P80211ENUM_msgitem_status_data_ok ) {
-			reg |= p80211rate_to_p2bit(msg->basicrate7.data);
-		}
-		if ( msg->basicrate8.status == P80211ENUM_msgitem_status_data_ok ) {
-			reg |= p80211rate_to_p2bit(msg->basicrate8.data);
-		}
-		if( reg == 0)
-			 reg = 0x03;
-		result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFBASICRATES, reg);
-		if ( result ) {
-			WLAN_LOG_ERROR("Failed to set basicrates=%d.\n", reg);
-			goto failed;
-		}
-
-		/* Operational rates (supprates and txratecontrol) */
-		reg = 0;
-		if ( msg->operationalrate1.status == P80211ENUM_msgitem_status_data_ok ) {
-			reg = p80211rate_to_p2bit(msg->operationalrate1.data);
-		}
-		if ( msg->operationalrate2.status == P80211ENUM_msgitem_status_data_ok ) {
-			reg |= p80211rate_to_p2bit(msg->operationalrate2.data);
-		}
-		if ( msg->operationalrate3.status == P80211ENUM_msgitem_status_data_ok ) {
-			reg |= p80211rate_to_p2bit(msg->operationalrate3.data);
-		}
-		if ( msg->operationalrate4.status == P80211ENUM_msgitem_status_data_ok ) {
-			reg |= p80211rate_to_p2bit(msg->operationalrate4.data);
-		}
-		if ( msg->operationalrate5.status == P80211ENUM_msgitem_status_data_ok ) {
-			reg |= p80211rate_to_p2bit(msg->operationalrate5.data);
-		}
-		if ( msg->operationalrate6.status == P80211ENUM_msgitem_status_data_ok ) {
-			reg |= p80211rate_to_p2bit(msg->operationalrate6.data);
-		}
-		if ( msg->operationalrate7.status == P80211ENUM_msgitem_status_data_ok ) {
-			reg |= p80211rate_to_p2bit(msg->operationalrate7.data);
-		}
-		if ( msg->operationalrate8.status == P80211ENUM_msgitem_status_data_ok ) {
-			reg |= p80211rate_to_p2bit(msg->operationalrate8.data);
-		}
-		if( reg == 0)
-			 reg = 0x0f;
-		result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFSUPPRATES, reg);
-		if ( result ) {
-			WLAN_LOG_ERROR("Failed to set supprates=%d.\n", reg);
-			goto failed;
-		}
-
- 		result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_TXRATECNTL, reg);
-		if ( result ) {
-			WLAN_LOG_ERROR("Failed to set txrates=%d.\n", reg);
-			goto failed;
-		}
-
-		/* Set the ssid */
-		memset(bytebuf, 0, 256);
-		pstr = (p80211pstrd_t*)&(msg->ssid.data);
-		prism2mgmt_pstr2bytestr(p2bytestr, pstr);
-		result = hfa384x_drvr_setconfig(
-			hw, HFA384x_RID_CNFDESIREDSSID,
-			bytebuf, HFA384x_RID_CNFDESIREDSSID_LEN);
-		if ( result ) {
-			WLAN_LOG_ERROR("Failed to set SSID\n");
-			goto failed;
-		}
-
-		/* Enable the Port */
-		result = hfa384x_cmd_enable(hw, 0);
-		if ( result ) {
-			WLAN_LOG_ERROR("Enable macport failed, result=%d.\n", result);
-			goto failed;
-		}
-
-		/* Fill in the join request */
-		joinreq.channel = msg->channel.data;
-		memcpy( joinreq.bssid, ((unsigned char *) &msg->bssid.data) + 1, WLAN_BSSID_LEN);
-		hw->joinreq = joinreq;
-		hw->join_ap = 1;
-
-		/* Send the join request */
-		result = hfa384x_drvr_setconfig( hw,
-			HFA384x_RID_JOINREQUEST,
-			&joinreq, HFA384x_RID_JOINREQUEST_LEN);
-                if(result != 0) {
-			WLAN_LOG_ERROR("Join request failed, result=%d.\n", result);
-			goto failed;
-		}
-
-	} else {
-
-		/*** ACCESS POINT ***/
-
-		/* Never supported by APs */
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-	}
-
-        goto done;
-failed:
-	WLAN_LOG_DEBUG(1, "Failed to set a config option, result=%d\n", result);
-	msg->resultcode.data = P80211ENUM_resultcode_invalid_parameters;
-
-done:
-        result = 0;
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* prism2mgmt_authenticate
-*
-* Station should be begin an authentication exchange.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-*	interrupt
-----------------------------------------------------------------*/
-int prism2mgmt_authenticate(wlandevice_t *wlandev, void *msgp)
-{
-	int 			result = 0;
-	hfa384x_t		*hw = wlandev->priv;
-	p80211msg_dot11req_authenticate_t	*msg = msgp;
-	DBFENTER;
-
-	if (!hw->ap) {
-
-		/*** STATION ***/
-
-		/* TODO: Decide how we're going to handle this one w/ Prism2 */
-		/*       It could be entertaining since Prism2 doesn't have  */
-		/*       an explicit way to control this */
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-	} else {
-
-		/*** ACCESS POINT ***/
-
-		/* Never supported by APs */
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-	}
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* prism2mgmt_deauthenticate
-*
-* Send a deauthenticate notification.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-*	interrupt
-----------------------------------------------------------------*/
-int prism2mgmt_deauthenticate(wlandevice_t *wlandev, void *msgp)
-{
-	int 			result = 0;
-	hfa384x_t		*hw = wlandev->priv;
-	p80211msg_dot11req_deauthenticate_t	*msg = msgp;
-	DBFENTER;
-
-	if (!hw->ap) {
-
-		/*** STATION ***/
-
-		/* TODO: Decide how we're going to handle this one w/ Prism2 */
-		/*       It could be entertaining since Prism2 doesn't have  */
-		/*       an explicit way to control this */
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-	} else {
-
-		/*** ACCESS POINT ***/
-		hfa384x_drvr_handover(hw, msg->peerstaaddress.data.data);
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_success;
-	}
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* prism2mgmt_associate
-*
-* Associate with an ESS.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-*	interrupt
-----------------------------------------------------------------*/
-int prism2mgmt_associate(wlandevice_t *wlandev, void *msgp)
-{
-	hfa384x_t		*hw = wlandev->priv;
-	int 			result = 0;
-	p80211msg_dot11req_associate_t	*msg = msgp;
-	DBFENTER;
-
-	if (!hw->ap) {
-
-		/*** STATION ***/
-
-#if 0
-		/* Set the TxRates */
-		reg = 0x000f;
-		hfa384x_drvr_setconfig16(hw, HFA384x_RID_TXRATECNTL, reg);
-#endif
-
-		/* Set the PortType */
-		/* ess port */
-		hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFPORTTYPE, 1);
-
-		/* Enable the Port */
-		hfa384x_drvr_enable(hw, 0);
-
-		/* Set the resultcode */
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_success;
-
-	} else {
-
-		/*** ACCESS POINT ***/
-
-		/* Never supported on AP */
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-	}
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* prism2mgmt_reassociate
-*
-* Renew association because of a BSS change.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-*	interrupt
-----------------------------------------------------------------*/
-int prism2mgmt_reassociate(wlandevice_t *wlandev, void *msgp)
-{
-	int 			result = 0;
-	hfa384x_t		*hw = wlandev->priv;
-	p80211msg_dot11req_reassociate_t	*msg = msgp;
-	DBFENTER;
-
-	if (!hw->ap) {
-
-		/*** STATION ***/
-
-		/* TODO: Not supported yet...not sure how we're going to do it */
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-	} else {
-
-		/*** ACCESS POINT ***/
-
-		/* Never supported on AP */
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-	}
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* prism2mgmt_disassociate
-*
-* Send a disassociation notification.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-*	interrupt
-----------------------------------------------------------------*/
-int prism2mgmt_disassociate(wlandevice_t *wlandev, void *msgp)
-{
-	int 			result = 0;
-	hfa384x_t		*hw = wlandev->priv;
-	p80211msg_dot11req_disassociate_t	*msg = msgp;
-	DBFENTER;
-
-	if (!hw->ap) {
-
-		/*** STATION ***/
-
-		/* TODO: Not supported yet...not sure how to do it */
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-	} else {
-
-		/*** ACCESS POINT ***/
-		hfa384x_drvr_handover(hw, msg->peerstaaddress.data.data);
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_success;
-	}
-
-	DBFEXIT;
-	return result;
-}
-
-
-/*----------------------------------------------------------------
-* prism2mgmt_reset
-*
-* Reset the MAC and MSD.  The p80211 layer has it's own handling
-* that should be done before and after this function.
-* Procedure:
-*   - disable system interrupts ??
-*   - disable MAC interrupts
-*   - restore system interrupts
-*   - issue the MAC initialize command
-*   - clear any MSD level state (including timers, queued events,
-*     etc.).  Note that if we're removing timer'd/queue events, we may
-*     need to have remained in the system interrupt disabled state.
-*     We should be left in the same state that we're in following
-*     driver initialization.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer, MAY BE NULL! for a driver local
-*			call.
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread, commonly wlanctl, but might be rmmod/pci_close.
-----------------------------------------------------------------*/
-int prism2mgmt_reset(wlandevice_t *wlandev, void *msgp)
-{
-	int 			result = 0;
-	hfa384x_t		*hw = wlandev->priv;
-	p80211msg_dot11req_reset_t	*msg = msgp;
-	DBFENTER;
-
-	/*
-	 * This is supported on both AP and STA and it's not allowed
-	 * to fail.
-	 */
-	if ( msgp ) {
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_success;
-		WLAN_LOG_INFO("dot11req_reset: the macaddress and "
-			"setdefaultmib arguments are currently unsupported.\n");
-	}
-
-	/*
-	 * If we got this far, the MSD must be in the MSDRUNNING state
-	 * therefore, we must stop and then restart the hw/MAC combo.
-	 */
-	hfa384x_drvr_stop(hw);
-	result = hfa384x_drvr_start(hw);
-	if (result != 0) {
-		WLAN_LOG_ERROR("dot11req_reset: Initialize command failed,"
-				" bad things will happen from here.\n");
-		return 0;
-	}
-
-	DBFEXIT;
-	return 0;
-}
-
-
 /*----------------------------------------------------------------
 * prism2mgmt_start
 *
@@ -1199,10 +532,9 @@ int prism2mgmt_start(wlandevice_t *wlandev, void *msgp)
 	p80211msg_dot11req_start_t	*msg = msgp;
 
 	p80211pstrd_t		*pstr;
-	UINT8			bytebuf[80];
+	u8			bytebuf[80];
 	hfa384x_bytestr_t	*p2bytestr = (hfa384x_bytestr_t*)bytebuf;
-	hfa384x_PCFInfo_data_t	*pcfinfo = (hfa384x_PCFInfo_data_t*)bytebuf;
-	UINT16			word;
+	u16			word;
 	DBFENTER;
 
 	wlandev->macmode = WLAN_MACMODE_NONE;
@@ -1210,170 +542,45 @@ int prism2mgmt_start(wlandevice_t *wlandev, void *msgp)
 	/* Set the SSID */
 	memcpy(&wlandev->ssid, &msg->ssid.data, sizeof(msg->ssid.data));
 
-	if (!hw->ap) {
-		/*** ADHOC IBSS ***/
-		/* see if current f/w is less than 8c3 */
-		if (HFA384x_FIRMWARE_VERSION(hw->ident_sta_fw.major,
-					     hw->ident_sta_fw.minor,
-					     hw->ident_sta_fw.variant) <
-		    HFA384x_FIRMWARE_VERSION(0,8,3)) {
-			/* Ad-Hoc not quite supported on Prism2 */
-			msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-			msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-			goto done;
-		}
-
-  		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-
-		/*** STATION ***/
-		/* Set the REQUIRED config items */
-		/* SSID */
-		pstr = (p80211pstrd_t*)&(msg->ssid.data);
-		prism2mgmt_pstr2bytestr(p2bytestr, pstr);
-		result = hfa384x_drvr_setconfig( hw, HFA384x_RID_CNFOWNSSID,
-				bytebuf, HFA384x_RID_CNFOWNSSID_LEN);
-		if ( result ) {
-			WLAN_LOG_ERROR("Failed to set CnfOwnSSID\n");
-			goto failed;
-		}
-		result = hfa384x_drvr_setconfig( hw, HFA384x_RID_CNFDESIREDSSID,
-				bytebuf, HFA384x_RID_CNFDESIREDSSID_LEN);
-		if ( result ) {
-			WLAN_LOG_ERROR("Failed to set CnfDesiredSSID\n");
-			goto failed;
-		}
-
-		/* bsstype - we use the default in the ap firmware */
-		/* IBSS port */
-		hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFPORTTYPE, 0);
-
-		/* beacon period */
-		word = msg->beaconperiod.data;
-		result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFAPBCNINT, word);
-		if ( result ) {
-			WLAN_LOG_ERROR("Failed to set beacon period=%d.\n", word);
-			goto failed;
-		}
-
-		/* dschannel */
-		word = msg->dschannel.data;
-		result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFOWNCHANNEL, word);
-		if ( result ) {
-			WLAN_LOG_ERROR("Failed to set channel=%d.\n", word);
-			goto failed;
-		}
-		/* Basic rates */
-		word = p80211rate_to_p2bit(msg->basicrate1.data);
-		if ( msg->basicrate2.status == P80211ENUM_msgitem_status_data_ok ) {
-			word |= p80211rate_to_p2bit(msg->basicrate2.data);
-		}
-		if ( msg->basicrate3.status == P80211ENUM_msgitem_status_data_ok ) {
-			word |= p80211rate_to_p2bit(msg->basicrate3.data);
-		}
-		if ( msg->basicrate4.status == P80211ENUM_msgitem_status_data_ok ) {
-			word |= p80211rate_to_p2bit(msg->basicrate4.data);
-		}
-		if ( msg->basicrate5.status == P80211ENUM_msgitem_status_data_ok ) {
-			word |= p80211rate_to_p2bit(msg->basicrate5.data);
-		}
-		if ( msg->basicrate6.status == P80211ENUM_msgitem_status_data_ok ) {
-			word |= p80211rate_to_p2bit(msg->basicrate6.data);
-		}
-		if ( msg->basicrate7.status == P80211ENUM_msgitem_status_data_ok ) {
-			word |= p80211rate_to_p2bit(msg->basicrate7.data);
-		}
-		if ( msg->basicrate8.status == P80211ENUM_msgitem_status_data_ok ) {
-			word |= p80211rate_to_p2bit(msg->basicrate8.data);
-		}
-		result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFBASICRATES, word);
-		if ( result ) {
-			WLAN_LOG_ERROR("Failed to set basicrates=%d.\n", word);
-			goto failed;
-		}
-
-		/* Operational rates (supprates and txratecontrol) */
-		word = p80211rate_to_p2bit(msg->operationalrate1.data);
-		if ( msg->operationalrate2.status == P80211ENUM_msgitem_status_data_ok ) {
-			word |= p80211rate_to_p2bit(msg->operationalrate2.data);
-		}
-		if ( msg->operationalrate3.status == P80211ENUM_msgitem_status_data_ok ) {
-			word |= p80211rate_to_p2bit(msg->operationalrate3.data);
-		}
-		if ( msg->operationalrate4.status == P80211ENUM_msgitem_status_data_ok ) {
-			word |= p80211rate_to_p2bit(msg->operationalrate4.data);
-		}
-		if ( msg->operationalrate5.status == P80211ENUM_msgitem_status_data_ok ) {
-			word |= p80211rate_to_p2bit(msg->operationalrate5.data);
-		}
-		if ( msg->operationalrate6.status == P80211ENUM_msgitem_status_data_ok ) {
-			word |= p80211rate_to_p2bit(msg->operationalrate6.data);
-		}
-		if ( msg->operationalrate7.status == P80211ENUM_msgitem_status_data_ok ) {
-			word |= p80211rate_to_p2bit(msg->operationalrate7.data);
-		}
-		if ( msg->operationalrate8.status == P80211ENUM_msgitem_status_data_ok ) {
-			word |= p80211rate_to_p2bit(msg->operationalrate8.data);
-		}
-		result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFSUPPRATES, word);
-		if ( result ) {
-			WLAN_LOG_ERROR("Failed to set supprates=%d.\n", word);
-			goto failed;
-		}
-
- 		result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_TXRATECNTL, word);
-		if ( result ) {
-			WLAN_LOG_ERROR("Failed to set txrates=%d.\n", word);
-			goto failed;
-		}
-
-		/* Set the macmode so the frame setup code knows what to do */
-		if ( msg->bsstype.data == P80211ENUM_bsstype_independent ) {
-			wlandev->macmode = WLAN_MACMODE_IBSS_STA;
-			/* lets extend the data length a bit */
-			hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFMAXDATALEN, 2304);
-		}
-
-		/* Enable the Port */
-		result = hfa384x_drvr_enable(hw, 0);
-		if ( result ) {
-			WLAN_LOG_ERROR("Enable macport failed, result=%d.\n", result);
-			goto failed;
-		}
-
-		msg->resultcode.data = P80211ENUM_resultcode_success;
-
+	/*** ADHOC IBSS ***/
+	/* see if current f/w is less than 8c3 */
+	if (HFA384x_FIRMWARE_VERSION(hw->ident_sta_fw.major,
+				     hw->ident_sta_fw.minor,
+				     hw->ident_sta_fw.variant) <
+	    HFA384x_FIRMWARE_VERSION(0,8,3)) {
+		/* Ad-Hoc not quite supported on Prism2 */
+		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
+		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
 		goto done;
 	}
 
-	/*** ACCESS POINT ***/
-
 	msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
 
-	/* Validate the command, if BSStype=infra is the tertiary loaded? */
-	if ( msg->bsstype.data == P80211ENUM_bsstype_independent ) {
-		WLAN_LOG_ERROR("AP driver cannot create IBSS.\n");
-		goto failed;
-	} else if ( hw->cap_sup_sta.id != 5) {
-		WLAN_LOG_ERROR("AP driver failed to detect AP firmware.\n");
-		goto failed;
-	}
-
+	/*** STATION ***/
 	/* Set the REQUIRED config items */
 	/* SSID */
 	pstr = (p80211pstrd_t*)&(msg->ssid.data);
 	prism2mgmt_pstr2bytestr(p2bytestr, pstr);
 	result = hfa384x_drvr_setconfig( hw, HFA384x_RID_CNFOWNSSID,
-				bytebuf, HFA384x_RID_CNFOWNSSID_LEN);
+					 bytebuf, HFA384x_RID_CNFOWNSSID_LEN);
+	if ( result ) {
+		WLAN_LOG_ERROR("Failed to set CnfOwnSSID\n");
+		goto failed;
+	}
+	result = hfa384x_drvr_setconfig( hw, HFA384x_RID_CNFDESIREDSSID,
+					 bytebuf, HFA384x_RID_CNFDESIREDSSID_LEN);
 	if ( result ) {
-		WLAN_LOG_ERROR("Failed to set SSID, result=0x%04x\n", result);
+		WLAN_LOG_ERROR("Failed to set CnfDesiredSSID\n");
 		goto failed;
 	}
 
 	/* bsstype - we use the default in the ap firmware */
+	/* IBSS port */
+	hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFPORTTYPE, 0);
 
 	/* beacon period */
 	word = msg->beaconperiod.data;
-	result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFAPBCNINT, word);
+	result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFAPBCNint, word);
 	if ( result ) {
 		WLAN_LOG_ERROR("Failed to set beacon period=%d.\n", word);
 		goto failed;
@@ -1443,98 +650,20 @@ int prism2mgmt_start(wlandevice_t *wlandev, void *msgp)
 		WLAN_LOG_ERROR("Failed to set supprates=%d.\n", word);
 		goto failed;
 	}
-	result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_TXRATECNTL0, word);
-	if ( result ) {
-		WLAN_LOG_ERROR("Failed to set txrates=%d.\n", word);
-		goto failed;
-	}
-
-	/* ibssatimwindow */
-	if (msg->ibssatimwindow.status == P80211ENUM_msgitem_status_data_ok) {
-		WLAN_LOG_INFO("prism2mgmt_start: atimwindow not used in "
-			       "Infrastructure mode, ignored.\n");
-	}
-
-	/* DTIM period */
-	word = msg->dtimperiod.data;
-	result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFOWNDTIMPER, word);
-	if ( result ) {
-		WLAN_LOG_ERROR("Failed to set dtim period=%d.\n", word);
-		goto failed;
-	}
-
-	/* probedelay */
-	if (msg->probedelay.status == P80211ENUM_msgitem_status_data_ok) {
-		WLAN_LOG_INFO("prism2mgmt_start: probedelay not "
-			       "supported in prism2, ignored.\n");
-	}
-
-	/* cfpollable, cfpollreq, cfpperiod, cfpmaxduration */
-	if (msg->cfpollable.data == P80211ENUM_truth_true &&
-	    msg->cfpollreq.data == P80211ENUM_truth_true ) {
-		WLAN_LOG_ERROR("cfpollable=cfpollreq=true is illegal.\n");
-		result = -1;
-		goto failed;
-	}
-
-	/* read the PCFInfo and update */
-	result = hfa384x_drvr_getconfig(hw, HFA384x_RID_CNFAPPCFINFO,
-					pcfinfo, HFA384x_RID_CNFAPPCFINFO_LEN);
-	if ( result ) {
-		WLAN_LOG_INFO("prism2mgmt_start: read(pcfinfo) failed, "
-				"assume it's "
-				"not supported, pcf settings ignored.\n");
-		goto pcf_skip;
-	}
-	if ((msg->cfpollable.data == P80211ENUM_truth_false &&
-	     msg->cfpollreq.data == P80211ENUM_truth_false) ) {
-	    	pcfinfo->MediumOccupancyLimit = 0;
-		pcfinfo->CFPPeriod = 0;
-		pcfinfo->CFPMaxDuration = 0;
-		pcfinfo->CFPFlags &= host2hfa384x_16((UINT16)~BIT0);
-
-		if ( msg->cfpperiod.status == P80211ENUM_msgitem_status_data_ok ||
-		     msg->cfpmaxduration.status == P80211ENUM_msgitem_status_data_ok ) {
-			WLAN_LOG_WARNING(
-				"Setting cfpperiod or cfpmaxduration when "
-				"cfpollable and cfreq are false is pointless.\n");
-		}
-	}
-	if ((msg->cfpollable.data == P80211ENUM_truth_true ||
-	     msg->cfpollreq.data == P80211ENUM_truth_true) ) {
-		if ( msg->cfpollable.data == P80211ENUM_truth_true) {
-			pcfinfo->CFPFlags |= host2hfa384x_16((UINT16)BIT0);
-		}
-
-		if ( msg->cfpperiod.status == P80211ENUM_msgitem_status_data_ok) {
-			pcfinfo->CFPPeriod = msg->cfpperiod.data;
-			pcfinfo->CFPPeriod = host2hfa384x_16(pcfinfo->CFPPeriod);
-		}
 
-		if ( msg->cfpmaxduration.status == P80211ENUM_msgitem_status_data_ok) {
-			pcfinfo->CFPMaxDuration = msg->cfpmaxduration.data;
-			pcfinfo->CFPMaxDuration = host2hfa384x_16(pcfinfo->CFPMaxDuration);
-			pcfinfo->MediumOccupancyLimit = pcfinfo->CFPMaxDuration;
-		}
-	}
-	result = hfa384x_drvr_setconfig(hw, HFA384x_RID_CNFAPPCFINFO,
-					pcfinfo, HFA384x_RID_CNFAPPCFINFO_LEN);
+	result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_TXRATECNTL, word);
 	if ( result ) {
-		WLAN_LOG_ERROR("write(pcfinfo) failed.\n");
+		WLAN_LOG_ERROR("Failed to set txrates=%d.\n", word);
 		goto failed;
 	}
 
-pcf_skip:
 	/* Set the macmode so the frame setup code knows what to do */
-	if ( msg->bsstype.data == P80211ENUM_bsstype_infrastructure ) {
-		wlandev->macmode = WLAN_MACMODE_ESS_AP;
+	if ( msg->bsstype.data == P80211ENUM_bsstype_independent ) {
+		wlandev->macmode = WLAN_MACMODE_IBSS_STA;
 		/* lets extend the data length a bit */
 		hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFMAXDATALEN, 2304);
 	}
 
-	/* Set the BSSID to the same as our MAC */
-	memcpy( wlandev->bssid, wlandev->netdev->dev_addr, WLAN_BSSID_LEN);
-
 	/* Enable the Port */
 	result = hfa384x_drvr_enable(hw, 0);
 	if ( result ) {
@@ -1556,80 +685,6 @@ done:
 	return result;
 }
 
-
-/*----------------------------------------------------------------
-* prism2mgmt_enable
-*
-* Start a BSS.  Any station can do this for IBSS, only AP for ESS.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-*	interrupt
-----------------------------------------------------------------*/
-int prism2mgmt_enable(wlandevice_t *wlandev, void *msgp)
-{
-	int			result = 0;
-	hfa384x_t		*hw = wlandev->priv;
-	p80211msg_p2req_enable_t	*msg = msgp;
-	DBFENTER;
-
-	if (!hw->ap) {
-
-		/*** STATION ***/
-
-		/* Ad-Hoc not quite supported on Prism2 */
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-		goto done;
-	}
-
-	/*** ACCESS POINT ***/
-
-	msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-
-	/* Is the tertiary loaded? */
-	if ( hw->cap_sup_sta.id != 5) {
-		WLAN_LOG_ERROR("AP driver failed to detect AP firmware.\n");
-		goto failed;
-	}
-
-	/* Set the macmode so the frame setup code knows what to do */
-	wlandev->macmode = WLAN_MACMODE_ESS_AP;
-
-	/* Set the BSSID to the same as our MAC */
-	memcpy( wlandev->bssid, wlandev->netdev->dev_addr, WLAN_BSSID_LEN);
-
-	/* Enable the Port */
-	result = hfa384x_drvr_enable(hw, 0);
-	if ( result ) {
-		WLAN_LOG_ERROR("Enable macport failed, result=%d.\n", result);
-		goto failed;
-	}
-
-	msg->resultcode.data = P80211ENUM_resultcode_success;
-
-	goto done;
-failed:
-	msg->resultcode.data = P80211ENUM_resultcode_invalid_parameters;
-
-done:
-	result = 0;
-
-	DBFEXIT;
-	return result;
-}
-
-
 /*----------------------------------------------------------------
 * prism2mgmt_readpda
 *
@@ -1696,402 +751,6 @@ int prism2mgmt_readpda(wlandevice_t *wlandev, void *msgp)
 }
 
 /*----------------------------------------------------------------
-* prism2mgmt_readcis
-*
-* Collect the CIS data and put it in the message.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-----------------------------------------------------------------*/
-int prism2mgmt_readcis(wlandevice_t *wlandev, void *msgp)
-{
-	int			result;
-	hfa384x_t		*hw = wlandev->priv;
-	p80211msg_p2req_readcis_t	*msg = msgp;
-
-	DBFENTER;
-
-        memset(msg->cis.data, 0, sizeof(msg->cis.data));
-
-	result = hfa384x_drvr_getconfig(hw, HFA384x_RID_CIS,
-					msg->cis.data, HFA384x_RID_CIS_LEN);
-	if ( result ) {
-		WLAN_LOG_INFO("prism2mgmt_readcis: read(cis) failed.\n");
-		msg->cis.status = P80211ENUM_msgitem_status_no_value;
-		msg->resultcode.data = P80211ENUM_resultcode_implementation_failure;
-
-		}
-	else {
-		msg->cis.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_success;
-		}
-
-	msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-
-	DBFEXIT;
-	return 0;
-}
-
-/*----------------------------------------------------------------
-* prism2mgmt_auxport_state
-*
-* Enables/Disables the card's auxiliary port.  Should be called
-* before and after a sequence of auxport_read()/auxport_write()
-* calls.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-----------------------------------------------------------------*/
-int prism2mgmt_auxport_state(wlandevice_t *wlandev, void *msgp)
-{
-	p80211msg_p2req_auxport_state_t	*msg = msgp;
-
-#if (WLAN_HOSTIF != WLAN_USB)
-	hfa384x_t		*hw = wlandev->priv;
-	DBFENTER;
-
-	msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-	if (msg->enable.data == P80211ENUM_truth_true) {
-		if ( hfa384x_cmd_aux_enable(hw, 0) ) {
-			msg->resultcode.data = P80211ENUM_resultcode_implementation_failure;
-		} else {
-			msg->resultcode.data = P80211ENUM_resultcode_success;
-		}
-	} else {
-		hfa384x_cmd_aux_disable(hw);
-		msg->resultcode.data = P80211ENUM_resultcode_success;
-	}
-
-#else /* !USB */
-	DBFENTER;
-
-	msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-	msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-
-#endif /* WLAN_HOSTIF != WLAN_USB */
-
-	DBFEXIT;
-	return 0;
-}
-
-
-/*----------------------------------------------------------------
-* prism2mgmt_auxport_read
-*
-* Copies data from the card using the auxport.  The auxport must
-* have previously been enabled.  Note: this is not the way to
-* do downloads, see the [ram|flash]dl functions.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-----------------------------------------------------------------*/
-int prism2mgmt_auxport_read(wlandevice_t *wlandev, void *msgp)
-{
-#if (WLAN_HOSTIF != WLAN_USB)
-	hfa384x_t		*hw = wlandev->priv;
-	p80211msg_p2req_auxport_read_t	*msg = msgp;
-	UINT32			addr;
-	UINT32			len;
-	UINT8*			buf;
-	UINT32			maxlen = sizeof(msg->data.data);
-	DBFENTER;
-
-	if ( hw->auxen ) {
-		addr = msg->addr.data;
-		len = msg->len.data;
-		buf = msg->data.data;
-		if ( len <= maxlen ) {  /* max read/write size */
-			hfa384x_copy_from_aux(hw, addr, HFA384x_AUX_CTL_EXTDS, buf, len);
-			msg->resultcode.data = P80211ENUM_resultcode_success;
-		} else {
-			WLAN_LOG_DEBUG(1,"Attempt to read > maxlen from auxport.\n");
-			msg->resultcode.data = P80211ENUM_resultcode_refused;
-		}
-
-	} else {
-		msg->resultcode.data = P80211ENUM_resultcode_refused;
-	}
-	msg->data.status = P80211ENUM_msgitem_status_data_ok;
-	msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-
-	DBFEXIT;
-	return 0;
-#else
-	DBFENTER;
-
-	WLAN_LOG_ERROR("prism2mgmt_auxport_read: Not supported on USB.\n");
-
-	DBFEXIT;
-	return 0;
-#endif
-}
-
-
-/*----------------------------------------------------------------
-* prism2mgmt_auxport_write
-*
-* Copies data to the card using the auxport.  The auxport must
-* have previously been enabled.  Note: this is not the way to
-* do downloads, see the [ram|flash]dl functions.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-----------------------------------------------------------------*/
-int prism2mgmt_auxport_write(wlandevice_t *wlandev, void *msgp)
-{
-#if (WLAN_HOSTIF != WLAN_USB)
-	hfa384x_t		*hw = wlandev->priv;
-	p80211msg_p2req_auxport_write_t	*msg = msgp;
-	UINT32			addr;
-	UINT32			len;
-	UINT8*			buf;
-	UINT32			maxlen = sizeof(msg->data.data);
-	DBFENTER;
-
-	if ( hw->auxen ) {
-		addr = msg->addr.data;
-		len = msg->len.data;
-		buf = msg->data.data;
-		if ( len <= maxlen ) {  /* max read/write size */
-			hfa384x_copy_to_aux(hw, addr, HFA384x_AUX_CTL_EXTDS, buf, len);
-		} else {
-			WLAN_LOG_DEBUG(1,"Attempt to write > maxlen from auxport.\n");
-			msg->resultcode.data = P80211ENUM_resultcode_refused;
-		}
-
-	} else {
-		msg->resultcode.data = P80211ENUM_resultcode_refused;
-	}
-	msg->data.status = P80211ENUM_msgitem_status_data_ok;
-	msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-
-	DBFEXIT;
-	return 0;
-#else
-	DBFENTER;
-	WLAN_LOG_ERROR("prism2mgmt_auxport_read: Not supported on USB.\n");
-	DBFEXIT;
-	return 0;
-#endif
-}
-
-/*----------------------------------------------------------------
-* prism2mgmt_low_level
-*
-* Puts the card into the desired test mode.
-*
-* Arguments:
-*       wlandev         wlan device structure
-*       msgp            ptr to msg buffer
-*
-* Returns:
-*       0       success and done
-*       <0      success, but we're waiting for something to finish.
-*       >0      an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*       process thread  (usually)
-----------------------------------------------------------------*/
-int prism2mgmt_low_level(wlandevice_t *wlandev, void *msgp)
-{
-	hfa384x_t		*hw = wlandev->priv;
-        p80211msg_p2req_low_level_t     *msg = msgp;
-	hfa384x_metacmd_t cmd;
-        DBFENTER;
-
-        msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-
-        /* call some routine to execute the test command */
-	cmd.cmd = (UINT16) msg->command.data;
-	cmd.parm0 = (UINT16) msg->param0.data;
-	cmd.parm1 = (UINT16) msg->param1.data;
-	cmd.parm2 = (UINT16) msg->param2.data;
-
-        hfa384x_drvr_low_level(hw,&cmd);
-
-        msg->resp0.data = (UINT32) cmd.result.resp0;
-        msg->resp1.data = (UINT32) cmd.result.resp1;
-        msg->resp2.data = (UINT32) cmd.result.resp2;
-
-        msg->resultcode.data = P80211ENUM_resultcode_success;
-
-        DBFEXIT;
-        return 0;
-}
-
-/*----------------------------------------------------------------
-* prism2mgmt_test_command
-*
-* Puts the card into the desired test mode.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-----------------------------------------------------------------*/
-int prism2mgmt_test_command(wlandevice_t *wlandev, void *msgp)
-{
-	hfa384x_t		*hw = wlandev->priv;
-	p80211msg_p2req_test_command_t	*msg = msgp;
-	hfa384x_metacmd_t cmd;
-
-        DBFENTER;
-
-	cmd.cmd = ((UINT16) msg->testcode.data) << 8 | 0x38;
-	cmd.parm0 = (UINT16) msg->testparam.data;
-	cmd.parm1 = 0;
-	cmd.parm2 = 0;
-
-        /* call some routine to execute the test command */
-
-        hfa384x_drvr_low_level(hw,&cmd);
-
-        msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-        msg->resultcode.data = P80211ENUM_resultcode_success;
-
-        msg->status.status = P80211ENUM_msgitem_status_data_ok;
-        msg->status.data = cmd.result.status;
-        msg->resp0.status = P80211ENUM_msgitem_status_data_ok;
-        msg->resp0.data = cmd.result.resp0;
-        msg->resp1.status = P80211ENUM_msgitem_status_data_ok;
-        msg->resp1.data = cmd.result.resp1;
-        msg->resp2.status = P80211ENUM_msgitem_status_data_ok;
-        msg->resp2.data = cmd.result.resp2;
-
-	DBFEXIT;
-	return 0;
-}
-
-
-/*----------------------------------------------------------------
-* prism2mgmt_mmi_read
-*
-* Read from one of the MMI registers.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-----------------------------------------------------------------*/
-int prism2mgmt_mmi_read(wlandevice_t *wlandev, void *msgp)
-{
-	hfa384x_t		*hw = wlandev->priv;
-	p80211msg_p2req_mmi_read_t	*msg = msgp;
-	UINT32 resp = 0;
-
-	DBFENTER;
-
-	msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-
-	/* call some routine to execute the test command */
-
-	hfa384x_drvr_mmi_read(hw, msg->addr.data, &resp);
-
-	/* I'm not sure if this is "architecturally" correct, but it
-           is expedient. */
-
-	msg->value.status = P80211ENUM_msgitem_status_data_ok;
-	msg->value.data = resp;
-	msg->resultcode.data = P80211ENUM_resultcode_success;
-
-	DBFEXIT;
-	return 0;
-}
-
-/*----------------------------------------------------------------
-* prism2mgmt_mmi_write
-*
-* Write a data value to one of the MMI registers.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-----------------------------------------------------------------*/
-int prism2mgmt_mmi_write(wlandevice_t *wlandev, void *msgp)
-{
-	hfa384x_t		*hw = wlandev->priv;
-	p80211msg_p2req_mmi_write_t	*msg = msgp;
-	DBFENTER;
-
-	msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-
-	/* call some routine to execute the test command */
-
-	hfa384x_drvr_mmi_write(hw, msg->addr.data, msg->data.data);
-
-	msg->resultcode.data = P80211ENUM_resultcode_success;
-
-	DBFEXIT;
-	return 0;
-}
-
-/*----------------------------------------------------------------
 * prism2mgmt_ramdl_state
 *
 * Establishes the beginning/end of a card RAM download session.
@@ -2179,9 +838,9 @@ int prism2mgmt_ramdl_write(wlandevice_t *wlandev, void *msgp)
 {
 	hfa384x_t		*hw = wlandev->priv;
 	p80211msg_p2req_ramdl_write_t	*msg = msgp;
-	UINT32			addr;
-	UINT32			len;
-	UINT8			*buf;
+	u32			addr;
+	u32			len;
+	u8			*buf;
 	DBFENTER;
 
 	if (wlandev->msdstate != WLAN_MSD_FWLOAD) {
@@ -2319,9 +978,9 @@ int prism2mgmt_flashdl_write(wlandevice_t *wlandev, void *msgp)
 {
 	hfa384x_t		*hw = wlandev->priv;
 	p80211msg_p2req_flashdl_write_t	*msg = msgp;
-	UINT32			addr;
-	UINT32			len;
-	UINT8			*buf;
+	u32			addr;
+	u32			len;
+	u8			*buf;
 	DBFENTER;
 
 	if (wlandev->msdstate != WLAN_MSD_FWLOAD) {
@@ -2361,247 +1020,6 @@ int prism2mgmt_flashdl_write(wlandevice_t *wlandev, void *msgp)
 	return 0;
 }
 
-
-/*----------------------------------------------------------------
-* prism2mgmt_dump_state
-*
-* Dumps the driver's and hardware's current state via the kernel
-* log at KERN_NOTICE level.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-----------------------------------------------------------------*/
-int prism2mgmt_dump_state(wlandevice_t *wlandev, void *msgp)
-{
-	p80211msg_p2req_dump_state_t	*msg = msgp;
-	int				result = 0;
-
-#if (WLAN_HOSTIF != WLAN_USB)
-	hfa384x_t		*hw = wlandev->priv;
-	UINT16				auxbuf[15];
-	DBFENTER;
-
-	WLAN_LOG_NOTICE("prism2 driver and hardware state:\n");
-	if  ( (result = hfa384x_cmd_aux_enable(hw, 0)) ) {
-		WLAN_LOG_ERROR("aux_enable failed, result=%d\n", result);
-		goto failed;
-	}
-	hfa384x_copy_from_aux(hw,
-		0x01e2,
-		HFA384x_AUX_CTL_EXTDS,
-		auxbuf,
-		sizeof(auxbuf));
-	hfa384x_cmd_aux_disable(hw);
-	WLAN_LOG_NOTICE("  cmac: FreeBlocks=%d\n", auxbuf[5]);
-	WLAN_LOG_NOTICE("  cmac: IntEn=0x%02x EvStat=0x%02x\n",
-		hfa384x_getreg(hw, HFA384x_INTEN),
-		hfa384x_getreg(hw, HFA384x_EVSTAT));
-
-	#ifdef USE_FID_STACK
-	WLAN_LOG_NOTICE("  drvr: txfid_top=%d stacksize=%d\n",
-		hw->txfid_top,HFA384x_DRVR_FIDSTACKLEN_MAX);
-	#else
-	WLAN_LOG_NOTICE("  drvr: txfid_head=%d txfid_tail=%d txfid_N=%d\n",
-		hw->txfid_head, hw->txfid_tail, hw->txfid_N);
-	#endif
-
-	msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-	msg->resultcode.data = P80211ENUM_resultcode_success;
-
-#else /* (WLAN_HOSTIF == WLAN_USB) */
-
-	DBFENTER;
-
-	msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-	msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-	goto failed;
-
-#endif /* (WLAN_HOSTIF != WLAN_USB) */
-
-failed:
-	DBFEXIT;
-	return result;
-}
-
-/*----------------------------------------------------------------
-* prism2mgmt_channel_info
-*
-* Issues a ChannelInfoRequest.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-----------------------------------------------------------------*/
-int prism2mgmt_channel_info(wlandevice_t *wlandev, void *msgp)
-{
-	p80211msg_p2req_channel_info_t	*msg=msgp;
-	hfa384x_t			*hw = wlandev->priv;
-	int				result, i, n=0;
-	UINT16				channel_mask=0;
-	hfa384x_ChannelInfoRequest_data_t	chinforeq;
-	// unsigned long 			now;
-
-	DBFENTER;
-
-	if (!hw->ap) {
-
-		/*** STATION ***/
-
-		/* Not supported in STA f/w */
-		P80211_SET_INT(msg->resultcode, P80211ENUM_resultcode_not_supported);
-		goto done;
-	}
-
-	/*** ACCESS POINT ***/
-
-#define CHINFO_TIMEOUT 2
-
-	P80211_SET_INT(msg->resultcode, P80211ENUM_resultcode_success);
-
-	/* setting default value for channellist = all channels */
-	if (!msg->channellist.data) {
-		P80211_SET_INT(msg->channellist, 0x00007FFE);
-	}
-	/* setting default value for channeldwelltime = 100 ms */
-	if (!msg->channeldwelltime.data) {
-		P80211_SET_INT(msg->channeldwelltime, 100);
-	}
-	channel_mask = (UINT16) (msg->channellist.data >> 1);
-	for (i=0, n=0; i < 14; i++) {
-		if (channel_mask & (1<<i)) {
-			n++;
-		}
-	}
-	P80211_SET_INT(msg->numchinfo, n);
-	chinforeq.channelList = host2hfa384x_16(channel_mask);
-	chinforeq.channelDwellTime = host2hfa384x_16(msg->channeldwelltime.data);
-
-	atomic_set(&hw->channel_info.done, 1);
-
-	result = hfa384x_drvr_setconfig( hw, HFA384x_RID_CHANNELINFOREQUEST,
-					 &chinforeq, HFA384x_RID_CHANNELINFOREQUEST_LEN);
-	if ( result ) {
-		WLAN_LOG_ERROR("setconfig(CHANNELINFOREQUEST) failed. result=%d\n",
-				result);
-		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-		goto done;
-	}
-	/*
-	now = jiffies;
-	while (atomic_read(&hw->channel_info.done) != 1) {
-		if ((jiffies - now) > CHINFO_TIMEOUT*HZ) {
-			WLAN_LOG_NOTICE("ChannelInfo results not received in %d seconds, aborting.\n",
-					CHINFO_TIMEOUT);
-			msg->resultcode.data = P80211ENUM_resultcode_timeout;
-			goto done;
-		}
-		current->state = TASK_INTERRUPTIBLE;
-		schedule_timeout(HZ/4);
-		current->state = TASK_RUNNING;
-	}
-	*/
-
-done:
-
-	DBFEXIT;
-	return 0;
-}
-
-/*----------------------------------------------------------------
-* prism2mgmt_channel_info_results
-*
-* Returns required ChannelInfo result.
-*
-* Arguments:
-*	wlandev		wlan device structure
-*	msgp		ptr to msg buffer
-*
-* Returns:
-*	0	success and done
-*	<0	success, but we're waiting for something to finish.
-*	>0	an error occurred while handling the message.
-* Side effects:
-*
-* Call context:
-*	process thread  (usually)
-----------------------------------------------------------------*/
-int prism2mgmt_channel_info_results(wlandevice_t *wlandev, void *msgp)
-{
-	hfa384x_t			*hw = wlandev->priv;
-
-	p80211msg_p2req_channel_info_results_t	*msg=msgp;
-	int				result=0;
-	int		channel;
-
-	DBFENTER;
-
-	if (!hw->ap) {
-
-		/*** STATION ***/
-
-		/* Not supported in STA f/w */
-		P80211_SET_INT(msg->resultcode, P80211ENUM_resultcode_not_supported);
-		goto done;
-	}
-
-	/*** ACCESS POINT ***/
-
-	switch (atomic_read(&hw->channel_info.done)) {
-	case 0: msg->resultcode.status = P80211ENUM_msgitem_status_no_value;
-		goto done;
-	case 1: msg->resultcode.status = P80211ENUM_msgitem_status_incomplete_itemdata;
-		goto done;
-	}
-
-	P80211_SET_INT(msg->resultcode, P80211ENUM_resultcode_success);
-	channel=msg->channel.data-1;
-
-	if (channel < 0 || ! (hw->channel_info.results.scanchannels & 1<<channel) ) {
-		msg->resultcode.data = P80211ENUM_resultcode_invalid_parameters;
-		goto done;
-	}
-	WLAN_LOG_DEBUG(2, "chinfo_results: channel %d, avg/peak level=%d/%d dB, active=%d\n",
-			channel+1,
-			hw->channel_info.results.result[channel].anl,
-			hw->channel_info.results.result[channel].pnl,
-			hw->channel_info.results.result[channel].active
-		);
-	P80211_SET_INT(msg->avgnoiselevel, hw->channel_info.results.result[channel].anl);
-	P80211_SET_INT(msg->peaknoiselevel, hw->channel_info.results.result[channel].pnl);
-	P80211_SET_INT(msg->bssactive, hw->channel_info.results.result[channel].active &
-		HFA384x_CHINFORESULT_BSSACTIVE
-                ? P80211ENUM_truth_true
-                : P80211ENUM_truth_false) ;
-	P80211_SET_INT(msg->pcfactive, hw->channel_info.results.result[channel].active &
-		HFA384x_CHINFORESULT_PCFACTIVE
-                ? P80211ENUM_truth_true
-                : P80211ENUM_truth_false) ;
-
-done:
-	DBFEXIT;
-	return result;
-}
-
-
 /*----------------------------------------------------------------
 * prism2mgmt_autojoin
 *
@@ -2625,11 +1043,11 @@ int prism2mgmt_autojoin(wlandevice_t *wlandev, void *msgp)
 {
 	hfa384x_t			*hw = wlandev->priv;
 	int 			result = 0;
-	UINT16			reg;
-	UINT16			port_type;
+	u16			reg;
+	u16			port_type;
 	p80211msg_lnxreq_autojoin_t	*msg = msgp;
 	p80211pstrd_t		*pstr;
-	UINT8			bytebuf[256];
+	u8			bytebuf[256];
 	hfa384x_bytestr_t	*p2bytestr = (hfa384x_bytestr_t*)bytebuf;
 	DBFENTER;
 
@@ -2638,16 +1056,6 @@ int prism2mgmt_autojoin(wlandevice_t *wlandev, void *msgp)
 	/* Set the SSID */
 	memcpy(&wlandev->ssid, &msg->ssid.data, sizeof(msg->ssid.data));
 
-	if (hw->ap) {
-
-		/*** ACCESS POINT ***/
-
-		/* Never supported on AP */
-		msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
-		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-		goto done;
-	}
-
 	/* Disable the Port */
 	hfa384x_drvr_disable(hw, 0);
 
@@ -2699,7 +1107,6 @@ int prism2mgmt_autojoin(wlandevice_t *wlandev, void *msgp)
 	msg->resultcode.status = P80211ENUM_msgitem_status_data_ok;
 	msg->resultcode.data = P80211ENUM_resultcode_success;
 
-done:
 	DBFEXIT;
 	return result;
 }
@@ -2730,7 +1137,7 @@ int prism2mgmt_wlansniff(wlandevice_t *wlandev, void *msgp)
 	p80211msg_lnxreq_wlansniff_t	*msg = msgp;
 
 	hfa384x_t			*hw = wlandev->priv;
-	UINT16			word;
+	u16			word;
 
 	DBFENTER;
 
diff --git a/drivers/staging/wlan-ng/prism2mgmt.h b/drivers/staging/wlan-ng/prism2mgmt.h
index 733fd999c92..caf808d5796 100644
--- a/drivers/staging/wlan-ng/prism2mgmt.h
+++ b/drivers/staging/wlan-ng/prism2mgmt.h
@@ -73,10 +73,6 @@
 /*=============================================================*/
 /*------ Static variable externs ------------------------------*/
 
-#if (WLAN_HOSTIF != WLAN_USB)
-extern int      prism2_bap_timeout;
-extern int	prism2_irq_evread_max;
-#endif
 extern int	prism2_debug;
 extern int      prism2_reset_holdtime;
 extern int      prism2_reset_settletime;
@@ -84,8 +80,8 @@ extern int      prism2_reset_settletime;
 /*--- Function Declarations -----------------------------------*/
 /*=============================================================*/
 
-UINT32
-prism2sta_ifstate(wlandevice_t *wlandev, UINT32 ifstate);
+u32
+prism2sta_ifstate(wlandevice_t *wlandev, u32 ifstate);
 
 void
 prism2sta_ev_dtim(wlandevice_t *wlandev);
@@ -94,47 +90,24 @@ prism2sta_ev_infdrop(wlandevice_t *wlandev);
 void
 prism2sta_ev_info(wlandevice_t *wlandev, hfa384x_InfFrame_t *inf);
 void
-prism2sta_ev_txexc(wlandevice_t *wlandev, UINT16 status);
+prism2sta_ev_txexc(wlandevice_t *wlandev, u16 status);
 void
-prism2sta_ev_tx(wlandevice_t *wlandev, UINT16 status);
+prism2sta_ev_tx(wlandevice_t *wlandev, u16 status);
 void
 prism2sta_ev_rx(wlandevice_t *wlandev, struct sk_buff *skb);
 void
 prism2sta_ev_alloc(wlandevice_t *wlandev);
 
-
 int prism2mgmt_mibset_mibget(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_powermgmt(wlandevice_t *wlandev, void *msgp);
 int prism2mgmt_scan(wlandevice_t *wlandev, void *msgp);
 int prism2mgmt_scan_results(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_join(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_p2_join(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_authenticate(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_deauthenticate(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_associate(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_reassociate(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_disassociate(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_reset(wlandevice_t *wlandev, void *msgp);
 int prism2mgmt_start(wlandevice_t *wlandev, void *msgp);
 int prism2mgmt_wlansniff(wlandevice_t *wlandev, void *msgp);
 int prism2mgmt_readpda(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_readcis(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_auxport_state(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_auxport_read(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_auxport_write(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_low_level(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_test_command(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_mmi_read(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_mmi_write(wlandevice_t *wlandev, void *msgp);
 int prism2mgmt_ramdl_state(wlandevice_t *wlandev, void *msgp);
 int prism2mgmt_ramdl_write(wlandevice_t *wlandev, void *msgp);
 int prism2mgmt_flashdl_state(wlandevice_t *wlandev, void *msgp);
 int prism2mgmt_flashdl_write(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_mm_state(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_dump_state(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_enable(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_channel_info(wlandevice_t *wlandev, void *msgp);
-int prism2mgmt_channel_info_results(wlandevice_t *wlandev, void *msgp);
 int prism2mgmt_autojoin(wlandevice_t *wlandev, void *msgp);
 
 /*---------------------------------------------------------------
@@ -142,31 +115,31 @@ int prism2mgmt_autojoin(wlandevice_t *wlandev, void *msgp);
 * Prism2 data types
 ---------------------------------------------------------------*/
 /* byte area conversion functions*/
-void prism2mgmt_pstr2bytearea(UINT8 *bytearea, p80211pstrd_t *pstr);
-void prism2mgmt_bytearea2pstr(UINT8 *bytearea, p80211pstrd_t *pstr, int len);
+void prism2mgmt_pstr2bytearea(u8 *bytearea, p80211pstrd_t *pstr);
+void prism2mgmt_bytearea2pstr(u8 *bytearea, p80211pstrd_t *pstr, int len);
 
 /* byte string conversion functions*/
 void prism2mgmt_pstr2bytestr(hfa384x_bytestr_t *bytestr, p80211pstrd_t *pstr);
 void prism2mgmt_bytestr2pstr(hfa384x_bytestr_t *bytestr, p80211pstrd_t *pstr);
 
 /* integer conversion functions */
-void prism2mgmt_prism2int2p80211int(UINT16 *prism2int, UINT32 *wlanint);
-void prism2mgmt_p80211int2prism2int(UINT16 *prism2int, UINT32 *wlanint);
+void prism2mgmt_prism2int2p80211int(u16 *prism2int, u32 *wlanint);
+void prism2mgmt_p80211int2prism2int(u16 *prism2int, u32 *wlanint);
 
 /* enumerated integer conversion functions */
-void prism2mgmt_prism2enum2p80211enum(UINT16 *prism2enum, UINT32 *wlanenum, UINT16 rid);
-void prism2mgmt_p80211enum2prism2enum(UINT16 *prism2enum, UINT32 *wlanenum, UINT16 rid);
+void prism2mgmt_prism2enum2p80211enum(u16 *prism2enum, u32 *wlanenum, u16 rid);
+void prism2mgmt_p80211enum2prism2enum(u16 *prism2enum, u32 *wlanenum, u16 rid);
 
 /* functions to convert a bit area to/from an Operational Rate Set */
-void prism2mgmt_get_oprateset(UINT16 *rate, p80211pstrd_t *pstr);
-void prism2mgmt_set_oprateset(UINT16 *rate, p80211pstrd_t *pstr);
+void prism2mgmt_get_oprateset(u16 *rate, p80211pstrd_t *pstr);
+void prism2mgmt_set_oprateset(u16 *rate, p80211pstrd_t *pstr);
 
 /* functions to convert Group Addresses */
-void prism2mgmt_get_grpaddr(UINT32 did,
+void prism2mgmt_get_grpaddr(u32 did,
 	p80211pstrd_t *pstr, hfa384x_t *priv );
-int prism2mgmt_set_grpaddr(UINT32 did,
-	UINT8 *prism2buf, p80211pstrd_t *pstr, hfa384x_t *priv );
-int prism2mgmt_get_grpaddr_index( UINT32 did );
+int prism2mgmt_set_grpaddr(u32 did,
+	u8 *prism2buf, p80211pstrd_t *pstr, hfa384x_t *priv );
+int prism2mgmt_get_grpaddr_index( u32 did );
 
 void prism2sta_processing_defer(struct work_struct *data);
 
diff --git a/drivers/staging/wlan-ng/prism2mib.c b/drivers/staging/wlan-ng/prism2mib.c
index eac06f793d8..539c4479d38 100644
--- a/drivers/staging/wlan-ng/prism2mib.c
+++ b/drivers/staging/wlan-ng/prism2mib.c
@@ -54,9 +54,6 @@
 /* System Includes */
 #define WLAN_DBVAR	prism2_debug
 
-#include "version.h"
-
-
 #include <linux/version.h>
 
 #include <linux/module.h>
@@ -69,26 +66,7 @@
 #include <asm/io.h>
 #include <linux/delay.h>
 #include <asm/byteorder.h>
-
-#include "wlan_compat.h"
-
-//#if (WLAN_HOSTIF == WLAN_PCMCIA)
-//#include <pcmcia/version.h>
-//#include <pcmcia/cs_types.h>
-//#include <pcmcia/cs.h>
-//#include <pcmcia/cistpl.h>
-//#include <pcmcia/ds.h>
-//#include <pcmcia/cisreg.h>
-//#endif
-//
-//#if ((WLAN_HOSTIF == WLAN_PLX) || (WLAN_HOSTIF == WLAN_PCI))
-//#include <linux/ioport.h>
-//#include <linux/pci.h>
-//endif
-
-//#if (WLAN_HOSTIF == WLAN_USB)
 #include <linux/usb.h>
-//#endif
 
 /*================================================================*/
 /* Project Includes */
@@ -112,18 +90,17 @@
 /*================================================================*/
 /* Local Types */
 
-#define  F_AP         0x1        /* MIB is supported on Access Points. */
-#define  F_STA        0x2        /* MIB is supported on stations. */
-#define  F_READ       0x4        /* MIB may be read. */
-#define  F_WRITE      0x8        /* MIB may be written. */
+#define  F_STA        0x1        /* MIB is supported on stations. */
+#define  F_READ       0x2        /* MIB may be read. */
+#define  F_WRITE      0x4        /* MIB may be written. */
 
 typedef struct mibrec
 {
-    UINT32   did;
-    UINT16   flag;
-    UINT16   parm1;
-    UINT16   parm2;
-    UINT16   parm3;
+    u32   did;
+    u16   flag;
+    u16   parm1;
+    u16   parm2;
+    u16   parm3;
     int      (*func)(struct mibrec                *mib,
                      int                          isget,
                      wlandevice_t                 *wlandev,
@@ -135,14 +112,6 @@ typedef struct mibrec
 /*================================================================*/
 /* Local Function Declarations */
 
-static int prism2mib_bytestr2pstr(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data);
-
 static int prism2mib_bytearea2pstr(
 mibrec_t                     *mib,
 int                          isget,
@@ -159,38 +128,6 @@ hfa384x_t                    *hw,
 p80211msg_dot11req_mibset_t  *msg,
 void                         *data);
 
-static int prism2mib_uint32array(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data);
-
-static int prism2mib_uint32offset(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data);
-
-static int prism2mib_truth(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data);
-
-static int prism2mib_preamble(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data);
-
 static int prism2mib_flag(
 mibrec_t                     *mib,
 int                          isget,
@@ -199,22 +136,6 @@ hfa384x_t                    *hw,
 p80211msg_dot11req_mibset_t  *msg,
 void                         *data);
 
-static int prism2mib_appcfinfoflag(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data);
-
-static int prism2mib_regulatorydomains(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data);
-
 static int prism2mib_wepdefaultkey(
 mibrec_t                     *mib,
 int                          isget,
@@ -223,14 +144,6 @@ hfa384x_t                    *hw,
 p80211msg_dot11req_mibset_t  *msg,
 void                         *data);
 
-static int prism2mib_powermanagement(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data);
-
 static int prism2mib_privacyinvoked(
 mibrec_t                     *mib,
 int                          isget,
@@ -255,46 +168,6 @@ hfa384x_t                    *hw,
 p80211msg_dot11req_mibset_t  *msg,
 void                         *data);
 
-static int prism2mib_operationalrateset(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data);
-
-static int prism2mib_groupaddress(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data);
-
-static int prism2mib_fwid(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data);
-
-static int prism2mib_authalg(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data);
-
-static int prism2mib_authalgenable(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data);
-
 static int prism2mib_priv(
 mibrec_t                     *mib,
 int                          isget,
@@ -303,980 +176,92 @@ hfa384x_t                    *hw,
 p80211msg_dot11req_mibset_t  *msg,
 void                         *data);
 
-static void prism2mib_priv_authlist(
-hfa384x_t      *hw,
-prism2sta_authlist_t  *list);
-
-static void prism2mib_priv_accessmode(
-hfa384x_t         *hw,
-UINT32            mode);
-
-static void prism2mib_priv_accessallow(
-hfa384x_t         *hw,
-p80211macarray_t  *macarray);
-
-static void prism2mib_priv_accessdeny(
-hfa384x_t         *hw,
-p80211macarray_t  *macarray);
-
-static void prism2mib_priv_deauthenticate(
-hfa384x_t         *hw,
-UINT8             *addr);
-
 /*================================================================*/
 /* Local Static Definitions */
 
 static mibrec_t mibtab[] = {
 
     /* dot11smt MIB's */
-
-    { DIDmib_dot11smt_dot11StationConfigTable_dot11StationID,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFOWNMACADDR, HFA384x_RID_CNFOWNMACADDR_LEN, 0,
-          prism2mib_bytearea2pstr },
-    { DIDmib_dot11smt_dot11StationConfigTable_dot11MediumOccupancyLimit,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFAPPCFINFO, HFA384x_RID_CNFAPPCFINFO_LEN, 0,
-          prism2mib_uint32offset },
-    { DIDmib_dot11smt_dot11StationConfigTable_dot11CFPollable,
-          F_STA | F_READ,
-          HFA384x_RID_CFPOLLABLE, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_dot11smt_dot11StationConfigTable_dot11CFPPeriod,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFAPPCFINFO, HFA384x_RID_CNFAPPCFINFO_LEN, 1,
-          prism2mib_uint32offset },
-    { DIDmib_dot11smt_dot11StationConfigTable_dot11CFPMaxDuration,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFAPPCFINFO, HFA384x_RID_CNFAPPCFINFO_LEN, 2,
-          prism2mib_uint32offset },
-    { DIDmib_dot11smt_dot11StationConfigTable_dot11AuthenticationResponseTimeOut,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFAUTHRSPTIMEOUT, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_dot11smt_dot11StationConfigTable_dot11PrivacyOptionImplemented,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_PRIVACYOPTIMP, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_dot11smt_dot11StationConfigTable_dot11PowerManagementMode,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFPMENABLED, 0, 0,
-          prism2mib_powermanagement },
-    { DIDmib_dot11smt_dot11StationConfigTable_dot11DesiredSSID,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFDESIREDSSID, HFA384x_RID_CNFDESIREDSSID_LEN, 0,
-          prism2mib_bytestr2pstr },
-    { DIDmib_dot11smt_dot11StationConfigTable_dot11DesiredBSSType,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_priv },
-    { DIDmib_dot11smt_dot11StationConfigTable_dot11OperationalRateSet,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_TXRATECNTL, 0, 0,
-          prism2mib_operationalrateset },
-    { DIDmib_dot11smt_dot11StationConfigTable_dot11OperationalRateSet,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_TXRATECNTL0, 0, 0,
-          prism2mib_operationalrateset },
-    { DIDmib_dot11smt_dot11StationConfigTable_dot11BeaconPeriod,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFAPBCNINT, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_dot11smt_dot11StationConfigTable_dot11DTIMPeriod,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFOWNDTIMPER, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_dot11smt_dot11StationConfigTable_dot11AssociationResponseTimeOut,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_PROTOCOLRSPTIME, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithm1,
-          F_AP | F_STA | F_READ,
-          1, 0, 0,
-          prism2mib_authalg },
-    { DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithm2,
-          F_AP | F_STA | F_READ,
-          2, 0, 0,
-          prism2mib_authalg },
-    { DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithm3,
-          F_AP | F_STA | F_READ,
-          3, 0, 0,
-          prism2mib_authalg },
-    { DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithm4,
-          F_AP | F_STA | F_READ,
-          4, 0, 0,
-          prism2mib_authalg },
-    { DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithm5,
-          F_AP | F_STA | F_READ,
-          5, 0, 0,
-          prism2mib_authalg },
-    { DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithm6,
-          F_AP | F_STA | F_READ,
-          6, 0, 0,
-          prism2mib_authalg },
-    { DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithmsEnable1,
-          F_AP | F_STA | F_READ | F_WRITE,
-          1, 0, 0,
-          prism2mib_authalgenable },
-    { DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithmsEnable2,
-          F_AP | F_STA | F_READ | F_WRITE,
-          2, 0, 0,
-          prism2mib_authalgenable },
-    { DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithmsEnable3,
-          F_AP | F_STA | F_READ | F_WRITE,
-          3, 0, 0,
-          prism2mib_authalgenable },
-    { DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithmsEnable4,
-          F_AP | F_STA | F_READ | F_WRITE,
-          4, 0, 0,
-          prism2mib_authalgenable },
-    { DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithmsEnable5,
-          F_AP | F_STA | F_READ | F_WRITE,
-          5, 0, 0,
-          prism2mib_authalgenable },
-    { DIDmib_dot11smt_dot11AuthenticationAlgorithmsTable_dot11AuthenticationAlgorithmsEnable6,
-          F_AP | F_STA | F_READ | F_WRITE,
-          6, 0, 0,
-          prism2mib_authalgenable },
     { DIDmib_dot11smt_dot11WEPDefaultKeysTable_dot11WEPDefaultKey0,
-          F_AP | F_STA | F_WRITE,
+          F_STA | F_WRITE,
           HFA384x_RID_CNFWEPDEFAULTKEY0, 0, 0,
           prism2mib_wepdefaultkey },
     { DIDmib_dot11smt_dot11WEPDefaultKeysTable_dot11WEPDefaultKey1,
-          F_AP | F_STA | F_WRITE,
+          F_STA | F_WRITE,
           HFA384x_RID_CNFWEPDEFAULTKEY1, 0, 0,
           prism2mib_wepdefaultkey },
     { DIDmib_dot11smt_dot11WEPDefaultKeysTable_dot11WEPDefaultKey2,
-          F_AP | F_STA | F_WRITE,
+          F_STA | F_WRITE,
           HFA384x_RID_CNFWEPDEFAULTKEY2, 0, 0,
           prism2mib_wepdefaultkey },
     { DIDmib_dot11smt_dot11WEPDefaultKeysTable_dot11WEPDefaultKey3,
-          F_AP | F_STA | F_WRITE,
+          F_STA | F_WRITE,
           HFA384x_RID_CNFWEPDEFAULTKEY3, 0, 0,
           prism2mib_wepdefaultkey },
     { DIDmib_dot11smt_dot11PrivacyTable_dot11PrivacyInvoked,
-          F_AP | F_STA | F_READ | F_WRITE,
+          F_STA | F_READ | F_WRITE,
           HFA384x_RID_CNFWEPFLAGS, HFA384x_WEPFLAGS_PRIVINVOKED, 0,
           prism2mib_privacyinvoked },
     { DIDmib_dot11smt_dot11PrivacyTable_dot11WEPDefaultKeyID,
-          F_AP | F_STA | F_READ | F_WRITE,
+          F_STA | F_READ | F_WRITE,
           HFA384x_RID_CNFWEPDEFAULTKEYID, 0, 0,
           prism2mib_uint32 },
     { DIDmib_dot11smt_dot11PrivacyTable_dot11ExcludeUnencrypted,
-          F_AP | F_STA | F_READ | F_WRITE,
+          F_STA | F_READ | F_WRITE,
           HFA384x_RID_CNFWEPFLAGS, HFA384x_WEPFLAGS_EXCLUDE, 0,
           prism2mib_excludeunencrypted },
-    { DIDmib_dot11phy_dot11PhyOperationTable_dot11ShortPreambleEnabled,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFSHORTPREAMBLE, 0, 0,
-          prism2mib_preamble },
 
     /* dot11mac MIB's */
 
     { DIDmib_dot11mac_dot11OperationTable_dot11MACAddress,
-          F_AP | F_STA | F_READ | F_WRITE,
+          F_STA | F_READ | F_WRITE,
           HFA384x_RID_CNFOWNMACADDR, HFA384x_RID_CNFOWNMACADDR_LEN, 0,
           prism2mib_bytearea2pstr },
     { DIDmib_dot11mac_dot11OperationTable_dot11RTSThreshold,
           F_STA | F_READ | F_WRITE,
           HFA384x_RID_RTSTHRESH, 0, 0,
           prism2mib_uint32 },
-    { DIDmib_dot11mac_dot11OperationTable_dot11RTSThreshold,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_RTSTHRESH0, 0, 0,
-          prism2mib_uint32 },
     { DIDmib_dot11mac_dot11OperationTable_dot11ShortRetryLimit,
-          F_AP | F_STA | F_READ,
+          F_STA | F_READ,
           HFA384x_RID_SHORTRETRYLIMIT, 0, 0,
           prism2mib_uint32 },
     { DIDmib_dot11mac_dot11OperationTable_dot11LongRetryLimit,
-          F_AP | F_STA | F_READ,
+          F_STA | F_READ,
           HFA384x_RID_LONGRETRYLIMIT, 0, 0,
           prism2mib_uint32 },
     { DIDmib_dot11mac_dot11OperationTable_dot11FragmentationThreshold,
           F_STA | F_READ | F_WRITE,
           HFA384x_RID_FRAGTHRESH, 0, 0,
           prism2mib_fragmentationthreshold },
-    { DIDmib_dot11mac_dot11OperationTable_dot11FragmentationThreshold,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_FRAGTHRESH0, 0, 0,
-          prism2mib_fragmentationthreshold },
     { DIDmib_dot11mac_dot11OperationTable_dot11MaxTransmitMSDULifetime,
-          F_AP | F_STA | F_READ,
+          F_STA | F_READ,
           HFA384x_RID_MAXTXLIFETIME, 0, 0,
           prism2mib_uint32 },
-    { DIDmib_dot11mac_dot11OperationTable_dot11MaxReceiveLifetime,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_MAXRXLIFETIME, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address1,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address2,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address3,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address4,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address5,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address6,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address7,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address8,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address9,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address10,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address11,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address12,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address13,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address14,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address15,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address16,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address17,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address18,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address19,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address20,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address21,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address22,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address23,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address24,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address25,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address26,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address27,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address28,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address29,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address30,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address31,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
-    { DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address32,
-          F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_groupaddress },
 
     /* dot11phy MIB's */
 
-    { DIDmib_dot11phy_dot11PhyOperationTable_dot11PHYType,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_PHYTYPE, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_dot11phy_dot11PhyOperationTable_dot11TempType,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_TEMPTYPE, 0, 0,
-          prism2mib_uint32 },
     { DIDmib_dot11phy_dot11PhyDSSSTable_dot11CurrentChannel,
           F_STA | F_READ,
           HFA384x_RID_CURRENTCHANNEL, 0, 0,
           prism2mib_uint32 },
-    { DIDmib_dot11phy_dot11PhyDSSSTable_dot11CurrentChannel,
-          F_AP | F_READ,
-          HFA384x_RID_CNFOWNCHANNEL, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_dot11phy_dot11PhyDSSSTable_dot11CurrentCCAMode,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_CCAMODE, 0, 0,
+    { DIDmib_dot11phy_dot11PhyTxPowerTable_dot11CurrentTxPowerLevel,
+          F_STA | F_READ | F_WRITE,
+          HFA384x_RID_TXPOWERMAX, 0, 0,
           prism2mib_uint32 },
 
-    /* p2Table MIB's */
-
-    { DIDmib_p2_p2Table_p2MMTx,
-          F_AP | F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_priv },
-    { DIDmib_p2_p2Table_p2EarlyBeacon,
-          F_AP | F_READ | F_WRITE,
-          BIT7, 0, 0,
-          prism2mib_appcfinfoflag },
-    { DIDmib_p2_p2Table_p2ReceivedFrameStatistics,
-          F_AP | F_STA | F_READ,
-          0, 0, 0,
-          prism2mib_priv },
-    { DIDmib_p2_p2Table_p2CommunicationTallies,
-          F_AP | F_STA | F_READ,
-          0, 0, 0,
-          prism2mib_priv },
-    { DIDmib_p2_p2Table_p2Authenticated,
-          F_AP | F_READ,
-          0, 0, 0,
-          prism2mib_priv },
-    { DIDmib_p2_p2Table_p2Associated,
-          F_AP | F_READ,
-          0, 0, 0,
-          prism2mib_priv },
-    { DIDmib_p2_p2Table_p2PowerSaveUserCount,
-          F_AP | F_READ,
-          0, 0, 0,
-          prism2mib_priv },
-    { DIDmib_p2_p2Table_p2Comment,
-          F_AP | F_STA | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_priv },
-    { DIDmib_p2_p2Table_p2AccessMode,
-          F_AP | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_priv },
-    { DIDmib_p2_p2Table_p2AccessAllow,
-          F_AP | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_priv },
-    { DIDmib_p2_p2Table_p2AccessDeny,
-          F_AP | F_READ | F_WRITE,
-          0, 0, 0,
-          prism2mib_priv },
-    { DIDmib_p2_p2Table_p2ChannelInfoResults,
-          F_AP | F_READ,
-          0, 0, 0,
-          prism2mib_priv },
-
     /* p2Static MIB's */
 
     { DIDmib_p2_p2Static_p2CnfPortType,
           F_STA | F_READ | F_WRITE,
           HFA384x_RID_CNFPORTTYPE, 0, 0,
           prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfOwnMACAddress,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFOWNMACADDR, HFA384x_RID_CNFOWNMACADDR_LEN, 0,
-          prism2mib_bytearea2pstr },
-    { DIDmib_p2_p2Static_p2CnfDesiredSSID,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFDESIREDSSID, HFA384x_RID_CNFDESIREDSSID_LEN, 0,
-          prism2mib_bytestr2pstr },
-    { DIDmib_p2_p2Static_p2CnfOwnChannel,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFOWNCHANNEL, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfOwnSSID,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFOWNSSID, HFA384x_RID_CNFOWNSSID_LEN, 0,
-          prism2mib_bytestr2pstr },
-    { DIDmib_p2_p2Static_p2CnfOwnATIMWindow,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFOWNATIMWIN, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfSystemScale,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFSYSSCALE, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfMaxDataLength,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFMAXDATALEN, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfWDSAddress,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFWDSADDR, HFA384x_RID_CNFWDSADDR_LEN, 0,
-          prism2mib_bytearea2pstr },
-    { DIDmib_p2_p2Static_p2CnfPMEnabled,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFPMENABLED, 0, 0,
-          prism2mib_truth },
-    { DIDmib_p2_p2Static_p2CnfPMEPS,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFPMEPS, 0, 0,
-          prism2mib_truth },
-    { DIDmib_p2_p2Static_p2CnfMulticastReceive,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFMULTICASTRX, 0, 0,
-          prism2mib_truth },
-    { DIDmib_p2_p2Static_p2CnfMaxSleepDuration,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFMAXSLEEPDUR, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfPMHoldoverDuration,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFPMHOLDDUR, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfOwnName,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFOWNNAME, HFA384x_RID_CNFOWNNAME_LEN, 0,
-          prism2mib_bytestr2pstr },
-    { DIDmib_p2_p2Static_p2CnfOwnDTIMPeriod,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFOWNDTIMPER, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfWDSAddress1,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFWDSADDR1, HFA384x_RID_CNFWDSADDR1_LEN, 0,
-          prism2mib_bytearea2pstr },
-    { DIDmib_p2_p2Static_p2CnfWDSAddress2,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFWDSADDR2, HFA384x_RID_CNFWDSADDR2_LEN, 0,
-          prism2mib_bytearea2pstr },
-    { DIDmib_p2_p2Static_p2CnfWDSAddress3,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFWDSADDR3, HFA384x_RID_CNFWDSADDR3_LEN, 0,
-          prism2mib_bytearea2pstr },
-    { DIDmib_p2_p2Static_p2CnfWDSAddress4,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFWDSADDR4, HFA384x_RID_CNFWDSADDR4_LEN, 0,
-          prism2mib_bytearea2pstr },
-    { DIDmib_p2_p2Static_p2CnfWDSAddress5,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFWDSADDR5, HFA384x_RID_CNFWDSADDR5_LEN, 0,
-          prism2mib_bytearea2pstr },
-    { DIDmib_p2_p2Static_p2CnfWDSAddress6,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFWDSADDR6, HFA384x_RID_CNFWDSADDR6_LEN, 0,
-          prism2mib_bytearea2pstr },
-    { DIDmib_p2_p2Static_p2CnfMulticastPMBuffering,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFMCASTPMBUFF, 0, 0,
-          prism2mib_truth },
-    { DIDmib_p2_p2Static_p2CnfWEPDefaultKeyID,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFWEPDEFAULTKEYID, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfWEPDefaultKey0,
-          F_AP | F_STA | F_WRITE,
-          HFA384x_RID_CNFWEPDEFAULTKEY0, 0, 0,
-          prism2mib_wepdefaultkey },
-    { DIDmib_p2_p2Static_p2CnfWEPDefaultKey1,
-          F_AP | F_STA | F_WRITE,
-          HFA384x_RID_CNFWEPDEFAULTKEY1, 0, 0,
-          prism2mib_wepdefaultkey },
-    { DIDmib_p2_p2Static_p2CnfWEPDefaultKey2,
-          F_AP | F_STA | F_WRITE,
-          HFA384x_RID_CNFWEPDEFAULTKEY2, 0, 0,
-          prism2mib_wepdefaultkey },
-    { DIDmib_p2_p2Static_p2CnfWEPDefaultKey3,
-          F_AP | F_STA | F_WRITE,
-          HFA384x_RID_CNFWEPDEFAULTKEY3, 0, 0,
-          prism2mib_wepdefaultkey },
-    { DIDmib_p2_p2Static_p2CnfWEPFlags,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFWEPFLAGS, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfAuthentication,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFAUTHENTICATION, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfMaxAssociatedStations,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFMAXASSOCSTATIONS, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfTxControl,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFTXCONTROL, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfRoamingMode,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFROAMINGMODE, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfHostAuthentication,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFHOSTAUTHASSOC, 0, 0,
-          prism2mib_truth },
-    { DIDmib_p2_p2Static_p2CnfRcvCrcError,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFRCVCRCERROR, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfAltRetryCount,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFALTRETRYCNT, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfBeaconInterval,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFAPBCNINT, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfMediumOccupancyLimit,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFAPPCFINFO, HFA384x_RID_CNFAPPCFINFO_LEN, 0,
-          prism2mib_uint32offset },
-    { DIDmib_p2_p2Static_p2CnfCFPPeriod,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFAPPCFINFO, HFA384x_RID_CNFAPPCFINFO_LEN, 1,
-          prism2mib_uint32offset },
-    { DIDmib_p2_p2Static_p2CnfCFPMaxDuration,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFAPPCFINFO, HFA384x_RID_CNFAPPCFINFO_LEN, 2,
-          prism2mib_uint32offset },
-    { DIDmib_p2_p2Static_p2CnfCFPFlags,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFAPPCFINFO, HFA384x_RID_CNFAPPCFINFO_LEN, 3,
-          prism2mib_uint32offset },
-    { DIDmib_p2_p2Static_p2CnfSTAPCFInfo,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFSTAPCFINFO, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfPriorityQUsage,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFPRIORITYQUSAGE, HFA384x_RID_CNFPRIOQUSAGE_LEN, 0,
-          prism2mib_uint32array },
-    { DIDmib_p2_p2Static_p2CnfTIMCtrl,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFTIMCTRL, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfThirty2Tally,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFTHIRTY2TALLY, 0, 0,
-          prism2mib_truth },
-    { DIDmib_p2_p2Static_p2CnfEnhSecurity,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFENHSECURITY, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfShortPreamble,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFSHORTPREAMBLE, 0, 0,
-          prism2mib_preamble },
-    { DIDmib_p2_p2Static_p2CnfExcludeLongPreamble,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_CNFEXCLONGPREAMBLE, 0, 0,
-          prism2mib_truth },
-    { DIDmib_p2_p2Static_p2CnfAuthenticationRspTO,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFAUTHRSPTIMEOUT, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfBasicRates,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFBASICRATES, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Static_p2CnfSupportedRates,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CNFSUPPRATES, 0, 0,
-          prism2mib_uint32 },
-
-    /* p2Dynamic MIB's */
-
-    { DIDmib_p2_p2Dynamic_p2CreateIBSS,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_CREATEIBSS, 0, 0,
-          prism2mib_truth },
-    { DIDmib_p2_p2Dynamic_p2FragmentationThreshold,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_FRAGTHRESH, 0, 0,
-          prism2mib_fragmentationthreshold },
-    { DIDmib_p2_p2Dynamic_p2RTSThreshold,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_RTSTHRESH, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Dynamic_p2TxRateControl,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_TXRATECNTL, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Dynamic_p2PromiscuousMode,
-          F_STA | F_READ | F_WRITE,
-          HFA384x_RID_PROMISCMODE, 0, 0,
-          prism2mib_truth },
-    { DIDmib_p2_p2Dynamic_p2FragmentationThreshold0,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_FRAGTHRESH0, 0, 0,
-          prism2mib_fragmentationthreshold },
-    { DIDmib_p2_p2Dynamic_p2FragmentationThreshold1,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_FRAGTHRESH1, 0, 0,
-          prism2mib_fragmentationthreshold },
-    { DIDmib_p2_p2Dynamic_p2FragmentationThreshold2,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_FRAGTHRESH2, 0, 0,
-          prism2mib_fragmentationthreshold },
-    { DIDmib_p2_p2Dynamic_p2FragmentationThreshold3,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_FRAGTHRESH3, 0, 0,
-          prism2mib_fragmentationthreshold },
-    { DIDmib_p2_p2Dynamic_p2FragmentationThreshold4,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_FRAGTHRESH4, 0, 0,
-          prism2mib_fragmentationthreshold },
-    { DIDmib_p2_p2Dynamic_p2FragmentationThreshold5,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_FRAGTHRESH5, 0, 0,
-          prism2mib_fragmentationthreshold },
-    { DIDmib_p2_p2Dynamic_p2FragmentationThreshold6,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_FRAGTHRESH6, 0, 0,
-          prism2mib_fragmentationthreshold },
-    { DIDmib_p2_p2Dynamic_p2RTSThreshold0,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_RTSTHRESH0, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Dynamic_p2RTSThreshold1,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_RTSTHRESH1, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Dynamic_p2RTSThreshold2,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_RTSTHRESH2, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Dynamic_p2RTSThreshold3,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_RTSTHRESH3, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Dynamic_p2RTSThreshold4,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_RTSTHRESH4, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Dynamic_p2RTSThreshold5,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_RTSTHRESH5, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Dynamic_p2RTSThreshold6,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_RTSTHRESH6, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Dynamic_p2TxRateControl0,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_TXRATECNTL0, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Dynamic_p2TxRateControl1,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_TXRATECNTL1, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Dynamic_p2TxRateControl2,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_TXRATECNTL2, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Dynamic_p2TxRateControl3,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_TXRATECNTL3, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Dynamic_p2TxRateControl4,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_TXRATECNTL4, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Dynamic_p2TxRateControl5,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_TXRATECNTL5, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Dynamic_p2TxRateControl6,
-          F_AP | F_READ | F_WRITE,
-          HFA384x_RID_TXRATECNTL6, 0, 0,
-          prism2mib_uint32 },
-
-    /* p2Behavior MIB's */
-
-    { DIDmib_p2_p2Behavior_p2TickTime,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_ITICKTIME, 0, 0,
-          prism2mib_uint32 },
-
-    /* p2NIC MIB's */
-
-    { DIDmib_p2_p2NIC_p2MaxLoadTime,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_MAXLOADTIME, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2NIC_p2DLBufferPage,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_DOWNLOADBUFFER, HFA384x_RID_DOWNLOADBUFFER_LEN, 0,
-          prism2mib_uint32offset },
-    { DIDmib_p2_p2NIC_p2DLBufferOffset,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_DOWNLOADBUFFER, HFA384x_RID_DOWNLOADBUFFER_LEN, 1,
-          prism2mib_uint32offset },
-    { DIDmib_p2_p2NIC_p2DLBufferLength,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_DOWNLOADBUFFER, HFA384x_RID_DOWNLOADBUFFER_LEN, 2,
-          prism2mib_uint32offset },
-    { DIDmib_p2_p2NIC_p2PRIIdentity,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_PRIIDENTITY, HFA384x_RID_PRIIDENTITY_LEN, 0,
-          prism2mib_uint32array },
-    { DIDmib_p2_p2NIC_p2PRISupRange,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_PRISUPRANGE, HFA384x_RID_PRISUPRANGE_LEN, 0,
-          prism2mib_uint32array },
-    { DIDmib_p2_p2NIC_p2CFIActRanges,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_PRI_CFIACTRANGES, HFA384x_RID_CFIACTRANGES_LEN, 0,
-          prism2mib_uint32array },
-    { DIDmib_p2_p2NIC_p2BuildSequence,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_BUILDSEQ, HFA384x_RID_BUILDSEQ_LEN, 0,
-          prism2mib_uint32array },
-    { DIDmib_p2_p2NIC_p2PrimaryFWID,
-          F_AP | F_STA | F_READ,
-          0, 0, 0,
-          prism2mib_fwid },
-    { DIDmib_p2_p2NIC_p2SecondaryFWID,
-          F_AP | F_STA | F_READ,
-          0, 0, 0,
-          prism2mib_fwid },
-    { DIDmib_p2_p2NIC_p2TertiaryFWID,
-          F_AP | F_READ,
-          0, 0, 0,
-          prism2mib_fwid },
-    { DIDmib_p2_p2NIC_p2NICSerialNumber,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_NICSERIALNUMBER, HFA384x_RID_NICSERIALNUMBER_LEN, 0,
-          prism2mib_bytearea2pstr },
-    { DIDmib_p2_p2NIC_p2NICIdentity,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_NICIDENTITY, HFA384x_RID_NICIDENTITY_LEN, 0,
-          prism2mib_uint32array },
-    { DIDmib_p2_p2NIC_p2MFISupRange,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_MFISUPRANGE, HFA384x_RID_MFISUPRANGE_LEN, 0,
-          prism2mib_uint32array },
-    { DIDmib_p2_p2NIC_p2CFISupRange,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_CFISUPRANGE, HFA384x_RID_CFISUPRANGE_LEN, 0,
-          prism2mib_uint32array },
-    { DIDmib_p2_p2NIC_p2ChannelList,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_CHANNELLIST, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2NIC_p2RegulatoryDomains,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_REGULATORYDOMAINS, HFA384x_RID_REGULATORYDOMAINS_LEN, 0,
-          prism2mib_regulatorydomains },
-    { DIDmib_p2_p2NIC_p2TempType,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_TEMPTYPE, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2NIC_p2STAIdentity,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_STAIDENTITY, HFA384x_RID_STAIDENTITY_LEN, 0,
-          prism2mib_uint32array },
-    { DIDmib_p2_p2NIC_p2STASupRange,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_STASUPRANGE, HFA384x_RID_STASUPRANGE_LEN, 0,
-          prism2mib_uint32array },
-    { DIDmib_p2_p2NIC_p2MFIActRanges,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_STA_MFIACTRANGES, HFA384x_RID_MFIACTRANGES_LEN, 0,
-          prism2mib_uint32array },
-    { DIDmib_p2_p2NIC_p2STACFIActRanges,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_STA_CFIACTRANGES, HFA384x_RID_CFIACTRANGES2_LEN, 0,
-          prism2mib_uint32array },
 
     /* p2MAC MIB's */
 
-    { DIDmib_p2_p2MAC_p2PortStatus,
-          F_STA | F_READ,
-          HFA384x_RID_PORTSTATUS, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2MAC_p2CurrentSSID,
-          F_STA | F_READ,
-          HFA384x_RID_CURRENTSSID, HFA384x_RID_CURRENTSSID_LEN, 0,
-          prism2mib_bytestr2pstr },
-    { DIDmib_p2_p2MAC_p2CurrentBSSID,
-          F_STA | F_READ,
-          HFA384x_RID_CURRENTBSSID, HFA384x_RID_CURRENTBSSID_LEN, 0,
-          prism2mib_bytearea2pstr },
-    { DIDmib_p2_p2MAC_p2CommsQuality,
-          F_STA | F_READ,
-          HFA384x_RID_COMMSQUALITY, HFA384x_RID_COMMSQUALITY_LEN, 0,
-          prism2mib_uint32array },
-    { DIDmib_p2_p2MAC_p2CommsQualityCQ,
-          F_STA | F_READ,
-          HFA384x_RID_COMMSQUALITY, HFA384x_RID_COMMSQUALITY_LEN, 0,
-          prism2mib_uint32offset },
-    { DIDmib_p2_p2MAC_p2CommsQualityASL,
-          F_STA | F_READ,
-          HFA384x_RID_COMMSQUALITY, HFA384x_RID_COMMSQUALITY_LEN, 1,
-          prism2mib_uint32offset },
-    { DIDmib_p2_p2MAC_p2CommsQualityANL,
-          F_STA | F_READ,
-          HFA384x_RID_COMMSQUALITY, HFA384x_RID_COMMSQUALITY_LEN, 2,
-          prism2mib_uint32offset },
-    { DIDmib_p2_p2MAC_p2dbmCommsQuality,
-          F_STA | F_READ,
-          HFA384x_RID_DBMCOMMSQUALITY, HFA384x_RID_DBMCOMMSQUALITY_LEN, 0,
-          prism2mib_uint32array },
-    { DIDmib_p2_p2MAC_p2dbmCommsQualityCQ,
-          F_STA | F_READ,
-          HFA384x_RID_COMMSQUALITY, HFA384x_RID_COMMSQUALITY_LEN, 0,
-          prism2mib_uint32offset },
-    { DIDmib_p2_p2MAC_p2dbmCommsQualityASL,
-          F_STA | F_READ,
-          HFA384x_RID_COMMSQUALITY, HFA384x_RID_COMMSQUALITY_LEN, 1,
-          prism2mib_uint32offset },
-    { DIDmib_p2_p2MAC_p2dbmCommsQualityANL,
-          F_STA | F_READ,
-          HFA384x_RID_COMMSQUALITY, HFA384x_RID_COMMSQUALITY_LEN, 2,
-          prism2mib_uint32offset },
     { DIDmib_p2_p2MAC_p2CurrentTxRate,
           F_STA | F_READ,
           HFA384x_RID_CURRENTTXRATE, 0, 0,
           prism2mib_uint32 },
-    { DIDmib_p2_p2MAC_p2CurrentBeaconInterval,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_CURRENTBCNINT, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2MAC_p2StaCurrentScaleThresholds,
-          F_STA | F_READ,
-          HFA384x_RID_CURRENTSCALETHRESH, HFA384x_RID_STACURSCALETHRESH_LEN, 0,
-          prism2mib_uint32array },
-    { DIDmib_p2_p2MAC_p2APCurrentScaleThresholds,
-          F_AP | F_READ,
-          HFA384x_RID_CURRENTSCALETHRESH, HFA384x_RID_APCURSCALETHRESH_LEN, 0,
-          prism2mib_uint32array },
-    { DIDmib_p2_p2MAC_p2ProtocolRspTime,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_PROTOCOLRSPTIME, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2MAC_p2ShortRetryLimit,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_SHORTRETRYLIMIT, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2MAC_p2LongRetryLimit,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_LONGRETRYLIMIT, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2MAC_p2MaxTransmitLifetime,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_MAXTXLIFETIME, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2MAC_p2MaxReceiveLifetime,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_MAXRXLIFETIME, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2MAC_p2CFPollable,
-          F_STA | F_READ,
-          HFA384x_RID_CFPOLLABLE, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2MAC_p2AuthenticationAlgorithms,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_AUTHALGORITHMS, HFA384x_RID_AUTHALGORITHMS_LEN, 0,
-          prism2mib_uint32array },
-    { DIDmib_p2_p2MAC_p2PrivacyOptionImplemented,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_PRIVACYOPTIMP, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2MAC_p2CurrentTxRate1,
-          F_AP | F_READ,
-          HFA384x_RID_CURRENTTXRATE1, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2MAC_p2CurrentTxRate2,
-          F_AP | F_READ,
-          HFA384x_RID_CURRENTTXRATE2, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2MAC_p2CurrentTxRate3,
-          F_AP | F_READ,
-          HFA384x_RID_CURRENTTXRATE3, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2MAC_p2CurrentTxRate4,
-          F_AP | F_READ,
-          HFA384x_RID_CURRENTTXRATE4, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2MAC_p2CurrentTxRate5,
-          F_AP | F_READ,
-          HFA384x_RID_CURRENTTXRATE5, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2MAC_p2CurrentTxRate6,
-          F_AP | F_READ,
-          HFA384x_RID_CURRENTTXRATE6, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2MAC_p2OwnMACAddress,
-          F_AP | F_READ,
-          HFA384x_RID_OWNMACADDRESS, HFA384x_RID_OWNMACADDRESS_LEN, 0,
-          prism2mib_bytearea2pstr },
-
-    /* p2Modem MIB's */
-
-    { DIDmib_p2_p2Modem_p2PHYType,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_PHYTYPE, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Modem_p2CurrentChannel,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_CURRENTCHANNEL, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Modem_p2CurrentPowerState,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_CURRENTPOWERSTATE, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Modem_p2CCAMode,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_CCAMODE, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Modem_p2TxPowerMax,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_TXPOWERMAX, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_dot11phy_dot11PhyTxPowerTable_dot11CurrentTxPowerLevel,
-          F_AP | F_STA | F_READ | F_WRITE,
-          HFA384x_RID_TXPOWERMAX, 0, 0,
-          prism2mib_uint32 },
-    { DIDmib_p2_p2Modem_p2SupportedDataRates,
-          F_AP | F_STA | F_READ,
-          HFA384x_RID_SUPPORTEDDATARATES, HFA384x_RID_SUPPORTEDDATARATES_LEN, 0,
-          prism2mib_bytestr2pstr },
 
     /* And finally, lnx mibs */
     { DIDmib_lnx_lnxConfigTable_lnxRSNAIE,
@@ -1285,94 +270,6 @@ static mibrec_t mibtab[] = {
           prism2mib_priv },
     { 0, 0, 0, 0, 0, NULL}};
 
-/*----------------------------------------------------------------
-These MIB's are not supported at this time:
-
-DIDmib_dot11phy_dot11PhyOperationTable_dot11ChannelAgilityPresent
-DIDmib_dot11phy_dot11PhyOperationTable_dot11ChannelAgilityEnabled
-DIDmib_dot11phy_dot11PhyDSSSTable_dot11PBCCOptionImplemented
-DIDmib_dot11phy_dot11RegDomainsSupportedTable_dot11RegDomainsSupportIndex
-DIDmib_dot11phy_dot11SupportedDataRatesTxTable_dot11SupportedDataRatesTxIndex
-DIDmib_dot11phy_dot11SupportedDataRatesTxTable_dot11SupportedDataRatesTxValue
-DIDmib_dot11phy_dot11SupportedDataRatesRxTable_dot11SupportedDataRatesRxIndex
-DIDmib_dot11phy_dot11SupportedDataRatesRxTable_dot11SupportedDataRatesRxValue
-
-DIDmib_dot11phy_dot11RegDomainsSupportedTable_dot11RegDomainsSupportValue
-TODO: need to investigate why wlan has this as enumerated and Prism2 has this
-      as btye str.
-
-DIDmib_dot11phy_dot11PhyDSSSTable_dot11ShortPreambleOptionImplemented
-TODO: Find out the firmware version number(s) for identifying
-      whether the firmware is capable of short preamble. TRUE or FALSE
-      will be returned based on the version of the firmware.
-
-WEP Key mappings aren't supported in the f/w.
-DIDmib_dot11smt_dot11WEPKeyMappingsTable_dot11WEPKeyMappingIndex
-DIDmib_dot11smt_dot11WEPKeyMappingsTable_dot11WEPKeyMappingAddress
-DIDmib_dot11smt_dot11WEPKeyMappingsTable_dot11WEPKeyMappingWEPOn
-DIDmib_dot11smt_dot11WEPKeyMappingsTable_dot11WEPKeyMappingValue
-DIDmib_dot11smt_dot11PrivacyTable_dot11WEPKeyMappingLength
-
-TODO: implement counters.
-DIDmib_dot11smt_dot11PrivacyTable_dot11WEPICVErrorCount
-DIDmib_dot11smt_dot11PrivacyTable_dot11WEPExcludedCount
-DIDmib_dot11mac_dot11CountersTable_dot11TransmittedFragmentCount
-DIDmib_dot11mac_dot11CountersTable_dot11MulticastTransmittedFrameCount
-DIDmib_dot11mac_dot11CountersTable_dot11FailedCount
-DIDmib_dot11mac_dot11CountersTable_dot11RetryCount
-DIDmib_dot11mac_dot11CountersTable_dot11MultipleRetryCount
-DIDmib_dot11mac_dot11CountersTable_dot11FrameDuplicateCount
-DIDmib_dot11mac_dot11CountersTable_dot11RTSSuccessCount
-DIDmib_dot11mac_dot11CountersTable_dot11RTSFailureCount
-DIDmib_dot11mac_dot11CountersTable_dot11ACKFailureCount
-DIDmib_dot11mac_dot11CountersTable_dot11ReceivedFragmentCount
-DIDmib_dot11mac_dot11CountersTable_dot11MulticastReceivedFrameCount
-DIDmib_dot11mac_dot11CountersTable_dot11FCSErrorCount
-DIDmib_dot11mac_dot11CountersTable_dot11TransmittedFrameCount
-DIDmib_dot11mac_dot11CountersTable_dot11WEPUndecryptableCount
-
-TODO: implement sane values for these.
-DIDmib_dot11mac_dot11OperationTable_dot11ManufacturerID
-DIDmib_dot11mac_dot11OperationTable_dot11ProductID
-
-Not too worried about these at the moment.
-DIDmib_dot11phy_dot11PhyAntennaTable_dot11CurrentTxAntenna
-DIDmib_dot11phy_dot11PhyAntennaTable_dot11DiversitySupport
-DIDmib_dot11phy_dot11PhyAntennaTable_dot11CurrentRxAntenna
-DIDmib_dot11phy_dot11PhyTxPowerTable_dot11NumberSupportedPowerLevels
-DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel1
-DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel2
-DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel3
-DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel4
-DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel5
-DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel6
-DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel7
-DIDmib_dot11phy_dot11PhyTxPowerTable_dot11TxPowerLevel8
-DIDmib_dot11phy_dot11PhyTxPowerTable_dot11CurrentTxPowerLevel
-
-Ummm, FH and IR don't apply
-DIDmib_dot11phy_dot11PhyFHSSTable_dot11HopTime
-DIDmib_dot11phy_dot11PhyFHSSTable_dot11CurrentChannelNumber
-DIDmib_dot11phy_dot11PhyFHSSTable_dot11MaxDwellTime
-DIDmib_dot11phy_dot11PhyFHSSTable_dot11CurrentDwellTime
-DIDmib_dot11phy_dot11PhyFHSSTable_dot11CurrentSet
-DIDmib_dot11phy_dot11PhyFHSSTable_dot11CurrentPattern
-DIDmib_dot11phy_dot11PhyFHSSTable_dot11CurrentIndex
-DIDmib_dot11phy_dot11PhyDSSSTable_dot11CCAModeSupported
-DIDmib_dot11phy_dot11PhyDSSSTable_dot11EDThreshold
-DIDmib_dot11phy_dot11PhyIRTable_dot11CCAWatchdogTimerMax
-DIDmib_dot11phy_dot11PhyIRTable_dot11CCAWatchdogCountMax
-DIDmib_dot11phy_dot11PhyIRTable_dot11CCAWatchdogTimerMin
-DIDmib_dot11phy_dot11PhyIRTable_dot11CCAWatchdogCountMin
-
-We just don't have enough antennas right now to worry about this.
-DIDmib_dot11phy_dot11AntennasListTable_dot11AntennaListIndex
-DIDmib_dot11phy_dot11AntennasListTable_dot11SupportedTxAntenna
-DIDmib_dot11phy_dot11AntennasListTable_dot11SupportedRxAntenna
-DIDmib_dot11phy_dot11AntennasListTable_dot11DiversitySelectionRx
-
-------------------------------------------------------------------*/
-
 /*================================================================*/
 /* Function Definitions */
 
@@ -1401,7 +298,8 @@ int prism2mgmt_mibset_mibget(wlandevice_t *wlandev, void *msgp)
 	hfa384x_t		*hw = wlandev->priv;
 	int			result, isget;
 	mibrec_t		*mib;
-	UINT16			which;
+
+	u16			which;
 
 	p80211msg_dot11req_mibset_t	*msg = msgp;
 	p80211itemd_t			*mibitem;
@@ -1415,7 +313,7 @@ int prism2mgmt_mibset_mibget(wlandevice_t *wlandev, void *msgp)
 	** Determine if this is an Access Point or a station.
 	*/
 
-	which = hw->ap ? F_AP : F_STA;
+	which = F_STA;
 
 	/*
 	** Find the MIB in the MIB table.  Note that a MIB may be in the
@@ -1491,59 +389,6 @@ done:
 }
 
 /*----------------------------------------------------------------
-* prism2mib_bytestr2pstr
-*
-* Get/set pstr data to/from a byte string.
-*
-* MIB record parameters:
-*       parm1    Prism2 RID value.
-*       parm2    Number of bytes of RID data.
-*       parm3    Not used.
-*
-* Arguments:
-*       mib      MIB record.
-*       isget    MIBGET/MIBSET flag.
-*       wlandev  wlan device structure.
-*       priv     "priv" structure.
-*       hw       "hw" structure.
-*       msg      Message structure.
-*       data     Data buffer.
-*
-* Returns:
-*       0   - Success.
-*       ~0  - Error.
-*
-----------------------------------------------------------------*/
-
-static int prism2mib_bytestr2pstr(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data)
-{
-	int                result;
-	p80211pstrd_t      *pstr = (p80211pstrd_t*) data;
-	UINT8              bytebuf[MIB_TMP_MAXLEN];
-	hfa384x_bytestr_t  *p2bytestr = (hfa384x_bytestr_t*) bytebuf;
-
-	DBFENTER;
-
-	if (isget) {
-		result = hfa384x_drvr_getconfig(hw, mib->parm1, bytebuf, mib->parm2);
-		prism2mgmt_bytestr2pstr(p2bytestr, pstr);
-	} else {
-		memset(bytebuf, 0, mib->parm2);
-		prism2mgmt_pstr2bytestr(p2bytestr, pstr);
-		result = hfa384x_drvr_setconfig(hw, mib->parm1, bytebuf, mib->parm2);
-	}
-
-	DBFEXIT;
-	return(result);
-}
-
-/*----------------------------------------------------------------
 * prism2mib_bytearea2pstr
 *
 * Get/set pstr data to/from a byte area.
@@ -1578,13 +423,13 @@ void                         *data)
 {
 	int            result;
 	p80211pstrd_t  *pstr = (p80211pstrd_t*) data;
-	UINT8          bytebuf[MIB_TMP_MAXLEN];
+	u8          bytebuf[MIB_TMP_MAXLEN];
 
 	DBFENTER;
 
 	if (isget) {
 		result = hfa384x_drvr_getconfig(hw, mib->parm1, bytebuf, mib->parm2);
-		prism2mgmt_bytearea2pstr(bytebuf, pstr,	mib->parm2);
+		prism2mgmt_bytearea2pstr(bytebuf, pstr, mib->parm2);
 	} else {
 		memset(bytebuf, 0, mib->parm2);
 		prism2mgmt_pstr2bytearea(bytebuf, pstr);
@@ -1629,9 +474,9 @@ p80211msg_dot11req_mibset_t  *msg,
 void                         *data)
 {
 	int     result;
-	UINT32  *uint32 = (UINT32*) data;
-	UINT8   bytebuf[MIB_TMP_MAXLEN];
-	UINT16  *wordbuf = (UINT16*) bytebuf;
+	u32  *uint32 = (u32*) data;
+	u8   bytebuf[MIB_TMP_MAXLEN];
+	u16  *wordbuf = (u16*) bytebuf;
 
 	DBFENTER;
 
@@ -1654,178 +499,6 @@ void                         *data)
 }
 
 /*----------------------------------------------------------------
-* prism2mib_uint32array
-*
-* Get/set an array of uint32 data.
-*
-* MIB record parameters:
-*       parm1    Prism2 RID value.
-*       parm2    Number of bytes of RID data.
-*       parm3    Not used.
-*
-* Arguments:
-*       mib      MIB record.
-*       isget    MIBGET/MIBSET flag.
-*       wlandev  wlan device structure.
-*       priv     "priv" structure.
-*       hw       "hw" structure.
-*       msg      Message structure.
-*       data     Data buffer.
-*
-* Returns:
-*       0   - Success.
-*       ~0  - Error.
-*
-----------------------------------------------------------------*/
-
-static int prism2mib_uint32array(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data)
-{
-	int     result;
-	UINT32  *uint32 = (UINT32 *) data;
-	UINT8   bytebuf[MIB_TMP_MAXLEN];
-	UINT16  *wordbuf = (UINT16*) bytebuf;
-	int     i, cnt;
-
-	DBFENTER;
-
-	cnt = mib->parm2 / sizeof(UINT16);
-
-	if (isget) {
-		result = hfa384x_drvr_getconfig(hw, mib->parm1, wordbuf, mib->parm2);
-		for (i = 0; i < cnt; i++)
-			prism2mgmt_prism2int2p80211int(wordbuf+i, uint32+i);
-	} else {
-		for (i = 0; i < cnt; i++)
-			prism2mgmt_p80211int2prism2int(wordbuf+i, uint32+i);
-		result = hfa384x_drvr_setconfig(hw, mib->parm1, wordbuf, mib->parm2);
-		}
-
-	DBFEXIT;
-	return(result);
-}
-
-/*----------------------------------------------------------------
-* prism2mib_uint32offset
-*
-* Get/set a single element in an array of uint32 data.
-*
-* MIB record parameters:
-*       parm1    Prism2 RID value.
-*       parm2    Number of bytes of RID data.
-*       parm3    Element index.
-*
-* Arguments:
-*       mib      MIB record.
-*       isget    MIBGET/MIBSET flag.
-*       wlandev  wlan device structure.
-*       priv     "priv" structure.
-*       hw       "hw" structure.
-*       msg      Message structure.
-*       data     Data buffer.
-*
-* Returns:
-*       0   - Success.
-*       ~0  - Error.
-*
-----------------------------------------------------------------*/
-
-static int prism2mib_uint32offset(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data)
-{
-	int     result;
-	UINT32  *uint32 = (UINT32*) data;
-	UINT8   bytebuf[MIB_TMP_MAXLEN];
-	UINT16  *wordbuf = (UINT16*) bytebuf;
-	UINT16  cnt;
-
-	DBFENTER;
-
-	cnt = mib->parm2 / sizeof(UINT16);
-
-	result = hfa384x_drvr_getconfig(hw, mib->parm1, wordbuf, mib->parm2);
-	if (result == 0) {
-		if (isget) {
-			if (mib->parm3 < cnt)
-				prism2mgmt_prism2int2p80211int(wordbuf+mib->parm3, uint32);
-			else
-				*uint32 = 0;
-		} else {
-			if (mib->parm3 < cnt) {
-				prism2mgmt_p80211int2prism2int(wordbuf+mib->parm3, uint32);
-				result = hfa384x_drvr_setconfig(hw, mib->parm1, wordbuf, mib->parm2);
-			}
-		}
-	}
-
-	DBFEXIT;
-	return(result);
-}
-
-/*----------------------------------------------------------------
-* prism2mib_truth
-*
-* Get/set truth data.
-*
-* MIB record parameters:
-*       parm1    Prism2 RID value.
-*       parm2    Not used.
-*       parm3    Not used.
-*
-* Arguments:
-*       mib      MIB record.
-*       isget    MIBGET/MIBSET flag.
-*       wlandev  wlan device structure.
-*       priv     "priv" structure.
-*       hw       "hw" structure.
-*       msg      Message structure.
-*       data     Data buffer.
-*
-* Returns:
-*       0   - Success.
-*       ~0  - Error.
-*
-----------------------------------------------------------------*/
-
-static int prism2mib_truth(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data)
-{
-	int     result;
-	UINT32  *uint32 = (UINT32*) data;
-	UINT8   bytebuf[MIB_TMP_MAXLEN];
-	UINT16  *wordbuf = (UINT16*) bytebuf;
-
-	DBFENTER;
-
-	if (isget) {
-		result = hfa384x_drvr_getconfig16(hw, mib->parm1, wordbuf);
-		*uint32 = (*wordbuf) ?
-				P80211ENUM_truth_true : P80211ENUM_truth_false;
-	} else {
-		*wordbuf = ((*uint32) == P80211ENUM_truth_true) ? 1 : 0;
-		result = hfa384x_drvr_setconfig16(hw, mib->parm1, *wordbuf);
-	}
-
-	DBFEXIT;
-	return(result);
-}
-
-/*----------------------------------------------------------------
 * prism2mib_flag
 *
 * Get/set a flag.
@@ -1859,10 +532,10 @@ p80211msg_dot11req_mibset_t  *msg,
 void                         *data)
 {
 	int     result;
-	UINT32  *uint32 = (UINT32*) data;
-	UINT8   bytebuf[MIB_TMP_MAXLEN];
-	UINT16  *wordbuf = (UINT16*) bytebuf;
-	UINT32  flags;
+	u32  *uint32 = (u32*) data;
+	u8   bytebuf[MIB_TMP_MAXLEN];
+	u16  *wordbuf = (u16*) bytebuf;
+	u32  flags;
 
 	DBFENTER;
 
@@ -1893,121 +566,6 @@ void                         *data)
 }
 
 /*----------------------------------------------------------------
-* prism2mib_appcfinfoflag
-*
-* Get/set a single flag in the APPCFINFO record.
-*
-* MIB record parameters:
-*       parm1    Bit to get/set.
-*       parm2    Not used.
-*       parm3    Not used.
-*
-* Arguments:
-*       mib      MIB record.
-*       isget    MIBGET/MIBSET flag.
-*       wlandev  wlan device structure.
-*       priv     "priv" structure.
-*       hw       "hw" structure.
-*       msg      Message structure.
-*       data     Data buffer.
-*
-* Returns:
-*       0   - Success.
-*       ~0  - Error.
-*
-----------------------------------------------------------------*/
-
-static int prism2mib_appcfinfoflag(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data)
-{
-	int     result;
-	UINT32  *uint32 = (UINT32*) data;
-	UINT8   bytebuf[MIB_TMP_MAXLEN];
-	UINT16  *wordbuf = (UINT16*) bytebuf;
-	UINT16  word;
-
-	DBFENTER;
-
-	result = hfa384x_drvr_getconfig(hw, HFA384x_RID_CNFAPPCFINFO,
-					bytebuf, HFA384x_RID_CNFAPPCFINFO_LEN);
-	if (result == 0) {
-		if (isget) {
-			*uint32 = (hfa384x2host_16(wordbuf[3]) & mib->parm1) ?
-				P80211ENUM_truth_true : P80211ENUM_truth_false;
-		} else {
-			word = hfa384x2host_16(wordbuf[3]);
-			word = ((*uint32) == P80211ENUM_truth_true) ?
-				(word | mib->parm1) : (word & ~mib->parm1);
-			wordbuf[3] = host2hfa384x_16(word);
-			result = hfa384x_drvr_setconfig(hw, HFA384x_RID_CNFAPPCFINFO,
-					bytebuf, HFA384x_RID_CNFAPPCFINFO_LEN);
-		}
-	}
-
-	DBFEXIT;
-	return(result);
-}
-
-/*----------------------------------------------------------------
-* prism2mib_regulatorydomains
-*
-* Get regulatory domain data.
-*
-* MIB record parameters:
-*       parm1    Prism2 RID value.
-*       parm2    Number of bytes of RID data.
-*       parm3    Not used.
-*
-* Arguments:
-*       mib      MIB record.
-*       isget    MIBGET/MIBSET flag.
-*       wlandev  wlan device structure.
-*       priv     "priv" structure.
-*       hw       "hw" structure.
-*       msg      Message structure.
-*       data     Data buffer.
-*
-* Returns:
-*       0   - Success.
-*       ~0  - Error.
-*
-----------------------------------------------------------------*/
-
-static int prism2mib_regulatorydomains(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data)
-{
-	int            result;
-	UINT32         cnt;
-	p80211pstrd_t  *pstr = (p80211pstrd_t*) data;
-	UINT8          bytebuf[MIB_TMP_MAXLEN];
-	UINT16         *wordbuf = (UINT16*) bytebuf;
-
-	DBFENTER;
-
-	result = 0;
-
-	if (isget) {
-		result = hfa384x_drvr_getconfig(hw, mib->parm1, wordbuf, mib->parm2);
-		prism2mgmt_prism2int2p80211int(wordbuf, &cnt);
-		pstr->len = (UINT8) cnt;
-		memcpy(pstr->data, &wordbuf[1], pstr->len);
-	}
-
-	DBFEXIT;
-	return(result);
-}
-
-/*----------------------------------------------------------------
 * prism2mib_wepdefaultkey
 *
 * Get/set WEP default keys.
@@ -2042,8 +600,8 @@ void                         *data)
 {
 	int            result;
 	p80211pstrd_t  *pstr = (p80211pstrd_t*) data;
-	UINT8          bytebuf[MIB_TMP_MAXLEN];
-	UINT16         len;
+	u8          bytebuf[MIB_TMP_MAXLEN];
+	u16         len;
 
 	DBFENTER;
 
@@ -2062,114 +620,6 @@ void                         *data)
 }
 
 /*----------------------------------------------------------------
-* prism2mib_powermanagement
-*
-* Get/set 802.11 power management value.  Note that this is defined differently
-* by 802.11 and Prism2:
-*
-*       Meaning     802.11       Prism2
-*        active       1           false
-*      powersave      2           true
-*
-* MIB record parameters:
-*       parm1    Prism2 RID value.
-*       parm2    Not used.
-*       parm3    Not used.
-*
-* Arguments:
-*       mib      MIB record.
-*       isget    MIBGET/MIBSET flag.
-*       wlandev  wlan device structure.
-*       priv     "priv" structure.
-*       hw       "hw" structure.
-*       msg      Message structure.
-*       data     Data buffer.
-*
-* Returns:
-*       0   - Success.
-*       ~0  - Error.
-*
-----------------------------------------------------------------*/
-
-static int prism2mib_powermanagement(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data)
-{
-	int     result;
-	UINT32  *uint32 = (UINT32*) data;
-	UINT32  value;
-
-	DBFENTER;
-
-	if (isget) {
-		result = prism2mib_uint32(mib, isget, wlandev, hw, msg, &value);
-		*uint32 = (value == 0) ? 1 : 2;
-	} else {
-		value = ((*uint32) == 1) ? 0 : 1;
-		result = prism2mib_uint32(mib, isget, wlandev, hw, msg, &value);
-	}
-
-	DBFEXIT;
-	return(result);
-}
-
-/*----------------------------------------------------------------
-* prism2mib_preamble
-*
-* Get/set Prism2 short preamble
-*
-* MIB record parameters:
-*       parm1    Prism2 RID value.
-*       parm2    Not used.
-*       parm3    Not used.
-*
-* Arguments:
-*       mib      MIB record.
-*       isget    MIBGET/MIBSET flag.
-*       wlandev  wlan device structure.
-*       priv     "priv" structure.
-*       hw       "hw" structure.
-*       msg      Message structure.
-*       data     Data buffer.
-*
-* Returns:
-*       0   - Success.
-*       ~0  - Error.
-*
-----------------------------------------------------------------*/
-
-static int prism2mib_preamble(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data)
-{
-	int     result;
-	UINT32  *uint32 = (UINT32*) data;
-	UINT8   bytebuf[MIB_TMP_MAXLEN];
-	UINT16  *wordbuf = (UINT16*) bytebuf;
-
-	DBFENTER;
-
-	if (isget) {
-		result = hfa384x_drvr_getconfig16(hw, mib->parm1, wordbuf);
-		*uint32 = *wordbuf;
-	} else {
-		*wordbuf = *uint32;
-		result = hfa384x_drvr_setconfig16(hw, mib->parm1, *wordbuf);
-	}
-
-	DBFEXIT;
-	return(result);
-}
-
-/*----------------------------------------------------------------
 * prism2mib_privacyinvoked
 *
 * Get/set the dot11PrivacyInvoked value.
@@ -2296,7 +746,7 @@ p80211msg_dot11req_mibset_t  *msg,
 void                         *data)
 {
 	int     result;
-	UINT32  *uint32 = (UINT32*) data;
+	u32  *uint32 = (u32*) data;
 
 	DBFENTER;
 
@@ -2315,349 +765,6 @@ void                         *data)
 }
 
 /*----------------------------------------------------------------
-* prism2mib_operationalrateset
-*
-* Get/set the operational rate set.
-*
-* MIB record parameters:
-*       parm1    Prism2 RID value.
-*       parm2    Not used.
-*       parm3    Not used.
-*
-* Arguments:
-*       mib      MIB record.
-*       isget    MIBGET/MIBSET flag.
-*       wlandev  wlan device structure.
-*       priv     "priv" structure.
-*       hw       "hw" structure.
-*       msg      Message structure.
-*       data     Data buffer.
-*
-* Returns:
-*       0   - Success.
-*       ~0  - Error.
-*
-----------------------------------------------------------------*/
-
-static int prism2mib_operationalrateset(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data)
-{
-	int            result;
-	p80211pstrd_t  *pstr = (p80211pstrd_t *) data;
-	UINT8          bytebuf[MIB_TMP_MAXLEN];
-	UINT16         *wordbuf = (UINT16*) bytebuf;
-
-	DBFENTER;
-
-	if (isget) {
-		result = hfa384x_drvr_getconfig16(hw, mib->parm1, wordbuf);
-		prism2mgmt_get_oprateset(wordbuf, pstr);
-	} else {
-		prism2mgmt_set_oprateset(wordbuf, pstr);
-		result = hfa384x_drvr_setconfig16(hw, mib->parm1, *wordbuf);
-		result = hfa384x_drvr_setconfig16(hw, HFA384x_RID_CNFSUPPRATES, *wordbuf);
-	}
-
-	DBFEXIT;
-	return(result);
-}
-
-/*----------------------------------------------------------------
-* prism2mib_groupaddress
-*
-* Get/set the dot11GroupAddressesTable.
-*
-* MIB record parameters:
-*       parm1    Not used.
-*       parm2    Not used.
-*       parm3    Not used.
-*
-* Arguments:
-*       mib      MIB record.
-*       isget    MIBGET/MIBSET flag.
-*       wlandev  wlan device structure.
-*       priv     "priv" structure.
-*       hw       "hw" structure.
-*       msg      Message structure.
-*       data     Data buffer.
-*
-* Returns:
-*       0   - Success.
-*       ~0  - Error.
-*
-----------------------------------------------------------------*/
-
-static int prism2mib_groupaddress(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data)
-{
-	int            result;
-	p80211pstrd_t  *pstr = (p80211pstrd_t *) data;
-	UINT8          bytebuf[MIB_TMP_MAXLEN];
-	UINT16         len;
-
-	DBFENTER;
-
-	/* TODO: fix this.  f/w doesn't support mcast filters */
-
-	if (isget) {
-		prism2mgmt_get_grpaddr(mib->did, pstr, hw);
-		return(0);
-	}
-
-	result = prism2mgmt_set_grpaddr(mib->did, bytebuf, pstr, hw);
-	if (result != 0) {
-		msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-		return(result);
-	}
-
-	if (hw->dot11_grpcnt <= MAX_PRISM2_GRP_ADDR) {
-		len = hw->dot11_grpcnt * WLAN_ADDR_LEN;
-		memcpy(bytebuf, hw->dot11_grp_addr[0], len);
-		result = hfa384x_drvr_setconfig(hw, HFA384x_RID_GROUPADDR, bytebuf, len);
-
-		/*
-		** Turn off promiscuous mode if count is equal to MAX.  We may
-		** have been at a higher count in promiscuous mode and need to
-		** turn it off.
-		*/
-
-		/* but only if we're not already in promisc mode. :) */
-		if ((hw->dot11_grpcnt == MAX_PRISM2_GRP_ADDR) &&
-		    !( wlandev->netdev->flags & IFF_PROMISC)) {
-			result = hfa384x_drvr_setconfig16(hw,
-					     HFA384x_RID_PROMISCMODE, 0);
-		}
-	} else {
-
-		/*
-		** Clear group addresses in card and set to promiscuous mode.
-		*/
-
-		memset(bytebuf, 0, sizeof(bytebuf));
-		result = hfa384x_drvr_setconfig(hw, HFA384x_RID_GROUPADDR,
-						bytebuf, 0);
-		if (result == 0) {
-			result = hfa384x_drvr_setconfig16(hw,
-					HFA384x_RID_PROMISCMODE, 1);
-		}
-	}
-
-	DBFEXIT;
-	return(result);
-}
-
-/*----------------------------------------------------------------
-* prism2mib_fwid
-*
-* Get the firmware ID.
-*
-* MIB record parameters:
-*       parm1    Not used.
-*       parm2    Not used.
-*       parm3    Not used.
-*
-* Arguments:
-*       mib      MIB record.
-*       isget    MIBGET/MIBSET flag.
-*       wlandev  wlan device structure.
-*       priv     "priv" structure.
-*       hw       "hw" structure.
-*       msg      Message structure.
-*       data     Data buffer.
-*
-* Returns:
-*       0   - Success.
-*       ~0  - Error.
-*
-----------------------------------------------------------------*/
-
-static int prism2mib_fwid(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data)
-{
-	int             result;
-	p80211pstrd_t   *pstr = (p80211pstrd_t *) data;
-	hfa384x_FWID_t  fwid;
-
-	DBFENTER;
-
-	if (isget) {
-		result = hfa384x_drvr_getconfig(hw, HFA384x_RID_FWID,
-						&fwid, HFA384x_RID_FWID_LEN);
-		if (mib->did == DIDmib_p2_p2NIC_p2PrimaryFWID) {
-			fwid.primary[HFA384x_FWID_LEN - 1] = '\0';
-			pstr->len = strlen(fwid.primary);
-			memcpy(pstr->data, fwid.primary, pstr->len);
-		} else {
-			fwid.secondary[HFA384x_FWID_LEN - 1] = '\0';
-			pstr->len = strlen(fwid.secondary);
-			memcpy(pstr->data, fwid.secondary, pstr->len);
-		}
-	} else
-		result = 0;     /* Should never happen. */
-
-	DBFEXIT;
-	return(result);
-}
-
-/*----------------------------------------------------------------
-* prism2mib_authalg
-*
-* Get values from the AuhtenticationAlgorithmsTable.
-*
-* MIB record parameters:
-*       parm1    Table index (1-6).
-*       parm2    Not used.
-*       parm3    Not used.
-*
-* Arguments:
-*       mib      MIB record.
-*       isget    MIBGET/MIBSET flag.
-*       wlandev  wlan device structure.
-*       priv     "priv" structure.
-*       hw       "hw" structure.
-*       msg      Message structure.
-*       data     Data buffer.
-*
-* Returns:
-*       0   - Success.
-*       ~0  - Error.
-*
-----------------------------------------------------------------*/
-
-static int prism2mib_authalg(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data)
-{
-	UINT32  *uint32 = (UINT32*) data;
-
-	DBFENTER;
-
-	/* MSM: pkx supplied code that  code queries RID FD4D....but the f/w's
-         *  results are bogus. Therefore, we have to simulate the appropriate
-         *  results here in the driver based on our knowledge of existing MAC
-         *  features.  That's the whole point behind this ugly function.
-         */
-
-	if (isget) {
-		msg->resultcode.data = P80211ENUM_resultcode_success;
-		switch (mib->parm1) {
-			case 1: /* Open System */
-				*uint32 = P80211ENUM_authalg_opensystem;
-				break;
-			case 2: /* SharedKey */
-				*uint32 = P80211ENUM_authalg_sharedkey;
-				break;
-			default:
-				*uint32 = 0;
-				msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-				break;
-		}
-	}
-
-	DBFEXIT;
-	return(0);
-}
-
-/*----------------------------------------------------------------
-* prism2mib_authalgenable
-*
-* Get/set the enable values from the AuhtenticationAlgorithmsTable.
-*
-* MIB record parameters:
-*       parm1    Table index (1-6).
-*       parm2    Not used.
-*       parm3    Not used.
-*
-* Arguments:
-*       mib      MIB record.
-*       isget    MIBGET/MIBSET flag.
-*       wlandev  wlan device structure.
-*       priv     "priv" structure.
-*       hw       "hw" structure.
-*       msg      Message structure.
-*       data     Data buffer.
-*
-* Returns:
-*       0   - Success.
-*       ~0  - Error.
-*
-----------------------------------------------------------------*/
-
-static int prism2mib_authalgenable(
-mibrec_t                     *mib,
-int                          isget,
-wlandevice_t                 *wlandev,
-hfa384x_t                    *hw,
-p80211msg_dot11req_mibset_t  *msg,
-void                         *data)
-{
-	int     result;
-	UINT32  *uint32 = (UINT32*) data;
-
-	int     index;
-	UINT16  cnf_auth;
-	UINT16	mask;
-
-	DBFENTER;
-
-	index = mib->parm1 - 1;
-
-	result = hfa384x_drvr_getconfig16( hw,
-			HFA384x_RID_CNFAUTHENTICATION, &cnf_auth);
-	WLAN_LOG_DEBUG(2,"cnfAuthentication0=%d, index=%d\n", cnf_auth, index);
-
-	if (isget) {
-		if ( index == 0 || index == 1 ) {
-			*uint32 = (cnf_auth & (1<<index)) ?
-				P80211ENUM_truth_true: P80211ENUM_truth_false;
-		} else {
-			*uint32 = P80211ENUM_truth_false;
-			msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-		}
-	} else {
-		if ( index == 0 || index == 1 ) {
-			mask = 1 << index;
-			if (*uint32==P80211ENUM_truth_true ) {
-				cnf_auth |= mask;
-			} else {
-				cnf_auth &= ~mask;
-			}
-			result = hfa384x_drvr_setconfig16( hw,
-					HFA384x_RID_CNFAUTHENTICATION, cnf_auth);
-			WLAN_LOG_DEBUG(2,"cnfAuthentication:=%d\n", cnf_auth);
-			if ( result ) {
-				WLAN_LOG_DEBUG(1,"Unable to set p2cnfAuthentication to %d\n", cnf_auth);
-				msg->resultcode.data = P80211ENUM_resultcode_implementation_failure;
-			}
-		} else {
-			msg->resultcode.data = P80211ENUM_resultcode_not_supported;
-		}
-	}
-
-	DBFEXIT;
-	return(result);
-}
-
-/*----------------------------------------------------------------
 * prism2mib_priv
 *
 * Get/set values in the "priv" data structure.
@@ -2690,218 +797,18 @@ hfa384x_t                    *hw,
 p80211msg_dot11req_mibset_t  *msg,
 void                         *data)
 {
-	UINT32            *uint32 = (UINT32*) data;
 	p80211pstrd_t     *pstr = (p80211pstrd_t*) data;
-	p80211macarray_t  *macarray = (p80211macarray_t *) data;
-
-	int  i, cnt, result, done;
-
-	prism2sta_authlist_t  old;
-
-	/*
-	** "test" is a lot longer than necessary but who cares?  ...as long as
-	** it is long enough!
-	*/
 
-	UINT8  test[sizeof(wlandev->rx) + sizeof(hw->tallies)];
+	int  result;
 
 	DBFENTER;
 
 	switch (mib->did) {
-	case DIDmib_p2_p2Table_p2ReceivedFrameStatistics:
-
-		/*
-		** Note: The values in this record are changed by the
-		** interrupt handler and therefore cannot be guaranteed
-		** to be stable while they are being copied.  However,
-		** the interrupt handler will take priority over this
-		** code.  Hence, if the same values are copied twice,
-		** then we are ensured that the values have not been
-		** changed.  If they have, then just try again.  Don't
-		** try more than 10 times...if we still haven't got it,
-		** then the values we do have are probably good enough.
-		** This scheme for copying values is used in order to
-		** prevent having to block the interrupt handler while
-		** we copy the values.
-		*/
-
-		if (isget)
-			for (i = 0; i < 10; i++) {
-				memcpy(data, &wlandev->rx, sizeof(wlandev->rx));
-				memcpy(test, &wlandev->rx, sizeof(wlandev->rx));
-				if (memcmp(data, test, sizeof(wlandev->rx)) == 0) break;
-			}
-
-		break;
-
-	case DIDmib_p2_p2Table_p2CommunicationTallies:
-
-		/*
-		** Note: The values in this record are changed by the
-		** interrupt handler and therefore cannot be guaranteed
-		** to be stable while they are being copied.  See the
-		** note above about copying values.
-		*/
-
-		if (isget) {
-			result = hfa384x_drvr_commtallies(hw);
-
-			/* ?????? We need to wait a bit here for the */
-			/*   tallies to get updated. ?????? */
-			/* MSM: TODO: The right way to do this is to
-			 *      add a "commtallie" wait queue to the
-			 *      priv structure that gets run every time
-			 *      we receive a commtally info frame.
-			 *      This process would sleep on that
-			 *      queue and get awakened when the
-			 *      the requested info frame arrives.
-			 *      Don't have time to do and test this
-			 *      right now.
-			 */
-
-			/* Ugh, this is nasty. */
-			for (i = 0; i < 10; i++) {
-				memcpy(data,
-				       &hw->tallies,
-				       sizeof(hw->tallies));
-				memcpy(test,
-				       &hw->tallies,
-				       sizeof(hw->tallies));
-				if ( memcmp(data,
-					    test,
-					    sizeof(hw->tallies)) == 0)
-					break;
-			}
-		}
-
-		break;
-
-	case DIDmib_p2_p2Table_p2Authenticated:
-
-		if (isget) {
-			prism2mib_priv_authlist(hw, &old);
-
-			macarray->cnt = 0;
-			for (i = 0; i < old.cnt; i++) {
-				if (!old.assoc[i]) {
-					memcpy(macarray->data[macarray->cnt], old.addr[i], WLAN_ADDR_LEN);
-					macarray->cnt++;
-				}
-			}
-		}
-
-		break;
-
-	case DIDmib_p2_p2Table_p2Associated:
-
-		if (isget) {
-			prism2mib_priv_authlist(hw, &old);
-
-			macarray->cnt = 0;
-			for (i = 0; i < old.cnt; i++) {
-				if (old.assoc[i]) {
-					memcpy(macarray->data[macarray->cnt], old.addr[i], WLAN_ADDR_LEN);
-					macarray->cnt++;
-				}
-			}
-		}
-
-		break;
-
-	case DIDmib_p2_p2Table_p2PowerSaveUserCount:
-
-		if (isget)
-			*uint32 = hw->psusercount;
-
-		break;
-
-	case DIDmib_p2_p2Table_p2Comment:
-
-		if (isget) {
-			pstr->len = strlen(hw->comment);
-			memcpy(pstr->data, hw->comment, pstr->len);
-		} else {
-			cnt = pstr->len;
-			if (cnt < 0) cnt = 0;
-			if (cnt >= sizeof(hw->comment))
-				cnt = sizeof(hw->comment)-1;
-			memcpy(hw->comment, pstr->data, cnt);
-			pstr->data[cnt] = '\0';
-		}
-
-		break;
-
-	case DIDmib_p2_p2Table_p2AccessMode:
-
-		if (isget)
-			*uint32 = hw->accessmode;
-		else
-			prism2mib_priv_accessmode(hw, *uint32);
-
-		break;
-
-	case DIDmib_p2_p2Table_p2AccessAllow:
-
-		if (isget) {
-			macarray->cnt = hw->allow.cnt;
-			memcpy(macarray->data, hw->allow.addr,
-			       macarray->cnt*WLAN_ADDR_LEN);
-		} else {
-			prism2mib_priv_accessallow(hw, macarray);
-		}
-
-		break;
-
-	case DIDmib_p2_p2Table_p2AccessDeny:
-
-		if (isget) {
-			macarray->cnt = hw->deny.cnt;
-			memcpy(macarray->data, hw->deny.addr,
-			       macarray->cnt*WLAN_ADDR_LEN);
-		} else {
-			prism2mib_priv_accessdeny(hw, macarray);
-		}
-
-		break;
-
-	case DIDmib_p2_p2Table_p2ChannelInfoResults:
-
-		if (isget) {
-			done = atomic_read(&hw->channel_info.done);
-			if (done == 0) {
-				msg->resultcode.status = P80211ENUM_msgitem_status_no_value;
-				break;
-			}
-			if (done == 1) {
-				msg->resultcode.status = P80211ENUM_msgitem_status_incomplete_itemdata;
-				break;
-			}
-
-			for (i = 0; i < 14; i++, uint32 += 5) {
-				uint32[0] = i+1;
-				uint32[1] = hw->channel_info.results.result[i].anl;
-				uint32[2] = hw->channel_info.results.result[i].pnl;
-				uint32[3] = (hw->channel_info.results.result[i].active & HFA384x_CHINFORESULT_BSSACTIVE) ? 1 : 0;
-				uint32[4] = (hw->channel_info.results.result[i].active & HFA384x_CHINFORESULT_PCFACTIVE) ? 1 : 0;
-			}
-		}
-
-		break;
-
-	case DIDmib_dot11smt_dot11StationConfigTable_dot11DesiredBSSType:
-
-		if (isget)
-			*uint32 = hw->dot11_desired_bss_type;
-		else
-			hw->dot11_desired_bss_type = *uint32;
-
-		break;
-
 	case DIDmib_lnx_lnxConfigTable_lnxRSNAIE: {
 		hfa384x_WPAData_t wpa;
 		if (isget) {
 			hfa384x_drvr_getconfig( hw, HFA384x_RID_CNFWPADATA,
-						(UINT8 *) &wpa, sizeof(wpa));
+						(u8 *) &wpa, sizeof(wpa));
 			pstr->len = hfa384x2host_16(wpa.datalen);
 			memcpy(pstr->data, wpa.data, pstr->len);
 		} else {
@@ -2909,7 +816,7 @@ void                         *data)
 			memcpy(wpa.data, pstr->data, pstr->len);
 
 			result = hfa384x_drvr_setconfig(hw, HFA384x_RID_CNFWPADATA,
-				(UINT8 *) &wpa, sizeof(wpa));
+				(u8 *) &wpa, sizeof(wpa));
 		}
 		break;
 	}
@@ -2922,345 +829,6 @@ void                         *data)
 }
 
 /*----------------------------------------------------------------
-* prism2mib_priv_authlist
-*
-* Get a copy of the list of authenticated stations.
-*
-* Arguments:
-*       priv     "priv" structure.
-*       list     List of authenticated stations.
-*
-* Returns:
-*	Nothing
-*
-----------------------------------------------------------------*/
-
-static void prism2mib_priv_authlist(
-hfa384x_t             *hw,
-prism2sta_authlist_t  *list)
-{
-	prism2sta_authlist_t  test;
-	int                   i;
-
-	DBFENTER;
-
-	/*
-	** Note: The values in this record are changed by the interrupt
-	** handler and therefore cannot be guaranteed to be stable while
-	** they are being copied.  However, the interrupt handler will
-	** take priority over this code.  Hence, if the same values are
-	** copied twice, then we are ensured that the values have not
-	** been changed.  If they have, then just try again.  Don't try
-	** more than 10 times...the list of authenticated stations is
-	** unlikely to be changing frequently enough that we can't get
-	** a snapshot in 10 tries.  Don't try more than this so that we
-	** don't risk locking-up for long periods of time.  If we still
-	** haven't got the snapshot, then generate an error message and
-	** return an empty list (since this is the only valid list that
-	** we can guarentee).  This scheme for copying values is used in
-	** order to prevent having to block the interrupt handler while
-	** we copy the values.
-	*/
-
-	for (i = 0; i < 10; i++) {
-		memcpy(list, &hw->authlist, sizeof(prism2sta_authlist_t));
-		memcpy(&test, &hw->authlist, sizeof(prism2sta_authlist_t));
-		if (memcmp(list, &test, sizeof(prism2sta_authlist_t)) == 0)
-			break;
-	}
-
-	if (i >= 10) {
-		list->cnt = 0;
-		WLAN_LOG_ERROR("Could not obtain snapshot of authenticated stations.\n");
-		}
-
-	DBFEXIT;
-	return;
-}
-
-/*----------------------------------------------------------------
-* prism2mib_priv_accessmode
-*
-* Set the Access Mode.
-*
-* Arguments:
-*       priv     "priv" structure.
-*       hw       "hw" structure.
-*       mode     New access mode.
-*
-* Returns:
-*	Nothing
-*
-----------------------------------------------------------------*/
-
-static void prism2mib_priv_accessmode(
-hfa384x_t         *hw,
-UINT32            mode)
-{
-	prism2sta_authlist_t  old;
-	int                   i, j, deauth;
-	UINT8                 *addr;
-
-	DBFENTER;
-
-	/*
-	** If the mode is not changing or it is changing to "All", then it's
-	** okay to go ahead without a lot of messing around.  Otherwise, the
-	** access mode is changing in a way that may leave some stations
-	** authenticated which should not be authenticated.  It will be
-	** necessary to de-authenticate these stations.
-	*/
-
-	if (mode == WLAN_ACCESS_ALL || mode == hw->accessmode) {
-		hw->accessmode = mode;
-		return;
-	}
-
-	/*
-	** Switch to the new access mode.  Once this is done, then the interrupt
-	** handler (which uses this value) will be prevented from authenticating
-	** ADDITIONAL stations which should not be authenticated.  Then get a
-	** copy of the current list of authenticated stations.
-	*/
-
-	hw->accessmode = mode;
-
-	prism2mib_priv_authlist(hw, &old);
-
-	/*
-	** Now go through the list of previously authenticated stations (some
-	** of which might de-authenticate themselves while we are processing it
-	** but that is okay).  Any station which no longer matches the access
-	** mode, must be de-authenticated.
-	*/
-
-	for (i = 0; i < old.cnt; i++) {
-		addr = old.addr[i];
-
-		if (mode == WLAN_ACCESS_NONE)
-			deauth = 1;
-		else {
-			if (mode == WLAN_ACCESS_ALLOW) {
-				for (j = 0; j < hw->allow.cnt; j++)
-					if (memcmp(addr, hw->allow.addr[j],
-							WLAN_ADDR_LEN) == 0)
-						break;
-				deauth = (j >= hw->allow.cnt);
-			} else {
-				for (j = 0; j < hw->deny.cnt; j++)
-					if (memcmp(addr, hw->deny.addr[j],
-							WLAN_ADDR_LEN) == 0)
-						break;
-				deauth = (j < hw->deny.cnt);
-			}
-		}
-
-		if (deauth) prism2mib_priv_deauthenticate(hw, addr);
-	}
-
-	DBFEXIT;
-	return;
-}
-
-/*----------------------------------------------------------------
-* prism2mib_priv_accessallow
-*
-* Change the list of allowed MAC addresses.
-*
-* Arguments:
-*       priv      "priv" structure.
-*       hw        "hw" structure.
-*       macarray  New array of MAC addresses.
-*
-* Returns:
-*	Nothing
-*
-----------------------------------------------------------------*/
-
-static void prism2mib_priv_accessallow(
-hfa384x_t         *hw,
-p80211macarray_t  *macarray)
-{
-	prism2sta_authlist_t  old;
-	int                   i, j;
-
-	DBFENTER;
-
-	/*
-	** Change the access list.  Note that the interrupt handler may be in
-	** the middle of using the access list!!!  Since the interrupt handler
-	** will	always have priority over this process and this is the only
-	** process that will modify the list, this problem can be handled as
-	** follows:
-	**
-	**    1. Set the "modify" flag.
-	**    2. Change the first copy of the list.
-	**    3. Clear the "modify" flag.
-	**    4. Change the backup copy of the list.
-	**
-	** The interrupt handler will check the "modify" flag.  If NOT set, then
-	** the first copy of the list is valid and may be used.  Otherwise, the
-	** first copy is being changed but the backup copy is valid and may be
-	** used.  Doing things this way prevents having to have the interrupt
-	** handler block while the list is being updated.
-	*/
-
-	hw->allow.modify = 1;
-
-	hw->allow.cnt = macarray->cnt;
-	memcpy(hw->allow.addr, macarray->data, macarray->cnt*WLAN_ADDR_LEN);
-
-	hw->allow.modify = 0;
-
-	hw->allow.cnt1 = macarray->cnt;
-	memcpy(hw->allow.addr1, macarray->data, macarray->cnt*WLAN_ADDR_LEN);
-
-	/*
-	** If the current access mode is "Allow", then changing the access
-	** list may leave some stations authenticated which should not be
-	** authenticated.  It will be necessary to de-authenticate these
-	** stations.  Otherwise, the list can be changed without a lot of fuss.
-	*/
-
-	if (hw->accessmode == WLAN_ACCESS_ALLOW) {
-
-		/*
-		** Go through the list of authenticated stations (some of
-		** which might de-authenticate themselves while we are
-		** processing it but that is okay).  Any station which is
-		** no longer in the list of allowed stations, must be
-		** de-authenticated.
-		*/
-
-		prism2mib_priv_authlist(hw, &old);
-
-		for (i = 0; i < old.cnt; i++) {
-			for (j = 0; j < hw->allow.cnt; j++)
-				if (memcmp(old.addr[i], hw->allow.addr[j],
-							WLAN_ADDR_LEN) == 0)
-					break;
-			if (j >= hw->allow.cnt)
-				prism2mib_priv_deauthenticate(hw, old.addr[i]);
-		}
-	}
-
-	DBFEXIT;
-	return;
-}
-
-/*----------------------------------------------------------------
-* prism2mib_priv_accessdeny
-*
-* Change the list of denied MAC addresses.
-*
-* Arguments:
-*       priv      "priv" structure.
-*       hw        "hw" structure.
-*       macarray  New array of MAC addresses.
-*
-* Returns:
-*	Nothing
-*
-----------------------------------------------------------------*/
-
-static void prism2mib_priv_accessdeny(
-hfa384x_t         *hw,
-p80211macarray_t  *macarray)
-{
-	prism2sta_authlist_t  old;
-	int                   i, j;
-
-	DBFENTER;
-
-	/*
-	** Change the access list.  Note that the interrupt handler may be in
-	** the middle of using the access list!!!  Since the interrupt handler
-	** will always have priority over this process and this is the only
-	** process that will modify the list, this problem can be handled as
-	** follows:
-	**
-	**    1. Set the "modify" flag.
-	**    2. Change the first copy of the list.
-	**    3. Clear the "modify" flag.
-	**    4. Change the backup copy of the list.
-	**
-	** The interrupt handler will check the "modify" flag.  If NOT set, then
-	** the first copy of the list is valid and may be used.  Otherwise, the
-	** first copy is being changed but the backup copy is valid and may be
-	** used.  Doing things this way prevents having to have the interrupt
-	** handler block while the list is being updated.
-	*/
-
-	hw->deny.modify = 1;
-
-	hw->deny.cnt = macarray->cnt;
-	memcpy(hw->deny.addr, macarray->data, macarray->cnt*WLAN_ADDR_LEN);
-
-	hw->deny.modify = 0;
-
-	hw->deny.cnt1 = macarray->cnt;
-	memcpy(hw->deny.addr1, macarray->data, macarray->cnt*WLAN_ADDR_LEN);
-
-	/*
-	** If the current access mode is "Deny", then changing the access
-	** list may leave some stations authenticated which should not be
-	** authenticated.  It will be necessary to de-authenticate these
-	** stations.  Otherwise, the list can be changed without a lot of fuss.
-	*/
-
-	if (hw->accessmode == WLAN_ACCESS_DENY) {
-
-		/*
-		** Go through the list of authenticated stations (some of
-		** which might de-authenticate themselves while we are
-		** processing it but that is okay).  Any station which is
-		** now in the list of denied stations, must be de-authenticated.
-		*/
-
-		prism2mib_priv_authlist(hw, &old);
-
-		for (i = 0; i < old.cnt; i++)
-			for (j = 0; j < hw->deny.cnt; j++)
-				if (memcmp(old.addr[i], hw->deny.addr[j],
-							 WLAN_ADDR_LEN) == 0) {
-					prism2mib_priv_deauthenticate(hw, old.addr[i]);
-					break;
-				}
-	}
-
-	DBFEXIT;
-	return;
-}
-
-/*----------------------------------------------------------------
-* prism2mib_priv_deauthenticate
-*
-* De-authenticate a station.  This is done by sending a HandoverAddress
-* information frame to the firmware.  This should work, according to
-* Intersil.
-*
-* Arguments:
-*       priv     "priv" structure.
-*       hw       "hw" structure.
-*       addr     MAC address of station to be de-authenticated.
-*
-* Returns:
-*	Nothing
-*
-----------------------------------------------------------------*/
-
-static void prism2mib_priv_deauthenticate(
-hfa384x_t         *hw,
-UINT8             *addr)
-{
-	DBFENTER;
-	hfa384x_drvr_handover(hw, addr);
-	DBFEXIT;
-	return;
-}
-
-
-/*----------------------------------------------------------------
 * prism2mgmt_pstr2bytestr
 *
 * Convert the pstr data in the WLAN message structure into an hfa384x
@@ -3279,7 +847,7 @@ void prism2mgmt_pstr2bytestr(hfa384x_bytestr_t *bytestr, p80211pstrd_t *pstr)
 {
 	DBFENTER;
 
-	bytestr->len = host2hfa384x_16((UINT16)(pstr->len));
+	bytestr->len = host2hfa384x_16((u16)(pstr->len));
 	memcpy(bytestr->data, pstr->data, pstr->len);
 	DBFEXIT;
 }
@@ -3300,7 +868,7 @@ void prism2mgmt_pstr2bytestr(hfa384x_bytestr_t *bytestr, p80211pstrd_t *pstr)
 *
 ----------------------------------------------------------------*/
 
-void prism2mgmt_pstr2bytearea(UINT8 *bytearea, p80211pstrd_t *pstr)
+void prism2mgmt_pstr2bytearea(u8 *bytearea, p80211pstrd_t *pstr)
 {
 	DBFENTER;
 
@@ -3328,7 +896,7 @@ void prism2mgmt_bytestr2pstr(hfa384x_bytestr_t *bytestr, p80211pstrd_t *pstr)
 {
 	DBFENTER;
 
-	pstr->len = (UINT8)(hfa384x2host_16((UINT16)(bytestr->len)));
+	pstr->len = (u8)(hfa384x2host_16((u16)(bytestr->len)));
 	memcpy(pstr->data, bytestr->data, pstr->len);
 	DBFEXIT;
 }
@@ -3349,11 +917,11 @@ void prism2mgmt_bytestr2pstr(hfa384x_bytestr_t *bytestr, p80211pstrd_t *pstr)
 *
 ----------------------------------------------------------------*/
 
-void prism2mgmt_bytearea2pstr(UINT8 *bytearea, p80211pstrd_t *pstr, int len)
+void prism2mgmt_bytearea2pstr(u8 *bytearea, p80211pstrd_t *pstr, int len)
 {
 	DBFENTER;
 
-	pstr->len = (UINT8)len;
+	pstr->len = (u8)len;
 	memcpy(pstr->data, bytearea, len);
 	DBFEXIT;
 }
@@ -3373,11 +941,11 @@ void prism2mgmt_bytearea2pstr(UINT8 *bytearea, p80211pstrd_t *pstr, int len)
 *
 ----------------------------------------------------------------*/
 
-void prism2mgmt_prism2int2p80211int(UINT16 *prism2int, UINT32 *wlanint)
+void prism2mgmt_prism2int2p80211int(u16 *prism2int, u32 *wlanint)
 {
 	DBFENTER;
 
-	*wlanint = (UINT32)hfa384x2host_16(*prism2int);
+	*wlanint = (u32)hfa384x2host_16(*prism2int);
 	DBFEXIT;
 }
 
@@ -3396,11 +964,11 @@ void prism2mgmt_prism2int2p80211int(UINT16 *prism2int, UINT32 *wlanint)
 *
 ----------------------------------------------------------------*/
 
-void prism2mgmt_p80211int2prism2int(UINT16 *prism2int, UINT32 *wlanint)
+void prism2mgmt_p80211int2prism2int(u16 *prism2int, u32 *wlanint)
 {
 	DBFENTER;
 
-	*prism2int = host2hfa384x_16((UINT16)(*wlanint));
+	*prism2int = host2hfa384x_16((u16)(*wlanint));
 	DBFEXIT;
 }
 
@@ -3419,7 +987,7 @@ void prism2mgmt_p80211int2prism2int(UINT16 *prism2int, UINT32 *wlanint)
 *	Nothing
 *
 ----------------------------------------------------------------*/
-void prism2mgmt_prism2enum2p80211enum(UINT16 *prism2enum, UINT32 *wlanenum, UINT16 rid)
+void prism2mgmt_prism2enum2p80211enum(u16 *prism2enum, u32 *wlanenum, u16 rid)
 {
 	DBFENTER;
 
@@ -3445,7 +1013,7 @@ void prism2mgmt_prism2enum2p80211enum(UINT16 *prism2enum, UINT32 *wlanenum, UINT
 *	Nothing
 *
 ----------------------------------------------------------------*/
-void prism2mgmt_p80211enum2prism2enum(UINT16 *prism2enum, UINT32 *wlanenum, UINT16 rid)
+void prism2mgmt_p80211enum2prism2enum(u16 *prism2enum, u32 *wlanenum, u16 rid)
 {
 	DBFENTER;
 
@@ -3471,10 +1039,10 @@ void prism2mgmt_p80211enum2prism2enum(UINT16 *prism2enum, UINT32 *wlanenum, UINT
 *	Nothing
 *
 ----------------------------------------------------------------*/
-void prism2mgmt_get_oprateset(UINT16 *rate, p80211pstrd_t *pstr)
+void prism2mgmt_get_oprateset(u16 *rate, p80211pstrd_t *pstr)
 {
-	UINT8	len;
-	UINT8	*datarate;
+	u8	len;
+	u8	*datarate;
 
 	DBFENTER;
 
@@ -3483,29 +1051,29 @@ void prism2mgmt_get_oprateset(UINT16 *rate, p80211pstrd_t *pstr)
 
  	/* 1 Mbps */
 	if ( BIT0 & (*rate) ) {
-		len += (UINT8)1;
-		*datarate = (UINT8)2;
+		len += (u8)1;
+		*datarate = (u8)2;
 		datarate++;
 	}
 
  	/* 2 Mbps */
 	if ( BIT1 & (*rate) ) {
-		len += (UINT8)1;
-		*datarate = (UINT8)4;
+		len += (u8)1;
+		*datarate = (u8)4;
 		datarate++;
 	}
 
  	/* 5.5 Mbps */
 	if ( BIT2 & (*rate) ) {
-		len += (UINT8)1;
-		*datarate = (UINT8)11;
+		len += (u8)1;
+		*datarate = (u8)11;
 		datarate++;
 	}
 
  	/* 11 Mbps */
 	if ( BIT3 & (*rate) ) {
-		len += (UINT8)1;
-		*datarate = (UINT8)22;
+		len += (u8)1;
+		*datarate = (u8)22;
 		datarate++;
 	}
 
@@ -3530,9 +1098,9 @@ void prism2mgmt_get_oprateset(UINT16 *rate, p80211pstrd_t *pstr)
 *	Nothing
 *
 ----------------------------------------------------------------*/
-void prism2mgmt_set_oprateset(UINT16 *rate, p80211pstrd_t *pstr)
+void prism2mgmt_set_oprateset(u16 *rate, p80211pstrd_t *pstr)
 {
-	UINT8	*datarate;
+	u8	*datarate;
 	int	i;
 
 	DBFENTER;
@@ -3565,233 +1133,3 @@ void prism2mgmt_set_oprateset(UINT16 *rate, p80211pstrd_t *pstr)
 	DBFEXIT;
 	return;
 }
-
-
-
-/*----------------------------------------------------------------
-* prism2mgmt_get_grpaddr
-*
-* Retrieves a particular group address from the list of
-* group addresses.
-*
-* Arguments:
-*	did		mibitem did
-*	pstr		wlan octet string
-*	priv		prism2 driver private data structure
-*
-* Returns:
-*	Nothing
-*
-----------------------------------------------------------------*/
-void prism2mgmt_get_grpaddr(UINT32 did, p80211pstrd_t *pstr,
-	hfa384x_t *hw )
-{
-	int	index;
-
-	DBFENTER;
-
-	index = prism2mgmt_get_grpaddr_index(did);
-
-	if ( index >= 0 ) {
-		pstr->len = WLAN_ADDR_LEN;
-		memcpy(pstr->data, hw->dot11_grp_addr[index],
-			WLAN_ADDR_LEN);
-	}
-
-	DBFEXIT;
-	return;
-}
-
-
-
-/*----------------------------------------------------------------
-* prism2mgmt_set_grpaddr
-*
-* Convert the wlan octet string into an hfa384x bit area.
-*
-* Arguments:
-*	did		mibitem did
-*	buf
-*	groups
-*
-* Returns:
-*	0	Success
-*	!0	Error
-*
-----------------------------------------------------------------*/
-int prism2mgmt_set_grpaddr(UINT32 did, UINT8 *prism2buf,
-	p80211pstrd_t *pstr, hfa384x_t *hw )
-{
-	UINT8	no_addr[WLAN_ADDR_LEN];
-	int	index;
-
-	DBFENTER;
-
-	memset(no_addr, 0, WLAN_ADDR_LEN);
-	if (memcmp(no_addr, pstr->data, WLAN_ADDR_LEN) != 0) {
-
-		/*
-		** The address is NOT 0 so we are "adding" an address to the
-		** group address list.  Check to make sure we aren't trying
-		** to add more than the maximum allowed number of group
-		** addresses in the list.  The new address is added to the
-		** end of the list regardless of the DID used to add the
-		** address.
-		*/
-
-		if (hw->dot11_grpcnt >= MAX_GRP_ADDR) return(-1);
-
-		memcpy(hw->dot11_grp_addr[hw->dot11_grpcnt], pstr->data,
-								 WLAN_ADDR_LEN);
-		hw->dot11_grpcnt += 1;
-	} else {
-
-		/*
-		** The address is 0.  Interpret this as "deleting" an address
-		** from the group address list.  Get the address index from
-		** the DID.  If this is within the range of used addresses,
-		** then delete the specified address by shifting all following
-		** addresses down.  Then clear the last address (which should
-		** now be unused).  If the address index is NOT within the
-		** range of used addresses, then just ignore the address.
-		*/
-
-		index = prism2mgmt_get_grpaddr_index(did);
-		if (index >= 0 && index < hw->dot11_grpcnt) {
-			hw->dot11_grpcnt -= 1;
-			memmove(hw->dot11_grp_addr[index],
-				hw->dot11_grp_addr[index + 1],
-				((hw->dot11_grpcnt)-index) * WLAN_ADDR_LEN);
-			memset(hw->dot11_grp_addr[hw->dot11_grpcnt], 0,
-								 WLAN_ADDR_LEN);
-		}
-	}
-
-	DBFEXIT;
-	return(0);
-}
-
-
-/*----------------------------------------------------------------
-* prism2mgmt_get_grpaddr_index
-*
-* Gets the index in the group address list based on the did.
-*
-* Arguments:
-*	did		mibitem did
-*
-* Returns:
-*	>= 0	If valid did
-*	< 0	If not valid did
-*
-----------------------------------------------------------------*/
-int prism2mgmt_get_grpaddr_index( UINT32 did )
-{
-	int	index;
-
-	DBFENTER;
-
-	index = -1;
-
-	switch (did) {
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address1:
-		index = 0;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address2:
-		index = 1;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address3:
-		index = 2;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address4:
-		index = 3;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address5:
-		index = 4;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address6:
-		index = 5;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address7:
-		index = 6;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address8:
-		index = 7;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address9:
-		index = 8;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address10:
-		index = 9;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address11:
-		index = 10;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address12:
-		index = 11;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address13:
-		index = 12;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address14:
-		index = 13;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address15:
-		index = 14;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address16:
-		index = 15;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address17:
-		index = 16;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address18:
-		index = 17;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address19:
-		index = 18;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address20:
-		index = 19;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address21:
-		index = 20;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address22:
-		index = 21;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address23:
-		index = 22;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address24:
-		index = 23;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address25:
-		index = 24;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address26:
-		index = 25;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address27:
-		index = 26;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address28:
-		index = 27;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address29:
-		index = 28;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address30:
-		index = 29;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address31:
-		index = 30;
-		break;
-	case DIDmib_dot11mac_dot11GroupAddressesTable_dot11Address32:
-		index = 31;
-		break;
-	}
-
-	DBFEXIT;
-	return index;
-}
diff --git a/drivers/staging/wlan-ng/prism2sta.c b/drivers/staging/wlan-ng/prism2sta.c
index 18aa15f9e41..b279c97cbc0 100644
--- a/drivers/staging/wlan-ng/prism2sta.c
+++ b/drivers/staging/wlan-ng/prism2sta.c
@@ -54,16 +54,9 @@
 /* System Includes */
 #define WLAN_DBVAR	prism2_debug
 
-#include "version.h"
-
-
 #include <linux/version.h>
-
 #include <linux/module.h>
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,4,25))
 #include <linux/moduleparam.h>
-#endif
-
 #include <linux/kernel.h>
 #include <linux/sched.h>
 #include <linux/types.h>
@@ -71,34 +64,15 @@
 #include <linux/slab.h>
 #include <linux/wireless.h>
 #include <linux/netdevice.h>
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0))
-#include <linux/tqueue.h>
-#else
 #include <linux/workqueue.h>
-#endif
 
 #include <asm/io.h>
 #include <linux/delay.h>
 #include <asm/byteorder.h>
 #include <linux/if_arp.h>
 
-#if (WLAN_HOSTIF == WLAN_PCMCIA)
-#include <pcmcia/version.h>
-#include <pcmcia/cs_types.h>
-#include <pcmcia/cs.h>
-#include <pcmcia/cistpl.h>
-#include <pcmcia/ds.h>
-#include <pcmcia/cisreg.h>
-#endif
-
 #include "wlan_compat.h"
 
-#if ((WLAN_HOSTIF == WLAN_PLX) || (WLAN_HOSTIF == WLAN_PCI))
-#include <linux/ioport.h>
-#include <linux/pci.h>
-#endif
-
 /*================================================================*/
 /* Project Includes */
 
@@ -126,34 +100,7 @@
 /*================================================================*/
 /* Local Static Definitions */
 
-#if (WLAN_HOSTIF == WLAN_PCMCIA)
-#define DRIVER_SUFFIX	"_cs"
-#elif (WLAN_HOSTIF == WLAN_PLX)
-#define DRIVER_SUFFIX	"_plx"
-typedef char* dev_info_t;
-#elif (WLAN_HOSTIF == WLAN_PCI)
-#define DRIVER_SUFFIX	"_pci"
-typedef char* dev_info_t;
-#elif (WLAN_HOSTIF == WLAN_USB)
-#define DRIVER_SUFFIX	"_usb"
-typedef char* dev_info_t;
-#else
-#error "HOSTIF unsupported or undefined!"
-#endif
-
-static char		*version = "prism2" DRIVER_SUFFIX ".o: " WLAN_RELEASE;
-static dev_info_t	dev_info = "prism2" DRIVER_SUFFIX;
-
-#if (WLAN_HOSTIF == WLAN_PLX || WLAN_HOSTIF == WLAN_PCI)
-#ifdef CONFIG_PM
-static int prism2sta_suspend_pci(struct pci_dev *pdev, pm_message_t state);
-static int prism2sta_resume_pci(struct pci_dev *pdev);
-#endif
-#endif
-
-#if (WLAN_HOSTIF == WLAN_PCI)
-
-#endif /* WLAN_PCI */
+static char *dev_info = "prism2_usb";
 
 static wlandevice_t *create_wlan(void);
 
@@ -163,16 +110,7 @@ static wlandevice_t *create_wlan(void);
 int      prism2_reset_holdtime=30;	/* Reset hold time in ms */
 int	 prism2_reset_settletime=100;	/* Reset settle time in ms */
 
-#if (WLAN_HOSTIF == WLAN_USB)
 static int	prism2_doreset=0;		/* Do a reset at init? */
-#else
-static int      prism2_doreset=1;		/* Do a reset at init? */
-int             prism2_bap_timeout=1000;        /* BAP timeout */
-int		prism2_irq_evread_max=20;	/* Maximum number of
-						 * ev_reads (loops)
-						 * in irq handler
-						 */
-#endif
 
 #ifdef WLAN_INCLUDE_DEBUG
 int prism2_debug=0;
@@ -188,13 +126,6 @@ MODULE_PARM_DESC( prism2_reset_holdtime, "reset hold time in ms");
 module_param( prism2_reset_settletime, int, 0644);
 MODULE_PARM_DESC( prism2_reset_settletime, "reset settle time in ms");
 
-#if (WLAN_HOSTIF != WLAN_USB)
-module_param( prism2_bap_timeout, int, 0644);
-MODULE_PARM_DESC(prism2_bap_timeout, "BufferAccessPath Timeout in 10*n us");
-module_param( prism2_irq_evread_max, int, 0644);
-MODULE_PARM_DESC( prism2_irq_evread_max, "Maximim number of event reads in interrupt handler");
-#endif
-
 MODULE_LICENSE("Dual MPL/GPL");
 
 /*================================================================*/
@@ -231,17 +162,6 @@ static void	prism2sta_inf_authreq_defer(
 static void	prism2sta_inf_psusercnt(
 			wlandevice_t *wlandev, hfa384x_InfFrame_t *inf);
 
-#ifdef CONFIG_PROC_FS
-static int
-prism2sta_proc_read(
-	char	*page,
-	char	**start,
-	off_t	offset,
-	int	count,
-	int	*eof,
-	void	*data);
-#endif
-
 /*================================================================*/
 /* Function Definitions */
 
@@ -267,7 +187,7 @@ inline void dmpmem(void *buf, int n)
 	int c;
 	for ( c= 0; c < n; c++) {
 		if ( (c % 16) == 0 ) printk(KERN_DEBUG"dmp[%d]: ", c);
-		printk("%02x ", ((UINT8*)buf)[c]);
+		printk("%02x ", ((u8*)buf)[c]);
 		if ( (c % 16) == 15 ) printk("\n");
 	}
 	if ( (c % 16) != 0 ) printk("\n");
@@ -299,10 +219,6 @@ static int prism2sta_open(wlandevice_t *wlandev)
 {
 	DBFENTER;
 
-#ifdef ANCIENT_MODULE_CODE
-	MOD_INC_USE_COUNT;
-#endif
-
 	/* We don't currently have to do anything else.
 	 * The setup of the MAC should be subsequently completed via
 	 * the mlme commands.
@@ -341,10 +257,6 @@ static int prism2sta_close(wlandevice_t *wlandev)
 {
 	DBFENTER;
 
-#ifdef ANCIENT_MODULE_CODE
-	MOD_DEC_USE_COUNT;
-#endif
-
 	/* We don't currently have to do anything else.
 	 * Higher layers know we're not ready from dev->start==0 and
 	 * dev->tbusy==1.  Our rx path knows to not pass up received
@@ -463,10 +375,6 @@ static int prism2sta_mlmerequest(wlandevice_t *wlandev, p80211msg_t *msg)
 		WLAN_LOG_DEBUG(2,"Received mibset request\n");
 		result = prism2mgmt_mibset_mibget(wlandev, msg);
 		break;
-	case DIDmsg_dot11req_powermgmt :
-		WLAN_LOG_DEBUG(2,"Received powermgmt request\n");
-		result = prism2mgmt_powermgmt(wlandev, msg);
-		break;
 	case DIDmsg_dot11req_scan :
 		WLAN_LOG_DEBUG(2,"Received scan request\n");
 		result = prism2mgmt_scan(wlandev, msg);
@@ -475,34 +383,6 @@ static int prism2sta_mlmerequest(wlandevice_t *wlandev, p80211msg_t *msg)
 		WLAN_LOG_DEBUG(2,"Received scan_results request\n");
 		result = prism2mgmt_scan_results(wlandev, msg);
 		break;
-	case DIDmsg_dot11req_join :
-		WLAN_LOG_DEBUG(2,"Received join request\n");
-		result = prism2mgmt_join(wlandev, msg);
-		break;
-	case DIDmsg_dot11req_authenticate :
-		WLAN_LOG_DEBUG(2,"Received authenticate request\n");
-		result = prism2mgmt_authenticate(wlandev, msg);
-		break;
-	case DIDmsg_dot11req_deauthenticate :
-		WLAN_LOG_DEBUG(2,"Received mlme deauthenticate request\n");
-		result = prism2mgmt_deauthenticate(wlandev, msg);
-		break;
-	case DIDmsg_dot11req_associate :
-		WLAN_LOG_DEBUG(2,"Received mlme associate request\n");
-		result = prism2mgmt_associate(wlandev, msg);
-		break;
-	case DIDmsg_dot11req_reassociate :
-		WLAN_LOG_DEBUG(2,"Received mlme reassociate request\n");
-		result = prism2mgmt_reassociate(wlandev, msg);
-		break;
-	case DIDmsg_dot11req_disassociate :
-		WLAN_LOG_DEBUG(2,"Received mlme disassociate request\n");
-		result = prism2mgmt_disassociate(wlandev, msg);
-		break;
-	case DIDmsg_dot11req_reset :
-		WLAN_LOG_DEBUG(2,"Received mlme reset request\n");
-		result = prism2mgmt_reset(wlandev, msg);
-		break;
 	case DIDmsg_dot11req_start :
 		WLAN_LOG_DEBUG(2,"Received mlme start request\n");
 		result = prism2mgmt_start(wlandev, msg);
@@ -510,46 +390,10 @@ static int prism2sta_mlmerequest(wlandevice_t *wlandev, p80211msg_t *msg)
 	/*
 	 * Prism2 specific messages
 	 */
-	case DIDmsg_p2req_join :
-		WLAN_LOG_DEBUG(2,"Received p2 join request\n");
-		result = prism2mgmt_p2_join(wlandev, msg);
-		break;
        	case DIDmsg_p2req_readpda :
 		WLAN_LOG_DEBUG(2,"Received mlme readpda request\n");
 		result = prism2mgmt_readpda(wlandev, msg);
 		break;
-	case DIDmsg_p2req_readcis :
-		WLAN_LOG_DEBUG(2,"Received mlme readcis request\n");
-		result = prism2mgmt_readcis(wlandev, msg);
-		break;
-	case DIDmsg_p2req_auxport_state :
-		WLAN_LOG_DEBUG(2,"Received mlme auxport_state request\n");
-		result = prism2mgmt_auxport_state(wlandev, msg);
-		break;
-	case DIDmsg_p2req_auxport_read :
-		WLAN_LOG_DEBUG(2,"Received mlme auxport_read request\n");
-		result = prism2mgmt_auxport_read(wlandev, msg);
-		break;
-	case DIDmsg_p2req_auxport_write :
-		WLAN_LOG_DEBUG(2,"Received mlme auxport_write request\n");
-		result = prism2mgmt_auxport_write(wlandev, msg);
-		break;
-	case DIDmsg_p2req_low_level :
-		WLAN_LOG_DEBUG(2,"Received mlme low_level request\n");
-		result = prism2mgmt_low_level(wlandev, msg);
-		break;
-        case DIDmsg_p2req_test_command :
-                WLAN_LOG_DEBUG(2,"Received mlme test_command request\n");
-                result = prism2mgmt_test_command(wlandev, msg);
-                break;
-        case DIDmsg_p2req_mmi_read :
-                WLAN_LOG_DEBUG(2,"Received mlme mmi_read request\n");
-                result = prism2mgmt_mmi_read(wlandev, msg);
-                break;
-        case DIDmsg_p2req_mmi_write :
-                WLAN_LOG_DEBUG(2,"Received mlme mmi_write request\n");
-                result = prism2mgmt_mmi_write(wlandev, msg);
-                break;
 	case DIDmsg_p2req_ramdl_state :
 		WLAN_LOG_DEBUG(2,"Received mlme ramdl_state request\n");
 		result = prism2mgmt_ramdl_state(wlandev, msg);
@@ -566,18 +410,6 @@ static int prism2sta_mlmerequest(wlandevice_t *wlandev, p80211msg_t *msg)
 		WLAN_LOG_DEBUG(2,"Received mlme flashdl_write request\n");
 		result = prism2mgmt_flashdl_write(wlandev, msg);
 		break;
-	case DIDmsg_p2req_dump_state :
-		WLAN_LOG_DEBUG(2,"Received mlme dump_state request\n");
-		result = prism2mgmt_dump_state(wlandev, msg);
-		break;
-	case DIDmsg_p2req_channel_info :
-		WLAN_LOG_DEBUG(2,"Received mlme channel_info request\n");
-		result = prism2mgmt_channel_info(wlandev, msg);
-		break;
-	case DIDmsg_p2req_channel_info_results :
-		WLAN_LOG_DEBUG(2,"Received mlme channel_info_results request\n");
-		result = prism2mgmt_channel_info_results(wlandev, msg);
-		break;
 	/*
 	 * Linux specific messages
 	 */
@@ -603,18 +435,11 @@ static int prism2sta_mlmerequest(wlandevice_t *wlandev, p80211msg_t *msg)
 		WLAN_LOG_DEBUG(2,"Received mlme autojoin request\n");
 		result = prism2mgmt_autojoin(wlandev, msg);
 		break;
-	case DIDmsg_p2req_enable :
-		WLAN_LOG_DEBUG(2,"Received mlme enable request\n");
-		result = prism2mgmt_enable(wlandev, msg);
-		break;
 	case DIDmsg_lnxreq_commsquality: {
 		p80211msg_lnxreq_commsquality_t *qualmsg;
 
 		WLAN_LOG_DEBUG(2,"Received commsquality request\n");
 
-		if (hw->ap)
-			break;
-
 		qualmsg = (p80211msg_lnxreq_commsquality_t*) msg;
 
 		qualmsg->link.status = P80211ENUM_msgitem_status_data_ok;
@@ -659,10 +484,10 @@ static int prism2sta_mlmerequest(wlandevice_t *wlandev, p80211msg_t *msg)
 *	process thread  (usually)
 *	interrupt
 ----------------------------------------------------------------*/
-UINT32 prism2sta_ifstate(wlandevice_t *wlandev, UINT32 ifstate)
+u32 prism2sta_ifstate(wlandevice_t *wlandev, u32 ifstate)
 {
         hfa384x_t               *hw = (hfa384x_t *)wlandev->priv;
-	UINT32 			result;
+	u32 			result;
 	DBFENTER;
 
 	result = P80211ENUM_resultcode_implementation_failure;
@@ -679,9 +504,6 @@ UINT32 prism2sta_ifstate(wlandevice_t *wlandev, UINT32 ifstate)
 			 * Initialize the device+driver sufficiently
 			 * for firmware loading.
 			 */
-#if (WLAN_HOSTIF != WLAN_USB)
-			result=hfa384x_cmd_initialize(hw);
-#else
 			if ((result=hfa384x_drvr_start(hw))) {
 				WLAN_LOG_ERROR(
 					"hfa384x_drvr_start() failed,"
@@ -691,7 +513,6 @@ UINT32 prism2sta_ifstate(wlandevice_t *wlandev, UINT32 ifstate)
 				wlandev->msdstate = WLAN_MSD_HWPRESENT;
 				break;
 			}
-#endif
 			wlandev->msdstate = WLAN_MSD_FWLOAD;
 			result = P80211ENUM_resultcode_success;
 			break;
@@ -841,8 +662,8 @@ static int prism2sta_getcardinfo(wlandevice_t *wlandev)
 {
 	int 			result = 0;
         hfa384x_t               *hw = (hfa384x_t *)wlandev->priv;
-	UINT16                  temp;
-	UINT8			snum[HFA384x_RID_NICSERIALNUMBER_LEN];
+	u16                  temp;
+	u8			snum[HFA384x_RID_NICSERIALNUMBER_LEN];
 	char			pstr[(HFA384x_RID_NICSERIALNUMBER_LEN * 4) + 1];
 
 	DBFENTER;
@@ -907,20 +728,20 @@ static int prism2sta_getcardinfo(wlandevice_t *wlandev)
 
 	/* strip out the 'special' variant bits */
 	hw->mm_mods = hw->ident_sta_fw.variant & (BIT14 | BIT15);
-	hw->ident_sta_fw.variant &= ~((UINT16)(BIT14 | BIT15));
+	hw->ident_sta_fw.variant &= ~((u16)(BIT14 | BIT15));
 
 	if  ( hw->ident_sta_fw.id == 0x1f ) {
-		hw->ap = 0;
 		WLAN_LOG_INFO(
 			"ident: sta f/w: id=0x%02x %d.%d.%d\n",
 			hw->ident_sta_fw.id, hw->ident_sta_fw.major,
 			hw->ident_sta_fw.minor, hw->ident_sta_fw.variant);
 	} else {
-		hw->ap = 1;
 		WLAN_LOG_INFO(
 			"ident:  ap f/w: id=0x%02x %d.%d.%d\n",
 			hw->ident_sta_fw.id, hw->ident_sta_fw.major,
 			hw->ident_sta_fw.minor, hw->ident_sta_fw.variant);
+		WLAN_LOG_ERROR("Unsupported Tertiary AP firmeare loaded!\n");
+		goto failed;
 	}
 
 	/* Compatibility range, Modem supplier */
@@ -1168,7 +989,7 @@ static int prism2sta_setmulticast(wlandevice_t *wlandev, netdevice_t *dev)
 	int result = 0;
 	hfa384x_t               *hw = (hfa384x_t *)wlandev->priv;
 
-	UINT16  promisc;
+	u16  promisc;
 
 	DBFENTER;
 
@@ -1176,10 +997,6 @@ static int prism2sta_setmulticast(wlandevice_t *wlandev, netdevice_t *dev)
 	if ( hw->state != HFA384x_STATE_RUNNING )
 		goto exit;
 
-	/* If we're an AP, do nothing here */
-	if (hw->ap)
-		goto exit;
-
 	if ( (dev->flags & (IFF_PROMISC | IFF_ALLMULTI)) != 0 )
 		promisc = P80211ENUM_truth_true;
 	else
@@ -1247,9 +1064,9 @@ static void prism2sta_inf_handover(wlandevice_t *wlandev, hfa384x_InfFrame_t *in
 static void prism2sta_inf_tallies(wlandevice_t *wlandev, hfa384x_InfFrame_t *inf)
 {
         hfa384x_t               *hw = (hfa384x_t *)wlandev->priv;
-	UINT16			*src16;
-	UINT32			*dst;
-	UINT32			*src32;
+	u16			*src16;
+	u32			*dst;
+	u32			*src32;
 	int			i;
 	int			cnt;
 
@@ -1260,15 +1077,15 @@ static void prism2sta_inf_tallies(wlandevice_t *wlandev, hfa384x_InfFrame_t *inf
 	** record length of the info record.
 	*/
 
-	cnt = sizeof(hfa384x_CommTallies32_t) / sizeof(UINT32);
+	cnt = sizeof(hfa384x_CommTallies32_t) / sizeof(u32);
 	if (inf->framelen > 22) {
-		dst   = (UINT32 *) &hw->tallies;
-		src32 = (UINT32 *) &inf->info.commtallies32;
+		dst   = (u32 *) &hw->tallies;
+		src32 = (u32 *) &inf->info.commtallies32;
 		for (i = 0; i < cnt; i++, dst++, src32++)
 			*dst += hfa384x2host_32(*src32);
 	} else {
-		dst   = (UINT32 *) &hw->tallies;
-		src16 = (UINT16 *) &inf->info.commtallies16;
+		dst   = (u32 *) &hw->tallies;
+		src16 = (u16 *) &inf->info.commtallies16;
 		for (i = 0; i < cnt; i++, dst++, src16++)
 			*dst += hfa384x2host_16(*src16);
 	}
@@ -1308,7 +1125,7 @@ static void prism2sta_inf_scanresults(wlandevice_t *wlandev,
 	DBFENTER;
 
 	/* Get the number of results, first in bytes, then in results */
-	nbss = (inf->framelen * sizeof(UINT16)) -
+	nbss = (inf->framelen * sizeof(u16)) -
 		sizeof(inf->infotype) -
 		sizeof(inf->info.scanresult.scanreason);
 	nbss /= sizeof(hfa384x_ScanResultSub_t);
@@ -1500,7 +1317,7 @@ void prism2sta_processing_defer(struct work_struct *data)
 
 		/* Don't call this in monitor mode */
 		if ( wlandev->netdev->type == ARPHRD_ETHER ) {
-			UINT16			portstatus;
+			u16			portstatus;
 
 			WLAN_LOG_INFO("linkstatus=CONNECTED\n");
 
@@ -1836,7 +1653,7 @@ static void prism2sta_inf_authreq_defer(wlandevice_t *wlandev,
 	hfa384x_authenticateStation_data_t  rec;
 
 	int    i, added, result, cnt;
-	UINT8  *addr;
+	u8  *addr;
 
 	DBFENTER;
 
@@ -2163,7 +1980,7 @@ void prism2sta_ev_info(wlandevice_t *wlandev, hfa384x_InfFrame_t *inf)
 * Call context:
 *	interrupt
 ----------------------------------------------------------------*/
-void prism2sta_ev_txexc(wlandevice_t *wlandev, UINT16 status)
+void prism2sta_ev_txexc(wlandevice_t *wlandev, u16 status)
 {
 	DBFENTER;
 
@@ -2190,7 +2007,7 @@ void prism2sta_ev_txexc(wlandevice_t *wlandev, UINT16 status)
 * Call context:
 *	interrupt
 ----------------------------------------------------------------*/
-void prism2sta_ev_tx(wlandevice_t *wlandev, UINT16 status)
+void prism2sta_ev_tx(wlandevice_t *wlandev, u16 status)
 {
 	DBFENTER;
 	WLAN_LOG_DEBUG(4, "Tx Complete, status=0x%04x\n", status);
@@ -2247,47 +2064,12 @@ void prism2sta_ev_alloc(wlandevice_t *wlandev)
 {
 	DBFENTER;
 
-	p80211netdev_wake_queue(wlandev);
+	netif_wake_queue(wlandev->netdev);
 
 	DBFEXIT;
 	return;
 }
 
-#if (WLAN_HOSTIF == WLAN_PLX || WLAN_HOSTIF == WLAN_PCI)
-#ifdef CONFIG_PM
-static int prism2sta_suspend_pci(struct pci_dev *pdev, pm_message_t state)
-{
-       	wlandevice_t		*wlandev;
-
-	wlandev = (wlandevice_t *) pci_get_drvdata(pdev);
-
-	/* reset hardware */
-	if (wlandev) {
-		prism2sta_ifstate(wlandev, P80211ENUM_ifstate_disable);
-		p80211_suspend(wlandev);
-	}
-
-	// call a netif_device_detach(wlandev->netdev) ?
-
-	return 0;
-}
-
-static int prism2sta_resume_pci (struct pci_dev *pdev)
-{
-       	wlandevice_t		*wlandev;
-
-	wlandev = (wlandevice_t *) pci_get_drvdata(pdev);
-
-	if (wlandev) {
-		prism2sta_ifstate(wlandev, P80211ENUM_ifstate_disable);
-		p80211_resume(wlandev);
-	}
-
-        return 0;
-}
-#endif
-#endif
-
 /*----------------------------------------------------------------
 * create_wlan
 *
@@ -2334,9 +2116,6 @@ static wlandevice_t *create_wlan(void)
 	wlandev->open = prism2sta_open;
 	wlandev->close = prism2sta_close;
 	wlandev->reset = prism2sta_reset;
-#ifdef CONFIG_PROC_FS
-	wlandev->nsd_proc_read = prism2sta_proc_read;
-#endif
 	wlandev->txframe = prism2sta_txframe;
 	wlandev->mlmerequest = prism2sta_mlmerequest;
 	wlandev->set_multicast_list = prism2sta_setmulticast;
@@ -2351,75 +2130,6 @@ static wlandevice_t *create_wlan(void)
 	return wlandev;
 }
 
-#ifdef CONFIG_PROC_FS
-static int
-prism2sta_proc_read(
-	char	*page,
-	char	**start,
-	off_t	offset,
-	int	count,
-	int	*eof,
-	void	*data)
-{
-	char	 *p = page;
-	wlandevice_t *wlandev = (wlandevice_t *) data;
-	hfa384x_t *hw = (hfa384x_t *) wlandev->priv;
-
-	UINT16 hwtype = 0;
-
-	DBFENTER;
-	if (offset != 0) {
-		*eof = 1;
-		goto exit;
-	}
-
-	// XXX 0x0001 for prism2.5/3, 0x0000 for prism2.
-	hwtype = BIT0;
-
-#if (WLAN_HOSTIF != WLAN_USB)
-	if (hw->isram16)
-		hwtype |= BIT1;
-#endif
-
-#if (WLAN_HOSTIF == WLAN_PCI)
-	hwtype |= BIT2;
-#endif
-
-#define PRISM2_CVS_ID "$Id: prism2sta.c 1826 2007-03-19 15:37:00Z pizza $"
-
-	p += sprintf(p, "# %s version %s (%s) '%s'\n\n",
-		     dev_info,
-		     WLAN_RELEASE, WLAN_BUILD_DATE, PRISM2_CVS_ID);
-
-	p += sprintf(p, "# nic h/w: id=0x%02x %d.%d.%d\n",
-		     hw->ident_nic.id, hw->ident_nic.major,
-		     hw->ident_nic.minor, hw->ident_nic.variant);
-
-	p += sprintf(p, "# pri f/w: id=0x%02x %d.%d.%d\n",
-		     hw->ident_pri_fw.id, hw->ident_pri_fw.major,
-		     hw->ident_pri_fw.minor, hw->ident_pri_fw.variant);
-
-	if (hw->ident_sta_fw.id == 0x1f) {
-		p += sprintf(p, "# sta f/w: id=0x%02x %d.%d.%d\n",
-			     hw->ident_sta_fw.id, hw->ident_sta_fw.major,
-			     hw->ident_sta_fw.minor, hw->ident_sta_fw.variant);
-	} else {
-		p += sprintf(p, "# ap f/w: id=0x%02x %d.%d.%d\n",
-			     hw->ident_sta_fw.id, hw->ident_sta_fw.major,
-			     hw->ident_sta_fw.minor, hw->ident_sta_fw.variant);
-	}
-
-#if (WLAN_HOSTIF != WLAN_USB)
-	p += sprintf(p, "# initial nic hw type, needed for SSF ramdl\n");
-	p += sprintf(p, "initnichw=%04x\n", hwtype);
-#endif
-
- exit:
-	DBFEXIT;
-	return (p - page);
-}
-#endif
-
 void prism2sta_commsqual_defer(struct work_struct *data)
 {
 	hfa384x_t		*hw = container_of(data, struct hfa384x, commsqual_bh);
diff --git a/drivers/staging/wlan-ng/prism2_usb.c b/drivers/staging/wlan-ng/prism2usb.c
index e45be237450..8f7b1f281f0 100644
--- a/drivers/staging/wlan-ng/prism2_usb.c
+++ b/drivers/staging/wlan-ng/prism2usb.c
@@ -1,13 +1,8 @@
-#define WLAN_HOSTIF WLAN_USB
 #include "hfa384x_usb.c"
 #include "prism2mgmt.c"
 #include "prism2mib.c"
 #include "prism2sta.c"
 
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,4,0))
-#error "prism2_usb requires at least a 2.4.x kernel!"
-#endif
-
 #define PRISM_USB_DEVICE(vid, pid, name) \
            USB_DEVICE(vid, pid),  \
            .driver_info = (unsigned long) name
@@ -80,23 +75,11 @@ MODULE_DEVICE_TABLE(usb, usb_prism_tbl);
 *	I'm not sure, assume it's interrupt.
 *
 ----------------------------------------------------------------*/
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0))
-static void __devinit *prism2sta_probe_usb(
-	struct usb_device *dev,
-	unsigned int ifnum,
-	const struct usb_device_id *id)
-#else
 static int prism2sta_probe_usb(
 	struct usb_interface *interface,
 	const struct usb_device_id *id)
-#endif
 {
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0))
-	struct usb_interface *interface;
-#else
 	struct usb_device *dev;
-#endif
 
 	wlandevice_t	*wlandev = NULL;
 	hfa384x_t	*hw = NULL;
@@ -104,12 +87,7 @@ static int prism2sta_probe_usb(
 
 	DBFENTER;
 
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0))
-	interface = &dev->actconfig->interface[ifnum];
-#else
 	dev = interface_to_usbdev(interface);
-#endif
-
 
 	if ((wlandev = create_wlan()) == NULL) {
 		WLAN_LOG_ERROR("%s: Memory allocation failure.\n", dev_info);
@@ -131,14 +109,7 @@ static int prism2sta_probe_usb(
 	/* Register the wlandev, this gets us a name and registers the
 	 * linux netdevice.
 	 */
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0))
 	SET_NETDEV_DEV(wlandev->netdev, &(interface->dev));
-#endif
-        if ( register_wlandev(wlandev) != 0 ) {
-		WLAN_LOG_ERROR("%s: register_wlandev() failed.\n", dev_info);
-		result = -EIO;
-		goto failed;
-        }
 
 	/* Do a chip-level reset on the MAC */
 	if (prism2_doreset) {
@@ -156,15 +127,19 @@ static int prism2sta_probe_usb(
 		}
 	}
 
-#ifndef NEW_MODULE_CODE
-	usb_inc_dev_use(dev);
-#endif
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,15))
 	usb_get_dev(dev);
-#endif
 
 	wlandev->msdstate = WLAN_MSD_HWPRESENT;
 
+        if ( register_wlandev(wlandev) != 0 ) {
+		WLAN_LOG_ERROR("%s: register_wlandev() failed.\n", dev_info);
+		result = -EIO;
+		goto failed;
+        }
+
+/* enable the card */
+	prism2sta_ifstate(wlandev, P80211ENUM_ifstate_enable);
+
 	goto done;
 
  failed:
@@ -175,12 +150,8 @@ static int prism2sta_probe_usb(
  done:
 	DBFEXIT;
 
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0))
-	return wlandev;
-#else
 	usb_set_intfdata(interface, wlandev);
 	return result;
-#endif
 }
 
 
@@ -203,25 +174,14 @@ static int prism2sta_probe_usb(
 * Call context:
 *	process
 ----------------------------------------------------------------*/
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0))
-static void __devexit
-prism2sta_disconnect_usb(struct usb_device *dev, void *ptr)
-#else
 static void
 prism2sta_disconnect_usb(struct usb_interface *interface)
-#endif
 {
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0))
 	wlandevice_t		*wlandev;
-#else
-	wlandevice_t		*wlandev = (wlandevice_t*)ptr;
-#endif
 
         DBFENTER;
 
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0))
 	wlandev = (wlandevice_t *) usb_get_intfdata(interface);
-#endif
 
 	if ( wlandev != NULL ) {
 		LIST_HEAD(cleanlist);
@@ -296,12 +256,7 @@ prism2sta_disconnect_usb(struct usb_interface *interface)
 		unregister_wlandev(wlandev);
 		wlan_unsetup(wlandev);
 
-#ifndef NEW_MODULE_CODE
-		usb_dec_dev_use(hw->usb);
-#endif
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,15))
 		usb_put_dev(hw->usb);
-#endif
 
 		hfa384x_destroy(hw);
 		kfree(hw);
@@ -311,17 +266,12 @@ prism2sta_disconnect_usb(struct usb_interface *interface)
 
  exit:
 
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,5,0))
 	usb_set_intfdata(interface, NULL);
-#endif
 	DBFEXIT;
 }
 
 
 static struct usb_driver prism2_usb_driver = {
-#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,4,19)) && (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,16))
-	.owner = THIS_MODULE,
-#endif
 	.name = "prism2_usb",
 	.probe = prism2sta_probe_usb,
 	.disconnect = prism2sta_disconnect_usb,
@@ -329,15 +279,10 @@ static struct usb_driver prism2_usb_driver = {
 	/* fops, minor? */
 };
 
-#ifdef MODULE
-
 static int __init prism2usb_init(void)
 {
         DBFENTER;
 
-        WLAN_LOG_NOTICE("%s Loaded\n", version);
-        WLAN_LOG_NOTICE("dev_info is: %s\n", dev_info);
-
 	/* This call will result in calls to prism2sta_probe_usb. */
 	return usb_register(&prism2_usb_driver);
 
@@ -350,12 +295,8 @@ static void __exit prism2usb_cleanup(void)
 
 	usb_deregister(&prism2_usb_driver);
 
-        printk(KERN_NOTICE "%s Unloaded\n", version);
-
 	DBFEXIT;
 };
 
 module_init(prism2usb_init);
 module_exit(prism2usb_cleanup);
-
-#endif // module
diff --git a/drivers/staging/wlan-ng/version.h b/drivers/staging/wlan-ng/version.h
deleted file mode 100644
index 305f8823944..00000000000
--- a/drivers/staging/wlan-ng/version.h
+++ /dev/null
@@ -1,64 +0,0 @@
-/* src/include/wlan/version.h
-*
-*
-* Copyright (C) 1999 AbsoluteValue Systems, Inc.  All Rights Reserved.
-* --------------------------------------------------------------------
-*
-* linux-wlan
-*
-*   The contents of this file are subject to the Mozilla Public
-*   License Version 1.1 (the "License"); you may not use this file
-*   except in compliance with the License. You may obtain a copy of
-*   the License at http://www.mozilla.org/MPL/
-*
-*   Software distributed under the License is distributed on an "AS
-*   IS" basis, WITHOUT WARRANTY OF ANY KIND, either express or
-*   implied. See the License for the specific language governing
-*   rights and limitations under the License.
-*
-*   Alternatively, the contents of this file may be used under the
-*   terms of the GNU Public License version 2 (the "GPL"), in which
-*   case the provisions of the GPL are applicable instead of the
-*   above.  If you wish to allow the use of your version of this file
-*   only under the terms of the GPL and not to allow others to use
-*   your version of this file under the MPL, indicate your decision
-*   by deleting the provisions above and replace them with the notice
-*   and other provisions required by the GPL.  If you do not delete
-*   the provisions above, a recipient may use your version of this
-*   file under either the MPL or the GPL.
-*
-* --------------------------------------------------------------------
-*
-* Inquiries regarding the linux-wlan Open Source project can be
-* made directly to:
-*
-* AbsoluteValue Systems Inc.
-* info@linux-wlan.com
-* http://www.linux-wlan.com
-*
-* --------------------------------------------------------------------
-*
-* Portions of the development of this software were funded by
-* Intersil Corporation as part of PRISM(R) chipset product development.
-*
-* --------------------------------------------------------------------
-*/
-#ifndef _WLAN_VERSION_H
-#define _WLAN_VERSION_H
-#ifndef KERNEL_VERSION
-#define KERNEL_VERSION(a,b,c) (((a) << 16) + ((b) << 8) + (c))
-#endif
-
-/* WLAN_HOSTIF (generally set on the command line, not detected) */
-#define WLAN_NONE                       0
-#define WLAN_PCMCIA                     1
-#define WLAN_ISA                        2
-#define WLAN_PCI                        3
-#define WLAN_USB                        4
-#define WLAN_PLX                        5
-#define WLAN_SLAVE                      6
-#define WLAN_RELEASE	"0.2.8"
-#define WLAN_RELEASE_CODE 0x000208
-#define WLAN_BUILD_DATE "Thu Oct  2 11:04:42 PDT 2008"
-
-#endif
diff --git a/drivers/staging/wlan-ng/wlan_compat.h b/drivers/staging/wlan-ng/wlan_compat.h
index 59dfa8f84cb..8b8a510685c 100644
--- a/drivers/staging/wlan-ng/wlan_compat.h
+++ b/drivers/staging/wlan-ng/wlan_compat.h
@@ -49,114 +49,6 @@
 #define _WLAN_COMPAT_H
 
 /*=============================================================*/
-/*------ Establish Platform Identity --------------------------*/
-/*=============================================================*/
-/* Key macros: */
-/* WLAN_CPU_FAMILY */
-	#define WLAN_Ix86			1
-	#define WLAN_PPC			2
-	#define WLAN_Ix96			3
-	#define WLAN_ARM			4
-	#define WLAN_ALPHA			5
-	#define WLAN_MIPS			6
-	#define WLAN_HPPA			7
-	#define WLAN_SPARC			8
-	#define WLAN_SH    			9
-	#define WLAN_x86_64                     10
-/* WLAN_SYSARCH */
-	#define WLAN_PCAT			1
-	#define WLAN_MBX			2
-	#define WLAN_RPX			3
-	#define WLAN_LWARCH			4
-	#define WLAN_PMAC			5
-	#define WLAN_SKIFF			6
-	#define WLAN_BITSY			7
-	#define WLAN_ALPHAARCH			7
-	#define WLAN_MIPSARCH			9
-	#define WLAN_HPPAARCH			10
-	#define WLAN_SPARCARCH			11
-	#define WLAN_SHARCH   			12
-
-/* Note: the PLX HOSTIF above refers to some vendors implementations for */
-/*       PCI.  It's a PLX chip that is a PCI to PCMCIA adapter, but it   */
-/*       isn't a real PCMCIA host interface adapter providing all the    */
-/*       card&socket services.                                           */
-
-#if (defined(CONFIG_PPC) || defined(CONFIG_8xx) || defined(__powerpc__))
-#ifndef __ppc__
-#define __ppc__
-#endif
-#endif
-
-#if defined(__KERNEL__)
-
-#ifndef AUTOCONF_INCLUDED
-#include <linux/config.h>
-#endif
-
-#if defined(__x86_64__)
-	#define WLAN_CPU_FAMILY		WLAN_x86_64
-	#define WLAN_SYSARCH		WLAN_PCAT
-#elif defined(__i386__) || defined(__i486__) || defined(__i586__) || defined(__i686__)
-	#define WLAN_CPU_FAMILY		WLAN_Ix86
-	#define WLAN_SYSARCH		WLAN_PCAT
-#elif defined(__ppc__)
-	#define WLAN_CPU_FAMILY		WLAN_PPC
-	#if defined(CONFIG_MBX)
-		#define WLAN_SYSARCH	WLAN_MBX
-	#elif defined(CONFIG_RPXLITE)
-		#define WLAN_SYSARCH	WLAN_RPX
-	#elif defined(CONFIG_RPXCLASSIC)
-		#define WLAN_SYSARCH	WLAN_RPX
-	#else
-		#define WLAN_SYSARCH	WLAN_PMAC
-	#endif
-#elif defined(__arm__)
-	#define WLAN_CPU_FAMILY		WLAN_ARM
-	#define WLAN_SYSARCH		WLAN_SKIFF
-#elif defined(__alpha__)
-	#define WLAN_CPU_FAMILY		WLAN_ALPHA
-	#define WLAN_SYSARCH		WLAN_ALPHAARCH
-#elif defined(__mips__)
-	#define WLAN_CPU_FAMILY		WLAN_MIPS
-	#define WLAN_SYSARCH		WLAN_MIPSARCH
-#elif defined(__hppa__)
-	#define WLAN_CPU_FAMILY		WLAN_HPPA
-	#define WLAN_SYSARCH		WLAN_HPPAARCH
-#elif defined(__sparc__)
-        #define WLAN_CPU_FAMILY         WLAN_SPARC
-        #define WLAN_SYSARCH            WLAN_SPARC
-#elif defined(__sh__)
-        #define WLAN_CPU_FAMILY         WLAN_SH
-        #define WLAN_SYSARCH            WLAN_SHARCH
-        #ifndef __LITTLE_ENDIAN__
-        #define __LITTLE_ENDIAN__
-        #endif
-#else
-	#error "No CPU identified!"
-#endif
-#endif /* __KERNEL__ */
-
-/*
-   Some big endian machines implicitly do all I/O in little endian mode.
-
-   In particular:
-          Linux/PPC on PowerMacs (PCI)
-	  Arm/Intel Xscale (PCI)
-
-   This may also affect PLX boards and other BE &| PPC platforms;
-   as new ones are discovered, add them below.
-*/
-
-#if defined(WLAN_HOSTIF)
-#if ((WLAN_HOSTIF == WLAN_PCI) || (WLAN_HOSTIF == WLAN_PLX))
-#if ((WLAN_SYSARCH == WLAN_SKIFF) || (WLAN_SYSARCH == WLAN_PMAC) || (WLAN_SYSARCH == WLAN_SPARC))
-#define REVERSE_ENDIAN
-#endif
-#endif
-#endif
-
-/*=============================================================*/
 /*------ Bit settings -----------------------------------------*/
 /*=============================================================*/
 
@@ -193,30 +85,6 @@
 #define BIT30	0x40000000
 #define BIT31	0x80000000
 
-#include <linux/types.h>
-
-typedef u_int8_t	UINT8;
-typedef u_int16_t	UINT16;
-typedef u_int32_t	UINT32;
-
-typedef int8_t		INT8;
-typedef int16_t		INT16;
-typedef int32_t		INT32;
-
-typedef unsigned int    UINT;
-typedef signed int      INT;
-
-typedef u_int64_t	UINT64;
-typedef int64_t		INT64;
-
-#define UINT8_MAX	(0xffUL)
-#define UINT16_MAX	(0xffffUL)
-#define UINT32_MAX	(0xffffffffUL)
-
-#define INT8_MAX	(0x7fL)
-#define INT16_MAX	(0x7fffL)
-#define INT32_MAX	(0x7fffffffL)
-
 /*=============================================================*/
 /*------ Compiler Portability Macros --------------------------*/
 /*=============================================================*/
@@ -230,20 +98,9 @@ typedef int64_t		INT64;
 #define WLAN_DBVAR	wlan_debug
 #endif
 
-#ifndef KERNEL_VERSION
-#define KERNEL_VERSION(a,b,c) (((a) << 16) + ((b) << 8) + (c))
-#endif
+#define WLAN_RELEASE	"0.3.0-lkml"
 
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,0))
-#  if (LINUX_VERSION_CODE > KERNEL_VERSION(2,6,8))
-#    include <linux/hardirq.h>
-#  else
-#    include <asm/hardirq.h>
-#  endif
-#elif defined(__KERNEL__)
-#  define PREEMPT_MASK  (0x000000FFUL)
-#  define preempt_count() (0UL)
-#endif
+#include <linux/hardirq.h>
 
 #define WLAN_LOG_ERROR(x,args...) printk(KERN_ERR "%s: " x , __func__ , ##args);
 
@@ -254,20 +111,17 @@ typedef int64_t		INT64;
 #define WLAN_LOG_INFO(args... ) printk(KERN_INFO args)
 
 #if defined(WLAN_INCLUDE_DEBUG)
-	#define WLAN_ASSERT(c) if ((!(c)) && WLAN_DBVAR >= 1) { \
-		WLAN_LOG_DEBUG(1, "Assertion failure!\n"); }
 	#define WLAN_HEX_DUMP( l, x, p, n)	if( WLAN_DBVAR >= (l) ){ \
 		int __i__; \
 		printk(KERN_DEBUG x ":"); \
 		for( __i__=0; __i__ < (n); __i__++) \
-			printk( " %02x", ((UINT8*)(p))[__i__]); \
+			printk( " %02x", ((u8*)(p))[__i__]); \
 		printk("\n"); }
 	#define DBFENTER { if ( WLAN_DBVAR >= 5 ){ WLAN_LOG_DEBUG(3,"---->\n"); } }
 	#define DBFEXIT  { if ( WLAN_DBVAR >= 5 ){ WLAN_LOG_DEBUG(3,"<----\n"); } }
 
 	#define WLAN_LOG_DEBUG(l,x,args...) if ( WLAN_DBVAR >= (l)) printk(KERN_DEBUG "%s(%lu): " x ,  __func__, (preempt_count() & PREEMPT_MASK), ##args );
 #else
-	#define WLAN_ASSERT(c)
 	#define WLAN_HEX_DUMP( l, s, p, n)
 	#define DBFENTER
 	#define DBFEXIT
@@ -275,413 +129,11 @@ typedef int64_t		INT64;
 	#define WLAN_LOG_DEBUG(l, s, args...)
 #endif
 
-#ifdef CONFIG_SMP
-#define __SMP__			1
-#endif
-
-#if defined(__KERNEL__)
-
-#if ((LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0)) || (LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,19)))
-#define URB_ONLY_CALLBACK
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,19))
-#define PT_REGS    , struct pt_regs *regs
-#else
-#define PT_REGS
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,7))
-#  define del_singleshot_timer_sync(a)  del_timer_sync(a)
-#endif
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,4,17))
-#define CONFIG_NETLINK		1
-#endif
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,4,0))
-#define kfree_s(a, b)	kfree((a))
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,2,18))
-#ifndef init_waitqueue_head
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,0,16))
-#define init_waitqueue_head(p)  (*(p) = NULL)
-#else
-#define init_waitqueue_head(p)  init_waitqueue(p)
-#endif
-typedef struct wait_queue *wait_queue_head_t;
-typedef struct wait_queue wait_queue_t;
-#define set_current_state(b)  { current->state = (b); mb(); }
-#define init_waitqueue_entry(a, b) { (a)->task = current; }
-#endif
-#endif
-
-#ifndef wait_event_interruptible_timeout
-// retval == 0; signal met; we're good.
-// retval < 0; interrupted by signal.
-// retval > 0; timed out.
-
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(2,4,0))  // fixme?
-
-#define __wait_event_interruptible_timeout(wq, condition, ret)            \
-do {                                                                      \
-          wait_queue_t __wait;                                            \
-          init_waitqueue_entry(&__wait, current);                         \
-	                                                                  \
-          add_wait_queue(&wq, &__wait);                                   \
-          for (;;) {                                                      \
-                  set_current_state(TASK_INTERRUPTIBLE);                  \
-                  if (condition)                                          \
-                          break;                                          \
-                  if (!signal_pending(current)) {                         \
-                          ret = schedule_timeout(ret)    ;                \
-                          if (!ret)                                       \
-                                 break;                                   \
-                          continue;                                       \
-                  }                                                       \
-                  ret = -ERESTARTSYS;                                     \
-                  break;                                                  \
-          }                                                               \
-          set_current_state(TASK_RUNNING);                                \
-          remove_wait_queue(&wq, &__wait);                                \
-} while (0)
-
-#else // 2.2
-
-
-#define __wait_event_interruptible_timeout(wq, condition, ret)          \
-do {                                                                    \
-        struct wait_queue __wait;                                       \
-                                                                        \
-        __wait.task = current;                                          \
-        add_wait_queue(&wq, &__wait);                                   \
-        for (;;) {                                                      \
-                current->state = TASK_INTERRUPTIBLE;                    \
-                if (condition)                                          \
-                        break;                                          \
-                if (!signal_pending(current)) {                         \
-                        ret = schedule_timeout(ret);                    \
-                        if (!ret)                                       \
-                               break;                                   \
-                        continue;                                       \
-                }                                                       \
-                ret = -ERESTARTSYS;                                     \
-                break;                                                  \
-        }                                                               \
-        current->state = TASK_RUNNING;                                  \
-        remove_wait_queue(&wq, &__wait);                                \
-} while (0)
-
-#endif  // version >= 2.4
-
-#define wait_event_interruptible_timeout(wq, condition, timeout)	  \
-({									  \
-	long __ret = timeout;						  \
-	if (!(condition))						  \
-		__wait_event_interruptible_timeout(wq, condition, __ret); \
-	__ret;								  \
-})
-
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,4,20))
-#ifdef _LINUX_LIST_H
-
-static inline void list_move_tail(struct list_head *list,
-          struct list_head *head)
-{
-        __list_del(list->prev, list->next);
-        list_add_tail(list, head);
-}
-
-static inline void __list_splice(struct list_head *list,
-				 struct list_head *head)
-{
-      struct list_head *first = list->next;
-      struct list_head *last = list->prev;
-      struct list_head *at = head->next;
-
-      first->prev = head;
-      head->next = first;
-
-      last->next = at;
-      at->prev = last;
-}
-
-static inline void list_move(struct list_head *list, struct list_head *head)
-{
-      __list_del(list->prev, list->next);
-      list_add(list, head);
-}
-
-static inline void list_splice_init(struct list_head *list,
-            struct list_head *head)
-{
-	if (!list_empty(list)) {
-		__list_splice(list, head);
-		INIT_LIST_HEAD(list);
-	}
-}
-
-
-#endif  // LIST_H
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,1,90))
-#define spin_lock(l)            do { } while (0)
-#define spin_unlock(l)          do { } while (0)
-#define spin_lock_irqsave(l,f)  do { save_flags(f); cli(); } while (0)
-#define spin_unlock_irqrestore(l,f) do { restore_flags(f); } while (0)
-#define spin_lock_init(s)       do { } while (0)
-#define spin_trylock(l)         (1)
-typedef int spinlock_t;
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,4,0)) // XXX ???
-#define spin_lock_bh         spin_lock
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,4,0))
-#ifdef CONFIG_SMP
-#define spin_is_locked(x)       (*(volatile char *)(&(x)->lock) <= 0)
-#else
-#define spin_is_locked(l)       (0)
-#endif
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,4,28))
-#define __user
-#define __iomem
-#endif
-
-#ifdef _LINUX_PROC_FS_H
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,3,25))
-
-extern inline struct proc_dir_entry *
-create_proc_read_entry(const char *name, mode_t mode,
-                       struct proc_dir_entry *base,
-                       read_proc_t *read_proc, void *data)
-{
-    struct proc_dir_entry *res = create_proc_entry(name, mode, base);
-    if (res) {
-        res->read_proc = read_proc;
-        res->data = data;
-    }
-    return res;
-}
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,3,29))
-#ifndef proc_mkdir
-#define proc_mkdir(name, root) create_proc_entry(name, S_IFDIR, root)
-#endif
-#endif
-#endif /* _LINUX_PROC_FS_H */
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0))
-#ifndef INIT_TQUEUE
-#define PREPARE_TQUEUE(_tq, _routine, _data)                    \
-        do {                                                    \
-                (_tq)->routine = _routine;                      \
-                (_tq)->data = _data;                            \
-        } while (0)
-#define INIT_TQUEUE(_tq, _routine, _data)                       \
-        do {                                                    \
-                INIT_LIST_HEAD(&(_tq)->list);                   \
-                (_tq)->sync = 0;                                \
-                PREPARE_TQUEUE((_tq), (_routine), (_data));     \
-        } while (0)
-#endif
-
-#ifndef container_of
-#define container_of(ptr, type, member) ({			\
-        const typeof( ((type *)0)->member ) *__mptr = (ptr);	\
-        (type *)( (char *)__mptr - offsetof(type,member) );})
-#endif
-
-#ifndef INIT_WORK
-#define work_struct tq_struct
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,4,0))
-#define schedule_work(a)   queue_task(a, &tq_scheduler)
-#else
-#define schedule_work(a)  schedule_task(a)
-#endif
-
-#define flush_scheduled_work  flush_scheduled_tasks
-#define INIT_WORK2(_wq, _routine)  INIT_TQUEUE(_wq, (void (*)(void *))_routine, _wq)
-#endif
-
-#else // >= 2.5 kernel
-
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
-#define INIT_WORK2(_wq, _routine)	INIT_WORK(_wq, (void (*)(void *))_routine, _wq)
-#else
-#define INIT_WORK2(_wq, _routine)	INIT_WORK(_wq, _routine)
-#endif
-
-#endif // >= 2.5 kernel
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,3,38))
-typedef struct device netdevice_t;
-#elif (LINUX_VERSION_CODE < KERNEL_VERSION(2,4,4))
-typedef struct net_device netdevice_t;
-#else
 #undef netdevice_t
 typedef struct net_device netdevice_t;
-#endif
 
-#ifdef WIRELESS_EXT
-#if (WIRELESS_EXT < 13)
-struct iw_request_info
-{
-        __u16           cmd;            /* Wireless Extension command */
-        __u16           flags;          /* More to come ;-) */
-};
-#endif
-#endif
-
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,1,18))
-#define MODULE_PARM(a,b)        extern int __bogus_decl
-#define MODULE_AUTHOR(a)        extern int __bogus_decl
-#define MODULE_DESCRIPTION(a)   extern int __bogus_decl
-#define MODULE_SUPPORTED_DEVICE(a) extern int __bogus_decl
-#undef  GET_USE_COUNT
-#define GET_USE_COUNT(m)        mod_use_count_
-#endif
-
-#ifndef MODULE_OWNER
-#define MODULE_OWNER(a)         extern int __bogus_decl
-#define ANCIENT_MODULE_CODE
-#endif
-
-#ifndef MODULE_LICENSE
-#define MODULE_LICENSE(m)       extern int __bogus_decl
-#endif
-
-/* TODO:  Do we care about this? */
-#ifndef MODULE_DEVICE_TABLE
-#define MODULE_DEVICE_TABLE(foo,bar)
-#endif
-
-#define wlan_minutes2ticks(a) ((a)*(wlan_ticks_per_sec *  60))
-#define wlan_seconds2ticks(a) ((a)*(wlan_ticks_per_sec))
-
-#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,5,47))
-#define NEW_MODULE_CODE
-#ifdef ANCIENT_MODULE_CODE
-#undef ANCIENT_MODULE_CODE
-#endif
-#elif (LINUX_VERSION_CODE < KERNEL_VERSION(2,4,25))
-#define module_param(name, type, perm)                                       \
-        static inline void *__check_existence_##name(void) { return &name; } \
-        MODULE_PARM(name, _MODULE_PARM_STRING_ ## type)
-
-#define _MODULE_PARM_STRING_byte "b"
-#define _MODULE_PARM_STRING_short "h"
-#define _MODULE_PARM_STRING_ushort "h"
-#define _MODULE_PARM_STRING_int "i"
-#define _MODULE_PARM_STRING_uint "i"
-#define _MODULE_PARM_STRING_long "l"
-#define _MODULE_PARM_STRING_ulong "l"
-#define _MODULE_PARM_STRING_bool "i"
-#endif
-
-/* linux < 2.5.69 */
-#ifndef IRQ_NONE
-typedef void irqreturn_t;
-#define IRQ_NONE
-#define IRQ_HANDLED
-#define IRQ_RETVAL(x)
-#endif
-
-#ifndef in_atomic
-#define in_atomic()  0
-#endif
-
-#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,6,13))
 #define URB_ASYNC_UNLINK 0
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,7))
-#define URB_ASYNC_UNLINK  USB_ASYNC_UNLINK
-#define usb_fill_bulk_urb  FILL_BULK_URB
-#define usb_kill_urb  usb_unlink_urb
-#else
 #define USB_QUEUE_BULK 0
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,11))
-typedef u32 pm_message_t;
-#endif
-
-#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,6,9))
-#define hotplug_path  "/etc/hotplug/wlan.agent"
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0))
-#define free_netdev(x)       kfree(x)
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,9))
-#define eth_hdr(x)           (x)->mac.ethernet
-#endif
-
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,4,0))
-#define del_timer_sync(a)       del_timer(a)
-#endif
-
-#ifndef might_sleep
-#define might_sleep(a)   do { } while (0)
-#endif
-
-/* Apparently 2.4.2 ethtool is quite different, maybe newer too? */
-#if (defined(SIOETHTOOL) && !defined(ETHTOOL_GDRVINFO))
-#undef SIOETHTOOL
-#endif
-
-// pcmcia-cs stuff
-#if ((LINUX_VERSION_CODE < KERNEL_VERSION(2,5,68)) && \
-     !defined(pcmcia_access_configuration_register))
-#define pcmcia_access_configuration_register(handle, reg) \
-        CardServices(AccessConfigurationRegister, handle, reg)
-#define pcmcia_register_client(handle, reg) \
-        CardServices(RegisterClient, handle, reg)
-#define pcmcia_deregister_client(handle) \
-        CardServices(DeregisterClient, handle)
-#define pcmcia_get_first_tuple(handle, tuple) \
-        CardServices(GetFirstTuple, handle, tuple)
-#define pcmcia_get_next_tuple(handle, tuple) \
-        CardServices(GetNextTuple, handle, tuple)
-#define pcmcia_get_tuple_data(handle, tuple) \
-        CardServices(GetTupleData, handle, tuple)
-#define pcmcia_parse_tuple(handle, tuple, parse) \
-        CardServices(ParseTuple, handle, tuple, parse)
-#define pcmcia_get_configuration_info(handle, config) \
-        CardServices(GetConfigurationInfo, handle, config)
-#define pcmcia_request_io(handle, req) \
-        CardServices(RequestIO, handle, req)
-#define pcmcia_request_irq(handle, req) \
-        CardServices(RequestIRQ, handle, req)
-#define pcmcia_request_configuration(handle, req) \
-        CardServices(RequestConfiguration, handle, req)
-#define pcmcia_release_configuration(handle) \
-        CardServices(ReleaseConfiguration, handle)
-#define pcmcia_release_io(handle, req) \
-        CardServices(ReleaseIO, handle, req)
-#define pcmcia_release_irq(handle, req) \
-        CardServices(ReleaseIRQ, handle, req)
-#define pcmcia_release_window(win) \
-        CardServices(ReleaseWindow, win)
-#define pcmcia_get_card_services_info(info) \
-        CardServices(GetCardServicesInfo, info)
-#define pcmcia_report_error(handle, err) \
-        CardServices(ReportError, handle, err)
-#endif
-
-#endif /* __KERNEL__ */
 
 /*=============================================================*/
 /*------ Hardware Portability Macros --------------------------*/
@@ -692,22 +144,6 @@ typedef u32 pm_message_t;
 #define host2ieee16(n)	__cpu_to_le16(n)
 #define host2ieee32(n)	__cpu_to_le32(n)
 
-#if (WLAN_CPU_FAMILY != WLAN_MIPS)
-typedef UINT32 phys_t;
-#endif
-
-#if (WLAN_CPU_FAMILY == WLAN_PPC)
-       #define wlan_inw(a)                     in_be16((unsigned short *)((a)+_IO_BASE))
-       #define wlan_inw_le16_to_cpu(a)         inw((a))
-       #define wlan_outw(v,a)                  out_be16((unsigned short *)((a)+_IO_BASE), (v))
-       #define wlan_outw_cpu_to_le16(v,a)      outw((v),(a))
-#else
-       #define wlan_inw(a)                     inw((a))
-       #define wlan_inw_le16_to_cpu(a)         __cpu_to_le16(inw((a)))
-       #define wlan_outw(v,a)                  outw((v),(a))
-       #define wlan_outw_cpu_to_le16(v,a)      outw(__cpu_to_le16((v)),(a))
-#endif
-
 /*=============================================================*/
 /*--- General Macros ------------------------------------------*/
 /*=============================================================*/
diff --git a/drivers/thermal/thermal_sys.c b/drivers/thermal/thermal_sys.c
index fe07462d594..8171ca17b93 100644
--- a/drivers/thermal/thermal_sys.c
+++ b/drivers/thermal/thermal_sys.c
@@ -579,7 +579,7 @@ static void thermal_release(struct device *dev)
 	struct thermal_zone_device *tz;
 	struct thermal_cooling_device *cdev;
 
-	if (!strncmp(dev->bus_id, "thermal_zone", sizeof "thermal_zone" - 1)) {
+	if (!strncmp(dev_name(dev), "thermal_zone", sizeof "thermal_zone" - 1)) {
 		tz = to_thermal_zone(dev);
 		kfree(tz);
 	} else {
@@ -630,7 +630,7 @@ struct thermal_cooling_device *thermal_cooling_device_register(char *type,
 	cdev->ops = ops;
 	cdev->device.class = &thermal_class;
 	cdev->devdata = devdata;
-	sprintf(cdev->device.bus_id, "cooling_device%d", cdev->id);
+	dev_set_name(&cdev->device, "cooling_device%d", cdev->id);
 	result = device_register(&cdev->device);
 	if (result) {
 		release_idr(&thermal_cdev_idr, &thermal_idr_lock, cdev->id);
@@ -769,7 +769,7 @@ struct thermal_zone_device *thermal_zone_device_register(char *type,
 	tz->device.class = &thermal_class;
 	tz->devdata = devdata;
 	tz->trips = trips;
-	sprintf(tz->device.bus_id, "thermal_zone%d", tz->id);
+	dev_set_name(&tz->device, "thermal_zone%d", tz->id);
 	result = device_register(&tz->device);
 	if (result) {
 		release_idr(&thermal_tz_idr, &thermal_idr_lock, tz->id);
diff --git a/drivers/uio/uio.c b/drivers/uio/uio.c
index 2d2440cd57a..4ca85a113aa 100644
--- a/drivers/uio/uio.c
+++ b/drivers/uio/uio.c
@@ -35,6 +35,7 @@ struct uio_device {
 	int			vma_count;
 	struct uio_info		*info;
 	struct kobject		*map_dir;
+	struct kobject		*portio_dir;
 };
 
 static int uio_major;
@@ -75,17 +76,17 @@ static ssize_t map_offset_show(struct uio_mem *mem, char *buf)
 	return sprintf(buf, "0x%lx\n", mem->addr & ~PAGE_MASK);
 }
 
-struct uio_sysfs_entry {
+struct map_sysfs_entry {
 	struct attribute attr;
 	ssize_t (*show)(struct uio_mem *, char *);
 	ssize_t (*store)(struct uio_mem *, const char *, size_t);
 };
 
-static struct uio_sysfs_entry addr_attribute =
+static struct map_sysfs_entry addr_attribute =
 	__ATTR(addr, S_IRUGO, map_addr_show, NULL);
-static struct uio_sysfs_entry size_attribute =
+static struct map_sysfs_entry size_attribute =
 	__ATTR(size, S_IRUGO, map_size_show, NULL);
-static struct uio_sysfs_entry offset_attribute =
+static struct map_sysfs_entry offset_attribute =
 	__ATTR(offset, S_IRUGO, map_offset_show, NULL);
 
 static struct attribute *attrs[] = {
@@ -106,9 +107,9 @@ static ssize_t map_type_show(struct kobject *kobj, struct attribute *attr,
 {
 	struct uio_map *map = to_map(kobj);
 	struct uio_mem *mem = map->mem;
-	struct uio_sysfs_entry *entry;
+	struct map_sysfs_entry *entry;
 
-	entry = container_of(attr, struct uio_sysfs_entry, attr);
+	entry = container_of(attr, struct map_sysfs_entry, attr);
 
 	if (!entry->show)
 		return -EIO;
@@ -116,16 +117,93 @@ static ssize_t map_type_show(struct kobject *kobj, struct attribute *attr,
 	return entry->show(mem, buf);
 }
 
-static struct sysfs_ops uio_sysfs_ops = {
+static struct sysfs_ops map_sysfs_ops = {
 	.show = map_type_show,
 };
 
 static struct kobj_type map_attr_type = {
 	.release	= map_release,
-	.sysfs_ops	= &uio_sysfs_ops,
+	.sysfs_ops	= &map_sysfs_ops,
 	.default_attrs	= attrs,
 };
 
+struct uio_portio {
+	struct kobject kobj;
+	struct uio_port *port;
+};
+#define to_portio(portio) container_of(portio, struct uio_portio, kobj)
+
+static ssize_t portio_start_show(struct uio_port *port, char *buf)
+{
+	return sprintf(buf, "0x%lx\n", port->start);
+}
+
+static ssize_t portio_size_show(struct uio_port *port, char *buf)
+{
+	return sprintf(buf, "0x%lx\n", port->size);
+}
+
+static ssize_t portio_porttype_show(struct uio_port *port, char *buf)
+{
+	const char *porttypes[] = {"none", "x86", "gpio", "other"};
+
+	if ((port->porttype < 0) || (port->porttype > UIO_PORT_OTHER))
+		return -EINVAL;
+
+	return sprintf(buf, "port_%s\n", porttypes[port->porttype]);
+}
+
+struct portio_sysfs_entry {
+	struct attribute attr;
+	ssize_t (*show)(struct uio_port *, char *);
+	ssize_t (*store)(struct uio_port *, const char *, size_t);
+};
+
+static struct portio_sysfs_entry portio_start_attribute =
+	__ATTR(start, S_IRUGO, portio_start_show, NULL);
+static struct portio_sysfs_entry portio_size_attribute =
+	__ATTR(size, S_IRUGO, portio_size_show, NULL);
+static struct portio_sysfs_entry portio_porttype_attribute =
+	__ATTR(porttype, S_IRUGO, portio_porttype_show, NULL);
+
+static struct attribute *portio_attrs[] = {
+	&portio_start_attribute.attr,
+	&portio_size_attribute.attr,
+	&portio_porttype_attribute.attr,
+	NULL,
+};
+
+static void portio_release(struct kobject *kobj)
+{
+	struct uio_portio *portio = to_portio(kobj);
+	kfree(portio);
+}
+
+static ssize_t portio_type_show(struct kobject *kobj, struct attribute *attr,
+			     char *buf)
+{
+	struct uio_portio *portio = to_portio(kobj);
+	struct uio_port *port = portio->port;
+	struct portio_sysfs_entry *entry;
+
+	entry = container_of(attr, struct portio_sysfs_entry, attr);
+
+	if (!entry->show)
+		return -EIO;
+
+	return entry->show(port, buf);
+}
+
+static struct sysfs_ops portio_sysfs_ops = {
+	.show = portio_type_show,
+};
+
+static struct kobj_type portio_attr_type = {
+	.release	= portio_release,
+	.sysfs_ops	= &portio_sysfs_ops,
+	.default_attrs	= portio_attrs,
+};
+
 static ssize_t show_name(struct device *dev,
 			 struct device_attribute *attr, char *buf)
 {
@@ -177,10 +255,13 @@ static struct attribute_group uio_attr_grp = {
 static int uio_dev_add_attributes(struct uio_device *idev)
 {
 	int ret;
-	int mi;
+	int mi, pi;
 	int map_found = 0;
+	int portio_found = 0;
 	struct uio_mem *mem;
 	struct uio_map *map;
+	struct uio_port *port;
+	struct uio_portio *portio;
 
 	ret = sysfs_create_group(&idev->dev->kobj, &uio_attr_grp);
 	if (ret)
@@ -195,25 +276,58 @@ static int uio_dev_add_attributes(struct uio_device *idev)
 			idev->map_dir = kobject_create_and_add("maps",
 							&idev->dev->kobj);
 			if (!idev->map_dir)
-				goto err;
+				goto err_map;
 		}
 		map = kzalloc(sizeof(*map), GFP_KERNEL);
 		if (!map)
-			goto err;
+			goto err_map;
 		kobject_init(&map->kobj, &map_attr_type);
 		map->mem = mem;
 		mem->map = map;
 		ret = kobject_add(&map->kobj, idev->map_dir, "map%d", mi);
 		if (ret)
-			goto err;
+			goto err_map;
 		ret = kobject_uevent(&map->kobj, KOBJ_ADD);
 		if (ret)
-			goto err;
+			goto err_map;
+	}
+
+	for (pi = 0; pi < MAX_UIO_PORT_REGIONS; pi++) {
+		port = &idev->info->port[pi];
+		if (port->size == 0)
+			break;
+		if (!portio_found) {
+			portio_found = 1;
+			idev->portio_dir = kobject_create_and_add("portio",
+							&idev->dev->kobj);
+			if (!idev->portio_dir)
+				goto err_portio;
+		}
+		portio = kzalloc(sizeof(*portio), GFP_KERNEL);
+		if (!portio)
+			goto err_portio;
+		kobject_init(&portio->kobj, &portio_attr_type);
+		portio->port = port;
+		port->portio = portio;
+		ret = kobject_add(&portio->kobj, idev->portio_dir,
+							"port%d", pi);
+		if (ret)
+			goto err_portio;
+		ret = kobject_uevent(&portio->kobj, KOBJ_ADD);
+		if (ret)
+			goto err_portio;
 	}
 
 	return 0;
 
-err:
+err_portio:
+	for (pi--; pi >= 0; pi--) {
+		port = &idev->info->port[pi];
+		portio = port->portio;
+		kobject_put(&portio->kobj);
+	}
+	kobject_put(idev->portio_dir);
+err_map:
 	for (mi--; mi>=0; mi--) {
 		mem = &idev->info->mem[mi];
 		map = mem->map;
@@ -228,15 +342,26 @@ err_group:
 
 static void uio_dev_del_attributes(struct uio_device *idev)
 {
-	int mi;
+	int i;
 	struct uio_mem *mem;
-	for (mi = 0; mi < MAX_UIO_MAPS; mi++) {
-		mem = &idev->info->mem[mi];
+	struct uio_port *port;
+
+	for (i = 0; i < MAX_UIO_MAPS; i++) {
+		mem = &idev->info->mem[i];
 		if (mem->size == 0)
 			break;
 		kobject_put(&mem->map->kobj);
 	}
 	kobject_put(idev->map_dir);
+
+	for (i = 0; i < MAX_UIO_PORT_REGIONS; i++) {
+		port = &idev->info->port[i];
+		if (port->size == 0)
+			break;
+		kobject_put(&port->portio->kobj);
+	}
+	kobject_put(idev->portio_dir);
+
 	sysfs_remove_group(&idev->dev->kobj, &uio_attr_grp);
 }
 
diff --git a/drivers/uio/uio_cif.c b/drivers/uio/uio_cif.c
index 57376060b97..c60b8fcf0e3 100644
--- a/drivers/uio/uio_cif.c
+++ b/drivers/uio/uio_cif.c
@@ -57,8 +57,7 @@ static int __devinit hilscher_pci_probe(struct pci_dev *dev,
 	info->mem[0].addr = pci_resource_start(dev, 0);
 	if (!info->mem[0].addr)
 		goto out_release;
-	info->mem[0].internal_addr = ioremap(pci_resource_start(dev, 0),
-					     pci_resource_len(dev, 0));
+	info->mem[0].internal_addr = pci_ioremap_bar(dev, 0);
 	if (!info->mem[0].internal_addr)
 		goto out_release;
 
diff --git a/drivers/uio/uio_pdrv_genirq.c b/drivers/uio/uio_pdrv_genirq.c
index 1f82c83a92a..3f06818cf9f 100644
--- a/drivers/uio/uio_pdrv_genirq.c
+++ b/drivers/uio/uio_pdrv_genirq.c
@@ -81,7 +81,8 @@ static int uio_pdrv_genirq_probe(struct platform_device *pdev)
 		goto bad0;
 	}
 
-	if (uioinfo->handler || uioinfo->irqcontrol || uioinfo->irq_flags) {
+	if (uioinfo->handler || uioinfo->irqcontrol ||
+	    uioinfo->irq_flags & IRQF_SHARED) {
 		dev_err(&pdev->dev, "interrupt configuration error\n");
 		goto bad0;
 	}
@@ -132,7 +133,7 @@ static int uio_pdrv_genirq_probe(struct platform_device *pdev)
 	 * Interrupt sharing is not supported.
 	 */
 
-	uioinfo->irq_flags = IRQF_DISABLED;
+	uioinfo->irq_flags |= IRQF_DISABLED;
 	uioinfo->handler = uio_pdrv_genirq_handler;
 	uioinfo->irqcontrol = uio_pdrv_genirq_irqcontrol;
 	uioinfo->priv = priv;
diff --git a/drivers/usb/core/usb.c b/drivers/usb/core/usb.c
index be1fa0723f2..399e15fc505 100644
--- a/drivers/usb/core/usb.c
+++ b/drivers/usb/core/usb.c
@@ -286,7 +286,7 @@ static int usb_dev_restore(struct device *dev)
 	return usb_resume(dev);
 }
 
-static struct pm_ops usb_device_pm_ops = {
+static struct dev_pm_ops usb_device_pm_ops = {
 	.prepare =	usb_dev_prepare,
 	.complete =	usb_dev_complete,
 	.suspend =	usb_dev_suspend,
@@ -301,7 +301,7 @@ static struct pm_ops usb_device_pm_ops = {
 
 #define ksuspend_usb_init()	0
 #define ksuspend_usb_cleanup()	do {} while (0)
-#define usb_device_pm_ops	(*(struct pm_ops *)0)
+#define usb_device_pm_ops	(*(struct dev_pm_ops *)0)
 
 #endif	/* CONFIG_PM */
 
diff --git a/drivers/usb/gadget/at91_udc.c b/drivers/usb/gadget/at91_udc.c
index a8a1de41332..0b2bb8f0706 100644
--- a/drivers/usb/gadget/at91_udc.c
+++ b/drivers/usb/gadget/at91_udc.c
@@ -1474,7 +1474,7 @@ static struct at91_udc controller = {
 		.ep0	= &controller.ep[0].ep,
 		.name	= driver_name,
 		.dev	= {
-			.bus_id = "gadget",
+			.init_name = "gadget",
 			.release = nop_release,
 		}
 	},
diff --git a/drivers/usb/gadget/atmel_usba_udc.c b/drivers/usb/gadget/atmel_usba_udc.c
index ae30ab1d264..65b03e3445a 100644
--- a/drivers/usb/gadget/atmel_usba_udc.c
+++ b/drivers/usb/gadget/atmel_usba_udc.c
@@ -1034,7 +1034,7 @@ static struct usba_udc the_udc = {
 		.is_dualspeed	= 1,
 		.name		= "atmel_usba_udc",
 		.dev	= {
-			.bus_id		= "gadget",
+			.init_name	= "gadget",
 			.release	= nop_release,
 		},
 	},
diff --git a/drivers/usb/gadget/fsl_qe_udc.c b/drivers/usb/gadget/fsl_qe_udc.c
index b3408ff39fb..f4027256509 100644
--- a/drivers/usb/gadget/fsl_qe_udc.c
+++ b/drivers/usb/gadget/fsl_qe_udc.c
@@ -2545,7 +2545,7 @@ static int __devinit qe_udc_probe(struct of_device *ofdev,
 
 	device_initialize(&udc_controller->gadget.dev);
 
-	strcpy(udc_controller->gadget.dev.bus_id, "gadget");
+	dev_set_name(&udc_controller->gadget.dev, "gadget");
 
 	udc_controller->gadget.dev.release = qe_udc_release;
 	udc_controller->gadget.dev.parent = &ofdev->dev;
diff --git a/drivers/usb/gadget/lh7a40x_udc.c b/drivers/usb/gadget/lh7a40x_udc.c
index c6e7df04c69..d554b089560 100644
--- a/drivers/usb/gadget/lh7a40x_udc.c
+++ b/drivers/usb/gadget/lh7a40x_udc.c
@@ -1981,7 +1981,7 @@ static struct lh7a40x_udc memory = {
 		   .ep0 = &memory.ep[0].ep,
 		   .name = driver_name,
 		   .dev = {
-			   .bus_id = "gadget",
+			   .init_name = "gadget",
 			   .release = nop_release,
 			   },
 		   },
diff --git a/drivers/usb/gadget/pxa25x_udc.c b/drivers/usb/gadget/pxa25x_udc.c
index 8c5026be79d..697a0ca349b 100644
--- a/drivers/usb/gadget/pxa25x_udc.c
+++ b/drivers/usb/gadget/pxa25x_udc.c
@@ -1833,7 +1833,7 @@ static struct pxa25x_udc memory = {
 		.ep0		= &memory.ep[0].ep,
 		.name		= driver_name,
 		.dev = {
-			.bus_id		= "gadget",
+			.init_name	= "gadget",
 			.release	= nop_release,
 		},
 	},
diff --git a/drivers/usb/gadget/pxa27x_udc.c b/drivers/usb/gadget/pxa27x_udc.c
index 944e4ff641d..65110d02a20 100644
--- a/drivers/usb/gadget/pxa27x_udc.c
+++ b/drivers/usb/gadget/pxa27x_udc.c
@@ -2162,7 +2162,7 @@ static struct pxa_udc memory = {
 		.ep0		= &memory.udc_usb_ep[0].usb_ep,
 		.name		= driver_name,
 		.dev = {
-			.bus_id		= "gadget",
+			.init_name	= "gadget",
 		},
 	},
 
diff --git a/drivers/usb/gadget/s3c2410_udc.c b/drivers/usb/gadget/s3c2410_udc.c
index 8d8d6516598..c7e25563680 100644
--- a/drivers/usb/gadget/s3c2410_udc.c
+++ b/drivers/usb/gadget/s3c2410_udc.c
@@ -1727,7 +1727,7 @@ static struct s3c2410_udc memory = {
 		.ep0		= &memory.ep[0].ep,
 		.name		= gadget_name,
 		.dev = {
-			.bus_id		= "gadget",
+			.init_name	= "gadget",
 		},
 	},
 
diff --git a/drivers/video/aty/aty128fb.c b/drivers/video/aty/aty128fb.c
index 243ea4ab20c..db16112cf19 100644
--- a/drivers/video/aty/aty128fb.c
+++ b/drivers/video/aty/aty128fb.c
@@ -2051,7 +2051,7 @@ static int __devinit aty128_probe(struct pci_dev *pdev, const struct pci_device_
 
 	/* Virtualize mmio region */
 	info->fix.mmio_start = reg_addr;
-	par->regbase = ioremap(reg_addr, pci_resource_len(pdev, 2));
+	par->regbase = pci_ioremap_bar(pdev, 2);
 	if (!par->regbase)
 		goto err_free_info;
 
diff --git a/drivers/video/backlight/backlight.c b/drivers/video/backlight/backlight.c
index fab0bc874b5..0664fc03223 100644
--- a/drivers/video/backlight/backlight.c
+++ b/drivers/video/backlight/backlight.c
@@ -217,7 +217,7 @@ struct backlight_device *backlight_device_register(const char *name,
 	new_bd->dev.class = backlight_class;
 	new_bd->dev.parent = parent;
 	new_bd->dev.release = bl_device_release;
-	strlcpy(new_bd->dev.bus_id, name, BUS_ID_SIZE);
+	dev_set_name(&new_bd->dev, name);
 	dev_set_drvdata(&new_bd->dev, devdata);
 
 	rc = device_register(&new_bd->dev);
diff --git a/drivers/video/backlight/lcd.c b/drivers/video/backlight/lcd.c
index 680e57b616c..b6449470106 100644
--- a/drivers/video/backlight/lcd.c
+++ b/drivers/video/backlight/lcd.c
@@ -208,7 +208,7 @@ struct lcd_device *lcd_device_register(const char *name, struct device *parent,
 	new_ld->dev.class = lcd_class;
 	new_ld->dev.parent = parent;
 	new_ld->dev.release = lcd_device_release;
-	strlcpy(new_ld->dev.bus_id, name, BUS_ID_SIZE);
+	dev_set_name(&new_ld->dev, name);
 	dev_set_drvdata(&new_ld->dev, devdata);
 
 	rc = device_register(&new_ld->dev);
diff --git a/drivers/video/bfin-t350mcqb-fb.c b/drivers/video/bfin-t350mcqb-fb.c
index 7d1b819e501..a9b3ada05d9 100644
--- a/drivers/video/bfin-t350mcqb-fb.c
+++ b/drivers/video/bfin-t350mcqb-fb.c
@@ -255,7 +255,7 @@ static int bfin_t350mcqb_fb_check_var(struct fb_var_screeninfo *var,
 {
 
 	if (var->bits_per_pixel != LCD_BPP) {
-		pr_debug("%s: depth not supported: %u BPP\n", __FUNCTION__,
+		pr_debug("%s: depth not supported: %u BPP\n", __func__,
 			 var->bits_per_pixel);
 		return -EINVAL;
 	}
@@ -264,7 +264,7 @@ static int bfin_t350mcqb_fb_check_var(struct fb_var_screeninfo *var,
 	    info->var.xres_virtual != var->xres_virtual ||
 	    info->var.yres_virtual != var->yres_virtual) {
 		pr_debug("%s: Resolution not supported: X%u x Y%u \n",
-			 __FUNCTION__, var->xres, var->yres);
+			 __func__, var->xres, var->yres);
 		return -EINVAL;
 	}
 
@@ -274,7 +274,7 @@ static int bfin_t350mcqb_fb_check_var(struct fb_var_screeninfo *var,
 
 	if ((info->fix.line_length * var->yres_virtual) > info->fix.smem_len) {
 		pr_debug("%s: Memory Limit requested yres_virtual = %u\n",
-			 __FUNCTION__, var->yres_virtual);
+			 __func__, var->yres_virtual);
 		return -ENOMEM;
 	}
 
diff --git a/drivers/video/carminefb.c b/drivers/video/carminefb.c
index c9b191319a9..c7ff3c1a266 100644
--- a/drivers/video/carminefb.c
+++ b/drivers/video/carminefb.c
@@ -168,7 +168,7 @@ static int carmine_setcolreg(unsigned regno, unsigned red, unsigned green,
 	blue >>= 8;
 	transp >>= 8;
 
-	((u32 *)info->pseudo_palette)[regno] = be32_to_cpu(transp << 24 |
+	((__be32 *)info->pseudo_palette)[regno] = cpu_to_be32(transp << 24 |
 		red << 0 | green << 8 | blue << 16);
 	return 0;
 }
diff --git a/drivers/video/cyber2000fb.c b/drivers/video/cyber2000fb.c
index 39d5d643a50..7a9e42e3a9a 100644
--- a/drivers/video/cyber2000fb.c
+++ b/drivers/video/cyber2000fb.c
@@ -1583,8 +1583,7 @@ cyberpro_pci_probe(struct pci_dev *dev, const struct pci_device_id *id)
 		goto failed_release;
 
 	cfb->dev = dev;
-	cfb->region = ioremap(pci_resource_start(dev, 0),
-			      pci_resource_len(dev, 0));
+	cfb->region = pci_ioremap_bar(dev, 0);
 	if (!cfb->region)
 		goto failed_ioremap;
 
diff --git a/drivers/video/fbmem.c b/drivers/video/fbmem.c
index 3c65b0d6761..756efeb91ab 100644
--- a/drivers/video/fbmem.c
+++ b/drivers/video/fbmem.c
@@ -510,6 +510,10 @@ static int fb_prepare_extra_logos(struct fb_info *info, unsigned int height,
 		fb_logo_ex_num = 0;
 
 	for (i = 0; i < fb_logo_ex_num; i++) {
+		if (fb_logo_ex[i].logo->type != fb_logo.logo->type) {
+			fb_logo_ex[i].logo = NULL;
+			continue;
+		}
 		height += fb_logo_ex[i].logo->height;
 		if (height > yres) {
 			height -= fb_logo_ex[i].logo->height;
diff --git a/drivers/video/gbefb.c b/drivers/video/gbefb.c
index f89c3cce1e0..fe5b519860b 100644
--- a/drivers/video/gbefb.c
+++ b/drivers/video/gbefb.c
@@ -912,6 +912,7 @@ static int gbefb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
 {
 	unsigned int line_length;
 	struct gbe_timing_info timing;
+	int ret;
 
 	/* Limit bpp to 8, 16, and 32 */
 	if (var->bits_per_pixel <= 8)
@@ -930,8 +931,10 @@ static int gbefb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
 
 	var->grayscale = 0;	/* No grayscale for now */
 
-	if ((var->pixclock = compute_gbe_timing(var, &timing)) < 0)
-		return(-EINVAL);
+	ret = compute_gbe_timing(var, &timing);
+	var->pixclock = ret;
+	if (ret < 0)
+		return -EINVAL;
 
 	/* Adjust virtual resolution, if necessary */
 	if (var->xres > var->xres_virtual || (!ywrap && !ypan))
diff --git a/drivers/video/geode/gx1fb_core.c b/drivers/video/geode/gx1fb_core.c
index bb20a228976..751e491ca8c 100644
--- a/drivers/video/geode/gx1fb_core.c
+++ b/drivers/video/geode/gx1fb_core.c
@@ -217,8 +217,7 @@ static int __init gx1fb_map_video_memory(struct fb_info *info, struct pci_dev *d
 	ret = pci_request_region(dev, 0, "gx1fb (video)");
 	if (ret < 0)
 		return ret;
-	par->vid_regs = ioremap(pci_resource_start(dev, 0),
-				pci_resource_len(dev, 0));
+	par->vid_regs = pci_ioremap_bar(dev, 0);
 	if (!par->vid_regs)
 		return -ENOMEM;
 
diff --git a/drivers/video/geode/gxfb_core.c b/drivers/video/geode/gxfb_core.c
index de2b8f9876a..48411892631 100644
--- a/drivers/video/geode/gxfb_core.c
+++ b/drivers/video/geode/gxfb_core.c
@@ -242,23 +242,21 @@ static int __init gxfb_map_video_memory(struct fb_info *info, struct pci_dev *de
 	ret = pci_request_region(dev, 3, "gxfb (video processor)");
 	if (ret < 0)
 		return ret;
-	par->vid_regs = ioremap(pci_resource_start(dev, 3),
-				pci_resource_len(dev, 3));
+	par->vid_regs = pci_ioremap_bar(dev, 3);
 	if (!par->vid_regs)
 		return -ENOMEM;
 
 	ret = pci_request_region(dev, 2, "gxfb (display controller)");
 	if (ret < 0)
 		return ret;
-	par->dc_regs = ioremap(pci_resource_start(dev, 2), pci_resource_len(dev, 2));
+	par->dc_regs = pci_ioremap_bar(dev, 2);
 	if (!par->dc_regs)
 		return -ENOMEM;
 
 	ret = pci_request_region(dev, 1, "gxfb (graphics processor)");
 	if (ret < 0)
 		return ret;
-	par->gp_regs = ioremap(pci_resource_start(dev, 1),
-	pci_resource_len(dev, 1));
+	par->gp_regs = pci_ioremap_bar(dev, 1);
 
 	if (!par->gp_regs)
 		return -ENOMEM;
diff --git a/drivers/video/geode/lxfb_core.c b/drivers/video/geode/lxfb_core.c
index 2cd9b74d222..b965ecdbc60 100644
--- a/drivers/video/geode/lxfb_core.c
+++ b/drivers/video/geode/lxfb_core.c
@@ -379,20 +379,17 @@ static int __init lxfb_map_video_memory(struct fb_info *info,
 	if (info->screen_base == NULL)
 		return ret;
 
-	par->gp_regs = ioremap(pci_resource_start(dev, 1),
-				pci_resource_len(dev, 1));
+	par->gp_regs = pci_ioremap_bar(dev, 1);
 
 	if (par->gp_regs == NULL)
 		return ret;
 
-	par->dc_regs = ioremap(pci_resource_start(dev, 2),
-			       pci_resource_len(dev, 2));
+	par->dc_regs = pci_ioremap_bar(dev, 2);
 
 	if (par->dc_regs == NULL)
 		return ret;
 
-	par->vp_regs = ioremap(pci_resource_start(dev, 3),
-			       pci_resource_len(dev, 3));
+	par->vp_regs = pci_ioremap_bar(dev, 3);
 
 	if (par->vp_regs == NULL)
 		return ret;
diff --git a/drivers/video/gxt4500.c b/drivers/video/gxt4500.c
index 564557792be..896e53dea90 100644
--- a/drivers/video/gxt4500.c
+++ b/drivers/video/gxt4500.c
@@ -648,7 +648,7 @@ static int __devinit gxt4500_probe(struct pci_dev *pdev,
 	info->pseudo_palette = par->pseudo_palette;
 
 	info->fix.mmio_start = reg_phys;
-	par->regs = ioremap(reg_phys, pci_resource_len(pdev, 0));
+	par->regs = pci_ioremap_bar(pdev, 0);
 	if (!par->regs) {
 		dev_err(&pdev->dev, "gxt4500: cannot map registers\n");
 		goto err_free_all;
@@ -656,7 +656,7 @@ static int __devinit gxt4500_probe(struct pci_dev *pdev,
 
 	info->fix.smem_start = fb_phys;
 	info->fix.smem_len = pci_resource_len(pdev, 1);
-	info->screen_base = ioremap(fb_phys, pci_resource_len(pdev, 1));
+	info->screen_base = pci_ioremap_bar(pdev, 1);
 	if (!info->screen_base) {
 		dev_err(&pdev->dev, "gxt4500: cannot map framebuffer\n");
 		goto err_unmap_regs;
diff --git a/drivers/video/i810/i810_accel.c b/drivers/video/i810/i810_accel.c
index 76764ea3486..f5bedee4310 100644
--- a/drivers/video/i810/i810_accel.c
+++ b/drivers/video/i810/i810_accel.c
@@ -301,8 +301,10 @@ void i810fb_fillrect(struct fb_info *info, const struct fb_fillrect *rect)
 	u32 dx, dy, width, height, dest, rop = 0, color = 0;
 
 	if (!info->var.accel_flags || par->dev_flags & LOCKUP ||
-	    par->depth == 4) 
-		return cfb_fillrect(info, rect);
+	    par->depth == 4) {
+		cfb_fillrect(info, rect);
+		return;
+	}
 
 	if (par->depth == 1) 
 		color = rect->color;
@@ -327,8 +329,10 @@ void i810fb_copyarea(struct fb_info *info, const struct fb_copyarea *region)
 	u32 sx, sy, dx, dy, pitch, width, height, src, dest, xdir;
 
 	if (!info->var.accel_flags || par->dev_flags & LOCKUP ||
-	    par->depth == 4)
-		return cfb_copyarea(info, region);
+	    par->depth == 4) {
+		cfb_copyarea(info, region);
+		return;
+	}
 
 	dx = region->dx * par->depth;
 	sx = region->sx * par->depth;
@@ -366,8 +370,10 @@ void i810fb_imageblit(struct fb_info *info, const struct fb_image *image)
 	u32 fg = 0, bg = 0, size, dst;
 	
 	if (!info->var.accel_flags || par->dev_flags & LOCKUP ||
-	    par->depth == 4 || image->depth != 1) 
-		return cfb_imageblit(info, image);
+	    par->depth == 4 || image->depth != 1) {
+		cfb_imageblit(info, image);
+		return;
+	}
 
 	switch (info->var.bits_per_pixel) {
 	case 8:
diff --git a/drivers/video/intelfb/intelfbdrv.c b/drivers/video/intelfb/intelfbdrv.c
index a09e2364935..6d8e5415c80 100644
--- a/drivers/video/intelfb/intelfbdrv.c
+++ b/drivers/video/intelfb/intelfbdrv.c
@@ -1493,8 +1493,10 @@ static void intelfb_fillrect (struct fb_info *info,
 	DBG_MSG("intelfb_fillrect\n");
 #endif
 
-	if (!ACCEL(dinfo, info) || dinfo->depth == 4)
-		return cfb_fillrect(info, rect);
+	if (!ACCEL(dinfo, info) || dinfo->depth == 4) {
+		cfb_fillrect(info, rect);
+		return;
+	}
 
 	if (rect->rop == ROP_COPY)
 		rop = PAT_ROP_GXCOPY;
@@ -1521,8 +1523,10 @@ static void intelfb_copyarea(struct fb_info *info,
 	DBG_MSG("intelfb_copyarea\n");
 #endif
 
-	if (!ACCEL(dinfo, info) || dinfo->depth == 4)
-		return cfb_copyarea(info, region);
+	if (!ACCEL(dinfo, info) || dinfo->depth == 4) {
+		cfb_copyarea(info, region);
+		return;
+	}
 
 	intelfbhw_do_bitblt(dinfo, region->sx, region->sy, region->dx,
 			    region->dy, region->width, region->height,
@@ -1540,8 +1544,10 @@ static void intelfb_imageblit(struct fb_info *info,
 #endif
 
 	if (!ACCEL(dinfo, info) || dinfo->depth == 4
-	    || image->depth != 1)
-		return cfb_imageblit(info, image);
+	    || image->depth != 1) {
+		cfb_imageblit(info, image);
+		return;
+	}
 
 	if (dinfo->depth != 8) {
 		fgcolor = dinfo->pseudo_palette[image->fg_color];
@@ -1554,8 +1560,10 @@ static void intelfb_imageblit(struct fb_info *info,
 	if (!intelfbhw_do_drawglyph(dinfo, fgcolor, bgcolor, image->width,
 				    image->height, image->data,
 				    image->dx, image->dy,
-				    dinfo->pitch, info->var.bits_per_pixel))
-		return cfb_imageblit(info, image);
+				    dinfo->pitch, info->var.bits_per_pixel)) {
+		cfb_imageblit(info, image);
+		return;
+	}
 }
 
 static int intelfb_cursor(struct fb_info *info, struct fb_cursor *cursor)
diff --git a/drivers/video/modedb.c b/drivers/video/modedb.c
index d3c3af53a29..16186240c5f 100644
--- a/drivers/video/modedb.c
+++ b/drivers/video/modedb.c
@@ -329,7 +329,7 @@ const struct fb_videomode vesa_modes[] = {
 	  FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
 	  FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
 	/* 17 1152x864-75 VESA */
-	{ NULL, 75, 1153, 864, 9259, 256, 64, 32, 1, 128, 3,
+	{ NULL, 75, 1152, 864, 9259, 256, 64, 32, 1, 128, 3,
 	  FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
 	  FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA },
 	/* 18 1280x960-60 VESA */
diff --git a/drivers/video/neofb.c b/drivers/video/neofb.c
index bfb802d26d5..588527a254c 100644
--- a/drivers/video/neofb.c
+++ b/drivers/video/neofb.c
@@ -1453,7 +1453,8 @@ neo2200_imageblit(struct fb_info *info, const struct fb_image *image)
 			 * is less than 16 bits wide. This is due to insufficient
 			 * padding when writing the image. We need to adjust
 			 * struct fb_pixmap. Not yet done. */
-			return cfb_imageblit(info, image);
+			cfb_imageblit(info, image);
+			return;
 		}
 		bltCntl_flags = NEO_BC0_SRC_MONO;
 	} else if (image->depth == info->var.bits_per_pixel) {
@@ -1461,7 +1462,8 @@ neo2200_imageblit(struct fb_info *info, const struct fb_image *image)
 	} else {
 		/* We don't currently support hardware acceleration if image
 		 * depth is different from display */
-		return cfb_imageblit(info, image);
+		cfb_imageblit(info, image);
+		return;
 	}
 
 	switch (info->var.bits_per_pixel) {
diff --git a/drivers/video/nvidia/nv_accel.c b/drivers/video/nvidia/nv_accel.c
index fa4821c5572..ad6472a894e 100644
--- a/drivers/video/nvidia/nv_accel.c
+++ b/drivers/video/nvidia/nv_accel.c
@@ -300,8 +300,10 @@ void nvidiafb_copyarea(struct fb_info *info, const struct fb_copyarea *region)
 	if (info->state != FBINFO_STATE_RUNNING)
 		return;
 
-	if (par->lockup)
-		return cfb_copyarea(info, region);
+	if (par->lockup) {
+		cfb_copyarea(info, region);
+		return;
+	}
 
 	NVDmaStart(info, par, BLIT_POINT_SRC, 3);
 	NVDmaNext(par, (region->sy << 16) | region->sx);
@@ -319,8 +321,10 @@ void nvidiafb_fillrect(struct fb_info *info, const struct fb_fillrect *rect)
 	if (info->state != FBINFO_STATE_RUNNING)
 		return;
 
-	if (par->lockup)
-		return cfb_fillrect(info, rect);
+	if (par->lockup) {
+		cfb_fillrect(info, rect);
+		return;
+	}
 
 	if (info->var.bits_per_pixel == 8)
 		color = rect->color;
diff --git a/drivers/video/output.c b/drivers/video/output.c
index f2df5519c9c..5e6439ae739 100644
--- a/drivers/video/output.c
+++ b/drivers/video/output.c
@@ -96,7 +96,7 @@ struct output_device *video_output_register(const char *name,
 	new_dev->props = op;
 	new_dev->dev.class = &video_output_class;
 	new_dev->dev.parent = dev;
-	strlcpy(new_dev->dev.bus_id,name, BUS_ID_SIZE);
+	dev_set_name(&new_dev->dev, name);
 	dev_set_drvdata(&new_dev->dev, devdata);
 	ret_code = device_register(&new_dev->dev);
 	if (ret_code) {
diff --git a/drivers/video/pm3fb.c b/drivers/video/pm3fb.c
index 68089d1456c..6666f45a2f8 100644
--- a/drivers/video/pm3fb.c
+++ b/drivers/video/pm3fb.c
@@ -539,8 +539,10 @@ static void pm3fb_imageblit(struct fb_info *info, const struct fb_image *image)
 		bgx = par->palette[image->bg_color];
 		break;
 	}
-	if (image->depth != 1)
-		return cfb_imageblit(info, image);
+	if (image->depth != 1) {
+		cfb_imageblit(info, image);
+		return;
+	}
 
 	if (info->var.bits_per_pixel == 8) {
 		fgx |= fgx << 8;
diff --git a/drivers/video/sm501fb.c b/drivers/video/sm501fb.c
index f94ae84a58c..dcd98793d56 100644
--- a/drivers/video/sm501fb.c
+++ b/drivers/video/sm501fb.c
@@ -159,6 +159,9 @@ static int sm501_alloc_mem(struct sm501fb_info *inf, struct sm501_mem *mem,
 		break;
 
 	case SM501_MEMF_PANEL:
+		if (size > inf->fbmem_len)
+			return -ENOMEM;
+
 		ptr = inf->fbmem_len - size;
 		fbi = inf->fb[HEAD_CRT];
 
@@ -172,9 +175,6 @@ static int sm501_alloc_mem(struct sm501fb_info *inf, struct sm501_mem *mem,
 		if (fbi && ptr < fbi->fix.smem_len)
 			return -ENOMEM;
 
-		if (ptr < 0)
-			return -ENOMEM;
-
 		break;
 
 	case SM501_MEMF_CRT:
diff --git a/drivers/video/via/viafbdev.c b/drivers/video/via/viafbdev.c
index e21fe5b6f9f..37b433a08ce 100644
--- a/drivers/video/via/viafbdev.c
+++ b/drivers/video/via/viafbdev.c
@@ -870,8 +870,10 @@ static void viafb_fillrect(struct fb_info *info,
 	u32 col = 0, rop = 0;
 	int pitch;
 
-	if (!viafb_accel)
-		return cfb_fillrect(info, rect);
+	if (!viafb_accel) {
+		cfb_fillrect(info, rect);
+		return;
+	}
 
 	if (!rect->width || !rect->height)
 		return;
@@ -937,8 +939,10 @@ static void viafb_copyarea(struct fb_info *info,
 
 	DEBUG_MSG(KERN_INFO "viafb_copyarea!!\n");
 
-	if (!viafb_accel)
-		return cfb_copyarea(info, area);
+	if (!viafb_accel) {
+		cfb_copyarea(info, area);
+		return;
+	}
 
 	if (!area->width || !area->height)
 		return;
@@ -994,8 +998,10 @@ static void viafb_imageblit(struct fb_info *info,
 	int i;
 	int pitch;
 
-	if (!viafb_accel)
-		return cfb_imageblit(info, image);
+	if (!viafb_accel) {
+		cfb_imageblit(info, image);
+		return;
+	}
 
 	udata = (u32 *) image->data;
 
diff --git a/drivers/virtio/virtio_pci.c b/drivers/virtio/virtio_pci.c
index 265fdf2d127..bef6b45e8a5 100644
--- a/drivers/virtio/virtio_pci.c
+++ b/drivers/virtio/virtio_pci.c
@@ -73,10 +73,7 @@ MODULE_DEVICE_TABLE(pci, virtio_pci_id_table);
 /* A PCI device has it's own struct device and so does a virtio device so
  * we create a place for the virtio devices to show up in sysfs.  I think it
  * would make more sense for virtio to not insist on having it's own device. */
-static struct device virtio_pci_root = {
-	.parent		= NULL,
-	.init_name	= "virtio-pci",
-};
+static struct device *virtio_pci_root;
 
 /* Convert a generic virtio device to our structure */
 static struct virtio_pci_device *to_vp_device(struct virtio_device *vdev)
@@ -343,7 +340,7 @@ static int __devinit virtio_pci_probe(struct pci_dev *pci_dev,
 	if (vp_dev == NULL)
 		return -ENOMEM;
 
-	vp_dev->vdev.dev.parent = &virtio_pci_root;
+	vp_dev->vdev.dev.parent = virtio_pci_root;
 	vp_dev->vdev.dev.release = virtio_pci_release_dev;
 	vp_dev->vdev.config = &virtio_pci_config_ops;
 	vp_dev->pci_dev = pci_dev;
@@ -437,13 +434,13 @@ static int __init virtio_pci_init(void)
 {
 	int err;
 
-	err = device_register(&virtio_pci_root);
-	if (err)
-		return err;
+	virtio_pci_root = root_device_register("virtio-pci");
+	if (IS_ERR(virtio_pci_root))
+		return PTR_ERR(virtio_pci_root);
 
 	err = pci_register_driver(&virtio_pci_driver);
 	if (err)
-		device_unregister(&virtio_pci_root);
+		device_unregister(virtio_pci_root);
 
 	return err;
 }
@@ -452,8 +449,8 @@ module_init(virtio_pci_init);
 
 static void __exit virtio_pci_exit(void)
 {
-	device_unregister(&virtio_pci_root);
 	pci_unregister_driver(&virtio_pci_driver);
+	root_device_unregister(virtio_pci_root);
 }
 
 module_exit(virtio_pci_exit);
diff --git a/drivers/w1/w1.c b/drivers/w1/w1.c
index 3b615d4022e..acc7e3b7fe1 100644
--- a/drivers/w1/w1.c
+++ b/drivers/w1/w1.c
@@ -197,7 +197,7 @@ struct device_driver w1_master_driver = {
 struct device w1_master_device = {
 	.parent = NULL,
 	.bus = &w1_bus_type,
-	.bus_id = "w1 bus master",
+	.init_name = "w1 bus master",
 	.driver = &w1_master_driver,
 	.release = &w1_master_release
 };
@@ -211,7 +211,7 @@ static struct device_driver w1_slave_driver = {
 struct device w1_slave_device = {
 	.parent = NULL,
 	.bus = &w1_bus_type,
-	.bus_id = "w1 bus slave",
+	.init_name = "w1 bus slave",
 	.driver = &w1_slave_driver,
 	.release = &w1_slave_release
 };
@@ -573,7 +573,7 @@ static int w1_uevent(struct device *dev, struct kobj_uevent_env *env)
 	}
 
 	dev_dbg(dev, "Hotplug event for %s %s, bus_id=%s.\n",
-			event_owner, name, dev->bus_id);
+			event_owner, name, dev_name(dev));
 
 	if (dev->driver != &w1_slave_driver || !sl)
 		return 0;
@@ -605,8 +605,7 @@ static int __w1_attach_slave_device(struct w1_slave *sl)
 	sl->dev.bus = &w1_bus_type;
 	sl->dev.release = &w1_slave_release;
 
-	snprintf(&sl->dev.bus_id[0], sizeof(sl->dev.bus_id),
-		 "%02x-%012llx",
+	dev_set_name(&sl->dev, "%02x-%012llx",
 		 (unsigned int) sl->reg_num.family,
 		 (unsigned long long) sl->reg_num.id);
 	snprintf(&sl->name[0], sizeof(sl->name),
@@ -615,13 +614,13 @@ static int __w1_attach_slave_device(struct w1_slave *sl)
 		 (unsigned long long) sl->reg_num.id);
 
 	dev_dbg(&sl->dev, "%s: registering %s as %p.\n", __func__,
-		&sl->dev.bus_id[0], sl);
+		dev_name(&sl->dev), sl);
 
 	err = device_register(&sl->dev);
 	if (err < 0) {
 		dev_err(&sl->dev,
 			"Device registration [%s] failed. err=%d\n",
-			sl->dev.bus_id, err);
+			dev_name(&sl->dev), err);
 		return err;
 	}
 
@@ -630,7 +629,7 @@ static int __w1_attach_slave_device(struct w1_slave *sl)
 	if (err < 0) {
 		dev_err(&sl->dev,
 			"sysfs file creation for [%s] failed. err=%d\n",
-			sl->dev.bus_id, err);
+			dev_name(&sl->dev), err);
 		goto out_unreg;
 	}
 
@@ -639,7 +638,7 @@ static int __w1_attach_slave_device(struct w1_slave *sl)
 	if (err < 0) {
 		dev_err(&sl->dev,
 			"sysfs file creation for [%s] failed. err=%d\n",
-			sl->dev.bus_id, err);
+			dev_name(&sl->dev), err);
 		goto out_rem1;
 	}
 
@@ -648,7 +647,7 @@ static int __w1_attach_slave_device(struct w1_slave *sl)
 	    ((err = sl->family->fops->add_slave(sl)) < 0)) {
 		dev_err(&sl->dev,
 			"sysfs file creation for [%s] failed. err=%d\n",
-			sl->dev.bus_id, err);
+			dev_name(&sl->dev), err);
 		goto out_rem2;
 	}
 
diff --git a/drivers/w1/w1_int.c b/drivers/w1/w1_int.c
index a3a54567bfb..4a46ed58ece 100644
--- a/drivers/w1/w1_int.c
+++ b/drivers/w1/w1_int.c
@@ -75,8 +75,7 @@ static struct w1_master * w1_alloc_dev(u32 id, int slave_count, int slave_ttl,
 	mutex_init(&dev->mutex);
 
 	memcpy(&dev->dev, device, sizeof(struct device));
-	snprintf(dev->dev.bus_id, sizeof(dev->dev.bus_id),
-		  "w1_bus_master%u", dev->id);
+	dev_set_name(&dev->dev, "w1_bus_master%u", dev->id);
 	snprintf(dev->name, sizeof(dev->name), "w1_bus_master%u", dev->id);
 
 	dev->driver = driver;
diff --git a/drivers/xen/xenbus/xenbus_probe.c b/drivers/xen/xenbus/xenbus_probe.c
index 7f24a98a446..b2a03184a24 100644
--- a/drivers/xen/xenbus/xenbus_probe.c
+++ b/drivers/xen/xenbus/xenbus_probe.c
@@ -99,15 +99,15 @@ static int xenbus_uevent(struct device *_dev, struct kobj_uevent_env *env)
 }
 
 /* device/<type>/<id> => <type>-<id> */
-static int frontend_bus_id(char bus_id[BUS_ID_SIZE], const char *nodename)
+static int frontend_bus_id(char bus_id[XEN_BUS_ID_SIZE], const char *nodename)
 {
 	nodename = strchr(nodename, '/');
-	if (!nodename || strlen(nodename + 1) >= BUS_ID_SIZE) {
+	if (!nodename || strlen(nodename + 1) >= XEN_BUS_ID_SIZE) {
 		printk(KERN_WARNING "XENBUS: bad frontend %s\n", nodename);
 		return -EINVAL;
 	}
 
-	strlcpy(bus_id, nodename + 1, BUS_ID_SIZE);
+	strlcpy(bus_id, nodename + 1, XEN_BUS_ID_SIZE);
 	if (!strchr(bus_id, '/')) {
 		printk(KERN_WARNING "XENBUS: bus_id %s no slash\n", bus_id);
 		return -EINVAL;
@@ -460,6 +460,7 @@ int xenbus_probe_node(struct xen_bus_type *bus,
 		      const char *type,
 		      const char *nodename)
 {
+	char devname[XEN_BUS_ID_SIZE];
 	int err;
 	struct xenbus_device *xendev;
 	size_t stringlen;
@@ -494,10 +495,12 @@ int xenbus_probe_node(struct xen_bus_type *bus,
 	xendev->dev.bus = &bus->bus;
 	xendev->dev.release = xenbus_dev_release;
 
-	err = bus->get_bus_id(xendev->dev.bus_id, xendev->nodename);
+	err = bus->get_bus_id(devname, xendev->nodename);
 	if (err)
 		goto fail;
 
+	dev_set_name(&xendev->dev, devname);
+
 	/* Register with generic device framework. */
 	err = device_register(&xendev->dev);
 	if (err)
@@ -611,7 +614,7 @@ void xenbus_dev_changed(const char *node, struct xen_bus_type *bus)
 {
 	int exists, rootlen;
 	struct xenbus_device *dev;
-	char type[BUS_ID_SIZE];
+	char type[XEN_BUS_ID_SIZE];
 	const char *p, *root;
 
 	if (char_count(node, '/') < 2)
@@ -625,8 +628,8 @@ void xenbus_dev_changed(const char *node, struct xen_bus_type *bus)
 
 	/* backend/<type>/... or device/<type>/... */
 	p = strchr(node, '/') + 1;
-	snprintf(type, BUS_ID_SIZE, "%.*s", (int)strcspn(p, "/"), p);
-	type[BUS_ID_SIZE-1] = '\0';
+	snprintf(type, XEN_BUS_ID_SIZE, "%.*s", (int)strcspn(p, "/"), p);
+	type[XEN_BUS_ID_SIZE-1] = '\0';
 
 	rootlen = strsep_len(node, '/', bus->levels);
 	if (rootlen < 0)
@@ -674,7 +677,7 @@ static int suspend_dev(struct device *dev, void *data)
 		err = drv->suspend(xdev);
 	if (err)
 		printk(KERN_WARNING
-		       "xenbus: suspend %s failed: %i\n", dev->bus_id, err);
+		       "xenbus: suspend %s failed: %i\n", dev_name(dev), err);
 	return 0;
 }
 
@@ -695,7 +698,7 @@ static int suspend_cancel_dev(struct device *dev, void *data)
 	if (err)
 		printk(KERN_WARNING
 		       "xenbus: suspend_cancel %s failed: %i\n",
-		       dev->bus_id, err);
+		       dev_name(dev), err);
 	return 0;
 }
 
@@ -717,7 +720,7 @@ static int resume_dev(struct device *dev, void *data)
 	if (err) {
 		printk(KERN_WARNING
 		       "xenbus: resume (talk_to_otherend) %s failed: %i\n",
-		       dev->bus_id, err);
+		       dev_name(dev), err);
 		return err;
 	}
 
@@ -728,7 +731,7 @@ static int resume_dev(struct device *dev, void *data)
 		if (err) {
 			printk(KERN_WARNING
 			       "xenbus: resume %s failed: %i\n",
-			       dev->bus_id, err);
+			       dev_name(dev), err);
 			return err;
 		}
 	}
@@ -737,7 +740,7 @@ static int resume_dev(struct device *dev, void *data)
 	if (err) {
 		printk(KERN_WARNING
 		       "xenbus_probe: resume (watch_otherend) %s failed: "
-		       "%d.\n", dev->bus_id, err);
+		       "%d.\n", dev_name(dev), err);
 		return err;
 	}
 
diff --git a/drivers/xen/xenbus/xenbus_probe.h b/drivers/xen/xenbus/xenbus_probe.h
index e09b19415a4..6c5e3185a6a 100644
--- a/drivers/xen/xenbus/xenbus_probe.h
+++ b/drivers/xen/xenbus/xenbus_probe.h
@@ -34,6 +34,8 @@
 #ifndef _XENBUS_PROBE_H
 #define _XENBUS_PROBE_H
 
+#define XEN_BUS_ID_SIZE			20
+
 #ifdef CONFIG_XEN_BACKEND
 extern void xenbus_backend_suspend(int (*fn)(struct device *, void *));
 extern void xenbus_backend_resume(int (*fn)(struct device *, void *));
@@ -52,7 +54,7 @@ struct xen_bus_type
 {
 	char *root;
 	unsigned int levels;
-	int (*get_bus_id)(char bus_id[BUS_ID_SIZE], const char *nodename);
+	int (*get_bus_id)(char bus_id[XEN_BUS_ID_SIZE], const char *nodename);
 	int (*probe)(const char *type, const char *dir);
 	struct bus_type bus;
 };